3884 files changed, 157403 insertions, 124095 deletions

diff --git a/drivers/Kconfig b/drivers/Kconfig
index af02a8a8ec4a..694d5a70d6ce 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -184,4 +184,6 @@ source "drivers/ras/Kconfig"
 
 source "drivers/thunderbolt/Kconfig"
 
+source "drivers/android/Kconfig"
+
 endmenu
diff --git a/drivers/Makefile b/drivers/Makefile
index ebee55537a05..67d2334dc41e 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -161,3 +161,5 @@ obj-$(CONFIG_POWERCAP)		+= powercap/
 obj-$(CONFIG_MCB)		+= mcb/
 obj-$(CONFIG_RAS)		+= ras/
 obj-$(CONFIG_THUNDERBOLT)	+= thunderbolt/
+obj-$(CONFIG_CORESIGHT)		+= coresight/
+obj-$(CONFIG_ANDROID)		+= android/
diff --git a/drivers/acpi/acpi_pad.c b/drivers/acpi/acpi_pad.c
index f148a0580e04..c7b105c0e1d3 100644
--- a/drivers/acpi/acpi_pad.c
+++ b/drivers/acpi/acpi_pad.c
@@ -350,12 +350,10 @@ static ssize_t acpi_pad_idlecpus_store(struct device *dev,
 static ssize_t acpi_pad_idlecpus_show(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
-	int n = 0;
-	n = cpumask_scnprintf(buf, PAGE_SIZE-2, to_cpumask(pad_busy_cpus_bits));
-	buf[n++] = '\n';
-	buf[n] = '\0';
-	return n;
+	return cpumap_print_to_pagebuf(false, buf,
+				       to_cpumask(pad_busy_cpus_bits));
 }
+
 static DEVICE_ATTR(idlecpus, S_IRUGO|S_IWUSR,
 	acpi_pad_idlecpus_show,
 	acpi_pad_idlecpus_store);
diff --git a/drivers/acpi/apei/ghes.c b/drivers/acpi/apei/ghes.c
index 1b6aa514848f..e82d0976a5d0 100644
--- a/drivers/acpi/apei/ghes.c
+++ b/drivers/acpi/apei/ghes.c
@@ -1087,7 +1087,6 @@ static int ghes_remove(struct platform_device *ghes_dev)
 static struct platform_driver ghes_platform_driver = {
 	.driver		= {
 		.name	= "GHES",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= ghes_probe,
 	.remove		= ghes_remove,
diff --git a/drivers/acpi/blacklist.c b/drivers/acpi/blacklist.c
index 7556e7c4a055..9b693d54c743 100644
--- a/drivers/acpi/blacklist.c
+++ b/drivers/acpi/blacklist.c
@@ -305,60 +305,6 @@ static struct dmi_system_id acpi_osi_dmi_table[] __initdata = {
 	 */
 
 	/*
-	 * Lenovo has a mix of systems OSI(Linux) situations
-	 * and thus we can not wildcard the vendor.
-	 *
-	 * _OSI(Linux) helps sound
-	 * DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad R61"),
-	 * DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T61"),
-	 * T400, T500
-	 * _OSI(Linux) has Linux specific hooks
-	 * DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad X61"),
-	 * _OSI(Linux) is a NOP:
-	 * DMI_MATCH(DMI_PRODUCT_VERSION, "3000 N100"),
-	 * DMI_MATCH(DMI_PRODUCT_VERSION, "LENOVO3000 V100"),
-	 */
-	{
-	.callback = dmi_enable_osi_linux,
-	.ident = "Lenovo ThinkPad R61",
-	.matches = {
-		     DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		     DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad R61"),
-		},
-	},
-	{
-	.callback = dmi_enable_osi_linux,
-	.ident = "Lenovo ThinkPad T61",
-	.matches = {
-		     DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		     DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T61"),
-		},
-	},
-	{
-	.callback = dmi_enable_osi_linux,
-	.ident = "Lenovo ThinkPad X61",
-	.matches = {
-		     DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		     DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad X61"),
-		},
-	},
-	{
-	.callback = dmi_enable_osi_linux,
-	.ident = "Lenovo ThinkPad T400",
-	.matches = {
-		     DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		     DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T400"),
-		},
-	},
-	{
-	.callback = dmi_enable_osi_linux,
-	.ident = "Lenovo ThinkPad T500",
-	.matches = {
-		     DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		     DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T500"),
-		},
-	},
-	/*
 	 * Without this this EEEpc exports a non working WMI interface, with
 	 * this it exports a working "good old" eeepc_laptop interface, fixing
 	 * both brightness control, and rfkill not working.
diff --git a/drivers/acpi/device_pm.c b/drivers/acpi/device_pm.c
index 897640188acd..c2daa85fc9f7 100644
--- a/drivers/acpi/device_pm.c
+++ b/drivers/acpi/device_pm.c
@@ -680,13 +680,21 @@ static int acpi_device_wakeup(struct acpi_device *adev, u32 target_state,
 		if (error)
 			return error;
 
+		if (adev->wakeup.flags.enabled)
+			return 0;
+
 		res = acpi_enable_gpe(wakeup->gpe_device, wakeup->gpe_number);
-		if (ACPI_FAILURE(res)) {
+		if (ACPI_SUCCESS(res)) {
+			adev->wakeup.flags.enabled = 1;
+		} else {
 			acpi_disable_wakeup_device_power(adev);
 			return -EIO;
 		}
 	} else {
-		acpi_disable_gpe(wakeup->gpe_device, wakeup->gpe_number);
+		if (adev->wakeup.flags.enabled) {
+			acpi_disable_gpe(wakeup->gpe_device, wakeup->gpe_number);
+			adev->wakeup.flags.enabled = 0;
+		}
 		acpi_disable_wakeup_device_power(adev);
 	}
 	return 0;
diff --git a/drivers/acpi/ec.c b/drivers/acpi/ec.c
index 5f9b74b9b71f..1b5853f384e2 100644
--- a/drivers/acpi/ec.c
+++ b/drivers/acpi/ec.c
@@ -844,6 +844,8 @@ static int ec_install_handlers(struct acpi_ec *ec)
 
 static void ec_remove_handlers(struct acpi_ec *ec)
 {
+	if (!test_bit(EC_FLAGS_HANDLERS_INSTALLED, &ec->flags))
+		return;
 	acpi_disable_gpe(NULL, ec->gpe);
 	if (ACPI_FAILURE(acpi_remove_address_space_handler(ec->handle,
 				ACPI_ADR_SPACE_EC, &acpi_ec_space_handler)))
diff --git a/drivers/acpi/fan.c b/drivers/acpi/fan.c
index caf9b76b7ef8..7a36f02598a6 100644
--- a/drivers/acpi/fan.c
+++ b/drivers/acpi/fan.c
@@ -325,6 +325,7 @@ static int acpi_fan_probe(struct platform_device *pdev)
 	struct thermal_cooling_device *cdev;
 	struct acpi_fan *fan;
 	struct acpi_device *device = ACPI_COMPANION(&pdev->dev);
+	char *name;
 
 	fan = devm_kzalloc(&pdev->dev, sizeof(*fan), GFP_KERNEL);
 	if (!fan) {
@@ -346,7 +347,12 @@ static int acpi_fan_probe(struct platform_device *pdev)
 		}
 	}
 
-	cdev = thermal_cooling_device_register("Fan", device,
+	if (!strncmp(pdev->name, "PNP0C0B", strlen("PNP0C0B")))
+		name = "Fan";
+	else
+		name = acpi_device_bid(device);
+
+	cdev = thermal_cooling_device_register(name, device,
 						&fan_cooling_ops);
 	if (IS_ERR(cdev)) {
 		result = PTR_ERR(cdev);
diff --git a/drivers/acpi/pci_irq.c b/drivers/acpi/pci_irq.c
index 7cc4e33179f9..5277a0ee5704 100644
--- a/drivers/acpi/pci_irq.c
+++ b/drivers/acpi/pci_irq.c
@@ -413,6 +413,9 @@ int acpi_pci_irq_enable(struct pci_dev *dev)
 		return 0;
 	}
 
+	if (dev->irq_managed && dev->irq > 0)
+		return 0;
+
 	entry = acpi_pci_irq_lookup(dev, pin);
 	if (!entry) {
 		/*
@@ -456,6 +459,7 @@ int acpi_pci_irq_enable(struct pci_dev *dev)
 		return rc;
 	}
 	dev->irq = rc;
+	dev->irq_managed = 1;
 
 	if (link)
 		snprintf(link_desc, sizeof(link_desc), " -> Link[%s]", link);
@@ -478,7 +482,7 @@ void acpi_pci_irq_disable(struct pci_dev *dev)
 	u8 pin;
 
 	pin = dev->pin;
-	if (!pin)
+	if (!pin || !dev->irq_managed || dev->irq <= 0)
 		return;
 
 	/* Keep IOAPIC pin configuration when suspending */
@@ -506,6 +510,9 @@ void acpi_pci_irq_disable(struct pci_dev *dev)
 	 */
 
 	dev_dbg(&dev->dev, "PCI INT %c disabled\n", pin_name(pin));
-	if (gsi >= 0 && dev->irq > 0)
+	if (gsi >= 0) {
 		acpi_unregister_gsi(gsi);
+		dev->irq = 0;
+		dev->irq_managed = 0;
+	}
 }
diff --git a/drivers/acpi/pci_root.c b/drivers/acpi/pci_root.c
index cd4de7e038ea..c6bcb8c719d8 100644
--- a/drivers/acpi/pci_root.c
+++ b/drivers/acpi/pci_root.c
@@ -33,6 +33,7 @@
 #include <linux/pci.h>
 #include <linux/pci-acpi.h>
 #include <linux/pci-aspm.h>
+#include <linux/dmar.h>
 #include <linux/acpi.h>
 #include <linux/slab.h>
 #include <linux/dmi.h>
@@ -525,6 +526,7 @@ static int acpi_pci_root_add(struct acpi_device *device,
 	struct acpi_pci_root *root;
 	acpi_handle handle = device->handle;
 	int no_aspm = 0, clear_aspm = 0;
+	bool hotadd = system_state != SYSTEM_BOOTING;
 
 	root = kzalloc(sizeof(struct acpi_pci_root), GFP_KERNEL);
 	if (!root)
@@ -571,6 +573,11 @@ static int acpi_pci_root_add(struct acpi_device *device,
 	strcpy(acpi_device_class(device), ACPI_PCI_ROOT_CLASS);
 	device->driver_data = root;
 
+	if (hotadd && dmar_device_add(handle)) {
+		result = -ENXIO;
+		goto end;
+	}
+
 	pr_info(PREFIX "%s [%s] (domain %04x %pR)\n",
 	       acpi_device_name(device), acpi_device_bid(device),
 	       root->segment, &root->secondary);
@@ -597,7 +604,7 @@ static int acpi_pci_root_add(struct acpi_device *device,
 			root->segment, (unsigned int)root->secondary.start);
 		device->driver_data = NULL;
 		result = -ENODEV;
-		goto end;
+		goto remove_dmar;
 	}
 
 	if (clear_aspm) {
@@ -611,7 +618,7 @@ static int acpi_pci_root_add(struct acpi_device *device,
 	if (device->wakeup.flags.run_wake)
 		device_set_run_wake(root->bus->bridge, true);
 
-	if (system_state != SYSTEM_BOOTING) {
+	if (hotadd) {
 		pcibios_resource_survey_bus(root->bus);
 		pci_assign_unassigned_root_bus_resources(root->bus);
 	}
@@ -621,6 +628,9 @@ static int acpi_pci_root_add(struct acpi_device *device,
 	pci_unlock_rescan_remove();
 	return 1;
 
+remove_dmar:
+	if (hotadd)
+		dmar_device_remove(handle);
 end:
 	kfree(root);
 	return result;
@@ -639,6 +649,8 @@ static void acpi_pci_root_remove(struct acpi_device *device)
 
 	pci_remove_root_bus(root->bus);
 
+	dmar_device_remove(device->handle);
+
 	pci_unlock_rescan_remove();
 
 	kfree(root);
diff --git a/drivers/acpi/processor_core.c b/drivers/acpi/processor_core.c
index ef58f46c8442..342942f90a10 100644
--- a/drivers/acpi/processor_core.c
+++ b/drivers/acpi/processor_core.c
@@ -125,13 +125,12 @@ static int map_mat_entry(acpi_handle handle, int type, u32 acpi_id)
 	}
 
 	header = (struct acpi_subtable_header *)obj->buffer.pointer;
-	if (header->type == ACPI_MADT_TYPE_LOCAL_APIC) {
+	if (header->type == ACPI_MADT_TYPE_LOCAL_APIC)
 		map_lapic_id(header, acpi_id, &apic_id);
-	} else if (header->type == ACPI_MADT_TYPE_LOCAL_SAPIC) {
+	else if (header->type == ACPI_MADT_TYPE_LOCAL_SAPIC)
 		map_lsapic_id(header, type, acpi_id, &apic_id);
-	} else if (header->type == ACPI_MADT_TYPE_LOCAL_X2APIC) {
+	else if (header->type == ACPI_MADT_TYPE_LOCAL_X2APIC)
 		map_x2apic_id(header, type, acpi_id, &apic_id);
-	}
 
 exit:
 	kfree(buffer.pointer);
@@ -164,7 +163,7 @@ int acpi_map_cpuid(int apic_id, u32 acpi_id)
 		 * For example,
 		 *
 		 * Scope (_PR)
-                 * {
+		 * {
 		 *     Processor (CPU0, 0x00, 0x00000410, 0x06) {}
 		 *     Processor (CPU1, 0x01, 0x00000410, 0x06) {}
 		 *     Processor (CPU2, 0x02, 0x00000410, 0x06) {}
diff --git a/drivers/acpi/processor_idle.c b/drivers/acpi/processor_idle.c
index 499536504698..87b704e41877 100644
--- a/drivers/acpi/processor_idle.c
+++ b/drivers/acpi/processor_idle.c
@@ -985,8 +985,6 @@ static int acpi_processor_setup_cpuidle_states(struct acpi_processor *pr)
 		state->flags = 0;
 		switch (cx->type) {
 			case ACPI_STATE_C1:
-			if (cx->entry_method != ACPI_CSTATE_FFH)
-				state->flags |= CPUIDLE_FLAG_TIME_INVALID;
 
 			state->enter = acpi_idle_enter_c1;
 			state->enter_dead = acpi_idle_play_dead;
diff --git a/drivers/acpi/resource.c b/drivers/acpi/resource.c
index 2ba8f02ced36..782a0d15c25f 100644
--- a/drivers/acpi/resource.c
+++ b/drivers/acpi/resource.c
@@ -200,7 +200,7 @@ bool acpi_dev_resource_address_space(struct acpi_resource *ares,
 
 	status = acpi_resource_to_address64(ares, &addr);
 	if (ACPI_FAILURE(status))
-		return true;
+		return false;
 
 	res->start = addr.minimum;
 	res->end = addr.maximum;
diff --git a/drivers/acpi/scan.c b/drivers/acpi/scan.c
index 1b1cf558d3d3..16914cc30882 100644
--- a/drivers/acpi/scan.c
+++ b/drivers/acpi/scan.c
@@ -2214,7 +2214,7 @@ static void acpi_device_dep_initialize(struct acpi_device *adev)
 	status = acpi_evaluate_reference(adev->handle, "_DEP", NULL,
 					&dep_devices);
 	if (ACPI_FAILURE(status)) {
-		dev_err(&adev->dev, "Failed to evaluate _DEP.\n");
+		dev_dbg(&adev->dev, "Failed to evaluate _DEP.\n");
 		return;
 	}
 
@@ -2224,7 +2224,7 @@ static void acpi_device_dep_initialize(struct acpi_device *adev)
 
 		status = acpi_get_object_info(dep_devices.handles[i], &info);
 		if (ACPI_FAILURE(status)) {
-			dev_err(&adev->dev, "Error reading device info\n");
+			dev_dbg(&adev->dev, "Error reading _DEP device info\n");
 			continue;
 		}
 
diff --git a/drivers/acpi/utils.c b/drivers/acpi/utils.c
index dd8ff63ee2b4..cd49a3982b6a 100644
--- a/drivers/acpi/utils.c
+++ b/drivers/acpi/utils.c
@@ -346,22 +346,16 @@ acpi_evaluate_reference(acpi_handle handle,
 	package = buffer.pointer;
 
 	if ((buffer.length == 0) || !package) {
-		printk(KERN_ERR PREFIX "No return object (len %X ptr %p)\n",
-			    (unsigned)buffer.length, package);
 		status = AE_BAD_DATA;
 		acpi_util_eval_error(handle, pathname, status);
 		goto end;
 	}
 	if (package->type != ACPI_TYPE_PACKAGE) {
-		printk(KERN_ERR PREFIX "Expecting a [Package], found type %X\n",
-			    package->type);
 		status = AE_BAD_DATA;
 		acpi_util_eval_error(handle, pathname, status);
 		goto end;
 	}
 	if (!package->package.count) {
-		printk(KERN_ERR PREFIX "[Package] has zero elements (%p)\n",
-			    package);
 		status = AE_BAD_DATA;
 		acpi_util_eval_error(handle, pathname, status);
 		goto end;
@@ -380,17 +374,13 @@ acpi_evaluate_reference(acpi_handle handle,
 
 		if (element->type != ACPI_TYPE_LOCAL_REFERENCE) {
 			status = AE_BAD_DATA;
-			printk(KERN_ERR PREFIX
-				    "Expecting a [Reference] package element, found type %X\n",
-				    element->type);
 			acpi_util_eval_error(handle, pathname, status);
 			break;
 		}
 
 		if (!element->reference.handle) {
-			printk(KERN_WARNING PREFIX "Invalid reference in"
-			       " package %s\n", pathname);
 			status = AE_NULL_ENTRY;
+			acpi_util_eval_error(handle, pathname, status);
 			break;
 		}
 		/* Get the  acpi_handle. */
diff --git a/drivers/acpi/video.c b/drivers/acpi/video.c
index 185a57d13723..c72e79d2c5ad 100644
--- a/drivers/acpi/video.c
+++ b/drivers/acpi/video.c
@@ -155,6 +155,7 @@ struct acpi_video_bus {
 	u8 dos_setting;
 	struct acpi_video_enumerated_device *attached_array;
 	u8 attached_count;
+	u8 child_count;
 	struct acpi_video_bus_cap cap;
 	struct acpi_video_bus_flags flags;
 	struct list_head video_device_list;
@@ -504,6 +505,23 @@ static struct dmi_system_id video_dmi_table[] __initdata = {
 		DMI_MATCH(DMI_PRODUCT_NAME, "HP ENVY 15 Notebook PC"),
 		},
 	},
+
+	{
+	 .callback = video_disable_native_backlight,
+	 .ident = "SAMSUNG 870Z5E/880Z5E/680Z5E",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD."),
+		DMI_MATCH(DMI_PRODUCT_NAME, "870Z5E/880Z5E/680Z5E"),
+		},
+	},
+	{
+	 .callback = video_disable_native_backlight,
+	 .ident = "SAMSUNG 370R4E/370R4V/370R5E/3570RE/370R5V",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD."),
+		DMI_MATCH(DMI_PRODUCT_NAME, "370R4E/370R4V/370R5E/3570RE/370R5V"),
+		},
+	},
 	{}
 };
 
@@ -1159,8 +1177,12 @@ static bool acpi_video_device_in_dod(struct acpi_video_device *device)
 	struct acpi_video_bus *video = device->video;
 	int i;
 
-	/* If we have a broken _DOD, no need to test */
-	if (!video->attached_count)
+	/*
+	 * If we have a broken _DOD or we have more than 8 output devices
+	 * under the graphics controller node that we can't proper deal with
+	 * in the operation region code currently, no need to test.
+	 */
+	if (!video->attached_count || video->child_count > 8)
 		return true;
 
 	for (i = 0; i < video->attached_count; i++) {
@@ -1413,6 +1435,7 @@ acpi_video_bus_get_devices(struct acpi_video_bus *video,
 			dev_err(&dev->dev, "Can't attach device\n");
 			break;
 		}
+		video->child_count++;
 	}
 	return status;
 }
diff --git a/drivers/amba/bus.c b/drivers/amba/bus.c
index 973a3332a85f..52ddd9fbb55e 100644
--- a/drivers/amba/bus.c
+++ b/drivers/amba/bus.c
@@ -95,8 +95,12 @@ static int amba_pm_runtime_suspend(struct device *dev)
 	struct amba_device *pcdev = to_amba_device(dev);
 	int ret = pm_generic_runtime_suspend(dev);
 
-	if (ret == 0 && dev->driver)
-		clk_disable_unprepare(pcdev->pclk);
+	if (ret == 0 && dev->driver) {
+		if (pm_runtime_is_irq_safe(dev))
+			clk_disable(pcdev->pclk);
+		else
+			clk_disable_unprepare(pcdev->pclk);
+	}
 
 	return ret;
 }
@@ -107,7 +111,10 @@ static int amba_pm_runtime_resume(struct device *dev)
 	int ret;
 
 	if (dev->driver) {
-		ret = clk_prepare_enable(pcdev->pclk);
+		if (pm_runtime_is_irq_safe(dev))
+			ret = clk_enable(pcdev->pclk);
+		else
+			ret = clk_prepare_enable(pcdev->pclk);
 		/* Failure is probably fatal to the system, but... */
 		if (ret)
 			return ret;
@@ -115,7 +122,7 @@ static int amba_pm_runtime_resume(struct device *dev)
 
 	return pm_generic_runtime_resume(dev);
 }
-#endif
+#endif /* CONFIG_PM */
 
 static const struct dev_pm_ops amba_pm = {
 	.suspend	= pm_generic_suspend,
@@ -336,7 +343,7 @@ int amba_device_add(struct amba_device *dev, struct resource *parent)
 
 		amba_put_disable_pclk(dev);
 
-		if (cid == AMBA_CID)
+		if (cid == AMBA_CID || cid == CORESIGHT_CID)
 			dev->periphid = pid;
 
 		if (!dev->periphid)
diff --git a/drivers/amba/tegra-ahb.c b/drivers/amba/tegra-ahb.c
index d8961ef4d2e7..c6dc3548e5d1 100644
--- a/drivers/amba/tegra-ahb.c
+++ b/drivers/amba/tegra-ahb.c
@@ -277,7 +277,6 @@ static struct platform_driver tegra_ahb_driver = {
 	.probe = tegra_ahb_probe,
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = tegra_ahb_of_match,
 		.pm = &tegra_ahb_pm,
 	},
diff --git a/drivers/android/Kconfig b/drivers/android/Kconfig
new file mode 100644
index 000000000000..bdfc6c6f4f5a
--- /dev/null
+++ b/drivers/android/Kconfig
@@ -0,0 +1,37 @@
+menu "Android"
+
+config ANDROID
+	bool "Android Drivers"
+	---help---
+	  Enable support for various drivers needed on the Android platform
+
+if ANDROID
+
+config ANDROID_BINDER_IPC
+	bool "Android Binder IPC Driver"
+	depends on MMU
+	default n
+	---help---
+	  Binder is used in Android for both communication between processes,
+	  and remote method invocation.
+
+	  This means one Android process can call a method/routine in another
+	  Android process, using Binder to identify, invoke and pass arguments
+	  between said processes.
+
+config ANDROID_BINDER_IPC_32BIT
+	bool
+	depends on !64BIT && ANDROID_BINDER_IPC
+	default y
+	---help---
+	  The Binder API has been changed to support both 32 and 64bit
+	  applications in a mixed environment.
+
+	  Enable this to support an old 32-bit Android user-space (v4.4 and
+	  earlier).
+
+	  Note that enabling this will break newer Android user-space.
+
+endif # if ANDROID
+
+endmenu
diff --git a/drivers/android/Makefile b/drivers/android/Makefile
new file mode 100644
index 000000000000..3b7e4b072c58
--- /dev/null
+++ b/drivers/android/Makefile
@@ -0,0 +1,3 @@
+ccflags-y += -I$(src)			# needed for trace events
+
+obj-$(CONFIG_ANDROID_BINDER_IPC)	+= binder.o
diff --git a/drivers/staging/android/binder.c b/drivers/android/binder.c
index c69c40d69d5c..8c43521d3f11 100644
--- a/drivers/staging/android/binder.c
+++ b/drivers/android/binder.c
@@ -38,7 +38,11 @@
 #include <linux/slab.h>
 #include <linux/pid_namespace.h>
 
-#include "binder.h"
+#ifdef CONFIG_ANDROID_BINDER_IPC_32BIT
+#define BINDER_IPC_32BIT 1
+#endif
+
+#include <uapi/linux/android/binder.h>
 #include "binder_trace.h"
 
 static DEFINE_MUTEX(binder_main_lock);
diff --git a/drivers/staging/android/binder_trace.h b/drivers/android/binder_trace.h
index 7f20f3dc8369..7f20f3dc8369 100644
--- a/drivers/staging/android/binder_trace.h
+++ b/drivers/android/binder_trace.h
diff --git a/drivers/ata/Kconfig b/drivers/ata/Kconfig
index cd4cccbfd2ab..a3a13605a9c4 100644
--- a/drivers/ata/Kconfig
+++ b/drivers/ata/Kconfig
@@ -61,7 +61,7 @@ config ATA_ACPI
 
 config SATA_ZPODD
 	bool "SATA Zero Power Optical Disc Drive (ZPODD) support"
-	depends on ATA_ACPI && PM_RUNTIME
+	depends on ATA_ACPI && PM
 	default n
 	help
 	  This option adds support for SATA Zero Power Optical Disc
diff --git a/drivers/ata/ahci_da850.c b/drivers/ata/ahci_da850.c
index ad1e71ec10cf..ce8a7a6d6c7f 100644
--- a/drivers/ata/ahci_da850.c
+++ b/drivers/ata/ahci_da850.c
@@ -103,7 +103,6 @@ static struct platform_driver ahci_da850_driver = {
 	.remove = ata_platform_remove_one,
 	.driver = {
 		.name = "ahci_da850",
-		.owner = THIS_MODULE,
 		.pm = &ahci_da850_pm_ops,
 	},
 };
diff --git a/drivers/ata/ahci_imx.c b/drivers/ata/ahci_imx.c
index f3970b4ed889..35d51c59a370 100644
--- a/drivers/ata/ahci_imx.c
+++ b/drivers/ata/ahci_imx.c
@@ -679,7 +679,6 @@ static struct platform_driver imx_ahci_driver = {
 	.remove = ata_platform_remove_one,
 	.driver = {
 		.name = "ahci-imx",
-		.owner = THIS_MODULE,
 		.of_match_table = imx_ahci_of_match,
 		.pm = &ahci_imx_pm_ops,
 	},
diff --git a/drivers/ata/ahci_mvebu.c b/drivers/ata/ahci_mvebu.c
index 68672d2692ee..64bb08432b69 100644
--- a/drivers/ata/ahci_mvebu.c
+++ b/drivers/ata/ahci_mvebu.c
@@ -115,7 +115,6 @@ static struct platform_driver ahci_mvebu_driver = {
 	.remove = ata_platform_remove_one,
 	.driver = {
 		.name = "ahci-mvebu",
-		.owner = THIS_MODULE,
 		.of_match_table = ahci_mvebu_of_match,
 	},
 };
diff --git a/drivers/ata/ahci_platform.c b/drivers/ata/ahci_platform.c
index 06f1d59fa678..18d539837045 100644
--- a/drivers/ata/ahci_platform.c
+++ b/drivers/ata/ahci_platform.c
@@ -76,7 +76,6 @@ static struct platform_driver ahci_driver = {
 	.remove = ata_platform_remove_one,
 	.driver = {
 		.name = "ahci",
-		.owner = THIS_MODULE,
 		.of_match_table = ahci_of_match,
 		.pm = &ahci_pm_ops,
 	},
diff --git a/drivers/ata/ahci_st.c b/drivers/ata/ahci_st.c
index 835d6eea84fd..2f9e8317cc16 100644
--- a/drivers/ata/ahci_st.c
+++ b/drivers/ata/ahci_st.c
@@ -230,7 +230,6 @@ MODULE_DEVICE_TABLE(of, st_ahci_match);
 static struct platform_driver st_ahci_driver = {
 	.driver = {
 		.name = "st_ahci",
-		.owner = THIS_MODULE,
 		.pm = &st_ahci_pm_ops,
 		.of_match_table = of_match_ptr(st_ahci_match),
 	},
diff --git a/drivers/ata/ahci_sunxi.c b/drivers/ata/ahci_sunxi.c
index e44d675a30ec..e2e0da539a2f 100644
--- a/drivers/ata/ahci_sunxi.c
+++ b/drivers/ata/ahci_sunxi.c
@@ -27,6 +27,12 @@
 #include <linux/regulator/consumer.h>
 #include "ahci.h"
 
+/* Insmod parameters */
+static bool enable_pmp;
+module_param(enable_pmp, bool, 0);
+MODULE_PARM_DESC(enable_pmp,
+	"Enable support for sata port multipliers, only use if you use a pmp!");
+
 #define AHCI_BISTAFR	0x00a0
 #define AHCI_BISTCR	0x00a4
 #define AHCI_BISTFCTR	0x00a8
@@ -184,7 +190,15 @@ static int ahci_sunxi_probe(struct platform_device *pdev)
 		goto disable_resources;
 
 	hpriv->flags = AHCI_HFLAG_32BIT_ONLY | AHCI_HFLAG_NO_MSI |
-		       AHCI_HFLAG_NO_PMP | AHCI_HFLAG_YES_NCQ;
+		       AHCI_HFLAG_YES_NCQ;
+
+	/*
+	 * The sunxi sata controller seems to be unable to successfully do a
+	 * soft reset if no pmp is attached, so disable pmp use unless
+	 * requested, otherwise directly attached disks do not work.
+	 */
+	if (!enable_pmp)
+		hpriv->flags |= AHCI_HFLAG_NO_PMP;
 
 	rc = ahci_platform_init_host(pdev, hpriv, &ahci_sunxi_port_info);
 	if (rc)
@@ -238,7 +252,6 @@ static struct platform_driver ahci_sunxi_driver = {
 	.remove = ata_platform_remove_one,
 	.driver = {
 		.name = "ahci-sunxi",
-		.owner = THIS_MODULE,
 		.of_match_table = ahci_sunxi_of_match,
 		.pm = &ahci_sunxi_pm_ops,
 	},
diff --git a/drivers/ata/ahci_xgene.c b/drivers/ata/ahci_xgene.c
index 0f8538f238b6..feeb8f1e2fe8 100644
--- a/drivers/ata/ahci_xgene.c
+++ b/drivers/ata/ahci_xgene.c
@@ -544,7 +544,6 @@ static struct platform_driver xgene_ahci_driver = {
 	.remove = ata_platform_remove_one,
 	.driver = {
 		.name = "xgene-ahci",
-		.owner = THIS_MODULE,
 		.of_match_table = xgene_ahci_of_match,
 	},
 };
diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c
index c5ba15af87d3..5c84fb5c3372 100644
--- a/drivers/ata/libata-core.c
+++ b/drivers/ata/libata-core.c
@@ -1043,8 +1043,8 @@ const char *sata_spd_string(unsigned int spd)
  *	None.
  *
  *	RETURNS:
- *	Device type, %ATA_DEV_ATA, %ATA_DEV_ATAPI, %ATA_DEV_PMP or
- *	%ATA_DEV_UNKNOWN the event of failure.
+ *	Device type, %ATA_DEV_ATA, %ATA_DEV_ATAPI, %ATA_DEV_PMP,
+ *	%ATA_DEV_ZAC, or %ATA_DEV_UNKNOWN the event of failure.
  */
 unsigned int ata_dev_classify(const struct ata_taskfile *tf)
 {
@@ -1089,6 +1089,11 @@ unsigned int ata_dev_classify(const struct ata_taskfile *tf)
 		return ATA_DEV_SEMB;
 	}
 
+	if ((tf->lbam == 0xcd) && (tf->lbah == 0xab)) {
+		DPRINTK("found ZAC device by sig\n");
+		return ATA_DEV_ZAC;
+	}
+
 	DPRINTK("unknown device\n");
 	return ATA_DEV_UNKNOWN;
 }
@@ -1329,7 +1334,7 @@ static int ata_hpa_resize(struct ata_device *dev)
 	int rc;
 
 	/* do we need to do it? */
-	if (dev->class != ATA_DEV_ATA ||
+	if ((dev->class != ATA_DEV_ATA && dev->class != ATA_DEV_ZAC) ||
 	    !ata_id_has_lba(dev->id) || !ata_id_hpa_enabled(dev->id) ||
 	    (dev->horkage & ATA_HORKAGE_BROKEN_HPA))
 		return 0;
@@ -1889,6 +1894,7 @@ retry:
 	case ATA_DEV_SEMB:
 		class = ATA_DEV_ATA;	/* some hard drives report SEMB sig */
 	case ATA_DEV_ATA:
+	case ATA_DEV_ZAC:
 		tf.command = ATA_CMD_ID_ATA;
 		break;
 	case ATA_DEV_ATAPI:
@@ -1980,7 +1986,7 @@ retry:
 	rc = -EINVAL;
 	reason = "device reports invalid type";
 
-	if (class == ATA_DEV_ATA) {
+	if (class == ATA_DEV_ATA || class == ATA_DEV_ZAC) {
 		if (!ata_id_is_ata(id) && !ata_id_is_cfa(id))
 			goto err_out;
 		if (ap->host->flags & ATA_HOST_IGNORE_ATA &&
@@ -2015,7 +2021,8 @@ retry:
 			goto retry;
 	}
 
-	if ((flags & ATA_READID_POSTRESET) && class == ATA_DEV_ATA) {
+	if ((flags & ATA_READID_POSTRESET) &&
+	    (class == ATA_DEV_ATA || class == ATA_DEV_ZAC)) {
 		/*
 		 * The exact sequence expected by certain pre-ATA4 drives is:
 		 * SRST RESET
@@ -2280,7 +2287,7 @@ int ata_dev_configure(struct ata_device *dev)
 			sizeof(modelbuf));
 
 	/* ATA-specific feature tests */
-	if (dev->class == ATA_DEV_ATA) {
+	if (dev->class == ATA_DEV_ATA || dev->class == ATA_DEV_ZAC) {
 		if (ata_id_is_cfa(id)) {
 			/* CPRM may make this media unusable */
 			if (id[ATA_ID_CFA_KEY_MGMT] & 1)
@@ -4033,6 +4040,7 @@ int ata_dev_revalidate(struct ata_device *dev, unsigned int new_class,
 	if (ata_class_enabled(new_class) &&
 	    new_class != ATA_DEV_ATA &&
 	    new_class != ATA_DEV_ATAPI &&
+	    new_class != ATA_DEV_ZAC &&
 	    new_class != ATA_DEV_SEMB) {
 		ata_dev_info(dev, "class mismatch %u != %u\n",
 			     dev->class, new_class);
diff --git a/drivers/ata/libata-eh.c b/drivers/ata/libata-eh.c
index dad83df555c4..3dbec8954c86 100644
--- a/drivers/ata/libata-eh.c
+++ b/drivers/ata/libata-eh.c
@@ -1809,6 +1809,7 @@ static unsigned int ata_eh_analyze_tf(struct ata_queued_cmd *qc,
 
 	switch (qc->dev->class) {
 	case ATA_DEV_ATA:
+	case ATA_DEV_ZAC:
 		if (err & ATA_ICRC)
 			qc->err_mask |= AC_ERR_ATA_BUS;
 		if (err & (ATA_UNC | ATA_AMNF))
@@ -3792,7 +3793,8 @@ int ata_eh_recover(struct ata_port *ap, ata_prereset_fn_t prereset,
 				struct ata_eh_context *ehc = &link->eh_context;
 				unsigned long tmp;
 
-				if (dev->class != ATA_DEV_ATA)
+				if (dev->class != ATA_DEV_ATA &&
+				    dev->class != ATA_DEV_ZAC)
 					continue;
 				if (!(ehc->i.dev_action[dev->devno] &
 				      ATA_EH_PARK))
@@ -3873,7 +3875,8 @@ int ata_eh_recover(struct ata_port *ap, ata_prereset_fn_t prereset,
 
 		/* retry flush if necessary */
 		ata_for_each_dev(dev, link, ALL) {
-			if (dev->class != ATA_DEV_ATA)
+			if (dev->class != ATA_DEV_ATA &&
+			    dev->class != ATA_DEV_ZAC)
 				continue;
 			rc = ata_eh_maybe_retry_flush(dev);
 			if (rc)
diff --git a/drivers/ata/libata-scsi.c b/drivers/ata/libata-scsi.c
index dd45c6a03e5d..e364e86e84d7 100644
--- a/drivers/ata/libata-scsi.c
+++ b/drivers/ata/libata-scsi.c
@@ -235,7 +235,8 @@ static ssize_t ata_scsi_park_store(struct device *device,
 		rc = -ENODEV;
 		goto unlock;
 	}
-	if (dev->class != ATA_DEV_ATA) {
+	if (dev->class != ATA_DEV_ATA &&
+	    dev->class != ATA_DEV_ZAC) {
 		rc = -EOPNOTSUPP;
 		goto unlock;
 	}
@@ -1961,6 +1962,7 @@ static void ata_scsi_rbuf_fill(struct ata_scsi_args *args,
 static unsigned int ata_scsiop_inq_std(struct ata_scsi_args *args, u8 *rbuf)
 {
 	const u8 versions[] = {
+		0x00,
 		0x60,	/* SAM-3 (no version claimed) */
 
 		0x03,
@@ -1969,6 +1971,20 @@ static unsigned int ata_scsiop_inq_std(struct ata_scsi_args *args, u8 *rbuf)
 		0x02,
 		0x60	/* SPC-3 (no version claimed) */
 	};
+	const u8 versions_zbc[] = {
+		0x00,
+		0xA0,	/* SAM-5 (no version claimed) */
+
+		0x04,
+		0xC0,	/* SBC-3 (no version claimed) */
+
+		0x04,
+		0x60,	/* SPC-4 (no version claimed) */
+
+		0x60,
+		0x20,   /* ZBC (no version claimed) */
+	};
+
 	u8 hdr[] = {
 		TYPE_DISK,
 		0,
@@ -1983,6 +1999,11 @@ static unsigned int ata_scsiop_inq_std(struct ata_scsi_args *args, u8 *rbuf)
 	if (ata_id_removeable(args->id))
 		hdr[1] |= (1 << 7);
 
+	if (args->dev->class == ATA_DEV_ZAC) {
+		hdr[0] = TYPE_ZBC;
+		hdr[2] = 0x6; /* ZBC is defined in SPC-4 */
+	}
+
 	memcpy(rbuf, hdr, sizeof(hdr));
 	memcpy(&rbuf[8], "ATA     ", 8);
 	ata_id_string(args->id, &rbuf[16], ATA_ID_PROD, 16);
@@ -1995,7 +2016,10 @@ static unsigned int ata_scsiop_inq_std(struct ata_scsi_args *args, u8 *rbuf)
 	if (rbuf[32] == 0 || rbuf[32] == ' ')
 		memcpy(&rbuf[32], "n/a ", 4);
 
-	memcpy(rbuf + 59, versions, sizeof(versions));
+	if (args->dev->class == ATA_DEV_ZAC)
+		memcpy(rbuf + 58, versions_zbc, sizeof(versions_zbc));
+	else
+		memcpy(rbuf + 58, versions, sizeof(versions));
 
 	return 0;
 }
@@ -2564,7 +2588,6 @@ static void atapi_request_sense(struct ata_queued_cmd *qc)
 
 	DPRINTK("ATAPI request sense\n");
 
-	/* FIXME: is this needed? */
 	memset(cmd->sense_buffer, 0, SCSI_SENSE_BUFFERSIZE);
 
 #ifdef CONFIG_ATA_SFF
@@ -3405,7 +3428,7 @@ static inline int __ata_scsi_queuecmd(struct scsi_cmnd *scmd,
 	ata_xlat_func_t xlat_func;
 	int rc = 0;
 
-	if (dev->class == ATA_DEV_ATA) {
+	if (dev->class == ATA_DEV_ATA || dev->class == ATA_DEV_ZAC) {
 		if (unlikely(!scmd->cmd_len || scmd->cmd_len > dev->cdb_len))
 			goto bad_cdb_len;
 
diff --git a/drivers/ata/libata-transport.c b/drivers/ata/libata-transport.c
index e37413228228..3227b7c8a05f 100644
--- a/drivers/ata/libata-transport.c
+++ b/drivers/ata/libata-transport.c
@@ -143,6 +143,7 @@ static struct {
 	{ ATA_DEV_PMP_UNSUP,		"pmp" },
 	{ ATA_DEV_SEMB,			"semb" },
 	{ ATA_DEV_SEMB_UNSUP,		"semb" },
+	{ ATA_DEV_ZAC,			"zac" },
 	{ ATA_DEV_NONE,			"none" }
 };
 ata_bitfield_name_search(class, ata_class_names)
diff --git a/drivers/ata/pata_arasan_cf.c b/drivers/ata/pata_arasan_cf.c
index 38216b991474..a9b0c820f2eb 100644
--- a/drivers/ata/pata_arasan_cf.c
+++ b/drivers/ata/pata_arasan_cf.c
@@ -961,7 +961,6 @@ static struct platform_driver arasan_cf_driver = {
 	.remove		= arasan_cf_remove,
 	.driver		= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 		.pm	= &arasan_cf_pm_ops,
 		.of_match_table = of_match_ptr(arasan_cf_id_table),
 	},
diff --git a/drivers/ata/pata_at32.c b/drivers/ata/pata_at32.c
index d59d5239405f..9aeb7a6dd4d4 100644
--- a/drivers/ata/pata_at32.c
+++ b/drivers/ata/pata_at32.c
@@ -389,7 +389,6 @@ static struct platform_driver pata_at32_driver = {
 	.remove	       = __exit_p(pata_at32_remove),
 	.driver	       = {
 		.name  = "at32_ide",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/ata/pata_at91.c b/drivers/ata/pata_at91.c
index 8a66f23af4c4..9e85937d36a9 100644
--- a/drivers/ata/pata_at91.c
+++ b/drivers/ata/pata_at91.c
@@ -444,7 +444,6 @@ static struct platform_driver pata_at91_driver = {
 	.remove		= pata_at91_remove,
 	.driver		= {
 		.name		= DRV_NAME,
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/ata/pata_bf54x.c b/drivers/ata/pata_bf54x.c
index 03f2f2bc83bd..dd7410019d15 100644
--- a/drivers/ata/pata_bf54x.c
+++ b/drivers/ata/pata_bf54x.c
@@ -1657,7 +1657,6 @@ static struct platform_driver bfin_atapi_driver = {
 	.resume			= bfin_atapi_resume,
 	.driver = {
 		.name		= DRV_NAME,
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/ata/pata_ep93xx.c b/drivers/ata/pata_ep93xx.c
index 4d37c5415fc7..bd6b089c67a3 100644
--- a/drivers/ata/pata_ep93xx.c
+++ b/drivers/ata/pata_ep93xx.c
@@ -1021,7 +1021,6 @@ static int ep93xx_pata_remove(struct platform_device *pdev)
 static struct platform_driver ep93xx_pata_platform_driver = {
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 	},
 	.probe = ep93xx_pata_probe,
 	.remove = ep93xx_pata_remove,
diff --git a/drivers/ata/pata_imx.c b/drivers/ata/pata_imx.c
index 989ff5ac69ec..139d20778b29 100644
--- a/drivers/ata/pata_imx.c
+++ b/drivers/ata/pata_imx.c
@@ -240,7 +240,6 @@ static struct platform_driver pata_imx_driver = {
 	.driver = {
 		.name		= DRV_NAME,
 		.of_match_table	= imx_pata_dt_ids,
-		.owner		= THIS_MODULE,
 		.pm		= &pata_imx_pm_ops,
 	},
 };
diff --git a/drivers/ata/pata_ixp4xx_cf.c b/drivers/ata/pata_ixp4xx_cf.c
index ddf470c2341d..abda44183512 100644
--- a/drivers/ata/pata_ixp4xx_cf.c
+++ b/drivers/ata/pata_ixp4xx_cf.c
@@ -193,7 +193,6 @@ static int ixp4xx_pata_probe(struct platform_device *pdev)
 static struct platform_driver ixp4xx_pata_platform_driver = {
 	.driver	 = {
 		.name   = DRV_NAME,
-		.owner  = THIS_MODULE,
 	},
 	.probe		= ixp4xx_pata_probe,
 	.remove		= ata_platform_remove_one,
diff --git a/drivers/ata/pata_mpc52xx.c b/drivers/ata/pata_mpc52xx.c
index ccd1c83a05cc..252ba27fa63b 100644
--- a/drivers/ata/pata_mpc52xx.c
+++ b/drivers/ata/pata_mpc52xx.c
@@ -863,7 +863,6 @@ static struct platform_driver mpc52xx_ata_of_platform_driver = {
 #endif
 	.driver		= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = mpc52xx_ata_of_match,
 	},
 };
diff --git a/drivers/ata/pata_octeon_cf.c b/drivers/ata/pata_octeon_cf.c
index 2a97d3a531ec..80a80548ad0a 100644
--- a/drivers/ata/pata_octeon_cf.c
+++ b/drivers/ata/pata_octeon_cf.c
@@ -1059,7 +1059,6 @@ static struct platform_driver octeon_cf_driver = {
 	.probe		= octeon_cf_probe,
 	.driver		= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = octeon_cf_match,
 		.shutdown = octeon_cf_shutdown
 	},
diff --git a/drivers/ata/pata_of_platform.c b/drivers/ata/pata_of_platform.c
index 64965398914a..dcc408abe171 100644
--- a/drivers/ata/pata_of_platform.c
+++ b/drivers/ata/pata_of_platform.c
@@ -75,7 +75,6 @@ MODULE_DEVICE_TABLE(of, pata_of_platform_match);
 static struct platform_driver pata_of_platform_driver = {
 	.driver = {
 		.name = "pata_of_platform",
-		.owner = THIS_MODULE,
 		.of_match_table = pata_of_platform_match,
 	},
 	.probe		= pata_of_platform_probe,
diff --git a/drivers/ata/pata_palmld.c b/drivers/ata/pata_palmld.c
index df2bb7504fc8..8c0d7d736b7a 100644
--- a/drivers/ata/pata_palmld.c
+++ b/drivers/ata/pata_palmld.c
@@ -124,7 +124,6 @@ static int palmld_pata_remove(struct platform_device *dev)
 static struct platform_driver palmld_pata_platform_driver = {
 	.driver	 = {
 		.name   = DRV_NAME,
-		.owner  = THIS_MODULE,
 	},
 	.probe		= palmld_pata_probe,
 	.remove		= palmld_pata_remove,
diff --git a/drivers/ata/pata_platform.c b/drivers/ata/pata_platform.c
index f8cff3e247c5..1eedfe46d7c8 100644
--- a/drivers/ata/pata_platform.c
+++ b/drivers/ata/pata_platform.c
@@ -224,7 +224,6 @@ static struct platform_driver pata_platform_driver = {
 	.remove		= ata_platform_remove_one,
 	.driver = {
 		.name		= DRV_NAME,
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/ata/pata_pxa.c b/drivers/ata/pata_pxa.c
index 73259bfda1e3..c36b3e6531d8 100644
--- a/drivers/ata/pata_pxa.c
+++ b/drivers/ata/pata_pxa.c
@@ -385,7 +385,6 @@ static struct platform_driver pxa_ata_driver = {
 	.remove		= pxa_ata_remove,
 	.driver		= {
 		.name		= DRV_NAME,
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/ata/pata_rb532_cf.c b/drivers/ata/pata_rb532_cf.c
index 3c5eb8fa6bd1..6d08446b877c 100644
--- a/drivers/ata/pata_rb532_cf.c
+++ b/drivers/ata/pata_rb532_cf.c
@@ -193,7 +193,6 @@ static struct platform_driver rb532_pata_platform_driver = {
 	.remove		= rb532_pata_driver_remove,
 	.driver	 = {
 		.name   = DRV_NAME,
-		.owner  = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/ata/pata_samsung_cf.c b/drivers/ata/pata_samsung_cf.c
index 1a24a5dc3940..fa44eb2872db 100644
--- a/drivers/ata/pata_samsung_cf.c
+++ b/drivers/ata/pata_samsung_cf.c
@@ -656,7 +656,6 @@ static struct platform_driver pata_s3c_driver = {
 	.id_table	= pata_s3c_driver_ids,
 	.driver		= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 #ifdef CONFIG_PM_SLEEP
 		.pm	= &pata_s3c_pm_ops,
 #endif
diff --git a/drivers/ata/sata_dwc_460ex.c b/drivers/ata/sata_dwc_460ex.c
index 0bb2cabd2197..c7ddef89e7b0 100644
--- a/drivers/ata/sata_dwc_460ex.c
+++ b/drivers/ata/sata_dwc_460ex.c
@@ -1807,7 +1807,6 @@ MODULE_DEVICE_TABLE(of, sata_dwc_match);
 static struct platform_driver sata_dwc_driver = {
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = sata_dwc_match,
 	},
 	.probe = sata_dwc_probe,
diff --git a/drivers/ata/sata_fsl.c b/drivers/ata/sata_fsl.c
index 65071591b143..f9054cd36a72 100644
--- a/drivers/ata/sata_fsl.c
+++ b/drivers/ata/sata_fsl.c
@@ -1624,7 +1624,6 @@ MODULE_DEVICE_TABLE(of, fsl_sata_match);
 static struct platform_driver fsl_sata_driver = {
 	.driver = {
 		.name = "fsl-sata",
-		.owner = THIS_MODULE,
 		.of_match_table = fsl_sata_match,
 	},
 	.probe		= sata_fsl_probe,
diff --git a/drivers/ata/sata_highbank.c b/drivers/ata/sata_highbank.c
index ce2b99a1ed70..24e311fe2c1c 100644
--- a/drivers/ata/sata_highbank.c
+++ b/drivers/ata/sata_highbank.c
@@ -634,7 +634,6 @@ static struct platform_driver ahci_highbank_driver = {
 	.remove = ata_platform_remove_one,
         .driver = {
                 .name = "highbank-ahci",
-                .owner = THIS_MODULE,
                 .of_match_table = ahci_of_match,
                 .pm = &ahci_highbank_pm_ops,
         },
diff --git a/drivers/ata/sata_mv.c b/drivers/ata/sata_mv.c
index 391cfda1e83f..f9a0e34eb111 100644
--- a/drivers/ata/sata_mv.c
+++ b/drivers/ata/sata_mv.c
@@ -4280,7 +4280,6 @@ static struct platform_driver mv_platform_driver = {
 	.resume		= mv_platform_resume,
 	.driver		= {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(mv_sata_dt_ids),
 	},
 };
diff --git a/drivers/ata/sata_rcar.c b/drivers/ata/sata_rcar.c
index ea1fbc1d4c5f..cb0d2e644af5 100644
--- a/drivers/ata/sata_rcar.c
+++ b/drivers/ata/sata_rcar.c
@@ -1004,7 +1004,6 @@ static struct platform_driver sata_rcar_driver = {
 	.id_table	= sata_rcar_id_table,
 	.driver = {
 		.name		= DRV_NAME,
-		.owner		= THIS_MODULE,
 		.of_match_table	= sata_rcar_match,
 #ifdef CONFIG_PM_SLEEP
 		.pm		= &sata_rcar_pm_ops,
diff --git a/drivers/atm/fore200e.c b/drivers/atm/fore200e.c
index d4725fc0395d..d5d9eafbbfcf 100644
--- a/drivers/atm/fore200e.c
+++ b/drivers/atm/fore200e.c
@@ -2687,7 +2687,6 @@ MODULE_DEVICE_TABLE(of, fore200e_sba_match);
 static struct platform_driver fore200e_sba_driver = {
 	.driver = {
 		.name = "fore_200e",
-		.owner = THIS_MODULE,
 		.of_match_table = fore200e_sba_match,
 	},
 	.probe		= fore200e_sba_probe,
diff --git a/drivers/base/Makefile b/drivers/base/Makefile
index 53c3fe1aeb29..527d291706e8 100644
--- a/drivers/base/Makefile
+++ b/drivers/base/Makefile
@@ -4,7 +4,7 @@ obj-y			:= component.o core.o bus.o dd.o syscore.o \
 			   driver.o class.o platform.o \
 			   cpu.o firmware.o init.o map.o devres.o \
 			   attribute_container.o transport_class.o \
-			   topology.o container.o property.o
+			   topology.o container.o property.o cacheinfo.o
 obj-$(CONFIG_DEVTMPFS)	+= devtmpfs.o
 obj-$(CONFIG_DMA_CMA) += dma-contiguous.o
 obj-y			+= power/
diff --git a/drivers/base/bus.c b/drivers/base/bus.c
index 83e910a57563..876bae5ade33 100644
--- a/drivers/base/bus.c
+++ b/drivers/base/bus.c
@@ -254,13 +254,15 @@ static ssize_t store_drivers_probe(struct bus_type *bus,
 				   const char *buf, size_t count)
 {
 	struct device *dev;
+	int err = -EINVAL;
 
 	dev = bus_find_device_by_name(bus, NULL, buf);
 	if (!dev)
 		return -ENODEV;
-	if (bus_rescan_devices_helper(dev, NULL) != 0)
-		return -EINVAL;
-	return count;
+	if (bus_rescan_devices_helper(dev, NULL) == 0)
+		err = count;
+	put_device(dev);
+	return err;
 }
 
 static struct device *next_device(struct klist_iter *i)
diff --git a/drivers/base/cacheinfo.c b/drivers/base/cacheinfo.c
new file mode 100644
index 000000000000..6e64563361f0
--- /dev/null
+++ b/drivers/base/cacheinfo.c
@@ -0,0 +1,539 @@
+/*
+ * cacheinfo support - processor cache information via sysfs
+ *
+ * Based on arch/x86/kernel/cpu/intel_cacheinfo.c
+ * Author: Sudeep Holla <sudeep.holla@arm.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 "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * 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/bitops.h>
+#include <linux/cacheinfo.h>
+#include <linux/compiler.h>
+#include <linux/cpu.h>
+#include <linux/device.h>
+#include <linux/init.h>
+#include <linux/of.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/smp.h>
+#include <linux/sysfs.h>
+
+/* pointer to per cpu cacheinfo */
+static DEFINE_PER_CPU(struct cpu_cacheinfo, ci_cpu_cacheinfo);
+#define ci_cacheinfo(cpu)	(&per_cpu(ci_cpu_cacheinfo, cpu))
+#define cache_leaves(cpu)	(ci_cacheinfo(cpu)->num_leaves)
+#define per_cpu_cacheinfo(cpu)	(ci_cacheinfo(cpu)->info_list)
+
+struct cpu_cacheinfo *get_cpu_cacheinfo(unsigned int cpu)
+{
+	return ci_cacheinfo(cpu);
+}
+
+#ifdef CONFIG_OF
+static int cache_setup_of_node(unsigned int cpu)
+{
+	struct device_node *np;
+	struct cacheinfo *this_leaf;
+	struct device *cpu_dev = get_cpu_device(cpu);
+	struct cpu_cacheinfo *this_cpu_ci = get_cpu_cacheinfo(cpu);
+	unsigned int index = 0;
+
+	/* skip if of_node is already populated */
+	if (this_cpu_ci->info_list->of_node)
+		return 0;
+
+	if (!cpu_dev) {
+		pr_err("No cpu device for CPU %d\n", cpu);
+		return -ENODEV;
+	}
+	np = cpu_dev->of_node;
+	if (!np) {
+		pr_err("Failed to find cpu%d device node\n", cpu);
+		return -ENOENT;
+	}
+
+	while (np && index < cache_leaves(cpu)) {
+		this_leaf = this_cpu_ci->info_list + index;
+		if (this_leaf->level != 1)
+			np = of_find_next_cache_node(np);
+		else
+			np = of_node_get(np);/* cpu node itself */
+		this_leaf->of_node = np;
+		index++;
+	}
+	return 0;
+}
+
+static inline bool cache_leaves_are_shared(struct cacheinfo *this_leaf,
+					   struct cacheinfo *sib_leaf)
+{
+	return sib_leaf->of_node == this_leaf->of_node;
+}
+#else
+static inline int cache_setup_of_node(unsigned int cpu) { return 0; }
+static inline bool cache_leaves_are_shared(struct cacheinfo *this_leaf,
+					   struct cacheinfo *sib_leaf)
+{
+	/*
+	 * For non-DT systems, assume unique level 1 cache, system-wide
+	 * shared caches for all other levels. This will be used only if
+	 * arch specific code has not populated shared_cpu_map
+	 */
+	return !(this_leaf->level == 1);
+}
+#endif
+
+static int cache_shared_cpu_map_setup(unsigned int cpu)
+{
+	struct cpu_cacheinfo *this_cpu_ci = get_cpu_cacheinfo(cpu);
+	struct cacheinfo *this_leaf, *sib_leaf;
+	unsigned int index;
+	int ret;
+
+	ret = cache_setup_of_node(cpu);
+	if (ret)
+		return ret;
+
+	for (index = 0; index < cache_leaves(cpu); index++) {
+		unsigned int i;
+
+		this_leaf = this_cpu_ci->info_list + index;
+		/* skip if shared_cpu_map is already populated */
+		if (!cpumask_empty(&this_leaf->shared_cpu_map))
+			continue;
+
+		cpumask_set_cpu(cpu, &this_leaf->shared_cpu_map);
+		for_each_online_cpu(i) {
+			struct cpu_cacheinfo *sib_cpu_ci = get_cpu_cacheinfo(i);
+
+			if (i == cpu || !sib_cpu_ci->info_list)
+				continue;/* skip if itself or no cacheinfo */
+			sib_leaf = sib_cpu_ci->info_list + index;
+			if (cache_leaves_are_shared(this_leaf, sib_leaf)) {
+				cpumask_set_cpu(cpu, &sib_leaf->shared_cpu_map);
+				cpumask_set_cpu(i, &this_leaf->shared_cpu_map);
+			}
+		}
+	}
+
+	return 0;
+}
+
+static void cache_shared_cpu_map_remove(unsigned int cpu)
+{
+	struct cpu_cacheinfo *this_cpu_ci = get_cpu_cacheinfo(cpu);
+	struct cacheinfo *this_leaf, *sib_leaf;
+	unsigned int sibling, index;
+
+	for (index = 0; index < cache_leaves(cpu); index++) {
+		this_leaf = this_cpu_ci->info_list + index;
+		for_each_cpu(sibling, &this_leaf->shared_cpu_map) {
+			struct cpu_cacheinfo *sib_cpu_ci;
+
+			if (sibling == cpu) /* skip itself */
+				continue;
+			sib_cpu_ci = get_cpu_cacheinfo(sibling);
+			sib_leaf = sib_cpu_ci->info_list + index;
+			cpumask_clear_cpu(cpu, &sib_leaf->shared_cpu_map);
+			cpumask_clear_cpu(sibling, &this_leaf->shared_cpu_map);
+		}
+		of_node_put(this_leaf->of_node);
+	}
+}
+
+static void free_cache_attributes(unsigned int cpu)
+{
+	cache_shared_cpu_map_remove(cpu);
+
+	kfree(per_cpu_cacheinfo(cpu));
+	per_cpu_cacheinfo(cpu) = NULL;
+}
+
+int __weak init_cache_level(unsigned int cpu)
+{
+	return -ENOENT;
+}
+
+int __weak populate_cache_leaves(unsigned int cpu)
+{
+	return -ENOENT;
+}
+
+static int detect_cache_attributes(unsigned int cpu)
+{
+	int ret;
+
+	if (init_cache_level(cpu))
+		return -ENOENT;
+
+	per_cpu_cacheinfo(cpu) = kcalloc(cache_leaves(cpu),
+					 sizeof(struct cacheinfo), GFP_KERNEL);
+	if (per_cpu_cacheinfo(cpu) == NULL)
+		return -ENOMEM;
+
+	ret = populate_cache_leaves(cpu);
+	if (ret)
+		goto free_ci;
+	/*
+	 * For systems using DT for cache hierarcy, of_node and shared_cpu_map
+	 * will be set up here only if they are not populated already
+	 */
+	ret = cache_shared_cpu_map_setup(cpu);
+	if (ret)
+		goto free_ci;
+	return 0;
+
+free_ci:
+	free_cache_attributes(cpu);
+	return ret;
+}
+
+/* pointer to cpuX/cache device */
+static DEFINE_PER_CPU(struct device *, ci_cache_dev);
+#define per_cpu_cache_dev(cpu)	(per_cpu(ci_cache_dev, cpu))
+
+static cpumask_t cache_dev_map;
+
+/* pointer to array of devices for cpuX/cache/indexY */
+static DEFINE_PER_CPU(struct device **, ci_index_dev);
+#define per_cpu_index_dev(cpu)	(per_cpu(ci_index_dev, cpu))
+#define per_cache_index_dev(cpu, idx)	((per_cpu_index_dev(cpu))[idx])
+
+#define show_one(file_name, object)				\
+static ssize_t file_name##_show(struct device *dev,		\
+		struct device_attribute *attr, char *buf)	\
+{								\
+	struct cacheinfo *this_leaf = dev_get_drvdata(dev);	\
+	return sprintf(buf, "%u\n", this_leaf->object);		\
+}
+
+show_one(level, level);
+show_one(coherency_line_size, coherency_line_size);
+show_one(number_of_sets, number_of_sets);
+show_one(physical_line_partition, physical_line_partition);
+show_one(ways_of_associativity, ways_of_associativity);
+
+static ssize_t size_show(struct device *dev,
+			 struct device_attribute *attr, char *buf)
+{
+	struct cacheinfo *this_leaf = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%uK\n", this_leaf->size >> 10);
+}
+
+static ssize_t shared_cpumap_show_func(struct device *dev, bool list, char *buf)
+{
+	struct cacheinfo *this_leaf = dev_get_drvdata(dev);
+	const struct cpumask *mask = &this_leaf->shared_cpu_map;
+
+	return cpumap_print_to_pagebuf(list, buf, mask);
+}
+
+static ssize_t shared_cpu_map_show(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	return shared_cpumap_show_func(dev, false, buf);
+}
+
+static ssize_t shared_cpu_list_show(struct device *dev,
+				    struct device_attribute *attr, char *buf)
+{
+	return shared_cpumap_show_func(dev, true, buf);
+}
+
+static ssize_t type_show(struct device *dev,
+			 struct device_attribute *attr, char *buf)
+{
+	struct cacheinfo *this_leaf = dev_get_drvdata(dev);
+
+	switch (this_leaf->type) {
+	case CACHE_TYPE_DATA:
+		return sprintf(buf, "Data\n");
+	case CACHE_TYPE_INST:
+		return sprintf(buf, "Instruction\n");
+	case CACHE_TYPE_UNIFIED:
+		return sprintf(buf, "Unified\n");
+	default:
+		return -EINVAL;
+	}
+}
+
+static ssize_t allocation_policy_show(struct device *dev,
+				      struct device_attribute *attr, char *buf)
+{
+	struct cacheinfo *this_leaf = dev_get_drvdata(dev);
+	unsigned int ci_attr = this_leaf->attributes;
+	int n = 0;
+
+	if ((ci_attr & CACHE_READ_ALLOCATE) && (ci_attr & CACHE_WRITE_ALLOCATE))
+		n = sprintf(buf, "ReadWriteAllocate\n");
+	else if (ci_attr & CACHE_READ_ALLOCATE)
+		n = sprintf(buf, "ReadAllocate\n");
+	else if (ci_attr & CACHE_WRITE_ALLOCATE)
+		n = sprintf(buf, "WriteAllocate\n");
+	return n;
+}
+
+static ssize_t write_policy_show(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	struct cacheinfo *this_leaf = dev_get_drvdata(dev);
+	unsigned int ci_attr = this_leaf->attributes;
+	int n = 0;
+
+	if (ci_attr & CACHE_WRITE_THROUGH)
+		n = sprintf(buf, "WriteThrough\n");
+	else if (ci_attr & CACHE_WRITE_BACK)
+		n = sprintf(buf, "WriteBack\n");
+	return n;
+}
+
+static DEVICE_ATTR_RO(level);
+static DEVICE_ATTR_RO(type);
+static DEVICE_ATTR_RO(coherency_line_size);
+static DEVICE_ATTR_RO(ways_of_associativity);
+static DEVICE_ATTR_RO(number_of_sets);
+static DEVICE_ATTR_RO(size);
+static DEVICE_ATTR_RO(allocation_policy);
+static DEVICE_ATTR_RO(write_policy);
+static DEVICE_ATTR_RO(shared_cpu_map);
+static DEVICE_ATTR_RO(shared_cpu_list);
+static DEVICE_ATTR_RO(physical_line_partition);
+
+static struct attribute *cache_default_attrs[] = {
+	&dev_attr_type.attr,
+	&dev_attr_level.attr,
+	&dev_attr_shared_cpu_map.attr,
+	&dev_attr_shared_cpu_list.attr,
+	&dev_attr_coherency_line_size.attr,
+	&dev_attr_ways_of_associativity.attr,
+	&dev_attr_number_of_sets.attr,
+	&dev_attr_size.attr,
+	&dev_attr_allocation_policy.attr,
+	&dev_attr_write_policy.attr,
+	&dev_attr_physical_line_partition.attr,
+	NULL
+};
+
+static umode_t
+cache_default_attrs_is_visible(struct kobject *kobj,
+			       struct attribute *attr, int unused)
+{
+	struct device *dev = kobj_to_dev(kobj);
+	struct cacheinfo *this_leaf = dev_get_drvdata(dev);
+	const struct cpumask *mask = &this_leaf->shared_cpu_map;
+	umode_t mode = attr->mode;
+
+	if ((attr == &dev_attr_type.attr) && this_leaf->type)
+		return mode;
+	if ((attr == &dev_attr_level.attr) && this_leaf->level)
+		return mode;
+	if ((attr == &dev_attr_shared_cpu_map.attr) && !cpumask_empty(mask))
+		return mode;
+	if ((attr == &dev_attr_shared_cpu_list.attr) && !cpumask_empty(mask))
+		return mode;
+	if ((attr == &dev_attr_coherency_line_size.attr) &&
+	    this_leaf->coherency_line_size)
+		return mode;
+	if ((attr == &dev_attr_ways_of_associativity.attr) &&
+	    this_leaf->size) /* allow 0 = full associativity */
+		return mode;
+	if ((attr == &dev_attr_number_of_sets.attr) &&
+	    this_leaf->number_of_sets)
+		return mode;
+	if ((attr == &dev_attr_size.attr) && this_leaf->size)
+		return mode;
+	if ((attr == &dev_attr_write_policy.attr) &&
+	    (this_leaf->attributes & CACHE_WRITE_POLICY_MASK))
+		return mode;
+	if ((attr == &dev_attr_allocation_policy.attr) &&
+	    (this_leaf->attributes & CACHE_ALLOCATE_POLICY_MASK))
+		return mode;
+	if ((attr == &dev_attr_physical_line_partition.attr) &&
+	    this_leaf->physical_line_partition)
+		return mode;
+
+	return 0;
+}
+
+static const struct attribute_group cache_default_group = {
+	.attrs = cache_default_attrs,
+	.is_visible = cache_default_attrs_is_visible,
+};
+
+static const struct attribute_group *cache_default_groups[] = {
+	&cache_default_group,
+	NULL,
+};
+
+static const struct attribute_group *cache_private_groups[] = {
+	&cache_default_group,
+	NULL, /* Place holder for private group */
+	NULL,
+};
+
+const struct attribute_group *
+__weak cache_get_priv_group(struct cacheinfo *this_leaf)
+{
+	return NULL;
+}
+
+static const struct attribute_group **
+cache_get_attribute_groups(struct cacheinfo *this_leaf)
+{
+	const struct attribute_group *priv_group =
+			cache_get_priv_group(this_leaf);
+
+	if (!priv_group)
+		return cache_default_groups;
+
+	if (!cache_private_groups[1])
+		cache_private_groups[1] = priv_group;
+
+	return cache_private_groups;
+}
+
+/* Add/Remove cache interface for CPU device */
+static void cpu_cache_sysfs_exit(unsigned int cpu)
+{
+	int i;
+	struct device *ci_dev;
+
+	if (per_cpu_index_dev(cpu)) {
+		for (i = 0; i < cache_leaves(cpu); i++) {
+			ci_dev = per_cache_index_dev(cpu, i);
+			if (!ci_dev)
+				continue;
+			device_unregister(ci_dev);
+		}
+		kfree(per_cpu_index_dev(cpu));
+		per_cpu_index_dev(cpu) = NULL;
+	}
+	device_unregister(per_cpu_cache_dev(cpu));
+	per_cpu_cache_dev(cpu) = NULL;
+}
+
+static int cpu_cache_sysfs_init(unsigned int cpu)
+{
+	struct device *dev = get_cpu_device(cpu);
+
+	if (per_cpu_cacheinfo(cpu) == NULL)
+		return -ENOENT;
+
+	per_cpu_cache_dev(cpu) = cpu_device_create(dev, NULL, NULL, "cache");
+	if (IS_ERR(per_cpu_cache_dev(cpu)))
+		return PTR_ERR(per_cpu_cache_dev(cpu));
+
+	/* Allocate all required memory */
+	per_cpu_index_dev(cpu) = kcalloc(cache_leaves(cpu),
+					 sizeof(struct device *), GFP_KERNEL);
+	if (unlikely(per_cpu_index_dev(cpu) == NULL))
+		goto err_out;
+
+	return 0;
+
+err_out:
+	cpu_cache_sysfs_exit(cpu);
+	return -ENOMEM;
+}
+
+static int cache_add_dev(unsigned int cpu)
+{
+	unsigned int i;
+	int rc;
+	struct device *ci_dev, *parent;
+	struct cacheinfo *this_leaf;
+	struct cpu_cacheinfo *this_cpu_ci = get_cpu_cacheinfo(cpu);
+	const struct attribute_group **cache_groups;
+
+	rc = cpu_cache_sysfs_init(cpu);
+	if (unlikely(rc < 0))
+		return rc;
+
+	parent = per_cpu_cache_dev(cpu);
+	for (i = 0; i < cache_leaves(cpu); i++) {
+		this_leaf = this_cpu_ci->info_list + i;
+		if (this_leaf->disable_sysfs)
+			continue;
+		cache_groups = cache_get_attribute_groups(this_leaf);
+		ci_dev = cpu_device_create(parent, this_leaf, cache_groups,
+					   "index%1u", i);
+		if (IS_ERR(ci_dev)) {
+			rc = PTR_ERR(ci_dev);
+			goto err;
+		}
+		per_cache_index_dev(cpu, i) = ci_dev;
+	}
+	cpumask_set_cpu(cpu, &cache_dev_map);
+
+	return 0;
+err:
+	cpu_cache_sysfs_exit(cpu);
+	return rc;
+}
+
+static void cache_remove_dev(unsigned int cpu)
+{
+	if (!cpumask_test_cpu(cpu, &cache_dev_map))
+		return;
+	cpumask_clear_cpu(cpu, &cache_dev_map);
+
+	cpu_cache_sysfs_exit(cpu);
+}
+
+static int cacheinfo_cpu_callback(struct notifier_block *nfb,
+				  unsigned long action, void *hcpu)
+{
+	unsigned int cpu = (unsigned long)hcpu;
+	int rc = 0;
+
+	switch (action & ~CPU_TASKS_FROZEN) {
+	case CPU_ONLINE:
+		rc = detect_cache_attributes(cpu);
+		if (!rc)
+			rc = cache_add_dev(cpu);
+		break;
+	case CPU_DEAD:
+		cache_remove_dev(cpu);
+		if (per_cpu_cacheinfo(cpu))
+			free_cache_attributes(cpu);
+		break;
+	}
+	return notifier_from_errno(rc);
+}
+
+static int __init cacheinfo_sysfs_init(void)
+{
+	int cpu, rc = 0;
+
+	cpu_notifier_register_begin();
+
+	for_each_online_cpu(cpu) {
+		rc = detect_cache_attributes(cpu);
+		if (rc)
+			goto out;
+		rc = cache_add_dev(cpu);
+		if (rc) {
+			free_cache_attributes(cpu);
+			pr_err("error populating cacheinfo..cpu%d\n", cpu);
+			goto out;
+		}
+	}
+	__hotcpu_notifier(cacheinfo_cpu_callback, 0);
+
+out:
+	cpu_notifier_register_done();
+	return rc;
+}
+
+device_initcall(cacheinfo_sysfs_init);
diff --git a/drivers/base/core.c b/drivers/base/core.c
index 842d04707de6..97e2baf6e5d8 100644
--- a/drivers/base/core.c
+++ b/drivers/base/core.c
@@ -1021,18 +1021,6 @@ int device_add(struct device *dev)
 	if (error)
 		goto attrError;
 
-	if (MAJOR(dev->devt)) {
-		error = device_create_file(dev, &dev_attr_dev);
-		if (error)
-			goto ueventattrError;
-
-		error = device_create_sys_dev_entry(dev);
-		if (error)
-			goto devtattrError;
-
-		devtmpfs_create_node(dev);
-	}
-
 	error = device_add_class_symlinks(dev);
 	if (error)
 		goto SymlinkError;
@@ -1047,6 +1035,18 @@ int device_add(struct device *dev)
 		goto DPMError;
 	device_pm_add(dev);
 
+	if (MAJOR(dev->devt)) {
+		error = device_create_file(dev, &dev_attr_dev);
+		if (error)
+			goto DevAttrError;
+
+		error = device_create_sys_dev_entry(dev);
+		if (error)
+			goto SysEntryError;
+
+		devtmpfs_create_node(dev);
+	}
+
 	/* Notify clients of device addition.  This call must come
 	 * after dpm_sysfs_add() and before kobject_uevent().
 	 */
@@ -1076,6 +1076,12 @@ int device_add(struct device *dev)
 done:
 	put_device(dev);
 	return error;
+ SysEntryError:
+	if (MAJOR(dev->devt))
+		device_remove_file(dev, &dev_attr_dev);
+ DevAttrError:
+	device_pm_remove(dev);
+	dpm_sysfs_remove(dev);
  DPMError:
 	bus_remove_device(dev);
  BusError:
@@ -1083,14 +1089,6 @@ done:
  AttrsError:
 	device_remove_class_symlinks(dev);
  SymlinkError:
-	if (MAJOR(dev->devt))
-		devtmpfs_delete_node(dev);
-	if (MAJOR(dev->devt))
-		device_remove_sys_dev_entry(dev);
- devtattrError:
-	if (MAJOR(dev->devt))
-		device_remove_file(dev, &dev_attr_dev);
- ueventattrError:
 	device_remove_file(dev, &dev_attr_uevent);
  attrError:
 	kobject_uevent(&dev->kobj, KOBJ_REMOVE);
diff --git a/drivers/base/cpu.c b/drivers/base/cpu.c
index 006b1bc5297d..f829a4c71749 100644
--- a/drivers/base/cpu.c
+++ b/drivers/base/cpu.c
@@ -207,11 +207,8 @@ static ssize_t show_cpus_attr(struct device *dev,
 			      char *buf)
 {
 	struct cpu_attr *ca = container_of(attr, struct cpu_attr, attr);
-	int n = cpulist_scnprintf(buf, PAGE_SIZE-2, *(ca->map));
 
-	buf[n++] = '\n';
-	buf[n] = '\0';
-	return n;
+	return cpumap_print_to_pagebuf(true, buf, *ca->map);
 }
 
 #define _CPU_ATTR(name, map) \
@@ -366,6 +363,60 @@ struct device *get_cpu_device(unsigned cpu)
 }
 EXPORT_SYMBOL_GPL(get_cpu_device);
 
+static void device_create_release(struct device *dev)
+{
+	kfree(dev);
+}
+
+static struct device *
+__cpu_device_create(struct device *parent, void *drvdata,
+		    const struct attribute_group **groups,
+		    const char *fmt, va_list args)
+{
+	struct device *dev = NULL;
+	int retval = -ENODEV;
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		retval = -ENOMEM;
+		goto error;
+	}
+
+	device_initialize(dev);
+	dev->parent = parent;
+	dev->groups = groups;
+	dev->release = device_create_release;
+	dev_set_drvdata(dev, drvdata);
+
+	retval = kobject_set_name_vargs(&dev->kobj, fmt, args);
+	if (retval)
+		goto error;
+
+	retval = device_add(dev);
+	if (retval)
+		goto error;
+
+	return dev;
+
+error:
+	put_device(dev);
+	return ERR_PTR(retval);
+}
+
+struct device *cpu_device_create(struct device *parent, void *drvdata,
+				 const struct attribute_group **groups,
+				 const char *fmt, ...)
+{
+	va_list vargs;
+	struct device *dev;
+
+	va_start(vargs, fmt);
+	dev = __cpu_device_create(parent, drvdata, groups, fmt, vargs);
+	va_end(vargs);
+	return dev;
+}
+EXPORT_SYMBOL_GPL(cpu_device_create);
+
 #ifdef CONFIG_GENERIC_CPU_AUTOPROBE
 static DEVICE_ATTR(modalias, 0444, print_cpu_modalias, NULL);
 #endif
diff --git a/drivers/base/devcoredump.c b/drivers/base/devcoredump.c
index 96614b04544c..1bd120a0b084 100644
--- a/drivers/base/devcoredump.c
+++ b/drivers/base/devcoredump.c
@@ -31,6 +31,11 @@
 #include <linux/fs.h>
 #include <linux/workqueue.h>
 
+static struct class devcd_class;
+
+/* global disable flag, for security purposes */
+static bool devcd_disabled;
+
 /* if data isn't read by userspace after 5 minutes then delete it */
 #define DEVCD_TIMEOUT	(HZ * 60 * 5)
 
@@ -121,11 +126,51 @@ static const struct attribute_group *devcd_dev_groups[] = {
 	&devcd_dev_group, NULL,
 };
 
+static int devcd_free(struct device *dev, void *data)
+{
+	struct devcd_entry *devcd = dev_to_devcd(dev);
+
+	flush_delayed_work(&devcd->del_wk);
+	return 0;
+}
+
+static ssize_t disabled_show(struct class *class, struct class_attribute *attr,
+			     char *buf)
+{
+	return sprintf(buf, "%d\n", devcd_disabled);
+}
+
+static ssize_t disabled_store(struct class *class, struct class_attribute *attr,
+			      const char *buf, size_t count)
+{
+	long tmp = simple_strtol(buf, NULL, 10);
+
+	/*
+	 * This essentially makes the attribute write-once, since you can't
+	 * go back to not having it disabled. This is intentional, it serves
+	 * as a system lockdown feature.
+	 */
+	if (tmp != 1)
+		return -EINVAL;
+
+	devcd_disabled = true;
+
+	class_for_each_device(&devcd_class, NULL, NULL, devcd_free);
+
+	return count;
+}
+
+static struct class_attribute devcd_class_attrs[] = {
+	__ATTR_RW(disabled),
+	__ATTR_NULL
+};
+
 static struct class devcd_class = {
 	.name		= "devcoredump",
 	.owner		= THIS_MODULE,
 	.dev_release	= devcd_dev_release,
 	.dev_groups	= devcd_dev_groups,
+	.class_attrs	= devcd_class_attrs,
 };
 
 static ssize_t devcd_readv(char *buffer, loff_t offset, size_t count,
@@ -192,6 +237,9 @@ void dev_coredumpm(struct device *dev, struct module *owner,
 	struct devcd_entry *devcd;
 	struct device *existing;
 
+	if (devcd_disabled)
+		goto free;
+
 	existing = class_find_device(&devcd_class, NULL, dev,
 				     devcd_match_failing);
 	if (existing) {
@@ -249,14 +297,6 @@ static int __init devcoredump_init(void)
 }
 __initcall(devcoredump_init);
 
-static int devcd_free(struct device *dev, void *data)
-{
-	struct devcd_entry *devcd = dev_to_devcd(dev);
-
-	flush_delayed_work(&devcd->del_wk);
-	return 0;
-}
-
 static void __exit devcoredump_exit(void)
 {
 	class_for_each_device(&devcd_class, NULL, NULL, devcd_free);
diff --git a/drivers/base/firmware_class.c b/drivers/base/firmware_class.c
index 3d785ebb48d3..58470c395301 100644
--- a/drivers/base/firmware_class.c
+++ b/drivers/base/firmware_class.c
@@ -591,8 +591,7 @@ static int fw_map_pages_buf(struct firmware_buf *buf)
 	if (!buf->is_paged_buf)
 		return 0;
 
-	if (buf->data)
-		vunmap(buf->data);
+	vunmap(buf->data);
 	buf->data = vmap(buf->pages, buf->nr_pages, 0, PAGE_KERNEL_RO);
 	if (!buf->data)
 		return -ENOMEM;
@@ -925,7 +924,7 @@ static int _request_firmware_load(struct firmware_priv *fw_priv,
 		kobject_uevent(&fw_priv->dev.kobj, KOBJ_ADD);
 	}
 
-	wait_for_completion(&buf->completion);
+	retval = wait_for_completion_interruptible(&buf->completion);
 
 	cancel_delayed_work_sync(&fw_priv->timeout_work);
 	if (is_fw_load_aborted(buf))
@@ -1004,7 +1003,7 @@ static int sync_cached_firmware_buf(struct firmware_buf *buf)
 			break;
 		}
 		mutex_unlock(&fw_lock);
-		wait_for_completion(&buf->completion);
+		ret = wait_for_completion_interruptible(&buf->completion);
 		mutex_lock(&fw_lock);
 	}
 	mutex_unlock(&fw_lock);
diff --git a/drivers/base/memory.c b/drivers/base/memory.c
index 7c5d87191b28..85be040a21c8 100644
--- a/drivers/base/memory.c
+++ b/drivers/base/memory.c
@@ -228,8 +228,8 @@ memory_block_action(unsigned long phys_index, unsigned long action, int online_t
 	struct page *first_page;
 	int ret;
 
-	first_page = pfn_to_page(phys_index << PFN_SECTION_SHIFT);
-	start_pfn = page_to_pfn(first_page);
+	start_pfn = phys_index << PFN_SECTION_SHIFT;
+	first_page = pfn_to_page(start_pfn);
 
 	switch (action) {
 		case MEM_ONLINE:
diff --git a/drivers/base/node.c b/drivers/base/node.c
index 472168cd0c97..a3b82e9c7f20 100644
--- a/drivers/base/node.c
+++ b/drivers/base/node.c
@@ -25,32 +25,26 @@ static struct bus_type node_subsys = {
 };
 
 
-static ssize_t node_read_cpumap(struct device *dev, int type, char *buf)
+static ssize_t node_read_cpumap(struct device *dev, bool list, char *buf)
 {
 	struct node *node_dev = to_node(dev);
 	const struct cpumask *mask = cpumask_of_node(node_dev->dev.id);
-	int len;
 
 	/* 2008/04/07: buf currently PAGE_SIZE, need 9 chars per 32 bits. */
 	BUILD_BUG_ON((NR_CPUS/32 * 9) > (PAGE_SIZE-1));
 
-	len = type?
-		cpulist_scnprintf(buf, PAGE_SIZE-2, mask) :
-		cpumask_scnprintf(buf, PAGE_SIZE-2, mask);
- 	buf[len++] = '\n';
- 	buf[len] = '\0';
-	return len;
+	return cpumap_print_to_pagebuf(list, buf, mask);
 }
 
 static inline ssize_t node_read_cpumask(struct device *dev,
 				struct device_attribute *attr, char *buf)
 {
-	return node_read_cpumap(dev, 0, buf);
+	return node_read_cpumap(dev, false, buf);
 }
 static inline ssize_t node_read_cpulist(struct device *dev,
 				struct device_attribute *attr, char *buf)
 {
-	return node_read_cpumap(dev, 1, buf);
+	return node_read_cpumap(dev, true, buf);
 }
 
 static DEVICE_ATTR(cpumap,  S_IRUGO, node_read_cpumask, NULL);
diff --git a/drivers/base/platform.c b/drivers/base/platform.c
index 233ececd15a3..9421fed40905 100644
--- a/drivers/base/platform.c
+++ b/drivers/base/platform.c
@@ -580,9 +580,10 @@ void platform_driver_unregister(struct platform_driver *drv)
 EXPORT_SYMBOL_GPL(platform_driver_unregister);
 
 /**
- * platform_driver_probe - register driver for non-hotpluggable device
+ * __platform_driver_probe - register driver for non-hotpluggable device
  * @drv: platform driver structure
  * @probe: the driver probe routine, probably from an __init section
+ * @module: module which will be the owner of the driver
  *
  * Use this instead of platform_driver_register() when you know the device
  * is not hotpluggable and has already been registered, and you want to
@@ -598,8 +599,8 @@ EXPORT_SYMBOL_GPL(platform_driver_unregister);
  * Returns zero if the driver registered and bound to a device, else returns
  * a negative error code and with the driver not registered.
  */
-int __init_or_module platform_driver_probe(struct platform_driver *drv,
-		int (*probe)(struct platform_device *))
+int __init_or_module __platform_driver_probe(struct platform_driver *drv,
+		int (*probe)(struct platform_device *), struct module *module)
 {
 	int retval, code;
 
@@ -614,7 +615,7 @@ int __init_or_module platform_driver_probe(struct platform_driver *drv,
 
 	/* temporary section violation during probe() */
 	drv->probe = probe;
-	retval = code = platform_driver_register(drv);
+	retval = code = __platform_driver_register(drv, module);
 
 	/*
 	 * Fixup that section violation, being paranoid about code scanning
@@ -633,27 +634,28 @@ int __init_or_module platform_driver_probe(struct platform_driver *drv,
 		platform_driver_unregister(drv);
 	return retval;
 }
-EXPORT_SYMBOL_GPL(platform_driver_probe);
+EXPORT_SYMBOL_GPL(__platform_driver_probe);
 
 /**
- * platform_create_bundle - register driver and create corresponding device
+ * __platform_create_bundle - register driver and create corresponding device
  * @driver: platform driver structure
  * @probe: the driver probe routine, probably from an __init section
  * @res: set of resources that needs to be allocated for the device
  * @n_res: number of resources
  * @data: platform specific data for this platform device
  * @size: size of platform specific data
+ * @module: module which will be the owner of the driver
  *
  * Use this in legacy-style modules that probe hardware directly and
  * register a single platform device and corresponding platform driver.
  *
  * Returns &struct platform_device pointer on success, or ERR_PTR() on error.
  */
-struct platform_device * __init_or_module platform_create_bundle(
+struct platform_device * __init_or_module __platform_create_bundle(
 			struct platform_driver *driver,
 			int (*probe)(struct platform_device *),
 			struct resource *res, unsigned int n_res,
-			const void *data, size_t size)
+			const void *data, size_t size, struct module *module)
 {
 	struct platform_device *pdev;
 	int error;
@@ -676,7 +678,7 @@ struct platform_device * __init_or_module platform_create_bundle(
 	if (error)
 		goto err_pdev_put;
 
-	error = platform_driver_probe(driver, probe);
+	error = __platform_driver_probe(driver, probe, module);
 	if (error)
 		goto err_pdev_del;
 
@@ -689,7 +691,7 @@ err_pdev_put:
 err_out:
 	return ERR_PTR(error);
 }
-EXPORT_SYMBOL_GPL(platform_create_bundle);
+EXPORT_SYMBOL_GPL(__platform_create_bundle);
 
 /* modalias support enables more hands-off userspace setup:
  * (a) environment variable lets new-style hotplug events work once system is
diff --git a/drivers/base/power/domain.c b/drivers/base/power/domain.c
index 6a103a35ea9b..0d8780c04a5e 100644
--- a/drivers/base/power/domain.c
+++ b/drivers/base/power/domain.c
@@ -2088,7 +2088,7 @@ EXPORT_SYMBOL_GPL(of_genpd_del_provider);
  * Returns a valid pointer to struct generic_pm_domain on success or ERR_PTR()
  * on failure.
  */
-static struct generic_pm_domain *of_genpd_get_from_provider(
+struct generic_pm_domain *of_genpd_get_from_provider(
 					struct of_phandle_args *genpdspec)
 {
 	struct generic_pm_domain *genpd = ERR_PTR(-ENOENT);
@@ -2108,6 +2108,7 @@ static struct generic_pm_domain *of_genpd_get_from_provider(
 
 	return genpd;
 }
+EXPORT_SYMBOL_GPL(of_genpd_get_from_provider);
 
 /**
  * genpd_dev_pm_detach - Detach a device from its PM domain.
diff --git a/drivers/base/power/opp.c b/drivers/base/power/opp.c
index 2d195f3a1998..106c69359306 100644
--- a/drivers/base/power/opp.c
+++ b/drivers/base/power/opp.c
@@ -84,7 +84,11 @@ struct dev_pm_opp {
  *
  * This is an internal data structure maintaining the link to opps attached to
  * a device. This structure is not meant to be shared to users as it is
- * meant for book keeping and private to OPP library
+ * meant for book keeping and private to OPP library.
+ *
+ * Because the opp structures can be used from both rcu and srcu readers, we
+ * need to wait for the grace period of both of them before freeing any
+ * resources. And so we have used kfree_rcu() from within call_srcu() handlers.
  */
 struct device_opp {
 	struct list_head node;
@@ -104,6 +108,14 @@ static LIST_HEAD(dev_opp_list);
 /* Lock to allow exclusive modification to the device and opp lists */
 static DEFINE_MUTEX(dev_opp_list_lock);
 
+#define opp_rcu_lockdep_assert()					\
+do {									\
+	rcu_lockdep_assert(rcu_read_lock_held() ||			\
+				lockdep_is_held(&dev_opp_list_lock),	\
+			   "Missing rcu_read_lock() or "		\
+			   "dev_opp_list_lock protection");		\
+} while (0)
+
 /**
  * find_device_opp() - find device_opp struct using device pointer
  * @dev:	device pointer used to lookup device OPPs
@@ -204,9 +216,7 @@ EXPORT_SYMBOL_GPL(dev_pm_opp_get_freq);
  * This function returns the number of available opps if there are any,
  * else returns 0 if none or the corresponding error value.
  *
- * Locking: This function must be called under rcu_read_lock(). This function
- * internally references two RCU protected structures: device_opp and opp which
- * are safe as long as we are under a common RCU locked section.
+ * Locking: This function takes rcu_read_lock().
  */
 int dev_pm_opp_get_opp_count(struct device *dev)
 {
@@ -214,11 +224,14 @@ int dev_pm_opp_get_opp_count(struct device *dev)
 	struct dev_pm_opp *temp_opp;
 	int count = 0;
 
+	rcu_read_lock();
+
 	dev_opp = find_device_opp(dev);
 	if (IS_ERR(dev_opp)) {
-		int r = PTR_ERR(dev_opp);
-		dev_err(dev, "%s: device OPP not found (%d)\n", __func__, r);
-		return r;
+		count = PTR_ERR(dev_opp);
+		dev_err(dev, "%s: device OPP not found (%d)\n",
+			__func__, count);
+		goto out_unlock;
 	}
 
 	list_for_each_entry_rcu(temp_opp, &dev_opp->opp_list, node) {
@@ -226,6 +239,8 @@ int dev_pm_opp_get_opp_count(struct device *dev)
 			count++;
 	}
 
+out_unlock:
+	rcu_read_unlock();
 	return count;
 }
 EXPORT_SYMBOL_GPL(dev_pm_opp_get_opp_count);
@@ -263,6 +278,8 @@ struct dev_pm_opp *dev_pm_opp_find_freq_exact(struct device *dev,
 	struct device_opp *dev_opp;
 	struct dev_pm_opp *temp_opp, *opp = ERR_PTR(-ERANGE);
 
+	opp_rcu_lockdep_assert();
+
 	dev_opp = find_device_opp(dev);
 	if (IS_ERR(dev_opp)) {
 		int r = PTR_ERR(dev_opp);
@@ -309,6 +326,8 @@ struct dev_pm_opp *dev_pm_opp_find_freq_ceil(struct device *dev,
 	struct device_opp *dev_opp;
 	struct dev_pm_opp *temp_opp, *opp = ERR_PTR(-ERANGE);
 
+	opp_rcu_lockdep_assert();
+
 	if (!dev || !freq) {
 		dev_err(dev, "%s: Invalid argument freq=%p\n", __func__, freq);
 		return ERR_PTR(-EINVAL);
@@ -357,6 +376,8 @@ struct dev_pm_opp *dev_pm_opp_find_freq_floor(struct device *dev,
 	struct device_opp *dev_opp;
 	struct dev_pm_opp *temp_opp, *opp = ERR_PTR(-ERANGE);
 
+	opp_rcu_lockdep_assert();
+
 	if (!dev || !freq) {
 		dev_err(dev, "%s: Invalid argument freq=%p\n", __func__, freq);
 		return ERR_PTR(-EINVAL);
@@ -382,12 +403,34 @@ struct dev_pm_opp *dev_pm_opp_find_freq_floor(struct device *dev,
 }
 EXPORT_SYMBOL_GPL(dev_pm_opp_find_freq_floor);
 
+static struct device_opp *add_device_opp(struct device *dev)
+{
+	struct device_opp *dev_opp;
+
+	/*
+	 * Allocate a new device OPP table. In the infrequent case where a new
+	 * device is needed to be added, we pay this penalty.
+	 */
+	dev_opp = kzalloc(sizeof(*dev_opp), GFP_KERNEL);
+	if (!dev_opp)
+		return NULL;
+
+	dev_opp->dev = dev;
+	srcu_init_notifier_head(&dev_opp->srcu_head);
+	INIT_LIST_HEAD(&dev_opp->opp_list);
+
+	/* Secure the device list modification */
+	list_add_rcu(&dev_opp->node, &dev_opp_list);
+	return dev_opp;
+}
+
 static int dev_pm_opp_add_dynamic(struct device *dev, unsigned long freq,
 				  unsigned long u_volt, bool dynamic)
 {
 	struct device_opp *dev_opp = NULL;
 	struct dev_pm_opp *opp, *new_opp;
 	struct list_head *head;
+	int ret;
 
 	/* allocate new OPP node */
 	new_opp = kzalloc(sizeof(*new_opp), GFP_KERNEL);
@@ -400,7 +443,6 @@ static int dev_pm_opp_add_dynamic(struct device *dev, unsigned long freq,
 	mutex_lock(&dev_opp_list_lock);
 
 	/* populate the opp table */
-	new_opp->dev_opp = dev_opp;
 	new_opp->rate = freq;
 	new_opp->u_volt = u_volt;
 	new_opp->available = true;
@@ -409,27 +451,12 @@ static int dev_pm_opp_add_dynamic(struct device *dev, unsigned long freq,
 	/* Check for existing list for 'dev' */
 	dev_opp = find_device_opp(dev);
 	if (IS_ERR(dev_opp)) {
-		/*
-		 * Allocate a new device OPP table. In the infrequent case
-		 * where a new device is needed to be added, we pay this
-		 * penalty.
-		 */
-		dev_opp = kzalloc(sizeof(struct device_opp), GFP_KERNEL);
+		dev_opp = add_device_opp(dev);
 		if (!dev_opp) {
-			mutex_unlock(&dev_opp_list_lock);
-			kfree(new_opp);
-			dev_warn(dev,
-				"%s: Unable to create device OPP structure\n",
-				__func__);
-			return -ENOMEM;
+			ret = -ENOMEM;
+			goto free_opp;
 		}
 
-		dev_opp->dev = dev;
-		srcu_init_notifier_head(&dev_opp->srcu_head);
-		INIT_LIST_HEAD(&dev_opp->opp_list);
-
-		/* Secure the device list modification */
-		list_add_rcu(&dev_opp->node, &dev_opp_list);
 		head = &dev_opp->opp_list;
 		goto list_add;
 	}
@@ -448,18 +475,17 @@ static int dev_pm_opp_add_dynamic(struct device *dev, unsigned long freq,
 
 	/* Duplicate OPPs ? */
 	if (new_opp->rate == opp->rate) {
-		int ret = opp->available && new_opp->u_volt == opp->u_volt ?
+		ret = opp->available && new_opp->u_volt == opp->u_volt ?
 			0 : -EEXIST;
 
 		dev_warn(dev, "%s: duplicate OPPs detected. Existing: freq: %lu, volt: %lu, enabled: %d. New: freq: %lu, volt: %lu, enabled: %d\n",
 			 __func__, opp->rate, opp->u_volt, opp->available,
 			 new_opp->rate, new_opp->u_volt, new_opp->available);
-		mutex_unlock(&dev_opp_list_lock);
-		kfree(new_opp);
-		return ret;
+		goto free_opp;
 	}
 
 list_add:
+	new_opp->dev_opp = dev_opp;
 	list_add_rcu(&new_opp->node, head);
 	mutex_unlock(&dev_opp_list_lock);
 
@@ -469,6 +495,11 @@ list_add:
 	 */
 	srcu_notifier_call_chain(&dev_opp->srcu_head, OPP_EVENT_ADD, new_opp);
 	return 0;
+
+free_opp:
+	mutex_unlock(&dev_opp_list_lock);
+	kfree(new_opp);
+	return ret;
 }
 
 /**
@@ -511,10 +542,11 @@ static void kfree_device_rcu(struct rcu_head *head)
 {
 	struct device_opp *device_opp = container_of(head, struct device_opp, rcu_head);
 
-	kfree(device_opp);
+	kfree_rcu(device_opp, rcu_head);
 }
 
-void __dev_pm_opp_remove(struct device_opp *dev_opp, struct dev_pm_opp *opp)
+static void __dev_pm_opp_remove(struct device_opp *dev_opp,
+				struct dev_pm_opp *opp)
 {
 	/*
 	 * Notify the changes in the availability of the operable
@@ -592,7 +624,7 @@ EXPORT_SYMBOL_GPL(dev_pm_opp_remove);
 static int opp_set_availability(struct device *dev, unsigned long freq,
 		bool availability_req)
 {
-	struct device_opp *tmp_dev_opp, *dev_opp = ERR_PTR(-ENODEV);
+	struct device_opp *dev_opp;
 	struct dev_pm_opp *new_opp, *tmp_opp, *opp = ERR_PTR(-ENODEV);
 	int r = 0;
 
@@ -606,12 +638,7 @@ static int opp_set_availability(struct device *dev, unsigned long freq,
 	mutex_lock(&dev_opp_list_lock);
 
 	/* Find the device_opp */
-	list_for_each_entry(tmp_dev_opp, &dev_opp_list, node) {
-		if (dev == tmp_dev_opp->dev) {
-			dev_opp = tmp_dev_opp;
-			break;
-		}
-	}
+	dev_opp = find_device_opp(dev);
 	if (IS_ERR(dev_opp)) {
 		r = PTR_ERR(dev_opp);
 		dev_warn(dev, "%s: Device OPP not found (%d)\n", __func__, r);
@@ -768,14 +795,20 @@ EXPORT_SYMBOL_GPL(of_init_opp_table);
  */
 void of_free_opp_table(struct device *dev)
 {
-	struct device_opp *dev_opp = find_device_opp(dev);
+	struct device_opp *dev_opp;
 	struct dev_pm_opp *opp, *tmp;
 
 	/* Check for existing list for 'dev' */
 	dev_opp = find_device_opp(dev);
-	if (WARN(IS_ERR(dev_opp), "%s: dev_opp: %ld\n", dev_name(dev),
-		 PTR_ERR(dev_opp)))
+	if (IS_ERR(dev_opp)) {
+		int error = PTR_ERR(dev_opp);
+		if (error != -ENODEV)
+			WARN(1, "%s: dev_opp: %d\n",
+			     IS_ERR_OR_NULL(dev) ?
+					"Invalid device" : dev_name(dev),
+			     error);
 		return;
+	}
 
 	/* Hold our list modification lock here */
 	mutex_lock(&dev_opp_list_lock);
diff --git a/drivers/base/topology.c b/drivers/base/topology.c
index be7c1fb7c0c9..6491f45200a7 100644
--- a/drivers/base/topology.c
+++ b/drivers/base/topology.c
@@ -29,75 +29,52 @@
 #include <linux/hardirq.h>
 #include <linux/topology.h>
 
-#define define_one_ro_named(_name, _func)				\
-	static DEVICE_ATTR(_name, 0444, _func, NULL)
-
-#define define_one_ro(_name)				\
-	static DEVICE_ATTR(_name, 0444, show_##_name, NULL)
-
 #define define_id_show_func(name)				\
-static ssize_t show_##name(struct device *dev,			\
+static ssize_t name##_show(struct device *dev,			\
 		struct device_attribute *attr, char *buf)	\
 {								\
 	return sprintf(buf, "%d\n", topology_##name(dev->id));	\
 }
 
-#if defined(topology_thread_cpumask) || defined(topology_core_cpumask) || \
-    defined(topology_book_cpumask)
-static ssize_t show_cpumap(int type, const struct cpumask *mask, char *buf)
-{
-	ptrdiff_t len = PTR_ALIGN(buf + PAGE_SIZE - 1, PAGE_SIZE) - buf;
-	int n = 0;
-
-	if (len > 1) {
-		n = type?
-			cpulist_scnprintf(buf, len-2, mask) :
-			cpumask_scnprintf(buf, len-2, mask);
-		buf[n++] = '\n';
-		buf[n] = '\0';
-	}
-	return n;
-}
-#endif
-
-#define define_siblings_show_map(name)					\
-static ssize_t show_##name(struct device *dev,				\
+#define define_siblings_show_map(name, mask)				\
+static ssize_t name##_show(struct device *dev,				\
 			   struct device_attribute *attr, char *buf)	\
 {									\
-	return show_cpumap(0, topology_##name(dev->id), buf);		\
+	return cpumap_print_to_pagebuf(false, buf, topology_##mask(dev->id));\
 }
 
-#define define_siblings_show_list(name)					\
-static ssize_t show_##name##_list(struct device *dev,			\
-				  struct device_attribute *attr,	\
-				  char *buf)				\
+#define define_siblings_show_list(name, mask)				\
+static ssize_t name##_list_show(struct device *dev,			\
+				struct device_attribute *attr,		\
+				char *buf)				\
 {									\
-	return show_cpumap(1, topology_##name(dev->id), buf);		\
+	return cpumap_print_to_pagebuf(true, buf, topology_##mask(dev->id));\
 }
 
-#define define_siblings_show_func(name)		\
-	define_siblings_show_map(name); define_siblings_show_list(name)
+#define define_siblings_show_func(name, mask)	\
+	define_siblings_show_map(name, mask);	\
+	define_siblings_show_list(name, mask)
 
 define_id_show_func(physical_package_id);
-define_one_ro(physical_package_id);
+static DEVICE_ATTR_RO(physical_package_id);
 
 define_id_show_func(core_id);
-define_one_ro(core_id);
+static DEVICE_ATTR_RO(core_id);
 
-define_siblings_show_func(thread_cpumask);
-define_one_ro_named(thread_siblings, show_thread_cpumask);
-define_one_ro_named(thread_siblings_list, show_thread_cpumask_list);
+define_siblings_show_func(thread_siblings, thread_cpumask);
+static DEVICE_ATTR_RO(thread_siblings);
+static DEVICE_ATTR_RO(thread_siblings_list);
 
-define_siblings_show_func(core_cpumask);
-define_one_ro_named(core_siblings, show_core_cpumask);
-define_one_ro_named(core_siblings_list, show_core_cpumask_list);
+define_siblings_show_func(core_siblings, core_cpumask);
+static DEVICE_ATTR_RO(core_siblings);
+static DEVICE_ATTR_RO(core_siblings_list);
 
 #ifdef CONFIG_SCHED_BOOK
 define_id_show_func(book_id);
-define_one_ro(book_id);
-define_siblings_show_func(book_cpumask);
-define_one_ro_named(book_siblings, show_book_cpumask);
-define_one_ro_named(book_siblings_list, show_book_cpumask_list);
+static DEVICE_ATTR_RO(book_id);
+define_siblings_show_func(book_siblings, book_cpumask);
+static DEVICE_ATTR_RO(book_siblings);
+static DEVICE_ATTR_RO(book_siblings_list);
 #endif
 
 static struct attribute *default_attrs[] = {
diff --git a/drivers/bcma/driver_chipcommon.c b/drivers/bcma/driver_chipcommon.c
index 19f679667ca4..84d4a95e6caf 100644
--- a/drivers/bcma/driver_chipcommon.c
+++ b/drivers/bcma/driver_chipcommon.c
@@ -79,7 +79,9 @@ static int bcma_chipco_watchdog_ticks_per_ms(struct bcma_drv_cc *cc)
 
 	if (cc->capabilities & BCMA_CC_CAP_PMU) {
 		if (bus->chipinfo.id == BCMA_CHIP_ID_BCM4706)
-			/* 4706 CC and PMU watchdogs are clocked at 1/4 of ALP clock */
+			/* 4706 CC and PMU watchdogs are clocked at 1/4 of ALP
+			 * clock
+			 */
 			return bcma_chipco_get_alp_clock(cc) / 4000;
 		else
 			/* based on 32KHz ILP clock */
@@ -97,7 +99,8 @@ int bcma_chipco_watchdog_register(struct bcma_drv_cc *cc)
 	wdt.driver_data = cc;
 	wdt.timer_set = bcma_chipco_watchdog_timer_set_wdt;
 	wdt.timer_set_ms = bcma_chipco_watchdog_timer_set_ms_wdt;
-	wdt.max_timer_ms = bcma_chipco_watchdog_get_max_timer(cc) / cc->ticks_per_ms;
+	wdt.max_timer_ms =
+		bcma_chipco_watchdog_get_max_timer(cc) / cc->ticks_per_ms;
 
 	pdev = platform_device_register_data(NULL, "bcm47xx-wdt",
 					     cc->core->bus->num, &wdt,
@@ -335,7 +338,8 @@ void bcma_chipco_serial_init(struct bcma_drv_cc *cc)
 				       | BCMA_CC_CORECTL_UARTCLKEN);
 		}
 	} else {
-		bcma_err(cc->core->bus, "serial not supported on this device ccrev: 0x%x\n", ccrev);
+		bcma_err(cc->core->bus, "serial not supported on this device ccrev: 0x%x\n",
+			 ccrev);
 		return;
 	}
 
diff --git a/drivers/bcma/driver_mips.c b/drivers/bcma/driver_mips.c
index 5ec69c3d409d..04faf6df959f 100644
--- a/drivers/bcma/driver_mips.c
+++ b/drivers/bcma/driver_mips.c
@@ -20,6 +20,9 @@
 #include <linux/serial_core.h>
 #include <linux/serial_reg.h>
 #include <linux/time.h>
+#ifdef CONFIG_BCM47XX
+#include <bcm47xx_nvram.h>
+#endif
 
 enum bcma_boot_dev {
 	BCMA_BOOT_DEV_UNK = 0,
@@ -316,10 +319,16 @@ static void bcma_core_mips_flash_detect(struct bcma_drv_mips *mcore)
 	switch (boot_dev) {
 	case BCMA_BOOT_DEV_PARALLEL:
 	case BCMA_BOOT_DEV_SERIAL:
-		/* TODO: Init NVRAM using BCMA_SOC_FLASH2 window */
+#ifdef CONFIG_BCM47XX
+		bcm47xx_nvram_init_from_mem(BCMA_SOC_FLASH2,
+					    BCMA_SOC_FLASH2_SZ);
+#endif
 		break;
 	case BCMA_BOOT_DEV_NAND:
-		/* TODO: Init NVRAM using BCMA_SOC_FLASH1 window */
+#ifdef CONFIG_BCM47XX
+		bcm47xx_nvram_init_from_mem(BCMA_SOC_FLASH1,
+					    BCMA_SOC_FLASH1_SZ);
+#endif
 		break;
 	default:
 		break;
diff --git a/drivers/block/amiflop.c b/drivers/block/amiflop.c
index 758da2287d9a..5fd50a284168 100644
--- a/drivers/block/amiflop.c
+++ b/drivers/block/amiflop.c
@@ -1864,7 +1864,6 @@ static int __exit amiga_floppy_remove(struct platform_device *pdev)
 static struct platform_driver amiga_floppy_driver = {
 	.driver   = {
 		.name	= "amiga-floppy",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/block/aoe/aoeblk.c b/drivers/block/aoe/aoeblk.c
index dd73e1ff1759..46c282fff104 100644
--- a/drivers/block/aoe/aoeblk.c
+++ b/drivers/block/aoe/aoeblk.c
@@ -395,7 +395,7 @@ aoeblk_gdalloc(void *vp)
 	WARN_ON(d->flags & DEVFL_TKILL);
 	WARN_ON(d->gd);
 	WARN_ON(d->flags & DEVFL_UP);
-	blk_queue_max_hw_sectors(q, BLK_DEF_MAX_SECTORS);
+	blk_queue_max_hw_sectors(q, 1024);
 	q->backing_dev_info.name = "aoe";
 	q->backing_dev_info.ra_pages = READ_AHEAD / PAGE_CACHE_SIZE;
 	d->bufpool = mp;
diff --git a/drivers/block/drbd/drbd_actlog.c b/drivers/block/drbd/drbd_actlog.c
index a2dfa169237d..1318e3217cb0 100644
--- a/drivers/block/drbd/drbd_actlog.c
+++ b/drivers/block/drbd/drbd_actlog.c
@@ -827,8 +827,7 @@ static int update_sync_bits(struct drbd_device *device,
  *
  */
 int __drbd_change_sync(struct drbd_device *device, sector_t sector, int size,
-		enum update_sync_bits_mode mode,
-		const char *file, const unsigned int line)
+		enum update_sync_bits_mode mode)
 {
 	/* Is called from worker and receiver context _only_ */
 	unsigned long sbnr, ebnr, lbnr;
diff --git a/drivers/block/drbd/drbd_int.h b/drivers/block/drbd/drbd_int.h
index 9b22f8f01b57..b905e9888b88 100644
--- a/drivers/block/drbd/drbd_int.h
+++ b/drivers/block/drbd/drbd_int.h
@@ -1454,7 +1454,6 @@ extern int is_valid_ar_handle(struct drbd_request *, sector_t);
 
 
 /* drbd_nl.c */
-extern int drbd_msg_put_info(struct sk_buff *skb, const char *info);
 extern void drbd_suspend_io(struct drbd_device *device);
 extern void drbd_resume_io(struct drbd_device *device);
 extern char *ppsize(char *buf, unsigned long long size);
@@ -1558,52 +1557,31 @@ extern void drbd_set_recv_tcq(struct drbd_device *device, int tcq_enabled);
 extern void _drbd_clear_done_ee(struct drbd_device *device, struct list_head *to_be_freed);
 extern int drbd_connected(struct drbd_peer_device *);
 
-/* Yes, there is kernel_setsockopt, but only since 2.6.18.
- * So we have our own copy of it here. */
-static inline int drbd_setsockopt(struct socket *sock, int level, int optname,
-				  char *optval, int optlen)
-{
-	mm_segment_t oldfs = get_fs();
-	char __user *uoptval;
-	int err;
-
-	uoptval = (char __user __force *)optval;
-
-	set_fs(KERNEL_DS);
-	if (level == SOL_SOCKET)
-		err = sock_setsockopt(sock, level, optname, uoptval, optlen);
-	else
-		err = sock->ops->setsockopt(sock, level, optname, uoptval,
-					    optlen);
-	set_fs(oldfs);
-	return err;
-}
-
 static inline void drbd_tcp_cork(struct socket *sock)
 {
 	int val = 1;
-	(void) drbd_setsockopt(sock, SOL_TCP, TCP_CORK,
+	(void) kernel_setsockopt(sock, SOL_TCP, TCP_CORK,
 			(char*)&val, sizeof(val));
 }
 
 static inline void drbd_tcp_uncork(struct socket *sock)
 {
 	int val = 0;
-	(void) drbd_setsockopt(sock, SOL_TCP, TCP_CORK,
+	(void) kernel_setsockopt(sock, SOL_TCP, TCP_CORK,
 			(char*)&val, sizeof(val));
 }
 
 static inline void drbd_tcp_nodelay(struct socket *sock)
 {
 	int val = 1;
-	(void) drbd_setsockopt(sock, SOL_TCP, TCP_NODELAY,
+	(void) kernel_setsockopt(sock, SOL_TCP, TCP_NODELAY,
 			(char*)&val, sizeof(val));
 }
 
 static inline void drbd_tcp_quickack(struct socket *sock)
 {
 	int val = 2;
-	(void) drbd_setsockopt(sock, SOL_TCP, TCP_QUICKACK,
+	(void) kernel_setsockopt(sock, SOL_TCP, TCP_QUICKACK,
 			(char*)&val, sizeof(val));
 }
 
@@ -1662,14 +1640,13 @@ extern void drbd_advance_rs_marks(struct drbd_device *device, unsigned long stil
 
 enum update_sync_bits_mode { RECORD_RS_FAILED, SET_OUT_OF_SYNC, SET_IN_SYNC };
 extern int __drbd_change_sync(struct drbd_device *device, sector_t sector, int size,
-		enum update_sync_bits_mode mode,
-		const char *file, const unsigned int line);
+		enum update_sync_bits_mode mode);
 #define drbd_set_in_sync(device, sector, size) \
-	__drbd_change_sync(device, sector, size, SET_IN_SYNC, __FILE__, __LINE__)
+	__drbd_change_sync(device, sector, size, SET_IN_SYNC)
 #define drbd_set_out_of_sync(device, sector, size) \
-	__drbd_change_sync(device, sector, size, SET_OUT_OF_SYNC, __FILE__, __LINE__)
+	__drbd_change_sync(device, sector, size, SET_OUT_OF_SYNC)
 #define drbd_rs_failed_io(device, sector, size) \
-	__drbd_change_sync(device, sector, size, RECORD_RS_FAILED, __FILE__, __LINE__)
+	__drbd_change_sync(device, sector, size, RECORD_RS_FAILED)
 extern void drbd_al_shrink(struct drbd_device *device);
 extern int drbd_initialize_al(struct drbd_device *, void *);
 
diff --git a/drivers/block/drbd/drbd_main.c b/drivers/block/drbd/drbd_main.c
index 973c185c9cfe..1fc83427199c 100644
--- a/drivers/block/drbd/drbd_main.c
+++ b/drivers/block/drbd/drbd_main.c
@@ -2532,10 +2532,6 @@ int set_resource_options(struct drbd_resource *resource, struct res_opts *res_op
 
 	if (!zalloc_cpumask_var(&new_cpu_mask, GFP_KERNEL))
 		return -ENOMEM;
-		/*
-		retcode = ERR_NOMEM;
-		drbd_msg_put_info("unable to allocate cpumask");
-		*/
 
 	/* silently ignore cpu mask on UP kernel */
 	if (nr_cpu_ids > 1 && res_opts->cpu_mask[0] != 0) {
@@ -2731,7 +2727,7 @@ enum drbd_ret_code drbd_create_device(struct drbd_config_context *adm_ctx, unsig
 
 	device = minor_to_device(minor);
 	if (device)
-		return ERR_MINOR_EXISTS;
+		return ERR_MINOR_OR_VOLUME_EXISTS;
 
 	/* GFP_KERNEL, we are outside of all write-out paths */
 	device = kzalloc(sizeof(struct drbd_device), GFP_KERNEL);
@@ -2793,20 +2789,16 @@ enum drbd_ret_code drbd_create_device(struct drbd_config_context *adm_ctx, unsig
 
 	id = idr_alloc(&drbd_devices, device, minor, minor + 1, GFP_KERNEL);
 	if (id < 0) {
-		if (id == -ENOSPC) {
-			err = ERR_MINOR_EXISTS;
-			drbd_msg_put_info(adm_ctx->reply_skb, "requested minor exists already");
-		}
+		if (id == -ENOSPC)
+			err = ERR_MINOR_OR_VOLUME_EXISTS;
 		goto out_no_minor_idr;
 	}
 	kref_get(&device->kref);
 
 	id = idr_alloc(&resource->devices, device, vnr, vnr + 1, GFP_KERNEL);
 	if (id < 0) {
-		if (id == -ENOSPC) {
-			err = ERR_MINOR_EXISTS;
-			drbd_msg_put_info(adm_ctx->reply_skb, "requested minor exists already");
-		}
+		if (id == -ENOSPC)
+			err = ERR_MINOR_OR_VOLUME_EXISTS;
 		goto out_idr_remove_minor;
 	}
 	kref_get(&device->kref);
@@ -2825,10 +2817,8 @@ enum drbd_ret_code drbd_create_device(struct drbd_config_context *adm_ctx, unsig
 
 		id = idr_alloc(&connection->peer_devices, peer_device, vnr, vnr + 1, GFP_KERNEL);
 		if (id < 0) {
-			if (id == -ENOSPC) {
+			if (id == -ENOSPC)
 				err = ERR_INVALID_REQUEST;
-				drbd_msg_put_info(adm_ctx->reply_skb, "requested volume exists already");
-			}
 			goto out_idr_remove_from_resource;
 		}
 		kref_get(&connection->kref);
@@ -2836,7 +2826,6 @@ enum drbd_ret_code drbd_create_device(struct drbd_config_context *adm_ctx, unsig
 
 	if (init_submitter(device)) {
 		err = ERR_NOMEM;
-		drbd_msg_put_info(adm_ctx->reply_skb, "unable to create submit workqueue");
 		goto out_idr_remove_vol;
 	}
 
diff --git a/drivers/block/drbd/drbd_nl.c b/drivers/block/drbd/drbd_nl.c
index 1cd47df44bda..74df8cfad414 100644
--- a/drivers/block/drbd/drbd_nl.c
+++ b/drivers/block/drbd/drbd_nl.c
@@ -92,7 +92,7 @@ static void drbd_adm_send_reply(struct sk_buff *skb, struct genl_info *info)
 
 /* Used on a fresh "drbd_adm_prepare"d reply_skb, this cannot fail: The only
  * reason it could fail was no space in skb, and there are 4k available. */
-int drbd_msg_put_info(struct sk_buff *skb, const char *info)
+static int drbd_msg_put_info(struct sk_buff *skb, const char *info)
 {
 	struct nlattr *nla;
 	int err = -EMSGSIZE;
@@ -588,7 +588,7 @@ drbd_set_role(struct drbd_device *const device, enum drbd_role new_role, int for
 	val.i  = 0; val.role  = new_role;
 
 	while (try++ < max_tries) {
-		rv = _drbd_request_state(device, mask, val, CS_WAIT_COMPLETE);
+		rv = _drbd_request_state_holding_state_mutex(device, mask, val, CS_WAIT_COMPLETE);
 
 		/* in case we first succeeded to outdate,
 		 * but now suddenly could establish a connection */
@@ -2052,7 +2052,7 @@ check_net_options(struct drbd_connection *connection, struct net_conf *new_net_c
 	rv = _check_net_options(connection, rcu_dereference(connection->net_conf), new_net_conf);
 	rcu_read_unlock();
 
-	/* connection->volumes protected by genl_lock() here */
+	/* connection->peer_devices protected by genl_lock() here */
 	idr_for_each_entry(&connection->peer_devices, peer_device, i) {
 		struct drbd_device *device = peer_device->device;
 		if (!device->bitmap) {
@@ -3483,7 +3483,7 @@ int drbd_adm_new_minor(struct sk_buff *skb, struct genl_info *info)
 	 * that first_peer_device(device)->connection and device->vnr match the request. */
 	if (adm_ctx.device) {
 		if (info->nlhdr->nlmsg_flags & NLM_F_EXCL)
-			retcode = ERR_MINOR_EXISTS;
+			retcode = ERR_MINOR_OR_VOLUME_EXISTS;
 		/* else: still NO_ERROR */
 		goto out;
 	}
@@ -3530,6 +3530,27 @@ out:
 	return 0;
 }
 
+static int adm_del_resource(struct drbd_resource *resource)
+{
+	struct drbd_connection *connection;
+
+	for_each_connection(connection, resource) {
+		if (connection->cstate > C_STANDALONE)
+			return ERR_NET_CONFIGURED;
+	}
+	if (!idr_is_empty(&resource->devices))
+		return ERR_RES_IN_USE;
+
+	list_del_rcu(&resource->resources);
+	/* Make sure all threads have actually stopped: state handling only
+	 * does drbd_thread_stop_nowait(). */
+	list_for_each_entry(connection, &resource->connections, connections)
+		drbd_thread_stop(&connection->worker);
+	synchronize_rcu();
+	drbd_free_resource(resource);
+	return NO_ERROR;
+}
+
 int drbd_adm_down(struct sk_buff *skb, struct genl_info *info)
 {
 	struct drbd_config_context adm_ctx;
@@ -3575,14 +3596,6 @@ int drbd_adm_down(struct sk_buff *skb, struct genl_info *info)
 		}
 	}
 
-	/* If we reach this, all volumes (of this connection) are Secondary,
-	 * Disconnected, Diskless, aka Unconfigured. Make sure all threads have
-	 * actually stopped, state handling only does drbd_thread_stop_nowait(). */
-	for_each_connection(connection, resource)
-		drbd_thread_stop(&connection->worker);
-
-	/* Now, nothing can fail anymore */
-
 	/* delete volumes */
 	idr_for_each_entry(&resource->devices, device, i) {
 		retcode = adm_del_minor(device);
@@ -3593,10 +3606,7 @@ int drbd_adm_down(struct sk_buff *skb, struct genl_info *info)
 		}
 	}
 
-	list_del_rcu(&resource->resources);
-	synchronize_rcu();
-	drbd_free_resource(resource);
-	retcode = NO_ERROR;
+	retcode = adm_del_resource(resource);
 out:
 	mutex_unlock(&resource->adm_mutex);
 finish:
@@ -3608,7 +3618,6 @@ int drbd_adm_del_resource(struct sk_buff *skb, struct genl_info *info)
 {
 	struct drbd_config_context adm_ctx;
 	struct drbd_resource *resource;
-	struct drbd_connection *connection;
 	enum drbd_ret_code retcode;
 
 	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_RESOURCE);
@@ -3616,27 +3625,10 @@ int drbd_adm_del_resource(struct sk_buff *skb, struct genl_info *info)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto finish;
-
 	resource = adm_ctx.resource;
-	mutex_lock(&resource->adm_mutex);
-	for_each_connection(connection, resource) {
-		if (connection->cstate > C_STANDALONE) {
-			retcode = ERR_NET_CONFIGURED;
-			goto out;
-		}
-	}
-	if (!idr_is_empty(&resource->devices)) {
-		retcode = ERR_RES_IN_USE;
-		goto out;
-	}
 
-	list_del_rcu(&resource->resources);
-	for_each_connection(connection, resource)
-		drbd_thread_stop(&connection->worker);
-	synchronize_rcu();
-	drbd_free_resource(resource);
-	retcode = NO_ERROR;
-out:
+	mutex_lock(&resource->adm_mutex);
+	retcode = adm_del_resource(resource);
 	mutex_unlock(&resource->adm_mutex);
 finish:
 	drbd_adm_finish(&adm_ctx, info, retcode);
diff --git a/drivers/block/drbd/drbd_receiver.c b/drivers/block/drbd/drbd_receiver.c
index 6960fb064731..d169b4a79267 100644
--- a/drivers/block/drbd/drbd_receiver.c
+++ b/drivers/block/drbd/drbd_receiver.c
@@ -2482,7 +2482,7 @@ bool drbd_rs_c_min_rate_throttle(struct drbd_device *device)
 			atomic_read(&device->rs_sect_ev);
 
 	if (atomic_read(&device->ap_actlog_cnt)
-	    || !device->rs_last_events || curr_events - device->rs_last_events > 64) {
+	    || curr_events - device->rs_last_events > 64) {
 		unsigned long rs_left;
 		int i;
 
diff --git a/drivers/block/drbd/drbd_req.c b/drivers/block/drbd/drbd_req.c
index 5a01c53dddeb..34f2f0ba409b 100644
--- a/drivers/block/drbd/drbd_req.c
+++ b/drivers/block/drbd/drbd_req.c
@@ -36,29 +36,15 @@ static bool drbd_may_do_local_read(struct drbd_device *device, sector_t sector,
 /* Update disk stats at start of I/O request */
 static void _drbd_start_io_acct(struct drbd_device *device, struct drbd_request *req)
 {
-	const int rw = bio_data_dir(req->master_bio);
-	int cpu;
-	cpu = part_stat_lock();
-	part_round_stats(cpu, &device->vdisk->part0);
-	part_stat_inc(cpu, &device->vdisk->part0, ios[rw]);
-	part_stat_add(cpu, &device->vdisk->part0, sectors[rw], req->i.size >> 9);
-	(void) cpu; /* The macro invocations above want the cpu argument, I do not like
-		       the compiler warning about cpu only assigned but never used... */
-	part_inc_in_flight(&device->vdisk->part0, rw);
-	part_stat_unlock();
+	generic_start_io_acct(bio_data_dir(req->master_bio), req->i.size >> 9,
+			      &device->vdisk->part0);
 }
 
 /* Update disk stats when completing request upwards */
 static void _drbd_end_io_acct(struct drbd_device *device, struct drbd_request *req)
 {
-	int rw = bio_data_dir(req->master_bio);
-	unsigned long duration = jiffies - req->start_jif;
-	int cpu;
-	cpu = part_stat_lock();
-	part_stat_add(cpu, &device->vdisk->part0, ticks[rw], duration);
-	part_round_stats(cpu, &device->vdisk->part0);
-	part_dec_in_flight(&device->vdisk->part0, rw);
-	part_stat_unlock();
+	generic_end_io_acct(bio_data_dir(req->master_bio),
+			    &device->vdisk->part0, req->start_jif);
 }
 
 static struct drbd_request *drbd_req_new(struct drbd_device *device,
@@ -1545,6 +1531,7 @@ int drbd_merge_bvec(struct request_queue *q, struct bvec_merge_data *bvm, struct
 		struct request_queue * const b =
 			device->ldev->backing_bdev->bd_disk->queue;
 		if (b->merge_bvec_fn) {
+			bvm->bi_bdev = device->ldev->backing_bdev;
 			backing_limit = b->merge_bvec_fn(b, bvm, bvec);
 			limit = min(limit, backing_limit);
 		}
@@ -1628,7 +1615,7 @@ void request_timer_fn(unsigned long data)
 		 time_after(now, req_peer->pre_send_jif + ent) &&
 		!time_in_range(now, connection->last_reconnect_jif, connection->last_reconnect_jif + ent)) {
 		drbd_warn(device, "Remote failed to finish a request within ko-count * timeout\n");
-		_drbd_set_state(_NS(device, conn, C_TIMEOUT), CS_VERBOSE | CS_HARD, NULL);
+		_conn_request_state(connection, NS(conn, C_TIMEOUT), CS_VERBOSE | CS_HARD);
 	}
 	if (dt && oldest_submit_jif != now &&
 		 time_after(now, oldest_submit_jif + dt) &&
diff --git a/drivers/block/drbd/drbd_state.c b/drivers/block/drbd/drbd_state.c
index 84b11f887d73..2d7dd269b6a8 100644
--- a/drivers/block/drbd/drbd_state.c
+++ b/drivers/block/drbd/drbd_state.c
@@ -215,6 +215,18 @@ static bool no_peer_wf_report_params(struct drbd_connection *connection)
 	return rv;
 }
 
+static void wake_up_all_devices(struct drbd_connection *connection)
+{
+	struct drbd_peer_device *peer_device;
+	int vnr;
+
+	rcu_read_lock();
+	idr_for_each_entry(&connection->peer_devices, peer_device, vnr)
+		wake_up(&peer_device->device->state_wait);
+	rcu_read_unlock();
+
+}
+
 
 /**
  * cl_wide_st_chg() - true if the state change is a cluster wide one
@@ -410,6 +422,22 @@ _drbd_request_state(struct drbd_device *device, union drbd_state mask,
 	return rv;
 }
 
+enum drbd_state_rv
+_drbd_request_state_holding_state_mutex(struct drbd_device *device, union drbd_state mask,
+		    union drbd_state val, enum chg_state_flags f)
+{
+	enum drbd_state_rv rv;
+
+	BUG_ON(f & CS_SERIALIZE);
+
+	wait_event_cmd(device->state_wait,
+		       (rv = drbd_req_state(device, mask, val, f)) != SS_IN_TRANSIENT_STATE,
+		       mutex_unlock(device->state_mutex),
+		       mutex_lock(device->state_mutex));
+
+	return rv;
+}
+
 static void print_st(struct drbd_device *device, const char *name, union drbd_state ns)
 {
 	drbd_err(device, " %s = { cs:%s ro:%s/%s ds:%s/%s %c%c%c%c%c%c }\n",
@@ -629,14 +657,11 @@ is_valid_soft_transition(union drbd_state os, union drbd_state ns, struct drbd_c
 	if (ns.conn == C_DISCONNECTING && os.conn == C_UNCONNECTED)
 		rv = SS_IN_TRANSIENT_STATE;
 
-	/* if (ns.conn == os.conn && ns.conn == C_WF_REPORT_PARAMS)
-	   rv = SS_IN_TRANSIENT_STATE; */
-
 	/* While establishing a connection only allow cstate to change.
-	   Delay/refuse role changes, detach attach etc... */
+	   Delay/refuse role changes, detach attach etc... (they do not touch cstate) */
 	if (test_bit(STATE_SENT, &connection->flags) &&
-	    !(os.conn == C_WF_REPORT_PARAMS ||
-	      (ns.conn == C_WF_REPORT_PARAMS && os.conn == C_WF_CONNECTION)))
+	    !((ns.conn == C_WF_REPORT_PARAMS && os.conn == C_WF_CONNECTION) ||
+	      (ns.conn >= C_CONNECTED && os.conn == C_WF_REPORT_PARAMS)))
 		rv = SS_IN_TRANSIENT_STATE;
 
 	if ((ns.conn == C_VERIFY_S || ns.conn == C_VERIFY_T) && os.conn < C_CONNECTED)
@@ -1032,8 +1057,10 @@ __drbd_set_state(struct drbd_device *device, union drbd_state ns,
 
 	/* Wake up role changes, that were delayed because of connection establishing */
 	if (os.conn == C_WF_REPORT_PARAMS && ns.conn != C_WF_REPORT_PARAMS &&
-	    no_peer_wf_report_params(connection))
+	    no_peer_wf_report_params(connection)) {
 		clear_bit(STATE_SENT, &connection->flags);
+		wake_up_all_devices(connection);
+	}
 
 	wake_up(&device->misc_wait);
 	wake_up(&device->state_wait);
@@ -1072,7 +1099,6 @@ __drbd_set_state(struct drbd_device *device, union drbd_state ns,
 
 		set_ov_position(device, ns.conn);
 		device->rs_start = now;
-		device->rs_last_events = 0;
 		device->rs_last_sect_ev = 0;
 		device->ov_last_oos_size = 0;
 		device->ov_last_oos_start = 0;
diff --git a/drivers/block/drbd/drbd_state.h b/drivers/block/drbd/drbd_state.h
index cc41605ba21c..7f53c40823cd 100644
--- a/drivers/block/drbd/drbd_state.h
+++ b/drivers/block/drbd/drbd_state.h
@@ -117,6 +117,11 @@ extern enum drbd_state_rv _drbd_request_state(struct drbd_device *,
 					      union drbd_state,
 					      union drbd_state,
 					      enum chg_state_flags);
+
+extern enum drbd_state_rv
+_drbd_request_state_holding_state_mutex(struct drbd_device *, union drbd_state,
+					union drbd_state, enum chg_state_flags);
+
 extern enum drbd_state_rv __drbd_set_state(struct drbd_device *, union drbd_state,
 					   enum chg_state_flags,
 					   struct completion *done);
diff --git a/drivers/block/drbd/drbd_worker.c b/drivers/block/drbd/drbd_worker.c
index d2d1f97511bd..d0fae55d871d 100644
--- a/drivers/block/drbd/drbd_worker.c
+++ b/drivers/block/drbd/drbd_worker.c
@@ -1592,11 +1592,15 @@ void drbd_resync_after_changed(struct drbd_device *device)
 
 void drbd_rs_controller_reset(struct drbd_device *device)
 {
+	struct gendisk *disk = device->ldev->backing_bdev->bd_contains->bd_disk;
 	struct fifo_buffer *plan;
 
 	atomic_set(&device->rs_sect_in, 0);
 	atomic_set(&device->rs_sect_ev, 0);
 	device->rs_in_flight = 0;
+	device->rs_last_events =
+		(int)part_stat_read(&disk->part0, sectors[0]) +
+		(int)part_stat_read(&disk->part0, sectors[1]);
 
 	/* Updating the RCU protected object in place is necessary since
 	   this function gets called from atomic context.
@@ -1743,7 +1747,6 @@ void drbd_start_resync(struct drbd_device *device, enum drbd_conns side)
 		device->rs_failed    = 0;
 		device->rs_paused    = 0;
 		device->rs_same_csum = 0;
-		device->rs_last_events = 0;
 		device->rs_last_sect_ev = 0;
 		device->rs_total     = tw;
 		device->rs_start     = now;
diff --git a/drivers/block/mg_disk.c b/drivers/block/mg_disk.c
index e352cac707e8..145ce2aa2e78 100644
--- a/drivers/block/mg_disk.c
+++ b/drivers/block/mg_disk.c
@@ -1082,7 +1082,6 @@ static struct platform_driver mg_disk_driver = {
 	.remove = mg_remove,
 	.driver = {
 		.name = MG_DEV_NAME,
-		.owner = THIS_MODULE,
 		.pm = &mg_pm,
 	}
 };
diff --git a/drivers/block/mtip32xx/mtip32xx.c b/drivers/block/mtip32xx/mtip32xx.c
index 1bd5f523f8fd..3bd7ca9853a8 100644
--- a/drivers/block/mtip32xx/mtip32xx.c
+++ b/drivers/block/mtip32xx/mtip32xx.c
@@ -3775,9 +3775,10 @@ static bool mtip_check_unal_depth(struct blk_mq_hw_ctx *hctx,
 	return false;
 }
 
-static int mtip_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *rq,
-		bool last)
+static int mtip_queue_rq(struct blk_mq_hw_ctx *hctx,
+			 const struct blk_mq_queue_data *bd)
 {
+	struct request *rq = bd->rq;
 	int ret;
 
 	if (unlikely(mtip_check_unal_depth(hctx, rq)))
diff --git a/drivers/block/null_blk.c b/drivers/block/null_blk.c
index 8001e812018b..ae9f615382f6 100644
--- a/drivers/block/null_blk.c
+++ b/drivers/block/null_blk.c
@@ -78,7 +78,33 @@ module_param(home_node, int, S_IRUGO);
 MODULE_PARM_DESC(home_node, "Home node for the device");
 
 static int queue_mode = NULL_Q_MQ;
-module_param(queue_mode, int, S_IRUGO);
+
+static int null_param_store_val(const char *str, int *val, int min, int max)
+{
+	int ret, new_val;
+
+	ret = kstrtoint(str, 10, &new_val);
+	if (ret)
+		return -EINVAL;
+
+	if (new_val < min || new_val > max)
+		return -EINVAL;
+
+	*val = new_val;
+	return 0;
+}
+
+static int null_set_queue_mode(const char *str, const struct kernel_param *kp)
+{
+	return null_param_store_val(str, &queue_mode, NULL_Q_BIO, NULL_Q_MQ);
+}
+
+static struct kernel_param_ops null_queue_mode_param_ops = {
+	.set	= null_set_queue_mode,
+	.get	= param_get_int,
+};
+
+device_param_cb(queue_mode, &null_queue_mode_param_ops, &queue_mode, S_IRUGO);
 MODULE_PARM_DESC(queue_mode, "Block interface to use (0=bio,1=rq,2=multiqueue)");
 
 static int gb = 250;
@@ -94,7 +120,19 @@ module_param(nr_devices, int, S_IRUGO);
 MODULE_PARM_DESC(nr_devices, "Number of devices to register");
 
 static int irqmode = NULL_IRQ_SOFTIRQ;
-module_param(irqmode, int, S_IRUGO);
+
+static int null_set_irqmode(const char *str, const struct kernel_param *kp)
+{
+	return null_param_store_val(str, &irqmode, NULL_IRQ_NONE,
+					NULL_IRQ_TIMER);
+}
+
+static struct kernel_param_ops null_irqmode_param_ops = {
+	.set	= null_set_irqmode,
+	.get	= param_get_int,
+};
+
+device_param_cb(irqmode, &null_irqmode_param_ops, &irqmode, S_IRUGO);
 MODULE_PARM_DESC(irqmode, "IRQ completion handler. 0-none, 1-softirq, 2-timer");
 
 static int completion_nsec = 10000;
@@ -313,15 +351,15 @@ static void null_request_fn(struct request_queue *q)
 	}
 }
 
-static int null_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *rq,
-		bool last)
+static int null_queue_rq(struct blk_mq_hw_ctx *hctx,
+			 const struct blk_mq_queue_data *bd)
 {
-	struct nullb_cmd *cmd = blk_mq_rq_to_pdu(rq);
+	struct nullb_cmd *cmd = blk_mq_rq_to_pdu(bd->rq);
 
-	cmd->rq = rq;
+	cmd->rq = bd->rq;
 	cmd->nq = hctx->driver_data;
 
-	blk_mq_start_request(rq);
+	blk_mq_start_request(bd->rq);
 
 	null_handle_cmd(cmd);
 	return BLK_MQ_RQ_QUEUE_OK;
diff --git a/drivers/block/nvme-core.c b/drivers/block/nvme-core.c
index e2bb8afbeae5..b1d5d8797315 100644
--- a/drivers/block/nvme-core.c
+++ b/drivers/block/nvme-core.c
@@ -13,9 +13,9 @@
  */
 
 #include <linux/nvme.h>
-#include <linux/bio.h>
 #include <linux/bitops.h>
 #include <linux/blkdev.h>
+#include <linux/blk-mq.h>
 #include <linux/cpu.h>
 #include <linux/delay.h>
 #include <linux/errno.h>
@@ -33,7 +33,6 @@
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/pci.h>
-#include <linux/percpu.h>
 #include <linux/poison.h>
 #include <linux/ptrace.h>
 #include <linux/sched.h>
@@ -42,12 +41,12 @@
 #include <scsi/sg.h>
 #include <asm-generic/io-64-nonatomic-lo-hi.h>
 
-#include <trace/events/block.h>
-
 #define NVME_Q_DEPTH		1024
+#define NVME_AQ_DEPTH		64
 #define SQ_SIZE(depth)		(depth * sizeof(struct nvme_command))
 #define CQ_SIZE(depth)		(depth * sizeof(struct nvme_completion))
 #define ADMIN_TIMEOUT		(admin_timeout * HZ)
+#define SHUTDOWN_TIMEOUT	(shutdown_timeout * HZ)
 #define IOD_TIMEOUT		(retry_time * HZ)
 
 static unsigned char admin_timeout = 60;
@@ -62,6 +61,10 @@ static unsigned char retry_time = 30;
 module_param(retry_time, byte, 0644);
 MODULE_PARM_DESC(retry_time, "time in seconds to retry failed I/O");
 
+static unsigned char shutdown_timeout = 5;
+module_param(shutdown_timeout, byte, 0644);
+MODULE_PARM_DESC(shutdown_timeout, "timeout in seconds for controller shutdown");
+
 static int nvme_major;
 module_param(nvme_major, int, 0);
 
@@ -76,10 +79,12 @@ static wait_queue_head_t nvme_kthread_wait;
 static struct notifier_block nvme_nb;
 
 static void nvme_reset_failed_dev(struct work_struct *ws);
+static int nvme_process_cq(struct nvme_queue *nvmeq);
 
 struct async_cmd_info {
 	struct kthread_work work;
 	struct kthread_worker *worker;
+	struct request *req;
 	u32 result;
 	int status;
 	void *ctx;
@@ -90,7 +95,7 @@ struct async_cmd_info {
  * commands and one for I/O commands).
  */
 struct nvme_queue {
-	struct rcu_head r_head;
+	struct llist_node node;
 	struct device *q_dmadev;
 	struct nvme_dev *dev;
 	char irqname[24];	/* nvme4294967295-65535\0 */
@@ -99,10 +104,6 @@ struct nvme_queue {
 	volatile struct nvme_completion *cqes;
 	dma_addr_t sq_dma_addr;
 	dma_addr_t cq_dma_addr;
-	wait_queue_head_t sq_full;
-	wait_queue_t sq_cong_wait;
-	struct bio_list sq_cong;
-	struct list_head iod_bio;
 	u32 __iomem *q_db;
 	u16 q_depth;
 	u16 cq_vector;
@@ -112,10 +113,8 @@ struct nvme_queue {
 	u16 qid;
 	u8 cq_phase;
 	u8 cqe_seen;
-	u8 q_suspended;
-	cpumask_var_t cpu_mask;
 	struct async_cmd_info cmdinfo;
-	unsigned long cmdid_data[];
+	struct blk_mq_hw_ctx *hctx;
 };
 
 /*
@@ -143,62 +142,79 @@ typedef void (*nvme_completion_fn)(struct nvme_queue *, void *,
 struct nvme_cmd_info {
 	nvme_completion_fn fn;
 	void *ctx;
-	unsigned long timeout;
 	int aborted;
+	struct nvme_queue *nvmeq;
 };
 
-static struct nvme_cmd_info *nvme_cmd_info(struct nvme_queue *nvmeq)
+static int nvme_admin_init_hctx(struct blk_mq_hw_ctx *hctx, void *data,
+				unsigned int hctx_idx)
 {
-	return (void *)&nvmeq->cmdid_data[BITS_TO_LONGS(nvmeq->q_depth)];
+	struct nvme_dev *dev = data;
+	struct nvme_queue *nvmeq = dev->queues[0];
+
+	WARN_ON(nvmeq->hctx);
+	nvmeq->hctx = hctx;
+	hctx->driver_data = nvmeq;
+	return 0;
 }
 
-static unsigned nvme_queue_extra(int depth)
+static int nvme_admin_init_request(void *data, struct request *req,
+				unsigned int hctx_idx, unsigned int rq_idx,
+				unsigned int numa_node)
 {
-	return DIV_ROUND_UP(depth, 8) + (depth * sizeof(struct nvme_cmd_info));
+	struct nvme_dev *dev = data;
+	struct nvme_cmd_info *cmd = blk_mq_rq_to_pdu(req);
+	struct nvme_queue *nvmeq = dev->queues[0];
+
+	BUG_ON(!nvmeq);
+	cmd->nvmeq = nvmeq;
+	return 0;
 }
 
-/**
- * alloc_cmdid() - Allocate a Command ID
- * @nvmeq: The queue that will be used for this command
- * @ctx: A pointer that will be passed to the handler
- * @handler: The function to call on completion
- *
- * Allocate a Command ID for a queue.  The data passed in will
- * be passed to the completion handler.  This is implemented by using
- * the bottom two bits of the ctx pointer to store the handler ID.
- * Passing in a pointer that's not 4-byte aligned will cause a BUG.
- * We can change this if it becomes a problem.
- *
- * May be called with local interrupts disabled and the q_lock held,
- * or with interrupts enabled and no locks held.
- */
-static int alloc_cmdid(struct nvme_queue *nvmeq, void *ctx,
-				nvme_completion_fn handler, unsigned timeout)
+static void nvme_exit_hctx(struct blk_mq_hw_ctx *hctx, unsigned int hctx_idx)
 {
-	int depth = nvmeq->q_depth - 1;
-	struct nvme_cmd_info *info = nvme_cmd_info(nvmeq);
-	int cmdid;
+	struct nvme_queue *nvmeq = hctx->driver_data;
 
-	do {
-		cmdid = find_first_zero_bit(nvmeq->cmdid_data, depth);
-		if (cmdid >= depth)
-			return -EBUSY;
-	} while (test_and_set_bit(cmdid, nvmeq->cmdid_data));
+	nvmeq->hctx = NULL;
+}
+
+static int nvme_init_hctx(struct blk_mq_hw_ctx *hctx, void *data,
+			  unsigned int hctx_idx)
+{
+	struct nvme_dev *dev = data;
+	struct nvme_queue *nvmeq = dev->queues[
+					(hctx_idx % dev->queue_count) + 1];
+
+	if (!nvmeq->hctx)
+		nvmeq->hctx = hctx;
 
-	info[cmdid].fn = handler;
-	info[cmdid].ctx = ctx;
-	info[cmdid].timeout = jiffies + timeout;
-	info[cmdid].aborted = 0;
-	return cmdid;
+	/* nvmeq queues are shared between namespaces. We assume here that
+	 * blk-mq map the tags so they match up with the nvme queue tags. */
+	WARN_ON(nvmeq->hctx->tags != hctx->tags);
+
+	hctx->driver_data = nvmeq;
+	return 0;
 }
 
-static int alloc_cmdid_killable(struct nvme_queue *nvmeq, void *ctx,
-				nvme_completion_fn handler, unsigned timeout)
+static int nvme_init_request(void *data, struct request *req,
+				unsigned int hctx_idx, unsigned int rq_idx,
+				unsigned int numa_node)
 {
-	int cmdid;
-	wait_event_killable(nvmeq->sq_full,
-		(cmdid = alloc_cmdid(nvmeq, ctx, handler, timeout)) >= 0);
-	return (cmdid < 0) ? -EINTR : cmdid;
+	struct nvme_dev *dev = data;
+	struct nvme_cmd_info *cmd = blk_mq_rq_to_pdu(req);
+	struct nvme_queue *nvmeq = dev->queues[hctx_idx + 1];
+
+	BUG_ON(!nvmeq);
+	cmd->nvmeq = nvmeq;
+	return 0;
+}
+
+static void nvme_set_info(struct nvme_cmd_info *cmd, void *ctx,
+				nvme_completion_fn handler)
+{
+	cmd->fn = handler;
+	cmd->ctx = ctx;
+	cmd->aborted = 0;
 }
 
 /* Special values must be less than 0x1000 */
@@ -206,17 +222,12 @@ static int alloc_cmdid_killable(struct nvme_queue *nvmeq, void *ctx,
 #define CMD_CTX_CANCELLED	(0x30C + CMD_CTX_BASE)
 #define CMD_CTX_COMPLETED	(0x310 + CMD_CTX_BASE)
 #define CMD_CTX_INVALID		(0x314 + CMD_CTX_BASE)
-#define CMD_CTX_ABORT		(0x318 + CMD_CTX_BASE)
 
 static void special_completion(struct nvme_queue *nvmeq, void *ctx,
 						struct nvme_completion *cqe)
 {
 	if (ctx == CMD_CTX_CANCELLED)
 		return;
-	if (ctx == CMD_CTX_ABORT) {
-		++nvmeq->dev->abort_limit;
-		return;
-	}
 	if (ctx == CMD_CTX_COMPLETED) {
 		dev_warn(nvmeq->q_dmadev,
 				"completed id %d twice on queue %d\n",
@@ -229,99 +240,89 @@ static void special_completion(struct nvme_queue *nvmeq, void *ctx,
 				cqe->command_id, le16_to_cpup(&cqe->sq_id));
 		return;
 	}
-
 	dev_warn(nvmeq->q_dmadev, "Unknown special completion %p\n", ctx);
 }
 
-static void async_completion(struct nvme_queue *nvmeq, void *ctx,
-						struct nvme_completion *cqe)
-{
-	struct async_cmd_info *cmdinfo = ctx;
-	cmdinfo->result = le32_to_cpup(&cqe->result);
-	cmdinfo->status = le16_to_cpup(&cqe->status) >> 1;
-	queue_kthread_work(cmdinfo->worker, &cmdinfo->work);
-}
-
-/*
- * Called with local interrupts disabled and the q_lock held.  May not sleep.
- */
-static void *free_cmdid(struct nvme_queue *nvmeq, int cmdid,
-						nvme_completion_fn *fn)
+static void *cancel_cmd_info(struct nvme_cmd_info *cmd, nvme_completion_fn *fn)
 {
 	void *ctx;
-	struct nvme_cmd_info *info = nvme_cmd_info(nvmeq);
 
-	if (cmdid >= nvmeq->q_depth || !info[cmdid].fn) {
-		if (fn)
-			*fn = special_completion;
-		return CMD_CTX_INVALID;
-	}
 	if (fn)
-		*fn = info[cmdid].fn;
-	ctx = info[cmdid].ctx;
-	info[cmdid].fn = special_completion;
-	info[cmdid].ctx = CMD_CTX_COMPLETED;
-	clear_bit(cmdid, nvmeq->cmdid_data);
-	wake_up(&nvmeq->sq_full);
+		*fn = cmd->fn;
+	ctx = cmd->ctx;
+	cmd->fn = special_completion;
+	cmd->ctx = CMD_CTX_CANCELLED;
 	return ctx;
 }
 
-static void *cancel_cmdid(struct nvme_queue *nvmeq, int cmdid,
-						nvme_completion_fn *fn)
+static void async_req_completion(struct nvme_queue *nvmeq, void *ctx,
+						struct nvme_completion *cqe)
 {
-	void *ctx;
-	struct nvme_cmd_info *info = nvme_cmd_info(nvmeq);
-	if (fn)
-		*fn = info[cmdid].fn;
-	ctx = info[cmdid].ctx;
-	info[cmdid].fn = special_completion;
-	info[cmdid].ctx = CMD_CTX_CANCELLED;
-	return ctx;
-}
+	struct request *req = ctx;
 
-static struct nvme_queue *raw_nvmeq(struct nvme_dev *dev, int qid)
-{
-	return rcu_dereference_raw(dev->queues[qid]);
+	u32 result = le32_to_cpup(&cqe->result);
+	u16 status = le16_to_cpup(&cqe->status) >> 1;
+
+	if (status == NVME_SC_SUCCESS || status == NVME_SC_ABORT_REQ)
+		++nvmeq->dev->event_limit;
+	if (status == NVME_SC_SUCCESS)
+		dev_warn(nvmeq->q_dmadev,
+			"async event result %08x\n", result);
+
+	blk_mq_free_hctx_request(nvmeq->hctx, req);
 }
 
-static struct nvme_queue *get_nvmeq(struct nvme_dev *dev) __acquires(RCU)
+static void abort_completion(struct nvme_queue *nvmeq, void *ctx,
+						struct nvme_completion *cqe)
 {
-	struct nvme_queue *nvmeq;
-	unsigned queue_id = get_cpu_var(*dev->io_queue);
+	struct request *req = ctx;
 
-	rcu_read_lock();
-	nvmeq = rcu_dereference(dev->queues[queue_id]);
-	if (nvmeq)
-		return nvmeq;
+	u16 status = le16_to_cpup(&cqe->status) >> 1;
+	u32 result = le32_to_cpup(&cqe->result);
 
-	rcu_read_unlock();
-	put_cpu_var(*dev->io_queue);
-	return NULL;
+	blk_mq_free_hctx_request(nvmeq->hctx, req);
+
+	dev_warn(nvmeq->q_dmadev, "Abort status:%x result:%x", status, result);
+	++nvmeq->dev->abort_limit;
 }
 
-static void put_nvmeq(struct nvme_queue *nvmeq) __releases(RCU)
+static void async_completion(struct nvme_queue *nvmeq, void *ctx,
+						struct nvme_completion *cqe)
 {
-	rcu_read_unlock();
-	put_cpu_var(nvmeq->dev->io_queue);
+	struct async_cmd_info *cmdinfo = ctx;
+	cmdinfo->result = le32_to_cpup(&cqe->result);
+	cmdinfo->status = le16_to_cpup(&cqe->status) >> 1;
+	queue_kthread_work(cmdinfo->worker, &cmdinfo->work);
+	blk_mq_free_hctx_request(nvmeq->hctx, cmdinfo->req);
 }
 
-static struct nvme_queue *lock_nvmeq(struct nvme_dev *dev, int q_idx)
-							__acquires(RCU)
+static inline struct nvme_cmd_info *get_cmd_from_tag(struct nvme_queue *nvmeq,
+				  unsigned int tag)
 {
-	struct nvme_queue *nvmeq;
+	struct blk_mq_hw_ctx *hctx = nvmeq->hctx;
+	struct request *req = blk_mq_tag_to_rq(hctx->tags, tag);
 
-	rcu_read_lock();
-	nvmeq = rcu_dereference(dev->queues[q_idx]);
-	if (nvmeq)
-		return nvmeq;
-
-	rcu_read_unlock();
-	return NULL;
+	return blk_mq_rq_to_pdu(req);
 }
 
-static void unlock_nvmeq(struct nvme_queue *nvmeq) __releases(RCU)
+/*
+ * Called with local interrupts disabled and the q_lock held.  May not sleep.
+ */
+static void *nvme_finish_cmd(struct nvme_queue *nvmeq, int tag,
+						nvme_completion_fn *fn)
 {
-	rcu_read_unlock();
+	struct nvme_cmd_info *cmd = get_cmd_from_tag(nvmeq, tag);
+	void *ctx;
+	if (tag >= nvmeq->q_depth) {
+		*fn = special_completion;
+		return CMD_CTX_INVALID;
+	}
+	if (fn)
+		*fn = cmd->fn;
+	ctx = cmd->ctx;
+	cmd->fn = special_completion;
+	cmd->ctx = CMD_CTX_COMPLETED;
+	return ctx;
 }
 
 /**
@@ -331,26 +332,29 @@ static void unlock_nvmeq(struct nvme_queue *nvmeq) __releases(RCU)
  *
  * Safe to use from interrupt context
  */
-static int nvme_submit_cmd(struct nvme_queue *nvmeq, struct nvme_command *cmd)
+static int __nvme_submit_cmd(struct nvme_queue *nvmeq, struct nvme_command *cmd)
 {
-	unsigned long flags;
-	u16 tail;
-	spin_lock_irqsave(&nvmeq->q_lock, flags);
-	if (nvmeq->q_suspended) {
-		spin_unlock_irqrestore(&nvmeq->q_lock, flags);
-		return -EBUSY;
-	}
-	tail = nvmeq->sq_tail;
+	u16 tail = nvmeq->sq_tail;
+
 	memcpy(&nvmeq->sq_cmds[tail], cmd, sizeof(*cmd));
 	if (++tail == nvmeq->q_depth)
 		tail = 0;
 	writel(tail, nvmeq->q_db);
 	nvmeq->sq_tail = tail;
-	spin_unlock_irqrestore(&nvmeq->q_lock, flags);
 
 	return 0;
 }
 
+static int nvme_submit_cmd(struct nvme_queue *nvmeq, struct nvme_command *cmd)
+{
+	unsigned long flags;
+	int ret;
+	spin_lock_irqsave(&nvmeq->q_lock, flags);
+	ret = __nvme_submit_cmd(nvmeq, cmd);
+	spin_unlock_irqrestore(&nvmeq->q_lock, flags);
+	return ret;
+}
+
 static __le64 **iod_list(struct nvme_iod *iod)
 {
 	return ((void *)iod) + iod->offset;
@@ -361,17 +365,17 @@ static __le64 **iod_list(struct nvme_iod *iod)
  * as it only leads to a small amount of wasted memory for the lifetime of
  * the I/O.
  */
-static int nvme_npages(unsigned size)
+static int nvme_npages(unsigned size, struct nvme_dev *dev)
 {
-	unsigned nprps = DIV_ROUND_UP(size + PAGE_SIZE, PAGE_SIZE);
-	return DIV_ROUND_UP(8 * nprps, PAGE_SIZE - 8);
+	unsigned nprps = DIV_ROUND_UP(size + dev->page_size, dev->page_size);
+	return DIV_ROUND_UP(8 * nprps, dev->page_size - 8);
 }
 
 static struct nvme_iod *
-nvme_alloc_iod(unsigned nseg, unsigned nbytes, gfp_t gfp)
+nvme_alloc_iod(unsigned nseg, unsigned nbytes, struct nvme_dev *dev, gfp_t gfp)
 {
 	struct nvme_iod *iod = kmalloc(sizeof(struct nvme_iod) +
-				sizeof(__le64 *) * nvme_npages(nbytes) +
+				sizeof(__le64 *) * nvme_npages(nbytes, dev) +
 				sizeof(struct scatterlist) * nseg, gfp);
 
 	if (iod) {
@@ -380,7 +384,6 @@ nvme_alloc_iod(unsigned nseg, unsigned nbytes, gfp_t gfp)
 		iod->length = nbytes;
 		iod->nents = 0;
 		iod->first_dma = 0ULL;
-		iod->start_time = jiffies;
 	}
 
 	return iod;
@@ -388,7 +391,7 @@ nvme_alloc_iod(unsigned nseg, unsigned nbytes, gfp_t gfp)
 
 void nvme_free_iod(struct nvme_dev *dev, struct nvme_iod *iod)
 {
-	const int last_prp = PAGE_SIZE / 8 - 1;
+	const int last_prp = dev->page_size / 8 - 1;
 	int i;
 	__le64 **list = iod_list(iod);
 	dma_addr_t prp_dma = iod->first_dma;
@@ -404,65 +407,49 @@ void nvme_free_iod(struct nvme_dev *dev, struct nvme_iod *iod)
 	kfree(iod);
 }
 
-static void nvme_start_io_acct(struct bio *bio)
-{
-	struct gendisk *disk = bio->bi_bdev->bd_disk;
-	if (blk_queue_io_stat(disk->queue)) {
-		const int rw = bio_data_dir(bio);
-		int cpu = part_stat_lock();
-		part_round_stats(cpu, &disk->part0);
-		part_stat_inc(cpu, &disk->part0, ios[rw]);
-		part_stat_add(cpu, &disk->part0, sectors[rw],
-							bio_sectors(bio));
-		part_inc_in_flight(&disk->part0, rw);
-		part_stat_unlock();
-	}
-}
-
-static void nvme_end_io_acct(struct bio *bio, unsigned long start_time)
+static int nvme_error_status(u16 status)
 {
-	struct gendisk *disk = bio->bi_bdev->bd_disk;
-	if (blk_queue_io_stat(disk->queue)) {
-		const int rw = bio_data_dir(bio);
-		unsigned long duration = jiffies - start_time;
-		int cpu = part_stat_lock();
-		part_stat_add(cpu, &disk->part0, ticks[rw], duration);
-		part_round_stats(cpu, &disk->part0);
-		part_dec_in_flight(&disk->part0, rw);
-		part_stat_unlock();
+	switch (status & 0x7ff) {
+	case NVME_SC_SUCCESS:
+		return 0;
+	case NVME_SC_CAP_EXCEEDED:
+		return -ENOSPC;
+	default:
+		return -EIO;
 	}
 }
 
-static void bio_completion(struct nvme_queue *nvmeq, void *ctx,
+static void req_completion(struct nvme_queue *nvmeq, void *ctx,
 						struct nvme_completion *cqe)
 {
 	struct nvme_iod *iod = ctx;
-	struct bio *bio = iod->private;
+	struct request *req = iod->private;
+	struct nvme_cmd_info *cmd_rq = blk_mq_rq_to_pdu(req);
+
 	u16 status = le16_to_cpup(&cqe->status) >> 1;
-	int error = 0;
 
 	if (unlikely(status)) {
-		if (!(status & NVME_SC_DNR ||
-				bio->bi_rw & REQ_FAILFAST_MASK) &&
-				(jiffies - iod->start_time) < IOD_TIMEOUT) {
-			if (!waitqueue_active(&nvmeq->sq_full))
-				add_wait_queue(&nvmeq->sq_full,
-							&nvmeq->sq_cong_wait);
-			list_add_tail(&iod->node, &nvmeq->iod_bio);
-			wake_up(&nvmeq->sq_full);
+		if (!(status & NVME_SC_DNR || blk_noretry_request(req))
+		    && (jiffies - req->start_time) < req->timeout) {
+			blk_mq_requeue_request(req);
+			blk_mq_kick_requeue_list(req->q);
 			return;
 		}
-		error = -EIO;
-	}
-	if (iod->nents) {
-		dma_unmap_sg(nvmeq->q_dmadev, iod->sg, iod->nents,
-			bio_data_dir(bio) ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
-		nvme_end_io_acct(bio, iod->start_time);
-	}
+		req->errors = nvme_error_status(status);
+	} else
+		req->errors = 0;
+
+	if (cmd_rq->aborted)
+		dev_warn(&nvmeq->dev->pci_dev->dev,
+			"completing aborted command with status:%04x\n",
+			status);
+
+	if (iod->nents)
+		dma_unmap_sg(&nvmeq->dev->pci_dev->dev, iod->sg, iod->nents,
+			rq_data_dir(req) ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
 	nvme_free_iod(nvmeq->dev, iod);
 
-	trace_block_bio_complete(bdev_get_queue(bio->bi_bdev), bio, error);
-	bio_endio(bio, error);
+	blk_mq_complete_request(req);
 }
 
 /* length is in bytes.  gfp flags indicates whether we may sleep. */
@@ -479,26 +466,27 @@ int nvme_setup_prps(struct nvme_dev *dev, struct nvme_iod *iod, int total_len,
 	__le64 **list = iod_list(iod);
 	dma_addr_t prp_dma;
 	int nprps, i;
+	u32 page_size = dev->page_size;
 
-	length -= (PAGE_SIZE - offset);
+	length -= (page_size - offset);
 	if (length <= 0)
 		return total_len;
 
-	dma_len -= (PAGE_SIZE - offset);
+	dma_len -= (page_size - offset);
 	if (dma_len) {
-		dma_addr += (PAGE_SIZE - offset);
+		dma_addr += (page_size - offset);
 	} else {
 		sg = sg_next(sg);
 		dma_addr = sg_dma_address(sg);
 		dma_len = sg_dma_len(sg);
 	}
 
-	if (length <= PAGE_SIZE) {
+	if (length <= page_size) {
 		iod->first_dma = dma_addr;
 		return total_len;
 	}
 
-	nprps = DIV_ROUND_UP(length, PAGE_SIZE);
+	nprps = DIV_ROUND_UP(length, page_size);
 	if (nprps <= (256 / 8)) {
 		pool = dev->prp_small_pool;
 		iod->npages = 0;
@@ -511,13 +499,13 @@ int nvme_setup_prps(struct nvme_dev *dev, struct nvme_iod *iod, int total_len,
 	if (!prp_list) {
 		iod->first_dma = dma_addr;
 		iod->npages = -1;
-		return (total_len - length) + PAGE_SIZE;
+		return (total_len - length) + page_size;
 	}
 	list[0] = prp_list;
 	iod->first_dma = prp_dma;
 	i = 0;
 	for (;;) {
-		if (i == PAGE_SIZE / 8) {
+		if (i == page_size >> 3) {
 			__le64 *old_prp_list = prp_list;
 			prp_list = dma_pool_alloc(pool, gfp, &prp_dma);
 			if (!prp_list)
@@ -528,9 +516,9 @@ int nvme_setup_prps(struct nvme_dev *dev, struct nvme_iod *iod, int total_len,
 			i = 1;
 		}
 		prp_list[i++] = cpu_to_le64(dma_addr);
-		dma_len -= PAGE_SIZE;
-		dma_addr += PAGE_SIZE;
-		length -= PAGE_SIZE;
+		dma_len -= page_size;
+		dma_addr += page_size;
+		length -= page_size;
 		if (length <= 0)
 			break;
 		if (dma_len > 0)
@@ -544,88 +532,25 @@ int nvme_setup_prps(struct nvme_dev *dev, struct nvme_iod *iod, int total_len,
 	return total_len;
 }
 
-static int nvme_split_and_submit(struct bio *bio, struct nvme_queue *nvmeq,
-				 int len)
-{
-	struct bio *split = bio_split(bio, len >> 9, GFP_ATOMIC, NULL);
-	if (!split)
-		return -ENOMEM;
-
-	trace_block_split(bdev_get_queue(bio->bi_bdev), bio,
-					split->bi_iter.bi_sector);
-	bio_chain(split, bio);
-
-	if (!waitqueue_active(&nvmeq->sq_full))
-		add_wait_queue(&nvmeq->sq_full, &nvmeq->sq_cong_wait);
-	bio_list_add(&nvmeq->sq_cong, split);
-	bio_list_add(&nvmeq->sq_cong, bio);
-	wake_up(&nvmeq->sq_full);
-
-	return 0;
-}
-
-/* NVMe scatterlists require no holes in the virtual address */
-#define BIOVEC_NOT_VIRT_MERGEABLE(vec1, vec2)	((vec2)->bv_offset || \
-			(((vec1)->bv_offset + (vec1)->bv_len) % PAGE_SIZE))
-
-static int nvme_map_bio(struct nvme_queue *nvmeq, struct nvme_iod *iod,
-		struct bio *bio, enum dma_data_direction dma_dir, int psegs)
-{
-	struct bio_vec bvec, bvprv;
-	struct bvec_iter iter;
-	struct scatterlist *sg = NULL;
-	int length = 0, nsegs = 0, split_len = bio->bi_iter.bi_size;
-	int first = 1;
-
-	if (nvmeq->dev->stripe_size)
-		split_len = nvmeq->dev->stripe_size -
-			((bio->bi_iter.bi_sector << 9) &
-			 (nvmeq->dev->stripe_size - 1));
-
-	sg_init_table(iod->sg, psegs);
-	bio_for_each_segment(bvec, bio, iter) {
-		if (!first && BIOVEC_PHYS_MERGEABLE(&bvprv, &bvec)) {
-			sg->length += bvec.bv_len;
-		} else {
-			if (!first && BIOVEC_NOT_VIRT_MERGEABLE(&bvprv, &bvec))
-				return nvme_split_and_submit(bio, nvmeq,
-							     length);
-
-			sg = sg ? sg + 1 : iod->sg;
-			sg_set_page(sg, bvec.bv_page,
-				    bvec.bv_len, bvec.bv_offset);
-			nsegs++;
-		}
-
-		if (split_len - length < bvec.bv_len)
-			return nvme_split_and_submit(bio, nvmeq, split_len);
-		length += bvec.bv_len;
-		bvprv = bvec;
-		first = 0;
-	}
-	iod->nents = nsegs;
-	sg_mark_end(sg);
-	if (dma_map_sg(nvmeq->q_dmadev, iod->sg, iod->nents, dma_dir) == 0)
-		return -ENOMEM;
-
-	BUG_ON(length != bio->bi_iter.bi_size);
-	return length;
-}
-
-static int nvme_submit_discard(struct nvme_queue *nvmeq, struct nvme_ns *ns,
-		struct bio *bio, struct nvme_iod *iod, int cmdid)
+/*
+ * We reuse the small pool to allocate the 16-byte range here as it is not
+ * worth having a special pool for these or additional cases to handle freeing
+ * the iod.
+ */
+static void nvme_submit_discard(struct nvme_queue *nvmeq, struct nvme_ns *ns,
+		struct request *req, struct nvme_iod *iod)
 {
 	struct nvme_dsm_range *range =
 				(struct nvme_dsm_range *)iod_list(iod)[0];
 	struct nvme_command *cmnd = &nvmeq->sq_cmds[nvmeq->sq_tail];
 
 	range->cattr = cpu_to_le32(0);
-	range->nlb = cpu_to_le32(bio->bi_iter.bi_size >> ns->lba_shift);
-	range->slba = cpu_to_le64(nvme_block_nr(ns, bio->bi_iter.bi_sector));
+	range->nlb = cpu_to_le32(blk_rq_bytes(req) >> ns->lba_shift);
+	range->slba = cpu_to_le64(nvme_block_nr(ns, blk_rq_pos(req)));
 
 	memset(cmnd, 0, sizeof(*cmnd));
 	cmnd->dsm.opcode = nvme_cmd_dsm;
-	cmnd->dsm.command_id = cmdid;
+	cmnd->dsm.command_id = req->tag;
 	cmnd->dsm.nsid = cpu_to_le32(ns->ns_id);
 	cmnd->dsm.prp1 = cpu_to_le64(iod->first_dma);
 	cmnd->dsm.nr = 0;
@@ -634,11 +559,9 @@ static int nvme_submit_discard(struct nvme_queue *nvmeq, struct nvme_ns *ns,
 	if (++nvmeq->sq_tail == nvmeq->q_depth)
 		nvmeq->sq_tail = 0;
 	writel(nvmeq->sq_tail, nvmeq->q_db);
-
-	return 0;
 }
 
-static int nvme_submit_flush(struct nvme_queue *nvmeq, struct nvme_ns *ns,
+static void nvme_submit_flush(struct nvme_queue *nvmeq, struct nvme_ns *ns,
 								int cmdid)
 {
 	struct nvme_command *cmnd = &nvmeq->sq_cmds[nvmeq->sq_tail];
@@ -651,49 +574,34 @@ static int nvme_submit_flush(struct nvme_queue *nvmeq, struct nvme_ns *ns,
 	if (++nvmeq->sq_tail == nvmeq->q_depth)
 		nvmeq->sq_tail = 0;
 	writel(nvmeq->sq_tail, nvmeq->q_db);
-
-	return 0;
 }
 
-static int nvme_submit_iod(struct nvme_queue *nvmeq, struct nvme_iod *iod)
+static int nvme_submit_iod(struct nvme_queue *nvmeq, struct nvme_iod *iod,
+							struct nvme_ns *ns)
 {
-	struct bio *bio = iod->private;
-	struct nvme_ns *ns = bio->bi_bdev->bd_disk->private_data;
+	struct request *req = iod->private;
 	struct nvme_command *cmnd;
-	int cmdid;
-	u16 control;
-	u32 dsmgmt;
-
-	cmdid = alloc_cmdid(nvmeq, iod, bio_completion, NVME_IO_TIMEOUT);
-	if (unlikely(cmdid < 0))
-		return cmdid;
+	u16 control = 0;
+	u32 dsmgmt = 0;
 
-	if (bio->bi_rw & REQ_DISCARD)
-		return nvme_submit_discard(nvmeq, ns, bio, iod, cmdid);
-	if (bio->bi_rw & REQ_FLUSH)
-		return nvme_submit_flush(nvmeq, ns, cmdid);
-
-	control = 0;
-	if (bio->bi_rw & REQ_FUA)
+	if (req->cmd_flags & REQ_FUA)
 		control |= NVME_RW_FUA;
-	if (bio->bi_rw & (REQ_FAILFAST_DEV | REQ_RAHEAD))
+	if (req->cmd_flags & (REQ_FAILFAST_DEV | REQ_RAHEAD))
 		control |= NVME_RW_LR;
 
-	dsmgmt = 0;
-	if (bio->bi_rw & REQ_RAHEAD)
+	if (req->cmd_flags & REQ_RAHEAD)
 		dsmgmt |= NVME_RW_DSM_FREQ_PREFETCH;
 
 	cmnd = &nvmeq->sq_cmds[nvmeq->sq_tail];
 	memset(cmnd, 0, sizeof(*cmnd));
 
-	cmnd->rw.opcode = bio_data_dir(bio) ? nvme_cmd_write : nvme_cmd_read;
-	cmnd->rw.command_id = cmdid;
+	cmnd->rw.opcode = (rq_data_dir(req) ? nvme_cmd_write : nvme_cmd_read);
+	cmnd->rw.command_id = req->tag;
 	cmnd->rw.nsid = cpu_to_le32(ns->ns_id);
 	cmnd->rw.prp1 = cpu_to_le64(sg_dma_address(iod->sg));
 	cmnd->rw.prp2 = cpu_to_le64(iod->first_dma);
-	cmnd->rw.slba = cpu_to_le64(nvme_block_nr(ns, bio->bi_iter.bi_sector));
-	cmnd->rw.length =
-		cpu_to_le16((bio->bi_iter.bi_size >> ns->lba_shift) - 1);
+	cmnd->rw.slba = cpu_to_le64(nvme_block_nr(ns, blk_rq_pos(req)));
+	cmnd->rw.length = cpu_to_le16((blk_rq_bytes(req) >> ns->lba_shift) - 1);
 	cmnd->rw.control = cpu_to_le16(control);
 	cmnd->rw.dsmgmt = cpu_to_le32(dsmgmt);
 
@@ -704,45 +612,26 @@ static int nvme_submit_iod(struct nvme_queue *nvmeq, struct nvme_iod *iod)
 	return 0;
 }
 
-static int nvme_split_flush_data(struct nvme_queue *nvmeq, struct bio *bio)
-{
-	struct bio *split = bio_clone(bio, GFP_ATOMIC);
-	if (!split)
-		return -ENOMEM;
-
-	split->bi_iter.bi_size = 0;
-	split->bi_phys_segments = 0;
-	bio->bi_rw &= ~REQ_FLUSH;
-	bio_chain(split, bio);
-
-	if (!waitqueue_active(&nvmeq->sq_full))
-		add_wait_queue(&nvmeq->sq_full, &nvmeq->sq_cong_wait);
-	bio_list_add(&nvmeq->sq_cong, split);
-	bio_list_add(&nvmeq->sq_cong, bio);
-	wake_up_process(nvme_thread);
-
-	return 0;
-}
-
-/*
- * Called with local interrupts disabled and the q_lock held.  May not sleep.
- */
-static int nvme_submit_bio_queue(struct nvme_queue *nvmeq, struct nvme_ns *ns,
-								struct bio *bio)
+static int nvme_queue_rq(struct blk_mq_hw_ctx *hctx,
+			 const struct blk_mq_queue_data *bd)
 {
+	struct nvme_ns *ns = hctx->queue->queuedata;
+	struct nvme_queue *nvmeq = hctx->driver_data;
+	struct request *req = bd->rq;
+	struct nvme_cmd_info *cmd = blk_mq_rq_to_pdu(req);
 	struct nvme_iod *iod;
-	int psegs = bio_phys_segments(ns->queue, bio);
-	int result;
+	int psegs = req->nr_phys_segments;
+	enum dma_data_direction dma_dir;
+	unsigned size = !(req->cmd_flags & REQ_DISCARD) ? blk_rq_bytes(req) :
+						sizeof(struct nvme_dsm_range);
 
-	if ((bio->bi_rw & REQ_FLUSH) && psegs)
-		return nvme_split_flush_data(nvmeq, bio);
-
-	iod = nvme_alloc_iod(psegs, bio->bi_iter.bi_size, GFP_ATOMIC);
+	iod = nvme_alloc_iod(psegs, size, ns->dev, GFP_ATOMIC);
 	if (!iod)
-		return -ENOMEM;
+		return BLK_MQ_RQ_QUEUE_BUSY;
 
-	iod->private = bio;
-	if (bio->bi_rw & REQ_DISCARD) {
+	iod->private = req;
+
+	if (req->cmd_flags & REQ_DISCARD) {
 		void *range;
 		/*
 		 * We reuse the small pool to allocate the 16-byte range here
@@ -752,35 +641,50 @@ static int nvme_submit_bio_queue(struct nvme_queue *nvmeq, struct nvme_ns *ns,
 		range = dma_pool_alloc(nvmeq->dev->prp_small_pool,
 						GFP_ATOMIC,
 						&iod->first_dma);
-		if (!range) {
-			result = -ENOMEM;
-			goto free_iod;
-		}
+		if (!range)
+			goto retry_cmd;
 		iod_list(iod)[0] = (__le64 *)range;
 		iod->npages = 0;
 	} else if (psegs) {
-		result = nvme_map_bio(nvmeq, iod, bio,
-			bio_data_dir(bio) ? DMA_TO_DEVICE : DMA_FROM_DEVICE,
-			psegs);
-		if (result <= 0)
-			goto free_iod;
-		if (nvme_setup_prps(nvmeq->dev, iod, result, GFP_ATOMIC) !=
-								result) {
-			result = -ENOMEM;
-			goto free_iod;
+		dma_dir = rq_data_dir(req) ? DMA_TO_DEVICE : DMA_FROM_DEVICE;
+
+		sg_init_table(iod->sg, psegs);
+		iod->nents = blk_rq_map_sg(req->q, req, iod->sg);
+		if (!iod->nents)
+			goto error_cmd;
+
+		if (!dma_map_sg(nvmeq->q_dmadev, iod->sg, iod->nents, dma_dir))
+			goto retry_cmd;
+
+		if (blk_rq_bytes(req) !=
+                    nvme_setup_prps(nvmeq->dev, iod, blk_rq_bytes(req), GFP_ATOMIC)) {
+			dma_unmap_sg(&nvmeq->dev->pci_dev->dev, iod->sg,
+					iod->nents, dma_dir);
+			goto retry_cmd;
 		}
-		nvme_start_io_acct(bio);
 	}
-	if (unlikely(nvme_submit_iod(nvmeq, iod))) {
-		if (!waitqueue_active(&nvmeq->sq_full))
-			add_wait_queue(&nvmeq->sq_full, &nvmeq->sq_cong_wait);
-		list_add_tail(&iod->node, &nvmeq->iod_bio);
-	}
-	return 0;
 
- free_iod:
+	blk_mq_start_request(req);
+
+	nvme_set_info(cmd, iod, req_completion);
+	spin_lock_irq(&nvmeq->q_lock);
+	if (req->cmd_flags & REQ_DISCARD)
+		nvme_submit_discard(nvmeq, ns, req, iod);
+	else if (req->cmd_flags & REQ_FLUSH)
+		nvme_submit_flush(nvmeq, ns, req->tag);
+	else
+		nvme_submit_iod(nvmeq, iod, ns);
+
+	nvme_process_cq(nvmeq);
+	spin_unlock_irq(&nvmeq->q_lock);
+	return BLK_MQ_RQ_QUEUE_OK;
+
+ error_cmd:
 	nvme_free_iod(nvmeq->dev, iod);
-	return result;
+	return BLK_MQ_RQ_QUEUE_ERROR;
+ retry_cmd:
+	nvme_free_iod(nvmeq->dev, iod);
+	return BLK_MQ_RQ_QUEUE_BUSY;
 }
 
 static int nvme_process_cq(struct nvme_queue *nvmeq)
@@ -801,8 +705,7 @@ static int nvme_process_cq(struct nvme_queue *nvmeq)
 			head = 0;
 			phase = !phase;
 		}
-
-		ctx = free_cmdid(nvmeq, cqe.command_id, &fn);
+		ctx = nvme_finish_cmd(nvmeq, cqe.command_id, &fn);
 		fn(nvmeq, ctx, &cqe);
 	}
 
@@ -823,29 +726,13 @@ static int nvme_process_cq(struct nvme_queue *nvmeq)
 	return 1;
 }
 
-static void nvme_make_request(struct request_queue *q, struct bio *bio)
+/* Admin queue isn't initialized as a request queue. If at some point this
+ * happens anyway, make sure to notify the user */
+static int nvme_admin_queue_rq(struct blk_mq_hw_ctx *hctx,
+			       const struct blk_mq_queue_data *bd)
 {
-	struct nvme_ns *ns = q->queuedata;
-	struct nvme_queue *nvmeq = get_nvmeq(ns->dev);
-	int result = -EBUSY;
-
-	if (!nvmeq) {
-		bio_endio(bio, -EIO);
-		return;
-	}
-
-	spin_lock_irq(&nvmeq->q_lock);
-	if (!nvmeq->q_suspended && bio_list_empty(&nvmeq->sq_cong))
-		result = nvme_submit_bio_queue(nvmeq, ns, bio);
-	if (unlikely(result)) {
-		if (!waitqueue_active(&nvmeq->sq_full))
-			add_wait_queue(&nvmeq->sq_full, &nvmeq->sq_cong_wait);
-		bio_list_add(&nvmeq->sq_cong, bio);
-	}
-
-	nvme_process_cq(nvmeq);
-	spin_unlock_irq(&nvmeq->q_lock);
-	put_nvmeq(nvmeq);
+	WARN_ON_ONCE(1);
+	return BLK_MQ_RQ_QUEUE_ERROR;
 }
 
 static irqreturn_t nvme_irq(int irq, void *data)
@@ -869,10 +756,11 @@ static irqreturn_t nvme_irq_check(int irq, void *data)
 	return IRQ_WAKE_THREAD;
 }
 
-static void nvme_abort_command(struct nvme_queue *nvmeq, int cmdid)
+static void nvme_abort_cmd_info(struct nvme_queue *nvmeq, struct nvme_cmd_info *
+								cmd_info)
 {
 	spin_lock_irq(&nvmeq->q_lock);
-	cancel_cmdid(nvmeq, cmdid, NULL);
+	cancel_cmd_info(cmd_info, NULL);
 	spin_unlock_irq(&nvmeq->q_lock);
 }
 
@@ -895,47 +783,40 @@ static void sync_completion(struct nvme_queue *nvmeq, void *ctx,
  * Returns 0 on success.  If the result is negative, it's a Linux error code;
  * if the result is positive, it's an NVM Express status code
  */
-static int nvme_submit_sync_cmd(struct nvme_dev *dev, int q_idx,
-						struct nvme_command *cmd,
+static int nvme_submit_sync_cmd(struct request *req, struct nvme_command *cmd,
 						u32 *result, unsigned timeout)
 {
-	int cmdid, ret;
+	int ret;
 	struct sync_cmd_info cmdinfo;
-	struct nvme_queue *nvmeq;
-
-	nvmeq = lock_nvmeq(dev, q_idx);
-	if (!nvmeq)
-		return -ENODEV;
+	struct nvme_cmd_info *cmd_rq = blk_mq_rq_to_pdu(req);
+	struct nvme_queue *nvmeq = cmd_rq->nvmeq;
 
 	cmdinfo.task = current;
 	cmdinfo.status = -EINTR;
 
-	cmdid = alloc_cmdid(nvmeq, &cmdinfo, sync_completion, timeout);
-	if (cmdid < 0) {
-		unlock_nvmeq(nvmeq);
-		return cmdid;
-	}
-	cmd->common.command_id = cmdid;
+	cmd->common.command_id = req->tag;
+
+	nvme_set_info(cmd_rq, &cmdinfo, sync_completion);
 
 	set_current_state(TASK_KILLABLE);
 	ret = nvme_submit_cmd(nvmeq, cmd);
 	if (ret) {
-		free_cmdid(nvmeq, cmdid, NULL);
-		unlock_nvmeq(nvmeq);
+		nvme_finish_cmd(nvmeq, req->tag, NULL);
 		set_current_state(TASK_RUNNING);
-		return ret;
 	}
-	unlock_nvmeq(nvmeq);
-	schedule_timeout(timeout);
-
-	if (cmdinfo.status == -EINTR) {
-		nvmeq = lock_nvmeq(dev, q_idx);
-		if (nvmeq) {
-			nvme_abort_command(nvmeq, cmdid);
-			unlock_nvmeq(nvmeq);
-		}
+	ret = schedule_timeout(timeout);
+
+	/*
+	 * Ensure that sync_completion has either run, or that it will
+	 * never run.
+	 */
+	nvme_abort_cmd_info(nvmeq, blk_mq_rq_to_pdu(req));
+
+	/*
+	 * We never got the completion
+	 */
+	if (cmdinfo.status == -EINTR)
 		return -EINTR;
-	}
 
 	if (result)
 		*result = cmdinfo.result;
@@ -943,59 +824,99 @@ static int nvme_submit_sync_cmd(struct nvme_dev *dev, int q_idx,
 	return cmdinfo.status;
 }
 
-static int nvme_submit_async_cmd(struct nvme_queue *nvmeq,
+static int nvme_submit_async_admin_req(struct nvme_dev *dev)
+{
+	struct nvme_queue *nvmeq = dev->queues[0];
+	struct nvme_command c;
+	struct nvme_cmd_info *cmd_info;
+	struct request *req;
+
+	req = blk_mq_alloc_request(dev->admin_q, WRITE, GFP_ATOMIC, false);
+	if (IS_ERR(req))
+		return PTR_ERR(req);
+
+	cmd_info = blk_mq_rq_to_pdu(req);
+	nvme_set_info(cmd_info, req, async_req_completion);
+
+	memset(&c, 0, sizeof(c));
+	c.common.opcode = nvme_admin_async_event;
+	c.common.command_id = req->tag;
+
+	return __nvme_submit_cmd(nvmeq, &c);
+}
+
+static int nvme_submit_admin_async_cmd(struct nvme_dev *dev,
 			struct nvme_command *cmd,
 			struct async_cmd_info *cmdinfo, unsigned timeout)
 {
-	int cmdid;
+	struct nvme_queue *nvmeq = dev->queues[0];
+	struct request *req;
+	struct nvme_cmd_info *cmd_rq;
+
+	req = blk_mq_alloc_request(dev->admin_q, WRITE, GFP_KERNEL, false);
+	if (IS_ERR(req))
+		return PTR_ERR(req);
 
-	cmdid = alloc_cmdid_killable(nvmeq, cmdinfo, async_completion, timeout);
-	if (cmdid < 0)
-		return cmdid;
+	req->timeout = timeout;
+	cmd_rq = blk_mq_rq_to_pdu(req);
+	cmdinfo->req = req;
+	nvme_set_info(cmd_rq, cmdinfo, async_completion);
 	cmdinfo->status = -EINTR;
-	cmd->common.command_id = cmdid;
+
+	cmd->common.command_id = req->tag;
+
 	return nvme_submit_cmd(nvmeq, cmd);
 }
 
-int nvme_submit_admin_cmd(struct nvme_dev *dev, struct nvme_command *cmd,
-								u32 *result)
+static int __nvme_submit_admin_cmd(struct nvme_dev *dev, struct nvme_command *cmd,
+						u32 *result, unsigned timeout)
 {
-	return nvme_submit_sync_cmd(dev, 0, cmd, result, ADMIN_TIMEOUT);
+	int res;
+	struct request *req;
+
+	req = blk_mq_alloc_request(dev->admin_q, WRITE, GFP_KERNEL, false);
+	if (IS_ERR(req))
+		return PTR_ERR(req);
+	res = nvme_submit_sync_cmd(req, cmd, result, timeout);
+	blk_mq_free_request(req);
+	return res;
 }
 
-int nvme_submit_io_cmd(struct nvme_dev *dev, struct nvme_command *cmd,
+int nvme_submit_admin_cmd(struct nvme_dev *dev, struct nvme_command *cmd,
 								u32 *result)
 {
-	return nvme_submit_sync_cmd(dev, smp_processor_id() + 1, cmd, result,
-							NVME_IO_TIMEOUT);
+	return __nvme_submit_admin_cmd(dev, cmd, result, ADMIN_TIMEOUT);
 }
 
-static int nvme_submit_admin_cmd_async(struct nvme_dev *dev,
-		struct nvme_command *cmd, struct async_cmd_info *cmdinfo)
+int nvme_submit_io_cmd(struct nvme_dev *dev, struct nvme_ns *ns,
+					struct nvme_command *cmd, u32 *result)
 {
-	return nvme_submit_async_cmd(raw_nvmeq(dev, 0), cmd, cmdinfo,
-								ADMIN_TIMEOUT);
+	int res;
+	struct request *req;
+
+	req = blk_mq_alloc_request(ns->queue, WRITE, (GFP_KERNEL|__GFP_WAIT),
+									false);
+	if (IS_ERR(req))
+		return PTR_ERR(req);
+	res = nvme_submit_sync_cmd(req, cmd, result, NVME_IO_TIMEOUT);
+	blk_mq_free_request(req);
+	return res;
 }
 
 static int adapter_delete_queue(struct nvme_dev *dev, u8 opcode, u16 id)
 {
-	int status;
 	struct nvme_command c;
 
 	memset(&c, 0, sizeof(c));
 	c.delete_queue.opcode = opcode;
 	c.delete_queue.qid = cpu_to_le16(id);
 
-	status = nvme_submit_admin_cmd(dev, &c, NULL);
-	if (status)
-		return -EIO;
-	return 0;
+	return nvme_submit_admin_cmd(dev, &c, NULL);
 }
 
 static int adapter_alloc_cq(struct nvme_dev *dev, u16 qid,
 						struct nvme_queue *nvmeq)
 {
-	int status;
 	struct nvme_command c;
 	int flags = NVME_QUEUE_PHYS_CONTIG | NVME_CQ_IRQ_ENABLED;
 
@@ -1007,16 +928,12 @@ static int adapter_alloc_cq(struct nvme_dev *dev, u16 qid,
 	c.create_cq.cq_flags = cpu_to_le16(flags);
 	c.create_cq.irq_vector = cpu_to_le16(nvmeq->cq_vector);
 
-	status = nvme_submit_admin_cmd(dev, &c, NULL);
-	if (status)
-		return -EIO;
-	return 0;
+	return nvme_submit_admin_cmd(dev, &c, NULL);
 }
 
 static int adapter_alloc_sq(struct nvme_dev *dev, u16 qid,
 						struct nvme_queue *nvmeq)
 {
-	int status;
 	struct nvme_command c;
 	int flags = NVME_QUEUE_PHYS_CONTIG | NVME_SQ_PRIO_MEDIUM;
 
@@ -1028,10 +945,7 @@ static int adapter_alloc_sq(struct nvme_dev *dev, u16 qid,
 	c.create_sq.sq_flags = cpu_to_le16(flags);
 	c.create_sq.cqid = cpu_to_le16(qid);
 
-	status = nvme_submit_admin_cmd(dev, &c, NULL);
-	if (status)
-		return -EIO;
-	return 0;
+	return nvme_submit_admin_cmd(dev, &c, NULL);
 }
 
 static int adapter_delete_cq(struct nvme_dev *dev, u16 cqid)
@@ -1087,28 +1001,27 @@ int nvme_set_features(struct nvme_dev *dev, unsigned fid, unsigned dword11,
 }
 
 /**
- * nvme_abort_cmd - Attempt aborting a command
- * @cmdid: Command id of a timed out IO
- * @queue: The queue with timed out IO
+ * nvme_abort_req - Attempt aborting a request
  *
  * Schedule controller reset if the command was already aborted once before and
  * still hasn't been returned to the driver, or if this is the admin queue.
  */
-static void nvme_abort_cmd(int cmdid, struct nvme_queue *nvmeq)
+static void nvme_abort_req(struct request *req)
 {
-	int a_cmdid;
-	struct nvme_command cmd;
+	struct nvme_cmd_info *cmd_rq = blk_mq_rq_to_pdu(req);
+	struct nvme_queue *nvmeq = cmd_rq->nvmeq;
 	struct nvme_dev *dev = nvmeq->dev;
-	struct nvme_cmd_info *info = nvme_cmd_info(nvmeq);
-	struct nvme_queue *adminq;
+	struct request *abort_req;
+	struct nvme_cmd_info *abort_cmd;
+	struct nvme_command cmd;
 
-	if (!nvmeq->qid || info[cmdid].aborted) {
+	if (!nvmeq->qid || cmd_rq->aborted) {
 		if (work_busy(&dev->reset_work))
 			return;
 		list_del_init(&dev->node);
 		dev_warn(&dev->pci_dev->dev,
-			"I/O %d QID %d timeout, reset controller\n", cmdid,
-								nvmeq->qid);
+			"I/O %d QID %d timeout, reset controller\n",
+							req->tag, nvmeq->qid);
 		dev->reset_workfn = nvme_reset_failed_dev;
 		queue_work(nvme_workq, &dev->reset_work);
 		return;
@@ -1117,120 +1030,110 @@ static void nvme_abort_cmd(int cmdid, struct nvme_queue *nvmeq)
 	if (!dev->abort_limit)
 		return;
 
-	adminq = rcu_dereference(dev->queues[0]);
-	a_cmdid = alloc_cmdid(adminq, CMD_CTX_ABORT, special_completion,
-								ADMIN_TIMEOUT);
-	if (a_cmdid < 0)
+	abort_req = blk_mq_alloc_request(dev->admin_q, WRITE, GFP_ATOMIC,
+									false);
+	if (IS_ERR(abort_req))
 		return;
 
+	abort_cmd = blk_mq_rq_to_pdu(abort_req);
+	nvme_set_info(abort_cmd, abort_req, abort_completion);
+
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.abort.opcode = nvme_admin_abort_cmd;
-	cmd.abort.cid = cmdid;
+	cmd.abort.cid = req->tag;
 	cmd.abort.sqid = cpu_to_le16(nvmeq->qid);
-	cmd.abort.command_id = a_cmdid;
+	cmd.abort.command_id = abort_req->tag;
 
 	--dev->abort_limit;
-	info[cmdid].aborted = 1;
-	info[cmdid].timeout = jiffies + ADMIN_TIMEOUT;
+	cmd_rq->aborted = 1;
 
-	dev_warn(nvmeq->q_dmadev, "Aborting I/O %d QID %d\n", cmdid,
+	dev_warn(nvmeq->q_dmadev, "Aborting I/O %d QID %d\n", req->tag,
 							nvmeq->qid);
-	nvme_submit_cmd(adminq, &cmd);
+	if (nvme_submit_cmd(dev->queues[0], &cmd) < 0) {
+		dev_warn(nvmeq->q_dmadev,
+				"Could not abort I/O %d QID %d",
+				req->tag, nvmeq->qid);
+		blk_mq_free_request(abort_req);
+	}
 }
 
-/**
- * nvme_cancel_ios - Cancel outstanding I/Os
- * @queue: The queue to cancel I/Os on
- * @timeout: True to only cancel I/Os which have timed out
- */
-static void nvme_cancel_ios(struct nvme_queue *nvmeq, bool timeout)
+static void nvme_cancel_queue_ios(struct blk_mq_hw_ctx *hctx,
+				struct request *req, void *data, bool reserved)
 {
-	int depth = nvmeq->q_depth - 1;
-	struct nvme_cmd_info *info = nvme_cmd_info(nvmeq);
-	unsigned long now = jiffies;
-	int cmdid;
+	struct nvme_queue *nvmeq = data;
+	void *ctx;
+	nvme_completion_fn fn;
+	struct nvme_cmd_info *cmd;
+	static struct nvme_completion cqe = {
+		.status = cpu_to_le16(NVME_SC_ABORT_REQ << 1),
+	};
 
-	for_each_set_bit(cmdid, nvmeq->cmdid_data, depth) {
-		void *ctx;
-		nvme_completion_fn fn;
-		static struct nvme_completion cqe = {
-			.status = cpu_to_le16(NVME_SC_ABORT_REQ << 1),
-		};
+	cmd = blk_mq_rq_to_pdu(req);
 
-		if (timeout && !time_after(now, info[cmdid].timeout))
-			continue;
-		if (info[cmdid].ctx == CMD_CTX_CANCELLED)
-			continue;
-		if (timeout && nvmeq->dev->initialized) {
-			nvme_abort_cmd(cmdid, nvmeq);
-			continue;
-		}
-		dev_warn(nvmeq->q_dmadev, "Cancelling I/O %d QID %d\n", cmdid,
-								nvmeq->qid);
-		ctx = cancel_cmdid(nvmeq, cmdid, &fn);
-		fn(nvmeq, ctx, &cqe);
-	}
+	if (cmd->ctx == CMD_CTX_CANCELLED)
+		return;
+
+	dev_warn(nvmeq->q_dmadev, "Cancelling I/O %d QID %d\n",
+						req->tag, nvmeq->qid);
+	ctx = cancel_cmd_info(cmd, &fn);
+	fn(nvmeq, ctx, &cqe);
 }
 
-static void nvme_free_queue(struct rcu_head *r)
+static enum blk_eh_timer_return nvme_timeout(struct request *req, bool reserved)
 {
-	struct nvme_queue *nvmeq = container_of(r, struct nvme_queue, r_head);
+	struct nvme_cmd_info *cmd = blk_mq_rq_to_pdu(req);
+	struct nvme_queue *nvmeq = cmd->nvmeq;
 
-	spin_lock_irq(&nvmeq->q_lock);
-	while (bio_list_peek(&nvmeq->sq_cong)) {
-		struct bio *bio = bio_list_pop(&nvmeq->sq_cong);
-		bio_endio(bio, -EIO);
-	}
-	while (!list_empty(&nvmeq->iod_bio)) {
-		static struct nvme_completion cqe = {
-			.status = cpu_to_le16(
-				(NVME_SC_ABORT_REQ | NVME_SC_DNR) << 1),
-		};
-		struct nvme_iod *iod = list_first_entry(&nvmeq->iod_bio,
-							struct nvme_iod,
-							node);
-		list_del(&iod->node);
-		bio_completion(nvmeq, iod, &cqe);
-	}
-	spin_unlock_irq(&nvmeq->q_lock);
+	dev_warn(nvmeq->q_dmadev, "Timeout I/O %d QID %d\n", req->tag,
+							nvmeq->qid);
+	if (nvmeq->dev->initialized)
+		nvme_abort_req(req);
+
+	/*
+	 * The aborted req will be completed on receiving the abort req.
+	 * We enable the timer again. If hit twice, it'll cause a device reset,
+	 * as the device then is in a faulty state.
+	 */
+	return BLK_EH_RESET_TIMER;
+}
 
+static void nvme_free_queue(struct nvme_queue *nvmeq)
+{
 	dma_free_coherent(nvmeq->q_dmadev, CQ_SIZE(nvmeq->q_depth),
 				(void *)nvmeq->cqes, nvmeq->cq_dma_addr);
 	dma_free_coherent(nvmeq->q_dmadev, SQ_SIZE(nvmeq->q_depth),
 					nvmeq->sq_cmds, nvmeq->sq_dma_addr);
-	if (nvmeq->qid)
-		free_cpumask_var(nvmeq->cpu_mask);
 	kfree(nvmeq);
 }
 
 static void nvme_free_queues(struct nvme_dev *dev, int lowest)
 {
+	LLIST_HEAD(q_list);
+	struct nvme_queue *nvmeq, *next;
+	struct llist_node *entry;
 	int i;
 
 	for (i = dev->queue_count - 1; i >= lowest; i--) {
-		struct nvme_queue *nvmeq = raw_nvmeq(dev, i);
-		rcu_assign_pointer(dev->queues[i], NULL);
-		call_rcu(&nvmeq->r_head, nvme_free_queue);
+		struct nvme_queue *nvmeq = dev->queues[i];
+		llist_add(&nvmeq->node, &q_list);
 		dev->queue_count--;
+		dev->queues[i] = NULL;
 	}
+	synchronize_rcu();
+	entry = llist_del_all(&q_list);
+	llist_for_each_entry_safe(nvmeq, next, entry, node)
+		nvme_free_queue(nvmeq);
 }
 
 /**
  * nvme_suspend_queue - put queue into suspended state
  * @nvmeq - queue to suspend
- *
- * Returns 1 if already suspended, 0 otherwise.
  */
 static int nvme_suspend_queue(struct nvme_queue *nvmeq)
 {
 	int vector = nvmeq->dev->entry[nvmeq->cq_vector].vector;
 
 	spin_lock_irq(&nvmeq->q_lock);
-	if (nvmeq->q_suspended) {
-		spin_unlock_irq(&nvmeq->q_lock);
-		return 1;
-	}
-	nvmeq->q_suspended = 1;
 	nvmeq->dev->online_queues--;
 	spin_unlock_irq(&nvmeq->q_lock);
 
@@ -1242,15 +1145,18 @@ static int nvme_suspend_queue(struct nvme_queue *nvmeq)
 
 static void nvme_clear_queue(struct nvme_queue *nvmeq)
 {
+	struct blk_mq_hw_ctx *hctx = nvmeq->hctx;
+
 	spin_lock_irq(&nvmeq->q_lock);
 	nvme_process_cq(nvmeq);
-	nvme_cancel_ios(nvmeq, false);
+	if (hctx && hctx->tags)
+		blk_mq_tag_busy_iter(hctx, nvme_cancel_queue_ios, nvmeq);
 	spin_unlock_irq(&nvmeq->q_lock);
 }
 
 static void nvme_disable_queue(struct nvme_dev *dev, int qid)
 {
-	struct nvme_queue *nvmeq = raw_nvmeq(dev, qid);
+	struct nvme_queue *nvmeq = dev->queues[qid];
 
 	if (!nvmeq)
 		return;
@@ -1270,25 +1176,20 @@ static struct nvme_queue *nvme_alloc_queue(struct nvme_dev *dev, int qid,
 							int depth, int vector)
 {
 	struct device *dmadev = &dev->pci_dev->dev;
-	unsigned extra = nvme_queue_extra(depth);
-	struct nvme_queue *nvmeq = kzalloc(sizeof(*nvmeq) + extra, GFP_KERNEL);
+	struct nvme_queue *nvmeq = kzalloc(sizeof(*nvmeq), GFP_KERNEL);
 	if (!nvmeq)
 		return NULL;
 
-	nvmeq->cqes = dma_alloc_coherent(dmadev, CQ_SIZE(depth),
-					&nvmeq->cq_dma_addr, GFP_KERNEL);
+	nvmeq->cqes = dma_zalloc_coherent(dmadev, CQ_SIZE(depth),
+					  &nvmeq->cq_dma_addr, GFP_KERNEL);
 	if (!nvmeq->cqes)
 		goto free_nvmeq;
-	memset((void *)nvmeq->cqes, 0, CQ_SIZE(depth));
 
 	nvmeq->sq_cmds = dma_alloc_coherent(dmadev, SQ_SIZE(depth),
 					&nvmeq->sq_dma_addr, GFP_KERNEL);
 	if (!nvmeq->sq_cmds)
 		goto free_cqdma;
 
-	if (qid && !zalloc_cpumask_var(&nvmeq->cpu_mask, GFP_KERNEL))
-		goto free_sqdma;
-
 	nvmeq->q_dmadev = dmadev;
 	nvmeq->dev = dev;
 	snprintf(nvmeq->irqname, sizeof(nvmeq->irqname), "nvme%dq%d",
@@ -1296,23 +1197,15 @@ static struct nvme_queue *nvme_alloc_queue(struct nvme_dev *dev, int qid,
 	spin_lock_init(&nvmeq->q_lock);
 	nvmeq->cq_head = 0;
 	nvmeq->cq_phase = 1;
-	init_waitqueue_head(&nvmeq->sq_full);
-	init_waitqueue_entry(&nvmeq->sq_cong_wait, nvme_thread);
-	bio_list_init(&nvmeq->sq_cong);
-	INIT_LIST_HEAD(&nvmeq->iod_bio);
 	nvmeq->q_db = &dev->dbs[qid * 2 * dev->db_stride];
 	nvmeq->q_depth = depth;
 	nvmeq->cq_vector = vector;
 	nvmeq->qid = qid;
-	nvmeq->q_suspended = 1;
 	dev->queue_count++;
-	rcu_assign_pointer(dev->queues[qid], nvmeq);
+	dev->queues[qid] = nvmeq;
 
 	return nvmeq;
 
- free_sqdma:
-	dma_free_coherent(dmadev, SQ_SIZE(depth), (void *)nvmeq->sq_cmds,
-							nvmeq->sq_dma_addr);
  free_cqdma:
 	dma_free_coherent(dmadev, CQ_SIZE(depth), (void *)nvmeq->cqes,
 							nvmeq->cq_dma_addr);
@@ -1335,17 +1228,15 @@ static int queue_request_irq(struct nvme_dev *dev, struct nvme_queue *nvmeq,
 static void nvme_init_queue(struct nvme_queue *nvmeq, u16 qid)
 {
 	struct nvme_dev *dev = nvmeq->dev;
-	unsigned extra = nvme_queue_extra(nvmeq->q_depth);
 
+	spin_lock_irq(&nvmeq->q_lock);
 	nvmeq->sq_tail = 0;
 	nvmeq->cq_head = 0;
 	nvmeq->cq_phase = 1;
 	nvmeq->q_db = &dev->dbs[qid * 2 * dev->db_stride];
-	memset(nvmeq->cmdid_data, 0, extra);
 	memset((void *)nvmeq->cqes, 0, CQ_SIZE(nvmeq->q_depth));
-	nvme_cancel_ios(nvmeq, false);
-	nvmeq->q_suspended = 0;
 	dev->online_queues++;
+	spin_unlock_irq(&nvmeq->q_lock);
 }
 
 static int nvme_create_queue(struct nvme_queue *nvmeq, int qid)
@@ -1365,10 +1256,7 @@ static int nvme_create_queue(struct nvme_queue *nvmeq, int qid)
 	if (result < 0)
 		goto release_sq;
 
-	spin_lock_irq(&nvmeq->q_lock);
 	nvme_init_queue(nvmeq, qid);
-	spin_unlock_irq(&nvmeq->q_lock);
-
 	return result;
 
  release_sq:
@@ -1408,27 +1296,32 @@ static int nvme_wait_ready(struct nvme_dev *dev, u64 cap, bool enabled)
  */
 static int nvme_disable_ctrl(struct nvme_dev *dev, u64 cap)
 {
-	u32 cc = readl(&dev->bar->cc);
+	dev->ctrl_config &= ~NVME_CC_SHN_MASK;
+	dev->ctrl_config &= ~NVME_CC_ENABLE;
+	writel(dev->ctrl_config, &dev->bar->cc);
 
-	if (cc & NVME_CC_ENABLE)
-		writel(cc & ~NVME_CC_ENABLE, &dev->bar->cc);
 	return nvme_wait_ready(dev, cap, false);
 }
 
 static int nvme_enable_ctrl(struct nvme_dev *dev, u64 cap)
 {
+	dev->ctrl_config &= ~NVME_CC_SHN_MASK;
+	dev->ctrl_config |= NVME_CC_ENABLE;
+	writel(dev->ctrl_config, &dev->bar->cc);
+
 	return nvme_wait_ready(dev, cap, true);
 }
 
 static int nvme_shutdown_ctrl(struct nvme_dev *dev)
 {
 	unsigned long timeout;
-	u32 cc;
 
-	cc = (readl(&dev->bar->cc) & ~NVME_CC_SHN_MASK) | NVME_CC_SHN_NORMAL;
-	writel(cc, &dev->bar->cc);
+	dev->ctrl_config &= ~NVME_CC_SHN_MASK;
+	dev->ctrl_config |= NVME_CC_SHN_NORMAL;
 
-	timeout = 2 * HZ + jiffies;
+	writel(dev->ctrl_config, &dev->bar->cc);
+
+	timeout = SHUTDOWN_TIMEOUT + jiffies;
 	while ((readl(&dev->bar->csts) & NVME_CSTS_SHST_MASK) !=
 							NVME_CSTS_SHST_CMPLT) {
 		msleep(100);
@@ -1444,20 +1337,86 @@ static int nvme_shutdown_ctrl(struct nvme_dev *dev)
 	return 0;
 }
 
+static struct blk_mq_ops nvme_mq_admin_ops = {
+	.queue_rq	= nvme_admin_queue_rq,
+	.map_queue	= blk_mq_map_queue,
+	.init_hctx	= nvme_admin_init_hctx,
+	.exit_hctx	= nvme_exit_hctx,
+	.init_request	= nvme_admin_init_request,
+	.timeout	= nvme_timeout,
+};
+
+static struct blk_mq_ops nvme_mq_ops = {
+	.queue_rq	= nvme_queue_rq,
+	.map_queue	= blk_mq_map_queue,
+	.init_hctx	= nvme_init_hctx,
+	.exit_hctx	= nvme_exit_hctx,
+	.init_request	= nvme_init_request,
+	.timeout	= nvme_timeout,
+};
+
+static int nvme_alloc_admin_tags(struct nvme_dev *dev)
+{
+	if (!dev->admin_q) {
+		dev->admin_tagset.ops = &nvme_mq_admin_ops;
+		dev->admin_tagset.nr_hw_queues = 1;
+		dev->admin_tagset.queue_depth = NVME_AQ_DEPTH - 1;
+		dev->admin_tagset.timeout = ADMIN_TIMEOUT;
+		dev->admin_tagset.numa_node = dev_to_node(&dev->pci_dev->dev);
+		dev->admin_tagset.cmd_size = sizeof(struct nvme_cmd_info);
+		dev->admin_tagset.driver_data = dev;
+
+		if (blk_mq_alloc_tag_set(&dev->admin_tagset))
+			return -ENOMEM;
+
+		dev->admin_q = blk_mq_init_queue(&dev->admin_tagset);
+		if (!dev->admin_q) {
+			blk_mq_free_tag_set(&dev->admin_tagset);
+			return -ENOMEM;
+		}
+	}
+
+	return 0;
+}
+
+static void nvme_free_admin_tags(struct nvme_dev *dev)
+{
+	if (dev->admin_q)
+		blk_mq_free_tag_set(&dev->admin_tagset);
+}
+
 static int nvme_configure_admin_queue(struct nvme_dev *dev)
 {
 	int result;
 	u32 aqa;
 	u64 cap = readq(&dev->bar->cap);
 	struct nvme_queue *nvmeq;
+	unsigned page_shift = PAGE_SHIFT;
+	unsigned dev_page_min = NVME_CAP_MPSMIN(cap) + 12;
+	unsigned dev_page_max = NVME_CAP_MPSMAX(cap) + 12;
+
+	if (page_shift < dev_page_min) {
+		dev_err(&dev->pci_dev->dev,
+				"Minimum device page size (%u) too large for "
+				"host (%u)\n", 1 << dev_page_min,
+				1 << page_shift);
+		return -ENODEV;
+	}
+	if (page_shift > dev_page_max) {
+		dev_info(&dev->pci_dev->dev,
+				"Device maximum page size (%u) smaller than "
+				"host (%u); enabling work-around\n",
+				1 << dev_page_max, 1 << page_shift);
+		page_shift = dev_page_max;
+	}
 
 	result = nvme_disable_ctrl(dev, cap);
 	if (result < 0)
 		return result;
 
-	nvmeq = raw_nvmeq(dev, 0);
+	nvmeq = dev->queues[0];
 	if (!nvmeq) {
-		nvmeq = nvme_alloc_queue(dev, 0, 64, 0);
+		nvmeq = nvme_alloc_queue(dev, 0, NVME_AQ_DEPTH, 0);
 		if (!nvmeq)
 			return -ENOMEM;
 	}
@@ -1465,27 +1424,35 @@ static int nvme_configure_admin_queue(struct nvme_dev *dev)
 	aqa = nvmeq->q_depth - 1;
 	aqa |= aqa << 16;
 
-	dev->ctrl_config = NVME_CC_ENABLE | NVME_CC_CSS_NVM;
-	dev->ctrl_config |= (PAGE_SHIFT - 12) << NVME_CC_MPS_SHIFT;
+	dev->page_size = 1 << page_shift;
+
+	dev->ctrl_config = NVME_CC_CSS_NVM;
+	dev->ctrl_config |= (page_shift - 12) << NVME_CC_MPS_SHIFT;
 	dev->ctrl_config |= NVME_CC_ARB_RR | NVME_CC_SHN_NONE;
 	dev->ctrl_config |= NVME_CC_IOSQES | NVME_CC_IOCQES;
 
 	writel(aqa, &dev->bar->aqa);
 	writeq(nvmeq->sq_dma_addr, &dev->bar->asq);
 	writeq(nvmeq->cq_dma_addr, &dev->bar->acq);
-	writel(dev->ctrl_config, &dev->bar->cc);
 
 	result = nvme_enable_ctrl(dev, cap);
 	if (result)
-		return result;
+		goto free_nvmeq;
+
+	result = nvme_alloc_admin_tags(dev);
+	if (result)
+		goto free_nvmeq;
 
 	result = queue_request_irq(dev, nvmeq, nvmeq->irqname);
 	if (result)
-		return result;
+		goto free_tags;
 
-	spin_lock_irq(&nvmeq->q_lock);
-	nvme_init_queue(nvmeq, 0);
-	spin_unlock_irq(&nvmeq->q_lock);
+	return result;
+
+ free_tags:
+	nvme_free_admin_tags(dev);
+ free_nvmeq:
+	nvme_free_queues(dev, 0);
 	return result;
 }
 
@@ -1516,7 +1483,7 @@ struct nvme_iod *nvme_map_user_pages(struct nvme_dev *dev, int write,
 	}
 
 	err = -ENOMEM;
-	iod = nvme_alloc_iod(count, length, GFP_KERNEL);
+	iod = nvme_alloc_iod(count, length, dev, GFP_KERNEL);
 	if (!iod)
 		goto put_pages;
 
@@ -1644,7 +1611,7 @@ static int nvme_submit_io(struct nvme_ns *ns, struct nvme_user_io __user *uio)
 	if (length != (io.nblocks + 1) << ns->lba_shift)
 		status = -ENOMEM;
 	else
-		status = nvme_submit_io_cmd(dev, &c, NULL);
+		status = nvme_submit_io_cmd(dev, ns, &c, NULL);
 
 	if (meta_len) {
 		if (status == NVME_SC_SUCCESS && !(io.opcode & 1)) {
@@ -1676,10 +1643,10 @@ static int nvme_submit_io(struct nvme_ns *ns, struct nvme_user_io __user *uio)
 	return status;
 }
 
-static int nvme_user_admin_cmd(struct nvme_dev *dev,
-					struct nvme_admin_cmd __user *ucmd)
+static int nvme_user_cmd(struct nvme_dev *dev, struct nvme_ns *ns,
+			struct nvme_passthru_cmd __user *ucmd)
 {
-	struct nvme_admin_cmd cmd;
+	struct nvme_passthru_cmd cmd;
 	struct nvme_command c;
 	int status, length;
 	struct nvme_iod *uninitialized_var(iod);
@@ -1716,10 +1683,23 @@ static int nvme_user_admin_cmd(struct nvme_dev *dev,
 
 	timeout = cmd.timeout_ms ? msecs_to_jiffies(cmd.timeout_ms) :
 								ADMIN_TIMEOUT;
+
 	if (length != cmd.data_len)
 		status = -ENOMEM;
-	else
-		status = nvme_submit_sync_cmd(dev, 0, &c, &cmd.result, timeout);
+	else if (ns) {
+		struct request *req;
+
+		req = blk_mq_alloc_request(ns->queue, WRITE,
+						(GFP_KERNEL|__GFP_WAIT), false);
+		if (IS_ERR(req))
+			status = PTR_ERR(req);
+		else {
+			status = nvme_submit_sync_cmd(req, &c, &cmd.result,
+								timeout);
+			blk_mq_free_request(req);
+		}
+	} else
+		status = __nvme_submit_admin_cmd(dev, &c, &cmd.result, timeout);
 
 	if (cmd.data_len) {
 		nvme_unmap_user_pages(dev, cmd.opcode & 1, iod);
@@ -1743,7 +1723,9 @@ static int nvme_ioctl(struct block_device *bdev, fmode_t mode, unsigned int cmd,
 		force_successful_syscall_return();
 		return ns->ns_id;
 	case NVME_IOCTL_ADMIN_CMD:
-		return nvme_user_admin_cmd(ns->dev, (void __user *)arg);
+		return nvme_user_cmd(ns->dev, NULL, (void __user *)arg);
+	case NVME_IOCTL_IO_CMD:
+		return nvme_user_cmd(ns->dev, ns, (void __user *)arg);
 	case NVME_IOCTL_SUBMIT_IO:
 		return nvme_submit_io(ns, (void __user *)arg);
 	case SG_GET_VERSION_NUM:
@@ -1759,11 +1741,9 @@ static int nvme_ioctl(struct block_device *bdev, fmode_t mode, unsigned int cmd,
 static int nvme_compat_ioctl(struct block_device *bdev, fmode_t mode,
 					unsigned int cmd, unsigned long arg)
 {
-	struct nvme_ns *ns = bdev->bd_disk->private_data;
-
 	switch (cmd) {
 	case SG_IO:
-		return nvme_sg_io32(ns, arg);
+		return -ENOIOCTLCMD;
 	}
 	return nvme_ioctl(bdev, mode, cmd, arg);
 }
@@ -1773,11 +1753,18 @@ static int nvme_compat_ioctl(struct block_device *bdev, fmode_t mode,
 
 static int nvme_open(struct block_device *bdev, fmode_t mode)
 {
-	struct nvme_ns *ns = bdev->bd_disk->private_data;
-	struct nvme_dev *dev = ns->dev;
+	int ret = 0;
+	struct nvme_ns *ns;
 
-	kref_get(&dev->kref);
-	return 0;
+	spin_lock(&dev_list_lock);
+	ns = bdev->bd_disk->private_data;
+	if (!ns)
+		ret = -ENXIO;
+	else if (!kref_get_unless_zero(&ns->dev->kref))
+		ret = -ENXIO;
+	spin_unlock(&dev_list_lock);
+
+	return ret;
 }
 
 static void nvme_free_dev(struct kref *kref);
@@ -1799,6 +1786,35 @@ static int nvme_getgeo(struct block_device *bd, struct hd_geometry *geo)
 	return 0;
 }
 
+static int nvme_revalidate_disk(struct gendisk *disk)
+{
+	struct nvme_ns *ns = disk->private_data;
+	struct nvme_dev *dev = ns->dev;
+	struct nvme_id_ns *id;
+	dma_addr_t dma_addr;
+	int lbaf;
+
+	id = dma_alloc_coherent(&dev->pci_dev->dev, 4096, &dma_addr,
+								GFP_KERNEL);
+	if (!id) {
+		dev_warn(&dev->pci_dev->dev, "%s: Memory alocation failure\n",
+								__func__);
+		return 0;
+	}
+
+	if (nvme_identify(dev, ns->ns_id, 0, dma_addr))
+		goto free;
+
+	lbaf = id->flbas & 0xf;
+	ns->lba_shift = id->lbaf[lbaf].ds;
+
+	blk_queue_logical_block_size(ns->queue, 1 << ns->lba_shift);
+	set_capacity(disk, le64_to_cpup(&id->nsze) << (ns->lba_shift - 9));
+ free:
+	dma_free_coherent(&dev->pci_dev->dev, 4096, id, dma_addr);
+	return 0;
+}
+
 static const struct block_device_operations nvme_fops = {
 	.owner		= THIS_MODULE,
 	.ioctl		= nvme_ioctl,
@@ -1806,43 +1822,9 @@ static const struct block_device_operations nvme_fops = {
 	.open		= nvme_open,
 	.release	= nvme_release,
 	.getgeo		= nvme_getgeo,
+	.revalidate_disk= nvme_revalidate_disk,
 };
 
-static void nvme_resubmit_iods(struct nvme_queue *nvmeq)
-{
-	struct nvme_iod *iod, *next;
-
-	list_for_each_entry_safe(iod, next, &nvmeq->iod_bio, node) {
-		if (unlikely(nvme_submit_iod(nvmeq, iod)))
-			break;
-		list_del(&iod->node);
-		if (bio_list_empty(&nvmeq->sq_cong) &&
-						list_empty(&nvmeq->iod_bio))
-			remove_wait_queue(&nvmeq->sq_full,
-						&nvmeq->sq_cong_wait);
-	}
-}
-
-static void nvme_resubmit_bios(struct nvme_queue *nvmeq)
-{
-	while (bio_list_peek(&nvmeq->sq_cong)) {
-		struct bio *bio = bio_list_pop(&nvmeq->sq_cong);
-		struct nvme_ns *ns = bio->bi_bdev->bd_disk->private_data;
-
-		if (bio_list_empty(&nvmeq->sq_cong) &&
-						list_empty(&nvmeq->iod_bio))
-			remove_wait_queue(&nvmeq->sq_full,
-							&nvmeq->sq_cong_wait);
-		if (nvme_submit_bio_queue(nvmeq, ns, bio)) {
-			if (!waitqueue_active(&nvmeq->sq_full))
-				add_wait_queue(&nvmeq->sq_full,
-							&nvmeq->sq_cong_wait);
-			bio_list_add_head(&nvmeq->sq_cong, bio);
-			break;
-		}
-	}
-}
-
 static int nvme_kthread(void *data)
 {
 	struct nvme_dev *dev, *next;
@@ -1858,28 +1840,26 @@ static int nvme_kthread(void *data)
 					continue;
 				list_del_init(&dev->node);
 				dev_warn(&dev->pci_dev->dev,
-					"Failed status, reset controller\n");
+					"Failed status: %x, reset controller\n",
+					readl(&dev->bar->csts));
 				dev->reset_workfn = nvme_reset_failed_dev;
 				queue_work(nvme_workq, &dev->reset_work);
 				continue;
 			}
-			rcu_read_lock();
 			for (i = 0; i < dev->queue_count; i++) {
-				struct nvme_queue *nvmeq =
-						rcu_dereference(dev->queues[i]);
+				struct nvme_queue *nvmeq = dev->queues[i];
 				if (!nvmeq)
 					continue;
 				spin_lock_irq(&nvmeq->q_lock);
-				if (nvmeq->q_suspended)
-					goto unlock;
 				nvme_process_cq(nvmeq);
-				nvme_cancel_ios(nvmeq, true);
-				nvme_resubmit_bios(nvmeq);
-				nvme_resubmit_iods(nvmeq);
- unlock:
+
+				while ((i == 0) && (dev->event_limit > 0)) {
+					if (nvme_submit_async_admin_req(dev))
+						break;
+					dev->event_limit--;
+				}
 				spin_unlock_irq(&nvmeq->q_lock);
 			}
-			rcu_read_unlock();
 		}
 		spin_unlock(&dev_list_lock);
 		schedule_timeout(round_jiffies_relative(HZ));
@@ -1902,28 +1882,28 @@ static struct nvme_ns *nvme_alloc_ns(struct nvme_dev *dev, unsigned nsid,
 {
 	struct nvme_ns *ns;
 	struct gendisk *disk;
+	int node = dev_to_node(&dev->pci_dev->dev);
 	int lbaf;
 
 	if (rt->attributes & NVME_LBART_ATTRIB_HIDE)
 		return NULL;
 
-	ns = kzalloc(sizeof(*ns), GFP_KERNEL);
+	ns = kzalloc_node(sizeof(*ns), GFP_KERNEL, node);
 	if (!ns)
 		return NULL;
-	ns->queue = blk_alloc_queue(GFP_KERNEL);
-	if (!ns->queue)
+	ns->queue = blk_mq_init_queue(&dev->tagset);
+	if (IS_ERR(ns->queue))
 		goto out_free_ns;
-	ns->queue->queue_flags = QUEUE_FLAG_DEFAULT;
 	queue_flag_set_unlocked(QUEUE_FLAG_NOMERGES, ns->queue);
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, ns->queue);
-	queue_flag_clear_unlocked(QUEUE_FLAG_ADD_RANDOM, ns->queue);
-	blk_queue_make_request(ns->queue, nvme_make_request);
+	queue_flag_set_unlocked(QUEUE_FLAG_SG_GAPS, ns->queue);
 	ns->dev = dev;
 	ns->queue->queuedata = ns;
 
-	disk = alloc_disk(0);
+	disk = alloc_disk_node(0, node);
 	if (!disk)
 		goto out_free_queue;
+
 	ns->ns_id = nsid;
 	ns->disk = disk;
 	lbaf = id->flbas & 0xf;
@@ -1932,6 +1912,8 @@ static struct nvme_ns *nvme_alloc_ns(struct nvme_dev *dev, unsigned nsid,
 	blk_queue_logical_block_size(ns->queue, 1 << ns->lba_shift);
 	if (dev->max_hw_sectors)
 		blk_queue_max_hw_sectors(ns->queue, dev->max_hw_sectors);
+	if (dev->stripe_size)
+		blk_queue_chunk_sectors(ns->queue, dev->stripe_size >> 9);
 	if (dev->vwc & NVME_CTRL_VWC_PRESENT)
 		blk_queue_flush(ns->queue, REQ_FLUSH | REQ_FUA);
 
@@ -1957,143 +1939,19 @@ static struct nvme_ns *nvme_alloc_ns(struct nvme_dev *dev, unsigned nsid,
 	return NULL;
 }
 
-static int nvme_find_closest_node(int node)
-{
-	int n, val, min_val = INT_MAX, best_node = node;
-
-	for_each_online_node(n) {
-		if (n == node)
-			continue;
-		val = node_distance(node, n);
-		if (val < min_val) {
-			min_val = val;
-			best_node = n;
-		}
-	}
-	return best_node;
-}
-
-static void nvme_set_queue_cpus(cpumask_t *qmask, struct nvme_queue *nvmeq,
-								int count)
-{
-	int cpu;
-	for_each_cpu(cpu, qmask) {
-		if (cpumask_weight(nvmeq->cpu_mask) >= count)
-			break;
-		if (!cpumask_test_and_set_cpu(cpu, nvmeq->cpu_mask))
-			*per_cpu_ptr(nvmeq->dev->io_queue, cpu) = nvmeq->qid;
-	}
-}
-
-static void nvme_add_cpus(cpumask_t *mask, const cpumask_t *unassigned_cpus,
-	const cpumask_t *new_mask, struct nvme_queue *nvmeq, int cpus_per_queue)
-{
-	int next_cpu;
-	for_each_cpu(next_cpu, new_mask) {
-		cpumask_or(mask, mask, get_cpu_mask(next_cpu));
-		cpumask_or(mask, mask, topology_thread_cpumask(next_cpu));
-		cpumask_and(mask, mask, unassigned_cpus);
-		nvme_set_queue_cpus(mask, nvmeq, cpus_per_queue);
-	}
-}
-
 static void nvme_create_io_queues(struct nvme_dev *dev)
 {
-	unsigned i, max;
+	unsigned i;
 
-	max = min(dev->max_qid, num_online_cpus());
-	for (i = dev->queue_count; i <= max; i++)
+	for (i = dev->queue_count; i <= dev->max_qid; i++)
 		if (!nvme_alloc_queue(dev, i, dev->q_depth, i - 1))
 			break;
 
-	max = min(dev->queue_count - 1, num_online_cpus());
-	for (i = dev->online_queues; i <= max; i++)
-		if (nvme_create_queue(raw_nvmeq(dev, i), i))
+	for (i = dev->online_queues; i <= dev->queue_count - 1; i++)
+		if (nvme_create_queue(dev->queues[i], i))
 			break;
 }
 
-/*
- * If there are fewer queues than online cpus, this will try to optimally
- * assign a queue to multiple cpus by grouping cpus that are "close" together:
- * thread siblings, core, socket, closest node, then whatever else is
- * available.
- */
-static void nvme_assign_io_queues(struct nvme_dev *dev)
-{
-	unsigned cpu, cpus_per_queue, queues, remainder, i;
-	cpumask_var_t unassigned_cpus;
-
-	nvme_create_io_queues(dev);
-
-	queues = min(dev->online_queues - 1, num_online_cpus());
-	if (!queues)
-		return;
-
-	cpus_per_queue = num_online_cpus() / queues;
-	remainder = queues - (num_online_cpus() - queues * cpus_per_queue);
-
-	if (!alloc_cpumask_var(&unassigned_cpus, GFP_KERNEL))
-		return;
-
-	cpumask_copy(unassigned_cpus, cpu_online_mask);
-	cpu = cpumask_first(unassigned_cpus);
-	for (i = 1; i <= queues; i++) {
-		struct nvme_queue *nvmeq = lock_nvmeq(dev, i);
-		cpumask_t mask;
-
-		cpumask_clear(nvmeq->cpu_mask);
-		if (!cpumask_weight(unassigned_cpus)) {
-			unlock_nvmeq(nvmeq);
-			break;
-		}
-
-		mask = *get_cpu_mask(cpu);
-		nvme_set_queue_cpus(&mask, nvmeq, cpus_per_queue);
-		if (cpus_weight(mask) < cpus_per_queue)
-			nvme_add_cpus(&mask, unassigned_cpus,
-				topology_thread_cpumask(cpu),
-				nvmeq, cpus_per_queue);
-		if (cpus_weight(mask) < cpus_per_queue)
-			nvme_add_cpus(&mask, unassigned_cpus,
-				topology_core_cpumask(cpu),
-				nvmeq, cpus_per_queue);
-		if (cpus_weight(mask) < cpus_per_queue)
-			nvme_add_cpus(&mask, unassigned_cpus,
-				cpumask_of_node(cpu_to_node(cpu)),
-				nvmeq, cpus_per_queue);
-		if (cpus_weight(mask) < cpus_per_queue)
-			nvme_add_cpus(&mask, unassigned_cpus,
-				cpumask_of_node(
-					nvme_find_closest_node(
-						cpu_to_node(cpu))),
-				nvmeq, cpus_per_queue);
-		if (cpus_weight(mask) < cpus_per_queue)
-			nvme_add_cpus(&mask, unassigned_cpus,
-				unassigned_cpus,
-				nvmeq, cpus_per_queue);
-
-		WARN(cpumask_weight(nvmeq->cpu_mask) != cpus_per_queue,
-			"nvme%d qid:%d mis-matched queue-to-cpu assignment\n",
-			dev->instance, i);
-
-		irq_set_affinity_hint(dev->entry[nvmeq->cq_vector].vector,
-							nvmeq->cpu_mask);
-		cpumask_andnot(unassigned_cpus, unassigned_cpus,
-						nvmeq->cpu_mask);
-		cpu = cpumask_next(cpu, unassigned_cpus);
-		if (remainder && !--remainder)
-			cpus_per_queue++;
-		unlock_nvmeq(nvmeq);
-	}
-	WARN(cpumask_weight(unassigned_cpus), "nvme%d unassigned online cpus\n",
-								dev->instance);
-	i = 0;
-	cpumask_andnot(unassigned_cpus, cpu_possible_mask, cpu_online_mask);
-	for_each_cpu(cpu, unassigned_cpus)
-		*per_cpu_ptr(dev->io_queue, cpu) = (i++ % queues) + 1;
-	free_cpumask_var(unassigned_cpus);
-}
-
 static int set_queue_count(struct nvme_dev *dev, int count)
 {
 	int status;
@@ -2107,7 +1965,7 @@ static int set_queue_count(struct nvme_dev *dev, int count)
 	if (status > 0) {
 		dev_err(&dev->pci_dev->dev, "Could not set queue count (%d)\n",
 									status);
-		return -EBUSY;
+		return 0;
 	}
 	return min(result & 0xffff, result >> 16) + 1;
 }
@@ -2117,39 +1975,15 @@ static size_t db_bar_size(struct nvme_dev *dev, unsigned nr_io_queues)
 	return 4096 + ((nr_io_queues + 1) * 8 * dev->db_stride);
 }
 
-static void nvme_cpu_workfn(struct work_struct *work)
-{
-	struct nvme_dev *dev = container_of(work, struct nvme_dev, cpu_work);
-	if (dev->initialized)
-		nvme_assign_io_queues(dev);
-}
-
-static int nvme_cpu_notify(struct notifier_block *self,
-				unsigned long action, void *hcpu)
-{
-	struct nvme_dev *dev;
-
-	switch (action) {
-	case CPU_ONLINE:
-	case CPU_DEAD:
-		spin_lock(&dev_list_lock);
-		list_for_each_entry(dev, &dev_list, node)
-			schedule_work(&dev->cpu_work);
-		spin_unlock(&dev_list_lock);
-		break;
-	}
-	return NOTIFY_OK;
-}
-
 static int nvme_setup_io_queues(struct nvme_dev *dev)
 {
-	struct nvme_queue *adminq = raw_nvmeq(dev, 0);
+	struct nvme_queue *adminq = dev->queues[0];
 	struct pci_dev *pdev = dev->pci_dev;
 	int result, i, vecs, nr_io_queues, size;
 
 	nr_io_queues = num_possible_cpus();
 	result = set_queue_count(dev, nr_io_queues);
-	if (result < 0)
+	if (result <= 0)
 		return result;
 	if (result < nr_io_queues)
 		nr_io_queues = result;
@@ -2172,6 +2006,13 @@ static int nvme_setup_io_queues(struct nvme_dev *dev)
 	/* Deregister the admin queue's interrupt */
 	free_irq(dev->entry[0].vector, adminq);
 
+	/*
+	 * If we enable msix early due to not intx, disable it again before
+	 * setting up the full range we need.
+	 */
+	if (!pdev->irq)
+		pci_disable_msix(pdev);
+
 	for (i = 0; i < nr_io_queues; i++)
 		dev->entry[i].entry = i;
 	vecs = pci_enable_msix_range(pdev, dev->entry, 1, nr_io_queues);
@@ -2195,14 +2036,12 @@ static int nvme_setup_io_queues(struct nvme_dev *dev)
 	dev->max_qid = nr_io_queues;
 
 	result = queue_request_irq(dev, adminq, adminq->irqname);
-	if (result) {
-		adminq->q_suspended = 1;
+	if (result)
 		goto free_queues;
-	}
 
 	/* Free previously allocated queues that are no longer usable */
 	nvme_free_queues(dev, nr_io_queues + 1);
-	nvme_assign_io_queues(dev);
+	nvme_create_io_queues(dev);
 
 	return 0;
 
@@ -2245,14 +2084,37 @@ static int nvme_dev_add(struct nvme_dev *dev)
 	dev->oncs = le16_to_cpup(&ctrl->oncs);
 	dev->abort_limit = ctrl->acl + 1;
 	dev->vwc = ctrl->vwc;
+	dev->event_limit = min(ctrl->aerl + 1, 8);
 	memcpy(dev->serial, ctrl->sn, sizeof(ctrl->sn));
 	memcpy(dev->model, ctrl->mn, sizeof(ctrl->mn));
 	memcpy(dev->firmware_rev, ctrl->fr, sizeof(ctrl->fr));
 	if (ctrl->mdts)
 		dev->max_hw_sectors = 1 << (ctrl->mdts + shift - 9);
 	if ((pdev->vendor == PCI_VENDOR_ID_INTEL) &&
-			(pdev->device == 0x0953) && ctrl->vs[3])
+			(pdev->device == 0x0953) && ctrl->vs[3]) {
+		unsigned int max_hw_sectors;
+
 		dev->stripe_size = 1 << (ctrl->vs[3] + shift);
+		max_hw_sectors = dev->stripe_size >> (shift - 9);
+		if (dev->max_hw_sectors) {
+			dev->max_hw_sectors = min(max_hw_sectors,
+							dev->max_hw_sectors);
+		} else
+			dev->max_hw_sectors = max_hw_sectors;
+	}
+
+	dev->tagset.ops = &nvme_mq_ops;
+	dev->tagset.nr_hw_queues = dev->online_queues - 1;
+	dev->tagset.timeout = NVME_IO_TIMEOUT;
+	dev->tagset.numa_node = dev_to_node(&dev->pci_dev->dev);
+	dev->tagset.queue_depth =
+				min_t(int, dev->q_depth, BLK_MQ_MAX_DEPTH) - 1;
+	dev->tagset.cmd_size = sizeof(struct nvme_cmd_info);
+	dev->tagset.flags = BLK_MQ_F_SHOULD_MERGE;
+	dev->tagset.driver_data = dev;
+
+	if (blk_mq_alloc_tag_set(&dev->tagset))
+		goto out;
 
 	id_ns = mem;
 	for (i = 1; i <= nn; i++) {
@@ -2293,6 +2155,9 @@ static int nvme_dev_map(struct nvme_dev *dev)
 	dev->entry[0].vector = pdev->irq;
 	pci_set_master(pdev);
 	bars = pci_select_bars(pdev, IORESOURCE_MEM);
+	if (!bars)
+		goto disable_pci;
+
 	if (pci_request_selected_regions(pdev, bars, "nvme"))
 		goto disable_pci;
 
@@ -2303,10 +2168,22 @@ static int nvme_dev_map(struct nvme_dev *dev)
 	dev->bar = ioremap(pci_resource_start(pdev, 0), 8192);
 	if (!dev->bar)
 		goto disable;
+
 	if (readl(&dev->bar->csts) == -1) {
 		result = -ENODEV;
 		goto unmap;
 	}
+
+	/*
+	 * Some devices don't advertse INTx interrupts, pre-enable a single
+	 * MSIX vec for setup. We'll adjust this later.
+	 */
+	if (!pdev->irq) {
+		result = pci_enable_msix(pdev, dev->entry, 1);
+		if (result < 0)
+			goto unmap;
+	}
+
 	cap = readq(&dev->bar->cap);
 	dev->q_depth = min_t(int, NVME_CAP_MQES(cap) + 1, NVME_Q_DEPTH);
 	dev->db_stride = 1 << NVME_CAP_STRIDE(cap);
@@ -2402,7 +2279,8 @@ static int adapter_async_del_queue(struct nvme_queue *nvmeq, u8 opcode,
 	c.delete_queue.qid = cpu_to_le16(nvmeq->qid);
 
 	init_kthread_work(&nvmeq->cmdinfo.work, fn);
-	return nvme_submit_admin_cmd_async(nvmeq->dev, &c, &nvmeq->cmdinfo);
+	return nvme_submit_admin_async_cmd(nvmeq->dev, &c, &nvmeq->cmdinfo,
+								ADMIN_TIMEOUT);
 }
 
 static void nvme_del_cq_work_handler(struct kthread_work *work)
@@ -2465,7 +2343,7 @@ static void nvme_disable_io_queues(struct nvme_dev *dev)
 	atomic_set(&dq.refcount, 0);
 	dq.worker = &worker;
 	for (i = dev->queue_count - 1; i > 0; i--) {
-		struct nvme_queue *nvmeq = raw_nvmeq(dev, i);
+		struct nvme_queue *nvmeq = dev->queues[i];
 
 		if (nvme_suspend_queue(nvmeq))
 			continue;
@@ -2501,13 +2379,16 @@ static void nvme_dev_list_remove(struct nvme_dev *dev)
 static void nvme_dev_shutdown(struct nvme_dev *dev)
 {
 	int i;
+	u32 csts = -1;
 
 	dev->initialized = 0;
 	nvme_dev_list_remove(dev);
 
-	if (!dev->bar || (dev->bar && readl(&dev->bar->csts) == -1)) {
+	if (dev->bar)
+		csts = readl(&dev->bar->csts);
+	if (csts & NVME_CSTS_CFS || !(csts & NVME_CSTS_RDY)) {
 		for (i = dev->queue_count - 1; i >= 0; i--) {
-			struct nvme_queue *nvmeq = raw_nvmeq(dev, i);
+			struct nvme_queue *nvmeq = dev->queues[i];
 			nvme_suspend_queue(nvmeq);
 			nvme_clear_queue(nvmeq);
 		}
@@ -2519,6 +2400,12 @@ static void nvme_dev_shutdown(struct nvme_dev *dev)
 	nvme_dev_unmap(dev);
 }
 
+static void nvme_dev_remove_admin(struct nvme_dev *dev)
+{
+	if (dev->admin_q && !blk_queue_dying(dev->admin_q))
+		blk_cleanup_queue(dev->admin_q);
+}
+
 static void nvme_dev_remove(struct nvme_dev *dev)
 {
 	struct nvme_ns *ns;
@@ -2590,6 +2477,11 @@ static void nvme_free_namespaces(struct nvme_dev *dev)
 
 	list_for_each_entry_safe(ns, next, &dev->namespaces, list) {
 		list_del(&ns->list);
+
+		spin_lock(&dev_list_lock);
+		ns->disk->private_data = NULL;
+		spin_unlock(&dev_list_lock);
+
 		put_disk(ns->disk);
 		kfree(ns);
 	}
@@ -2599,8 +2491,10 @@ static void nvme_free_dev(struct kref *kref)
 {
 	struct nvme_dev *dev = container_of(kref, struct nvme_dev, kref);
 
+	pci_dev_put(dev->pci_dev);
 	nvme_free_namespaces(dev);
-	free_percpu(dev->io_queue);
+	nvme_release_instance(dev);
+	blk_mq_free_tag_set(&dev->tagset);
 	kfree(dev->queues);
 	kfree(dev->entry);
 	kfree(dev);
@@ -2625,9 +2519,16 @@ static int nvme_dev_release(struct inode *inode, struct file *f)
 static long nvme_dev_ioctl(struct file *f, unsigned int cmd, unsigned long arg)
 {
 	struct nvme_dev *dev = f->private_data;
+	struct nvme_ns *ns;
+
 	switch (cmd) {
 	case NVME_IOCTL_ADMIN_CMD:
-		return nvme_user_admin_cmd(dev, (void __user *)arg);
+		return nvme_user_cmd(dev, NULL, (void __user *)arg);
+	case NVME_IOCTL_IO_CMD:
+		if (list_empty(&dev->namespaces))
+			return -ENOTTY;
+		ns = list_first_entry(&dev->namespaces, struct nvme_ns, list);
+		return nvme_user_cmd(dev, ns, (void __user *)arg);
 	default:
 		return -ENOTTY;
 	}
@@ -2641,6 +2542,22 @@ static const struct file_operations nvme_dev_fops = {
 	.compat_ioctl	= nvme_dev_ioctl,
 };
 
+static void nvme_set_irq_hints(struct nvme_dev *dev)
+{
+	struct nvme_queue *nvmeq;
+	int i;
+
+	for (i = 0; i < dev->online_queues; i++) {
+		nvmeq = dev->queues[i];
+
+		if (!nvmeq->hctx)
+			continue;
+
+		irq_set_affinity_hint(dev->entry[nvmeq->cq_vector].vector,
+							nvmeq->hctx->cpumask);
+	}
+}
+
 static int nvme_dev_start(struct nvme_dev *dev)
 {
 	int result;
@@ -2664,7 +2581,7 @@ static int nvme_dev_start(struct nvme_dev *dev)
 
 	if (start_thread) {
 		nvme_thread = kthread_run(nvme_kthread, NULL, "nvme");
-		wake_up(&nvme_kthread_wait);
+		wake_up_all(&nvme_kthread_wait);
 	} else
 		wait_event_killable(nvme_kthread_wait, nvme_thread);
 
@@ -2673,10 +2590,14 @@ static int nvme_dev_start(struct nvme_dev *dev)
 		goto disable;
 	}
 
+	nvme_init_queue(dev->queues[0], 0);
+
 	result = nvme_setup_io_queues(dev);
-	if (result && result != -EBUSY)
+	if (result)
 		goto disable;
 
+	nvme_set_irq_hints(dev);
+
 	return result;
 
  disable:
@@ -2693,7 +2614,7 @@ static int nvme_remove_dead_ctrl(void *arg)
 	struct pci_dev *pdev = dev->pci_dev;
 
 	if (pci_get_drvdata(pdev))
-		pci_stop_and_remove_bus_device(pdev);
+		pci_stop_and_remove_bus_device_locked(pdev);
 	kref_put(&dev->kref, nvme_free_dev);
 	return 0;
 }
@@ -2702,8 +2623,8 @@ static void nvme_remove_disks(struct work_struct *ws)
 {
 	struct nvme_dev *dev = container_of(ws, struct nvme_dev, reset_work);
 
-	nvme_dev_remove(dev);
 	nvme_free_queues(dev, 1);
+	nvme_dev_remove(dev);
 }
 
 static int nvme_dev_resume(struct nvme_dev *dev)
@@ -2711,9 +2632,9 @@ static int nvme_dev_resume(struct nvme_dev *dev)
 	int ret;
 
 	ret = nvme_dev_start(dev);
-	if (ret && ret != -EBUSY)
+	if (ret)
 		return ret;
-	if (ret == -EBUSY) {
+	if (dev->online_queues < 2) {
 		spin_lock(&dev_list_lock);
 		dev->reset_workfn = nvme_remove_disks;
 		queue_work(nvme_workq, &dev->reset_work);
@@ -2727,7 +2648,7 @@ static void nvme_dev_reset(struct nvme_dev *dev)
 {
 	nvme_dev_shutdown(dev);
 	if (nvme_dev_resume(dev)) {
-		dev_err(&dev->pci_dev->dev, "Device failed to resume\n");
+		dev_warn(&dev->pci_dev->dev, "Device failed to resume\n");
 		kref_get(&dev->kref);
 		if (IS_ERR(kthread_run(nvme_remove_dead_ctrl, dev, "nvme%d",
 							dev->instance))) {
@@ -2752,33 +2673,33 @@ static void nvme_reset_workfn(struct work_struct *work)
 
 static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
-	int result = -ENOMEM;
+	int node, result = -ENOMEM;
 	struct nvme_dev *dev;
 
-	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	node = dev_to_node(&pdev->dev);
+	if (node == NUMA_NO_NODE)
+		set_dev_node(&pdev->dev, 0);
+
+	dev = kzalloc_node(sizeof(*dev), GFP_KERNEL, node);
 	if (!dev)
 		return -ENOMEM;
-	dev->entry = kcalloc(num_possible_cpus(), sizeof(*dev->entry),
-								GFP_KERNEL);
+	dev->entry = kzalloc_node(num_possible_cpus() * sizeof(*dev->entry),
+							GFP_KERNEL, node);
 	if (!dev->entry)
 		goto free;
-	dev->queues = kcalloc(num_possible_cpus() + 1, sizeof(void *),
-								GFP_KERNEL);
+	dev->queues = kzalloc_node((num_possible_cpus() + 1) * sizeof(void *),
+							GFP_KERNEL, node);
 	if (!dev->queues)
 		goto free;
-	dev->io_queue = alloc_percpu(unsigned short);
-	if (!dev->io_queue)
-		goto free;
 
 	INIT_LIST_HEAD(&dev->namespaces);
 	dev->reset_workfn = nvme_reset_failed_dev;
 	INIT_WORK(&dev->reset_work, nvme_reset_workfn);
-	INIT_WORK(&dev->cpu_work, nvme_cpu_workfn);
-	dev->pci_dev = pdev;
+	dev->pci_dev = pci_dev_get(pdev);
 	pci_set_drvdata(pdev, dev);
 	result = nvme_set_instance(dev);
 	if (result)
-		goto free;
+		goto put_pci;
 
 	result = nvme_setup_prp_pools(dev);
 	if (result)
@@ -2786,17 +2707,14 @@ static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	kref_init(&dev->kref);
 	result = nvme_dev_start(dev);
-	if (result) {
-		if (result == -EBUSY)
-			goto create_cdev;
+	if (result)
 		goto release_pools;
-	}
 
-	result = nvme_dev_add(dev);
+	if (dev->online_queues > 1)
+		result = nvme_dev_add(dev);
 	if (result)
 		goto shutdown;
 
- create_cdev:
 	scnprintf(dev->name, sizeof(dev->name), "nvme%d", dev->instance);
 	dev->miscdev.minor = MISC_DYNAMIC_MINOR;
 	dev->miscdev.parent = &pdev->dev;
@@ -2806,11 +2724,14 @@ static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (result)
 		goto remove;
 
+	nvme_set_irq_hints(dev);
+
 	dev->initialized = 1;
 	return 0;
 
  remove:
 	nvme_dev_remove(dev);
+	nvme_dev_remove_admin(dev);
 	nvme_free_namespaces(dev);
  shutdown:
 	nvme_dev_shutdown(dev);
@@ -2819,8 +2740,9 @@ static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	nvme_release_prp_pools(dev);
  release:
 	nvme_release_instance(dev);
+ put_pci:
+	pci_dev_put(dev->pci_dev);
  free:
-	free_percpu(dev->io_queue);
 	kfree(dev->queues);
 	kfree(dev->entry);
 	kfree(dev);
@@ -2829,12 +2751,12 @@ static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 static void nvme_reset_notify(struct pci_dev *pdev, bool prepare)
 {
-       struct nvme_dev *dev = pci_get_drvdata(pdev);
+	struct nvme_dev *dev = pci_get_drvdata(pdev);
 
-       if (prepare)
-               nvme_dev_shutdown(dev);
-       else
-               nvme_dev_resume(dev);
+	if (prepare)
+		nvme_dev_shutdown(dev);
+	else
+		nvme_dev_resume(dev);
 }
 
 static void nvme_shutdown(struct pci_dev *pdev)
@@ -2853,13 +2775,12 @@ static void nvme_remove(struct pci_dev *pdev)
 
 	pci_set_drvdata(pdev, NULL);
 	flush_work(&dev->reset_work);
-	flush_work(&dev->cpu_work);
 	misc_deregister(&dev->miscdev);
 	nvme_dev_remove(dev);
 	nvme_dev_shutdown(dev);
+	nvme_dev_remove_admin(dev);
 	nvme_free_queues(dev, 0);
-	rcu_barrier();
-	nvme_release_instance(dev);
+	nvme_free_admin_tags(dev);
 	nvme_release_prp_pools(dev);
 	kref_put(&dev->kref, nvme_free_dev);
 }
@@ -2942,18 +2863,11 @@ static int __init nvme_init(void)
 	else if (result > 0)
 		nvme_major = result;
 
-	nvme_nb.notifier_call = &nvme_cpu_notify;
-	result = register_hotcpu_notifier(&nvme_nb);
-	if (result)
-		goto unregister_blkdev;
-
 	result = pci_register_driver(&nvme_driver);
 	if (result)
-		goto unregister_hotcpu;
+		goto unregister_blkdev;
 	return 0;
 
- unregister_hotcpu:
-	unregister_hotcpu_notifier(&nvme_nb);
  unregister_blkdev:
 	unregister_blkdev(nvme_major, "nvme");
  kill_workq:
@@ -2973,6 +2887,6 @@ static void __exit nvme_exit(void)
 
 MODULE_AUTHOR("Matthew Wilcox <willy@linux.intel.com>");
 MODULE_LICENSE("GPL");
-MODULE_VERSION("0.9");
+MODULE_VERSION("1.0");
 module_init(nvme_init);
 module_exit(nvme_exit);
diff --git a/drivers/block/nvme-scsi.c b/drivers/block/nvme-scsi.c
index 0b4b2775600e..5e78568026c3 100644
--- a/drivers/block/nvme-scsi.c
+++ b/drivers/block/nvme-scsi.c
@@ -2105,7 +2105,7 @@ static int nvme_trans_do_nvme_io(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 
 		nvme_offset += unit_num_blocks;
 
-		nvme_sc = nvme_submit_io_cmd(dev, &c, NULL);
+		nvme_sc = nvme_submit_io_cmd(dev, ns, &c, NULL);
 		if (nvme_sc != NVME_SC_SUCCESS) {
 			nvme_unmap_user_pages(dev,
 				(is_write) ? DMA_TO_DEVICE : DMA_FROM_DEVICE,
@@ -2658,7 +2658,7 @@ static int nvme_trans_start_stop(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 			c.common.opcode = nvme_cmd_flush;
 			c.common.nsid = cpu_to_le32(ns->ns_id);
 
-			nvme_sc = nvme_submit_io_cmd(ns->dev, &c, NULL);
+			nvme_sc = nvme_submit_io_cmd(ns->dev, ns, &c, NULL);
 			res = nvme_trans_status_code(hdr, nvme_sc);
 			if (res)
 				goto out;
@@ -2686,7 +2686,7 @@ static int nvme_trans_synchronize_cache(struct nvme_ns *ns,
 	c.common.opcode = nvme_cmd_flush;
 	c.common.nsid = cpu_to_le32(ns->ns_id);
 
-	nvme_sc = nvme_submit_io_cmd(ns->dev, &c, NULL);
+	nvme_sc = nvme_submit_io_cmd(ns->dev, ns, &c, NULL);
 
 	res = nvme_trans_status_code(hdr, nvme_sc);
 	if (res)
@@ -2894,7 +2894,7 @@ static int nvme_trans_unmap(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	c.dsm.nr = cpu_to_le32(ndesc - 1);
 	c.dsm.attributes = cpu_to_le32(NVME_DSMGMT_AD);
 
-	nvme_sc = nvme_submit_io_cmd(dev, &c, NULL);
+	nvme_sc = nvme_submit_io_cmd(dev, ns, &c, NULL);
 	res = nvme_trans_status_code(hdr, nvme_sc);
 
 	dma_free_coherent(&dev->pci_dev->dev, ndesc * sizeof(*range),
@@ -2915,6 +2915,14 @@ static int nvme_scsi_translate(struct nvme_ns *ns, struct sg_io_hdr *hdr)
 	if (copy_from_user(cmd, hdr->cmdp, hdr->cmd_len))
 		return -EFAULT;
 
+	/*
+	 * Prime the hdr with good status for scsi commands that don't require
+	 * an nvme command for translation.
+	 */
+	retcode = nvme_trans_status_code(hdr, NVME_SC_SUCCESS);
+	if (retcode)
+		return retcode;
+
 	opcode = cmd[0];
 
 	switch (opcode) {
@@ -3016,152 +3024,6 @@ int nvme_sg_io(struct nvme_ns *ns, struct sg_io_hdr __user *u_hdr)
 	return retcode;
 }
 
-#ifdef CONFIG_COMPAT
-typedef struct sg_io_hdr32 {
-	compat_int_t interface_id;	/* [i] 'S' for SCSI generic (required) */
-	compat_int_t dxfer_direction;	/* [i] data transfer direction  */
-	unsigned char cmd_len;		/* [i] SCSI command length ( <= 16 bytes) */
-	unsigned char mx_sb_len;		/* [i] max length to write to sbp */
-	unsigned short iovec_count;	/* [i] 0 implies no scatter gather */
-	compat_uint_t dxfer_len;		/* [i] byte count of data transfer */
-	compat_uint_t dxferp;		/* [i], [*io] points to data transfer memory
-					      or scatter gather list */
-	compat_uptr_t cmdp;		/* [i], [*i] points to command to perform */
-	compat_uptr_t sbp;		/* [i], [*o] points to sense_buffer memory */
-	compat_uint_t timeout;		/* [i] MAX_UINT->no timeout (unit: millisec) */
-	compat_uint_t flags;		/* [i] 0 -> default, see SG_FLAG... */
-	compat_int_t pack_id;		/* [i->o] unused internally (normally) */
-	compat_uptr_t usr_ptr;		/* [i->o] unused internally */
-	unsigned char status;		/* [o] scsi status */
-	unsigned char masked_status;	/* [o] shifted, masked scsi status */
-	unsigned char msg_status;		/* [o] messaging level data (optional) */
-	unsigned char sb_len_wr;		/* [o] byte count actually written to sbp */
-	unsigned short host_status;	/* [o] errors from host adapter */
-	unsigned short driver_status;	/* [o] errors from software driver */
-	compat_int_t resid;		/* [o] dxfer_len - actual_transferred */
-	compat_uint_t duration;		/* [o] time taken by cmd (unit: millisec) */
-	compat_uint_t info;		/* [o] auxiliary information */
-} sg_io_hdr32_t;  /* 64 bytes long (on sparc32) */
-
-typedef struct sg_iovec32 {
-	compat_uint_t iov_base;
-	compat_uint_t iov_len;
-} sg_iovec32_t;
-
-static int sg_build_iovec(sg_io_hdr_t __user *sgio, void __user *dxferp, u16 iovec_count)
-{
-	sg_iovec_t __user *iov = (sg_iovec_t __user *) (sgio + 1);
-	sg_iovec32_t __user *iov32 = dxferp;
-	int i;
-
-	for (i = 0; i < iovec_count; i++) {
-		u32 base, len;
-
-		if (get_user(base, &iov32[i].iov_base) ||
-		    get_user(len, &iov32[i].iov_len) ||
-		    put_user(compat_ptr(base), &iov[i].iov_base) ||
-		    put_user(len, &iov[i].iov_len))
-			return -EFAULT;
-	}
-
-	if (put_user(iov, &sgio->dxferp))
-		return -EFAULT;
-	return 0;
-}
-
-int nvme_sg_io32(struct nvme_ns *ns, unsigned long arg)
-{
-	sg_io_hdr32_t __user *sgio32 = (sg_io_hdr32_t __user *)arg;
-	sg_io_hdr_t __user *sgio;
-	u16 iovec_count;
-	u32 data;
-	void __user *dxferp;
-	int err;
-	int interface_id;
-
-	if (get_user(interface_id, &sgio32->interface_id))
-		return -EFAULT;
-	if (interface_id != 'S')
-		return -EINVAL;
-
-	if (get_user(iovec_count, &sgio32->iovec_count))
-		return -EFAULT;
-
-	{
-		void __user *top = compat_alloc_user_space(0);
-		void __user *new = compat_alloc_user_space(sizeof(sg_io_hdr_t) +
-				       (iovec_count * sizeof(sg_iovec_t)));
-		if (new > top)
-			return -EINVAL;
-
-		sgio = new;
-	}
-
-	/* Ok, now construct.  */
-	if (copy_in_user(&sgio->interface_id, &sgio32->interface_id,
-			 (2 * sizeof(int)) +
-			 (2 * sizeof(unsigned char)) +
-			 (1 * sizeof(unsigned short)) +
-			 (1 * sizeof(unsigned int))))
-		return -EFAULT;
-
-	if (get_user(data, &sgio32->dxferp))
-		return -EFAULT;
-	dxferp = compat_ptr(data);
-	if (iovec_count) {
-		if (sg_build_iovec(sgio, dxferp, iovec_count))
-			return -EFAULT;
-	} else {
-		if (put_user(dxferp, &sgio->dxferp))
-			return -EFAULT;
-	}
-
-	{
-		unsigned char __user *cmdp;
-		unsigned char __user *sbp;
-
-		if (get_user(data, &sgio32->cmdp))
-			return -EFAULT;
-		cmdp = compat_ptr(data);
-
-		if (get_user(data, &sgio32->sbp))
-			return -EFAULT;
-		sbp = compat_ptr(data);
-
-		if (put_user(cmdp, &sgio->cmdp) ||
-		    put_user(sbp, &sgio->sbp))
-			return -EFAULT;
-	}
-
-	if (copy_in_user(&sgio->timeout, &sgio32->timeout,
-			 3 * sizeof(int)))
-		return -EFAULT;
-
-	if (get_user(data, &sgio32->usr_ptr))
-		return -EFAULT;
-	if (put_user(compat_ptr(data), &sgio->usr_ptr))
-		return -EFAULT;
-
-	err = nvme_sg_io(ns, sgio);
-	if (err >= 0) {
-		void __user *datap;
-
-		if (copy_in_user(&sgio32->pack_id, &sgio->pack_id,
-				 sizeof(int)) ||
-		    get_user(datap, &sgio->usr_ptr) ||
-		    put_user((u32)(unsigned long)datap,
-			     &sgio32->usr_ptr) ||
-		    copy_in_user(&sgio32->status, &sgio->status,
-				 (4 * sizeof(unsigned char)) +
-				 (2 * sizeof(unsigned short)) +
-				 (3 * sizeof(int))))
-			err = -EFAULT;
-	}
-
-	return err;
-}
-#endif
-
 int nvme_sg_get_version_num(int __user *ip)
 {
 	return put_user(sg_version_num, ip);
diff --git a/drivers/block/rbd.c b/drivers/block/rbd.c
index 27b71a0b72d0..3ec85dfce124 100644
--- a/drivers/block/rbd.c
+++ b/drivers/block/rbd.c
@@ -2370,8 +2370,12 @@ static void rbd_img_obj_request_fill(struct rbd_obj_request *obj_request,
 		opcode = CEPH_OSD_OP_READ;
 	}
 
-	osd_req_op_extent_init(osd_request, num_ops, opcode, offset, length,
-				0, 0);
+	if (opcode == CEPH_OSD_OP_DELETE)
+		osd_req_op_init(osd_request, num_ops, opcode);
+	else
+		osd_req_op_extent_init(osd_request, num_ops, opcode,
+				       offset, length, 0, 0);
+
 	if (obj_request->type == OBJ_REQUEST_BIO)
 		osd_req_op_extent_osd_data_bio(osd_request, num_ops,
 					obj_request->bio_list, length);
@@ -3405,8 +3409,7 @@ err_rq:
 	if (result)
 		rbd_warn(rbd_dev, "%s %llx at %llx result %d",
 			 obj_op_name(op_type), length, offset, result);
-	if (snapc)
-		ceph_put_snap_context(snapc);
+	ceph_put_snap_context(snapc);
 	blk_end_request_all(rq, result);
 }
 
diff --git a/drivers/block/rsxx/dev.c b/drivers/block/rsxx/dev.c
index 40ee7705df63..ac8c62cb4875 100644
--- a/drivers/block/rsxx/dev.c
+++ b/drivers/block/rsxx/dev.c
@@ -112,37 +112,16 @@ static const struct block_device_operations rsxx_fops = {
 
 static void disk_stats_start(struct rsxx_cardinfo *card, struct bio *bio)
 {
-	struct hd_struct *part0 = &card->gendisk->part0;
-	int rw = bio_data_dir(bio);
-	int cpu;
-
-	cpu = part_stat_lock();
-
-	part_round_stats(cpu, part0);
-	part_inc_in_flight(part0, rw);
-
-	part_stat_unlock();
+	generic_start_io_acct(bio_data_dir(bio), bio_sectors(bio),
+			     &card->gendisk->part0);
 }
 
 static void disk_stats_complete(struct rsxx_cardinfo *card,
 				struct bio *bio,
 				unsigned long start_time)
 {
-	struct hd_struct *part0 = &card->gendisk->part0;
-	unsigned long duration = jiffies - start_time;
-	int rw = bio_data_dir(bio);
-	int cpu;
-
-	cpu = part_stat_lock();
-
-	part_stat_add(cpu, part0, sectors[rw], bio_sectors(bio));
-	part_stat_inc(cpu, part0, ios[rw]);
-	part_stat_add(cpu, part0, ticks[rw], duration);
-
-	part_round_stats(cpu, part0);
-	part_dec_in_flight(part0, rw);
-
-	part_stat_unlock();
+	generic_end_io_acct(bio_data_dir(bio), &card->gendisk->part0,
+			   start_time);
 }
 
 static void bio_dma_done_cb(struct rsxx_cardinfo *card,
diff --git a/drivers/block/sunvdc.c b/drivers/block/sunvdc.c
index 0ebadf93b6c5..4b911ed96ea3 100644
--- a/drivers/block/sunvdc.c
+++ b/drivers/block/sunvdc.c
@@ -23,8 +23,8 @@
 
 #define DRV_MODULE_NAME		"sunvdc"
 #define PFX DRV_MODULE_NAME	": "
-#define DRV_MODULE_VERSION	"1.1"
-#define DRV_MODULE_RELDATE	"February 13, 2013"
+#define DRV_MODULE_VERSION	"1.2"
+#define DRV_MODULE_RELDATE	"November 24, 2014"
 
 static char version[] =
 	DRV_MODULE_NAME ".c:v" DRV_MODULE_VERSION " (" DRV_MODULE_RELDATE ")\n";
@@ -40,6 +40,8 @@ MODULE_VERSION(DRV_MODULE_VERSION);
 #define WAITING_FOR_GEN_CMD	0x04
 #define WAITING_FOR_ANY		-1
 
+static struct workqueue_struct *sunvdc_wq;
+
 struct vdc_req_entry {
 	struct request		*req;
 };
@@ -60,6 +62,10 @@ struct vdc_port {
 	u64			max_xfer_size;
 	u32			vdisk_block_size;
 
+	u64			ldc_timeout;
+	struct timer_list	ldc_reset_timer;
+	struct work_struct	ldc_reset_work;
+
 	/* The server fills these in for us in the disk attribute
 	 * ACK packet.
 	 */
@@ -71,6 +77,10 @@ struct vdc_port {
 	char			disk_name[32];
 };
 
+static void vdc_ldc_reset(struct vdc_port *port);
+static void vdc_ldc_reset_work(struct work_struct *work);
+static void vdc_ldc_reset_timer(unsigned long _arg);
+
 static inline struct vdc_port *to_vdc_port(struct vio_driver_state *vio)
 {
 	return container_of(vio, struct vdc_port, vio);
@@ -150,6 +160,21 @@ static const struct block_device_operations vdc_fops = {
 	.ioctl		= vdc_ioctl,
 };
 
+static void vdc_blk_queue_start(struct vdc_port *port)
+{
+	struct vio_dring_state *dr = &port->vio.drings[VIO_DRIVER_TX_RING];
+
+	/* restart blk queue when ring is half emptied. also called after
+	 * handshake completes, so check for initial handshake before we've
+	 * allocated a disk.
+	 */
+	if (port->disk && blk_queue_stopped(port->disk->queue) &&
+	    vdc_tx_dring_avail(dr) * 100 / VDC_TX_RING_SIZE >= 50) {
+		blk_start_queue(port->disk->queue);
+	}
+
+}
+
 static void vdc_finish(struct vio_driver_state *vio, int err, int waiting_for)
 {
 	if (vio->cmp &&
@@ -163,7 +188,11 @@ static void vdc_finish(struct vio_driver_state *vio, int err, int waiting_for)
 
 static void vdc_handshake_complete(struct vio_driver_state *vio)
 {
+	struct vdc_port *port = to_vdc_port(vio);
+
+	del_timer(&port->ldc_reset_timer);
 	vdc_finish(vio, 0, WAITING_FOR_LINK_UP);
+	vdc_blk_queue_start(port);
 }
 
 static int vdc_handle_unknown(struct vdc_port *port, void *arg)
@@ -269,7 +298,7 @@ static void vdc_end_one(struct vdc_port *port, struct vio_dring_state *dr,
 
 	ldc_unmap(port->vio.lp, desc->cookies, desc->ncookies);
 	desc->hdr.state = VIO_DESC_FREE;
-	dr->cons = (index + 1) & (VDC_TX_RING_SIZE - 1);
+	dr->cons = vio_dring_next(dr, index);
 
 	req = rqe->req;
 	if (req == NULL) {
@@ -281,10 +310,7 @@ static void vdc_end_one(struct vdc_port *port, struct vio_dring_state *dr,
 
 	__blk_end_request(req, (desc->status ? -EIO : 0), desc->size);
 
-	/* restart blk queue when ring is half emptied */
-	if (blk_queue_stopped(port->disk->queue) &&
-	    vdc_tx_dring_avail(dr) * 100 / VDC_TX_RING_SIZE >= 50)
-		blk_start_queue(port->disk->queue);
+	vdc_blk_queue_start(port);
 }
 
 static int vdc_ack(struct vdc_port *port, void *msgbuf)
@@ -317,17 +343,20 @@ static void vdc_event(void *arg, int event)
 
 	spin_lock_irqsave(&vio->lock, flags);
 
-	if (unlikely(event == LDC_EVENT_RESET ||
-		     event == LDC_EVENT_UP)) {
+	if (unlikely(event == LDC_EVENT_RESET)) {
 		vio_link_state_change(vio, event);
-		spin_unlock_irqrestore(&vio->lock, flags);
-		return;
+		queue_work(sunvdc_wq, &port->ldc_reset_work);
+		goto out;
+	}
+
+	if (unlikely(event == LDC_EVENT_UP)) {
+		vio_link_state_change(vio, event);
+		goto out;
 	}
 
 	if (unlikely(event != LDC_EVENT_DATA_READY)) {
-		printk(KERN_WARNING PFX "Unexpected LDC event %d\n", event);
-		spin_unlock_irqrestore(&vio->lock, flags);
-		return;
+		pr_warn(PFX "Unexpected LDC event %d\n", event);
+		goto out;
 	}
 
 	err = 0;
@@ -371,6 +400,7 @@ static void vdc_event(void *arg, int event)
 	}
 	if (err < 0)
 		vdc_finish(&port->vio, err, WAITING_FOR_ANY);
+out:
 	spin_unlock_irqrestore(&vio->lock, flags);
 }
 
@@ -403,6 +433,8 @@ static int __vdc_tx_trigger(struct vdc_port *port)
 			delay = 128;
 	} while (err == -EAGAIN);
 
+	if (err == -ENOTCONN)
+		vdc_ldc_reset(port);
 	return err;
 }
 
@@ -472,7 +504,7 @@ static int __send_request(struct request *req)
 		printk(KERN_ERR PFX "vdc_tx_trigger() failure, err=%d\n", err);
 	} else {
 		port->req_id++;
-		dr->prod = (dr->prod + 1) & (VDC_TX_RING_SIZE - 1);
+		dr->prod = vio_dring_next(dr, dr->prod);
 	}
 
 	return err;
@@ -626,7 +658,7 @@ static int generic_request(struct vdc_port *port, u8 op, void *buf, int len)
 	err = __vdc_tx_trigger(port);
 	if (err >= 0) {
 		port->req_id++;
-		dr->prod = (dr->prod + 1) & (VDC_TX_RING_SIZE - 1);
+		dr->prod = vio_dring_next(dr, dr->prod);
 		spin_unlock_irqrestore(&port->vio.lock, flags);
 
 		wait_for_completion(&comp.com);
@@ -690,12 +722,9 @@ static void vdc_free_tx_ring(struct vdc_port *port)
 	}
 }
 
-static int probe_disk(struct vdc_port *port)
+static int vdc_port_up(struct vdc_port *port)
 {
 	struct vio_completion comp;
-	struct request_queue *q;
-	struct gendisk *g;
-	int err;
 
 	init_completion(&comp.com);
 	comp.err = 0;
@@ -703,10 +732,27 @@ static int probe_disk(struct vdc_port *port)
 	port->vio.cmp = &comp;
 
 	vio_port_up(&port->vio);
-
 	wait_for_completion(&comp.com);
-	if (comp.err)
-		return comp.err;
+	return comp.err;
+}
+
+static void vdc_port_down(struct vdc_port *port)
+{
+	ldc_disconnect(port->vio.lp);
+	ldc_unbind(port->vio.lp);
+	vdc_free_tx_ring(port);
+	vio_ldc_free(&port->vio);
+}
+
+static int probe_disk(struct vdc_port *port)
+{
+	struct request_queue *q;
+	struct gendisk *g;
+	int err;
+
+	err = vdc_port_up(port);
+	if (err)
+		return err;
 
 	if (vdc_version_supported(port, 1, 1)) {
 		/* vdisk_size should be set during the handshake, if it wasn't
@@ -819,6 +865,7 @@ static int vdc_port_probe(struct vio_dev *vdev, const struct vio_device_id *id)
 	struct mdesc_handle *hp;
 	struct vdc_port *port;
 	int err;
+	const u64 *ldc_timeout;
 
 	print_version();
 
@@ -848,6 +895,16 @@ static int vdc_port_probe(struct vio_dev *vdev, const struct vio_device_id *id)
 			 VDCBLK_NAME "%c", 'a' + ((int)vdev->dev_no % 26));
 	port->vdisk_size = -1;
 
+	/* Actual wall time may be double due to do_generic_file_read() doing
+	 * a readahead I/O first, and once that fails it will try to read a
+	 * single page.
+	 */
+	ldc_timeout = mdesc_get_property(hp, vdev->mp, "vdc-timeout", NULL);
+	port->ldc_timeout = ldc_timeout ? *ldc_timeout : 0;
+	setup_timer(&port->ldc_reset_timer, vdc_ldc_reset_timer,
+		    (unsigned long)port);
+	INIT_WORK(&port->ldc_reset_work, vdc_ldc_reset_work);
+
 	err = vio_driver_init(&port->vio, vdev, VDEV_DISK,
 			      vdc_versions, ARRAY_SIZE(vdc_versions),
 			      &vdc_vio_ops, port->disk_name);
@@ -896,8 +953,21 @@ static int vdc_port_remove(struct vio_dev *vdev)
 	struct vdc_port *port = dev_get_drvdata(&vdev->dev);
 
 	if (port) {
+		unsigned long flags;
+
+		spin_lock_irqsave(&port->vio.lock, flags);
+		blk_stop_queue(port->disk->queue);
+		spin_unlock_irqrestore(&port->vio.lock, flags);
+
+		flush_work(&port->ldc_reset_work);
+		del_timer_sync(&port->ldc_reset_timer);
 		del_timer_sync(&port->vio.timer);
 
+		del_gendisk(port->disk);
+		blk_cleanup_queue(port->disk->queue);
+		put_disk(port->disk);
+		port->disk = NULL;
+
 		vdc_free_tx_ring(port);
 		vio_ldc_free(&port->vio);
 
@@ -908,6 +978,102 @@ static int vdc_port_remove(struct vio_dev *vdev)
 	return 0;
 }
 
+static void vdc_requeue_inflight(struct vdc_port *port)
+{
+	struct vio_dring_state *dr = &port->vio.drings[VIO_DRIVER_TX_RING];
+	u32 idx;
+
+	for (idx = dr->cons; idx != dr->prod; idx = vio_dring_next(dr, idx)) {
+		struct vio_disk_desc *desc = vio_dring_entry(dr, idx);
+		struct vdc_req_entry *rqe = &port->rq_arr[idx];
+		struct request *req;
+
+		ldc_unmap(port->vio.lp, desc->cookies, desc->ncookies);
+		desc->hdr.state = VIO_DESC_FREE;
+		dr->cons = vio_dring_next(dr, idx);
+
+		req = rqe->req;
+		if (req == NULL) {
+			vdc_end_special(port, desc);
+			continue;
+		}
+
+		rqe->req = NULL;
+		blk_requeue_request(port->disk->queue, req);
+	}
+}
+
+static void vdc_queue_drain(struct vdc_port *port)
+{
+	struct request *req;
+
+	while ((req = blk_fetch_request(port->disk->queue)) != NULL)
+		__blk_end_request_all(req, -EIO);
+}
+
+static void vdc_ldc_reset_timer(unsigned long _arg)
+{
+	struct vdc_port *port = (struct vdc_port *) _arg;
+	struct vio_driver_state *vio = &port->vio;
+	unsigned long flags;
+
+	spin_lock_irqsave(&vio->lock, flags);
+	if (!(port->vio.hs_state & VIO_HS_COMPLETE)) {
+		pr_warn(PFX "%s ldc down %llu seconds, draining queue\n",
+			port->disk_name, port->ldc_timeout);
+		vdc_queue_drain(port);
+		vdc_blk_queue_start(port);
+	}
+	spin_unlock_irqrestore(&vio->lock, flags);
+}
+
+static void vdc_ldc_reset_work(struct work_struct *work)
+{
+	struct vdc_port *port;
+	struct vio_driver_state *vio;
+	unsigned long flags;
+
+	port = container_of(work, struct vdc_port, ldc_reset_work);
+	vio = &port->vio;
+
+	spin_lock_irqsave(&vio->lock, flags);
+	vdc_ldc_reset(port);
+	spin_unlock_irqrestore(&vio->lock, flags);
+}
+
+static void vdc_ldc_reset(struct vdc_port *port)
+{
+	int err;
+
+	assert_spin_locked(&port->vio.lock);
+
+	pr_warn(PFX "%s ldc link reset\n", port->disk_name);
+	blk_stop_queue(port->disk->queue);
+	vdc_requeue_inflight(port);
+	vdc_port_down(port);
+
+	err = vio_ldc_alloc(&port->vio, &vdc_ldc_cfg, port);
+	if (err) {
+		pr_err(PFX "%s vio_ldc_alloc:%d\n", port->disk_name, err);
+		return;
+	}
+
+	err = vdc_alloc_tx_ring(port);
+	if (err) {
+		pr_err(PFX "%s vio_alloc_tx_ring:%d\n", port->disk_name, err);
+		goto err_free_ldc;
+	}
+
+	if (port->ldc_timeout)
+		mod_timer(&port->ldc_reset_timer,
+			  round_jiffies(jiffies + HZ * port->ldc_timeout));
+	mod_timer(&port->vio.timer, round_jiffies(jiffies + HZ));
+	return;
+
+err_free_ldc:
+	vio_ldc_free(&port->vio);
+}
+
 static const struct vio_device_id vdc_port_match[] = {
 	{
 		.type = "vdc-port",
@@ -927,9 +1093,13 @@ static int __init vdc_init(void)
 {
 	int err;
 
+	sunvdc_wq = alloc_workqueue("sunvdc", 0, 0);
+	if (!sunvdc_wq)
+		return -ENOMEM;
+
 	err = register_blkdev(0, VDCBLK_NAME);
 	if (err < 0)
-		goto out_err;
+		goto out_free_wq;
 
 	vdc_major = err;
 
@@ -943,7 +1113,8 @@ out_unregister_blkdev:
 	unregister_blkdev(vdc_major, VDCBLK_NAME);
 	vdc_major = 0;
 
-out_err:
+out_free_wq:
+	destroy_workqueue(sunvdc_wq);
 	return err;
 }
 
@@ -951,6 +1122,7 @@ static void __exit vdc_exit(void)
 {
 	vio_unregister_driver(&vdc_port_driver);
 	unregister_blkdev(vdc_major, VDCBLK_NAME);
+	destroy_workqueue(sunvdc_wq);
 }
 
 module_init(vdc_init);
diff --git a/drivers/block/swim.c b/drivers/block/swim.c
index 6b44bbe528b7..b5afd495d482 100644
--- a/drivers/block/swim.c
+++ b/drivers/block/swim.c
@@ -971,7 +971,6 @@ static struct platform_driver swim_driver = {
 	.remove = swim_remove,
 	.driver   = {
 		.name	= CARDNAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/block/virtio_blk.c b/drivers/block/virtio_blk.c
index 1fb9e09fbbc5..7ef7c098708f 100644
--- a/drivers/block/virtio_blk.c
+++ b/drivers/block/virtio_blk.c
@@ -159,10 +159,11 @@ static void virtblk_done(struct virtqueue *vq)
 	spin_unlock_irqrestore(&vblk->vqs[qid].lock, flags);
 }
 
-static int virtio_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *req,
-		bool last)
+static int virtio_queue_rq(struct blk_mq_hw_ctx *hctx,
+			   const struct blk_mq_queue_data *bd)
 {
 	struct virtio_blk *vblk = hctx->queue->queuedata;
+	struct request *req = bd->rq;
 	struct virtblk_req *vbr = blk_mq_rq_to_pdu(req);
 	unsigned long flags;
 	unsigned int num;
@@ -223,7 +224,7 @@ static int virtio_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *req,
 		return BLK_MQ_RQ_QUEUE_ERROR;
 	}
 
-	if (last && virtqueue_kick_prepare(vblk->vqs[qid].vq))
+	if (bd->last && virtqueue_kick_prepare(vblk->vqs[qid].vq))
 		notify = true;
 	spin_unlock_irqrestore(&vblk->vqs[qid].lock, flags);
 
diff --git a/drivers/block/xen-blkfront.c b/drivers/block/xen-blkfront.c
index 5ac312f6e0be..2236c6f31608 100644
--- a/drivers/block/xen-blkfront.c
+++ b/drivers/block/xen-blkfront.c
@@ -126,7 +126,6 @@ struct blkfront_info
 	unsigned int persistent_gnts_c;
 	unsigned long shadow_free;
 	unsigned int feature_flush;
-	unsigned int flush_op;
 	unsigned int feature_discard:1;
 	unsigned int feature_secdiscard:1;
 	unsigned int discard_granularity;
@@ -479,7 +478,19 @@ static int blkif_queue_request(struct request *req)
 				 * way.  (It's also a FLUSH+FUA, since it is
 				 * guaranteed ordered WRT previous writes.)
 				 */
-				ring_req->operation = info->flush_op;
+				switch (info->feature_flush &
+					((REQ_FLUSH|REQ_FUA))) {
+				case REQ_FLUSH|REQ_FUA:
+					ring_req->operation =
+						BLKIF_OP_WRITE_BARRIER;
+					break;
+				case REQ_FLUSH:
+					ring_req->operation =
+						BLKIF_OP_FLUSH_DISKCACHE;
+					break;
+				default:
+					ring_req->operation = 0;
+				}
 			}
 			ring_req->u.rw.nr_segments = nseg;
 		}
@@ -582,12 +593,14 @@ static inline void flush_requests(struct blkfront_info *info)
 		notify_remote_via_irq(info->irq);
 }
 
-static inline bool blkif_request_flush_valid(struct request *req,
-					     struct blkfront_info *info)
+static inline bool blkif_request_flush_invalid(struct request *req,
+					       struct blkfront_info *info)
 {
 	return ((req->cmd_type != REQ_TYPE_FS) ||
-		((req->cmd_flags & (REQ_FLUSH | REQ_FUA)) &&
-		!info->flush_op));
+		((req->cmd_flags & REQ_FLUSH) &&
+		 !(info->feature_flush & REQ_FLUSH)) ||
+		((req->cmd_flags & REQ_FUA) &&
+		 !(info->feature_flush & REQ_FUA)));
 }
 
 /*
@@ -612,8 +625,8 @@ static void do_blkif_request(struct request_queue *rq)
 
 		blk_start_request(req);
 
-		if (blkif_request_flush_valid(req, info)) {
-			__blk_end_request_all(req, -EIO);
+		if (blkif_request_flush_invalid(req, info)) {
+			__blk_end_request_all(req, -EOPNOTSUPP);
 			continue;
 		}
 
@@ -683,20 +696,26 @@ static int xlvbd_init_blk_queue(struct gendisk *gd, u16 sector_size,
 	return 0;
 }
 
+static const char *flush_info(unsigned int feature_flush)
+{
+	switch (feature_flush & ((REQ_FLUSH | REQ_FUA))) {
+	case REQ_FLUSH|REQ_FUA:
+		return "barrier: enabled;";
+	case REQ_FLUSH:
+		return "flush diskcache: enabled;";
+	default:
+		return "barrier or flush: disabled;";
+	}
+}
 
 static void xlvbd_flush(struct blkfront_info *info)
 {
 	blk_queue_flush(info->rq, info->feature_flush);
-	printk(KERN_INFO "blkfront: %s: %s: %s %s %s %s %s\n",
-	       info->gd->disk_name,
-	       info->flush_op == BLKIF_OP_WRITE_BARRIER ?
-		"barrier" : (info->flush_op == BLKIF_OP_FLUSH_DISKCACHE ?
-		"flush diskcache" : "barrier or flush"),
-	       info->feature_flush ? "enabled;" : "disabled;",
-	       "persistent grants:",
-	       info->feature_persistent ? "enabled;" : "disabled;",
-	       "indirect descriptors:",
-	       info->max_indirect_segments ? "enabled;" : "disabled;");
+	pr_info("blkfront: %s: %s %s %s %s %s\n",
+		info->gd->disk_name, flush_info(info->feature_flush),
+		"persistent grants:", info->feature_persistent ?
+		"enabled;" : "disabled;", "indirect descriptors:",
+		info->max_indirect_segments ? "enabled;" : "disabled;");
 }
 
 static int xen_translate_vdev(int vdevice, int *minor, unsigned int *offset)
@@ -1188,7 +1207,6 @@ static irqreturn_t blkif_interrupt(int irq, void *dev_id)
 				if (error == -EOPNOTSUPP)
 					error = 0;
 				info->feature_flush = 0;
-				info->flush_op = 0;
 				xlvbd_flush(info);
 			}
 			/* fall through */
@@ -1808,7 +1826,6 @@ static void blkfront_connect(struct blkfront_info *info)
 		physical_sector_size = sector_size;
 
 	info->feature_flush = 0;
-	info->flush_op = 0;
 
 	err = xenbus_gather(XBT_NIL, info->xbdev->otherend,
 			    "feature-barrier", "%d", &barrier,
@@ -1821,10 +1838,8 @@ static void blkfront_connect(struct blkfront_info *info)
 	 *
 	 * If there are barriers, then we use flush.
 	 */
-	if (!err && barrier) {
+	if (!err && barrier)
 		info->feature_flush = REQ_FLUSH | REQ_FUA;
-		info->flush_op = BLKIF_OP_WRITE_BARRIER;
-	}
 	/*
 	 * And if there is "feature-flush-cache" use that above
 	 * barriers.
@@ -1833,10 +1848,8 @@ static void blkfront_connect(struct blkfront_info *info)
 			    "feature-flush-cache", "%d", &flush,
 			    NULL);
 
-	if (!err && flush) {
+	if (!err && flush)
 		info->feature_flush = REQ_FLUSH;
-		info->flush_op = BLKIF_OP_FLUSH_DISKCACHE;
-	}
 
 	err = xenbus_gather(XBT_NIL, info->xbdev->otherend,
 			    "feature-discard", "%d", &discard,
diff --git a/drivers/block/zram/zram_drv.c b/drivers/block/zram/zram_drv.c
index 3920ee45aa59..bd8bda386e02 100644
--- a/drivers/block/zram/zram_drv.c
+++ b/drivers/block/zram/zram_drv.c
@@ -44,15 +44,14 @@ static const char *default_compressor = "lzo";
 static unsigned int num_devices = 1;
 
 #define ZRAM_ATTR_RO(name)						\
-static ssize_t zram_attr_##name##_show(struct device *d,		\
+static ssize_t name##_show(struct device *d,		\
 				struct device_attribute *attr, char *b)	\
 {									\
 	struct zram *zram = dev_to_zram(d);				\
 	return scnprintf(b, PAGE_SIZE, "%llu\n",			\
 		(u64)atomic64_read(&zram->stats.name));			\
 }									\
-static struct device_attribute dev_attr_##name =			\
-	__ATTR(name, S_IRUGO, zram_attr_##name##_show, NULL);
+static DEVICE_ATTR_RO(name);
 
 static inline int init_done(struct zram *zram)
 {
@@ -287,19 +286,18 @@ static inline int is_partial_io(struct bio_vec *bvec)
 /*
  * Check if request is within bounds and aligned on zram logical blocks.
  */
-static inline int valid_io_request(struct zram *zram, struct bio *bio)
+static inline int valid_io_request(struct zram *zram,
+		sector_t start, unsigned int size)
 {
-	u64 start, end, bound;
+	u64 end, bound;
 
 	/* unaligned request */
-	if (unlikely(bio->bi_iter.bi_sector &
-		     (ZRAM_SECTOR_PER_LOGICAL_BLOCK - 1)))
+	if (unlikely(start & (ZRAM_SECTOR_PER_LOGICAL_BLOCK - 1)))
 		return 0;
-	if (unlikely(bio->bi_iter.bi_size & (ZRAM_LOGICAL_BLOCK_SIZE - 1)))
+	if (unlikely(size & (ZRAM_LOGICAL_BLOCK_SIZE - 1)))
 		return 0;
 
-	start = bio->bi_iter.bi_sector;
-	end = start + (bio->bi_iter.bi_size >> SECTOR_SHIFT);
+	end = start + (size >> SECTOR_SHIFT);
 	bound = zram->disksize >> SECTOR_SHIFT;
 	/* out of range range */
 	if (unlikely(start >= bound || end > bound || start > end))
@@ -453,7 +451,7 @@ static int zram_decompress_page(struct zram *zram, char *mem, u32 index)
 }
 
 static int zram_bvec_read(struct zram *zram, struct bio_vec *bvec,
-			  u32 index, int offset, struct bio *bio)
+			  u32 index, int offset)
 {
 	int ret;
 	struct page *page;
@@ -645,14 +643,13 @@ out:
 }
 
 static int zram_bvec_rw(struct zram *zram, struct bio_vec *bvec, u32 index,
-			int offset, struct bio *bio)
+			int offset, int rw)
 {
 	int ret;
-	int rw = bio_data_dir(bio);
 
 	if (rw == READ) {
 		atomic64_inc(&zram->stats.num_reads);
-		ret = zram_bvec_read(zram, bvec, index, offset, bio);
+		ret = zram_bvec_read(zram, bvec, index, offset);
 	} else {
 		atomic64_inc(&zram->stats.num_writes);
 		ret = zram_bvec_write(zram, bvec, index, offset);
@@ -853,7 +850,7 @@ out:
 
 static void __zram_make_request(struct zram *zram, struct bio *bio)
 {
-	int offset;
+	int offset, rw;
 	u32 index;
 	struct bio_vec bvec;
 	struct bvec_iter iter;
@@ -868,6 +865,7 @@ static void __zram_make_request(struct zram *zram, struct bio *bio)
 		return;
 	}
 
+	rw = bio_data_dir(bio);
 	bio_for_each_segment(bvec, bio, iter) {
 		int max_transfer_size = PAGE_SIZE - offset;
 
@@ -882,15 +880,15 @@ static void __zram_make_request(struct zram *zram, struct bio *bio)
 			bv.bv_len = max_transfer_size;
 			bv.bv_offset = bvec.bv_offset;
 
-			if (zram_bvec_rw(zram, &bv, index, offset, bio) < 0)
+			if (zram_bvec_rw(zram, &bv, index, offset, rw) < 0)
 				goto out;
 
 			bv.bv_len = bvec.bv_len - max_transfer_size;
 			bv.bv_offset += max_transfer_size;
-			if (zram_bvec_rw(zram, &bv, index + 1, 0, bio) < 0)
+			if (zram_bvec_rw(zram, &bv, index + 1, 0, rw) < 0)
 				goto out;
 		} else
-			if (zram_bvec_rw(zram, &bvec, index, offset, bio) < 0)
+			if (zram_bvec_rw(zram, &bvec, index, offset, rw) < 0)
 				goto out;
 
 		update_position(&index, &offset, &bvec);
@@ -915,7 +913,8 @@ static void zram_make_request(struct request_queue *queue, struct bio *bio)
 	if (unlikely(!init_done(zram)))
 		goto error;
 
-	if (!valid_io_request(zram, bio)) {
+	if (!valid_io_request(zram, bio->bi_iter.bi_sector,
+					bio->bi_iter.bi_size)) {
 		atomic64_inc(&zram->stats.invalid_io);
 		goto error;
 	}
@@ -945,25 +944,64 @@ static void zram_slot_free_notify(struct block_device *bdev,
 	atomic64_inc(&zram->stats.notify_free);
 }
 
+static int zram_rw_page(struct block_device *bdev, sector_t sector,
+		       struct page *page, int rw)
+{
+	int offset, err;
+	u32 index;
+	struct zram *zram;
+	struct bio_vec bv;
+
+	zram = bdev->bd_disk->private_data;
+	if (!valid_io_request(zram, sector, PAGE_SIZE)) {
+		atomic64_inc(&zram->stats.invalid_io);
+		return -EINVAL;
+	}
+
+	down_read(&zram->init_lock);
+	if (unlikely(!init_done(zram))) {
+		err = -EIO;
+		goto out_unlock;
+	}
+
+	index = sector >> SECTORS_PER_PAGE_SHIFT;
+	offset = sector & (SECTORS_PER_PAGE - 1) << SECTOR_SHIFT;
+
+	bv.bv_page = page;
+	bv.bv_len = PAGE_SIZE;
+	bv.bv_offset = 0;
+
+	err = zram_bvec_rw(zram, &bv, index, offset, rw);
+out_unlock:
+	up_read(&zram->init_lock);
+	/*
+	 * If I/O fails, just return error(ie, non-zero) without
+	 * calling page_endio.
+	 * It causes resubmit the I/O with bio request by upper functions
+	 * of rw_page(e.g., swap_readpage, __swap_writepage) and
+	 * bio->bi_end_io does things to handle the error
+	 * (e.g., SetPageError, set_page_dirty and extra works).
+	 */
+	if (err == 0)
+		page_endio(page, rw, 0);
+	return err;
+}
+
 static const struct block_device_operations zram_devops = {
 	.swap_slot_free_notify = zram_slot_free_notify,
+	.rw_page = zram_rw_page,
 	.owner = THIS_MODULE
 };
 
-static DEVICE_ATTR(disksize, S_IRUGO | S_IWUSR,
-		disksize_show, disksize_store);
-static DEVICE_ATTR(initstate, S_IRUGO, initstate_show, NULL);
-static DEVICE_ATTR(reset, S_IWUSR, NULL, reset_store);
-static DEVICE_ATTR(orig_data_size, S_IRUGO, orig_data_size_show, NULL);
-static DEVICE_ATTR(mem_used_total, S_IRUGO, mem_used_total_show, NULL);
-static DEVICE_ATTR(mem_limit, S_IRUGO | S_IWUSR, mem_limit_show,
-		mem_limit_store);
-static DEVICE_ATTR(mem_used_max, S_IRUGO | S_IWUSR, mem_used_max_show,
-		mem_used_max_store);
-static DEVICE_ATTR(max_comp_streams, S_IRUGO | S_IWUSR,
-		max_comp_streams_show, max_comp_streams_store);
-static DEVICE_ATTR(comp_algorithm, S_IRUGO | S_IWUSR,
-		comp_algorithm_show, comp_algorithm_store);
+static DEVICE_ATTR_RW(disksize);
+static DEVICE_ATTR_RO(initstate);
+static DEVICE_ATTR_WO(reset);
+static DEVICE_ATTR_RO(orig_data_size);
+static DEVICE_ATTR_RO(mem_used_total);
+static DEVICE_ATTR_RW(mem_limit);
+static DEVICE_ATTR_RW(mem_used_max);
+static DEVICE_ATTR_RW(max_comp_streams);
+static DEVICE_ATTR_RW(comp_algorithm);
 
 ZRAM_ATTR_RO(num_reads);
 ZRAM_ATTR_RO(num_writes);
diff --git a/drivers/block/zram/zram_drv.h b/drivers/block/zram/zram_drv.h
index c6ee271317f5..b05a816b09ac 100644
--- a/drivers/block/zram/zram_drv.h
+++ b/drivers/block/zram/zram_drv.h
@@ -66,8 +66,8 @@ static const size_t max_zpage_size = PAGE_SIZE / 4 * 3;
 /* Flags for zram pages (table[page_no].value) */
 enum zram_pageflags {
 	/* Page consists entirely of zeros */
-	ZRAM_ZERO = ZRAM_FLAG_SHIFT + 1,
-	ZRAM_ACCESS,	/* page in now accessed */
+	ZRAM_ZERO = ZRAM_FLAG_SHIFT,
+	ZRAM_ACCESS,	/* page is now accessed */
 
 	__NR_ZRAM_PAGEFLAGS,
 };
diff --git a/drivers/bluetooth/ath3k.c b/drivers/bluetooth/ath3k.c
index fce758896280..1ee27ac18de0 100644
--- a/drivers/bluetooth/ath3k.c
+++ b/drivers/bluetooth/ath3k.c
@@ -87,6 +87,7 @@ static const struct usb_device_id ath3k_table[] = {
 	{ USB_DEVICE(0x04CA, 0x3007) },
 	{ USB_DEVICE(0x04CA, 0x3008) },
 	{ USB_DEVICE(0x04CA, 0x300b) },
+	{ USB_DEVICE(0x04CA, 0x3010) },
 	{ USB_DEVICE(0x0930, 0x0219) },
 	{ USB_DEVICE(0x0930, 0x0220) },
 	{ USB_DEVICE(0x0930, 0x0227) },
@@ -140,6 +141,7 @@ static const struct usb_device_id ath3k_blist_tbl[] = {
 	{ USB_DEVICE(0x04ca, 0x3007), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3008), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x300b), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x04ca, 0x3010), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0930, 0x0219), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0930, 0x0220), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0930, 0x0227), .driver_info = BTUSB_ATH3012 },
diff --git a/drivers/bluetooth/bfusb.c b/drivers/bluetooth/bfusb.c
index b2e7e94a6771..fcfb72e9e0ee 100644
--- a/drivers/bluetooth/bfusb.c
+++ b/drivers/bluetooth/bfusb.c
@@ -696,6 +696,8 @@ static int bfusb_probe(struct usb_interface *intf, const struct usb_device_id *i
 	hdev->flush = bfusb_flush;
 	hdev->send  = bfusb_send_frame;
 
+	set_bit(HCI_QUIRK_BROKEN_LOCAL_COMMANDS, &hdev->quirks);
+
 	if (hci_register_dev(hdev) < 0) {
 		BT_ERR("Can't register HCI device");
 		hci_free_dev(hdev);
diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c
index 31dd24ac9926..f051a93c6cad 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -49,6 +49,7 @@ static struct usb_driver btusb_driver;
 #define BTUSB_INTEL_BOOT	0x200
 #define BTUSB_BCM_PATCHRAM	0x400
 #define BTUSB_MARVELL		0x800
+#define BTUSB_AVM		0x1000
 
 static const struct usb_device_id btusb_table[] = {
 	/* Generic Bluetooth USB device */
@@ -85,7 +86,7 @@ static const struct usb_device_id btusb_table[] = {
 	{ USB_DEVICE(0x05ac, 0x8281) },
 
 	/* AVM BlueFRITZ! USB v2.0 */
-	{ USB_DEVICE(0x057c, 0x3800) },
+	{ USB_DEVICE(0x057c, 0x3800), .driver_info = BTUSB_AVM },
 
 	/* Bluetooth Ultraport Module from IBM */
 	{ USB_DEVICE(0x04bf, 0x030a) },
@@ -167,6 +168,7 @@ static const struct usb_device_id blacklist_table[] = {
 	{ USB_DEVICE(0x04ca, 0x3007), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3008), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x300b), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x04ca, 0x3010), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0930, 0x0219), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0930, 0x0220), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0930, 0x0227), .driver_info = BTUSB_ATH3012 },
@@ -1942,6 +1944,31 @@ static int btusb_set_bdaddr_bcm(struct hci_dev *hdev, const bdaddr_t *bdaddr)
 	return 0;
 }
 
+static int btusb_set_bdaddr_ath3012(struct hci_dev *hdev,
+				    const bdaddr_t *bdaddr)
+{
+	struct sk_buff *skb;
+	u8 buf[10];
+	long ret;
+
+	buf[0] = 0x01;
+	buf[1] = 0x01;
+	buf[2] = 0x00;
+	buf[3] = sizeof(bdaddr_t);
+	memcpy(buf + 4, bdaddr, sizeof(bdaddr_t));
+
+	skb = __hci_cmd_sync(hdev, 0xfc0b, sizeof(buf), buf, HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		ret = PTR_ERR(skb);
+		BT_ERR("%s: Change address command failed (%ld)",
+		       hdev->name, ret);
+		return ret;
+	}
+	kfree_skb(skb);
+
+	return 0;
+}
+
 static int btusb_probe(struct usb_interface *intf,
 		       const struct usb_device_id *id)
 {
@@ -2054,9 +2081,15 @@ static int btusb_probe(struct usb_interface *intf,
 	if (id->driver_info & BTUSB_MARVELL)
 		hdev->set_bdaddr = btusb_set_bdaddr_marvell;
 
+	if (id->driver_info & BTUSB_AVM)
+		set_bit(HCI_QUIRK_BROKEN_LOCAL_COMMANDS, &hdev->quirks);
+
 	if (id->driver_info & BTUSB_INTEL_BOOT)
 		set_bit(HCI_QUIRK_RAW_DEVICE, &hdev->quirks);
 
+	if (id->driver_info & BTUSB_ATH3012)
+		hdev->set_bdaddr = btusb_set_bdaddr_ath3012;
+
 	/* Interface numbers are hardcoded in the specification */
 	data->isoc = usb_ifnum_to_if(data->udev, 1);
 
diff --git a/drivers/bluetooth/btwilink.c b/drivers/bluetooth/btwilink.c
index f038dba19e36..55c135b7757a 100644
--- a/drivers/bluetooth/btwilink.c
+++ b/drivers/bluetooth/btwilink.c
@@ -349,7 +349,6 @@ static struct platform_driver btwilink_driver = {
 	.remove = bt_ti_remove,
 	.driver = {
 		.name = "btwilink",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/bus/brcmstb_gisb.c b/drivers/bus/brcmstb_gisb.c
index 46de8dc39eb4..738612c45266 100644
--- a/drivers/bus/brcmstb_gisb.c
+++ b/drivers/bus/brcmstb_gisb.c
@@ -389,7 +389,6 @@ static const struct dev_pm_ops brcmstb_gisb_arb_pm_ops = {
 static struct platform_driver brcmstb_gisb_arb_driver = {
 	.driver = {
 		.name	= "brcm-gisb-arb",
-		.owner	= THIS_MODULE,
 		.of_match_table = brcmstb_gisb_arb_of_match,
 		.pm	= &brcmstb_gisb_arb_pm_ops,
 	},
diff --git a/drivers/bus/imx-weim.c b/drivers/bus/imx-weim.c
index 75c9681f8021..0958b6981773 100644
--- a/drivers/bus/imx-weim.c
+++ b/drivers/bus/imx-weim.c
@@ -206,7 +206,6 @@ static int __init weim_probe(struct platform_device *pdev)
 static struct platform_driver weim_driver = {
 	.driver = {
 		.name		= "imx-weim",
-		.owner		= THIS_MODULE,
 		.of_match_table	= weim_id_table,
 	},
 };
diff --git a/drivers/bus/omap-ocp2scp.c b/drivers/bus/omap-ocp2scp.c
index 5511f9814ddd..723ec06ad2c8 100644
--- a/drivers/bus/omap-ocp2scp.c
+++ b/drivers/bus/omap-ocp2scp.c
@@ -77,7 +77,6 @@ static struct platform_driver omap_ocp2scp_driver = {
 	.remove		= omap_ocp2scp_remove,
 	.driver		= {
 		.name	= "omap-ocp2scp",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(omap_ocp2scp_id_table),
 	},
 };
diff --git a/drivers/bus/omap_l3_noc.c b/drivers/bus/omap_l3_noc.c
index 17d86595951c..029bc73de001 100644
--- a/drivers/bus/omap_l3_noc.c
+++ b/drivers/bus/omap_l3_noc.c
@@ -358,7 +358,6 @@ static struct platform_driver omap_l3_driver = {
 	.probe		= omap_l3_probe,
 	.driver		= {
 		.name		= "omap_l3_noc",
-		.owner		= THIS_MODULE,
 		.pm		= L3_DEV_PM_OPS,
 		.of_match_table = of_match_ptr(l3_noc_match),
 	},
diff --git a/drivers/char/agp/ali-agp.c b/drivers/char/agp/ali-agp.c
index 19db03667650..dcbbb4ea3cc1 100644
--- a/drivers/char/agp/ali-agp.c
+++ b/drivers/char/agp/ali-agp.c
@@ -417,6 +417,6 @@ static void __exit agp_ali_cleanup(void)
 module_init(agp_ali_init);
 module_exit(agp_ali_cleanup);
 
-MODULE_AUTHOR("Dave Jones <davej@redhat.com>");
+MODULE_AUTHOR("Dave Jones");
 MODULE_LICENSE("GPL and additional rights");
 
diff --git a/drivers/char/agp/amd64-agp.c b/drivers/char/agp/amd64-agp.c
index 3b47ed0310e1..0ef350010766 100644
--- a/drivers/char/agp/amd64-agp.c
+++ b/drivers/char/agp/amd64-agp.c
@@ -813,6 +813,6 @@ static void __exit agp_amd64_cleanup(void)
 module_init(agp_amd64_mod_init);
 module_exit(agp_amd64_cleanup);
 
-MODULE_AUTHOR("Dave Jones <davej@redhat.com>, Andi Kleen");
+MODULE_AUTHOR("Dave Jones, Andi Kleen");
 module_param(agp_try_unsupported, bool, 0);
 MODULE_LICENSE("GPL");
diff --git a/drivers/char/agp/ati-agp.c b/drivers/char/agp/ati-agp.c
index 18a7a6baa304..75a9786a77e6 100644
--- a/drivers/char/agp/ati-agp.c
+++ b/drivers/char/agp/ati-agp.c
@@ -579,6 +579,6 @@ static void __exit agp_ati_cleanup(void)
 module_init(agp_ati_init);
 module_exit(agp_ati_cleanup);
 
-MODULE_AUTHOR("Dave Jones <davej@redhat.com>");
+MODULE_AUTHOR("Dave Jones");
 MODULE_LICENSE("GPL and additional rights");
 
diff --git a/drivers/char/agp/backend.c b/drivers/char/agp/backend.c
index 317c28ce8328..38ffb281df97 100644
--- a/drivers/char/agp/backend.c
+++ b/drivers/char/agp/backend.c
@@ -356,7 +356,7 @@ static __init int agp_setup(char *s)
 __setup("agp=", agp_setup);
 #endif
 
-MODULE_AUTHOR("Dave Jones <davej@redhat.com>");
+MODULE_AUTHOR("Dave Jones, Jeff Hartmann");
 MODULE_DESCRIPTION("AGP GART driver");
 MODULE_LICENSE("GPL and additional rights");
 MODULE_ALIAS_MISCDEV(AGPGART_MINOR);
diff --git a/drivers/char/agp/intel-agp.c b/drivers/char/agp/intel-agp.c
index f9b9ca5d31b7..0a21daed5b62 100644
--- a/drivers/char/agp/intel-agp.c
+++ b/drivers/char/agp/intel-agp.c
@@ -920,5 +920,5 @@ static void __exit agp_intel_cleanup(void)
 module_init(agp_intel_init);
 module_exit(agp_intel_cleanup);
 
-MODULE_AUTHOR("Dave Jones <davej@redhat.com>");
+MODULE_AUTHOR("Dave Jones, Various @Intel");
 MODULE_LICENSE("GPL and additional rights");
diff --git a/drivers/char/agp/intel-gtt.c b/drivers/char/agp/intel-gtt.c
index 9a024f899dd4..92aa43fa8d70 100644
--- a/drivers/char/agp/intel-gtt.c
+++ b/drivers/char/agp/intel-gtt.c
@@ -153,7 +153,6 @@ static struct page *i8xx_alloc_pages(void)
 		__free_pages(page, 2);
 		return NULL;
 	}
-	get_page(page);
 	atomic_inc(&agp_bridge->current_memory_agp);
 	return page;
 }
@@ -164,7 +163,6 @@ static void i8xx_destroy_pages(struct page *page)
 		return;
 
 	set_pages_wb(page, 4);
-	put_page(page);
 	__free_pages(page, 2);
 	atomic_dec(&agp_bridge->current_memory_agp);
 }
@@ -300,7 +298,6 @@ static int intel_gtt_setup_scratch_page(void)
 	page = alloc_page(GFP_KERNEL | GFP_DMA32 | __GFP_ZERO);
 	if (page == NULL)
 		return -ENOMEM;
-	get_page(page);
 	set_pages_uc(page, 1);
 
 	if (intel_private.needs_dmar) {
@@ -560,7 +557,6 @@ static void intel_gtt_teardown_scratch_page(void)
 	set_pages_wb(intel_private.scratch_page, 1);
 	pci_unmap_page(intel_private.pcidev, intel_private.scratch_page_dma,
 		       PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
-	put_page(intel_private.scratch_page);
 	__free_page(intel_private.scratch_page);
 }
 
@@ -1442,5 +1438,5 @@ void intel_gmch_remove(void)
 }
 EXPORT_SYMBOL(intel_gmch_remove);
 
-MODULE_AUTHOR("Dave Jones <davej@redhat.com>");
+MODULE_AUTHOR("Dave Jones, Various @Intel");
 MODULE_LICENSE("GPL and additional rights");
diff --git a/drivers/char/agp/nvidia-agp.c b/drivers/char/agp/nvidia-agp.c
index a1861b75eb31..6c8d39cb566e 100644
--- a/drivers/char/agp/nvidia-agp.c
+++ b/drivers/char/agp/nvidia-agp.c
@@ -1,7 +1,7 @@
 /*
  * Nvidia AGPGART routines.
  * Based upon a 2.4 agpgart diff by the folks from NVIDIA, and hacked up
- * to work in 2.5 by Dave Jones <davej@redhat.com>
+ * to work in 2.5 by Dave Jones.
  */
 
 #include <linux/module.h>
diff --git a/drivers/char/agp/via-agp.c b/drivers/char/agp/via-agp.c
index 228f20cddc05..a4961d35e940 100644
--- a/drivers/char/agp/via-agp.c
+++ b/drivers/char/agp/via-agp.c
@@ -595,4 +595,4 @@ module_init(agp_via_init);
 module_exit(agp_via_cleanup);
 
 MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Dave Jones <davej@redhat.com>");
+MODULE_AUTHOR("Dave Jones");
diff --git a/drivers/char/hangcheck-timer.c b/drivers/char/hangcheck-timer.c
index ebc4c73d8ca4..a7c5c59675f0 100644
--- a/drivers/char/hangcheck-timer.c
+++ b/drivers/char/hangcheck-timer.c
@@ -168,8 +168,8 @@ static int __init hangcheck_init(void)
 	printk("Hangcheck: starting hangcheck timer %s (tick is %d seconds, margin is %d seconds).\n",
 	       VERSION_STR, hangcheck_tick, hangcheck_margin);
 	hangcheck_tsc_margin =
-		(unsigned long long)(hangcheck_margin + hangcheck_tick);
-	hangcheck_tsc_margin *= (unsigned long long)TIMER_FREQ;
+		(unsigned long long)hangcheck_margin + hangcheck_tick;
+	hangcheck_tsc_margin *= TIMER_FREQ;
 
 	hangcheck_tsc = ktime_get_ns();
 	mod_timer(&hangcheck_ticktock, jiffies + (hangcheck_tick*HZ));
diff --git a/drivers/char/hw_random/Kconfig b/drivers/char/hw_random/Kconfig
index 91a04ae8003c..de57b38809c7 100644
--- a/drivers/char/hw_random/Kconfig
+++ b/drivers/char/hw_random/Kconfig
@@ -64,7 +64,7 @@ config HW_RANDOM_AMD
 
 config HW_RANDOM_ATMEL
 	tristate "Atmel Random Number Generator support"
-	depends on ARCH_AT91 && HAVE_CLK
+	depends on ARCH_AT91 && HAVE_CLK && OF
 	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Random Number
diff --git a/drivers/char/hw_random/atmel-rng.c b/drivers/char/hw_random/atmel-rng.c
index 851bc7e20ad2..0fcc9e69a346 100644
--- a/drivers/char/hw_random/atmel-rng.c
+++ b/drivers/char/hw_random/atmel-rng.c
@@ -67,7 +67,7 @@ static int atmel_trng_probe(struct platform_device *pdev)
 	if (IS_ERR(trng->clk))
 		return PTR_ERR(trng->clk);
 
-	ret = clk_enable(trng->clk);
+	ret = clk_prepare_enable(trng->clk);
 	if (ret)
 		return ret;
 
@@ -95,7 +95,7 @@ static int atmel_trng_remove(struct platform_device *pdev)
 	hwrng_unregister(&trng->rng);
 
 	writel(TRNG_KEY, trng->base + TRNG_CR);
-	clk_disable(trng->clk);
+	clk_disable_unprepare(trng->clk);
 
 	return 0;
 }
@@ -105,7 +105,7 @@ static int atmel_trng_suspend(struct device *dev)
 {
 	struct atmel_trng *trng = dev_get_drvdata(dev);
 
-	clk_disable(trng->clk);
+	clk_disable_unprepare(trng->clk);
 
 	return 0;
 }
@@ -114,7 +114,7 @@ static int atmel_trng_resume(struct device *dev)
 {
 	struct atmel_trng *trng = dev_get_drvdata(dev);
 
-	return clk_enable(trng->clk);
+	return clk_prepare_enable(trng->clk);
 }
 
 static const struct dev_pm_ops atmel_trng_pm_ops = {
@@ -123,15 +123,21 @@ static const struct dev_pm_ops atmel_trng_pm_ops = {
 };
 #endif /* CONFIG_PM */
 
+static const struct of_device_id atmel_trng_dt_ids[] = {
+	{ .compatible = "atmel,at91sam9g45-trng" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, atmel_trng_dt_ids);
+
 static struct platform_driver atmel_trng_driver = {
 	.probe		= atmel_trng_probe,
 	.remove		= atmel_trng_remove,
 	.driver		= {
 		.name	= "atmel-trng",
-		.owner	= THIS_MODULE,
 #ifdef CONFIG_PM
 		.pm	= &atmel_trng_pm_ops,
 #endif /* CONFIG_PM */
+		.of_match_table = atmel_trng_dt_ids,
 	},
 };
 
diff --git a/drivers/char/hw_random/bcm2835-rng.c b/drivers/char/hw_random/bcm2835-rng.c
index e900961cdd2e..7192ec25f667 100644
--- a/drivers/char/hw_random/bcm2835-rng.c
+++ b/drivers/char/hw_random/bcm2835-rng.c
@@ -99,7 +99,6 @@ MODULE_DEVICE_TABLE(of, bcm2835_rng_of_match);
 static struct platform_driver bcm2835_rng_driver = {
 	.driver = {
 		.name = "bcm2835-rng",
-		.owner = THIS_MODULE,
 		.of_match_table = bcm2835_rng_of_match,
 	},
 	.probe		= bcm2835_rng_probe,
diff --git a/drivers/char/hw_random/bcm63xx-rng.c b/drivers/char/hw_random/bcm63xx-rng.c
index 36581ea562cb..ba6a65ac023b 100644
--- a/drivers/char/hw_random/bcm63xx-rng.c
+++ b/drivers/char/hw_random/bcm63xx-rng.c
@@ -162,7 +162,6 @@ static struct platform_driver bcm63xx_rng_driver = {
 	.remove		= bcm63xx_rng_remove,
 	.driver		= {
 		.name	= "bcm63xx-rng",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/char/hw_random/core.c b/drivers/char/hw_random/core.c
index aa30a25c8d49..1500cfd799a7 100644
--- a/drivers/char/hw_random/core.c
+++ b/drivers/char/hw_random/core.c
@@ -281,7 +281,6 @@ static ssize_t hwrng_attr_available_show(struct device *dev,
 					 char *buf)
 {
 	int err;
-	ssize_t ret = 0;
 	struct hwrng *rng;
 
 	err = mutex_lock_interruptible(&rng_mutex);
@@ -289,16 +288,13 @@ static ssize_t hwrng_attr_available_show(struct device *dev,
 		return -ERESTARTSYS;
 	buf[0] = '\0';
 	list_for_each_entry(rng, &rng_list, list) {
-		strncat(buf, rng->name, PAGE_SIZE - ret - 1);
-		ret += strlen(rng->name);
-		strncat(buf, " ", PAGE_SIZE - ret - 1);
-		ret++;
+		strlcat(buf, rng->name, PAGE_SIZE);
+		strlcat(buf, " ", PAGE_SIZE);
 	}
-	strncat(buf, "\n", PAGE_SIZE - ret - 1);
-	ret++;
+	strlcat(buf, "\n", PAGE_SIZE);
 	mutex_unlock(&rng_mutex);
 
-	return ret;
+	return strlen(buf);
 }
 
 static DEVICE_ATTR(rng_current, S_IRUGO | S_IWUSR,
diff --git a/drivers/char/hw_random/exynos-rng.c b/drivers/char/hw_random/exynos-rng.c
index 993efd7f6c7e..fed0830bf724 100644
--- a/drivers/char/hw_random/exynos-rng.c
+++ b/drivers/char/hw_random/exynos-rng.c
@@ -169,7 +169,6 @@ static UNIVERSAL_DEV_PM_OPS(exynos_rng_pm_ops, exynos_rng_runtime_suspend,
 static struct platform_driver exynos_rng_driver = {
 	.driver		= {
 		.name	= "exynos-rng",
-		.owner	= THIS_MODULE,
 		.pm	= &exynos_rng_pm_ops,
 	},
 	.probe		= exynos_rng_probe,
diff --git a/drivers/char/hw_random/msm-rng.c b/drivers/char/hw_random/msm-rng.c
index 148521e51dc6..cea1c703d62f 100644
--- a/drivers/char/hw_random/msm-rng.c
+++ b/drivers/char/hw_random/msm-rng.c
@@ -185,7 +185,6 @@ static struct platform_driver msm_rng_driver = {
 	.remove = msm_rng_remove,
 	.driver = {
 		.name = KBUILD_MODNAME,
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(msm_rng_of_match),
 	}
 };
diff --git a/drivers/char/hw_random/mxc-rnga.c b/drivers/char/hw_random/mxc-rnga.c
index 6a86b6f56af2..6cbb72ec6013 100644
--- a/drivers/char/hw_random/mxc-rnga.c
+++ b/drivers/char/hw_random/mxc-rnga.c
@@ -206,7 +206,6 @@ static int __exit mxc_rnga_remove(struct platform_device *pdev)
 static struct platform_driver mxc_rnga_driver = {
 	.driver = {
 		   .name = "mxc_rnga",
-		   .owner = THIS_MODULE,
 		   },
 	.remove = __exit_p(mxc_rnga_remove),
 };
diff --git a/drivers/char/hw_random/n2-drv.c b/drivers/char/hw_random/n2-drv.c
index 292a5889f675..843d6f6aee7a 100644
--- a/drivers/char/hw_random/n2-drv.c
+++ b/drivers/char/hw_random/n2-drv.c
@@ -750,7 +750,6 @@ MODULE_DEVICE_TABLE(of, n2rng_match);
 static struct platform_driver n2rng_driver = {
 	.driver = {
 		.name = "n2rng",
-		.owner = THIS_MODULE,
 		.of_match_table = n2rng_match,
 	},
 	.probe		= n2rng_probe,
diff --git a/drivers/char/hw_random/octeon-rng.c b/drivers/char/hw_random/octeon-rng.c
index b5cc3420c659..be1c3f607398 100644
--- a/drivers/char/hw_random/octeon-rng.c
+++ b/drivers/char/hw_random/octeon-rng.c
@@ -117,7 +117,6 @@ static int __exit octeon_rng_remove(struct platform_device *pdev)
 static struct platform_driver octeon_rng_driver = {
 	.driver = {
 		.name		= "octeon_rng",
-		.owner		= THIS_MODULE,
 	},
 	.probe		= octeon_rng_probe,
 	.remove		= __exit_p(octeon_rng_remove),
diff --git a/drivers/char/hw_random/omap-rng.c b/drivers/char/hw_random/omap-rng.c
index f66ea258382f..d14dcf788f17 100644
--- a/drivers/char/hw_random/omap-rng.c
+++ b/drivers/char/hw_random/omap-rng.c
@@ -456,7 +456,6 @@ static SIMPLE_DEV_PM_OPS(omap_rng_pm, omap_rng_suspend, omap_rng_resume);
 static struct platform_driver omap_rng_driver = {
 	.driver = {
 		.name		= "omap_rng",
-		.owner		= THIS_MODULE,
 		.pm		= OMAP_RNG_PM,
 		.of_match_table = of_match_ptr(omap_rng_of_match),
 	},
diff --git a/drivers/char/hw_random/omap3-rom-rng.c b/drivers/char/hw_random/omap3-rom-rng.c
index 6f2eaffed623..a405cdcd8dd2 100644
--- a/drivers/char/hw_random/omap3-rom-rng.c
+++ b/drivers/char/hw_random/omap3-rom-rng.c
@@ -126,7 +126,6 @@ static int omap3_rom_rng_remove(struct platform_device *pdev)
 static struct platform_driver omap3_rom_rng_driver = {
 	.driver = {
 		.name		= "omap3-rom-rng",
-		.owner		= THIS_MODULE,
 	},
 	.probe		= omap3_rom_rng_probe,
 	.remove		= omap3_rom_rng_remove,
diff --git a/drivers/char/hw_random/pasemi-rng.c b/drivers/char/hw_random/pasemi-rng.c
index c0347d1dded0..3eb7bdd7f93b 100644
--- a/drivers/char/hw_random/pasemi-rng.c
+++ b/drivers/char/hw_random/pasemi-rng.c
@@ -142,7 +142,6 @@ static struct of_device_id rng_match[] = {
 static struct platform_driver rng_driver = {
 	.driver = {
 		.name = "pasemi-rng",
-		.owner = THIS_MODULE,
 		.of_match_table = rng_match,
 	},
 	.probe		= rng_probe,
diff --git a/drivers/char/hw_random/ppc4xx-rng.c b/drivers/char/hw_random/ppc4xx-rng.c
index 521f76b0934b..c85d31a5f9e3 100644
--- a/drivers/char/hw_random/ppc4xx-rng.c
+++ b/drivers/char/hw_random/ppc4xx-rng.c
@@ -133,7 +133,6 @@ static struct of_device_id ppc4xx_rng_match[] = {
 static struct platform_driver ppc4xx_rng_driver = {
 	.driver = {
 		.name = MODULE_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = ppc4xx_rng_match,
 	},
 	.probe = ppc4xx_rng_probe,
diff --git a/drivers/char/hw_random/timeriomem-rng.c b/drivers/char/hw_random/timeriomem-rng.c
index b6ab9ac3f34d..cf37db263ecd 100644
--- a/drivers/char/hw_random/timeriomem-rng.c
+++ b/drivers/char/hw_random/timeriomem-rng.c
@@ -200,7 +200,6 @@ MODULE_DEVICE_TABLE(of, timeriomem_rng_match);
 static struct platform_driver timeriomem_rng_driver = {
 	.driver = {
 		.name		= "timeriomem_rng",
-		.owner		= THIS_MODULE,
 		.of_match_table	= timeriomem_rng_match,
 	},
 	.probe		= timeriomem_rng_probe,
diff --git a/drivers/char/hw_random/tx4939-rng.c b/drivers/char/hw_random/tx4939-rng.c
index 09c5fbea2b93..a7b694913416 100644
--- a/drivers/char/hw_random/tx4939-rng.c
+++ b/drivers/char/hw_random/tx4939-rng.c
@@ -158,7 +158,6 @@ static int __exit tx4939_rng_remove(struct platform_device *dev)
 static struct platform_driver tx4939_rng_driver = {
 	.driver		= {
 		.name	= "tx4939-rng",
-		.owner	= THIS_MODULE,
 	},
 	.remove = tx4939_rng_remove,
 };
diff --git a/drivers/char/i8k.c b/drivers/char/i8k.c
index 34174d01462e..e34a019eb930 100644
--- a/drivers/char/i8k.c
+++ b/drivers/char/i8k.c
@@ -298,7 +298,7 @@ static int i8k_get_temp(int sensor)
 	int temp;
 
 #ifdef I8K_TEMPERATURE_BUG
-	static int prev[4];
+	static int prev[4] = { I8K_MAX_TEMP+1, I8K_MAX_TEMP+1, I8K_MAX_TEMP+1, I8K_MAX_TEMP+1 };
 #endif
 	regs.ebx = sensor & 0xff;
 	rc = i8k_smm(&regs);
@@ -317,10 +317,12 @@ static int i8k_get_temp(int sensor)
 	 */
 	if (temp > I8K_MAX_TEMP) {
 		temp = prev[sensor];
-		prev[sensor] = I8K_MAX_TEMP;
+		prev[sensor] = I8K_MAX_TEMP+1;
 	} else {
 		prev[sensor] = temp;
 	}
+	if (temp > I8K_MAX_TEMP)
+		return -ERANGE;
 #endif
 
 	return temp;
@@ -499,6 +501,8 @@ static ssize_t i8k_hwmon_show_temp(struct device *dev,
 	int temp;
 
 	temp = i8k_get_temp(index);
+	if (temp == -ERANGE)
+		return -EINVAL;
 	if (temp < 0)
 		return temp;
 	return sprintf(buf, "%d\n", temp * 1000);
@@ -610,17 +614,17 @@ static int __init i8k_init_hwmon(void)
 
 	/* CPU temperature attributes, if temperature reading is OK */
 	err = i8k_get_temp(0);
-	if (err >= 0)
+	if (err >= 0 || err == -ERANGE)
 		i8k_hwmon_flags |= I8K_HWMON_HAVE_TEMP1;
 	/* check for additional temperature sensors */
 	err = i8k_get_temp(1);
-	if (err >= 0)
+	if (err >= 0 || err == -ERANGE)
 		i8k_hwmon_flags |= I8K_HWMON_HAVE_TEMP2;
 	err = i8k_get_temp(2);
-	if (err >= 0)
+	if (err >= 0 || err == -ERANGE)
 		i8k_hwmon_flags |= I8K_HWMON_HAVE_TEMP3;
 	err = i8k_get_temp(3);
-	if (err >= 0)
+	if (err >= 0 || err == -ERANGE)
 		i8k_hwmon_flags |= I8K_HWMON_HAVE_TEMP4;
 
 	/* Left fan attributes, if left fan is present */
@@ -711,6 +715,14 @@ static struct dmi_system_id i8k_dmi_table[] __initdata = {
 		.driver_data = (void *)&i8k_config_data[DELL_LATITUDE_D520],
 	},
 	{
+		.ident = "Dell Latitude E6440",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "Latitude E6440"),
+		},
+		.driver_data = (void *)&i8k_config_data[DELL_LATITUDE_E6540],
+	},
+	{
 		.ident = "Dell Latitude E6540",
 		.matches = {
 			DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
@@ -788,6 +800,8 @@ static struct dmi_system_id i8k_dmi_table[] __initdata = {
 	{ }
 };
 
+MODULE_DEVICE_TABLE(dmi, i8k_dmi_table);
+
 /*
  * Probe for the presence of a supported laptop.
  */
diff --git a/drivers/char/ipmi/Kconfig b/drivers/char/ipmi/Kconfig
index db1c9b7adaa6..6ed9e9fe5233 100644
--- a/drivers/char/ipmi/Kconfig
+++ b/drivers/char/ipmi/Kconfig
@@ -62,6 +62,20 @@ config IPMI_SI_PROBE_DEFAULTS
 	 only be available on older systems if the "ipmi_si_intf.trydefaults=1"
 	 boot argument is passed.
 
+config IPMI_SSIF
+       tristate 'IPMI SMBus handler (SSIF)'
+       select I2C
+       help
+         Provides a driver for a SMBus interface to a BMC, meaning that you
+	 have a driver that must be accessed over an I2C bus instead of a
+	 standard interface.  This module requires I2C support.
+
+config IPMI_POWERNV
+       depends on PPC_POWERNV
+       tristate 'POWERNV (OPAL firmware) IPMI interface'
+       help
+         Provides a driver for OPAL firmware-based IPMI interfaces.
+
 config IPMI_WATCHDOG
        tristate 'IPMI Watchdog Timer'
        help
diff --git a/drivers/char/ipmi/Makefile b/drivers/char/ipmi/Makefile
index 16a93648d54e..f3ffde1f5f1f 100644
--- a/drivers/char/ipmi/Makefile
+++ b/drivers/char/ipmi/Makefile
@@ -7,5 +7,7 @@ ipmi_si-y := ipmi_si_intf.o ipmi_kcs_sm.o ipmi_smic_sm.o ipmi_bt_sm.o
 obj-$(CONFIG_IPMI_HANDLER) += ipmi_msghandler.o
 obj-$(CONFIG_IPMI_DEVICE_INTERFACE) += ipmi_devintf.o
 obj-$(CONFIG_IPMI_SI) += ipmi_si.o
+obj-$(CONFIG_IPMI_SSIF) += ipmi_ssif.o
+obj-$(CONFIG_IPMI_POWERNV) += ipmi_powernv.o
 obj-$(CONFIG_IPMI_WATCHDOG) += ipmi_watchdog.o
 obj-$(CONFIG_IPMI_POWEROFF) += ipmi_poweroff.o
diff --git a/drivers/char/ipmi/ipmi_msghandler.c b/drivers/char/ipmi/ipmi_msghandler.c
index f816211f062f..6b65fa4e0c55 100644
--- a/drivers/char/ipmi/ipmi_msghandler.c
+++ b/drivers/char/ipmi/ipmi_msghandler.c
@@ -56,6 +56,8 @@ static int ipmi_init_msghandler(void);
 static void smi_recv_tasklet(unsigned long);
 static void handle_new_recv_msgs(ipmi_smi_t intf);
 static void need_waiter(ipmi_smi_t intf);
+static int handle_one_recv_msg(ipmi_smi_t          intf,
+			       struct ipmi_smi_msg *msg);
 
 static int initialized;
 
@@ -191,25 +193,14 @@ struct ipmi_proc_entry {
 #endif
 
 struct bmc_device {
-	struct platform_device *dev;
+	struct platform_device pdev;
 	struct ipmi_device_id  id;
 	unsigned char          guid[16];
 	int                    guid_set;
-
-	struct kref	       refcount;
-
-	/* bmc device attributes */
-	struct device_attribute device_id_attr;
-	struct device_attribute provides_dev_sdrs_attr;
-	struct device_attribute revision_attr;
-	struct device_attribute firmware_rev_attr;
-	struct device_attribute version_attr;
-	struct device_attribute add_dev_support_attr;
-	struct device_attribute manufacturer_id_attr;
-	struct device_attribute product_id_attr;
-	struct device_attribute guid_attr;
-	struct device_attribute aux_firmware_rev_attr;
+	char                   name[16];
+	struct kref	       usecount;
 };
+#define to_bmc_device(x) container_of((x), struct bmc_device, pdev.dev)
 
 /*
  * Various statistics for IPMI, these index stats[] in the ipmi_smi
@@ -323,6 +314,9 @@ struct ipmi_smi {
 
 	struct kref refcount;
 
+	/* Set when the interface is being unregistered. */
+	bool in_shutdown;
+
 	/* Used for a list of interfaces. */
 	struct list_head link;
 
@@ -341,7 +335,6 @@ struct ipmi_smi {
 
 	struct bmc_device *bmc;
 	char *my_dev_name;
-	char *sysfs_name;
 
 	/*
 	 * This is the lower-layer's sender routine.  Note that you
@@ -377,11 +370,16 @@ struct ipmi_smi {
 	 * periodic timer interrupt.  The tasklet is for handling received
 	 * messages directly from the handler.
 	 */
-	spinlock_t       waiting_msgs_lock;
-	struct list_head waiting_msgs;
+	spinlock_t       waiting_rcv_msgs_lock;
+	struct list_head waiting_rcv_msgs;
 	atomic_t	 watchdog_pretimeouts_to_deliver;
 	struct tasklet_struct recv_tasklet;
 
+	spinlock_t             xmit_msgs_lock;
+	struct list_head       xmit_msgs;
+	struct ipmi_smi_msg    *curr_msg;
+	struct list_head       hp_xmit_msgs;
+
 	/*
 	 * The list of command receivers that are registered for commands
 	 * on this interface.
@@ -474,6 +472,18 @@ static DEFINE_MUTEX(smi_watchers_mutex);
 #define ipmi_get_stat(intf, stat) \
 	((unsigned int) atomic_read(&(intf)->stats[IPMI_STAT_ ## stat]))
 
+static char *addr_src_to_str[] = { "invalid", "hotmod", "hardcoded", "SPMI",
+				   "ACPI", "SMBIOS", "PCI",
+				   "device-tree", "default" };
+
+const char *ipmi_addr_src_to_str(enum ipmi_addr_src src)
+{
+	if (src > SI_DEFAULT)
+		src = 0; /* Invalid */
+	return addr_src_to_str[src];
+}
+EXPORT_SYMBOL(ipmi_addr_src_to_str);
+
 static int is_lan_addr(struct ipmi_addr *addr)
 {
 	return addr->addr_type == IPMI_LAN_ADDR_TYPE;
@@ -517,7 +527,7 @@ static void clean_up_interface_data(ipmi_smi_t intf)
 
 	tasklet_kill(&intf->recv_tasklet);
 
-	free_smi_msg_list(&intf->waiting_msgs);
+	free_smi_msg_list(&intf->waiting_rcv_msgs);
 	free_recv_msg_list(&intf->waiting_events);
 
 	/*
@@ -1473,6 +1483,30 @@ static inline void format_lan_msg(struct ipmi_smi_msg   *smi_msg,
 	smi_msg->msgid = msgid;
 }
 
+static void smi_send(ipmi_smi_t intf, struct ipmi_smi_handlers *handlers,
+		     struct ipmi_smi_msg *smi_msg, int priority)
+{
+	int run_to_completion = intf->run_to_completion;
+	unsigned long flags;
+
+	if (!run_to_completion)
+		spin_lock_irqsave(&intf->xmit_msgs_lock, flags);
+	if (intf->curr_msg) {
+		if (priority > 0)
+			list_add_tail(&smi_msg->link, &intf->hp_xmit_msgs);
+		else
+			list_add_tail(&smi_msg->link, &intf->xmit_msgs);
+		smi_msg = NULL;
+	} else {
+		intf->curr_msg = smi_msg;
+	}
+	if (!run_to_completion)
+		spin_unlock_irqrestore(&intf->xmit_msgs_lock, flags);
+
+	if (smi_msg)
+		handlers->sender(intf->send_info, smi_msg);
+}
+
 /*
  * Separate from ipmi_request so that the user does not have to be
  * supplied in certain circumstances (mainly at panic time).  If
@@ -1497,7 +1531,6 @@ static int i_ipmi_request(ipmi_user_t          user,
 	struct ipmi_smi_msg      *smi_msg;
 	struct ipmi_recv_msg     *recv_msg;
 	unsigned long            flags;
-	struct ipmi_smi_handlers *handlers;
 
 
 	if (supplied_recv)
@@ -1520,8 +1553,7 @@ static int i_ipmi_request(ipmi_user_t          user,
 	}
 
 	rcu_read_lock();
-	handlers = intf->handlers;
-	if (!handlers) {
+	if (intf->in_shutdown) {
 		rv = -ENODEV;
 		goto out_err;
 	}
@@ -1856,7 +1888,7 @@ static int i_ipmi_request(ipmi_user_t          user,
 	}
 #endif
 
-	handlers->sender(intf->send_info, smi_msg, priority);
+	smi_send(intf, intf->handlers, smi_msg, priority);
 	rcu_read_unlock();
 
 	return 0;
@@ -2153,7 +2185,7 @@ static void remove_proc_entries(ipmi_smi_t smi)
 static int __find_bmc_guid(struct device *dev, void *data)
 {
 	unsigned char *id = data;
-	struct bmc_device *bmc = dev_get_drvdata(dev);
+	struct bmc_device *bmc = to_bmc_device(dev);
 	return memcmp(bmc->guid, id, 16) == 0;
 }
 
@@ -2164,7 +2196,7 @@ static struct bmc_device *ipmi_find_bmc_guid(struct device_driver *drv,
 
 	dev = driver_find_device(drv, NULL, guid, __find_bmc_guid);
 	if (dev)
-		return dev_get_drvdata(dev);
+		return to_bmc_device(dev);
 	else
 		return NULL;
 }
@@ -2177,7 +2209,7 @@ struct prod_dev_id {
 static int __find_bmc_prod_dev_id(struct device *dev, void *data)
 {
 	struct prod_dev_id *id = data;
-	struct bmc_device *bmc = dev_get_drvdata(dev);
+	struct bmc_device *bmc = to_bmc_device(dev);
 
 	return (bmc->id.product_id == id->product_id
 		&& bmc->id.device_id == id->device_id);
@@ -2195,7 +2227,7 @@ static struct bmc_device *ipmi_find_bmc_prod_dev_id(
 
 	dev = driver_find_device(drv, NULL, &id, __find_bmc_prod_dev_id);
 	if (dev)
-		return dev_get_drvdata(dev);
+		return to_bmc_device(dev);
 	else
 		return NULL;
 }
@@ -2204,84 +2236,94 @@ static ssize_t device_id_show(struct device *dev,
 			      struct device_attribute *attr,
 			      char *buf)
 {
-	struct bmc_device *bmc = dev_get_drvdata(dev);
+	struct bmc_device *bmc = to_bmc_device(dev);
 
 	return snprintf(buf, 10, "%u\n", bmc->id.device_id);
 }
+static DEVICE_ATTR(device_id, S_IRUGO, device_id_show, NULL);
 
-static ssize_t provides_dev_sdrs_show(struct device *dev,
-				      struct device_attribute *attr,
-				      char *buf)
+static ssize_t provides_device_sdrs_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf)
 {
-	struct bmc_device *bmc = dev_get_drvdata(dev);
+	struct bmc_device *bmc = to_bmc_device(dev);
 
 	return snprintf(buf, 10, "%u\n",
 			(bmc->id.device_revision & 0x80) >> 7);
 }
+static DEVICE_ATTR(provides_device_sdrs, S_IRUGO, provides_device_sdrs_show,
+		   NULL);
 
 static ssize_t revision_show(struct device *dev, struct device_attribute *attr,
 			     char *buf)
 {
-	struct bmc_device *bmc = dev_get_drvdata(dev);
+	struct bmc_device *bmc = to_bmc_device(dev);
 
 	return snprintf(buf, 20, "%u\n",
 			bmc->id.device_revision & 0x0F);
 }
+static DEVICE_ATTR(revision, S_IRUGO, revision_show, NULL);
 
-static ssize_t firmware_rev_show(struct device *dev,
-				 struct device_attribute *attr,
-				 char *buf)
+static ssize_t firmware_revision_show(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf)
 {
-	struct bmc_device *bmc = dev_get_drvdata(dev);
+	struct bmc_device *bmc = to_bmc_device(dev);
 
 	return snprintf(buf, 20, "%u.%x\n", bmc->id.firmware_revision_1,
 			bmc->id.firmware_revision_2);
 }
+static DEVICE_ATTR(firmware_revision, S_IRUGO, firmware_revision_show, NULL);
 
 static ssize_t ipmi_version_show(struct device *dev,
 				 struct device_attribute *attr,
 				 char *buf)
 {
-	struct bmc_device *bmc = dev_get_drvdata(dev);
+	struct bmc_device *bmc = to_bmc_device(dev);
 
 	return snprintf(buf, 20, "%u.%u\n",
 			ipmi_version_major(&bmc->id),
 			ipmi_version_minor(&bmc->id));
 }
+static DEVICE_ATTR(ipmi_version, S_IRUGO, ipmi_version_show, NULL);
 
 static ssize_t add_dev_support_show(struct device *dev,
 				    struct device_attribute *attr,
 				    char *buf)
 {
-	struct bmc_device *bmc = dev_get_drvdata(dev);
+	struct bmc_device *bmc = to_bmc_device(dev);
 
 	return snprintf(buf, 10, "0x%02x\n",
 			bmc->id.additional_device_support);
 }
+static DEVICE_ATTR(additional_device_support, S_IRUGO, add_dev_support_show,
+		   NULL);
 
 static ssize_t manufacturer_id_show(struct device *dev,
 				    struct device_attribute *attr,
 				    char *buf)
 {
-	struct bmc_device *bmc = dev_get_drvdata(dev);
+	struct bmc_device *bmc = to_bmc_device(dev);
 
 	return snprintf(buf, 20, "0x%6.6x\n", bmc->id.manufacturer_id);
 }
+static DEVICE_ATTR(manufacturer_id, S_IRUGO, manufacturer_id_show, NULL);
 
 static ssize_t product_id_show(struct device *dev,
 			       struct device_attribute *attr,
 			       char *buf)
 {
-	struct bmc_device *bmc = dev_get_drvdata(dev);
+	struct bmc_device *bmc = to_bmc_device(dev);
 
 	return snprintf(buf, 10, "0x%4.4x\n", bmc->id.product_id);
 }
+static DEVICE_ATTR(product_id, S_IRUGO, product_id_show, NULL);
 
 static ssize_t aux_firmware_rev_show(struct device *dev,
 				     struct device_attribute *attr,
 				     char *buf)
 {
-	struct bmc_device *bmc = dev_get_drvdata(dev);
+	struct bmc_device *bmc = to_bmc_device(dev);
 
 	return snprintf(buf, 21, "0x%02x 0x%02x 0x%02x 0x%02x\n",
 			bmc->id.aux_firmware_revision[3],
@@ -2289,175 +2331,95 @@ static ssize_t aux_firmware_rev_show(struct device *dev,
 			bmc->id.aux_firmware_revision[1],
 			bmc->id.aux_firmware_revision[0]);
 }
+static DEVICE_ATTR(aux_firmware_revision, S_IRUGO, aux_firmware_rev_show, NULL);
 
 static ssize_t guid_show(struct device *dev, struct device_attribute *attr,
 			 char *buf)
 {
-	struct bmc_device *bmc = dev_get_drvdata(dev);
+	struct bmc_device *bmc = to_bmc_device(dev);
 
 	return snprintf(buf, 100, "%Lx%Lx\n",
 			(long long) bmc->guid[0],
 			(long long) bmc->guid[8]);
 }
+static DEVICE_ATTR(guid, S_IRUGO, guid_show, NULL);
+
+static struct attribute *bmc_dev_attrs[] = {
+	&dev_attr_device_id.attr,
+	&dev_attr_provides_device_sdrs.attr,
+	&dev_attr_revision.attr,
+	&dev_attr_firmware_revision.attr,
+	&dev_attr_ipmi_version.attr,
+	&dev_attr_additional_device_support.attr,
+	&dev_attr_manufacturer_id.attr,
+	&dev_attr_product_id.attr,
+	NULL
+};
 
-static void remove_files(struct bmc_device *bmc)
-{
-	if (!bmc->dev)
-		return;
+static struct attribute_group bmc_dev_attr_group = {
+	.attrs		= bmc_dev_attrs,
+};
 
-	device_remove_file(&bmc->dev->dev,
-			   &bmc->device_id_attr);
-	device_remove_file(&bmc->dev->dev,
-			   &bmc->provides_dev_sdrs_attr);
-	device_remove_file(&bmc->dev->dev,
-			   &bmc->revision_attr);
-	device_remove_file(&bmc->dev->dev,
-			   &bmc->firmware_rev_attr);
-	device_remove_file(&bmc->dev->dev,
-			   &bmc->version_attr);
-	device_remove_file(&bmc->dev->dev,
-			   &bmc->add_dev_support_attr);
-	device_remove_file(&bmc->dev->dev,
-			   &bmc->manufacturer_id_attr);
-	device_remove_file(&bmc->dev->dev,
-			   &bmc->product_id_attr);
+static const struct attribute_group *bmc_dev_attr_groups[] = {
+	&bmc_dev_attr_group,
+	NULL
+};
 
-	if (bmc->id.aux_firmware_revision_set)
-		device_remove_file(&bmc->dev->dev,
-				   &bmc->aux_firmware_rev_attr);
-	if (bmc->guid_set)
-		device_remove_file(&bmc->dev->dev,
-				   &bmc->guid_attr);
+static struct device_type bmc_device_type = {
+	.groups		= bmc_dev_attr_groups,
+};
+
+static void
+release_bmc_device(struct device *dev)
+{
+	kfree(to_bmc_device(dev));
 }
 
 static void
 cleanup_bmc_device(struct kref *ref)
 {
-	struct bmc_device *bmc;
+	struct bmc_device *bmc = container_of(ref, struct bmc_device, usecount);
 
-	bmc = container_of(ref, struct bmc_device, refcount);
+	if (bmc->id.aux_firmware_revision_set)
+		device_remove_file(&bmc->pdev.dev,
+				   &dev_attr_aux_firmware_revision);
+	if (bmc->guid_set)
+		device_remove_file(&bmc->pdev.dev,
+				   &dev_attr_guid);
 
-	remove_files(bmc);
-	platform_device_unregister(bmc->dev);
-	kfree(bmc);
+	platform_device_unregister(&bmc->pdev);
 }
 
 static void ipmi_bmc_unregister(ipmi_smi_t intf)
 {
 	struct bmc_device *bmc = intf->bmc;
 
-	if (intf->sysfs_name) {
-		sysfs_remove_link(&intf->si_dev->kobj, intf->sysfs_name);
-		kfree(intf->sysfs_name);
-		intf->sysfs_name = NULL;
-	}
+	sysfs_remove_link(&intf->si_dev->kobj, "bmc");
 	if (intf->my_dev_name) {
-		sysfs_remove_link(&bmc->dev->dev.kobj, intf->my_dev_name);
+		sysfs_remove_link(&bmc->pdev.dev.kobj, intf->my_dev_name);
 		kfree(intf->my_dev_name);
 		intf->my_dev_name = NULL;
 	}
 
 	mutex_lock(&ipmidriver_mutex);
-	kref_put(&bmc->refcount, cleanup_bmc_device);
+	kref_put(&bmc->usecount, cleanup_bmc_device);
 	intf->bmc = NULL;
 	mutex_unlock(&ipmidriver_mutex);
 }
 
-static int create_files(struct bmc_device *bmc)
+static int create_bmc_files(struct bmc_device *bmc)
 {
 	int err;
 
-	bmc->device_id_attr.attr.name = "device_id";
-	bmc->device_id_attr.attr.mode = S_IRUGO;
-	bmc->device_id_attr.show = device_id_show;
-	sysfs_attr_init(&bmc->device_id_attr.attr);
-
-	bmc->provides_dev_sdrs_attr.attr.name = "provides_device_sdrs";
-	bmc->provides_dev_sdrs_attr.attr.mode = S_IRUGO;
-	bmc->provides_dev_sdrs_attr.show = provides_dev_sdrs_show;
-	sysfs_attr_init(&bmc->provides_dev_sdrs_attr.attr);
-
-	bmc->revision_attr.attr.name = "revision";
-	bmc->revision_attr.attr.mode = S_IRUGO;
-	bmc->revision_attr.show = revision_show;
-	sysfs_attr_init(&bmc->revision_attr.attr);
-
-	bmc->firmware_rev_attr.attr.name = "firmware_revision";
-	bmc->firmware_rev_attr.attr.mode = S_IRUGO;
-	bmc->firmware_rev_attr.show = firmware_rev_show;
-	sysfs_attr_init(&bmc->firmware_rev_attr.attr);
-
-	bmc->version_attr.attr.name = "ipmi_version";
-	bmc->version_attr.attr.mode = S_IRUGO;
-	bmc->version_attr.show = ipmi_version_show;
-	sysfs_attr_init(&bmc->version_attr.attr);
-
-	bmc->add_dev_support_attr.attr.name = "additional_device_support";
-	bmc->add_dev_support_attr.attr.mode = S_IRUGO;
-	bmc->add_dev_support_attr.show = add_dev_support_show;
-	sysfs_attr_init(&bmc->add_dev_support_attr.attr);
-
-	bmc->manufacturer_id_attr.attr.name = "manufacturer_id";
-	bmc->manufacturer_id_attr.attr.mode = S_IRUGO;
-	bmc->manufacturer_id_attr.show = manufacturer_id_show;
-	sysfs_attr_init(&bmc->manufacturer_id_attr.attr);
-
-	bmc->product_id_attr.attr.name = "product_id";
-	bmc->product_id_attr.attr.mode = S_IRUGO;
-	bmc->product_id_attr.show = product_id_show;
-	sysfs_attr_init(&bmc->product_id_attr.attr);
-
-	bmc->guid_attr.attr.name = "guid";
-	bmc->guid_attr.attr.mode = S_IRUGO;
-	bmc->guid_attr.show = guid_show;
-	sysfs_attr_init(&bmc->guid_attr.attr);
-
-	bmc->aux_firmware_rev_attr.attr.name = "aux_firmware_revision";
-	bmc->aux_firmware_rev_attr.attr.mode = S_IRUGO;
-	bmc->aux_firmware_rev_attr.show = aux_firmware_rev_show;
-	sysfs_attr_init(&bmc->aux_firmware_rev_attr.attr);
-
-	err = device_create_file(&bmc->dev->dev,
-			   &bmc->device_id_attr);
-	if (err)
-		goto out;
-	err = device_create_file(&bmc->dev->dev,
-			   &bmc->provides_dev_sdrs_attr);
-	if (err)
-		goto out_devid;
-	err = device_create_file(&bmc->dev->dev,
-			   &bmc->revision_attr);
-	if (err)
-		goto out_sdrs;
-	err = device_create_file(&bmc->dev->dev,
-			   &bmc->firmware_rev_attr);
-	if (err)
-		goto out_rev;
-	err = device_create_file(&bmc->dev->dev,
-			   &bmc->version_attr);
-	if (err)
-		goto out_firm;
-	err = device_create_file(&bmc->dev->dev,
-			   &bmc->add_dev_support_attr);
-	if (err)
-		goto out_version;
-	err = device_create_file(&bmc->dev->dev,
-			   &bmc->manufacturer_id_attr);
-	if (err)
-		goto out_add_dev;
-	err = device_create_file(&bmc->dev->dev,
-			   &bmc->product_id_attr);
-	if (err)
-		goto out_manu;
 	if (bmc->id.aux_firmware_revision_set) {
-		err = device_create_file(&bmc->dev->dev,
-				   &bmc->aux_firmware_rev_attr);
+		err = device_create_file(&bmc->pdev.dev,
+					 &dev_attr_aux_firmware_revision);
 		if (err)
-			goto out_prod_id;
+			goto out;
 	}
 	if (bmc->guid_set) {
-		err = device_create_file(&bmc->dev->dev,
-				   &bmc->guid_attr);
+		err = device_create_file(&bmc->pdev.dev,
+					 &dev_attr_guid);
 		if (err)
 			goto out_aux_firm;
 	}
@@ -2466,44 +2428,17 @@ static int create_files(struct bmc_device *bmc)
 
 out_aux_firm:
 	if (bmc->id.aux_firmware_revision_set)
-		device_remove_file(&bmc->dev->dev,
-				   &bmc->aux_firmware_rev_attr);
-out_prod_id:
-	device_remove_file(&bmc->dev->dev,
-			   &bmc->product_id_attr);
-out_manu:
-	device_remove_file(&bmc->dev->dev,
-			   &bmc->manufacturer_id_attr);
-out_add_dev:
-	device_remove_file(&bmc->dev->dev,
-			   &bmc->add_dev_support_attr);
-out_version:
-	device_remove_file(&bmc->dev->dev,
-			   &bmc->version_attr);
-out_firm:
-	device_remove_file(&bmc->dev->dev,
-			   &bmc->firmware_rev_attr);
-out_rev:
-	device_remove_file(&bmc->dev->dev,
-			   &bmc->revision_attr);
-out_sdrs:
-	device_remove_file(&bmc->dev->dev,
-			   &bmc->provides_dev_sdrs_attr);
-out_devid:
-	device_remove_file(&bmc->dev->dev,
-			   &bmc->device_id_attr);
+		device_remove_file(&bmc->pdev.dev,
+				   &dev_attr_aux_firmware_revision);
 out:
 	return err;
 }
 
-static int ipmi_bmc_register(ipmi_smi_t intf, int ifnum,
-			     const char *sysfs_name)
+static int ipmi_bmc_register(ipmi_smi_t intf, int ifnum)
 {
 	int               rv;
 	struct bmc_device *bmc = intf->bmc;
 	struct bmc_device *old_bmc;
-	int               size;
-	char              dummy[1];
 
 	mutex_lock(&ipmidriver_mutex);
 
@@ -2527,7 +2462,7 @@ static int ipmi_bmc_register(ipmi_smi_t intf, int ifnum,
 		intf->bmc = old_bmc;
 		bmc = old_bmc;
 
-		kref_get(&bmc->refcount);
+		kref_get(&bmc->usecount);
 		mutex_unlock(&ipmidriver_mutex);
 
 		printk(KERN_INFO
@@ -2537,12 +2472,12 @@ static int ipmi_bmc_register(ipmi_smi_t intf, int ifnum,
 		       bmc->id.product_id,
 		       bmc->id.device_id);
 	} else {
-		char name[14];
 		unsigned char orig_dev_id = bmc->id.device_id;
 		int warn_printed = 0;
 
-		snprintf(name, sizeof(name),
+		snprintf(bmc->name, sizeof(bmc->name),
 			 "ipmi_bmc.%4.4x", bmc->id.product_id);
+		bmc->pdev.name = bmc->name;
 
 		while (ipmi_find_bmc_prod_dev_id(&ipmidriver.driver,
 						 bmc->id.product_id,
@@ -2566,23 +2501,16 @@ static int ipmi_bmc_register(ipmi_smi_t intf, int ifnum,
 			}
 		}
 
-		bmc->dev = platform_device_alloc(name, bmc->id.device_id);
-		if (!bmc->dev) {
-			mutex_unlock(&ipmidriver_mutex);
-			printk(KERN_ERR
-			       "ipmi_msghandler:"
-			       " Unable to allocate platform device\n");
-			return -ENOMEM;
-		}
-		bmc->dev->dev.driver = &ipmidriver.driver;
-		dev_set_drvdata(&bmc->dev->dev, bmc);
-		kref_init(&bmc->refcount);
+		bmc->pdev.dev.driver = &ipmidriver.driver;
+		bmc->pdev.id = bmc->id.device_id;
+		bmc->pdev.dev.release = release_bmc_device;
+		bmc->pdev.dev.type = &bmc_device_type;
+		kref_init(&bmc->usecount);
 
-		rv = platform_device_add(bmc->dev);
+		rv = platform_device_register(&bmc->pdev);
 		mutex_unlock(&ipmidriver_mutex);
 		if (rv) {
-			platform_device_put(bmc->dev);
-			bmc->dev = NULL;
+			put_device(&bmc->pdev.dev);
 			printk(KERN_ERR
 			       "ipmi_msghandler:"
 			       " Unable to register bmc device: %d\n",
@@ -2594,10 +2522,10 @@ static int ipmi_bmc_register(ipmi_smi_t intf, int ifnum,
 			return rv;
 		}
 
-		rv = create_files(bmc);
+		rv = create_bmc_files(bmc);
 		if (rv) {
 			mutex_lock(&ipmidriver_mutex);
-			platform_device_unregister(bmc->dev);
+			platform_device_unregister(&bmc->pdev);
 			mutex_unlock(&ipmidriver_mutex);
 
 			return rv;
@@ -2614,44 +2542,26 @@ static int ipmi_bmc_register(ipmi_smi_t intf, int ifnum,
 	 * create symlink from system interface device to bmc device
 	 * and back.
 	 */
-	intf->sysfs_name = kstrdup(sysfs_name, GFP_KERNEL);
-	if (!intf->sysfs_name) {
-		rv = -ENOMEM;
-		printk(KERN_ERR
-		       "ipmi_msghandler: allocate link to BMC: %d\n",
-		       rv);
-		goto out_err;
-	}
-
-	rv = sysfs_create_link(&intf->si_dev->kobj,
-			       &bmc->dev->dev.kobj, intf->sysfs_name);
+	rv = sysfs_create_link(&intf->si_dev->kobj, &bmc->pdev.dev.kobj, "bmc");
 	if (rv) {
-		kfree(intf->sysfs_name);
-		intf->sysfs_name = NULL;
 		printk(KERN_ERR
 		       "ipmi_msghandler: Unable to create bmc symlink: %d\n",
 		       rv);
 		goto out_err;
 	}
 
-	size = snprintf(dummy, 0, "ipmi%d", ifnum);
-	intf->my_dev_name = kmalloc(size+1, GFP_KERNEL);
+	intf->my_dev_name = kasprintf(GFP_KERNEL, "ipmi%d", ifnum);
 	if (!intf->my_dev_name) {
-		kfree(intf->sysfs_name);
-		intf->sysfs_name = NULL;
 		rv = -ENOMEM;
 		printk(KERN_ERR
 		       "ipmi_msghandler: allocate link from BMC: %d\n",
 		       rv);
 		goto out_err;
 	}
-	snprintf(intf->my_dev_name, size+1, "ipmi%d", ifnum);
 
-	rv = sysfs_create_link(&bmc->dev->dev.kobj, &intf->si_dev->kobj,
+	rv = sysfs_create_link(&bmc->pdev.dev.kobj, &intf->si_dev->kobj,
 			       intf->my_dev_name);
 	if (rv) {
-		kfree(intf->sysfs_name);
-		intf->sysfs_name = NULL;
 		kfree(intf->my_dev_name);
 		intf->my_dev_name = NULL;
 		printk(KERN_ERR
@@ -2850,7 +2760,6 @@ int ipmi_register_smi(struct ipmi_smi_handlers *handlers,
 		      void		       *send_info,
 		      struct ipmi_device_id    *device_id,
 		      struct device            *si_dev,
-		      const char               *sysfs_name,
 		      unsigned char            slave_addr)
 {
 	int              i, j;
@@ -2909,12 +2818,15 @@ int ipmi_register_smi(struct ipmi_smi_handlers *handlers,
 #ifdef CONFIG_PROC_FS
 	mutex_init(&intf->proc_entry_lock);
 #endif
-	spin_lock_init(&intf->waiting_msgs_lock);
-	INIT_LIST_HEAD(&intf->waiting_msgs);
+	spin_lock_init(&intf->waiting_rcv_msgs_lock);
+	INIT_LIST_HEAD(&intf->waiting_rcv_msgs);
 	tasklet_init(&intf->recv_tasklet,
 		     smi_recv_tasklet,
 		     (unsigned long) intf);
 	atomic_set(&intf->watchdog_pretimeouts_to_deliver, 0);
+	spin_lock_init(&intf->xmit_msgs_lock);
+	INIT_LIST_HEAD(&intf->xmit_msgs);
+	INIT_LIST_HEAD(&intf->hp_xmit_msgs);
 	spin_lock_init(&intf->events_lock);
 	atomic_set(&intf->event_waiters, 0);
 	intf->ticks_to_req_ev = IPMI_REQUEST_EV_TIME;
@@ -2984,7 +2896,7 @@ int ipmi_register_smi(struct ipmi_smi_handlers *handlers,
 	if (rv == 0)
 		rv = add_proc_entries(intf, i);
 
-	rv = ipmi_bmc_register(intf, i, sysfs_name);
+	rv = ipmi_bmc_register(intf, i);
 
  out:
 	if (rv) {
@@ -3014,12 +2926,50 @@ int ipmi_register_smi(struct ipmi_smi_handlers *handlers,
 }
 EXPORT_SYMBOL(ipmi_register_smi);
 
+static void deliver_smi_err_response(ipmi_smi_t intf,
+				     struct ipmi_smi_msg *msg,
+				     unsigned char err)
+{
+	msg->rsp[0] = msg->data[0] | 4;
+	msg->rsp[1] = msg->data[1];
+	msg->rsp[2] = err;
+	msg->rsp_size = 3;
+	/* It's an error, so it will never requeue, no need to check return. */
+	handle_one_recv_msg(intf, msg);
+}
+
 static void cleanup_smi_msgs(ipmi_smi_t intf)
 {
 	int              i;
 	struct seq_table *ent;
+	struct ipmi_smi_msg *msg;
+	struct list_head *entry;
+	struct list_head tmplist;
+
+	/* Clear out our transmit queues and hold the messages. */
+	INIT_LIST_HEAD(&tmplist);
+	list_splice_tail(&intf->hp_xmit_msgs, &tmplist);
+	list_splice_tail(&intf->xmit_msgs, &tmplist);
+
+	/* Current message first, to preserve order */
+	while (intf->curr_msg && !list_empty(&intf->waiting_rcv_msgs)) {
+		/* Wait for the message to clear out. */
+		schedule_timeout(1);
+	}
 
 	/* No need for locks, the interface is down. */
+
+	/*
+	 * Return errors for all pending messages in queue and in the
+	 * tables waiting for remote responses.
+	 */
+	while (!list_empty(&tmplist)) {
+		entry = tmplist.next;
+		list_del(entry);
+		msg = list_entry(entry, struct ipmi_smi_msg, link);
+		deliver_smi_err_response(intf, msg, IPMI_ERR_UNSPECIFIED);
+	}
+
 	for (i = 0; i < IPMI_IPMB_NUM_SEQ; i++) {
 		ent = &(intf->seq_table[i]);
 		if (!ent->inuse)
@@ -3031,20 +2981,33 @@ static void cleanup_smi_msgs(ipmi_smi_t intf)
 int ipmi_unregister_smi(ipmi_smi_t intf)
 {
 	struct ipmi_smi_watcher *w;
-	int    intf_num = intf->intf_num;
+	int intf_num = intf->intf_num;
+	ipmi_user_t user;
 
 	ipmi_bmc_unregister(intf);
 
 	mutex_lock(&smi_watchers_mutex);
 	mutex_lock(&ipmi_interfaces_mutex);
 	intf->intf_num = -1;
-	intf->handlers = NULL;
+	intf->in_shutdown = true;
 	list_del_rcu(&intf->link);
 	mutex_unlock(&ipmi_interfaces_mutex);
 	synchronize_rcu();
 
 	cleanup_smi_msgs(intf);
 
+	/* Clean up the effects of users on the lower-level software. */
+	mutex_lock(&ipmi_interfaces_mutex);
+	rcu_read_lock();
+	list_for_each_entry_rcu(user, &intf->users, link) {
+		module_put(intf->handlers->owner);
+		if (intf->handlers->dec_usecount)
+			intf->handlers->dec_usecount(intf->send_info);
+	}
+	rcu_read_unlock();
+	intf->handlers = NULL;
+	mutex_unlock(&ipmi_interfaces_mutex);
+
 	remove_proc_entries(intf);
 
 	/*
@@ -3134,7 +3097,6 @@ static int handle_ipmb_get_msg_cmd(ipmi_smi_t          intf,
 	ipmi_user_t              user = NULL;
 	struct ipmi_ipmb_addr    *ipmb_addr;
 	struct ipmi_recv_msg     *recv_msg;
-	struct ipmi_smi_handlers *handlers;
 
 	if (msg->rsp_size < 10) {
 		/* Message not big enough, just ignore it. */
@@ -3188,9 +3150,8 @@ static int handle_ipmb_get_msg_cmd(ipmi_smi_t          intf,
 	}
 #endif
 		rcu_read_lock();
-		handlers = intf->handlers;
-		if (handlers) {
-			handlers->sender(intf->send_info, msg, 0);
+		if (!intf->in_shutdown) {
+			smi_send(intf, intf->handlers, msg, 0);
 			/*
 			 * We used the message, so return the value
 			 * that causes it to not be freed or
@@ -3857,32 +3818,32 @@ static void handle_new_recv_msgs(ipmi_smi_t intf)
 
 	/* See if any waiting messages need to be processed. */
 	if (!run_to_completion)
-		spin_lock_irqsave(&intf->waiting_msgs_lock, flags);
-	while (!list_empty(&intf->waiting_msgs)) {
-		smi_msg = list_entry(intf->waiting_msgs.next,
+		spin_lock_irqsave(&intf->waiting_rcv_msgs_lock, flags);
+	while (!list_empty(&intf->waiting_rcv_msgs)) {
+		smi_msg = list_entry(intf->waiting_rcv_msgs.next,
 				     struct ipmi_smi_msg, link);
-		list_del(&smi_msg->link);
 		if (!run_to_completion)
-			spin_unlock_irqrestore(&intf->waiting_msgs_lock, flags);
+			spin_unlock_irqrestore(&intf->waiting_rcv_msgs_lock,
+					       flags);
 		rv = handle_one_recv_msg(intf, smi_msg);
 		if (!run_to_completion)
-			spin_lock_irqsave(&intf->waiting_msgs_lock, flags);
-		if (rv == 0) {
-			/* Message handled */
-			ipmi_free_smi_msg(smi_msg);
-		} else if (rv < 0) {
-			/* Fatal error on the message, del but don't free. */
-		} else {
+			spin_lock_irqsave(&intf->waiting_rcv_msgs_lock, flags);
+		if (rv > 0) {
 			/*
 			 * To preserve message order, quit if we
 			 * can't handle a message.
 			 */
-			list_add(&smi_msg->link, &intf->waiting_msgs);
 			break;
+		} else {
+			list_del(&smi_msg->link);
+			if (rv == 0)
+				/* Message handled */
+				ipmi_free_smi_msg(smi_msg);
+			/* If rv < 0, fatal error, del but don't free. */
 		}
 	}
 	if (!run_to_completion)
-		spin_unlock_irqrestore(&intf->waiting_msgs_lock, flags);
+		spin_unlock_irqrestore(&intf->waiting_rcv_msgs_lock, flags);
 
 	/*
 	 * If the pretimout count is non-zero, decrement one from it and
@@ -3903,7 +3864,41 @@ static void handle_new_recv_msgs(ipmi_smi_t intf)
 
 static void smi_recv_tasklet(unsigned long val)
 {
-	handle_new_recv_msgs((ipmi_smi_t) val);
+	unsigned long flags = 0; /* keep us warning-free. */
+	ipmi_smi_t intf = (ipmi_smi_t) val;
+	int run_to_completion = intf->run_to_completion;
+	struct ipmi_smi_msg *newmsg = NULL;
+
+	/*
+	 * Start the next message if available.
+	 *
+	 * Do this here, not in the actual receiver, because we may deadlock
+	 * because the lower layer is allowed to hold locks while calling
+	 * message delivery.
+	 */
+	if (!run_to_completion)
+		spin_lock_irqsave(&intf->xmit_msgs_lock, flags);
+	if (intf->curr_msg == NULL && !intf->in_shutdown) {
+		struct list_head *entry = NULL;
+
+		/* Pick the high priority queue first. */
+		if (!list_empty(&intf->hp_xmit_msgs))
+			entry = intf->hp_xmit_msgs.next;
+		else if (!list_empty(&intf->xmit_msgs))
+			entry = intf->xmit_msgs.next;
+
+		if (entry) {
+			list_del(entry);
+			newmsg = list_entry(entry, struct ipmi_smi_msg, link);
+			intf->curr_msg = newmsg;
+		}
+	}
+	if (!run_to_completion)
+		spin_unlock_irqrestore(&intf->xmit_msgs_lock, flags);
+	if (newmsg)
+		intf->handlers->sender(intf->send_info, newmsg);
+
+	handle_new_recv_msgs(intf);
 }
 
 /* Handle a new message from the lower layer. */
@@ -3911,13 +3906,16 @@ void ipmi_smi_msg_received(ipmi_smi_t          intf,
 			   struct ipmi_smi_msg *msg)
 {
 	unsigned long flags = 0; /* keep us warning-free. */
-	int           run_to_completion;
-
+	int run_to_completion = intf->run_to_completion;
 
 	if ((msg->data_size >= 2)
 	    && (msg->data[0] == (IPMI_NETFN_APP_REQUEST << 2))
 	    && (msg->data[1] == IPMI_SEND_MSG_CMD)
 	    && (msg->user_data == NULL)) {
+
+		if (intf->in_shutdown)
+			goto free_msg;
+
 		/*
 		 * This is the local response to a command send, start
 		 * the timer for these.  The user_data will not be
@@ -3953,29 +3951,40 @@ void ipmi_smi_msg_received(ipmi_smi_t          intf,
 			/* The message was sent, start the timer. */
 			intf_start_seq_timer(intf, msg->msgid);
 
+free_msg:
 		ipmi_free_smi_msg(msg);
-		goto out;
+	} else {
+		/*
+		 * To preserve message order, we keep a queue and deliver from
+		 * a tasklet.
+		 */
+		if (!run_to_completion)
+			spin_lock_irqsave(&intf->waiting_rcv_msgs_lock, flags);
+		list_add_tail(&msg->link, &intf->waiting_rcv_msgs);
+		if (!run_to_completion)
+			spin_unlock_irqrestore(&intf->waiting_rcv_msgs_lock,
+					       flags);
 	}
 
-	/*
-	 * To preserve message order, if the list is not empty, we
-	 * tack this message onto the end of the list.
-	 */
-	run_to_completion = intf->run_to_completion;
 	if (!run_to_completion)
-		spin_lock_irqsave(&intf->waiting_msgs_lock, flags);
-	list_add_tail(&msg->link, &intf->waiting_msgs);
+		spin_lock_irqsave(&intf->xmit_msgs_lock, flags);
+	if (msg == intf->curr_msg)
+		intf->curr_msg = NULL;
 	if (!run_to_completion)
-		spin_unlock_irqrestore(&intf->waiting_msgs_lock, flags);
+		spin_unlock_irqrestore(&intf->xmit_msgs_lock, flags);
 
-	tasklet_schedule(&intf->recv_tasklet);
- out:
-	return;
+	if (run_to_completion)
+		smi_recv_tasklet((unsigned long) intf);
+	else
+		tasklet_schedule(&intf->recv_tasklet);
 }
 EXPORT_SYMBOL(ipmi_smi_msg_received);
 
 void ipmi_smi_watchdog_pretimeout(ipmi_smi_t intf)
 {
+	if (intf->in_shutdown)
+		return;
+
 	atomic_set(&intf->watchdog_pretimeouts_to_deliver, 1);
 	tasklet_schedule(&intf->recv_tasklet);
 }
@@ -4017,7 +4026,7 @@ static void check_msg_timeout(ipmi_smi_t intf, struct seq_table *ent,
 	struct ipmi_recv_msg     *msg;
 	struct ipmi_smi_handlers *handlers;
 
-	if (intf->intf_num == -1)
+	if (intf->in_shutdown)
 		return;
 
 	if (!ent->inuse)
@@ -4082,8 +4091,7 @@ static void check_msg_timeout(ipmi_smi_t intf, struct seq_table *ent,
 				ipmi_inc_stat(intf,
 					      retransmitted_ipmb_commands);
 
-			intf->handlers->sender(intf->send_info,
-					       smi_msg, 0);
+			smi_send(intf, intf->handlers, smi_msg, 0);
 		} else
 			ipmi_free_smi_msg(smi_msg);
 
@@ -4145,15 +4153,12 @@ static unsigned int ipmi_timeout_handler(ipmi_smi_t intf, long timeout_period)
 
 static void ipmi_request_event(ipmi_smi_t intf)
 {
-	struct ipmi_smi_handlers *handlers;
-
 	/* No event requests when in maintenance mode. */
 	if (intf->maintenance_mode_enable)
 		return;
 
-	handlers = intf->handlers;
-	if (handlers)
-		handlers->request_events(intf->send_info);
+	if (!intf->in_shutdown)
+		intf->handlers->request_events(intf->send_info);
 }
 
 static struct timer_list ipmi_timer;
@@ -4548,6 +4553,7 @@ static int ipmi_init_msghandler(void)
 	proc_ipmi_root = proc_mkdir("ipmi", NULL);
 	if (!proc_ipmi_root) {
 	    printk(KERN_ERR PFX "Unable to create IPMI proc dir");
+	    driver_unregister(&ipmidriver.driver);
 	    return -ENOMEM;
 	}
 
diff --git a/drivers/char/ipmi/ipmi_powernv.c b/drivers/char/ipmi/ipmi_powernv.c
new file mode 100644
index 000000000000..79524ed2a3cb
--- /dev/null
+++ b/drivers/char/ipmi/ipmi_powernv.c
@@ -0,0 +1,310 @@
+/*
+ * PowerNV OPAL IPMI driver
+ *
+ * Copyright 2014 IBM Corp.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ */
+
+#define pr_fmt(fmt)        "ipmi-powernv: " fmt
+
+#include <linux/ipmi_smi.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/of.h>
+
+#include <asm/opal.h>
+
+
+struct ipmi_smi_powernv {
+	u64			interface_id;
+	struct ipmi_device_id	ipmi_id;
+	ipmi_smi_t		intf;
+	u64			event;
+	struct notifier_block	event_nb;
+
+	/**
+	 * We assume that there can only be one outstanding request, so
+	 * keep the pending message in cur_msg. We protect this from concurrent
+	 * updates through send & recv calls, (and consequently opal_msg, which
+	 * is in-use when cur_msg is set) with msg_lock
+	 */
+	spinlock_t		msg_lock;
+	struct ipmi_smi_msg	*cur_msg;
+	struct opal_ipmi_msg	*opal_msg;
+};
+
+static int ipmi_powernv_start_processing(void *send_info, ipmi_smi_t intf)
+{
+	struct ipmi_smi_powernv *smi = send_info;
+
+	smi->intf = intf;
+	return 0;
+}
+
+static void send_error_reply(struct ipmi_smi_powernv *smi,
+		struct ipmi_smi_msg *msg, u8 completion_code)
+{
+	msg->rsp[0] = msg->data[0] | 0x4;
+	msg->rsp[1] = msg->data[1];
+	msg->rsp[2] = completion_code;
+	msg->rsp_size = 3;
+	ipmi_smi_msg_received(smi->intf, msg);
+}
+
+static void ipmi_powernv_send(void *send_info, struct ipmi_smi_msg *msg)
+{
+	struct ipmi_smi_powernv *smi = send_info;
+	struct opal_ipmi_msg *opal_msg;
+	unsigned long flags;
+	int comp, rc;
+	size_t size;
+
+	/* ensure data_len will fit in the opal_ipmi_msg buffer... */
+	if (msg->data_size > IPMI_MAX_MSG_LENGTH) {
+		comp = IPMI_REQ_LEN_EXCEEDED_ERR;
+		goto err;
+	}
+
+	/* ... and that we at least have netfn and cmd bytes */
+	if (msg->data_size < 2) {
+		comp = IPMI_REQ_LEN_INVALID_ERR;
+		goto err;
+	}
+
+	spin_lock_irqsave(&smi->msg_lock, flags);
+
+	if (smi->cur_msg) {
+		comp = IPMI_NODE_BUSY_ERR;
+		goto err_unlock;
+	}
+
+	/* format our data for the OPAL API */
+	opal_msg = smi->opal_msg;
+	opal_msg->version = OPAL_IPMI_MSG_FORMAT_VERSION_1;
+	opal_msg->netfn = msg->data[0];
+	opal_msg->cmd = msg->data[1];
+	if (msg->data_size > 2)
+		memcpy(opal_msg->data, msg->data + 2, msg->data_size - 2);
+
+	/* data_size already includes the netfn and cmd bytes */
+	size = sizeof(*opal_msg) + msg->data_size - 2;
+
+	pr_devel("%s: opal_ipmi_send(0x%llx, %p, %ld)\n", __func__,
+			smi->interface_id, opal_msg, size);
+	rc = opal_ipmi_send(smi->interface_id, opal_msg, size);
+	pr_devel("%s:  -> %d\n", __func__, rc);
+
+	if (!rc) {
+		smi->cur_msg = msg;
+		spin_unlock_irqrestore(&smi->msg_lock, flags);
+		return;
+	}
+
+	comp = IPMI_ERR_UNSPECIFIED;
+err_unlock:
+	spin_unlock_irqrestore(&smi->msg_lock, flags);
+err:
+	send_error_reply(smi, msg, comp);
+}
+
+static int ipmi_powernv_recv(struct ipmi_smi_powernv *smi)
+{
+	struct opal_ipmi_msg *opal_msg;
+	struct ipmi_smi_msg *msg;
+	unsigned long flags;
+	uint64_t size;
+	int rc;
+
+	pr_devel("%s: opal_ipmi_recv(%llx, msg, sz)\n", __func__,
+			smi->interface_id);
+
+	spin_lock_irqsave(&smi->msg_lock, flags);
+
+	if (!smi->cur_msg) {
+		pr_warn("no current message?\n");
+		return 0;
+	}
+
+	msg = smi->cur_msg;
+	opal_msg = smi->opal_msg;
+
+	size = cpu_to_be64(sizeof(*opal_msg) + IPMI_MAX_MSG_LENGTH);
+
+	rc = opal_ipmi_recv(smi->interface_id,
+			opal_msg,
+			&size);
+	size = be64_to_cpu(size);
+	pr_devel("%s:   -> %d (size %lld)\n", __func__,
+			rc, rc == 0 ? size : 0);
+	if (rc) {
+		spin_unlock_irqrestore(&smi->msg_lock, flags);
+		ipmi_free_smi_msg(msg);
+		return 0;
+	}
+
+	if (size < sizeof(*opal_msg)) {
+		spin_unlock_irqrestore(&smi->msg_lock, flags);
+		pr_warn("unexpected IPMI message size %lld\n", size);
+		return 0;
+	}
+
+	if (opal_msg->version != OPAL_IPMI_MSG_FORMAT_VERSION_1) {
+		spin_unlock_irqrestore(&smi->msg_lock, flags);
+		pr_warn("unexpected IPMI message format (version %d)\n",
+				opal_msg->version);
+		return 0;
+	}
+
+	msg->rsp[0] = opal_msg->netfn;
+	msg->rsp[1] = opal_msg->cmd;
+	if (size > sizeof(*opal_msg))
+		memcpy(&msg->rsp[2], opal_msg->data, size - sizeof(*opal_msg));
+	msg->rsp_size = 2 + size - sizeof(*opal_msg);
+
+	smi->cur_msg = NULL;
+	spin_unlock_irqrestore(&smi->msg_lock, flags);
+	ipmi_smi_msg_received(smi->intf, msg);
+	return 0;
+}
+
+static void ipmi_powernv_request_events(void *send_info)
+{
+}
+
+static void ipmi_powernv_set_run_to_completion(void *send_info,
+		bool run_to_completion)
+{
+}
+
+static void ipmi_powernv_poll(void *send_info)
+{
+	struct ipmi_smi_powernv *smi = send_info;
+
+	ipmi_powernv_recv(smi);
+}
+
+static struct ipmi_smi_handlers ipmi_powernv_smi_handlers = {
+	.owner			= THIS_MODULE,
+	.start_processing	= ipmi_powernv_start_processing,
+	.sender			= ipmi_powernv_send,
+	.request_events		= ipmi_powernv_request_events,
+	.set_run_to_completion	= ipmi_powernv_set_run_to_completion,
+	.poll			= ipmi_powernv_poll,
+};
+
+static int ipmi_opal_event(struct notifier_block *nb,
+			  unsigned long events, void *change)
+{
+	struct ipmi_smi_powernv *smi = container_of(nb,
+					struct ipmi_smi_powernv, event_nb);
+
+	if (events & smi->event)
+		ipmi_powernv_recv(smi);
+	return 0;
+}
+
+static int ipmi_powernv_probe(struct platform_device *pdev)
+{
+	struct ipmi_smi_powernv *ipmi;
+	struct device *dev;
+	u32 prop;
+	int rc;
+
+	if (!pdev || !pdev->dev.of_node)
+		return -ENODEV;
+
+	dev = &pdev->dev;
+
+	ipmi = devm_kzalloc(dev, sizeof(*ipmi), GFP_KERNEL);
+	if (!ipmi)
+		return -ENOMEM;
+
+	spin_lock_init(&ipmi->msg_lock);
+
+	rc = of_property_read_u32(dev->of_node, "ibm,ipmi-interface-id",
+			&prop);
+	if (rc) {
+		dev_warn(dev, "No interface ID property\n");
+		goto err_free;
+	}
+	ipmi->interface_id = prop;
+
+	rc = of_property_read_u32(dev->of_node, "interrupts", &prop);
+	if (rc) {
+		dev_warn(dev, "No interrupts property\n");
+		goto err_free;
+	}
+
+	ipmi->event = 1ull << prop;
+	ipmi->event_nb.notifier_call = ipmi_opal_event;
+
+	rc = opal_notifier_register(&ipmi->event_nb);
+	if (rc) {
+		dev_warn(dev, "OPAL notifier registration failed (%d)\n", rc);
+		goto err_free;
+	}
+
+	ipmi->opal_msg = devm_kmalloc(dev,
+			sizeof(*ipmi->opal_msg) + IPMI_MAX_MSG_LENGTH,
+			GFP_KERNEL);
+	if (!ipmi->opal_msg) {
+		rc = -ENOMEM;
+		goto err_unregister;
+	}
+
+	/* todo: query actual ipmi_device_id */
+	rc = ipmi_register_smi(&ipmi_powernv_smi_handlers, ipmi,
+			&ipmi->ipmi_id, dev, 0);
+	if (rc) {
+		dev_warn(dev, "IPMI SMI registration failed (%d)\n", rc);
+		goto err_free_msg;
+	}
+
+	dev_set_drvdata(dev, ipmi);
+	return 0;
+
+err_free_msg:
+	devm_kfree(dev, ipmi->opal_msg);
+err_unregister:
+	opal_notifier_unregister(&ipmi->event_nb);
+err_free:
+	devm_kfree(dev, ipmi);
+	return rc;
+}
+
+static int ipmi_powernv_remove(struct platform_device *pdev)
+{
+	struct ipmi_smi_powernv *smi = dev_get_drvdata(&pdev->dev);
+
+	ipmi_unregister_smi(smi->intf);
+	opal_notifier_unregister(&smi->event_nb);
+	return 0;
+}
+
+static const struct of_device_id ipmi_powernv_match[] = {
+	{ .compatible = "ibm,opal-ipmi" },
+	{ },
+};
+
+
+static struct platform_driver powernv_ipmi_driver = {
+	.driver = {
+		.name		= "ipmi-powernv",
+		.owner		= THIS_MODULE,
+		.of_match_table	= ipmi_powernv_match,
+	},
+	.probe	= ipmi_powernv_probe,
+	.remove	= ipmi_powernv_remove,
+};
+
+
+module_platform_driver(powernv_ipmi_driver);
+
+MODULE_DEVICE_TABLE(of, ipmi_powernv_match);
+MODULE_DESCRIPTION("powernv IPMI driver");
+MODULE_AUTHOR("Jeremy Kerr <jk@ozlabs.org>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/char/ipmi/ipmi_si_intf.c b/drivers/char/ipmi/ipmi_si_intf.c
index 5c4e1f625bbb..967b73aa4e66 100644
--- a/drivers/char/ipmi/ipmi_si_intf.c
+++ b/drivers/char/ipmi/ipmi_si_intf.c
@@ -92,12 +92,9 @@ enum si_intf_state {
 	SI_GETTING_FLAGS,
 	SI_GETTING_EVENTS,
 	SI_CLEARING_FLAGS,
-	SI_CLEARING_FLAGS_THEN_SET_IRQ,
 	SI_GETTING_MESSAGES,
-	SI_ENABLE_INTERRUPTS1,
-	SI_ENABLE_INTERRUPTS2,
-	SI_DISABLE_INTERRUPTS1,
-	SI_DISABLE_INTERRUPTS2
+	SI_CHECKING_ENABLES,
+	SI_SETTING_ENABLES
 	/* FIXME - add watchdog stuff. */
 };
 
@@ -111,10 +108,6 @@ enum si_type {
 };
 static char *si_to_str[] = { "kcs", "smic", "bt" };
 
-static char *ipmi_addr_src_to_str[] = { NULL, "hotmod", "hardcoded", "SPMI",
-					"ACPI", "SMBIOS", "PCI",
-					"device-tree", "default" };
-
 #define DEVICE_NAME "ipmi_si"
 
 static struct platform_driver ipmi_driver;
@@ -174,8 +167,7 @@ struct smi_info {
 	struct si_sm_handlers  *handlers;
 	enum si_type           si_type;
 	spinlock_t             si_lock;
-	struct list_head       xmit_msgs;
-	struct list_head       hp_xmit_msgs;
+	struct ipmi_smi_msg    *waiting_msg;
 	struct ipmi_smi_msg    *curr_msg;
 	enum si_intf_state     si_state;
 
@@ -254,9 +246,6 @@ struct smi_info {
 	/* The time (in jiffies) the last timeout occurred at. */
 	unsigned long       last_timeout_jiffies;
 
-	/* Used to gracefully stop the timer without race conditions. */
-	atomic_t            stop_operation;
-
 	/* Are we waiting for the events, pretimeouts, received msgs? */
 	atomic_t            need_watch;
 
@@ -268,6 +257,16 @@ struct smi_info {
 	 */
 	bool interrupt_disabled;
 
+	/*
+	 * Does the BMC support events?
+	 */
+	bool supports_event_msg_buff;
+
+	/*
+	 * Did we get an attention that we did not handle?
+	 */
+	bool got_attn;
+
 	/* From the get device id response... */
 	struct ipmi_device_id device_id;
 
@@ -332,7 +331,10 @@ static void deliver_recv_msg(struct smi_info *smi_info,
 			     struct ipmi_smi_msg *msg)
 {
 	/* Deliver the message to the upper layer. */
-	ipmi_smi_msg_received(smi_info->intf, msg);
+	if (smi_info->intf)
+		ipmi_smi_msg_received(smi_info->intf, msg);
+	else
+		ipmi_free_smi_msg(msg);
 }
 
 static void return_hosed_msg(struct smi_info *smi_info, int cCode)
@@ -356,28 +358,18 @@ static void return_hosed_msg(struct smi_info *smi_info, int cCode)
 static enum si_sm_result start_next_msg(struct smi_info *smi_info)
 {
 	int              rv;
-	struct list_head *entry = NULL;
 #ifdef DEBUG_TIMING
 	struct timeval t;
 #endif
 
-	/* Pick the high priority queue first. */
-	if (!list_empty(&(smi_info->hp_xmit_msgs))) {
-		entry = smi_info->hp_xmit_msgs.next;
-	} else if (!list_empty(&(smi_info->xmit_msgs))) {
-		entry = smi_info->xmit_msgs.next;
-	}
-
-	if (!entry) {
+	if (!smi_info->waiting_msg) {
 		smi_info->curr_msg = NULL;
 		rv = SI_SM_IDLE;
 	} else {
 		int err;
 
-		list_del(entry);
-		smi_info->curr_msg = list_entry(entry,
-						struct ipmi_smi_msg,
-						link);
+		smi_info->curr_msg = smi_info->waiting_msg;
+		smi_info->waiting_msg = NULL;
 #ifdef DEBUG_TIMING
 		do_gettimeofday(&t);
 		printk(KERN_DEBUG "**Start2: %d.%9.9d\n", t.tv_sec, t.tv_usec);
@@ -401,30 +393,15 @@ static enum si_sm_result start_next_msg(struct smi_info *smi_info)
 	return rv;
 }
 
-static void start_enable_irq(struct smi_info *smi_info)
+static void start_check_enables(struct smi_info *smi_info)
 {
 	unsigned char msg[2];
 
-	/*
-	 * If we are enabling interrupts, we have to tell the
-	 * BMC to use them.
-	 */
 	msg[0] = (IPMI_NETFN_APP_REQUEST << 2);
 	msg[1] = IPMI_GET_BMC_GLOBAL_ENABLES_CMD;
 
 	smi_info->handlers->start_transaction(smi_info->si_sm, msg, 2);
-	smi_info->si_state = SI_ENABLE_INTERRUPTS1;
-}
-
-static void start_disable_irq(struct smi_info *smi_info)
-{
-	unsigned char msg[2];
-
-	msg[0] = (IPMI_NETFN_APP_REQUEST << 2);
-	msg[1] = IPMI_GET_BMC_GLOBAL_ENABLES_CMD;
-
-	smi_info->handlers->start_transaction(smi_info->si_sm, msg, 2);
-	smi_info->si_state = SI_DISABLE_INTERRUPTS1;
+	smi_info->si_state = SI_CHECKING_ENABLES;
 }
 
 static void start_clear_flags(struct smi_info *smi_info)
@@ -440,6 +417,32 @@ static void start_clear_flags(struct smi_info *smi_info)
 	smi_info->si_state = SI_CLEARING_FLAGS;
 }
 
+static void start_getting_msg_queue(struct smi_info *smi_info)
+{
+	smi_info->curr_msg->data[0] = (IPMI_NETFN_APP_REQUEST << 2);
+	smi_info->curr_msg->data[1] = IPMI_GET_MSG_CMD;
+	smi_info->curr_msg->data_size = 2;
+
+	smi_info->handlers->start_transaction(
+		smi_info->si_sm,
+		smi_info->curr_msg->data,
+		smi_info->curr_msg->data_size);
+	smi_info->si_state = SI_GETTING_MESSAGES;
+}
+
+static void start_getting_events(struct smi_info *smi_info)
+{
+	smi_info->curr_msg->data[0] = (IPMI_NETFN_APP_REQUEST << 2);
+	smi_info->curr_msg->data[1] = IPMI_READ_EVENT_MSG_BUFFER_CMD;
+	smi_info->curr_msg->data_size = 2;
+
+	smi_info->handlers->start_transaction(
+		smi_info->si_sm,
+		smi_info->curr_msg->data,
+		smi_info->curr_msg->data_size);
+	smi_info->si_state = SI_GETTING_EVENTS;
+}
+
 static void smi_mod_timer(struct smi_info *smi_info, unsigned long new_val)
 {
 	smi_info->last_timeout_jiffies = jiffies;
@@ -453,22 +456,45 @@ static void smi_mod_timer(struct smi_info *smi_info, unsigned long new_val)
  * polled until we can allocate some memory.  Once we have some
  * memory, we will re-enable the interrupt.
  */
-static inline void disable_si_irq(struct smi_info *smi_info)
+static inline bool disable_si_irq(struct smi_info *smi_info)
 {
 	if ((smi_info->irq) && (!smi_info->interrupt_disabled)) {
-		start_disable_irq(smi_info);
 		smi_info->interrupt_disabled = true;
-		if (!atomic_read(&smi_info->stop_operation))
-			smi_mod_timer(smi_info, jiffies + SI_TIMEOUT_JIFFIES);
+		start_check_enables(smi_info);
+		return true;
 	}
+	return false;
 }
 
-static inline void enable_si_irq(struct smi_info *smi_info)
+static inline bool enable_si_irq(struct smi_info *smi_info)
 {
 	if ((smi_info->irq) && (smi_info->interrupt_disabled)) {
-		start_enable_irq(smi_info);
 		smi_info->interrupt_disabled = false;
+		start_check_enables(smi_info);
+		return true;
 	}
+	return false;
+}
+
+/*
+ * Allocate a message.  If unable to allocate, start the interrupt
+ * disable process and return NULL.  If able to allocate but
+ * interrupts are disabled, free the message and return NULL after
+ * starting the interrupt enable process.
+ */
+static struct ipmi_smi_msg *alloc_msg_handle_irq(struct smi_info *smi_info)
+{
+	struct ipmi_smi_msg *msg;
+
+	msg = ipmi_alloc_smi_msg();
+	if (!msg) {
+		if (!disable_si_irq(smi_info))
+			smi_info->si_state = SI_NORMAL;
+	} else if (enable_si_irq(smi_info)) {
+		ipmi_free_smi_msg(msg);
+		msg = NULL;
+	}
+	return msg;
 }
 
 static void handle_flags(struct smi_info *smi_info)
@@ -480,45 +506,22 @@ static void handle_flags(struct smi_info *smi_info)
 
 		start_clear_flags(smi_info);
 		smi_info->msg_flags &= ~WDT_PRE_TIMEOUT_INT;
-		ipmi_smi_watchdog_pretimeout(smi_info->intf);
+		if (smi_info->intf)
+			ipmi_smi_watchdog_pretimeout(smi_info->intf);
 	} else if (smi_info->msg_flags & RECEIVE_MSG_AVAIL) {
 		/* Messages available. */
-		smi_info->curr_msg = ipmi_alloc_smi_msg();
-		if (!smi_info->curr_msg) {
-			disable_si_irq(smi_info);
-			smi_info->si_state = SI_NORMAL;
+		smi_info->curr_msg = alloc_msg_handle_irq(smi_info);
+		if (!smi_info->curr_msg)
 			return;
-		}
-		enable_si_irq(smi_info);
-
-		smi_info->curr_msg->data[0] = (IPMI_NETFN_APP_REQUEST << 2);
-		smi_info->curr_msg->data[1] = IPMI_GET_MSG_CMD;
-		smi_info->curr_msg->data_size = 2;
 
-		smi_info->handlers->start_transaction(
-			smi_info->si_sm,
-			smi_info->curr_msg->data,
-			smi_info->curr_msg->data_size);
-		smi_info->si_state = SI_GETTING_MESSAGES;
+		start_getting_msg_queue(smi_info);
 	} else if (smi_info->msg_flags & EVENT_MSG_BUFFER_FULL) {
 		/* Events available. */
-		smi_info->curr_msg = ipmi_alloc_smi_msg();
-		if (!smi_info->curr_msg) {
-			disable_si_irq(smi_info);
-			smi_info->si_state = SI_NORMAL;
+		smi_info->curr_msg = alloc_msg_handle_irq(smi_info);
+		if (!smi_info->curr_msg)
 			return;
-		}
-		enable_si_irq(smi_info);
-
-		smi_info->curr_msg->data[0] = (IPMI_NETFN_APP_REQUEST << 2);
-		smi_info->curr_msg->data[1] = IPMI_READ_EVENT_MSG_BUFFER_CMD;
-		smi_info->curr_msg->data_size = 2;
 
-		smi_info->handlers->start_transaction(
-			smi_info->si_sm,
-			smi_info->curr_msg->data,
-			smi_info->curr_msg->data_size);
-		smi_info->si_state = SI_GETTING_EVENTS;
+		start_getting_events(smi_info);
 	} else if (smi_info->msg_flags & OEM_DATA_AVAIL &&
 		   smi_info->oem_data_avail_handler) {
 		if (smi_info->oem_data_avail_handler(smi_info))
@@ -527,6 +530,55 @@ static void handle_flags(struct smi_info *smi_info)
 		smi_info->si_state = SI_NORMAL;
 }
 
+/*
+ * Global enables we care about.
+ */
+#define GLOBAL_ENABLES_MASK (IPMI_BMC_EVT_MSG_BUFF | IPMI_BMC_RCV_MSG_INTR | \
+			     IPMI_BMC_EVT_MSG_INTR)
+
+static u8 current_global_enables(struct smi_info *smi_info, u8 base,
+				 bool *irq_on)
+{
+	u8 enables = 0;
+
+	if (smi_info->supports_event_msg_buff)
+		enables |= IPMI_BMC_EVT_MSG_BUFF;
+	else
+		enables &= ~IPMI_BMC_EVT_MSG_BUFF;
+
+	if (smi_info->irq && !smi_info->interrupt_disabled)
+		enables |= IPMI_BMC_RCV_MSG_INTR;
+	else
+		enables &= ~IPMI_BMC_RCV_MSG_INTR;
+
+	if (smi_info->supports_event_msg_buff &&
+	    smi_info->irq && !smi_info->interrupt_disabled)
+
+		enables |= IPMI_BMC_EVT_MSG_INTR;
+	else
+		enables &= ~IPMI_BMC_EVT_MSG_INTR;
+
+	*irq_on = enables & (IPMI_BMC_EVT_MSG_INTR | IPMI_BMC_RCV_MSG_INTR);
+
+	return enables;
+}
+
+static void check_bt_irq(struct smi_info *smi_info, bool irq_on)
+{
+	u8 irqstate = smi_info->io.inputb(&smi_info->io, IPMI_BT_INTMASK_REG);
+
+	irqstate &= IPMI_BT_INTMASK_ENABLE_IRQ_BIT;
+
+	if ((bool)irqstate == irq_on)
+		return;
+
+	if (irq_on)
+		smi_info->io.outputb(&smi_info->io, IPMI_BT_INTMASK_REG,
+				     IPMI_BT_INTMASK_ENABLE_IRQ_BIT);
+	else
+		smi_info->io.outputb(&smi_info->io, IPMI_BT_INTMASK_REG, 0);
+}
+
 static void handle_transaction_done(struct smi_info *smi_info)
 {
 	struct ipmi_smi_msg *msg;
@@ -581,7 +633,6 @@ static void handle_transaction_done(struct smi_info *smi_info)
 	}
 
 	case SI_CLEARING_FLAGS:
-	case SI_CLEARING_FLAGS_THEN_SET_IRQ:
 	{
 		unsigned char msg[3];
 
@@ -592,10 +643,7 @@ static void handle_transaction_done(struct smi_info *smi_info)
 			dev_warn(smi_info->dev,
 				 "Error clearing flags: %2.2x\n", msg[2]);
 		}
-		if (smi_info->si_state == SI_CLEARING_FLAGS_THEN_SET_IRQ)
-			start_enable_irq(smi_info);
-		else
-			smi_info->si_state = SI_NORMAL;
+		smi_info->si_state = SI_NORMAL;
 		break;
 	}
 
@@ -675,9 +723,11 @@ static void handle_transaction_done(struct smi_info *smi_info)
 		break;
 	}
 
-	case SI_ENABLE_INTERRUPTS1:
+	case SI_CHECKING_ENABLES:
 	{
 		unsigned char msg[4];
+		u8 enables;
+		bool irq_on;
 
 		/* We got the flags from the SMI, now handle them. */
 		smi_info->handlers->get_result(smi_info->si_sm, msg, 4);
@@ -687,70 +737,53 @@ static void handle_transaction_done(struct smi_info *smi_info)
 			dev_warn(smi_info->dev,
 				 "Maybe ok, but ipmi might run very slowly.\n");
 			smi_info->si_state = SI_NORMAL;
-		} else {
+			break;
+		}
+		enables = current_global_enables(smi_info, 0, &irq_on);
+		if (smi_info->si_type == SI_BT)
+			/* BT has its own interrupt enable bit. */
+			check_bt_irq(smi_info, irq_on);
+		if (enables != (msg[3] & GLOBAL_ENABLES_MASK)) {
+			/* Enables are not correct, fix them. */
 			msg[0] = (IPMI_NETFN_APP_REQUEST << 2);
 			msg[1] = IPMI_SET_BMC_GLOBAL_ENABLES_CMD;
-			msg[2] = (msg[3] |
-				  IPMI_BMC_RCV_MSG_INTR |
-				  IPMI_BMC_EVT_MSG_INTR);
+			msg[2] = enables | (msg[3] & ~GLOBAL_ENABLES_MASK);
 			smi_info->handlers->start_transaction(
 				smi_info->si_sm, msg, 3);
-			smi_info->si_state = SI_ENABLE_INTERRUPTS2;
+			smi_info->si_state = SI_SETTING_ENABLES;
+		} else if (smi_info->supports_event_msg_buff) {
+			smi_info->curr_msg = ipmi_alloc_smi_msg();
+			if (!smi_info->curr_msg) {
+				smi_info->si_state = SI_NORMAL;
+				break;
+			}
+			start_getting_msg_queue(smi_info);
+		} else {
+			smi_info->si_state = SI_NORMAL;
 		}
 		break;
 	}
 
-	case SI_ENABLE_INTERRUPTS2:
+	case SI_SETTING_ENABLES:
 	{
 		unsigned char msg[4];
 
-		/* We got the flags from the SMI, now handle them. */
 		smi_info->handlers->get_result(smi_info->si_sm, msg, 4);
-		if (msg[2] != 0) {
+		if (msg[2] != 0)
 			dev_warn(smi_info->dev,
-				 "Couldn't set irq info: %x.\n", msg[2]);
-			dev_warn(smi_info->dev,
-				 "Maybe ok, but ipmi might run very slowly.\n");
-		} else
-			smi_info->interrupt_disabled = false;
-		smi_info->si_state = SI_NORMAL;
-		break;
-	}
-
-	case SI_DISABLE_INTERRUPTS1:
-	{
-		unsigned char msg[4];
+				 "Could not set the global enables: 0x%x.\n",
+				 msg[2]);
 
-		/* We got the flags from the SMI, now handle them. */
-		smi_info->handlers->get_result(smi_info->si_sm, msg, 4);
-		if (msg[2] != 0) {
-			dev_warn(smi_info->dev, "Could not disable interrupts"
-				 ", failed get.\n");
-			smi_info->si_state = SI_NORMAL;
+		if (smi_info->supports_event_msg_buff) {
+			smi_info->curr_msg = ipmi_alloc_smi_msg();
+			if (!smi_info->curr_msg) {
+				smi_info->si_state = SI_NORMAL;
+				break;
+			}
+			start_getting_msg_queue(smi_info);
 		} else {
-			msg[0] = (IPMI_NETFN_APP_REQUEST << 2);
-			msg[1] = IPMI_SET_BMC_GLOBAL_ENABLES_CMD;
-			msg[2] = (msg[3] &
-				  ~(IPMI_BMC_RCV_MSG_INTR |
-				    IPMI_BMC_EVT_MSG_INTR));
-			smi_info->handlers->start_transaction(
-				smi_info->si_sm, msg, 3);
-			smi_info->si_state = SI_DISABLE_INTERRUPTS2;
-		}
-		break;
-	}
-
-	case SI_DISABLE_INTERRUPTS2:
-	{
-		unsigned char msg[4];
-
-		/* We got the flags from the SMI, now handle them. */
-		smi_info->handlers->get_result(smi_info->si_sm, msg, 4);
-		if (msg[2] != 0) {
-			dev_warn(smi_info->dev, "Could not disable interrupts"
-				 ", failed set.\n");
+			smi_info->si_state = SI_NORMAL;
 		}
-		smi_info->si_state = SI_NORMAL;
 		break;
 	}
 	}
@@ -808,25 +841,35 @@ static enum si_sm_result smi_event_handler(struct smi_info *smi_info,
 	 * We prefer handling attn over new messages.  But don't do
 	 * this if there is not yet an upper layer to handle anything.
 	 */
-	if (likely(smi_info->intf) && si_sm_result == SI_SM_ATTN) {
+	if (likely(smi_info->intf) &&
+	    (si_sm_result == SI_SM_ATTN || smi_info->got_attn)) {
 		unsigned char msg[2];
 
-		smi_inc_stat(smi_info, attentions);
+		if (smi_info->si_state != SI_NORMAL) {
+			/*
+			 * We got an ATTN, but we are doing something else.
+			 * Handle the ATTN later.
+			 */
+			smi_info->got_attn = true;
+		} else {
+			smi_info->got_attn = false;
+			smi_inc_stat(smi_info, attentions);
 
-		/*
-		 * Got a attn, send down a get message flags to see
-		 * what's causing it.  It would be better to handle
-		 * this in the upper layer, but due to the way
-		 * interrupts work with the SMI, that's not really
-		 * possible.
-		 */
-		msg[0] = (IPMI_NETFN_APP_REQUEST << 2);
-		msg[1] = IPMI_GET_MSG_FLAGS_CMD;
+			/*
+			 * Got a attn, send down a get message flags to see
+			 * what's causing it.  It would be better to handle
+			 * this in the upper layer, but due to the way
+			 * interrupts work with the SMI, that's not really
+			 * possible.
+			 */
+			msg[0] = (IPMI_NETFN_APP_REQUEST << 2);
+			msg[1] = IPMI_GET_MSG_FLAGS_CMD;
 
-		smi_info->handlers->start_transaction(
-			smi_info->si_sm, msg, 2);
-		smi_info->si_state = SI_GETTING_FLAGS;
-		goto restart;
+			smi_info->handlers->start_transaction(
+				smi_info->si_sm, msg, 2);
+			smi_info->si_state = SI_GETTING_FLAGS;
+			goto restart;
+		}
 	}
 
 	/* If we are currently idle, try to start the next message. */
@@ -846,19 +889,21 @@ static enum si_sm_result smi_event_handler(struct smi_info *smi_info,
 		 */
 		atomic_set(&smi_info->req_events, 0);
 
-		smi_info->curr_msg = ipmi_alloc_smi_msg();
-		if (!smi_info->curr_msg)
-			goto out;
-
-		smi_info->curr_msg->data[0] = (IPMI_NETFN_APP_REQUEST << 2);
-		smi_info->curr_msg->data[1] = IPMI_READ_EVENT_MSG_BUFFER_CMD;
-		smi_info->curr_msg->data_size = 2;
+		/*
+		 * Take this opportunity to check the interrupt and
+		 * message enable state for the BMC.  The BMC can be
+		 * asynchronously reset, and may thus get interrupts
+		 * disable and messages disabled.
+		 */
+		if (smi_info->supports_event_msg_buff || smi_info->irq) {
+			start_check_enables(smi_info);
+		} else {
+			smi_info->curr_msg = alloc_msg_handle_irq(smi_info);
+			if (!smi_info->curr_msg)
+				goto out;
 
-		smi_info->handlers->start_transaction(
-			smi_info->si_sm,
-			smi_info->curr_msg->data,
-			smi_info->curr_msg->data_size);
-		smi_info->si_state = SI_GETTING_EVENTS;
+			start_getting_events(smi_info);
+		}
 		goto restart;
 	}
  out:
@@ -879,8 +924,7 @@ static void check_start_timer_thread(struct smi_info *smi_info)
 }
 
 static void sender(void                *send_info,
-		   struct ipmi_smi_msg *msg,
-		   int                 priority)
+		   struct ipmi_smi_msg *msg)
 {
 	struct smi_info   *smi_info = send_info;
 	enum si_sm_result result;
@@ -889,14 +933,8 @@ static void sender(void                *send_info,
 	struct timeval    t;
 #endif
 
-	if (atomic_read(&smi_info->stop_operation)) {
-		msg->rsp[0] = msg->data[0] | 4;
-		msg->rsp[1] = msg->data[1];
-		msg->rsp[2] = IPMI_ERR_UNSPECIFIED;
-		msg->rsp_size = 3;
-		deliver_recv_msg(smi_info, msg);
-		return;
-	}
+	BUG_ON(smi_info->waiting_msg);
+	smi_info->waiting_msg = msg;
 
 #ifdef DEBUG_TIMING
 	do_gettimeofday(&t);
@@ -905,16 +943,16 @@ static void sender(void                *send_info,
 
 	if (smi_info->run_to_completion) {
 		/*
-		 * If we are running to completion, then throw it in
-		 * the list and run transactions until everything is
-		 * clear.  Priority doesn't matter here.
+		 * If we are running to completion, start it and run
+		 * transactions until everything is clear.
 		 */
+		smi_info->curr_msg = smi_info->waiting_msg;
+		smi_info->waiting_msg = NULL;
 
 		/*
 		 * Run to completion means we are single-threaded, no
 		 * need for locks.
 		 */
-		list_add_tail(&(msg->link), &(smi_info->xmit_msgs));
 
 		result = smi_event_handler(smi_info, 0);
 		while (result != SI_SM_IDLE) {
@@ -926,11 +964,6 @@ static void sender(void                *send_info,
 	}
 
 	spin_lock_irqsave(&smi_info->si_lock, flags);
-	if (priority > 0)
-		list_add_tail(&msg->link, &smi_info->hp_xmit_msgs);
-	else
-		list_add_tail(&msg->link, &smi_info->xmit_msgs);
-
 	check_start_timer_thread(smi_info);
 	spin_unlock_irqrestore(&smi_info->si_lock, flags);
 }
@@ -1068,8 +1101,7 @@ static void request_events(void *send_info)
 {
 	struct smi_info *smi_info = send_info;
 
-	if (atomic_read(&smi_info->stop_operation) ||
-				!smi_info->has_event_buffer)
+	if (!smi_info->has_event_buffer)
 		return;
 
 	atomic_set(&smi_info->req_events, 1);
@@ -1697,7 +1729,7 @@ static int parse_str(struct hotmod_vals *v, int *val, char *name, char **curr)
 	}
 	*s = '\0';
 	s++;
-	for (i = 0; hotmod_ops[i].name; i++) {
+	for (i = 0; v[i].name; i++) {
 		if (strcmp(*curr, v[i].name) == 0) {
 			*val = v[i].val;
 			*curr = s;
@@ -2133,6 +2165,9 @@ static int try_init_spmi(struct SPMITable *spmi)
 	case 3:	/* BT */
 		info->si_type = SI_BT;
 		break;
+	case 4: /* SSIF, just ignore */
+		kfree(info);
+		return -EIO;
 	default:
 		printk(KERN_INFO PFX "Unknown ACPI/SPMI SI type %d\n",
 		       spmi->InterfaceType);
@@ -2250,6 +2285,8 @@ static int ipmi_pnp_probe(struct pnp_dev *dev,
 	case 3:
 		info->si_type = SI_BT;
 		break;
+	case 4: /* SSIF, just ignore */
+		goto err_free;
 	default:
 		dev_info(&dev->dev, "unknown IPMI type %lld\n", tmp);
 		goto err_free;
@@ -2751,7 +2788,6 @@ static struct of_device_id ipmi_match[] =
 static struct platform_driver ipmi_driver = {
 	.driver = {
 		.name = DEVICE_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = ipmi_match,
 	},
 	.probe		= ipmi_probe,
@@ -2913,9 +2949,11 @@ static int try_enable_event_buffer(struct smi_info *smi_info)
 		goto out;
 	}
 
-	if (resp[3] & IPMI_BMC_EVT_MSG_BUFF)
+	if (resp[3] & IPMI_BMC_EVT_MSG_BUFF) {
 		/* buffer is already enabled, nothing to do. */
+		smi_info->supports_event_msg_buff = true;
 		goto out;
+	}
 
 	msg[0] = IPMI_NETFN_APP_REQUEST << 2;
 	msg[1] = IPMI_SET_BMC_GLOBAL_ENABLES_CMD;
@@ -2948,6 +2986,9 @@ static int try_enable_event_buffer(struct smi_info *smi_info)
 		 * that the event buffer is not supported.
 		 */
 		rv = -ENOENT;
+	else
+		smi_info->supports_event_msg_buff = true;
+
  out:
 	kfree(resp);
 	return rv;
@@ -3188,15 +3229,10 @@ static void setup_xaction_handlers(struct smi_info *smi_info)
 
 static inline void wait_for_timer_and_thread(struct smi_info *smi_info)
 {
-	if (smi_info->intf) {
-		/*
-		 * The timer and thread are only running if the
-		 * interface has been started up and registered.
-		 */
-		if (smi_info->thread != NULL)
-			kthread_stop(smi_info->thread);
+	if (smi_info->thread != NULL)
+		kthread_stop(smi_info->thread);
+	if (smi_info->timer_running)
 		del_timer_sync(&smi_info->si_timer);
-	}
 }
 
 static struct ipmi_default_vals
@@ -3274,8 +3310,8 @@ static int add_smi(struct smi_info *new_smi)
 	int rv = 0;
 
 	printk(KERN_INFO PFX "Adding %s-specified %s state machine",
-			ipmi_addr_src_to_str[new_smi->addr_source],
-			si_to_str[new_smi->si_type]);
+	       ipmi_addr_src_to_str(new_smi->addr_source),
+	       si_to_str[new_smi->si_type]);
 	mutex_lock(&smi_infos_lock);
 	if (!is_new_interface(new_smi)) {
 		printk(KERN_CONT " duplicate interface\n");
@@ -3305,7 +3341,7 @@ static int try_smi_init(struct smi_info *new_smi)
 	printk(KERN_INFO PFX "Trying %s-specified %s state"
 	       " machine at %s address 0x%lx, slave address 0x%x,"
 	       " irq %d\n",
-	       ipmi_addr_src_to_str[new_smi->addr_source],
+	       ipmi_addr_src_to_str(new_smi->addr_source),
 	       si_to_str[new_smi->si_type],
 	       addr_space_to_str[new_smi->io.addr_type],
 	       new_smi->io.addr_data,
@@ -3371,8 +3407,7 @@ static int try_smi_init(struct smi_info *new_smi)
 	setup_oem_data_handler(new_smi);
 	setup_xaction_handlers(new_smi);
 
-	INIT_LIST_HEAD(&(new_smi->xmit_msgs));
-	INIT_LIST_HEAD(&(new_smi->hp_xmit_msgs));
+	new_smi->waiting_msg = NULL;
 	new_smi->curr_msg = NULL;
 	atomic_set(&new_smi->req_events, 0);
 	new_smi->run_to_completion = false;
@@ -3380,7 +3415,6 @@ static int try_smi_init(struct smi_info *new_smi)
 		atomic_set(&new_smi->stats[i], 0);
 
 	new_smi->interrupt_disabled = true;
-	atomic_set(&new_smi->stop_operation, 0);
 	atomic_set(&new_smi->need_watch, 0);
 	new_smi->intf_num = smi_num;
 	smi_num++;
@@ -3394,9 +3428,15 @@ static int try_smi_init(struct smi_info *new_smi)
 	 * timer to avoid racing with the timer.
 	 */
 	start_clear_flags(new_smi);
-	/* IRQ is defined to be set when non-zero. */
-	if (new_smi->irq)
-		new_smi->si_state = SI_CLEARING_FLAGS_THEN_SET_IRQ;
+
+	/*
+	 * IRQ is defined to be set when non-zero.  req_events will
+	 * cause a global flags check that will enable interrupts.
+	 */
+	if (new_smi->irq) {
+		new_smi->interrupt_disabled = false;
+		atomic_set(&new_smi->req_events, 1);
+	}
 
 	if (!new_smi->dev) {
 		/*
@@ -3428,7 +3468,6 @@ static int try_smi_init(struct smi_info *new_smi)
 			       new_smi,
 			       &new_smi->device_id,
 			       new_smi->dev,
-			       "bmc",
 			       new_smi->slave_addr);
 	if (rv) {
 		dev_err(new_smi->dev, "Unable to register device: error %d\n",
@@ -3466,15 +3505,15 @@ static int try_smi_init(struct smi_info *new_smi)
 	return 0;
 
  out_err_stop_timer:
-	atomic_inc(&new_smi->stop_operation);
 	wait_for_timer_and_thread(new_smi);
 
  out_err:
 	new_smi->interrupt_disabled = true;
 
 	if (new_smi->intf) {
-		ipmi_unregister_smi(new_smi->intf);
+		ipmi_smi_t intf = new_smi->intf;
 		new_smi->intf = NULL;
+		ipmi_unregister_smi(intf);
 	}
 
 	if (new_smi->irq_cleanup) {
@@ -3653,60 +3692,49 @@ module_init(init_ipmi_si);
 static void cleanup_one_si(struct smi_info *to_clean)
 {
 	int           rv = 0;
-	unsigned long flags;
 
 	if (!to_clean)
 		return;
 
+	if (to_clean->intf) {
+		ipmi_smi_t intf = to_clean->intf;
+
+		to_clean->intf = NULL;
+		rv = ipmi_unregister_smi(intf);
+		if (rv) {
+			pr_err(PFX "Unable to unregister device: errno=%d\n",
+			       rv);
+		}
+	}
+
 	if (to_clean->dev)
 		dev_set_drvdata(to_clean->dev, NULL);
 
 	list_del(&to_clean->link);
 
-	/* Tell the driver that we are shutting down. */
-	atomic_inc(&to_clean->stop_operation);
-
 	/*
-	 * Make sure the timer and thread are stopped and will not run
-	 * again.
+	 * Make sure that interrupts, the timer and the thread are
+	 * stopped and will not run again.
 	 */
+	if (to_clean->irq_cleanup)
+		to_clean->irq_cleanup(to_clean);
 	wait_for_timer_and_thread(to_clean);
 
 	/*
 	 * Timeouts are stopped, now make sure the interrupts are off
-	 * for the device.  A little tricky with locks to make sure
-	 * there are no races.
+	 * in the BMC.  Note that timers and CPU interrupts are off,
+	 * so no need for locks.
 	 */
-	spin_lock_irqsave(&to_clean->si_lock, flags);
 	while (to_clean->curr_msg || (to_clean->si_state != SI_NORMAL)) {
-		spin_unlock_irqrestore(&to_clean->si_lock, flags);
 		poll(to_clean);
 		schedule_timeout_uninterruptible(1);
-		spin_lock_irqsave(&to_clean->si_lock, flags);
 	}
 	disable_si_irq(to_clean);
-	spin_unlock_irqrestore(&to_clean->si_lock, flags);
 	while (to_clean->curr_msg || (to_clean->si_state != SI_NORMAL)) {
 		poll(to_clean);
 		schedule_timeout_uninterruptible(1);
 	}
 
-	/* Clean up interrupts and make sure that everything is done. */
-	if (to_clean->irq_cleanup)
-		to_clean->irq_cleanup(to_clean);
-	while (to_clean->curr_msg || (to_clean->si_state != SI_NORMAL)) {
-		poll(to_clean);
-		schedule_timeout_uninterruptible(1);
-	}
-
-	if (to_clean->intf)
-		rv = ipmi_unregister_smi(to_clean->intf);
-
-	if (rv) {
-		printk(KERN_ERR PFX "Unable to unregister device: errno=%d\n",
-		       rv);
-	}
-
 	if (to_clean->handlers)
 		to_clean->handlers->cleanup(to_clean->si_sm);
 
diff --git a/drivers/char/ipmi/ipmi_ssif.c b/drivers/char/ipmi/ipmi_ssif.c
new file mode 100644
index 000000000000..fd5a5e85d7dc
--- /dev/null
+++ b/drivers/char/ipmi/ipmi_ssif.c
@@ -0,0 +1,1871 @@
+/*
+ * ipmi_ssif.c
+ *
+ * The interface to the IPMI driver for SMBus access to a SMBus
+ * compliant device.  Called SSIF by the IPMI spec.
+ *
+ * Author: Intel Corporation
+ *         Todd Davis <todd.c.davis@intel.com>
+ *
+ * Rewritten by Corey Minyard <minyard@acm.org> to support the
+ * non-blocking I2C interface, add support for multi-part
+ * transactions, add PEC support, and general clenaup.
+ *
+ * Copyright 2003 Intel Corporation
+ * Copyright 2005 MontaVista Software
+ *
+ *  This program is free software; you can redistribute it and/or modify it
+ *  under the terms of the GNU General Public License as published by the
+ *  Free Software Foundation; either version 2 of the License, or (at your
+ *  option) any later version.
+ */
+
+/*
+ * This file holds the "policy" for the interface to the SSIF state
+ * machine.  It does the configuration, handles timers and interrupts,
+ * and drives the real SSIF state machine.
+ */
+
+/*
+ * TODO: Figure out how to use SMB alerts.  This will require a new
+ * interface into the I2C driver, I believe.
+ */
+
+#include <linux/version.h>
+#if defined(MODVERSIONS)
+#include <linux/modversions.h>
+#endif
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/sched.h>
+#include <linux/seq_file.h>
+#include <linux/timer.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/spinlock.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <linux/i2c.h>
+#include <linux/ipmi_smi.h>
+#include <linux/init.h>
+#include <linux/dmi.h>
+#include <linux/kthread.h>
+#include <linux/acpi.h>
+#include <linux/ctype.h>
+
+#define PFX "ipmi_ssif: "
+#define DEVICE_NAME "ipmi_ssif"
+
+#define IPMI_GET_SYSTEM_INTERFACE_CAPABILITIES_CMD	0x57
+
+#define	SSIF_IPMI_REQUEST			2
+#define	SSIF_IPMI_MULTI_PART_REQUEST_START	6
+#define	SSIF_IPMI_MULTI_PART_REQUEST_MIDDLE	7
+#define	SSIF_IPMI_RESPONSE			3
+#define	SSIF_IPMI_MULTI_PART_RESPONSE_MIDDLE	9
+
+/* ssif_debug is a bit-field
+ *	SSIF_DEBUG_MSG -	commands and their responses
+ *	SSIF_DEBUG_STATES -	message states
+ *	SSIF_DEBUG_TIMING -	 Measure times between events in the driver
+ */
+#define SSIF_DEBUG_TIMING	4
+#define SSIF_DEBUG_STATE	2
+#define SSIF_DEBUG_MSG		1
+#define SSIF_NODEBUG		0
+#define SSIF_DEFAULT_DEBUG	(SSIF_NODEBUG)
+
+/*
+ * Timer values
+ */
+#define SSIF_MSG_USEC		20000	/* 20ms between message tries. */
+#define SSIF_MSG_PART_USEC	5000	/* 5ms for a message part */
+
+/* How many times to we retry sending/receiving the message. */
+#define	SSIF_SEND_RETRIES	5
+#define	SSIF_RECV_RETRIES	250
+
+#define SSIF_MSG_MSEC		(SSIF_MSG_USEC / 1000)
+#define SSIF_MSG_JIFFIES	((SSIF_MSG_USEC * 1000) / TICK_NSEC)
+#define SSIF_MSG_PART_JIFFIES	((SSIF_MSG_PART_USEC * 1000) / TICK_NSEC)
+
+enum ssif_intf_state {
+	SSIF_NORMAL,
+	SSIF_GETTING_FLAGS,
+	SSIF_GETTING_EVENTS,
+	SSIF_CLEARING_FLAGS,
+	SSIF_GETTING_MESSAGES,
+	/* FIXME - add watchdog stuff. */
+};
+
+#define SSIF_IDLE(ssif)	 ((ssif)->ssif_state == SSIF_NORMAL \
+			  && (ssif)->curr_msg == NULL)
+
+/*
+ * Indexes into stats[] in ssif_info below.
+ */
+enum ssif_stat_indexes {
+	/* Number of total messages sent. */
+	SSIF_STAT_sent_messages = 0,
+
+	/*
+	 * Number of message parts sent.  Messages may be broken into
+	 * parts if they are long.
+	 */
+	SSIF_STAT_sent_messages_parts,
+
+	/*
+	 * Number of time a message was retried.
+	 */
+	SSIF_STAT_send_retries,
+
+	/*
+	 * Number of times the send of a message failed.
+	 */
+	SSIF_STAT_send_errors,
+
+	/*
+	 * Number of message responses received.
+	 */
+	SSIF_STAT_received_messages,
+
+	/*
+	 * Number of message fragments received.
+	 */
+	SSIF_STAT_received_message_parts,
+
+	/*
+	 * Number of times the receive of a message was retried.
+	 */
+	SSIF_STAT_receive_retries,
+
+	/*
+	 * Number of errors receiving messages.
+	 */
+	SSIF_STAT_receive_errors,
+
+	/*
+	 * Number of times a flag fetch was requested.
+	 */
+	SSIF_STAT_flag_fetches,
+
+	/*
+	 * Number of times the hardware didn't follow the state machine.
+	 */
+	SSIF_STAT_hosed,
+
+	/*
+	 * Number of received events.
+	 */
+	SSIF_STAT_events,
+
+	/* Number of asyncronous messages received. */
+	SSIF_STAT_incoming_messages,
+
+	/* Number of watchdog pretimeouts. */
+	SSIF_STAT_watchdog_pretimeouts,
+
+	/* Always add statistics before this value, it must be last. */
+	SSIF_NUM_STATS
+};
+
+struct ssif_addr_info {
+	unsigned short addr;
+	struct i2c_board_info binfo;
+	char *adapter_name;
+	int debug;
+	int slave_addr;
+	enum ipmi_addr_src addr_src;
+	union ipmi_smi_info_union addr_info;
+
+	struct mutex clients_mutex;
+	struct list_head clients;
+
+	struct list_head link;
+};
+
+struct ssif_info;
+
+typedef void (*ssif_i2c_done)(struct ssif_info *ssif_info, int result,
+			     unsigned char *data, unsigned int len);
+
+struct ssif_info {
+	ipmi_smi_t          intf;
+	int                 intf_num;
+	spinlock_t	    lock;
+	struct ipmi_smi_msg *waiting_msg;
+	struct ipmi_smi_msg *curr_msg;
+	enum ssif_intf_state ssif_state;
+	unsigned long       ssif_debug;
+
+	struct ipmi_smi_handlers handlers;
+
+	enum ipmi_addr_src addr_source; /* ACPI, PCI, SMBIOS, hardcode, etc. */
+	union ipmi_smi_info_union addr_info;
+
+	/*
+	 * Flags from the last GET_MSG_FLAGS command, used when an ATTN
+	 * is set to hold the flags until we are done handling everything
+	 * from the flags.
+	 */
+#define RECEIVE_MSG_AVAIL	0x01
+#define EVENT_MSG_BUFFER_FULL	0x02
+#define WDT_PRE_TIMEOUT_INT	0x08
+	unsigned char       msg_flags;
+
+	bool		    has_event_buffer;
+
+	/*
+	 * If set to true, this will request events the next time the
+	 * state machine is idle.
+	 */
+	bool                req_events;
+
+	/*
+	 * If set to true, this will request flags the next time the
+	 * state machine is idle.
+	 */
+	bool                req_flags;
+
+	/*
+	 * Used to perform timer operations when run-to-completion
+	 * mode is on.  This is a countdown timer.
+	 */
+	int                 rtc_us_timer;
+
+	/* Used for sending/receiving data.  +1 for the length. */
+	unsigned char data[IPMI_MAX_MSG_LENGTH + 1];
+	unsigned int  data_len;
+
+	/* Temp receive buffer, gets copied into data. */
+	unsigned char recv[I2C_SMBUS_BLOCK_MAX];
+
+	struct i2c_client *client;
+	ssif_i2c_done done_handler;
+
+	/* Thread interface handling */
+	struct task_struct *thread;
+	struct completion wake_thread;
+	bool stopping;
+	int i2c_read_write;
+	int i2c_command;
+	unsigned char *i2c_data;
+	unsigned int i2c_size;
+
+	/* From the device id response. */
+	struct ipmi_device_id device_id;
+
+	struct timer_list retry_timer;
+	int retries_left;
+
+	/* Info from SSIF cmd */
+	unsigned char max_xmit_msg_size;
+	unsigned char max_recv_msg_size;
+	unsigned int  multi_support;
+	int           supports_pec;
+
+#define SSIF_NO_MULTI		0
+#define SSIF_MULTI_2_PART	1
+#define SSIF_MULTI_n_PART	2
+	unsigned char *multi_data;
+	unsigned int  multi_len;
+	unsigned int  multi_pos;
+
+	atomic_t stats[SSIF_NUM_STATS];
+};
+
+#define ssif_inc_stat(ssif, stat) \
+	atomic_inc(&(ssif)->stats[SSIF_STAT_ ## stat])
+#define ssif_get_stat(ssif, stat) \
+	((unsigned int) atomic_read(&(ssif)->stats[SSIF_STAT_ ## stat]))
+
+static bool initialized;
+
+static atomic_t next_intf = ATOMIC_INIT(0);
+
+static void return_hosed_msg(struct ssif_info *ssif_info,
+			     struct ipmi_smi_msg *msg);
+static void start_next_msg(struct ssif_info *ssif_info, unsigned long *flags);
+static int start_send(struct ssif_info *ssif_info,
+		      unsigned char   *data,
+		      unsigned int    len);
+
+static unsigned long *ipmi_ssif_lock_cond(struct ssif_info *ssif_info,
+					  unsigned long *flags)
+{
+	spin_lock_irqsave(&ssif_info->lock, *flags);
+	return flags;
+}
+
+static void ipmi_ssif_unlock_cond(struct ssif_info *ssif_info,
+				  unsigned long *flags)
+{
+	spin_unlock_irqrestore(&ssif_info->lock, *flags);
+}
+
+static void deliver_recv_msg(struct ssif_info *ssif_info,
+			     struct ipmi_smi_msg *msg)
+{
+	ipmi_smi_t    intf = ssif_info->intf;
+
+	if (!intf) {
+		ipmi_free_smi_msg(msg);
+	} else if (msg->rsp_size < 0) {
+		return_hosed_msg(ssif_info, msg);
+		pr_err(PFX
+		       "Malformed message in deliver_recv_msg: rsp_size = %d\n",
+		       msg->rsp_size);
+	} else {
+		ipmi_smi_msg_received(intf, msg);
+	}
+}
+
+static void return_hosed_msg(struct ssif_info *ssif_info,
+			     struct ipmi_smi_msg *msg)
+{
+	ssif_inc_stat(ssif_info, hosed);
+
+	/* Make it a response */
+	msg->rsp[0] = msg->data[0] | 4;
+	msg->rsp[1] = msg->data[1];
+	msg->rsp[2] = 0xFF; /* Unknown error. */
+	msg->rsp_size = 3;
+
+	deliver_recv_msg(ssif_info, msg);
+}
+
+/*
+ * Must be called with the message lock held.  This will release the
+ * message lock.  Note that the caller will check SSIF_IDLE and start a
+ * new operation, so there is no need to check for new messages to
+ * start in here.
+ */
+static void start_clear_flags(struct ssif_info *ssif_info, unsigned long *flags)
+{
+	unsigned char msg[3];
+
+	ssif_info->msg_flags &= ~WDT_PRE_TIMEOUT_INT;
+	ssif_info->ssif_state = SSIF_CLEARING_FLAGS;
+	ipmi_ssif_unlock_cond(ssif_info, flags);
+
+	/* Make sure the watchdog pre-timeout flag is not set at startup. */
+	msg[0] = (IPMI_NETFN_APP_REQUEST << 2);
+	msg[1] = IPMI_CLEAR_MSG_FLAGS_CMD;
+	msg[2] = WDT_PRE_TIMEOUT_INT;
+
+	if (start_send(ssif_info, msg, 3) != 0) {
+		/* Error, just go to normal state. */
+		ssif_info->ssif_state = SSIF_NORMAL;
+	}
+}
+
+static void start_flag_fetch(struct ssif_info *ssif_info, unsigned long *flags)
+{
+	unsigned char mb[2];
+
+	ssif_info->req_flags = false;
+	ssif_info->ssif_state = SSIF_GETTING_FLAGS;
+	ipmi_ssif_unlock_cond(ssif_info, flags);
+
+	mb[0] = (IPMI_NETFN_APP_REQUEST << 2);
+	mb[1] = IPMI_GET_MSG_FLAGS_CMD;
+	if (start_send(ssif_info, mb, 2) != 0)
+		ssif_info->ssif_state = SSIF_NORMAL;
+}
+
+static void check_start_send(struct ssif_info *ssif_info, unsigned long *flags,
+			     struct ipmi_smi_msg *msg)
+{
+	if (start_send(ssif_info, msg->data, msg->data_size) != 0) {
+		unsigned long oflags;
+
+		flags = ipmi_ssif_lock_cond(ssif_info, &oflags);
+		ssif_info->curr_msg = NULL;
+		ssif_info->ssif_state = SSIF_NORMAL;
+		ipmi_ssif_unlock_cond(ssif_info, flags);
+		ipmi_free_smi_msg(msg);
+	}
+}
+
+static void start_event_fetch(struct ssif_info *ssif_info, unsigned long *flags)
+{
+	struct ipmi_smi_msg *msg;
+
+	ssif_info->req_events = false;
+
+	msg = ipmi_alloc_smi_msg();
+	if (!msg) {
+		ssif_info->ssif_state = SSIF_NORMAL;
+		return;
+	}
+
+	ssif_info->curr_msg = msg;
+	ssif_info->ssif_state = SSIF_GETTING_EVENTS;
+	ipmi_ssif_unlock_cond(ssif_info, flags);
+
+	msg->data[0] = (IPMI_NETFN_APP_REQUEST << 2);
+	msg->data[1] = IPMI_READ_EVENT_MSG_BUFFER_CMD;
+	msg->data_size = 2;
+
+	check_start_send(ssif_info, flags, msg);
+}
+
+static void start_recv_msg_fetch(struct ssif_info *ssif_info,
+				 unsigned long *flags)
+{
+	struct ipmi_smi_msg *msg;
+
+	msg = ipmi_alloc_smi_msg();
+	if (!msg) {
+		ssif_info->ssif_state = SSIF_NORMAL;
+		return;
+	}
+
+	ssif_info->curr_msg = msg;
+	ssif_info->ssif_state = SSIF_GETTING_MESSAGES;
+	ipmi_ssif_unlock_cond(ssif_info, flags);
+
+	msg->data[0] = (IPMI_NETFN_APP_REQUEST << 2);
+	msg->data[1] = IPMI_GET_MSG_CMD;
+	msg->data_size = 2;
+
+	check_start_send(ssif_info, flags, msg);
+}
+
+/*
+ * Must be called with the message lock held.  This will release the
+ * message lock.  Note that the caller will check SSIF_IDLE and start a
+ * new operation, so there is no need to check for new messages to
+ * start in here.
+ */
+static void handle_flags(struct ssif_info *ssif_info, unsigned long *flags)
+{
+	if (ssif_info->msg_flags & WDT_PRE_TIMEOUT_INT) {
+		ipmi_smi_t intf = ssif_info->intf;
+		/* Watchdog pre-timeout */
+		ssif_inc_stat(ssif_info, watchdog_pretimeouts);
+		start_clear_flags(ssif_info, flags);
+		if (intf)
+			ipmi_smi_watchdog_pretimeout(intf);
+	} else if (ssif_info->msg_flags & RECEIVE_MSG_AVAIL)
+		/* Messages available. */
+		start_recv_msg_fetch(ssif_info, flags);
+	else if (ssif_info->msg_flags & EVENT_MSG_BUFFER_FULL)
+		/* Events available. */
+		start_event_fetch(ssif_info, flags);
+	else {
+		ssif_info->ssif_state = SSIF_NORMAL;
+		ipmi_ssif_unlock_cond(ssif_info, flags);
+	}
+}
+
+static int ipmi_ssif_thread(void *data)
+{
+	struct ssif_info *ssif_info = data;
+
+	while (!kthread_should_stop()) {
+		int result;
+
+		/* Wait for something to do */
+		wait_for_completion(&ssif_info->wake_thread);
+		init_completion(&ssif_info->wake_thread);
+
+		if (ssif_info->stopping)
+			break;
+
+		if (ssif_info->i2c_read_write == I2C_SMBUS_WRITE) {
+			result = i2c_smbus_write_block_data(
+				ssif_info->client, SSIF_IPMI_REQUEST,
+				ssif_info->i2c_data[0],
+				ssif_info->i2c_data + 1);
+			ssif_info->done_handler(ssif_info, result, NULL, 0);
+		} else {
+			result = i2c_smbus_read_block_data(
+				ssif_info->client, SSIF_IPMI_RESPONSE,
+				ssif_info->i2c_data);
+			if (result < 0)
+				ssif_info->done_handler(ssif_info, result,
+							NULL, 0);
+			else
+				ssif_info->done_handler(ssif_info, 0,
+							ssif_info->i2c_data,
+							result);
+		}
+	}
+
+	return 0;
+}
+
+static int ssif_i2c_send(struct ssif_info *ssif_info,
+			ssif_i2c_done handler,
+			int read_write, int command,
+			unsigned char *data, unsigned int size)
+{
+	ssif_info->done_handler = handler;
+
+	ssif_info->i2c_read_write = read_write;
+	ssif_info->i2c_command = command;
+	ssif_info->i2c_data = data;
+	ssif_info->i2c_size = size;
+	complete(&ssif_info->wake_thread);
+	return 0;
+}
+
+
+static void msg_done_handler(struct ssif_info *ssif_info, int result,
+			     unsigned char *data, unsigned int len);
+
+static void retry_timeout(unsigned long data)
+{
+	struct ssif_info *ssif_info = (void *) data;
+	int rv;
+
+	if (ssif_info->stopping)
+		return;
+
+	ssif_info->rtc_us_timer = 0;
+
+	rv = ssif_i2c_send(ssif_info, msg_done_handler, I2C_SMBUS_READ,
+			  SSIF_IPMI_RESPONSE,
+			  ssif_info->recv, I2C_SMBUS_BLOCK_DATA);
+	if (rv < 0) {
+		/* request failed, just return the error. */
+		if (ssif_info->ssif_debug & SSIF_DEBUG_MSG)
+			pr_info("Error from i2c_non_blocking_op(5)\n");
+
+		msg_done_handler(ssif_info, -EIO, NULL, 0);
+	}
+}
+
+static int start_resend(struct ssif_info *ssif_info);
+
+static void msg_done_handler(struct ssif_info *ssif_info, int result,
+			     unsigned char *data, unsigned int len)
+{
+	struct ipmi_smi_msg *msg;
+	unsigned long oflags, *flags;
+	int rv;
+
+	/*
+	 * We are single-threaded here, so no need for a lock until we
+	 * start messing with driver states or the queues.
+	 */
+
+	if (result < 0) {
+		ssif_info->retries_left--;
+		if (ssif_info->retries_left > 0) {
+			ssif_inc_stat(ssif_info, receive_retries);
+
+			mod_timer(&ssif_info->retry_timer,
+				  jiffies + SSIF_MSG_JIFFIES);
+			ssif_info->rtc_us_timer = SSIF_MSG_USEC;
+			return;
+		}
+
+		ssif_inc_stat(ssif_info, receive_errors);
+
+		if  (ssif_info->ssif_debug & SSIF_DEBUG_MSG)
+			pr_info("Error in msg_done_handler: %d\n", result);
+		len = 0;
+		goto continue_op;
+	}
+
+	if ((len > 1) && (ssif_info->multi_pos == 0)
+				&& (data[0] == 0x00) && (data[1] == 0x01)) {
+		/* Start of multi-part read.  Start the next transaction. */
+		int i;
+
+		ssif_inc_stat(ssif_info, received_message_parts);
+
+		/* Remove the multi-part read marker. */
+		for (i = 0; i < (len-2); i++)
+			ssif_info->data[i] = data[i+2];
+		len -= 2;
+		ssif_info->multi_len = len;
+		ssif_info->multi_pos = 1;
+
+		rv = ssif_i2c_send(ssif_info, msg_done_handler, I2C_SMBUS_READ,
+				  SSIF_IPMI_MULTI_PART_RESPONSE_MIDDLE,
+				  ssif_info->recv, I2C_SMBUS_BLOCK_DATA);
+		if (rv < 0) {
+			if (ssif_info->ssif_debug & SSIF_DEBUG_MSG)
+				pr_info("Error from i2c_non_blocking_op(1)\n");
+
+			result = -EIO;
+		} else
+			return;
+	} else if (ssif_info->multi_pos) {
+		/* Middle of multi-part read.  Start the next transaction. */
+		int i;
+		unsigned char blocknum;
+
+		if (len == 0) {
+			result = -EIO;
+			if (ssif_info->ssif_debug & SSIF_DEBUG_MSG)
+				pr_info(PFX "Middle message with no data\n");
+
+			goto continue_op;
+		}
+
+		blocknum = data[ssif_info->multi_len];
+
+		if (ssif_info->multi_len+len-1 > IPMI_MAX_MSG_LENGTH) {
+			/* Received message too big, abort the operation. */
+			result = -E2BIG;
+			if (ssif_info->ssif_debug & SSIF_DEBUG_MSG)
+				pr_info("Received message too big\n");
+
+			goto continue_op;
+		}
+
+		/* Remove the blocknum from the data. */
+		for (i = 0; i < (len-1); i++)
+			ssif_info->data[i+ssif_info->multi_len] = data[i+1];
+		len--;
+		ssif_info->multi_len += len;
+		if (blocknum == 0xff) {
+			/* End of read */
+			len = ssif_info->multi_len;
+			data = ssif_info->data;
+		} else if ((blocknum+1) != ssif_info->multi_pos) {
+			/*
+			 * Out of sequence block, just abort.  Block
+			 * numbers start at zero for the second block,
+			 * but multi_pos starts at one, so the +1.
+			 */
+			result = -EIO;
+		} else {
+			ssif_inc_stat(ssif_info, received_message_parts);
+
+			ssif_info->multi_pos++;
+
+			rv = ssif_i2c_send(ssif_info, msg_done_handler,
+					   I2C_SMBUS_READ,
+					   SSIF_IPMI_MULTI_PART_RESPONSE_MIDDLE,
+					   ssif_info->recv,
+					   I2C_SMBUS_BLOCK_DATA);
+			if (rv < 0) {
+				if (ssif_info->ssif_debug & SSIF_DEBUG_MSG)
+					pr_info(PFX
+						"Error from i2c_non_blocking_op(2)\n");
+
+				result = -EIO;
+			} else
+				return;
+		}
+	}
+
+	if (result < 0) {
+		ssif_inc_stat(ssif_info, receive_errors);
+	} else {
+		ssif_inc_stat(ssif_info, received_messages);
+		ssif_inc_stat(ssif_info, received_message_parts);
+	}
+
+
+ continue_op:
+	if (ssif_info->ssif_debug & SSIF_DEBUG_STATE)
+		pr_info(PFX "DONE 1: state = %d, result=%d.\n",
+			ssif_info->ssif_state, result);
+
+	flags = ipmi_ssif_lock_cond(ssif_info, &oflags);
+	msg = ssif_info->curr_msg;
+	if (msg) {
+		msg->rsp_size = len;
+		if (msg->rsp_size > IPMI_MAX_MSG_LENGTH)
+			msg->rsp_size = IPMI_MAX_MSG_LENGTH;
+		memcpy(msg->rsp, data, msg->rsp_size);
+		ssif_info->curr_msg = NULL;
+	}
+
+	switch (ssif_info->ssif_state) {
+	case SSIF_NORMAL:
+		ipmi_ssif_unlock_cond(ssif_info, flags);
+		if (!msg)
+			break;
+
+		if (result < 0)
+			return_hosed_msg(ssif_info, msg);
+		else
+			deliver_recv_msg(ssif_info, msg);
+		break;
+
+	case SSIF_GETTING_FLAGS:
+		/* We got the flags from the SSIF, now handle them. */
+		if ((result < 0) || (len < 4) || (data[2] != 0)) {
+			/*
+			 * Error fetching flags, or invalid length,
+			 * just give up for now.
+			 */
+			ssif_info->ssif_state = SSIF_NORMAL;
+			ipmi_ssif_unlock_cond(ssif_info, flags);
+			pr_warn(PFX "Error getting flags: %d %d, %x\n",
+			       result, len, data[2]);
+		} else if (data[0] != (IPMI_NETFN_APP_REQUEST | 1) << 2
+			   || data[1] != IPMI_GET_MSG_FLAGS_CMD) {
+			pr_warn(PFX "Invalid response getting flags: %x %x\n",
+				data[0], data[1]);
+		} else {
+			ssif_inc_stat(ssif_info, flag_fetches);
+			ssif_info->msg_flags = data[3];
+			handle_flags(ssif_info, flags);
+		}
+		break;
+
+	case SSIF_CLEARING_FLAGS:
+		/* We cleared the flags. */
+		if ((result < 0) || (len < 3) || (data[2] != 0)) {
+			/* Error clearing flags */
+			pr_warn(PFX "Error clearing flags: %d %d, %x\n",
+			       result, len, data[2]);
+		} else if (data[0] != (IPMI_NETFN_APP_REQUEST | 1) << 2
+			   || data[1] != IPMI_CLEAR_MSG_FLAGS_CMD) {
+			pr_warn(PFX "Invalid response clearing flags: %x %x\n",
+				data[0], data[1]);
+		}
+		ssif_info->ssif_state = SSIF_NORMAL;
+		ipmi_ssif_unlock_cond(ssif_info, flags);
+		break;
+
+	case SSIF_GETTING_EVENTS:
+		if ((result < 0) || (len < 3) || (msg->rsp[2] != 0)) {
+			/* Error getting event, probably done. */
+			msg->done(msg);
+
+			/* Take off the event flag. */
+			ssif_info->msg_flags &= ~EVENT_MSG_BUFFER_FULL;
+			handle_flags(ssif_info, flags);
+		} else if (msg->rsp[0] != (IPMI_NETFN_APP_REQUEST | 1) << 2
+			   || msg->rsp[1] != IPMI_READ_EVENT_MSG_BUFFER_CMD) {
+			pr_warn(PFX "Invalid response getting events: %x %x\n",
+				msg->rsp[0], msg->rsp[1]);
+			msg->done(msg);
+			/* Take off the event flag. */
+			ssif_info->msg_flags &= ~EVENT_MSG_BUFFER_FULL;
+			handle_flags(ssif_info, flags);
+		} else {
+			handle_flags(ssif_info, flags);
+			ssif_inc_stat(ssif_info, events);
+			deliver_recv_msg(ssif_info, msg);
+		}
+		break;
+
+	case SSIF_GETTING_MESSAGES:
+		if ((result < 0) || (len < 3) || (msg->rsp[2] != 0)) {
+			/* Error getting event, probably done. */
+			msg->done(msg);
+
+			/* Take off the msg flag. */
+			ssif_info->msg_flags &= ~RECEIVE_MSG_AVAIL;
+			handle_flags(ssif_info, flags);
+		} else if (msg->rsp[0] != (IPMI_NETFN_APP_REQUEST | 1) << 2
+			   || msg->rsp[1] != IPMI_GET_MSG_CMD) {
+			pr_warn(PFX "Invalid response clearing flags: %x %x\n",
+				msg->rsp[0], msg->rsp[1]);
+			msg->done(msg);
+
+			/* Take off the msg flag. */
+			ssif_info->msg_flags &= ~RECEIVE_MSG_AVAIL;
+			handle_flags(ssif_info, flags);
+		} else {
+			ssif_inc_stat(ssif_info, incoming_messages);
+			handle_flags(ssif_info, flags);
+			deliver_recv_msg(ssif_info, msg);
+		}
+		break;
+	}
+
+	flags = ipmi_ssif_lock_cond(ssif_info, &oflags);
+	if (SSIF_IDLE(ssif_info) && !ssif_info->stopping) {
+		if (ssif_info->req_events)
+			start_event_fetch(ssif_info, flags);
+		else if (ssif_info->req_flags)
+			start_flag_fetch(ssif_info, flags);
+		else
+			start_next_msg(ssif_info, flags);
+	} else
+		ipmi_ssif_unlock_cond(ssif_info, flags);
+
+	if (ssif_info->ssif_debug & SSIF_DEBUG_STATE)
+		pr_info(PFX "DONE 2: state = %d.\n", ssif_info->ssif_state);
+}
+
+static void msg_written_handler(struct ssif_info *ssif_info, int result,
+				unsigned char *data, unsigned int len)
+{
+	int rv;
+
+	/* We are single-threaded here, so no need for a lock. */
+	if (result < 0) {
+		ssif_info->retries_left--;
+		if (ssif_info->retries_left > 0) {
+			if (!start_resend(ssif_info)) {
+				ssif_inc_stat(ssif_info, send_retries);
+				return;
+			}
+			/* request failed, just return the error. */
+			ssif_inc_stat(ssif_info, send_errors);
+
+			if (ssif_info->ssif_debug & SSIF_DEBUG_MSG)
+				pr_info(PFX
+					"Out of retries in msg_written_handler\n");
+			msg_done_handler(ssif_info, -EIO, NULL, 0);
+			return;
+		}
+
+		ssif_inc_stat(ssif_info, send_errors);
+
+		/*
+		 * Got an error on transmit, let the done routine
+		 * handle it.
+		 */
+		if (ssif_info->ssif_debug & SSIF_DEBUG_MSG)
+			pr_info("Error in msg_written_handler: %d\n", result);
+
+		msg_done_handler(ssif_info, result, NULL, 0);
+		return;
+	}
+
+	if (ssif_info->multi_data) {
+		/* In the middle of a multi-data write. */
+		int left;
+
+		ssif_inc_stat(ssif_info, sent_messages_parts);
+
+		left = ssif_info->multi_len - ssif_info->multi_pos;
+		if (left > 32)
+			left = 32;
+		/* Length byte. */
+		ssif_info->multi_data[ssif_info->multi_pos] = left;
+		ssif_info->multi_pos += left;
+		if (left < 32)
+			/*
+			 * Write is finished.  Note that we must end
+			 * with a write of less than 32 bytes to
+			 * complete the transaction, even if it is
+			 * zero bytes.
+			 */
+			ssif_info->multi_data = NULL;
+
+		rv = ssif_i2c_send(ssif_info, msg_written_handler,
+				  I2C_SMBUS_WRITE,
+				  SSIF_IPMI_MULTI_PART_REQUEST_MIDDLE,
+				  ssif_info->multi_data + ssif_info->multi_pos,
+				  I2C_SMBUS_BLOCK_DATA);
+		if (rv < 0) {
+			/* request failed, just return the error. */
+			ssif_inc_stat(ssif_info, send_errors);
+
+			if (ssif_info->ssif_debug & SSIF_DEBUG_MSG)
+				pr_info("Error from i2c_non_blocking_op(3)\n");
+			msg_done_handler(ssif_info, -EIO, NULL, 0);
+		}
+	} else {
+		ssif_inc_stat(ssif_info, sent_messages);
+		ssif_inc_stat(ssif_info, sent_messages_parts);
+
+		/* Wait a jiffie then request the next message */
+		ssif_info->retries_left = SSIF_RECV_RETRIES;
+		ssif_info->rtc_us_timer = SSIF_MSG_PART_USEC;
+		mod_timer(&ssif_info->retry_timer,
+			  jiffies + SSIF_MSG_PART_JIFFIES);
+		return;
+	}
+}
+
+static int start_resend(struct ssif_info *ssif_info)
+{
+	int rv;
+	int command;
+
+	if (ssif_info->data_len > 32) {
+		command = SSIF_IPMI_MULTI_PART_REQUEST_START;
+		ssif_info->multi_data = ssif_info->data;
+		ssif_info->multi_len = ssif_info->data_len;
+		/*
+		 * Subtle thing, this is 32, not 33, because we will
+		 * overwrite the thing at position 32 (which was just
+		 * transmitted) with the new length.
+		 */
+		ssif_info->multi_pos = 32;
+		ssif_info->data[0] = 32;
+	} else {
+		ssif_info->multi_data = NULL;
+		command = SSIF_IPMI_REQUEST;
+		ssif_info->data[0] = ssif_info->data_len;
+	}
+
+	rv = ssif_i2c_send(ssif_info, msg_written_handler, I2C_SMBUS_WRITE,
+			  command, ssif_info->data, I2C_SMBUS_BLOCK_DATA);
+	if (rv && (ssif_info->ssif_debug & SSIF_DEBUG_MSG))
+		pr_info("Error from i2c_non_blocking_op(4)\n");
+	return rv;
+}
+
+static int start_send(struct ssif_info *ssif_info,
+		      unsigned char   *data,
+		      unsigned int    len)
+{
+	if (len > IPMI_MAX_MSG_LENGTH)
+		return -E2BIG;
+	if (len > ssif_info->max_xmit_msg_size)
+		return -E2BIG;
+
+	ssif_info->retries_left = SSIF_SEND_RETRIES;
+	memcpy(ssif_info->data+1, data, len);
+	ssif_info->data_len = len;
+	return start_resend(ssif_info);
+}
+
+/* Must be called with the message lock held. */
+static void start_next_msg(struct ssif_info *ssif_info, unsigned long *flags)
+{
+	struct ipmi_smi_msg *msg;
+	unsigned long oflags;
+
+ restart:
+	if (!SSIF_IDLE(ssif_info)) {
+		ipmi_ssif_unlock_cond(ssif_info, flags);
+		return;
+	}
+
+	if (!ssif_info->waiting_msg) {
+		ssif_info->curr_msg = NULL;
+		ipmi_ssif_unlock_cond(ssif_info, flags);
+	} else {
+		int rv;
+
+		ssif_info->curr_msg = ssif_info->waiting_msg;
+		ssif_info->waiting_msg = NULL;
+		ipmi_ssif_unlock_cond(ssif_info, flags);
+		rv = start_send(ssif_info,
+				ssif_info->curr_msg->data,
+				ssif_info->curr_msg->data_size);
+		if (rv) {
+			msg = ssif_info->curr_msg;
+			ssif_info->curr_msg = NULL;
+			return_hosed_msg(ssif_info, msg);
+			flags = ipmi_ssif_lock_cond(ssif_info, &oflags);
+			goto restart;
+		}
+	}
+}
+
+static void sender(void                *send_info,
+		   struct ipmi_smi_msg *msg)
+{
+	struct ssif_info *ssif_info = (struct ssif_info *) send_info;
+	unsigned long oflags, *flags;
+
+	BUG_ON(ssif_info->waiting_msg);
+	ssif_info->waiting_msg = msg;
+
+	flags = ipmi_ssif_lock_cond(ssif_info, &oflags);
+	start_next_msg(ssif_info, flags);
+
+	if (ssif_info->ssif_debug & SSIF_DEBUG_TIMING) {
+		struct timeval t;
+
+		do_gettimeofday(&t);
+		pr_info("**Enqueue %02x %02x: %ld.%6.6ld\n",
+		       msg->data[0], msg->data[1], t.tv_sec, t.tv_usec);
+	}
+}
+
+static int get_smi_info(void *send_info, struct ipmi_smi_info *data)
+{
+	struct ssif_info *ssif_info = send_info;
+
+	data->addr_src = ssif_info->addr_source;
+	data->dev = &ssif_info->client->dev;
+	data->addr_info = ssif_info->addr_info;
+	get_device(data->dev);
+
+	return 0;
+}
+
+/*
+ * Instead of having our own timer to periodically check the message
+ * flags, we let the message handler drive us.
+ */
+static void request_events(void *send_info)
+{
+	struct ssif_info *ssif_info = (struct ssif_info *) send_info;
+	unsigned long oflags, *flags;
+
+	if (!ssif_info->has_event_buffer)
+		return;
+
+	flags = ipmi_ssif_lock_cond(ssif_info, &oflags);
+	/*
+	 * Request flags first, not events, because the lower layer
+	 * doesn't have a way to send an attention.  But make sure
+	 * event checking still happens.
+	 */
+	ssif_info->req_events = true;
+	if (SSIF_IDLE(ssif_info))
+		start_flag_fetch(ssif_info, flags);
+	else {
+		ssif_info->req_flags = true;
+		ipmi_ssif_unlock_cond(ssif_info, flags);
+	}
+}
+
+static int inc_usecount(void *send_info)
+{
+	struct ssif_info *ssif_info = send_info;
+
+	if (!i2c_get_adapter(ssif_info->client->adapter->nr))
+		return -ENODEV;
+
+	i2c_use_client(ssif_info->client);
+	return 0;
+}
+
+static void dec_usecount(void *send_info)
+{
+	struct ssif_info *ssif_info = send_info;
+
+	i2c_release_client(ssif_info->client);
+	i2c_put_adapter(ssif_info->client->adapter);
+}
+
+static int ssif_start_processing(void *send_info,
+				 ipmi_smi_t intf)
+{
+	struct ssif_info *ssif_info = send_info;
+
+	ssif_info->intf = intf;
+
+	return 0;
+}
+
+#define MAX_SSIF_BMCS 4
+
+static unsigned short addr[MAX_SSIF_BMCS];
+static int num_addrs;
+module_param_array(addr, ushort, &num_addrs, 0);
+MODULE_PARM_DESC(addr, "The addresses to scan for IPMI BMCs on the SSIFs.");
+
+static char *adapter_name[MAX_SSIF_BMCS];
+static int num_adapter_names;
+module_param_array(adapter_name, charp, &num_adapter_names, 0);
+MODULE_PARM_DESC(adapter_name, "The string name of the I2C device that has the BMC.  By default all devices are scanned.");
+
+static int slave_addrs[MAX_SSIF_BMCS];
+static int num_slave_addrs;
+module_param_array(slave_addrs, int, &num_slave_addrs, 0);
+MODULE_PARM_DESC(slave_addrs,
+		 "The default IPMB slave address for the controller.");
+
+/*
+ * Bit 0 enables message debugging, bit 1 enables state debugging, and
+ * bit 2 enables timing debugging.  This is an array indexed by
+ * interface number"
+ */
+static int dbg[MAX_SSIF_BMCS];
+static int num_dbg;
+module_param_array(dbg, int, &num_dbg, 0);
+MODULE_PARM_DESC(dbg, "Turn on debugging.");
+
+static bool ssif_dbg_probe;
+module_param_named(dbg_probe, ssif_dbg_probe, bool, 0);
+MODULE_PARM_DESC(dbg_probe, "Enable debugging of probing of adapters.");
+
+static int use_thread;
+module_param(use_thread, int, 0);
+MODULE_PARM_DESC(use_thread, "Use the thread interface.");
+
+static bool ssif_tryacpi = 1;
+module_param_named(tryacpi, ssif_tryacpi, bool, 0);
+MODULE_PARM_DESC(tryacpi, "Setting this to zero will disable the default scan of the interfaces identified via ACPI");
+
+static bool ssif_trydmi = 1;
+module_param_named(trydmi, ssif_trydmi, bool, 0);
+MODULE_PARM_DESC(trydmi, "Setting this to zero will disable the default scan of the interfaces identified via DMI (SMBIOS)");
+
+static DEFINE_MUTEX(ssif_infos_mutex);
+static LIST_HEAD(ssif_infos);
+
+static int ssif_remove(struct i2c_client *client)
+{
+	struct ssif_info *ssif_info = i2c_get_clientdata(client);
+	int rv;
+
+	if (!ssif_info)
+		return 0;
+
+	i2c_set_clientdata(client, NULL);
+
+	/*
+	 * After this point, we won't deliver anything asychronously
+	 * to the message handler.  We can unregister ourself.
+	 */
+	rv = ipmi_unregister_smi(ssif_info->intf);
+	if (rv) {
+		pr_err(PFX "Unable to unregister device: errno=%d\n", rv);
+		return rv;
+	}
+	ssif_info->intf = NULL;
+
+	/* make sure the driver is not looking for flags any more. */
+	while (ssif_info->ssif_state != SSIF_NORMAL)
+		schedule_timeout(1);
+
+	ssif_info->stopping = true;
+	del_timer_sync(&ssif_info->retry_timer);
+	if (ssif_info->thread) {
+		complete(&ssif_info->wake_thread);
+		kthread_stop(ssif_info->thread);
+	}
+
+	/*
+	 * No message can be outstanding now, we have removed the
+	 * upper layer and it permitted us to do so.
+	 */
+	kfree(ssif_info);
+	return 0;
+}
+
+static int do_cmd(struct i2c_client *client, int len, unsigned char *msg,
+		  int *resp_len, unsigned char *resp)
+{
+	int retry_cnt;
+	int ret;
+
+	retry_cnt = SSIF_SEND_RETRIES;
+ retry1:
+	ret = i2c_smbus_write_block_data(client, SSIF_IPMI_REQUEST, len, msg);
+	if (ret) {
+		retry_cnt--;
+		if (retry_cnt > 0)
+			goto retry1;
+		return -ENODEV;
+	}
+
+	ret = -ENODEV;
+	retry_cnt = SSIF_RECV_RETRIES;
+	while (retry_cnt > 0) {
+		ret = i2c_smbus_read_block_data(client, SSIF_IPMI_RESPONSE,
+						resp);
+		if (ret > 0)
+			break;
+		msleep(SSIF_MSG_MSEC);
+		retry_cnt--;
+		if (retry_cnt <= 0)
+			break;
+	}
+
+	if (ret > 0) {
+		/* Validate that the response is correct. */
+		if (ret < 3 ||
+		    (resp[0] != (msg[0] | (1 << 2))) ||
+		    (resp[1] != msg[1]))
+			ret = -EINVAL;
+		else {
+			*resp_len = ret;
+			ret = 0;
+		}
+	}
+
+	return ret;
+}
+
+static int ssif_detect(struct i2c_client *client, struct i2c_board_info *info)
+{
+	unsigned char *resp;
+	unsigned char msg[3];
+	int           rv;
+	int           len;
+
+	resp = kmalloc(IPMI_MAX_MSG_LENGTH, GFP_KERNEL);
+	if (!resp)
+		return -ENOMEM;
+
+	/* Do a Get Device ID command, since it is required. */
+	msg[0] = IPMI_NETFN_APP_REQUEST << 2;
+	msg[1] = IPMI_GET_DEVICE_ID_CMD;
+	rv = do_cmd(client, 2, msg, &len, resp);
+	if (rv)
+		rv = -ENODEV;
+	else
+		strlcpy(info->type, DEVICE_NAME, I2C_NAME_SIZE);
+	kfree(resp);
+	return rv;
+}
+
+static int smi_type_proc_show(struct seq_file *m, void *v)
+{
+	return seq_puts(m, "ssif\n");
+}
+
+static int smi_type_proc_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, smi_type_proc_show, inode->i_private);
+}
+
+static const struct file_operations smi_type_proc_ops = {
+	.open		= smi_type_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int smi_stats_proc_show(struct seq_file *m, void *v)
+{
+	struct ssif_info *ssif_info = m->private;
+
+	seq_printf(m, "sent_messages:          %u\n",
+		   ssif_get_stat(ssif_info, sent_messages));
+	seq_printf(m, "sent_messages_parts:    %u\n",
+		   ssif_get_stat(ssif_info, sent_messages_parts));
+	seq_printf(m, "send_retries:           %u\n",
+		   ssif_get_stat(ssif_info, send_retries));
+	seq_printf(m, "send_errors:            %u\n",
+		   ssif_get_stat(ssif_info, send_errors));
+	seq_printf(m, "received_messages:      %u\n",
+		   ssif_get_stat(ssif_info, received_messages));
+	seq_printf(m, "received_message_parts: %u\n",
+		   ssif_get_stat(ssif_info, received_message_parts));
+	seq_printf(m, "receive_retries:        %u\n",
+		   ssif_get_stat(ssif_info, receive_retries));
+	seq_printf(m, "receive_errors:         %u\n",
+		   ssif_get_stat(ssif_info, receive_errors));
+	seq_printf(m, "flag_fetches:           %u\n",
+		   ssif_get_stat(ssif_info, flag_fetches));
+	seq_printf(m, "hosed:                  %u\n",
+		   ssif_get_stat(ssif_info, hosed));
+	seq_printf(m, "events:                 %u\n",
+		   ssif_get_stat(ssif_info, events));
+	seq_printf(m, "watchdog_pretimeouts:   %u\n",
+		   ssif_get_stat(ssif_info, watchdog_pretimeouts));
+	return 0;
+}
+
+static int smi_stats_proc_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, smi_stats_proc_show, PDE_DATA(inode));
+}
+
+static const struct file_operations smi_stats_proc_ops = {
+	.open		= smi_stats_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static struct ssif_addr_info *ssif_info_find(unsigned short addr,
+					     char *adapter_name,
+					     bool match_null_name)
+{
+	struct ssif_addr_info *info, *found = NULL;
+
+restart:
+	list_for_each_entry(info, &ssif_infos, link) {
+		if (info->binfo.addr == addr) {
+			if (info->adapter_name || adapter_name) {
+				if (!info->adapter_name != !adapter_name) {
+					/* One is NULL and one is not */
+					continue;
+				}
+				if (strcmp(info->adapter_name, adapter_name))
+					/* Names to not match */
+					continue;
+			}
+			found = info;
+			break;
+		}
+	}
+
+	if (!found && match_null_name) {
+		/* Try to get an exact match first, then try with a NULL name */
+		adapter_name = NULL;
+		match_null_name = false;
+		goto restart;
+	}
+
+	return found;
+}
+
+static bool check_acpi(struct ssif_info *ssif_info, struct device *dev)
+{
+#ifdef CONFIG_ACPI
+	acpi_handle acpi_handle;
+
+	acpi_handle = ACPI_HANDLE(dev);
+	if (acpi_handle) {
+		ssif_info->addr_source = SI_ACPI;
+		ssif_info->addr_info.acpi_info.acpi_handle = acpi_handle;
+		return true;
+	}
+#endif
+	return false;
+}
+
+static int ssif_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	unsigned char     msg[3];
+	unsigned char     *resp;
+	struct ssif_info   *ssif_info;
+	int               rv = 0;
+	int               len;
+	int               i;
+	u8		  slave_addr = 0;
+	struct ssif_addr_info *addr_info = NULL;
+
+
+	resp = kmalloc(IPMI_MAX_MSG_LENGTH, GFP_KERNEL);
+	if (!resp)
+		return -ENOMEM;
+
+	ssif_info = kzalloc(sizeof(*ssif_info), GFP_KERNEL);
+	if (!ssif_info) {
+		kfree(resp);
+		return -ENOMEM;
+	}
+
+	if (!check_acpi(ssif_info, &client->dev)) {
+		addr_info = ssif_info_find(client->addr, client->adapter->name,
+					   true);
+		if (!addr_info) {
+			/* Must have come in through sysfs. */
+			ssif_info->addr_source = SI_HOTMOD;
+		} else {
+			ssif_info->addr_source = addr_info->addr_src;
+			ssif_info->ssif_debug = addr_info->debug;
+			ssif_info->addr_info = addr_info->addr_info;
+			slave_addr = addr_info->slave_addr;
+		}
+	}
+
+	pr_info(PFX "Trying %s-specified SSIF interface at i2c address 0x%x, adapter %s, slave address 0x%x\n",
+	       ipmi_addr_src_to_str(ssif_info->addr_source),
+	       client->addr, client->adapter->name, slave_addr);
+
+	/*
+	 * Do a Get Device ID command, since it comes back with some
+	 * useful info.
+	 */
+	msg[0] = IPMI_NETFN_APP_REQUEST << 2;
+	msg[1] = IPMI_GET_DEVICE_ID_CMD;
+	rv = do_cmd(client, 2, msg, &len, resp);
+	if (rv)
+		goto out;
+
+	rv = ipmi_demangle_device_id(resp, len, &ssif_info->device_id);
+	if (rv)
+		goto out;
+
+	ssif_info->client = client;
+	i2c_set_clientdata(client, ssif_info);
+
+	/* Now check for system interface capabilities */
+	msg[0] = IPMI_NETFN_APP_REQUEST << 2;
+	msg[1] = IPMI_GET_SYSTEM_INTERFACE_CAPABILITIES_CMD;
+	msg[2] = 0; /* SSIF */
+	rv = do_cmd(client, 3, msg, &len, resp);
+	if (!rv && (len >= 3) && (resp[2] == 0)) {
+		if (len < 7) {
+			if (ssif_dbg_probe)
+				pr_info(PFX "SSIF info too short: %d\n", len);
+			goto no_support;
+		}
+
+		/* Got a good SSIF response, handle it. */
+		ssif_info->max_xmit_msg_size = resp[5];
+		ssif_info->max_recv_msg_size = resp[6];
+		ssif_info->multi_support = (resp[4] >> 6) & 0x3;
+		ssif_info->supports_pec = (resp[4] >> 3) & 0x1;
+
+		/* Sanitize the data */
+		switch (ssif_info->multi_support) {
+		case SSIF_NO_MULTI:
+			if (ssif_info->max_xmit_msg_size > 32)
+				ssif_info->max_xmit_msg_size = 32;
+			if (ssif_info->max_recv_msg_size > 32)
+				ssif_info->max_recv_msg_size = 32;
+			break;
+
+		case SSIF_MULTI_2_PART:
+			if (ssif_info->max_xmit_msg_size > 64)
+				ssif_info->max_xmit_msg_size = 64;
+			if (ssif_info->max_recv_msg_size > 62)
+				ssif_info->max_recv_msg_size = 62;
+			break;
+
+		case SSIF_MULTI_n_PART:
+			break;
+
+		default:
+			/* Data is not sane, just give up. */
+			goto no_support;
+		}
+	} else {
+ no_support:
+		/* Assume no multi-part or PEC support */
+		pr_info(PFX "Error fetching SSIF: %d %d %2.2x, your system probably doesn't support this command so  using defaults\n",
+		       rv, len, resp[2]);
+
+		ssif_info->max_xmit_msg_size = 32;
+		ssif_info->max_recv_msg_size = 32;
+		ssif_info->multi_support = SSIF_NO_MULTI;
+		ssif_info->supports_pec = 0;
+	}
+
+	/* Make sure the NMI timeout is cleared. */
+	msg[0] = IPMI_NETFN_APP_REQUEST << 2;
+	msg[1] = IPMI_CLEAR_MSG_FLAGS_CMD;
+	msg[2] = WDT_PRE_TIMEOUT_INT;
+	rv = do_cmd(client, 3, msg, &len, resp);
+	if (rv || (len < 3) || (resp[2] != 0))
+		pr_warn(PFX "Unable to clear message flags: %d %d %2.2x\n",
+			rv, len, resp[2]);
+
+	/* Attempt to enable the event buffer. */
+	msg[0] = IPMI_NETFN_APP_REQUEST << 2;
+	msg[1] = IPMI_GET_BMC_GLOBAL_ENABLES_CMD;
+	rv = do_cmd(client, 2, msg, &len, resp);
+	if (rv || (len < 4) || (resp[2] != 0)) {
+		pr_warn(PFX "Error getting global enables: %d %d %2.2x\n",
+			rv, len, resp[2]);
+		rv = 0; /* Not fatal */
+		goto found;
+	}
+
+	if (resp[3] & IPMI_BMC_EVT_MSG_BUFF) {
+		ssif_info->has_event_buffer = true;
+		/* buffer is already enabled, nothing to do. */
+		goto found;
+	}
+
+	msg[0] = IPMI_NETFN_APP_REQUEST << 2;
+	msg[1] = IPMI_SET_BMC_GLOBAL_ENABLES_CMD;
+	msg[2] = resp[3] | IPMI_BMC_EVT_MSG_BUFF;
+	rv = do_cmd(client, 3, msg, &len, resp);
+	if (rv || (len < 2)) {
+		pr_warn(PFX "Error getting global enables: %d %d %2.2x\n",
+			rv, len, resp[2]);
+		rv = 0; /* Not fatal */
+		goto found;
+	}
+
+	if (resp[2] == 0)
+		/* A successful return means the event buffer is supported. */
+		ssif_info->has_event_buffer = true;
+
+ found:
+	ssif_info->intf_num = atomic_inc_return(&next_intf);
+
+	if (ssif_dbg_probe) {
+		pr_info("ssif_probe: i2c_probe found device at i2c address %x\n",
+			client->addr);
+	}
+
+	spin_lock_init(&ssif_info->lock);
+	ssif_info->ssif_state = SSIF_NORMAL;
+	init_timer(&ssif_info->retry_timer);
+	ssif_info->retry_timer.data = (unsigned long) ssif_info;
+	ssif_info->retry_timer.function = retry_timeout;
+
+	for (i = 0; i < SSIF_NUM_STATS; i++)
+		atomic_set(&ssif_info->stats[i], 0);
+
+	if (ssif_info->supports_pec)
+		ssif_info->client->flags |= I2C_CLIENT_PEC;
+
+	ssif_info->handlers.owner = THIS_MODULE;
+	ssif_info->handlers.start_processing = ssif_start_processing;
+	ssif_info->handlers.get_smi_info = get_smi_info;
+	ssif_info->handlers.sender = sender;
+	ssif_info->handlers.request_events = request_events;
+	ssif_info->handlers.inc_usecount = inc_usecount;
+	ssif_info->handlers.dec_usecount = dec_usecount;
+
+	{
+		unsigned int thread_num;
+
+		thread_num = ((ssif_info->client->adapter->nr << 8) |
+			      ssif_info->client->addr);
+		init_completion(&ssif_info->wake_thread);
+		ssif_info->thread = kthread_run(ipmi_ssif_thread, ssif_info,
+					       "kssif%4.4x", thread_num);
+		if (IS_ERR(ssif_info->thread)) {
+			rv = PTR_ERR(ssif_info->thread);
+			dev_notice(&ssif_info->client->dev,
+				   "Could not start kernel thread: error %d\n",
+				   rv);
+			goto out;
+		}
+	}
+
+	rv = ipmi_register_smi(&ssif_info->handlers,
+			       ssif_info,
+			       &ssif_info->device_id,
+			       &ssif_info->client->dev,
+			       slave_addr);
+	 if (rv) {
+		pr_err(PFX "Unable to register device: error %d\n", rv);
+		goto out;
+	}
+
+	rv = ipmi_smi_add_proc_entry(ssif_info->intf, "type",
+				     &smi_type_proc_ops,
+				     ssif_info);
+	if (rv) {
+		pr_err(PFX "Unable to create proc entry: %d\n", rv);
+		goto out_err_unreg;
+	}
+
+	rv = ipmi_smi_add_proc_entry(ssif_info->intf, "ssif_stats",
+				     &smi_stats_proc_ops,
+				     ssif_info);
+	if (rv) {
+		pr_err(PFX "Unable to create proc entry: %d\n", rv);
+		goto out_err_unreg;
+	}
+
+ out:
+	if (rv)
+		kfree(ssif_info);
+	kfree(resp);
+	return rv;
+
+ out_err_unreg:
+	ipmi_unregister_smi(ssif_info->intf);
+	goto out;
+}
+
+static int ssif_adapter_handler(struct device *adev, void *opaque)
+{
+	struct ssif_addr_info *addr_info = opaque;
+
+	if (adev->type != &i2c_adapter_type)
+		return 0;
+
+	i2c_new_device(to_i2c_adapter(adev), &addr_info->binfo);
+
+	if (!addr_info->adapter_name)
+		return 1; /* Only try the first I2C adapter by default. */
+	return 0;
+}
+
+static int new_ssif_client(int addr, char *adapter_name,
+			   int debug, int slave_addr,
+			   enum ipmi_addr_src addr_src)
+{
+	struct ssif_addr_info *addr_info;
+	int rv = 0;
+
+	mutex_lock(&ssif_infos_mutex);
+	if (ssif_info_find(addr, adapter_name, false)) {
+		rv = -EEXIST;
+		goto out_unlock;
+	}
+
+	addr_info = kzalloc(sizeof(*addr_info), GFP_KERNEL);
+	if (!addr_info) {
+		rv = -ENOMEM;
+		goto out_unlock;
+	}
+
+	if (adapter_name) {
+		addr_info->adapter_name = kstrdup(adapter_name, GFP_KERNEL);
+		if (!addr_info->adapter_name) {
+			kfree(addr_info);
+			rv = -ENOMEM;
+			goto out_unlock;
+		}
+	}
+
+	strncpy(addr_info->binfo.type, DEVICE_NAME,
+		sizeof(addr_info->binfo.type));
+	addr_info->binfo.addr = addr;
+	addr_info->binfo.platform_data = addr_info;
+	addr_info->debug = debug;
+	addr_info->slave_addr = slave_addr;
+	addr_info->addr_src = addr_src;
+
+	list_add_tail(&addr_info->link, &ssif_infos);
+
+	if (initialized)
+		i2c_for_each_dev(addr_info, ssif_adapter_handler);
+	/* Otherwise address list will get it */
+
+out_unlock:
+	mutex_unlock(&ssif_infos_mutex);
+	return rv;
+}
+
+static void free_ssif_clients(void)
+{
+	struct ssif_addr_info *info, *tmp;
+
+	mutex_lock(&ssif_infos_mutex);
+	list_for_each_entry_safe(info, tmp, &ssif_infos, link) {
+		list_del(&info->link);
+		kfree(info->adapter_name);
+		kfree(info);
+	}
+	mutex_unlock(&ssif_infos_mutex);
+}
+
+static unsigned short *ssif_address_list(void)
+{
+	struct ssif_addr_info *info;
+	unsigned int count = 0, i;
+	unsigned short *address_list;
+
+	list_for_each_entry(info, &ssif_infos, link)
+		count++;
+
+	address_list = kzalloc(sizeof(*address_list) * (count + 1), GFP_KERNEL);
+	if (!address_list)
+		return NULL;
+
+	i = 0;
+	list_for_each_entry(info, &ssif_infos, link) {
+		unsigned short addr = info->binfo.addr;
+		int j;
+
+		for (j = 0; j < i; j++) {
+			if (address_list[j] == addr)
+				goto skip_addr;
+		}
+		address_list[i] = addr;
+skip_addr:
+		i++;
+	}
+	address_list[i] = I2C_CLIENT_END;
+
+	return address_list;
+}
+
+#ifdef CONFIG_ACPI
+static struct acpi_device_id ssif_acpi_match[] = {
+	{ "IPI0001", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(acpi, ssif_acpi_match);
+
+/*
+ * Once we get an ACPI failure, we don't try any more, because we go
+ * through the tables sequentially.  Once we don't find a table, there
+ * are no more.
+ */
+static int acpi_failure;
+
+/*
+ * Defined in the IPMI 2.0 spec.
+ */
+struct SPMITable {
+	s8	Signature[4];
+	u32	Length;
+	u8	Revision;
+	u8	Checksum;
+	s8	OEMID[6];
+	s8	OEMTableID[8];
+	s8	OEMRevision[4];
+	s8	CreatorID[4];
+	s8	CreatorRevision[4];
+	u8	InterfaceType;
+	u8	IPMIlegacy;
+	s16	SpecificationRevision;
+
+	/*
+	 * Bit 0 - SCI interrupt supported
+	 * Bit 1 - I/O APIC/SAPIC
+	 */
+	u8	InterruptType;
+
+	/*
+	 * If bit 0 of InterruptType is set, then this is the SCI
+	 * interrupt in the GPEx_STS register.
+	 */
+	u8	GPE;
+
+	s16	Reserved;
+
+	/*
+	 * If bit 1 of InterruptType is set, then this is the I/O
+	 * APIC/SAPIC interrupt.
+	 */
+	u32	GlobalSystemInterrupt;
+
+	/* The actual register address. */
+	struct acpi_generic_address addr;
+
+	u8	UID[4];
+
+	s8      spmi_id[1]; /* A '\0' terminated array starts here. */
+};
+
+static int try_init_spmi(struct SPMITable *spmi)
+{
+	unsigned short myaddr;
+
+	if (num_addrs >= MAX_SSIF_BMCS)
+		return -1;
+
+	if (spmi->IPMIlegacy != 1) {
+		pr_warn("IPMI: Bad SPMI legacy: %d\n", spmi->IPMIlegacy);
+		return -ENODEV;
+	}
+
+	if (spmi->InterfaceType != 4)
+		return -ENODEV;
+
+	if (spmi->addr.space_id != ACPI_ADR_SPACE_SMBUS) {
+		pr_warn(PFX "Invalid ACPI SSIF I/O Address type: %d\n",
+			spmi->addr.space_id);
+		return -EIO;
+	}
+
+	myaddr = spmi->addr.address >> 1;
+
+	return new_ssif_client(myaddr, NULL, 0, 0, SI_SPMI);
+}
+
+static void spmi_find_bmc(void)
+{
+	acpi_status      status;
+	struct SPMITable *spmi;
+	int              i;
+
+	if (acpi_disabled)
+		return;
+
+	if (acpi_failure)
+		return;
+
+	for (i = 0; ; i++) {
+		status = acpi_get_table(ACPI_SIG_SPMI, i+1,
+					(struct acpi_table_header **)&spmi);
+		if (status != AE_OK)
+			return;
+
+		try_init_spmi(spmi);
+	}
+}
+#else
+static void spmi_find_bmc(void) { }
+#endif
+
+#ifdef CONFIG_DMI
+static int decode_dmi(const struct dmi_device *dmi_dev)
+{
+	struct dmi_header *dm = dmi_dev->device_data;
+	u8             *data = (u8 *) dm;
+	u8             len = dm->length;
+	unsigned short myaddr;
+	int            slave_addr;
+
+	if (num_addrs >= MAX_SSIF_BMCS)
+		return -1;
+
+	if (len < 9)
+		return -1;
+
+	if (data[0x04] != 4) /* Not SSIF */
+		return -1;
+
+	if ((data[8] >> 1) == 0) {
+		/*
+		 * Some broken systems put the I2C address in
+		 * the slave address field.  We try to
+		 * accommodate them here.
+		 */
+		myaddr = data[6] >> 1;
+		slave_addr = 0;
+	} else {
+		myaddr = data[8] >> 1;
+		slave_addr = data[6];
+	}
+
+	return new_ssif_client(myaddr, NULL, 0, 0, SI_SMBIOS);
+}
+
+static void dmi_iterator(void)
+{
+	const struct dmi_device *dev = NULL;
+
+	while ((dev = dmi_find_device(DMI_DEV_TYPE_IPMI, NULL, dev)))
+		decode_dmi(dev);
+}
+#else
+static void dmi_iterator(void) { }
+#endif
+
+static const struct i2c_device_id ssif_id[] = {
+	{ DEVICE_NAME, 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ssif_id);
+
+static struct i2c_driver ssif_i2c_driver = {
+	.class		= I2C_CLASS_HWMON,
+	.driver		= {
+		.owner			= THIS_MODULE,
+		.name			= DEVICE_NAME
+	},
+	.probe		= ssif_probe,
+	.remove		= ssif_remove,
+	.id_table	= ssif_id,
+	.detect		= ssif_detect
+};
+
+static int init_ipmi_ssif(void)
+{
+	int i;
+	int rv;
+
+	if (initialized)
+		return 0;
+
+	pr_info("IPMI SSIF Interface driver\n");
+
+	/* build list for i2c from addr list */
+	for (i = 0; i < num_addrs; i++) {
+		rv = new_ssif_client(addr[i], adapter_name[i],
+				     dbg[i], slave_addrs[i],
+				     SI_HARDCODED);
+		if (!rv)
+			pr_err(PFX
+			       "Couldn't add hardcoded device at addr 0x%x\n",
+			       addr[i]);
+	}
+
+	if (ssif_tryacpi)
+		ssif_i2c_driver.driver.acpi_match_table	=
+			ACPI_PTR(ssif_acpi_match);
+	if (ssif_trydmi)
+		dmi_iterator();
+	if (ssif_tryacpi)
+		spmi_find_bmc();
+
+	ssif_i2c_driver.address_list = ssif_address_list();
+
+	rv = i2c_add_driver(&ssif_i2c_driver);
+	if (!rv)
+		initialized = true;
+
+	return rv;
+}
+module_init(init_ipmi_ssif);
+
+static void cleanup_ipmi_ssif(void)
+{
+	if (!initialized)
+		return;
+
+	initialized = false;
+
+	i2c_del_driver(&ssif_i2c_driver);
+
+	free_ssif_clients();
+}
+module_exit(cleanup_ipmi_ssif);
+
+MODULE_AUTHOR("Todd C Davis <todd.c.davis@intel.com>, Corey Minyard <minyard@acm.org>");
+MODULE_DESCRIPTION("IPMI driver for management controllers on a SMBus");
+MODULE_LICENSE("GPL");
diff --git a/drivers/char/sonypi.c b/drivers/char/sonypi.c
index 7cc1fe2241fd..e496daefe9e0 100644
--- a/drivers/char/sonypi.c
+++ b/drivers/char/sonypi.c
@@ -1482,7 +1482,6 @@ static void sonypi_shutdown(struct platform_device *dev)
 static struct platform_driver sonypi_driver = {
 	.driver		= {
 		.name	= "sonypi",
-		.owner	= THIS_MODULE,
 		.pm	= SONYPI_PM,
 	},
 	.probe		= sonypi_probe,
diff --git a/drivers/char/tb0219.c b/drivers/char/tb0219.c
index 47b9fdfcf083..480a777db577 100644
--- a/drivers/char/tb0219.c
+++ b/drivers/char/tb0219.c
@@ -337,7 +337,6 @@ static struct platform_driver tb0219_device_driver = {
 	.remove		= tb0219_remove,
 	.driver		= {
 		.name	= "TB0219",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/char/tpm/tpm_atmel.c b/drivers/char/tpm/tpm_atmel.c
index 6069d13ae4ac..435c8b9dd2f8 100644
--- a/drivers/char/tpm/tpm_atmel.c
+++ b/drivers/char/tpm/tpm_atmel.c
@@ -152,7 +152,6 @@ static SIMPLE_DEV_PM_OPS(tpm_atml_pm, tpm_pm_suspend, tpm_pm_resume);
 static struct platform_driver atml_drv = {
 	.driver = {
 		.name = "tpm_atmel",
-		.owner		= THIS_MODULE,
 		.pm		= &tpm_atml_pm,
 	},
 };
diff --git a/drivers/char/tpm/tpm_nsc.c b/drivers/char/tpm/tpm_nsc.c
index 3179ec9cffdc..4d0a17ea8cde 100644
--- a/drivers/char/tpm/tpm_nsc.c
+++ b/drivers/char/tpm/tpm_nsc.c
@@ -258,7 +258,6 @@ static SIMPLE_DEV_PM_OPS(tpm_nsc_pm, tpm_pm_suspend, tpm_pm_resume);
 static struct platform_driver nsc_drv = {
 	.driver          = {
 		.name    = "tpm_nsc",
-		.owner   = THIS_MODULE,
 		.pm      = &tpm_nsc_pm,
 	},
 };
diff --git a/drivers/char/tpm/tpm_tis.c b/drivers/char/tpm/tpm_tis.c
index 2c46734b266d..6f1985496112 100644
--- a/drivers/char/tpm/tpm_tis.c
+++ b/drivers/char/tpm/tpm_tis.c
@@ -837,7 +837,6 @@ MODULE_PARM_DESC(hid, "Set additional specific HID for this driver to probe");
 static struct platform_driver tis_drv = {
 	.driver = {
 		.name = "tpm_tis",
-		.owner		= THIS_MODULE,
 		.pm		= &tpm_tis_pm,
 	},
 };
diff --git a/drivers/char/xillybus/xillybus_of.c b/drivers/char/xillybus/xillybus_of.c
index 1ca0c7a4f1be..2002a3a28146 100644
--- a/drivers/char/xillybus/xillybus_of.c
+++ b/drivers/char/xillybus/xillybus_of.c
@@ -179,7 +179,6 @@ static struct platform_driver xillybus_platform_driver = {
 	.remove = xilly_drv_remove,
 	.driver = {
 		.name = xillyname,
-		.owner = THIS_MODULE,
 		.of_match_table = xillybus_of_match,
 	},
 };
diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig
index 455fd17d938e..3f44f292d066 100644
--- a/drivers/clk/Kconfig
+++ b/drivers/clk/Kconfig
@@ -28,7 +28,7 @@ config COMMON_CLK_WM831X
 	depends on MFD_WM831X
 	---help---
           Supports the clocking subsystem of the WM831x/2x series of
-	  PMICs from Wolfson Microlectronics.
+	  PMICs from Wolfson Microelectronics.
 
 source "drivers/clk/versatile/Kconfig"
 
diff --git a/drivers/clk/at91/clk-programmable.c b/drivers/clk/at91/clk-programmable.c
index 62e2509f9df1..bbdb1b985c91 100644
--- a/drivers/clk/at91/clk-programmable.c
+++ b/drivers/clk/at91/clk-programmable.c
@@ -57,7 +57,7 @@ static unsigned long clk_programmable_recalc_rate(struct clk_hw *hw,
 static long clk_programmable_determine_rate(struct clk_hw *hw,
 					    unsigned long rate,
 					    unsigned long *best_parent_rate,
-					    struct clk **best_parent_clk)
+					    struct clk_hw **best_parent_hw)
 {
 	struct clk *parent = NULL;
 	long best_rate = -EINVAL;
@@ -84,7 +84,7 @@ static long clk_programmable_determine_rate(struct clk_hw *hw,
 		if (best_rate < 0 || (rate - tmp_rate) < (rate - best_rate)) {
 			best_rate = tmp_rate;
 			*best_parent_rate = parent_rate;
-			*best_parent_clk = parent;
+			*best_parent_hw = __clk_get_hw(parent);
 		}
 
 		if (!best_rate)
diff --git a/drivers/clk/bcm/clk-kona.c b/drivers/clk/bcm/clk-kona.c
index 95af2e665dd3..1c06f6f3a8c5 100644
--- a/drivers/clk/bcm/clk-kona.c
+++ b/drivers/clk/bcm/clk-kona.c
@@ -1032,7 +1032,7 @@ static long kona_peri_clk_round_rate(struct clk_hw *hw, unsigned long rate,
 }
 
 static long kona_peri_clk_determine_rate(struct clk_hw *hw, unsigned long rate,
-		unsigned long *best_parent_rate, struct clk **best_parent)
+		unsigned long *best_parent_rate, struct clk_hw **best_parent)
 {
 	struct kona_clk *bcm_clk = to_kona_clk(hw);
 	struct clk *clk = hw->clk;
@@ -1075,7 +1075,7 @@ static long kona_peri_clk_determine_rate(struct clk_hw *hw, unsigned long rate,
 		if (delta < best_delta) {
 			best_delta = delta;
 			best_rate = other_rate;
-			*best_parent = parent;
+			*best_parent = __clk_get_hw(parent);
 			*best_parent_rate = parent_rate;
 		}
 	}
diff --git a/drivers/clk/clk-axm5516.c b/drivers/clk/clk-axm5516.c
index d2f1e119b450..0f6368ceec4c 100644
--- a/drivers/clk/clk-axm5516.c
+++ b/drivers/clk/clk-axm5516.c
@@ -593,7 +593,6 @@ static struct platform_driver axmclk_driver = {
 	.remove		= axmclk_remove,
 	.driver		= {
 		.name	= "clk-axm5516",
-		.owner	= THIS_MODULE,
 		.of_match_table = axmclk_match_table,
 	},
 };
diff --git a/drivers/clk/clk-composite.c b/drivers/clk/clk-composite.c
index b9355daf8065..4386697236a7 100644
--- a/drivers/clk/clk-composite.c
+++ b/drivers/clk/clk-composite.c
@@ -57,7 +57,7 @@ static unsigned long clk_composite_recalc_rate(struct clk_hw *hw,
 
 static long clk_composite_determine_rate(struct clk_hw *hw, unsigned long rate,
 					unsigned long *best_parent_rate,
-					struct clk **best_parent_p)
+					struct clk_hw **best_parent_p)
 {
 	struct clk_composite *composite = to_clk_composite(hw);
 	const struct clk_ops *rate_ops = composite->rate_ops;
@@ -80,8 +80,9 @@ static long clk_composite_determine_rate(struct clk_hw *hw, unsigned long rate,
 		*best_parent_p = NULL;
 
 		if (__clk_get_flags(hw->clk) & CLK_SET_RATE_NO_REPARENT) {
-			*best_parent_p = clk_get_parent(mux_hw->clk);
-			*best_parent_rate = __clk_get_rate(*best_parent_p);
+			parent = clk_get_parent(mux_hw->clk);
+			*best_parent_p = __clk_get_hw(parent);
+			*best_parent_rate = __clk_get_rate(parent);
 
 			return rate_ops->round_rate(rate_hw, rate,
 						    best_parent_rate);
@@ -103,7 +104,7 @@ static long clk_composite_determine_rate(struct clk_hw *hw, unsigned long rate,
 
 			if (!rate_diff || !*best_parent_p
 				       || best_rate_diff > rate_diff) {
-				*best_parent_p = parent;
+				*best_parent_p = __clk_get_hw(parent);
 				*best_parent_rate = parent_rate;
 				best_rate_diff = rate_diff;
 				best_rate = tmp_rate;
diff --git a/drivers/clk/clk-ls1x.c b/drivers/clk/clk-ls1x.c
index f20b750235f6..ca80103ac188 100644
--- a/drivers/clk/clk-ls1x.c
+++ b/drivers/clk/clk-ls1x.c
@@ -15,7 +15,8 @@
 
 #include <loongson1.h>
 
-#define OSC	33
+#define OSC		(33 * 1000000)
+#define DIV_APB		2
 
 static DEFINE_SPINLOCK(_lock);
 
@@ -29,13 +30,12 @@ static void ls1x_pll_clk_disable(struct clk_hw *hw)
 }
 
 static unsigned long ls1x_pll_recalc_rate(struct clk_hw *hw,
-					     unsigned long parent_rate)
+					  unsigned long parent_rate)
 {
 	u32 pll, rate;
 
 	pll = __raw_readl(LS1X_CLK_PLL_FREQ);
-	rate = ((12 + (pll & 0x3f)) * 1000000) +
-		((((pll >> 8) & 0x3ff) * 1000000) >> 10);
+	rate = 12 + (pll & 0x3f) + (((pll >> 8) & 0x3ff) >> 10);
 	rate *= OSC;
 	rate >>= 1;
 
@@ -48,8 +48,10 @@ static const struct clk_ops ls1x_pll_clk_ops = {
 	.recalc_rate = ls1x_pll_recalc_rate,
 };
 
-static struct clk * __init clk_register_pll(struct device *dev,
-	 const char *name, const char *parent_name, unsigned long flags)
+static struct clk *__init clk_register_pll(struct device *dev,
+					   const char *name,
+					   const char *parent_name,
+					   unsigned long flags)
 {
 	struct clk_hw *hw;
 	struct clk *clk;
@@ -78,34 +80,83 @@ static struct clk * __init clk_register_pll(struct device *dev,
 	return clk;
 }
 
+static const char const *cpu_parents[] = { "cpu_clk_div", "osc_33m_clk", };
+static const char const *ahb_parents[] = { "ahb_clk_div", "osc_33m_clk", };
+static const char const *dc_parents[] = { "dc_clk_div", "osc_33m_clk", };
+
 void __init ls1x_clk_init(void)
 {
 	struct clk *clk;
 
-	clk = clk_register_pll(NULL, "pll_clk", NULL, CLK_IS_ROOT);
-	clk_prepare_enable(clk);
-
-	clk = clk_register_divider(NULL, "cpu_clk", "pll_clk",
-			CLK_SET_RATE_PARENT, LS1X_CLK_PLL_DIV, DIV_CPU_SHIFT,
-			DIV_CPU_WIDTH, CLK_DIVIDER_ONE_BASED, &_lock);
-	clk_prepare_enable(clk);
-	clk_register_clkdev(clk, "cpu", NULL);
-
-	clk = clk_register_divider(NULL, "dc_clk", "pll_clk",
-			CLK_SET_RATE_PARENT, LS1X_CLK_PLL_DIV, DIV_DC_SHIFT,
-			DIV_DC_WIDTH, CLK_DIVIDER_ONE_BASED, &_lock);
-	clk_prepare_enable(clk);
-	clk_register_clkdev(clk, "dc", NULL);
-
-	clk = clk_register_divider(NULL, "ahb_clk", "pll_clk",
-			CLK_SET_RATE_PARENT, LS1X_CLK_PLL_DIV, DIV_DDR_SHIFT,
-			DIV_DDR_WIDTH, CLK_DIVIDER_ONE_BASED, &_lock);
-	clk_prepare_enable(clk);
-	clk_register_clkdev(clk, "ahb", NULL);
+	clk = clk_register_fixed_rate(NULL, "osc_33m_clk", NULL, CLK_IS_ROOT,
+				      OSC);
+	clk_register_clkdev(clk, "osc_33m_clk", NULL);
+
+	/* clock derived from 33 MHz OSC clk */
+	clk = clk_register_pll(NULL, "pll_clk", "osc_33m_clk", 0);
+	clk_register_clkdev(clk, "pll_clk", NULL);
+
+	/* clock derived from PLL clk */
+	/*                                 _____
+	 *         _______________________|     |
+	 * OSC ___/                       | MUX |___ CPU CLK
+	 *        \___ PLL ___ CPU DIV ___|     |
+	 *                                |_____|
+	 */
+	clk = clk_register_divider(NULL, "cpu_clk_div", "pll_clk",
+				   CLK_GET_RATE_NOCACHE, LS1X_CLK_PLL_DIV,
+				   DIV_CPU_SHIFT, DIV_CPU_WIDTH,
+				   CLK_DIVIDER_ONE_BASED |
+				   CLK_DIVIDER_ROUND_CLOSEST, &_lock);
+	clk_register_clkdev(clk, "cpu_clk_div", NULL);
+	clk = clk_register_mux(NULL, "cpu_clk", cpu_parents,
+			       ARRAY_SIZE(cpu_parents),
+			       CLK_SET_RATE_NO_REPARENT, LS1X_CLK_PLL_DIV,
+			       BYPASS_CPU_SHIFT, BYPASS_CPU_WIDTH, 0, &_lock);
+	clk_register_clkdev(clk, "cpu_clk", NULL);
+
+	/*                                 _____
+	 *         _______________________|     |
+	 * OSC ___/                       | MUX |___ DC  CLK
+	 *        \___ PLL ___ DC  DIV ___|     |
+	 *                                |_____|
+	 */
+	clk = clk_register_divider(NULL, "dc_clk_div", "pll_clk",
+				   0, LS1X_CLK_PLL_DIV, DIV_DC_SHIFT,
+				   DIV_DC_WIDTH, CLK_DIVIDER_ONE_BASED, &_lock);
+	clk_register_clkdev(clk, "dc_clk_div", NULL);
+	clk = clk_register_mux(NULL, "dc_clk", dc_parents,
+			       ARRAY_SIZE(dc_parents),
+			       CLK_SET_RATE_NO_REPARENT, LS1X_CLK_PLL_DIV,
+			       BYPASS_DC_SHIFT, BYPASS_DC_WIDTH, 0, &_lock);
+	clk_register_clkdev(clk, "dc_clk", NULL);
+
+	/*                                 _____
+	 *         _______________________|     |
+	 * OSC ___/                       | MUX |___ DDR CLK
+	 *        \___ PLL ___ DDR DIV ___|     |
+	 *                                |_____|
+	 */
+	clk = clk_register_divider(NULL, "ahb_clk_div", "pll_clk",
+				   0, LS1X_CLK_PLL_DIV, DIV_DDR_SHIFT,
+				   DIV_DDR_WIDTH, CLK_DIVIDER_ONE_BASED,
+				   &_lock);
+	clk_register_clkdev(clk, "ahb_clk_div", NULL);
+	clk = clk_register_mux(NULL, "ahb_clk", ahb_parents,
+			       ARRAY_SIZE(ahb_parents),
+			       CLK_SET_RATE_NO_REPARENT, LS1X_CLK_PLL_DIV,
+			       BYPASS_DDR_SHIFT, BYPASS_DDR_WIDTH, 0, &_lock);
+	clk_register_clkdev(clk, "ahb_clk", NULL);
 	clk_register_clkdev(clk, "stmmaceth", NULL);
 
-	clk = clk_register_fixed_factor(NULL, "apb_clk", "ahb_clk", 0, 1, 2);
-	clk_prepare_enable(clk);
-	clk_register_clkdev(clk, "apb", NULL);
+	/* clock derived from AHB clk */
+	/* APB clk is always half of the AHB clk */
+	clk = clk_register_fixed_factor(NULL, "apb_clk", "ahb_clk", 0, 1,
+					DIV_APB);
+	clk_register_clkdev(clk, "apb_clk", NULL);
+	clk_register_clkdev(clk, "ls1x_i2c", NULL);
+	clk_register_clkdev(clk, "ls1x_pwmtimer", NULL);
+	clk_register_clkdev(clk, "ls1x_spi", NULL);
+	clk_register_clkdev(clk, "ls1x_wdt", NULL);
 	clk_register_clkdev(clk, "serial8250", NULL);
 }
diff --git a/drivers/clk/clk-mux.c b/drivers/clk/clk-mux.c
index 4f96ff3ba728..6e1ecf94bf58 100644
--- a/drivers/clk/clk-mux.c
+++ b/drivers/clk/clk-mux.c
@@ -77,7 +77,7 @@ static int clk_mux_set_parent(struct clk_hw *hw, u8 index)
 
 	else {
 		if (mux->flags & CLK_MUX_INDEX_BIT)
-			index = (1 << ffs(index));
+			index = 1 << index;
 
 		if (mux->flags & CLK_MUX_INDEX_ONE)
 			index++;
diff --git a/drivers/clk/clk-ppc-corenet.c b/drivers/clk/clk-ppc-corenet.c
index 8e58edfeeb37..b6e6c85507a5 100644
--- a/drivers/clk/clk-ppc-corenet.c
+++ b/drivers/clk/clk-ppc-corenet.c
@@ -294,7 +294,6 @@ static const struct of_device_id ppc_clk_ids[] __initconst = {
 static struct platform_driver ppc_corenet_clk_driver __initdata = {
 	.driver = {
 		.name = "ppc_corenet_clock",
-		.owner = THIS_MODULE,
 		.of_match_table = ppc_clk_ids,
 	},
 	.probe = ppc_corenet_clk_probe,
diff --git a/drivers/clk/clk-s2mps11.c b/drivers/clk/clk-s2mps11.c
index 7bb13af8e214..bfa1e64e267d 100644
--- a/drivers/clk/clk-s2mps11.c
+++ b/drivers/clk/clk-s2mps11.c
@@ -218,7 +218,7 @@ static int s2mps11_clk_probe(struct platform_device *pdev)
 	default:
 		dev_err(&pdev->dev, "Invalid device type\n");
 		return -EINVAL;
-	};
+	}
 
 	/* Store clocks of_node in first element of s2mps11_clks array */
 	s2mps11_clks->clk_np = s2mps11_clk_parse_dt(pdev, clks_init);
@@ -312,7 +312,6 @@ MODULE_DEVICE_TABLE(platform, s2mps11_clk_id);
 static struct platform_driver s2mps11_clk_driver = {
 	.driver = {
 		.name  = "s2mps11-clk",
-		.owner = THIS_MODULE,
 	},
 	.probe = s2mps11_clk_probe,
 	.remove = s2mps11_clk_remove,
diff --git a/drivers/clk/clk.c b/drivers/clk/clk.c
index 4896ae9e23da..f4963b7d4e17 100644
--- a/drivers/clk/clk.c
+++ b/drivers/clk/clk.c
@@ -240,7 +240,6 @@ static const struct file_operations clk_dump_fops = {
 	.release	= single_release,
 };
 
-/* caller must hold prepare_lock */
 static int clk_debug_create_one(struct clk *clk, struct dentry *pdentry)
 {
 	struct dentry *d;
@@ -354,13 +353,13 @@ out:
 	mutex_unlock(&clk_debug_lock);
 }
 
-struct dentry *clk_debugfs_add_file(struct clk *clk, char *name, umode_t mode,
+struct dentry *clk_debugfs_add_file(struct clk_hw *hw, char *name, umode_t mode,
 				void *data, const struct file_operations *fops)
 {
 	struct dentry *d = NULL;
 
-	if (clk->dentry)
-		d = debugfs_create_file(name, mode, clk->dentry, data, fops);
+	if (hw->clk->dentry)
+		d = debugfs_create_file(name, mode, hw->clk->dentry, data, fops);
 
 	return d;
 }
@@ -574,11 +573,6 @@ unsigned int __clk_get_enable_count(struct clk *clk)
 	return !clk ? 0 : clk->enable_count;
 }
 
-unsigned int __clk_get_prepare_count(struct clk *clk)
-{
-	return !clk ? 0 : clk->prepare_count;
-}
-
 unsigned long __clk_get_rate(struct clk *clk)
 {
 	unsigned long ret;
@@ -601,7 +595,7 @@ out:
 }
 EXPORT_SYMBOL_GPL(__clk_get_rate);
 
-unsigned long __clk_get_accuracy(struct clk *clk)
+static unsigned long __clk_get_accuracy(struct clk *clk)
 {
 	if (!clk)
 		return 0;
@@ -707,7 +701,7 @@ struct clk *__clk_lookup(const char *name)
  */
 long __clk_mux_determine_rate(struct clk_hw *hw, unsigned long rate,
 			      unsigned long *best_parent_rate,
-			      struct clk **best_parent_p)
+			      struct clk_hw **best_parent_p)
 {
 	struct clk *clk = hw->clk, *parent, *best_parent = NULL;
 	int i, num_parents;
@@ -743,7 +737,7 @@ long __clk_mux_determine_rate(struct clk_hw *hw, unsigned long rate,
 
 out:
 	if (best_parent)
-		*best_parent_p = best_parent;
+		*best_parent_p = best_parent->hw;
 	*best_parent_rate = best;
 
 	return best;
@@ -951,6 +945,7 @@ unsigned long __clk_round_rate(struct clk *clk, unsigned long rate)
 {
 	unsigned long parent_rate = 0;
 	struct clk *parent;
+	struct clk_hw *parent_hw;
 
 	if (!clk)
 		return 0;
@@ -959,10 +954,11 @@ unsigned long __clk_round_rate(struct clk *clk, unsigned long rate)
 	if (parent)
 		parent_rate = parent->rate;
 
-	if (clk->ops->determine_rate)
+	if (clk->ops->determine_rate) {
+		parent_hw = parent ? parent->hw : NULL;
 		return clk->ops->determine_rate(clk->hw, rate, &parent_rate,
-						&parent);
-	else if (clk->ops->round_rate)
+						&parent_hw);
+	} else if (clk->ops->round_rate)
 		return clk->ops->round_rate(clk->hw, rate, &parent_rate);
 	else if (clk->flags & CLK_SET_RATE_PARENT)
 		return __clk_round_rate(clk->parent, rate);
@@ -1350,6 +1346,7 @@ static struct clk *clk_calc_new_rates(struct clk *clk, unsigned long rate)
 {
 	struct clk *top = clk;
 	struct clk *old_parent, *parent;
+	struct clk_hw *parent_hw;
 	unsigned long best_parent_rate = 0;
 	unsigned long new_rate;
 	int p_index = 0;
@@ -1365,9 +1362,11 @@ static struct clk *clk_calc_new_rates(struct clk *clk, unsigned long rate)
 
 	/* find the closest rate and parent clk/rate */
 	if (clk->ops->determine_rate) {
+		parent_hw = parent ? parent->hw : NULL;
 		new_rate = clk->ops->determine_rate(clk->hw, rate,
 						    &best_parent_rate,
-						    &parent);
+						    &parent_hw);
+		parent = parent_hw->clk;
 	} else if (clk->ops->round_rate) {
 		new_rate = clk->ops->round_rate(clk->hw, rate,
 						&best_parent_rate);
@@ -1614,7 +1613,7 @@ static struct clk *__clk_init_parent(struct clk *clk)
 
 	if (clk->num_parents == 1) {
 		if (IS_ERR_OR_NULL(clk->parent))
-			ret = clk->parent = __clk_lookup(clk->parent_names[0]);
+			clk->parent = __clk_lookup(clk->parent_names[0]);
 		ret = clk->parent;
 		goto out;
 	}
@@ -1944,7 +1943,6 @@ int __clk_init(struct device *dev, struct clk *clk)
 	else
 		clk->rate = 0;
 
-	clk_debug_register(clk);
 	/*
 	 * walk the list of orphan clocks and reparent any that are children of
 	 * this clock
@@ -1979,6 +1977,9 @@ int __clk_init(struct device *dev, struct clk *clk)
 out:
 	clk_prepare_unlock();
 
+	if (!ret)
+		clk_debug_register(clk);
+
 	return ret;
 }
 
@@ -2273,14 +2274,17 @@ int __clk_get(struct clk *clk)
 
 void __clk_put(struct clk *clk)
 {
+	struct module *owner;
+
 	if (!clk || WARN_ON_ONCE(IS_ERR(clk)))
 		return;
 
 	clk_prepare_lock();
+	owner = clk->owner;
 	kref_put(&clk->ref, __clk_release);
 	clk_prepare_unlock();
 
-	module_put(clk->owner);
+	module_put(owner);
 }
 
 /***        clk rate change notifiers        ***/
diff --git a/drivers/clk/hisilicon/clk-hi3620.c b/drivers/clk/hisilicon/clk-hi3620.c
index 339945d2503b..007144f81f50 100644
--- a/drivers/clk/hisilicon/clk-hi3620.c
+++ b/drivers/clk/hisilicon/clk-hi3620.c
@@ -38,44 +38,44 @@
 #include "clk.h"
 
 /* clock parent list */
-static const char *timer0_mux_p[] __initdata = { "osc32k", "timerclk01", };
-static const char *timer1_mux_p[] __initdata = { "osc32k", "timerclk01", };
-static const char *timer2_mux_p[] __initdata = { "osc32k", "timerclk23", };
-static const char *timer3_mux_p[] __initdata = { "osc32k", "timerclk23", };
-static const char *timer4_mux_p[] __initdata = { "osc32k", "timerclk45", };
-static const char *timer5_mux_p[] __initdata = { "osc32k", "timerclk45", };
-static const char *timer6_mux_p[] __initdata = { "osc32k", "timerclk67", };
-static const char *timer7_mux_p[] __initdata = { "osc32k", "timerclk67", };
-static const char *timer8_mux_p[] __initdata = { "osc32k", "timerclk89", };
-static const char *timer9_mux_p[] __initdata = { "osc32k", "timerclk89", };
-static const char *uart0_mux_p[] __initdata = { "osc26m", "pclk", };
-static const char *uart1_mux_p[] __initdata = { "osc26m", "pclk", };
-static const char *uart2_mux_p[] __initdata = { "osc26m", "pclk", };
-static const char *uart3_mux_p[] __initdata = { "osc26m", "pclk", };
-static const char *uart4_mux_p[] __initdata = { "osc26m", "pclk", };
-static const char *spi0_mux_p[] __initdata = { "osc26m", "rclk_cfgaxi", };
-static const char *spi1_mux_p[] __initdata = { "osc26m", "rclk_cfgaxi", };
-static const char *spi2_mux_p[] __initdata = { "osc26m", "rclk_cfgaxi", };
+static const char *timer0_mux_p[] __initconst = { "osc32k", "timerclk01", };
+static const char *timer1_mux_p[] __initconst = { "osc32k", "timerclk01", };
+static const char *timer2_mux_p[] __initconst = { "osc32k", "timerclk23", };
+static const char *timer3_mux_p[] __initconst = { "osc32k", "timerclk23", };
+static const char *timer4_mux_p[] __initconst = { "osc32k", "timerclk45", };
+static const char *timer5_mux_p[] __initconst = { "osc32k", "timerclk45", };
+static const char *timer6_mux_p[] __initconst = { "osc32k", "timerclk67", };
+static const char *timer7_mux_p[] __initconst = { "osc32k", "timerclk67", };
+static const char *timer8_mux_p[] __initconst = { "osc32k", "timerclk89", };
+static const char *timer9_mux_p[] __initconst = { "osc32k", "timerclk89", };
+static const char *uart0_mux_p[] __initconst = { "osc26m", "pclk", };
+static const char *uart1_mux_p[] __initconst = { "osc26m", "pclk", };
+static const char *uart2_mux_p[] __initconst = { "osc26m", "pclk", };
+static const char *uart3_mux_p[] __initconst = { "osc26m", "pclk", };
+static const char *uart4_mux_p[] __initconst = { "osc26m", "pclk", };
+static const char *spi0_mux_p[] __initconst = { "osc26m", "rclk_cfgaxi", };
+static const char *spi1_mux_p[] __initconst = { "osc26m", "rclk_cfgaxi", };
+static const char *spi2_mux_p[] __initconst = { "osc26m", "rclk_cfgaxi", };
 /* share axi parent */
-static const char *saxi_mux_p[] __initdata = { "armpll3", "armpll2", };
-static const char *pwm0_mux_p[] __initdata = { "osc32k", "osc26m", };
-static const char *pwm1_mux_p[] __initdata = { "osc32k", "osc26m", };
-static const char *sd_mux_p[] __initdata = { "armpll2", "armpll3", };
-static const char *mmc1_mux_p[] __initdata = { "armpll2", "armpll3", };
-static const char *mmc1_mux2_p[] __initdata = { "osc26m", "mmc1_div", };
-static const char *g2d_mux_p[] __initdata = { "armpll2", "armpll3", };
-static const char *venc_mux_p[] __initdata = { "armpll2", "armpll3", };
-static const char *vdec_mux_p[] __initdata = { "armpll2", "armpll3", };
-static const char *vpp_mux_p[] __initdata = { "armpll2", "armpll3", };
-static const char *edc0_mux_p[] __initdata = { "armpll2", "armpll3", };
-static const char *ldi0_mux_p[] __initdata = { "armpll2", "armpll4",
+static const char *saxi_mux_p[] __initconst = { "armpll3", "armpll2", };
+static const char *pwm0_mux_p[] __initconst = { "osc32k", "osc26m", };
+static const char *pwm1_mux_p[] __initconst = { "osc32k", "osc26m", };
+static const char *sd_mux_p[] __initconst = { "armpll2", "armpll3", };
+static const char *mmc1_mux_p[] __initconst = { "armpll2", "armpll3", };
+static const char *mmc1_mux2_p[] __initconst = { "osc26m", "mmc1_div", };
+static const char *g2d_mux_p[] __initconst = { "armpll2", "armpll3", };
+static const char *venc_mux_p[] __initconst = { "armpll2", "armpll3", };
+static const char *vdec_mux_p[] __initconst = { "armpll2", "armpll3", };
+static const char *vpp_mux_p[] __initconst = { "armpll2", "armpll3", };
+static const char *edc0_mux_p[] __initconst = { "armpll2", "armpll3", };
+static const char *ldi0_mux_p[] __initconst = { "armpll2", "armpll4",
 					     "armpll3", "armpll5", };
-static const char *edc1_mux_p[] __initdata = { "armpll2", "armpll3", };
-static const char *ldi1_mux_p[] __initdata = { "armpll2", "armpll4",
+static const char *edc1_mux_p[] __initconst = { "armpll2", "armpll3", };
+static const char *ldi1_mux_p[] __initconst = { "armpll2", "armpll4",
 					     "armpll3", "armpll5", };
-static const char *rclk_hsic_p[] __initdata = { "armpll3", "armpll2", };
-static const char *mmc2_mux_p[] __initdata = { "armpll2", "armpll3", };
-static const char *mmc3_mux_p[] __initdata = { "armpll2", "armpll3", };
+static const char *rclk_hsic_p[] __initconst = { "armpll3", "armpll2", };
+static const char *mmc2_mux_p[] __initconst = { "armpll2", "armpll3", };
+static const char *mmc3_mux_p[] __initconst = { "armpll2", "armpll3", };
 
 
 /* fixed rate clocks */
@@ -296,7 +296,7 @@ static unsigned long mmc_clk_recalc_rate(struct clk_hw *hw,
 
 static long mmc_clk_determine_rate(struct clk_hw *hw, unsigned long rate,
 			      unsigned long *best_parent_rate,
-			      struct clk **best_parent_p)
+			      struct clk_hw **best_parent_p)
 {
 	struct clk_mmc *mclk = to_mmc(hw);
 	unsigned long best = 0;
diff --git a/drivers/clk/mmp/Makefile b/drivers/clk/mmp/Makefile
index 392d78044ce3..3caaf7cc169c 100644
--- a/drivers/clk/mmp/Makefile
+++ b/drivers/clk/mmp/Makefile
@@ -2,7 +2,12 @@
 # Makefile for mmp specific clk
 #
 
-obj-y += clk-apbc.o clk-apmu.o clk-frac.o
+obj-y += clk-apbc.o clk-apmu.o clk-frac.o clk-mix.o clk-gate.o clk.o
+
+obj-$(CONFIG_RESET_CONTROLLER) += reset.o
+
+obj-$(CONFIG_MACH_MMP_DT) += clk-of-pxa168.o clk-of-pxa910.o
+obj-$(CONFIG_MACH_MMP2_DT) += clk-of-mmp2.o
 
 obj-$(CONFIG_CPU_PXA168) += clk-pxa168.o
 obj-$(CONFIG_CPU_PXA910) += clk-pxa910.o
diff --git a/drivers/clk/mmp/clk-frac.c b/drivers/clk/mmp/clk-frac.c
index 23a56f561812..584a9927993b 100644
--- a/drivers/clk/mmp/clk-frac.c
+++ b/drivers/clk/mmp/clk-frac.c
@@ -22,19 +22,12 @@
  * numerator/denominator = Fin / (Fout * factor)
  */
 
-#define to_clk_factor(hw) container_of(hw, struct clk_factor, hw)
-struct clk_factor {
-	struct clk_hw		hw;
-	void __iomem		*base;
-	struct clk_factor_masks	*masks;
-	struct clk_factor_tbl	*ftbl;
-	unsigned int		ftbl_cnt;
-};
+#define to_clk_factor(hw) container_of(hw, struct mmp_clk_factor, hw)
 
 static long clk_factor_round_rate(struct clk_hw *hw, unsigned long drate,
 		unsigned long *prate)
 {
-	struct clk_factor *factor = to_clk_factor(hw);
+	struct mmp_clk_factor *factor = to_clk_factor(hw);
 	unsigned long rate = 0, prev_rate;
 	int i;
 
@@ -58,8 +51,8 @@ static long clk_factor_round_rate(struct clk_hw *hw, unsigned long drate,
 static unsigned long clk_factor_recalc_rate(struct clk_hw *hw,
 		unsigned long parent_rate)
 {
-	struct clk_factor *factor = to_clk_factor(hw);
-	struct clk_factor_masks *masks = factor->masks;
+	struct mmp_clk_factor *factor = to_clk_factor(hw);
+	struct mmp_clk_factor_masks *masks = factor->masks;
 	unsigned int val, num, den;
 
 	val = readl_relaxed(factor->base);
@@ -81,11 +74,12 @@ static unsigned long clk_factor_recalc_rate(struct clk_hw *hw,
 static int clk_factor_set_rate(struct clk_hw *hw, unsigned long drate,
 				unsigned long prate)
 {
-	struct clk_factor *factor = to_clk_factor(hw);
-	struct clk_factor_masks *masks = factor->masks;
+	struct mmp_clk_factor *factor = to_clk_factor(hw);
+	struct mmp_clk_factor_masks *masks = factor->masks;
 	int i;
 	unsigned long val;
 	unsigned long prev_rate, rate = 0;
+	unsigned long flags = 0;
 
 	for (i = 0; i < factor->ftbl_cnt; i++) {
 		prev_rate = rate;
@@ -97,6 +91,9 @@ static int clk_factor_set_rate(struct clk_hw *hw, unsigned long drate,
 	if (i > 0)
 		i--;
 
+	if (factor->lock)
+		spin_lock_irqsave(factor->lock, flags);
+
 	val = readl_relaxed(factor->base);
 
 	val &= ~(masks->num_mask << masks->num_shift);
@@ -107,21 +104,65 @@ static int clk_factor_set_rate(struct clk_hw *hw, unsigned long drate,
 
 	writel_relaxed(val, factor->base);
 
+	if (factor->lock)
+		spin_unlock_irqrestore(factor->lock, flags);
+
 	return 0;
 }
 
+static void clk_factor_init(struct clk_hw *hw)
+{
+	struct mmp_clk_factor *factor = to_clk_factor(hw);
+	struct mmp_clk_factor_masks *masks = factor->masks;
+	u32 val, num, den;
+	int i;
+	unsigned long flags = 0;
+
+	if (factor->lock)
+		spin_lock_irqsave(factor->lock, flags);
+
+	val = readl(factor->base);
+
+	/* calculate numerator */
+	num = (val >> masks->num_shift) & masks->num_mask;
+
+	/* calculate denominator */
+	den = (val >> masks->den_shift) & masks->den_mask;
+
+	for (i = 0; i < factor->ftbl_cnt; i++)
+		if (den == factor->ftbl[i].den && num == factor->ftbl[i].num)
+			break;
+
+	if (i >= factor->ftbl_cnt) {
+		val &= ~(masks->num_mask << masks->num_shift);
+		val |= (factor->ftbl[0].num & masks->num_mask) <<
+			masks->num_shift;
+
+		val &= ~(masks->den_mask << masks->den_shift);
+		val |= (factor->ftbl[0].den & masks->den_mask) <<
+			masks->den_shift;
+
+		writel(val, factor->base);
+	}
+
+	if (factor->lock)
+		spin_unlock_irqrestore(factor->lock, flags);
+}
+
 static struct clk_ops clk_factor_ops = {
 	.recalc_rate = clk_factor_recalc_rate,
 	.round_rate = clk_factor_round_rate,
 	.set_rate = clk_factor_set_rate,
+	.init = clk_factor_init,
 };
 
 struct clk *mmp_clk_register_factor(const char *name, const char *parent_name,
 		unsigned long flags, void __iomem *base,
-		struct clk_factor_masks *masks, struct clk_factor_tbl *ftbl,
-		unsigned int ftbl_cnt)
+		struct mmp_clk_factor_masks *masks,
+		struct mmp_clk_factor_tbl *ftbl,
+		unsigned int ftbl_cnt, spinlock_t *lock)
 {
-	struct clk_factor *factor;
+	struct mmp_clk_factor *factor;
 	struct clk_init_data init;
 	struct clk *clk;
 
@@ -142,6 +183,7 @@ struct clk *mmp_clk_register_factor(const char *name, const char *parent_name,
 	factor->ftbl = ftbl;
 	factor->ftbl_cnt = ftbl_cnt;
 	factor->hw.init = &init;
+	factor->lock = lock;
 
 	init.name = name;
 	init.ops = &clk_factor_ops;
diff --git a/drivers/clk/mmp/clk-gate.c b/drivers/clk/mmp/clk-gate.c
new file mode 100644
index 000000000000..adbd9d64ded2
--- /dev/null
+++ b/drivers/clk/mmp/clk-gate.c
@@ -0,0 +1,133 @@
+/*
+ * mmp gate clock operation source file
+ *
+ * Copyright (C) 2014 Marvell
+ * Chao Xie <chao.xie@marvell.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-provider.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/delay.h>
+
+#include "clk.h"
+
+/*
+ * Some clocks will have mutiple bits to enable the clocks, and
+ * the bits to disable the clock is not same as enabling bits.
+ */
+
+#define to_clk_mmp_gate(hw)	container_of(hw, struct mmp_clk_gate, hw)
+
+static int mmp_clk_gate_enable(struct clk_hw *hw)
+{
+	struct mmp_clk_gate *gate = to_clk_mmp_gate(hw);
+	struct clk *clk = hw->clk;
+	unsigned long flags = 0;
+	unsigned long rate;
+	u32 tmp;
+
+	if (gate->lock)
+		spin_lock_irqsave(gate->lock, flags);
+
+	tmp = readl(gate->reg);
+	tmp &= ~gate->mask;
+	tmp |= gate->val_enable;
+	writel(tmp, gate->reg);
+
+	if (gate->lock)
+		spin_unlock_irqrestore(gate->lock, flags);
+
+	if (gate->flags & MMP_CLK_GATE_NEED_DELAY) {
+		rate = __clk_get_rate(clk);
+		/* Need delay 2 cycles. */
+		udelay(2000000/rate);
+	}
+
+	return 0;
+}
+
+static void mmp_clk_gate_disable(struct clk_hw *hw)
+{
+	struct mmp_clk_gate *gate = to_clk_mmp_gate(hw);
+	unsigned long flags = 0;
+	u32 tmp;
+
+	if (gate->lock)
+		spin_lock_irqsave(gate->lock, flags);
+
+	tmp = readl(gate->reg);
+	tmp &= ~gate->mask;
+	tmp |= gate->val_disable;
+	writel(tmp, gate->reg);
+
+	if (gate->lock)
+		spin_unlock_irqrestore(gate->lock, flags);
+}
+
+static int mmp_clk_gate_is_enabled(struct clk_hw *hw)
+{
+	struct mmp_clk_gate *gate = to_clk_mmp_gate(hw);
+	unsigned long flags = 0;
+	u32 tmp;
+
+	if (gate->lock)
+		spin_lock_irqsave(gate->lock, flags);
+
+	tmp = readl(gate->reg);
+
+	if (gate->lock)
+		spin_unlock_irqrestore(gate->lock, flags);
+
+	return (tmp & gate->mask) == gate->val_enable;
+}
+
+const struct clk_ops mmp_clk_gate_ops = {
+	.enable = mmp_clk_gate_enable,
+	.disable = mmp_clk_gate_disable,
+	.is_enabled = mmp_clk_gate_is_enabled,
+};
+
+struct clk *mmp_clk_register_gate(struct device *dev, const char *name,
+		const char *parent_name, unsigned long flags,
+		void __iomem *reg, u32 mask, u32 val_enable, u32 val_disable,
+		unsigned int gate_flags, spinlock_t *lock)
+{
+	struct mmp_clk_gate *gate;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	/* allocate the gate */
+	gate = kzalloc(sizeof(*gate), GFP_KERNEL);
+	if (!gate) {
+		pr_err("%s:%s could not allocate gate clk\n", __func__, name);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	init.name = name;
+	init.ops = &mmp_clk_gate_ops;
+	init.flags = flags | CLK_IS_BASIC;
+	init.parent_names = (parent_name ? &parent_name : NULL);
+	init.num_parents = (parent_name ? 1 : 0);
+
+	/* struct clk_gate assignments */
+	gate->reg = reg;
+	gate->mask = mask;
+	gate->val_enable = val_enable;
+	gate->val_disable = val_disable;
+	gate->flags = gate_flags;
+	gate->lock = lock;
+	gate->hw.init = &init;
+
+	clk = clk_register(dev, &gate->hw);
+
+	if (IS_ERR(clk))
+		kfree(gate);
+
+	return clk;
+}
diff --git a/drivers/clk/mmp/clk-mix.c b/drivers/clk/mmp/clk-mix.c
new file mode 100644
index 000000000000..48fa53c7ce5e
--- /dev/null
+++ b/drivers/clk/mmp/clk-mix.c
@@ -0,0 +1,513 @@
+/*
+ * mmp mix(div and mux) clock operation source file
+ *
+ * Copyright (C) 2014 Marvell
+ * Chao Xie <chao.xie@marvell.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-provider.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/err.h>
+
+#include "clk.h"
+
+/*
+ * The mix clock is a clock combined mux and div type clock.
+ * Because the div field and mux field need to be set at same
+ * time, we can not divide it into 2 types of clock
+ */
+
+#define to_clk_mix(hw)	container_of(hw, struct mmp_clk_mix, hw)
+
+static unsigned int _get_maxdiv(struct mmp_clk_mix *mix)
+{
+	unsigned int div_mask = (1 << mix->reg_info.width_div) - 1;
+	unsigned int maxdiv = 0;
+	struct clk_div_table *clkt;
+
+	if (mix->div_flags & CLK_DIVIDER_ONE_BASED)
+		return div_mask;
+	if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)
+		return 1 << div_mask;
+	if (mix->div_table) {
+		for (clkt = mix->div_table; clkt->div; clkt++)
+			if (clkt->div > maxdiv)
+				maxdiv = clkt->div;
+		return maxdiv;
+	}
+	return div_mask + 1;
+}
+
+static unsigned int _get_div(struct mmp_clk_mix *mix, unsigned int val)
+{
+	struct clk_div_table *clkt;
+
+	if (mix->div_flags & CLK_DIVIDER_ONE_BASED)
+		return val;
+	if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)
+		return 1 << val;
+	if (mix->div_table) {
+		for (clkt = mix->div_table; clkt->div; clkt++)
+			if (clkt->val == val)
+				return clkt->div;
+		if (clkt->div == 0)
+			return 0;
+	}
+	return val + 1;
+}
+
+static unsigned int _get_mux(struct mmp_clk_mix *mix, unsigned int val)
+{
+	int num_parents = __clk_get_num_parents(mix->hw.clk);
+	int i;
+
+	if (mix->mux_flags & CLK_MUX_INDEX_BIT)
+		return ffs(val) - 1;
+	if (mix->mux_flags & CLK_MUX_INDEX_ONE)
+		return val - 1;
+	if (mix->mux_table) {
+		for (i = 0; i < num_parents; i++)
+			if (mix->mux_table[i] == val)
+				return i;
+		if (i == num_parents)
+			return 0;
+	}
+
+	return val;
+}
+static unsigned int _get_div_val(struct mmp_clk_mix *mix, unsigned int div)
+{
+	struct clk_div_table *clkt;
+
+	if (mix->div_flags & CLK_DIVIDER_ONE_BASED)
+		return div;
+	if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)
+		return __ffs(div);
+	if (mix->div_table) {
+		for (clkt = mix->div_table; clkt->div; clkt++)
+			if (clkt->div == div)
+				return clkt->val;
+		if (clkt->div == 0)
+			return 0;
+	}
+
+	return div - 1;
+}
+
+static unsigned int _get_mux_val(struct mmp_clk_mix *mix, unsigned int mux)
+{
+	if (mix->mux_table)
+		return mix->mux_table[mux];
+
+	return mux;
+}
+
+static void _filter_clk_table(struct mmp_clk_mix *mix,
+				struct mmp_clk_mix_clk_table *table,
+				unsigned int table_size)
+{
+	int i;
+	struct mmp_clk_mix_clk_table *item;
+	struct clk *parent, *clk;
+	unsigned long parent_rate;
+
+	clk = mix->hw.clk;
+
+	for (i = 0; i < table_size; i++) {
+		item = &table[i];
+		parent = clk_get_parent_by_index(clk, item->parent_index);
+		parent_rate = __clk_get_rate(parent);
+		if (parent_rate % item->rate) {
+			item->valid = 0;
+		} else {
+			item->divisor = parent_rate / item->rate;
+			item->valid = 1;
+		}
+	}
+}
+
+static int _set_rate(struct mmp_clk_mix *mix, u32 mux_val, u32 div_val,
+			unsigned int change_mux, unsigned int change_div)
+{
+	struct mmp_clk_mix_reg_info *ri = &mix->reg_info;
+	u8 width, shift;
+	u32 mux_div, fc_req;
+	int ret, timeout = 50;
+	unsigned long flags = 0;
+
+	if (!change_mux && !change_div)
+		return -EINVAL;
+
+	if (mix->lock)
+		spin_lock_irqsave(mix->lock, flags);
+
+	if (mix->type == MMP_CLK_MIX_TYPE_V1
+		|| mix->type == MMP_CLK_MIX_TYPE_V2)
+		mux_div = readl(ri->reg_clk_ctrl);
+	else
+		mux_div = readl(ri->reg_clk_sel);
+
+	if (change_div) {
+		width = ri->width_div;
+		shift = ri->shift_div;
+		mux_div &= ~MMP_CLK_BITS_MASK(width, shift);
+		mux_div |= MMP_CLK_BITS_SET_VAL(div_val, width, shift);
+	}
+
+	if (change_mux) {
+		width = ri->width_mux;
+		shift = ri->shift_mux;
+		mux_div &= ~MMP_CLK_BITS_MASK(width, shift);
+		mux_div |= MMP_CLK_BITS_SET_VAL(mux_val, width, shift);
+	}
+
+	if (mix->type == MMP_CLK_MIX_TYPE_V1) {
+		writel(mux_div, ri->reg_clk_ctrl);
+	} else if (mix->type == MMP_CLK_MIX_TYPE_V2) {
+		mux_div |= (1 << ri->bit_fc);
+		writel(mux_div, ri->reg_clk_ctrl);
+
+		do {
+			fc_req = readl(ri->reg_clk_ctrl);
+			timeout--;
+			if (!(fc_req & (1 << ri->bit_fc)))
+				break;
+		} while (timeout);
+
+		if (timeout == 0) {
+			pr_err("%s:%s cannot do frequency change\n",
+				__func__, __clk_get_name(mix->hw.clk));
+			ret = -EBUSY;
+			goto error;
+		}
+	} else {
+		fc_req = readl(ri->reg_clk_ctrl);
+		fc_req |= 1 << ri->bit_fc;
+		writel(fc_req, ri->reg_clk_ctrl);
+		writel(mux_div, ri->reg_clk_sel);
+		fc_req &= ~(1 << ri->bit_fc);
+	}
+
+	ret = 0;
+error:
+	if (mix->lock)
+		spin_unlock_irqrestore(mix->lock, flags);
+
+	return ret;
+}
+
+static long mmp_clk_mix_determine_rate(struct clk_hw *hw, unsigned long rate,
+					unsigned long *best_parent_rate,
+					struct clk_hw **best_parent_clk)
+{
+	struct mmp_clk_mix *mix = to_clk_mix(hw);
+	struct mmp_clk_mix_clk_table *item;
+	struct clk *parent, *parent_best, *mix_clk;
+	unsigned long parent_rate, mix_rate, mix_rate_best, parent_rate_best;
+	unsigned long gap, gap_best;
+	u32 div_val_max;
+	unsigned int div;
+	int i, j;
+
+	mix_clk = hw->clk;
+
+	parent = NULL;
+	mix_rate_best = 0;
+	parent_rate_best = 0;
+	gap_best = rate;
+	parent_best = NULL;
+
+	if (mix->table) {
+		for (i = 0; i < mix->table_size; i++) {
+			item = &mix->table[i];
+			if (item->valid == 0)
+				continue;
+			parent = clk_get_parent_by_index(mix_clk,
+							item->parent_index);
+			parent_rate = __clk_get_rate(parent);
+			mix_rate = parent_rate / item->divisor;
+			gap = abs(mix_rate - rate);
+			if (parent_best == NULL || gap < gap_best) {
+				parent_best = parent;
+				parent_rate_best = parent_rate;
+				mix_rate_best = mix_rate;
+				gap_best = gap;
+				if (gap_best == 0)
+					goto found;
+			}
+		}
+	} else {
+		for (i = 0; i < __clk_get_num_parents(mix_clk); i++) {
+			parent = clk_get_parent_by_index(mix_clk, i);
+			parent_rate = __clk_get_rate(parent);
+			div_val_max = _get_maxdiv(mix);
+			for (j = 0; j < div_val_max; j++) {
+				div = _get_div(mix, j);
+				mix_rate = parent_rate / div;
+				gap = abs(mix_rate - rate);
+				if (parent_best == NULL || gap < gap_best) {
+					parent_best = parent;
+					parent_rate_best = parent_rate;
+					mix_rate_best = mix_rate;
+					gap_best = gap;
+					if (gap_best == 0)
+						goto found;
+				}
+			}
+		}
+	}
+
+found:
+	*best_parent_rate = parent_rate_best;
+	*best_parent_clk = __clk_get_hw(parent_best);
+
+	return mix_rate_best;
+}
+
+static int mmp_clk_mix_set_rate_and_parent(struct clk_hw *hw,
+						unsigned long rate,
+						unsigned long parent_rate,
+						u8 index)
+{
+	struct mmp_clk_mix *mix = to_clk_mix(hw);
+	unsigned int div;
+	u32 div_val, mux_val;
+
+	div = parent_rate / rate;
+	div_val = _get_div_val(mix, div);
+	mux_val = _get_mux_val(mix, index);
+
+	return _set_rate(mix, mux_val, div_val, 1, 1);
+}
+
+static u8 mmp_clk_mix_get_parent(struct clk_hw *hw)
+{
+	struct mmp_clk_mix *mix = to_clk_mix(hw);
+	struct mmp_clk_mix_reg_info *ri = &mix->reg_info;
+	unsigned long flags = 0;
+	u32 mux_div = 0;
+	u8 width, shift;
+	u32 mux_val;
+
+	if (mix->lock)
+		spin_lock_irqsave(mix->lock, flags);
+
+	if (mix->type == MMP_CLK_MIX_TYPE_V1
+		|| mix->type == MMP_CLK_MIX_TYPE_V2)
+		mux_div = readl(ri->reg_clk_ctrl);
+	else
+		mux_div = readl(ri->reg_clk_sel);
+
+	if (mix->lock)
+		spin_unlock_irqrestore(mix->lock, flags);
+
+	width = mix->reg_info.width_mux;
+	shift = mix->reg_info.shift_mux;
+
+	mux_val = MMP_CLK_BITS_GET_VAL(mux_div, width, shift);
+
+	return _get_mux(mix, mux_val);
+}
+
+static unsigned long mmp_clk_mix_recalc_rate(struct clk_hw *hw,
+					unsigned long parent_rate)
+{
+	struct mmp_clk_mix *mix = to_clk_mix(hw);
+	struct mmp_clk_mix_reg_info *ri = &mix->reg_info;
+	unsigned long flags = 0;
+	u32 mux_div = 0;
+	u8 width, shift;
+	unsigned int div;
+
+	if (mix->lock)
+		spin_lock_irqsave(mix->lock, flags);
+
+	if (mix->type == MMP_CLK_MIX_TYPE_V1
+		|| mix->type == MMP_CLK_MIX_TYPE_V2)
+		mux_div = readl(ri->reg_clk_ctrl);
+	else
+		mux_div = readl(ri->reg_clk_sel);
+
+	if (mix->lock)
+		spin_unlock_irqrestore(mix->lock, flags);
+
+	width = mix->reg_info.width_div;
+	shift = mix->reg_info.shift_div;
+
+	div = _get_div(mix, MMP_CLK_BITS_GET_VAL(mux_div, width, shift));
+
+	return parent_rate / div;
+}
+
+static int mmp_clk_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct mmp_clk_mix *mix = to_clk_mix(hw);
+	struct mmp_clk_mix_clk_table *item;
+	int i;
+	u32 div_val, mux_val;
+
+	if (mix->table) {
+		for (i = 0; i < mix->table_size; i++) {
+			item = &mix->table[i];
+			if (item->valid == 0)
+				continue;
+			if (item->parent_index == index)
+				break;
+		}
+		if (i < mix->table_size) {
+			div_val = _get_div_val(mix, item->divisor);
+			mux_val = _get_mux_val(mix, item->parent_index);
+		} else
+			return -EINVAL;
+	} else {
+		mux_val = _get_mux_val(mix, index);
+		div_val = 0;
+	}
+
+	return _set_rate(mix, mux_val, div_val, 1, div_val ? 1 : 0);
+}
+
+static int mmp_clk_set_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long best_parent_rate)
+{
+	struct mmp_clk_mix *mix = to_clk_mix(hw);
+	struct mmp_clk_mix_clk_table *item;
+	unsigned long parent_rate;
+	unsigned int best_divisor;
+	struct clk *mix_clk, *parent;
+	int i;
+
+	best_divisor = best_parent_rate / rate;
+
+	mix_clk = hw->clk;
+	if (mix->table) {
+		for (i = 0; i < mix->table_size; i++) {
+			item = &mix->table[i];
+			if (item->valid == 0)
+				continue;
+			parent = clk_get_parent_by_index(mix_clk,
+							item->parent_index);
+			parent_rate = __clk_get_rate(parent);
+			if (parent_rate == best_parent_rate
+				&& item->divisor == best_divisor)
+				break;
+		}
+		if (i < mix->table_size)
+			return _set_rate(mix,
+					_get_mux_val(mix, item->parent_index),
+					_get_div_val(mix, item->divisor),
+					1, 1);
+		else
+			return -EINVAL;
+	} else {
+		for (i = 0; i < __clk_get_num_parents(mix_clk); i++) {
+			parent = clk_get_parent_by_index(mix_clk, i);
+			parent_rate = __clk_get_rate(parent);
+			if (parent_rate == best_parent_rate)
+				break;
+		}
+		if (i < __clk_get_num_parents(mix_clk))
+			return _set_rate(mix, _get_mux_val(mix, i),
+					_get_div_val(mix, best_divisor), 1, 1);
+		else
+			return -EINVAL;
+	}
+}
+
+static void mmp_clk_mix_init(struct clk_hw *hw)
+{
+	struct mmp_clk_mix *mix = to_clk_mix(hw);
+
+	if (mix->table)
+		_filter_clk_table(mix, mix->table, mix->table_size);
+}
+
+const struct clk_ops mmp_clk_mix_ops = {
+	.determine_rate = mmp_clk_mix_determine_rate,
+	.set_rate_and_parent = mmp_clk_mix_set_rate_and_parent,
+	.set_rate = mmp_clk_set_rate,
+	.set_parent = mmp_clk_set_parent,
+	.get_parent = mmp_clk_mix_get_parent,
+	.recalc_rate = mmp_clk_mix_recalc_rate,
+	.init = mmp_clk_mix_init,
+};
+
+struct clk *mmp_clk_register_mix(struct device *dev,
+					const char *name,
+					const char **parent_names,
+					u8 num_parents,
+					unsigned long flags,
+					struct mmp_clk_mix_config *config,
+					spinlock_t *lock)
+{
+	struct mmp_clk_mix *mix;
+	struct clk *clk;
+	struct clk_init_data init;
+	size_t table_bytes;
+
+	mix = kzalloc(sizeof(*mix), GFP_KERNEL);
+	if (!mix) {
+		pr_err("%s:%s: could not allocate mmp mix clk\n",
+			__func__, name);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	init.name = name;
+	init.flags = flags | CLK_GET_RATE_NOCACHE;
+	init.parent_names = parent_names;
+	init.num_parents = num_parents;
+	init.ops = &mmp_clk_mix_ops;
+
+	memcpy(&mix->reg_info, &config->reg_info, sizeof(config->reg_info));
+	if (config->table) {
+		table_bytes = sizeof(*config->table) * config->table_size;
+		mix->table = kzalloc(table_bytes, GFP_KERNEL);
+		if (!mix->table) {
+			pr_err("%s:%s: could not allocate mmp mix table\n",
+				__func__, name);
+			kfree(mix);
+			return ERR_PTR(-ENOMEM);
+		}
+		memcpy(mix->table, config->table, table_bytes);
+		mix->table_size = config->table_size;
+	}
+
+	if (config->mux_table) {
+		table_bytes = sizeof(u32) * num_parents;
+		mix->mux_table = kzalloc(table_bytes, GFP_KERNEL);
+		if (!mix->mux_table) {
+			pr_err("%s:%s: could not allocate mmp mix mux-table\n",
+				__func__, name);
+			kfree(mix->table);
+			kfree(mix);
+			return ERR_PTR(-ENOMEM);
+		}
+		memcpy(mix->mux_table, config->mux_table, table_bytes);
+	}
+
+	mix->div_flags = config->div_flags;
+	mix->mux_flags = config->mux_flags;
+	mix->lock = lock;
+	mix->hw.init = &init;
+
+	if (config->reg_info.bit_fc >= 32)
+		mix->type = MMP_CLK_MIX_TYPE_V1;
+	else if (config->reg_info.reg_clk_sel)
+		mix->type = MMP_CLK_MIX_TYPE_V3;
+	else
+		mix->type = MMP_CLK_MIX_TYPE_V2;
+	clk = clk_register(dev, &mix->hw);
+
+	if (IS_ERR(clk)) {
+		kfree(mix->mux_table);
+		kfree(mix->table);
+		kfree(mix);
+	}
+
+	return clk;
+}
diff --git a/drivers/clk/mmp/clk-mmp2.c b/drivers/clk/mmp/clk-mmp2.c
index b2721cae257a..5c90a4230fa3 100644
--- a/drivers/clk/mmp/clk-mmp2.c
+++ b/drivers/clk/mmp/clk-mmp2.c
@@ -54,7 +54,7 @@
 
 static DEFINE_SPINLOCK(clk_lock);
 
-static struct clk_factor_masks uart_factor_masks = {
+static struct mmp_clk_factor_masks uart_factor_masks = {
 	.factor = 2,
 	.num_mask = 0x1fff,
 	.den_mask = 0x1fff,
@@ -62,7 +62,7 @@ static struct clk_factor_masks uart_factor_masks = {
 	.den_shift = 0,
 };
 
-static struct clk_factor_tbl uart_factor_tbl[] = {
+static struct mmp_clk_factor_tbl uart_factor_tbl[] = {
 	{.num = 14634, .den = 2165},	/*14.745MHZ */
 	{.num = 3521, .den = 689},	/*19.23MHZ */
 	{.num = 9679, .den = 5728},	/*58.9824MHZ */
@@ -191,7 +191,7 @@ void __init mmp2_clk_init(void)
 	clk = mmp_clk_register_factor("uart_pll", "pll1_4", 0,
 				mpmu_base + MPMU_UART_PLL,
 				&uart_factor_masks, uart_factor_tbl,
-				ARRAY_SIZE(uart_factor_tbl));
+				ARRAY_SIZE(uart_factor_tbl), &clk_lock);
 	clk_set_rate(clk, 14745600);
 	clk_register_clkdev(clk, "uart_pll", NULL);
 
diff --git a/drivers/clk/mmp/clk-of-mmp2.c b/drivers/clk/mmp/clk-of-mmp2.c
new file mode 100644
index 000000000000..2cbc2b43ae52
--- /dev/null
+++ b/drivers/clk/mmp/clk-of-mmp2.c
@@ -0,0 +1,334 @@
+/*
+ * mmp2 clock framework source file
+ *
+ * Copyright (C) 2012 Marvell
+ * Chao Xie <xiechao.mail@gmail.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/module.h>
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/of_address.h>
+
+#include <dt-bindings/clock/marvell,mmp2.h>
+
+#include "clk.h"
+#include "reset.h"
+
+#define APBC_RTC	0x0
+#define APBC_TWSI0	0x4
+#define APBC_TWSI1	0x8
+#define APBC_TWSI2	0xc
+#define APBC_TWSI3	0x10
+#define APBC_TWSI4	0x7c
+#define APBC_TWSI5	0x80
+#define APBC_KPC	0x18
+#define APBC_UART0	0x2c
+#define APBC_UART1	0x30
+#define APBC_UART2	0x34
+#define APBC_UART3	0x88
+#define APBC_GPIO	0x38
+#define APBC_PWM0	0x3c
+#define APBC_PWM1	0x40
+#define APBC_PWM2	0x44
+#define APBC_PWM3	0x48
+#define APBC_SSP0	0x50
+#define APBC_SSP1	0x54
+#define APBC_SSP2	0x58
+#define APBC_SSP3	0x5c
+#define APMU_SDH0	0x54
+#define APMU_SDH1	0x58
+#define APMU_SDH2	0xe8
+#define APMU_SDH3	0xec
+#define APMU_USB	0x5c
+#define APMU_DISP0	0x4c
+#define APMU_DISP1	0x110
+#define APMU_CCIC0	0x50
+#define APMU_CCIC1	0xf4
+#define MPMU_UART_PLL	0x14
+
+struct mmp2_clk_unit {
+	struct mmp_clk_unit unit;
+	void __iomem *mpmu_base;
+	void __iomem *apmu_base;
+	void __iomem *apbc_base;
+};
+
+static struct mmp_param_fixed_rate_clk fixed_rate_clks[] = {
+	{MMP2_CLK_CLK32, "clk32", NULL, CLK_IS_ROOT, 32768},
+	{MMP2_CLK_VCTCXO, "vctcxo", NULL, CLK_IS_ROOT, 26000000},
+	{MMP2_CLK_PLL1, "pll1", NULL, CLK_IS_ROOT, 800000000},
+	{MMP2_CLK_PLL2, "pll2", NULL, CLK_IS_ROOT, 960000000},
+	{MMP2_CLK_USB_PLL, "usb_pll", NULL, CLK_IS_ROOT, 480000000},
+};
+
+static struct mmp_param_fixed_factor_clk fixed_factor_clks[] = {
+	{MMP2_CLK_PLL1_2, "pll1_2", "pll1", 1, 2, 0},
+	{MMP2_CLK_PLL1_4, "pll1_4", "pll1_2", 1, 2, 0},
+	{MMP2_CLK_PLL1_8, "pll1_8", "pll1_4", 1, 2, 0},
+	{MMP2_CLK_PLL1_16, "pll1_16", "pll1_8", 1, 2, 0},
+	{MMP2_CLK_PLL1_20, "pll1_20", "pll1_4", 1, 5, 0},
+	{MMP2_CLK_PLL1_3, "pll1_3", "pll1", 1, 3, 0},
+	{MMP2_CLK_PLL1_6, "pll1_6", "pll1_3", 1, 2, 0},
+	{MMP2_CLK_PLL1_12, "pll1_12", "pll1_6", 1, 2, 0},
+	{MMP2_CLK_PLL2_2, "pll2_2", "pll2", 1, 2, 0},
+	{MMP2_CLK_PLL2_4, "pll2_4", "pll2_2", 1, 2, 0},
+	{MMP2_CLK_PLL2_8, "pll2_8", "pll2_4", 1, 2, 0},
+	{MMP2_CLK_PLL2_16, "pll2_16", "pll2_8", 1, 2, 0},
+	{MMP2_CLK_PLL2_3, "pll2_3", "pll2", 1, 3, 0},
+	{MMP2_CLK_PLL2_6, "pll2_6", "pll2_3", 1, 2, 0},
+	{MMP2_CLK_PLL2_12, "pll2_12", "pll2_6", 1, 2, 0},
+	{MMP2_CLK_VCTCXO_2, "vctcxo_2", "vctcxo", 1, 2, 0},
+	{MMP2_CLK_VCTCXO_4, "vctcxo_4", "vctcxo_2", 1, 2, 0},
+};
+
+static struct mmp_clk_factor_masks uart_factor_masks = {
+	.factor = 2,
+	.num_mask = 0x1fff,
+	.den_mask = 0x1fff,
+	.num_shift = 16,
+	.den_shift = 0,
+};
+
+static struct mmp_clk_factor_tbl uart_factor_tbl[] = {
+	{.num = 14634, .den = 2165},	/*14.745MHZ */
+	{.num = 3521, .den = 689},	/*19.23MHZ */
+	{.num = 9679, .den = 5728},	/*58.9824MHZ */
+	{.num = 15850, .den = 9451},	/*59.429MHZ */
+};
+
+static void mmp2_pll_init(struct mmp2_clk_unit *pxa_unit)
+{
+	struct clk *clk;
+	struct mmp_clk_unit *unit = &pxa_unit->unit;
+
+	mmp_register_fixed_rate_clks(unit, fixed_rate_clks,
+					ARRAY_SIZE(fixed_rate_clks));
+
+	mmp_register_fixed_factor_clks(unit, fixed_factor_clks,
+					ARRAY_SIZE(fixed_factor_clks));
+
+	clk = mmp_clk_register_factor("uart_pll", "pll1_4",
+				CLK_SET_RATE_PARENT,
+				pxa_unit->mpmu_base + MPMU_UART_PLL,
+				&uart_factor_masks, uart_factor_tbl,
+				ARRAY_SIZE(uart_factor_tbl), NULL);
+	mmp_clk_add(unit, MMP2_CLK_UART_PLL, clk);
+}
+
+static DEFINE_SPINLOCK(uart0_lock);
+static DEFINE_SPINLOCK(uart1_lock);
+static DEFINE_SPINLOCK(uart2_lock);
+static const char *uart_parent_names[] = {"uart_pll", "vctcxo"};
+
+static DEFINE_SPINLOCK(ssp0_lock);
+static DEFINE_SPINLOCK(ssp1_lock);
+static DEFINE_SPINLOCK(ssp2_lock);
+static DEFINE_SPINLOCK(ssp3_lock);
+static const char *ssp_parent_names[] = {"vctcxo_4", "vctcxo_2", "vctcxo", "pll1_16"};
+
+static DEFINE_SPINLOCK(reset_lock);
+
+static struct mmp_param_mux_clk apbc_mux_clks[] = {
+	{0, "uart0_mux", uart_parent_names, ARRAY_SIZE(uart_parent_names), CLK_SET_RATE_PARENT, APBC_UART0, 4, 3, 0, &uart0_lock},
+	{0, "uart1_mux", uart_parent_names, ARRAY_SIZE(uart_parent_names), CLK_SET_RATE_PARENT, APBC_UART1, 4, 3, 0, &uart1_lock},
+	{0, "uart2_mux", uart_parent_names, ARRAY_SIZE(uart_parent_names), CLK_SET_RATE_PARENT, APBC_UART2, 4, 3, 0, &uart2_lock},
+	{0, "uart3_mux", uart_parent_names, ARRAY_SIZE(uart_parent_names), CLK_SET_RATE_PARENT, APBC_UART3, 4, 3, 0, &uart2_lock},
+	{0, "ssp0_mux", ssp_parent_names, ARRAY_SIZE(ssp_parent_names), CLK_SET_RATE_PARENT, APBC_SSP0, 4, 3, 0, &ssp0_lock},
+	{0, "ssp1_mux", ssp_parent_names, ARRAY_SIZE(ssp_parent_names), CLK_SET_RATE_PARENT, APBC_SSP1, 4, 3, 0, &ssp1_lock},
+	{0, "ssp2_mux", ssp_parent_names, ARRAY_SIZE(ssp_parent_names), CLK_SET_RATE_PARENT, APBC_SSP2, 4, 3, 0, &ssp2_lock},
+	{0, "ssp3_mux", ssp_parent_names, ARRAY_SIZE(ssp_parent_names), CLK_SET_RATE_PARENT, APBC_SSP3, 4, 3, 0, &ssp3_lock},
+};
+
+static struct mmp_param_gate_clk apbc_gate_clks[] = {
+	{MMP2_CLK_TWSI0, "twsi0_clk", "vctcxo", CLK_SET_RATE_PARENT, APBC_TWSI0, 0x7, 0x3, 0x0, 0, &reset_lock},
+	{MMP2_CLK_TWSI1, "twsi1_clk", "vctcxo", CLK_SET_RATE_PARENT, APBC_TWSI1, 0x7, 0x3, 0x0, 0, &reset_lock},
+	{MMP2_CLK_TWSI2, "twsi2_clk", "vctcxo", CLK_SET_RATE_PARENT, APBC_TWSI2, 0x7, 0x3, 0x0, 0, &reset_lock},
+	{MMP2_CLK_TWSI3, "twsi3_clk", "vctcxo", CLK_SET_RATE_PARENT, APBC_TWSI3, 0x7, 0x3, 0x0, 0, &reset_lock},
+	{MMP2_CLK_TWSI4, "twsi4_clk", "vctcxo", CLK_SET_RATE_PARENT, APBC_TWSI4, 0x7, 0x3, 0x0, 0, &reset_lock},
+	{MMP2_CLK_TWSI5, "twsi5_clk", "vctcxo", CLK_SET_RATE_PARENT, APBC_TWSI5, 0x7, 0x3, 0x0, 0, &reset_lock},
+	{MMP2_CLK_GPIO, "gpio_clk", "vctcxo", CLK_SET_RATE_PARENT, APBC_GPIO, 0x7, 0x3, 0x0, 0, &reset_lock},
+	{MMP2_CLK_KPC, "kpc_clk", "clk32", CLK_SET_RATE_PARENT, APBC_KPC, 0x7, 0x3, 0x0, MMP_CLK_GATE_NEED_DELAY, &reset_lock},
+	{MMP2_CLK_RTC, "rtc_clk", "clk32", CLK_SET_RATE_PARENT, APBC_RTC, 0x87, 0x83, 0x0, MMP_CLK_GATE_NEED_DELAY, &reset_lock},
+	{MMP2_CLK_PWM0, "pwm0_clk", "pll1_48", CLK_SET_RATE_PARENT, APBC_PWM0, 0x7, 0x3, 0x0, 0, &reset_lock},
+	{MMP2_CLK_PWM1, "pwm1_clk", "pll1_48", CLK_SET_RATE_PARENT, APBC_PWM1, 0x7, 0x3, 0x0, 0, &reset_lock},
+	{MMP2_CLK_PWM2, "pwm2_clk", "pll1_48", CLK_SET_RATE_PARENT, APBC_PWM2, 0x7, 0x3, 0x0, 0, &reset_lock},
+	{MMP2_CLK_PWM3, "pwm3_clk", "pll1_48", CLK_SET_RATE_PARENT, APBC_PWM3, 0x7, 0x3, 0x0, 0, &reset_lock},
+	/* The gate clocks has mux parent. */
+	{MMP2_CLK_UART0, "uart0_clk", "uart0_mux", CLK_SET_RATE_PARENT, APBC_UART0, 0x7, 0x3, 0x0, 0, &uart0_lock},
+	{MMP2_CLK_UART1, "uart1_clk", "uart1_mux", CLK_SET_RATE_PARENT, APBC_UART1, 0x7, 0x3, 0x0, 0, &uart1_lock},
+	{MMP2_CLK_UART2, "uart2_clk", "uart2_mux", CLK_SET_RATE_PARENT, APBC_UART2, 0x7, 0x3, 0x0, 0, &uart2_lock},
+	{MMP2_CLK_UART3, "uart3_clk", "uart3_mux", CLK_SET_RATE_PARENT, APBC_UART3, 0x7, 0x3, 0x0, 0, &uart2_lock},
+	{MMP2_CLK_SSP0, "ssp0_clk", "ssp0_mux", CLK_SET_RATE_PARENT, APBC_SSP0, 0x7, 0x3, 0x0, 0, &ssp0_lock},
+	{MMP2_CLK_SSP1, "ssp1_clk", "ssp1_mux", CLK_SET_RATE_PARENT, APBC_SSP1, 0x7, 0x3, 0x0, 0, &ssp1_lock},
+	{MMP2_CLK_SSP2, "ssp2_clk", "ssp2_mux", CLK_SET_RATE_PARENT, APBC_SSP2, 0x7, 0x3, 0x0, 0, &ssp2_lock},
+	{MMP2_CLK_SSP3, "ssp3_clk", "ssp3_mux", CLK_SET_RATE_PARENT, APBC_SSP3, 0x7, 0x3, 0x0, 0, &ssp3_lock},
+};
+
+static void mmp2_apb_periph_clk_init(struct mmp2_clk_unit *pxa_unit)
+{
+	struct mmp_clk_unit *unit = &pxa_unit->unit;
+
+	mmp_register_mux_clks(unit, apbc_mux_clks, pxa_unit->apbc_base,
+				ARRAY_SIZE(apbc_mux_clks));
+
+	mmp_register_gate_clks(unit, apbc_gate_clks, pxa_unit->apbc_base,
+				ARRAY_SIZE(apbc_gate_clks));
+}
+
+static DEFINE_SPINLOCK(sdh_lock);
+static const char *sdh_parent_names[] = {"pll1_4", "pll2", "usb_pll", "pll1"};
+static struct mmp_clk_mix_config sdh_mix_config = {
+	.reg_info = DEFINE_MIX_REG_INFO(4, 10, 2, 8, 32),
+};
+
+static DEFINE_SPINLOCK(usb_lock);
+
+static DEFINE_SPINLOCK(disp0_lock);
+static DEFINE_SPINLOCK(disp1_lock);
+static const char *disp_parent_names[] = {"pll1", "pll1_16", "pll2", "vctcxo"};
+
+static DEFINE_SPINLOCK(ccic0_lock);
+static DEFINE_SPINLOCK(ccic1_lock);
+static const char *ccic_parent_names[] = {"pll1_2", "pll1_16", "vctcxo"};
+static struct mmp_clk_mix_config ccic0_mix_config = {
+	.reg_info = DEFINE_MIX_REG_INFO(4, 17, 2, 6, 32),
+};
+static struct mmp_clk_mix_config ccic1_mix_config = {
+	.reg_info = DEFINE_MIX_REG_INFO(4, 16, 2, 6, 32),
+};
+
+static struct mmp_param_mux_clk apmu_mux_clks[] = {
+	{MMP2_CLK_DISP0_MUX, "disp0_mux", disp_parent_names, ARRAY_SIZE(disp_parent_names), CLK_SET_RATE_PARENT, APMU_DISP0, 6, 2, 0, &disp0_lock},
+	{MMP2_CLK_DISP1_MUX, "disp1_mux", disp_parent_names, ARRAY_SIZE(disp_parent_names), CLK_SET_RATE_PARENT, APMU_DISP1, 6, 2, 0, &disp1_lock},
+};
+
+static struct mmp_param_div_clk apmu_div_clks[] = {
+	{0, "disp0_div", "disp0_mux", CLK_SET_RATE_PARENT, APMU_DISP0, 8, 4, 0, &disp0_lock},
+	{0, "disp0_sphy_div", "disp0_mux", CLK_SET_RATE_PARENT, APMU_DISP0, 15, 5, 0, &disp0_lock},
+	{0, "disp1_div", "disp1_mux", CLK_SET_RATE_PARENT, APMU_DISP1, 8, 4, 0, &disp1_lock},
+	{0, "ccic0_sphy_div", "ccic0_mix_clk", CLK_SET_RATE_PARENT, APMU_CCIC0, 10, 5, 0, &ccic0_lock},
+	{0, "ccic1_sphy_div", "ccic1_mix_clk", CLK_SET_RATE_PARENT, APMU_CCIC1, 10, 5, 0, &ccic1_lock},
+};
+
+static struct mmp_param_gate_clk apmu_gate_clks[] = {
+	{MMP2_CLK_USB, "usb_clk", "usb_pll", 0, APMU_USB, 0x9, 0x9, 0x0, 0, &usb_lock},
+	/* The gate clocks has mux parent. */
+	{MMP2_CLK_SDH0, "sdh0_clk", "sdh_mix_clk", CLK_SET_RATE_PARENT, APMU_SDH0, 0x1b, 0x1b, 0x0, 0, &sdh_lock},
+	{MMP2_CLK_SDH1, "sdh1_clk", "sdh_mix_clk", CLK_SET_RATE_PARENT, APMU_SDH1, 0x1b, 0x1b, 0x0, 0, &sdh_lock},
+	{MMP2_CLK_SDH1, "sdh2_clk", "sdh_mix_clk", CLK_SET_RATE_PARENT, APMU_SDH2, 0x1b, 0x1b, 0x0, 0, &sdh_lock},
+	{MMP2_CLK_SDH1, "sdh3_clk", "sdh_mix_clk", CLK_SET_RATE_PARENT, APMU_SDH3, 0x1b, 0x1b, 0x0, 0, &sdh_lock},
+	{MMP2_CLK_DISP0, "disp0_clk", "disp0_div", CLK_SET_RATE_PARENT, APMU_DISP0, 0x1b, 0x1b, 0x0, 0, &disp0_lock},
+	{MMP2_CLK_DISP0_SPHY, "disp0_sphy_clk", "disp0_sphy_div", CLK_SET_RATE_PARENT, APMU_DISP0, 0x1024, 0x1024, 0x0, 0, &disp0_lock},
+	{MMP2_CLK_DISP1, "disp1_clk", "disp1_div", CLK_SET_RATE_PARENT, APMU_DISP1, 0x1b, 0x1b, 0x0, 0, &disp1_lock},
+	{MMP2_CLK_CCIC_ARBITER, "ccic_arbiter", "vctcxo", CLK_SET_RATE_PARENT, APMU_CCIC0, 0x1800, 0x1800, 0x0, 0, &ccic0_lock},
+	{MMP2_CLK_CCIC0, "ccic0_clk", "ccic0_mix_clk", CLK_SET_RATE_PARENT, APMU_CCIC0, 0x1b, 0x1b, 0x0, 0, &ccic0_lock},
+	{MMP2_CLK_CCIC0_PHY, "ccic0_phy_clk", "ccic0_mix_clk", CLK_SET_RATE_PARENT, APMU_CCIC0, 0x24, 0x24, 0x0, 0, &ccic0_lock},
+	{MMP2_CLK_CCIC0_SPHY, "ccic0_sphy_clk", "ccic0_sphy_div", CLK_SET_RATE_PARENT, APMU_CCIC0, 0x300, 0x300, 0x0, 0, &ccic0_lock},
+	{MMP2_CLK_CCIC1, "ccic1_clk", "ccic1_mix_clk", CLK_SET_RATE_PARENT, APMU_CCIC1, 0x1b, 0x1b, 0x0, 0, &ccic1_lock},
+	{MMP2_CLK_CCIC1_PHY, "ccic1_phy_clk", "ccic1_mix_clk", CLK_SET_RATE_PARENT, APMU_CCIC1, 0x24, 0x24, 0x0, 0, &ccic1_lock},
+	{MMP2_CLK_CCIC1_SPHY, "ccic1_sphy_clk", "ccic1_sphy_div", CLK_SET_RATE_PARENT, APMU_CCIC1, 0x300, 0x300, 0x0, 0, &ccic1_lock},
+};
+
+static void mmp2_axi_periph_clk_init(struct mmp2_clk_unit *pxa_unit)
+{
+	struct clk *clk;
+	struct mmp_clk_unit *unit = &pxa_unit->unit;
+
+	sdh_mix_config.reg_info.reg_clk_ctrl = pxa_unit->apmu_base + APMU_SDH0;
+	clk = mmp_clk_register_mix(NULL, "sdh_mix_clk", sdh_parent_names,
+					ARRAY_SIZE(sdh_parent_names),
+					CLK_SET_RATE_PARENT,
+					&sdh_mix_config, &sdh_lock);
+
+	ccic0_mix_config.reg_info.reg_clk_ctrl = pxa_unit->apmu_base + APMU_CCIC0;
+	clk = mmp_clk_register_mix(NULL, "ccic0_mix_clk", ccic_parent_names,
+					ARRAY_SIZE(ccic_parent_names),
+					CLK_SET_RATE_PARENT,
+					&ccic0_mix_config, &ccic0_lock);
+	mmp_clk_add(unit, MMP2_CLK_CCIC0_MIX, clk);
+
+	ccic1_mix_config.reg_info.reg_clk_ctrl = pxa_unit->apmu_base + APMU_CCIC1;
+	clk = mmp_clk_register_mix(NULL, "ccic1_mix_clk", ccic_parent_names,
+					ARRAY_SIZE(ccic_parent_names),
+					CLK_SET_RATE_PARENT,
+					&ccic1_mix_config, &ccic1_lock);
+	mmp_clk_add(unit, MMP2_CLK_CCIC1_MIX, clk);
+
+	mmp_register_mux_clks(unit, apmu_mux_clks, pxa_unit->apmu_base,
+				ARRAY_SIZE(apmu_mux_clks));
+
+	mmp_register_div_clks(unit, apmu_div_clks, pxa_unit->apmu_base,
+				ARRAY_SIZE(apmu_div_clks));
+
+	mmp_register_gate_clks(unit, apmu_gate_clks, pxa_unit->apmu_base,
+				ARRAY_SIZE(apmu_gate_clks));
+}
+
+static void mmp2_clk_reset_init(struct device_node *np,
+				struct mmp2_clk_unit *pxa_unit)
+{
+	struct mmp_clk_reset_cell *cells;
+	int i, nr_resets;
+
+	nr_resets = ARRAY_SIZE(apbc_gate_clks);
+	cells = kcalloc(nr_resets, sizeof(*cells), GFP_KERNEL);
+	if (!cells)
+		return;
+
+	for (i = 0; i < nr_resets; i++) {
+		cells[i].clk_id = apbc_gate_clks[i].id;
+		cells[i].reg = pxa_unit->apbc_base + apbc_gate_clks[i].offset;
+		cells[i].flags = 0;
+		cells[i].lock = apbc_gate_clks[i].lock;
+		cells[i].bits = 0x4;
+	}
+
+	mmp_clk_reset_register(np, cells, nr_resets);
+}
+
+static void __init mmp2_clk_init(struct device_node *np)
+{
+	struct mmp2_clk_unit *pxa_unit;
+
+	pxa_unit = kzalloc(sizeof(*pxa_unit), GFP_KERNEL);
+	if (!pxa_unit)
+		return;
+
+	pxa_unit->mpmu_base = of_iomap(np, 0);
+	if (!pxa_unit->mpmu_base) {
+		pr_err("failed to map mpmu registers\n");
+		return;
+	}
+
+	pxa_unit->apmu_base = of_iomap(np, 1);
+	if (!pxa_unit->mpmu_base) {
+		pr_err("failed to map apmu registers\n");
+		return;
+	}
+
+	pxa_unit->apbc_base = of_iomap(np, 2);
+	if (!pxa_unit->apbc_base) {
+		pr_err("failed to map apbc registers\n");
+		return;
+	}
+
+	mmp_clk_init(np, &pxa_unit->unit, MMP2_NR_CLKS);
+
+	mmp2_pll_init(pxa_unit);
+
+	mmp2_apb_periph_clk_init(pxa_unit);
+
+	mmp2_axi_periph_clk_init(pxa_unit);
+
+	mmp2_clk_reset_init(np, pxa_unit);
+}
+
+CLK_OF_DECLARE(mmp2_clk, "marvell,mmp2-clock", mmp2_clk_init);
diff --git a/drivers/clk/mmp/clk-of-pxa168.c b/drivers/clk/mmp/clk-of-pxa168.c
new file mode 100644
index 000000000000..5b1810dc4bd2
--- /dev/null
+++ b/drivers/clk/mmp/clk-of-pxa168.c
@@ -0,0 +1,279 @@
+/*
+ * pxa168 clock framework source file
+ *
+ * Copyright (C) 2012 Marvell
+ * Chao Xie <xiechao.mail@gmail.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/module.h>
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/of_address.h>
+
+#include <dt-bindings/clock/marvell,pxa168.h>
+
+#include "clk.h"
+#include "reset.h"
+
+#define APBC_RTC	0x28
+#define APBC_TWSI0	0x2c
+#define APBC_KPC	0x30
+#define APBC_UART0	0x0
+#define APBC_UART1	0x4
+#define APBC_GPIO	0x8
+#define APBC_PWM0	0xc
+#define APBC_PWM1	0x10
+#define APBC_PWM2	0x14
+#define APBC_PWM3	0x18
+#define APBC_SSP0	0x81c
+#define APBC_SSP1	0x820
+#define APBC_SSP2	0x84c
+#define APBC_SSP3	0x858
+#define APBC_SSP4	0x85c
+#define APBC_TWSI1	0x6c
+#define APBC_UART2	0x70
+#define APMU_SDH0	0x54
+#define APMU_SDH1	0x58
+#define APMU_USB	0x5c
+#define APMU_DISP0	0x4c
+#define APMU_CCIC0	0x50
+#define APMU_DFC	0x60
+#define MPMU_UART_PLL	0x14
+
+struct pxa168_clk_unit {
+	struct mmp_clk_unit unit;
+	void __iomem *mpmu_base;
+	void __iomem *apmu_base;
+	void __iomem *apbc_base;
+};
+
+static struct mmp_param_fixed_rate_clk fixed_rate_clks[] = {
+	{PXA168_CLK_CLK32, "clk32", NULL, CLK_IS_ROOT, 32768},
+	{PXA168_CLK_VCTCXO, "vctcxo", NULL, CLK_IS_ROOT, 26000000},
+	{PXA168_CLK_PLL1, "pll1", NULL, CLK_IS_ROOT, 624000000},
+};
+
+static struct mmp_param_fixed_factor_clk fixed_factor_clks[] = {
+	{PXA168_CLK_PLL1_2, "pll1_2", "pll1", 1, 2, 0},
+	{PXA168_CLK_PLL1_4, "pll1_4", "pll1_2", 1, 2, 0},
+	{PXA168_CLK_PLL1_8, "pll1_8", "pll1_4", 1, 2, 0},
+	{PXA168_CLK_PLL1_16, "pll1_16", "pll1_8", 1, 2, 0},
+	{PXA168_CLK_PLL1_6, "pll1_6", "pll1_2", 1, 3, 0},
+	{PXA168_CLK_PLL1_12, "pll1_12", "pll1_6", 1, 2, 0},
+	{PXA168_CLK_PLL1_24, "pll1_24", "pll1_12", 1, 2, 0},
+	{PXA168_CLK_PLL1_48, "pll1_48", "pll1_24", 1, 2, 0},
+	{PXA168_CLK_PLL1_96, "pll1_96", "pll1_48", 1, 2, 0},
+	{PXA168_CLK_PLL1_13, "pll1_13", "pll1", 1, 13, 0},
+	{PXA168_CLK_PLL1_13_1_5, "pll1_13_1_5", "pll1_13", 2, 3, 0},
+	{PXA168_CLK_PLL1_2_1_5, "pll1_2_1_5", "pll1_2", 2, 3, 0},
+	{PXA168_CLK_PLL1_3_16, "pll1_3_16", "pll1", 3, 16, 0},
+};
+
+static struct mmp_clk_factor_masks uart_factor_masks = {
+	.factor = 2,
+	.num_mask = 0x1fff,
+	.den_mask = 0x1fff,
+	.num_shift = 16,
+	.den_shift = 0,
+};
+
+static struct mmp_clk_factor_tbl uart_factor_tbl[] = {
+	{.num = 8125, .den = 1536},	/*14.745MHZ */
+};
+
+static void pxa168_pll_init(struct pxa168_clk_unit *pxa_unit)
+{
+	struct clk *clk;
+	struct mmp_clk_unit *unit = &pxa_unit->unit;
+
+	mmp_register_fixed_rate_clks(unit, fixed_rate_clks,
+					ARRAY_SIZE(fixed_rate_clks));
+
+	mmp_register_fixed_factor_clks(unit, fixed_factor_clks,
+					ARRAY_SIZE(fixed_factor_clks));
+
+	clk = mmp_clk_register_factor("uart_pll", "pll1_4",
+				CLK_SET_RATE_PARENT,
+				pxa_unit->mpmu_base + MPMU_UART_PLL,
+				&uart_factor_masks, uart_factor_tbl,
+				ARRAY_SIZE(uart_factor_tbl), NULL);
+	mmp_clk_add(unit, PXA168_CLK_UART_PLL, clk);
+}
+
+static DEFINE_SPINLOCK(uart0_lock);
+static DEFINE_SPINLOCK(uart1_lock);
+static DEFINE_SPINLOCK(uart2_lock);
+static const char *uart_parent_names[] = {"pll1_3_16", "uart_pll"};
+
+static DEFINE_SPINLOCK(ssp0_lock);
+static DEFINE_SPINLOCK(ssp1_lock);
+static DEFINE_SPINLOCK(ssp2_lock);
+static DEFINE_SPINLOCK(ssp3_lock);
+static DEFINE_SPINLOCK(ssp4_lock);
+static const char *ssp_parent_names[] = {"pll1_96", "pll1_48", "pll1_24", "pll1_12"};
+
+static DEFINE_SPINLOCK(reset_lock);
+
+static struct mmp_param_mux_clk apbc_mux_clks[] = {
+	{0, "uart0_mux", uart_parent_names, ARRAY_SIZE(uart_parent_names), CLK_SET_RATE_PARENT, APBC_UART0, 4, 3, 0, &uart0_lock},
+	{0, "uart1_mux", uart_parent_names, ARRAY_SIZE(uart_parent_names), CLK_SET_RATE_PARENT, APBC_UART1, 4, 3, 0, &uart1_lock},
+	{0, "uart2_mux", uart_parent_names, ARRAY_SIZE(uart_parent_names), CLK_SET_RATE_PARENT, APBC_UART2, 4, 3, 0, &uart2_lock},
+	{0, "ssp0_mux", ssp_parent_names, ARRAY_SIZE(ssp_parent_names), CLK_SET_RATE_PARENT, APBC_SSP0, 4, 3, 0, &ssp0_lock},
+	{0, "ssp1_mux", ssp_parent_names, ARRAY_SIZE(ssp_parent_names), CLK_SET_RATE_PARENT, APBC_SSP1, 4, 3, 0, &ssp1_lock},
+	{0, "ssp2_mux", ssp_parent_names, ARRAY_SIZE(ssp_parent_names), CLK_SET_RATE_PARENT, APBC_SSP2, 4, 3, 0, &ssp2_lock},
+	{0, "ssp3_mux", ssp_parent_names, ARRAY_SIZE(ssp_parent_names), CLK_SET_RATE_PARENT, APBC_SSP3, 4, 3, 0, &ssp3_lock},
+	{0, "ssp4_mux", ssp_parent_names, ARRAY_SIZE(ssp_parent_names), CLK_SET_RATE_PARENT, APBC_SSP4, 4, 3, 0, &ssp4_lock},
+};
+
+static struct mmp_param_gate_clk apbc_gate_clks[] = {
+	{PXA168_CLK_TWSI0, "twsi0_clk", "pll1_13_1_5", CLK_SET_RATE_PARENT, APBC_TWSI0, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA168_CLK_TWSI1, "twsi1_clk", "pll1_13_1_5", CLK_SET_RATE_PARENT, APBC_TWSI1, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA168_CLK_GPIO, "gpio_clk", "vctcxo", CLK_SET_RATE_PARENT, APBC_GPIO, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA168_CLK_KPC, "kpc_clk", "clk32", CLK_SET_RATE_PARENT, APBC_KPC, 0x3, 0x3, 0x0, MMP_CLK_GATE_NEED_DELAY, NULL},
+	{PXA168_CLK_RTC, "rtc_clk", "clk32", CLK_SET_RATE_PARENT, APBC_RTC, 0x83, 0x83, 0x0, MMP_CLK_GATE_NEED_DELAY, NULL},
+	{PXA168_CLK_PWM0, "pwm0_clk", "pll1_48", CLK_SET_RATE_PARENT, APBC_PWM0, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA168_CLK_PWM1, "pwm1_clk", "pll1_48", CLK_SET_RATE_PARENT, APBC_PWM1, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA168_CLK_PWM2, "pwm2_clk", "pll1_48", CLK_SET_RATE_PARENT, APBC_PWM2, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA168_CLK_PWM3, "pwm3_clk", "pll1_48", CLK_SET_RATE_PARENT, APBC_PWM3, 0x3, 0x3, 0x0, 0, &reset_lock},
+	/* The gate clocks has mux parent. */
+	{PXA168_CLK_UART0, "uart0_clk", "uart0_mux", CLK_SET_RATE_PARENT, APBC_UART0, 0x3, 0x3, 0x0, 0, &uart0_lock},
+	{PXA168_CLK_UART1, "uart1_clk", "uart1_mux", CLK_SET_RATE_PARENT, APBC_UART1, 0x3, 0x3, 0x0, 0, &uart1_lock},
+	{PXA168_CLK_UART2, "uart2_clk", "uart2_mux", CLK_SET_RATE_PARENT, APBC_UART2, 0x3, 0x3, 0x0, 0, &uart2_lock},
+	{PXA168_CLK_SSP0, "ssp0_clk", "ssp0_mux", CLK_SET_RATE_PARENT, APBC_SSP0, 0x3, 0x3, 0x0, 0, &ssp0_lock},
+	{PXA168_CLK_SSP1, "ssp1_clk", "ssp1_mux", CLK_SET_RATE_PARENT, APBC_SSP1, 0x3, 0x3, 0x0, 0, &ssp1_lock},
+	{PXA168_CLK_SSP2, "ssp2_clk", "ssp2_mux", CLK_SET_RATE_PARENT, APBC_SSP2, 0x3, 0x3, 0x0, 0, &ssp2_lock},
+	{PXA168_CLK_SSP3, "ssp3_clk", "ssp3_mux", CLK_SET_RATE_PARENT, APBC_SSP3, 0x3, 0x3, 0x0, 0, &ssp3_lock},
+	{PXA168_CLK_SSP4, "ssp4_clk", "ssp4_mux", CLK_SET_RATE_PARENT, APBC_SSP4, 0x3, 0x3, 0x0, 0, &ssp4_lock},
+};
+
+static void pxa168_apb_periph_clk_init(struct pxa168_clk_unit *pxa_unit)
+{
+	struct mmp_clk_unit *unit = &pxa_unit->unit;
+
+	mmp_register_mux_clks(unit, apbc_mux_clks, pxa_unit->apbc_base,
+				ARRAY_SIZE(apbc_mux_clks));
+
+	mmp_register_gate_clks(unit, apbc_gate_clks, pxa_unit->apbc_base,
+				ARRAY_SIZE(apbc_gate_clks));
+
+}
+
+static DEFINE_SPINLOCK(sdh0_lock);
+static DEFINE_SPINLOCK(sdh1_lock);
+static const char *sdh_parent_names[] = {"pll1_12", "pll1_13"};
+
+static DEFINE_SPINLOCK(usb_lock);
+
+static DEFINE_SPINLOCK(disp0_lock);
+static const char *disp_parent_names[] = {"pll1_2", "pll1_12"};
+
+static DEFINE_SPINLOCK(ccic0_lock);
+static const char *ccic_parent_names[] = {"pll1_2", "pll1_12"};
+static const char *ccic_phy_parent_names[] = {"pll1_6", "pll1_12"};
+
+static struct mmp_param_mux_clk apmu_mux_clks[] = {
+	{0, "sdh0_mux", sdh_parent_names, ARRAY_SIZE(sdh_parent_names), CLK_SET_RATE_PARENT, APMU_SDH0, 6, 1, 0, &sdh0_lock},
+	{0, "sdh1_mux", sdh_parent_names, ARRAY_SIZE(sdh_parent_names), CLK_SET_RATE_PARENT, APMU_SDH1, 6, 1, 0, &sdh1_lock},
+	{0, "disp0_mux", disp_parent_names, ARRAY_SIZE(disp_parent_names), CLK_SET_RATE_PARENT, APMU_DISP0, 6, 1, 0, &disp0_lock},
+	{0, "ccic0_mux", ccic_parent_names, ARRAY_SIZE(ccic_parent_names), CLK_SET_RATE_PARENT, APMU_CCIC0, 6, 1, 0, &ccic0_lock},
+	{0, "ccic0_phy_mux", ccic_phy_parent_names, ARRAY_SIZE(ccic_phy_parent_names), CLK_SET_RATE_PARENT, APMU_CCIC0, 7, 1, 0, &ccic0_lock},
+};
+
+static struct mmp_param_div_clk apmu_div_clks[] = {
+	{0, "ccic0_sphy_div", "ccic0_mux", CLK_SET_RATE_PARENT, APMU_CCIC0, 10, 5, 0, &ccic0_lock},
+};
+
+static struct mmp_param_gate_clk apmu_gate_clks[] = {
+	{PXA168_CLK_DFC, "dfc_clk", "pll1_4", CLK_SET_RATE_PARENT, APMU_DFC, 0x19b, 0x19b, 0x0, 0, NULL},
+	{PXA168_CLK_USB, "usb_clk", "usb_pll", 0, APMU_USB, 0x9, 0x9, 0x0, 0, &usb_lock},
+	{PXA168_CLK_SPH, "sph_clk", "usb_pll", 0, APMU_USB, 0x12, 0x12, 0x0, 0, &usb_lock},
+	/* The gate clocks has mux parent. */
+	{PXA168_CLK_SDH0, "sdh0_clk", "sdh0_mux", CLK_SET_RATE_PARENT, APMU_SDH0, 0x1b, 0x1b, 0x0, 0, &sdh0_lock},
+	{PXA168_CLK_SDH1, "sdh1_clk", "sdh1_mux", CLK_SET_RATE_PARENT, APMU_SDH1, 0x1b, 0x1b, 0x0, 0, &sdh1_lock},
+	{PXA168_CLK_DISP0, "disp0_clk", "disp0_mux", CLK_SET_RATE_PARENT, APMU_DISP0, 0x1b, 0x1b, 0x0, 0, &disp0_lock},
+	{PXA168_CLK_CCIC0, "ccic0_clk", "ccic0_mux", CLK_SET_RATE_PARENT, APMU_CCIC0, 0x1b, 0x1b, 0x0, 0, &ccic0_lock},
+	{PXA168_CLK_CCIC0_PHY, "ccic0_phy_clk", "ccic0_phy_mux", CLK_SET_RATE_PARENT, APMU_CCIC0, 0x24, 0x24, 0x0, 0, &ccic0_lock},
+	{PXA168_CLK_CCIC0_SPHY, "ccic0_sphy_clk", "ccic0_sphy_div", CLK_SET_RATE_PARENT, APMU_CCIC0, 0x300, 0x300, 0x0, 0, &ccic0_lock},
+};
+
+static void pxa168_axi_periph_clk_init(struct pxa168_clk_unit *pxa_unit)
+{
+	struct mmp_clk_unit *unit = &pxa_unit->unit;
+
+	mmp_register_mux_clks(unit, apmu_mux_clks, pxa_unit->apmu_base,
+				ARRAY_SIZE(apmu_mux_clks));
+
+	mmp_register_div_clks(unit, apmu_div_clks, pxa_unit->apmu_base,
+				ARRAY_SIZE(apmu_div_clks));
+
+	mmp_register_gate_clks(unit, apmu_gate_clks, pxa_unit->apmu_base,
+				ARRAY_SIZE(apmu_gate_clks));
+}
+
+static void pxa168_clk_reset_init(struct device_node *np,
+				struct pxa168_clk_unit *pxa_unit)
+{
+	struct mmp_clk_reset_cell *cells;
+	int i, nr_resets;
+
+	nr_resets = ARRAY_SIZE(apbc_gate_clks);
+	cells = kcalloc(nr_resets, sizeof(*cells), GFP_KERNEL);
+	if (!cells)
+		return;
+
+	for (i = 0; i < nr_resets; i++) {
+		cells[i].clk_id = apbc_gate_clks[i].id;
+		cells[i].reg = pxa_unit->apbc_base + apbc_gate_clks[i].offset;
+		cells[i].flags = 0;
+		cells[i].lock = apbc_gate_clks[i].lock;
+		cells[i].bits = 0x4;
+	}
+
+	mmp_clk_reset_register(np, cells, nr_resets);
+}
+
+static void __init pxa168_clk_init(struct device_node *np)
+{
+	struct pxa168_clk_unit *pxa_unit;
+
+	pxa_unit = kzalloc(sizeof(*pxa_unit), GFP_KERNEL);
+	if (!pxa_unit)
+		return;
+
+	pxa_unit->mpmu_base = of_iomap(np, 0);
+	if (!pxa_unit->mpmu_base) {
+		pr_err("failed to map mpmu registers\n");
+		return;
+	}
+
+	pxa_unit->apmu_base = of_iomap(np, 1);
+	if (!pxa_unit->mpmu_base) {
+		pr_err("failed to map apmu registers\n");
+		return;
+	}
+
+	pxa_unit->apbc_base = of_iomap(np, 2);
+	if (!pxa_unit->apbc_base) {
+		pr_err("failed to map apbc registers\n");
+		return;
+	}
+
+	mmp_clk_init(np, &pxa_unit->unit, PXA168_NR_CLKS);
+
+	pxa168_pll_init(pxa_unit);
+
+	pxa168_apb_periph_clk_init(pxa_unit);
+
+	pxa168_axi_periph_clk_init(pxa_unit);
+
+	pxa168_clk_reset_init(np, pxa_unit);
+}
+
+CLK_OF_DECLARE(pxa168_clk, "marvell,pxa168-clock", pxa168_clk_init);
diff --git a/drivers/clk/mmp/clk-of-pxa910.c b/drivers/clk/mmp/clk-of-pxa910.c
new file mode 100644
index 000000000000..5e3c80dad336
--- /dev/null
+++ b/drivers/clk/mmp/clk-of-pxa910.c
@@ -0,0 +1,301 @@
+/*
+ * pxa910 clock framework source file
+ *
+ * Copyright (C) 2012 Marvell
+ * Chao Xie <xiechao.mail@gmail.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/module.h>
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/of_address.h>
+
+#include <dt-bindings/clock/marvell,pxa910.h>
+
+#include "clk.h"
+#include "reset.h"
+
+#define APBC_RTC	0x28
+#define APBC_TWSI0	0x2c
+#define APBC_KPC	0x18
+#define APBC_UART0	0x0
+#define APBC_UART1	0x4
+#define APBC_GPIO	0x8
+#define APBC_PWM0	0xc
+#define APBC_PWM1	0x10
+#define APBC_PWM2	0x14
+#define APBC_PWM3	0x18
+#define APBC_SSP0	0x1c
+#define APBC_SSP1	0x20
+#define APBC_SSP2	0x4c
+#define APBCP_TWSI1	0x28
+#define APBCP_UART2	0x1c
+#define APMU_SDH0	0x54
+#define APMU_SDH1	0x58
+#define APMU_USB	0x5c
+#define APMU_DISP0	0x4c
+#define APMU_CCIC0	0x50
+#define APMU_DFC	0x60
+#define MPMU_UART_PLL	0x14
+
+struct pxa910_clk_unit {
+	struct mmp_clk_unit unit;
+	void __iomem *mpmu_base;
+	void __iomem *apmu_base;
+	void __iomem *apbc_base;
+	void __iomem *apbcp_base;
+};
+
+static struct mmp_param_fixed_rate_clk fixed_rate_clks[] = {
+	{PXA910_CLK_CLK32, "clk32", NULL, CLK_IS_ROOT, 32768},
+	{PXA910_CLK_VCTCXO, "vctcxo", NULL, CLK_IS_ROOT, 26000000},
+	{PXA910_CLK_PLL1, "pll1", NULL, CLK_IS_ROOT, 624000000},
+};
+
+static struct mmp_param_fixed_factor_clk fixed_factor_clks[] = {
+	{PXA910_CLK_PLL1_2, "pll1_2", "pll1", 1, 2, 0},
+	{PXA910_CLK_PLL1_4, "pll1_4", "pll1_2", 1, 2, 0},
+	{PXA910_CLK_PLL1_8, "pll1_8", "pll1_4", 1, 2, 0},
+	{PXA910_CLK_PLL1_16, "pll1_16", "pll1_8", 1, 2, 0},
+	{PXA910_CLK_PLL1_6, "pll1_6", "pll1_2", 1, 3, 0},
+	{PXA910_CLK_PLL1_12, "pll1_12", "pll1_6", 1, 2, 0},
+	{PXA910_CLK_PLL1_24, "pll1_24", "pll1_12", 1, 2, 0},
+	{PXA910_CLK_PLL1_48, "pll1_48", "pll1_24", 1, 2, 0},
+	{PXA910_CLK_PLL1_96, "pll1_96", "pll1_48", 1, 2, 0},
+	{PXA910_CLK_PLL1_13, "pll1_13", "pll1", 1, 13, 0},
+	{PXA910_CLK_PLL1_13_1_5, "pll1_13_1_5", "pll1_13", 2, 3, 0},
+	{PXA910_CLK_PLL1_2_1_5, "pll1_2_1_5", "pll1_2", 2, 3, 0},
+	{PXA910_CLK_PLL1_3_16, "pll1_3_16", "pll1", 3, 16, 0},
+};
+
+static struct mmp_clk_factor_masks uart_factor_masks = {
+	.factor = 2,
+	.num_mask = 0x1fff,
+	.den_mask = 0x1fff,
+	.num_shift = 16,
+	.den_shift = 0,
+};
+
+static struct mmp_clk_factor_tbl uart_factor_tbl[] = {
+	{.num = 8125, .den = 1536},	/*14.745MHZ */
+};
+
+static void pxa910_pll_init(struct pxa910_clk_unit *pxa_unit)
+{
+	struct clk *clk;
+	struct mmp_clk_unit *unit = &pxa_unit->unit;
+
+	mmp_register_fixed_rate_clks(unit, fixed_rate_clks,
+					ARRAY_SIZE(fixed_rate_clks));
+
+	mmp_register_fixed_factor_clks(unit, fixed_factor_clks,
+					ARRAY_SIZE(fixed_factor_clks));
+
+	clk = mmp_clk_register_factor("uart_pll", "pll1_4",
+				CLK_SET_RATE_PARENT,
+				pxa_unit->mpmu_base + MPMU_UART_PLL,
+				&uart_factor_masks, uart_factor_tbl,
+				ARRAY_SIZE(uart_factor_tbl), NULL);
+	mmp_clk_add(unit, PXA910_CLK_UART_PLL, clk);
+}
+
+static DEFINE_SPINLOCK(uart0_lock);
+static DEFINE_SPINLOCK(uart1_lock);
+static DEFINE_SPINLOCK(uart2_lock);
+static const char *uart_parent_names[] = {"pll1_3_16", "uart_pll"};
+
+static DEFINE_SPINLOCK(ssp0_lock);
+static DEFINE_SPINLOCK(ssp1_lock);
+static const char *ssp_parent_names[] = {"pll1_96", "pll1_48", "pll1_24", "pll1_12"};
+
+static DEFINE_SPINLOCK(reset_lock);
+
+static struct mmp_param_mux_clk apbc_mux_clks[] = {
+	{0, "uart0_mux", uart_parent_names, ARRAY_SIZE(uart_parent_names), CLK_SET_RATE_PARENT, APBC_UART0, 4, 3, 0, &uart0_lock},
+	{0, "uart1_mux", uart_parent_names, ARRAY_SIZE(uart_parent_names), CLK_SET_RATE_PARENT, APBC_UART1, 4, 3, 0, &uart1_lock},
+	{0, "ssp0_mux", ssp_parent_names, ARRAY_SIZE(ssp_parent_names), CLK_SET_RATE_PARENT, APBC_SSP0, 4, 3, 0, &ssp0_lock},
+	{0, "ssp1_mux", ssp_parent_names, ARRAY_SIZE(ssp_parent_names), CLK_SET_RATE_PARENT, APBC_SSP1, 4, 3, 0, &ssp1_lock},
+};
+
+static struct mmp_param_mux_clk apbcp_mux_clks[] = {
+	{0, "uart2_mux", uart_parent_names, ARRAY_SIZE(uart_parent_names), CLK_SET_RATE_PARENT, APBCP_UART2, 4, 3, 0, &uart2_lock},
+};
+
+static struct mmp_param_gate_clk apbc_gate_clks[] = {
+	{PXA910_CLK_TWSI0, "twsi0_clk", "pll1_13_1_5", CLK_SET_RATE_PARENT, APBC_TWSI0, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA910_CLK_GPIO, "gpio_clk", "vctcxo", CLK_SET_RATE_PARENT, APBC_GPIO, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA910_CLK_KPC, "kpc_clk", "clk32", CLK_SET_RATE_PARENT, APBC_KPC, 0x3, 0x3, 0x0, MMP_CLK_GATE_NEED_DELAY, NULL},
+	{PXA910_CLK_RTC, "rtc_clk", "clk32", CLK_SET_RATE_PARENT, APBC_RTC, 0x83, 0x83, 0x0, MMP_CLK_GATE_NEED_DELAY, NULL},
+	{PXA910_CLK_PWM0, "pwm0_clk", "pll1_48", CLK_SET_RATE_PARENT, APBC_PWM0, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA910_CLK_PWM1, "pwm1_clk", "pll1_48", CLK_SET_RATE_PARENT, APBC_PWM1, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA910_CLK_PWM2, "pwm2_clk", "pll1_48", CLK_SET_RATE_PARENT, APBC_PWM2, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA910_CLK_PWM3, "pwm3_clk", "pll1_48", CLK_SET_RATE_PARENT, APBC_PWM3, 0x3, 0x3, 0x0, 0, &reset_lock},
+	/* The gate clocks has mux parent. */
+	{PXA910_CLK_UART0, "uart0_clk", "uart0_mux", CLK_SET_RATE_PARENT, APBC_UART0, 0x3, 0x3, 0x0, 0, &uart0_lock},
+	{PXA910_CLK_UART1, "uart1_clk", "uart1_mux", CLK_SET_RATE_PARENT, APBC_UART1, 0x3, 0x3, 0x0, 0, &uart1_lock},
+	{PXA910_CLK_SSP0, "ssp0_clk", "ssp0_mux", CLK_SET_RATE_PARENT, APBC_SSP0, 0x3, 0x3, 0x0, 0, &ssp0_lock},
+	{PXA910_CLK_SSP1, "ssp1_clk", "ssp1_mux", CLK_SET_RATE_PARENT, APBC_SSP1, 0x3, 0x3, 0x0, 0, &ssp1_lock},
+};
+
+static struct mmp_param_gate_clk apbcp_gate_clks[] = {
+	{PXA910_CLK_TWSI1, "twsi1_clk", "pll1_13_1_5", CLK_SET_RATE_PARENT, APBCP_TWSI1, 0x3, 0x3, 0x0, 0, &reset_lock},
+	/* The gate clocks has mux parent. */
+	{PXA910_CLK_UART2, "uart2_clk", "uart2_mux", CLK_SET_RATE_PARENT, APBCP_UART2, 0x3, 0x3, 0x0, 0, &uart2_lock},
+};
+
+static void pxa910_apb_periph_clk_init(struct pxa910_clk_unit *pxa_unit)
+{
+	struct mmp_clk_unit *unit = &pxa_unit->unit;
+
+	mmp_register_mux_clks(unit, apbc_mux_clks, pxa_unit->apbc_base,
+				ARRAY_SIZE(apbc_mux_clks));
+
+	mmp_register_mux_clks(unit, apbcp_mux_clks, pxa_unit->apbcp_base,
+				ARRAY_SIZE(apbcp_mux_clks));
+
+	mmp_register_gate_clks(unit, apbc_gate_clks, pxa_unit->apbc_base,
+				ARRAY_SIZE(apbc_gate_clks));
+
+	mmp_register_gate_clks(unit, apbcp_gate_clks, pxa_unit->apbcp_base,
+				ARRAY_SIZE(apbcp_gate_clks));
+}
+
+static DEFINE_SPINLOCK(sdh0_lock);
+static DEFINE_SPINLOCK(sdh1_lock);
+static const char *sdh_parent_names[] = {"pll1_12", "pll1_13"};
+
+static DEFINE_SPINLOCK(usb_lock);
+
+static DEFINE_SPINLOCK(disp0_lock);
+static const char *disp_parent_names[] = {"pll1_2", "pll1_12"};
+
+static DEFINE_SPINLOCK(ccic0_lock);
+static const char *ccic_parent_names[] = {"pll1_2", "pll1_12"};
+static const char *ccic_phy_parent_names[] = {"pll1_6", "pll1_12"};
+
+static struct mmp_param_mux_clk apmu_mux_clks[] = {
+	{0, "sdh0_mux", sdh_parent_names, ARRAY_SIZE(sdh_parent_names), CLK_SET_RATE_PARENT, APMU_SDH0, 6, 1, 0, &sdh0_lock},
+	{0, "sdh1_mux", sdh_parent_names, ARRAY_SIZE(sdh_parent_names), CLK_SET_RATE_PARENT, APMU_SDH1, 6, 1, 0, &sdh1_lock},
+	{0, "disp0_mux", disp_parent_names, ARRAY_SIZE(disp_parent_names), CLK_SET_RATE_PARENT, APMU_DISP0, 6, 1, 0, &disp0_lock},
+	{0, "ccic0_mux", ccic_parent_names, ARRAY_SIZE(ccic_parent_names), CLK_SET_RATE_PARENT, APMU_CCIC0, 6, 1, 0, &ccic0_lock},
+	{0, "ccic0_phy_mux", ccic_phy_parent_names, ARRAY_SIZE(ccic_phy_parent_names), CLK_SET_RATE_PARENT, APMU_CCIC0, 7, 1, 0, &ccic0_lock},
+};
+
+static struct mmp_param_div_clk apmu_div_clks[] = {
+	{0, "ccic0_sphy_div", "ccic0_mux", CLK_SET_RATE_PARENT, APMU_CCIC0, 10, 5, 0, &ccic0_lock},
+};
+
+static struct mmp_param_gate_clk apmu_gate_clks[] = {
+	{PXA910_CLK_DFC, "dfc_clk", "pll1_4", CLK_SET_RATE_PARENT, APMU_DFC, 0x19b, 0x19b, 0x0, 0, NULL},
+	{PXA910_CLK_USB, "usb_clk", "usb_pll", 0, APMU_USB, 0x9, 0x9, 0x0, 0, &usb_lock},
+	{PXA910_CLK_SPH, "sph_clk", "usb_pll", 0, APMU_USB, 0x12, 0x12, 0x0, 0, &usb_lock},
+	/* The gate clocks has mux parent. */
+	{PXA910_CLK_SDH0, "sdh0_clk", "sdh0_mux", CLK_SET_RATE_PARENT, APMU_SDH0, 0x1b, 0x1b, 0x0, 0, &sdh0_lock},
+	{PXA910_CLK_SDH1, "sdh1_clk", "sdh1_mux", CLK_SET_RATE_PARENT, APMU_SDH1, 0x1b, 0x1b, 0x0, 0, &sdh1_lock},
+	{PXA910_CLK_DISP0, "disp0_clk", "disp0_mux", CLK_SET_RATE_PARENT, APMU_DISP0, 0x1b, 0x1b, 0x0, 0, &disp0_lock},
+	{PXA910_CLK_CCIC0, "ccic0_clk", "ccic0_mux", CLK_SET_RATE_PARENT, APMU_CCIC0, 0x1b, 0x1b, 0x0, 0, &ccic0_lock},
+	{PXA910_CLK_CCIC0_PHY, "ccic0_phy_clk", "ccic0_phy_mux", CLK_SET_RATE_PARENT, APMU_CCIC0, 0x24, 0x24, 0x0, 0, &ccic0_lock},
+	{PXA910_CLK_CCIC0_SPHY, "ccic0_sphy_clk", "ccic0_sphy_div", CLK_SET_RATE_PARENT, APMU_CCIC0, 0x300, 0x300, 0x0, 0, &ccic0_lock},
+};
+
+static void pxa910_axi_periph_clk_init(struct pxa910_clk_unit *pxa_unit)
+{
+	struct mmp_clk_unit *unit = &pxa_unit->unit;
+
+	mmp_register_mux_clks(unit, apmu_mux_clks, pxa_unit->apmu_base,
+				ARRAY_SIZE(apmu_mux_clks));
+
+	mmp_register_div_clks(unit, apmu_div_clks, pxa_unit->apmu_base,
+				ARRAY_SIZE(apmu_div_clks));
+
+	mmp_register_gate_clks(unit, apmu_gate_clks, pxa_unit->apmu_base,
+				ARRAY_SIZE(apmu_gate_clks));
+}
+
+static void pxa910_clk_reset_init(struct device_node *np,
+				struct pxa910_clk_unit *pxa_unit)
+{
+	struct mmp_clk_reset_cell *cells;
+	int i, base, nr_resets_apbc, nr_resets_apbcp, nr_resets;
+
+	nr_resets_apbc = ARRAY_SIZE(apbc_gate_clks);
+	nr_resets_apbcp = ARRAY_SIZE(apbcp_gate_clks);
+	nr_resets = nr_resets_apbc + nr_resets_apbcp;
+	cells = kcalloc(nr_resets, sizeof(*cells), GFP_KERNEL);
+	if (!cells)
+		return;
+
+	base = 0;
+	for (i = 0; i < nr_resets_apbc; i++) {
+		cells[base + i].clk_id = apbc_gate_clks[i].id;
+		cells[base + i].reg =
+			pxa_unit->apbc_base + apbc_gate_clks[i].offset;
+		cells[base + i].flags = 0;
+		cells[base + i].lock = apbc_gate_clks[i].lock;
+		cells[base + i].bits = 0x4;
+	}
+
+	base = nr_resets_apbc;
+	for (i = 0; i < nr_resets_apbcp; i++) {
+		cells[base + i].clk_id = apbcp_gate_clks[i].id;
+		cells[base + i].reg =
+			pxa_unit->apbc_base + apbc_gate_clks[i].offset;
+		cells[base + i].flags = 0;
+		cells[base + i].lock = apbc_gate_clks[i].lock;
+		cells[base + i].bits = 0x4;
+	}
+
+	mmp_clk_reset_register(np, cells, nr_resets);
+}
+
+static void __init pxa910_clk_init(struct device_node *np)
+{
+	struct pxa910_clk_unit *pxa_unit;
+
+	pxa_unit = kzalloc(sizeof(*pxa_unit), GFP_KERNEL);
+	if (!pxa_unit)
+		return;
+
+	pxa_unit->mpmu_base = of_iomap(np, 0);
+	if (!pxa_unit->mpmu_base) {
+		pr_err("failed to map mpmu registers\n");
+		return;
+	}
+
+	pxa_unit->apmu_base = of_iomap(np, 1);
+	if (!pxa_unit->mpmu_base) {
+		pr_err("failed to map apmu registers\n");
+		return;
+	}
+
+	pxa_unit->apbc_base = of_iomap(np, 2);
+	if (!pxa_unit->apbc_base) {
+		pr_err("failed to map apbc registers\n");
+		return;
+	}
+
+	pxa_unit->apbcp_base = of_iomap(np, 3);
+	if (!pxa_unit->mpmu_base) {
+		pr_err("failed to map apbcp registers\n");
+		return;
+	}
+
+	mmp_clk_init(np, &pxa_unit->unit, PXA910_NR_CLKS);
+
+	pxa910_pll_init(pxa_unit);
+
+	pxa910_apb_periph_clk_init(pxa_unit);
+
+	pxa910_axi_periph_clk_init(pxa_unit);
+
+	pxa910_clk_reset_init(np, pxa_unit);
+}
+
+CLK_OF_DECLARE(pxa910_clk, "marvell,pxa910-clock", pxa910_clk_init);
diff --git a/drivers/clk/mmp/clk-pxa168.c b/drivers/clk/mmp/clk-pxa168.c
index 014396b028a2..93e967c0f972 100644
--- a/drivers/clk/mmp/clk-pxa168.c
+++ b/drivers/clk/mmp/clk-pxa168.c
@@ -47,7 +47,7 @@
 
 static DEFINE_SPINLOCK(clk_lock);
 
-static struct clk_factor_masks uart_factor_masks = {
+static struct mmp_clk_factor_masks uart_factor_masks = {
 	.factor = 2,
 	.num_mask = 0x1fff,
 	.den_mask = 0x1fff,
@@ -55,7 +55,7 @@ static struct clk_factor_masks uart_factor_masks = {
 	.den_shift = 0,
 };
 
-static struct clk_factor_tbl uart_factor_tbl[] = {
+static struct mmp_clk_factor_tbl uart_factor_tbl[] = {
 	{.num = 8125, .den = 1536},	/*14.745MHZ */
 };
 
@@ -158,7 +158,7 @@ void __init pxa168_clk_init(void)
 	uart_pll = mmp_clk_register_factor("uart_pll", "pll1_4", 0,
 				mpmu_base + MPMU_UART_PLL,
 				&uart_factor_masks, uart_factor_tbl,
-				ARRAY_SIZE(uart_factor_tbl));
+				ARRAY_SIZE(uart_factor_tbl), &clk_lock);
 	clk_set_rate(uart_pll, 14745600);
 	clk_register_clkdev(uart_pll, "uart_pll", NULL);
 
diff --git a/drivers/clk/mmp/clk-pxa910.c b/drivers/clk/mmp/clk-pxa910.c
index 9efc6a47535d..993abcdb32cc 100644
--- a/drivers/clk/mmp/clk-pxa910.c
+++ b/drivers/clk/mmp/clk-pxa910.c
@@ -45,7 +45,7 @@
 
 static DEFINE_SPINLOCK(clk_lock);
 
-static struct clk_factor_masks uart_factor_masks = {
+static struct mmp_clk_factor_masks uart_factor_masks = {
 	.factor = 2,
 	.num_mask = 0x1fff,
 	.den_mask = 0x1fff,
@@ -53,7 +53,7 @@ static struct clk_factor_masks uart_factor_masks = {
 	.den_shift = 0,
 };
 
-static struct clk_factor_tbl uart_factor_tbl[] = {
+static struct mmp_clk_factor_tbl uart_factor_tbl[] = {
 	{.num = 8125, .den = 1536},	/*14.745MHZ */
 };
 
@@ -163,7 +163,7 @@ void __init pxa910_clk_init(void)
 	uart_pll =  mmp_clk_register_factor("uart_pll", "pll1_4", 0,
 				mpmu_base + MPMU_UART_PLL,
 				&uart_factor_masks, uart_factor_tbl,
-				ARRAY_SIZE(uart_factor_tbl));
+				ARRAY_SIZE(uart_factor_tbl), &clk_lock);
 	clk_set_rate(uart_pll, 14745600);
 	clk_register_clkdev(uart_pll, "uart_pll", NULL);
 
diff --git a/drivers/clk/mmp/clk.c b/drivers/clk/mmp/clk.c
new file mode 100644
index 000000000000..cf038ef54c59
--- /dev/null
+++ b/drivers/clk/mmp/clk.c
@@ -0,0 +1,192 @@
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+
+#include "clk.h"
+
+void mmp_clk_init(struct device_node *np, struct mmp_clk_unit *unit,
+		int nr_clks)
+{
+	static struct clk **clk_table;
+
+	clk_table = kcalloc(nr_clks, sizeof(struct clk *), GFP_KERNEL);
+	if (!clk_table)
+		return;
+
+	unit->clk_table = clk_table;
+	unit->nr_clks = nr_clks;
+	unit->clk_data.clks = clk_table;
+	unit->clk_data.clk_num = nr_clks;
+	of_clk_add_provider(np, of_clk_src_onecell_get, &unit->clk_data);
+}
+
+void mmp_register_fixed_rate_clks(struct mmp_clk_unit *unit,
+				struct mmp_param_fixed_rate_clk *clks,
+				int size)
+{
+	int i;
+	struct clk *clk;
+
+	for (i = 0; i < size; i++) {
+		clk = clk_register_fixed_rate(NULL, clks[i].name,
+					clks[i].parent_name,
+					clks[i].flags,
+					clks[i].fixed_rate);
+		if (IS_ERR(clk)) {
+			pr_err("%s: failed to register clock %s\n",
+			       __func__, clks[i].name);
+			continue;
+		}
+		if (clks[i].id)
+			unit->clk_table[clks[i].id] = clk;
+	}
+}
+
+void mmp_register_fixed_factor_clks(struct mmp_clk_unit *unit,
+				struct mmp_param_fixed_factor_clk *clks,
+				int size)
+{
+	struct clk *clk;
+	int i;
+
+	for (i = 0; i < size; i++) {
+		clk = clk_register_fixed_factor(NULL, clks[i].name,
+						clks[i].parent_name,
+						clks[i].flags, clks[i].mult,
+						clks[i].div);
+		if (IS_ERR(clk)) {
+			pr_err("%s: failed to register clock %s\n",
+			       __func__, clks[i].name);
+			continue;
+		}
+		if (clks[i].id)
+			unit->clk_table[clks[i].id] = clk;
+	}
+}
+
+void mmp_register_general_gate_clks(struct mmp_clk_unit *unit,
+				struct mmp_param_general_gate_clk *clks,
+				void __iomem *base, int size)
+{
+	struct clk *clk;
+	int i;
+
+	for (i = 0; i < size; i++) {
+		clk = clk_register_gate(NULL, clks[i].name,
+					clks[i].parent_name,
+					clks[i].flags,
+					base + clks[i].offset,
+					clks[i].bit_idx,
+					clks[i].gate_flags,
+					clks[i].lock);
+
+		if (IS_ERR(clk)) {
+			pr_err("%s: failed to register clock %s\n",
+			       __func__, clks[i].name);
+			continue;
+		}
+		if (clks[i].id)
+			unit->clk_table[clks[i].id] = clk;
+	}
+}
+
+void mmp_register_gate_clks(struct mmp_clk_unit *unit,
+			struct mmp_param_gate_clk *clks,
+			void __iomem *base, int size)
+{
+	struct clk *clk;
+	int i;
+
+	for (i = 0; i < size; i++) {
+		clk = mmp_clk_register_gate(NULL, clks[i].name,
+					clks[i].parent_name,
+					clks[i].flags,
+					base + clks[i].offset,
+					clks[i].mask,
+					clks[i].val_enable,
+					clks[i].val_disable,
+					clks[i].gate_flags,
+					clks[i].lock);
+
+		if (IS_ERR(clk)) {
+			pr_err("%s: failed to register clock %s\n",
+			       __func__, clks[i].name);
+			continue;
+		}
+		if (clks[i].id)
+			unit->clk_table[clks[i].id] = clk;
+	}
+}
+
+void mmp_register_mux_clks(struct mmp_clk_unit *unit,
+			struct mmp_param_mux_clk *clks,
+			void __iomem *base, int size)
+{
+	struct clk *clk;
+	int i;
+
+	for (i = 0; i < size; i++) {
+		clk = clk_register_mux(NULL, clks[i].name,
+					clks[i].parent_name,
+					clks[i].num_parents,
+					clks[i].flags,
+					base + clks[i].offset,
+					clks[i].shift,
+					clks[i].width,
+					clks[i].mux_flags,
+					clks[i].lock);
+
+		if (IS_ERR(clk)) {
+			pr_err("%s: failed to register clock %s\n",
+			       __func__, clks[i].name);
+			continue;
+		}
+		if (clks[i].id)
+			unit->clk_table[clks[i].id] = clk;
+	}
+}
+
+void mmp_register_div_clks(struct mmp_clk_unit *unit,
+			struct mmp_param_div_clk *clks,
+			void __iomem *base, int size)
+{
+	struct clk *clk;
+	int i;
+
+	for (i = 0; i < size; i++) {
+		clk = clk_register_divider(NULL, clks[i].name,
+					clks[i].parent_name,
+					clks[i].flags,
+					base + clks[i].offset,
+					clks[i].shift,
+					clks[i].width,
+					clks[i].div_flags,
+					clks[i].lock);
+
+		if (IS_ERR(clk)) {
+			pr_err("%s: failed to register clock %s\n",
+			       __func__, clks[i].name);
+			continue;
+		}
+		if (clks[i].id)
+			unit->clk_table[clks[i].id] = clk;
+	}
+}
+
+void mmp_clk_add(struct mmp_clk_unit *unit, unsigned int id,
+			struct clk *clk)
+{
+	if (IS_ERR_OR_NULL(clk)) {
+		pr_err("CLK %d has invalid pointer %p\n", id, clk);
+		return;
+	}
+	if (id > unit->nr_clks) {
+		pr_err("CLK %d is invalid\n", id);
+		return;
+	}
+
+	unit->clk_table[id] = clk;
+}
diff --git a/drivers/clk/mmp/clk.h b/drivers/clk/mmp/clk.h
index ab86dd4a416a..adf9b711b037 100644
--- a/drivers/clk/mmp/clk.h
+++ b/drivers/clk/mmp/clk.h
@@ -7,19 +7,123 @@
 #define APBC_NO_BUS_CTRL	BIT(0)
 #define APBC_POWER_CTRL		BIT(1)
 
-struct clk_factor_masks {
-	unsigned int	factor;
-	unsigned int	num_mask;
-	unsigned int	den_mask;
-	unsigned int	num_shift;
-	unsigned int	den_shift;
+
+/* Clock type "factor" */
+struct mmp_clk_factor_masks {
+	unsigned int factor;
+	unsigned int num_mask;
+	unsigned int den_mask;
+	unsigned int num_shift;
+	unsigned int den_shift;
 };
 
-struct clk_factor_tbl {
+struct mmp_clk_factor_tbl {
 	unsigned int num;
 	unsigned int den;
 };
 
+struct mmp_clk_factor {
+	struct clk_hw hw;
+	void __iomem *base;
+	struct mmp_clk_factor_masks *masks;
+	struct mmp_clk_factor_tbl *ftbl;
+	unsigned int ftbl_cnt;
+	spinlock_t *lock;
+};
+
+extern struct clk *mmp_clk_register_factor(const char *name,
+		const char *parent_name, unsigned long flags,
+		void __iomem *base, struct mmp_clk_factor_masks *masks,
+		struct mmp_clk_factor_tbl *ftbl, unsigned int ftbl_cnt,
+		spinlock_t *lock);
+
+/* Clock type "mix" */
+#define MMP_CLK_BITS_MASK(width, shift)			\
+		(((1 << (width)) - 1) << (shift))
+#define MMP_CLK_BITS_GET_VAL(data, width, shift)	\
+		((data & MMP_CLK_BITS_MASK(width, shift)) >> (shift))
+#define MMP_CLK_BITS_SET_VAL(val, width, shift)		\
+		(((val) << (shift)) & MMP_CLK_BITS_MASK(width, shift))
+
+enum {
+	MMP_CLK_MIX_TYPE_V1,
+	MMP_CLK_MIX_TYPE_V2,
+	MMP_CLK_MIX_TYPE_V3,
+};
+
+/* The register layout */
+struct mmp_clk_mix_reg_info {
+	void __iomem *reg_clk_ctrl;
+	void __iomem *reg_clk_sel;
+	u8 width_div;
+	u8 shift_div;
+	u8 width_mux;
+	u8 shift_mux;
+	u8 bit_fc;
+};
+
+/* The suggested clock table from user. */
+struct mmp_clk_mix_clk_table {
+	unsigned long rate;
+	u8 parent_index;
+	unsigned int divisor;
+	unsigned int valid;
+};
+
+struct mmp_clk_mix_config {
+	struct mmp_clk_mix_reg_info reg_info;
+	struct mmp_clk_mix_clk_table *table;
+	unsigned int table_size;
+	u32 *mux_table;
+	struct clk_div_table *div_table;
+	u8 div_flags;
+	u8 mux_flags;
+};
+
+struct mmp_clk_mix {
+	struct clk_hw hw;
+	struct mmp_clk_mix_reg_info reg_info;
+	struct mmp_clk_mix_clk_table *table;
+	u32 *mux_table;
+	struct clk_div_table *div_table;
+	unsigned int table_size;
+	u8 div_flags;
+	u8 mux_flags;
+	unsigned int type;
+	spinlock_t *lock;
+};
+
+extern const struct clk_ops mmp_clk_mix_ops;
+extern struct clk *mmp_clk_register_mix(struct device *dev,
+					const char *name,
+					const char **parent_names,
+					u8 num_parents,
+					unsigned long flags,
+					struct mmp_clk_mix_config *config,
+					spinlock_t *lock);
+
+
+/* Clock type "gate". MMP private gate */
+#define MMP_CLK_GATE_NEED_DELAY		BIT(0)
+
+struct mmp_clk_gate {
+	struct clk_hw hw;
+	void __iomem *reg;
+	u32 mask;
+	u32 val_enable;
+	u32 val_disable;
+	unsigned int flags;
+	spinlock_t *lock;
+};
+
+extern const struct clk_ops mmp_clk_gate_ops;
+extern struct clk *mmp_clk_register_gate(struct device *dev, const char *name,
+			const char *parent_name, unsigned long flags,
+			void __iomem *reg, u32 mask, u32 val_enable,
+			u32 val_disable, unsigned int gate_flags,
+			spinlock_t *lock);
+
+
 extern struct clk *mmp_clk_register_pll2(const char *name,
 		const char *parent_name, unsigned long flags);
 extern struct clk *mmp_clk_register_apbc(const char *name,
@@ -28,8 +132,108 @@ extern struct clk *mmp_clk_register_apbc(const char *name,
 extern struct clk *mmp_clk_register_apmu(const char *name,
 		const char *parent_name, void __iomem *base, u32 enable_mask,
 		spinlock_t *lock);
-extern struct clk *mmp_clk_register_factor(const char *name,
-		const char *parent_name, unsigned long flags,
-		void __iomem *base, struct clk_factor_masks *masks,
-		struct clk_factor_tbl *ftbl, unsigned int ftbl_cnt);
+
+struct mmp_clk_unit {
+	unsigned int nr_clks;
+	struct clk **clk_table;
+	struct clk_onecell_data clk_data;
+};
+
+struct mmp_param_fixed_rate_clk {
+	unsigned int id;
+	char *name;
+	const char *parent_name;
+	unsigned long flags;
+	unsigned long fixed_rate;
+};
+void mmp_register_fixed_rate_clks(struct mmp_clk_unit *unit,
+				struct mmp_param_fixed_rate_clk *clks,
+				int size);
+
+struct mmp_param_fixed_factor_clk {
+	unsigned int id;
+	char *name;
+	const char *parent_name;
+	unsigned long mult;
+	unsigned long div;
+	unsigned long flags;
+};
+void mmp_register_fixed_factor_clks(struct mmp_clk_unit *unit,
+				struct mmp_param_fixed_factor_clk *clks,
+				int size);
+
+struct mmp_param_general_gate_clk {
+	unsigned int id;
+	const char *name;
+	const char *parent_name;
+	unsigned long flags;
+	unsigned long offset;
+	u8 bit_idx;
+	u8 gate_flags;
+	spinlock_t *lock;
+};
+void mmp_register_general_gate_clks(struct mmp_clk_unit *unit,
+				struct mmp_param_general_gate_clk *clks,
+				void __iomem *base, int size);
+
+struct mmp_param_gate_clk {
+	unsigned int id;
+	char *name;
+	const char *parent_name;
+	unsigned long flags;
+	unsigned long offset;
+	u32 mask;
+	u32 val_enable;
+	u32 val_disable;
+	unsigned int gate_flags;
+	spinlock_t *lock;
+};
+void mmp_register_gate_clks(struct mmp_clk_unit *unit,
+			struct mmp_param_gate_clk *clks,
+			void __iomem *base, int size);
+
+struct mmp_param_mux_clk {
+	unsigned int id;
+	char *name;
+	const char **parent_name;
+	u8 num_parents;
+	unsigned long flags;
+	unsigned long offset;
+	u8 shift;
+	u8 width;
+	u8 mux_flags;
+	spinlock_t *lock;
+};
+void mmp_register_mux_clks(struct mmp_clk_unit *unit,
+			struct mmp_param_mux_clk *clks,
+			void __iomem *base, int size);
+
+struct mmp_param_div_clk {
+	unsigned int id;
+	char *name;
+	const char *parent_name;
+	unsigned long flags;
+	unsigned long offset;
+	u8 shift;
+	u8 width;
+	u8 div_flags;
+	spinlock_t *lock;
+};
+void mmp_register_div_clks(struct mmp_clk_unit *unit,
+			struct mmp_param_div_clk *clks,
+			void __iomem *base, int size);
+
+#define DEFINE_MIX_REG_INFO(w_d, s_d, w_m, s_m, fc)	\
+{							\
+	.width_div = (w_d),				\
+	.shift_div = (s_d),				\
+	.width_mux = (w_m),				\
+	.shift_mux = (s_m),				\
+	.bit_fc = (fc),					\
+}
+
+void mmp_clk_init(struct device_node *np, struct mmp_clk_unit *unit,
+		int nr_clks);
+void mmp_clk_add(struct mmp_clk_unit *unit, unsigned int id,
+		struct clk *clk);
 #endif
diff --git a/drivers/clk/mmp/reset.c b/drivers/clk/mmp/reset.c
new file mode 100644
index 000000000000..b54da1fe73f0
--- /dev/null
+++ b/drivers/clk/mmp/reset.c
@@ -0,0 +1,99 @@
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/reset-controller.h>
+
+#include "reset.h"
+
+#define rcdev_to_unit(rcdev) container_of(rcdev, struct mmp_clk_reset_unit, rcdev)
+
+static int mmp_of_reset_xlate(struct reset_controller_dev *rcdev,
+			  const struct of_phandle_args *reset_spec)
+{
+	struct mmp_clk_reset_unit *unit = rcdev_to_unit(rcdev);
+	struct mmp_clk_reset_cell *cell;
+	int i;
+
+	if (WARN_ON(reset_spec->args_count != rcdev->of_reset_n_cells))
+		return -EINVAL;
+
+	for (i = 0; i < rcdev->nr_resets; i++) {
+		cell = &unit->cells[i];
+		if (cell->clk_id == reset_spec->args[0])
+			break;
+	}
+
+	if (i == rcdev->nr_resets)
+		return -EINVAL;
+
+	return i;
+}
+
+static int mmp_clk_reset_assert(struct reset_controller_dev *rcdev,
+				unsigned long id)
+{
+	struct mmp_clk_reset_unit *unit = rcdev_to_unit(rcdev);
+	struct mmp_clk_reset_cell *cell;
+	unsigned long flags = 0;
+	u32 val;
+
+	cell = &unit->cells[id];
+	if (cell->lock)
+		spin_lock_irqsave(cell->lock, flags);
+
+	val = readl(cell->reg);
+	val |= cell->bits;
+	writel(val, cell->reg);
+
+	if (cell->lock)
+		spin_unlock_irqrestore(cell->lock, flags);
+
+	return 0;
+}
+
+static int mmp_clk_reset_deassert(struct reset_controller_dev *rcdev,
+				unsigned long id)
+{
+	struct mmp_clk_reset_unit *unit = rcdev_to_unit(rcdev);
+	struct mmp_clk_reset_cell *cell;
+	unsigned long flags = 0;
+	u32 val;
+
+	cell = &unit->cells[id];
+	if (cell->lock)
+		spin_lock_irqsave(cell->lock, flags);
+
+	val = readl(cell->reg);
+	val &= ~cell->bits;
+	writel(val, cell->reg);
+
+	if (cell->lock)
+		spin_unlock_irqrestore(cell->lock, flags);
+
+	return 0;
+}
+
+static struct reset_control_ops mmp_clk_reset_ops = {
+	.assert		= mmp_clk_reset_assert,
+	.deassert	= mmp_clk_reset_deassert,
+};
+
+void mmp_clk_reset_register(struct device_node *np,
+			struct mmp_clk_reset_cell *cells, int nr_resets)
+{
+	struct mmp_clk_reset_unit *unit;
+
+	unit = kzalloc(sizeof(*unit), GFP_KERNEL);
+	if (!unit)
+		return;
+
+	unit->cells = cells;
+	unit->rcdev.of_reset_n_cells = 1;
+	unit->rcdev.nr_resets = nr_resets;
+	unit->rcdev.ops = &mmp_clk_reset_ops;
+	unit->rcdev.of_node = np;
+	unit->rcdev.of_xlate = mmp_of_reset_xlate;
+
+	reset_controller_register(&unit->rcdev);
+}
diff --git a/drivers/clk/mmp/reset.h b/drivers/clk/mmp/reset.h
new file mode 100644
index 000000000000..be8b1a7000f7
--- /dev/null
+++ b/drivers/clk/mmp/reset.h
@@ -0,0 +1,31 @@
+#ifndef __MACH_MMP_CLK_RESET_H
+#define __MACH_MMP_CLK_RESET_H
+
+#include <linux/reset-controller.h>
+
+#define MMP_RESET_INVERT	1
+
+struct mmp_clk_reset_cell {
+	unsigned int clk_id;
+	void __iomem *reg;
+	u32 bits;
+	unsigned int flags;
+	spinlock_t *lock;
+};
+
+struct mmp_clk_reset_unit {
+	struct reset_controller_dev rcdev;
+	struct mmp_clk_reset_cell *cells;
+};
+
+#ifdef CONFIG_RESET_CONTROLLER
+void mmp_clk_reset_register(struct device_node *np,
+			struct mmp_clk_reset_cell *cells, int nr_resets);
+#else
+static inline void mmp_clk_reset_register(struct device_node *np,
+			struct mmp_clk_reset_cell *cells, int nr_resets)
+{
+}
+#endif
+
+#endif
diff --git a/drivers/clk/pxa/Makefile b/drivers/clk/pxa/Makefile
index 4ff2abcd500b..38e915344605 100644
--- a/drivers/clk/pxa/Makefile
+++ b/drivers/clk/pxa/Makefile
@@ -1,2 +1,3 @@
 obj-y				+= clk-pxa.o
+obj-$(CONFIG_PXA25x)		+= clk-pxa25x.o
 obj-$(CONFIG_PXA27x)		+= clk-pxa27x.o
diff --git a/drivers/clk/pxa/clk-pxa.c b/drivers/clk/pxa/clk-pxa.c
index ef3c05389c0a..4e834753ab09 100644
--- a/drivers/clk/pxa/clk-pxa.c
+++ b/drivers/clk/pxa/clk-pxa.c
@@ -26,12 +26,20 @@ static struct clk_onecell_data onecell_data = {
 	.clk_num = CLK_MAX,
 };
 
-#define to_pxa_clk(_hw) container_of(_hw, struct pxa_clk_cken, hw)
+struct pxa_clk {
+	struct clk_hw hw;
+	struct clk_fixed_factor lp;
+	struct clk_fixed_factor hp;
+	struct clk_gate gate;
+	bool (*is_in_low_power)(void);
+};
+
+#define to_pxa_clk(_hw) container_of(_hw, struct pxa_clk, hw)
 
 static unsigned long cken_recalc_rate(struct clk_hw *hw,
 				      unsigned long parent_rate)
 {
-	struct pxa_clk_cken *pclk = to_pxa_clk(hw);
+	struct pxa_clk *pclk = to_pxa_clk(hw);
 	struct clk_fixed_factor *fix;
 
 	if (!pclk->is_in_low_power || pclk->is_in_low_power())
@@ -48,7 +56,7 @@ static struct clk_ops cken_rate_ops = {
 
 static u8 cken_get_parent(struct clk_hw *hw)
 {
-	struct pxa_clk_cken *pclk = to_pxa_clk(hw);
+	struct pxa_clk *pclk = to_pxa_clk(hw);
 
 	if (!pclk->is_in_low_power)
 		return 0;
@@ -69,29 +77,32 @@ void __init clkdev_pxa_register(int ckid, const char *con_id,
 		clk_register_clkdev(clk, con_id, dev_id);
 }
 
-int __init clk_pxa_cken_init(struct pxa_clk_cken *clks, int nb_clks)
+int __init clk_pxa_cken_init(const struct desc_clk_cken *clks, int nb_clks)
 {
 	int i;
-	struct pxa_clk_cken *pclk;
+	struct pxa_clk *pxa_clk;
 	struct clk *clk;
 
 	for (i = 0; i < nb_clks; i++) {
-		pclk = clks + i;
-		pclk->gate.lock = &lock;
-		clk = clk_register_composite(NULL, pclk->name,
-					     pclk->parent_names, 2,
-					     &pclk->hw, &cken_mux_ops,
-					     &pclk->hw, &cken_rate_ops,
-					     &pclk->gate.hw, &clk_gate_ops,
-					     pclk->flags);
-		clkdev_pxa_register(pclk->ckid, pclk->con_id, pclk->dev_id,
-				    clk);
+		pxa_clk = kzalloc(sizeof(*pxa_clk), GFP_KERNEL);
+		pxa_clk->is_in_low_power = clks[i].is_in_low_power;
+		pxa_clk->lp = clks[i].lp;
+		pxa_clk->hp = clks[i].hp;
+		pxa_clk->gate = clks[i].gate;
+		pxa_clk->gate.lock = &lock;
+		clk = clk_register_composite(NULL, clks[i].name,
+					     clks[i].parent_names, 2,
+					     &pxa_clk->hw, &cken_mux_ops,
+					     &pxa_clk->hw, &cken_rate_ops,
+					     &pxa_clk->gate.hw, &clk_gate_ops,
+					     clks[i].flags);
+		clkdev_pxa_register(clks[i].ckid, clks[i].con_id,
+				    clks[i].dev_id, clk);
 	}
 	return 0;
 }
 
-static void __init pxa_dt_clocks_init(struct device_node *np)
+void __init clk_pxa_dt_common_init(struct device_node *np)
 {
 	of_clk_add_provider(np, of_clk_src_onecell_get, &onecell_data);
 }
-CLK_OF_DECLARE(pxa_clks, "marvell,pxa-clocks", pxa_dt_clocks_init);
diff --git a/drivers/clk/pxa/clk-pxa.h b/drivers/clk/pxa/clk-pxa.h
index 5fe219d06b49..323965430111 100644
--- a/drivers/clk/pxa/clk-pxa.h
+++ b/drivers/clk/pxa/clk-pxa.h
@@ -25,7 +25,7 @@
 	static struct clk_ops name ## _rate_ops = {		\
 		.recalc_rate = name ## _get_rate,		\
 	};							\
-	static struct clk *clk_register_ ## name(void)		\
+	static struct clk * __init clk_register_ ## name(void)	\
 	{							\
 		return clk_register_composite(NULL, clk_name,	\
 			name ## _parents,			\
@@ -40,7 +40,7 @@
 	static struct clk_ops name ## _rate_ops = {		\
 		.recalc_rate = name ## _get_rate,		\
 	};							\
-	static struct clk *clk_register_ ## name(void)		\
+	static struct clk * __init clk_register_ ## name(void)	\
 	{							\
 		return clk_register_composite(NULL, clk_name,	\
 			name ## _parents,			\
@@ -66,7 +66,7 @@
  *  |    Clock   | --- | / div_hp  |
  *  +------------+     +-----------+
  */
-struct pxa_clk_cken {
+struct desc_clk_cken {
 	struct clk_hw hw;
 	int ckid;
 	const char *name;
@@ -102,6 +102,7 @@ static int dummy_clk_set_parent(struct clk_hw *hw, u8 index)
 
 extern void clkdev_pxa_register(int ckid, const char *con_id,
 				const char *dev_id, struct clk *clk);
-extern int clk_pxa_cken_init(struct pxa_clk_cken *clks, int nb_clks);
+extern int clk_pxa_cken_init(const struct desc_clk_cken *clks, int nb_clks);
+void clk_pxa_dt_common_init(struct device_node *np);
 
 #endif
diff --git a/drivers/clk/pxa/clk-pxa25x.c b/drivers/clk/pxa/clk-pxa25x.c
new file mode 100644
index 000000000000..6cd88d963a7f
--- /dev/null
+++ b/drivers/clk/pxa/clk-pxa25x.c
@@ -0,0 +1,273 @@
+/*
+ * Marvell PXA25x family clocks
+ *
+ * Copyright (C) 2014 Robert Jarzmik
+ *
+ * Heavily inspired from former arch/arm/mach-pxa/pxa25x.c.
+ *
+ * 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.
+ *
+ * For non-devicetree platforms. Once pxa is fully converted to devicetree, this
+ * should go away.
+ */
+#include <linux/clk-provider.h>
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <mach/pxa25x.h>
+#include <mach/pxa2xx-regs.h>
+
+#include <dt-bindings/clock/pxa-clock.h>
+#include "clk-pxa.h"
+
+#define KHz 1000
+#define MHz (1000 * 1000)
+
+enum {
+	PXA_CORE_RUN = 0,
+	PXA_CORE_TURBO,
+};
+
+/*
+ * Various clock factors driven by the CCCR register.
+ */
+
+/* Crystal Frequency to Memory Frequency Multiplier (L) */
+static unsigned char L_clk_mult[32] = { 0, 27, 32, 36, 40, 45, 0, };
+
+/* Memory Frequency to Run Mode Frequency Multiplier (M) */
+static unsigned char M_clk_mult[4] = { 0, 1, 2, 4 };
+
+/* Run Mode Frequency to Turbo Mode Frequency Multiplier (N) */
+/* Note: we store the value N * 2 here. */
+static unsigned char N2_clk_mult[8] = { 0, 0, 2, 3, 4, 0, 6, 0 };
+
+static const char * const get_freq_khz[] = {
+	"core", "run", "cpll", "memory"
+};
+
+/*
+ * Get the clock frequency as reflected by CCCR and the turbo flag.
+ * We assume these values have been applied via a fcs.
+ * If info is not 0 we also display the current settings.
+ */
+unsigned int pxa25x_get_clk_frequency_khz(int info)
+{
+	struct clk *clk;
+	unsigned long clks[5];
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(get_freq_khz); i++) {
+		clk = clk_get(NULL, get_freq_khz[i]);
+		if (IS_ERR(clk)) {
+			clks[i] = 0;
+		} else {
+			clks[i] = clk_get_rate(clk);
+			clk_put(clk);
+		}
+	}
+
+	if (info) {
+		pr_info("Run Mode clock: %ld.%02ldMHz\n",
+			clks[1] / 1000000, (clks[1] % 1000000) / 10000);
+		pr_info("Turbo Mode clock: %ld.%02ldMHz\n",
+			clks[2] / 1000000, (clks[2] % 1000000) / 10000);
+		pr_info("Memory clock: %ld.%02ldMHz\n",
+			clks[3] / 1000000, (clks[3] % 1000000) / 10000);
+	}
+
+	return (unsigned int)clks[0];
+}
+
+static unsigned long clk_pxa25x_memory_get_rate(struct clk_hw *hw,
+						unsigned long parent_rate)
+{
+	unsigned long cccr = CCCR;
+	unsigned int m = M_clk_mult[(cccr >> 5) & 0x03];
+
+	return parent_rate / m;
+}
+PARENTS(clk_pxa25x_memory) = { "run" };
+RATE_RO_OPS(clk_pxa25x_memory, "memory");
+
+PARENTS(pxa25x_pbus95) = { "ppll_95_85mhz", "ppll_95_85mhz" };
+PARENTS(pxa25x_pbus147) = { "ppll_147_46mhz", "ppll_147_46mhz" };
+PARENTS(pxa25x_osc3) = { "osc_3_6864mhz", "osc_3_6864mhz" };
+
+#define PXA25X_CKEN(dev_id, con_id, parents, mult, div,			\
+		    bit, is_lp, flags)					\
+	PXA_CKEN(dev_id, con_id, bit, parents, mult, div, mult, div,	\
+		 is_lp,  &CKEN, CKEN_ ## bit, flags)
+#define PXA25X_PBUS95_CKEN(dev_id, con_id, bit, mult_hp, div_hp, delay)	\
+	PXA25X_CKEN(dev_id, con_id, pxa25x_pbus95_parents, mult_hp,	\
+		    div_hp, bit, NULL, 0)
+#define PXA25X_PBUS147_CKEN(dev_id, con_id, bit, mult_hp, div_hp, delay)\
+	PXA25X_CKEN(dev_id, con_id, pxa25x_pbus147_parents, mult_hp,	\
+		    div_hp, bit, NULL, 0)
+#define PXA25X_OSC3_CKEN(dev_id, con_id, bit, mult_hp, div_hp, delay)	\
+	PXA25X_CKEN(dev_id, con_id, pxa25x_osc3_parents, mult_hp,	\
+		    div_hp, bit, NULL, 0)
+
+#define PXA25X_CKEN_1RATE(dev_id, con_id, bit, parents, delay)		\
+	PXA_CKEN_1RATE(dev_id, con_id, bit, parents,			\
+		       &CKEN, CKEN_ ## bit, 0)
+#define PXA25X_CKEN_1RATE_AO(dev_id, con_id, bit, parents, delay)	\
+	PXA_CKEN_1RATE(dev_id, con_id, bit, parents,			\
+		       &CKEN, CKEN_ ## bit, CLK_IGNORE_UNUSED)
+
+static struct desc_clk_cken pxa25x_clocks[] __initdata = {
+	PXA25X_PBUS95_CKEN("pxa2xx-mci.0", NULL, MMC, 1, 5, 0),
+	PXA25X_PBUS95_CKEN("pxa2xx-i2c.0", NULL, I2C, 1, 3, 0),
+	PXA25X_PBUS95_CKEN("pxa2xx-ir", "FICPCLK", FICP, 1, 2, 0),
+	PXA25X_PBUS95_CKEN("pxa25x-udc", NULL, USB, 1, 2, 5),
+	PXA25X_PBUS147_CKEN("pxa2xx-uart.0", NULL, FFUART, 1, 10, 1),
+	PXA25X_PBUS147_CKEN("pxa2xx-uart.1", NULL, BTUART, 1, 10, 1),
+	PXA25X_PBUS147_CKEN("pxa2xx-uart.2", NULL, STUART, 1, 10, 1),
+	PXA25X_PBUS147_CKEN("pxa2xx-uart.3", NULL, HWUART, 1, 10, 1),
+	PXA25X_PBUS147_CKEN("pxa2xx-i2s", NULL, I2S, 1, 10, 0),
+	PXA25X_PBUS147_CKEN(NULL, "AC97CLK", AC97, 1, 12, 0),
+	PXA25X_OSC3_CKEN("pxa25x-ssp.0", NULL, SSP, 1, 1, 0),
+	PXA25X_OSC3_CKEN("pxa25x-nssp.1", NULL, NSSP, 1, 1, 0),
+	PXA25X_OSC3_CKEN("pxa25x-nssp.2", NULL, ASSP, 1, 1, 0),
+	PXA25X_OSC3_CKEN("pxa25x-pwm.0", NULL, PWM0, 1, 1, 0),
+	PXA25X_OSC3_CKEN("pxa25x-pwm.1", NULL, PWM1, 1, 1, 0),
+
+	PXA25X_CKEN_1RATE("pxa2xx-fb", NULL, LCD, clk_pxa25x_memory_parents, 0),
+	PXA25X_CKEN_1RATE_AO("pxa2xx-pcmcia", NULL, MEMC,
+			     clk_pxa25x_memory_parents, 0),
+};
+
+static u8 clk_pxa25x_core_get_parent(struct clk_hw *hw)
+{
+	unsigned long clkcfg;
+	unsigned int t;
+
+	asm("mrc\tp14, 0, %0, c6, c0, 0" : "=r" (clkcfg));
+	t  = clkcfg & (1 << 0);
+	if (t)
+		return PXA_CORE_TURBO;
+	return PXA_CORE_RUN;
+}
+
+static unsigned long clk_pxa25x_core_get_rate(struct clk_hw *hw,
+					      unsigned long parent_rate)
+{
+	return parent_rate;
+}
+PARENTS(clk_pxa25x_core) = { "run", "cpll" };
+MUX_RO_RATE_RO_OPS(clk_pxa25x_core, "core");
+
+static unsigned long clk_pxa25x_run_get_rate(struct clk_hw *hw,
+					     unsigned long parent_rate)
+{
+	unsigned long cccr = CCCR;
+	unsigned int n2 = N2_clk_mult[(cccr >> 7) & 0x07];
+
+	return (parent_rate / n2) * 2;
+}
+PARENTS(clk_pxa25x_run) = { "cpll" };
+RATE_RO_OPS(clk_pxa25x_run, "run");
+
+static unsigned long clk_pxa25x_cpll_get_rate(struct clk_hw *hw,
+	unsigned long parent_rate)
+{
+	unsigned long clkcfg, cccr = CCCR;
+	unsigned int l, m, n2, t;
+
+	asm("mrc\tp14, 0, %0, c6, c0, 0" : "=r" (clkcfg));
+	t = clkcfg & (1 << 0);
+	l  =  L_clk_mult[(cccr >> 0) & 0x1f];
+	m = M_clk_mult[(cccr >> 5) & 0x03];
+	n2 = N2_clk_mult[(cccr >> 7) & 0x07];
+
+	if (t)
+		return m * l * n2 * parent_rate / 2;
+	return m * l * parent_rate;
+}
+PARENTS(clk_pxa25x_cpll) = { "osc_3_6864mhz" };
+RATE_RO_OPS(clk_pxa25x_cpll, "cpll");
+
+static void __init pxa25x_register_core(void)
+{
+	clk_register_clk_pxa25x_cpll();
+	clk_register_clk_pxa25x_run();
+	clkdev_pxa_register(CLK_CORE, "core", NULL,
+			    clk_register_clk_pxa25x_core());
+}
+
+static void __init pxa25x_register_plls(void)
+{
+	clk_register_fixed_rate(NULL, "osc_3_6864mhz", NULL,
+				CLK_GET_RATE_NOCACHE | CLK_IS_ROOT,
+				3686400);
+	clk_register_fixed_rate(NULL, "osc_32_768khz", NULL,
+				CLK_GET_RATE_NOCACHE | CLK_IS_ROOT,
+				32768);
+	clk_register_fixed_rate(NULL, "clk_dummy", NULL, CLK_IS_ROOT, 0);
+	clk_register_fixed_factor(NULL, "ppll_95_85mhz", "osc_3_6864mhz",
+				  0, 26, 1);
+	clk_register_fixed_factor(NULL, "ppll_147_46mhz", "osc_3_6864mhz",
+				  0, 40, 1);
+}
+
+static void __init pxa25x_base_clocks_init(void)
+{
+	pxa25x_register_plls();
+	pxa25x_register_core();
+	clk_register_clk_pxa25x_memory();
+}
+
+#define DUMMY_CLK(_con_id, _dev_id, _parent) \
+	{ .con_id = _con_id, .dev_id = _dev_id, .parent = _parent }
+struct dummy_clk {
+	const char *con_id;
+	const char *dev_id;
+	const char *parent;
+};
+static struct dummy_clk dummy_clks[] __initdata = {
+	DUMMY_CLK(NULL, "pxa25x-gpio", "osc_32_768khz"),
+	DUMMY_CLK(NULL, "pxa26x-gpio", "osc_32_768khz"),
+	DUMMY_CLK("GPIO11_CLK", NULL, "osc_3_6864mhz"),
+	DUMMY_CLK("GPIO12_CLK", NULL, "osc_32_768khz"),
+	DUMMY_CLK(NULL, "sa1100-rtc", "osc_32_768khz"),
+	DUMMY_CLK("OSTIMER0", NULL, "osc_32_768khz"),
+	DUMMY_CLK("UARTCLK", "pxa2xx-ir", "STUART"),
+};
+
+static void __init pxa25x_dummy_clocks_init(void)
+{
+	struct clk *clk;
+	struct dummy_clk *d;
+	const char *name;
+	int i;
+
+	/*
+	 * All pinctrl logic has been wiped out of the clock driver, especially
+	 * for gpio11 and gpio12 outputs. Machine code should ensure proper pin
+	 * control (ie. pxa2xx_mfp_config() invocation).
+	 */
+	for (i = 0; i < ARRAY_SIZE(dummy_clks); i++) {
+		d = &dummy_clks[i];
+		name = d->dev_id ? d->dev_id : d->con_id;
+		clk = clk_register_fixed_factor(NULL, name, d->parent, 0, 1, 1);
+		clk_register_clkdev(clk, d->con_id, d->dev_id);
+	}
+}
+
+int __init pxa25x_clocks_init(void)
+{
+	pxa25x_base_clocks_init();
+	pxa25x_dummy_clocks_init();
+	return clk_pxa_cken_init(pxa25x_clocks, ARRAY_SIZE(pxa25x_clocks));
+}
+
+static void __init pxa25x_dt_clocks_init(struct device_node *np)
+{
+	pxa25x_clocks_init();
+	clk_pxa_dt_common_init(np);
+}
+CLK_OF_DECLARE(pxa25x_clks, "marvell,pxa250-core-clocks",
+	       pxa25x_dt_clocks_init);
diff --git a/drivers/clk/pxa/clk-pxa27x.c b/drivers/clk/pxa/clk-pxa27x.c
index 88b9fe13fa44..5f9b54b024b9 100644
--- a/drivers/clk/pxa/clk-pxa27x.c
+++ b/drivers/clk/pxa/clk-pxa27x.c
@@ -111,7 +111,7 @@ PARENTS(pxa27x_membus) = { "lcd_base", "lcd_base" };
 	PXA_CKEN_1RATE(dev_id, con_id, bit, parents,			\
 		       &CKEN, CKEN_ ## bit, CLK_IGNORE_UNUSED)
 
-static struct pxa_clk_cken pxa27x_clocks[] = {
+static struct desc_clk_cken pxa27x_clocks[] __initdata = {
 	PXA27X_PBUS_CKEN("pxa2xx-uart.0", NULL, FFUART, 2, 42, 1),
 	PXA27X_PBUS_CKEN("pxa2xx-uart.1", NULL, BTUART, 2, 42, 1),
 	PXA27X_PBUS_CKEN("pxa2xx-uart.2", NULL, STUART, 2, 42, 1),
@@ -368,3 +368,10 @@ static int __init pxa27x_clocks_init(void)
 	return clk_pxa_cken_init(pxa27x_clocks, ARRAY_SIZE(pxa27x_clocks));
 }
 postcore_initcall(pxa27x_clocks_init);
+
+static void __init pxa27x_dt_clocks_init(struct device_node *np)
+{
+	pxa27x_clocks_init();
+	clk_pxa_dt_common_init(np);
+}
+CLK_OF_DECLARE(pxa_clks, "marvell,pxa270-clocks", pxa27x_dt_clocks_init);
diff --git a/drivers/clk/qcom/clk-pll.c b/drivers/clk/qcom/clk-pll.c
index b823bc3b6250..60873a7f45d9 100644
--- a/drivers/clk/qcom/clk-pll.c
+++ b/drivers/clk/qcom/clk-pll.c
@@ -141,7 +141,7 @@ struct pll_freq_tbl *find_freq(const struct pll_freq_tbl *f, unsigned long rate)
 
 static long
 clk_pll_determine_rate(struct clk_hw *hw, unsigned long rate,
-		       unsigned long *p_rate, struct clk **p)
+		       unsigned long *p_rate, struct clk_hw **p)
 {
 	struct clk_pll *pll = to_clk_pll(hw);
 	const struct pll_freq_tbl *f;
diff --git a/drivers/clk/qcom/clk-rcg.c b/drivers/clk/qcom/clk-rcg.c
index b6e6959e89aa..0b93972c8807 100644
--- a/drivers/clk/qcom/clk-rcg.c
+++ b/drivers/clk/qcom/clk-rcg.c
@@ -368,16 +368,17 @@ clk_dyn_rcg_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
 
 static long _freq_tbl_determine_rate(struct clk_hw *hw,
 		const struct freq_tbl *f, unsigned long rate,
-		unsigned long *p_rate, struct clk **p)
+		unsigned long *p_rate, struct clk_hw **p_hw)
 {
 	unsigned long clk_flags;
+	struct clk *p;
 
 	f = qcom_find_freq(f, rate);
 	if (!f)
 		return -EINVAL;
 
 	clk_flags = __clk_get_flags(hw->clk);
-	*p = clk_get_parent_by_index(hw->clk, f->src);
+	p = clk_get_parent_by_index(hw->clk, f->src);
 	if (clk_flags & CLK_SET_RATE_PARENT) {
 		rate = rate * f->pre_div;
 		if (f->n) {
@@ -387,15 +388,16 @@ static long _freq_tbl_determine_rate(struct clk_hw *hw,
 			rate = tmp;
 		}
 	} else {
-		rate =  __clk_get_rate(*p);
+		rate =  __clk_get_rate(p);
 	}
+	*p_hw = __clk_get_hw(p);
 	*p_rate = rate;
 
 	return f->freq;
 }
 
 static long clk_rcg_determine_rate(struct clk_hw *hw, unsigned long rate,
-		unsigned long *p_rate, struct clk **p)
+		unsigned long *p_rate, struct clk_hw **p)
 {
 	struct clk_rcg *rcg = to_clk_rcg(hw);
 
@@ -403,7 +405,7 @@ static long clk_rcg_determine_rate(struct clk_hw *hw, unsigned long rate,
 }
 
 static long clk_dyn_rcg_determine_rate(struct clk_hw *hw, unsigned long rate,
-		unsigned long *p_rate, struct clk **p)
+		unsigned long *p_rate, struct clk_hw **p)
 {
 	struct clk_dyn_rcg *rcg = to_clk_dyn_rcg(hw);
 
@@ -411,13 +413,15 @@ static long clk_dyn_rcg_determine_rate(struct clk_hw *hw, unsigned long rate,
 }
 
 static long clk_rcg_bypass_determine_rate(struct clk_hw *hw, unsigned long rate,
-		unsigned long *p_rate, struct clk **p)
+		unsigned long *p_rate, struct clk_hw **p_hw)
 {
 	struct clk_rcg *rcg = to_clk_rcg(hw);
 	const struct freq_tbl *f = rcg->freq_tbl;
+	struct clk *p;
 
-	*p = clk_get_parent_by_index(hw->clk, f->src);
-	*p_rate = __clk_round_rate(*p, rate);
+	p = clk_get_parent_by_index(hw->clk, f->src);
+	*p_hw = __clk_get_hw(p);
+	*p_rate = __clk_round_rate(p, rate);
 
 	return *p_rate;
 }
diff --git a/drivers/clk/qcom/clk-rcg2.c b/drivers/clk/qcom/clk-rcg2.c
index cfa9eb4fe9ca..08b8b3729f53 100644
--- a/drivers/clk/qcom/clk-rcg2.c
+++ b/drivers/clk/qcom/clk-rcg2.c
@@ -175,16 +175,17 @@ clk_rcg2_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
 
 static long _freq_tbl_determine_rate(struct clk_hw *hw,
 		const struct freq_tbl *f, unsigned long rate,
-		unsigned long *p_rate, struct clk **p)
+		unsigned long *p_rate, struct clk_hw **p_hw)
 {
 	unsigned long clk_flags;
+	struct clk *p;
 
 	f = qcom_find_freq(f, rate);
 	if (!f)
 		return -EINVAL;
 
 	clk_flags = __clk_get_flags(hw->clk);
-	*p = clk_get_parent_by_index(hw->clk, f->src);
+	p = clk_get_parent_by_index(hw->clk, f->src);
 	if (clk_flags & CLK_SET_RATE_PARENT) {
 		if (f->pre_div) {
 			rate /= 2;
@@ -198,15 +199,16 @@ static long _freq_tbl_determine_rate(struct clk_hw *hw,
 			rate = tmp;
 		}
 	} else {
-		rate =  __clk_get_rate(*p);
+		rate =  __clk_get_rate(p);
 	}
+	*p_hw = __clk_get_hw(p);
 	*p_rate = rate;
 
 	return f->freq;
 }
 
 static long clk_rcg2_determine_rate(struct clk_hw *hw, unsigned long rate,
-		unsigned long *p_rate, struct clk **p)
+		unsigned long *p_rate, struct clk_hw **p)
 {
 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 
@@ -359,7 +361,7 @@ static int clk_edp_pixel_set_rate_and_parent(struct clk_hw *hw,
 }
 
 static long clk_edp_pixel_determine_rate(struct clk_hw *hw, unsigned long rate,
-				 unsigned long *p_rate, struct clk **p)
+				 unsigned long *p_rate, struct clk_hw **p)
 {
 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 	const struct freq_tbl *f = rcg->freq_tbl;
@@ -371,7 +373,7 @@ static long clk_edp_pixel_determine_rate(struct clk_hw *hw, unsigned long rate,
 	u32 hid_div;
 
 	/* Force the correct parent */
-	*p = clk_get_parent_by_index(hw->clk, f->src);
+	*p = __clk_get_hw(clk_get_parent_by_index(hw->clk, f->src));
 
 	if (src_rate == 810000000)
 		frac = frac_table_810m;
@@ -410,18 +412,20 @@ const struct clk_ops clk_edp_pixel_ops = {
 EXPORT_SYMBOL_GPL(clk_edp_pixel_ops);
 
 static long clk_byte_determine_rate(struct clk_hw *hw, unsigned long rate,
-			 unsigned long *p_rate, struct clk **p)
+			 unsigned long *p_rate, struct clk_hw **p_hw)
 {
 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 	const struct freq_tbl *f = rcg->freq_tbl;
 	unsigned long parent_rate, div;
 	u32 mask = BIT(rcg->hid_width) - 1;
+	struct clk *p;
 
 	if (rate == 0)
 		return -EINVAL;
 
-	*p = clk_get_parent_by_index(hw->clk, f->src);
-	*p_rate = parent_rate = __clk_round_rate(*p, rate);
+	p = clk_get_parent_by_index(hw->clk, f->src);
+	*p_hw = __clk_get_hw(p);
+	*p_rate = parent_rate = __clk_round_rate(p, rate);
 
 	div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;
 	div = min_t(u32, div, mask);
@@ -472,14 +476,16 @@ static const struct frac_entry frac_table_pixel[] = {
 };
 
 static long clk_pixel_determine_rate(struct clk_hw *hw, unsigned long rate,
-				 unsigned long *p_rate, struct clk **p)
+				 unsigned long *p_rate, struct clk_hw **p)
 {
 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 	unsigned long request, src_rate;
 	int delta = 100000;
 	const struct freq_tbl *f = rcg->freq_tbl;
 	const struct frac_entry *frac = frac_table_pixel;
-	struct clk *parent = *p = clk_get_parent_by_index(hw->clk, f->src);
+	struct clk *parent = clk_get_parent_by_index(hw->clk, f->src);
+
+	*p = __clk_get_hw(parent);
 
 	for (; frac->num; frac++) {
 		request = (rate * frac->den) / frac->num;
diff --git a/drivers/clk/qcom/gcc-apq8084.c b/drivers/clk/qcom/gcc-apq8084.c
index ee52eb1c838a..e3ef90264214 100644
--- a/drivers/clk/qcom/gcc-apq8084.c
+++ b/drivers/clk/qcom/gcc-apq8084.c
@@ -3589,7 +3589,6 @@ static struct platform_driver gcc_apq8084_driver = {
 	.remove		= gcc_apq8084_remove,
 	.driver		= {
 		.name	= "gcc-apq8084",
-		.owner	= THIS_MODULE,
 		.of_match_table = gcc_apq8084_match_table,
 	},
 };
diff --git a/drivers/clk/qcom/gcc-ipq806x.c b/drivers/clk/qcom/gcc-ipq806x.c
index 5cd62a709ac7..afed5eb0691e 100644
--- a/drivers/clk/qcom/gcc-ipq806x.c
+++ b/drivers/clk/qcom/gcc-ipq806x.c
@@ -2431,7 +2431,6 @@ static struct platform_driver gcc_ipq806x_driver = {
 	.remove		= gcc_ipq806x_remove,
 	.driver		= {
 		.name	= "gcc-ipq806x",
-		.owner	= THIS_MODULE,
 		.of_match_table = gcc_ipq806x_match_table,
 	},
 };
diff --git a/drivers/clk/qcom/gcc-msm8660.c b/drivers/clk/qcom/gcc-msm8660.c
index 0c4b727ae429..f366e68f7316 100644
--- a/drivers/clk/qcom/gcc-msm8660.c
+++ b/drivers/clk/qcom/gcc-msm8660.c
@@ -2744,7 +2744,6 @@ static struct platform_driver gcc_msm8660_driver = {
 	.remove		= gcc_msm8660_remove,
 	.driver		= {
 		.name	= "gcc-msm8660",
-		.owner	= THIS_MODULE,
 		.of_match_table = gcc_msm8660_match_table,
 	},
 };
diff --git a/drivers/clk/qcom/gcc-msm8960.c b/drivers/clk/qcom/gcc-msm8960.c
index 007534f7a2d7..b0b562b9ce0e 100644
--- a/drivers/clk/qcom/gcc-msm8960.c
+++ b/drivers/clk/qcom/gcc-msm8960.c
@@ -3519,7 +3519,6 @@ static struct platform_driver gcc_msm8960_driver = {
 	.remove		= gcc_msm8960_remove,
 	.driver		= {
 		.name	= "gcc-msm8960",
-		.owner	= THIS_MODULE,
 		.of_match_table = gcc_msm8960_match_table,
 	},
 };
diff --git a/drivers/clk/qcom/gcc-msm8974.c b/drivers/clk/qcom/gcc-msm8974.c
index 7af7c18d2144..a6937fe78d8a 100644
--- a/drivers/clk/qcom/gcc-msm8974.c
+++ b/drivers/clk/qcom/gcc-msm8974.c
@@ -2737,7 +2737,6 @@ static struct platform_driver gcc_msm8974_driver = {
 	.remove		= gcc_msm8974_remove,
 	.driver		= {
 		.name	= "gcc-msm8974",
-		.owner	= THIS_MODULE,
 		.of_match_table = gcc_msm8974_match_table,
 	},
 };
diff --git a/drivers/clk/rockchip/Makefile b/drivers/clk/rockchip/Makefile
index bd8514d63634..2714097f90db 100644
--- a/drivers/clk/rockchip/Makefile
+++ b/drivers/clk/rockchip/Makefile
@@ -6,6 +6,7 @@ obj-y	+= clk-rockchip.o
 obj-y	+= clk.o
 obj-y	+= clk-pll.o
 obj-y	+= clk-cpu.o
+obj-y	+= clk-mmc-phase.o
 obj-$(CONFIG_RESET_CONTROLLER)	+= softrst.o
 
 obj-y	+= clk-rk3188.o
diff --git a/drivers/clk/rockchip/clk-mmc-phase.c b/drivers/clk/rockchip/clk-mmc-phase.c
new file mode 100644
index 000000000000..c842e3b60f21
--- /dev/null
+++ b/drivers/clk/rockchip/clk-mmc-phase.c
@@ -0,0 +1,154 @@
+/*
+ * Copyright 2014 Google, Inc
+ * Author: Alexandru M Stan <amstan@chromium.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/slab.h>
+#include <linux/clk-provider.h>
+#include "clk.h"
+
+struct rockchip_mmc_clock {
+	struct clk_hw	hw;
+	void __iomem	*reg;
+	int		id;
+	int		shift;
+};
+
+#define to_mmc_clock(_hw) container_of(_hw, struct rockchip_mmc_clock, hw)
+
+#define RK3288_MMC_CLKGEN_DIV 2
+
+static unsigned long rockchip_mmc_recalc(struct clk_hw *hw,
+					 unsigned long parent_rate)
+{
+	return parent_rate / RK3288_MMC_CLKGEN_DIV;
+}
+
+#define ROCKCHIP_MMC_DELAY_SEL BIT(10)
+#define ROCKCHIP_MMC_DEGREE_MASK 0x3
+#define ROCKCHIP_MMC_DELAYNUM_OFFSET 2
+#define ROCKCHIP_MMC_DELAYNUM_MASK (0xff << ROCKCHIP_MMC_DELAYNUM_OFFSET)
+
+#define PSECS_PER_SEC 1000000000000LL
+
+/*
+ * Each fine delay is between 40ps-80ps. Assume each fine delay is 60ps to
+ * simplify calculations. So 45degs could be anywhere between 33deg and 66deg.
+ */
+#define ROCKCHIP_MMC_DELAY_ELEMENT_PSEC 60
+
+static int rockchip_mmc_get_phase(struct clk_hw *hw)
+{
+	struct rockchip_mmc_clock *mmc_clock = to_mmc_clock(hw);
+	unsigned long rate = clk_get_rate(hw->clk);
+	u32 raw_value;
+	u16 degrees;
+	u32 delay_num = 0;
+
+	raw_value = readl(mmc_clock->reg) >> (mmc_clock->shift);
+
+	degrees = (raw_value & ROCKCHIP_MMC_DEGREE_MASK) * 90;
+
+	if (raw_value & ROCKCHIP_MMC_DELAY_SEL) {
+		/* degrees/delaynum * 10000 */
+		unsigned long factor = (ROCKCHIP_MMC_DELAY_ELEMENT_PSEC / 10) *
+					36 * (rate / 1000000);
+
+		delay_num = (raw_value & ROCKCHIP_MMC_DELAYNUM_MASK);
+		delay_num >>= ROCKCHIP_MMC_DELAYNUM_OFFSET;
+		degrees += delay_num * factor / 10000;
+	}
+
+	return degrees % 360;
+}
+
+static int rockchip_mmc_set_phase(struct clk_hw *hw, int degrees)
+{
+	struct rockchip_mmc_clock *mmc_clock = to_mmc_clock(hw);
+	unsigned long rate = clk_get_rate(hw->clk);
+	u8 nineties, remainder;
+	u8 delay_num;
+	u32 raw_value;
+	u64 delay;
+
+	/* allow 22 to be 22.5 */
+	degrees++;
+	/* floor to 22.5 increment */
+	degrees -= ((degrees) * 10 % 225) / 10;
+
+	nineties = degrees / 90;
+	/* 22.5 multiples */
+	remainder = (degrees % 90) / 22;
+
+	delay = PSECS_PER_SEC;
+	do_div(delay, rate);
+	/* / 360 / 22.5 */
+	do_div(delay, 16);
+	do_div(delay, ROCKCHIP_MMC_DELAY_ELEMENT_PSEC);
+
+	delay *= remainder;
+	delay_num = (u8) min(delay, 255ULL);
+
+	raw_value = delay_num ? ROCKCHIP_MMC_DELAY_SEL : 0;
+	raw_value |= delay_num << ROCKCHIP_MMC_DELAYNUM_OFFSET;
+	raw_value |= nineties;
+	writel(HIWORD_UPDATE(raw_value, 0x07ff, mmc_clock->shift), mmc_clock->reg);
+
+	pr_debug("%s->set_phase(%d) delay_nums=%u reg[0x%p]=0x%03x actual_degrees=%d\n",
+		__clk_get_name(hw->clk), degrees, delay_num,
+		mmc_clock->reg, raw_value>>(mmc_clock->shift),
+		rockchip_mmc_get_phase(hw)
+	);
+
+	return 0;
+}
+
+static const struct clk_ops rockchip_mmc_clk_ops = {
+	.recalc_rate	= rockchip_mmc_recalc,
+	.get_phase	= rockchip_mmc_get_phase,
+	.set_phase	= rockchip_mmc_set_phase,
+};
+
+struct clk *rockchip_clk_register_mmc(const char *name,
+				const char **parent_names, u8 num_parents,
+				void __iomem *reg, int shift)
+{
+	struct clk_init_data init;
+	struct rockchip_mmc_clock *mmc_clock;
+	struct clk *clk;
+
+	mmc_clock = kmalloc(sizeof(*mmc_clock), GFP_KERNEL);
+	if (!mmc_clock)
+		return NULL;
+
+	init.num_parents = num_parents;
+	init.parent_names = parent_names;
+	init.ops = &rockchip_mmc_clk_ops;
+
+	mmc_clock->hw.init = &init;
+	mmc_clock->reg = reg;
+	mmc_clock->shift = shift;
+
+	if (name)
+		init.name = name;
+
+	clk = clk_register(NULL, &mmc_clock->hw);
+	if (IS_ERR(clk))
+		goto err_free;
+
+	return clk;
+
+err_free:
+	kfree(mmc_clock);
+	return NULL;
+}
diff --git a/drivers/clk/rockchip/clk-pll.c b/drivers/clk/rockchip/clk-pll.c
index a3e886a38480..f8d3baf275b2 100644
--- a/drivers/clk/rockchip/clk-pll.c
+++ b/drivers/clk/rockchip/clk-pll.c
@@ -39,6 +39,7 @@ struct rockchip_clk_pll {
 	int			lock_offset;
 	unsigned int		lock_shift;
 	enum rockchip_pll_type	type;
+	u8			flags;
 	const struct rockchip_pll_rate_table *rate_table;
 	unsigned int		rate_count;
 	spinlock_t		*lock;
@@ -257,6 +258,55 @@ static int rockchip_rk3066_pll_is_enabled(struct clk_hw *hw)
 	return !(pllcon & RK3066_PLLCON3_PWRDOWN);
 }
 
+static void rockchip_rk3066_pll_init(struct clk_hw *hw)
+{
+	struct rockchip_clk_pll *pll = to_rockchip_clk_pll(hw);
+	const struct rockchip_pll_rate_table *rate;
+	unsigned int nf, nr, no, bwadj;
+	unsigned long drate;
+	u32 pllcon;
+
+	if (!(pll->flags & ROCKCHIP_PLL_SYNC_RATE))
+		return;
+
+	drate = __clk_get_rate(hw->clk);
+	rate = rockchip_get_pll_settings(pll, drate);
+
+	/* when no rate setting for the current rate, rely on clk_set_rate */
+	if (!rate)
+		return;
+
+	pllcon = readl_relaxed(pll->reg_base + RK3066_PLLCON(0));
+	nr = ((pllcon >> RK3066_PLLCON0_NR_SHIFT) & RK3066_PLLCON0_NR_MASK) + 1;
+	no = ((pllcon >> RK3066_PLLCON0_OD_SHIFT) & RK3066_PLLCON0_OD_MASK) + 1;
+
+	pllcon = readl_relaxed(pll->reg_base + RK3066_PLLCON(1));
+	nf = ((pllcon >> RK3066_PLLCON1_NF_SHIFT) & RK3066_PLLCON1_NF_MASK) + 1;
+
+	pllcon = readl_relaxed(pll->reg_base + RK3066_PLLCON(2));
+	bwadj = (pllcon >> RK3066_PLLCON2_BWADJ_SHIFT) & RK3066_PLLCON2_BWADJ_MASK;
+
+	pr_debug("%s: pll %s@%lu: nr (%d:%d); no (%d:%d); nf(%d:%d), bwadj(%d:%d)\n",
+		 __func__, __clk_get_name(hw->clk), drate, rate->nr, nr,
+		rate->no, no, rate->nf, nf, rate->bwadj, bwadj);
+	if (rate->nr != nr || rate->no != no || rate->nf != nf
+					     || rate->bwadj != bwadj) {
+		struct clk *parent = __clk_get_parent(hw->clk);
+		unsigned long prate;
+
+		if (!parent) {
+			pr_warn("%s: parent of %s not available\n",
+				__func__, __clk_get_name(hw->clk));
+			return;
+		}
+
+		pr_debug("%s: pll %s: rate params do not match rate table, adjusting\n",
+			 __func__, __clk_get_name(hw->clk));
+		prate = __clk_get_rate(parent);
+		rockchip_rk3066_pll_set_rate(hw, drate, prate);
+	}
+}
+
 static const struct clk_ops rockchip_rk3066_pll_clk_norate_ops = {
 	.recalc_rate = rockchip_rk3066_pll_recalc_rate,
 	.enable = rockchip_rk3066_pll_enable,
@@ -271,6 +321,7 @@ static const struct clk_ops rockchip_rk3066_pll_clk_ops = {
 	.enable = rockchip_rk3066_pll_enable,
 	.disable = rockchip_rk3066_pll_disable,
 	.is_enabled = rockchip_rk3066_pll_is_enabled,
+	.init = rockchip_rk3066_pll_init,
 };
 
 /*
@@ -282,7 +333,7 @@ struct clk *rockchip_clk_register_pll(enum rockchip_pll_type pll_type,
 		void __iomem *base, int con_offset, int grf_lock_offset,
 		int lock_shift, int mode_offset, int mode_shift,
 		struct rockchip_pll_rate_table *rate_table,
-		spinlock_t *lock)
+		u8 clk_pll_flags, spinlock_t *lock)
 {
 	const char *pll_parents[3];
 	struct clk_init_data init;
@@ -345,8 +396,22 @@ struct clk *rockchip_clk_register_pll(enum rockchip_pll_type pll_type,
 	pll->reg_base = base + con_offset;
 	pll->lock_offset = grf_lock_offset;
 	pll->lock_shift = lock_shift;
+	pll->flags = clk_pll_flags;
 	pll->lock = lock;
 
+	/* create the mux on top of the real pll */
+	pll->pll_mux_ops = &clk_mux_ops;
+	pll_mux = &pll->pll_mux;
+	pll_mux->reg = base + mode_offset;
+	pll_mux->shift = mode_shift;
+	pll_mux->mask = PLL_MODE_MASK;
+	pll_mux->flags = 0;
+	pll_mux->lock = lock;
+	pll_mux->hw.init = &init;
+
+	if (pll_type == pll_rk3066)
+		pll_mux->flags |= CLK_MUX_HIWORD_MASK;
+
 	pll_clk = clk_register(NULL, &pll->hw);
 	if (IS_ERR(pll_clk)) {
 		pr_err("%s: failed to register pll clock %s : %ld\n",
@@ -355,10 +420,6 @@ struct clk *rockchip_clk_register_pll(enum rockchip_pll_type pll_type,
 		goto err_pll;
 	}
 
-	/* create the mux on top of the real pll */
-	pll->pll_mux_ops = &clk_mux_ops;
-	pll_mux = &pll->pll_mux;
-
 	/* the actual muxing is xin24m, pll-output, xin32k */
 	pll_parents[0] = parent_names[0];
 	pll_parents[1] = pll_name;
@@ -370,16 +431,6 @@ struct clk *rockchip_clk_register_pll(enum rockchip_pll_type pll_type,
 	init.parent_names = pll_parents;
 	init.num_parents = ARRAY_SIZE(pll_parents);
 
-	pll_mux->reg = base + mode_offset;
-	pll_mux->shift = mode_shift;
-	pll_mux->mask = PLL_MODE_MASK;
-	pll_mux->flags = 0;
-	pll_mux->lock = lock;
-	pll_mux->hw.init = &init;
-
-	if (pll_type == pll_rk3066)
-		pll_mux->flags |= CLK_MUX_HIWORD_MASK;
-
 	mux_clk = clk_register(NULL, &pll_mux->hw);
 	if (IS_ERR(mux_clk))
 		goto err_mux;
diff --git a/drivers/clk/rockchip/clk-rk3188.c b/drivers/clk/rockchip/clk-rk3188.c
index beed49c79126..c54078960847 100644
--- a/drivers/clk/rockchip/clk-rk3188.c
+++ b/drivers/clk/rockchip/clk-rk3188.c
@@ -212,13 +212,13 @@ PNAME(mux_sclk_macref_p)	= { "mac_src", "ext_rmii" };
 
 static struct rockchip_pll_clock rk3188_pll_clks[] __initdata = {
 	[apll] = PLL(pll_rk3066, PLL_APLL, "apll", mux_pll_p, 0, RK2928_PLL_CON(0),
-		     RK2928_MODE_CON, 0, 6, rk3188_pll_rates),
+		     RK2928_MODE_CON, 0, 6, 0, rk3188_pll_rates),
 	[dpll] = PLL(pll_rk3066, PLL_DPLL, "dpll", mux_pll_p, 0, RK2928_PLL_CON(4),
-		     RK2928_MODE_CON, 4, 5, NULL),
+		     RK2928_MODE_CON, 4, 5, 0, NULL),
 	[cpll] = PLL(pll_rk3066, PLL_CPLL, "cpll", mux_pll_p, 0, RK2928_PLL_CON(8),
-		     RK2928_MODE_CON, 8, 7, rk3188_pll_rates),
+		     RK2928_MODE_CON, 8, 7, ROCKCHIP_PLL_SYNC_RATE, rk3188_pll_rates),
 	[gpll] = PLL(pll_rk3066, PLL_GPLL, "gpll", mux_pll_p, 0, RK2928_PLL_CON(12),
-		     RK2928_MODE_CON, 12, 8, rk3188_pll_rates),
+		     RK2928_MODE_CON, 12, 8, ROCKCHIP_PLL_SYNC_RATE, rk3188_pll_rates),
 };
 
 #define MFLAGS CLK_MUX_HIWORD_MASK
@@ -257,9 +257,9 @@ static struct rockchip_clk_branch common_clk_branches[] __initdata = {
 	GATE(0, "hclk_vdpu", "aclk_vdpu", 0,
 			RK2928_CLKGATE_CON(3), 12, GFLAGS),
 
-	GATE(0, "gpll_ddr", "gpll", 0,
+	GATE(0, "gpll_ddr", "gpll", CLK_IGNORE_UNUSED,
 			RK2928_CLKGATE_CON(1), 7, GFLAGS),
-	COMPOSITE(0, "ddrphy", mux_ddrphy_p, 0,
+	COMPOSITE(0, "ddrphy", mux_ddrphy_p, CLK_IGNORE_UNUSED,
 			RK2928_CLKSEL_CON(26), 8, 1, MFLAGS, 0, 2, DFLAGS | CLK_DIVIDER_POWER_OF_TWO,
 			RK2928_CLKGATE_CON(0), 2, GFLAGS),
 
@@ -270,10 +270,10 @@ static struct rockchip_clk_branch common_clk_branches[] __initdata = {
 			RK2928_CLKGATE_CON(0), 6, GFLAGS),
 	GATE(0, "pclk_cpu", "pclk_cpu_pre", 0,
 			RK2928_CLKGATE_CON(0), 5, GFLAGS),
-	GATE(0, "hclk_cpu", "hclk_cpu_pre", 0,
+	GATE(0, "hclk_cpu", "hclk_cpu_pre", CLK_IGNORE_UNUSED,
 			RK2928_CLKGATE_CON(0), 4, GFLAGS),
 
-	COMPOSITE(0, "aclk_lcdc0_pre", mux_pll_src_cpll_gpll_p, 0,
+	COMPOSITE(0, "aclk_lcdc0_pre", mux_pll_src_cpll_gpll_p, CLK_IGNORE_UNUSED,
 			RK2928_CLKSEL_CON(31), 7, 1, MFLAGS, 0, 5, DFLAGS,
 			RK2928_CLKGATE_CON(3), 0, GFLAGS),
 	COMPOSITE(0, "aclk_lcdc1_pre", mux_pll_src_cpll_gpll_p, 0,
@@ -304,9 +304,9 @@ static struct rockchip_clk_branch common_clk_branches[] __initdata = {
 	 * the 480m are generated inside the usb block from these clocks,
 	 * but they are also a source for the hsicphy clock.
 	 */
-	GATE(SCLK_OTGPHY0, "sclk_otgphy0", "usb480m", 0,
+	GATE(SCLK_OTGPHY0, "sclk_otgphy0", "usb480m", CLK_IGNORE_UNUSED,
 			RK2928_CLKGATE_CON(1), 5, GFLAGS),
-	GATE(SCLK_OTGPHY1, "sclk_otgphy1", "usb480m", 0,
+	GATE(SCLK_OTGPHY1, "sclk_otgphy1", "usb480m", CLK_IGNORE_UNUSED,
 			RK2928_CLKGATE_CON(1), 6, GFLAGS),
 
 	COMPOSITE(0, "mac_src", mux_mac_p, 0,
@@ -320,9 +320,9 @@ static struct rockchip_clk_branch common_clk_branches[] __initdata = {
 	COMPOSITE(0, "hsadc_src", mux_pll_src_gpll_cpll_p, 0,
 			RK2928_CLKSEL_CON(22), 0, 1, MFLAGS, 8, 8, DFLAGS,
 			RK2928_CLKGATE_CON(2), 6, GFLAGS),
-	COMPOSITE_FRAC(0, "hsadc_frac", "hsadc_src",
+	COMPOSITE_FRAC(0, "hsadc_frac", "hsadc_src", 0,
 			RK2928_CLKSEL_CON(23), 0,
-			RK2928_CLKGATE_CON(2), 7, 0, GFLAGS),
+			RK2928_CLKGATE_CON(2), 7, GFLAGS),
 	MUX(SCLK_HSADC, "sclk_hsadc", mux_sclk_hsadc_p, 0,
 			RK2928_CLKSEL_CON(22), 4, 2, MFLAGS),
 
@@ -330,6 +330,15 @@ static struct rockchip_clk_branch common_clk_branches[] __initdata = {
 			RK2928_CLKSEL_CON(24), 8, 8, DFLAGS,
 			RK2928_CLKGATE_CON(2), 8, GFLAGS),
 
+	COMPOSITE_NOMUX(0, "spdif_pre", "i2s_src", 0,
+			RK2928_CLKSEL_CON(5), 0, 7, DFLAGS,
+			RK2928_CLKGATE_CON(0), 13, GFLAGS),
+	COMPOSITE_FRAC(0, "spdif_frac", "spdif_pll", 0,
+			RK2928_CLKSEL_CON(9), 0,
+			RK2928_CLKGATE_CON(0), 14, GFLAGS),
+	MUX(SCLK_SPDIF, "sclk_spdif", mux_sclk_spdif_p, 0,
+			RK2928_CLKSEL_CON(5), 8, 2, MFLAGS),
+
 	/*
 	 * Clock-Architecture Diagram 4
 	 */
@@ -399,8 +408,8 @@ static struct rockchip_clk_branch common_clk_branches[] __initdata = {
 
 	/* aclk_cpu gates */
 	GATE(ACLK_DMA1, "aclk_dma1", "aclk_cpu", 0, RK2928_CLKGATE_CON(5), 0, GFLAGS),
-	GATE(0, "aclk_intmem", "aclk_cpu", 0, RK2928_CLKGATE_CON(4), 12, GFLAGS),
-	GATE(0, "aclk_strc_sys", "aclk_cpu", 0, RK2928_CLKGATE_CON(4), 10, GFLAGS),
+	GATE(0, "aclk_intmem", "aclk_cpu", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(4), 12, GFLAGS),
+	GATE(0, "aclk_strc_sys", "aclk_cpu", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(4), 10, GFLAGS),
 
 	/* hclk_cpu gates */
 	GATE(HCLK_ROM, "hclk_rom", "hclk_cpu", 0, RK2928_CLKGATE_CON(5), 6, GFLAGS),
@@ -410,14 +419,14 @@ static struct rockchip_clk_branch common_clk_branches[] __initdata = {
 	/* hclk_ahb2apb is part of a clk branch */
 	GATE(0, "hclk_vio_bus", "hclk_cpu", 0, RK2928_CLKGATE_CON(6), 12, GFLAGS),
 	GATE(HCLK_LCDC0, "hclk_lcdc0", "hclk_cpu", 0, RK2928_CLKGATE_CON(6), 1, GFLAGS),
-	GATE(HCLK_LCDC1, "hclk_lcdc1", "aclk_cpu", 0, RK2928_CLKGATE_CON(6), 2, GFLAGS),
+	GATE(HCLK_LCDC1, "hclk_lcdc1", "hclk_cpu", 0, RK2928_CLKGATE_CON(6), 2, GFLAGS),
 	GATE(HCLK_CIF0, "hclk_cif0", "hclk_cpu", 0, RK2928_CLKGATE_CON(6), 4, GFLAGS),
 	GATE(HCLK_IPP, "hclk_ipp", "hclk_cpu", 0, RK2928_CLKGATE_CON(6), 9, GFLAGS),
 	GATE(HCLK_RGA, "hclk_rga", "hclk_cpu", 0, RK2928_CLKGATE_CON(6), 10, GFLAGS),
 
 	/* hclk_peri gates */
-	GATE(0, "hclk_peri_axi_matrix", "hclk_peri", 0, RK2928_CLKGATE_CON(4), 0, GFLAGS),
-	GATE(0, "hclk_peri_ahb_arbi", "hclk_peri", 0, RK2928_CLKGATE_CON(4), 6, GFLAGS),
+	GATE(0, "hclk_peri_axi_matrix", "hclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(4), 0, GFLAGS),
+	GATE(0, "hclk_peri_ahb_arbi", "hclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(4), 6, GFLAGS),
 	GATE(0, "hclk_emem_peri", "hclk_peri", 0, RK2928_CLKGATE_CON(4), 7, GFLAGS),
 	GATE(HCLK_EMAC, "hclk_emac", "hclk_peri", 0, RK2928_CLKGATE_CON(7), 0, GFLAGS),
 	GATE(HCLK_NANDC0, "hclk_nandc0", "hclk_peri", 0, RK2928_CLKGATE_CON(5), 9, GFLAGS),
@@ -457,18 +466,18 @@ static struct rockchip_clk_branch common_clk_branches[] __initdata = {
 	GATE(0, "pclk_ddrupctl", "pclk_cpu", 0, RK2928_CLKGATE_CON(5), 7, GFLAGS),
 	GATE(0, "pclk_ddrpubl", "pclk_cpu", 0, RK2928_CLKGATE_CON(9), 6, GFLAGS),
 	GATE(0, "pclk_dbg", "pclk_cpu", 0, RK2928_CLKGATE_CON(9), 1, GFLAGS),
-	GATE(PCLK_GRF, "pclk_grf", "pclk_cpu", 0, RK2928_CLKGATE_CON(5), 4, GFLAGS),
-	GATE(PCLK_PMU, "pclk_pmu", "pclk_cpu", 0, RK2928_CLKGATE_CON(5), 5, GFLAGS),
+	GATE(PCLK_GRF, "pclk_grf", "pclk_cpu", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(5), 4, GFLAGS),
+	GATE(PCLK_PMU, "pclk_pmu", "pclk_cpu", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(5), 5, GFLAGS),
 
 	/* aclk_peri */
 	GATE(ACLK_DMA2, "aclk_dma2", "aclk_peri", 0, RK2928_CLKGATE_CON(5), 1, GFLAGS),
 	GATE(ACLK_SMC, "aclk_smc", "aclk_peri", 0, RK2928_CLKGATE_CON(5), 8, GFLAGS),
-	GATE(0, "aclk_peri_niu", "aclk_peri", 0, RK2928_CLKGATE_CON(4), 4, GFLAGS),
-	GATE(0, "aclk_cpu_peri", "aclk_peri", 0, RK2928_CLKGATE_CON(4), 2, GFLAGS),
-	GATE(0, "aclk_peri_axi_matrix", "aclk_peri", 0, RK2928_CLKGATE_CON(4), 3, GFLAGS),
+	GATE(0, "aclk_peri_niu", "aclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(4), 4, GFLAGS),
+	GATE(0, "aclk_cpu_peri", "aclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(4), 2, GFLAGS),
+	GATE(0, "aclk_peri_axi_matrix", "aclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(4), 3, GFLAGS),
 
 	/* pclk_peri gates */
-	GATE(0, "pclk_peri_axi_matrix", "pclk_peri", 0, RK2928_CLKGATE_CON(4), 1, GFLAGS),
+	GATE(0, "pclk_peri_axi_matrix", "pclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(4), 1, GFLAGS),
 	GATE(PCLK_PWM23, "pclk_pwm23", "pclk_peri", 0, RK2928_CLKGATE_CON(7), 11, GFLAGS),
 	GATE(PCLK_WDT, "pclk_wdt", "pclk_peri", 0, RK2928_CLKGATE_CON(7), 15, GFLAGS),
 	GATE(PCLK_SPI0, "pclk_spi0", "pclk_peri", 0, RK2928_CLKGATE_CON(7), 12, GFLAGS),
@@ -511,7 +520,7 @@ static struct rockchip_clk_branch rk3066a_clk_branches[] __initdata = {
 							    | CLK_DIVIDER_READ_ONLY,
 			RK2928_CLKGATE_CON(4), 9, GFLAGS),
 
-	GATE(CORE_L2C, "core_l2c", "aclk_cpu", 0,
+	GATE(CORE_L2C, "core_l2c", "aclk_cpu", CLK_IGNORE_UNUSED,
 			RK2928_CLKGATE_CON(9), 4, GFLAGS),
 
 	COMPOSITE(0, "aclk_peri_pre", mux_pll_src_gpll_cpll_p, 0,
@@ -577,14 +586,6 @@ static struct rockchip_clk_branch rk3066a_clk_branches[] __initdata = {
 			RK2928_CLKGATE_CON(0), 12, GFLAGS),
 	MUX(SCLK_I2S2, "sclk_i2s2", mux_sclk_i2s2_p, 0,
 			RK2928_CLKSEL_CON(4), 8, 2, MFLAGS),
-	COMPOSITE_NOMUX(0, "spdif_pre", "i2s_src", 0,
-			RK2928_CLKSEL_CON(5), 0, 7, DFLAGS,
-			RK2928_CLKGATE_CON(0), 13, GFLAGS),
-	COMPOSITE_FRAC(0, "spdif_frac", "spdif_pll", 0,
-			RK2928_CLKSEL_CON(9), 0,
-			RK2928_CLKGATE_CON(0), 14, GFLAGS),
-	MUX(SCLK_SPDIF, "sclk_spdif", mux_sclk_spdif_p, 0,
-			RK2928_CLKSEL_CON(5), 8, 2, MFLAGS),
 
 	GATE(HCLK_I2S1, "hclk_i2s1", "hclk_cpu", 0, RK2928_CLKGATE_CON(7), 3, GFLAGS),
 	GATE(HCLK_I2S2, "hclk_i2s2", "hclk_cpu", 0, RK2928_CLKGATE_CON(7), 4, GFLAGS),
@@ -618,7 +619,7 @@ PNAME(mux_hsicphy_p)		= { "sclk_otgphy0", "sclk_otgphy1",
 				    "gpll", "cpll" };
 
 static struct rockchip_clk_branch rk3188_clk_branches[] __initdata = {
-	COMPOSITE_NOMUX_DIVTBL(0, "aclk_core", "armclk", 0,
+	COMPOSITE_NOMUX_DIVTBL(0, "aclk_core", "armclk", CLK_IGNORE_UNUSED,
 			RK2928_CLKSEL_CON(1), 3, 3, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			div_rk3188_aclk_core_t, RK2928_CLKGATE_CON(0), 7, GFLAGS),
 
@@ -633,7 +634,7 @@ static struct rockchip_clk_branch rk3188_clk_branches[] __initdata = {
 			RK2928_CLKSEL_CON(1), 14, 2, DFLAGS | CLK_DIVIDER_POWER_OF_TWO,
 			RK2928_CLKGATE_CON(4), 9, GFLAGS),
 
-	GATE(CORE_L2C, "core_l2c", "armclk", 0,
+	GATE(CORE_L2C, "core_l2c", "armclk", CLK_IGNORE_UNUSED,
 			RK2928_CLKGATE_CON(9), 4, GFLAGS),
 
 	COMPOSITE(0, "aclk_peri_pre", mux_pll_src_cpll_gpll_p, 0,
@@ -663,7 +664,7 @@ static struct rockchip_clk_branch rk3188_clk_branches[] __initdata = {
 			RK2928_CLKSEL_CON(30), 0, 2, DFLAGS,
 			RK2928_CLKGATE_CON(3), 6, GFLAGS),
 	DIV(0, "sclk_hsicphy_12m", "sclk_hsicphy_480m", 0,
-			RK2928_CLKGATE_CON(11), 8, 6, DFLAGS),
+			RK2928_CLKSEL_CON(11), 8, 6, DFLAGS),
 
 	MUX(0, "i2s_src", mux_pll_src_gpll_cpll_p, 0,
 			RK2928_CLKSEL_CON(2), 15, 1, MFLAGS),
@@ -675,14 +676,6 @@ static struct rockchip_clk_branch rk3188_clk_branches[] __initdata = {
 			RK2928_CLKGATE_CON(0), 10, GFLAGS),
 	MUX(SCLK_I2S0, "sclk_i2s0", mux_sclk_i2s0_p, 0,
 			RK2928_CLKSEL_CON(3), 8, 2, MFLAGS),
-	COMPOSITE_NOMUX(0, "spdif_pre", "i2s_src", 0,
-			RK2928_CLKSEL_CON(5), 0, 7, DFLAGS,
-			RK2928_CLKGATE_CON(13), 13, GFLAGS),
-	COMPOSITE_FRAC(0, "spdif_frac", "spdif_pll", 0,
-			RK2928_CLKSEL_CON(9), 0,
-			RK2928_CLKGATE_CON(0), 14, GFLAGS),
-	MUX(SCLK_SPDIF, "sclk_spdif", mux_sclk_spdif_p, 0,
-			RK2928_CLKSEL_CON(5), 8, 2, MFLAGS),
 
 	GATE(0, "hclk_imem0", "hclk_cpu", 0, RK2928_CLKGATE_CON(4), 14, GFLAGS),
 	GATE(0, "hclk_imem1", "hclk_cpu", 0, RK2928_CLKGATE_CON(4), 15, GFLAGS),
diff --git a/drivers/clk/rockchip/clk-rk3288.c b/drivers/clk/rockchip/clk-rk3288.c
index 23278291da44..40d267f5dea3 100644
--- a/drivers/clk/rockchip/clk-rk3288.c
+++ b/drivers/clk/rockchip/clk-rk3288.c
@@ -16,6 +16,7 @@
 #include <linux/clk-provider.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
+#include <linux/syscore_ops.h>
 #include <dt-bindings/clock/rk3288-cru.h>
 #include "clk.h"
 
@@ -83,11 +84,13 @@ struct rockchip_pll_rate_table rk3288_pll_rates[] = {
 	RK3066_PLL_RATE( 742500000, 8, 495, 2),
 	RK3066_PLL_RATE( 696000000, 1, 58, 2),
 	RK3066_PLL_RATE( 600000000, 1, 50, 2),
-	RK3066_PLL_RATE( 594000000, 2, 198, 4),
+	RK3066_PLL_RATE_BWADJ(594000000, 1, 198, 8, 1),
 	RK3066_PLL_RATE( 552000000, 1, 46, 2),
 	RK3066_PLL_RATE( 504000000, 1, 84, 4),
+	RK3066_PLL_RATE( 500000000, 3, 125, 2),
 	RK3066_PLL_RATE( 456000000, 1, 76, 4),
 	RK3066_PLL_RATE( 408000000, 1, 68, 4),
+	RK3066_PLL_RATE( 400000000, 3, 100, 2),
 	RK3066_PLL_RATE( 384000000, 2, 128, 4),
 	RK3066_PLL_RATE( 360000000, 1, 60, 4),
 	RK3066_PLL_RATE( 312000000, 1, 52, 4),
@@ -173,41 +176,41 @@ PNAME(mux_aclk_cpu_src_p)	= { "cpll_aclk_cpu", "gpll_aclk_cpu" };
 PNAME(mux_pll_src_cpll_gpll_p)		= { "cpll", "gpll" };
 PNAME(mux_pll_src_npll_cpll_gpll_p)	= { "npll", "cpll", "gpll" };
 PNAME(mux_pll_src_cpll_gpll_npll_p)	= { "cpll", "gpll", "npll" };
-PNAME(mux_pll_src_cpll_gpll_usb480m_p)	= { "cpll", "gpll", "usb480m" };
+PNAME(mux_pll_src_cpll_gpll_usb480m_p)	= { "cpll", "gpll", "usbphy480m_src" };
+PNAME(mux_pll_src_cpll_gll_usb_npll_p)	= { "cpll", "gpll", "usbphy480m_src", "npll" };
 
 PNAME(mux_mmc_src_p)	= { "cpll", "gpll", "xin24m", "xin24m" };
 PNAME(mux_i2s_pre_p)	= { "i2s_src", "i2s_frac", "ext_i2s", "xin12m" };
 PNAME(mux_i2s_clkout_p)	= { "i2s_pre", "xin12m" };
 PNAME(mux_spdif_p)	= { "spdif_pre", "spdif_frac", "xin12m" };
 PNAME(mux_spdif_8ch_p)	= { "spdif_8ch_pre", "spdif_8ch_frac", "xin12m" };
-PNAME(mux_uart0_pll_p)	= { "cpll", "gpll", "usbphy_480m_src", "npll" };
 PNAME(mux_uart0_p)	= { "uart0_src", "uart0_frac", "xin24m" };
 PNAME(mux_uart1_p)	= { "uart1_src", "uart1_frac", "xin24m" };
 PNAME(mux_uart2_p)	= { "uart2_src", "uart2_frac", "xin24m" };
 PNAME(mux_uart3_p)	= { "uart3_src", "uart3_frac", "xin24m" };
 PNAME(mux_uart4_p)	= { "uart4_src", "uart4_frac", "xin24m" };
 PNAME(mux_cif_out_p)	= { "cif_src", "xin24m" };
-PNAME(mux_macref_p)	= { "mac_src", "ext_gmac" };
+PNAME(mux_mac_p)	= { "mac_pll_src", "ext_gmac" };
 PNAME(mux_hsadcout_p)	= { "hsadc_src", "ext_hsadc" };
 PNAME(mux_edp_24m_p)	= { "ext_edp_24m", "xin24m" };
 PNAME(mux_tspout_p)	= { "cpll", "gpll", "npll", "xin27m" };
 
-PNAME(mux_usbphy480m_p)		= { "sclk_otgphy0", "sclk_otgphy1",
-				    "sclk_otgphy2" };
+PNAME(mux_usbphy480m_p)		= { "sclk_otgphy1", "sclk_otgphy2",
+				    "sclk_otgphy0" };
 PNAME(mux_hsicphy480m_p)	= { "cpll", "gpll", "usbphy480m_src" };
 PNAME(mux_hsicphy12m_p)		= { "hsicphy12m_xin12m", "hsicphy12m_usbphy" };
 
 static struct rockchip_pll_clock rk3288_pll_clks[] __initdata = {
 	[apll] = PLL(pll_rk3066, PLL_APLL, "apll", mux_pll_p, 0, RK3288_PLL_CON(0),
-		     RK3288_MODE_CON, 0, 6, rk3288_pll_rates),
+		     RK3288_MODE_CON, 0, 6, 0, rk3288_pll_rates),
 	[dpll] = PLL(pll_rk3066, PLL_DPLL, "dpll", mux_pll_p, 0, RK3288_PLL_CON(4),
-		     RK3288_MODE_CON, 4, 5, NULL),
+		     RK3288_MODE_CON, 4, 5, 0, NULL),
 	[cpll] = PLL(pll_rk3066, PLL_CPLL, "cpll", mux_pll_p, 0, RK3288_PLL_CON(8),
-		     RK3288_MODE_CON, 8, 7, rk3288_pll_rates),
+		     RK3288_MODE_CON, 8, 7, ROCKCHIP_PLL_SYNC_RATE, rk3288_pll_rates),
 	[gpll] = PLL(pll_rk3066, PLL_GPLL, "gpll", mux_pll_p, 0, RK3288_PLL_CON(12),
-		     RK3288_MODE_CON, 12, 8, rk3288_pll_rates),
+		     RK3288_MODE_CON, 12, 8, ROCKCHIP_PLL_SYNC_RATE, rk3288_pll_rates),
 	[npll] = PLL(pll_rk3066, PLL_NPLL, "npll",  mux_pll_p, 0, RK3288_PLL_CON(16),
-		     RK3288_MODE_CON, 14, 9, rk3288_pll_rates),
+		     RK3288_MODE_CON, 14, 9, ROCKCHIP_PLL_SYNC_RATE, rk3288_pll_rates),
 };
 
 static struct clk_div_table div_hclk_cpu_t[] = {
@@ -226,67 +229,67 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	 * Clock-Architecture Diagram 1
 	 */
 
-	GATE(0, "apll_core", "apll", 0,
+	GATE(0, "apll_core", "apll", CLK_IGNORE_UNUSED,
 			RK3288_CLKGATE_CON(0), 1, GFLAGS),
-	GATE(0, "gpll_core", "gpll", 0,
+	GATE(0, "gpll_core", "gpll", CLK_IGNORE_UNUSED,
 			RK3288_CLKGATE_CON(0), 2, GFLAGS),
 
-	COMPOSITE_NOMUX(0, "armcore0", "armclk", 0,
+	COMPOSITE_NOMUX(0, "armcore0", "armclk", CLK_IGNORE_UNUSED,
 			RK3288_CLKSEL_CON(36), 0, 3, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3288_CLKGATE_CON(12), 0, GFLAGS),
-	COMPOSITE_NOMUX(0, "armcore1", "armclk", 0,
+	COMPOSITE_NOMUX(0, "armcore1", "armclk", CLK_IGNORE_UNUSED,
 			RK3288_CLKSEL_CON(36), 4, 3, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3288_CLKGATE_CON(12), 1, GFLAGS),
-	COMPOSITE_NOMUX(0, "armcore2", "armclk", 0,
+	COMPOSITE_NOMUX(0, "armcore2", "armclk", CLK_IGNORE_UNUSED,
 			RK3288_CLKSEL_CON(36), 8, 3, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3288_CLKGATE_CON(12), 2, GFLAGS),
-	COMPOSITE_NOMUX(0, "armcore3", "armclk", 0,
+	COMPOSITE_NOMUX(0, "armcore3", "armclk", CLK_IGNORE_UNUSED,
 			RK3288_CLKSEL_CON(36), 12, 3, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3288_CLKGATE_CON(12), 3, GFLAGS),
-	COMPOSITE_NOMUX(0, "l2ram", "armclk", 0,
+	COMPOSITE_NOMUX(0, "l2ram", "armclk", CLK_IGNORE_UNUSED,
 			RK3288_CLKSEL_CON(37), 0, 3, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3288_CLKGATE_CON(12), 4, GFLAGS),
-	COMPOSITE_NOMUX(0, "aclk_core_m0", "armclk", 0,
+	COMPOSITE_NOMUX(0, "aclk_core_m0", "armclk", CLK_IGNORE_UNUSED,
 			RK3288_CLKSEL_CON(0), 0, 4, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3288_CLKGATE_CON(12), 5, GFLAGS),
-	COMPOSITE_NOMUX(0, "aclk_core_mp", "armclk", 0,
+	COMPOSITE_NOMUX(0, "aclk_core_mp", "armclk", CLK_IGNORE_UNUSED,
 			RK3288_CLKSEL_CON(0), 4, 4, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3288_CLKGATE_CON(12), 6, GFLAGS),
 	COMPOSITE_NOMUX(0, "atclk", "armclk", 0,
 			RK3288_CLKSEL_CON(37), 4, 5, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3288_CLKGATE_CON(12), 7, GFLAGS),
-	COMPOSITE_NOMUX(0, "pclk_dbg_pre", "armclk", 0,
+	COMPOSITE_NOMUX(0, "pclk_dbg_pre", "armclk", CLK_IGNORE_UNUSED,
 			RK3288_CLKSEL_CON(37), 9, 5, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3288_CLKGATE_CON(12), 8, GFLAGS),
 	GATE(0, "pclk_dbg", "pclk_dbg_pre", 0,
 			RK3288_CLKGATE_CON(12), 9, GFLAGS),
-	GATE(0, "cs_dbg", "pclk_dbg_pre", 0,
+	GATE(0, "cs_dbg", "pclk_dbg_pre", CLK_IGNORE_UNUSED,
 			RK3288_CLKGATE_CON(12), 10, GFLAGS),
 	GATE(0, "pclk_core_niu", "pclk_dbg_pre", 0,
 			RK3288_CLKGATE_CON(12), 11, GFLAGS),
 
-	GATE(0, "dpll_ddr", "dpll", 0,
+	GATE(0, "dpll_ddr", "dpll", CLK_IGNORE_UNUSED,
 			RK3288_CLKGATE_CON(0), 8, GFLAGS),
 	GATE(0, "gpll_ddr", "gpll", 0,
 			RK3288_CLKGATE_CON(0), 9, GFLAGS),
-	COMPOSITE_NOGATE(0, "ddrphy", mux_ddrphy_p, 0,
+	COMPOSITE_NOGATE(0, "ddrphy", mux_ddrphy_p, CLK_IGNORE_UNUSED,
 			RK3288_CLKSEL_CON(26), 2, 1, MFLAGS, 0, 2,
 					DFLAGS | CLK_DIVIDER_POWER_OF_TWO),
 
-	GATE(0, "gpll_aclk_cpu", "gpll", 0,
+	GATE(0, "gpll_aclk_cpu", "gpll", CLK_IGNORE_UNUSED,
 			RK3288_CLKGATE_CON(0), 10, GFLAGS),
-	GATE(0, "cpll_aclk_cpu", "cpll", 0,
+	GATE(0, "cpll_aclk_cpu", "cpll", CLK_IGNORE_UNUSED,
 			RK3288_CLKGATE_CON(0), 11, GFLAGS),
-	COMPOSITE_NOGATE(0, "aclk_cpu_src", mux_aclk_cpu_src_p, 0,
+	COMPOSITE_NOGATE(0, "aclk_cpu_src", mux_aclk_cpu_src_p, CLK_IGNORE_UNUSED,
 			RK3288_CLKSEL_CON(1), 15, 1, MFLAGS, 3, 5, DFLAGS),
-	DIV(0, "aclk_cpu_pre", "aclk_cpu_src", 0,
+	DIV(0, "aclk_cpu_pre", "aclk_cpu_src", CLK_SET_RATE_PARENT,
 			RK3288_CLKSEL_CON(1), 0, 3, DFLAGS),
-	GATE(ACLK_CPU, "aclk_cpu", "aclk_cpu_pre", 0,
+	GATE(ACLK_CPU, "aclk_cpu", "aclk_cpu_pre", CLK_IGNORE_UNUSED,
 			RK3288_CLKGATE_CON(0), 3, GFLAGS),
-	COMPOSITE_NOMUX(PCLK_CPU, "pclk_cpu", "aclk_cpu_pre", 0,
+	COMPOSITE_NOMUX(PCLK_CPU, "pclk_cpu", "aclk_cpu_pre", CLK_IGNORE_UNUSED,
 			RK3288_CLKSEL_CON(1), 12, 3, DFLAGS,
 			RK3288_CLKGATE_CON(0), 5, GFLAGS),
-	COMPOSITE_NOMUX_DIVTBL(HCLK_CPU, "hclk_cpu", "aclk_cpu_pre", 0,
+	COMPOSITE_NOMUX_DIVTBL(HCLK_CPU, "hclk_cpu", "aclk_cpu_pre", CLK_IGNORE_UNUSED,
 			RK3288_CLKSEL_CON(1), 8, 2, DFLAGS, div_hclk_cpu_t,
 			RK3288_CLKGATE_CON(0), 4, GFLAGS),
 	GATE(0, "c2c_host", "aclk_cpu_src", 0,
@@ -294,7 +297,7 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	COMPOSITE_NOMUX(0, "crypto", "aclk_cpu_pre", 0,
 			RK3288_CLKSEL_CON(26), 6, 2, DFLAGS,
 			RK3288_CLKGATE_CON(5), 4, GFLAGS),
-	GATE(0, "aclk_bus_2pmu", "aclk_cpu_pre", 0,
+	GATE(0, "aclk_bus_2pmu", "aclk_cpu_pre", CLK_IGNORE_UNUSED,
 			RK3288_CLKGATE_CON(0), 7, GFLAGS),
 
 	COMPOSITE(0, "i2s_src", mux_pll_src_cpll_gpll_p, 0,
@@ -305,7 +308,7 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 			RK3288_CLKGATE_CON(4), 2, GFLAGS),
 	MUX(0, "i2s_pre", mux_i2s_pre_p, CLK_SET_RATE_PARENT,
 			RK3288_CLKSEL_CON(4), 8, 2, MFLAGS),
-	COMPOSITE_NODIV(0, "i2s0_clkout", mux_i2s_clkout_p, CLK_SET_RATE_PARENT,
+	COMPOSITE_NODIV(SCLK_I2S0_OUT, "i2s0_clkout", mux_i2s_clkout_p, 0,
 			RK3288_CLKSEL_CON(4), 12, 1, MFLAGS,
 			RK3288_CLKGATE_CON(4), 0, GFLAGS),
 	GATE(SCLK_I2S0, "sclk_i2s0", "i2s_pre", CLK_SET_RATE_PARENT,
@@ -325,7 +328,7 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	COMPOSITE_NOMUX(0, "spdif_8ch_pre", "spdif_src", 0,
 			RK3288_CLKSEL_CON(40), 0, 7, DFLAGS,
 			RK3288_CLKGATE_CON(4), 7, GFLAGS),
-	COMPOSITE_FRAC(0, "spdif_8ch_frac", "spdif_8ch_src", 0,
+	COMPOSITE_FRAC(0, "spdif_8ch_frac", "spdif_8ch_pre", 0,
 			RK3288_CLKSEL_CON(41), 0,
 			RK3288_CLKGATE_CON(4), 8, GFLAGS),
 	COMPOSITE_NODIV(SCLK_SPDIF8CH, "sclk_spdif_8ch", mux_spdif_8ch_p, 0,
@@ -373,12 +376,12 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	GATE(HCLK_VCODEC, "hclk_vcodec", "hclk_vcodec_pre", 0,
 		RK3288_CLKGATE_CON(9), 1, GFLAGS),
 
-	COMPOSITE(0, "aclk_vio0", mux_pll_src_cpll_gpll_usb480m_p, 0,
+	COMPOSITE(0, "aclk_vio0", mux_pll_src_cpll_gpll_usb480m_p, CLK_IGNORE_UNUSED,
 			RK3288_CLKSEL_CON(31), 6, 2, MFLAGS, 0, 5, DFLAGS,
 			RK3288_CLKGATE_CON(3), 0, GFLAGS),
 	DIV(0, "hclk_vio", "aclk_vio0", 0,
 			RK3288_CLKSEL_CON(28), 8, 5, DFLAGS),
-	COMPOSITE(0, "aclk_vio1", mux_pll_src_cpll_gpll_usb480m_p, 0,
+	COMPOSITE(0, "aclk_vio1", mux_pll_src_cpll_gpll_usb480m_p, CLK_IGNORE_UNUSED,
 			RK3288_CLKSEL_CON(31), 14, 2, MFLAGS, 8, 5, DFLAGS,
 			RK3288_CLKGATE_CON(3), 2, GFLAGS),
 
@@ -436,24 +439,24 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 
 	DIV(0, "pclk_pd_alive", "gpll", 0,
 			RK3288_CLKSEL_CON(33), 8, 5, DFLAGS),
-	COMPOSITE_NOMUX(0, "pclk_pd_pmu", "gpll", 0,
+	COMPOSITE_NOMUX(0, "pclk_pd_pmu", "gpll", CLK_IGNORE_UNUSED,
 			RK3288_CLKSEL_CON(33), 0, 5, DFLAGS,
 			RK3288_CLKGATE_CON(5), 8, GFLAGS),
 
-	COMPOSITE(SCLK_GPU, "sclk_gpu", mux_pll_src_cpll_gpll_usb480m_p, 0,
+	COMPOSITE(SCLK_GPU, "sclk_gpu", mux_pll_src_cpll_gll_usb_npll_p, 0,
 			RK3288_CLKSEL_CON(34), 6, 2, MFLAGS, 0, 5, DFLAGS,
 			RK3288_CLKGATE_CON(5), 7, GFLAGS),
 
-	COMPOSITE(0, "aclk_peri_src", mux_pll_src_cpll_gpll_p, 0,
+	COMPOSITE(0, "aclk_peri_src", mux_pll_src_cpll_gpll_p, CLK_IGNORE_UNUSED,
 			RK3288_CLKSEL_CON(10), 15, 1, MFLAGS, 0, 5, DFLAGS,
 			RK3288_CLKGATE_CON(2), 0, GFLAGS),
 	COMPOSITE_NOMUX(PCLK_PERI, "pclk_peri", "aclk_peri_src", 0,
 			RK3288_CLKSEL_CON(10), 12, 2, DFLAGS | CLK_DIVIDER_POWER_OF_TWO,
 			RK3288_CLKGATE_CON(2), 3, GFLAGS),
-	COMPOSITE_NOMUX(HCLK_PERI, "hclk_peri", "aclk_peri_src", 0,
+	COMPOSITE_NOMUX(HCLK_PERI, "hclk_peri", "aclk_peri_src", CLK_IGNORE_UNUSED,
 			RK3288_CLKSEL_CON(10), 8, 2, DFLAGS | CLK_DIVIDER_POWER_OF_TWO,
 			RK3288_CLKGATE_CON(2), 2, GFLAGS),
-	GATE(ACLK_PERI, "aclk_peri", "aclk_peri_src", 0,
+	GATE(ACLK_PERI, "aclk_peri", "aclk_peri_src", CLK_IGNORE_UNUSED,
 			RK3288_CLKGATE_CON(2), 1, GFLAGS),
 
 	/*
@@ -483,6 +486,18 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 			RK3288_CLKSEL_CON(12), 14, 2, MFLAGS, 8, 6, DFLAGS,
 			RK3288_CLKGATE_CON(13), 3, GFLAGS),
 
+	MMC(SCLK_SDMMC_DRV,    "sdmmc_drv",    "sclk_sdmmc", RK3288_SDMMC_CON0, 1),
+	MMC(SCLK_SDMMC_SAMPLE, "sdmmc_sample", "sclk_sdmmc", RK3288_SDMMC_CON1, 0),
+
+	MMC(SCLK_SDIO0_DRV,    "sdio0_drv",    "sclk_sdio0", RK3288_SDIO0_CON0, 1),
+	MMC(SCLK_SDIO0_SAMPLE, "sdio0_sample", "sclk_sdio0", RK3288_SDIO0_CON1, 0),
+
+	MMC(SCLK_SDIO1_DRV,    "sdio1_drv",    "sclk_sdio1", RK3288_SDIO1_CON0, 1),
+	MMC(SCLK_SDIO1_SAMPLE, "sdio1_sample", "sclk_sdio1", RK3288_SDIO1_CON1, 0),
+
+	MMC(SCLK_EMMC_DRV,     "emmc_drv",     "sclk_emmc",  RK3288_EMMC_CON0,  1),
+	MMC(SCLK_EMMC_SAMPLE,  "emmc_sample",  "sclk_emmc",  RK3288_EMMC_CON1,  0),
+
 	COMPOSITE(0, "sclk_tspout", mux_tspout_p, 0,
 			RK3288_CLKSEL_CON(35), 14, 2, MFLAGS, 8, 5, DFLAGS,
 			RK3288_CLKGATE_CON(4), 11, GFLAGS),
@@ -490,13 +505,13 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 			RK3288_CLKSEL_CON(35), 6, 2, MFLAGS, 0, 5, DFLAGS,
 			RK3288_CLKGATE_CON(4), 10, GFLAGS),
 
-	GATE(SCLK_OTGPHY0, "sclk_otgphy0", "usb480m", 0,
+	GATE(SCLK_OTGPHY0, "sclk_otgphy0", "usb480m", CLK_IGNORE_UNUSED,
 			RK3288_CLKGATE_CON(13), 4, GFLAGS),
-	GATE(SCLK_OTGPHY1, "sclk_otgphy1", "usb480m", 0,
+	GATE(SCLK_OTGPHY1, "sclk_otgphy1", "usb480m", CLK_IGNORE_UNUSED,
 			RK3288_CLKGATE_CON(13), 5, GFLAGS),
-	GATE(SCLK_OTGPHY2, "sclk_otgphy2", "usb480m", 0,
+	GATE(SCLK_OTGPHY2, "sclk_otgphy2", "usb480m", CLK_IGNORE_UNUSED,
 			RK3288_CLKGATE_CON(13), 6, GFLAGS),
-	GATE(SCLK_OTG_ADP, "sclk_otg_adp", "xin32k", 0,
+	GATE(SCLK_OTG_ADP, "sclk_otg_adp", "xin32k", CLK_IGNORE_UNUSED,
 			RK3288_CLKGATE_CON(13), 7, GFLAGS),
 
 	COMPOSITE_NOMUX(SCLK_TSADC, "sclk_tsadc", "xin32k", 0,
@@ -517,7 +532,7 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 			RK3288_CLKSEL_CON(38), 15, 1, MFLAGS, 8, 5, DFLAGS,
 			RK3288_CLKGATE_CON(5), 6, GFLAGS),
 
-	COMPOSITE(0, "uart0_src", mux_uart0_pll_p, 0,
+	COMPOSITE(0, "uart0_src", mux_pll_src_cpll_gll_usb_npll_p, 0,
 			RK3288_CLKSEL_CON(13), 13, 2, MFLAGS, 0, 7, DFLAGS,
 			RK3288_CLKGATE_CON(1), 8, GFLAGS),
 	COMPOSITE_FRAC(0, "uart0_frac", "uart0_src", 0,
@@ -560,18 +575,18 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	MUX(SCLK_UART4, "sclk_uart4", mux_uart4_p, 0,
 			RK3288_CLKSEL_CON(3), 8, 2, MFLAGS),
 
-	COMPOSITE(0, "mac_src", mux_pll_src_npll_cpll_gpll_p, 0,
+	COMPOSITE(0, "mac_pll_src", mux_pll_src_npll_cpll_gpll_p, 0,
 			RK3288_CLKSEL_CON(21), 0, 2, MFLAGS, 8, 5, DFLAGS,
 			RK3288_CLKGATE_CON(2), 5, GFLAGS),
-	MUX(0, "macref", mux_macref_p, 0,
+	MUX(SCLK_MAC, "mac_clk", mux_mac_p, 0,
 			RK3288_CLKSEL_CON(21), 4, 1, MFLAGS),
-	GATE(0, "sclk_macref_out", "macref", 0,
+	GATE(SCLK_MACREF_OUT, "sclk_macref_out", "mac_clk", 0,
 			RK3288_CLKGATE_CON(5), 3, GFLAGS),
-	GATE(SCLK_MACREF, "sclk_macref", "macref", 0,
+	GATE(SCLK_MACREF, "sclk_macref", "mac_clk", 0,
 			RK3288_CLKGATE_CON(5), 2, GFLAGS),
-	GATE(SCLK_MAC_RX, "sclk_mac_rx", "macref", 0,
+	GATE(SCLK_MAC_RX, "sclk_mac_rx", "mac_clk", 0,
 			RK3288_CLKGATE_CON(5), 0, GFLAGS),
-	GATE(SCLK_MAC_TX, "sclk_mac_tx", "macref", 0,
+	GATE(SCLK_MAC_TX, "sclk_mac_tx", "mac_clk", 0,
 			RK3288_CLKGATE_CON(5), 1, GFLAGS),
 
 	COMPOSITE(0, "hsadc_src", mux_pll_src_cpll_gpll_p, 0,
@@ -585,7 +600,7 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 
 	COMPOSITE_NODIV(0, "usbphy480m_src", mux_usbphy480m_p, 0,
 			RK3288_CLKSEL_CON(13), 11, 2, MFLAGS,
-			RK3288_CLKGATE_CON(5), 15, GFLAGS),
+			RK3288_CLKGATE_CON(5), 14, GFLAGS),
 	COMPOSITE_NODIV(SCLK_HSICPHY480M, "sclk_hsicphy480m", mux_hsicphy480m_p, 0,
 			RK3288_CLKSEL_CON(29), 0, 2, MFLAGS,
 			RK3288_CLKGATE_CON(3), 6, GFLAGS),
@@ -601,19 +616,19 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	 */
 
 	/* aclk_cpu gates */
-	GATE(0, "sclk_intmem0", "aclk_cpu", 0, RK3288_CLKGATE_CON(10), 5, GFLAGS),
-	GATE(0, "sclk_intmem1", "aclk_cpu", 0, RK3288_CLKGATE_CON(10), 6, GFLAGS),
-	GATE(0, "sclk_intmem2", "aclk_cpu", 0, RK3288_CLKGATE_CON(10), 7, GFLAGS),
+	GATE(0, "sclk_intmem0", "aclk_cpu", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(10), 5, GFLAGS),
+	GATE(0, "sclk_intmem1", "aclk_cpu", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(10), 6, GFLAGS),
+	GATE(0, "sclk_intmem2", "aclk_cpu", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(10), 7, GFLAGS),
 	GATE(ACLK_DMAC1, "aclk_dmac1", "aclk_cpu", 0, RK3288_CLKGATE_CON(10), 12, GFLAGS),
-	GATE(0, "aclk_strc_sys", "aclk_cpu", 0, RK3288_CLKGATE_CON(10), 13, GFLAGS),
-	GATE(0, "aclk_intmem", "aclk_cpu", 0, RK3288_CLKGATE_CON(10), 4, GFLAGS),
+	GATE(0, "aclk_strc_sys", "aclk_cpu", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(10), 13, GFLAGS),
+	GATE(0, "aclk_intmem", "aclk_cpu", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(10), 4, GFLAGS),
 	GATE(ACLK_CRYPTO, "aclk_crypto", "aclk_cpu", 0, RK3288_CLKGATE_CON(11), 6, GFLAGS),
 	GATE(0, "aclk_ccp", "aclk_cpu", 0, RK3288_CLKGATE_CON(11), 8, GFLAGS),
 
 	/* hclk_cpu gates */
 	GATE(HCLK_CRYPTO, "hclk_crypto", "hclk_cpu", 0, RK3288_CLKGATE_CON(11), 7, GFLAGS),
 	GATE(HCLK_I2S0, "hclk_i2s0", "hclk_cpu", 0, RK3288_CLKGATE_CON(10), 8, GFLAGS),
-	GATE(HCLK_ROM, "hclk_rom", "hclk_cpu", 0, RK3288_CLKGATE_CON(10), 9, GFLAGS),
+	GATE(HCLK_ROM, "hclk_rom", "hclk_cpu", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(10), 9, GFLAGS),
 	GATE(HCLK_SPDIF, "hclk_spdif", "hclk_cpu", 0, RK3288_CLKGATE_CON(10), 10, GFLAGS),
 	GATE(HCLK_SPDIF8CH, "hclk_spdif_8ch", "hclk_cpu", 0, RK3288_CLKGATE_CON(10), 11, GFLAGS),
 
@@ -622,42 +637,42 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	GATE(PCLK_TIMER, "pclk_timer", "pclk_cpu", 0, RK3288_CLKGATE_CON(10), 1, GFLAGS),
 	GATE(PCLK_I2C0, "pclk_i2c0", "pclk_cpu", 0, RK3288_CLKGATE_CON(10), 2, GFLAGS),
 	GATE(PCLK_I2C2, "pclk_i2c2", "pclk_cpu", 0, RK3288_CLKGATE_CON(10), 3, GFLAGS),
-	GATE(0, "pclk_ddrupctl0", "pclk_cpu", 0, RK3288_CLKGATE_CON(10), 14, GFLAGS),
-	GATE(0, "pclk_publ0", "pclk_cpu", 0, RK3288_CLKGATE_CON(10), 15, GFLAGS),
-	GATE(0, "pclk_ddrupctl1", "pclk_cpu", 0, RK3288_CLKGATE_CON(11), 0, GFLAGS),
-	GATE(0, "pclk_publ1", "pclk_cpu", 0, RK3288_CLKGATE_CON(11), 1, GFLAGS),
+	GATE(PCLK_DDRUPCTL0, "pclk_ddrupctl0", "pclk_cpu", 0, RK3288_CLKGATE_CON(10), 14, GFLAGS),
+	GATE(PCLK_PUBL0, "pclk_publ0", "pclk_cpu", 0, RK3288_CLKGATE_CON(10), 15, GFLAGS),
+	GATE(PCLK_DDRUPCTL1, "pclk_ddrupctl1", "pclk_cpu", 0, RK3288_CLKGATE_CON(11), 0, GFLAGS),
+	GATE(PCLK_PUBL1, "pclk_publ1", "pclk_cpu", 0, RK3288_CLKGATE_CON(11), 1, GFLAGS),
 	GATE(0, "pclk_efuse_1024", "pclk_cpu", 0, RK3288_CLKGATE_CON(11), 2, GFLAGS),
 	GATE(PCLK_TZPC, "pclk_tzpc", "pclk_cpu", 0, RK3288_CLKGATE_CON(11), 3, GFLAGS),
 	GATE(PCLK_UART2, "pclk_uart2", "pclk_cpu", 0, RK3288_CLKGATE_CON(11), 9, GFLAGS),
 	GATE(0, "pclk_efuse_256", "pclk_cpu", 0, RK3288_CLKGATE_CON(11), 10, GFLAGS),
-	GATE(PCLK_RKPWM, "pclk_rkpwm", "pclk_cpu", 0, RK3288_CLKGATE_CON(11), 11, GFLAGS),
+	GATE(PCLK_RKPWM, "pclk_rkpwm", "pclk_cpu", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(11), 11, GFLAGS),
 
 	/* ddrctrl [DDR Controller PHY clock] gates */
-	GATE(0, "nclk_ddrupctl0", "ddrphy", 0, RK3288_CLKGATE_CON(11), 4, GFLAGS),
-	GATE(0, "nclk_ddrupctl1", "ddrphy", 0, RK3288_CLKGATE_CON(11), 5, GFLAGS),
+	GATE(0, "nclk_ddrupctl0", "ddrphy", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(11), 4, GFLAGS),
+	GATE(0, "nclk_ddrupctl1", "ddrphy", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(11), 5, GFLAGS),
 
 	/* ddrphy gates */
-	GATE(0, "sclk_ddrphy0", "ddrphy", 0, RK3288_CLKGATE_CON(4), 12, GFLAGS),
-	GATE(0, "sclk_ddrphy1", "ddrphy", 0, RK3288_CLKGATE_CON(4), 13, GFLAGS),
+	GATE(0, "sclk_ddrphy0", "ddrphy", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(4), 12, GFLAGS),
+	GATE(0, "sclk_ddrphy1", "ddrphy", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(4), 13, GFLAGS),
 
 	/* aclk_peri gates */
-	GATE(0, "aclk_peri_axi_matrix", "aclk_peri", 0, RK3288_CLKGATE_CON(6), 2, GFLAGS),
+	GATE(0, "aclk_peri_axi_matrix", "aclk_peri", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(6), 2, GFLAGS),
 	GATE(ACLK_DMAC2, "aclk_dmac2", "aclk_peri", 0, RK3288_CLKGATE_CON(6), 3, GFLAGS),
-	GATE(0, "aclk_peri_niu", "aclk_peri", 0, RK3288_CLKGATE_CON(7), 11, GFLAGS),
-	GATE(ACLK_MMU, "aclk_mmu", "aclk_peri", 0, RK3288_CLKGATE_CON(8), 12, GFLAGS),
+	GATE(0, "aclk_peri_niu", "aclk_peri", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(7), 11, GFLAGS),
+	GATE(ACLK_MMU, "aclk_mmu", "aclk_peri", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(8), 12, GFLAGS),
 	GATE(ACLK_GMAC, "aclk_gmac", "aclk_peri", 0, RK3288_CLKGATE_CON(8), 0, GFLAGS),
 	GATE(HCLK_GPS, "hclk_gps", "aclk_peri", 0, RK3288_CLKGATE_CON(8), 2, GFLAGS),
 
 	/* hclk_peri gates */
-	GATE(0, "hclk_peri_matrix", "hclk_peri", 0, RK3288_CLKGATE_CON(6), 0, GFLAGS),
-	GATE(HCLK_OTG0, "hclk_otg0", "hclk_peri", 0, RK3288_CLKGATE_CON(7), 4, GFLAGS),
+	GATE(0, "hclk_peri_matrix", "hclk_peri", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(6), 0, GFLAGS),
+	GATE(HCLK_OTG0, "hclk_otg0", "hclk_peri", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(7), 4, GFLAGS),
 	GATE(HCLK_USBHOST0, "hclk_host0", "hclk_peri", 0, RK3288_CLKGATE_CON(7), 6, GFLAGS),
-	GATE(HCLK_USBHOST1, "hclk_host1", "hclk_peri", 0, RK3288_CLKGATE_CON(7), 7, GFLAGS),
+	GATE(HCLK_USBHOST1, "hclk_host1", "hclk_peri", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(7), 7, GFLAGS),
 	GATE(HCLK_HSIC, "hclk_hsic", "hclk_peri", 0, RK3288_CLKGATE_CON(7), 8, GFLAGS),
-	GATE(0, "hclk_usb_peri", "hclk_peri", 0, RK3288_CLKGATE_CON(7), 9, GFLAGS),
-	GATE(0, "hclk_peri_ahb_arbi", "hclk_peri", 0, RK3288_CLKGATE_CON(7), 10, GFLAGS),
-	GATE(0, "hclk_emem", "hclk_peri", 0, RK3288_CLKGATE_CON(7), 12, GFLAGS),
-	GATE(0, "hclk_mem", "hclk_peri", 0, RK3288_CLKGATE_CON(7), 13, GFLAGS),
+	GATE(0, "hclk_usb_peri", "hclk_peri", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(7), 9, GFLAGS),
+	GATE(0, "hclk_peri_ahb_arbi", "hclk_peri", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(7), 10, GFLAGS),
+	GATE(0, "hclk_emem", "hclk_peri", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(7), 12, GFLAGS),
+	GATE(0, "hclk_mem", "hclk_peri", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(7), 13, GFLAGS),
 	GATE(HCLK_NANDC0, "hclk_nandc0", "hclk_peri", 0, RK3288_CLKGATE_CON(7), 14, GFLAGS),
 	GATE(HCLK_NANDC1, "hclk_nandc1", "hclk_peri", 0, RK3288_CLKGATE_CON(7), 15, GFLAGS),
 	GATE(HCLK_TSP, "hclk_tsp", "hclk_peri", 0, RK3288_CLKGATE_CON(8), 8, GFLAGS),
@@ -669,7 +684,7 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	GATE(0, "pmu_hclk_otg0", "hclk_peri", 0, RK3288_CLKGATE_CON(7), 5, GFLAGS),
 
 	/* pclk_peri gates */
-	GATE(0, "pclk_peri_matrix", "pclk_peri", 0, RK3288_CLKGATE_CON(6), 1, GFLAGS),
+	GATE(0, "pclk_peri_matrix", "pclk_peri", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(6), 1, GFLAGS),
 	GATE(PCLK_SPI0, "pclk_spi0", "pclk_peri", 0, RK3288_CLKGATE_CON(6), 4, GFLAGS),
 	GATE(PCLK_SPI1, "pclk_spi1", "pclk_peri", 0, RK3288_CLKGATE_CON(6), 5, GFLAGS),
 	GATE(PCLK_SPI2, "pclk_spi2", "pclk_peri", 0, RK3288_CLKGATE_CON(6), 6, GFLAGS),
@@ -705,48 +720,48 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	GATE(PCLK_GPIO4, "pclk_gpio4", "pclk_pd_alive", 0, RK3288_CLKGATE_CON(14), 4, GFLAGS),
 	GATE(PCLK_GPIO5, "pclk_gpio5", "pclk_pd_alive", 0, RK3288_CLKGATE_CON(14), 5, GFLAGS),
 	GATE(PCLK_GPIO6, "pclk_gpio6", "pclk_pd_alive", 0, RK3288_CLKGATE_CON(14), 6, GFLAGS),
-	GATE(PCLK_GRF, "pclk_grf", "pclk_pd_alive", 0, RK3288_CLKGATE_CON(14), 11, GFLAGS),
-	GATE(0, "pclk_alive_niu", "pclk_pd_alive", 0, RK3288_CLKGATE_CON(14), 12, GFLAGS),
+	GATE(PCLK_GRF, "pclk_grf", "pclk_pd_alive", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(14), 11, GFLAGS),
+	GATE(0, "pclk_alive_niu", "pclk_pd_alive", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(14), 12, GFLAGS),
 
 	/* pclk_pd_pmu gates */
-	GATE(PCLK_PMU, "pclk_pmu", "pclk_pd_pmu", 0, RK3288_CLKGATE_CON(17), 0, GFLAGS),
-	GATE(0, "pclk_intmem1", "pclk_pd_pmu", 0, RK3288_CLKGATE_CON(17), 1, GFLAGS),
-	GATE(0, "pclk_pmu_niu", "pclk_pd_pmu", 0, RK3288_CLKGATE_CON(17), 2, GFLAGS),
-	GATE(PCLK_SGRF, "pclk_sgrf", "pclk_pd_pmu", 0, RK3288_CLKGATE_CON(17), 3, GFLAGS),
+	GATE(PCLK_PMU, "pclk_pmu", "pclk_pd_pmu", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(17), 0, GFLAGS),
+	GATE(0, "pclk_intmem1", "pclk_pd_pmu", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(17), 1, GFLAGS),
+	GATE(0, "pclk_pmu_niu", "pclk_pd_pmu", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(17), 2, GFLAGS),
+	GATE(PCLK_SGRF, "pclk_sgrf", "pclk_pd_pmu", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(17), 3, GFLAGS),
 	GATE(PCLK_GPIO0, "pclk_gpio0", "pclk_pd_pmu", 0, RK3288_CLKGATE_CON(17), 4, GFLAGS),
 
 	/* hclk_vio gates */
 	GATE(HCLK_RGA, "hclk_rga", "hclk_vio", 0, RK3288_CLKGATE_CON(15), 1, GFLAGS),
 	GATE(HCLK_VOP0, "hclk_vop0", "hclk_vio", 0, RK3288_CLKGATE_CON(15), 6, GFLAGS),
 	GATE(HCLK_VOP1, "hclk_vop1", "hclk_vio", 0, RK3288_CLKGATE_CON(15), 8, GFLAGS),
-	GATE(HCLK_VIO_AHB_ARBI, "hclk_vio_ahb_arbi", "hclk_vio", 0, RK3288_CLKGATE_CON(15), 9, GFLAGS),
-	GATE(HCLK_VIO_NIU, "hclk_vio_niu", "hclk_vio", 0, RK3288_CLKGATE_CON(15), 10, GFLAGS),
+	GATE(HCLK_VIO_AHB_ARBI, "hclk_vio_ahb_arbi", "hclk_vio", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(15), 9, GFLAGS),
+	GATE(HCLK_VIO_NIU, "hclk_vio_niu", "hclk_vio", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(15), 10, GFLAGS),
 	GATE(HCLK_VIP, "hclk_vip", "hclk_vio", 0, RK3288_CLKGATE_CON(15), 15, GFLAGS),
 	GATE(HCLK_IEP, "hclk_iep", "hclk_vio", 0, RK3288_CLKGATE_CON(15), 3, GFLAGS),
 	GATE(HCLK_ISP, "hclk_isp", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 1, GFLAGS),
-	GATE(HCLK_VIO2_H2P, "hclk_vio2_h2p", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 10, GFLAGS),
+	GATE(HCLK_VIO2_H2P, "hclk_vio2_h2p", "hclk_vio", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(16), 10, GFLAGS),
 	GATE(PCLK_MIPI_DSI0, "pclk_mipi_dsi0", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 4, GFLAGS),
 	GATE(PCLK_MIPI_DSI1, "pclk_mipi_dsi1", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 5, GFLAGS),
 	GATE(PCLK_MIPI_CSI, "pclk_mipi_csi", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 6, GFLAGS),
 	GATE(PCLK_LVDS_PHY, "pclk_lvds_phy", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 7, GFLAGS),
-	GATE(PCLK_EDP_CTRL, "pclk_edp_ctrl", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 8, GFLAGS),
+	GATE(PCLK_EDP_CTRL, "pclk_edp_ctrl", "hclk_vio", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(16), 8, GFLAGS),
 	GATE(PCLK_HDMI_CTRL, "pclk_hdmi_ctrl", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 9, GFLAGS),
-	GATE(PCLK_VIO2_H2P, "pclk_vio2_h2p", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 11, GFLAGS),
+	GATE(PCLK_VIO2_H2P, "pclk_vio2_h2p", "hclk_vio", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(16), 11, GFLAGS),
 
 	/* aclk_vio0 gates */
 	GATE(ACLK_VOP0, "aclk_vop0", "aclk_vio0", 0, RK3288_CLKGATE_CON(15), 5, GFLAGS),
 	GATE(ACLK_IEP, "aclk_iep", "aclk_vio0", 0, RK3288_CLKGATE_CON(15), 2, GFLAGS),
-	GATE(ACLK_VIO0_NIU, "aclk_vio0_niu", "aclk_vio0", 0, RK3288_CLKGATE_CON(15), 11, GFLAGS),
+	GATE(ACLK_VIO0_NIU, "aclk_vio0_niu", "aclk_vio0", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(15), 11, GFLAGS),
 	GATE(ACLK_VIP, "aclk_vip", "aclk_vio0", 0, RK3288_CLKGATE_CON(15), 14, GFLAGS),
 
 	/* aclk_vio1 gates */
 	GATE(ACLK_VOP1, "aclk_vop1", "aclk_vio1", 0, RK3288_CLKGATE_CON(15), 7, GFLAGS),
 	GATE(ACLK_ISP, "aclk_isp", "aclk_vio1", 0, RK3288_CLKGATE_CON(16), 2, GFLAGS),
-	GATE(ACLK_VIO1_NIU, "aclk_vio1_niu", "aclk_vio1", 0, RK3288_CLKGATE_CON(15), 12, GFLAGS),
+	GATE(ACLK_VIO1_NIU, "aclk_vio1_niu", "aclk_vio1", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(15), 12, GFLAGS),
 
 	/* aclk_rga_pre gates */
 	GATE(ACLK_RGA, "aclk_rga", "aclk_rga_pre", 0, RK3288_CLKGATE_CON(15), 0, GFLAGS),
-	GATE(ACLK_RGA_NIU, "aclk_rga_niu", "aclk_rga_pre", 0, RK3288_CLKGATE_CON(15), 13, GFLAGS),
+	GATE(ACLK_RGA_NIU, "aclk_rga_niu", "aclk_rga_pre", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(15), 13, GFLAGS),
 
 	/*
 	 * Other ungrouped clocks.
@@ -762,6 +777,64 @@ static const char *rk3288_critical_clocks[] __initconst = {
 	"hclk_peri",
 };
 
+#ifdef CONFIG_PM_SLEEP
+static void __iomem *rk3288_cru_base;
+
+/* Some CRU registers will be reset in maskrom when the system
+ * wakes up from fastboot.
+ * So save them before suspend, restore them after resume.
+ */
+static const int rk3288_saved_cru_reg_ids[] = {
+	RK3288_MODE_CON,
+	RK3288_CLKSEL_CON(0),
+	RK3288_CLKSEL_CON(1),
+	RK3288_CLKSEL_CON(10),
+	RK3288_CLKSEL_CON(33),
+	RK3288_CLKSEL_CON(37),
+};
+
+static u32 rk3288_saved_cru_regs[ARRAY_SIZE(rk3288_saved_cru_reg_ids)];
+
+static int rk3288_clk_suspend(void)
+{
+	int i, reg_id;
+
+	for (i = 0; i < ARRAY_SIZE(rk3288_saved_cru_reg_ids); i++) {
+		reg_id = rk3288_saved_cru_reg_ids[i];
+
+		rk3288_saved_cru_regs[i] =
+				readl_relaxed(rk3288_cru_base + reg_id);
+	}
+	return 0;
+}
+
+static void rk3288_clk_resume(void)
+{
+	int i, reg_id;
+
+	for (i = ARRAY_SIZE(rk3288_saved_cru_reg_ids) - 1; i >= 0; i--) {
+		reg_id = rk3288_saved_cru_reg_ids[i];
+
+		writel_relaxed(rk3288_saved_cru_regs[i] | 0xffff0000,
+			       rk3288_cru_base + reg_id);
+	}
+}
+
+static struct syscore_ops rk3288_clk_syscore_ops = {
+	.suspend = rk3288_clk_suspend,
+	.resume = rk3288_clk_resume,
+};
+
+static void rk3288_clk_sleep_init(void __iomem *reg_base)
+{
+	rk3288_cru_base = reg_base;
+	register_syscore_ops(&rk3288_clk_syscore_ops);
+}
+
+#else /* CONFIG_PM_SLEEP */
+static void rk3288_clk_sleep_init(void __iomem *reg_base) {}
+#endif
+
 static void __init rk3288_clk_init(struct device_node *np)
 {
 	void __iomem *reg_base;
@@ -810,5 +883,6 @@ static void __init rk3288_clk_init(struct device_node *np)
 				  ROCKCHIP_SOFTRST_HIWORD_MASK);
 
 	rockchip_register_restart_notifier(RK3288_GLB_SRST_FST);
+	rk3288_clk_sleep_init(reg_base);
 }
 CLK_OF_DECLARE(rk3288_cru, "rockchip,rk3288-cru", rk3288_clk_init);
diff --git a/drivers/clk/rockchip/clk.c b/drivers/clk/rockchip/clk.c
index 880a266f0143..20e05bbb3a67 100644
--- a/drivers/clk/rockchip/clk.c
+++ b/drivers/clk/rockchip/clk.c
@@ -197,7 +197,8 @@ void __init rockchip_clk_register_plls(struct rockchip_pll_clock *list,
 				list->parent_names, list->num_parents,
 				reg_base, list->con_offset, grf_lock_offset,
 				list->lock_shift, list->mode_offset,
-				list->mode_shift, list->rate_table, &clk_lock);
+				list->mode_shift, list->rate_table,
+				list->pll_flags, &clk_lock);
 		if (IS_ERR(clk)) {
 			pr_err("%s: failed to register clock %s\n", __func__,
 				list->name);
@@ -244,9 +245,6 @@ void __init rockchip_clk_register_branches(
 					list->div_flags, &clk_lock);
 			break;
 		case branch_fraction_divider:
-			/* keep all gates untouched for now */
-			flags |= CLK_IGNORE_UNUSED;
-
 			clk = rockchip_clk_register_frac_branch(list->name,
 				list->parent_names, list->num_parents,
 				reg_base, list->muxdiv_offset, list->div_flags,
@@ -256,18 +254,12 @@ void __init rockchip_clk_register_branches(
 		case branch_gate:
 			flags |= CLK_SET_RATE_PARENT;
 
-			/* keep all gates untouched for now */
-			flags |= CLK_IGNORE_UNUSED;
-
 			clk = clk_register_gate(NULL, list->name,
 				list->parent_names[0], flags,
 				reg_base + list->gate_offset,
 				list->gate_shift, list->gate_flags, &clk_lock);
 			break;
 		case branch_composite:
-			/* keep all gates untouched for now */
-			flags |= CLK_IGNORE_UNUSED;
-
 			clk = rockchip_clk_register_branch(list->name,
 				list->parent_names, list->num_parents,
 				reg_base, list->muxdiv_offset, list->mux_shift,
@@ -277,6 +269,14 @@ void __init rockchip_clk_register_branches(
 				list->gate_offset, list->gate_shift,
 				list->gate_flags, flags, &clk_lock);
 			break;
+		case branch_mmc:
+			clk = rockchip_clk_register_mmc(
+				list->name,
+				list->parent_names, list->num_parents,
+				reg_base + list->muxdiv_offset,
+				list->div_shift
+			);
+			break;
 		}
 
 		/* none of the cases above matched */
diff --git a/drivers/clk/rockchip/clk.h b/drivers/clk/rockchip/clk.h
index ca009ab0a33a..58d2e3bdf22f 100644
--- a/drivers/clk/rockchip/clk.h
+++ b/drivers/clk/rockchip/clk.h
@@ -48,6 +48,14 @@
 #define RK3288_GLB_SRST_SND		0x1b4
 #define RK3288_SOFTRST_CON(x)		(x * 0x4 + 0x1b8)
 #define RK3288_MISC_CON			0x1e8
+#define RK3288_SDMMC_CON0		0x200
+#define RK3288_SDMMC_CON1		0x204
+#define RK3288_SDIO0_CON0		0x208
+#define RK3288_SDIO0_CON1		0x20c
+#define RK3288_SDIO1_CON0		0x210
+#define RK3288_SDIO1_CON1		0x214
+#define RK3288_EMMC_CON0		0x218
+#define RK3288_EMMC_CON1		0x21c
 
 enum rockchip_pll_type {
 	pll_rk3066,
@@ -62,6 +70,15 @@ enum rockchip_pll_type {
 	.bwadj = (_nf >> 1),			\
 }
 
+#define RK3066_PLL_RATE_BWADJ(_rate, _nr, _nf, _no, _bw)	\
+{								\
+	.rate	= _rate##U,					\
+	.nr = _nr,						\
+	.nf = _nf,						\
+	.no = _no,						\
+	.bwadj = _bw,						\
+}
+
 struct rockchip_pll_rate_table {
 	unsigned long rate;
 	unsigned int nr;
@@ -81,7 +98,12 @@ struct rockchip_pll_rate_table {
  * @mode_shift: offset inside the mode-register for the mode of this pll.
  * @lock_shift: offset inside the lock register for the lock status.
  * @type: Type of PLL to be registered.
+ * @pll_flags: hardware-specific flags
  * @rate_table: Table of usable pll rates
+ *
+ * Flags:
+ * ROCKCHIP_PLL_SYNC_RATE - check rate parameters to match against the
+ *	rate_table parameters and ajust them if necessary.
  */
 struct rockchip_pll_clock {
 	unsigned int		id;
@@ -94,11 +116,14 @@ struct rockchip_pll_clock {
 	int			mode_shift;
 	int			lock_shift;
 	enum rockchip_pll_type	type;
+	u8			pll_flags;
 	struct rockchip_pll_rate_table *rate_table;
 };
 
+#define ROCKCHIP_PLL_SYNC_RATE		BIT(0)
+
 #define PLL(_type, _id, _name, _pnames, _flags, _con, _mode, _mshift,	\
-		_lshift, _rtable)					\
+		_lshift, _pflags, _rtable)				\
 	{								\
 		.id		= _id,					\
 		.type		= _type,				\
@@ -110,6 +135,7 @@ struct rockchip_pll_clock {
 		.mode_offset	= _mode,				\
 		.mode_shift	= _mshift,				\
 		.lock_shift	= _lshift,				\
+		.pll_flags	= _pflags,				\
 		.rate_table	= _rtable,				\
 	}
 
@@ -118,7 +144,7 @@ struct clk *rockchip_clk_register_pll(enum rockchip_pll_type pll_type,
 		void __iomem *base, int con_offset, int grf_lock_offset,
 		int lock_shift, int reg_mode, int mode_shift,
 		struct rockchip_pll_rate_table *rate_table,
-		spinlock_t *lock);
+		u8 clk_pll_flags, spinlock_t *lock);
 
 struct rockchip_cpuclk_clksel {
 	int reg;
@@ -152,6 +178,10 @@ struct clk *rockchip_clk_register_cpuclk(const char *name,
 			const struct rockchip_cpuclk_rate_table *rates,
 			int nrates, void __iomem *reg_base, spinlock_t *lock);
 
+struct clk *rockchip_clk_register_mmc(const char *name,
+				const char **parent_names, u8 num_parents,
+				void __iomem *reg, int shift);
+
 #define PNAME(x) static const char *x[] __initconst
 
 enum rockchip_clk_branch_type {
@@ -160,6 +190,7 @@ enum rockchip_clk_branch_type {
 	branch_divider,
 	branch_fraction_divider,
 	branch_gate,
+	branch_mmc,
 };
 
 struct rockchip_clk_branch {
@@ -352,6 +383,16 @@ struct rockchip_clk_branch {
 		.gate_flags	= gf,				\
 	}
 
+#define MMC(_id, cname, pname, offset, shift)			\
+	{							\
+		.id		= _id,				\
+		.branch_type	= branch_mmc,			\
+		.name		= cname,			\
+		.parent_names	= (const char *[]){ pname },	\
+		.num_parents	= 1,				\
+		.muxdiv_offset	= offset,			\
+		.div_shift	= shift,			\
+	}
 
 void rockchip_clk_init(struct device_node *np, void __iomem *base,
 		       unsigned long nr_clks);
diff --git a/drivers/clk/samsung/Makefile b/drivers/clk/samsung/Makefile
index 6fb4bc602e8a..006c6f294310 100644
--- a/drivers/clk/samsung/Makefile
+++ b/drivers/clk/samsung/Makefile
@@ -5,6 +5,7 @@
 obj-$(CONFIG_COMMON_CLK)	+= clk.o clk-pll.o
 obj-$(CONFIG_SOC_EXYNOS3250)	+= clk-exynos3250.o
 obj-$(CONFIG_ARCH_EXYNOS4)	+= clk-exynos4.o
+obj-$(CONFIG_SOC_EXYNOS4415)	+= clk-exynos4415.o
 obj-$(CONFIG_SOC_EXYNOS5250)	+= clk-exynos5250.o
 obj-$(CONFIG_SOC_EXYNOS5260)	+= clk-exynos5260.o
 obj-$(CONFIG_SOC_EXYNOS5410)	+= clk-exynos5410.o
@@ -12,6 +13,7 @@ obj-$(CONFIG_SOC_EXYNOS5420)	+= clk-exynos5420.o
 obj-$(CONFIG_SOC_EXYNOS5440)	+= clk-exynos5440.o
 obj-$(CONFIG_ARCH_EXYNOS)	+= clk-exynos-audss.o
 obj-$(CONFIG_ARCH_EXYNOS)	+= clk-exynos-clkout.o
+obj-$(CONFIG_ARCH_EXYNOS7)	+= clk-exynos7.o
 obj-$(CONFIG_S3C2410_COMMON_CLK)+= clk-s3c2410.o
 obj-$(CONFIG_S3C2410_COMMON_DCLK)+= clk-s3c2410-dclk.o
 obj-$(CONFIG_S3C2412_COMMON_CLK)+= clk-s3c2412.o
diff --git a/drivers/clk/samsung/clk-exynos-audss.c b/drivers/clk/samsung/clk-exynos-audss.c
index 13eae14c2cc2..f2c2ccce49bb 100644
--- a/drivers/clk/samsung/clk-exynos-audss.c
+++ b/drivers/clk/samsung/clk-exynos-audss.c
@@ -29,6 +29,13 @@ static DEFINE_SPINLOCK(lock);
 static struct clk **clk_table;
 static void __iomem *reg_base;
 static struct clk_onecell_data clk_data;
+/*
+ * On Exynos5420 this will be a clock which has to be enabled before any
+ * access to audss registers. Typically a child of EPLL.
+ *
+ * On other platforms this will be -ENODEV.
+ */
+static struct clk *epll;
 
 #define ASS_CLK_SRC 0x0
 #define ASS_CLK_DIV 0x4
@@ -98,6 +105,8 @@ static int exynos_audss_clk_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to map audss registers\n");
 		return PTR_ERR(reg_base);
 	}
+	/* EPLL don't have to be enabled for boards other than Exynos5420 */
+	epll = ERR_PTR(-ENODEV);
 
 	clk_table = devm_kzalloc(&pdev->dev,
 				sizeof(struct clk *) * EXYNOS_AUDSS_MAX_CLKS,
@@ -115,8 +124,20 @@ static int exynos_audss_clk_probe(struct platform_device *pdev)
 	pll_in = devm_clk_get(&pdev->dev, "pll_in");
 	if (!IS_ERR(pll_ref))
 		mout_audss_p[0] = __clk_get_name(pll_ref);
-	if (!IS_ERR(pll_in))
+	if (!IS_ERR(pll_in)) {
 		mout_audss_p[1] = __clk_get_name(pll_in);
+
+		if (variant == TYPE_EXYNOS5420) {
+			epll = pll_in;
+
+			ret = clk_prepare_enable(epll);
+			if (ret) {
+				dev_err(&pdev->dev,
+						"failed to prepare the epll clock\n");
+				return ret;
+			}
+		}
+	}
 	clk_table[EXYNOS_MOUT_AUDSS] = clk_register_mux(NULL, "mout_audss",
 				mout_audss_p, ARRAY_SIZE(mout_audss_p),
 				CLK_SET_RATE_NO_REPARENT,
@@ -203,6 +224,9 @@ unregister:
 			clk_unregister(clk_table[i]);
 	}
 
+	if (!IS_ERR(epll))
+		clk_disable_unprepare(epll);
+
 	return ret;
 }
 
@@ -210,6 +234,10 @@ static int exynos_audss_clk_remove(struct platform_device *pdev)
 {
 	int i;
 
+#ifdef CONFIG_PM_SLEEP
+	unregister_syscore_ops(&exynos_audss_clk_syscore_ops);
+#endif
+
 	of_clk_del_provider(pdev->dev.of_node);
 
 	for (i = 0; i < clk_data.clk_num; i++) {
@@ -217,13 +245,15 @@ static int exynos_audss_clk_remove(struct platform_device *pdev)
 			clk_unregister(clk_table[i]);
 	}
 
+	if (!IS_ERR(epll))
+		clk_disable_unprepare(epll);
+
 	return 0;
 }
 
 static struct platform_driver exynos_audss_clk_driver = {
 	.driver	= {
 		.name = "exynos-audss-clk",
-		.owner = THIS_MODULE,
 		.of_match_table = exynos_audss_clk_of_match,
 	},
 	.probe = exynos_audss_clk_probe,
diff --git a/drivers/clk/samsung/clk-exynos4.c b/drivers/clk/samsung/clk-exynos4.c
index 940f02837b82..88e8c6bbd77f 100644
--- a/drivers/clk/samsung/clk-exynos4.c
+++ b/drivers/clk/samsung/clk-exynos4.c
@@ -505,7 +505,7 @@ static struct samsung_fixed_rate_clock exynos4_fixed_rate_ext_clks[] __initdata
 /* fixed rate clocks generated inside the soc */
 static struct samsung_fixed_rate_clock exynos4_fixed_rate_clks[] __initdata = {
 	FRATE(0, "sclk_hdmi24m", NULL, CLK_IS_ROOT, 24000000),
-	FRATE(CLK_SCLK_HDMIPHY, "sclk_hdmiphy", NULL, CLK_IS_ROOT, 27000000),
+	FRATE(CLK_SCLK_HDMIPHY, "sclk_hdmiphy", "hdmi", 0, 27000000),
 	FRATE(0, "sclk_usbphy0", NULL, CLK_IS_ROOT, 48000000),
 };
 
diff --git a/drivers/clk/samsung/clk-exynos4415.c b/drivers/clk/samsung/clk-exynos4415.c
new file mode 100644
index 000000000000..2123fc251e0f
--- /dev/null
+++ b/drivers/clk/samsung/clk-exynos4415.c
@@ -0,0 +1,1144 @@
+/*
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ * Author: Chanwoo Choi <cw00.choi@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Common Clock Framework support for Exynos4415 SoC.
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/syscore_ops.h>
+
+#include <dt-bindings/clock/exynos4415.h>
+
+#include "clk.h"
+#include "clk-pll.h"
+
+#define SRC_LEFTBUS		0x4200
+#define DIV_LEFTBUS		0x4500
+#define GATE_IP_LEFTBUS		0x4800
+#define GATE_IP_IMAGE		0x4930
+#define SRC_RIGHTBUS		0x8200
+#define DIV_RIGHTBUS		0x8500
+#define GATE_IP_RIGHTBUS	0x8800
+#define GATE_IP_PERIR		0x8960
+#define EPLL_LOCK		0xc010
+#define G3D_PLL_LOCK		0xc020
+#define DISP_PLL_LOCK		0xc030
+#define ISP_PLL_LOCK		0xc040
+#define EPLL_CON0		0xc110
+#define EPLL_CON1		0xc114
+#define EPLL_CON2		0xc118
+#define G3D_PLL_CON0		0xc120
+#define G3D_PLL_CON1		0xc124
+#define G3D_PLL_CON2		0xc128
+#define ISP_PLL_CON0		0xc130
+#define ISP_PLL_CON1		0xc134
+#define ISP_PLL_CON2		0xc138
+#define DISP_PLL_CON0		0xc140
+#define DISP_PLL_CON1		0xc144
+#define DISP_PLL_CON2		0xc148
+#define SRC_TOP0		0xc210
+#define SRC_TOP1		0xc214
+#define SRC_CAM			0xc220
+#define SRC_TV			0xc224
+#define SRC_MFC			0xc228
+#define SRC_G3D			0xc22c
+#define SRC_LCD			0xc234
+#define SRC_ISP			0xc238
+#define SRC_MAUDIO		0xc23c
+#define SRC_FSYS		0xc240
+#define SRC_PERIL0		0xc250
+#define SRC_PERIL1		0xc254
+#define SRC_CAM1		0xc258
+#define SRC_TOP_ISP0		0xc25c
+#define SRC_TOP_ISP1		0xc260
+#define SRC_MASK_TOP		0xc310
+#define SRC_MASK_CAM		0xc320
+#define SRC_MASK_TV		0xc324
+#define SRC_MASK_LCD		0xc334
+#define SRC_MASK_ISP		0xc338
+#define SRC_MASK_MAUDIO		0xc33c
+#define SRC_MASK_FSYS		0xc340
+#define SRC_MASK_PERIL0		0xc350
+#define SRC_MASK_PERIL1		0xc354
+#define DIV_TOP			0xc510
+#define DIV_CAM			0xc520
+#define DIV_TV			0xc524
+#define DIV_MFC			0xc528
+#define DIV_G3D			0xc52c
+#define DIV_LCD			0xc534
+#define DIV_ISP			0xc538
+#define DIV_MAUDIO		0xc53c
+#define DIV_FSYS0		0xc540
+#define DIV_FSYS1		0xc544
+#define DIV_FSYS2		0xc548
+#define DIV_PERIL0		0xc550
+#define DIV_PERIL1		0xc554
+#define DIV_PERIL2		0xc558
+#define DIV_PERIL3		0xc55c
+#define DIV_PERIL4		0xc560
+#define DIV_PERIL5		0xc564
+#define DIV_CAM1		0xc568
+#define DIV_TOP_ISP1		0xc56c
+#define DIV_TOP_ISP0		0xc570
+#define CLKDIV2_RATIO		0xc580
+#define GATE_SCLK_CAM		0xc820
+#define GATE_SCLK_TV		0xc824
+#define GATE_SCLK_MFC		0xc828
+#define GATE_SCLK_G3D		0xc82c
+#define GATE_SCLK_LCD		0xc834
+#define GATE_SCLK_MAUDIO	0xc83c
+#define GATE_SCLK_FSYS		0xc840
+#define GATE_SCLK_PERIL		0xc850
+#define GATE_IP_CAM		0xc920
+#define GATE_IP_TV		0xc924
+#define GATE_IP_MFC		0xc928
+#define GATE_IP_G3D		0xc92c
+#define GATE_IP_LCD		0xc934
+#define GATE_IP_FSYS		0xc940
+#define GATE_IP_PERIL		0xc950
+#define GATE_BLOCK		0xc970
+#define APLL_LOCK		0x14000
+#define APLL_CON0		0x14100
+#define SRC_CPU			0x14200
+#define DIV_CPU0		0x14500
+#define DIV_CPU1		0x14504
+
+enum exynos4415_plls {
+	apll, epll, g3d_pll, isp_pll, disp_pll,
+	nr_plls,
+};
+
+static struct samsung_clk_provider *exynos4415_ctx;
+
+/*
+ * Support for CMU save/restore across system suspends
+ */
+#ifdef CONFIG_PM_SLEEP
+static struct samsung_clk_reg_dump *exynos4415_clk_regs;
+
+static unsigned long exynos4415_cmu_clk_regs[] __initdata = {
+	SRC_LEFTBUS,
+	DIV_LEFTBUS,
+	GATE_IP_LEFTBUS,
+	GATE_IP_IMAGE,
+	SRC_RIGHTBUS,
+	DIV_RIGHTBUS,
+	GATE_IP_RIGHTBUS,
+	GATE_IP_PERIR,
+	EPLL_LOCK,
+	G3D_PLL_LOCK,
+	DISP_PLL_LOCK,
+	ISP_PLL_LOCK,
+	EPLL_CON0,
+	EPLL_CON1,
+	EPLL_CON2,
+	G3D_PLL_CON0,
+	G3D_PLL_CON1,
+	G3D_PLL_CON2,
+	ISP_PLL_CON0,
+	ISP_PLL_CON1,
+	ISP_PLL_CON2,
+	DISP_PLL_CON0,
+	DISP_PLL_CON1,
+	DISP_PLL_CON2,
+	SRC_TOP0,
+	SRC_TOP1,
+	SRC_CAM,
+	SRC_TV,
+	SRC_MFC,
+	SRC_G3D,
+	SRC_LCD,
+	SRC_ISP,
+	SRC_MAUDIO,
+	SRC_FSYS,
+	SRC_PERIL0,
+	SRC_PERIL1,
+	SRC_CAM1,
+	SRC_TOP_ISP0,
+	SRC_TOP_ISP1,
+	SRC_MASK_TOP,
+	SRC_MASK_CAM,
+	SRC_MASK_TV,
+	SRC_MASK_LCD,
+	SRC_MASK_ISP,
+	SRC_MASK_MAUDIO,
+	SRC_MASK_FSYS,
+	SRC_MASK_PERIL0,
+	SRC_MASK_PERIL1,
+	DIV_TOP,
+	DIV_CAM,
+	DIV_TV,
+	DIV_MFC,
+	DIV_G3D,
+	DIV_LCD,
+	DIV_ISP,
+	DIV_MAUDIO,
+	DIV_FSYS0,
+	DIV_FSYS1,
+	DIV_FSYS2,
+	DIV_PERIL0,
+	DIV_PERIL1,
+	DIV_PERIL2,
+	DIV_PERIL3,
+	DIV_PERIL4,
+	DIV_PERIL5,
+	DIV_CAM1,
+	DIV_TOP_ISP1,
+	DIV_TOP_ISP0,
+	CLKDIV2_RATIO,
+	GATE_SCLK_CAM,
+	GATE_SCLK_TV,
+	GATE_SCLK_MFC,
+	GATE_SCLK_G3D,
+	GATE_SCLK_LCD,
+	GATE_SCLK_MAUDIO,
+	GATE_SCLK_FSYS,
+	GATE_SCLK_PERIL,
+	GATE_IP_CAM,
+	GATE_IP_TV,
+	GATE_IP_MFC,
+	GATE_IP_G3D,
+	GATE_IP_LCD,
+	GATE_IP_FSYS,
+	GATE_IP_PERIL,
+	GATE_BLOCK,
+	APLL_LOCK,
+	APLL_CON0,
+	SRC_CPU,
+	DIV_CPU0,
+	DIV_CPU1,
+};
+
+static int exynos4415_clk_suspend(void)
+{
+	samsung_clk_save(exynos4415_ctx->reg_base, exynos4415_clk_regs,
+				ARRAY_SIZE(exynos4415_cmu_clk_regs));
+
+	return 0;
+}
+
+static void exynos4415_clk_resume(void)
+{
+	samsung_clk_restore(exynos4415_ctx->reg_base, exynos4415_clk_regs,
+				ARRAY_SIZE(exynos4415_cmu_clk_regs));
+}
+
+static struct syscore_ops exynos4415_clk_syscore_ops = {
+	.suspend = exynos4415_clk_suspend,
+	.resume = exynos4415_clk_resume,
+};
+
+static void exynos4415_clk_sleep_init(void)
+{
+	exynos4415_clk_regs =
+		samsung_clk_alloc_reg_dump(exynos4415_cmu_clk_regs,
+					ARRAY_SIZE(exynos4415_cmu_clk_regs));
+	if (!exynos4415_clk_regs) {
+		pr_warn("%s: Failed to allocate sleep save data\n", __func__);
+		return;
+	}
+
+	register_syscore_ops(&exynos4415_clk_syscore_ops);
+}
+#else
+static inline void exynos4415_clk_sleep_init(void) { }
+#endif
+
+/* list of all parent clock list */
+PNAME(mout_g3d_pllsrc_p)	= { "fin_pll", };
+
+PNAME(mout_apll_p)		= { "fin_pll", "fout_apll", };
+PNAME(mout_g3d_pll_p)		= { "fin_pll", "fout_g3d_pll", };
+PNAME(mout_isp_pll_p)		= { "fin_pll", "fout_isp_pll", };
+PNAME(mout_disp_pll_p)		= { "fin_pll", "fout_disp_pll", };
+
+PNAME(mout_mpll_user_p)		= { "fin_pll", "div_mpll_pre", };
+PNAME(mout_epll_p)		= { "fin_pll", "fout_epll", };
+PNAME(mout_core_p)		= { "mout_apll", "mout_mpll_user_c", };
+PNAME(mout_hpm_p)		= { "mout_apll", "mout_mpll_user_c", };
+
+PNAME(mout_ebi_p)		= { "div_aclk_200", "div_aclk_160", };
+PNAME(mout_ebi_1_p)		= { "mout_ebi", "mout_g3d_pll", };
+
+PNAME(mout_gdl_p)		= { "mout_mpll_user_l", };
+PNAME(mout_gdr_p)		= { "mout_mpll_user_r", };
+
+PNAME(mout_aclk_266_p)		= { "mout_mpll_user_t", "mout_g3d_pll", };
+
+PNAME(group_epll_g3dpll_p)	= { "mout_epll", "mout_g3d_pll" };
+PNAME(group_sclk_p)		= { "xxti", "xusbxti",
+				    "none", "mout_isp_pll",
+				    "none", "none", "div_mpll_pre",
+				    "mout_epll", "mout_g3d_pll", };
+PNAME(group_spdif_p)		= { "mout_audio0", "mout_audio1",
+				    "mout_audio2", "spdif_extclk", };
+PNAME(group_sclk_audio2_p)	= { "audiocdclk2", "none",
+				    "none", "mout_isp_pll",
+				    "mout_disp_pll", "xusbxti",
+				    "div_mpll_pre", "mout_epll",
+				    "mout_g3d_pll", };
+PNAME(group_sclk_audio1_p)	= { "audiocdclk1", "none",
+				    "none", "mout_isp_pll",
+				    "mout_disp_pll", "xusbxti",
+				    "div_mpll_pre", "mout_epll",
+				    "mout_g3d_pll", };
+PNAME(group_sclk_audio0_p)	= { "audiocdclk0", "none",
+				    "none", "mout_isp_pll",
+				    "mout_disp_pll", "xusbxti",
+				    "div_mpll_pre", "mout_epll",
+				    "mout_g3d_pll", };
+PNAME(group_fimc_lclk_p)	= { "xxti", "xusbxti",
+				    "none", "mout_isp_pll",
+				    "none", "mout_disp_pll",
+				    "mout_mpll_user_t", "mout_epll",
+				    "mout_g3d_pll", };
+PNAME(group_sclk_fimd0_p)	= { "xxti", "xusbxti",
+				    "m_bitclkhsdiv4_4l", "mout_isp_pll",
+				    "mout_disp_pll", "sclk_hdmiphy",
+				    "div_mpll_pre", "mout_epll",
+				    "mout_g3d_pll", };
+PNAME(mout_hdmi_p)		= { "sclk_pixel", "sclk_hdmiphy" };
+PNAME(mout_mfc_p)		= { "mout_mfc_0", "mout_mfc_1" };
+PNAME(mout_g3d_p)		= { "mout_g3d_0", "mout_g3d_1" };
+PNAME(mout_jpeg_p)		= { "mout_jpeg_0", "mout_jpeg_1" };
+PNAME(mout_jpeg1_p)		= { "mout_epll", "mout_g3d_pll" };
+PNAME(group_aclk_isp0_300_p)	= { "mout_isp_pll", "div_mpll_pre" };
+PNAME(group_aclk_isp0_400_user_p) = { "fin_pll", "div_aclk_400_mcuisp" };
+PNAME(group_aclk_isp0_300_user_p) = { "fin_pll", "mout_aclk_isp0_300" };
+PNAME(group_aclk_isp1_300_user_p) = { "fin_pll", "mout_aclk_isp1_300" };
+PNAME(group_mout_mpll_user_t_p)	= { "mout_mpll_user_t" };
+
+static struct samsung_fixed_factor_clock exynos4415_fixed_factor_clks[] __initdata = {
+	/* HACK: fin_pll hardcoded to xusbxti until detection is implemented. */
+	FFACTOR(CLK_FIN_PLL, "fin_pll", "xusbxti", 1, 1, 0),
+};
+
+static struct samsung_fixed_rate_clock exynos4415_fixed_rate_clks[] __initdata = {
+	FRATE(CLK_SCLK_HDMIPHY, "sclk_hdmiphy", NULL, CLK_IS_ROOT, 27000000),
+};
+
+static struct samsung_mux_clock exynos4415_mux_clks[] __initdata = {
+	/*
+	 * NOTE: Following table is sorted by register address in ascending
+	 * order and then bitfield shift in descending order, as it is done
+	 * in the User's Manual. When adding new entries, please make sure
+	 * that the order is preserved, to avoid merge conflicts and make
+	 * further work with defined data easier.
+	 */
+
+	/* SRC_LEFTBUS */
+	MUX(CLK_MOUT_MPLL_USER_L, "mout_mpll_user_l", mout_mpll_user_p,
+		SRC_LEFTBUS, 4, 1),
+	MUX(CLK_MOUT_GDL, "mout_gdl", mout_gdl_p, SRC_LEFTBUS, 0, 1),
+
+	/* SRC_RIGHTBUS */
+	MUX(CLK_MOUT_MPLL_USER_R, "mout_mpll_user_r", mout_mpll_user_p,
+		SRC_RIGHTBUS, 4, 1),
+	MUX(CLK_MOUT_GDR, "mout_gdr", mout_gdr_p, SRC_RIGHTBUS, 0, 1),
+
+	/* SRC_TOP0 */
+	MUX(CLK_MOUT_EBI, "mout_ebi", mout_ebi_p, SRC_TOP0, 28, 1),
+	MUX(CLK_MOUT_ACLK_200, "mout_aclk_200", group_mout_mpll_user_t_p,
+		SRC_TOP0, 24, 1),
+	MUX(CLK_MOUT_ACLK_160, "mout_aclk_160", group_mout_mpll_user_t_p,
+		SRC_TOP0, 20, 1),
+	MUX(CLK_MOUT_ACLK_100, "mout_aclk_100", group_mout_mpll_user_t_p,
+		SRC_TOP0, 16, 1),
+	MUX(CLK_MOUT_ACLK_266, "mout_aclk_266", mout_aclk_266_p,
+		SRC_TOP0, 12, 1),
+	MUX(CLK_MOUT_G3D_PLL, "mout_g3d_pll", mout_g3d_pll_p,
+		SRC_TOP0, 8, 1),
+	MUX(CLK_MOUT_EPLL, "mout_epll", mout_epll_p, SRC_TOP0, 4, 1),
+	MUX(CLK_MOUT_EBI_1, "mout_ebi_1", mout_ebi_1_p, SRC_TOP0, 0, 1),
+
+	/* SRC_TOP1 */
+	MUX(CLK_MOUT_ISP_PLL, "mout_isp_pll", mout_isp_pll_p,
+		SRC_TOP1, 28, 1),
+	MUX(CLK_MOUT_DISP_PLL, "mout_disp_pll", mout_disp_pll_p,
+		SRC_TOP1, 16, 1),
+	MUX(CLK_MOUT_MPLL_USER_T, "mout_mpll_user_t", mout_mpll_user_p,
+		SRC_TOP1, 12, 1),
+	MUX(CLK_MOUT_ACLK_400_MCUISP, "mout_aclk_400_mcuisp",
+		group_mout_mpll_user_t_p, SRC_TOP1, 8, 1),
+	MUX(CLK_MOUT_G3D_PLLSRC, "mout_g3d_pllsrc", mout_g3d_pllsrc_p,
+		SRC_TOP1, 0, 1),
+
+	/* SRC_CAM */
+	MUX(CLK_MOUT_CSIS1, "mout_csis1", group_fimc_lclk_p, SRC_CAM, 28, 4),
+	MUX(CLK_MOUT_CSIS0, "mout_csis0", group_fimc_lclk_p, SRC_CAM, 24, 4),
+	MUX(CLK_MOUT_CAM1, "mout_cam1", group_fimc_lclk_p, SRC_CAM, 20, 4),
+	MUX(CLK_MOUT_FIMC3_LCLK, "mout_fimc3_lclk", group_fimc_lclk_p, SRC_CAM,
+		12, 4),
+	MUX(CLK_MOUT_FIMC2_LCLK, "mout_fimc2_lclk", group_fimc_lclk_p, SRC_CAM,
+		8, 4),
+	MUX(CLK_MOUT_FIMC1_LCLK, "mout_fimc1_lclk", group_fimc_lclk_p, SRC_CAM,
+		4, 4),
+	MUX(CLK_MOUT_FIMC0_LCLK, "mout_fimc0_lclk", group_fimc_lclk_p, SRC_CAM,
+		0, 4),
+
+	/* SRC_TV */
+	MUX(CLK_MOUT_HDMI, "mout_hdmi", mout_hdmi_p, SRC_TV, 0, 1),
+
+	/* SRC_MFC */
+	MUX(CLK_MOUT_MFC, "mout_mfc", mout_mfc_p, SRC_MFC, 8, 1),
+	MUX(CLK_MOUT_MFC_1, "mout_mfc_1", group_epll_g3dpll_p, SRC_MFC, 4, 1),
+	MUX(CLK_MOUT_MFC_0, "mout_mfc_0", group_mout_mpll_user_t_p, SRC_MFC, 0,
+		1),
+
+	/* SRC_G3D */
+	MUX(CLK_MOUT_G3D, "mout_g3d", mout_g3d_p, SRC_G3D, 8, 1),
+	MUX(CLK_MOUT_G3D_1, "mout_g3d_1", group_epll_g3dpll_p, SRC_G3D, 4, 1),
+	MUX(CLK_MOUT_G3D_0, "mout_g3d_0", group_mout_mpll_user_t_p, SRC_G3D, 0,
+		1),
+
+	/* SRC_LCD */
+	MUX(CLK_MOUT_MIPI0, "mout_mipi0", group_fimc_lclk_p, SRC_LCD, 12, 4),
+	MUX(CLK_MOUT_FIMD0, "mout_fimd0", group_sclk_fimd0_p, SRC_LCD, 0, 4),
+
+	/* SRC_ISP */
+	MUX(CLK_MOUT_TSADC_ISP, "mout_tsadc_isp", group_fimc_lclk_p, SRC_ISP,
+		16, 4),
+	MUX(CLK_MOUT_UART_ISP, "mout_uart_isp", group_fimc_lclk_p, SRC_ISP,
+		12, 4),
+	MUX(CLK_MOUT_SPI1_ISP, "mout_spi1_isp", group_fimc_lclk_p, SRC_ISP,
+		8, 4),
+	MUX(CLK_MOUT_SPI0_ISP, "mout_spi0_isp", group_fimc_lclk_p, SRC_ISP,
+		4, 4),
+	MUX(CLK_MOUT_PWM_ISP, "mout_pwm_isp", group_fimc_lclk_p, SRC_ISP,
+		0, 4),
+
+	/* SRC_MAUDIO */
+	MUX(CLK_MOUT_AUDIO0, "mout_audio0", group_sclk_audio0_p, SRC_MAUDIO,
+		0, 4),
+
+	/* SRC_FSYS */
+	MUX(CLK_MOUT_TSADC, "mout_tsadc", group_sclk_p, SRC_FSYS, 28, 4),
+	MUX(CLK_MOUT_MMC2, "mout_mmc2", group_sclk_p, SRC_FSYS, 8, 4),
+	MUX(CLK_MOUT_MMC1, "mout_mmc1", group_sclk_p, SRC_FSYS, 4, 4),
+	MUX(CLK_MOUT_MMC0, "mout_mmc0", group_sclk_p, SRC_FSYS, 0, 4),
+
+	/* SRC_PERIL0 */
+	MUX(CLK_MOUT_UART3, "mout_uart3", group_sclk_p, SRC_PERIL0, 12, 4),
+	MUX(CLK_MOUT_UART2, "mout_uart2", group_sclk_p, SRC_PERIL0, 8, 4),
+	MUX(CLK_MOUT_UART1, "mout_uart1", group_sclk_p, SRC_PERIL0, 4, 4),
+	MUX(CLK_MOUT_UART0, "mout_uart0", group_sclk_p, SRC_PERIL0, 0, 4),
+
+	/* SRC_PERIL1 */
+	MUX(CLK_MOUT_SPI2, "mout_spi2", group_sclk_p, SRC_PERIL1, 24, 4),
+	MUX(CLK_MOUT_SPI1, "mout_spi1", group_sclk_p, SRC_PERIL1, 20, 4),
+	MUX(CLK_MOUT_SPI0, "mout_spi0", group_sclk_p, SRC_PERIL1, 16, 4),
+	MUX(CLK_MOUT_SPDIF, "mout_spdif", group_spdif_p, SRC_PERIL1, 8, 4),
+	MUX(CLK_MOUT_AUDIO2, "mout_audio2", group_sclk_audio2_p, SRC_PERIL1,
+		4, 4),
+	MUX(CLK_MOUT_AUDIO1, "mout_audio1", group_sclk_audio1_p, SRC_PERIL1,
+		0, 4),
+
+	/* SRC_CPU */
+	MUX(CLK_MOUT_MPLL_USER_C, "mout_mpll_user_c", mout_mpll_user_p,
+		SRC_CPU, 24, 1),
+	MUX(CLK_MOUT_HPM, "mout_hpm", mout_hpm_p, SRC_CPU, 20, 1),
+	MUX_F(CLK_MOUT_CORE, "mout_core", mout_core_p, SRC_CPU, 16, 1, 0,
+		CLK_MUX_READ_ONLY),
+	MUX_F(CLK_MOUT_APLL, "mout_apll", mout_apll_p, SRC_CPU, 0, 1,
+		CLK_SET_RATE_PARENT, 0),
+
+	/* SRC_CAM1 */
+	MUX(CLK_MOUT_PXLASYNC_CSIS1_FIMC, "mout_pxlasync_csis1",
+		group_fimc_lclk_p, SRC_CAM1, 20, 1),
+	MUX(CLK_MOUT_PXLASYNC_CSIS0_FIMC, "mout_pxlasync_csis0",
+		group_fimc_lclk_p, SRC_CAM1, 16, 1),
+	MUX(CLK_MOUT_JPEG, "mout_jpeg", mout_jpeg_p, SRC_CAM1, 8, 1),
+	MUX(CLK_MOUT_JPEG1, "mout_jpeg_1", mout_jpeg1_p, SRC_CAM1, 4, 1),
+	MUX(CLK_MOUT_JPEG0, "mout_jpeg_0", group_mout_mpll_user_t_p, SRC_CAM1,
+		0, 1),
+
+	/* SRC_TOP_ISP0 */
+	MUX(CLK_MOUT_ACLK_ISP0_300, "mout_aclk_isp0_300",
+		group_aclk_isp0_300_p, SRC_TOP_ISP0, 8, 1),
+	MUX(CLK_MOUT_ACLK_ISP0_400, "mout_aclk_isp0_400_user",
+		group_aclk_isp0_400_user_p, SRC_TOP_ISP0, 4, 1),
+	MUX(CLK_MOUT_ACLK_ISP0_300_USER, "mout_aclk_isp0_300_user",
+		group_aclk_isp0_300_user_p, SRC_TOP_ISP0, 0, 1),
+
+	/* SRC_TOP_ISP1 */
+	MUX(CLK_MOUT_ACLK_ISP1_300, "mout_aclk_isp1_300",
+		group_aclk_isp0_300_p, SRC_TOP_ISP1, 4, 1),
+	MUX(CLK_MOUT_ACLK_ISP1_300_USER, "mout_aclk_isp1_300_user",
+		group_aclk_isp1_300_user_p, SRC_TOP_ISP1, 0, 1),
+};
+
+static struct samsung_div_clock exynos4415_div_clks[] __initdata = {
+	/*
+	 * NOTE: Following table is sorted by register address in ascending
+	 * order and then bitfield shift in descending order, as it is done
+	 * in the User's Manual. When adding new entries, please make sure
+	 * that the order is preserved, to avoid merge conflicts and make
+	 * further work with defined data easier.
+	 */
+
+	/* DIV_LEFTBUS */
+	DIV(CLK_DIV_GPL, "div_gpl", "div_gdl", DIV_LEFTBUS, 4, 3),
+	DIV(CLK_DIV_GDL, "div_gdl", "mout_gdl", DIV_LEFTBUS, 0, 4),
+
+	/* DIV_RIGHTBUS */
+	DIV(CLK_DIV_GPR, "div_gpr", "div_gdr", DIV_RIGHTBUS, 4, 3),
+	DIV(CLK_DIV_GDR, "div_gdr", "mout_gdr", DIV_RIGHTBUS, 0, 4),
+
+	/* DIV_TOP */
+	DIV(CLK_DIV_ACLK_400_MCUISP, "div_aclk_400_mcuisp",
+		"mout_aclk_400_mcuisp", DIV_TOP, 24, 3),
+	DIV(CLK_DIV_EBI, "div_ebi", "mout_ebi_1", DIV_TOP, 16, 3),
+	DIV(CLK_DIV_ACLK_200, "div_aclk_200", "mout_aclk_200", DIV_TOP, 12, 3),
+	DIV(CLK_DIV_ACLK_160, "div_aclk_160", "mout_aclk_160", DIV_TOP, 8, 3),
+	DIV(CLK_DIV_ACLK_100, "div_aclk_100", "mout_aclk_100", DIV_TOP, 4, 4),
+	DIV(CLK_DIV_ACLK_266, "div_aclk_266", "mout_aclk_266", DIV_TOP, 0, 3),
+
+	/* DIV_CAM */
+	DIV(CLK_DIV_CSIS1, "div_csis1", "mout_csis1", DIV_CAM, 28, 4),
+	DIV(CLK_DIV_CSIS0, "div_csis0", "mout_csis0", DIV_CAM, 24, 4),
+	DIV(CLK_DIV_CAM1, "div_cam1", "mout_cam1", DIV_CAM, 20, 4),
+	DIV(CLK_DIV_FIMC3_LCLK, "div_fimc3_lclk", "mout_fimc3_lclk", DIV_CAM,
+		12, 4),
+	DIV(CLK_DIV_FIMC2_LCLK, "div_fimc2_lclk", "mout_fimc2_lclk", DIV_CAM,
+		8, 4),
+	DIV(CLK_DIV_FIMC1_LCLK, "div_fimc1_lclk", "mout_fimc1_lclk", DIV_CAM,
+		4, 4),
+	DIV(CLK_DIV_FIMC0_LCLK, "div_fimc0_lclk", "mout_fimc0_lclk", DIV_CAM,
+		0, 4),
+
+	/* DIV_TV */
+	DIV(CLK_DIV_TV_BLK, "div_tv_blk", "mout_g3d_pll", DIV_TV, 0, 4),
+
+	/* DIV_MFC */
+	DIV(CLK_DIV_MFC, "div_mfc", "mout_mfc", DIV_MFC, 0, 4),
+
+	/* DIV_G3D */
+	DIV(CLK_DIV_G3D, "div_g3d", "mout_g3d", DIV_G3D, 0, 4),
+
+	/* DIV_LCD */
+	DIV_F(CLK_DIV_MIPI0_PRE, "div_mipi0_pre", "div_mipi0", DIV_LCD, 20, 4,
+		CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_MIPI0, "div_mipi0", "mout_mipi0", DIV_LCD, 16, 4),
+	DIV(CLK_DIV_FIMD0, "div_fimd0", "mout_fimd0", DIV_LCD, 0, 4),
+
+	/* DIV_ISP */
+	DIV(CLK_DIV_UART_ISP, "div_uart_isp", "mout_uart_isp", DIV_ISP, 28, 4),
+	DIV_F(CLK_DIV_SPI1_ISP_PRE, "div_spi1_isp_pre", "div_spi1_isp",
+		DIV_ISP, 20, 8, CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_SPI1_ISP, "div_spi1_isp", "mout_spi1_isp", DIV_ISP, 16, 4),
+	DIV_F(CLK_DIV_SPI0_ISP_PRE, "div_spi0_isp_pre", "div_spi0_isp",
+		DIV_ISP, 8, 8, CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_SPI0_ISP, "div_spi0_isp", "mout_spi0_isp", DIV_ISP, 4, 4),
+	DIV(CLK_DIV_PWM_ISP, "div_pwm_isp", "mout_pwm_isp", DIV_ISP, 0, 4),
+
+	/* DIV_MAUDIO */
+	DIV(CLK_DIV_PCM0, "div_pcm0", "div_audio0", DIV_MAUDIO, 4, 8),
+	DIV(CLK_DIV_AUDIO0, "div_audio0", "mout_audio0", DIV_MAUDIO, 0, 4),
+
+	/* DIV_FSYS0 */
+	DIV_F(CLK_DIV_TSADC_PRE, "div_tsadc_pre", "div_tsadc", DIV_FSYS0, 8, 8,
+		CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_TSADC, "div_tsadc", "mout_tsadc", DIV_FSYS0, 0, 4),
+
+	/* DIV_FSYS1 */
+	DIV_F(CLK_DIV_MMC1_PRE, "div_mmc1_pre", "div_mmc1", DIV_FSYS1, 24, 8,
+		CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_MMC1, "div_mmc1", "mout_mmc1", DIV_FSYS1, 16, 4),
+	DIV_F(CLK_DIV_MMC0_PRE, "div_mmc0_pre", "div_mmc0", DIV_FSYS1, 8, 8,
+		CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_MMC0, "div_mmc0", "mout_mmc0", DIV_FSYS1, 0, 4),
+
+	/* DIV_FSYS2 */
+	DIV_F(CLK_DIV_MMC2_PRE, "div_mmc2_pre", "div_mmc2", DIV_FSYS2, 8, 8,
+		CLK_SET_RATE_PARENT, 0),
+	DIV_F(CLK_DIV_MMC2_PRE, "div_mmc2", "mout_mmc2", DIV_FSYS2, 0, 4,
+		CLK_SET_RATE_PARENT, 0),
+
+	/* DIV_PERIL0 */
+	DIV(CLK_DIV_UART3, "div_uart3", "mout_uart3", DIV_PERIL0, 12, 4),
+	DIV(CLK_DIV_UART2, "div_uart2", "mout_uart2", DIV_PERIL0, 8, 4),
+	DIV(CLK_DIV_UART1, "div_uart1", "mout_uart1", DIV_PERIL0, 4, 4),
+	DIV(CLK_DIV_UART0, "div_uart0", "mout_uart0", DIV_PERIL0, 0, 4),
+
+	/* DIV_PERIL1 */
+	DIV_F(CLK_DIV_SPI1_PRE, "div_spi1_pre", "div_spi1", DIV_PERIL1, 24, 8,
+		CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_SPI1, "div_spi1", "mout_spi1", DIV_PERIL1, 16, 4),
+	DIV_F(CLK_DIV_SPI0_PRE, "div_spi0_pre", "div_spi0", DIV_PERIL1, 8, 8,
+		CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_SPI0, "div_spi0", "mout_spi0", DIV_PERIL1, 0, 4),
+
+	/* DIV_PERIL2 */
+	DIV_F(CLK_DIV_SPI2_PRE, "div_spi2_pre", "div_spi2", DIV_PERIL2, 8, 8,
+		CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_SPI2, "div_spi2", "mout_spi2", DIV_PERIL2, 0, 4),
+
+	/* DIV_PERIL4 */
+	DIV(CLK_DIV_PCM2, "div_pcm2", "div_audio2", DIV_PERIL4, 20, 8),
+	DIV(CLK_DIV_AUDIO2, "div_audio2", "mout_audio2", DIV_PERIL4, 16, 4),
+	DIV(CLK_DIV_PCM1, "div_pcm1", "div_audio1", DIV_PERIL4, 20, 8),
+	DIV(CLK_DIV_AUDIO1, "div_audio1", "mout_audio1", DIV_PERIL4, 0, 4),
+
+	/* DIV_PERIL5 */
+	DIV(CLK_DIV_I2S1, "div_i2s1", "div_audio1", DIV_PERIL5, 0, 6),
+
+	/* DIV_CAM1 */
+	DIV(CLK_DIV_PXLASYNC_CSIS1_FIMC, "div_pxlasync_csis1_fimc",
+		"mout_pxlasync_csis1", DIV_CAM1, 24, 4),
+	DIV(CLK_DIV_PXLASYNC_CSIS0_FIMC, "div_pxlasync_csis0_fimc",
+		"mout_pxlasync_csis0", DIV_CAM1, 20, 4),
+	DIV(CLK_DIV_JPEG, "div_jpeg", "mout_jpeg", DIV_CAM1, 0, 4),
+
+	/* DIV_CPU0 */
+	DIV(CLK_DIV_CORE2, "div_core2", "div_core", DIV_CPU0, 28, 3),
+	DIV_F(CLK_DIV_APLL, "div_apll", "mout_apll", DIV_CPU0, 24, 3,
+			CLK_GET_RATE_NOCACHE, CLK_DIVIDER_READ_ONLY),
+	DIV(CLK_DIV_PCLK_DBG, "div_pclk_dbg", "div_core2", DIV_CPU0, 20, 3),
+	DIV(CLK_DIV_ATB, "div_atb", "div_core2", DIV_CPU0, 16, 3),
+	DIV(CLK_DIV_PERIPH, "div_periph", "div_core2", DIV_CPU0, 12, 3),
+	DIV(CLK_DIV_COREM1, "div_corem1", "div_core2", DIV_CPU0, 8, 3),
+	DIV(CLK_DIV_COREM0, "div_corem0", "div_core2", DIV_CPU0, 4, 3),
+	DIV_F(CLK_DIV_CORE, "div_core", "mout_core", DIV_CPU0, 0, 3,
+		CLK_GET_RATE_NOCACHE, CLK_DIVIDER_READ_ONLY),
+
+	/* DIV_CPU1 */
+	DIV(CLK_DIV_HPM, "div_hpm", "div_copy", DIV_CPU1, 4, 3),
+	DIV(CLK_DIV_COPY, "div_copy", "mout_hpm", DIV_CPU1, 0, 3),
+};
+
+static struct samsung_gate_clock exynos4415_gate_clks[] __initdata = {
+	/*
+	 * NOTE: Following table is sorted by register address in ascending
+	 * order and then bitfield shift in descending order, as it is done
+	 * in the User's Manual. When adding new entries, please make sure
+	 * that the order is preserved, to avoid merge conflicts and make
+	 * further work with defined data easier.
+	 */
+
+	/* GATE_IP_LEFTBUS */
+	GATE(CLK_ASYNC_G3D, "async_g3d", "div_aclk_100", GATE_IP_LEFTBUS, 6,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_ASYNC_MFCL, "async_mfcl", "div_aclk_100", GATE_IP_LEFTBUS, 4,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_ASYNC_TVX, "async_tvx", "div_aclk_100", GATE_IP_LEFTBUS, 3,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PPMULEFT, "ppmuleft", "div_aclk_100", GATE_IP_LEFTBUS, 1,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_GPIO_LEFT, "gpio_left", "div_aclk_100", GATE_IP_LEFTBUS, 0,
+		CLK_IGNORE_UNUSED, 0),
+
+	/* GATE_IP_IMAGE */
+	GATE(CLK_PPMUIMAGE, "ppmuimage", "div_aclk_100", GATE_IP_IMAGE,
+		9, 0, 0),
+	GATE(CLK_QEMDMA2, "qe_mdma2", "div_aclk_100", GATE_IP_IMAGE,
+		8, 0, 0),
+	GATE(CLK_QEROTATOR, "qe_rotator", "div_aclk_100", GATE_IP_IMAGE,
+		7, 0, 0),
+	GATE(CLK_SMMUMDMA2, "smmu_mdam2", "div_aclk_100", GATE_IP_IMAGE,
+		5, 0, 0),
+	GATE(CLK_SMMUROTATOR, "smmu_rotator", "div_aclk_100", GATE_IP_IMAGE,
+		4, 0, 0),
+	GATE(CLK_MDMA2, "mdma2", "div_aclk_100", GATE_IP_IMAGE, 2, 0, 0),
+	GATE(CLK_ROTATOR, "rotator", "div_aclk_100", GATE_IP_IMAGE, 1, 0, 0),
+
+	/* GATE_IP_RIGHTBUS */
+	GATE(CLK_ASYNC_ISPMX, "async_ispmx", "div_aclk_100",
+		GATE_IP_RIGHTBUS, 9, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_ASYNC_MAUDIOX, "async_maudiox", "div_aclk_100",
+		GATE_IP_RIGHTBUS, 7, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_ASYNC_MFCR, "async_mfcr", "div_aclk_100",
+		GATE_IP_RIGHTBUS, 6, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_ASYNC_FSYSD, "async_fsysd", "div_aclk_100",
+		GATE_IP_RIGHTBUS, 5, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_ASYNC_LCD0X, "async_lcd0x", "div_aclk_100",
+		GATE_IP_RIGHTBUS, 3, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_ASYNC_CAMX, "async_camx", "div_aclk_100",
+		GATE_IP_RIGHTBUS, 2, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PPMURIGHT, "ppmuright", "div_aclk_100",
+		GATE_IP_RIGHTBUS, 1, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_GPIO_RIGHT, "gpio_right", "div_aclk_100",
+		GATE_IP_RIGHTBUS, 0, CLK_IGNORE_UNUSED, 0),
+
+	/* GATE_IP_PERIR */
+	GATE(CLK_ANTIRBK_APBIF, "antirbk_apbif", "div_aclk_100",
+		GATE_IP_PERIR, 24, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_EFUSE_WRITER_APBIF, "efuse_writer_apbif", "div_aclk_100",
+		GATE_IP_PERIR, 23, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_MONOCNT, "monocnt", "div_aclk_100", GATE_IP_PERIR, 22,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC6, "tzpc6", "div_aclk_100", GATE_IP_PERIR, 21,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PROVISIONKEY1, "provisionkey1", "div_aclk_100",
+		GATE_IP_PERIR, 20, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PROVISIONKEY0, "provisionkey0", "div_aclk_100",
+		GATE_IP_PERIR, 19, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_CMU_ISPPART, "cmu_isppart", "div_aclk_100", GATE_IP_PERIR, 18,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TMU_APBIF, "tmu_apbif", "div_aclk_100",
+		GATE_IP_PERIR, 17, 0, 0),
+	GATE(CLK_KEYIF, "keyif", "div_aclk_100", GATE_IP_PERIR, 16, 0, 0),
+	GATE(CLK_RTC, "rtc", "div_aclk_100", GATE_IP_PERIR, 15, 0, 0),
+	GATE(CLK_WDT, "wdt", "div_aclk_100", GATE_IP_PERIR, 14, 0, 0),
+	GATE(CLK_MCT, "mct", "div_aclk_100", GATE_IP_PERIR, 13, 0, 0),
+	GATE(CLK_SECKEY, "seckey", "div_aclk_100", GATE_IP_PERIR, 12,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_HDMI_CEC, "hdmi_cec", "div_aclk_100", GATE_IP_PERIR, 11,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC5, "tzpc5", "div_aclk_100", GATE_IP_PERIR, 10,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC4, "tzpc4", "div_aclk_100", GATE_IP_PERIR, 9,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC3, "tzpc3", "div_aclk_100", GATE_IP_PERIR, 8,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC2, "tzpc2", "div_aclk_100", GATE_IP_PERIR, 7,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC1, "tzpc1", "div_aclk_100", GATE_IP_PERIR, 6,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC0, "tzpc0", "div_aclk_100", GATE_IP_PERIR, 5,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_CMU_COREPART, "cmu_corepart", "div_aclk_100", GATE_IP_PERIR, 4,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_CMU_TOPPART, "cmu_toppart", "div_aclk_100", GATE_IP_PERIR, 3,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PMU_APBIF, "pmu_apbif", "div_aclk_100", GATE_IP_PERIR, 2,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_SYSREG, "sysreg", "div_aclk_100", GATE_IP_PERIR, 1,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_CHIP_ID, "chip_id", "div_aclk_100", GATE_IP_PERIR, 0,
+		CLK_IGNORE_UNUSED, 0),
+
+	/* GATE_SCLK_CAM - non-completed */
+	GATE(CLK_SCLK_PXLAYSNC_CSIS1_FIMC, "sclk_pxlasync_csis1_fimc",
+		"div_pxlasync_csis1_fimc", GATE_SCLK_CAM, 11,
+		CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_PXLAYSNC_CSIS0_FIMC, "sclk_pxlasync_csis0_fimc",
+		"div_pxlasync_csis0_fimc", GATE_SCLK_CAM,
+		10, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_JPEG, "sclk_jpeg", "div_jpeg",
+		GATE_SCLK_CAM, 8, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_CSIS1, "sclk_csis1", "div_csis1",
+		GATE_SCLK_CAM, 7, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_CSIS0, "sclk_csis0", "div_csis0",
+		GATE_SCLK_CAM, 6, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_CAM1, "sclk_cam1", "div_cam1",
+		GATE_SCLK_CAM, 5, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_FIMC3_LCLK, "sclk_fimc3_lclk", "div_fimc3_lclk",
+		GATE_SCLK_CAM, 3, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_FIMC2_LCLK, "sclk_fimc2_lclk", "div_fimc2_lclk",
+		GATE_SCLK_CAM, 2, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_FIMC1_LCLK, "sclk_fimc1_lclk", "div_fimc1_lclk",
+		GATE_SCLK_CAM, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_FIMC0_LCLK, "sclk_fimc0_lclk", "div_fimc0_lclk",
+		GATE_SCLK_CAM, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_SCLK_TV */
+	GATE(CLK_SCLK_PIXEL, "sclk_pixel", "div_tv_blk",
+		GATE_SCLK_TV, 3, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_HDMI, "sclk_hdmi", "mout_hdmi",
+		GATE_SCLK_TV, 2, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_MIXER, "sclk_mixer", "div_tv_blk",
+		GATE_SCLK_TV, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_SCLK_MFC */
+	GATE(CLK_SCLK_MFC, "sclk_mfc", "div_mfc",
+		GATE_SCLK_MFC, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_SCLK_G3D */
+	GATE(CLK_SCLK_G3D, "sclk_g3d", "div_g3d",
+		GATE_SCLK_G3D, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_SCLK_LCD */
+	GATE(CLK_SCLK_MIPIDPHY4L, "sclk_mipidphy4l", "div_mipi0",
+		GATE_SCLK_LCD, 4, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_MIPI0, "sclk_mipi0", "div_mipi0_pre",
+		GATE_SCLK_LCD, 3, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_MDNIE0, "sclk_mdnie0", "div_fimd0",
+		GATE_SCLK_LCD, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_FIMD0, "sclk_fimd0", "div_fimd0",
+		GATE_SCLK_LCD, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_SCLK_MAUDIO */
+	GATE(CLK_SCLK_PCM0, "sclk_pcm0", "div_pcm0",
+		GATE_SCLK_MAUDIO, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_AUDIO0, "sclk_audio0", "div_audio0",
+		GATE_SCLK_MAUDIO, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_SCLK_FSYS */
+	GATE(CLK_SCLK_TSADC, "sclk_tsadc", "div_tsadc_pre",
+		GATE_SCLK_FSYS, 9, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_EBI, "sclk_ebi", "div_ebi",
+		GATE_SCLK_FSYS, 6, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_MMC2, "sclk_mmc2", "div_mmc2_pre",
+		GATE_SCLK_FSYS, 2, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_MMC1, "sclk_mmc1", "div_mmc1_pre",
+		GATE_SCLK_FSYS, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_MMC0, "sclk_mmc0", "div_mmc0_pre",
+		GATE_SCLK_FSYS, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_SCLK_PERIL */
+	GATE(CLK_SCLK_I2S, "sclk_i2s1", "div_i2s1",
+		GATE_SCLK_PERIL, 18, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_PCM2, "sclk_pcm2", "div_pcm2",
+		GATE_SCLK_PERIL, 16, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_PCM1, "sclk_pcm1", "div_pcm1",
+		GATE_SCLK_PERIL, 15, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_AUDIO2, "sclk_audio2", "div_audio2",
+		GATE_SCLK_PERIL, 14, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_AUDIO1, "sclk_audio1", "div_audio1",
+		GATE_SCLK_PERIL, 13, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_SPDIF, "sclk_spdif", "mout_spdif",
+		GATE_SCLK_PERIL, 10, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_SPI2, "sclk_spi2", "div_spi2_pre",
+		GATE_SCLK_PERIL, 8, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_SPI1, "sclk_spi1", "div_spi1_pre",
+		GATE_SCLK_PERIL, 7, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_SPI0, "sclk_spi0", "div_spi0_pre",
+		GATE_SCLK_PERIL, 6, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_UART3, "sclk_uart3", "div_uart3",
+		GATE_SCLK_PERIL, 3, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_UART2, "sclk_uart2", "div_uart2",
+		GATE_SCLK_PERIL, 2, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_UART1, "sclk_uart1", "div_uart1",
+		GATE_SCLK_PERIL, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_UART0, "sclk_uart0", "div_uart0",
+		GATE_SCLK_PERIL, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_IP_CAM */
+	GATE(CLK_SMMUFIMC_LITE2, "smmufimc_lite2", "div_aclk_160", GATE_IP_CAM,
+		22, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_FIMC_LITE2, "fimc_lite2", "div_aclk_160", GATE_IP_CAM,
+		20, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PIXELASYNCM1, "pixelasyncm1", "div_aclk_160", GATE_IP_CAM,
+		18, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PIXELASYNCM0, "pixelasyncm0", "div_aclk_160", GATE_IP_CAM,
+		17, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PPMUCAMIF, "ppmucamif", "div_aclk_160", GATE_IP_CAM,
+		16, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_SMMUJPEG, "smmujpeg", "div_aclk_160", GATE_IP_CAM, 11, 0, 0),
+	GATE(CLK_SMMUFIMC3, "smmufimc3", "div_aclk_160", GATE_IP_CAM, 10, 0, 0),
+	GATE(CLK_SMMUFIMC2, "smmufimc2", "div_aclk_160", GATE_IP_CAM, 9, 0, 0),
+	GATE(CLK_SMMUFIMC1, "smmufimc1", "div_aclk_160", GATE_IP_CAM, 8, 0, 0),
+	GATE(CLK_SMMUFIMC0, "smmufimc0", "div_aclk_160", GATE_IP_CAM, 7, 0, 0),
+	GATE(CLK_JPEG, "jpeg", "div_aclk_160", GATE_IP_CAM, 6, 0, 0),
+	GATE(CLK_CSIS1, "csis1", "div_aclk_160", GATE_IP_CAM, 5, 0, 0),
+	GATE(CLK_CSIS0, "csis0", "div_aclk_160", GATE_IP_CAM, 4, 0, 0),
+	GATE(CLK_FIMC3, "fimc3", "div_aclk_160", GATE_IP_CAM, 3, 0, 0),
+	GATE(CLK_FIMC2, "fimc2", "div_aclk_160", GATE_IP_CAM, 2, 0, 0),
+	GATE(CLK_FIMC1, "fimc1", "div_aclk_160", GATE_IP_CAM, 1, 0, 0),
+	GATE(CLK_FIMC0, "fimc0", "div_aclk_160", GATE_IP_CAM, 0, 0, 0),
+
+	/* GATE_IP_TV */
+	GATE(CLK_PPMUTV, "ppmutv", "div_aclk_100", GATE_IP_TV, 5, 0, 0),
+	GATE(CLK_SMMUTV, "smmutv", "div_aclk_100", GATE_IP_TV, 4, 0, 0),
+	GATE(CLK_HDMI, "hdmi", "div_aclk_100", GATE_IP_TV, 3, 0, 0),
+	GATE(CLK_MIXER, "mixer", "div_aclk_100", GATE_IP_TV, 1, 0, 0),
+	GATE(CLK_VP, "vp", "div_aclk_100", GATE_IP_TV, 0, 0, 0),
+
+	/* GATE_IP_MFC */
+	GATE(CLK_PPMUMFC_R, "ppmumfc_r", "div_aclk_200", GATE_IP_MFC, 4,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PPMUMFC_L, "ppmumfc_l", "div_aclk_200", GATE_IP_MFC, 3,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_SMMUMFC_R, "smmumfc_r", "div_aclk_200", GATE_IP_MFC, 2, 0, 0),
+	GATE(CLK_SMMUMFC_L, "smmumfc_l", "div_aclk_200", GATE_IP_MFC, 1, 0, 0),
+	GATE(CLK_MFC, "mfc", "div_aclk_200", GATE_IP_MFC, 0, 0, 0),
+
+	/* GATE_IP_G3D */
+	GATE(CLK_PPMUG3D, "ppmug3d", "div_aclk_200", GATE_IP_G3D, 1,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_G3D, "g3d", "div_aclk_200", GATE_IP_G3D, 0, 0, 0),
+
+	/* GATE_IP_LCD */
+	GATE(CLK_PPMULCD0, "ppmulcd0", "div_aclk_160", GATE_IP_LCD, 5,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_SMMUFIMD0, "smmufimd0", "div_aclk_160", GATE_IP_LCD, 4, 0, 0),
+	GATE(CLK_DSIM0, "dsim0", "div_aclk_160", GATE_IP_LCD, 3, 0, 0),
+	GATE(CLK_SMIES, "smies", "div_aclk_160", GATE_IP_LCD, 2, 0, 0),
+	GATE(CLK_MIE0, "mie0", "div_aclk_160", GATE_IP_LCD, 1, 0, 0),
+	GATE(CLK_FIMD0, "fimd0", "div_aclk_160", GATE_IP_LCD, 0, 0, 0),
+
+	/* GATE_IP_FSYS */
+	GATE(CLK_TSADC, "tsadc", "div_aclk_200", GATE_IP_FSYS, 20, 0, 0),
+	GATE(CLK_PPMUFILE, "ppmufile", "div_aclk_200", GATE_IP_FSYS, 17,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_NFCON, "nfcon", "div_aclk_200", GATE_IP_FSYS, 16, 0, 0),
+	GATE(CLK_USBDEVICE, "usbdevice", "div_aclk_200", GATE_IP_FSYS, 13,
+		0, 0),
+	GATE(CLK_USBHOST, "usbhost", "div_aclk_200", GATE_IP_FSYS, 12, 0, 0),
+	GATE(CLK_SROMC, "sromc", "div_aclk_200", GATE_IP_FSYS, 11, 0, 0),
+	GATE(CLK_SDMMC2, "sdmmc2", "div_aclk_200", GATE_IP_FSYS, 7, 0, 0),
+	GATE(CLK_SDMMC1, "sdmmc1", "div_aclk_200", GATE_IP_FSYS, 6, 0, 0),
+	GATE(CLK_SDMMC0, "sdmmc0", "div_aclk_200", GATE_IP_FSYS, 5, 0, 0),
+	GATE(CLK_PDMA1, "pdma1", "div_aclk_200", GATE_IP_FSYS, 1, 0, 0),
+	GATE(CLK_PDMA0, "pdma0", "div_aclk_200", GATE_IP_FSYS, 0, 0, 0),
+
+	/* GATE_IP_PERIL */
+	GATE(CLK_SPDIF, "spdif", "div_aclk_100", GATE_IP_PERIL, 26, 0, 0),
+	GATE(CLK_PWM, "pwm", "div_aclk_100", GATE_IP_PERIL, 24, 0, 0),
+	GATE(CLK_PCM2, "pcm2", "div_aclk_100", GATE_IP_PERIL, 23, 0, 0),
+	GATE(CLK_PCM1, "pcm1", "div_aclk_100", GATE_IP_PERIL, 22, 0, 0),
+	GATE(CLK_I2S1, "i2s1", "div_aclk_100", GATE_IP_PERIL, 20, 0, 0),
+	GATE(CLK_SPI2, "spi2", "div_aclk_100", GATE_IP_PERIL, 18, 0, 0),
+	GATE(CLK_SPI1, "spi1", "div_aclk_100", GATE_IP_PERIL, 17, 0, 0),
+	GATE(CLK_SPI0, "spi0", "div_aclk_100", GATE_IP_PERIL, 16, 0, 0),
+	GATE(CLK_I2CHDMI, "i2chdmi", "div_aclk_100", GATE_IP_PERIL, 14, 0, 0),
+	GATE(CLK_I2C7, "i2c7", "div_aclk_100", GATE_IP_PERIL, 13, 0, 0),
+	GATE(CLK_I2C6, "i2c6", "div_aclk_100", GATE_IP_PERIL, 12, 0, 0),
+	GATE(CLK_I2C5, "i2c5", "div_aclk_100", GATE_IP_PERIL, 11, 0, 0),
+	GATE(CLK_I2C4, "i2c4", "div_aclk_100", GATE_IP_PERIL, 10, 0, 0),
+	GATE(CLK_I2C3, "i2c3", "div_aclk_100", GATE_IP_PERIL, 9, 0, 0),
+	GATE(CLK_I2C2, "i2c2", "div_aclk_100", GATE_IP_PERIL, 8, 0, 0),
+	GATE(CLK_I2C1, "i2c1", "div_aclk_100", GATE_IP_PERIL, 7, 0, 0),
+	GATE(CLK_I2C0, "i2c0", "div_aclk_100", GATE_IP_PERIL, 6, 0, 0),
+	GATE(CLK_UART3, "uart3", "div_aclk_100", GATE_IP_PERIL, 3, 0, 0),
+	GATE(CLK_UART2, "uart2", "div_aclk_100", GATE_IP_PERIL, 2, 0, 0),
+	GATE(CLK_UART1, "uart1", "div_aclk_100", GATE_IP_PERIL, 1, 0, 0),
+	GATE(CLK_UART0, "uart0", "div_aclk_100", GATE_IP_PERIL, 0, 0, 0),
+};
+
+/*
+ * APLL & MPLL & BPLL & ISP_PLL & DISP_PLL & G3D_PLL
+ */
+static struct samsung_pll_rate_table exynos4415_pll_rates[] = {
+	PLL_35XX_RATE(1600000000, 400, 3,  1),
+	PLL_35XX_RATE(1500000000, 250, 2,  1),
+	PLL_35XX_RATE(1400000000, 175, 3,  0),
+	PLL_35XX_RATE(1300000000, 325, 3,  1),
+	PLL_35XX_RATE(1200000000, 400, 4,  1),
+	PLL_35XX_RATE(1100000000, 275, 3,  1),
+	PLL_35XX_RATE(1066000000, 533, 6,  1),
+	PLL_35XX_RATE(1000000000, 250, 3,  1),
+	PLL_35XX_RATE(960000000,  320, 4,  1),
+	PLL_35XX_RATE(900000000,  300, 4,  1),
+	PLL_35XX_RATE(850000000,  425, 6,  1),
+	PLL_35XX_RATE(800000000,  200, 3,  1),
+	PLL_35XX_RATE(700000000,  175, 3,  1),
+	PLL_35XX_RATE(667000000,  667, 12, 1),
+	PLL_35XX_RATE(600000000,  400, 4,  2),
+	PLL_35XX_RATE(550000000,  275, 3,  2),
+	PLL_35XX_RATE(533000000,  533, 6,  2),
+	PLL_35XX_RATE(520000000,  260, 3,  2),
+	PLL_35XX_RATE(500000000,  250, 3,  2),
+	PLL_35XX_RATE(440000000,  220, 3,  2),
+	PLL_35XX_RATE(400000000,  200, 3,  2),
+	PLL_35XX_RATE(350000000,  175, 3,  2),
+	PLL_35XX_RATE(300000000,  300, 3,  3),
+	PLL_35XX_RATE(266000000,  266, 3,  3),
+	PLL_35XX_RATE(200000000,  200, 3,  3),
+	PLL_35XX_RATE(160000000,  160, 3,  3),
+	PLL_35XX_RATE(100000000,  200, 3,  4),
+	{ /* sentinel */ }
+};
+
+/* EPLL */
+static struct samsung_pll_rate_table exynos4415_epll_rates[] = {
+	PLL_36XX_RATE(800000000, 200, 3, 1,     0),
+	PLL_36XX_RATE(288000000,  96, 2, 2,     0),
+	PLL_36XX_RATE(192000000, 128, 2, 3,     0),
+	PLL_36XX_RATE(144000000,  96, 2, 3,     0),
+	PLL_36XX_RATE(96000000,  128, 2, 4,     0),
+	PLL_36XX_RATE(84000000,  112, 2, 4,     0),
+	PLL_36XX_RATE(80750011,  107, 2, 4, 43691),
+	PLL_36XX_RATE(73728004,   98, 2, 4, 19923),
+	PLL_36XX_RATE(67987602,  271, 3, 5, 62285),
+	PLL_36XX_RATE(65911004,  175, 2, 5, 49982),
+	PLL_36XX_RATE(50000000,  200, 3, 5,     0),
+	PLL_36XX_RATE(49152003,  131, 2, 5,  4719),
+	PLL_36XX_RATE(48000000,  128, 2, 5,     0),
+	PLL_36XX_RATE(45250000,  181, 3, 5,     0),
+	{ /* sentinel */ }
+};
+
+static struct samsung_pll_clock exynos4415_plls[nr_plls] __initdata = {
+	[apll] = PLL(pll_35xx, CLK_FOUT_APLL, "fout_apll", "fin_pll",
+			APLL_LOCK, APLL_CON0, NULL),
+	[epll] = PLL(pll_36xx, CLK_FOUT_EPLL, "fout_epll", "fin_pll",
+			EPLL_LOCK, EPLL_CON0, NULL),
+	[g3d_pll] = PLL(pll_35xx, CLK_FOUT_G3D_PLL, "fout_g3d_pll",
+			"mout_g3d_pllsrc", G3D_PLL_LOCK, G3D_PLL_CON0, NULL),
+	[isp_pll] = PLL(pll_35xx, CLK_FOUT_ISP_PLL, "fout_isp_pll", "fin_pll",
+			ISP_PLL_LOCK, ISP_PLL_CON0, NULL),
+	[disp_pll] = PLL(pll_35xx, CLK_FOUT_DISP_PLL, "fout_disp_pll",
+			"fin_pll", DISP_PLL_LOCK, DISP_PLL_CON0, NULL),
+};
+
+static void __init exynos4415_cmu_init(struct device_node *np)
+{
+	void __iomem *reg_base;
+
+	reg_base = of_iomap(np, 0);
+	if (!reg_base)
+		panic("%s: failed to map registers\n", __func__);
+
+	exynos4415_ctx = samsung_clk_init(np, reg_base, CLK_NR_CLKS);
+	if (!exynos4415_ctx)
+		panic("%s: unable to allocate context.\n", __func__);
+
+	exynos4415_plls[apll].rate_table = exynos4415_pll_rates;
+	exynos4415_plls[epll].rate_table = exynos4415_epll_rates;
+	exynos4415_plls[g3d_pll].rate_table = exynos4415_pll_rates;
+	exynos4415_plls[isp_pll].rate_table = exynos4415_pll_rates;
+	exynos4415_plls[disp_pll].rate_table = exynos4415_pll_rates;
+
+	samsung_clk_register_fixed_factor(exynos4415_ctx,
+				exynos4415_fixed_factor_clks,
+				ARRAY_SIZE(exynos4415_fixed_factor_clks));
+	samsung_clk_register_fixed_rate(exynos4415_ctx,
+				exynos4415_fixed_rate_clks,
+				ARRAY_SIZE(exynos4415_fixed_rate_clks));
+
+	samsung_clk_register_pll(exynos4415_ctx, exynos4415_plls,
+				ARRAY_SIZE(exynos4415_plls), reg_base);
+	samsung_clk_register_mux(exynos4415_ctx, exynos4415_mux_clks,
+				ARRAY_SIZE(exynos4415_mux_clks));
+	samsung_clk_register_div(exynos4415_ctx, exynos4415_div_clks,
+				ARRAY_SIZE(exynos4415_div_clks));
+	samsung_clk_register_gate(exynos4415_ctx, exynos4415_gate_clks,
+				ARRAY_SIZE(exynos4415_gate_clks));
+
+	exynos4415_clk_sleep_init();
+
+	samsung_clk_of_add_provider(np, exynos4415_ctx);
+}
+CLK_OF_DECLARE(exynos4415_cmu, "samsung,exynos4415-cmu", exynos4415_cmu_init);
+
+/*
+ * CMU DMC
+ */
+
+#define MPLL_LOCK		0x008
+#define MPLL_CON0		0x108
+#define MPLL_CON1		0x10c
+#define MPLL_CON2		0x110
+#define BPLL_LOCK		0x118
+#define BPLL_CON0		0x218
+#define BPLL_CON1		0x21c
+#define BPLL_CON2		0x220
+#define SRC_DMC			0x300
+#define DIV_DMC1		0x504
+
+enum exynos4415_dmc_plls {
+	mpll, bpll,
+	nr_dmc_plls,
+};
+
+static struct samsung_clk_provider *exynos4415_dmc_ctx;
+
+#ifdef CONFIG_PM_SLEEP
+static struct samsung_clk_reg_dump *exynos4415_dmc_clk_regs;
+
+static unsigned long exynos4415_cmu_dmc_clk_regs[] __initdata = {
+	MPLL_LOCK,
+	MPLL_CON0,
+	MPLL_CON1,
+	MPLL_CON2,
+	BPLL_LOCK,
+	BPLL_CON0,
+	BPLL_CON1,
+	BPLL_CON2,
+	SRC_DMC,
+	DIV_DMC1,
+};
+
+static int exynos4415_dmc_clk_suspend(void)
+{
+	samsung_clk_save(exynos4415_dmc_ctx->reg_base,
+				exynos4415_dmc_clk_regs,
+				ARRAY_SIZE(exynos4415_cmu_dmc_clk_regs));
+	return 0;
+}
+
+static void exynos4415_dmc_clk_resume(void)
+{
+	samsung_clk_restore(exynos4415_dmc_ctx->reg_base,
+				exynos4415_dmc_clk_regs,
+				ARRAY_SIZE(exynos4415_cmu_dmc_clk_regs));
+}
+
+static struct syscore_ops exynos4415_dmc_clk_syscore_ops = {
+	.suspend = exynos4415_dmc_clk_suspend,
+	.resume = exynos4415_dmc_clk_resume,
+};
+
+static void exynos4415_dmc_clk_sleep_init(void)
+{
+	exynos4415_dmc_clk_regs =
+		samsung_clk_alloc_reg_dump(exynos4415_cmu_dmc_clk_regs,
+				ARRAY_SIZE(exynos4415_cmu_dmc_clk_regs));
+	if (!exynos4415_dmc_clk_regs) {
+		pr_warn("%s: Failed to allocate sleep save data\n", __func__);
+		return;
+	}
+
+	register_syscore_ops(&exynos4415_dmc_clk_syscore_ops);
+}
+#else
+static inline void exynos4415_dmc_clk_sleep_init(void) { }
+#endif /* CONFIG_PM_SLEEP */
+
+PNAME(mout_mpll_p)		= { "fin_pll", "fout_mpll", };
+PNAME(mout_bpll_p)		= { "fin_pll", "fout_bpll", };
+PNAME(mbpll_p)			= { "mout_mpll", "mout_bpll", };
+
+static struct samsung_mux_clock exynos4415_dmc_mux_clks[] __initdata = {
+	MUX(CLK_DMC_MOUT_MPLL, "mout_mpll", mout_mpll_p, SRC_DMC, 12, 1),
+	MUX(CLK_DMC_MOUT_BPLL, "mout_bpll", mout_bpll_p, SRC_DMC, 10, 1),
+	MUX(CLK_DMC_MOUT_DPHY, "mout_dphy", mbpll_p, SRC_DMC, 8, 1),
+	MUX(CLK_DMC_MOUT_DMC_BUS, "mout_dmc_bus", mbpll_p, SRC_DMC, 4, 1),
+};
+
+static struct samsung_div_clock exynos4415_dmc_div_clks[] __initdata = {
+	DIV(CLK_DMC_DIV_DMC, "div_dmc", "div_dmc_pre", DIV_DMC1, 27, 3),
+	DIV(CLK_DMC_DIV_DPHY, "div_dphy", "mout_dphy", DIV_DMC1, 23, 3),
+	DIV(CLK_DMC_DIV_DMC_PRE, "div_dmc_pre", "mout_dmc_bus",
+		DIV_DMC1, 19, 2),
+	DIV(CLK_DMC_DIV_DMCP, "div_dmcp", "div_dmcd", DIV_DMC1, 15, 3),
+	DIV(CLK_DMC_DIV_DMCD, "div_dmcd", "div_dmc", DIV_DMC1, 11, 3),
+	DIV(CLK_DMC_DIV_MPLL_PRE, "div_mpll_pre", "mout_mpll", DIV_DMC1, 8, 2),
+};
+
+static struct samsung_pll_clock exynos4415_dmc_plls[nr_dmc_plls] __initdata = {
+	[mpll] = PLL(pll_35xx, CLK_DMC_FOUT_MPLL, "fout_mpll", "fin_pll",
+		MPLL_LOCK, MPLL_CON0, NULL),
+	[bpll] = PLL(pll_35xx, CLK_DMC_FOUT_BPLL, "fout_bpll", "fin_pll",
+		BPLL_LOCK, BPLL_CON0, NULL),
+};
+
+static void __init exynos4415_cmu_dmc_init(struct device_node *np)
+{
+	void __iomem *reg_base;
+
+	reg_base = of_iomap(np, 0);
+	if (!reg_base)
+		panic("%s: failed to map registers\n", __func__);
+
+	exynos4415_dmc_ctx = samsung_clk_init(np, reg_base, NR_CLKS_DMC);
+	if (!exynos4415_dmc_ctx)
+		panic("%s: unable to allocate context.\n", __func__);
+
+	exynos4415_dmc_plls[mpll].rate_table = exynos4415_pll_rates;
+	exynos4415_dmc_plls[bpll].rate_table = exynos4415_pll_rates;
+
+	samsung_clk_register_pll(exynos4415_dmc_ctx, exynos4415_dmc_plls,
+				ARRAY_SIZE(exynos4415_dmc_plls), reg_base);
+	samsung_clk_register_mux(exynos4415_dmc_ctx, exynos4415_dmc_mux_clks,
+				ARRAY_SIZE(exynos4415_dmc_mux_clks));
+	samsung_clk_register_div(exynos4415_dmc_ctx, exynos4415_dmc_div_clks,
+				ARRAY_SIZE(exynos4415_dmc_div_clks));
+
+	exynos4415_dmc_clk_sleep_init();
+
+	samsung_clk_of_add_provider(np, exynos4415_dmc_ctx);
+}
+CLK_OF_DECLARE(exynos4415_cmu_dmc, "samsung,exynos4415-cmu-dmc",
+		exynos4415_cmu_dmc_init);
diff --git a/drivers/clk/samsung/clk-exynos5260.c b/drivers/clk/samsung/clk-exynos5260.c
index 2527e39aadcf..e2e5193d1049 100644
--- a/drivers/clk/samsung/clk-exynos5260.c
+++ b/drivers/clk/samsung/clk-exynos5260.c
@@ -11,10 +11,8 @@
 
 #include <linux/clk.h>
 #include <linux/clkdev.h>
-#include <linux/clk-provider.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
-#include <linux/syscore_ops.h>
 
 #include "clk-exynos5260.h"
 #include "clk.h"
@@ -22,39 +20,6 @@
 
 #include <dt-bindings/clock/exynos5260-clk.h>
 
-static LIST_HEAD(clock_reg_cache_list);
-
-struct exynos5260_clock_reg_cache {
-	struct list_head node;
-	void __iomem *reg_base;
-	struct samsung_clk_reg_dump *rdump;
-	unsigned int rd_num;
-};
-
-struct exynos5260_cmu_info {
-	/* list of pll clocks and respective count */
-	struct samsung_pll_clock *pll_clks;
-	unsigned int nr_pll_clks;
-	/* list of mux clocks and respective count */
-	struct samsung_mux_clock *mux_clks;
-	unsigned int nr_mux_clks;
-	/* list of div clocks and respective count */
-	struct samsung_div_clock *div_clks;
-	unsigned int nr_div_clks;
-	/* list of gate clocks and respective count */
-	struct samsung_gate_clock *gate_clks;
-	unsigned int nr_gate_clks;
-	/* list of fixed clocks and respective count */
-	struct samsung_fixed_rate_clock *fixed_clks;
-	unsigned int nr_fixed_clks;
-	/* total number of clocks with IDs assigned*/
-	unsigned int nr_clk_ids;
-
-	/* list and number of clocks registers */
-	unsigned long *clk_regs;
-	unsigned int nr_clk_regs;
-};
-
 /*
  * Applicable for all 2550 Type PLLS for Exynos5260, listed below
  * DISP_PLL, EGL_PLL, KFC_PLL, MEM_PLL, BUS_PLL, MEDIA_PLL, G3D_PLL.
@@ -113,104 +78,6 @@ static struct samsung_pll_rate_table pll2650_24mhz_tbl[] __initdata = {
 	PLL_36XX_RATE(66000000, 176, 2, 5, 0),
 };
 
-#ifdef CONFIG_PM_SLEEP
-
-static int exynos5260_clk_suspend(void)
-{
-	struct exynos5260_clock_reg_cache *cache;
-
-	list_for_each_entry(cache, &clock_reg_cache_list, node)
-		samsung_clk_save(cache->reg_base, cache->rdump,
-				cache->rd_num);
-
-	return 0;
-}
-
-static void exynos5260_clk_resume(void)
-{
-	struct exynos5260_clock_reg_cache *cache;
-
-	list_for_each_entry(cache, &clock_reg_cache_list, node)
-		samsung_clk_restore(cache->reg_base, cache->rdump,
-				cache->rd_num);
-}
-
-static struct syscore_ops exynos5260_clk_syscore_ops = {
-	.suspend = exynos5260_clk_suspend,
-	.resume = exynos5260_clk_resume,
-};
-
-static void exynos5260_clk_sleep_init(void __iomem *reg_base,
-			unsigned long *rdump,
-			unsigned long nr_rdump)
-{
-	struct exynos5260_clock_reg_cache *reg_cache;
-
-	reg_cache = kzalloc(sizeof(struct exynos5260_clock_reg_cache),
-			GFP_KERNEL);
-	if (!reg_cache)
-		panic("could not allocate register cache.\n");
-
-	reg_cache->rdump = samsung_clk_alloc_reg_dump(rdump, nr_rdump);
-
-	if (!reg_cache->rdump)
-		panic("could not allocate register dump storage.\n");
-
-	if (list_empty(&clock_reg_cache_list))
-		register_syscore_ops(&exynos5260_clk_syscore_ops);
-
-	reg_cache->rd_num = nr_rdump;
-	reg_cache->reg_base = reg_base;
-	list_add_tail(&reg_cache->node, &clock_reg_cache_list);
-}
-
-#else
-static void exynos5260_clk_sleep_init(void __iomem *reg_base,
-			unsigned long *rdump,
-			unsigned long nr_rdump){}
-#endif
-
-/*
- * Common function which registers plls, muxes, dividers and gates
- * for each CMU. It also add CMU register list to register cache.
- */
-
-void __init exynos5260_cmu_register_one(struct device_node *np,
-			struct exynos5260_cmu_info *cmu)
-{
-	void __iomem *reg_base;
-	struct samsung_clk_provider *ctx;
-
-	reg_base = of_iomap(np, 0);
-	if (!reg_base)
-		panic("%s: failed to map registers\n", __func__);
-
-	ctx = samsung_clk_init(np, reg_base, cmu->nr_clk_ids);
-	if (!ctx)
-		panic("%s: unable to alllocate ctx\n", __func__);
-
-	if (cmu->pll_clks)
-		samsung_clk_register_pll(ctx, cmu->pll_clks, cmu->nr_pll_clks,
-			reg_base);
-	if (cmu->mux_clks)
-		samsung_clk_register_mux(ctx,  cmu->mux_clks,
-			cmu->nr_mux_clks);
-	if (cmu->div_clks)
-		samsung_clk_register_div(ctx, cmu->div_clks, cmu->nr_div_clks);
-	if (cmu->gate_clks)
-		samsung_clk_register_gate(ctx, cmu->gate_clks,
-			cmu->nr_gate_clks);
-	if (cmu->fixed_clks)
-		samsung_clk_register_fixed_rate(ctx, cmu->fixed_clks,
-			cmu->nr_fixed_clks);
-	if (cmu->clk_regs)
-		exynos5260_clk_sleep_init(reg_base, cmu->clk_regs,
-			cmu->nr_clk_regs);
-
-	samsung_clk_of_add_provider(np, ctx);
-}
-
-
 /* CMU_AUD */
 
 static unsigned long aud_clk_regs[] __initdata = {
@@ -268,7 +135,7 @@ struct samsung_gate_clock aud_gate_clks[] __initdata = {
 
 static void __init exynos5260_clk_aud_init(struct device_node *np)
 {
-	struct exynos5260_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = {0};
 
 	cmu.mux_clks = aud_mux_clks;
 	cmu.nr_mux_clks = ARRAY_SIZE(aud_mux_clks);
@@ -280,7 +147,7 @@ static void __init exynos5260_clk_aud_init(struct device_node *np)
 	cmu.clk_regs = aud_clk_regs;
 	cmu.nr_clk_regs = ARRAY_SIZE(aud_clk_regs);
 
-	exynos5260_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_aud, "samsung,exynos5260-clock-aud",
@@ -458,7 +325,7 @@ struct samsung_gate_clock disp_gate_clks[] __initdata = {
 
 static void __init exynos5260_clk_disp_init(struct device_node *np)
 {
-	struct exynos5260_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = {0};
 
 	cmu.mux_clks = disp_mux_clks;
 	cmu.nr_mux_clks = ARRAY_SIZE(disp_mux_clks);
@@ -470,7 +337,7 @@ static void __init exynos5260_clk_disp_init(struct device_node *np)
 	cmu.clk_regs = disp_clk_regs;
 	cmu.nr_clk_regs = ARRAY_SIZE(disp_clk_regs);
 
-	exynos5260_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_disp, "samsung,exynos5260-clock-disp",
@@ -522,7 +389,7 @@ static struct samsung_pll_clock egl_pll_clks[] __initdata = {
 
 static void __init exynos5260_clk_egl_init(struct device_node *np)
 {
-	struct exynos5260_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = {0};
 
 	cmu.pll_clks = egl_pll_clks;
 	cmu.nr_pll_clks =  ARRAY_SIZE(egl_pll_clks);
@@ -534,7 +401,7 @@ static void __init exynos5260_clk_egl_init(struct device_node *np)
 	cmu.clk_regs = egl_clk_regs;
 	cmu.nr_clk_regs = ARRAY_SIZE(egl_clk_regs);
 
-	exynos5260_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_egl, "samsung,exynos5260-clock-egl",
@@ -624,7 +491,7 @@ struct samsung_gate_clock fsys_gate_clks[] __initdata = {
 
 static void __init exynos5260_clk_fsys_init(struct device_node *np)
 {
-	struct exynos5260_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = {0};
 
 	cmu.mux_clks = fsys_mux_clks;
 	cmu.nr_mux_clks = ARRAY_SIZE(fsys_mux_clks);
@@ -634,7 +501,7 @@ static void __init exynos5260_clk_fsys_init(struct device_node *np)
 	cmu.clk_regs = fsys_clk_regs;
 	cmu.nr_clk_regs = ARRAY_SIZE(fsys_clk_regs);
 
-	exynos5260_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_fsys, "samsung,exynos5260-clock-fsys",
@@ -713,7 +580,7 @@ struct samsung_gate_clock g2d_gate_clks[] __initdata = {
 
 static void __init exynos5260_clk_g2d_init(struct device_node *np)
 {
-	struct exynos5260_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = {0};
 
 	cmu.mux_clks = g2d_mux_clks;
 	cmu.nr_mux_clks = ARRAY_SIZE(g2d_mux_clks);
@@ -725,7 +592,7 @@ static void __init exynos5260_clk_g2d_init(struct device_node *np)
 	cmu.clk_regs = g2d_clk_regs;
 	cmu.nr_clk_regs = ARRAY_SIZE(g2d_clk_regs);
 
-	exynos5260_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_g2d, "samsung,exynos5260-clock-g2d",
@@ -774,7 +641,7 @@ static struct samsung_pll_clock g3d_pll_clks[] __initdata = {
 
 static void __init exynos5260_clk_g3d_init(struct device_node *np)
 {
-	struct exynos5260_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = {0};
 
 	cmu.pll_clks = g3d_pll_clks;
 	cmu.nr_pll_clks =  ARRAY_SIZE(g3d_pll_clks);
@@ -788,7 +655,7 @@ static void __init exynos5260_clk_g3d_init(struct device_node *np)
 	cmu.clk_regs = g3d_clk_regs;
 	cmu.nr_clk_regs = ARRAY_SIZE(g3d_clk_regs);
 
-	exynos5260_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_g3d, "samsung,exynos5260-clock-g3d",
@@ -909,7 +776,7 @@ struct samsung_gate_clock gscl_gate_clks[] __initdata = {
 
 static void __init exynos5260_clk_gscl_init(struct device_node *np)
 {
-	struct exynos5260_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = {0};
 
 	cmu.mux_clks = gscl_mux_clks;
 	cmu.nr_mux_clks = ARRAY_SIZE(gscl_mux_clks);
@@ -921,7 +788,7 @@ static void __init exynos5260_clk_gscl_init(struct device_node *np)
 	cmu.clk_regs = gscl_clk_regs;
 	cmu.nr_clk_regs = ARRAY_SIZE(gscl_clk_regs);
 
-	exynos5260_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_gscl, "samsung,exynos5260-clock-gscl",
@@ -1028,7 +895,7 @@ struct samsung_gate_clock isp_gate_clks[] __initdata = {
 
 static void __init exynos5260_clk_isp_init(struct device_node *np)
 {
-	struct exynos5260_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = {0};
 
 	cmu.mux_clks = isp_mux_clks;
 	cmu.nr_mux_clks = ARRAY_SIZE(isp_mux_clks);
@@ -1040,7 +907,7 @@ static void __init exynos5260_clk_isp_init(struct device_node *np)
 	cmu.clk_regs = isp_clk_regs;
 	cmu.nr_clk_regs = ARRAY_SIZE(isp_clk_regs);
 
-	exynos5260_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_isp, "samsung,exynos5260-clock-isp",
@@ -1092,7 +959,7 @@ static struct samsung_pll_clock kfc_pll_clks[] __initdata = {
 
 static void __init exynos5260_clk_kfc_init(struct device_node *np)
 {
-	struct exynos5260_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = {0};
 
 	cmu.pll_clks = kfc_pll_clks;
 	cmu.nr_pll_clks =  ARRAY_SIZE(kfc_pll_clks);
@@ -1104,7 +971,7 @@ static void __init exynos5260_clk_kfc_init(struct device_node *np)
 	cmu.clk_regs = kfc_clk_regs;
 	cmu.nr_clk_regs = ARRAY_SIZE(kfc_clk_regs);
 
-	exynos5260_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_kfc, "samsung,exynos5260-clock-kfc",
@@ -1148,7 +1015,7 @@ struct samsung_gate_clock mfc_gate_clks[] __initdata = {
 
 static void __init exynos5260_clk_mfc_init(struct device_node *np)
 {
-	struct exynos5260_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = {0};
 
 	cmu.mux_clks = mfc_mux_clks;
 	cmu.nr_mux_clks = ARRAY_SIZE(mfc_mux_clks);
@@ -1160,7 +1027,7 @@ static void __init exynos5260_clk_mfc_init(struct device_node *np)
 	cmu.clk_regs = mfc_clk_regs;
 	cmu.nr_clk_regs = ARRAY_SIZE(mfc_clk_regs);
 
-	exynos5260_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_mfc, "samsung,exynos5260-clock-mfc",
@@ -1295,7 +1162,7 @@ static struct samsung_pll_clock mif_pll_clks[] __initdata = {
 
 static void __init exynos5260_clk_mif_init(struct device_node *np)
 {
-	struct exynos5260_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = {0};
 
 	cmu.pll_clks = mif_pll_clks;
 	cmu.nr_pll_clks =  ARRAY_SIZE(mif_pll_clks);
@@ -1309,7 +1176,7 @@ static void __init exynos5260_clk_mif_init(struct device_node *np)
 	cmu.clk_regs = mif_clk_regs;
 	cmu.nr_clk_regs = ARRAY_SIZE(mif_clk_regs);
 
-	exynos5260_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_mif, "samsung,exynos5260-clock-mif",
@@ -1503,7 +1370,7 @@ struct samsung_gate_clock peri_gate_clks[] __initdata = {
 
 static void __init exynos5260_clk_peri_init(struct device_node *np)
 {
-	struct exynos5260_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = {0};
 
 	cmu.mux_clks = peri_mux_clks;
 	cmu.nr_mux_clks = ARRAY_SIZE(peri_mux_clks);
@@ -1515,7 +1382,7 @@ static void __init exynos5260_clk_peri_init(struct device_node *np)
 	cmu.clk_regs = peri_clk_regs;
 	cmu.nr_clk_regs = ARRAY_SIZE(peri_clk_regs);
 
-	exynos5260_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_peri, "samsung,exynos5260-clock-peri",
@@ -1959,7 +1826,7 @@ static struct samsung_pll_clock top_pll_clks[] __initdata = {
 
 static void __init exynos5260_clk_top_init(struct device_node *np)
 {
-	struct exynos5260_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = {0};
 
 	cmu.pll_clks = top_pll_clks;
 	cmu.nr_pll_clks =  ARRAY_SIZE(top_pll_clks);
@@ -1975,7 +1842,7 @@ static void __init exynos5260_clk_top_init(struct device_node *np)
 	cmu.clk_regs = top_clk_regs;
 	cmu.nr_clk_regs = ARRAY_SIZE(top_clk_regs);
 
-	exynos5260_cmu_register_one(np, &cmu);
+	samsung_cmu_register_one(np, &cmu);
 }
 
 CLK_OF_DECLARE(exynos5260_clk_top, "samsung,exynos5260-clock-top",
diff --git a/drivers/clk/samsung/clk-exynos7.c b/drivers/clk/samsung/clk-exynos7.c
new file mode 100644
index 000000000000..ea4483b8d62e
--- /dev/null
+++ b/drivers/clk/samsung/clk-exynos7.c
@@ -0,0 +1,743 @@
+/*
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ * Author: Naveen Krishna Ch <naveenkrishna.ch@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.
+ *
+*/
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+
+#include "clk.h"
+#include <dt-bindings/clock/exynos7-clk.h>
+
+/* Register Offset definitions for CMU_TOPC (0x10570000) */
+#define CC_PLL_LOCK		0x0000
+#define BUS0_PLL_LOCK		0x0004
+#define BUS1_DPLL_LOCK		0x0008
+#define MFC_PLL_LOCK		0x000C
+#define AUD_PLL_LOCK		0x0010
+#define CC_PLL_CON0		0x0100
+#define BUS0_PLL_CON0		0x0110
+#define BUS1_DPLL_CON0		0x0120
+#define MFC_PLL_CON0		0x0130
+#define AUD_PLL_CON0		0x0140
+#define MUX_SEL_TOPC0		0x0200
+#define MUX_SEL_TOPC1		0x0204
+#define MUX_SEL_TOPC2		0x0208
+#define MUX_SEL_TOPC3		0x020C
+#define DIV_TOPC0		0x0600
+#define DIV_TOPC1		0x0604
+#define DIV_TOPC3		0x060C
+
+static struct samsung_fixed_factor_clock topc_fixed_factor_clks[] __initdata = {
+	FFACTOR(0, "ffac_topc_bus0_pll_div2", "mout_bus0_pll_ctrl", 1, 2, 0),
+	FFACTOR(0, "ffac_topc_bus0_pll_div4",
+		"ffac_topc_bus0_pll_div2", 1, 2, 0),
+	FFACTOR(0, "ffac_topc_bus1_pll_div2", "mout_bus1_pll_ctrl", 1, 2, 0),
+	FFACTOR(0, "ffac_topc_cc_pll_div2", "mout_cc_pll_ctrl", 1, 2, 0),
+	FFACTOR(0, "ffac_topc_mfc_pll_div2", "mout_mfc_pll_ctrl", 1, 2, 0),
+};
+
+/* List of parent clocks for Muxes in CMU_TOPC */
+PNAME(mout_bus0_pll_ctrl_p)	= { "fin_pll", "fout_bus0_pll" };
+PNAME(mout_bus1_pll_ctrl_p)	= { "fin_pll", "fout_bus1_pll" };
+PNAME(mout_cc_pll_ctrl_p)	= { "fin_pll", "fout_cc_pll" };
+PNAME(mout_mfc_pll_ctrl_p)	= { "fin_pll", "fout_mfc_pll" };
+
+PNAME(mout_topc_group2) = { "mout_sclk_bus0_pll_cmuc",
+	"mout_sclk_bus1_pll_cmuc", "mout_sclk_cc_pll_cmuc",
+	"mout_sclk_mfc_pll_cmuc" };
+
+PNAME(mout_sclk_bus0_pll_cmuc_p) = { "mout_bus0_pll_ctrl",
+	"ffac_topc_bus0_pll_div2", "ffac_topc_bus0_pll_div4"};
+PNAME(mout_sclk_bus1_pll_cmuc_p) = { "mout_bus1_pll_ctrl",
+	"ffac_topc_bus1_pll_div2"};
+PNAME(mout_sclk_cc_pll_cmuc_p) = { "mout_cc_pll_ctrl",
+	"ffac_topc_cc_pll_div2"};
+PNAME(mout_sclk_mfc_pll_cmuc_p) = { "mout_mfc_pll_ctrl",
+	"ffac_topc_mfc_pll_div2"};
+
+
+PNAME(mout_sclk_bus0_pll_out_p) = {"mout_bus0_pll_ctrl",
+	"ffac_topc_bus0_pll_div2"};
+
+static unsigned long topc_clk_regs[] __initdata = {
+	CC_PLL_LOCK,
+	BUS0_PLL_LOCK,
+	BUS1_DPLL_LOCK,
+	MFC_PLL_LOCK,
+	AUD_PLL_LOCK,
+	CC_PLL_CON0,
+	BUS0_PLL_CON0,
+	BUS1_DPLL_CON0,
+	MFC_PLL_CON0,
+	AUD_PLL_CON0,
+	MUX_SEL_TOPC0,
+	MUX_SEL_TOPC1,
+	MUX_SEL_TOPC2,
+	MUX_SEL_TOPC3,
+	DIV_TOPC0,
+	DIV_TOPC1,
+	DIV_TOPC3,
+};
+
+static struct samsung_mux_clock topc_mux_clks[] __initdata = {
+	MUX(0, "mout_bus0_pll_ctrl", mout_bus0_pll_ctrl_p, MUX_SEL_TOPC0, 0, 1),
+	MUX(0, "mout_bus1_pll_ctrl", mout_bus1_pll_ctrl_p, MUX_SEL_TOPC0, 4, 1),
+	MUX(0, "mout_cc_pll_ctrl", mout_cc_pll_ctrl_p, MUX_SEL_TOPC0, 8, 1),
+	MUX(0, "mout_mfc_pll_ctrl", mout_mfc_pll_ctrl_p, MUX_SEL_TOPC0, 12, 1),
+
+	MUX(0, "mout_sclk_bus0_pll_cmuc", mout_sclk_bus0_pll_cmuc_p,
+		MUX_SEL_TOPC0, 16, 2),
+	MUX(0, "mout_sclk_bus1_pll_cmuc", mout_sclk_bus1_pll_cmuc_p,
+		MUX_SEL_TOPC0, 20, 1),
+	MUX(0, "mout_sclk_cc_pll_cmuc", mout_sclk_cc_pll_cmuc_p,
+		MUX_SEL_TOPC0, 24, 1),
+	MUX(0, "mout_sclk_mfc_pll_cmuc", mout_sclk_mfc_pll_cmuc_p,
+		MUX_SEL_TOPC0, 28, 1),
+
+	MUX(0, "mout_sclk_bus0_pll_out", mout_sclk_bus0_pll_out_p,
+		MUX_SEL_TOPC1, 16, 1),
+
+	MUX(0, "mout_aclk_ccore_133", mout_topc_group2,	MUX_SEL_TOPC2, 4, 2),
+
+	MUX(0, "mout_aclk_peris_66", mout_topc_group2, MUX_SEL_TOPC3, 24, 2),
+};
+
+static struct samsung_div_clock topc_div_clks[] __initdata = {
+	DIV(DOUT_ACLK_CCORE_133, "dout_aclk_ccore_133", "mout_aclk_ccore_133",
+		DIV_TOPC0, 4, 4),
+
+	DIV(DOUT_ACLK_PERIS, "dout_aclk_peris_66", "mout_aclk_peris_66",
+		DIV_TOPC1, 24, 4),
+
+	DIV(DOUT_SCLK_BUS0_PLL, "dout_sclk_bus0_pll", "mout_sclk_bus0_pll_out",
+		DIV_TOPC3, 0, 3),
+	DIV(DOUT_SCLK_BUS1_PLL, "dout_sclk_bus1_pll", "mout_bus1_pll_ctrl",
+		DIV_TOPC3, 8, 3),
+	DIV(DOUT_SCLK_CC_PLL, "dout_sclk_cc_pll", "mout_cc_pll_ctrl",
+		DIV_TOPC3, 12, 3),
+	DIV(DOUT_SCLK_MFC_PLL, "dout_sclk_mfc_pll", "mout_mfc_pll_ctrl",
+		DIV_TOPC3, 16, 3),
+};
+
+static struct samsung_pll_clock topc_pll_clks[] __initdata = {
+	PLL(pll_1451x, 0, "fout_bus0_pll", "fin_pll", BUS0_PLL_LOCK,
+		BUS0_PLL_CON0, NULL),
+	PLL(pll_1452x, 0, "fout_cc_pll", "fin_pll", CC_PLL_LOCK,
+		CC_PLL_CON0, NULL),
+	PLL(pll_1452x, 0, "fout_bus1_pll", "fin_pll", BUS1_DPLL_LOCK,
+		BUS1_DPLL_CON0, NULL),
+	PLL(pll_1452x, 0, "fout_mfc_pll", "fin_pll", MFC_PLL_LOCK,
+		MFC_PLL_CON0, NULL),
+	PLL(pll_1460x, 0, "fout_aud_pll", "fin_pll", AUD_PLL_LOCK,
+		AUD_PLL_CON0, NULL),
+};
+
+static struct samsung_cmu_info topc_cmu_info __initdata = {
+	.pll_clks		= topc_pll_clks,
+	.nr_pll_clks		= ARRAY_SIZE(topc_pll_clks),
+	.mux_clks		= topc_mux_clks,
+	.nr_mux_clks		= ARRAY_SIZE(topc_mux_clks),
+	.div_clks		= topc_div_clks,
+	.nr_div_clks		= ARRAY_SIZE(topc_div_clks),
+	.fixed_factor_clks	= topc_fixed_factor_clks,
+	.nr_fixed_factor_clks	= ARRAY_SIZE(topc_fixed_factor_clks),
+	.nr_clk_ids		= TOPC_NR_CLK,
+	.clk_regs		= topc_clk_regs,
+	.nr_clk_regs		= ARRAY_SIZE(topc_clk_regs),
+};
+
+static void __init exynos7_clk_topc_init(struct device_node *np)
+{
+	samsung_cmu_register_one(np, &topc_cmu_info);
+}
+
+CLK_OF_DECLARE(exynos7_clk_topc, "samsung,exynos7-clock-topc",
+	exynos7_clk_topc_init);
+
+/* Register Offset definitions for CMU_TOP0 (0x105D0000) */
+#define MUX_SEL_TOP00			0x0200
+#define MUX_SEL_TOP01			0x0204
+#define MUX_SEL_TOP03			0x020C
+#define MUX_SEL_TOP0_PERIC3		0x023C
+#define DIV_TOP03			0x060C
+#define DIV_TOP0_PERIC3			0x063C
+#define ENABLE_SCLK_TOP0_PERIC3		0x0A3C
+
+/* List of parent clocks for Muxes in CMU_TOP0 */
+PNAME(mout_bus0_pll_p)	= { "fin_pll", "dout_sclk_bus0_pll" };
+PNAME(mout_bus1_pll_p)	= { "fin_pll", "dout_sclk_bus1_pll" };
+PNAME(mout_cc_pll_p)	= { "fin_pll", "dout_sclk_cc_pll" };
+PNAME(mout_mfc_pll_p)	= { "fin_pll", "dout_sclk_mfc_pll" };
+
+PNAME(mout_top0_half_bus0_pll_p) = {"mout_top0_bus0_pll",
+	"ffac_top0_bus0_pll_div2"};
+PNAME(mout_top0_half_bus1_pll_p) = {"mout_top0_bus1_pll",
+	"ffac_top0_bus1_pll_div2"};
+PNAME(mout_top0_half_cc_pll_p) = {"mout_top0_cc_pll",
+	"ffac_top0_cc_pll_div2"};
+PNAME(mout_top0_half_mfc_pll_p) = {"mout_top0_mfc_pll",
+	"ffac_top0_mfc_pll_div2"};
+
+PNAME(mout_top0_group1) = {"mout_top0_half_bus0_pll",
+	"mout_top0_half_bus1_pll", "mout_top0_half_cc_pll",
+	"mout_top0_half_mfc_pll"};
+
+static unsigned long top0_clk_regs[] __initdata = {
+	MUX_SEL_TOP00,
+	MUX_SEL_TOP01,
+	MUX_SEL_TOP03,
+	MUX_SEL_TOP0_PERIC3,
+	DIV_TOP03,
+	DIV_TOP0_PERIC3,
+	ENABLE_SCLK_TOP0_PERIC3,
+};
+
+static struct samsung_mux_clock top0_mux_clks[] __initdata = {
+	MUX(0, "mout_top0_mfc_pll", mout_mfc_pll_p, MUX_SEL_TOP00, 4, 1),
+	MUX(0, "mout_top0_cc_pll", mout_cc_pll_p, MUX_SEL_TOP00, 8, 1),
+	MUX(0, "mout_top0_bus1_pll", mout_bus1_pll_p, MUX_SEL_TOP00, 12, 1),
+	MUX(0, "mout_top0_bus0_pll", mout_bus0_pll_p, MUX_SEL_TOP00, 16, 1),
+
+	MUX(0, "mout_top0_half_mfc_pll", mout_top0_half_mfc_pll_p,
+		MUX_SEL_TOP01, 4, 1),
+	MUX(0, "mout_top0_half_cc_pll", mout_top0_half_cc_pll_p,
+		MUX_SEL_TOP01, 8, 1),
+	MUX(0, "mout_top0_half_bus1_pll", mout_top0_half_bus1_pll_p,
+		MUX_SEL_TOP01, 12, 1),
+	MUX(0, "mout_top0_half_bus0_pll", mout_top0_half_bus0_pll_p,
+		MUX_SEL_TOP01, 16, 1),
+
+	MUX(0, "mout_aclk_peric1_66", mout_top0_group1, MUX_SEL_TOP03, 12, 2),
+	MUX(0, "mout_aclk_peric0_66", mout_top0_group1, MUX_SEL_TOP03, 20, 2),
+
+	MUX(0, "mout_sclk_uart3", mout_top0_group1, MUX_SEL_TOP0_PERIC3, 4, 2),
+	MUX(0, "mout_sclk_uart2", mout_top0_group1, MUX_SEL_TOP0_PERIC3, 8, 2),
+	MUX(0, "mout_sclk_uart1", mout_top0_group1, MUX_SEL_TOP0_PERIC3, 12, 2),
+	MUX(0, "mout_sclk_uart0", mout_top0_group1, MUX_SEL_TOP0_PERIC3, 16, 2),
+};
+
+static struct samsung_div_clock top0_div_clks[] __initdata = {
+	DIV(DOUT_ACLK_PERIC1, "dout_aclk_peric1_66", "mout_aclk_peric1_66",
+		DIV_TOP03, 12, 6),
+	DIV(DOUT_ACLK_PERIC0, "dout_aclk_peric0_66", "mout_aclk_peric0_66",
+		DIV_TOP03, 20, 6),
+
+	DIV(0, "dout_sclk_uart3", "mout_sclk_uart3", DIV_TOP0_PERIC3, 4, 4),
+	DIV(0, "dout_sclk_uart2", "mout_sclk_uart2", DIV_TOP0_PERIC3, 8, 4),
+	DIV(0, "dout_sclk_uart1", "mout_sclk_uart1", DIV_TOP0_PERIC3, 12, 4),
+	DIV(0, "dout_sclk_uart0", "mout_sclk_uart0", DIV_TOP0_PERIC3, 16, 4),
+};
+
+static struct samsung_gate_clock top0_gate_clks[] __initdata = {
+	GATE(CLK_SCLK_UART3, "sclk_uart3", "dout_sclk_uart3",
+		ENABLE_SCLK_TOP0_PERIC3, 4, 0, 0),
+	GATE(CLK_SCLK_UART2, "sclk_uart2", "dout_sclk_uart2",
+		ENABLE_SCLK_TOP0_PERIC3, 8, 0, 0),
+	GATE(CLK_SCLK_UART1, "sclk_uart1", "dout_sclk_uart1",
+		ENABLE_SCLK_TOP0_PERIC3, 12, 0, 0),
+	GATE(CLK_SCLK_UART0, "sclk_uart0", "dout_sclk_uart0",
+		ENABLE_SCLK_TOP0_PERIC3, 16, 0, 0),
+};
+
+static struct samsung_fixed_factor_clock top0_fixed_factor_clks[] __initdata = {
+	FFACTOR(0, "ffac_top0_bus0_pll_div2", "mout_top0_bus0_pll", 1, 2, 0),
+	FFACTOR(0, "ffac_top0_bus1_pll_div2", "mout_top0_bus1_pll", 1, 2, 0),
+	FFACTOR(0, "ffac_top0_cc_pll_div2", "mout_top0_cc_pll", 1, 2, 0),
+	FFACTOR(0, "ffac_top0_mfc_pll_div2", "mout_top0_mfc_pll", 1, 2, 0),
+};
+
+static struct samsung_cmu_info top0_cmu_info __initdata = {
+	.mux_clks		= top0_mux_clks,
+	.nr_mux_clks		= ARRAY_SIZE(top0_mux_clks),
+	.div_clks		= top0_div_clks,
+	.nr_div_clks		= ARRAY_SIZE(top0_div_clks),
+	.gate_clks		= top0_gate_clks,
+	.nr_gate_clks		= ARRAY_SIZE(top0_gate_clks),
+	.fixed_factor_clks	= top0_fixed_factor_clks,
+	.nr_fixed_factor_clks	= ARRAY_SIZE(top0_fixed_factor_clks),
+	.nr_clk_ids		= TOP0_NR_CLK,
+	.clk_regs		= top0_clk_regs,
+	.nr_clk_regs		= ARRAY_SIZE(top0_clk_regs),
+};
+
+static void __init exynos7_clk_top0_init(struct device_node *np)
+{
+	samsung_cmu_register_one(np, &top0_cmu_info);
+}
+
+CLK_OF_DECLARE(exynos7_clk_top0, "samsung,exynos7-clock-top0",
+	exynos7_clk_top0_init);
+
+/* Register Offset definitions for CMU_TOP1 (0x105E0000) */
+#define MUX_SEL_TOP10			0x0200
+#define MUX_SEL_TOP11			0x0204
+#define MUX_SEL_TOP13			0x020C
+#define MUX_SEL_TOP1_FSYS0		0x0224
+#define MUX_SEL_TOP1_FSYS1		0x0228
+#define DIV_TOP13			0x060C
+#define DIV_TOP1_FSYS0			0x0624
+#define DIV_TOP1_FSYS1			0x0628
+#define ENABLE_ACLK_TOP13		0x080C
+#define ENABLE_SCLK_TOP1_FSYS0		0x0A24
+#define ENABLE_SCLK_TOP1_FSYS1		0x0A28
+
+/* List of parent clocks for Muxes in CMU_TOP1 */
+PNAME(mout_top1_bus0_pll_p)	= { "fin_pll", "dout_sclk_bus0_pll" };
+PNAME(mout_top1_bus1_pll_p)	= { "fin_pll", "dout_sclk_bus1_pll_b" };
+PNAME(mout_top1_cc_pll_p)	= { "fin_pll", "dout_sclk_cc_pll_b" };
+PNAME(mout_top1_mfc_pll_p)	= { "fin_pll", "dout_sclk_mfc_pll_b" };
+
+PNAME(mout_top1_half_bus0_pll_p) = {"mout_top1_bus0_pll",
+	"ffac_top1_bus0_pll_div2"};
+PNAME(mout_top1_half_bus1_pll_p) = {"mout_top1_bus1_pll",
+	"ffac_top1_bus1_pll_div2"};
+PNAME(mout_top1_half_cc_pll_p) = {"mout_top1_cc_pll",
+	"ffac_top1_cc_pll_div2"};
+PNAME(mout_top1_half_mfc_pll_p) = {"mout_top1_mfc_pll",
+	"ffac_top1_mfc_pll_div2"};
+
+PNAME(mout_top1_group1) = {"mout_top1_half_bus0_pll",
+	"mout_top1_half_bus1_pll", "mout_top1_half_cc_pll",
+	"mout_top1_half_mfc_pll"};
+
+static unsigned long top1_clk_regs[] __initdata = {
+	MUX_SEL_TOP10,
+	MUX_SEL_TOP11,
+	MUX_SEL_TOP13,
+	MUX_SEL_TOP1_FSYS0,
+	MUX_SEL_TOP1_FSYS1,
+	DIV_TOP13,
+	DIV_TOP1_FSYS0,
+	DIV_TOP1_FSYS1,
+	ENABLE_ACLK_TOP13,
+	ENABLE_SCLK_TOP1_FSYS0,
+	ENABLE_SCLK_TOP1_FSYS1,
+};
+
+static struct samsung_mux_clock top1_mux_clks[] __initdata = {
+	MUX(0, "mout_top1_mfc_pll", mout_top1_mfc_pll_p, MUX_SEL_TOP10, 4, 1),
+	MUX(0, "mout_top1_cc_pll", mout_top1_cc_pll_p, MUX_SEL_TOP10, 8, 1),
+	MUX(0, "mout_top1_bus1_pll", mout_top1_bus1_pll_p,
+		MUX_SEL_TOP10, 12, 1),
+	MUX(0, "mout_top1_bus0_pll", mout_top1_bus0_pll_p,
+		MUX_SEL_TOP10, 16, 1),
+
+	MUX(0, "mout_top1_half_mfc_pll", mout_top1_half_mfc_pll_p,
+		MUX_SEL_TOP11, 4, 1),
+	MUX(0, "mout_top1_half_cc_pll", mout_top1_half_cc_pll_p,
+		MUX_SEL_TOP11, 8, 1),
+	MUX(0, "mout_top1_half_bus1_pll", mout_top1_half_bus1_pll_p,
+		MUX_SEL_TOP11, 12, 1),
+	MUX(0, "mout_top1_half_bus0_pll", mout_top1_half_bus0_pll_p,
+		MUX_SEL_TOP11, 16, 1),
+
+	MUX(0, "mout_aclk_fsys1_200", mout_top1_group1, MUX_SEL_TOP13, 24, 2),
+	MUX(0, "mout_aclk_fsys0_200", mout_top1_group1, MUX_SEL_TOP13, 28, 2),
+
+	MUX(0, "mout_sclk_mmc2", mout_top1_group1, MUX_SEL_TOP1_FSYS0, 24, 2),
+
+	MUX(0, "mout_sclk_mmc1", mout_top1_group1, MUX_SEL_TOP1_FSYS1, 24, 2),
+	MUX(0, "mout_sclk_mmc0", mout_top1_group1, MUX_SEL_TOP1_FSYS1, 28, 2),
+};
+
+static struct samsung_div_clock top1_div_clks[] __initdata = {
+	DIV(DOUT_ACLK_FSYS1_200, "dout_aclk_fsys1_200", "mout_aclk_fsys1_200",
+		DIV_TOP13, 24, 4),
+	DIV(DOUT_ACLK_FSYS0_200, "dout_aclk_fsys0_200", "mout_aclk_fsys0_200",
+		DIV_TOP13, 28, 4),
+
+	DIV(DOUT_SCLK_MMC2, "dout_sclk_mmc2", "mout_sclk_mmc2",
+		DIV_TOP1_FSYS0, 24, 4),
+
+	DIV(DOUT_SCLK_MMC1, "dout_sclk_mmc1", "mout_sclk_mmc1",
+		DIV_TOP1_FSYS1, 24, 4),
+	DIV(DOUT_SCLK_MMC0, "dout_sclk_mmc0", "mout_sclk_mmc0",
+		DIV_TOP1_FSYS1, 28, 4),
+};
+
+static struct samsung_gate_clock top1_gate_clks[] __initdata = {
+	GATE(CLK_SCLK_MMC2, "sclk_mmc2", "dout_sclk_mmc2",
+		ENABLE_SCLK_TOP1_FSYS0, 24, CLK_SET_RATE_PARENT, 0),
+
+	GATE(CLK_SCLK_MMC1, "sclk_mmc1", "dout_sclk_mmc1",
+		ENABLE_SCLK_TOP1_FSYS1, 24, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_MMC0, "sclk_mmc0", "dout_sclk_mmc0",
+		ENABLE_SCLK_TOP1_FSYS1, 28, CLK_SET_RATE_PARENT, 0),
+};
+
+static struct samsung_fixed_factor_clock top1_fixed_factor_clks[] __initdata = {
+	FFACTOR(0, "ffac_top1_bus0_pll_div2", "mout_top1_bus0_pll", 1, 2, 0),
+	FFACTOR(0, "ffac_top1_bus1_pll_div2", "mout_top1_bus1_pll", 1, 2, 0),
+	FFACTOR(0, "ffac_top1_cc_pll_div2", "mout_top1_cc_pll", 1, 2, 0),
+	FFACTOR(0, "ffac_top1_mfc_pll_div2", "mout_top1_mfc_pll", 1, 2, 0),
+};
+
+static struct samsung_cmu_info top1_cmu_info __initdata = {
+	.mux_clks		= top1_mux_clks,
+	.nr_mux_clks		= ARRAY_SIZE(top1_mux_clks),
+	.div_clks		= top1_div_clks,
+	.nr_div_clks		= ARRAY_SIZE(top1_div_clks),
+	.gate_clks		= top1_gate_clks,
+	.nr_gate_clks		= ARRAY_SIZE(top1_gate_clks),
+	.fixed_factor_clks	= top1_fixed_factor_clks,
+	.nr_fixed_factor_clks	= ARRAY_SIZE(top1_fixed_factor_clks),
+	.nr_clk_ids		= TOP1_NR_CLK,
+	.clk_regs		= top1_clk_regs,
+	.nr_clk_regs		= ARRAY_SIZE(top1_clk_regs),
+};
+
+static void __init exynos7_clk_top1_init(struct device_node *np)
+{
+	samsung_cmu_register_one(np, &top1_cmu_info);
+}
+
+CLK_OF_DECLARE(exynos7_clk_top1, "samsung,exynos7-clock-top1",
+	exynos7_clk_top1_init);
+
+/* Register Offset definitions for CMU_CCORE (0x105B0000) */
+#define MUX_SEL_CCORE			0x0200
+#define DIV_CCORE			0x0600
+#define ENABLE_ACLK_CCORE0		0x0800
+#define ENABLE_ACLK_CCORE1		0x0804
+#define ENABLE_PCLK_CCORE		0x0900
+
+/*
+ * List of parent clocks for Muxes in CMU_CCORE
+ */
+PNAME(mout_aclk_ccore_133_p)	= { "fin_pll", "dout_aclk_ccore_133" };
+
+static unsigned long ccore_clk_regs[] __initdata = {
+	MUX_SEL_CCORE,
+	ENABLE_PCLK_CCORE,
+};
+
+static struct samsung_mux_clock ccore_mux_clks[] __initdata = {
+	MUX(0, "mout_aclk_ccore_133_user", mout_aclk_ccore_133_p,
+		MUX_SEL_CCORE, 1, 1),
+};
+
+static struct samsung_gate_clock ccore_gate_clks[] __initdata = {
+	GATE(PCLK_RTC, "pclk_rtc", "mout_aclk_ccore_133_user",
+		ENABLE_PCLK_CCORE, 8, 0, 0),
+};
+
+static struct samsung_cmu_info ccore_cmu_info __initdata = {
+	.mux_clks		= ccore_mux_clks,
+	.nr_mux_clks		= ARRAY_SIZE(ccore_mux_clks),
+	.gate_clks		= ccore_gate_clks,
+	.nr_gate_clks		= ARRAY_SIZE(ccore_gate_clks),
+	.nr_clk_ids		= CCORE_NR_CLK,
+	.clk_regs		= ccore_clk_regs,
+	.nr_clk_regs		= ARRAY_SIZE(ccore_clk_regs),
+};
+
+static void __init exynos7_clk_ccore_init(struct device_node *np)
+{
+	samsung_cmu_register_one(np, &ccore_cmu_info);
+}
+
+CLK_OF_DECLARE(exynos7_clk_ccore, "samsung,exynos7-clock-ccore",
+	exynos7_clk_ccore_init);
+
+/* Register Offset definitions for CMU_PERIC0 (0x13610000) */
+#define MUX_SEL_PERIC0			0x0200
+#define ENABLE_PCLK_PERIC0		0x0900
+#define ENABLE_SCLK_PERIC0		0x0A00
+
+/* List of parent clocks for Muxes in CMU_PERIC0 */
+PNAME(mout_aclk_peric0_66_p)	= { "fin_pll", "dout_aclk_peric0_66" };
+PNAME(mout_sclk_uart0_p)	= { "fin_pll", "sclk_uart0" };
+
+static unsigned long peric0_clk_regs[] __initdata = {
+	MUX_SEL_PERIC0,
+	ENABLE_PCLK_PERIC0,
+	ENABLE_SCLK_PERIC0,
+};
+
+static struct samsung_mux_clock peric0_mux_clks[] __initdata = {
+	MUX(0, "mout_aclk_peric0_66_user", mout_aclk_peric0_66_p,
+		MUX_SEL_PERIC0, 0, 1),
+	MUX(0, "mout_sclk_uart0_user", mout_sclk_uart0_p,
+		MUX_SEL_PERIC0, 16, 1),
+};
+
+static struct samsung_gate_clock peric0_gate_clks[] __initdata = {
+	GATE(PCLK_HSI2C0, "pclk_hsi2c0", "mout_aclk_peric0_66_user",
+		ENABLE_PCLK_PERIC0, 8, 0, 0),
+	GATE(PCLK_HSI2C1, "pclk_hsi2c1", "mout_aclk_peric0_66_user",
+		ENABLE_PCLK_PERIC0, 9, 0, 0),
+	GATE(PCLK_HSI2C4, "pclk_hsi2c4", "mout_aclk_peric0_66_user",
+		ENABLE_PCLK_PERIC0, 10, 0, 0),
+	GATE(PCLK_HSI2C5, "pclk_hsi2c5", "mout_aclk_peric0_66_user",
+		ENABLE_PCLK_PERIC0, 11, 0, 0),
+	GATE(PCLK_HSI2C9, "pclk_hsi2c9", "mout_aclk_peric0_66_user",
+		ENABLE_PCLK_PERIC0, 12, 0, 0),
+	GATE(PCLK_HSI2C10, "pclk_hsi2c10", "mout_aclk_peric0_66_user",
+		ENABLE_PCLK_PERIC0, 13, 0, 0),
+	GATE(PCLK_HSI2C11, "pclk_hsi2c11", "mout_aclk_peric0_66_user",
+		ENABLE_PCLK_PERIC0, 14, 0, 0),
+	GATE(PCLK_UART0, "pclk_uart0", "mout_aclk_peric0_66_user",
+		ENABLE_PCLK_PERIC0, 16, 0, 0),
+	GATE(PCLK_ADCIF, "pclk_adcif", "mout_aclk_peric0_66_user",
+		ENABLE_PCLK_PERIC0, 20, 0, 0),
+	GATE(PCLK_PWM, "pclk_pwm", "mout_aclk_peric0_66_user",
+		ENABLE_PCLK_PERIC0, 21, 0, 0),
+
+	GATE(SCLK_UART0, "sclk_uart0_user", "mout_sclk_uart0_user",
+		ENABLE_SCLK_PERIC0, 16, 0, 0),
+	GATE(SCLK_PWM, "sclk_pwm", "fin_pll", ENABLE_SCLK_PERIC0, 21, 0, 0),
+};
+
+static struct samsung_cmu_info peric0_cmu_info __initdata = {
+	.mux_clks		= peric0_mux_clks,
+	.nr_mux_clks		= ARRAY_SIZE(peric0_mux_clks),
+	.gate_clks		= peric0_gate_clks,
+	.nr_gate_clks		= ARRAY_SIZE(peric0_gate_clks),
+	.nr_clk_ids		= PERIC0_NR_CLK,
+	.clk_regs		= peric0_clk_regs,
+	.nr_clk_regs		= ARRAY_SIZE(peric0_clk_regs),
+};
+
+static void __init exynos7_clk_peric0_init(struct device_node *np)
+{
+	samsung_cmu_register_one(np, &peric0_cmu_info);
+}
+
+/* Register Offset definitions for CMU_PERIC1 (0x14C80000) */
+#define MUX_SEL_PERIC10			0x0200
+#define MUX_SEL_PERIC11			0x0204
+#define ENABLE_PCLK_PERIC1		0x0900
+#define ENABLE_SCLK_PERIC10		0x0A00
+
+CLK_OF_DECLARE(exynos7_clk_peric0, "samsung,exynos7-clock-peric0",
+	exynos7_clk_peric0_init);
+
+/* List of parent clocks for Muxes in CMU_PERIC1 */
+PNAME(mout_aclk_peric1_66_p)	= { "fin_pll", "dout_aclk_peric1_66" };
+PNAME(mout_sclk_uart1_p)	= { "fin_pll", "sclk_uart1" };
+PNAME(mout_sclk_uart2_p)	= { "fin_pll", "sclk_uart2" };
+PNAME(mout_sclk_uart3_p)	= { "fin_pll", "sclk_uart3" };
+
+static unsigned long peric1_clk_regs[] __initdata = {
+	MUX_SEL_PERIC10,
+	MUX_SEL_PERIC11,
+	ENABLE_PCLK_PERIC1,
+	ENABLE_SCLK_PERIC10,
+};
+
+static struct samsung_mux_clock peric1_mux_clks[] __initdata = {
+	MUX(0, "mout_aclk_peric1_66_user", mout_aclk_peric1_66_p,
+		MUX_SEL_PERIC10, 0, 1),
+
+	MUX(0, "mout_sclk_uart1_user", mout_sclk_uart1_p,
+		MUX_SEL_PERIC11, 20, 1),
+	MUX(0, "mout_sclk_uart2_user", mout_sclk_uart2_p,
+		MUX_SEL_PERIC11, 24, 1),
+	MUX(0, "mout_sclk_uart3_user", mout_sclk_uart3_p,
+		MUX_SEL_PERIC11, 28, 1),
+};
+
+static struct samsung_gate_clock peric1_gate_clks[] __initdata = {
+	GATE(PCLK_HSI2C2, "pclk_hsi2c2", "mout_aclk_peric1_66_user",
+		ENABLE_PCLK_PERIC1, 4, 0, 0),
+	GATE(PCLK_HSI2C3, "pclk_hsi2c3", "mout_aclk_peric1_66_user",
+		ENABLE_PCLK_PERIC1, 5, 0, 0),
+	GATE(PCLK_HSI2C6, "pclk_hsi2c6", "mout_aclk_peric1_66_user",
+		ENABLE_PCLK_PERIC1, 6, 0, 0),
+	GATE(PCLK_HSI2C7, "pclk_hsi2c7", "mout_aclk_peric1_66_user",
+		ENABLE_PCLK_PERIC1, 7, 0, 0),
+	GATE(PCLK_HSI2C8, "pclk_hsi2c8", "mout_aclk_peric1_66_user",
+		ENABLE_PCLK_PERIC1, 8, 0, 0),
+	GATE(PCLK_UART1, "pclk_uart1", "mout_aclk_peric1_66_user",
+		ENABLE_PCLK_PERIC1, 9, 0, 0),
+	GATE(PCLK_UART2, "pclk_uart2", "mout_aclk_peric1_66_user",
+		ENABLE_PCLK_PERIC1, 10, 0, 0),
+	GATE(PCLK_UART3, "pclk_uart3", "mout_aclk_peric1_66_user",
+		ENABLE_PCLK_PERIC1, 11, 0, 0),
+
+	GATE(SCLK_UART1, "sclk_uart1_user", "mout_sclk_uart1_user",
+		ENABLE_SCLK_PERIC10, 9, 0, 0),
+	GATE(SCLK_UART2, "sclk_uart2_user", "mout_sclk_uart2_user",
+		ENABLE_SCLK_PERIC10, 10, 0, 0),
+	GATE(SCLK_UART3, "sclk_uart3_user", "mout_sclk_uart3_user",
+		ENABLE_SCLK_PERIC10, 11, 0, 0),
+};
+
+static struct samsung_cmu_info peric1_cmu_info __initdata = {
+	.mux_clks		= peric1_mux_clks,
+	.nr_mux_clks		= ARRAY_SIZE(peric1_mux_clks),
+	.gate_clks		= peric1_gate_clks,
+	.nr_gate_clks		= ARRAY_SIZE(peric1_gate_clks),
+	.nr_clk_ids		= PERIC1_NR_CLK,
+	.clk_regs		= peric1_clk_regs,
+	.nr_clk_regs		= ARRAY_SIZE(peric1_clk_regs),
+};
+
+static void __init exynos7_clk_peric1_init(struct device_node *np)
+{
+	samsung_cmu_register_one(np, &peric1_cmu_info);
+}
+
+CLK_OF_DECLARE(exynos7_clk_peric1, "samsung,exynos7-clock-peric1",
+	exynos7_clk_peric1_init);
+
+/* Register Offset definitions for CMU_PERIS (0x10040000) */
+#define MUX_SEL_PERIS			0x0200
+#define ENABLE_PCLK_PERIS		0x0900
+#define ENABLE_PCLK_PERIS_SECURE_CHIPID	0x0910
+#define ENABLE_SCLK_PERIS		0x0A00
+#define ENABLE_SCLK_PERIS_SECURE_CHIPID	0x0A10
+
+/* List of parent clocks for Muxes in CMU_PERIS */
+PNAME(mout_aclk_peris_66_p) = { "fin_pll", "dout_aclk_peris_66" };
+
+static unsigned long peris_clk_regs[] __initdata = {
+	MUX_SEL_PERIS,
+	ENABLE_PCLK_PERIS,
+	ENABLE_PCLK_PERIS_SECURE_CHIPID,
+	ENABLE_SCLK_PERIS,
+	ENABLE_SCLK_PERIS_SECURE_CHIPID,
+};
+
+static struct samsung_mux_clock peris_mux_clks[] __initdata = {
+	MUX(0, "mout_aclk_peris_66_user",
+		mout_aclk_peris_66_p, MUX_SEL_PERIS, 0, 1),
+};
+
+static struct samsung_gate_clock peris_gate_clks[] __initdata = {
+	GATE(PCLK_WDT, "pclk_wdt", "mout_aclk_peris_66_user",
+		ENABLE_PCLK_PERIS, 6, 0, 0),
+	GATE(PCLK_TMU, "pclk_tmu_apbif", "mout_aclk_peris_66_user",
+		ENABLE_PCLK_PERIS, 10, 0, 0),
+
+	GATE(PCLK_CHIPID, "pclk_chipid", "mout_aclk_peris_66_user",
+		ENABLE_PCLK_PERIS_SECURE_CHIPID, 0, 0, 0),
+	GATE(SCLK_CHIPID, "sclk_chipid", "fin_pll",
+		ENABLE_SCLK_PERIS_SECURE_CHIPID, 0, 0, 0),
+
+	GATE(SCLK_TMU, "sclk_tmu", "fin_pll", ENABLE_SCLK_PERIS, 10, 0, 0),
+};
+
+static struct samsung_cmu_info peris_cmu_info __initdata = {
+	.mux_clks		= peris_mux_clks,
+	.nr_mux_clks		= ARRAY_SIZE(peris_mux_clks),
+	.gate_clks		= peris_gate_clks,
+	.nr_gate_clks		= ARRAY_SIZE(peris_gate_clks),
+	.nr_clk_ids		= PERIS_NR_CLK,
+	.clk_regs		= peris_clk_regs,
+	.nr_clk_regs		= ARRAY_SIZE(peris_clk_regs),
+};
+
+static void __init exynos7_clk_peris_init(struct device_node *np)
+{
+	samsung_cmu_register_one(np, &peris_cmu_info);
+}
+
+CLK_OF_DECLARE(exynos7_clk_peris, "samsung,exynos7-clock-peris",
+	exynos7_clk_peris_init);
+
+/* Register Offset definitions for CMU_FSYS0 (0x10E90000) */
+#define MUX_SEL_FSYS00			0x0200
+#define MUX_SEL_FSYS01			0x0204
+#define ENABLE_ACLK_FSYS01		0x0804
+
+/*
+ * List of parent clocks for Muxes in CMU_FSYS0
+ */
+PNAME(mout_aclk_fsys0_200_p)	= { "fin_pll", "dout_aclk_fsys0_200" };
+PNAME(mout_sclk_mmc2_p)		= { "fin_pll", "sclk_mmc2" };
+
+static unsigned long fsys0_clk_regs[] __initdata = {
+	MUX_SEL_FSYS00,
+	MUX_SEL_FSYS01,
+	ENABLE_ACLK_FSYS01,
+};
+
+static struct samsung_mux_clock fsys0_mux_clks[] __initdata = {
+	MUX(0, "mout_aclk_fsys0_200_user", mout_aclk_fsys0_200_p,
+		MUX_SEL_FSYS00, 24, 1),
+
+	MUX(0, "mout_sclk_mmc2_user", mout_sclk_mmc2_p, MUX_SEL_FSYS01, 24, 1),
+};
+
+static struct samsung_gate_clock fsys0_gate_clks[] __initdata = {
+	GATE(ACLK_MMC2, "aclk_mmc2", "mout_aclk_fsys0_200_user",
+		ENABLE_ACLK_FSYS01, 31, 0, 0),
+};
+
+static struct samsung_cmu_info fsys0_cmu_info __initdata = {
+	.mux_clks		= fsys0_mux_clks,
+	.nr_mux_clks		= ARRAY_SIZE(fsys0_mux_clks),
+	.gate_clks		= fsys0_gate_clks,
+	.nr_gate_clks		= ARRAY_SIZE(fsys0_gate_clks),
+	.nr_clk_ids		= TOP1_NR_CLK,
+	.clk_regs		= fsys0_clk_regs,
+	.nr_clk_regs		= ARRAY_SIZE(fsys0_clk_regs),
+};
+
+static void __init exynos7_clk_fsys0_init(struct device_node *np)
+{
+	samsung_cmu_register_one(np, &fsys0_cmu_info);
+}
+
+CLK_OF_DECLARE(exynos7_clk_fsys0, "samsung,exynos7-clock-fsys0",
+	exynos7_clk_fsys0_init);
+
+/* Register Offset definitions for CMU_FSYS1 (0x156E0000) */
+#define MUX_SEL_FSYS10			0x0200
+#define MUX_SEL_FSYS11			0x0204
+#define ENABLE_ACLK_FSYS1		0x0800
+
+/*
+ * List of parent clocks for Muxes in CMU_FSYS1
+ */
+PNAME(mout_aclk_fsys1_200_p)	= { "fin_pll",  "dout_aclk_fsys1_200" };
+PNAME(mout_sclk_mmc0_p)		= { "fin_pll", "sclk_mmc0" };
+PNAME(mout_sclk_mmc1_p)		= { "fin_pll", "sclk_mmc1" };
+
+static unsigned long fsys1_clk_regs[] __initdata = {
+	MUX_SEL_FSYS10,
+	MUX_SEL_FSYS11,
+	ENABLE_ACLK_FSYS1,
+};
+
+static struct samsung_mux_clock fsys1_mux_clks[] __initdata = {
+	MUX(0, "mout_aclk_fsys1_200_user", mout_aclk_fsys1_200_p,
+		MUX_SEL_FSYS10, 28, 1),
+
+	MUX(0, "mout_sclk_mmc1_user", mout_sclk_mmc1_p, MUX_SEL_FSYS11, 24, 1),
+	MUX(0, "mout_sclk_mmc0_user", mout_sclk_mmc0_p, MUX_SEL_FSYS11, 28, 1),
+};
+
+static struct samsung_gate_clock fsys1_gate_clks[] __initdata = {
+	GATE(ACLK_MMC1, "aclk_mmc1", "mout_aclk_fsys1_200_user",
+		ENABLE_ACLK_FSYS1, 29, 0, 0),
+	GATE(ACLK_MMC0, "aclk_mmc0", "mout_aclk_fsys1_200_user",
+		ENABLE_ACLK_FSYS1, 30, 0, 0),
+};
+
+static struct samsung_cmu_info fsys1_cmu_info __initdata = {
+	.mux_clks		= fsys1_mux_clks,
+	.nr_mux_clks		= ARRAY_SIZE(fsys1_mux_clks),
+	.gate_clks		= fsys1_gate_clks,
+	.nr_gate_clks		= ARRAY_SIZE(fsys1_gate_clks),
+	.nr_clk_ids		= TOP1_NR_CLK,
+	.clk_regs		= fsys1_clk_regs,
+	.nr_clk_regs		= ARRAY_SIZE(fsys1_clk_regs),
+};
+
+static void __init exynos7_clk_fsys1_init(struct device_node *np)
+{
+	samsung_cmu_register_one(np, &fsys1_cmu_info);
+}
+
+CLK_OF_DECLARE(exynos7_clk_fsys1, "samsung,exynos7-clock-fsys1",
+	exynos7_clk_fsys1_init);
diff --git a/drivers/clk/samsung/clk-pll.c b/drivers/clk/samsung/clk-pll.c
index b07fad2a9167..9d70e5c03804 100644
--- a/drivers/clk/samsung/clk-pll.c
+++ b/drivers/clk/samsung/clk-pll.c
@@ -482,6 +482,8 @@ static const struct clk_ops samsung_pll45xx_clk_min_ops = {
 
 #define PLL46XX_VSEL_MASK	(1)
 #define PLL46XX_MDIV_MASK	(0x1FF)
+#define PLL1460X_MDIV_MASK	(0x3FF)
+
 #define PLL46XX_PDIV_MASK	(0x3F)
 #define PLL46XX_SDIV_MASK	(0x7)
 #define PLL46XX_VSEL_SHIFT	(27)
@@ -511,13 +513,15 @@ static unsigned long samsung_pll46xx_recalc_rate(struct clk_hw *hw,
 
 	pll_con0 = __raw_readl(pll->con_reg);
 	pll_con1 = __raw_readl(pll->con_reg + 4);
-	mdiv = (pll_con0 >> PLL46XX_MDIV_SHIFT) & PLL46XX_MDIV_MASK;
+	mdiv = (pll_con0 >> PLL46XX_MDIV_SHIFT) & ((pll->type == pll_1460x) ?
+				PLL1460X_MDIV_MASK : PLL46XX_MDIV_MASK);
 	pdiv = (pll_con0 >> PLL46XX_PDIV_SHIFT) & PLL46XX_PDIV_MASK;
 	sdiv = (pll_con0 >> PLL46XX_SDIV_SHIFT) & PLL46XX_SDIV_MASK;
 	kdiv = pll->type == pll_4650c ? pll_con1 & PLL4650C_KDIV_MASK :
 					pll_con1 & PLL46XX_KDIV_MASK;
 
-	shift = pll->type == pll_4600 ? 16 : 10;
+	shift = ((pll->type == pll_4600) || (pll->type == pll_1460x)) ? 16 : 10;
+
 	fvco *= (mdiv << shift) + kdiv;
 	do_div(fvco, (pdiv << sdiv));
 	fvco >>= shift;
@@ -573,14 +577,21 @@ static int samsung_pll46xx_set_rate(struct clk_hw *hw, unsigned long drate,
 		lock = 0xffff;
 
 	/* Set PLL PMS and VSEL values. */
-	con0 &= ~((PLL46XX_MDIV_MASK << PLL46XX_MDIV_SHIFT) |
+	if (pll->type == pll_1460x) {
+		con0 &= ~((PLL1460X_MDIV_MASK << PLL46XX_MDIV_SHIFT) |
+			(PLL46XX_PDIV_MASK << PLL46XX_PDIV_SHIFT) |
+			(PLL46XX_SDIV_MASK << PLL46XX_SDIV_SHIFT));
+	} else {
+		con0 &= ~((PLL46XX_MDIV_MASK << PLL46XX_MDIV_SHIFT) |
 			(PLL46XX_PDIV_MASK << PLL46XX_PDIV_SHIFT) |
 			(PLL46XX_SDIV_MASK << PLL46XX_SDIV_SHIFT) |
 			(PLL46XX_VSEL_MASK << PLL46XX_VSEL_SHIFT));
+		con0 |=	rate->vsel << PLL46XX_VSEL_SHIFT;
+	}
+
 	con0 |= (rate->mdiv << PLL46XX_MDIV_SHIFT) |
 			(rate->pdiv << PLL46XX_PDIV_SHIFT) |
-			(rate->sdiv << PLL46XX_SDIV_SHIFT) |
-			(rate->vsel << PLL46XX_VSEL_SHIFT);
+			(rate->sdiv << PLL46XX_SDIV_SHIFT);
 
 	/* Set PLL K, MFR and MRR values. */
 	con1 = __raw_readl(pll->con_reg + 0x4);
@@ -1190,6 +1201,9 @@ static void __init _samsung_clk_register_pll(struct samsung_clk_provider *ctx,
 	/* clk_ops for 35xx and 2550 are similar */
 	case pll_35xx:
 	case pll_2550:
+	case pll_1450x:
+	case pll_1451x:
+	case pll_1452x:
 		if (!pll->rate_table)
 			init.ops = &samsung_pll35xx_clk_min_ops;
 		else
@@ -1223,6 +1237,7 @@ static void __init _samsung_clk_register_pll(struct samsung_clk_provider *ctx,
 	case pll_4600:
 	case pll_4650:
 	case pll_4650c:
+	case pll_1460x:
 		if (!pll->rate_table)
 			init.ops = &samsung_pll46xx_clk_min_ops;
 		else
diff --git a/drivers/clk/samsung/clk-pll.h b/drivers/clk/samsung/clk-pll.h
index c0ed4d41fd90..213de9af8b4f 100644
--- a/drivers/clk/samsung/clk-pll.h
+++ b/drivers/clk/samsung/clk-pll.h
@@ -33,6 +33,10 @@ enum samsung_pll_type {
 	pll_s3c2440_mpll,
 	pll_2550xx,
 	pll_2650xx,
+	pll_1450x,
+	pll_1451x,
+	pll_1452x,
+	pll_1460x,
 };
 
 #define PLL_35XX_RATE(_rate, _m, _p, _s)			\
diff --git a/drivers/clk/samsung/clk-s5pv210-audss.c b/drivers/clk/samsung/clk-s5pv210-audss.c
index a8053b4aca56..de4455b75e8a 100644
--- a/drivers/clk/samsung/clk-s5pv210-audss.c
+++ b/drivers/clk/samsung/clk-s5pv210-audss.c
@@ -216,7 +216,6 @@ static const struct of_device_id s5pv210_audss_clk_of_match[] = {
 static struct platform_driver s5pv210_audss_clk_driver = {
 	.driver	= {
 		.name = "s5pv210-audss-clk",
-		.owner = THIS_MODULE,
 		.of_match_table = s5pv210_audss_clk_of_match,
 	},
 	.probe = s5pv210_audss_clk_probe,
diff --git a/drivers/clk/samsung/clk.c b/drivers/clk/samsung/clk.c
index deab84d9f37d..4bda54095a16 100644
--- a/drivers/clk/samsung/clk.c
+++ b/drivers/clk/samsung/clk.c
@@ -11,9 +11,13 @@
  * clock framework for Samsung platforms.
 */
 
+#include <linux/of_address.h>
 #include <linux/syscore_ops.h>
+
 #include "clk.h"
 
+static LIST_HEAD(clock_reg_cache_list);
+
 void samsung_clk_save(void __iomem *base,
 				    struct samsung_clk_reg_dump *rd,
 				    unsigned int num_regs)
@@ -281,7 +285,6 @@ void __init samsung_clk_register_gate(struct samsung_clk_provider *ctx,
  * obtain the clock speed of all external fixed clock sources from device
  * tree and register it
  */
-#ifdef CONFIG_OF
 void __init samsung_clk_of_register_fixed_ext(struct samsung_clk_provider *ctx,
 			struct samsung_fixed_rate_clock *fixed_rate_clk,
 			unsigned int nr_fixed_rate_clk,
@@ -298,7 +301,6 @@ void __init samsung_clk_of_register_fixed_ext(struct samsung_clk_provider *ctx,
 	}
 	samsung_clk_register_fixed_rate(ctx, fixed_rate_clk, nr_fixed_rate_clk);
 }
-#endif
 
 /* utility function to get the rate of a specified clock */
 unsigned long _get_rate(const char *clk_name)
@@ -313,3 +315,99 @@ unsigned long _get_rate(const char *clk_name)
 
 	return clk_get_rate(clk);
 }
+
+#ifdef CONFIG_PM_SLEEP
+static int samsung_clk_suspend(void)
+{
+	struct samsung_clock_reg_cache *reg_cache;
+
+	list_for_each_entry(reg_cache, &clock_reg_cache_list, node)
+		samsung_clk_save(reg_cache->reg_base, reg_cache->rdump,
+				reg_cache->rd_num);
+	return 0;
+}
+
+static void samsung_clk_resume(void)
+{
+	struct samsung_clock_reg_cache *reg_cache;
+
+	list_for_each_entry(reg_cache, &clock_reg_cache_list, node)
+		samsung_clk_restore(reg_cache->reg_base, reg_cache->rdump,
+				reg_cache->rd_num);
+}
+
+static struct syscore_ops samsung_clk_syscore_ops = {
+	.suspend = samsung_clk_suspend,
+	.resume = samsung_clk_resume,
+};
+
+static void samsung_clk_sleep_init(void __iomem *reg_base,
+		const unsigned long *rdump,
+		unsigned long nr_rdump)
+{
+	struct samsung_clock_reg_cache *reg_cache;
+
+	reg_cache = kzalloc(sizeof(struct samsung_clock_reg_cache),
+			GFP_KERNEL);
+	if (!reg_cache)
+		panic("could not allocate register reg_cache.\n");
+	reg_cache->rdump = samsung_clk_alloc_reg_dump(rdump, nr_rdump);
+
+	if (!reg_cache->rdump)
+		panic("could not allocate register dump storage.\n");
+
+	if (list_empty(&clock_reg_cache_list))
+		register_syscore_ops(&samsung_clk_syscore_ops);
+
+	reg_cache->reg_base = reg_base;
+	reg_cache->rd_num = nr_rdump;
+	list_add_tail(&reg_cache->node, &clock_reg_cache_list);
+}
+
+#else
+static void samsung_clk_sleep_init(void __iomem *reg_base,
+		const unsigned long *rdump,
+		unsigned long nr_rdump) {}
+#endif
+
+/*
+ * Common function which registers plls, muxes, dividers and gates
+ * for each CMU. It also add CMU register list to register cache.
+ */
+void __init samsung_cmu_register_one(struct device_node *np,
+			struct samsung_cmu_info *cmu)
+{
+	void __iomem *reg_base;
+	struct samsung_clk_provider *ctx;
+
+	reg_base = of_iomap(np, 0);
+	if (!reg_base)
+		panic("%s: failed to map registers\n", __func__);
+
+	ctx = samsung_clk_init(np, reg_base, cmu->nr_clk_ids);
+	if (!ctx)
+		panic("%s: unable to alllocate ctx\n", __func__);
+
+	if (cmu->pll_clks)
+		samsung_clk_register_pll(ctx, cmu->pll_clks, cmu->nr_pll_clks,
+			reg_base);
+	if (cmu->mux_clks)
+		samsung_clk_register_mux(ctx, cmu->mux_clks,
+			cmu->nr_mux_clks);
+	if (cmu->div_clks)
+		samsung_clk_register_div(ctx, cmu->div_clks, cmu->nr_div_clks);
+	if (cmu->gate_clks)
+		samsung_clk_register_gate(ctx, cmu->gate_clks,
+			cmu->nr_gate_clks);
+	if (cmu->fixed_clks)
+		samsung_clk_register_fixed_rate(ctx, cmu->fixed_clks,
+			cmu->nr_fixed_clks);
+	if (cmu->fixed_factor_clks)
+		samsung_clk_register_fixed_factor(ctx, cmu->fixed_factor_clks,
+			cmu->nr_fixed_factor_clks);
+	if (cmu->clk_regs)
+		samsung_clk_sleep_init(reg_base, cmu->clk_regs,
+			cmu->nr_clk_regs);
+
+	samsung_clk_of_add_provider(np, ctx);
+}
diff --git a/drivers/clk/samsung/clk.h b/drivers/clk/samsung/clk.h
index 66ab36b5cef1..8acabe1f32c4 100644
--- a/drivers/clk/samsung/clk.h
+++ b/drivers/clk/samsung/clk.h
@@ -13,19 +13,15 @@
 #ifndef __SAMSUNG_CLK_H
 #define __SAMSUNG_CLK_H
 
-#include <linux/clk.h>
 #include <linux/clkdev.h>
-#include <linux/io.h>
 #include <linux/clk-provider.h>
-#include <linux/of.h>
-#include <linux/of_address.h>
 #include "clk-pll.h"
 
 /**
  * struct samsung_clk_provider: information about clock provider
  * @reg_base: virtual address for the register base.
  * @clk_data: holds clock related data like clk* and number of clocks.
- * @lock: maintains exclusion bwtween callbacks for a given clock-provider.
+ * @lock: maintains exclusion between callbacks for a given clock-provider.
  */
 struct samsung_clk_provider {
 	void __iomem *reg_base;
@@ -324,6 +320,40 @@ struct samsung_pll_clock {
 	__PLL(_typ, _id, NULL, _name, _pname, CLK_GET_RATE_NOCACHE,	\
 		_lock, _con, _rtable, _alias)
 
+struct samsung_clock_reg_cache {
+	struct list_head node;
+	void __iomem *reg_base;
+	struct samsung_clk_reg_dump *rdump;
+	unsigned int rd_num;
+};
+
+struct samsung_cmu_info {
+	/* list of pll clocks and respective count */
+	struct samsung_pll_clock *pll_clks;
+	unsigned int nr_pll_clks;
+	/* list of mux clocks and respective count */
+	struct samsung_mux_clock *mux_clks;
+	unsigned int nr_mux_clks;
+	/* list of div clocks and respective count */
+	struct samsung_div_clock *div_clks;
+	unsigned int nr_div_clks;
+	/* list of gate clocks and respective count */
+	struct samsung_gate_clock *gate_clks;
+	unsigned int nr_gate_clks;
+	/* list of fixed clocks and respective count */
+	struct samsung_fixed_rate_clock *fixed_clks;
+	unsigned int nr_fixed_clks;
+	/* list of fixed factor clocks and respective count */
+	struct samsung_fixed_factor_clock *fixed_factor_clks;
+	unsigned int nr_fixed_factor_clks;
+	/* total number of clocks with IDs assigned*/
+	unsigned int nr_clk_ids;
+
+	/* list and number of clocks registers */
+	unsigned long *clk_regs;
+	unsigned int nr_clk_regs;
+};
+
 extern struct samsung_clk_provider *__init samsung_clk_init(
 			struct device_node *np, void __iomem *base,
 			unsigned long nr_clks);
@@ -362,6 +392,9 @@ extern void __init samsung_clk_register_pll(struct samsung_clk_provider *ctx,
 			struct samsung_pll_clock *pll_list,
 			unsigned int nr_clk, void __iomem *base);
 
+extern void __init samsung_cmu_register_one(struct device_node *,
+			struct samsung_cmu_info *);
+
 extern unsigned long _get_rate(const char *clk_name);
 
 extern void samsung_clk_save(void __iomem *base,
diff --git a/drivers/clk/shmobile/clk-div6.c b/drivers/clk/shmobile/clk-div6.c
index f065f694cb65..639241e31e03 100644
--- a/drivers/clk/shmobile/clk-div6.c
+++ b/drivers/clk/shmobile/clk-div6.c
@@ -32,6 +32,9 @@ struct div6_clock {
 	struct clk_hw hw;
 	void __iomem *reg;
 	unsigned int div;
+	u32 src_shift;
+	u32 src_width;
+	u8 *parents;
 };
 
 #define to_div6_clock(_hw) container_of(_hw, struct div6_clock, hw)
@@ -39,8 +42,11 @@ struct div6_clock {
 static int cpg_div6_clock_enable(struct clk_hw *hw)
 {
 	struct div6_clock *clock = to_div6_clock(hw);
+	u32 val;
 
-	clk_writel(CPG_DIV6_DIV(clock->div - 1), clock->reg);
+	val = (clk_readl(clock->reg) & ~(CPG_DIV6_DIV_MASK | CPG_DIV6_CKSTP))
+	    | CPG_DIV6_DIV(clock->div - 1);
+	clk_writel(val, clock->reg);
 
 	return 0;
 }
@@ -52,7 +58,7 @@ static void cpg_div6_clock_disable(struct clk_hw *hw)
 	/* DIV6 clocks require the divisor field to be non-zero when stopping
 	 * the clock.
 	 */
-	clk_writel(CPG_DIV6_CKSTP | CPG_DIV6_DIV(CPG_DIV6_DIV_MASK),
+	clk_writel(clk_readl(clock->reg) | CPG_DIV6_CKSTP | CPG_DIV6_DIV_MASK,
 		   clock->reg);
 }
 
@@ -94,12 +100,53 @@ static int cpg_div6_clock_set_rate(struct clk_hw *hw, unsigned long rate,
 {
 	struct div6_clock *clock = to_div6_clock(hw);
 	unsigned int div = cpg_div6_clock_calc_div(rate, parent_rate);
+	u32 val;
 
 	clock->div = div;
 
+	val = clk_readl(clock->reg) & ~CPG_DIV6_DIV_MASK;
 	/* Only program the new divisor if the clock isn't stopped. */
-	if (!(clk_readl(clock->reg) & CPG_DIV6_CKSTP))
-		clk_writel(CPG_DIV6_DIV(clock->div - 1), clock->reg);
+	if (!(val & CPG_DIV6_CKSTP))
+		clk_writel(val | CPG_DIV6_DIV(clock->div - 1), clock->reg);
+
+	return 0;
+}
+
+static u8 cpg_div6_clock_get_parent(struct clk_hw *hw)
+{
+	struct div6_clock *clock = to_div6_clock(hw);
+	unsigned int i;
+	u8 hw_index;
+
+	if (clock->src_width == 0)
+		return 0;
+
+	hw_index = (clk_readl(clock->reg) >> clock->src_shift) &
+		   (BIT(clock->src_width) - 1);
+	for (i = 0; i < __clk_get_num_parents(hw->clk); i++) {
+		if (clock->parents[i] == hw_index)
+			return i;
+	}
+
+	pr_err("%s: %s DIV6 clock set to invalid parent %u\n",
+	       __func__, __clk_get_name(hw->clk), hw_index);
+	return 0;
+}
+
+static int cpg_div6_clock_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct div6_clock *clock = to_div6_clock(hw);
+	u8 hw_index;
+	u32 mask;
+
+	if (index >= __clk_get_num_parents(hw->clk))
+		return -EINVAL;
+
+	mask = ~((BIT(clock->src_width) - 1) << clock->src_shift);
+	hw_index = clock->parents[index];
+
+	clk_writel((clk_readl(clock->reg) & mask) |
+		(hw_index << clock->src_shift), clock->reg);
 
 	return 0;
 }
@@ -108,6 +155,8 @@ static const struct clk_ops cpg_div6_clock_ops = {
 	.enable = cpg_div6_clock_enable,
 	.disable = cpg_div6_clock_disable,
 	.is_enabled = cpg_div6_clock_is_enabled,
+	.get_parent = cpg_div6_clock_get_parent,
+	.set_parent = cpg_div6_clock_set_parent,
 	.recalc_rate = cpg_div6_clock_recalc_rate,
 	.round_rate = cpg_div6_clock_round_rate,
 	.set_rate = cpg_div6_clock_set_rate,
@@ -115,20 +164,33 @@ static const struct clk_ops cpg_div6_clock_ops = {
 
 static void __init cpg_div6_clock_init(struct device_node *np)
 {
+	unsigned int num_parents, valid_parents;
+	const char **parent_names;
 	struct clk_init_data init;
 	struct div6_clock *clock;
-	const char *parent_name;
 	const char *name;
 	struct clk *clk;
+	unsigned int i;
 	int ret;
 
 	clock = kzalloc(sizeof(*clock), GFP_KERNEL);
-	if (!clock) {
-		pr_err("%s: failed to allocate %s DIV6 clock\n",
+	if (!clock)
+		return;
+
+	num_parents = of_clk_get_parent_count(np);
+	if (num_parents < 1) {
+		pr_err("%s: no parent found for %s DIV6 clock\n",
 		       __func__, np->name);
 		return;
 	}
 
+	clock->parents = kmalloc_array(num_parents, sizeof(*clock->parents),
+		GFP_KERNEL);
+	parent_names = kmalloc_array(num_parents, sizeof(*parent_names),
+				GFP_KERNEL);
+	if (!parent_names)
+		return;
+
 	/* Remap the clock register and read the divisor. Disabling the
 	 * clock overwrites the divisor, so we need to cache its value for the
 	 * enable operation.
@@ -150,9 +212,34 @@ static void __init cpg_div6_clock_init(struct device_node *np)
 		goto error;
 	}
 
-	parent_name = of_clk_get_parent_name(np, 0);
-	if (parent_name == NULL) {
-		pr_err("%s: failed to get %s DIV6 clock parent name\n",
+
+	for (i = 0, valid_parents = 0; i < num_parents; i++) {
+		const char *name = of_clk_get_parent_name(np, i);
+
+		if (name) {
+			parent_names[valid_parents] = name;
+			clock->parents[valid_parents] = i;
+			valid_parents++;
+		}
+	}
+
+	switch (num_parents) {
+	case 1:
+		/* fixed parent clock */
+		clock->src_shift = clock->src_width = 0;
+		break;
+	case 4:
+		/* clock with EXSRC bits 6-7 */
+		clock->src_shift = 6;
+		clock->src_width = 2;
+		break;
+	case 8:
+		/* VCLK with EXSRC bits 12-14 */
+		clock->src_shift = 12;
+		clock->src_width = 3;
+		break;
+	default:
+		pr_err("%s: invalid number of parents for DIV6 clock %s\n",
 		       __func__, np->name);
 		goto error;
 	}
@@ -161,8 +248,8 @@ static void __init cpg_div6_clock_init(struct device_node *np)
 	init.name = name;
 	init.ops = &cpg_div6_clock_ops;
 	init.flags = CLK_IS_BASIC;
-	init.parent_names = &parent_name;
-	init.num_parents = 1;
+	init.parent_names = parent_names;
+	init.num_parents = valid_parents;
 
 	clock->hw.init = &init;
 
@@ -175,11 +262,13 @@ static void __init cpg_div6_clock_init(struct device_node *np)
 
 	of_clk_add_provider(np, of_clk_src_simple_get, clk);
 
+	kfree(parent_names);
 	return;
 
 error:
 	if (clock->reg)
 		iounmap(clock->reg);
+	kfree(parent_names);
 	kfree(clock);
 }
 CLK_OF_DECLARE(cpg_div6_clk, "renesas,cpg-div6-clock", cpg_div6_clock_init);
diff --git a/drivers/clk/sunxi/Makefile b/drivers/clk/sunxi/Makefile
index 7ddc2b553846..a66953c0f430 100644
--- a/drivers/clk/sunxi/Makefile
+++ b/drivers/clk/sunxi/Makefile
@@ -7,6 +7,7 @@ obj-y += clk-a10-hosc.o
 obj-y += clk-a20-gmac.o
 obj-y += clk-mod0.o
 obj-y += clk-sun8i-mbus.o
+obj-y += clk-sun9i-core.o
 
 obj-$(CONFIG_MFD_SUN6I_PRCM) += \
 	clk-sun6i-ar100.o clk-sun6i-apb0.o clk-sun6i-apb0-gates.o \
diff --git a/drivers/clk/sunxi/clk-a20-gmac.c b/drivers/clk/sunxi/clk-a20-gmac.c
index 5296fd6dd7b3..0dcf4f205fb8 100644
--- a/drivers/clk/sunxi/clk-a20-gmac.c
+++ b/drivers/clk/sunxi/clk-a20-gmac.c
@@ -53,6 +53,11 @@ static DEFINE_SPINLOCK(gmac_lock);
 #define SUN7I_A20_GMAC_MASK	0x3
 #define SUN7I_A20_GMAC_PARENTS	2
 
+static u32 sun7i_a20_gmac_mux_table[SUN7I_A20_GMAC_PARENTS] = {
+	0x00, /* Select mii_phy_tx_clk */
+	0x02, /* Select gmac_int_tx_clk */
+};
+
 static void __init sun7i_a20_gmac_clk_setup(struct device_node *node)
 {
 	struct clk *clk;
@@ -90,7 +95,7 @@ static void __init sun7i_a20_gmac_clk_setup(struct device_node *node)
 	gate->lock = &gmac_lock;
 	mux->reg = reg;
 	mux->mask = SUN7I_A20_GMAC_MASK;
-	mux->flags = CLK_MUX_INDEX_BIT;
+	mux->table = sun7i_a20_gmac_mux_table;
 	mux->lock = &gmac_lock;
 
 	clk = clk_register_composite(NULL, clk_name,
diff --git a/drivers/clk/sunxi/clk-factors.c b/drivers/clk/sunxi/clk-factors.c
index f83ba097126c..62e08fb58554 100644
--- a/drivers/clk/sunxi/clk-factors.c
+++ b/drivers/clk/sunxi/clk-factors.c
@@ -81,7 +81,7 @@ static long clk_factors_round_rate(struct clk_hw *hw, unsigned long rate,
 
 static long clk_factors_determine_rate(struct clk_hw *hw, unsigned long rate,
 				       unsigned long *best_parent_rate,
-				       struct clk **best_parent_p)
+				       struct clk_hw **best_parent_p)
 {
 	struct clk *clk = hw->clk, *parent, *best_parent = NULL;
 	int i, num_parents;
@@ -108,7 +108,7 @@ static long clk_factors_determine_rate(struct clk_hw *hw, unsigned long rate,
 	}
 
 	if (best_parent)
-		*best_parent_p = best_parent;
+		*best_parent_p = __clk_get_hw(best_parent);
 	*best_parent_rate = best;
 
 	return best_child_rate;
@@ -224,7 +224,7 @@ struct clk * __init sunxi_factors_register(struct device_node *node,
 		/* set up gate properties */
 		mux->reg = reg;
 		mux->shift = data->mux;
-		mux->mask = SUNXI_FACTORS_MUX_MASK;
+		mux->mask = data->muxmask;
 		mux->lock = factors->lock;
 		mux_hw = &mux->hw;
 	}
diff --git a/drivers/clk/sunxi/clk-factors.h b/drivers/clk/sunxi/clk-factors.h
index 9913840018d3..912238fde132 100644
--- a/drivers/clk/sunxi/clk-factors.h
+++ b/drivers/clk/sunxi/clk-factors.h
@@ -7,8 +7,6 @@
 
 #define SUNXI_FACTORS_NOT_APPLICABLE	(0)
 
-#define SUNXI_FACTORS_MUX_MASK 0x3
-
 struct clk_factors_config {
 	u8 nshift;
 	u8 nwidth;
@@ -24,6 +22,7 @@ struct clk_factors_config {
 struct factors_data {
 	int enable;
 	int mux;
+	int muxmask;
 	struct clk_factors_config *table;
 	void (*getter) (u32 *rate, u32 parent_rate, u8 *n, u8 *k, u8 *m, u8 *p);
 	const char *name;
diff --git a/drivers/clk/sunxi/clk-mod0.c b/drivers/clk/sunxi/clk-mod0.c
index 4a563850ee6e..da0524eaee94 100644
--- a/drivers/clk/sunxi/clk-mod0.c
+++ b/drivers/clk/sunxi/clk-mod0.c
@@ -70,6 +70,7 @@ static struct clk_factors_config sun4i_a10_mod0_config = {
 static const struct factors_data sun4i_a10_mod0_data __initconst = {
 	.enable = 31,
 	.mux = 24,
+	.muxmask = BIT(1) | BIT(0),
 	.table = &sun4i_a10_mod0_config,
 	.getter = sun4i_a10_get_mod0_factors,
 };
diff --git a/drivers/clk/sunxi/clk-sun6i-ar100.c b/drivers/clk/sunxi/clk-sun6i-ar100.c
index acca53290be2..3d282fb8f85c 100644
--- a/drivers/clk/sunxi/clk-sun6i-ar100.c
+++ b/drivers/clk/sunxi/clk-sun6i-ar100.c
@@ -46,7 +46,7 @@ static unsigned long ar100_recalc_rate(struct clk_hw *hw,
 
 static long ar100_determine_rate(struct clk_hw *hw, unsigned long rate,
 				 unsigned long *best_parent_rate,
-				 struct clk **best_parent_clk)
+				 struct clk_hw **best_parent_clk)
 {
 	int nparents = __clk_get_num_parents(hw->clk);
 	long best_rate = -EINVAL;
@@ -100,7 +100,7 @@ static long ar100_determine_rate(struct clk_hw *hw, unsigned long rate,
 
 		tmp_rate = (parent_rate >> shift) / div;
 		if (!*best_parent_clk || tmp_rate > best_rate) {
-			*best_parent_clk = parent;
+			*best_parent_clk = __clk_get_hw(parent);
 			*best_parent_rate = parent_rate;
 			best_rate = tmp_rate;
 		}
diff --git a/drivers/clk/sunxi/clk-sun8i-mbus.c b/drivers/clk/sunxi/clk-sun8i-mbus.c
index 8e49b44cee41..ef49786eefd3 100644
--- a/drivers/clk/sunxi/clk-sun8i-mbus.c
+++ b/drivers/clk/sunxi/clk-sun8i-mbus.c
@@ -60,6 +60,7 @@ static struct clk_factors_config sun8i_a23_mbus_config = {
 static const struct factors_data sun8i_a23_mbus_data __initconst = {
 	.enable = 31,
 	.mux = 24,
+	.muxmask = BIT(1) | BIT(0),
 	.table = &sun8i_a23_mbus_config,
 	.getter = sun8i_a23_get_mbus_factors,
 };
diff --git a/drivers/clk/sunxi/clk-sun9i-core.c b/drivers/clk/sunxi/clk-sun9i-core.c
new file mode 100644
index 000000000000..3cb9036d91bb
--- /dev/null
+++ b/drivers/clk/sunxi/clk-sun9i-core.c
@@ -0,0 +1,271 @@
+/*
+ * Copyright 2014 Chen-Yu Tsai
+ *
+ * Chen-Yu Tsai <wens@csie.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/log2.h>
+
+#include "clk-factors.h"
+
+
+/**
+ * sun9i_a80_get_pll4_factors() - calculates n, p, m factors for PLL1
+ * PLL4 rate is calculated as follows
+ * rate = (parent_rate * n >> p) / (m + 1);
+ * parent_rate is always 24Mhz
+ *
+ * p and m are named div1 and div2 in Allwinner's SDK
+ */
+
+static void sun9i_a80_get_pll4_factors(u32 *freq, u32 parent_rate,
+				       u8 *n, u8 *k, u8 *m, u8 *p)
+{
+	int div;
+
+	/* Normalize value to a 6M multiple */
+	div = DIV_ROUND_UP(*freq, 6000000);
+
+	/* divs above 256 cannot be odd */
+	if (div > 256)
+		div = round_up(div, 2);
+
+	/* divs above 512 must be a multiple of 4 */
+	if (div > 512)
+		div = round_up(div, 4);
+
+	*freq = 6000000 * div;
+
+	/* we were called to round the frequency, we can now return */
+	if (n == NULL)
+		return;
+
+	/* p will be 1 for divs under 512 */
+	if (div < 512)
+		*p = 1;
+	else
+		*p = 0;
+
+	/* m will be 1 if div is odd */
+	if (div & 1)
+		*m = 1;
+	else
+		*m = 0;
+
+	/* calculate a suitable n based on m and p */
+	*n = div / (*p + 1) / (*m + 1);
+}
+
+static struct clk_factors_config sun9i_a80_pll4_config = {
+	.mshift = 18,
+	.mwidth = 1,
+	.nshift = 8,
+	.nwidth = 8,
+	.pshift = 16,
+	.pwidth = 1,
+};
+
+static const struct factors_data sun9i_a80_pll4_data __initconst = {
+	.enable = 31,
+	.table = &sun9i_a80_pll4_config,
+	.getter = sun9i_a80_get_pll4_factors,
+};
+
+static DEFINE_SPINLOCK(sun9i_a80_pll4_lock);
+
+static void __init sun9i_a80_pll4_setup(struct device_node *node)
+{
+	sunxi_factors_register(node, &sun9i_a80_pll4_data, &sun9i_a80_pll4_lock);
+}
+CLK_OF_DECLARE(sun9i_a80_pll4, "allwinner,sun9i-a80-pll4-clk", sun9i_a80_pll4_setup);
+
+
+/**
+ * sun9i_a80_get_gt_factors() - calculates m factor for GT
+ * GT rate is calculated as follows
+ * rate = parent_rate / (m + 1);
+ */
+
+static void sun9i_a80_get_gt_factors(u32 *freq, u32 parent_rate,
+				     u8 *n, u8 *k, u8 *m, u8 *p)
+{
+	u32 div;
+
+	if (parent_rate < *freq)
+		*freq = parent_rate;
+
+	div = DIV_ROUND_UP(parent_rate, *freq);
+
+	/* maximum divider is 4 */
+	if (div > 4)
+		div = 4;
+
+	*freq = parent_rate / div;
+
+	/* we were called to round the frequency, we can now return */
+	if (!m)
+		return;
+
+	*m = div;
+}
+
+static struct clk_factors_config sun9i_a80_gt_config = {
+	.mshift = 0,
+	.mwidth = 2,
+};
+
+static const struct factors_data sun9i_a80_gt_data __initconst = {
+	.mux = 24,
+	.muxmask = BIT(1) | BIT(0),
+	.table = &sun9i_a80_gt_config,
+	.getter = sun9i_a80_get_gt_factors,
+};
+
+static DEFINE_SPINLOCK(sun9i_a80_gt_lock);
+
+static void __init sun9i_a80_gt_setup(struct device_node *node)
+{
+	struct clk *gt = sunxi_factors_register(node, &sun9i_a80_gt_data,
+						&sun9i_a80_gt_lock);
+
+	/* The GT bus clock needs to be always enabled */
+	__clk_get(gt);
+	clk_prepare_enable(gt);
+}
+CLK_OF_DECLARE(sun9i_a80_gt, "allwinner,sun9i-a80-gt-clk", sun9i_a80_gt_setup);
+
+
+/**
+ * sun9i_a80_get_ahb_factors() - calculates p factor for AHB0/1/2
+ * AHB rate is calculated as follows
+ * rate = parent_rate >> p;
+ */
+
+static void sun9i_a80_get_ahb_factors(u32 *freq, u32 parent_rate,
+				      u8 *n, u8 *k, u8 *m, u8 *p)
+{
+	u32 _p;
+
+	if (parent_rate < *freq)
+		*freq = parent_rate;
+
+	_p = order_base_2(DIV_ROUND_UP(parent_rate, *freq));
+
+	/* maximum p is 3 */
+	if (_p > 3)
+		_p = 3;
+
+	*freq = parent_rate >> _p;
+
+	/* we were called to round the frequency, we can now return */
+	if (!p)
+		return;
+
+	*p = _p;
+}
+
+static struct clk_factors_config sun9i_a80_ahb_config = {
+	.pshift = 0,
+	.pwidth = 2,
+};
+
+static const struct factors_data sun9i_a80_ahb_data __initconst = {
+	.mux = 24,
+	.muxmask = BIT(1) | BIT(0),
+	.table = &sun9i_a80_ahb_config,
+	.getter = sun9i_a80_get_ahb_factors,
+};
+
+static DEFINE_SPINLOCK(sun9i_a80_ahb_lock);
+
+static void __init sun9i_a80_ahb_setup(struct device_node *node)
+{
+	sunxi_factors_register(node, &sun9i_a80_ahb_data, &sun9i_a80_ahb_lock);
+}
+CLK_OF_DECLARE(sun9i_a80_ahb, "allwinner,sun9i-a80-ahb-clk", sun9i_a80_ahb_setup);
+
+
+static const struct factors_data sun9i_a80_apb0_data __initconst = {
+	.mux = 24,
+	.muxmask = BIT(0),
+	.table = &sun9i_a80_ahb_config,
+	.getter = sun9i_a80_get_ahb_factors,
+};
+
+static DEFINE_SPINLOCK(sun9i_a80_apb0_lock);
+
+static void __init sun9i_a80_apb0_setup(struct device_node *node)
+{
+	sunxi_factors_register(node, &sun9i_a80_apb0_data, &sun9i_a80_apb0_lock);
+}
+CLK_OF_DECLARE(sun9i_a80_apb0, "allwinner,sun9i-a80-apb0-clk", sun9i_a80_apb0_setup);
+
+
+/**
+ * sun9i_a80_get_apb1_factors() - calculates m, p factors for APB1
+ * APB1 rate is calculated as follows
+ * rate = (parent_rate >> p) / (m + 1);
+ */
+
+static void sun9i_a80_get_apb1_factors(u32 *freq, u32 parent_rate,
+				       u8 *n, u8 *k, u8 *m, u8 *p)
+{
+	u32 div;
+	u8 calcm, calcp;
+
+	if (parent_rate < *freq)
+		*freq = parent_rate;
+
+	div = DIV_ROUND_UP(parent_rate, *freq);
+
+	/* Highest possible divider is 256 (p = 3, m = 31) */
+	if (div > 256)
+		div = 256;
+
+	calcp = order_base_2(div);
+	calcm = (parent_rate >> calcp) - 1;
+	*freq = (parent_rate >> calcp) / (calcm + 1);
+
+	/* we were called to round the frequency, we can now return */
+	if (n == NULL)
+		return;
+
+	*m = calcm;
+	*p = calcp;
+}
+
+static struct clk_factors_config sun9i_a80_apb1_config = {
+	.mshift = 0,
+	.mwidth = 5,
+	.pshift = 16,
+	.pwidth = 2,
+};
+
+static const struct factors_data sun9i_a80_apb1_data __initconst = {
+	.mux = 24,
+	.muxmask = BIT(0),
+	.table = &sun9i_a80_apb1_config,
+	.getter = sun9i_a80_get_apb1_factors,
+};
+
+static DEFINE_SPINLOCK(sun9i_a80_apb1_lock);
+
+static void __init sun9i_a80_apb1_setup(struct device_node *node)
+{
+	sunxi_factors_register(node, &sun9i_a80_apb1_data, &sun9i_a80_apb1_lock);
+}
+CLK_OF_DECLARE(sun9i_a80_apb1, "allwinner,sun9i-a80-apb1-clk", sun9i_a80_apb1_setup);
diff --git a/drivers/clk/sunxi/clk-sunxi.c b/drivers/clk/sunxi/clk-sunxi.c
index d5dc951264ca..570202582dcf 100644
--- a/drivers/clk/sunxi/clk-sunxi.c
+++ b/drivers/clk/sunxi/clk-sunxi.c
@@ -245,9 +245,9 @@ static void sun4i_get_pll5_factors(u32 *freq, u32 parent_rate,
 }
 
 /**
- * sun6i_a31_get_pll6_factors() - calculates n, k factors for A31 PLL6
- * PLL6 rate is calculated as follows
- * rate = parent_rate * n * (k + 1) / 2
+ * sun6i_a31_get_pll6_factors() - calculates n, k factors for A31 PLL6x2
+ * PLL6x2 rate is calculated as follows
+ * rate = parent_rate * (n + 1) * (k + 1)
  * parent_rate is always 24Mhz
  */
 
@@ -256,13 +256,7 @@ static void sun6i_a31_get_pll6_factors(u32 *freq, u32 parent_rate,
 {
 	u8 div;
 
-	/*
-	 * We always have 24MHz / 2, so we can just say that our
-	 * parent clock is 12MHz.
-	 */
-	parent_rate = parent_rate / 2;
-
-	/* Normalize value to a parent_rate multiple (24M / 2) */
+	/* Normalize value to a parent_rate multiple (24M) */
 	div = *freq / parent_rate;
 	*freq = parent_rate * div;
 
@@ -274,7 +268,7 @@ static void sun6i_a31_get_pll6_factors(u32 *freq, u32 parent_rate,
 	if (*k > 3)
 		*k = 3;
 
-	*n = DIV_ROUND_UP(div, (*k+1));
+	*n = DIV_ROUND_UP(div, (*k+1)) - 1;
 }
 
 /**
@@ -445,6 +439,7 @@ static struct clk_factors_config sun6i_a31_pll6_config = {
 	.nwidth = 5,
 	.kshift = 4,
 	.kwidth = 2,
+	.n_start = 1,
 };
 
 static struct clk_factors_config sun4i_apb1_config = {
@@ -504,9 +499,12 @@ static const struct factors_data sun6i_a31_pll6_data __initconst = {
 	.enable = 31,
 	.table = &sun6i_a31_pll6_config,
 	.getter = sun6i_a31_get_pll6_factors,
+	.name = "pll6x2",
 };
 
 static const struct factors_data sun4i_apb1_data __initconst = {
+	.mux = 24,
+	.muxmask = BIT(1) | BIT(0),
 	.table = &sun4i_apb1_config,
 	.getter = sun4i_get_apb1_factors,
 };
@@ -514,6 +512,7 @@ static const struct factors_data sun4i_apb1_data __initconst = {
 static const struct factors_data sun7i_a20_out_data __initconst = {
 	.enable = 31,
 	.mux = 24,
+	.muxmask = BIT(1) | BIT(0),
 	.table = &sun7i_a20_out_config,
 	.getter = sun7i_a20_get_out_factors,
 };
@@ -544,10 +543,6 @@ static const struct mux_data sun6i_a31_ahb1_mux_data __initconst = {
 	.shift = 12,
 };
 
-static const struct mux_data sun4i_apb1_mux_data __initconst = {
-	.shift = 24,
-};
-
 static void __init sunxi_mux_clk_setup(struct device_node *node,
 				       struct mux_data *data)
 {
@@ -633,12 +628,6 @@ static const struct div_data sun4i_apb0_data __initconst = {
 	.table	= sun4i_apb0_table,
 };
 
-static const struct div_data sun6i_a31_apb2_div_data __initconst = {
-	.shift	= 0,
-	.pow	= 0,
-	.width	= 4,
-};
-
 static void __init sunxi_divider_clk_setup(struct device_node *node,
 					   struct div_data *data)
 {
@@ -757,6 +746,18 @@ static const struct gates_data sun8i_a23_ahb1_gates_data __initconst = {
 	.mask = {0x25386742, 0x2505111},
 };
 
+static const struct gates_data sun9i_a80_ahb0_gates_data __initconst = {
+	.mask = {0xF5F12B},
+};
+
+static const struct gates_data sun9i_a80_ahb1_gates_data __initconst = {
+	.mask = {0x1E20003},
+};
+
+static const struct gates_data sun9i_a80_ahb2_gates_data __initconst = {
+	.mask = {0x9B7},
+};
+
 static const struct gates_data sun4i_apb0_gates_data __initconst = {
 	.mask = {0x4EF},
 };
@@ -773,6 +774,10 @@ static const struct gates_data sun7i_a20_apb0_gates_data __initconst = {
 	.mask = { 0x4ff },
 };
 
+static const struct gates_data sun9i_a80_apb0_gates_data __initconst = {
+	.mask = {0xEB822},
+};
+
 static const struct gates_data sun4i_apb1_gates_data __initconst = {
 	.mask = {0xFF00F7},
 };
@@ -801,6 +806,10 @@ static const struct gates_data sun7i_a20_apb1_gates_data __initconst = {
 	.mask = { 0xff80ff },
 };
 
+static const struct gates_data sun9i_a80_apb1_gates_data __initconst = {
+	.mask = {0x3F001F},
+};
+
 static const struct gates_data sun8i_a23_apb2_gates_data __initconst = {
 	.mask = {0x1F0007},
 };
@@ -893,6 +902,7 @@ static void __init sunxi_gates_clk_setup(struct device_node *node,
 
 struct divs_data {
 	const struct factors_data *factors; /* data for the factor clock */
+	int ndivs; /* number of children */
 	struct {
 		u8 fixed; /* is it a fixed divisor? if not... */
 		struct clk_div_table *table; /* is it a table based divisor? */
@@ -912,6 +922,7 @@ static struct clk_div_table pll6_sata_tbl[] = {
 
 static const struct divs_data pll5_divs_data __initconst = {
 	.factors = &sun4i_pll5_data,
+	.ndivs = 2,
 	.div = {
 		{ .shift = 0, .pow = 0, }, /* M, DDR */
 		{ .shift = 16, .pow = 1, }, /* P, other */
@@ -920,12 +931,21 @@ static const struct divs_data pll5_divs_data __initconst = {
 
 static const struct divs_data pll6_divs_data __initconst = {
 	.factors = &sun4i_pll6_data,
+	.ndivs = 2,
 	.div = {
 		{ .shift = 0, .table = pll6_sata_tbl, .gate = 14 }, /* M, SATA */
 		{ .fixed = 2 }, /* P, other */
 	}
 };
 
+static const struct divs_data sun6i_a31_pll6_divs_data __initconst = {
+	.factors = &sun6i_a31_pll6_data,
+	.ndivs = 1,
+	.div = {
+		{ .fixed = 2 }, /* normal output */
+	}
+};
+
 /**
  * sunxi_divs_clk_setup() - Setup function for leaf divisors on clocks
  *
@@ -950,7 +970,7 @@ static void __init sunxi_divs_clk_setup(struct device_node *node,
 	struct clk_fixed_factor *fix_factor;
 	struct clk_divider *divider;
 	void __iomem *reg;
-	int i = 0;
+	int ndivs = SUNXI_DIVS_MAX_QTY, i = 0;
 	int flags, clkflags;
 
 	/* Set up factor clock that we will be dividing */
@@ -973,7 +993,11 @@ static void __init sunxi_divs_clk_setup(struct device_node *node,
 	 * our RAM clock! */
 	clkflags = !strcmp("pll5", parent) ? 0 : CLK_SET_RATE_PARENT;
 
-	for (i = 0; i < SUNXI_DIVS_MAX_QTY; i++) {
+	/* if number of children known, use it */
+	if (data->ndivs)
+		ndivs = data->ndivs;
+
+	for (i = 0; i < ndivs; i++) {
 		if (of_property_read_string_index(node, "clock-output-names",
 						  i, &clk_name) != 0)
 			break;
@@ -1062,7 +1086,6 @@ static const struct of_device_id clk_factors_match[] __initconst = {
 	{.compatible = "allwinner,sun6i-a31-pll1-clk", .data = &sun6i_a31_pll1_data,},
 	{.compatible = "allwinner,sun8i-a23-pll1-clk", .data = &sun8i_a23_pll1_data,},
 	{.compatible = "allwinner,sun7i-a20-pll4-clk", .data = &sun7i_a20_pll4_data,},
-	{.compatible = "allwinner,sun6i-a31-pll6-clk", .data = &sun6i_a31_pll6_data,},
 	{.compatible = "allwinner,sun4i-a10-apb1-clk", .data = &sun4i_apb1_data,},
 	{.compatible = "allwinner,sun7i-a20-out-clk", .data = &sun7i_a20_out_data,},
 	{}
@@ -1074,7 +1097,6 @@ static const struct of_device_id clk_div_match[] __initconst = {
 	{.compatible = "allwinner,sun8i-a23-axi-clk", .data = &sun8i_a23_axi_data,},
 	{.compatible = "allwinner,sun4i-a10-ahb-clk", .data = &sun4i_ahb_data,},
 	{.compatible = "allwinner,sun4i-a10-apb0-clk", .data = &sun4i_apb0_data,},
-	{.compatible = "allwinner,sun6i-a31-apb2-div-clk", .data = &sun6i_a31_apb2_div_data,},
 	{}
 };
 
@@ -1082,13 +1104,13 @@ static const struct of_device_id clk_div_match[] __initconst = {
 static const struct of_device_id clk_divs_match[] __initconst = {
 	{.compatible = "allwinner,sun4i-a10-pll5-clk", .data = &pll5_divs_data,},
 	{.compatible = "allwinner,sun4i-a10-pll6-clk", .data = &pll6_divs_data,},
+	{.compatible = "allwinner,sun6i-a31-pll6-clk", .data = &sun6i_a31_pll6_divs_data,},
 	{}
 };
 
 /* Matches for mux clocks */
 static const struct of_device_id clk_mux_match[] __initconst = {
 	{.compatible = "allwinner,sun4i-a10-cpu-clk", .data = &sun4i_cpu_mux_data,},
-	{.compatible = "allwinner,sun4i-a10-apb1-mux-clk", .data = &sun4i_apb1_mux_data,},
 	{.compatible = "allwinner,sun6i-a31-ahb1-mux-clk", .data = &sun6i_a31_ahb1_mux_data,},
 	{}
 };
@@ -1102,16 +1124,21 @@ static const struct of_device_id clk_gates_match[] __initconst = {
 	{.compatible = "allwinner,sun6i-a31-ahb1-gates-clk", .data = &sun6i_a31_ahb1_gates_data,},
 	{.compatible = "allwinner,sun7i-a20-ahb-gates-clk", .data = &sun7i_a20_ahb_gates_data,},
 	{.compatible = "allwinner,sun8i-a23-ahb1-gates-clk", .data = &sun8i_a23_ahb1_gates_data,},
+	{.compatible = "allwinner,sun9i-a80-ahb0-gates-clk", .data = &sun9i_a80_ahb0_gates_data,},
+	{.compatible = "allwinner,sun9i-a80-ahb1-gates-clk", .data = &sun9i_a80_ahb1_gates_data,},
+	{.compatible = "allwinner,sun9i-a80-ahb2-gates-clk", .data = &sun9i_a80_ahb2_gates_data,},
 	{.compatible = "allwinner,sun4i-a10-apb0-gates-clk", .data = &sun4i_apb0_gates_data,},
 	{.compatible = "allwinner,sun5i-a10s-apb0-gates-clk", .data = &sun5i_a10s_apb0_gates_data,},
 	{.compatible = "allwinner,sun5i-a13-apb0-gates-clk", .data = &sun5i_a13_apb0_gates_data,},
 	{.compatible = "allwinner,sun7i-a20-apb0-gates-clk", .data = &sun7i_a20_apb0_gates_data,},
+	{.compatible = "allwinner,sun9i-a80-apb0-gates-clk", .data = &sun9i_a80_apb0_gates_data,},
 	{.compatible = "allwinner,sun4i-a10-apb1-gates-clk", .data = &sun4i_apb1_gates_data,},
 	{.compatible = "allwinner,sun5i-a10s-apb1-gates-clk", .data = &sun5i_a10s_apb1_gates_data,},
 	{.compatible = "allwinner,sun5i-a13-apb1-gates-clk", .data = &sun5i_a13_apb1_gates_data,},
 	{.compatible = "allwinner,sun6i-a31-apb1-gates-clk", .data = &sun6i_a31_apb1_gates_data,},
 	{.compatible = "allwinner,sun7i-a20-apb1-gates-clk", .data = &sun7i_a20_apb1_gates_data,},
 	{.compatible = "allwinner,sun8i-a23-apb1-gates-clk", .data = &sun8i_a23_apb1_gates_data,},
+	{.compatible = "allwinner,sun9i-a80-apb1-gates-clk", .data = &sun9i_a80_apb1_gates_data,},
 	{.compatible = "allwinner,sun6i-a31-apb2-gates-clk", .data = &sun6i_a31_apb2_gates_data,},
 	{.compatible = "allwinner,sun8i-a23-apb2-gates-clk", .data = &sun8i_a23_apb2_gates_data,},
 	{.compatible = "allwinner,sun4i-a10-usb-clk", .data = &sun4i_a10_usb_gates_data,},
@@ -1200,3 +1227,9 @@ static void __init sun6i_init_clocks(struct device_node *node)
 }
 CLK_OF_DECLARE(sun6i_a31_clk_init, "allwinner,sun6i-a31", sun6i_init_clocks);
 CLK_OF_DECLARE(sun8i_a23_clk_init, "allwinner,sun8i-a23", sun6i_init_clocks);
+
+static void __init sun9i_init_clocks(struct device_node *node)
+{
+	sunxi_init_clocks(NULL, 0);
+}
+CLK_OF_DECLARE(sun9i_a80_clk_init, "allwinner,sun9i-a80", sun9i_init_clocks);
diff --git a/drivers/clk/ux500/abx500-clk.c b/drivers/clk/ux500/abx500-clk.c
index e7bd62cf60b3..3e5e05101302 100644
--- a/drivers/clk/ux500/abx500-clk.c
+++ b/drivers/clk/ux500/abx500-clk.c
@@ -121,7 +121,6 @@ static int abx500_clk_probe(struct platform_device *pdev)
 static struct platform_driver abx500_clk_driver = {
 	.driver = {
 		.name = "abx500-clk",
-		.owner = THIS_MODULE,
 	},
 	.probe	= abx500_clk_probe,
 };
diff --git a/drivers/clk/x86/clk-lpt.c b/drivers/clk/x86/clk-lpt.c
index 812f83f8b0c6..f827083defc4 100644
--- a/drivers/clk/x86/clk-lpt.c
+++ b/drivers/clk/x86/clk-lpt.c
@@ -42,7 +42,6 @@ static int lpt_clk_probe(struct platform_device *pdev)
 static struct platform_driver lpt_clk_driver = {
 	.driver = {
 		.name = "clk-lpt",
-		.owner = THIS_MODULE,
 	},
 	.probe = lpt_clk_probe,
 };
diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index f657a48d20eb..fc01ec27d3c8 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -224,4 +224,9 @@ config CLKSRC_VERSATILE
 	  ARM Versatile, RealView and Versatile Express reference
 	  platforms.
 
+config CLKSRC_MIPS_GIC
+	bool
+	depends on MIPS_GIC
+	select CLKSRC_OF
+
 endmenu
diff --git a/drivers/clocksource/Makefile b/drivers/clocksource/Makefile
index fae0435cc23d..94d90b24b56b 100644
--- a/drivers/clocksource/Makefile
+++ b/drivers/clocksource/Makefile
@@ -47,3 +47,4 @@ obj-$(CONFIG_ARCH_HAS_TICK_BROADCAST)	+= dummy_timer.o
 obj-$(CONFIG_ARCH_KEYSTONE)		+= timer-keystone.o
 obj-$(CONFIG_ARCH_INTEGRATOR_AP)	+= timer-integrator-ap.o
 obj-$(CONFIG_CLKSRC_VERSATILE)		+= versatile.o
+obj-$(CONFIG_CLKSRC_MIPS_GIC)		+= mips-gic-timer.o
diff --git a/drivers/clocksource/mips-gic-timer.c b/drivers/clocksource/mips-gic-timer.c
new file mode 100644
index 000000000000..3bd31b1321f6
--- /dev/null
+++ b/drivers/clocksource/mips-gic-timer.c
@@ -0,0 +1,166 @@
+/*
+ * 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) 2012 MIPS Technologies, Inc.  All rights reserved.
+ */
+#include <linux/clockchips.h>
+#include <linux/cpu.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/irqchip/mips-gic.h>
+#include <linux/notifier.h>
+#include <linux/of_irq.h>
+#include <linux/percpu.h>
+#include <linux/smp.h>
+#include <linux/time.h>
+
+static DEFINE_PER_CPU(struct clock_event_device, gic_clockevent_device);
+static int gic_timer_irq;
+static unsigned int gic_frequency;
+
+static int gic_next_event(unsigned long delta, struct clock_event_device *evt)
+{
+	u64 cnt;
+	int res;
+
+	cnt = gic_read_count();
+	cnt += (u64)delta;
+	gic_write_cpu_compare(cnt, cpumask_first(evt->cpumask));
+	res = ((int)(gic_read_count() - cnt) >= 0) ? -ETIME : 0;
+	return res;
+}
+
+static void gic_set_clock_mode(enum clock_event_mode mode,
+				struct clock_event_device *evt)
+{
+	/* Nothing to do ...  */
+}
+
+static irqreturn_t gic_compare_interrupt(int irq, void *dev_id)
+{
+	struct clock_event_device *cd = dev_id;
+
+	gic_write_compare(gic_read_compare());
+	cd->event_handler(cd);
+	return IRQ_HANDLED;
+}
+
+struct irqaction gic_compare_irqaction = {
+	.handler = gic_compare_interrupt,
+	.percpu_dev_id = &gic_clockevent_device,
+	.flags = IRQF_PERCPU | IRQF_TIMER,
+	.name = "timer",
+};
+
+static void gic_clockevent_cpu_init(struct clock_event_device *cd)
+{
+	unsigned int cpu = smp_processor_id();
+
+	cd->name		= "MIPS GIC";
+	cd->features		= CLOCK_EVT_FEAT_ONESHOT |
+				  CLOCK_EVT_FEAT_C3STOP;
+
+	cd->rating		= 350;
+	cd->irq			= gic_timer_irq;
+	cd->cpumask		= cpumask_of(cpu);
+	cd->set_next_event	= gic_next_event;
+	cd->set_mode		= gic_set_clock_mode;
+
+	clockevents_config_and_register(cd, gic_frequency, 0x300, 0x7fffffff);
+
+	enable_percpu_irq(gic_timer_irq, IRQ_TYPE_NONE);
+}
+
+static void gic_clockevent_cpu_exit(struct clock_event_device *cd)
+{
+	disable_percpu_irq(gic_timer_irq);
+}
+
+static int gic_cpu_notifier(struct notifier_block *nb, unsigned long action,
+				void *data)
+{
+	switch (action & ~CPU_TASKS_FROZEN) {
+	case CPU_STARTING:
+		gic_clockevent_cpu_init(this_cpu_ptr(&gic_clockevent_device));
+		break;
+	case CPU_DYING:
+		gic_clockevent_cpu_exit(this_cpu_ptr(&gic_clockevent_device));
+		break;
+	}
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block gic_cpu_nb = {
+	.notifier_call = gic_cpu_notifier,
+};
+
+static int gic_clockevent_init(void)
+{
+	if (!cpu_has_counter || !gic_frequency)
+		return -ENXIO;
+
+	setup_percpu_irq(gic_timer_irq, &gic_compare_irqaction);
+
+	register_cpu_notifier(&gic_cpu_nb);
+
+	gic_clockevent_cpu_init(this_cpu_ptr(&gic_clockevent_device));
+
+	return 0;
+}
+
+static cycle_t gic_hpt_read(struct clocksource *cs)
+{
+	return gic_read_count();
+}
+
+static struct clocksource gic_clocksource = {
+	.name	= "GIC",
+	.read	= gic_hpt_read,
+	.flags	= CLOCK_SOURCE_IS_CONTINUOUS,
+};
+
+static void __init __gic_clocksource_init(void)
+{
+	/* Set clocksource mask. */
+	gic_clocksource.mask = CLOCKSOURCE_MASK(gic_get_count_width());
+
+	/* Calculate a somewhat reasonable rating value. */
+	gic_clocksource.rating = 200 + gic_frequency / 10000000;
+
+	clocksource_register_hz(&gic_clocksource, gic_frequency);
+
+	gic_clockevent_init();
+}
+
+void __init gic_clocksource_init(unsigned int frequency)
+{
+	gic_frequency = frequency;
+	gic_timer_irq = MIPS_GIC_IRQ_BASE +
+		GIC_LOCAL_TO_HWIRQ(GIC_LOCAL_INT_COMPARE);
+
+	__gic_clocksource_init();
+}
+
+static void __init gic_clocksource_of_init(struct device_node *node)
+{
+	if (WARN_ON(!gic_present || !node->parent ||
+		    !of_device_is_compatible(node->parent, "mti,gic")))
+		return;
+
+	if (of_property_read_u32(node, "clock-frequency", &gic_frequency)) {
+		pr_err("GIC frequency not specified.\n");
+		return;
+	}
+	gic_timer_irq = irq_of_parse_and_map(node, 0);
+	if (!gic_timer_irq) {
+		pr_err("GIC timer IRQ not specified.\n");
+		return;
+	}
+
+	__gic_clocksource_init();
+}
+CLOCKSOURCE_OF_DECLARE(mips_gic_timer, "mti,gic-timer",
+		       gic_clocksource_of_init);
diff --git a/drivers/connector/connector.c b/drivers/connector/connector.c
index f612d68629dc..30f522848c73 100644
--- a/drivers/connector/connector.c
+++ b/drivers/connector/connector.c
@@ -141,12 +141,18 @@ EXPORT_SYMBOL_GPL(cn_netlink_send);
  */
 static int cn_call_callback(struct sk_buff *skb)
 {
+	struct nlmsghdr *nlh;
 	struct cn_callback_entry *i, *cbq = NULL;
 	struct cn_dev *dev = &cdev;
 	struct cn_msg *msg = nlmsg_data(nlmsg_hdr(skb));
 	struct netlink_skb_parms *nsp = &NETLINK_CB(skb);
 	int err = -ENODEV;
 
+	/* verify msg->len is within skb */
+	nlh = nlmsg_hdr(skb);
+	if (nlh->nlmsg_len < NLMSG_HDRLEN + sizeof(struct cn_msg) + msg->len)
+		return -EINVAL;
+
 	spin_lock_bh(&dev->cbdev->queue_lock);
 	list_for_each_entry(i, &dev->cbdev->queue_list, callback_entry) {
 		if (cn_cb_equal(&i->id.id, &msg->id)) {
diff --git a/drivers/coresight/Makefile b/drivers/coresight/Makefile
new file mode 100644
index 000000000000..4b4bec890ef5
--- /dev/null
+++ b/drivers/coresight/Makefile
@@ -0,0 +1,11 @@
+#
+# Makefile for CoreSight drivers.
+#
+obj-$(CONFIG_CORESIGHT) += coresight.o
+obj-$(CONFIG_OF) += of_coresight.o
+obj-$(CONFIG_CORESIGHT_LINK_AND_SINK_TMC) += coresight-tmc.o
+obj-$(CONFIG_CORESIGHT_SINK_TPIU) += coresight-tpiu.o
+obj-$(CONFIG_CORESIGHT_SINK_ETBV10) += coresight-etb10.o
+obj-$(CONFIG_CORESIGHT_LINKS_AND_SINKS) += coresight-funnel.o \
+					   coresight-replicator.o
+obj-$(CONFIG_CORESIGHT_SOURCE_ETM3X) += coresight-etm3x.o coresight-etm-cp14.o
diff --git a/drivers/coresight/coresight-etb10.c b/drivers/coresight/coresight-etb10.c
new file mode 100644
index 000000000000..c922d4aded8a
--- /dev/null
+++ b/drivers/coresight/coresight-etb10.c
@@ -0,0 +1,537 @@
+/* Copyright (c) 2011-2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/uaccess.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/clk.h>
+#include <linux/seq_file.h>
+#include <linux/coresight.h>
+#include <linux/amba/bus.h>
+
+#include "coresight-priv.h"
+
+#define ETB_RAM_DEPTH_REG	0x004
+#define ETB_STATUS_REG		0x00c
+#define ETB_RAM_READ_DATA_REG	0x010
+#define ETB_RAM_READ_POINTER	0x014
+#define ETB_RAM_WRITE_POINTER	0x018
+#define ETB_TRG			0x01c
+#define ETB_CTL_REG		0x020
+#define ETB_RWD_REG		0x024
+#define ETB_FFSR		0x300
+#define ETB_FFCR		0x304
+#define ETB_ITMISCOP0		0xee0
+#define ETB_ITTRFLINACK		0xee4
+#define ETB_ITTRFLIN		0xee8
+#define ETB_ITATBDATA0		0xeeC
+#define ETB_ITATBCTR2		0xef0
+#define ETB_ITATBCTR1		0xef4
+#define ETB_ITATBCTR0		0xef8
+
+/* register description */
+/* STS - 0x00C */
+#define ETB_STATUS_RAM_FULL	BIT(0)
+/* CTL - 0x020 */
+#define ETB_CTL_CAPT_EN		BIT(0)
+/* FFCR - 0x304 */
+#define ETB_FFCR_EN_FTC		BIT(0)
+#define ETB_FFCR_FON_MAN	BIT(6)
+#define ETB_FFCR_STOP_FI	BIT(12)
+#define ETB_FFCR_STOP_TRIGGER	BIT(13)
+
+#define ETB_FFCR_BIT		6
+#define ETB_FFSR_BIT		1
+#define ETB_FRAME_SIZE_WORDS	4
+
+/**
+ * struct etb_drvdata - specifics associated to an ETB component
+ * @base:	memory mapped base address for this component.
+ * @dev:	the device entity associated to this component.
+ * @csdev:	component vitals needed by the framework.
+ * @miscdev:	specifics to handle "/dev/xyz.etb" entry.
+ * @clk:	the clock this component is associated to.
+ * @spinlock:	only one at a time pls.
+ * @in_use:	synchronise user space access to etb buffer.
+ * @buf:	area of memory where ETB buffer content gets sent.
+ * @buffer_depth: size of @buf.
+ * @enable:	this ETB is being used.
+ * @trigger_cntr: amount of words to store after a trigger.
+ */
+struct etb_drvdata {
+	void __iomem		*base;
+	struct device		*dev;
+	struct coresight_device	*csdev;
+	struct miscdevice	miscdev;
+	struct clk		*clk;
+	spinlock_t		spinlock;
+	atomic_t		in_use;
+	u8			*buf;
+	u32			buffer_depth;
+	bool			enable;
+	u32			trigger_cntr;
+};
+
+static unsigned int etb_get_buffer_depth(struct etb_drvdata *drvdata)
+{
+	int ret;
+	u32 depth = 0;
+
+	ret = clk_prepare_enable(drvdata->clk);
+	if (ret)
+		return ret;
+
+	/* RO registers don't need locking */
+	depth = readl_relaxed(drvdata->base + ETB_RAM_DEPTH_REG);
+
+	clk_disable_unprepare(drvdata->clk);
+	return depth;
+}
+
+static void etb_enable_hw(struct etb_drvdata *drvdata)
+{
+	int i;
+	u32 depth;
+
+	CS_UNLOCK(drvdata->base);
+
+	depth = drvdata->buffer_depth;
+	/* reset write RAM pointer address */
+	writel_relaxed(0x0, drvdata->base + ETB_RAM_WRITE_POINTER);
+	/* clear entire RAM buffer */
+	for (i = 0; i < depth; i++)
+		writel_relaxed(0x0, drvdata->base + ETB_RWD_REG);
+
+	/* reset write RAM pointer address */
+	writel_relaxed(0x0, drvdata->base + ETB_RAM_WRITE_POINTER);
+	/* reset read RAM pointer address */
+	writel_relaxed(0x0, drvdata->base + ETB_RAM_READ_POINTER);
+
+	writel_relaxed(drvdata->trigger_cntr, drvdata->base + ETB_TRG);
+	writel_relaxed(ETB_FFCR_EN_FTC | ETB_FFCR_STOP_TRIGGER,
+		       drvdata->base + ETB_FFCR);
+	/* ETB trace capture enable */
+	writel_relaxed(ETB_CTL_CAPT_EN, drvdata->base + ETB_CTL_REG);
+
+	CS_LOCK(drvdata->base);
+}
+
+static int etb_enable(struct coresight_device *csdev)
+{
+	struct etb_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+	int ret;
+	unsigned long flags;
+
+	ret = clk_prepare_enable(drvdata->clk);
+	if (ret)
+		return ret;
+
+	spin_lock_irqsave(&drvdata->spinlock, flags);
+	etb_enable_hw(drvdata);
+	drvdata->enable = true;
+	spin_unlock_irqrestore(&drvdata->spinlock, flags);
+
+	dev_info(drvdata->dev, "ETB enabled\n");
+	return 0;
+}
+
+static void etb_disable_hw(struct etb_drvdata *drvdata)
+{
+	u32 ffcr;
+
+	CS_UNLOCK(drvdata->base);
+
+	ffcr = readl_relaxed(drvdata->base + ETB_FFCR);
+	/* stop formatter when a stop has completed */
+	ffcr |= ETB_FFCR_STOP_FI;
+	writel_relaxed(ffcr, drvdata->base + ETB_FFCR);
+	/* manually generate a flush of the system */
+	ffcr |= ETB_FFCR_FON_MAN;
+	writel_relaxed(ffcr, drvdata->base + ETB_FFCR);
+
+	if (coresight_timeout(drvdata->base, ETB_FFCR, ETB_FFCR_BIT, 0)) {
+		dev_err(drvdata->dev,
+			"timeout observed when probing at offset %#x\n",
+			ETB_FFCR);
+	}
+
+	/* disable trace capture */
+	writel_relaxed(0x0, drvdata->base + ETB_CTL_REG);
+
+	if (coresight_timeout(drvdata->base, ETB_FFSR, ETB_FFSR_BIT, 1)) {
+		dev_err(drvdata->dev,
+			"timeout observed when probing at offset %#x\n",
+			ETB_FFCR);
+	}
+
+	CS_LOCK(drvdata->base);
+}
+
+static void etb_dump_hw(struct etb_drvdata *drvdata)
+{
+	int i;
+	u8 *buf_ptr;
+	u32 read_data, depth;
+	u32 read_ptr, write_ptr;
+	u32 frame_off, frame_endoff;
+
+	CS_UNLOCK(drvdata->base);
+
+	read_ptr = readl_relaxed(drvdata->base + ETB_RAM_READ_POINTER);
+	write_ptr = readl_relaxed(drvdata->base + ETB_RAM_WRITE_POINTER);
+
+	frame_off = write_ptr % ETB_FRAME_SIZE_WORDS;
+	frame_endoff = ETB_FRAME_SIZE_WORDS - frame_off;
+	if (frame_off) {
+		dev_err(drvdata->dev,
+			"write_ptr: %lu not aligned to formatter frame size\n",
+			(unsigned long)write_ptr);
+		dev_err(drvdata->dev, "frameoff: %lu, frame_endoff: %lu\n",
+			(unsigned long)frame_off, (unsigned long)frame_endoff);
+		write_ptr += frame_endoff;
+	}
+
+	if ((readl_relaxed(drvdata->base + ETB_STATUS_REG)
+		      & ETB_STATUS_RAM_FULL) == 0)
+		writel_relaxed(0x0, drvdata->base + ETB_RAM_READ_POINTER);
+	else
+		writel_relaxed(write_ptr, drvdata->base + ETB_RAM_READ_POINTER);
+
+	depth = drvdata->buffer_depth;
+	buf_ptr = drvdata->buf;
+	for (i = 0; i < depth; i++) {
+		read_data = readl_relaxed(drvdata->base +
+					  ETB_RAM_READ_DATA_REG);
+		*buf_ptr++ = read_data >> 0;
+		*buf_ptr++ = read_data >> 8;
+		*buf_ptr++ = read_data >> 16;
+		*buf_ptr++ = read_data >> 24;
+	}
+
+	if (frame_off) {
+		buf_ptr -= (frame_endoff * 4);
+		for (i = 0; i < frame_endoff; i++) {
+			*buf_ptr++ = 0x0;
+			*buf_ptr++ = 0x0;
+			*buf_ptr++ = 0x0;
+			*buf_ptr++ = 0x0;
+		}
+	}
+
+	writel_relaxed(read_ptr, drvdata->base + ETB_RAM_READ_POINTER);
+
+	CS_LOCK(drvdata->base);
+}
+
+static void etb_disable(struct coresight_device *csdev)
+{
+	struct etb_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+	unsigned long flags;
+
+	spin_lock_irqsave(&drvdata->spinlock, flags);
+	etb_disable_hw(drvdata);
+	etb_dump_hw(drvdata);
+	drvdata->enable = false;
+	spin_unlock_irqrestore(&drvdata->spinlock, flags);
+
+	clk_disable_unprepare(drvdata->clk);
+
+	dev_info(drvdata->dev, "ETB disabled\n");
+}
+
+static const struct coresight_ops_sink etb_sink_ops = {
+	.enable		= etb_enable,
+	.disable	= etb_disable,
+};
+
+static const struct coresight_ops etb_cs_ops = {
+	.sink_ops	= &etb_sink_ops,
+};
+
+static void etb_dump(struct etb_drvdata *drvdata)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&drvdata->spinlock, flags);
+	if (drvdata->enable) {
+		etb_disable_hw(drvdata);
+		etb_dump_hw(drvdata);
+		etb_enable_hw(drvdata);
+	}
+	spin_unlock_irqrestore(&drvdata->spinlock, flags);
+
+	dev_info(drvdata->dev, "ETB dumped\n");
+}
+
+static int etb_open(struct inode *inode, struct file *file)
+{
+	struct etb_drvdata *drvdata = container_of(file->private_data,
+						   struct etb_drvdata, miscdev);
+
+	if (atomic_cmpxchg(&drvdata->in_use, 0, 1))
+		return -EBUSY;
+
+	dev_dbg(drvdata->dev, "%s: successfully opened\n", __func__);
+	return 0;
+}
+
+static ssize_t etb_read(struct file *file, char __user *data,
+				size_t len, loff_t *ppos)
+{
+	u32 depth;
+	struct etb_drvdata *drvdata = container_of(file->private_data,
+						   struct etb_drvdata, miscdev);
+
+	etb_dump(drvdata);
+
+	depth = drvdata->buffer_depth;
+	if (*ppos + len > depth * 4)
+		len = depth * 4 - *ppos;
+
+	if (copy_to_user(data, drvdata->buf + *ppos, len)) {
+		dev_dbg(drvdata->dev, "%s: copy_to_user failed\n", __func__);
+		return -EFAULT;
+	}
+
+	*ppos += len;
+
+	dev_dbg(drvdata->dev, "%s: %d bytes copied, %d bytes left\n",
+		__func__, len, (int) (depth * 4 - *ppos));
+	return len;
+}
+
+static int etb_release(struct inode *inode, struct file *file)
+{
+	struct etb_drvdata *drvdata = container_of(file->private_data,
+						   struct etb_drvdata, miscdev);
+	atomic_set(&drvdata->in_use, 0);
+
+	dev_dbg(drvdata->dev, "%s: released\n", __func__);
+	return 0;
+}
+
+static const struct file_operations etb_fops = {
+	.owner		= THIS_MODULE,
+	.open		= etb_open,
+	.read		= etb_read,
+	.release	= etb_release,
+	.llseek		= no_llseek,
+};
+
+static ssize_t status_show(struct device *dev,
+			   struct device_attribute *attr, char *buf)
+{
+	int ret;
+	unsigned long flags;
+	u32 etb_rdr, etb_sr, etb_rrp, etb_rwp;
+	u32 etb_trg, etb_cr, etb_ffsr, etb_ffcr;
+	struct etb_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = clk_prepare_enable(drvdata->clk);
+	if (ret)
+		goto out;
+
+	spin_lock_irqsave(&drvdata->spinlock, flags);
+	CS_UNLOCK(drvdata->base);
+
+	etb_rdr = readl_relaxed(drvdata->base + ETB_RAM_DEPTH_REG);
+	etb_sr = readl_relaxed(drvdata->base + ETB_STATUS_REG);
+	etb_rrp = readl_relaxed(drvdata->base + ETB_RAM_READ_POINTER);
+	etb_rwp = readl_relaxed(drvdata->base + ETB_RAM_WRITE_POINTER);
+	etb_trg = readl_relaxed(drvdata->base + ETB_TRG);
+	etb_cr = readl_relaxed(drvdata->base + ETB_CTL_REG);
+	etb_ffsr = readl_relaxed(drvdata->base + ETB_FFSR);
+	etb_ffcr = readl_relaxed(drvdata->base + ETB_FFCR);
+
+	CS_LOCK(drvdata->base);
+	spin_unlock_irqrestore(&drvdata->spinlock, flags);
+
+	clk_disable_unprepare(drvdata->clk);
+
+	return sprintf(buf,
+		       "Depth:\t\t0x%x\n"
+		       "Status:\t\t0x%x\n"
+		       "RAM read ptr:\t0x%x\n"
+		       "RAM wrt ptr:\t0x%x\n"
+		       "Trigger cnt:\t0x%x\n"
+		       "Control:\t0x%x\n"
+		       "Flush status:\t0x%x\n"
+		       "Flush ctrl:\t0x%x\n",
+		       etb_rdr, etb_sr, etb_rrp, etb_rwp,
+		       etb_trg, etb_cr, etb_ffsr, etb_ffcr);
+out:
+	return -EINVAL;
+}
+static DEVICE_ATTR_RO(status);
+
+static ssize_t trigger_cntr_show(struct device *dev,
+			    struct device_attribute *attr, char *buf)
+{
+	struct etb_drvdata *drvdata = dev_get_drvdata(dev->parent);
+	unsigned long val = drvdata->trigger_cntr;
+
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t trigger_cntr_store(struct device *dev,
+			     struct device_attribute *attr,
+			     const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etb_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	drvdata->trigger_cntr = val;
+	return size;
+}
+static DEVICE_ATTR_RW(trigger_cntr);
+
+static struct attribute *coresight_etb_attrs[] = {
+	&dev_attr_trigger_cntr.attr,
+	&dev_attr_status.attr,
+	NULL,
+};
+ATTRIBUTE_GROUPS(coresight_etb);
+
+static int etb_probe(struct amba_device *adev, const struct amba_id *id)
+{
+	int ret;
+	void __iomem *base;
+	struct device *dev = &adev->dev;
+	struct coresight_platform_data *pdata = NULL;
+	struct etb_drvdata *drvdata;
+	struct resource *res = &adev->res;
+	struct coresight_desc *desc;
+	struct device_node *np = adev->dev.of_node;
+
+	if (np) {
+		pdata = of_get_coresight_platform_data(dev, np);
+		if (IS_ERR(pdata))
+			return PTR_ERR(pdata);
+		adev->dev.platform_data = pdata;
+	}
+
+	drvdata = devm_kzalloc(dev, sizeof(*drvdata), GFP_KERNEL);
+	if (!drvdata)
+		return -ENOMEM;
+
+	drvdata->dev = &adev->dev;
+	dev_set_drvdata(dev, drvdata);
+
+	/* validity for the resource is already checked by the AMBA core */
+	base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	drvdata->base = base;
+
+	spin_lock_init(&drvdata->spinlock);
+
+	drvdata->clk = adev->pclk;
+	ret = clk_prepare_enable(drvdata->clk);
+	if (ret)
+		return ret;
+
+	drvdata->buffer_depth =  etb_get_buffer_depth(drvdata);
+	clk_disable_unprepare(drvdata->clk);
+
+	if (drvdata->buffer_depth < 0)
+		return -EINVAL;
+
+	drvdata->buf = devm_kzalloc(dev,
+				    drvdata->buffer_depth * 4, GFP_KERNEL);
+	if (!drvdata->buf)
+		return -ENOMEM;
+
+	desc = devm_kzalloc(dev, sizeof(*desc), GFP_KERNEL);
+	if (!desc)
+		return -ENOMEM;
+
+	desc->type = CORESIGHT_DEV_TYPE_SINK;
+	desc->subtype.sink_subtype = CORESIGHT_DEV_SUBTYPE_SINK_BUFFER;
+	desc->ops = &etb_cs_ops;
+	desc->pdata = pdata;
+	desc->dev = dev;
+	desc->groups = coresight_etb_groups;
+	drvdata->csdev = coresight_register(desc);
+	if (IS_ERR(drvdata->csdev))
+		return PTR_ERR(drvdata->csdev);
+
+	drvdata->miscdev.name = pdata->name;
+	drvdata->miscdev.minor = MISC_DYNAMIC_MINOR;
+	drvdata->miscdev.fops = &etb_fops;
+	ret = misc_register(&drvdata->miscdev);
+	if (ret)
+		goto err_misc_register;
+
+	dev_info(dev, "ETB initialized\n");
+	return 0;
+
+err_misc_register:
+	coresight_unregister(drvdata->csdev);
+	return ret;
+}
+
+static int etb_remove(struct amba_device *adev)
+{
+	struct etb_drvdata *drvdata = amba_get_drvdata(adev);
+
+	misc_deregister(&drvdata->miscdev);
+	coresight_unregister(drvdata->csdev);
+	return 0;
+}
+
+static struct amba_id etb_ids[] = {
+	{
+		.id	= 0x0003b907,
+		.mask	= 0x0003ffff,
+	},
+	{ 0, 0},
+};
+
+static struct amba_driver etb_driver = {
+	.drv = {
+		.name	= "coresight-etb10",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= etb_probe,
+	.remove		= etb_remove,
+	.id_table	= etb_ids,
+};
+
+static int __init etb_init(void)
+{
+	return amba_driver_register(&etb_driver);
+}
+module_init(etb_init);
+
+static void __exit etb_exit(void)
+{
+	amba_driver_unregister(&etb_driver);
+}
+module_exit(etb_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("CoreSight Embedded Trace Buffer driver");
diff --git a/drivers/coresight/coresight-etm-cp14.c b/drivers/coresight/coresight-etm-cp14.c
new file mode 100644
index 000000000000..12a220682117
--- /dev/null
+++ b/drivers/coresight/coresight-etm-cp14.c
@@ -0,0 +1,591 @@
+/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/bug.h>
+#include <asm/hardware/cp14.h>
+
+#include "coresight-etm.h"
+
+int etm_readl_cp14(u32 reg, unsigned int *val)
+{
+	switch (reg) {
+	case ETMCR:
+		*val = etm_read(ETMCR);
+		return 0;
+	case ETMCCR:
+		*val = etm_read(ETMCCR);
+		return 0;
+	case ETMTRIGGER:
+		*val = etm_read(ETMTRIGGER);
+		return 0;
+	case ETMSR:
+		*val = etm_read(ETMSR);
+		return 0;
+	case ETMSCR:
+		*val = etm_read(ETMSCR);
+		return 0;
+	case ETMTSSCR:
+		*val = etm_read(ETMTSSCR);
+		return 0;
+	case ETMTEEVR:
+		*val = etm_read(ETMTEEVR);
+		return 0;
+	case ETMTECR1:
+		*val = etm_read(ETMTECR1);
+		return 0;
+	case ETMFFLR:
+		*val = etm_read(ETMFFLR);
+		return 0;
+	case ETMACVRn(0):
+		*val = etm_read(ETMACVR0);
+		return 0;
+	case ETMACVRn(1):
+		*val = etm_read(ETMACVR1);
+		return 0;
+	case ETMACVRn(2):
+		*val = etm_read(ETMACVR2);
+		return 0;
+	case ETMACVRn(3):
+		*val = etm_read(ETMACVR3);
+		return 0;
+	case ETMACVRn(4):
+		*val = etm_read(ETMACVR4);
+		return 0;
+	case ETMACVRn(5):
+		*val = etm_read(ETMACVR5);
+		return 0;
+	case ETMACVRn(6):
+		*val = etm_read(ETMACVR6);
+		return 0;
+	case ETMACVRn(7):
+		*val = etm_read(ETMACVR7);
+		return 0;
+	case ETMACVRn(8):
+		*val = etm_read(ETMACVR8);
+		return 0;
+	case ETMACVRn(9):
+		*val = etm_read(ETMACVR9);
+		return 0;
+	case ETMACVRn(10):
+		*val = etm_read(ETMACVR10);
+		return 0;
+	case ETMACVRn(11):
+		*val = etm_read(ETMACVR11);
+		return 0;
+	case ETMACVRn(12):
+		*val = etm_read(ETMACVR12);
+		return 0;
+	case ETMACVRn(13):
+		*val = etm_read(ETMACVR13);
+		return 0;
+	case ETMACVRn(14):
+		*val = etm_read(ETMACVR14);
+		return 0;
+	case ETMACVRn(15):
+		*val = etm_read(ETMACVR15);
+		return 0;
+	case ETMACTRn(0):
+		*val = etm_read(ETMACTR0);
+		return 0;
+	case ETMACTRn(1):
+		*val = etm_read(ETMACTR1);
+		return 0;
+	case ETMACTRn(2):
+		*val = etm_read(ETMACTR2);
+		return 0;
+	case ETMACTRn(3):
+		*val = etm_read(ETMACTR3);
+		return 0;
+	case ETMACTRn(4):
+		*val = etm_read(ETMACTR4);
+		return 0;
+	case ETMACTRn(5):
+		*val = etm_read(ETMACTR5);
+		return 0;
+	case ETMACTRn(6):
+		*val = etm_read(ETMACTR6);
+		return 0;
+	case ETMACTRn(7):
+		*val = etm_read(ETMACTR7);
+		return 0;
+	case ETMACTRn(8):
+		*val = etm_read(ETMACTR8);
+		return 0;
+	case ETMACTRn(9):
+		*val = etm_read(ETMACTR9);
+		return 0;
+	case ETMACTRn(10):
+		*val = etm_read(ETMACTR10);
+		return 0;
+	case ETMACTRn(11):
+		*val = etm_read(ETMACTR11);
+		return 0;
+	case ETMACTRn(12):
+		*val = etm_read(ETMACTR12);
+		return 0;
+	case ETMACTRn(13):
+		*val = etm_read(ETMACTR13);
+		return 0;
+	case ETMACTRn(14):
+		*val = etm_read(ETMACTR14);
+		return 0;
+	case ETMACTRn(15):
+		*val = etm_read(ETMACTR15);
+		return 0;
+	case ETMCNTRLDVRn(0):
+		*val = etm_read(ETMCNTRLDVR0);
+		return 0;
+	case ETMCNTRLDVRn(1):
+		*val = etm_read(ETMCNTRLDVR1);
+		return 0;
+	case ETMCNTRLDVRn(2):
+		*val = etm_read(ETMCNTRLDVR2);
+		return 0;
+	case ETMCNTRLDVRn(3):
+		*val = etm_read(ETMCNTRLDVR3);
+		return 0;
+	case ETMCNTENRn(0):
+		*val = etm_read(ETMCNTENR0);
+		return 0;
+	case ETMCNTENRn(1):
+		*val = etm_read(ETMCNTENR1);
+		return 0;
+	case ETMCNTENRn(2):
+		*val = etm_read(ETMCNTENR2);
+		return 0;
+	case ETMCNTENRn(3):
+		*val = etm_read(ETMCNTENR3);
+		return 0;
+	case ETMCNTRLDEVRn(0):
+		*val = etm_read(ETMCNTRLDEVR0);
+		return 0;
+	case ETMCNTRLDEVRn(1):
+		*val = etm_read(ETMCNTRLDEVR1);
+		return 0;
+	case ETMCNTRLDEVRn(2):
+		*val = etm_read(ETMCNTRLDEVR2);
+		return 0;
+	case ETMCNTRLDEVRn(3):
+		*val = etm_read(ETMCNTRLDEVR3);
+		return 0;
+	case ETMCNTVRn(0):
+		*val = etm_read(ETMCNTVR0);
+		return 0;
+	case ETMCNTVRn(1):
+		*val = etm_read(ETMCNTVR1);
+		return 0;
+	case ETMCNTVRn(2):
+		*val = etm_read(ETMCNTVR2);
+		return 0;
+	case ETMCNTVRn(3):
+		*val = etm_read(ETMCNTVR3);
+		return 0;
+	case ETMSQ12EVR:
+		*val = etm_read(ETMSQ12EVR);
+		return 0;
+	case ETMSQ21EVR:
+		*val = etm_read(ETMSQ21EVR);
+		return 0;
+	case ETMSQ23EVR:
+		*val = etm_read(ETMSQ23EVR);
+		return 0;
+	case ETMSQ31EVR:
+		*val = etm_read(ETMSQ31EVR);
+		return 0;
+	case ETMSQ32EVR:
+		*val = etm_read(ETMSQ32EVR);
+		return 0;
+	case ETMSQ13EVR:
+		*val = etm_read(ETMSQ13EVR);
+		return 0;
+	case ETMSQR:
+		*val = etm_read(ETMSQR);
+		return 0;
+	case ETMEXTOUTEVRn(0):
+		*val = etm_read(ETMEXTOUTEVR0);
+		return 0;
+	case ETMEXTOUTEVRn(1):
+		*val = etm_read(ETMEXTOUTEVR1);
+		return 0;
+	case ETMEXTOUTEVRn(2):
+		*val = etm_read(ETMEXTOUTEVR2);
+		return 0;
+	case ETMEXTOUTEVRn(3):
+		*val = etm_read(ETMEXTOUTEVR3);
+		return 0;
+	case ETMCIDCVRn(0):
+		*val = etm_read(ETMCIDCVR0);
+		return 0;
+	case ETMCIDCVRn(1):
+		*val = etm_read(ETMCIDCVR1);
+		return 0;
+	case ETMCIDCVRn(2):
+		*val = etm_read(ETMCIDCVR2);
+		return 0;
+	case ETMCIDCMR:
+		*val = etm_read(ETMCIDCMR);
+		return 0;
+	case ETMIMPSPEC0:
+		*val = etm_read(ETMIMPSPEC0);
+		return 0;
+	case ETMIMPSPEC1:
+		*val = etm_read(ETMIMPSPEC1);
+		return 0;
+	case ETMIMPSPEC2:
+		*val = etm_read(ETMIMPSPEC2);
+		return 0;
+	case ETMIMPSPEC3:
+		*val = etm_read(ETMIMPSPEC3);
+		return 0;
+	case ETMIMPSPEC4:
+		*val = etm_read(ETMIMPSPEC4);
+		return 0;
+	case ETMIMPSPEC5:
+		*val = etm_read(ETMIMPSPEC5);
+		return 0;
+	case ETMIMPSPEC6:
+		*val = etm_read(ETMIMPSPEC6);
+		return 0;
+	case ETMIMPSPEC7:
+		*val = etm_read(ETMIMPSPEC7);
+		return 0;
+	case ETMSYNCFR:
+		*val = etm_read(ETMSYNCFR);
+		return 0;
+	case ETMIDR:
+		*val = etm_read(ETMIDR);
+		return 0;
+	case ETMCCER:
+		*val = etm_read(ETMCCER);
+		return 0;
+	case ETMEXTINSELR:
+		*val = etm_read(ETMEXTINSELR);
+		return 0;
+	case ETMTESSEICR:
+		*val = etm_read(ETMTESSEICR);
+		return 0;
+	case ETMEIBCR:
+		*val = etm_read(ETMEIBCR);
+		return 0;
+	case ETMTSEVR:
+		*val = etm_read(ETMTSEVR);
+		return 0;
+	case ETMAUXCR:
+		*val = etm_read(ETMAUXCR);
+		return 0;
+	case ETMTRACEIDR:
+		*val = etm_read(ETMTRACEIDR);
+		return 0;
+	case ETMVMIDCVR:
+		*val = etm_read(ETMVMIDCVR);
+		return 0;
+	case ETMOSLSR:
+		*val = etm_read(ETMOSLSR);
+		return 0;
+	case ETMOSSRR:
+		*val = etm_read(ETMOSSRR);
+		return 0;
+	case ETMPDCR:
+		*val = etm_read(ETMPDCR);
+		return 0;
+	case ETMPDSR:
+		*val = etm_read(ETMPDSR);
+		return 0;
+	default:
+		*val = 0;
+		return -EINVAL;
+	}
+}
+
+int etm_writel_cp14(u32 reg, u32 val)
+{
+	switch (reg) {
+	case ETMCR:
+		etm_write(val, ETMCR);
+		break;
+	case ETMTRIGGER:
+		etm_write(val, ETMTRIGGER);
+		break;
+	case ETMSR:
+		etm_write(val, ETMSR);
+		break;
+	case ETMTSSCR:
+		etm_write(val, ETMTSSCR);
+		break;
+	case ETMTEEVR:
+		etm_write(val, ETMTEEVR);
+		break;
+	case ETMTECR1:
+		etm_write(val, ETMTECR1);
+		break;
+	case ETMFFLR:
+		etm_write(val, ETMFFLR);
+		break;
+	case ETMACVRn(0):
+		etm_write(val, ETMACVR0);
+		break;
+	case ETMACVRn(1):
+		etm_write(val, ETMACVR1);
+		break;
+	case ETMACVRn(2):
+		etm_write(val, ETMACVR2);
+		break;
+	case ETMACVRn(3):
+		etm_write(val, ETMACVR3);
+		break;
+	case ETMACVRn(4):
+		etm_write(val, ETMACVR4);
+		break;
+	case ETMACVRn(5):
+		etm_write(val, ETMACVR5);
+		break;
+	case ETMACVRn(6):
+		etm_write(val, ETMACVR6);
+		break;
+	case ETMACVRn(7):
+		etm_write(val, ETMACVR7);
+		break;
+	case ETMACVRn(8):
+		etm_write(val, ETMACVR8);
+		break;
+	case ETMACVRn(9):
+		etm_write(val, ETMACVR9);
+		break;
+	case ETMACVRn(10):
+		etm_write(val, ETMACVR10);
+		break;
+	case ETMACVRn(11):
+		etm_write(val, ETMACVR11);
+		break;
+	case ETMACVRn(12):
+		etm_write(val, ETMACVR12);
+		break;
+	case ETMACVRn(13):
+		etm_write(val, ETMACVR13);
+		break;
+	case ETMACVRn(14):
+		etm_write(val, ETMACVR14);
+		break;
+	case ETMACVRn(15):
+		etm_write(val, ETMACVR15);
+		break;
+	case ETMACTRn(0):
+		etm_write(val, ETMACTR0);
+		break;
+	case ETMACTRn(1):
+		etm_write(val, ETMACTR1);
+		break;
+	case ETMACTRn(2):
+		etm_write(val, ETMACTR2);
+		break;
+	case ETMACTRn(3):
+		etm_write(val, ETMACTR3);
+		break;
+	case ETMACTRn(4):
+		etm_write(val, ETMACTR4);
+		break;
+	case ETMACTRn(5):
+		etm_write(val, ETMACTR5);
+		break;
+	case ETMACTRn(6):
+		etm_write(val, ETMACTR6);
+		break;
+	case ETMACTRn(7):
+		etm_write(val, ETMACTR7);
+		break;
+	case ETMACTRn(8):
+		etm_write(val, ETMACTR8);
+		break;
+	case ETMACTRn(9):
+		etm_write(val, ETMACTR9);
+		break;
+	case ETMACTRn(10):
+		etm_write(val, ETMACTR10);
+		break;
+	case ETMACTRn(11):
+		etm_write(val, ETMACTR11);
+		break;
+	case ETMACTRn(12):
+		etm_write(val, ETMACTR12);
+		break;
+	case ETMACTRn(13):
+		etm_write(val, ETMACTR13);
+		break;
+	case ETMACTRn(14):
+		etm_write(val, ETMACTR14);
+		break;
+	case ETMACTRn(15):
+		etm_write(val, ETMACTR15);
+		break;
+	case ETMCNTRLDVRn(0):
+		etm_write(val, ETMCNTRLDVR0);
+		break;
+	case ETMCNTRLDVRn(1):
+		etm_write(val, ETMCNTRLDVR1);
+		break;
+	case ETMCNTRLDVRn(2):
+		etm_write(val, ETMCNTRLDVR2);
+		break;
+	case ETMCNTRLDVRn(3):
+		etm_write(val, ETMCNTRLDVR3);
+		break;
+	case ETMCNTENRn(0):
+		etm_write(val, ETMCNTENR0);
+		break;
+	case ETMCNTENRn(1):
+		etm_write(val, ETMCNTENR1);
+		break;
+	case ETMCNTENRn(2):
+		etm_write(val, ETMCNTENR2);
+		break;
+	case ETMCNTENRn(3):
+		etm_write(val, ETMCNTENR3);
+		break;
+	case ETMCNTRLDEVRn(0):
+		etm_write(val, ETMCNTRLDEVR0);
+		break;
+	case ETMCNTRLDEVRn(1):
+		etm_write(val, ETMCNTRLDEVR1);
+		break;
+	case ETMCNTRLDEVRn(2):
+		etm_write(val, ETMCNTRLDEVR2);
+		break;
+	case ETMCNTRLDEVRn(3):
+		etm_write(val, ETMCNTRLDEVR3);
+		break;
+	case ETMCNTVRn(0):
+		etm_write(val, ETMCNTVR0);
+		break;
+	case ETMCNTVRn(1):
+		etm_write(val, ETMCNTVR1);
+		break;
+	case ETMCNTVRn(2):
+		etm_write(val, ETMCNTVR2);
+		break;
+	case ETMCNTVRn(3):
+		etm_write(val, ETMCNTVR3);
+		break;
+	case ETMSQ12EVR:
+		etm_write(val, ETMSQ12EVR);
+		break;
+	case ETMSQ21EVR:
+		etm_write(val, ETMSQ21EVR);
+		break;
+	case ETMSQ23EVR:
+		etm_write(val, ETMSQ23EVR);
+		break;
+	case ETMSQ31EVR:
+		etm_write(val, ETMSQ31EVR);
+		break;
+	case ETMSQ32EVR:
+		etm_write(val, ETMSQ32EVR);
+		break;
+	case ETMSQ13EVR:
+		etm_write(val, ETMSQ13EVR);
+		break;
+	case ETMSQR:
+		etm_write(val, ETMSQR);
+		break;
+	case ETMEXTOUTEVRn(0):
+		etm_write(val, ETMEXTOUTEVR0);
+		break;
+	case ETMEXTOUTEVRn(1):
+		etm_write(val, ETMEXTOUTEVR1);
+		break;
+	case ETMEXTOUTEVRn(2):
+		etm_write(val, ETMEXTOUTEVR2);
+		break;
+	case ETMEXTOUTEVRn(3):
+		etm_write(val, ETMEXTOUTEVR3);
+		break;
+	case ETMCIDCVRn(0):
+		etm_write(val, ETMCIDCVR0);
+		break;
+	case ETMCIDCVRn(1):
+		etm_write(val, ETMCIDCVR1);
+		break;
+	case ETMCIDCVRn(2):
+		etm_write(val, ETMCIDCVR2);
+		break;
+	case ETMCIDCMR:
+		etm_write(val, ETMCIDCMR);
+		break;
+	case ETMIMPSPEC0:
+		etm_write(val, ETMIMPSPEC0);
+		break;
+	case ETMIMPSPEC1:
+		etm_write(val, ETMIMPSPEC1);
+		break;
+	case ETMIMPSPEC2:
+		etm_write(val, ETMIMPSPEC2);
+		break;
+	case ETMIMPSPEC3:
+		etm_write(val, ETMIMPSPEC3);
+		break;
+	case ETMIMPSPEC4:
+		etm_write(val, ETMIMPSPEC4);
+		break;
+	case ETMIMPSPEC5:
+		etm_write(val, ETMIMPSPEC5);
+		break;
+	case ETMIMPSPEC6:
+		etm_write(val, ETMIMPSPEC6);
+		break;
+	case ETMIMPSPEC7:
+		etm_write(val, ETMIMPSPEC7);
+		break;
+	case ETMSYNCFR:
+		etm_write(val, ETMSYNCFR);
+		break;
+	case ETMEXTINSELR:
+		etm_write(val, ETMEXTINSELR);
+		break;
+	case ETMTESSEICR:
+		etm_write(val, ETMTESSEICR);
+		break;
+	case ETMEIBCR:
+		etm_write(val, ETMEIBCR);
+		break;
+	case ETMTSEVR:
+		etm_write(val, ETMTSEVR);
+		break;
+	case ETMAUXCR:
+		etm_write(val, ETMAUXCR);
+		break;
+	case ETMTRACEIDR:
+		etm_write(val, ETMTRACEIDR);
+		break;
+	case ETMVMIDCVR:
+		etm_write(val, ETMVMIDCVR);
+		break;
+	case ETMOSLAR:
+		etm_write(val, ETMOSLAR);
+		break;
+	case ETMOSSRR:
+		etm_write(val, ETMOSSRR);
+		break;
+	case ETMPDCR:
+		etm_write(val, ETMPDCR);
+		break;
+	case ETMPDSR:
+		etm_write(val, ETMPDSR);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
diff --git a/drivers/coresight/coresight-etm.h b/drivers/coresight/coresight-etm.h
new file mode 100644
index 000000000000..501c5fac8a45
--- /dev/null
+++ b/drivers/coresight/coresight-etm.h
@@ -0,0 +1,251 @@
+/* Copyright (c) 2014-2015, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef _CORESIGHT_CORESIGHT_ETM_H
+#define _CORESIGHT_CORESIGHT_ETM_H
+
+#include <linux/spinlock.h>
+#include "coresight-priv.h"
+
+/*
+ * Device registers:
+ * 0x000 - 0x2FC: Trace         registers
+ * 0x300 - 0x314: Management    registers
+ * 0x318 - 0xEFC: Trace         registers
+ *
+ * Coresight registers
+ * 0xF00 - 0xF9C: Management    registers
+ * 0xFA0 - 0xFA4: Management    registers in PFTv1.0
+ *                Trace         registers in PFTv1.1
+ * 0xFA8 - 0xFFC: Management    registers
+ */
+
+/* Trace registers (0x000-0x2FC) */
+#define ETMCR			0x000
+#define ETMCCR			0x004
+#define ETMTRIGGER		0x008
+#define ETMSR			0x010
+#define ETMSCR			0x014
+#define ETMTSSCR		0x018
+#define ETMTECR2		0x01c
+#define ETMTEEVR		0x020
+#define ETMTECR1		0x024
+#define ETMFFLR			0x02c
+#define ETMACVRn(n)		(0x040 + (n * 4))
+#define ETMACTRn(n)		(0x080 + (n * 4))
+#define ETMCNTRLDVRn(n)		(0x140 + (n * 4))
+#define ETMCNTENRn(n)		(0x150 + (n * 4))
+#define ETMCNTRLDEVRn(n)	(0x160 + (n * 4))
+#define ETMCNTVRn(n)		(0x170 + (n * 4))
+#define ETMSQ12EVR		0x180
+#define ETMSQ21EVR		0x184
+#define ETMSQ23EVR		0x188
+#define ETMSQ31EVR		0x18c
+#define ETMSQ32EVR		0x190
+#define ETMSQ13EVR		0x194
+#define ETMSQR			0x19c
+#define ETMEXTOUTEVRn(n)	(0x1a0 + (n * 4))
+#define ETMCIDCVRn(n)		(0x1b0 + (n * 4))
+#define ETMCIDCMR		0x1bc
+#define ETMIMPSPEC0		0x1c0
+#define ETMIMPSPEC1		0x1c4
+#define ETMIMPSPEC2		0x1c8
+#define ETMIMPSPEC3		0x1cc
+#define ETMIMPSPEC4		0x1d0
+#define ETMIMPSPEC5		0x1d4
+#define ETMIMPSPEC6		0x1d8
+#define ETMIMPSPEC7		0x1dc
+#define ETMSYNCFR		0x1e0
+#define ETMIDR			0x1e4
+#define ETMCCER			0x1e8
+#define ETMEXTINSELR		0x1ec
+#define ETMTESSEICR		0x1f0
+#define ETMEIBCR		0x1f4
+#define ETMTSEVR		0x1f8
+#define ETMAUXCR		0x1fc
+#define ETMTRACEIDR		0x200
+#define ETMVMIDCVR		0x240
+/* Management registers (0x300-0x314) */
+#define ETMOSLAR		0x300
+#define ETMOSLSR		0x304
+#define ETMOSSRR		0x308
+#define ETMPDCR			0x310
+#define ETMPDSR			0x314
+#define ETM_MAX_ADDR_CMP	16
+#define ETM_MAX_CNTR		4
+#define ETM_MAX_CTXID_CMP	3
+
+/* Register definition */
+/* ETMCR - 0x00 */
+#define ETMCR_PWD_DWN		BIT(0)
+#define ETMCR_STALL_MODE	BIT(7)
+#define ETMCR_ETM_PRG		BIT(10)
+#define ETMCR_ETM_EN		BIT(11)
+#define ETMCR_CYC_ACC		BIT(12)
+#define ETMCR_CTXID_SIZE	(BIT(14)|BIT(15))
+#define ETMCR_TIMESTAMP_EN	BIT(28)
+/* ETMCCR - 0x04 */
+#define ETMCCR_FIFOFULL		BIT(23)
+/* ETMPDCR - 0x310 */
+#define ETMPDCR_PWD_UP		BIT(3)
+/* ETMTECR1 - 0x024 */
+#define ETMTECR1_ADDR_COMP_1	BIT(0)
+#define ETMTECR1_INC_EXC	BIT(24)
+#define ETMTECR1_START_STOP	BIT(25)
+/* ETMCCER - 0x1E8 */
+#define ETMCCER_TIMESTAMP	BIT(22)
+
+#define ETM_MODE_EXCLUDE	BIT(0)
+#define ETM_MODE_CYCACC		BIT(1)
+#define ETM_MODE_STALL		BIT(2)
+#define ETM_MODE_TIMESTAMP	BIT(3)
+#define ETM_MODE_CTXID		BIT(4)
+#define ETM_MODE_ALL		0x1f
+
+#define ETM_SQR_MASK		0x3
+#define ETM_TRACEID_MASK	0x3f
+#define ETM_EVENT_MASK		0x1ffff
+#define ETM_SYNC_MASK		0xfff
+#define ETM_ALL_MASK		0xffffffff
+
+#define ETMSR_PROG_BIT		1
+#define ETM_SEQ_STATE_MAX_VAL	(0x2)
+#define PORT_SIZE_MASK		(GENMASK(21, 21) | GENMASK(6, 4))
+
+#define ETM_HARD_WIRE_RES_A	/* Hard wired, always true */	\
+				((0x0f << 0)	|		\
+				/* Resource index A */		\
+				(0x06 << 4))
+
+#define ETM_ADD_COMP_0		/* Single addr comparator 1 */	\
+				((0x00 << 7)	|		\
+				/* Resource index B */		\
+				(0x00 << 11))
+
+#define ETM_EVENT_NOT_A		BIT(14) /* NOT(A) */
+
+#define ETM_DEFAULT_EVENT_VAL	(ETM_HARD_WIRE_RES_A	|	\
+				 ETM_ADD_COMP_0		|	\
+				 ETM_EVENT_NOT_A)
+/**
+ * struct etm_drvdata - specifics associated to an ETM component
+ * @base:	memory mapped base address for this component.
+ * @dev:	the device entity associated to this component.
+ * @csdev:	component vitals needed by the framework.
+ * @clk:	the clock this component is associated to.
+ * @spinlock:	only one at a time pls.
+ * @cpu:	the cpu this component is affined to.
+ * @port_size:	port size as reported by ETMCR bit 4-6 and 21.
+ * @arch:	ETM/PTM version number.
+ * @use_cpu14:	true if management registers need to be accessed via CP14.
+ * @enable:	is this ETM/PTM currently tracing.
+ * @sticky_enable: true if ETM base configuration has been done.
+ * @boot_enable:true if we should start tracing at boot time.
+ * @os_unlock:	true if access to management registers is allowed.
+ * @nr_addr_cmp:Number of pairs of address comparators as found in ETMCCR.
+ * @nr_cntr:	Number of counters as found in ETMCCR bit 13-15.
+ * @nr_ext_inp:	Number of external input as found in ETMCCR bit 17-19.
+ * @nr_ext_out:	Number of external output as found in ETMCCR bit 20-22.
+ * @nr_ctxid_cmp: Number of contextID comparators as found in ETMCCR bit 24-25.
+ * @etmccr:	value of register ETMCCR.
+ * @etmccer:	value of register ETMCCER.
+ * @traceid:	value of the current ID for this component.
+ * @mode:	controls various modes supported by this ETM/PTM.
+ * @ctrl:	used in conjunction with @mode.
+ * @trigger_event: setting for register ETMTRIGGER.
+ * @startstop_ctrl: setting for register ETMTSSCR.
+ * @enable_event: setting for register ETMTEEVR.
+ * @enable_ctrl1: setting for register ETMTECR1.
+ * @fifofull_level: setting for register ETMFFLR.
+ * @addr_idx:	index for the address comparator selection.
+ * @addr_val:	value for address comparator register.
+ * @addr_acctype: access type for address comparator register.
+ * @addr_type:	current status of the comparator register.
+ * @cntr_idx:	index for the counter register selection.
+ * @cntr_rld_val: reload value of a counter register.
+ * @cntr_event:	control for counter enable register.
+ * @cntr_rld_event: value for counter reload event register.
+ * @cntr_val:	counter value register.
+ * @seq_12_event: event causing the transition from 1 to 2.
+ * @seq_21_event: event causing the transition from 2 to 1.
+ * @seq_23_event: event causing the transition from 2 to 3.
+ * @seq_31_event: event causing the transition from 3 to 1.
+ * @seq_32_event: event causing the transition from 3 to 2.
+ * @seq_13_event: event causing the transition from 1 to 3.
+ * @seq_curr_state: current value of the sequencer register.
+ * @ctxid_idx: index for the context ID registers.
+ * @ctxid_val: value for the context ID to trigger on.
+ * @ctxid_mask: mask applicable to all the context IDs.
+ * @sync_freq:	Synchronisation frequency.
+ * @timestamp_event: Defines an event that requests the insertion
+		     of a timestamp into the trace stream.
+ */
+struct etm_drvdata {
+	void __iomem			*base;
+	struct device			*dev;
+	struct coresight_device		*csdev;
+	struct clk			*clk;
+	spinlock_t			spinlock;
+	int				cpu;
+	int				port_size;
+	u8				arch;
+	bool				use_cp14;
+	bool				enable;
+	bool				sticky_enable;
+	bool				boot_enable;
+	bool				os_unlock;
+	u8				nr_addr_cmp;
+	u8				nr_cntr;
+	u8				nr_ext_inp;
+	u8				nr_ext_out;
+	u8				nr_ctxid_cmp;
+	u32				etmccr;
+	u32				etmccer;
+	u32				traceid;
+	u32				mode;
+	u32				ctrl;
+	u32				trigger_event;
+	u32				startstop_ctrl;
+	u32				enable_event;
+	u32				enable_ctrl1;
+	u32				fifofull_level;
+	u8				addr_idx;
+	u32				addr_val[ETM_MAX_ADDR_CMP];
+	u32				addr_acctype[ETM_MAX_ADDR_CMP];
+	u32				addr_type[ETM_MAX_ADDR_CMP];
+	u8				cntr_idx;
+	u32				cntr_rld_val[ETM_MAX_CNTR];
+	u32				cntr_event[ETM_MAX_CNTR];
+	u32				cntr_rld_event[ETM_MAX_CNTR];
+	u32				cntr_val[ETM_MAX_CNTR];
+	u32				seq_12_event;
+	u32				seq_21_event;
+	u32				seq_23_event;
+	u32				seq_31_event;
+	u32				seq_32_event;
+	u32				seq_13_event;
+	u32				seq_curr_state;
+	u8				ctxid_idx;
+	u32				ctxid_val[ETM_MAX_CTXID_CMP];
+	u32				ctxid_mask;
+	u32				sync_freq;
+	u32				timestamp_event;
+};
+
+enum etm_addr_type {
+	ETM_ADDR_TYPE_NONE,
+	ETM_ADDR_TYPE_SINGLE,
+	ETM_ADDR_TYPE_RANGE,
+	ETM_ADDR_TYPE_START,
+	ETM_ADDR_TYPE_STOP,
+};
+#endif
diff --git a/drivers/coresight/coresight-etm3x.c b/drivers/coresight/coresight-etm3x.c
new file mode 100644
index 000000000000..d9e3ed6aa857
--- /dev/null
+++ b/drivers/coresight/coresight-etm3x.c
@@ -0,0 +1,1928 @@
+/* Copyright (c) 2011-2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/smp.h>
+#include <linux/sysfs.h>
+#include <linux/stat.h>
+#include <linux/clk.h>
+#include <linux/cpu.h>
+#include <linux/of.h>
+#include <linux/coresight.h>
+#include <linux/amba/bus.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+#include <asm/sections.h>
+
+#include "coresight-etm.h"
+
+#ifdef CONFIG_CORESIGHT_SOURCE_ETM_DEFAULT_ENABLE
+static int boot_enable = 1;
+#else
+static int boot_enable;
+#endif
+module_param_named(
+	boot_enable, boot_enable, int, S_IRUGO
+);
+
+/* The number of ETM/PTM currently registered */
+static int etm_count;
+static struct etm_drvdata *etmdrvdata[NR_CPUS];
+
+static inline void etm_writel(struct etm_drvdata *drvdata,
+			      u32 val, u32 off)
+{
+	if (drvdata->use_cp14) {
+		if (etm_writel_cp14(off, val)) {
+			dev_err(drvdata->dev,
+				"invalid CP14 access to ETM reg: %#x", off);
+		}
+	} else {
+		writel_relaxed(val, drvdata->base + off);
+	}
+}
+
+static inline unsigned int etm_readl(struct etm_drvdata *drvdata, u32 off)
+{
+	u32 val;
+
+	if (drvdata->use_cp14) {
+		if (etm_readl_cp14(off, &val)) {
+			dev_err(drvdata->dev,
+				"invalid CP14 access to ETM reg: %#x", off);
+		}
+	} else {
+		val = readl_relaxed(drvdata->base + off);
+	}
+
+	return val;
+}
+
+/*
+ * Memory mapped writes to clear os lock are not supported on some processors
+ * and OS lock must be unlocked before any memory mapped access on such
+ * processors, otherwise memory mapped reads/writes will be invalid.
+ */
+static void etm_os_unlock(void *info)
+{
+	struct etm_drvdata *drvdata = (struct etm_drvdata *)info;
+	/* Writing any value to ETMOSLAR unlocks the trace registers */
+	etm_writel(drvdata, 0x0, ETMOSLAR);
+	isb();
+}
+
+static void etm_set_pwrdwn(struct etm_drvdata *drvdata)
+{
+	u32 etmcr;
+
+	/* Ensure pending cp14 accesses complete before setting pwrdwn */
+	mb();
+	isb();
+	etmcr = etm_readl(drvdata, ETMCR);
+	etmcr |= ETMCR_PWD_DWN;
+	etm_writel(drvdata, etmcr, ETMCR);
+}
+
+static void etm_clr_pwrdwn(struct etm_drvdata *drvdata)
+{
+	u32 etmcr;
+
+	etmcr = etm_readl(drvdata, ETMCR);
+	etmcr &= ~ETMCR_PWD_DWN;
+	etm_writel(drvdata, etmcr, ETMCR);
+	/* Ensure pwrup completes before subsequent cp14 accesses */
+	mb();
+	isb();
+}
+
+static void etm_set_pwrup(struct etm_drvdata *drvdata)
+{
+	u32 etmpdcr;
+
+	etmpdcr = readl_relaxed(drvdata->base + ETMPDCR);
+	etmpdcr |= ETMPDCR_PWD_UP;
+	writel_relaxed(etmpdcr, drvdata->base + ETMPDCR);
+	/* Ensure pwrup completes before subsequent cp14 accesses */
+	mb();
+	isb();
+}
+
+static void etm_clr_pwrup(struct etm_drvdata *drvdata)
+{
+	u32 etmpdcr;
+
+	/* Ensure pending cp14 accesses complete before clearing pwrup */
+	mb();
+	isb();
+	etmpdcr = readl_relaxed(drvdata->base + ETMPDCR);
+	etmpdcr &= ~ETMPDCR_PWD_UP;
+	writel_relaxed(etmpdcr, drvdata->base + ETMPDCR);
+}
+
+/**
+ * coresight_timeout_etm - loop until a bit has changed to a specific state.
+ * @drvdata: etm's private data structure.
+ * @offset: address of a register, starting from @addr.
+ * @position: the position of the bit of interest.
+ * @value: the value the bit should have.
+ *
+ * Basically the same as @coresight_timeout except for the register access
+ * method where we have to account for CP14 configurations.
+
+ * Return: 0 as soon as the bit has taken the desired state or -EAGAIN if
+ * TIMEOUT_US has elapsed, which ever happens first.
+ */
+
+static int coresight_timeout_etm(struct etm_drvdata *drvdata, u32 offset,
+				  int position, int value)
+{
+	int i;
+	u32 val;
+
+	for (i = TIMEOUT_US; i > 0; i--) {
+		val = etm_readl(drvdata, offset);
+		/* Waiting on the bit to go from 0 to 1 */
+		if (value) {
+			if (val & BIT(position))
+				return 0;
+		/* Waiting on the bit to go from 1 to 0 */
+		} else {
+			if (!(val & BIT(position)))
+				return 0;
+		}
+
+		/*
+		 * Delay is arbitrary - the specification doesn't say how long
+		 * we are expected to wait.  Extra check required to make sure
+		 * we don't wait needlessly on the last iteration.
+		 */
+		if (i - 1)
+			udelay(1);
+	}
+
+	return -EAGAIN;
+}
+
+
+static void etm_set_prog(struct etm_drvdata *drvdata)
+{
+	u32 etmcr;
+
+	etmcr = etm_readl(drvdata, ETMCR);
+	etmcr |= ETMCR_ETM_PRG;
+	etm_writel(drvdata, etmcr, ETMCR);
+	/*
+	 * Recommended by spec for cp14 accesses to ensure etmcr write is
+	 * complete before polling etmsr
+	 */
+	isb();
+	if (coresight_timeout_etm(drvdata, ETMSR, ETMSR_PROG_BIT, 1)) {
+		dev_err(drvdata->dev,
+			"timeout observed when probing at offset %#x\n", ETMSR);
+	}
+}
+
+static void etm_clr_prog(struct etm_drvdata *drvdata)
+{
+	u32 etmcr;
+
+	etmcr = etm_readl(drvdata, ETMCR);
+	etmcr &= ~ETMCR_ETM_PRG;
+	etm_writel(drvdata, etmcr, ETMCR);
+	/*
+	 * Recommended by spec for cp14 accesses to ensure etmcr write is
+	 * complete before polling etmsr
+	 */
+	isb();
+	if (coresight_timeout_etm(drvdata, ETMSR, ETMSR_PROG_BIT, 0)) {
+		dev_err(drvdata->dev,
+			"timeout observed when probing at offset %#x\n", ETMSR);
+	}
+}
+
+static void etm_set_default(struct etm_drvdata *drvdata)
+{
+	int i;
+
+	drvdata->trigger_event = ETM_DEFAULT_EVENT_VAL;
+	drvdata->enable_event = ETM_HARD_WIRE_RES_A;
+
+	drvdata->seq_12_event = ETM_DEFAULT_EVENT_VAL;
+	drvdata->seq_21_event = ETM_DEFAULT_EVENT_VAL;
+	drvdata->seq_23_event = ETM_DEFAULT_EVENT_VAL;
+	drvdata->seq_31_event = ETM_DEFAULT_EVENT_VAL;
+	drvdata->seq_32_event = ETM_DEFAULT_EVENT_VAL;
+	drvdata->seq_13_event = ETM_DEFAULT_EVENT_VAL;
+	drvdata->timestamp_event = ETM_DEFAULT_EVENT_VAL;
+
+	for (i = 0; i < drvdata->nr_cntr; i++) {
+		drvdata->cntr_rld_val[i] = 0x0;
+		drvdata->cntr_event[i] = ETM_DEFAULT_EVENT_VAL;
+		drvdata->cntr_rld_event[i] = ETM_DEFAULT_EVENT_VAL;
+		drvdata->cntr_val[i] = 0x0;
+	}
+
+	drvdata->seq_curr_state = 0x0;
+	drvdata->ctxid_idx = 0x0;
+	for (i = 0; i < drvdata->nr_ctxid_cmp; i++)
+		drvdata->ctxid_val[i] = 0x0;
+	drvdata->ctxid_mask = 0x0;
+}
+
+static void etm_enable_hw(void *info)
+{
+	int i;
+	u32 etmcr;
+	struct etm_drvdata *drvdata = info;
+
+	CS_UNLOCK(drvdata->base);
+
+	/* Turn engine on */
+	etm_clr_pwrdwn(drvdata);
+	/* Apply power to trace registers */
+	etm_set_pwrup(drvdata);
+	/* Make sure all registers are accessible */
+	etm_os_unlock(drvdata);
+
+	etm_set_prog(drvdata);
+
+	etmcr = etm_readl(drvdata, ETMCR);
+	etmcr &= (ETMCR_PWD_DWN | ETMCR_ETM_PRG);
+	etmcr |= drvdata->port_size;
+	etm_writel(drvdata, drvdata->ctrl | etmcr, ETMCR);
+	etm_writel(drvdata, drvdata->trigger_event, ETMTRIGGER);
+	etm_writel(drvdata, drvdata->startstop_ctrl, ETMTSSCR);
+	etm_writel(drvdata, drvdata->enable_event, ETMTEEVR);
+	etm_writel(drvdata, drvdata->enable_ctrl1, ETMTECR1);
+	etm_writel(drvdata, drvdata->fifofull_level, ETMFFLR);
+	for (i = 0; i < drvdata->nr_addr_cmp; i++) {
+		etm_writel(drvdata, drvdata->addr_val[i], ETMACVRn(i));
+		etm_writel(drvdata, drvdata->addr_acctype[i], ETMACTRn(i));
+	}
+	for (i = 0; i < drvdata->nr_cntr; i++) {
+		etm_writel(drvdata, drvdata->cntr_rld_val[i], ETMCNTRLDVRn(i));
+		etm_writel(drvdata, drvdata->cntr_event[i], ETMCNTENRn(i));
+		etm_writel(drvdata, drvdata->cntr_rld_event[i],
+			   ETMCNTRLDEVRn(i));
+		etm_writel(drvdata, drvdata->cntr_val[i], ETMCNTVRn(i));
+	}
+	etm_writel(drvdata, drvdata->seq_12_event, ETMSQ12EVR);
+	etm_writel(drvdata, drvdata->seq_21_event, ETMSQ21EVR);
+	etm_writel(drvdata, drvdata->seq_23_event, ETMSQ23EVR);
+	etm_writel(drvdata, drvdata->seq_31_event, ETMSQ31EVR);
+	etm_writel(drvdata, drvdata->seq_32_event, ETMSQ32EVR);
+	etm_writel(drvdata, drvdata->seq_13_event, ETMSQ13EVR);
+	etm_writel(drvdata, drvdata->seq_curr_state, ETMSQR);
+	for (i = 0; i < drvdata->nr_ext_out; i++)
+		etm_writel(drvdata, ETM_DEFAULT_EVENT_VAL, ETMEXTOUTEVRn(i));
+	for (i = 0; i < drvdata->nr_ctxid_cmp; i++)
+		etm_writel(drvdata, drvdata->ctxid_val[i], ETMCIDCVRn(i));
+	etm_writel(drvdata, drvdata->ctxid_mask, ETMCIDCMR);
+	etm_writel(drvdata, drvdata->sync_freq, ETMSYNCFR);
+	/* No external input selected */
+	etm_writel(drvdata, 0x0, ETMEXTINSELR);
+	etm_writel(drvdata, drvdata->timestamp_event, ETMTSEVR);
+	/* No auxiliary control selected */
+	etm_writel(drvdata, 0x0, ETMAUXCR);
+	etm_writel(drvdata, drvdata->traceid, ETMTRACEIDR);
+	/* No VMID comparator value selected */
+	etm_writel(drvdata, 0x0, ETMVMIDCVR);
+
+	/* Ensures trace output is enabled from this ETM */
+	etm_writel(drvdata, drvdata->ctrl | ETMCR_ETM_EN | etmcr, ETMCR);
+
+	etm_clr_prog(drvdata);
+	CS_LOCK(drvdata->base);
+
+	dev_dbg(drvdata->dev, "cpu: %d enable smp call done\n", drvdata->cpu);
+}
+
+static int etm_trace_id_simple(struct etm_drvdata *drvdata)
+{
+	if (!drvdata->enable)
+		return drvdata->traceid;
+
+	return (etm_readl(drvdata, ETMTRACEIDR) & ETM_TRACEID_MASK);
+}
+
+static int etm_trace_id(struct coresight_device *csdev)
+{
+	struct etm_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+	unsigned long flags;
+	int trace_id = -1;
+
+	if (!drvdata->enable)
+		return drvdata->traceid;
+
+	if (clk_prepare_enable(drvdata->clk))
+		goto out;
+
+	spin_lock_irqsave(&drvdata->spinlock, flags);
+
+	CS_UNLOCK(drvdata->base);
+	trace_id = (etm_readl(drvdata, ETMTRACEIDR) & ETM_TRACEID_MASK);
+	CS_LOCK(drvdata->base);
+
+	spin_unlock_irqrestore(&drvdata->spinlock, flags);
+	clk_disable_unprepare(drvdata->clk);
+out:
+	return trace_id;
+}
+
+static int etm_enable(struct coresight_device *csdev)
+{
+	struct etm_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+	int ret;
+
+	ret = clk_prepare_enable(drvdata->clk);
+	if (ret)
+		goto err_clk;
+
+	spin_lock(&drvdata->spinlock);
+
+	/*
+	 * Configure the ETM only if the CPU is online.  If it isn't online
+	 * hw configuration will take place when 'CPU_STARTING' is received
+	 * in @etm_cpu_callback.
+	 */
+	if (cpu_online(drvdata->cpu)) {
+		ret = smp_call_function_single(drvdata->cpu,
+					       etm_enable_hw, drvdata, 1);
+		if (ret)
+			goto err;
+	}
+
+	drvdata->enable = true;
+	drvdata->sticky_enable = true;
+
+	spin_unlock(&drvdata->spinlock);
+
+	dev_info(drvdata->dev, "ETM tracing enabled\n");
+	return 0;
+err:
+	spin_unlock(&drvdata->spinlock);
+	clk_disable_unprepare(drvdata->clk);
+err_clk:
+	return ret;
+}
+
+static void etm_disable_hw(void *info)
+{
+	int i;
+	struct etm_drvdata *drvdata = info;
+
+	CS_UNLOCK(drvdata->base);
+	etm_set_prog(drvdata);
+
+	/* Program trace enable to low by using always false event */
+	etm_writel(drvdata, ETM_HARD_WIRE_RES_A | ETM_EVENT_NOT_A, ETMTEEVR);
+
+	/* Read back sequencer and counters for post trace analysis */
+	drvdata->seq_curr_state = (etm_readl(drvdata, ETMSQR) & ETM_SQR_MASK);
+
+	for (i = 0; i < drvdata->nr_cntr; i++)
+		drvdata->cntr_val[i] = etm_readl(drvdata, ETMCNTVRn(i));
+
+	etm_set_pwrdwn(drvdata);
+	CS_LOCK(drvdata->base);
+
+	dev_dbg(drvdata->dev, "cpu: %d disable smp call done\n", drvdata->cpu);
+}
+
+static void etm_disable(struct coresight_device *csdev)
+{
+	struct etm_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+
+	/*
+	 * Taking hotplug lock here protects from clocks getting disabled
+	 * with tracing being left on (crash scenario) if user disable occurs
+	 * after cpu online mask indicates the cpu is offline but before the
+	 * DYING hotplug callback is serviced by the ETM driver.
+	 */
+	get_online_cpus();
+	spin_lock(&drvdata->spinlock);
+
+	/*
+	 * Executing etm_disable_hw on the cpu whose ETM is being disabled
+	 * ensures that register writes occur when cpu is powered.
+	 */
+	smp_call_function_single(drvdata->cpu, etm_disable_hw, drvdata, 1);
+	drvdata->enable = false;
+
+	spin_unlock(&drvdata->spinlock);
+	put_online_cpus();
+
+	clk_disable_unprepare(drvdata->clk);
+
+	dev_info(drvdata->dev, "ETM tracing disabled\n");
+}
+
+static const struct coresight_ops_source etm_source_ops = {
+	.trace_id	= etm_trace_id,
+	.enable		= etm_enable,
+	.disable	= etm_disable,
+};
+
+static const struct coresight_ops etm_cs_ops = {
+	.source_ops	= &etm_source_ops,
+};
+
+static ssize_t nr_addr_cmp_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->nr_addr_cmp;
+	return sprintf(buf, "%#lx\n", val);
+}
+static DEVICE_ATTR_RO(nr_addr_cmp);
+
+static ssize_t nr_cntr_show(struct device *dev,
+			    struct device_attribute *attr, char *buf)
+{	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->nr_cntr;
+	return sprintf(buf, "%#lx\n", val);
+}
+static DEVICE_ATTR_RO(nr_cntr);
+
+static ssize_t nr_ctxid_cmp_show(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->nr_ctxid_cmp;
+	return sprintf(buf, "%#lx\n", val);
+}
+static DEVICE_ATTR_RO(nr_ctxid_cmp);
+
+static ssize_t etmsr_show(struct device *dev,
+			  struct device_attribute *attr, char *buf)
+{
+	int ret;
+	unsigned long flags, val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = clk_prepare_enable(drvdata->clk);
+	if (ret)
+		return ret;
+
+	spin_lock_irqsave(&drvdata->spinlock, flags);
+	CS_UNLOCK(drvdata->base);
+
+	val = etm_readl(drvdata, ETMSR);
+
+	CS_LOCK(drvdata->base);
+	spin_unlock_irqrestore(&drvdata->spinlock, flags);
+	clk_disable_unprepare(drvdata->clk);
+
+	return sprintf(buf, "%#lx\n", val);
+}
+static DEVICE_ATTR_RO(etmsr);
+
+static ssize_t reset_store(struct device *dev,
+			   struct device_attribute *attr,
+			   const char *buf, size_t size)
+{
+	int i, ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	if (val) {
+		spin_lock(&drvdata->spinlock);
+		drvdata->mode = ETM_MODE_EXCLUDE;
+		drvdata->ctrl = 0x0;
+		drvdata->trigger_event = ETM_DEFAULT_EVENT_VAL;
+		drvdata->startstop_ctrl = 0x0;
+		drvdata->addr_idx = 0x0;
+		for (i = 0; i < drvdata->nr_addr_cmp; i++) {
+			drvdata->addr_val[i] = 0x0;
+			drvdata->addr_acctype[i] = 0x0;
+			drvdata->addr_type[i] = ETM_ADDR_TYPE_NONE;
+		}
+		drvdata->cntr_idx = 0x0;
+
+		etm_set_default(drvdata);
+		spin_unlock(&drvdata->spinlock);
+	}
+
+	return size;
+}
+static DEVICE_ATTR_WO(reset);
+
+static ssize_t mode_show(struct device *dev,
+			 struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->mode;
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t mode_store(struct device *dev,
+			  struct device_attribute *attr,
+			  const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	spin_lock(&drvdata->spinlock);
+	drvdata->mode = val & ETM_MODE_ALL;
+
+	if (drvdata->mode & ETM_MODE_EXCLUDE)
+		drvdata->enable_ctrl1 |= ETMTECR1_INC_EXC;
+	else
+		drvdata->enable_ctrl1 &= ~ETMTECR1_INC_EXC;
+
+	if (drvdata->mode & ETM_MODE_CYCACC)
+		drvdata->ctrl |= ETMCR_CYC_ACC;
+	else
+		drvdata->ctrl &= ~ETMCR_CYC_ACC;
+
+	if (drvdata->mode & ETM_MODE_STALL) {
+		if (!(drvdata->etmccr & ETMCCR_FIFOFULL)) {
+			dev_warn(drvdata->dev, "stall mode not supported\n");
+			return -EINVAL;
+		}
+		drvdata->ctrl |= ETMCR_STALL_MODE;
+	 } else
+		drvdata->ctrl &= ~ETMCR_STALL_MODE;
+
+	if (drvdata->mode & ETM_MODE_TIMESTAMP) {
+		if (!(drvdata->etmccer & ETMCCER_TIMESTAMP)) {
+			dev_warn(drvdata->dev, "timestamp not supported\n");
+			return -EINVAL;
+		}
+		drvdata->ctrl |= ETMCR_TIMESTAMP_EN;
+	} else
+		drvdata->ctrl &= ~ETMCR_TIMESTAMP_EN;
+
+	if (drvdata->mode & ETM_MODE_CTXID)
+		drvdata->ctrl |= ETMCR_CTXID_SIZE;
+	else
+		drvdata->ctrl &= ~ETMCR_CTXID_SIZE;
+	spin_unlock(&drvdata->spinlock);
+
+	return size;
+}
+static DEVICE_ATTR_RW(mode);
+
+static ssize_t trigger_event_show(struct device *dev,
+				  struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->trigger_event;
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t trigger_event_store(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	drvdata->trigger_event = val & ETM_EVENT_MASK;
+
+	return size;
+}
+static DEVICE_ATTR_RW(trigger_event);
+
+static ssize_t enable_event_show(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->enable_event;
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t enable_event_store(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	drvdata->enable_event = val & ETM_EVENT_MASK;
+
+	return size;
+}
+static DEVICE_ATTR_RW(enable_event);
+
+static ssize_t fifofull_level_show(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->fifofull_level;
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t fifofull_level_store(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	drvdata->fifofull_level = val;
+
+	return size;
+}
+static DEVICE_ATTR_RW(fifofull_level);
+
+static ssize_t addr_idx_show(struct device *dev,
+			     struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->addr_idx;
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t addr_idx_store(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	if (val >= drvdata->nr_addr_cmp)
+		return -EINVAL;
+
+	/*
+	 * Use spinlock to ensure index doesn't change while it gets
+	 * dereferenced multiple times within a spinlock block elsewhere.
+	 */
+	spin_lock(&drvdata->spinlock);
+	drvdata->addr_idx = val;
+	spin_unlock(&drvdata->spinlock);
+
+	return size;
+}
+static DEVICE_ATTR_RW(addr_idx);
+
+static ssize_t addr_single_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	u8 idx;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->addr_idx;
+	if (!(drvdata->addr_type[idx] == ETM_ADDR_TYPE_NONE ||
+	      drvdata->addr_type[idx] == ETM_ADDR_TYPE_SINGLE)) {
+		spin_unlock(&drvdata->spinlock);
+		return -EINVAL;
+	}
+
+	val = drvdata->addr_val[idx];
+	spin_unlock(&drvdata->spinlock);
+
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t addr_single_store(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf, size_t size)
+{
+	u8 idx;
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->addr_idx;
+	if (!(drvdata->addr_type[idx] == ETM_ADDR_TYPE_NONE ||
+	      drvdata->addr_type[idx] == ETM_ADDR_TYPE_SINGLE)) {
+		spin_unlock(&drvdata->spinlock);
+		return -EINVAL;
+	}
+
+	drvdata->addr_val[idx] = val;
+	drvdata->addr_type[idx] = ETM_ADDR_TYPE_SINGLE;
+	spin_unlock(&drvdata->spinlock);
+
+	return size;
+}
+static DEVICE_ATTR_RW(addr_single);
+
+static ssize_t addr_range_show(struct device *dev,
+			       struct device_attribute *attr, char *buf)
+{
+	u8 idx;
+	unsigned long val1, val2;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->addr_idx;
+	if (idx % 2 != 0) {
+		spin_unlock(&drvdata->spinlock);
+		return -EPERM;
+	}
+	if (!((drvdata->addr_type[idx] == ETM_ADDR_TYPE_NONE &&
+	       drvdata->addr_type[idx + 1] == ETM_ADDR_TYPE_NONE) ||
+	      (drvdata->addr_type[idx] == ETM_ADDR_TYPE_RANGE &&
+	       drvdata->addr_type[idx + 1] == ETM_ADDR_TYPE_RANGE))) {
+		spin_unlock(&drvdata->spinlock);
+		return -EPERM;
+	}
+
+	val1 = drvdata->addr_val[idx];
+	val2 = drvdata->addr_val[idx + 1];
+	spin_unlock(&drvdata->spinlock);
+
+	return sprintf(buf, "%#lx %#lx\n", val1, val2);
+}
+
+static ssize_t addr_range_store(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t size)
+{
+	u8 idx;
+	unsigned long val1, val2;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (sscanf(buf, "%lx %lx", &val1, &val2) != 2)
+		return -EINVAL;
+	/* Lower address comparator cannot have a higher address value */
+	if (val1 > val2)
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->addr_idx;
+	if (idx % 2 != 0) {
+		spin_unlock(&drvdata->spinlock);
+		return -EPERM;
+	}
+	if (!((drvdata->addr_type[idx] == ETM_ADDR_TYPE_NONE &&
+	       drvdata->addr_type[idx + 1] == ETM_ADDR_TYPE_NONE) ||
+	      (drvdata->addr_type[idx] == ETM_ADDR_TYPE_RANGE &&
+	       drvdata->addr_type[idx + 1] == ETM_ADDR_TYPE_RANGE))) {
+		spin_unlock(&drvdata->spinlock);
+		return -EPERM;
+	}
+
+	drvdata->addr_val[idx] = val1;
+	drvdata->addr_type[idx] = ETM_ADDR_TYPE_RANGE;
+	drvdata->addr_val[idx + 1] = val2;
+	drvdata->addr_type[idx + 1] = ETM_ADDR_TYPE_RANGE;
+	drvdata->enable_ctrl1 |= (1 << (idx/2));
+	spin_unlock(&drvdata->spinlock);
+
+	return size;
+}
+static DEVICE_ATTR_RW(addr_range);
+
+static ssize_t addr_start_show(struct device *dev,
+			       struct device_attribute *attr, char *buf)
+{
+	u8 idx;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->addr_idx;
+	if (!(drvdata->addr_type[idx] == ETM_ADDR_TYPE_NONE ||
+	      drvdata->addr_type[idx] == ETM_ADDR_TYPE_START)) {
+		spin_unlock(&drvdata->spinlock);
+		return -EPERM;
+	}
+
+	val = drvdata->addr_val[idx];
+	spin_unlock(&drvdata->spinlock);
+
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t addr_start_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t size)
+{
+	u8 idx;
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->addr_idx;
+	if (!(drvdata->addr_type[idx] == ETM_ADDR_TYPE_NONE ||
+	      drvdata->addr_type[idx] == ETM_ADDR_TYPE_START)) {
+		spin_unlock(&drvdata->spinlock);
+		return -EPERM;
+	}
+
+	drvdata->addr_val[idx] = val;
+	drvdata->addr_type[idx] = ETM_ADDR_TYPE_START;
+	drvdata->startstop_ctrl |= (1 << idx);
+	drvdata->enable_ctrl1 |= BIT(25);
+	spin_unlock(&drvdata->spinlock);
+
+	return size;
+}
+static DEVICE_ATTR_RW(addr_start);
+
+static ssize_t addr_stop_show(struct device *dev,
+			      struct device_attribute *attr, char *buf)
+{
+	u8 idx;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->addr_idx;
+	if (!(drvdata->addr_type[idx] == ETM_ADDR_TYPE_NONE ||
+	      drvdata->addr_type[idx] == ETM_ADDR_TYPE_STOP)) {
+		spin_unlock(&drvdata->spinlock);
+		return -EPERM;
+	}
+
+	val = drvdata->addr_val[idx];
+	spin_unlock(&drvdata->spinlock);
+
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t addr_stop_store(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t size)
+{
+	u8 idx;
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->addr_idx;
+	if (!(drvdata->addr_type[idx] == ETM_ADDR_TYPE_NONE ||
+	      drvdata->addr_type[idx] == ETM_ADDR_TYPE_STOP)) {
+		spin_unlock(&drvdata->spinlock);
+		return -EPERM;
+	}
+
+	drvdata->addr_val[idx] = val;
+	drvdata->addr_type[idx] = ETM_ADDR_TYPE_STOP;
+	drvdata->startstop_ctrl |= (1 << (idx + 16));
+	drvdata->enable_ctrl1 |= ETMTECR1_START_STOP;
+	spin_unlock(&drvdata->spinlock);
+
+	return size;
+}
+static DEVICE_ATTR_RW(addr_stop);
+
+static ssize_t addr_acctype_show(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	val = drvdata->addr_acctype[drvdata->addr_idx];
+	spin_unlock(&drvdata->spinlock);
+
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t addr_acctype_store(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	spin_lock(&drvdata->spinlock);
+	drvdata->addr_acctype[drvdata->addr_idx] = val;
+	spin_unlock(&drvdata->spinlock);
+
+	return size;
+}
+static DEVICE_ATTR_RW(addr_acctype);
+
+static ssize_t cntr_idx_show(struct device *dev,
+			     struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->cntr_idx;
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t cntr_idx_store(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	if (val >= drvdata->nr_cntr)
+		return -EINVAL;
+	/*
+	 * Use spinlock to ensure index doesn't change while it gets
+	 * dereferenced multiple times within a spinlock block elsewhere.
+	 */
+	spin_lock(&drvdata->spinlock);
+	drvdata->cntr_idx = val;
+	spin_unlock(&drvdata->spinlock);
+
+	return size;
+}
+static DEVICE_ATTR_RW(cntr_idx);
+
+static ssize_t cntr_rld_val_show(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	val = drvdata->cntr_rld_val[drvdata->cntr_idx];
+	spin_unlock(&drvdata->spinlock);
+
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t cntr_rld_val_store(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	spin_lock(&drvdata->spinlock);
+	drvdata->cntr_rld_val[drvdata->cntr_idx] = val;
+	spin_unlock(&drvdata->spinlock);
+
+	return size;
+}
+static DEVICE_ATTR_RW(cntr_rld_val);
+
+static ssize_t cntr_event_show(struct device *dev,
+			       struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	val = drvdata->cntr_event[drvdata->cntr_idx];
+	spin_unlock(&drvdata->spinlock);
+
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t cntr_event_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	spin_lock(&drvdata->spinlock);
+	drvdata->cntr_event[drvdata->cntr_idx] = val & ETM_EVENT_MASK;
+	spin_unlock(&drvdata->spinlock);
+
+	return size;
+}
+static DEVICE_ATTR_RW(cntr_event);
+
+static ssize_t cntr_rld_event_show(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	val = drvdata->cntr_rld_event[drvdata->cntr_idx];
+	spin_unlock(&drvdata->spinlock);
+
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t cntr_rld_event_store(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	spin_lock(&drvdata->spinlock);
+	drvdata->cntr_rld_event[drvdata->cntr_idx] = val & ETM_EVENT_MASK;
+	spin_unlock(&drvdata->spinlock);
+
+	return size;
+}
+static DEVICE_ATTR_RW(cntr_rld_event);
+
+static ssize_t cntr_val_show(struct device *dev,
+			     struct device_attribute *attr, char *buf)
+{
+	int i, ret = 0;
+	u32 val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (!drvdata->enable) {
+		spin_lock(&drvdata->spinlock);
+		for (i = 0; i < drvdata->nr_cntr; i++)
+			ret += sprintf(buf, "counter %d: %x\n",
+				       i, drvdata->cntr_val[i]);
+		spin_unlock(&drvdata->spinlock);
+		return ret;
+	}
+
+	for (i = 0; i < drvdata->nr_cntr; i++) {
+		val = etm_readl(drvdata, ETMCNTVRn(i));
+		ret += sprintf(buf, "counter %d: %x\n", i, val);
+	}
+
+	return ret;
+}
+
+static ssize_t cntr_val_store(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	spin_lock(&drvdata->spinlock);
+	drvdata->cntr_val[drvdata->cntr_idx] = val;
+	spin_unlock(&drvdata->spinlock);
+
+	return size;
+}
+static DEVICE_ATTR_RW(cntr_val);
+
+static ssize_t seq_12_event_show(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->seq_12_event;
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t seq_12_event_store(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	drvdata->seq_12_event = val & ETM_EVENT_MASK;
+	return size;
+}
+static DEVICE_ATTR_RW(seq_12_event);
+
+static ssize_t seq_21_event_show(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->seq_21_event;
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t seq_21_event_store(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	drvdata->seq_21_event = val & ETM_EVENT_MASK;
+	return size;
+}
+static DEVICE_ATTR_RW(seq_21_event);
+
+static ssize_t seq_23_event_show(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->seq_23_event;
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t seq_23_event_store(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	drvdata->seq_23_event = val & ETM_EVENT_MASK;
+	return size;
+}
+static DEVICE_ATTR_RW(seq_23_event);
+
+static ssize_t seq_31_event_show(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->seq_31_event;
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t seq_31_event_store(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	drvdata->seq_31_event = val & ETM_EVENT_MASK;
+	return size;
+}
+static DEVICE_ATTR_RW(seq_31_event);
+
+static ssize_t seq_32_event_show(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->seq_32_event;
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t seq_32_event_store(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	drvdata->seq_32_event = val & ETM_EVENT_MASK;
+	return size;
+}
+static DEVICE_ATTR_RW(seq_32_event);
+
+static ssize_t seq_13_event_show(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->seq_13_event;
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t seq_13_event_store(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	drvdata->seq_13_event = val & ETM_EVENT_MASK;
+	return size;
+}
+static DEVICE_ATTR_RW(seq_13_event);
+
+static ssize_t seq_curr_state_show(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	int ret;
+	unsigned long val, flags;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (!drvdata->enable) {
+		val = drvdata->seq_curr_state;
+		goto out;
+	}
+
+	ret = clk_prepare_enable(drvdata->clk);
+	if (ret)
+		return ret;
+
+	spin_lock_irqsave(&drvdata->spinlock, flags);
+
+	CS_UNLOCK(drvdata->base);
+	val = (etm_readl(drvdata, ETMSQR) & ETM_SQR_MASK);
+	CS_LOCK(drvdata->base);
+
+	spin_unlock_irqrestore(&drvdata->spinlock, flags);
+	clk_disable_unprepare(drvdata->clk);
+out:
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t seq_curr_state_store(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	if (val > ETM_SEQ_STATE_MAX_VAL)
+		return -EINVAL;
+
+	drvdata->seq_curr_state = val;
+
+	return size;
+}
+static DEVICE_ATTR_RW(seq_curr_state);
+
+static ssize_t ctxid_idx_show(struct device *dev,
+			      struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->ctxid_idx;
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t ctxid_idx_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	if (val >= drvdata->nr_ctxid_cmp)
+		return -EINVAL;
+
+	/*
+	 * Use spinlock to ensure index doesn't change while it gets
+	 * dereferenced multiple times within a spinlock block elsewhere.
+	 */
+	spin_lock(&drvdata->spinlock);
+	drvdata->ctxid_idx = val;
+	spin_unlock(&drvdata->spinlock);
+
+	return size;
+}
+static DEVICE_ATTR_RW(ctxid_idx);
+
+static ssize_t ctxid_val_show(struct device *dev,
+			      struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	val = drvdata->ctxid_val[drvdata->ctxid_idx];
+	spin_unlock(&drvdata->spinlock);
+
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t ctxid_val_store(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	spin_lock(&drvdata->spinlock);
+	drvdata->ctxid_val[drvdata->ctxid_idx] = val;
+	spin_unlock(&drvdata->spinlock);
+
+	return size;
+}
+static DEVICE_ATTR_RW(ctxid_val);
+
+static ssize_t ctxid_mask_show(struct device *dev,
+			       struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->ctxid_mask;
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t ctxid_mask_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	drvdata->ctxid_mask = val;
+	return size;
+}
+static DEVICE_ATTR_RW(ctxid_mask);
+
+static ssize_t sync_freq_show(struct device *dev,
+			      struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->sync_freq;
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t sync_freq_store(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	drvdata->sync_freq = val & ETM_SYNC_MASK;
+	return size;
+}
+static DEVICE_ATTR_RW(sync_freq);
+
+static ssize_t timestamp_event_show(struct device *dev,
+				    struct device_attribute *attr, char *buf)
+{
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->timestamp_event;
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t timestamp_event_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	drvdata->timestamp_event = val & ETM_EVENT_MASK;
+	return size;
+}
+static DEVICE_ATTR_RW(timestamp_event);
+
+static ssize_t status_show(struct device *dev,
+			   struct device_attribute *attr, char *buf)
+{
+	int ret;
+	unsigned long flags;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = clk_prepare_enable(drvdata->clk);
+	if (ret)
+		return ret;
+
+	spin_lock_irqsave(&drvdata->spinlock, flags);
+
+	CS_UNLOCK(drvdata->base);
+	ret = sprintf(buf,
+		      "ETMCCR: 0x%08x\n"
+		      "ETMCCER: 0x%08x\n"
+		      "ETMSCR: 0x%08x\n"
+		      "ETMIDR: 0x%08x\n"
+		      "ETMCR: 0x%08x\n"
+		      "ETMTRACEIDR: 0x%08x\n"
+		      "Enable event: 0x%08x\n"
+		      "Enable start/stop: 0x%08x\n"
+		      "Enable control: CR1 0x%08x CR2 0x%08x\n"
+		      "CPU affinity: %d\n",
+		      drvdata->etmccr, drvdata->etmccer,
+		      etm_readl(drvdata, ETMSCR), etm_readl(drvdata, ETMIDR),
+		      etm_readl(drvdata, ETMCR), etm_trace_id_simple(drvdata),
+		      etm_readl(drvdata, ETMTEEVR),
+		      etm_readl(drvdata, ETMTSSCR),
+		      etm_readl(drvdata, ETMTECR1),
+		      etm_readl(drvdata, ETMTECR2),
+		      drvdata->cpu);
+	CS_LOCK(drvdata->base);
+
+	spin_unlock_irqrestore(&drvdata->spinlock, flags);
+	clk_disable_unprepare(drvdata->clk);
+
+	return ret;
+}
+static DEVICE_ATTR_RO(status);
+
+static ssize_t traceid_show(struct device *dev,
+			    struct device_attribute *attr, char *buf)
+{
+	int ret;
+	unsigned long val, flags;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (!drvdata->enable) {
+		val = drvdata->traceid;
+		goto out;
+	}
+
+	ret = clk_prepare_enable(drvdata->clk);
+	if (ret)
+		return ret;
+
+	spin_lock_irqsave(&drvdata->spinlock, flags);
+	CS_UNLOCK(drvdata->base);
+
+	val = (etm_readl(drvdata, ETMTRACEIDR) & ETM_TRACEID_MASK);
+
+	CS_LOCK(drvdata->base);
+	spin_unlock_irqrestore(&drvdata->spinlock, flags);
+	clk_disable_unprepare(drvdata->clk);
+out:
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t traceid_store(struct device *dev,
+			     struct device_attribute *attr,
+			     const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	drvdata->traceid = val & ETM_TRACEID_MASK;
+	return size;
+}
+static DEVICE_ATTR_RW(traceid);
+
+static struct attribute *coresight_etm_attrs[] = {
+	&dev_attr_nr_addr_cmp.attr,
+	&dev_attr_nr_cntr.attr,
+	&dev_attr_nr_ctxid_cmp.attr,
+	&dev_attr_etmsr.attr,
+	&dev_attr_reset.attr,
+	&dev_attr_mode.attr,
+	&dev_attr_trigger_event.attr,
+	&dev_attr_enable_event.attr,
+	&dev_attr_fifofull_level.attr,
+	&dev_attr_addr_idx.attr,
+	&dev_attr_addr_single.attr,
+	&dev_attr_addr_range.attr,
+	&dev_attr_addr_start.attr,
+	&dev_attr_addr_stop.attr,
+	&dev_attr_addr_acctype.attr,
+	&dev_attr_cntr_idx.attr,
+	&dev_attr_cntr_rld_val.attr,
+	&dev_attr_cntr_event.attr,
+	&dev_attr_cntr_rld_event.attr,
+	&dev_attr_cntr_val.attr,
+	&dev_attr_seq_12_event.attr,
+	&dev_attr_seq_21_event.attr,
+	&dev_attr_seq_23_event.attr,
+	&dev_attr_seq_31_event.attr,
+	&dev_attr_seq_32_event.attr,
+	&dev_attr_seq_13_event.attr,
+	&dev_attr_seq_curr_state.attr,
+	&dev_attr_ctxid_idx.attr,
+	&dev_attr_ctxid_val.attr,
+	&dev_attr_ctxid_mask.attr,
+	&dev_attr_sync_freq.attr,
+	&dev_attr_timestamp_event.attr,
+	&dev_attr_status.attr,
+	&dev_attr_traceid.attr,
+	NULL,
+};
+ATTRIBUTE_GROUPS(coresight_etm);
+
+static int etm_cpu_callback(struct notifier_block *nfb, unsigned long action,
+			    void *hcpu)
+{
+	unsigned int cpu = (unsigned long)hcpu;
+
+	if (!etmdrvdata[cpu])
+		goto out;
+
+	switch (action & (~CPU_TASKS_FROZEN)) {
+	case CPU_STARTING:
+		spin_lock(&etmdrvdata[cpu]->spinlock);
+		if (!etmdrvdata[cpu]->os_unlock) {
+			etm_os_unlock(etmdrvdata[cpu]);
+			etmdrvdata[cpu]->os_unlock = true;
+		}
+
+		if (etmdrvdata[cpu]->enable)
+			etm_enable_hw(etmdrvdata[cpu]);
+		spin_unlock(&etmdrvdata[cpu]->spinlock);
+		break;
+
+	case CPU_ONLINE:
+		if (etmdrvdata[cpu]->boot_enable &&
+		    !etmdrvdata[cpu]->sticky_enable)
+			coresight_enable(etmdrvdata[cpu]->csdev);
+		break;
+
+	case CPU_DYING:
+		spin_lock(&etmdrvdata[cpu]->spinlock);
+		if (etmdrvdata[cpu]->enable)
+			etm_disable_hw(etmdrvdata[cpu]);
+		spin_unlock(&etmdrvdata[cpu]->spinlock);
+		break;
+	}
+out:
+	return NOTIFY_OK;
+}
+
+static struct notifier_block etm_cpu_notifier = {
+	.notifier_call = etm_cpu_callback,
+};
+
+static bool etm_arch_supported(u8 arch)
+{
+	switch (arch) {
+	case ETM_ARCH_V3_3:
+		break;
+	case ETM_ARCH_V3_5:
+		break;
+	case PFT_ARCH_V1_0:
+		break;
+	case PFT_ARCH_V1_1:
+		break;
+	default:
+		return false;
+	}
+	return true;
+}
+
+static void etm_init_arch_data(void *info)
+{
+	u32 etmidr;
+	u32 etmccr;
+	struct etm_drvdata *drvdata = info;
+
+	CS_UNLOCK(drvdata->base);
+
+	/* First dummy read */
+	(void)etm_readl(drvdata, ETMPDSR);
+	/* Provide power to ETM: ETMPDCR[3] == 1 */
+	etm_set_pwrup(drvdata);
+	/*
+	 * Clear power down bit since when this bit is set writes to
+	 * certain registers might be ignored.
+	 */
+	etm_clr_pwrdwn(drvdata);
+	/*
+	 * Set prog bit. It will be set from reset but this is included to
+	 * ensure it is set
+	 */
+	etm_set_prog(drvdata);
+
+	/* Find all capabilities */
+	etmidr = etm_readl(drvdata, ETMIDR);
+	drvdata->arch = BMVAL(etmidr, 4, 11);
+	drvdata->port_size = etm_readl(drvdata, ETMCR) & PORT_SIZE_MASK;
+
+	drvdata->etmccer = etm_readl(drvdata, ETMCCER);
+	etmccr = etm_readl(drvdata, ETMCCR);
+	drvdata->etmccr = etmccr;
+	drvdata->nr_addr_cmp = BMVAL(etmccr, 0, 3) * 2;
+	drvdata->nr_cntr = BMVAL(etmccr, 13, 15);
+	drvdata->nr_ext_inp = BMVAL(etmccr, 17, 19);
+	drvdata->nr_ext_out = BMVAL(etmccr, 20, 22);
+	drvdata->nr_ctxid_cmp = BMVAL(etmccr, 24, 25);
+
+	etm_set_pwrdwn(drvdata);
+	etm_clr_pwrup(drvdata);
+	CS_LOCK(drvdata->base);
+}
+
+static void etm_init_default_data(struct etm_drvdata *drvdata)
+{
+	static int etm3x_traceid;
+
+	u32 flags = (1 << 0 | /* instruction execute*/
+		     3 << 3 | /* ARM instruction */
+		     0 << 5 | /* No data value comparison */
+		     0 << 7 | /* No exact mach */
+		     0 << 8 | /* Ignore context ID */
+		     0 << 10); /* Security ignored */
+
+	/*
+	 * Initial configuration only - guarantees sources handled by
+	 * this driver have a unique ID at startup time but not between
+	 * all other types of sources.  For that we lean on the core
+	 * framework.
+	 */
+	drvdata->traceid = etm3x_traceid++;
+	drvdata->ctrl = (ETMCR_CYC_ACC | ETMCR_TIMESTAMP_EN);
+	drvdata->enable_ctrl1 = ETMTECR1_ADDR_COMP_1;
+	if (drvdata->nr_addr_cmp >= 2) {
+		drvdata->addr_val[0] = (u32) _stext;
+		drvdata->addr_val[1] = (u32) _etext;
+		drvdata->addr_acctype[0] = flags;
+		drvdata->addr_acctype[1] = flags;
+		drvdata->addr_type[0] = ETM_ADDR_TYPE_RANGE;
+		drvdata->addr_type[1] = ETM_ADDR_TYPE_RANGE;
+	}
+
+	etm_set_default(drvdata);
+}
+
+static int etm_probe(struct amba_device *adev, const struct amba_id *id)
+{
+	int ret;
+	void __iomem *base;
+	struct device *dev = &adev->dev;
+	struct coresight_platform_data *pdata = NULL;
+	struct etm_drvdata *drvdata;
+	struct resource *res = &adev->res;
+	struct coresight_desc *desc;
+	struct device_node *np = adev->dev.of_node;
+
+	desc = devm_kzalloc(dev, sizeof(*desc), GFP_KERNEL);
+	if (!desc)
+		return -ENOMEM;
+
+	drvdata = devm_kzalloc(dev, sizeof(*drvdata), GFP_KERNEL);
+	if (!drvdata)
+		return -ENOMEM;
+
+	if (np) {
+		pdata = of_get_coresight_platform_data(dev, np);
+		if (IS_ERR(pdata))
+			return PTR_ERR(pdata);
+
+		adev->dev.platform_data = pdata;
+		drvdata->use_cp14 = of_property_read_bool(np, "arm,cp14");
+	}
+
+	drvdata->dev = &adev->dev;
+	dev_set_drvdata(dev, drvdata);
+
+	/* Validity for the resource is already checked by the AMBA core */
+	base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	drvdata->base = base;
+
+	spin_lock_init(&drvdata->spinlock);
+
+	drvdata->clk = adev->pclk;
+	ret = clk_prepare_enable(drvdata->clk);
+	if (ret)
+		return ret;
+
+	drvdata->cpu = pdata ? pdata->cpu : 0;
+
+	get_online_cpus();
+	etmdrvdata[drvdata->cpu] = drvdata;
+
+	if (!smp_call_function_single(drvdata->cpu, etm_os_unlock, drvdata, 1))
+		drvdata->os_unlock = true;
+
+	if (smp_call_function_single(drvdata->cpu,
+				     etm_init_arch_data,  drvdata, 1))
+		dev_err(dev, "ETM arch init failed\n");
+
+	if (!etm_count++)
+		register_hotcpu_notifier(&etm_cpu_notifier);
+
+	put_online_cpus();
+
+	if (etm_arch_supported(drvdata->arch) == false) {
+		ret = -EINVAL;
+		goto err_arch_supported;
+	}
+	etm_init_default_data(drvdata);
+
+	clk_disable_unprepare(drvdata->clk);
+
+	desc->type = CORESIGHT_DEV_TYPE_SOURCE;
+	desc->subtype.source_subtype = CORESIGHT_DEV_SUBTYPE_SOURCE_PROC;
+	desc->ops = &etm_cs_ops;
+	desc->pdata = pdata;
+	desc->dev = dev;
+	desc->groups = coresight_etm_groups;
+	drvdata->csdev = coresight_register(desc);
+	if (IS_ERR(drvdata->csdev)) {
+		ret = PTR_ERR(drvdata->csdev);
+		goto err_arch_supported;
+	}
+
+	dev_info(dev, "ETM initialized\n");
+
+	if (boot_enable) {
+		coresight_enable(drvdata->csdev);
+		drvdata->boot_enable = true;
+	}
+
+	return 0;
+
+err_arch_supported:
+	clk_disable_unprepare(drvdata->clk);
+	if (--etm_count == 0)
+		unregister_hotcpu_notifier(&etm_cpu_notifier);
+	return ret;
+}
+
+static int etm_remove(struct amba_device *adev)
+{
+	struct etm_drvdata *drvdata = amba_get_drvdata(adev);
+
+	coresight_unregister(drvdata->csdev);
+	if (--etm_count == 0)
+		unregister_hotcpu_notifier(&etm_cpu_notifier);
+
+	return 0;
+}
+
+static struct amba_id etm_ids[] = {
+	{	/* ETM 3.3 */
+		.id	= 0x0003b921,
+		.mask	= 0x0003ffff,
+	},
+	{	/* ETM 3.5 */
+		.id	= 0x0003b956,
+		.mask	= 0x0003ffff,
+	},
+	{	/* PTM 1.0 */
+		.id	= 0x0003b950,
+		.mask	= 0x0003ffff,
+	},
+	{	/* PTM 1.1 */
+		.id	= 0x0003b95f,
+		.mask	= 0x0003ffff,
+	},
+	{ 0, 0},
+};
+
+static struct amba_driver etm_driver = {
+	.drv = {
+		.name	= "coresight-etm3x",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= etm_probe,
+	.remove		= etm_remove,
+	.id_table	= etm_ids,
+};
+
+int __init etm_init(void)
+{
+	return amba_driver_register(&etm_driver);
+}
+module_init(etm_init);
+
+void __exit etm_exit(void)
+{
+	amba_driver_unregister(&etm_driver);
+}
+module_exit(etm_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("CoreSight Program Flow Trace driver");
diff --git a/drivers/coresight/coresight-funnel.c b/drivers/coresight/coresight-funnel.c
new file mode 100644
index 000000000000..2108edffe1f4
--- /dev/null
+++ b/drivers/coresight/coresight-funnel.c
@@ -0,0 +1,268 @@
+/* Copyright (c) 2011-2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/coresight.h>
+#include <linux/amba/bus.h>
+
+#include "coresight-priv.h"
+
+#define FUNNEL_FUNCTL		0x000
+#define FUNNEL_PRICTL		0x004
+
+#define FUNNEL_HOLDTIME_MASK	0xf00
+#define FUNNEL_HOLDTIME_SHFT	0x8
+#define FUNNEL_HOLDTIME		(0x7 << FUNNEL_HOLDTIME_SHFT)
+
+/**
+ * struct funnel_drvdata - specifics associated to a funnel component
+ * @base:	memory mapped base address for this component.
+ * @dev:	the device entity associated to this component.
+ * @csdev:	component vitals needed by the framework.
+ * @clk:	the clock this component is associated to.
+ * @priority:	port selection order.
+ */
+struct funnel_drvdata {
+	void __iomem		*base;
+	struct device		*dev;
+	struct coresight_device	*csdev;
+	struct clk		*clk;
+	unsigned long		priority;
+};
+
+static void funnel_enable_hw(struct funnel_drvdata *drvdata, int port)
+{
+	u32 functl;
+
+	CS_UNLOCK(drvdata->base);
+
+	functl = readl_relaxed(drvdata->base + FUNNEL_FUNCTL);
+	functl &= ~FUNNEL_HOLDTIME_MASK;
+	functl |= FUNNEL_HOLDTIME;
+	functl |= (1 << port);
+	writel_relaxed(functl, drvdata->base + FUNNEL_FUNCTL);
+	writel_relaxed(drvdata->priority, drvdata->base + FUNNEL_PRICTL);
+
+	CS_LOCK(drvdata->base);
+}
+
+static int funnel_enable(struct coresight_device *csdev, int inport,
+			 int outport)
+{
+	struct funnel_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+	int ret;
+
+	ret = clk_prepare_enable(drvdata->clk);
+	if (ret)
+		return ret;
+
+	funnel_enable_hw(drvdata, inport);
+
+	dev_info(drvdata->dev, "FUNNEL inport %d enabled\n", inport);
+	return 0;
+}
+
+static void funnel_disable_hw(struct funnel_drvdata *drvdata, int inport)
+{
+	u32 functl;
+
+	CS_UNLOCK(drvdata->base);
+
+	functl = readl_relaxed(drvdata->base + FUNNEL_FUNCTL);
+	functl &= ~(1 << inport);
+	writel_relaxed(functl, drvdata->base + FUNNEL_FUNCTL);
+
+	CS_LOCK(drvdata->base);
+}
+
+static void funnel_disable(struct coresight_device *csdev, int inport,
+			   int outport)
+{
+	struct funnel_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+
+	funnel_disable_hw(drvdata, inport);
+
+	clk_disable_unprepare(drvdata->clk);
+
+	dev_info(drvdata->dev, "FUNNEL inport %d disabled\n", inport);
+}
+
+static const struct coresight_ops_link funnel_link_ops = {
+	.enable		= funnel_enable,
+	.disable	= funnel_disable,
+};
+
+static const struct coresight_ops funnel_cs_ops = {
+	.link_ops	= &funnel_link_ops,
+};
+
+static ssize_t priority_show(struct device *dev,
+			     struct device_attribute *attr, char *buf)
+{
+	struct funnel_drvdata *drvdata = dev_get_drvdata(dev->parent);
+	unsigned long val = drvdata->priority;
+
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t priority_store(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct funnel_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	drvdata->priority = val;
+	return size;
+}
+static DEVICE_ATTR_RW(priority);
+
+static u32 get_funnel_ctrl_hw(struct funnel_drvdata *drvdata)
+{
+	u32 functl;
+
+	CS_UNLOCK(drvdata->base);
+	functl = readl_relaxed(drvdata->base + FUNNEL_FUNCTL);
+	CS_LOCK(drvdata->base);
+
+	return functl;
+}
+
+static ssize_t funnel_ctrl_show(struct device *dev,
+			     struct device_attribute *attr, char *buf)
+{
+	int ret;
+	u32 val;
+	struct funnel_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = clk_prepare_enable(drvdata->clk);
+	if (ret)
+		return ret;
+
+	val = get_funnel_ctrl_hw(drvdata);
+	clk_disable_unprepare(drvdata->clk);
+
+	return sprintf(buf, "%#x\n", val);
+}
+static DEVICE_ATTR_RO(funnel_ctrl);
+
+static struct attribute *coresight_funnel_attrs[] = {
+	&dev_attr_funnel_ctrl.attr,
+	&dev_attr_priority.attr,
+	NULL,
+};
+ATTRIBUTE_GROUPS(coresight_funnel);
+
+static int funnel_probe(struct amba_device *adev, const struct amba_id *id)
+{
+	void __iomem *base;
+	struct device *dev = &adev->dev;
+	struct coresight_platform_data *pdata = NULL;
+	struct funnel_drvdata *drvdata;
+	struct resource *res = &adev->res;
+	struct coresight_desc *desc;
+	struct device_node *np = adev->dev.of_node;
+
+	if (np) {
+		pdata = of_get_coresight_platform_data(dev, np);
+		if (IS_ERR(pdata))
+			return PTR_ERR(pdata);
+		adev->dev.platform_data = pdata;
+	}
+
+	drvdata = devm_kzalloc(dev, sizeof(*drvdata), GFP_KERNEL);
+	if (!drvdata)
+		return -ENOMEM;
+
+	drvdata->dev = &adev->dev;
+	dev_set_drvdata(dev, drvdata);
+
+	/* Validity for the resource is already checked by the AMBA core */
+	base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	drvdata->base = base;
+
+	drvdata->clk = adev->pclk;
+
+	desc = devm_kzalloc(dev, sizeof(*desc), GFP_KERNEL);
+	if (!desc)
+		return -ENOMEM;
+
+	desc->type = CORESIGHT_DEV_TYPE_LINK;
+	desc->subtype.link_subtype = CORESIGHT_DEV_SUBTYPE_LINK_MERG;
+	desc->ops = &funnel_cs_ops;
+	desc->pdata = pdata;
+	desc->dev = dev;
+	desc->groups = coresight_funnel_groups;
+	drvdata->csdev = coresight_register(desc);
+	if (IS_ERR(drvdata->csdev))
+		return PTR_ERR(drvdata->csdev);
+
+	dev_info(dev, "FUNNEL initialized\n");
+	return 0;
+}
+
+static int funnel_remove(struct amba_device *adev)
+{
+	struct funnel_drvdata *drvdata = amba_get_drvdata(adev);
+
+	coresight_unregister(drvdata->csdev);
+	return 0;
+}
+
+static struct amba_id funnel_ids[] = {
+	{
+		.id     = 0x0003b908,
+		.mask   = 0x0003ffff,
+	},
+	{ 0, 0},
+};
+
+static struct amba_driver funnel_driver = {
+	.drv = {
+		.name	= "coresight-funnel",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= funnel_probe,
+	.remove		= funnel_remove,
+	.id_table	= funnel_ids,
+};
+
+static int __init funnel_init(void)
+{
+	return amba_driver_register(&funnel_driver);
+}
+module_init(funnel_init);
+
+static void __exit funnel_exit(void)
+{
+	amba_driver_unregister(&funnel_driver);
+}
+module_exit(funnel_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("CoreSight Funnel driver");
diff --git a/drivers/coresight/coresight-priv.h b/drivers/coresight/coresight-priv.h
new file mode 100644
index 000000000000..7b3372fca4f6
--- /dev/null
+++ b/drivers/coresight/coresight-priv.h
@@ -0,0 +1,63 @@
+/* Copyright (c) 2011-2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef _CORESIGHT_PRIV_H
+#define _CORESIGHT_PRIV_H
+
+#include <linux/bitops.h>
+#include <linux/io.h>
+#include <linux/coresight.h>
+
+/*
+ * Coresight management registers (0xf00-0xfcc)
+ * 0xfa0 - 0xfa4: Management	registers in PFTv1.0
+ *		  Trace		registers in PFTv1.1
+ */
+#define CORESIGHT_ITCTRL	0xf00
+#define CORESIGHT_CLAIMSET	0xfa0
+#define CORESIGHT_CLAIMCLR	0xfa4
+#define CORESIGHT_LAR		0xfb0
+#define CORESIGHT_LSR		0xfb4
+#define CORESIGHT_AUTHSTATUS	0xfb8
+#define CORESIGHT_DEVID		0xfc8
+#define CORESIGHT_DEVTYPE	0xfcc
+
+#define TIMEOUT_US		100
+#define BMVAL(val, lsb, msb)	((val & GENMASK(msb, lsb)) >> lsb)
+
+static inline void CS_LOCK(void __iomem *addr)
+{
+	do {
+		/* Wait for things to settle */
+		mb();
+		writel_relaxed(0x0, addr + CORESIGHT_LAR);
+	} while (0);
+}
+
+static inline void CS_UNLOCK(void __iomem *addr)
+{
+	do {
+		writel_relaxed(CORESIGHT_UNLOCK, addr + CORESIGHT_LAR);
+		/* Make sure everyone has seen this */
+		mb();
+	} while (0);
+}
+
+#ifdef CONFIG_CORESIGHT_SOURCE_ETM3X
+extern int etm_readl_cp14(u32 off, unsigned int *val);
+extern int etm_writel_cp14(u32 off, u32 val);
+#else
+static inline int etm_readl_cp14(u32 off, unsigned int *val) { return 0; }
+static inline int etm_writel_cp14(u32 val, u32 off) { return 0; }
+#endif
+
+#endif
diff --git a/drivers/coresight/coresight-replicator.c b/drivers/coresight/coresight-replicator.c
new file mode 100644
index 000000000000..a2dfcf903551
--- /dev/null
+++ b/drivers/coresight/coresight-replicator.c
@@ -0,0 +1,137 @@
+/* Copyright (c) 2011-2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/of.h>
+#include <linux/coresight.h>
+
+#include "coresight-priv.h"
+
+/**
+ * struct replicator_drvdata - specifics associated to a replicator component
+ * @dev:	the device entity associated with this component
+ * @csdev:	component vitals needed by the framework
+ */
+struct replicator_drvdata {
+	struct device		*dev;
+	struct coresight_device	*csdev;
+};
+
+static int replicator_enable(struct coresight_device *csdev, int inport,
+			     int outport)
+{
+	struct replicator_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+
+	dev_info(drvdata->dev, "REPLICATOR enabled\n");
+	return 0;
+}
+
+static void replicator_disable(struct coresight_device *csdev, int inport,
+			       int outport)
+{
+	struct replicator_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+
+	dev_info(drvdata->dev, "REPLICATOR disabled\n");
+}
+
+static const struct coresight_ops_link replicator_link_ops = {
+	.enable		= replicator_enable,
+	.disable	= replicator_disable,
+};
+
+static const struct coresight_ops replicator_cs_ops = {
+	.link_ops	= &replicator_link_ops,
+};
+
+static int replicator_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct coresight_platform_data *pdata = NULL;
+	struct replicator_drvdata *drvdata;
+	struct coresight_desc *desc;
+	struct device_node *np = pdev->dev.of_node;
+
+	if (np) {
+		pdata = of_get_coresight_platform_data(dev, np);
+		if (IS_ERR(pdata))
+			return PTR_ERR(pdata);
+		pdev->dev.platform_data = pdata;
+	}
+
+	drvdata = devm_kzalloc(dev, sizeof(*drvdata), GFP_KERNEL);
+	if (!drvdata)
+		return -ENOMEM;
+
+	drvdata->dev = &pdev->dev;
+	platform_set_drvdata(pdev, drvdata);
+
+	desc = devm_kzalloc(dev, sizeof(*desc), GFP_KERNEL);
+	if (!desc)
+		return -ENOMEM;
+
+	desc->type = CORESIGHT_DEV_TYPE_LINK;
+	desc->subtype.sink_subtype = CORESIGHT_DEV_SUBTYPE_LINK_SPLIT;
+	desc->ops = &replicator_cs_ops;
+	desc->pdata = pdev->dev.platform_data;
+	desc->dev = &pdev->dev;
+	drvdata->csdev = coresight_register(desc);
+	if (IS_ERR(drvdata->csdev))
+		return PTR_ERR(drvdata->csdev);
+
+	dev_info(dev, "REPLICATOR initialized\n");
+	return 0;
+}
+
+static int replicator_remove(struct platform_device *pdev)
+{
+	struct replicator_drvdata *drvdata = platform_get_drvdata(pdev);
+
+	coresight_unregister(drvdata->csdev);
+	return 0;
+}
+
+static struct of_device_id replicator_match[] = {
+	{.compatible = "arm,coresight-replicator"},
+	{}
+};
+
+static struct platform_driver replicator_driver = {
+	.probe          = replicator_probe,
+	.remove         = replicator_remove,
+	.driver         = {
+		.name   = "coresight-replicator",
+		.of_match_table = replicator_match,
+	},
+};
+
+static int __init replicator_init(void)
+{
+	return platform_driver_register(&replicator_driver);
+}
+module_init(replicator_init);
+
+static void __exit replicator_exit(void)
+{
+	platform_driver_unregister(&replicator_driver);
+}
+module_exit(replicator_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("CoreSight Replicator driver");
diff --git a/drivers/coresight/coresight-tmc.c b/drivers/coresight/coresight-tmc.c
new file mode 100644
index 000000000000..ce2c293f1707
--- /dev/null
+++ b/drivers/coresight/coresight-tmc.c
@@ -0,0 +1,776 @@
+/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/uaccess.h>
+#include <linux/slab.h>
+#include <linux/dma-mapping.h>
+#include <linux/spinlock.h>
+#include <linux/clk.h>
+#include <linux/of.h>
+#include <linux/coresight.h>
+#include <linux/amba/bus.h>
+
+#include "coresight-priv.h"
+
+#define TMC_RSZ			0x004
+#define TMC_STS			0x00c
+#define TMC_RRD			0x010
+#define TMC_RRP			0x014
+#define TMC_RWP			0x018
+#define TMC_TRG			0x01c
+#define TMC_CTL			0x020
+#define TMC_RWD			0x024
+#define TMC_MODE		0x028
+#define TMC_LBUFLEVEL		0x02c
+#define TMC_CBUFLEVEL		0x030
+#define TMC_BUFWM		0x034
+#define TMC_RRPHI		0x038
+#define TMC_RWPHI		0x03c
+#define TMC_AXICTL		0x110
+#define TMC_DBALO		0x118
+#define TMC_DBAHI		0x11c
+#define TMC_FFSR		0x300
+#define TMC_FFCR		0x304
+#define TMC_PSCR		0x308
+#define TMC_ITMISCOP0		0xee0
+#define TMC_ITTRFLIN		0xee8
+#define TMC_ITATBDATA0		0xeec
+#define TMC_ITATBCTR2		0xef0
+#define TMC_ITATBCTR1		0xef4
+#define TMC_ITATBCTR0		0xef8
+
+/* register description */
+/* TMC_CTL - 0x020 */
+#define TMC_CTL_CAPT_EN		BIT(0)
+/* TMC_STS - 0x00C */
+#define TMC_STS_TRIGGERED	BIT(1)
+/* TMC_AXICTL - 0x110 */
+#define TMC_AXICTL_PROT_CTL_B0	BIT(0)
+#define TMC_AXICTL_PROT_CTL_B1	BIT(1)
+#define TMC_AXICTL_SCT_GAT_MODE	BIT(7)
+#define TMC_AXICTL_WR_BURST_LEN 0xF00
+/* TMC_FFCR - 0x304 */
+#define TMC_FFCR_EN_FMT		BIT(0)
+#define TMC_FFCR_EN_TI		BIT(1)
+#define TMC_FFCR_FON_FLIN	BIT(4)
+#define TMC_FFCR_FON_TRIG_EVT	BIT(5)
+#define TMC_FFCR_FLUSHMAN	BIT(6)
+#define TMC_FFCR_TRIGON_TRIGIN	BIT(8)
+#define TMC_FFCR_STOP_ON_FLUSH	BIT(12)
+
+#define TMC_STS_TRIGGERED_BIT	2
+#define TMC_FFCR_FLUSHMAN_BIT	6
+
+enum tmc_config_type {
+	TMC_CONFIG_TYPE_ETB,
+	TMC_CONFIG_TYPE_ETR,
+	TMC_CONFIG_TYPE_ETF,
+};
+
+enum tmc_mode {
+	TMC_MODE_CIRCULAR_BUFFER,
+	TMC_MODE_SOFTWARE_FIFO,
+	TMC_MODE_HARDWARE_FIFO,
+};
+
+enum tmc_mem_intf_width {
+	TMC_MEM_INTF_WIDTH_32BITS	= 0x2,
+	TMC_MEM_INTF_WIDTH_64BITS	= 0x3,
+	TMC_MEM_INTF_WIDTH_128BITS	= 0x4,
+	TMC_MEM_INTF_WIDTH_256BITS	= 0x5,
+};
+
+/**
+ * struct tmc_drvdata - specifics associated to an TMC component
+ * @base:	memory mapped base address for this component.
+ * @dev:	the device entity associated to this component.
+ * @csdev:	component vitals needed by the framework.
+ * @miscdev:	specifics to handle "/dev/xyz.tmc" entry.
+ * @clk:	the clock this component is associated to.
+ * @spinlock:	only one at a time pls.
+ * @read_count:	manages preparation of buffer for reading.
+ * @buf:	area of memory where trace data get sent.
+ * @paddr:	DMA start location in RAM.
+ * @vaddr:	virtual representation of @paddr.
+ * @size:	@buf size.
+ * @enable:	this TMC is being used.
+ * @config_type: TMC variant, must be of type @tmc_config_type.
+ * @trigger_cntr: amount of words to store after a trigger.
+ */
+struct tmc_drvdata {
+	void __iomem		*base;
+	struct device		*dev;
+	struct coresight_device	*csdev;
+	struct miscdevice	miscdev;
+	struct clk		*clk;
+	spinlock_t		spinlock;
+	int			read_count;
+	bool			reading;
+	char			*buf;
+	dma_addr_t		paddr;
+	void __iomem		*vaddr;
+	u32			size;
+	bool			enable;
+	enum tmc_config_type	config_type;
+	u32			trigger_cntr;
+};
+
+static void tmc_wait_for_ready(struct tmc_drvdata *drvdata)
+{
+	/* Ensure formatter, unformatter and hardware fifo are empty */
+	if (coresight_timeout(drvdata->base,
+			      TMC_STS, TMC_STS_TRIGGERED_BIT, 1)) {
+		dev_err(drvdata->dev,
+			"timeout observed when probing at offset %#x\n",
+			TMC_STS);
+	}
+}
+
+static void tmc_flush_and_stop(struct tmc_drvdata *drvdata)
+{
+	u32 ffcr;
+
+	ffcr = readl_relaxed(drvdata->base + TMC_FFCR);
+	ffcr |= TMC_FFCR_STOP_ON_FLUSH;
+	writel_relaxed(ffcr, drvdata->base + TMC_FFCR);
+	ffcr |= TMC_FFCR_FLUSHMAN;
+	writel_relaxed(ffcr, drvdata->base + TMC_FFCR);
+	/* Ensure flush completes */
+	if (coresight_timeout(drvdata->base,
+			      TMC_FFCR, TMC_FFCR_FLUSHMAN_BIT, 0)) {
+		dev_err(drvdata->dev,
+			"timeout observed when probing at offset %#x\n",
+			TMC_FFCR);
+	}
+
+	tmc_wait_for_ready(drvdata);
+}
+
+static void tmc_enable_hw(struct tmc_drvdata *drvdata)
+{
+	writel_relaxed(TMC_CTL_CAPT_EN, drvdata->base + TMC_CTL);
+}
+
+static void tmc_disable_hw(struct tmc_drvdata *drvdata)
+{
+	writel_relaxed(0x0, drvdata->base + TMC_CTL);
+}
+
+static void tmc_etb_enable_hw(struct tmc_drvdata *drvdata)
+{
+	/* Zero out the memory to help with debug */
+	memset(drvdata->buf, 0, drvdata->size);
+
+	CS_UNLOCK(drvdata->base);
+
+	writel_relaxed(TMC_MODE_CIRCULAR_BUFFER, drvdata->base + TMC_MODE);
+	writel_relaxed(TMC_FFCR_EN_FMT | TMC_FFCR_EN_TI |
+		       TMC_FFCR_FON_FLIN | TMC_FFCR_FON_TRIG_EVT |
+		       TMC_FFCR_TRIGON_TRIGIN,
+		       drvdata->base + TMC_FFCR);
+
+	writel_relaxed(drvdata->trigger_cntr, drvdata->base + TMC_TRG);
+	tmc_enable_hw(drvdata);
+
+	CS_LOCK(drvdata->base);
+}
+
+static void tmc_etr_enable_hw(struct tmc_drvdata *drvdata)
+{
+	u32 axictl;
+
+	/* Zero out the memory to help with debug */
+	memset(drvdata->vaddr, 0, drvdata->size);
+
+	CS_UNLOCK(drvdata->base);
+
+	writel_relaxed(drvdata->size / 4, drvdata->base + TMC_RSZ);
+	writel_relaxed(TMC_MODE_CIRCULAR_BUFFER, drvdata->base + TMC_MODE);
+
+	axictl = readl_relaxed(drvdata->base + TMC_AXICTL);
+	axictl |= TMC_AXICTL_WR_BURST_LEN;
+	writel_relaxed(axictl, drvdata->base + TMC_AXICTL);
+	axictl &= ~TMC_AXICTL_SCT_GAT_MODE;
+	writel_relaxed(axictl, drvdata->base + TMC_AXICTL);
+	axictl = (axictl &
+		  ~(TMC_AXICTL_PROT_CTL_B0 | TMC_AXICTL_PROT_CTL_B1)) |
+		  TMC_AXICTL_PROT_CTL_B1;
+	writel_relaxed(axictl, drvdata->base + TMC_AXICTL);
+
+	writel_relaxed(drvdata->paddr, drvdata->base + TMC_DBALO);
+	writel_relaxed(0x0, drvdata->base + TMC_DBAHI);
+	writel_relaxed(TMC_FFCR_EN_FMT | TMC_FFCR_EN_TI |
+		       TMC_FFCR_FON_FLIN | TMC_FFCR_FON_TRIG_EVT |
+		       TMC_FFCR_TRIGON_TRIGIN,
+		       drvdata->base + TMC_FFCR);
+	writel_relaxed(drvdata->trigger_cntr, drvdata->base + TMC_TRG);
+	tmc_enable_hw(drvdata);
+
+	CS_LOCK(drvdata->base);
+}
+
+static void tmc_etf_enable_hw(struct tmc_drvdata *drvdata)
+{
+	CS_UNLOCK(drvdata->base);
+
+	writel_relaxed(TMC_MODE_HARDWARE_FIFO, drvdata->base + TMC_MODE);
+	writel_relaxed(TMC_FFCR_EN_FMT | TMC_FFCR_EN_TI,
+		       drvdata->base + TMC_FFCR);
+	writel_relaxed(0x0, drvdata->base + TMC_BUFWM);
+	tmc_enable_hw(drvdata);
+
+	CS_LOCK(drvdata->base);
+}
+
+static int tmc_enable(struct tmc_drvdata *drvdata, enum tmc_mode mode)
+{
+	int ret;
+	unsigned long flags;
+
+	ret = clk_prepare_enable(drvdata->clk);
+	if (ret)
+		return ret;
+
+	spin_lock_irqsave(&drvdata->spinlock, flags);
+	if (drvdata->reading) {
+		spin_unlock_irqrestore(&drvdata->spinlock, flags);
+		clk_disable_unprepare(drvdata->clk);
+		return -EBUSY;
+	}
+
+	if (drvdata->config_type == TMC_CONFIG_TYPE_ETB) {
+		tmc_etb_enable_hw(drvdata);
+	} else if (drvdata->config_type == TMC_CONFIG_TYPE_ETR) {
+		tmc_etr_enable_hw(drvdata);
+	} else {
+		if (mode == TMC_MODE_CIRCULAR_BUFFER)
+			tmc_etb_enable_hw(drvdata);
+		else
+			tmc_etf_enable_hw(drvdata);
+	}
+	drvdata->enable = true;
+	spin_unlock_irqrestore(&drvdata->spinlock, flags);
+
+	dev_info(drvdata->dev, "TMC enabled\n");
+	return 0;
+}
+
+static int tmc_enable_sink(struct coresight_device *csdev)
+{
+	struct tmc_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+
+	return tmc_enable(drvdata, TMC_MODE_CIRCULAR_BUFFER);
+}
+
+static int tmc_enable_link(struct coresight_device *csdev, int inport,
+			   int outport)
+{
+	struct tmc_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+
+	return tmc_enable(drvdata, TMC_MODE_HARDWARE_FIFO);
+}
+
+static void tmc_etb_dump_hw(struct tmc_drvdata *drvdata)
+{
+	enum tmc_mem_intf_width memwidth;
+	u8 memwords;
+	char *bufp;
+	u32 read_data;
+	int i;
+
+	memwidth = BMVAL(readl_relaxed(drvdata->base + CORESIGHT_DEVID), 8, 10);
+	if (memwidth == TMC_MEM_INTF_WIDTH_32BITS)
+		memwords = 1;
+	else if (memwidth == TMC_MEM_INTF_WIDTH_64BITS)
+		memwords = 2;
+	else if (memwidth == TMC_MEM_INTF_WIDTH_128BITS)
+		memwords = 4;
+	else
+		memwords = 8;
+
+	bufp = drvdata->buf;
+	while (1) {
+		for (i = 0; i < memwords; i++) {
+			read_data = readl_relaxed(drvdata->base + TMC_RRD);
+			if (read_data == 0xFFFFFFFF)
+				return;
+			memcpy(bufp, &read_data, 4);
+			bufp += 4;
+		}
+	}
+}
+
+static void tmc_etb_disable_hw(struct tmc_drvdata *drvdata)
+{
+	CS_UNLOCK(drvdata->base);
+
+	tmc_flush_and_stop(drvdata);
+	tmc_etb_dump_hw(drvdata);
+	tmc_disable_hw(drvdata);
+
+	CS_LOCK(drvdata->base);
+}
+
+static void tmc_etr_dump_hw(struct tmc_drvdata *drvdata)
+{
+	u32 rwp, val;
+
+	rwp = readl_relaxed(drvdata->base + TMC_RWP);
+	val = readl_relaxed(drvdata->base + TMC_STS);
+
+	/* How much memory do we still have */
+	if (val & BIT(0))
+		drvdata->buf = drvdata->vaddr + rwp - drvdata->paddr;
+	else
+		drvdata->buf = drvdata->vaddr;
+}
+
+static void tmc_etr_disable_hw(struct tmc_drvdata *drvdata)
+{
+	CS_UNLOCK(drvdata->base);
+
+	tmc_flush_and_stop(drvdata);
+	tmc_etr_dump_hw(drvdata);
+	tmc_disable_hw(drvdata);
+
+	CS_LOCK(drvdata->base);
+}
+
+static void tmc_etf_disable_hw(struct tmc_drvdata *drvdata)
+{
+	CS_UNLOCK(drvdata->base);
+
+	tmc_flush_and_stop(drvdata);
+	tmc_disable_hw(drvdata);
+
+	CS_LOCK(drvdata->base);
+}
+
+static void tmc_disable(struct tmc_drvdata *drvdata, enum tmc_mode mode)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&drvdata->spinlock, flags);
+	if (drvdata->reading)
+		goto out;
+
+	if (drvdata->config_type == TMC_CONFIG_TYPE_ETB) {
+		tmc_etb_disable_hw(drvdata);
+	} else if (drvdata->config_type == TMC_CONFIG_TYPE_ETR) {
+		tmc_etr_disable_hw(drvdata);
+	} else {
+		if (mode == TMC_MODE_CIRCULAR_BUFFER)
+			tmc_etb_disable_hw(drvdata);
+		else
+			tmc_etf_disable_hw(drvdata);
+	}
+out:
+	drvdata->enable = false;
+	spin_unlock_irqrestore(&drvdata->spinlock, flags);
+
+	clk_disable_unprepare(drvdata->clk);
+
+	dev_info(drvdata->dev, "TMC disabled\n");
+}
+
+static void tmc_disable_sink(struct coresight_device *csdev)
+{
+	struct tmc_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+
+	tmc_disable(drvdata, TMC_MODE_CIRCULAR_BUFFER);
+}
+
+static void tmc_disable_link(struct coresight_device *csdev, int inport,
+			     int outport)
+{
+	struct tmc_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+
+	tmc_disable(drvdata, TMC_MODE_HARDWARE_FIFO);
+}
+
+static const struct coresight_ops_sink tmc_sink_ops = {
+	.enable		= tmc_enable_sink,
+	.disable	= tmc_disable_sink,
+};
+
+static const struct coresight_ops_link tmc_link_ops = {
+	.enable		= tmc_enable_link,
+	.disable	= tmc_disable_link,
+};
+
+static const struct coresight_ops tmc_etb_cs_ops = {
+	.sink_ops	= &tmc_sink_ops,
+};
+
+static const struct coresight_ops tmc_etr_cs_ops = {
+	.sink_ops	= &tmc_sink_ops,
+};
+
+static const struct coresight_ops tmc_etf_cs_ops = {
+	.sink_ops	= &tmc_sink_ops,
+	.link_ops	= &tmc_link_ops,
+};
+
+static int tmc_read_prepare(struct tmc_drvdata *drvdata)
+{
+	int ret;
+	unsigned long flags;
+	enum tmc_mode mode;
+
+	spin_lock_irqsave(&drvdata->spinlock, flags);
+	if (!drvdata->enable)
+		goto out;
+
+	if (drvdata->config_type == TMC_CONFIG_TYPE_ETB) {
+		tmc_etb_disable_hw(drvdata);
+	} else if (drvdata->config_type == TMC_CONFIG_TYPE_ETR) {
+		tmc_etr_disable_hw(drvdata);
+	} else {
+		mode = readl_relaxed(drvdata->base + TMC_MODE);
+		if (mode == TMC_MODE_CIRCULAR_BUFFER) {
+			tmc_etb_disable_hw(drvdata);
+		} else {
+			ret = -ENODEV;
+			goto err;
+		}
+	}
+out:
+	drvdata->reading = true;
+	spin_unlock_irqrestore(&drvdata->spinlock, flags);
+
+	dev_info(drvdata->dev, "TMC read start\n");
+	return 0;
+err:
+	spin_unlock_irqrestore(&drvdata->spinlock, flags);
+	return ret;
+}
+
+static void tmc_read_unprepare(struct tmc_drvdata *drvdata)
+{
+	unsigned long flags;
+	enum tmc_mode mode;
+
+	spin_lock_irqsave(&drvdata->spinlock, flags);
+	if (!drvdata->enable)
+		goto out;
+
+	if (drvdata->config_type == TMC_CONFIG_TYPE_ETB) {
+		tmc_etb_enable_hw(drvdata);
+	} else if (drvdata->config_type == TMC_CONFIG_TYPE_ETR) {
+		tmc_etr_enable_hw(drvdata);
+	} else {
+		mode = readl_relaxed(drvdata->base + TMC_MODE);
+		if (mode == TMC_MODE_CIRCULAR_BUFFER)
+			tmc_etb_enable_hw(drvdata);
+	}
+out:
+	drvdata->reading = false;
+	spin_unlock_irqrestore(&drvdata->spinlock, flags);
+
+	dev_info(drvdata->dev, "TMC read end\n");
+}
+
+static int tmc_open(struct inode *inode, struct file *file)
+{
+	struct tmc_drvdata *drvdata = container_of(file->private_data,
+						   struct tmc_drvdata, miscdev);
+	int ret = 0;
+
+	if (drvdata->read_count++)
+		goto out;
+
+	ret = tmc_read_prepare(drvdata);
+	if (ret)
+		return ret;
+out:
+	nonseekable_open(inode, file);
+
+	dev_dbg(drvdata->dev, "%s: successfully opened\n", __func__);
+	return 0;
+}
+
+static ssize_t tmc_read(struct file *file, char __user *data, size_t len,
+			loff_t *ppos)
+{
+	struct tmc_drvdata *drvdata = container_of(file->private_data,
+						   struct tmc_drvdata, miscdev);
+	char *bufp = drvdata->buf + *ppos;
+
+	if (*ppos + len > drvdata->size)
+		len = drvdata->size - *ppos;
+
+	if (drvdata->config_type == TMC_CONFIG_TYPE_ETR) {
+		if (bufp == (char *)(drvdata->vaddr + drvdata->size))
+			bufp = drvdata->vaddr;
+		else if (bufp > (char *)(drvdata->vaddr + drvdata->size))
+			bufp -= drvdata->size;
+		if ((bufp + len) > (char *)(drvdata->vaddr + drvdata->size))
+			len = (char *)(drvdata->vaddr + drvdata->size) - bufp;
+	}
+
+	if (copy_to_user(data, bufp, len)) {
+		dev_dbg(drvdata->dev, "%s: copy_to_user failed\n", __func__);
+		return -EFAULT;
+	}
+
+	*ppos += len;
+
+	dev_dbg(drvdata->dev, "%s: %d bytes copied, %d bytes left\n",
+		__func__, len, (int) (drvdata->size - *ppos));
+	return len;
+}
+
+static int tmc_release(struct inode *inode, struct file *file)
+{
+	struct tmc_drvdata *drvdata = container_of(file->private_data,
+						   struct tmc_drvdata, miscdev);
+
+	if (--drvdata->read_count) {
+		if (drvdata->read_count < 0) {
+			dev_err(drvdata->dev, "mismatched close\n");
+			drvdata->read_count = 0;
+		}
+		goto out;
+	}
+
+	tmc_read_unprepare(drvdata);
+out:
+	dev_dbg(drvdata->dev, "%s: released\n", __func__);
+	return 0;
+}
+
+static const struct file_operations tmc_fops = {
+	.owner		= THIS_MODULE,
+	.open		= tmc_open,
+	.read		= tmc_read,
+	.release	= tmc_release,
+	.llseek		= no_llseek,
+};
+
+static ssize_t trigger_cntr_show(struct device *dev,
+			    struct device_attribute *attr, char *buf)
+{
+	struct tmc_drvdata *drvdata = dev_get_drvdata(dev->parent);
+	unsigned long val = drvdata->trigger_cntr;
+
+	return sprintf(buf, "%#lx\n", val);
+}
+
+static ssize_t trigger_cntr_store(struct device *dev,
+			     struct device_attribute *attr,
+			     const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct tmc_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	ret = kstrtoul(buf, 16, &val);
+	if (ret)
+		return ret;
+
+	drvdata->trigger_cntr = val;
+	return size;
+}
+static DEVICE_ATTR_RW(trigger_cntr);
+
+static struct attribute *coresight_etb_attrs[] = {
+	&dev_attr_trigger_cntr.attr,
+	NULL,
+};
+ATTRIBUTE_GROUPS(coresight_etb);
+
+static struct attribute *coresight_etr_attrs[] = {
+	&dev_attr_trigger_cntr.attr,
+	NULL,
+};
+ATTRIBUTE_GROUPS(coresight_etr);
+
+static struct attribute *coresight_etf_attrs[] = {
+	&dev_attr_trigger_cntr.attr,
+	NULL,
+};
+ATTRIBUTE_GROUPS(coresight_etf);
+
+static int tmc_probe(struct amba_device *adev, const struct amba_id *id)
+{
+	int ret = 0;
+	u32 devid;
+	void __iomem *base;
+	struct device *dev = &adev->dev;
+	struct coresight_platform_data *pdata = NULL;
+	struct tmc_drvdata *drvdata;
+	struct resource *res = &adev->res;
+	struct coresight_desc *desc;
+	struct device_node *np = adev->dev.of_node;
+
+	if (np) {
+		pdata = of_get_coresight_platform_data(dev, np);
+		if (IS_ERR(pdata))
+			return PTR_ERR(pdata);
+		adev->dev.platform_data = pdata;
+	}
+
+	drvdata = devm_kzalloc(dev, sizeof(*drvdata), GFP_KERNEL);
+	if (!drvdata)
+		return -ENOMEM;
+
+	drvdata->dev = &adev->dev;
+	dev_set_drvdata(dev, drvdata);
+
+	/* Validity for the resource is already checked by the AMBA core */
+	base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	drvdata->base = base;
+
+	spin_lock_init(&drvdata->spinlock);
+
+	drvdata->clk = adev->pclk;
+	ret = clk_prepare_enable(drvdata->clk);
+	if (ret)
+		return ret;
+
+	devid = readl_relaxed(drvdata->base + CORESIGHT_DEVID);
+	drvdata->config_type = BMVAL(devid, 6, 7);
+
+	if (drvdata->config_type == TMC_CONFIG_TYPE_ETR) {
+		if (np)
+			ret = of_property_read_u32(np,
+						   "arm,buffer-size",
+						   &drvdata->size);
+		if (ret)
+			drvdata->size = SZ_1M;
+	} else {
+		drvdata->size = readl_relaxed(drvdata->base + TMC_RSZ) * 4;
+	}
+
+	clk_disable_unprepare(drvdata->clk);
+
+	if (drvdata->config_type == TMC_CONFIG_TYPE_ETR) {
+		drvdata->vaddr = dma_alloc_coherent(dev, drvdata->size,
+						&drvdata->paddr, GFP_KERNEL);
+		if (!drvdata->vaddr)
+			return -ENOMEM;
+
+		memset(drvdata->vaddr, 0, drvdata->size);
+		drvdata->buf = drvdata->vaddr;
+	} else {
+		drvdata->buf = devm_kzalloc(dev, drvdata->size, GFP_KERNEL);
+		if (!drvdata->buf)
+			return -ENOMEM;
+	}
+
+	desc = devm_kzalloc(dev, sizeof(*desc), GFP_KERNEL);
+	if (!desc) {
+		ret = -ENOMEM;
+		goto err_devm_kzalloc;
+	}
+
+	desc->pdata = pdata;
+	desc->dev = dev;
+	desc->subtype.sink_subtype = CORESIGHT_DEV_SUBTYPE_SINK_BUFFER;
+
+	if (drvdata->config_type == TMC_CONFIG_TYPE_ETB) {
+		desc->type = CORESIGHT_DEV_TYPE_SINK;
+		desc->ops = &tmc_etb_cs_ops;
+		desc->groups = coresight_etb_groups;
+	} else if (drvdata->config_type == TMC_CONFIG_TYPE_ETR) {
+		desc->type = CORESIGHT_DEV_TYPE_SINK;
+		desc->ops = &tmc_etr_cs_ops;
+		desc->groups = coresight_etr_groups;
+	} else {
+		desc->type = CORESIGHT_DEV_TYPE_LINKSINK;
+		desc->subtype.link_subtype = CORESIGHT_DEV_SUBTYPE_LINK_FIFO;
+		desc->ops = &tmc_etf_cs_ops;
+		desc->groups = coresight_etf_groups;
+	}
+
+	drvdata->csdev = coresight_register(desc);
+	if (IS_ERR(drvdata->csdev)) {
+		ret = PTR_ERR(drvdata->csdev);
+		goto err_devm_kzalloc;
+	}
+
+	drvdata->miscdev.name = pdata->name;
+	drvdata->miscdev.minor = MISC_DYNAMIC_MINOR;
+	drvdata->miscdev.fops = &tmc_fops;
+	ret = misc_register(&drvdata->miscdev);
+	if (ret)
+		goto err_misc_register;
+
+	dev_info(dev, "TMC initialized\n");
+	return 0;
+
+err_misc_register:
+	coresight_unregister(drvdata->csdev);
+err_devm_kzalloc:
+	if (drvdata->config_type == TMC_CONFIG_TYPE_ETR)
+		dma_free_coherent(dev, drvdata->size,
+				&drvdata->paddr, GFP_KERNEL);
+	return ret;
+}
+
+static int tmc_remove(struct amba_device *adev)
+{
+	struct tmc_drvdata *drvdata = amba_get_drvdata(adev);
+
+	misc_deregister(&drvdata->miscdev);
+	coresight_unregister(drvdata->csdev);
+	if (drvdata->config_type == TMC_CONFIG_TYPE_ETR)
+		dma_free_coherent(drvdata->dev, drvdata->size,
+				  &drvdata->paddr, GFP_KERNEL);
+
+	return 0;
+}
+
+static struct amba_id tmc_ids[] = {
+	{
+		.id     = 0x0003b961,
+		.mask   = 0x0003ffff,
+	},
+	{ 0, 0},
+};
+
+static struct amba_driver tmc_driver = {
+	.drv = {
+		.name   = "coresight-tmc",
+		.owner  = THIS_MODULE,
+	},
+	.probe		= tmc_probe,
+	.remove		= tmc_remove,
+	.id_table	= tmc_ids,
+};
+
+static int __init tmc_init(void)
+{
+	return amba_driver_register(&tmc_driver);
+}
+module_init(tmc_init);
+
+static void __exit tmc_exit(void)
+{
+	amba_driver_unregister(&tmc_driver);
+}
+module_exit(tmc_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("CoreSight Trace Memory Controller driver");
diff --git a/drivers/coresight/coresight-tpiu.c b/drivers/coresight/coresight-tpiu.c
new file mode 100644
index 000000000000..ae101082791a
--- /dev/null
+++ b/drivers/coresight/coresight-tpiu.c
@@ -0,0 +1,217 @@
+/* Copyright (c) 2011-2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/coresight.h>
+#include <linux/amba/bus.h>
+
+#include "coresight-priv.h"
+
+#define TPIU_SUPP_PORTSZ	0x000
+#define TPIU_CURR_PORTSZ	0x004
+#define TPIU_SUPP_TRIGMODES	0x100
+#define TPIU_TRIG_CNTRVAL	0x104
+#define TPIU_TRIG_MULT		0x108
+#define TPIU_SUPP_TESTPATM	0x200
+#define TPIU_CURR_TESTPATM	0x204
+#define TPIU_TEST_PATREPCNTR	0x208
+#define TPIU_FFSR		0x300
+#define TPIU_FFCR		0x304
+#define TPIU_FSYNC_CNTR		0x308
+#define TPIU_EXTCTL_INPORT	0x400
+#define TPIU_EXTCTL_OUTPORT	0x404
+#define TPIU_ITTRFLINACK	0xee4
+#define TPIU_ITTRFLIN		0xee8
+#define TPIU_ITATBDATA0		0xeec
+#define TPIU_ITATBCTR2		0xef0
+#define TPIU_ITATBCTR1		0xef4
+#define TPIU_ITATBCTR0		0xef8
+
+/** register definition **/
+/* FFCR - 0x304 */
+#define FFCR_FON_MAN		BIT(6)
+
+/**
+ * @base:	memory mapped base address for this component.
+ * @dev:	the device entity associated to this component.
+ * @csdev:	component vitals needed by the framework.
+ * @clk:	the clock this component is associated to.
+ */
+struct tpiu_drvdata {
+	void __iomem		*base;
+	struct device		*dev;
+	struct coresight_device	*csdev;
+	struct clk		*clk;
+};
+
+static void tpiu_enable_hw(struct tpiu_drvdata *drvdata)
+{
+	CS_UNLOCK(drvdata->base);
+
+	/* TODO: fill this up */
+
+	CS_LOCK(drvdata->base);
+}
+
+static int tpiu_enable(struct coresight_device *csdev)
+{
+	struct tpiu_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+	int ret;
+
+	ret = clk_prepare_enable(drvdata->clk);
+	if (ret)
+		return ret;
+
+	tpiu_enable_hw(drvdata);
+
+	dev_info(drvdata->dev, "TPIU enabled\n");
+	return 0;
+}
+
+static void tpiu_disable_hw(struct tpiu_drvdata *drvdata)
+{
+	CS_UNLOCK(drvdata->base);
+
+	/* Clear formatter controle reg. */
+	writel_relaxed(0x0, drvdata->base + TPIU_FFCR);
+	/* Generate manual flush */
+	writel_relaxed(FFCR_FON_MAN, drvdata->base + TPIU_FFCR);
+
+	CS_LOCK(drvdata->base);
+}
+
+static void tpiu_disable(struct coresight_device *csdev)
+{
+	struct tpiu_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+
+	tpiu_disable_hw(drvdata);
+
+	clk_disable_unprepare(drvdata->clk);
+
+	dev_info(drvdata->dev, "TPIU disabled\n");
+}
+
+static const struct coresight_ops_sink tpiu_sink_ops = {
+	.enable		= tpiu_enable,
+	.disable	= tpiu_disable,
+};
+
+static const struct coresight_ops tpiu_cs_ops = {
+	.sink_ops	= &tpiu_sink_ops,
+};
+
+static int tpiu_probe(struct amba_device *adev, const struct amba_id *id)
+{
+	int ret;
+	void __iomem *base;
+	struct device *dev = &adev->dev;
+	struct coresight_platform_data *pdata = NULL;
+	struct tpiu_drvdata *drvdata;
+	struct resource *res = &adev->res;
+	struct coresight_desc *desc;
+	struct device_node *np = adev->dev.of_node;
+
+	if (np) {
+		pdata = of_get_coresight_platform_data(dev, np);
+		if (IS_ERR(pdata))
+			return PTR_ERR(pdata);
+		adev->dev.platform_data = pdata;
+	}
+
+	drvdata = devm_kzalloc(dev, sizeof(*drvdata), GFP_KERNEL);
+	if (!drvdata)
+		return -ENOMEM;
+
+	drvdata->dev = &adev->dev;
+	dev_set_drvdata(dev, drvdata);
+
+	/* Validity for the resource is already checked by the AMBA core */
+	base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	drvdata->base = base;
+
+	drvdata->clk = adev->pclk;
+	ret = clk_prepare_enable(drvdata->clk);
+	if (ret)
+		return ret;
+
+	/* Disable tpiu to support older devices */
+	tpiu_disable_hw(drvdata);
+
+	clk_disable_unprepare(drvdata->clk);
+
+	desc = devm_kzalloc(dev, sizeof(*desc), GFP_KERNEL);
+	if (!desc)
+		return -ENOMEM;
+
+	desc->type = CORESIGHT_DEV_TYPE_SINK;
+	desc->subtype.sink_subtype = CORESIGHT_DEV_SUBTYPE_SINK_PORT;
+	desc->ops = &tpiu_cs_ops;
+	desc->pdata = pdata;
+	desc->dev = dev;
+	drvdata->csdev = coresight_register(desc);
+	if (IS_ERR(drvdata->csdev))
+		return PTR_ERR(drvdata->csdev);
+
+	dev_info(dev, "TPIU initialized\n");
+	return 0;
+}
+
+static int tpiu_remove(struct amba_device *adev)
+{
+	struct tpiu_drvdata *drvdata = amba_get_drvdata(adev);
+
+	coresight_unregister(drvdata->csdev);
+	return 0;
+}
+
+static struct amba_id tpiu_ids[] = {
+	{
+		.id	= 0x0003b912,
+		.mask	= 0x0003ffff,
+	},
+	{ 0, 0},
+};
+
+static struct amba_driver tpiu_driver = {
+	.drv = {
+		.name	= "coresight-tpiu",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= tpiu_probe,
+	.remove		= tpiu_remove,
+	.id_table	= tpiu_ids,
+};
+
+static int __init tpiu_init(void)
+{
+	return amba_driver_register(&tpiu_driver);
+}
+module_init(tpiu_init);
+
+static void __exit tpiu_exit(void)
+{
+	amba_driver_unregister(&tpiu_driver);
+}
+module_exit(tpiu_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("CoreSight Trace Port Interface Unit driver");
diff --git a/drivers/coresight/coresight.c b/drivers/coresight/coresight.c
new file mode 100644
index 000000000000..6e0181f84425
--- /dev/null
+++ b/drivers/coresight/coresight.c
@@ -0,0 +1,717 @@
+/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/export.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/clk.h>
+#include <linux/coresight.h>
+#include <linux/of_platform.h>
+#include <linux/delay.h>
+
+#include "coresight-priv.h"
+
+static DEFINE_MUTEX(coresight_mutex);
+
+static int coresight_id_match(struct device *dev, void *data)
+{
+	int trace_id, i_trace_id;
+	struct coresight_device *csdev, *i_csdev;
+
+	csdev = data;
+	i_csdev = to_coresight_device(dev);
+
+	/*
+	 * No need to care about oneself and components that are not
+	 * sources or not enabled
+	 */
+	if (i_csdev == csdev || !i_csdev->enable ||
+	    i_csdev->type != CORESIGHT_DEV_TYPE_SOURCE)
+		return 0;
+
+	/* Get the source ID for both compoment */
+	trace_id = source_ops(csdev)->trace_id(csdev);
+	i_trace_id = source_ops(i_csdev)->trace_id(i_csdev);
+
+	/* All you need is one */
+	if (trace_id == i_trace_id)
+		return 1;
+
+	return 0;
+}
+
+static int coresight_source_is_unique(struct coresight_device *csdev)
+{
+	int trace_id = source_ops(csdev)->trace_id(csdev);
+
+	/* this shouldn't happen */
+	if (trace_id < 0)
+		return 0;
+
+	return !bus_for_each_dev(&coresight_bustype, NULL,
+				 csdev, coresight_id_match);
+}
+
+static int coresight_find_link_inport(struct coresight_device *csdev)
+{
+	int i;
+	struct coresight_device *parent;
+	struct coresight_connection *conn;
+
+	parent = container_of(csdev->path_link.next,
+			      struct coresight_device, path_link);
+
+	for (i = 0; i < parent->nr_outport; i++) {
+		conn = &parent->conns[i];
+		if (conn->child_dev == csdev)
+			return conn->child_port;
+	}
+
+	dev_err(&csdev->dev, "couldn't find inport, parent: %s, child: %s\n",
+		dev_name(&parent->dev), dev_name(&csdev->dev));
+
+	return 0;
+}
+
+static int coresight_find_link_outport(struct coresight_device *csdev)
+{
+	int i;
+	struct coresight_device *child;
+	struct coresight_connection *conn;
+
+	child = container_of(csdev->path_link.prev,
+			     struct coresight_device, path_link);
+
+	for (i = 0; i < csdev->nr_outport; i++) {
+		conn = &csdev->conns[i];
+		if (conn->child_dev == child)
+			return conn->outport;
+	}
+
+	dev_err(&csdev->dev, "couldn't find outport, parent: %s, child: %s\n",
+		dev_name(&csdev->dev), dev_name(&child->dev));
+
+	return 0;
+}
+
+static int coresight_enable_sink(struct coresight_device *csdev)
+{
+	int ret;
+
+	if (!csdev->enable) {
+		if (sink_ops(csdev)->enable) {
+			ret = sink_ops(csdev)->enable(csdev);
+			if (ret)
+				return ret;
+		}
+		csdev->enable = true;
+	}
+
+	atomic_inc(csdev->refcnt);
+
+	return 0;
+}
+
+static void coresight_disable_sink(struct coresight_device *csdev)
+{
+	if (atomic_dec_return(csdev->refcnt) == 0) {
+		if (sink_ops(csdev)->disable) {
+			sink_ops(csdev)->disable(csdev);
+			csdev->enable = false;
+		}
+	}
+}
+
+static int coresight_enable_link(struct coresight_device *csdev)
+{
+	int ret;
+	int link_subtype;
+	int refport, inport, outport;
+
+	inport = coresight_find_link_inport(csdev);
+	outport = coresight_find_link_outport(csdev);
+	link_subtype = csdev->subtype.link_subtype;
+
+	if (link_subtype == CORESIGHT_DEV_SUBTYPE_LINK_MERG)
+		refport = inport;
+	else if (link_subtype == CORESIGHT_DEV_SUBTYPE_LINK_SPLIT)
+		refport = outport;
+	else
+		refport = 0;
+
+	if (atomic_inc_return(&csdev->refcnt[refport]) == 1) {
+		if (link_ops(csdev)->enable) {
+			ret = link_ops(csdev)->enable(csdev, inport, outport);
+			if (ret)
+				return ret;
+		}
+	}
+
+	csdev->enable = true;
+
+	return 0;
+}
+
+static void coresight_disable_link(struct coresight_device *csdev)
+{
+	int i, nr_conns;
+	int link_subtype;
+	int refport, inport, outport;
+
+	inport = coresight_find_link_inport(csdev);
+	outport = coresight_find_link_outport(csdev);
+	link_subtype = csdev->subtype.link_subtype;
+
+	if (link_subtype == CORESIGHT_DEV_SUBTYPE_LINK_MERG) {
+		refport = inport;
+		nr_conns = csdev->nr_inport;
+	} else if (link_subtype == CORESIGHT_DEV_SUBTYPE_LINK_SPLIT) {
+		refport = outport;
+		nr_conns = csdev->nr_outport;
+	} else {
+		refport = 0;
+		nr_conns = 1;
+	}
+
+	if (atomic_dec_return(&csdev->refcnt[refport]) == 0) {
+		if (link_ops(csdev)->disable)
+			link_ops(csdev)->disable(csdev, inport, outport);
+	}
+
+	for (i = 0; i < nr_conns; i++)
+		if (atomic_read(&csdev->refcnt[i]) != 0)
+			return;
+
+	csdev->enable = false;
+}
+
+static int coresight_enable_source(struct coresight_device *csdev)
+{
+	int ret;
+
+	if (!coresight_source_is_unique(csdev)) {
+		dev_warn(&csdev->dev, "traceID %d not unique\n",
+			 source_ops(csdev)->trace_id(csdev));
+		return -EINVAL;
+	}
+
+	if (!csdev->enable) {
+		if (source_ops(csdev)->enable) {
+			ret = source_ops(csdev)->enable(csdev);
+			if (ret)
+				return ret;
+		}
+		csdev->enable = true;
+	}
+
+	atomic_inc(csdev->refcnt);
+
+	return 0;
+}
+
+static void coresight_disable_source(struct coresight_device *csdev)
+{
+	if (atomic_dec_return(csdev->refcnt) == 0) {
+		if (source_ops(csdev)->disable) {
+			source_ops(csdev)->disable(csdev);
+			csdev->enable = false;
+		}
+	}
+}
+
+static int coresight_enable_path(struct list_head *path)
+{
+	int ret = 0;
+	struct coresight_device *cd;
+
+	list_for_each_entry(cd, path, path_link) {
+		if (cd == list_first_entry(path, struct coresight_device,
+					   path_link)) {
+			ret = coresight_enable_sink(cd);
+		} else if (list_is_last(&cd->path_link, path)) {
+			/*
+			 * Don't enable the source just yet - this needs to
+			 * happen at the very end when all links and sink
+			 * along the path have been configured properly.
+			 */
+			;
+		} else {
+			ret = coresight_enable_link(cd);
+		}
+		if (ret)
+			goto err;
+	}
+
+	return 0;
+err:
+	list_for_each_entry_continue_reverse(cd, path, path_link) {
+		if (cd == list_first_entry(path, struct coresight_device,
+					   path_link)) {
+			coresight_disable_sink(cd);
+		} else if (list_is_last(&cd->path_link, path)) {
+			;
+		} else {
+			coresight_disable_link(cd);
+		}
+	}
+
+	return ret;
+}
+
+static int coresight_disable_path(struct list_head *path)
+{
+	struct coresight_device *cd;
+
+	list_for_each_entry_reverse(cd, path, path_link) {
+		if (cd == list_first_entry(path, struct coresight_device,
+					   path_link)) {
+			coresight_disable_sink(cd);
+		} else if (list_is_last(&cd->path_link, path)) {
+			/*
+			 * The source has already been stopped, no need
+			 * to do it again here.
+			 */
+			;
+		} else {
+			coresight_disable_link(cd);
+		}
+	}
+
+	return 0;
+}
+
+static int coresight_build_paths(struct coresight_device *csdev,
+				 struct list_head *path,
+				 bool enable)
+{
+	int i, ret = -EINVAL;
+	struct coresight_connection *conn;
+
+	list_add(&csdev->path_link, path);
+
+	if (csdev->type == CORESIGHT_DEV_TYPE_SINK && csdev->activated) {
+		if (enable)
+			ret = coresight_enable_path(path);
+		else
+			ret = coresight_disable_path(path);
+	} else {
+		for (i = 0; i < csdev->nr_outport; i++) {
+			conn = &csdev->conns[i];
+			if (coresight_build_paths(conn->child_dev,
+						    path, enable) == 0)
+				ret = 0;
+		}
+	}
+
+	if (list_first_entry(path, struct coresight_device, path_link) != csdev)
+		dev_err(&csdev->dev, "wrong device in %s\n", __func__);
+
+	list_del(&csdev->path_link);
+
+	return ret;
+}
+
+int coresight_enable(struct coresight_device *csdev)
+{
+	int ret = 0;
+	LIST_HEAD(path);
+
+	mutex_lock(&coresight_mutex);
+	if (csdev->type != CORESIGHT_DEV_TYPE_SOURCE) {
+		ret = -EINVAL;
+		dev_err(&csdev->dev, "wrong device type in %s\n", __func__);
+		goto out;
+	}
+	if (csdev->enable)
+		goto out;
+
+	if (coresight_build_paths(csdev, &path, true)) {
+		dev_err(&csdev->dev, "building path(s) failed\n");
+		goto out;
+	}
+
+	if (coresight_enable_source(csdev))
+		dev_err(&csdev->dev, "source enable failed\n");
+out:
+	mutex_unlock(&coresight_mutex);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(coresight_enable);
+
+void coresight_disable(struct coresight_device *csdev)
+{
+	LIST_HEAD(path);
+
+	mutex_lock(&coresight_mutex);
+	if (csdev->type != CORESIGHT_DEV_TYPE_SOURCE) {
+		dev_err(&csdev->dev, "wrong device type in %s\n", __func__);
+		goto out;
+	}
+	if (!csdev->enable)
+		goto out;
+
+	coresight_disable_source(csdev);
+	if (coresight_build_paths(csdev, &path, false))
+		dev_err(&csdev->dev, "releasing path(s) failed\n");
+
+out:
+	mutex_unlock(&coresight_mutex);
+}
+EXPORT_SYMBOL_GPL(coresight_disable);
+
+static ssize_t enable_sink_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct coresight_device *csdev = to_coresight_device(dev);
+
+	return scnprintf(buf, PAGE_SIZE, "%u\n", (unsigned)csdev->activated);
+}
+
+static ssize_t enable_sink_store(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf, size_t size)
+{
+	int ret;
+	unsigned long val;
+	struct coresight_device *csdev = to_coresight_device(dev);
+
+	ret = kstrtoul(buf, 10, &val);
+	if (ret)
+		return ret;
+
+	if (val)
+		csdev->activated = true;
+	else
+		csdev->activated = false;
+
+	return size;
+
+}
+static DEVICE_ATTR_RW(enable_sink);
+
+static ssize_t enable_source_show(struct device *dev,
+				  struct device_attribute *attr, char *buf)
+{
+	struct coresight_device *csdev = to_coresight_device(dev);
+
+	return scnprintf(buf, PAGE_SIZE, "%u\n", (unsigned)csdev->enable);
+}
+
+static ssize_t enable_source_store(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf, size_t size)
+{
+	int ret = 0;
+	unsigned long val;
+	struct coresight_device *csdev = to_coresight_device(dev);
+
+	ret = kstrtoul(buf, 10, &val);
+	if (ret)
+		return ret;
+
+	if (val) {
+		ret = coresight_enable(csdev);
+		if (ret)
+			return ret;
+	} else {
+		coresight_disable(csdev);
+	}
+
+	return size;
+}
+static DEVICE_ATTR_RW(enable_source);
+
+static struct attribute *coresight_sink_attrs[] = {
+	&dev_attr_enable_sink.attr,
+	NULL,
+};
+ATTRIBUTE_GROUPS(coresight_sink);
+
+static struct attribute *coresight_source_attrs[] = {
+	&dev_attr_enable_source.attr,
+	NULL,
+};
+ATTRIBUTE_GROUPS(coresight_source);
+
+static struct device_type coresight_dev_type[] = {
+	{
+		.name = "none",
+	},
+	{
+		.name = "sink",
+		.groups = coresight_sink_groups,
+	},
+	{
+		.name = "link",
+	},
+	{
+		.name = "linksink",
+		.groups = coresight_sink_groups,
+	},
+	{
+		.name = "source",
+		.groups = coresight_source_groups,
+	},
+};
+
+static void coresight_device_release(struct device *dev)
+{
+	struct coresight_device *csdev = to_coresight_device(dev);
+
+	kfree(csdev);
+}
+
+static int coresight_orphan_match(struct device *dev, void *data)
+{
+	int i;
+	bool still_orphan = false;
+	struct coresight_device *csdev, *i_csdev;
+	struct coresight_connection *conn;
+
+	csdev = data;
+	i_csdev = to_coresight_device(dev);
+
+	/* No need to check oneself */
+	if (csdev == i_csdev)
+		return 0;
+
+	/* Move on to another component if no connection is orphan */
+	if (!i_csdev->orphan)
+		return 0;
+	/*
+	 * Circle throuch all the connection of that component.  If we find
+	 * an orphan connection whose name matches @csdev, link it.
+	 */
+	for (i = 0; i < i_csdev->nr_outport; i++)	{
+		conn = &i_csdev->conns[i];
+
+		/* We have found at least one orphan connection */
+		if (conn->child_dev == NULL) {
+			/* Does it match this newly added device? */
+			if (!strcmp(dev_name(&csdev->dev), conn->child_name))
+				conn->child_dev = csdev;
+		} else {
+			/* Too bad, this component still has an orphan */
+			still_orphan = true;
+		}
+	}
+
+	i_csdev->orphan = still_orphan;
+
+	/*
+	 * Returning '0' ensures that all known component on the
+	 * bus will be checked.
+	 */
+	return 0;
+}
+
+static void coresight_fixup_orphan_conns(struct coresight_device *csdev)
+{
+	/*
+	 * No need to check for a return value as orphan connection(s)
+	 * are hooked-up with each newly added component.
+	 */
+	bus_for_each_dev(&coresight_bustype, NULL,
+				 csdev, coresight_orphan_match);
+}
+
+
+static int coresight_name_match(struct device *dev, void *data)
+{
+	char *to_match;
+	struct coresight_device *i_csdev;
+
+	to_match = data;
+	i_csdev = to_coresight_device(dev);
+
+	if (!strcmp(to_match, dev_name(&i_csdev->dev)))
+		return 1;
+
+	return 0;
+}
+
+static void coresight_fixup_device_conns(struct coresight_device *csdev)
+{
+	int i;
+	struct device *dev = NULL;
+	struct coresight_connection *conn;
+
+	for (i = 0; i < csdev->nr_outport; i++) {
+		conn = &csdev->conns[i];
+		dev = bus_find_device(&coresight_bustype, NULL,
+				      (void *)conn->child_name,
+				      coresight_name_match);
+
+		if (dev) {
+			conn->child_dev = to_coresight_device(dev);
+		} else {
+			csdev->orphan = true;
+			conn->child_dev = NULL;
+		}
+	}
+}
+
+/**
+ * coresight_timeout - loop until a bit has changed to a specific state.
+ * @addr: base address of the area of interest.
+ * @offset: address of a register, starting from @addr.
+ * @position: the position of the bit of interest.
+ * @value: the value the bit should have.
+ *
+ * Return: 0 as soon as the bit has taken the desired state or -EAGAIN if
+ * TIMEOUT_US has elapsed, which ever happens first.
+ */
+
+int coresight_timeout(void __iomem *addr, u32 offset, int position, int value)
+{
+	int i;
+	u32 val;
+
+	for (i = TIMEOUT_US; i > 0; i--) {
+		val = __raw_readl(addr + offset);
+		/* waiting on the bit to go from 0 to 1 */
+		if (value) {
+			if (val & BIT(position))
+				return 0;
+		/* waiting on the bit to go from 1 to 0 */
+		} else {
+			if (!(val & BIT(position)))
+				return 0;
+		}
+
+		/*
+		 * Delay is arbitrary - the specification doesn't say how long
+		 * we are expected to wait.  Extra check required to make sure
+		 * we don't wait needlessly on the last iteration.
+		 */
+		if (i - 1)
+			udelay(1);
+	}
+
+	return -EAGAIN;
+}
+
+struct bus_type coresight_bustype = {
+	.name	= "coresight",
+};
+
+static int __init coresight_init(void)
+{
+	return bus_register(&coresight_bustype);
+}
+postcore_initcall(coresight_init);
+
+struct coresight_device *coresight_register(struct coresight_desc *desc)
+{
+	int i;
+	int ret;
+	int link_subtype;
+	int nr_refcnts = 1;
+	atomic_t *refcnts = NULL;
+	struct coresight_device *csdev;
+	struct coresight_connection *conns;
+
+	csdev = kzalloc(sizeof(*csdev), GFP_KERNEL);
+	if (!csdev) {
+		ret = -ENOMEM;
+		goto err_kzalloc_csdev;
+	}
+
+	if (desc->type == CORESIGHT_DEV_TYPE_LINK ||
+	    desc->type == CORESIGHT_DEV_TYPE_LINKSINK) {
+		link_subtype = desc->subtype.link_subtype;
+
+		if (link_subtype == CORESIGHT_DEV_SUBTYPE_LINK_MERG)
+			nr_refcnts = desc->pdata->nr_inport;
+		else if (link_subtype == CORESIGHT_DEV_SUBTYPE_LINK_SPLIT)
+			nr_refcnts = desc->pdata->nr_outport;
+	}
+
+	refcnts = kcalloc(nr_refcnts, sizeof(*refcnts), GFP_KERNEL);
+	if (!refcnts) {
+		ret = -ENOMEM;
+		goto err_kzalloc_refcnts;
+	}
+
+	csdev->refcnt = refcnts;
+
+	csdev->nr_inport = desc->pdata->nr_inport;
+	csdev->nr_outport = desc->pdata->nr_outport;
+	conns = kcalloc(csdev->nr_outport, sizeof(*conns), GFP_KERNEL);
+	if (!conns) {
+		ret = -ENOMEM;
+		goto err_kzalloc_conns;
+	}
+
+	for (i = 0; i < csdev->nr_outport; i++) {
+		conns[i].outport = desc->pdata->outports[i];
+		conns[i].child_name = desc->pdata->child_names[i];
+		conns[i].child_port = desc->pdata->child_ports[i];
+	}
+
+	csdev->conns = conns;
+
+	csdev->type = desc->type;
+	csdev->subtype = desc->subtype;
+	csdev->ops = desc->ops;
+	csdev->orphan = false;
+
+	csdev->dev.type = &coresight_dev_type[desc->type];
+	csdev->dev.groups = desc->groups;
+	csdev->dev.parent = desc->dev;
+	csdev->dev.release = coresight_device_release;
+	csdev->dev.bus = &coresight_bustype;
+	dev_set_name(&csdev->dev, "%s", desc->pdata->name);
+
+	ret = device_register(&csdev->dev);
+	if (ret)
+		goto err_device_register;
+
+	mutex_lock(&coresight_mutex);
+
+	coresight_fixup_device_conns(csdev);
+	coresight_fixup_orphan_conns(csdev);
+
+	mutex_unlock(&coresight_mutex);
+
+	return csdev;
+
+err_device_register:
+	kfree(conns);
+err_kzalloc_conns:
+	kfree(refcnts);
+err_kzalloc_refcnts:
+	kfree(csdev);
+err_kzalloc_csdev:
+	return ERR_PTR(ret);
+}
+EXPORT_SYMBOL_GPL(coresight_register);
+
+void coresight_unregister(struct coresight_device *csdev)
+{
+	mutex_lock(&coresight_mutex);
+
+	kfree(csdev->conns);
+	device_unregister(&csdev->dev);
+
+	mutex_unlock(&coresight_mutex);
+}
+EXPORT_SYMBOL_GPL(coresight_unregister);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/coresight/of_coresight.c b/drivers/coresight/of_coresight.c
new file mode 100644
index 000000000000..5030c0734508
--- /dev/null
+++ b/drivers/coresight/of_coresight.c
@@ -0,0 +1,204 @@
+/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_graph.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/amba/bus.h>
+#include <linux/coresight.h>
+#include <asm/smp_plat.h>
+
+
+static int of_dev_node_match(struct device *dev, void *data)
+{
+	return dev->of_node == data;
+}
+
+static struct device *
+of_coresight_get_endpoint_device(struct device_node *endpoint)
+{
+	struct device *dev = NULL;
+
+	/*
+	 * If we have a non-configuable replicator, it will be found on the
+	 * platform bus.
+	 */
+	dev = bus_find_device(&platform_bus_type, NULL,
+			      endpoint, of_dev_node_match);
+	if (dev)
+		return dev;
+
+	/*
+	 * We have a configurable component - circle through the AMBA bus
+	 * looking for the device that matches the endpoint node.
+	 */
+	return bus_find_device(&amba_bustype, NULL,
+			       endpoint, of_dev_node_match);
+}
+
+static struct device_node *of_get_coresight_endpoint(
+		const struct device_node *parent, struct device_node *prev)
+{
+	struct device_node *node = of_graph_get_next_endpoint(parent, prev);
+
+	of_node_put(prev);
+	return node;
+}
+
+static void of_coresight_get_ports(struct device_node *node,
+				   int *nr_inport, int *nr_outport)
+{
+	struct device_node *ep = NULL;
+	int in = 0, out = 0;
+
+	do {
+		ep = of_get_coresight_endpoint(node, ep);
+		if (!ep)
+			break;
+
+		if (of_property_read_bool(ep, "slave-mode"))
+			in++;
+		else
+			out++;
+
+	} while (ep);
+
+	*nr_inport = in;
+	*nr_outport = out;
+}
+
+static int of_coresight_alloc_memory(struct device *dev,
+			struct coresight_platform_data *pdata)
+{
+	/* List of output port on this component */
+	pdata->outports = devm_kzalloc(dev, pdata->nr_outport *
+				       sizeof(*pdata->outports),
+				       GFP_KERNEL);
+	if (!pdata->outports)
+		return -ENOMEM;
+
+	/* Children connected to this component via @outport */
+	 pdata->child_names = devm_kzalloc(dev, pdata->nr_outport *
+					  sizeof(*pdata->child_names),
+					  GFP_KERNEL);
+	if (!pdata->child_names)
+		return -ENOMEM;
+
+	/* Port number on the child this component is connected to */
+	pdata->child_ports = devm_kzalloc(dev, pdata->nr_outport *
+					  sizeof(*pdata->child_ports),
+					  GFP_KERNEL);
+	if (!pdata->child_ports)
+		return -ENOMEM;
+
+	return 0;
+}
+
+struct coresight_platform_data *of_get_coresight_platform_data(
+				struct device *dev, struct device_node *node)
+{
+	int i = 0, ret = 0;
+	struct coresight_platform_data *pdata;
+	struct of_endpoint endpoint, rendpoint;
+	struct device *rdev;
+	struct device_node *cpu;
+	struct device_node *ep = NULL;
+	struct device_node *rparent = NULL;
+	struct device_node *rport = NULL;
+
+	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return ERR_PTR(-ENOMEM);
+
+	/* Use device name as debugfs handle */
+	pdata->name = dev_name(dev);
+
+	/* Get the number of input and output port for this component */
+	of_coresight_get_ports(node, &pdata->nr_inport, &pdata->nr_outport);
+
+	if (pdata->nr_outport) {
+		ret = of_coresight_alloc_memory(dev, pdata);
+		if (ret)
+			return ERR_PTR(ret);
+
+		/* Iterate through each port to discover topology */
+		do {
+			/* Get a handle on a port */
+			ep = of_get_coresight_endpoint(node, ep);
+			if (!ep)
+				break;
+
+			/*
+			 * No need to deal with input ports, processing for as
+			 * processing for output ports will deal with them.
+			 */
+			if (of_find_property(ep, "slave-mode", NULL))
+				continue;
+
+			/* Get a handle on the local endpoint */
+			ret = of_graph_parse_endpoint(ep, &endpoint);
+
+			if (ret)
+				continue;
+
+			/* The local out port number */
+			pdata->outports[i] = endpoint.id;
+
+			/*
+			 * Get a handle on the remote port and parent
+			 * attached to it.
+			 */
+			rparent = of_graph_get_remote_port_parent(ep);
+			rport = of_graph_get_remote_port(ep);
+
+			if (!rparent || !rport)
+				continue;
+
+			if (of_graph_parse_endpoint(rport, &rendpoint))
+				continue;
+
+			rdev = of_coresight_get_endpoint_device(rparent);
+			if (!dev)
+				continue;
+
+			pdata->child_names[i] = dev_name(rdev);
+			pdata->child_ports[i] = rendpoint.id;
+
+			i++;
+		} while (ep);
+	}
+
+	/* Affinity defaults to CPU0 */
+	pdata->cpu = 0;
+	cpu = of_parse_phandle(node, "cpu", 0);
+	if (cpu) {
+		const u32 *mpidr;
+		int len, index;
+
+		mpidr = of_get_property(cpu, "reg", &len);
+		if (mpidr && len == 4) {
+			index = get_logical_index(be32_to_cpup(mpidr));
+			if (index != -EINVAL)
+				pdata->cpu = index;
+		}
+	}
+
+	return pdata;
+}
+EXPORT_SYMBOL_GPL(of_get_coresight_platform_data);
diff --git a/drivers/cpufreq/arm_big_little_dt.c b/drivers/cpufreq/arm_big_little_dt.c
index ef0b3f1324d5..36d91dba2965 100644
--- a/drivers/cpufreq/arm_big_little_dt.c
+++ b/drivers/cpufreq/arm_big_little_dt.c
@@ -106,7 +106,6 @@ static int generic_bL_remove(struct platform_device *pdev)
 static struct platform_driver generic_bL_platdrv = {
 	.driver = {
 		.name	= "arm-bL-cpufreq-dt",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= generic_bL_probe,
 	.remove		= generic_bL_remove,
diff --git a/drivers/cpufreq/cpufreq-dt.c b/drivers/cpufreq/cpufreq-dt.c
index 9bc2720628a4..fde97d6e31d6 100644
--- a/drivers/cpufreq/cpufreq-dt.c
+++ b/drivers/cpufreq/cpufreq-dt.c
@@ -197,7 +197,7 @@ static int cpufreq_init(struct cpufreq_policy *policy)
 
 	ret = allocate_resources(policy->cpu, &cpu_dev, &cpu_reg, &cpu_clk);
 	if (ret) {
-		pr_err("%s: Failed to allocate resources\n: %d", __func__, ret);
+		pr_err("%s: Failed to allocate resources: %d\n", __func__, ret);
 		return ret;
 	}
 
@@ -211,6 +211,17 @@ static int cpufreq_init(struct cpufreq_policy *policy)
 	/* OPPs might be populated at runtime, don't check for error here */
 	of_init_opp_table(cpu_dev);
 
+	/*
+	 * But we need OPP table to function so if it is not there let's
+	 * give platform code chance to provide it for us.
+	 */
+	ret = dev_pm_opp_get_opp_count(cpu_dev);
+	if (ret <= 0) {
+		pr_debug("OPP table is not ready, deferring probe\n");
+		ret = -EPROBE_DEFER;
+		goto out_free_opp;
+	}
+
 	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
 	if (!priv) {
 		ret = -ENOMEM;
@@ -400,7 +411,6 @@ static int dt_cpufreq_remove(struct platform_device *pdev)
 static struct platform_driver dt_cpufreq_platdrv = {
 	.driver = {
 		.name	= "cpufreq-dt",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= dt_cpufreq_probe,
 	.remove		= dt_cpufreq_remove,
diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index a09a29c312a9..46bed4f81cde 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -2028,6 +2028,12 @@ static int __cpufreq_governor(struct cpufreq_policy *policy,
 	/* Don't start any governor operations if we are entering suspend */
 	if (cpufreq_suspended)
 		return 0;
+	/*
+	 * Governor might not be initiated here if ACPI _PPC changed
+	 * notification happened, so check it.
+	 */
+	if (!policy->governor)
+		return -EINVAL;
 
 	if (policy->governor->max_transition_latency &&
 	    policy->cpuinfo.transition_latency >
diff --git a/drivers/cpufreq/davinci-cpufreq.c b/drivers/cpufreq/davinci-cpufreq.c
index 28a16dc6e02e..7e336d20c184 100644
--- a/drivers/cpufreq/davinci-cpufreq.c
+++ b/drivers/cpufreq/davinci-cpufreq.c
@@ -169,7 +169,6 @@ static int __exit davinci_cpufreq_remove(struct platform_device *pdev)
 static struct platform_driver davinci_cpufreq_driver = {
 	.driver = {
 		.name	 = "cpufreq-davinci",
-		.owner	 = THIS_MODULE,
 	},
 	.remove = __exit_p(davinci_cpufreq_remove),
 };
diff --git a/drivers/cpufreq/dbx500-cpufreq.c b/drivers/cpufreq/dbx500-cpufreq.c
index 4bebc1b5db48..5c3ec1dd4921 100644
--- a/drivers/cpufreq/dbx500-cpufreq.c
+++ b/drivers/cpufreq/dbx500-cpufreq.c
@@ -69,7 +69,6 @@ static int dbx500_cpufreq_probe(struct platform_device *pdev)
 static struct platform_driver dbx500_cpufreq_plat_driver = {
 	.driver = {
 		.name = "cpufreq-ux500",
-		.owner = THIS_MODULE,
 	},
 	.probe = dbx500_cpufreq_probe,
 };
diff --git a/drivers/cpufreq/exynos-cpufreq.c b/drivers/cpufreq/exynos-cpufreq.c
index 1e0ec57bf6e3..f99a0b0b7c06 100644
--- a/drivers/cpufreq/exynos-cpufreq.c
+++ b/drivers/cpufreq/exynos-cpufreq.c
@@ -211,7 +211,6 @@ err_vdd_arm:
 static struct platform_driver exynos_cpufreq_platdrv = {
 	.driver = {
 		.name	= "exynos-cpufreq",
-		.owner	= THIS_MODULE,
 	},
 	.probe = exynos_cpufreq_probe,
 };
diff --git a/drivers/cpufreq/exynos5440-cpufreq.c b/drivers/cpufreq/exynos5440-cpufreq.c
index 27a57ed9eb2c..21a90ed7f3d8 100644
--- a/drivers/cpufreq/exynos5440-cpufreq.c
+++ b/drivers/cpufreq/exynos5440-cpufreq.c
@@ -442,7 +442,6 @@ static int exynos_cpufreq_remove(struct platform_device *pdev)
 static struct platform_driver exynos_cpufreq_platdrv = {
 	.driver = {
 		.name	= "exynos5440-cpufreq",
-		.owner	= THIS_MODULE,
 		.of_match_table = exynos_cpufreq_match,
 	},
 	.probe		= exynos_cpufreq_probe,
diff --git a/drivers/cpufreq/imx6q-cpufreq.c b/drivers/cpufreq/imx6q-cpufreq.c
index 5da1d131f770..380a90d3c57e 100644
--- a/drivers/cpufreq/imx6q-cpufreq.c
+++ b/drivers/cpufreq/imx6q-cpufreq.c
@@ -357,7 +357,6 @@ static int imx6q_cpufreq_remove(struct platform_device *pdev)
 static struct platform_driver imx6q_cpufreq_platdrv = {
 	.driver = {
 		.name	= "imx6q-cpufreq",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= imx6q_cpufreq_probe,
 	.remove		= imx6q_cpufreq_remove,
diff --git a/drivers/cpufreq/integrator-cpufreq.c b/drivers/cpufreq/integrator-cpufreq.c
index 6bd69adc3c5e..129e266f7621 100644
--- a/drivers/cpufreq/integrator-cpufreq.c
+++ b/drivers/cpufreq/integrator-cpufreq.c
@@ -226,7 +226,6 @@ static const struct of_device_id integrator_cpufreq_match[] = {
 static struct platform_driver integrator_cpufreq_driver = {
 	.driver = {
 		.name = "integrator-cpufreq",
-		.owner = THIS_MODULE,
 		.of_match_table = integrator_cpufreq_match,
 	},
 	.remove = __exit_p(integrator_cpufreq_remove),
diff --git a/drivers/cpufreq/intel_pstate.c b/drivers/cpufreq/intel_pstate.c
index 1405b393c93d..742eefba12c2 100644
--- a/drivers/cpufreq/intel_pstate.c
+++ b/drivers/cpufreq/intel_pstate.c
@@ -199,7 +199,14 @@ static signed int pid_calc(struct _pid *pid, int32_t busy)
 
 	pid->integral += fp_error;
 
-	/* limit the integral term */
+	/*
+	 * We limit the integral here so that it will never
+	 * get higher than 30.  This prevents it from becoming
+	 * too large an input over long periods of time and allows
+	 * it to get factored out sooner.
+	 *
+	 * The value of 30 was chosen through experimentation.
+	 */
 	integral_limit = int_tofp(30);
 	if (pid->integral > integral_limit)
 		pid->integral = integral_limit;
@@ -616,6 +623,11 @@ static void intel_pstate_get_min_max(struct cpudata *cpu, int *min, int *max)
 	if (limits.no_turbo || limits.turbo_disabled)
 		max_perf = cpu->pstate.max_pstate;
 
+	/*
+	 * performance can be limited by user through sysfs, by cpufreq
+	 * policy, or by cpu specific default values determined through
+	 * experimentation.
+	 */
 	max_perf_adj = fp_toint(mul_fp(int_tofp(max_perf), limits.max_perf));
 	*max = clamp_t(int, max_perf_adj,
 			cpu->pstate.min_pstate, cpu->pstate.turbo_pstate);
@@ -717,11 +729,29 @@ static inline int32_t intel_pstate_get_scaled_busy(struct cpudata *cpu)
 	u32 duration_us;
 	u32 sample_time;
 
+	/*
+	 * core_busy is the ratio of actual performance to max
+	 * max_pstate is the max non turbo pstate available
+	 * current_pstate was the pstate that was requested during
+	 * 	the last sample period.
+	 *
+	 * We normalize core_busy, which was our actual percent
+	 * performance to what we requested during the last sample
+	 * period. The result will be a percentage of busy at a
+	 * specified pstate.
+	 */
 	core_busy = cpu->sample.core_pct_busy;
 	max_pstate = int_tofp(cpu->pstate.max_pstate);
 	current_pstate = int_tofp(cpu->pstate.current_pstate);
 	core_busy = mul_fp(core_busy, div_fp(max_pstate, current_pstate));
 
+	/*
+	 * Since we have a deferred timer, it will not fire unless
+	 * we are in C0.  So, determine if the actual elapsed time
+	 * is significantly greater (3x) than our sample interval.  If it
+	 * is, then we were idle for a long enough period of time
+	 * to adjust our busyness.
+	 */
 	sample_time = pid_params.sample_rate_ms  * USEC_PER_MSEC;
 	duration_us = (u32) ktime_us_delta(cpu->sample.time,
 					   cpu->last_sample_time);
@@ -948,6 +978,7 @@ static struct cpufreq_driver intel_pstate_driver = {
 
 static int __initdata no_load;
 static int __initdata no_hwp;
+static unsigned int force_load;
 
 static int intel_pstate_msrs_not_valid(void)
 {
@@ -1094,7 +1125,8 @@ static bool intel_pstate_platform_pwr_mgmt_exists(void)
 			case PSS:
 				return intel_pstate_no_acpi_pss();
 			case PPC:
-				return intel_pstate_has_acpi_ppc();
+				return intel_pstate_has_acpi_ppc() &&
+					(!force_load);
 			}
 	}
 
@@ -1175,6 +1207,8 @@ static int __init intel_pstate_setup(char *str)
 		no_load = 1;
 	if (!strcmp(str, "no_hwp"))
 		no_hwp = 1;
+	if (!strcmp(str, "force"))
+		force_load = 1;
 	return 0;
 }
 early_param("intel_pstate", intel_pstate_setup);
diff --git a/drivers/cpufreq/kirkwood-cpufreq.c b/drivers/cpufreq/kirkwood-cpufreq.c
index 7906d4acfe40..be42f103db60 100644
--- a/drivers/cpufreq/kirkwood-cpufreq.c
+++ b/drivers/cpufreq/kirkwood-cpufreq.c
@@ -183,7 +183,6 @@ static struct platform_driver kirkwood_cpufreq_platform_driver = {
 	.remove = kirkwood_cpufreq_remove,
 	.driver = {
 		.name = "kirkwood-cpufreq",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/cpufreq/longhaul.c b/drivers/cpufreq/longhaul.c
index c913906a719e..0f6b229afcb9 100644
--- a/drivers/cpufreq/longhaul.c
+++ b/drivers/cpufreq/longhaul.c
@@ -1,5 +1,5 @@
 /*
- *  (C) 2001-2004  Dave Jones. <davej@redhat.com>
+ *  (C) 2001-2004  Dave Jones.
  *  (C) 2002  Padraig Brady. <padraig@antefacto.com>
  *
  *  Licensed under the terms of the GNU GPL License version 2.
@@ -1008,7 +1008,7 @@ MODULE_PARM_DESC(revid_errata, "Ignore CPU Revision ID");
 module_param(enable, int, 0644);
 MODULE_PARM_DESC(enable, "Enable driver");
 
-MODULE_AUTHOR("Dave Jones <davej@redhat.com>");
+MODULE_AUTHOR("Dave Jones");
 MODULE_DESCRIPTION("Longhaul driver for VIA Cyrix processors.");
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/cpufreq/loongson2_cpufreq.c b/drivers/cpufreq/loongson2_cpufreq.c
index 9fa177206032..fc897babab55 100644
--- a/drivers/cpufreq/loongson2_cpufreq.c
+++ b/drivers/cpufreq/loongson2_cpufreq.c
@@ -130,7 +130,6 @@ MODULE_DEVICE_TABLE(platform, platform_device_ids);
 static struct platform_driver platform_driver = {
 	.driver = {
 		.name = "loongson2_cpufreq",
-		.owner = THIS_MODULE,
 	},
 	.id_table = platform_device_ids,
 };
diff --git a/drivers/cpufreq/omap-cpufreq.c b/drivers/cpufreq/omap-cpufreq.c
index 5f69c9aa703c..e3866e0d5bf8 100644
--- a/drivers/cpufreq/omap-cpufreq.c
+++ b/drivers/cpufreq/omap-cpufreq.c
@@ -195,7 +195,6 @@ static int omap_cpufreq_remove(struct platform_device *pdev)
 static struct platform_driver omap_cpufreq_platdrv = {
 	.driver = {
 		.name	= "omap-cpufreq",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= omap_cpufreq_probe,
 	.remove		= omap_cpufreq_remove,
diff --git a/drivers/cpufreq/powernow-k6.c b/drivers/cpufreq/powernow-k6.c
index f91027259c3c..e6f24b281e3e 100644
--- a/drivers/cpufreq/powernow-k6.c
+++ b/drivers/cpufreq/powernow-k6.c
@@ -300,7 +300,7 @@ static void __exit powernow_k6_exit(void)
 }
 
 
-MODULE_AUTHOR("Arjan van de Ven, Dave Jones <davej@redhat.com>, "
+MODULE_AUTHOR("Arjan van de Ven, Dave Jones, "
 		"Dominik Brodowski <linux@brodo.de>");
 MODULE_DESCRIPTION("PowerNow! driver for AMD K6-2+ / K6-3+ processors.");
 MODULE_LICENSE("GPL");
diff --git a/drivers/cpufreq/powernow-k7.c b/drivers/cpufreq/powernow-k7.c
index e61e224475ad..37c5742482d8 100644
--- a/drivers/cpufreq/powernow-k7.c
+++ b/drivers/cpufreq/powernow-k7.c
@@ -1,7 +1,6 @@
 /*
  *  AMD K7 Powernow driver.
  *  (C) 2003 Dave Jones on behalf of SuSE Labs.
- *  (C) 2003-2004 Dave Jones <davej@redhat.com>
  *
  *  Licensed under the terms of the GNU GPL License version 2.
  *  Based upon datasheets & sample CPUs kindly provided by AMD.
@@ -701,7 +700,7 @@ static void __exit powernow_exit(void)
 module_param(acpi_force,  int, 0444);
 MODULE_PARM_DESC(acpi_force, "Force ACPI to be used.");
 
-MODULE_AUTHOR("Dave Jones <davej@redhat.com>");
+MODULE_AUTHOR("Dave Jones");
 MODULE_DESCRIPTION("Powernow driver for AMD K7 processors.");
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/cpufreq/s5pv210-cpufreq.c b/drivers/cpufreq/s5pv210-cpufreq.c
index 567caa6313ff..b0dac7d6ba31 100644
--- a/drivers/cpufreq/s5pv210-cpufreq.c
+++ b/drivers/cpufreq/s5pv210-cpufreq.c
@@ -656,7 +656,6 @@ static int s5pv210_cpufreq_probe(struct platform_device *pdev)
 static struct platform_driver s5pv210_cpufreq_platdrv = {
 	.driver = {
 		.name	= "s5pv210-cpufreq",
-		.owner	= THIS_MODULE,
 	},
 	.probe = s5pv210_cpufreq_probe,
 };
diff --git a/drivers/cpufreq/spear-cpufreq.c b/drivers/cpufreq/spear-cpufreq.c
index 38678396636d..4894924a3ca2 100644
--- a/drivers/cpufreq/spear-cpufreq.c
+++ b/drivers/cpufreq/spear-cpufreq.c
@@ -236,7 +236,6 @@ out_put_node:
 static struct platform_driver spear_cpufreq_platdrv = {
 	.driver = {
 		.name	= "spear-cpufreq",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= spear_cpufreq_probe,
 };
diff --git a/drivers/cpufreq/speedstep-ich.c b/drivers/cpufreq/speedstep-ich.c
index 1a07b5904ed5..e56d632a8b21 100644
--- a/drivers/cpufreq/speedstep-ich.c
+++ b/drivers/cpufreq/speedstep-ich.c
@@ -378,8 +378,7 @@ static void __exit speedstep_exit(void)
 }
 
 
-MODULE_AUTHOR("Dave Jones <davej@redhat.com>, "
-		"Dominik Brodowski <linux@brodo.de>");
+MODULE_AUTHOR("Dave Jones, Dominik Brodowski <linux@brodo.de>");
 MODULE_DESCRIPTION("Speedstep driver for Intel mobile processors on chipsets "
 		"with ICH-M southbridges.");
 MODULE_LICENSE("GPL");
diff --git a/drivers/cpufreq/vexpress-spc-cpufreq.c b/drivers/cpufreq/vexpress-spc-cpufreq.c
index 7f7c9c01b44e..433e93fd4900 100644
--- a/drivers/cpufreq/vexpress-spc-cpufreq.c
+++ b/drivers/cpufreq/vexpress-spc-cpufreq.c
@@ -60,7 +60,6 @@ static int ve_spc_cpufreq_remove(struct platform_device *pdev)
 static struct platform_driver ve_spc_cpufreq_platdrv = {
 	.driver = {
 		.name	= "vexpress-spc-cpufreq",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= ve_spc_cpufreq_probe,
 	.remove		= ve_spc_cpufreq_remove,
diff --git a/drivers/cpuidle/cpuidle-at91.c b/drivers/cpuidle/cpuidle-at91.c
index 1964ff07117c..aae7bfc1ea36 100644
--- a/drivers/cpuidle/cpuidle-at91.c
+++ b/drivers/cpuidle/cpuidle-at91.c
@@ -60,7 +60,6 @@ static int at91_cpuidle_probe(struct platform_device *dev)
 static struct platform_driver at91_cpuidle_driver = {
 	.driver = {
 		.name = "cpuidle-at91",
-		.owner = THIS_MODULE,
 	},
 	.probe = at91_cpuidle_probe,
 };
diff --git a/drivers/cpuidle/cpuidle-calxeda.c b/drivers/cpuidle/cpuidle-calxeda.c
index 6541b0bfdfaa..9445e6cc02be 100644
--- a/drivers/cpuidle/cpuidle-calxeda.c
+++ b/drivers/cpuidle/cpuidle-calxeda.c
@@ -72,7 +72,6 @@ static int calxeda_cpuidle_probe(struct platform_device *pdev)
 static struct platform_driver calxeda_cpuidle_plat_driver = {
         .driver = {
                 .name = "cpuidle-calxeda",
-                .owner = THIS_MODULE,
         },
         .probe = calxeda_cpuidle_probe,
 };
diff --git a/drivers/cpuidle/cpuidle-clps711x.c b/drivers/cpuidle/cpuidle-clps711x.c
index 5243811daa6e..18a7f7380508 100644
--- a/drivers/cpuidle/cpuidle-clps711x.c
+++ b/drivers/cpuidle/cpuidle-clps711x.c
@@ -54,7 +54,6 @@ static int __init clps711x_cpuidle_probe(struct platform_device *pdev)
 static struct platform_driver clps711x_cpuidle_driver = {
 	.driver	= {
 		.name	= CLPS711X_CPUIDLE_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver_probe(clps711x_cpuidle_driver, clps711x_cpuidle_probe);
diff --git a/drivers/cpuidle/cpuidle-exynos.c b/drivers/cpuidle/cpuidle-exynos.c
index 64d12a855ec6..4003a3160865 100644
--- a/drivers/cpuidle/cpuidle-exynos.c
+++ b/drivers/cpuidle/cpuidle-exynos.c
@@ -74,7 +74,6 @@ static struct platform_driver exynos_cpuidle_driver = {
 	.probe	= exynos_cpuidle_probe,
 	.driver = {
 		.name = "exynos_cpuidle",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/cpuidle/cpuidle-kirkwood.c b/drivers/cpuidle/cpuidle-kirkwood.c
index d88f8d7c2143..cea0a6c4b1db 100644
--- a/drivers/cpuidle/cpuidle-kirkwood.c
+++ b/drivers/cpuidle/cpuidle-kirkwood.c
@@ -77,7 +77,6 @@ static struct platform_driver kirkwood_cpuidle_driver = {
 	.remove = kirkwood_cpuidle_remove,
 	.driver = {
 		   .name = "kirkwood_cpuidle",
-		   .owner = THIS_MODULE,
 		   },
 };
 
diff --git a/drivers/cpuidle/cpuidle-mvebu-v7.c b/drivers/cpuidle/cpuidle-mvebu-v7.c
index dd4c176df2a3..38e68618513a 100644
--- a/drivers/cpuidle/cpuidle-mvebu-v7.c
+++ b/drivers/cpuidle/cpuidle-mvebu-v7.c
@@ -114,7 +114,6 @@ static int mvebu_v7_cpuidle_probe(struct platform_device *pdev)
 static struct platform_driver armadaxp_cpuidle_plat_driver = {
 	.driver = {
 		.name = "cpuidle-armada-xp",
-		.owner = THIS_MODULE,
 	},
 	.probe = mvebu_v7_cpuidle_probe,
 };
@@ -124,7 +123,6 @@ module_platform_driver(armadaxp_cpuidle_plat_driver);
 static struct platform_driver armada370_cpuidle_plat_driver = {
 	.driver = {
 		.name = "cpuidle-armada-370",
-		.owner = THIS_MODULE,
 	},
 	.probe = mvebu_v7_cpuidle_probe,
 };
@@ -134,7 +132,6 @@ module_platform_driver(armada370_cpuidle_plat_driver);
 static struct platform_driver armada38x_cpuidle_plat_driver = {
 	.driver = {
 		.name = "cpuidle-armada-38x",
-		.owner = THIS_MODULE,
 	},
 	.probe = mvebu_v7_cpuidle_probe,
 };
diff --git a/drivers/cpuidle/cpuidle-powernv.c b/drivers/cpuidle/cpuidle-powernv.c
index e9248bb9173a..aedec0957934 100644
--- a/drivers/cpuidle/cpuidle-powernv.c
+++ b/drivers/cpuidle/cpuidle-powernv.c
@@ -16,13 +16,10 @@
 
 #include <asm/machdep.h>
 #include <asm/firmware.h>
+#include <asm/opal.h>
 #include <asm/runlatch.h>
 
-/* Flags and constants used in PowerNV platform */
-
 #define MAX_POWERNV_IDLE_STATES	8
-#define IDLE_USE_INST_NAP	0x00010000 /* Use nap instruction */
-#define IDLE_USE_INST_SLEEP	0x00020000 /* Use sleep instruction */
 
 struct cpuidle_driver powernv_idle_driver = {
 	.name             = "powernv_idle",
@@ -197,7 +194,7 @@ static int powernv_add_idle_states(void)
 		 * target residency to be 10x exit_latency
 		 */
 		latency_ns = be32_to_cpu(idle_state_latency[i]);
-		if (flags & IDLE_USE_INST_NAP) {
+		if (flags & OPAL_PM_NAP_ENABLED) {
 			/* Add NAP state */
 			strcpy(powernv_states[nr_idle_states].name, "Nap");
 			strcpy(powernv_states[nr_idle_states].desc, "Nap");
@@ -210,7 +207,8 @@ static int powernv_add_idle_states(void)
 			nr_idle_states++;
 		}
 
-		if (flags & IDLE_USE_INST_SLEEP) {
+		if (flags & OPAL_PM_SLEEP_ENABLED ||
+			flags & OPAL_PM_SLEEP_ENABLED_ER1) {
 			/* Add FASTSLEEP state */
 			strcpy(powernv_states[nr_idle_states].name, "FastSleep");
 			strcpy(powernv_states[nr_idle_states].desc, "FastSleep");
diff --git a/drivers/cpuidle/cpuidle-ux500.c b/drivers/cpuidle/cpuidle-ux500.c
index 292e65a90308..66f81e410f0d 100644
--- a/drivers/cpuidle/cpuidle-ux500.c
+++ b/drivers/cpuidle/cpuidle-ux500.c
@@ -122,7 +122,6 @@ static int dbx500_cpuidle_probe(struct platform_device *pdev)
 static struct platform_driver dbx500_cpuidle_plat_driver = {
 	.driver = {
 		.name = "cpuidle-dbx500",
-		.owner = THIS_MODULE,
 	},
 	.probe = dbx500_cpuidle_probe,
 };
diff --git a/drivers/cpuidle/cpuidle-zynq.c b/drivers/cpuidle/cpuidle-zynq.c
index 022dec86de8e..002b8c9f98f5 100644
--- a/drivers/cpuidle/cpuidle-zynq.c
+++ b/drivers/cpuidle/cpuidle-zynq.c
@@ -71,7 +71,6 @@ static int zynq_cpuidle_probe(struct platform_device *pdev)
 static struct platform_driver zynq_cpuidle_driver = {
 	.driver = {
 		.name = "cpuidle-zynq",
-		.owner = THIS_MODULE,
 	},
 	.probe = zynq_cpuidle_probe,
 };
diff --git a/drivers/cpuidle/governors/ladder.c b/drivers/cpuidle/governors/ladder.c
index 37263d9a1051..401c0106ed34 100644
--- a/drivers/cpuidle/governors/ladder.c
+++ b/drivers/cpuidle/governors/ladder.c
@@ -79,12 +79,7 @@ static int ladder_select_state(struct cpuidle_driver *drv,
 
 	last_state = &ldev->states[last_idx];
 
-	if (!(drv->states[last_idx].flags & CPUIDLE_FLAG_TIME_INVALID)) {
-		last_residency = cpuidle_get_last_residency(dev) - \
-					 drv->states[last_idx].exit_latency;
-	}
-	else
-		last_residency = last_state->threshold.promotion_time + 1;
+	last_residency = cpuidle_get_last_residency(dev) - drv->states[last_idx].exit_latency;
 
 	/* consider promotion */
 	if (last_idx < drv->state_count - 1 &&
diff --git a/drivers/cpuidle/governors/menu.c b/drivers/cpuidle/governors/menu.c
index 659d7b0c9ebf..40580794e23d 100644
--- a/drivers/cpuidle/governors/menu.c
+++ b/drivers/cpuidle/governors/menu.c
@@ -396,8 +396,8 @@ static void menu_update(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 	 * power state and occurrence of the wakeup event.
 	 *
 	 * If the entered idle state didn't support residency measurements,
-	 * we are basically lost in the dark how much time passed.
-	 * As a compromise, assume we slept for the whole expected time.
+	 * we use them anyway if they are short, and if long,
+	 * truncate to the whole expected time.
 	 *
 	 * Any measured amount of time will include the exit latency.
 	 * Since we are interested in when the wakeup begun, not when it
@@ -405,22 +405,17 @@ static void menu_update(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 	 * the measured amount of time is less than the exit latency,
 	 * assume the state was never reached and the exit latency is 0.
 	 */
-	if (unlikely(target->flags & CPUIDLE_FLAG_TIME_INVALID)) {
-		/* Use timer value as is */
-		measured_us = data->next_timer_us;
 
-	} else {
-		/* Use measured value */
-		measured_us = cpuidle_get_last_residency(dev);
+	/* measured value */
+	measured_us = cpuidle_get_last_residency(dev);
 
-		/* Deduct exit latency */
-		if (measured_us > target->exit_latency)
-			measured_us -= target->exit_latency;
+	/* Deduct exit latency */
+	if (measured_us > target->exit_latency)
+		measured_us -= target->exit_latency;
 
-		/* Make sure our coefficients do not exceed unity */
-		if (measured_us > data->next_timer_us)
-			measured_us = data->next_timer_us;
-	}
+	/* Make sure our coefficients do not exceed unity */
+	if (measured_us > data->next_timer_us)
+		measured_us = data->next_timer_us;
 
 	/* Update our correction ratio */
 	new_factor = data->correction_factor[data->bucket];
diff --git a/drivers/crypto/amcc/crypto4xx_core.c b/drivers/crypto/amcc/crypto4xx_core.c
index e4c6c58fbb03..d02b77150070 100644
--- a/drivers/crypto/amcc/crypto4xx_core.c
+++ b/drivers/crypto/amcc/crypto4xx_core.c
@@ -1288,7 +1288,6 @@ static const struct of_device_id crypto4xx_match[] = {
 static struct platform_driver crypto4xx_driver = {
 	.driver = {
 		.name = "crypto4xx",
-		.owner = THIS_MODULE,
 		.of_match_table = crypto4xx_match,
 	},
 	.probe		= crypto4xx_probe,
diff --git a/drivers/crypto/atmel-aes.c b/drivers/crypto/atmel-aes.c
index a083474991ab..53d1c330f8a8 100644
--- a/drivers/crypto/atmel-aes.c
+++ b/drivers/crypto/atmel-aes.c
@@ -1473,7 +1473,6 @@ static struct platform_driver atmel_aes_driver = {
 	.remove		= atmel_aes_remove,
 	.driver		= {
 		.name	= "atmel_aes",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(atmel_aes_dt_ids),
 	},
 };
diff --git a/drivers/crypto/atmel-sha.c b/drivers/crypto/atmel-sha.c
index 9a4f69eaa5e0..d94f07c78e19 100644
--- a/drivers/crypto/atmel-sha.c
+++ b/drivers/crypto/atmel-sha.c
@@ -1529,7 +1529,6 @@ static struct platform_driver atmel_sha_driver = {
 	.remove		= atmel_sha_remove,
 	.driver		= {
 		.name	= "atmel_sha",
-		.owner	= THIS_MODULE,
 		.of_match_table	= of_match_ptr(atmel_sha_dt_ids),
 	},
 };
diff --git a/drivers/crypto/atmel-tdes.c b/drivers/crypto/atmel-tdes.c
index d3a9041938ea..5e7c896cde30 100644
--- a/drivers/crypto/atmel-tdes.c
+++ b/drivers/crypto/atmel-tdes.c
@@ -1524,7 +1524,6 @@ static struct platform_driver atmel_tdes_driver = {
 	.remove		= atmel_tdes_remove,
 	.driver		= {
 		.name	= "atmel_tdes",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(atmel_tdes_dt_ids),
 	},
 };
diff --git a/drivers/crypto/bfin_crc.c b/drivers/crypto/bfin_crc.c
index b099e33cb073..9ae149bddb6e 100644
--- a/drivers/crypto/bfin_crc.c
+++ b/drivers/crypto/bfin_crc.c
@@ -21,13 +21,13 @@
 #include <linux/scatterlist.h>
 #include <linux/dma-mapping.h>
 #include <linux/delay.h>
-#include <linux/unaligned/access_ok.h>
 #include <linux/crypto.h>
 #include <linux/cryptohash.h>
 #include <crypto/scatterwalk.h>
 #include <crypto/algapi.h>
 #include <crypto/hash.h>
 #include <crypto/internal/hash.h>
+#include <asm/unaligned.h>
 
 #include <asm/dma.h>
 #include <asm/portmux.h>
@@ -724,7 +724,6 @@ static struct platform_driver bfin_crypto_crc_driver = {
 	.resume    = bfin_crypto_crc_resume,
 	.driver    = {
 		.name  = DRIVER_NAME,
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/crypto/caam/caamalg.c b/drivers/crypto/caam/caamalg.c
index a80ea853701d..3187400daf31 100644
--- a/drivers/crypto/caam/caamalg.c
+++ b/drivers/crypto/caam/caamalg.c
@@ -60,6 +60,7 @@
 #define CAAM_CRA_PRIORITY		3000
 /* max key is sum of AES_MAX_KEY_SIZE, max split key size */
 #define CAAM_MAX_KEY_SIZE		(AES_MAX_KEY_SIZE + \
+					 CTR_RFC3686_NONCE_SIZE + \
 					 SHA512_DIGEST_SIZE * 2)
 /* max IV is max of AES_BLOCK_SIZE, DES3_EDE_BLOCK_SIZE */
 #define CAAM_MAX_IV_LENGTH		16
@@ -70,17 +71,34 @@
 #define DESC_AEAD_DEC_LEN		(DESC_AEAD_BASE + 18 * CAAM_CMD_SZ)
 #define DESC_AEAD_GIVENC_LEN		(DESC_AEAD_ENC_LEN + 7 * CAAM_CMD_SZ)
 
+/* Note: Nonce is counted in enckeylen */
+#define DESC_AEAD_CTR_RFC3686_LEN	(6 * CAAM_CMD_SZ)
+
 #define DESC_AEAD_NULL_BASE		(3 * CAAM_CMD_SZ)
 #define DESC_AEAD_NULL_ENC_LEN		(DESC_AEAD_NULL_BASE + 14 * CAAM_CMD_SZ)
 #define DESC_AEAD_NULL_DEC_LEN		(DESC_AEAD_NULL_BASE + 17 * CAAM_CMD_SZ)
 
+#define DESC_GCM_BASE			(3 * CAAM_CMD_SZ)
+#define DESC_GCM_ENC_LEN		(DESC_GCM_BASE + 23 * CAAM_CMD_SZ)
+#define DESC_GCM_DEC_LEN		(DESC_GCM_BASE + 19 * CAAM_CMD_SZ)
+
+#define DESC_RFC4106_BASE		(3 * CAAM_CMD_SZ)
+#define DESC_RFC4106_ENC_LEN		(DESC_RFC4106_BASE + 15 * CAAM_CMD_SZ)
+#define DESC_RFC4106_DEC_LEN		(DESC_RFC4106_BASE + 14 * CAAM_CMD_SZ)
+#define DESC_RFC4106_GIVENC_LEN		(DESC_RFC4106_BASE + 21 * CAAM_CMD_SZ)
+
+#define DESC_RFC4543_BASE		(3 * CAAM_CMD_SZ)
+#define DESC_RFC4543_ENC_LEN		(DESC_RFC4543_BASE + 25 * CAAM_CMD_SZ)
+#define DESC_RFC4543_DEC_LEN		(DESC_RFC4543_BASE + 27 * CAAM_CMD_SZ)
+#define DESC_RFC4543_GIVENC_LEN		(DESC_RFC4543_BASE + 30 * CAAM_CMD_SZ)
+
 #define DESC_ABLKCIPHER_BASE		(3 * CAAM_CMD_SZ)
 #define DESC_ABLKCIPHER_ENC_LEN		(DESC_ABLKCIPHER_BASE + \
 					 20 * CAAM_CMD_SZ)
 #define DESC_ABLKCIPHER_DEC_LEN		(DESC_ABLKCIPHER_BASE + \
 					 15 * CAAM_CMD_SZ)
 
-#define DESC_MAX_USED_BYTES		(DESC_AEAD_GIVENC_LEN + \
+#define DESC_MAX_USED_BYTES		(DESC_RFC4543_GIVENC_LEN + \
 					 CAAM_MAX_KEY_SIZE)
 #define DESC_MAX_USED_LEN		(DESC_MAX_USED_BYTES / CAAM_CMD_SZ)
 
@@ -128,11 +146,13 @@ static inline void aead_append_src_dst(u32 *desc, u32 msg_type)
 /*
  * For aead encrypt and decrypt, read iv for both classes
  */
-static inline void aead_append_ld_iv(u32 *desc, int ivsize)
+static inline void aead_append_ld_iv(u32 *desc, int ivsize, int ivoffset)
 {
-	append_cmd(desc, CMD_SEQ_LOAD | LDST_SRCDST_BYTE_CONTEXT |
-		   LDST_CLASS_1_CCB | ivsize);
-	append_move(desc, MOVE_SRC_CLASS1CTX | MOVE_DEST_CLASS2INFIFO | ivsize);
+	append_seq_load(desc, ivsize, LDST_CLASS_1_CCB |
+			LDST_SRCDST_BYTE_CONTEXT |
+			(ivoffset << LDST_OFFSET_SHIFT));
+	append_move(desc, MOVE_SRC_CLASS1CTX | MOVE_DEST_CLASS2INFIFO |
+		    (ivoffset << MOVE_OFFSET_SHIFT) | ivsize);
 }
 
 /*
@@ -178,35 +198,60 @@ struct caam_ctx {
 };
 
 static void append_key_aead(u32 *desc, struct caam_ctx *ctx,
-			    int keys_fit_inline)
+			    int keys_fit_inline, bool is_rfc3686)
 {
+	u32 *nonce;
+	unsigned int enckeylen = ctx->enckeylen;
+
+	/*
+	 * RFC3686 specific:
+	 *	| ctx->key = {AUTH_KEY, ENC_KEY, NONCE}
+	 *	| enckeylen = encryption key size + nonce size
+	 */
+	if (is_rfc3686)
+		enckeylen -= CTR_RFC3686_NONCE_SIZE;
+
 	if (keys_fit_inline) {
 		append_key_as_imm(desc, ctx->key, ctx->split_key_pad_len,
 				  ctx->split_key_len, CLASS_2 |
 				  KEY_DEST_MDHA_SPLIT | KEY_ENC);
 		append_key_as_imm(desc, (void *)ctx->key +
-				  ctx->split_key_pad_len, ctx->enckeylen,
-				  ctx->enckeylen, CLASS_1 | KEY_DEST_CLASS_REG);
+				  ctx->split_key_pad_len, enckeylen,
+				  enckeylen, CLASS_1 | KEY_DEST_CLASS_REG);
 	} else {
 		append_key(desc, ctx->key_dma, ctx->split_key_len, CLASS_2 |
 			   KEY_DEST_MDHA_SPLIT | KEY_ENC);
 		append_key(desc, ctx->key_dma + ctx->split_key_pad_len,
-			   ctx->enckeylen, CLASS_1 | KEY_DEST_CLASS_REG);
+			   enckeylen, CLASS_1 | KEY_DEST_CLASS_REG);
+	}
+
+	/* Load Counter into CONTEXT1 reg */
+	if (is_rfc3686) {
+		nonce = (u32 *)((void *)ctx->key + ctx->split_key_pad_len +
+			       enckeylen);
+		append_load_imm_u32(desc, *nonce, LDST_CLASS_IND_CCB |
+				    LDST_SRCDST_BYTE_OUTFIFO | LDST_IMM);
+		append_move(desc,
+			    MOVE_SRC_OUTFIFO |
+			    MOVE_DEST_CLASS1CTX |
+			    (16 << MOVE_OFFSET_SHIFT) |
+			    (CTR_RFC3686_NONCE_SIZE << MOVE_LEN_SHIFT));
 	}
 }
 
 static void init_sh_desc_key_aead(u32 *desc, struct caam_ctx *ctx,
-				  int keys_fit_inline)
+				  int keys_fit_inline, bool is_rfc3686)
 {
 	u32 *key_jump_cmd;
 
-	init_sh_desc(desc, HDR_SHARE_SERIAL);
+	/* Note: Context registers are saved. */
+	init_sh_desc(desc, HDR_SHARE_SERIAL | HDR_SAVECTX);
 
 	/* Skip if already shared */
 	key_jump_cmd = append_jump(desc, JUMP_JSL | JUMP_TEST_ALL |
 				   JUMP_COND_SHRD);
 
-	append_key_aead(desc, ctx, keys_fit_inline);
+	append_key_aead(desc, ctx, keys_fit_inline, is_rfc3686);
 
 	set_jump_tgt_here(desc, key_jump_cmd);
 }
@@ -406,10 +451,17 @@ static int aead_set_sh_desc(struct crypto_aead *aead)
 {
 	struct aead_tfm *tfm = &aead->base.crt_aead;
 	struct caam_ctx *ctx = crypto_aead_ctx(aead);
+	struct crypto_tfm *ctfm = crypto_aead_tfm(aead);
+	const char *alg_name = crypto_tfm_alg_name(ctfm);
 	struct device *jrdev = ctx->jrdev;
-	bool keys_fit_inline = false;
+	bool keys_fit_inline;
 	u32 geniv, moveiv;
+	u32 ctx1_iv_off = 0;
 	u32 *desc;
+	const bool ctr_mode = ((ctx->class1_alg_type & OP_ALG_AAI_MASK) ==
+			       OP_ALG_AAI_CTR_MOD128);
+	const bool is_rfc3686 = (ctr_mode &&
+				 (strstr(alg_name, "rfc3686") != NULL));
 
 	if (!ctx->authsize)
 		return 0;
@@ -419,18 +471,36 @@ static int aead_set_sh_desc(struct crypto_aead *aead)
 		return aead_null_set_sh_desc(aead);
 
 	/*
+	 * AES-CTR needs to load IV in CONTEXT1 reg
+	 * at an offset of 128bits (16bytes)
+	 * CONTEXT1[255:128] = IV
+	 */
+	if (ctr_mode)
+		ctx1_iv_off = 16;
+
+	/*
+	 * RFC3686 specific:
+	 *	CONTEXT1[255:128] = {NONCE, IV, COUNTER}
+	 */
+	if (is_rfc3686)
+		ctx1_iv_off = 16 + CTR_RFC3686_NONCE_SIZE;
+
+	/*
 	 * Job Descriptor and Shared Descriptors
 	 * must all fit into the 64-word Descriptor h/w Buffer
 	 */
+	keys_fit_inline = false;
 	if (DESC_AEAD_ENC_LEN + DESC_JOB_IO_LEN +
-	    ctx->split_key_pad_len + ctx->enckeylen <=
+	    ctx->split_key_pad_len + ctx->enckeylen +
+	    (is_rfc3686 ? DESC_AEAD_CTR_RFC3686_LEN : 0) <=
 	    CAAM_DESC_BYTES_MAX)
 		keys_fit_inline = true;
 
 	/* aead_encrypt shared descriptor */
 	desc = ctx->sh_desc_enc;
 
-	init_sh_desc_key_aead(desc, ctx, keys_fit_inline);
+	/* Note: Context registers are saved. */
+	init_sh_desc_key_aead(desc, ctx, keys_fit_inline, is_rfc3686);
 
 	/* Class 2 operation */
 	append_operation(desc, ctx->class2_alg_type |
@@ -448,7 +518,15 @@ static int aead_set_sh_desc(struct crypto_aead *aead)
 	/* read assoc before reading payload */
 	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS2 | FIFOLD_TYPE_MSG |
 			     KEY_VLF);
-	aead_append_ld_iv(desc, tfm->ivsize);
+	aead_append_ld_iv(desc, tfm->ivsize, ctx1_iv_off);
+
+	/* Load Counter into CONTEXT1 reg */
+	if (is_rfc3686)
+		append_load_imm_u32(desc, be32_to_cpu(1), LDST_IMM |
+				    LDST_CLASS_1_CCB |
+				    LDST_SRCDST_BYTE_CONTEXT |
+				    ((ctx1_iv_off + CTR_RFC3686_IV_SIZE) <<
+				     LDST_OFFSET_SHIFT));
 
 	/* Class 1 operation */
 	append_operation(desc, ctx->class1_alg_type |
@@ -482,14 +560,16 @@ static int aead_set_sh_desc(struct crypto_aead *aead)
 	 */
 	keys_fit_inline = false;
 	if (DESC_AEAD_DEC_LEN + DESC_JOB_IO_LEN +
-	    ctx->split_key_pad_len + ctx->enckeylen <=
+	    ctx->split_key_pad_len + ctx->enckeylen +
+	    (is_rfc3686 ? DESC_AEAD_CTR_RFC3686_LEN : 0) <=
 	    CAAM_DESC_BYTES_MAX)
 		keys_fit_inline = true;
 
 	/* aead_decrypt shared descriptor */
 	desc = ctx->sh_desc_dec;
 
-	init_sh_desc_key_aead(desc, ctx, keys_fit_inline);
+	/* Note: Context registers are saved. */
+	init_sh_desc_key_aead(desc, ctx, keys_fit_inline, is_rfc3686);
 
 	/* Class 2 operation */
 	append_operation(desc, ctx->class2_alg_type |
@@ -506,9 +586,22 @@ static int aead_set_sh_desc(struct crypto_aead *aead)
 	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS2 | FIFOLD_TYPE_MSG |
 			     KEY_VLF);
 
-	aead_append_ld_iv(desc, tfm->ivsize);
+	aead_append_ld_iv(desc, tfm->ivsize, ctx1_iv_off);
 
-	append_dec_op1(desc, ctx->class1_alg_type);
+	/* Load Counter into CONTEXT1 reg */
+	if (is_rfc3686)
+		append_load_imm_u32(desc, be32_to_cpu(1), LDST_IMM |
+				    LDST_CLASS_1_CCB |
+				    LDST_SRCDST_BYTE_CONTEXT |
+				    ((ctx1_iv_off + CTR_RFC3686_IV_SIZE) <<
+				     LDST_OFFSET_SHIFT));
+
+	/* Choose operation */
+	if (ctr_mode)
+		append_operation(desc, ctx->class1_alg_type |
+				 OP_ALG_AS_INITFINAL | OP_ALG_DECRYPT);
+	else
+		append_dec_op1(desc, ctx->class1_alg_type);
 
 	/* Read and write cryptlen bytes */
 	append_math_add(desc, VARSEQINLEN, ZERO, REG2, CAAM_CMD_SZ);
@@ -538,14 +631,16 @@ static int aead_set_sh_desc(struct crypto_aead *aead)
 	 */
 	keys_fit_inline = false;
 	if (DESC_AEAD_GIVENC_LEN + DESC_JOB_IO_LEN +
-	    ctx->split_key_pad_len + ctx->enckeylen <=
+	    ctx->split_key_pad_len + ctx->enckeylen +
+	    (is_rfc3686 ? DESC_AEAD_CTR_RFC3686_LEN : 0) <=
 	    CAAM_DESC_BYTES_MAX)
 		keys_fit_inline = true;
 
 	/* aead_givencrypt shared descriptor */
 	desc = ctx->sh_desc_givenc;
 
-	init_sh_desc_key_aead(desc, ctx, keys_fit_inline);
+	/* Note: Context registers are saved. */
+	init_sh_desc_key_aead(desc, ctx, keys_fit_inline, is_rfc3686);
 
 	/* Generate IV */
 	geniv = NFIFOENTRY_STYPE_PAD | NFIFOENTRY_DEST_DECO |
@@ -554,13 +649,16 @@ static int aead_set_sh_desc(struct crypto_aead *aead)
 	append_load_imm_u32(desc, geniv, LDST_CLASS_IND_CCB |
 			    LDST_SRCDST_WORD_INFO_FIFO | LDST_IMM);
 	append_cmd(desc, CMD_LOAD | DISABLE_AUTO_INFO_FIFO);
-	append_move(desc, MOVE_SRC_INFIFO |
-		    MOVE_DEST_CLASS1CTX | (tfm->ivsize << MOVE_LEN_SHIFT));
+	append_move(desc, MOVE_WAITCOMP |
+		    MOVE_SRC_INFIFO | MOVE_DEST_CLASS1CTX |
+		    (ctx1_iv_off << MOVE_OFFSET_SHIFT) |
+		    (tfm->ivsize << MOVE_LEN_SHIFT));
 	append_cmd(desc, CMD_LOAD | ENABLE_AUTO_INFO_FIFO);
 
 	/* Copy IV to class 1 context */
-	append_move(desc, MOVE_SRC_CLASS1CTX |
-		    MOVE_DEST_OUTFIFO | (tfm->ivsize << MOVE_LEN_SHIFT));
+	append_move(desc, MOVE_SRC_CLASS1CTX | MOVE_DEST_OUTFIFO |
+		    (ctx1_iv_off << MOVE_OFFSET_SHIFT) |
+		    (tfm->ivsize << MOVE_LEN_SHIFT));
 
 	/* Return to encryption */
 	append_operation(desc, ctx->class2_alg_type |
@@ -576,7 +674,7 @@ static int aead_set_sh_desc(struct crypto_aead *aead)
 	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS2 | FIFOLD_TYPE_MSG |
 			     KEY_VLF);
 
-	/* Copy iv from class 1 ctx to class 2 fifo*/
+	/* Copy iv from outfifo to class 2 fifo */
 	moveiv = NFIFOENTRY_STYPE_OFIFO | NFIFOENTRY_DEST_CLASS2 |
 		 NFIFOENTRY_DTYPE_MSG | (tfm->ivsize << NFIFOENTRY_DLEN_SHIFT);
 	append_load_imm_u32(desc, moveiv, LDST_CLASS_IND_CCB |
@@ -584,6 +682,14 @@ static int aead_set_sh_desc(struct crypto_aead *aead)
 	append_load_imm_u32(desc, tfm->ivsize, LDST_CLASS_2_CCB |
 			    LDST_SRCDST_WORD_DATASZ_REG | LDST_IMM);
 
+	/* Load Counter into CONTEXT1 reg */
+	if (is_rfc3686)
+		append_load_imm_u32(desc, be32_to_cpu(1), LDST_IMM |
+				    LDST_CLASS_1_CCB |
+				    LDST_SRCDST_BYTE_CONTEXT |
+				    ((ctx1_iv_off + CTR_RFC3686_IV_SIZE) <<
+				     LDST_OFFSET_SHIFT));
+
 	/* Class 1 operation */
 	append_operation(desc, ctx->class1_alg_type |
 			 OP_ALG_AS_INITFINAL | OP_ALG_ENCRYPT);
@@ -630,6 +736,912 @@ static int aead_setauthsize(struct crypto_aead *authenc,
 	return 0;
 }
 
+static int gcm_set_sh_desc(struct crypto_aead *aead)
+{
+	struct aead_tfm *tfm = &aead->base.crt_aead;
+	struct caam_ctx *ctx = crypto_aead_ctx(aead);
+	struct device *jrdev = ctx->jrdev;
+	bool keys_fit_inline = false;
+	u32 *key_jump_cmd, *zero_payload_jump_cmd,
+	    *zero_assoc_jump_cmd1, *zero_assoc_jump_cmd2;
+	u32 *desc;
+
+	if (!ctx->enckeylen || !ctx->authsize)
+		return 0;
+
+	/*
+	 * AES GCM encrypt shared descriptor
+	 * Job Descriptor and Shared Descriptor
+	 * must fit into the 64-word Descriptor h/w Buffer
+	 */
+	if (DESC_GCM_ENC_LEN + DESC_JOB_IO_LEN +
+	    ctx->enckeylen <= CAAM_DESC_BYTES_MAX)
+		keys_fit_inline = true;
+
+	desc = ctx->sh_desc_enc;
+
+	init_sh_desc(desc, HDR_SHARE_SERIAL);
+
+	/* skip key loading if they are loaded due to sharing */
+	key_jump_cmd = append_jump(desc, JUMP_JSL | JUMP_TEST_ALL |
+				   JUMP_COND_SHRD | JUMP_COND_SELF);
+	if (keys_fit_inline)
+		append_key_as_imm(desc, (void *)ctx->key, ctx->enckeylen,
+				  ctx->enckeylen, CLASS_1 | KEY_DEST_CLASS_REG);
+	else
+		append_key(desc, ctx->key_dma, ctx->enckeylen,
+			   CLASS_1 | KEY_DEST_CLASS_REG);
+	set_jump_tgt_here(desc, key_jump_cmd);
+
+	/* class 1 operation */
+	append_operation(desc, ctx->class1_alg_type |
+			 OP_ALG_AS_INITFINAL | OP_ALG_ENCRYPT);
+
+	/* cryptlen = seqoutlen - authsize */
+	append_math_sub_imm_u32(desc, REG3, SEQOUTLEN, IMM, ctx->authsize);
+
+	/* assoclen + cryptlen = seqinlen - ivsize */
+	append_math_sub_imm_u32(desc, REG2, SEQINLEN, IMM, tfm->ivsize);
+
+	/* assoclen = (assoclen + cryptlen) - cryptlen */
+	append_math_sub(desc, REG1, REG2, REG3, CAAM_CMD_SZ);
+
+	/* if cryptlen is ZERO jump to zero-payload commands */
+	append_math_add(desc, VARSEQOUTLEN, ZERO, REG3, CAAM_CMD_SZ);
+	zero_payload_jump_cmd = append_jump(desc, JUMP_TEST_ALL |
+					    JUMP_COND_MATH_Z);
+	/* read IV */
+	append_seq_fifo_load(desc, tfm->ivsize, FIFOLD_CLASS_CLASS1 |
+			     FIFOLD_TYPE_IV | FIFOLD_TYPE_FLUSH1);
+
+	/* if assoclen is ZERO, skip reading the assoc data */
+	append_math_add(desc, VARSEQINLEN, ZERO, REG1, CAAM_CMD_SZ);
+	zero_assoc_jump_cmd1 = append_jump(desc, JUMP_TEST_ALL |
+					   JUMP_COND_MATH_Z);
+
+	/* read assoc data */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 | FIFOLDST_VLF |
+			     FIFOLD_TYPE_AAD | FIFOLD_TYPE_FLUSH1);
+	set_jump_tgt_here(desc, zero_assoc_jump_cmd1);
+
+	append_math_add(desc, VARSEQINLEN, ZERO, REG3, CAAM_CMD_SZ);
+
+	/* write encrypted data */
+	append_seq_fifo_store(desc, 0, FIFOST_TYPE_MESSAGE_DATA | FIFOLDST_VLF);
+
+	/* read payload data */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 | FIFOLDST_VLF |
+			     FIFOLD_TYPE_MSG | FIFOLD_TYPE_LAST1);
+
+	/* jump the zero-payload commands */
+	append_jump(desc, JUMP_TEST_ALL | 7);
+
+	/* zero-payload commands */
+	set_jump_tgt_here(desc, zero_payload_jump_cmd);
+
+	/* if assoclen is ZERO, jump to IV reading - is the only input data */
+	append_math_add(desc, VARSEQINLEN, ZERO, REG1, CAAM_CMD_SZ);
+	zero_assoc_jump_cmd2 = append_jump(desc, JUMP_TEST_ALL |
+					   JUMP_COND_MATH_Z);
+	/* read IV */
+	append_seq_fifo_load(desc, tfm->ivsize, FIFOLD_CLASS_CLASS1 |
+			     FIFOLD_TYPE_IV | FIFOLD_TYPE_FLUSH1);
+
+	/* read assoc data */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 | FIFOLDST_VLF |
+			     FIFOLD_TYPE_AAD | FIFOLD_TYPE_LAST1);
+
+	/* jump to ICV writing */
+	append_jump(desc, JUMP_TEST_ALL | 2);
+
+	/* read IV - is the only input data */
+	set_jump_tgt_here(desc, zero_assoc_jump_cmd2);
+	append_seq_fifo_load(desc, tfm->ivsize, FIFOLD_CLASS_CLASS1 |
+			     FIFOLD_TYPE_IV | FIFOLD_TYPE_FLUSH1 |
+			     FIFOLD_TYPE_LAST1);
+
+	/* write ICV */
+	append_seq_store(desc, ctx->authsize, LDST_CLASS_1_CCB |
+			 LDST_SRCDST_BYTE_CONTEXT);
+
+	ctx->sh_desc_enc_dma = dma_map_single(jrdev, desc,
+					      desc_bytes(desc),
+					      DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->sh_desc_enc_dma)) {
+		dev_err(jrdev, "unable to map shared descriptor\n");
+		return -ENOMEM;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "gcm enc shdesc@"__stringify(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+		       desc_bytes(desc), 1);
+#endif
+
+	/*
+	 * Job Descriptor and Shared Descriptors
+	 * must all fit into the 64-word Descriptor h/w Buffer
+	 */
+	keys_fit_inline = false;
+	if (DESC_GCM_DEC_LEN + DESC_JOB_IO_LEN +
+	    ctx->enckeylen <= CAAM_DESC_BYTES_MAX)
+		keys_fit_inline = true;
+
+	desc = ctx->sh_desc_dec;
+
+	init_sh_desc(desc, HDR_SHARE_SERIAL);
+
+	/* skip key loading if they are loaded due to sharing */
+	key_jump_cmd = append_jump(desc, JUMP_JSL |
+				   JUMP_TEST_ALL | JUMP_COND_SHRD |
+				   JUMP_COND_SELF);
+	if (keys_fit_inline)
+		append_key_as_imm(desc, (void *)ctx->key, ctx->enckeylen,
+				  ctx->enckeylen, CLASS_1 | KEY_DEST_CLASS_REG);
+	else
+		append_key(desc, ctx->key_dma, ctx->enckeylen,
+			   CLASS_1 | KEY_DEST_CLASS_REG);
+	set_jump_tgt_here(desc, key_jump_cmd);
+
+	/* class 1 operation */
+	append_operation(desc, ctx->class1_alg_type |
+			 OP_ALG_AS_INITFINAL | OP_ALG_DECRYPT | OP_ALG_ICV_ON);
+
+	/* assoclen + cryptlen = seqinlen - ivsize - icvsize */
+	append_math_sub_imm_u32(desc, REG3, SEQINLEN, IMM,
+				ctx->authsize + tfm->ivsize);
+
+	/* assoclen = (assoclen + cryptlen) - cryptlen */
+	append_math_sub(desc, REG2, SEQOUTLEN, REG0, CAAM_CMD_SZ);
+	append_math_sub(desc, REG1, REG3, REG2, CAAM_CMD_SZ);
+
+	/* read IV */
+	append_seq_fifo_load(desc, tfm->ivsize, FIFOLD_CLASS_CLASS1 |
+			     FIFOLD_TYPE_IV | FIFOLD_TYPE_FLUSH1);
+
+	/* jump to zero-payload command if cryptlen is zero */
+	append_math_add(desc, VARSEQOUTLEN, ZERO, REG2, CAAM_CMD_SZ);
+	zero_payload_jump_cmd = append_jump(desc, JUMP_TEST_ALL |
+					    JUMP_COND_MATH_Z);
+
+	append_math_add(desc, VARSEQINLEN, ZERO, REG1, CAAM_CMD_SZ);
+	/* if asoclen is ZERO, skip reading assoc data */
+	zero_assoc_jump_cmd1 = append_jump(desc, JUMP_TEST_ALL |
+					   JUMP_COND_MATH_Z);
+	/* read assoc data */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 | FIFOLDST_VLF |
+			     FIFOLD_TYPE_AAD | FIFOLD_TYPE_FLUSH1);
+	set_jump_tgt_here(desc, zero_assoc_jump_cmd1);
+
+	append_math_add(desc, VARSEQINLEN, ZERO, REG2, CAAM_CMD_SZ);
+
+	/* store encrypted data */
+	append_seq_fifo_store(desc, 0, FIFOST_TYPE_MESSAGE_DATA | FIFOLDST_VLF);
+
+	/* read payload data */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 | FIFOLDST_VLF |
+			     FIFOLD_TYPE_MSG | FIFOLD_TYPE_FLUSH1);
+
+	/* jump the zero-payload commands */
+	append_jump(desc, JUMP_TEST_ALL | 4);
+
+	/* zero-payload command */
+	set_jump_tgt_here(desc, zero_payload_jump_cmd);
+
+	/* if assoclen is ZERO, jump to ICV reading */
+	append_math_add(desc, VARSEQINLEN, ZERO, REG1, CAAM_CMD_SZ);
+	zero_assoc_jump_cmd2 = append_jump(desc, JUMP_TEST_ALL |
+					   JUMP_COND_MATH_Z);
+	/* read assoc data */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 | FIFOLDST_VLF |
+			     FIFOLD_TYPE_AAD | FIFOLD_TYPE_FLUSH1);
+	set_jump_tgt_here(desc, zero_assoc_jump_cmd2);
+
+	/* read ICV */
+	append_seq_fifo_load(desc, ctx->authsize, FIFOLD_CLASS_CLASS1 |
+			     FIFOLD_TYPE_ICV | FIFOLD_TYPE_LAST1);
+
+	ctx->sh_desc_dec_dma = dma_map_single(jrdev, desc,
+					      desc_bytes(desc),
+					      DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->sh_desc_dec_dma)) {
+		dev_err(jrdev, "unable to map shared descriptor\n");
+		return -ENOMEM;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "gcm dec shdesc@"__stringify(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+		       desc_bytes(desc), 1);
+#endif
+
+	return 0;
+}
+
+static int gcm_setauthsize(struct crypto_aead *authenc, unsigned int authsize)
+{
+	struct caam_ctx *ctx = crypto_aead_ctx(authenc);
+
+	ctx->authsize = authsize;
+	gcm_set_sh_desc(authenc);
+
+	return 0;
+}
+
+static int rfc4106_set_sh_desc(struct crypto_aead *aead)
+{
+	struct aead_tfm *tfm = &aead->base.crt_aead;
+	struct caam_ctx *ctx = crypto_aead_ctx(aead);
+	struct device *jrdev = ctx->jrdev;
+	bool keys_fit_inline = false;
+	u32 *key_jump_cmd, *move_cmd, *write_iv_cmd;
+	u32 *desc;
+	u32 geniv;
+
+	if (!ctx->enckeylen || !ctx->authsize)
+		return 0;
+
+	/*
+	 * RFC4106 encrypt shared descriptor
+	 * Job Descriptor and Shared Descriptor
+	 * must fit into the 64-word Descriptor h/w Buffer
+	 */
+	if (DESC_RFC4106_ENC_LEN + DESC_JOB_IO_LEN +
+	    ctx->enckeylen <= CAAM_DESC_BYTES_MAX)
+		keys_fit_inline = true;
+
+	desc = ctx->sh_desc_enc;
+
+	init_sh_desc(desc, HDR_SHARE_SERIAL);
+
+	/* Skip key loading if it is loaded due to sharing */
+	key_jump_cmd = append_jump(desc, JUMP_JSL | JUMP_TEST_ALL |
+				   JUMP_COND_SHRD);
+	if (keys_fit_inline)
+		append_key_as_imm(desc, (void *)ctx->key, ctx->enckeylen,
+				  ctx->enckeylen, CLASS_1 | KEY_DEST_CLASS_REG);
+	else
+		append_key(desc, ctx->key_dma, ctx->enckeylen,
+			   CLASS_1 | KEY_DEST_CLASS_REG);
+	set_jump_tgt_here(desc, key_jump_cmd);
+
+	/* Class 1 operation */
+	append_operation(desc, ctx->class1_alg_type |
+			 OP_ALG_AS_INITFINAL | OP_ALG_ENCRYPT);
+
+	/* cryptlen = seqoutlen - authsize */
+	append_math_sub_imm_u32(desc, REG3, SEQOUTLEN, IMM, ctx->authsize);
+	append_math_add(desc, VARSEQOUTLEN, ZERO, REG3, CAAM_CMD_SZ);
+
+	/* assoclen + cryptlen = seqinlen - ivsize */
+	append_math_sub_imm_u32(desc, REG2, SEQINLEN, IMM, tfm->ivsize);
+
+	/* assoclen = (assoclen + cryptlen) - cryptlen */
+	append_math_sub(desc, VARSEQINLEN, REG2, REG3, CAAM_CMD_SZ);
+
+	/* Read Salt */
+	append_fifo_load_as_imm(desc, (void *)(ctx->key + ctx->enckeylen),
+				4, FIFOLD_CLASS_CLASS1 | FIFOLD_TYPE_IV);
+	/* Read AES-GCM-ESP IV */
+	append_seq_fifo_load(desc, tfm->ivsize, FIFOLD_CLASS_CLASS1 |
+			     FIFOLD_TYPE_IV | FIFOLD_TYPE_FLUSH1);
+
+	/* Read assoc data */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 | FIFOLDST_VLF |
+			     FIFOLD_TYPE_AAD | FIFOLD_TYPE_FLUSH1);
+
+	/* Will read cryptlen bytes */
+	append_math_add(desc, VARSEQINLEN, ZERO, REG3, CAAM_CMD_SZ);
+
+	/* Write encrypted data */
+	append_seq_fifo_store(desc, 0, FIFOST_TYPE_MESSAGE_DATA | FIFOLDST_VLF);
+
+	/* Read payload data */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 | FIFOLDST_VLF |
+			     FIFOLD_TYPE_MSG | FIFOLD_TYPE_LAST1);
+
+	/* Write ICV */
+	append_seq_store(desc, ctx->authsize, LDST_CLASS_1_CCB |
+			 LDST_SRCDST_BYTE_CONTEXT);
+
+	ctx->sh_desc_enc_dma = dma_map_single(jrdev, desc,
+					      desc_bytes(desc),
+					      DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->sh_desc_enc_dma)) {
+		dev_err(jrdev, "unable to map shared descriptor\n");
+		return -ENOMEM;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "rfc4106 enc shdesc@"__stringify(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+		       desc_bytes(desc), 1);
+#endif
+
+	/*
+	 * Job Descriptor and Shared Descriptors
+	 * must all fit into the 64-word Descriptor h/w Buffer
+	 */
+	keys_fit_inline = false;
+	if (DESC_RFC4106_DEC_LEN + DESC_JOB_IO_LEN +
+	    ctx->enckeylen <= CAAM_DESC_BYTES_MAX)
+		keys_fit_inline = true;
+
+	desc = ctx->sh_desc_dec;
+
+	init_sh_desc(desc, HDR_SHARE_SERIAL);
+
+	/* Skip key loading if it is loaded due to sharing */
+	key_jump_cmd = append_jump(desc, JUMP_JSL |
+				   JUMP_TEST_ALL | JUMP_COND_SHRD);
+	if (keys_fit_inline)
+		append_key_as_imm(desc, (void *)ctx->key, ctx->enckeylen,
+				  ctx->enckeylen, CLASS_1 | KEY_DEST_CLASS_REG);
+	else
+		append_key(desc, ctx->key_dma, ctx->enckeylen,
+			   CLASS_1 | KEY_DEST_CLASS_REG);
+	set_jump_tgt_here(desc, key_jump_cmd);
+
+	/* Class 1 operation */
+	append_operation(desc, ctx->class1_alg_type |
+			 OP_ALG_AS_INITFINAL | OP_ALG_DECRYPT | OP_ALG_ICV_ON);
+
+	/* assoclen + cryptlen = seqinlen - ivsize - icvsize */
+	append_math_sub_imm_u32(desc, REG3, SEQINLEN, IMM,
+				ctx->authsize + tfm->ivsize);
+
+	/* assoclen = (assoclen + cryptlen) - cryptlen */
+	append_math_sub(desc, REG2, SEQOUTLEN, REG0, CAAM_CMD_SZ);
+	append_math_sub(desc, VARSEQINLEN, REG3, REG2, CAAM_CMD_SZ);
+
+	/* Will write cryptlen bytes */
+	append_math_sub(desc, VARSEQOUTLEN, SEQOUTLEN, REG0, CAAM_CMD_SZ);
+
+	/* Read Salt */
+	append_fifo_load_as_imm(desc, (void *)(ctx->key + ctx->enckeylen),
+				4, FIFOLD_CLASS_CLASS1 | FIFOLD_TYPE_IV);
+	/* Read AES-GCM-ESP IV */
+	append_seq_fifo_load(desc, tfm->ivsize, FIFOLD_CLASS_CLASS1 |
+			     FIFOLD_TYPE_IV | FIFOLD_TYPE_FLUSH1);
+
+	/* Read assoc data */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 | FIFOLDST_VLF |
+			     FIFOLD_TYPE_AAD | FIFOLD_TYPE_FLUSH1);
+
+	/* Will read cryptlen bytes */
+	append_math_add(desc, VARSEQINLEN, ZERO, REG2, CAAM_CMD_SZ);
+
+	/* Store payload data */
+	append_seq_fifo_store(desc, 0, FIFOST_TYPE_MESSAGE_DATA | FIFOLDST_VLF);
+
+	/* Read encrypted data */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 | FIFOLDST_VLF |
+			     FIFOLD_TYPE_MSG | FIFOLD_TYPE_FLUSH1);
+
+	/* Read ICV */
+	append_seq_fifo_load(desc, ctx->authsize, FIFOLD_CLASS_CLASS1 |
+			     FIFOLD_TYPE_ICV | FIFOLD_TYPE_LAST1);
+
+	ctx->sh_desc_dec_dma = dma_map_single(jrdev, desc,
+					      desc_bytes(desc),
+					      DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->sh_desc_dec_dma)) {
+		dev_err(jrdev, "unable to map shared descriptor\n");
+		return -ENOMEM;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "rfc4106 dec shdesc@"__stringify(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+		       desc_bytes(desc), 1);
+#endif
+
+	/*
+	 * Job Descriptor and Shared Descriptors
+	 * must all fit into the 64-word Descriptor h/w Buffer
+	 */
+	keys_fit_inline = false;
+	if (DESC_RFC4106_GIVENC_LEN + DESC_JOB_IO_LEN +
+	    ctx->split_key_pad_len + ctx->enckeylen <=
+	    CAAM_DESC_BYTES_MAX)
+		keys_fit_inline = true;
+
+	/* rfc4106_givencrypt shared descriptor */
+	desc = ctx->sh_desc_givenc;
+
+	init_sh_desc(desc, HDR_SHARE_SERIAL);
+
+	/* Skip key loading if it is loaded due to sharing */
+	key_jump_cmd = append_jump(desc, JUMP_JSL | JUMP_TEST_ALL |
+				   JUMP_COND_SHRD);
+	if (keys_fit_inline)
+		append_key_as_imm(desc, (void *)ctx->key, ctx->enckeylen,
+				  ctx->enckeylen, CLASS_1 | KEY_DEST_CLASS_REG);
+	else
+		append_key(desc, ctx->key_dma, ctx->enckeylen,
+			   CLASS_1 | KEY_DEST_CLASS_REG);
+	set_jump_tgt_here(desc, key_jump_cmd);
+
+	/* Generate IV */
+	geniv = NFIFOENTRY_STYPE_PAD | NFIFOENTRY_DEST_DECO |
+		NFIFOENTRY_DTYPE_MSG | NFIFOENTRY_LC1 |
+		NFIFOENTRY_PTYPE_RND | (tfm->ivsize << NFIFOENTRY_DLEN_SHIFT);
+	append_load_imm_u32(desc, geniv, LDST_CLASS_IND_CCB |
+			    LDST_SRCDST_WORD_INFO_FIFO | LDST_IMM);
+	append_cmd(desc, CMD_LOAD | DISABLE_AUTO_INFO_FIFO);
+	move_cmd = append_move(desc, MOVE_SRC_INFIFO | MOVE_DEST_DESCBUF |
+			       (tfm->ivsize << MOVE_LEN_SHIFT));
+	append_cmd(desc, CMD_LOAD | ENABLE_AUTO_INFO_FIFO);
+
+	/* Copy generated IV to OFIFO */
+	write_iv_cmd = append_move(desc, MOVE_SRC_DESCBUF | MOVE_DEST_OUTFIFO |
+				   (tfm->ivsize << MOVE_LEN_SHIFT));
+
+	/* Class 1 operation */
+	append_operation(desc, ctx->class1_alg_type |
+			 OP_ALG_AS_INITFINAL | OP_ALG_ENCRYPT);
+
+	/* ivsize + cryptlen = seqoutlen - authsize */
+	append_math_sub_imm_u32(desc, REG3, SEQOUTLEN, IMM, ctx->authsize);
+
+	/* assoclen = seqinlen - (ivsize + cryptlen) */
+	append_math_sub(desc, VARSEQINLEN, SEQINLEN, REG3, CAAM_CMD_SZ);
+
+	/* Will write ivsize + cryptlen */
+	append_math_add(desc, VARSEQOUTLEN, REG3, REG0, CAAM_CMD_SZ);
+
+	/* Read Salt and generated IV */
+	append_cmd(desc, CMD_FIFO_LOAD | FIFOLD_CLASS_CLASS1 | FIFOLD_TYPE_IV |
+		   FIFOLD_TYPE_FLUSH1 | IMMEDIATE | 12);
+	/* Append Salt */
+	append_data(desc, (void *)(ctx->key + ctx->enckeylen), 4);
+	set_move_tgt_here(desc, move_cmd);
+	set_move_tgt_here(desc, write_iv_cmd);
+	/* Blank commands. Will be overwritten by generated IV. */
+	append_cmd(desc, 0x00000000);
+	append_cmd(desc, 0x00000000);
+	/* End of blank commands */
+
+	/* No need to reload iv */
+	append_seq_fifo_load(desc, tfm->ivsize, FIFOLD_CLASS_SKIP);
+
+	/* Read assoc data */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 | FIFOLDST_VLF |
+			     FIFOLD_TYPE_AAD | FIFOLD_TYPE_FLUSH1);
+
+	/* Will read cryptlen */
+	append_math_add(desc, VARSEQINLEN, SEQINLEN, REG0, CAAM_CMD_SZ);
+
+	/* Store generated IV and encrypted data */
+	append_seq_fifo_store(desc, 0, FIFOST_TYPE_MESSAGE_DATA | FIFOLDST_VLF);
+
+	/* Read payload data */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 | FIFOLDST_VLF |
+			     FIFOLD_TYPE_MSG | FIFOLD_TYPE_LAST1);
+
+	/* Write ICV */
+	append_seq_store(desc, ctx->authsize, LDST_CLASS_1_CCB |
+			 LDST_SRCDST_BYTE_CONTEXT);
+
+	ctx->sh_desc_givenc_dma = dma_map_single(jrdev, desc,
+						 desc_bytes(desc),
+						 DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->sh_desc_givenc_dma)) {
+		dev_err(jrdev, "unable to map shared descriptor\n");
+		return -ENOMEM;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR,
+		       "rfc4106 givenc shdesc@"__stringify(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+		       desc_bytes(desc), 1);
+#endif
+
+	return 0;
+}
+
+static int rfc4106_setauthsize(struct crypto_aead *authenc,
+			       unsigned int authsize)
+{
+	struct caam_ctx *ctx = crypto_aead_ctx(authenc);
+
+	ctx->authsize = authsize;
+	rfc4106_set_sh_desc(authenc);
+
+	return 0;
+}
+
+static int rfc4543_set_sh_desc(struct crypto_aead *aead)
+{
+	struct aead_tfm *tfm = &aead->base.crt_aead;
+	struct caam_ctx *ctx = crypto_aead_ctx(aead);
+	struct device *jrdev = ctx->jrdev;
+	bool keys_fit_inline = false;
+	u32 *key_jump_cmd, *write_iv_cmd, *write_aad_cmd;
+	u32 *read_move_cmd, *write_move_cmd;
+	u32 *desc;
+	u32 geniv;
+
+	if (!ctx->enckeylen || !ctx->authsize)
+		return 0;
+
+	/*
+	 * RFC4543 encrypt shared descriptor
+	 * Job Descriptor and Shared Descriptor
+	 * must fit into the 64-word Descriptor h/w Buffer
+	 */
+	if (DESC_RFC4543_ENC_LEN + DESC_JOB_IO_LEN +
+	    ctx->enckeylen <= CAAM_DESC_BYTES_MAX)
+		keys_fit_inline = true;
+
+	desc = ctx->sh_desc_enc;
+
+	init_sh_desc(desc, HDR_SHARE_SERIAL);
+
+	/* Skip key loading if it is loaded due to sharing */
+	key_jump_cmd = append_jump(desc, JUMP_JSL | JUMP_TEST_ALL |
+				   JUMP_COND_SHRD);
+	if (keys_fit_inline)
+		append_key_as_imm(desc, (void *)ctx->key, ctx->enckeylen,
+				  ctx->enckeylen, CLASS_1 | KEY_DEST_CLASS_REG);
+	else
+		append_key(desc, ctx->key_dma, ctx->enckeylen,
+			   CLASS_1 | KEY_DEST_CLASS_REG);
+	set_jump_tgt_here(desc, key_jump_cmd);
+
+	/* Class 1 operation */
+	append_operation(desc, ctx->class1_alg_type |
+			 OP_ALG_AS_INITFINAL | OP_ALG_ENCRYPT);
+
+	/* Load AES-GMAC ESP IV into Math1 register */
+	append_cmd(desc, CMD_SEQ_LOAD | LDST_SRCDST_WORD_DECO_MATH1 |
+		   LDST_CLASS_DECO | tfm->ivsize);
+
+	/* Wait the DMA transaction to finish */
+	append_jump(desc, JUMP_TEST_ALL | JUMP_COND_CALM |
+		    (1 << JUMP_OFFSET_SHIFT));
+
+	/* Overwrite blank immediate AES-GMAC ESP IV data */
+	write_iv_cmd = append_move(desc, MOVE_SRC_MATH1 | MOVE_DEST_DESCBUF |
+				   (tfm->ivsize << MOVE_LEN_SHIFT));
+
+	/* Overwrite blank immediate AAD data */
+	write_aad_cmd = append_move(desc, MOVE_SRC_MATH1 | MOVE_DEST_DESCBUF |
+				    (tfm->ivsize << MOVE_LEN_SHIFT));
+
+	/* cryptlen = seqoutlen - authsize */
+	append_math_sub_imm_u32(desc, REG3, SEQOUTLEN, IMM, ctx->authsize);
+
+	/* assoclen = (seqinlen - ivsize) - cryptlen */
+	append_math_sub(desc, VARSEQINLEN, SEQINLEN, REG3, CAAM_CMD_SZ);
+
+	/* Read Salt and AES-GMAC ESP IV */
+	append_cmd(desc, CMD_FIFO_LOAD | FIFOLD_CLASS_CLASS1 | IMMEDIATE |
+		   FIFOLD_TYPE_IV | FIFOLD_TYPE_FLUSH1 | (4 + tfm->ivsize));
+	/* Append Salt */
+	append_data(desc, (void *)(ctx->key + ctx->enckeylen), 4);
+	set_move_tgt_here(desc, write_iv_cmd);
+	/* Blank commands. Will be overwritten by AES-GMAC ESP IV. */
+	append_cmd(desc, 0x00000000);
+	append_cmd(desc, 0x00000000);
+	/* End of blank commands */
+
+	/* Read assoc data */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 | FIFOLDST_VLF |
+			     FIFOLD_TYPE_AAD);
+
+	/* Will read cryptlen bytes */
+	append_math_add(desc, VARSEQINLEN, ZERO, REG3, CAAM_CMD_SZ);
+
+	/* Will write cryptlen bytes */
+	append_math_add(desc, VARSEQOUTLEN, ZERO, REG3, CAAM_CMD_SZ);
+
+	/*
+	 * MOVE_LEN opcode is not available in all SEC HW revisions,
+	 * thus need to do some magic, i.e. self-patch the descriptor
+	 * buffer.
+	 */
+	read_move_cmd = append_move(desc, MOVE_SRC_DESCBUF | MOVE_DEST_MATH3 |
+				    (0x6 << MOVE_LEN_SHIFT));
+	write_move_cmd = append_move(desc, MOVE_SRC_MATH3 | MOVE_DEST_DESCBUF |
+				     (0x8 << MOVE_LEN_SHIFT));
+
+	/* Authenticate AES-GMAC ESP IV  */
+	append_cmd(desc, CMD_FIFO_LOAD | FIFOLD_CLASS_CLASS1 | IMMEDIATE |
+		   FIFOLD_TYPE_AAD | tfm->ivsize);
+	set_move_tgt_here(desc, write_aad_cmd);
+	/* Blank commands. Will be overwritten by AES-GMAC ESP IV. */
+	append_cmd(desc, 0x00000000);
+	append_cmd(desc, 0x00000000);
+	/* End of blank commands */
+
+	/* Read and write cryptlen bytes */
+	aead_append_src_dst(desc, FIFOLD_TYPE_AAD);
+
+	set_move_tgt_here(desc, read_move_cmd);
+	set_move_tgt_here(desc, write_move_cmd);
+	append_cmd(desc, CMD_LOAD | DISABLE_AUTO_INFO_FIFO);
+	/* Move payload data to OFIFO */
+	append_move(desc, MOVE_SRC_INFIFO_CL | MOVE_DEST_OUTFIFO);
+
+	/* Write ICV */
+	append_seq_store(desc, ctx->authsize, LDST_CLASS_1_CCB |
+			 LDST_SRCDST_BYTE_CONTEXT);
+
+	ctx->sh_desc_enc_dma = dma_map_single(jrdev, desc,
+					      desc_bytes(desc),
+					      DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->sh_desc_enc_dma)) {
+		dev_err(jrdev, "unable to map shared descriptor\n");
+		return -ENOMEM;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "rfc4543 enc shdesc@"__stringify(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+		       desc_bytes(desc), 1);
+#endif
+
+	/*
+	 * Job Descriptor and Shared Descriptors
+	 * must all fit into the 64-word Descriptor h/w Buffer
+	 */
+	keys_fit_inline = false;
+	if (DESC_RFC4543_DEC_LEN + DESC_JOB_IO_LEN +
+	    ctx->enckeylen <= CAAM_DESC_BYTES_MAX)
+		keys_fit_inline = true;
+
+	desc = ctx->sh_desc_dec;
+
+	init_sh_desc(desc, HDR_SHARE_SERIAL);
+
+	/* Skip key loading if it is loaded due to sharing */
+	key_jump_cmd = append_jump(desc, JUMP_JSL |
+				   JUMP_TEST_ALL | JUMP_COND_SHRD);
+	if (keys_fit_inline)
+		append_key_as_imm(desc, (void *)ctx->key, ctx->enckeylen,
+				  ctx->enckeylen, CLASS_1 | KEY_DEST_CLASS_REG);
+	else
+		append_key(desc, ctx->key_dma, ctx->enckeylen,
+			   CLASS_1 | KEY_DEST_CLASS_REG);
+	set_jump_tgt_here(desc, key_jump_cmd);
+
+	/* Class 1 operation */
+	append_operation(desc, ctx->class1_alg_type |
+			 OP_ALG_AS_INITFINAL | OP_ALG_DECRYPT | OP_ALG_ICV_ON);
+
+	/* Load AES-GMAC ESP IV into Math1 register */
+	append_cmd(desc, CMD_SEQ_LOAD | LDST_SRCDST_WORD_DECO_MATH1 |
+		   LDST_CLASS_DECO | tfm->ivsize);
+
+	/* Wait the DMA transaction to finish */
+	append_jump(desc, JUMP_TEST_ALL | JUMP_COND_CALM |
+		    (1 << JUMP_OFFSET_SHIFT));
+
+	/* assoclen + cryptlen = (seqinlen - ivsize) - icvsize */
+	append_math_sub_imm_u32(desc, REG3, SEQINLEN, IMM, ctx->authsize);
+
+	/* Overwrite blank immediate AES-GMAC ESP IV data */
+	write_iv_cmd = append_move(desc, MOVE_SRC_MATH1 | MOVE_DEST_DESCBUF |
+				   (tfm->ivsize << MOVE_LEN_SHIFT));
+
+	/* Overwrite blank immediate AAD data */
+	write_aad_cmd = append_move(desc, MOVE_SRC_MATH1 | MOVE_DEST_DESCBUF |
+				    (tfm->ivsize << MOVE_LEN_SHIFT));
+
+	/* assoclen = (assoclen + cryptlen) - cryptlen */
+	append_math_sub(desc, REG2, SEQOUTLEN, REG0, CAAM_CMD_SZ);
+	append_math_sub(desc, VARSEQINLEN, REG3, REG2, CAAM_CMD_SZ);
+
+	/*
+	 * MOVE_LEN opcode is not available in all SEC HW revisions,
+	 * thus need to do some magic, i.e. self-patch the descriptor
+	 * buffer.
+	 */
+	read_move_cmd = append_move(desc, MOVE_SRC_DESCBUF | MOVE_DEST_MATH3 |
+				    (0x6 << MOVE_LEN_SHIFT));
+	write_move_cmd = append_move(desc, MOVE_SRC_MATH3 | MOVE_DEST_DESCBUF |
+				     (0x8 << MOVE_LEN_SHIFT));
+
+	/* Read Salt and AES-GMAC ESP IV */
+	append_cmd(desc, CMD_FIFO_LOAD | FIFOLD_CLASS_CLASS1 | IMMEDIATE |
+		   FIFOLD_TYPE_IV | FIFOLD_TYPE_FLUSH1 | (4 + tfm->ivsize));
+	/* Append Salt */
+	append_data(desc, (void *)(ctx->key + ctx->enckeylen), 4);
+	set_move_tgt_here(desc, write_iv_cmd);
+	/* Blank commands. Will be overwritten by AES-GMAC ESP IV. */
+	append_cmd(desc, 0x00000000);
+	append_cmd(desc, 0x00000000);
+	/* End of blank commands */
+
+	/* Read assoc data */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 | FIFOLDST_VLF |
+			     FIFOLD_TYPE_AAD);
+
+	/* Will read cryptlen bytes */
+	append_math_add(desc, VARSEQINLEN, ZERO, REG2, CAAM_CMD_SZ);
+
+	/* Will write cryptlen bytes */
+	append_math_add(desc, VARSEQOUTLEN, ZERO, REG2, CAAM_CMD_SZ);
+
+	/* Authenticate AES-GMAC ESP IV  */
+	append_cmd(desc, CMD_FIFO_LOAD | FIFOLD_CLASS_CLASS1 | IMMEDIATE |
+		   FIFOLD_TYPE_AAD | tfm->ivsize);
+	set_move_tgt_here(desc, write_aad_cmd);
+	/* Blank commands. Will be overwritten by AES-GMAC ESP IV. */
+	append_cmd(desc, 0x00000000);
+	append_cmd(desc, 0x00000000);
+	/* End of blank commands */
+
+	/* Store payload data */
+	append_seq_fifo_store(desc, 0, FIFOST_TYPE_MESSAGE_DATA | FIFOLDST_VLF);
+
+	/* In-snoop cryptlen data */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_BOTH | FIFOLDST_VLF |
+			     FIFOLD_TYPE_AAD | FIFOLD_TYPE_LAST2FLUSH1);
+
+	set_move_tgt_here(desc, read_move_cmd);
+	set_move_tgt_here(desc, write_move_cmd);
+	append_cmd(desc, CMD_LOAD | DISABLE_AUTO_INFO_FIFO);
+	/* Move payload data to OFIFO */
+	append_move(desc, MOVE_SRC_INFIFO_CL | MOVE_DEST_OUTFIFO);
+	append_cmd(desc, CMD_LOAD | ENABLE_AUTO_INFO_FIFO);
+
+	/* Read ICV */
+	append_seq_fifo_load(desc, ctx->authsize, FIFOLD_CLASS_CLASS1 |
+			     FIFOLD_TYPE_ICV | FIFOLD_TYPE_LAST1);
+
+	ctx->sh_desc_dec_dma = dma_map_single(jrdev, desc,
+					      desc_bytes(desc),
+					      DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->sh_desc_dec_dma)) {
+		dev_err(jrdev, "unable to map shared descriptor\n");
+		return -ENOMEM;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "rfc4543 dec shdesc@"__stringify(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+		       desc_bytes(desc), 1);
+#endif
+
+	/*
+	 * Job Descriptor and Shared Descriptors
+	 * must all fit into the 64-word Descriptor h/w Buffer
+	 */
+	keys_fit_inline = false;
+	if (DESC_RFC4543_GIVENC_LEN + DESC_JOB_IO_LEN +
+	    ctx->enckeylen <= CAAM_DESC_BYTES_MAX)
+		keys_fit_inline = true;
+
+	/* rfc4543_givencrypt shared descriptor */
+	desc = ctx->sh_desc_givenc;
+
+	init_sh_desc(desc, HDR_SHARE_SERIAL);
+
+	/* Skip key loading if it is loaded due to sharing */
+	key_jump_cmd = append_jump(desc, JUMP_JSL | JUMP_TEST_ALL |
+				   JUMP_COND_SHRD);
+	if (keys_fit_inline)
+		append_key_as_imm(desc, (void *)ctx->key, ctx->enckeylen,
+				  ctx->enckeylen, CLASS_1 | KEY_DEST_CLASS_REG);
+	else
+		append_key(desc, ctx->key_dma, ctx->enckeylen,
+			   CLASS_1 | KEY_DEST_CLASS_REG);
+	set_jump_tgt_here(desc, key_jump_cmd);
+
+	/* Generate IV */
+	geniv = NFIFOENTRY_STYPE_PAD | NFIFOENTRY_DEST_DECO |
+		NFIFOENTRY_DTYPE_MSG | NFIFOENTRY_LC1 |
+		NFIFOENTRY_PTYPE_RND | (tfm->ivsize << NFIFOENTRY_DLEN_SHIFT);
+	append_load_imm_u32(desc, geniv, LDST_CLASS_IND_CCB |
+			    LDST_SRCDST_WORD_INFO_FIFO | LDST_IMM);
+	append_cmd(desc, CMD_LOAD | DISABLE_AUTO_INFO_FIFO);
+	/* Move generated IV to Math1 register */
+	append_move(desc, MOVE_SRC_INFIFO | MOVE_DEST_MATH1 |
+		    (tfm->ivsize << MOVE_LEN_SHIFT));
+	append_cmd(desc, CMD_LOAD | ENABLE_AUTO_INFO_FIFO);
+
+	/* Overwrite blank immediate AES-GMAC IV data */
+	write_iv_cmd = append_move(desc, MOVE_SRC_MATH1 | MOVE_DEST_DESCBUF |
+				   (tfm->ivsize << MOVE_LEN_SHIFT));
+
+	/* Overwrite blank immediate AAD data */
+	write_aad_cmd = append_move(desc, MOVE_SRC_MATH1 | MOVE_DEST_DESCBUF |
+				    (tfm->ivsize << MOVE_LEN_SHIFT));
+
+	/* Copy generated IV to OFIFO */
+	append_move(desc, MOVE_SRC_MATH1 | MOVE_DEST_OUTFIFO |
+		    (tfm->ivsize << MOVE_LEN_SHIFT));
+
+	/* Class 1 operation */
+	append_operation(desc, ctx->class1_alg_type |
+			 OP_ALG_AS_INITFINAL | OP_ALG_ENCRYPT);
+
+	/* ivsize + cryptlen = seqoutlen - authsize */
+	append_math_sub_imm_u32(desc, REG3, SEQOUTLEN, IMM, ctx->authsize);
+
+	/* assoclen = seqinlen - (ivsize + cryptlen) */
+	append_math_sub(desc, VARSEQINLEN, SEQINLEN, REG3, CAAM_CMD_SZ);
+
+	/* Will write ivsize + cryptlen */
+	append_math_add(desc, VARSEQOUTLEN, REG3, REG0, CAAM_CMD_SZ);
+
+	/*
+	 * MOVE_LEN opcode is not available in all SEC HW revisions,
+	 * thus need to do some magic, i.e. self-patch the descriptor
+	 * buffer.
+	 */
+	read_move_cmd = append_move(desc, MOVE_SRC_DESCBUF | MOVE_DEST_MATH3 |
+				    (0x6 << MOVE_LEN_SHIFT));
+	write_move_cmd = append_move(desc, MOVE_SRC_MATH3 | MOVE_DEST_DESCBUF |
+				     (0x8 << MOVE_LEN_SHIFT));
+
+	/* Read Salt and AES-GMAC generated IV */
+	append_cmd(desc, CMD_FIFO_LOAD | FIFOLD_CLASS_CLASS1 | IMMEDIATE |
+		   FIFOLD_TYPE_IV | FIFOLD_TYPE_FLUSH1 | (4 + tfm->ivsize));
+	/* Append Salt */
+	append_data(desc, (void *)(ctx->key + ctx->enckeylen), 4);
+	set_move_tgt_here(desc, write_iv_cmd);
+	/* Blank commands. Will be overwritten by AES-GMAC generated IV. */
+	append_cmd(desc, 0x00000000);
+	append_cmd(desc, 0x00000000);
+	/* End of blank commands */
+
+	/* No need to reload iv */
+	append_seq_fifo_load(desc, tfm->ivsize, FIFOLD_CLASS_SKIP);
+
+	/* Read assoc data */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 | FIFOLDST_VLF |
+			     FIFOLD_TYPE_AAD);
+
+	/* Will read cryptlen */
+	append_math_add(desc, VARSEQINLEN, SEQINLEN, REG0, CAAM_CMD_SZ);
+
+	/* Authenticate AES-GMAC IV  */
+	append_cmd(desc, CMD_FIFO_LOAD | FIFOLD_CLASS_CLASS1 | IMMEDIATE |
+		   FIFOLD_TYPE_AAD | tfm->ivsize);
+	set_move_tgt_here(desc, write_aad_cmd);
+	/* Blank commands. Will be overwritten by AES-GMAC IV. */
+	append_cmd(desc, 0x00000000);
+	append_cmd(desc, 0x00000000);
+	/* End of blank commands */
+
+	/* Read and write cryptlen bytes */
+	aead_append_src_dst(desc, FIFOLD_TYPE_AAD);
+
+	set_move_tgt_here(desc, read_move_cmd);
+	set_move_tgt_here(desc, write_move_cmd);
+	append_cmd(desc, CMD_LOAD | DISABLE_AUTO_INFO_FIFO);
+	/* Move payload data to OFIFO */
+	append_move(desc, MOVE_SRC_INFIFO_CL | MOVE_DEST_OUTFIFO);
+
+	/* Write ICV */
+	append_seq_store(desc, ctx->authsize, LDST_CLASS_1_CCB |
+			 LDST_SRCDST_BYTE_CONTEXT);
+
+	ctx->sh_desc_givenc_dma = dma_map_single(jrdev, desc,
+						 desc_bytes(desc),
+						 DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->sh_desc_givenc_dma)) {
+		dev_err(jrdev, "unable to map shared descriptor\n");
+		return -ENOMEM;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR,
+		       "rfc4543 givenc shdesc@"__stringify(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+		       desc_bytes(desc), 1);
+#endif
+
+	return 0;
+}
+
+static int rfc4543_setauthsize(struct crypto_aead *authenc,
+			       unsigned int authsize)
+{
+	struct caam_ctx *ctx = crypto_aead_ctx(authenc);
+
+	ctx->authsize = authsize;
+	rfc4543_set_sh_desc(authenc);
+
+	return 0;
+}
+
 static u32 gen_split_aead_key(struct caam_ctx *ctx, const u8 *key_in,
 			      u32 authkeylen)
 {
@@ -703,20 +1715,154 @@ badkey:
 	return -EINVAL;
 }
 
+static int gcm_setkey(struct crypto_aead *aead,
+		      const u8 *key, unsigned int keylen)
+{
+	struct caam_ctx *ctx = crypto_aead_ctx(aead);
+	struct device *jrdev = ctx->jrdev;
+	int ret = 0;
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "key in @"__stringify(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, key, keylen, 1);
+#endif
+
+	memcpy(ctx->key, key, keylen);
+	ctx->key_dma = dma_map_single(jrdev, ctx->key, keylen,
+				      DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->key_dma)) {
+		dev_err(jrdev, "unable to map key i/o memory\n");
+		return -ENOMEM;
+	}
+	ctx->enckeylen = keylen;
+
+	ret = gcm_set_sh_desc(aead);
+	if (ret) {
+		dma_unmap_single(jrdev, ctx->key_dma, ctx->enckeylen,
+				 DMA_TO_DEVICE);
+	}
+
+	return ret;
+}
+
+static int rfc4106_setkey(struct crypto_aead *aead,
+			  const u8 *key, unsigned int keylen)
+{
+	struct caam_ctx *ctx = crypto_aead_ctx(aead);
+	struct device *jrdev = ctx->jrdev;
+	int ret = 0;
+
+	if (keylen < 4)
+		return -EINVAL;
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "key in @"__stringify(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, key, keylen, 1);
+#endif
+
+	memcpy(ctx->key, key, keylen);
+
+	/*
+	 * The last four bytes of the key material are used as the salt value
+	 * in the nonce. Update the AES key length.
+	 */
+	ctx->enckeylen = keylen - 4;
+
+	ctx->key_dma = dma_map_single(jrdev, ctx->key, ctx->enckeylen,
+				      DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->key_dma)) {
+		dev_err(jrdev, "unable to map key i/o memory\n");
+		return -ENOMEM;
+	}
+
+	ret = rfc4106_set_sh_desc(aead);
+	if (ret) {
+		dma_unmap_single(jrdev, ctx->key_dma, ctx->enckeylen,
+				 DMA_TO_DEVICE);
+	}
+
+	return ret;
+}
+
+static int rfc4543_setkey(struct crypto_aead *aead,
+			  const u8 *key, unsigned int keylen)
+{
+	struct caam_ctx *ctx = crypto_aead_ctx(aead);
+	struct device *jrdev = ctx->jrdev;
+	int ret = 0;
+
+	if (keylen < 4)
+		return -EINVAL;
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "key in @"__stringify(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, key, keylen, 1);
+#endif
+
+	memcpy(ctx->key, key, keylen);
+
+	/*
+	 * The last four bytes of the key material are used as the salt value
+	 * in the nonce. Update the AES key length.
+	 */
+	ctx->enckeylen = keylen - 4;
+
+	ctx->key_dma = dma_map_single(jrdev, ctx->key, ctx->enckeylen,
+				      DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->key_dma)) {
+		dev_err(jrdev, "unable to map key i/o memory\n");
+		return -ENOMEM;
+	}
+
+	ret = rfc4543_set_sh_desc(aead);
+	if (ret) {
+		dma_unmap_single(jrdev, ctx->key_dma, ctx->enckeylen,
+				 DMA_TO_DEVICE);
+	}
+
+	return ret;
+}
+
 static int ablkcipher_setkey(struct crypto_ablkcipher *ablkcipher,
 			     const u8 *key, unsigned int keylen)
 {
 	struct caam_ctx *ctx = crypto_ablkcipher_ctx(ablkcipher);
-	struct ablkcipher_tfm *tfm = &ablkcipher->base.crt_ablkcipher;
+	struct ablkcipher_tfm *crt = &ablkcipher->base.crt_ablkcipher;
+	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(ablkcipher);
+	const char *alg_name = crypto_tfm_alg_name(tfm);
 	struct device *jrdev = ctx->jrdev;
 	int ret = 0;
 	u32 *key_jump_cmd;
 	u32 *desc;
+	u32 *nonce;
+	u32 geniv;
+	u32 ctx1_iv_off = 0;
+	const bool ctr_mode = ((ctx->class1_alg_type & OP_ALG_AAI_MASK) ==
+			       OP_ALG_AAI_CTR_MOD128);
+	const bool is_rfc3686 = (ctr_mode &&
+				 (strstr(alg_name, "rfc3686") != NULL));
 
 #ifdef DEBUG
 	print_hex_dump(KERN_ERR, "key in @"__stringify(__LINE__)": ",
 		       DUMP_PREFIX_ADDRESS, 16, 4, key, keylen, 1);
 #endif
+	/*
+	 * AES-CTR needs to load IV in CONTEXT1 reg
+	 * at an offset of 128bits (16bytes)
+	 * CONTEXT1[255:128] = IV
+	 */
+	if (ctr_mode)
+		ctx1_iv_off = 16;
+
+	/*
+	 * RFC3686 specific:
+	 *	| CONTEXT1[255:128] = {NONCE, IV, COUNTER}
+	 *	| *key = {KEY, NONCE}
+	 */
+	if (is_rfc3686) {
+		ctx1_iv_off = 16 + CTR_RFC3686_NONCE_SIZE;
+		keylen -= CTR_RFC3686_NONCE_SIZE;
+	}
 
 	memcpy(ctx->key, key, keylen);
 	ctx->key_dma = dma_map_single(jrdev, ctx->key, keylen,
@@ -729,7 +1875,7 @@ static int ablkcipher_setkey(struct crypto_ablkcipher *ablkcipher,
 
 	/* ablkcipher_encrypt shared descriptor */
 	desc = ctx->sh_desc_enc;
-	init_sh_desc(desc, HDR_SHARE_SERIAL);
+	init_sh_desc(desc, HDR_SHARE_SERIAL | HDR_SAVECTX);
 	/* Skip if already shared */
 	key_jump_cmd = append_jump(desc, JUMP_JSL | JUMP_TEST_ALL |
 				   JUMP_COND_SHRD);
@@ -739,11 +1885,31 @@ static int ablkcipher_setkey(struct crypto_ablkcipher *ablkcipher,
 			  ctx->enckeylen, CLASS_1 |
 			  KEY_DEST_CLASS_REG);
 
+	/* Load nonce into CONTEXT1 reg */
+	if (is_rfc3686) {
+		nonce = (u32 *)(key + keylen);
+		append_load_imm_u32(desc, *nonce, LDST_CLASS_IND_CCB |
+				    LDST_SRCDST_BYTE_OUTFIFO | LDST_IMM);
+		append_move(desc, MOVE_WAITCOMP |
+			    MOVE_SRC_OUTFIFO |
+			    MOVE_DEST_CLASS1CTX |
+			    (16 << MOVE_OFFSET_SHIFT) |
+			    (CTR_RFC3686_NONCE_SIZE << MOVE_LEN_SHIFT));
+	}
+
 	set_jump_tgt_here(desc, key_jump_cmd);
 
 	/* Load iv */
-	append_cmd(desc, CMD_SEQ_LOAD | LDST_SRCDST_BYTE_CONTEXT |
-		   LDST_CLASS_1_CCB | tfm->ivsize);
+	append_seq_load(desc, crt->ivsize, LDST_SRCDST_BYTE_CONTEXT |
+			LDST_CLASS_1_CCB | (ctx1_iv_off << LDST_OFFSET_SHIFT));
+
+	/* Load counter into CONTEXT1 reg */
+	if (is_rfc3686)
+		append_load_imm_u32(desc, be32_to_cpu(1), LDST_IMM |
+				    LDST_CLASS_1_CCB |
+				    LDST_SRCDST_BYTE_CONTEXT |
+				    ((ctx1_iv_off + CTR_RFC3686_IV_SIZE) <<
+				     LDST_OFFSET_SHIFT));
 
 	/* Load operation */
 	append_operation(desc, ctx->class1_alg_type |
@@ -768,7 +1934,7 @@ static int ablkcipher_setkey(struct crypto_ablkcipher *ablkcipher,
 	/* ablkcipher_decrypt shared descriptor */
 	desc = ctx->sh_desc_dec;
 
-	init_sh_desc(desc, HDR_SHARE_SERIAL);
+	init_sh_desc(desc, HDR_SHARE_SERIAL | HDR_SAVECTX);
 	/* Skip if already shared */
 	key_jump_cmd = append_jump(desc, JUMP_JSL | JUMP_TEST_ALL |
 				   JUMP_COND_SHRD);
@@ -778,14 +1944,38 @@ static int ablkcipher_setkey(struct crypto_ablkcipher *ablkcipher,
 			  ctx->enckeylen, CLASS_1 |
 			  KEY_DEST_CLASS_REG);
 
+	/* Load nonce into CONTEXT1 reg */
+	if (is_rfc3686) {
+		nonce = (u32 *)(key + keylen);
+		append_load_imm_u32(desc, *nonce, LDST_CLASS_IND_CCB |
+				    LDST_SRCDST_BYTE_OUTFIFO | LDST_IMM);
+		append_move(desc, MOVE_WAITCOMP |
+			    MOVE_SRC_OUTFIFO |
+			    MOVE_DEST_CLASS1CTX |
+			    (16 << MOVE_OFFSET_SHIFT) |
+			    (CTR_RFC3686_NONCE_SIZE << MOVE_LEN_SHIFT));
+	}
+
 	set_jump_tgt_here(desc, key_jump_cmd);
 
 	/* load IV */
-	append_cmd(desc, CMD_SEQ_LOAD | LDST_SRCDST_BYTE_CONTEXT |
-		   LDST_CLASS_1_CCB | tfm->ivsize);
+	append_seq_load(desc, crt->ivsize, LDST_SRCDST_BYTE_CONTEXT |
+			LDST_CLASS_1_CCB | (ctx1_iv_off << LDST_OFFSET_SHIFT));
+
+	/* Load counter into CONTEXT1 reg */
+	if (is_rfc3686)
+		append_load_imm_u32(desc, be32_to_cpu(1), LDST_IMM |
+				    LDST_CLASS_1_CCB |
+				    LDST_SRCDST_BYTE_CONTEXT |
+				    ((ctx1_iv_off + CTR_RFC3686_IV_SIZE) <<
+				     LDST_OFFSET_SHIFT));
 
 	/* Choose operation */
-	append_dec_op1(desc, ctx->class1_alg_type);
+	if (ctr_mode)
+		append_operation(desc, ctx->class1_alg_type |
+				 OP_ALG_AS_INITFINAL | OP_ALG_DECRYPT);
+	else
+		append_dec_op1(desc, ctx->class1_alg_type);
 
 	/* Perform operation */
 	ablkcipher_append_src_dst(desc);
@@ -804,6 +1994,83 @@ static int ablkcipher_setkey(struct crypto_ablkcipher *ablkcipher,
 		       DUMP_PREFIX_ADDRESS, 16, 4, desc,
 		       desc_bytes(desc), 1);
 #endif
+	/* ablkcipher_givencrypt shared descriptor */
+	desc = ctx->sh_desc_givenc;
+
+	init_sh_desc(desc, HDR_SHARE_SERIAL | HDR_SAVECTX);
+	/* Skip if already shared */
+	key_jump_cmd = append_jump(desc, JUMP_JSL | JUMP_TEST_ALL |
+				   JUMP_COND_SHRD);
+
+	/* Load class1 key only */
+	append_key_as_imm(desc, (void *)ctx->key, ctx->enckeylen,
+			  ctx->enckeylen, CLASS_1 |
+			  KEY_DEST_CLASS_REG);
+
+	/* Load Nonce into CONTEXT1 reg */
+	if (is_rfc3686) {
+		nonce = (u32 *)(key + keylen);
+		append_load_imm_u32(desc, *nonce, LDST_CLASS_IND_CCB |
+				    LDST_SRCDST_BYTE_OUTFIFO | LDST_IMM);
+		append_move(desc, MOVE_WAITCOMP |
+			    MOVE_SRC_OUTFIFO |
+			    MOVE_DEST_CLASS1CTX |
+			    (16 << MOVE_OFFSET_SHIFT) |
+			    (CTR_RFC3686_NONCE_SIZE << MOVE_LEN_SHIFT));
+	}
+	set_jump_tgt_here(desc, key_jump_cmd);
+
+	/* Generate IV */
+	geniv = NFIFOENTRY_STYPE_PAD | NFIFOENTRY_DEST_DECO |
+		NFIFOENTRY_DTYPE_MSG | NFIFOENTRY_LC1 |
+		NFIFOENTRY_PTYPE_RND | (crt->ivsize << NFIFOENTRY_DLEN_SHIFT);
+	append_load_imm_u32(desc, geniv, LDST_CLASS_IND_CCB |
+			    LDST_SRCDST_WORD_INFO_FIFO | LDST_IMM);
+	append_cmd(desc, CMD_LOAD | DISABLE_AUTO_INFO_FIFO);
+	append_move(desc, MOVE_WAITCOMP |
+		    MOVE_SRC_INFIFO |
+		    MOVE_DEST_CLASS1CTX |
+		    (crt->ivsize << MOVE_LEN_SHIFT) |
+		    (ctx1_iv_off << MOVE_OFFSET_SHIFT));
+	append_cmd(desc, CMD_LOAD | ENABLE_AUTO_INFO_FIFO);
+
+	/* Copy generated IV to memory */
+	append_seq_store(desc, crt->ivsize,
+			 LDST_SRCDST_BYTE_CONTEXT | LDST_CLASS_1_CCB |
+			 (ctx1_iv_off << LDST_OFFSET_SHIFT));
+
+	/* Load Counter into CONTEXT1 reg */
+	if (is_rfc3686)
+		append_load_imm_u32(desc, (u32)1, LDST_IMM |
+				    LDST_CLASS_1_CCB |
+				    LDST_SRCDST_BYTE_CONTEXT |
+				    ((ctx1_iv_off + CTR_RFC3686_IV_SIZE) <<
+				     LDST_OFFSET_SHIFT));
+
+	if (ctx1_iv_off)
+		append_jump(desc, JUMP_JSL | JUMP_TEST_ALL | JUMP_COND_NCP |
+			    (1 << JUMP_OFFSET_SHIFT));
+
+	/* Load operation */
+	append_operation(desc, ctx->class1_alg_type |
+			 OP_ALG_AS_INITFINAL | OP_ALG_ENCRYPT);
+
+	/* Perform operation */
+	ablkcipher_append_src_dst(desc);
+
+	ctx->sh_desc_givenc_dma = dma_map_single(jrdev, desc,
+						 desc_bytes(desc),
+						 DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->sh_desc_givenc_dma)) {
+		dev_err(jrdev, "unable to map shared descriptor\n");
+		return -ENOMEM;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR,
+		       "ablkcipher givenc shdesc@" __stringify(__LINE__) ": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+		       desc_bytes(desc), 1);
+#endif
 
 	return ret;
 }
@@ -1088,6 +2355,7 @@ static void init_aead_job(u32 *sh_desc, dma_addr_t ptr,
 	u32 out_options = 0, in_options;
 	dma_addr_t dst_dma, src_dma;
 	int len, sec4_sg_index = 0;
+	bool is_gcm = false;
 
 #ifdef DEBUG
 	debug("assoclen %d cryptlen %d authsize %d\n",
@@ -1106,11 +2374,19 @@ static void init_aead_job(u32 *sh_desc, dma_addr_t ptr,
 		       desc_bytes(sh_desc), 1);
 #endif
 
+	if (((ctx->class1_alg_type & OP_ALG_ALGSEL_MASK) ==
+	      OP_ALG_ALGSEL_AES) &&
+	    ((ctx->class1_alg_type & OP_ALG_AAI_MASK) == OP_ALG_AAI_GCM))
+		is_gcm = true;
+
 	len = desc_len(sh_desc);
 	init_job_desc_shared(desc, ptr, len, HDR_SHARE_DEFER | HDR_REVERSE);
 
 	if (all_contig) {
-		src_dma = sg_dma_address(req->assoc);
+		if (is_gcm)
+			src_dma = edesc->iv_dma;
+		else
+			src_dma = sg_dma_address(req->assoc);
 		in_options = 0;
 	} else {
 		src_dma = edesc->sec4_sg_dma;
@@ -1164,6 +2440,7 @@ static void init_aead_giv_job(u32 *sh_desc, dma_addr_t ptr,
 	u32 out_options = 0, in_options;
 	dma_addr_t dst_dma, src_dma;
 	int len, sec4_sg_index = 0;
+	bool is_gcm = false;
 
 #ifdef DEBUG
 	debug("assoclen %d cryptlen %d authsize %d\n",
@@ -1181,11 +2458,19 @@ static void init_aead_giv_job(u32 *sh_desc, dma_addr_t ptr,
 		       desc_bytes(sh_desc), 1);
 #endif
 
+	if (((ctx->class1_alg_type & OP_ALG_ALGSEL_MASK) ==
+	      OP_ALG_ALGSEL_AES) &&
+	    ((ctx->class1_alg_type & OP_ALG_AAI_MASK) == OP_ALG_AAI_GCM))
+		is_gcm = true;
+
 	len = desc_len(sh_desc);
 	init_job_desc_shared(desc, ptr, len, HDR_SHARE_DEFER | HDR_REVERSE);
 
 	if (contig & GIV_SRC_CONTIG) {
-		src_dma = sg_dma_address(req->assoc);
+		if (is_gcm)
+			src_dma = edesc->iv_dma;
+		else
+			src_dma = sg_dma_address(req->assoc);
 		in_options = 0;
 	} else {
 		src_dma = edesc->sec4_sg_dma;
@@ -1200,7 +2485,8 @@ static void init_aead_giv_job(u32 *sh_desc, dma_addr_t ptr,
 	} else {
 		if (likely(req->src == req->dst)) {
 			dst_dma = src_dma + sizeof(struct sec4_sg_entry) *
-				  edesc->assoc_nents;
+				  (edesc->assoc_nents +
+				   (is_gcm ? 1 + edesc->src_nents : 0));
 			out_options = LDST_SGF;
 		} else {
 			dst_dma = edesc->sec4_sg_dma +
@@ -1272,6 +2558,54 @@ static void init_ablkcipher_job(u32 *sh_desc, dma_addr_t ptr,
 }
 
 /*
+ * Fill in ablkcipher givencrypt job descriptor
+ */
+static void init_ablkcipher_giv_job(u32 *sh_desc, dma_addr_t ptr,
+				    struct ablkcipher_edesc *edesc,
+				    struct ablkcipher_request *req,
+				    bool iv_contig)
+{
+	struct crypto_ablkcipher *ablkcipher = crypto_ablkcipher_reqtfm(req);
+	int ivsize = crypto_ablkcipher_ivsize(ablkcipher);
+	u32 *desc = edesc->hw_desc;
+	u32 out_options, in_options;
+	dma_addr_t dst_dma, src_dma;
+	int len, sec4_sg_index = 0;
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "presciv@" __stringify(__LINE__) ": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, req->info,
+		       ivsize, 1);
+	print_hex_dump(KERN_ERR, "src    @" __stringify(__LINE__) ": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(req->src),
+		       edesc->src_nents ? 100 : req->nbytes, 1);
+#endif
+
+	len = desc_len(sh_desc);
+	init_job_desc_shared(desc, ptr, len, HDR_SHARE_DEFER | HDR_REVERSE);
+
+	if (!edesc->src_nents) {
+		src_dma = sg_dma_address(req->src);
+		in_options = 0;
+	} else {
+		src_dma = edesc->sec4_sg_dma;
+		sec4_sg_index += edesc->src_nents;
+		in_options = LDST_SGF;
+	}
+	append_seq_in_ptr(desc, src_dma, req->nbytes, in_options);
+
+	if (iv_contig) {
+		dst_dma = edesc->iv_dma;
+		out_options = 0;
+	} else {
+		dst_dma = edesc->sec4_sg_dma +
+			  sec4_sg_index * sizeof(struct sec4_sg_entry);
+		out_options = LDST_SGF;
+	}
+	append_seq_out_ptr(desc, dst_dma, req->nbytes + ivsize, out_options);
+}
+
+/*
  * allocate and map the aead extended descriptor
  */
 static struct aead_edesc *aead_edesc_alloc(struct aead_request *req,
@@ -1292,6 +2626,7 @@ static struct aead_edesc *aead_edesc_alloc(struct aead_request *req,
 	int ivsize = crypto_aead_ivsize(aead);
 	int sec4_sg_index, sec4_sg_len = 0, sec4_sg_bytes;
 	unsigned int authsize = ctx->authsize;
+	bool is_gcm = false;
 
 	assoc_nents = sg_count(req->assoc, req->assoclen, &assoc_chained);
 
@@ -1326,15 +2661,31 @@ static struct aead_edesc *aead_edesc_alloc(struct aead_request *req,
 		return ERR_PTR(-ENOMEM);
 	}
 
-	/* Check if data are contiguous */
-	if (assoc_nents || sg_dma_address(req->assoc) + req->assoclen !=
-	    iv_dma || src_nents || iv_dma + ivsize !=
-	    sg_dma_address(req->src)) {
-		all_contig = false;
+	if (((ctx->class1_alg_type & OP_ALG_ALGSEL_MASK) ==
+	      OP_ALG_ALGSEL_AES) &&
+	    ((ctx->class1_alg_type & OP_ALG_AAI_MASK) == OP_ALG_AAI_GCM))
+		is_gcm = true;
+
+	/*
+	 * Check if data are contiguous.
+	 * GCM expected input sequence: IV, AAD, text
+	 * All other - expected input sequence: AAD, IV, text
+	 */
+	if (is_gcm)
+		all_contig = (!assoc_nents &&
+			      iv_dma + ivsize == sg_dma_address(req->assoc) &&
+			      !src_nents && sg_dma_address(req->assoc) +
+			      req->assoclen == sg_dma_address(req->src));
+	else
+		all_contig = (!assoc_nents && sg_dma_address(req->assoc) +
+			      req->assoclen == iv_dma && !src_nents &&
+			      iv_dma + ivsize == sg_dma_address(req->src));
+	if (!all_contig) {
 		assoc_nents = assoc_nents ? : 1;
 		src_nents = src_nents ? : 1;
 		sec4_sg_len = assoc_nents + 1 + src_nents;
 	}
+
 	sec4_sg_len += dst_nents;
 
 	sec4_sg_bytes = sec4_sg_len * sizeof(struct sec4_sg_entry);
@@ -1361,14 +2712,26 @@ static struct aead_edesc *aead_edesc_alloc(struct aead_request *req,
 
 	sec4_sg_index = 0;
 	if (!all_contig) {
-		sg_to_sec4_sg(req->assoc,
-			      (assoc_nents ? : 1),
-			      edesc->sec4_sg +
-			      sec4_sg_index, 0);
-		sec4_sg_index += assoc_nents ? : 1;
+		if (!is_gcm) {
+			sg_to_sec4_sg(req->assoc,
+				      (assoc_nents ? : 1),
+				      edesc->sec4_sg +
+				      sec4_sg_index, 0);
+			sec4_sg_index += assoc_nents ? : 1;
+		}
+
 		dma_to_sec4_sg_one(edesc->sec4_sg + sec4_sg_index,
 				   iv_dma, ivsize, 0);
 		sec4_sg_index += 1;
+
+		if (is_gcm) {
+			sg_to_sec4_sg(req->assoc,
+				      (assoc_nents ? : 1),
+				      edesc->sec4_sg +
+				      sec4_sg_index, 0);
+			sec4_sg_index += assoc_nents ? : 1;
+		}
+
 		sg_to_sec4_sg_last(req->src,
 				   (src_nents ? : 1),
 				   edesc->sec4_sg +
@@ -1490,6 +2853,7 @@ static struct aead_edesc *aead_giv_edesc_alloc(struct aead_givcrypt_request
 	int ivsize = crypto_aead_ivsize(aead);
 	bool assoc_chained = false, src_chained = false, dst_chained = false;
 	int sec4_sg_index, sec4_sg_len = 0, sec4_sg_bytes;
+	bool is_gcm = false;
 
 	assoc_nents = sg_count(req->assoc, req->assoclen, &assoc_chained);
 	src_nents = sg_count(req->src, req->cryptlen, &src_chained);
@@ -1516,24 +2880,53 @@ static struct aead_edesc *aead_giv_edesc_alloc(struct aead_givcrypt_request
 		return ERR_PTR(-ENOMEM);
 	}
 
-	/* Check if data are contiguous */
-	if (assoc_nents || sg_dma_address(req->assoc) + req->assoclen !=
-	    iv_dma || src_nents || iv_dma + ivsize != sg_dma_address(req->src))
-		contig &= ~GIV_SRC_CONTIG;
+	if (((ctx->class1_alg_type & OP_ALG_ALGSEL_MASK) ==
+	      OP_ALG_ALGSEL_AES) &&
+	    ((ctx->class1_alg_type & OP_ALG_AAI_MASK) == OP_ALG_AAI_GCM))
+		is_gcm = true;
+
+	/*
+	 * Check if data are contiguous.
+	 * GCM expected input sequence: IV, AAD, text
+	 * All other - expected input sequence: AAD, IV, text
+	 */
+
+	if (is_gcm) {
+		if (assoc_nents || iv_dma + ivsize !=
+		    sg_dma_address(req->assoc) || src_nents ||
+		    sg_dma_address(req->assoc) + req->assoclen !=
+		    sg_dma_address(req->src))
+			contig &= ~GIV_SRC_CONTIG;
+	} else {
+		if (assoc_nents ||
+		    sg_dma_address(req->assoc) + req->assoclen != iv_dma ||
+		    src_nents || iv_dma + ivsize != sg_dma_address(req->src))
+			contig &= ~GIV_SRC_CONTIG;
+	}
+
 	if (dst_nents || iv_dma + ivsize != sg_dma_address(req->dst))
 		contig &= ~GIV_DST_CONTIG;
-	if (unlikely(req->src != req->dst)) {
-		dst_nents = dst_nents ? : 1;
-		sec4_sg_len += 1;
-	}
+
 	if (!(contig & GIV_SRC_CONTIG)) {
 		assoc_nents = assoc_nents ? : 1;
 		src_nents = src_nents ? : 1;
 		sec4_sg_len += assoc_nents + 1 + src_nents;
-		if (likely(req->src == req->dst))
+		if (req->src == req->dst &&
+		    (src_nents || iv_dma + ivsize != sg_dma_address(req->src)))
 			contig &= ~GIV_DST_CONTIG;
 	}
-	sec4_sg_len += dst_nents;
+
+	/*
+	 * Add new sg entries for GCM output sequence.
+	 * Expected output sequence: IV, encrypted text.
+	 */
+	if (is_gcm && req->src == req->dst && !(contig & GIV_DST_CONTIG))
+		sec4_sg_len += 1 + src_nents;
+
+	if (unlikely(req->src != req->dst)) {
+		dst_nents = dst_nents ? : 1;
+		sec4_sg_len += 1 + dst_nents;
+	}
 
 	sec4_sg_bytes = sec4_sg_len * sizeof(struct sec4_sg_entry);
 
@@ -1559,18 +2952,36 @@ static struct aead_edesc *aead_giv_edesc_alloc(struct aead_givcrypt_request
 
 	sec4_sg_index = 0;
 	if (!(contig & GIV_SRC_CONTIG)) {
-		sg_to_sec4_sg(req->assoc, assoc_nents,
-			      edesc->sec4_sg +
-			      sec4_sg_index, 0);
-		sec4_sg_index += assoc_nents;
+		if (!is_gcm) {
+			sg_to_sec4_sg(req->assoc, assoc_nents,
+				      edesc->sec4_sg + sec4_sg_index, 0);
+			sec4_sg_index += assoc_nents;
+		}
+
 		dma_to_sec4_sg_one(edesc->sec4_sg + sec4_sg_index,
 				   iv_dma, ivsize, 0);
 		sec4_sg_index += 1;
+
+		if (is_gcm) {
+			sg_to_sec4_sg(req->assoc, assoc_nents,
+				      edesc->sec4_sg + sec4_sg_index, 0);
+			sec4_sg_index += assoc_nents;
+		}
+
 		sg_to_sec4_sg_last(req->src, src_nents,
 				   edesc->sec4_sg +
 				   sec4_sg_index, 0);
 		sec4_sg_index += src_nents;
 	}
+
+	if (is_gcm && req->src == req->dst && !(contig & GIV_DST_CONTIG)) {
+		dma_to_sec4_sg_one(edesc->sec4_sg + sec4_sg_index,
+				   iv_dma, ivsize, 0);
+		sec4_sg_index += 1;
+		sg_to_sec4_sg_last(req->src, src_nents,
+				   edesc->sec4_sg + sec4_sg_index, 0);
+	}
+
 	if (unlikely(req->src != req->dst && !(contig & GIV_DST_CONTIG))) {
 		dma_to_sec4_sg_one(edesc->sec4_sg + sec4_sg_index,
 				   iv_dma, ivsize, 0);
@@ -1814,6 +3225,151 @@ static int ablkcipher_decrypt(struct ablkcipher_request *req)
 	return ret;
 }
 
+/*
+ * allocate and map the ablkcipher extended descriptor
+ * for ablkcipher givencrypt
+ */
+static struct ablkcipher_edesc *ablkcipher_giv_edesc_alloc(
+				struct skcipher_givcrypt_request *greq,
+				int desc_bytes,
+				bool *iv_contig_out)
+{
+	struct ablkcipher_request *req = &greq->creq;
+	struct crypto_ablkcipher *ablkcipher = crypto_ablkcipher_reqtfm(req);
+	struct caam_ctx *ctx = crypto_ablkcipher_ctx(ablkcipher);
+	struct device *jrdev = ctx->jrdev;
+	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
+					  CRYPTO_TFM_REQ_MAY_SLEEP)) ?
+		       GFP_KERNEL : GFP_ATOMIC;
+	int src_nents, dst_nents = 0, sec4_sg_bytes;
+	struct ablkcipher_edesc *edesc;
+	dma_addr_t iv_dma = 0;
+	bool iv_contig = false;
+	int sgc;
+	int ivsize = crypto_ablkcipher_ivsize(ablkcipher);
+	bool src_chained = false, dst_chained = false;
+	int sec4_sg_index;
+
+	src_nents = sg_count(req->src, req->nbytes, &src_chained);
+
+	if (unlikely(req->dst != req->src))
+		dst_nents = sg_count(req->dst, req->nbytes, &dst_chained);
+
+	if (likely(req->src == req->dst)) {
+		sgc = dma_map_sg_chained(jrdev, req->src, src_nents ? : 1,
+					 DMA_BIDIRECTIONAL, src_chained);
+	} else {
+		sgc = dma_map_sg_chained(jrdev, req->src, src_nents ? : 1,
+					 DMA_TO_DEVICE, src_chained);
+		sgc = dma_map_sg_chained(jrdev, req->dst, dst_nents ? : 1,
+					 DMA_FROM_DEVICE, dst_chained);
+	}
+
+	/*
+	 * Check if iv can be contiguous with source and destination.
+	 * If so, include it. If not, create scatterlist.
+	 */
+	iv_dma = dma_map_single(jrdev, greq->giv, ivsize, DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, iv_dma)) {
+		dev_err(jrdev, "unable to map IV\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	if (!dst_nents && iv_dma + ivsize == sg_dma_address(req->dst))
+		iv_contig = true;
+	else
+		dst_nents = dst_nents ? : 1;
+	sec4_sg_bytes = ((iv_contig ? 0 : 1) + src_nents + dst_nents) *
+			sizeof(struct sec4_sg_entry);
+
+	/* allocate space for base edesc and hw desc commands, link tables */
+	edesc = kmalloc(sizeof(*edesc) + desc_bytes +
+			sec4_sg_bytes, GFP_DMA | flags);
+	if (!edesc) {
+		dev_err(jrdev, "could not allocate extended descriptor\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	edesc->src_nents = src_nents;
+	edesc->src_chained = src_chained;
+	edesc->dst_nents = dst_nents;
+	edesc->dst_chained = dst_chained;
+	edesc->sec4_sg_bytes = sec4_sg_bytes;
+	edesc->sec4_sg = (void *)edesc + sizeof(struct ablkcipher_edesc) +
+			 desc_bytes;
+
+	sec4_sg_index = 0;
+	if (src_nents) {
+		sg_to_sec4_sg_last(req->src, src_nents, edesc->sec4_sg, 0);
+		sec4_sg_index += src_nents;
+	}
+
+	if (!iv_contig) {
+		dma_to_sec4_sg_one(edesc->sec4_sg + sec4_sg_index,
+				   iv_dma, ivsize, 0);
+		sec4_sg_index += 1;
+		sg_to_sec4_sg_last(req->dst, dst_nents,
+				   edesc->sec4_sg + sec4_sg_index, 0);
+	}
+
+	edesc->sec4_sg_dma = dma_map_single(jrdev, edesc->sec4_sg,
+					    sec4_sg_bytes, DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, edesc->sec4_sg_dma)) {
+		dev_err(jrdev, "unable to map S/G table\n");
+		return ERR_PTR(-ENOMEM);
+	}
+	edesc->iv_dma = iv_dma;
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR,
+		       "ablkcipher sec4_sg@" __stringify(__LINE__) ": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, edesc->sec4_sg,
+		       sec4_sg_bytes, 1);
+#endif
+
+	*iv_contig_out = iv_contig;
+	return edesc;
+}
+
+static int ablkcipher_givencrypt(struct skcipher_givcrypt_request *creq)
+{
+	struct ablkcipher_request *req = &creq->creq;
+	struct ablkcipher_edesc *edesc;
+	struct crypto_ablkcipher *ablkcipher = crypto_ablkcipher_reqtfm(req);
+	struct caam_ctx *ctx = crypto_ablkcipher_ctx(ablkcipher);
+	struct device *jrdev = ctx->jrdev;
+	bool iv_contig;
+	u32 *desc;
+	int ret = 0;
+
+	/* allocate extended descriptor */
+	edesc = ablkcipher_giv_edesc_alloc(creq, DESC_JOB_IO_LEN *
+				       CAAM_CMD_SZ, &iv_contig);
+	if (IS_ERR(edesc))
+		return PTR_ERR(edesc);
+
+	/* Create and submit job descriptor*/
+	init_ablkcipher_giv_job(ctx->sh_desc_givenc, ctx->sh_desc_givenc_dma,
+				edesc, req, iv_contig);
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR,
+		       "ablkcipher jobdesc@" __stringify(__LINE__) ": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, edesc->hw_desc,
+		       desc_bytes(edesc->hw_desc), 1);
+#endif
+	desc = edesc->hw_desc;
+	ret = caam_jr_enqueue(jrdev, desc, ablkcipher_encrypt_done, req);
+
+	if (!ret) {
+		ret = -EINPROGRESS;
+	} else {
+		ablkcipher_unmap(jrdev, edesc, req);
+		kfree(edesc);
+	}
+
+	return ret;
+}
+
 #define template_aead		template_u.aead
 #define template_ablkcipher	template_u.ablkcipher
 struct caam_alg_template {
@@ -2309,17 +3865,188 @@ static struct caam_alg_template driver_algs[] = {
 				   OP_ALG_AAI_HMAC_PRECOMP,
 		.alg_op = OP_ALG_ALGSEL_SHA512 | OP_ALG_AAI_HMAC,
 	},
+	{
+		.name = "authenc(hmac(md5),rfc3686(ctr(aes)))",
+		.driver_name = "authenc-hmac-md5-rfc3686-ctr-aes-caam",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = aead_setkey,
+			.setauthsize = aead_setauthsize,
+			.encrypt = aead_encrypt,
+			.decrypt = aead_decrypt,
+			.givencrypt = aead_givencrypt,
+			.geniv = "<built-in>",
+			.ivsize = CTR_RFC3686_IV_SIZE,
+			.maxauthsize = MD5_DIGEST_SIZE,
+			},
+		.class1_alg_type = OP_ALG_ALGSEL_AES | OP_ALG_AAI_CTR_MOD128,
+		.class2_alg_type = OP_ALG_ALGSEL_MD5 | OP_ALG_AAI_HMAC_PRECOMP,
+		.alg_op = OP_ALG_ALGSEL_MD5 | OP_ALG_AAI_HMAC,
+	},
+	{
+		.name = "authenc(hmac(sha1),rfc3686(ctr(aes)))",
+		.driver_name = "authenc-hmac-sha1-rfc3686-ctr-aes-caam",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = aead_setkey,
+			.setauthsize = aead_setauthsize,
+			.encrypt = aead_encrypt,
+			.decrypt = aead_decrypt,
+			.givencrypt = aead_givencrypt,
+			.geniv = "<built-in>",
+			.ivsize = CTR_RFC3686_IV_SIZE,
+			.maxauthsize = SHA1_DIGEST_SIZE,
+			},
+		.class1_alg_type = OP_ALG_ALGSEL_AES | OP_ALG_AAI_CTR_MOD128,
+		.class2_alg_type = OP_ALG_ALGSEL_SHA1 | OP_ALG_AAI_HMAC_PRECOMP,
+		.alg_op = OP_ALG_ALGSEL_SHA1 | OP_ALG_AAI_HMAC,
+	},
+	{
+		.name = "authenc(hmac(sha224),rfc3686(ctr(aes)))",
+		.driver_name = "authenc-hmac-sha224-rfc3686-ctr-aes-caam",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = aead_setkey,
+			.setauthsize = aead_setauthsize,
+			.encrypt = aead_encrypt,
+			.decrypt = aead_decrypt,
+			.givencrypt = aead_givencrypt,
+			.geniv = "<built-in>",
+			.ivsize = CTR_RFC3686_IV_SIZE,
+			.maxauthsize = SHA224_DIGEST_SIZE,
+			},
+		.class1_alg_type = OP_ALG_ALGSEL_AES | OP_ALG_AAI_CTR_MOD128,
+		.class2_alg_type = OP_ALG_ALGSEL_SHA224 |
+				   OP_ALG_AAI_HMAC_PRECOMP,
+		.alg_op = OP_ALG_ALGSEL_SHA224 | OP_ALG_AAI_HMAC,
+	},
+	{
+		.name = "authenc(hmac(sha256),rfc3686(ctr(aes)))",
+		.driver_name = "authenc-hmac-sha256-rfc3686-ctr-aes-caam",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = aead_setkey,
+			.setauthsize = aead_setauthsize,
+			.encrypt = aead_encrypt,
+			.decrypt = aead_decrypt,
+			.givencrypt = aead_givencrypt,
+			.geniv = "<built-in>",
+			.ivsize = CTR_RFC3686_IV_SIZE,
+			.maxauthsize = SHA256_DIGEST_SIZE,
+			},
+		.class1_alg_type = OP_ALG_ALGSEL_AES | OP_ALG_AAI_CTR_MOD128,
+		.class2_alg_type = OP_ALG_ALGSEL_SHA256 |
+				   OP_ALG_AAI_HMAC_PRECOMP,
+		.alg_op = OP_ALG_ALGSEL_SHA256 | OP_ALG_AAI_HMAC,
+	},
+	{
+		.name = "authenc(hmac(sha384),rfc3686(ctr(aes)))",
+		.driver_name = "authenc-hmac-sha384-rfc3686-ctr-aes-caam",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = aead_setkey,
+			.setauthsize = aead_setauthsize,
+			.encrypt = aead_encrypt,
+			.decrypt = aead_decrypt,
+			.givencrypt = aead_givencrypt,
+			.geniv = "<built-in>",
+			.ivsize = CTR_RFC3686_IV_SIZE,
+			.maxauthsize = SHA384_DIGEST_SIZE,
+			},
+		.class1_alg_type = OP_ALG_ALGSEL_AES | OP_ALG_AAI_CTR_MOD128,
+		.class2_alg_type = OP_ALG_ALGSEL_SHA384 |
+				   OP_ALG_AAI_HMAC_PRECOMP,
+		.alg_op = OP_ALG_ALGSEL_SHA384 | OP_ALG_AAI_HMAC,
+	},
+	{
+		.name = "authenc(hmac(sha512),rfc3686(ctr(aes)))",
+		.driver_name = "authenc-hmac-sha512-rfc3686-ctr-aes-caam",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = aead_setkey,
+			.setauthsize = aead_setauthsize,
+			.encrypt = aead_encrypt,
+			.decrypt = aead_decrypt,
+			.givencrypt = aead_givencrypt,
+			.geniv = "<built-in>",
+			.ivsize = CTR_RFC3686_IV_SIZE,
+			.maxauthsize = SHA512_DIGEST_SIZE,
+			},
+		.class1_alg_type = OP_ALG_ALGSEL_AES | OP_ALG_AAI_CTR_MOD128,
+		.class2_alg_type = OP_ALG_ALGSEL_SHA512 |
+				   OP_ALG_AAI_HMAC_PRECOMP,
+		.alg_op = OP_ALG_ALGSEL_SHA512 | OP_ALG_AAI_HMAC,
+	},
+	{
+		.name = "rfc4106(gcm(aes))",
+		.driver_name = "rfc4106-gcm-aes-caam",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = rfc4106_setkey,
+			.setauthsize = rfc4106_setauthsize,
+			.encrypt = aead_encrypt,
+			.decrypt = aead_decrypt,
+			.givencrypt = aead_givencrypt,
+			.geniv = "<built-in>",
+			.ivsize = 8,
+			.maxauthsize = AES_BLOCK_SIZE,
+			},
+		.class1_alg_type = OP_ALG_ALGSEL_AES | OP_ALG_AAI_GCM,
+	},
+	{
+		.name = "rfc4543(gcm(aes))",
+		.driver_name = "rfc4543-gcm-aes-caam",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = rfc4543_setkey,
+			.setauthsize = rfc4543_setauthsize,
+			.encrypt = aead_encrypt,
+			.decrypt = aead_decrypt,
+			.givencrypt = aead_givencrypt,
+			.geniv = "<built-in>",
+			.ivsize = 8,
+			.maxauthsize = AES_BLOCK_SIZE,
+			},
+		.class1_alg_type = OP_ALG_ALGSEL_AES | OP_ALG_AAI_GCM,
+	},
+	/* Galois Counter Mode */
+	{
+		.name = "gcm(aes)",
+		.driver_name = "gcm-aes-caam",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = gcm_setkey,
+			.setauthsize = gcm_setauthsize,
+			.encrypt = aead_encrypt,
+			.decrypt = aead_decrypt,
+			.givencrypt = NULL,
+			.geniv = "<built-in>",
+			.ivsize = 12,
+			.maxauthsize = AES_BLOCK_SIZE,
+			},
+		.class1_alg_type = OP_ALG_ALGSEL_AES | OP_ALG_AAI_GCM,
+	},
 	/* ablkcipher descriptor */
 	{
 		.name = "cbc(aes)",
 		.driver_name = "cbc-aes-caam",
 		.blocksize = AES_BLOCK_SIZE,
-		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.type = CRYPTO_ALG_TYPE_GIVCIPHER,
 		.template_ablkcipher = {
 			.setkey = ablkcipher_setkey,
 			.encrypt = ablkcipher_encrypt,
 			.decrypt = ablkcipher_decrypt,
-			.geniv = "eseqiv",
+			.givencrypt = ablkcipher_givencrypt,
+			.geniv = "<built-in>",
 			.min_keysize = AES_MIN_KEY_SIZE,
 			.max_keysize = AES_MAX_KEY_SIZE,
 			.ivsize = AES_BLOCK_SIZE,
@@ -2330,12 +4057,13 @@ static struct caam_alg_template driver_algs[] = {
 		.name = "cbc(des3_ede)",
 		.driver_name = "cbc-3des-caam",
 		.blocksize = DES3_EDE_BLOCK_SIZE,
-		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.type = CRYPTO_ALG_TYPE_GIVCIPHER,
 		.template_ablkcipher = {
 			.setkey = ablkcipher_setkey,
 			.encrypt = ablkcipher_encrypt,
 			.decrypt = ablkcipher_decrypt,
-			.geniv = "eseqiv",
+			.givencrypt = ablkcipher_givencrypt,
+			.geniv = "<built-in>",
 			.min_keysize = DES3_EDE_KEY_SIZE,
 			.max_keysize = DES3_EDE_KEY_SIZE,
 			.ivsize = DES3_EDE_BLOCK_SIZE,
@@ -2346,17 +4074,53 @@ static struct caam_alg_template driver_algs[] = {
 		.name = "cbc(des)",
 		.driver_name = "cbc-des-caam",
 		.blocksize = DES_BLOCK_SIZE,
-		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.type = CRYPTO_ALG_TYPE_GIVCIPHER,
 		.template_ablkcipher = {
 			.setkey = ablkcipher_setkey,
 			.encrypt = ablkcipher_encrypt,
 			.decrypt = ablkcipher_decrypt,
-			.geniv = "eseqiv",
+			.givencrypt = ablkcipher_givencrypt,
+			.geniv = "<built-in>",
 			.min_keysize = DES_KEY_SIZE,
 			.max_keysize = DES_KEY_SIZE,
 			.ivsize = DES_BLOCK_SIZE,
 			},
 		.class1_alg_type = OP_ALG_ALGSEL_DES | OP_ALG_AAI_CBC,
+	},
+	{
+		.name = "ctr(aes)",
+		.driver_name = "ctr-aes-caam",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.template_ablkcipher = {
+			.setkey = ablkcipher_setkey,
+			.encrypt = ablkcipher_encrypt,
+			.decrypt = ablkcipher_decrypt,
+			.geniv = "chainiv",
+			.min_keysize = AES_MIN_KEY_SIZE,
+			.max_keysize = AES_MAX_KEY_SIZE,
+			.ivsize = AES_BLOCK_SIZE,
+			},
+		.class1_alg_type = OP_ALG_ALGSEL_AES | OP_ALG_AAI_CTR_MOD128,
+	},
+	{
+		.name = "rfc3686(ctr(aes))",
+		.driver_name = "rfc3686-ctr-aes-caam",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_GIVCIPHER,
+		.template_ablkcipher = {
+			.setkey = ablkcipher_setkey,
+			.encrypt = ablkcipher_encrypt,
+			.decrypt = ablkcipher_decrypt,
+			.givencrypt = ablkcipher_givencrypt,
+			.geniv = "<built-in>",
+			.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,
+			},
+		.class1_alg_type = OP_ALG_ALGSEL_AES | OP_ALG_AAI_CTR_MOD128,
 	}
 };
 
@@ -2457,6 +4221,10 @@ static struct caam_crypto_alg *caam_alg_alloc(struct caam_alg_template
 	alg->cra_flags = CRYPTO_ALG_ASYNC | CRYPTO_ALG_KERN_DRIVER_ONLY |
 			 template->type;
 	switch (template->type) {
+	case CRYPTO_ALG_TYPE_GIVCIPHER:
+		alg->cra_type = &crypto_givcipher_type;
+		alg->cra_ablkcipher = template->template_ablkcipher;
+		break;
 	case CRYPTO_ALG_TYPE_ABLKCIPHER:
 		alg->cra_type = &crypto_ablkcipher_type;
 		alg->cra_ablkcipher = template->template_ablkcipher;
diff --git a/drivers/crypto/caam/compat.h b/drivers/crypto/caam/compat.h
index f227922cea38..acd7743e2603 100644
--- a/drivers/crypto/caam/compat.h
+++ b/drivers/crypto/caam/compat.h
@@ -28,6 +28,7 @@
 #include <crypto/algapi.h>
 #include <crypto/null.h>
 #include <crypto/aes.h>
+#include <crypto/ctr.h>
 #include <crypto/des.h>
 #include <crypto/sha.h>
 #include <crypto/md5.h>
diff --git a/drivers/crypto/caam/ctrl.c b/drivers/crypto/caam/ctrl.c
index 31000c8c4a90..70f1e6f37336 100644
--- a/drivers/crypto/caam/ctrl.c
+++ b/drivers/crypto/caam/ctrl.c
@@ -720,7 +720,6 @@ MODULE_DEVICE_TABLE(of, caam_match);
 static struct platform_driver caam_driver = {
 	.driver = {
 		.name = "caam",
-		.owner = THIS_MODULE,
 		.of_match_table = caam_match,
 	},
 	.probe       = caam_probe,
diff --git a/drivers/crypto/caam/desc_constr.h b/drivers/crypto/caam/desc_constr.h
index 7eec20bb3849..9f79fd7bd4d7 100644
--- a/drivers/crypto/caam/desc_constr.h
+++ b/drivers/crypto/caam/desc_constr.h
@@ -192,6 +192,8 @@ static inline void append_##cmd(u32 *desc, unsigned int len, u32 options) \
 	PRINT_POS; \
 	append_cmd(desc, CMD_##op | len | options); \
 }
+
+APPEND_CMD_LEN(seq_load, SEQ_LOAD)
 APPEND_CMD_LEN(seq_store, SEQ_STORE)
 APPEND_CMD_LEN(seq_fifo_load, SEQ_FIFO_LOAD)
 APPEND_CMD_LEN(seq_fifo_store, SEQ_FIFO_STORE)
diff --git a/drivers/crypto/caam/error.c b/drivers/crypto/caam/error.c
index 6531054a44c8..66d73bf54166 100644
--- a/drivers/crypto/caam/error.c
+++ b/drivers/crypto/caam/error.c
@@ -213,27 +213,36 @@ void caam_jr_strstatus(struct device *jrdev, u32 status)
 		void (*report_ssed)(struct device *jrdev, const u32 status,
 				    const char *error);
 		const char *error;
-	} status_src[] = {
+	} status_src[16] = {
 		{ NULL, "No error" },
 		{ NULL, NULL },
 		{ report_ccb_status, "CCB" },
 		{ report_jump_status, "Jump" },
 		{ report_deco_status, "DECO" },
-		{ NULL, NULL },
+		{ NULL, "Queue Manager Interface" },
 		{ report_jr_status, "Job Ring" },
 		{ report_cond_code_status, "Condition Code" },
+		{ NULL, NULL },
+		{ NULL, NULL },
+		{ NULL, NULL },
+		{ NULL, NULL },
+		{ NULL, NULL },
+		{ NULL, NULL },
+		{ NULL, NULL },
+		{ NULL, NULL },
 	};
 	u32 ssrc = status >> JRSTA_SSRC_SHIFT;
 	const char *error = status_src[ssrc].error;
 
 	/*
-	 * If there is no further error handling function, just
-	 * print the error code, error string and exit. Otherwise
-	 * call the handler function.
+	 * If there is an error handling function, call it to report the error.
+	 * Otherwise print the error source name.
 	 */
-	if (!status_src[ssrc].report_ssed)
-		dev_err(jrdev, "%08x: %s: \n", status, status_src[ssrc].error);
-	else
+	if (status_src[ssrc].report_ssed)
 		status_src[ssrc].report_ssed(jrdev, status, error);
+	else if (error)
+		dev_err(jrdev, "%d: %s\n", ssrc, error);
+	else
+		dev_err(jrdev, "%d: unknown error source\n", ssrc);
 }
 EXPORT_SYMBOL(caam_jr_strstatus);
diff --git a/drivers/crypto/caam/jr.c b/drivers/crypto/caam/jr.c
index 4d18e27ffa9e..9b3ef1bc9bd7 100644
--- a/drivers/crypto/caam/jr.c
+++ b/drivers/crypto/caam/jr.c
@@ -181,8 +181,6 @@ static void caam_jr_dequeue(unsigned long devarg)
 		for (i = 0; CIRC_CNT(head, tail + i, JOBR_DEPTH) >= 1; i++) {
 			sw_idx = (tail + i) & (JOBR_DEPTH - 1);
 
-			smp_read_barrier_depends();
-
 			if (jrp->outring[hw_idx].desc ==
 			    jrp->entinfo[sw_idx].desc_addr_dma)
 				break; /* found */
@@ -218,7 +216,6 @@ static void caam_jr_dequeue(unsigned long devarg)
 		if (sw_idx == tail) {
 			do {
 				tail = (tail + 1) & (JOBR_DEPTH - 1);
-				smp_read_barrier_depends();
 			} while (CIRC_CNT(head, tail, JOBR_DEPTH) >= 1 &&
 				 jrp->entinfo[tail].desc_addr_dma == 0);
 
@@ -514,7 +511,6 @@ MODULE_DEVICE_TABLE(of, caam_jr_match);
 static struct platform_driver caam_jr_driver = {
 	.driver = {
 		.name = "caam_jr",
-		.owner = THIS_MODULE,
 		.of_match_table = caam_jr_match,
 	},
 	.probe       = caam_jr_probe,
diff --git a/drivers/crypto/ccp/ccp-platform.c b/drivers/crypto/ccp/ccp-platform.c
index b0a2806908f1..8c50bad25f7e 100644
--- a/drivers/crypto/ccp/ccp-platform.c
+++ b/drivers/crypto/ccp/ccp-platform.c
@@ -208,7 +208,6 @@ static const struct of_device_id ccp_platform_ids[] = {
 static struct platform_driver ccp_platform_driver = {
 	.driver = {
 		.name = "AMD Cryptographic Coprocessor",
-		.owner = THIS_MODULE,
 		.of_match_table = ccp_platform_ids,
 	},
 	.probe = ccp_platform_probe,
diff --git a/drivers/crypto/mv_cesa.c b/drivers/crypto/mv_cesa.c
index 032c72c1f953..f91f15ddee92 100644
--- a/drivers/crypto/mv_cesa.c
+++ b/drivers/crypto/mv_cesa.c
@@ -1180,7 +1180,6 @@ static struct platform_driver marvell_crypto = {
 	.probe		= mv_probe,
 	.remove		= mv_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "mv_crypto",
 		.of_match_table = mv_cesa_of_match_table,
 	},
diff --git a/drivers/crypto/mxs-dcp.c b/drivers/crypto/mxs-dcp.c
index b5f7e6db24d4..829d6394fb33 100644
--- a/drivers/crypto/mxs-dcp.c
+++ b/drivers/crypto/mxs-dcp.c
@@ -1090,7 +1090,6 @@ static struct platform_driver mxs_dcp_driver = {
 	.remove	= mxs_dcp_remove,
 	.driver	= {
 		.name		= "mxs-dcp",
-		.owner		= THIS_MODULE,
 		.of_match_table	= mxs_dcp_dt_ids,
 	},
 };
diff --git a/drivers/crypto/n2_core.c b/drivers/crypto/n2_core.c
index f8e3207fecb1..afd136b45f49 100644
--- a/drivers/crypto/n2_core.c
+++ b/drivers/crypto/n2_core.c
@@ -2210,7 +2210,6 @@ MODULE_DEVICE_TABLE(of, n2_crypto_match);
 static struct platform_driver n2_crypto_driver = {
 	.driver = {
 		.name		=	"n2cp",
-		.owner		=	THIS_MODULE,
 		.of_match_table	=	n2_crypto_match,
 	},
 	.probe		=	n2_crypto_probe,
@@ -2238,7 +2237,6 @@ MODULE_DEVICE_TABLE(of, n2_mau_match);
 static struct platform_driver n2_mau_driver = {
 	.driver = {
 		.name		=	"ncp",
-		.owner		=	THIS_MODULE,
 		.of_match_table	=	n2_mau_match,
 	},
 	.probe		=	n2_mau_probe,
diff --git a/drivers/crypto/nx/nx-aes-cbc.c b/drivers/crypto/nx/nx-aes-cbc.c
index cc00b52306ba..a066cc3450ae 100644
--- a/drivers/crypto/nx/nx-aes-cbc.c
+++ b/drivers/crypto/nx/nx-aes-cbc.c
@@ -72,27 +72,19 @@ static int cbc_aes_nx_crypt(struct blkcipher_desc *desc,
 	struct nx_csbcpb *csbcpb = nx_ctx->csbcpb;
 	unsigned long irq_flags;
 	unsigned int processed = 0, to_process;
-	u32 max_sg_len;
 	int rc;
 
 	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
 
-	max_sg_len = min_t(u32, nx_driver.of.max_sg_len/sizeof(struct nx_sg),
-			   nx_ctx->ap->sglen);
-
 	if (enc)
 		NX_CPB_FDM(csbcpb) |= NX_FDM_ENDE_ENCRYPT;
 	else
 		NX_CPB_FDM(csbcpb) &= ~NX_FDM_ENDE_ENCRYPT;
 
 	do {
-		to_process = min_t(u64, nbytes - processed,
-				   nx_ctx->ap->databytelen);
-		to_process = min_t(u64, to_process,
-				   NX_PAGE_SIZE * (max_sg_len - 1));
-		to_process = to_process & ~(AES_BLOCK_SIZE - 1);
+		to_process = nbytes - processed;
 
-		rc = nx_build_sg_lists(nx_ctx, desc, dst, src, to_process,
+		rc = nx_build_sg_lists(nx_ctx, desc, dst, src, &to_process,
 				       processed, csbcpb->cpb.aes_cbc.iv);
 		if (rc)
 			goto out;
diff --git a/drivers/crypto/nx/nx-aes-ccm.c b/drivers/crypto/nx/nx-aes-ccm.c
index 5ecd4c2414aa..67f80813a06f 100644
--- a/drivers/crypto/nx/nx-aes-ccm.c
+++ b/drivers/crypto/nx/nx-aes-ccm.c
@@ -181,6 +181,7 @@ static int generate_pat(u8                   *iv,
 	unsigned int iauth_len = 0;
 	u8 tmp[16], *b1 = NULL, *b0 = NULL, *result = NULL;
 	int rc;
+	unsigned int max_sg_len;
 
 	/* zero the ctr value */
 	memset(iv + 15 - iv[0], 0, iv[0] + 1);
@@ -248,10 +249,19 @@ static int generate_pat(u8                   *iv,
 	if (!req->assoclen) {
 		return rc;
 	} else if (req->assoclen <= 14) {
-		nx_insg = nx_build_sg_list(nx_insg, b1, 16, nx_ctx->ap->sglen);
-		nx_outsg = nx_build_sg_list(nx_outsg, tmp, 16,
+		unsigned int len = 16;
+
+		nx_insg = nx_build_sg_list(nx_insg, b1, &len, nx_ctx->ap->sglen);
+
+		if (len != 16)
+			return -EINVAL;
+
+		nx_outsg = nx_build_sg_list(nx_outsg, tmp, &len,
 					    nx_ctx->ap->sglen);
 
+		if (len != 16)
+			return -EINVAL;
+
 		/* 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) *
@@ -273,21 +283,24 @@ static int generate_pat(u8                   *iv,
 		atomic64_add(req->assoclen, &(nx_ctx->stats->aes_bytes));
 
 	} else {
-		u32 max_sg_len;
 		unsigned int processed = 0, to_process;
 
-		/* page_limit: number of sg entries that fit on one page */
-		max_sg_len = min_t(u32,
-				   nx_driver.of.max_sg_len/sizeof(struct nx_sg),
-				   nx_ctx->ap->sglen);
-
 		processed += iauth_len;
 
+		/* page_limit: number of sg entries that fit on one page */
+		max_sg_len = min_t(u64, nx_ctx->ap->sglen,
+				nx_driver.of.max_sg_len/sizeof(struct nx_sg));
+		max_sg_len = min_t(u64, max_sg_len,
+				nx_ctx->ap->databytelen/NX_PAGE_SIZE);
+
 		do {
 			to_process = min_t(u32, req->assoclen - processed,
 					   nx_ctx->ap->databytelen);
-			to_process = min_t(u64, to_process,
-					   NX_PAGE_SIZE * (max_sg_len - 1));
+
+			nx_insg = nx_walk_and_build(nx_ctx->in_sg,
+						    nx_ctx->ap->sglen,
+						    req->assoc, processed,
+						    &to_process);
 
 			if ((to_process + processed) < req->assoclen) {
 				NX_CPB_FDM(nx_ctx->csbcpb_aead) |=
@@ -297,10 +310,6 @@ static int generate_pat(u8                   *iv,
 					~NX_FDM_INTERMEDIATE;
 			}
 
-			nx_insg = nx_walk_and_build(nx_ctx->in_sg,
-						    nx_ctx->ap->sglen,
-						    req->assoc, processed,
-						    to_process);
 
 			nx_ctx->op_aead.inlen = (nx_ctx->in_sg - nx_insg) *
 						sizeof(struct nx_sg);
@@ -343,7 +352,6 @@ static int ccm_nx_decrypt(struct aead_request   *req,
 	struct nx_ccm_priv *priv = &nx_ctx->priv.ccm;
 	unsigned long irq_flags;
 	unsigned int processed = 0, to_process;
-	u32 max_sg_len;
 	int rc = -1;
 
 	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
@@ -360,19 +368,12 @@ static int ccm_nx_decrypt(struct aead_request   *req,
 	if (rc)
 		goto out;
 
-	/* page_limit: number of sg entries that fit on one page */
-	max_sg_len = min_t(u32, nx_driver.of.max_sg_len/sizeof(struct nx_sg),
-			   nx_ctx->ap->sglen);
-
 	do {
 
 		/* to_process: the AES_BLOCK_SIZE data chunk to process in this
 		 * update. This value is bound by sg list limits.
 		 */
-		to_process = min_t(u64, nbytes - processed,
-				   nx_ctx->ap->databytelen);
-		to_process = min_t(u64, to_process,
-				   NX_PAGE_SIZE * (max_sg_len - 1));
+		to_process = nbytes - processed;
 
 		if ((to_process + processed) < nbytes)
 			NX_CPB_FDM(csbcpb) |= NX_FDM_INTERMEDIATE;
@@ -382,7 +383,7 @@ static int ccm_nx_decrypt(struct aead_request   *req,
 		NX_CPB_FDM(nx_ctx->csbcpb) &= ~NX_FDM_ENDE_ENCRYPT;
 
 		rc = nx_build_sg_lists(nx_ctx, desc, req->dst, req->src,
-					to_process, processed,
+					&to_process, processed,
 					csbcpb->cpb.aes_ccm.iv_or_ctr);
 		if (rc)
 			goto out;
@@ -427,7 +428,6 @@ static int ccm_nx_encrypt(struct aead_request   *req,
 	unsigned int authsize = crypto_aead_authsize(crypto_aead_reqtfm(req));
 	unsigned long irq_flags;
 	unsigned int processed = 0, to_process;
-	u32 max_sg_len;
 	int rc = -1;
 
 	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
@@ -437,18 +437,11 @@ static int ccm_nx_encrypt(struct aead_request   *req,
 	if (rc)
 		goto out;
 
-	/* page_limit: number of sg entries that fit on one page */
-	max_sg_len = min_t(u32, nx_driver.of.max_sg_len/sizeof(struct nx_sg),
-			   nx_ctx->ap->sglen);
-
 	do {
 		/* to process: the AES_BLOCK_SIZE data chunk to process in this
 		 * update. This value is bound by sg list limits.
 		 */
-		to_process = min_t(u64, nbytes - processed,
-				   nx_ctx->ap->databytelen);
-		to_process = min_t(u64, to_process,
-				   NX_PAGE_SIZE * (max_sg_len - 1));
+		to_process = nbytes - processed;
 
 		if ((to_process + processed) < nbytes)
 			NX_CPB_FDM(csbcpb) |= NX_FDM_INTERMEDIATE;
@@ -458,7 +451,7 @@ static int ccm_nx_encrypt(struct aead_request   *req,
 		NX_CPB_FDM(csbcpb) |= NX_FDM_ENDE_ENCRYPT;
 
 		rc = nx_build_sg_lists(nx_ctx, desc, req->dst, req->src,
-					to_process, processed,
+					&to_process, processed,
 				       csbcpb->cpb.aes_ccm.iv_or_ctr);
 		if (rc)
 			goto out;
diff --git a/drivers/crypto/nx/nx-aes-ctr.c b/drivers/crypto/nx/nx-aes-ctr.c
index a37d009dc75c..2617cd4d54dd 100644
--- a/drivers/crypto/nx/nx-aes-ctr.c
+++ b/drivers/crypto/nx/nx-aes-ctr.c
@@ -90,22 +90,14 @@ static int ctr_aes_nx_crypt(struct blkcipher_desc *desc,
 	struct nx_csbcpb *csbcpb = nx_ctx->csbcpb;
 	unsigned long irq_flags;
 	unsigned int processed = 0, to_process;
-	u32 max_sg_len;
 	int rc;
 
 	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
 
-	max_sg_len = min_t(u32, nx_driver.of.max_sg_len/sizeof(struct nx_sg),
-			   nx_ctx->ap->sglen);
-
 	do {
-		to_process = min_t(u64, nbytes - processed,
-				   nx_ctx->ap->databytelen);
-		to_process = min_t(u64, to_process,
-				   NX_PAGE_SIZE * (max_sg_len - 1));
-		to_process = to_process & ~(AES_BLOCK_SIZE - 1);
+		to_process = nbytes - processed;
 
-		rc = nx_build_sg_lists(nx_ctx, desc, dst, src, to_process,
+		rc = nx_build_sg_lists(nx_ctx, desc, dst, src, &to_process,
 				       processed, csbcpb->cpb.aes_ctr.iv);
 		if (rc)
 			goto out;
@@ -143,6 +135,7 @@ static int ctr3686_aes_nx_crypt(struct blkcipher_desc *desc,
 
 	memcpy(iv + CTR_RFC3686_NONCE_SIZE,
 	       desc->info, CTR_RFC3686_IV_SIZE);
+	iv[12] = iv[13] = iv[14] = 0;
 	iv[15] = 1;
 
 	desc->info = nx_ctx->priv.ctr.iv;
diff --git a/drivers/crypto/nx/nx-aes-ecb.c b/drivers/crypto/nx/nx-aes-ecb.c
index 85a8d23cf29d..cfdde8b8bc76 100644
--- a/drivers/crypto/nx/nx-aes-ecb.c
+++ b/drivers/crypto/nx/nx-aes-ecb.c
@@ -72,27 +72,19 @@ static int ecb_aes_nx_crypt(struct blkcipher_desc *desc,
 	struct nx_csbcpb *csbcpb = nx_ctx->csbcpb;
 	unsigned long irq_flags;
 	unsigned int processed = 0, to_process;
-	u32 max_sg_len;
 	int rc;
 
 	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
 
-	max_sg_len = min_t(u32, nx_driver.of.max_sg_len/sizeof(struct nx_sg),
-			   nx_ctx->ap->sglen);
-
 	if (enc)
 		NX_CPB_FDM(csbcpb) |= NX_FDM_ENDE_ENCRYPT;
 	else
 		NX_CPB_FDM(csbcpb) &= ~NX_FDM_ENDE_ENCRYPT;
 
 	do {
-		to_process = min_t(u64, nbytes - processed,
-				   nx_ctx->ap->databytelen);
-		to_process = min_t(u64, to_process,
-				   NX_PAGE_SIZE * (max_sg_len - 1));
-		to_process = to_process & ~(AES_BLOCK_SIZE - 1);
+		to_process = nbytes - processed;
 
-		rc = nx_build_sg_lists(nx_ctx, desc, dst, src, to_process,
+		rc = nx_build_sg_lists(nx_ctx, desc, dst, src, &to_process,
 				processed, NULL);
 		if (rc)
 			goto out;
diff --git a/drivers/crypto/nx/nx-aes-gcm.c b/drivers/crypto/nx/nx-aes-gcm.c
index 025d9a8d5b19..88c562434bc0 100644
--- a/drivers/crypto/nx/nx-aes-gcm.c
+++ b/drivers/crypto/nx/nx-aes-gcm.c
@@ -131,7 +131,7 @@ static int nx_gca(struct nx_crypto_ctx  *nx_ctx,
 	struct nx_sg *nx_sg = nx_ctx->in_sg;
 	unsigned int nbytes = req->assoclen;
 	unsigned int processed = 0, to_process;
-	u32 max_sg_len;
+	unsigned int max_sg_len;
 
 	if (nbytes <= AES_BLOCK_SIZE) {
 		scatterwalk_start(&walk, req->assoc);
@@ -143,8 +143,10 @@ static int nx_gca(struct nx_crypto_ctx  *nx_ctx,
 	NX_CPB_FDM(csbcpb_aead) &= ~NX_FDM_CONTINUATION;
 
 	/* page_limit: number of sg entries that fit on one page */
-	max_sg_len = min_t(u32, nx_driver.of.max_sg_len/sizeof(struct nx_sg),
+	max_sg_len = min_t(u64, nx_driver.of.max_sg_len/sizeof(struct nx_sg),
 			   nx_ctx->ap->sglen);
+	max_sg_len = min_t(u64, max_sg_len,
+			   nx_ctx->ap->databytelen/NX_PAGE_SIZE);
 
 	do {
 		/*
@@ -156,13 +158,14 @@ static int nx_gca(struct nx_crypto_ctx  *nx_ctx,
 		to_process = min_t(u64, to_process,
 				   NX_PAGE_SIZE * (max_sg_len - 1));
 
+		nx_sg = nx_walk_and_build(nx_ctx->in_sg, max_sg_len,
+					  req->assoc, processed, &to_process);
+
 		if ((to_process + processed) < nbytes)
 			NX_CPB_FDM(csbcpb_aead) |= NX_FDM_INTERMEDIATE;
 		else
 			NX_CPB_FDM(csbcpb_aead) &= ~NX_FDM_INTERMEDIATE;
 
-		nx_sg = nx_walk_and_build(nx_ctx->in_sg, nx_ctx->ap->sglen,
-					  req->assoc, processed, to_process);
 		nx_ctx->op_aead.inlen = (nx_ctx->in_sg - nx_sg)
 					* sizeof(struct nx_sg);
 
@@ -195,7 +198,7 @@ static int gmac(struct aead_request *req, struct blkcipher_desc *desc)
 	struct nx_sg *nx_sg;
 	unsigned int nbytes = req->assoclen;
 	unsigned int processed = 0, to_process;
-	u32 max_sg_len;
+	unsigned int max_sg_len;
 
 	/* Set GMAC mode */
 	csbcpb->cpb.hdr.mode = NX_MODE_AES_GMAC;
@@ -203,8 +206,10 @@ static int gmac(struct aead_request *req, struct blkcipher_desc *desc)
 	NX_CPB_FDM(csbcpb) &= ~NX_FDM_CONTINUATION;
 
 	/* page_limit: number of sg entries that fit on one page */
-	max_sg_len = min_t(u32, nx_driver.of.max_sg_len/sizeof(struct nx_sg),
+	max_sg_len = min_t(u64, nx_driver.of.max_sg_len/sizeof(struct nx_sg),
 			   nx_ctx->ap->sglen);
+	max_sg_len = min_t(u64, max_sg_len,
+			   nx_ctx->ap->databytelen/NX_PAGE_SIZE);
 
 	/* Copy IV */
 	memcpy(csbcpb->cpb.aes_gcm.iv_or_cnt, desc->info, AES_BLOCK_SIZE);
@@ -219,13 +224,14 @@ static int gmac(struct aead_request *req, struct blkcipher_desc *desc)
 		to_process = min_t(u64, to_process,
 				   NX_PAGE_SIZE * (max_sg_len - 1));
 
+		nx_sg = nx_walk_and_build(nx_ctx->in_sg, max_sg_len,
+					  req->assoc, processed, &to_process);
+
 		if ((to_process + processed) < nbytes)
 			NX_CPB_FDM(csbcpb) |= NX_FDM_INTERMEDIATE;
 		else
 			NX_CPB_FDM(csbcpb) &= ~NX_FDM_INTERMEDIATE;
 
-		nx_sg = nx_walk_and_build(nx_ctx->in_sg, nx_ctx->ap->sglen,
-					  req->assoc, processed, to_process);
 		nx_ctx->op.inlen = (nx_ctx->in_sg - nx_sg)
 					* sizeof(struct nx_sg);
 
@@ -264,6 +270,7 @@ static int gcm_empty(struct aead_request *req, struct blkcipher_desc *desc,
 	struct nx_csbcpb *csbcpb = nx_ctx->csbcpb;
 	char out[AES_BLOCK_SIZE];
 	struct nx_sg *in_sg, *out_sg;
+	int len;
 
 	/* 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
@@ -279,11 +286,22 @@ static int gcm_empty(struct aead_request *req, struct blkcipher_desc *desc,
 	else
 		NX_CPB_FDM(csbcpb) &= ~NX_FDM_ENDE_ENCRYPT;
 
+	len = AES_BLOCK_SIZE;
+
 	/* Encrypt the counter/IV */
 	in_sg = nx_build_sg_list(nx_ctx->in_sg, (u8 *) desc->info,
-				 AES_BLOCK_SIZE, nx_ctx->ap->sglen);
-	out_sg = nx_build_sg_list(nx_ctx->out_sg, (u8 *) out, sizeof(out),
+				 &len, nx_ctx->ap->sglen);
+
+	if (len != AES_BLOCK_SIZE)
+		return -EINVAL;
+
+	len = sizeof(out);
+	out_sg = nx_build_sg_list(nx_ctx->out_sg, (u8 *) out, &len,
 				  nx_ctx->ap->sglen);
+
+	if (len != sizeof(out))
+		return -EINVAL;
+
 	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);
 
@@ -317,7 +335,6 @@ static int gcm_aes_nx_crypt(struct aead_request *req, int enc)
 	unsigned int nbytes = req->cryptlen;
 	unsigned int processed = 0, to_process;
 	unsigned long irq_flags;
-	u32 max_sg_len;
 	int rc = -EINVAL;
 
 	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
@@ -354,33 +371,24 @@ static int gcm_aes_nx_crypt(struct aead_request *req, int enc)
 		nbytes -= crypto_aead_authsize(crypto_aead_reqtfm(req));
 	}
 
-	/* page_limit: number of sg entries that fit on one page */
-	max_sg_len = min_t(u32, nx_driver.of.max_sg_len/sizeof(struct nx_sg),
-			   nx_ctx->ap->sglen);
-
 	do {
-		/*
-		 * to_process: the data chunk to process in this update.
-		 * This value is bound by sg list limits.
-		 */
-		to_process = min_t(u64, nbytes - processed,
-				   nx_ctx->ap->databytelen);
-		to_process = min_t(u64, to_process,
-				   NX_PAGE_SIZE * (max_sg_len - 1));
-
-		if ((to_process + processed) < nbytes)
-			NX_CPB_FDM(csbcpb) |= NX_FDM_INTERMEDIATE;
-		else
-			NX_CPB_FDM(csbcpb) &= ~NX_FDM_INTERMEDIATE;
+		to_process = nbytes - processed;
 
 		csbcpb->cpb.aes_gcm.bit_length_data = nbytes * 8;
 		desc.tfm = (struct crypto_blkcipher *) req->base.tfm;
 		rc = nx_build_sg_lists(nx_ctx, &desc, req->dst,
-				       req->src, to_process, processed,
+				       req->src, &to_process, processed,
 				       csbcpb->cpb.aes_gcm.iv_or_cnt);
+
 		if (rc)
 			goto out;
 
+		if ((to_process + processed) < nbytes)
+			NX_CPB_FDM(csbcpb) |= NX_FDM_INTERMEDIATE;
+		else
+			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)
diff --git a/drivers/crypto/nx/nx-aes-xcbc.c b/drivers/crypto/nx/nx-aes-xcbc.c
index 03c4bf57d066..8c2faffab4a3 100644
--- a/drivers/crypto/nx/nx-aes-xcbc.c
+++ b/drivers/crypto/nx/nx-aes-xcbc.c
@@ -75,6 +75,7 @@ static int nx_xcbc_empty(struct shash_desc *desc, u8 *out)
 	u8 keys[2][AES_BLOCK_SIZE];
 	u8 key[32];
 	int rc = 0;
+	int len;
 
 	/* Change to ECB mode */
 	csbcpb->cpb.hdr.mode = NX_MODE_AES_ECB;
@@ -86,11 +87,20 @@ static int nx_xcbc_empty(struct shash_desc *desc, u8 *out)
 	memset(keys[0], 0x01, sizeof(keys[0]));
 	memset(keys[1], 0x03, sizeof(keys[1]));
 
+	len = sizeof(keys);
 	/* Generate K1 and K3 encrypting the patterns */
-	in_sg = nx_build_sg_list(nx_ctx->in_sg, (u8 *) keys, sizeof(keys),
+	in_sg = nx_build_sg_list(nx_ctx->in_sg, (u8 *) keys, &len,
 				 nx_ctx->ap->sglen);
-	out_sg = nx_build_sg_list(nx_ctx->out_sg, (u8 *) keys, sizeof(keys),
+
+	if (len != sizeof(keys))
+		return -EINVAL;
+
+	out_sg = nx_build_sg_list(nx_ctx->out_sg, (u8 *) keys, &len,
 				  nx_ctx->ap->sglen);
+
+	if (len != sizeof(keys))
+		return -EINVAL;
+
 	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);
 
@@ -103,12 +113,23 @@ static int nx_xcbc_empty(struct shash_desc *desc, u8 *out)
 	/* XOr K3 with the padding for a 0 length message */
 	keys[1][0] ^= 0x80;
 
+	len = sizeof(keys[1]);
+
 	/* Encrypt the final result */
 	memcpy(csbcpb->cpb.aes_ecb.key, keys[0], AES_BLOCK_SIZE);
-	in_sg = nx_build_sg_list(nx_ctx->in_sg, (u8 *) keys[1], sizeof(keys[1]),
+	in_sg = nx_build_sg_list(nx_ctx->in_sg, (u8 *) keys[1], &len,
 				 nx_ctx->ap->sglen);
-	out_sg = nx_build_sg_list(nx_ctx->out_sg, out, AES_BLOCK_SIZE,
+
+	if (len != sizeof(keys[1]))
+		return -EINVAL;
+
+	len = AES_BLOCK_SIZE;
+	out_sg = nx_build_sg_list(nx_ctx->out_sg, out, &len,
 				  nx_ctx->ap->sglen);
+
+	if (len != AES_BLOCK_SIZE)
+		return -EINVAL;
+
 	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);
 
@@ -133,6 +154,7 @@ static int nx_xcbc_init(struct shash_desc *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;
+	int len;
 
 	nx_ctx_init(nx_ctx, HCOP_FC_AES);
 
@@ -144,8 +166,13 @@ static int nx_xcbc_init(struct shash_desc *desc)
 	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);
 
+	len = AES_BLOCK_SIZE;
 	out_sg = nx_build_sg_list(nx_ctx->out_sg, (u8 *)sctx->state,
-				  AES_BLOCK_SIZE, nx_ctx->ap->sglen);
+				  &len, nx_ctx->ap->sglen);
+
+	if (len != AES_BLOCK_SIZE)
+		return -EINVAL;
+
 	nx_ctx->op.outlen = (nx_ctx->out_sg - out_sg) * sizeof(struct nx_sg);
 
 	return 0;
@@ -159,10 +186,11 @@ static int nx_xcbc_update(struct shash_desc *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, total;
-	u32 max_sg_len;
+	u32 to_process = 0, leftover, total;
+	unsigned int max_sg_len;
 	unsigned long irq_flags;
 	int rc = 0;
+	int data_len;
 
 	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
 
@@ -180,17 +208,15 @@ static int nx_xcbc_update(struct shash_desc *desc,
 	}
 
 	in_sg = nx_ctx->in_sg;
-	max_sg_len = min_t(u32, nx_driver.of.max_sg_len/sizeof(struct nx_sg),
+	max_sg_len = min_t(u64, nx_driver.of.max_sg_len/sizeof(struct nx_sg),
 				nx_ctx->ap->sglen);
+	max_sg_len = min_t(u64, max_sg_len,
+				nx_ctx->ap->databytelen/NX_PAGE_SIZE);
 
 	do {
-
-		/* to_process: the AES_BLOCK_SIZE data chunk to process in this
-		 * update */
-		to_process = min_t(u64, total, nx_ctx->ap->databytelen);
-		to_process = min_t(u64, to_process,
-					NX_PAGE_SIZE * (max_sg_len - 1));
+		to_process = total - to_process;
 		to_process = to_process & ~(AES_BLOCK_SIZE - 1);
+
 		leftover = total - to_process;
 
 		/* the hardware will not accept a 0 byte operation for this
@@ -204,15 +230,24 @@ static int nx_xcbc_update(struct shash_desc *desc,
 		}
 
 		if (sctx->count) {
+			data_len = sctx->count;
 			in_sg = nx_build_sg_list(nx_ctx->in_sg,
 						(u8 *) sctx->buffer,
-						sctx->count,
+						&data_len,
 						max_sg_len);
+			if (data_len != sctx->count)
+				return -EINVAL;
 		}
+
+		data_len = to_process - sctx->count;
 		in_sg = nx_build_sg_list(in_sg,
 					(u8 *) data,
-					to_process - sctx->count,
+					&data_len,
 					max_sg_len);
+
+		if (data_len != to_process - sctx->count)
+			return -EINVAL;
+
 		nx_ctx->op.inlen = (nx_ctx->in_sg - in_sg) *
 					sizeof(struct nx_sg);
 
@@ -263,6 +298,7 @@ static int nx_xcbc_final(struct shash_desc *desc, u8 *out)
 	struct nx_sg *in_sg, *out_sg;
 	unsigned long irq_flags;
 	int rc = 0;
+	int len;
 
 	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
 
@@ -285,11 +321,20 @@ static int nx_xcbc_final(struct shash_desc *desc, u8 *out)
 	 * this is not an intermediate operation */
 	NX_CPB_FDM(csbcpb) &= ~NX_FDM_INTERMEDIATE;
 
+	len = sctx->count;
 	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,
+				 &len, nx_ctx->ap->sglen);
+
+	if (len != sctx->count)
+		return -EINVAL;
+
+	len = AES_BLOCK_SIZE;
+	out_sg = nx_build_sg_list(nx_ctx->out_sg, out, &len,
 				  nx_ctx->ap->sglen);
 
+	if (len != AES_BLOCK_SIZE)
+		return -EINVAL;
+
 	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);
 
diff --git a/drivers/crypto/nx/nx-sha256.c b/drivers/crypto/nx/nx-sha256.c
index da0b24a7633f..23621da624c3 100644
--- a/drivers/crypto/nx/nx-sha256.c
+++ b/drivers/crypto/nx/nx-sha256.c
@@ -23,6 +23,7 @@
 #include <crypto/sha.h>
 #include <linux/module.h>
 #include <asm/vio.h>
+#include <asm/byteorder.h>
 
 #include "nx_csbcpb.h"
 #include "nx.h"
@@ -32,7 +33,8 @@ 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;
+	int len;
+	int rc;
 
 	nx_ctx_init(nx_ctx, HCOP_FC_SHA);
 
@@ -41,10 +43,28 @@ static int nx_sha256_init(struct shash_desc *desc)
 	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);
 
+	len = SHA256_DIGEST_SIZE;
+	rc = nx_sha_build_sg_list(nx_ctx, nx_ctx->out_sg,
+				  &nx_ctx->op.outlen,
+				  &len,
+				  (u8 *) sctx->state,
+				  NX_DS_SHA256);
+
+	if (rc)
+		goto out;
+
+	sctx->state[0] = __cpu_to_be32(SHA256_H0);
+	sctx->state[1] = __cpu_to_be32(SHA256_H1);
+	sctx->state[2] = __cpu_to_be32(SHA256_H2);
+	sctx->state[3] = __cpu_to_be32(SHA256_H3);
+	sctx->state[4] = __cpu_to_be32(SHA256_H4);
+	sctx->state[5] = __cpu_to_be32(SHA256_H5);
+	sctx->state[6] = __cpu_to_be32(SHA256_H6);
+	sctx->state[7] = __cpu_to_be32(SHA256_H7);
+	sctx->count = 0;
+
+out:
 	return 0;
 }
 
@@ -54,11 +74,11 @@ static int nx_sha256_update(struct shash_desc *desc, const u8 *data,
 	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, total;
-	u32 max_sg_len;
+	u64 to_process = 0, leftover, total;
 	unsigned long irq_flags;
 	int rc = 0;
+	int data_len;
+	u64 buf_len = (sctx->count % SHA256_BLOCK_SIZE);
 
 	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
 
@@ -66,16 +86,16 @@ static int nx_sha256_update(struct shash_desc *desc, const u8 *data,
 	 *  1: < SHA256_BLOCK_SIZE: copy into state, return 0
 	 *  2: >= SHA256_BLOCK_SIZE: process X blocks, copy in leftover
 	 */
-	total = sctx->count + len;
+	total = (sctx->count % SHA256_BLOCK_SIZE) + len;
 	if (total < SHA256_BLOCK_SIZE) {
-		memcpy(sctx->buf + sctx->count, data, len);
+		memcpy(sctx->buf + buf_len, data, len);
 		sctx->count += len;
 		goto out;
 	}
 
-	in_sg = nx_ctx->in_sg;
-	max_sg_len = min_t(u32, nx_driver.of.max_sg_len/sizeof(struct nx_sg),
-			   nx_ctx->ap->sglen);
+	memcpy(csbcpb->cpb.sha256.message_digest, sctx->state, SHA256_DIGEST_SIZE);
+	NX_CPB_FDM(csbcpb) |= NX_FDM_INTERMEDIATE;
+	NX_CPB_FDM(csbcpb) |= NX_FDM_CONTINUATION;
 
 	do {
 		/*
@@ -83,34 +103,42 @@ static int nx_sha256_update(struct shash_desc *desc, const u8 *data,
 		 * this update. This value is also restricted by the sg list
 		 * limits.
 		 */
-		to_process = min_t(u64, total, nx_ctx->ap->databytelen);
-		to_process = min_t(u64, to_process,
-				   NX_PAGE_SIZE * (max_sg_len - 1));
+		to_process = total - to_process;
 		to_process = to_process & ~(SHA256_BLOCK_SIZE - 1);
-		leftover = total - to_process;
 
-		if (sctx->count) {
-			in_sg = nx_build_sg_list(nx_ctx->in_sg,
-						 (u8 *) sctx->buf,
-						 sctx->count, max_sg_len);
+		if (buf_len) {
+			data_len = buf_len;
+			rc = nx_sha_build_sg_list(nx_ctx, nx_ctx->in_sg,
+						  &nx_ctx->op.inlen,
+						  &data_len,
+						  (u8 *) sctx->buf,
+						  NX_DS_SHA256);
+
+			if (rc || data_len != buf_len)
+				goto out;
 		}
-		in_sg = nx_build_sg_list(in_sg, (u8 *) data,
-					 to_process - sctx->count,
-					 max_sg_len);
-		nx_ctx->op.inlen = (nx_ctx->in_sg - in_sg) *
-					sizeof(struct nx_sg);
-
-		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,
+
+		data_len = to_process - buf_len;
+		rc = nx_sha_build_sg_list(nx_ctx, nx_ctx->in_sg,
+					  &nx_ctx->op.inlen,
+					  &data_len,
+					  (u8 *) data,
+					  NX_DS_SHA256);
+
+		if (rc)
+			goto out;
+
+		to_process = (data_len + buf_len);
+		leftover = total - to_process;
+
+		/*
+		 * 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);
-		}
 
-		NX_CPB_FDM(csbcpb) |= NX_FDM_INTERMEDIATE;
 		if (!nx_ctx->op.inlen || !nx_ctx->op.outlen) {
 			rc = -EINVAL;
 			goto out;
@@ -122,22 +150,19 @@ static int nx_sha256_update(struct shash_desc *desc, const u8 *data,
 			goto out;
 
 		atomic_inc(&(nx_ctx->stats->sha256_ops));
-		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;
 
 		total -= to_process;
-		data += to_process - sctx->count;
-		sctx->count = 0;
-		in_sg = nx_ctx->in_sg;
+		data += to_process - buf_len;
+		buf_len = 0;
+
 	} while (leftover >= SHA256_BLOCK_SIZE);
 
 	/* copy the leftover back into the state struct */
 	if (leftover)
 		memcpy(sctx->buf, data, leftover);
-	sctx->count = leftover;
+
+	sctx->count += len;
+	memcpy(sctx->state, csbcpb->cpb.sha256.message_digest, SHA256_DIGEST_SIZE);
 out:
 	spin_unlock_irqrestore(&nx_ctx->lock, irq_flags);
 	return rc;
@@ -148,34 +173,46 @@ 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;
-	u32 max_sg_len;
 	unsigned long irq_flags;
 	int rc;
+	int len;
 
 	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
 
-	max_sg_len = min_t(u32, nx_driver.of.max_sg_len, nx_ctx->ap->sglen);
-
-	if (NX_CPB_FDM(csbcpb) & NX_FDM_CONTINUATION) {
+	/* final is represented by continuing the operation and indicating that
+	 * this is not an intermediate operation */
+	if (sctx->count >= SHA256_BLOCK_SIZE) {
 		/* 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);
+		memcpy(csbcpb->cpb.sha256.input_partial_digest, sctx->state, SHA256_DIGEST_SIZE);
+		NX_CPB_FDM(csbcpb) &= ~NX_FDM_INTERMEDIATE;
+		NX_CPB_FDM(csbcpb) |= NX_FDM_CONTINUATION;
+	} else {
+		NX_CPB_FDM(csbcpb) &= ~NX_FDM_INTERMEDIATE;
+		NX_CPB_FDM(csbcpb) &= ~NX_FDM_CONTINUATION;
 	}
 
-	/* 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);
 
-	csbcpb->cpb.sha256.message_bit_length += (u64)(sctx->count * 8);
+	len = sctx->count & (SHA256_BLOCK_SIZE - 1);
+	rc = nx_sha_build_sg_list(nx_ctx, nx_ctx->in_sg,
+				  &nx_ctx->op.inlen,
+				  &len,
+				  (u8 *) sctx->buf,
+				  NX_DS_SHA256);
 
-	in_sg = nx_build_sg_list(nx_ctx->in_sg, (u8 *)sctx->buf,
-				 sctx->count, max_sg_len);
-	out_sg = nx_build_sg_list(nx_ctx->out_sg, out, SHA256_DIGEST_SIZE,
-				  max_sg_len);
-	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 (rc || len != (sctx->count & (SHA256_BLOCK_SIZE - 1)))
+		goto out;
+
+	len = SHA256_DIGEST_SIZE;
+	rc = nx_sha_build_sg_list(nx_ctx, nx_ctx->out_sg,
+				  &nx_ctx->op.outlen,
+				  &len,
+				  out,
+				  NX_DS_SHA256);
+
+	if (rc || len != SHA256_DIGEST_SIZE)
+		goto out;
 
 	if (!nx_ctx->op.outlen) {
 		rc = -EINVAL;
@@ -189,8 +226,7 @@ static int nx_sha256_final(struct shash_desc *desc, u8 *out)
 
 	atomic_inc(&(nx_ctx->stats->sha256_ops));
 
-	atomic64_add(csbcpb->cpb.sha256.message_bit_length / 8,
-		     &(nx_ctx->stats->sha256_bytes));
+	atomic64_add(sctx->count, &(nx_ctx->stats->sha256_bytes));
 	memcpy(out, csbcpb->cpb.sha256.message_digest, SHA256_DIGEST_SIZE);
 out:
 	spin_unlock_irqrestore(&nx_ctx->lock, irq_flags);
@@ -200,62 +236,18 @@ out:
 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;
-	unsigned long irq_flags;
-
-	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
 
-	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;
-	}
+	memcpy(out, sctx, sizeof(*sctx));
 
-	spin_unlock_irqrestore(&nx_ctx->lock, irq_flags);
 	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;
-	unsigned long irq_flags;
-
-	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
 
-	memcpy(sctx->buf, ictx->buf, sizeof(ictx->buf));
+	memcpy(sctx, in, sizeof(*sctx));
 
-	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;
-	}
-
-	spin_unlock_irqrestore(&nx_ctx->lock, irq_flags);
 	return 0;
 }
 
diff --git a/drivers/crypto/nx/nx-sha512.c b/drivers/crypto/nx/nx-sha512.c
index 4ae5b0f221d5..b3adf1022673 100644
--- a/drivers/crypto/nx/nx-sha512.c
+++ b/drivers/crypto/nx/nx-sha512.c
@@ -32,7 +32,8 @@ 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;
+	int len;
+	int rc;
 
 	nx_ctx_init(nx_ctx, HCOP_FC_SHA);
 
@@ -41,10 +42,28 @@ static int nx_sha512_init(struct shash_desc *desc)
 	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);
 
+	len = SHA512_DIGEST_SIZE;
+	rc = nx_sha_build_sg_list(nx_ctx, nx_ctx->out_sg,
+				  &nx_ctx->op.outlen,
+				  &len,
+				  (u8 *)sctx->state,
+				  NX_DS_SHA512);
+
+	if (rc || len != SHA512_DIGEST_SIZE)
+		goto out;
+
+	sctx->state[0] = __cpu_to_be64(SHA512_H0);
+	sctx->state[1] = __cpu_to_be64(SHA512_H1);
+	sctx->state[2] = __cpu_to_be64(SHA512_H2);
+	sctx->state[3] = __cpu_to_be64(SHA512_H3);
+	sctx->state[4] = __cpu_to_be64(SHA512_H4);
+	sctx->state[5] = __cpu_to_be64(SHA512_H5);
+	sctx->state[6] = __cpu_to_be64(SHA512_H6);
+	sctx->state[7] = __cpu_to_be64(SHA512_H7);
+	sctx->count[0] = 0;
+
+out:
 	return 0;
 }
 
@@ -54,11 +73,11 @@ static int nx_sha512_update(struct shash_desc *desc, const u8 *data,
 	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, total, spbc_bits;
-	u32 max_sg_len;
+	u64 to_process, leftover = 0, total;
 	unsigned long irq_flags;
 	int rc = 0;
+	int data_len;
+	u64 buf_len = (sctx->count[0] % SHA512_BLOCK_SIZE);
 
 	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
 
@@ -66,16 +85,16 @@ static int nx_sha512_update(struct shash_desc *desc, const u8 *data,
 	 *  1: < SHA512_BLOCK_SIZE: copy into state, return 0
 	 *  2: >= SHA512_BLOCK_SIZE: process X blocks, copy in leftover
 	 */
-	total = sctx->count[0] + len;
+	total = (sctx->count[0] % SHA512_BLOCK_SIZE) + len;
 	if (total < SHA512_BLOCK_SIZE) {
-		memcpy(sctx->buf + sctx->count[0], data, len);
+		memcpy(sctx->buf + buf_len, data, len);
 		sctx->count[0] += len;
 		goto out;
 	}
 
-	in_sg = nx_ctx->in_sg;
-	max_sg_len = min_t(u32, nx_driver.of.max_sg_len/sizeof(struct nx_sg),
-			   nx_ctx->ap->sglen);
+	memcpy(csbcpb->cpb.sha512.message_digest, sctx->state, SHA512_DIGEST_SIZE);
+	NX_CPB_FDM(csbcpb) |= NX_FDM_INTERMEDIATE;
+	NX_CPB_FDM(csbcpb) |= NX_FDM_CONTINUATION;
 
 	do {
 		/*
@@ -83,34 +102,43 @@ static int nx_sha512_update(struct shash_desc *desc, const u8 *data,
 		 * this update. This value is also restricted by the sg list
 		 * limits.
 		 */
-		to_process = min_t(u64, total, nx_ctx->ap->databytelen);
-		to_process = min_t(u64, to_process,
-				   NX_PAGE_SIZE * (max_sg_len - 1));
+		to_process = total - leftover;
 		to_process = to_process & ~(SHA512_BLOCK_SIZE - 1);
 		leftover = total - to_process;
 
-		if (sctx->count[0]) {
-			in_sg = nx_build_sg_list(nx_ctx->in_sg,
-						 (u8 *) sctx->buf,
-						 sctx->count[0], max_sg_len);
+		if (buf_len) {
+			data_len = buf_len;
+			rc = nx_sha_build_sg_list(nx_ctx, nx_ctx->in_sg,
+						  &nx_ctx->op.inlen,
+						  &data_len,
+						  (u8 *) sctx->buf,
+						  NX_DS_SHA512);
+
+			if (rc || data_len != buf_len)
+				goto out;
 		}
-		in_sg = nx_build_sg_list(in_sg, (u8 *) data,
-					 to_process - sctx->count[0],
-					 max_sg_len);
-		nx_ctx->op.inlen = (nx_ctx->in_sg - in_sg) *
-					sizeof(struct nx_sg);
-
-		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,
+
+		data_len = to_process - buf_len;
+		rc = nx_sha_build_sg_list(nx_ctx, nx_ctx->in_sg,
+					  &nx_ctx->op.inlen,
+					  &data_len,
+					  (u8 *) data,
+					  NX_DS_SHA512);
+
+		if (rc || data_len != (to_process - buf_len))
+			goto out;
+
+		to_process = (data_len + buf_len);
+		leftover = total - to_process;
+
+		/*
+		 * 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);
-		}
 
-		NX_CPB_FDM(csbcpb) |= NX_FDM_INTERMEDIATE;
 		if (!nx_ctx->op.inlen || !nx_ctx->op.outlen) {
 			rc = -EINVAL;
 			goto out;
@@ -122,24 +150,18 @@ static int nx_sha512_update(struct shash_desc *desc, const u8 *data,
 			goto out;
 
 		atomic_inc(&(nx_ctx->stats->sha512_ops));
-		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;
 
 		total -= to_process;
-		data += to_process - sctx->count[0];
-		sctx->count[0] = 0;
-		in_sg = nx_ctx->in_sg;
+		data += to_process - buf_len;
+		buf_len = 0;
+
 	} while (leftover >= SHA512_BLOCK_SIZE);
 
 	/* copy the leftover back into the state struct */
 	if (leftover)
 		memcpy(sctx->buf, data, leftover);
-	sctx->count[0] = leftover;
+	sctx->count[0] += len;
+	memcpy(sctx->state, csbcpb->cpb.sha512.message_digest, SHA512_DIGEST_SIZE);
 out:
 	spin_unlock_irqrestore(&nx_ctx->lock, irq_flags);
 	return rc;
@@ -150,39 +172,52 @@ 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;
-	u32 max_sg_len;
 	u64 count0;
 	unsigned long irq_flags;
 	int rc;
+	int len;
 
 	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
 
-	max_sg_len = min_t(u32, nx_driver.of.max_sg_len, nx_ctx->ap->sglen);
-
-	if (NX_CPB_FDM(csbcpb) & NX_FDM_CONTINUATION) {
+	/* final is represented by continuing the operation and indicating that
+	 * this is not an intermediate operation */
+	if (sctx->count[0] >= SHA512_BLOCK_SIZE) {
 		/* 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);
+		memcpy(csbcpb->cpb.sha512.input_partial_digest, sctx->state,
+							SHA512_DIGEST_SIZE);
+		NX_CPB_FDM(csbcpb) &= ~NX_FDM_INTERMEDIATE;
+		NX_CPB_FDM(csbcpb) |= NX_FDM_CONTINUATION;
+	} else {
+		NX_CPB_FDM(csbcpb) &= ~NX_FDM_INTERMEDIATE;
+		NX_CPB_FDM(csbcpb) &= ~NX_FDM_CONTINUATION;
 	}
 
-	/* 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++;
+	csbcpb->cpb.sha512.message_bit_length_lo = count0;
 
-	in_sg = nx_build_sg_list(nx_ctx->in_sg, sctx->buf, sctx->count[0],
-				 max_sg_len);
-	out_sg = nx_build_sg_list(nx_ctx->out_sg, out, SHA512_DIGEST_SIZE,
-				  max_sg_len);
-	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);
+	len = sctx->count[0] & (SHA512_BLOCK_SIZE - 1);
+	rc = nx_sha_build_sg_list(nx_ctx, nx_ctx->in_sg,
+				  &nx_ctx->op.inlen,
+				  &len,
+				  (u8 *)sctx->buf,
+				  NX_DS_SHA512);
+
+	if (rc || len != (sctx->count[0] & (SHA512_BLOCK_SIZE - 1)))
+		goto out;
+
+	len = SHA512_DIGEST_SIZE;
+	rc = nx_sha_build_sg_list(nx_ctx, nx_ctx->out_sg,
+				  &nx_ctx->op.outlen,
+				  &len,
+				  out,
+				  NX_DS_SHA512);
+
+	if (rc)
+		goto out;
 
 	if (!nx_ctx->op.outlen) {
 		rc = -EINVAL;
@@ -195,8 +230,7 @@ static int nx_sha512_final(struct shash_desc *desc, u8 *out)
 		goto out;
 
 	atomic_inc(&(nx_ctx->stats->sha512_ops));
-	atomic64_add(csbcpb->cpb.sha512.message_bit_length_lo / 8,
-		     &(nx_ctx->stats->sha512_bytes));
+	atomic64_add(sctx->count[0], &(nx_ctx->stats->sha512_bytes));
 
 	memcpy(out, csbcpb->cpb.sha512.message_digest, SHA512_DIGEST_SIZE);
 out:
@@ -207,74 +241,18 @@ out:
 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;
-	unsigned long irq_flags;
 
-	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
+	memcpy(out, sctx, sizeof(*sctx));
 
-	/* 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;
-	}
-
-	spin_unlock_irqrestore(&nx_ctx->lock, irq_flags);
 	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;
-	unsigned long irq_flags;
-
-	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
-
-	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;
-	}
+	memcpy(sctx, in, sizeof(*sctx));
 
-	spin_unlock_irqrestore(&nx_ctx->lock, irq_flags);
 	return 0;
 }
 
diff --git a/drivers/crypto/nx/nx.c b/drivers/crypto/nx/nx.c
index 5533fe31c90d..a392465d3e3f 100644
--- a/drivers/crypto/nx/nx.c
+++ b/drivers/crypto/nx/nx.c
@@ -90,7 +90,7 @@ int nx_hcall_sync(struct nx_crypto_ctx *nx_ctx,
  */
 struct nx_sg *nx_build_sg_list(struct nx_sg *sg_head,
 			       u8           *start_addr,
-			       unsigned int  len,
+			       unsigned int *len,
 			       u32           sgmax)
 {
 	unsigned int sg_len = 0;
@@ -106,7 +106,7 @@ struct nx_sg *nx_build_sg_list(struct nx_sg *sg_head,
 	else
 		sg_addr = __pa(sg_addr);
 
-	end_addr = sg_addr + len;
+	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
@@ -118,7 +118,7 @@ struct nx_sg *nx_build_sg_list(struct nx_sg *sg_head,
 	 * Also when using vmalloc'ed data, every time that a system page
 	 * boundary is crossed the physical address needs to be re-calculated.
 	 */
-	for (sg = sg_head; sg_len < len; sg++) {
+	for (sg = sg_head; sg_len < *len; sg++) {
 		u64 next_page;
 
 		sg->addr = sg_addr;
@@ -133,15 +133,17 @@ struct nx_sg *nx_build_sg_list(struct nx_sg *sg_head,
 				is_vmalloc_addr(start_addr + sg_len)) {
 			sg_addr = page_to_phys(vmalloc_to_page(
 						start_addr + sg_len));
-			end_addr = sg_addr + len - sg_len;
+			end_addr = sg_addr + *len - sg_len;
 		}
 
 		if ((sg - sg_head) == sgmax) {
 			pr_err("nx: scatter/gather list overflow, pid: %d\n",
 			       current->pid);
-			return NULL;
+			sg++;
+			break;
 		}
 	}
+	*len = sg_len;
 
 	/* return the moved sg_head pointer */
 	return sg;
@@ -160,11 +162,11 @@ 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)
+				unsigned int       *src_len)
 {
 	struct scatter_walk walk;
 	struct nx_sg *nx_sg = nx_dst;
-	unsigned int n, offset = 0, len = src_len;
+	unsigned int n, offset = 0, len = *src_len;
 	char *dst;
 
 	/* we need to fast forward through @start bytes first */
@@ -182,27 +184,101 @@ struct nx_sg *nx_walk_and_build(struct nx_sg       *nx_dst,
 	 * element we're currently looking at */
 	scatterwalk_advance(&walk, start - offset);
 
-	while (len && nx_sg) {
+	while (len && (nx_sg - nx_dst) < sglen) {
 		n = scatterwalk_clamp(&walk, len);
 		if (!n) {
-			scatterwalk_start(&walk, sg_next(walk.sg));
+			/* In cases where we have scatterlist chain scatterwalk_sg_next
+			 * handles with it properly */
+			scatterwalk_start(&walk, scatterwalk_sg_next(walk.sg));
 			n = scatterwalk_clamp(&walk, len);
 		}
 		dst = scatterwalk_map(&walk);
 
-		nx_sg = nx_build_sg_list(nx_sg, dst, n, sglen);
+		nx_sg = nx_build_sg_list(nx_sg, dst, &n, sglen - (nx_sg - nx_dst));
 		len -= n;
 
 		scatterwalk_unmap(dst);
 		scatterwalk_advance(&walk, n);
 		scatterwalk_done(&walk, SCATTERWALK_FROM_SG, len);
 	}
+	/* update to_process */
+	*src_len -= len;
 
 	/* return the moved destination pointer */
 	return nx_sg;
 }
 
 /**
+ * trim_sg_list - ensures the bound in sg list.
+ * @sg: sg list head
+ * @end: sg lisg end
+ * @delta:  is the amount we need to crop in order to bound the list.
+ *
+ */
+static long int trim_sg_list(struct nx_sg *sg, struct nx_sg *end, unsigned int delta)
+{
+	while (delta && end > sg) {
+		struct nx_sg *last = end - 1;
+
+		if (last->len > delta) {
+			last->len -= delta;
+			delta = 0;
+		} else {
+			end--;
+			delta -= last->len;
+		}
+	}
+	return (sg - end) * sizeof(struct nx_sg);
+}
+
+/**
+ * nx_sha_build_sg_list - walk and build sg list to sha modes
+ *			  using right bounds and limits.
+ * @nx_ctx: NX crypto context for the lists we're building
+ * @nx_sg: current sg list in or out list
+ * @op_len: current op_len to be used in order to build a sg list
+ * @nbytes:  number or bytes to be processed
+ * @offset: buf offset
+ * @mode: SHA256 or SHA512
+ */
+int nx_sha_build_sg_list(struct nx_crypto_ctx *nx_ctx,
+			  struct nx_sg 	      *nx_in_outsg,
+			  s64		      *op_len,
+			  unsigned int        *nbytes,
+			  u8 		      *offset,
+			  u32		      mode)
+{
+	unsigned int delta = 0;
+	unsigned int total = *nbytes;
+	struct nx_sg *nx_insg = nx_in_outsg;
+	unsigned int max_sg_len;
+
+	max_sg_len = min_t(u64, nx_ctx->ap->sglen,
+			nx_driver.of.max_sg_len/sizeof(struct nx_sg));
+	max_sg_len = min_t(u64, max_sg_len,
+			nx_ctx->ap->databytelen/NX_PAGE_SIZE);
+
+	*nbytes = min_t(u64, *nbytes, nx_ctx->ap->databytelen);
+	nx_insg = nx_build_sg_list(nx_insg, offset, nbytes, max_sg_len);
+
+	switch (mode) {
+	case NX_DS_SHA256:
+		if (*nbytes < total)
+			delta = *nbytes - (*nbytes & ~(SHA256_BLOCK_SIZE - 1));
+		break;
+	case NX_DS_SHA512:
+		if (*nbytes < total)
+			delta = *nbytes - (*nbytes & ~(SHA512_BLOCK_SIZE - 1));
+		break;
+	default:
+		return -EINVAL;
+	}
+	*op_len = trim_sg_list(nx_in_outsg, nx_insg, delta);
+
+	return 0;
+}
+
+/**
  * nx_build_sg_lists - walk the input scatterlists and build arrays of NX
  *                     scatterlists based on them.
  *
@@ -223,26 +299,39 @@ int nx_build_sg_lists(struct nx_crypto_ctx  *nx_ctx,
 		      struct blkcipher_desc *desc,
 		      struct scatterlist    *dst,
 		      struct scatterlist    *src,
-		      unsigned int           nbytes,
+		      unsigned int          *nbytes,
 		      unsigned int           offset,
 		      u8                    *iv)
 {
+	unsigned int delta = 0;
+	unsigned int total = *nbytes;
 	struct nx_sg *nx_insg = nx_ctx->in_sg;
 	struct nx_sg *nx_outsg = nx_ctx->out_sg;
+	unsigned int max_sg_len;
+
+	max_sg_len = min_t(u64, nx_ctx->ap->sglen,
+			nx_driver.of.max_sg_len/sizeof(struct nx_sg));
+	max_sg_len = min_t(u64, max_sg_len,
+			nx_ctx->ap->databytelen/NX_PAGE_SIZE);
 
 	if (iv)
 		memcpy(iv, desc->info, AES_BLOCK_SIZE);
 
-	nx_insg = nx_walk_and_build(nx_insg, nx_ctx->ap->sglen, src,
-				    offset, nbytes);
-	nx_outsg = nx_walk_and_build(nx_outsg, nx_ctx->ap->sglen, dst,
-				    offset, nbytes);
+	*nbytes = min_t(u64, *nbytes, nx_ctx->ap->databytelen);
+
+	nx_outsg = nx_walk_and_build(nx_outsg, max_sg_len, dst,
+					offset, nbytes);
+	nx_insg = nx_walk_and_build(nx_insg, max_sg_len, src,
+					offset, nbytes);
+
+	if (*nbytes < total)
+		delta = *nbytes - (*nbytes & ~(AES_BLOCK_SIZE - 1));
 
 	/* 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);
+	nx_ctx->op.inlen = trim_sg_list(nx_ctx->in_sg, nx_insg, delta);
+	nx_ctx->op.outlen = trim_sg_list(nx_ctx->out_sg, nx_outsg, delta);
 
 	return 0;
 }
@@ -540,10 +629,10 @@ static int nx_crypto_ctx_init(struct nx_crypto_ctx *nx_ctx, u32 fc, u32 mode)
 
 	/* 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) +
+		nx_ctx->kmem_len = (5 * NX_PAGE_SIZE) +
 				   sizeof(struct nx_csbcpb);
 	else
-		nx_ctx->kmem_len = (3 * NX_PAGE_SIZE) +
+		nx_ctx->kmem_len = (4 * NX_PAGE_SIZE) +
 				   sizeof(struct nx_csbcpb);
 
 	nx_ctx->kmem = kmalloc(nx_ctx->kmem_len, GFP_KERNEL);
diff --git a/drivers/crypto/nx/nx.h b/drivers/crypto/nx/nx.h
index befda07ca1da..6c9ecaaead52 100644
--- a/drivers/crypto/nx/nx.h
+++ b/drivers/crypto/nx/nx.h
@@ -153,13 +153,15 @@ 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_sha_build_sg_list(struct nx_crypto_ctx *, struct nx_sg *,
+			 s64 *, unsigned int *, u8 *, u32);
+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,
+		      struct scatterlist *, struct scatterlist *, unsigned int *,
 		      unsigned int, u8 *);
 struct nx_sg *nx_walk_and_build(struct nx_sg *, unsigned int,
 				struct scatterlist *, unsigned int,
-				unsigned int);
+				unsigned int *);
 
 #ifdef CONFIG_DEBUG_FS
 #define NX_DEBUGFS_INIT(drv)	nx_debugfs_init(drv)
diff --git a/drivers/crypto/omap-aes.c b/drivers/crypto/omap-aes.c
index cb98fa54573d..f79dd410dede 100644
--- a/drivers/crypto/omap-aes.c
+++ b/drivers/crypto/omap-aes.c
@@ -1314,7 +1314,6 @@ static struct platform_driver omap_aes_driver = {
 	.remove	= omap_aes_remove,
 	.driver	= {
 		.name	= "omap-aes",
-		.owner	= THIS_MODULE,
 		.pm	= &omap_aes_pm_ops,
 		.of_match_table	= omap_aes_of_match,
 	},
diff --git a/drivers/crypto/omap-des.c b/drivers/crypto/omap-des.c
index b8bc84be8741..e350f5be4d2e 100644
--- a/drivers/crypto/omap-des.c
+++ b/drivers/crypto/omap-des.c
@@ -1222,7 +1222,6 @@ static struct platform_driver omap_des_driver = {
 	.remove	= omap_des_remove,
 	.driver	= {
 		.name	= "omap-des",
-		.owner	= THIS_MODULE,
 		.pm	= &omap_des_pm_ops,
 		.of_match_table	= of_match_ptr(omap_des_of_match),
 	},
diff --git a/drivers/crypto/omap-sham.c b/drivers/crypto/omap-sham.c
index 24ef48965e45..3c76696ee578 100644
--- a/drivers/crypto/omap-sham.c
+++ b/drivers/crypto/omap-sham.c
@@ -2029,7 +2029,6 @@ static struct platform_driver omap_sham_driver = {
 	.remove	= omap_sham_remove,
 	.driver	= {
 		.name	= "omap-sham",
-		.owner	= THIS_MODULE,
 		.pm	= &omap_sham_pm_ops,
 		.of_match_table	= omap_sham_of_match,
 	},
diff --git a/drivers/crypto/padlock-aes.c b/drivers/crypto/padlock-aes.c
index 633ba945e153..c178ed8c3908 100644
--- a/drivers/crypto/padlock-aes.c
+++ b/drivers/crypto/padlock-aes.c
@@ -563,4 +563,4 @@ MODULE_DESCRIPTION("VIA PadLock AES algorithm support");
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Michal Ludvig");
 
-MODULE_ALIAS("aes");
+MODULE_ALIAS_CRYPTO("aes");
diff --git a/drivers/crypto/padlock-sha.c b/drivers/crypto/padlock-sha.c
index bace885634f2..95f7d27ce491 100644
--- a/drivers/crypto/padlock-sha.c
+++ b/drivers/crypto/padlock-sha.c
@@ -593,7 +593,7 @@ MODULE_DESCRIPTION("VIA PadLock SHA1/SHA256 algorithms support.");
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Michal Ludvig");
 
-MODULE_ALIAS("sha1-all");
-MODULE_ALIAS("sha256-all");
-MODULE_ALIAS("sha1-padlock");
-MODULE_ALIAS("sha256-padlock");
+MODULE_ALIAS_CRYPTO("sha1-all");
+MODULE_ALIAS_CRYPTO("sha256-all");
+MODULE_ALIAS_CRYPTO("sha1-padlock");
+MODULE_ALIAS_CRYPTO("sha256-padlock");
diff --git a/drivers/crypto/qat/qat_common/adf_accel_devices.h b/drivers/crypto/qat/qat_common/adf_accel_devices.h
index fe7b3f06f6e6..2ed425664a16 100644
--- a/drivers/crypto/qat/qat_common/adf_accel_devices.h
+++ b/drivers/crypto/qat/qat_common/adf_accel_devices.h
@@ -56,8 +56,6 @@
 #define PCI_VENDOR_ID_INTEL 0x8086
 #define ADF_DH895XCC_DEVICE_NAME "dh895xcc"
 #define ADF_DH895XCC_PCI_DEVICE_ID 0x435
-#define ADF_DH895XCC_PMISC_BAR 1
-#define ADF_DH895XCC_ETR_BAR 2
 #define ADF_PCI_MAX_BARS 3
 #define ADF_DEVICE_NAME_LENGTH 32
 #define ADF_ETR_MAX_RINGS_PER_BANK 16
diff --git a/drivers/crypto/qat/qat_common/adf_aer.c b/drivers/crypto/qat/qat_common/adf_aer.c
index c29d4c3926bf..10ce4a2854ab 100644
--- a/drivers/crypto/qat/qat_common/adf_aer.c
+++ b/drivers/crypto/qat/qat_common/adf_aer.c
@@ -90,7 +90,7 @@ static void adf_dev_restore(struct adf_accel_dev *accel_dev)
 	uint16_t ppdstat = 0, bridge_ctl = 0;
 	int pending = 0;
 
-	pr_info("QAT: Reseting device qat_dev%d\n", accel_dev->accel_id);
+	pr_info("QAT: Resetting device qat_dev%d\n", accel_dev->accel_id);
 	pci_read_config_word(pdev, PPDSTAT_OFFSET, &ppdstat);
 	pending = ppdstat & PCI_EXP_DEVSTA_TRPND;
 	if (pending) {
diff --git a/drivers/crypto/qat/qat_common/adf_ctl_drv.c b/drivers/crypto/qat/qat_common/adf_ctl_drv.c
index 244d73378f0e..7ee93f881db6 100644
--- a/drivers/crypto/qat/qat_common/adf_ctl_drv.c
+++ b/drivers/crypto/qat/qat_common/adf_ctl_drv.c
@@ -52,6 +52,7 @@
 #include <linux/pci.h>
 #include <linux/cdev.h>
 #include <linux/uaccess.h>
+#include <linux/crypto.h>
 
 #include "adf_accel_devices.h"
 #include "adf_common_drv.h"
@@ -487,4 +488,4 @@ module_exit(adf_unregister_ctl_device_driver);
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_AUTHOR("Intel");
 MODULE_DESCRIPTION("Intel(R) QuickAssist Technology");
-MODULE_ALIAS("intel_qat");
+MODULE_ALIAS_CRYPTO("intel_qat");
diff --git a/drivers/crypto/qat/qat_common/adf_dev_mgr.c b/drivers/crypto/qat/qat_common/adf_dev_mgr.c
index ae71555c0868..4a0a829d4500 100644
--- a/drivers/crypto/qat/qat_common/adf_dev_mgr.c
+++ b/drivers/crypto/qat/qat_common/adf_dev_mgr.c
@@ -129,12 +129,13 @@ struct adf_accel_dev *adf_devmgr_get_first(void)
  * Function returns acceleration device associated with the given pci device.
  * To be used by QAT device specific drivers.
  *
- * Return: pinter to accel_dev or NULL if not found.
+ * Return: pointer to accel_dev or NULL if not found.
  */
 struct adf_accel_dev *adf_devmgr_pci_to_accel_dev(struct pci_dev *pci_dev)
 {
 	struct list_head *itr;
 
+	mutex_lock(&table_lock);
 	list_for_each(itr, &accel_table) {
 		struct adf_accel_dev *ptr =
 				list_entry(itr, struct adf_accel_dev, list);
@@ -144,6 +145,7 @@ struct adf_accel_dev *adf_devmgr_pci_to_accel_dev(struct pci_dev *pci_dev)
 			return ptr;
 		}
 	}
+	mutex_unlock(&table_lock);
 	return NULL;
 }
 EXPORT_SYMBOL_GPL(adf_devmgr_pci_to_accel_dev);
@@ -152,6 +154,7 @@ struct adf_accel_dev *adf_devmgr_get_dev_by_id(uint32_t id)
 {
 	struct list_head *itr;
 
+	mutex_lock(&table_lock);
 	list_for_each(itr, &accel_table) {
 		struct adf_accel_dev *ptr =
 				list_entry(itr, struct adf_accel_dev, list);
@@ -161,6 +164,7 @@ struct adf_accel_dev *adf_devmgr_get_dev_by_id(uint32_t id)
 			return ptr;
 		}
 	}
+	mutex_unlock(&table_lock);
 	return NULL;
 }
 
diff --git a/drivers/crypto/qat/qat_common/adf_transport.c b/drivers/crypto/qat/qat_common/adf_transport.c
index 9dd2cb72a4e8..7dd54aaee9fa 100644
--- a/drivers/crypto/qat/qat_common/adf_transport.c
+++ b/drivers/crypto/qat/qat_common/adf_transport.c
@@ -376,8 +376,9 @@ static inline int adf_get_cfg_int(struct adf_accel_dev *accel_dev,
 	return 0;
 }
 
-static void adf_enable_coalesc(struct adf_etr_bank_data *bank,
-			       const char *section, uint32_t bank_num_in_accel)
+static void adf_get_coalesc_timer(struct adf_etr_bank_data *bank,
+				  const char *section,
+				  uint32_t bank_num_in_accel)
 {
 	if (adf_get_cfg_int(bank->accel_dev, section,
 			    ADF_ETRMGR_COALESCE_TIMER_FORMAT,
@@ -396,7 +397,7 @@ static int adf_init_bank(struct adf_accel_dev *accel_dev,
 	struct adf_hw_device_data *hw_data = accel_dev->hw_device;
 	struct adf_etr_ring_data *ring;
 	struct adf_etr_ring_data *tx_ring;
-	uint32_t i, coalesc_enabled;
+	uint32_t i, coalesc_enabled = 0;
 
 	memset(bank, 0, sizeof(*bank));
 	bank->bank_number = bank_num;
@@ -407,10 +408,10 @@ static int adf_init_bank(struct adf_accel_dev *accel_dev,
 	/* Enable IRQ coalescing always. This will allow to use
 	 * the optimised flag and coalesc register.
 	 * If it is disabled in the config file just use min time value */
-	if (adf_get_cfg_int(accel_dev, "Accelerator0",
-			    ADF_ETRMGR_COALESCING_ENABLED_FORMAT,
-			    bank_num, &coalesc_enabled) && coalesc_enabled)
-		adf_enable_coalesc(bank, "Accelerator0", bank_num);
+	if ((adf_get_cfg_int(accel_dev, "Accelerator0",
+			     ADF_ETRMGR_COALESCING_ENABLED_FORMAT, bank_num,
+			     &coalesc_enabled) == 0) && coalesc_enabled)
+		adf_get_coalesc_timer(bank, "Accelerator0", bank_num);
 	else
 		bank->irq_coalesc_timer = ADF_COALESCING_MIN_TIME;
 
diff --git a/drivers/crypto/qat/qat_common/adf_transport_access_macros.h b/drivers/crypto/qat/qat_common/adf_transport_access_macros.h
index 91d88d676580..160c9a36c919 100644
--- a/drivers/crypto/qat/qat_common/adf_transport_access_macros.h
+++ b/drivers/crypto/qat/qat_common/adf_transport_access_macros.h
@@ -83,14 +83,14 @@
 #define ADF_MAX_RING_SIZE ADF_RING_SIZE_4M
 #define ADF_DEFAULT_RING_SIZE ADF_RING_SIZE_16K
 
-/* Valid internal msg size values internal */
+/* Valid internal msg size values */
 #define ADF_MSG_SIZE_32 0x01
 #define ADF_MSG_SIZE_64 0x02
 #define ADF_MSG_SIZE_128 0x04
 #define ADF_MIN_MSG_SIZE ADF_MSG_SIZE_32
 #define ADF_MAX_MSG_SIZE ADF_MSG_SIZE_128
 
-/* Size to bytes conversion macros for ring and msg values */
+/* Size to bytes conversion macros for ring and msg size values */
 #define ADF_MSG_SIZE_TO_BYTES(SIZE) (SIZE << 5)
 #define ADF_BYTES_TO_MSG_SIZE(SIZE) (SIZE >> 5)
 #define ADF_SIZE_TO_RING_SIZE_IN_BYTES(SIZE) ((1 << (SIZE - 1)) << 7)
@@ -100,8 +100,11 @@
 #define ADF_RING_SIZE_BYTES_MIN(SIZE) ((SIZE < ADF_RING_SIZE_4K) ? \
 				ADF_RING_SIZE_4K : SIZE)
 #define ADF_RING_SIZE_MODULO(SIZE) (SIZE + 0x6)
+#define ADF_SIZE_TO_POW(SIZE) ((((SIZE & 0x4) >> 1) | ((SIZE & 0x4) >> 2) | \
+				SIZE) & ~0x4)
+/* Max outstanding requests */
 #define ADF_MAX_INFLIGHTS(RING_SIZE, MSG_SIZE) \
-	((((1 << (RING_SIZE - 1)) << 4) >> MSG_SIZE) - 1)
+	((((1 << (RING_SIZE - 1)) << 3) >> ADF_SIZE_TO_POW(MSG_SIZE)) - 1)
 #define BUILD_RING_CONFIG(size)	\
 	((ADF_RING_NEAR_WATERMARK_0 << ADF_RING_CONFIG_NEAR_FULL_WM) \
 	| (ADF_RING_NEAR_WATERMARK_0 << ADF_RING_CONFIG_NEAR_EMPTY_WM) \
diff --git a/drivers/crypto/qat/qat_common/qat_algs.c b/drivers/crypto/qat/qat_common/qat_algs.c
index 9e9619cd4a79..19eea1c832ac 100644
--- a/drivers/crypto/qat/qat_common/qat_algs.c
+++ b/drivers/crypto/qat/qat_common/qat_algs.c
@@ -161,7 +161,7 @@ static int qat_alg_do_precomputes(struct icp_qat_hw_auth_algo_blk *hash,
 	__be64 *hash512_state_out;
 	int i, offset;
 
-	memset(auth_state.data, '\0', MAX_AUTH_STATE_SIZE + 64);
+	memzero_explicit(auth_state.data, MAX_AUTH_STATE_SIZE + 64);
 	shash->tfm = ctx->hash_tfm;
 	shash->flags = 0x0;
 
@@ -174,13 +174,13 @@ static int qat_alg_do_precomputes(struct icp_qat_hw_auth_algo_blk *hash,
 
 		memcpy(ipad, buff, digest_size);
 		memcpy(opad, buff, digest_size);
-		memset(ipad + digest_size, 0, block_size - digest_size);
-		memset(opad + digest_size, 0, block_size - digest_size);
+		memzero_explicit(ipad + digest_size, block_size - digest_size);
+		memzero_explicit(opad + digest_size, block_size - digest_size);
 	} else {
 		memcpy(ipad, auth_key, auth_keylen);
 		memcpy(opad, auth_key, auth_keylen);
-		memset(ipad + auth_keylen, 0, block_size - auth_keylen);
-		memset(opad + auth_keylen, 0, block_size - auth_keylen);
+		memzero_explicit(ipad + auth_keylen, block_size - auth_keylen);
+		memzero_explicit(opad + auth_keylen, block_size - auth_keylen);
 	}
 
 	for (i = 0; i < block_size; i++) {
@@ -254,6 +254,8 @@ static int qat_alg_do_precomputes(struct icp_qat_hw_auth_algo_blk *hash,
 	default:
 		return -EFAULT;
 	}
+	memzero_explicit(ipad, block_size);
+	memzero_explicit(opad, block_size);
 	return 0;
 }
 
@@ -466,7 +468,6 @@ static int qat_alg_init_sessions(struct qat_alg_session_ctx *ctx,
 		break;
 	default:
 		goto bad_key;
-		break;
 	}
 
 	if (qat_alg_init_enc_session(ctx, alg, &keys))
@@ -493,12 +494,12 @@ static int qat_alg_setkey(struct crypto_aead *tfm, const uint8_t *key,
 	if (ctx->enc_cd) {
 		/* rekeying */
 		dev = &GET_DEV(ctx->inst->accel_dev);
-		memset(ctx->enc_cd, 0, sizeof(struct qat_alg_cd));
-		memset(ctx->dec_cd, 0, sizeof(struct qat_alg_cd));
-		memset(&ctx->enc_fw_req_tmpl, 0,
-		       sizeof(struct icp_qat_fw_la_bulk_req));
-		memset(&ctx->dec_fw_req_tmpl, 0,
-		       sizeof(struct icp_qat_fw_la_bulk_req));
+		memzero_explicit(ctx->enc_cd, sizeof(struct qat_alg_cd));
+		memzero_explicit(ctx->dec_cd, sizeof(struct qat_alg_cd));
+		memzero_explicit(&ctx->enc_fw_req_tmpl,
+				 sizeof(struct icp_qat_fw_la_bulk_req));
+		memzero_explicit(&ctx->dec_fw_req_tmpl,
+				 sizeof(struct icp_qat_fw_la_bulk_req));
 	} else {
 		/* new key */
 		int node = get_current_node();
@@ -535,10 +536,12 @@ static int qat_alg_setkey(struct crypto_aead *tfm, const uint8_t *key,
 	return 0;
 
 out_free_all:
+	memzero_explicit(ctx->dec_cd, sizeof(struct qat_alg_cd));
 	dma_free_coherent(dev, sizeof(struct qat_alg_cd),
 			  ctx->dec_cd, ctx->dec_cd_paddr);
 	ctx->dec_cd = NULL;
 out_free_enc:
+	memzero_explicit(ctx->enc_cd, sizeof(struct qat_alg_cd));
 	dma_free_coherent(dev, sizeof(struct qat_alg_cd),
 			  ctx->enc_cd, ctx->enc_cd_paddr);
 	ctx->enc_cd = NULL;
@@ -836,7 +839,7 @@ static int qat_alg_init(struct crypto_tfm *tfm,
 {
 	struct qat_alg_session_ctx *ctx = crypto_tfm_ctx(tfm);
 
-	memset(ctx, '\0', sizeof(*ctx));
+	memzero_explicit(ctx, sizeof(*ctx));
 	ctx->hash_tfm = crypto_alloc_shash(hash_name, 0, 0);
 	if (IS_ERR(ctx->hash_tfm))
 		return -EFAULT;
@@ -876,12 +879,16 @@ static void qat_alg_exit(struct crypto_tfm *tfm)
 		return;
 
 	dev = &GET_DEV(inst->accel_dev);
-	if (ctx->enc_cd)
+	if (ctx->enc_cd) {
+		memzero_explicit(ctx->enc_cd, sizeof(struct qat_alg_cd));
 		dma_free_coherent(dev, sizeof(struct qat_alg_cd),
 				  ctx->enc_cd, ctx->enc_cd_paddr);
-	if (ctx->dec_cd)
+	}
+	if (ctx->dec_cd) {
+		memzero_explicit(ctx->dec_cd, sizeof(struct qat_alg_cd));
 		dma_free_coherent(dev, sizeof(struct qat_alg_cd),
 				  ctx->dec_cd, ctx->dec_cd_paddr);
+	}
 	qat_crypto_put_instance(inst);
 }
 
diff --git a/drivers/crypto/qat/qat_common/qat_hal.c b/drivers/crypto/qat/qat_common/qat_hal.c
index 9b8a31521ff3..b818c19713bf 100644
--- a/drivers/crypto/qat/qat_common/qat_hal.c
+++ b/drivers/crypto/qat/qat_common/qat_hal.c
@@ -679,7 +679,8 @@ int qat_hal_init(struct adf_accel_dev *accel_dev)
 	struct icp_qat_fw_loader_handle *handle;
 	struct adf_accel_pci *pci_info = &accel_dev->accel_pci_dev;
 	struct adf_hw_device_data *hw_data = accel_dev->hw_device;
-	struct adf_bar *bar = &pci_info->pci_bars[ADF_DH895XCC_PMISC_BAR];
+	struct adf_bar *bar =
+			&pci_info->pci_bars[hw_data->get_misc_bar_id(hw_data)];
 
 	handle = kzalloc(sizeof(*handle), GFP_KERNEL);
 	if (!handle)
diff --git a/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.h b/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.h
index 65dd1ff93d3b..01e0be21e93a 100644
--- a/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.h
+++ b/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.h
@@ -48,6 +48,8 @@
 #define ADF_DH895x_HW_DATA_H_
 
 /* PCIe configuration space */
+#define ADF_DH895XCC_PMISC_BAR 1
+#define ADF_DH895XCC_ETR_BAR 2
 #define ADF_DH895XCC_RX_RINGS_OFFSET 8
 #define ADF_DH895XCC_TX_RINGS_MASK 0xFF
 #define ADF_DH895XCC_FUSECTL_OFFSET 0x40
diff --git a/drivers/crypto/qat/qat_dh895xcc/adf_isr.c b/drivers/crypto/qat/qat_dh895xcc/adf_isr.c
index d96ee21b9b77..fe8f89697ad8 100644
--- a/drivers/crypto/qat/qat_dh895xcc/adf_isr.c
+++ b/drivers/crypto/qat/qat_dh895xcc/adf_isr.c
@@ -186,10 +186,8 @@ static int adf_isr_alloc_msix_entry_table(struct adf_accel_dev *accel_dev)
 	accel_dev->accel_pci_dev.msix_entries.names = names;
 	return 0;
 err:
-	for (i = 0; i < msix_num_entries; i++) {
-		if (*(names + i))
-			kfree(*(names + i));
-	}
+	for (i = 0; i < msix_num_entries; i++)
+		kfree(*(names + i));
 	kfree(entries);
 	kfree(names);
 	return -ENOMEM;
@@ -203,10 +201,8 @@ static void adf_isr_free_msix_entry_table(struct adf_accel_dev *accel_dev)
 	int i;
 
 	kfree(accel_dev->accel_pci_dev.msix_entries.entries);
-	for (i = 0; i < msix_num_entries; i++) {
-		if (*(names + i))
-			kfree(*(names + i));
-	}
+	for (i = 0; i < msix_num_entries; i++)
+		kfree(*(names + i));
 	kfree(names);
 }
 
diff --git a/drivers/crypto/qce/core.c b/drivers/crypto/qce/core.c
index 33ae3545dc48..718b32a3112e 100644
--- a/drivers/crypto/qce/core.c
+++ b/drivers/crypto/qce/core.c
@@ -273,7 +273,6 @@ static struct platform_driver qce_crypto_driver = {
 	.probe = qce_crypto_probe,
 	.remove = qce_crypto_remove,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = KBUILD_MODNAME,
 		.of_match_table = qce_crypto_of_match,
 	},
diff --git a/drivers/crypto/s5p-sss.c b/drivers/crypto/s5p-sss.c
index 4197ad9a711b..f214a8755827 100644
--- a/drivers/crypto/s5p-sss.c
+++ b/drivers/crypto/s5p-sss.c
@@ -745,7 +745,6 @@ static struct platform_driver s5p_aes_crypto = {
 	.probe	= s5p_aes_probe,
 	.remove	= s5p_aes_remove,
 	.driver	= {
-		.owner	= THIS_MODULE,
 		.name	= "s5p-secss",
 		.of_match_table = s5p_sss_dt_match,
 	},
diff --git a/drivers/crypto/sahara.c b/drivers/crypto/sahara.c
index 164e1ec624e3..220b92f7eabc 100644
--- a/drivers/crypto/sahara.c
+++ b/drivers/crypto/sahara.c
@@ -3,6 +3,7 @@
  *
  * Support for SAHARA cryptographic accelerator.
  *
+ * Copyright (c) 2014 Steffen Trumtrar <s.trumtrar@pengutronix.de>
  * Copyright (c) 2013 Vista Silicon S.L.
  * Author: Javier Martin <javier.martin@vista-silicon.com>
  *
@@ -15,6 +16,10 @@
 
 #include <crypto/algapi.h>
 #include <crypto/aes.h>
+#include <crypto/hash.h>
+#include <crypto/internal/hash.h>
+#include <crypto/scatterwalk.h>
+#include <crypto/sha.h>
 
 #include <linux/clk.h>
 #include <linux/crypto.h>
@@ -22,12 +27,19 @@
 #include <linux/io.h>
 #include <linux/irq.h>
 #include <linux/kernel.h>
+#include <linux/kthread.h>
 #include <linux/module.h>
+#include <linux/mutex.h>
 #include <linux/of.h>
+#include <linux/of_device.h>
 #include <linux/platform_device.h>
 
+#define SHA_BUFFER_LEN		PAGE_SIZE
+#define SAHARA_MAX_SHA_BLOCK_SIZE	SHA256_BLOCK_SIZE
+
 #define SAHARA_NAME "sahara"
 #define SAHARA_VERSION_3	3
+#define SAHARA_VERSION_4	4
 #define SAHARA_TIMEOUT_MS	1000
 #define SAHARA_MAX_HW_DESC	2
 #define SAHARA_MAX_HW_LINK	20
@@ -36,7 +48,6 @@
 #define FLAGS_ENCRYPT		BIT(0)
 #define FLAGS_CBC		BIT(1)
 #define FLAGS_NEW_KEY		BIT(3)
-#define FLAGS_BUSY		4
 
 #define SAHARA_HDR_BASE			0x00800000
 #define SAHARA_HDR_SKHA_ALG_AES	0
@@ -50,6 +61,23 @@
 #define SAHARA_HDR_CHA_MDHA		(2 << 28)
 #define SAHARA_HDR_PARITY_BIT		(1 << 31)
 
+#define SAHARA_HDR_MDHA_SET_MODE_MD_KEY	0x20880000
+#define SAHARA_HDR_MDHA_SET_MODE_HASH	0x208D0000
+#define SAHARA_HDR_MDHA_HASH		0xA0850000
+#define SAHARA_HDR_MDHA_STORE_DIGEST	0x20820000
+#define SAHARA_HDR_MDHA_ALG_SHA1	0
+#define SAHARA_HDR_MDHA_ALG_MD5		1
+#define SAHARA_HDR_MDHA_ALG_SHA256	2
+#define SAHARA_HDR_MDHA_ALG_SHA224	3
+#define SAHARA_HDR_MDHA_PDATA		(1 << 2)
+#define SAHARA_HDR_MDHA_HMAC		(1 << 3)
+#define SAHARA_HDR_MDHA_INIT		(1 << 5)
+#define SAHARA_HDR_MDHA_IPAD		(1 << 6)
+#define SAHARA_HDR_MDHA_OPAD		(1 << 7)
+#define SAHARA_HDR_MDHA_SWAP		(1 << 8)
+#define SAHARA_HDR_MDHA_MAC_FULL	(1 << 9)
+#define SAHARA_HDR_MDHA_SSL		(1 << 10)
+
 /* SAHARA can only process one request at a time */
 #define SAHARA_QUEUE_LENGTH	1
 
@@ -117,31 +145,74 @@ struct sahara_hw_link {
 };
 
 struct sahara_ctx {
-	struct sahara_dev *dev;
 	unsigned long flags;
+
+	/* AES-specific context */
 	int keylen;
 	u8 key[AES_KEYSIZE_128];
 	struct crypto_ablkcipher *fallback;
+
+	/* SHA-specific context */
+	struct crypto_shash *shash_fallback;
 };
 
 struct sahara_aes_reqctx {
 	unsigned long mode;
 };
 
+/*
+ * struct sahara_sha_reqctx - private data per request
+ * @buf: holds data for requests smaller than block_size
+ * @rembuf: used to prepare one block_size-aligned request
+ * @context: hw-specific context for request. Digest is extracted from this
+ * @mode: specifies what type of hw-descriptor needs to be built
+ * @digest_size: length of digest for this request
+ * @context_size: length of hw-context for this request.
+ *                Always digest_size + 4
+ * @buf_cnt: number of bytes saved in buf
+ * @sg_in_idx: number of hw links
+ * @in_sg: scatterlist for input data
+ * @in_sg_chain: scatterlists for chained input data
+ * @in_sg_chained: specifies if chained scatterlists are used or not
+ * @total: total number of bytes for transfer
+ * @last: is this the last block
+ * @first: is this the first block
+ * @active: inside a transfer
+ */
+struct sahara_sha_reqctx {
+	u8			buf[SAHARA_MAX_SHA_BLOCK_SIZE];
+	u8			rembuf[SAHARA_MAX_SHA_BLOCK_SIZE];
+	u8			context[SHA256_DIGEST_SIZE + 4];
+	struct mutex		mutex;
+	unsigned int		mode;
+	unsigned int		digest_size;
+	unsigned int		context_size;
+	unsigned int		buf_cnt;
+	unsigned int		sg_in_idx;
+	struct scatterlist	*in_sg;
+	struct scatterlist	in_sg_chain[2];
+	bool			in_sg_chained;
+	size_t			total;
+	unsigned int		last;
+	unsigned int		first;
+	unsigned int		active;
+};
+
 struct sahara_dev {
 	struct device		*device;
+	unsigned int		version;
 	void __iomem		*regs_base;
 	struct clk		*clk_ipg;
 	struct clk		*clk_ahb;
+	struct mutex		queue_mutex;
+	struct task_struct	*kthread;
+	struct completion	dma_completion;
 
 	struct sahara_ctx	*ctx;
 	spinlock_t		lock;
 	struct crypto_queue	queue;
 	unsigned long		flags;
 
-	struct tasklet_struct	done_task;
-	struct tasklet_struct	queue_task;
-
 	struct sahara_hw_desc	*hw_desc[SAHARA_MAX_HW_DESC];
 	dma_addr_t		hw_phys_desc[SAHARA_MAX_HW_DESC];
 
@@ -151,10 +222,12 @@ struct sahara_dev {
 	u8			*iv_base;
 	dma_addr_t		iv_phys_base;
 
+	u8			*context_base;
+	dma_addr_t		context_phys_base;
+
 	struct sahara_hw_link	*hw_link[SAHARA_MAX_HW_LINK];
 	dma_addr_t		hw_phys_link[SAHARA_MAX_HW_LINK];
 
-	struct ablkcipher_request *req;
 	size_t			total;
 	struct scatterlist	*in_sg;
 	unsigned int		nb_in_sg;
@@ -162,7 +235,6 @@ struct sahara_dev {
 	unsigned int		nb_out_sg;
 
 	u32			error;
-	struct timer_list	watchdog;
 };
 
 static struct sahara_dev *dev_ptr;
@@ -401,34 +473,6 @@ static void sahara_dump_links(struct sahara_dev *dev)
 	dev_dbg(dev->device, "\n");
 }
 
-static void sahara_aes_done_task(unsigned long data)
-{
-	struct sahara_dev *dev = (struct sahara_dev *)data;
-
-	dma_unmap_sg(dev->device, dev->out_sg, dev->nb_out_sg,
-		DMA_TO_DEVICE);
-	dma_unmap_sg(dev->device, dev->in_sg, dev->nb_in_sg,
-		DMA_FROM_DEVICE);
-
-	spin_lock(&dev->lock);
-	clear_bit(FLAGS_BUSY, &dev->flags);
-	spin_unlock(&dev->lock);
-
-	dev->req->base.complete(&dev->req->base, dev->error);
-}
-
-static void sahara_watchdog(unsigned long data)
-{
-	struct sahara_dev *dev = (struct sahara_dev *)data;
-	unsigned int err = sahara_read(dev, SAHARA_REG_ERRSTATUS);
-	unsigned int stat = sahara_read(dev, SAHARA_REG_STATUS);
-
-	sahara_decode_status(dev, stat);
-	sahara_decode_error(dev, err);
-	dev->error = -ETIMEDOUT;
-	sahara_aes_done_task(data);
-}
-
 static int sahara_hw_descriptor_create(struct sahara_dev *dev)
 {
 	struct sahara_ctx *ctx = dev->ctx;
@@ -512,9 +556,6 @@ static int sahara_hw_descriptor_create(struct sahara_dev *dev)
 	sahara_dump_descriptors(dev);
 	sahara_dump_links(dev);
 
-	/* Start processing descriptor chain. */
-	mod_timer(&dev->watchdog,
-		  jiffies + msecs_to_jiffies(SAHARA_TIMEOUT_MS));
 	sahara_write(dev, dev->hw_phys_desc[0], SAHARA_REG_DAR);
 
 	return 0;
@@ -529,37 +570,19 @@ unmap_in:
 	return -EINVAL;
 }
 
-static void sahara_aes_queue_task(unsigned long data)
+static int sahara_aes_process(struct ablkcipher_request *req)
 {
-	struct sahara_dev *dev = (struct sahara_dev *)data;
-	struct crypto_async_request *async_req, *backlog;
+	struct sahara_dev *dev = dev_ptr;
 	struct sahara_ctx *ctx;
 	struct sahara_aes_reqctx *rctx;
-	struct ablkcipher_request *req;
 	int ret;
 
-	spin_lock(&dev->lock);
-	backlog = crypto_get_backlog(&dev->queue);
-	async_req = crypto_dequeue_request(&dev->queue);
-	if (!async_req)
-		clear_bit(FLAGS_BUSY, &dev->flags);
-	spin_unlock(&dev->lock);
-
-	if (!async_req)
-		return;
-
-	if (backlog)
-		backlog->complete(backlog, -EINPROGRESS);
-
-	req = ablkcipher_request_cast(async_req);
-
 	/* Request is ready to be dispatched by the device */
 	dev_dbg(dev->device,
 		"dispatch request (nbytes=%d, src=%p, dst=%p)\n",
 		req->nbytes, req->src, req->dst);
 
 	/* assign new request to device */
-	dev->req = req;
 	dev->total = req->nbytes;
 	dev->in_sg = req->src;
 	dev->out_sg = req->dst;
@@ -573,16 +596,25 @@ static void sahara_aes_queue_task(unsigned long data)
 		memcpy(dev->iv_base, req->info, AES_KEYSIZE_128);
 
 	/* assign new context to device */
-	ctx->dev = dev;
 	dev->ctx = ctx;
 
+	reinit_completion(&dev->dma_completion);
+
 	ret = sahara_hw_descriptor_create(dev);
-	if (ret < 0) {
-		spin_lock(&dev->lock);
-		clear_bit(FLAGS_BUSY, &dev->flags);
-		spin_unlock(&dev->lock);
-		dev->req->base.complete(&dev->req->base, ret);
+
+	ret = wait_for_completion_timeout(&dev->dma_completion,
+				msecs_to_jiffies(SAHARA_TIMEOUT_MS));
+	if (!ret) {
+		dev_err(dev->device, "AES timeout\n");
+		return -ETIMEDOUT;
 	}
+
+	dma_unmap_sg(dev->device, dev->out_sg, dev->nb_out_sg,
+		DMA_TO_DEVICE);
+	dma_unmap_sg(dev->device, dev->in_sg, dev->nb_in_sg,
+		DMA_FROM_DEVICE);
+
+	return 0;
 }
 
 static int sahara_aes_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
@@ -624,12 +656,9 @@ static int sahara_aes_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
 
 static int sahara_aes_crypt(struct ablkcipher_request *req, unsigned long mode)
 {
-	struct sahara_ctx *ctx = crypto_ablkcipher_ctx(
-		crypto_ablkcipher_reqtfm(req));
 	struct sahara_aes_reqctx *rctx = ablkcipher_request_ctx(req);
 	struct sahara_dev *dev = dev_ptr;
 	int err = 0;
-	int busy;
 
 	dev_dbg(dev->device, "nbytes: %d, enc: %d, cbc: %d\n",
 		req->nbytes, !!(mode & FLAGS_ENCRYPT), !!(mode & FLAGS_CBC));
@@ -640,16 +669,13 @@ static int sahara_aes_crypt(struct ablkcipher_request *req, unsigned long mode)
 		return -EINVAL;
 	}
 
-	ctx->dev = dev;
-
 	rctx->mode = mode;
-	spin_lock_bh(&dev->lock);
+
+	mutex_lock(&dev->queue_mutex);
 	err = ablkcipher_enqueue_request(&dev->queue, req);
-	busy = test_and_set_bit(FLAGS_BUSY, &dev->flags);
-	spin_unlock_bh(&dev->lock);
+	mutex_unlock(&dev->queue_mutex);
 
-	if (!busy)
-		tasklet_schedule(&dev->queue_task);
+	wake_up_process(dev->kthread);
 
 	return err;
 }
@@ -752,6 +778,484 @@ static void sahara_aes_cra_exit(struct crypto_tfm *tfm)
 	ctx->fallback = NULL;
 }
 
+static u32 sahara_sha_init_hdr(struct sahara_dev *dev,
+			      struct sahara_sha_reqctx *rctx)
+{
+	u32 hdr = 0;
+
+	hdr = rctx->mode;
+
+	if (rctx->first) {
+		hdr |= SAHARA_HDR_MDHA_SET_MODE_HASH;
+		hdr |= SAHARA_HDR_MDHA_INIT;
+	} else {
+		hdr |= SAHARA_HDR_MDHA_SET_MODE_MD_KEY;
+	}
+
+	if (rctx->last)
+		hdr |= SAHARA_HDR_MDHA_PDATA;
+
+	if (hweight_long(hdr) % 2 == 0)
+		hdr |= SAHARA_HDR_PARITY_BIT;
+
+	return hdr;
+}
+
+static int sahara_sha_hw_links_create(struct sahara_dev *dev,
+				       struct sahara_sha_reqctx *rctx,
+				       int start)
+{
+	struct scatterlist *sg;
+	unsigned int i;
+	int ret;
+
+	dev->in_sg = rctx->in_sg;
+
+	dev->nb_in_sg = sahara_sg_length(dev->in_sg, rctx->total);
+	if ((dev->nb_in_sg) > SAHARA_MAX_HW_LINK) {
+		dev_err(dev->device, "not enough hw links (%d)\n",
+			dev->nb_in_sg + dev->nb_out_sg);
+		return -EINVAL;
+	}
+
+	if (rctx->in_sg_chained) {
+		i = start;
+		sg = dev->in_sg;
+		while (sg) {
+			ret = dma_map_sg(dev->device, sg, 1,
+					 DMA_TO_DEVICE);
+			if (!ret)
+				return -EFAULT;
+
+			dev->hw_link[i]->len = sg->length;
+			dev->hw_link[i]->p = sg->dma_address;
+			dev->hw_link[i]->next = dev->hw_phys_link[i + 1];
+			sg = sg_next(sg);
+			i += 1;
+		}
+		dev->hw_link[i-1]->next = 0;
+	} else {
+		sg = dev->in_sg;
+		ret = dma_map_sg(dev->device, dev->in_sg, dev->nb_in_sg,
+				 DMA_TO_DEVICE);
+		if (!ret)
+			return -EFAULT;
+
+		for (i = start; i < dev->nb_in_sg + start; i++) {
+			dev->hw_link[i]->len = sg->length;
+			dev->hw_link[i]->p = sg->dma_address;
+			if (i == (dev->nb_in_sg + start - 1)) {
+				dev->hw_link[i]->next = 0;
+			} else {
+				dev->hw_link[i]->next = dev->hw_phys_link[i + 1];
+				sg = sg_next(sg);
+			}
+		}
+	}
+
+	return i;
+}
+
+static int sahara_sha_hw_data_descriptor_create(struct sahara_dev *dev,
+						struct sahara_sha_reqctx *rctx,
+						struct ahash_request *req,
+						int index)
+{
+	unsigned result_len;
+	int i = index;
+
+	if (rctx->first)
+		/* Create initial descriptor: #8*/
+		dev->hw_desc[index]->hdr = sahara_sha_init_hdr(dev, rctx);
+	else
+		/* Create hash descriptor: #10. Must follow #6. */
+		dev->hw_desc[index]->hdr = SAHARA_HDR_MDHA_HASH;
+
+	dev->hw_desc[index]->len1 = rctx->total;
+	if (dev->hw_desc[index]->len1 == 0) {
+		/* if len1 is 0, p1 must be 0, too */
+		dev->hw_desc[index]->p1 = 0;
+		rctx->sg_in_idx = 0;
+	} else {
+		/* Create input links */
+		dev->hw_desc[index]->p1 = dev->hw_phys_link[index];
+		i = sahara_sha_hw_links_create(dev, rctx, index);
+
+		rctx->sg_in_idx = index;
+		if (i < 0)
+			return i;
+	}
+
+	dev->hw_desc[index]->p2 = dev->hw_phys_link[i];
+
+	/* Save the context for the next operation */
+	result_len = rctx->context_size;
+	dev->hw_link[i]->p = dev->context_phys_base;
+
+	dev->hw_link[i]->len = result_len;
+	dev->hw_desc[index]->len2 = result_len;
+
+	dev->hw_link[i]->next = 0;
+
+	return 0;
+}
+
+/*
+ * Load descriptor aka #6
+ *
+ * To load a previously saved context back to the MDHA unit
+ *
+ * p1: Saved Context
+ * p2: NULL
+ *
+ */
+static int sahara_sha_hw_context_descriptor_create(struct sahara_dev *dev,
+						struct sahara_sha_reqctx *rctx,
+						struct ahash_request *req,
+						int index)
+{
+	dev->hw_desc[index]->hdr = sahara_sha_init_hdr(dev, rctx);
+
+	dev->hw_desc[index]->len1 = rctx->context_size;
+	dev->hw_desc[index]->p1 = dev->hw_phys_link[index];
+	dev->hw_desc[index]->len2 = 0;
+	dev->hw_desc[index]->p2 = 0;
+
+	dev->hw_link[index]->len = rctx->context_size;
+	dev->hw_link[index]->p = dev->context_phys_base;
+	dev->hw_link[index]->next = 0;
+
+	return 0;
+}
+
+static int sahara_walk_and_recalc(struct scatterlist *sg, unsigned int nbytes)
+{
+	if (!sg || !sg->length)
+		return nbytes;
+
+	while (nbytes && sg) {
+		if (nbytes <= sg->length) {
+			sg->length = nbytes;
+			sg_mark_end(sg);
+			break;
+		}
+		nbytes -= sg->length;
+		sg = scatterwalk_sg_next(sg);
+	}
+
+	return nbytes;
+}
+
+static int sahara_sha_prepare_request(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct sahara_sha_reqctx *rctx = ahash_request_ctx(req);
+	unsigned int hash_later;
+	unsigned int block_size;
+	unsigned int len;
+
+	block_size = crypto_tfm_alg_blocksize(crypto_ahash_tfm(tfm));
+
+	/* append bytes from previous operation */
+	len = rctx->buf_cnt + req->nbytes;
+
+	/* only the last transfer can be padded in hardware */
+	if (!rctx->last && (len < block_size)) {
+		/* to few data, save for next operation */
+		scatterwalk_map_and_copy(rctx->buf + rctx->buf_cnt, req->src,
+					 0, req->nbytes, 0);
+		rctx->buf_cnt += req->nbytes;
+
+		return 0;
+	}
+
+	/* add data from previous operation first */
+	if (rctx->buf_cnt)
+		memcpy(rctx->rembuf, rctx->buf, rctx->buf_cnt);
+
+	/* data must always be a multiple of block_size */
+	hash_later = rctx->last ? 0 : len & (block_size - 1);
+	if (hash_later) {
+		unsigned int offset = req->nbytes - hash_later;
+		/* Save remaining bytes for later use */
+		scatterwalk_map_and_copy(rctx->buf, req->src, offset,
+					hash_later, 0);
+	}
+
+	/* nbytes should now be multiple of blocksize */
+	req->nbytes = req->nbytes - hash_later;
+
+	sahara_walk_and_recalc(req->src, req->nbytes);
+
+	/* have data from previous operation and current */
+	if (rctx->buf_cnt && req->nbytes) {
+		sg_init_table(rctx->in_sg_chain, 2);
+		sg_set_buf(rctx->in_sg_chain, rctx->rembuf, rctx->buf_cnt);
+
+		scatterwalk_sg_chain(rctx->in_sg_chain, 2, req->src);
+
+		rctx->total = req->nbytes + rctx->buf_cnt;
+		rctx->in_sg = rctx->in_sg_chain;
+
+		rctx->in_sg_chained = true;
+		req->src = rctx->in_sg_chain;
+	/* only data from previous operation */
+	} else if (rctx->buf_cnt) {
+		if (req->src)
+			rctx->in_sg = req->src;
+		else
+			rctx->in_sg = rctx->in_sg_chain;
+		/* buf was copied into rembuf above */
+		sg_init_one(rctx->in_sg, rctx->rembuf, rctx->buf_cnt);
+		rctx->total = rctx->buf_cnt;
+		rctx->in_sg_chained = false;
+	/* no data from previous operation */
+	} else {
+		rctx->in_sg = req->src;
+		rctx->total = req->nbytes;
+		req->src = rctx->in_sg;
+		rctx->in_sg_chained = false;
+	}
+
+	/* on next call, we only have the remaining data in the buffer */
+	rctx->buf_cnt = hash_later;
+
+	return -EINPROGRESS;
+}
+
+static void sahara_sha_unmap_sg(struct sahara_dev *dev,
+				struct sahara_sha_reqctx *rctx)
+{
+	struct scatterlist *sg;
+
+	if (rctx->in_sg_chained) {
+		sg = dev->in_sg;
+		while (sg) {
+			dma_unmap_sg(dev->device, sg, 1, DMA_TO_DEVICE);
+			sg = sg_next(sg);
+		}
+	} else {
+		dma_unmap_sg(dev->device, dev->in_sg, dev->nb_in_sg,
+			DMA_TO_DEVICE);
+	}
+}
+
+static int sahara_sha_process(struct ahash_request *req)
+{
+	struct sahara_dev *dev = dev_ptr;
+	struct sahara_sha_reqctx *rctx = ahash_request_ctx(req);
+	int ret = -EINPROGRESS;
+
+	ret = sahara_sha_prepare_request(req);
+	if (!ret)
+		return ret;
+
+	if (rctx->first) {
+		sahara_sha_hw_data_descriptor_create(dev, rctx, req, 0);
+		dev->hw_desc[0]->next = 0;
+		rctx->first = 0;
+	} else {
+		memcpy(dev->context_base, rctx->context, rctx->context_size);
+
+		sahara_sha_hw_context_descriptor_create(dev, rctx, req, 0);
+		dev->hw_desc[0]->next = dev->hw_phys_desc[1];
+		sahara_sha_hw_data_descriptor_create(dev, rctx, req, 1);
+		dev->hw_desc[1]->next = 0;
+	}
+
+	sahara_dump_descriptors(dev);
+	sahara_dump_links(dev);
+
+	reinit_completion(&dev->dma_completion);
+
+	sahara_write(dev, dev->hw_phys_desc[0], SAHARA_REG_DAR);
+
+	ret = wait_for_completion_timeout(&dev->dma_completion,
+				msecs_to_jiffies(SAHARA_TIMEOUT_MS));
+	if (!ret) {
+		dev_err(dev->device, "SHA timeout\n");
+		return -ETIMEDOUT;
+	}
+
+	if (rctx->sg_in_idx)
+		sahara_sha_unmap_sg(dev, rctx);
+
+	memcpy(rctx->context, dev->context_base, rctx->context_size);
+
+	if (req->result)
+		memcpy(req->result, rctx->context, rctx->digest_size);
+
+	return 0;
+}
+
+static int sahara_queue_manage(void *data)
+{
+	struct sahara_dev *dev = (struct sahara_dev *)data;
+	struct crypto_async_request *async_req;
+	int ret = 0;
+
+	do {
+		__set_current_state(TASK_INTERRUPTIBLE);
+
+		mutex_lock(&dev->queue_mutex);
+		async_req = crypto_dequeue_request(&dev->queue);
+		mutex_unlock(&dev->queue_mutex);
+
+		if (async_req) {
+			if (crypto_tfm_alg_type(async_req->tfm) ==
+			    CRYPTO_ALG_TYPE_AHASH) {
+				struct ahash_request *req =
+					ahash_request_cast(async_req);
+
+				ret = sahara_sha_process(req);
+			} else {
+				struct ablkcipher_request *req =
+					ablkcipher_request_cast(async_req);
+
+				ret = sahara_aes_process(req);
+			}
+
+			async_req->complete(async_req, ret);
+
+			continue;
+		}
+
+		schedule();
+	} while (!kthread_should_stop());
+
+	return 0;
+}
+
+static int sahara_sha_enqueue(struct ahash_request *req, int last)
+{
+	struct sahara_sha_reqctx *rctx = ahash_request_ctx(req);
+	struct sahara_dev *dev = dev_ptr;
+	int ret;
+
+	if (!req->nbytes && !last)
+		return 0;
+
+	mutex_lock(&rctx->mutex);
+	rctx->last = last;
+
+	if (!rctx->active) {
+		rctx->active = 1;
+		rctx->first = 1;
+	}
+
+	mutex_lock(&dev->queue_mutex);
+	ret = crypto_enqueue_request(&dev->queue, &req->base);
+	mutex_unlock(&dev->queue_mutex);
+
+	wake_up_process(dev->kthread);
+	mutex_unlock(&rctx->mutex);
+
+	return ret;
+}
+
+static int sahara_sha_init(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct sahara_sha_reqctx *rctx = ahash_request_ctx(req);
+
+	memset(rctx, 0, sizeof(*rctx));
+
+	switch (crypto_ahash_digestsize(tfm)) {
+	case SHA1_DIGEST_SIZE:
+		rctx->mode |= SAHARA_HDR_MDHA_ALG_SHA1;
+		rctx->digest_size = SHA1_DIGEST_SIZE;
+		break;
+	case SHA256_DIGEST_SIZE:
+		rctx->mode |= SAHARA_HDR_MDHA_ALG_SHA256;
+		rctx->digest_size = SHA256_DIGEST_SIZE;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	rctx->context_size = rctx->digest_size + 4;
+	rctx->active = 0;
+
+	mutex_init(&rctx->mutex);
+
+	return 0;
+}
+
+static int sahara_sha_update(struct ahash_request *req)
+{
+	return sahara_sha_enqueue(req, 0);
+}
+
+static int sahara_sha_final(struct ahash_request *req)
+{
+	req->nbytes = 0;
+	return sahara_sha_enqueue(req, 1);
+}
+
+static int sahara_sha_finup(struct ahash_request *req)
+{
+	return sahara_sha_enqueue(req, 1);
+}
+
+static int sahara_sha_digest(struct ahash_request *req)
+{
+	sahara_sha_init(req);
+
+	return sahara_sha_finup(req);
+}
+
+static int sahara_sha_export(struct ahash_request *req, void *out)
+{
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct sahara_ctx *ctx = crypto_ahash_ctx(ahash);
+	struct sahara_sha_reqctx *rctx = ahash_request_ctx(req);
+
+	memcpy(out, ctx, sizeof(struct sahara_ctx));
+	memcpy(out + sizeof(struct sahara_sha_reqctx), rctx,
+	       sizeof(struct sahara_sha_reqctx));
+
+	return 0;
+}
+
+static int sahara_sha_import(struct ahash_request *req, const void *in)
+{
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct sahara_ctx *ctx = crypto_ahash_ctx(ahash);
+	struct sahara_sha_reqctx *rctx = ahash_request_ctx(req);
+
+	memcpy(ctx, in, sizeof(struct sahara_ctx));
+	memcpy(rctx, in + sizeof(struct sahara_sha_reqctx),
+	       sizeof(struct sahara_sha_reqctx));
+
+	return 0;
+}
+
+static int sahara_sha_cra_init(struct crypto_tfm *tfm)
+{
+	const char *name = crypto_tfm_alg_name(tfm);
+	struct sahara_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	ctx->shash_fallback = crypto_alloc_shash(name, 0,
+					CRYPTO_ALG_NEED_FALLBACK);
+	if (IS_ERR(ctx->shash_fallback)) {
+		pr_err("Error allocating fallback algo %s\n", name);
+		return PTR_ERR(ctx->shash_fallback);
+	}
+	crypto_ahash_set_reqsize(__crypto_ahash_cast(tfm),
+				 sizeof(struct sahara_sha_reqctx) +
+				 SHA_BUFFER_LEN + SHA256_BLOCK_SIZE);
+
+	return 0;
+}
+
+static void sahara_sha_cra_exit(struct crypto_tfm *tfm)
+{
+	struct sahara_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	crypto_free_shash(ctx->shash_fallback);
+	ctx->shash_fallback = NULL;
+}
+
 static struct crypto_alg aes_algs[] = {
 {
 	.cra_name		= "ecb(aes)",
@@ -797,14 +1301,66 @@ static struct crypto_alg aes_algs[] = {
 }
 };
 
+static struct ahash_alg sha_v3_algs[] = {
+{
+	.init		= sahara_sha_init,
+	.update		= sahara_sha_update,
+	.final		= sahara_sha_final,
+	.finup		= sahara_sha_finup,
+	.digest		= sahara_sha_digest,
+	.export		= sahara_sha_export,
+	.import		= sahara_sha_import,
+	.halg.digestsize	= SHA1_DIGEST_SIZE,
+	.halg.base	= {
+		.cra_name		= "sha1",
+		.cra_driver_name	= "sahara-sha1",
+		.cra_priority		= 300,
+		.cra_flags		= CRYPTO_ALG_TYPE_AHASH |
+						CRYPTO_ALG_ASYNC |
+						CRYPTO_ALG_NEED_FALLBACK,
+		.cra_blocksize		= SHA1_BLOCK_SIZE,
+		.cra_ctxsize		= sizeof(struct sahara_ctx),
+		.cra_alignmask		= 0,
+		.cra_module		= THIS_MODULE,
+		.cra_init		= sahara_sha_cra_init,
+		.cra_exit		= sahara_sha_cra_exit,
+	}
+},
+};
+
+static struct ahash_alg sha_v4_algs[] = {
+{
+	.init		= sahara_sha_init,
+	.update		= sahara_sha_update,
+	.final		= sahara_sha_final,
+	.finup		= sahara_sha_finup,
+	.digest		= sahara_sha_digest,
+	.export		= sahara_sha_export,
+	.import		= sahara_sha_import,
+	.halg.digestsize	= SHA256_DIGEST_SIZE,
+	.halg.base	= {
+		.cra_name		= "sha256",
+		.cra_driver_name	= "sahara-sha256",
+		.cra_priority		= 300,
+		.cra_flags		= CRYPTO_ALG_TYPE_AHASH |
+						CRYPTO_ALG_ASYNC |
+						CRYPTO_ALG_NEED_FALLBACK,
+		.cra_blocksize		= SHA256_BLOCK_SIZE,
+		.cra_ctxsize		= sizeof(struct sahara_ctx),
+		.cra_alignmask		= 0,
+		.cra_module		= THIS_MODULE,
+		.cra_init		= sahara_sha_cra_init,
+		.cra_exit		= sahara_sha_cra_exit,
+	}
+},
+};
+
 static irqreturn_t sahara_irq_handler(int irq, void *data)
 {
 	struct sahara_dev *dev = (struct sahara_dev *)data;
 	unsigned int stat = sahara_read(dev, SAHARA_REG_STATUS);
 	unsigned int err = sahara_read(dev, SAHARA_REG_ERRSTATUS);
 
-	del_timer(&dev->watchdog);
-
 	sahara_write(dev, SAHARA_CMD_CLEAR_INT | SAHARA_CMD_CLEAR_ERR,
 		     SAHARA_REG_CMD);
 
@@ -819,7 +1375,7 @@ static irqreturn_t sahara_irq_handler(int irq, void *data)
 		dev->error = -EINVAL;
 	}
 
-	tasklet_schedule(&dev->done_task);
+	complete(&dev->dma_completion);
 
 	return IRQ_HANDLED;
 }
@@ -827,7 +1383,8 @@ static irqreturn_t sahara_irq_handler(int irq, void *data)
 
 static int sahara_register_algs(struct sahara_dev *dev)
 {
-	int err, i, j;
+	int err;
+	unsigned int i, j, k, l;
 
 	for (i = 0; i < ARRAY_SIZE(aes_algs); i++) {
 		INIT_LIST_HEAD(&aes_algs[i].cra_list);
@@ -836,8 +1393,29 @@ static int sahara_register_algs(struct sahara_dev *dev)
 			goto err_aes_algs;
 	}
 
+	for (k = 0; k < ARRAY_SIZE(sha_v3_algs); k++) {
+		err = crypto_register_ahash(&sha_v3_algs[k]);
+		if (err)
+			goto err_sha_v3_algs;
+	}
+
+	if (dev->version > SAHARA_VERSION_3)
+		for (l = 0; l < ARRAY_SIZE(sha_v4_algs); l++) {
+			err = crypto_register_ahash(&sha_v4_algs[l]);
+			if (err)
+				goto err_sha_v4_algs;
+		}
+
 	return 0;
 
+err_sha_v4_algs:
+	for (j = 0; j < l; j++)
+		crypto_unregister_ahash(&sha_v4_algs[j]);
+
+err_sha_v3_algs:
+	for (j = 0; j < k; j++)
+		crypto_unregister_ahash(&sha_v4_algs[j]);
+
 err_aes_algs:
 	for (j = 0; j < i; j++)
 		crypto_unregister_alg(&aes_algs[j]);
@@ -847,10 +1425,17 @@ err_aes_algs:
 
 static void sahara_unregister_algs(struct sahara_dev *dev)
 {
-	int i;
+	unsigned int i;
 
 	for (i = 0; i < ARRAY_SIZE(aes_algs); i++)
 		crypto_unregister_alg(&aes_algs[i]);
+
+	for (i = 0; i < ARRAY_SIZE(sha_v4_algs); i++)
+		crypto_unregister_ahash(&sha_v3_algs[i]);
+
+	if (dev->version > SAHARA_VERSION_3)
+		for (i = 0; i < ARRAY_SIZE(sha_v4_algs); i++)
+			crypto_unregister_ahash(&sha_v4_algs[i]);
 }
 
 static struct platform_device_id sahara_platform_ids[] = {
@@ -860,6 +1445,7 @@ static struct platform_device_id sahara_platform_ids[] = {
 MODULE_DEVICE_TABLE(platform, sahara_platform_ids);
 
 static struct of_device_id sahara_dt_ids[] = {
+	{ .compatible = "fsl,imx53-sahara" },
 	{ .compatible = "fsl,imx27-sahara" },
 	{ /* sentinel */ }
 };
@@ -939,6 +1525,16 @@ static int sahara_probe(struct platform_device *pdev)
 	dev->iv_base = dev->key_base + AES_KEYSIZE_128;
 	dev->iv_phys_base = dev->key_phys_base + AES_KEYSIZE_128;
 
+	/* Allocate space for context: largest digest + message length field */
+	dev->context_base = dma_alloc_coherent(&pdev->dev,
+					SHA256_DIGEST_SIZE + 4,
+					&dev->context_phys_base, GFP_KERNEL);
+	if (!dev->context_base) {
+		dev_err(&pdev->dev, "Could not allocate memory for MDHA context\n");
+		err = -ENOMEM;
+		goto err_key;
+	}
+
 	/* Allocate space for HW links */
 	dev->hw_link[0] = dma_alloc_coherent(&pdev->dev,
 			SAHARA_MAX_HW_LINK * sizeof(struct sahara_hw_link),
@@ -956,28 +1552,40 @@ static int sahara_probe(struct platform_device *pdev)
 
 	crypto_init_queue(&dev->queue, SAHARA_QUEUE_LENGTH);
 
+	spin_lock_init(&dev->lock);
+	mutex_init(&dev->queue_mutex);
+
 	dev_ptr = dev;
 
-	tasklet_init(&dev->queue_task, sahara_aes_queue_task,
-		     (unsigned long)dev);
-	tasklet_init(&dev->done_task, sahara_aes_done_task,
-		     (unsigned long)dev);
+	dev->kthread = kthread_run(sahara_queue_manage, dev, "sahara_crypto");
+	if (IS_ERR(dev->kthread)) {
+		err = PTR_ERR(dev->kthread);
+		goto err_link;
+	}
 
-	init_timer(&dev->watchdog);
-	dev->watchdog.function = &sahara_watchdog;
-	dev->watchdog.data = (unsigned long)dev;
+	init_completion(&dev->dma_completion);
 
 	clk_prepare_enable(dev->clk_ipg);
 	clk_prepare_enable(dev->clk_ahb);
 
 	version = sahara_read(dev, SAHARA_REG_VERSION);
-	if (version != SAHARA_VERSION_3) {
+	if (of_device_is_compatible(pdev->dev.of_node, "fsl,imx27-sahara")) {
+		if (version != SAHARA_VERSION_3)
+			err = -ENODEV;
+	} else if (of_device_is_compatible(pdev->dev.of_node,
+			"fsl,imx53-sahara")) {
+		if (((version >> 8) & 0xff) != SAHARA_VERSION_4)
+			err = -ENODEV;
+		version = (version >> 8) & 0xff;
+	}
+	if (err == -ENODEV) {
 		dev_err(&pdev->dev, "SAHARA version %d not supported\n",
-			version);
-		err = -ENODEV;
+				version);
 		goto err_algs;
 	}
 
+	dev->version = version;
+
 	sahara_write(dev, SAHARA_CMD_RESET | SAHARA_CMD_MODE_BATCH,
 		     SAHARA_REG_CMD);
 	sahara_write(dev, SAHARA_CONTROL_SET_THROTTLE(0) |
@@ -1000,11 +1608,15 @@ err_algs:
 			  dev->hw_link[0], dev->hw_phys_link[0]);
 	clk_disable_unprepare(dev->clk_ipg);
 	clk_disable_unprepare(dev->clk_ahb);
+	kthread_stop(dev->kthread);
 	dev_ptr = NULL;
 err_link:
 	dma_free_coherent(&pdev->dev,
 			  2 * AES_KEYSIZE_128,
 			  dev->key_base, dev->key_phys_base);
+	dma_free_coherent(&pdev->dev,
+			  SHA256_DIGEST_SIZE,
+			  dev->context_base, dev->context_phys_base);
 err_key:
 	dma_free_coherent(&pdev->dev,
 			  SAHARA_MAX_HW_DESC * sizeof(struct sahara_hw_desc),
@@ -1027,8 +1639,7 @@ static int sahara_remove(struct platform_device *pdev)
 			  SAHARA_MAX_HW_DESC * sizeof(struct sahara_hw_desc),
 			  dev->hw_desc[0], dev->hw_phys_desc[0]);
 
-	tasklet_kill(&dev->done_task);
-	tasklet_kill(&dev->queue_task);
+	kthread_stop(dev->kthread);
 
 	sahara_unregister_algs(dev);
 
@@ -1045,7 +1656,6 @@ static struct platform_driver sahara_driver = {
 	.remove		= sahara_remove,
 	.driver		= {
 		.name	= SAHARA_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = sahara_dt_ids,
 	},
 	.id_table = sahara_platform_ids,
@@ -1055,4 +1665,5 @@ module_platform_driver(sahara_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Javier Martin <javier.martin@vista-silicon.com>");
+MODULE_AUTHOR("Steffen Trumtrar <s.trumtrar@pengutronix.de>");
 MODULE_DESCRIPTION("SAHARA2 HW crypto accelerator");
diff --git a/drivers/crypto/talitos.c b/drivers/crypto/talitos.c
index 624b8be0c365..067ec2193d71 100644
--- a/drivers/crypto/talitos.c
+++ b/drivers/crypto/talitos.c
@@ -2811,7 +2811,6 @@ MODULE_DEVICE_TABLE(of, talitos_match);
 static struct platform_driver talitos_driver = {
 	.driver = {
 		.name = "talitos",
-		.owner = THIS_MODULE,
 		.of_match_table = talitos_match,
 	},
 	.probe = talitos_probe,
diff --git a/drivers/crypto/ux500/cryp/cryp_core.c b/drivers/crypto/ux500/cryp/cryp_core.c
index 92105f3dc8e0..f831bb952b2f 100644
--- a/drivers/crypto/ux500/cryp/cryp_core.c
+++ b/drivers/crypto/ux500/cryp/cryp_core.c
@@ -1688,6 +1688,7 @@ static void ux500_cryp_shutdown(struct platform_device *pdev)
 
 }
 
+#ifdef CONFIG_PM_SLEEP
 static int ux500_cryp_suspend(struct device *dev)
 {
 	int ret;
@@ -1768,6 +1769,7 @@ static int ux500_cryp_resume(struct device *dev)
 
 	return ret;
 }
+#endif
 
 static SIMPLE_DEV_PM_OPS(ux500_cryp_pm, ux500_cryp_suspend, ux500_cryp_resume);
 
@@ -1781,7 +1783,6 @@ static struct platform_driver cryp_driver = {
 	.remove = ux500_cryp_remove,
 	.shutdown = ux500_cryp_shutdown,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name  = "cryp1",
 		.of_match_table = ux500_cryp_match,
 		.pm    = &ux500_cryp_pm,
@@ -1810,7 +1811,7 @@ 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_ALIAS_CRYPTO("aes-all");
+MODULE_ALIAS_CRYPTO("des-all");
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/crypto/ux500/hash/hash_core.c b/drivers/crypto/ux500/hash/hash_core.c
index 1c73f4fbc252..70a20871e998 100644
--- a/drivers/crypto/ux500/hash/hash_core.c
+++ b/drivers/crypto/ux500/hash/hash_core.c
@@ -1881,6 +1881,7 @@ static void ux500_hash_shutdown(struct platform_device *pdev)
 			__func__);
 }
 
+#ifdef CONFIG_PM_SLEEP
 /**
  * ux500_hash_suspend - Function that suspends the hash device.
  * @dev:	Device to suspend.
@@ -1949,6 +1950,7 @@ static int ux500_hash_resume(struct device *dev)
 
 	return ret;
 }
+#endif
 
 static SIMPLE_DEV_PM_OPS(ux500_hash_pm, ux500_hash_suspend, ux500_hash_resume);
 
@@ -1962,7 +1964,6 @@ static struct platform_driver hash_driver = {
 	.remove = ux500_hash_remove,
 	.shutdown = ux500_hash_shutdown,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name  = "hash1",
 		.of_match_table = ux500_hash_match,
 		.pm    = &ux500_hash_pm,
@@ -1995,7 +1996,7 @@ 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");
+MODULE_ALIAS_CRYPTO("sha1-all");
+MODULE_ALIAS_CRYPTO("sha256-all");
+MODULE_ALIAS_CRYPTO("hmac-sha1-all");
+MODULE_ALIAS_CRYPTO("hmac-sha256-all");
diff --git a/drivers/devfreq/exynos/exynos4_bus.c b/drivers/devfreq/exynos/exynos4_bus.c
index d9b08d3b6830..da9509205169 100644
--- a/drivers/devfreq/exynos/exynos4_bus.c
+++ b/drivers/devfreq/exynos/exynos4_bus.c
@@ -1034,7 +1034,6 @@ static struct platform_driver exynos4_busfreq_driver = {
 	.id_table = exynos4_busfreq_id,
 	.driver = {
 		.name	= "exynos4-busfreq",
-		.owner	= THIS_MODULE,
 		.pm	= &exynos4_busfreq_pm_ops,
 	},
 };
diff --git a/drivers/devfreq/exynos/exynos5_bus.c b/drivers/devfreq/exynos/exynos5_bus.c
index 6cd0392e2798..297ea30d4159 100644
--- a/drivers/devfreq/exynos/exynos5_bus.c
+++ b/drivers/devfreq/exynos/exynos5_bus.c
@@ -393,7 +393,6 @@ static struct platform_driver exynos5_busfreq_int_driver = {
 	.remove		= exynos5_busfreq_int_remove,
 	.driver		= {
 		.name		= "exynos5-bus-int",
-		.owner		= THIS_MODULE,
 		.pm		= &exynos5_busfreq_int_pm_ops,
 	},
 };
diff --git a/drivers/dma-buf/fence.c b/drivers/dma-buf/fence.c
index 7bb9d65d9a2c..e5541117b3e9 100644
--- a/drivers/dma-buf/fence.c
+++ b/drivers/dma-buf/fence.c
@@ -283,7 +283,7 @@ EXPORT_SYMBOL(fence_add_callback);
  * @cb:		[in]	the callback to remove
  *
  * Remove a previously queued callback from the fence. This function returns
- * true if the callback is succesfully removed, or false if the fence has
+ * true if the callback is successfully removed, or false if the fence has
  * already been signaled.
  *
  * *WARNING*:
diff --git a/drivers/dma/Kconfig b/drivers/dma/Kconfig
index de469821bc1b..f2b2c4e87aef 100644
--- a/drivers/dma/Kconfig
+++ b/drivers/dma/Kconfig
@@ -107,6 +107,13 @@ config AT_HDMAC
 	help
 	  Support the Atmel AHB DMA controller.
 
+config AT_XDMAC
+	tristate "Atmel XDMA support"
+	depends on ARCH_AT91
+	select DMA_ENGINE
+	help
+	  Support the Atmel XDMA controller.
+
 config FSL_DMA
 	tristate "Freescale Elo series DMA support"
 	depends on FSL_SOC
@@ -395,12 +402,12 @@ config XILINX_VDMA
 
 config DMA_SUN6I
 	tristate "Allwinner A31 SoCs DMA support"
-	depends on MACH_SUN6I || COMPILE_TEST
+	depends on MACH_SUN6I || MACH_SUN8I || COMPILE_TEST
 	depends on RESET_CONTROLLER
 	select DMA_ENGINE
 	select DMA_VIRTUAL_CHANNELS
 	help
-	  Support for the DMA engine for Allwinner A31 SoCs.
+	  Support for the DMA engine first found in Allwinner A31 SoCs.
 
 config NBPFAXI_DMA
 	tristate "Renesas Type-AXI NBPF DMA support"
diff --git a/drivers/dma/Makefile b/drivers/dma/Makefile
index cb626c179911..2022b5451377 100644
--- a/drivers/dma/Makefile
+++ b/drivers/dma/Makefile
@@ -16,6 +16,7 @@ obj-$(CONFIG_PPC_BESTCOMM) += bestcomm/
 obj-$(CONFIG_MV_XOR) += mv_xor.o
 obj-$(CONFIG_DW_DMAC_CORE) += dw/
 obj-$(CONFIG_AT_HDMAC) += at_hdmac.o
+obj-$(CONFIG_AT_XDMAC) += at_xdmac.o
 obj-$(CONFIG_MX3_IPU) += ipu/
 obj-$(CONFIG_TXX9_DMAC) += txx9dmac.o
 obj-$(CONFIG_SH_DMAE_BASE) += sh/
diff --git a/drivers/dma/amba-pl08x.c b/drivers/dma/amba-pl08x.c
index e34024b000a4..1364d00881dd 100644
--- a/drivers/dma/amba-pl08x.c
+++ b/drivers/dma/amba-pl08x.c
@@ -2164,7 +2164,6 @@ static int pl08x_probe(struct amba_device *adev, const struct amba_id *id)
 			 __func__, ret);
 		goto out_no_memcpy;
 	}
-	pl08x->memcpy.chancnt = ret;
 
 	/* Register slave channels */
 	ret = pl08x_dma_init_virtual_channels(pl08x, &pl08x->slave,
@@ -2175,7 +2174,6 @@ static int pl08x_probe(struct amba_device *adev, const struct amba_id *id)
 				__func__, ret);
 		goto out_no_slave;
 	}
-	pl08x->slave.chancnt = ret;
 
 	ret = dma_async_device_register(&pl08x->memcpy);
 	if (ret) {
diff --git a/drivers/dma/at_xdmac.c b/drivers/dma/at_xdmac.c
new file mode 100644
index 000000000000..b60d77a22df6
--- /dev/null
+++ b/drivers/dma/at_xdmac.c
@@ -0,0 +1,1524 @@
+/*
+ * Driver for the Atmel Extensible DMA Controller (aka XDMAC on AT91 systems)
+ *
+ * Copyright (C) 2014 Atmel Corporation
+ *
+ * Author: Ludovic Desroches <ludovic.desroches@atmel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <asm/barrier.h>
+#include <dt-bindings/dma/at91.h>
+#include <linux/clk.h>
+#include <linux/dmaengine.h>
+#include <linux/dmapool.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/of_dma.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/pm.h>
+
+#include "dmaengine.h"
+
+/* Global registers */
+#define AT_XDMAC_GTYPE		0x00	/* Global Type Register */
+#define		AT_XDMAC_NB_CH(i)	(((i) & 0x1F) + 1)		/* Number of Channels Minus One */
+#define		AT_XDMAC_FIFO_SZ(i)	(((i) >> 5) & 0x7FF)		/* Number of Bytes */
+#define		AT_XDMAC_NB_REQ(i)	((((i) >> 16) & 0x3F) + 1)	/* Number of Peripheral Requests Minus One */
+#define AT_XDMAC_GCFG		0x04	/* Global Configuration Register */
+#define AT_XDMAC_GWAC		0x08	/* Global Weighted Arbiter Configuration Register */
+#define AT_XDMAC_GIE		0x0C	/* Global Interrupt Enable Register */
+#define AT_XDMAC_GID		0x10	/* Global Interrupt Disable Register */
+#define AT_XDMAC_GIM		0x14	/* Global Interrupt Mask Register */
+#define AT_XDMAC_GIS		0x18	/* Global Interrupt Status Register */
+#define AT_XDMAC_GE		0x1C	/* Global Channel Enable Register */
+#define AT_XDMAC_GD		0x20	/* Global Channel Disable Register */
+#define AT_XDMAC_GS		0x24	/* Global Channel Status Register */
+#define AT_XDMAC_GRS		0x28	/* Global Channel Read Suspend Register */
+#define AT_XDMAC_GWS		0x2C	/* Global Write Suspend Register */
+#define AT_XDMAC_GRWS		0x30	/* Global Channel Read Write Suspend Register */
+#define AT_XDMAC_GRWR		0x34	/* Global Channel Read Write Resume Register */
+#define AT_XDMAC_GSWR		0x38	/* Global Channel Software Request Register */
+#define AT_XDMAC_GSWS		0x3C	/* Global channel Software Request Status Register */
+#define AT_XDMAC_GSWF		0x40	/* Global Channel Software Flush Request Register */
+#define AT_XDMAC_VERSION	0xFFC	/* XDMAC Version Register */
+
+/* Channel relative registers offsets */
+#define AT_XDMAC_CIE		0x00	/* Channel Interrupt Enable Register */
+#define		AT_XDMAC_CIE_BIE	BIT(0)	/* End of Block Interrupt Enable Bit */
+#define		AT_XDMAC_CIE_LIE	BIT(1)	/* End of Linked List Interrupt Enable Bit */
+#define		AT_XDMAC_CIE_DIE	BIT(2)	/* End of Disable Interrupt Enable Bit */
+#define		AT_XDMAC_CIE_FIE	BIT(3)	/* End of Flush Interrupt Enable Bit */
+#define		AT_XDMAC_CIE_RBEIE	BIT(4)	/* Read Bus Error Interrupt Enable Bit */
+#define		AT_XDMAC_CIE_WBEIE	BIT(5)	/* Write Bus Error Interrupt Enable Bit */
+#define		AT_XDMAC_CIE_ROIE	BIT(6)	/* Request Overflow Interrupt Enable Bit */
+#define AT_XDMAC_CID		0x04	/* Channel Interrupt Disable Register */
+#define		AT_XDMAC_CID_BID	BIT(0)	/* End of Block Interrupt Disable Bit */
+#define		AT_XDMAC_CID_LID	BIT(1)	/* End of Linked List Interrupt Disable Bit */
+#define		AT_XDMAC_CID_DID	BIT(2)	/* End of Disable Interrupt Disable Bit */
+#define		AT_XDMAC_CID_FID	BIT(3)	/* End of Flush Interrupt Disable Bit */
+#define		AT_XDMAC_CID_RBEID	BIT(4)	/* Read Bus Error Interrupt Disable Bit */
+#define		AT_XDMAC_CID_WBEID	BIT(5)	/* Write Bus Error Interrupt Disable Bit */
+#define		AT_XDMAC_CID_ROID	BIT(6)	/* Request Overflow Interrupt Disable Bit */
+#define AT_XDMAC_CIM		0x08	/* Channel Interrupt Mask Register */
+#define		AT_XDMAC_CIM_BIM	BIT(0)	/* End of Block Interrupt Mask Bit */
+#define		AT_XDMAC_CIM_LIM	BIT(1)	/* End of Linked List Interrupt Mask Bit */
+#define		AT_XDMAC_CIM_DIM	BIT(2)	/* End of Disable Interrupt Mask Bit */
+#define		AT_XDMAC_CIM_FIM	BIT(3)	/* End of Flush Interrupt Mask Bit */
+#define		AT_XDMAC_CIM_RBEIM	BIT(4)	/* Read Bus Error Interrupt Mask Bit */
+#define		AT_XDMAC_CIM_WBEIM	BIT(5)	/* Write Bus Error Interrupt Mask Bit */
+#define		AT_XDMAC_CIM_ROIM	BIT(6)	/* Request Overflow Interrupt Mask Bit */
+#define AT_XDMAC_CIS		0x0C	/* Channel Interrupt Status Register */
+#define		AT_XDMAC_CIS_BIS	BIT(0)	/* End of Block Interrupt Status Bit */
+#define		AT_XDMAC_CIS_LIS	BIT(1)	/* End of Linked List Interrupt Status Bit */
+#define		AT_XDMAC_CIS_DIS	BIT(2)	/* End of Disable Interrupt Status Bit */
+#define		AT_XDMAC_CIS_FIS	BIT(3)	/* End of Flush Interrupt Status Bit */
+#define		AT_XDMAC_CIS_RBEIS	BIT(4)	/* Read Bus Error Interrupt Status Bit */
+#define		AT_XDMAC_CIS_WBEIS	BIT(5)	/* Write Bus Error Interrupt Status Bit */
+#define		AT_XDMAC_CIS_ROIS	BIT(6)	/* Request Overflow Interrupt Status Bit */
+#define AT_XDMAC_CSA		0x10	/* Channel Source Address Register */
+#define AT_XDMAC_CDA		0x14	/* Channel Destination Address Register */
+#define AT_XDMAC_CNDA		0x18	/* Channel Next Descriptor Address Register */
+#define		AT_XDMAC_CNDA_NDAIF(i)	((i) & 0x1)			/* Channel x Next Descriptor Interface */
+#define		AT_XDMAC_CNDA_NDA(i)	((i) & 0xfffffffc)		/* Channel x Next Descriptor Address */
+#define AT_XDMAC_CNDC		0x1C	/* Channel Next Descriptor Control Register */
+#define		AT_XDMAC_CNDC_NDE		(0x1 << 0)		/* Channel x Next Descriptor Enable */
+#define		AT_XDMAC_CNDC_NDSUP		(0x1 << 1)		/* Channel x Next Descriptor Source Update */
+#define		AT_XDMAC_CNDC_NDDUP		(0x1 << 2)		/* Channel x Next Descriptor Destination Update */
+#define		AT_XDMAC_CNDC_NDVIEW_NDV0	(0x0 << 3)		/* Channel x Next Descriptor View 0 */
+#define		AT_XDMAC_CNDC_NDVIEW_NDV1	(0x1 << 3)		/* Channel x Next Descriptor View 1 */
+#define		AT_XDMAC_CNDC_NDVIEW_NDV2	(0x2 << 3)		/* Channel x Next Descriptor View 2 */
+#define		AT_XDMAC_CNDC_NDVIEW_NDV3	(0x3 << 3)		/* Channel x Next Descriptor View 3 */
+#define AT_XDMAC_CUBC		0x20	/* Channel Microblock Control Register */
+#define AT_XDMAC_CBC		0x24	/* Channel Block Control Register */
+#define AT_XDMAC_CC		0x28	/* Channel Configuration Register */
+#define		AT_XDMAC_CC_TYPE	(0x1 << 0)	/* Channel Transfer Type */
+#define			AT_XDMAC_CC_TYPE_MEM_TRAN	(0x0 << 0)	/* Memory to Memory Transfer */
+#define			AT_XDMAC_CC_TYPE_PER_TRAN	(0x1 << 0)	/* Peripheral to Memory or Memory to Peripheral Transfer */
+#define		AT_XDMAC_CC_MBSIZE_MASK	(0x3 << 1)
+#define			AT_XDMAC_CC_MBSIZE_SINGLE	(0x0 << 1)
+#define			AT_XDMAC_CC_MBSIZE_FOUR		(0x1 << 1)
+#define			AT_XDMAC_CC_MBSIZE_EIGHT	(0x2 << 1)
+#define			AT_XDMAC_CC_MBSIZE_SIXTEEN	(0x3 << 1)
+#define		AT_XDMAC_CC_DSYNC	(0x1 << 4)	/* Channel Synchronization */
+#define			AT_XDMAC_CC_DSYNC_PER2MEM	(0x0 << 4)
+#define			AT_XDMAC_CC_DSYNC_MEM2PER	(0x1 << 4)
+#define		AT_XDMAC_CC_PROT	(0x1 << 5)	/* Channel Protection */
+#define			AT_XDMAC_CC_PROT_SEC		(0x0 << 5)
+#define			AT_XDMAC_CC_PROT_UNSEC		(0x1 << 5)
+#define		AT_XDMAC_CC_SWREQ	(0x1 << 6)	/* Channel Software Request Trigger */
+#define			AT_XDMAC_CC_SWREQ_HWR_CONNECTED	(0x0 << 6)
+#define			AT_XDMAC_CC_SWREQ_SWR_CONNECTED	(0x1 << 6)
+#define		AT_XDMAC_CC_MEMSET	(0x1 << 7)	/* Channel Fill Block of memory */
+#define			AT_XDMAC_CC_MEMSET_NORMAL_MODE	(0x0 << 7)
+#define			AT_XDMAC_CC_MEMSET_HW_MODE	(0x1 << 7)
+#define		AT_XDMAC_CC_CSIZE(i)	((0x7 & (i)) << 8)	/* Channel Chunk Size */
+#define		AT_XDMAC_CC_DWIDTH_OFFSET	11
+#define		AT_XDMAC_CC_DWIDTH_MASK	(0x3 << AT_XDMAC_CC_DWIDTH_OFFSET)
+#define		AT_XDMAC_CC_DWIDTH(i)	((0x3 & (i)) << AT_XDMAC_CC_DWIDTH_OFFSET)	/* Channel Data Width */
+#define			AT_XDMAC_CC_DWIDTH_BYTE		0x0
+#define			AT_XDMAC_CC_DWIDTH_HALFWORD	0x1
+#define			AT_XDMAC_CC_DWIDTH_WORD		0x2
+#define			AT_XDMAC_CC_DWIDTH_DWORD	0x3
+#define		AT_XDMAC_CC_SIF(i)	((0x1 & (i)) << 13)	/* Channel Source Interface Identifier */
+#define		AT_XDMAC_CC_DIF(i)	((0x1 & (i)) << 14)	/* Channel Destination Interface Identifier */
+#define		AT_XDMAC_CC_SAM_MASK	(0x3 << 16)	/* Channel Source Addressing Mode */
+#define			AT_XDMAC_CC_SAM_FIXED_AM	(0x0 << 16)
+#define			AT_XDMAC_CC_SAM_INCREMENTED_AM	(0x1 << 16)
+#define			AT_XDMAC_CC_SAM_UBS_AM		(0x2 << 16)
+#define			AT_XDMAC_CC_SAM_UBS_DS_AM	(0x3 << 16)
+#define		AT_XDMAC_CC_DAM_MASK	(0x3 << 18)	/* Channel Source Addressing Mode */
+#define			AT_XDMAC_CC_DAM_FIXED_AM	(0x0 << 18)
+#define			AT_XDMAC_CC_DAM_INCREMENTED_AM	(0x1 << 18)
+#define			AT_XDMAC_CC_DAM_UBS_AM		(0x2 << 18)
+#define			AT_XDMAC_CC_DAM_UBS_DS_AM	(0x3 << 18)
+#define		AT_XDMAC_CC_INITD	(0x1 << 21)	/* Channel Initialization Terminated (read only) */
+#define			AT_XDMAC_CC_INITD_TERMINATED	(0x0 << 21)
+#define			AT_XDMAC_CC_INITD_IN_PROGRESS	(0x1 << 21)
+#define		AT_XDMAC_CC_RDIP	(0x1 << 22)	/* Read in Progress (read only) */
+#define			AT_XDMAC_CC_RDIP_DONE		(0x0 << 22)
+#define			AT_XDMAC_CC_RDIP_IN_PROGRESS	(0x1 << 22)
+#define		AT_XDMAC_CC_WRIP	(0x1 << 23)	/* Write in Progress (read only) */
+#define			AT_XDMAC_CC_WRIP_DONE		(0x0 << 23)
+#define			AT_XDMAC_CC_WRIP_IN_PROGRESS	(0x1 << 23)
+#define		AT_XDMAC_CC_PERID(i)	(0x7f & (h) << 24)	/* Channel Peripheral Identifier */
+#define AT_XDMAC_CDS_MSP	0x2C	/* Channel Data Stride Memory Set Pattern */
+#define AT_XDMAC_CSUS		0x30	/* Channel Source Microblock Stride */
+#define AT_XDMAC_CDUS		0x34	/* Channel Destination Microblock Stride */
+
+#define AT_XDMAC_CHAN_REG_BASE	0x50	/* Channel registers base address */
+
+/* Microblock control members */
+#define AT_XDMAC_MBR_UBC_UBLEN_MAX	0xFFFFFFUL	/* Maximum Microblock Length */
+#define AT_XDMAC_MBR_UBC_NDE		(0x1 << 24)	/* Next Descriptor Enable */
+#define AT_XDMAC_MBR_UBC_NSEN		(0x1 << 25)	/* Next Descriptor Source Update */
+#define AT_XDMAC_MBR_UBC_NDEN		(0x1 << 26)	/* Next Descriptor Destination Update */
+#define AT_XDMAC_MBR_UBC_NDV0		(0x0 << 27)	/* Next Descriptor View 0 */
+#define AT_XDMAC_MBR_UBC_NDV1		(0x1 << 27)	/* Next Descriptor View 1 */
+#define AT_XDMAC_MBR_UBC_NDV2		(0x2 << 27)	/* Next Descriptor View 2 */
+#define AT_XDMAC_MBR_UBC_NDV3		(0x3 << 27)	/* Next Descriptor View 3 */
+
+#define AT_XDMAC_MAX_CHAN	0x20
+
+enum atc_status {
+	AT_XDMAC_CHAN_IS_CYCLIC = 0,
+	AT_XDMAC_CHAN_IS_PAUSED,
+};
+
+/* ----- Channels ----- */
+struct at_xdmac_chan {
+	struct dma_chan			chan;
+	void __iomem			*ch_regs;
+	u32				mask;		/* Channel Mask */
+	u32				cfg[3];		/* Channel Configuration Register */
+	#define	AT_XDMAC_CUR_CFG	0		/* Current channel conf */
+	#define	AT_XDMAC_DEV_TO_MEM_CFG	1		/* Predifined dev to mem channel conf */
+	#define	AT_XDMAC_MEM_TO_DEV_CFG	2		/* Predifined mem to dev channel conf */
+	u8				perid;		/* Peripheral ID */
+	u8				perif;		/* Peripheral Interface */
+	u8				memif;		/* Memory Interface */
+	u32				per_src_addr;
+	u32				per_dst_addr;
+	u32				save_cim;
+	u32				save_cnda;
+	u32				save_cndc;
+	unsigned long			status;
+	struct tasklet_struct		tasklet;
+
+	spinlock_t			lock;
+
+	struct list_head		xfers_list;
+	struct list_head		free_descs_list;
+};
+
+
+/* ----- Controller ----- */
+struct at_xdmac {
+	struct dma_device	dma;
+	void __iomem		*regs;
+	int			irq;
+	struct clk		*clk;
+	u32			save_gim;
+	u32			save_gs;
+	struct dma_pool		*at_xdmac_desc_pool;
+	struct at_xdmac_chan	chan[0];
+};
+
+
+/* ----- Descriptors ----- */
+
+/* Linked List Descriptor */
+struct at_xdmac_lld {
+	dma_addr_t	mbr_nda;	/* Next Descriptor Member */
+	u32		mbr_ubc;	/* Microblock Control Member */
+	dma_addr_t	mbr_sa;		/* Source Address Member */
+	dma_addr_t	mbr_da;		/* Destination Address Member */
+	u32		mbr_cfg;	/* Configuration Register */
+};
+
+
+struct at_xdmac_desc {
+	struct at_xdmac_lld		lld;
+	enum dma_transfer_direction	direction;
+	struct dma_async_tx_descriptor	tx_dma_desc;
+	struct list_head		desc_node;
+	/* Following members are only used by the first descriptor */
+	bool				active_xfer;
+	unsigned int			xfer_size;
+	struct list_head		descs_list;
+	struct list_head		xfer_node;
+};
+
+static inline void __iomem *at_xdmac_chan_reg_base(struct at_xdmac *atxdmac, unsigned int chan_nb)
+{
+	return atxdmac->regs + (AT_XDMAC_CHAN_REG_BASE + chan_nb * 0x40);
+}
+
+#define at_xdmac_read(atxdmac, reg) readl_relaxed((atxdmac)->regs + (reg))
+#define at_xdmac_write(atxdmac, reg, value) \
+	writel_relaxed((value), (atxdmac)->regs + (reg))
+
+#define at_xdmac_chan_read(atchan, reg) readl_relaxed((atchan)->ch_regs + (reg))
+#define at_xdmac_chan_write(atchan, reg, value) writel_relaxed((value), (atchan)->ch_regs + (reg))
+
+static inline struct at_xdmac_chan *to_at_xdmac_chan(struct dma_chan *dchan)
+{
+	return container_of(dchan, struct at_xdmac_chan, chan);
+}
+
+static struct device *chan2dev(struct dma_chan *chan)
+{
+	return &chan->dev->device;
+}
+
+static inline struct at_xdmac *to_at_xdmac(struct dma_device *ddev)
+{
+	return container_of(ddev, struct at_xdmac, dma);
+}
+
+static inline struct at_xdmac_desc *txd_to_at_desc(struct dma_async_tx_descriptor *txd)
+{
+	return container_of(txd, struct at_xdmac_desc, tx_dma_desc);
+}
+
+static inline int at_xdmac_chan_is_cyclic(struct at_xdmac_chan *atchan)
+{
+	return test_bit(AT_XDMAC_CHAN_IS_CYCLIC, &atchan->status);
+}
+
+static inline int at_xdmac_chan_is_paused(struct at_xdmac_chan *atchan)
+{
+	return test_bit(AT_XDMAC_CHAN_IS_PAUSED, &atchan->status);
+}
+
+static inline int at_xdmac_csize(u32 maxburst)
+{
+	int csize;
+
+	csize = ffs(maxburst) - 1;
+	if (csize > 4)
+		csize = -EINVAL;
+
+	return csize;
+};
+
+static inline u8 at_xdmac_get_dwidth(u32 cfg)
+{
+	return (cfg & AT_XDMAC_CC_DWIDTH_MASK) >> AT_XDMAC_CC_DWIDTH_OFFSET;
+};
+
+static unsigned int init_nr_desc_per_channel = 64;
+module_param(init_nr_desc_per_channel, uint, 0644);
+MODULE_PARM_DESC(init_nr_desc_per_channel,
+		 "initial descriptors per channel (default: 64)");
+
+
+static bool at_xdmac_chan_is_enabled(struct at_xdmac_chan *atchan)
+{
+	return at_xdmac_chan_read(atchan, AT_XDMAC_GS) & atchan->mask;
+}
+
+static void at_xdmac_off(struct at_xdmac *atxdmac)
+{
+	at_xdmac_write(atxdmac, AT_XDMAC_GD, -1L);
+
+	/* Wait that all chans are disabled. */
+	while (at_xdmac_read(atxdmac, AT_XDMAC_GS))
+		cpu_relax();
+
+	at_xdmac_write(atxdmac, AT_XDMAC_GID, -1L);
+}
+
+/* Call with lock hold. */
+static void at_xdmac_start_xfer(struct at_xdmac_chan *atchan,
+				struct at_xdmac_desc *first)
+{
+	struct at_xdmac	*atxdmac = to_at_xdmac(atchan->chan.device);
+	u32		reg;
+
+	dev_vdbg(chan2dev(&atchan->chan), "%s: desc 0x%p\n", __func__, first);
+
+	if (at_xdmac_chan_is_enabled(atchan))
+		return;
+
+	/* Set transfer as active to not try to start it again. */
+	first->active_xfer = true;
+
+	/* Tell xdmac where to get the first descriptor. */
+	reg = AT_XDMAC_CNDA_NDA(first->tx_dma_desc.phys)
+	      | AT_XDMAC_CNDA_NDAIF(atchan->memif);
+	at_xdmac_chan_write(atchan, AT_XDMAC_CNDA, reg);
+
+	/*
+	 * When doing memory to memory transfer we need to use the next
+	 * descriptor view 2 since some fields of the configuration register
+	 * depend on transfer size and src/dest addresses.
+	 */
+	if (is_slave_direction(first->direction)) {
+		reg = AT_XDMAC_CNDC_NDVIEW_NDV1;
+		if (first->direction == DMA_MEM_TO_DEV)
+			atchan->cfg[AT_XDMAC_CUR_CFG] =
+				atchan->cfg[AT_XDMAC_MEM_TO_DEV_CFG];
+		else
+			atchan->cfg[AT_XDMAC_CUR_CFG] =
+				atchan->cfg[AT_XDMAC_DEV_TO_MEM_CFG];
+		at_xdmac_chan_write(atchan, AT_XDMAC_CC,
+				    atchan->cfg[AT_XDMAC_CUR_CFG]);
+	} else {
+		/*
+		 * No need to write AT_XDMAC_CC reg, it will be done when the
+		 * descriptor is fecthed.
+		 */
+		reg = AT_XDMAC_CNDC_NDVIEW_NDV2;
+	}
+
+	reg |= AT_XDMAC_CNDC_NDDUP
+	       | AT_XDMAC_CNDC_NDSUP
+	       | AT_XDMAC_CNDC_NDE;
+	at_xdmac_chan_write(atchan, AT_XDMAC_CNDC, reg);
+
+	dev_vdbg(chan2dev(&atchan->chan),
+		 "%s: CC=0x%08x CNDA=0x%08x, CNDC=0x%08x, CSA=0x%08x, CDA=0x%08x, CUBC=0x%08x\n",
+		 __func__, at_xdmac_chan_read(atchan, AT_XDMAC_CC),
+		 at_xdmac_chan_read(atchan, AT_XDMAC_CNDA),
+		 at_xdmac_chan_read(atchan, AT_XDMAC_CNDC),
+		 at_xdmac_chan_read(atchan, AT_XDMAC_CSA),
+		 at_xdmac_chan_read(atchan, AT_XDMAC_CDA),
+		 at_xdmac_chan_read(atchan, AT_XDMAC_CUBC));
+
+	at_xdmac_chan_write(atchan, AT_XDMAC_CID, 0xffffffff);
+	reg = AT_XDMAC_CIE_RBEIE | AT_XDMAC_CIE_WBEIE | AT_XDMAC_CIE_ROIE;
+	/*
+	 * There is no end of list when doing cyclic dma, we need to get
+	 * an interrupt after each periods.
+	 */
+	if (at_xdmac_chan_is_cyclic(atchan))
+		at_xdmac_chan_write(atchan, AT_XDMAC_CIE,
+				    reg | AT_XDMAC_CIE_BIE);
+	else
+		at_xdmac_chan_write(atchan, AT_XDMAC_CIE,
+				    reg | AT_XDMAC_CIE_LIE);
+	at_xdmac_write(atxdmac, AT_XDMAC_GIE, atchan->mask);
+	dev_vdbg(chan2dev(&atchan->chan),
+		 "%s: enable channel (0x%08x)\n", __func__, atchan->mask);
+	wmb();
+	at_xdmac_write(atxdmac, AT_XDMAC_GE, atchan->mask);
+
+	dev_vdbg(chan2dev(&atchan->chan),
+		 "%s: CC=0x%08x CNDA=0x%08x, CNDC=0x%08x, CSA=0x%08x, CDA=0x%08x, CUBC=0x%08x\n",
+		 __func__, at_xdmac_chan_read(atchan, AT_XDMAC_CC),
+		 at_xdmac_chan_read(atchan, AT_XDMAC_CNDA),
+		 at_xdmac_chan_read(atchan, AT_XDMAC_CNDC),
+		 at_xdmac_chan_read(atchan, AT_XDMAC_CSA),
+		 at_xdmac_chan_read(atchan, AT_XDMAC_CDA),
+		 at_xdmac_chan_read(atchan, AT_XDMAC_CUBC));
+
+}
+
+static dma_cookie_t at_xdmac_tx_submit(struct dma_async_tx_descriptor *tx)
+{
+	struct at_xdmac_desc	*desc = txd_to_at_desc(tx);
+	struct at_xdmac_chan	*atchan = to_at_xdmac_chan(tx->chan);
+	dma_cookie_t		cookie;
+
+	spin_lock_bh(&atchan->lock);
+	cookie = dma_cookie_assign(tx);
+
+	dev_vdbg(chan2dev(tx->chan), "%s: atchan 0x%p, add desc 0x%p to xfers_list\n",
+		 __func__, atchan, desc);
+	list_add_tail(&desc->xfer_node, &atchan->xfers_list);
+	if (list_is_singular(&atchan->xfers_list))
+		at_xdmac_start_xfer(atchan, desc);
+
+	spin_unlock_bh(&atchan->lock);
+	return cookie;
+}
+
+static struct at_xdmac_desc *at_xdmac_alloc_desc(struct dma_chan *chan,
+						 gfp_t gfp_flags)
+{
+	struct at_xdmac_desc	*desc;
+	struct at_xdmac		*atxdmac = to_at_xdmac(chan->device);
+	dma_addr_t		phys;
+
+	desc = dma_pool_alloc(atxdmac->at_xdmac_desc_pool, gfp_flags, &phys);
+	if (desc) {
+		memset(desc, 0, sizeof(*desc));
+		INIT_LIST_HEAD(&desc->descs_list);
+		dma_async_tx_descriptor_init(&desc->tx_dma_desc, chan);
+		desc->tx_dma_desc.tx_submit = at_xdmac_tx_submit;
+		desc->tx_dma_desc.phys = phys;
+	}
+
+	return desc;
+}
+
+/* Call must be protected by lock. */
+static struct at_xdmac_desc *at_xdmac_get_desc(struct at_xdmac_chan *atchan)
+{
+	struct at_xdmac_desc *desc;
+
+	if (list_empty(&atchan->free_descs_list)) {
+		desc = at_xdmac_alloc_desc(&atchan->chan, GFP_NOWAIT);
+	} else {
+		desc = list_first_entry(&atchan->free_descs_list,
+					struct at_xdmac_desc, desc_node);
+		list_del(&desc->desc_node);
+		desc->active_xfer = false;
+	}
+
+	return desc;
+}
+
+static struct dma_chan *at_xdmac_xlate(struct of_phandle_args *dma_spec,
+				       struct of_dma *of_dma)
+{
+	struct at_xdmac		*atxdmac = of_dma->of_dma_data;
+	struct at_xdmac_chan	*atchan;
+	struct dma_chan		*chan;
+	struct device		*dev = atxdmac->dma.dev;
+
+	if (dma_spec->args_count != 1) {
+		dev_err(dev, "dma phandler args: bad number of args\n");
+		return NULL;
+	}
+
+	chan = dma_get_any_slave_channel(&atxdmac->dma);
+	if (!chan) {
+		dev_err(dev, "can't get a dma channel\n");
+		return NULL;
+	}
+
+	atchan = to_at_xdmac_chan(chan);
+	atchan->memif = AT91_XDMAC_DT_GET_MEM_IF(dma_spec->args[0]);
+	atchan->perif = AT91_XDMAC_DT_GET_PER_IF(dma_spec->args[0]);
+	atchan->perid = AT91_XDMAC_DT_GET_PERID(dma_spec->args[0]);
+	dev_dbg(dev, "chan dt cfg: memif=%u perif=%u perid=%u\n",
+		 atchan->memif, atchan->perif, atchan->perid);
+
+	return chan;
+}
+
+static int at_xdmac_set_slave_config(struct dma_chan *chan,
+				      struct dma_slave_config *sconfig)
+{
+	struct at_xdmac_chan	*atchan = to_at_xdmac_chan(chan);
+	u8 dwidth;
+	int csize;
+
+	atchan->cfg[AT_XDMAC_DEV_TO_MEM_CFG] =
+		AT91_XDMAC_DT_PERID(atchan->perid)
+		| AT_XDMAC_CC_DAM_INCREMENTED_AM
+		| AT_XDMAC_CC_SAM_FIXED_AM
+		| AT_XDMAC_CC_DIF(atchan->memif)
+		| AT_XDMAC_CC_SIF(atchan->perif)
+		| AT_XDMAC_CC_SWREQ_HWR_CONNECTED
+		| AT_XDMAC_CC_DSYNC_PER2MEM
+		| AT_XDMAC_CC_MBSIZE_SIXTEEN
+		| AT_XDMAC_CC_TYPE_PER_TRAN;
+	csize = at_xdmac_csize(sconfig->src_maxburst);
+	if (csize < 0) {
+		dev_err(chan2dev(chan), "invalid src maxburst value\n");
+		return -EINVAL;
+	}
+	atchan->cfg[AT_XDMAC_DEV_TO_MEM_CFG] |= AT_XDMAC_CC_CSIZE(csize);
+	dwidth = ffs(sconfig->src_addr_width) - 1;
+	atchan->cfg[AT_XDMAC_DEV_TO_MEM_CFG] |= AT_XDMAC_CC_DWIDTH(dwidth);
+
+
+	atchan->cfg[AT_XDMAC_MEM_TO_DEV_CFG] =
+		AT91_XDMAC_DT_PERID(atchan->perid)
+		| AT_XDMAC_CC_DAM_FIXED_AM
+		| AT_XDMAC_CC_SAM_INCREMENTED_AM
+		| AT_XDMAC_CC_DIF(atchan->perif)
+		| AT_XDMAC_CC_SIF(atchan->memif)
+		| AT_XDMAC_CC_SWREQ_HWR_CONNECTED
+		| AT_XDMAC_CC_DSYNC_MEM2PER
+		| AT_XDMAC_CC_MBSIZE_SIXTEEN
+		| AT_XDMAC_CC_TYPE_PER_TRAN;
+	csize = at_xdmac_csize(sconfig->dst_maxburst);
+	if (csize < 0) {
+		dev_err(chan2dev(chan), "invalid src maxburst value\n");
+		return -EINVAL;
+	}
+	atchan->cfg[AT_XDMAC_MEM_TO_DEV_CFG] |= AT_XDMAC_CC_CSIZE(csize);
+	dwidth = ffs(sconfig->dst_addr_width) - 1;
+	atchan->cfg[AT_XDMAC_MEM_TO_DEV_CFG] |= AT_XDMAC_CC_DWIDTH(dwidth);
+
+	/* Src and dst addr are needed to configure the link list descriptor. */
+	atchan->per_src_addr = sconfig->src_addr;
+	atchan->per_dst_addr = sconfig->dst_addr;
+
+	dev_dbg(chan2dev(chan),
+		"%s: cfg[dev2mem]=0x%08x, cfg[mem2dev]=0x%08x, per_src_addr=0x%08x, per_dst_addr=0x%08x\n",
+		__func__, atchan->cfg[AT_XDMAC_DEV_TO_MEM_CFG],
+		atchan->cfg[AT_XDMAC_MEM_TO_DEV_CFG],
+		atchan->per_src_addr, atchan->per_dst_addr);
+
+	return 0;
+}
+
+static struct dma_async_tx_descriptor *
+at_xdmac_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
+		       unsigned int sg_len, enum dma_transfer_direction direction,
+		       unsigned long flags, void *context)
+{
+	struct at_xdmac_chan	*atchan = to_at_xdmac_chan(chan);
+	struct at_xdmac_desc	*first = NULL, *prev = NULL;
+	struct scatterlist	*sg;
+	int			i;
+	u32			cfg;
+	unsigned int		xfer_size = 0;
+
+	if (!sgl)
+		return NULL;
+
+	if (!is_slave_direction(direction)) {
+		dev_err(chan2dev(chan), "invalid DMA direction\n");
+		return NULL;
+	}
+
+	dev_dbg(chan2dev(chan), "%s: sg_len=%d, dir=%s, flags=0x%lx\n",
+		 __func__, sg_len,
+		 direction == DMA_MEM_TO_DEV ? "to device" : "from device",
+		 flags);
+
+	/* Protect dma_sconfig field that can be modified by set_slave_conf. */
+	spin_lock_bh(&atchan->lock);
+
+	/* Prepare descriptors. */
+	for_each_sg(sgl, sg, sg_len, i) {
+		struct at_xdmac_desc	*desc = NULL;
+		u32			len, mem;
+
+		len = sg_dma_len(sg);
+		mem = sg_dma_address(sg);
+		if (unlikely(!len)) {
+			dev_err(chan2dev(chan), "sg data length is zero\n");
+			spin_unlock_bh(&atchan->lock);
+			return NULL;
+		}
+		dev_dbg(chan2dev(chan), "%s: * sg%d len=%u, mem=0x%08x\n",
+			 __func__, i, len, mem);
+
+		desc = at_xdmac_get_desc(atchan);
+		if (!desc) {
+			dev_err(chan2dev(chan), "can't get descriptor\n");
+			if (first)
+				list_splice_init(&first->descs_list, &atchan->free_descs_list);
+			spin_unlock_bh(&atchan->lock);
+			return NULL;
+		}
+
+		/* Linked list descriptor setup. */
+		if (direction == DMA_DEV_TO_MEM) {
+			desc->lld.mbr_sa = atchan->per_src_addr;
+			desc->lld.mbr_da = mem;
+			cfg = atchan->cfg[AT_XDMAC_DEV_TO_MEM_CFG];
+		} else {
+			desc->lld.mbr_sa = mem;
+			desc->lld.mbr_da = atchan->per_dst_addr;
+			cfg = atchan->cfg[AT_XDMAC_MEM_TO_DEV_CFG];
+		}
+		desc->lld.mbr_ubc = AT_XDMAC_MBR_UBC_NDV1		/* next descriptor view */
+			| AT_XDMAC_MBR_UBC_NDEN				/* next descriptor dst parameter update */
+			| AT_XDMAC_MBR_UBC_NSEN				/* next descriptor src parameter update */
+			| (i == sg_len - 1 ? 0 : AT_XDMAC_MBR_UBC_NDE)	/* descriptor fetch */
+			| len / (1 << at_xdmac_get_dwidth(cfg));	/* microblock length */
+		dev_dbg(chan2dev(chan),
+			 "%s: lld: mbr_sa=%pad, mbr_da=%pad, mbr_ubc=0x%08x\n",
+			 __func__, &desc->lld.mbr_sa, &desc->lld.mbr_da, desc->lld.mbr_ubc);
+
+		/* Chain lld. */
+		if (prev) {
+			prev->lld.mbr_nda = desc->tx_dma_desc.phys;
+			dev_dbg(chan2dev(chan),
+				 "%s: chain lld: prev=0x%p, mbr_nda=%pad\n",
+				 __func__, prev, &prev->lld.mbr_nda);
+		}
+
+		prev = desc;
+		if (!first)
+			first = desc;
+
+		dev_dbg(chan2dev(chan), "%s: add desc 0x%p to descs_list 0x%p\n",
+			 __func__, desc, first);
+		list_add_tail(&desc->desc_node, &first->descs_list);
+		xfer_size += len;
+	}
+
+	spin_unlock_bh(&atchan->lock);
+
+	first->tx_dma_desc.flags = flags;
+	first->xfer_size = xfer_size;
+	first->direction = direction;
+
+	return &first->tx_dma_desc;
+}
+
+static struct dma_async_tx_descriptor *
+at_xdmac_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t buf_addr,
+			 size_t buf_len, size_t period_len,
+			 enum dma_transfer_direction direction,
+			 unsigned long flags)
+{
+	struct at_xdmac_chan	*atchan = to_at_xdmac_chan(chan);
+	struct at_xdmac_desc	*first = NULL, *prev = NULL;
+	unsigned int		periods = buf_len / period_len;
+	int			i;
+	u32			cfg;
+
+	dev_dbg(chan2dev(chan), "%s: buf_addr=%pad, buf_len=%zd, period_len=%zd, dir=%s, flags=0x%lx\n",
+		__func__, &buf_addr, buf_len, period_len,
+		direction == DMA_MEM_TO_DEV ? "mem2per" : "per2mem", flags);
+
+	if (!is_slave_direction(direction)) {
+		dev_err(chan2dev(chan), "invalid DMA direction\n");
+		return NULL;
+	}
+
+	if (test_and_set_bit(AT_XDMAC_CHAN_IS_CYCLIC, &atchan->status)) {
+		dev_err(chan2dev(chan), "channel currently used\n");
+		return NULL;
+	}
+
+	for (i = 0; i < periods; i++) {
+		struct at_xdmac_desc	*desc = NULL;
+
+		spin_lock_bh(&atchan->lock);
+		desc = at_xdmac_get_desc(atchan);
+		if (!desc) {
+			dev_err(chan2dev(chan), "can't get descriptor\n");
+			if (first)
+				list_splice_init(&first->descs_list, &atchan->free_descs_list);
+			spin_unlock_bh(&atchan->lock);
+			return NULL;
+		}
+		spin_unlock_bh(&atchan->lock);
+		dev_dbg(chan2dev(chan),
+			"%s: desc=0x%p, tx_dma_desc.phys=%pad\n",
+			__func__, desc, &desc->tx_dma_desc.phys);
+
+		if (direction == DMA_DEV_TO_MEM) {
+			desc->lld.mbr_sa = atchan->per_src_addr;
+			desc->lld.mbr_da = buf_addr + i * period_len;
+			cfg = atchan->cfg[AT_XDMAC_DEV_TO_MEM_CFG];
+		} else {
+			desc->lld.mbr_sa = buf_addr + i * period_len;
+			desc->lld.mbr_da = atchan->per_dst_addr;
+			cfg = atchan->cfg[AT_XDMAC_MEM_TO_DEV_CFG];
+		}
+		desc->lld.mbr_ubc = AT_XDMAC_MBR_UBC_NDV1
+			| AT_XDMAC_MBR_UBC_NDEN
+			| AT_XDMAC_MBR_UBC_NSEN
+			| AT_XDMAC_MBR_UBC_NDE
+			| period_len >> at_xdmac_get_dwidth(cfg);
+
+		dev_dbg(chan2dev(chan),
+			 "%s: lld: mbr_sa=%pad, mbr_da=%pad, mbr_ubc=0x%08x\n",
+			 __func__, &desc->lld.mbr_sa, &desc->lld.mbr_da, desc->lld.mbr_ubc);
+
+		/* Chain lld. */
+		if (prev) {
+			prev->lld.mbr_nda = desc->tx_dma_desc.phys;
+			dev_dbg(chan2dev(chan),
+				 "%s: chain lld: prev=0x%p, mbr_nda=%pad\n",
+				 __func__, prev, &prev->lld.mbr_nda);
+		}
+
+		prev = desc;
+		if (!first)
+			first = desc;
+
+		dev_dbg(chan2dev(chan), "%s: add desc 0x%p to descs_list 0x%p\n",
+			 __func__, desc, first);
+		list_add_tail(&desc->desc_node, &first->descs_list);
+	}
+
+	prev->lld.mbr_nda = first->tx_dma_desc.phys;
+	dev_dbg(chan2dev(chan),
+		"%s: chain lld: prev=0x%p, mbr_nda=%pad\n",
+		__func__, prev, &prev->lld.mbr_nda);
+	first->tx_dma_desc.flags = flags;
+	first->xfer_size = buf_len;
+	first->direction = direction;
+
+	return &first->tx_dma_desc;
+}
+
+static struct dma_async_tx_descriptor *
+at_xdmac_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
+			 size_t len, unsigned long flags)
+{
+	struct at_xdmac_chan	*atchan = to_at_xdmac_chan(chan);
+	struct at_xdmac_desc	*first = NULL, *prev = NULL;
+	size_t			remaining_size = len, xfer_size = 0, ublen;
+	dma_addr_t		src_addr = src, dst_addr = dest;
+	u32			dwidth;
+	/*
+	 * WARNING: We don't know the direction, it involves we can't
+	 * dynamically set the source and dest interface so we have to use the
+	 * same one. Only interface 0 allows EBI access. Hopefully we can
+	 * access DDR through both ports (at least on SAMA5D4x), so we can use
+	 * the same interface for source and dest, that solves the fact we
+	 * don't know the direction.
+	 */
+	u32			chan_cc = AT_XDMAC_CC_DAM_INCREMENTED_AM
+					| AT_XDMAC_CC_SAM_INCREMENTED_AM
+					| AT_XDMAC_CC_DIF(0)
+					| AT_XDMAC_CC_SIF(0)
+					| AT_XDMAC_CC_MBSIZE_SIXTEEN
+					| AT_XDMAC_CC_TYPE_MEM_TRAN;
+
+	dev_dbg(chan2dev(chan), "%s: src=%pad, dest=%pad, len=%zd, flags=0x%lx\n",
+		__func__, &src, &dest, len, flags);
+
+	if (unlikely(!len))
+		return NULL;
+
+	/*
+	 * Check address alignment to select the greater data width we can use.
+	 * Some XDMAC implementations don't provide dword transfer, in this
+	 * case selecting dword has the same behavior as selecting word transfers.
+	 */
+	if (!((src_addr | dst_addr) & 7)) {
+		dwidth = AT_XDMAC_CC_DWIDTH_DWORD;
+		dev_dbg(chan2dev(chan), "%s: dwidth: double word\n", __func__);
+	} else if (!((src_addr | dst_addr)  & 3)) {
+		dwidth = AT_XDMAC_CC_DWIDTH_WORD;
+		dev_dbg(chan2dev(chan), "%s: dwidth: word\n", __func__);
+	} else if (!((src_addr | dst_addr) & 1)) {
+		dwidth = AT_XDMAC_CC_DWIDTH_HALFWORD;
+		dev_dbg(chan2dev(chan), "%s: dwidth: half word\n", __func__);
+	} else {
+		dwidth = AT_XDMAC_CC_DWIDTH_BYTE;
+		dev_dbg(chan2dev(chan), "%s: dwidth: byte\n", __func__);
+	}
+
+	/* Prepare descriptors. */
+	while (remaining_size) {
+		struct at_xdmac_desc	*desc = NULL;
+
+		dev_dbg(chan2dev(chan), "%s: remaining_size=%zu\n", __func__, remaining_size);
+
+		spin_lock_bh(&atchan->lock);
+		desc = at_xdmac_get_desc(atchan);
+		spin_unlock_bh(&atchan->lock);
+		if (!desc) {
+			dev_err(chan2dev(chan), "can't get descriptor\n");
+			if (first)
+				list_splice_init(&first->descs_list, &atchan->free_descs_list);
+			return NULL;
+		}
+
+		/* Update src and dest addresses. */
+		src_addr += xfer_size;
+		dst_addr += xfer_size;
+
+		if (remaining_size >= AT_XDMAC_MBR_UBC_UBLEN_MAX << dwidth)
+			xfer_size = AT_XDMAC_MBR_UBC_UBLEN_MAX << dwidth;
+		else
+			xfer_size = remaining_size;
+
+		dev_dbg(chan2dev(chan), "%s: xfer_size=%zu\n", __func__, xfer_size);
+
+		/* Check remaining length and change data width if needed. */
+		if (!((src_addr | dst_addr | xfer_size) & 7)) {
+			dwidth = AT_XDMAC_CC_DWIDTH_DWORD;
+			dev_dbg(chan2dev(chan), "%s: dwidth: double word\n", __func__);
+		} else if (!((src_addr | dst_addr | xfer_size)  & 3)) {
+			dwidth = AT_XDMAC_CC_DWIDTH_WORD;
+			dev_dbg(chan2dev(chan), "%s: dwidth: word\n", __func__);
+		} else if (!((src_addr | dst_addr | xfer_size) & 1)) {
+			dwidth = AT_XDMAC_CC_DWIDTH_HALFWORD;
+			dev_dbg(chan2dev(chan), "%s: dwidth: half word\n", __func__);
+		} else if ((src_addr | dst_addr | xfer_size) & 1) {
+			dwidth = AT_XDMAC_CC_DWIDTH_BYTE;
+			dev_dbg(chan2dev(chan), "%s: dwidth: byte\n", __func__);
+		}
+		chan_cc |= AT_XDMAC_CC_DWIDTH(dwidth);
+
+		ublen = xfer_size >> dwidth;
+		remaining_size -= xfer_size;
+
+		desc->lld.mbr_sa = src_addr;
+		desc->lld.mbr_da = dst_addr;
+		desc->lld.mbr_ubc = AT_XDMAC_MBR_UBC_NDV2
+			| AT_XDMAC_MBR_UBC_NDEN
+			| AT_XDMAC_MBR_UBC_NSEN
+			| (remaining_size ? AT_XDMAC_MBR_UBC_NDE : 0)
+			| ublen;
+		desc->lld.mbr_cfg = chan_cc;
+
+		dev_dbg(chan2dev(chan),
+			 "%s: lld: mbr_sa=%pad, mbr_da=%pad, mbr_ubc=0x%08x, mbr_cfg=0x%08x\n",
+			 __func__, &desc->lld.mbr_sa, &desc->lld.mbr_da, desc->lld.mbr_ubc, desc->lld.mbr_cfg);
+
+		/* Chain lld. */
+		if (prev) {
+			prev->lld.mbr_nda = desc->tx_dma_desc.phys;
+			dev_dbg(chan2dev(chan),
+				 "%s: chain lld: prev=0x%p, mbr_nda=0x%08x\n",
+				 __func__, prev, prev->lld.mbr_nda);
+		}
+
+		prev = desc;
+		if (!first)
+			first = desc;
+
+		dev_dbg(chan2dev(chan), "%s: add desc 0x%p to descs_list 0x%p\n",
+			 __func__, desc, first);
+		list_add_tail(&desc->desc_node, &first->descs_list);
+	}
+
+	first->tx_dma_desc.flags = flags;
+	first->xfer_size = len;
+
+	return &first->tx_dma_desc;
+}
+
+static enum dma_status
+at_xdmac_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
+		struct dma_tx_state *txstate)
+{
+	struct at_xdmac_chan	*atchan = to_at_xdmac_chan(chan);
+	struct at_xdmac		*atxdmac = to_at_xdmac(atchan->chan.device);
+	struct at_xdmac_desc	*desc, *_desc;
+	struct list_head	*descs_list;
+	enum dma_status		ret;
+	int			residue;
+	u32			cur_nda, mask, value;
+	u8			dwidth = at_xdmac_get_dwidth(atchan->cfg[AT_XDMAC_CUR_CFG]);
+
+	ret = dma_cookie_status(chan, cookie, txstate);
+	if (ret == DMA_COMPLETE)
+		return ret;
+
+	if (!txstate)
+		return ret;
+
+	spin_lock_bh(&atchan->lock);
+
+	desc = list_first_entry(&atchan->xfers_list, struct at_xdmac_desc, xfer_node);
+
+	/*
+	 * If the transfer has not been started yet, don't need to compute the
+	 * residue, it's the transfer length.
+	 */
+	if (!desc->active_xfer) {
+		dma_set_residue(txstate, desc->xfer_size);
+		spin_unlock_bh(&atchan->lock);
+		return ret;
+	}
+
+	residue = desc->xfer_size;
+	/*
+	 * Flush FIFO: only relevant when the transfer is source peripheral
+	 * synchronized.
+	 */
+	mask = AT_XDMAC_CC_TYPE | AT_XDMAC_CC_DSYNC;
+	value = AT_XDMAC_CC_TYPE_PER_TRAN | AT_XDMAC_CC_DSYNC_PER2MEM;
+	if ((atchan->cfg[AT_XDMAC_CUR_CFG] & mask) == value) {
+		at_xdmac_write(atxdmac, AT_XDMAC_GSWF, atchan->mask);
+		while (!(at_xdmac_chan_read(atchan, AT_XDMAC_CIS) & AT_XDMAC_CIS_FIS))
+			cpu_relax();
+	}
+
+	cur_nda = at_xdmac_chan_read(atchan, AT_XDMAC_CNDA) & 0xfffffffc;
+	/*
+	 * Remove size of all microblocks already transferred and the current
+	 * one. Then add the remaining size to transfer of the current
+	 * microblock.
+	 */
+	descs_list = &desc->descs_list;
+	list_for_each_entry_safe(desc, _desc, descs_list, desc_node) {
+		residue -= (desc->lld.mbr_ubc & 0xffffff) << dwidth;
+		if ((desc->lld.mbr_nda & 0xfffffffc) == cur_nda)
+			break;
+	}
+	residue += at_xdmac_chan_read(atchan, AT_XDMAC_CUBC) << dwidth;
+
+	spin_unlock_bh(&atchan->lock);
+
+	dma_set_residue(txstate, residue);
+
+	dev_dbg(chan2dev(chan),
+		 "%s: desc=0x%p, tx_dma_desc.phys=%pad, tx_status=%d, cookie=%d, residue=%d\n",
+		 __func__, desc, &desc->tx_dma_desc.phys, ret, cookie, residue);
+
+	return ret;
+}
+
+/* Call must be protected by lock. */
+static void at_xdmac_remove_xfer(struct at_xdmac_chan *atchan,
+				    struct at_xdmac_desc *desc)
+{
+	dev_dbg(chan2dev(&atchan->chan), "%s: desc 0x%p\n", __func__, desc);
+
+	/*
+	 * Remove the transfer from the transfer list then move the transfer
+	 * descriptors into the free descriptors list.
+	 */
+	list_del(&desc->xfer_node);
+	list_splice_init(&desc->descs_list, &atchan->free_descs_list);
+}
+
+static void at_xdmac_advance_work(struct at_xdmac_chan *atchan)
+{
+	struct at_xdmac_desc	*desc;
+
+	spin_lock_bh(&atchan->lock);
+
+	/*
+	 * If channel is enabled, do nothing, advance_work will be triggered
+	 * after the interruption.
+	 */
+	if (!at_xdmac_chan_is_enabled(atchan) && !list_empty(&atchan->xfers_list)) {
+		desc = list_first_entry(&atchan->xfers_list,
+					struct at_xdmac_desc,
+					xfer_node);
+		dev_vdbg(chan2dev(&atchan->chan), "%s: desc 0x%p\n", __func__, desc);
+		if (!desc->active_xfer)
+			at_xdmac_start_xfer(atchan, desc);
+	}
+
+	spin_unlock_bh(&atchan->lock);
+}
+
+static void at_xdmac_handle_cyclic(struct at_xdmac_chan *atchan)
+{
+	struct at_xdmac_desc		*desc;
+	struct dma_async_tx_descriptor	*txd;
+
+	desc = list_first_entry(&atchan->xfers_list, struct at_xdmac_desc, xfer_node);
+	txd = &desc->tx_dma_desc;
+
+	if (txd->callback && (txd->flags & DMA_PREP_INTERRUPT))
+		txd->callback(txd->callback_param);
+}
+
+static void at_xdmac_tasklet(unsigned long data)
+{
+	struct at_xdmac_chan	*atchan = (struct at_xdmac_chan *)data;
+	struct at_xdmac_desc	*desc;
+	u32			error_mask;
+
+	dev_dbg(chan2dev(&atchan->chan), "%s: status=0x%08lx\n",
+		 __func__, atchan->status);
+
+	error_mask = AT_XDMAC_CIS_RBEIS
+		     | AT_XDMAC_CIS_WBEIS
+		     | AT_XDMAC_CIS_ROIS;
+
+	if (at_xdmac_chan_is_cyclic(atchan)) {
+		at_xdmac_handle_cyclic(atchan);
+	} else if ((atchan->status & AT_XDMAC_CIS_LIS)
+		   || (atchan->status & error_mask)) {
+		struct dma_async_tx_descriptor  *txd;
+
+		if (atchan->status & AT_XDMAC_CIS_RBEIS)
+			dev_err(chan2dev(&atchan->chan), "read bus error!!!");
+		if (atchan->status & AT_XDMAC_CIS_WBEIS)
+			dev_err(chan2dev(&atchan->chan), "write bus error!!!");
+		if (atchan->status & AT_XDMAC_CIS_ROIS)
+			dev_err(chan2dev(&atchan->chan), "request overflow error!!!");
+
+		spin_lock_bh(&atchan->lock);
+		desc = list_first_entry(&atchan->xfers_list,
+					struct at_xdmac_desc,
+					xfer_node);
+		dev_vdbg(chan2dev(&atchan->chan), "%s: desc 0x%p\n", __func__, desc);
+		BUG_ON(!desc->active_xfer);
+
+		txd = &desc->tx_dma_desc;
+
+		at_xdmac_remove_xfer(atchan, desc);
+		spin_unlock_bh(&atchan->lock);
+
+		if (!at_xdmac_chan_is_cyclic(atchan)) {
+			dma_cookie_complete(txd);
+			if (txd->callback && (txd->flags & DMA_PREP_INTERRUPT))
+				txd->callback(txd->callback_param);
+		}
+
+		dma_run_dependencies(txd);
+
+		at_xdmac_advance_work(atchan);
+	}
+}
+
+static irqreturn_t at_xdmac_interrupt(int irq, void *dev_id)
+{
+	struct at_xdmac		*atxdmac = (struct at_xdmac *)dev_id;
+	struct at_xdmac_chan	*atchan;
+	u32			imr, status, pending;
+	u32			chan_imr, chan_status;
+	int			i, ret = IRQ_NONE;
+
+	do {
+		imr = at_xdmac_read(atxdmac, AT_XDMAC_GIM);
+		status = at_xdmac_read(atxdmac, AT_XDMAC_GIS);
+		pending = status & imr;
+
+		dev_vdbg(atxdmac->dma.dev,
+			 "%s: status=0x%08x, imr=0x%08x, pending=0x%08x\n",
+			 __func__, status, imr, pending);
+
+		if (!pending)
+			break;
+
+		/* We have to find which channel has generated the interrupt. */
+		for (i = 0; i < atxdmac->dma.chancnt; i++) {
+			if (!((1 << i) & pending))
+				continue;
+
+			atchan = &atxdmac->chan[i];
+			chan_imr = at_xdmac_chan_read(atchan, AT_XDMAC_CIM);
+			chan_status = at_xdmac_chan_read(atchan, AT_XDMAC_CIS);
+			atchan->status = chan_status & chan_imr;
+			dev_vdbg(atxdmac->dma.dev,
+				 "%s: chan%d: imr=0x%x, status=0x%x\n",
+				 __func__, i, chan_imr, chan_status);
+			dev_vdbg(chan2dev(&atchan->chan),
+				 "%s: CC=0x%08x CNDA=0x%08x, CNDC=0x%08x, CSA=0x%08x, CDA=0x%08x, CUBC=0x%08x\n",
+				 __func__,
+				 at_xdmac_chan_read(atchan, AT_XDMAC_CC),
+				 at_xdmac_chan_read(atchan, AT_XDMAC_CNDA),
+				 at_xdmac_chan_read(atchan, AT_XDMAC_CNDC),
+				 at_xdmac_chan_read(atchan, AT_XDMAC_CSA),
+				 at_xdmac_chan_read(atchan, AT_XDMAC_CDA),
+				 at_xdmac_chan_read(atchan, AT_XDMAC_CUBC));
+
+			if (atchan->status & (AT_XDMAC_CIS_RBEIS | AT_XDMAC_CIS_WBEIS))
+				at_xdmac_write(atxdmac, AT_XDMAC_GD, atchan->mask);
+
+			tasklet_schedule(&atchan->tasklet);
+			ret = IRQ_HANDLED;
+		}
+
+	} while (pending);
+
+	return ret;
+}
+
+static void at_xdmac_issue_pending(struct dma_chan *chan)
+{
+	struct at_xdmac_chan *atchan = to_at_xdmac_chan(chan);
+
+	dev_dbg(chan2dev(&atchan->chan), "%s\n", __func__);
+
+	if (!at_xdmac_chan_is_cyclic(atchan))
+		at_xdmac_advance_work(atchan);
+
+	return;
+}
+
+static int at_xdmac_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
+			    unsigned long arg)
+{
+	struct at_xdmac_desc	*desc, *_desc;
+	struct at_xdmac_chan	*atchan = to_at_xdmac_chan(chan);
+	struct at_xdmac		*atxdmac = to_at_xdmac(atchan->chan.device);
+	int			ret = 0;
+
+	dev_dbg(chan2dev(chan), "%s: cmd=%d\n", __func__, cmd);
+
+	spin_lock_bh(&atchan->lock);
+
+	switch (cmd) {
+	case DMA_PAUSE:
+		at_xdmac_write(atxdmac, AT_XDMAC_GRWS, atchan->mask);
+		set_bit(AT_XDMAC_CHAN_IS_PAUSED, &atchan->status);
+		break;
+
+	case DMA_RESUME:
+		if (!at_xdmac_chan_is_paused(atchan))
+			break;
+
+		at_xdmac_write(atxdmac, AT_XDMAC_GRWR, atchan->mask);
+		clear_bit(AT_XDMAC_CHAN_IS_PAUSED, &atchan->status);
+		break;
+
+	case DMA_TERMINATE_ALL:
+		at_xdmac_write(atxdmac, AT_XDMAC_GD, atchan->mask);
+		while (at_xdmac_read(atxdmac, AT_XDMAC_GS) & atchan->mask)
+			cpu_relax();
+
+		/* Cancel all pending transfers. */
+		list_for_each_entry_safe(desc, _desc, &atchan->xfers_list, xfer_node)
+			at_xdmac_remove_xfer(atchan, desc);
+
+		clear_bit(AT_XDMAC_CHAN_IS_CYCLIC, &atchan->status);
+		break;
+
+	case DMA_SLAVE_CONFIG:
+		ret = at_xdmac_set_slave_config(chan,
+				(struct dma_slave_config *)arg);
+		break;
+
+	default:
+		dev_err(chan2dev(chan),
+			"unmanaged or unknown dma control cmd: %d\n", cmd);
+		ret = -ENXIO;
+	}
+
+	spin_unlock_bh(&atchan->lock);
+
+	return ret;
+}
+
+static int at_xdmac_alloc_chan_resources(struct dma_chan *chan)
+{
+	struct at_xdmac_chan	*atchan = to_at_xdmac_chan(chan);
+	struct at_xdmac_desc	*desc;
+	int			i;
+
+	spin_lock_bh(&atchan->lock);
+
+	if (at_xdmac_chan_is_enabled(atchan)) {
+		dev_err(chan2dev(chan),
+			"can't allocate channel resources (channel enabled)\n");
+		i = -EIO;
+		goto spin_unlock;
+	}
+
+	if (!list_empty(&atchan->free_descs_list)) {
+		dev_err(chan2dev(chan),
+			"can't allocate channel resources (channel not free from a previous use)\n");
+		i = -EIO;
+		goto spin_unlock;
+	}
+
+	for (i = 0; i < init_nr_desc_per_channel; i++) {
+		desc = at_xdmac_alloc_desc(chan, GFP_ATOMIC);
+		if (!desc) {
+			dev_warn(chan2dev(chan),
+				"only %d descriptors have been allocated\n", i);
+			break;
+		}
+		list_add_tail(&desc->desc_node, &atchan->free_descs_list);
+	}
+
+	dma_cookie_init(chan);
+
+	dev_dbg(chan2dev(chan), "%s: allocated %d descriptors\n", __func__, i);
+
+spin_unlock:
+	spin_unlock_bh(&atchan->lock);
+	return i;
+}
+
+static void at_xdmac_free_chan_resources(struct dma_chan *chan)
+{
+	struct at_xdmac_chan	*atchan = to_at_xdmac_chan(chan);
+	struct at_xdmac		*atxdmac = to_at_xdmac(chan->device);
+	struct at_xdmac_desc	*desc, *_desc;
+
+	list_for_each_entry_safe(desc, _desc, &atchan->free_descs_list, desc_node) {
+		dev_dbg(chan2dev(chan), "%s: freeing descriptor %p\n", __func__, desc);
+		list_del(&desc->desc_node);
+		dma_pool_free(atxdmac->at_xdmac_desc_pool, desc, desc->tx_dma_desc.phys);
+	}
+
+	return;
+}
+
+#define AT_XDMAC_DMA_BUSWIDTHS\
+	(BIT(DMA_SLAVE_BUSWIDTH_UNDEFINED) |\
+	BIT(DMA_SLAVE_BUSWIDTH_1_BYTE) |\
+	BIT(DMA_SLAVE_BUSWIDTH_2_BYTES) |\
+	BIT(DMA_SLAVE_BUSWIDTH_4_BYTES) |\
+	BIT(DMA_SLAVE_BUSWIDTH_8_BYTES))
+
+static int at_xdmac_device_slave_caps(struct dma_chan *dchan,
+				      struct dma_slave_caps *caps)
+{
+
+	caps->src_addr_widths = AT_XDMAC_DMA_BUSWIDTHS;
+	caps->dstn_addr_widths = AT_XDMAC_DMA_BUSWIDTHS;
+	caps->directions = BIT(DMA_DEV_TO_MEM) | BIT(DMA_MEM_TO_DEV);
+	caps->cmd_pause = true;
+	caps->cmd_terminate = true;
+	caps->residue_granularity = DMA_RESIDUE_GRANULARITY_BURST;
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int atmel_xdmac_prepare(struct device *dev)
+{
+	struct platform_device	*pdev = to_platform_device(dev);
+	struct at_xdmac		*atxdmac = platform_get_drvdata(pdev);
+	struct dma_chan		*chan, *_chan;
+
+	list_for_each_entry_safe(chan, _chan, &atxdmac->dma.channels, device_node) {
+		struct at_xdmac_chan	*atchan = to_at_xdmac_chan(chan);
+
+		/* Wait for transfer completion, except in cyclic case. */
+		if (at_xdmac_chan_is_enabled(atchan) && !at_xdmac_chan_is_cyclic(atchan))
+			return -EAGAIN;
+	}
+	return 0;
+}
+#else
+#	define atmel_xdmac_prepare NULL
+#endif
+
+#ifdef CONFIG_PM_SLEEP
+static int atmel_xdmac_suspend(struct device *dev)
+{
+	struct platform_device	*pdev = to_platform_device(dev);
+	struct at_xdmac		*atxdmac = platform_get_drvdata(pdev);
+	struct dma_chan		*chan, *_chan;
+
+	list_for_each_entry_safe(chan, _chan, &atxdmac->dma.channels, device_node) {
+		struct at_xdmac_chan	*atchan = to_at_xdmac_chan(chan);
+
+		if (at_xdmac_chan_is_cyclic(atchan)) {
+			if (!at_xdmac_chan_is_paused(atchan))
+				at_xdmac_control(chan, DMA_PAUSE, 0);
+			atchan->save_cim = at_xdmac_chan_read(atchan, AT_XDMAC_CIM);
+			atchan->save_cnda = at_xdmac_chan_read(atchan, AT_XDMAC_CNDA);
+			atchan->save_cndc = at_xdmac_chan_read(atchan, AT_XDMAC_CNDC);
+		}
+	}
+	atxdmac->save_gim = at_xdmac_read(atxdmac, AT_XDMAC_GIM);
+
+	at_xdmac_off(atxdmac);
+	clk_disable_unprepare(atxdmac->clk);
+	return 0;
+}
+
+static int atmel_xdmac_resume(struct device *dev)
+{
+	struct platform_device	*pdev = to_platform_device(dev);
+	struct at_xdmac		*atxdmac = platform_get_drvdata(pdev);
+	struct at_xdmac_chan	*atchan;
+	struct dma_chan		*chan, *_chan;
+	int			i;
+	u32			cfg;
+
+	clk_prepare_enable(atxdmac->clk);
+
+	/* Clear pending interrupts. */
+	for (i = 0; i < atxdmac->dma.chancnt; i++) {
+		atchan = &atxdmac->chan[i];
+		while (at_xdmac_chan_read(atchan, AT_XDMAC_CIS))
+			cpu_relax();
+	}
+
+	at_xdmac_write(atxdmac, AT_XDMAC_GIE, atxdmac->save_gim);
+	at_xdmac_write(atxdmac, AT_XDMAC_GE, atxdmac->save_gs);
+	list_for_each_entry_safe(chan, _chan, &atxdmac->dma.channels, device_node) {
+		atchan = to_at_xdmac_chan(chan);
+		cfg = atchan->cfg[AT_XDMAC_CUR_CFG];
+		at_xdmac_chan_write(atchan, AT_XDMAC_CC, cfg);
+		if (at_xdmac_chan_is_cyclic(atchan)) {
+			at_xdmac_chan_write(atchan, AT_XDMAC_CNDA, atchan->save_cnda);
+			at_xdmac_chan_write(atchan, AT_XDMAC_CNDC, atchan->save_cndc);
+			at_xdmac_chan_write(atchan, AT_XDMAC_CIE, atchan->save_cim);
+			wmb();
+			at_xdmac_write(atxdmac, AT_XDMAC_GE, atchan->mask);
+		}
+	}
+	return 0;
+}
+#endif /* CONFIG_PM_SLEEP */
+
+static int at_xdmac_probe(struct platform_device *pdev)
+{
+	struct resource	*res;
+	struct at_xdmac	*atxdmac;
+	int		irq, size, nr_channels, i, ret;
+	void __iomem	*base;
+	u32		reg;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -EINVAL;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
+
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	/*
+	 * Read number of xdmac channels, read helper function can't be used
+	 * since atxdmac is not yet allocated and we need to know the number
+	 * of channels to do the allocation.
+	 */
+	reg = readl_relaxed(base + AT_XDMAC_GTYPE);
+	nr_channels = AT_XDMAC_NB_CH(reg);
+	if (nr_channels > AT_XDMAC_MAX_CHAN) {
+		dev_err(&pdev->dev, "invalid number of channels (%u)\n",
+			nr_channels);
+		return -EINVAL;
+	}
+
+	size = sizeof(*atxdmac);
+	size += nr_channels * sizeof(struct at_xdmac_chan);
+	atxdmac = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
+	if (!atxdmac) {
+		dev_err(&pdev->dev, "can't allocate at_xdmac structure\n");
+		return -ENOMEM;
+	}
+
+	atxdmac->regs = base;
+	atxdmac->irq = irq;
+
+	atxdmac->clk = devm_clk_get(&pdev->dev, "dma_clk");
+	if (IS_ERR(atxdmac->clk)) {
+		dev_err(&pdev->dev, "can't get dma_clk\n");
+		return PTR_ERR(atxdmac->clk);
+	}
+
+	/* Do not use dev res to prevent races with tasklet */
+	ret = request_irq(atxdmac->irq, at_xdmac_interrupt, 0, "at_xdmac", atxdmac);
+	if (ret) {
+		dev_err(&pdev->dev, "can't request irq\n");
+		return ret;
+	}
+
+	ret = clk_prepare_enable(atxdmac->clk);
+	if (ret) {
+		dev_err(&pdev->dev, "can't prepare or enable clock\n");
+		goto err_free_irq;
+	}
+
+	atxdmac->at_xdmac_desc_pool =
+		dmam_pool_create(dev_name(&pdev->dev), &pdev->dev,
+				sizeof(struct at_xdmac_desc), 4, 0);
+	if (!atxdmac->at_xdmac_desc_pool) {
+		dev_err(&pdev->dev, "no memory for descriptors dma pool\n");
+		ret = -ENOMEM;
+		goto err_clk_disable;
+	}
+
+	dma_cap_set(DMA_CYCLIC, atxdmac->dma.cap_mask);
+	dma_cap_set(DMA_MEMCPY, atxdmac->dma.cap_mask);
+	dma_cap_set(DMA_SLAVE, atxdmac->dma.cap_mask);
+	/*
+	 * Without DMA_PRIVATE the driver is not able to allocate more than
+	 * one channel, second allocation fails in private_candidate.
+	 */
+	dma_cap_set(DMA_PRIVATE, atxdmac->dma.cap_mask);
+	atxdmac->dma.dev				= &pdev->dev;
+	atxdmac->dma.device_alloc_chan_resources	= at_xdmac_alloc_chan_resources;
+	atxdmac->dma.device_free_chan_resources		= at_xdmac_free_chan_resources;
+	atxdmac->dma.device_tx_status			= at_xdmac_tx_status;
+	atxdmac->dma.device_issue_pending		= at_xdmac_issue_pending;
+	atxdmac->dma.device_prep_dma_cyclic		= at_xdmac_prep_dma_cyclic;
+	atxdmac->dma.device_prep_dma_memcpy		= at_xdmac_prep_dma_memcpy;
+	atxdmac->dma.device_prep_slave_sg		= at_xdmac_prep_slave_sg;
+	atxdmac->dma.device_control			= at_xdmac_control;
+	atxdmac->dma.device_slave_caps			= at_xdmac_device_slave_caps;
+
+	/* Disable all chans and interrupts. */
+	at_xdmac_off(atxdmac);
+
+	/* Init channels. */
+	INIT_LIST_HEAD(&atxdmac->dma.channels);
+	for (i = 0; i < nr_channels; i++) {
+		struct at_xdmac_chan *atchan = &atxdmac->chan[i];
+
+		atchan->chan.device = &atxdmac->dma;
+		list_add_tail(&atchan->chan.device_node,
+			      &atxdmac->dma.channels);
+
+		atchan->ch_regs = at_xdmac_chan_reg_base(atxdmac, i);
+		atchan->mask = 1 << i;
+
+		spin_lock_init(&atchan->lock);
+		INIT_LIST_HEAD(&atchan->xfers_list);
+		INIT_LIST_HEAD(&atchan->free_descs_list);
+		tasklet_init(&atchan->tasklet, at_xdmac_tasklet,
+			     (unsigned long)atchan);
+
+		/* Clear pending interrupts. */
+		while (at_xdmac_chan_read(atchan, AT_XDMAC_CIS))
+			cpu_relax();
+	}
+	platform_set_drvdata(pdev, atxdmac);
+
+	ret = dma_async_device_register(&atxdmac->dma);
+	if (ret) {
+		dev_err(&pdev->dev, "fail to register DMA engine device\n");
+		goto err_clk_disable;
+	}
+
+	ret = of_dma_controller_register(pdev->dev.of_node,
+					 at_xdmac_xlate, atxdmac);
+	if (ret) {
+		dev_err(&pdev->dev, "could not register of dma controller\n");
+		goto err_dma_unregister;
+	}
+
+	dev_info(&pdev->dev, "%d channels, mapped at 0x%p\n",
+		 nr_channels, atxdmac->regs);
+
+	return 0;
+
+err_dma_unregister:
+	dma_async_device_unregister(&atxdmac->dma);
+err_clk_disable:
+	clk_disable_unprepare(atxdmac->clk);
+err_free_irq:
+	free_irq(atxdmac->irq, atxdmac->dma.dev);
+	return ret;
+}
+
+static int at_xdmac_remove(struct platform_device *pdev)
+{
+	struct at_xdmac	*atxdmac = (struct at_xdmac *)platform_get_drvdata(pdev);
+	int		i;
+
+	at_xdmac_off(atxdmac);
+	of_dma_controller_free(pdev->dev.of_node);
+	dma_async_device_unregister(&atxdmac->dma);
+	clk_disable_unprepare(atxdmac->clk);
+
+	synchronize_irq(atxdmac->irq);
+
+	free_irq(atxdmac->irq, atxdmac->dma.dev);
+
+	for (i = 0; i < atxdmac->dma.chancnt; i++) {
+		struct at_xdmac_chan *atchan = &atxdmac->chan[i];
+
+		tasklet_kill(&atchan->tasklet);
+		at_xdmac_free_chan_resources(&atchan->chan);
+	}
+
+	return 0;
+}
+
+static const struct dev_pm_ops atmel_xdmac_dev_pm_ops = {
+	.prepare	= atmel_xdmac_prepare,
+	SET_LATE_SYSTEM_SLEEP_PM_OPS(atmel_xdmac_suspend, atmel_xdmac_resume)
+};
+
+static const struct of_device_id atmel_xdmac_dt_ids[] = {
+	{
+		.compatible = "atmel,sama5d4-dma",
+	}, {
+		/* sentinel */
+	}
+};
+MODULE_DEVICE_TABLE(of, atmel_xdmac_dt_ids);
+
+static struct platform_driver at_xdmac_driver = {
+	.probe		= at_xdmac_probe,
+	.remove		= at_xdmac_remove,
+	.driver = {
+		.name		= "at_xdmac",
+		.owner		= THIS_MODULE,
+		.of_match_table	= of_match_ptr(atmel_xdmac_dt_ids),
+		.pm		= &atmel_xdmac_dev_pm_ops,
+	}
+};
+
+static int __init at_xdmac_init(void)
+{
+	return platform_driver_probe(&at_xdmac_driver, at_xdmac_probe);
+}
+subsys_initcall(at_xdmac_init);
+
+MODULE_DESCRIPTION("Atmel Extended DMA Controller driver");
+MODULE_AUTHOR("Ludovic Desroches <ludovic.desroches@atmel.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/dma/bcm2835-dma.c b/drivers/dma/bcm2835-dma.c
index 68007974961a..918b7b3f766f 100644
--- a/drivers/dma/bcm2835-dma.c
+++ b/drivers/dma/bcm2835-dma.c
@@ -525,8 +525,6 @@ static int bcm2835_dma_chan_init(struct bcm2835_dmadev *d, int chan_id, int irq)
 	vchan_init(&c->vc, &d->ddev);
 	INIT_LIST_HEAD(&c->node);
 
-	d->ddev.chancnt++;
-
 	c->chan_base = BCM2835_DMA_CHANIO(d->base, chan_id);
 	c->ch = chan_id;
 	c->irq_number = irq;
@@ -694,7 +692,6 @@ static struct platform_driver bcm2835_dma_driver = {
 	.remove	= bcm2835_dma_remove,
 	.driver = {
 		.name = "bcm2835-dma",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(bcm2835_dma_of_match),
 	},
 };
diff --git a/drivers/dma/bestcomm/bestcomm.c b/drivers/dma/bestcomm/bestcomm.c
index a8c2e2994d2e..fa378d88f6c8 100644
--- a/drivers/dma/bestcomm/bestcomm.c
+++ b/drivers/dma/bestcomm/bestcomm.c
@@ -495,7 +495,6 @@ static struct platform_driver mpc52xx_bcom_of_platform_driver = {
 	.remove		= mpc52xx_bcom_remove,
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = mpc52xx_bcom_of_match,
 	},
 };
diff --git a/drivers/dma/cppi41.c b/drivers/dma/cppi41.c
index a58eec3b2cad..b743adf56465 100644
--- a/drivers/dma/cppi41.c
+++ b/drivers/dma/cppi41.c
@@ -1,3 +1,4 @@
+#include <linux/delay.h>
 #include <linux/dmaengine.h>
 #include <linux/dma-mapping.h>
 #include <linux/platform_device.h>
@@ -567,7 +568,7 @@ static int cppi41_tear_down_chan(struct cppi41_channel *c)
 		reg |= GCR_TEARDOWN;
 		cppi_writel(reg, c->gcr_reg);
 		c->td_queued = 1;
-		c->td_retry = 100;
+		c->td_retry = 500;
 	}
 
 	if (!c->td_seen || !c->td_desc_seen) {
@@ -603,12 +604,16 @@ static int cppi41_tear_down_chan(struct cppi41_channel *c)
 	 * descriptor before the TD we fetch it from enqueue, it has to be
 	 * there waiting for us.
 	 */
-	if (!c->td_seen && c->td_retry)
+	if (!c->td_seen && c->td_retry) {
+		udelay(1);
 		return -EAGAIN;
-
+	}
 	WARN_ON(!c->td_retry);
+
 	if (!c->td_desc_seen) {
 		desc_phys = cppi41_pop_desc(cdd, c->q_num);
+		if (!desc_phys)
+			desc_phys = cppi41_pop_desc(cdd, c->q_comp_num);
 		WARN_ON(!desc_phys);
 	}
 
@@ -1088,7 +1093,6 @@ static struct platform_driver cpp41_dma_driver = {
 	.remove = cppi41_dma_remove,
 	.driver = {
 		.name = "cppi41-dma-engine",
-		.owner = THIS_MODULE,
 		.pm = &cppi41_pm_ops,
 		.of_match_table = of_match_ptr(cppi41_dma_ids),
 	},
diff --git a/drivers/dma/dma-jz4740.c b/drivers/dma/dma-jz4740.c
index ae2ab14e64b3..bdeafeefa5f6 100644
--- a/drivers/dma/dma-jz4740.c
+++ b/drivers/dma/dma-jz4740.c
@@ -563,10 +563,9 @@ static int jz4740_dma_probe(struct platform_device *pdev)
 	dd->device_prep_dma_cyclic = jz4740_dma_prep_dma_cyclic;
 	dd->device_control = jz4740_dma_control;
 	dd->dev = &pdev->dev;
-	dd->chancnt = JZ_DMA_NR_CHANS;
 	INIT_LIST_HEAD(&dd->channels);
 
-	for (i = 0; i < dd->chancnt; i++) {
+	for (i = 0; i < JZ_DMA_NR_CHANS; i++) {
 		chan = &dmadev->chan[i];
 		chan->id = i;
 		chan->vchan.desc_free = jz4740_dma_desc_free;
@@ -608,7 +607,6 @@ static struct platform_driver jz4740_dma_driver = {
 	.remove = jz4740_dma_remove,
 	.driver = {
 		.name = "jz4740-dma",
-		.owner = THIS_MODULE,
 	},
 };
 module_platform_driver(jz4740_dma_driver);
diff --git a/drivers/dma/dmaengine.c b/drivers/dma/dmaengine.c
index 24bfaf0b92ba..e057935e3023 100644
--- a/drivers/dma/dmaengine.c
+++ b/drivers/dma/dmaengine.c
@@ -330,8 +330,7 @@ static int __init dma_channel_table_init(void)
 	if (err) {
 		pr_err("initialization failure\n");
 		for_each_dma_cap_mask(cap, dma_cap_mask_all)
-			if (channel_table[cap])
-				free_percpu(channel_table[cap]);
+			free_percpu(channel_table[cap]);
 	}
 
 	return err;
diff --git a/drivers/dma/edma.c b/drivers/dma/edma.c
index 4cfaaa5a49be..b969206439b7 100644
--- a/drivers/dma/edma.c
+++ b/drivers/dma/edma.c
@@ -1092,7 +1092,6 @@ static struct platform_driver edma_driver = {
 	.remove		= edma_remove,
 	.driver = {
 		.name = "edma-dma-engine",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/dma/fsl-edma.c b/drivers/dma/fsl-edma.c
index 3c5711d5fe97..e9ebb89e1711 100644
--- a/drivers/dma/fsl-edma.c
+++ b/drivers/dma/fsl-edma.c
@@ -118,17 +118,17 @@
 				BIT(DMA_SLAVE_BUSWIDTH_8_BYTES)
 
 struct fsl_edma_hw_tcd {
-	u32	saddr;
-	u16	soff;
-	u16	attr;
-	u32	nbytes;
-	u32	slast;
-	u32	daddr;
-	u16	doff;
-	u16	citer;
-	u32	dlast_sga;
-	u16	csr;
-	u16	biter;
+	__le32	saddr;
+	__le16	soff;
+	__le16	attr;
+	__le32	nbytes;
+	__le32	slast;
+	__le32	daddr;
+	__le16	doff;
+	__le16	citer;
+	__le32	dlast_sga;
+	__le16	csr;
+	__le16	biter;
 };
 
 struct fsl_edma_sw_tcd {
@@ -175,18 +175,12 @@ struct fsl_edma_engine {
 };
 
 /*
- * R/W functions for big- or little-endian registers
- * the eDMA controller's endian is independent of the CPU core's endian.
+ * R/W functions for big- or little-endian registers:
+ * The eDMA controller's endian is independent of the CPU core's endian.
+ * For the big-endian IP module, the offset for 8-bit or 16-bit registers
+ * should also be swapped opposite to that in little-endian IP.
  */
 
-static u16 edma_readw(struct fsl_edma_engine *edma, void __iomem *addr)
-{
-	if (edma->big_endian)
-		return ioread16be(addr);
-	else
-		return ioread16(addr);
-}
-
 static u32 edma_readl(struct fsl_edma_engine *edma, void __iomem *addr)
 {
 	if (edma->big_endian)
@@ -197,13 +191,18 @@ static u32 edma_readl(struct fsl_edma_engine *edma, void __iomem *addr)
 
 static void edma_writeb(struct fsl_edma_engine *edma, u8 val, void __iomem *addr)
 {
-	iowrite8(val, addr);
+	/* swap the reg offset for these in big-endian mode */
+	if (edma->big_endian)
+		iowrite8(val, (void __iomem *)((unsigned long)addr ^ 0x3));
+	else
+		iowrite8(val, addr);
 }
 
 static void edma_writew(struct fsl_edma_engine *edma, u16 val, void __iomem *addr)
 {
+	/* swap the reg offset for these in big-endian mode */
 	if (edma->big_endian)
-		iowrite16be(val, addr);
+		iowrite16be(val, (void __iomem *)((unsigned long)addr ^ 0x2));
 	else
 		iowrite16(val, addr);
 }
@@ -254,13 +253,12 @@ static void fsl_edma_chan_mux(struct fsl_edma_chan *fsl_chan,
 	chans_per_mux = fsl_chan->edma->n_chans / DMAMUX_NR;
 	ch_off = fsl_chan->vchan.chan.chan_id % chans_per_mux;
 	muxaddr = fsl_chan->edma->muxbase[ch / chans_per_mux];
+	slot = EDMAMUX_CHCFG_SOURCE(slot);
 
 	if (enable)
-		edma_writeb(fsl_chan->edma,
-				EDMAMUX_CHCFG_ENBL | EDMAMUX_CHCFG_SOURCE(slot),
-				muxaddr + ch_off);
+		iowrite8(EDMAMUX_CHCFG_ENBL | slot, muxaddr + ch_off);
 	else
-		edma_writeb(fsl_chan->edma, EDMAMUX_CHCFG_DIS, muxaddr + ch_off);
+		iowrite8(EDMAMUX_CHCFG_DIS, muxaddr + ch_off);
 }
 
 static unsigned int fsl_edma_get_tcd_attr(enum dma_slave_buswidth addr_width)
@@ -286,9 +284,8 @@ static void fsl_edma_free_desc(struct virt_dma_desc *vdesc)
 
 	fsl_desc = to_fsl_edma_desc(vdesc);
 	for (i = 0; i < fsl_desc->n_tcds; i++)
-			dma_pool_free(fsl_desc->echan->tcd_pool,
-					fsl_desc->tcd[i].vtcd,
-					fsl_desc->tcd[i].ptcd);
+		dma_pool_free(fsl_desc->echan->tcd_pool, fsl_desc->tcd[i].vtcd,
+			      fsl_desc->tcd[i].ptcd);
 	kfree(fsl_desc);
 }
 
@@ -363,8 +360,8 @@ static size_t fsl_edma_desc_residue(struct fsl_edma_chan *fsl_chan,
 
 	/* calculate the total size in this desc */
 	for (len = i = 0; i < fsl_chan->edesc->n_tcds; i++)
-		len += edma_readl(fsl_chan->edma, &(edesc->tcd[i].vtcd->nbytes))
-			* edma_readw(fsl_chan->edma, &(edesc->tcd[i].vtcd->biter));
+		len += le32_to_cpu(edesc->tcd[i].vtcd->nbytes)
+			* le16_to_cpu(edesc->tcd[i].vtcd->biter);
 
 	if (!in_progress)
 		return len;
@@ -376,17 +373,15 @@ static size_t fsl_edma_desc_residue(struct fsl_edma_chan *fsl_chan,
 
 	/* figure out the finished and calculate the residue */
 	for (i = 0; i < fsl_chan->edesc->n_tcds; i++) {
-		size = edma_readl(fsl_chan->edma, &(edesc->tcd[i].vtcd->nbytes))
-			* edma_readw(fsl_chan->edma, &(edesc->tcd[i].vtcd->biter));
+		size = le32_to_cpu(edesc->tcd[i].vtcd->nbytes)
+			* le16_to_cpu(edesc->tcd[i].vtcd->biter);
 		if (dir == DMA_MEM_TO_DEV)
-			dma_addr = edma_readl(fsl_chan->edma,
-					&(edesc->tcd[i].vtcd->saddr));
+			dma_addr = le32_to_cpu(edesc->tcd[i].vtcd->saddr);
 		else
-			dma_addr = edma_readl(fsl_chan->edma,
-					&(edesc->tcd[i].vtcd->daddr));
+			dma_addr = le32_to_cpu(edesc->tcd[i].vtcd->daddr);
 
 		len -= size;
-		if (cur_addr > dma_addr && cur_addr < dma_addr + size) {
+		if (cur_addr >= dma_addr && cur_addr < dma_addr + size) {
 			len += dma_addr + size - cur_addr;
 			break;
 		}
@@ -424,55 +419,67 @@ static enum dma_status fsl_edma_tx_status(struct dma_chan *chan,
 	return fsl_chan->status;
 }
 
-static void fsl_edma_set_tcd_params(struct fsl_edma_chan *fsl_chan,
-		u32 src, u32 dst, u16 attr, u16 soff, u32 nbytes,
-		u32 slast, u16 citer, u16 biter, u32 doff, u32 dlast_sga,
-		u16 csr)
+static void fsl_edma_set_tcd_regs(struct fsl_edma_chan *fsl_chan,
+				  struct fsl_edma_hw_tcd *tcd)
 {
+	struct fsl_edma_engine *edma = fsl_chan->edma;
 	void __iomem *addr = fsl_chan->edma->membase;
 	u32 ch = fsl_chan->vchan.chan.chan_id;
 
 	/*
-	 * TCD parameters have been swapped in fill_tcd_params(),
-	 * so just write them to registers in the cpu endian here
+	 * TCD parameters are stored in struct fsl_edma_hw_tcd in little
+	 * endian format. However, we need to load the TCD registers in
+	 * big- or little-endian obeying the eDMA engine model endian.
 	 */
-	writew(0, addr + EDMA_TCD_CSR(ch));
-	writel(src, addr + EDMA_TCD_SADDR(ch));
-	writel(dst, addr + EDMA_TCD_DADDR(ch));
-	writew(attr, addr + EDMA_TCD_ATTR(ch));
-	writew(soff, addr + EDMA_TCD_SOFF(ch));
-	writel(nbytes, addr + EDMA_TCD_NBYTES(ch));
-	writel(slast, addr + EDMA_TCD_SLAST(ch));
-	writew(citer, addr + EDMA_TCD_CITER(ch));
-	writew(biter, addr + EDMA_TCD_BITER(ch));
-	writew(doff, addr + EDMA_TCD_DOFF(ch));
-	writel(dlast_sga, addr + EDMA_TCD_DLAST_SGA(ch));
-	writew(csr, addr + EDMA_TCD_CSR(ch));
-}
-
-static void fill_tcd_params(struct fsl_edma_engine *edma,
-		struct fsl_edma_hw_tcd *tcd, u32 src, u32 dst,
-		u16 attr, u16 soff, u32 nbytes, u32 slast, u16 citer,
-		u16 biter, u16 doff, u32 dlast_sga, bool major_int,
-		bool disable_req, bool enable_sg)
+	edma_writew(edma, 0, addr + EDMA_TCD_CSR(ch));
+	edma_writel(edma, le32_to_cpu(tcd->saddr), addr + EDMA_TCD_SADDR(ch));
+	edma_writel(edma, le32_to_cpu(tcd->daddr), addr + EDMA_TCD_DADDR(ch));
+
+	edma_writew(edma, le16_to_cpu(tcd->attr), addr + EDMA_TCD_ATTR(ch));
+	edma_writew(edma, le16_to_cpu(tcd->soff), addr + EDMA_TCD_SOFF(ch));
+
+	edma_writel(edma, le32_to_cpu(tcd->nbytes), addr + EDMA_TCD_NBYTES(ch));
+	edma_writel(edma, le32_to_cpu(tcd->slast), addr + EDMA_TCD_SLAST(ch));
+
+	edma_writew(edma, le16_to_cpu(tcd->citer), addr + EDMA_TCD_CITER(ch));
+	edma_writew(edma, le16_to_cpu(tcd->biter), addr + EDMA_TCD_BITER(ch));
+	edma_writew(edma, le16_to_cpu(tcd->doff), addr + EDMA_TCD_DOFF(ch));
+
+	edma_writel(edma, le32_to_cpu(tcd->dlast_sga), addr + EDMA_TCD_DLAST_SGA(ch));
+
+	edma_writew(edma, le16_to_cpu(tcd->csr), addr + EDMA_TCD_CSR(ch));
+}
+
+static inline
+void fsl_edma_fill_tcd(struct fsl_edma_hw_tcd *tcd, u32 src, u32 dst,
+		       u16 attr, u16 soff, u32 nbytes, u32 slast, u16 citer,
+		       u16 biter, u16 doff, u32 dlast_sga, bool major_int,
+		       bool disable_req, bool enable_sg)
 {
 	u16 csr = 0;
 
 	/*
-	 * eDMA hardware SGs require the TCD parameters stored in memory
-	 * the same endian as the eDMA module so that they can be loaded
-	 * automatically by the engine
+	 * eDMA hardware SGs require the TCDs to be stored in little
+	 * endian format irrespective of the register endian model.
+	 * So we put the value in little endian in memory, waiting
+	 * for fsl_edma_set_tcd_regs doing the swap.
 	 */
-	edma_writel(edma, src, &(tcd->saddr));
-	edma_writel(edma, dst, &(tcd->daddr));
-	edma_writew(edma, attr, &(tcd->attr));
-	edma_writew(edma, EDMA_TCD_SOFF_SOFF(soff), &(tcd->soff));
-	edma_writel(edma, EDMA_TCD_NBYTES_NBYTES(nbytes), &(tcd->nbytes));
-	edma_writel(edma, EDMA_TCD_SLAST_SLAST(slast), &(tcd->slast));
-	edma_writew(edma, EDMA_TCD_CITER_CITER(citer), &(tcd->citer));
-	edma_writew(edma, EDMA_TCD_DOFF_DOFF(doff), &(tcd->doff));
-	edma_writel(edma, EDMA_TCD_DLAST_SGA_DLAST_SGA(dlast_sga), &(tcd->dlast_sga));
-	edma_writew(edma, EDMA_TCD_BITER_BITER(biter), &(tcd->biter));
+	tcd->saddr = cpu_to_le32(src);
+	tcd->daddr = cpu_to_le32(dst);
+
+	tcd->attr = cpu_to_le16(attr);
+
+	tcd->soff = cpu_to_le16(EDMA_TCD_SOFF_SOFF(soff));
+
+	tcd->nbytes = cpu_to_le32(EDMA_TCD_NBYTES_NBYTES(nbytes));
+	tcd->slast = cpu_to_le32(EDMA_TCD_SLAST_SLAST(slast));
+
+	tcd->citer = cpu_to_le16(EDMA_TCD_CITER_CITER(citer));
+	tcd->doff = cpu_to_le16(EDMA_TCD_DOFF_DOFF(doff));
+
+	tcd->dlast_sga = cpu_to_le32(EDMA_TCD_DLAST_SGA_DLAST_SGA(dlast_sga));
+
+	tcd->biter = cpu_to_le16(EDMA_TCD_BITER_BITER(biter));
 	if (major_int)
 		csr |= EDMA_TCD_CSR_INT_MAJOR;
 
@@ -482,7 +489,7 @@ static void fill_tcd_params(struct fsl_edma_engine *edma,
 	if (enable_sg)
 		csr |= EDMA_TCD_CSR_E_SG;
 
-	edma_writew(edma, csr, &(tcd->csr));
+	tcd->csr = cpu_to_le16(csr);
 }
 
 static struct fsl_edma_desc *fsl_edma_alloc_desc(struct fsl_edma_chan *fsl_chan,
@@ -558,9 +565,9 @@ static struct dma_async_tx_descriptor *fsl_edma_prep_dma_cyclic(
 			doff = fsl_chan->fsc.addr_width;
 		}
 
-		fill_tcd_params(fsl_chan->edma, fsl_desc->tcd[i].vtcd, src_addr,
-				dst_addr, fsl_chan->fsc.attr, soff, nbytes, 0,
-				iter, iter, doff, last_sg, true, false, true);
+		fsl_edma_fill_tcd(fsl_desc->tcd[i].vtcd, src_addr, dst_addr,
+				  fsl_chan->fsc.attr, soff, nbytes, 0, iter,
+				  iter, doff, last_sg, true, false, true);
 		dma_buf_next += period_len;
 	}
 
@@ -607,16 +614,16 @@ static struct dma_async_tx_descriptor *fsl_edma_prep_slave_sg(
 		iter = sg_dma_len(sg) / nbytes;
 		if (i < sg_len - 1) {
 			last_sg = fsl_desc->tcd[(i + 1)].ptcd;
-			fill_tcd_params(fsl_chan->edma, fsl_desc->tcd[i].vtcd,
-					src_addr, dst_addr, fsl_chan->fsc.attr,
-					soff, nbytes, 0, iter, iter, doff, last_sg,
-					false, false, true);
+			fsl_edma_fill_tcd(fsl_desc->tcd[i].vtcd, src_addr,
+					  dst_addr, fsl_chan->fsc.attr, soff,
+					  nbytes, 0, iter, iter, doff, last_sg,
+					  false, false, true);
 		} else {
 			last_sg = 0;
-			fill_tcd_params(fsl_chan->edma, fsl_desc->tcd[i].vtcd,
-					src_addr, dst_addr, fsl_chan->fsc.attr,
-					soff, nbytes, 0, iter, iter, doff, last_sg,
-					true, true, false);
+			fsl_edma_fill_tcd(fsl_desc->tcd[i].vtcd, src_addr,
+					  dst_addr, fsl_chan->fsc.attr, soff,
+					  nbytes, 0, iter, iter, doff, last_sg,
+					  true, true, false);
 		}
 	}
 
@@ -625,17 +632,13 @@ static struct dma_async_tx_descriptor *fsl_edma_prep_slave_sg(
 
 static void fsl_edma_xfer_desc(struct fsl_edma_chan *fsl_chan)
 {
-	struct fsl_edma_hw_tcd *tcd;
 	struct virt_dma_desc *vdesc;
 
 	vdesc = vchan_next_desc(&fsl_chan->vchan);
 	if (!vdesc)
 		return;
 	fsl_chan->edesc = to_fsl_edma_desc(vdesc);
-	tcd = fsl_chan->edesc->tcd[0].vtcd;
-	fsl_edma_set_tcd_params(fsl_chan, tcd->saddr, tcd->daddr, tcd->attr,
-			tcd->soff, tcd->nbytes, tcd->slast, tcd->citer,
-			tcd->biter, tcd->doff, tcd->dlast_sga, tcd->csr);
+	fsl_edma_set_tcd_regs(fsl_chan, fsl_chan->edesc->tcd[0].vtcd);
 	fsl_edma_enable_request(fsl_chan);
 	fsl_chan->status = DMA_IN_PROGRESS;
 }
@@ -963,7 +966,6 @@ MODULE_DEVICE_TABLE(of, fsl_edma_dt_ids);
 static struct platform_driver fsl_edma_driver = {
 	.driver		= {
 		.name	= "fsl-edma",
-		.owner  = THIS_MODULE,
 		.of_match_table = fsl_edma_dt_ids,
 	},
 	.probe          = fsl_edma_probe,
diff --git a/drivers/dma/fsldma.c b/drivers/dma/fsldma.c
index 994bcb2c6b92..38821cdf862b 100644
--- a/drivers/dma/fsldma.c
+++ b/drivers/dma/fsldma.c
@@ -1337,7 +1337,6 @@ static int fsl_dma_chan_probe(struct fsldma_device *fdev,
 
 	/* Add the channel to DMA device channel list */
 	list_add_tail(&chan->common.device_node, &fdev->common.channels);
-	fdev->common.chancnt++;
 
 	dev_info(fdev->dev, "#%d (%s), irq %d\n", chan->id, compatible,
 		 chan->irq != NO_IRQ ? chan->irq : fdev->irq);
@@ -1540,7 +1539,6 @@ static const struct of_device_id fsldma_of_ids[] = {
 static struct platform_driver fsldma_of_driver = {
 	.driver = {
 		.name = "fsl-elo-dma",
-		.owner = THIS_MODULE,
 		.of_match_table = fsldma_of_ids,
 #ifdef CONFIG_PM
 		.pm = &fsldma_pm_ops,
diff --git a/drivers/dma/imx-dma.c b/drivers/dma/imx-dma.c
index 9d2c9e7374dc..10bbc0a675b0 100644
--- a/drivers/dma/imx-dma.c
+++ b/drivers/dma/imx-dma.c
@@ -1236,7 +1236,6 @@ static int imxdma_remove(struct platform_device *pdev)
 static struct platform_driver imxdma_driver = {
 	.driver		= {
 		.name	= "imx-dma",
-		.owner	= THIS_MODULE,
 		.of_match_table = imx_dma_of_dev_id,
 	},
 	.id_table	= imx_dma_devtype,
diff --git a/drivers/dma/imx-sdma.c b/drivers/dma/imx-sdma.c
index 88afc48c2ca7..d0df198f62e9 100644
--- a/drivers/dma/imx-sdma.c
+++ b/drivers/dma/imx-sdma.c
@@ -729,6 +729,7 @@ static void sdma_get_pc(struct sdma_channel *sdmac,
 	case IMX_DMATYPE_CSPI:
 	case IMX_DMATYPE_EXT:
 	case IMX_DMATYPE_SSI:
+	case IMX_DMATYPE_SAI:
 		per_2_emi = sdma->script_addrs->app_2_mcu_addr;
 		emi_2_per = sdma->script_addrs->mcu_2_app_addr;
 		break;
@@ -1287,7 +1288,8 @@ static void sdma_load_firmware(const struct firmware *fw, void *context)
 	unsigned short *ram_code;
 
 	if (!fw) {
-		dev_err(sdma->dev, "firmware not found\n");
+		dev_info(sdma->dev, "external firmware not found, using ROM firmware\n");
+		/* In this case we just use the ROM firmware. */
 		return;
 	}
 
@@ -1346,7 +1348,7 @@ static int sdma_get_firmware(struct sdma_engine *sdma,
 	return ret;
 }
 
-static int __init sdma_init(struct sdma_engine *sdma)
+static int sdma_init(struct sdma_engine *sdma)
 {
 	int i, ret;
 	dma_addr_t ccb_phys;
diff --git a/drivers/dma/ioat/dma_v3.c b/drivers/dma/ioat/dma_v3.c
index 895f869d6c2c..32eae38291e5 100644
--- a/drivers/dma/ioat/dma_v3.c
+++ b/drivers/dma/ioat/dma_v3.c
@@ -1265,9 +1265,17 @@ static int ioat_xor_val_self_test(struct ioatdma_device *device)
 	op = IOAT_OP_XOR;
 
 	dest_dma = dma_map_page(dev, dest, 0, PAGE_SIZE, DMA_FROM_DEVICE);
+	if (dma_mapping_error(dev, dest_dma))
+		goto dma_unmap;
+
 	for (i = 0; i < IOAT_NUM_SRC_TEST; i++)
+		dma_srcs[i] = DMA_ERROR_CODE;
+	for (i = 0; i < IOAT_NUM_SRC_TEST; i++) {
 		dma_srcs[i] = dma_map_page(dev, xor_srcs[i], 0, PAGE_SIZE,
 					   DMA_TO_DEVICE);
+		if (dma_mapping_error(dev, dma_srcs[i]))
+			goto dma_unmap;
+	}
 	tx = dma->device_prep_dma_xor(dma_chan, dest_dma, dma_srcs,
 				      IOAT_NUM_SRC_TEST, PAGE_SIZE,
 				      DMA_PREP_INTERRUPT);
@@ -1298,7 +1306,6 @@ static int ioat_xor_val_self_test(struct ioatdma_device *device)
 		goto dma_unmap;
 	}
 
-	dma_unmap_page(dev, dest_dma, PAGE_SIZE, DMA_FROM_DEVICE);
 	for (i = 0; i < IOAT_NUM_SRC_TEST; i++)
 		dma_unmap_page(dev, dma_srcs[i], PAGE_SIZE, DMA_TO_DEVICE);
 
@@ -1313,6 +1320,8 @@ static int ioat_xor_val_self_test(struct ioatdma_device *device)
 	}
 	dma_sync_single_for_device(dev, dest_dma, PAGE_SIZE, DMA_FROM_DEVICE);
 
+	dma_unmap_page(dev, dest_dma, PAGE_SIZE, DMA_FROM_DEVICE);
+
 	/* skip validate if the capability is not present */
 	if (!dma_has_cap(DMA_XOR_VAL, dma_chan->device->cap_mask))
 		goto free_resources;
@@ -1327,8 +1336,13 @@ static int ioat_xor_val_self_test(struct ioatdma_device *device)
 	xor_val_result = 1;
 
 	for (i = 0; i < IOAT_NUM_SRC_TEST + 1; i++)
+		dma_srcs[i] = DMA_ERROR_CODE;
+	for (i = 0; i < IOAT_NUM_SRC_TEST + 1; i++) {
 		dma_srcs[i] = dma_map_page(dev, xor_val_srcs[i], 0, PAGE_SIZE,
 					   DMA_TO_DEVICE);
+		if (dma_mapping_error(dev, dma_srcs[i]))
+			goto dma_unmap;
+	}
 	tx = dma->device_prep_dma_xor_val(dma_chan, dma_srcs,
 					  IOAT_NUM_SRC_TEST + 1, PAGE_SIZE,
 					  &xor_val_result, DMA_PREP_INTERRUPT);
@@ -1374,8 +1388,13 @@ static int ioat_xor_val_self_test(struct ioatdma_device *device)
 
 	xor_val_result = 0;
 	for (i = 0; i < IOAT_NUM_SRC_TEST + 1; i++)
+		dma_srcs[i] = DMA_ERROR_CODE;
+	for (i = 0; i < IOAT_NUM_SRC_TEST + 1; i++) {
 		dma_srcs[i] = dma_map_page(dev, xor_val_srcs[i], 0, PAGE_SIZE,
 					   DMA_TO_DEVICE);
+		if (dma_mapping_error(dev, dma_srcs[i]))
+			goto dma_unmap;
+	}
 	tx = dma->device_prep_dma_xor_val(dma_chan, dma_srcs,
 					  IOAT_NUM_SRC_TEST + 1, PAGE_SIZE,
 					  &xor_val_result, DMA_PREP_INTERRUPT);
@@ -1417,14 +1436,18 @@ static int ioat_xor_val_self_test(struct ioatdma_device *device)
 	goto free_resources;
 dma_unmap:
 	if (op == IOAT_OP_XOR) {
-		dma_unmap_page(dev, dest_dma, PAGE_SIZE, DMA_FROM_DEVICE);
+		if (dest_dma != DMA_ERROR_CODE)
+			dma_unmap_page(dev, dest_dma, PAGE_SIZE,
+				       DMA_FROM_DEVICE);
 		for (i = 0; i < IOAT_NUM_SRC_TEST; i++)
-			dma_unmap_page(dev, dma_srcs[i], PAGE_SIZE,
-				       DMA_TO_DEVICE);
+			if (dma_srcs[i] != DMA_ERROR_CODE)
+				dma_unmap_page(dev, dma_srcs[i], PAGE_SIZE,
+					       DMA_TO_DEVICE);
 	} else if (op == IOAT_OP_XOR_VAL) {
 		for (i = 0; i < IOAT_NUM_SRC_TEST + 1; i++)
-			dma_unmap_page(dev, dma_srcs[i], PAGE_SIZE,
-				       DMA_TO_DEVICE);
+			if (dma_srcs[i] != DMA_ERROR_CODE)
+				dma_unmap_page(dev, dma_srcs[i], PAGE_SIZE,
+					       DMA_TO_DEVICE);
 	}
 free_resources:
 	dma->device_free_chan_resources(dma_chan);
diff --git a/drivers/dma/iop-adma.c b/drivers/dma/iop-adma.c
index c56137bc3868..263d9f6a207e 100644
--- a/drivers/dma/iop-adma.c
+++ b/drivers/dma/iop-adma.c
@@ -1557,7 +1557,6 @@ static struct platform_driver iop_adma_driver = {
 	.probe		= iop_adma_probe,
 	.remove		= iop_adma_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "iop-adma",
 	},
 };
diff --git a/drivers/dma/ipu/ipu_idmac.c b/drivers/dma/ipu/ipu_idmac.c
index bbf62927bd72..c2b017ad139d 100644
--- a/drivers/dma/ipu/ipu_idmac.c
+++ b/drivers/dma/ipu/ipu_idmac.c
@@ -1783,7 +1783,6 @@ static int ipu_remove(struct platform_device *pdev)
 static struct platform_driver ipu_platform_driver = {
 	.driver = {
 		.name	= "ipu-core",
-		.owner	= THIS_MODULE,
 	},
 	.remove		= ipu_remove,
 };
diff --git a/drivers/dma/k3dma.c b/drivers/dma/k3dma.c
index a1f911aaf220..a1de14ab2c51 100644
--- a/drivers/dma/k3dma.c
+++ b/drivers/dma/k3dma.c
@@ -722,7 +722,6 @@ static int k3_dma_probe(struct platform_device *op)
 	d->slave.device_issue_pending = k3_dma_issue_pending;
 	d->slave.device_control = k3_dma_control;
 	d->slave.copy_align = DMA_ALIGN;
-	d->slave.chancnt = d->dma_requests;
 
 	/* init virtual channel */
 	d->chans = devm_kzalloc(&op->dev,
@@ -787,6 +786,7 @@ static int k3_dma_remove(struct platform_device *op)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
 static int k3_dma_suspend(struct device *dev)
 {
 	struct k3_dma_dev *d = dev_get_drvdata(dev);
@@ -816,13 +816,13 @@ static int k3_dma_resume(struct device *dev)
 	k3_dma_enable_dma(d, true);
 	return 0;
 }
+#endif
 
 static SIMPLE_DEV_PM_OPS(k3_dma_pmops, k3_dma_suspend, k3_dma_resume);
 
 static struct platform_driver k3_pdma_driver = {
 	.driver		= {
 		.name	= DRIVER_NAME,
-		.owner  = THIS_MODULE,
 		.pm	= &k3_dma_pmops,
 		.of_match_table = k3_pdma_dt_ids,
 	},
diff --git a/drivers/dma/mmp_pdma.c b/drivers/dma/mmp_pdma.c
index a1a4db5721b8..8b8952f35e6c 100644
--- a/drivers/dma/mmp_pdma.c
+++ b/drivers/dma/mmp_pdma.c
@@ -1098,7 +1098,6 @@ static const struct platform_device_id mmp_pdma_id_table[] = {
 static struct platform_driver mmp_pdma_driver = {
 	.driver		= {
 		.name	= "mmp-pdma",
-		.owner  = THIS_MODULE,
 		.of_match_table = mmp_pdma_dt_ids,
 	},
 	.id_table	= mmp_pdma_id_table,
diff --git a/drivers/dma/mmp_tdma.c b/drivers/dma/mmp_tdma.c
index c6bd015b7165..bfb46957c3dc 100644
--- a/drivers/dma/mmp_tdma.c
+++ b/drivers/dma/mmp_tdma.c
@@ -703,7 +703,6 @@ static const struct platform_device_id mmp_tdma_id_table[] = {
 static struct platform_driver mmp_tdma_driver = {
 	.driver		= {
 		.name	= "mmp-tdma",
-		.owner  = THIS_MODULE,
 		.of_match_table = mmp_tdma_dt_ids,
 	},
 	.id_table	= mmp_tdma_id_table,
diff --git a/drivers/dma/moxart-dma.c b/drivers/dma/moxart-dma.c
index 3258e484e4f6..53032bac06e0 100644
--- a/drivers/dma/moxart-dma.c
+++ b/drivers/dma/moxart-dma.c
@@ -677,7 +677,6 @@ static struct platform_driver moxart_driver = {
 	.remove	= moxart_remove,
 	.driver = {
 		.name		= "moxart-dma-engine",
-		.owner		= THIS_MODULE,
 		.of_match_table	= moxart_dma_match,
 	},
 };
diff --git a/drivers/dma/mpc512x_dma.c b/drivers/dma/mpc512x_dma.c
index 881db2bcb48b..01bec4023de2 100644
--- a/drivers/dma/mpc512x_dma.c
+++ b/drivers/dma/mpc512x_dma.c
@@ -885,6 +885,7 @@ static int mpc_dma_probe(struct platform_device *op)
 	struct resource res;
 	ulong regs_start, regs_size;
 	int retval, i;
+	u8 chancnt;
 
 	mdma = devm_kzalloc(dev, sizeof(struct mpc_dma), GFP_KERNEL);
 	if (!mdma) {
@@ -956,10 +957,6 @@ static int mpc_dma_probe(struct platform_device *op)
 
 	dma = &mdma->dma;
 	dma->dev = dev;
-	if (mdma->is_mpc8308)
-		dma->chancnt = MPC8308_DMACHAN_MAX;
-	else
-		dma->chancnt = MPC512x_DMACHAN_MAX;
 	dma->device_alloc_chan_resources = mpc_dma_alloc_chan_resources;
 	dma->device_free_chan_resources = mpc_dma_free_chan_resources;
 	dma->device_issue_pending = mpc_dma_issue_pending;
@@ -972,7 +969,12 @@ static int mpc_dma_probe(struct platform_device *op)
 	dma_cap_set(DMA_MEMCPY, dma->cap_mask);
 	dma_cap_set(DMA_SLAVE, dma->cap_mask);
 
-	for (i = 0; i < dma->chancnt; i++) {
+	if (mdma->is_mpc8308)
+		chancnt = MPC8308_DMACHAN_MAX;
+	else
+		chancnt = MPC512x_DMACHAN_MAX;
+
+	for (i = 0; i < chancnt; i++) {
 		mchan = &mdma->channels[i];
 
 		mchan->chan.device = dma;
@@ -1090,7 +1092,6 @@ static struct platform_driver mpc_dma_driver = {
 	.remove		= mpc_dma_remove,
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table	= mpc_dma_match,
 	},
 };
diff --git a/drivers/dma/mv_xor.c b/drivers/dma/mv_xor.c
index a63837ca1410..d7ac558c2c1c 100644
--- a/drivers/dma/mv_xor.c
+++ b/drivers/dma/mv_xor.c
@@ -1269,7 +1269,6 @@ static struct platform_driver mv_xor_driver = {
 	.probe		= mv_xor_probe,
 	.remove		= mv_xor_remove,
 	.driver		= {
-		.owner	        = THIS_MODULE,
 		.name	        = MV_XOR_NAME,
 		.of_match_table = of_match_ptr(mv_xor_dt_ids),
 	},
diff --git a/drivers/dma/nbpfaxi.c b/drivers/dma/nbpfaxi.c
index bda20e6e1007..d7d61e1a01c3 100644
--- a/drivers/dma/nbpfaxi.c
+++ b/drivers/dma/nbpfaxi.c
@@ -1500,7 +1500,6 @@ static const struct dev_pm_ops nbpf_pm_ops = {
 
 static struct platform_driver nbpf_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "dma-nbpf",
 		.of_match_table = nbpf_match,
 		.pm = &nbpf_pm_ops,
diff --git a/drivers/dma/omap-dma.c b/drivers/dma/omap-dma.c
index bbea8243f9e8..c0016a68b446 100644
--- a/drivers/dma/omap-dma.c
+++ b/drivers/dma/omap-dma.c
@@ -1074,8 +1074,6 @@ static int omap_dma_chan_init(struct omap_dmadev *od, int dma_sig)
 	vchan_init(&c->vc, &od->ddev);
 	INIT_LIST_HEAD(&c->node);
 
-	od->ddev.chancnt++;
-
 	return 0;
 }
 
@@ -1233,7 +1231,6 @@ static struct platform_driver omap_dma_driver = {
 	.remove	= omap_dma_remove,
 	.driver = {
 		.name = "omap-dma-engine",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(omap_dma_match),
 	},
 };
diff --git a/drivers/dma/pch_dma.c b/drivers/dma/pch_dma.c
index 9f9ca9fe5ce6..6e0e47d76b23 100644
--- a/drivers/dma/pch_dma.c
+++ b/drivers/dma/pch_dma.c
@@ -997,7 +997,7 @@ static void pch_dma_remove(struct pci_dev *pdev)
 #define PCI_DEVICE_ID_ML7831_DMA1_8CH	0x8810
 #define PCI_DEVICE_ID_ML7831_DMA2_4CH	0x8815
 
-const struct pci_device_id pch_dma_id_table[] = {
+static const struct pci_device_id pch_dma_id_table[] = {
 	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_EG20T_PCH_DMA_8CH), 8 },
 	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_EG20T_PCH_DMA_4CH), 4 },
 	{ PCI_VDEVICE(ROHM, PCI_DEVICE_ID_ML7213_DMA1_8CH), 8}, /* UART Video */
diff --git a/drivers/dma/pl330.c b/drivers/dma/pl330.c
index 19a99743cf52..bdf40b530032 100644
--- a/drivers/dma/pl330.c
+++ b/drivers/dma/pl330.c
@@ -27,6 +27,7 @@
 #include <linux/of.h>
 #include <linux/of_dma.h>
 #include <linux/err.h>
+#include <linux/pm_runtime.h>
 
 #include "dmaengine.h"
 #define PL330_MAX_CHAN		8
@@ -265,6 +266,9 @@ static unsigned cmd_line;
 
 #define NR_DEFAULT_DESC	16
 
+/* Delay for runtime PM autosuspend, ms */
+#define PL330_AUTOSUSPEND_DELAY 20
+
 /* Populated by the PL330 core driver for DMA API driver's info */
 struct pl330_config {
 	u32	periph_id;
@@ -1958,6 +1962,7 @@ static void pl330_tasklet(unsigned long data)
 	struct dma_pl330_chan *pch = (struct dma_pl330_chan *)data;
 	struct dma_pl330_desc *desc, *_dt;
 	unsigned long flags;
+	bool power_down = false;
 
 	spin_lock_irqsave(&pch->lock, flags);
 
@@ -1972,10 +1977,17 @@ static void pl330_tasklet(unsigned long data)
 	/* Try to submit a req imm. next to the last completed cookie */
 	fill_queue(pch);
 
-	/* Make sure the PL330 Channel thread is active */
-	spin_lock(&pch->thread->dmac->lock);
-	_start(pch->thread);
-	spin_unlock(&pch->thread->dmac->lock);
+	if (list_empty(&pch->work_list)) {
+		spin_lock(&pch->thread->dmac->lock);
+		_stop(pch->thread);
+		spin_unlock(&pch->thread->dmac->lock);
+		power_down = true;
+	} else {
+		/* Make sure the PL330 Channel thread is active */
+		spin_lock(&pch->thread->dmac->lock);
+		_start(pch->thread);
+		spin_unlock(&pch->thread->dmac->lock);
+	}
 
 	while (!list_empty(&pch->completed_list)) {
 		dma_async_tx_callback callback;
@@ -1990,6 +2002,12 @@ static void pl330_tasklet(unsigned long data)
 		if (pch->cyclic) {
 			desc->status = PREP;
 			list_move_tail(&desc->node, &pch->work_list);
+			if (power_down) {
+				spin_lock(&pch->thread->dmac->lock);
+				_start(pch->thread);
+				spin_unlock(&pch->thread->dmac->lock);
+				power_down = false;
+			}
 		} else {
 			desc->status = FREE;
 			list_move_tail(&desc->node, &pch->dmac->desc_pool);
@@ -2004,6 +2022,12 @@ static void pl330_tasklet(unsigned long data)
 		}
 	}
 	spin_unlock_irqrestore(&pch->lock, flags);
+
+	/* If work list empty, power down */
+	if (power_down) {
+		pm_runtime_mark_last_busy(pch->dmac->ddma.dev);
+		pm_runtime_put_autosuspend(pch->dmac->ddma.dev);
+	}
 }
 
 bool pl330_filter(struct dma_chan *chan, void *param)
@@ -2073,6 +2097,7 @@ static int pl330_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd, unsigned
 
 	switch (cmd) {
 	case DMA_TERMINATE_ALL:
+		pm_runtime_get_sync(pl330->ddma.dev);
 		spin_lock_irqsave(&pch->lock, flags);
 
 		spin_lock(&pl330->lock);
@@ -2099,10 +2124,15 @@ static int pl330_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd, unsigned
 			dma_cookie_complete(&desc->txd);
 		}
 
+		if (!list_empty(&pch->work_list))
+			pm_runtime_put(pl330->ddma.dev);
+
 		list_splice_tail_init(&pch->submitted_list, &pl330->desc_pool);
 		list_splice_tail_init(&pch->work_list, &pl330->desc_pool);
 		list_splice_tail_init(&pch->completed_list, &pl330->desc_pool);
 		spin_unlock_irqrestore(&pch->lock, flags);
+		pm_runtime_mark_last_busy(pl330->ddma.dev);
+		pm_runtime_put_autosuspend(pl330->ddma.dev);
 		break;
 	case DMA_SLAVE_CONFIG:
 		slave_config = (struct dma_slave_config *)arg;
@@ -2138,6 +2168,7 @@ static void pl330_free_chan_resources(struct dma_chan *chan)
 
 	tasklet_kill(&pch->task);
 
+	pm_runtime_get_sync(pch->dmac->ddma.dev);
 	spin_lock_irqsave(&pch->lock, flags);
 
 	pl330_release_channel(pch->thread);
@@ -2147,6 +2178,8 @@ static void pl330_free_chan_resources(struct dma_chan *chan)
 		list_splice_tail_init(&pch->work_list, &pch->dmac->desc_pool);
 
 	spin_unlock_irqrestore(&pch->lock, flags);
+	pm_runtime_mark_last_busy(pch->dmac->ddma.dev);
+	pm_runtime_put_autosuspend(pch->dmac->ddma.dev);
 }
 
 static enum dma_status
@@ -2162,6 +2195,15 @@ static void pl330_issue_pending(struct dma_chan *chan)
 	unsigned long flags;
 
 	spin_lock_irqsave(&pch->lock, flags);
+	if (list_empty(&pch->work_list)) {
+		/*
+		 * Warn on nothing pending. Empty submitted_list may
+		 * break our pm_runtime usage counter as it is
+		 * updated on work_list emptiness status.
+		 */
+		WARN_ON(list_empty(&pch->submitted_list));
+		pm_runtime_get_sync(pch->dmac->ddma.dev);
+	}
 	list_splice_tail_init(&pch->submitted_list, &pch->work_list);
 	spin_unlock_irqrestore(&pch->lock, flags);
 
@@ -2594,6 +2636,46 @@ static int pl330_dma_device_slave_caps(struct dma_chan *dchan,
 	return 0;
 }
 
+/*
+ * Runtime PM callbacks are provided by amba/bus.c driver.
+ *
+ * It is assumed here that IRQ safe runtime PM is chosen in probe and amba
+ * bus driver will only disable/enable the clock in runtime PM callbacks.
+ */
+static int __maybe_unused pl330_suspend(struct device *dev)
+{
+	struct amba_device *pcdev = to_amba_device(dev);
+
+	pm_runtime_disable(dev);
+
+	if (!pm_runtime_status_suspended(dev)) {
+		/* amba did not disable the clock */
+		amba_pclk_disable(pcdev);
+	}
+	amba_pclk_unprepare(pcdev);
+
+	return 0;
+}
+
+static int __maybe_unused pl330_resume(struct device *dev)
+{
+	struct amba_device *pcdev = to_amba_device(dev);
+	int ret;
+
+	ret = amba_pclk_prepare(pcdev);
+	if (ret)
+		return ret;
+
+	if (!pm_runtime_status_suspended(dev))
+		ret = amba_pclk_enable(pcdev);
+
+	pm_runtime_enable(dev);
+
+	return ret;
+}
+
+static SIMPLE_DEV_PM_OPS(pl330_pm, pl330_suspend, pl330_resume);
+
 static int
 pl330_probe(struct amba_device *adev, const struct amba_id *id)
 {
@@ -2619,6 +2701,9 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
 		return -ENOMEM;
 	}
 
+	pd = &pl330->ddma;
+	pd->dev = &adev->dev;
+
 	pl330->mcbufsz = pdat ? pdat->mcbuf_sz : 0;
 
 	res = &adev->res;
@@ -2655,7 +2740,6 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
 	if (!add_desc(pl330, GFP_KERNEL, NR_DEFAULT_DESC))
 		dev_warn(&adev->dev, "unable to allocate desc\n");
 
-	pd = &pl330->ddma;
 	INIT_LIST_HEAD(&pd->channels);
 
 	/* Initialize channel parameters */
@@ -2692,7 +2776,6 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
 		list_add_tail(&pch->chan.device_node, &pd->channels);
 	}
 
-	pd->dev = &adev->dev;
 	if (pdat) {
 		pd->cap_mask = pdat->cap_mask;
 	} else {
@@ -2747,6 +2830,12 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
 		pcfg->data_buf_dep, pcfg->data_bus_width / 8, pcfg->num_chan,
 		pcfg->num_peri, pcfg->num_events);
 
+	pm_runtime_irq_safe(&adev->dev);
+	pm_runtime_use_autosuspend(&adev->dev);
+	pm_runtime_set_autosuspend_delay(&adev->dev, PL330_AUTOSUSPEND_DELAY);
+	pm_runtime_mark_last_busy(&adev->dev);
+	pm_runtime_put_autosuspend(&adev->dev);
+
 	return 0;
 probe_err3:
 	/* Idle the DMAC */
@@ -2773,6 +2862,8 @@ static int pl330_remove(struct amba_device *adev)
 	struct pl330_dmac *pl330 = amba_get_drvdata(adev);
 	struct dma_pl330_chan *pch, *_p;
 
+	pm_runtime_get_noresume(pl330->ddma.dev);
+
 	if (adev->dev.of_node)
 		of_dma_controller_free(adev->dev.of_node);
 
@@ -2811,6 +2902,7 @@ static struct amba_driver pl330_driver = {
 	.drv = {
 		.owner = THIS_MODULE,
 		.name = "dma-pl330",
+		.pm = &pl330_pm,
 	},
 	.id_table = pl330_ids,
 	.probe = pl330_probe,
@@ -2819,6 +2911,6 @@ static struct amba_driver pl330_driver = {
 
 module_amba_driver(pl330_driver);
 
-MODULE_AUTHOR("Jaswinder Singh <jassi.brar@samsung.com>");
+MODULE_AUTHOR("Jaswinder Singh <jassisinghbrar@gmail.com>");
 MODULE_DESCRIPTION("API Driver for PL330 DMAC");
 MODULE_LICENSE("GPL");
diff --git a/drivers/dma/ppc4xx/adma.c b/drivers/dma/ppc4xx/adma.c
index ce7a8d7564ba..fa764a39cd36 100644
--- a/drivers/dma/ppc4xx/adma.c
+++ b/drivers/dma/ppc4xx/adma.c
@@ -4579,7 +4579,6 @@ static struct platform_driver ppc440spe_adma_driver = {
 	.remove = ppc440spe_adma_remove,
 	.driver = {
 		.name = "PPC440SP(E)-ADMA",
-		.owner = THIS_MODULE,
 		.of_match_table = ppc440spe_adma_of_match,
 	},
 };
diff --git a/drivers/dma/qcom_bam_dma.c b/drivers/dma/qcom_bam_dma.c
index 7a4bbb0f80a5..3122a99ec06b 100644
--- a/drivers/dma/qcom_bam_dma.c
+++ b/drivers/dma/qcom_bam_dma.c
@@ -79,35 +79,97 @@ struct bam_async_desc {
 	struct bam_desc_hw desc[0];
 };
 
-#define BAM_CTRL			0x0000
-#define BAM_REVISION			0x0004
-#define BAM_SW_REVISION			0x0080
-#define BAM_NUM_PIPES			0x003C
-#define BAM_TIMER			0x0040
-#define BAM_TIMER_CTRL			0x0044
-#define BAM_DESC_CNT_TRSHLD		0x0008
-#define BAM_IRQ_SRCS			0x000C
-#define BAM_IRQ_SRCS_MSK		0x0010
-#define BAM_IRQ_SRCS_UNMASKED		0x0030
-#define BAM_IRQ_STTS			0x0014
-#define BAM_IRQ_CLR			0x0018
-#define BAM_IRQ_EN			0x001C
-#define BAM_CNFG_BITS			0x007C
-#define BAM_IRQ_SRCS_EE(ee)		(0x0800 + ((ee) * 0x80))
-#define BAM_IRQ_SRCS_MSK_EE(ee)		(0x0804 + ((ee) * 0x80))
-#define BAM_P_CTRL(pipe)		(0x1000 + ((pipe) * 0x1000))
-#define BAM_P_RST(pipe)			(0x1004 + ((pipe) * 0x1000))
-#define BAM_P_HALT(pipe)		(0x1008 + ((pipe) * 0x1000))
-#define BAM_P_IRQ_STTS(pipe)		(0x1010 + ((pipe) * 0x1000))
-#define BAM_P_IRQ_CLR(pipe)		(0x1014 + ((pipe) * 0x1000))
-#define BAM_P_IRQ_EN(pipe)		(0x1018 + ((pipe) * 0x1000))
-#define BAM_P_EVNT_DEST_ADDR(pipe)	(0x182C + ((pipe) * 0x1000))
-#define BAM_P_EVNT_REG(pipe)		(0x1818 + ((pipe) * 0x1000))
-#define BAM_P_SW_OFSTS(pipe)		(0x1800 + ((pipe) * 0x1000))
-#define BAM_P_DATA_FIFO_ADDR(pipe)	(0x1824 + ((pipe) * 0x1000))
-#define BAM_P_DESC_FIFO_ADDR(pipe)	(0x181C + ((pipe) * 0x1000))
-#define BAM_P_EVNT_TRSHLD(pipe)		(0x1828 + ((pipe) * 0x1000))
-#define BAM_P_FIFO_SIZES(pipe)		(0x1820 + ((pipe) * 0x1000))
+enum bam_reg {
+	BAM_CTRL,
+	BAM_REVISION,
+	BAM_NUM_PIPES,
+	BAM_DESC_CNT_TRSHLD,
+	BAM_IRQ_SRCS,
+	BAM_IRQ_SRCS_MSK,
+	BAM_IRQ_SRCS_UNMASKED,
+	BAM_IRQ_STTS,
+	BAM_IRQ_CLR,
+	BAM_IRQ_EN,
+	BAM_CNFG_BITS,
+	BAM_IRQ_SRCS_EE,
+	BAM_IRQ_SRCS_MSK_EE,
+	BAM_P_CTRL,
+	BAM_P_RST,
+	BAM_P_HALT,
+	BAM_P_IRQ_STTS,
+	BAM_P_IRQ_CLR,
+	BAM_P_IRQ_EN,
+	BAM_P_EVNT_DEST_ADDR,
+	BAM_P_EVNT_REG,
+	BAM_P_SW_OFSTS,
+	BAM_P_DATA_FIFO_ADDR,
+	BAM_P_DESC_FIFO_ADDR,
+	BAM_P_EVNT_GEN_TRSHLD,
+	BAM_P_FIFO_SIZES,
+};
+
+struct reg_offset_data {
+	u32 base_offset;
+	unsigned int pipe_mult, evnt_mult, ee_mult;
+};
+
+static const struct reg_offset_data bam_v1_3_reg_info[] = {
+	[BAM_CTRL]		= { 0x0F80, 0x00, 0x00, 0x00 },
+	[BAM_REVISION]		= { 0x0F84, 0x00, 0x00, 0x00 },
+	[BAM_NUM_PIPES]		= { 0x0FBC, 0x00, 0x00, 0x00 },
+	[BAM_DESC_CNT_TRSHLD]	= { 0x0F88, 0x00, 0x00, 0x00 },
+	[BAM_IRQ_SRCS]		= { 0x0F8C, 0x00, 0x00, 0x00 },
+	[BAM_IRQ_SRCS_MSK]	= { 0x0F90, 0x00, 0x00, 0x00 },
+	[BAM_IRQ_SRCS_UNMASKED]	= { 0x0FB0, 0x00, 0x00, 0x00 },
+	[BAM_IRQ_STTS]		= { 0x0F94, 0x00, 0x00, 0x00 },
+	[BAM_IRQ_CLR]		= { 0x0F98, 0x00, 0x00, 0x00 },
+	[BAM_IRQ_EN]		= { 0x0F9C, 0x00, 0x00, 0x00 },
+	[BAM_CNFG_BITS]		= { 0x0FFC, 0x00, 0x00, 0x00 },
+	[BAM_IRQ_SRCS_EE]	= { 0x1800, 0x00, 0x00, 0x80 },
+	[BAM_IRQ_SRCS_MSK_EE]	= { 0x1804, 0x00, 0x00, 0x80 },
+	[BAM_P_CTRL]		= { 0x0000, 0x80, 0x00, 0x00 },
+	[BAM_P_RST]		= { 0x0004, 0x80, 0x00, 0x00 },
+	[BAM_P_HALT]		= { 0x0008, 0x80, 0x00, 0x00 },
+	[BAM_P_IRQ_STTS]	= { 0x0010, 0x80, 0x00, 0x00 },
+	[BAM_P_IRQ_CLR]		= { 0x0014, 0x80, 0x00, 0x00 },
+	[BAM_P_IRQ_EN]		= { 0x0018, 0x80, 0x00, 0x00 },
+	[BAM_P_EVNT_DEST_ADDR]	= { 0x102C, 0x00, 0x40, 0x00 },
+	[BAM_P_EVNT_REG]	= { 0x1018, 0x00, 0x40, 0x00 },
+	[BAM_P_SW_OFSTS]	= { 0x1000, 0x00, 0x40, 0x00 },
+	[BAM_P_DATA_FIFO_ADDR]	= { 0x1024, 0x00, 0x40, 0x00 },
+	[BAM_P_DESC_FIFO_ADDR]	= { 0x101C, 0x00, 0x40, 0x00 },
+	[BAM_P_EVNT_GEN_TRSHLD]	= { 0x1028, 0x00, 0x40, 0x00 },
+	[BAM_P_FIFO_SIZES]	= { 0x1020, 0x00, 0x40, 0x00 },
+};
+
+static const struct reg_offset_data bam_v1_4_reg_info[] = {
+	[BAM_CTRL]		= { 0x0000, 0x00, 0x00, 0x00 },
+	[BAM_REVISION]		= { 0x0004, 0x00, 0x00, 0x00 },
+	[BAM_NUM_PIPES]		= { 0x003C, 0x00, 0x00, 0x00 },
+	[BAM_DESC_CNT_TRSHLD]	= { 0x0008, 0x00, 0x00, 0x00 },
+	[BAM_IRQ_SRCS]		= { 0x000C, 0x00, 0x00, 0x00 },
+	[BAM_IRQ_SRCS_MSK]	= { 0x0010, 0x00, 0x00, 0x00 },
+	[BAM_IRQ_SRCS_UNMASKED]	= { 0x0030, 0x00, 0x00, 0x00 },
+	[BAM_IRQ_STTS]		= { 0x0014, 0x00, 0x00, 0x00 },
+	[BAM_IRQ_CLR]		= { 0x0018, 0x00, 0x00, 0x00 },
+	[BAM_IRQ_EN]		= { 0x001C, 0x00, 0x00, 0x00 },
+	[BAM_CNFG_BITS]		= { 0x007C, 0x00, 0x00, 0x00 },
+	[BAM_IRQ_SRCS_EE]	= { 0x0800, 0x00, 0x00, 0x80 },
+	[BAM_IRQ_SRCS_MSK_EE]	= { 0x0804, 0x00, 0x00, 0x80 },
+	[BAM_P_CTRL]		= { 0x1000, 0x1000, 0x00, 0x00 },
+	[BAM_P_RST]		= { 0x1004, 0x1000, 0x00, 0x00 },
+	[BAM_P_HALT]		= { 0x1008, 0x1000, 0x00, 0x00 },
+	[BAM_P_IRQ_STTS]	= { 0x1010, 0x1000, 0x00, 0x00 },
+	[BAM_P_IRQ_CLR]		= { 0x1014, 0x1000, 0x00, 0x00 },
+	[BAM_P_IRQ_EN]		= { 0x1018, 0x1000, 0x00, 0x00 },
+	[BAM_P_EVNT_DEST_ADDR]	= { 0x102C, 0x00, 0x1000, 0x00 },
+	[BAM_P_EVNT_REG]	= { 0x1018, 0x00, 0x1000, 0x00 },
+	[BAM_P_SW_OFSTS]	= { 0x1000, 0x00, 0x1000, 0x00 },
+	[BAM_P_DATA_FIFO_ADDR]	= { 0x1824, 0x00, 0x1000, 0x00 },
+	[BAM_P_DESC_FIFO_ADDR]	= { 0x181C, 0x00, 0x1000, 0x00 },
+	[BAM_P_EVNT_GEN_TRSHLD]	= { 0x1828, 0x00, 0x1000, 0x00 },
+	[BAM_P_FIFO_SIZES]	= { 0x1820, 0x00, 0x1000, 0x00 },
+};
 
 /* BAM CTRL */
 #define BAM_SW_RST			BIT(0)
@@ -297,6 +359,8 @@ struct bam_device {
 	/* execution environment ID, from DT */
 	u32 ee;
 
+	const struct reg_offset_data *layout;
+
 	struct clk *bamclk;
 	int irq;
 
@@ -305,6 +369,23 @@ struct bam_device {
 };
 
 /**
+ * bam_addr - returns BAM register address
+ * @bdev: bam device
+ * @pipe: pipe instance (ignored when register doesn't have multiple instances)
+ * @reg:  register enum
+ */
+static inline void __iomem *bam_addr(struct bam_device *bdev, u32 pipe,
+		enum bam_reg reg)
+{
+	const struct reg_offset_data r = bdev->layout[reg];
+
+	return bdev->regs + r.base_offset +
+		r.pipe_mult * pipe +
+		r.evnt_mult * pipe +
+		r.ee_mult * bdev->ee;
+}
+
+/**
  * bam_reset_channel - Reset individual BAM DMA channel
  * @bchan: bam channel
  *
@@ -317,8 +398,8 @@ static void bam_reset_channel(struct bam_chan *bchan)
 	lockdep_assert_held(&bchan->vc.lock);
 
 	/* reset channel */
-	writel_relaxed(1, bdev->regs + BAM_P_RST(bchan->id));
-	writel_relaxed(0, bdev->regs + BAM_P_RST(bchan->id));
+	writel_relaxed(1, bam_addr(bdev, bchan->id, BAM_P_RST));
+	writel_relaxed(0, bam_addr(bdev, bchan->id, BAM_P_RST));
 
 	/* don't allow cpu to reorder BAM register accesses done after this */
 	wmb();
@@ -347,17 +428,18 @@ static void bam_chan_init_hw(struct bam_chan *bchan,
 	 * because we allocated 1 more descriptor (8 bytes) than we can use
 	 */
 	writel_relaxed(ALIGN(bchan->fifo_phys, sizeof(struct bam_desc_hw)),
-			bdev->regs + BAM_P_DESC_FIFO_ADDR(bchan->id));
-	writel_relaxed(BAM_DESC_FIFO_SIZE, bdev->regs +
-			BAM_P_FIFO_SIZES(bchan->id));
+			bam_addr(bdev, bchan->id, BAM_P_DESC_FIFO_ADDR));
+	writel_relaxed(BAM_DESC_FIFO_SIZE,
+			bam_addr(bdev, bchan->id, BAM_P_FIFO_SIZES));
 
 	/* enable the per pipe interrupts, enable EOT, ERR, and INT irqs */
-	writel_relaxed(P_DEFAULT_IRQS_EN, bdev->regs + BAM_P_IRQ_EN(bchan->id));
+	writel_relaxed(P_DEFAULT_IRQS_EN,
+			bam_addr(bdev, bchan->id, BAM_P_IRQ_EN));
 
 	/* unmask the specific pipe and EE combo */
-	val = readl_relaxed(bdev->regs + BAM_IRQ_SRCS_MSK_EE(bdev->ee));
+	val = readl_relaxed(bam_addr(bdev, 0, BAM_IRQ_SRCS_MSK_EE));
 	val |= BIT(bchan->id);
-	writel_relaxed(val, bdev->regs + BAM_IRQ_SRCS_MSK_EE(bdev->ee));
+	writel_relaxed(val, bam_addr(bdev, 0, BAM_IRQ_SRCS_MSK_EE));
 
 	/* don't allow cpu to reorder the channel enable done below */
 	wmb();
@@ -367,7 +449,7 @@ static void bam_chan_init_hw(struct bam_chan *bchan,
 	if (dir == DMA_DEV_TO_MEM)
 		val |= P_DIRECTION;
 
-	writel_relaxed(val, bdev->regs + BAM_P_CTRL(bchan->id));
+	writel_relaxed(val, bam_addr(bdev, bchan->id, BAM_P_CTRL));
 
 	bchan->initialized = 1;
 
@@ -432,12 +514,12 @@ static void bam_free_chan(struct dma_chan *chan)
 	bchan->fifo_virt = NULL;
 
 	/* mask irq for pipe/channel */
-	val = readl_relaxed(bdev->regs + BAM_IRQ_SRCS_MSK_EE(bdev->ee));
+	val = readl_relaxed(bam_addr(bdev, 0, BAM_IRQ_SRCS_MSK_EE));
 	val &= ~BIT(bchan->id);
-	writel_relaxed(val, bdev->regs + BAM_IRQ_SRCS_MSK_EE(bdev->ee));
+	writel_relaxed(val, bam_addr(bdev, 0, BAM_IRQ_SRCS_MSK_EE));
 
 	/* disable irq */
-	writel_relaxed(0, bdev->regs + BAM_P_IRQ_EN(bchan->id));
+	writel_relaxed(0, bam_addr(bdev, bchan->id, BAM_P_IRQ_EN));
 }
 
 /**
@@ -583,14 +665,14 @@ static int bam_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 	switch (cmd) {
 	case DMA_PAUSE:
 		spin_lock_irqsave(&bchan->vc.lock, flag);
-		writel_relaxed(1, bdev->regs + BAM_P_HALT(bchan->id));
+		writel_relaxed(1, bam_addr(bdev, bchan->id, BAM_P_HALT));
 		bchan->paused = 1;
 		spin_unlock_irqrestore(&bchan->vc.lock, flag);
 		break;
 
 	case DMA_RESUME:
 		spin_lock_irqsave(&bchan->vc.lock, flag);
-		writel_relaxed(0, bdev->regs + BAM_P_HALT(bchan->id));
+		writel_relaxed(0, bam_addr(bdev, bchan->id, BAM_P_HALT));
 		bchan->paused = 0;
 		spin_unlock_irqrestore(&bchan->vc.lock, flag);
 		break;
@@ -626,7 +708,7 @@ static u32 process_channel_irqs(struct bam_device *bdev)
 	unsigned long flags;
 	struct bam_async_desc *async_desc;
 
-	srcs = readl_relaxed(bdev->regs + BAM_IRQ_SRCS_EE(bdev->ee));
+	srcs = readl_relaxed(bam_addr(bdev, 0, BAM_IRQ_SRCS_EE));
 
 	/* return early if no pipe/channel interrupts are present */
 	if (!(srcs & P_IRQ))
@@ -639,11 +721,9 @@ static u32 process_channel_irqs(struct bam_device *bdev)
 			continue;
 
 		/* clear pipe irq */
-		pipe_stts = readl_relaxed(bdev->regs +
-			BAM_P_IRQ_STTS(i));
+		pipe_stts = readl_relaxed(bam_addr(bdev, i, BAM_P_IRQ_STTS));
 
-		writel_relaxed(pipe_stts, bdev->regs +
-				BAM_P_IRQ_CLR(i));
+		writel_relaxed(pipe_stts, bam_addr(bdev, i, BAM_P_IRQ_CLR));
 
 		spin_lock_irqsave(&bchan->vc.lock, flags);
 		async_desc = bchan->curr_txd;
@@ -694,12 +774,12 @@ static irqreturn_t bam_dma_irq(int irq, void *data)
 		tasklet_schedule(&bdev->task);
 
 	if (srcs & BAM_IRQ)
-		clr_mask = readl_relaxed(bdev->regs + BAM_IRQ_STTS);
+		clr_mask = readl_relaxed(bam_addr(bdev, 0, BAM_IRQ_STTS));
 
 	/* don't allow reorder of the various accesses to the BAM registers */
 	mb();
 
-	writel_relaxed(clr_mask, bdev->regs + BAM_IRQ_CLR);
+	writel_relaxed(clr_mask, bam_addr(bdev, 0, BAM_IRQ_CLR));
 
 	return IRQ_HANDLED;
 }
@@ -763,7 +843,7 @@ static void bam_apply_new_config(struct bam_chan *bchan,
 	else
 		maxburst = bchan->slave.dst_maxburst;
 
-	writel_relaxed(maxburst, bdev->regs + BAM_DESC_CNT_TRSHLD);
+	writel_relaxed(maxburst, bam_addr(bdev, 0, BAM_DESC_CNT_TRSHLD));
 
 	bchan->reconfigure = 0;
 }
@@ -830,7 +910,7 @@ static void bam_start_dma(struct bam_chan *bchan)
 	/* ensure descriptor writes and dma start not reordered */
 	wmb();
 	writel_relaxed(bchan->tail * sizeof(struct bam_desc_hw),
-			bdev->regs + BAM_P_EVNT_REG(bchan->id));
+			bam_addr(bdev, bchan->id, BAM_P_EVNT_REG));
 }
 
 /**
@@ -918,43 +998,44 @@ static int bam_init(struct bam_device *bdev)
 	u32 val;
 
 	/* read revision and configuration information */
-	val = readl_relaxed(bdev->regs + BAM_REVISION) >> NUM_EES_SHIFT;
+	val = readl_relaxed(bam_addr(bdev, 0, BAM_REVISION)) >> NUM_EES_SHIFT;
 	val &= NUM_EES_MASK;
 
 	/* check that configured EE is within range */
 	if (bdev->ee >= val)
 		return -EINVAL;
 
-	val = readl_relaxed(bdev->regs + BAM_NUM_PIPES);
+	val = readl_relaxed(bam_addr(bdev, 0, BAM_NUM_PIPES));
 	bdev->num_channels = val & BAM_NUM_PIPES_MASK;
 
 	/* s/w reset bam */
 	/* after reset all pipes are disabled and idle */
-	val = readl_relaxed(bdev->regs + BAM_CTRL);
+	val = readl_relaxed(bam_addr(bdev, 0, BAM_CTRL));
 	val |= BAM_SW_RST;
-	writel_relaxed(val, bdev->regs + BAM_CTRL);
+	writel_relaxed(val, bam_addr(bdev, 0, BAM_CTRL));
 	val &= ~BAM_SW_RST;
-	writel_relaxed(val, bdev->regs + BAM_CTRL);
+	writel_relaxed(val, bam_addr(bdev, 0, BAM_CTRL));
 
 	/* make sure previous stores are visible before enabling BAM */
 	wmb();
 
 	/* enable bam */
 	val |= BAM_EN;
-	writel_relaxed(val, bdev->regs + BAM_CTRL);
+	writel_relaxed(val, bam_addr(bdev, 0, BAM_CTRL));
 
 	/* set descriptor threshhold, start with 4 bytes */
-	writel_relaxed(DEFAULT_CNT_THRSHLD, bdev->regs + BAM_DESC_CNT_TRSHLD);
+	writel_relaxed(DEFAULT_CNT_THRSHLD,
+			bam_addr(bdev, 0, BAM_DESC_CNT_TRSHLD));
 
 	/* Enable default set of h/w workarounds, ie all except BAM_FULL_PIPE */
-	writel_relaxed(BAM_CNFG_BITS_DEFAULT, bdev->regs + BAM_CNFG_BITS);
+	writel_relaxed(BAM_CNFG_BITS_DEFAULT, bam_addr(bdev, 0, BAM_CNFG_BITS));
 
 	/* enable irqs for errors */
 	writel_relaxed(BAM_ERROR_EN | BAM_HRESP_ERR_EN,
-				bdev->regs + BAM_IRQ_EN);
+			bam_addr(bdev, 0, BAM_IRQ_EN));
 
 	/* unmask global bam interrupt */
-	writel_relaxed(BAM_IRQ_MSK, bdev->regs + BAM_IRQ_SRCS_MSK_EE(bdev->ee));
+	writel_relaxed(BAM_IRQ_MSK, bam_addr(bdev, 0, BAM_IRQ_SRCS_MSK_EE));
 
 	return 0;
 }
@@ -969,9 +1050,18 @@ static void bam_channel_init(struct bam_device *bdev, struct bam_chan *bchan,
 	bchan->vc.desc_free = bam_dma_free_desc;
 }
 
+static const struct of_device_id bam_of_match[] = {
+	{ .compatible = "qcom,bam-v1.3.0", .data = &bam_v1_3_reg_info },
+	{ .compatible = "qcom,bam-v1.4.0", .data = &bam_v1_4_reg_info },
+	{}
+};
+
+MODULE_DEVICE_TABLE(of, bam_of_match);
+
 static int bam_dma_probe(struct platform_device *pdev)
 {
 	struct bam_device *bdev;
+	const struct of_device_id *match;
 	struct resource *iores;
 	int ret, i;
 
@@ -981,6 +1071,14 @@ static int bam_dma_probe(struct platform_device *pdev)
 
 	bdev->dev = &pdev->dev;
 
+	match = of_match_node(bam_of_match, pdev->dev.of_node);
+	if (!match) {
+		dev_err(&pdev->dev, "Unsupported BAM module\n");
+		return -ENODEV;
+	}
+
+	bdev->layout = match->data;
+
 	iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	bdev->regs = devm_ioremap_resource(&pdev->dev, iores);
 	if (IS_ERR(bdev->regs))
@@ -1084,7 +1182,7 @@ static int bam_dma_remove(struct platform_device *pdev)
 	dma_async_device_unregister(&bdev->common);
 
 	/* mask all interrupts for this execution environment */
-	writel_relaxed(0, bdev->regs + BAM_IRQ_SRCS_MSK_EE(bdev->ee));
+	writel_relaxed(0, bam_addr(bdev, 0,  BAM_IRQ_SRCS_MSK_EE));
 
 	devm_free_irq(bdev->dev, bdev->irq, bdev);
 
@@ -1104,18 +1202,11 @@ static int bam_dma_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static const struct of_device_id bam_of_match[] = {
-	{ .compatible = "qcom,bam-v1.4.0", },
-	{}
-};
-MODULE_DEVICE_TABLE(of, bam_of_match);
-
 static struct platform_driver bam_dma_driver = {
 	.probe = bam_dma_probe,
 	.remove = bam_dma_remove,
 	.driver = {
 		.name = "bam-dma-engine",
-		.owner = THIS_MODULE,
 		.of_match_table = bam_of_match,
 	},
 };
diff --git a/drivers/dma/s3c24xx-dma.c b/drivers/dma/s3c24xx-dma.c
index 7416572d1e40..6941a77521c3 100644
--- a/drivers/dma/s3c24xx-dma.c
+++ b/drivers/dma/s3c24xx-dma.c
@@ -1402,7 +1402,6 @@ static int s3c24xx_dma_remove(struct platform_device *pdev)
 static struct platform_driver s3c24xx_dma_driver = {
 	.driver		= {
 		.name	= "s3c24xx-dma",
-		.owner	= THIS_MODULE,
 	},
 	.id_table	= s3c24xx_dma_driver_ids,
 	.probe		= s3c24xx_dma_probe,
diff --git a/drivers/dma/sa11x0-dma.c b/drivers/dma/sa11x0-dma.c
index 4b0ef043729a..96bb62c39c41 100644
--- a/drivers/dma/sa11x0-dma.c
+++ b/drivers/dma/sa11x0-dma.c
@@ -829,7 +829,6 @@ static int sa11x0_dma_init_dmadev(struct dma_device *dmadev,
 {
 	unsigned i;
 
-	dmadev->chancnt = ARRAY_SIZE(chan_desc);
 	INIT_LIST_HEAD(&dmadev->channels);
 	dmadev->dev = dev;
 	dmadev->device_alloc_chan_resources = sa11x0_dma_alloc_chan_resources;
@@ -838,7 +837,7 @@ static int sa11x0_dma_init_dmadev(struct dma_device *dmadev,
 	dmadev->device_tx_status = sa11x0_dma_tx_status;
 	dmadev->device_issue_pending = sa11x0_dma_issue_pending;
 
-	for (i = 0; i < dmadev->chancnt; i++) {
+	for (i = 0; i < ARRAY_SIZE(chan_desc); i++) {
 		struct sa11x0_dma_chan *c;
 
 		c = kzalloc(sizeof(*c), GFP_KERNEL);
@@ -1064,7 +1063,6 @@ static const struct dev_pm_ops sa11x0_dma_pm_ops = {
 static struct platform_driver sa11x0_dma_driver = {
 	.driver = {
 		.name	= "sa11x0-dma",
-		.owner	= THIS_MODULE,
 		.pm	= &sa11x0_dma_pm_ops,
 	},
 	.probe		= sa11x0_dma_probe,
diff --git a/drivers/dma/sh/rcar-audmapp.c b/drivers/dma/sh/rcar-audmapp.c
index 80fd2aeb4870..d95bbdd721f4 100644
--- a/drivers/dma/sh/rcar-audmapp.c
+++ b/drivers/dma/sh/rcar-audmapp.c
@@ -253,7 +253,6 @@ static int audmapp_chan_probe(struct platform_device *pdev,
 
 static void audmapp_chan_remove(struct audmapp_device *audev)
 {
-	struct dma_device *dma_dev = &audev->shdma_dev.dma_dev;
 	struct shdma_chan *schan;
 	int i;
 
@@ -261,7 +260,6 @@ static void audmapp_chan_remove(struct audmapp_device *audev)
 		BUG_ON(!schan);
 		shdma_chan_remove(schan);
 	}
-	dma_dev->chancnt = 0;
 }
 
 static struct dma_chan *audmapp_of_xlate(struct of_phandle_args *dma_spec,
@@ -367,7 +365,6 @@ static struct platform_driver audmapp_driver = {
 	.probe		= audmapp_probe,
 	.remove		= audmapp_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "rcar-audmapp-engine",
 		.of_match_table = audmapp_of_match,
 	},
diff --git a/drivers/dma/sh/rcar-hpbdma.c b/drivers/dma/sh/rcar-hpbdma.c
index b212d9471ab5..20a6f6f2a018 100644
--- a/drivers/dma/sh/rcar-hpbdma.c
+++ b/drivers/dma/sh/rcar-hpbdma.c
@@ -619,7 +619,6 @@ error:
 
 static void hpb_dmae_chan_remove(struct hpb_dmae_device *hpbdev)
 {
-	struct dma_device *dma_dev = &hpbdev->shdma_dev.dma_dev;
 	struct shdma_chan *schan;
 	int i;
 
@@ -628,7 +627,6 @@ static void hpb_dmae_chan_remove(struct hpb_dmae_device *hpbdev)
 
 		shdma_chan_remove(schan);
 	}
-	dma_dev->chancnt = 0;
 }
 
 static int hpb_dmae_remove(struct platform_device *pdev)
@@ -655,7 +653,6 @@ static struct platform_driver hpb_dmae_driver = {
 	.remove		= hpb_dmae_remove,
 	.shutdown	= hpb_dmae_shutdown,
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= "hpb-dma-engine",
 	},
 };
diff --git a/drivers/dma/sh/shdma-base.c b/drivers/dma/sh/shdma-base.c
index 42d497416196..3a2adb131d46 100644
--- a/drivers/dma/sh/shdma-base.c
+++ b/drivers/dma/sh/shdma-base.c
@@ -391,6 +391,8 @@ static dma_async_tx_callback __ld_cleanup(struct shdma_chan *schan, bool all)
 				dev_dbg(schan->dev, "Bring down channel %d\n", schan->id);
 				pm_runtime_put(schan->dev);
 				schan->pm_state = SHDMA_PM_ESTABLISHED;
+			} else if (schan->pm_state == SHDMA_PM_PENDING) {
+				shdma_chan_xfer_ld_queue(schan);
 			}
 		}
 	}
@@ -951,7 +953,7 @@ void shdma_chan_probe(struct shdma_dev *sdev,
 	/* Add the channel to DMA device channel list */
 	list_add_tail(&schan->dma_chan.device_node,
 			&sdev->dma_dev.channels);
-	sdev->schan[sdev->dma_dev.chancnt++] = schan;
+	sdev->schan[id] = schan;
 }
 EXPORT_SYMBOL(shdma_chan_probe);
 
diff --git a/drivers/dma/sh/shdma-of.c b/drivers/dma/sh/shdma-of.c
index b4ff9d3e56d1..f999f9b0d314 100644
--- a/drivers/dma/sh/shdma-of.c
+++ b/drivers/dma/sh/shdma-of.c
@@ -66,7 +66,6 @@ MODULE_DEVICE_TABLE(of, sh_dmae_of_match);
 
 static struct platform_driver shdma_of = {
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "shdma-of",
 		.of_match_table = shdma_of_match,
 	},
diff --git a/drivers/dma/sh/shdmac.c b/drivers/dma/sh/shdmac.c
index 58eb85770eba..aec8a84784a4 100644
--- a/drivers/dma/sh/shdmac.c
+++ b/drivers/dma/sh/shdmac.c
@@ -572,7 +572,6 @@ err_no_irq:
 
 static void sh_dmae_chan_remove(struct sh_dmae_device *shdev)
 {
-	struct dma_device *dma_dev = &shdev->shdma_dev.dma_dev;
 	struct shdma_chan *schan;
 	int i;
 
@@ -581,7 +580,6 @@ static void sh_dmae_chan_remove(struct sh_dmae_device *shdev)
 
 		shdma_chan_remove(schan);
 	}
-	dma_dev->chancnt = 0;
 }
 
 static void sh_dmae_shutdown(struct platform_device *pdev)
@@ -925,7 +923,6 @@ static int sh_dmae_remove(struct platform_device *pdev)
 
 static struct platform_driver sh_dmae_driver = {
 	.driver 	= {
-		.owner	= THIS_MODULE,
 		.pm	= &sh_dmae_pm,
 		.name	= SH_DMAE_DRV_NAME,
 		.of_match_table = sh_dmae_of_match,
diff --git a/drivers/dma/sh/sudmac.c b/drivers/dma/sh/sudmac.c
index 3ce103909896..6da2eaa6c294 100644
--- a/drivers/dma/sh/sudmac.c
+++ b/drivers/dma/sh/sudmac.c
@@ -295,7 +295,6 @@ err_no_irq:
 
 static void sudmac_chan_remove(struct sudmac_device *su_dev)
 {
-	struct dma_device *dma_dev = &su_dev->shdma_dev.dma_dev;
 	struct shdma_chan *schan;
 	int i;
 
@@ -304,7 +303,6 @@ static void sudmac_chan_remove(struct sudmac_device *su_dev)
 
 		shdma_chan_remove(schan);
 	}
-	dma_dev->chancnt = 0;
 }
 
 static dma_addr_t sudmac_slave_addr(struct shdma_chan *schan)
@@ -411,7 +409,6 @@ static int sudmac_remove(struct platform_device *pdev)
 
 static struct platform_driver sudmac_driver = {
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= SUDMAC_DRV_NAME,
 	},
 	.probe		= sudmac_probe,
diff --git a/drivers/dma/sirf-dma.c b/drivers/dma/sirf-dma.c
index aac03ab10c54..3492a5f91d31 100644
--- a/drivers/dma/sirf-dma.c
+++ b/drivers/dma/sirf-dma.c
@@ -735,7 +735,6 @@ static int sirfsoc_dma_probe(struct platform_device *op)
 
 	dma = &sdma->dma;
 	dma->dev = dev;
-	dma->chancnt = SIRFSOC_DMA_CHANNELS;
 
 	dma->device_alloc_chan_resources = sirfsoc_dma_alloc_chan_resources;
 	dma->device_free_chan_resources = sirfsoc_dma_free_chan_resources;
@@ -752,7 +751,7 @@ static int sirfsoc_dma_probe(struct platform_device *op)
 	dma_cap_set(DMA_INTERLEAVE, dma->cap_mask);
 	dma_cap_set(DMA_PRIVATE, dma->cap_mask);
 
-	for (i = 0; i < dma->chancnt; i++) {
+	for (i = 0; i < SIRFSOC_DMA_CHANNELS; i++) {
 		schan = &sdma->channels[i];
 
 		schan->chan.device = dma;
@@ -835,6 +834,7 @@ static int sirfsoc_dma_runtime_resume(struct device *dev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
 static int sirfsoc_dma_pm_suspend(struct device *dev)
 {
 	struct sirfsoc_dma *sdma = dev_get_drvdata(dev);
@@ -916,6 +916,7 @@ static int sirfsoc_dma_pm_resume(struct device *dev)
 
 	return 0;
 }
+#endif
 
 static const struct dev_pm_ops sirfsoc_dma_pm_ops = {
 	SET_RUNTIME_PM_OPS(sirfsoc_dma_runtime_suspend, sirfsoc_dma_runtime_resume, NULL)
@@ -933,7 +934,6 @@ static struct platform_driver sirfsoc_dma_driver = {
 	.remove		= sirfsoc_dma_remove,
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 		.pm = &sirfsoc_dma_pm_ops,
 		.of_match_table	= sirfsoc_dma_match,
 	},
diff --git a/drivers/dma/ste_dma40.c b/drivers/dma/ste_dma40.c
index d9ca3e32d748..15d49461c0d2 100644
--- a/drivers/dma/ste_dma40.c
+++ b/drivers/dma/ste_dma40.c
@@ -3432,6 +3432,7 @@ static int __init d40_lcla_allocate(struct d40_base *base)
 
 			d40_err(base->dev, "Failed to allocate %d pages.\n",
 				base->lcla_pool.pages);
+			ret = -ENOMEM;
 
 			for (j = 0; j < i; j++)
 				free_pages(page_list[j], base->lcla_pool.pages);
@@ -3749,7 +3750,6 @@ static const struct of_device_id d40_match[] = {
 
 static struct platform_driver d40_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name  = D40_NAME,
 		.pm = &dma40_pm_ops,
 		.of_match_table = d40_match,
diff --git a/drivers/dma/sun6i-dma.c b/drivers/dma/sun6i-dma.c
index 91292f5513ff..159f1736a16f 100644
--- a/drivers/dma/sun6i-dma.c
+++ b/drivers/dma/sun6i-dma.c
@@ -18,6 +18,7 @@
 #include <linux/interrupt.h>
 #include <linux/module.h>
 #include <linux/of_dma.h>
+#include <linux/of_device.h>
 #include <linux/platform_device.h>
 #include <linux/reset.h>
 #include <linux/slab.h>
@@ -26,24 +27,6 @@
 #include "virt-dma.h"
 
 /*
- * There's 16 physical channels that can work in parallel.
- *
- * However we have 30 different endpoints for our requests.
- *
- * Since the channels are able to handle only an unidirectional
- * transfer, we need to allocate more virtual channels so that
- * everyone can grab one channel.
- *
- * Some devices can't work in both direction (mostly because it
- * wouldn't make sense), so we have a bit fewer virtual channels than
- * 2 channels per endpoints.
- */
-
-#define NR_MAX_CHANNELS		16
-#define NR_MAX_REQUESTS		30
-#define NR_MAX_VCHANS		53
-
-/*
  * Common registers
  */
 #define DMA_IRQ_EN(x)		((x) * 0x04)
@@ -60,6 +43,12 @@
 #define DMA_STAT		0x30
 
 /*
+ * sun8i specific registers
+ */
+#define SUN8I_DMA_GATE		0x20
+#define SUN8I_DMA_GATE_ENABLE	0x4
+
+/*
  * Channels specific registers
  */
 #define DMA_CHAN_ENABLE		0x00
@@ -102,6 +91,19 @@
 #define DRQ_SDRAM	1
 
 /*
+ * Hardware channels / ports representation
+ *
+ * The hardware is used in several SoCs, with differing numbers
+ * of channels and endpoints. This structure ties those numbers
+ * to a certain compatible string.
+ */
+struct sun6i_dma_config {
+	u32 nr_max_channels;
+	u32 nr_max_requests;
+	u32 nr_max_vchans;
+};
+
+/*
  * Hardware representation of the LLI
  *
  * The hardware will be fed the physical address of this structure,
@@ -159,6 +161,7 @@ struct sun6i_dma_dev {
 	struct dma_pool		*pool;
 	struct sun6i_pchan	*pchans;
 	struct sun6i_vchan	*vchans;
+	const struct sun6i_dma_config *cfg;
 };
 
 static struct device *chan2dev(struct dma_chan *chan)
@@ -426,6 +429,7 @@ static int sun6i_dma_start_desc(struct sun6i_vchan *vchan)
 static void sun6i_dma_tasklet(unsigned long data)
 {
 	struct sun6i_dma_dev *sdev = (struct sun6i_dma_dev *)data;
+	const struct sun6i_dma_config *cfg = sdev->cfg;
 	struct sun6i_vchan *vchan;
 	struct sun6i_pchan *pchan;
 	unsigned int pchan_alloc = 0;
@@ -453,7 +457,7 @@ static void sun6i_dma_tasklet(unsigned long data)
 	}
 
 	spin_lock_irq(&sdev->lock);
-	for (pchan_idx = 0; pchan_idx < NR_MAX_CHANNELS; pchan_idx++) {
+	for (pchan_idx = 0; pchan_idx < cfg->nr_max_channels; pchan_idx++) {
 		pchan = &sdev->pchans[pchan_idx];
 
 		if (pchan->vchan || list_empty(&sdev->pending))
@@ -474,7 +478,7 @@ static void sun6i_dma_tasklet(unsigned long data)
 	}
 	spin_unlock_irq(&sdev->lock);
 
-	for (pchan_idx = 0; pchan_idx < NR_MAX_CHANNELS; pchan_idx++) {
+	for (pchan_idx = 0; pchan_idx < cfg->nr_max_channels; pchan_idx++) {
 		if (!(pchan_alloc & BIT(pchan_idx)))
 			continue;
 
@@ -496,7 +500,7 @@ static irqreturn_t sun6i_dma_interrupt(int irq, void *dev_id)
 	int i, j, ret = IRQ_NONE;
 	u32 status;
 
-	for (i = 0; i < 2; i++) {
+	for (i = 0; i < sdev->cfg->nr_max_channels / DMA_IRQ_CHAN_NR; i++) {
 		status = readl(sdev->base + DMA_IRQ_STAT(i));
 		if (!status)
 			continue;
@@ -506,7 +510,7 @@ static irqreturn_t sun6i_dma_interrupt(int irq, void *dev_id)
 
 		writel(status, sdev->base + DMA_IRQ_STAT(i));
 
-		for (j = 0; (j < 8) && status; j++) {
+		for (j = 0; (j < DMA_IRQ_CHAN_NR) && status; j++) {
 			if (status & DMA_IRQ_QUEUE) {
 				pchan = sdev->pchans + j;
 				vchan = pchan->vchan;
@@ -519,7 +523,7 @@ static irqreturn_t sun6i_dma_interrupt(int irq, void *dev_id)
 				}
 			}
 
-			status = status >> 4;
+			status = status >> DMA_IRQ_CHAN_WIDTH;
 		}
 
 		if (!atomic_read(&sdev->tasklet_shutdown))
@@ -815,7 +819,7 @@ static struct dma_chan *sun6i_dma_of_xlate(struct of_phandle_args *dma_spec,
 	struct dma_chan *chan;
 	u8 port = dma_spec->args[0];
 
-	if (port > NR_MAX_REQUESTS)
+	if (port > sdev->cfg->nr_max_requests)
 		return NULL;
 
 	chan = dma_get_any_slave_channel(&sdev->slave);
@@ -848,7 +852,7 @@ static inline void sun6i_dma_free(struct sun6i_dma_dev *sdev)
 {
 	int i;
 
-	for (i = 0; i < NR_MAX_VCHANS; i++) {
+	for (i = 0; i < sdev->cfg->nr_max_vchans; i++) {
 		struct sun6i_vchan *vchan = &sdev->vchans[i];
 
 		list_del(&vchan->vc.chan.device_node);
@@ -856,8 +860,48 @@ static inline void sun6i_dma_free(struct sun6i_dma_dev *sdev)
 	}
 }
 
+/*
+ * For A31:
+ *
+ * There's 16 physical channels that can work in parallel.
+ *
+ * However we have 30 different endpoints for our requests.
+ *
+ * Since the channels are able to handle only an unidirectional
+ * transfer, we need to allocate more virtual channels so that
+ * everyone can grab one channel.
+ *
+ * Some devices can't work in both direction (mostly because it
+ * wouldn't make sense), so we have a bit fewer virtual channels than
+ * 2 channels per endpoints.
+ */
+
+static struct sun6i_dma_config sun6i_a31_dma_cfg = {
+	.nr_max_channels = 16,
+	.nr_max_requests = 30,
+	.nr_max_vchans   = 53,
+};
+
+/*
+ * The A23 only has 8 physical channels, a maximum DRQ port id of 24,
+ * and a total of 37 usable source and destination endpoints.
+ */
+
+static struct sun6i_dma_config sun8i_a23_dma_cfg = {
+	.nr_max_channels = 8,
+	.nr_max_requests = 24,
+	.nr_max_vchans   = 37,
+};
+
+static struct of_device_id sun6i_dma_match[] = {
+	{ .compatible = "allwinner,sun6i-a31-dma", .data = &sun6i_a31_dma_cfg },
+	{ .compatible = "allwinner,sun8i-a23-dma", .data = &sun8i_a23_dma_cfg },
+	{ /* sentinel */ }
+};
+
 static int sun6i_dma_probe(struct platform_device *pdev)
 {
+	const struct of_device_id *device;
 	struct sun6i_dma_dev *sdc;
 	struct resource *res;
 	int ret, i;
@@ -866,6 +910,11 @@ static int sun6i_dma_probe(struct platform_device *pdev)
 	if (!sdc)
 		return -ENOMEM;
 
+	device = of_match_device(sun6i_dma_match, &pdev->dev);
+	if (!device)
+		return -ENODEV;
+	sdc->cfg = device->data;
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	sdc->base = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(sdc->base))
@@ -912,31 +961,30 @@ static int sun6i_dma_probe(struct platform_device *pdev)
 	sdc->slave.device_prep_slave_sg		= sun6i_dma_prep_slave_sg;
 	sdc->slave.device_prep_dma_memcpy	= sun6i_dma_prep_dma_memcpy;
 	sdc->slave.device_control		= sun6i_dma_control;
-	sdc->slave.chancnt			= NR_MAX_VCHANS;
 	sdc->slave.copy_align			= 4;
 
 	sdc->slave.dev = &pdev->dev;
 
-	sdc->pchans = devm_kcalloc(&pdev->dev, NR_MAX_CHANNELS,
+	sdc->pchans = devm_kcalloc(&pdev->dev, sdc->cfg->nr_max_channels,
 				   sizeof(struct sun6i_pchan), GFP_KERNEL);
 	if (!sdc->pchans)
 		return -ENOMEM;
 
-	sdc->vchans = devm_kcalloc(&pdev->dev, NR_MAX_VCHANS,
+	sdc->vchans = devm_kcalloc(&pdev->dev, sdc->cfg->nr_max_vchans,
 				   sizeof(struct sun6i_vchan), GFP_KERNEL);
 	if (!sdc->vchans)
 		return -ENOMEM;
 
 	tasklet_init(&sdc->task, sun6i_dma_tasklet, (unsigned long)sdc);
 
-	for (i = 0; i < NR_MAX_CHANNELS; i++) {
+	for (i = 0; i < sdc->cfg->nr_max_channels; i++) {
 		struct sun6i_pchan *pchan = &sdc->pchans[i];
 
 		pchan->idx = i;
 		pchan->base = sdc->base + 0x100 + i * 0x40;
 	}
 
-	for (i = 0; i < NR_MAX_VCHANS; i++) {
+	for (i = 0; i < sdc->cfg->nr_max_vchans; i++) {
 		struct sun6i_vchan *vchan = &sdc->vchans[i];
 
 		INIT_LIST_HEAD(&vchan->node);
@@ -976,6 +1024,15 @@ static int sun6i_dma_probe(struct platform_device *pdev)
 		goto err_dma_unregister;
 	}
 
+	/*
+	 * sun8i variant requires us to toggle a dma gating register,
+	 * as seen in Allwinner's SDK. This register is not documented
+	 * in the A23 user manual.
+	 */
+	if (of_device_is_compatible(pdev->dev.of_node,
+				    "allwinner,sun8i-a23-dma"))
+		writel(SUN8I_DMA_GATE_ENABLE, sdc->base + SUN8I_DMA_GATE);
+
 	return 0;
 
 err_dma_unregister:
@@ -1008,11 +1065,6 @@ static int sun6i_dma_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id sun6i_dma_match[] = {
-	{ .compatible = "allwinner,sun6i-a31-dma" },
-	{ /* sentinel */ }
-};
-
 static struct platform_driver sun6i_dma_driver = {
 	.probe		= sun6i_dma_probe,
 	.remove		= sun6i_dma_remove,
diff --git a/drivers/dma/tegra20-apb-dma.c b/drivers/dma/tegra20-apb-dma.c
index 1c867d0303db..d8450c3f35f0 100644
--- a/drivers/dma/tegra20-apb-dma.c
+++ b/drivers/dma/tegra20-apb-dma.c
@@ -1597,7 +1597,6 @@ static const struct dev_pm_ops tegra_dma_dev_pm_ops = {
 static struct platform_driver tegra_dmac_driver = {
 	.driver = {
 		.name	= "tegra-apbdma",
-		.owner = THIS_MODULE,
 		.pm	= &tegra_dma_dev_pm_ops,
 		.of_match_table = tegra_dma_of_match,
 	},
diff --git a/drivers/dma/timb_dma.c b/drivers/dma/timb_dma.c
index 4506a7b4f972..2407ccf1a64b 100644
--- a/drivers/dma/timb_dma.c
+++ b/drivers/dma/timb_dma.c
@@ -783,7 +783,6 @@ static int td_remove(struct platform_device *pdev)
 static struct platform_driver td_driver = {
 	.driver = {
 		.name	= DRIVER_NAME,
-		.owner  = THIS_MODULE,
 	},
 	.probe	= td_probe,
 	.remove	= td_remove,
diff --git a/drivers/dma/txx9dmac.c b/drivers/dma/txx9dmac.c
index 17686caf64d5..0659ec9c4488 100644
--- a/drivers/dma/txx9dmac.c
+++ b/drivers/dma/txx9dmac.c
@@ -76,7 +76,7 @@ static void channel64_write_CHAR(const struct txx9dmac_chan *dc, dma_addr_t val)
 
 static void channel64_clear_CHAR(const struct txx9dmac_chan *dc)
 {
-#if defined(CONFIG_32BIT) && !defined(CONFIG_64BIT_PHYS_ADDR)
+#if defined(CONFIG_32BIT) && !defined(CONFIG_PHYS_ADDR_T_64BIT)
 	channel64_writel(dc, CHAR, 0);
 	channel64_writel(dc, __pad_CHAR, 0);
 #else
diff --git a/drivers/dma/txx9dmac.h b/drivers/dma/txx9dmac.h
index f5a760598882..f6517b928bab 100644
--- a/drivers/dma/txx9dmac.h
+++ b/drivers/dma/txx9dmac.h
@@ -67,7 +67,7 @@ static inline bool txx9_dma_have_SMPCHN(void)
 
 /* Hardware register definitions. */
 struct txx9dmac_cregs {
-#if defined(CONFIG_32BIT) && !defined(CONFIG_64BIT_PHYS_ADDR)
+#if defined(CONFIG_32BIT) && !defined(CONFIG_PHYS_ADDR_T_64BIT)
 	TXX9_DMA_REG32(CHAR);	/* Chain Address Register */
 #else
 	u64 CHAR;		/* Chain Address Register */
@@ -201,7 +201,7 @@ static inline bool is_dmac64(const struct txx9dmac_chan *dc)
 #ifdef TXX9_DMA_USE_SIMPLE_CHAIN
 /* Hardware descriptor definition. (for simple-chain) */
 struct txx9dmac_hwdesc {
-#if defined(CONFIG_32BIT) && !defined(CONFIG_64BIT_PHYS_ADDR)
+#if defined(CONFIG_32BIT) && !defined(CONFIG_PHYS_ADDR_T_64BIT)
 	TXX9_DMA_REG32(CHAR);
 #else
 	u64 CHAR;
diff --git a/drivers/dma/xilinx/xilinx_vdma.c b/drivers/dma/xilinx/xilinx_vdma.c
index a6e64767186e..4a3a8f3137b3 100644
--- a/drivers/dma/xilinx/xilinx_vdma.c
+++ b/drivers/dma/xilinx/xilinx_vdma.c
@@ -942,6 +942,9 @@ xilinx_vdma_dma_prep_interleaved(struct dma_chan *dchan,
 	if (!xt->numf || !xt->sgl[0].size)
 		return NULL;
 
+	if (xt->frame_size != 1)
+		return NULL;
+
 	/* Allocate a transaction descriptor. */
 	desc = xilinx_vdma_alloc_tx_descriptor(chan);
 	if (!desc)
@@ -960,7 +963,7 @@ xilinx_vdma_dma_prep_interleaved(struct dma_chan *dchan,
 	hw = &segment->hw;
 	hw->vsize = xt->numf;
 	hw->hsize = xt->sgl[0].size;
-	hw->stride = xt->sgl[0].icg <<
+	hw->stride = (xt->sgl[0].icg + xt->sgl[0].size) <<
 			XILINX_VDMA_FRMDLY_STRIDE_STRIDE_SHIFT;
 	hw->stride |= chan->config.frm_dly <<
 			XILINX_VDMA_FRMDLY_STRIDE_FRMDLY_SHIFT;
@@ -971,9 +974,11 @@ xilinx_vdma_dma_prep_interleaved(struct dma_chan *dchan,
 		hw->buf_addr = xt->src_start;
 
 	/* Link the previous next descriptor to current */
-	prev = list_last_entry(&desc->segments,
-				struct xilinx_vdma_tx_segment, node);
-	prev->hw.next_desc = segment->phys;
+	if (!list_empty(&desc->segments)) {
+		prev = list_last_entry(&desc->segments,
+				       struct xilinx_vdma_tx_segment, node);
+		prev->hw.next_desc = segment->phys;
+	}
 
 	/* Insert the segment into the descriptor segments list. */
 	list_add_tail(&segment->node, &desc->segments);
diff --git a/drivers/edac/cell_edac.c b/drivers/edac/cell_edac.c
index a12c8552f6a6..a9259b069dcd 100644
--- a/drivers/edac/cell_edac.c
+++ b/drivers/edac/cell_edac.c
@@ -245,7 +245,6 @@ static int cell_edac_remove(struct platform_device *pdev)
 static struct platform_driver cell_edac_driver = {
 	.driver		= {
 		.name	= "cbe-mic",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= cell_edac_probe,
 	.remove		= cell_edac_remove,
diff --git a/drivers/edac/mpc85xx_edac.c b/drivers/edac/mpc85xx_edac.c
index 7d3742edbaa2..ffb1a9a15ccd 100644
--- a/drivers/edac/mpc85xx_edac.c
+++ b/drivers/edac/mpc85xx_edac.c
@@ -715,7 +715,6 @@ static struct platform_driver mpc85xx_l2_err_driver = {
 	.remove = mpc85xx_l2_err_remove,
 	.driver = {
 		.name = "mpc85xx_l2_err",
-		.owner = THIS_MODULE,
 		.of_match_table = mpc85xx_l2_err_of_match,
 	},
 };
@@ -1215,7 +1214,6 @@ static struct platform_driver mpc85xx_mc_err_driver = {
 	.remove = mpc85xx_mc_err_remove,
 	.driver = {
 		.name = "mpc85xx_mc_err",
-		.owner = THIS_MODULE,
 		.of_match_table = mpc85xx_mc_err_of_match,
 	},
 };
diff --git a/drivers/edac/ppc4xx_edac.c b/drivers/edac/ppc4xx_edac.c
index 41593539cec4..1b64fd060821 100644
--- a/drivers/edac/ppc4xx_edac.c
+++ b/drivers/edac/ppc4xx_edac.c
@@ -204,7 +204,6 @@ static struct platform_driver ppc4xx_edac_driver = {
 	.probe			= ppc4xx_edac_probe,
 	.remove			= ppc4xx_edac_remove,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = PPC4XX_EDAC_MODULE_NAME,
 		.of_match_table = ppc4xx_edac_match,
 	},
diff --git a/drivers/edac/tile_edac.c b/drivers/edac/tile_edac.c
index 578f915ee195..71381642ce2a 100644
--- a/drivers/edac/tile_edac.c
+++ b/drivers/edac/tile_edac.c
@@ -199,7 +199,6 @@ static int tile_edac_mc_remove(struct platform_device *pdev)
 static struct platform_driver tile_edac_mc_driver = {
 	.driver		= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= tile_edac_mc_probe,
 	.remove		= tile_edac_mc_remove,
diff --git a/drivers/extcon/extcon-adc-jack.c b/drivers/extcon/extcon-adc-jack.c
index d860229e4de1..5d7ab577fba9 100644
--- a/drivers/extcon/extcon-adc-jack.c
+++ b/drivers/extcon/extcon-adc-jack.c
@@ -182,7 +182,6 @@ static struct platform_driver adc_jack_driver = {
 	.remove         = adc_jack_remove,
 	.driver         = {
 		.name   = "adc-jack",
-		.owner  = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/extcon/extcon-arizona.c b/drivers/extcon/extcon-arizona.c
index ba51588cc000..63f01c42aed4 100644
--- a/drivers/extcon/extcon-arizona.c
+++ b/drivers/extcon/extcon-arizona.c
@@ -1469,7 +1469,6 @@ static int arizona_extcon_remove(struct platform_device *pdev)
 static struct platform_driver arizona_extcon_driver = {
 	.driver		= {
 		.name	= "arizona-extcon",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= arizona_extcon_probe,
 	.remove		= arizona_extcon_remove,
diff --git a/drivers/extcon/extcon-class.c b/drivers/extcon/extcon-class.c
index 4c2f2c543bb7..043dcd9946c9 100644
--- a/drivers/extcon/extcon-class.c
+++ b/drivers/extcon/extcon-class.c
@@ -29,6 +29,7 @@
 #include <linux/fs.h>
 #include <linux/err.h>
 #include <linux/extcon.h>
+#include <linux/of.h>
 #include <linux/slab.h>
 #include <linux/sysfs.h>
 #include <linux/of.h>
@@ -997,13 +998,16 @@ struct extcon_dev *extcon_get_edev_by_phandle(struct device *dev, int index)
 		return ERR_PTR(-ENODEV);
 	}
 
-	edev = extcon_get_extcon_dev(node->name);
-	if (!edev) {
-		dev_err(dev, "unable to get extcon device : %s\n", node->name);
-		return ERR_PTR(-ENODEV);
+	mutex_lock(&extcon_dev_list_lock);
+	list_for_each_entry(edev, &extcon_dev_list, entry) {
+		if (edev->dev.parent && edev->dev.parent->of_node == node) {
+			mutex_unlock(&extcon_dev_list_lock);
+			return edev;
+		}
 	}
+	mutex_unlock(&extcon_dev_list_lock);
 
-	return edev;
+	return ERR_PTR(-EPROBE_DEFER);
 }
 #else
 struct extcon_dev *extcon_get_edev_by_phandle(struct device *dev, int index)
diff --git a/drivers/extcon/extcon-gpio.c b/drivers/extcon/extcon-gpio.c
index 72f19a37fd01..7af33fc433cd 100644
--- a/drivers/extcon/extcon-gpio.c
+++ b/drivers/extcon/extcon-gpio.c
@@ -181,7 +181,6 @@ static struct platform_driver gpio_extcon_driver = {
 	.remove		= gpio_extcon_remove,
 	.driver		= {
 		.name	= "extcon-gpio",
-		.owner	= THIS_MODULE,
 		.pm	= &gpio_extcon_pm_ops,
 	},
 };
diff --git a/drivers/extcon/extcon-max14577.c b/drivers/extcon/extcon-max14577.c
index 7309743d0da1..c1bf0cf747b0 100644
--- a/drivers/extcon/extcon-max14577.c
+++ b/drivers/extcon/extcon-max14577.c
@@ -1,7 +1,7 @@
 /*
  * extcon-max14577.c - MAX14577/77836 extcon driver to support MUIC
  *
- * Copyright (C) 2013,2014 Samsung Electrnoics
+ * Copyright (C) 2013,2014 Samsung Electronics
  * Chanwoo Choi <cw00.choi@samsung.com>
  * Krzysztof Kozlowski <k.kozlowski@samsung.com>
  *
@@ -807,7 +807,6 @@ MODULE_DEVICE_TABLE(platform, max14577_muic_id);
 static struct platform_driver max14577_muic_driver = {
 	.driver		= {
 		.name	= "max14577-muic",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= max14577_muic_probe,
 	.remove		= max14577_muic_remove,
diff --git a/drivers/extcon/extcon-max77693.c b/drivers/extcon/extcon-max77693.c
index 490e27475bac..740a14d35072 100644
--- a/drivers/extcon/extcon-max77693.c
+++ b/drivers/extcon/extcon-max77693.c
@@ -232,7 +232,7 @@ static const char *max77693_extcon_cable[] = {
 	[EXTCON_CABLE_JIG_USB_ON]		= "JIG-USB-ON",
 	[EXTCON_CABLE_JIG_USB_OFF]		= "JIG-USB-OFF",
 	[EXTCON_CABLE_JIG_UART_OFF]		= "JIG-UART-OFF",
-	[EXTCON_CABLE_JIG_UART_ON]		= "Dock-Car",
+	[EXTCON_CABLE_JIG_UART_ON]		= "JIG-UART-ON",
 	[EXTCON_CABLE_DOCK_SMART]		= "Dock-Smart",
 	[EXTCON_CABLE_DOCK_DESK]		= "Dock-Desk",
 	[EXTCON_CABLE_DOCK_AUDIO]		= "Dock-Audio",
@@ -532,9 +532,6 @@ static int max77693_muic_dock_handler(struct max77693_muic_info *info,
 		extcon_set_cable_state(info->edev, "Dock-Smart", attached);
 		extcon_set_cable_state(info->edev, "MHL", attached);
 		goto out;
-	case MAX77693_MUIC_ADC_FACTORY_MODE_UART_ON:	/* Dock-Car */
-		strcpy(dock_name, "Dock-Car");
-		break;
 	case MAX77693_MUIC_ADC_AUDIO_MODE_REMOTE:	/* Dock-Desk */
 		strcpy(dock_name, "Dock-Desk");
 		break;
@@ -669,6 +666,11 @@ static int max77693_muic_jig_handler(struct max77693_muic_info *info,
 		strcpy(cable_name, "JIG-UART-OFF");
 		path = CONTROL1_SW_UART;
 		break;
+	case MAX77693_MUIC_ADC_FACTORY_MODE_UART_ON:	/* ADC_JIG_UART_ON */
+		/* PATH:AP_UART */
+		strcpy(cable_name, "JIG-UART-ON");
+		path = CONTROL1_SW_UART;
+		break;
 	default:
 		dev_err(info->dev, "failed to detect %s jig cable\n",
 			attached ? "attached" : "detached");
@@ -708,13 +710,13 @@ static int max77693_muic_adc_handler(struct max77693_muic_info *info)
 	case MAX77693_MUIC_ADC_FACTORY_MODE_USB_OFF:
 	case MAX77693_MUIC_ADC_FACTORY_MODE_USB_ON:
 	case MAX77693_MUIC_ADC_FACTORY_MODE_UART_OFF:
+	case MAX77693_MUIC_ADC_FACTORY_MODE_UART_ON:
 		/* JIG */
 		ret = max77693_muic_jig_handler(info, cable_type, attached);
 		if (ret < 0)
 			return ret;
 		break;
 	case MAX77693_MUIC_ADC_RESERVED_ACC_3:		/* Dock-Smart */
-	case MAX77693_MUIC_ADC_FACTORY_MODE_UART_ON:	/* Dock-Car */
 	case MAX77693_MUIC_ADC_AUDIO_MODE_REMOTE:	/* Dock-Desk */
 	case MAX77693_MUIC_ADC_AV_CABLE_NOLOAD:		/* Dock-Audio */
 		/*
@@ -1301,7 +1303,6 @@ static int max77693_muic_remove(struct platform_device *pdev)
 static struct platform_driver max77693_muic_driver = {
 	.driver		= {
 		.name	= DEV_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= max77693_muic_probe,
 	.remove		= max77693_muic_remove,
diff --git a/drivers/extcon/extcon-max8997.c b/drivers/extcon/extcon-max8997.c
index 75e501c98005..fc1678fa95c4 100644
--- a/drivers/extcon/extcon-max8997.c
+++ b/drivers/extcon/extcon-max8997.c
@@ -792,7 +792,6 @@ static int max8997_muic_remove(struct platform_device *pdev)
 static struct platform_driver max8997_muic_driver = {
 	.driver		= {
 		.name	= DEV_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= max8997_muic_probe,
 	.remove		= max8997_muic_remove,
diff --git a/drivers/extcon/extcon-palmas.c b/drivers/extcon/extcon-palmas.c
index 230e1220ce48..11c6757b6c40 100644
--- a/drivers/extcon/extcon-palmas.c
+++ b/drivers/extcon/extcon-palmas.c
@@ -291,7 +291,6 @@ static struct platform_driver palmas_usb_driver = {
 	.driver = {
 		.name = "palmas-usb",
 		.of_match_table = of_palmas_match_tbl,
-		.owner = THIS_MODULE,
 		.pm = &palmas_pm_ops,
 	},
 };
diff --git a/drivers/firewire/core-device.c b/drivers/firewire/core-device.c
index 2c6d5e118ac1..f9e3aee6a211 100644
--- a/drivers/firewire/core-device.c
+++ b/drivers/firewire/core-device.c
@@ -115,6 +115,9 @@ static int textual_leaf_to_string(const u32 *block, char *buf, size_t size)
  *
  * The string is taken from a minimal ASCII text descriptor leaf after
  * the immediate entry with @key.  The string is zero-terminated.
+ * An overlong string is silently truncated such that it and the
+ * zero byte fit into @size.
+ *
  * Returns strlen(buf) or a negative error code.
  */
 int fw_csr_string(const u32 *directory, int key, char *buf, size_t size)
diff --git a/drivers/firewire/ohci.c b/drivers/firewire/ohci.c
index a66a3217f1d9..aff9018d0658 100644
--- a/drivers/firewire/ohci.c
+++ b/drivers/firewire/ohci.c
@@ -689,8 +689,7 @@ static void ar_context_release(struct ar_context *ctx)
 {
 	unsigned int i;
 
-	if (ctx->buffer)
-		vm_unmap_ram(ctx->buffer, AR_BUFFERS + AR_WRAPAROUND_PAGES);
+	vunmap(ctx->buffer);
 
 	for (i = 0; i < AR_BUFFERS; i++)
 		if (ctx->pages[i]) {
@@ -1018,8 +1017,7 @@ static int ar_context_init(struct ar_context *ctx, struct fw_ohci *ohci,
 		pages[i]              = ctx->pages[i];
 	for (i = 0; i < AR_WRAPAROUND_PAGES; i++)
 		pages[AR_BUFFERS + i] = ctx->pages[i];
-	ctx->buffer = vm_map_ram(pages, AR_BUFFERS + AR_WRAPAROUND_PAGES,
-				 -1, PAGE_KERNEL);
+	ctx->buffer = vmap(pages, ARRAY_SIZE(pages), VM_MAP, PAGE_KERNEL);
 	if (!ctx->buffer)
 		goto out_of_memory;
 
diff --git a/drivers/firewire/sbp2.c b/drivers/firewire/sbp2.c
index 7aef911fdc71..64ac8f8f5098 100644
--- a/drivers/firewire/sbp2.c
+++ b/drivers/firewire/sbp2.c
@@ -174,6 +174,7 @@ struct sbp2_target {
 	unsigned int mgt_orb_timeout;
 	unsigned int max_payload;
 
+	spinlock_t lock;
 	int dont_block;	/* counter for each logical unit */
 	int blocked;	/* ditto */
 };
@@ -270,6 +271,7 @@ struct sbp2_orb {
 	dma_addr_t request_bus;
 	int rcode;
 	void (*callback)(struct sbp2_orb * orb, struct sbp2_status * status);
+	struct sbp2_logical_unit *lu;
 	struct list_head link;
 };
 
@@ -321,7 +323,6 @@ struct sbp2_command_orb {
 		u8 command_block[SBP2_MAX_CDB_SIZE];
 	} request;
 	struct scsi_cmnd *cmd;
-	struct sbp2_logical_unit *lu;
 
 	struct sbp2_pointer page_table[SG_ALL] __attribute__((aligned(8)));
 	dma_addr_t page_table_bus;
@@ -444,7 +445,7 @@ static void sbp2_status_write(struct fw_card *card, struct fw_request *request,
 	}
 
 	/* Lookup the orb corresponding to this status write. */
-	spin_lock_irqsave(&card->lock, flags);
+	spin_lock_irqsave(&lu->tgt->lock, flags);
 	list_for_each_entry(orb, &lu->orb_list, link) {
 		if (STATUS_GET_ORB_HIGH(status) == 0 &&
 		    STATUS_GET_ORB_LOW(status) == orb->request_bus) {
@@ -453,7 +454,7 @@ static void sbp2_status_write(struct fw_card *card, struct fw_request *request,
 			break;
 		}
 	}
-	spin_unlock_irqrestore(&card->lock, flags);
+	spin_unlock_irqrestore(&lu->tgt->lock, flags);
 
 	if (&orb->link != &lu->orb_list) {
 		orb->callback(orb, &status);
@@ -480,18 +481,18 @@ static void complete_transaction(struct fw_card *card, int rcode,
 	 * been set and only does the cleanup if the transaction
 	 * failed and we didn't already get a status write.
 	 */
-	spin_lock_irqsave(&card->lock, flags);
+	spin_lock_irqsave(&orb->lu->tgt->lock, flags);
 
 	if (orb->rcode == -1)
 		orb->rcode = rcode;
 	if (orb->rcode != RCODE_COMPLETE) {
 		list_del(&orb->link);
-		spin_unlock_irqrestore(&card->lock, flags);
+		spin_unlock_irqrestore(&orb->lu->tgt->lock, flags);
 
 		orb->callback(orb, NULL);
 		kref_put(&orb->kref, free_orb); /* orb callback reference */
 	} else {
-		spin_unlock_irqrestore(&card->lock, flags);
+		spin_unlock_irqrestore(&orb->lu->tgt->lock, flags);
 	}
 
 	kref_put(&orb->kref, free_orb); /* transaction callback reference */
@@ -507,9 +508,10 @@ static void sbp2_send_orb(struct sbp2_orb *orb, struct sbp2_logical_unit *lu,
 	orb_pointer.high = 0;
 	orb_pointer.low = cpu_to_be32(orb->request_bus);
 
-	spin_lock_irqsave(&device->card->lock, flags);
+	orb->lu = lu;
+	spin_lock_irqsave(&lu->tgt->lock, flags);
 	list_add_tail(&orb->link, &lu->orb_list);
-	spin_unlock_irqrestore(&device->card->lock, flags);
+	spin_unlock_irqrestore(&lu->tgt->lock, flags);
 
 	kref_get(&orb->kref); /* transaction callback reference */
 	kref_get(&orb->kref); /* orb callback reference */
@@ -524,13 +526,12 @@ static int sbp2_cancel_orbs(struct sbp2_logical_unit *lu)
 	struct fw_device *device = target_parent_device(lu->tgt);
 	struct sbp2_orb *orb, *next;
 	struct list_head list;
-	unsigned long flags;
 	int retval = -ENOENT;
 
 	INIT_LIST_HEAD(&list);
-	spin_lock_irqsave(&device->card->lock, flags);
+	spin_lock_irq(&lu->tgt->lock);
 	list_splice_init(&lu->orb_list, &list);
-	spin_unlock_irqrestore(&device->card->lock, flags);
+	spin_unlock_irq(&lu->tgt->lock);
 
 	list_for_each_entry_safe(orb, next, &list, link) {
 		retval = 0;
@@ -687,16 +688,11 @@ static void sbp2_agent_reset_no_wait(struct sbp2_logical_unit *lu)
 			&d, 4, complete_agent_reset_write_no_wait, t);
 }
 
-static inline void sbp2_allow_block(struct sbp2_logical_unit *lu)
+static inline void sbp2_allow_block(struct sbp2_target *tgt)
 {
-	/*
-	 * We may access dont_block without taking card->lock here:
-	 * All callers of sbp2_allow_block() and all callers of sbp2_unblock()
-	 * are currently serialized against each other.
-	 * And a wrong result in sbp2_conditionally_block()'s access of
-	 * dont_block is rather harmless, it simply misses its first chance.
-	 */
-	--lu->tgt->dont_block;
+	spin_lock_irq(&tgt->lock);
+	--tgt->dont_block;
+	spin_unlock_irq(&tgt->lock);
 }
 
 /*
@@ -705,7 +701,7 @@ static inline void sbp2_allow_block(struct sbp2_logical_unit *lu)
  *     logical units have been finished (indicated by dont_block == 0).
  *   - lu->generation is stale.
  *
- * Note, scsi_block_requests() must be called while holding card->lock,
+ * Note, scsi_block_requests() must be called while holding tgt->lock,
  * otherwise it might foil sbp2_[conditionally_]unblock()'s attempt to
  * unblock the target.
  */
@@ -717,20 +713,20 @@ static void sbp2_conditionally_block(struct sbp2_logical_unit *lu)
 		container_of((void *)tgt, struct Scsi_Host, hostdata[0]);
 	unsigned long flags;
 
-	spin_lock_irqsave(&card->lock, flags);
+	spin_lock_irqsave(&tgt->lock, flags);
 	if (!tgt->dont_block && !lu->blocked &&
 	    lu->generation != card->generation) {
 		lu->blocked = true;
 		if (++tgt->blocked == 1)
 			scsi_block_requests(shost);
 	}
-	spin_unlock_irqrestore(&card->lock, flags);
+	spin_unlock_irqrestore(&tgt->lock, flags);
 }
 
 /*
  * Unblocks lu->tgt as soon as all its logical units can be unblocked.
  * Note, it is harmless to run scsi_unblock_requests() outside the
- * card->lock protected section.  On the other hand, running it inside
+ * tgt->lock protected section.  On the other hand, running it inside
  * the section might clash with shost->host_lock.
  */
 static void sbp2_conditionally_unblock(struct sbp2_logical_unit *lu)
@@ -739,15 +735,14 @@ static void sbp2_conditionally_unblock(struct sbp2_logical_unit *lu)
 	struct fw_card *card = target_parent_device(tgt)->card;
 	struct Scsi_Host *shost =
 		container_of((void *)tgt, struct Scsi_Host, hostdata[0]);
-	unsigned long flags;
 	bool unblock = false;
 
-	spin_lock_irqsave(&card->lock, flags);
+	spin_lock_irq(&tgt->lock);
 	if (lu->blocked && lu->generation == card->generation) {
 		lu->blocked = false;
 		unblock = --tgt->blocked == 0;
 	}
-	spin_unlock_irqrestore(&card->lock, flags);
+	spin_unlock_irq(&tgt->lock);
 
 	if (unblock)
 		scsi_unblock_requests(shost);
@@ -756,19 +751,17 @@ static void sbp2_conditionally_unblock(struct sbp2_logical_unit *lu)
 /*
  * Prevents future blocking of tgt and unblocks it.
  * Note, it is harmless to run scsi_unblock_requests() outside the
- * card->lock protected section.  On the other hand, running it inside
+ * tgt->lock protected section.  On the other hand, running it inside
  * the section might clash with shost->host_lock.
  */
 static void sbp2_unblock(struct sbp2_target *tgt)
 {
-	struct fw_card *card = target_parent_device(tgt)->card;
 	struct Scsi_Host *shost =
 		container_of((void *)tgt, struct Scsi_Host, hostdata[0]);
-	unsigned long flags;
 
-	spin_lock_irqsave(&card->lock, flags);
+	spin_lock_irq(&tgt->lock);
 	++tgt->dont_block;
-	spin_unlock_irqrestore(&card->lock, flags);
+	spin_unlock_irq(&tgt->lock);
 
 	scsi_unblock_requests(shost);
 }
@@ -904,7 +897,7 @@ static void sbp2_login(struct work_struct *work)
 	/* No error during __scsi_add_device() */
 	lu->has_sdev = true;
 	scsi_device_put(sdev);
-	sbp2_allow_block(lu);
+	sbp2_allow_block(tgt);
 
 	return;
 
@@ -1163,6 +1156,7 @@ static int sbp2_probe(struct fw_unit *unit, const struct ieee1394_device_id *id)
 	dev_set_drvdata(&unit->device, tgt);
 	tgt->unit = unit;
 	INIT_LIST_HEAD(&tgt->lu_list);
+	spin_lock_init(&tgt->lock);
 	tgt->guid = (u64)device->config_rom[3] << 32 | device->config_rom[4];
 
 	if (fw_device_enable_phys_dma(device) < 0)
@@ -1359,12 +1353,12 @@ static void complete_command_orb(struct sbp2_orb *base_orb,
 {
 	struct sbp2_command_orb *orb =
 		container_of(base_orb, struct sbp2_command_orb, base);
-	struct fw_device *device = target_parent_device(orb->lu->tgt);
+	struct fw_device *device = target_parent_device(base_orb->lu->tgt);
 	int result;
 
 	if (status != NULL) {
 		if (STATUS_GET_DEAD(*status))
-			sbp2_agent_reset_no_wait(orb->lu);
+			sbp2_agent_reset_no_wait(base_orb->lu);
 
 		switch (STATUS_GET_RESPONSE(*status)) {
 		case SBP2_STATUS_REQUEST_COMPLETE:
@@ -1390,7 +1384,7 @@ static void complete_command_orb(struct sbp2_orb *base_orb,
 		 * or when sending the write (less likely).
 		 */
 		result = DID_BUS_BUSY << 16;
-		sbp2_conditionally_block(orb->lu);
+		sbp2_conditionally_block(base_orb->lu);
 	}
 
 	dma_unmap_single(device->card->device, orb->base.request_bus,
@@ -1487,7 +1481,6 @@ static int sbp2_scsi_queuecommand(struct Scsi_Host *shost,
 	/* Initialize rcode to something not RCODE_COMPLETE. */
 	orb->base.rcode = -1;
 	kref_init(&orb->base.kref);
-	orb->lu = lu;
 	orb->cmd = cmd;
 	orb->request.next.high = cpu_to_be32(SBP2_ORB_NULL);
 	orb->request.misc = cpu_to_be32(
diff --git a/drivers/firmware/dcdbas.c b/drivers/firmware/dcdbas.c
index 7160c43c59fc..829eec8959f2 100644
--- a/drivers/firmware/dcdbas.c
+++ b/drivers/firmware/dcdbas.c
@@ -578,7 +578,6 @@ static int dcdbas_remove(struct platform_device *dev)
 static struct platform_driver dcdbas_driver = {
 	.driver		= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= dcdbas_probe,
 	.remove		= dcdbas_remove,
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index 23dfd5f59b39..633ec216e185 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -112,6 +112,20 @@ config GPIO_MAX730X
 
 comment "Memory mapped GPIO drivers:"
 
+config GPIO_74XX_MMIO
+	tristate "GPIO driver for 74xx-ICs with MMIO access"
+	depends on OF_GPIO
+	select GPIO_GENERIC
+	help
+	  Say yes here to support GPIO functionality for 74xx-compatible ICs
+	  with MMIO access. Compatible models include:
+	    1 bit:	741G125 (Input), 741G74 (Output)
+	    2 bits:	742G125 (Input), 7474 (Output)
+	    4 bits:	74125 (Input), 74175 (Output)
+	    6 bits:	74365 (Input), 74174 (Output)
+	    8 bits:	74244 (Input), 74273 (Output)
+	    16 bits:	741624 (Input), 7416374 (Output)
+
 config GPIO_CLPS711X
 	tristate "CLPS711X GPIO support"
 	depends on ARCH_CLPS711X || COMPILE_TEST
@@ -134,6 +148,8 @@ config GPIO_GENERIC_PLATFORM
 
 config GPIO_DWAPB
 	tristate "Synopsys DesignWare APB GPIO driver"
+	depends on ARM
+	depends on OF_GPIO
 	select GPIO_GENERIC
 	select GENERIC_IRQ_CHIP
 	help
@@ -333,6 +349,13 @@ config GPIO_TZ1090_PDC
 	help
 	  Say yes here to support Toumaz Xenif TZ1090 PDC GPIOs.
 
+config GPIO_VF610
+	def_bool y
+	depends on ARCH_MXC && SOC_VF610
+	select GPIOLIB_IRQCHIP
+	help
+	  Say yes here to support Vybrid vf610 GPIOs.
+
 config GPIO_XGENE
 	bool "APM X-Gene GPIO controller support"
 	depends on ARM64 && OF_GPIO
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index e60677b8ccb4..81755f1305e6 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -13,6 +13,7 @@ obj-$(CONFIG_GPIO_ACPI)		+= gpiolib-acpi.o
 obj-$(CONFIG_GPIO_GENERIC)	+= gpio-generic.o
 
 obj-$(CONFIG_GPIO_74X164)	+= gpio-74x164.o
+obj-$(CONFIG_GPIO_74XX_MMIO)	+= gpio-74xx-mmio.o
 obj-$(CONFIG_GPIO_ADNP)		+= gpio-adnp.o
 obj-$(CONFIG_GPIO_ADP5520)	+= gpio-adp5520.o
 obj-$(CONFIG_GPIO_ADP5588)	+= gpio-adp5588.o
@@ -96,6 +97,7 @@ obj-$(CONFIG_GPIO_TWL6040)	+= gpio-twl6040.o
 obj-$(CONFIG_GPIO_TZ1090)	+= gpio-tz1090.o
 obj-$(CONFIG_GPIO_TZ1090_PDC)	+= gpio-tz1090-pdc.o
 obj-$(CONFIG_GPIO_UCB1400)	+= gpio-ucb1400.o
+obj-$(CONFIG_GPIO_VF610)	+= gpio-vf610.o
 obj-$(CONFIG_GPIO_VIPERBOARD)	+= gpio-viperboard.o
 obj-$(CONFIG_GPIO_VR41XX)	+= gpio-vr41xx.o
 obj-$(CONFIG_GPIO_VX855)	+= gpio-vx855.o
diff --git a/drivers/gpio/gpio-74xx-mmio.c b/drivers/gpio/gpio-74xx-mmio.c
new file mode 100644
index 000000000000..0763655cca6c
--- /dev/null
+++ b/drivers/gpio/gpio-74xx-mmio.c
@@ -0,0 +1,170 @@
+/*
+ * 74xx MMIO GPIO driver
+ *
+ *  Copyright (C) 2014 Alexander Shiyan <shc_work@mail.ru>
+ *
+ * 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/gpio.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/basic_mmio_gpio.h>
+#include <linux/platform_device.h>
+
+#define MMIO_74XX_DIR_IN	(0 << 8)
+#define MMIO_74XX_DIR_OUT	(1 << 8)
+#define MMIO_74XX_BIT_CNT(x)	((x) & 0xff)
+
+struct mmio_74xx_gpio_priv {
+	struct bgpio_chip	bgc;
+	unsigned		flags;
+};
+
+static const struct of_device_id mmio_74xx_gpio_ids[] = {
+	{
+		.compatible	= "ti,741g125",
+		.data		= (const void *)(MMIO_74XX_DIR_IN | 1),
+	},
+	{
+		.compatible	= "ti,742g125",
+		.data		= (const void *)(MMIO_74XX_DIR_IN | 2),
+	},
+	{
+		.compatible	= "ti,74125",
+		.data		= (const void *)(MMIO_74XX_DIR_IN | 4),
+	},
+	{
+		.compatible	= "ti,74365",
+		.data		= (const void *)(MMIO_74XX_DIR_IN | 6),
+	},
+	{
+		.compatible	= "ti,74244",
+		.data		= (const void *)(MMIO_74XX_DIR_IN | 8),
+	},
+	{
+		.compatible	= "ti,741624",
+		.data		= (const void *)(MMIO_74XX_DIR_IN | 16),
+	},
+	{
+		.compatible	= "ti,741g74",
+		.data		= (const void *)(MMIO_74XX_DIR_OUT | 1),
+	},
+	{
+		.compatible	= "ti,7474",
+		.data		= (const void *)(MMIO_74XX_DIR_OUT | 2),
+	},
+	{
+		.compatible	= "ti,74175",
+		.data		= (const void *)(MMIO_74XX_DIR_OUT | 4),
+	},
+	{
+		.compatible	= "ti,74174",
+		.data		= (const void *)(MMIO_74XX_DIR_OUT | 6),
+	},
+	{
+		.compatible	= "ti,74273",
+		.data		= (const void *)(MMIO_74XX_DIR_OUT | 8),
+	},
+	{
+		.compatible	= "ti,7416374",
+		.data		= (const void *)(MMIO_74XX_DIR_OUT | 16),
+	},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, mmio_74xx_gpio_ids);
+
+static inline struct mmio_74xx_gpio_priv *to_74xx_gpio(struct gpio_chip *gc)
+{
+	struct bgpio_chip *bgc = to_bgpio_chip(gc);
+
+	return container_of(bgc, struct mmio_74xx_gpio_priv, bgc);
+}
+
+static int mmio_74xx_get_direction(struct gpio_chip *gc, unsigned offset)
+{
+	struct mmio_74xx_gpio_priv *priv = to_74xx_gpio(gc);
+
+	return (priv->flags & MMIO_74XX_DIR_OUT) ? GPIOF_DIR_OUT : GPIOF_DIR_IN;
+}
+
+static int mmio_74xx_dir_in(struct gpio_chip *gc, unsigned int gpio)
+{
+	struct mmio_74xx_gpio_priv *priv = to_74xx_gpio(gc);
+
+	return (priv->flags & MMIO_74XX_DIR_OUT) ? -ENOTSUPP : 0;
+}
+
+static int mmio_74xx_dir_out(struct gpio_chip *gc, unsigned int gpio, int val)
+{
+	struct mmio_74xx_gpio_priv *priv = to_74xx_gpio(gc);
+
+	if (priv->flags & MMIO_74XX_DIR_OUT) {
+		gc->set(gc, gpio, val);
+		return 0;
+	}
+
+	return -ENOTSUPP;
+}
+
+static int mmio_74xx_gpio_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *of_id =
+		of_match_device(mmio_74xx_gpio_ids, &pdev->dev);
+	struct mmio_74xx_gpio_priv *priv;
+	struct resource *res;
+	void __iomem *dat;
+	int err;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	dat = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(dat))
+		return PTR_ERR(dat);
+
+	priv->flags = (unsigned)of_id->data;
+
+	err = bgpio_init(&priv->bgc, &pdev->dev,
+			 DIV_ROUND_UP(MMIO_74XX_BIT_CNT(priv->flags), 8),
+			 dat, NULL, NULL, NULL, NULL, 0);
+	if (err)
+		return err;
+
+	priv->bgc.gc.direction_input = mmio_74xx_dir_in;
+	priv->bgc.gc.direction_output = mmio_74xx_dir_out;
+	priv->bgc.gc.get_direction = mmio_74xx_get_direction;
+	priv->bgc.gc.ngpio = MMIO_74XX_BIT_CNT(priv->flags);
+	priv->bgc.gc.owner = THIS_MODULE;
+
+	platform_set_drvdata(pdev, priv);
+
+	return gpiochip_add(&priv->bgc.gc);
+}
+
+static int mmio_74xx_gpio_remove(struct platform_device *pdev)
+{
+	struct mmio_74xx_gpio_priv *priv = platform_get_drvdata(pdev);
+
+	return bgpio_remove(&priv->bgc);
+}
+
+static struct platform_driver mmio_74xx_gpio_driver = {
+	.driver	= {
+		.name		= "74xx-mmio-gpio",
+		.of_match_table	= mmio_74xx_gpio_ids,
+	},
+	.probe	= mmio_74xx_gpio_probe,
+	.remove	= mmio_74xx_gpio_remove,
+};
+module_platform_driver(mmio_74xx_gpio_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Alexander Shiyan <shc_work@mail.ru>");
+MODULE_DESCRIPTION("74xx MMIO GPIO driver");
diff --git a/drivers/gpio/gpio-adp5520.c b/drivers/gpio/gpio-adp5520.c
index b08bd169e568..caff711ca5a9 100644
--- a/drivers/gpio/gpio-adp5520.c
+++ b/drivers/gpio/gpio-adp5520.c
@@ -177,7 +177,6 @@ static int adp5520_gpio_remove(struct platform_device *pdev)
 static struct platform_driver adp5520_gpio_driver = {
 	.driver	= {
 		.name	= "adp5520-gpio",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= adp5520_gpio_probe,
 	.remove		= adp5520_gpio_remove,
diff --git a/drivers/gpio/gpio-amd8111.c b/drivers/gpio/gpio-amd8111.c
index 3c09f1a6872a..d3d2d1099f64 100644
--- a/drivers/gpio/gpio-amd8111.c
+++ b/drivers/gpio/gpio-amd8111.c
@@ -223,6 +223,7 @@ found:
 	if (err) {
 		printk(KERN_ERR "GPIO registering failed (%d)\n",
 		       err);
+		ioport_unmap(gp.pm);
 		release_region(gp.pmbase + PMBASE_OFFSET, PMBASE_SIZE);
 		goto out;
 	}
diff --git a/drivers/gpio/gpio-bcm-kona.c b/drivers/gpio/gpio-bcm-kona.c
index de0801e9767a..b164ce837b43 100644
--- a/drivers/gpio/gpio-bcm-kona.c
+++ b/drivers/gpio/gpio-bcm-kona.c
@@ -470,7 +470,7 @@ static int bcm_kona_gpio_irq_reqres(struct irq_data *d)
 {
 	struct bcm_kona_gpio *kona_gpio = irq_data_get_irq_chip_data(d);
 
-	if (gpio_lock_as_irq(&kona_gpio->gpio_chip, d->hwirq)) {
+	if (gpiochip_lock_as_irq(&kona_gpio->gpio_chip, d->hwirq)) {
 		dev_err(kona_gpio->gpio_chip.dev,
 			"unable to lock HW IRQ %lu for IRQ\n",
 			d->hwirq);
@@ -483,7 +483,7 @@ static void bcm_kona_gpio_irq_relres(struct irq_data *d)
 {
 	struct bcm_kona_gpio *kona_gpio = irq_data_get_irq_chip_data(d);
 
-	gpio_unlock_as_irq(&kona_gpio->gpio_chip, d->hwirq);
+	gpiochip_unlock_as_irq(&kona_gpio->gpio_chip, d->hwirq);
 }
 
 static struct irq_chip bcm_gpio_irq_chip = {
@@ -668,7 +668,6 @@ err_irq_domain:
 static struct platform_driver bcm_kona_gpio_driver = {
 	.driver = {
 			.name = "bcm-kona-gpio",
-			.owner = THIS_MODULE,
 			.of_match_table = bcm_kona_gpio_of_match,
 	},
 	.probe = bcm_kona_gpio_probe,
diff --git a/drivers/gpio/gpio-clps711x.c b/drivers/gpio/gpio-clps711x.c
index e1e861239e95..b6908f1ff1ab 100644
--- a/drivers/gpio/gpio-clps711x.c
+++ b/drivers/gpio/gpio-clps711x.c
@@ -87,7 +87,6 @@ MODULE_DEVICE_TABLE(of, clps711x_gpio_ids);
 static struct platform_driver clps711x_gpio_driver = {
 	.driver	= {
 		.name		= "clps711x-gpio",
-		.owner		= THIS_MODULE,
 		.of_match_table	= of_match_ptr(clps711x_gpio_ids),
 	},
 	.probe	= clps711x_gpio_probe,
diff --git a/drivers/gpio/gpio-crystalcove.c b/drivers/gpio/gpio-crystalcove.c
index bbfe7f508502..55d4803d71b0 100644
--- a/drivers/gpio/gpio-crystalcove.c
+++ b/drivers/gpio/gpio-crystalcove.c
@@ -379,7 +379,6 @@ static struct platform_driver crystalcove_gpio_driver = {
 	.remove = crystalcove_gpio_remove,
 	.driver = {
 		.name = "crystal_cove_gpio",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/gpio/gpio-cs5535.c b/drivers/gpio/gpio-cs5535.c
index 668127fe90ef..7b0b198a563d 100644
--- a/drivers/gpio/gpio-cs5535.c
+++ b/drivers/gpio/gpio-cs5535.c
@@ -322,7 +322,8 @@ static int cs5535_gpio_probe(struct platform_device *pdev)
 		goto done;
 	}
 
-	if (!request_region(res->start, resource_size(res), pdev->name)) {
+	if (!devm_request_region(&pdev->dev, res->start, resource_size(res),
+				 pdev->name)) {
 		dev_err(&pdev->dev, "can't request region\n");
 		goto done;
 	}
@@ -348,31 +349,24 @@ static int cs5535_gpio_probe(struct platform_device *pdev)
 	/* finally, register with the generic GPIO API */
 	err = gpiochip_add(&cs5535_gpio_chip.chip);
 	if (err)
-		goto release_region;
+		goto done;
 
 	return 0;
 
-release_region:
-	release_region(res->start, resource_size(res));
 done:
 	return err;
 }
 
 static int cs5535_gpio_remove(struct platform_device *pdev)
 {
-	struct resource *r;
-
 	gpiochip_remove(&cs5535_gpio_chip.chip);
 
-	r = platform_get_resource(pdev, IORESOURCE_IO, 0);
-	release_region(r->start, resource_size(r));
 	return 0;
 }
 
 static struct platform_driver cs5535_gpio_driver = {
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 	},
 	.probe = cs5535_gpio_probe,
 	.remove = cs5535_gpio_remove,
diff --git a/drivers/gpio/gpio-da9052.c b/drivers/gpio/gpio-da9052.c
index c5bccd4dec96..389a4d2a4926 100644
--- a/drivers/gpio/gpio-da9052.c
+++ b/drivers/gpio/gpio-da9052.c
@@ -246,7 +246,6 @@ static struct platform_driver da9052_gpio_driver = {
 	.remove = da9052_gpio_remove,
 	.driver = {
 		.name	= "da9052-gpio",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/gpio/gpio-da9055.c b/drivers/gpio/gpio-da9055.c
index 9167c4331081..b8d757036887 100644
--- a/drivers/gpio/gpio-da9055.c
+++ b/drivers/gpio/gpio-da9055.c
@@ -183,7 +183,6 @@ static struct platform_driver da9055_gpio_driver = {
 	.remove = da9055_gpio_remove,
 	.driver = {
 		.name	= "da9055-gpio",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/gpio/gpio-davinci.c b/drivers/gpio/gpio-davinci.c
index 9f0682534e2f..c5e05c82d67c 100644
--- a/drivers/gpio/gpio-davinci.c
+++ b/drivers/gpio/gpio-davinci.c
@@ -234,11 +234,6 @@ static int davinci_gpio_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(dev, "Invalid memory resource\n");
-		return -EBUSY;
-	}
-
 	gpio_base = devm_ioremap_resource(dev, res);
 	if (IS_ERR(gpio_base))
 		return PTR_ERR(gpio_base);
@@ -619,7 +614,6 @@ static struct platform_driver davinci_gpio_driver = {
 	.probe		= davinci_gpio_probe,
 	.driver		= {
 		.name		= "davinci_gpio",
-		.owner		= THIS_MODULE,
 		.of_match_table	= of_match_ptr(davinci_gpio_ids),
 	},
 };
diff --git a/drivers/gpio/gpio-dwapb.c b/drivers/gpio/gpio-dwapb.c
index b43cd84b61f1..b4eb6a657d34 100644
--- a/drivers/gpio/gpio-dwapb.c
+++ b/drivers/gpio/gpio-dwapb.c
@@ -194,7 +194,7 @@ static int dwapb_irq_reqres(struct irq_data *d)
 	struct dwapb_gpio *gpio = igc->private;
 	struct bgpio_chip *bgc = &gpio->ports[0].bgc;
 
-	if (gpio_lock_as_irq(&bgc->gc, irqd_to_hwirq(d))) {
+	if (gpiochip_lock_as_irq(&bgc->gc, irqd_to_hwirq(d))) {
 		dev_err(gpio->dev, "unable to lock HW IRQ %lu for IRQ\n",
 			irqd_to_hwirq(d));
 		return -EINVAL;
@@ -208,7 +208,7 @@ static void dwapb_irq_relres(struct irq_data *d)
 	struct dwapb_gpio *gpio = igc->private;
 	struct bgpio_chip *bgc = &gpio->ports[0].bgc;
 
-	gpio_unlock_as_irq(&bgc->gc, irqd_to_hwirq(d));
+	gpiochip_unlock_as_irq(&bgc->gc, irqd_to_hwirq(d));
 }
 
 static int dwapb_irq_set_type(struct irq_data *d, u32 type)
@@ -703,7 +703,6 @@ static SIMPLE_DEV_PM_OPS(dwapb_gpio_pm_ops, dwapb_gpio_suspend,
 static struct platform_driver dwapb_gpio_driver = {
 	.driver		= {
 		.name	= "gpio-dwapb",
-		.owner	= THIS_MODULE,
 		.pm	= &dwapb_gpio_pm_ops,
 		.of_match_table = of_match_ptr(dwapb_of_match),
 	},
diff --git a/drivers/gpio/gpio-em.c b/drivers/gpio/gpio-em.c
index fe49ec3cdb7d..3cfcfc620c8e 100644
--- a/drivers/gpio/gpio-em.c
+++ b/drivers/gpio/gpio-em.c
@@ -103,7 +103,7 @@ static int em_gio_irq_reqres(struct irq_data *d)
 {
 	struct em_gio_priv *p = irq_data_get_irq_chip_data(d);
 
-	if (gpio_lock_as_irq(&p->gpio_chip, irqd_to_hwirq(d))) {
+	if (gpiochip_lock_as_irq(&p->gpio_chip, irqd_to_hwirq(d))) {
 		dev_err(p->gpio_chip.dev,
 			"unable to lock HW IRQ %lu for IRQ\n",
 			irqd_to_hwirq(d));
@@ -116,7 +116,7 @@ static void em_gio_irq_relres(struct irq_data *d)
 {
 	struct em_gio_priv *p = irq_data_get_irq_chip_data(d);
 
-	gpio_unlock_as_irq(&p->gpio_chip, irqd_to_hwirq(d));
+	gpiochip_unlock_as_irq(&p->gpio_chip, irqd_to_hwirq(d));
 }
 
 
@@ -330,12 +330,7 @@ static int em_gio_probe(struct platform_device *pdev)
 			goto err0;
 		}
 
-		ret = of_alias_get_id(pdev->dev.of_node, "gpio");
-		if (ret < 0) {
-			dev_err(&pdev->dev, "Couldn't get OF id\n");
-			goto err0;
-		}
-		pdata->gpio_base = ret * 32; /* 32 GPIOs per instance */
+		pdata->gpio_base = -1;
 	}
 
 	gpio_chip = &p->gpio_chip;
@@ -428,7 +423,6 @@ static struct platform_driver em_gio_device_driver = {
 	.driver		= {
 		.name	= "em_gio",
 		.of_match_table = em_gio_dt_ids,
-		.owner		= THIS_MODULE,
 	}
 };
 
diff --git a/drivers/gpio/gpio-ep93xx.c b/drivers/gpio/gpio-ep93xx.c
index dcc2bb4074ef..45684f36ddb1 100644
--- a/drivers/gpio/gpio-ep93xx.c
+++ b/drivers/gpio/gpio-ep93xx.c
@@ -374,7 +374,6 @@ static int ep93xx_gpio_probe(struct platform_device *pdev)
 static struct platform_driver ep93xx_gpio_driver = {
 	.driver		= {
 		.name	= "gpio-ep93xx",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= ep93xx_gpio_probe,
 };
diff --git a/drivers/gpio/gpio-f7188x.c b/drivers/gpio/gpio-f7188x.c
index fd3202f968ff..1be291ac6319 100644
--- a/drivers/gpio/gpio-f7188x.c
+++ b/drivers/gpio/gpio-f7188x.c
@@ -417,7 +417,6 @@ err:
 
 static struct platform_driver f7188x_gpio_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= DRVNAME,
 	},
 	.probe		= f7188x_gpio_probe,
diff --git a/drivers/gpio/gpio-ge.c b/drivers/gpio/gpio-ge.c
index 1237a73c3c91..aea5c2a53cc0 100644
--- a/drivers/gpio/gpio-ge.c
+++ b/drivers/gpio/gpio-ge.c
@@ -120,7 +120,6 @@ static int __init gef_gpio_probe(struct platform_device *pdev)
 static struct platform_driver gef_gpio_driver = {
 	.driver = {
 		.name		= "gef-gpio",
-		.owner		= THIS_MODULE,
 		.of_match_table	= gef_gpio_ids,
 	},
 };
diff --git a/drivers/gpio/gpio-grgpio.c b/drivers/gpio/gpio-grgpio.c
index 66ad3df9d9cf..09daaf2aeb56 100644
--- a/drivers/gpio/gpio-grgpio.c
+++ b/drivers/gpio/gpio-grgpio.c
@@ -441,6 +441,7 @@ static int grgpio_probe(struct platform_device *ofdev)
 	err = gpiochip_add(gc);
 	if (err) {
 		dev_err(&ofdev->dev, "Could not add gpiochip\n");
+		irq_domain_remove(priv->domain);
 		return err;
 	}
 
@@ -490,7 +491,6 @@ MODULE_DEVICE_TABLE(of, grgpio_match);
 static struct platform_driver grgpio_driver = {
 	.driver = {
 		.name = "grgpio",
-		.owner = THIS_MODULE,
 		.of_match_table = grgpio_match,
 	},
 	.probe = grgpio_probe,
diff --git a/drivers/gpio/gpio-ich.c b/drivers/gpio/gpio-ich.c
index 3784e81e7762..7818cd1453ae 100644
--- a/drivers/gpio/gpio-ich.c
+++ b/drivers/gpio/gpio-ich.c
@@ -526,7 +526,6 @@ static int ichx_gpio_remove(struct platform_device *pdev)
 
 static struct platform_driver ichx_gpio_driver = {
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= DRV_NAME,
 	},
 	.probe		= ichx_gpio_probe,
diff --git a/drivers/gpio/gpio-iop.c b/drivers/gpio/gpio-iop.c
index 0a5e9d3f308c..2ed0237a8baf 100644
--- a/drivers/gpio/gpio-iop.c
+++ b/drivers/gpio/gpio-iop.c
@@ -120,7 +120,6 @@ static int iop3xx_gpio_probe(struct platform_device *pdev)
 static struct platform_driver iop3xx_gpio_driver = {
 	.driver = {
 		.name = "gpio-iop",
-		.owner = THIS_MODULE,
 	},
 	.probe = iop3xx_gpio_probe,
 };
diff --git a/drivers/gpio/gpio-janz-ttl.c b/drivers/gpio/gpio-janz-ttl.c
index 29ffe22ad97a..3a1664335f5e 100644
--- a/drivers/gpio/gpio-janz-ttl.c
+++ b/drivers/gpio/gpio-janz-ttl.c
@@ -203,7 +203,6 @@ static int ttl_remove(struct platform_device *pdev)
 static struct platform_driver ttl_driver = {
 	.driver		= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= ttl_probe,
 	.remove		= ttl_remove,
diff --git a/drivers/gpio/gpio-kempld.c b/drivers/gpio/gpio-kempld.c
index fd150adeebf9..443518f63f15 100644
--- a/drivers/gpio/gpio-kempld.c
+++ b/drivers/gpio/gpio-kempld.c
@@ -206,7 +206,6 @@ static int kempld_gpio_remove(struct platform_device *pdev)
 static struct platform_driver kempld_gpio_driver = {
 	.driver = {
 		.name = "kempld-gpio",
-		.owner = THIS_MODULE,
 	},
 	.probe		= kempld_gpio_probe,
 	.remove		= kempld_gpio_remove,
diff --git a/drivers/gpio/gpio-lp3943.c b/drivers/gpio/gpio-lp3943.c
index 6bbdad805b78..cfc5b12b43ad 100644
--- a/drivers/gpio/gpio-lp3943.c
+++ b/drivers/gpio/gpio-lp3943.c
@@ -231,7 +231,6 @@ static struct platform_driver lp3943_gpio_driver = {
 	.remove = lp3943_gpio_remove,
 	.driver = {
 		.name = "lp3943-gpio",
-		.owner = THIS_MODULE,
 		.of_match_table = lp3943_gpio_of_match,
 	},
 };
diff --git a/drivers/gpio/gpio-lpc32xx.c b/drivers/gpio/gpio-lpc32xx.c
index b9b9799b368b..47e2dde63734 100644
--- a/drivers/gpio/gpio-lpc32xx.c
+++ b/drivers/gpio/gpio-lpc32xx.c
@@ -569,7 +569,6 @@ static const struct of_device_id lpc32xx_gpio_of_match[] = {
 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,
diff --git a/drivers/gpio/gpio-lynxpoint.c b/drivers/gpio/gpio-lynxpoint.c
index fa945ec9ccff..127c755b38dc 100644
--- a/drivers/gpio/gpio-lynxpoint.c
+++ b/drivers/gpio/gpio-lynxpoint.c
@@ -450,7 +450,6 @@ static struct platform_driver lp_gpio_driver = {
 	.remove         = lp_gpio_remove,
 	.driver         = {
 		.name   = "lp_gpio",
-		.owner  = THIS_MODULE,
 		.pm	= &lp_gpio_pm_ops,
 		.acpi_match_table = ACPI_PTR(lynxpoint_gpio_acpi_match),
 	},
diff --git a/drivers/gpio/gpio-mcp23s08.c b/drivers/gpio/gpio-mcp23s08.c
index 8488e2fd307c..da9c316059bc 100644
--- a/drivers/gpio/gpio-mcp23s08.c
+++ b/drivers/gpio/gpio-mcp23s08.c
@@ -65,6 +65,7 @@ struct mcp23s08_ops {
 
 struct mcp23s08 {
 	u8			addr;
+	bool			irq_active_high;
 
 	u16			cache[11];
 	u16			irq_rise;
@@ -444,7 +445,7 @@ static int mcp23s08_irq_reqres(struct irq_data *data)
 {
 	struct mcp23s08 *mcp = irq_data_get_irq_chip_data(data);
 
-	if (gpio_lock_as_irq(&mcp->chip, data->hwirq)) {
+	if (gpiochip_lock_as_irq(&mcp->chip, data->hwirq)) {
 		dev_err(mcp->chip.dev,
 			"unable to lock HW IRQ %lu for IRQ usage\n",
 			data->hwirq);
@@ -458,7 +459,7 @@ static void mcp23s08_irq_relres(struct irq_data *data)
 {
 	struct mcp23s08 *mcp = irq_data_get_irq_chip_data(data);
 
-	gpio_unlock_as_irq(&mcp->chip, data->hwirq);
+	gpiochip_unlock_as_irq(&mcp->chip, data->hwirq);
 }
 
 static struct irq_chip mcp23s08_irq_chip = {
@@ -476,6 +477,7 @@ static int mcp23s08_irq_setup(struct mcp23s08 *mcp)
 {
 	struct gpio_chip *chip = &mcp->chip;
 	int err, irq, j;
+	unsigned long irqflags = IRQF_ONESHOT | IRQF_SHARED;
 
 	mutex_init(&mcp->irq_lock);
 
@@ -484,9 +486,13 @@ static int mcp23s08_irq_setup(struct mcp23s08 *mcp)
 	if (!mcp->irq_domain)
 		return -ENODEV;
 
+	if (mcp->irq_active_high)
+		irqflags |= IRQF_TRIGGER_HIGH;
+	else
+		irqflags |= IRQF_TRIGGER_LOW;
+
 	err = devm_request_threaded_irq(chip->dev, mcp->irq, NULL, mcp23s08_irq,
-					IRQF_TRIGGER_LOW | IRQF_ONESHOT,
-					dev_name(chip->dev), mcp);
+					irqflags, dev_name(chip->dev), mcp);
 	if (err != 0) {
 		dev_err(chip->dev, "unable to request IRQ#%d: %d\n",
 			mcp->irq, err);
@@ -514,8 +520,6 @@ static void mcp23s08_irq_teardown(struct mcp23s08 *mcp)
 {
 	unsigned int irq, i;
 
-	free_irq(mcp->irq, mcp);
-
 	for (i = 0; i < mcp->chip.ngpio; i++) {
 		irq = irq_find_mapping(mcp->irq_domain, i);
 		if (irq > 0)
@@ -590,6 +594,7 @@ static int mcp23s08_probe_one(struct mcp23s08 *mcp, struct device *dev,
 
 	mcp->data = data;
 	mcp->addr = addr;
+	mcp->irq_active_high = false;
 
 	mcp->chip.direction_input = mcp23s08_direction_input;
 	mcp->chip.get = mcp23s08_get;
@@ -649,14 +654,25 @@ static int mcp23s08_probe_one(struct mcp23s08 *mcp, struct device *dev,
 		goto fail;
 
 	mcp->irq_controller = pdata->irq_controller;
-	if (mcp->irq && mcp->irq_controller && (type == MCP_TYPE_017))
-		mirror = pdata->mirror;
+	if (mcp->irq && mcp->irq_controller) {
+		mcp->irq_active_high =
+			of_property_read_bool(mcp->chip.dev->of_node,
+					      "microchip,irq-active-high");
 
-	if ((status & IOCON_SEQOP) || !(status & IOCON_HAEN) || mirror) {
+		if (type == MCP_TYPE_017)
+			mirror = pdata->mirror;
+	}
+
+	if ((status & IOCON_SEQOP) || !(status & IOCON_HAEN) || mirror ||
+	     mcp->irq_active_high) {
 		/* mcp23s17 has IOCON twice, make sure they are in sync */
 		status &= ~(IOCON_SEQOP | (IOCON_SEQOP << 8));
 		status |= IOCON_HAEN | (IOCON_HAEN << 8);
-		status &= ~(IOCON_INTPOL | (IOCON_INTPOL << 8));
+		if (mcp->irq_active_high)
+			status |= IOCON_INTPOL | (IOCON_INTPOL << 8);
+		else
+			status &= ~(IOCON_INTPOL | (IOCON_INTPOL << 8));
+
 		if (mirror)
 			status |= IOCON_MIRROR | (IOCON_MIRROR << 8);
 
@@ -936,11 +952,14 @@ static int mcp23s08_probe(struct spi_device *spi)
 		return -ENOMEM;
 	spi_set_drvdata(spi, data);
 
+	spi->irq = irq_of_parse_and_map(spi->dev.of_node, 0);
+
 	for (addr = 0; addr < ARRAY_SIZE(pdata->chip); addr++) {
 		if (!(spi_present_mask & (1 << addr)))
 			continue;
 		chips--;
 		data->mcp[addr] = &data->chip[chips];
+		data->mcp[addr]->irq = spi->irq;
 		status = mcp23s08_probe_one(data->mcp[addr], &spi->dev, spi,
 					    0x40 | (addr << 1), type, pdata,
 					    addr);
@@ -981,6 +1000,8 @@ static int mcp23s08_remove(struct spi_device *spi)
 		if (!data->mcp[addr])
 			continue;
 
+		if (spi->irq && data->mcp[addr]->irq_controller)
+			mcp23s08_irq_teardown(data->mcp[addr]);
 		gpiochip_remove(&data->mcp[addr]->chip);
 	}
 	kfree(data);
diff --git a/drivers/gpio/gpio-mm-lantiq.c b/drivers/gpio/gpio-mm-lantiq.c
index 2983dfbd0668..f228b1ce0ce0 100644
--- a/drivers/gpio/gpio-mm-lantiq.c
+++ b/drivers/gpio/gpio-mm-lantiq.c
@@ -145,7 +145,6 @@ 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,
 	},
 };
diff --git a/drivers/gpio/gpio-moxart.c b/drivers/gpio/gpio-moxart.c
index 4661e181be04..31e2551ed903 100644
--- a/drivers/gpio/gpio-moxart.c
+++ b/drivers/gpio/gpio-moxart.c
@@ -142,7 +142,6 @@ static const struct of_device_id moxart_gpio_match[] = {
 static struct platform_driver moxart_gpio_driver = {
 	.driver	= {
 		.name		= "moxart-gpio",
-		.owner		= THIS_MODULE,
 		.of_match_table	= moxart_gpio_match,
 	},
 	.probe	= moxart_gpio_probe,
diff --git a/drivers/gpio/gpio-mpc5200.c b/drivers/gpio/gpio-mpc5200.c
index 42647f26c9e0..8ce6c9510035 100644
--- a/drivers/gpio/gpio-mpc5200.c
+++ b/drivers/gpio/gpio-mpc5200.c
@@ -192,7 +192,6 @@ static const struct of_device_id mpc52xx_wkup_gpiochip_match[] = {
 static struct platform_driver mpc52xx_wkup_gpiochip_driver = {
 	.driver = {
 		.name = "mpc5200-gpio-wkup",
-		.owner = THIS_MODULE,
 		.of_match_table = mpc52xx_wkup_gpiochip_match,
 	},
 	.probe = mpc52xx_wkup_gpiochip_probe,
@@ -347,7 +346,6 @@ static const struct of_device_id mpc52xx_simple_gpiochip_match[] = {
 static struct platform_driver mpc52xx_simple_gpiochip_driver = {
 	.driver = {
 		.name = "mpc5200-gpio",
-		.owner = THIS_MODULE,
 		.of_match_table = mpc52xx_simple_gpiochip_match,
 	},
 	.probe = mpc52xx_simple_gpiochip_probe,
diff --git a/drivers/gpio/gpio-mpc8xxx.c b/drivers/gpio/gpio-mpc8xxx.c
index d7d6d72eba33..d1ff879e6ff2 100644
--- a/drivers/gpio/gpio-mpc8xxx.c
+++ b/drivers/gpio/gpio-mpc8xxx.c
@@ -105,6 +105,32 @@ static void mpc8xxx_gpio_set(struct gpio_chip *gc, unsigned int gpio, int val)
 	spin_unlock_irqrestore(&mpc8xxx_gc->lock, flags);
 }
 
+static void mpc8xxx_gpio_set_multiple(struct gpio_chip *gc,
+				      unsigned long *mask, unsigned long *bits)
+{
+	struct of_mm_gpio_chip *mm = to_of_mm_gpio_chip(gc);
+	struct mpc8xxx_gpio_chip *mpc8xxx_gc = to_mpc8xxx_gpio_chip(mm);
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&mpc8xxx_gc->lock, flags);
+
+	for (i = 0; i < gc->ngpio; i++) {
+		if (*mask == 0)
+			break;
+		if (__test_and_clear_bit(i, mask)) {
+			if (test_bit(i, bits))
+				mpc8xxx_gc->data |= mpc8xxx_gpio2mask(i);
+			else
+				mpc8xxx_gc->data &= ~mpc8xxx_gpio2mask(i);
+		}
+	}
+
+	out_be32(mm->regs + GPIO_DAT, mpc8xxx_gc->data);
+
+	spin_unlock_irqrestore(&mpc8xxx_gc->lock, flags);
+}
+
 static int mpc8xxx_gpio_dir_in(struct gpio_chip *gc, unsigned int gpio)
 {
 	struct of_mm_gpio_chip *mm = to_of_mm_gpio_chip(gc);
@@ -344,6 +370,7 @@ static void __init mpc8xxx_add_controller(struct device_node *np)
 	gc->get = of_device_is_compatible(np, "fsl,mpc8572-gpio") ?
 		mpc8572_gpio_get : mpc8xxx_gpio_get;
 	gc->set = mpc8xxx_gpio_set;
+	gc->set_multiple = mpc8xxx_gpio_set_multiple;
 	gc->to_irq = mpc8xxx_gpio_to_irq;
 
 	ret = of_mm_gpiochip_add(np, mm_gc);
diff --git a/drivers/gpio/gpio-msic.c b/drivers/gpio/gpio-msic.c
index 8f70ded82a2b..01acf0a8cdb1 100644
--- a/drivers/gpio/gpio-msic.c
+++ b/drivers/gpio/gpio-msic.c
@@ -321,7 +321,6 @@ err:
 static struct platform_driver platform_msic_gpio_driver = {
 	.driver = {
 		.name		= "msic_gpio",
-		.owner		= THIS_MODULE,
 	},
 	.probe		= platform_msic_gpio_probe,
 };
diff --git a/drivers/gpio/gpio-msm-v1.c b/drivers/gpio/gpio-msm-v1.c
index 73b73969d361..edf285e26667 100644
--- a/drivers/gpio/gpio-msm-v1.c
+++ b/drivers/gpio/gpio-msm-v1.c
@@ -686,7 +686,7 @@ static int gpio_msm_v1_probe(struct platform_device *pdev)
 	irq_set_chained_handler(irq1, msm_gpio_irq_handler);
 	irq_set_chained_handler(irq2, msm_gpio_irq_handler);
 	irq_set_irq_wake(irq1, 1);
-	irq_set_irq_wake(irq2, 2);
+	irq_set_irq_wake(irq2, 1);
 	return 0;
 }
 
@@ -701,7 +701,6 @@ MODULE_DEVICE_TABLE(platform, gpio_msm_v1_device_ids);
 static struct platform_driver gpio_msm_v1_driver = {
 	.driver = {
 		.name = "gpio-msm-v1",
-		.owner = THIS_MODULE,
 	},
 	.probe = gpio_msm_v1_probe,
 	.id_table = gpio_msm_v1_device_ids,
diff --git a/drivers/gpio/gpio-msm-v2.c b/drivers/gpio/gpio-msm-v2.c
index 94f57670df9a..52ff18229fdc 100644
--- a/drivers/gpio/gpio-msm-v2.c
+++ b/drivers/gpio/gpio-msm-v2.c
@@ -450,7 +450,6 @@ static struct platform_driver msm_gpio_driver = {
 	.remove = msm_gpio_remove,
 	.driver = {
 		.name = "msmgpio",
-		.owner = THIS_MODULE,
 		.of_match_table = msm_gpio_of_match,
 	},
 };
diff --git a/drivers/gpio/gpio-mvebu.c b/drivers/gpio/gpio-mvebu.c
index 418e38650363..7bc3e9b288f3 100644
--- a/drivers/gpio/gpio-mvebu.c
+++ b/drivers/gpio/gpio-mvebu.c
@@ -83,6 +83,14 @@ struct mvebu_gpio_chip {
 	int		   irqbase;
 	struct irq_domain *domain;
 	int                soc_variant;
+
+	/* Used to preserve GPIO registers accross suspend/resume */
+	u32                out_reg;
+	u32                io_conf_reg;
+	u32                blink_en_reg;
+	u32                in_pol_reg;
+	u32                edge_mask_regs[4];
+	u32                level_mask_regs[4];
 };
 
 /*
@@ -554,6 +562,93 @@ static const struct of_device_id mvebu_gpio_of_match[] = {
 };
 MODULE_DEVICE_TABLE(of, mvebu_gpio_of_match);
 
+static int mvebu_gpio_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct mvebu_gpio_chip *mvchip = platform_get_drvdata(pdev);
+	int i;
+
+	mvchip->out_reg = readl(mvebu_gpioreg_out(mvchip));
+	mvchip->io_conf_reg = readl(mvebu_gpioreg_io_conf(mvchip));
+	mvchip->blink_en_reg = readl(mvebu_gpioreg_blink(mvchip));
+	mvchip->in_pol_reg = readl(mvebu_gpioreg_in_pol(mvchip));
+
+	switch (mvchip->soc_variant) {
+	case MVEBU_GPIO_SOC_VARIANT_ORION:
+		mvchip->edge_mask_regs[0] =
+			readl(mvchip->membase + GPIO_EDGE_MASK_OFF);
+		mvchip->level_mask_regs[0] =
+			readl(mvchip->membase + GPIO_LEVEL_MASK_OFF);
+		break;
+	case MVEBU_GPIO_SOC_VARIANT_MV78200:
+		for (i = 0; i < 2; i++) {
+			mvchip->edge_mask_regs[i] =
+				readl(mvchip->membase +
+				      GPIO_EDGE_MASK_MV78200_OFF(i));
+			mvchip->level_mask_regs[i] =
+				readl(mvchip->membase +
+				      GPIO_LEVEL_MASK_MV78200_OFF(i));
+		}
+		break;
+	case MVEBU_GPIO_SOC_VARIANT_ARMADAXP:
+		for (i = 0; i < 4; i++) {
+			mvchip->edge_mask_regs[i] =
+				readl(mvchip->membase +
+				      GPIO_EDGE_MASK_ARMADAXP_OFF(i));
+			mvchip->level_mask_regs[i] =
+				readl(mvchip->membase +
+				      GPIO_LEVEL_MASK_ARMADAXP_OFF(i));
+		}
+		break;
+	default:
+		BUG();
+	}
+
+	return 0;
+}
+
+static int mvebu_gpio_resume(struct platform_device *pdev)
+{
+	struct mvebu_gpio_chip *mvchip = platform_get_drvdata(pdev);
+	int i;
+
+	writel(mvchip->out_reg, mvebu_gpioreg_out(mvchip));
+	writel(mvchip->io_conf_reg, mvebu_gpioreg_io_conf(mvchip));
+	writel(mvchip->blink_en_reg, mvebu_gpioreg_blink(mvchip));
+	writel(mvchip->in_pol_reg, mvebu_gpioreg_in_pol(mvchip));
+
+	switch (mvchip->soc_variant) {
+	case MVEBU_GPIO_SOC_VARIANT_ORION:
+		writel(mvchip->edge_mask_regs[0],
+		       mvchip->membase + GPIO_EDGE_MASK_OFF);
+		writel(mvchip->level_mask_regs[0],
+		       mvchip->membase + GPIO_LEVEL_MASK_OFF);
+		break;
+	case MVEBU_GPIO_SOC_VARIANT_MV78200:
+		for (i = 0; i < 2; i++) {
+			writel(mvchip->edge_mask_regs[i],
+			       mvchip->membase + GPIO_EDGE_MASK_MV78200_OFF(i));
+			writel(mvchip->level_mask_regs[i],
+			       mvchip->membase +
+			       GPIO_LEVEL_MASK_MV78200_OFF(i));
+		}
+		break;
+	case MVEBU_GPIO_SOC_VARIANT_ARMADAXP:
+		for (i = 0; i < 4; i++) {
+			writel(mvchip->edge_mask_regs[i],
+			       mvchip->membase +
+			       GPIO_EDGE_MASK_ARMADAXP_OFF(i));
+			writel(mvchip->level_mask_regs[i],
+			       mvchip->membase +
+			       GPIO_LEVEL_MASK_ARMADAXP_OFF(i));
+		}
+		break;
+	default:
+		BUG();
+	}
+
+	return 0;
+}
+
 static int mvebu_gpio_probe(struct platform_device *pdev)
 {
 	struct mvebu_gpio_chip *mvchip;
@@ -577,6 +672,8 @@ static int mvebu_gpio_probe(struct platform_device *pdev)
 	if (!mvchip)
 		return -ENOMEM;
 
+	platform_set_drvdata(pdev, mvchip);
+
 	if (of_property_read_u32(pdev->dev.of_node, "ngpios", &ngpios)) {
 		dev_err(&pdev->dev, "Missing ngpios OF property\n");
 		return -ENODEV;
@@ -731,9 +828,10 @@ static int mvebu_gpio_probe(struct platform_device *pdev)
 static struct platform_driver mvebu_gpio_driver = {
 	.driver		= {
 		.name	        = "mvebu-gpio",
-		.owner	        = THIS_MODULE,
 		.of_match_table = mvebu_gpio_of_match,
 	},
 	.probe		= mvebu_gpio_probe,
+	.suspend        = mvebu_gpio_suspend,
+	.resume         = mvebu_gpio_resume,
 };
 module_platform_driver(mvebu_gpio_driver);
diff --git a/drivers/gpio/gpio-mxc.c b/drivers/gpio/gpio-mxc.c
index f4e54a92e04a..9f7446a7ac64 100644
--- a/drivers/gpio/gpio-mxc.c
+++ b/drivers/gpio/gpio-mxc.c
@@ -496,7 +496,6 @@ out_bgio:
 static struct platform_driver mxc_gpio_driver = {
 	.driver		= {
 		.name	= "gpio-mxc",
-		.owner	= THIS_MODULE,
 		.of_match_table = mxc_gpio_dt_ids,
 	},
 	.probe		= mxc_gpio_probe,
diff --git a/drivers/gpio/gpio-mxs.c b/drivers/gpio/gpio-mxs.c
index 8ffdd7d2bade..84cbda6acdda 100644
--- a/drivers/gpio/gpio-mxs.c
+++ b/drivers/gpio/gpio-mxs.c
@@ -227,6 +227,18 @@ static int mxs_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
 	return irq_find_mapping(port->domain, offset);
 }
 
+static int mxs_gpio_get_direction(struct gpio_chip *gc, unsigned offset)
+{
+	struct bgpio_chip *bgc = to_bgpio_chip(gc);
+	struct mxs_gpio_port *port =
+		container_of(bgc, struct mxs_gpio_port, bgc);
+	u32 mask = 1 << offset;
+	u32 dir;
+
+	dir = readl(port->base + PINCTRL_DOE(port));
+	return !(dir & mask);
+}
+
 static struct platform_device_id mxs_gpio_ids[] = {
 	{
 		.name = "imx23-gpio",
@@ -320,6 +332,7 @@ static int mxs_gpio_probe(struct platform_device *pdev)
 		goto out_irqdesc_free;
 
 	port->bgc.gc.to_irq = mxs_gpio_to_irq;
+	port->bgc.gc.get_direction = mxs_gpio_get_direction;
 	port->bgc.gc.base = port->id * 32;
 
 	err = gpiochip_add(&port->bgc.gc);
@@ -338,7 +351,6 @@ out_irqdesc_free:
 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,
diff --git a/drivers/gpio/gpio-octeon.c b/drivers/gpio/gpio-octeon.c
index 5c5770c99c80..62ae251d4490 100644
--- a/drivers/gpio/gpio-octeon.c
+++ b/drivers/gpio/gpio-octeon.c
@@ -144,7 +144,6 @@ MODULE_DEVICE_TABLE(of, octeon_gpio_match);
 static struct platform_driver octeon_gpio_driver = {
 	.driver = {
 		.name		= "octeon_gpio",
-		.owner		= THIS_MODULE,
 		.of_match_table = octeon_gpio_match,
 	},
 	.probe		= octeon_gpio_probe,
diff --git a/drivers/gpio/gpio-omap.c b/drivers/gpio/gpio-omap.c
index 3d6b445665ad..30646cfe0efa 100644
--- a/drivers/gpio/gpio-omap.c
+++ b/drivers/gpio/gpio-omap.c
@@ -800,7 +800,7 @@ static void omap_gpio_irq_shutdown(struct irq_data *d)
 	unsigned offset = GPIO_INDEX(bank, gpio);
 
 	spin_lock_irqsave(&bank->lock, flags);
-	gpio_unlock_as_irq(&bank->chip, offset);
+	gpiochip_unlock_as_irq(&bank->chip, offset);
 	bank->irq_usage &= ~(BIT(offset));
 	omap_disable_gpio_module(bank, offset);
 	omap_reset_gpio(bank, gpio);
diff --git a/drivers/gpio/gpio-pl061.c b/drivers/gpio/gpio-pl061.c
index 84b49cfb81a8..04756130437f 100644
--- a/drivers/gpio/gpio-pl061.c
+++ b/drivers/gpio/gpio-pl061.c
@@ -52,28 +52,34 @@ struct pl061_gpio {
 
 	void __iomem		*base;
 	struct gpio_chip	gc;
+	bool			uses_pinctrl;
 
 #ifdef CONFIG_PM
 	struct pl061_context_save_regs csave_regs;
 #endif
 };
 
-static int pl061_gpio_request(struct gpio_chip *chip, unsigned offset)
+static int pl061_gpio_request(struct gpio_chip *gc, 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;
+	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
+	int gpio = gc->base + offset;
 
-	return pinctrl_request_gpio(gpio);
+	if (chip->uses_pinctrl)
+		return pinctrl_request_gpio(gpio);
+	return 0;
 }
 
-static void pl061_gpio_free(struct gpio_chip *chip, unsigned offset)
+static void pl061_gpio_free(struct gpio_chip *gc, unsigned offset)
 {
-	int gpio = chip->base + offset;
+	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
+	int gpio = gc->base + offset;
 
-	pinctrl_free_gpio(gpio);
+	if (chip->uses_pinctrl)
+		pinctrl_free_gpio(gpio);
 }
 
 static int pl061_direction_input(struct gpio_chip *gc, unsigned offset)
@@ -263,6 +269,8 @@ static int pl061_probe(struct amba_device *adev, const struct amba_id *id)
 		return PTR_ERR(chip->base);
 
 	spin_lock_init(&chip->lock);
+	if (of_property_read_bool(dev->of_node, "gpio-ranges"))
+		chip->uses_pinctrl = true;
 
 	chip->gc.request = pl061_gpio_request;
 	chip->gc.free = pl061_gpio_free;
diff --git a/drivers/gpio/gpio-rc5t583.c b/drivers/gpio/gpio-rc5t583.c
index 769233d2da6d..6eabf239676b 100644
--- a/drivers/gpio/gpio-rc5t583.c
+++ b/drivers/gpio/gpio-rc5t583.c
@@ -155,7 +155,6 @@ static int rc5t583_gpio_remove(struct platform_device *pdev)
 static struct platform_driver rc5t583_gpio_driver = {
 	.driver = {
 		.name    = "rc5t583-gpio",
-		.owner   = THIS_MODULE,
 	},
 	.probe		= rc5t583_gpio_probe,
 	.remove		= rc5t583_gpio_remove,
diff --git a/drivers/gpio/gpio-rcar.c b/drivers/gpio/gpio-rcar.c
index bf6c09450fee..584484e3f1e3 100644
--- a/drivers/gpio/gpio-rcar.c
+++ b/drivers/gpio/gpio-rcar.c
@@ -1,6 +1,7 @@
 /*
  * Renesas R-Car GPIO Support
  *
+ *  Copyright (C) 2014 Renesas Electronics Corporation
  *  Copyright (C) 2013 Magnus Damm
  *
  * This program is free software; you can redistribute it and/or modify
@@ -291,22 +292,30 @@ struct gpio_rcar_info {
 	bool has_both_edge_trigger;
 };
 
+static const struct gpio_rcar_info gpio_rcar_info_gen1 = {
+	.has_both_edge_trigger = false,
+};
+
+static const struct gpio_rcar_info gpio_rcar_info_gen2 = {
+	.has_both_edge_trigger = true,
+};
+
 static const struct of_device_id gpio_rcar_of_table[] = {
 	{
 		.compatible = "renesas,gpio-r8a7790",
-		.data = (void *)&(const struct gpio_rcar_info) {
-			.has_both_edge_trigger = true,
-		},
+		.data = &gpio_rcar_info_gen2,
 	}, {
 		.compatible = "renesas,gpio-r8a7791",
-		.data = (void *)&(const struct gpio_rcar_info) {
-			.has_both_edge_trigger = true,
-		},
+		.data = &gpio_rcar_info_gen2,
+	}, {
+		.compatible = "renesas,gpio-r8a7793",
+		.data = &gpio_rcar_info_gen2,
+	}, {
+		.compatible = "renesas,gpio-r8a7794",
+		.data = &gpio_rcar_info_gen2,
 	}, {
 		.compatible = "renesas,gpio-rcar",
-		.data = (void *)&(const struct gpio_rcar_info) {
-			.has_both_edge_trigger = false,
-		},
+		.data = &gpio_rcar_info_gen1,
 	}, {
 		/* Terminator */
 	},
diff --git a/drivers/gpio/gpio-sch.c b/drivers/gpio/gpio-sch.c
index 99720c8bc8ed..0a0cf1307d2f 100644
--- a/drivers/gpio/gpio-sch.c
+++ b/drivers/gpio/gpio-sch.c
@@ -250,7 +250,6 @@ static int sch_gpio_remove(struct platform_device *pdev)
 static struct platform_driver sch_gpio_driver = {
 	.driver = {
 		.name = "sch_gpio",
-		.owner = THIS_MODULE,
 	},
 	.probe		= sch_gpio_probe,
 	.remove		= sch_gpio_remove,
diff --git a/drivers/gpio/gpio-spear-spics.c b/drivers/gpio/gpio-spear-spics.c
index 353263c85d26..69ffca5b073b 100644
--- a/drivers/gpio/gpio-spear-spics.c
+++ b/drivers/gpio/gpio-spear-spics.c
@@ -191,7 +191,6 @@ MODULE_DEVICE_TABLE(of, spics_gpio_of_match);
 static struct platform_driver spics_gpio_driver = {
 	.probe = spics_gpio_probe,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "spear-spics-gpio",
 		.of_match_table = spics_gpio_of_match,
 	},
@@ -204,5 +203,5 @@ static int __init spics_gpio_init(void)
 subsys_initcall(spics_gpio_init);
 
 MODULE_AUTHOR("Shiraz Hashim <shiraz.linux.kernel@gmail.com>");
-MODULE_DESCRIPTION("ST Microlectronics SPEAr SPI Chip Select Abstraction");
+MODULE_DESCRIPTION("STMicroelectronics SPEAr SPI Chip Select Abstraction");
 MODULE_LICENSE("GPL");
diff --git a/drivers/gpio/gpio-sta2x11.c b/drivers/gpio/gpio-sta2x11.c
index 68e3fcb1acea..18579ac65b2b 100644
--- a/drivers/gpio/gpio-sta2x11.c
+++ b/drivers/gpio/gpio-sta2x11.c
@@ -429,7 +429,6 @@ err_free_descs:
 static struct platform_driver sta2x11_gpio_platform_driver = {
 	.driver = {
 		.name	= "sta2x11-gpio",
-		.owner	= THIS_MODULE,
 	},
 	.probe = gsta_probe,
 };
diff --git a/drivers/gpio/gpio-stp-xway.c b/drivers/gpio/gpio-stp-xway.c
index 7e359b7cce1b..202361eb7279 100644
--- a/drivers/gpio/gpio-stp-xway.c
+++ b/drivers/gpio/gpio-stp-xway.c
@@ -199,21 +199,17 @@ static int xway_stp_hw_init(struct xway_stp *chip)
 
 static int xway_stp_probe(struct platform_device *pdev)
 {
-	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	struct resource *res;
 	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;
 
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	chip->virt = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(chip->virt))
 		return PTR_ERR(chip->virt);
@@ -287,7 +283,6 @@ 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,
 	},
 };
diff --git a/drivers/gpio/gpio-syscon.c b/drivers/gpio/gpio-syscon.c
index e82fde4b6898..257e2989215c 100644
--- a/drivers/gpio/gpio-syscon.c
+++ b/drivers/gpio/gpio-syscon.c
@@ -253,7 +253,6 @@ static int syscon_gpio_remove(struct platform_device *pdev)
 static struct platform_driver syscon_gpio_driver = {
 	.driver	= {
 		.name		= "gpio-syscon",
-		.owner		= THIS_MODULE,
 		.of_match_table	= syscon_gpio_ids,
 	},
 	.probe	= syscon_gpio_probe,
diff --git a/drivers/gpio/gpio-tb10x.c b/drivers/gpio/gpio-tb10x.c
index 9e615be8032c..62ab9f4b2cd3 100644
--- a/drivers/gpio/gpio-tb10x.c
+++ b/drivers/gpio/gpio-tb10x.c
@@ -195,18 +195,13 @@ static int tb10x_gpio_probe(struct platform_device *pdev)
 	if (of_property_read_u32(dn, "abilis,ngpio", &ngpio))
 		return -EINVAL;
 
-	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!mem) {
-		dev_err(&pdev->dev, "No memory resource defined.\n");
-		return -EINVAL;
-	}
-
 	tb10x_gpio = devm_kzalloc(&pdev->dev, sizeof(*tb10x_gpio), GFP_KERNEL);
 	if (tb10x_gpio == NULL)
 		return -ENOMEM;
 
 	spin_lock_init(&tb10x_gpio->spinlock);
 
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	tb10x_gpio->base = devm_ioremap_resource(&pdev->dev, mem);
 	if (IS_ERR(tb10x_gpio->base))
 		return PTR_ERR(tb10x_gpio->base);
@@ -316,7 +311,6 @@ static struct platform_driver tb10x_gpio_driver = {
 	.driver = {
 		.name	= "tb10x-gpio",
 		.of_match_table = tb10x_gpio_dt_ids,
-		.owner	= THIS_MODULE,
 	}
 };
 
diff --git a/drivers/gpio/gpio-tegra.c b/drivers/gpio/gpio-tegra.c
index 4e8fb8261a87..1741981d53c8 100644
--- a/drivers/gpio/gpio-tegra.c
+++ b/drivers/gpio/gpio-tegra.c
@@ -233,7 +233,7 @@ static int tegra_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 		return -EINVAL;
 	}
 
-	ret = gpio_lock_as_irq(&tegra_gpio_chip, gpio);
+	ret = gpiochip_lock_as_irq(&tegra_gpio_chip, gpio);
 	if (ret) {
 		dev_err(dev, "unable to lock Tegra GPIO %d as IRQ\n", gpio);
 		return ret;
@@ -263,7 +263,7 @@ static void tegra_gpio_irq_shutdown(struct irq_data *d)
 {
 	int gpio = d->hwirq;
 
-	gpio_unlock_as_irq(&tegra_gpio_chip, gpio);
+	gpiochip_unlock_as_irq(&tegra_gpio_chip, gpio);
 }
 
 static void tegra_gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
@@ -528,7 +528,6 @@ static int tegra_gpio_probe(struct platform_device *pdev)
 static struct platform_driver tegra_gpio_driver = {
 	.driver		= {
 		.name	= "tegra-gpio",
-		.owner	= THIS_MODULE,
 		.pm	= &tegra_gpio_pm_ops,
 		.of_match_table = tegra_gpio_of_match,
 	},
diff --git a/drivers/gpio/gpio-timberdale.c b/drivers/gpio/gpio-timberdale.c
index a685a3cbbc81..e8f97e03c9bb 100644
--- a/drivers/gpio/gpio-timberdale.c
+++ b/drivers/gpio/gpio-timberdale.c
@@ -330,7 +330,6 @@ static int timbgpio_remove(struct platform_device *pdev)
 static struct platform_driver timbgpio_platform_driver = {
 	.driver = {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= timbgpio_probe,
 	.remove		= timbgpio_remove,
diff --git a/drivers/gpio/gpio-tps65912.c b/drivers/gpio/gpio-tps65912.c
index 22052d84c63b..472fb5b8779f 100644
--- a/drivers/gpio/gpio-tps65912.c
+++ b/drivers/gpio/gpio-tps65912.c
@@ -124,7 +124,6 @@ static int tps65912_gpio_remove(struct platform_device *pdev)
 static struct platform_driver tps65912_gpio_driver = {
 	.driver = {
 		.name = "tps65912-gpio",
-		.owner = THIS_MODULE,
 	},
 	.probe = tps65912_gpio_probe,
 	.remove = tps65912_gpio_remove,
diff --git a/drivers/gpio/gpio-ts5500.c b/drivers/gpio/gpio-ts5500.c
index de18591ff11e..92fbabd82879 100644
--- a/drivers/gpio/gpio-ts5500.c
+++ b/drivers/gpio/gpio-ts5500.c
@@ -452,7 +452,6 @@ MODULE_DEVICE_TABLE(platform, ts5500_dio_ids);
 static struct platform_driver ts5500_dio_driver = {
 	.driver = {
 		.name = "ts5500-dio",
-		.owner = THIS_MODULE,
 	},
 	.probe = ts5500_dio_probe,
 	.remove = ts5500_dio_remove,
diff --git a/drivers/gpio/gpio-twl4030.c b/drivers/gpio/gpio-twl4030.c
index 118828b3736f..9e1dbb9877c1 100644
--- a/drivers/gpio/gpio-twl4030.c
+++ b/drivers/gpio/gpio-twl4030.c
@@ -605,7 +605,6 @@ MODULE_ALIAS("platform:twl4030_gpio");
 static struct platform_driver gpio_twl4030_driver = {
 	.driver = {
 		.name	= "twl4030_gpio",
-		.owner	= THIS_MODULE,
 		.of_match_table = twl_gpio_match,
 	},
 	.probe		= gpio_twl4030_probe,
diff --git a/drivers/gpio/gpio-twl6040.c b/drivers/gpio/gpio-twl6040.c
index f28e04b88aa9..c946e7eef3ee 100644
--- a/drivers/gpio/gpio-twl6040.c
+++ b/drivers/gpio/gpio-twl6040.c
@@ -121,7 +121,6 @@ MODULE_ALIAS("platform:twl6040-gpo");
 static struct platform_driver gpo_twl6040_driver = {
 	.driver = {
 		.name	= "twl6040-gpo",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= gpo_twl6040_probe,
 	.remove		= gpo_twl6040_remove,
diff --git a/drivers/gpio/gpio-tz1090-pdc.c b/drivers/gpio/gpio-tz1090-pdc.c
index f512da299b3d..d7536226b847 100644
--- a/drivers/gpio/gpio-tz1090-pdc.c
+++ b/drivers/gpio/gpio-tz1090-pdc.c
@@ -230,7 +230,6 @@ static struct of_device_id tz1090_pdc_gpio_of_match[] = {
 static struct platform_driver tz1090_pdc_gpio_driver = {
 	.driver = {
 		.name		= "tz1090-pdc-gpio",
-		.owner		= THIS_MODULE,
 		.of_match_table	= tz1090_pdc_gpio_of_match,
 	},
 	.probe		= tz1090_pdc_gpio_probe,
diff --git a/drivers/gpio/gpio-tz1090.c b/drivers/gpio/gpio-tz1090.c
index 6107d0682fd6..e3024bbba447 100644
--- a/drivers/gpio/gpio-tz1090.c
+++ b/drivers/gpio/gpio-tz1090.c
@@ -593,7 +593,6 @@ static struct of_device_id tz1090_gpio_of_match[] = {
 static struct platform_driver tz1090_gpio_driver = {
 	.driver = {
 		.name		= "tz1090-gpio",
-		.owner		= THIS_MODULE,
 		.of_match_table	= tz1090_gpio_of_match,
 	},
 	.probe		= tz1090_gpio_probe,
diff --git a/drivers/gpio/gpio-vf610.c b/drivers/gpio/gpio-vf610.c
new file mode 100644
index 000000000000..4ee4cee832ec
--- /dev/null
+++ b/drivers/gpio/gpio-vf610.c
@@ -0,0 +1,295 @@
+/*
+ * vf610 GPIO support through PORT and GPIO module
+ *
+ * Copyright (c) 2014 Toradex AG.
+ *
+ * Author: Stefan Agner <stefan@agner.ch>.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/bitops.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/irq.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_irq.h>
+
+#define VF610_GPIO_PER_PORT		32
+
+struct vf610_gpio_port {
+	struct gpio_chip gc;
+	void __iomem *base;
+	void __iomem *gpio_base;
+	u8 irqc[VF610_GPIO_PER_PORT];
+	int irq;
+};
+
+#define GPIO_PDOR		0x00
+#define GPIO_PSOR		0x04
+#define GPIO_PCOR		0x08
+#define GPIO_PTOR		0x0c
+#define GPIO_PDIR		0x10
+
+#define PORT_PCR(n)		((n) * 0x4)
+#define PORT_PCR_IRQC_OFFSET	16
+
+#define PORT_ISFR		0xa0
+#define PORT_DFER		0xc0
+#define PORT_DFCR		0xc4
+#define PORT_DFWR		0xc8
+
+#define PORT_INT_OFF		0x0
+#define PORT_INT_LOGIC_ZERO	0x8
+#define PORT_INT_RISING_EDGE	0x9
+#define PORT_INT_FALLING_EDGE	0xa
+#define PORT_INT_EITHER_EDGE	0xb
+#define PORT_INT_LOGIC_ONE	0xc
+
+static const struct of_device_id vf610_gpio_dt_ids[] = {
+	{ .compatible = "fsl,vf610-gpio" },
+	{ /* sentinel */ }
+};
+
+static inline void vf610_gpio_writel(u32 val, void __iomem *reg)
+{
+	writel_relaxed(val, reg);
+}
+
+static inline u32 vf610_gpio_readl(void __iomem *reg)
+{
+	return readl_relaxed(reg);
+}
+
+static int vf610_gpio_request(struct gpio_chip *chip, unsigned offset)
+{
+	return pinctrl_request_gpio(chip->base + offset);
+}
+
+static void vf610_gpio_free(struct gpio_chip *chip, unsigned offset)
+{
+	pinctrl_free_gpio(chip->base + offset);
+}
+
+static int vf610_gpio_get(struct gpio_chip *gc, unsigned int gpio)
+{
+	struct vf610_gpio_port *port =
+		container_of(gc, struct vf610_gpio_port, gc);
+
+	return !!(vf610_gpio_readl(port->gpio_base + GPIO_PDIR) & BIT(gpio));
+}
+
+static void vf610_gpio_set(struct gpio_chip *gc, unsigned int gpio, int val)
+{
+	struct vf610_gpio_port *port =
+		container_of(gc, struct vf610_gpio_port, gc);
+	unsigned long mask = BIT(gpio);
+
+	if (val)
+		vf610_gpio_writel(mask, port->gpio_base + GPIO_PSOR);
+	else
+		vf610_gpio_writel(mask, port->gpio_base + GPIO_PCOR);
+}
+
+static int vf610_gpio_direction_input(struct gpio_chip *chip, unsigned gpio)
+{
+	return pinctrl_gpio_direction_input(chip->base + gpio);
+}
+
+static int vf610_gpio_direction_output(struct gpio_chip *chip, unsigned gpio,
+				       int value)
+{
+	vf610_gpio_set(chip, gpio, value);
+
+	return pinctrl_gpio_direction_output(chip->base + gpio);
+}
+
+static void vf610_gpio_irq_handler(u32 irq, struct irq_desc *desc)
+{
+	struct vf610_gpio_port *port = irq_get_handler_data(irq);
+	struct irq_chip *chip = irq_desc_get_chip(desc);
+	int pin;
+	unsigned long irq_isfr;
+
+	chained_irq_enter(chip, desc);
+
+	irq_isfr = vf610_gpio_readl(port->base + PORT_ISFR);
+
+	for_each_set_bit(pin, &irq_isfr, VF610_GPIO_PER_PORT) {
+		vf610_gpio_writel(BIT(pin), port->base + PORT_ISFR);
+
+		generic_handle_irq(irq_find_mapping(port->gc.irqdomain, pin));
+	}
+
+	chained_irq_exit(chip, desc);
+}
+
+static void vf610_gpio_irq_ack(struct irq_data *d)
+{
+	struct vf610_gpio_port *port = irq_data_get_irq_chip_data(d);
+	int gpio = d->hwirq;
+
+	vf610_gpio_writel(BIT(gpio), port->base + PORT_ISFR);
+}
+
+static int vf610_gpio_irq_set_type(struct irq_data *d, u32 type)
+{
+	struct vf610_gpio_port *port = irq_data_get_irq_chip_data(d);
+	u8 irqc;
+
+	switch (type) {
+	case IRQ_TYPE_EDGE_RISING:
+		irqc = PORT_INT_RISING_EDGE;
+		break;
+	case IRQ_TYPE_EDGE_FALLING:
+		irqc = PORT_INT_FALLING_EDGE;
+		break;
+	case IRQ_TYPE_EDGE_BOTH:
+		irqc = PORT_INT_EITHER_EDGE;
+		break;
+	case IRQ_TYPE_LEVEL_LOW:
+		irqc = PORT_INT_LOGIC_ZERO;
+		break;
+	case IRQ_TYPE_LEVEL_HIGH:
+		irqc = PORT_INT_LOGIC_ONE;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	port->irqc[d->hwirq] = irqc;
+
+	return 0;
+}
+
+static void vf610_gpio_irq_mask(struct irq_data *d)
+{
+	struct vf610_gpio_port *port = irq_data_get_irq_chip_data(d);
+	void __iomem *pcr_base = port->base + PORT_PCR(d->hwirq);
+
+	vf610_gpio_writel(0, pcr_base);
+}
+
+static void vf610_gpio_irq_unmask(struct irq_data *d)
+{
+	struct vf610_gpio_port *port = irq_data_get_irq_chip_data(d);
+	void __iomem *pcr_base = port->base + PORT_PCR(d->hwirq);
+
+	vf610_gpio_writel(port->irqc[d->hwirq] << PORT_PCR_IRQC_OFFSET,
+			  pcr_base);
+}
+
+static int vf610_gpio_irq_set_wake(struct irq_data *d, u32 enable)
+{
+	struct vf610_gpio_port *port = irq_data_get_irq_chip_data(d);
+
+	if (enable)
+		enable_irq_wake(port->irq);
+	else
+		disable_irq_wake(port->irq);
+
+	return 0;
+}
+
+static struct irq_chip vf610_gpio_irq_chip = {
+	.name		= "gpio-vf610",
+	.irq_ack	= vf610_gpio_irq_ack,
+	.irq_mask	= vf610_gpio_irq_mask,
+	.irq_unmask	= vf610_gpio_irq_unmask,
+	.irq_set_type	= vf610_gpio_irq_set_type,
+	.irq_set_wake	= vf610_gpio_irq_set_wake,
+};
+
+static int vf610_gpio_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct vf610_gpio_port *port;
+	struct resource *iores;
+	struct gpio_chip *gc;
+	int ret;
+
+	port = devm_kzalloc(&pdev->dev, sizeof(*port), GFP_KERNEL);
+	if (!port)
+		return -ENOMEM;
+
+	iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	port->base = devm_ioremap_resource(dev, iores);
+	if (IS_ERR(port->base))
+		return PTR_ERR(port->base);
+
+	iores = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	port->gpio_base = devm_ioremap_resource(dev, iores);
+	if (IS_ERR(port->gpio_base))
+		return PTR_ERR(port->gpio_base);
+
+	port->irq = platform_get_irq(pdev, 0);
+	if (port->irq < 0)
+		return port->irq;
+
+	gc = &port->gc;
+	gc->of_node = np;
+	gc->dev = dev;
+	gc->label = "vf610-gpio",
+	gc->ngpio = VF610_GPIO_PER_PORT,
+	gc->base = of_alias_get_id(np, "gpio") * VF610_GPIO_PER_PORT;
+
+	gc->request = vf610_gpio_request,
+	gc->free = vf610_gpio_free,
+	gc->direction_input = vf610_gpio_direction_input,
+	gc->get = vf610_gpio_get,
+	gc->direction_output = vf610_gpio_direction_output,
+	gc->set = vf610_gpio_set,
+
+	ret = gpiochip_add(gc);
+	if (ret < 0)
+		return ret;
+
+	/* Clear the interrupt status register for all GPIO's */
+	vf610_gpio_writel(~0, port->base + PORT_ISFR);
+
+	ret = gpiochip_irqchip_add(gc, &vf610_gpio_irq_chip, 0,
+				   handle_simple_irq, IRQ_TYPE_NONE);
+	if (ret) {
+		dev_err(dev, "failed to add irqchip\n");
+		gpiochip_remove(gc);
+		return ret;
+	}
+	gpiochip_set_chained_irqchip(gc, &vf610_gpio_irq_chip, port->irq,
+				     vf610_gpio_irq_handler);
+
+	return 0;
+}
+
+static struct platform_driver vf610_gpio_driver = {
+	.driver		= {
+		.name	= "gpio-vf610",
+		.owner	= THIS_MODULE,
+		.of_match_table = vf610_gpio_dt_ids,
+	},
+	.probe		= vf610_gpio_probe,
+};
+
+static int __init gpio_vf610_init(void)
+{
+	return platform_driver_register(&vf610_gpio_driver);
+}
+device_initcall(gpio_vf610_init);
+
+MODULE_AUTHOR("Stefan Agner <stefan@agner.ch>");
+MODULE_DESCRIPTION("Freescale VF610 GPIO");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/gpio/gpio-vr41xx.c b/drivers/gpio/gpio-vr41xx.c
index dbf28fa03f67..c1caa459c02d 100644
--- a/drivers/gpio/gpio-vr41xx.c
+++ b/drivers/gpio/gpio-vr41xx.c
@@ -138,7 +138,7 @@ static void unmask_giuint_low(struct irq_data *d)
 
 static unsigned int startup_giuint(struct irq_data *data)
 {
-	if (gpio_lock_as_irq(&vr41xx_gpio_chip, data->hwirq))
+	if (gpiochip_lock_as_irq(&vr41xx_gpio_chip, data->hwirq))
 		dev_err(vr41xx_gpio_chip.dev,
 			"unable to lock HW IRQ %lu for IRQ\n",
 			data->hwirq);
@@ -150,7 +150,7 @@ static unsigned int startup_giuint(struct irq_data *data)
 static void shutdown_giuint(struct irq_data *data)
 {
 	mask_giuint_low(data);
-	gpio_unlock_as_irq(&vr41xx_gpio_chip, data->hwirq);
+	gpiochip_unlock_as_irq(&vr41xx_gpio_chip, data->hwirq);
 }
 
 static struct irq_chip giuint_low_irq_chip = {
@@ -591,7 +591,6 @@ static struct platform_driver giu_device_driver = {
 	.remove		= giu_remove,
 	.driver		= {
 		.name	= "GIU",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/gpio/gpio-vx855.c b/drivers/gpio/gpio-vx855.c
index 85971d4e23c1..9d21d2fcc327 100644
--- a/drivers/gpio/gpio-vx855.c
+++ b/drivers/gpio/gpio-vx855.c
@@ -306,7 +306,6 @@ static int vx855gpio_remove(struct platform_device *pdev)
 static struct platform_driver vx855gpio_driver = {
 	.driver = {
 		.name	= MODULE_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= vx855gpio_probe,
 	.remove		= vx855gpio_remove,
diff --git a/drivers/gpio/gpio-xgene.c b/drivers/gpio/gpio-xgene.c
index 7d489221dc1f..18a8182d4fec 100644
--- a/drivers/gpio/gpio-xgene.c
+++ b/drivers/gpio/gpio-xgene.c
@@ -229,7 +229,6 @@ MODULE_DEVICE_TABLE(of, xgene_gpio_of_match);
 static struct platform_driver xgene_gpio_driver = {
 	.driver = {
 		.name = "xgene-gpio",
-		.owner = THIS_MODULE,
 		.of_match_table = xgene_gpio_of_match,
 		.pm     = XGENE_GPIO_PM_OPS,
 	},
diff --git a/drivers/gpio/gpio-xtensa.c b/drivers/gpio/gpio-xtensa.c
index 7081304d6797..93ec95df67a3 100644
--- a/drivers/gpio/gpio-xtensa.c
+++ b/drivers/gpio/gpio-xtensa.c
@@ -157,7 +157,6 @@ static int xtensa_gpio_probe(struct platform_device *pdev)
 static struct platform_driver xtensa_gpio_driver = {
 	.driver		= {
 		.name		= "xtensa-gpio",
-		.owner		= THIS_MODULE,
 	},
 	.probe		= xtensa_gpio_probe,
 };
diff --git a/drivers/gpio/gpio-zevio.c b/drivers/gpio/gpio-zevio.c
index 54e54e4cc6c4..f769cd53f4e4 100644
--- a/drivers/gpio/gpio-zevio.c
+++ b/drivers/gpio/gpio-zevio.c
@@ -212,7 +212,6 @@ MODULE_DEVICE_TABLE(of, zevio_gpio_of_match);
 static struct platform_driver zevio_gpio_driver = {
 	.driver		= {
 		.name	= "gpio-zevio",
-		.owner	= THIS_MODULE,
 		.of_match_table = zevio_gpio_of_match,
 	},
 	.probe		= zevio_gpio_probe,
diff --git a/drivers/gpio/gpiolib-acpi.c b/drivers/gpio/gpiolib-acpi.c
index c3bdaff71c25..c0929d938ced 100644
--- a/drivers/gpio/gpiolib-acpi.c
+++ b/drivers/gpio/gpiolib-acpi.c
@@ -209,7 +209,7 @@ static acpi_status acpi_gpiochip_request_interrupt(struct acpi_resource *ares,
 
 	gpiod_direction_input(desc);
 
-	ret = gpio_lock_as_irq(chip, pin);
+	ret = gpiochip_lock_as_irq(chip, pin);
 	if (ret) {
 		dev_err(chip->dev, "Failed to lock GPIO as interrupt\n");
 		goto fail_free_desc;
@@ -265,7 +265,7 @@ static acpi_status acpi_gpiochip_request_interrupt(struct acpi_resource *ares,
 fail_free_event:
 	kfree(event);
 fail_unlock_irq:
-	gpio_unlock_as_irq(chip, pin);
+	gpiochip_unlock_as_irq(chip, pin);
 fail_free_desc:
 	gpiochip_free_own_desc(desc);
 
@@ -336,7 +336,7 @@ void acpi_gpiochip_free_interrupts(struct gpio_chip *chip)
 		desc = event->desc;
 		if (WARN_ON(IS_ERR(desc)))
 			continue;
-		gpio_unlock_as_irq(chip, event->pin);
+		gpiochip_unlock_as_irq(chip, event->pin);
 		gpiochip_free_own_desc(desc);
 		list_del(&event->node);
 		kfree(event);
diff --git a/drivers/gpio/gpiolib-legacy.c b/drivers/gpio/gpiolib-legacy.c
index 078ae6c2df79..8b830996fe02 100644
--- a/drivers/gpio/gpiolib-legacy.c
+++ b/drivers/gpio/gpiolib-legacy.c
@@ -24,6 +24,10 @@ int gpio_request_one(unsigned gpio, unsigned long flags, const char *label)
 
 	desc = gpio_to_desc(gpio);
 
+	/* Compatibility: assume unavailable "valid" GPIOs will appear later */
+	if (!desc && gpio_is_valid(gpio))
+		return -EPROBE_DEFER;
+
 	err = gpiod_request(desc, label);
 	if (err)
 		return err;
@@ -62,7 +66,13 @@ EXPORT_SYMBOL_GPL(gpio_request_one);
 
 int gpio_request(unsigned gpio, const char *label)
 {
-	return gpiod_request(gpio_to_desc(gpio), label);
+	struct gpio_desc *desc = gpio_to_desc(gpio);
+
+	/* Compatibility: assume unavailable "valid" GPIOs will appear later */
+	if (!desc && gpio_is_valid(gpio))
+		return -EPROBE_DEFER;
+
+	return gpiod_request(desc, label);
 }
 EXPORT_SYMBOL_GPL(gpio_request);
 
diff --git a/drivers/gpio/gpiolib-sysfs.c b/drivers/gpio/gpiolib-sysfs.c
index 5f2150b619a7..2ac1800b58bb 100644
--- a/drivers/gpio/gpiolib-sysfs.c
+++ b/drivers/gpio/gpiolib-sysfs.c
@@ -41,7 +41,7 @@ static DEFINE_MUTEX(sysfs_lock);
 static ssize_t gpio_direction_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
-	const struct gpio_desc	*desc = dev_get_drvdata(dev);
+	struct gpio_desc	*desc = dev_get_drvdata(dev);
 	ssize_t			status;
 
 	mutex_lock(&sysfs_lock);
@@ -161,7 +161,7 @@ static int gpio_setup_irq(struct gpio_desc *desc, struct device *dev,
 	desc->flags &= ~GPIO_TRIGGER_MASK;
 
 	if (!gpio_flags) {
-		gpio_unlock_as_irq(desc->chip, gpio_chip_hwgpio(desc));
+		gpiochip_unlock_as_irq(desc->chip, gpio_chip_hwgpio(desc));
 		ret = 0;
 		goto free_id;
 	}
@@ -200,7 +200,7 @@ static int gpio_setup_irq(struct gpio_desc *desc, struct device *dev,
 	if (ret < 0)
 		goto free_id;
 
-	ret = gpio_lock_as_irq(desc->chip, gpio_chip_hwgpio(desc));
+	ret = gpiochip_lock_as_irq(desc->chip, gpio_chip_hwgpio(desc));
 	if (ret < 0) {
 		gpiod_warn(desc, "failed to flag the GPIO for IRQ\n");
 		goto free_id;
diff --git a/drivers/gpio/gpiolib.c b/drivers/gpio/gpiolib.c
index 58659dbe702a..487afe6f22fc 100644
--- a/drivers/gpio/gpiolib.c
+++ b/drivers/gpio/gpiolib.c
@@ -47,8 +47,6 @@
  */
 DEFINE_SPINLOCK(gpio_lock);
 
-static struct gpio_desc gpio_desc[ARCH_NR_GPIOS];
-
 #define GPIO_OFFSET_VALID(chip, offset) (offset >= 0 && offset < chip->ngpio)
 
 static DEFINE_MUTEX(gpio_lookup_lock);
@@ -65,10 +63,24 @@ static inline void desc_set_label(struct gpio_desc *d, const char *label)
  */
 struct gpio_desc *gpio_to_desc(unsigned gpio)
 {
-	if (WARN(!gpio_is_valid(gpio), "invalid GPIO %d\n", gpio))
-		return NULL;
-	else
-		return &gpio_desc[gpio];
+	struct gpio_chip *chip;
+	unsigned long flags;
+
+	spin_lock_irqsave(&gpio_lock, flags);
+
+	list_for_each_entry(chip, &gpio_chips, list) {
+		if (chip->base <= gpio && chip->base + chip->ngpio > gpio) {
+			spin_unlock_irqrestore(&gpio_lock, flags);
+			return &chip->desc[gpio - chip->base];
+		}
+	}
+
+	spin_unlock_irqrestore(&gpio_lock, flags);
+
+	if (!gpio_is_valid(gpio))
+		WARN(1, "invalid GPIO %d\n", gpio);
+
+	return NULL;
 }
 EXPORT_SYMBOL_GPL(gpio_to_desc);
 
@@ -91,7 +103,7 @@ struct gpio_desc *gpiochip_get_desc(struct gpio_chip *chip,
  */
 int desc_to_gpio(const struct gpio_desc *desc)
 {
-	return desc - &gpio_desc[0];
+	return desc->chip->base + (desc - &desc->chip->desc[0]);
 }
 EXPORT_SYMBOL_GPL(desc_to_gpio);
 
@@ -138,7 +150,7 @@ static int gpiochip_find_base(int ngpio)
  *
  * This function may sleep if gpiod_cansleep() is true.
  */
-int gpiod_get_direction(const struct gpio_desc *desc)
+int gpiod_get_direction(struct gpio_desc *desc)
 {
 	struct gpio_chip	*chip;
 	unsigned		offset;
@@ -154,13 +166,11 @@ int gpiod_get_direction(const struct gpio_desc *desc)
 	if (status > 0) {
 		/* GPIOF_DIR_IN, or other positive */
 		status = 1;
-		/* FLAG_IS_OUT is just a cache of the result of get_direction(),
-		 * so it does not affect constness per se */
-		clear_bit(FLAG_IS_OUT, &((struct gpio_desc *)desc)->flags);
+		clear_bit(FLAG_IS_OUT, &desc->flags);
 	}
 	if (status == 0) {
 		/* GPIOF_DIR_OUT */
-		set_bit(FLAG_IS_OUT, &((struct gpio_desc *)desc)->flags);
+		set_bit(FLAG_IS_OUT, &desc->flags);
 	}
 	return status;
 }
@@ -206,7 +216,7 @@ static int gpiochip_add_to_list(struct gpio_chip *chip)
 /**
  * gpiochip_add() - register a gpio_chip
  * @chip: the chip to register, with chip->base initialized
- * Context: potentially before irqs or kmalloc will work
+ * Context: potentially before irqs will work
  *
  * Returns a negative errno if the chip can't be registered, such as
  * because the chip->base is invalid or already associated with a
@@ -226,12 +236,11 @@ int gpiochip_add(struct gpio_chip *chip)
 	int		status = 0;
 	unsigned	id;
 	int		base = chip->base;
+	struct gpio_desc *descs;
 
-	if ((!gpio_is_valid(base) || !gpio_is_valid(base + chip->ngpio - 1))
-			&& base >= 0) {
-		status = -EINVAL;
-		goto fail;
-	}
+	descs = kcalloc(chip->ngpio, sizeof(descs[0]), GFP_KERNEL);
+	if (!descs)
+		return -ENOMEM;
 
 	spin_lock_irqsave(&gpio_lock, flags);
 
@@ -247,10 +256,8 @@ int gpiochip_add(struct gpio_chip *chip)
 	status = gpiochip_add_to_list(chip);
 
 	if (status == 0) {
-		chip->desc = &gpio_desc[chip->base];
-
 		for (id = 0; id < chip->ngpio; id++) {
-			struct gpio_desc *desc = &chip->desc[id];
+			struct gpio_desc *desc = &descs[id];
 			desc->chip = chip;
 
 			/* REVISIT:  most hardware initializes GPIOs as
@@ -266,6 +273,8 @@ int gpiochip_add(struct gpio_chip *chip)
 		}
 	}
 
+	chip->desc = descs;
+
 	spin_unlock_irqrestore(&gpio_lock, flags);
 
 #ifdef CONFIG_PINCTRL
@@ -291,6 +300,9 @@ int gpiochip_add(struct gpio_chip *chip)
 unlock:
 	spin_unlock_irqrestore(&gpio_lock, flags);
 fail:
+	kfree(descs);
+	chip->desc = NULL;
+
 	/* failures here can mean systems won't boot... */
 	pr_err("%s: GPIOs %d..%d (%s) failed to register\n", __func__,
 		chip->base, chip->base + chip->ngpio - 1,
@@ -331,6 +343,9 @@ void gpiochip_remove(struct gpio_chip *chip)
 	list_del(&chip->list);
 	spin_unlock_irqrestore(&gpio_lock, flags);
 	gpiochip_unexport(chip);
+
+	kfree(chip->desc);
+	chip->desc = NULL;
 }
 EXPORT_SYMBOL_GPL(gpiochip_remove);
 
@@ -495,7 +510,7 @@ static int gpiochip_irq_reqres(struct irq_data *d)
 {
 	struct gpio_chip *chip = irq_data_get_irq_chip_data(d);
 
-	if (gpio_lock_as_irq(chip, d->hwirq)) {
+	if (gpiochip_lock_as_irq(chip, d->hwirq)) {
 		chip_err(chip,
 			"unable to lock HW IRQ %lu for IRQ\n",
 			d->hwirq);
@@ -508,7 +523,7 @@ static void gpiochip_irq_relres(struct irq_data *d)
 {
 	struct gpio_chip *chip = irq_data_get_irq_chip_data(d);
 
-	gpio_unlock_as_irq(chip, d->hwirq);
+	gpiochip_unlock_as_irq(chip, d->hwirq);
 }
 
 static int gpiochip_to_irq(struct gpio_chip *chip, unsigned offset)
@@ -1254,6 +1269,88 @@ static void _gpiod_set_raw_value(struct gpio_desc *desc, bool value)
 		chip->set(chip, gpio_chip_hwgpio(desc), value);
 }
 
+/*
+ * set multiple outputs on the same chip;
+ * use the chip's set_multiple function if available;
+ * otherwise set the outputs sequentially;
+ * @mask: bit mask array; one bit per output; BITS_PER_LONG bits per word
+ *        defines which outputs are to be changed
+ * @bits: bit value array; one bit per output; BITS_PER_LONG bits per word
+ *        defines the values the outputs specified by mask are to be set to
+ */
+static void gpio_chip_set_multiple(struct gpio_chip *chip,
+				   unsigned long *mask, unsigned long *bits)
+{
+	if (chip->set_multiple) {
+		chip->set_multiple(chip, mask, bits);
+	} else {
+		int i;
+		for (i = 0; i < chip->ngpio; i++) {
+			if (mask[BIT_WORD(i)] == 0) {
+				/* no more set bits in this mask word;
+				 * skip ahead to the next word */
+				i = (BIT_WORD(i) + 1) * BITS_PER_LONG - 1;
+				continue;
+			}
+			/* set outputs if the corresponding mask bit is set */
+			if (__test_and_clear_bit(i, mask)) {
+				chip->set(chip, i, test_bit(i, bits));
+			}
+		}
+	}
+}
+
+static void gpiod_set_array_priv(bool raw, bool can_sleep,
+				 unsigned int array_size,
+				 struct gpio_desc **desc_array,
+				 int *value_array)
+{
+	int i = 0;
+
+	while (i < array_size) {
+		struct gpio_chip *chip = desc_array[i]->chip;
+		unsigned long mask[BITS_TO_LONGS(chip->ngpio)];
+		unsigned long bits[BITS_TO_LONGS(chip->ngpio)];
+		int count = 0;
+
+		if (!can_sleep) {
+			WARN_ON(chip->can_sleep);
+		}
+		memset(mask, 0, sizeof(mask));
+		do {
+			struct gpio_desc *desc = desc_array[i];
+			int hwgpio = gpio_chip_hwgpio(desc);
+			int value = value_array[i];
+
+			if (!raw && test_bit(FLAG_ACTIVE_LOW, &desc->flags))
+				value = !value;
+			trace_gpio_value(desc_to_gpio(desc), 0, value);
+			/*
+			 * collect all normal outputs belonging to the same chip
+			 * open drain and open source outputs are set individually
+			 */
+			if (test_bit(FLAG_OPEN_DRAIN, &desc->flags)) {
+				_gpio_set_open_drain_value(desc,value);
+			} else if (test_bit(FLAG_OPEN_SOURCE, &desc->flags)) {
+				_gpio_set_open_source_value(desc, value);
+			} else {
+				__set_bit(hwgpio, mask);
+				if (value) {
+					__set_bit(hwgpio, bits);
+				} else {
+					__clear_bit(hwgpio, bits);
+				}
+				count++;
+			}
+			i++;
+		} while ((i < array_size) && (desc_array[i]->chip == chip));
+		/* push collected bits to outputs */
+		if (count != 0) {
+			gpio_chip_set_multiple(chip, mask, bits);
+		}
+	}
+}
+
 /**
  * gpiod_set_raw_value() - assign a gpio's raw value
  * @desc: gpio whose value will be assigned
@@ -1299,6 +1396,48 @@ void gpiod_set_value(struct gpio_desc *desc, int value)
 EXPORT_SYMBOL_GPL(gpiod_set_value);
 
 /**
+ * gpiod_set_raw_array() - assign values to an array of GPIOs
+ * @array_size: number of elements in the descriptor / value arrays
+ * @desc_array: array of GPIO descriptors whose values will be assigned
+ * @value_array: array of values to assign
+ *
+ * Set the raw values of the GPIOs, i.e. the values of the physical lines
+ * without regard for their ACTIVE_LOW status.
+ *
+ * This function should be called from contexts where we cannot sleep, and will
+ * complain if the GPIO chip functions potentially sleep.
+ */
+void gpiod_set_raw_array(unsigned int array_size,
+			 struct gpio_desc **desc_array, int *value_array)
+{
+	if (!desc_array)
+		return;
+	gpiod_set_array_priv(true, false, array_size, desc_array, value_array);
+}
+EXPORT_SYMBOL_GPL(gpiod_set_raw_array);
+
+/**
+ * gpiod_set_array() - assign values to an array of GPIOs
+ * @array_size: number of elements in the descriptor / value arrays
+ * @desc_array: array of GPIO descriptors whose values will be assigned
+ * @value_array: array of values to assign
+ *
+ * Set the logical values of the GPIOs, i.e. taking their ACTIVE_LOW status
+ * into account.
+ *
+ * This function should be called from contexts where we cannot sleep, and will
+ * complain if the GPIO chip functions potentially sleep.
+ */
+void gpiod_set_array(unsigned int array_size,
+		     struct gpio_desc **desc_array, int *value_array)
+{
+	if (!desc_array)
+		return;
+	gpiod_set_array_priv(false, false, array_size, desc_array, value_array);
+}
+EXPORT_SYMBOL_GPL(gpiod_set_array);
+
+/**
  * gpiod_cansleep() - report whether gpio value access may sleep
  * @desc: gpio to check
  *
@@ -1332,14 +1471,14 @@ int gpiod_to_irq(const struct gpio_desc *desc)
 EXPORT_SYMBOL_GPL(gpiod_to_irq);
 
 /**
- * gpio_lock_as_irq() - lock a GPIO to be used as IRQ
+ * gpiochip_lock_as_irq() - lock a GPIO to be used as IRQ
  * @chip: the chip the GPIO to lock belongs to
  * @offset: the offset of the GPIO to lock as IRQ
  *
  * This is used directly by GPIO drivers that want to lock down
  * a certain GPIO line to be used for IRQs.
  */
-int gpio_lock_as_irq(struct gpio_chip *chip, unsigned int offset)
+int gpiochip_lock_as_irq(struct gpio_chip *chip, unsigned int offset)
 {
 	if (offset >= chip->ngpio)
 		return -EINVAL;
@@ -1354,24 +1493,24 @@ int gpio_lock_as_irq(struct gpio_chip *chip, unsigned int offset)
 	set_bit(FLAG_USED_AS_IRQ, &chip->desc[offset].flags);
 	return 0;
 }
-EXPORT_SYMBOL_GPL(gpio_lock_as_irq);
+EXPORT_SYMBOL_GPL(gpiochip_lock_as_irq);
 
 /**
- * gpio_unlock_as_irq() - unlock a GPIO used as IRQ
+ * gpiochip_unlock_as_irq() - unlock a GPIO used as IRQ
  * @chip: the chip the GPIO to lock belongs to
  * @offset: the offset of the GPIO to lock as IRQ
  *
  * This is used directly by GPIO drivers that want to indicate
  * that a certain GPIO is no longer used exclusively for IRQ.
  */
-void gpio_unlock_as_irq(struct gpio_chip *chip, unsigned int offset)
+void gpiochip_unlock_as_irq(struct gpio_chip *chip, unsigned int offset)
 {
 	if (offset >= chip->ngpio)
 		return;
 
 	clear_bit(FLAG_USED_AS_IRQ, &chip->desc[offset].flags);
 }
-EXPORT_SYMBOL_GPL(gpio_unlock_as_irq);
+EXPORT_SYMBOL_GPL(gpiochip_unlock_as_irq);
 
 /**
  * gpiod_get_raw_value_cansleep() - return a gpio's raw value
@@ -1458,6 +1597,50 @@ void gpiod_set_value_cansleep(struct gpio_desc *desc, int value)
 EXPORT_SYMBOL_GPL(gpiod_set_value_cansleep);
 
 /**
+ * gpiod_set_raw_array_cansleep() - assign values to an array of GPIOs
+ * @array_size: number of elements in the descriptor / value arrays
+ * @desc_array: array of GPIO descriptors whose values will be assigned
+ * @value_array: array of values to assign
+ *
+ * Set the raw values of the GPIOs, i.e. the values of the physical lines
+ * without regard for their ACTIVE_LOW status.
+ *
+ * This function is to be called from contexts that can sleep.
+ */
+void gpiod_set_raw_array_cansleep(unsigned int array_size,
+				  struct gpio_desc **desc_array,
+				  int *value_array)
+{
+	might_sleep_if(extra_checks);
+	if (!desc_array)
+		return;
+	gpiod_set_array_priv(true, true, array_size, desc_array, value_array);
+}
+EXPORT_SYMBOL_GPL(gpiod_set_raw_array_cansleep);
+
+/**
+ * gpiod_set_array_cansleep() - assign values to an array of GPIOs
+ * @array_size: number of elements in the descriptor / value arrays
+ * @desc_array: array of GPIO descriptors whose values will be assigned
+ * @value_array: array of values to assign
+ *
+ * Set the logical values of the GPIOs, i.e. taking their ACTIVE_LOW status
+ * into account.
+ *
+ * This function is to be called from contexts that can sleep.
+ */
+void gpiod_set_array_cansleep(unsigned int array_size,
+			      struct gpio_desc **desc_array,
+			      int *value_array)
+{
+	might_sleep_if(extra_checks);
+	if (!desc_array)
+		return;
+	gpiod_set_array_priv(false, true, array_size, desc_array, value_array);
+}
+EXPORT_SYMBOL_GPL(gpiod_set_array_cansleep);
+
+/**
  * gpiod_add_lookup_table() - register GPIO device consumers
  * @table: table of consumers to register
  */
diff --git a/drivers/gpu/drm/Kconfig b/drivers/gpu/drm/Kconfig
index e3b4b0f02b3d..c3413b6adb17 100644
--- a/drivers/gpu/drm/Kconfig
+++ b/drivers/gpu/drm/Kconfig
@@ -167,6 +167,8 @@ config DRM_SAVAGE
 
 source "drivers/gpu/drm/exynos/Kconfig"
 
+source "drivers/gpu/drm/rockchip/Kconfig"
+
 source "drivers/gpu/drm/vmwgfx/Kconfig"
 
 source "drivers/gpu/drm/gma500/Kconfig"
@@ -200,3 +202,7 @@ source "drivers/gpu/drm/tegra/Kconfig"
 source "drivers/gpu/drm/panel/Kconfig"
 
 source "drivers/gpu/drm/sti/Kconfig"
+
+source "drivers/gpu/drm/amd/amdkfd/Kconfig"
+
+source "drivers/gpu/drm/imx/Kconfig"
diff --git a/drivers/gpu/drm/Makefile b/drivers/gpu/drm/Makefile
index 9292a761ea6d..66e40398b3d3 100644
--- a/drivers/gpu/drm/Makefile
+++ b/drivers/gpu/drm/Makefile
@@ -14,7 +14,7 @@ drm-y       :=	drm_auth.o drm_bufs.o drm_cache.o \
 		drm_info.o drm_debugfs.o drm_encoder_slave.o \
 		drm_trace_points.o drm_global.o drm_prime.o \
 		drm_rect.o drm_vma_manager.o drm_flip_work.o \
-		drm_modeset_lock.o
+		drm_modeset_lock.o drm_atomic.o
 
 drm-$(CONFIG_COMPAT) += drm_ioc32.o
 drm-$(CONFIG_DRM_GEM_CMA_HELPER) += drm_gem_cma_helper.o
@@ -23,7 +23,7 @@ drm-$(CONFIG_DRM_PANEL) += drm_panel.o
 drm-$(CONFIG_OF) += drm_of.o
 
 drm_kms_helper-y := drm_crtc_helper.o drm_dp_helper.o drm_probe_helper.o \
-		drm_plane_helper.o drm_dp_mst_topology.o
+		drm_plane_helper.o drm_dp_mst_topology.o drm_atomic_helper.o
 drm_kms_helper-$(CONFIG_DRM_LOAD_EDID_FIRMWARE) += drm_edid_load.o
 drm_kms_helper-$(CONFIG_DRM_KMS_FB_HELPER) += drm_fb_helper.o
 drm_kms_helper-$(CONFIG_DRM_KMS_CMA_HELPER) += drm_fb_cma_helper.o
@@ -49,6 +49,7 @@ obj-$(CONFIG_DRM_VMWGFX)+= vmwgfx/
 obj-$(CONFIG_DRM_VIA)	+=via/
 obj-$(CONFIG_DRM_NOUVEAU) +=nouveau/
 obj-$(CONFIG_DRM_EXYNOS) +=exynos/
+obj-$(CONFIG_DRM_ROCKCHIP) +=rockchip/
 obj-$(CONFIG_DRM_GMA500) += gma500/
 obj-$(CONFIG_DRM_UDL) += udl/
 obj-$(CONFIG_DRM_AST) += ast/
@@ -62,6 +63,8 @@ obj-$(CONFIG_DRM_BOCHS) += bochs/
 obj-$(CONFIG_DRM_MSM) += msm/
 obj-$(CONFIG_DRM_TEGRA) += tegra/
 obj-$(CONFIG_DRM_STI) += sti/
+obj-$(CONFIG_DRM_IMX) += imx/
 obj-y			+= i2c/
 obj-y			+= panel/
 obj-y			+= bridge/
+obj-$(CONFIG_HSA_AMD) += amd/amdkfd/
diff --git a/drivers/gpu/drm/README.drm b/drivers/gpu/drm/README.drm
deleted file mode 100644
index b5b332722581..000000000000
--- a/drivers/gpu/drm/README.drm
+++ /dev/null
@@ -1,43 +0,0 @@
-************************************************************
-* For the very latest on DRI development, please see:      *
-*     http://dri.freedesktop.org/                          *
-************************************************************
-
-The Direct Rendering Manager (drm) is a device-independent kernel-level
-device driver that provides support for the XFree86 Direct Rendering
-Infrastructure (DRI).
-
-The DRM supports the Direct Rendering Infrastructure (DRI) in four major
-ways:
-
-    1. The DRM provides synchronized access to the graphics hardware via
-       the use of an optimized two-tiered lock.
-
-    2. The DRM enforces the DRI security policy for access to the graphics
-       hardware by only allowing authenticated X11 clients access to
-       restricted regions of memory.
-
-    3. The DRM provides a generic DMA engine, complete with multiple
-       queues and the ability to detect the need for an OpenGL context
-       switch.
-
-    4. The DRM is extensible via the use of small device-specific modules
-       that rely extensively on the API exported by the DRM module.
-
-
-Documentation on the DRI is available from:
-    http://dri.freedesktop.org/wiki/Documentation
-    http://sourceforge.net/project/showfiles.php?group_id=387
-    http://dri.sourceforge.net/doc/
-
-For specific information about kernel-level support, see:
-
-    The Direct Rendering Manager, Kernel Support for the Direct Rendering
-    Infrastructure
-    http://dri.sourceforge.net/doc/drm_low_level.html
-
-    Hardware Locking for the Direct Rendering Infrastructure
-    http://dri.sourceforge.net/doc/hardware_locking_low_level.html
-
-    A Security Analysis of the Direct Rendering Infrastructure
-    http://dri.sourceforge.net/doc/security_low_level.html
diff --git a/drivers/gpu/drm/amd/amdkfd/Kconfig b/drivers/gpu/drm/amd/amdkfd/Kconfig
new file mode 100644
index 000000000000..8dfac37ff327
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/Kconfig
@@ -0,0 +1,9 @@
+#
+# Heterogenous system architecture configuration
+#
+
+config HSA_AMD
+	tristate "HSA kernel driver for AMD GPU devices"
+	depends on DRM_RADEON && AMD_IOMMU_V2 && X86_64
+	help
+	  Enable this if you want to use HSA features on AMD GPU devices.
diff --git a/drivers/gpu/drm/amd/amdkfd/Makefile b/drivers/gpu/drm/amd/amdkfd/Makefile
new file mode 100644
index 000000000000..be6246de5091
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/Makefile
@@ -0,0 +1,14 @@
+#
+# Makefile for Heterogenous System Architecture support for AMD GPU devices
+#
+
+ccflags-y := -Iinclude/drm -Idrivers/gpu/drm/amd/include/
+
+amdkfd-y	:= kfd_module.o kfd_device.o kfd_chardev.o kfd_topology.o \
+		kfd_pasid.o kfd_doorbell.o kfd_flat_memory.o \
+		kfd_process.o kfd_queue.o kfd_mqd_manager.o \
+		kfd_kernel_queue.o kfd_packet_manager.o \
+		kfd_process_queue_manager.o kfd_device_queue_manager.o \
+		kfd_interrupt.o
+
+obj-$(CONFIG_HSA_AMD)	+= amdkfd.o
diff --git a/drivers/gpu/drm/amd/amdkfd/cik_regs.h b/drivers/gpu/drm/amd/amdkfd/cik_regs.h
new file mode 100644
index 000000000000..607fc5ceadbe
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/cik_regs.h
@@ -0,0 +1,221 @@
+/*
+ * Copyright 2014 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.
+ */
+
+#ifndef CIK_REGS_H
+#define CIK_REGS_H
+
+#define IH_VMID_0_LUT					0x3D40u
+
+#define BIF_DOORBELL_CNTL				0x530Cu
+
+#define	SRBM_GFX_CNTL					0xE44
+#define	PIPEID(x)					((x) << 0)
+#define	MEID(x)						((x) << 2)
+#define	VMID(x)						((x) << 4)
+#define	QUEUEID(x)					((x) << 8)
+
+#define	SQ_CONFIG					0x8C00
+
+#define	SH_MEM_BASES					0x8C28
+/* if PTR32, these are the bases for scratch and lds */
+#define	PRIVATE_BASE(x)					((x) << 0) /* scratch */
+#define	SHARED_BASE(x)					((x) << 16) /* LDS */
+#define	SH_MEM_APE1_BASE				0x8C2C
+/* if PTR32, this is the base location of GPUVM */
+#define	SH_MEM_APE1_LIMIT				0x8C30
+/* if PTR32, this is the upper limit of GPUVM */
+#define	SH_MEM_CONFIG					0x8C34
+#define	PTR32						(1 << 0)
+#define PRIVATE_ATC					(1 << 1)
+#define	ALIGNMENT_MODE(x)				((x) << 2)
+#define	SH_MEM_ALIGNMENT_MODE_DWORD			0
+#define	SH_MEM_ALIGNMENT_MODE_DWORD_STRICT		1
+#define	SH_MEM_ALIGNMENT_MODE_STRICT			2
+#define	SH_MEM_ALIGNMENT_MODE_UNALIGNED			3
+#define	DEFAULT_MTYPE(x)				((x) << 4)
+#define	APE1_MTYPE(x)					((x) << 7)
+
+/* valid for both DEFAULT_MTYPE and APE1_MTYPE */
+#define	MTYPE_CACHED					0
+#define	MTYPE_NONCACHED					3
+
+
+#define SH_STATIC_MEM_CONFIG				0x9604u
+
+#define	TC_CFG_L1_LOAD_POLICY0				0xAC68
+#define	TC_CFG_L1_LOAD_POLICY1				0xAC6C
+#define	TC_CFG_L1_STORE_POLICY				0xAC70
+#define	TC_CFG_L2_LOAD_POLICY0				0xAC74
+#define	TC_CFG_L2_LOAD_POLICY1				0xAC78
+#define	TC_CFG_L2_STORE_POLICY0				0xAC7C
+#define	TC_CFG_L2_STORE_POLICY1				0xAC80
+#define	TC_CFG_L2_ATOMIC_POLICY				0xAC84
+#define	TC_CFG_L1_VOLATILE				0xAC88
+#define	TC_CFG_L2_VOLATILE				0xAC8C
+
+#define CP_PQ_WPTR_POLL_CNTL				0xC20C
+#define	WPTR_POLL_EN					(1 << 31)
+
+#define CPC_INT_CNTL					0xC2D0
+#define CP_ME1_PIPE0_INT_CNTL				0xC214
+#define CP_ME1_PIPE1_INT_CNTL				0xC218
+#define CP_ME1_PIPE2_INT_CNTL				0xC21C
+#define CP_ME1_PIPE3_INT_CNTL				0xC220
+#define CP_ME2_PIPE0_INT_CNTL				0xC224
+#define CP_ME2_PIPE1_INT_CNTL				0xC228
+#define CP_ME2_PIPE2_INT_CNTL				0xC22C
+#define CP_ME2_PIPE3_INT_CNTL				0xC230
+#define DEQUEUE_REQUEST_INT_ENABLE			(1 << 13)
+#define WRM_POLL_TIMEOUT_INT_ENABLE			(1 << 17)
+#define PRIV_REG_INT_ENABLE				(1 << 23)
+#define TIME_STAMP_INT_ENABLE				(1 << 26)
+#define GENERIC2_INT_ENABLE				(1 << 29)
+#define GENERIC1_INT_ENABLE				(1 << 30)
+#define GENERIC0_INT_ENABLE				(1 << 31)
+#define CP_ME1_PIPE0_INT_STATUS				0xC214
+#define CP_ME1_PIPE1_INT_STATUS				0xC218
+#define CP_ME1_PIPE2_INT_STATUS				0xC21C
+#define CP_ME1_PIPE3_INT_STATUS				0xC220
+#define CP_ME2_PIPE0_INT_STATUS				0xC224
+#define CP_ME2_PIPE1_INT_STATUS				0xC228
+#define CP_ME2_PIPE2_INT_STATUS				0xC22C
+#define CP_ME2_PIPE3_INT_STATUS				0xC230
+#define DEQUEUE_REQUEST_INT_STATUS			(1 << 13)
+#define WRM_POLL_TIMEOUT_INT_STATUS			(1 << 17)
+#define PRIV_REG_INT_STATUS				(1 << 23)
+#define TIME_STAMP_INT_STATUS				(1 << 26)
+#define GENERIC2_INT_STATUS				(1 << 29)
+#define GENERIC1_INT_STATUS				(1 << 30)
+#define GENERIC0_INT_STATUS				(1 << 31)
+
+#define CP_HPD_EOP_BASE_ADDR				0xC904
+#define CP_HPD_EOP_BASE_ADDR_HI				0xC908
+#define CP_HPD_EOP_VMID					0xC90C
+#define CP_HPD_EOP_CONTROL				0xC910
+#define	EOP_SIZE(x)					((x) << 0)
+#define	EOP_SIZE_MASK					(0x3f << 0)
+#define CP_MQD_BASE_ADDR				0xC914
+#define CP_MQD_BASE_ADDR_HI				0xC918
+#define CP_HQD_ACTIVE					0xC91C
+#define CP_HQD_VMID					0xC920
+
+#define CP_HQD_PERSISTENT_STATE				0xC924u
+#define	DEFAULT_CP_HQD_PERSISTENT_STATE			(0x33U << 8)
+#define	PRELOAD_REQ					(1 << 0)
+
+#define CP_HQD_PIPE_PRIORITY				0xC928u
+#define CP_HQD_QUEUE_PRIORITY				0xC92Cu
+#define CP_HQD_QUANTUM					0xC930u
+#define	QUANTUM_EN					1U
+#define	QUANTUM_SCALE_1MS				(1U << 4)
+#define	QUANTUM_DURATION(x)				((x) << 8)
+
+#define CP_HQD_PQ_BASE					0xC934
+#define CP_HQD_PQ_BASE_HI				0xC938
+#define CP_HQD_PQ_RPTR					0xC93C
+#define CP_HQD_PQ_RPTR_REPORT_ADDR			0xC940
+#define CP_HQD_PQ_RPTR_REPORT_ADDR_HI			0xC944
+#define CP_HQD_PQ_WPTR_POLL_ADDR			0xC948
+#define CP_HQD_PQ_WPTR_POLL_ADDR_HI			0xC94C
+#define CP_HQD_PQ_DOORBELL_CONTROL			0xC950
+#define	DOORBELL_OFFSET(x)				((x) << 2)
+#define	DOORBELL_OFFSET_MASK				(0x1fffff << 2)
+#define	DOORBELL_SOURCE					(1 << 28)
+#define	DOORBELL_SCHD_HIT				(1 << 29)
+#define	DOORBELL_EN					(1 << 30)
+#define	DOORBELL_HIT					(1 << 31)
+#define CP_HQD_PQ_WPTR					0xC954
+#define CP_HQD_PQ_CONTROL				0xC958
+#define	QUEUE_SIZE(x)					((x) << 0)
+#define	QUEUE_SIZE_MASK					(0x3f << 0)
+#define	RPTR_BLOCK_SIZE(x)				((x) << 8)
+#define	RPTR_BLOCK_SIZE_MASK				(0x3f << 8)
+#define	MIN_AVAIL_SIZE(x)				((x) << 20)
+#define	PQ_ATC_EN					(1 << 23)
+#define	PQ_VOLATILE					(1 << 26)
+#define	NO_UPDATE_RPTR					(1 << 27)
+#define	UNORD_DISPATCH					(1 << 28)
+#define	ROQ_PQ_IB_FLIP					(1 << 29)
+#define	PRIV_STATE					(1 << 30)
+#define	KMD_QUEUE					(1 << 31)
+
+#define	DEFAULT_RPTR_BLOCK_SIZE				RPTR_BLOCK_SIZE(5)
+#define	DEFAULT_MIN_AVAIL_SIZE				MIN_AVAIL_SIZE(3)
+
+#define CP_HQD_IB_BASE_ADDR				0xC95Cu
+#define CP_HQD_IB_BASE_ADDR_HI				0xC960u
+#define CP_HQD_IB_RPTR					0xC964u
+#define CP_HQD_IB_CONTROL				0xC968u
+#define	IB_ATC_EN					(1U << 23)
+#define	DEFAULT_MIN_IB_AVAIL_SIZE			(3U << 20)
+
+#define CP_HQD_DEQUEUE_REQUEST				0xC974
+#define	DEQUEUE_REQUEST_DRAIN				1
+#define DEQUEUE_REQUEST_RESET				2
+#define		DEQUEUE_INT					(1U << 8)
+
+#define CP_HQD_SEMA_CMD					0xC97Cu
+#define CP_HQD_MSG_TYPE					0xC980u
+#define CP_HQD_ATOMIC0_PREOP_LO				0xC984u
+#define CP_HQD_ATOMIC0_PREOP_HI				0xC988u
+#define CP_HQD_ATOMIC1_PREOP_LO				0xC98Cu
+#define CP_HQD_ATOMIC1_PREOP_HI				0xC990u
+#define CP_HQD_HQ_SCHEDULER0				0xC994u
+#define CP_HQD_HQ_SCHEDULER1				0xC998u
+
+
+#define CP_MQD_CONTROL					0xC99C
+#define	MQD_VMID(x)					((x) << 0)
+#define	MQD_VMID_MASK					(0xf << 0)
+#define	MQD_CONTROL_PRIV_STATE_EN			(1U << 8)
+
+#define GRBM_GFX_INDEX					0x30800
+#define	INSTANCE_INDEX(x)				((x) << 0)
+#define	SH_INDEX(x)					((x) << 8)
+#define	SE_INDEX(x)					((x) << 16)
+#define	SH_BROADCAST_WRITES				(1 << 29)
+#define	INSTANCE_BROADCAST_WRITES			(1 << 30)
+#define	SE_BROADCAST_WRITES				(1 << 31)
+
+#define SQC_CACHES					0x30d20
+#define SQC_POLICY					0x8C38u
+#define SQC_VOLATILE					0x8C3Cu
+
+#define CP_PERFMON_CNTL					0x36020
+
+#define ATC_VMID0_PASID_MAPPING				0x339Cu
+#define	ATC_VMID_PASID_MAPPING_UPDATE_STATUS		0x3398u
+#define	ATC_VMID_PASID_MAPPING_VALID			(1U << 31)
+
+#define ATC_VM_APERTURE0_CNTL				0x3310u
+#define	ATS_ACCESS_MODE_NEVER				0
+#define	ATS_ACCESS_MODE_ALWAYS				1
+
+#define ATC_VM_APERTURE0_CNTL2				0x3318u
+#define ATC_VM_APERTURE0_HIGH_ADDR			0x3308u
+#define ATC_VM_APERTURE0_LOW_ADDR			0x3300u
+#define ATC_VM_APERTURE1_CNTL				0x3314u
+#define ATC_VM_APERTURE1_CNTL2				0x331Cu
+#define ATC_VM_APERTURE1_HIGH_ADDR			0x330Cu
+#define ATC_VM_APERTURE1_LOW_ADDR			0x3304u
+
+#endif
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_chardev.c b/drivers/gpu/drm/amd/amdkfd/kfd_chardev.c
new file mode 100644
index 000000000000..7d4974b83af7
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_chardev.c
@@ -0,0 +1,591 @@
+/*
+ * Copyright 2014 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.
+ */
+
+#include <linux/device.h>
+#include <linux/export.h>
+#include <linux/err.h>
+#include <linux/fs.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/compat.h>
+#include <uapi/linux/kfd_ioctl.h>
+#include <linux/time.h>
+#include <linux/mm.h>
+#include <linux/uaccess.h>
+#include <uapi/asm-generic/mman-common.h>
+#include <asm/processor.h>
+#include "kfd_priv.h"
+#include "kfd_device_queue_manager.h"
+
+static long kfd_ioctl(struct file *, unsigned int, unsigned long);
+static int kfd_open(struct inode *, struct file *);
+static int kfd_mmap(struct file *, struct vm_area_struct *);
+
+static const char kfd_dev_name[] = "kfd";
+
+static const struct file_operations kfd_fops = {
+	.owner = THIS_MODULE,
+	.unlocked_ioctl = kfd_ioctl,
+	.compat_ioctl = kfd_ioctl,
+	.open = kfd_open,
+	.mmap = kfd_mmap,
+};
+
+static int kfd_char_dev_major = -1;
+static struct class *kfd_class;
+struct device *kfd_device;
+
+int kfd_chardev_init(void)
+{
+	int err = 0;
+
+	kfd_char_dev_major = register_chrdev(0, kfd_dev_name, &kfd_fops);
+	err = kfd_char_dev_major;
+	if (err < 0)
+		goto err_register_chrdev;
+
+	kfd_class = class_create(THIS_MODULE, kfd_dev_name);
+	err = PTR_ERR(kfd_class);
+	if (IS_ERR(kfd_class))
+		goto err_class_create;
+
+	kfd_device = device_create(kfd_class, NULL,
+					MKDEV(kfd_char_dev_major, 0),
+					NULL, kfd_dev_name);
+	err = PTR_ERR(kfd_device);
+	if (IS_ERR(kfd_device))
+		goto err_device_create;
+
+	return 0;
+
+err_device_create:
+	class_destroy(kfd_class);
+err_class_create:
+	unregister_chrdev(kfd_char_dev_major, kfd_dev_name);
+err_register_chrdev:
+	return err;
+}
+
+void kfd_chardev_exit(void)
+{
+	device_destroy(kfd_class, MKDEV(kfd_char_dev_major, 0));
+	class_destroy(kfd_class);
+	unregister_chrdev(kfd_char_dev_major, kfd_dev_name);
+}
+
+struct device *kfd_chardev(void)
+{
+	return kfd_device;
+}
+
+
+static int kfd_open(struct inode *inode, struct file *filep)
+{
+	struct kfd_process *process;
+	bool is_32bit_user_mode;
+
+	if (iminor(inode) != 0)
+		return -ENODEV;
+
+	is_32bit_user_mode = is_compat_task();
+
+	if (is_32bit_user_mode == true) {
+		dev_warn(kfd_device,
+			"Process %d (32-bit) failed to open /dev/kfd\n"
+			"32-bit processes are not supported by amdkfd\n",
+			current->pid);
+		return -EPERM;
+	}
+
+	process = kfd_create_process(current);
+	if (IS_ERR(process))
+		return PTR_ERR(process);
+
+	dev_dbg(kfd_device, "process %d opened, compat mode (32 bit) - %d\n",
+		process->pasid, process->is_32bit_user_mode);
+
+	return 0;
+}
+
+static long kfd_ioctl_get_version(struct file *filep, struct kfd_process *p,
+					void __user *arg)
+{
+	struct kfd_ioctl_get_version_args args;
+	int err = 0;
+
+	args.major_version = KFD_IOCTL_MAJOR_VERSION;
+	args.minor_version = KFD_IOCTL_MINOR_VERSION;
+
+	if (copy_to_user(arg, &args, sizeof(args)))
+		err = -EFAULT;
+
+	return err;
+}
+
+static int set_queue_properties_from_user(struct queue_properties *q_properties,
+				struct kfd_ioctl_create_queue_args *args)
+{
+	if (args->queue_percentage > KFD_MAX_QUEUE_PERCENTAGE) {
+		pr_err("kfd: queue percentage must be between 0 to KFD_MAX_QUEUE_PERCENTAGE\n");
+		return -EINVAL;
+	}
+
+	if (args->queue_priority > KFD_MAX_QUEUE_PRIORITY) {
+		pr_err("kfd: queue priority must be between 0 to KFD_MAX_QUEUE_PRIORITY\n");
+		return -EINVAL;
+	}
+
+	if ((args->ring_base_address) &&
+		(!access_ok(VERIFY_WRITE,
+			(const void __user *) args->ring_base_address,
+			sizeof(uint64_t)))) {
+		pr_err("kfd: can't access ring base address\n");
+		return -EFAULT;
+	}
+
+	if (!is_power_of_2(args->ring_size) && (args->ring_size != 0)) {
+		pr_err("kfd: ring size must be a power of 2 or 0\n");
+		return -EINVAL;
+	}
+
+	if (!access_ok(VERIFY_WRITE,
+			(const void __user *) args->read_pointer_address,
+			sizeof(uint32_t))) {
+		pr_err("kfd: can't access read pointer\n");
+		return -EFAULT;
+	}
+
+	if (!access_ok(VERIFY_WRITE,
+			(const void __user *) args->write_pointer_address,
+			sizeof(uint32_t))) {
+		pr_err("kfd: can't access write pointer\n");
+		return -EFAULT;
+	}
+
+	q_properties->is_interop = false;
+	q_properties->queue_percent = args->queue_percentage;
+	q_properties->priority = args->queue_priority;
+	q_properties->queue_address = args->ring_base_address;
+	q_properties->queue_size = args->ring_size;
+	q_properties->read_ptr = (uint32_t *) args->read_pointer_address;
+	q_properties->write_ptr = (uint32_t *) args->write_pointer_address;
+	if (args->queue_type == KFD_IOC_QUEUE_TYPE_COMPUTE ||
+		args->queue_type == KFD_IOC_QUEUE_TYPE_COMPUTE_AQL)
+		q_properties->type = KFD_QUEUE_TYPE_COMPUTE;
+	else
+		return -ENOTSUPP;
+
+	if (args->queue_type == KFD_IOC_QUEUE_TYPE_COMPUTE_AQL)
+		q_properties->format = KFD_QUEUE_FORMAT_AQL;
+	else
+		q_properties->format = KFD_QUEUE_FORMAT_PM4;
+
+	pr_debug("Queue Percentage (%d, %d)\n",
+			q_properties->queue_percent, args->queue_percentage);
+
+	pr_debug("Queue Priority (%d, %d)\n",
+			q_properties->priority, args->queue_priority);
+
+	pr_debug("Queue Address (0x%llX, 0x%llX)\n",
+			q_properties->queue_address, args->ring_base_address);
+
+	pr_debug("Queue Size (0x%llX, %u)\n",
+			q_properties->queue_size, args->ring_size);
+
+	pr_debug("Queue r/w Pointers (0x%llX, 0x%llX)\n",
+			(uint64_t) q_properties->read_ptr,
+			(uint64_t) q_properties->write_ptr);
+
+	pr_debug("Queue Format (%d)\n", q_properties->format);
+
+	return 0;
+}
+
+static long kfd_ioctl_create_queue(struct file *filep, struct kfd_process *p,
+					void __user *arg)
+{
+	struct kfd_ioctl_create_queue_args args;
+	struct kfd_dev *dev;
+	int err = 0;
+	unsigned int queue_id;
+	struct kfd_process_device *pdd;
+	struct queue_properties q_properties;
+
+	memset(&q_properties, 0, sizeof(struct queue_properties));
+
+	if (copy_from_user(&args, arg, sizeof(args)))
+		return -EFAULT;
+
+	pr_debug("kfd: creating queue ioctl\n");
+
+	err = set_queue_properties_from_user(&q_properties, &args);
+	if (err)
+		return err;
+
+	dev = kfd_device_by_id(args.gpu_id);
+	if (dev == NULL)
+		return -EINVAL;
+
+	mutex_lock(&p->mutex);
+
+	pdd = kfd_bind_process_to_device(dev, p);
+	if (IS_ERR(pdd)) {
+		err = PTR_ERR(pdd);
+		goto err_bind_process;
+	}
+
+	pr_debug("kfd: creating queue for PASID %d on GPU 0x%x\n",
+			p->pasid,
+			dev->id);
+
+	err = pqm_create_queue(&p->pqm, dev, filep, &q_properties, 0,
+				KFD_QUEUE_TYPE_COMPUTE, &queue_id);
+	if (err != 0)
+		goto err_create_queue;
+
+	args.queue_id = queue_id;
+
+	/* Return gpu_id as doorbell offset for mmap usage */
+	args.doorbell_offset = args.gpu_id << PAGE_SHIFT;
+
+	if (copy_to_user(arg, &args, sizeof(args))) {
+		err = -EFAULT;
+		goto err_copy_args_out;
+	}
+
+	mutex_unlock(&p->mutex);
+
+	pr_debug("kfd: queue id %d was created successfully\n", args.queue_id);
+
+	pr_debug("ring buffer address == 0x%016llX\n",
+			args.ring_base_address);
+
+	pr_debug("read ptr address    == 0x%016llX\n",
+			args.read_pointer_address);
+
+	pr_debug("write ptr address   == 0x%016llX\n",
+			args.write_pointer_address);
+
+	return 0;
+
+err_copy_args_out:
+	pqm_destroy_queue(&p->pqm, queue_id);
+err_create_queue:
+err_bind_process:
+	mutex_unlock(&p->mutex);
+	return err;
+}
+
+static int kfd_ioctl_destroy_queue(struct file *filp, struct kfd_process *p,
+					void __user *arg)
+{
+	int retval;
+	struct kfd_ioctl_destroy_queue_args args;
+
+	if (copy_from_user(&args, arg, sizeof(args)))
+		return -EFAULT;
+
+	pr_debug("kfd: destroying queue id %d for PASID %d\n",
+				args.queue_id,
+				p->pasid);
+
+	mutex_lock(&p->mutex);
+
+	retval = pqm_destroy_queue(&p->pqm, args.queue_id);
+
+	mutex_unlock(&p->mutex);
+	return retval;
+}
+
+static int kfd_ioctl_update_queue(struct file *filp, struct kfd_process *p,
+					void __user *arg)
+{
+	int retval;
+	struct kfd_ioctl_update_queue_args args;
+	struct queue_properties properties;
+
+	if (copy_from_user(&args, arg, sizeof(args)))
+		return -EFAULT;
+
+	if (args.queue_percentage > KFD_MAX_QUEUE_PERCENTAGE) {
+		pr_err("kfd: queue percentage must be between 0 to KFD_MAX_QUEUE_PERCENTAGE\n");
+		return -EINVAL;
+	}
+
+	if (args.queue_priority > KFD_MAX_QUEUE_PRIORITY) {
+		pr_err("kfd: queue priority must be between 0 to KFD_MAX_QUEUE_PRIORITY\n");
+		return -EINVAL;
+	}
+
+	if ((args.ring_base_address) &&
+		(!access_ok(VERIFY_WRITE,
+			(const void __user *) args.ring_base_address,
+			sizeof(uint64_t)))) {
+		pr_err("kfd: can't access ring base address\n");
+		return -EFAULT;
+	}
+
+	if (!is_power_of_2(args.ring_size) && (args.ring_size != 0)) {
+		pr_err("kfd: ring size must be a power of 2 or 0\n");
+		return -EINVAL;
+	}
+
+	properties.queue_address = args.ring_base_address;
+	properties.queue_size = args.ring_size;
+	properties.queue_percent = args.queue_percentage;
+	properties.priority = args.queue_priority;
+
+	pr_debug("kfd: updating queue id %d for PASID %d\n",
+			args.queue_id, p->pasid);
+
+	mutex_lock(&p->mutex);
+
+	retval = pqm_update_queue(&p->pqm, args.queue_id, &properties);
+
+	mutex_unlock(&p->mutex);
+
+	return retval;
+}
+
+static long kfd_ioctl_set_memory_policy(struct file *filep,
+				struct kfd_process *p, void __user *arg)
+{
+	struct kfd_ioctl_set_memory_policy_args args;
+	struct kfd_dev *dev;
+	int err = 0;
+	struct kfd_process_device *pdd;
+	enum cache_policy default_policy, alternate_policy;
+
+	if (copy_from_user(&args, arg, sizeof(args)))
+		return -EFAULT;
+
+	if (args.default_policy != KFD_IOC_CACHE_POLICY_COHERENT
+	    && args.default_policy != KFD_IOC_CACHE_POLICY_NONCOHERENT) {
+		return -EINVAL;
+	}
+
+	if (args.alternate_policy != KFD_IOC_CACHE_POLICY_COHERENT
+	    && args.alternate_policy != KFD_IOC_CACHE_POLICY_NONCOHERENT) {
+		return -EINVAL;
+	}
+
+	dev = kfd_device_by_id(args.gpu_id);
+	if (dev == NULL)
+		return -EINVAL;
+
+	mutex_lock(&p->mutex);
+
+	pdd = kfd_bind_process_to_device(dev, p);
+	if (IS_ERR(pdd)) {
+		err = PTR_ERR(pdd);
+		goto out;
+	}
+
+	default_policy = (args.default_policy == KFD_IOC_CACHE_POLICY_COHERENT)
+			 ? cache_policy_coherent : cache_policy_noncoherent;
+
+	alternate_policy =
+		(args.alternate_policy == KFD_IOC_CACHE_POLICY_COHERENT)
+		   ? cache_policy_coherent : cache_policy_noncoherent;
+
+	if (!dev->dqm->set_cache_memory_policy(dev->dqm,
+				&pdd->qpd,
+				default_policy,
+				alternate_policy,
+				(void __user *)args.alternate_aperture_base,
+				args.alternate_aperture_size))
+		err = -EINVAL;
+
+out:
+	mutex_unlock(&p->mutex);
+
+	return err;
+}
+
+static long kfd_ioctl_get_clock_counters(struct file *filep,
+				struct kfd_process *p, void __user *arg)
+{
+	struct kfd_ioctl_get_clock_counters_args args;
+	struct kfd_dev *dev;
+	struct timespec time;
+
+	if (copy_from_user(&args, arg, sizeof(args)))
+		return -EFAULT;
+
+	dev = kfd_device_by_id(args.gpu_id);
+	if (dev == NULL)
+		return -EINVAL;
+
+	/* Reading GPU clock counter from KGD */
+	args.gpu_clock_counter = kfd2kgd->get_gpu_clock_counter(dev->kgd);
+
+	/* No access to rdtsc. Using raw monotonic time */
+	getrawmonotonic(&time);
+	args.cpu_clock_counter = (uint64_t)timespec_to_ns(&time);
+
+	get_monotonic_boottime(&time);
+	args.system_clock_counter = (uint64_t)timespec_to_ns(&time);
+
+	/* Since the counter is in nano-seconds we use 1GHz frequency */
+	args.system_clock_freq = 1000000000;
+
+	if (copy_to_user(arg, &args, sizeof(args)))
+		return -EFAULT;
+
+	return 0;
+}
+
+
+static int kfd_ioctl_get_process_apertures(struct file *filp,
+				struct kfd_process *p, void __user *arg)
+{
+	struct kfd_ioctl_get_process_apertures_args args;
+	struct kfd_process_device_apertures *pAperture;
+	struct kfd_process_device *pdd;
+
+	dev_dbg(kfd_device, "get apertures for PASID %d", p->pasid);
+
+	if (copy_from_user(&args, arg, sizeof(args)))
+		return -EFAULT;
+
+	args.num_of_nodes = 0;
+
+	mutex_lock(&p->mutex);
+
+	/*if the process-device list isn't empty*/
+	if (kfd_has_process_device_data(p)) {
+		/* Run over all pdd of the process */
+		pdd = kfd_get_first_process_device_data(p);
+		do {
+			pAperture = &args.process_apertures[args.num_of_nodes];
+			pAperture->gpu_id = pdd->dev->id;
+			pAperture->lds_base = pdd->lds_base;
+			pAperture->lds_limit = pdd->lds_limit;
+			pAperture->gpuvm_base = pdd->gpuvm_base;
+			pAperture->gpuvm_limit = pdd->gpuvm_limit;
+			pAperture->scratch_base = pdd->scratch_base;
+			pAperture->scratch_limit = pdd->scratch_limit;
+
+			dev_dbg(kfd_device,
+				"node id %u\n", args.num_of_nodes);
+			dev_dbg(kfd_device,
+				"gpu id %u\n", pdd->dev->id);
+			dev_dbg(kfd_device,
+				"lds_base %llX\n", pdd->lds_base);
+			dev_dbg(kfd_device,
+				"lds_limit %llX\n", pdd->lds_limit);
+			dev_dbg(kfd_device,
+				"gpuvm_base %llX\n", pdd->gpuvm_base);
+			dev_dbg(kfd_device,
+				"gpuvm_limit %llX\n", pdd->gpuvm_limit);
+			dev_dbg(kfd_device,
+				"scratch_base %llX\n", pdd->scratch_base);
+			dev_dbg(kfd_device,
+				"scratch_limit %llX\n", pdd->scratch_limit);
+
+			args.num_of_nodes++;
+		} while ((pdd = kfd_get_next_process_device_data(p, pdd)) != NULL &&
+				(args.num_of_nodes < NUM_OF_SUPPORTED_GPUS));
+	}
+
+	mutex_unlock(&p->mutex);
+
+	if (copy_to_user(arg, &args, sizeof(args)))
+		return -EFAULT;
+
+	return 0;
+}
+
+static long kfd_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
+{
+	struct kfd_process *process;
+	long err = -EINVAL;
+
+	dev_dbg(kfd_device,
+		"ioctl cmd 0x%x (#%d), arg 0x%lx\n",
+		cmd, _IOC_NR(cmd), arg);
+
+	process = kfd_get_process(current);
+	if (IS_ERR(process))
+		return PTR_ERR(process);
+
+	switch (cmd) {
+	case KFD_IOC_GET_VERSION:
+		err = kfd_ioctl_get_version(filep, process, (void __user *)arg);
+		break;
+	case KFD_IOC_CREATE_QUEUE:
+		err = kfd_ioctl_create_queue(filep, process,
+						(void __user *)arg);
+		break;
+
+	case KFD_IOC_DESTROY_QUEUE:
+		err = kfd_ioctl_destroy_queue(filep, process,
+						(void __user *)arg);
+		break;
+
+	case KFD_IOC_SET_MEMORY_POLICY:
+		err = kfd_ioctl_set_memory_policy(filep, process,
+						(void __user *)arg);
+		break;
+
+	case KFD_IOC_GET_CLOCK_COUNTERS:
+		err = kfd_ioctl_get_clock_counters(filep, process,
+						(void __user *)arg);
+		break;
+
+	case KFD_IOC_GET_PROCESS_APERTURES:
+		err = kfd_ioctl_get_process_apertures(filep, process,
+						(void __user *)arg);
+		break;
+
+	case KFD_IOC_UPDATE_QUEUE:
+		err = kfd_ioctl_update_queue(filep, process,
+						(void __user *)arg);
+		break;
+
+	default:
+		dev_err(kfd_device,
+			"unknown ioctl cmd 0x%x, arg 0x%lx)\n",
+			cmd, arg);
+		err = -EINVAL;
+		break;
+	}
+
+	if (err < 0)
+		dev_err(kfd_device,
+			"ioctl error %ld for ioctl cmd 0x%x (#%d)\n",
+			err, cmd, _IOC_NR(cmd));
+
+	return err;
+}
+
+static int kfd_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	struct kfd_process *process;
+
+	process = kfd_get_process(current);
+	if (IS_ERR(process))
+		return PTR_ERR(process);
+
+	return kfd_doorbell_mmap(process, vma);
+}
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_crat.h b/drivers/gpu/drm/amd/amdkfd/kfd_crat.h
new file mode 100644
index 000000000000..a374fa3d3ee6
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_crat.h
@@ -0,0 +1,294 @@
+/*
+ * Copyright 2014 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.
+ */
+
+#ifndef KFD_CRAT_H_INCLUDED
+#define KFD_CRAT_H_INCLUDED
+
+#include <linux/types.h>
+
+#pragma pack(1)
+
+/*
+ * 4CC signature values for the CRAT and CDIT ACPI tables
+ */
+
+#define CRAT_SIGNATURE	"CRAT"
+#define CDIT_SIGNATURE	"CDIT"
+
+/*
+ * Component Resource Association Table (CRAT)
+ */
+
+#define CRAT_OEMID_LENGTH	6
+#define CRAT_OEMTABLEID_LENGTH	8
+#define CRAT_RESERVED_LENGTH	6
+
+#define CRAT_OEMID_64BIT_MASK ((1ULL << (CRAT_OEMID_LENGTH * 8)) - 1)
+
+struct crat_header {
+	uint32_t	signature;
+	uint32_t	length;
+	uint8_t		revision;
+	uint8_t		checksum;
+	uint8_t		oem_id[CRAT_OEMID_LENGTH];
+	uint8_t		oem_table_id[CRAT_OEMTABLEID_LENGTH];
+	uint32_t	oem_revision;
+	uint32_t	creator_id;
+	uint32_t	creator_revision;
+	uint32_t	total_entries;
+	uint16_t	num_domains;
+	uint8_t		reserved[CRAT_RESERVED_LENGTH];
+};
+
+/*
+ * The header structure is immediately followed by total_entries of the
+ * data definitions
+ */
+
+/*
+ * The currently defined subtype entries in the CRAT
+ */
+#define CRAT_SUBTYPE_COMPUTEUNIT_AFFINITY	0
+#define CRAT_SUBTYPE_MEMORY_AFFINITY		1
+#define CRAT_SUBTYPE_CACHE_AFFINITY		2
+#define CRAT_SUBTYPE_TLB_AFFINITY		3
+#define CRAT_SUBTYPE_CCOMPUTE_AFFINITY		4
+#define CRAT_SUBTYPE_IOLINK_AFFINITY		5
+#define CRAT_SUBTYPE_MAX			6
+
+#define CRAT_SIBLINGMAP_SIZE	32
+
+/*
+ * ComputeUnit Affinity structure and definitions
+ */
+#define CRAT_CU_FLAGS_ENABLED		0x00000001
+#define CRAT_CU_FLAGS_HOT_PLUGGABLE	0x00000002
+#define CRAT_CU_FLAGS_CPU_PRESENT	0x00000004
+#define CRAT_CU_FLAGS_GPU_PRESENT	0x00000008
+#define CRAT_CU_FLAGS_IOMMU_PRESENT	0x00000010
+#define CRAT_CU_FLAGS_RESERVED		0xffffffe0
+
+#define CRAT_COMPUTEUNIT_RESERVED_LENGTH 4
+
+struct crat_subtype_computeunit {
+	uint8_t		type;
+	uint8_t		length;
+	uint16_t	reserved;
+	uint32_t	flags;
+	uint32_t	proximity_domain;
+	uint32_t	processor_id_low;
+	uint16_t	num_cpu_cores;
+	uint16_t	num_simd_cores;
+	uint16_t	max_waves_simd;
+	uint16_t	io_count;
+	uint16_t	hsa_capability;
+	uint16_t	lds_size_in_kb;
+	uint8_t		wave_front_size;
+	uint8_t		num_banks;
+	uint16_t	micro_engine_id;
+	uint8_t		num_arrays;
+	uint8_t		num_cu_per_array;
+	uint8_t		num_simd_per_cu;
+	uint8_t		max_slots_scatch_cu;
+	uint8_t		reserved2[CRAT_COMPUTEUNIT_RESERVED_LENGTH];
+};
+
+/*
+ * HSA Memory Affinity structure and definitions
+ */
+#define CRAT_MEM_FLAGS_ENABLED		0x00000001
+#define CRAT_MEM_FLAGS_HOT_PLUGGABLE	0x00000002
+#define CRAT_MEM_FLAGS_NON_VOLATILE	0x00000004
+#define CRAT_MEM_FLAGS_RESERVED		0xfffffff8
+
+#define CRAT_MEMORY_RESERVED_LENGTH 8
+
+struct crat_subtype_memory {
+	uint8_t		type;
+	uint8_t		length;
+	uint16_t	reserved;
+	uint32_t	flags;
+	uint32_t	promixity_domain;
+	uint32_t	base_addr_low;
+	uint32_t	base_addr_high;
+	uint32_t	length_low;
+	uint32_t	length_high;
+	uint32_t	width;
+	uint8_t		reserved2[CRAT_MEMORY_RESERVED_LENGTH];
+};
+
+/*
+ * HSA Cache Affinity structure and definitions
+ */
+#define CRAT_CACHE_FLAGS_ENABLED	0x00000001
+#define CRAT_CACHE_FLAGS_DATA_CACHE	0x00000002
+#define CRAT_CACHE_FLAGS_INST_CACHE	0x00000004
+#define CRAT_CACHE_FLAGS_CPU_CACHE	0x00000008
+#define CRAT_CACHE_FLAGS_SIMD_CACHE	0x00000010
+#define CRAT_CACHE_FLAGS_RESERVED	0xffffffe0
+
+#define CRAT_CACHE_RESERVED_LENGTH 8
+
+struct crat_subtype_cache {
+	uint8_t		type;
+	uint8_t		length;
+	uint16_t	reserved;
+	uint32_t	flags;
+	uint32_t	processor_id_low;
+	uint8_t		sibling_map[CRAT_SIBLINGMAP_SIZE];
+	uint32_t	cache_size;
+	uint8_t		cache_level;
+	uint8_t		lines_per_tag;
+	uint16_t	cache_line_size;
+	uint8_t		associativity;
+	uint8_t		cache_properties;
+	uint16_t	cache_latency;
+	uint8_t		reserved2[CRAT_CACHE_RESERVED_LENGTH];
+};
+
+/*
+ * HSA TLB Affinity structure and definitions
+ */
+#define CRAT_TLB_FLAGS_ENABLED	0x00000001
+#define CRAT_TLB_FLAGS_DATA_TLB	0x00000002
+#define CRAT_TLB_FLAGS_INST_TLB	0x00000004
+#define CRAT_TLB_FLAGS_CPU_TLB	0x00000008
+#define CRAT_TLB_FLAGS_SIMD_TLB	0x00000010
+#define CRAT_TLB_FLAGS_RESERVED	0xffffffe0
+
+#define CRAT_TLB_RESERVED_LENGTH 4
+
+struct crat_subtype_tlb {
+	uint8_t		type;
+	uint8_t		length;
+	uint16_t	reserved;
+	uint32_t	flags;
+	uint32_t	processor_id_low;
+	uint8_t		sibling_map[CRAT_SIBLINGMAP_SIZE];
+	uint32_t	tlb_level;
+	uint8_t		data_tlb_associativity_2mb;
+	uint8_t		data_tlb_size_2mb;
+	uint8_t		instruction_tlb_associativity_2mb;
+	uint8_t		instruction_tlb_size_2mb;
+	uint8_t		data_tlb_associativity_4k;
+	uint8_t		data_tlb_size_4k;
+	uint8_t		instruction_tlb_associativity_4k;
+	uint8_t		instruction_tlb_size_4k;
+	uint8_t		data_tlb_associativity_1gb;
+	uint8_t		data_tlb_size_1gb;
+	uint8_t		instruction_tlb_associativity_1gb;
+	uint8_t		instruction_tlb_size_1gb;
+	uint8_t		reserved2[CRAT_TLB_RESERVED_LENGTH];
+};
+
+/*
+ * HSA CCompute/APU Affinity structure and definitions
+ */
+#define CRAT_CCOMPUTE_FLAGS_ENABLED	0x00000001
+#define CRAT_CCOMPUTE_FLAGS_RESERVED	0xfffffffe
+
+#define CRAT_CCOMPUTE_RESERVED_LENGTH 16
+
+struct crat_subtype_ccompute {
+	uint8_t		type;
+	uint8_t		length;
+	uint16_t	reserved;
+	uint32_t	flags;
+	uint32_t	processor_id_low;
+	uint8_t		sibling_map[CRAT_SIBLINGMAP_SIZE];
+	uint32_t	apu_size;
+	uint8_t		reserved2[CRAT_CCOMPUTE_RESERVED_LENGTH];
+};
+
+/*
+ * HSA IO Link Affinity structure and definitions
+ */
+#define CRAT_IOLINK_FLAGS_ENABLED	0x00000001
+#define CRAT_IOLINK_FLAGS_COHERENCY	0x00000002
+#define CRAT_IOLINK_FLAGS_RESERVED	0xfffffffc
+
+/*
+ * IO interface types
+ */
+#define CRAT_IOLINK_TYPE_UNDEFINED	0
+#define CRAT_IOLINK_TYPE_HYPERTRANSPORT	1
+#define CRAT_IOLINK_TYPE_PCIEXPRESS	2
+#define CRAT_IOLINK_TYPE_OTHER		3
+#define CRAT_IOLINK_TYPE_MAX		255
+
+#define CRAT_IOLINK_RESERVED_LENGTH 24
+
+struct crat_subtype_iolink {
+	uint8_t		type;
+	uint8_t		length;
+	uint16_t	reserved;
+	uint32_t	flags;
+	uint32_t	proximity_domain_from;
+	uint32_t	proximity_domain_to;
+	uint8_t		io_interface_type;
+	uint8_t		version_major;
+	uint16_t	version_minor;
+	uint32_t	minimum_latency;
+	uint32_t	maximum_latency;
+	uint32_t	minimum_bandwidth_mbs;
+	uint32_t	maximum_bandwidth_mbs;
+	uint32_t	recommended_transfer_size;
+	uint8_t		reserved2[CRAT_IOLINK_RESERVED_LENGTH];
+};
+
+/*
+ * HSA generic sub-type header
+ */
+
+#define CRAT_SUBTYPE_FLAGS_ENABLED 0x00000001
+
+struct crat_subtype_generic {
+	uint8_t		type;
+	uint8_t		length;
+	uint16_t	reserved;
+	uint32_t	flags;
+};
+
+/*
+ * Component Locality Distance Information Table (CDIT)
+ */
+#define CDIT_OEMID_LENGTH	6
+#define CDIT_OEMTABLEID_LENGTH	8
+
+struct cdit_header {
+	uint32_t	signature;
+	uint32_t	length;
+	uint8_t		revision;
+	uint8_t		checksum;
+	uint8_t		oem_id[CDIT_OEMID_LENGTH];
+	uint8_t		oem_table_id[CDIT_OEMTABLEID_LENGTH];
+	uint32_t	oem_revision;
+	uint32_t	creator_id;
+	uint32_t	creator_revision;
+	uint32_t	total_entries;
+	uint16_t	num_domains;
+	uint8_t		entry[1];
+};
+
+#pragma pack()
+
+#endif /* KFD_CRAT_H_INCLUDED */
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_device.c b/drivers/gpu/drm/amd/amdkfd/kfd_device.c
new file mode 100644
index 000000000000..43884ebd4303
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_device.c
@@ -0,0 +1,308 @@
+/*
+ * Copyright 2014 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.
+ */
+
+#include <linux/amd-iommu.h>
+#include <linux/bsearch.h>
+#include <linux/pci.h>
+#include <linux/slab.h>
+#include "kfd_priv.h"
+#include "kfd_device_queue_manager.h"
+
+#define MQD_SIZE_ALIGNED 768
+
+static const struct kfd_device_info kaveri_device_info = {
+	.max_pasid_bits = 16,
+	.ih_ring_entry_size = 4 * sizeof(uint32_t),
+	.mqd_size_aligned = MQD_SIZE_ALIGNED
+};
+
+struct kfd_deviceid {
+	unsigned short did;
+	const struct kfd_device_info *device_info;
+};
+
+/* Please keep this sorted by increasing device id. */
+static const struct kfd_deviceid supported_devices[] = {
+	{ 0x1304, &kaveri_device_info },	/* Kaveri */
+	{ 0x1305, &kaveri_device_info },	/* Kaveri */
+	{ 0x1306, &kaveri_device_info },	/* Kaveri */
+	{ 0x1307, &kaveri_device_info },	/* Kaveri */
+	{ 0x1309, &kaveri_device_info },	/* Kaveri */
+	{ 0x130A, &kaveri_device_info },	/* Kaveri */
+	{ 0x130B, &kaveri_device_info },	/* Kaveri */
+	{ 0x130C, &kaveri_device_info },	/* Kaveri */
+	{ 0x130D, &kaveri_device_info },	/* Kaveri */
+	{ 0x130E, &kaveri_device_info },	/* Kaveri */
+	{ 0x130F, &kaveri_device_info },	/* Kaveri */
+	{ 0x1310, &kaveri_device_info },	/* Kaveri */
+	{ 0x1311, &kaveri_device_info },	/* Kaveri */
+	{ 0x1312, &kaveri_device_info },	/* Kaveri */
+	{ 0x1313, &kaveri_device_info },	/* Kaveri */
+	{ 0x1315, &kaveri_device_info },	/* Kaveri */
+	{ 0x1316, &kaveri_device_info },	/* Kaveri */
+	{ 0x1317, &kaveri_device_info },	/* Kaveri */
+	{ 0x1318, &kaveri_device_info },	/* Kaveri */
+	{ 0x131B, &kaveri_device_info },	/* Kaveri */
+	{ 0x131C, &kaveri_device_info },	/* Kaveri */
+	{ 0x131D, &kaveri_device_info },	/* Kaveri */
+};
+
+static const struct kfd_device_info *lookup_device_info(unsigned short did)
+{
+	size_t i;
+
+	for (i = 0; i < ARRAY_SIZE(supported_devices); i++) {
+		if (supported_devices[i].did == did) {
+			BUG_ON(supported_devices[i].device_info == NULL);
+			return supported_devices[i].device_info;
+		}
+	}
+
+	return NULL;
+}
+
+struct kfd_dev *kgd2kfd_probe(struct kgd_dev *kgd, struct pci_dev *pdev)
+{
+	struct kfd_dev *kfd;
+
+	const struct kfd_device_info *device_info =
+					lookup_device_info(pdev->device);
+
+	if (!device_info)
+		return NULL;
+
+	kfd = kzalloc(sizeof(*kfd), GFP_KERNEL);
+	if (!kfd)
+		return NULL;
+
+	kfd->kgd = kgd;
+	kfd->device_info = device_info;
+	kfd->pdev = pdev;
+	kfd->init_complete = false;
+
+	return kfd;
+}
+
+static bool device_iommu_pasid_init(struct kfd_dev *kfd)
+{
+	const u32 required_iommu_flags = AMD_IOMMU_DEVICE_FLAG_ATS_SUP |
+					AMD_IOMMU_DEVICE_FLAG_PRI_SUP |
+					AMD_IOMMU_DEVICE_FLAG_PASID_SUP;
+
+	struct amd_iommu_device_info iommu_info;
+	unsigned int pasid_limit;
+	int err;
+
+	err = amd_iommu_device_info(kfd->pdev, &iommu_info);
+	if (err < 0) {
+		dev_err(kfd_device,
+			"error getting iommu info. is the iommu enabled?\n");
+		return false;
+	}
+
+	if ((iommu_info.flags & required_iommu_flags) != required_iommu_flags) {
+		dev_err(kfd_device, "error required iommu flags ats(%i), pri(%i), pasid(%i)\n",
+		       (iommu_info.flags & AMD_IOMMU_DEVICE_FLAG_ATS_SUP) != 0,
+		       (iommu_info.flags & AMD_IOMMU_DEVICE_FLAG_PRI_SUP) != 0,
+		       (iommu_info.flags & AMD_IOMMU_DEVICE_FLAG_PASID_SUP) != 0);
+		return false;
+	}
+
+	pasid_limit = min_t(unsigned int,
+			(unsigned int)1 << kfd->device_info->max_pasid_bits,
+			iommu_info.max_pasids);
+	/*
+	 * last pasid is used for kernel queues doorbells
+	 * in the future the last pasid might be used for a kernel thread.
+	 */
+	pasid_limit = min_t(unsigned int,
+				pasid_limit,
+				kfd->doorbell_process_limit - 1);
+
+	err = amd_iommu_init_device(kfd->pdev, pasid_limit);
+	if (err < 0) {
+		dev_err(kfd_device, "error initializing iommu device\n");
+		return false;
+	}
+
+	if (!kfd_set_pasid_limit(pasid_limit)) {
+		dev_err(kfd_device, "error setting pasid limit\n");
+		amd_iommu_free_device(kfd->pdev);
+		return false;
+	}
+
+	return true;
+}
+
+static void iommu_pasid_shutdown_callback(struct pci_dev *pdev, int pasid)
+{
+	struct kfd_dev *dev = kfd_device_by_pci_dev(pdev);
+
+	if (dev)
+		kfd_unbind_process_from_device(dev, pasid);
+}
+
+bool kgd2kfd_device_init(struct kfd_dev *kfd,
+			 const struct kgd2kfd_shared_resources *gpu_resources)
+{
+	unsigned int size;
+
+	kfd->shared_resources = *gpu_resources;
+
+	/* calculate max size of mqds needed for queues */
+	size = max_num_of_processes *
+		max_num_of_queues_per_process *
+		kfd->device_info->mqd_size_aligned;
+
+	/* add another 512KB for all other allocations on gart */
+	size += 512 * 1024;
+
+	if (kfd2kgd->init_sa_manager(kfd->kgd, size)) {
+		dev_err(kfd_device,
+			"Error initializing sa manager for device (%x:%x)\n",
+			kfd->pdev->vendor, kfd->pdev->device);
+		goto out;
+	}
+
+	kfd_doorbell_init(kfd);
+
+	if (kfd_topology_add_device(kfd) != 0) {
+		dev_err(kfd_device,
+			"Error adding device (%x:%x) to topology\n",
+			kfd->pdev->vendor, kfd->pdev->device);
+		goto kfd_topology_add_device_error;
+	}
+
+	if (kfd_interrupt_init(kfd)) {
+		dev_err(kfd_device,
+			"Error initializing interrupts for device (%x:%x)\n",
+			kfd->pdev->vendor, kfd->pdev->device);
+		goto kfd_interrupt_error;
+	}
+
+	if (!device_iommu_pasid_init(kfd)) {
+		dev_err(kfd_device,
+			"Error initializing iommuv2 for device (%x:%x)\n",
+			kfd->pdev->vendor, kfd->pdev->device);
+		goto device_iommu_pasid_error;
+	}
+	amd_iommu_set_invalidate_ctx_cb(kfd->pdev,
+						iommu_pasid_shutdown_callback);
+
+	kfd->dqm = device_queue_manager_init(kfd);
+	if (!kfd->dqm) {
+		dev_err(kfd_device,
+			"Error initializing queue manager for device (%x:%x)\n",
+			kfd->pdev->vendor, kfd->pdev->device);
+		goto device_queue_manager_error;
+	}
+
+	if (kfd->dqm->start(kfd->dqm) != 0) {
+		dev_err(kfd_device,
+			"Error starting queuen manager for device (%x:%x)\n",
+			kfd->pdev->vendor, kfd->pdev->device);
+		goto dqm_start_error;
+	}
+
+	kfd->init_complete = true;
+	dev_info(kfd_device, "added device (%x:%x)\n", kfd->pdev->vendor,
+		 kfd->pdev->device);
+
+	pr_debug("kfd: Starting kfd with the following scheduling policy %d\n",
+		sched_policy);
+
+	goto out;
+
+dqm_start_error:
+	device_queue_manager_uninit(kfd->dqm);
+device_queue_manager_error:
+	amd_iommu_free_device(kfd->pdev);
+device_iommu_pasid_error:
+	kfd_interrupt_exit(kfd);
+kfd_interrupt_error:
+	kfd_topology_remove_device(kfd);
+kfd_topology_add_device_error:
+	kfd2kgd->fini_sa_manager(kfd->kgd);
+	dev_err(kfd_device,
+		"device (%x:%x) NOT added due to errors\n",
+		kfd->pdev->vendor, kfd->pdev->device);
+out:
+	return kfd->init_complete;
+}
+
+void kgd2kfd_device_exit(struct kfd_dev *kfd)
+{
+	if (kfd->init_complete) {
+		device_queue_manager_uninit(kfd->dqm);
+		amd_iommu_free_device(kfd->pdev);
+		kfd_interrupt_exit(kfd);
+		kfd_topology_remove_device(kfd);
+	}
+
+	kfree(kfd);
+}
+
+void kgd2kfd_suspend(struct kfd_dev *kfd)
+{
+	BUG_ON(kfd == NULL);
+
+	if (kfd->init_complete) {
+		kfd->dqm->stop(kfd->dqm);
+		amd_iommu_set_invalidate_ctx_cb(kfd->pdev, NULL);
+		amd_iommu_free_device(kfd->pdev);
+	}
+}
+
+int kgd2kfd_resume(struct kfd_dev *kfd)
+{
+	unsigned int pasid_limit;
+	int err;
+
+	BUG_ON(kfd == NULL);
+
+	pasid_limit = kfd_get_pasid_limit();
+
+	if (kfd->init_complete) {
+		err = amd_iommu_init_device(kfd->pdev, pasid_limit);
+		if (err < 0)
+			return -ENXIO;
+		amd_iommu_set_invalidate_ctx_cb(kfd->pdev,
+						iommu_pasid_shutdown_callback);
+		kfd->dqm->start(kfd->dqm);
+	}
+
+	return 0;
+}
+
+/* This is called directly from KGD at ISR. */
+void kgd2kfd_interrupt(struct kfd_dev *kfd, const void *ih_ring_entry)
+{
+	if (kfd->init_complete) {
+		spin_lock(&kfd->interrupt_lock);
+
+		if (kfd->interrupts_active
+		    && enqueue_ih_ring_entry(kfd, ih_ring_entry))
+			schedule_work(&kfd->interrupt_work);
+
+		spin_unlock(&kfd->interrupt_lock);
+	}
+}
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager.c b/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager.c
new file mode 100644
index 000000000000..924e90c072e5
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager.c
@@ -0,0 +1,1062 @@
+/*
+ * Copyright 2014 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.
+ *
+ */
+
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <linux/types.h>
+#include <linux/printk.h>
+#include <linux/bitops.h>
+#include "kfd_priv.h"
+#include "kfd_device_queue_manager.h"
+#include "kfd_mqd_manager.h"
+#include "cik_regs.h"
+#include "kfd_kernel_queue.h"
+#include "../../radeon/cik_reg.h"
+
+/* Size of the per-pipe EOP queue */
+#define CIK_HPD_EOP_BYTES_LOG2 11
+#define CIK_HPD_EOP_BYTES (1U << CIK_HPD_EOP_BYTES_LOG2)
+
+static bool is_mem_initialized;
+
+static int init_memory(struct device_queue_manager *dqm);
+static int set_pasid_vmid_mapping(struct device_queue_manager *dqm,
+					unsigned int pasid, unsigned int vmid);
+
+static int create_compute_queue_nocpsch(struct device_queue_manager *dqm,
+					struct queue *q,
+					struct qcm_process_device *qpd);
+static int execute_queues_cpsch(struct device_queue_manager *dqm, bool lock);
+static int destroy_queues_cpsch(struct device_queue_manager *dqm, bool lock);
+
+
+static inline unsigned int get_pipes_num(struct device_queue_manager *dqm)
+{
+	BUG_ON(!dqm || !dqm->dev);
+	return dqm->dev->shared_resources.compute_pipe_count;
+}
+
+static inline unsigned int get_first_pipe(struct device_queue_manager *dqm)
+{
+	BUG_ON(!dqm);
+	return dqm->dev->shared_resources.first_compute_pipe;
+}
+
+static inline unsigned int get_pipes_num_cpsch(void)
+{
+	return PIPE_PER_ME_CP_SCHEDULING;
+}
+
+static inline unsigned int
+get_sh_mem_bases_nybble_64(struct kfd_process_device *pdd)
+{
+	uint32_t nybble;
+
+	nybble = (pdd->lds_base >> 60) & 0x0E;
+
+	return nybble;
+
+}
+
+static inline unsigned int get_sh_mem_bases_32(struct kfd_process_device *pdd)
+{
+	unsigned int shared_base;
+
+	shared_base = (pdd->lds_base >> 16) & 0xFF;
+
+	return shared_base;
+}
+
+static uint32_t compute_sh_mem_bases_64bit(unsigned int top_address_nybble);
+static void init_process_memory(struct device_queue_manager *dqm,
+				struct qcm_process_device *qpd)
+{
+	struct kfd_process_device *pdd;
+	unsigned int temp;
+
+	BUG_ON(!dqm || !qpd);
+
+	pdd = qpd_to_pdd(qpd);
+
+	/* check if sh_mem_config register already configured */
+	if (qpd->sh_mem_config == 0) {
+		qpd->sh_mem_config =
+			ALIGNMENT_MODE(SH_MEM_ALIGNMENT_MODE_UNALIGNED) |
+			DEFAULT_MTYPE(MTYPE_NONCACHED) |
+			APE1_MTYPE(MTYPE_NONCACHED);
+		qpd->sh_mem_ape1_limit = 0;
+		qpd->sh_mem_ape1_base = 0;
+	}
+
+	if (qpd->pqm->process->is_32bit_user_mode) {
+		temp = get_sh_mem_bases_32(pdd);
+		qpd->sh_mem_bases = SHARED_BASE(temp);
+		qpd->sh_mem_config |= PTR32;
+	} else {
+		temp = get_sh_mem_bases_nybble_64(pdd);
+		qpd->sh_mem_bases = compute_sh_mem_bases_64bit(temp);
+	}
+
+	pr_debug("kfd: is32bit process: %d sh_mem_bases nybble: 0x%X and register 0x%X\n",
+		qpd->pqm->process->is_32bit_user_mode, temp, qpd->sh_mem_bases);
+}
+
+static void program_sh_mem_settings(struct device_queue_manager *dqm,
+					struct qcm_process_device *qpd)
+{
+	return kfd2kgd->program_sh_mem_settings(dqm->dev->kgd, qpd->vmid,
+						qpd->sh_mem_config,
+						qpd->sh_mem_ape1_base,
+						qpd->sh_mem_ape1_limit,
+						qpd->sh_mem_bases);
+}
+
+static int allocate_vmid(struct device_queue_manager *dqm,
+			struct qcm_process_device *qpd,
+			struct queue *q)
+{
+	int bit, allocated_vmid;
+
+	if (dqm->vmid_bitmap == 0)
+		return -ENOMEM;
+
+	bit = find_first_bit((unsigned long *)&dqm->vmid_bitmap, CIK_VMID_NUM);
+	clear_bit(bit, (unsigned long *)&dqm->vmid_bitmap);
+
+	/* Kaveri kfd vmid's starts from vmid 8 */
+	allocated_vmid = bit + KFD_VMID_START_OFFSET;
+	pr_debug("kfd: vmid allocation %d\n", allocated_vmid);
+	qpd->vmid = allocated_vmid;
+	q->properties.vmid = allocated_vmid;
+
+	set_pasid_vmid_mapping(dqm, q->process->pasid, q->properties.vmid);
+	program_sh_mem_settings(dqm, qpd);
+
+	return 0;
+}
+
+static void deallocate_vmid(struct device_queue_manager *dqm,
+				struct qcm_process_device *qpd,
+				struct queue *q)
+{
+	int bit = qpd->vmid - KFD_VMID_START_OFFSET;
+
+	set_bit(bit, (unsigned long *)&dqm->vmid_bitmap);
+	qpd->vmid = 0;
+	q->properties.vmid = 0;
+}
+
+static int create_queue_nocpsch(struct device_queue_manager *dqm,
+				struct queue *q,
+				struct qcm_process_device *qpd,
+				int *allocated_vmid)
+{
+	int retval;
+
+	BUG_ON(!dqm || !q || !qpd || !allocated_vmid);
+
+	pr_debug("kfd: In func %s\n", __func__);
+	print_queue(q);
+
+	mutex_lock(&dqm->lock);
+
+	if (list_empty(&qpd->queues_list)) {
+		retval = allocate_vmid(dqm, qpd, q);
+		if (retval != 0) {
+			mutex_unlock(&dqm->lock);
+			return retval;
+		}
+	}
+	*allocated_vmid = qpd->vmid;
+	q->properties.vmid = qpd->vmid;
+
+	retval = create_compute_queue_nocpsch(dqm, q, qpd);
+
+	if (retval != 0) {
+		if (list_empty(&qpd->queues_list)) {
+			deallocate_vmid(dqm, qpd, q);
+			*allocated_vmid = 0;
+		}
+		mutex_unlock(&dqm->lock);
+		return retval;
+	}
+
+	list_add(&q->list, &qpd->queues_list);
+	dqm->queue_count++;
+
+	mutex_unlock(&dqm->lock);
+	return 0;
+}
+
+static int allocate_hqd(struct device_queue_manager *dqm, struct queue *q)
+{
+	bool set;
+	int pipe, bit;
+
+	set = false;
+
+	for (pipe = dqm->next_pipe_to_allocate; pipe < get_pipes_num(dqm);
+			pipe = (pipe + 1) % get_pipes_num(dqm)) {
+		if (dqm->allocated_queues[pipe] != 0) {
+			bit = find_first_bit(
+				(unsigned long *)&dqm->allocated_queues[pipe],
+				QUEUES_PER_PIPE);
+
+			clear_bit(bit,
+				(unsigned long *)&dqm->allocated_queues[pipe]);
+			q->pipe = pipe;
+			q->queue = bit;
+			set = true;
+			break;
+		}
+	}
+
+	if (set == false)
+		return -EBUSY;
+
+	pr_debug("kfd: DQM %s hqd slot - pipe (%d) queue(%d)\n",
+				__func__, q->pipe, q->queue);
+	/* horizontal hqd allocation */
+	dqm->next_pipe_to_allocate = (pipe + 1) % get_pipes_num(dqm);
+
+	return 0;
+}
+
+static inline void deallocate_hqd(struct device_queue_manager *dqm,
+				struct queue *q)
+{
+	set_bit(q->queue, (unsigned long *)&dqm->allocated_queues[q->pipe]);
+}
+
+static int create_compute_queue_nocpsch(struct device_queue_manager *dqm,
+					struct queue *q,
+					struct qcm_process_device *qpd)
+{
+	int retval;
+	struct mqd_manager *mqd;
+
+	BUG_ON(!dqm || !q || !qpd);
+
+	mqd = dqm->get_mqd_manager(dqm, KFD_MQD_TYPE_CIK_COMPUTE);
+	if (mqd == NULL)
+		return -ENOMEM;
+
+	retval = allocate_hqd(dqm, q);
+	if (retval != 0)
+		return retval;
+
+	retval = mqd->init_mqd(mqd, &q->mqd, &q->mqd_mem_obj,
+				&q->gart_mqd_addr, &q->properties);
+	if (retval != 0) {
+		deallocate_hqd(dqm, q);
+		return retval;
+	}
+
+	return 0;
+}
+
+static int destroy_queue_nocpsch(struct device_queue_manager *dqm,
+				struct qcm_process_device *qpd,
+				struct queue *q)
+{
+	int retval;
+	struct mqd_manager *mqd;
+
+	BUG_ON(!dqm || !q || !q->mqd || !qpd);
+
+	retval = 0;
+
+	pr_debug("kfd: In Func %s\n", __func__);
+
+	mutex_lock(&dqm->lock);
+	mqd = dqm->get_mqd_manager(dqm, KFD_MQD_TYPE_CIK_COMPUTE);
+	if (mqd == NULL) {
+		retval = -ENOMEM;
+		goto out;
+	}
+
+	retval = mqd->destroy_mqd(mqd, q->mqd,
+				KFD_PREEMPT_TYPE_WAVEFRONT,
+				QUEUE_PREEMPT_DEFAULT_TIMEOUT_MS,
+				q->pipe, q->queue);
+
+	if (retval != 0)
+		goto out;
+
+	deallocate_hqd(dqm, q);
+
+	mqd->uninit_mqd(mqd, q->mqd, q->mqd_mem_obj);
+
+	list_del(&q->list);
+	if (list_empty(&qpd->queues_list))
+		deallocate_vmid(dqm, qpd, q);
+	dqm->queue_count--;
+out:
+	mutex_unlock(&dqm->lock);
+	return retval;
+}
+
+static int update_queue(struct device_queue_manager *dqm, struct queue *q)
+{
+	int retval;
+	struct mqd_manager *mqd;
+
+	BUG_ON(!dqm || !q || !q->mqd);
+
+	mutex_lock(&dqm->lock);
+	mqd = dqm->get_mqd_manager(dqm, KFD_MQD_TYPE_CIK_COMPUTE);
+	if (mqd == NULL) {
+		mutex_unlock(&dqm->lock);
+		return -ENOMEM;
+	}
+
+	retval = mqd->update_mqd(mqd, q->mqd, &q->properties);
+	if (q->properties.is_active == true)
+		dqm->queue_count++;
+	else
+		dqm->queue_count--;
+
+	if (sched_policy != KFD_SCHED_POLICY_NO_HWS)
+		retval = execute_queues_cpsch(dqm, false);
+
+	mutex_unlock(&dqm->lock);
+	return retval;
+}
+
+static struct mqd_manager *get_mqd_manager_nocpsch(
+		struct device_queue_manager *dqm, enum KFD_MQD_TYPE type)
+{
+	struct mqd_manager *mqd;
+
+	BUG_ON(!dqm || type >= KFD_MQD_TYPE_MAX);
+
+	pr_debug("kfd: In func %s mqd type %d\n", __func__, type);
+
+	mqd = dqm->mqds[type];
+	if (!mqd) {
+		mqd = mqd_manager_init(type, dqm->dev);
+		if (mqd == NULL)
+			pr_err("kfd: mqd manager is NULL");
+		dqm->mqds[type] = mqd;
+	}
+
+	return mqd;
+}
+
+static int register_process_nocpsch(struct device_queue_manager *dqm,
+					struct qcm_process_device *qpd)
+{
+	struct device_process_node *n;
+
+	BUG_ON(!dqm || !qpd);
+
+	pr_debug("kfd: In func %s\n", __func__);
+
+	n = kzalloc(sizeof(struct device_process_node), GFP_KERNEL);
+	if (!n)
+		return -ENOMEM;
+
+	n->qpd = qpd;
+
+	mutex_lock(&dqm->lock);
+	list_add(&n->list, &dqm->queues);
+
+	init_process_memory(dqm, qpd);
+	dqm->processes_count++;
+
+	mutex_unlock(&dqm->lock);
+
+	return 0;
+}
+
+static int unregister_process_nocpsch(struct device_queue_manager *dqm,
+					struct qcm_process_device *qpd)
+{
+	int retval;
+	struct device_process_node *cur, *next;
+
+	BUG_ON(!dqm || !qpd);
+
+	BUG_ON(!list_empty(&qpd->queues_list));
+
+	pr_debug("kfd: In func %s\n", __func__);
+
+	retval = 0;
+	mutex_lock(&dqm->lock);
+
+	list_for_each_entry_safe(cur, next, &dqm->queues, list) {
+		if (qpd == cur->qpd) {
+			list_del(&cur->list);
+			kfree(cur);
+			dqm->processes_count--;
+			goto out;
+		}
+	}
+	/* qpd not found in dqm list */
+	retval = 1;
+out:
+	mutex_unlock(&dqm->lock);
+	return retval;
+}
+
+static int
+set_pasid_vmid_mapping(struct device_queue_manager *dqm, unsigned int pasid,
+			unsigned int vmid)
+{
+	uint32_t pasid_mapping;
+
+	pasid_mapping = (pasid == 0) ? 0 : (uint32_t)pasid |
+						ATC_VMID_PASID_MAPPING_VALID;
+	return kfd2kgd->set_pasid_vmid_mapping(dqm->dev->kgd, pasid_mapping,
+						vmid);
+}
+
+static uint32_t compute_sh_mem_bases_64bit(unsigned int top_address_nybble)
+{
+	/* In 64-bit mode, we can only control the top 3 bits of the LDS,
+	 * scratch and GPUVM apertures.
+	 * The hardware fills in the remaining 59 bits according to the
+	 * following pattern:
+	 * LDS:		X0000000'00000000 - X0000001'00000000 (4GB)
+	 * Scratch:	X0000001'00000000 - X0000002'00000000 (4GB)
+	 * GPUVM:	Y0010000'00000000 - Y0020000'00000000 (1TB)
+	 *
+	 * (where X/Y is the configurable nybble with the low-bit 0)
+	 *
+	 * LDS and scratch will have the same top nybble programmed in the
+	 * top 3 bits of SH_MEM_BASES.PRIVATE_BASE.
+	 * GPUVM can have a different top nybble programmed in the
+	 * top 3 bits of SH_MEM_BASES.SHARED_BASE.
+	 * We don't bother to support different top nybbles
+	 * for LDS/Scratch and GPUVM.
+	 */
+
+	BUG_ON((top_address_nybble & 1) || top_address_nybble > 0xE ||
+		top_address_nybble == 0);
+
+	return PRIVATE_BASE(top_address_nybble << 12) |
+			SHARED_BASE(top_address_nybble << 12);
+}
+
+static int init_memory(struct device_queue_manager *dqm)
+{
+	int i, retval;
+
+	for (i = 8; i < 16; i++)
+		set_pasid_vmid_mapping(dqm, 0, i);
+
+	retval = kfd2kgd->init_memory(dqm->dev->kgd);
+	if (retval == 0)
+		is_mem_initialized = true;
+	return retval;
+}
+
+
+static int init_pipelines(struct device_queue_manager *dqm,
+			unsigned int pipes_num, unsigned int first_pipe)
+{
+	void *hpdptr;
+	struct mqd_manager *mqd;
+	unsigned int i, err, inx;
+	uint64_t pipe_hpd_addr;
+
+	BUG_ON(!dqm || !dqm->dev);
+
+	pr_debug("kfd: In func %s\n", __func__);
+
+	/*
+	 * Allocate memory for the HPDs. This is hardware-owned per-pipe data.
+	 * The driver never accesses this memory after zeroing it.
+	 * It doesn't even have to be saved/restored on suspend/resume
+	 * because it contains no data when there are no active queues.
+	 */
+
+	err = kfd2kgd->allocate_mem(dqm->dev->kgd,
+				CIK_HPD_EOP_BYTES * pipes_num,
+				PAGE_SIZE,
+				KFD_MEMPOOL_SYSTEM_WRITECOMBINE,
+				(struct kgd_mem **) &dqm->pipeline_mem);
+
+	if (err) {
+		pr_err("kfd: error allocate vidmem num pipes: %d\n",
+			pipes_num);
+		return -ENOMEM;
+	}
+
+	hpdptr = dqm->pipeline_mem->cpu_ptr;
+	dqm->pipelines_addr = dqm->pipeline_mem->gpu_addr;
+
+	memset(hpdptr, 0, CIK_HPD_EOP_BYTES * pipes_num);
+
+	mqd = dqm->get_mqd_manager(dqm, KFD_MQD_TYPE_CIK_COMPUTE);
+	if (mqd == NULL) {
+		kfd2kgd->free_mem(dqm->dev->kgd,
+				(struct kgd_mem *) dqm->pipeline_mem);
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < pipes_num; i++) {
+		inx = i + first_pipe;
+		pipe_hpd_addr = dqm->pipelines_addr + i * CIK_HPD_EOP_BYTES;
+		pr_debug("kfd: pipeline address %llX\n", pipe_hpd_addr);
+		/* = log2(bytes/4)-1 */
+		kfd2kgd->init_pipeline(dqm->dev->kgd, i,
+				CIK_HPD_EOP_BYTES_LOG2 - 3, pipe_hpd_addr);
+	}
+
+	return 0;
+}
+
+
+static int init_scheduler(struct device_queue_manager *dqm)
+{
+	int retval;
+
+	BUG_ON(!dqm);
+
+	pr_debug("kfd: In %s\n", __func__);
+
+	retval = init_pipelines(dqm, get_pipes_num(dqm), KFD_DQM_FIRST_PIPE);
+	if (retval != 0)
+		return retval;
+
+	retval = init_memory(dqm);
+
+	return retval;
+}
+
+static int initialize_nocpsch(struct device_queue_manager *dqm)
+{
+	int i;
+
+	BUG_ON(!dqm);
+
+	pr_debug("kfd: In func %s num of pipes: %d\n",
+			__func__, get_pipes_num(dqm));
+
+	mutex_init(&dqm->lock);
+	INIT_LIST_HEAD(&dqm->queues);
+	dqm->queue_count = dqm->next_pipe_to_allocate = 0;
+	dqm->allocated_queues = kcalloc(get_pipes_num(dqm),
+					sizeof(unsigned int), GFP_KERNEL);
+	if (!dqm->allocated_queues) {
+		mutex_destroy(&dqm->lock);
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < get_pipes_num(dqm); i++)
+		dqm->allocated_queues[i] = (1 << QUEUES_PER_PIPE) - 1;
+
+	dqm->vmid_bitmap = (1 << VMID_PER_DEVICE) - 1;
+
+	init_scheduler(dqm);
+	return 0;
+}
+
+static void uninitialize_nocpsch(struct device_queue_manager *dqm)
+{
+	int i;
+
+	BUG_ON(!dqm);
+
+	BUG_ON(dqm->queue_count > 0 || dqm->processes_count > 0);
+
+	kfree(dqm->allocated_queues);
+	for (i = 0 ; i < KFD_MQD_TYPE_MAX ; i++)
+		kfree(dqm->mqds[i]);
+	mutex_destroy(&dqm->lock);
+	kfd2kgd->free_mem(dqm->dev->kgd,
+			(struct kgd_mem *) dqm->pipeline_mem);
+}
+
+static int start_nocpsch(struct device_queue_manager *dqm)
+{
+	return 0;
+}
+
+static int stop_nocpsch(struct device_queue_manager *dqm)
+{
+	return 0;
+}
+
+/*
+ * Device Queue Manager implementation for cp scheduler
+ */
+
+static int set_sched_resources(struct device_queue_manager *dqm)
+{
+	struct scheduling_resources res;
+	unsigned int queue_num, queue_mask;
+
+	BUG_ON(!dqm);
+
+	pr_debug("kfd: In func %s\n", __func__);
+
+	queue_num = get_pipes_num_cpsch() * QUEUES_PER_PIPE;
+	queue_mask = (1 << queue_num) - 1;
+	res.vmid_mask = (1 << VMID_PER_DEVICE) - 1;
+	res.vmid_mask <<= KFD_VMID_START_OFFSET;
+	res.queue_mask = queue_mask << (get_first_pipe(dqm) * QUEUES_PER_PIPE);
+	res.gws_mask = res.oac_mask = res.gds_heap_base =
+						res.gds_heap_size = 0;
+
+	pr_debug("kfd: scheduling resources:\n"
+			"      vmid mask: 0x%8X\n"
+			"      queue mask: 0x%8llX\n",
+			res.vmid_mask, res.queue_mask);
+
+	return pm_send_set_resources(&dqm->packets, &res);
+}
+
+static int initialize_cpsch(struct device_queue_manager *dqm)
+{
+	int retval;
+
+	BUG_ON(!dqm);
+
+	pr_debug("kfd: In func %s num of pipes: %d\n",
+			__func__, get_pipes_num_cpsch());
+
+	mutex_init(&dqm->lock);
+	INIT_LIST_HEAD(&dqm->queues);
+	dqm->queue_count = dqm->processes_count = 0;
+	dqm->active_runlist = false;
+	retval = init_pipelines(dqm, get_pipes_num(dqm), 0);
+	if (retval != 0)
+		goto fail_init_pipelines;
+
+	return 0;
+
+fail_init_pipelines:
+	mutex_destroy(&dqm->lock);
+	return retval;
+}
+
+static int start_cpsch(struct device_queue_manager *dqm)
+{
+	struct device_process_node *node;
+	int retval;
+
+	BUG_ON(!dqm);
+
+	retval = 0;
+
+	retval = pm_init(&dqm->packets, dqm);
+	if (retval != 0)
+		goto fail_packet_manager_init;
+
+	retval = set_sched_resources(dqm);
+	if (retval != 0)
+		goto fail_set_sched_resources;
+
+	pr_debug("kfd: allocating fence memory\n");
+
+	/* allocate fence memory on the gart */
+	retval = kfd2kgd->allocate_mem(dqm->dev->kgd,
+					sizeof(*dqm->fence_addr),
+					32,
+					KFD_MEMPOOL_SYSTEM_WRITECOMBINE,
+					(struct kgd_mem **) &dqm->fence_mem);
+
+	if (retval != 0)
+		goto fail_allocate_vidmem;
+
+	dqm->fence_addr = dqm->fence_mem->cpu_ptr;
+	dqm->fence_gpu_addr = dqm->fence_mem->gpu_addr;
+
+	list_for_each_entry(node, &dqm->queues, list)
+		if (node->qpd->pqm->process && dqm->dev)
+			kfd_bind_process_to_device(dqm->dev,
+						node->qpd->pqm->process);
+
+	execute_queues_cpsch(dqm, true);
+
+	return 0;
+fail_allocate_vidmem:
+fail_set_sched_resources:
+	pm_uninit(&dqm->packets);
+fail_packet_manager_init:
+	return retval;
+}
+
+static int stop_cpsch(struct device_queue_manager *dqm)
+{
+	struct device_process_node *node;
+	struct kfd_process_device *pdd;
+
+	BUG_ON(!dqm);
+
+	destroy_queues_cpsch(dqm, true);
+
+	list_for_each_entry(node, &dqm->queues, list) {
+		pdd = qpd_to_pdd(node->qpd);
+		pdd->bound = false;
+	}
+	kfd2kgd->free_mem(dqm->dev->kgd,
+			(struct kgd_mem *) dqm->fence_mem);
+	pm_uninit(&dqm->packets);
+
+	return 0;
+}
+
+static int create_kernel_queue_cpsch(struct device_queue_manager *dqm,
+					struct kernel_queue *kq,
+					struct qcm_process_device *qpd)
+{
+	BUG_ON(!dqm || !kq || !qpd);
+
+	pr_debug("kfd: In func %s\n", __func__);
+
+	mutex_lock(&dqm->lock);
+	list_add(&kq->list, &qpd->priv_queue_list);
+	dqm->queue_count++;
+	qpd->is_debug = true;
+	execute_queues_cpsch(dqm, false);
+	mutex_unlock(&dqm->lock);
+
+	return 0;
+}
+
+static void destroy_kernel_queue_cpsch(struct device_queue_manager *dqm,
+					struct kernel_queue *kq,
+					struct qcm_process_device *qpd)
+{
+	BUG_ON(!dqm || !kq);
+
+	pr_debug("kfd: In %s\n", __func__);
+
+	mutex_lock(&dqm->lock);
+	destroy_queues_cpsch(dqm, false);
+	list_del(&kq->list);
+	dqm->queue_count--;
+	qpd->is_debug = false;
+	execute_queues_cpsch(dqm, false);
+	mutex_unlock(&dqm->lock);
+}
+
+static int create_queue_cpsch(struct device_queue_manager *dqm, struct queue *q,
+			struct qcm_process_device *qpd, int *allocate_vmid)
+{
+	int retval;
+	struct mqd_manager *mqd;
+
+	BUG_ON(!dqm || !q || !qpd);
+
+	retval = 0;
+
+	if (allocate_vmid)
+		*allocate_vmid = 0;
+
+	mutex_lock(&dqm->lock);
+
+	mqd = dqm->get_mqd_manager(dqm, KFD_MQD_TYPE_CIK_CP);
+	if (mqd == NULL) {
+		mutex_unlock(&dqm->lock);
+		return -ENOMEM;
+	}
+
+	retval = mqd->init_mqd(mqd, &q->mqd, &q->mqd_mem_obj,
+				&q->gart_mqd_addr, &q->properties);
+	if (retval != 0)
+		goto out;
+
+	list_add(&q->list, &qpd->queues_list);
+	if (q->properties.is_active) {
+		dqm->queue_count++;
+		retval = execute_queues_cpsch(dqm, false);
+	}
+
+out:
+	mutex_unlock(&dqm->lock);
+	return retval;
+}
+
+static int fence_wait_timeout(unsigned int *fence_addr,
+				unsigned int fence_value,
+				unsigned long timeout)
+{
+	BUG_ON(!fence_addr);
+	timeout += jiffies;
+
+	while (*fence_addr != fence_value) {
+		if (time_after(jiffies, timeout)) {
+			pr_err("kfd: qcm fence wait loop timeout expired\n");
+			return -ETIME;
+		}
+		cpu_relax();
+	}
+
+	return 0;
+}
+
+static int destroy_queues_cpsch(struct device_queue_manager *dqm, bool lock)
+{
+	int retval;
+
+	BUG_ON(!dqm);
+
+	retval = 0;
+
+	if (lock)
+		mutex_lock(&dqm->lock);
+	if (dqm->active_runlist == false)
+		goto out;
+	retval = pm_send_unmap_queue(&dqm->packets, KFD_QUEUE_TYPE_COMPUTE,
+			KFD_PREEMPT_TYPE_FILTER_ALL_QUEUES, 0, false, 0);
+	if (retval != 0)
+		goto out;
+
+	*dqm->fence_addr = KFD_FENCE_INIT;
+	pm_send_query_status(&dqm->packets, dqm->fence_gpu_addr,
+				KFD_FENCE_COMPLETED);
+	/* should be timed out */
+	fence_wait_timeout(dqm->fence_addr, KFD_FENCE_COMPLETED,
+				QUEUE_PREEMPT_DEFAULT_TIMEOUT_MS);
+	pm_release_ib(&dqm->packets);
+	dqm->active_runlist = false;
+
+out:
+	if (lock)
+		mutex_unlock(&dqm->lock);
+	return retval;
+}
+
+static int execute_queues_cpsch(struct device_queue_manager *dqm, bool lock)
+{
+	int retval;
+
+	BUG_ON(!dqm);
+
+	if (lock)
+		mutex_lock(&dqm->lock);
+
+	retval = destroy_queues_cpsch(dqm, false);
+	if (retval != 0) {
+		pr_err("kfd: the cp might be in an unrecoverable state due to an unsuccessful queues preemption");
+		goto out;
+	}
+
+	if (dqm->queue_count <= 0 || dqm->processes_count <= 0) {
+		retval = 0;
+		goto out;
+	}
+
+	if (dqm->active_runlist) {
+		retval = 0;
+		goto out;
+	}
+
+	retval = pm_send_runlist(&dqm->packets, &dqm->queues);
+	if (retval != 0) {
+		pr_err("kfd: failed to execute runlist");
+		goto out;
+	}
+	dqm->active_runlist = true;
+
+out:
+	if (lock)
+		mutex_unlock(&dqm->lock);
+	return retval;
+}
+
+static int destroy_queue_cpsch(struct device_queue_manager *dqm,
+				struct qcm_process_device *qpd,
+				struct queue *q)
+{
+	int retval;
+	struct mqd_manager *mqd;
+
+	BUG_ON(!dqm || !qpd || !q);
+
+	retval = 0;
+
+	/* remove queue from list to prevent rescheduling after preemption */
+	mutex_lock(&dqm->lock);
+
+	mqd = dqm->get_mqd_manager(dqm, KFD_MQD_TYPE_CIK_CP);
+	if (!mqd) {
+		retval = -ENOMEM;
+		goto failed;
+	}
+
+	list_del(&q->list);
+	dqm->queue_count--;
+
+	execute_queues_cpsch(dqm, false);
+
+	mqd->uninit_mqd(mqd, q->mqd, q->mqd_mem_obj);
+
+	mutex_unlock(&dqm->lock);
+
+	return 0;
+
+failed:
+	mutex_unlock(&dqm->lock);
+	return retval;
+}
+
+/*
+ * Low bits must be 0000/FFFF as required by HW, high bits must be 0 to
+ * stay in user mode.
+ */
+#define APE1_FIXED_BITS_MASK 0xFFFF80000000FFFFULL
+/* APE1 limit is inclusive and 64K aligned. */
+#define APE1_LIMIT_ALIGNMENT 0xFFFF
+
+static bool set_cache_memory_policy(struct device_queue_manager *dqm,
+				   struct qcm_process_device *qpd,
+				   enum cache_policy default_policy,
+				   enum cache_policy alternate_policy,
+				   void __user *alternate_aperture_base,
+				   uint64_t alternate_aperture_size)
+{
+	uint32_t default_mtype;
+	uint32_t ape1_mtype;
+
+	pr_debug("kfd: In func %s\n", __func__);
+
+	mutex_lock(&dqm->lock);
+
+	if (alternate_aperture_size == 0) {
+		/* base > limit disables APE1 */
+		qpd->sh_mem_ape1_base = 1;
+		qpd->sh_mem_ape1_limit = 0;
+	} else {
+		/*
+		 * In FSA64, APE1_Base[63:0] = { 16{SH_MEM_APE1_BASE[31]},
+		 *			SH_MEM_APE1_BASE[31:0], 0x0000 }
+		 * APE1_Limit[63:0] = { 16{SH_MEM_APE1_LIMIT[31]},
+		 *			SH_MEM_APE1_LIMIT[31:0], 0xFFFF }
+		 * Verify that the base and size parameters can be
+		 * represented in this format and convert them.
+		 * Additionally restrict APE1 to user-mode addresses.
+		 */
+
+		uint64_t base = (uintptr_t)alternate_aperture_base;
+		uint64_t limit = base + alternate_aperture_size - 1;
+
+		if (limit <= base)
+			goto out;
+
+		if ((base & APE1_FIXED_BITS_MASK) != 0)
+			goto out;
+
+		if ((limit & APE1_FIXED_BITS_MASK) != APE1_LIMIT_ALIGNMENT)
+			goto out;
+
+		qpd->sh_mem_ape1_base = base >> 16;
+		qpd->sh_mem_ape1_limit = limit >> 16;
+	}
+
+	default_mtype = (default_policy == cache_policy_coherent) ?
+			MTYPE_NONCACHED :
+			MTYPE_CACHED;
+
+	ape1_mtype = (alternate_policy == cache_policy_coherent) ?
+			MTYPE_NONCACHED :
+			MTYPE_CACHED;
+
+	qpd->sh_mem_config = (qpd->sh_mem_config & PTR32)
+			| ALIGNMENT_MODE(SH_MEM_ALIGNMENT_MODE_UNALIGNED)
+			| DEFAULT_MTYPE(default_mtype)
+			| APE1_MTYPE(ape1_mtype);
+
+	if ((sched_policy == KFD_SCHED_POLICY_NO_HWS) && (qpd->vmid != 0))
+		program_sh_mem_settings(dqm, qpd);
+
+	pr_debug("kfd: sh_mem_config: 0x%x, ape1_base: 0x%x, ape1_limit: 0x%x\n",
+		qpd->sh_mem_config, qpd->sh_mem_ape1_base,
+		qpd->sh_mem_ape1_limit);
+
+	mutex_unlock(&dqm->lock);
+	return true;
+
+out:
+	mutex_unlock(&dqm->lock);
+	return false;
+}
+
+struct device_queue_manager *device_queue_manager_init(struct kfd_dev *dev)
+{
+	struct device_queue_manager *dqm;
+
+	BUG_ON(!dev);
+
+	dqm = kzalloc(sizeof(struct device_queue_manager), GFP_KERNEL);
+	if (!dqm)
+		return NULL;
+
+	dqm->dev = dev;
+	switch (sched_policy) {
+	case KFD_SCHED_POLICY_HWS:
+	case KFD_SCHED_POLICY_HWS_NO_OVERSUBSCRIPTION:
+		/* initialize dqm for cp scheduling */
+		dqm->create_queue = create_queue_cpsch;
+		dqm->initialize = initialize_cpsch;
+		dqm->start = start_cpsch;
+		dqm->stop = stop_cpsch;
+		dqm->destroy_queue = destroy_queue_cpsch;
+		dqm->update_queue = update_queue;
+		dqm->get_mqd_manager = get_mqd_manager_nocpsch;
+		dqm->register_process = register_process_nocpsch;
+		dqm->unregister_process = unregister_process_nocpsch;
+		dqm->uninitialize = uninitialize_nocpsch;
+		dqm->create_kernel_queue = create_kernel_queue_cpsch;
+		dqm->destroy_kernel_queue = destroy_kernel_queue_cpsch;
+		dqm->set_cache_memory_policy = set_cache_memory_policy;
+		break;
+	case KFD_SCHED_POLICY_NO_HWS:
+		/* initialize dqm for no cp scheduling */
+		dqm->start = start_nocpsch;
+		dqm->stop = stop_nocpsch;
+		dqm->create_queue = create_queue_nocpsch;
+		dqm->destroy_queue = destroy_queue_nocpsch;
+		dqm->update_queue = update_queue;
+		dqm->get_mqd_manager = get_mqd_manager_nocpsch;
+		dqm->register_process = register_process_nocpsch;
+		dqm->unregister_process = unregister_process_nocpsch;
+		dqm->initialize = initialize_nocpsch;
+		dqm->uninitialize = uninitialize_nocpsch;
+		dqm->set_cache_memory_policy = set_cache_memory_policy;
+		break;
+	default:
+		BUG();
+		break;
+	}
+
+	if (dqm->initialize(dqm) != 0) {
+		kfree(dqm);
+		return NULL;
+	}
+
+	return dqm;
+}
+
+void device_queue_manager_uninit(struct device_queue_manager *dqm)
+{
+	BUG_ON(!dqm);
+
+	dqm->uninitialize(dqm);
+	kfree(dqm);
+}
+
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager.h b/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager.h
new file mode 100644
index 000000000000..c3f189e8ae35
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager.h
@@ -0,0 +1,146 @@
+/*
+ * Copyright 2014 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.
+ *
+ */
+
+#ifndef KFD_DEVICE_QUEUE_MANAGER_H_
+#define KFD_DEVICE_QUEUE_MANAGER_H_
+
+#include <linux/rwsem.h>
+#include <linux/list.h>
+#include "kfd_priv.h"
+#include "kfd_mqd_manager.h"
+
+#define QUEUE_PREEMPT_DEFAULT_TIMEOUT_MS	(500)
+#define QUEUES_PER_PIPE				(8)
+#define PIPE_PER_ME_CP_SCHEDULING		(3)
+#define CIK_VMID_NUM				(8)
+#define KFD_VMID_START_OFFSET			(8)
+#define VMID_PER_DEVICE				CIK_VMID_NUM
+#define KFD_DQM_FIRST_PIPE			(0)
+
+struct device_process_node {
+	struct qcm_process_device *qpd;
+	struct list_head list;
+};
+
+/**
+ * struct device_queue_manager
+ *
+ * @create_queue: Queue creation routine.
+ *
+ * @destroy_queue: Queue destruction routine.
+ *
+ * @update_queue: Queue update routine.
+ *
+ * @get_mqd_manager: Returns the mqd manager according to the mqd type.
+ *
+ * @exeute_queues: Dispatches the queues list to the H/W.
+ *
+ * @register_process: This routine associates a specific process with device.
+ *
+ * @unregister_process: destroys the associations between process to device.
+ *
+ * @initialize: Initializes the pipelines and memory module for that device.
+ *
+ * @start: Initializes the resources/modules the the device needs for queues
+ * execution. This function is called on device initialization and after the
+ * system woke up after suspension.
+ *
+ * @stop: This routine stops execution of all the active queue running on the
+ * H/W and basically this function called on system suspend.
+ *
+ * @uninitialize: Destroys all the device queue manager resources allocated in
+ * initialize routine.
+ *
+ * @create_kernel_queue: Creates kernel queue. Used for debug queue.
+ *
+ * @destroy_kernel_queue: Destroys kernel queue. Used for debug queue.
+ *
+ * @set_cache_memory_policy: Sets memory policy (cached/ non cached) for the
+ * memory apertures.
+ *
+ * This struct is a base class for the kfd queues scheduler in the
+ * device level. The device base class should expose the basic operations
+ * for queue creation and queue destruction. This base class hides the
+ * scheduling mode of the driver and the specific implementation of the
+ * concrete device. This class is the only class in the queues scheduler
+ * that configures the H/W.
+ */
+
+struct device_queue_manager {
+	int	(*create_queue)(struct device_queue_manager *dqm,
+				struct queue *q,
+				struct qcm_process_device *qpd,
+				int *allocate_vmid);
+	int	(*destroy_queue)(struct device_queue_manager *dqm,
+				struct qcm_process_device *qpd,
+				struct queue *q);
+	int	(*update_queue)(struct device_queue_manager *dqm,
+				struct queue *q);
+
+	struct mqd_manager * (*get_mqd_manager)
+					(struct device_queue_manager *dqm,
+					enum KFD_MQD_TYPE type);
+
+	int	(*register_process)(struct device_queue_manager *dqm,
+					struct qcm_process_device *qpd);
+	int	(*unregister_process)(struct device_queue_manager *dqm,
+					struct qcm_process_device *qpd);
+	int	(*initialize)(struct device_queue_manager *dqm);
+	int	(*start)(struct device_queue_manager *dqm);
+	int	(*stop)(struct device_queue_manager *dqm);
+	void	(*uninitialize)(struct device_queue_manager *dqm);
+	int	(*create_kernel_queue)(struct device_queue_manager *dqm,
+					struct kernel_queue *kq,
+					struct qcm_process_device *qpd);
+	void	(*destroy_kernel_queue)(struct device_queue_manager *dqm,
+					struct kernel_queue *kq,
+					struct qcm_process_device *qpd);
+	bool	(*set_cache_memory_policy)(struct device_queue_manager *dqm,
+					   struct qcm_process_device *qpd,
+					   enum cache_policy default_policy,
+					   enum cache_policy alternate_policy,
+					   void __user *alternate_aperture_base,
+					   uint64_t alternate_aperture_size);
+
+
+	struct mqd_manager	*mqds[KFD_MQD_TYPE_MAX];
+	struct packet_manager	packets;
+	struct kfd_dev		*dev;
+	struct mutex		lock;
+	struct list_head	queues;
+	unsigned int		processes_count;
+	unsigned int		queue_count;
+	unsigned int		next_pipe_to_allocate;
+	unsigned int		*allocated_queues;
+	unsigned int		vmid_bitmap;
+	uint64_t		pipelines_addr;
+	struct kfd_mem_obj	*pipeline_mem;
+	uint64_t		fence_gpu_addr;
+	unsigned int		*fence_addr;
+	struct kfd_mem_obj	*fence_mem;
+	bool			active_runlist;
+};
+
+
+
+#endif /* KFD_DEVICE_QUEUE_MANAGER_H_ */
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_doorbell.c b/drivers/gpu/drm/amd/amdkfd/kfd_doorbell.c
new file mode 100644
index 000000000000..b5791a5c7c06
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_doorbell.c
@@ -0,0 +1,256 @@
+/*
+ * Copyright 2014 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.
+ */
+#include "kfd_priv.h"
+#include <linux/mm.h>
+#include <linux/mman.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+
+/*
+ * This extension supports a kernel level doorbells management for
+ * the kernel queues.
+ * Basically the last doorbells page is devoted to kernel queues
+ * and that's assures that any user process won't get access to the
+ * kernel doorbells page
+ */
+static DEFINE_MUTEX(doorbell_mutex);
+static unsigned long doorbell_available_index[
+	DIV_ROUND_UP(KFD_MAX_NUM_OF_QUEUES_PER_PROCESS, BITS_PER_LONG)] = { 0 };
+
+#define KERNEL_DOORBELL_PASID 1
+#define KFD_SIZE_OF_DOORBELL_IN_BYTES 4
+
+/*
+ * Each device exposes a doorbell aperture, a PCI MMIO aperture that
+ * receives 32-bit writes that are passed to queues as wptr values.
+ * The doorbells are intended to be written by applications as part
+ * of queueing work on user-mode queues.
+ * We assign doorbells to applications in PAGE_SIZE-sized and aligned chunks.
+ * We map the doorbell address space into user-mode when a process creates
+ * its first queue on each device.
+ * Although the mapping is done by KFD, it is equivalent to an mmap of
+ * the /dev/kfd with the particular device encoded in the mmap offset.
+ * There will be other uses for mmap of /dev/kfd, so only a range of
+ * offsets (KFD_MMAP_DOORBELL_START-END) is used for doorbells.
+ */
+
+/* # of doorbell bytes allocated for each process. */
+static inline size_t doorbell_process_allocation(void)
+{
+	return roundup(KFD_SIZE_OF_DOORBELL_IN_BYTES *
+			KFD_MAX_NUM_OF_QUEUES_PER_PROCESS,
+			PAGE_SIZE);
+}
+
+/* Doorbell calculations for device init. */
+void kfd_doorbell_init(struct kfd_dev *kfd)
+{
+	size_t doorbell_start_offset;
+	size_t doorbell_aperture_size;
+	size_t doorbell_process_limit;
+
+	/*
+	 * We start with calculations in bytes because the input data might
+	 * only be byte-aligned.
+	 * Only after we have done the rounding can we assume any alignment.
+	 */
+
+	doorbell_start_offset =
+			roundup(kfd->shared_resources.doorbell_start_offset,
+					doorbell_process_allocation());
+
+	doorbell_aperture_size =
+			rounddown(kfd->shared_resources.doorbell_aperture_size,
+					doorbell_process_allocation());
+
+	if (doorbell_aperture_size > doorbell_start_offset)
+		doorbell_process_limit =
+			(doorbell_aperture_size - doorbell_start_offset) /
+						doorbell_process_allocation();
+	else
+		doorbell_process_limit = 0;
+
+	kfd->doorbell_base = kfd->shared_resources.doorbell_physical_address +
+				doorbell_start_offset;
+
+	kfd->doorbell_id_offset = doorbell_start_offset / sizeof(u32);
+	kfd->doorbell_process_limit = doorbell_process_limit - 1;
+
+	kfd->doorbell_kernel_ptr = ioremap(kfd->doorbell_base,
+						doorbell_process_allocation());
+
+	BUG_ON(!kfd->doorbell_kernel_ptr);
+
+	pr_debug("kfd: doorbell initialization:\n");
+	pr_debug("kfd: doorbell base           == 0x%08lX\n",
+			(uintptr_t)kfd->doorbell_base);
+
+	pr_debug("kfd: doorbell_id_offset      == 0x%08lX\n",
+			kfd->doorbell_id_offset);
+
+	pr_debug("kfd: doorbell_process_limit  == 0x%08lX\n",
+			doorbell_process_limit);
+
+	pr_debug("kfd: doorbell_kernel_offset  == 0x%08lX\n",
+			(uintptr_t)kfd->doorbell_base);
+
+	pr_debug("kfd: doorbell aperture size  == 0x%08lX\n",
+			kfd->shared_resources.doorbell_aperture_size);
+
+	pr_debug("kfd: doorbell kernel address == 0x%08lX\n",
+			(uintptr_t)kfd->doorbell_kernel_ptr);
+}
+
+int kfd_doorbell_mmap(struct kfd_process *process, struct vm_area_struct *vma)
+{
+	phys_addr_t address;
+	struct kfd_dev *dev;
+
+	/*
+	 * For simplicitly we only allow mapping of the entire doorbell
+	 * allocation of a single device & process.
+	 */
+	if (vma->vm_end - vma->vm_start != doorbell_process_allocation())
+		return -EINVAL;
+
+	/* Find kfd device according to gpu id */
+	dev = kfd_device_by_id(vma->vm_pgoff);
+	if (dev == NULL)
+		return -EINVAL;
+
+	/* Find if pdd exists for combination of process and gpu id */
+	if (!kfd_get_process_device_data(dev, process, 0))
+		return -EINVAL;
+
+	/* Calculate physical address of doorbell */
+	address = kfd_get_process_doorbells(dev, process);
+
+	vma->vm_flags |= VM_IO | VM_DONTCOPY | VM_DONTEXPAND | VM_NORESERVE |
+				VM_DONTDUMP | VM_PFNMAP;
+
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	pr_debug("kfd: mapping doorbell page in kfd_doorbell_mmap\n"
+		 "     target user address == 0x%08llX\n"
+		 "     physical address    == 0x%08llX\n"
+		 "     vm_flags            == 0x%04lX\n"
+		 "     size                == 0x%04lX\n",
+		 (unsigned long long) vma->vm_start, address, vma->vm_flags,
+		 doorbell_process_allocation());
+
+
+	return io_remap_pfn_range(vma,
+				vma->vm_start,
+				address >> PAGE_SHIFT,
+				doorbell_process_allocation(),
+				vma->vm_page_prot);
+}
+
+
+/* get kernel iomem pointer for a doorbell */
+u32 __iomem *kfd_get_kernel_doorbell(struct kfd_dev *kfd,
+					unsigned int *doorbell_off)
+{
+	u32 inx;
+
+	BUG_ON(!kfd || !doorbell_off);
+
+	mutex_lock(&doorbell_mutex);
+	inx = find_first_zero_bit(doorbell_available_index,
+					KFD_MAX_NUM_OF_QUEUES_PER_PROCESS);
+
+	__set_bit(inx, doorbell_available_index);
+	mutex_unlock(&doorbell_mutex);
+
+	if (inx >= KFD_MAX_NUM_OF_QUEUES_PER_PROCESS)
+		return NULL;
+
+	/*
+	 * Calculating the kernel doorbell offset using "faked" kernel
+	 * pasid that allocated for kernel queues only
+	 */
+	*doorbell_off = KERNEL_DOORBELL_PASID * (doorbell_process_allocation() /
+							sizeof(u32)) + inx;
+
+	pr_debug("kfd: get kernel queue doorbell\n"
+			 "     doorbell offset   == 0x%08d\n"
+			 "     kernel address    == 0x%08lX\n",
+		*doorbell_off, (uintptr_t)(kfd->doorbell_kernel_ptr + inx));
+
+	return kfd->doorbell_kernel_ptr + inx;
+}
+
+void kfd_release_kernel_doorbell(struct kfd_dev *kfd, u32 __iomem *db_addr)
+{
+	unsigned int inx;
+
+	BUG_ON(!kfd || !db_addr);
+
+	inx = (unsigned int)(db_addr - kfd->doorbell_kernel_ptr);
+
+	mutex_lock(&doorbell_mutex);
+	__clear_bit(inx, doorbell_available_index);
+	mutex_unlock(&doorbell_mutex);
+}
+
+inline void write_kernel_doorbell(u32 __iomem *db, u32 value)
+{
+	if (db) {
+		writel(value, db);
+		pr_debug("writing %d to doorbell address 0x%p\n", value, db);
+	}
+}
+
+/*
+ * queue_ids are in the range [0,MAX_PROCESS_QUEUES) and are mapped 1:1
+ * to doorbells with the process's doorbell page
+ */
+unsigned int kfd_queue_id_to_doorbell(struct kfd_dev *kfd,
+					struct kfd_process *process,
+					unsigned int queue_id)
+{
+	/*
+	 * doorbell_id_offset accounts for doorbells taken by KGD.
+	 * pasid * doorbell_process_allocation/sizeof(u32) adjusts
+	 * to the process's doorbells
+	 */
+	return kfd->doorbell_id_offset +
+		process->pasid * (doorbell_process_allocation()/sizeof(u32)) +
+		queue_id;
+}
+
+uint64_t kfd_get_number_elems(struct kfd_dev *kfd)
+{
+	uint64_t num_of_elems = (kfd->shared_resources.doorbell_aperture_size -
+				kfd->shared_resources.doorbell_start_offset) /
+					doorbell_process_allocation() + 1;
+
+	return num_of_elems;
+
+}
+
+phys_addr_t kfd_get_process_doorbells(struct kfd_dev *dev,
+					struct kfd_process *process)
+{
+	return dev->doorbell_base +
+		process->pasid * doorbell_process_allocation();
+}
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_flat_memory.c b/drivers/gpu/drm/amd/amdkfd/kfd_flat_memory.c
new file mode 100644
index 000000000000..e64aa99e5e41
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_flat_memory.c
@@ -0,0 +1,354 @@
+/*
+ * Copyright 2014 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.
+ *
+ */
+
+#include <linux/device.h>
+#include <linux/export.h>
+#include <linux/err.h>
+#include <linux/fs.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/compat.h>
+#include <uapi/linux/kfd_ioctl.h>
+#include <linux/time.h>
+#include "kfd_priv.h"
+#include <linux/mm.h>
+#include <uapi/asm-generic/mman-common.h>
+#include <asm/processor.h>
+
+/*
+ * The primary memory I/O features being added for revisions of gfxip
+ * beyond 7.0 (Kaveri) are:
+ *
+ * Access to ATC/IOMMU mapped memory w/ associated extension of VA to 48b
+ *
+ * “Flat” shader memory access – These are new shader vector memory
+ * operations that do not reference a T#/V# so a “pointer” is what is
+ * sourced from the vector gprs for direct access to memory.
+ * This pointer space has the Shared(LDS) and Private(Scratch) memory
+ * mapped into this pointer space as apertures.
+ * The hardware then determines how to direct the memory request
+ * based on what apertures the request falls in.
+ *
+ * Unaligned support and alignment check
+ *
+ *
+ * System Unified Address - SUA
+ *
+ * The standard usage for GPU virtual addresses are that they are mapped by
+ * a set of page tables we call GPUVM and these page tables are managed by
+ * a combination of vidMM/driver software components.  The current virtual
+ * address (VA) range for GPUVM is 40b.
+ *
+ * As of gfxip7.1 and beyond we’re adding the ability for compute memory
+ * clients (CP/RLC, DMA, SHADER(ifetch, scalar, and vector ops)) to access
+ * the same page tables used by host x86 processors and that are managed by
+ * the operating system. This is via a technique and hardware called ATC/IOMMU.
+ * The GPU has the capability of accessing both the GPUVM and ATC address
+ * spaces for a given VMID (process) simultaneously and we call this feature
+ * system unified address (SUA).
+ *
+ * There are three fundamental address modes of operation for a given VMID
+ * (process) on the GPU:
+ *
+ *	HSA64 – 64b pointers and the default address space is ATC
+ *	HSA32 – 32b pointers and the default address space is ATC
+ *	GPUVM – 64b pointers and the default address space is GPUVM (driver
+ *		model mode)
+ *
+ *
+ * HSA64 - ATC/IOMMU 64b
+ *
+ * A 64b pointer in the AMD64/IA64 CPU architecture is not fully utilized
+ * by the CPU so an AMD CPU can only access the high area
+ * (VA[63:47] == 0x1FFFF) and low area (VA[63:47 == 0) of the address space
+ * so the actual VA carried to translation is 48b.  There is a “hole” in
+ * the middle of the 64b VA space.
+ *
+ * The GPU not only has access to all of the CPU accessible address space via
+ * ATC/IOMMU, but it also has access to the GPUVM address space.  The “system
+ * unified address” feature (SUA) is the mapping of GPUVM and ATC address
+ * spaces into a unified pointer space.  The method we take for 64b mode is
+ * to map the full 40b GPUVM address space into the hole of the 64b address
+ * space.
+
+ * The GPUVM_Base/GPUVM_Limit defines the aperture in the 64b space where we
+ * direct requests to be translated via GPUVM page tables instead of the
+ * IOMMU path.
+ *
+ *
+ * 64b to 49b Address conversion
+ *
+ * Note that there are still significant portions of unused regions (holes)
+ * in the 64b address space even for the GPU.  There are several places in
+ * the pipeline (sw and hw), we wish to compress the 64b virtual address
+ * to a 49b address.  This 49b address is constituted of an “ATC” bit
+ * plus a 48b virtual address.  This 49b address is what is passed to the
+ * translation hardware.  ATC==0 means the 48b address is a GPUVM address
+ * (max of 2^40 – 1) intended to be translated via GPUVM page tables.
+ * ATC==1 means the 48b address is intended to be translated via IOMMU
+ * page tables.
+ *
+ * A 64b pointer is compared to the apertures that are defined (Base/Limit), in
+ * this case the GPUVM aperture (red) is defined and if a pointer falls in this
+ * aperture, we subtract the GPUVM_Base address and set the ATC bit to zero
+ * as part of the 64b to 49b conversion.
+ *
+ * Where this 64b to 49b conversion is done is a function of the usage.
+ * Most GPU memory access is via memory objects where the driver builds
+ * a descriptor which consists of a base address and a memory access by
+ * the GPU usually consists of some kind of an offset or Cartesian coordinate
+ * that references this memory descriptor.  This is the case for shader
+ * instructions that reference the T# or V# constants, or for specified
+ * locations of assets (ex. the shader program location).  In these cases
+ * the driver is what handles the 64b to 49b conversion and the base
+ * address in the descriptor (ex. V# or T# or shader program location)
+ * is defined as a 48b address w/ an ATC bit.  For this usage a given
+ * memory object cannot straddle multiple apertures in the 64b address
+ * space. For example a shader program cannot jump in/out between ATC
+ * and GPUVM space.
+ *
+ * In some cases we wish to pass a 64b pointer to the GPU hardware and
+ * the GPU hw does the 64b to 49b conversion before passing memory
+ * requests to the cache/memory system.  This is the case for the
+ * S_LOAD and FLAT_* shader memory instructions where we have 64b pointers
+ * in scalar and vector GPRs respectively.
+ *
+ * In all cases (no matter where the 64b -> 49b conversion is done), the gfxip
+ * hardware sends a 48b address along w/ an ATC bit, to the memory controller
+ * on the memory request interfaces.
+ *
+ *	<client>_MC_rdreq_atc   // read request ATC bit
+ *
+ *		0 : <client>_MC_rdreq_addr is a GPUVM VA
+ *
+ *		1 : <client>_MC_rdreq_addr is a ATC VA
+ *
+ *
+ * “Spare” aperture (APE1)
+ *
+ * We use the GPUVM aperture to differentiate ATC vs. GPUVM, but we also use
+ * apertures to set the Mtype field for S_LOAD/FLAT_* ops which is input to the
+ * config tables for setting cache policies. The “spare” (APE1) aperture is
+ * motivated by getting a different Mtype from the default.
+ * The default aperture isn’t an actual base/limit aperture; it is just the
+ * address space that doesn’t hit any defined base/limit apertures.
+ * The following diagram is a complete picture of the gfxip7.x SUA apertures.
+ * The APE1 can be placed either below or above
+ * the hole (cannot be in the hole).
+ *
+ *
+ * General Aperture definitions and rules
+ *
+ * An aperture register definition consists of a Base, Limit, Mtype, and
+ * usually an ATC bit indicating which translation tables that aperture uses.
+ * In all cases (for SUA and DUA apertures discussed later), aperture base
+ * and limit definitions are 64KB aligned.
+ *
+ *	<ape>_Base[63:0] = { <ape>_Base_register[63:16], 0x0000 }
+ *
+ *	<ape>_Limit[63:0] = { <ape>_Limit_register[63:16], 0xFFFF }
+ *
+ * The base and limit are considered inclusive to an aperture so being
+ * inside an aperture means (address >= Base) AND (address <= Limit).
+ *
+ * In no case is a payload that straddles multiple apertures expected to work.
+ * For example a load_dword_x4 that starts in one aperture and ends in another,
+ * does not work.  For the vector FLAT_* ops we have detection capability in
+ * the shader for reporting a “memory violation” back to the
+ * SQ block for use in traps.
+ * A memory violation results when an op falls into the hole,
+ * or a payload straddles multiple apertures.  The S_LOAD instruction
+ * does not have this detection.
+ *
+ * Apertures cannot overlap.
+ *
+ *
+ *
+ * HSA32 - ATC/IOMMU 32b
+ *
+ * For HSA32 mode, the pointers are interpreted as 32 bits and use a single GPR
+ * instead of two for the S_LOAD and FLAT_* ops. The entire GPUVM space of 40b
+ * will not fit so there is only partial visibility to the GPUVM
+ * space (defined by the aperture) for S_LOAD and FLAT_* ops.
+ * There is no spare (APE1) aperture for HSA32 mode.
+ *
+ *
+ * GPUVM 64b mode (driver model)
+ *
+ * This mode is related to HSA64 in that the difference really is that
+ * the default aperture is GPUVM (ATC==0) and not ATC space.
+ * We have gfxip7.x hardware that has FLAT_* and S_LOAD support for
+ * SUA GPUVM mode, but does not support HSA32/HSA64.
+ *
+ *
+ * Device Unified Address - DUA
+ *
+ * Device unified address (DUA) is the name of the feature that maps the
+ * Shared(LDS) memory and Private(Scratch) memory into the overall address
+ * space for use by the new FLAT_* vector memory ops.  The Shared and
+ * Private memories are mapped as apertures into the address space,
+ * and the hardware detects when a FLAT_* memory request is to be redirected
+ * to the LDS or Scratch memory when it falls into one of these apertures.
+ * Like the SUA apertures, the Shared/Private apertures are 64KB aligned and
+ * the base/limit is “in” the aperture. For both HSA64 and GPUVM SUA modes,
+ * the Shared/Private apertures are always placed in a limited selection of
+ * options in the hole of the 64b address space. For HSA32 mode, the
+ * Shared/Private apertures can be placed anywhere in the 32b space
+ * except at 0.
+ *
+ *
+ * HSA64 Apertures for FLAT_* vector ops
+ *
+ * For HSA64 SUA mode, the Shared and Private apertures are always placed
+ * in the hole w/ a limited selection of possible locations. The requests
+ * that fall in the private aperture are expanded as a function of the
+ * work-item id (tid) and redirected to the location of the
+ * “hidden private memory”. The hidden private can be placed in either GPUVM
+ * or ATC space. The addresses that fall in the shared aperture are
+ * re-directed to the on-chip LDS memory hardware.
+ *
+ *
+ * HSA32 Apertures for FLAT_* vector ops
+ *
+ * In HSA32 mode, the Private and Shared apertures can be placed anywhere
+ * in the 32b space except at 0 (Private or Shared Base at zero disables
+ * the apertures). If the base address of the apertures are non-zero
+ * (ie apertures exists), the size is always 64KB.
+ *
+ *
+ * GPUVM Apertures for FLAT_* vector ops
+ *
+ * In GPUVM mode, the Shared/Private apertures are specified identically
+ * to HSA64 mode where they are always in the hole at a limited selection
+ * of locations.
+ *
+ *
+ * Aperture Definitions for SUA and DUA
+ *
+ * The interpretation of the aperture register definitions for a given
+ * VMID is a function of the “SUA Mode” which is one of HSA64, HSA32, or
+ * GPUVM64 discussed in previous sections. The mode is first decoded, and
+ * then the remaining register decode is a function of the mode.
+ *
+ *
+ * SUA Mode Decode
+ *
+ * For the S_LOAD and FLAT_* shader operations, the SUA mode is decoded from
+ * the COMPUTE_DISPATCH_INITIATOR:DATA_ATC bit and
+ * the SH_MEM_CONFIG:PTR32 bits.
+ *
+ * COMPUTE_DISPATCH_INITIATOR:DATA_ATC    SH_MEM_CONFIG:PTR32        Mode
+ *
+ * 1                                              0                  HSA64
+ *
+ * 1                                              1                  HSA32
+ *
+ * 0                                              X                 GPUVM64
+ *
+ * In general the hardware will ignore the PTR32 bit and treat
+ * as “0” whenever DATA_ATC = “0”, but sw should set PTR32=0
+ * when DATA_ATC=0.
+ *
+ * The DATA_ATC bit is only set for compute dispatches.
+ * All “Draw” dispatches are hardcoded to GPUVM64 mode
+ * for FLAT_* / S_LOAD operations.
+ */
+
+#define MAKE_GPUVM_APP_BASE(gpu_num) \
+	(((uint64_t)(gpu_num) << 61) + 0x1000000000000L)
+
+#define MAKE_GPUVM_APP_LIMIT(base) \
+	(((uint64_t)(base) & \
+		0xFFFFFF0000000000UL) | 0xFFFFFFFFFFL)
+
+#define MAKE_SCRATCH_APP_BASE(gpu_num) \
+	(((uint64_t)(gpu_num) << 61) + 0x100000000L)
+
+#define MAKE_SCRATCH_APP_LIMIT(base) \
+	(((uint64_t)base & 0xFFFFFFFF00000000UL) | 0xFFFFFFFF)
+
+#define MAKE_LDS_APP_BASE(gpu_num) \
+	(((uint64_t)(gpu_num) << 61) + 0x0)
+#define MAKE_LDS_APP_LIMIT(base) \
+	(((uint64_t)(base) & 0xFFFFFFFF00000000UL) | 0xFFFFFFFF)
+
+int kfd_init_apertures(struct kfd_process *process)
+{
+	uint8_t id  = 0;
+	struct kfd_dev *dev;
+	struct kfd_process_device *pdd;
+
+	/*Iterating over all devices*/
+	while ((dev = kfd_topology_enum_kfd_devices(id)) != NULL &&
+		id < NUM_OF_SUPPORTED_GPUS) {
+
+		pdd = kfd_get_process_device_data(dev, process, 1);
+		if (!pdd)
+			return -1;
+
+		/*
+		 * For 64 bit process aperture will be statically reserved in
+		 * the x86_64 non canonical process address space
+		 * amdkfd doesn't currently support apertures for 32 bit process
+		 */
+		if (process->is_32bit_user_mode) {
+			pdd->lds_base = pdd->lds_limit = 0;
+			pdd->gpuvm_base = pdd->gpuvm_limit = 0;
+			pdd->scratch_base = pdd->scratch_limit = 0;
+		} else {
+			/*
+			 * node id couldn't be 0 - the three MSB bits of
+			 * aperture shoudn't be 0
+			 */
+			pdd->lds_base = MAKE_LDS_APP_BASE(id + 1);
+
+			pdd->lds_limit = MAKE_LDS_APP_LIMIT(pdd->lds_base);
+
+			pdd->gpuvm_base = MAKE_GPUVM_APP_BASE(id + 1);
+
+			pdd->gpuvm_limit =
+					MAKE_GPUVM_APP_LIMIT(pdd->gpuvm_base);
+
+			pdd->scratch_base = MAKE_SCRATCH_APP_BASE(id + 1);
+
+			pdd->scratch_limit =
+				MAKE_SCRATCH_APP_LIMIT(pdd->scratch_base);
+		}
+
+		dev_dbg(kfd_device, "node id %u\n", id);
+		dev_dbg(kfd_device, "gpu id %u\n", pdd->dev->id);
+		dev_dbg(kfd_device, "lds_base %llX\n", pdd->lds_base);
+		dev_dbg(kfd_device, "lds_limit %llX\n", pdd->lds_limit);
+		dev_dbg(kfd_device, "gpuvm_base %llX\n", pdd->gpuvm_base);
+		dev_dbg(kfd_device, "gpuvm_limit %llX\n", pdd->gpuvm_limit);
+		dev_dbg(kfd_device, "scratch_base %llX\n", pdd->scratch_base);
+		dev_dbg(kfd_device, "scratch_limit %llX\n", pdd->scratch_limit);
+
+		id++;
+	}
+
+	return 0;
+}
+
+
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_interrupt.c b/drivers/gpu/drm/amd/amdkfd/kfd_interrupt.c
new file mode 100644
index 000000000000..5b999095a1f7
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_interrupt.c
@@ -0,0 +1,176 @@
+/*
+ * Copyright 2014 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.
+ */
+
+/*
+ * KFD Interrupts.
+ *
+ * AMD GPUs deliver interrupts by pushing an interrupt description onto the
+ * interrupt ring and then sending an interrupt. KGD receives the interrupt
+ * in ISR and sends us a pointer to each new entry on the interrupt ring.
+ *
+ * We generally can't process interrupt-signaled events from ISR, so we call
+ * out to each interrupt client module (currently only the scheduler) to ask if
+ * each interrupt is interesting. If they return true, then it requires further
+ * processing so we copy it to an internal interrupt ring and call each
+ * interrupt client again from a work-queue.
+ *
+ * There's no acknowledgment for the interrupts we use. The hardware simply
+ * queues a new interrupt each time without waiting.
+ *
+ * The fixed-size internal queue means that it's possible for us to lose
+ * interrupts because we have no back-pressure to the hardware.
+ */
+
+#include <linux/slab.h>
+#include <linux/device.h>
+#include "kfd_priv.h"
+
+#define KFD_INTERRUPT_RING_SIZE 256
+
+static void interrupt_wq(struct work_struct *);
+
+int kfd_interrupt_init(struct kfd_dev *kfd)
+{
+	void *interrupt_ring = kmalloc_array(KFD_INTERRUPT_RING_SIZE,
+					kfd->device_info->ih_ring_entry_size,
+					GFP_KERNEL);
+	if (!interrupt_ring)
+		return -ENOMEM;
+
+	kfd->interrupt_ring = interrupt_ring;
+	kfd->interrupt_ring_size =
+		KFD_INTERRUPT_RING_SIZE * kfd->device_info->ih_ring_entry_size;
+	atomic_set(&kfd->interrupt_ring_wptr, 0);
+	atomic_set(&kfd->interrupt_ring_rptr, 0);
+
+	spin_lock_init(&kfd->interrupt_lock);
+
+	INIT_WORK(&kfd->interrupt_work, interrupt_wq);
+
+	kfd->interrupts_active = true;
+
+	/*
+	 * After this function returns, the interrupt will be enabled. This
+	 * barrier ensures that the interrupt running on a different processor
+	 * sees all the above writes.
+	 */
+	smp_wmb();
+
+	return 0;
+}
+
+void kfd_interrupt_exit(struct kfd_dev *kfd)
+{
+	/*
+	 * Stop the interrupt handler from writing to the ring and scheduling
+	 * workqueue items. The spinlock ensures that any interrupt running
+	 * after we have unlocked sees interrupts_active = false.
+	 */
+	unsigned long flags;
+
+	spin_lock_irqsave(&kfd->interrupt_lock, flags);
+	kfd->interrupts_active = false;
+	spin_unlock_irqrestore(&kfd->interrupt_lock, flags);
+
+	/*
+	 * Flush_scheduled_work ensures that there are no outstanding
+	 * work-queue items that will access interrupt_ring. New work items
+	 * can't be created because we stopped interrupt handling above.
+	 */
+	flush_scheduled_work();
+
+	kfree(kfd->interrupt_ring);
+}
+
+/*
+ * This assumes that it can't be called concurrently with itself
+ * but only with dequeue_ih_ring_entry.
+ */
+bool enqueue_ih_ring_entry(struct kfd_dev *kfd,	const void *ih_ring_entry)
+{
+	unsigned int rptr = atomic_read(&kfd->interrupt_ring_rptr);
+	unsigned int wptr = atomic_read(&kfd->interrupt_ring_wptr);
+
+	if ((rptr - wptr) % kfd->interrupt_ring_size ==
+					kfd->device_info->ih_ring_entry_size) {
+		/* This is very bad, the system is likely to hang. */
+		dev_err_ratelimited(kfd_chardev(),
+			"Interrupt ring overflow, dropping interrupt.\n");
+		return false;
+	}
+
+	memcpy(kfd->interrupt_ring + wptr, ih_ring_entry,
+			kfd->device_info->ih_ring_entry_size);
+
+	wptr = (wptr + kfd->device_info->ih_ring_entry_size) %
+			kfd->interrupt_ring_size;
+	smp_wmb(); /* Ensure memcpy'd data is visible before wptr update. */
+	atomic_set(&kfd->interrupt_ring_wptr, wptr);
+
+	return true;
+}
+
+/*
+ * This assumes that it can't be called concurrently with itself
+ * but only with enqueue_ih_ring_entry.
+ */
+static bool dequeue_ih_ring_entry(struct kfd_dev *kfd, void *ih_ring_entry)
+{
+	/*
+	 * Assume that wait queues have an implicit barrier, i.e. anything that
+	 * happened in the ISR before it queued work is visible.
+	 */
+
+	unsigned int wptr = atomic_read(&kfd->interrupt_ring_wptr);
+	unsigned int rptr = atomic_read(&kfd->interrupt_ring_rptr);
+
+	if (rptr == wptr)
+		return false;
+
+	memcpy(ih_ring_entry, kfd->interrupt_ring + rptr,
+			kfd->device_info->ih_ring_entry_size);
+
+	rptr = (rptr + kfd->device_info->ih_ring_entry_size) %
+			kfd->interrupt_ring_size;
+
+	/*
+	 * Ensure the rptr write update is not visible until
+	 * memcpy has finished reading.
+	 */
+	smp_mb();
+	atomic_set(&kfd->interrupt_ring_rptr, rptr);
+
+	return true;
+}
+
+static void interrupt_wq(struct work_struct *work)
+{
+	struct kfd_dev *dev = container_of(work, struct kfd_dev,
+						interrupt_work);
+
+	uint32_t ih_ring_entry[DIV_ROUND_UP(
+				dev->device_info->ih_ring_entry_size,
+				sizeof(uint32_t))];
+
+	while (dequeue_ih_ring_entry(dev, ih_ring_entry))
+		;
+}
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_kernel_queue.c b/drivers/gpu/drm/amd/amdkfd/kfd_kernel_queue.c
new file mode 100644
index 000000000000..935071410724
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_kernel_queue.c
@@ -0,0 +1,353 @@
+/*
+ * Copyright 2014 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.
+ *
+ */
+
+#include <linux/types.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/printk.h>
+#include <linux/sched.h>
+#include "kfd_kernel_queue.h"
+#include "kfd_priv.h"
+#include "kfd_device_queue_manager.h"
+#include "kfd_pm4_headers.h"
+#include "kfd_pm4_opcodes.h"
+
+#define PM4_COUNT_ZERO (((1 << 15) - 1) << 16)
+
+static bool initialize(struct kernel_queue *kq, struct kfd_dev *dev,
+		enum kfd_queue_type type, unsigned int queue_size)
+{
+	struct queue_properties prop;
+	int retval;
+	union PM4_MES_TYPE_3_HEADER nop;
+
+	BUG_ON(!kq || !dev);
+	BUG_ON(type != KFD_QUEUE_TYPE_DIQ && type != KFD_QUEUE_TYPE_HIQ);
+
+	pr_debug("kfd: In func %s initializing queue type %d size %d\n",
+			__func__, KFD_QUEUE_TYPE_HIQ, queue_size);
+
+	nop.opcode = IT_NOP;
+	nop.type = PM4_TYPE_3;
+	nop.u32all |= PM4_COUNT_ZERO;
+
+	kq->dev = dev;
+	kq->nop_packet = nop.u32all;
+	switch (type) {
+	case KFD_QUEUE_TYPE_DIQ:
+	case KFD_QUEUE_TYPE_HIQ:
+		kq->mqd = dev->dqm->get_mqd_manager(dev->dqm,
+						KFD_MQD_TYPE_CIK_HIQ);
+		break;
+	default:
+		BUG();
+		break;
+	}
+
+	if (kq->mqd == NULL)
+		return false;
+
+	prop.doorbell_ptr = kfd_get_kernel_doorbell(dev, &prop.doorbell_off);
+
+	if (prop.doorbell_ptr == NULL)
+		goto err_get_kernel_doorbell;
+
+	retval = kfd2kgd->allocate_mem(dev->kgd,
+					queue_size,
+					PAGE_SIZE,
+					KFD_MEMPOOL_SYSTEM_WRITECOMBINE,
+					(struct kgd_mem **) &kq->pq);
+
+	if (retval != 0)
+		goto err_pq_allocate_vidmem;
+
+	kq->pq_kernel_addr = kq->pq->cpu_ptr;
+	kq->pq_gpu_addr = kq->pq->gpu_addr;
+
+	retval = kfd2kgd->allocate_mem(dev->kgd,
+					sizeof(*kq->rptr_kernel),
+					32,
+					KFD_MEMPOOL_SYSTEM_WRITECOMBINE,
+					(struct kgd_mem **) &kq->rptr_mem);
+
+	if (retval != 0)
+		goto err_rptr_allocate_vidmem;
+
+	kq->rptr_kernel = kq->rptr_mem->cpu_ptr;
+	kq->rptr_gpu_addr = kq->rptr_mem->gpu_addr;
+
+	retval = kfd2kgd->allocate_mem(dev->kgd,
+					sizeof(*kq->wptr_kernel),
+					32,
+					KFD_MEMPOOL_SYSTEM_WRITECOMBINE,
+					(struct kgd_mem **) &kq->wptr_mem);
+
+	if (retval != 0)
+		goto err_wptr_allocate_vidmem;
+
+	kq->wptr_kernel = kq->wptr_mem->cpu_ptr;
+	kq->wptr_gpu_addr = kq->wptr_mem->gpu_addr;
+
+	memset(kq->pq_kernel_addr, 0, queue_size);
+	memset(kq->rptr_kernel, 0, sizeof(*kq->rptr_kernel));
+	memset(kq->wptr_kernel, 0, sizeof(*kq->wptr_kernel));
+
+	prop.queue_size = queue_size;
+	prop.is_interop = false;
+	prop.priority = 1;
+	prop.queue_percent = 100;
+	prop.type = type;
+	prop.vmid = 0;
+	prop.queue_address = kq->pq_gpu_addr;
+	prop.read_ptr = (uint32_t *) kq->rptr_gpu_addr;
+	prop.write_ptr = (uint32_t *) kq->wptr_gpu_addr;
+
+	if (init_queue(&kq->queue, prop) != 0)
+		goto err_init_queue;
+
+	kq->queue->device = dev;
+	kq->queue->process = kfd_get_process(current);
+
+	retval = kq->mqd->init_mqd(kq->mqd, &kq->queue->mqd,
+					&kq->queue->mqd_mem_obj,
+					&kq->queue->gart_mqd_addr,
+					&kq->queue->properties);
+	if (retval != 0)
+		goto err_init_mqd;
+
+	/* assign HIQ to HQD */
+	if (type == KFD_QUEUE_TYPE_HIQ) {
+		pr_debug("assigning hiq to hqd\n");
+		kq->queue->pipe = KFD_CIK_HIQ_PIPE;
+		kq->queue->queue = KFD_CIK_HIQ_QUEUE;
+		kq->mqd->load_mqd(kq->mqd, kq->queue->mqd, kq->queue->pipe,
+					kq->queue->queue, NULL);
+	} else {
+		/* allocate fence for DIQ */
+
+		retval = kfd2kgd->allocate_mem(dev->kgd,
+					sizeof(uint32_t),
+					32,
+					KFD_MEMPOOL_SYSTEM_WRITECOMBINE,
+					(struct kgd_mem **) &kq->fence_mem_obj);
+
+		if (retval != 0)
+			goto err_alloc_fence;
+
+		kq->fence_kernel_address = kq->fence_mem_obj->cpu_ptr;
+		kq->fence_gpu_addr = kq->fence_mem_obj->gpu_addr;
+	}
+
+	print_queue(kq->queue);
+
+	return true;
+err_alloc_fence:
+err_init_mqd:
+	uninit_queue(kq->queue);
+err_init_queue:
+	kfd2kgd->free_mem(dev->kgd, (struct kgd_mem *) kq->wptr_mem);
+err_wptr_allocate_vidmem:
+	kfd2kgd->free_mem(dev->kgd, (struct kgd_mem *) kq->rptr_mem);
+err_rptr_allocate_vidmem:
+	kfd2kgd->free_mem(dev->kgd, (struct kgd_mem *) kq->pq);
+err_pq_allocate_vidmem:
+	pr_err("kfd: error init pq\n");
+	kfd_release_kernel_doorbell(dev, prop.doorbell_ptr);
+err_get_kernel_doorbell:
+	pr_err("kfd: error init doorbell");
+	return false;
+
+}
+
+static void uninitialize(struct kernel_queue *kq)
+{
+	BUG_ON(!kq);
+
+	if (kq->queue->properties.type == KFD_QUEUE_TYPE_HIQ)
+		kq->mqd->destroy_mqd(kq->mqd,
+					NULL,
+					false,
+					QUEUE_PREEMPT_DEFAULT_TIMEOUT_MS,
+					kq->queue->pipe,
+					kq->queue->queue);
+
+	kfd2kgd->free_mem(kq->dev->kgd, (struct kgd_mem *) kq->rptr_mem);
+	kfd2kgd->free_mem(kq->dev->kgd, (struct kgd_mem *) kq->wptr_mem);
+	kfd2kgd->free_mem(kq->dev->kgd, (struct kgd_mem *) kq->pq);
+	kfd_release_kernel_doorbell(kq->dev,
+					kq->queue->properties.doorbell_ptr);
+	uninit_queue(kq->queue);
+}
+
+static int acquire_packet_buffer(struct kernel_queue *kq,
+		size_t packet_size_in_dwords, unsigned int **buffer_ptr)
+{
+	size_t available_size;
+	size_t queue_size_dwords;
+	uint32_t wptr, rptr;
+	unsigned int *queue_address;
+
+	BUG_ON(!kq || !buffer_ptr);
+
+	rptr = *kq->rptr_kernel;
+	wptr = *kq->wptr_kernel;
+	queue_address = (unsigned int *)kq->pq_kernel_addr;
+	queue_size_dwords = kq->queue->properties.queue_size / sizeof(uint32_t);
+
+	pr_debug("kfd: In func %s\nrptr: %d\nwptr: %d\nqueue_address 0x%p\n",
+			__func__, rptr, wptr, queue_address);
+
+	available_size = (rptr - 1 - wptr + queue_size_dwords) %
+							queue_size_dwords;
+
+	if (packet_size_in_dwords >= queue_size_dwords ||
+			packet_size_in_dwords >= available_size) {
+		/*
+		 * make sure calling functions know
+		 * acquire_packet_buffer() failed
+		 */
+		*buffer_ptr = NULL;
+		return -ENOMEM;
+	}
+
+	if (wptr + packet_size_in_dwords >= queue_size_dwords) {
+		while (wptr > 0) {
+			queue_address[wptr] = kq->nop_packet;
+			wptr = (wptr + 1) % queue_size_dwords;
+		}
+	}
+
+	*buffer_ptr = &queue_address[wptr];
+	kq->pending_wptr = wptr + packet_size_in_dwords;
+
+	return 0;
+}
+
+static void submit_packet(struct kernel_queue *kq)
+{
+#ifdef DEBUG
+	int i;
+#endif
+
+	BUG_ON(!kq);
+
+#ifdef DEBUG
+	for (i = *kq->wptr_kernel; i < kq->pending_wptr; i++) {
+		pr_debug("0x%2X ", kq->pq_kernel_addr[i]);
+		if (i % 15 == 0)
+			pr_debug("\n");
+	}
+	pr_debug("\n");
+#endif
+
+	*kq->wptr_kernel = kq->pending_wptr;
+	write_kernel_doorbell(kq->queue->properties.doorbell_ptr,
+				kq->pending_wptr);
+}
+
+static int sync_with_hw(struct kernel_queue *kq, unsigned long timeout_ms)
+{
+	unsigned long org_timeout_ms;
+
+	BUG_ON(!kq);
+
+	org_timeout_ms = timeout_ms;
+	timeout_ms += jiffies * 1000 / HZ;
+	while (*kq->wptr_kernel != *kq->rptr_kernel) {
+		if (time_after(jiffies * 1000 / HZ, timeout_ms)) {
+			pr_err("kfd: kernel_queue %s timeout expired %lu\n",
+				__func__, org_timeout_ms);
+			pr_err("kfd: wptr: %d rptr: %d\n",
+				*kq->wptr_kernel, *kq->rptr_kernel);
+			return -ETIME;
+		}
+		schedule();
+	}
+
+	return 0;
+}
+
+static void rollback_packet(struct kernel_queue *kq)
+{
+	BUG_ON(!kq);
+	kq->pending_wptr = *kq->queue->properties.write_ptr;
+}
+
+struct kernel_queue *kernel_queue_init(struct kfd_dev *dev,
+					enum kfd_queue_type type)
+{
+	struct kernel_queue *kq;
+
+	BUG_ON(!dev);
+
+	kq = kzalloc(sizeof(struct kernel_queue), GFP_KERNEL);
+	if (!kq)
+		return NULL;
+
+	kq->initialize = initialize;
+	kq->uninitialize = uninitialize;
+	kq->acquire_packet_buffer = acquire_packet_buffer;
+	kq->submit_packet = submit_packet;
+	kq->sync_with_hw = sync_with_hw;
+	kq->rollback_packet = rollback_packet;
+
+	if (kq->initialize(kq, dev, type, KFD_KERNEL_QUEUE_SIZE) == false) {
+		pr_err("kfd: failed to init kernel queue\n");
+		kfree(kq);
+		return NULL;
+	}
+	return kq;
+}
+
+void kernel_queue_uninit(struct kernel_queue *kq)
+{
+	BUG_ON(!kq);
+
+	kq->uninitialize(kq);
+	kfree(kq);
+}
+
+static __attribute__((unused)) void test_kq(struct kfd_dev *dev)
+{
+	struct kernel_queue *kq;
+	uint32_t *buffer, i;
+	int retval;
+
+	BUG_ON(!dev);
+
+	pr_debug("kfd: starting kernel queue test\n");
+
+	kq = kernel_queue_init(dev, KFD_QUEUE_TYPE_HIQ);
+	BUG_ON(!kq);
+
+	retval = kq->acquire_packet_buffer(kq, 5, &buffer);
+	BUG_ON(retval != 0);
+	for (i = 0; i < 5; i++)
+		buffer[i] = kq->nop_packet;
+	kq->submit_packet(kq);
+	kq->sync_with_hw(kq, 1000);
+
+	pr_debug("kfd: ending kernel queue test\n");
+}
+
+
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_kernel_queue.h b/drivers/gpu/drm/amd/amdkfd/kfd_kernel_queue.h
new file mode 100644
index 000000000000..dcd2bdb68d44
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_kernel_queue.h
@@ -0,0 +1,69 @@
+/*
+ * Copyright 2014 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.
+ *
+ */
+
+#ifndef KFD_KERNEL_QUEUE_H_
+#define KFD_KERNEL_QUEUE_H_
+
+#include <linux/list.h>
+#include <linux/types.h>
+#include "kfd_priv.h"
+
+struct kernel_queue {
+	/* interface */
+	bool	(*initialize)(struct kernel_queue *kq, struct kfd_dev *dev,
+			enum kfd_queue_type type, unsigned int queue_size);
+	void	(*uninitialize)(struct kernel_queue *kq);
+	int	(*acquire_packet_buffer)(struct kernel_queue *kq,
+					size_t packet_size_in_dwords,
+					unsigned int **buffer_ptr);
+
+	void	(*submit_packet)(struct kernel_queue *kq);
+	int	(*sync_with_hw)(struct kernel_queue *kq,
+				unsigned long timeout_ms);
+	void	(*rollback_packet)(struct kernel_queue *kq);
+
+	/* data */
+	struct kfd_dev		*dev;
+	struct mqd_manager	*mqd;
+	struct queue		*queue;
+	uint32_t		pending_wptr;
+	unsigned int		nop_packet;
+
+	struct kfd_mem_obj	*rptr_mem;
+	uint32_t		*rptr_kernel;
+	uint64_t		rptr_gpu_addr;
+	struct kfd_mem_obj	*wptr_mem;
+	uint32_t		*wptr_kernel;
+	uint64_t		wptr_gpu_addr;
+	struct kfd_mem_obj	*pq;
+	uint64_t		pq_gpu_addr;
+	uint32_t		*pq_kernel_addr;
+
+	struct kfd_mem_obj	*fence_mem_obj;
+	uint64_t		fence_gpu_addr;
+	void			*fence_kernel_address;
+
+	struct list_head	list;
+};
+
+#endif /* KFD_KERNEL_QUEUE_H_ */
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_module.c b/drivers/gpu/drm/amd/amdkfd/kfd_module.c
new file mode 100644
index 000000000000..95d5af138e6e
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_module.c
@@ -0,0 +1,159 @@
+/*
+ * Copyright 2014 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.
+ */
+
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/moduleparam.h>
+#include <linux/device.h>
+#include "kfd_priv.h"
+
+#define KFD_DRIVER_AUTHOR	"AMD Inc. and others"
+
+#define KFD_DRIVER_DESC		"Standalone HSA driver for AMD's GPUs"
+#define KFD_DRIVER_DATE		"20141113"
+#define KFD_DRIVER_MAJOR	0
+#define KFD_DRIVER_MINOR	7
+#define KFD_DRIVER_PATCHLEVEL	0
+
+const struct kfd2kgd_calls *kfd2kgd;
+static const struct kgd2kfd_calls kgd2kfd = {
+	.exit		= kgd2kfd_exit,
+	.probe		= kgd2kfd_probe,
+	.device_init	= kgd2kfd_device_init,
+	.device_exit	= kgd2kfd_device_exit,
+	.interrupt	= kgd2kfd_interrupt,
+	.suspend	= kgd2kfd_suspend,
+	.resume		= kgd2kfd_resume,
+};
+
+int sched_policy = KFD_SCHED_POLICY_HWS;
+module_param(sched_policy, int, 0444);
+MODULE_PARM_DESC(sched_policy,
+	"Kernel cmdline parameter that defines the amdkfd scheduling policy");
+
+int max_num_of_processes = KFD_MAX_NUM_OF_PROCESSES_DEFAULT;
+module_param(max_num_of_processes, int, 0444);
+MODULE_PARM_DESC(max_num_of_processes,
+	"Kernel cmdline parameter that defines the amdkfd maximum number of supported processes");
+
+int max_num_of_queues_per_process = KFD_MAX_NUM_OF_QUEUES_PER_PROCESS_DEFAULT;
+module_param(max_num_of_queues_per_process, int, 0444);
+MODULE_PARM_DESC(max_num_of_queues_per_process,
+	"Kernel cmdline parameter that defines the amdkfd maximum number of supported queues per process");
+
+bool kgd2kfd_init(unsigned interface_version,
+		  const struct kfd2kgd_calls *f2g,
+		  const struct kgd2kfd_calls **g2f)
+{
+	/*
+	 * Only one interface version is supported,
+	 * no kfd/kgd version skew allowed.
+	 */
+	if (interface_version != KFD_INTERFACE_VERSION)
+		return false;
+
+	/* Protection against multiple amd kgd loads */
+	if (kfd2kgd)
+		return true;
+
+	kfd2kgd = f2g;
+	*g2f = &kgd2kfd;
+
+	return true;
+}
+EXPORT_SYMBOL(kgd2kfd_init);
+
+void kgd2kfd_exit(void)
+{
+}
+
+static int __init kfd_module_init(void)
+{
+	int err;
+
+	kfd2kgd = NULL;
+
+	/* Verify module parameters */
+	if ((sched_policy < KFD_SCHED_POLICY_HWS) ||
+		(sched_policy > KFD_SCHED_POLICY_NO_HWS)) {
+		pr_err("kfd: sched_policy has invalid value\n");
+		return -1;
+	}
+
+	/* Verify module parameters */
+	if ((max_num_of_processes < 0) ||
+		(max_num_of_processes > KFD_MAX_NUM_OF_PROCESSES)) {
+		pr_err("kfd: max_num_of_processes must be between 0 to KFD_MAX_NUM_OF_PROCESSES\n");
+		return -1;
+	}
+
+	if ((max_num_of_queues_per_process < 0) ||
+		(max_num_of_queues_per_process >
+			KFD_MAX_NUM_OF_QUEUES_PER_PROCESS)) {
+		pr_err("kfd: max_num_of_queues_per_process must be between 0 to KFD_MAX_NUM_OF_QUEUES_PER_PROCESS\n");
+		return -1;
+	}
+
+	err = kfd_pasid_init();
+	if (err < 0)
+		goto err_pasid;
+
+	err = kfd_chardev_init();
+	if (err < 0)
+		goto err_ioctl;
+
+	err = kfd_topology_init();
+	if (err < 0)
+		goto err_topology;
+
+	kfd_process_create_wq();
+
+	dev_info(kfd_device, "Initialized module\n");
+
+	return 0;
+
+err_topology:
+	kfd_chardev_exit();
+err_ioctl:
+	kfd_pasid_exit();
+err_pasid:
+	return err;
+}
+
+static void __exit kfd_module_exit(void)
+{
+	kfd_process_destroy_wq();
+	kfd_topology_shutdown();
+	kfd_chardev_exit();
+	kfd_pasid_exit();
+	dev_info(kfd_device, "Removed module\n");
+}
+
+module_init(kfd_module_init);
+module_exit(kfd_module_exit);
+
+MODULE_AUTHOR(KFD_DRIVER_AUTHOR);
+MODULE_DESCRIPTION(KFD_DRIVER_DESC);
+MODULE_LICENSE("GPL and additional rights");
+MODULE_VERSION(__stringify(KFD_DRIVER_MAJOR) "."
+	       __stringify(KFD_DRIVER_MINOR) "."
+	       __stringify(KFD_DRIVER_PATCHLEVEL));
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_mqd_manager.c b/drivers/gpu/drm/amd/amdkfd/kfd_mqd_manager.c
new file mode 100644
index 000000000000..adc31474e786
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_mqd_manager.c
@@ -0,0 +1,346 @@
+/*
+ * Copyright 2014 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.
+ *
+ */
+
+#include <linux/printk.h>
+#include <linux/slab.h>
+#include "kfd_priv.h"
+#include "kfd_mqd_manager.h"
+#include "cik_regs.h"
+#include "../../radeon/cik_reg.h"
+
+inline void busy_wait(unsigned long ms)
+{
+	while (time_before(jiffies, ms))
+		cpu_relax();
+}
+
+static inline struct cik_mqd *get_mqd(void *mqd)
+{
+	return (struct cik_mqd *)mqd;
+}
+
+static int init_mqd(struct mqd_manager *mm, void **mqd,
+		struct kfd_mem_obj **mqd_mem_obj, uint64_t *gart_addr,
+		struct queue_properties *q)
+{
+	uint64_t addr;
+	struct cik_mqd *m;
+	int retval;
+
+	BUG_ON(!mm || !q || !mqd);
+
+	pr_debug("kfd: In func %s\n", __func__);
+
+	retval = kfd2kgd->allocate_mem(mm->dev->kgd,
+					sizeof(struct cik_mqd),
+					256,
+					KFD_MEMPOOL_SYSTEM_WRITECOMBINE,
+					(struct kgd_mem **) mqd_mem_obj);
+
+	if (retval != 0)
+		return -ENOMEM;
+
+	m = (struct cik_mqd *) (*mqd_mem_obj)->cpu_ptr;
+	addr = (*mqd_mem_obj)->gpu_addr;
+
+	memset(m, 0, ALIGN(sizeof(struct cik_mqd), 256));
+
+	m->header = 0xC0310800;
+	m->compute_pipelinestat_enable = 1;
+	m->compute_static_thread_mgmt_se0 = 0xFFFFFFFF;
+	m->compute_static_thread_mgmt_se1 = 0xFFFFFFFF;
+	m->compute_static_thread_mgmt_se2 = 0xFFFFFFFF;
+	m->compute_static_thread_mgmt_se3 = 0xFFFFFFFF;
+
+	/*
+	 * Make sure to use the last queue state saved on mqd when the cp
+	 * reassigns the queue, so when queue is switched on/off (e.g over
+	 * subscription or quantum timeout) the context will be consistent
+	 */
+	m->cp_hqd_persistent_state =
+				DEFAULT_CP_HQD_PERSISTENT_STATE | PRELOAD_REQ;
+
+	m->cp_mqd_control             = MQD_CONTROL_PRIV_STATE_EN;
+	m->cp_mqd_base_addr_lo        = lower_32_bits(addr);
+	m->cp_mqd_base_addr_hi        = upper_32_bits(addr);
+
+	m->cp_hqd_ib_control = DEFAULT_MIN_IB_AVAIL_SIZE | IB_ATC_EN;
+	/* Although WinKFD writes this, I suspect it should not be necessary */
+	m->cp_hqd_ib_control = IB_ATC_EN | DEFAULT_MIN_IB_AVAIL_SIZE;
+
+	m->cp_hqd_quantum = QUANTUM_EN | QUANTUM_SCALE_1MS |
+				QUANTUM_DURATION(10);
+
+	/*
+	 * Pipe Priority
+	 * Identifies the pipe relative priority when this queue is connected
+	 * to the pipeline. The pipe priority is against the GFX pipe and HP3D.
+	 * In KFD we are using a fixed pipe priority set to CS_MEDIUM.
+	 * 0 = CS_LOW (typically below GFX)
+	 * 1 = CS_MEDIUM (typically between HP3D and GFX
+	 * 2 = CS_HIGH (typically above HP3D)
+	 */
+	m->cp_hqd_pipe_priority = 1;
+	m->cp_hqd_queue_priority = 15;
+
+	*mqd = m;
+	if (gart_addr != NULL)
+		*gart_addr = addr;
+	retval = mm->update_mqd(mm, m, q);
+
+	return retval;
+}
+
+static void uninit_mqd(struct mqd_manager *mm, void *mqd,
+			struct kfd_mem_obj *mqd_mem_obj)
+{
+	BUG_ON(!mm || !mqd);
+	kfd2kgd->free_mem(mm->dev->kgd, (struct kgd_mem *) mqd_mem_obj);
+}
+
+static int load_mqd(struct mqd_manager *mm, void *mqd, uint32_t pipe_id,
+			uint32_t queue_id, uint32_t __user *wptr)
+{
+	return kfd2kgd->hqd_load(mm->dev->kgd, mqd, pipe_id, queue_id, wptr);
+
+}
+
+static int update_mqd(struct mqd_manager *mm, void *mqd,
+			struct queue_properties *q)
+{
+	struct cik_mqd *m;
+
+	BUG_ON(!mm || !q || !mqd);
+
+	pr_debug("kfd: In func %s\n", __func__);
+
+	m = get_mqd(mqd);
+	m->cp_hqd_pq_control = DEFAULT_RPTR_BLOCK_SIZE |
+				DEFAULT_MIN_AVAIL_SIZE | PQ_ATC_EN;
+
+	/*
+	 * Calculating queue size which is log base 2 of actual queue size -1
+	 * dwords and another -1 for ffs
+	 */
+	m->cp_hqd_pq_control |= ffs(q->queue_size / sizeof(unsigned int))
+								- 1 - 1;
+	m->cp_hqd_pq_base_lo = lower_32_bits((uint64_t)q->queue_address >> 8);
+	m->cp_hqd_pq_base_hi = upper_32_bits((uint64_t)q->queue_address >> 8);
+	m->cp_hqd_pq_rptr_report_addr_lo = lower_32_bits((uint64_t)q->read_ptr);
+	m->cp_hqd_pq_rptr_report_addr_hi = upper_32_bits((uint64_t)q->read_ptr);
+	m->cp_hqd_pq_doorbell_control = DOORBELL_EN |
+					DOORBELL_OFFSET(q->doorbell_off);
+
+	m->cp_hqd_vmid = q->vmid;
+
+	if (q->format == KFD_QUEUE_FORMAT_AQL) {
+		m->cp_hqd_iq_rptr = AQL_ENABLE;
+		m->cp_hqd_pq_control |= NO_UPDATE_RPTR;
+	}
+
+	m->cp_hqd_active = 0;
+	q->is_active = false;
+	if (q->queue_size > 0 &&
+			q->queue_address != 0 &&
+			q->queue_percent > 0) {
+		m->cp_hqd_active = 1;
+		q->is_active = true;
+	}
+
+	return 0;
+}
+
+static int destroy_mqd(struct mqd_manager *mm, void *mqd,
+			enum kfd_preempt_type type,
+			unsigned int timeout, uint32_t pipe_id,
+			uint32_t queue_id)
+{
+	return kfd2kgd->hqd_destroy(mm->dev->kgd, type, timeout,
+					pipe_id, queue_id);
+}
+
+static bool is_occupied(struct mqd_manager *mm, void *mqd,
+			uint64_t queue_address,	uint32_t pipe_id,
+			uint32_t queue_id)
+{
+
+	return kfd2kgd->hqd_is_occupies(mm->dev->kgd, queue_address,
+					pipe_id, queue_id);
+
+}
+
+/*
+ * HIQ MQD Implementation, concrete implementation for HIQ MQD implementation.
+ * The HIQ queue in Kaveri is using the same MQD structure as all the user mode
+ * queues but with different initial values.
+ */
+
+static int init_mqd_hiq(struct mqd_manager *mm, void **mqd,
+		struct kfd_mem_obj **mqd_mem_obj, uint64_t *gart_addr,
+		struct queue_properties *q)
+{
+	uint64_t addr;
+	struct cik_mqd *m;
+	int retval;
+
+	BUG_ON(!mm || !q || !mqd || !mqd_mem_obj);
+
+	pr_debug("kfd: In func %s\n", __func__);
+
+	retval = kfd2kgd->allocate_mem(mm->dev->kgd,
+					sizeof(struct cik_mqd),
+					256,
+					KFD_MEMPOOL_SYSTEM_WRITECOMBINE,
+					(struct kgd_mem **) mqd_mem_obj);
+
+	if (retval != 0)
+		return -ENOMEM;
+
+	m = (struct cik_mqd *) (*mqd_mem_obj)->cpu_ptr;
+	addr = (*mqd_mem_obj)->gpu_addr;
+
+	memset(m, 0, ALIGN(sizeof(struct cik_mqd), 256));
+
+	m->header = 0xC0310800;
+	m->compute_pipelinestat_enable = 1;
+	m->compute_static_thread_mgmt_se0 = 0xFFFFFFFF;
+	m->compute_static_thread_mgmt_se1 = 0xFFFFFFFF;
+	m->compute_static_thread_mgmt_se2 = 0xFFFFFFFF;
+	m->compute_static_thread_mgmt_se3 = 0xFFFFFFFF;
+
+	m->cp_hqd_persistent_state = DEFAULT_CP_HQD_PERSISTENT_STATE |
+					PRELOAD_REQ;
+	m->cp_hqd_quantum = QUANTUM_EN | QUANTUM_SCALE_1MS |
+				QUANTUM_DURATION(10);
+
+	m->cp_mqd_control             = MQD_CONTROL_PRIV_STATE_EN;
+	m->cp_mqd_base_addr_lo        = lower_32_bits(addr);
+	m->cp_mqd_base_addr_hi        = upper_32_bits(addr);
+
+	m->cp_hqd_ib_control = DEFAULT_MIN_IB_AVAIL_SIZE;
+
+	/*
+	 * Pipe Priority
+	 * Identifies the pipe relative priority when this queue is connected
+	 * to the pipeline. The pipe priority is against the GFX pipe and HP3D.
+	 * In KFD we are using a fixed pipe priority set to CS_MEDIUM.
+	 * 0 = CS_LOW (typically below GFX)
+	 * 1 = CS_MEDIUM (typically between HP3D and GFX
+	 * 2 = CS_HIGH (typically above HP3D)
+	 */
+	m->cp_hqd_pipe_priority = 1;
+	m->cp_hqd_queue_priority = 15;
+
+	*mqd = m;
+	if (gart_addr)
+		*gart_addr = addr;
+	retval = mm->update_mqd(mm, m, q);
+
+	return retval;
+}
+
+static int update_mqd_hiq(struct mqd_manager *mm, void *mqd,
+				struct queue_properties *q)
+{
+	struct cik_mqd *m;
+
+	BUG_ON(!mm || !q || !mqd);
+
+	pr_debug("kfd: In func %s\n", __func__);
+
+	m = get_mqd(mqd);
+	m->cp_hqd_pq_control = DEFAULT_RPTR_BLOCK_SIZE |
+				DEFAULT_MIN_AVAIL_SIZE |
+				PRIV_STATE |
+				KMD_QUEUE;
+
+	/*
+	 * Calculating queue size which is log base 2 of actual queue
+	 * size -1 dwords
+	 */
+	m->cp_hqd_pq_control |= ffs(q->queue_size / sizeof(unsigned int))
+								- 1 - 1;
+	m->cp_hqd_pq_base_lo = lower_32_bits((uint64_t)q->queue_address >> 8);
+	m->cp_hqd_pq_base_hi = upper_32_bits((uint64_t)q->queue_address >> 8);
+	m->cp_hqd_pq_rptr_report_addr_lo = lower_32_bits((uint64_t)q->read_ptr);
+	m->cp_hqd_pq_rptr_report_addr_hi = upper_32_bits((uint64_t)q->read_ptr);
+	m->cp_hqd_pq_doorbell_control = DOORBELL_EN |
+					DOORBELL_OFFSET(q->doorbell_off);
+
+	m->cp_hqd_vmid = q->vmid;
+
+	m->cp_hqd_active = 0;
+	q->is_active = false;
+	if (q->queue_size > 0 &&
+			q->queue_address != 0 &&
+			q->queue_percent > 0) {
+		m->cp_hqd_active = 1;
+		q->is_active = true;
+	}
+
+	return 0;
+}
+
+struct mqd_manager *mqd_manager_init(enum KFD_MQD_TYPE type,
+					struct kfd_dev *dev)
+{
+	struct mqd_manager *mqd;
+
+	BUG_ON(!dev);
+	BUG_ON(type >= KFD_MQD_TYPE_MAX);
+
+	pr_debug("kfd: In func %s\n", __func__);
+
+	mqd = kzalloc(sizeof(struct mqd_manager), GFP_KERNEL);
+	if (!mqd)
+		return NULL;
+
+	mqd->dev = dev;
+
+	switch (type) {
+	case KFD_MQD_TYPE_CIK_CP:
+	case KFD_MQD_TYPE_CIK_COMPUTE:
+		mqd->init_mqd = init_mqd;
+		mqd->uninit_mqd = uninit_mqd;
+		mqd->load_mqd = load_mqd;
+		mqd->update_mqd = update_mqd;
+		mqd->destroy_mqd = destroy_mqd;
+		mqd->is_occupied = is_occupied;
+		break;
+	case KFD_MQD_TYPE_CIK_HIQ:
+		mqd->init_mqd = init_mqd_hiq;
+		mqd->uninit_mqd = uninit_mqd;
+		mqd->load_mqd = load_mqd;
+		mqd->update_mqd = update_mqd_hiq;
+		mqd->destroy_mqd = destroy_mqd;
+		mqd->is_occupied = is_occupied;
+		break;
+	default:
+		kfree(mqd);
+		return NULL;
+	}
+
+	return mqd;
+}
+
+/* SDMA queues should be implemented here when the cp will supports them */
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_mqd_manager.h b/drivers/gpu/drm/amd/amdkfd/kfd_mqd_manager.h
new file mode 100644
index 000000000000..213a71e0b6c7
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_mqd_manager.h
@@ -0,0 +1,91 @@
+/*
+ * Copyright 2014 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.
+ *
+ */
+
+#ifndef KFD_MQD_MANAGER_H_
+#define KFD_MQD_MANAGER_H_
+
+#include "kfd_priv.h"
+
+/**
+ * struct mqd_manager
+ *
+ * @init_mqd: Allocates the mqd buffer on local gpu memory and initialize it.
+ *
+ * @load_mqd: Loads the mqd to a concrete hqd slot. Used only for no cp
+ * scheduling mode.
+ *
+ * @update_mqd: Handles a update call for the MQD
+ *
+ * @destroy_mqd: Destroys the HQD slot and by that preempt the relevant queue.
+ * Used only for no cp scheduling.
+ *
+ * @uninit_mqd: Releases the mqd buffer from local gpu memory.
+ *
+ * @is_occupied: Checks if the relevant HQD slot is occupied.
+ *
+ * @mqd_mutex: Mqd manager mutex.
+ *
+ * @dev: The kfd device structure coupled with this module.
+ *
+ * MQD stands for Memory Queue Descriptor which represents the current queue
+ * state in the memory and initiate the HQD (Hardware Queue Descriptor) state.
+ * This structure is actually a base class for the different types of MQDs
+ * structures for the variant ASICs that should be supported in the future.
+ * This base class is also contains all the MQD specific operations.
+ * Another important thing to mention is that each queue has a MQD that keeps
+ * his state (or context) after each preemption or reassignment.
+ * Basically there are a instances of the mqd manager class per MQD type per
+ * ASIC. Currently the kfd driver supports only Kaveri so there are instances
+ * per KFD_MQD_TYPE for each device.
+ *
+ */
+
+struct mqd_manager {
+	int	(*init_mqd)(struct mqd_manager *mm, void **mqd,
+			struct kfd_mem_obj **mqd_mem_obj, uint64_t *gart_addr,
+			struct queue_properties *q);
+
+	int	(*load_mqd)(struct mqd_manager *mm, void *mqd,
+				uint32_t pipe_id, uint32_t queue_id,
+				uint32_t __user *wptr);
+
+	int	(*update_mqd)(struct mqd_manager *mm, void *mqd,
+				struct queue_properties *q);
+
+	int	(*destroy_mqd)(struct mqd_manager *mm, void *mqd,
+				enum kfd_preempt_type type,
+				unsigned int timeout, uint32_t pipe_id,
+				uint32_t queue_id);
+
+	void	(*uninit_mqd)(struct mqd_manager *mm, void *mqd,
+				struct kfd_mem_obj *mqd_mem_obj);
+
+	bool	(*is_occupied)(struct mqd_manager *mm, void *mqd,
+				uint64_t queue_address,	uint32_t pipe_id,
+				uint32_t queue_id);
+
+	struct mutex	mqd_mutex;
+	struct kfd_dev	*dev;
+};
+
+#endif /* KFD_MQD_MANAGER_H_ */
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_packet_manager.c b/drivers/gpu/drm/amd/amdkfd/kfd_packet_manager.c
new file mode 100644
index 000000000000..5ce9233d2004
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_packet_manager.c
@@ -0,0 +1,565 @@
+/*
+ * Copyright 2014 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.
+ *
+ */
+
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include "kfd_device_queue_manager.h"
+#include "kfd_kernel_queue.h"
+#include "kfd_priv.h"
+#include "kfd_pm4_headers.h"
+#include "kfd_pm4_opcodes.h"
+
+static inline void inc_wptr(unsigned int *wptr, unsigned int increment_bytes,
+				unsigned int buffer_size_bytes)
+{
+	unsigned int temp = *wptr + increment_bytes / sizeof(uint32_t);
+
+	BUG_ON((temp * sizeof(uint32_t)) > buffer_size_bytes);
+	*wptr = temp;
+}
+
+static unsigned int build_pm4_header(unsigned int opcode, size_t packet_size)
+{
+	union PM4_MES_TYPE_3_HEADER header;
+
+	header.u32all = 0;
+	header.opcode = opcode;
+	header.count = packet_size/sizeof(uint32_t) - 2;
+	header.type = PM4_TYPE_3;
+
+	return header.u32all;
+}
+
+static void pm_calc_rlib_size(struct packet_manager *pm,
+				unsigned int *rlib_size,
+				bool *over_subscription)
+{
+	unsigned int process_count, queue_count;
+
+	BUG_ON(!pm || !rlib_size || !over_subscription);
+
+	process_count = pm->dqm->processes_count;
+	queue_count = pm->dqm->queue_count;
+
+	/* check if there is over subscription*/
+	*over_subscription = false;
+	if ((process_count > 1) ||
+		queue_count > PIPE_PER_ME_CP_SCHEDULING * QUEUES_PER_PIPE) {
+		*over_subscription = true;
+		pr_debug("kfd: over subscribed runlist\n");
+	}
+
+	/* calculate run list ib allocation size */
+	*rlib_size = process_count * sizeof(struct pm4_map_process) +
+		     queue_count * sizeof(struct pm4_map_queues);
+
+	/*
+	 * Increase the allocation size in case we need a chained run list
+	 * when over subscription
+	 */
+	if (*over_subscription)
+		*rlib_size += sizeof(struct pm4_runlist);
+
+	pr_debug("kfd: runlist ib size %d\n", *rlib_size);
+}
+
+static int pm_allocate_runlist_ib(struct packet_manager *pm,
+				unsigned int **rl_buffer,
+				uint64_t *rl_gpu_buffer,
+				unsigned int *rl_buffer_size,
+				bool *is_over_subscription)
+{
+	int retval;
+
+	BUG_ON(!pm);
+	BUG_ON(pm->allocated == true);
+	BUG_ON(is_over_subscription == NULL);
+
+	pm_calc_rlib_size(pm, rl_buffer_size, is_over_subscription);
+
+	retval = kfd2kgd->allocate_mem(pm->dqm->dev->kgd,
+					*rl_buffer_size,
+					PAGE_SIZE,
+					KFD_MEMPOOL_SYSTEM_WRITECOMBINE,
+					(struct kgd_mem **) &pm->ib_buffer_obj);
+
+	if (retval != 0) {
+		pr_err("kfd: failed to allocate runlist IB\n");
+		return retval;
+	}
+
+	*(void **)rl_buffer = pm->ib_buffer_obj->cpu_ptr;
+	*rl_gpu_buffer = pm->ib_buffer_obj->gpu_addr;
+
+	memset(*rl_buffer, 0, *rl_buffer_size);
+	pm->allocated = true;
+	return retval;
+}
+
+static int pm_create_runlist(struct packet_manager *pm, uint32_t *buffer,
+			uint64_t ib, size_t ib_size_in_dwords, bool chain)
+{
+	struct pm4_runlist *packet;
+
+	BUG_ON(!pm || !buffer || !ib);
+
+	packet = (struct pm4_runlist *)buffer;
+
+	memset(buffer, 0, sizeof(struct pm4_runlist));
+	packet->header.u32all = build_pm4_header(IT_RUN_LIST,
+						sizeof(struct pm4_runlist));
+
+	packet->bitfields4.ib_size = ib_size_in_dwords;
+	packet->bitfields4.chain = chain ? 1 : 0;
+	packet->bitfields4.offload_polling = 0;
+	packet->bitfields4.valid = 1;
+	packet->ordinal2 = lower_32_bits(ib);
+	packet->bitfields3.ib_base_hi = upper_32_bits(ib);
+
+	return 0;
+}
+
+static int pm_create_map_process(struct packet_manager *pm, uint32_t *buffer,
+				struct qcm_process_device *qpd)
+{
+	struct pm4_map_process *packet;
+	struct queue *cur;
+	uint32_t num_queues;
+
+	BUG_ON(!pm || !buffer || !qpd);
+
+	packet = (struct pm4_map_process *)buffer;
+
+	pr_debug("kfd: In func %s\n", __func__);
+
+	memset(buffer, 0, sizeof(struct pm4_map_process));
+
+	packet->header.u32all = build_pm4_header(IT_MAP_PROCESS,
+					sizeof(struct pm4_map_process));
+	packet->bitfields2.diq_enable = (qpd->is_debug) ? 1 : 0;
+	packet->bitfields2.process_quantum = 1;
+	packet->bitfields2.pasid = qpd->pqm->process->pasid;
+	packet->bitfields3.page_table_base = qpd->page_table_base;
+	packet->bitfields10.gds_size = qpd->gds_size;
+	packet->bitfields10.num_gws = qpd->num_gws;
+	packet->bitfields10.num_oac = qpd->num_oac;
+	num_queues = 0;
+	list_for_each_entry(cur, &qpd->queues_list, list)
+		num_queues++;
+	packet->bitfields10.num_queues = num_queues;
+
+	packet->sh_mem_config = qpd->sh_mem_config;
+	packet->sh_mem_bases = qpd->sh_mem_bases;
+	packet->sh_mem_ape1_base = qpd->sh_mem_ape1_base;
+	packet->sh_mem_ape1_limit = qpd->sh_mem_ape1_limit;
+
+	packet->gds_addr_lo = lower_32_bits(qpd->gds_context_area);
+	packet->gds_addr_hi = upper_32_bits(qpd->gds_context_area);
+
+	return 0;
+}
+
+static int pm_create_map_queue(struct packet_manager *pm, uint32_t *buffer,
+				struct queue *q)
+{
+	struct pm4_map_queues *packet;
+
+	BUG_ON(!pm || !buffer || !q);
+
+	pr_debug("kfd: In func %s\n", __func__);
+
+	packet = (struct pm4_map_queues *)buffer;
+	memset(buffer, 0, sizeof(struct pm4_map_queues));
+
+	packet->header.u32all = build_pm4_header(IT_MAP_QUEUES,
+						sizeof(struct pm4_map_queues));
+	packet->bitfields2.alloc_format =
+				alloc_format__mes_map_queues__one_per_pipe;
+	packet->bitfields2.num_queues = 1;
+	packet->bitfields2.queue_sel =
+		queue_sel__mes_map_queues__map_to_hws_determined_queue_slots;
+
+	packet->bitfields2.vidmem = (q->properties.is_interop) ?
+			vidmem__mes_map_queues__uses_video_memory :
+			vidmem__mes_map_queues__uses_no_video_memory;
+
+	switch (q->properties.type) {
+	case KFD_QUEUE_TYPE_COMPUTE:
+	case KFD_QUEUE_TYPE_DIQ:
+		packet->bitfields2.engine_sel =
+				engine_sel__mes_map_queues__compute;
+		break;
+	case KFD_QUEUE_TYPE_SDMA:
+		packet->bitfields2.engine_sel =
+				engine_sel__mes_map_queues__sdma0;
+		break;
+	default:
+		BUG();
+		break;
+	}
+
+	packet->mes_map_queues_ordinals[0].bitfields3.doorbell_offset =
+			q->properties.doorbell_off;
+
+	packet->mes_map_queues_ordinals[0].mqd_addr_lo =
+			lower_32_bits(q->gart_mqd_addr);
+
+	packet->mes_map_queues_ordinals[0].mqd_addr_hi =
+			upper_32_bits(q->gart_mqd_addr);
+
+	packet->mes_map_queues_ordinals[0].wptr_addr_lo =
+			lower_32_bits((uint64_t)q->properties.write_ptr);
+
+	packet->mes_map_queues_ordinals[0].wptr_addr_hi =
+			upper_32_bits((uint64_t)q->properties.write_ptr);
+
+	return 0;
+}
+
+static int pm_create_runlist_ib(struct packet_manager *pm,
+				struct list_head *queues,
+				uint64_t *rl_gpu_addr,
+				size_t *rl_size_bytes)
+{
+	unsigned int alloc_size_bytes;
+	unsigned int *rl_buffer, rl_wptr, i;
+	int retval, proccesses_mapped;
+	struct device_process_node *cur;
+	struct qcm_process_device *qpd;
+	struct queue *q;
+	struct kernel_queue *kq;
+	bool is_over_subscription;
+
+	BUG_ON(!pm || !queues || !rl_size_bytes || !rl_gpu_addr);
+
+	rl_wptr = retval = proccesses_mapped = 0;
+
+	retval = pm_allocate_runlist_ib(pm, &rl_buffer, rl_gpu_addr,
+				&alloc_size_bytes, &is_over_subscription);
+	if (retval != 0)
+		return retval;
+
+	*rl_size_bytes = alloc_size_bytes;
+
+	pr_debug("kfd: In func %s\n", __func__);
+	pr_debug("kfd: building runlist ib process count: %d queues count %d\n",
+		pm->dqm->processes_count, pm->dqm->queue_count);
+
+	/* build the run list ib packet */
+	list_for_each_entry(cur, queues, list) {
+		qpd = cur->qpd;
+		/* build map process packet */
+		if (proccesses_mapped >= pm->dqm->processes_count) {
+			pr_debug("kfd: not enough space left in runlist IB\n");
+			pm_release_ib(pm);
+			return -ENOMEM;
+		}
+		retval = pm_create_map_process(pm, &rl_buffer[rl_wptr], qpd);
+		if (retval != 0)
+			return retval;
+		proccesses_mapped++;
+		inc_wptr(&rl_wptr, sizeof(struct pm4_map_process),
+				alloc_size_bytes);
+
+		list_for_each_entry(kq, &qpd->priv_queue_list, list) {
+			if (kq->queue->properties.is_active != true)
+				continue;
+			retval = pm_create_map_queue(pm, &rl_buffer[rl_wptr],
+							kq->queue);
+			if (retval != 0)
+				return retval;
+			inc_wptr(&rl_wptr, sizeof(struct pm4_map_queues),
+					alloc_size_bytes);
+		}
+
+		list_for_each_entry(q, &qpd->queues_list, list) {
+			if (q->properties.is_active != true)
+				continue;
+			retval = pm_create_map_queue(pm,
+						&rl_buffer[rl_wptr], q);
+			if (retval != 0)
+				return retval;
+			inc_wptr(&rl_wptr, sizeof(struct pm4_map_queues),
+					alloc_size_bytes);
+		}
+	}
+
+	pr_debug("kfd: finished map process and queues to runlist\n");
+
+	if (is_over_subscription)
+		pm_create_runlist(pm, &rl_buffer[rl_wptr], *rl_gpu_addr,
+				alloc_size_bytes / sizeof(uint32_t), true);
+
+	for (i = 0; i < alloc_size_bytes / sizeof(uint32_t); i++)
+		pr_debug("0x%2X ", rl_buffer[i]);
+	pr_debug("\n");
+
+	return 0;
+}
+
+int pm_init(struct packet_manager *pm, struct device_queue_manager *dqm)
+{
+	BUG_ON(!dqm);
+
+	pm->dqm = dqm;
+	mutex_init(&pm->lock);
+	pm->priv_queue = kernel_queue_init(dqm->dev, KFD_QUEUE_TYPE_HIQ);
+	if (pm->priv_queue == NULL) {
+		mutex_destroy(&pm->lock);
+		return -ENOMEM;
+	}
+	pm->allocated = false;
+
+	return 0;
+}
+
+void pm_uninit(struct packet_manager *pm)
+{
+	BUG_ON(!pm);
+
+	mutex_destroy(&pm->lock);
+	kernel_queue_uninit(pm->priv_queue);
+}
+
+int pm_send_set_resources(struct packet_manager *pm,
+				struct scheduling_resources *res)
+{
+	struct pm4_set_resources *packet;
+
+	BUG_ON(!pm || !res);
+
+	pr_debug("kfd: In func %s\n", __func__);
+
+	mutex_lock(&pm->lock);
+	pm->priv_queue->acquire_packet_buffer(pm->priv_queue,
+					sizeof(*packet) / sizeof(uint32_t),
+			(unsigned int **)&packet);
+	if (packet == NULL) {
+		mutex_unlock(&pm->lock);
+		pr_err("kfd: failed to allocate buffer on kernel queue\n");
+		return -ENOMEM;
+	}
+
+	memset(packet, 0, sizeof(struct pm4_set_resources));
+	packet->header.u32all = build_pm4_header(IT_SET_RESOURCES,
+					sizeof(struct pm4_set_resources));
+
+	packet->bitfields2.queue_type =
+			queue_type__mes_set_resources__hsa_interface_queue_hiq;
+	packet->bitfields2.vmid_mask = res->vmid_mask;
+	packet->bitfields2.unmap_latency = KFD_UNMAP_LATENCY;
+	packet->bitfields7.oac_mask = res->oac_mask;
+	packet->bitfields8.gds_heap_base = res->gds_heap_base;
+	packet->bitfields8.gds_heap_size = res->gds_heap_size;
+
+	packet->gws_mask_lo = lower_32_bits(res->gws_mask);
+	packet->gws_mask_hi = upper_32_bits(res->gws_mask);
+
+	packet->queue_mask_lo = lower_32_bits(res->queue_mask);
+	packet->queue_mask_hi = upper_32_bits(res->queue_mask);
+
+	pm->priv_queue->submit_packet(pm->priv_queue);
+	pm->priv_queue->sync_with_hw(pm->priv_queue, KFD_HIQ_TIMEOUT);
+
+	mutex_unlock(&pm->lock);
+
+	return 0;
+}
+
+int pm_send_runlist(struct packet_manager *pm, struct list_head *dqm_queues)
+{
+	uint64_t rl_gpu_ib_addr;
+	uint32_t *rl_buffer;
+	size_t rl_ib_size, packet_size_dwords;
+	int retval;
+
+	BUG_ON(!pm || !dqm_queues);
+
+	retval = pm_create_runlist_ib(pm, dqm_queues, &rl_gpu_ib_addr,
+					&rl_ib_size);
+	if (retval != 0)
+		goto fail_create_runlist_ib;
+
+	pr_debug("kfd: runlist IB address: 0x%llX\n", rl_gpu_ib_addr);
+
+	packet_size_dwords = sizeof(struct pm4_runlist) / sizeof(uint32_t);
+	mutex_lock(&pm->lock);
+
+	retval = pm->priv_queue->acquire_packet_buffer(pm->priv_queue,
+					packet_size_dwords, &rl_buffer);
+	if (retval != 0)
+		goto fail_acquire_packet_buffer;
+
+	retval = pm_create_runlist(pm, rl_buffer, rl_gpu_ib_addr,
+					rl_ib_size / sizeof(uint32_t), false);
+	if (retval != 0)
+		goto fail_create_runlist;
+
+	pm->priv_queue->submit_packet(pm->priv_queue);
+	pm->priv_queue->sync_with_hw(pm->priv_queue, KFD_HIQ_TIMEOUT);
+
+	mutex_unlock(&pm->lock);
+
+	return retval;
+
+fail_create_runlist:
+	pm->priv_queue->rollback_packet(pm->priv_queue);
+fail_acquire_packet_buffer:
+	mutex_unlock(&pm->lock);
+fail_create_runlist_ib:
+	if (pm->allocated == true)
+		pm_release_ib(pm);
+	return retval;
+}
+
+int pm_send_query_status(struct packet_manager *pm, uint64_t fence_address,
+			uint32_t fence_value)
+{
+	int retval;
+	struct pm4_query_status *packet;
+
+	BUG_ON(!pm || !fence_address);
+
+	mutex_lock(&pm->lock);
+	retval = pm->priv_queue->acquire_packet_buffer(
+			pm->priv_queue,
+			sizeof(struct pm4_query_status) / sizeof(uint32_t),
+			(unsigned int **)&packet);
+	if (retval != 0)
+		goto fail_acquire_packet_buffer;
+
+	packet->header.u32all = build_pm4_header(IT_QUERY_STATUS,
+					sizeof(struct pm4_query_status));
+
+	packet->bitfields2.context_id = 0;
+	packet->bitfields2.interrupt_sel =
+			interrupt_sel__mes_query_status__completion_status;
+	packet->bitfields2.command =
+			command__mes_query_status__fence_only_after_write_ack;
+
+	packet->addr_hi = upper_32_bits((uint64_t)fence_address);
+	packet->addr_lo = lower_32_bits((uint64_t)fence_address);
+	packet->data_hi = upper_32_bits((uint64_t)fence_value);
+	packet->data_lo = lower_32_bits((uint64_t)fence_value);
+
+	pm->priv_queue->submit_packet(pm->priv_queue);
+	pm->priv_queue->sync_with_hw(pm->priv_queue, KFD_HIQ_TIMEOUT);
+	mutex_unlock(&pm->lock);
+
+	return 0;
+
+fail_acquire_packet_buffer:
+	mutex_unlock(&pm->lock);
+	return retval;
+}
+
+int pm_send_unmap_queue(struct packet_manager *pm, enum kfd_queue_type type,
+			enum kfd_preempt_type_filter mode,
+			uint32_t filter_param, bool reset,
+			unsigned int sdma_engine)
+{
+	int retval;
+	uint32_t *buffer;
+	struct pm4_unmap_queues *packet;
+
+	BUG_ON(!pm);
+
+	mutex_lock(&pm->lock);
+	retval = pm->priv_queue->acquire_packet_buffer(
+			pm->priv_queue,
+			sizeof(struct pm4_unmap_queues) / sizeof(uint32_t),
+			&buffer);
+	if (retval != 0)
+		goto err_acquire_packet_buffer;
+
+	packet = (struct pm4_unmap_queues *)buffer;
+	memset(buffer, 0, sizeof(struct pm4_unmap_queues));
+
+	packet->header.u32all = build_pm4_header(IT_UNMAP_QUEUES,
+					sizeof(struct pm4_unmap_queues));
+	switch (type) {
+	case KFD_QUEUE_TYPE_COMPUTE:
+	case KFD_QUEUE_TYPE_DIQ:
+		packet->bitfields2.engine_sel =
+			engine_sel__mes_unmap_queues__compute;
+		break;
+	case KFD_QUEUE_TYPE_SDMA:
+		packet->bitfields2.engine_sel =
+			engine_sel__mes_unmap_queues__sdma0 + sdma_engine;
+		break;
+	default:
+		BUG();
+		break;
+	}
+
+	if (reset)
+		packet->bitfields2.action =
+				action__mes_unmap_queues__reset_queues;
+	else
+		packet->bitfields2.action =
+				action__mes_unmap_queues__preempt_queues;
+
+	switch (mode) {
+	case KFD_PREEMPT_TYPE_FILTER_SINGLE_QUEUE:
+		packet->bitfields2.queue_sel =
+				queue_sel__mes_unmap_queues__perform_request_on_specified_queues;
+		packet->bitfields2.num_queues = 1;
+		packet->bitfields3b.doorbell_offset0 = filter_param;
+		break;
+	case KFD_PREEMPT_TYPE_FILTER_BY_PASID:
+		packet->bitfields2.queue_sel =
+				queue_sel__mes_unmap_queues__perform_request_on_pasid_queues;
+		packet->bitfields3a.pasid = filter_param;
+		break;
+	case KFD_PREEMPT_TYPE_FILTER_ALL_QUEUES:
+		packet->bitfields2.queue_sel =
+				queue_sel__mes_unmap_queues__perform_request_on_all_active_queues;
+		break;
+	default:
+		BUG();
+		break;
+	};
+
+	pm->priv_queue->submit_packet(pm->priv_queue);
+	pm->priv_queue->sync_with_hw(pm->priv_queue, KFD_HIQ_TIMEOUT);
+
+	mutex_unlock(&pm->lock);
+	return 0;
+
+err_acquire_packet_buffer:
+	mutex_unlock(&pm->lock);
+	return retval;
+}
+
+void pm_release_ib(struct packet_manager *pm)
+{
+	BUG_ON(!pm);
+
+	mutex_lock(&pm->lock);
+	if (pm->allocated) {
+		kfd2kgd->free_mem(pm->dqm->dev->kgd,
+				(struct kgd_mem *) pm->ib_buffer_obj);
+		pm->allocated = false;
+	}
+	mutex_unlock(&pm->lock);
+}
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_pasid.c b/drivers/gpu/drm/amd/amdkfd/kfd_pasid.c
new file mode 100644
index 000000000000..71699ad97d74
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_pasid.c
@@ -0,0 +1,96 @@
+/*
+ * Copyright 2014 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.
+ */
+
+#include <linux/slab.h>
+#include <linux/types.h>
+#include "kfd_priv.h"
+
+static unsigned long *pasid_bitmap;
+static unsigned int pasid_limit;
+static DEFINE_MUTEX(pasid_mutex);
+
+int kfd_pasid_init(void)
+{
+	pasid_limit = max_num_of_processes;
+
+	pasid_bitmap = kzalloc(BITS_TO_LONGS(pasid_limit), GFP_KERNEL);
+	if (!pasid_bitmap)
+		return -ENOMEM;
+
+	set_bit(0, pasid_bitmap); /* PASID 0 is reserved. */
+
+	return 0;
+}
+
+void kfd_pasid_exit(void)
+{
+	kfree(pasid_bitmap);
+}
+
+bool kfd_set_pasid_limit(unsigned int new_limit)
+{
+	if (new_limit < pasid_limit) {
+		bool ok;
+
+		mutex_lock(&pasid_mutex);
+
+		/* ensure that no pasids >= new_limit are in-use */
+		ok = (find_next_bit(pasid_bitmap, pasid_limit, new_limit) ==
+								pasid_limit);
+		if (ok)
+			pasid_limit = new_limit;
+
+		mutex_unlock(&pasid_mutex);
+
+		return ok;
+	}
+
+	return true;
+}
+
+inline unsigned int kfd_get_pasid_limit(void)
+{
+	return pasid_limit;
+}
+
+unsigned int kfd_pasid_alloc(void)
+{
+	unsigned int found;
+
+	mutex_lock(&pasid_mutex);
+
+	found = find_first_zero_bit(pasid_bitmap, pasid_limit);
+	if (found == pasid_limit)
+		found = 0;
+	else
+		set_bit(found, pasid_bitmap);
+
+	mutex_unlock(&pasid_mutex);
+
+	return found;
+}
+
+void kfd_pasid_free(unsigned int pasid)
+{
+	BUG_ON(pasid == 0 || pasid >= pasid_limit);
+	clear_bit(pasid, pasid_bitmap);
+}
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_pm4_headers.h b/drivers/gpu/drm/amd/amdkfd/kfd_pm4_headers.h
new file mode 100644
index 000000000000..071ad5724bd2
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_pm4_headers.h
@@ -0,0 +1,405 @@
+/*
+ * Copyright 2014 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.
+ *
+ */
+
+#ifndef KFD_PM4_HEADERS_H_
+#define KFD_PM4_HEADERS_H_
+
+#ifndef PM4_MES_HEADER_DEFINED
+#define PM4_MES_HEADER_DEFINED
+union PM4_MES_TYPE_3_HEADER {
+	struct {
+		uint32_t reserved1:8;	/* < reserved */
+		uint32_t opcode:8;	/* < IT opcode */
+		uint32_t count:14;	/* < number of DWORDs - 1
+					 * in the information body.
+					 */
+		uint32_t type:2;	/* < packet identifier.
+					 * It should be 3 for type 3 packets
+					 */
+	};
+	uint32_t u32all;
+};
+#endif /* PM4_MES_HEADER_DEFINED */
+
+/* --------------------MES_SET_RESOURCES-------------------- */
+
+#ifndef PM4_MES_SET_RESOURCES_DEFINED
+#define PM4_MES_SET_RESOURCES_DEFINED
+enum set_resources_queue_type_enum {
+	queue_type__mes_set_resources__kernel_interface_queue_kiq = 0,
+	queue_type__mes_set_resources__hsa_interface_queue_hiq = 1,
+	queue_type__mes_set_resources__hsa_debug_interface_queue = 4
+};
+
+struct pm4_set_resources {
+	union {
+		union PM4_MES_TYPE_3_HEADER header;	/* header */
+		uint32_t ordinal1;
+	};
+
+	union {
+		struct {
+			uint32_t vmid_mask:16;
+			uint32_t unmap_latency:8;
+			uint32_t reserved1:5;
+			enum set_resources_queue_type_enum queue_type:3;
+		} bitfields2;
+		uint32_t ordinal2;
+	};
+
+	uint32_t queue_mask_lo;
+	uint32_t queue_mask_hi;
+	uint32_t gws_mask_lo;
+	uint32_t gws_mask_hi;
+
+	union {
+		struct {
+			uint32_t oac_mask:16;
+			uint32_t reserved2:16;
+		} bitfields7;
+		uint32_t ordinal7;
+	};
+
+	union {
+		struct {
+			uint32_t gds_heap_base:6;
+			uint32_t reserved3:5;
+			uint32_t gds_heap_size:6;
+			uint32_t reserved4:15;
+		} bitfields8;
+		uint32_t ordinal8;
+	};
+
+};
+#endif
+
+/*--------------------MES_RUN_LIST-------------------- */
+
+#ifndef PM4_MES_RUN_LIST_DEFINED
+#define PM4_MES_RUN_LIST_DEFINED
+
+struct pm4_runlist {
+	union {
+		union PM4_MES_TYPE_3_HEADER header;	/* header */
+		uint32_t ordinal1;
+	};
+
+	union {
+		struct {
+			uint32_t reserved1:2;
+			uint32_t ib_base_lo:30;
+		} bitfields2;
+		uint32_t ordinal2;
+	};
+
+	union {
+		struct {
+			uint32_t ib_base_hi:16;
+			uint32_t reserved2:16;
+		} bitfields3;
+		uint32_t ordinal3;
+	};
+
+	union {
+		struct {
+			uint32_t ib_size:20;
+			uint32_t chain:1;
+			uint32_t offload_polling:1;
+			uint32_t reserved3:1;
+			uint32_t valid:1;
+			uint32_t reserved4:8;
+		} bitfields4;
+		uint32_t ordinal4;
+	};
+
+};
+#endif
+
+/*--------------------MES_MAP_PROCESS-------------------- */
+
+#ifndef PM4_MES_MAP_PROCESS_DEFINED
+#define PM4_MES_MAP_PROCESS_DEFINED
+
+struct pm4_map_process {
+	union {
+		union PM4_MES_TYPE_3_HEADER header;	/* header */
+		uint32_t ordinal1;
+	};
+
+	union {
+		struct {
+			uint32_t pasid:16;
+			uint32_t reserved1:8;
+			uint32_t diq_enable:1;
+			uint32_t process_quantum:7;
+		} bitfields2;
+		uint32_t ordinal2;
+	};
+
+	union {
+		struct {
+			uint32_t page_table_base:28;
+			uint32_t reserved3:4;
+		} bitfields3;
+		uint32_t ordinal3;
+	};
+
+	uint32_t sh_mem_bases;
+	uint32_t sh_mem_ape1_base;
+	uint32_t sh_mem_ape1_limit;
+	uint32_t sh_mem_config;
+	uint32_t gds_addr_lo;
+	uint32_t gds_addr_hi;
+
+	union {
+		struct {
+			uint32_t num_gws:6;
+			uint32_t reserved4:2;
+			uint32_t num_oac:4;
+			uint32_t reserved5:4;
+			uint32_t gds_size:6;
+			uint32_t num_queues:10;
+		} bitfields10;
+		uint32_t ordinal10;
+	};
+
+};
+#endif
+
+/*--------------------MES_MAP_QUEUES--------------------*/
+
+#ifndef PM4_MES_MAP_QUEUES_DEFINED
+#define PM4_MES_MAP_QUEUES_DEFINED
+enum map_queues_queue_sel_enum {
+	queue_sel__mes_map_queues__map_to_specified_queue_slots = 0,
+	queue_sel__mes_map_queues__map_to_hws_determined_queue_slots = 1,
+	queue_sel__mes_map_queues__enable_process_queues = 2
+};
+
+enum map_queues_vidmem_enum {
+	vidmem__mes_map_queues__uses_no_video_memory = 0,
+	vidmem__mes_map_queues__uses_video_memory = 1
+};
+
+enum map_queues_alloc_format_enum {
+	alloc_format__mes_map_queues__one_per_pipe = 0,
+	alloc_format__mes_map_queues__all_on_one_pipe = 1
+};
+
+enum map_queues_engine_sel_enum {
+	engine_sel__mes_map_queues__compute = 0,
+	engine_sel__mes_map_queues__sdma0 = 2,
+	engine_sel__mes_map_queues__sdma1 = 3
+};
+
+struct pm4_map_queues {
+	union {
+		union PM4_MES_TYPE_3_HEADER header;	/* header */
+		uint32_t ordinal1;
+	};
+
+	union {
+		struct {
+			uint32_t reserved1:4;
+			enum map_queues_queue_sel_enum queue_sel:2;
+			uint32_t reserved2:2;
+			uint32_t vmid:4;
+			uint32_t reserved3:4;
+			enum map_queues_vidmem_enum vidmem:2;
+			uint32_t reserved4:6;
+			enum map_queues_alloc_format_enum alloc_format:2;
+			enum map_queues_engine_sel_enum engine_sel:3;
+			uint32_t num_queues:3;
+		} bitfields2;
+		uint32_t ordinal2;
+	};
+
+	struct {
+		union {
+			struct {
+				uint32_t reserved5:2;
+				uint32_t doorbell_offset:21;
+				uint32_t reserved6:3;
+				uint32_t queue:6;
+			} bitfields3;
+			uint32_t ordinal3;
+		};
+
+		uint32_t mqd_addr_lo;
+		uint32_t mqd_addr_hi;
+		uint32_t wptr_addr_lo;
+		uint32_t wptr_addr_hi;
+
+	} mes_map_queues_ordinals[1];	/* 1..N of these ordinal groups */
+
+};
+#endif
+
+/*--------------------MES_QUERY_STATUS--------------------*/
+
+#ifndef PM4_MES_QUERY_STATUS_DEFINED
+#define PM4_MES_QUERY_STATUS_DEFINED
+enum query_status_interrupt_sel_enum {
+	interrupt_sel__mes_query_status__completion_status = 0,
+	interrupt_sel__mes_query_status__process_status = 1,
+	interrupt_sel__mes_query_status__queue_status = 2
+};
+
+enum query_status_command_enum {
+	command__mes_query_status__interrupt_only = 0,
+	command__mes_query_status__fence_only_immediate = 1,
+	command__mes_query_status__fence_only_after_write_ack = 2,
+	command__mes_query_status__fence_wait_for_write_ack_send_interrupt = 3
+};
+
+enum query_status_engine_sel_enum {
+	engine_sel__mes_query_status__compute = 0,
+	engine_sel__mes_query_status__sdma0_queue = 2,
+	engine_sel__mes_query_status__sdma1_queue = 3
+};
+
+struct pm4_query_status {
+	union {
+		union PM4_MES_TYPE_3_HEADER header;	/* header */
+		uint32_t ordinal1;
+	};
+
+	union {
+		struct {
+			uint32_t context_id:28;
+			enum query_status_interrupt_sel_enum interrupt_sel:2;
+			enum query_status_command_enum command:2;
+		} bitfields2;
+		uint32_t ordinal2;
+	};
+
+	union {
+		struct {
+			uint32_t pasid:16;
+			uint32_t reserved1:16;
+		} bitfields3a;
+		struct {
+			uint32_t reserved2:2;
+			uint32_t doorbell_offset:21;
+			uint32_t reserved3:3;
+			enum query_status_engine_sel_enum engine_sel:3;
+			uint32_t reserved4:3;
+		} bitfields3b;
+		uint32_t ordinal3;
+	};
+
+	uint32_t addr_lo;
+	uint32_t addr_hi;
+	uint32_t data_lo;
+	uint32_t data_hi;
+};
+#endif
+
+/*--------------------MES_UNMAP_QUEUES--------------------*/
+
+#ifndef PM4_MES_UNMAP_QUEUES_DEFINED
+#define PM4_MES_UNMAP_QUEUES_DEFINED
+enum unmap_queues_action_enum {
+	action__mes_unmap_queues__preempt_queues = 0,
+	action__mes_unmap_queues__reset_queues = 1,
+	action__mes_unmap_queues__disable_process_queues = 2
+};
+
+enum unmap_queues_queue_sel_enum {
+	queue_sel__mes_unmap_queues__perform_request_on_specified_queues = 0,
+	queue_sel__mes_unmap_queues__perform_request_on_pasid_queues = 1,
+	queue_sel__mes_unmap_queues__perform_request_on_all_active_queues = 2
+};
+
+enum unmap_queues_engine_sel_enum {
+	engine_sel__mes_unmap_queues__compute = 0,
+	engine_sel__mes_unmap_queues__sdma0 = 2,
+	engine_sel__mes_unmap_queues__sdma1 = 3
+};
+
+struct pm4_unmap_queues {
+	union {
+		union PM4_MES_TYPE_3_HEADER header;	/* header */
+		uint32_t ordinal1;
+	};
+
+	union {
+		struct {
+			enum unmap_queues_action_enum action:2;
+			uint32_t reserved1:2;
+			enum unmap_queues_queue_sel_enum queue_sel:2;
+			uint32_t reserved2:20;
+			enum unmap_queues_engine_sel_enum engine_sel:3;
+			uint32_t num_queues:3;
+		} bitfields2;
+		uint32_t ordinal2;
+	};
+
+	union {
+		struct {
+			uint32_t pasid:16;
+			uint32_t reserved3:16;
+		} bitfields3a;
+		struct {
+			uint32_t reserved4:2;
+			uint32_t doorbell_offset0:21;
+			uint32_t reserved5:9;
+		} bitfields3b;
+		uint32_t ordinal3;
+	};
+
+	union {
+		struct {
+			uint32_t reserved6:2;
+			uint32_t doorbell_offset1:21;
+			uint32_t reserved7:9;
+		} bitfields4;
+		uint32_t ordinal4;
+	};
+
+	union {
+		struct {
+			uint32_t reserved8:2;
+			uint32_t doorbell_offset2:21;
+			uint32_t reserved9:9;
+		} bitfields5;
+		uint32_t ordinal5;
+	};
+
+	union {
+		struct {
+			uint32_t reserved10:2;
+			uint32_t doorbell_offset3:21;
+			uint32_t reserved11:9;
+		} bitfields6;
+		uint32_t ordinal6;
+	};
+
+};
+#endif
+
+enum {
+	CACHE_FLUSH_AND_INV_TS_EVENT = 0x00000014
+};
+
+#endif /* KFD_PM4_HEADERS_H_ */
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_pm4_opcodes.h b/drivers/gpu/drm/amd/amdkfd/kfd_pm4_opcodes.h
new file mode 100644
index 000000000000..b72fa3b8c2d4
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_pm4_opcodes.h
@@ -0,0 +1,107 @@
+/*
+ * Copyright 2014 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.
+ *
+ */
+
+
+#ifndef KFD_PM4_OPCODES_H
+#define KFD_PM4_OPCODES_H
+
+enum it_opcode_type {
+	IT_NOP                               = 0x10,
+	IT_SET_BASE                          = 0x11,
+	IT_CLEAR_STATE                       = 0x12,
+	IT_INDEX_BUFFER_SIZE                 = 0x13,
+	IT_DISPATCH_DIRECT                   = 0x15,
+	IT_DISPATCH_INDIRECT                 = 0x16,
+	IT_ATOMIC_GDS                        = 0x1D,
+	IT_OCCLUSION_QUERY                   = 0x1F,
+	IT_SET_PREDICATION                   = 0x20,
+	IT_REG_RMW                           = 0x21,
+	IT_COND_EXEC                         = 0x22,
+	IT_PRED_EXEC                         = 0x23,
+	IT_DRAW_INDIRECT                     = 0x24,
+	IT_DRAW_INDEX_INDIRECT               = 0x25,
+	IT_INDEX_BASE                        = 0x26,
+	IT_DRAW_INDEX_2                      = 0x27,
+	IT_CONTEXT_CONTROL                   = 0x28,
+	IT_INDEX_TYPE                        = 0x2A,
+	IT_DRAW_INDIRECT_MULTI               = 0x2C,
+	IT_DRAW_INDEX_AUTO                   = 0x2D,
+	IT_NUM_INSTANCES                     = 0x2F,
+	IT_DRAW_INDEX_MULTI_AUTO             = 0x30,
+	IT_INDIRECT_BUFFER_CNST              = 0x33,
+	IT_STRMOUT_BUFFER_UPDATE             = 0x34,
+	IT_DRAW_INDEX_OFFSET_2               = 0x35,
+	IT_DRAW_PREAMBLE                     = 0x36,
+	IT_WRITE_DATA                        = 0x37,
+	IT_DRAW_INDEX_INDIRECT_MULTI         = 0x38,
+	IT_MEM_SEMAPHORE                     = 0x39,
+	IT_COPY_DW                           = 0x3B,
+	IT_WAIT_REG_MEM                      = 0x3C,
+	IT_INDIRECT_BUFFER                   = 0x3F,
+	IT_COPY_DATA                         = 0x40,
+	IT_PFP_SYNC_ME                       = 0x42,
+	IT_SURFACE_SYNC                      = 0x43,
+	IT_COND_WRITE                        = 0x45,
+	IT_EVENT_WRITE                       = 0x46,
+	IT_EVENT_WRITE_EOP                   = 0x47,
+	IT_EVENT_WRITE_EOS                   = 0x48,
+	IT_RELEASE_MEM                       = 0x49,
+	IT_PREAMBLE_CNTL                     = 0x4A,
+	IT_DMA_DATA                          = 0x50,
+	IT_ACQUIRE_MEM                       = 0x58,
+	IT_REWIND                            = 0x59,
+	IT_LOAD_UCONFIG_REG                  = 0x5E,
+	IT_LOAD_SH_REG                       = 0x5F,
+	IT_LOAD_CONFIG_REG                   = 0x60,
+	IT_LOAD_CONTEXT_REG                  = 0x61,
+	IT_SET_CONFIG_REG                    = 0x68,
+	IT_SET_CONTEXT_REG                   = 0x69,
+	IT_SET_CONTEXT_REG_INDIRECT          = 0x73,
+	IT_SET_SH_REG                        = 0x76,
+	IT_SET_SH_REG_OFFSET                 = 0x77,
+	IT_SET_QUEUE_REG                     = 0x78,
+	IT_SET_UCONFIG_REG                   = 0x79,
+	IT_SCRATCH_RAM_WRITE                 = 0x7D,
+	IT_SCRATCH_RAM_READ                  = 0x7E,
+	IT_LOAD_CONST_RAM                    = 0x80,
+	IT_WRITE_CONST_RAM                   = 0x81,
+	IT_DUMP_CONST_RAM                    = 0x83,
+	IT_INCREMENT_CE_COUNTER              = 0x84,
+	IT_INCREMENT_DE_COUNTER              = 0x85,
+	IT_WAIT_ON_CE_COUNTER                = 0x86,
+	IT_WAIT_ON_DE_COUNTER_DIFF           = 0x88,
+	IT_SWITCH_BUFFER                     = 0x8B,
+	IT_SET_RESOURCES                     = 0xA0,
+	IT_MAP_PROCESS                       = 0xA1,
+	IT_MAP_QUEUES                        = 0xA2,
+	IT_UNMAP_QUEUES                      = 0xA3,
+	IT_QUERY_STATUS                      = 0xA4,
+	IT_RUN_LIST                          = 0xA5,
+};
+
+#define PM4_TYPE_0 0
+#define PM4_TYPE_2 2
+#define PM4_TYPE_3 3
+
+#endif /* KFD_PM4_OPCODES_H */
+
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_priv.h b/drivers/gpu/drm/amd/amdkfd/kfd_priv.h
new file mode 100644
index 000000000000..f9fb81e3bb09
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_priv.h
@@ -0,0 +1,600 @@
+/*
+ * Copyright 2014 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.
+ */
+
+#ifndef KFD_PRIV_H_INCLUDED
+#define KFD_PRIV_H_INCLUDED
+
+#include <linux/hashtable.h>
+#include <linux/mmu_notifier.h>
+#include <linux/mutex.h>
+#include <linux/types.h>
+#include <linux/atomic.h>
+#include <linux/workqueue.h>
+#include <linux/spinlock.h>
+#include <linux/kfd_ioctl.h>
+#include <kgd_kfd_interface.h>
+
+#define KFD_SYSFS_FILE_MODE 0444
+
+/*
+ * When working with cp scheduler we should assign the HIQ manually or via
+ * the radeon driver to a fixed hqd slot, here are the fixed HIQ hqd slot
+ * definitions for Kaveri. In Kaveri only the first ME queues participates
+ * in the cp scheduling taking that in mind we set the HIQ slot in the
+ * second ME.
+ */
+#define KFD_CIK_HIQ_PIPE 4
+#define KFD_CIK_HIQ_QUEUE 0
+
+/* GPU ID hash width in bits */
+#define KFD_GPU_ID_HASH_WIDTH 16
+
+/* Macro for allocating structures */
+#define kfd_alloc_struct(ptr_to_struct)	\
+	((typeof(ptr_to_struct)) kzalloc(sizeof(*ptr_to_struct), GFP_KERNEL))
+
+/* Kernel module parameter to specify maximum number of supported processes */
+extern int max_num_of_processes;
+
+#define KFD_MAX_NUM_OF_PROCESSES_DEFAULT 32
+#define KFD_MAX_NUM_OF_PROCESSES 512
+
+/*
+ * Kernel module parameter to specify maximum number of supported queues
+ * per process
+ */
+extern int max_num_of_queues_per_process;
+
+#define KFD_MAX_NUM_OF_QUEUES_PER_PROCESS_DEFAULT 128
+#define KFD_MAX_NUM_OF_QUEUES_PER_PROCESS 1024
+
+#define KFD_KERNEL_QUEUE_SIZE 2048
+
+/* Kernel module parameter to specify the scheduling policy */
+extern int sched_policy;
+
+/**
+ * enum kfd_sched_policy
+ *
+ * @KFD_SCHED_POLICY_HWS: H/W scheduling policy known as command processor (cp)
+ * scheduling. In this scheduling mode we're using the firmware code to
+ * schedule the user mode queues and kernel queues such as HIQ and DIQ.
+ * the HIQ queue is used as a special queue that dispatches the configuration
+ * to the cp and the user mode queues list that are currently running.
+ * the DIQ queue is a debugging queue that dispatches debugging commands to the
+ * firmware.
+ * in this scheduling mode user mode queues over subscription feature is
+ * enabled.
+ *
+ * @KFD_SCHED_POLICY_HWS_NO_OVERSUBSCRIPTION: The same as above but the over
+ * subscription feature disabled.
+ *
+ * @KFD_SCHED_POLICY_NO_HWS: no H/W scheduling policy is a mode which directly
+ * set the command processor registers and sets the queues "manually". This
+ * mode is used *ONLY* for debugging proposes.
+ *
+ */
+enum kfd_sched_policy {
+	KFD_SCHED_POLICY_HWS = 0,
+	KFD_SCHED_POLICY_HWS_NO_OVERSUBSCRIPTION,
+	KFD_SCHED_POLICY_NO_HWS
+};
+
+enum cache_policy {
+	cache_policy_coherent,
+	cache_policy_noncoherent
+};
+
+struct kfd_device_info {
+	unsigned int max_pasid_bits;
+	size_t ih_ring_entry_size;
+	uint16_t mqd_size_aligned;
+};
+
+struct kfd_dev {
+	struct kgd_dev *kgd;
+
+	const struct kfd_device_info *device_info;
+	struct pci_dev *pdev;
+
+	unsigned int id;		/* topology stub index */
+
+	phys_addr_t doorbell_base;	/* Start of actual doorbells used by
+					 * KFD. It is aligned for mapping
+					 * into user mode
+					 */
+	size_t doorbell_id_offset;	/* Doorbell offset (from KFD doorbell
+					 * to HW doorbell, GFX reserved some
+					 * at the start)
+					 */
+	size_t doorbell_process_limit;	/* Number of processes we have doorbell
+					 * space for.
+					 */
+	u32 __iomem *doorbell_kernel_ptr; /* This is a pointer for a doorbells
+					   * page used by kernel queue
+					   */
+
+	struct kgd2kfd_shared_resources shared_resources;
+
+	void *interrupt_ring;
+	size_t interrupt_ring_size;
+	atomic_t interrupt_ring_rptr;
+	atomic_t interrupt_ring_wptr;
+	struct work_struct interrupt_work;
+	spinlock_t interrupt_lock;
+
+	/* QCM Device instance */
+	struct device_queue_manager *dqm;
+
+	bool init_complete;
+	/*
+	 * Interrupts of interest to KFD are copied
+	 * from the HW ring into a SW ring.
+	 */
+	bool interrupts_active;
+};
+
+/* KGD2KFD callbacks */
+void kgd2kfd_exit(void);
+struct kfd_dev *kgd2kfd_probe(struct kgd_dev *kgd, struct pci_dev *pdev);
+bool kgd2kfd_device_init(struct kfd_dev *kfd,
+			 const struct kgd2kfd_shared_resources *gpu_resources);
+void kgd2kfd_device_exit(struct kfd_dev *kfd);
+
+extern const struct kfd2kgd_calls *kfd2kgd;
+
+struct kfd_mem_obj {
+	void *bo;
+	uint64_t gpu_addr;
+	uint32_t *cpu_ptr;
+};
+
+enum kfd_mempool {
+	KFD_MEMPOOL_SYSTEM_CACHEABLE = 1,
+	KFD_MEMPOOL_SYSTEM_WRITECOMBINE = 2,
+	KFD_MEMPOOL_FRAMEBUFFER = 3,
+};
+
+/* Character device interface */
+int kfd_chardev_init(void);
+void kfd_chardev_exit(void);
+struct device *kfd_chardev(void);
+
+/**
+ * enum kfd_preempt_type_filter
+ *
+ * @KFD_PREEMPT_TYPE_FILTER_SINGLE_QUEUE: Preempts single queue.
+ *
+ * @KFD_PRERMPT_TYPE_FILTER_ALL_QUEUES: Preempts all queues in the
+ *						running queues list.
+ *
+ * @KFD_PRERMPT_TYPE_FILTER_BY_PASID: Preempts queues that belongs to
+ *						specific process.
+ *
+ */
+enum kfd_preempt_type_filter {
+	KFD_PREEMPT_TYPE_FILTER_SINGLE_QUEUE,
+	KFD_PREEMPT_TYPE_FILTER_ALL_QUEUES,
+	KFD_PREEMPT_TYPE_FILTER_BY_PASID
+};
+
+enum kfd_preempt_type {
+	KFD_PREEMPT_TYPE_WAVEFRONT,
+	KFD_PREEMPT_TYPE_WAVEFRONT_RESET
+};
+
+/**
+ * enum kfd_queue_type
+ *
+ * @KFD_QUEUE_TYPE_COMPUTE: Regular user mode queue type.
+ *
+ * @KFD_QUEUE_TYPE_SDMA: Sdma user mode queue type.
+ *
+ * @KFD_QUEUE_TYPE_HIQ: HIQ queue type.
+ *
+ * @KFD_QUEUE_TYPE_DIQ: DIQ queue type.
+ */
+enum kfd_queue_type  {
+	KFD_QUEUE_TYPE_COMPUTE,
+	KFD_QUEUE_TYPE_SDMA,
+	KFD_QUEUE_TYPE_HIQ,
+	KFD_QUEUE_TYPE_DIQ
+};
+
+enum kfd_queue_format {
+	KFD_QUEUE_FORMAT_PM4,
+	KFD_QUEUE_FORMAT_AQL
+};
+
+/**
+ * struct queue_properties
+ *
+ * @type: The queue type.
+ *
+ * @queue_id: Queue identifier.
+ *
+ * @queue_address: Queue ring buffer address.
+ *
+ * @queue_size: Queue ring buffer size.
+ *
+ * @priority: Defines the queue priority relative to other queues in the
+ * process.
+ * This is just an indication and HW scheduling may override the priority as
+ * necessary while keeping the relative prioritization.
+ * the priority granularity is from 0 to f which f is the highest priority.
+ * currently all queues are initialized with the highest priority.
+ *
+ * @queue_percent: This field is partially implemented and currently a zero in
+ * this field defines that the queue is non active.
+ *
+ * @read_ptr: User space address which points to the number of dwords the
+ * cp read from the ring buffer. This field updates automatically by the H/W.
+ *
+ * @write_ptr: Defines the number of dwords written to the ring buffer.
+ *
+ * @doorbell_ptr: This field aim is to notify the H/W of new packet written to
+ * the queue ring buffer. This field should be similar to write_ptr and the user
+ * should update this field after he updated the write_ptr.
+ *
+ * @doorbell_off: The doorbell offset in the doorbell pci-bar.
+ *
+ * @is_interop: Defines if this is a interop queue. Interop queue means that the
+ * queue can access both graphics and compute resources.
+ *
+ * @is_active: Defines if the queue is active or not.
+ *
+ * @vmid: If the scheduling mode is no cp scheduling the field defines the vmid
+ * of the queue.
+ *
+ * This structure represents the queue properties for each queue no matter if
+ * it's user mode or kernel mode queue.
+ *
+ */
+struct queue_properties {
+	enum kfd_queue_type type;
+	enum kfd_queue_format format;
+	unsigned int queue_id;
+	uint64_t queue_address;
+	uint64_t  queue_size;
+	uint32_t priority;
+	uint32_t queue_percent;
+	uint32_t *read_ptr;
+	uint32_t *write_ptr;
+	uint32_t __iomem *doorbell_ptr;
+	uint32_t doorbell_off;
+	bool is_interop;
+	bool is_active;
+	/* Not relevant for user mode queues in cp scheduling */
+	unsigned int vmid;
+};
+
+/**
+ * struct queue
+ *
+ * @list: Queue linked list.
+ *
+ * @mqd: The queue MQD.
+ *
+ * @mqd_mem_obj: The MQD local gpu memory object.
+ *
+ * @gart_mqd_addr: The MQD gart mc address.
+ *
+ * @properties: The queue properties.
+ *
+ * @mec: Used only in no cp scheduling mode and identifies to micro engine id
+ * that the queue should be execute on.
+ *
+ * @pipe: Used only in no cp scheduling mode and identifies the queue's pipe id.
+ *
+ * @queue: Used only in no cp scheduliong mode and identifies the queue's slot.
+ *
+ * @process: The kfd process that created this queue.
+ *
+ * @device: The kfd device that created this queue.
+ *
+ * This structure represents user mode compute queues.
+ * It contains all the necessary data to handle such queues.
+ *
+ */
+
+struct queue {
+	struct list_head list;
+	void *mqd;
+	struct kfd_mem_obj *mqd_mem_obj;
+	uint64_t gart_mqd_addr;
+	struct queue_properties properties;
+
+	uint32_t mec;
+	uint32_t pipe;
+	uint32_t queue;
+
+	struct kfd_process	*process;
+	struct kfd_dev		*device;
+};
+
+/*
+ * Please read the kfd_mqd_manager.h description.
+ */
+enum KFD_MQD_TYPE {
+	KFD_MQD_TYPE_CIK_COMPUTE = 0, /* for no cp scheduling */
+	KFD_MQD_TYPE_CIK_HIQ, /* for hiq */
+	KFD_MQD_TYPE_CIK_CP, /* for cp queues and diq */
+	KFD_MQD_TYPE_CIK_SDMA, /* for sdma queues */
+	KFD_MQD_TYPE_MAX
+};
+
+struct scheduling_resources {
+	unsigned int vmid_mask;
+	enum kfd_queue_type type;
+	uint64_t queue_mask;
+	uint64_t gws_mask;
+	uint32_t oac_mask;
+	uint32_t gds_heap_base;
+	uint32_t gds_heap_size;
+};
+
+struct process_queue_manager {
+	/* data */
+	struct kfd_process	*process;
+	unsigned int		num_concurrent_processes;
+	struct list_head	queues;
+	unsigned long		*queue_slot_bitmap;
+};
+
+struct qcm_process_device {
+	/* The Device Queue Manager that owns this data */
+	struct device_queue_manager *dqm;
+	struct process_queue_manager *pqm;
+	/* Device Queue Manager lock */
+	struct mutex *lock;
+	/* Queues list */
+	struct list_head queues_list;
+	struct list_head priv_queue_list;
+
+	unsigned int queue_count;
+	unsigned int vmid;
+	bool is_debug;
+	/*
+	 * All the memory management data should be here too
+	 */
+	uint64_t gds_context_area;
+	uint32_t sh_mem_config;
+	uint32_t sh_mem_bases;
+	uint32_t sh_mem_ape1_base;
+	uint32_t sh_mem_ape1_limit;
+	uint32_t page_table_base;
+	uint32_t gds_size;
+	uint32_t num_gws;
+	uint32_t num_oac;
+};
+
+/* Data that is per-process-per device. */
+struct kfd_process_device {
+	/*
+	 * List of all per-device data for a process.
+	 * Starts from kfd_process.per_device_data.
+	 */
+	struct list_head per_device_list;
+
+	/* The device that owns this data. */
+	struct kfd_dev *dev;
+
+
+	/* per-process-per device QCM data structure */
+	struct qcm_process_device qpd;
+
+	/*Apertures*/
+	uint64_t lds_base;
+	uint64_t lds_limit;
+	uint64_t gpuvm_base;
+	uint64_t gpuvm_limit;
+	uint64_t scratch_base;
+	uint64_t scratch_limit;
+
+	/* Is this process/pasid bound to this device? (amd_iommu_bind_pasid) */
+	bool bound;
+};
+
+#define qpd_to_pdd(x) container_of(x, struct kfd_process_device, qpd)
+
+/* Process data */
+struct kfd_process {
+	/*
+	 * kfd_process are stored in an mm_struct*->kfd_process*
+	 * hash table (kfd_processes in kfd_process.c)
+	 */
+	struct hlist_node kfd_processes;
+
+	struct mm_struct *mm;
+
+	struct mutex mutex;
+
+	/*
+	 * In any process, the thread that started main() is the lead
+	 * thread and outlives the rest.
+	 * It is here because amd_iommu_bind_pasid wants a task_struct.
+	 */
+	struct task_struct *lead_thread;
+
+	/* We want to receive a notification when the mm_struct is destroyed */
+	struct mmu_notifier mmu_notifier;
+
+	/* Use for delayed freeing of kfd_process structure */
+	struct rcu_head	rcu;
+
+	unsigned int pasid;
+
+	/*
+	 * List of kfd_process_device structures,
+	 * one for each device the process is using.
+	 */
+	struct list_head per_device_data;
+
+	struct process_queue_manager pqm;
+
+	/* The process's queues. */
+	size_t queue_array_size;
+
+	/* Size is queue_array_size, up to MAX_PROCESS_QUEUES. */
+	struct kfd_queue **queues;
+
+	unsigned long allocated_queue_bitmap[DIV_ROUND_UP(KFD_MAX_NUM_OF_QUEUES_PER_PROCESS, BITS_PER_LONG)];
+
+	/*Is the user space process 32 bit?*/
+	bool is_32bit_user_mode;
+};
+
+void kfd_process_create_wq(void);
+void kfd_process_destroy_wq(void);
+struct kfd_process *kfd_create_process(const struct task_struct *);
+struct kfd_process *kfd_get_process(const struct task_struct *);
+
+struct kfd_process_device *kfd_bind_process_to_device(struct kfd_dev *dev,
+							struct kfd_process *p);
+void kfd_unbind_process_from_device(struct kfd_dev *dev, unsigned int pasid);
+struct kfd_process_device *kfd_get_process_device_data(struct kfd_dev *dev,
+							struct kfd_process *p,
+							int create_pdd);
+
+/* Process device data iterator */
+struct kfd_process_device *kfd_get_first_process_device_data(struct kfd_process *p);
+struct kfd_process_device *kfd_get_next_process_device_data(struct kfd_process *p,
+						struct kfd_process_device *pdd);
+bool kfd_has_process_device_data(struct kfd_process *p);
+
+/* PASIDs */
+int kfd_pasid_init(void);
+void kfd_pasid_exit(void);
+bool kfd_set_pasid_limit(unsigned int new_limit);
+unsigned int kfd_get_pasid_limit(void);
+unsigned int kfd_pasid_alloc(void);
+void kfd_pasid_free(unsigned int pasid);
+
+/* Doorbells */
+void kfd_doorbell_init(struct kfd_dev *kfd);
+int kfd_doorbell_mmap(struct kfd_process *process, struct vm_area_struct *vma);
+u32 __iomem *kfd_get_kernel_doorbell(struct kfd_dev *kfd,
+					unsigned int *doorbell_off);
+void kfd_release_kernel_doorbell(struct kfd_dev *kfd, u32 __iomem *db_addr);
+u32 read_kernel_doorbell(u32 __iomem *db);
+void write_kernel_doorbell(u32 __iomem *db, u32 value);
+unsigned int kfd_queue_id_to_doorbell(struct kfd_dev *kfd,
+					struct kfd_process *process,
+					unsigned int queue_id);
+
+extern struct device *kfd_device;
+
+/* Topology */
+int kfd_topology_init(void);
+void kfd_topology_shutdown(void);
+int kfd_topology_add_device(struct kfd_dev *gpu);
+int kfd_topology_remove_device(struct kfd_dev *gpu);
+struct kfd_dev *kfd_device_by_id(uint32_t gpu_id);
+struct kfd_dev *kfd_device_by_pci_dev(const struct pci_dev *pdev);
+struct kfd_dev *kfd_topology_enum_kfd_devices(uint8_t idx);
+
+/* Interrupts */
+int kfd_interrupt_init(struct kfd_dev *dev);
+void kfd_interrupt_exit(struct kfd_dev *dev);
+void kgd2kfd_interrupt(struct kfd_dev *kfd, const void *ih_ring_entry);
+bool enqueue_ih_ring_entry(struct kfd_dev *kfd,	const void *ih_ring_entry);
+
+/* Power Management */
+void kgd2kfd_suspend(struct kfd_dev *kfd);
+int kgd2kfd_resume(struct kfd_dev *kfd);
+
+/* amdkfd Apertures */
+int kfd_init_apertures(struct kfd_process *process);
+
+/* Queue Context Management */
+inline uint32_t lower_32(uint64_t x);
+inline uint32_t upper_32(uint64_t x);
+
+int init_queue(struct queue **q, struct queue_properties properties);
+void uninit_queue(struct queue *q);
+void print_queue_properties(struct queue_properties *q);
+void print_queue(struct queue *q);
+
+struct mqd_manager *mqd_manager_init(enum KFD_MQD_TYPE type,
+					struct kfd_dev *dev);
+struct device_queue_manager *device_queue_manager_init(struct kfd_dev *dev);
+void device_queue_manager_uninit(struct device_queue_manager *dqm);
+struct kernel_queue *kernel_queue_init(struct kfd_dev *dev,
+					enum kfd_queue_type type);
+void kernel_queue_uninit(struct kernel_queue *kq);
+
+/* Process Queue Manager */
+struct process_queue_node {
+	struct queue *q;
+	struct kernel_queue *kq;
+	struct list_head process_queue_list;
+};
+
+int pqm_init(struct process_queue_manager *pqm, struct kfd_process *p);
+void pqm_uninit(struct process_queue_manager *pqm);
+int pqm_create_queue(struct process_queue_manager *pqm,
+			    struct kfd_dev *dev,
+			    struct file *f,
+			    struct queue_properties *properties,
+			    unsigned int flags,
+			    enum kfd_queue_type type,
+			    unsigned int *qid);
+int pqm_destroy_queue(struct process_queue_manager *pqm, unsigned int qid);
+int pqm_update_queue(struct process_queue_manager *pqm, unsigned int qid,
+			struct queue_properties *p);
+
+/* Packet Manager */
+
+#define KFD_HIQ_TIMEOUT (500)
+
+#define KFD_FENCE_COMPLETED (100)
+#define KFD_FENCE_INIT   (10)
+#define KFD_UNMAP_LATENCY (150)
+
+struct packet_manager {
+	struct device_queue_manager *dqm;
+	struct kernel_queue *priv_queue;
+	struct mutex lock;
+	bool allocated;
+	struct kfd_mem_obj *ib_buffer_obj;
+};
+
+int pm_init(struct packet_manager *pm, struct device_queue_manager *dqm);
+void pm_uninit(struct packet_manager *pm);
+int pm_send_set_resources(struct packet_manager *pm,
+				struct scheduling_resources *res);
+int pm_send_runlist(struct packet_manager *pm, struct list_head *dqm_queues);
+int pm_send_query_status(struct packet_manager *pm, uint64_t fence_address,
+				uint32_t fence_value);
+
+int pm_send_unmap_queue(struct packet_manager *pm, enum kfd_queue_type type,
+			enum kfd_preempt_type_filter mode,
+			uint32_t filter_param, bool reset,
+			unsigned int sdma_engine);
+
+void pm_release_ib(struct packet_manager *pm);
+
+uint64_t kfd_get_number_elems(struct kfd_dev *kfd);
+phys_addr_t kfd_get_process_doorbells(struct kfd_dev *dev,
+					struct kfd_process *process);
+
+#endif
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_process.c b/drivers/gpu/drm/amd/amdkfd/kfd_process.c
new file mode 100644
index 000000000000..3c76ef05cbcf
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_process.c
@@ -0,0 +1,419 @@
+/*
+ * Copyright 2014 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.
+ */
+
+#include <linux/mutex.h>
+#include <linux/log2.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/amd-iommu.h>
+#include <linux/notifier.h>
+#include <linux/compat.h>
+
+struct mm_struct;
+
+#include "kfd_priv.h"
+
+/*
+ * Initial size for the array of queues.
+ * The allocated size is doubled each time
+ * it is exceeded up to MAX_PROCESS_QUEUES.
+ */
+#define INITIAL_QUEUE_ARRAY_SIZE 16
+
+/*
+ * List of struct kfd_process (field kfd_process).
+ * Unique/indexed by mm_struct*
+ */
+#define KFD_PROCESS_TABLE_SIZE 5 /* bits: 32 entries */
+static DEFINE_HASHTABLE(kfd_processes_table, KFD_PROCESS_TABLE_SIZE);
+static DEFINE_MUTEX(kfd_processes_mutex);
+
+DEFINE_STATIC_SRCU(kfd_processes_srcu);
+
+static struct workqueue_struct *kfd_process_wq;
+
+struct kfd_process_release_work {
+	struct work_struct kfd_work;
+	struct kfd_process *p;
+};
+
+static struct kfd_process *find_process(const struct task_struct *thread);
+static struct kfd_process *create_process(const struct task_struct *thread);
+
+void kfd_process_create_wq(void)
+{
+	if (!kfd_process_wq)
+		kfd_process_wq = create_workqueue("kfd_process_wq");
+}
+
+void kfd_process_destroy_wq(void)
+{
+	if (kfd_process_wq) {
+		flush_workqueue(kfd_process_wq);
+		destroy_workqueue(kfd_process_wq);
+		kfd_process_wq = NULL;
+	}
+}
+
+struct kfd_process *kfd_create_process(const struct task_struct *thread)
+{
+	struct kfd_process *process;
+
+	BUG_ON(!kfd_process_wq);
+
+	if (thread->mm == NULL)
+		return ERR_PTR(-EINVAL);
+
+	/* Only the pthreads threading model is supported. */
+	if (thread->group_leader->mm != thread->mm)
+		return ERR_PTR(-EINVAL);
+
+	/* Take mmap_sem because we call __mmu_notifier_register inside */
+	down_write(&thread->mm->mmap_sem);
+
+	/*
+	 * take kfd processes mutex before starting of process creation
+	 * so there won't be a case where two threads of the same process
+	 * create two kfd_process structures
+	 */
+	mutex_lock(&kfd_processes_mutex);
+
+	/* A prior open of /dev/kfd could have already created the process. */
+	process = find_process(thread);
+	if (process)
+		pr_debug("kfd: process already found\n");
+
+	if (!process)
+		process = create_process(thread);
+
+	mutex_unlock(&kfd_processes_mutex);
+
+	up_write(&thread->mm->mmap_sem);
+
+	return process;
+}
+
+struct kfd_process *kfd_get_process(const struct task_struct *thread)
+{
+	struct kfd_process *process;
+
+	if (thread->mm == NULL)
+		return ERR_PTR(-EINVAL);
+
+	/* Only the pthreads threading model is supported. */
+	if (thread->group_leader->mm != thread->mm)
+		return ERR_PTR(-EINVAL);
+
+	process = find_process(thread);
+
+	return process;
+}
+
+static struct kfd_process *find_process_by_mm(const struct mm_struct *mm)
+{
+	struct kfd_process *process;
+
+	hash_for_each_possible_rcu(kfd_processes_table, process,
+					kfd_processes, (uintptr_t)mm)
+		if (process->mm == mm)
+			return process;
+
+	return NULL;
+}
+
+static struct kfd_process *find_process(const struct task_struct *thread)
+{
+	struct kfd_process *p;
+	int idx;
+
+	idx = srcu_read_lock(&kfd_processes_srcu);
+	p = find_process_by_mm(thread->mm);
+	srcu_read_unlock(&kfd_processes_srcu, idx);
+
+	return p;
+}
+
+static void kfd_process_wq_release(struct work_struct *work)
+{
+	struct kfd_process_release_work *my_work;
+	struct kfd_process_device *pdd, *temp;
+	struct kfd_process *p;
+
+	my_work = (struct kfd_process_release_work *) work;
+
+	p = my_work->p;
+
+	mutex_lock(&p->mutex);
+
+	list_for_each_entry_safe(pdd, temp, &p->per_device_data,
+							per_device_list) {
+		amd_iommu_unbind_pasid(pdd->dev->pdev, p->pasid);
+		list_del(&pdd->per_device_list);
+
+		kfree(pdd);
+	}
+
+	kfd_pasid_free(p->pasid);
+
+	mutex_unlock(&p->mutex);
+
+	mutex_destroy(&p->mutex);
+
+	kfree(p->queues);
+
+	kfree(p);
+
+	kfree((void *)work);
+}
+
+static void kfd_process_destroy_delayed(struct rcu_head *rcu)
+{
+	struct kfd_process_release_work *work;
+	struct kfd_process *p;
+
+	BUG_ON(!kfd_process_wq);
+
+	p = container_of(rcu, struct kfd_process, rcu);
+	BUG_ON(atomic_read(&p->mm->mm_count) <= 0);
+
+	mmdrop(p->mm);
+
+	work = (struct kfd_process_release_work *)
+		kmalloc(sizeof(struct kfd_process_release_work), GFP_ATOMIC);
+
+	if (work) {
+		INIT_WORK((struct work_struct *) work, kfd_process_wq_release);
+		work->p = p;
+		queue_work(kfd_process_wq, (struct work_struct *) work);
+	}
+}
+
+static void kfd_process_notifier_release(struct mmu_notifier *mn,
+					struct mm_struct *mm)
+{
+	struct kfd_process *p;
+
+	/*
+	 * The kfd_process structure can not be free because the
+	 * mmu_notifier srcu is read locked
+	 */
+	p = container_of(mn, struct kfd_process, mmu_notifier);
+	BUG_ON(p->mm != mm);
+
+	mutex_lock(&kfd_processes_mutex);
+	hash_del_rcu(&p->kfd_processes);
+	mutex_unlock(&kfd_processes_mutex);
+	synchronize_srcu(&kfd_processes_srcu);
+
+	mutex_lock(&p->mutex);
+
+	/* In case our notifier is called before IOMMU notifier */
+	pqm_uninit(&p->pqm);
+
+	mutex_unlock(&p->mutex);
+
+	/*
+	 * Because we drop mm_count inside kfd_process_destroy_delayed
+	 * and because the mmu_notifier_unregister function also drop
+	 * mm_count we need to take an extra count here.
+	 */
+	atomic_inc(&p->mm->mm_count);
+	mmu_notifier_unregister_no_release(&p->mmu_notifier, p->mm);
+	mmu_notifier_call_srcu(&p->rcu, &kfd_process_destroy_delayed);
+}
+
+static const struct mmu_notifier_ops kfd_process_mmu_notifier_ops = {
+	.release = kfd_process_notifier_release,
+};
+
+static struct kfd_process *create_process(const struct task_struct *thread)
+{
+	struct kfd_process *process;
+	int err = -ENOMEM;
+
+	process = kzalloc(sizeof(*process), GFP_KERNEL);
+
+	if (!process)
+		goto err_alloc_process;
+
+	process->queues = kmalloc_array(INITIAL_QUEUE_ARRAY_SIZE,
+					sizeof(process->queues[0]), GFP_KERNEL);
+	if (!process->queues)
+		goto err_alloc_queues;
+
+	process->pasid = kfd_pasid_alloc();
+	if (process->pasid == 0)
+		goto err_alloc_pasid;
+
+	mutex_init(&process->mutex);
+
+	process->mm = thread->mm;
+
+	/* register notifier */
+	process->mmu_notifier.ops = &kfd_process_mmu_notifier_ops;
+	err = __mmu_notifier_register(&process->mmu_notifier, process->mm);
+	if (err)
+		goto err_mmu_notifier;
+
+	hash_add_rcu(kfd_processes_table, &process->kfd_processes,
+			(uintptr_t)process->mm);
+
+	process->lead_thread = thread->group_leader;
+
+	process->queue_array_size = INITIAL_QUEUE_ARRAY_SIZE;
+
+	INIT_LIST_HEAD(&process->per_device_data);
+
+	err = pqm_init(&process->pqm, process);
+	if (err != 0)
+		goto err_process_pqm_init;
+
+	/* init process apertures*/
+	process->is_32bit_user_mode = is_compat_task();
+	if (kfd_init_apertures(process) != 0)
+		goto err_init_apretures;
+
+	return process;
+
+err_init_apretures:
+	pqm_uninit(&process->pqm);
+err_process_pqm_init:
+	hash_del_rcu(&process->kfd_processes);
+	synchronize_rcu();
+	mmu_notifier_unregister_no_release(&process->mmu_notifier, process->mm);
+err_mmu_notifier:
+	kfd_pasid_free(process->pasid);
+err_alloc_pasid:
+	kfree(process->queues);
+err_alloc_queues:
+	kfree(process);
+err_alloc_process:
+	return ERR_PTR(err);
+}
+
+struct kfd_process_device *kfd_get_process_device_data(struct kfd_dev *dev,
+							struct kfd_process *p,
+							int create_pdd)
+{
+	struct kfd_process_device *pdd = NULL;
+
+	list_for_each_entry(pdd, &p->per_device_data, per_device_list)
+		if (pdd->dev == dev)
+			return pdd;
+
+	if (create_pdd) {
+		pdd = kzalloc(sizeof(*pdd), GFP_KERNEL);
+		if (pdd != NULL) {
+			pdd->dev = dev;
+			INIT_LIST_HEAD(&pdd->qpd.queues_list);
+			INIT_LIST_HEAD(&pdd->qpd.priv_queue_list);
+			pdd->qpd.dqm = dev->dqm;
+			list_add(&pdd->per_device_list, &p->per_device_data);
+		}
+	}
+
+	return pdd;
+}
+
+/*
+ * Direct the IOMMU to bind the process (specifically the pasid->mm)
+ * to the device.
+ * Unbinding occurs when the process dies or the device is removed.
+ *
+ * Assumes that the process lock is held.
+ */
+struct kfd_process_device *kfd_bind_process_to_device(struct kfd_dev *dev,
+							struct kfd_process *p)
+{
+	struct kfd_process_device *pdd = kfd_get_process_device_data(dev, p, 1);
+	int err;
+
+	if (pdd == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	if (pdd->bound)
+		return pdd;
+
+	err = amd_iommu_bind_pasid(dev->pdev, p->pasid, p->lead_thread);
+	if (err < 0)
+		return ERR_PTR(err);
+
+	pdd->bound = true;
+
+	return pdd;
+}
+
+void kfd_unbind_process_from_device(struct kfd_dev *dev, unsigned int pasid)
+{
+	struct kfd_process *p;
+	struct kfd_process_device *pdd;
+	int idx, i;
+
+	BUG_ON(dev == NULL);
+
+	idx = srcu_read_lock(&kfd_processes_srcu);
+
+	hash_for_each_rcu(kfd_processes_table, i, p, kfd_processes)
+		if (p->pasid == pasid)
+			break;
+
+	srcu_read_unlock(&kfd_processes_srcu, idx);
+
+	BUG_ON(p->pasid != pasid);
+
+	mutex_lock(&p->mutex);
+
+	pqm_uninit(&p->pqm);
+
+	pdd = kfd_get_process_device_data(dev, p, 0);
+
+	/*
+	 * Just mark pdd as unbound, because we still need it to call
+	 * amd_iommu_unbind_pasid() in when the process exits.
+	 * We don't call amd_iommu_unbind_pasid() here
+	 * because the IOMMU called us.
+	 */
+	if (pdd)
+		pdd->bound = false;
+
+	mutex_unlock(&p->mutex);
+}
+
+struct kfd_process_device *kfd_get_first_process_device_data(struct kfd_process *p)
+{
+	return list_first_entry(&p->per_device_data,
+				struct kfd_process_device,
+				per_device_list);
+}
+
+struct kfd_process_device *kfd_get_next_process_device_data(struct kfd_process *p,
+						struct kfd_process_device *pdd)
+{
+	if (list_is_last(&pdd->per_device_list, &p->per_device_data))
+		return NULL;
+	return list_next_entry(pdd, per_device_list);
+}
+
+bool kfd_has_process_device_data(struct kfd_process *p)
+{
+	return !(list_empty(&p->per_device_data));
+}
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_process_queue_manager.c b/drivers/gpu/drm/amd/amdkfd/kfd_process_queue_manager.c
new file mode 100644
index 000000000000..47526780d736
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_process_queue_manager.c
@@ -0,0 +1,343 @@
+/*
+ * Copyright 2014 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.
+ *
+ */
+
+#include <linux/slab.h>
+#include <linux/list.h>
+#include "kfd_device_queue_manager.h"
+#include "kfd_priv.h"
+#include "kfd_kernel_queue.h"
+
+static inline struct process_queue_node *get_queue_by_qid(
+			struct process_queue_manager *pqm, unsigned int qid)
+{
+	struct process_queue_node *pqn;
+
+	BUG_ON(!pqm);
+
+	list_for_each_entry(pqn, &pqm->queues, process_queue_list) {
+		if (pqn->q && pqn->q->properties.queue_id == qid)
+			return pqn;
+		if (pqn->kq && pqn->kq->queue->properties.queue_id == qid)
+			return pqn;
+	}
+
+	return NULL;
+}
+
+static int find_available_queue_slot(struct process_queue_manager *pqm,
+					unsigned int *qid)
+{
+	unsigned long found;
+
+	BUG_ON(!pqm || !qid);
+
+	pr_debug("kfd: in %s\n", __func__);
+
+	found = find_first_zero_bit(pqm->queue_slot_bitmap,
+			max_num_of_queues_per_process);
+
+	pr_debug("kfd: the new slot id %lu\n", found);
+
+	if (found >= max_num_of_queues_per_process) {
+		pr_info("amdkfd: Can not open more queues for process with pasid %d\n",
+				pqm->process->pasid);
+		return -ENOMEM;
+	}
+
+	set_bit(found, pqm->queue_slot_bitmap);
+	*qid = found;
+
+	return 0;
+}
+
+int pqm_init(struct process_queue_manager *pqm, struct kfd_process *p)
+{
+	BUG_ON(!pqm);
+
+	INIT_LIST_HEAD(&pqm->queues);
+	pqm->queue_slot_bitmap =
+			kzalloc(DIV_ROUND_UP(max_num_of_queues_per_process,
+					BITS_PER_BYTE), GFP_KERNEL);
+	if (pqm->queue_slot_bitmap == NULL)
+		return -ENOMEM;
+	pqm->process = p;
+
+	return 0;
+}
+
+void pqm_uninit(struct process_queue_manager *pqm)
+{
+	int retval;
+	struct process_queue_node *pqn, *next;
+
+	BUG_ON(!pqm);
+
+	pr_debug("In func %s\n", __func__);
+
+	list_for_each_entry_safe(pqn, next, &pqm->queues, process_queue_list) {
+		retval = pqm_destroy_queue(
+				pqm,
+				(pqn->q != NULL) ?
+					pqn->q->properties.queue_id :
+					pqn->kq->queue->properties.queue_id);
+
+		if (retval != 0) {
+			pr_err("kfd: failed to destroy queue\n");
+			return;
+		}
+	}
+	kfree(pqm->queue_slot_bitmap);
+	pqm->queue_slot_bitmap = NULL;
+}
+
+static int create_cp_queue(struct process_queue_manager *pqm,
+				struct kfd_dev *dev, struct queue **q,
+				struct queue_properties *q_properties,
+				struct file *f, unsigned int qid)
+{
+	int retval;
+
+	retval = 0;
+
+	/* Doorbell initialized in user space*/
+	q_properties->doorbell_ptr = NULL;
+
+	q_properties->doorbell_off =
+			kfd_queue_id_to_doorbell(dev, pqm->process, qid);
+
+	/* let DQM handle it*/
+	q_properties->vmid = 0;
+	q_properties->queue_id = qid;
+	q_properties->type = KFD_QUEUE_TYPE_COMPUTE;
+
+	retval = init_queue(q, *q_properties);
+	if (retval != 0)
+		goto err_init_queue;
+
+	(*q)->device = dev;
+	(*q)->process = pqm->process;
+
+	pr_debug("kfd: PQM After init queue");
+
+	return retval;
+
+err_init_queue:
+	return retval;
+}
+
+int pqm_create_queue(struct process_queue_manager *pqm,
+			    struct kfd_dev *dev,
+			    struct file *f,
+			    struct queue_properties *properties,
+			    unsigned int flags,
+			    enum kfd_queue_type type,
+			    unsigned int *qid)
+{
+	int retval;
+	struct kfd_process_device *pdd;
+	struct queue_properties q_properties;
+	struct queue *q;
+	struct process_queue_node *pqn;
+	struct kernel_queue *kq;
+
+	BUG_ON(!pqm || !dev || !properties || !qid);
+
+	memset(&q_properties, 0, sizeof(struct queue_properties));
+	memcpy(&q_properties, properties, sizeof(struct queue_properties));
+	q = NULL;
+	kq = NULL;
+
+	pdd = kfd_get_process_device_data(dev, pqm->process, 1);
+	BUG_ON(!pdd);
+
+	retval = find_available_queue_slot(pqm, qid);
+	if (retval != 0)
+		return retval;
+
+	if (list_empty(&pqm->queues)) {
+		pdd->qpd.pqm = pqm;
+		dev->dqm->register_process(dev->dqm, &pdd->qpd);
+	}
+
+	pqn = kzalloc(sizeof(struct process_queue_node), GFP_KERNEL);
+	if (!pqn) {
+		retval = -ENOMEM;
+		goto err_allocate_pqn;
+	}
+
+	switch (type) {
+	case KFD_QUEUE_TYPE_COMPUTE:
+		/* check if there is over subscription */
+		if ((sched_policy == KFD_SCHED_POLICY_HWS_NO_OVERSUBSCRIPTION) &&
+		((dev->dqm->processes_count >= VMID_PER_DEVICE) ||
+		(dev->dqm->queue_count >= PIPE_PER_ME_CP_SCHEDULING * QUEUES_PER_PIPE))) {
+			pr_err("kfd: over-subscription is not allowed in radeon_kfd.sched_policy == 1\n");
+			retval = -EPERM;
+			goto err_create_queue;
+		}
+
+		retval = create_cp_queue(pqm, dev, &q, &q_properties, f, *qid);
+		if (retval != 0)
+			goto err_create_queue;
+		pqn->q = q;
+		pqn->kq = NULL;
+		retval = dev->dqm->create_queue(dev->dqm, q, &pdd->qpd,
+						&q->properties.vmid);
+		print_queue(q);
+		break;
+	case KFD_QUEUE_TYPE_DIQ:
+		kq = kernel_queue_init(dev, KFD_QUEUE_TYPE_DIQ);
+		if (kq == NULL) {
+			retval = -ENOMEM;
+			goto err_create_queue;
+		}
+		kq->queue->properties.queue_id = *qid;
+		pqn->kq = kq;
+		pqn->q = NULL;
+		retval = dev->dqm->create_kernel_queue(dev->dqm, kq, &pdd->qpd);
+		break;
+	default:
+		BUG();
+		break;
+	}
+
+	if (retval != 0) {
+		pr_err("kfd: error dqm create queue\n");
+		goto err_create_queue;
+	}
+
+	pr_debug("kfd: PQM After DQM create queue\n");
+
+	list_add(&pqn->process_queue_list, &pqm->queues);
+
+	if (q) {
+		*properties = q->properties;
+		pr_debug("kfd: PQM done creating queue\n");
+		print_queue_properties(properties);
+	}
+
+	return retval;
+
+err_create_queue:
+	kfree(pqn);
+err_allocate_pqn:
+	clear_bit(*qid, pqm->queue_slot_bitmap);
+	return retval;
+}
+
+int pqm_destroy_queue(struct process_queue_manager *pqm, unsigned int qid)
+{
+	struct process_queue_node *pqn;
+	struct kfd_process_device *pdd;
+	struct device_queue_manager *dqm;
+	struct kfd_dev *dev;
+	int retval;
+
+	dqm = NULL;
+
+	BUG_ON(!pqm);
+	retval = 0;
+
+	pr_debug("kfd: In Func %s\n", __func__);
+
+	pqn = get_queue_by_qid(pqm, qid);
+	if (pqn == NULL) {
+		pr_err("kfd: queue id does not match any known queue\n");
+		return -EINVAL;
+	}
+
+	dev = NULL;
+	if (pqn->kq)
+		dev = pqn->kq->dev;
+	if (pqn->q)
+		dev = pqn->q->device;
+	BUG_ON(!dev);
+
+	pdd = kfd_get_process_device_data(dev, pqm->process, 1);
+	BUG_ON(!pdd);
+
+	if (pqn->kq) {
+		/* destroy kernel queue (DIQ) */
+		dqm = pqn->kq->dev->dqm;
+		dqm->destroy_kernel_queue(dqm, pqn->kq, &pdd->qpd);
+		kernel_queue_uninit(pqn->kq);
+	}
+
+	if (pqn->q) {
+		dqm = pqn->q->device->dqm;
+		retval = dqm->destroy_queue(dqm, &pdd->qpd, pqn->q);
+		if (retval != 0)
+			return retval;
+
+		uninit_queue(pqn->q);
+	}
+
+	list_del(&pqn->process_queue_list);
+	kfree(pqn);
+	clear_bit(qid, pqm->queue_slot_bitmap);
+
+	if (list_empty(&pqm->queues))
+		dqm->unregister_process(dqm, &pdd->qpd);
+
+	return retval;
+}
+
+int pqm_update_queue(struct process_queue_manager *pqm, unsigned int qid,
+			struct queue_properties *p)
+{
+	int retval;
+	struct process_queue_node *pqn;
+
+	BUG_ON(!pqm);
+
+	pqn = get_queue_by_qid(pqm, qid);
+	BUG_ON(!pqn);
+
+	pqn->q->properties.queue_address = p->queue_address;
+	pqn->q->properties.queue_size = p->queue_size;
+	pqn->q->properties.queue_percent = p->queue_percent;
+	pqn->q->properties.priority = p->priority;
+
+	retval = pqn->q->device->dqm->update_queue(pqn->q->device->dqm, pqn->q);
+	if (retval != 0)
+		return retval;
+
+	return 0;
+}
+
+static __attribute__((unused)) struct kernel_queue *pqm_get_kernel_queue(
+					struct process_queue_manager *pqm,
+					unsigned int qid)
+{
+	struct process_queue_node *pqn;
+
+	BUG_ON(!pqm);
+
+	pqn = get_queue_by_qid(pqm, qid);
+	if (pqn && pqn->kq)
+		return pqn->kq;
+
+	return NULL;
+}
+
+
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_queue.c b/drivers/gpu/drm/amd/amdkfd/kfd_queue.c
new file mode 100644
index 000000000000..9a0c90b0702e
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_queue.c
@@ -0,0 +1,85 @@
+/*
+ * Copyright 2014 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.
+ *
+ */
+
+#include <linux/slab.h>
+#include "kfd_priv.h"
+
+void print_queue_properties(struct queue_properties *q)
+{
+	if (!q)
+		return;
+
+	pr_debug("Printing queue properties:\n");
+	pr_debug("Queue Type: %u\n", q->type);
+	pr_debug("Queue Size: %llu\n", q->queue_size);
+	pr_debug("Queue percent: %u\n", q->queue_percent);
+	pr_debug("Queue Address: 0x%llX\n", q->queue_address);
+	pr_debug("Queue Id: %u\n", q->queue_id);
+	pr_debug("Queue Process Vmid: %u\n", q->vmid);
+	pr_debug("Queue Read Pointer: 0x%p\n", q->read_ptr);
+	pr_debug("Queue Write Pointer: 0x%p\n", q->write_ptr);
+	pr_debug("Queue Doorbell Pointer: 0x%p\n", q->doorbell_ptr);
+	pr_debug("Queue Doorbell Offset: %u\n", q->doorbell_off);
+}
+
+void print_queue(struct queue *q)
+{
+	if (!q)
+		return;
+	pr_debug("Printing queue:\n");
+	pr_debug("Queue Type: %u\n", q->properties.type);
+	pr_debug("Queue Size: %llu\n", q->properties.queue_size);
+	pr_debug("Queue percent: %u\n", q->properties.queue_percent);
+	pr_debug("Queue Address: 0x%llX\n", q->properties.queue_address);
+	pr_debug("Queue Id: %u\n", q->properties.queue_id);
+	pr_debug("Queue Process Vmid: %u\n", q->properties.vmid);
+	pr_debug("Queue Read Pointer: 0x%p\n", q->properties.read_ptr);
+	pr_debug("Queue Write Pointer: 0x%p\n", q->properties.write_ptr);
+	pr_debug("Queue Doorbell Pointer: 0x%p\n", q->properties.doorbell_ptr);
+	pr_debug("Queue Doorbell Offset: %u\n", q->properties.doorbell_off);
+	pr_debug("Queue MQD Address: 0x%p\n", q->mqd);
+	pr_debug("Queue MQD Gart: 0x%llX\n", q->gart_mqd_addr);
+	pr_debug("Queue Process Address: 0x%p\n", q->process);
+	pr_debug("Queue Device Address: 0x%p\n", q->device);
+}
+
+int init_queue(struct queue **q, struct queue_properties properties)
+{
+	struct queue *tmp;
+
+	BUG_ON(!q);
+
+	tmp = kzalloc(sizeof(struct queue), GFP_KERNEL);
+	if (!tmp)
+		return -ENOMEM;
+
+	memcpy(&tmp->properties, &properties, sizeof(struct queue_properties));
+
+	*q = tmp;
+	return 0;
+}
+
+void uninit_queue(struct queue *q)
+{
+	kfree(q);
+}
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_topology.c b/drivers/gpu/drm/amd/amdkfd/kfd_topology.c
new file mode 100644
index 000000000000..b11792d7e70e
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_topology.c
@@ -0,0 +1,1239 @@
+/*
+ * Copyright 2014 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.
+ */
+
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/pci.h>
+#include <linux/errno.h>
+#include <linux/acpi.h>
+#include <linux/hash.h>
+#include <linux/cpufreq.h>
+
+#include "kfd_priv.h"
+#include "kfd_crat.h"
+#include "kfd_topology.h"
+
+static struct list_head topology_device_list;
+static int topology_crat_parsed;
+static struct kfd_system_properties sys_props;
+
+static DECLARE_RWSEM(topology_lock);
+
+struct kfd_dev *kfd_device_by_id(uint32_t gpu_id)
+{
+	struct kfd_topology_device *top_dev;
+	struct kfd_dev *device = NULL;
+
+	down_read(&topology_lock);
+
+	list_for_each_entry(top_dev, &topology_device_list, list)
+		if (top_dev->gpu_id == gpu_id) {
+			device = top_dev->gpu;
+			break;
+		}
+
+	up_read(&topology_lock);
+
+	return device;
+}
+
+struct kfd_dev *kfd_device_by_pci_dev(const struct pci_dev *pdev)
+{
+	struct kfd_topology_device *top_dev;
+	struct kfd_dev *device = NULL;
+
+	down_read(&topology_lock);
+
+	list_for_each_entry(top_dev, &topology_device_list, list)
+		if (top_dev->gpu->pdev == pdev) {
+			device = top_dev->gpu;
+			break;
+		}
+
+	up_read(&topology_lock);
+
+	return device;
+}
+
+static int kfd_topology_get_crat_acpi(void *crat_image, size_t *size)
+{
+	struct acpi_table_header *crat_table;
+	acpi_status status;
+
+	if (!size)
+		return -EINVAL;
+
+	/*
+	 * Fetch the CRAT table from ACPI
+	 */
+	status = acpi_get_table(CRAT_SIGNATURE, 0, &crat_table);
+	if (status == AE_NOT_FOUND) {
+		pr_warn("CRAT table not found\n");
+		return -ENODATA;
+	} else if (ACPI_FAILURE(status)) {
+		const char *err = acpi_format_exception(status);
+
+		pr_err("CRAT table error: %s\n", err);
+		return -EINVAL;
+	}
+
+	if (*size >= crat_table->length && crat_image != NULL)
+		memcpy(crat_image, crat_table, crat_table->length);
+
+	*size = crat_table->length;
+
+	return 0;
+}
+
+static void kfd_populated_cu_info_cpu(struct kfd_topology_device *dev,
+		struct crat_subtype_computeunit *cu)
+{
+	BUG_ON(!dev);
+	BUG_ON(!cu);
+
+	dev->node_props.cpu_cores_count = cu->num_cpu_cores;
+	dev->node_props.cpu_core_id_base = cu->processor_id_low;
+	if (cu->hsa_capability & CRAT_CU_FLAGS_IOMMU_PRESENT)
+		dev->node_props.capability |= HSA_CAP_ATS_PRESENT;
+
+	pr_info("CU CPU: cores=%d id_base=%d\n", cu->num_cpu_cores,
+			cu->processor_id_low);
+}
+
+static void kfd_populated_cu_info_gpu(struct kfd_topology_device *dev,
+		struct crat_subtype_computeunit *cu)
+{
+	BUG_ON(!dev);
+	BUG_ON(!cu);
+
+	dev->node_props.simd_id_base = cu->processor_id_low;
+	dev->node_props.simd_count = cu->num_simd_cores;
+	dev->node_props.lds_size_in_kb = cu->lds_size_in_kb;
+	dev->node_props.max_waves_per_simd = cu->max_waves_simd;
+	dev->node_props.wave_front_size = cu->wave_front_size;
+	dev->node_props.mem_banks_count = cu->num_banks;
+	dev->node_props.array_count = cu->num_arrays;
+	dev->node_props.cu_per_simd_array = cu->num_cu_per_array;
+	dev->node_props.simd_per_cu = cu->num_simd_per_cu;
+	dev->node_props.max_slots_scratch_cu = cu->max_slots_scatch_cu;
+	if (cu->hsa_capability & CRAT_CU_FLAGS_HOT_PLUGGABLE)
+		dev->node_props.capability |= HSA_CAP_HOT_PLUGGABLE;
+	pr_info("CU GPU: simds=%d id_base=%d\n", cu->num_simd_cores,
+				cu->processor_id_low);
+}
+
+/* kfd_parse_subtype_cu is called when the topology mutex is already acquired */
+static int kfd_parse_subtype_cu(struct crat_subtype_computeunit *cu)
+{
+	struct kfd_topology_device *dev;
+	int i = 0;
+
+	BUG_ON(!cu);
+
+	pr_info("Found CU entry in CRAT table with proximity_domain=%d caps=%x\n",
+			cu->proximity_domain, cu->hsa_capability);
+	list_for_each_entry(dev, &topology_device_list, list) {
+		if (cu->proximity_domain == i) {
+			if (cu->flags & CRAT_CU_FLAGS_CPU_PRESENT)
+				kfd_populated_cu_info_cpu(dev, cu);
+
+			if (cu->flags & CRAT_CU_FLAGS_GPU_PRESENT)
+				kfd_populated_cu_info_gpu(dev, cu);
+			break;
+		}
+		i++;
+	}
+
+	return 0;
+}
+
+/*
+ * kfd_parse_subtype_mem is called when the topology mutex is
+ * already acquired
+ */
+static int kfd_parse_subtype_mem(struct crat_subtype_memory *mem)
+{
+	struct kfd_mem_properties *props;
+	struct kfd_topology_device *dev;
+	int i = 0;
+
+	BUG_ON(!mem);
+
+	pr_info("Found memory entry in CRAT table with proximity_domain=%d\n",
+			mem->promixity_domain);
+	list_for_each_entry(dev, &topology_device_list, list) {
+		if (mem->promixity_domain == i) {
+			props = kfd_alloc_struct(props);
+			if (props == NULL)
+				return -ENOMEM;
+
+			if (dev->node_props.cpu_cores_count == 0)
+				props->heap_type = HSA_MEM_HEAP_TYPE_FB_PRIVATE;
+			else
+				props->heap_type = HSA_MEM_HEAP_TYPE_SYSTEM;
+
+			if (mem->flags & CRAT_MEM_FLAGS_HOT_PLUGGABLE)
+				props->flags |= HSA_MEM_FLAGS_HOT_PLUGGABLE;
+			if (mem->flags & CRAT_MEM_FLAGS_NON_VOLATILE)
+				props->flags |= HSA_MEM_FLAGS_NON_VOLATILE;
+
+			props->size_in_bytes =
+				((uint64_t)mem->length_high << 32) +
+							mem->length_low;
+			props->width = mem->width;
+
+			dev->mem_bank_count++;
+			list_add_tail(&props->list, &dev->mem_props);
+
+			break;
+		}
+		i++;
+	}
+
+	return 0;
+}
+
+/*
+ * kfd_parse_subtype_cache is called when the topology mutex
+ * is already acquired
+ */
+static int kfd_parse_subtype_cache(struct crat_subtype_cache *cache)
+{
+	struct kfd_cache_properties *props;
+	struct kfd_topology_device *dev;
+	uint32_t id;
+
+	BUG_ON(!cache);
+
+	id = cache->processor_id_low;
+
+	pr_info("Found cache entry in CRAT table with processor_id=%d\n", id);
+	list_for_each_entry(dev, &topology_device_list, list)
+		if (id == dev->node_props.cpu_core_id_base ||
+		    id == dev->node_props.simd_id_base) {
+			props = kfd_alloc_struct(props);
+			if (props == NULL)
+				return -ENOMEM;
+
+			props->processor_id_low = id;
+			props->cache_level = cache->cache_level;
+			props->cache_size = cache->cache_size;
+			props->cacheline_size = cache->cache_line_size;
+			props->cachelines_per_tag = cache->lines_per_tag;
+			props->cache_assoc = cache->associativity;
+			props->cache_latency = cache->cache_latency;
+
+			if (cache->flags & CRAT_CACHE_FLAGS_DATA_CACHE)
+				props->cache_type |= HSA_CACHE_TYPE_DATA;
+			if (cache->flags & CRAT_CACHE_FLAGS_INST_CACHE)
+				props->cache_type |= HSA_CACHE_TYPE_INSTRUCTION;
+			if (cache->flags & CRAT_CACHE_FLAGS_CPU_CACHE)
+				props->cache_type |= HSA_CACHE_TYPE_CPU;
+			if (cache->flags & CRAT_CACHE_FLAGS_SIMD_CACHE)
+				props->cache_type |= HSA_CACHE_TYPE_HSACU;
+
+			dev->cache_count++;
+			dev->node_props.caches_count++;
+			list_add_tail(&props->list, &dev->cache_props);
+
+			break;
+		}
+
+	return 0;
+}
+
+/*
+ * kfd_parse_subtype_iolink is called when the topology mutex
+ * is already acquired
+ */
+static int kfd_parse_subtype_iolink(struct crat_subtype_iolink *iolink)
+{
+	struct kfd_iolink_properties *props;
+	struct kfd_topology_device *dev;
+	uint32_t i = 0;
+	uint32_t id_from;
+	uint32_t id_to;
+
+	BUG_ON(!iolink);
+
+	id_from = iolink->proximity_domain_from;
+	id_to = iolink->proximity_domain_to;
+
+	pr_info("Found IO link entry in CRAT table with id_from=%d\n", id_from);
+	list_for_each_entry(dev, &topology_device_list, list) {
+		if (id_from == i) {
+			props = kfd_alloc_struct(props);
+			if (props == NULL)
+				return -ENOMEM;
+
+			props->node_from = id_from;
+			props->node_to = id_to;
+			props->ver_maj = iolink->version_major;
+			props->ver_min = iolink->version_minor;
+
+			/*
+			 * weight factor (derived from CDIR), currently always 1
+			 */
+			props->weight = 1;
+
+			props->min_latency = iolink->minimum_latency;
+			props->max_latency = iolink->maximum_latency;
+			props->min_bandwidth = iolink->minimum_bandwidth_mbs;
+			props->max_bandwidth = iolink->maximum_bandwidth_mbs;
+			props->rec_transfer_size =
+					iolink->recommended_transfer_size;
+
+			dev->io_link_count++;
+			dev->node_props.io_links_count++;
+			list_add_tail(&props->list, &dev->io_link_props);
+
+			break;
+		}
+		i++;
+	}
+
+	return 0;
+}
+
+static int kfd_parse_subtype(struct crat_subtype_generic *sub_type_hdr)
+{
+	struct crat_subtype_computeunit *cu;
+	struct crat_subtype_memory *mem;
+	struct crat_subtype_cache *cache;
+	struct crat_subtype_iolink *iolink;
+	int ret = 0;
+
+	BUG_ON(!sub_type_hdr);
+
+	switch (sub_type_hdr->type) {
+	case CRAT_SUBTYPE_COMPUTEUNIT_AFFINITY:
+		cu = (struct crat_subtype_computeunit *)sub_type_hdr;
+		ret = kfd_parse_subtype_cu(cu);
+		break;
+	case CRAT_SUBTYPE_MEMORY_AFFINITY:
+		mem = (struct crat_subtype_memory *)sub_type_hdr;
+		ret = kfd_parse_subtype_mem(mem);
+		break;
+	case CRAT_SUBTYPE_CACHE_AFFINITY:
+		cache = (struct crat_subtype_cache *)sub_type_hdr;
+		ret = kfd_parse_subtype_cache(cache);
+		break;
+	case CRAT_SUBTYPE_TLB_AFFINITY:
+		/*
+		 * For now, nothing to do here
+		 */
+		pr_info("Found TLB entry in CRAT table (not processing)\n");
+		break;
+	case CRAT_SUBTYPE_CCOMPUTE_AFFINITY:
+		/*
+		 * For now, nothing to do here
+		 */
+		pr_info("Found CCOMPUTE entry in CRAT table (not processing)\n");
+		break;
+	case CRAT_SUBTYPE_IOLINK_AFFINITY:
+		iolink = (struct crat_subtype_iolink *)sub_type_hdr;
+		ret = kfd_parse_subtype_iolink(iolink);
+		break;
+	default:
+		pr_warn("Unknown subtype (%d) in CRAT\n",
+				sub_type_hdr->type);
+	}
+
+	return ret;
+}
+
+static void kfd_release_topology_device(struct kfd_topology_device *dev)
+{
+	struct kfd_mem_properties *mem;
+	struct kfd_cache_properties *cache;
+	struct kfd_iolink_properties *iolink;
+
+	BUG_ON(!dev);
+
+	list_del(&dev->list);
+
+	while (dev->mem_props.next != &dev->mem_props) {
+		mem = container_of(dev->mem_props.next,
+				struct kfd_mem_properties, list);
+		list_del(&mem->list);
+		kfree(mem);
+	}
+
+	while (dev->cache_props.next != &dev->cache_props) {
+		cache = container_of(dev->cache_props.next,
+				struct kfd_cache_properties, list);
+		list_del(&cache->list);
+		kfree(cache);
+	}
+
+	while (dev->io_link_props.next != &dev->io_link_props) {
+		iolink = container_of(dev->io_link_props.next,
+				struct kfd_iolink_properties, list);
+		list_del(&iolink->list);
+		kfree(iolink);
+	}
+
+	kfree(dev);
+
+	sys_props.num_devices--;
+}
+
+static void kfd_release_live_view(void)
+{
+	struct kfd_topology_device *dev;
+
+	while (topology_device_list.next != &topology_device_list) {
+		dev = container_of(topology_device_list.next,
+				 struct kfd_topology_device, list);
+		kfd_release_topology_device(dev);
+}
+
+	memset(&sys_props, 0, sizeof(sys_props));
+}
+
+static struct kfd_topology_device *kfd_create_topology_device(void)
+{
+	struct kfd_topology_device *dev;
+
+	dev = kfd_alloc_struct(dev);
+	if (dev == NULL) {
+		pr_err("No memory to allocate a topology device");
+		return NULL;
+	}
+
+	INIT_LIST_HEAD(&dev->mem_props);
+	INIT_LIST_HEAD(&dev->cache_props);
+	INIT_LIST_HEAD(&dev->io_link_props);
+
+	list_add_tail(&dev->list, &topology_device_list);
+	sys_props.num_devices++;
+
+	return dev;
+}
+
+static int kfd_parse_crat_table(void *crat_image)
+{
+	struct kfd_topology_device *top_dev;
+	struct crat_subtype_generic *sub_type_hdr;
+	uint16_t node_id;
+	int ret;
+	struct crat_header *crat_table = (struct crat_header *)crat_image;
+	uint16_t num_nodes;
+	uint32_t image_len;
+
+	if (!crat_image)
+		return -EINVAL;
+
+	num_nodes = crat_table->num_domains;
+	image_len = crat_table->length;
+
+	pr_info("Parsing CRAT table with %d nodes\n", num_nodes);
+
+	for (node_id = 0; node_id < num_nodes; node_id++) {
+		top_dev = kfd_create_topology_device();
+		if (!top_dev) {
+			kfd_release_live_view();
+			return -ENOMEM;
+		}
+	}
+
+	sys_props.platform_id =
+		(*((uint64_t *)crat_table->oem_id)) & CRAT_OEMID_64BIT_MASK;
+	sys_props.platform_oem = *((uint64_t *)crat_table->oem_table_id);
+	sys_props.platform_rev = crat_table->revision;
+
+	sub_type_hdr = (struct crat_subtype_generic *)(crat_table+1);
+	while ((char *)sub_type_hdr + sizeof(struct crat_subtype_generic) <
+			((char *)crat_image) + image_len) {
+		if (sub_type_hdr->flags & CRAT_SUBTYPE_FLAGS_ENABLED) {
+			ret = kfd_parse_subtype(sub_type_hdr);
+			if (ret != 0) {
+				kfd_release_live_view();
+				return ret;
+			}
+		}
+
+		sub_type_hdr = (typeof(sub_type_hdr))((char *)sub_type_hdr +
+				sub_type_hdr->length);
+	}
+
+	sys_props.generation_count++;
+	topology_crat_parsed = 1;
+
+	return 0;
+}
+
+
+#define sysfs_show_gen_prop(buffer, fmt, ...) \
+		snprintf(buffer, PAGE_SIZE, "%s"fmt, buffer, __VA_ARGS__)
+#define sysfs_show_32bit_prop(buffer, name, value) \
+		sysfs_show_gen_prop(buffer, "%s %u\n", name, value)
+#define sysfs_show_64bit_prop(buffer, name, value) \
+		sysfs_show_gen_prop(buffer, "%s %llu\n", name, value)
+#define sysfs_show_32bit_val(buffer, value) \
+		sysfs_show_gen_prop(buffer, "%u\n", value)
+#define sysfs_show_str_val(buffer, value) \
+		sysfs_show_gen_prop(buffer, "%s\n", value)
+
+static ssize_t sysprops_show(struct kobject *kobj, struct attribute *attr,
+		char *buffer)
+{
+	ssize_t ret;
+
+	/* Making sure that the buffer is an empty string */
+	buffer[0] = 0;
+
+	if (attr == &sys_props.attr_genid) {
+		ret = sysfs_show_32bit_val(buffer, sys_props.generation_count);
+	} else if (attr == &sys_props.attr_props) {
+		sysfs_show_64bit_prop(buffer, "platform_oem",
+				sys_props.platform_oem);
+		sysfs_show_64bit_prop(buffer, "platform_id",
+				sys_props.platform_id);
+		ret = sysfs_show_64bit_prop(buffer, "platform_rev",
+				sys_props.platform_rev);
+	} else {
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static const struct sysfs_ops sysprops_ops = {
+	.show = sysprops_show,
+};
+
+static struct kobj_type sysprops_type = {
+	.sysfs_ops = &sysprops_ops,
+};
+
+static ssize_t iolink_show(struct kobject *kobj, struct attribute *attr,
+		char *buffer)
+{
+	ssize_t ret;
+	struct kfd_iolink_properties *iolink;
+
+	/* Making sure that the buffer is an empty string */
+	buffer[0] = 0;
+
+	iolink = container_of(attr, struct kfd_iolink_properties, attr);
+	sysfs_show_32bit_prop(buffer, "type", iolink->iolink_type);
+	sysfs_show_32bit_prop(buffer, "version_major", iolink->ver_maj);
+	sysfs_show_32bit_prop(buffer, "version_minor", iolink->ver_min);
+	sysfs_show_32bit_prop(buffer, "node_from", iolink->node_from);
+	sysfs_show_32bit_prop(buffer, "node_to", iolink->node_to);
+	sysfs_show_32bit_prop(buffer, "weight", iolink->weight);
+	sysfs_show_32bit_prop(buffer, "min_latency", iolink->min_latency);
+	sysfs_show_32bit_prop(buffer, "max_latency", iolink->max_latency);
+	sysfs_show_32bit_prop(buffer, "min_bandwidth", iolink->min_bandwidth);
+	sysfs_show_32bit_prop(buffer, "max_bandwidth", iolink->max_bandwidth);
+	sysfs_show_32bit_prop(buffer, "recommended_transfer_size",
+			iolink->rec_transfer_size);
+	ret = sysfs_show_32bit_prop(buffer, "flags", iolink->flags);
+
+	return ret;
+}
+
+static const struct sysfs_ops iolink_ops = {
+	.show = iolink_show,
+};
+
+static struct kobj_type iolink_type = {
+	.sysfs_ops = &iolink_ops,
+};
+
+static ssize_t mem_show(struct kobject *kobj, struct attribute *attr,
+		char *buffer)
+{
+	ssize_t ret;
+	struct kfd_mem_properties *mem;
+
+	/* Making sure that the buffer is an empty string */
+	buffer[0] = 0;
+
+	mem = container_of(attr, struct kfd_mem_properties, attr);
+	sysfs_show_32bit_prop(buffer, "heap_type", mem->heap_type);
+	sysfs_show_64bit_prop(buffer, "size_in_bytes", mem->size_in_bytes);
+	sysfs_show_32bit_prop(buffer, "flags", mem->flags);
+	sysfs_show_32bit_prop(buffer, "width", mem->width);
+	ret = sysfs_show_32bit_prop(buffer, "mem_clk_max", mem->mem_clk_max);
+
+	return ret;
+}
+
+static const struct sysfs_ops mem_ops = {
+	.show = mem_show,
+};
+
+static struct kobj_type mem_type = {
+	.sysfs_ops = &mem_ops,
+};
+
+static ssize_t kfd_cache_show(struct kobject *kobj, struct attribute *attr,
+		char *buffer)
+{
+	ssize_t ret;
+	uint32_t i;
+	struct kfd_cache_properties *cache;
+
+	/* Making sure that the buffer is an empty string */
+	buffer[0] = 0;
+
+	cache = container_of(attr, struct kfd_cache_properties, attr);
+	sysfs_show_32bit_prop(buffer, "processor_id_low",
+			cache->processor_id_low);
+	sysfs_show_32bit_prop(buffer, "level", cache->cache_level);
+	sysfs_show_32bit_prop(buffer, "size", cache->cache_size);
+	sysfs_show_32bit_prop(buffer, "cache_line_size", cache->cacheline_size);
+	sysfs_show_32bit_prop(buffer, "cache_lines_per_tag",
+			cache->cachelines_per_tag);
+	sysfs_show_32bit_prop(buffer, "association", cache->cache_assoc);
+	sysfs_show_32bit_prop(buffer, "latency", cache->cache_latency);
+	sysfs_show_32bit_prop(buffer, "type", cache->cache_type);
+	snprintf(buffer, PAGE_SIZE, "%ssibling_map ", buffer);
+	for (i = 0; i < KFD_TOPOLOGY_CPU_SIBLINGS; i++)
+		ret = snprintf(buffer, PAGE_SIZE, "%s%d%s",
+				buffer, cache->sibling_map[i],
+				(i == KFD_TOPOLOGY_CPU_SIBLINGS-1) ?
+						"\n" : ",");
+
+	return ret;
+}
+
+static const struct sysfs_ops cache_ops = {
+	.show = kfd_cache_show,
+};
+
+static struct kobj_type cache_type = {
+	.sysfs_ops = &cache_ops,
+};
+
+static ssize_t node_show(struct kobject *kobj, struct attribute *attr,
+		char *buffer)
+{
+	ssize_t ret;
+	struct kfd_topology_device *dev;
+	char public_name[KFD_TOPOLOGY_PUBLIC_NAME_SIZE];
+	uint32_t i;
+
+	/* Making sure that the buffer is an empty string */
+	buffer[0] = 0;
+
+	if (strcmp(attr->name, "gpu_id") == 0) {
+		dev = container_of(attr, struct kfd_topology_device,
+				attr_gpuid);
+		ret = sysfs_show_32bit_val(buffer, dev->gpu_id);
+	} else if (strcmp(attr->name, "name") == 0) {
+		dev = container_of(attr, struct kfd_topology_device,
+				attr_name);
+		for (i = 0; i < KFD_TOPOLOGY_PUBLIC_NAME_SIZE; i++) {
+			public_name[i] =
+					(char)dev->node_props.marketing_name[i];
+			if (dev->node_props.marketing_name[i] == 0)
+				break;
+		}
+		public_name[KFD_TOPOLOGY_PUBLIC_NAME_SIZE-1] = 0x0;
+		ret = sysfs_show_str_val(buffer, public_name);
+	} else {
+		dev = container_of(attr, struct kfd_topology_device,
+				attr_props);
+		sysfs_show_32bit_prop(buffer, "cpu_cores_count",
+				dev->node_props.cpu_cores_count);
+		sysfs_show_32bit_prop(buffer, "simd_count",
+				dev->node_props.simd_count);
+
+		if (dev->mem_bank_count < dev->node_props.mem_banks_count) {
+			pr_warn("kfd: mem_banks_count truncated from %d to %d\n",
+					dev->node_props.mem_banks_count,
+					dev->mem_bank_count);
+			sysfs_show_32bit_prop(buffer, "mem_banks_count",
+					dev->mem_bank_count);
+		} else {
+			sysfs_show_32bit_prop(buffer, "mem_banks_count",
+					dev->node_props.mem_banks_count);
+		}
+
+		sysfs_show_32bit_prop(buffer, "caches_count",
+				dev->node_props.caches_count);
+		sysfs_show_32bit_prop(buffer, "io_links_count",
+				dev->node_props.io_links_count);
+		sysfs_show_32bit_prop(buffer, "cpu_core_id_base",
+				dev->node_props.cpu_core_id_base);
+		sysfs_show_32bit_prop(buffer, "simd_id_base",
+				dev->node_props.simd_id_base);
+		sysfs_show_32bit_prop(buffer, "capability",
+				dev->node_props.capability);
+		sysfs_show_32bit_prop(buffer, "max_waves_per_simd",
+				dev->node_props.max_waves_per_simd);
+		sysfs_show_32bit_prop(buffer, "lds_size_in_kb",
+				dev->node_props.lds_size_in_kb);
+		sysfs_show_32bit_prop(buffer, "gds_size_in_kb",
+				dev->node_props.gds_size_in_kb);
+		sysfs_show_32bit_prop(buffer, "wave_front_size",
+				dev->node_props.wave_front_size);
+		sysfs_show_32bit_prop(buffer, "array_count",
+				dev->node_props.array_count);
+		sysfs_show_32bit_prop(buffer, "simd_arrays_per_engine",
+				dev->node_props.simd_arrays_per_engine);
+		sysfs_show_32bit_prop(buffer, "cu_per_simd_array",
+				dev->node_props.cu_per_simd_array);
+		sysfs_show_32bit_prop(buffer, "simd_per_cu",
+				dev->node_props.simd_per_cu);
+		sysfs_show_32bit_prop(buffer, "max_slots_scratch_cu",
+				dev->node_props.max_slots_scratch_cu);
+		sysfs_show_32bit_prop(buffer, "vendor_id",
+				dev->node_props.vendor_id);
+		sysfs_show_32bit_prop(buffer, "device_id",
+				dev->node_props.device_id);
+		sysfs_show_32bit_prop(buffer, "location_id",
+				dev->node_props.location_id);
+
+		if (dev->gpu) {
+			sysfs_show_32bit_prop(buffer, "max_engine_clk_fcompute",
+					kfd2kgd->get_max_engine_clock_in_mhz(
+						dev->gpu->kgd));
+			sysfs_show_64bit_prop(buffer, "local_mem_size",
+					kfd2kgd->get_vmem_size(dev->gpu->kgd));
+
+			sysfs_show_32bit_prop(buffer, "fw_version",
+					kfd2kgd->get_fw_version(
+							dev->gpu->kgd,
+							KGD_ENGINE_MEC1));
+
+		}
+
+		ret = sysfs_show_32bit_prop(buffer, "max_engine_clk_ccompute",
+				cpufreq_quick_get_max(0)/1000);
+	}
+
+	return ret;
+}
+
+static const struct sysfs_ops node_ops = {
+	.show = node_show,
+};
+
+static struct kobj_type node_type = {
+	.sysfs_ops = &node_ops,
+};
+
+static void kfd_remove_sysfs_file(struct kobject *kobj, struct attribute *attr)
+{
+	sysfs_remove_file(kobj, attr);
+	kobject_del(kobj);
+	kobject_put(kobj);
+}
+
+static void kfd_remove_sysfs_node_entry(struct kfd_topology_device *dev)
+{
+	struct kfd_iolink_properties *iolink;
+	struct kfd_cache_properties *cache;
+	struct kfd_mem_properties *mem;
+
+	BUG_ON(!dev);
+
+	if (dev->kobj_iolink) {
+		list_for_each_entry(iolink, &dev->io_link_props, list)
+			if (iolink->kobj) {
+				kfd_remove_sysfs_file(iolink->kobj,
+							&iolink->attr);
+				iolink->kobj = NULL;
+			}
+		kobject_del(dev->kobj_iolink);
+		kobject_put(dev->kobj_iolink);
+		dev->kobj_iolink = NULL;
+	}
+
+	if (dev->kobj_cache) {
+		list_for_each_entry(cache, &dev->cache_props, list)
+			if (cache->kobj) {
+				kfd_remove_sysfs_file(cache->kobj,
+							&cache->attr);
+				cache->kobj = NULL;
+			}
+		kobject_del(dev->kobj_cache);
+		kobject_put(dev->kobj_cache);
+		dev->kobj_cache = NULL;
+	}
+
+	if (dev->kobj_mem) {
+		list_for_each_entry(mem, &dev->mem_props, list)
+			if (mem->kobj) {
+				kfd_remove_sysfs_file(mem->kobj, &mem->attr);
+				mem->kobj = NULL;
+			}
+		kobject_del(dev->kobj_mem);
+		kobject_put(dev->kobj_mem);
+		dev->kobj_mem = NULL;
+	}
+
+	if (dev->kobj_node) {
+		sysfs_remove_file(dev->kobj_node, &dev->attr_gpuid);
+		sysfs_remove_file(dev->kobj_node, &dev->attr_name);
+		sysfs_remove_file(dev->kobj_node, &dev->attr_props);
+		kobject_del(dev->kobj_node);
+		kobject_put(dev->kobj_node);
+		dev->kobj_node = NULL;
+	}
+}
+
+static int kfd_build_sysfs_node_entry(struct kfd_topology_device *dev,
+		uint32_t id)
+{
+	struct kfd_iolink_properties *iolink;
+	struct kfd_cache_properties *cache;
+	struct kfd_mem_properties *mem;
+	int ret;
+	uint32_t i;
+
+	BUG_ON(!dev);
+
+	/*
+	 * Creating the sysfs folders
+	 */
+	BUG_ON(dev->kobj_node);
+	dev->kobj_node = kfd_alloc_struct(dev->kobj_node);
+	if (!dev->kobj_node)
+		return -ENOMEM;
+
+	ret = kobject_init_and_add(dev->kobj_node, &node_type,
+			sys_props.kobj_nodes, "%d", id);
+	if (ret < 0)
+		return ret;
+
+	dev->kobj_mem = kobject_create_and_add("mem_banks", dev->kobj_node);
+	if (!dev->kobj_mem)
+		return -ENOMEM;
+
+	dev->kobj_cache = kobject_create_and_add("caches", dev->kobj_node);
+	if (!dev->kobj_cache)
+		return -ENOMEM;
+
+	dev->kobj_iolink = kobject_create_and_add("io_links", dev->kobj_node);
+	if (!dev->kobj_iolink)
+		return -ENOMEM;
+
+	/*
+	 * Creating sysfs files for node properties
+	 */
+	dev->attr_gpuid.name = "gpu_id";
+	dev->attr_gpuid.mode = KFD_SYSFS_FILE_MODE;
+	sysfs_attr_init(&dev->attr_gpuid);
+	dev->attr_name.name = "name";
+	dev->attr_name.mode = KFD_SYSFS_FILE_MODE;
+	sysfs_attr_init(&dev->attr_name);
+	dev->attr_props.name = "properties";
+	dev->attr_props.mode = KFD_SYSFS_FILE_MODE;
+	sysfs_attr_init(&dev->attr_props);
+	ret = sysfs_create_file(dev->kobj_node, &dev->attr_gpuid);
+	if (ret < 0)
+		return ret;
+	ret = sysfs_create_file(dev->kobj_node, &dev->attr_name);
+	if (ret < 0)
+		return ret;
+	ret = sysfs_create_file(dev->kobj_node, &dev->attr_props);
+	if (ret < 0)
+		return ret;
+
+	i = 0;
+	list_for_each_entry(mem, &dev->mem_props, list) {
+		mem->kobj = kzalloc(sizeof(struct kobject), GFP_KERNEL);
+		if (!mem->kobj)
+			return -ENOMEM;
+		ret = kobject_init_and_add(mem->kobj, &mem_type,
+				dev->kobj_mem, "%d", i);
+		if (ret < 0)
+			return ret;
+
+		mem->attr.name = "properties";
+		mem->attr.mode = KFD_SYSFS_FILE_MODE;
+		sysfs_attr_init(&mem->attr);
+		ret = sysfs_create_file(mem->kobj, &mem->attr);
+		if (ret < 0)
+			return ret;
+		i++;
+	}
+
+	i = 0;
+	list_for_each_entry(cache, &dev->cache_props, list) {
+		cache->kobj = kzalloc(sizeof(struct kobject), GFP_KERNEL);
+		if (!cache->kobj)
+			return -ENOMEM;
+		ret = kobject_init_and_add(cache->kobj, &cache_type,
+				dev->kobj_cache, "%d", i);
+		if (ret < 0)
+			return ret;
+
+		cache->attr.name = "properties";
+		cache->attr.mode = KFD_SYSFS_FILE_MODE;
+		sysfs_attr_init(&cache->attr);
+		ret = sysfs_create_file(cache->kobj, &cache->attr);
+		if (ret < 0)
+			return ret;
+		i++;
+	}
+
+	i = 0;
+	list_for_each_entry(iolink, &dev->io_link_props, list) {
+		iolink->kobj = kzalloc(sizeof(struct kobject), GFP_KERNEL);
+		if (!iolink->kobj)
+			return -ENOMEM;
+		ret = kobject_init_and_add(iolink->kobj, &iolink_type,
+				dev->kobj_iolink, "%d", i);
+		if (ret < 0)
+			return ret;
+
+		iolink->attr.name = "properties";
+		iolink->attr.mode = KFD_SYSFS_FILE_MODE;
+		sysfs_attr_init(&iolink->attr);
+		ret = sysfs_create_file(iolink->kobj, &iolink->attr);
+		if (ret < 0)
+			return ret;
+		i++;
+}
+
+	return 0;
+}
+
+static int kfd_build_sysfs_node_tree(void)
+{
+	struct kfd_topology_device *dev;
+	int ret;
+	uint32_t i = 0;
+
+	list_for_each_entry(dev, &topology_device_list, list) {
+		ret = kfd_build_sysfs_node_entry(dev, 0);
+		if (ret < 0)
+			return ret;
+		i++;
+	}
+
+	return 0;
+}
+
+static void kfd_remove_sysfs_node_tree(void)
+{
+	struct kfd_topology_device *dev;
+
+	list_for_each_entry(dev, &topology_device_list, list)
+		kfd_remove_sysfs_node_entry(dev);
+}
+
+static int kfd_topology_update_sysfs(void)
+{
+	int ret;
+
+	pr_info("Creating topology SYSFS entries\n");
+	if (sys_props.kobj_topology == NULL) {
+		sys_props.kobj_topology =
+				kfd_alloc_struct(sys_props.kobj_topology);
+		if (!sys_props.kobj_topology)
+			return -ENOMEM;
+
+		ret = kobject_init_and_add(sys_props.kobj_topology,
+				&sysprops_type,  &kfd_device->kobj,
+				"topology");
+		if (ret < 0)
+			return ret;
+
+		sys_props.kobj_nodes = kobject_create_and_add("nodes",
+				sys_props.kobj_topology);
+		if (!sys_props.kobj_nodes)
+			return -ENOMEM;
+
+		sys_props.attr_genid.name = "generation_id";
+		sys_props.attr_genid.mode = KFD_SYSFS_FILE_MODE;
+		sysfs_attr_init(&sys_props.attr_genid);
+		ret = sysfs_create_file(sys_props.kobj_topology,
+				&sys_props.attr_genid);
+		if (ret < 0)
+			return ret;
+
+		sys_props.attr_props.name = "system_properties";
+		sys_props.attr_props.mode = KFD_SYSFS_FILE_MODE;
+		sysfs_attr_init(&sys_props.attr_props);
+		ret = sysfs_create_file(sys_props.kobj_topology,
+				&sys_props.attr_props);
+		if (ret < 0)
+			return ret;
+	}
+
+	kfd_remove_sysfs_node_tree();
+
+	return kfd_build_sysfs_node_tree();
+}
+
+static void kfd_topology_release_sysfs(void)
+{
+	kfd_remove_sysfs_node_tree();
+	if (sys_props.kobj_topology) {
+		sysfs_remove_file(sys_props.kobj_topology,
+				&sys_props.attr_genid);
+		sysfs_remove_file(sys_props.kobj_topology,
+				&sys_props.attr_props);
+		if (sys_props.kobj_nodes) {
+			kobject_del(sys_props.kobj_nodes);
+			kobject_put(sys_props.kobj_nodes);
+			sys_props.kobj_nodes = NULL;
+		}
+		kobject_del(sys_props.kobj_topology);
+		kobject_put(sys_props.kobj_topology);
+		sys_props.kobj_topology = NULL;
+	}
+}
+
+int kfd_topology_init(void)
+{
+	void *crat_image = NULL;
+	size_t image_size = 0;
+	int ret;
+
+	/*
+	 * Initialize the head for the topology device list
+	 */
+	INIT_LIST_HEAD(&topology_device_list);
+	init_rwsem(&topology_lock);
+	topology_crat_parsed = 0;
+
+	memset(&sys_props, 0, sizeof(sys_props));
+
+	/*
+	 * Get the CRAT image from the ACPI
+	 */
+	ret = kfd_topology_get_crat_acpi(crat_image, &image_size);
+	if (ret == 0 && image_size > 0) {
+		pr_info("Found CRAT image with size=%zd\n", image_size);
+		crat_image = kmalloc(image_size, GFP_KERNEL);
+		if (!crat_image) {
+			ret = -ENOMEM;
+			pr_err("No memory for allocating CRAT image\n");
+			goto err;
+		}
+		ret = kfd_topology_get_crat_acpi(crat_image, &image_size);
+
+		if (ret == 0) {
+			down_write(&topology_lock);
+			ret = kfd_parse_crat_table(crat_image);
+			if (ret == 0)
+				ret = kfd_topology_update_sysfs();
+			up_write(&topology_lock);
+		} else {
+			pr_err("Couldn't get CRAT table size from ACPI\n");
+		}
+		kfree(crat_image);
+	} else if (ret == -ENODATA) {
+		ret = 0;
+	} else {
+		pr_err("Couldn't get CRAT table size from ACPI\n");
+	}
+
+err:
+	pr_info("Finished initializing topology ret=%d\n", ret);
+	return ret;
+}
+
+void kfd_topology_shutdown(void)
+{
+	kfd_topology_release_sysfs();
+	kfd_release_live_view();
+}
+
+static void kfd_debug_print_topology(void)
+{
+	struct kfd_topology_device *dev;
+	uint32_t i = 0;
+
+	pr_info("DEBUG PRINT OF TOPOLOGY:");
+	list_for_each_entry(dev, &topology_device_list, list) {
+		pr_info("Node: %d\n", i);
+		pr_info("\tGPU assigned: %s\n", (dev->gpu ? "yes" : "no"));
+		pr_info("\tCPU count: %d\n", dev->node_props.cpu_cores_count);
+		pr_info("\tSIMD count: %d", dev->node_props.simd_count);
+		i++;
+	}
+}
+
+static uint32_t kfd_generate_gpu_id(struct kfd_dev *gpu)
+{
+	uint32_t hashout;
+	uint32_t buf[7];
+	int i;
+
+	if (!gpu)
+		return 0;
+
+	buf[0] = gpu->pdev->devfn;
+	buf[1] = gpu->pdev->subsystem_vendor;
+	buf[2] = gpu->pdev->subsystem_device;
+	buf[3] = gpu->pdev->device;
+	buf[4] = gpu->pdev->bus->number;
+	buf[5] = (uint32_t)(kfd2kgd->get_vmem_size(gpu->kgd) & 0xffffffff);
+	buf[6] = (uint32_t)(kfd2kgd->get_vmem_size(gpu->kgd) >> 32);
+
+	for (i = 0, hashout = 0; i < 7; i++)
+		hashout ^= hash_32(buf[i], KFD_GPU_ID_HASH_WIDTH);
+
+	return hashout;
+}
+
+static struct kfd_topology_device *kfd_assign_gpu(struct kfd_dev *gpu)
+{
+	struct kfd_topology_device *dev;
+	struct kfd_topology_device *out_dev = NULL;
+
+	BUG_ON(!gpu);
+
+	list_for_each_entry(dev, &topology_device_list, list)
+		if (dev->gpu == NULL && dev->node_props.simd_count > 0) {
+			dev->gpu = gpu;
+			out_dev = dev;
+			break;
+		}
+
+	return out_dev;
+}
+
+static void kfd_notify_gpu_change(uint32_t gpu_id, int arrival)
+{
+	/*
+	 * TODO: Generate an event for thunk about the arrival/removal
+	 * of the GPU
+	 */
+}
+
+int kfd_topology_add_device(struct kfd_dev *gpu)
+{
+	uint32_t gpu_id;
+	struct kfd_topology_device *dev;
+	int res;
+
+	BUG_ON(!gpu);
+
+	gpu_id = kfd_generate_gpu_id(gpu);
+
+	pr_debug("kfd: Adding new GPU (ID: 0x%x) to topology\n", gpu_id);
+
+	down_write(&topology_lock);
+	/*
+	 * Try to assign the GPU to existing topology device (generated from
+	 * CRAT table
+	 */
+	dev = kfd_assign_gpu(gpu);
+	if (!dev) {
+		pr_info("GPU was not found in the current topology. Extending.\n");
+		kfd_debug_print_topology();
+		dev = kfd_create_topology_device();
+		if (!dev) {
+			res = -ENOMEM;
+			goto err;
+		}
+		dev->gpu = gpu;
+
+		/*
+		 * TODO: Make a call to retrieve topology information from the
+		 * GPU vBIOS
+		 */
+
+		/*
+		 * Update the SYSFS tree, since we added another topology device
+		 */
+		if (kfd_topology_update_sysfs() < 0)
+			kfd_topology_release_sysfs();
+
+	}
+
+	dev->gpu_id = gpu_id;
+	gpu->id = gpu_id;
+	dev->node_props.vendor_id = gpu->pdev->vendor;
+	dev->node_props.device_id = gpu->pdev->device;
+	dev->node_props.location_id = (gpu->pdev->bus->number << 24) +
+			(gpu->pdev->devfn & 0xffffff);
+	/*
+	 * TODO: Retrieve max engine clock values from KGD
+	 */
+
+	res = 0;
+
+err:
+	up_write(&topology_lock);
+
+	if (res == 0)
+		kfd_notify_gpu_change(gpu_id, 1);
+
+	return res;
+}
+
+int kfd_topology_remove_device(struct kfd_dev *gpu)
+{
+	struct kfd_topology_device *dev;
+	uint32_t gpu_id;
+	int res = -ENODEV;
+
+	BUG_ON(!gpu);
+
+	down_write(&topology_lock);
+
+	list_for_each_entry(dev, &topology_device_list, list)
+		if (dev->gpu == gpu) {
+			gpu_id = dev->gpu_id;
+			kfd_remove_sysfs_node_entry(dev);
+			kfd_release_topology_device(dev);
+			res = 0;
+			if (kfd_topology_update_sysfs() < 0)
+				kfd_topology_release_sysfs();
+			break;
+		}
+
+	up_write(&topology_lock);
+
+	if (res == 0)
+		kfd_notify_gpu_change(gpu_id, 0);
+
+	return res;
+}
+
+/*
+ * When idx is out of bounds, the function will return NULL
+ */
+struct kfd_dev *kfd_topology_enum_kfd_devices(uint8_t idx)
+{
+
+	struct kfd_topology_device *top_dev;
+	struct kfd_dev *device = NULL;
+	uint8_t device_idx = 0;
+
+	down_read(&topology_lock);
+
+	list_for_each_entry(top_dev, &topology_device_list, list) {
+		if (device_idx == idx) {
+			device = top_dev->gpu;
+			break;
+		}
+
+		device_idx++;
+	}
+
+	up_read(&topology_lock);
+
+	return device;
+
+}
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_topology.h b/drivers/gpu/drm/amd/amdkfd/kfd_topology.h
new file mode 100644
index 000000000000..989624b3cd14
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_topology.h
@@ -0,0 +1,168 @@
+/*
+ * Copyright 2014 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.
+ */
+
+#ifndef __KFD_TOPOLOGY_H__
+#define __KFD_TOPOLOGY_H__
+
+#include <linux/types.h>
+#include <linux/list.h>
+#include "kfd_priv.h"
+
+#define KFD_TOPOLOGY_PUBLIC_NAME_SIZE 128
+
+#define HSA_CAP_HOT_PLUGGABLE			0x00000001
+#define HSA_CAP_ATS_PRESENT			0x00000002
+#define HSA_CAP_SHARED_WITH_GRAPHICS		0x00000004
+#define HSA_CAP_QUEUE_SIZE_POW2			0x00000008
+#define HSA_CAP_QUEUE_SIZE_32BIT		0x00000010
+#define HSA_CAP_QUEUE_IDLE_EVENT		0x00000020
+#define HSA_CAP_VA_LIMIT			0x00000040
+#define HSA_CAP_WATCH_POINTS_SUPPORTED		0x00000080
+#define HSA_CAP_WATCH_POINTS_TOTALBITS_MASK	0x00000f00
+#define HSA_CAP_WATCH_POINTS_TOTALBITS_SHIFT	8
+#define HSA_CAP_RESERVED			0xfffff000
+
+struct kfd_node_properties {
+	uint32_t cpu_cores_count;
+	uint32_t simd_count;
+	uint32_t mem_banks_count;
+	uint32_t caches_count;
+	uint32_t io_links_count;
+	uint32_t cpu_core_id_base;
+	uint32_t simd_id_base;
+	uint32_t capability;
+	uint32_t max_waves_per_simd;
+	uint32_t lds_size_in_kb;
+	uint32_t gds_size_in_kb;
+	uint32_t wave_front_size;
+	uint32_t array_count;
+	uint32_t simd_arrays_per_engine;
+	uint32_t cu_per_simd_array;
+	uint32_t simd_per_cu;
+	uint32_t max_slots_scratch_cu;
+	uint32_t engine_id;
+	uint32_t vendor_id;
+	uint32_t device_id;
+	uint32_t location_id;
+	uint32_t max_engine_clk_fcompute;
+	uint32_t max_engine_clk_ccompute;
+	uint16_t marketing_name[KFD_TOPOLOGY_PUBLIC_NAME_SIZE];
+};
+
+#define HSA_MEM_HEAP_TYPE_SYSTEM	0
+#define HSA_MEM_HEAP_TYPE_FB_PUBLIC	1
+#define HSA_MEM_HEAP_TYPE_FB_PRIVATE	2
+#define HSA_MEM_HEAP_TYPE_GPU_GDS	3
+#define HSA_MEM_HEAP_TYPE_GPU_LDS	4
+#define HSA_MEM_HEAP_TYPE_GPU_SCRATCH	5
+
+#define HSA_MEM_FLAGS_HOT_PLUGGABLE	0x00000001
+#define HSA_MEM_FLAGS_NON_VOLATILE	0x00000002
+#define HSA_MEM_FLAGS_RESERVED		0xfffffffc
+
+struct kfd_mem_properties {
+	struct list_head	list;
+	uint32_t		heap_type;
+	uint64_t		size_in_bytes;
+	uint32_t		flags;
+	uint32_t		width;
+	uint32_t		mem_clk_max;
+	struct kobject		*kobj;
+	struct attribute	attr;
+};
+
+#define KFD_TOPOLOGY_CPU_SIBLINGS 256
+
+#define HSA_CACHE_TYPE_DATA		0x00000001
+#define HSA_CACHE_TYPE_INSTRUCTION	0x00000002
+#define HSA_CACHE_TYPE_CPU		0x00000004
+#define HSA_CACHE_TYPE_HSACU		0x00000008
+#define HSA_CACHE_TYPE_RESERVED		0xfffffff0
+
+struct kfd_cache_properties {
+	struct list_head	list;
+	uint32_t		processor_id_low;
+	uint32_t		cache_level;
+	uint32_t		cache_size;
+	uint32_t		cacheline_size;
+	uint32_t		cachelines_per_tag;
+	uint32_t		cache_assoc;
+	uint32_t		cache_latency;
+	uint32_t		cache_type;
+	uint8_t			sibling_map[KFD_TOPOLOGY_CPU_SIBLINGS];
+	struct kobject		*kobj;
+	struct attribute	attr;
+};
+
+struct kfd_iolink_properties {
+	struct list_head	list;
+	uint32_t		iolink_type;
+	uint32_t		ver_maj;
+	uint32_t		ver_min;
+	uint32_t		node_from;
+	uint32_t		node_to;
+	uint32_t		weight;
+	uint32_t		min_latency;
+	uint32_t		max_latency;
+	uint32_t		min_bandwidth;
+	uint32_t		max_bandwidth;
+	uint32_t		rec_transfer_size;
+	uint32_t		flags;
+	struct kobject		*kobj;
+	struct attribute	attr;
+};
+
+struct kfd_topology_device {
+	struct list_head		list;
+	uint32_t			gpu_id;
+	struct kfd_node_properties	node_props;
+	uint32_t			mem_bank_count;
+	struct list_head		mem_props;
+	uint32_t			cache_count;
+	struct list_head		cache_props;
+	uint32_t			io_link_count;
+	struct list_head		io_link_props;
+	struct kfd_dev			*gpu;
+	struct kobject			*kobj_node;
+	struct kobject			*kobj_mem;
+	struct kobject			*kobj_cache;
+	struct kobject			*kobj_iolink;
+	struct attribute		attr_gpuid;
+	struct attribute		attr_name;
+	struct attribute		attr_props;
+};
+
+struct kfd_system_properties {
+	uint32_t		num_devices;     /* Number of H-NUMA nodes */
+	uint32_t		generation_count;
+	uint64_t		platform_oem;
+	uint64_t		platform_id;
+	uint64_t		platform_rev;
+	struct kobject		*kobj_topology;
+	struct kobject		*kobj_nodes;
+	struct attribute	attr_genid;
+	struct attribute	attr_props;
+};
+
+
+
+#endif /* __KFD_TOPOLOGY_H__ */
diff --git a/drivers/gpu/drm/amd/include/kgd_kfd_interface.h b/drivers/gpu/drm/amd/include/kgd_kfd_interface.h
new file mode 100644
index 000000000000..47b551970a14
--- /dev/null
+++ b/drivers/gpu/drm/amd/include/kgd_kfd_interface.h
@@ -0,0 +1,200 @@
+/*
+ * Copyright 2014 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.
+ */
+
+/*
+ * This file defines the private interface between the
+ * AMD kernel graphics drivers and the AMD KFD.
+ */
+
+#ifndef KGD_KFD_INTERFACE_H_INCLUDED
+#define KGD_KFD_INTERFACE_H_INCLUDED
+
+#include <linux/types.h>
+
+struct pci_dev;
+
+#define KFD_INTERFACE_VERSION 1
+
+struct kfd_dev;
+struct kgd_dev;
+
+struct kgd_mem;
+
+enum kgd_memory_pool {
+	KGD_POOL_SYSTEM_CACHEABLE = 1,
+	KGD_POOL_SYSTEM_WRITECOMBINE = 2,
+	KGD_POOL_FRAMEBUFFER = 3,
+};
+
+enum kgd_engine_type {
+	KGD_ENGINE_PFP = 1,
+	KGD_ENGINE_ME,
+	KGD_ENGINE_CE,
+	KGD_ENGINE_MEC1,
+	KGD_ENGINE_MEC2,
+	KGD_ENGINE_RLC,
+	KGD_ENGINE_SDMA,
+	KGD_ENGINE_MAX
+};
+
+struct kgd2kfd_shared_resources {
+	/* Bit n == 1 means VMID n is available for KFD. */
+	unsigned int compute_vmid_bitmap;
+
+	/* Compute pipes are counted starting from MEC0/pipe0 as 0. */
+	unsigned int first_compute_pipe;
+
+	/* Number of MEC pipes available for KFD. */
+	unsigned int compute_pipe_count;
+
+	/* Base address of doorbell aperture. */
+	phys_addr_t doorbell_physical_address;
+
+	/* Size in bytes of doorbell aperture. */
+	size_t doorbell_aperture_size;
+
+	/* Number of bytes at start of aperture reserved for KGD. */
+	size_t doorbell_start_offset;
+};
+
+/**
+ * struct kgd2kfd_calls
+ *
+ * @exit: Notifies amdkfd that kgd module is unloaded
+ *
+ * @probe: Notifies amdkfd about a probe done on a device in the kgd driver.
+ *
+ * @device_init: Initialize the newly probed device (if it is a device that
+ * amdkfd supports)
+ *
+ * @device_exit: Notifies amdkfd about a removal of a kgd device
+ *
+ * @suspend: Notifies amdkfd about a suspend action done to a kgd device
+ *
+ * @resume: Notifies amdkfd about a resume action done to a kgd device
+ *
+ * This structure contains function callback pointers so the kgd driver
+ * will notify to the amdkfd about certain status changes.
+ *
+ */
+struct kgd2kfd_calls {
+	void (*exit)(void);
+	struct kfd_dev* (*probe)(struct kgd_dev *kgd, struct pci_dev *pdev);
+	bool (*device_init)(struct kfd_dev *kfd,
+			const struct kgd2kfd_shared_resources *gpu_resources);
+	void (*device_exit)(struct kfd_dev *kfd);
+	void (*interrupt)(struct kfd_dev *kfd, const void *ih_ring_entry);
+	void (*suspend)(struct kfd_dev *kfd);
+	int (*resume)(struct kfd_dev *kfd);
+};
+
+/**
+ * struct kfd2kgd_calls
+ *
+ * @init_sa_manager: Initialize an instance of the sa manager, used by
+ * amdkfd for all system memory allocations that are mapped to the GART
+ * address space
+ *
+ * @fini_sa_manager: Releases all memory allocations for amdkfd that are
+ * handled by kgd sa manager
+ *
+ * @allocate_mem: Allocate a buffer from amdkfd's sa manager. The buffer can
+ * be used for mqds, hpds, kernel queue, fence and runlists
+ *
+ * @free_mem: Frees a buffer that was allocated by amdkfd's sa manager
+ *
+ * @get_vmem_size: Retrieves (physical) size of VRAM
+ *
+ * @get_gpu_clock_counter: Retrieves GPU clock counter
+ *
+ * @get_max_engine_clock_in_mhz: Retrieves maximum GPU clock in MHz
+ *
+ * @program_sh_mem_settings: A function that should initiate the memory
+ * properties such as main aperture memory type (cache / non cached) and
+ * secondary aperture base address, size and memory type.
+ * This function is used only for no cp scheduling mode.
+ *
+ * @set_pasid_vmid_mapping: Exposes pasid/vmid pair to the H/W for no cp
+ * scheduling mode. Only used for no cp scheduling mode.
+ *
+ * @init_memory: Initializes memory apertures to fixed base/limit address
+ * and non cached memory types.
+ *
+ * @init_pipeline: Initialized the compute pipelines.
+ *
+ * @hqd_load: Loads the mqd structure to a H/W hqd slot. used only for no cp
+ * sceduling mode.
+ *
+ * @hqd_is_occupies: Checks if a hqd slot is occupied.
+ *
+ * @hqd_destroy: Destructs and preempts the queue assigned to that hqd slot.
+ *
+ * @get_fw_version: Returns FW versions from the header
+ *
+ * This structure contains function pointers to services that the kgd driver
+ * provides to amdkfd driver.
+ *
+ */
+struct kfd2kgd_calls {
+	/* Memory management. */
+	int (*init_sa_manager)(struct kgd_dev *kgd, unsigned int size);
+	void (*fini_sa_manager)(struct kgd_dev *kgd);
+	int (*allocate_mem)(struct kgd_dev *kgd, size_t size, size_t alignment,
+			enum kgd_memory_pool pool, struct kgd_mem **mem);
+
+	void (*free_mem)(struct kgd_dev *kgd, struct kgd_mem *mem);
+
+	uint64_t (*get_vmem_size)(struct kgd_dev *kgd);
+	uint64_t (*get_gpu_clock_counter)(struct kgd_dev *kgd);
+
+	uint32_t (*get_max_engine_clock_in_mhz)(struct kgd_dev *kgd);
+
+	/* Register access functions */
+	void (*program_sh_mem_settings)(struct kgd_dev *kgd, uint32_t vmid,
+			uint32_t sh_mem_config,	uint32_t sh_mem_ape1_base,
+			uint32_t sh_mem_ape1_limit, uint32_t sh_mem_bases);
+
+	int (*set_pasid_vmid_mapping)(struct kgd_dev *kgd, unsigned int pasid,
+					unsigned int vmid);
+
+	int (*init_memory)(struct kgd_dev *kgd);
+	int (*init_pipeline)(struct kgd_dev *kgd, uint32_t pipe_id,
+				uint32_t hpd_size, uint64_t hpd_gpu_addr);
+
+	int (*hqd_load)(struct kgd_dev *kgd, void *mqd, uint32_t pipe_id,
+			uint32_t queue_id, uint32_t __user *wptr);
+
+	bool (*hqd_is_occupies)(struct kgd_dev *kgd, uint64_t queue_address,
+				uint32_t pipe_id, uint32_t queue_id);
+
+	int (*hqd_destroy)(struct kgd_dev *kgd, uint32_t reset_type,
+				unsigned int timeout, uint32_t pipe_id,
+				uint32_t queue_id);
+	uint16_t (*get_fw_version)(struct kgd_dev *kgd,
+				enum kgd_engine_type type);
+};
+
+bool kgd2kfd_init(unsigned interface_version,
+		  const struct kfd2kgd_calls *f2g,
+		  const struct kgd2kfd_calls **g2f);
+
+#endif /* KGD_KFD_INTERFACE_H_INCLUDED */
diff --git a/drivers/gpu/drm/armada/armada_crtc.c b/drivers/gpu/drm/armada/armada_crtc.c
index e4a1490b42c2..e3a7a5078e5c 100644
--- a/drivers/gpu/drm/armada/armada_crtc.c
+++ b/drivers/gpu/drm/armada/armada_crtc.c
@@ -12,6 +12,7 @@
 #include <linux/platform_device.h>
 #include <drm/drmP.h>
 #include <drm/drm_crtc_helper.h>
+#include <drm/drm_plane_helper.h>
 #include "armada_crtc.h"
 #include "armada_drm.h"
 #include "armada_fb.h"
diff --git a/drivers/gpu/drm/armada/armada_drv.c b/drivers/gpu/drm/armada/armada_drv.c
index 908e5316eac4..b01420c84864 100644
--- a/drivers/gpu/drm/armada/armada_drv.c
+++ b/drivers/gpu/drm/armada/armada_drv.c
@@ -486,7 +486,6 @@ static struct platform_driver armada_drm_platform_driver = {
 	.remove	= armada_drm_remove,
 	.driver	= {
 		.name	= "armada-drm",
-		.owner	= THIS_MODULE,
 	},
 	.id_table = armada_drm_platform_ids,
 };
diff --git a/drivers/gpu/drm/ast/ast_mode.c b/drivers/gpu/drm/ast/ast_mode.c
index 9dc0fd5c1ea4..b7ee2634e47c 100644
--- a/drivers/gpu/drm/ast/ast_mode.c
+++ b/drivers/gpu/drm/ast/ast_mode.c
@@ -31,6 +31,7 @@
 #include <drm/drmP.h>
 #include <drm/drm_crtc.h>
 #include <drm/drm_crtc_helper.h>
+#include <drm/drm_plane_helper.h>
 #include "ast_drv.h"
 
 #include "ast_tables.h"
diff --git a/drivers/gpu/drm/bochs/bochs_fbdev.c b/drivers/gpu/drm/bochs/bochs_fbdev.c
index fe95d31cd110..61dbf09dff5d 100644
--- a/drivers/gpu/drm/bochs/bochs_fbdev.c
+++ b/drivers/gpu/drm/bochs/bochs_fbdev.c
@@ -9,6 +9,17 @@
 
 /* ---------------------------------------------------------------------- */
 
+static int bochsfb_mmap(struct fb_info *info,
+			struct vm_area_struct *vma)
+{
+	struct drm_fb_helper *fb_helper = info->par;
+	struct bochs_device *bochs =
+		container_of(fb_helper, struct bochs_device, fb.helper);
+	struct bochs_bo *bo = gem_to_bochs_bo(bochs->fb.gfb.obj);
+
+	return ttm_fbdev_mmap(vma, &bo->bo);
+}
+
 static struct fb_ops bochsfb_ops = {
 	.owner = THIS_MODULE,
 	.fb_check_var = drm_fb_helper_check_var,
@@ -19,6 +30,7 @@ static struct fb_ops bochsfb_ops = {
 	.fb_pan_display = drm_fb_helper_pan_display,
 	.fb_blank = drm_fb_helper_blank,
 	.fb_setcmap = drm_fb_helper_setcmap,
+	.fb_mmap = bochsfb_mmap,
 };
 
 static int bochsfb_create_object(struct bochs_device *bochs,
@@ -123,11 +135,9 @@ static int bochsfb_create(struct drm_fb_helper *helper,
 	info->screen_base = bo->kmap.virtual;
 	info->screen_size = size;
 
-#if 0
-	/* FIXME: get this right for mmap(/dev/fb0) */
-	info->fix.smem_start = bochs_bo_mmap_offset(bo);
+	drm_vma_offset_remove(&bo->bo.bdev->vma_manager, &bo->bo.vma_node);
+	info->fix.smem_start = 0;
 	info->fix.smem_len = size;
-#endif
 
 	ret = fb_alloc_cmap(&info->cmap, 256, 0);
 	if (ret) {
diff --git a/drivers/gpu/drm/bochs/bochs_hw.c b/drivers/gpu/drm/bochs/bochs_hw.c
index dbe619e6aab4..460389702d31 100644
--- a/drivers/gpu/drm/bochs/bochs_hw.c
+++ b/drivers/gpu/drm/bochs/bochs_hw.c
@@ -51,11 +51,10 @@ int bochs_hw_init(struct drm_device *dev, uint32_t flags)
 {
 	struct bochs_device *bochs = dev->dev_private;
 	struct pci_dev *pdev = dev->pdev;
-	unsigned long addr, size, mem, ioaddr, iosize;
+	unsigned long addr, size, mem, ioaddr, iosize, qext_size;
 	u16 id;
 
-	if (/* (ent->driver_data == BOCHS_QEMU_STDVGA) && */
-	    (pdev->resource[2].flags & IORESOURCE_MEM)) {
+	if (pdev->resource[2].flags & IORESOURCE_MEM) {
 		/* mmio bar with vga and bochs registers present */
 		if (pci_request_region(pdev, 2, "bochs-drm") != 0) {
 			DRM_ERROR("Cannot request mmio region\n");
@@ -116,6 +115,24 @@ int bochs_hw_init(struct drm_device *dev, uint32_t flags)
 		 size / 1024, addr,
 		 bochs->ioports ? "ioports" : "mmio",
 		 ioaddr);
+
+	if (bochs->mmio && pdev->revision >= 2) {
+		qext_size = readl(bochs->mmio + 0x600);
+		if (qext_size < 4 || qext_size > iosize)
+			goto noext;
+		DRM_DEBUG("Found qemu ext regs, size %ld\n", qext_size);
+		if (qext_size >= 8) {
+#ifdef __BIG_ENDIAN
+			writel(0xbebebebe, bochs->mmio + 0x604);
+#else
+			writel(0x1e1e1e1e, bochs->mmio + 0x604);
+#endif
+			DRM_DEBUG("  qext endian: 0x%x\n",
+				  readl(bochs->mmio + 0x604));
+		}
+	}
+
+noext:
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/bochs/bochs_kms.c b/drivers/gpu/drm/bochs/bochs_kms.c
index 6b7efcf363d6..85f0f8cf1fb8 100644
--- a/drivers/gpu/drm/bochs/bochs_kms.c
+++ b/drivers/gpu/drm/bochs/bochs_kms.c
@@ -6,6 +6,7 @@
  */
 
 #include "bochs.h"
+#include <drm/drm_plane_helper.h>
 
 static int defx = 1024;
 static int defy = 768;
@@ -108,11 +109,32 @@ static void bochs_crtc_gamma_set(struct drm_crtc *crtc, u16 *red, u16 *green,
 {
 }
 
+static int bochs_crtc_page_flip(struct drm_crtc *crtc,
+				struct drm_framebuffer *fb,
+				struct drm_pending_vblank_event *event,
+				uint32_t page_flip_flags)
+{
+	struct bochs_device *bochs =
+		container_of(crtc, struct bochs_device, crtc);
+	struct drm_framebuffer *old_fb = crtc->primary->fb;
+	unsigned long irqflags;
+
+	crtc->primary->fb = fb;
+	bochs_crtc_mode_set_base(crtc, 0, 0, old_fb);
+	if (event) {
+		spin_lock_irqsave(&bochs->dev->event_lock, irqflags);
+		drm_send_vblank_event(bochs->dev, -1, event);
+		spin_unlock_irqrestore(&bochs->dev->event_lock, irqflags);
+	}
+	return 0;
+}
+
 /* These provide the minimum set of functions required to handle a CRTC */
 static const struct drm_crtc_funcs bochs_crtc_funcs = {
 	.gamma_set = bochs_crtc_gamma_set,
 	.set_config = drm_crtc_helper_set_config,
 	.destroy = drm_crtc_cleanup,
+	.page_flip = bochs_crtc_page_flip,
 };
 
 static const struct drm_crtc_helper_funcs bochs_helper_funcs = {
diff --git a/drivers/gpu/drm/cirrus/cirrus_drv.h b/drivers/gpu/drm/cirrus/cirrus_drv.h
index d44e69daa239..693a4565c4ff 100644
--- a/drivers/gpu/drm/cirrus/cirrus_drv.h
+++ b/drivers/gpu/drm/cirrus/cirrus_drv.h
@@ -210,6 +210,9 @@ int cirrus_framebuffer_init(struct drm_device *dev,
 			    struct drm_mode_fb_cmd2 *mode_cmd,
 			    struct drm_gem_object *obj);
 
+bool cirrus_check_framebuffer(struct cirrus_device *cdev, int width, int height,
+			      int bpp, int pitch);
+
 				/* cirrus_display.c */
 int cirrus_modeset_init(struct cirrus_device *cdev);
 void cirrus_modeset_fini(struct cirrus_device *cdev);
diff --git a/drivers/gpu/drm/cirrus/cirrus_fbdev.c b/drivers/gpu/drm/cirrus/cirrus_fbdev.c
index d231b1c317af..502a89eb54b5 100644
--- a/drivers/gpu/drm/cirrus/cirrus_fbdev.c
+++ b/drivers/gpu/drm/cirrus/cirrus_fbdev.c
@@ -139,6 +139,7 @@ static int cirrusfb_create_object(struct cirrus_fbdev *afbdev,
 			       struct drm_gem_object **gobj_p)
 {
 	struct drm_device *dev = afbdev->helper.dev;
+	struct cirrus_device *cdev = dev->dev_private;
 	u32 bpp, depth;
 	u32 size;
 	struct drm_gem_object *gobj;
@@ -146,8 +147,10 @@ static int cirrusfb_create_object(struct cirrus_fbdev *afbdev,
 	int ret = 0;
 	drm_fb_get_bpp_depth(mode_cmd->pixel_format, &depth, &bpp);
 
-	if (bpp > 24)
+	if (!cirrus_check_framebuffer(cdev, mode_cmd->width, mode_cmd->height,
+				      bpp, mode_cmd->pitches[0]))
 		return -EINVAL;
+
 	size = mode_cmd->pitches[0] * mode_cmd->height;
 	ret = cirrus_gem_create(dev, size, true, &gobj);
 	if (ret)
diff --git a/drivers/gpu/drm/cirrus/cirrus_main.c b/drivers/gpu/drm/cirrus/cirrus_main.c
index 99c1983f99d2..4c2d68e9102d 100644
--- a/drivers/gpu/drm/cirrus/cirrus_main.c
+++ b/drivers/gpu/drm/cirrus/cirrus_main.c
@@ -49,14 +49,16 @@ cirrus_user_framebuffer_create(struct drm_device *dev,
 			       struct drm_file *filp,
 			       struct drm_mode_fb_cmd2 *mode_cmd)
 {
+	struct cirrus_device *cdev = dev->dev_private;
 	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)
+
+	if (!cirrus_check_framebuffer(cdev, mode_cmd->width, mode_cmd->height,
+				      bpp, mode_cmd->pitches[0]))
 		return ERR_PTR(-EINVAL);
 
 	obj = drm_gem_object_lookup(dev, filp, mode_cmd->handles[0]);
@@ -96,8 +98,7 @@ 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;
+	cdev->mc.vram_size = pci_resource_len(cdev->dev->pdev, 0);
 
 	if (!request_mem_region(cdev->mc.vram_base, cdev->mc.vram_size,
 				"cirrusdrmfb_vram")) {
@@ -179,17 +180,22 @@ int cirrus_driver_load(struct drm_device *dev, unsigned long flags)
 	}
 
 	r = cirrus_mm_init(cdev);
-	if (r)
+	if (r) {
 		dev_err(&dev->pdev->dev, "fatal err on mm init\n");
+		goto out;
+	}
 
 	r = cirrus_modeset_init(cdev);
-	if (r)
+	if (r) {
 		dev_err(&dev->pdev->dev, "Fatal error during modeset init: %d\n", r);
+		goto out;
+	}
 
 	dev->mode_config.funcs = (void *)&cirrus_mode_funcs;
+
+	return 0;
 out:
-	if (r)
-		cirrus_driver_unload(dev);
+	cirrus_driver_unload(dev);
 	return r;
 }
 
@@ -307,3 +313,21 @@ out_unlock:
 	return ret;
 
 }
+
+bool cirrus_check_framebuffer(struct cirrus_device *cdev, int width, int height,
+			      int bpp, int pitch)
+{
+	const int max_pitch = 0x1FF << 3; /* (4096 - 1) & ~111b bytes */
+	const int max_size = cdev->mc.vram_size;
+
+	if (bpp > 32)
+		return false;
+
+	if (pitch > max_pitch)
+		return false;
+
+	if (pitch * height > max_size)
+		return false;
+
+	return true;
+}
diff --git a/drivers/gpu/drm/cirrus/cirrus_mode.c b/drivers/gpu/drm/cirrus/cirrus_mode.c
index c7c5a9d91fa0..99d4a74ffeaf 100644
--- a/drivers/gpu/drm/cirrus/cirrus_mode.c
+++ b/drivers/gpu/drm/cirrus/cirrus_mode.c
@@ -16,6 +16,7 @@
  */
 #include <drm/drmP.h>
 #include <drm/drm_crtc_helper.h>
+#include <drm/drm_plane_helper.h>
 
 #include <video/cirrus.h>
 
diff --git a/drivers/gpu/drm/drm_atomic.c b/drivers/gpu/drm/drm_atomic.c
new file mode 100644
index 000000000000..ff5f034cc405
--- /dev/null
+++ b/drivers/gpu/drm/drm_atomic.c
@@ -0,0 +1,657 @@
+/*
+ * Copyright (C) 2014 Red Hat
+ * Copyright (C) 2014 Intel Corp.
+ *
+ * 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:
+ * Rob Clark <robdclark@gmail.com>
+ * Daniel Vetter <daniel.vetter@ffwll.ch>
+ */
+
+
+#include <drm/drmP.h>
+#include <drm/drm_atomic.h>
+#include <drm/drm_plane_helper.h>
+
+static void kfree_state(struct drm_atomic_state *state)
+{
+	kfree(state->connectors);
+	kfree(state->connector_states);
+	kfree(state->crtcs);
+	kfree(state->crtc_states);
+	kfree(state->planes);
+	kfree(state->plane_states);
+	kfree(state);
+}
+
+/**
+ * drm_atomic_state_alloc - allocate atomic state
+ * @dev: DRM device
+ *
+ * This allocates an empty atomic state to track updates.
+ */
+struct drm_atomic_state *
+drm_atomic_state_alloc(struct drm_device *dev)
+{
+	struct drm_atomic_state *state;
+
+	state = kzalloc(sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return NULL;
+
+	state->num_connector = ACCESS_ONCE(dev->mode_config.num_connector);
+
+	state->crtcs = kcalloc(dev->mode_config.num_crtc,
+			       sizeof(*state->crtcs), GFP_KERNEL);
+	if (!state->crtcs)
+		goto fail;
+	state->crtc_states = kcalloc(dev->mode_config.num_crtc,
+				     sizeof(*state->crtc_states), GFP_KERNEL);
+	if (!state->crtc_states)
+		goto fail;
+	state->planes = kcalloc(dev->mode_config.num_total_plane,
+				sizeof(*state->planes), GFP_KERNEL);
+	if (!state->planes)
+		goto fail;
+	state->plane_states = kcalloc(dev->mode_config.num_total_plane,
+				      sizeof(*state->plane_states), GFP_KERNEL);
+	if (!state->plane_states)
+		goto fail;
+	state->connectors = kcalloc(state->num_connector,
+				    sizeof(*state->connectors),
+				    GFP_KERNEL);
+	if (!state->connectors)
+		goto fail;
+	state->connector_states = kcalloc(state->num_connector,
+					  sizeof(*state->connector_states),
+					  GFP_KERNEL);
+	if (!state->connector_states)
+		goto fail;
+
+	state->dev = dev;
+
+	DRM_DEBUG_KMS("Allocate atomic state %p\n", state);
+
+	return state;
+fail:
+	kfree_state(state);
+
+	return NULL;
+}
+EXPORT_SYMBOL(drm_atomic_state_alloc);
+
+/**
+ * drm_atomic_state_clear - clear state object
+ * @state: atomic state
+ *
+ * When the w/w mutex algorithm detects a deadlock we need to back off and drop
+ * all locks. So someone else could sneak in and change the current modeset
+ * configuration. Which means that all the state assembled in @state is no
+ * longer an atomic update to the current state, but to some arbitrary earlier
+ * state. Which could break assumptions the driver's ->atomic_check likely
+ * relies on.
+ *
+ * Hence we must clear all cached state and completely start over, using this
+ * function.
+ */
+void drm_atomic_state_clear(struct drm_atomic_state *state)
+{
+	struct drm_device *dev = state->dev;
+	struct drm_mode_config *config = &dev->mode_config;
+	int i;
+
+	DRM_DEBUG_KMS("Clearing atomic state %p\n", state);
+
+	for (i = 0; i < state->num_connector; i++) {
+		struct drm_connector *connector = state->connectors[i];
+
+		if (!connector)
+			continue;
+
+		WARN_ON(!drm_modeset_is_locked(&config->connection_mutex));
+
+		connector->funcs->atomic_destroy_state(connector,
+						       state->connector_states[i]);
+	}
+
+	for (i = 0; i < config->num_crtc; i++) {
+		struct drm_crtc *crtc = state->crtcs[i];
+
+		if (!crtc)
+			continue;
+
+		crtc->funcs->atomic_destroy_state(crtc,
+						  state->crtc_states[i]);
+	}
+
+	for (i = 0; i < config->num_total_plane; i++) {
+		struct drm_plane *plane = state->planes[i];
+
+		if (!plane)
+			continue;
+
+		plane->funcs->atomic_destroy_state(plane,
+						   state->plane_states[i]);
+	}
+}
+EXPORT_SYMBOL(drm_atomic_state_clear);
+
+/**
+ * drm_atomic_state_free - free all memory for an atomic state
+ * @state: atomic state to deallocate
+ *
+ * This frees all memory associated with an atomic state, including all the
+ * per-object state for planes, crtcs and connectors.
+ */
+void drm_atomic_state_free(struct drm_atomic_state *state)
+{
+	drm_atomic_state_clear(state);
+
+	DRM_DEBUG_KMS("Freeing atomic state %p\n", state);
+
+	kfree_state(state);
+}
+EXPORT_SYMBOL(drm_atomic_state_free);
+
+/**
+ * drm_atomic_get_crtc_state - get crtc state
+ * @state: global atomic state object
+ * @crtc: crtc to get state object for
+ *
+ * This function returns the crtc state for the given crtc, allocating it if
+ * needed. It will also grab the relevant crtc lock to make sure that the state
+ * is consistent.
+ *
+ * Returns:
+ *
+ * Either the allocated state or the error code encoded into the pointer. When
+ * the error is EDEADLK then the w/w mutex code has detected a deadlock and the
+ * entire atomic sequence must be restarted. All other errors are fatal.
+ */
+struct drm_crtc_state *
+drm_atomic_get_crtc_state(struct drm_atomic_state *state,
+			  struct drm_crtc *crtc)
+{
+	int ret, index;
+	struct drm_crtc_state *crtc_state;
+
+	index = drm_crtc_index(crtc);
+
+	if (state->crtc_states[index])
+		return state->crtc_states[index];
+
+	ret = drm_modeset_lock(&crtc->mutex, state->acquire_ctx);
+	if (ret)
+		return ERR_PTR(ret);
+
+	crtc_state = crtc->funcs->atomic_duplicate_state(crtc);
+	if (!crtc_state)
+		return ERR_PTR(-ENOMEM);
+
+	state->crtc_states[index] = crtc_state;
+	state->crtcs[index] = crtc;
+	crtc_state->state = state;
+
+	DRM_DEBUG_KMS("Added [CRTC:%d] %p state to %p\n",
+		      crtc->base.id, crtc_state, state);
+
+	return crtc_state;
+}
+EXPORT_SYMBOL(drm_atomic_get_crtc_state);
+
+/**
+ * drm_atomic_get_plane_state - get plane state
+ * @state: global atomic state object
+ * @plane: plane to get state object for
+ *
+ * This function returns the plane state for the given plane, allocating it if
+ * needed. It will also grab the relevant plane lock to make sure that the state
+ * is consistent.
+ *
+ * Returns:
+ *
+ * Either the allocated state or the error code encoded into the pointer. When
+ * the error is EDEADLK then the w/w mutex code has detected a deadlock and the
+ * entire atomic sequence must be restarted. All other errors are fatal.
+ */
+struct drm_plane_state *
+drm_atomic_get_plane_state(struct drm_atomic_state *state,
+			  struct drm_plane *plane)
+{
+	int ret, index;
+	struct drm_plane_state *plane_state;
+
+	index = drm_plane_index(plane);
+
+	if (state->plane_states[index])
+		return state->plane_states[index];
+
+	ret = drm_modeset_lock(&plane->mutex, state->acquire_ctx);
+	if (ret)
+		return ERR_PTR(ret);
+
+	plane_state = plane->funcs->atomic_duplicate_state(plane);
+	if (!plane_state)
+		return ERR_PTR(-ENOMEM);
+
+	state->plane_states[index] = plane_state;
+	state->planes[index] = plane;
+	plane_state->state = state;
+
+	DRM_DEBUG_KMS("Added [PLANE:%d] %p state to %p\n",
+		      plane->base.id, plane_state, state);
+
+	if (plane_state->crtc) {
+		struct drm_crtc_state *crtc_state;
+
+		crtc_state = drm_atomic_get_crtc_state(state,
+						       plane_state->crtc);
+		if (IS_ERR(crtc_state))
+			return ERR_CAST(crtc_state);
+	}
+
+	return plane_state;
+}
+EXPORT_SYMBOL(drm_atomic_get_plane_state);
+
+/**
+ * drm_atomic_get_connector_state - get connector state
+ * @state: global atomic state object
+ * @connector: connector to get state object for
+ *
+ * This function returns the connector state for the given connector,
+ * allocating it if needed. It will also grab the relevant connector lock to
+ * make sure that the state is consistent.
+ *
+ * Returns:
+ *
+ * Either the allocated state or the error code encoded into the pointer. When
+ * the error is EDEADLK then the w/w mutex code has detected a deadlock and the
+ * entire atomic sequence must be restarted. All other errors are fatal.
+ */
+struct drm_connector_state *
+drm_atomic_get_connector_state(struct drm_atomic_state *state,
+			  struct drm_connector *connector)
+{
+	int ret, index;
+	struct drm_mode_config *config = &connector->dev->mode_config;
+	struct drm_connector_state *connector_state;
+
+	ret = drm_modeset_lock(&config->connection_mutex, state->acquire_ctx);
+	if (ret)
+		return ERR_PTR(ret);
+
+	index = drm_connector_index(connector);
+
+	/*
+	 * Construction of atomic state updates can race with a connector
+	 * hot-add which might overflow. In this case flip the table and just
+	 * restart the entire ioctl - no one is fast enough to livelock a cpu
+	 * with physical hotplug events anyway.
+	 *
+	 * Note that we only grab the indexes once we have the right lock to
+	 * prevent hotplug/unplugging of connectors. So removal is no problem,
+	 * at most the array is a bit too large.
+	 */
+	if (index >= state->num_connector) {
+		DRM_DEBUG_KMS("Hot-added connector would overflow state array, restarting\n");
+		return ERR_PTR(-EAGAIN);
+	}
+
+	if (state->connector_states[index])
+		return state->connector_states[index];
+
+	connector_state = connector->funcs->atomic_duplicate_state(connector);
+	if (!connector_state)
+		return ERR_PTR(-ENOMEM);
+
+	state->connector_states[index] = connector_state;
+	state->connectors[index] = connector;
+	connector_state->state = state;
+
+	DRM_DEBUG_KMS("Added [CONNECTOR:%d] %p state to %p\n",
+		      connector->base.id, connector_state, state);
+
+	if (connector_state->crtc) {
+		struct drm_crtc_state *crtc_state;
+
+		crtc_state = drm_atomic_get_crtc_state(state,
+						       connector_state->crtc);
+		if (IS_ERR(crtc_state))
+			return ERR_CAST(crtc_state);
+	}
+
+	return connector_state;
+}
+EXPORT_SYMBOL(drm_atomic_get_connector_state);
+
+/**
+ * drm_atomic_set_crtc_for_plane - set crtc for plane
+ * @state: the incoming atomic state
+ * @plane: the plane whose incoming state to update
+ * @crtc: crtc to use for the plane
+ *
+ * Changing the assigned crtc for a plane requires us to grab the lock and state
+ * for the new crtc, as needed. This function takes care of all these details
+ * besides updating the pointer in the state object itself.
+ *
+ * Returns:
+ * 0 on success or can fail with -EDEADLK or -ENOMEM. When the error is EDEADLK
+ * then the w/w mutex code has detected a deadlock and the entire atomic
+ * sequence must be restarted. All other errors are fatal.
+ */
+int
+drm_atomic_set_crtc_for_plane(struct drm_atomic_state *state,
+			      struct drm_plane *plane, struct drm_crtc *crtc)
+{
+	struct drm_plane_state *plane_state =
+			drm_atomic_get_plane_state(state, plane);
+	struct drm_crtc_state *crtc_state;
+
+	if (WARN_ON(IS_ERR(plane_state)))
+		return PTR_ERR(plane_state);
+
+	if (plane_state->crtc) {
+		crtc_state = drm_atomic_get_crtc_state(plane_state->state,
+						       plane_state->crtc);
+		if (WARN_ON(IS_ERR(crtc_state)))
+			return PTR_ERR(crtc_state);
+
+		crtc_state->plane_mask &= ~(1 << drm_plane_index(plane));
+	}
+
+	plane_state->crtc = crtc;
+
+	if (crtc) {
+		crtc_state = drm_atomic_get_crtc_state(plane_state->state,
+						       crtc);
+		if (IS_ERR(crtc_state))
+			return PTR_ERR(crtc_state);
+		crtc_state->plane_mask |= (1 << drm_plane_index(plane));
+	}
+
+	if (crtc)
+		DRM_DEBUG_KMS("Link plane state %p to [CRTC:%d]\n",
+			      plane_state, crtc->base.id);
+	else
+		DRM_DEBUG_KMS("Link plane state %p to [NOCRTC]\n", plane_state);
+
+	return 0;
+}
+EXPORT_SYMBOL(drm_atomic_set_crtc_for_plane);
+
+/**
+ * drm_atomic_set_fb_for_plane - set crtc for plane
+ * @plane_state: atomic state object for the plane
+ * @fb: fb to use for the plane
+ *
+ * Changing the assigned framebuffer for a plane requires us to grab a reference
+ * to the new fb and drop the reference to the old fb, if there is one. This
+ * function takes care of all these details besides updating the pointer in the
+ * state object itself.
+ */
+void
+drm_atomic_set_fb_for_plane(struct drm_plane_state *plane_state,
+			    struct drm_framebuffer *fb)
+{
+	if (plane_state->fb)
+		drm_framebuffer_unreference(plane_state->fb);
+	if (fb)
+		drm_framebuffer_reference(fb);
+	plane_state->fb = fb;
+
+	if (fb)
+		DRM_DEBUG_KMS("Set [FB:%d] for plane state %p\n",
+			      fb->base.id, plane_state);
+	else
+		DRM_DEBUG_KMS("Set [NOFB] for plane state %p\n", plane_state);
+}
+EXPORT_SYMBOL(drm_atomic_set_fb_for_plane);
+
+/**
+ * drm_atomic_set_crtc_for_connector - set crtc for connector
+ * @conn_state: atomic state object for the connector
+ * @crtc: crtc to use for the connector
+ *
+ * Changing the assigned crtc for a connector requires us to grab the lock and
+ * state for the new crtc, as needed. This function takes care of all these
+ * details besides updating the pointer in the state object itself.
+ *
+ * Returns:
+ * 0 on success or can fail with -EDEADLK or -ENOMEM. When the error is EDEADLK
+ * then the w/w mutex code has detected a deadlock and the entire atomic
+ * sequence must be restarted. All other errors are fatal.
+ */
+int
+drm_atomic_set_crtc_for_connector(struct drm_connector_state *conn_state,
+				  struct drm_crtc *crtc)
+{
+	struct drm_crtc_state *crtc_state;
+
+	if (crtc) {
+		crtc_state = drm_atomic_get_crtc_state(conn_state->state, crtc);
+		if (IS_ERR(crtc_state))
+			return PTR_ERR(crtc_state);
+	}
+
+	conn_state->crtc = crtc;
+
+	if (crtc)
+		DRM_DEBUG_KMS("Link connector state %p to [CRTC:%d]\n",
+			      conn_state, crtc->base.id);
+	else
+		DRM_DEBUG_KMS("Link connector state %p to [NOCRTC]\n",
+			      conn_state);
+
+	return 0;
+}
+EXPORT_SYMBOL(drm_atomic_set_crtc_for_connector);
+
+/**
+ * drm_atomic_add_affected_connectors - add connectors for crtc
+ * @state: atomic state
+ * @crtc: DRM crtc
+ *
+ * This function walks the current configuration and adds all connectors
+ * currently using @crtc to the atomic configuration @state. Note that this
+ * function must acquire the connection mutex. This can potentially cause
+ * unneeded seralization if the update is just for the planes on one crtc. Hence
+ * drivers and helpers should only call this when really needed (e.g. when a
+ * full modeset needs to happen due to some change).
+ *
+ * Returns:
+ * 0 on success or can fail with -EDEADLK or -ENOMEM. When the error is EDEADLK
+ * then the w/w mutex code has detected a deadlock and the entire atomic
+ * sequence must be restarted. All other errors are fatal.
+ */
+int
+drm_atomic_add_affected_connectors(struct drm_atomic_state *state,
+				   struct drm_crtc *crtc)
+{
+	struct drm_mode_config *config = &state->dev->mode_config;
+	struct drm_connector *connector;
+	struct drm_connector_state *conn_state;
+	int ret;
+
+	ret = drm_modeset_lock(&config->connection_mutex, state->acquire_ctx);
+	if (ret)
+		return ret;
+
+	DRM_DEBUG_KMS("Adding all current connectors for [CRTC:%d] to %p\n",
+		      crtc->base.id, state);
+
+	/*
+	 * Changed connectors are already in @state, so only need to look at the
+	 * current configuration.
+	 */
+	list_for_each_entry(connector, &config->connector_list, head) {
+		if (connector->state->crtc != crtc)
+			continue;
+
+		conn_state = drm_atomic_get_connector_state(state, connector);
+		if (IS_ERR(conn_state))
+			return PTR_ERR(conn_state);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(drm_atomic_add_affected_connectors);
+
+/**
+ * drm_atomic_connectors_for_crtc - count number of connected outputs
+ * @state: atomic state
+ * @crtc: DRM crtc
+ *
+ * This function counts all connectors which will be connected to @crtc
+ * according to @state. Useful to recompute the enable state for @crtc.
+ */
+int
+drm_atomic_connectors_for_crtc(struct drm_atomic_state *state,
+			       struct drm_crtc *crtc)
+{
+	int i, num_connected_connectors = 0;
+
+	for (i = 0; i < state->num_connector; i++) {
+		struct drm_connector_state *conn_state;
+
+		conn_state = state->connector_states[i];
+
+		if (conn_state && conn_state->crtc == crtc)
+			num_connected_connectors++;
+	}
+
+	DRM_DEBUG_KMS("State %p has %i connectors for [CRTC:%d]\n",
+		      state, num_connected_connectors, crtc->base.id);
+
+	return num_connected_connectors;
+}
+EXPORT_SYMBOL(drm_atomic_connectors_for_crtc);
+
+/**
+ * drm_atomic_legacy_backoff - locking backoff for legacy ioctls
+ * @state: atomic state
+ *
+ * This function should be used by legacy entry points which don't understand
+ * -EDEADLK semantics. For simplicity this one will grab all modeset locks after
+ *  the slowpath completed.
+ */
+void drm_atomic_legacy_backoff(struct drm_atomic_state *state)
+{
+	int ret;
+
+retry:
+	drm_modeset_backoff(state->acquire_ctx);
+
+	ret = drm_modeset_lock(&state->dev->mode_config.connection_mutex,
+			       state->acquire_ctx);
+	if (ret)
+		goto retry;
+	ret = drm_modeset_lock_all_crtcs(state->dev,
+					 state->acquire_ctx);
+	if (ret)
+		goto retry;
+}
+EXPORT_SYMBOL(drm_atomic_legacy_backoff);
+
+/**
+ * drm_atomic_check_only - check whether a given config would work
+ * @state: atomic configuration to check
+ *
+ * Note that this function can return -EDEADLK if the driver needed to acquire
+ * more locks but encountered a deadlock. The caller must then do the usual w/w
+ * backoff dance and restart. All other errors are fatal.
+ *
+ * Returns:
+ * 0 on success, negative error code on failure.
+ */
+int drm_atomic_check_only(struct drm_atomic_state *state)
+{
+	struct drm_mode_config *config = &state->dev->mode_config;
+
+	DRM_DEBUG_KMS("checking %p\n", state);
+
+	if (config->funcs->atomic_check)
+		return config->funcs->atomic_check(state->dev, state);
+	else
+		return 0;
+}
+EXPORT_SYMBOL(drm_atomic_check_only);
+
+/**
+ * drm_atomic_commit - commit configuration atomically
+ * @state: atomic configuration to check
+ *
+ * Note that this function can return -EDEADLK if the driver needed to acquire
+ * more locks but encountered a deadlock. The caller must then do the usual w/w
+ * backoff dance and restart. All other errors are fatal.
+ *
+ * Also note that on successful execution ownership of @state is transferred
+ * from the caller of this function to the function itself. The caller must not
+ * free or in any other way access @state. If the function fails then the caller
+ * must clean up @state itself.
+ *
+ * Returns:
+ * 0 on success, negative error code on failure.
+ */
+int drm_atomic_commit(struct drm_atomic_state *state)
+{
+	struct drm_mode_config *config = &state->dev->mode_config;
+	int ret;
+
+	ret = drm_atomic_check_only(state);
+	if (ret)
+		return ret;
+
+	DRM_DEBUG_KMS("commiting %p\n", state);
+
+	return config->funcs->atomic_commit(state->dev, state, false);
+}
+EXPORT_SYMBOL(drm_atomic_commit);
+
+/**
+ * drm_atomic_async_commit - atomic&async configuration commit
+ * @state: atomic configuration to check
+ *
+ * Note that this function can return -EDEADLK if the driver needed to acquire
+ * more locks but encountered a deadlock. The caller must then do the usual w/w
+ * backoff dance and restart. All other errors are fatal.
+ *
+ * Also note that on successful execution ownership of @state is transferred
+ * from the caller of this function to the function itself. The caller must not
+ * free or in any other way access @state. If the function fails then the caller
+ * must clean up @state itself.
+ *
+ * Returns:
+ * 0 on success, negative error code on failure.
+ */
+int drm_atomic_async_commit(struct drm_atomic_state *state)
+{
+	struct drm_mode_config *config = &state->dev->mode_config;
+	int ret;
+
+	ret = drm_atomic_check_only(state);
+	if (ret)
+		return ret;
+
+	DRM_DEBUG_KMS("commiting %p asynchronously\n", state);
+
+	return config->funcs->atomic_commit(state->dev, state, true);
+}
+EXPORT_SYMBOL(drm_atomic_async_commit);
diff --git a/drivers/gpu/drm/drm_atomic_helper.c b/drivers/gpu/drm/drm_atomic_helper.c
new file mode 100644
index 000000000000..bbdbe4721573
--- /dev/null
+++ b/drivers/gpu/drm/drm_atomic_helper.c
@@ -0,0 +1,1966 @@
+/*
+ * Copyright (C) 2014 Red Hat
+ * Copyright (C) 2014 Intel Corp.
+ *
+ * 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:
+ * Rob Clark <robdclark@gmail.com>
+ * Daniel Vetter <daniel.vetter@ffwll.ch>
+ */
+
+#include <drm/drmP.h>
+#include <drm/drm_atomic.h>
+#include <drm/drm_plane_helper.h>
+#include <drm/drm_crtc_helper.h>
+#include <drm/drm_atomic_helper.h>
+#include <linux/fence.h>
+
+/**
+ * DOC: overview
+ *
+ * This helper library provides implementations of check and commit functions on
+ * top of the CRTC modeset helper callbacks and the plane helper callbacks. It
+ * also provides convenience implementations for the atomic state handling
+ * callbacks for drivers which don't need to subclass the drm core structures to
+ * add their own additional internal state.
+ *
+ * This library also provides default implementations for the check callback in
+ * drm_atomic_helper_check and for the commit callback with
+ * drm_atomic_helper_commit. But the individual stages and callbacks are expose
+ * to allow drivers to mix and match and e.g. use the plane helpers only
+ * together with a driver private modeset implementation.
+ *
+ * This library also provides implementations for all the legacy driver
+ * interfaces on top of the atomic interface. See drm_atomic_helper_set_config,
+ * drm_atomic_helper_disable_plane, drm_atomic_helper_disable_plane and the
+ * various functions to implement set_property callbacks. New drivers must not
+ * implement these functions themselves but must use the provided helpers.
+ */
+static void
+drm_atomic_helper_plane_changed(struct drm_atomic_state *state,
+				struct drm_plane_state *plane_state,
+				struct drm_plane *plane)
+{
+	struct drm_crtc_state *crtc_state;
+
+	if (plane->state->crtc) {
+		crtc_state = state->crtc_states[drm_crtc_index(plane->state->crtc)];
+
+		if (WARN_ON(!crtc_state))
+			return;
+
+		crtc_state->planes_changed = true;
+	}
+
+	if (plane_state->crtc) {
+		crtc_state =
+			state->crtc_states[drm_crtc_index(plane_state->crtc)];
+
+		if (WARN_ON(!crtc_state))
+			return;
+
+		crtc_state->planes_changed = true;
+	}
+}
+
+static struct drm_crtc *
+get_current_crtc_for_encoder(struct drm_device *dev,
+			     struct drm_encoder *encoder)
+{
+	struct drm_mode_config *config = &dev->mode_config;
+	struct drm_connector *connector;
+
+	WARN_ON(!drm_modeset_is_locked(&config->connection_mutex));
+
+	list_for_each_entry(connector, &config->connector_list, head) {
+		if (connector->state->best_encoder != encoder)
+			continue;
+
+		return connector->state->crtc;
+	}
+
+	return NULL;
+}
+
+static int
+steal_encoder(struct drm_atomic_state *state,
+	      struct drm_encoder *encoder,
+	      struct drm_crtc *encoder_crtc)
+{
+	struct drm_mode_config *config = &state->dev->mode_config;
+	struct drm_crtc_state *crtc_state;
+	struct drm_connector *connector;
+	struct drm_connector_state *connector_state;
+	int ret;
+
+	/*
+	 * We can only steal an encoder coming from a connector, which means we
+	 * must already hold the connection_mutex.
+	 */
+	WARN_ON(!drm_modeset_is_locked(&config->connection_mutex));
+
+	DRM_DEBUG_KMS("[ENCODER:%d:%s] in use on [CRTC:%d], stealing it\n",
+		      encoder->base.id, encoder->name,
+		      encoder_crtc->base.id);
+
+	crtc_state = drm_atomic_get_crtc_state(state, encoder_crtc);
+	if (IS_ERR(crtc_state))
+		return PTR_ERR(crtc_state);
+
+	crtc_state->mode_changed = true;
+
+	list_for_each_entry(connector, &config->connector_list, head) {
+		if (connector->state->best_encoder != encoder)
+			continue;
+
+		DRM_DEBUG_KMS("Stealing encoder from [CONNECTOR:%d:%s]\n",
+			      connector->base.id,
+			      connector->name);
+
+		connector_state = drm_atomic_get_connector_state(state,
+								 connector);
+		if (IS_ERR(connector_state))
+			return PTR_ERR(connector_state);
+
+		ret = drm_atomic_set_crtc_for_connector(connector_state, NULL);
+		if (ret)
+			return ret;
+		connector_state->best_encoder = NULL;
+	}
+
+	return 0;
+}
+
+static int
+update_connector_routing(struct drm_atomic_state *state, int conn_idx)
+{
+	struct drm_connector_helper_funcs *funcs;
+	struct drm_encoder *new_encoder;
+	struct drm_crtc *encoder_crtc;
+	struct drm_connector *connector;
+	struct drm_connector_state *connector_state;
+	struct drm_crtc_state *crtc_state;
+	int idx, ret;
+
+	connector = state->connectors[conn_idx];
+	connector_state = state->connector_states[conn_idx];
+
+	if (!connector)
+		return 0;
+
+	DRM_DEBUG_KMS("Updating routing for [CONNECTOR:%d:%s]\n",
+			connector->base.id,
+			connector->name);
+
+	if (connector->state->crtc != connector_state->crtc) {
+		if (connector->state->crtc) {
+			idx = drm_crtc_index(connector->state->crtc);
+
+			crtc_state = state->crtc_states[idx];
+			crtc_state->mode_changed = true;
+		}
+
+		if (connector_state->crtc) {
+			idx = drm_crtc_index(connector_state->crtc);
+
+			crtc_state = state->crtc_states[idx];
+			crtc_state->mode_changed = true;
+		}
+	}
+
+	if (!connector_state->crtc) {
+		DRM_DEBUG_KMS("Disabling [CONNECTOR:%d:%s]\n",
+				connector->base.id,
+				connector->name);
+
+		connector_state->best_encoder = NULL;
+
+		return 0;
+	}
+
+	funcs = connector->helper_private;
+	new_encoder = funcs->best_encoder(connector);
+
+	if (!new_encoder) {
+		DRM_DEBUG_KMS("No suitable encoder found for [CONNECTOR:%d:%s]\n",
+			      connector->base.id,
+			      connector->name);
+		return -EINVAL;
+	}
+
+	if (new_encoder == connector_state->best_encoder) {
+		DRM_DEBUG_KMS("[CONNECTOR:%d:%s] keeps [ENCODER:%d:%s], now on [CRTC:%d]\n",
+			      connector->base.id,
+			      connector->name,
+			      new_encoder->base.id,
+			      new_encoder->name,
+			      connector_state->crtc->base.id);
+
+		return 0;
+	}
+
+	encoder_crtc = get_current_crtc_for_encoder(state->dev,
+						    new_encoder);
+
+	if (encoder_crtc) {
+		ret = steal_encoder(state, new_encoder, encoder_crtc);
+		if (ret) {
+			DRM_DEBUG_KMS("Encoder stealing failed for [CONNECTOR:%d:%s]\n",
+				      connector->base.id,
+				      connector->name);
+			return ret;
+		}
+	}
+
+	connector_state->best_encoder = new_encoder;
+	idx = drm_crtc_index(connector_state->crtc);
+
+	crtc_state = state->crtc_states[idx];
+	crtc_state->mode_changed = true;
+
+	DRM_DEBUG_KMS("[CONNECTOR:%d:%s] using [ENCODER:%d:%s] on [CRTC:%d]\n",
+		      connector->base.id,
+		      connector->name,
+		      new_encoder->base.id,
+		      new_encoder->name,
+		      connector_state->crtc->base.id);
+
+	return 0;
+}
+
+static int
+mode_fixup(struct drm_atomic_state *state)
+{
+	int ncrtcs = state->dev->mode_config.num_crtc;
+	struct drm_crtc_state *crtc_state;
+	struct drm_connector_state *conn_state;
+	int i;
+	bool ret;
+
+	for (i = 0; i < ncrtcs; i++) {
+		crtc_state = state->crtc_states[i];
+
+		if (!crtc_state || !crtc_state->mode_changed)
+			continue;
+
+		drm_mode_copy(&crtc_state->adjusted_mode, &crtc_state->mode);
+	}
+
+	for (i = 0; i < state->num_connector; i++) {
+		struct drm_encoder_helper_funcs *funcs;
+		struct drm_encoder *encoder;
+
+		conn_state = state->connector_states[i];
+
+		if (!conn_state)
+			continue;
+
+		WARN_ON(!!conn_state->best_encoder != !!conn_state->crtc);
+
+		if (!conn_state->crtc || !conn_state->best_encoder)
+			continue;
+
+		crtc_state =
+			state->crtc_states[drm_crtc_index(conn_state->crtc)];
+
+		/*
+		 * Each encoder has at most one connector (since we always steal
+		 * it away), so we won't call ->mode_fixup twice.
+		 */
+		encoder = conn_state->best_encoder;
+		funcs = encoder->helper_private;
+
+		if (encoder->bridge && encoder->bridge->funcs->mode_fixup) {
+			ret = encoder->bridge->funcs->mode_fixup(
+					encoder->bridge, &crtc_state->mode,
+					&crtc_state->adjusted_mode);
+			if (!ret) {
+				DRM_DEBUG_KMS("Bridge fixup failed\n");
+				return -EINVAL;
+			}
+		}
+
+
+		ret = funcs->mode_fixup(encoder, &crtc_state->mode,
+					&crtc_state->adjusted_mode);
+		if (!ret) {
+			DRM_DEBUG_KMS("[ENCODER:%d:%s] fixup failed\n",
+				      encoder->base.id, encoder->name);
+			return -EINVAL;
+		}
+	}
+
+	for (i = 0; i < ncrtcs; i++) {
+		struct drm_crtc_helper_funcs *funcs;
+		struct drm_crtc *crtc;
+
+		crtc_state = state->crtc_states[i];
+		crtc = state->crtcs[i];
+
+		if (!crtc_state || !crtc_state->mode_changed)
+			continue;
+
+		funcs = crtc->helper_private;
+		ret = funcs->mode_fixup(crtc, &crtc_state->mode,
+					&crtc_state->adjusted_mode);
+		if (!ret) {
+			DRM_DEBUG_KMS("[CRTC:%d] fixup failed\n",
+				      crtc->base.id);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int
+drm_atomic_helper_check_modeset(struct drm_device *dev,
+				struct drm_atomic_state *state)
+{
+	int ncrtcs = dev->mode_config.num_crtc;
+	struct drm_crtc *crtc;
+	struct drm_crtc_state *crtc_state;
+	int i, ret;
+
+	for (i = 0; i < ncrtcs; i++) {
+		crtc = state->crtcs[i];
+		crtc_state = state->crtc_states[i];
+
+		if (!crtc)
+			continue;
+
+		if (!drm_mode_equal(&crtc->state->mode, &crtc_state->mode)) {
+			DRM_DEBUG_KMS("[CRTC:%d] mode changed\n",
+				      crtc->base.id);
+			crtc_state->mode_changed = true;
+		}
+
+		if (crtc->state->enable != crtc_state->enable) {
+			DRM_DEBUG_KMS("[CRTC:%d] enable changed\n",
+				      crtc->base.id);
+			crtc_state->mode_changed = true;
+		}
+	}
+
+	for (i = 0; i < state->num_connector; i++) {
+		/*
+		 * This only sets crtc->mode_changed for routing changes,
+		 * drivers must set crtc->mode_changed themselves when connector
+		 * properties need to be updated.
+		 */
+		ret = update_connector_routing(state, i);
+		if (ret)
+			return ret;
+	}
+
+	/*
+	 * After all the routing has been prepared we need to add in any
+	 * connector which is itself unchanged, but who's crtc changes it's
+	 * configuration. This must be done before calling mode_fixup in case a
+	 * crtc only changed its mode but has the same set of connectors.
+	 */
+	for (i = 0; i < ncrtcs; i++) {
+		int num_connectors;
+
+		crtc = state->crtcs[i];
+		crtc_state = state->crtc_states[i];
+
+		if (!crtc || !crtc_state->mode_changed)
+			continue;
+
+		DRM_DEBUG_KMS("[CRTC:%d] needs full modeset, enable: %c\n",
+			      crtc->base.id,
+			      crtc_state->enable ? 'y' : 'n');
+
+		ret = drm_atomic_add_affected_connectors(state, crtc);
+		if (ret != 0)
+			return ret;
+
+		num_connectors = drm_atomic_connectors_for_crtc(state,
+								crtc);
+
+		if (crtc_state->enable != !!num_connectors) {
+			DRM_DEBUG_KMS("[CRTC:%d] enabled/connectors mismatch\n",
+				      crtc->base.id);
+
+			return -EINVAL;
+		}
+	}
+
+	return mode_fixup(state);
+}
+
+/**
+ * drm_atomic_helper_check - validate state object
+ * @dev: DRM device
+ * @state: the driver state object
+ *
+ * Check the state object to see if the requested state is physically possible.
+ * Only crtcs and planes have check callbacks, so for any additional (global)
+ * checking that a driver needs it can simply wrap that around this function.
+ * Drivers without such needs can directly use this as their ->atomic_check()
+ * callback.
+ *
+ * RETURNS
+ * Zero for success or -errno
+ */
+int drm_atomic_helper_check(struct drm_device *dev,
+			    struct drm_atomic_state *state)
+{
+	int nplanes = dev->mode_config.num_total_plane;
+	int ncrtcs = dev->mode_config.num_crtc;
+	int i, ret = 0;
+
+	for (i = 0; i < nplanes; i++) {
+		struct drm_plane_helper_funcs *funcs;
+		struct drm_plane *plane = state->planes[i];
+		struct drm_plane_state *plane_state = state->plane_states[i];
+
+		if (!plane)
+			continue;
+
+		funcs = plane->helper_private;
+
+		drm_atomic_helper_plane_changed(state, plane_state, plane);
+
+		if (!funcs || !funcs->atomic_check)
+			continue;
+
+		ret = funcs->atomic_check(plane, plane_state);
+		if (ret) {
+			DRM_DEBUG_KMS("[PLANE:%d] atomic check failed\n",
+				      plane->base.id);
+			return ret;
+		}
+	}
+
+	for (i = 0; i < ncrtcs; i++) {
+		struct drm_crtc_helper_funcs *funcs;
+		struct drm_crtc *crtc = state->crtcs[i];
+
+		if (!crtc)
+			continue;
+
+		funcs = crtc->helper_private;
+
+		if (!funcs || !funcs->atomic_check)
+			continue;
+
+		ret = funcs->atomic_check(crtc, state->crtc_states[i]);
+		if (ret) {
+			DRM_DEBUG_KMS("[CRTC:%d] atomic check failed\n",
+				      crtc->base.id);
+			return ret;
+		}
+	}
+
+	ret = drm_atomic_helper_check_modeset(dev, state);
+	if (ret)
+		return ret;
+
+	return ret;
+}
+EXPORT_SYMBOL(drm_atomic_helper_check);
+
+static void
+disable_outputs(struct drm_device *dev, struct drm_atomic_state *old_state)
+{
+	int ncrtcs = old_state->dev->mode_config.num_crtc;
+	int i;
+
+	for (i = 0; i < old_state->num_connector; i++) {
+		struct drm_connector_state *old_conn_state;
+		struct drm_connector *connector;
+		struct drm_encoder_helper_funcs *funcs;
+		struct drm_encoder *encoder;
+
+		old_conn_state = old_state->connector_states[i];
+		connector = old_state->connectors[i];
+
+		/* Shut down everything that's in the changeset and currently
+		 * still on. So need to check the old, saved state. */
+		if (!old_conn_state || !old_conn_state->crtc)
+			continue;
+
+		encoder = old_conn_state->best_encoder;
+
+		/* We shouldn't get this far if we didn't previously have
+		 * an encoder.. but WARN_ON() rather than explode.
+		 */
+		if (WARN_ON(!encoder))
+			continue;
+
+		funcs = encoder->helper_private;
+
+		/*
+		 * Each encoder has at most one connector (since we always steal
+		 * it away), so we won't call call disable hooks twice.
+		 */
+		if (encoder->bridge)
+			encoder->bridge->funcs->disable(encoder->bridge);
+
+		/* Right function depends upon target state. */
+		if (connector->state->crtc)
+			funcs->prepare(encoder);
+		else if (funcs->disable)
+			funcs->disable(encoder);
+		else
+			funcs->dpms(encoder, DRM_MODE_DPMS_OFF);
+
+		if (encoder->bridge)
+			encoder->bridge->funcs->post_disable(encoder->bridge);
+	}
+
+	for (i = 0; i < ncrtcs; i++) {
+		struct drm_crtc_helper_funcs *funcs;
+		struct drm_crtc *crtc;
+
+		crtc = old_state->crtcs[i];
+
+		/* Shut down everything that needs a full modeset. */
+		if (!crtc || !crtc->state->mode_changed)
+			continue;
+
+		funcs = crtc->helper_private;
+
+		/* Right function depends upon target state. */
+		if (crtc->state->enable)
+			funcs->prepare(crtc);
+		else if (funcs->disable)
+			funcs->disable(crtc);
+		else
+			funcs->dpms(crtc, DRM_MODE_DPMS_OFF);
+	}
+}
+
+static void
+set_routing_links(struct drm_device *dev, struct drm_atomic_state *old_state)
+{
+	int ncrtcs = old_state->dev->mode_config.num_crtc;
+	int i;
+
+	/* clear out existing links */
+	for (i = 0; i < old_state->num_connector; i++) {
+		struct drm_connector *connector;
+
+		connector = old_state->connectors[i];
+
+		if (!connector || !connector->encoder)
+			continue;
+
+		WARN_ON(!connector->encoder->crtc);
+
+		connector->encoder->crtc = NULL;
+		connector->encoder = NULL;
+	}
+
+	/* set new links */
+	for (i = 0; i < old_state->num_connector; i++) {
+		struct drm_connector *connector;
+
+		connector = old_state->connectors[i];
+
+		if (!connector || !connector->state->crtc)
+			continue;
+
+		if (WARN_ON(!connector->state->best_encoder))
+			continue;
+
+		connector->encoder = connector->state->best_encoder;
+		connector->encoder->crtc = connector->state->crtc;
+	}
+
+	/* set legacy state in the crtc structure */
+	for (i = 0; i < ncrtcs; i++) {
+		struct drm_crtc *crtc;
+
+		crtc = old_state->crtcs[i];
+
+		if (!crtc)
+			continue;
+
+		crtc->mode = crtc->state->mode;
+		crtc->enabled = crtc->state->enable;
+		crtc->x = crtc->primary->state->src_x >> 16;
+		crtc->y = crtc->primary->state->src_y >> 16;
+	}
+}
+
+static void
+crtc_set_mode(struct drm_device *dev, struct drm_atomic_state *old_state)
+{
+	int ncrtcs = old_state->dev->mode_config.num_crtc;
+	int i;
+
+	for (i = 0; i < ncrtcs; i++) {
+		struct drm_crtc_helper_funcs *funcs;
+		struct drm_crtc *crtc;
+
+		crtc = old_state->crtcs[i];
+
+		if (!crtc || !crtc->state->mode_changed)
+			continue;
+
+		funcs = crtc->helper_private;
+
+		if (crtc->state->enable)
+			funcs->mode_set_nofb(crtc);
+	}
+
+	for (i = 0; i < old_state->num_connector; i++) {
+		struct drm_connector *connector;
+		struct drm_crtc_state *new_crtc_state;
+		struct drm_encoder_helper_funcs *funcs;
+		struct drm_encoder *encoder;
+		struct drm_display_mode *mode, *adjusted_mode;
+
+		connector = old_state->connectors[i];
+
+		if (!connector || !connector->state->best_encoder)
+			continue;
+
+		encoder = connector->state->best_encoder;
+		funcs = encoder->helper_private;
+		new_crtc_state = connector->state->crtc->state;
+		mode = &new_crtc_state->mode;
+		adjusted_mode = &new_crtc_state->adjusted_mode;
+
+		/*
+		 * Each encoder has at most one connector (since we always steal
+		 * it away), so we won't call call mode_set hooks twice.
+		 */
+		funcs->mode_set(encoder, mode, adjusted_mode);
+
+		if (encoder->bridge && encoder->bridge->funcs->mode_set)
+			encoder->bridge->funcs->mode_set(encoder->bridge,
+							 mode, adjusted_mode);
+	}
+}
+
+/**
+ * drm_atomic_helper_commit_pre_planes - modeset commit before plane updates
+ * @dev: DRM device
+ * @state: atomic state
+ *
+ * This function commits the modeset changes that need to be committed before
+ * updating planes. It shuts down all the outputs that need to be shut down and
+ * prepares them (if required) with the new mode.
+ */
+void drm_atomic_helper_commit_pre_planes(struct drm_device *dev,
+					 struct drm_atomic_state *state)
+{
+	disable_outputs(dev, state);
+	set_routing_links(dev, state);
+	crtc_set_mode(dev, state);
+}
+EXPORT_SYMBOL(drm_atomic_helper_commit_pre_planes);
+
+/**
+ * drm_atomic_helper_commit_post_planes - modeset commit after plane updates
+ * @dev: DRM device
+ * @old_state: atomic state object with old state structures
+ *
+ * This function commits the modeset changes that need to be committed after
+ * updating planes: It enables all the outputs with the new configuration which
+ * had to be turned off for the update.
+ */
+void drm_atomic_helper_commit_post_planes(struct drm_device *dev,
+					  struct drm_atomic_state *old_state)
+{
+	int ncrtcs = old_state->dev->mode_config.num_crtc;
+	int i;
+
+	for (i = 0; i < ncrtcs; i++) {
+		struct drm_crtc_helper_funcs *funcs;
+		struct drm_crtc *crtc;
+
+		crtc = old_state->crtcs[i];
+
+		/* Need to filter out CRTCs where only planes change. */
+		if (!crtc || !crtc->state->mode_changed)
+			continue;
+
+		funcs = crtc->helper_private;
+
+		if (crtc->state->enable)
+			funcs->commit(crtc);
+	}
+
+	for (i = 0; i < old_state->num_connector; i++) {
+		struct drm_connector *connector;
+		struct drm_encoder_helper_funcs *funcs;
+		struct drm_encoder *encoder;
+
+		connector = old_state->connectors[i];
+
+		if (!connector || !connector->state->best_encoder)
+			continue;
+
+		encoder = connector->state->best_encoder;
+		funcs = encoder->helper_private;
+
+		/*
+		 * Each encoder has at most one connector (since we always steal
+		 * it away), so we won't call call enable hooks twice.
+		 */
+		if (encoder->bridge)
+			encoder->bridge->funcs->pre_enable(encoder->bridge);
+
+		funcs->commit(encoder);
+
+		if (encoder->bridge)
+			encoder->bridge->funcs->enable(encoder->bridge);
+	}
+}
+EXPORT_SYMBOL(drm_atomic_helper_commit_post_planes);
+
+static void wait_for_fences(struct drm_device *dev,
+			    struct drm_atomic_state *state)
+{
+	int nplanes = dev->mode_config.num_total_plane;
+	int i;
+
+	for (i = 0; i < nplanes; i++) {
+		struct drm_plane *plane = state->planes[i];
+
+		if (!plane || !plane->state->fence)
+			continue;
+
+		WARN_ON(!plane->state->fb);
+
+		fence_wait(plane->state->fence, false);
+		fence_put(plane->state->fence);
+		plane->state->fence = NULL;
+	}
+}
+
+static bool framebuffer_changed(struct drm_device *dev,
+				struct drm_atomic_state *old_state,
+				struct drm_crtc *crtc)
+{
+	struct drm_plane *plane;
+	struct drm_plane_state *old_plane_state;
+	int nplanes = old_state->dev->mode_config.num_total_plane;
+	int i;
+
+	for (i = 0; i < nplanes; i++) {
+		plane = old_state->planes[i];
+		old_plane_state = old_state->plane_states[i];
+
+		if (!plane)
+			continue;
+
+		if (plane->state->crtc != crtc &&
+		    old_plane_state->crtc != crtc)
+			continue;
+
+		if (plane->state->fb != old_plane_state->fb)
+			return true;
+	}
+
+	return false;
+}
+
+/**
+ * drm_atomic_helper_wait_for_vblanks - wait for vblank on crtcs
+ * @dev: DRM device
+ * @old_state: atomic state object with old state structures
+ *
+ * Helper to, after atomic commit, wait for vblanks on all effected
+ * crtcs (ie. before cleaning up old framebuffers using
+ * drm_atomic_helper_cleanup_planes()). It will only wait on crtcs where the
+ * framebuffers have actually changed to optimize for the legacy cursor and
+ * plane update use-case.
+ */
+void
+drm_atomic_helper_wait_for_vblanks(struct drm_device *dev,
+		struct drm_atomic_state *old_state)
+{
+	struct drm_crtc *crtc;
+	struct drm_crtc_state *old_crtc_state;
+	int ncrtcs = old_state->dev->mode_config.num_crtc;
+	int i, ret;
+
+	for (i = 0; i < ncrtcs; i++) {
+		crtc = old_state->crtcs[i];
+		old_crtc_state = old_state->crtc_states[i];
+
+		if (!crtc)
+			continue;
+
+		/* No one cares about the old state, so abuse it for tracking
+		 * and store whether we hold a vblank reference (and should do a
+		 * vblank wait) in the ->enable boolean. */
+		old_crtc_state->enable = false;
+
+		if (!crtc->state->enable)
+			continue;
+
+		if (!framebuffer_changed(dev, old_state, crtc))
+			continue;
+
+		ret = drm_crtc_vblank_get(crtc);
+		if (ret != 0)
+			continue;
+
+		old_crtc_state->enable = true;
+		old_crtc_state->last_vblank_count = drm_vblank_count(dev, i);
+	}
+
+	for (i = 0; i < ncrtcs; i++) {
+		crtc = old_state->crtcs[i];
+		old_crtc_state = old_state->crtc_states[i];
+
+		if (!crtc || !old_crtc_state->enable)
+			continue;
+
+		ret = wait_event_timeout(dev->vblank[i].queue,
+				old_crtc_state->last_vblank_count !=
+					drm_vblank_count(dev, i),
+				msecs_to_jiffies(50));
+
+		drm_crtc_vblank_put(crtc);
+	}
+}
+EXPORT_SYMBOL(drm_atomic_helper_wait_for_vblanks);
+
+/**
+ * drm_atomic_helper_commit - commit validated state object
+ * @dev: DRM device
+ * @state: the driver state object
+ * @async: asynchronous commit
+ *
+ * This function commits a with drm_atomic_helper_check() pre-validated state
+ * object. This can still fail when e.g. the framebuffer reservation fails. For
+ * now this doesn't implement asynchronous commits.
+ *
+ * RETURNS
+ * Zero for success or -errno.
+ */
+int drm_atomic_helper_commit(struct drm_device *dev,
+			     struct drm_atomic_state *state,
+			     bool async)
+{
+	int ret;
+
+	if (async)
+		return -EBUSY;
+
+	ret = drm_atomic_helper_prepare_planes(dev, state);
+	if (ret)
+		return ret;
+
+	/*
+	 * This is the point of no return - everything below never fails except
+	 * when the hw goes bonghits. Which means we can commit the new state on
+	 * the software side now.
+	 */
+
+	drm_atomic_helper_swap_state(dev, state);
+
+	/*
+	 * Everything below can be run asynchronously without the need to grab
+	 * any modeset locks at all under one conditions: It must be guaranteed
+	 * that the asynchronous work has either been cancelled (if the driver
+	 * supports it, which at least requires that the framebuffers get
+	 * cleaned up with drm_atomic_helper_cleanup_planes()) or completed
+	 * before the new state gets committed on the software side with
+	 * drm_atomic_helper_swap_state().
+	 *
+	 * This scheme allows new atomic state updates to be prepared and
+	 * checked in parallel to the asynchronous completion of the previous
+	 * update. Which is important since compositors need to figure out the
+	 * composition of the next frame right after having submitted the
+	 * current layout.
+	 */
+
+	wait_for_fences(dev, state);
+
+	drm_atomic_helper_commit_pre_planes(dev, state);
+
+	drm_atomic_helper_commit_planes(dev, state);
+
+	drm_atomic_helper_commit_post_planes(dev, state);
+
+	drm_atomic_helper_wait_for_vblanks(dev, state);
+
+	drm_atomic_helper_cleanup_planes(dev, state);
+
+	drm_atomic_state_free(state);
+
+	return 0;
+}
+EXPORT_SYMBOL(drm_atomic_helper_commit);
+
+/**
+ * DOC: implementing async commit
+ *
+ * For now the atomic helpers don't support async commit directly. If there is
+ * real need it could be added though, using the dma-buf fence infrastructure
+ * for generic synchronization with outstanding rendering.
+ *
+ * For now drivers have to implement async commit themselves, with the following
+ * sequence being the recommended one:
+ *
+ * 1. Run drm_atomic_helper_prepare_planes() first. This is the only function
+ * which commit needs to call which can fail, so we want to run it first and
+ * synchronously.
+ *
+ * 2. Synchronize with any outstanding asynchronous commit worker threads which
+ * might be affected the new state update. This can be done by either cancelling
+ * or flushing the work items, depending upon whether the driver can deal with
+ * cancelled updates. Note that it is important to ensure that the framebuffer
+ * cleanup is still done when cancelling.
+ *
+ * For sufficient parallelism it is recommended to have a work item per crtc
+ * (for updates which don't touch global state) and a global one. Then we only
+ * need to synchronize with the crtc work items for changed crtcs and the global
+ * work item, which allows nice concurrent updates on disjoint sets of crtcs.
+ *
+ * 3. The software state is updated synchronously with
+ * drm_atomic_helper_swap_state. Doing this under the protection of all modeset
+ * locks means concurrent callers never see inconsistent state. And doing this
+ * while it's guaranteed that no relevant async worker runs means that async
+ * workers do not need grab any locks. Actually they must not grab locks, for
+ * otherwise the work flushing will deadlock.
+ *
+ * 4. Schedule a work item to do all subsequent steps, using the split-out
+ * commit helpers: a) pre-plane commit b) plane commit c) post-plane commit and
+ * then cleaning up the framebuffers after the old framebuffer is no longer
+ * being displayed.
+ */
+
+/**
+ * drm_atomic_helper_prepare_planes - prepare plane resources after commit
+ * @dev: DRM device
+ * @state: atomic state object with old state structures
+ *
+ * This function prepares plane state, specifically framebuffers, for the new
+ * configuration. If any failure is encountered this function will call
+ * ->cleanup_fb on any already successfully prepared framebuffer.
+ *
+ * Returns:
+ * 0 on success, negative error code on failure.
+ */
+int drm_atomic_helper_prepare_planes(struct drm_device *dev,
+				     struct drm_atomic_state *state)
+{
+	int nplanes = dev->mode_config.num_total_plane;
+	int ret, i;
+
+	for (i = 0; i < nplanes; i++) {
+		struct drm_plane_helper_funcs *funcs;
+		struct drm_plane *plane = state->planes[i];
+		struct drm_framebuffer *fb;
+
+		if (!plane)
+			continue;
+
+		funcs = plane->helper_private;
+
+		fb = state->plane_states[i]->fb;
+
+		if (fb && funcs->prepare_fb) {
+			ret = funcs->prepare_fb(plane, fb);
+			if (ret)
+				goto fail;
+		}
+	}
+
+	return 0;
+
+fail:
+	for (i--; i >= 0; i--) {
+		struct drm_plane_helper_funcs *funcs;
+		struct drm_plane *plane = state->planes[i];
+		struct drm_framebuffer *fb;
+
+		if (!plane)
+			continue;
+
+		funcs = plane->helper_private;
+
+		fb = state->plane_states[i]->fb;
+
+		if (fb && funcs->cleanup_fb)
+			funcs->cleanup_fb(plane, fb);
+
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL(drm_atomic_helper_prepare_planes);
+
+/**
+ * drm_atomic_helper_commit_planes - commit plane state
+ * @dev: DRM device
+ * @old_state: atomic state object with old state structures
+ *
+ * This function commits the new plane state using the plane and atomic helper
+ * functions for planes and crtcs. It assumes that the atomic state has already
+ * been pushed into the relevant object state pointers, since this step can no
+ * longer fail.
+ *
+ * It still requires the global state object @old_state to know which planes and
+ * crtcs need to be updated though.
+ */
+void drm_atomic_helper_commit_planes(struct drm_device *dev,
+				     struct drm_atomic_state *old_state)
+{
+	int nplanes = dev->mode_config.num_total_plane;
+	int ncrtcs = dev->mode_config.num_crtc;
+	int i;
+
+	for (i = 0; i < ncrtcs; i++) {
+		struct drm_crtc_helper_funcs *funcs;
+		struct drm_crtc *crtc = old_state->crtcs[i];
+
+		if (!crtc)
+			continue;
+
+		funcs = crtc->helper_private;
+
+		if (!funcs || !funcs->atomic_begin)
+			continue;
+
+		funcs->atomic_begin(crtc);
+	}
+
+	for (i = 0; i < nplanes; i++) {
+		struct drm_plane_helper_funcs *funcs;
+		struct drm_plane *plane = old_state->planes[i];
+		struct drm_plane_state *old_plane_state;
+
+		if (!plane)
+			continue;
+
+		funcs = plane->helper_private;
+
+		if (!funcs || !funcs->atomic_update)
+			continue;
+
+		old_plane_state = old_state->plane_states[i];
+
+		funcs->atomic_update(plane, old_plane_state);
+	}
+
+	for (i = 0; i < ncrtcs; i++) {
+		struct drm_crtc_helper_funcs *funcs;
+		struct drm_crtc *crtc = old_state->crtcs[i];
+
+		if (!crtc)
+			continue;
+
+		funcs = crtc->helper_private;
+
+		if (!funcs || !funcs->atomic_flush)
+			continue;
+
+		funcs->atomic_flush(crtc);
+	}
+}
+EXPORT_SYMBOL(drm_atomic_helper_commit_planes);
+
+/**
+ * drm_atomic_helper_cleanup_planes - cleanup plane resources after commit
+ * @dev: DRM device
+ * @old_state: atomic state object with old state structures
+ *
+ * This function cleans up plane state, specifically framebuffers, from the old
+ * configuration. Hence the old configuration must be perserved in @old_state to
+ * be able to call this function.
+ *
+ * This function must also be called on the new state when the atomic update
+ * fails at any point after calling drm_atomic_helper_prepare_planes().
+ */
+void drm_atomic_helper_cleanup_planes(struct drm_device *dev,
+				      struct drm_atomic_state *old_state)
+{
+	int nplanes = dev->mode_config.num_total_plane;
+	int i;
+
+	for (i = 0; i < nplanes; i++) {
+		struct drm_plane_helper_funcs *funcs;
+		struct drm_plane *plane = old_state->planes[i];
+		struct drm_framebuffer *old_fb;
+
+		if (!plane)
+			continue;
+
+		funcs = plane->helper_private;
+
+		old_fb = old_state->plane_states[i]->fb;
+
+		if (old_fb && funcs->cleanup_fb)
+			funcs->cleanup_fb(plane, old_fb);
+	}
+}
+EXPORT_SYMBOL(drm_atomic_helper_cleanup_planes);
+
+/**
+ * drm_atomic_helper_swap_state - store atomic state into current sw state
+ * @dev: DRM device
+ * @state: atomic state
+ *
+ * This function stores the atomic state into the current state pointers in all
+ * driver objects. It should be called after all failing steps have been done
+ * and succeeded, but before the actual hardware state is committed.
+ *
+ * For cleanup and error recovery the current state for all changed objects will
+ * be swaped into @state.
+ *
+ * With that sequence it fits perfectly into the plane prepare/cleanup sequence:
+ *
+ * 1. Call drm_atomic_helper_prepare_planes() with the staged atomic state.
+ *
+ * 2. Do any other steps that might fail.
+ *
+ * 3. Put the staged state into the current state pointers with this function.
+ *
+ * 4. Actually commit the hardware state.
+ *
+ * 5. Call drm_atomic_helper_cleanup_planes with @state, which since step 3
+ * contains the old state. Also do any other cleanup required with that state.
+ */
+void drm_atomic_helper_swap_state(struct drm_device *dev,
+				  struct drm_atomic_state *state)
+{
+	int i;
+
+	for (i = 0; i < dev->mode_config.num_connector; i++) {
+		struct drm_connector *connector = state->connectors[i];
+
+		if (!connector)
+			continue;
+
+		connector->state->state = state;
+		swap(state->connector_states[i], connector->state);
+		connector->state->state = NULL;
+	}
+
+	for (i = 0; i < dev->mode_config.num_crtc; i++) {
+		struct drm_crtc *crtc = state->crtcs[i];
+
+		if (!crtc)
+			continue;
+
+		crtc->state->state = state;
+		swap(state->crtc_states[i], crtc->state);
+		crtc->state->state = NULL;
+	}
+
+	for (i = 0; i < dev->mode_config.num_total_plane; i++) {
+		struct drm_plane *plane = state->planes[i];
+
+		if (!plane)
+			continue;
+
+		plane->state->state = state;
+		swap(state->plane_states[i], plane->state);
+		plane->state->state = NULL;
+	}
+}
+EXPORT_SYMBOL(drm_atomic_helper_swap_state);
+
+/**
+ * drm_atomic_helper_update_plane - Helper for primary plane update using atomic
+ * @plane: plane object to update
+ * @crtc: owning CRTC of owning plane
+ * @fb: framebuffer to flip onto plane
+ * @crtc_x: x offset of primary plane on crtc
+ * @crtc_y: y offset of primary plane on crtc
+ * @crtc_w: width of primary plane rectangle on crtc
+ * @crtc_h: height of primary plane rectangle on crtc
+ * @src_x: x offset of @fb for panning
+ * @src_y: y offset of @fb for panning
+ * @src_w: width of source rectangle in @fb
+ * @src_h: height of source rectangle in @fb
+ *
+ * Provides a default plane update handler using the atomic driver interface.
+ *
+ * RETURNS:
+ * Zero on success, error code on failure
+ */
+int drm_atomic_helper_update_plane(struct drm_plane *plane,
+				   struct drm_crtc *crtc,
+				   struct drm_framebuffer *fb,
+				   int crtc_x, int crtc_y,
+				   unsigned int crtc_w, unsigned int crtc_h,
+				   uint32_t src_x, uint32_t src_y,
+				   uint32_t src_w, uint32_t src_h)
+{
+	struct drm_atomic_state *state;
+	struct drm_plane_state *plane_state;
+	int ret = 0;
+
+	state = drm_atomic_state_alloc(plane->dev);
+	if (!state)
+		return -ENOMEM;
+
+	state->acquire_ctx = drm_modeset_legacy_acquire_ctx(crtc);
+retry:
+	plane_state = drm_atomic_get_plane_state(state, plane);
+	if (IS_ERR(plane_state)) {
+		ret = PTR_ERR(plane_state);
+		goto fail;
+	}
+
+	ret = drm_atomic_set_crtc_for_plane(state, plane, crtc);
+	if (ret != 0)
+		goto fail;
+	drm_atomic_set_fb_for_plane(plane_state, fb);
+	plane_state->crtc_x = crtc_x;
+	plane_state->crtc_y = crtc_y;
+	plane_state->crtc_h = crtc_h;
+	plane_state->crtc_w = crtc_w;
+	plane_state->src_x = src_x;
+	plane_state->src_y = src_y;
+	plane_state->src_h = src_h;
+	plane_state->src_w = src_w;
+
+	ret = drm_atomic_commit(state);
+	if (ret != 0)
+		goto fail;
+
+	/* Driver takes ownership of state on successful commit. */
+	return 0;
+fail:
+	if (ret == -EDEADLK)
+		goto backoff;
+
+	drm_atomic_state_free(state);
+
+	return ret;
+backoff:
+	drm_atomic_state_clear(state);
+	drm_atomic_legacy_backoff(state);
+
+	/*
+	 * Someone might have exchanged the framebuffer while we dropped locks
+	 * in the backoff code. We need to fix up the fb refcount tracking the
+	 * core does for us.
+	 */
+	plane->old_fb = plane->fb;
+
+	goto retry;
+}
+EXPORT_SYMBOL(drm_atomic_helper_update_plane);
+
+/**
+ * drm_atomic_helper_disable_plane - Helper for primary plane disable using * atomic
+ * @plane: plane to disable
+ *
+ * Provides a default plane disable handler using the atomic driver interface.
+ *
+ * RETURNS:
+ * Zero on success, error code on failure
+ */
+int drm_atomic_helper_disable_plane(struct drm_plane *plane)
+{
+	struct drm_atomic_state *state;
+	struct drm_plane_state *plane_state;
+	int ret = 0;
+
+	/*
+	 * FIXME: Without plane->crtc set we can't get at the implicit legacy
+	 * acquire context. The real fix will be to wire the acquire ctx through
+	 * everywhere we need it, but meanwhile prevent chaos by just skipping
+	 * this noop. The critical case is the cursor ioctls which a) only grab
+	 * crtc/cursor-plane locks (so we need the crtc to get at the right
+	 * acquire context) and b) can try to disable the plane multiple times.
+	 */
+	if (!plane->crtc)
+		return 0;
+
+	state = drm_atomic_state_alloc(plane->dev);
+	if (!state)
+		return -ENOMEM;
+
+	state->acquire_ctx = drm_modeset_legacy_acquire_ctx(plane->crtc);
+retry:
+	plane_state = drm_atomic_get_plane_state(state, plane);
+	if (IS_ERR(plane_state)) {
+		ret = PTR_ERR(plane_state);
+		goto fail;
+	}
+
+	ret = drm_atomic_set_crtc_for_plane(state, plane, NULL);
+	if (ret != 0)
+		goto fail;
+	drm_atomic_set_fb_for_plane(plane_state, NULL);
+	plane_state->crtc_x = 0;
+	plane_state->crtc_y = 0;
+	plane_state->crtc_h = 0;
+	plane_state->crtc_w = 0;
+	plane_state->src_x = 0;
+	plane_state->src_y = 0;
+	plane_state->src_h = 0;
+	plane_state->src_w = 0;
+
+	ret = drm_atomic_commit(state);
+	if (ret != 0)
+		goto fail;
+
+	/* Driver takes ownership of state on successful commit. */
+	return 0;
+fail:
+	if (ret == -EDEADLK)
+		goto backoff;
+
+	drm_atomic_state_free(state);
+
+	return ret;
+backoff:
+	drm_atomic_state_clear(state);
+	drm_atomic_legacy_backoff(state);
+
+	/*
+	 * Someone might have exchanged the framebuffer while we dropped locks
+	 * in the backoff code. We need to fix up the fb refcount tracking the
+	 * core does for us.
+	 */
+	plane->old_fb = plane->fb;
+
+	goto retry;
+}
+EXPORT_SYMBOL(drm_atomic_helper_disable_plane);
+
+static int update_output_state(struct drm_atomic_state *state,
+			       struct drm_mode_set *set)
+{
+	struct drm_device *dev = set->crtc->dev;
+	struct drm_connector_state *conn_state;
+	int ncrtcs = state->dev->mode_config.num_crtc;
+	int ret, i, j;
+
+	ret = drm_modeset_lock(&dev->mode_config.connection_mutex,
+			       state->acquire_ctx);
+	if (ret)
+		return ret;
+
+	/* First grab all affected connector/crtc states. */
+	for (i = 0; i < set->num_connectors; i++) {
+		conn_state = drm_atomic_get_connector_state(state,
+							    set->connectors[i]);
+		if (IS_ERR(conn_state))
+			return PTR_ERR(conn_state);
+	}
+
+	for (i = 0; i < ncrtcs; i++) {
+		struct drm_crtc *crtc = state->crtcs[i];
+
+		if (!crtc)
+			continue;
+
+		ret = drm_atomic_add_affected_connectors(state, crtc);
+		if (ret)
+			return ret;
+	}
+
+	/* Then recompute connector->crtc links and crtc enabling state. */
+	for (i = 0; i < state->num_connector; i++) {
+		struct drm_connector *connector;
+
+		connector = state->connectors[i];
+		conn_state = state->connector_states[i];
+
+		if (!connector)
+			continue;
+
+		if (conn_state->crtc == set->crtc) {
+			ret = drm_atomic_set_crtc_for_connector(conn_state,
+								NULL);
+			if (ret)
+				return ret;
+		}
+
+		for (j = 0; j < set->num_connectors; j++) {
+			if (set->connectors[j] == connector) {
+				ret = drm_atomic_set_crtc_for_connector(conn_state,
+									set->crtc);
+				if (ret)
+					return ret;
+				break;
+			}
+		}
+	}
+
+	for (i = 0; i < ncrtcs; i++) {
+		struct drm_crtc *crtc = state->crtcs[i];
+		struct drm_crtc_state *crtc_state = state->crtc_states[i];
+
+		if (!crtc)
+			continue;
+
+		/* Don't update ->enable for the CRTC in the set_config request,
+		 * since a mismatch would indicate a bug in the upper layers.
+		 * The actual modeset code later on will catch any
+		 * inconsistencies here. */
+		if (crtc == set->crtc)
+			continue;
+
+		crtc_state->enable =
+			drm_atomic_connectors_for_crtc(state, crtc);
+	}
+
+	return 0;
+}
+
+/**
+ * drm_atomic_helper_set_config - set a new config from userspace
+ * @set: mode set configuration
+ *
+ * Provides a default crtc set_config handler using the atomic driver interface.
+ *
+ * Returns:
+ * Returns 0 on success, negative errno numbers on failure.
+ */
+int drm_atomic_helper_set_config(struct drm_mode_set *set)
+{
+	struct drm_atomic_state *state;
+	struct drm_crtc *crtc = set->crtc;
+	struct drm_crtc_state *crtc_state;
+	struct drm_plane_state *primary_state;
+	int ret = 0;
+
+	state = drm_atomic_state_alloc(crtc->dev);
+	if (!state)
+		return -ENOMEM;
+
+	state->acquire_ctx = drm_modeset_legacy_acquire_ctx(crtc);
+retry:
+	crtc_state = drm_atomic_get_crtc_state(state, crtc);
+	if (IS_ERR(crtc_state)) {
+		ret = PTR_ERR(crtc_state);
+		goto fail;
+	}
+
+	primary_state = drm_atomic_get_plane_state(state, crtc->primary);
+	if (IS_ERR(primary_state)) {
+		ret = PTR_ERR(primary_state);
+		goto fail;
+	}
+
+	if (!set->mode) {
+		WARN_ON(set->fb);
+		WARN_ON(set->num_connectors);
+
+		crtc_state->enable = false;
+
+		ret = drm_atomic_set_crtc_for_plane(state, crtc->primary, NULL);
+		if (ret != 0)
+			goto fail;
+
+		drm_atomic_set_fb_for_plane(primary_state, NULL);
+
+		goto commit;
+	}
+
+	WARN_ON(!set->fb);
+	WARN_ON(!set->num_connectors);
+
+	crtc_state->enable = true;
+	drm_mode_copy(&crtc_state->mode, set->mode);
+
+	ret = drm_atomic_set_crtc_for_plane(state, crtc->primary, crtc);
+	if (ret != 0)
+		goto fail;
+	drm_atomic_set_fb_for_plane(primary_state, set->fb);
+	primary_state->crtc_x = 0;
+	primary_state->crtc_y = 0;
+	primary_state->crtc_h = set->mode->vdisplay;
+	primary_state->crtc_w = set->mode->hdisplay;
+	primary_state->src_x = set->x << 16;
+	primary_state->src_y = set->y << 16;
+	primary_state->src_h = set->mode->vdisplay << 16;
+	primary_state->src_w = set->mode->hdisplay << 16;
+
+commit:
+	ret = update_output_state(state, set);
+	if (ret)
+		goto fail;
+
+	ret = drm_atomic_commit(state);
+	if (ret != 0)
+		goto fail;
+
+	/* Driver takes ownership of state on successful commit. */
+	return 0;
+fail:
+	if (ret == -EDEADLK)
+		goto backoff;
+
+	drm_atomic_state_free(state);
+
+	return ret;
+backoff:
+	drm_atomic_state_clear(state);
+	drm_atomic_legacy_backoff(state);
+
+	/*
+	 * Someone might have exchanged the framebuffer while we dropped locks
+	 * in the backoff code. We need to fix up the fb refcount tracking the
+	 * core does for us.
+	 */
+	crtc->primary->old_fb = crtc->primary->fb;
+
+	goto retry;
+}
+EXPORT_SYMBOL(drm_atomic_helper_set_config);
+
+/**
+ * drm_atomic_helper_crtc_set_property - helper for crtc prorties
+ * @crtc: DRM crtc
+ * @property: DRM property
+ * @val: value of property
+ *
+ * Provides a default plane disablle handler using the atomic driver interface.
+ *
+ * RETURNS:
+ * Zero on success, error code on failure
+ */
+int
+drm_atomic_helper_crtc_set_property(struct drm_crtc *crtc,
+				    struct drm_property *property,
+				    uint64_t val)
+{
+	struct drm_atomic_state *state;
+	struct drm_crtc_state *crtc_state;
+	int ret = 0;
+
+	state = drm_atomic_state_alloc(crtc->dev);
+	if (!state)
+		return -ENOMEM;
+
+	/* ->set_property is always called with all locks held. */
+	state->acquire_ctx = crtc->dev->mode_config.acquire_ctx;
+retry:
+	crtc_state = drm_atomic_get_crtc_state(state, crtc);
+	if (IS_ERR(crtc_state)) {
+		ret = PTR_ERR(crtc_state);
+		goto fail;
+	}
+
+	ret = crtc->funcs->atomic_set_property(crtc, crtc_state,
+					       property, val);
+	if (ret)
+		goto fail;
+
+	ret = drm_atomic_commit(state);
+	if (ret != 0)
+		goto fail;
+
+	/* Driver takes ownership of state on successful commit. */
+	return 0;
+fail:
+	if (ret == -EDEADLK)
+		goto backoff;
+
+	drm_atomic_state_free(state);
+
+	return ret;
+backoff:
+	drm_atomic_state_clear(state);
+	drm_atomic_legacy_backoff(state);
+
+	goto retry;
+}
+EXPORT_SYMBOL(drm_atomic_helper_crtc_set_property);
+
+/**
+ * drm_atomic_helper_plane_set_property - helper for plane prorties
+ * @plane: DRM plane
+ * @property: DRM property
+ * @val: value of property
+ *
+ * Provides a default plane disable handler using the atomic driver interface.
+ *
+ * RETURNS:
+ * Zero on success, error code on failure
+ */
+int
+drm_atomic_helper_plane_set_property(struct drm_plane *plane,
+				    struct drm_property *property,
+				    uint64_t val)
+{
+	struct drm_atomic_state *state;
+	struct drm_plane_state *plane_state;
+	int ret = 0;
+
+	state = drm_atomic_state_alloc(plane->dev);
+	if (!state)
+		return -ENOMEM;
+
+	/* ->set_property is always called with all locks held. */
+	state->acquire_ctx = plane->dev->mode_config.acquire_ctx;
+retry:
+	plane_state = drm_atomic_get_plane_state(state, plane);
+	if (IS_ERR(plane_state)) {
+		ret = PTR_ERR(plane_state);
+		goto fail;
+	}
+
+	ret = plane->funcs->atomic_set_property(plane, plane_state,
+					       property, val);
+	if (ret)
+		goto fail;
+
+	ret = drm_atomic_commit(state);
+	if (ret != 0)
+		goto fail;
+
+	/* Driver takes ownership of state on successful commit. */
+	return 0;
+fail:
+	if (ret == -EDEADLK)
+		goto backoff;
+
+	drm_atomic_state_free(state);
+
+	return ret;
+backoff:
+	drm_atomic_state_clear(state);
+	drm_atomic_legacy_backoff(state);
+
+	goto retry;
+}
+EXPORT_SYMBOL(drm_atomic_helper_plane_set_property);
+
+/**
+ * drm_atomic_helper_connector_set_property - helper for connector prorties
+ * @connector: DRM connector
+ * @property: DRM property
+ * @val: value of property
+ *
+ * Provides a default plane disablle handler using the atomic driver interface.
+ *
+ * RETURNS:
+ * Zero on success, error code on failure
+ */
+int
+drm_atomic_helper_connector_set_property(struct drm_connector *connector,
+				    struct drm_property *property,
+				    uint64_t val)
+{
+	struct drm_atomic_state *state;
+	struct drm_connector_state *connector_state;
+	int ret = 0;
+
+	state = drm_atomic_state_alloc(connector->dev);
+	if (!state)
+		return -ENOMEM;
+
+	/* ->set_property is always called with all locks held. */
+	state->acquire_ctx = connector->dev->mode_config.acquire_ctx;
+retry:
+	connector_state = drm_atomic_get_connector_state(state, connector);
+	if (IS_ERR(connector_state)) {
+		ret = PTR_ERR(connector_state);
+		goto fail;
+	}
+
+	ret = connector->funcs->atomic_set_property(connector, connector_state,
+					       property, val);
+	if (ret)
+		goto fail;
+
+	ret = drm_atomic_commit(state);
+	if (ret != 0)
+		goto fail;
+
+	/* Driver takes ownership of state on successful commit. */
+	return 0;
+fail:
+	if (ret == -EDEADLK)
+		goto backoff;
+
+	drm_atomic_state_free(state);
+
+	return ret;
+backoff:
+	drm_atomic_state_clear(state);
+	drm_atomic_legacy_backoff(state);
+
+	goto retry;
+}
+EXPORT_SYMBOL(drm_atomic_helper_connector_set_property);
+
+/**
+ * drm_atomic_helper_page_flip - execute a legacy page flip
+ * @crtc: DRM crtc
+ * @fb: DRM framebuffer
+ * @event: optional DRM event to signal upon completion
+ * @flags: flip flags for non-vblank sync'ed updates
+ *
+ * Provides a default page flip implementation using the atomic driver interface.
+ *
+ * Note that for now so called async page flips (i.e. updates which are not
+ * synchronized to vblank) are not supported, since the atomic interfaces have
+ * no provisions for this yet.
+ *
+ * Returns:
+ * Returns 0 on success, negative errno numbers on failure.
+ */
+int drm_atomic_helper_page_flip(struct drm_crtc *crtc,
+				struct drm_framebuffer *fb,
+				struct drm_pending_vblank_event *event,
+				uint32_t flags)
+{
+	struct drm_plane *plane = crtc->primary;
+	struct drm_atomic_state *state;
+	struct drm_plane_state *plane_state;
+	struct drm_crtc_state *crtc_state;
+	int ret = 0;
+
+	if (flags & DRM_MODE_PAGE_FLIP_ASYNC)
+		return -EINVAL;
+
+	state = drm_atomic_state_alloc(plane->dev);
+	if (!state)
+		return -ENOMEM;
+
+	state->acquire_ctx = drm_modeset_legacy_acquire_ctx(crtc);
+retry:
+	crtc_state = drm_atomic_get_crtc_state(state, crtc);
+	if (IS_ERR(crtc_state)) {
+		ret = PTR_ERR(crtc_state);
+		goto fail;
+	}
+	crtc_state->event = event;
+
+	plane_state = drm_atomic_get_plane_state(state, plane);
+	if (IS_ERR(plane_state)) {
+		ret = PTR_ERR(plane_state);
+		goto fail;
+	}
+
+	ret = drm_atomic_set_crtc_for_plane(state, plane, crtc);
+	if (ret != 0)
+		goto fail;
+	drm_atomic_set_fb_for_plane(plane_state, fb);
+
+	ret = drm_atomic_async_commit(state);
+	if (ret != 0)
+		goto fail;
+
+	/* TODO: ->page_flip is the only driver callback where the core
+	 * doesn't update plane->fb. For now patch it up here. */
+	plane->fb = plane->state->fb;
+
+	/* Driver takes ownership of state on successful async commit. */
+	return 0;
+fail:
+	if (ret == -EDEADLK)
+		goto backoff;
+
+	drm_atomic_state_free(state);
+
+	return ret;
+backoff:
+	drm_atomic_state_clear(state);
+	drm_atomic_legacy_backoff(state);
+
+	/*
+	 * Someone might have exchanged the framebuffer while we dropped locks
+	 * in the backoff code. We need to fix up the fb refcount tracking the
+	 * core does for us.
+	 */
+	plane->old_fb = plane->fb;
+
+	goto retry;
+}
+EXPORT_SYMBOL(drm_atomic_helper_page_flip);
+
+/**
+ * DOC: atomic state reset and initialization
+ *
+ * Both the drm core and the atomic helpers assume that there is always the full
+ * and correct atomic software state for all connectors, CRTCs and planes
+ * available. Which is a bit a problem on driver load and also after system
+ * suspend. One way to solve this is to have a hardware state read-out
+ * infrastructure which reconstructs the full software state (e.g. the i915
+ * driver).
+ *
+ * The simpler solution is to just reset the software state to everything off,
+ * which is easiest to do by calling drm_mode_config_reset(). To facilitate this
+ * the atomic helpers provide default reset implementations for all hooks.
+ */
+
+/**
+ * drm_atomic_helper_crtc_reset - default ->reset hook for CRTCs
+ * @crtc: drm CRTC
+ *
+ * Resets the atomic state for @crtc by freeing the state pointer (which might
+ * be NULL, e.g. at driver load time) and allocating a new empty state object.
+ */
+void drm_atomic_helper_crtc_reset(struct drm_crtc *crtc)
+{
+	kfree(crtc->state);
+	crtc->state = kzalloc(sizeof(*crtc->state), GFP_KERNEL);
+}
+EXPORT_SYMBOL(drm_atomic_helper_crtc_reset);
+
+/**
+ * drm_atomic_helper_crtc_duplicate_state - default state duplicate hook
+ * @crtc: drm CRTC
+ *
+ * Default CRTC state duplicate hook for drivers which don't have their own
+ * subclassed CRTC state structure.
+ */
+struct drm_crtc_state *
+drm_atomic_helper_crtc_duplicate_state(struct drm_crtc *crtc)
+{
+	struct drm_crtc_state *state;
+
+	if (WARN_ON(!crtc->state))
+		return NULL;
+
+	state = kmemdup(crtc->state, sizeof(*crtc->state), GFP_KERNEL);
+
+	if (state) {
+		state->mode_changed = false;
+		state->planes_changed = false;
+		state->event = NULL;
+	}
+
+	return state;
+}
+EXPORT_SYMBOL(drm_atomic_helper_crtc_duplicate_state);
+
+/**
+ * drm_atomic_helper_crtc_destroy_state - default state destroy hook
+ * @crtc: drm CRTC
+ * @state: CRTC state object to release
+ *
+ * Default CRTC state destroy hook for drivers which don't have their own
+ * subclassed CRTC state structure.
+ */
+void drm_atomic_helper_crtc_destroy_state(struct drm_crtc *crtc,
+					  struct drm_crtc_state *state)
+{
+	kfree(state);
+}
+EXPORT_SYMBOL(drm_atomic_helper_crtc_destroy_state);
+
+/**
+ * drm_atomic_helper_plane_reset - default ->reset hook for planes
+ * @plane: drm plane
+ *
+ * Resets the atomic state for @plane by freeing the state pointer (which might
+ * be NULL, e.g. at driver load time) and allocating a new empty state object.
+ */
+void drm_atomic_helper_plane_reset(struct drm_plane *plane)
+{
+	if (plane->state && plane->state->fb)
+		drm_framebuffer_unreference(plane->state->fb);
+
+	kfree(plane->state);
+	plane->state = kzalloc(sizeof(*plane->state), GFP_KERNEL);
+}
+EXPORT_SYMBOL(drm_atomic_helper_plane_reset);
+
+/**
+ * drm_atomic_helper_plane_duplicate_state - default state duplicate hook
+ * @plane: drm plane
+ *
+ * Default plane state duplicate hook for drivers which don't have their own
+ * subclassed plane state structure.
+ */
+struct drm_plane_state *
+drm_atomic_helper_plane_duplicate_state(struct drm_plane *plane)
+{
+	struct drm_plane_state *state;
+
+	if (WARN_ON(!plane->state))
+		return NULL;
+
+	state = kmemdup(plane->state, sizeof(*plane->state), GFP_KERNEL);
+
+	if (state && state->fb)
+		drm_framebuffer_reference(state->fb);
+
+	return state;
+}
+EXPORT_SYMBOL(drm_atomic_helper_plane_duplicate_state);
+
+/**
+ * drm_atomic_helper_plane_destroy_state - default state destroy hook
+ * @plane: drm plane
+ * @state: plane state object to release
+ *
+ * Default plane state destroy hook for drivers which don't have their own
+ * subclassed plane state structure.
+ */
+void drm_atomic_helper_plane_destroy_state(struct drm_plane *plane,
+					   struct drm_plane_state *state)
+{
+	if (state->fb)
+		drm_framebuffer_unreference(state->fb);
+
+	kfree(state);
+}
+EXPORT_SYMBOL(drm_atomic_helper_plane_destroy_state);
+
+/**
+ * drm_atomic_helper_connector_reset - default ->reset hook for connectors
+ * @connector: drm connector
+ *
+ * Resets the atomic state for @connector by freeing the state pointer (which
+ * might be NULL, e.g. at driver load time) and allocating a new empty state
+ * object.
+ */
+void drm_atomic_helper_connector_reset(struct drm_connector *connector)
+{
+	kfree(connector->state);
+	connector->state = kzalloc(sizeof(*connector->state), GFP_KERNEL);
+}
+EXPORT_SYMBOL(drm_atomic_helper_connector_reset);
+
+/**
+ * drm_atomic_helper_connector_duplicate_state - default state duplicate hook
+ * @connector: drm connector
+ *
+ * Default connector state duplicate hook for drivers which don't have their own
+ * subclassed connector state structure.
+ */
+struct drm_connector_state *
+drm_atomic_helper_connector_duplicate_state(struct drm_connector *connector)
+{
+	if (WARN_ON(!connector->state))
+		return NULL;
+
+	return kmemdup(connector->state, sizeof(*connector->state), GFP_KERNEL);
+}
+EXPORT_SYMBOL(drm_atomic_helper_connector_duplicate_state);
+
+/**
+ * drm_atomic_helper_connector_destroy_state - default state destroy hook
+ * @connector: drm connector
+ * @state: connector state object to release
+ *
+ * Default connector state destroy hook for drivers which don't have their own
+ * subclassed connector state structure.
+ */
+void drm_atomic_helper_connector_destroy_state(struct drm_connector *connector,
+					  struct drm_connector_state *state)
+{
+	kfree(state);
+}
+EXPORT_SYMBOL(drm_atomic_helper_connector_destroy_state);
diff --git a/drivers/gpu/drm/drm_crtc.c b/drivers/gpu/drm/drm_crtc.c
index e79c8d3700d8..5213da499d39 100644
--- a/drivers/gpu/drm/drm_crtc.c
+++ b/drivers/gpu/drm/drm_crtc.c
@@ -683,7 +683,7 @@ int drm_crtc_init_with_planes(struct drm_device *dev, struct drm_crtc *crtc,
 	drm_modeset_lock_init(&crtc->mutex);
 	ret = drm_mode_object_get(dev, &crtc->base, DRM_MODE_OBJECT_CRTC);
 	if (ret)
-		goto out;
+		return ret;
 
 	crtc->base.properties = &crtc->properties;
 
@@ -697,9 +697,7 @@ int drm_crtc_init_with_planes(struct drm_device *dev, struct drm_crtc *crtc,
 	if (cursor)
 		cursor->possible_crtcs = 1 << drm_crtc_index(crtc);
 
- out:
-
-	return ret;
+	return 0;
 }
 EXPORT_SYMBOL(drm_crtc_init_with_planes);
 
@@ -723,6 +721,12 @@ void drm_crtc_cleanup(struct drm_crtc *crtc)
 	drm_mode_object_put(dev, &crtc->base);
 	list_del(&crtc->head);
 	dev->mode_config.num_crtc--;
+
+	WARN_ON(crtc->state && !crtc->funcs->atomic_destroy_state);
+	if (crtc->state && crtc->funcs->atomic_destroy_state)
+		crtc->funcs->atomic_destroy_state(crtc, crtc->state);
+
+	memset(crtc, 0, sizeof(*crtc));
 }
 EXPORT_SYMBOL(drm_crtc_cleanup);
 
@@ -766,7 +770,6 @@ static void drm_mode_remove(struct drm_connector *connector,
 /**
  * drm_connector_get_cmdline_mode - reads the user's cmdline mode
  * @connector: connector to quwery
- * @mode: returned mode
  *
  * The kernel supports per-connector configration of its consoles through
  * use of the video= parameter. This function parses that option and
@@ -870,6 +873,8 @@ int drm_connector_init(struct drm_device *dev,
 
 	drm_connector_get_cmdline_mode(connector);
 
+	/* We should add connectors at the end to avoid upsetting the connector
+	 * index too much. */
 	list_add_tail(&connector->head, &dev->mode_config.connector_list);
 	dev->mode_config.num_connector++;
 
@@ -905,6 +910,11 @@ void drm_connector_cleanup(struct drm_connector *connector)
 	struct drm_device *dev = connector->dev;
 	struct drm_display_mode *mode, *t;
 
+	if (connector->tile_group) {
+		drm_mode_put_tile_group(dev, connector->tile_group);
+		connector->tile_group = NULL;
+	}
+
 	list_for_each_entry_safe(mode, t, &connector->probed_modes, head)
 		drm_mode_remove(connector, mode);
 
@@ -919,6 +929,13 @@ void drm_connector_cleanup(struct drm_connector *connector)
 	connector->name = NULL;
 	list_del(&connector->head);
 	dev->mode_config.num_connector--;
+
+	WARN_ON(connector->state && !connector->funcs->atomic_destroy_state);
+	if (connector->state && connector->funcs->atomic_destroy_state)
+		connector->funcs->atomic_destroy_state(connector,
+						       connector->state);
+
+	memset(connector, 0, sizeof(*connector));
 }
 EXPORT_SYMBOL(drm_connector_cleanup);
 
@@ -933,6 +950,9 @@ unsigned int drm_connector_index(struct drm_connector *connector)
 {
 	unsigned int index = 0;
 	struct drm_connector *tmp;
+	struct drm_mode_config *config = &connector->dev->mode_config;
+
+	WARN_ON(!drm_modeset_is_locked(&config->connection_mutex));
 
 	list_for_each_entry(tmp, &connector->dev->mode_config.connector_list, head) {
 		if (tmp == connector)
@@ -1057,6 +1077,8 @@ void drm_bridge_cleanup(struct drm_bridge *bridge)
 	list_del(&bridge->head);
 	dev->mode_config.num_bridge--;
 	drm_modeset_unlock_all(dev);
+
+	memset(bridge, 0, sizeof(*bridge));
 }
 EXPORT_SYMBOL(drm_bridge_cleanup);
 
@@ -1123,10 +1145,11 @@ void drm_encoder_cleanup(struct drm_encoder *encoder)
 	drm_modeset_lock_all(dev);
 	drm_mode_object_put(dev, &encoder->base);
 	kfree(encoder->name);
-	encoder->name = NULL;
 	list_del(&encoder->head);
 	dev->mode_config.num_encoder--;
 	drm_modeset_unlock_all(dev);
+
+	memset(encoder, 0, sizeof(*encoder));
 }
 EXPORT_SYMBOL(drm_encoder_cleanup);
 
@@ -1153,11 +1176,11 @@ int drm_universal_plane_init(struct drm_device *dev, struct drm_plane *plane,
 {
 	int ret;
 
-	drm_modeset_lock_all(dev);
-
 	ret = drm_mode_object_get(dev, &plane->base, DRM_MODE_OBJECT_PLANE);
 	if (ret)
-		goto out;
+		return ret;
+
+	drm_modeset_lock_init(&plane->mutex);
 
 	plane->base.properties = &plane->properties;
 	plane->dev = dev;
@@ -1167,8 +1190,7 @@ int drm_universal_plane_init(struct drm_device *dev, struct drm_plane *plane,
 	if (!plane->format_types) {
 		DRM_DEBUG_KMS("out of memory when allocating plane\n");
 		drm_mode_object_put(dev, &plane->base);
-		ret = -ENOMEM;
-		goto out;
+		return -ENOMEM;
 	}
 
 	memcpy(plane->format_types, formats, format_count * sizeof(uint32_t));
@@ -1185,10 +1207,7 @@ int drm_universal_plane_init(struct drm_device *dev, struct drm_plane *plane,
 				   dev->mode_config.plane_type_property,
 				   plane->type);
 
- out:
-	drm_modeset_unlock_all(dev);
-
-	return ret;
+	return 0;
 }
 EXPORT_SYMBOL(drm_universal_plane_init);
 
@@ -1246,6 +1265,12 @@ void drm_plane_cleanup(struct drm_plane *plane)
 	if (plane->type == DRM_PLANE_TYPE_OVERLAY)
 		dev->mode_config.num_overlay_plane--;
 	drm_modeset_unlock_all(dev);
+
+	WARN_ON(plane->state && !plane->funcs->atomic_destroy_state);
+	if (plane->state && plane->funcs->atomic_destroy_state)
+		plane->funcs->atomic_destroy_state(plane, plane->state);
+
+	memset(plane, 0, sizeof(*plane));
 }
 EXPORT_SYMBOL(drm_plane_cleanup);
 
@@ -1328,6 +1353,11 @@ static int drm_mode_create_standard_connector_properties(struct drm_device *dev)
 				       "PATH", 0);
 	dev->mode_config.path_property = dev_path;
 
+	dev->mode_config.tile_property = drm_property_create(dev,
+							     DRM_MODE_PROP_BLOB |
+							     DRM_MODE_PROP_IMMUTABLE,
+							     "TILE", 0);
+
 	return 0;
 }
 
@@ -1388,12 +1418,13 @@ EXPORT_SYMBOL(drm_mode_create_dvi_i_properties);
  * responsible for allocating a list of format names and passing them to
  * this routine.
  */
-int drm_mode_create_tv_properties(struct drm_device *dev, int num_modes,
+int drm_mode_create_tv_properties(struct drm_device *dev,
+				  unsigned int num_modes,
 				  char *modes[])
 {
 	struct drm_property *tv_selector;
 	struct drm_property *tv_subconnector;
-	int i;
+	unsigned int i;
 
 	if (dev->mode_config.tv_select_subconnector_property)
 		return 0;
@@ -1491,7 +1522,7 @@ EXPORT_SYMBOL(drm_mode_create_scaling_mode_property);
  * connectors.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_create_aspect_ratio_property(struct drm_device *dev)
 {
@@ -1535,6 +1566,30 @@ int drm_mode_create_dirty_info_property(struct drm_device *dev)
 }
 EXPORT_SYMBOL(drm_mode_create_dirty_info_property);
 
+/**
+ * drm_mode_create_suggested_offset_properties - create suggests offset properties
+ * @dev: DRM device
+ *
+ * Create the the suggested x/y offset property for connectors.
+ */
+int drm_mode_create_suggested_offset_properties(struct drm_device *dev)
+{
+	if (dev->mode_config.suggested_x_property && dev->mode_config.suggested_y_property)
+		return 0;
+
+	dev->mode_config.suggested_x_property =
+		drm_property_create_range(dev, DRM_MODE_PROP_IMMUTABLE, "suggested X", 0, 0xffffffff);
+
+	dev->mode_config.suggested_y_property =
+		drm_property_create_range(dev, DRM_MODE_PROP_IMMUTABLE, "suggested Y", 0, 0xffffffff);
+
+	if (dev->mode_config.suggested_x_property == NULL ||
+	    dev->mode_config.suggested_y_property == NULL)
+		return -ENOMEM;
+	return 0;
+}
+EXPORT_SYMBOL(drm_mode_create_suggested_offset_properties);
+
 static int drm_mode_group_init(struct drm_device *dev, struct drm_mode_group *group)
 {
 	uint32_t total_objects = 0;
@@ -1651,7 +1706,7 @@ static void drm_crtc_convert_to_umode(struct drm_mode_modeinfo *out,
  * the caller.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 static int drm_crtc_convert_umode(struct drm_display_mode *out,
 				  const struct drm_mode_modeinfo *in)
@@ -1694,7 +1749,7 @@ static int drm_crtc_convert_umode(struct drm_display_mode *out,
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_getresources(struct drm_device *dev, void *data,
 			  struct drm_file *file_priv)
@@ -1745,7 +1800,9 @@ int drm_mode_getresources(struct drm_device *dev, void *data,
 	card_res->count_fbs = fb_count;
 	mutex_unlock(&file_priv->fbs_lock);
 
-	drm_modeset_lock_all(dev);
+	/* mode_config.mutex protects the connector list against e.g. DP MST
+	 * connector hot-adding. CRTC/Plane lists are invariant. */
+	mutex_lock(&dev->mode_config.mutex);
 	if (!drm_is_primary_client(file_priv)) {
 
 		mode_group = NULL;
@@ -1865,7 +1922,7 @@ int drm_mode_getresources(struct drm_device *dev, void *data,
 		  card_res->count_connectors, card_res->count_encoders);
 
 out:
-	drm_modeset_unlock_all(dev);
+	mutex_unlock(&dev->mode_config.mutex);
 	return ret;
 }
 
@@ -1880,26 +1937,22 @@ out:
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_getcrtc(struct drm_device *dev,
 		     void *data, struct drm_file *file_priv)
 {
 	struct drm_mode_crtc *crtc_resp = data;
 	struct drm_crtc *crtc;
-	int ret = 0;
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return -EINVAL;
 
-	drm_modeset_lock_all(dev);
-
 	crtc = drm_crtc_find(dev, crtc_resp->crtc_id);
-	if (!crtc) {
-		ret = -ENOENT;
-		goto out;
-	}
+	if (!crtc)
+		return -ENOENT;
 
+	drm_modeset_lock_crtc(crtc, crtc->primary);
 	crtc_resp->x = crtc->x;
 	crtc_resp->y = crtc->y;
 	crtc_resp->gamma_size = crtc->gamma_size;
@@ -1916,10 +1969,9 @@ int drm_mode_getcrtc(struct drm_device *dev,
 	} else {
 		crtc_resp->mode_valid = 0;
 	}
+	drm_modeset_unlock_crtc(crtc);
 
-out:
-	drm_modeset_unlock_all(dev);
-	return ret;
+	return 0;
 }
 
 static bool drm_mode_expose_to_userspace(const struct drm_display_mode *mode,
@@ -1935,6 +1987,15 @@ static bool drm_mode_expose_to_userspace(const struct drm_display_mode *mode,
 	return true;
 }
 
+static struct drm_encoder *drm_connector_get_encoder(struct drm_connector *connector)
+{
+	/* For atomic drivers only state objects are synchronously updated and
+	 * protected by modeset locks, so check those first. */
+	if (connector->state)
+		return connector->state->best_encoder;
+	return connector->encoder;
+}
+
 /**
  * drm_mode_getconnector - get connector configuration
  * @dev: drm device for the ioctl
@@ -1946,13 +2007,14 @@ static bool drm_mode_expose_to_userspace(const struct drm_display_mode *mode,
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_getconnector(struct drm_device *dev, void *data,
 			  struct drm_file *file_priv)
 {
 	struct drm_mode_get_connector *out_resp = data;
 	struct drm_connector *connector;
+	struct drm_encoder *encoder;
 	struct drm_display_mode *mode;
 	int mode_count = 0;
 	int props_count = 0;
@@ -2008,8 +2070,10 @@ int drm_mode_getconnector(struct drm_device *dev, void *data,
 	out_resp->subpixel = connector->display_info.subpixel_order;
 	out_resp->connection = connector->status;
 	drm_modeset_lock(&dev->mode_config.connection_mutex, NULL);
-	if (connector->encoder)
-		out_resp->encoder_id = connector->encoder->base.id;
+
+	encoder = drm_connector_get_encoder(connector);
+	if (encoder)
+		out_resp->encoder_id = encoder->base.id;
 	else
 		out_resp->encoder_id = 0;
 	drm_modeset_unlock(&dev->mode_config.connection_mutex);
@@ -2079,6 +2143,33 @@ out:
 	return ret;
 }
 
+static struct drm_crtc *drm_encoder_get_crtc(struct drm_encoder *encoder)
+{
+	struct drm_connector *connector;
+	struct drm_device *dev = encoder->dev;
+	bool uses_atomic = false;
+
+	/* For atomic drivers only state objects are synchronously updated and
+	 * protected by modeset locks, so check those first. */
+	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
+		if (!connector->state)
+			continue;
+
+		uses_atomic = true;
+
+		if (connector->state->best_encoder != encoder)
+			continue;
+
+		return connector->state->crtc;
+	}
+
+	/* Don't return stale data (e.g. pending async disable). */
+	if (uses_atomic)
+		return NULL;
+
+	return encoder->crtc;
+}
+
 /**
  * drm_mode_getencoder - get encoder configuration
  * @dev: drm device for the ioctl
@@ -2090,37 +2181,38 @@ out:
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_getencoder(struct drm_device *dev, void *data,
 			struct drm_file *file_priv)
 {
 	struct drm_mode_get_encoder *enc_resp = data;
 	struct drm_encoder *encoder;
-	int ret = 0;
+	struct drm_crtc *crtc;
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return -EINVAL;
 
-	drm_modeset_lock_all(dev);
 	encoder = drm_encoder_find(dev, enc_resp->encoder_id);
-	if (!encoder) {
-		ret = -ENOENT;
-		goto out;
-	}
+	if (!encoder)
+		return -ENOENT;
 
-	if (encoder->crtc)
+	drm_modeset_lock(&dev->mode_config.connection_mutex, NULL);
+	crtc = drm_encoder_get_crtc(encoder);
+	if (crtc)
+		enc_resp->crtc_id = crtc->base.id;
+	else if (encoder->crtc)
 		enc_resp->crtc_id = encoder->crtc->base.id;
 	else
 		enc_resp->crtc_id = 0;
+	drm_modeset_unlock(&dev->mode_config.connection_mutex);
+
 	enc_resp->encoder_type = encoder->encoder_type;
 	enc_resp->encoder_id = encoder->base.id;
 	enc_resp->possible_crtcs = encoder->possible_crtcs;
 	enc_resp->possible_clones = encoder->possible_clones;
 
-out:
-	drm_modeset_unlock_all(dev);
-	return ret;
+	return 0;
 }
 
 /**
@@ -2134,7 +2226,7 @@ out:
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_getplane_res(struct drm_device *dev, void *data,
 			  struct drm_file *file_priv)
@@ -2143,13 +2235,12 @@ int drm_mode_getplane_res(struct drm_device *dev, void *data,
 	struct drm_mode_config *config;
 	struct drm_plane *plane;
 	uint32_t __user *plane_ptr;
-	int copied = 0, ret = 0;
+	int copied = 0;
 	unsigned num_planes;
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return -EINVAL;
 
-	drm_modeset_lock_all(dev);
 	config = &dev->mode_config;
 
 	if (file_priv->universal_planes)
@@ -2165,6 +2256,7 @@ int drm_mode_getplane_res(struct drm_device *dev, void *data,
 	    (plane_resp->count_planes >= num_planes)) {
 		plane_ptr = (uint32_t __user *)(unsigned long)plane_resp->plane_id_ptr;
 
+		/* Plane lists are invariant, no locking needed. */
 		list_for_each_entry(plane, &config->plane_list, head) {
 			/*
 			 * Unless userspace set the 'universal planes'
@@ -2174,18 +2266,14 @@ int drm_mode_getplane_res(struct drm_device *dev, void *data,
 			    !file_priv->universal_planes)
 				continue;
 
-			if (put_user(plane->base.id, plane_ptr + copied)) {
-				ret = -EFAULT;
-				goto out;
-			}
+			if (put_user(plane->base.id, plane_ptr + copied))
+				return -EFAULT;
 			copied++;
 		}
 	}
 	plane_resp->count_planes = num_planes;
 
-out:
-	drm_modeset_unlock_all(dev);
-	return ret;
+	return 0;
 }
 
 /**
@@ -2199,7 +2287,7 @@ out:
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_getplane(struct drm_device *dev, void *data,
 		      struct drm_file *file_priv)
@@ -2207,18 +2295,15 @@ int drm_mode_getplane(struct drm_device *dev, void *data,
 	struct drm_mode_get_plane *plane_resp = data;
 	struct drm_plane *plane;
 	uint32_t __user *format_ptr;
-	int ret = 0;
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return -EINVAL;
 
-	drm_modeset_lock_all(dev);
 	plane = drm_plane_find(dev, plane_resp->plane_id);
-	if (!plane) {
-		ret = -ENOENT;
-		goto out;
-	}
+	if (!plane)
+		return -ENOENT;
 
+	drm_modeset_lock(&plane->mutex, NULL);
 	if (plane->crtc)
 		plane_resp->crtc_id = plane->crtc->base.id;
 	else
@@ -2228,6 +2313,7 @@ int drm_mode_getplane(struct drm_device *dev, void *data,
 		plane_resp->fb_id = plane->fb->base.id;
 	else
 		plane_resp->fb_id = 0;
+	drm_modeset_unlock(&plane->mutex);
 
 	plane_resp->plane_id = plane->base.id;
 	plane_resp->possible_crtcs = plane->possible_crtcs;
@@ -2243,15 +2329,12 @@ int drm_mode_getplane(struct drm_device *dev, void *data,
 		if (copy_to_user(format_ptr,
 				 plane->format_types,
 				 sizeof(uint32_t) * plane->format_count)) {
-			ret = -EFAULT;
-			goto out;
+			return -EFAULT;
 		}
 	}
 	plane_resp->count_format_types = plane->format_count;
 
-out:
-	drm_modeset_unlock_all(dev);
-	return ret;
+	return 0;
 }
 
 /*
@@ -2274,7 +2357,7 @@ static int __setplane_internal(struct drm_plane *plane,
 {
 	int ret = 0;
 	unsigned int fb_width, fb_height;
-	int i;
+	unsigned int i;
 
 	/* No fb means shut it down */
 	if (!fb) {
@@ -2378,13 +2461,12 @@ static int setplane_internal(struct drm_plane *plane,
  * valid crtc).
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_setplane(struct drm_device *dev, void *data,
 		      struct drm_file *file_priv)
 {
 	struct drm_mode_set_plane *plane_req = data;
-	struct drm_mode_object *obj;
 	struct drm_plane *plane;
 	struct drm_crtc *crtc = NULL;
 	struct drm_framebuffer *fb = NULL;
@@ -2407,14 +2489,12 @@ int drm_mode_setplane(struct drm_device *dev, void *data,
 	 * First, find the plane, crtc, and fb objects.  If not available,
 	 * we don't bother to call the driver.
 	 */
-	obj = drm_mode_object_find(dev, plane_req->plane_id,
-				   DRM_MODE_OBJECT_PLANE);
-	if (!obj) {
+	plane = drm_plane_find(dev, plane_req->plane_id);
+	if (!plane) {
 		DRM_DEBUG_KMS("Unknown plane ID %d\n",
 			      plane_req->plane_id);
 		return -ENOENT;
 	}
-	plane = obj_to_plane(obj);
 
 	if (plane_req->fb_id) {
 		fb = drm_framebuffer_lookup(dev, plane_req->fb_id);
@@ -2424,14 +2504,12 @@ int drm_mode_setplane(struct drm_device *dev, void *data,
 			return -ENOENT;
 		}
 
-		obj = drm_mode_object_find(dev, plane_req->crtc_id,
-					   DRM_MODE_OBJECT_CRTC);
-		if (!obj) {
+		crtc = drm_crtc_find(dev, plane_req->crtc_id);
+		if (!crtc) {
 			DRM_DEBUG_KMS("Unknown crtc ID %d\n",
 				      plane_req->crtc_id);
 			return -ENOENT;
 		}
-		crtc = obj_to_crtc(obj);
 	}
 
 	/*
@@ -2453,7 +2531,7 @@ int drm_mode_setplane(struct drm_device *dev, void *data,
  * interface. The only thing it adds is correct refcounting dance.
  * 
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_set_config_internal(struct drm_mode_set *set)
 {
@@ -2546,7 +2624,7 @@ EXPORT_SYMBOL(drm_crtc_check_viewport);
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_setcrtc(struct drm_device *dev, void *data,
 		     struct drm_file *file_priv)
@@ -2709,7 +2787,7 @@ out:
  * userspace wants to make use of these capabilities.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 static int drm_mode_cursor_universal(struct drm_crtc *crtc,
 				     struct drm_mode_cursor2 *req,
@@ -2810,7 +2888,7 @@ static int drm_mode_cursor_common(struct drm_device *dev,
 	 * If this crtc has a universal cursor plane, call that plane's update
 	 * handler rather than using legacy cursor handlers.
 	 */
-	drm_modeset_lock_crtc(crtc);
+	drm_modeset_lock_crtc(crtc, crtc->cursor);
 	if (crtc->cursor) {
 		ret = drm_mode_cursor_universal(crtc, req, file_priv);
 		goto out;
@@ -2857,7 +2935,7 @@ out:
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_cursor_ioctl(struct drm_device *dev,
 			  void *data, struct drm_file *file_priv)
@@ -2884,7 +2962,7 @@ int drm_mode_cursor_ioctl(struct drm_device *dev,
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_cursor2_ioctl(struct drm_device *dev,
 			   void *data, struct drm_file *file_priv)
@@ -2943,23 +3021,21 @@ EXPORT_SYMBOL(drm_mode_legacy_fb_format);
  * @file_priv: drm file for the ioctl call
  *
  * Add a new FB to the specified CRTC, given a user request. This is the
- * original addfb ioclt which only supported RGB formats.
+ * original addfb ioctl which only supported RGB formats.
  *
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_addfb(struct drm_device *dev,
 		   void *data, struct drm_file *file_priv)
 {
 	struct drm_mode_fb_cmd *or = data;
 	struct drm_mode_fb_cmd2 r = {};
-	struct drm_mode_config *config = &dev->mode_config;
-	struct drm_framebuffer *fb;
-	int ret = 0;
+	int ret;
 
-	/* Use new struct with format internally */
+	/* convert to new format and call new ioctl */
 	r.fb_id = or->fb_id;
 	r.width = or->width;
 	r.height = or->height;
@@ -2967,28 +3043,13 @@ int drm_mode_addfb(struct drm_device *dev,
 	r.pixel_format = drm_mode_legacy_fb_format(or->bpp, or->depth);
 	r.handles[0] = or->handle;
 
-	if (!drm_core_check_feature(dev, DRIVER_MODESET))
-		return -EINVAL;
-
-	if ((config->min_width > r.width) || (r.width > config->max_width))
-		return -EINVAL;
-
-	if ((config->min_height > r.height) || (r.height > config->max_height))
-		return -EINVAL;
+	ret = drm_mode_addfb2(dev, &r, file_priv);
+	if (ret)
+		return ret;
 
-	fb = dev->mode_config.funcs->fb_create(dev, file_priv, &r);
-	if (IS_ERR(fb)) {
-		DRM_DEBUG_KMS("could not create framebuffer\n");
-		return PTR_ERR(fb);
-	}
+	or->fb_id = r.fb_id;
 
-	mutex_lock(&file_priv->fbs_lock);
-	or->fb_id = fb->base.id;
-	list_add(&fb->filp_head, &file_priv->fbs);
-	DRM_DEBUG_KMS("[FB:%d]\n", fb->base.id);
-	mutex_unlock(&file_priv->fbs_lock);
-
-	return ret;
+	return 0;
 }
 
 static int format_check(const struct drm_mode_fb_cmd2 *r)
@@ -3080,7 +3141,7 @@ static int framebuffer_check(const struct drm_mode_fb_cmd2 *r)
 	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);
+		DRM_DEBUG_KMS("bad framebuffer width %u\n", r->width);
 		return -EINVAL;
 	}
 
@@ -3170,7 +3231,7 @@ static struct drm_framebuffer *add_framebuffer_internal(struct drm_device *dev,
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_addfb2(struct drm_device *dev,
 		    void *data, struct drm_file *file_priv)
@@ -3198,7 +3259,7 @@ int drm_mode_addfb2(struct drm_device *dev,
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_rmfb(struct drm_device *dev,
 		   void *data, struct drm_file *file_priv)
@@ -3252,7 +3313,7 @@ fail_lookup:
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_getfb(struct drm_device *dev,
 		   void *data, struct drm_file *file_priv)
@@ -3313,7 +3374,7 @@ int drm_mode_getfb(struct drm_device *dev,
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_dirtyfb_ioctl(struct drm_device *dev,
 			   void *data, struct drm_file *file_priv)
@@ -3393,7 +3454,7 @@ out_err1:
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 void drm_fb_release(struct drm_file *priv)
 {
@@ -3402,7 +3463,7 @@ void drm_fb_release(struct drm_file *priv)
 
 	/*
 	 * When the file gets released that means no one else can access the fb
-	 * list any more, so no need to grab fpriv->fbs_lock. And we need to to
+	 * list any more, so no need to grab fpriv->fbs_lock. And we need to
 	 * avoid upsetting lockdep since the universal cursor code adds a
 	 * framebuffer while holding mutex locks.
 	 *
@@ -3435,6 +3496,10 @@ void drm_fb_release(struct drm_file *priv)
  * object with drm_object_attach_property. The returned property object must be
  * freed with drm_property_destroy.
  *
+ * Note that the DRM core keeps a per-device list of properties and that, if
+ * drm_mode_config_cleanup() is called, it will destroy all properties created
+ * by the driver.
+ *
  * Returns:
  * A pointer to the newly created property on success, NULL on failure.
  */
@@ -3462,7 +3527,7 @@ struct drm_property *drm_property_create(struct drm_device *dev, int flags,
 
 	property->flags = flags;
 	property->num_values = num_values;
-	INIT_LIST_HEAD(&property->enum_blob_list);
+	INIT_LIST_HEAD(&property->enum_list);
 
 	if (name) {
 		strncpy(property->name, name, DRM_PROP_NAME_LEN);
@@ -3611,7 +3676,7 @@ static struct drm_property *property_create_range(struct drm_device *dev,
  * object with drm_object_attach_property. The returned property object must be
  * freed with drm_property_destroy.
  *
- * Userspace is allowed to set any interger value in the (min, max) range
+ * Userspace is allowed to set any integer value in the (min, max) range
  * inclusive.
  *
  * Returns:
@@ -3684,8 +3749,8 @@ int drm_property_add_enum(struct drm_property *property, int index,
 			(value > 63))
 		return -EINVAL;
 
-	if (!list_empty(&property->enum_blob_list)) {
-		list_for_each_entry(prop_enum, &property->enum_blob_list, head) {
+	if (!list_empty(&property->enum_list)) {
+		list_for_each_entry(prop_enum, &property->enum_list, head) {
 			if (prop_enum->value == value) {
 				strncpy(prop_enum->name, name, DRM_PROP_NAME_LEN);
 				prop_enum->name[DRM_PROP_NAME_LEN-1] = '\0';
@@ -3703,7 +3768,7 @@ int drm_property_add_enum(struct drm_property *property, int index,
 	prop_enum->value = value;
 
 	property->values[index] = value;
-	list_add_tail(&prop_enum->head, &property->enum_blob_list);
+	list_add_tail(&prop_enum->head, &property->enum_list);
 	return 0;
 }
 EXPORT_SYMBOL(drm_property_add_enum);
@@ -3720,7 +3785,7 @@ void drm_property_destroy(struct drm_device *dev, struct drm_property *property)
 {
 	struct drm_property_enum *prop_enum, *pt;
 
-	list_for_each_entry_safe(prop_enum, pt, &property->enum_blob_list, head) {
+	list_for_each_entry_safe(prop_enum, pt, &property->enum_list, head) {
 		list_del(&prop_enum->head);
 		kfree(prop_enum);
 	}
@@ -3823,17 +3888,20 @@ int drm_object_property_get_value(struct drm_mode_object *obj,
 EXPORT_SYMBOL(drm_object_property_get_value);
 
 /**
- * drm_mode_getproperty_ioctl - get the current value of a connector's property
+ * drm_mode_getproperty_ioctl - get the property metadata
  * @dev: DRM device
  * @data: ioctl data
  * @file_priv: DRM file info
  *
- * This function retrieves the current value for an connectors's property.
+ * This function retrieves the metadata for a given property, like the different
+ * possible values for an enum property or the limits for a range property.
+ *
+ * Blob properties are special
  *
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_getproperty_ioctl(struct drm_device *dev,
 			       void *data, struct drm_file *file_priv)
@@ -3841,16 +3909,12 @@ int drm_mode_getproperty_ioctl(struct drm_device *dev,
 	struct drm_mode_get_property *out_resp = data;
 	struct drm_property *property;
 	int enum_count = 0;
-	int blob_count = 0;
 	int value_count = 0;
 	int ret = 0, i;
 	int copied;
 	struct drm_property_enum *prop_enum;
 	struct drm_mode_property_enum __user *enum_ptr;
-	struct drm_property_blob *prop_blob;
-	uint32_t __user *blob_id_ptr;
 	uint64_t __user *values_ptr;
-	uint32_t __user *blob_length_ptr;
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return -EINVAL;
@@ -3864,11 +3928,8 @@ int drm_mode_getproperty_ioctl(struct drm_device *dev,
 
 	if (drm_property_type_is(property, DRM_MODE_PROP_ENUM) ||
 			drm_property_type_is(property, DRM_MODE_PROP_BITMASK)) {
-		list_for_each_entry(prop_enum, &property->enum_blob_list, head)
+		list_for_each_entry(prop_enum, &property->enum_list, head)
 			enum_count++;
-	} else if (drm_property_type_is(property, DRM_MODE_PROP_BLOB)) {
-		list_for_each_entry(prop_blob, &property->enum_blob_list, head)
-			blob_count++;
 	}
 
 	value_count = property->num_values;
@@ -3893,7 +3954,7 @@ int drm_mode_getproperty_ioctl(struct drm_device *dev,
 		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;
-			list_for_each_entry(prop_enum, &property->enum_blob_list, head) {
+			list_for_each_entry(prop_enum, &property->enum_list, head) {
 
 				if (copy_to_user(&enum_ptr[copied].value, &prop_enum->value, sizeof(uint64_t))) {
 					ret = -EFAULT;
@@ -3911,35 +3972,24 @@ int drm_mode_getproperty_ioctl(struct drm_device *dev,
 		out_resp->count_enum_blobs = enum_count;
 	}
 
-	if (drm_property_type_is(property, DRM_MODE_PROP_BLOB)) {
-		if ((out_resp->count_enum_blobs >= blob_count) && blob_count) {
-			copied = 0;
-			blob_id_ptr = (uint32_t __user *)(unsigned long)out_resp->enum_blob_ptr;
-			blob_length_ptr = (uint32_t __user *)(unsigned long)out_resp->values_ptr;
-
-			list_for_each_entry(prop_blob, &property->enum_blob_list, head) {
-				if (put_user(prop_blob->base.id, blob_id_ptr + copied)) {
-					ret = -EFAULT;
-					goto done;
-				}
-
-				if (put_user(prop_blob->length, blob_length_ptr + copied)) {
-					ret = -EFAULT;
-					goto done;
-				}
-
-				copied++;
-			}
-		}
-		out_resp->count_enum_blobs = blob_count;
-	}
+	/*
+	 * NOTE: The idea seems to have been to use this to read all the blob
+	 * property values. But nothing ever added them to the corresponding
+	 * list, userspace always used the special-purpose get_blob ioctl to
+	 * read the value for a blob property. It also doesn't make a lot of
+	 * sense to return values here when everything else is just metadata for
+	 * the property itself.
+	 */
+	if (drm_property_type_is(property, DRM_MODE_PROP_BLOB))
+		out_resp->count_enum_blobs = 0;
 done:
 	drm_modeset_unlock_all(dev);
 	return ret;
 }
 
-static struct drm_property_blob *drm_property_create_blob(struct drm_device *dev, int length,
-							  void *data)
+static struct drm_property_blob *
+drm_property_create_blob(struct drm_device *dev, size_t length,
+			 const void *data)
 {
 	struct drm_property_blob *blob;
 	int ret;
@@ -3985,7 +4035,7 @@ static void drm_property_destroy_blob(struct drm_device *dev,
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_getblob_ioctl(struct drm_device *dev,
 			   void *data, struct drm_file *file_priv)
@@ -4019,12 +4069,25 @@ done:
 	return ret;
 }
 
+/**
+ * drm_mode_connector_set_path_property - set tile property on connector
+ * @connector: connector to set property on.
+ * @path: path to use for property.
+ *
+ * This creates a property to expose to userspace to specify a
+ * connector path. This is mainly used for DisplayPort MST where
+ * connectors have a topology and we want to allow userspace to give
+ * them more meaningful names.
+ *
+ * Returns:
+ * Zero on success, negative errno on failure.
+ */
 int drm_mode_connector_set_path_property(struct drm_connector *connector,
-					 char *path)
+					 const char *path)
 {
 	struct drm_device *dev = connector->dev;
-	int ret, size;
-	size = strlen(path) + 1;
+	size_t size = strlen(path) + 1;
+	int ret;
 
 	connector->path_blob_ptr = drm_property_create_blob(connector->dev,
 							    size, path);
@@ -4039,6 +4102,52 @@ int drm_mode_connector_set_path_property(struct drm_connector *connector,
 EXPORT_SYMBOL(drm_mode_connector_set_path_property);
 
 /**
+ * drm_mode_connector_set_tile_property - set tile property on connector
+ * @connector: connector to set property on.
+ *
+ * This looks up the tile information for a connector, and creates a
+ * property for userspace to parse if it exists. The property is of
+ * the form of 8 integers using ':' as a separator.
+ *
+ * Returns:
+ * Zero on success, errno on failure.
+ */
+int drm_mode_connector_set_tile_property(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	int ret, size;
+	char tile[256];
+
+	if (connector->tile_blob_ptr)
+		drm_property_destroy_blob(dev, connector->tile_blob_ptr);
+
+	if (!connector->has_tile) {
+		connector->tile_blob_ptr = NULL;
+		ret = drm_object_property_set_value(&connector->base,
+						    dev->mode_config.tile_property, 0);
+		return ret;
+	}
+
+	snprintf(tile, 256, "%d:%d:%d:%d:%d:%d:%d:%d",
+		 connector->tile_group->id, connector->tile_is_single_monitor,
+		 connector->num_h_tile, connector->num_v_tile,
+		 connector->tile_h_loc, connector->tile_v_loc,
+		 connector->tile_h_size, connector->tile_v_size);
+	size = strlen(tile) + 1;
+
+	connector->tile_blob_ptr = drm_property_create_blob(connector->dev,
+							    size, tile);
+	if (!connector->tile_blob_ptr)
+		return -EINVAL;
+
+	ret = drm_object_property_set_value(&connector->base,
+					    dev->mode_config.tile_property,
+					    connector->tile_blob_ptr->base.id);
+	return ret;
+}
+EXPORT_SYMBOL(drm_mode_connector_set_tile_property);
+
+/**
  * drm_mode_connector_update_edid_property - update the edid property of a connector
  * @connector: drm connector
  * @edid: new value of the edid property
@@ -4047,13 +4156,14 @@ EXPORT_SYMBOL(drm_mode_connector_set_path_property);
  * connector's edid property.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_connector_update_edid_property(struct drm_connector *connector,
-					    struct edid *edid)
+					    const struct edid *edid)
 {
 	struct drm_device *dev = connector->dev;
-	int ret, size;
+	size_t size;
+	int ret;
 
 	/* ignore requests to set edid when overridden */
 	if (connector->override_edid)
@@ -4143,7 +4253,7 @@ static bool drm_property_change_is_valid(struct drm_property *property,
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_connector_property_set_ioctl(struct drm_device *dev,
 				       void *data, struct drm_file *file_priv)
@@ -4226,7 +4336,7 @@ int drm_mode_plane_set_obj_prop(struct drm_plane *plane,
 EXPORT_SYMBOL(drm_mode_plane_set_obj_prop);
 
 /**
- * drm_mode_getproperty_ioctl - get the current value of a object's property
+ * drm_mode_obj_get_properties_ioctl - get the current value of a object's property
  * @dev: DRM device
  * @data: ioctl data
  * @file_priv: DRM file info
@@ -4238,7 +4348,7 @@ EXPORT_SYMBOL(drm_mode_plane_set_obj_prop);
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_obj_get_properties_ioctl(struct drm_device *dev, void *data,
 				      struct drm_file *file_priv)
@@ -4310,7 +4420,7 @@ out:
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_obj_set_property_ioctl(struct drm_device *dev, void *data,
 				    struct drm_file *file_priv)
@@ -4382,7 +4492,7 @@ out:
  * possible_clones and possible_crtcs bitmasks.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_connector_attach_encoder(struct drm_connector *connector,
 				      struct drm_encoder *encoder)
@@ -4409,7 +4519,7 @@ EXPORT_SYMBOL(drm_mode_connector_attach_encoder);
  * fixed gamma table size.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_crtc_set_gamma_size(struct drm_crtc *crtc,
 				 int gamma_size)
@@ -4438,7 +4548,7 @@ EXPORT_SYMBOL(drm_mode_crtc_set_gamma_size);
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_gamma_set_ioctl(struct drm_device *dev,
 			     void *data, struct drm_file *file_priv)
@@ -4510,7 +4620,7 @@ out:
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_gamma_get_ioctl(struct drm_device *dev,
 			     void *data, struct drm_file *file_priv)
@@ -4576,7 +4686,7 @@ out:
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_page_flip_ioctl(struct drm_device *dev,
 			     void *data, struct drm_file *file_priv)
@@ -4599,7 +4709,7 @@ int drm_mode_page_flip_ioctl(struct drm_device *dev,
 	if (!crtc)
 		return -ENOENT;
 
-	drm_modeset_lock_crtc(crtc);
+	drm_modeset_lock_crtc(crtc, crtc->primary);
 	if (crtc->primary->fb == NULL) {
 		/* The framebuffer is currently unbound, presumably
 		 * due to a hotplug event, that userspace has not
@@ -4742,7 +4852,7 @@ EXPORT_SYMBOL(drm_mode_config_reset);
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_create_dumb_ioctl(struct drm_device *dev,
 			       void *data, struct drm_file *file_priv)
@@ -4769,6 +4879,16 @@ int drm_mode_create_dumb_ioctl(struct drm_device *dev,
 	if (PAGE_ALIGN(size) == 0)
 		return -EINVAL;
 
+	/*
+	 * handle, pitch and size are output parameters. Zero them out to
+	 * prevent drivers from accidentally using uninitialized data. Since
+	 * not all existing userspace is clearing these fields properly we
+	 * cannot reject IOCTL with garbage in them.
+	 */
+	args->handle = 0;
+	args->pitch = 0;
+	args->size = 0;
+
 	return dev->driver->dumb_create(file_priv, dev, args);
 }
 
@@ -4784,7 +4904,7 @@ int drm_mode_create_dumb_ioctl(struct drm_device *dev,
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_mmap_dumb_ioctl(struct drm_device *dev,
 			     void *data, struct drm_file *file_priv)
@@ -4811,7 +4931,7 @@ int drm_mode_mmap_dumb_ioctl(struct drm_device *dev,
  * Called by the user via ioctl.
  *
  * Returns:
- * Zero on success, errno on failure.
+ * Zero on success, negative errno on failure.
  */
 int drm_mode_destroy_dumb_ioctl(struct drm_device *dev,
 				void *data, struct drm_file *file_priv)
@@ -5097,6 +5217,7 @@ void drm_mode_config_init(struct drm_device *dev)
 	INIT_LIST_HEAD(&dev->mode_config.property_blob_list);
 	INIT_LIST_HEAD(&dev->mode_config.plane_list);
 	idr_init(&dev->mode_config.crtc_idr);
+	idr_init(&dev->mode_config.tile_idr);
 
 	drm_modeset_lock_all(dev);
 	drm_mode_create_standard_connector_properties(dev);
@@ -5184,6 +5305,7 @@ void drm_mode_config_cleanup(struct drm_device *dev)
 		crtc->funcs->destroy(crtc);
 	}
 
+	idr_destroy(&dev->mode_config.tile_idr);
 	idr_destroy(&dev->mode_config.crtc_idr);
 	drm_modeset_lock_fini(&dev->mode_config.connection_mutex);
 }
@@ -5206,3 +5328,100 @@ struct drm_property *drm_mode_create_rotation_property(struct drm_device *dev,
 					   supported_rotations);
 }
 EXPORT_SYMBOL(drm_mode_create_rotation_property);
+
+/**
+ * DOC: Tile group
+ *
+ * Tile groups are used to represent tiled monitors with a unique
+ * integer identifier. Tiled monitors using DisplayID v1.3 have
+ * a unique 8-byte handle, we store this in a tile group, so we
+ * have a common identifier for all tiles in a monitor group.
+ */
+static void drm_tile_group_free(struct kref *kref)
+{
+	struct drm_tile_group *tg = container_of(kref, struct drm_tile_group, refcount);
+	struct drm_device *dev = tg->dev;
+	mutex_lock(&dev->mode_config.idr_mutex);
+	idr_remove(&dev->mode_config.tile_idr, tg->id);
+	mutex_unlock(&dev->mode_config.idr_mutex);
+	kfree(tg);
+}
+
+/**
+ * drm_mode_put_tile_group - drop a reference to a tile group.
+ * @dev: DRM device
+ * @tg: tile group to drop reference to.
+ *
+ * drop reference to tile group and free if 0.
+ */
+void drm_mode_put_tile_group(struct drm_device *dev,
+			     struct drm_tile_group *tg)
+{
+	kref_put(&tg->refcount, drm_tile_group_free);
+}
+
+/**
+ * drm_mode_get_tile_group - get a reference to an existing tile group
+ * @dev: DRM device
+ * @topology: 8-bytes unique per monitor.
+ *
+ * Use the unique bytes to get a reference to an existing tile group.
+ *
+ * RETURNS:
+ * tile group or NULL if not found.
+ */
+struct drm_tile_group *drm_mode_get_tile_group(struct drm_device *dev,
+					       char topology[8])
+{
+	struct drm_tile_group *tg;
+	int id;
+	mutex_lock(&dev->mode_config.idr_mutex);
+	idr_for_each_entry(&dev->mode_config.tile_idr, tg, id) {
+		if (!memcmp(tg->group_data, topology, 8)) {
+			if (!kref_get_unless_zero(&tg->refcount))
+				tg = NULL;
+			mutex_unlock(&dev->mode_config.idr_mutex);
+			return tg;
+		}
+	}
+	mutex_unlock(&dev->mode_config.idr_mutex);
+	return NULL;
+}
+
+/**
+ * drm_mode_create_tile_group - create a tile group from a displayid description
+ * @dev: DRM device
+ * @topology: 8-bytes unique per monitor.
+ *
+ * Create a tile group for the unique monitor, and get a unique
+ * identifier for the tile group.
+ *
+ * RETURNS:
+ * new tile group or error.
+ */
+struct drm_tile_group *drm_mode_create_tile_group(struct drm_device *dev,
+						  char topology[8])
+{
+	struct drm_tile_group *tg;
+	int ret;
+
+	tg = kzalloc(sizeof(*tg), GFP_KERNEL);
+	if (!tg)
+		return ERR_PTR(-ENOMEM);
+
+	kref_init(&tg->refcount);
+	memcpy(tg->group_data, topology, 8);
+	tg->dev = dev;
+
+	mutex_lock(&dev->mode_config.idr_mutex);
+	ret = idr_alloc(&dev->mode_config.tile_idr, tg, 1, 0, GFP_KERNEL);
+	if (ret >= 0) {
+		tg->id = ret;
+	} else {
+		kfree(tg);
+		tg = ERR_PTR(ret);
+	}
+
+	mutex_unlock(&dev->mode_config.idr_mutex);
+	return tg;
+}
diff --git a/drivers/gpu/drm/drm_crtc_helper.c b/drivers/gpu/drm/drm_crtc_helper.c
index 6c65a0a28fbd..d552708409de 100644
--- a/drivers/gpu/drm/drm_crtc_helper.c
+++ b/drivers/gpu/drm/drm_crtc_helper.c
@@ -34,12 +34,35 @@
 #include <linux/moduleparam.h>
 
 #include <drm/drmP.h>
+#include <drm/drm_atomic.h>
 #include <drm/drm_crtc.h>
 #include <drm/drm_fourcc.h>
 #include <drm/drm_crtc_helper.h>
 #include <drm/drm_fb_helper.h>
+#include <drm/drm_plane_helper.h>
+#include <drm/drm_atomic_helper.h>
 #include <drm/drm_edid.h>
 
+/**
+ * DOC: overview
+ *
+ * The CRTC modeset helper library provides a default set_config implementation
+ * in drm_crtc_helper_set_config(). Plus a few other convenience functions using
+ * the same callbacks which drivers can use to e.g. restore the modeset
+ * configuration on resume with drm_helper_resume_force_mode().
+ *
+ * The driver callbacks are mostly compatible with the atomic modeset helpers,
+ * except for the handling of the primary plane: Atomic helpers require that the
+ * primary plane is implemented as a real standalone plane and not directly tied
+ * to the CRTC state. For easier transition this library provides functions to
+ * implement the old semantics required by the CRTC helpers using the new plane
+ * and atomic helper callbacks.
+ *
+ * Drivers are strongly urged to convert to the atomic helpers (by way of first
+ * converting to the plane helpers). New drivers must not use these functions
+ * but need to implement the atomic interface instead, potentially using the
+ * atomic helpers for that.
+ */
 MODULE_AUTHOR("David Airlie, Jesse Barnes");
 MODULE_DESCRIPTION("DRM KMS helper");
 MODULE_LICENSE("GPL and additional rights");
@@ -888,3 +911,112 @@ void drm_helper_resume_force_mode(struct drm_device *dev)
 	drm_modeset_unlock_all(dev);
 }
 EXPORT_SYMBOL(drm_helper_resume_force_mode);
+
+/**
+ * drm_helper_crtc_mode_set - mode_set implementation for atomic plane helpers
+ * @crtc: DRM CRTC
+ * @mode: DRM display mode which userspace requested
+ * @adjusted_mode: DRM display mode adjusted by ->mode_fixup callbacks
+ * @x: x offset of the CRTC scanout area on the underlying framebuffer
+ * @y: y offset of the CRTC scanout area on the underlying framebuffer
+ * @old_fb: previous framebuffer
+ *
+ * This function implements a callback useable as the ->mode_set callback
+ * required by the crtc helpers. Besides the atomic plane helper functions for
+ * the primary plane the driver must also provide the ->mode_set_nofb callback
+ * to set up the crtc.
+ *
+ * This is a transitional helper useful for converting drivers to the atomic
+ * interfaces.
+ */
+int drm_helper_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_crtc_state *crtc_state;
+	struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+	int ret;
+
+	if (crtc->funcs->atomic_duplicate_state)
+		crtc_state = crtc->funcs->atomic_duplicate_state(crtc);
+	else if (crtc->state)
+		crtc_state = kmemdup(crtc->state, sizeof(*crtc_state),
+				     GFP_KERNEL);
+	else
+		crtc_state = kzalloc(sizeof(*crtc_state), GFP_KERNEL);
+	if (!crtc_state)
+		return -ENOMEM;
+
+	crtc_state->enable = true;
+	crtc_state->planes_changed = true;
+	crtc_state->mode_changed = true;
+	drm_mode_copy(&crtc_state->mode, mode);
+	drm_mode_copy(&crtc_state->adjusted_mode, adjusted_mode);
+
+	if (crtc_funcs->atomic_check) {
+		ret = crtc_funcs->atomic_check(crtc, crtc_state);
+		if (ret) {
+			kfree(crtc_state);
+
+			return ret;
+		}
+	}
+
+	swap(crtc->state, crtc_state);
+
+	crtc_funcs->mode_set_nofb(crtc);
+
+	if (crtc_state) {
+		if (crtc->funcs->atomic_destroy_state)
+			crtc->funcs->atomic_destroy_state(crtc, crtc_state);
+		else
+			kfree(crtc_state);
+	}
+
+	return drm_helper_crtc_mode_set_base(crtc, x, y, old_fb);
+}
+EXPORT_SYMBOL(drm_helper_crtc_mode_set);
+
+/**
+ * drm_helper_crtc_mode_set_base - mode_set_base implementation for atomic plane helpers
+ * @crtc: DRM CRTC
+ * @x: x offset of the CRTC scanout area on the underlying framebuffer
+ * @y: y offset of the CRTC scanout area on the underlying framebuffer
+ * @old_fb: previous framebuffer
+ *
+ * This function implements a callback useable as the ->mode_set_base used
+ * required by the crtc helpers. The driver must provide the atomic plane helper
+ * functions for the primary plane.
+ *
+ * This is a transitional helper useful for converting drivers to the atomic
+ * interfaces.
+ */
+int drm_helper_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
+				  struct drm_framebuffer *old_fb)
+{
+	struct drm_plane_state *plane_state;
+	struct drm_plane *plane = crtc->primary;
+
+	if (plane->funcs->atomic_duplicate_state)
+		plane_state = plane->funcs->atomic_duplicate_state(plane);
+	else if (plane->state)
+		plane_state = drm_atomic_helper_plane_duplicate_state(plane);
+	else
+		plane_state = kzalloc(sizeof(*plane_state), GFP_KERNEL);
+	if (!plane_state)
+		return -ENOMEM;
+
+	plane_state->crtc = crtc;
+	drm_atomic_set_fb_for_plane(plane_state, crtc->primary->fb);
+	plane_state->crtc_x = 0;
+	plane_state->crtc_y = 0;
+	plane_state->crtc_h = crtc->mode.vdisplay;
+	plane_state->crtc_w = crtc->mode.hdisplay;
+	plane_state->src_x = x << 16;
+	plane_state->src_y = y << 16;
+	plane_state->src_h = crtc->mode.vdisplay << 16;
+	plane_state->src_w = crtc->mode.hdisplay << 16;
+
+	return drm_plane_helper_commit(plane, plane_state, old_fb);
+}
+EXPORT_SYMBOL(drm_helper_crtc_mode_set_base);
diff --git a/drivers/gpu/drm/drm_dp_helper.c b/drivers/gpu/drm/drm_dp_helper.c
index 08e33b8b13a4..79968e39c8d0 100644
--- a/drivers/gpu/drm/drm_dp_helper.c
+++ b/drivers/gpu/drm/drm_dp_helper.c
@@ -39,198 +39,6 @@
  * blocks, ...
  */
 
-/* Run a single AUX_CH I2C transaction, writing/reading data as necessary */
-static int
-i2c_algo_dp_aux_transaction(struct i2c_adapter *adapter, int mode,
-			    uint8_t write_byte, uint8_t *read_byte)
-{
-	struct i2c_algo_dp_aux_data *algo_data = adapter->algo_data;
-	int ret;
-
-	ret = (*algo_data->aux_ch)(adapter, mode,
-				   write_byte, read_byte);
-	return ret;
-}
-
-/*
- * I2C over AUX CH
- */
-
-/*
- * Send the address. If the I2C link is running, this 'restarts'
- * the connection with the new address, this is used for doing
- * a write followed by a read (as needed for DDC)
- */
-static int
-i2c_algo_dp_aux_address(struct i2c_adapter *adapter, u16 address, bool reading)
-{
-	struct i2c_algo_dp_aux_data *algo_data = adapter->algo_data;
-	int mode = MODE_I2C_START;
-	int ret;
-
-	if (reading)
-		mode |= MODE_I2C_READ;
-	else
-		mode |= MODE_I2C_WRITE;
-	algo_data->address = address;
-	algo_data->running = true;
-	ret = i2c_algo_dp_aux_transaction(adapter, mode, 0, NULL);
-	return ret;
-}
-
-/*
- * Stop the I2C transaction. This closes out the link, sending
- * a bare address packet with the MOT bit turned off
- */
-static void
-i2c_algo_dp_aux_stop(struct i2c_adapter *adapter, bool reading)
-{
-	struct i2c_algo_dp_aux_data *algo_data = adapter->algo_data;
-	int mode = MODE_I2C_STOP;
-
-	if (reading)
-		mode |= MODE_I2C_READ;
-	else
-		mode |= MODE_I2C_WRITE;
-	if (algo_data->running) {
-		(void) i2c_algo_dp_aux_transaction(adapter, mode, 0, NULL);
-		algo_data->running = false;
-	}
-}
-
-/*
- * Write a single byte to the current I2C address, the
- * the I2C link must be running or this returns -EIO
- */
-static int
-i2c_algo_dp_aux_put_byte(struct i2c_adapter *adapter, u8 byte)
-{
-	struct i2c_algo_dp_aux_data *algo_data = adapter->algo_data;
-	int ret;
-
-	if (!algo_data->running)
-		return -EIO;
-
-	ret = i2c_algo_dp_aux_transaction(adapter, MODE_I2C_WRITE, byte, NULL);
-	return ret;
-}
-
-/*
- * Read a single byte from the current I2C address, the
- * I2C link must be running or this returns -EIO
- */
-static int
-i2c_algo_dp_aux_get_byte(struct i2c_adapter *adapter, u8 *byte_ret)
-{
-	struct i2c_algo_dp_aux_data *algo_data = adapter->algo_data;
-	int ret;
-
-	if (!algo_data->running)
-		return -EIO;
-
-	ret = i2c_algo_dp_aux_transaction(adapter, MODE_I2C_READ, 0, byte_ret);
-	return ret;
-}
-
-static int
-i2c_algo_dp_aux_xfer(struct i2c_adapter *adapter,
-		     struct i2c_msg *msgs,
-		     int num)
-{
-	int ret = 0;
-	bool reading = false;
-	int m;
-	int b;
-
-	for (m = 0; m < num; m++) {
-		u16 len = msgs[m].len;
-		u8 *buf = msgs[m].buf;
-		reading = (msgs[m].flags & I2C_M_RD) != 0;
-		ret = i2c_algo_dp_aux_address(adapter, msgs[m].addr, reading);
-		if (ret < 0)
-			break;
-		if (reading) {
-			for (b = 0; b < len; b++) {
-				ret = i2c_algo_dp_aux_get_byte(adapter, &buf[b]);
-				if (ret < 0)
-					break;
-			}
-		} else {
-			for (b = 0; b < len; b++) {
-				ret = i2c_algo_dp_aux_put_byte(adapter, buf[b]);
-				if (ret < 0)
-					break;
-			}
-		}
-		if (ret < 0)
-			break;
-	}
-	if (ret >= 0)
-		ret = num;
-	i2c_algo_dp_aux_stop(adapter, reading);
-	DRM_DEBUG_KMS("dp_aux_xfer return %d\n", ret);
-	return ret;
-}
-
-static u32
-i2c_algo_dp_aux_functionality(struct i2c_adapter *adapter)
-{
-	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL |
-	       I2C_FUNC_SMBUS_READ_BLOCK_DATA |
-	       I2C_FUNC_SMBUS_BLOCK_PROC_CALL |
-	       I2C_FUNC_10BIT_ADDR;
-}
-
-static const struct i2c_algorithm i2c_dp_aux_algo = {
-	.master_xfer	= i2c_algo_dp_aux_xfer,
-	.functionality	= i2c_algo_dp_aux_functionality,
-};
-
-static void
-i2c_dp_aux_reset_bus(struct i2c_adapter *adapter)
-{
-	(void) i2c_algo_dp_aux_address(adapter, 0, false);
-	(void) i2c_algo_dp_aux_stop(adapter, false);
-}
-
-static int
-i2c_dp_aux_prepare_bus(struct i2c_adapter *adapter)
-{
-	adapter->algo = &i2c_dp_aux_algo;
-	adapter->retries = 3;
-	i2c_dp_aux_reset_bus(adapter);
-	return 0;
-}
-
-/**
- * i2c_dp_aux_add_bus() - register an i2c adapter using the aux ch helper
- * @adapter: i2c adapter to register
- *
- * This registers an i2c adapter that uses dp aux channel as it's underlaying
- * transport. The driver needs to fill out the &i2c_algo_dp_aux_data structure
- * and store it in the algo_data member of the @adapter argument. This will be
- * used by the i2c over dp aux algorithm to drive the hardware.
- *
- * RETURNS:
- * 0 on success, -ERRNO on failure.
- *
- * IMPORTANT:
- * This interface is deprecated, please switch to the new dp aux helpers and
- * drm_dp_aux_register().
- */
-int
-i2c_dp_aux_add_bus(struct i2c_adapter *adapter)
-{
-	int error;
-
-	error = i2c_dp_aux_prepare_bus(adapter);
-	if (error)
-		return error;
-	error = i2c_add_adapter(adapter);
-	return error;
-}
-EXPORT_SYMBOL(i2c_dp_aux_add_bus);
-
 /* Helpers for DP link training */
 static u8 dp_link_status(const u8 link_status[DP_LINK_STATUS_SIZE], int r)
 {
@@ -378,10 +186,11 @@ static int drm_dp_dpcd_access(struct drm_dp_aux *aux, u8 request,
 
 	/*
 	 * The specification doesn't give any recommendation on how often to
-	 * retry native transactions, so retry 7 times like for I2C-over-AUX
-	 * transactions.
+	 * retry native transactions. We used to retry 7 times like for
+	 * aux i2c transactions but real world devices this wasn't
+	 * sufficient, bump to 32 which makes Dell 4k monitors happier.
 	 */
-	for (retry = 0; retry < 7; retry++) {
+	for (retry = 0; retry < 32; retry++) {
 
 		mutex_lock(&aux->hw_mutex);
 		err = aux->transfer(aux, &msg);
@@ -654,10 +463,12 @@ static int drm_dp_i2c_do_msg(struct drm_dp_aux *aux, struct drm_dp_aux_msg *msg)
 
 		case DP_AUX_I2C_REPLY_NACK:
 			DRM_DEBUG_KMS("I2C nack\n");
+			aux->i2c_nack_count++;
 			return -EREMOTEIO;
 
 		case DP_AUX_I2C_REPLY_DEFER:
 			DRM_DEBUG_KMS("I2C defer\n");
+			aux->i2c_defer_count++;
 			usleep_range(400, 500);
 			continue;
 
diff --git a/drivers/gpu/drm/drm_dp_mst_topology.c b/drivers/gpu/drm/drm_dp_mst_topology.c
index 070f913d2dba..9a5b68717ec8 100644
--- a/drivers/gpu/drm/drm_dp_mst_topology.c
+++ b/drivers/gpu/drm/drm_dp_mst_topology.c
@@ -839,6 +839,8 @@ static void drm_dp_put_mst_branch_device(struct drm_dp_mst_branch *mstb)
 
 static void drm_dp_port_teardown_pdt(struct drm_dp_mst_port *port, int old_pdt)
 {
+	struct drm_dp_mst_branch *mstb;
+
 	switch (old_pdt) {
 	case DP_PEER_DEVICE_DP_LEGACY_CONV:
 	case DP_PEER_DEVICE_SST_SINK:
@@ -846,8 +848,9 @@ static void drm_dp_port_teardown_pdt(struct drm_dp_mst_port *port, int old_pdt)
 		drm_dp_mst_unregister_i2c_bus(&port->aux);
 		break;
 	case DP_PEER_DEVICE_MST_BRANCHING:
-		drm_dp_put_mst_branch_device(port->mstb);
+		mstb = port->mstb;
 		port->mstb = NULL;
+		drm_dp_put_mst_branch_device(mstb);
 		break;
 	}
 }
@@ -858,6 +861,8 @@ static void drm_dp_destroy_port(struct kref *kref)
 	struct drm_dp_mst_topology_mgr *mgr = port->mgr;
 	if (!port->input) {
 		port->vcpi.num_slots = 0;
+
+		kfree(port->cached_edid);
 		if (port->connector)
 			(*port->mgr->cbs->destroy_connector)(mgr, port->connector);
 		drm_dp_port_teardown_pdt(port, port->pdt);
@@ -1011,19 +1016,20 @@ static void drm_dp_check_port_guid(struct drm_dp_mst_branch *mstb,
 
 static void build_mst_prop_path(struct drm_dp_mst_port *port,
 				struct drm_dp_mst_branch *mstb,
-				char *proppath)
+				char *proppath,
+				size_t proppath_size)
 {
 	int i;
 	char temp[8];
-	snprintf(proppath, 255, "mst:%d", mstb->mgr->conn_base_id);
+	snprintf(proppath, proppath_size, "mst:%d", mstb->mgr->conn_base_id);
 	for (i = 0; i < (mstb->lct - 1); i++) {
 		int shift = (i % 2) ? 0 : 4;
 		int port_num = mstb->rad[i / 2] >> shift;
-		snprintf(temp, 8, "-%d", port_num);
-		strncat(proppath, temp, 255);
+		snprintf(temp, sizeof(temp), "-%d", port_num);
+		strlcat(proppath, temp, proppath_size);
 	}
-	snprintf(temp, 8, "-%d", port->port_num);
-	strncat(proppath, temp, 255);
+	snprintf(temp, sizeof(temp), "-%d", port->port_num);
+	strlcat(proppath, temp, proppath_size);
 }
 
 static void drm_dp_add_port(struct drm_dp_mst_branch *mstb,
@@ -1094,8 +1100,12 @@ static void drm_dp_add_port(struct drm_dp_mst_branch *mstb,
 
 	if (created && !port->input) {
 		char proppath[255];
-		build_mst_prop_path(port, mstb, proppath);
+		build_mst_prop_path(port, mstb, proppath, sizeof(proppath));
 		port->connector = (*mstb->mgr->cbs->add_connector)(mstb->mgr, port, proppath);
+
+		if (port->port_num >= 8) {
+			port->cached_edid = drm_get_edid(port->connector, &port->aux.ddc);
+		}
 	}
 
 	/* put reference to this port */
@@ -1798,17 +1808,27 @@ static int drm_dp_send_up_ack_reply(struct drm_dp_mst_topology_mgr *mgr,
 	return 0;
 }
 
-static int drm_dp_get_vc_payload_bw(int dp_link_bw, int dp_link_count)
+static bool drm_dp_get_vc_payload_bw(int dp_link_bw,
+				     int dp_link_count,
+				     int *out)
 {
 	switch (dp_link_bw) {
+	default:
+		DRM_DEBUG_KMS("invalid link bandwidth in DPCD: %x (link count: %d)\n",
+			      dp_link_bw, dp_link_count);
+		return false;
+
 	case DP_LINK_BW_1_62:
-		return 3 * dp_link_count;
+		*out = 3 * dp_link_count;
+		break;
 	case DP_LINK_BW_2_7:
-		return 5 * dp_link_count;
+		*out = 5 * dp_link_count;
+		break;
 	case DP_LINK_BW_5_4:
-		return 10 * dp_link_count;
+		*out = 10 * dp_link_count;
+		break;
 	}
-	BUG();
+	return true;
 }
 
 /**
@@ -1840,7 +1860,13 @@ int drm_dp_mst_topology_mgr_set_mst(struct drm_dp_mst_topology_mgr *mgr, bool ms
 			goto out_unlock;
 		}
 
-		mgr->pbn_div = drm_dp_get_vc_payload_bw(mgr->dpcd[1], mgr->dpcd[2] & DP_MAX_LANE_COUNT_MASK);
+		if (!drm_dp_get_vc_payload_bw(mgr->dpcd[1],
+					      mgr->dpcd[2] & DP_MAX_LANE_COUNT_MASK,
+					      &mgr->pbn_div)) {
+			ret = -EINVAL;
+			goto out_unlock;
+		}
+
 		mgr->total_pbn = 2560;
 		mgr->total_slots = DIV_ROUND_UP(mgr->total_pbn, mgr->pbn_div);
 		mgr->avail_slots = mgr->total_slots;
@@ -2150,7 +2176,8 @@ EXPORT_SYMBOL(drm_dp_mst_hpd_irq);
  * This returns the current connection state for a port. It validates the
  * port pointer still exists so the caller doesn't require a reference
  */
-enum drm_connector_status drm_dp_mst_detect_port(struct drm_dp_mst_topology_mgr *mgr, struct drm_dp_mst_port *port)
+enum drm_connector_status drm_dp_mst_detect_port(struct drm_connector *connector,
+						 struct drm_dp_mst_topology_mgr *mgr, struct drm_dp_mst_port *port)
 {
 	enum drm_connector_status status = connector_status_disconnected;
 
@@ -2169,6 +2196,10 @@ enum drm_connector_status drm_dp_mst_detect_port(struct drm_dp_mst_topology_mgr
 
 	case DP_PEER_DEVICE_SST_SINK:
 		status = connector_status_connected;
+		/* for logical ports - cache the EDID */
+		if (port->port_num >= 8 && !port->cached_edid) {
+			port->cached_edid = drm_get_edid(connector, &port->aux.ddc);
+		}
 		break;
 	case DP_PEER_DEVICE_DP_LEGACY_CONV:
 		if (port->ldps)
@@ -2200,7 +2231,12 @@ struct edid *drm_dp_mst_get_edid(struct drm_connector *connector, struct drm_dp_
 	if (!port)
 		return NULL;
 
-	edid = drm_get_edid(connector, &port->aux.ddc);
+	if (port->cached_edid)
+		edid = drm_edid_duplicate(port->cached_edid);
+	else
+		edid = drm_get_edid(connector, &port->aux.ddc);
+
+	drm_mode_connector_set_tile_property(connector);
 	drm_dp_put_port(port);
 	return edid;
 }
diff --git a/drivers/gpu/drm/drm_drv.c b/drivers/gpu/drm/drm_drv.c
index bc3da32d4585..4f41377b0b80 100644
--- a/drivers/gpu/drm/drm_drv.c
+++ b/drivers/gpu/drm/drm_drv.c
@@ -56,7 +56,7 @@ static struct idr drm_minors_idr;
 struct class *drm_class;
 static struct dentry *drm_debugfs_root;
 
-void drm_err(const char *func, const char *format, ...)
+void drm_err(const char *format, ...)
 {
 	struct va_format vaf;
 	va_list args;
@@ -66,7 +66,8 @@ void drm_err(const char *func, const char *format, ...)
 	vaf.fmt = format;
 	vaf.va = &args;
 
-	printk(KERN_ERR "[" DRM_NAME ":%s] *ERROR* %pV", func, &vaf);
+	printk(KERN_ERR "[" DRM_NAME ":%pf] *ERROR* %pV",
+	       __builtin_return_address(0), &vaf);
 
 	va_end(args);
 }
@@ -534,6 +535,8 @@ static void drm_fs_inode_free(struct inode *inode)
  * The initial ref-count of the object is 1. Use drm_dev_ref() and
  * drm_dev_unref() to take and drop further ref-counts.
  *
+ * Note that for purely virtual devices @parent can be NULL.
+ *
  * RETURNS:
  * Pointer to new DRM device, or NULL if out of memory.
  */
diff --git a/drivers/gpu/drm/drm_edid.c b/drivers/gpu/drm/drm_edid.c
index 3bf999134bcc..53bc7a628909 100644
--- a/drivers/gpu/drm/drm_edid.c
+++ b/drivers/gpu/drm/drm_edid.c
@@ -34,6 +34,7 @@
 #include <linux/module.h>
 #include <drm/drmP.h>
 #include <drm/drm_edid.h>
+#include <drm/drm_displayid.h>
 
 #define version_greater(edid, maj, min) \
 	(((edid)->version > (maj)) || \
@@ -1014,6 +1015,27 @@ module_param_named(edid_fixup, edid_fixup, int, 0400);
 MODULE_PARM_DESC(edid_fixup,
 		 "Minimum number of valid EDID header bytes (0-8, default 6)");
 
+static void drm_get_displayid(struct drm_connector *connector,
+			      struct edid *edid);
+
+static int drm_edid_block_checksum(const u8 *raw_edid)
+{
+	int i;
+	u8 csum = 0;
+	for (i = 0; i < EDID_LENGTH; i++)
+		csum += raw_edid[i];
+
+	return csum;
+}
+
+static bool drm_edid_is_zero(const u8 *in_edid, int length)
+{
+	if (memchr_inv(in_edid, 0, length))
+		return false;
+
+	return true;
+}
+
 /**
  * drm_edid_block_valid - Sanity check the EDID block (base or extension)
  * @raw_edid: pointer to raw EDID block
@@ -1027,8 +1049,7 @@ MODULE_PARM_DESC(edid_fixup,
  */
 bool drm_edid_block_valid(u8 *raw_edid, int block, bool print_bad_edid)
 {
-	int i;
-	u8 csum = 0;
+	u8 csum;
 	struct edid *edid = (struct edid *)raw_edid;
 
 	if (WARN_ON(!raw_edid))
@@ -1048,8 +1069,7 @@ bool drm_edid_block_valid(u8 *raw_edid, int block, bool print_bad_edid)
 		}
 	}
 
-	for (i = 0; i < EDID_LENGTH; i++)
-		csum += raw_edid[i];
+	csum = drm_edid_block_checksum(raw_edid);
 	if (csum) {
 		if (print_bad_edid) {
 			DRM_ERROR("EDID checksum is invalid, remainder is %d\n", csum);
@@ -1080,9 +1100,13 @@ bool drm_edid_block_valid(u8 *raw_edid, int block, bool print_bad_edid)
 
 bad:
 	if (print_bad_edid) {
-		printk(KERN_ERR "Raw EDID:\n");
-		print_hex_dump(KERN_ERR, " \t", DUMP_PREFIX_NONE, 16, 1,
+		if (drm_edid_is_zero(raw_edid, EDID_LENGTH)) {
+			printk(KERN_ERR "EDID block is all zeroes\n");
+		} else {
+			printk(KERN_ERR "Raw EDID:\n");
+			print_hex_dump(KERN_ERR, " \t", DUMP_PREFIX_NONE, 16, 1,
 			       raw_edid, EDID_LENGTH, false);
+		}
 	}
 	return false;
 }
@@ -1115,7 +1139,7 @@ EXPORT_SYMBOL(drm_edid_is_valid);
 #define DDC_SEGMENT_ADDR 0x30
 /**
  * drm_do_probe_ddc_edid() - get EDID information via I2C
- * @adapter: I2C device adaptor
+ * @data: I2C device adapter
  * @buf: EDID data buffer to be filled
  * @block: 128 byte EDID block to start fetching from
  * @len: EDID data buffer length to fetch
@@ -1125,9 +1149,9 @@ EXPORT_SYMBOL(drm_edid_is_valid);
  * Return: 0 on success or -1 on failure.
  */
 static int
-drm_do_probe_ddc_edid(struct i2c_adapter *adapter, unsigned char *buf,
-		      int block, int len)
+drm_do_probe_ddc_edid(void *data, u8 *buf, unsigned int block, size_t len)
 {
+	struct i2c_adapter *adapter = data;
 	unsigned char start = block * EDID_LENGTH;
 	unsigned char segment = block >> 1;
 	unsigned char xfers = segment ? 3 : 2;
@@ -1176,16 +1200,26 @@ drm_do_probe_ddc_edid(struct i2c_adapter *adapter, unsigned char *buf,
 	return ret == xfers ? 0 : -1;
 }
 
-static bool drm_edid_is_zero(u8 *in_edid, int length)
-{
-	if (memchr_inv(in_edid, 0, length))
-		return false;
-
-	return true;
-}
-
-static u8 *
-drm_do_get_edid(struct drm_connector *connector, struct i2c_adapter *adapter)
+/**
+ * drm_do_get_edid - get EDID data using a custom EDID block read function
+ * @connector: connector we're probing
+ * @get_edid_block: EDID block read function
+ * @data: private data passed to the block read function
+ *
+ * When the I2C adapter connected to the DDC bus is hidden behind a device that
+ * exposes a different interface to read EDID blocks this function can be used
+ * to get EDID data using a custom block read function.
+ *
+ * As in the general case the DDC bus is accessible by the kernel at the I2C
+ * level, drivers must make all reasonable efforts to expose it as an I2C
+ * adapter and use drm_get_edid() instead of abusing this function.
+ *
+ * Return: Pointer to valid EDID or NULL if we couldn't find any.
+ */
+struct edid *drm_do_get_edid(struct drm_connector *connector,
+	int (*get_edid_block)(void *data, u8 *buf, unsigned int block,
+			      size_t len),
+	void *data)
 {
 	int i, j = 0, valid_extensions = 0;
 	u8 *block, *new;
@@ -1196,7 +1230,7 @@ drm_do_get_edid(struct drm_connector *connector, struct i2c_adapter *adapter)
 
 	/* base block fetch */
 	for (i = 0; i < 4; i++) {
-		if (drm_do_probe_ddc_edid(adapter, block, 0, EDID_LENGTH))
+		if (get_edid_block(data, block, 0, EDID_LENGTH))
 			goto out;
 		if (drm_edid_block_valid(block, 0, print_bad_edid))
 			break;
@@ -1210,7 +1244,7 @@ drm_do_get_edid(struct drm_connector *connector, struct i2c_adapter *adapter)
 
 	/* if there's no extensions, we're done */
 	if (block[0x7e] == 0)
-		return block;
+		return (struct edid *)block;
 
 	new = krealloc(block, (block[0x7e] + 1) * EDID_LENGTH, GFP_KERNEL);
 	if (!new)
@@ -1219,7 +1253,7 @@ drm_do_get_edid(struct drm_connector *connector, struct i2c_adapter *adapter)
 
 	for (j = 1; j <= block[0x7e]; j++) {
 		for (i = 0; i < 4; i++) {
-			if (drm_do_probe_ddc_edid(adapter,
+			if (get_edid_block(data,
 				  block + (valid_extensions + 1) * EDID_LENGTH,
 				  j, EDID_LENGTH))
 				goto out;
@@ -1247,7 +1281,7 @@ drm_do_get_edid(struct drm_connector *connector, struct i2c_adapter *adapter)
 		block = new;
 	}
 
-	return block;
+	return (struct edid *)block;
 
 carp:
 	if (print_bad_edid) {
@@ -1260,6 +1294,7 @@ out:
 	kfree(block);
 	return NULL;
 }
+EXPORT_SYMBOL_GPL(drm_do_get_edid);
 
 /**
  * drm_probe_ddc() - probe DDC presence
@@ -1289,11 +1324,14 @@ EXPORT_SYMBOL(drm_probe_ddc);
 struct edid *drm_get_edid(struct drm_connector *connector,
 			  struct i2c_adapter *adapter)
 {
-	struct edid *edid = NULL;
+	struct edid *edid;
 
-	if (drm_probe_ddc(adapter))
-		edid = (struct edid *)drm_do_get_edid(connector, adapter);
+	if (!drm_probe_ddc(adapter))
+		return NULL;
 
+	edid = drm_do_get_edid(connector, drm_do_probe_ddc_edid, adapter);
+	if (edid)
+		drm_get_displayid(connector, edid);
 	return edid;
 }
 EXPORT_SYMBOL(drm_get_edid);
@@ -2389,7 +2427,7 @@ add_detailed_modes(struct drm_connector *connector, struct edid *edid,
 /*
  * Search EDID for CEA extension block.
  */
-static u8 *drm_find_cea_extension(struct edid *edid)
+static u8 *drm_find_edid_extension(struct edid *edid, int ext_id)
 {
 	u8 *edid_ext = NULL;
 	int i;
@@ -2401,7 +2439,7 @@ static u8 *drm_find_cea_extension(struct edid *edid)
 	/* Find CEA extension */
 	for (i = 0; i < edid->extensions; i++) {
 		edid_ext = (u8 *)edid + EDID_LENGTH * (i + 1);
-		if (edid_ext[0] == CEA_EXT)
+		if (edid_ext[0] == ext_id)
 			break;
 	}
 
@@ -2411,6 +2449,16 @@ static u8 *drm_find_cea_extension(struct edid *edid)
 	return edid_ext;
 }
 
+static u8 *drm_find_cea_extension(struct edid *edid)
+{
+	return drm_find_edid_extension(edid, CEA_EXT);
+}
+
+static u8 *drm_find_displayid_extension(struct edid *edid)
+{
+	return drm_find_edid_extension(edid, DISPLAYID_EXT);
+}
+
 /*
  * Calculate the alternate clock for the CEA mode
  * (60Hz vs. 59.94Hz etc.)
@@ -3128,9 +3176,12 @@ void drm_edid_to_eld(struct drm_connector *connector, struct edid *edid)
 		}
 	}
 	eld[5] |= sad_count << 4;
-	eld[2] = (20 + mnl + sad_count * 3 + 3) / 4;
 
-	DRM_DEBUG_KMS("ELD size %d, SAD count %d\n", (int)eld[2], sad_count);
+	eld[DRM_ELD_BASELINE_ELD_LEN] =
+		DIV_ROUND_UP(drm_eld_calc_baseline_block_size(eld), 4);
+
+	DRM_DEBUG_KMS("ELD size %d, SAD count %d\n",
+		      drm_eld_size(eld), sad_count);
 }
 EXPORT_SYMBOL(drm_edid_to_eld);
 
@@ -3868,3 +3919,123 @@ drm_hdmi_vendor_infoframe_from_display_mode(struct hdmi_vendor_infoframe *frame,
 	return 0;
 }
 EXPORT_SYMBOL(drm_hdmi_vendor_infoframe_from_display_mode);
+
+static int drm_parse_display_id(struct drm_connector *connector,
+				u8 *displayid, int length,
+				bool is_edid_extension)
+{
+	/* if this is an EDID extension the first byte will be 0x70 */
+	int idx = 0;
+	struct displayid_hdr *base;
+	struct displayid_block *block;
+	u8 csum = 0;
+	int i;
+
+	if (is_edid_extension)
+		idx = 1;
+
+	base = (struct displayid_hdr *)&displayid[idx];
+
+	DRM_DEBUG_KMS("base revision 0x%x, length %d, %d %d\n",
+		      base->rev, base->bytes, base->prod_id, base->ext_count);
+
+	if (base->bytes + 5 > length - idx)
+		return -EINVAL;
+
+	for (i = idx; i <= base->bytes + 5; i++) {
+		csum += displayid[i];
+	}
+	if (csum) {
+		DRM_ERROR("DisplayID checksum invalid, remainder is %d\n", csum);
+		return -EINVAL;
+	}
+
+	block = (struct displayid_block *)&displayid[idx + 4];
+	DRM_DEBUG_KMS("block id %d, rev %d, len %d\n",
+		      block->tag, block->rev, block->num_bytes);
+
+	switch (block->tag) {
+	case DATA_BLOCK_TILED_DISPLAY: {
+		struct displayid_tiled_block *tile = (struct displayid_tiled_block *)block;
+
+		u16 w, h;
+		u8 tile_v_loc, tile_h_loc;
+		u8 num_v_tile, num_h_tile;
+		struct drm_tile_group *tg;
+
+		w = tile->tile_size[0] | tile->tile_size[1] << 8;
+		h = tile->tile_size[2] | tile->tile_size[3] << 8;
+
+		num_v_tile = (tile->topo[0] & 0xf) | (tile->topo[2] & 0x30);
+		num_h_tile = (tile->topo[0] >> 4) | ((tile->topo[2] >> 2) & 0x30);
+		tile_v_loc = (tile->topo[1] & 0xf) | ((tile->topo[2] & 0x3) << 4);
+		tile_h_loc = (tile->topo[1] >> 4) | (((tile->topo[2] >> 2) & 0x3) << 4);
+
+		connector->has_tile = true;
+		if (tile->tile_cap & 0x80)
+			connector->tile_is_single_monitor = true;
+
+		connector->num_h_tile = num_h_tile + 1;
+		connector->num_v_tile = num_v_tile + 1;
+		connector->tile_h_loc = tile_h_loc;
+		connector->tile_v_loc = tile_v_loc;
+		connector->tile_h_size = w + 1;
+		connector->tile_v_size = h + 1;
+
+		DRM_DEBUG_KMS("tile cap 0x%x\n", tile->tile_cap);
+		DRM_DEBUG_KMS("tile_size %d x %d\n", w + 1, h + 1);
+		DRM_DEBUG_KMS("topo num tiles %dx%d, location %dx%d\n",
+		       num_h_tile + 1, num_v_tile + 1, tile_h_loc, tile_v_loc);
+		DRM_DEBUG_KMS("vend %c%c%c\n", tile->topology_id[0], tile->topology_id[1], tile->topology_id[2]);
+
+		tg = drm_mode_get_tile_group(connector->dev, tile->topology_id);
+		if (!tg) {
+			tg = drm_mode_create_tile_group(connector->dev, tile->topology_id);
+		}
+		if (!tg)
+			return -ENOMEM;
+
+		if (connector->tile_group != tg) {
+			/* if we haven't got a pointer,
+			   take the reference, drop ref to old tile group */
+			if (connector->tile_group) {
+				drm_mode_put_tile_group(connector->dev, connector->tile_group);
+			}
+			connector->tile_group = tg;
+		} else
+			/* if same tile group, then release the ref we just took. */
+			drm_mode_put_tile_group(connector->dev, tg);
+	}
+		break;
+	default:
+		printk("unknown displayid tag %d\n", block->tag);
+		break;
+	}
+	return 0;
+}
+
+static void drm_get_displayid(struct drm_connector *connector,
+			      struct edid *edid)
+{
+	void *displayid = NULL;
+	int ret;
+	connector->has_tile = false;
+	displayid = drm_find_displayid_extension(edid);
+	if (!displayid) {
+		/* drop reference to any tile group we had */
+		goto out_drop_ref;
+	}
+
+	ret = drm_parse_display_id(connector, displayid, EDID_LENGTH, true);
+	if (ret < 0)
+		goto out_drop_ref;
+	if (!connector->has_tile)
+		goto out_drop_ref;
+	return;
+out_drop_ref:
+	if (connector->tile_group) {
+		drm_mode_put_tile_group(connector->dev, connector->tile_group);
+		connector->tile_group = NULL;
+	}
+	return;
+}
diff --git a/drivers/gpu/drm/drm_edid_load.c b/drivers/gpu/drm/drm_edid_load.c
index 0a235fe61c9b..732cb6f8e653 100644
--- a/drivers/gpu/drm/drm_edid_load.c
+++ b/drivers/gpu/drm/drm_edid_load.c
@@ -254,8 +254,7 @@ static void *edid_load(struct drm_connector *connector, const char *name,
 	    name, connector_name);
 
 out:
-	if (fw)
-		release_firmware(fw);
+	release_firmware(fw);
 	return edid;
 }
 
diff --git a/drivers/gpu/drm/drm_fb_helper.c b/drivers/gpu/drm/drm_fb_helper.c
index 0c0c39bac23d..52ce26d6b4fb 100644
--- a/drivers/gpu/drm/drm_fb_helper.c
+++ b/drivers/gpu/drm/drm_fb_helper.c
@@ -347,9 +347,18 @@ bool drm_fb_helper_restore_fbdev_mode_unlocked(struct drm_fb_helper *fb_helper)
 {
 	struct drm_device *dev = fb_helper->dev;
 	bool ret;
+	bool do_delayed = false;
+
 	drm_modeset_lock_all(dev);
 	ret = restore_fbdev_mode(fb_helper);
+
+	do_delayed = fb_helper->delayed_hotplug;
+	if (do_delayed)
+		fb_helper->delayed_hotplug = false;
 	drm_modeset_unlock_all(dev);
+
+	if (do_delayed)
+		drm_fb_helper_hotplug_event(fb_helper);
 	return ret;
 }
 EXPORT_SYMBOL(drm_fb_helper_restore_fbdev_mode_unlocked);
@@ -888,10 +897,6 @@ int drm_fb_helper_set_par(struct fb_info *info)
 
 	drm_fb_helper_restore_fbdev_mode_unlocked(fb_helper);
 
-	if (fb_helper->delayed_hotplug) {
-		fb_helper->delayed_hotplug = false;
-		drm_fb_helper_hotplug_event(fb_helper);
-	}
 	return 0;
 }
 EXPORT_SYMBOL(drm_fb_helper_set_par);
@@ -995,19 +1000,21 @@ static int drm_fb_helper_single_fb_probe(struct drm_fb_helper *fb_helper,
 	crtc_count = 0;
 	for (i = 0; i < fb_helper->crtc_count; i++) {
 		struct drm_display_mode *desired_mode;
+		int x, y;
 		desired_mode = fb_helper->crtc_info[i].desired_mode;
-
+		x = fb_helper->crtc_info[i].x;
+		y = fb_helper->crtc_info[i].y;
 		if (desired_mode) {
 			if (gamma_size == 0)
 				gamma_size = fb_helper->crtc_info[i].mode_set.crtc->gamma_size;
-			if (desired_mode->hdisplay < sizes.fb_width)
-				sizes.fb_width = desired_mode->hdisplay;
-			if (desired_mode->vdisplay < sizes.fb_height)
-				sizes.fb_height = desired_mode->vdisplay;
-			if (desired_mode->hdisplay > sizes.surface_width)
-				sizes.surface_width = desired_mode->hdisplay;
-			if (desired_mode->vdisplay > sizes.surface_height)
-				sizes.surface_height = desired_mode->vdisplay;
+			if (desired_mode->hdisplay + x < sizes.fb_width)
+				sizes.fb_width = desired_mode->hdisplay + x;
+			if (desired_mode->vdisplay + y < sizes.fb_height)
+				sizes.fb_height = desired_mode->vdisplay + y;
+			if (desired_mode->hdisplay + x > sizes.surface_width)
+				sizes.surface_width = desired_mode->hdisplay + x;
+			if (desired_mode->vdisplay + y > sizes.surface_height)
+				sizes.surface_height = desired_mode->vdisplay + y;
 			crtc_count++;
 		}
 	}
@@ -1307,6 +1314,7 @@ static void drm_enable_connectors(struct drm_fb_helper *fb_helper,
 
 static bool drm_target_cloned(struct drm_fb_helper *fb_helper,
 			      struct drm_display_mode **modes,
+			      struct drm_fb_offset *offsets,
 			      bool *enabled, int width, int height)
 {
 	int count, i, j;
@@ -1378,27 +1386,88 @@ static bool drm_target_cloned(struct drm_fb_helper *fb_helper,
 	return false;
 }
 
+static int drm_get_tile_offsets(struct drm_fb_helper *fb_helper,
+				struct drm_display_mode **modes,
+				struct drm_fb_offset *offsets,
+				int idx,
+				int h_idx, int v_idx)
+{
+	struct drm_fb_helper_connector *fb_helper_conn;
+	int i;
+	int hoffset = 0, voffset = 0;
+
+	for (i = 0; i < fb_helper->connector_count; i++) {
+		fb_helper_conn = fb_helper->connector_info[i];
+		if (!fb_helper_conn->connector->has_tile)
+			continue;
+
+		if (!modes[i] && (h_idx || v_idx)) {
+			DRM_DEBUG_KMS("no modes for connector tiled %d %d\n", i,
+				      fb_helper_conn->connector->base.id);
+			continue;
+		}
+		if (fb_helper_conn->connector->tile_h_loc < h_idx)
+			hoffset += modes[i]->hdisplay;
+
+		if (fb_helper_conn->connector->tile_v_loc < v_idx)
+			voffset += modes[i]->vdisplay;
+	}
+	offsets[idx].x = hoffset;
+	offsets[idx].y = voffset;
+	DRM_DEBUG_KMS("returned %d %d for %d %d\n", hoffset, voffset, h_idx, v_idx);
+	return 0;
+}
+
 static bool drm_target_preferred(struct drm_fb_helper *fb_helper,
 				 struct drm_display_mode **modes,
+				 struct drm_fb_offset *offsets,
 				 bool *enabled, int width, int height)
 {
 	struct drm_fb_helper_connector *fb_helper_conn;
 	int i;
-
+	uint64_t conn_configured = 0, mask;
+	int tile_pass = 0;
+	mask = (1 << fb_helper->connector_count) - 1;
+retry:
 	for (i = 0; i < fb_helper->connector_count; i++) {
 		fb_helper_conn = fb_helper->connector_info[i];
 
-		if (enabled[i] == false)
+		if (conn_configured & (1 << i))
 			continue;
 
+		if (enabled[i] == false) {
+			conn_configured |= (1 << i);
+			continue;
+		}
+
+		/* first pass over all the untiled connectors */
+		if (tile_pass == 0 && fb_helper_conn->connector->has_tile)
+			continue;
+
+		if (tile_pass == 1) {
+			if (fb_helper_conn->connector->tile_h_loc != 0 ||
+			    fb_helper_conn->connector->tile_v_loc != 0)
+				continue;
+
+		} else {
+			if (fb_helper_conn->connector->tile_h_loc != tile_pass -1 &&
+			    fb_helper_conn->connector->tile_v_loc != tile_pass - 1)
+			/* if this tile_pass doesn't cover any of the tiles - keep going */
+				continue;
+
+			/* find the tile offsets for this pass - need
+			   to find all tiles left and above */
+			drm_get_tile_offsets(fb_helper, modes, offsets,
+					     i, fb_helper_conn->connector->tile_h_loc, fb_helper_conn->connector->tile_v_loc);
+		}
 		DRM_DEBUG_KMS("looking for cmdline mode on connector %d\n",
 			      fb_helper_conn->connector->base.id);
 
 		/* got for command line mode first */
 		modes[i] = drm_pick_cmdline_mode(fb_helper_conn, width, height);
 		if (!modes[i]) {
-			DRM_DEBUG_KMS("looking for preferred mode on connector %d\n",
-				      fb_helper_conn->connector->base.id);
+			DRM_DEBUG_KMS("looking for preferred mode on connector %d %d\n",
+				      fb_helper_conn->connector->base.id, fb_helper_conn->connector->tile_group ? fb_helper_conn->connector->tile_group->id : 0);
 			modes[i] = drm_has_preferred_mode(fb_helper_conn, width, height);
 		}
 		/* No preferred modes, pick one off the list */
@@ -1408,6 +1477,12 @@ static bool drm_target_preferred(struct drm_fb_helper *fb_helper,
 		}
 		DRM_DEBUG_KMS("found mode %s\n", modes[i] ? modes[i]->name :
 			  "none");
+		conn_configured |= (1 << i);
+	}
+
+	if ((conn_configured & mask) != mask) {
+		tile_pass++;
+		goto retry;
 	}
 	return true;
 }
@@ -1497,6 +1572,7 @@ static void drm_setup_crtcs(struct drm_fb_helper *fb_helper)
 	struct drm_device *dev = fb_helper->dev;
 	struct drm_fb_helper_crtc **crtcs;
 	struct drm_display_mode **modes;
+	struct drm_fb_offset *offsets;
 	struct drm_mode_set *modeset;
 	bool *enabled;
 	int width, height;
@@ -1511,9 +1587,11 @@ static void drm_setup_crtcs(struct drm_fb_helper *fb_helper)
 			sizeof(struct drm_fb_helper_crtc *), GFP_KERNEL);
 	modes = kcalloc(dev->mode_config.num_connector,
 			sizeof(struct drm_display_mode *), GFP_KERNEL);
+	offsets = kcalloc(dev->mode_config.num_connector,
+			  sizeof(struct drm_fb_offset), GFP_KERNEL);
 	enabled = kcalloc(dev->mode_config.num_connector,
 			  sizeof(bool), GFP_KERNEL);
-	if (!crtcs || !modes || !enabled) {
+	if (!crtcs || !modes || !enabled || !offsets) {
 		DRM_ERROR("Memory allocation failed\n");
 		goto out;
 	}
@@ -1523,14 +1601,16 @@ static void drm_setup_crtcs(struct drm_fb_helper *fb_helper)
 
 	if (!(fb_helper->funcs->initial_config &&
 	      fb_helper->funcs->initial_config(fb_helper, crtcs, modes,
+					       offsets,
 					       enabled, width, height))) {
 		memset(modes, 0, dev->mode_config.num_connector*sizeof(modes[0]));
 		memset(crtcs, 0, dev->mode_config.num_connector*sizeof(crtcs[0]));
+		memset(offsets, 0, dev->mode_config.num_connector*sizeof(offsets[0]));
 
-		if (!drm_target_cloned(fb_helper,
-				       modes, enabled, width, height) &&
-		    !drm_target_preferred(fb_helper,
-					  modes, enabled, width, height))
+		if (!drm_target_cloned(fb_helper, modes, offsets,
+				       enabled, width, height) &&
+		    !drm_target_preferred(fb_helper, modes, offsets,
+					  enabled, width, height))
 			DRM_ERROR("Unable to find initial modes\n");
 
 		DRM_DEBUG_KMS("picking CRTCs for %dx%d config\n",
@@ -1550,18 +1630,23 @@ static void drm_setup_crtcs(struct drm_fb_helper *fb_helper)
 	for (i = 0; i < fb_helper->connector_count; i++) {
 		struct drm_display_mode *mode = modes[i];
 		struct drm_fb_helper_crtc *fb_crtc = crtcs[i];
+		struct drm_fb_offset *offset = &offsets[i];
 		modeset = &fb_crtc->mode_set;
 
 		if (mode && fb_crtc) {
-			DRM_DEBUG_KMS("desired mode %s set on crtc %d\n",
-				      mode->name, fb_crtc->mode_set.crtc->base.id);
+			DRM_DEBUG_KMS("desired mode %s set on crtc %d (%d,%d)\n",
+				      mode->name, fb_crtc->mode_set.crtc->base.id, offset->x, offset->y);
 			fb_crtc->desired_mode = mode;
+			fb_crtc->x = offset->x;
+			fb_crtc->y = offset->y;
 			if (modeset->mode)
 				drm_mode_destroy(dev, modeset->mode);
 			modeset->mode = drm_mode_duplicate(dev,
 							   fb_crtc->desired_mode);
 			modeset->connectors[modeset->num_connectors++] = fb_helper->connector_info[i]->connector;
 			modeset->fb = fb_helper->fb;
+			modeset->x = offset->x;
+			modeset->y = offset->y;
 		}
 	}
 
@@ -1570,7 +1655,6 @@ static void drm_setup_crtcs(struct drm_fb_helper *fb_helper)
 		modeset = &fb_helper->crtc_info[i].mode_set;
 		if (modeset->num_connectors == 0) {
 			BUG_ON(modeset->fb);
-			BUG_ON(modeset->num_connectors);
 			if (modeset->mode)
 				drm_mode_destroy(dev, modeset->mode);
 			modeset->mode = NULL;
@@ -1579,6 +1663,7 @@ static void drm_setup_crtcs(struct drm_fb_helper *fb_helper)
 out:
 	kfree(crtcs);
 	kfree(modes);
+	kfree(offsets);
 	kfree(enabled);
 }
 
diff --git a/drivers/gpu/drm/drm_flip_work.c b/drivers/gpu/drm/drm_flip_work.c
index f9c7fa3d0012..43d9b950ef9f 100644
--- a/drivers/gpu/drm/drm_flip_work.c
+++ b/drivers/gpu/drm/drm_flip_work.c
@@ -25,6 +25,44 @@
 #include "drm_flip_work.h"
 
 /**
+ * drm_flip_work_allocate_task - allocate a flip-work task
+ * @data: data associated to the task
+ * @flags: allocator flags
+ *
+ * Allocate a drm_flip_task object and attach private data to it.
+ */
+struct drm_flip_task *drm_flip_work_allocate_task(void *data, gfp_t flags)
+{
+	struct drm_flip_task *task;
+
+	task = kzalloc(sizeof(*task), flags);
+	if (task)
+		task->data = data;
+
+	return task;
+}
+EXPORT_SYMBOL(drm_flip_work_allocate_task);
+
+/**
+ * drm_flip_work_queue_task - queue a specific task
+ * @work: the flip-work
+ * @task: the task to handle
+ *
+ * Queues task, that will later be run (passed back to drm_flip_func_t
+ * func) on a work queue after drm_flip_work_commit() is called.
+ */
+void drm_flip_work_queue_task(struct drm_flip_work *work,
+			      struct drm_flip_task *task)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&work->lock, flags);
+	list_add_tail(&task->node, &work->queued);
+	spin_unlock_irqrestore(&work->lock, flags);
+}
+EXPORT_SYMBOL(drm_flip_work_queue_task);
+
+/**
  * drm_flip_work_queue - queue work
  * @work: the flip-work
  * @val: the value to queue
@@ -34,10 +72,14 @@
  */
 void drm_flip_work_queue(struct drm_flip_work *work, void *val)
 {
-	if (kfifo_put(&work->fifo, val)) {
-		atomic_inc(&work->pending);
+	struct drm_flip_task *task;
+
+	task = drm_flip_work_allocate_task(val,
+				drm_can_sleep() ? GFP_KERNEL : GFP_ATOMIC);
+	if (task) {
+		drm_flip_work_queue_task(work, task);
 	} else {
-		DRM_ERROR("%s fifo full!\n", work->name);
+		DRM_ERROR("%s could not allocate task!\n", work->name);
 		work->func(work, val);
 	}
 }
@@ -56,9 +98,12 @@ EXPORT_SYMBOL(drm_flip_work_queue);
 void drm_flip_work_commit(struct drm_flip_work *work,
 		struct workqueue_struct *wq)
 {
-	uint32_t pending = atomic_read(&work->pending);
-	atomic_add(pending, &work->count);
-	atomic_sub(pending, &work->pending);
+	unsigned long flags;
+
+	spin_lock_irqsave(&work->lock, flags);
+	list_splice_tail(&work->queued, &work->commited);
+	INIT_LIST_HEAD(&work->queued);
+	spin_unlock_irqrestore(&work->lock, flags);
 	queue_work(wq, &work->worker);
 }
 EXPORT_SYMBOL(drm_flip_work_commit);
@@ -66,47 +111,46 @@ EXPORT_SYMBOL(drm_flip_work_commit);
 static void flip_worker(struct work_struct *w)
 {
 	struct drm_flip_work *work = container_of(w, struct drm_flip_work, worker);
-	uint32_t count = atomic_read(&work->count);
-	void *val = NULL;
+	struct list_head tasks;
+	unsigned long flags;
+
+	while (1) {
+		struct drm_flip_task *task, *tmp;
+
+		INIT_LIST_HEAD(&tasks);
+		spin_lock_irqsave(&work->lock, flags);
+		list_splice_tail(&work->commited, &tasks);
+		INIT_LIST_HEAD(&work->commited);
+		spin_unlock_irqrestore(&work->lock, flags);
 
-	atomic_sub(count, &work->count);
+		if (list_empty(&tasks))
+			break;
 
-	while(count--)
-		if (!WARN_ON(!kfifo_get(&work->fifo, &val)))
-			work->func(work, val);
+		list_for_each_entry_safe(task, tmp, &tasks, node) {
+			work->func(work, task->data);
+			kfree(task);
+		}
+	}
 }
 
 /**
  * drm_flip_work_init - initialize flip-work
  * @work: the flip-work to initialize
- * @size: the max queue depth
  * @name: debug name
  * @func: the callback work function
  *
  * Initializes/allocates resources for the flip-work
- *
- * RETURNS:
- * Zero on success, error code on failure.
  */
-int drm_flip_work_init(struct drm_flip_work *work, int size,
+void drm_flip_work_init(struct drm_flip_work *work,
 		const char *name, drm_flip_func_t func)
 {
-	int ret;
-
 	work->name = name;
-	atomic_set(&work->count, 0);
-	atomic_set(&work->pending, 0);
+	INIT_LIST_HEAD(&work->queued);
+	INIT_LIST_HEAD(&work->commited);
+	spin_lock_init(&work->lock);
 	work->func = func;
 
-	ret = kfifo_alloc(&work->fifo, size, GFP_KERNEL);
-	if (ret) {
-		DRM_ERROR("could not allocate %s fifo\n", name);
-		return ret;
-	}
-
 	INIT_WORK(&work->worker, flip_worker);
-
-	return 0;
 }
 EXPORT_SYMBOL(drm_flip_work_init);
 
@@ -118,7 +162,6 @@ EXPORT_SYMBOL(drm_flip_work_init);
  */
 void drm_flip_work_cleanup(struct drm_flip_work *work)
 {
-	WARN_ON(!kfifo_is_empty(&work->fifo));
-	kfifo_free(&work->fifo);
+	WARN_ON(!list_empty(&work->queued) || !list_empty(&work->commited));
 }
 EXPORT_SYMBOL(drm_flip_work_cleanup);
diff --git a/drivers/gpu/drm/drm_fops.c b/drivers/gpu/drm/drm_fops.c
index ed7bc68f7e87..0b9514b6cd64 100644
--- a/drivers/gpu/drm/drm_fops.c
+++ b/drivers/gpu/drm/drm_fops.c
@@ -515,16 +515,19 @@ ssize_t drm_read(struct file *filp, char __user *buffer,
 	size_t total;
 	ssize_t ret;
 
-	ret = wait_event_interruptible(file_priv->event_wait,
-				       !list_empty(&file_priv->event_list));
-	if (ret < 0)
-		return ret;
+	if ((filp->f_flags & O_NONBLOCK) == 0) {
+		ret = wait_event_interruptible(file_priv->event_wait,
+					       !list_empty(&file_priv->event_list));
+		if (ret < 0)
+			return ret;
+	}
 
 	total = 0;
 	while (drm_dequeue_event(file_priv, total, count, &e)) {
 		if (copy_to_user(buffer + total,
 				 e->event, e->event->length)) {
 			total = -EFAULT;
+			e->destroy(e);
 			break;
 		}
 
@@ -532,7 +535,7 @@ ssize_t drm_read(struct file *filp, char __user *buffer,
 		e->destroy(e);
 	}
 
-	return total;
+	return total ?: -EAGAIN;
 }
 EXPORT_SYMBOL(drm_read);
 
diff --git a/drivers/gpu/drm/drm_gem.c b/drivers/gpu/drm/drm_gem.c
index f6ca51259fa3..16a164770713 100644
--- a/drivers/gpu/drm/drm_gem.c
+++ b/drivers/gpu/drm/drm_gem.c
@@ -188,7 +188,7 @@ drm_gem_remove_prime_handles(struct drm_gem_object *obj, struct drm_file *filp)
 }
 
 /**
- * drm_gem_object_free - release resources bound to userspace handles
+ * drm_gem_object_handle_free - release resources bound to userspace handles
  * @obj: GEM object to clean up.
  *
  * Called after the last handle to the object has been closed
@@ -309,7 +309,7 @@ EXPORT_SYMBOL(drm_gem_dumb_destroy);
  * drm_gem_handle_create_tail - internal functions to create a handle
  * @file_priv: drm file-private structure to register the handle for
  * @obj: object to register
- * @handlep: pionter to return the created handle to the caller
+ * @handlep: pointer to return the created handle to the caller
  * 
  * This expects the dev->object_name_lock to be held already and will drop it
  * before returning. Used to avoid races in establishing new handles when
@@ -362,7 +362,7 @@ drm_gem_handle_create_tail(struct drm_file *file_priv,
 }
 
 /**
- * gem_handle_create - create a gem handle for an object
+ * drm_gem_handle_create - create a gem handle for an object
  * @file_priv: drm file-private structure to register the handle for
  * @obj: object to register
  * @handlep: pionter to return the created handle to the caller
@@ -371,10 +371,9 @@ drm_gem_handle_create_tail(struct drm_file *file_priv,
  * to the object, which includes a regular reference count. Callers
  * will likely want to dereference the object afterwards.
  */
-int
-drm_gem_handle_create(struct drm_file *file_priv,
-		       struct drm_gem_object *obj,
-		       u32 *handlep)
+int drm_gem_handle_create(struct drm_file *file_priv,
+			  struct drm_gem_object *obj,
+			  u32 *handlep)
 {
 	mutex_lock(&obj->dev->object_name_lock);
 
diff --git a/drivers/gpu/drm/drm_gem_cma_helper.c b/drivers/gpu/drm/drm_gem_cma_helper.c
index 0316310e2cc4..e419eedf751d 100644
--- a/drivers/gpu/drm/drm_gem_cma_helper.c
+++ b/drivers/gpu/drm/drm_gem_cma_helper.c
@@ -29,18 +29,31 @@
 #include <drm/drm_gem_cma_helper.h>
 #include <drm/drm_vma_manager.h>
 
-/*
+/**
+ * DOC: cma helpers
+ *
+ * The Contiguous Memory Allocator reserves a pool of memory at early boot
+ * that is used to service requests for large blocks of contiguous memory.
+ *
+ * The DRM GEM/CMA helpers use this allocator as a means to provide buffer
+ * objects that are physically contiguous in memory. This is useful for
+ * display drivers that are unable to map scattered buffers via an IOMMU.
+ */
+
+/**
  * __drm_gem_cma_create - Create a GEM CMA object without allocating memory
- * @drm: The drm device
- * @size: The GEM object size
+ * @drm: DRM device
+ * @size: size of the object to allocate
  *
- * This function creates and initializes a GEM CMA object of the given size, but
- * doesn't allocate any memory to back the object.
+ * This function creates and initializes a GEM CMA object of the given size,
+ * but doesn't allocate any memory to back the object.
  *
- * Return a struct drm_gem_cma_object* on success or ERR_PTR values on failure.
+ * Returns:
+ * A struct drm_gem_cma_object * on success or an ERR_PTR()-encoded negative
+ * error code on failure.
  */
 static struct drm_gem_cma_object *
-__drm_gem_cma_create(struct drm_device *drm, unsigned int size)
+__drm_gem_cma_create(struct drm_device *drm, size_t size)
 {
 	struct drm_gem_cma_object *cma_obj;
 	struct drm_gem_object *gem_obj;
@@ -69,14 +82,21 @@ error:
 	return ERR_PTR(ret);
 }
 
-/*
+/**
  * drm_gem_cma_create - allocate an object with the given size
+ * @drm: DRM device
+ * @size: size of the object to allocate
+ *
+ * This function creates a CMA GEM object and allocates a contiguous chunk of
+ * memory as backing store. The backing memory has the writecombine attribute
+ * set.
  *
- * returns a struct drm_gem_cma_object* on success or ERR_PTR values
- * on failure.
+ * Returns:
+ * A struct drm_gem_cma_object * on success or an ERR_PTR()-encoded negative
+ * error code on failure.
  */
 struct drm_gem_cma_object *drm_gem_cma_create(struct drm_device *drm,
-		unsigned int size)
+					      size_t size)
 {
 	struct drm_gem_cma_object *cma_obj;
 	int ret;
@@ -104,17 +124,26 @@ error:
 }
 EXPORT_SYMBOL_GPL(drm_gem_cma_create);
 
-/*
- * drm_gem_cma_create_with_handle - allocate an object with the given
- * size and create a gem handle on it
+/**
+ * drm_gem_cma_create_with_handle - allocate an object with the given size and
+ *     return a GEM handle to it
+ * @file_priv: DRM file-private structure to register the handle for
+ * @drm: DRM device
+ * @size: size of the object to allocate
+ * @handle: return location for the GEM handle
+ *
+ * This function creates a CMA GEM object, allocating a physically contiguous
+ * chunk of memory as backing store. The GEM object is then added to the list
+ * of object associated with the given file and a handle to it is returned.
  *
- * returns a struct drm_gem_cma_object* on success or ERR_PTR values
- * on failure.
+ * Returns:
+ * A struct drm_gem_cma_object * on success or an ERR_PTR()-encoded negative
+ * error code on failure.
  */
-static struct drm_gem_cma_object *drm_gem_cma_create_with_handle(
-		struct drm_file *file_priv,
-		struct drm_device *drm, unsigned int size,
-		unsigned int *handle)
+static struct drm_gem_cma_object *
+drm_gem_cma_create_with_handle(struct drm_file *file_priv,
+			       struct drm_device *drm, size_t size,
+			       uint32_t *handle)
 {
 	struct drm_gem_cma_object *cma_obj;
 	struct drm_gem_object *gem_obj;
@@ -145,16 +174,19 @@ err_handle_create:
 	return ERR_PTR(ret);
 }
 
-/*
- * drm_gem_cma_free_object - (struct drm_driver)->gem_free_object callback
- * function
+/**
+ * drm_gem_cma_free_object - free resources associated with a CMA GEM object
+ * @gem_obj: GEM object to free
+ *
+ * This function frees the backing memory of the CMA GEM object, cleans up the
+ * GEM object state and frees the memory used to store the object itself.
+ * Drivers using the CMA helpers should set this as their DRM driver's
+ * ->gem_free_object() callback.
  */
 void drm_gem_cma_free_object(struct drm_gem_object *gem_obj)
 {
 	struct drm_gem_cma_object *cma_obj;
 
-	drm_gem_free_mmap_offset(gem_obj);
-
 	cma_obj = to_drm_gem_cma_obj(gem_obj);
 
 	if (cma_obj->vaddr) {
@@ -170,18 +202,26 @@ void drm_gem_cma_free_object(struct drm_gem_object *gem_obj)
 }
 EXPORT_SYMBOL_GPL(drm_gem_cma_free_object);
 
-/*
- * drm_gem_cma_dumb_create - (struct drm_driver)->dumb_create callback
- * function
+/**
+ * drm_gem_cma_dumb_create_internal - create a dumb buffer object
+ * @file_priv: DRM file-private structure to create the dumb buffer for
+ * @drm: DRM device
+ * @args: IOCTL data
+ *
+ * This aligns the pitch and size arguments to the minimum required. This is
+ * an internal helper that can be wrapped by a driver to account for hardware
+ * with more specific alignment requirements. It should not be used directly
+ * as the ->dumb_create() callback in a DRM driver.
  *
- * This aligns the pitch and size arguments to the minimum required. wrap
- * this into your own function if you need bigger alignment.
+ * Returns:
+ * 0 on success or a negative error code on failure.
  */
-int drm_gem_cma_dumb_create(struct drm_file *file_priv,
-		struct drm_device *dev, struct drm_mode_create_dumb *args)
+int drm_gem_cma_dumb_create_internal(struct drm_file *file_priv,
+				     struct drm_device *drm,
+				     struct drm_mode_create_dumb *args)
 {
+	unsigned int min_pitch = DIV_ROUND_UP(args->width * args->bpp, 8);
 	struct drm_gem_cma_object *cma_obj;
-	int min_pitch = DIV_ROUND_UP(args->width * args->bpp, 8);
 
 	if (args->pitch < min_pitch)
 		args->pitch = min_pitch;
@@ -189,18 +229,63 @@ int drm_gem_cma_dumb_create(struct drm_file *file_priv,
 	if (args->size < args->pitch * args->height)
 		args->size = args->pitch * args->height;
 
-	cma_obj = drm_gem_cma_create_with_handle(file_priv, dev,
-			args->size, &args->handle);
+	cma_obj = drm_gem_cma_create_with_handle(file_priv, drm, args->size,
+						 &args->handle);
+	return PTR_ERR_OR_ZERO(cma_obj);
+}
+EXPORT_SYMBOL_GPL(drm_gem_cma_dumb_create_internal);
+
+/**
+ * drm_gem_cma_dumb_create - create a dumb buffer object
+ * @file_priv: DRM file-private structure to create the dumb buffer for
+ * @drm: DRM device
+ * @args: IOCTL data
+ *
+ * This function computes the pitch of the dumb buffer and rounds it up to an
+ * integer number of bytes per pixel. Drivers for hardware that doesn't have
+ * any additional restrictions on the pitch can directly use this function as
+ * their ->dumb_create() callback.
+ *
+ * For hardware with additional restrictions, drivers can adjust the fields
+ * set up by userspace and pass the IOCTL data along to the
+ * drm_gem_cma_dumb_create_internal() function.
+ *
+ * Returns:
+ * 0 on success or a negative error code on failure.
+ */
+int drm_gem_cma_dumb_create(struct drm_file *file_priv,
+			    struct drm_device *drm,
+			    struct drm_mode_create_dumb *args)
+{
+	struct drm_gem_cma_object *cma_obj;
+
+	args->pitch = DIV_ROUND_UP(args->width * args->bpp, 8);
+	args->size = args->pitch * args->height;
+
+	cma_obj = drm_gem_cma_create_with_handle(file_priv, drm, args->size,
+						 &args->handle);
 	return PTR_ERR_OR_ZERO(cma_obj);
 }
 EXPORT_SYMBOL_GPL(drm_gem_cma_dumb_create);
 
-/*
- * drm_gem_cma_dumb_map_offset - (struct drm_driver)->dumb_map_offset callback
- * function
+/**
+ * drm_gem_cma_dumb_map_offset - return the fake mmap offset for a CMA GEM
+ *     object
+ * @file_priv: DRM file-private structure containing the GEM object
+ * @drm: DRM device
+ * @handle: GEM object handle
+ * @offset: return location for the fake mmap offset
+ *
+ * This function look up an object by its handle and returns the fake mmap
+ * offset associated with it. Drivers using the CMA helpers should set this
+ * as their DRM driver's ->dumb_map_offset() callback.
+ *
+ * Returns:
+ * 0 on success or a negative error code on failure.
  */
 int drm_gem_cma_dumb_map_offset(struct drm_file *file_priv,
-		struct drm_device *drm, uint32_t handle, uint64_t *offset)
+				struct drm_device *drm, u32 handle,
+				u64 *offset)
 {
 	struct drm_gem_object *gem_obj;
 
@@ -208,7 +293,7 @@ int drm_gem_cma_dumb_map_offset(struct drm_file *file_priv,
 
 	gem_obj = drm_gem_object_lookup(drm, file_priv, handle);
 	if (!gem_obj) {
-		dev_err(drm->dev, "failed to lookup gem object\n");
+		dev_err(drm->dev, "failed to lookup GEM object\n");
 		mutex_unlock(&drm->struct_mutex);
 		return -EINVAL;
 	}
@@ -251,8 +336,20 @@ static int drm_gem_cma_mmap_obj(struct drm_gem_cma_object *cma_obj,
 	return ret;
 }
 
-/*
- * drm_gem_cma_mmap - (struct file_operation)->mmap callback function
+/**
+ * drm_gem_cma_mmap - memory-map a CMA GEM object
+ * @filp: file object
+ * @vma: VMA for the area to be mapped
+ *
+ * This function implements an augmented version of the GEM DRM file mmap
+ * operation for CMA objects: In addition to the usual GEM VMA setup it
+ * immediately faults in the entire object instead of using on-demaind
+ * faulting. Drivers which employ the CMA helpers should use this function
+ * as their ->mmap() handler in the DRM device file's file_operations
+ * structure.
+ *
+ * Returns:
+ * 0 on success or a negative error code on failure.
  */
 int drm_gem_cma_mmap(struct file *filp, struct vm_area_struct *vma)
 {
@@ -272,7 +369,16 @@ int drm_gem_cma_mmap(struct file *filp, struct vm_area_struct *vma)
 EXPORT_SYMBOL_GPL(drm_gem_cma_mmap);
 
 #ifdef CONFIG_DEBUG_FS
-void drm_gem_cma_describe(struct drm_gem_cma_object *cma_obj, struct seq_file *m)
+/**
+ * drm_gem_cma_describe - describe a CMA GEM object for debugfs
+ * @cma_obj: CMA GEM object
+ * @m: debugfs file handle
+ *
+ * This function can be used to dump a human-readable representation of the
+ * CMA GEM object into a synthetic file.
+ */
+void drm_gem_cma_describe(struct drm_gem_cma_object *cma_obj,
+			  struct seq_file *m)
 {
 	struct drm_gem_object *obj = &cma_obj->base;
 	struct drm_device *dev = obj->dev;
@@ -291,7 +397,18 @@ void drm_gem_cma_describe(struct drm_gem_cma_object *cma_obj, struct seq_file *m
 EXPORT_SYMBOL_GPL(drm_gem_cma_describe);
 #endif
 
-/* low-level interface prime helpers */
+/**
+ * drm_gem_cma_prime_get_sg_table - provide a scatter/gather table of pinned
+ *     pages for a CMA GEM object
+ * @obj: GEM object
+ *
+ * This function exports a scatter/gather table suitable for PRIME usage by
+ * calling the standard DMA mapping API. Drivers using the CMA helpers should
+ * set this as their DRM driver's ->gem_prime_get_sg_table() callback.
+ *
+ * Returns:
+ * A pointer to the scatter/gather table of pinned pages or NULL on failure.
+ */
 struct sg_table *drm_gem_cma_prime_get_sg_table(struct drm_gem_object *obj)
 {
 	struct drm_gem_cma_object *cma_obj = to_drm_gem_cma_obj(obj);
@@ -315,6 +432,23 @@ out:
 }
 EXPORT_SYMBOL_GPL(drm_gem_cma_prime_get_sg_table);
 
+/**
+ * drm_gem_cma_prime_import_sg_table - produce a CMA GEM object from another
+ *     driver's scatter/gather table of pinned pages
+ * @dev: device to import into
+ * @attach: DMA-BUF attachment
+ * @sgt: scatter/gather table of pinned pages
+ *
+ * This function imports a scatter/gather table exported via DMA-BUF by
+ * another driver. Imported buffers must be physically contiguous in memory
+ * (i.e. the scatter/gather table must contain a single entry). Drivers that
+ * use the CMA helpers should set this as their DRM driver's
+ * ->gem_prime_import_sg_table() callback.
+ *
+ * Returns:
+ * A pointer to a newly created GEM object or an ERR_PTR-encoded negative
+ * error code on failure.
+ */
 struct drm_gem_object *
 drm_gem_cma_prime_import_sg_table(struct drm_device *dev,
 				  struct dma_buf_attachment *attach,
@@ -339,6 +473,18 @@ drm_gem_cma_prime_import_sg_table(struct drm_device *dev,
 }
 EXPORT_SYMBOL_GPL(drm_gem_cma_prime_import_sg_table);
 
+/**
+ * drm_gem_cma_prime_mmap - memory-map an exported CMA GEM object
+ * @obj: GEM object
+ * @vma: VMA for the area to be mapped
+ *
+ * This function maps a buffer imported via DRM PRIME into a userspace
+ * process's address space. Drivers that use the CMA helpers should set this
+ * as their DRM driver's ->gem_prime_mmap() callback.
+ *
+ * Returns:
+ * 0 on success or a negative error code on failure.
+ */
 int drm_gem_cma_prime_mmap(struct drm_gem_object *obj,
 			   struct vm_area_struct *vma)
 {
@@ -357,6 +503,20 @@ int drm_gem_cma_prime_mmap(struct drm_gem_object *obj,
 }
 EXPORT_SYMBOL_GPL(drm_gem_cma_prime_mmap);
 
+/**
+ * drm_gem_cma_prime_vmap - map a CMA GEM object into the kernel's virtual
+ *     address space
+ * @obj: GEM object
+ *
+ * This function maps a buffer exported via DRM PRIME into the kernel's
+ * virtual address space. Since the CMA buffers are already mapped into the
+ * kernel virtual address space this simply returns the cached virtual
+ * address. Drivers using the CMA helpers should set this as their DRM
+ * driver's ->gem_prime_vmap() callback.
+ *
+ * Returns:
+ * The kernel virtual address of the CMA GEM object's backing store.
+ */
 void *drm_gem_cma_prime_vmap(struct drm_gem_object *obj)
 {
 	struct drm_gem_cma_object *cma_obj = to_drm_gem_cma_obj(obj);
@@ -365,6 +525,17 @@ void *drm_gem_cma_prime_vmap(struct drm_gem_object *obj)
 }
 EXPORT_SYMBOL_GPL(drm_gem_cma_prime_vmap);
 
+/**
+ * drm_gem_cma_prime_vunmap - unmap a CMA GEM object from the kernel's virtual
+ *     address space
+ * @obj: GEM object
+ * @vaddr: kernel virtual address where the CMA GEM object was mapped
+ *
+ * This function removes a buffer exported via DRM PRIME from the kernel's
+ * virtual address space. This is a no-op because CMA buffers cannot be
+ * unmapped from kernel space. Drivers using the CMA helpers should set this
+ * as their DRM driver's ->gem_prime_vunmap() callback.
+ */
 void drm_gem_cma_prime_vunmap(struct drm_gem_object *obj, void *vaddr)
 {
 	/* Nothing to do */
diff --git a/drivers/gpu/drm/drm_irq.c b/drivers/gpu/drm/drm_irq.c
index 5ef03c216a27..4d79dad9d44f 100644
--- a/drivers/gpu/drm/drm_irq.c
+++ b/drivers/gpu/drm/drm_irq.c
@@ -166,7 +166,7 @@ static void vblank_disable_and_save(struct drm_device *dev, int crtc)
 	spin_lock_irqsave(&dev->vblank_time_lock, irqflags);
 
 	/*
-	 * If the vblank interrupt was already disbled update the count
+	 * If the vblank interrupt was already disabled update the count
 	 * and timestamp to maintain the appearance that the counter
 	 * has been ticking all along until this time. This makes the
 	 * count account for the entire time between drm_vblank_on() and
@@ -830,6 +830,8 @@ drm_get_last_vbltimestamp(struct drm_device *dev, int crtc,
  * vblank events since the system was booted, including lost events due to
  * modesetting activity.
  *
+ * This is the legacy version of drm_crtc_vblank_count().
+ *
  * Returns:
  * The software vblank counter.
  */
@@ -844,6 +846,25 @@ u32 drm_vblank_count(struct drm_device *dev, int crtc)
 EXPORT_SYMBOL(drm_vblank_count);
 
 /**
+ * drm_crtc_vblank_count - retrieve "cooked" vblank counter value
+ * @crtc: which counter to retrieve
+ *
+ * Fetches the "cooked" vblank count value that represents the number of
+ * vblank events since the system was booted, including lost events due to
+ * modesetting activity.
+ *
+ * This is the native KMS version of drm_vblank_count().
+ *
+ * Returns:
+ * The software vblank counter.
+ */
+u32 drm_crtc_vblank_count(struct drm_crtc *crtc)
+{
+	return drm_vblank_count(crtc->dev, drm_crtc_index(crtc));
+}
+EXPORT_SYMBOL(drm_crtc_vblank_count);
+
+/**
  * drm_vblank_count_and_time - retrieve "cooked" vblank counter value
  * and the system timestamp corresponding to that vblank counter value.
  *
@@ -904,6 +925,8 @@ static void send_vblank_event(struct drm_device *dev,
  *
  * Updates sequence # and timestamp on event, and sends it to userspace.
  * Caller must hold event lock.
+ *
+ * This is the legacy version of drm_crtc_send_vblank_event().
  */
 void drm_send_vblank_event(struct drm_device *dev, int crtc,
 		struct drm_pending_vblank_event *e)
@@ -923,6 +946,23 @@ void drm_send_vblank_event(struct drm_device *dev, int crtc,
 EXPORT_SYMBOL(drm_send_vblank_event);
 
 /**
+ * drm_crtc_send_vblank_event - helper to send vblank event after pageflip
+ * @crtc: the source CRTC of the vblank event
+ * @e: the event to send
+ *
+ * Updates sequence # and timestamp on event, and sends it to userspace.
+ * Caller must hold event lock.
+ *
+ * This is the native KMS version of drm_send_vblank_event().
+ */
+void drm_crtc_send_vblank_event(struct drm_crtc *crtc,
+				struct drm_pending_vblank_event *e)
+{
+	drm_send_vblank_event(crtc->dev, drm_crtc_index(crtc), e);
+}
+EXPORT_SYMBOL(drm_crtc_send_vblank_event);
+
+/**
  * drm_vblank_enable - enable the vblank interrupt on a CRTC
  * @dev: DRM device
  * @crtc: CRTC in question
@@ -1029,7 +1069,8 @@ void drm_vblank_put(struct drm_device *dev, int crtc)
 {
 	struct drm_vblank_crtc *vblank = &dev->vblank[crtc];
 
-	BUG_ON(atomic_read(&vblank->refcount) == 0);
+	if (WARN_ON(atomic_read(&vblank->refcount) == 0))
+		return;
 
 	if (WARN_ON(crtc >= dev->num_crtcs))
 		return;
@@ -1190,7 +1231,7 @@ EXPORT_SYMBOL(drm_crtc_vblank_off);
  *
  * This functions restores the vblank interrupt state captured with
  * drm_vblank_off() again. Note that calls to drm_vblank_on() and
- * drm_vblank_off() can be unbalanced and so can also be unconditionaly called
+ * drm_vblank_off() can be unbalanced and so can also be unconditionally called
  * in driver load code to reflect the current hardware state of the crtc.
  *
  * This is the legacy version of drm_crtc_vblank_on().
@@ -1237,7 +1278,7 @@ EXPORT_SYMBOL(drm_vblank_on);
  *
  * This functions restores the vblank interrupt state captured with
  * drm_vblank_off() again. Note that calls to drm_vblank_on() and
- * drm_vblank_off() can be unbalanced and so can also be unconditionaly called
+ * drm_vblank_off() can be unbalanced and so can also be unconditionally called
  * in driver load code to reflect the current hardware state of the crtc.
  *
  * This is the native kms version of drm_vblank_on().
@@ -1593,6 +1634,8 @@ static void drm_handle_vblank_events(struct drm_device *dev, int crtc)
  *
  * Drivers should call this routine in their vblank interrupt handlers to
  * update the vblank counter and send any signals that may be pending.
+ *
+ * This is the legacy version of drm_crtc_handle_vblank().
  */
 bool drm_handle_vblank(struct drm_device *dev, int crtc)
 {
@@ -1669,3 +1712,21 @@ bool drm_handle_vblank(struct drm_device *dev, int crtc)
 	return true;
 }
 EXPORT_SYMBOL(drm_handle_vblank);
+
+/**
+ * drm_crtc_handle_vblank - handle a vblank event
+ * @crtc: where this event occurred
+ *
+ * Drivers should call this routine in their vblank interrupt handlers to
+ * update the vblank counter and send any signals that may be pending.
+ *
+ * This is the native KMS version of drm_handle_vblank().
+ *
+ * Returns:
+ * True if the event was successfully handled, false on failure.
+ */
+bool drm_crtc_handle_vblank(struct drm_crtc *crtc)
+{
+	return drm_handle_vblank(crtc->dev, drm_crtc_index(crtc));
+}
+EXPORT_SYMBOL(drm_crtc_handle_vblank);
diff --git a/drivers/gpu/drm/drm_mipi_dsi.c b/drivers/gpu/drm/drm_mipi_dsi.c
index eb6dfe52cab2..c0644bb865f2 100644
--- a/drivers/gpu/drm/drm_mipi_dsi.c
+++ b/drivers/gpu/drm/drm_mipi_dsi.c
@@ -35,6 +35,16 @@
 
 #include <video/mipi_display.h>
 
+/**
+ * DOC: dsi helpers
+ *
+ * These functions contain some common logic and helpers to deal with MIPI DSI
+ * peripherals.
+ *
+ * Helpers are provided for a number of standard MIPI DSI command as well as a
+ * subset of the MIPI DCS command set.
+ */
+
 static int mipi_dsi_device_match(struct device *dev, struct device_driver *drv)
 {
 	return of_driver_match_device(dev, drv);
@@ -57,6 +67,29 @@ static struct bus_type mipi_dsi_bus_type = {
 	.pm = &mipi_dsi_device_pm_ops,
 };
 
+static int of_device_match(struct device *dev, void *data)
+{
+	return dev->of_node == data;
+}
+
+/**
+ * of_find_mipi_dsi_device_by_node() - find the MIPI DSI device matching a
+ *    device tree node
+ * @np: device tree node
+ *
+ * Return: A pointer to the MIPI DSI device corresponding to @np or NULL if no
+ *    such device exists (or has not been registered yet).
+ */
+struct mipi_dsi_device *of_find_mipi_dsi_device_by_node(struct device_node *np)
+{
+	struct device *dev;
+
+	dev = bus_find_device(&mipi_dsi_bus_type, NULL, np, of_device_match);
+
+	return dev ? to_mipi_dsi_device(dev) : NULL;
+}
+EXPORT_SYMBOL(of_find_mipi_dsi_device_by_node);
+
 static void mipi_dsi_dev_release(struct device *dev)
 {
 	struct mipi_dsi_device *dsi = to_mipi_dsi_device(dev);
@@ -198,59 +231,351 @@ int mipi_dsi_detach(struct mipi_dsi_device *dsi)
 }
 EXPORT_SYMBOL(mipi_dsi_detach);
 
+static ssize_t mipi_dsi_device_transfer(struct mipi_dsi_device *dsi,
+					struct mipi_dsi_msg *msg)
+{
+	const struct mipi_dsi_host_ops *ops = dsi->host->ops;
+
+	if (!ops || !ops->transfer)
+		return -ENOSYS;
+
+	if (dsi->mode_flags & MIPI_DSI_MODE_LPM)
+		msg->flags |= MIPI_DSI_MSG_USE_LPM;
+
+	return ops->transfer(dsi->host, msg);
+}
+
 /**
- * mipi_dsi_dcs_write - send DCS write command
- * @dsi: DSI device
- * @data: pointer to the command followed by parameters
- * @len: length of @data
+ * mipi_dsi_packet_format_is_short - check if a packet is of the short format
+ * @type: MIPI DSI data type of the packet
+ *
+ * Return: true if the packet for the given data type is a short packet, false
+ * otherwise.
  */
-ssize_t mipi_dsi_dcs_write(struct mipi_dsi_device *dsi, const void *data,
-			    size_t len)
+bool mipi_dsi_packet_format_is_short(u8 type)
+{
+	switch (type) {
+	case MIPI_DSI_V_SYNC_START:
+	case MIPI_DSI_V_SYNC_END:
+	case MIPI_DSI_H_SYNC_START:
+	case MIPI_DSI_H_SYNC_END:
+	case MIPI_DSI_END_OF_TRANSMISSION:
+	case MIPI_DSI_COLOR_MODE_OFF:
+	case MIPI_DSI_COLOR_MODE_ON:
+	case MIPI_DSI_SHUTDOWN_PERIPHERAL:
+	case MIPI_DSI_TURN_ON_PERIPHERAL:
+	case MIPI_DSI_GENERIC_SHORT_WRITE_0_PARAM:
+	case MIPI_DSI_GENERIC_SHORT_WRITE_1_PARAM:
+	case MIPI_DSI_GENERIC_SHORT_WRITE_2_PARAM:
+	case MIPI_DSI_GENERIC_READ_REQUEST_0_PARAM:
+	case MIPI_DSI_GENERIC_READ_REQUEST_1_PARAM:
+	case MIPI_DSI_GENERIC_READ_REQUEST_2_PARAM:
+	case MIPI_DSI_DCS_SHORT_WRITE:
+	case MIPI_DSI_DCS_SHORT_WRITE_PARAM:
+	case MIPI_DSI_DCS_READ:
+	case MIPI_DSI_SET_MAXIMUM_RETURN_PACKET_SIZE:
+		return true;
+	}
+
+	return false;
+}
+EXPORT_SYMBOL(mipi_dsi_packet_format_is_short);
+
+/**
+ * mipi_dsi_packet_format_is_long - check if a packet is of the long format
+ * @type: MIPI DSI data type of the packet
+ *
+ * Return: true if the packet for the given data type is a long packet, false
+ * otherwise.
+ */
+bool mipi_dsi_packet_format_is_long(u8 type)
+{
+	switch (type) {
+	case MIPI_DSI_NULL_PACKET:
+	case MIPI_DSI_BLANKING_PACKET:
+	case MIPI_DSI_GENERIC_LONG_WRITE:
+	case MIPI_DSI_DCS_LONG_WRITE:
+	case MIPI_DSI_LOOSELY_PACKED_PIXEL_STREAM_YCBCR20:
+	case MIPI_DSI_PACKED_PIXEL_STREAM_YCBCR24:
+	case MIPI_DSI_PACKED_PIXEL_STREAM_YCBCR16:
+	case MIPI_DSI_PACKED_PIXEL_STREAM_30:
+	case MIPI_DSI_PACKED_PIXEL_STREAM_36:
+	case MIPI_DSI_PACKED_PIXEL_STREAM_YCBCR12:
+	case MIPI_DSI_PACKED_PIXEL_STREAM_16:
+	case MIPI_DSI_PACKED_PIXEL_STREAM_18:
+	case MIPI_DSI_PIXEL_STREAM_3BYTE_18:
+	case MIPI_DSI_PACKED_PIXEL_STREAM_24:
+		return true;
+	}
+
+	return false;
+}
+EXPORT_SYMBOL(mipi_dsi_packet_format_is_long);
+
+/**
+ * mipi_dsi_create_packet - create a packet from a message according to the
+ *     DSI protocol
+ * @packet: pointer to a DSI packet structure
+ * @msg: message to translate into a packet
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_create_packet(struct mipi_dsi_packet *packet,
+			   const struct mipi_dsi_msg *msg)
+{
+	const u8 *tx = msg->tx_buf;
+
+	if (!packet || !msg)
+		return -EINVAL;
+
+	/* do some minimum sanity checking */
+	if (!mipi_dsi_packet_format_is_short(msg->type) &&
+	    !mipi_dsi_packet_format_is_long(msg->type))
+		return -EINVAL;
+
+	if (msg->channel > 3)
+		return -EINVAL;
+
+	memset(packet, 0, sizeof(*packet));
+	packet->header[0] = ((msg->channel & 0x3) << 6) | (msg->type & 0x3f);
+
+	/* TODO: compute ECC if hardware support is not available */
+
+	/*
+	 * Long write packets contain the word count in header bytes 1 and 2.
+	 * The payload follows the header and is word count bytes long.
+	 *
+	 * Short write packets encode up to two parameters in header bytes 1
+	 * and 2.
+	 */
+	if (mipi_dsi_packet_format_is_long(msg->type)) {
+		packet->header[1] = (msg->tx_len >> 0) & 0xff;
+		packet->header[2] = (msg->tx_len >> 8) & 0xff;
+
+		packet->payload_length = msg->tx_len;
+		packet->payload = tx;
+	} else {
+		packet->header[1] = (msg->tx_len > 0) ? tx[0] : 0;
+		packet->header[2] = (msg->tx_len > 1) ? tx[1] : 0;
+	}
+
+	packet->size = sizeof(packet->header) + packet->payload_length;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_create_packet);
+
+/*
+ * mipi_dsi_set_maximum_return_packet_size() - specify the maximum size of the
+ *    the payload in a long packet transmitted from the peripheral back to the
+ *    host processor
+ * @dsi: DSI peripheral device
+ * @value: the maximum size of the payload
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_set_maximum_return_packet_size(struct mipi_dsi_device *dsi,
+					    u16 value)
+{
+	u8 tx[2] = { value & 0xff, value >> 8 };
+	struct mipi_dsi_msg msg = {
+		.channel = dsi->channel,
+		.type = MIPI_DSI_SET_MAXIMUM_RETURN_PACKET_SIZE,
+		.tx_len = sizeof(tx),
+		.tx_buf = tx,
+	};
+
+	return mipi_dsi_device_transfer(dsi, &msg);
+}
+EXPORT_SYMBOL(mipi_dsi_set_maximum_return_packet_size);
+
+/**
+ * mipi_dsi_generic_write() - transmit data using a generic write packet
+ * @dsi: DSI peripheral device
+ * @payload: buffer containing the payload
+ * @size: size of payload buffer
+ *
+ * This function will automatically choose the right data type depending on
+ * the payload length.
+ *
+ * Return: The number of bytes transmitted on success or a negative error code
+ * on failure.
+ */
+ssize_t mipi_dsi_generic_write(struct mipi_dsi_device *dsi, const void *payload,
+			       size_t size)
+{
+	struct mipi_dsi_msg msg = {
+		.channel = dsi->channel,
+		.tx_buf = payload,
+		.tx_len = size
+	};
+
+	switch (size) {
+	case 0:
+		msg.type = MIPI_DSI_GENERIC_SHORT_WRITE_0_PARAM;
+		break;
+
+	case 1:
+		msg.type = MIPI_DSI_GENERIC_SHORT_WRITE_1_PARAM;
+		break;
+
+	case 2:
+		msg.type = MIPI_DSI_GENERIC_SHORT_WRITE_2_PARAM;
+		break;
+
+	default:
+		msg.type = MIPI_DSI_GENERIC_LONG_WRITE;
+		break;
+	}
+
+	return mipi_dsi_device_transfer(dsi, &msg);
+}
+EXPORT_SYMBOL(mipi_dsi_generic_write);
+
+/**
+ * mipi_dsi_generic_read() - receive data using a generic read packet
+ * @dsi: DSI peripheral device
+ * @params: buffer containing the request parameters
+ * @num_params: number of request parameters
+ * @data: buffer in which to return the received data
+ * @size: size of receive buffer
+ *
+ * This function will automatically choose the right data type depending on
+ * the number of parameters passed in.
+ *
+ * Return: The number of bytes successfully read or a negative error code on
+ * failure.
+ */
+ssize_t mipi_dsi_generic_read(struct mipi_dsi_device *dsi, const void *params,
+			      size_t num_params, void *data, size_t size)
+{
+	struct mipi_dsi_msg msg = {
+		.channel = dsi->channel,
+		.tx_len = num_params,
+		.tx_buf = params,
+		.rx_len = size,
+		.rx_buf = data
+	};
+
+	switch (num_params) {
+	case 0:
+		msg.type = MIPI_DSI_GENERIC_READ_REQUEST_0_PARAM;
+		break;
+
+	case 1:
+		msg.type = MIPI_DSI_GENERIC_READ_REQUEST_1_PARAM;
+		break;
+
+	case 2:
+		msg.type = MIPI_DSI_GENERIC_READ_REQUEST_2_PARAM;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return mipi_dsi_device_transfer(dsi, &msg);
+}
+EXPORT_SYMBOL(mipi_dsi_generic_read);
+
+/**
+ * mipi_dsi_dcs_write_buffer() - transmit a DCS command with payload
+ * @dsi: DSI peripheral device
+ * @data: buffer containing data to be transmitted
+ * @len: size of transmission buffer
+ *
+ * This function will automatically choose the right data type depending on
+ * the command payload length.
+ *
+ * Return: The number of bytes successfully transmitted or a negative error
+ * code on failure.
+ */
+ssize_t mipi_dsi_dcs_write_buffer(struct mipi_dsi_device *dsi,
+				  const void *data, size_t len)
 {
-	const struct mipi_dsi_host_ops *ops = dsi->host->ops;
 	struct mipi_dsi_msg msg = {
 		.channel = dsi->channel,
 		.tx_buf = data,
 		.tx_len = len
 	};
 
-	if (!ops || !ops->transfer)
-		return -ENOSYS;
-
 	switch (len) {
 	case 0:
 		return -EINVAL;
+
 	case 1:
 		msg.type = MIPI_DSI_DCS_SHORT_WRITE;
 		break;
+
 	case 2:
 		msg.type = MIPI_DSI_DCS_SHORT_WRITE_PARAM;
 		break;
+
 	default:
 		msg.type = MIPI_DSI_DCS_LONG_WRITE;
 		break;
 	}
 
-	if (dsi->mode_flags & MIPI_DSI_MODE_LPM)
-		msg.flags = MIPI_DSI_MSG_USE_LPM;
+	return mipi_dsi_device_transfer(dsi, &msg);
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_write_buffer);
 
-	return ops->transfer(dsi->host, &msg);
+/**
+ * mipi_dsi_dcs_write() - send DCS write command
+ * @dsi: DSI peripheral device
+ * @cmd: DCS command
+ * @data: buffer containing the command payload
+ * @len: command payload length
+ *
+ * This function will automatically choose the right data type depending on
+ * the command payload length.
+ *
+ * Return: The number of bytes successfully transmitted or a negative error
+ * code on failure.
+ */
+ssize_t mipi_dsi_dcs_write(struct mipi_dsi_device *dsi, u8 cmd,
+			   const void *data, size_t len)
+{
+	ssize_t err;
+	size_t size;
+	u8 *tx;
+
+	if (len > 0) {
+		size = 1 + len;
+
+		tx = kmalloc(size, GFP_KERNEL);
+		if (!tx)
+			return -ENOMEM;
+
+		/* concatenate the DCS command byte and the payload */
+		tx[0] = cmd;
+		memcpy(&tx[1], data, len);
+	} else {
+		tx = &cmd;
+		size = 1;
+	}
+
+	err = mipi_dsi_dcs_write_buffer(dsi, tx, size);
+
+	if (len > 0)
+		kfree(tx);
+
+	return err;
 }
 EXPORT_SYMBOL(mipi_dsi_dcs_write);
 
 /**
- * mipi_dsi_dcs_read - send DCS read request command
- * @dsi: DSI device
- * @cmd: DCS read command
- * @data: pointer to read buffer
- * @len: length of @data
+ * mipi_dsi_dcs_read() - send DCS read request command
+ * @dsi: DSI peripheral device
+ * @cmd: DCS command
+ * @data: buffer in which to receive data
+ * @len: size of receive buffer
  *
- * Function returns number of read bytes or error code.
+ * Return: The number of bytes read or a negative error code on failure.
  */
 ssize_t mipi_dsi_dcs_read(struct mipi_dsi_device *dsi, u8 cmd, void *data,
 			  size_t len)
 {
-	const struct mipi_dsi_host_ops *ops = dsi->host->ops;
 	struct mipi_dsi_msg msg = {
 		.channel = dsi->channel,
 		.type = MIPI_DSI_DCS_READ,
@@ -260,15 +585,282 @@ ssize_t mipi_dsi_dcs_read(struct mipi_dsi_device *dsi, u8 cmd, void *data,
 		.rx_len = len
 	};
 
-	if (!ops || !ops->transfer)
-		return -ENOSYS;
+	return mipi_dsi_device_transfer(dsi, &msg);
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_read);
 
-	if (dsi->mode_flags & MIPI_DSI_MODE_LPM)
-		msg.flags = MIPI_DSI_MSG_USE_LPM;
+/**
+ * mipi_dsi_dcs_nop() - send DCS nop packet
+ * @dsi: DSI peripheral device
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_nop(struct mipi_dsi_device *dsi)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_NOP, NULL, 0);
+	if (err < 0)
+		return err;
 
-	return ops->transfer(dsi->host, &msg);
+	return 0;
 }
-EXPORT_SYMBOL(mipi_dsi_dcs_read);
+EXPORT_SYMBOL(mipi_dsi_dcs_nop);
+
+/**
+ * mipi_dsi_dcs_soft_reset() - perform a software reset of the display module
+ * @dsi: DSI peripheral device
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_soft_reset(struct mipi_dsi_device *dsi)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_SOFT_RESET, NULL, 0);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_soft_reset);
+
+/**
+ * mipi_dsi_dcs_get_power_mode() - query the display module's current power
+ *    mode
+ * @dsi: DSI peripheral device
+ * @mode: return location for the current power mode
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_get_power_mode(struct mipi_dsi_device *dsi, u8 *mode)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_read(dsi, MIPI_DCS_GET_POWER_MODE, mode,
+				sizeof(*mode));
+	if (err <= 0) {
+		if (err == 0)
+			err = -ENODATA;
+
+		return err;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_get_power_mode);
+
+/**
+ * mipi_dsi_dcs_get_pixel_format() - gets the pixel format for the RGB image
+ *    data used by the interface
+ * @dsi: DSI peripheral device
+ * @format: return location for the pixel format
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_get_pixel_format(struct mipi_dsi_device *dsi, u8 *format)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_read(dsi, MIPI_DCS_GET_PIXEL_FORMAT, format,
+				sizeof(*format));
+	if (err <= 0) {
+		if (err == 0)
+			err = -ENODATA;
+
+		return err;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_get_pixel_format);
+
+/**
+ * mipi_dsi_dcs_enter_sleep_mode() - disable all unnecessary blocks inside the
+ *    display module except interface communication
+ * @dsi: DSI peripheral device
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_enter_sleep_mode(struct mipi_dsi_device *dsi)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_ENTER_SLEEP_MODE, NULL, 0);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_enter_sleep_mode);
+
+/**
+ * mipi_dsi_dcs_exit_sleep_mode() - enable all blocks inside the display
+ *    module
+ * @dsi: DSI peripheral device
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_exit_sleep_mode(struct mipi_dsi_device *dsi)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_EXIT_SLEEP_MODE, NULL, 0);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_exit_sleep_mode);
+
+/**
+ * mipi_dsi_dcs_set_display_off() - stop displaying the image data on the
+ *    display device
+ * @dsi: DSI peripheral device
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_set_display_off(struct mipi_dsi_device *dsi)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_SET_DISPLAY_OFF, NULL, 0);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_set_display_off);
+
+/**
+ * mipi_dsi_dcs_set_display_on() - start displaying the image data on the
+ *    display device
+ * @dsi: DSI peripheral device
+ *
+ * Return: 0 on success or a negative error code on failure
+ */
+int mipi_dsi_dcs_set_display_on(struct mipi_dsi_device *dsi)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_SET_DISPLAY_ON, NULL, 0);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_set_display_on);
+
+/**
+ * mipi_dsi_dcs_set_column_address() - define the column extent of the frame
+ *    memory accessed by the host processor
+ * @dsi: DSI peripheral device
+ * @start: first column of frame memory
+ * @end: last column of frame memory
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_set_column_address(struct mipi_dsi_device *dsi, u16 start,
+				    u16 end)
+{
+	u8 payload[4] = { start >> 8, start & 0xff, end >> 8, end & 0xff };
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_SET_COLUMN_ADDRESS, payload,
+				 sizeof(payload));
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_set_column_address);
+
+/**
+ * mipi_dsi_dcs_set_page_address() - define the page extent of the frame
+ *    memory accessed by the host processor
+ * @dsi: DSI peripheral device
+ * @start: first page of frame memory
+ * @end: last page of frame memory
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_set_page_address(struct mipi_dsi_device *dsi, u16 start,
+				  u16 end)
+{
+	u8 payload[4] = { start >> 8, start & 0xff, end >> 8, end & 0xff };
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_SET_PAGE_ADDRESS, payload,
+				 sizeof(payload));
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_set_page_address);
+
+/**
+ * mipi_dsi_dcs_set_tear_off() - turn off the display module's Tearing Effect
+ *    output signal on the TE signal line
+ * @dsi: DSI peripheral device
+ *
+ * Return: 0 on success or a negative error code on failure
+ */
+int mipi_dsi_dcs_set_tear_off(struct mipi_dsi_device *dsi)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_SET_TEAR_OFF, NULL, 0);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_set_tear_off);
+
+/**
+ * mipi_dsi_dcs_set_tear_on() - turn on the display module's Tearing Effect
+ *    output signal on the TE signal line.
+ * @dsi: DSI peripheral device
+ * @mode: the Tearing Effect Output Line mode
+ *
+ * Return: 0 on success or a negative error code on failure
+ */
+int mipi_dsi_dcs_set_tear_on(struct mipi_dsi_device *dsi,
+			     enum mipi_dsi_dcs_tear_mode mode)
+{
+	u8 value = mode;
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_SET_TEAR_ON, &value,
+				 sizeof(value));
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_set_tear_on);
+
+/**
+ * mipi_dsi_dcs_set_pixel_format() - sets the pixel format for the RGB image
+ *    data used by the interface
+ * @dsi: DSI peripheral device
+ * @format: pixel format
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_dcs_set_pixel_format(struct mipi_dsi_device *dsi, u8 format)
+{
+	ssize_t err;
+
+	err = mipi_dsi_dcs_write(dsi, MIPI_DCS_SET_PIXEL_FORMAT, &format,
+				 sizeof(format));
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+EXPORT_SYMBOL(mipi_dsi_dcs_set_pixel_format);
 
 static int mipi_dsi_drv_probe(struct device *dev)
 {
@@ -295,12 +887,18 @@ static void mipi_dsi_drv_shutdown(struct device *dev)
 }
 
 /**
- * mipi_dsi_driver_register - register a driver for DSI devices
+ * mipi_dsi_driver_register_full() - register a driver for DSI devices
  * @drv: DSI driver structure
+ * @owner: owner module
+ *
+ * Return: 0 on success or a negative error code on failure.
  */
-int mipi_dsi_driver_register(struct mipi_dsi_driver *drv)
+int mipi_dsi_driver_register_full(struct mipi_dsi_driver *drv,
+				  struct module *owner)
 {
 	drv->driver.bus = &mipi_dsi_bus_type;
+	drv->driver.owner = owner;
+
 	if (drv->probe)
 		drv->driver.probe = mipi_dsi_drv_probe;
 	if (drv->remove)
@@ -310,11 +908,13 @@ int mipi_dsi_driver_register(struct mipi_dsi_driver *drv)
 
 	return driver_register(&drv->driver);
 }
-EXPORT_SYMBOL(mipi_dsi_driver_register);
+EXPORT_SYMBOL(mipi_dsi_driver_register_full);
 
 /**
- * mipi_dsi_driver_unregister - unregister a driver for DSI devices
+ * mipi_dsi_driver_unregister() - unregister a driver for DSI devices
  * @drv: DSI driver structure
+ *
+ * Return: 0 on success or a negative error code on failure.
  */
 void mipi_dsi_driver_unregister(struct mipi_dsi_driver *drv)
 {
diff --git a/drivers/gpu/drm/drm_modes.c b/drivers/gpu/drm/drm_modes.c
index d1b7d2006529..6d8b941c8200 100644
--- a/drivers/gpu/drm/drm_modes.c
+++ b/drivers/gpu/drm/drm_modes.c
@@ -914,7 +914,7 @@ EXPORT_SYMBOL(drm_mode_equal_no_clocks_no_stereo);
  *
  * This function is a helper which can be used to validate modes against size
  * limitations of the DRM device/connector. If a mode is too big its status
- * memeber is updated with the appropriate validation failure code. The list
+ * member is updated with the appropriate validation failure code. The list
  * itself is not changed.
  */
 void drm_mode_validate_size(struct drm_device *dev,
diff --git a/drivers/gpu/drm/drm_modeset_lock.c b/drivers/gpu/drm/drm_modeset_lock.c
index 474e4d12a2d8..51cc47d827d8 100644
--- a/drivers/gpu/drm/drm_modeset_lock.c
+++ b/drivers/gpu/drm/drm_modeset_lock.c
@@ -157,14 +157,20 @@ void drm_modeset_unlock_all(struct drm_device *dev)
 EXPORT_SYMBOL(drm_modeset_unlock_all);
 
 /**
- * drm_modeset_lock_crtc - lock crtc with hidden acquire ctx
- * @crtc: drm crtc
+ * drm_modeset_lock_crtc - lock crtc with hidden acquire ctx for a plane update
+ * @crtc: DRM CRTC
+ * @plane: DRM plane to be updated on @crtc
+ *
+ * This function locks the given crtc and plane (which should be either the
+ * primary or cursor plane) using a hidden acquire context. This is necessary so
+ * that drivers internally using the atomic interfaces can grab further locks
+ * with the lock acquire context.
  *
- * This function locks the given crtc using a hidden acquire context. This is
- * necessary so that drivers internally using the atomic interfaces can grab
- * further locks with the lock acquire context.
+ * Note that @plane can be NULL, e.g. when the cursor support hasn't yet been
+ * converted to universal planes yet.
  */
-void drm_modeset_lock_crtc(struct drm_crtc *crtc)
+void drm_modeset_lock_crtc(struct drm_crtc *crtc,
+			   struct drm_plane *plane)
 {
 	struct drm_modeset_acquire_ctx *ctx;
 	int ret;
@@ -180,6 +186,18 @@ retry:
 	if (ret)
 		goto fail;
 
+	if (plane) {
+		ret = drm_modeset_lock(&plane->mutex, ctx);
+		if (ret)
+			goto fail;
+
+		if (plane->crtc) {
+			ret = drm_modeset_lock(&plane->crtc->mutex, ctx);
+			if (ret)
+				goto fail;
+		}
+	}
+
 	WARN_ON(crtc->acquire_ctx);
 
 	/* now we hold the locks, so now that it is safe, stash the
@@ -437,15 +455,14 @@ void drm_modeset_unlock(struct drm_modeset_lock *lock)
 }
 EXPORT_SYMBOL(drm_modeset_unlock);
 
-/* Temporary.. until we have sufficiently fine grained locking, there
- * are a couple scenarios where it is convenient to grab all crtc locks.
- * It is planned to remove this:
- */
+/* In some legacy codepaths it's convenient to just grab all the crtc and plane
+ * related locks. */
 int drm_modeset_lock_all_crtcs(struct drm_device *dev,
 		struct drm_modeset_acquire_ctx *ctx)
 {
 	struct drm_mode_config *config = &dev->mode_config;
 	struct drm_crtc *crtc;
+	struct drm_plane *plane;
 	int ret = 0;
 
 	list_for_each_entry(crtc, &config->crtc_list, head) {
@@ -454,6 +471,12 @@ int drm_modeset_lock_all_crtcs(struct drm_device *dev,
 			return ret;
 	}
 
+	list_for_each_entry(plane, &config->plane_list, head) {
+		ret = drm_modeset_lock(&plane->mutex, ctx);
+		if (ret)
+			return ret;
+	}
+
 	return 0;
 }
 EXPORT_SYMBOL(drm_modeset_lock_all_crtcs);
diff --git a/drivers/gpu/drm/drm_plane_helper.c b/drivers/gpu/drm/drm_plane_helper.c
index 827ec1a3040b..18a1ac6ac22f 100644
--- a/drivers/gpu/drm/drm_plane_helper.c
+++ b/drivers/gpu/drm/drm_plane_helper.c
@@ -27,10 +27,38 @@
 #include <drm/drmP.h>
 #include <drm/drm_plane_helper.h>
 #include <drm/drm_rect.h>
-#include <drm/drm_plane_helper.h>
+#include <drm/drm_atomic.h>
+#include <drm/drm_crtc_helper.h>
+#include <drm/drm_atomic_helper.h>
 
 #define SUBPIXEL_MASK 0xffff
 
+/**
+ * DOC: overview
+ *
+ * This helper library has two parts. The first part has support to implement
+ * primary plane support on top of the normal CRTC configuration interface.
+ * Since the legacy ->set_config interface ties the primary plane together with
+ * the CRTC state this does not allow userspace to disable the primary plane
+ * itself.  To avoid too much duplicated code use
+ * drm_plane_helper_check_update() which can be used to enforce the same
+ * restrictions as primary planes had thus. The default primary plane only
+ * expose XRBG8888 and ARGB8888 as valid pixel formats for the attached
+ * framebuffer.
+ *
+ * Drivers are highly recommended to implement proper support for primary
+ * planes, and newly merged drivers must not rely upon these transitional
+ * helpers.
+ *
+ * The second part also implements transitional helpers which allow drivers to
+ * gradually switch to the atomic helper infrastructure for plane updates. Once
+ * that switch is complete drivers shouldn't use these any longer, instead using
+ * the proper legacy implementations for update and disable plane hooks provided
+ * by the atomic helpers.
+ *
+ * Again drivers are strongly urged to switch to the new interfaces.
+ */
+
 /*
  * This is the minimal list of formats that seem to be safe for modeset use
  * with all current DRM drivers.  Most hardware can actually support more
@@ -127,6 +155,11 @@ int drm_plane_helper_check_update(struct drm_plane *plane,
 		return -ERANGE;
 	}
 
+	if (!fb) {
+		*visible = false;
+		return 0;
+	}
+
 	*visible = drm_rect_clip_scaled(src, dest, clip, hscale, vscale);
 	if (!*visible)
 		/*
@@ -369,3 +402,171 @@ int drm_crtc_init(struct drm_device *dev, struct drm_crtc *crtc,
 	return drm_crtc_init_with_planes(dev, crtc, primary, NULL, funcs);
 }
 EXPORT_SYMBOL(drm_crtc_init);
+
+int drm_plane_helper_commit(struct drm_plane *plane,
+			    struct drm_plane_state *plane_state,
+			    struct drm_framebuffer *old_fb)
+{
+	struct drm_plane_helper_funcs *plane_funcs;
+	struct drm_crtc *crtc[2];
+	struct drm_crtc_helper_funcs *crtc_funcs[2];
+	int i, ret = 0;
+
+	plane_funcs = plane->helper_private;
+
+	/* Since this is a transitional helper we can't assume that plane->state
+	 * is always valid. Hence we need to use plane->crtc instead of
+	 * plane->state->crtc as the old crtc. */
+	crtc[0] = plane->crtc;
+	crtc[1] = crtc[0] != plane_state->crtc ? plane_state->crtc : NULL;
+
+	for (i = 0; i < 2; i++)
+		crtc_funcs[i] = crtc[i] ? crtc[i]->helper_private : NULL;
+
+	if (plane_funcs->atomic_check) {
+		ret = plane_funcs->atomic_check(plane, plane_state);
+		if (ret)
+			goto out;
+	}
+
+	if (plane_funcs->prepare_fb && plane_state->fb) {
+		ret = plane_funcs->prepare_fb(plane, plane_state->fb);
+		if (ret)
+			goto out;
+	}
+
+	/* Point of no return, commit sw state. */
+	swap(plane->state, plane_state);
+
+	for (i = 0; i < 2; i++) {
+		if (crtc_funcs[i] && crtc_funcs[i]->atomic_begin)
+			crtc_funcs[i]->atomic_begin(crtc[i]);
+	}
+
+	plane_funcs->atomic_update(plane, plane_state);
+
+	for (i = 0; i < 2; i++) {
+		if (crtc_funcs[i] && crtc_funcs[i]->atomic_flush)
+			crtc_funcs[i]->atomic_flush(crtc[i]);
+	}
+
+	for (i = 0; i < 2; i++) {
+		if (!crtc[i])
+			continue;
+
+		/* There's no other way to figure out whether the crtc is running. */
+		ret = drm_crtc_vblank_get(crtc[i]);
+		if (ret == 0) {
+			drm_crtc_wait_one_vblank(crtc[i]);
+			drm_crtc_vblank_put(crtc[i]);
+		}
+
+		ret = 0;
+	}
+
+	if (plane_funcs->cleanup_fb && old_fb)
+		plane_funcs->cleanup_fb(plane, old_fb);
+out:
+	if (plane_state) {
+		if (plane->funcs->atomic_destroy_state)
+			plane->funcs->atomic_destroy_state(plane, plane_state);
+		else
+			drm_atomic_helper_plane_destroy_state(plane, plane_state);
+	}
+
+	return ret;
+}
+
+/**
+ * drm_plane_helper_update() - Helper for primary plane update
+ * @plane: plane object to update
+ * @crtc: owning CRTC of owning plane
+ * @fb: framebuffer to flip onto plane
+ * @crtc_x: x offset of primary plane on crtc
+ * @crtc_y: y offset of primary plane on crtc
+ * @crtc_w: width of primary plane rectangle on crtc
+ * @crtc_h: height of primary plane rectangle on crtc
+ * @src_x: x offset of @fb for panning
+ * @src_y: y offset of @fb for panning
+ * @src_w: width of source rectangle in @fb
+ * @src_h: height of source rectangle in @fb
+ *
+ * Provides a default plane update handler using the atomic plane update
+ * functions. It is fully left to the driver to check plane constraints and
+ * handle corner-cases like a fully occluded or otherwise invisible plane.
+ *
+ * This is useful for piecewise transitioning of a driver to the atomic helpers.
+ *
+ * RETURNS:
+ * Zero on success, error code on failure
+ */
+int drm_plane_helper_update(struct drm_plane *plane, struct drm_crtc *crtc,
+			    struct drm_framebuffer *fb,
+			    int crtc_x, int crtc_y,
+			    unsigned int crtc_w, unsigned int crtc_h,
+			    uint32_t src_x, uint32_t src_y,
+			    uint32_t src_w, uint32_t src_h)
+{
+	struct drm_plane_state *plane_state;
+
+	if (plane->funcs->atomic_duplicate_state)
+		plane_state = plane->funcs->atomic_duplicate_state(plane);
+	else if (plane->state)
+		plane_state = drm_atomic_helper_plane_duplicate_state(plane);
+	else
+		plane_state = kzalloc(sizeof(*plane_state), GFP_KERNEL);
+	if (!plane_state)
+		return -ENOMEM;
+
+	plane_state->crtc = crtc;
+	drm_atomic_set_fb_for_plane(plane_state, fb);
+	plane_state->crtc_x = crtc_x;
+	plane_state->crtc_y = crtc_y;
+	plane_state->crtc_h = crtc_h;
+	plane_state->crtc_w = crtc_w;
+	plane_state->src_x = src_x;
+	plane_state->src_y = src_y;
+	plane_state->src_h = src_h;
+	plane_state->src_w = src_w;
+
+	return drm_plane_helper_commit(plane, plane_state, plane->fb);
+}
+EXPORT_SYMBOL(drm_plane_helper_update);
+
+/**
+ * drm_plane_helper_disable() - Helper for primary plane disable
+ * @plane: plane to disable
+ *
+ * Provides a default plane disable handler using the atomic plane update
+ * functions. It is fully left to the driver to check plane constraints and
+ * handle corner-cases like a fully occluded or otherwise invisible plane.
+ *
+ * This is useful for piecewise transitioning of a driver to the atomic helpers.
+ *
+ * RETURNS:
+ * Zero on success, error code on failure
+ */
+int drm_plane_helper_disable(struct drm_plane *plane)
+{
+	struct drm_plane_state *plane_state;
+
+	/* crtc helpers love to call disable functions for already disabled hw
+	 * functions. So cope with that. */
+	if (!plane->crtc)
+		return 0;
+
+	if (plane->funcs->atomic_duplicate_state)
+		plane_state = plane->funcs->atomic_duplicate_state(plane);
+	else if (plane->state)
+		plane_state = drm_atomic_helper_plane_duplicate_state(plane);
+	else
+		plane_state = kzalloc(sizeof(*plane_state), GFP_KERNEL);
+	if (!plane_state)
+		return -ENOMEM;
+
+	plane_state->crtc = NULL;
+	drm_atomic_set_fb_for_plane(plane_state, NULL);
+
+	return drm_plane_helper_commit(plane, plane_state, plane->fb);
+}
+EXPORT_SYMBOL(drm_plane_helper_disable);
diff --git a/drivers/gpu/drm/drm_prime.c b/drivers/gpu/drm/drm_prime.c
index 78ca30808422..7482b06cd08f 100644
--- a/drivers/gpu/drm/drm_prime.c
+++ b/drivers/gpu/drm/drm_prime.c
@@ -328,7 +328,7 @@ static const struct dma_buf_ops drm_gem_prime_dmabuf_ops =  {
  */
 
 /**
- * drm_gem_prime_export - helper library implemention of the export callback
+ * drm_gem_prime_export - helper library implementation of the export callback
  * @dev: drm_device to export from
  * @obj: GEM object to export
  * @flags: flags like DRM_CLOEXEC
@@ -483,7 +483,7 @@ out_unlock:
 EXPORT_SYMBOL(drm_gem_prime_handle_to_fd);
 
 /**
- * drm_gem_prime_import - helper library implemention of the import callback
+ * drm_gem_prime_import - helper library implementation of the import callback
  * @dev: drm_device to import into
  * @dma_buf: dma-buf object to import
  *
@@ -669,7 +669,7 @@ int drm_prime_fd_to_handle_ioctl(struct drm_device *dev, void *data,
  * the driver is responsible for mapping the pages into the
  * importers address space for use with dma_buf itself.
  */
-struct sg_table *drm_prime_pages_to_sg(struct page **pages, int nr_pages)
+struct sg_table *drm_prime_pages_to_sg(struct page **pages, unsigned int nr_pages)
 {
 	struct sg_table *sg = NULL;
 	int ret;
diff --git a/drivers/gpu/drm/drm_probe_helper.c b/drivers/gpu/drm/drm_probe_helper.c
index 6857e9ad6339..7483a47de8e4 100644
--- a/drivers/gpu/drm/drm_probe_helper.c
+++ b/drivers/gpu/drm/drm_probe_helper.c
@@ -118,7 +118,8 @@ static int drm_helper_probe_single_connector_modes_merge_bits(struct drm_connect
 		mode->status = MODE_UNVERIFIED;
 
 	if (connector->force) {
-		if (connector->force == DRM_FORCE_ON)
+		if (connector->force == DRM_FORCE_ON ||
+		    connector->force == DRM_FORCE_ON_DIGITAL)
 			connector->status = connector_status_connected;
 		else
 			connector->status = connector_status_disconnected;
diff --git a/drivers/gpu/drm/exynos/exynos_dp_core.c b/drivers/gpu/drm/exynos/exynos_dp_core.c
index 6adb1e5cfb08..34d46aa75416 100644
--- a/drivers/gpu/drm/exynos/exynos_dp_core.c
+++ b/drivers/gpu/drm/exynos/exynos_dp_core.c
@@ -30,12 +30,17 @@
 #include <drm/drm_panel.h>
 #include <drm/bridge/ptn3460.h>
 
-#include "exynos_drm_drv.h"
 #include "exynos_dp_core.h"
 
 #define ctx_from_connector(c)	container_of(c, struct exynos_dp_device, \
 					connector)
 
+static inline struct exynos_dp_device *
+display_to_dp(struct exynos_drm_display *d)
+{
+	return container_of(d, struct exynos_dp_device, display);
+}
+
 struct bridge_init {
 	struct i2c_client *client;
 	struct device_node *node;
@@ -882,7 +887,7 @@ static void exynos_dp_hotplug(struct work_struct *work)
 
 static void exynos_dp_commit(struct exynos_drm_display *display)
 {
-	struct exynos_dp_device *dp = display->ctx;
+	struct exynos_dp_device *dp = display_to_dp(display);
 	int ret;
 
 	/* Keep the panel disabled while we configure video */
@@ -1020,7 +1025,7 @@ static int exynos_drm_attach_lcd_bridge(struct drm_device *dev,
 static int exynos_dp_create_connector(struct exynos_drm_display *display,
 				struct drm_encoder *encoder)
 {
-	struct exynos_dp_device *dp = display->ctx;
+	struct exynos_dp_device *dp = display_to_dp(display);
 	struct drm_connector *connector = &dp->connector;
 	int ret;
 
@@ -1052,33 +1057,19 @@ static int exynos_dp_create_connector(struct exynos_drm_display *display,
 
 static void exynos_dp_phy_init(struct exynos_dp_device *dp)
 {
-	if (dp->phy) {
+	if (dp->phy)
 		phy_power_on(dp->phy);
-	} else if (dp->phy_addr) {
-		u32 reg;
-
-		reg = __raw_readl(dp->phy_addr);
-		reg |= dp->enable_mask;
-		__raw_writel(reg, dp->phy_addr);
-	}
 }
 
 static void exynos_dp_phy_exit(struct exynos_dp_device *dp)
 {
-	if (dp->phy) {
+	if (dp->phy)
 		phy_power_off(dp->phy);
-	} else if (dp->phy_addr) {
-		u32 reg;
-
-		reg = __raw_readl(dp->phy_addr);
-		reg &= ~(dp->enable_mask);
-		__raw_writel(reg, dp->phy_addr);
-	}
 }
 
 static void exynos_dp_poweron(struct exynos_drm_display *display)
 {
-	struct exynos_dp_device *dp = display->ctx;
+	struct exynos_dp_device *dp = display_to_dp(display);
 
 	if (dp->dpms_mode == DRM_MODE_DPMS_ON)
 		return;
@@ -1099,7 +1090,7 @@ static void exynos_dp_poweron(struct exynos_drm_display *display)
 
 static void exynos_dp_poweroff(struct exynos_drm_display *display)
 {
-	struct exynos_dp_device *dp = display->ctx;
+	struct exynos_dp_device *dp = display_to_dp(display);
 
 	if (dp->dpms_mode != DRM_MODE_DPMS_ON)
 		return;
@@ -1124,7 +1115,7 @@ static void exynos_dp_poweroff(struct exynos_drm_display *display)
 
 static void exynos_dp_dpms(struct exynos_drm_display *display, int mode)
 {
-	struct exynos_dp_device *dp = display->ctx;
+	struct exynos_dp_device *dp = display_to_dp(display);
 
 	switch (mode) {
 	case DRM_MODE_DPMS_ON:
@@ -1147,11 +1138,6 @@ static struct exynos_drm_display_ops exynos_dp_display_ops = {
 	.commit = exynos_dp_commit,
 };
 
-static struct exynos_drm_display exynos_dp_display = {
-	.type = EXYNOS_DISPLAY_TYPE_LCD,
-	.ops = &exynos_dp_display_ops,
-};
-
 static struct video_info *exynos_dp_dt_parse_pdata(struct device *dev)
 {
 	struct device_node *dp_node = dev->of_node;
@@ -1210,44 +1196,6 @@ static struct video_info *exynos_dp_dt_parse_pdata(struct device *dev)
 	return dp_video_config;
 }
 
-static int exynos_dp_dt_parse_phydata(struct exynos_dp_device *dp)
-{
-	struct device_node *dp_phy_node = of_node_get(dp->dev->of_node);
-	u32 phy_base;
-	int ret = 0;
-
-	dp_phy_node = of_find_node_by_name(dp_phy_node, "dptx-phy");
-	if (!dp_phy_node) {
-		dp->phy = devm_phy_get(dp->dev, "dp");
-		return PTR_ERR_OR_ZERO(dp->phy);
-	}
-
-	if (of_property_read_u32(dp_phy_node, "reg", &phy_base)) {
-		dev_err(dp->dev, "failed to get reg for dptx-phy\n");
-		ret = -EINVAL;
-		goto err;
-	}
-
-	if (of_property_read_u32(dp_phy_node, "samsung,enable-mask",
-				&dp->enable_mask)) {
-		dev_err(dp->dev, "failed to get enable-mask for dptx-phy\n");
-		ret = -EINVAL;
-		goto err;
-	}
-
-	dp->phy_addr = ioremap(phy_base, SZ_4);
-	if (!dp->phy_addr) {
-		dev_err(dp->dev, "failed to ioremap dp-phy\n");
-		ret = -ENOMEM;
-		goto err;
-	}
-
-err:
-	of_node_put(dp_phy_node);
-
-	return ret;
-}
-
 static int exynos_dp_dt_parse_panel(struct exynos_dp_device *dp)
 {
 	int ret;
@@ -1263,10 +1211,10 @@ static int exynos_dp_dt_parse_panel(struct exynos_dp_device *dp)
 
 static int exynos_dp_bind(struct device *dev, struct device *master, void *data)
 {
+	struct exynos_dp_device *dp = dev_get_drvdata(dev);
 	struct platform_device *pdev = to_platform_device(dev);
 	struct drm_device *drm_dev = data;
 	struct resource *res;
-	struct exynos_dp_device *dp = exynos_dp_display.ctx;
 	unsigned int irq_flags;
 	int ret = 0;
 
@@ -1277,9 +1225,21 @@ static int exynos_dp_bind(struct device *dev, struct device *master, void *data)
 	if (IS_ERR(dp->video_info))
 		return PTR_ERR(dp->video_info);
 
-	ret = exynos_dp_dt_parse_phydata(dp);
-	if (ret)
-		return ret;
+	dp->phy = devm_phy_get(dp->dev, "dp");
+	if (IS_ERR(dp->phy)) {
+		dev_err(dp->dev, "no DP phy configured\n");
+		ret = PTR_ERR(dp->phy);
+		if (ret) {
+			/*
+			 * phy itself is not enabled, so we can move forward
+			 * assigning NULL to phy pointer.
+			 */
+			if (ret == -ENOSYS || ret == -ENODEV)
+				dp->phy = NULL;
+			else
+				return ret;
+		}
+	}
 
 	if (!dp->panel) {
 		ret = exynos_dp_dt_parse_panel(dp);
@@ -1346,17 +1306,15 @@ static int exynos_dp_bind(struct device *dev, struct device *master, void *data)
 
 	dp->drm_dev = drm_dev;
 
-	platform_set_drvdata(pdev, &exynos_dp_display);
-
-	return exynos_drm_create_enc_conn(drm_dev, &exynos_dp_display);
+	return exynos_drm_create_enc_conn(drm_dev, &dp->display);
 }
 
 static void exynos_dp_unbind(struct device *dev, struct device *master,
 				void *data)
 {
-	struct exynos_drm_display *display = dev_get_drvdata(dev);
+	struct exynos_dp_device *dp = dev_get_drvdata(dev);
 
-	exynos_dp_dpms(display, DRM_MODE_DPMS_OFF);
+	exynos_dp_dpms(&dp->display, DRM_MODE_DPMS_OFF);
 }
 
 static const struct component_ops exynos_dp_ops = {
@@ -1371,16 +1329,20 @@ static int exynos_dp_probe(struct platform_device *pdev)
 	struct exynos_dp_device *dp;
 	int ret;
 
-	ret = exynos_drm_component_add(&pdev->dev, EXYNOS_DEVICE_TYPE_CONNECTOR,
-					exynos_dp_display.type);
-	if (ret)
-		return ret;
-
 	dp = devm_kzalloc(&pdev->dev, sizeof(struct exynos_dp_device),
 				GFP_KERNEL);
 	if (!dp)
 		return -ENOMEM;
 
+	dp->display.type = EXYNOS_DISPLAY_TYPE_LCD;
+	dp->display.ops = &exynos_dp_display_ops;
+	platform_set_drvdata(pdev, dp);
+
+	ret = exynos_drm_component_add(&pdev->dev, EXYNOS_DEVICE_TYPE_CONNECTOR,
+					dp->display.type);
+	if (ret)
+		return ret;
+
 	panel_node = of_parse_phandle(dev->of_node, "panel", 0);
 	if (panel_node) {
 		dp->panel = of_drm_find_panel(panel_node);
@@ -1389,8 +1351,6 @@ static int exynos_dp_probe(struct platform_device *pdev)
 			return -EPROBE_DEFER;
 	}
 
-	exynos_dp_display.ctx = dp;
-
 	ret = component_add(&pdev->dev, &exynos_dp_ops);
 	if (ret)
 		exynos_drm_component_del(&pdev->dev,
@@ -1410,19 +1370,17 @@ static int exynos_dp_remove(struct platform_device *pdev)
 #ifdef CONFIG_PM_SLEEP
 static int exynos_dp_suspend(struct device *dev)
 {
-	struct platform_device *pdev = to_platform_device(dev);
-	struct exynos_drm_display *display = platform_get_drvdata(pdev);
+	struct exynos_dp_device *dp = dev_get_drvdata(dev);
 
-	exynos_dp_dpms(display, DRM_MODE_DPMS_OFF);
+	exynos_dp_dpms(&dp->display, DRM_MODE_DPMS_OFF);
 	return 0;
 }
 
 static int exynos_dp_resume(struct device *dev)
 {
-	struct platform_device *pdev = to_platform_device(dev);
-	struct exynos_drm_display *display = platform_get_drvdata(pdev);
+	struct exynos_dp_device *dp = dev_get_drvdata(dev);
 
-	exynos_dp_dpms(display, DRM_MODE_DPMS_ON);
+	exynos_dp_dpms(&dp->display, DRM_MODE_DPMS_ON);
 	return 0;
 }
 #endif
diff --git a/drivers/gpu/drm/exynos/exynos_dp_core.h b/drivers/gpu/drm/exynos/exynos_dp_core.h
index a1aee6931bd7..164f171168e7 100644
--- a/drivers/gpu/drm/exynos/exynos_dp_core.h
+++ b/drivers/gpu/drm/exynos/exynos_dp_core.h
@@ -17,6 +17,8 @@
 #include <drm/drm_dp_helper.h>
 #include <drm/exynos_drm.h>
 
+#include "exynos_drm_drv.h"
+
 #define DP_TIMEOUT_LOOP_COUNT 100
 #define MAX_CR_LOOP 5
 #define MAX_EQ_LOOP 5
@@ -145,6 +147,7 @@ struct link_train {
 };
 
 struct exynos_dp_device {
+	struct exynos_drm_display display;
 	struct device		*dev;
 	struct drm_device	*drm_dev;
 	struct drm_connector	connector;
@@ -153,8 +156,6 @@ struct exynos_dp_device {
 	struct clk		*clock;
 	unsigned int		irq;
 	void __iomem		*reg_base;
-	void __iomem		*phy_addr;
-	unsigned int		enable_mask;
 
 	struct video_info	*video_info;
 	struct link_train	link_train;
diff --git a/drivers/gpu/drm/exynos/exynos_drm_crtc.h b/drivers/gpu/drm/exynos/exynos_drm_crtc.h
index 690dcddab725..e353d353836f 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_crtc.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_crtc.h
@@ -15,10 +15,7 @@
 #ifndef _EXYNOS_DRM_CRTC_H_
 #define _EXYNOS_DRM_CRTC_H_
 
-struct drm_device;
-struct drm_crtc;
-struct exynos_drm_manager;
-struct exynos_drm_overlay;
+#include "exynos_drm_drv.h"
 
 int exynos_drm_crtc_create(struct exynos_drm_manager *manager);
 int exynos_drm_crtc_enable_vblank(struct drm_device *dev, int pipe);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_dpi.c b/drivers/gpu/drm/exynos/exynos_drm_dpi.c
index 3dc678ed9949..37678cf4425a 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_dpi.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_dpi.c
@@ -22,6 +22,7 @@
 #include "exynos_drm_drv.h"
 
 struct exynos_dpi {
+	struct exynos_drm_display display;
 	struct device *dev;
 	struct device_node *panel_node;
 
@@ -35,6 +36,11 @@ struct exynos_dpi {
 
 #define connector_to_dpi(c) container_of(c, struct exynos_dpi, connector)
 
+static inline struct exynos_dpi *display_to_dpi(struct exynos_drm_display *d)
+{
+	return container_of(d, struct exynos_dpi, display);
+}
+
 static enum drm_connector_status
 exynos_dpi_detect(struct drm_connector *connector, bool force)
 {
@@ -100,7 +106,7 @@ static struct drm_connector_helper_funcs exynos_dpi_connector_helper_funcs = {
 static int exynos_dpi_create_connector(struct exynos_drm_display *display,
 				       struct drm_encoder *encoder)
 {
-	struct exynos_dpi *ctx = display->ctx;
+	struct exynos_dpi *ctx = display_to_dpi(display);
 	struct drm_connector *connector = &ctx->connector;
 	int ret;
 
@@ -141,7 +147,7 @@ static void exynos_dpi_poweroff(struct exynos_dpi *ctx)
 
 static void exynos_dpi_dpms(struct exynos_drm_display *display, int mode)
 {
-	struct exynos_dpi *ctx = display->ctx;
+	struct exynos_dpi *ctx = display_to_dpi(display);
 
 	switch (mode) {
 	case DRM_MODE_DPMS_ON:
@@ -165,11 +171,6 @@ static struct exynos_drm_display_ops exynos_dpi_display_ops = {
 	.dpms = exynos_dpi_dpms
 };
 
-static struct exynos_drm_display exynos_dpi_display = {
-	.type = EXYNOS_DISPLAY_TYPE_LCD,
-	.ops = &exynos_dpi_display_ops,
-};
-
 /* of_* functions will be removed after merge of of_graph patches */
 static struct device_node *
 of_get_child_by_name_reg(struct device_node *parent, const char *name, u32 reg)
@@ -299,20 +300,21 @@ struct exynos_drm_display *exynos_dpi_probe(struct device *dev)
 	struct exynos_dpi *ctx;
 	int ret;
 
-	ret = exynos_drm_component_add(dev,
-					EXYNOS_DEVICE_TYPE_CONNECTOR,
-					exynos_dpi_display.type);
-	if (ret)
-		return ERR_PTR(ret);
-
 	ctx = devm_kzalloc(dev, sizeof(*ctx), GFP_KERNEL);
 	if (!ctx)
-		goto err_del_component;
+		return ERR_PTR(-ENOMEM);
 
+	ctx->display.type = EXYNOS_DISPLAY_TYPE_LCD;
+	ctx->display.ops = &exynos_dpi_display_ops;
 	ctx->dev = dev;
-	exynos_dpi_display.ctx = ctx;
 	ctx->dpms_mode = DRM_MODE_DPMS_OFF;
 
+	ret = exynos_drm_component_add(dev,
+					EXYNOS_DEVICE_TYPE_CONNECTOR,
+					ctx->display.type);
+	if (ret)
+		return ERR_PTR(ret);
+
 	ret = exynos_dpi_parse_dt(ctx);
 	if (ret < 0) {
 		devm_kfree(dev, ctx);
@@ -328,7 +330,7 @@ struct exynos_drm_display *exynos_dpi_probe(struct device *dev)
 		}
 	}
 
-	return &exynos_dpi_display;
+	return &ctx->display;
 
 err_del_component:
 	exynos_drm_component_del(dev, EXYNOS_DEVICE_TYPE_CONNECTOR);
@@ -336,16 +338,16 @@ err_del_component:
 	return NULL;
 }
 
-int exynos_dpi_remove(struct device *dev)
+int exynos_dpi_remove(struct exynos_drm_display *display)
 {
-	struct exynos_dpi *ctx = exynos_dpi_display.ctx;
+	struct exynos_dpi *ctx = display_to_dpi(display);
 
-	exynos_dpi_dpms(&exynos_dpi_display, DRM_MODE_DPMS_OFF);
+	exynos_dpi_dpms(&ctx->display, DRM_MODE_DPMS_OFF);
 
 	if (ctx->panel)
 		drm_panel_detach(ctx->panel);
 
-	exynos_drm_component_del(dev, EXYNOS_DEVICE_TYPE_CONNECTOR);
+	exynos_drm_component_del(ctx->dev, EXYNOS_DEVICE_TYPE_CONNECTOR);
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/exynos/exynos_drm_drv.c b/drivers/gpu/drm/exynos/exynos_drm_drv.c
index e5c4c6c8c967..121470a83d1a 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_drv.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_drv.c
@@ -203,8 +203,6 @@ static int exynos_drm_resume(struct drm_device *dev)
 	}
 	drm_modeset_unlock_all(dev);
 
-	drm_helper_resume_force_mode(dev);
-
 	return 0;
 }
 
@@ -475,8 +473,6 @@ void exynos_drm_component_del(struct device *dev,
 			list_del(&cdev->list);
 			kfree(cdev);
 		}
-
-		break;
 	}
 
 	mutex_unlock(&drm_component_lock);
@@ -556,187 +552,72 @@ static const struct component_master_ops exynos_drm_ops = {
 	.unbind		= exynos_drm_unbind,
 };
 
-static int exynos_drm_platform_probe(struct platform_device *pdev)
-{
-	struct component_match *match;
-	int ret;
-
-	pdev->dev.coherent_dma_mask = DMA_BIT_MASK(32);
-	exynos_drm_driver.num_ioctls = ARRAY_SIZE(exynos_ioctls);
-
+static struct platform_driver *const exynos_drm_kms_drivers[] = {
 #ifdef CONFIG_DRM_EXYNOS_FIMD
-	ret = platform_driver_register(&fimd_driver);
-	if (ret < 0)
-		return ret;
+	&fimd_driver,
 #endif
-
 #ifdef CONFIG_DRM_EXYNOS_DP
-	ret = platform_driver_register(&dp_driver);
-	if (ret < 0)
-		goto err_unregister_fimd_drv;
+	&dp_driver,
 #endif
-
 #ifdef CONFIG_DRM_EXYNOS_DSI
-	ret = platform_driver_register(&dsi_driver);
-	if (ret < 0)
-		goto err_unregister_dp_drv;
+	&dsi_driver,
 #endif
-
 #ifdef CONFIG_DRM_EXYNOS_HDMI
-	ret = platform_driver_register(&mixer_driver);
-	if (ret < 0)
-		goto err_unregister_dsi_drv;
-	ret = platform_driver_register(&hdmi_driver);
-	if (ret < 0)
-		goto err_unregister_mixer_drv;
+	&mixer_driver,
+	&hdmi_driver,
 #endif
+};
 
-	match = exynos_drm_match_add(&pdev->dev);
-	if (IS_ERR(match)) {
-		ret = PTR_ERR(match);
-		goto err_unregister_hdmi_drv;
-	}
-
-	ret = component_master_add_with_match(&pdev->dev, &exynos_drm_ops,
-						match);
-	if (ret < 0)
-		goto err_unregister_hdmi_drv;
-
+static struct platform_driver *const exynos_drm_non_kms_drivers[] = {
 #ifdef CONFIG_DRM_EXYNOS_G2D
-	ret = platform_driver_register(&g2d_driver);
-	if (ret < 0)
-		goto err_del_component_master;
+	&g2d_driver,
 #endif
-
 #ifdef CONFIG_DRM_EXYNOS_FIMC
-	ret = platform_driver_register(&fimc_driver);
-	if (ret < 0)
-		goto err_unregister_g2d_drv;
+	&fimc_driver,
 #endif
-
 #ifdef CONFIG_DRM_EXYNOS_ROTATOR
-	ret = platform_driver_register(&rotator_driver);
-	if (ret < 0)
-		goto err_unregister_fimc_drv;
+	&rotator_driver,
 #endif
-
 #ifdef CONFIG_DRM_EXYNOS_GSC
-	ret = platform_driver_register(&gsc_driver);
-	if (ret < 0)
-		goto err_unregister_rotator_drv;
-#endif
-
-#ifdef CONFIG_DRM_EXYNOS_IPP
-	ret = platform_driver_register(&ipp_driver);
-	if (ret < 0)
-		goto err_unregister_gsc_drv;
-
-	ret = exynos_platform_device_ipp_register();
-	if (ret < 0)
-		goto err_unregister_ipp_drv;
+	&gsc_driver,
 #endif
-
-	return ret;
-
 #ifdef CONFIG_DRM_EXYNOS_IPP
-err_unregister_ipp_drv:
-	platform_driver_unregister(&ipp_driver);
-err_unregister_gsc_drv:
-#endif
-
-#ifdef CONFIG_DRM_EXYNOS_GSC
-	platform_driver_unregister(&gsc_driver);
-err_unregister_rotator_drv:
+	&ipp_driver,
 #endif
+};
 
-#ifdef CONFIG_DRM_EXYNOS_ROTATOR
-	platform_driver_unregister(&rotator_driver);
-err_unregister_fimc_drv:
-#endif
-
-#ifdef CONFIG_DRM_EXYNOS_FIMC
-	platform_driver_unregister(&fimc_driver);
-err_unregister_g2d_drv:
-#endif
-
-#ifdef CONFIG_DRM_EXYNOS_G2D
-	platform_driver_unregister(&g2d_driver);
-err_del_component_master:
-#endif
-	component_master_del(&pdev->dev, &exynos_drm_ops);
-
-err_unregister_hdmi_drv:
-#ifdef CONFIG_DRM_EXYNOS_HDMI
-	platform_driver_unregister(&hdmi_driver);
-err_unregister_mixer_drv:
-	platform_driver_unregister(&mixer_driver);
-err_unregister_dsi_drv:
-#endif
+static int exynos_drm_platform_probe(struct platform_device *pdev)
+{
+	struct component_match *match;
 
-#ifdef CONFIG_DRM_EXYNOS_DSI
-	platform_driver_unregister(&dsi_driver);
-err_unregister_dp_drv:
-#endif
+	pdev->dev.coherent_dma_mask = DMA_BIT_MASK(32);
+	exynos_drm_driver.num_ioctls = ARRAY_SIZE(exynos_ioctls);
 
-#ifdef CONFIG_DRM_EXYNOS_DP
-	platform_driver_unregister(&dp_driver);
-err_unregister_fimd_drv:
-#endif
+	match = exynos_drm_match_add(&pdev->dev);
+	if (IS_ERR(match)) {
+		return PTR_ERR(match);
+	}
 
-#ifdef CONFIG_DRM_EXYNOS_FIMD
-	platform_driver_unregister(&fimd_driver);
-#endif
-	return ret;
+	return component_master_add_with_match(&pdev->dev, &exynos_drm_ops,
+					       match);
 }
 
 static int exynos_drm_platform_remove(struct platform_device *pdev)
 {
-#ifdef CONFIG_DRM_EXYNOS_IPP
-	exynos_platform_device_ipp_unregister();
-	platform_driver_unregister(&ipp_driver);
-#endif
-
-#ifdef CONFIG_DRM_EXYNOS_GSC
-	platform_driver_unregister(&gsc_driver);
-#endif
-
-#ifdef CONFIG_DRM_EXYNOS_ROTATOR
-	platform_driver_unregister(&rotator_driver);
-#endif
-
-#ifdef CONFIG_DRM_EXYNOS_FIMC
-	platform_driver_unregister(&fimc_driver);
-#endif
-
-#ifdef CONFIG_DRM_EXYNOS_G2D
-	platform_driver_unregister(&g2d_driver);
-#endif
-
-#ifdef CONFIG_DRM_EXYNOS_HDMI
-	platform_driver_unregister(&mixer_driver);
-	platform_driver_unregister(&hdmi_driver);
-#endif
-
-#ifdef CONFIG_DRM_EXYNOS_FIMD
-	platform_driver_unregister(&fimd_driver);
-#endif
-
-#ifdef CONFIG_DRM_EXYNOS_DSI
-	platform_driver_unregister(&dsi_driver);
-#endif
-
-#ifdef CONFIG_DRM_EXYNOS_DP
-	platform_driver_unregister(&dp_driver);
-#endif
 	component_master_del(&pdev->dev, &exynos_drm_ops);
 	return 0;
 }
 
+static const char * const strings[] = {
+	"samsung,exynos3",
+	"samsung,exynos4",
+	"samsung,exynos5",
+};
+
 static struct platform_driver exynos_drm_platform_driver = {
 	.probe	= exynos_drm_platform_probe,
 	.remove	= exynos_drm_platform_remove,
 	.driver	= {
-		.owner	= THIS_MODULE,
 		.name	= "exynos-drm",
 		.pm	= &exynos_drm_pm_ops,
 	},
@@ -744,7 +625,25 @@ static struct platform_driver exynos_drm_platform_driver = {
 
 static int exynos_drm_init(void)
 {
-	int ret;
+	bool is_exynos = false;
+	int ret, i, j;
+
+	/*
+	 * Register device object only in case of Exynos SoC.
+	 *
+	 * Below codes resolves temporarily infinite loop issue incurred
+	 * by Exynos drm driver when using multi-platform kernel.
+	 * So these codes will be replaced with more generic way later.
+	 */
+	for (i = 0; i < ARRAY_SIZE(strings); i++) {
+		if (of_machine_is_compatible(strings[i])) {
+			is_exynos = true;
+			break;
+		}
+	}
+
+	if (!is_exynos)
+		return -ENODEV;
 
 	/*
 	 * Register device object only in case of Exynos SoC.
@@ -763,24 +662,50 @@ static int exynos_drm_init(void)
 	if (IS_ERR(exynos_drm_pdev))
 		return PTR_ERR(exynos_drm_pdev);
 
-#ifdef CONFIG_DRM_EXYNOS_VIDI
 	ret = exynos_drm_probe_vidi();
 	if (ret < 0)
 		goto err_unregister_pd;
+
+	for (i = 0; i < ARRAY_SIZE(exynos_drm_kms_drivers); ++i) {
+		ret = platform_driver_register(exynos_drm_kms_drivers[i]);
+		if (ret < 0)
+			goto err_unregister_kms_drivers;
+	}
+
+	for (j = 0; j < ARRAY_SIZE(exynos_drm_non_kms_drivers); ++j) {
+		ret = platform_driver_register(exynos_drm_non_kms_drivers[j]);
+		if (ret < 0)
+			goto err_unregister_non_kms_drivers;
+	}
+
+#ifdef CONFIG_DRM_EXYNOS_IPP
+	ret = exynos_platform_device_ipp_register();
+	if (ret < 0)
+		goto err_unregister_non_kms_drivers;
 #endif
 
 	ret = platform_driver_register(&exynos_drm_platform_driver);
 	if (ret)
-		goto err_remove_vidi;
+		goto err_unregister_resources;
 
 	return 0;
 
-err_remove_vidi:
-#ifdef CONFIG_DRM_EXYNOS_VIDI
+err_unregister_resources:
+#ifdef CONFIG_DRM_EXYNOS_IPP
+	exynos_platform_device_ipp_unregister();
+#endif
+
+err_unregister_non_kms_drivers:
+	while (--j >= 0)
+		platform_driver_unregister(exynos_drm_non_kms_drivers[j]);
+
+err_unregister_kms_drivers:
+	while (--i >= 0)
+		platform_driver_unregister(exynos_drm_kms_drivers[i]);
+
 	exynos_drm_remove_vidi();
 
 err_unregister_pd:
-#endif
 	platform_device_unregister(exynos_drm_pdev);
 
 	return ret;
@@ -788,10 +713,22 @@ err_unregister_pd:
 
 static void exynos_drm_exit(void)
 {
+	int i;
+
+#ifdef CONFIG_DRM_EXYNOS_IPP
+	exynos_platform_device_ipp_unregister();
+#endif
+
+	for (i = ARRAY_SIZE(exynos_drm_non_kms_drivers) - 1; i >= 0; --i)
+		platform_driver_unregister(exynos_drm_non_kms_drivers[i]);
+
+	for (i = ARRAY_SIZE(exynos_drm_kms_drivers) - 1; i >= 0; --i)
+		platform_driver_unregister(exynos_drm_kms_drivers[i]);
+
 	platform_driver_unregister(&exynos_drm_platform_driver);
-#ifdef CONFIG_DRM_EXYNOS_VIDI
+
 	exynos_drm_remove_vidi();
-#endif
+
 	platform_device_unregister(exynos_drm_pdev);
 }
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_drv.h b/drivers/gpu/drm/exynos/exynos_drm_drv.h
index d22e640f59a0..2e5063488c50 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_drv.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_drv.h
@@ -15,6 +15,7 @@
 #ifndef _EXYNOS_DRM_DRV_H_
 #define _EXYNOS_DRM_DRV_H_
 
+#include <drm/drmP.h>
 #include <linux/module.h>
 
 #define MAX_CRTC	3
@@ -22,24 +23,6 @@
 #define MAX_FB_BUFFER	4
 #define DEFAULT_ZPOS	-1
 
-#define _wait_for(COND, MS) ({ \
-	unsigned long timeout__ = jiffies + msecs_to_jiffies(MS);	\
-	int ret__ = 0;							\
-	while (!(COND)) {						\
-		if (time_after(jiffies, timeout__)) {			\
-			ret__ = -ETIMEDOUT;				\
-			break;						\
-		}							\
-	}								\
-	ret__;								\
-})
-
-#define wait_for(COND, MS) _wait_for(COND, MS)
-
-struct drm_device;
-struct exynos_drm_overlay;
-struct drm_connector;
-
 /* This enumerates device type. */
 enum exynos_drm_device_type {
 	EXYNOS_DEVICE_TYPE_NONE,
@@ -83,10 +66,10 @@ enum exynos_drm_output_type {
  * @dma_addr: array of bus(accessed by dma) address to the memory region
  *	      allocated for a overlay.
  * @zpos: order of overlay layer(z position).
- * @default_win: a window to be enabled.
- * @color_key: color key on or off.
  * @index_color: if using color key feature then this value would be used
  *			as index color.
+ * @default_win: a window to be enabled.
+ * @color_key: color key on or off.
  * @local_path: in case of lcd type, local path mode on or off.
  * @transparency: transparency on or off.
  * @activated: activated or not.
@@ -114,19 +97,20 @@ struct exynos_drm_overlay {
 	uint32_t pixel_format;
 	dma_addr_t dma_addr[MAX_FB_BUFFER];
 	int zpos;
-
-	bool default_win;
-	bool color_key;
 	unsigned int index_color;
-	bool local_path;
-	bool transparency;
-	bool activated;
+
+	bool default_win:1;
+	bool color_key:1;
+	bool local_path:1;
+	bool transparency:1;
+	bool activated:1;
 };
 
 /*
  * Exynos DRM Display Structure.
  *	- this structure is common to analog tv, digital tv and lcd panel.
  *
+ * @create_connector: initialize and register a new connector
  * @remove: cleans up the display for removal
  * @mode_fixup: fix mode data comparing to hw specific display mode.
  * @mode_set: convert drm_display_mode to hw specific display mode and
@@ -168,7 +152,6 @@ struct exynos_drm_display {
 	struct drm_encoder *encoder;
 	struct drm_connector *connector;
 	struct exynos_drm_display_ops *ops;
-	void *ctx;
 };
 
 /*
@@ -227,7 +210,6 @@ struct exynos_drm_manager {
 	struct drm_crtc *crtc;
 	int pipe;
 	struct exynos_drm_manager_ops *ops;
-	void *ctx;
 };
 
 struct exynos_drm_g2d_private {
@@ -279,8 +261,6 @@ struct exynos_drm_private {
  * @dev: pointer to device object for subdrv device driver.
  * @drm_dev: pointer to drm_device and this pointer would be set
  *	when sub driver calls exynos_drm_subdrv_register().
- * @manager: subdrv has its own manager to control a hardware appropriately
- *     and we can access a hardware drawing on this manager.
  * @probe: this callback would be called by exynos drm driver after
  *     subdrv is registered to it.
  * @remove: this callback is used to release resources created
@@ -312,45 +292,34 @@ int exynos_drm_device_subdrv_remove(struct drm_device *dev);
 int exynos_drm_subdrv_open(struct drm_device *dev, struct drm_file *file);
 void exynos_drm_subdrv_close(struct drm_device *dev, struct drm_file *file);
 
-/*
- * this function registers exynos drm hdmi platform device. It ensures only one
- * instance of the device is created.
- */
-int exynos_platform_device_hdmi_register(void);
-
-/*
- * this function unregisters exynos drm hdmi platform device if it exists.
- */
-void exynos_platform_device_hdmi_unregister(void);
-
-/*
- * this function registers exynos drm ipp platform device.
- */
+#ifdef CONFIG_DRM_EXYNOS_IPP
 int exynos_platform_device_ipp_register(void);
-
-/*
- * this function unregisters exynos drm ipp platform device if it exists.
- */
 void exynos_platform_device_ipp_unregister(void);
+#else
+static inline int exynos_platform_device_ipp_register(void) { return 0; }
+static inline void exynos_platform_device_ipp_unregister(void) {}
+#endif
+
 
 #ifdef CONFIG_DRM_EXYNOS_DPI
 struct exynos_drm_display * exynos_dpi_probe(struct device *dev);
-int exynos_dpi_remove(struct device *dev);
+int exynos_dpi_remove(struct exynos_drm_display *display);
 #else
 static inline struct exynos_drm_display *
 exynos_dpi_probe(struct device *dev) { return NULL; }
-static inline int exynos_dpi_remove(struct device *dev) { return 0; }
+static inline int exynos_dpi_remove(struct exynos_drm_display *display)
+{
+	return 0;
+}
 #endif
 
-/*
- * this function registers exynos drm vidi platform device/driver.
- */
+#ifdef CONFIG_DRM_EXYNOS_VIDI
 int exynos_drm_probe_vidi(void);
-
-/*
- * this function unregister exynos drm vidi platform device/driver.
- */
 void exynos_drm_remove_vidi(void);
+#else
+static inline int exynos_drm_probe_vidi(void) { return 0; }
+static inline void exynos_drm_remove_vidi(void) {}
+#endif
 
 /* This function creates a encoder and a connector, and initializes them. */
 int exynos_drm_create_enc_conn(struct drm_device *dev,
diff --git a/drivers/gpu/drm/exynos/exynos_drm_dsi.c b/drivers/gpu/drm/exynos/exynos_drm_dsi.c
index acf7e9e39dcd..05fe93dc57a8 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_dsi.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_dsi.c
@@ -268,9 +268,9 @@ struct exynos_dsi_driver_data {
 };
 
 struct exynos_dsi {
+	struct exynos_drm_display display;
 	struct mipi_dsi_host dsi_host;
 	struct drm_connector connector;
-	struct drm_encoder *encoder;
 	struct device_node *panel_node;
 	struct drm_panel *panel;
 	struct device *dev;
@@ -304,6 +304,11 @@ struct exynos_dsi {
 #define host_to_dsi(host) container_of(host, struct exynos_dsi, dsi_host)
 #define connector_to_dsi(c) container_of(c, struct exynos_dsi, connector)
 
+static inline struct exynos_dsi *display_to_dsi(struct exynos_drm_display *d)
+{
+	return container_of(d, struct exynos_dsi, display);
+}
+
 static struct exynos_dsi_driver_data exynos3_dsi_driver_data = {
 	.plltmr_reg = 0x50,
 	.has_freqband = 1,
@@ -316,6 +321,11 @@ static struct exynos_dsi_driver_data exynos4_dsi_driver_data = {
 	.has_clklane_stop = 1,
 };
 
+static struct exynos_dsi_driver_data exynos4415_dsi_driver_data = {
+	.plltmr_reg = 0x58,
+	.has_clklane_stop = 1,
+};
+
 static struct exynos_dsi_driver_data exynos5_dsi_driver_data = {
 	.plltmr_reg = 0x58,
 };
@@ -325,6 +335,8 @@ static struct of_device_id exynos_dsi_of_match[] = {
 	  .data = &exynos3_dsi_driver_data },
 	{ .compatible = "samsung,exynos4210-mipi-dsi",
 	  .data = &exynos4_dsi_driver_data },
+	{ .compatible = "samsung,exynos4415-mipi-dsi",
+	  .data = &exynos4415_dsi_driver_data },
 	{ .compatible = "samsung,exynos5410-mipi-dsi",
 	  .data = &exynos5_dsi_driver_data },
 	{ }
@@ -1104,7 +1116,7 @@ static irqreturn_t exynos_dsi_irq(int irq, void *dev_id)
 static irqreturn_t exynos_dsi_te_irq_handler(int irq, void *dev_id)
 {
 	struct exynos_dsi *dsi = (struct exynos_dsi *)dev_id;
-	struct drm_encoder *encoder = dsi->encoder;
+	struct drm_encoder *encoder = dsi->display.encoder;
 
 	if (dsi->state & DSIM_STATE_ENABLED)
 		exynos_drm_crtc_te_handler(encoder->crtc);
@@ -1143,6 +1155,7 @@ static int exynos_dsi_init(struct exynos_dsi *dsi)
 static int exynos_dsi_register_te_irq(struct exynos_dsi *dsi)
 {
 	int ret;
+	int te_gpio_irq;
 
 	dsi->te_gpio = of_get_named_gpio(dsi->panel_node, "te-gpios", 0);
 	if (!gpio_is_valid(dsi->te_gpio)) {
@@ -1157,14 +1170,10 @@ static int exynos_dsi_register_te_irq(struct exynos_dsi *dsi)
 		goto out;
 	}
 
-	/*
-	 * This TE GPIO IRQ should not be set to IRQ_NOAUTOEN, because panel
-	 * calls drm_panel_init() first then calls mipi_dsi_attach() in probe().
-	 * It means that te_gpio is invalid when exynos_dsi_enable_irq() is
-	 * called by drm_panel_init() before panel is attached.
-	 */
-	ret = request_threaded_irq(gpio_to_irq(dsi->te_gpio),
-					exynos_dsi_te_irq_handler, NULL,
+	te_gpio_irq = gpio_to_irq(dsi->te_gpio);
+
+	irq_set_status_flags(te_gpio_irq, IRQ_NOAUTOEN);
+	ret = request_threaded_irq(te_gpio_irq, exynos_dsi_te_irq_handler, NULL,
 					IRQF_TRIGGER_RISING, "TE", dsi);
 	if (ret) {
 		dev_err(dsi->dev, "request interrupt failed with %d\n", ret);
@@ -1195,9 +1204,6 @@ static int exynos_dsi_host_attach(struct mipi_dsi_host *host,
 	dsi->mode_flags = device->mode_flags;
 	dsi->panel_node = device->dev.of_node;
 
-	if (dsi->connector.dev)
-		drm_helper_hpd_irq_event(dsi->connector.dev);
-
 	/*
 	 * This is a temporary solution and should be made by more generic way.
 	 *
@@ -1211,6 +1217,9 @@ static int exynos_dsi_host_attach(struct mipi_dsi_host *host,
 			return ret;
 	}
 
+	if (dsi->connector.dev)
+		drm_helper_hpd_irq_event(dsi->connector.dev);
+
 	return 0;
 }
 
@@ -1236,7 +1245,7 @@ static bool exynos_dsi_is_short_dsi_type(u8 type)
 }
 
 static ssize_t exynos_dsi_host_transfer(struct mipi_dsi_host *host,
-				       struct mipi_dsi_msg *msg)
+				        const struct mipi_dsi_msg *msg)
 {
 	struct exynos_dsi *dsi = host_to_dsi(host);
 	struct exynos_dsi_transfer xfer;
@@ -1369,16 +1378,17 @@ static int exynos_dsi_enable(struct exynos_dsi *dsi)
 	exynos_dsi_set_display_mode(dsi);
 	exynos_dsi_set_display_enable(dsi, true);
 
+	dsi->state |= DSIM_STATE_ENABLED;
+
 	ret = drm_panel_enable(dsi->panel);
 	if (ret < 0) {
+		dsi->state &= ~DSIM_STATE_ENABLED;
 		exynos_dsi_set_display_enable(dsi, false);
 		drm_panel_unprepare(dsi->panel);
 		exynos_dsi_poweroff(dsi);
 		return ret;
 	}
 
-	dsi->state |= DSIM_STATE_ENABLED;
-
 	return 0;
 }
 
@@ -1397,7 +1407,7 @@ static void exynos_dsi_disable(struct exynos_dsi *dsi)
 
 static void exynos_dsi_dpms(struct exynos_drm_display *display, int mode)
 {
-	struct exynos_dsi *dsi = display->ctx;
+	struct exynos_dsi *dsi = display_to_dsi(display);
 
 	if (dsi->panel) {
 		switch (mode) {
@@ -1474,7 +1484,7 @@ exynos_dsi_best_encoder(struct drm_connector *connector)
 {
 	struct exynos_dsi *dsi = connector_to_dsi(connector);
 
-	return dsi->encoder;
+	return dsi->display.encoder;
 }
 
 static struct drm_connector_helper_funcs exynos_dsi_connector_helper_funcs = {
@@ -1486,12 +1496,10 @@ static struct drm_connector_helper_funcs exynos_dsi_connector_helper_funcs = {
 static int exynos_dsi_create_connector(struct exynos_drm_display *display,
 				       struct drm_encoder *encoder)
 {
-	struct exynos_dsi *dsi = display->ctx;
+	struct exynos_dsi *dsi = display_to_dsi(display);
 	struct drm_connector *connector = &dsi->connector;
 	int ret;
 
-	dsi->encoder = encoder;
-
 	connector->polled = DRM_CONNECTOR_POLL_HPD;
 
 	ret = drm_connector_init(encoder->dev, connector,
@@ -1512,7 +1520,7 @@ static int exynos_dsi_create_connector(struct exynos_drm_display *display,
 static void exynos_dsi_mode_set(struct exynos_drm_display *display,
 			 struct drm_display_mode *mode)
 {
-	struct exynos_dsi *dsi = display->ctx;
+	struct exynos_dsi *dsi = display_to_dsi(display);
 	struct videomode *vm = &dsi->vm;
 
 	vm->hactive = mode->hdisplay;
@@ -1531,10 +1539,6 @@ static struct exynos_drm_display_ops exynos_dsi_display_ops = {
 	.dpms = exynos_dsi_dpms
 };
 
-static struct exynos_drm_display exynos_dsi_display = {
-	.type = EXYNOS_DISPLAY_TYPE_LCD,
-	.ops = &exynos_dsi_display_ops,
-};
 MODULE_DEVICE_TABLE(of, exynos_dsi_of_match);
 
 /* of_* functions will be removed after merge of of_graph patches */
@@ -1640,28 +1644,28 @@ end:
 static int exynos_dsi_bind(struct device *dev, struct device *master,
 				void *data)
 {
+	struct exynos_drm_display *display = dev_get_drvdata(dev);
+	struct exynos_dsi *dsi = display_to_dsi(display);
 	struct drm_device *drm_dev = data;
-	struct exynos_dsi *dsi;
 	int ret;
 
-	ret = exynos_drm_create_enc_conn(drm_dev, &exynos_dsi_display);
+	ret = exynos_drm_create_enc_conn(drm_dev, display);
 	if (ret) {
 		DRM_ERROR("Encoder create [%d] failed with %d\n",
-				exynos_dsi_display.type, ret);
+			  display->type, ret);
 		return ret;
 	}
 
-	dsi = exynos_dsi_display.ctx;
-
 	return mipi_dsi_host_register(&dsi->dsi_host);
 }
 
 static void exynos_dsi_unbind(struct device *dev, struct device *master,
 				void *data)
 {
-	struct exynos_dsi *dsi = exynos_dsi_display.ctx;
+	struct exynos_drm_display *display = dev_get_drvdata(dev);
+	struct exynos_dsi *dsi = display_to_dsi(display);
 
-	exynos_dsi_dpms(&exynos_dsi_display, DRM_MODE_DPMS_OFF);
+	exynos_dsi_dpms(display, DRM_MODE_DPMS_OFF);
 
 	mipi_dsi_host_unregister(&dsi->dsi_host);
 }
@@ -1673,22 +1677,23 @@ static const struct component_ops exynos_dsi_component_ops = {
 
 static int exynos_dsi_probe(struct platform_device *pdev)
 {
+	struct device *dev = &pdev->dev;
 	struct resource *res;
 	struct exynos_dsi *dsi;
 	int ret;
 
-	ret = exynos_drm_component_add(&pdev->dev, EXYNOS_DEVICE_TYPE_CONNECTOR,
-					exynos_dsi_display.type);
+	dsi = devm_kzalloc(dev, sizeof(*dsi), GFP_KERNEL);
+	if (!dsi)
+		return -ENOMEM;
+
+	dsi->display.type = EXYNOS_DISPLAY_TYPE_LCD;
+	dsi->display.ops = &exynos_dsi_display_ops;
+
+	ret = exynos_drm_component_add(dev, EXYNOS_DEVICE_TYPE_CONNECTOR,
+				       dsi->display.type);
 	if (ret)
 		return ret;
 
-	dsi = devm_kzalloc(&pdev->dev, sizeof(*dsi), GFP_KERNEL);
-	if (!dsi) {
-		dev_err(&pdev->dev, "failed to allocate dsi object.\n");
-		ret = -ENOMEM;
-		goto err_del_component;
-	}
-
 	/* To be checked as invalid one */
 	dsi->te_gpio = -ENOENT;
 
@@ -1697,9 +1702,9 @@ static int exynos_dsi_probe(struct platform_device *pdev)
 	INIT_LIST_HEAD(&dsi->transfer_list);
 
 	dsi->dsi_host.ops = &exynos_dsi_ops;
-	dsi->dsi_host.dev = &pdev->dev;
+	dsi->dsi_host.dev = dev;
 
-	dsi->dev = &pdev->dev;
+	dsi->dev = dev;
 	dsi->driver_data = exynos_dsi_get_driver_data(pdev);
 
 	ret = exynos_dsi_parse_dt(dsi);
@@ -1708,70 +1713,68 @@ static int exynos_dsi_probe(struct platform_device *pdev)
 
 	dsi->supplies[0].supply = "vddcore";
 	dsi->supplies[1].supply = "vddio";
-	ret = devm_regulator_bulk_get(&pdev->dev, ARRAY_SIZE(dsi->supplies),
+	ret = devm_regulator_bulk_get(dev, ARRAY_SIZE(dsi->supplies),
 				      dsi->supplies);
 	if (ret) {
-		dev_info(&pdev->dev, "failed to get regulators: %d\n", ret);
+		dev_info(dev, "failed to get regulators: %d\n", ret);
 		return -EPROBE_DEFER;
 	}
 
-	dsi->pll_clk = devm_clk_get(&pdev->dev, "pll_clk");
+	dsi->pll_clk = devm_clk_get(dev, "pll_clk");
 	if (IS_ERR(dsi->pll_clk)) {
-		dev_info(&pdev->dev, "failed to get dsi pll input clock\n");
+		dev_info(dev, "failed to get dsi pll input clock\n");
 		ret = PTR_ERR(dsi->pll_clk);
 		goto err_del_component;
 	}
 
-	dsi->bus_clk = devm_clk_get(&pdev->dev, "bus_clk");
+	dsi->bus_clk = devm_clk_get(dev, "bus_clk");
 	if (IS_ERR(dsi->bus_clk)) {
-		dev_info(&pdev->dev, "failed to get dsi bus clock\n");
+		dev_info(dev, "failed to get dsi bus clock\n");
 		ret = PTR_ERR(dsi->bus_clk);
 		goto err_del_component;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	dsi->reg_base = devm_ioremap_resource(&pdev->dev, res);
+	dsi->reg_base = devm_ioremap_resource(dev, res);
 	if (IS_ERR(dsi->reg_base)) {
-		dev_err(&pdev->dev, "failed to remap io region\n");
+		dev_err(dev, "failed to remap io region\n");
 		ret = PTR_ERR(dsi->reg_base);
 		goto err_del_component;
 	}
 
-	dsi->phy = devm_phy_get(&pdev->dev, "dsim");
+	dsi->phy = devm_phy_get(dev, "dsim");
 	if (IS_ERR(dsi->phy)) {
-		dev_info(&pdev->dev, "failed to get dsim phy\n");
+		dev_info(dev, "failed to get dsim phy\n");
 		ret = PTR_ERR(dsi->phy);
 		goto err_del_component;
 	}
 
 	dsi->irq = platform_get_irq(pdev, 0);
 	if (dsi->irq < 0) {
-		dev_err(&pdev->dev, "failed to request dsi irq resource\n");
+		dev_err(dev, "failed to request dsi irq resource\n");
 		ret = dsi->irq;
 		goto err_del_component;
 	}
 
 	irq_set_status_flags(dsi->irq, IRQ_NOAUTOEN);
-	ret = devm_request_threaded_irq(&pdev->dev, dsi->irq, NULL,
+	ret = devm_request_threaded_irq(dev, dsi->irq, NULL,
 					exynos_dsi_irq, IRQF_ONESHOT,
-					dev_name(&pdev->dev), dsi);
+					dev_name(dev), dsi);
 	if (ret) {
-		dev_err(&pdev->dev, "failed to request dsi irq\n");
+		dev_err(dev, "failed to request dsi irq\n");
 		goto err_del_component;
 	}
 
-	exynos_dsi_display.ctx = dsi;
-
-	platform_set_drvdata(pdev, &exynos_dsi_display);
+	platform_set_drvdata(pdev, &dsi->display);
 
-	ret = component_add(&pdev->dev, &exynos_dsi_component_ops);
+	ret = component_add(dev, &exynos_dsi_component_ops);
 	if (ret)
 		goto err_del_component;
 
 	return ret;
 
 err_del_component:
-	exynos_drm_component_del(&pdev->dev, EXYNOS_DEVICE_TYPE_CONNECTOR);
+	exynos_drm_component_del(dev, EXYNOS_DEVICE_TYPE_CONNECTOR);
 	return ret;
 }
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_encoder.h b/drivers/gpu/drm/exynos/exynos_drm_encoder.h
index b7a1620a7e79..26305d8dd93a 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_encoder.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_encoder.h
@@ -14,8 +14,6 @@
 #ifndef _EXYNOS_DRM_ENCODER_H_
 #define _EXYNOS_DRM_ENCODER_H_
 
-struct exynos_drm_manager;
-
 void exynos_drm_encoder_setup(struct drm_device *dev);
 struct drm_encoder *exynos_drm_encoder_create(struct drm_device *dev,
 			struct exynos_drm_display *mgr,
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fimd.c b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
index 085b066a9993..e5810d13bf9c 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fimd.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
@@ -84,8 +84,6 @@
 /* FIMD has totally five hardware windows. */
 #define WINDOWS_NR	5
 
-#define get_fimd_manager(mgr)	platform_get_drvdata(to_platform_device(dev))
-
 struct fimd_driver_data {
 	unsigned int timing_base;
 	unsigned int lcdblk_offset;
@@ -96,6 +94,7 @@ struct fimd_driver_data {
 	unsigned int has_clksel:1;
 	unsigned int has_limited_fmt:1;
 	unsigned int has_vidoutcon:1;
+	unsigned int has_vtsel:1;
 };
 
 static struct fimd_driver_data s3c64xx_fimd_driver_data = {
@@ -118,6 +117,17 @@ static struct fimd_driver_data exynos4_fimd_driver_data = {
 	.lcdblk_vt_shift = 10,
 	.lcdblk_bypass_shift = 1,
 	.has_shadowcon = 1,
+	.has_vtsel = 1,
+};
+
+static struct fimd_driver_data exynos4415_fimd_driver_data = {
+	.timing_base = 0x20000,
+	.lcdblk_offset = 0x210,
+	.lcdblk_vt_shift = 10,
+	.lcdblk_bypass_shift = 1,
+	.has_shadowcon = 1,
+	.has_vidoutcon = 1,
+	.has_vtsel = 1,
 };
 
 static struct fimd_driver_data exynos5_fimd_driver_data = {
@@ -127,6 +137,7 @@ static struct fimd_driver_data exynos5_fimd_driver_data = {
 	.lcdblk_bypass_shift = 15,
 	.has_shadowcon = 1,
 	.has_vidoutcon = 1,
+	.has_vtsel = 1,
 };
 
 struct fimd_win_data {
@@ -146,6 +157,7 @@ struct fimd_win_data {
 };
 
 struct fimd_context {
+	struct exynos_drm_manager	manager;
 	struct device			*dev;
 	struct drm_device		*drm_dev;
 	struct clk			*bus_clk;
@@ -173,6 +185,11 @@ struct fimd_context {
 	struct exynos_drm_display *display;
 };
 
+static inline struct fimd_context *mgr_to_fimd(struct exynos_drm_manager *mgr)
+{
+	return container_of(mgr, struct fimd_context, manager);
+}
+
 static const struct of_device_id fimd_driver_dt_match[] = {
 	{ .compatible = "samsung,s3c6400-fimd",
 	  .data = &s3c64xx_fimd_driver_data },
@@ -180,6 +197,8 @@ static const struct of_device_id fimd_driver_dt_match[] = {
 	  .data = &exynos3_fimd_driver_data },
 	{ .compatible = "samsung,exynos4210-fimd",
 	  .data = &exynos4_fimd_driver_data },
+	{ .compatible = "samsung,exynos4415-fimd",
+	  .data = &exynos4415_fimd_driver_data },
 	{ .compatible = "samsung,exynos5250-fimd",
 	  .data = &exynos5_fimd_driver_data },
 	{},
@@ -197,7 +216,7 @@ static inline struct fimd_driver_data *drm_fimd_get_driver_data(
 
 static void fimd_wait_for_vblank(struct exynos_drm_manager *mgr)
 {
-	struct fimd_context *ctx = mgr->ctx;
+	struct fimd_context *ctx = mgr_to_fimd(mgr);
 
 	if (ctx->suspended)
 		return;
@@ -214,9 +233,35 @@ static void fimd_wait_for_vblank(struct exynos_drm_manager *mgr)
 		DRM_DEBUG_KMS("vblank wait timed out.\n");
 }
 
+static void fimd_enable_video_output(struct fimd_context *ctx, int win,
+					bool enable)
+{
+	u32 val = readl(ctx->regs + WINCON(win));
+
+	if (enable)
+		val |= WINCONx_ENWIN;
+	else
+		val &= ~WINCONx_ENWIN;
+
+	writel(val, ctx->regs + WINCON(win));
+}
+
+static void fimd_enable_shadow_channel_path(struct fimd_context *ctx, int win,
+						bool enable)
+{
+	u32 val = readl(ctx->regs + SHADOWCON);
+
+	if (enable)
+		val |= SHADOWCON_CHx_ENABLE(win);
+	else
+		val &= ~SHADOWCON_CHx_ENABLE(win);
+
+	writel(val, ctx->regs + SHADOWCON);
+}
+
 static void fimd_clear_channel(struct exynos_drm_manager *mgr)
 {
-	struct fimd_context *ctx = mgr->ctx;
+	struct fimd_context *ctx = mgr_to_fimd(mgr);
 	int win, ch_enabled = 0;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
@@ -226,16 +271,12 @@ static void fimd_clear_channel(struct exynos_drm_manager *mgr)
 		u32 val = readl(ctx->regs + WINCON(win));
 
 		if (val & WINCONx_ENWIN) {
-			/* wincon */
-			val &= ~WINCONx_ENWIN;
-			writel(val, ctx->regs + WINCON(win));
-
-			/* unprotect windows */
-			if (ctx->driver_data->has_shadowcon) {
-				val = readl(ctx->regs + SHADOWCON);
-				val &= ~SHADOWCON_CHx_ENABLE(win);
-				writel(val, ctx->regs + SHADOWCON);
-			}
+			fimd_enable_video_output(ctx, win, false);
+
+			if (ctx->driver_data->has_shadowcon)
+				fimd_enable_shadow_channel_path(ctx, win,
+								false);
+
 			ch_enabled = 1;
 		}
 	}
@@ -253,7 +294,7 @@ static void fimd_clear_channel(struct exynos_drm_manager *mgr)
 static int fimd_mgr_initialize(struct exynos_drm_manager *mgr,
 			struct drm_device *drm_dev)
 {
-	struct fimd_context *ctx = mgr->ctx;
+	struct fimd_context *ctx = mgr_to_fimd(mgr);
 	struct exynos_drm_private *priv;
 	priv = drm_dev->dev_private;
 
@@ -275,7 +316,7 @@ static int fimd_mgr_initialize(struct exynos_drm_manager *mgr,
 
 static void fimd_mgr_remove(struct exynos_drm_manager *mgr)
 {
-	struct fimd_context *ctx = mgr->ctx;
+	struct fimd_context *ctx = mgr_to_fimd(mgr);
 
 	/* detach this sub driver from iommu mapping if supported. */
 	if (is_drm_iommu_supported(ctx->drm_dev))
@@ -315,14 +356,14 @@ static bool fimd_mode_fixup(struct exynos_drm_manager *mgr,
 static void fimd_mode_set(struct exynos_drm_manager *mgr,
 		const struct drm_display_mode *in_mode)
 {
-	struct fimd_context *ctx = mgr->ctx;
+	struct fimd_context *ctx = mgr_to_fimd(mgr);
 
 	drm_mode_copy(&ctx->mode, in_mode);
 }
 
 static void fimd_commit(struct exynos_drm_manager *mgr)
 {
-	struct fimd_context *ctx = mgr->ctx;
+	struct fimd_context *ctx = mgr_to_fimd(mgr);
 	struct drm_display_mode *mode = &ctx->mode;
 	struct fimd_driver_data *driver_data = ctx->driver_data;
 	void *timing_base = ctx->regs + driver_data->timing_base;
@@ -343,7 +384,8 @@ static void fimd_commit(struct exynos_drm_manager *mgr)
 		writel(0, timing_base + I80IFCONFBx(0));
 
 		/* set video type selection to I80 interface */
-		if (ctx->sysreg && regmap_update_bits(ctx->sysreg,
+		if (driver_data->has_vtsel && ctx->sysreg &&
+				regmap_update_bits(ctx->sysreg,
 					driver_data->lcdblk_offset,
 					0x3 << driver_data->lcdblk_vt_shift,
 					0x1 << driver_data->lcdblk_vt_shift)) {
@@ -421,7 +463,7 @@ static void fimd_commit(struct exynos_drm_manager *mgr)
 
 static int fimd_enable_vblank(struct exynos_drm_manager *mgr)
 {
-	struct fimd_context *ctx = mgr->ctx;
+	struct fimd_context *ctx = mgr_to_fimd(mgr);
 	u32 val;
 
 	if (ctx->suspended)
@@ -431,12 +473,19 @@ static int fimd_enable_vblank(struct exynos_drm_manager *mgr)
 		val = readl(ctx->regs + VIDINTCON0);
 
 		val |= VIDINTCON0_INT_ENABLE;
-		val |= VIDINTCON0_INT_FRAME;
 
-		val &= ~VIDINTCON0_FRAMESEL0_MASK;
-		val |= VIDINTCON0_FRAMESEL0_VSYNC;
-		val &= ~VIDINTCON0_FRAMESEL1_MASK;
-		val |= VIDINTCON0_FRAMESEL1_NONE;
+		if (ctx->i80_if) {
+			val |= VIDINTCON0_INT_I80IFDONE;
+			val |= VIDINTCON0_INT_SYSMAINCON;
+			val &= ~VIDINTCON0_INT_SYSSUBCON;
+		} else {
+			val |= VIDINTCON0_INT_FRAME;
+
+			val &= ~VIDINTCON0_FRAMESEL0_MASK;
+			val |= VIDINTCON0_FRAMESEL0_VSYNC;
+			val &= ~VIDINTCON0_FRAMESEL1_MASK;
+			val |= VIDINTCON0_FRAMESEL1_NONE;
+		}
 
 		writel(val, ctx->regs + VIDINTCON0);
 	}
@@ -446,7 +495,7 @@ static int fimd_enable_vblank(struct exynos_drm_manager *mgr)
 
 static void fimd_disable_vblank(struct exynos_drm_manager *mgr)
 {
-	struct fimd_context *ctx = mgr->ctx;
+	struct fimd_context *ctx = mgr_to_fimd(mgr);
 	u32 val;
 
 	if (ctx->suspended)
@@ -455,9 +504,15 @@ static void fimd_disable_vblank(struct exynos_drm_manager *mgr)
 	if (test_and_clear_bit(0, &ctx->irq_flags)) {
 		val = readl(ctx->regs + VIDINTCON0);
 
-		val &= ~VIDINTCON0_INT_FRAME;
 		val &= ~VIDINTCON0_INT_ENABLE;
 
+		if (ctx->i80_if) {
+			val &= ~VIDINTCON0_INT_I80IFDONE;
+			val &= ~VIDINTCON0_INT_SYSMAINCON;
+			val &= ~VIDINTCON0_INT_SYSSUBCON;
+		} else
+			val &= ~VIDINTCON0_INT_FRAME;
+
 		writel(val, ctx->regs + VIDINTCON0);
 	}
 }
@@ -465,7 +520,7 @@ static void fimd_disable_vblank(struct exynos_drm_manager *mgr)
 static void fimd_win_mode_set(struct exynos_drm_manager *mgr,
 			struct exynos_drm_overlay *overlay)
 {
-	struct fimd_context *ctx = mgr->ctx;
+	struct fimd_context *ctx = mgr_to_fimd(mgr);
 	struct fimd_win_data *win_data;
 	int win;
 	unsigned long offset;
@@ -623,7 +678,7 @@ static void fimd_shadow_protect_win(struct fimd_context *ctx,
 
 static void fimd_win_commit(struct exynos_drm_manager *mgr, int zpos)
 {
-	struct fimd_context *ctx = mgr->ctx;
+	struct fimd_context *ctx = mgr_to_fimd(mgr);
 	struct fimd_win_data *win_data;
 	int win = zpos;
 	unsigned long val, alpha, size;
@@ -730,20 +785,14 @@ static void fimd_win_commit(struct exynos_drm_manager *mgr, int zpos)
 	if (win != 0)
 		fimd_win_set_colkey(ctx, win);
 
-	/* wincon */
-	val = readl(ctx->regs + WINCON(win));
-	val |= WINCONx_ENWIN;
-	writel(val, ctx->regs + WINCON(win));
+	fimd_enable_video_output(ctx, win, true);
+
+	if (ctx->driver_data->has_shadowcon)
+		fimd_enable_shadow_channel_path(ctx, win, true);
 
 	/* Enable DMA channel and unprotect windows */
 	fimd_shadow_protect_win(ctx, win, false);
 
-	if (ctx->driver_data->has_shadowcon) {
-		val = readl(ctx->regs + SHADOWCON);
-		val |= SHADOWCON_CHx_ENABLE(win);
-		writel(val, ctx->regs + SHADOWCON);
-	}
-
 	win_data->enabled = true;
 
 	if (ctx->i80_if)
@@ -752,10 +801,9 @@ static void fimd_win_commit(struct exynos_drm_manager *mgr, int zpos)
 
 static void fimd_win_disable(struct exynos_drm_manager *mgr, int zpos)
 {
-	struct fimd_context *ctx = mgr->ctx;
+	struct fimd_context *ctx = mgr_to_fimd(mgr);
 	struct fimd_win_data *win_data;
 	int win = zpos;
-	u32 val;
 
 	if (win == DEFAULT_ZPOS)
 		win = ctx->default_win;
@@ -774,18 +822,12 @@ static void fimd_win_disable(struct exynos_drm_manager *mgr, int zpos)
 	/* protect windows */
 	fimd_shadow_protect_win(ctx, win, true);
 
-	/* wincon */
-	val = readl(ctx->regs + WINCON(win));
-	val &= ~WINCONx_ENWIN;
-	writel(val, ctx->regs + WINCON(win));
+	fimd_enable_video_output(ctx, win, false);
 
-	/* unprotect windows */
-	if (ctx->driver_data->has_shadowcon) {
-		val = readl(ctx->regs + SHADOWCON);
-		val &= ~SHADOWCON_CHx_ENABLE(win);
-		writel(val, ctx->regs + SHADOWCON);
-	}
+	if (ctx->driver_data->has_shadowcon)
+		fimd_enable_shadow_channel_path(ctx, win, false);
 
+	/* unprotect windows */
 	fimd_shadow_protect_win(ctx, win, false);
 
 	win_data->enabled = false;
@@ -793,7 +835,7 @@ static void fimd_win_disable(struct exynos_drm_manager *mgr, int zpos)
 
 static void fimd_window_suspend(struct exynos_drm_manager *mgr)
 {
-	struct fimd_context *ctx = mgr->ctx;
+	struct fimd_context *ctx = mgr_to_fimd(mgr);
 	struct fimd_win_data *win_data;
 	int i;
 
@@ -803,12 +845,11 @@ static void fimd_window_suspend(struct exynos_drm_manager *mgr)
 		if (win_data->enabled)
 			fimd_win_disable(mgr, i);
 	}
-	fimd_wait_for_vblank(mgr);
 }
 
 static void fimd_window_resume(struct exynos_drm_manager *mgr)
 {
-	struct fimd_context *ctx = mgr->ctx;
+	struct fimd_context *ctx = mgr_to_fimd(mgr);
 	struct fimd_win_data *win_data;
 	int i;
 
@@ -821,7 +862,7 @@ static void fimd_window_resume(struct exynos_drm_manager *mgr)
 
 static void fimd_apply(struct exynos_drm_manager *mgr)
 {
-	struct fimd_context *ctx = mgr->ctx;
+	struct fimd_context *ctx = mgr_to_fimd(mgr);
 	struct fimd_win_data *win_data;
 	int i;
 
@@ -838,7 +879,7 @@ static void fimd_apply(struct exynos_drm_manager *mgr)
 
 static int fimd_poweron(struct exynos_drm_manager *mgr)
 {
-	struct fimd_context *ctx = mgr->ctx;
+	struct fimd_context *ctx = mgr_to_fimd(mgr);
 	int ret;
 
 	if (!ctx->suspended)
@@ -886,7 +927,7 @@ bus_clk_err:
 
 static int fimd_poweroff(struct exynos_drm_manager *mgr)
 {
-	struct fimd_context *ctx = mgr->ctx;
+	struct fimd_context *ctx = mgr_to_fimd(mgr);
 
 	if (ctx->suspended)
 		return 0;
@@ -928,39 +969,41 @@ static void fimd_dpms(struct exynos_drm_manager *mgr, int mode)
 
 static void fimd_trigger(struct device *dev)
 {
-	struct exynos_drm_manager *mgr = get_fimd_manager(dev);
-	struct fimd_context *ctx = mgr->ctx;
+	struct fimd_context *ctx = dev_get_drvdata(dev);
 	struct fimd_driver_data *driver_data = ctx->driver_data;
 	void *timing_base = ctx->regs + driver_data->timing_base;
 	u32 reg;
 
-	atomic_set(&ctx->triggering, 1);
+	 /*
+	  * Skips triggering if in triggering state, because multiple triggering
+	  * requests can cause panel reset.
+	  */
+	if (atomic_read(&ctx->triggering))
+		return;
 
-	reg = readl(ctx->regs + VIDINTCON0);
-	reg |= (VIDINTCON0_INT_ENABLE | VIDINTCON0_INT_I80IFDONE |
-						VIDINTCON0_INT_SYSMAINCON);
-	writel(reg, ctx->regs + VIDINTCON0);
+	/* Enters triggering mode */
+	atomic_set(&ctx->triggering, 1);
 
 	reg = readl(timing_base + TRIGCON);
 	reg |= (TRGMODE_I80_RGB_ENABLE_I80 | SWTRGCMD_I80_RGB_ENABLE);
 	writel(reg, timing_base + TRIGCON);
+
+	/*
+	 * Exits triggering mode if vblank is not enabled yet, because when the
+	 * VIDINTCON0 register is not set, it can not exit from triggering mode.
+	 */
+	if (!test_bit(0, &ctx->irq_flags))
+		atomic_set(&ctx->triggering, 0);
 }
 
 static void fimd_te_handler(struct exynos_drm_manager *mgr)
 {
-	struct fimd_context *ctx = mgr->ctx;
+	struct fimd_context *ctx = mgr_to_fimd(mgr);
 
 	/* Checks the crtc is detached already from encoder */
 	if (ctx->pipe < 0 || !ctx->drm_dev)
 		return;
 
-	 /*
-	 * Skips to trigger if in triggering state, because multiple triggering
-	 * requests can cause panel reset.
-	 */
-	if (atomic_read(&ctx->triggering))
-		return;
-
 	/*
 	 * If there is a page flip request, triggers and handles the page flip
 	 * event so that current fb can be updated into panel GRAM.
@@ -972,10 +1015,10 @@ static void fimd_te_handler(struct exynos_drm_manager *mgr)
 	if (atomic_read(&ctx->wait_vsync_event)) {
 		atomic_set(&ctx->wait_vsync_event, 0);
 		wake_up(&ctx->wait_vsync_queue);
-
-		if (!atomic_read(&ctx->triggering))
-			drm_handle_vblank(ctx->drm_dev, ctx->pipe);
 	}
+
+	if (test_bit(0, &ctx->irq_flags))
+		drm_handle_vblank(ctx->drm_dev, ctx->pipe);
 }
 
 static struct exynos_drm_manager_ops fimd_manager_ops = {
@@ -992,11 +1035,6 @@ static struct exynos_drm_manager_ops fimd_manager_ops = {
 	.te_handler = fimd_te_handler,
 };
 
-static struct exynos_drm_manager fimd_manager = {
-	.type = EXYNOS_DISPLAY_TYPE_LCD,
-	.ops = &fimd_manager_ops,
-};
-
 static irqreturn_t fimd_irq_handler(int irq, void *dev_id)
 {
 	struct fimd_context *ctx = (struct fimd_context *)dev_id;
@@ -1013,16 +1051,10 @@ static irqreturn_t fimd_irq_handler(int irq, void *dev_id)
 		goto out;
 
 	if (ctx->i80_if) {
-		/* unset I80 frame done interrupt */
-		val = readl(ctx->regs + VIDINTCON0);
-		val &= ~(VIDINTCON0_INT_I80IFDONE | VIDINTCON0_INT_SYSMAINCON);
-		writel(val, ctx->regs + VIDINTCON0);
+		exynos_drm_crtc_finish_pageflip(ctx->drm_dev, ctx->pipe);
 
-		/* exit triggering mode */
+		/* Exits triggering mode */
 		atomic_set(&ctx->triggering, 0);
-
-		drm_handle_vblank(ctx->drm_dev, ctx->pipe);
-		exynos_drm_crtc_finish_pageflip(ctx->drm_dev, ctx->pipe);
 	} else {
 		drm_handle_vblank(ctx->drm_dev, ctx->pipe);
 		exynos_drm_crtc_finish_pageflip(ctx->drm_dev, ctx->pipe);
@@ -1040,11 +1072,11 @@ out:
 
 static int fimd_bind(struct device *dev, struct device *master, void *data)
 {
-	struct fimd_context *ctx = fimd_manager.ctx;
+	struct fimd_context *ctx = dev_get_drvdata(dev);
 	struct drm_device *drm_dev = data;
 
-	fimd_mgr_initialize(&fimd_manager, drm_dev);
-	exynos_drm_crtc_create(&fimd_manager);
+	fimd_mgr_initialize(&ctx->manager, drm_dev);
+	exynos_drm_crtc_create(&ctx->manager);
 	if (ctx->display)
 		exynos_drm_create_enc_conn(drm_dev, ctx->display);
 
@@ -1055,15 +1087,14 @@ static int fimd_bind(struct device *dev, struct device *master, void *data)
 static void fimd_unbind(struct device *dev, struct device *master,
 			void *data)
 {
-	struct exynos_drm_manager *mgr = dev_get_drvdata(dev);
-	struct fimd_context *ctx = fimd_manager.ctx;
+	struct fimd_context *ctx = dev_get_drvdata(dev);
 
-	fimd_dpms(mgr, DRM_MODE_DPMS_OFF);
+	fimd_dpms(&ctx->manager, DRM_MODE_DPMS_OFF);
 
 	if (ctx->display)
-		exynos_dpi_remove(dev);
+		exynos_dpi_remove(ctx->display);
 
-	fimd_mgr_remove(mgr);
+	fimd_mgr_remove(&ctx->manager);
 }
 
 static const struct component_ops fimd_component_ops = {
@@ -1079,21 +1110,20 @@ static int fimd_probe(struct platform_device *pdev)
 	struct resource *res;
 	int ret = -EINVAL;
 
-	ret = exynos_drm_component_add(&pdev->dev, EXYNOS_DEVICE_TYPE_CRTC,
-					fimd_manager.type);
-	if (ret)
-		return ret;
-
-	if (!dev->of_node) {
-		ret = -ENODEV;
-		goto err_del_component;
-	}
+	if (!dev->of_node)
+		return -ENODEV;
 
 	ctx = devm_kzalloc(dev, sizeof(*ctx), GFP_KERNEL);
-	if (!ctx) {
-		ret = -ENOMEM;
-		goto err_del_component;
-	}
+	if (!ctx)
+		return -ENOMEM;
+
+	ctx->manager.type = EXYNOS_DISPLAY_TYPE_LCD;
+	ctx->manager.ops = &fimd_manager_ops;
+
+	ret = exynos_drm_component_add(dev, EXYNOS_DEVICE_TYPE_CRTC,
+				       ctx->manager.type);
+	if (ret)
+		return ret;
 
 	ctx->dev = dev;
 	ctx->suspended = true;
@@ -1182,27 +1212,27 @@ static int fimd_probe(struct platform_device *pdev)
 	init_waitqueue_head(&ctx->wait_vsync_queue);
 	atomic_set(&ctx->wait_vsync_event, 0);
 
-	platform_set_drvdata(pdev, &fimd_manager);
-
-	fimd_manager.ctx = ctx;
+	platform_set_drvdata(pdev, ctx);
 
 	ctx->display = exynos_dpi_probe(dev);
-	if (IS_ERR(ctx->display))
-		return PTR_ERR(ctx->display);
+	if (IS_ERR(ctx->display)) {
+		ret = PTR_ERR(ctx->display);
+		goto err_del_component;
+	}
 
-	pm_runtime_enable(&pdev->dev);
+	pm_runtime_enable(dev);
 
-	ret = component_add(&pdev->dev, &fimd_component_ops);
+	ret = component_add(dev, &fimd_component_ops);
 	if (ret)
 		goto err_disable_pm_runtime;
 
 	return ret;
 
 err_disable_pm_runtime:
-	pm_runtime_disable(&pdev->dev);
+	pm_runtime_disable(dev);
 
 err_del_component:
-	exynos_drm_component_del(&pdev->dev, EXYNOS_DEVICE_TYPE_CRTC);
+	exynos_drm_component_del(dev, EXYNOS_DEVICE_TYPE_CRTC);
 	return ret;
 }
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_iommu.h b/drivers/gpu/drm/exynos/exynos_drm_iommu.h
index 72376d41c512..35d25889b476 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_iommu.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_iommu.h
@@ -40,7 +40,6 @@ static inline bool is_drm_iommu_supported(struct drm_device *drm_dev)
 
 #else
 
-struct dma_iommu_mapping;
 static inline int drm_create_iommu_mapping(struct drm_device *drm_dev)
 {
 	return 0;
diff --git a/drivers/gpu/drm/exynos/exynos_drm_ipp.c b/drivers/gpu/drm/exynos/exynos_drm_ipp.c
index 00d74b18f7cb..d5ad17dfc24d 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_ipp.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_ipp.c
@@ -426,18 +426,21 @@ int exynos_drm_ipp_set_property(struct drm_device *drm_dev, void *data,
 	c_node->start_work = ipp_create_cmd_work();
 	if (IS_ERR(c_node->start_work)) {
 		DRM_ERROR("failed to create start work.\n");
+		ret = PTR_ERR(c_node->start_work);
 		goto err_remove_id;
 	}
 
 	c_node->stop_work = ipp_create_cmd_work();
 	if (IS_ERR(c_node->stop_work)) {
 		DRM_ERROR("failed to create stop work.\n");
+		ret = PTR_ERR(c_node->stop_work);
 		goto err_free_start;
 	}
 
 	c_node->event_work = ipp_create_event_work();
 	if (IS_ERR(c_node->event_work)) {
 		DRM_ERROR("failed to create event work.\n");
+		ret = PTR_ERR(c_node->event_work);
 		goto err_free_stop;
 	}
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_vidi.c b/drivers/gpu/drm/exynos/exynos_drm_vidi.c
index 50faf913e574..45899fb63272 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_vidi.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_vidi.c
@@ -14,6 +14,7 @@
 
 #include <linux/kernel.h>
 #include <linux/platform_device.h>
+#include <linux/component.h>
 
 #include <drm/exynos_drm.h>
 
@@ -28,7 +29,6 @@
 /* vidi has totally three virtual windows. */
 #define WINDOWS_NR		3
 
-#define get_vidi_mgr(dev)	platform_get_drvdata(to_platform_device(dev))
 #define ctx_from_connector(c)	container_of(c, struct vidi_context, \
 					connector)
 
@@ -47,11 +47,13 @@ struct vidi_win_data {
 };
 
 struct vidi_context {
+	struct exynos_drm_manager	manager;
+	struct exynos_drm_display	display;
+	struct platform_device		*pdev;
 	struct drm_device		*drm_dev;
 	struct drm_crtc			*crtc;
 	struct drm_encoder		*encoder;
 	struct drm_connector		connector;
-	struct exynos_drm_subdrv	subdrv;
 	struct vidi_win_data		win_data[WINDOWS_NR];
 	struct edid			*raw_edid;
 	unsigned int			clkdiv;
@@ -66,6 +68,16 @@ struct vidi_context {
 	int				pipe;
 };
 
+static inline struct vidi_context *manager_to_vidi(struct exynos_drm_manager *m)
+{
+	return container_of(m, struct vidi_context, manager);
+}
+
+static inline struct vidi_context *display_to_vidi(struct exynos_drm_display *d)
+{
+	return container_of(d, struct vidi_context, display);
+}
+
 static const char fake_edid_info[] = {
 	0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x4c, 0x2d, 0x05, 0x05,
 	0x00, 0x00, 0x00, 0x00, 0x30, 0x12, 0x01, 0x03, 0x80, 0x10, 0x09, 0x78,
@@ -93,7 +105,7 @@ static const char fake_edid_info[] = {
 
 static void vidi_apply(struct exynos_drm_manager *mgr)
 {
-	struct vidi_context *ctx = mgr->ctx;
+	struct vidi_context *ctx = manager_to_vidi(mgr);
 	struct exynos_drm_manager_ops *mgr_ops = mgr->ops;
 	struct vidi_win_data *win_data;
 	int i;
@@ -110,7 +122,7 @@ static void vidi_apply(struct exynos_drm_manager *mgr)
 
 static void vidi_commit(struct exynos_drm_manager *mgr)
 {
-	struct vidi_context *ctx = mgr->ctx;
+	struct vidi_context *ctx = manager_to_vidi(mgr);
 
 	if (ctx->suspended)
 		return;
@@ -118,7 +130,7 @@ static void vidi_commit(struct exynos_drm_manager *mgr)
 
 static int vidi_enable_vblank(struct exynos_drm_manager *mgr)
 {
-	struct vidi_context *ctx = mgr->ctx;
+	struct vidi_context *ctx = manager_to_vidi(mgr);
 
 	if (ctx->suspended)
 		return -EPERM;
@@ -140,7 +152,7 @@ static int vidi_enable_vblank(struct exynos_drm_manager *mgr)
 
 static void vidi_disable_vblank(struct exynos_drm_manager *mgr)
 {
-	struct vidi_context *ctx = mgr->ctx;
+	struct vidi_context *ctx = manager_to_vidi(mgr);
 
 	if (ctx->suspended)
 		return;
@@ -152,7 +164,7 @@ static void vidi_disable_vblank(struct exynos_drm_manager *mgr)
 static void vidi_win_mode_set(struct exynos_drm_manager *mgr,
 			struct exynos_drm_overlay *overlay)
 {
-	struct vidi_context *ctx = mgr->ctx;
+	struct vidi_context *ctx = manager_to_vidi(mgr);
 	struct vidi_win_data *win_data;
 	int win;
 	unsigned long offset;
@@ -204,7 +216,7 @@ static void vidi_win_mode_set(struct exynos_drm_manager *mgr,
 
 static void vidi_win_commit(struct exynos_drm_manager *mgr, int zpos)
 {
-	struct vidi_context *ctx = mgr->ctx;
+	struct vidi_context *ctx = manager_to_vidi(mgr);
 	struct vidi_win_data *win_data;
 	int win = zpos;
 
@@ -229,7 +241,7 @@ static void vidi_win_commit(struct exynos_drm_manager *mgr, int zpos)
 
 static void vidi_win_disable(struct exynos_drm_manager *mgr, int zpos)
 {
-	struct vidi_context *ctx = mgr->ctx;
+	struct vidi_context *ctx = manager_to_vidi(mgr);
 	struct vidi_win_data *win_data;
 	int win = zpos;
 
@@ -247,7 +259,7 @@ static void vidi_win_disable(struct exynos_drm_manager *mgr, int zpos)
 
 static int vidi_power_on(struct exynos_drm_manager *mgr, bool enable)
 {
-	struct vidi_context *ctx = mgr->ctx;
+	struct vidi_context *ctx = manager_to_vidi(mgr);
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
@@ -271,7 +283,7 @@ static int vidi_power_on(struct exynos_drm_manager *mgr, bool enable)
 
 static void vidi_dpms(struct exynos_drm_manager *mgr, int mode)
 {
-	struct vidi_context *ctx = mgr->ctx;
+	struct vidi_context *ctx = manager_to_vidi(mgr);
 
 	DRM_DEBUG_KMS("%d\n", mode);
 
@@ -297,7 +309,7 @@ static void vidi_dpms(struct exynos_drm_manager *mgr, int mode)
 static int vidi_mgr_initialize(struct exynos_drm_manager *mgr,
 			struct drm_device *drm_dev)
 {
-	struct vidi_context *ctx = mgr->ctx;
+	struct vidi_context *ctx = manager_to_vidi(mgr);
 	struct exynos_drm_private *priv = drm_dev->dev_private;
 
 	mgr->drm_dev = ctx->drm_dev = drm_dev;
@@ -316,11 +328,6 @@ static struct exynos_drm_manager_ops vidi_manager_ops = {
 	.win_disable = vidi_win_disable,
 };
 
-static struct exynos_drm_manager vidi_manager = {
-	.type = EXYNOS_DISPLAY_TYPE_VIDI,
-	.ops = &vidi_manager_ops,
-};
-
 static void vidi_fake_vblank_handler(struct work_struct *work)
 {
 	struct vidi_context *ctx = container_of(work, struct vidi_context,
@@ -349,9 +356,8 @@ static void vidi_fake_vblank_handler(struct work_struct *work)
 static int vidi_show_connection(struct device *dev,
 				struct device_attribute *attr, char *buf)
 {
+	struct vidi_context *ctx = dev_get_drvdata(dev);
 	int rc;
-	struct exynos_drm_manager *mgr = get_vidi_mgr(dev);
-	struct vidi_context *ctx = mgr->ctx;
 
 	mutex_lock(&ctx->lock);
 
@@ -366,8 +372,7 @@ static int vidi_store_connection(struct device *dev,
 				struct device_attribute *attr,
 				const char *buf, size_t len)
 {
-	struct exynos_drm_manager *mgr = get_vidi_mgr(dev);
-	struct vidi_context *ctx = mgr->ctx;
+	struct vidi_context *ctx = dev_get_drvdata(dev);
 	int ret;
 
 	ret = kstrtoint(buf, 0, &ctx->connected);
@@ -420,7 +425,7 @@ int vidi_connection_ioctl(struct drm_device *drm_dev, void *data,
 		display = exynos_drm_get_display(encoder);
 
 		if (display->type == EXYNOS_DISPLAY_TYPE_VIDI) {
-			ctx = display->ctx;
+			ctx = display_to_vidi(display);
 			break;
 		}
 	}
@@ -530,7 +535,7 @@ static struct drm_connector_helper_funcs vidi_connector_helper_funcs = {
 static int vidi_create_connector(struct exynos_drm_display *display,
 				struct drm_encoder *encoder)
 {
-	struct vidi_context *ctx = display->ctx;
+	struct vidi_context *ctx = display_to_vidi(display);
 	struct drm_connector *connector = &ctx->connector;
 	int ret;
 
@@ -556,27 +561,22 @@ static struct exynos_drm_display_ops vidi_display_ops = {
 	.create_connector = vidi_create_connector,
 };
 
-static struct exynos_drm_display vidi_display = {
-	.type = EXYNOS_DISPLAY_TYPE_VIDI,
-	.ops = &vidi_display_ops,
-};
-
-static int vidi_subdrv_probe(struct drm_device *drm_dev, struct device *dev)
+static int vidi_bind(struct device *dev, struct device *master, void *data)
 {
-	struct exynos_drm_manager *mgr = get_vidi_mgr(dev);
-	struct vidi_context *ctx = mgr->ctx;
+	struct vidi_context *ctx = dev_get_drvdata(dev);
+	struct drm_device *drm_dev = data;
 	struct drm_crtc *crtc = ctx->crtc;
 	int ret;
 
-	vidi_mgr_initialize(mgr, drm_dev);
+	vidi_mgr_initialize(&ctx->manager, drm_dev);
 
-	ret = exynos_drm_crtc_create(&vidi_manager);
+	ret = exynos_drm_crtc_create(&ctx->manager);
 	if (ret) {
 		DRM_ERROR("failed to create crtc.\n");
 		return ret;
 	}
 
-	ret = exynos_drm_create_enc_conn(drm_dev, &vidi_display);
+	ret = exynos_drm_create_enc_conn(drm_dev, &ctx->display);
 	if (ret) {
 		crtc->funcs->destroy(crtc);
 		DRM_ERROR("failed to create encoder and connector.\n");
@@ -586,9 +586,18 @@ static int vidi_subdrv_probe(struct drm_device *drm_dev, struct device *dev)
 	return 0;
 }
 
+
+static void vidi_unbind(struct device *dev, struct device *master, void *data)
+{
+}
+
+static const struct component_ops vidi_component_ops = {
+	.bind	= vidi_bind,
+	.unbind = vidi_unbind,
+};
+
 static int vidi_probe(struct platform_device *pdev)
 {
-	struct exynos_drm_subdrv *subdrv;
 	struct vidi_context *ctx;
 	int ret;
 
@@ -596,40 +605,54 @@ static int vidi_probe(struct platform_device *pdev)
 	if (!ctx)
 		return -ENOMEM;
 
+	ctx->manager.type = EXYNOS_DISPLAY_TYPE_VIDI;
+	ctx->manager.ops = &vidi_manager_ops;
+	ctx->display.type = EXYNOS_DISPLAY_TYPE_VIDI;
+	ctx->display.ops = &vidi_display_ops;
 	ctx->default_win = 0;
+	ctx->pdev = pdev;
 
-	INIT_WORK(&ctx->work, vidi_fake_vblank_handler);
-
-	vidi_manager.ctx = ctx;
-	vidi_display.ctx = ctx;
+	ret = exynos_drm_component_add(&pdev->dev, EXYNOS_DEVICE_TYPE_CRTC,
+					ctx->manager.type);
+	if (ret)
+		return ret;
 
-	mutex_init(&ctx->lock);
+	ret = exynos_drm_component_add(&pdev->dev, EXYNOS_DEVICE_TYPE_CONNECTOR,
+					ctx->display.type);
+	if (ret)
+		goto err_del_crtc_component;
 
-	platform_set_drvdata(pdev, &vidi_manager);
+	INIT_WORK(&ctx->work, vidi_fake_vblank_handler);
 
-	subdrv = &ctx->subdrv;
-	subdrv->dev = &pdev->dev;
-	subdrv->probe = vidi_subdrv_probe;
+	mutex_init(&ctx->lock);
 
-	ret = exynos_drm_subdrv_register(subdrv);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "failed to register drm vidi device\n");
-		return ret;
-	}
+	platform_set_drvdata(pdev, ctx);
 
 	ret = device_create_file(&pdev->dev, &dev_attr_connection);
 	if (ret < 0) {
-		exynos_drm_subdrv_unregister(subdrv);
-		DRM_INFO("failed to create connection sysfs.\n");
+		DRM_ERROR("failed to create connection sysfs.\n");
+		goto err_del_conn_component;
 	}
 
-	return 0;
+	ret = component_add(&pdev->dev, &vidi_component_ops);
+	if (ret)
+		goto err_remove_file;
+
+	return ret;
+
+err_remove_file:
+	device_remove_file(&pdev->dev, &dev_attr_connection);
+err_del_conn_component:
+	exynos_drm_component_del(&pdev->dev, EXYNOS_DEVICE_TYPE_CONNECTOR);
+err_del_crtc_component:
+	exynos_drm_component_del(&pdev->dev, EXYNOS_DEVICE_TYPE_CRTC);
+
+	return ret;
 }
 
 static int vidi_remove(struct platform_device *pdev)
 {
-	struct exynos_drm_manager *mgr = platform_get_drvdata(pdev);
-	struct vidi_context *ctx = mgr->ctx;
+	struct vidi_context *ctx = platform_get_drvdata(pdev);
 
 	if (ctx->raw_edid != (struct edid *)fake_edid_info) {
 		kfree(ctx->raw_edid);
@@ -638,6 +661,10 @@ static int vidi_remove(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
+	component_del(&pdev->dev, &vidi_component_ops);
+	exynos_drm_component_del(&pdev->dev, EXYNOS_DEVICE_TYPE_CONNECTOR);
+	exynos_drm_component_del(&pdev->dev, EXYNOS_DEVICE_TYPE_CRTC);
+
 	return 0;
 }
 
@@ -668,12 +695,19 @@ int exynos_drm_probe_vidi(void)
 	return ret;
 }
 
+static int exynos_drm_remove_vidi_device(struct device *dev, void *data)
+{
+	platform_device_unregister(to_platform_device(dev));
+
+	return 0;
+}
+
 void exynos_drm_remove_vidi(void)
 {
-	struct vidi_context *ctx = vidi_manager.ctx;
-	struct exynos_drm_subdrv *subdrv = &ctx->subdrv;
-	struct platform_device *pdev = to_platform_device(subdrv->dev);
+	int ret = driver_for_each_device(&vidi_driver.driver, NULL, NULL,
+					 exynos_drm_remove_vidi_device);
+	/* silence compiler warning */
+	(void)ret;
 
 	platform_driver_unregister(&vidi_driver);
-	platform_device_unregister(pdev);
 }
diff --git a/drivers/gpu/drm/exynos/exynos_hdmi.c b/drivers/gpu/drm/exynos/exynos_hdmi.c
index 563a19e62eb2..5765a161abdd 100644
--- a/drivers/gpu/drm/exynos/exynos_hdmi.c
+++ b/drivers/gpu/drm/exynos/exynos_hdmi.c
@@ -49,7 +49,6 @@
 #include <linux/gpio.h>
 #include <media/s5p_hdmi.h>
 
-#define get_hdmi_display(dev)	platform_get_drvdata(to_platform_device(dev))
 #define ctx_from_connector(c)	container_of(c, struct hdmi_context, connector)
 
 #define HOTPLUG_DEBOUNCE_MS		1100
@@ -182,6 +181,7 @@ struct hdmi_conf_regs {
 };
 
 struct hdmi_context {
+	struct exynos_drm_display	display;
 	struct device			*dev;
 	struct drm_device		*drm_dev;
 	struct drm_connector		connector;
@@ -213,6 +213,11 @@ struct hdmi_context {
 	enum hdmi_type			type;
 };
 
+static inline struct hdmi_context *display_to_hdmi(struct exynos_drm_display *d)
+{
+	return container_of(d, struct hdmi_context, display);
+}
+
 struct hdmiphy_config {
 	int pixel_clock;
 	u8 conf[32];
@@ -1123,7 +1128,7 @@ static struct drm_connector_helper_funcs hdmi_connector_helper_funcs = {
 static int hdmi_create_connector(struct exynos_drm_display *display,
 			struct drm_encoder *encoder)
 {
-	struct hdmi_context *hdata = display->ctx;
+	struct hdmi_context *hdata = display_to_hdmi(display);
 	struct drm_connector *connector = &hdata->connector;
 	int ret;
 
@@ -2000,7 +2005,7 @@ static void hdmi_v14_mode_set(struct hdmi_context *hdata,
 static void hdmi_mode_set(struct exynos_drm_display *display,
 			struct drm_display_mode *mode)
 {
-	struct hdmi_context *hdata = display->ctx;
+	struct hdmi_context *hdata = display_to_hdmi(display);
 	struct drm_display_mode *m = mode;
 
 	DRM_DEBUG_KMS("xres=%d, yres=%d, refresh=%d, intl=%s\n",
@@ -2019,7 +2024,7 @@ static void hdmi_mode_set(struct exynos_drm_display *display,
 
 static void hdmi_commit(struct exynos_drm_display *display)
 {
-	struct hdmi_context *hdata = display->ctx;
+	struct hdmi_context *hdata = display_to_hdmi(display);
 
 	mutex_lock(&hdata->hdmi_mutex);
 	if (!hdata->powered) {
@@ -2033,7 +2038,7 @@ static void hdmi_commit(struct exynos_drm_display *display)
 
 static void hdmi_poweron(struct exynos_drm_display *display)
 {
-	struct hdmi_context *hdata = display->ctx;
+	struct hdmi_context *hdata = display_to_hdmi(display);
 	struct hdmi_resources *res = &hdata->res;
 
 	mutex_lock(&hdata->hdmi_mutex);
@@ -2064,7 +2069,7 @@ static void hdmi_poweron(struct exynos_drm_display *display)
 
 static void hdmi_poweroff(struct exynos_drm_display *display)
 {
-	struct hdmi_context *hdata = display->ctx;
+	struct hdmi_context *hdata = display_to_hdmi(display);
 	struct hdmi_resources *res = &hdata->res;
 
 	mutex_lock(&hdata->hdmi_mutex);
@@ -2099,7 +2104,7 @@ out:
 
 static void hdmi_dpms(struct exynos_drm_display *display, int mode)
 {
-	struct hdmi_context *hdata = display->ctx;
+	struct hdmi_context *hdata = display_to_hdmi(display);
 	struct drm_encoder *encoder = hdata->encoder;
 	struct drm_crtc *crtc = encoder->crtc;
 	struct drm_crtc_helper_funcs *funcs = NULL;
@@ -2143,11 +2148,6 @@ static struct exynos_drm_display_ops hdmi_display_ops = {
 	.commit		= hdmi_commit,
 };
 
-static struct exynos_drm_display hdmi_display = {
-	.type = EXYNOS_DISPLAY_TYPE_HDMI,
-	.ops = &hdmi_display_ops,
-};
-
 static void hdmi_hotplug_work_func(struct work_struct *work)
 {
 	struct hdmi_context *hdata;
@@ -2302,12 +2302,11 @@ MODULE_DEVICE_TABLE (of, hdmi_match_types);
 static int hdmi_bind(struct device *dev, struct device *master, void *data)
 {
 	struct drm_device *drm_dev = data;
-	struct hdmi_context *hdata;
+	struct hdmi_context *hdata = dev_get_drvdata(dev);
 
-	hdata = hdmi_display.ctx;
 	hdata->drm_dev = drm_dev;
 
-	return exynos_drm_create_enc_conn(drm_dev, &hdmi_display);
+	return exynos_drm_create_enc_conn(drm_dev, &hdata->display);
 }
 
 static void hdmi_unbind(struct device *dev, struct device *master, void *data)
@@ -2349,31 +2348,28 @@ static int hdmi_probe(struct platform_device *pdev)
 	struct resource *res;
 	int ret;
 
-	ret = exynos_drm_component_add(&pdev->dev, EXYNOS_DEVICE_TYPE_CONNECTOR,
-					hdmi_display.type);
-	if (ret)
-		return ret;
-
-	if (!dev->of_node) {
-		ret = -ENODEV;
-		goto err_del_component;
-	}
+	if (!dev->of_node)
+		return -ENODEV;
 
 	pdata = drm_hdmi_dt_parse_pdata(dev);
-	if (!pdata) {
-		ret = -EINVAL;
-		goto err_del_component;
-	}
+	if (!pdata)
+		return -EINVAL;
 
 	hdata = devm_kzalloc(dev, sizeof(struct hdmi_context), GFP_KERNEL);
-	if (!hdata) {
-		ret = -ENOMEM;
-		goto err_del_component;
-	}
+	if (!hdata)
+		return -ENOMEM;
+
+	hdata->display.type = EXYNOS_DISPLAY_TYPE_HDMI;
+	hdata->display.ops = &hdmi_display_ops;
+
+	ret = exynos_drm_component_add(&pdev->dev, EXYNOS_DEVICE_TYPE_CONNECTOR,
+					hdata->display.type);
+	if (ret)
+		return ret;
 
 	mutex_init(&hdata->hdmi_mutex);
 
-	platform_set_drvdata(pdev, &hdmi_display);
+	platform_set_drvdata(pdev, hdata);
 
 	match = of_match_node(hdmi_match_types, dev->of_node);
 	if (!match) {
@@ -2485,7 +2481,6 @@ out_get_phy_port:
 	}
 
 	pm_runtime_enable(dev);
-	hdmi_display.ctx = hdata;
 
 	ret = component_add(&pdev->dev, &hdmi_component_ops);
 	if (ret)
@@ -2510,7 +2505,7 @@ err_del_component:
 
 static int hdmi_remove(struct platform_device *pdev)
 {
-	struct hdmi_context *hdata = hdmi_display.ctx;
+	struct hdmi_context *hdata = platform_get_drvdata(pdev);
 
 	cancel_delayed_work_sync(&hdata->hotplug_work);
 
diff --git a/drivers/gpu/drm/exynos/exynos_mixer.c b/drivers/gpu/drm/exynos/exynos_mixer.c
index a41c84ee3a2d..820b76234ef4 100644
--- a/drivers/gpu/drm/exynos/exynos_mixer.c
+++ b/drivers/gpu/drm/exynos/exynos_mixer.c
@@ -40,8 +40,6 @@
 #include "exynos_drm_iommu.h"
 #include "exynos_mixer.h"
 
-#define get_mixer_manager(dev)	platform_get_drvdata(to_platform_device(dev))
-
 #define MIXER_WIN_NR		3
 #define MIXER_DEFAULT_WIN	0
 
@@ -86,6 +84,7 @@ enum mixer_version_id {
 };
 
 struct mixer_context {
+	struct exynos_drm_manager manager;
 	struct platform_device *pdev;
 	struct device		*dev;
 	struct drm_device	*drm_dev;
@@ -104,6 +103,11 @@ struct mixer_context {
 	atomic_t		wait_vsync_event;
 };
 
+static inline struct mixer_context *mgr_to_mixer(struct exynos_drm_manager *mgr)
+{
+	return container_of(mgr, struct mixer_context, manager);
+}
+
 struct mixer_drv_data {
 	enum mixer_version_id	version;
 	bool					is_vp_enabled;
@@ -854,7 +858,7 @@ static int mixer_initialize(struct exynos_drm_manager *mgr,
 			struct drm_device *drm_dev)
 {
 	int ret;
-	struct mixer_context *mixer_ctx = mgr->ctx;
+	struct mixer_context *mixer_ctx = mgr_to_mixer(mgr);
 	struct exynos_drm_private *priv;
 	priv = drm_dev->dev_private;
 
@@ -885,7 +889,7 @@ static int mixer_initialize(struct exynos_drm_manager *mgr,
 
 static void mixer_mgr_remove(struct exynos_drm_manager *mgr)
 {
-	struct mixer_context *mixer_ctx = mgr->ctx;
+	struct mixer_context *mixer_ctx = mgr_to_mixer(mgr);
 
 	if (is_drm_iommu_supported(mixer_ctx->drm_dev))
 		drm_iommu_detach_device(mixer_ctx->drm_dev, mixer_ctx->dev);
@@ -893,7 +897,7 @@ static void mixer_mgr_remove(struct exynos_drm_manager *mgr)
 
 static int mixer_enable_vblank(struct exynos_drm_manager *mgr)
 {
-	struct mixer_context *mixer_ctx = mgr->ctx;
+	struct mixer_context *mixer_ctx = mgr_to_mixer(mgr);
 	struct mixer_resources *res = &mixer_ctx->mixer_res;
 
 	if (!mixer_ctx->powered) {
@@ -910,7 +914,7 @@ static int mixer_enable_vblank(struct exynos_drm_manager *mgr)
 
 static void mixer_disable_vblank(struct exynos_drm_manager *mgr)
 {
-	struct mixer_context *mixer_ctx = mgr->ctx;
+	struct mixer_context *mixer_ctx = mgr_to_mixer(mgr);
 	struct mixer_resources *res = &mixer_ctx->mixer_res;
 
 	/* disable vsync interrupt */
@@ -920,7 +924,7 @@ static void mixer_disable_vblank(struct exynos_drm_manager *mgr)
 static void mixer_win_mode_set(struct exynos_drm_manager *mgr,
 			struct exynos_drm_overlay *overlay)
 {
-	struct mixer_context *mixer_ctx = mgr->ctx;
+	struct mixer_context *mixer_ctx = mgr_to_mixer(mgr);
 	struct hdmi_win_data *win_data;
 	int win;
 
@@ -971,7 +975,7 @@ static void mixer_win_mode_set(struct exynos_drm_manager *mgr,
 
 static void mixer_win_commit(struct exynos_drm_manager *mgr, int zpos)
 {
-	struct mixer_context *mixer_ctx = mgr->ctx;
+	struct mixer_context *mixer_ctx = mgr_to_mixer(mgr);
 	int win = zpos == DEFAULT_ZPOS ? MIXER_DEFAULT_WIN : zpos;
 
 	DRM_DEBUG_KMS("win: %d\n", win);
@@ -993,7 +997,7 @@ static void mixer_win_commit(struct exynos_drm_manager *mgr, int zpos)
 
 static void mixer_win_disable(struct exynos_drm_manager *mgr, int zpos)
 {
-	struct mixer_context *mixer_ctx = mgr->ctx;
+	struct mixer_context *mixer_ctx = mgr_to_mixer(mgr);
 	struct mixer_resources *res = &mixer_ctx->mixer_res;
 	int win = zpos == DEFAULT_ZPOS ? MIXER_DEFAULT_WIN : zpos;
 	unsigned long flags;
@@ -1021,7 +1025,7 @@ static void mixer_win_disable(struct exynos_drm_manager *mgr, int zpos)
 
 static void mixer_wait_for_vblank(struct exynos_drm_manager *mgr)
 {
-	struct mixer_context *mixer_ctx = mgr->ctx;
+	struct mixer_context *mixer_ctx = mgr_to_mixer(mgr);
 
 	mutex_lock(&mixer_ctx->mixer_mutex);
 	if (!mixer_ctx->powered) {
@@ -1048,7 +1052,7 @@ static void mixer_wait_for_vblank(struct exynos_drm_manager *mgr)
 
 static void mixer_window_suspend(struct exynos_drm_manager *mgr)
 {
-	struct mixer_context *ctx = mgr->ctx;
+	struct mixer_context *ctx = mgr_to_mixer(mgr);
 	struct hdmi_win_data *win_data;
 	int i;
 
@@ -1062,7 +1066,7 @@ static void mixer_window_suspend(struct exynos_drm_manager *mgr)
 
 static void mixer_window_resume(struct exynos_drm_manager *mgr)
 {
-	struct mixer_context *ctx = mgr->ctx;
+	struct mixer_context *ctx = mgr_to_mixer(mgr);
 	struct hdmi_win_data *win_data;
 	int i;
 
@@ -1077,7 +1081,7 @@ static void mixer_window_resume(struct exynos_drm_manager *mgr)
 
 static void mixer_poweron(struct exynos_drm_manager *mgr)
 {
-	struct mixer_context *ctx = mgr->ctx;
+	struct mixer_context *ctx = mgr_to_mixer(mgr);
 	struct mixer_resources *res = &ctx->mixer_res;
 
 	mutex_lock(&ctx->mixer_mutex);
@@ -1111,7 +1115,7 @@ static void mixer_poweron(struct exynos_drm_manager *mgr)
 
 static void mixer_poweroff(struct exynos_drm_manager *mgr)
 {
-	struct mixer_context *ctx = mgr->ctx;
+	struct mixer_context *ctx = mgr_to_mixer(mgr);
 	struct mixer_resources *res = &ctx->mixer_res;
 
 	mutex_lock(&ctx->mixer_mutex);
@@ -1187,11 +1191,6 @@ static struct exynos_drm_manager_ops mixer_manager_ops = {
 	.win_disable		= mixer_win_disable,
 };
 
-static struct exynos_drm_manager mixer_manager = {
-	.type			= EXYNOS_DISPLAY_TYPE_HDMI,
-	.ops			= &mixer_manager_ops,
-};
-
 static struct mixer_drv_data exynos5420_mxr_drv_data = {
 	.version = MXR_VER_128_0_0_184,
 	.is_vp_enabled = 0,
@@ -1249,48 +1248,17 @@ MODULE_DEVICE_TABLE(of, mixer_match_types);
 
 static int mixer_bind(struct device *dev, struct device *manager, void *data)
 {
-	struct platform_device *pdev = to_platform_device(dev);
+	struct mixer_context *ctx = dev_get_drvdata(dev);
 	struct drm_device *drm_dev = data;
-	struct mixer_context *ctx;
-	struct mixer_drv_data *drv;
 	int ret;
 
-	dev_info(dev, "probe start\n");
-
-	ctx = devm_kzalloc(&pdev->dev, sizeof(*ctx), GFP_KERNEL);
-	if (!ctx) {
-		DRM_ERROR("failed to alloc mixer context.\n");
-		return -ENOMEM;
-	}
-
-	mutex_init(&ctx->mixer_mutex);
-
-	if (dev->of_node) {
-		const struct of_device_id *match;
-		match = of_match_node(mixer_match_types, dev->of_node);
-		drv = (struct mixer_drv_data *)match->data;
-	} else {
-		drv = (struct mixer_drv_data *)
-			platform_get_device_id(pdev)->driver_data;
-	}
-
-	ctx->pdev = pdev;
-	ctx->dev = dev;
-	ctx->vp_enabled = drv->is_vp_enabled;
-	ctx->has_sclk = drv->has_sclk;
-	ctx->mxr_ver = drv->version;
-	init_waitqueue_head(&ctx->wait_vsync_queue);
-	atomic_set(&ctx->wait_vsync_event, 0);
-
-	mixer_manager.ctx = ctx;
-	ret = mixer_initialize(&mixer_manager, drm_dev);
+	ret = mixer_initialize(&ctx->manager, drm_dev);
 	if (ret)
 		return ret;
 
-	platform_set_drvdata(pdev, &mixer_manager);
-	ret = exynos_drm_crtc_create(&mixer_manager);
+	ret = exynos_drm_crtc_create(&ctx->manager);
 	if (ret) {
-		mixer_mgr_remove(&mixer_manager);
+		mixer_mgr_remove(&ctx->manager);
 		return ret;
 	}
 
@@ -1301,11 +1269,9 @@ static int mixer_bind(struct device *dev, struct device *manager, void *data)
 
 static void mixer_unbind(struct device *dev, struct device *master, void *data)
 {
-	struct exynos_drm_manager *mgr = dev_get_drvdata(dev);
+	struct mixer_context *ctx = dev_get_drvdata(dev);
 
-	dev_info(dev, "remove successful\n");
-
-	mixer_mgr_remove(mgr);
+	mixer_mgr_remove(&ctx->manager);
 
 	pm_runtime_disable(dev);
 }
@@ -1317,22 +1283,62 @@ static const struct component_ops mixer_component_ops = {
 
 static int mixer_probe(struct platform_device *pdev)
 {
+	struct device *dev = &pdev->dev;
+	struct mixer_drv_data *drv;
+	struct mixer_context *ctx;
 	int ret;
 
+	ctx = devm_kzalloc(&pdev->dev, sizeof(*ctx), GFP_KERNEL);
+	if (!ctx) {
+		DRM_ERROR("failed to alloc mixer context.\n");
+		return -ENOMEM;
+	}
+
+	mutex_init(&ctx->mixer_mutex);
+
+	ctx->manager.type = EXYNOS_DISPLAY_TYPE_HDMI;
+	ctx->manager.ops = &mixer_manager_ops;
+
+	if (dev->of_node) {
+		const struct of_device_id *match;
+
+		match = of_match_node(mixer_match_types, dev->of_node);
+		drv = (struct mixer_drv_data *)match->data;
+	} else {
+		drv = (struct mixer_drv_data *)
+			platform_get_device_id(pdev)->driver_data;
+	}
+
+	ctx->pdev = pdev;
+	ctx->dev = dev;
+	ctx->vp_enabled = drv->is_vp_enabled;
+	ctx->has_sclk = drv->has_sclk;
+	ctx->mxr_ver = drv->version;
+	init_waitqueue_head(&ctx->wait_vsync_queue);
+	atomic_set(&ctx->wait_vsync_event, 0);
+
+	platform_set_drvdata(pdev, ctx);
+
 	ret = exynos_drm_component_add(&pdev->dev, EXYNOS_DEVICE_TYPE_CRTC,
-					mixer_manager.type);
+					ctx->manager.type);
 	if (ret)
 		return ret;
 
 	ret = component_add(&pdev->dev, &mixer_component_ops);
-	if (ret)
+	if (ret) {
 		exynos_drm_component_del(&pdev->dev, EXYNOS_DEVICE_TYPE_CRTC);
+		return ret;
+	}
+
+	pm_runtime_enable(dev);
 
 	return ret;
 }
 
 static int mixer_remove(struct platform_device *pdev)
 {
+	pm_runtime_disable(&pdev->dev);
+
 	component_del(&pdev->dev, &mixer_component_ops);
 	exynos_drm_component_del(&pdev->dev, EXYNOS_DEVICE_TYPE_CRTC);
 
diff --git a/drivers/gpu/drm/gma500/Makefile b/drivers/gpu/drm/gma500/Makefile
index b15315576376..190e55f2f891 100644
--- a/drivers/gpu/drm/gma500/Makefile
+++ b/drivers/gpu/drm/gma500/Makefile
@@ -39,6 +39,7 @@ gma500_gfx-$(CONFIG_DRM_GMA3600) +=  cdv_device.o \
 gma500_gfx-$(CONFIG_DRM_GMA600) += oaktrail_device.o \
 	  oaktrail_crtc.o \
 	  oaktrail_lvds.o \
+	  oaktrail_lvds_i2c.o \
 	  oaktrail_hdmi.o \
 	  oaktrail_hdmi_i2c.o
 
diff --git a/drivers/gpu/drm/gma500/cdv_intel_dp.c b/drivers/gpu/drm/gma500/cdv_intel_dp.c
index 9f158eab517a..0fafb8e2483a 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_dp.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_dp.c
@@ -37,6 +37,201 @@
 #include "gma_display.h"
 #include <drm/drm_dp_helper.h>
 
+/**
+ * struct i2c_algo_dp_aux_data - driver interface structure for i2c over dp
+ * 				 aux algorithm
+ * @running: set by the algo indicating whether an i2c is ongoing or whether
+ * 	     the i2c bus is quiescent
+ * @address: i2c target address for the currently ongoing transfer
+ * @aux_ch: driver callback to transfer a single byte of the i2c payload
+ */
+struct i2c_algo_dp_aux_data {
+	bool running;
+	u16 address;
+	int (*aux_ch) (struct i2c_adapter *adapter,
+		       int mode, uint8_t write_byte,
+		       uint8_t *read_byte);
+};
+
+/* Run a single AUX_CH I2C transaction, writing/reading data as necessary */
+static int
+i2c_algo_dp_aux_transaction(struct i2c_adapter *adapter, int mode,
+			    uint8_t write_byte, uint8_t *read_byte)
+{
+	struct i2c_algo_dp_aux_data *algo_data = adapter->algo_data;
+	int ret;
+
+	ret = (*algo_data->aux_ch)(adapter, mode,
+				   write_byte, read_byte);
+	return ret;
+}
+
+/*
+ * I2C over AUX CH
+ */
+
+/*
+ * Send the address. If the I2C link is running, this 'restarts'
+ * the connection with the new address, this is used for doing
+ * a write followed by a read (as needed for DDC)
+ */
+static int
+i2c_algo_dp_aux_address(struct i2c_adapter *adapter, u16 address, bool reading)
+{
+	struct i2c_algo_dp_aux_data *algo_data = adapter->algo_data;
+	int mode = MODE_I2C_START;
+	int ret;
+
+	if (reading)
+		mode |= MODE_I2C_READ;
+	else
+		mode |= MODE_I2C_WRITE;
+	algo_data->address = address;
+	algo_data->running = true;
+	ret = i2c_algo_dp_aux_transaction(adapter, mode, 0, NULL);
+	return ret;
+}
+
+/*
+ * Stop the I2C transaction. This closes out the link, sending
+ * a bare address packet with the MOT bit turned off
+ */
+static void
+i2c_algo_dp_aux_stop(struct i2c_adapter *adapter, bool reading)
+{
+	struct i2c_algo_dp_aux_data *algo_data = adapter->algo_data;
+	int mode = MODE_I2C_STOP;
+
+	if (reading)
+		mode |= MODE_I2C_READ;
+	else
+		mode |= MODE_I2C_WRITE;
+	if (algo_data->running) {
+		(void) i2c_algo_dp_aux_transaction(adapter, mode, 0, NULL);
+		algo_data->running = false;
+	}
+}
+
+/*
+ * Write a single byte to the current I2C address, the
+ * the I2C link must be running or this returns -EIO
+ */
+static int
+i2c_algo_dp_aux_put_byte(struct i2c_adapter *adapter, u8 byte)
+{
+	struct i2c_algo_dp_aux_data *algo_data = adapter->algo_data;
+	int ret;
+
+	if (!algo_data->running)
+		return -EIO;
+
+	ret = i2c_algo_dp_aux_transaction(adapter, MODE_I2C_WRITE, byte, NULL);
+	return ret;
+}
+
+/*
+ * Read a single byte from the current I2C address, the
+ * I2C link must be running or this returns -EIO
+ */
+static int
+i2c_algo_dp_aux_get_byte(struct i2c_adapter *adapter, u8 *byte_ret)
+{
+	struct i2c_algo_dp_aux_data *algo_data = adapter->algo_data;
+	int ret;
+
+	if (!algo_data->running)
+		return -EIO;
+
+	ret = i2c_algo_dp_aux_transaction(adapter, MODE_I2C_READ, 0, byte_ret);
+	return ret;
+}
+
+static int
+i2c_algo_dp_aux_xfer(struct i2c_adapter *adapter,
+		     struct i2c_msg *msgs,
+		     int num)
+{
+	int ret = 0;
+	bool reading = false;
+	int m;
+	int b;
+
+	for (m = 0; m < num; m++) {
+		u16 len = msgs[m].len;
+		u8 *buf = msgs[m].buf;
+		reading = (msgs[m].flags & I2C_M_RD) != 0;
+		ret = i2c_algo_dp_aux_address(adapter, msgs[m].addr, reading);
+		if (ret < 0)
+			break;
+		if (reading) {
+			for (b = 0; b < len; b++) {
+				ret = i2c_algo_dp_aux_get_byte(adapter, &buf[b]);
+				if (ret < 0)
+					break;
+			}
+		} else {
+			for (b = 0; b < len; b++) {
+				ret = i2c_algo_dp_aux_put_byte(adapter, buf[b]);
+				if (ret < 0)
+					break;
+			}
+		}
+		if (ret < 0)
+			break;
+	}
+	if (ret >= 0)
+		ret = num;
+	i2c_algo_dp_aux_stop(adapter, reading);
+	DRM_DEBUG_KMS("dp_aux_xfer return %d\n", ret);
+	return ret;
+}
+
+static u32
+i2c_algo_dp_aux_functionality(struct i2c_adapter *adapter)
+{
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL |
+	       I2C_FUNC_SMBUS_READ_BLOCK_DATA |
+	       I2C_FUNC_SMBUS_BLOCK_PROC_CALL |
+	       I2C_FUNC_10BIT_ADDR;
+}
+
+static const struct i2c_algorithm i2c_dp_aux_algo = {
+	.master_xfer	= i2c_algo_dp_aux_xfer,
+	.functionality	= i2c_algo_dp_aux_functionality,
+};
+
+static void
+i2c_dp_aux_reset_bus(struct i2c_adapter *adapter)
+{
+	(void) i2c_algo_dp_aux_address(adapter, 0, false);
+	(void) i2c_algo_dp_aux_stop(adapter, false);
+}
+
+static int
+i2c_dp_aux_prepare_bus(struct i2c_adapter *adapter)
+{
+	adapter->algo = &i2c_dp_aux_algo;
+	adapter->retries = 3;
+	i2c_dp_aux_reset_bus(adapter);
+	return 0;
+}
+
+/*
+ * FIXME: This is the old dp aux helper, gma500 is the last driver that needs to
+ * be ported over to the new helper code in drm_dp_helper.c like i915 or radeon.
+ */
+static int __deprecated
+i2c_dp_aux_add_bus(struct i2c_adapter *adapter)
+{
+	int error;
+
+	error = i2c_dp_aux_prepare_bus(adapter);
+	if (error)
+		return error;
+	error = i2c_add_adapter(adapter);
+	return error;
+}
+
 #define _wait_for(COND, MS, W) ({ \
         unsigned long timeout__ = jiffies + msecs_to_jiffies(MS);       \
         int ret__ = 0;                                                  \
diff --git a/drivers/gpu/drm/gma500/mdfld_dsi_pkg_sender.c b/drivers/gpu/drm/gma500/mdfld_dsi_pkg_sender.c
index 87885d8c06e8..6b43ae3ffd73 100644
--- a/drivers/gpu/drm/gma500/mdfld_dsi_pkg_sender.c
+++ b/drivers/gpu/drm/gma500/mdfld_dsi_pkg_sender.c
@@ -25,6 +25,7 @@
  */
 
 #include <linux/freezer.h>
+#include <video/mipi_display.h>
 
 #include "mdfld_dsi_output.h"
 #include "mdfld_dsi_pkg_sender.h"
@@ -32,20 +33,6 @@
 
 #define MDFLD_DSI_READ_MAX_COUNT		5000
 
-enum data_type {
-	DSI_DT_GENERIC_SHORT_WRITE_0	= 0x03,
-	DSI_DT_GENERIC_SHORT_WRITE_1	= 0x13,
-	DSI_DT_GENERIC_SHORT_WRITE_2	= 0x23,
-	DSI_DT_GENERIC_READ_0		= 0x04,
-	DSI_DT_GENERIC_READ_1		= 0x14,
-	DSI_DT_GENERIC_READ_2		= 0x24,
-	DSI_DT_GENERIC_LONG_WRITE	= 0x29,
-	DSI_DT_DCS_SHORT_WRITE_0	= 0x05,
-	DSI_DT_DCS_SHORT_WRITE_1	= 0x15,
-	DSI_DT_DCS_READ			= 0x06,
-	DSI_DT_DCS_LONG_WRITE		= 0x39,
-};
-
 enum {
 	MDFLD_DSI_PANEL_MODE_SLEEP = 0x1,
 };
@@ -321,9 +308,9 @@ static int send_pkg_prepare(struct mdfld_dsi_pkg_sender *sender, u8 data_type,
 	u8 cmd;
 
 	switch (data_type) {
-	case DSI_DT_DCS_SHORT_WRITE_0:
-	case DSI_DT_DCS_SHORT_WRITE_1:
-	case DSI_DT_DCS_LONG_WRITE:
+	case MIPI_DSI_DCS_SHORT_WRITE:
+	case MIPI_DSI_DCS_SHORT_WRITE_PARAM:
+	case MIPI_DSI_DCS_LONG_WRITE:
 		cmd = *data;
 		break;
 	default:
@@ -334,12 +321,12 @@ static int send_pkg_prepare(struct mdfld_dsi_pkg_sender *sender, u8 data_type,
 	sender->status = MDFLD_DSI_PKG_SENDER_BUSY;
 
 	/*wait for 120 milliseconds in case exit_sleep_mode just be sent*/
-	if (unlikely(cmd == DCS_ENTER_SLEEP_MODE)) {
+	if (unlikely(cmd == MIPI_DCS_ENTER_SLEEP_MODE)) {
 		/*TODO: replace it with msleep later*/
 		mdelay(120);
 	}
 
-	if (unlikely(cmd == DCS_EXIT_SLEEP_MODE)) {
+	if (unlikely(cmd == MIPI_DCS_EXIT_SLEEP_MODE)) {
 		/*TODO: replace it with msleep later*/
 		mdelay(120);
 	}
@@ -352,9 +339,9 @@ static int send_pkg_done(struct mdfld_dsi_pkg_sender *sender, u8 data_type,
 	u8 cmd;
 
 	switch (data_type) {
-	case DSI_DT_DCS_SHORT_WRITE_0:
-	case DSI_DT_DCS_SHORT_WRITE_1:
-	case DSI_DT_DCS_LONG_WRITE:
+	case MIPI_DSI_DCS_SHORT_WRITE:
+	case MIPI_DSI_DCS_SHORT_WRITE_PARAM:
+	case MIPI_DSI_DCS_LONG_WRITE:
 		cmd = *data;
 		break;
 	default:
@@ -362,15 +349,15 @@ static int send_pkg_done(struct mdfld_dsi_pkg_sender *sender, u8 data_type,
 	}
 
 	/*update panel status*/
-	if (unlikely(cmd == DCS_ENTER_SLEEP_MODE)) {
+	if (unlikely(cmd == MIPI_DCS_ENTER_SLEEP_MODE)) {
 		sender->panel_mode |= MDFLD_DSI_PANEL_MODE_SLEEP;
 		/*TODO: replace it with msleep later*/
 		mdelay(120);
-	} else if (unlikely(cmd == DCS_EXIT_SLEEP_MODE)) {
+	} else if (unlikely(cmd == MIPI_DCS_EXIT_SLEEP_MODE)) {
 		sender->panel_mode &= ~MDFLD_DSI_PANEL_MODE_SLEEP;
 		/*TODO: replace it with msleep later*/
 		mdelay(120);
-	} else if (unlikely(cmd == DCS_SOFT_RESET)) {
+	} else if (unlikely(cmd == MIPI_DCS_SOFT_RESET)) {
 		/*TODO: replace it with msleep later*/
 		mdelay(5);
 	}
@@ -405,19 +392,19 @@ static int send_pkg(struct mdfld_dsi_pkg_sender *sender, u8 data_type,
 	}
 
 	switch (data_type) {
-	case DSI_DT_GENERIC_SHORT_WRITE_0:
-	case DSI_DT_GENERIC_SHORT_WRITE_1:
-	case DSI_DT_GENERIC_SHORT_WRITE_2:
-	case DSI_DT_GENERIC_READ_0:
-	case DSI_DT_GENERIC_READ_1:
-	case DSI_DT_GENERIC_READ_2:
-	case DSI_DT_DCS_SHORT_WRITE_0:
-	case DSI_DT_DCS_SHORT_WRITE_1:
-	case DSI_DT_DCS_READ:
+	case MIPI_DSI_GENERIC_SHORT_WRITE_0_PARAM:
+	case MIPI_DSI_GENERIC_SHORT_WRITE_1_PARAM:
+	case MIPI_DSI_GENERIC_SHORT_WRITE_2_PARAM:
+	case MIPI_DSI_GENERIC_READ_REQUEST_0_PARAM:
+	case MIPI_DSI_GENERIC_READ_REQUEST_1_PARAM:
+	case MIPI_DSI_GENERIC_READ_REQUEST_2_PARAM:
+	case MIPI_DSI_DCS_SHORT_WRITE:
+	case MIPI_DSI_DCS_SHORT_WRITE_PARAM:
+	case MIPI_DSI_DCS_READ:
 		ret = send_short_pkg(sender, data_type, data[0], data[1], hs);
 		break;
-	case DSI_DT_GENERIC_LONG_WRITE:
-	case DSI_DT_DCS_LONG_WRITE:
+	case MIPI_DSI_GENERIC_LONG_WRITE:
+	case MIPI_DSI_DCS_LONG_WRITE:
 		ret = send_long_pkg(sender, data_type, data, len, hs);
 		break;
 	}
@@ -440,7 +427,7 @@ int mdfld_dsi_send_mcs_long(struct mdfld_dsi_pkg_sender *sender, u8 *data,
 	}
 
 	spin_lock_irqsave(&sender->lock, flags);
-	send_pkg(sender, DSI_DT_DCS_LONG_WRITE, data, len, hs);
+	send_pkg(sender, MIPI_DSI_DCS_LONG_WRITE, data, len, hs);
 	spin_unlock_irqrestore(&sender->lock, flags);
 
 	return 0;
@@ -461,10 +448,10 @@ int mdfld_dsi_send_mcs_short(struct mdfld_dsi_pkg_sender *sender, u8 cmd,
 	data[0] = cmd;
 
 	if (param_num) {
-		data_type = DSI_DT_DCS_SHORT_WRITE_1;
+		data_type = MIPI_DSI_DCS_SHORT_WRITE_PARAM;
 		data[1] = param;
 	} else {
-		data_type = DSI_DT_DCS_SHORT_WRITE_0;
+		data_type = MIPI_DSI_DCS_SHORT_WRITE;
 		data[1] = 0;
 	}
 
@@ -489,17 +476,17 @@ int mdfld_dsi_send_gen_short(struct mdfld_dsi_pkg_sender *sender, u8 param0,
 
 	switch (param_num) {
 	case 0:
-		data_type = DSI_DT_GENERIC_SHORT_WRITE_0;
+		data_type = MIPI_DSI_GENERIC_SHORT_WRITE_0_PARAM;
 		data[0] = 0;
 		data[1] = 0;
 		break;
 	case 1:
-		data_type = DSI_DT_GENERIC_SHORT_WRITE_1;
+		data_type = MIPI_DSI_GENERIC_SHORT_WRITE_1_PARAM;
 		data[0] = param0;
 		data[1] = 0;
 		break;
 	case 2:
-		data_type = DSI_DT_GENERIC_SHORT_WRITE_2;
+		data_type = MIPI_DSI_GENERIC_SHORT_WRITE_2_PARAM;
 		data[0] = param0;
 		data[1] = param1;
 		break;
@@ -523,7 +510,7 @@ int mdfld_dsi_send_gen_long(struct mdfld_dsi_pkg_sender *sender, u8 *data,
 	}
 
 	spin_lock_irqsave(&sender->lock, flags);
-	send_pkg(sender, DSI_DT_GENERIC_LONG_WRITE, data, len, hs);
+	send_pkg(sender, MIPI_DSI_GENERIC_LONG_WRITE, data, len, hs);
 	spin_unlock_irqrestore(&sender->lock, flags);
 
 	return 0;
@@ -594,7 +581,7 @@ int mdfld_dsi_read_mcs(struct mdfld_dsi_pkg_sender *sender, u8 cmd,
 		return -EINVAL;
 	}
 
-	return __read_panel_data(sender, DSI_DT_DCS_READ, &cmd, 1,
+	return __read_panel_data(sender, MIPI_DSI_DCS_READ, &cmd, 1,
 				data, len, hs);
 }
 
diff --git a/drivers/gpu/drm/gma500/mdfld_dsi_pkg_sender.h b/drivers/gpu/drm/gma500/mdfld_dsi_pkg_sender.h
index 459cd7ea8b81..0478a21c15d5 100644
--- a/drivers/gpu/drm/gma500/mdfld_dsi_pkg_sender.h
+++ b/drivers/gpu/drm/gma500/mdfld_dsi_pkg_sender.h
@@ -62,18 +62,6 @@ struct mdfld_dsi_pkg_sender {
 	u32 mipi_cmd_len_reg;
 };
 
-/* DCS definitions */
-#define DCS_SOFT_RESET			0x01
-#define DCS_ENTER_SLEEP_MODE		0x10
-#define DCS_EXIT_SLEEP_MODE		0x11
-#define DCS_SET_DISPLAY_OFF		0x28
-#define DCS_SET_DISPLAY_ON		0x29
-#define DCS_SET_COLUMN_ADDRESS		0x2a
-#define DCS_SET_PAGE_ADDRESS		0x2b
-#define DCS_WRITE_MEM_START		0x2c
-#define DCS_SET_TEAR_OFF		0x34
-#define DCS_SET_TEAR_ON			0x35
-
 extern int mdfld_dsi_pkg_sender_init(struct mdfld_dsi_connector *dsi_connector,
 					int pipe);
 extern void mdfld_dsi_pkg_sender_destroy(struct mdfld_dsi_pkg_sender *sender);
diff --git a/drivers/gpu/drm/gma500/oaktrail_lvds.c b/drivers/gpu/drm/gma500/oaktrail_lvds.c
index 0d39da6e8b7a..83bbc271bcfb 100644
--- a/drivers/gpu/drm/gma500/oaktrail_lvds.c
+++ b/drivers/gpu/drm/gma500/oaktrail_lvds.c
@@ -359,22 +359,26 @@ void oaktrail_lvds_init(struct drm_device *dev,
 	 *    if closed, act like it's not there for now
 	 */
 
+	edid = NULL;
 	mutex_lock(&dev->mode_config.mutex);
 	i2c_adap = i2c_get_adapter(dev_priv->ops->i2c_bus);
-	if (i2c_adap == NULL)
-		dev_err(dev->dev, "No ddc adapter available!\n");
+	if (i2c_adap)
+		edid = drm_get_edid(connector, i2c_adap);
+	if (edid == NULL && dev_priv->lpc_gpio_base) {
+		oaktrail_lvds_i2c_init(encoder);
+		if (gma_encoder->ddc_bus != NULL) {
+			i2c_adap = &gma_encoder->ddc_bus->adapter;
+			edid = drm_get_edid(connector, i2c_adap);
+		}
+	}
 	/*
 	 * Attempt to get the fixed panel mode from DDC.  Assume that the
 	 * preferred mode is the right one.
 	 */
-	if (i2c_adap) {
-		edid = drm_get_edid(connector, i2c_adap);
-		if (edid) {
-			drm_mode_connector_update_edid_property(connector,
-									edid);
-			drm_add_edid_modes(connector, edid);
-			kfree(edid);
-		}
+	if (edid) {
+		drm_mode_connector_update_edid_property(connector, edid);
+		drm_add_edid_modes(connector, edid);
+		kfree(edid);
 
 		list_for_each_entry(scan, &connector->probed_modes, head) {
 			if (scan->type & DRM_MODE_TYPE_PREFERRED) {
@@ -383,7 +387,8 @@ void oaktrail_lvds_init(struct drm_device *dev,
 				goto out;	/* FIXME: check for quirks */
 			}
 		}
-	}
+	} else
+		dev_err(dev->dev, "No ddc adapter available!\n");
 	/*
 	 * If we didn't get EDID, try geting panel timing
 	 * from configuration data
@@ -411,8 +416,10 @@ failed_find:
 	mutex_unlock(&dev->mode_config.mutex);
 
 	dev_dbg(dev->dev, "No LVDS modes found, disabling.\n");
-	if (gma_encoder->ddc_bus)
+	if (gma_encoder->ddc_bus) {
 		psb_intel_i2c_destroy(gma_encoder->ddc_bus);
+		gma_encoder->ddc_bus = NULL;
+	}
 
 /* failed_ddc: */
 
diff --git a/drivers/gpu/drm/gma500/oaktrail_lvds_i2c.c b/drivers/gpu/drm/gma500/oaktrail_lvds_i2c.c
new file mode 100644
index 000000000000..f913a62eee5f
--- /dev/null
+++ b/drivers/gpu/drm/gma500/oaktrail_lvds_i2c.c
@@ -0,0 +1,170 @@
+/*
+ * Copyright (c) 2002-2010, Intel Corporation.
+ * Copyright (c) 2014 ATRON electronic GmbH
+ *   Author: Jan Safrata <jan.nikitenko@gmail.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 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/kernel.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/types.h>
+#include <linux/i2c.h>
+#include <linux/i2c-algo-bit.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+
+#include <drm/drmP.h>
+#include "psb_drv.h"
+#include "psb_intel_reg.h"
+
+
+/*
+ * LPC GPIO based I2C bus for LVDS of Atom E6xx
+ */
+
+/*-----------------------------------------------------------------------------
+ * LPC Register Offsets. Used for LVDS GPIO Bit Bashing. Registers are part
+ * Atom E6xx [D31:F0]
+ ----------------------------------------------------------------------------*/
+#define RGEN    0x20
+#define RGIO    0x24
+#define RGLVL   0x28
+#define RGTPE   0x2C
+#define RGTNE   0x30
+#define RGGPE   0x34
+#define RGSMI   0x38
+#define RGTS    0x3C
+
+/* The LVDS GPIO clock lines are GPIOSUS[3]
+ * The LVDS GPIO data lines are GPIOSUS[4]
+ */
+#define GPIO_CLOCK	0x08
+#define GPIO_DATA	0x10
+
+#define LPC_READ_REG(chan, r) inl((chan)->reg + (r))
+#define LPC_WRITE_REG(chan, r, val) outl((val), (chan)->reg + (r))
+
+static int get_clock(void *data)
+{
+	struct psb_intel_i2c_chan *chan = data;
+	u32 val, tmp;
+
+	val = LPC_READ_REG(chan, RGIO);
+	val |= GPIO_CLOCK;
+	LPC_WRITE_REG(chan, RGIO, val);
+	tmp = LPC_READ_REG(chan, RGLVL);
+	val = (LPC_READ_REG(chan, RGLVL) & GPIO_CLOCK) ? 1 : 0;
+
+	return val;
+}
+
+static int get_data(void *data)
+{
+	struct psb_intel_i2c_chan *chan = data;
+	u32 val, tmp;
+
+	val = LPC_READ_REG(chan, RGIO);
+	val |= GPIO_DATA;
+	LPC_WRITE_REG(chan, RGIO, val);
+	tmp = LPC_READ_REG(chan, RGLVL);
+	val = (LPC_READ_REG(chan, RGLVL) & GPIO_DATA) ? 1 : 0;
+
+	return val;
+}
+
+static void set_clock(void *data, int state_high)
+{
+	struct psb_intel_i2c_chan *chan = data;
+	u32 val;
+
+	if (state_high) {
+		val = LPC_READ_REG(chan, RGIO);
+		val |= GPIO_CLOCK;
+		LPC_WRITE_REG(chan, RGIO, val);
+	} else {
+		val = LPC_READ_REG(chan, RGIO);
+		val &= ~GPIO_CLOCK;
+		LPC_WRITE_REG(chan, RGIO, val);
+		val = LPC_READ_REG(chan, RGLVL);
+		val &= ~GPIO_CLOCK;
+		LPC_WRITE_REG(chan, RGLVL, val);
+	}
+}
+
+static void set_data(void *data, int state_high)
+{
+	struct psb_intel_i2c_chan *chan = data;
+	u32 val;
+
+	if (state_high) {
+		val = LPC_READ_REG(chan, RGIO);
+		val |= GPIO_DATA;
+		LPC_WRITE_REG(chan, RGIO, val);
+	} else {
+		val = LPC_READ_REG(chan, RGIO);
+		val &= ~GPIO_DATA;
+		LPC_WRITE_REG(chan, RGIO, val);
+		val = LPC_READ_REG(chan, RGLVL);
+		val &= ~GPIO_DATA;
+		LPC_WRITE_REG(chan, RGLVL, val);
+	}
+}
+
+void oaktrail_lvds_i2c_init(struct drm_encoder *encoder)
+{
+	struct drm_device *dev = encoder->dev;
+	struct gma_encoder *gma_encoder = to_gma_encoder(encoder);
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_i2c_chan *chan;
+
+	chan = kzalloc(sizeof(struct psb_intel_i2c_chan), GFP_KERNEL);
+	if (!chan)
+		return;
+
+	chan->drm_dev = dev;
+	chan->reg = dev_priv->lpc_gpio_base;
+	strncpy(chan->adapter.name, "gma500 LPC",  I2C_NAME_SIZE - 1);
+	chan->adapter.owner = THIS_MODULE;
+	chan->adapter.algo_data = &chan->algo;
+	chan->adapter.dev.parent = &dev->pdev->dev;
+	chan->algo.setsda = set_data;
+	chan->algo.setscl = set_clock;
+	chan->algo.getsda = get_data;
+	chan->algo.getscl = get_clock;
+	chan->algo.udelay = 100;
+	chan->algo.timeout = usecs_to_jiffies(2200);
+	chan->algo.data = chan;
+
+	i2c_set_adapdata(&chan->adapter, chan);
+
+	set_data(chan, 1);
+	set_clock(chan, 1);
+	udelay(50);
+
+	if (i2c_bit_add_bus(&chan->adapter)) {
+		kfree(chan);
+		return;
+	}
+
+	gma_encoder->ddc_bus = chan;
+}
diff --git a/drivers/gpu/drm/gma500/psb_drv.c b/drivers/gpu/drm/gma500/psb_drv.c
index 6ec3a905fdd2..92e7e5795398 100644
--- a/drivers/gpu/drm/gma500/psb_drv.c
+++ b/drivers/gpu/drm/gma500/psb_drv.c
@@ -212,6 +212,8 @@ static int psb_driver_unload(struct drm_device *dev)
 		}
 		if (dev_priv->aux_pdev)
 			pci_dev_put(dev_priv->aux_pdev);
+		if (dev_priv->lpc_pdev)
+			pci_dev_put(dev_priv->lpc_pdev);
 
 		/* Destroy VBT data */
 		psb_intel_destroy_bios(dev);
@@ -280,6 +282,24 @@ static int psb_driver_load(struct drm_device *dev, unsigned long flags)
 			DRM_DEBUG_KMS("Couldn't find aux pci device");
 		}
 		dev_priv->gmbus_reg = dev_priv->aux_reg;
+
+		dev_priv->lpc_pdev = pci_get_bus_and_slot(0, PCI_DEVFN(31, 0));
+		if (dev_priv->lpc_pdev) {
+			pci_read_config_word(dev_priv->lpc_pdev, PSB_LPC_GBA,
+				&dev_priv->lpc_gpio_base);
+			pci_write_config_dword(dev_priv->lpc_pdev, PSB_LPC_GBA,
+				(u32)dev_priv->lpc_gpio_base | (1L<<31));
+			pci_read_config_word(dev_priv->lpc_pdev, PSB_LPC_GBA,
+				&dev_priv->lpc_gpio_base);
+			dev_priv->lpc_gpio_base &= 0xffc0;
+			if (dev_priv->lpc_gpio_base)
+				DRM_DEBUG_KMS("Found LPC GPIO at 0x%04x\n",
+						dev_priv->lpc_gpio_base);
+			else {
+				pci_dev_put(dev_priv->lpc_pdev);
+				dev_priv->lpc_pdev = NULL;
+			}
+		}
 	} else {
 		dev_priv->gmbus_reg = dev_priv->vdc_reg;
 	}
diff --git a/drivers/gpu/drm/gma500/psb_drv.h b/drivers/gpu/drm/gma500/psb_drv.h
index 55ebe2bd88dd..e38057b91865 100644
--- a/drivers/gpu/drm/gma500/psb_drv.h
+++ b/drivers/gpu/drm/gma500/psb_drv.h
@@ -83,6 +83,7 @@ enum {
 #define PSB_PGETBL_CTL		 0x2020
 #define _PSB_PGETBL_ENABLED	 0x00000001
 #define PSB_SGX_2D_SLAVE_PORT	 0x4000
+#define PSB_LPC_GBA		 0x44
 
 /* TODO: To get rid of */
 #define PSB_TT_PRIV0_LIMIT	 (256*1024*1024)
@@ -441,6 +442,7 @@ struct psb_ops;
 struct drm_psb_private {
 	struct drm_device *dev;
 	struct pci_dev *aux_pdev; /* Currently only used by mrst */
+	struct pci_dev *lpc_pdev; /* Currently only used by mrst */
 	const struct psb_ops *ops;
 	const struct psb_offset *regmap;
 	
@@ -470,6 +472,7 @@ struct drm_psb_private {
 	uint8_t __iomem *sgx_reg;
 	uint8_t __iomem *vdc_reg;
 	uint8_t __iomem *aux_reg; /* Auxillary vdc pipe regs */
+	uint16_t lpc_gpio_base;
 	uint32_t gatt_free_offset;
 
 	/* Fencing / irq */
diff --git a/drivers/gpu/drm/gma500/psb_intel_display.c b/drivers/gpu/drm/gma500/psb_intel_display.c
index 87b50ba64ed4..b21a09451d1d 100644
--- a/drivers/gpu/drm/gma500/psb_intel_display.c
+++ b/drivers/gpu/drm/gma500/psb_intel_display.c
@@ -21,6 +21,7 @@
 #include <linux/i2c.h>
 
 #include <drm/drmP.h>
+#include <drm/drm_plane_helper.h>
 #include "framebuffer.h"
 #include "psb_drv.h"
 #include "psb_intel_drv.h"
diff --git a/drivers/gpu/drm/gma500/psb_intel_drv.h b/drivers/gpu/drm/gma500/psb_intel_drv.h
index 336bd3aa1a06..860dd2177ca1 100644
--- a/drivers/gpu/drm/gma500/psb_intel_drv.h
+++ b/drivers/gpu/drm/gma500/psb_intel_drv.h
@@ -223,6 +223,7 @@ extern void oaktrail_lvds_init(struct drm_device *dev,
 extern void oaktrail_wait_for_INTR_PKT_SENT(struct drm_device *dev);
 extern void oaktrail_dsi_init(struct drm_device *dev,
 			   struct psb_intel_mode_device *mode_dev);
+extern void oaktrail_lvds_i2c_init(struct drm_encoder *encoder);
 extern void mid_dsi_init(struct drm_device *dev,
 		    struct psb_intel_mode_device *mode_dev, int dsi_num);
 
diff --git a/drivers/gpu/drm/gma500/psb_intel_sdvo.c b/drivers/gpu/drm/gma500/psb_intel_sdvo.c
index 0be96fdb5e28..58529cea575d 100644
--- a/drivers/gpu/drm/gma500/psb_intel_sdvo.c
+++ b/drivers/gpu/drm/gma500/psb_intel_sdvo.c
@@ -1631,57 +1631,8 @@ static int psb_intel_sdvo_get_modes(struct drm_connector *connector)
 	return !list_empty(&connector->probed_modes);
 }
 
-static void
-psb_intel_sdvo_destroy_enhance_property(struct drm_connector *connector)
-{
-	struct psb_intel_sdvo_connector *psb_intel_sdvo_connector = to_psb_intel_sdvo_connector(connector);
-	struct drm_device *dev = connector->dev;
-
-	if (psb_intel_sdvo_connector->left)
-		drm_property_destroy(dev, psb_intel_sdvo_connector->left);
-	if (psb_intel_sdvo_connector->right)
-		drm_property_destroy(dev, psb_intel_sdvo_connector->right);
-	if (psb_intel_sdvo_connector->top)
-		drm_property_destroy(dev, psb_intel_sdvo_connector->top);
-	if (psb_intel_sdvo_connector->bottom)
-		drm_property_destroy(dev, psb_intel_sdvo_connector->bottom);
-	if (psb_intel_sdvo_connector->hpos)
-		drm_property_destroy(dev, psb_intel_sdvo_connector->hpos);
-	if (psb_intel_sdvo_connector->vpos)
-		drm_property_destroy(dev, psb_intel_sdvo_connector->vpos);
-	if (psb_intel_sdvo_connector->saturation)
-		drm_property_destroy(dev, psb_intel_sdvo_connector->saturation);
-	if (psb_intel_sdvo_connector->contrast)
-		drm_property_destroy(dev, psb_intel_sdvo_connector->contrast);
-	if (psb_intel_sdvo_connector->hue)
-		drm_property_destroy(dev, psb_intel_sdvo_connector->hue);
-	if (psb_intel_sdvo_connector->sharpness)
-		drm_property_destroy(dev, psb_intel_sdvo_connector->sharpness);
-	if (psb_intel_sdvo_connector->flicker_filter)
-		drm_property_destroy(dev, psb_intel_sdvo_connector->flicker_filter);
-	if (psb_intel_sdvo_connector->flicker_filter_2d)
-		drm_property_destroy(dev, psb_intel_sdvo_connector->flicker_filter_2d);
-	if (psb_intel_sdvo_connector->flicker_filter_adaptive)
-		drm_property_destroy(dev, psb_intel_sdvo_connector->flicker_filter_adaptive);
-	if (psb_intel_sdvo_connector->tv_luma_filter)
-		drm_property_destroy(dev, psb_intel_sdvo_connector->tv_luma_filter);
-	if (psb_intel_sdvo_connector->tv_chroma_filter)
-		drm_property_destroy(dev, psb_intel_sdvo_connector->tv_chroma_filter);
-	if (psb_intel_sdvo_connector->dot_crawl)
-		drm_property_destroy(dev, psb_intel_sdvo_connector->dot_crawl);
-	if (psb_intel_sdvo_connector->brightness)
-		drm_property_destroy(dev, psb_intel_sdvo_connector->brightness);
-}
-
 static void psb_intel_sdvo_destroy(struct drm_connector *connector)
 {
-	struct psb_intel_sdvo_connector *psb_intel_sdvo_connector = to_psb_intel_sdvo_connector(connector);
-
-	if (psb_intel_sdvo_connector->tv_format)
-		drm_property_destroy(connector->dev,
-				     psb_intel_sdvo_connector->tv_format);
-
-	psb_intel_sdvo_destroy_enhance_property(connector);
 	drm_connector_unregister(connector);
 	drm_connector_cleanup(connector);
 	kfree(connector);
diff --git a/drivers/gpu/drm/i2c/Kconfig b/drivers/gpu/drm/i2c/Kconfig
index 4d341db462a2..22c7ed63a001 100644
--- a/drivers/gpu/drm/i2c/Kconfig
+++ b/drivers/gpu/drm/i2c/Kconfig
@@ -1,6 +1,12 @@
 menu "I2C encoder or helper chips"
      depends on DRM && DRM_KMS_HELPER && I2C
 
+config DRM_I2C_ADV7511
+	tristate "AV7511 encoder"
+	select REGMAP_I2C
+	help
+	  Support for the Analog Device ADV7511(W) and ADV7513 HDMI encoders.
+
 config DRM_I2C_CH7006
 	tristate "Chrontel ch7006 TV encoder"
 	default m if DRM_NOUVEAU
diff --git a/drivers/gpu/drm/i2c/Makefile b/drivers/gpu/drm/i2c/Makefile
index 43aa33baebed..2c72eb584ab7 100644
--- a/drivers/gpu/drm/i2c/Makefile
+++ b/drivers/gpu/drm/i2c/Makefile
@@ -1,5 +1,7 @@
 ccflags-y := -Iinclude/drm
 
+obj-$(CONFIG_DRM_I2C_ADV7511) += adv7511.o
+
 ch7006-y := ch7006_drv.o ch7006_mode.o
 obj-$(CONFIG_DRM_I2C_CH7006) += ch7006.o
 
diff --git a/drivers/gpu/drm/i2c/adv7511.c b/drivers/gpu/drm/i2c/adv7511.c
new file mode 100644
index 000000000000..faf1c0c5ab2e
--- /dev/null
+++ b/drivers/gpu/drm/i2c/adv7511.c
@@ -0,0 +1,1010 @@
+/*
+ * Analog Devices ADV7511 HDMI transmitter driver
+ *
+ * Copyright 2012 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/device.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+
+#include <drm/drmP.h>
+#include <drm/drm_crtc_helper.h>
+#include <drm/drm_edid.h>
+#include <drm/drm_encoder_slave.h>
+
+#include "adv7511.h"
+
+struct adv7511 {
+	struct i2c_client *i2c_main;
+	struct i2c_client *i2c_edid;
+
+	struct regmap *regmap;
+	struct regmap *packet_memory_regmap;
+	enum drm_connector_status status;
+	int dpms_mode;
+
+	unsigned int f_tmds;
+
+	unsigned int current_edid_segment;
+	uint8_t edid_buf[256];
+
+	wait_queue_head_t wq;
+	struct drm_encoder *encoder;
+
+	bool embedded_sync;
+	enum adv7511_sync_polarity vsync_polarity;
+	enum adv7511_sync_polarity hsync_polarity;
+	bool rgb;
+
+	struct edid *edid;
+
+	struct gpio_desc *gpio_pd;
+};
+
+static struct adv7511 *encoder_to_adv7511(struct drm_encoder *encoder)
+{
+	return to_encoder_slave(encoder)->slave_priv;
+}
+
+/* ADI recommended values for proper operation. */
+static const struct reg_default adv7511_fixed_registers[] = {
+	{ 0x98, 0x03 },
+	{ 0x9a, 0xe0 },
+	{ 0x9c, 0x30 },
+	{ 0x9d, 0x61 },
+	{ 0xa2, 0xa4 },
+	{ 0xa3, 0xa4 },
+	{ 0xe0, 0xd0 },
+	{ 0xf9, 0x00 },
+	{ 0x55, 0x02 },
+};
+
+/* -----------------------------------------------------------------------------
+ * Register access
+ */
+
+static const uint8_t adv7511_register_defaults[] = {
+	0x12, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 00 */
+	0x00, 0x00, 0x01, 0x0e, 0xbc, 0x18, 0x01, 0x13,
+	0x25, 0x37, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 10 */
+	0x46, 0x62, 0x04, 0xa8, 0x00, 0x00, 0x1c, 0x84,
+	0x1c, 0xbf, 0x04, 0xa8, 0x1e, 0x70, 0x02, 0x1e, /* 20 */
+	0x00, 0x00, 0x04, 0xa8, 0x08, 0x12, 0x1b, 0xac,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 30 */
+	0x00, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 0xb0,
+	0x00, 0x50, 0x90, 0x7e, 0x79, 0x70, 0x00, 0x00, /* 40 */
+	0x00, 0xa8, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x02, 0x0d, 0x00, 0x00, 0x00, 0x00, /* 50 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 60 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x01, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 70 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 80 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0xc0, 0x00, 0x00, 0x00, /* 90 */
+	0x0b, 0x02, 0x00, 0x18, 0x5a, 0x60, 0x00, 0x00,
+	0x00, 0x00, 0x80, 0x80, 0x08, 0x04, 0x00, 0x00, /* a0 */
+	0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x40, 0x14,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* b0 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* c0 */
+	0x00, 0x03, 0x00, 0x00, 0x02, 0x00, 0x01, 0x04,
+	0x30, 0xff, 0x80, 0x80, 0x80, 0x00, 0x00, 0x00, /* d0 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x01,
+	0x80, 0x75, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, /* e0 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x75, 0x11, 0x00, /* f0 */
+	0x00, 0x7c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+};
+
+static bool adv7511_register_volatile(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case ADV7511_REG_CHIP_REVISION:
+	case ADV7511_REG_SPDIF_FREQ:
+	case ADV7511_REG_CTS_AUTOMATIC1:
+	case ADV7511_REG_CTS_AUTOMATIC2:
+	case ADV7511_REG_VIC_DETECTED:
+	case ADV7511_REG_VIC_SEND:
+	case ADV7511_REG_AUX_VIC_DETECTED:
+	case ADV7511_REG_STATUS:
+	case ADV7511_REG_GC(1):
+	case ADV7511_REG_INT(0):
+	case ADV7511_REG_INT(1):
+	case ADV7511_REG_PLL_STATUS:
+	case ADV7511_REG_AN(0):
+	case ADV7511_REG_AN(1):
+	case ADV7511_REG_AN(2):
+	case ADV7511_REG_AN(3):
+	case ADV7511_REG_AN(4):
+	case ADV7511_REG_AN(5):
+	case ADV7511_REG_AN(6):
+	case ADV7511_REG_AN(7):
+	case ADV7511_REG_HDCP_STATUS:
+	case ADV7511_REG_BCAPS:
+	case ADV7511_REG_BKSV(0):
+	case ADV7511_REG_BKSV(1):
+	case ADV7511_REG_BKSV(2):
+	case ADV7511_REG_BKSV(3):
+	case ADV7511_REG_BKSV(4):
+	case ADV7511_REG_DDC_STATUS:
+	case ADV7511_REG_BSTATUS(0):
+	case ADV7511_REG_BSTATUS(1):
+	case ADV7511_REG_CHIP_ID_HIGH:
+	case ADV7511_REG_CHIP_ID_LOW:
+		return true;
+	}
+
+	return false;
+}
+
+static const struct regmap_config adv7511_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.max_register = 0xff,
+	.cache_type = REGCACHE_RBTREE,
+	.reg_defaults_raw = adv7511_register_defaults,
+	.num_reg_defaults_raw = ARRAY_SIZE(adv7511_register_defaults),
+
+	.volatile_reg = adv7511_register_volatile,
+};
+
+/* -----------------------------------------------------------------------------
+ * Hardware configuration
+ */
+
+static void adv7511_set_colormap(struct adv7511 *adv7511, bool enable,
+				 const uint16_t *coeff,
+				 unsigned int scaling_factor)
+{
+	unsigned int i;
+
+	regmap_update_bits(adv7511->regmap, ADV7511_REG_CSC_UPPER(1),
+			   ADV7511_CSC_UPDATE_MODE, ADV7511_CSC_UPDATE_MODE);
+
+	if (enable) {
+		for (i = 0; i < 12; ++i) {
+			regmap_update_bits(adv7511->regmap,
+					   ADV7511_REG_CSC_UPPER(i),
+					   0x1f, coeff[i] >> 8);
+			regmap_write(adv7511->regmap,
+				     ADV7511_REG_CSC_LOWER(i),
+				     coeff[i] & 0xff);
+		}
+	}
+
+	if (enable)
+		regmap_update_bits(adv7511->regmap, ADV7511_REG_CSC_UPPER(0),
+				   0xe0, 0x80 | (scaling_factor << 5));
+	else
+		regmap_update_bits(adv7511->regmap, ADV7511_REG_CSC_UPPER(0),
+				   0x80, 0x00);
+
+	regmap_update_bits(adv7511->regmap, ADV7511_REG_CSC_UPPER(1),
+			   ADV7511_CSC_UPDATE_MODE, 0);
+}
+
+static int adv7511_packet_enable(struct adv7511 *adv7511, unsigned int packet)
+{
+	if (packet & 0xff)
+		regmap_update_bits(adv7511->regmap, ADV7511_REG_PACKET_ENABLE0,
+				   packet, 0xff);
+
+	if (packet & 0xff00) {
+		packet >>= 8;
+		regmap_update_bits(adv7511->regmap, ADV7511_REG_PACKET_ENABLE1,
+				   packet, 0xff);
+	}
+
+	return 0;
+}
+
+static int adv7511_packet_disable(struct adv7511 *adv7511, unsigned int packet)
+{
+	if (packet & 0xff)
+		regmap_update_bits(adv7511->regmap, ADV7511_REG_PACKET_ENABLE0,
+				   packet, 0x00);
+
+	if (packet & 0xff00) {
+		packet >>= 8;
+		regmap_update_bits(adv7511->regmap, ADV7511_REG_PACKET_ENABLE1,
+				   packet, 0x00);
+	}
+
+	return 0;
+}
+
+/* Coefficients for adv7511 color space conversion */
+static const uint16_t adv7511_csc_ycbcr_to_rgb[] = {
+	0x0734, 0x04ad, 0x0000, 0x1c1b,
+	0x1ddc, 0x04ad, 0x1f24, 0x0135,
+	0x0000, 0x04ad, 0x087c, 0x1b77,
+};
+
+static void adv7511_set_config_csc(struct adv7511 *adv7511,
+				   struct drm_connector *connector,
+				   bool rgb)
+{
+	struct adv7511_video_config config;
+	bool output_format_422, output_format_ycbcr;
+	unsigned int mode;
+	uint8_t infoframe[17];
+
+	if (adv7511->edid)
+		config.hdmi_mode = drm_detect_hdmi_monitor(adv7511->edid);
+	else
+		config.hdmi_mode = false;
+
+	hdmi_avi_infoframe_init(&config.avi_infoframe);
+
+	config.avi_infoframe.scan_mode = HDMI_SCAN_MODE_UNDERSCAN;
+
+	if (rgb) {
+		config.csc_enable = false;
+		config.avi_infoframe.colorspace = HDMI_COLORSPACE_RGB;
+	} else {
+		config.csc_scaling_factor = ADV7511_CSC_SCALING_4;
+		config.csc_coefficents = adv7511_csc_ycbcr_to_rgb;
+
+		if ((connector->display_info.color_formats &
+		     DRM_COLOR_FORMAT_YCRCB422) &&
+		    config.hdmi_mode) {
+			config.csc_enable = false;
+			config.avi_infoframe.colorspace =
+				HDMI_COLORSPACE_YUV422;
+		} else {
+			config.csc_enable = true;
+			config.avi_infoframe.colorspace = HDMI_COLORSPACE_RGB;
+		}
+	}
+
+	if (config.hdmi_mode) {
+		mode = ADV7511_HDMI_CFG_MODE_HDMI;
+
+		switch (config.avi_infoframe.colorspace) {
+		case HDMI_COLORSPACE_YUV444:
+			output_format_422 = false;
+			output_format_ycbcr = true;
+			break;
+		case HDMI_COLORSPACE_YUV422:
+			output_format_422 = true;
+			output_format_ycbcr = true;
+			break;
+		default:
+			output_format_422 = false;
+			output_format_ycbcr = false;
+			break;
+		}
+	} else {
+		mode = ADV7511_HDMI_CFG_MODE_DVI;
+		output_format_422 = false;
+		output_format_ycbcr = false;
+	}
+
+	adv7511_packet_disable(adv7511, ADV7511_PACKET_ENABLE_AVI_INFOFRAME);
+
+	adv7511_set_colormap(adv7511, config.csc_enable,
+			     config.csc_coefficents,
+			     config.csc_scaling_factor);
+
+	regmap_update_bits(adv7511->regmap, ADV7511_REG_VIDEO_INPUT_CFG1, 0x81,
+			   (output_format_422 << 7) | output_format_ycbcr);
+
+	regmap_update_bits(adv7511->regmap, ADV7511_REG_HDCP_HDMI_CFG,
+			   ADV7511_HDMI_CFG_MODE_MASK, mode);
+
+	hdmi_avi_infoframe_pack(&config.avi_infoframe, infoframe,
+				sizeof(infoframe));
+
+	/* The AVI infoframe id is not configurable */
+	regmap_bulk_write(adv7511->regmap, ADV7511_REG_AVI_INFOFRAME_VERSION,
+			  infoframe + 1, sizeof(infoframe) - 1);
+
+	adv7511_packet_enable(adv7511, ADV7511_PACKET_ENABLE_AVI_INFOFRAME);
+}
+
+static void adv7511_set_link_config(struct adv7511 *adv7511,
+				    const struct adv7511_link_config *config)
+{
+	/*
+	 * The input style values documented in the datasheet don't match the
+	 * hardware register field values :-(
+	 */
+	static const unsigned int input_styles[4] = { 0, 2, 1, 3 };
+
+	unsigned int clock_delay;
+	unsigned int color_depth;
+	unsigned int input_id;
+
+	clock_delay = (config->clock_delay + 1200) / 400;
+	color_depth = config->input_color_depth == 8 ? 3
+		    : (config->input_color_depth == 10 ? 1 : 2);
+
+	/* TODO Support input ID 6 */
+	if (config->input_colorspace != HDMI_COLORSPACE_YUV422)
+		input_id = config->input_clock == ADV7511_INPUT_CLOCK_DDR
+			 ? 5 : 0;
+	else if (config->input_clock == ADV7511_INPUT_CLOCK_DDR)
+		input_id = config->embedded_sync ? 8 : 7;
+	else if (config->input_clock == ADV7511_INPUT_CLOCK_2X)
+		input_id = config->embedded_sync ? 4 : 3;
+	else
+		input_id = config->embedded_sync ? 2 : 1;
+
+	regmap_update_bits(adv7511->regmap, ADV7511_REG_I2C_FREQ_ID_CFG, 0xf,
+			   input_id);
+	regmap_update_bits(adv7511->regmap, ADV7511_REG_VIDEO_INPUT_CFG1, 0x7e,
+			   (color_depth << 4) |
+			   (input_styles[config->input_style] << 2));
+	regmap_write(adv7511->regmap, ADV7511_REG_VIDEO_INPUT_CFG2,
+		     config->input_justification << 3);
+	regmap_write(adv7511->regmap, ADV7511_REG_TIMING_GEN_SEQ,
+		     config->sync_pulse << 2);
+
+	regmap_write(adv7511->regmap, 0xba, clock_delay << 5);
+
+	adv7511->embedded_sync = config->embedded_sync;
+	adv7511->hsync_polarity = config->hsync_polarity;
+	adv7511->vsync_polarity = config->vsync_polarity;
+	adv7511->rgb = config->input_colorspace == HDMI_COLORSPACE_RGB;
+}
+
+/* -----------------------------------------------------------------------------
+ * Interrupt and hotplug detection
+ */
+
+static bool adv7511_hpd(struct adv7511 *adv7511)
+{
+	unsigned int irq0;
+	int ret;
+
+	ret = regmap_read(adv7511->regmap, ADV7511_REG_INT(0), &irq0);
+	if (ret < 0)
+		return false;
+
+	if (irq0 & ADV7511_INT0_HDP) {
+		regmap_write(adv7511->regmap, ADV7511_REG_INT(0),
+			     ADV7511_INT0_HDP);
+		return true;
+	}
+
+	return false;
+}
+
+static irqreturn_t adv7511_irq_handler(int irq, void *devid)
+{
+	struct adv7511 *adv7511 = devid;
+
+	if (adv7511_hpd(adv7511))
+		drm_helper_hpd_irq_event(adv7511->encoder->dev);
+
+	wake_up_all(&adv7511->wq);
+
+	return IRQ_HANDLED;
+}
+
+static unsigned int adv7511_is_interrupt_pending(struct adv7511 *adv7511,
+						 unsigned int irq)
+{
+	unsigned int irq0, irq1;
+	unsigned int pending;
+	int ret;
+
+	ret = regmap_read(adv7511->regmap, ADV7511_REG_INT(0), &irq0);
+	if (ret < 0)
+		return 0;
+	ret = regmap_read(adv7511->regmap, ADV7511_REG_INT(1), &irq1);
+	if (ret < 0)
+		return 0;
+
+	pending = (irq1 << 8) | irq0;
+
+	return pending & irq;
+}
+
+static int adv7511_wait_for_interrupt(struct adv7511 *adv7511, int irq,
+				      int timeout)
+{
+	unsigned int pending;
+	int ret;
+
+	if (adv7511->i2c_main->irq) {
+		ret = wait_event_interruptible_timeout(adv7511->wq,
+				adv7511_is_interrupt_pending(adv7511, irq),
+				msecs_to_jiffies(timeout));
+		if (ret <= 0)
+			return 0;
+		pending = adv7511_is_interrupt_pending(adv7511, irq);
+	} else {
+		if (timeout < 25)
+			timeout = 25;
+		do {
+			pending = adv7511_is_interrupt_pending(adv7511, irq);
+			if (pending)
+				break;
+			msleep(25);
+			timeout -= 25;
+		} while (timeout >= 25);
+	}
+
+	return pending;
+}
+
+/* -----------------------------------------------------------------------------
+ * EDID retrieval
+ */
+
+static int adv7511_get_edid_block(void *data, u8 *buf, unsigned int block,
+				  size_t len)
+{
+	struct adv7511 *adv7511 = data;
+	struct i2c_msg xfer[2];
+	uint8_t offset;
+	unsigned int i;
+	int ret;
+
+	if (len > 128)
+		return -EINVAL;
+
+	if (adv7511->current_edid_segment != block / 2) {
+		unsigned int status;
+
+		ret = regmap_read(adv7511->regmap, ADV7511_REG_DDC_STATUS,
+				  &status);
+		if (ret < 0)
+			return ret;
+
+		if (status != 2) {
+			regmap_write(adv7511->regmap, ADV7511_REG_EDID_SEGMENT,
+				     block);
+			ret = adv7511_wait_for_interrupt(adv7511,
+					ADV7511_INT0_EDID_READY |
+					ADV7511_INT1_DDC_ERROR, 200);
+
+			if (!(ret & ADV7511_INT0_EDID_READY))
+				return -EIO;
+		}
+
+		regmap_write(adv7511->regmap, ADV7511_REG_INT(0),
+			     ADV7511_INT0_EDID_READY | ADV7511_INT1_DDC_ERROR);
+
+		/* Break this apart, hopefully more I2C controllers will
+		 * support 64 byte transfers than 256 byte transfers
+		 */
+
+		xfer[0].addr = adv7511->i2c_edid->addr;
+		xfer[0].flags = 0;
+		xfer[0].len = 1;
+		xfer[0].buf = &offset;
+		xfer[1].addr = adv7511->i2c_edid->addr;
+		xfer[1].flags = I2C_M_RD;
+		xfer[1].len = 64;
+		xfer[1].buf = adv7511->edid_buf;
+
+		offset = 0;
+
+		for (i = 0; i < 4; ++i) {
+			ret = i2c_transfer(adv7511->i2c_edid->adapter, xfer,
+					   ARRAY_SIZE(xfer));
+			if (ret < 0)
+				return ret;
+			else if (ret != 2)
+				return -EIO;
+
+			xfer[1].buf += 64;
+			offset += 64;
+		}
+
+		adv7511->current_edid_segment = block / 2;
+	}
+
+	if (block % 2 == 0)
+		memcpy(buf, adv7511->edid_buf, len);
+	else
+		memcpy(buf, adv7511->edid_buf + 128, len);
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Encoder operations
+ */
+
+static int adv7511_get_modes(struct drm_encoder *encoder,
+			     struct drm_connector *connector)
+{
+	struct adv7511 *adv7511 = encoder_to_adv7511(encoder);
+	struct edid *edid;
+	unsigned int count;
+
+	/* Reading the EDID only works if the device is powered */
+	if (adv7511->dpms_mode != DRM_MODE_DPMS_ON) {
+		regmap_write(adv7511->regmap, ADV7511_REG_INT(0),
+			     ADV7511_INT0_EDID_READY | ADV7511_INT1_DDC_ERROR);
+		regmap_update_bits(adv7511->regmap, ADV7511_REG_POWER,
+				   ADV7511_POWER_POWER_DOWN, 0);
+		adv7511->current_edid_segment = -1;
+	}
+
+	edid = drm_do_get_edid(connector, adv7511_get_edid_block, adv7511);
+
+	if (adv7511->dpms_mode != DRM_MODE_DPMS_ON)
+		regmap_update_bits(adv7511->regmap, ADV7511_REG_POWER,
+				   ADV7511_POWER_POWER_DOWN,
+				   ADV7511_POWER_POWER_DOWN);
+
+	kfree(adv7511->edid);
+	adv7511->edid = edid;
+	if (!edid)
+		return 0;
+
+	drm_mode_connector_update_edid_property(connector, edid);
+	count = drm_add_edid_modes(connector, edid);
+
+	adv7511_set_config_csc(adv7511, connector, adv7511->rgb);
+
+	return count;
+}
+
+static void adv7511_encoder_dpms(struct drm_encoder *encoder, int mode)
+{
+	struct adv7511 *adv7511 = encoder_to_adv7511(encoder);
+
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+		adv7511->current_edid_segment = -1;
+
+		regmap_write(adv7511->regmap, ADV7511_REG_INT(0),
+			     ADV7511_INT0_EDID_READY | ADV7511_INT1_DDC_ERROR);
+		regmap_update_bits(adv7511->regmap, ADV7511_REG_POWER,
+				   ADV7511_POWER_POWER_DOWN, 0);
+		/*
+		 * Per spec it is allowed to pulse the HDP signal to indicate
+		 * that the EDID information has changed. Some monitors do this
+		 * when they wakeup from standby or are enabled. When the HDP
+		 * goes low the adv7511 is reset and the outputs are disabled
+		 * which might cause the monitor to go to standby again. To
+		 * avoid this we ignore the HDP pin for the first few seconds
+		 * after enabeling the output.
+		 */
+		regmap_update_bits(adv7511->regmap, ADV7511_REG_POWER2,
+				   ADV7511_REG_POWER2_HDP_SRC_MASK,
+				   ADV7511_REG_POWER2_HDP_SRC_NONE);
+		/* Most of the registers are reset during power down or
+		 * when HPD is low
+		 */
+		regcache_sync(adv7511->regmap);
+		break;
+	default:
+		/* TODO: setup additional power down modes */
+		regmap_update_bits(adv7511->regmap, ADV7511_REG_POWER,
+				   ADV7511_POWER_POWER_DOWN,
+				   ADV7511_POWER_POWER_DOWN);
+		regcache_mark_dirty(adv7511->regmap);
+		break;
+	}
+
+	adv7511->dpms_mode = mode;
+}
+
+static enum drm_connector_status
+adv7511_encoder_detect(struct drm_encoder *encoder,
+		       struct drm_connector *connector)
+{
+	struct adv7511 *adv7511 = encoder_to_adv7511(encoder);
+	enum drm_connector_status status;
+	unsigned int val;
+	bool hpd;
+	int ret;
+
+	ret = regmap_read(adv7511->regmap, ADV7511_REG_STATUS, &val);
+	if (ret < 0)
+		return connector_status_disconnected;
+
+	if (val & ADV7511_STATUS_HPD)
+		status = connector_status_connected;
+	else
+		status = connector_status_disconnected;
+
+	hpd = adv7511_hpd(adv7511);
+
+	/* The chip resets itself when the cable is disconnected, so in case
+	 * there is a pending HPD interrupt and the cable is connected there was
+	 * at least one transition from disconnected to connected and the chip
+	 * has to be reinitialized. */
+	if (status == connector_status_connected && hpd &&
+	    adv7511->dpms_mode == DRM_MODE_DPMS_ON) {
+		regcache_mark_dirty(adv7511->regmap);
+		adv7511_encoder_dpms(encoder, adv7511->dpms_mode);
+		adv7511_get_modes(encoder, connector);
+		if (adv7511->status == connector_status_connected)
+			status = connector_status_disconnected;
+	} else {
+		/* Renable HDP sensing */
+		regmap_update_bits(adv7511->regmap, ADV7511_REG_POWER2,
+				   ADV7511_REG_POWER2_HDP_SRC_MASK,
+				   ADV7511_REG_POWER2_HDP_SRC_BOTH);
+	}
+
+	adv7511->status = status;
+	return status;
+}
+
+static int adv7511_encoder_mode_valid(struct drm_encoder *encoder,
+				      struct drm_display_mode *mode)
+{
+	if (mode->clock > 165000)
+		return MODE_CLOCK_HIGH;
+
+	if (mode->flags & DRM_MODE_FLAG_INTERLACE)
+		return MODE_NO_INTERLACE;
+
+	return MODE_OK;
+}
+
+static void adv7511_encoder_mode_set(struct drm_encoder *encoder,
+				     struct drm_display_mode *mode,
+				     struct drm_display_mode *adj_mode)
+{
+	struct adv7511 *adv7511 = encoder_to_adv7511(encoder);
+	unsigned int low_refresh_rate;
+	unsigned int hsync_polarity = 0;
+	unsigned int vsync_polarity = 0;
+
+	if (adv7511->embedded_sync) {
+		unsigned int hsync_offset, hsync_len;
+		unsigned int vsync_offset, vsync_len;
+
+		hsync_offset = adj_mode->crtc_hsync_start -
+			       adj_mode->crtc_hdisplay;
+		vsync_offset = adj_mode->crtc_vsync_start -
+			       adj_mode->crtc_vdisplay;
+		hsync_len = adj_mode->crtc_hsync_end -
+			    adj_mode->crtc_hsync_start;
+		vsync_len = adj_mode->crtc_vsync_end -
+			    adj_mode->crtc_vsync_start;
+
+		/* The hardware vsync generator has a off-by-one bug */
+		vsync_offset += 1;
+
+		regmap_write(adv7511->regmap, ADV7511_REG_HSYNC_PLACEMENT_MSB,
+			     ((hsync_offset >> 10) & 0x7) << 5);
+		regmap_write(adv7511->regmap, ADV7511_REG_SYNC_DECODER(0),
+			     (hsync_offset >> 2) & 0xff);
+		regmap_write(adv7511->regmap, ADV7511_REG_SYNC_DECODER(1),
+			     ((hsync_offset & 0x3) << 6) |
+			     ((hsync_len >> 4) & 0x3f));
+		regmap_write(adv7511->regmap, ADV7511_REG_SYNC_DECODER(2),
+			     ((hsync_len & 0xf) << 4) |
+			     ((vsync_offset >> 6) & 0xf));
+		regmap_write(adv7511->regmap, ADV7511_REG_SYNC_DECODER(3),
+			     ((vsync_offset & 0x3f) << 2) |
+			     ((vsync_len >> 8) & 0x3));
+		regmap_write(adv7511->regmap, ADV7511_REG_SYNC_DECODER(4),
+			     vsync_len & 0xff);
+
+		hsync_polarity = !(adj_mode->flags & DRM_MODE_FLAG_PHSYNC);
+		vsync_polarity = !(adj_mode->flags & DRM_MODE_FLAG_PVSYNC);
+	} else {
+		enum adv7511_sync_polarity mode_hsync_polarity;
+		enum adv7511_sync_polarity mode_vsync_polarity;
+
+		/**
+		 * If the input signal is always low or always high we want to
+		 * invert or let it passthrough depending on the polarity of the
+		 * current mode.
+		 **/
+		if (adj_mode->flags & DRM_MODE_FLAG_NHSYNC)
+			mode_hsync_polarity = ADV7511_SYNC_POLARITY_LOW;
+		else
+			mode_hsync_polarity = ADV7511_SYNC_POLARITY_HIGH;
+
+		if (adj_mode->flags & DRM_MODE_FLAG_NVSYNC)
+			mode_vsync_polarity = ADV7511_SYNC_POLARITY_LOW;
+		else
+			mode_vsync_polarity = ADV7511_SYNC_POLARITY_HIGH;
+
+		if (adv7511->hsync_polarity != mode_hsync_polarity &&
+		    adv7511->hsync_polarity !=
+		    ADV7511_SYNC_POLARITY_PASSTHROUGH)
+			hsync_polarity = 1;
+
+		if (adv7511->vsync_polarity != mode_vsync_polarity &&
+		    adv7511->vsync_polarity !=
+		    ADV7511_SYNC_POLARITY_PASSTHROUGH)
+			vsync_polarity = 1;
+	}
+
+	if (mode->vrefresh <= 24000)
+		low_refresh_rate = ADV7511_LOW_REFRESH_RATE_24HZ;
+	else if (mode->vrefresh <= 25000)
+		low_refresh_rate = ADV7511_LOW_REFRESH_RATE_25HZ;
+	else if (mode->vrefresh <= 30000)
+		low_refresh_rate = ADV7511_LOW_REFRESH_RATE_30HZ;
+	else
+		low_refresh_rate = ADV7511_LOW_REFRESH_RATE_NONE;
+
+	regmap_update_bits(adv7511->regmap, 0xfb,
+		0x6, low_refresh_rate << 1);
+	regmap_update_bits(adv7511->regmap, 0x17,
+		0x60, (vsync_polarity << 6) | (hsync_polarity << 5));
+
+	/*
+	 * TODO Test first order 4:2:2 to 4:4:4 up conversion method, which is
+	 * supposed to give better results.
+	 */
+
+	adv7511->f_tmds = mode->clock;
+}
+
+static struct drm_encoder_slave_funcs adv7511_encoder_funcs = {
+	.dpms = adv7511_encoder_dpms,
+	.mode_valid = adv7511_encoder_mode_valid,
+	.mode_set = adv7511_encoder_mode_set,
+	.detect = adv7511_encoder_detect,
+	.get_modes = adv7511_get_modes,
+};
+
+/* -----------------------------------------------------------------------------
+ * Probe & remove
+ */
+
+static int adv7511_parse_dt(struct device_node *np,
+			    struct adv7511_link_config *config)
+{
+	const char *str;
+	int ret;
+
+	memset(config, 0, sizeof(*config));
+
+	of_property_read_u32(np, "adi,input-depth", &config->input_color_depth);
+	if (config->input_color_depth != 8 && config->input_color_depth != 10 &&
+	    config->input_color_depth != 12)
+		return -EINVAL;
+
+	ret = of_property_read_string(np, "adi,input-colorspace", &str);
+	if (ret < 0)
+		return ret;
+
+	if (!strcmp(str, "rgb"))
+		config->input_colorspace = HDMI_COLORSPACE_RGB;
+	else if (!strcmp(str, "yuv422"))
+		config->input_colorspace = HDMI_COLORSPACE_YUV422;
+	else if (!strcmp(str, "yuv444"))
+		config->input_colorspace = HDMI_COLORSPACE_YUV444;
+	else
+		return -EINVAL;
+
+	ret = of_property_read_string(np, "adi,input-clock", &str);
+	if (ret < 0)
+		return ret;
+
+	if (!strcmp(str, "1x"))
+		config->input_clock = ADV7511_INPUT_CLOCK_1X;
+	else if (!strcmp(str, "2x"))
+		config->input_clock = ADV7511_INPUT_CLOCK_2X;
+	else if (!strcmp(str, "ddr"))
+		config->input_clock = ADV7511_INPUT_CLOCK_DDR;
+	else
+		return -EINVAL;
+
+	if (config->input_colorspace == HDMI_COLORSPACE_YUV422 ||
+	    config->input_clock != ADV7511_INPUT_CLOCK_1X) {
+		ret = of_property_read_u32(np, "adi,input-style",
+					   &config->input_style);
+		if (ret)
+			return ret;
+
+		if (config->input_style < 1 || config->input_style > 3)
+			return -EINVAL;
+
+		ret = of_property_read_string(np, "adi,input-justification",
+					      &str);
+		if (ret < 0)
+			return ret;
+
+		if (!strcmp(str, "left"))
+			config->input_justification =
+				ADV7511_INPUT_JUSTIFICATION_LEFT;
+		else if (!strcmp(str, "evenly"))
+			config->input_justification =
+				ADV7511_INPUT_JUSTIFICATION_EVENLY;
+		else if (!strcmp(str, "right"))
+			config->input_justification =
+				ADV7511_INPUT_JUSTIFICATION_RIGHT;
+		else
+			return -EINVAL;
+
+	} else {
+		config->input_style = 1;
+		config->input_justification = ADV7511_INPUT_JUSTIFICATION_LEFT;
+	}
+
+	of_property_read_u32(np, "adi,clock-delay", &config->clock_delay);
+	if (config->clock_delay < -1200 || config->clock_delay > 1600)
+		return -EINVAL;
+
+	config->embedded_sync = of_property_read_bool(np, "adi,embedded-sync");
+
+	/* Hardcode the sync pulse configurations for now. */
+	config->sync_pulse = ADV7511_INPUT_SYNC_PULSE_NONE;
+	config->vsync_polarity = ADV7511_SYNC_POLARITY_PASSTHROUGH;
+	config->hsync_polarity = ADV7511_SYNC_POLARITY_PASSTHROUGH;
+
+	return 0;
+}
+
+static const int edid_i2c_addr = 0x7e;
+static const int packet_i2c_addr = 0x70;
+static const int cec_i2c_addr = 0x78;
+
+static int adv7511_probe(struct i2c_client *i2c, const struct i2c_device_id *id)
+{
+	struct adv7511_link_config link_config;
+	struct adv7511 *adv7511;
+	struct device *dev = &i2c->dev;
+	unsigned int val;
+	int ret;
+
+	if (!dev->of_node)
+		return -EINVAL;
+
+	adv7511 = devm_kzalloc(dev, sizeof(*adv7511), GFP_KERNEL);
+	if (!adv7511)
+		return -ENOMEM;
+
+	adv7511->dpms_mode = DRM_MODE_DPMS_OFF;
+	adv7511->status = connector_status_disconnected;
+
+	ret = adv7511_parse_dt(dev->of_node, &link_config);
+	if (ret)
+		return ret;
+
+	/*
+	 * The power down GPIO is optional. If present, toggle it from active to
+	 * inactive to wake up the encoder.
+	 */
+	adv7511->gpio_pd = devm_gpiod_get_optional(dev, "pd", GPIOD_OUT_HIGH);
+	if (IS_ERR(adv7511->gpio_pd))
+		return PTR_ERR(adv7511->gpio_pd);
+
+	if (adv7511->gpio_pd) {
+		mdelay(5);
+		gpiod_set_value_cansleep(adv7511->gpio_pd, 0);
+	}
+
+	adv7511->regmap = devm_regmap_init_i2c(i2c, &adv7511_regmap_config);
+	if (IS_ERR(adv7511->regmap))
+		return PTR_ERR(adv7511->regmap);
+
+	ret = regmap_read(adv7511->regmap, ADV7511_REG_CHIP_REVISION, &val);
+	if (ret)
+		return ret;
+	dev_dbg(dev, "Rev. %d\n", val);
+
+	ret = regmap_register_patch(adv7511->regmap, adv7511_fixed_registers,
+				    ARRAY_SIZE(adv7511_fixed_registers));
+	if (ret)
+		return ret;
+
+	regmap_write(adv7511->regmap, ADV7511_REG_EDID_I2C_ADDR, edid_i2c_addr);
+	regmap_write(adv7511->regmap, ADV7511_REG_PACKET_I2C_ADDR,
+		     packet_i2c_addr);
+	regmap_write(adv7511->regmap, ADV7511_REG_CEC_I2C_ADDR, cec_i2c_addr);
+	adv7511_packet_disable(adv7511, 0xffff);
+
+	adv7511->i2c_main = i2c;
+	adv7511->i2c_edid = i2c_new_dummy(i2c->adapter, edid_i2c_addr >> 1);
+	if (!adv7511->i2c_edid)
+		return -ENOMEM;
+
+	if (i2c->irq) {
+		init_waitqueue_head(&adv7511->wq);
+
+		ret = devm_request_threaded_irq(dev, i2c->irq, NULL,
+						adv7511_irq_handler,
+						IRQF_ONESHOT, dev_name(dev),
+						adv7511);
+		if (ret)
+			goto err_i2c_unregister_device;
+	}
+
+	/* CEC is unused for now */
+	regmap_write(adv7511->regmap, ADV7511_REG_CEC_CTRL,
+		     ADV7511_CEC_CTRL_POWER_DOWN);
+
+	regmap_update_bits(adv7511->regmap, ADV7511_REG_POWER,
+			   ADV7511_POWER_POWER_DOWN, ADV7511_POWER_POWER_DOWN);
+
+	adv7511->current_edid_segment = -1;
+
+	i2c_set_clientdata(i2c, adv7511);
+
+	adv7511_set_link_config(adv7511, &link_config);
+
+	return 0;
+
+err_i2c_unregister_device:
+	i2c_unregister_device(adv7511->i2c_edid);
+
+	return ret;
+}
+
+static int adv7511_remove(struct i2c_client *i2c)
+{
+	struct adv7511 *adv7511 = i2c_get_clientdata(i2c);
+
+	i2c_unregister_device(adv7511->i2c_edid);
+
+	kfree(adv7511->edid);
+
+	return 0;
+}
+
+static int adv7511_encoder_init(struct i2c_client *i2c, struct drm_device *dev,
+				struct drm_encoder_slave *encoder)
+{
+
+	struct adv7511 *adv7511 = i2c_get_clientdata(i2c);
+
+	encoder->slave_priv = adv7511;
+	encoder->slave_funcs = &adv7511_encoder_funcs;
+
+	adv7511->encoder = &encoder->base;
+
+	return 0;
+}
+
+static const struct i2c_device_id adv7511_i2c_ids[] = {
+	{ "adv7511", 0 },
+	{ "adv7511w", 0 },
+	{ "adv7513", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, adv7511_i2c_ids);
+
+static const struct of_device_id adv7511_of_ids[] = {
+	{ .compatible = "adi,adv7511", },
+	{ .compatible = "adi,adv7511w", },
+	{ .compatible = "adi,adv7513", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, adv7511_of_ids);
+
+static struct drm_i2c_encoder_driver adv7511_driver = {
+	.i2c_driver = {
+		.driver = {
+			.name = "adv7511",
+			.of_match_table = adv7511_of_ids,
+		},
+		.id_table = adv7511_i2c_ids,
+		.probe = adv7511_probe,
+		.remove = adv7511_remove,
+	},
+
+	.encoder_init = adv7511_encoder_init,
+};
+
+static int __init adv7511_init(void)
+{
+	return drm_i2c_encoder_register(THIS_MODULE, &adv7511_driver);
+}
+module_init(adv7511_init);
+
+static void __exit adv7511_exit(void)
+{
+	drm_i2c_encoder_unregister(&adv7511_driver);
+}
+module_exit(adv7511_exit);
+
+MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
+MODULE_DESCRIPTION("ADV7511 HDMI transmitter driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpu/drm/i2c/adv7511.h b/drivers/gpu/drm/i2c/adv7511.h
new file mode 100644
index 000000000000..6599ed538426
--- /dev/null
+++ b/drivers/gpu/drm/i2c/adv7511.h
@@ -0,0 +1,289 @@
+/*
+ * Analog Devices ADV7511 HDMI transmitter driver
+ *
+ * Copyright 2012 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#ifndef __DRM_I2C_ADV7511_H__
+#define __DRM_I2C_ADV7511_H__
+
+#include <linux/hdmi.h>
+
+#define ADV7511_REG_CHIP_REVISION		0x00
+#define ADV7511_REG_N0				0x01
+#define ADV7511_REG_N1				0x02
+#define ADV7511_REG_N2				0x03
+#define ADV7511_REG_SPDIF_FREQ			0x04
+#define ADV7511_REG_CTS_AUTOMATIC1		0x05
+#define ADV7511_REG_CTS_AUTOMATIC2		0x06
+#define ADV7511_REG_CTS_MANUAL0			0x07
+#define ADV7511_REG_CTS_MANUAL1			0x08
+#define ADV7511_REG_CTS_MANUAL2			0x09
+#define ADV7511_REG_AUDIO_SOURCE		0x0a
+#define ADV7511_REG_AUDIO_CONFIG		0x0b
+#define ADV7511_REG_I2S_CONFIG			0x0c
+#define ADV7511_REG_I2S_WIDTH			0x0d
+#define ADV7511_REG_AUDIO_SUB_SRC0		0x0e
+#define ADV7511_REG_AUDIO_SUB_SRC1		0x0f
+#define ADV7511_REG_AUDIO_SUB_SRC2		0x10
+#define ADV7511_REG_AUDIO_SUB_SRC3		0x11
+#define ADV7511_REG_AUDIO_CFG1			0x12
+#define ADV7511_REG_AUDIO_CFG2			0x13
+#define ADV7511_REG_AUDIO_CFG3			0x14
+#define ADV7511_REG_I2C_FREQ_ID_CFG		0x15
+#define ADV7511_REG_VIDEO_INPUT_CFG1		0x16
+#define ADV7511_REG_CSC_UPPER(x)		(0x18 + (x) * 2)
+#define ADV7511_REG_CSC_LOWER(x)		(0x19 + (x) * 2)
+#define ADV7511_REG_SYNC_DECODER(x)		(0x30 + (x))
+#define ADV7511_REG_DE_GENERATOR		(0x35 + (x))
+#define ADV7511_REG_PIXEL_REPETITION		0x3b
+#define ADV7511_REG_VIC_MANUAL			0x3c
+#define ADV7511_REG_VIC_SEND			0x3d
+#define ADV7511_REG_VIC_DETECTED		0x3e
+#define ADV7511_REG_AUX_VIC_DETECTED		0x3f
+#define ADV7511_REG_PACKET_ENABLE0		0x40
+#define ADV7511_REG_POWER			0x41
+#define ADV7511_REG_STATUS			0x42
+#define ADV7511_REG_EDID_I2C_ADDR		0x43
+#define ADV7511_REG_PACKET_ENABLE1		0x44
+#define ADV7511_REG_PACKET_I2C_ADDR		0x45
+#define ADV7511_REG_DSD_ENABLE			0x46
+#define ADV7511_REG_VIDEO_INPUT_CFG2		0x48
+#define ADV7511_REG_INFOFRAME_UPDATE		0x4a
+#define ADV7511_REG_GC(x)			(0x4b + (x)) /* 0x4b - 0x51 */
+#define ADV7511_REG_AVI_INFOFRAME_VERSION	0x52
+#define ADV7511_REG_AVI_INFOFRAME_LENGTH	0x53
+#define ADV7511_REG_AVI_INFOFRAME_CHECKSUM	0x54
+#define ADV7511_REG_AVI_INFOFRAME(x)		(0x55 + (x)) /* 0x55 - 0x6f */
+#define ADV7511_REG_AUDIO_INFOFRAME_VERSION	0x70
+#define ADV7511_REG_AUDIO_INFOFRAME_LENGTH	0x71
+#define ADV7511_REG_AUDIO_INFOFRAME_CHECKSUM	0x72
+#define ADV7511_REG_AUDIO_INFOFRAME(x)		(0x73 + (x)) /* 0x73 - 0x7c */
+#define ADV7511_REG_INT_ENABLE(x)		(0x94 + (x))
+#define ADV7511_REG_INT(x)			(0x96 + (x))
+#define ADV7511_REG_INPUT_CLK_DIV		0x9d
+#define ADV7511_REG_PLL_STATUS			0x9e
+#define ADV7511_REG_HDMI_POWER			0xa1
+#define ADV7511_REG_HDCP_HDMI_CFG		0xaf
+#define ADV7511_REG_AN(x)			(0xb0 + (x)) /* 0xb0 - 0xb7 */
+#define ADV7511_REG_HDCP_STATUS			0xb8
+#define ADV7511_REG_BCAPS			0xbe
+#define ADV7511_REG_BKSV(x)			(0xc0 + (x)) /* 0xc0 - 0xc3 */
+#define ADV7511_REG_EDID_SEGMENT		0xc4
+#define ADV7511_REG_DDC_STATUS			0xc8
+#define ADV7511_REG_EDID_READ_CTRL		0xc9
+#define ADV7511_REG_BSTATUS(x)			(0xca + (x)) /* 0xca - 0xcb */
+#define ADV7511_REG_TIMING_GEN_SEQ		0xd0
+#define ADV7511_REG_POWER2			0xd6
+#define ADV7511_REG_HSYNC_PLACEMENT_MSB		0xfa
+
+#define ADV7511_REG_SYNC_ADJUSTMENT(x)		(0xd7 + (x)) /* 0xd7 - 0xdc */
+#define ADV7511_REG_TMDS_CLOCK_INV		0xde
+#define ADV7511_REG_ARC_CTRL			0xdf
+#define ADV7511_REG_CEC_I2C_ADDR		0xe1
+#define ADV7511_REG_CEC_CTRL			0xe2
+#define ADV7511_REG_CHIP_ID_HIGH		0xf5
+#define ADV7511_REG_CHIP_ID_LOW			0xf6
+
+#define ADV7511_CSC_ENABLE			BIT(7)
+#define ADV7511_CSC_UPDATE_MODE			BIT(5)
+
+#define ADV7511_INT0_HDP			BIT(7)
+#define ADV7511_INT0_VSYNC			BIT(5)
+#define ADV7511_INT0_AUDIO_FIFO_FULL		BIT(4)
+#define ADV7511_INT0_EDID_READY			BIT(2)
+#define ADV7511_INT0_HDCP_AUTHENTICATED		BIT(1)
+
+#define ADV7511_INT1_DDC_ERROR			BIT(7)
+#define ADV7511_INT1_BKSV			BIT(6)
+#define ADV7511_INT1_CEC_TX_READY		BIT(5)
+#define ADV7511_INT1_CEC_TX_ARBIT_LOST		BIT(4)
+#define ADV7511_INT1_CEC_TX_RETRY_TIMEOUT	BIT(3)
+#define ADV7511_INT1_CEC_RX_READY3		BIT(2)
+#define ADV7511_INT1_CEC_RX_READY2		BIT(1)
+#define ADV7511_INT1_CEC_RX_READY1		BIT(0)
+
+#define ADV7511_ARC_CTRL_POWER_DOWN		BIT(0)
+
+#define ADV7511_CEC_CTRL_POWER_DOWN		BIT(0)
+
+#define ADV7511_POWER_POWER_DOWN		BIT(6)
+
+#define ADV7511_HDMI_CFG_MODE_MASK		0x2
+#define ADV7511_HDMI_CFG_MODE_DVI		0x0
+#define ADV7511_HDMI_CFG_MODE_HDMI		0x2
+
+#define ADV7511_AUDIO_SELECT_I2C		0x0
+#define ADV7511_AUDIO_SELECT_SPDIF		0x1
+#define ADV7511_AUDIO_SELECT_DSD		0x2
+#define ADV7511_AUDIO_SELECT_HBR		0x3
+#define ADV7511_AUDIO_SELECT_DST		0x4
+
+#define ADV7511_I2S_SAMPLE_LEN_16		0x2
+#define ADV7511_I2S_SAMPLE_LEN_20		0x3
+#define ADV7511_I2S_SAMPLE_LEN_18		0x4
+#define ADV7511_I2S_SAMPLE_LEN_22		0x5
+#define ADV7511_I2S_SAMPLE_LEN_19		0x8
+#define ADV7511_I2S_SAMPLE_LEN_23		0x9
+#define ADV7511_I2S_SAMPLE_LEN_24		0xb
+#define ADV7511_I2S_SAMPLE_LEN_17		0xc
+#define ADV7511_I2S_SAMPLE_LEN_21		0xd
+
+#define ADV7511_SAMPLE_FREQ_44100		0x0
+#define ADV7511_SAMPLE_FREQ_48000		0x2
+#define ADV7511_SAMPLE_FREQ_32000		0x3
+#define ADV7511_SAMPLE_FREQ_88200		0x8
+#define ADV7511_SAMPLE_FREQ_96000		0xa
+#define ADV7511_SAMPLE_FREQ_176400		0xc
+#define ADV7511_SAMPLE_FREQ_192000		0xe
+
+#define ADV7511_STATUS_POWER_DOWN_POLARITY	BIT(7)
+#define ADV7511_STATUS_HPD			BIT(6)
+#define ADV7511_STATUS_MONITOR_SENSE		BIT(5)
+#define ADV7511_STATUS_I2S_32BIT_MODE		BIT(3)
+
+#define ADV7511_PACKET_ENABLE_N_CTS		BIT(8+6)
+#define ADV7511_PACKET_ENABLE_AUDIO_SAMPLE	BIT(8+5)
+#define ADV7511_PACKET_ENABLE_AVI_INFOFRAME	BIT(8+4)
+#define ADV7511_PACKET_ENABLE_AUDIO_INFOFRAME	BIT(8+3)
+#define ADV7511_PACKET_ENABLE_GC		BIT(7)
+#define ADV7511_PACKET_ENABLE_SPD		BIT(6)
+#define ADV7511_PACKET_ENABLE_MPEG		BIT(5)
+#define ADV7511_PACKET_ENABLE_ACP		BIT(4)
+#define ADV7511_PACKET_ENABLE_ISRC		BIT(3)
+#define ADV7511_PACKET_ENABLE_GM		BIT(2)
+#define ADV7511_PACKET_ENABLE_SPARE2		BIT(1)
+#define ADV7511_PACKET_ENABLE_SPARE1		BIT(0)
+
+#define ADV7511_REG_POWER2_HDP_SRC_MASK		0xc0
+#define ADV7511_REG_POWER2_HDP_SRC_BOTH		0x00
+#define ADV7511_REG_POWER2_HDP_SRC_HDP		0x40
+#define ADV7511_REG_POWER2_HDP_SRC_CEC		0x80
+#define ADV7511_REG_POWER2_HDP_SRC_NONE		0xc0
+#define ADV7511_REG_POWER2_TDMS_ENABLE		BIT(4)
+#define ADV7511_REG_POWER2_GATE_INPUT_CLK	BIT(0)
+
+#define ADV7511_LOW_REFRESH_RATE_NONE		0x0
+#define ADV7511_LOW_REFRESH_RATE_24HZ		0x1
+#define ADV7511_LOW_REFRESH_RATE_25HZ		0x2
+#define ADV7511_LOW_REFRESH_RATE_30HZ		0x3
+
+#define ADV7511_AUDIO_CFG3_LEN_MASK		0x0f
+#define ADV7511_I2C_FREQ_ID_CFG_RATE_MASK	0xf0
+
+#define ADV7511_AUDIO_SOURCE_I2S		0
+#define ADV7511_AUDIO_SOURCE_SPDIF		1
+
+#define ADV7511_I2S_FORMAT_I2S			0
+#define ADV7511_I2S_FORMAT_RIGHT_J		1
+#define ADV7511_I2S_FORMAT_LEFT_J		2
+
+#define ADV7511_PACKET(p, x)	    ((p) * 0x20 + (x))
+#define ADV7511_PACKET_SDP(x)	    ADV7511_PACKET(0, x)
+#define ADV7511_PACKET_MPEG(x)	    ADV7511_PACKET(1, x)
+#define ADV7511_PACKET_ACP(x)	    ADV7511_PACKET(2, x)
+#define ADV7511_PACKET_ISRC1(x)	    ADV7511_PACKET(3, x)
+#define ADV7511_PACKET_ISRC2(x)	    ADV7511_PACKET(4, x)
+#define ADV7511_PACKET_GM(x)	    ADV7511_PACKET(5, x)
+#define ADV7511_PACKET_SPARE(x)	    ADV7511_PACKET(6, x)
+
+enum adv7511_input_clock {
+	ADV7511_INPUT_CLOCK_1X,
+	ADV7511_INPUT_CLOCK_2X,
+	ADV7511_INPUT_CLOCK_DDR,
+};
+
+enum adv7511_input_justification {
+	ADV7511_INPUT_JUSTIFICATION_EVENLY = 0,
+	ADV7511_INPUT_JUSTIFICATION_RIGHT = 1,
+	ADV7511_INPUT_JUSTIFICATION_LEFT = 2,
+};
+
+enum adv7511_input_sync_pulse {
+	ADV7511_INPUT_SYNC_PULSE_DE = 0,
+	ADV7511_INPUT_SYNC_PULSE_HSYNC = 1,
+	ADV7511_INPUT_SYNC_PULSE_VSYNC = 2,
+	ADV7511_INPUT_SYNC_PULSE_NONE = 3,
+};
+
+/**
+ * enum adv7511_sync_polarity - Polarity for the input sync signals
+ * @ADV7511_SYNC_POLARITY_PASSTHROUGH:  Sync polarity matches that of
+ *				       the currently configured mode.
+ * @ADV7511_SYNC_POLARITY_LOW:	    Sync polarity is low
+ * @ADV7511_SYNC_POLARITY_HIGH:	    Sync polarity is high
+ *
+ * If the polarity is set to either LOW or HIGH the driver will configure the
+ * ADV7511 to internally invert the sync signal if required to match the sync
+ * polarity setting for the currently selected output mode.
+ *
+ * If the polarity is set to PASSTHROUGH, the ADV7511 will route the signal
+ * unchanged. This is used when the upstream graphics core already generates
+ * the sync signals with the correct polarity.
+ */
+enum adv7511_sync_polarity {
+	ADV7511_SYNC_POLARITY_PASSTHROUGH,
+	ADV7511_SYNC_POLARITY_LOW,
+	ADV7511_SYNC_POLARITY_HIGH,
+};
+
+/**
+ * struct adv7511_link_config - Describes adv7511 hardware configuration
+ * @input_color_depth:		Number of bits per color component (8, 10 or 12)
+ * @input_colorspace:		The input colorspace (RGB, YUV444, YUV422)
+ * @input_clock:		The input video clock style (1x, 2x, DDR)
+ * @input_style:		The input component arrangement variant
+ * @input_justification:	Video input format bit justification
+ * @clock_delay:		Clock delay for the input clock (in ps)
+ * @embedded_sync:		Video input uses BT.656-style embedded sync
+ * @sync_pulse:			Select the sync pulse
+ * @vsync_polarity:		vsync input signal configuration
+ * @hsync_polarity:		hsync input signal configuration
+ */
+struct adv7511_link_config {
+	unsigned int input_color_depth;
+	enum hdmi_colorspace input_colorspace;
+	enum adv7511_input_clock input_clock;
+	unsigned int input_style;
+	enum adv7511_input_justification input_justification;
+
+	int clock_delay;
+
+	bool embedded_sync;
+	enum adv7511_input_sync_pulse sync_pulse;
+	enum adv7511_sync_polarity vsync_polarity;
+	enum adv7511_sync_polarity hsync_polarity;
+};
+
+/**
+ * enum adv7511_csc_scaling - Scaling factor for the ADV7511 CSC
+ * @ADV7511_CSC_SCALING_1: CSC results are not scaled
+ * @ADV7511_CSC_SCALING_2: CSC results are scaled by a factor of two
+ * @ADV7511_CSC_SCALING_4: CSC results are scalled by a factor of four
+ */
+enum adv7511_csc_scaling {
+	ADV7511_CSC_SCALING_1 = 0,
+	ADV7511_CSC_SCALING_2 = 1,
+	ADV7511_CSC_SCALING_4 = 2,
+};
+
+/**
+ * struct adv7511_video_config - Describes adv7511 hardware configuration
+ * @csc_enable:			Whether to enable color space conversion
+ * @csc_scaling_factor:		Color space conversion scaling factor
+ * @csc_coefficents:		Color space conversion coefficents
+ * @hdmi_mode:			Whether to use HDMI or DVI output mode
+ * @avi_infoframe:		HDMI infoframe
+ */
+struct adv7511_video_config {
+	bool csc_enable;
+	enum adv7511_csc_scaling csc_scaling_factor;
+	const uint16_t *csc_coefficents;
+
+	bool hdmi_mode;
+	struct hdmi_avi_infoframe avi_infoframe;
+};
+
+#endif /* __DRM_I2C_ADV7511_H__ */
diff --git a/drivers/gpu/drm/i915/Makefile b/drivers/gpu/drm/i915/Makefile
index c1dd485aeb6c..e4083e41a600 100644
--- a/drivers/gpu/drm/i915/Makefile
+++ b/drivers/gpu/drm/i915/Makefile
@@ -11,7 +11,9 @@ i915-y := i915_drv.o \
 	  i915_params.o \
           i915_suspend.o \
 	  i915_sysfs.o \
-	  intel_pm.o
+	  intel_pm.o \
+	  intel_runtime_pm.o
+
 i915-$(CONFIG_COMPAT)   += i915_ioc32.o
 i915-$(CONFIG_DEBUG_FS) += i915_debugfs.o
 
@@ -38,13 +40,18 @@ i915-y += i915_cmd_parser.o \
 # autogenerated null render state
 i915-y += intel_renderstate_gen6.o \
 	  intel_renderstate_gen7.o \
-	  intel_renderstate_gen8.o
+	  intel_renderstate_gen8.o \
+	  intel_renderstate_gen9.o
 
 # modesetting core code
-i915-y += intel_bios.o \
+i915-y += intel_audio.o \
+	  intel_bios.o \
 	  intel_display.o \
+	  intel_fifo_underrun.o \
+	  intel_frontbuffer.o \
 	  intel_modes.o \
 	  intel_overlay.o \
+	  intel_psr.o \
 	  intel_sideband.o \
 	  intel_sprite.o
 i915-$(CONFIG_ACPI)		+= intel_acpi.o intel_opregion.o
diff --git a/drivers/gpu/drm/i915/i915_cmd_parser.c b/drivers/gpu/drm/i915/i915_cmd_parser.c
index 593b657d3e59..22c992a78ac6 100644
--- a/drivers/gpu/drm/i915/i915_cmd_parser.c
+++ b/drivers/gpu/drm/i915/i915_cmd_parser.c
@@ -73,7 +73,7 @@
  * those commands required by the parser. This generally works because command
  * opcode ranges have standard command length encodings. So for commands that
  * the parser does not need to check, it can easily skip them. This is
- * implementated via a per-ring length decoding vfunc.
+ * implemented via a per-ring length decoding vfunc.
  *
  * Unfortunately, there are a number of commands that do not follow the standard
  * length encoding for their opcode range, primarily amongst the MI_* commands.
@@ -138,6 +138,11 @@ static const struct drm_i915_cmd_descriptor common_cmds[] = {
 			.mask = MI_GLOBAL_GTT,
 			.expected = 0,
 	      }},						       ),
+	/*
+	 * MI_BATCH_BUFFER_START requires some special handling. It's not
+	 * really a 'skip' action but it doesn't seem like it's worth adding
+	 * a new action. See i915_parse_cmds().
+	 */
 	CMD(  MI_BATCH_BUFFER_START,            SMI,   !F,  0xFF,   S  ),
 };
 
@@ -408,6 +413,8 @@ static const u32 gen7_render_regs[] = {
 	REG64(PS_INVOCATION_COUNT),
 	REG64(PS_DEPTH_COUNT),
 	OACONTROL, /* Only allowed for LRI and SRM. See below. */
+	REG64(MI_PREDICATE_SRC0),
+	REG64(MI_PREDICATE_SRC1),
 	GEN7_3DPRIM_END_OFFSET,
 	GEN7_3DPRIM_START_VERTEX,
 	GEN7_3DPRIM_VERTEX_COUNT,
@@ -838,7 +845,7 @@ finish:
  * @ring: the ring in question
  *
  * Only certain platforms require software batch buffer command parsing, and
- * only when enabled via module paramter.
+ * only when enabled via module parameter.
  *
  * Return: true if the ring requires software command parsing
  */
@@ -847,12 +854,7 @@ bool i915_needs_cmd_parser(struct intel_engine_cs *ring)
 	if (!ring->needs_cmd_parser)
 		return false;
 
-	/*
-	 * XXX: VLV is Gen7 and therefore has cmd_tables, but has PPGTT
-	 * disabled. That will cause all of the parser's PPGTT checks to
-	 * fail. For now, disable parsing when PPGTT is off.
-	 */
-	if (USES_PPGTT(ring->dev))
+	if (!USES_PPGTT(ring->dev))
 		return false;
 
 	return (i915.enable_cmd_parser == 1);
@@ -888,8 +890,10 @@ static bool check_cmd(const struct intel_engine_cs *ring,
 		 * OACONTROL writes to only MI_LOAD_REGISTER_IMM commands.
 		 */
 		if (reg_addr == OACONTROL) {
-			if (desc->cmd.value == MI_LOAD_REGISTER_MEM)
+			if (desc->cmd.value == MI_LOAD_REGISTER_MEM) {
+				DRM_DEBUG_DRIVER("CMD: Rejected LRM to OACONTROL\n");
 				return false;
+			}
 
 			if (desc->cmd.value == MI_LOAD_REGISTER_IMM(1))
 				*oacontrol_set = (cmd[2] != 0);
@@ -958,7 +962,8 @@ static bool check_cmd(const struct intel_engine_cs *ring,
  * Parses the specified batch buffer looking for privilege violations as
  * described in the overview.
  *
- * Return: non-zero if the parser finds violations or otherwise fails
+ * Return: non-zero if the parser finds violations or otherwise fails; -EACCES
+ * if the batch appears legal but should use hardware parsing
  */
 int i915_parse_cmds(struct intel_engine_cs *ring,
 		    struct drm_i915_gem_object *batch_obj,
@@ -1005,6 +1010,16 @@ int i915_parse_cmds(struct intel_engine_cs *ring,
 			break;
 		}
 
+		/*
+		 * If the batch buffer contains a chained batch, return an
+		 * error that tells the caller to abort and dispatch the
+		 * workload as a non-secure batch.
+		 */
+		if (desc->cmd.value == MI_BATCH_BUFFER_START) {
+			ret = -EACCES;
+			break;
+		}
+
 		if (desc->flags & CMD_DESC_FIXED)
 			length = desc->length.fixed;
 		else
@@ -1059,6 +1074,8 @@ int i915_cmd_parser_get_version(void)
 	 *
 	 * 1. Initial version. Checks batches and reports violations, but leaves
 	 *    hardware parsing enabled (so does not allow new use cases).
+	 * 2. Allow access to the MI_PREDICATE_SRC0 and
+	 *    MI_PREDICATE_SRC1 registers.
 	 */
-	return 1;
+	return 2;
 }
diff --git a/drivers/gpu/drm/i915/i915_debugfs.c b/drivers/gpu/drm/i915/i915_debugfs.c
index 063b44817e08..779a275eb1fd 100644
--- a/drivers/gpu/drm/i915/i915_debugfs.c
+++ b/drivers/gpu/drm/i915/i915_debugfs.c
@@ -116,7 +116,7 @@ static const char *get_tiling_flag(struct drm_i915_gem_object *obj)
 
 static inline const char *get_global_flag(struct drm_i915_gem_object *obj)
 {
-	return obj->has_global_gtt_mapping ? "g" : " ";
+	return i915_gem_obj_to_ggtt(obj) ? "g" : " ";
 }
 
 static void
@@ -516,7 +516,6 @@ static int i915_gem_pageflip_info(struct seq_file *m, void *data)
 	struct drm_info_node *node = m->private;
 	struct drm_device *dev = node->minor->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	unsigned long flags;
 	struct intel_crtc *crtc;
 	int ret;
 
@@ -529,7 +528,7 @@ static int i915_gem_pageflip_info(struct seq_file *m, void *data)
 		const char plane = plane_name(crtc->plane);
 		struct intel_unpin_work *work;
 
-		spin_lock_irqsave(&dev->event_lock, flags);
+		spin_lock_irq(&dev->event_lock);
 		work = crtc->unpin_work;
 		if (work == NULL) {
 			seq_printf(m, "No flip due on pipe %c (plane %c)\n",
@@ -575,7 +574,7 @@ static int i915_gem_pageflip_info(struct seq_file *m, void *data)
 				seq_printf(m, "MMIO update completed? %d\n",  addr == work->gtt_offset);
 			}
 		}
-		spin_unlock_irqrestore(&dev->event_lock, flags);
+		spin_unlock_irq(&dev->event_lock);
 	}
 
 	mutex_unlock(&dev->struct_mutex);
@@ -717,7 +716,7 @@ static int i915_interrupt_info(struct seq_file *m, void *data)
 		}
 
 		for_each_pipe(dev_priv, pipe) {
-			if (!intel_display_power_enabled(dev_priv,
+			if (!intel_display_power_is_enabled(dev_priv,
 						POWER_DOMAIN_PIPE(pipe))) {
 				seq_printf(m, "Pipe %c power disabled\n",
 					   pipe_name(pipe));
@@ -1241,11 +1240,12 @@ static int vlv_drpc_info(struct seq_file *m)
 	struct drm_info_node *node = m->private;
 	struct drm_device *dev = node->minor->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 rpmodectl1, rcctl1;
+	u32 rpmodectl1, rcctl1, pw_status;
 	unsigned fw_rendercount = 0, fw_mediacount = 0;
 
 	intel_runtime_pm_get(dev_priv);
 
+	pw_status = I915_READ(VLV_GTLC_PW_STATUS);
 	rpmodectl1 = I915_READ(GEN6_RP_CONTROL);
 	rcctl1 = I915_READ(GEN6_RC_CONTROL);
 
@@ -1264,11 +1264,9 @@ static int vlv_drpc_info(struct seq_file *m)
 		   yesno(rcctl1 & (GEN7_RC_CTL_TO_MODE |
 					GEN6_RC_CTL_EI_MODE(1))));
 	seq_printf(m, "Render Power Well: %s\n",
-			(I915_READ(VLV_GTLC_PW_STATUS) &
-				VLV_GTLC_PW_RENDER_STATUS_MASK) ? "Up" : "Down");
+		   (pw_status & VLV_GTLC_PW_RENDER_STATUS_MASK) ? "Up" : "Down");
 	seq_printf(m, "Media Power Well: %s\n",
-			(I915_READ(VLV_GTLC_PW_STATUS) &
-				VLV_GTLC_PW_MEDIA_STATUS_MASK) ? "Up" : "Down");
+		   (pw_status & VLV_GTLC_PW_MEDIA_STATUS_MASK) ? "Up" : "Down");
 
 	seq_printf(m, "Render RC6 residency since boot: %u\n",
 		   I915_READ(VLV_GT_RENDER_RC6));
@@ -1774,6 +1772,50 @@ static int i915_context_status(struct seq_file *m, void *unused)
 	return 0;
 }
 
+static void i915_dump_lrc_obj(struct seq_file *m,
+			      struct intel_engine_cs *ring,
+			      struct drm_i915_gem_object *ctx_obj)
+{
+	struct page *page;
+	uint32_t *reg_state;
+	int j;
+	unsigned long ggtt_offset = 0;
+
+	if (ctx_obj == NULL) {
+		seq_printf(m, "Context on %s with no gem object\n",
+			   ring->name);
+		return;
+	}
+
+	seq_printf(m, "CONTEXT: %s %u\n", ring->name,
+		   intel_execlists_ctx_id(ctx_obj));
+
+	if (!i915_gem_obj_ggtt_bound(ctx_obj))
+		seq_puts(m, "\tNot bound in GGTT\n");
+	else
+		ggtt_offset = i915_gem_obj_ggtt_offset(ctx_obj);
+
+	if (i915_gem_object_get_pages(ctx_obj)) {
+		seq_puts(m, "\tFailed to get pages for context object\n");
+		return;
+	}
+
+	page = i915_gem_object_get_page(ctx_obj, 1);
+	if (!WARN_ON(page == NULL)) {
+		reg_state = kmap_atomic(page);
+
+		for (j = 0; j < 0x600 / sizeof(u32) / 4; j += 4) {
+			seq_printf(m, "\t[0x%08lx] 0x%08x 0x%08x 0x%08x 0x%08x\n",
+				   ggtt_offset + 4096 + (j * 4),
+				   reg_state[j], reg_state[j + 1],
+				   reg_state[j + 2], reg_state[j + 3]);
+		}
+		kunmap_atomic(reg_state);
+	}
+
+	seq_putc(m, '\n');
+}
+
 static int i915_dump_lrc(struct seq_file *m, void *unused)
 {
 	struct drm_info_node *node = (struct drm_info_node *) m->private;
@@ -1794,29 +1836,9 @@ static int i915_dump_lrc(struct seq_file *m, void *unused)
 
 	list_for_each_entry(ctx, &dev_priv->context_list, link) {
 		for_each_ring(ring, dev_priv, i) {
-			struct drm_i915_gem_object *ctx_obj = ctx->engine[i].state;
-
-			if (ring->default_context == ctx)
-				continue;
-
-			if (ctx_obj) {
-				struct page *page = i915_gem_object_get_page(ctx_obj, 1);
-				uint32_t *reg_state = kmap_atomic(page);
-				int j;
-
-				seq_printf(m, "CONTEXT: %s %u\n", ring->name,
-						intel_execlists_ctx_id(ctx_obj));
-
-				for (j = 0; j < 0x600 / sizeof(u32) / 4; j += 4) {
-					seq_printf(m, "\t[0x%08lx] 0x%08x 0x%08x 0x%08x 0x%08x\n",
-					i915_gem_obj_ggtt_offset(ctx_obj) + 4096 + (j * 4),
-					reg_state[j], reg_state[j + 1],
-					reg_state[j + 2], reg_state[j + 3]);
-				}
-				kunmap_atomic(reg_state);
-
-				seq_putc(m, '\n');
-			}
+			if (ring->default_context != ctx)
+				i915_dump_lrc_obj(m, ring,
+						  ctx->engine[i].state);
 		}
 	}
 
@@ -1849,6 +1871,8 @@ static int i915_execlists(struct seq_file *m, void *data)
 	if (ret)
 		return ret;
 
+	intel_runtime_pm_get(dev_priv);
+
 	for_each_ring(ring, dev_priv, ring_id) {
 		struct intel_ctx_submit_request *head_req = NULL;
 		int count = 0;
@@ -1900,6 +1924,7 @@ static int i915_execlists(struct seq_file *m, void *data)
 		seq_putc(m, '\n');
 	}
 
+	intel_runtime_pm_put(dev_priv);
 	mutex_unlock(&dev->struct_mutex);
 
 	return 0;
@@ -1973,6 +1998,8 @@ static int i915_swizzle_info(struct seq_file *m, void *data)
 	if (IS_GEN3(dev) || IS_GEN4(dev)) {
 		seq_printf(m, "DDC = 0x%08x\n",
 			   I915_READ(DCC));
+		seq_printf(m, "DDC2 = 0x%08x\n",
+			   I915_READ(DCC2));
 		seq_printf(m, "C0DRB3 = 0x%04x\n",
 			   I915_READ16(C0DRB3));
 		seq_printf(m, "C1DRB3 = 0x%04x\n",
@@ -1986,7 +2013,7 @@ static int i915_swizzle_info(struct seq_file *m, void *data)
 			   I915_READ(MAD_DIMM_C2));
 		seq_printf(m, "TILECTL = 0x%08x\n",
 			   I915_READ(TILECTL));
-		if (IS_GEN8(dev))
+		if (INTEL_INFO(dev)->gen >= 8)
 			seq_printf(m, "GAMTARBMODE = 0x%08x\n",
 				   I915_READ(GAMTARBMODE));
 		else
@@ -1995,6 +2022,10 @@ static int i915_swizzle_info(struct seq_file *m, void *data)
 		seq_printf(m, "DISP_ARB_CTL = 0x%08x\n",
 			   I915_READ(DISP_ARB_CTL));
 	}
+
+	if (dev_priv->quirks & QUIRK_PIN_SWIZZLED_PAGES)
+		seq_puts(m, "L-shaped memory detected\n");
+
 	intel_runtime_pm_put(dev_priv);
 	mutex_unlock(&dev->struct_mutex);
 
@@ -2628,14 +2659,15 @@ static int i915_shared_dplls_info(struct seq_file *m, void *unused)
 		struct intel_shared_dpll *pll = &dev_priv->shared_dplls[i];
 
 		seq_printf(m, "DPLL%i: %s, id: %i\n", i, pll->name, pll->id);
-		seq_printf(m, " refcount: %i, active: %i, on: %s\n", pll->refcount,
-			   pll->active, yesno(pll->on));
+		seq_printf(m, " crtc_mask: 0x%08x, active: %d, on: %s\n",
+			   pll->config.crtc_mask, pll->active, yesno(pll->on));
 		seq_printf(m, " tracked hardware state:\n");
-		seq_printf(m, " dpll:    0x%08x\n", pll->hw_state.dpll);
-		seq_printf(m, " dpll_md: 0x%08x\n", pll->hw_state.dpll_md);
-		seq_printf(m, " fp0:     0x%08x\n", pll->hw_state.fp0);
-		seq_printf(m, " fp1:     0x%08x\n", pll->hw_state.fp1);
-		seq_printf(m, " wrpll:   0x%08x\n", pll->hw_state.wrpll);
+		seq_printf(m, " dpll:    0x%08x\n", pll->config.hw_state.dpll);
+		seq_printf(m, " dpll_md: 0x%08x\n",
+			   pll->config.hw_state.dpll_md);
+		seq_printf(m, " fp0:     0x%08x\n", pll->config.hw_state.fp0);
+		seq_printf(m, " fp1:     0x%08x\n", pll->config.hw_state.fp1);
+		seq_printf(m, " wrpll:   0x%08x\n", pll->config.hw_state.wrpll);
 	}
 	drm_modeset_unlock_all(dev);
 
@@ -2656,18 +2688,18 @@ static int i915_wa_registers(struct seq_file *m, void *unused)
 
 	intel_runtime_pm_get(dev_priv);
 
-	seq_printf(m, "Workarounds applied: %d\n", dev_priv->num_wa_regs);
-	for (i = 0; i < dev_priv->num_wa_regs; ++i) {
-		u32 addr, mask;
-
-		addr = dev_priv->intel_wa_regs[i].addr;
-		mask = dev_priv->intel_wa_regs[i].mask;
-		dev_priv->intel_wa_regs[i].value = I915_READ(addr) | mask;
-		if (dev_priv->intel_wa_regs[i].addr)
-			seq_printf(m, "0x%X: 0x%08X, mask: 0x%08X\n",
-				   dev_priv->intel_wa_regs[i].addr,
-				   dev_priv->intel_wa_regs[i].value,
-				   dev_priv->intel_wa_regs[i].mask);
+	seq_printf(m, "Workarounds applied: %d\n", dev_priv->workarounds.count);
+	for (i = 0; i < dev_priv->workarounds.count; ++i) {
+		u32 addr, mask, value, read;
+		bool ok;
+
+		addr = dev_priv->workarounds.reg[i].addr;
+		mask = dev_priv->workarounds.reg[i].mask;
+		value = dev_priv->workarounds.reg[i].value;
+		read = I915_READ(addr);
+		ok = (value & mask) == (read & mask);
+		seq_printf(m, "0x%X: 0x%08X, mask: 0x%08X, read: 0x%08x, status: %s\n",
+			   addr, value, mask, read, ok ? "OK" : "FAIL");
 	}
 
 	intel_runtime_pm_put(dev_priv);
@@ -2676,6 +2708,42 @@ static int i915_wa_registers(struct seq_file *m, void *unused)
 	return 0;
 }
 
+static int i915_ddb_info(struct seq_file *m, void *unused)
+{
+	struct drm_info_node *node = m->private;
+	struct drm_device *dev = node->minor->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct skl_ddb_allocation *ddb;
+	struct skl_ddb_entry *entry;
+	enum pipe pipe;
+	int plane;
+
+	drm_modeset_lock_all(dev);
+
+	ddb = &dev_priv->wm.skl_hw.ddb;
+
+	seq_printf(m, "%-15s%8s%8s%8s\n", "", "Start", "End", "Size");
+
+	for_each_pipe(dev_priv, pipe) {
+		seq_printf(m, "Pipe %c\n", pipe_name(pipe));
+
+		for_each_plane(pipe, plane) {
+			entry = &ddb->plane[pipe][plane];
+			seq_printf(m, "  Plane%-8d%8u%8u%8u\n", plane + 1,
+				   entry->start, entry->end,
+				   skl_ddb_entry_size(entry));
+		}
+
+		entry = &ddb->cursor[pipe];
+		seq_printf(m, "  %-13s%8u%8u%8u\n", "Cursor", entry->start,
+			   entry->end, skl_ddb_entry_size(entry));
+	}
+
+	drm_modeset_unlock_all(dev);
+
+	return 0;
+}
+
 struct pipe_crc_info {
 	const char *name;
 	struct drm_device *dev;
@@ -2969,6 +3037,8 @@ static int i9xx_pipe_crc_auto_source(struct drm_device *dev, enum pipe pipe,
 				break;
 			}
 			break;
+		default:
+			break;
 		}
 	}
 	drm_modeset_unlock_all(dev);
@@ -3256,6 +3326,8 @@ static int pipe_crc_set_source(struct drm_device *dev, enum pipe pipe,
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_pipe_crc *pipe_crc = &dev_priv->pipe_crc[pipe];
+	struct intel_crtc *crtc = to_intel_crtc(intel_get_crtc_for_pipe(dev,
+									pipe));
 	u32 val = 0; /* shut up gcc */
 	int ret;
 
@@ -3266,6 +3338,11 @@ static int pipe_crc_set_source(struct drm_device *dev, enum pipe pipe,
 	if (pipe_crc->source && source)
 		return -EINVAL;
 
+	if (!intel_display_power_is_enabled(dev_priv, POWER_DOMAIN_PIPE(pipe))) {
+		DRM_DEBUG_KMS("Trying to capture CRC while pipe is off\n");
+		return -EIO;
+	}
+
 	if (IS_GEN2(dev))
 		ret = i8xx_pipe_crc_ctl_reg(&source, &val);
 	else if (INTEL_INFO(dev)->gen < 5)
@@ -3291,6 +3368,14 @@ static int pipe_crc_set_source(struct drm_device *dev, enum pipe pipe,
 		if (!pipe_crc->entries)
 			return -ENOMEM;
 
+		/*
+		 * When IPS gets enabled, the pipe CRC changes. Since IPS gets
+		 * enabled and disabled dynamically based on package C states,
+		 * user space can't make reliable use of the CRCs, so let's just
+		 * completely disable it.
+		 */
+		hsw_disable_ips(crtc);
+
 		spin_lock_irq(&pipe_crc->lock);
 		pipe_crc->head = 0;
 		pipe_crc->tail = 0;
@@ -3329,6 +3414,8 @@ static int pipe_crc_set_source(struct drm_device *dev, enum pipe pipe,
 			vlv_undo_pipe_scramble_reset(dev, pipe);
 		else if (IS_HASWELL(dev) && pipe == PIPE_A)
 			hsw_undo_trans_edp_pipe_A_crc_wa(dev);
+
+		hsw_enable_ips(crtc);
 	}
 
 	return 0;
@@ -3506,7 +3593,7 @@ static const struct file_operations i915_display_crc_ctl_fops = {
 	.write = display_crc_ctl_write
 };
 
-static void wm_latency_show(struct seq_file *m, const uint16_t wm[5])
+static void wm_latency_show(struct seq_file *m, const uint16_t wm[8])
 {
 	struct drm_device *dev = m->private;
 	int num_levels = ilk_wm_max_level(dev) + 1;
@@ -3517,13 +3604,17 @@ static void wm_latency_show(struct seq_file *m, const uint16_t wm[5])
 	for (level = 0; level < num_levels; level++) {
 		unsigned int latency = wm[level];
 
-		/* WM1+ latency values in 0.5us units */
-		if (level > 0)
+		/*
+		 * - WM1+ latency values in 0.5us units
+		 * - latencies are in us on gen9
+		 */
+		if (INTEL_INFO(dev)->gen >= 9)
+			latency *= 10;
+		else if (level > 0)
 			latency *= 5;
 
 		seq_printf(m, "WM%d %u (%u.%u usec)\n",
-			   level, wm[level],
-			   latency / 10, latency % 10);
+			   level, wm[level], latency / 10, latency % 10);
 	}
 
 	drm_modeset_unlock_all(dev);
@@ -3532,8 +3623,15 @@ static void wm_latency_show(struct seq_file *m, const uint16_t wm[5])
 static int pri_wm_latency_show(struct seq_file *m, void *data)
 {
 	struct drm_device *dev = m->private;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	const uint16_t *latencies;
+
+	if (INTEL_INFO(dev)->gen >= 9)
+		latencies = dev_priv->wm.skl_latency;
+	else
+		latencies = to_i915(dev)->wm.pri_latency;
 
-	wm_latency_show(m, to_i915(dev)->wm.pri_latency);
+	wm_latency_show(m, latencies);
 
 	return 0;
 }
@@ -3541,8 +3639,15 @@ static int pri_wm_latency_show(struct seq_file *m, void *data)
 static int spr_wm_latency_show(struct seq_file *m, void *data)
 {
 	struct drm_device *dev = m->private;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	const uint16_t *latencies;
+
+	if (INTEL_INFO(dev)->gen >= 9)
+		latencies = dev_priv->wm.skl_latency;
+	else
+		latencies = to_i915(dev)->wm.spr_latency;
 
-	wm_latency_show(m, to_i915(dev)->wm.spr_latency);
+	wm_latency_show(m, latencies);
 
 	return 0;
 }
@@ -3550,8 +3655,15 @@ static int spr_wm_latency_show(struct seq_file *m, void *data)
 static int cur_wm_latency_show(struct seq_file *m, void *data)
 {
 	struct drm_device *dev = m->private;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	const uint16_t *latencies;
+
+	if (INTEL_INFO(dev)->gen >= 9)
+		latencies = dev_priv->wm.skl_latency;
+	else
+		latencies = to_i915(dev)->wm.cur_latency;
 
-	wm_latency_show(m, to_i915(dev)->wm.cur_latency);
+	wm_latency_show(m, latencies);
 
 	return 0;
 }
@@ -3587,11 +3699,11 @@ static int cur_wm_latency_open(struct inode *inode, struct file *file)
 }
 
 static ssize_t wm_latency_write(struct file *file, const char __user *ubuf,
-				size_t len, loff_t *offp, uint16_t wm[5])
+				size_t len, loff_t *offp, uint16_t wm[8])
 {
 	struct seq_file *m = file->private_data;
 	struct drm_device *dev = m->private;
-	uint16_t new[5] = { 0 };
+	uint16_t new[8] = { 0 };
 	int num_levels = ilk_wm_max_level(dev) + 1;
 	int level;
 	int ret;
@@ -3605,7 +3717,9 @@ static ssize_t wm_latency_write(struct file *file, const char __user *ubuf,
 
 	tmp[len] = '\0';
 
-	ret = sscanf(tmp, "%hu %hu %hu %hu %hu", &new[0], &new[1], &new[2], &new[3], &new[4]);
+	ret = sscanf(tmp, "%hu %hu %hu %hu %hu %hu %hu %hu",
+		     &new[0], &new[1], &new[2], &new[3],
+		     &new[4], &new[5], &new[6], &new[7]);
 	if (ret != num_levels)
 		return -EINVAL;
 
@@ -3625,8 +3739,15 @@ static ssize_t pri_wm_latency_write(struct file *file, const char __user *ubuf,
 {
 	struct seq_file *m = file->private_data;
 	struct drm_device *dev = m->private;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint16_t *latencies;
 
-	return wm_latency_write(file, ubuf, len, offp, to_i915(dev)->wm.pri_latency);
+	if (INTEL_INFO(dev)->gen >= 9)
+		latencies = dev_priv->wm.skl_latency;
+	else
+		latencies = to_i915(dev)->wm.pri_latency;
+
+	return wm_latency_write(file, ubuf, len, offp, latencies);
 }
 
 static ssize_t spr_wm_latency_write(struct file *file, const char __user *ubuf,
@@ -3634,8 +3755,15 @@ static ssize_t spr_wm_latency_write(struct file *file, const char __user *ubuf,
 {
 	struct seq_file *m = file->private_data;
 	struct drm_device *dev = m->private;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint16_t *latencies;
 
-	return wm_latency_write(file, ubuf, len, offp, to_i915(dev)->wm.spr_latency);
+	if (INTEL_INFO(dev)->gen >= 9)
+		latencies = dev_priv->wm.skl_latency;
+	else
+		latencies = to_i915(dev)->wm.spr_latency;
+
+	return wm_latency_write(file, ubuf, len, offp, latencies);
 }
 
 static ssize_t cur_wm_latency_write(struct file *file, const char __user *ubuf,
@@ -3643,8 +3771,15 @@ static ssize_t cur_wm_latency_write(struct file *file, const char __user *ubuf,
 {
 	struct seq_file *m = file->private_data;
 	struct drm_device *dev = m->private;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint16_t *latencies;
+
+	if (INTEL_INFO(dev)->gen >= 9)
+		latencies = dev_priv->wm.skl_latency;
+	else
+		latencies = to_i915(dev)->wm.cur_latency;
 
-	return wm_latency_write(file, ubuf, len, offp, to_i915(dev)->wm.cur_latency);
+	return wm_latency_write(file, ubuf, len, offp, latencies);
 }
 
 static const struct file_operations i915_pri_wm_latency_fops = {
@@ -4187,6 +4322,7 @@ static const struct drm_info_list i915_debugfs_list[] = {
 	{"i915_shared_dplls_info", i915_shared_dplls_info, 0},
 	{"i915_dp_mst_info", i915_dp_mst_info, 0},
 	{"i915_wa_registers", i915_wa_registers, 0},
+	{"i915_ddb_info", i915_ddb_info, 0},
 };
 #define I915_DEBUGFS_ENTRIES ARRAY_SIZE(i915_debugfs_list)
 
diff --git a/drivers/gpu/drm/i915/i915_dma.c b/drivers/gpu/drm/i915/i915_dma.c
index 318ade9bb5af..ecee3bcc8772 100644
--- a/drivers/gpu/drm/i915/i915_dma.c
+++ b/drivers/gpu/drm/i915/i915_dma.c
@@ -50,884 +50,6 @@
 #include <linux/pm_runtime.h>
 #include <linux/oom.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)->buffer->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)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_i915_master_private *master_priv;
-
-	/*
-	 * The dri breadcrumb update races against the drm master disappearing.
-	 * Instead of trying to fix this (this is by far not the only ums issue)
-	 * just don't do the update in kms mode.
-	 */
-	if (drm_core_check_feature(dev, DRIVER_MODESET))
-		return;
-
-	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)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 addr;
-
-	addr = dev_priv->status_page_dmah->busaddr;
-	if (INTEL_INFO(dev)->gen >= 4)
-		addr |= (dev_priv->status_page_dmah->busaddr >> 28) & 0xf0;
-	I915_WRITE(HWS_PGA, addr);
-}
-
-/**
- * Frees the hardware status page, whether it's a physical address or a virtual
- * address set up by the X Server.
- */
-static void i915_free_hws(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_engine_cs *ring = LP_RING(dev_priv);
-
-	if (dev_priv->status_page_dmah) {
-		drm_pci_free(dev, dev_priv->status_page_dmah);
-		dev_priv->status_page_dmah = NULL;
-	}
-
-	if (ring->status_page.gfx_addr) {
-		ring->status_page.gfx_addr = 0;
-		iounmap(dev_priv->dri1.gfx_hws_cpu_addr);
-	}
-
-	/* Need to rewrite hardware status page */
-	I915_WRITE(HWS_PGA, 0x1ffff000);
-}
-
-void i915_kernel_lost_context(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_i915_master_private *master_priv;
-	struct intel_engine_cs *ring = LP_RING(dev_priv);
-	struct intel_ringbuffer *ringbuf = ring->buffer;
-
-	/*
-	 * We should never lose context on the ring with modesetting
-	 * as we don't expose it to userspace
-	 */
-	if (drm_core_check_feature(dev, DRIVER_MODESET))
-		return;
-
-	ringbuf->head = I915_READ_HEAD(ring) & HEAD_ADDR;
-	ringbuf->tail = I915_READ_TAIL(ring) & TAIL_ADDR;
-	ringbuf->space = ringbuf->head - (ringbuf->tail + I915_RING_FREE_SPACE);
-	if (ringbuf->space < 0)
-		ringbuf->space += ringbuf->size;
-
-	if (!dev->primary->master)
-		return;
-
-	master_priv = dev->primary->master->driver_priv;
-	if (ringbuf->head == ringbuf->tail && master_priv->sarea_priv)
-		master_priv->sarea_priv->perf_boxes |= I915_BOX_RING_EMPTY;
-}
-
-static int i915_dma_cleanup(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int i;
-
-	/* Make sure interrupts are disabled here because the uninstall ioctl
-	 * may not have been called from userspace and after dev_private
-	 * is freed, it's too late.
-	 */
-	if (dev->irq_enabled)
-		drm_irq_uninstall(dev);
-
-	mutex_lock(&dev->struct_mutex);
-	for (i = 0; i < I915_NUM_RINGS; i++)
-		intel_cleanup_ring_buffer(&dev_priv->ring[i]);
-	mutex_unlock(&dev->struct_mutex);
-
-	/* Clear the HWS virtual address at teardown */
-	if (I915_NEED_GFX_HWS(dev))
-		i915_free_hws(dev);
-
-	return 0;
-}
-
-static int i915_initialize(struct drm_device *dev, drm_i915_init_t *init)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_i915_master_private *master_priv = dev->primary->master->driver_priv;
-	int ret;
-
-	master_priv->sarea = drm_legacy_getsarea(dev);
-	if (master_priv->sarea) {
-		master_priv->sarea_priv = (drm_i915_sarea_t *)
-			((u8 *)master_priv->sarea->handle + init->sarea_priv_offset);
-	} else {
-		DRM_DEBUG_DRIVER("sarea not found assuming DRI2 userspace\n");
-	}
-
-	if (init->ring_size != 0) {
-		if (LP_RING(dev_priv)->buffer->obj != NULL) {
-			i915_dma_cleanup(dev);
-			DRM_ERROR("Client tried to initialize ringbuffer in "
-				  "GEM mode\n");
-			return -EINVAL;
-		}
-
-		ret = intel_render_ring_init_dri(dev,
-						 init->ring_start,
-						 init->ring_size);
-		if (ret) {
-			i915_dma_cleanup(dev);
-			return ret;
-		}
-	}
-
-	dev_priv->dri1.cpp = init->cpp;
-	dev_priv->dri1.back_offset = init->back_offset;
-	dev_priv->dri1.front_offset = init->front_offset;
-	dev_priv->dri1.current_page = 0;
-	if (master_priv->sarea_priv)
-		master_priv->sarea_priv->pf_current_page = 0;
-
-	/* Allow hardware batchbuffers unless told otherwise.
-	 */
-	dev_priv->dri1.allow_batchbuffer = 1;
-
-	return 0;
-}
-
-static int i915_dma_resume(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_engine_cs *ring = LP_RING(dev_priv);
-
-	DRM_DEBUG_DRIVER("%s\n", __func__);
-
-	if (ring->buffer->virtual_start == NULL) {
-		DRM_ERROR("can not ioremap virtual address for"
-			  " ring buffer\n");
-		return -ENOMEM;
-	}
-
-	/* Program Hardware Status Page */
-	if (!ring->status_page.page_addr) {
-		DRM_ERROR("Can not find hardware status page\n");
-		return -EINVAL;
-	}
-	DRM_DEBUG_DRIVER("hw status page @ %p\n",
-				ring->status_page.page_addr);
-	if (ring->status_page.gfx_addr != 0)
-		intel_ring_setup_status_page(ring);
-	else
-		i915_write_hws_pga(dev);
-
-	DRM_DEBUG_DRIVER("Enabled hardware status page\n");
-
-	return 0;
-}
-
-static int i915_dma_init(struct drm_device *dev, void *data,
-			 struct drm_file *file_priv)
-{
-	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);
-		break;
-	case I915_CLEANUP_DMA:
-		retcode = i915_dma_cleanup(dev);
-		break;
-	case I915_RESUME_DMA:
-		retcode = i915_dma_resume(dev);
-		break;
-	default:
-		retcode = -EINVAL;
-		break;
-	}
-
-	return retcode;
-}
-
-/* Implement basically the same security restrictions as hardware does
- * for MI_BATCH_NON_SECURE.  These can be made stricter at any time.
- *
- * Most of the calculations below involve calculating the size of a
- * particular instruction.  It's important to get the size right as
- * that tells us where the next instruction to check is.  Any illegal
- * instruction detected will be given a size of zero, which is a
- * signal to abort the rest of the buffer.
- */
-static int validate_cmd(int cmd)
-{
-	switch (((cmd >> 29) & 0x7)) {
-	case 0x0:
-		switch ((cmd >> 23) & 0x3f) {
-		case 0x0:
-			return 1;	/* MI_NOOP */
-		case 0x4:
-			return 1;	/* MI_FLUSH */
-		default:
-			return 0;	/* disallow everything else */
-		}
-		break;
-	case 0x1:
-		return 0;	/* reserved */
-	case 0x2:
-		return (cmd & 0xff) + 2;	/* 2d commands */
-	case 0x3:
-		if (((cmd >> 24) & 0x1f) <= 0x18)
-			return 1;
-
-		switch ((cmd >> 24) & 0x1f) {
-		case 0x1c:
-			return 1;
-		case 0x1d:
-			switch ((cmd >> 16) & 0xff) {
-			case 0x3:
-				return (cmd & 0x1f) + 2;
-			case 0x4:
-				return (cmd & 0xf) + 2;
-			default:
-				return (cmd & 0xffff) + 2;
-			}
-		case 0x1e:
-			if (cmd & (1 << 23))
-				return (cmd & 0xffff) + 1;
-			else
-				return 1;
-		case 0x1f:
-			if ((cmd & (1 << 23)) == 0)	/* inline vertices */
-				return (cmd & 0x1ffff) + 2;
-			else if (cmd & (1 << 17))	/* indirect random */
-				if ((cmd & 0xffff) == 0)
-					return 0;	/* unknown length, too hard */
-				else
-					return (((cmd & 0xffff) + 1) / 2) + 1;
-			else
-				return 2;	/* indirect sequential */
-		default:
-			return 0;
-		}
-	default:
-		return 0;
-	}
-
-	return 0;
-}
-
-static int i915_emit_cmds(struct drm_device *dev, int *buffer, int dwords)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int i, ret;
-
-	if ((dwords+1) * sizeof(int) >= LP_RING(dev_priv)->buffer->size - 8)
-		return -EINVAL;
-
-	for (i = 0; i < dwords;) {
-		int sz = validate_cmd(buffer[i]);
-
-		if (sz == 0 || i + sz > dwords)
-			return -EINVAL;
-		i += sz;
-	}
-
-	ret = BEGIN_LP_RING((dwords+1)&~1);
-	if (ret)
-		return ret;
-
-	for (i = 0; i < dwords; i++)
-		OUT_RING(buffer[i]);
-	if (dwords & 1)
-		OUT_RING(0);
-
-	ADVANCE_LP_RING();
-
-	return 0;
-}
-
-int
-i915_emit_box(struct drm_device *dev,
-	      struct drm_clip_rect *box,
-	      int DR1, int DR4)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int ret;
-
-	if (box->y2 <= box->y1 || box->x2 <= box->x1 ||
-	    box->y2 <= 0 || box->x2 <= 0) {
-		DRM_ERROR("Bad box %d,%d..%d,%d\n",
-			  box->x1, box->y1, box->x2, box->y2);
-		return -EINVAL;
-	}
-
-	if (INTEL_INFO(dev)->gen >= 4) {
-		ret = BEGIN_LP_RING(4);
-		if (ret)
-			return ret;
-
-		OUT_RING(GFX_OP_DRAWRECT_INFO_I965);
-		OUT_RING((box->x1 & 0xffff) | (box->y1 << 16));
-		OUT_RING(((box->x2 - 1) & 0xffff) | ((box->y2 - 1) << 16));
-		OUT_RING(DR4);
-	} else {
-		ret = BEGIN_LP_RING(6);
-		if (ret)
-			return ret;
-
-		OUT_RING(GFX_OP_DRAWRECT_INFO);
-		OUT_RING(DR1);
-		OUT_RING((box->x1 & 0xffff) | (box->y1 << 16));
-		OUT_RING(((box->x2 - 1) & 0xffff) | ((box->y2 - 1) << 16));
-		OUT_RING(DR4);
-		OUT_RING(0);
-	}
-	ADVANCE_LP_RING();
-
-	return 0;
-}
-
-/* XXX: Emitting the counter should really be moved to part of the IRQ
- * emit. For now, do it in both places:
- */
-
-static void i915_emit_breadcrumb(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_i915_master_private *master_priv = dev->primary->master->driver_priv;
-
-	dev_priv->dri1.counter++;
-	if (dev_priv->dri1.counter > 0x7FFFFFFFUL)
-		dev_priv->dri1.counter = 0;
-	if (master_priv->sarea_priv)
-		master_priv->sarea_priv->last_enqueue = dev_priv->dri1.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->dri1.counter);
-		OUT_RING(0);
-		ADVANCE_LP_RING();
-	}
-}
-
-static int i915_dispatch_cmdbuffer(struct drm_device *dev,
-				   drm_i915_cmdbuffer_t *cmd,
-				   struct drm_clip_rect *cliprects,
-				   void *cmdbuf)
-{
-	int nbox = cmd->num_cliprects;
-	int i = 0, count, ret;
-
-	if (cmd->sz & 0x3) {
-		DRM_ERROR("alignment");
-		return -EINVAL;
-	}
-
-	i915_kernel_lost_context(dev);
-
-	count = nbox ? nbox : 1;
-
-	for (i = 0; i < count; i++) {
-		if (i < nbox) {
-			ret = i915_emit_box(dev, &cliprects[i],
-					    cmd->DR1, cmd->DR4);
-			if (ret)
-				return ret;
-		}
-
-		ret = i915_emit_cmds(dev, cmdbuf, cmd->sz / 4);
-		if (ret)
-			return ret;
-	}
-
-	i915_emit_breadcrumb(dev);
-	return 0;
-}
-
-static int i915_dispatch_batchbuffer(struct drm_device *dev,
-				     drm_i915_batchbuffer_t *batch,
-				     struct drm_clip_rect *cliprects)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int nbox = batch->num_cliprects;
-	int i, count, ret;
-
-	if ((batch->start | batch->used) & 0x7) {
-		DRM_ERROR("alignment");
-		return -EINVAL;
-	}
-
-	i915_kernel_lost_context(dev);
-
-	count = nbox ? nbox : 1;
-	for (i = 0; i < count; i++) {
-		if (i < nbox) {
-			ret = i915_emit_box(dev, &cliprects[i],
-					    batch->DR1, batch->DR4);
-			if (ret)
-				return ret;
-		}
-
-		if (!IS_I830(dev) && !IS_845G(dev)) {
-			ret = BEGIN_LP_RING(2);
-			if (ret)
-				return ret;
-
-			if (INTEL_INFO(dev)->gen >= 4) {
-				OUT_RING(MI_BATCH_BUFFER_START | (2 << 6) | MI_BATCH_NON_SECURE_I965);
-				OUT_RING(batch->start);
-			} else {
-				OUT_RING(MI_BATCH_BUFFER_START | (2 << 6));
-				OUT_RING(batch->start | MI_BATCH_NON_SECURE);
-			}
-		} else {
-			ret = BEGIN_LP_RING(4);
-			if (ret)
-				return ret;
-
-			OUT_RING(MI_BATCH_BUFFER);
-			OUT_RING(batch->start | MI_BATCH_NON_SECURE);
-			OUT_RING(batch->start + batch->used - 4);
-			OUT_RING(0);
-		}
-		ADVANCE_LP_RING();
-	}
-
-
-	if (IS_G4X(dev) || IS_GEN5(dev)) {
-		if (BEGIN_LP_RING(2) == 0) {
-			OUT_RING(MI_FLUSH | MI_NO_WRITE_FLUSH | MI_INVALIDATE_ISP);
-			OUT_RING(MI_NOOP);
-			ADVANCE_LP_RING();
-		}
-	}
-
-	i915_emit_breadcrumb(dev);
-	return 0;
-}
-
-static int i915_dispatch_flip(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_i915_master_private *master_priv =
-		dev->primary->master->driver_priv;
-	int ret;
-
-	if (!master_priv->sarea_priv)
-		return -EINVAL;
-
-	DRM_DEBUG_DRIVER("%s: page=%d pfCurrentPage=%d\n",
-			  __func__,
-			 dev_priv->dri1.current_page,
-			 master_priv->sarea_priv->pf_current_page);
-
-	i915_kernel_lost_context(dev);
-
-	ret = BEGIN_LP_RING(10);
-	if (ret)
-		return ret;
-
-	OUT_RING(MI_FLUSH | MI_READ_FLUSH);
-	OUT_RING(0);
-
-	OUT_RING(CMD_OP_DISPLAYBUFFER_INFO | ASYNC_FLIP);
-	OUT_RING(0);
-	if (dev_priv->dri1.current_page == 0) {
-		OUT_RING(dev_priv->dri1.back_offset);
-		dev_priv->dri1.current_page = 1;
-	} else {
-		OUT_RING(dev_priv->dri1.front_offset);
-		dev_priv->dri1.current_page = 0;
-	}
-	OUT_RING(0);
-
-	OUT_RING(MI_WAIT_FOR_EVENT | MI_WAIT_FOR_PLANE_A_FLIP);
-	OUT_RING(0);
-
-	ADVANCE_LP_RING();
-
-	master_priv->sarea_priv->last_enqueue = dev_priv->dri1.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->dri1.counter);
-		OUT_RING(0);
-		ADVANCE_LP_RING();
-	}
-
-	master_priv->sarea_priv->pf_current_page = dev_priv->dri1.current_page;
-	return 0;
-}
-
-static int i915_quiescent(struct drm_device *dev)
-{
-	i915_kernel_lost_context(dev);
-	return intel_ring_idle(LP_RING(dev->dev_private));
-}
-
-static int i915_flush_ioctl(struct drm_device *dev, void *data,
-			    struct drm_file *file_priv)
-{
-	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);
-	ret = i915_quiescent(dev);
-	mutex_unlock(&dev->struct_mutex);
-
-	return ret;
-}
-
-static int i915_batchbuffer(struct drm_device *dev, void *data,
-			    struct drm_file *file_priv)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_i915_master_private *master_priv;
-	drm_i915_sarea_t *sarea_priv;
-	drm_i915_batchbuffer_t *batch = data;
-	int ret;
-	struct drm_clip_rect *cliprects = NULL;
-
-	if (drm_core_check_feature(dev, DRIVER_MODESET))
-		return -ENODEV;
-
-	master_priv = dev->primary->master->driver_priv;
-	sarea_priv = (drm_i915_sarea_t *) master_priv->sarea_priv;
-
-	if (!dev_priv->dri1.allow_batchbuffer) {
-		DRM_ERROR("Batchbuffer ioctl disabled\n");
-		return -EINVAL;
-	}
-
-	DRM_DEBUG_DRIVER("i915 batchbuffer, start %x used %d cliprects %d\n",
-			batch->start, batch->used, batch->num_cliprects);
-
-	RING_LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	if (batch->num_cliprects < 0)
-		return -EINVAL;
-
-	if (batch->num_cliprects) {
-		cliprects = kcalloc(batch->num_cliprects,
-				    sizeof(*cliprects),
-				    GFP_KERNEL);
-		if (cliprects == NULL)
-			return -ENOMEM;
-
-		ret = copy_from_user(cliprects, batch->cliprects,
-				     batch->num_cliprects *
-				     sizeof(struct drm_clip_rect));
-		if (ret != 0) {
-			ret = -EFAULT;
-			goto fail_free;
-		}
-	}
-
-	mutex_lock(&dev->struct_mutex);
-	ret = i915_dispatch_batchbuffer(dev, batch, cliprects);
-	mutex_unlock(&dev->struct_mutex);
-
-	if (sarea_priv)
-		sarea_priv->last_dispatch = READ_BREADCRUMB(dev_priv);
-
-fail_free:
-	kfree(cliprects);
-
-	return ret;
-}
-
-static int i915_cmdbuffer(struct drm_device *dev, void *data,
-			  struct drm_file *file_priv)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_i915_master_private *master_priv;
-	drm_i915_sarea_t *sarea_priv;
-	drm_i915_cmdbuffer_t *cmdbuf = data;
-	struct drm_clip_rect *cliprects = NULL;
-	void *batch_data;
-	int ret;
-
-	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;
-
-	master_priv = dev->primary->master->driver_priv;
-	sarea_priv = (drm_i915_sarea_t *) master_priv->sarea_priv;
-
-	RING_LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	if (cmdbuf->num_cliprects < 0)
-		return -EINVAL;
-
-	batch_data = kmalloc(cmdbuf->sz, GFP_KERNEL);
-	if (batch_data == NULL)
-		return -ENOMEM;
-
-	ret = copy_from_user(batch_data, cmdbuf->buf, cmdbuf->sz);
-	if (ret != 0) {
-		ret = -EFAULT;
-		goto fail_batch_free;
-	}
-
-	if (cmdbuf->num_cliprects) {
-		cliprects = kcalloc(cmdbuf->num_cliprects,
-				    sizeof(*cliprects), GFP_KERNEL);
-		if (cliprects == NULL) {
-			ret = -ENOMEM;
-			goto fail_batch_free;
-		}
-
-		ret = copy_from_user(cliprects, cmdbuf->cliprects,
-				     cmdbuf->num_cliprects *
-				     sizeof(struct drm_clip_rect));
-		if (ret != 0) {
-			ret = -EFAULT;
-			goto fail_clip_free;
-		}
-	}
-
-	mutex_lock(&dev->struct_mutex);
-	ret = i915_dispatch_cmdbuffer(dev, cmdbuf, cliprects, batch_data);
-	mutex_unlock(&dev->struct_mutex);
-	if (ret) {
-		DRM_ERROR("i915_dispatch_cmdbuffer failed\n");
-		goto fail_clip_free;
-	}
-
-	if (sarea_priv)
-		sarea_priv->last_dispatch = READ_BREADCRUMB(dev_priv);
-
-fail_clip_free:
-	kfree(cliprects);
-fail_batch_free:
-	kfree(batch_data);
-
-	return ret;
-}
-
-static int i915_emit_irq(struct drm_device *dev)
-{
-	struct drm_i915_private *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->dri1.counter++;
-	if (dev_priv->dri1.counter > 0x7FFFFFFFUL)
-		dev_priv->dri1.counter = 1;
-	if (master_priv->sarea_priv)
-		master_priv->sarea_priv->last_enqueue = dev_priv->dri1.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->dri1.counter);
-		OUT_RING(MI_USER_INTERRUPT);
-		ADVANCE_LP_RING();
-	}
-
-	return dev_priv->dri1.counter;
-}
-
-static int i915_wait_irq(struct drm_device *dev, int irq_nr)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_i915_master_private *master_priv = dev->primary->master->driver_priv;
-	int ret = 0;
-	struct intel_engine_cs *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 * 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->dri1.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)
-{
-	struct drm_i915_private *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)->buffer->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 (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)
-{
-	struct drm_i915_private *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)
-{
-	struct drm_i915_private *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);
-
-	mutex_lock(&dev->struct_mutex);
-	ret = i915_dispatch_flip(dev);
-	mutex_unlock(&dev->struct_mutex);
-
-	return ret;
-}
 
 static int i915_getparam(struct drm_device *dev, void *data,
 			 struct drm_file *file_priv)
@@ -936,21 +58,12 @@ static int i915_getparam(struct drm_device *dev, void *data,
 	drm_i915_getparam_t *param = data;
 	int value;
 
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-
 	switch (param->param) {
 	case I915_PARAM_IRQ_ACTIVE:
-		value = dev->pdev->irq ? 1 : 0;
-		break;
 	case I915_PARAM_ALLOW_BATCHBUFFER:
-		value = dev_priv->dri1.allow_batchbuffer ? 1 : 0;
-		break;
 	case I915_PARAM_LAST_DISPATCH:
-		value = READ_BREADCRUMB(dev_priv);
-		break;
+		/* Reject all old ums/dri params. */
+		return -ENODEV;
 	case I915_PARAM_CHIPSET_ID:
 		value = dev->pdev->device;
 		break;
@@ -1027,6 +140,9 @@ static int i915_getparam(struct drm_device *dev, void *data,
 	case I915_PARAM_CMD_PARSER_VERSION:
 		value = i915_cmd_parser_get_version();
 		break;
+	case I915_PARAM_HAS_COHERENT_PHYS_GTT:
+		value = 1;
+		break;
 	default:
 		DRM_DEBUG("Unknown parameter %d\n", param->param);
 		return -EINVAL;
@@ -1046,19 +162,13 @@ static int i915_setparam(struct drm_device *dev, void *data,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	drm_i915_setparam_t *param = data;
 
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-
 	switch (param->param) {
 	case I915_SETPARAM_USE_MI_BATCHBUFFER_START:
-		break;
 	case I915_SETPARAM_TEX_LRU_LOG_GRANULARITY:
-		break;
 	case I915_SETPARAM_ALLOW_BATCHBUFFER:
-		dev_priv->dri1.allow_batchbuffer = param->value ? 1 : 0;
-		break;
+		/* Reject all old ums/dri params. */
+		return -ENODEV;
+
 	case I915_SETPARAM_NUM_USED_FENCES:
 		if (param->value > dev_priv->num_fence_regs ||
 		    param->value < 0)
@@ -1075,54 +185,6 @@ static int i915_setparam(struct drm_device *dev, void *data,
 	return 0;
 }
 
-static int i915_set_status_page(struct drm_device *dev, void *data,
-				struct drm_file *file_priv)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	drm_i915_hws_addr_t *hws = data;
-	struct intel_engine_cs *ring;
-
-	if (drm_core_check_feature(dev, DRIVER_MODESET))
-		return -ENODEV;
-
-	if (!I915_NEED_GFX_HWS(dev))
-		return -EINVAL;
-
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-
-	if (drm_core_check_feature(dev, DRIVER_MODESET)) {
-		WARN(1, "tried to set status page when mode setting active\n");
-		return 0;
-	}
-
-	DRM_DEBUG_DRIVER("set status page addr 0x%08x\n", (u32)hws->addr);
-
-	ring = LP_RING(dev_priv);
-	ring->status_page.gfx_addr = hws->addr & (0x1ffff<<12);
-
-	dev_priv->dri1.gfx_hws_cpu_addr =
-		ioremap_wc(dev_priv->gtt.mappable_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;
-	}
-
-	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",
-			 ring->status_page.gfx_addr);
-	DRM_DEBUG_DRIVER("load hws at %p\n",
-			 ring->status_page.page_addr);
-	return 0;
-}
-
 static int i915_get_bridge_dev(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -1275,12 +337,12 @@ static void i915_switcheroo_set_state(struct pci_dev *pdev, enum vga_switcheroo_
 		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);
+		i915_resume_legacy(dev);
 		dev->switch_power_state = DRM_SWITCH_POWER_ON;
 	} else {
 		pr_err("switched off\n");
 		dev->switch_power_state = DRM_SWITCH_POWER_CHANGING;
-		i915_suspend(dev, pmm);
+		i915_suspend_legacy(dev, pmm);
 		dev->switch_power_state = DRM_SWITCH_POWER_OFF;
 	}
 }
@@ -1338,14 +400,7 @@ static int i915_load_modeset_init(struct drm_device *dev)
 
 	intel_power_domains_init_hw(dev_priv);
 
-	/*
-	 * We enable some interrupt sources in our postinstall hooks, so mark
-	 * interrupts as enabled _before_ actually enabling them to avoid
-	 * special cases in our ordering checks.
-	 */
-	dev_priv->pm._irqs_disabled = false;
-
-	ret = drm_irq_install(dev, dev->pdev->irq);
+	ret = intel_irq_install(dev_priv);
 	if (ret)
 		goto cleanup_gem_stolen;
 
@@ -1370,7 +425,7 @@ static int i915_load_modeset_init(struct drm_device *dev)
 		goto cleanup_gem;
 
 	/* Only enable hotplug handling once the fbdev is fully set up. */
-	intel_hpd_init(dev);
+	intel_hpd_init(dev_priv);
 
 	/*
 	 * Some ports require correctly set-up hpd registers for detection to
@@ -1405,30 +460,6 @@ out:
 	return ret;
 }
 
-int i915_master_create(struct drm_device *dev, struct drm_master *master)
-{
-	struct drm_i915_master_private *master_priv;
-
-	master_priv = kzalloc(sizeof(*master_priv), GFP_KERNEL);
-	if (!master_priv)
-		return -ENOMEM;
-
-	master->driver_priv = master_priv;
-	return 0;
-}
-
-void i915_master_destroy(struct drm_device *dev, struct drm_master *master)
-{
-	struct drm_i915_master_private *master_priv = master->driver_priv;
-
-	if (!master_priv)
-		return;
-
-	kfree(master_priv);
-
-	master->driver_priv = NULL;
-}
-
 #if IS_ENABLED(CONFIG_FB)
 static int i915_kick_out_firmware_fb(struct drm_i915_private *dev_priv)
 {
@@ -1534,7 +565,7 @@ static void intel_device_info_runtime_init(struct drm_device *dev)
 
 	info = (struct intel_device_info *)&dev_priv->info;
 
-	if (IS_VALLEYVIEW(dev))
+	if (IS_VALLEYVIEW(dev) || INTEL_INFO(dev)->gen == 9)
 		for_each_pipe(dev_priv, pipe)
 			info->num_sprites[pipe] = 2;
 	else
@@ -1614,7 +645,7 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 
 	spin_lock_init(&dev_priv->irq_lock);
 	spin_lock_init(&dev_priv->gpu_error.lock);
-	spin_lock_init(&dev_priv->backlight_lock);
+	mutex_init(&dev_priv->backlight_lock);
 	spin_lock_init(&dev_priv->uncore.lock);
 	spin_lock_init(&dev_priv->mm.object_stat_lock);
 	spin_lock_init(&dev_priv->mmio_flip_lock);
@@ -1742,7 +773,7 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 		goto out_freewq;
 	}
 
-	intel_irq_init(dev);
+	intel_irq_init(dev_priv);
 	intel_uncore_sanitize(dev);
 
 	/* Try to make sure MCHBAR is enabled before poking at it */
@@ -1784,9 +815,6 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 			DRM_ERROR("failed to init modeset\n");
 			goto out_power_well;
 		}
-	} else {
-		/* Start out suspended in ums mode. */
-		dev_priv->ums.mm_suspended = 1;
 	}
 
 	i915_setup_sysfs(dev);
@@ -1800,12 +828,12 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 	if (IS_GEN5(dev))
 		intel_gpu_ips_init(dev_priv);
 
-	intel_init_runtime_pm(dev_priv);
+	intel_runtime_pm_enable(dev_priv);
 
 	return 0;
 
 out_power_well:
-	intel_power_domains_remove(dev_priv);
+	intel_power_domains_fini(dev_priv);
 	drm_vblank_cleanup(dev);
 out_gem_unload:
 	WARN_ON(unregister_oom_notifier(&dev_priv->mm.oom_notifier));
@@ -1848,16 +876,10 @@ int i915_driver_unload(struct drm_device *dev)
 		return ret;
 	}
 
-	intel_fini_runtime_pm(dev_priv);
+	intel_power_domains_fini(dev_priv);
 
 	intel_gpu_ips_teardown();
 
-	/* The i915.ko module is still not prepared to be loaded when
-	 * the power well is not enabled, so just enable it in case
-	 * we're going to unload/reload. */
-	intel_display_set_init_power(dev_priv, true);
-	intel_power_domains_remove(dev_priv);
-
 	i915_teardown_sysfs(dev);
 
 	WARN_ON(unregister_oom_notifier(&dev_priv->mm.oom_notifier));
@@ -1868,8 +890,12 @@ int i915_driver_unload(struct drm_device *dev)
 
 	acpi_video_unregister();
 
-	if (drm_core_check_feature(dev, DRIVER_MODESET)) {
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
 		intel_fbdev_fini(dev);
+
+	drm_vblank_cleanup(dev);
+
+	if (drm_core_check_feature(dev, DRIVER_MODESET)) {
 		intel_modeset_cleanup(dev);
 
 		/*
@@ -1905,13 +931,8 @@ int i915_driver_unload(struct drm_device *dev)
 		i915_gem_context_fini(dev);
 		mutex_unlock(&dev->struct_mutex);
 		i915_gem_cleanup_stolen(dev);
-
-		if (!I915_NEED_GFX_HWS(dev))
-			i915_free_hws(dev);
 	}
 
-	drm_vblank_cleanup(dev);
-
 	intel_teardown_gmbus(dev);
 	intel_teardown_mchbar(dev);
 
@@ -1959,23 +980,8 @@ int i915_driver_open(struct drm_device *dev, struct drm_file *file)
  */
 void i915_driver_lastclose(struct drm_device *dev)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	/* On gen6+ we refuse to init without kms enabled, but then the drm core
-	 * goes right around and calls lastclose. Check for this and don't clean
-	 * up anything. */
-	if (!dev_priv)
-		return;
-
-	if (drm_core_check_feature(dev, DRIVER_MODESET)) {
-		intel_fbdev_restore_mode(dev);
-		vga_switcheroo_process_delayed_switch();
-		return;
-	}
-
-	i915_gem_lastclose(dev);
-
-	i915_dma_cleanup(dev);
+	intel_fbdev_restore_mode(dev);
+	vga_switcheroo_process_delayed_switch();
 }
 
 void i915_driver_preclose(struct drm_device *dev, struct drm_file *file)
@@ -1999,24 +1005,24 @@ void i915_driver_postclose(struct drm_device *dev, struct drm_file *file)
 }
 
 const struct drm_ioctl_desc i915_ioctls[] = {
-	DRM_IOCTL_DEF_DRV(I915_INIT, i915_dma_init, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF_DRV(I915_FLUSH, i915_flush_ioctl, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(I915_FLIP, i915_flip_bufs, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(I915_BATCHBUFFER, i915_batchbuffer, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(I915_IRQ_EMIT, i915_irq_emit, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(I915_IRQ_WAIT, i915_irq_wait, DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(I915_INIT, drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF_DRV(I915_FLUSH, drm_noop, DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(I915_FLIP, drm_noop, DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(I915_BATCHBUFFER, drm_noop, DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(I915_IRQ_EMIT, drm_noop, DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(I915_IRQ_WAIT, drm_noop, DRM_AUTH),
 	DRM_IOCTL_DEF_DRV(I915_GETPARAM, i915_getparam, DRM_AUTH|DRM_RENDER_ALLOW),
 	DRM_IOCTL_DEF_DRV(I915_SETPARAM, i915_setparam, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
 	DRM_IOCTL_DEF_DRV(I915_ALLOC, drm_noop, DRM_AUTH),
 	DRM_IOCTL_DEF_DRV(I915_FREE, drm_noop, DRM_AUTH),
 	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_CMDBUFFER, drm_noop, 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,  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),
-	DRM_IOCTL_DEF_DRV(I915_GEM_INIT, i915_gem_init_ioctl, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY|DRM_UNLOCKED),
+	DRM_IOCTL_DEF_DRV(I915_GET_VBLANK_PIPE,  drm_noop, DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(I915_VBLANK_SWAP, drm_noop, DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(I915_HWS_ADDR, drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF_DRV(I915_GEM_INIT, drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY|DRM_UNLOCKED),
 	DRM_IOCTL_DEF_DRV(I915_GEM_EXECBUFFER, i915_gem_execbuffer, DRM_AUTH|DRM_UNLOCKED),
 	DRM_IOCTL_DEF_DRV(I915_GEM_EXECBUFFER2, i915_gem_execbuffer2, DRM_AUTH|DRM_UNLOCKED|DRM_RENDER_ALLOW),
 	DRM_IOCTL_DEF_DRV(I915_GEM_PIN, i915_gem_pin_ioctl, DRM_AUTH|DRM_ROOT_ONLY|DRM_UNLOCKED),
@@ -2025,8 +1031,8 @@ const struct drm_ioctl_desc i915_ioctls[] = {
 	DRM_IOCTL_DEF_DRV(I915_GEM_SET_CACHING, i915_gem_set_caching_ioctl, DRM_UNLOCKED|DRM_RENDER_ALLOW),
 	DRM_IOCTL_DEF_DRV(I915_GEM_GET_CACHING, i915_gem_get_caching_ioctl, DRM_UNLOCKED|DRM_RENDER_ALLOW),
 	DRM_IOCTL_DEF_DRV(I915_GEM_THROTTLE, i915_gem_throttle_ioctl, DRM_AUTH|DRM_UNLOCKED|DRM_RENDER_ALLOW),
-	DRM_IOCTL_DEF_DRV(I915_GEM_ENTERVT, i915_gem_entervt_ioctl, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY|DRM_UNLOCKED),
-	DRM_IOCTL_DEF_DRV(I915_GEM_LEAVEVT, i915_gem_leavevt_ioctl, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY|DRM_UNLOCKED),
+	DRM_IOCTL_DEF_DRV(I915_GEM_ENTERVT, drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY|DRM_UNLOCKED),
+	DRM_IOCTL_DEF_DRV(I915_GEM_LEAVEVT, drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY|DRM_UNLOCKED),
 	DRM_IOCTL_DEF_DRV(I915_GEM_CREATE, i915_gem_create_ioctl, DRM_UNLOCKED|DRM_RENDER_ALLOW),
 	DRM_IOCTL_DEF_DRV(I915_GEM_PREAD, i915_gem_pread_ioctl, DRM_UNLOCKED|DRM_RENDER_ALLOW),
 	DRM_IOCTL_DEF_DRV(I915_GEM_PWRITE, i915_gem_pwrite_ioctl, DRM_UNLOCKED|DRM_RENDER_ALLOW),
diff --git a/drivers/gpu/drm/i915/i915_drv.c b/drivers/gpu/drm/i915/i915_drv.c
index 2318b4c7a8f8..574057cd1d09 100644
--- a/drivers/gpu/drm/i915/i915_drv.c
+++ b/drivers/gpu/drm/i915/i915_drv.c
@@ -356,6 +356,19 @@ static const struct intel_device_info intel_cherryview_info = {
 	CURSOR_OFFSETS,
 };
 
+static const struct intel_device_info intel_skylake_info = {
+	.is_preliminary = 1,
+	.is_skylake = 1,
+	.gen = 9, .num_pipes = 3,
+	.need_gfx_hws = 1, .has_hotplug = 1,
+	.ring_mask = RENDER_RING | BSD_RING | BLT_RING | VEBOX_RING,
+	.has_llc = 1,
+	.has_ddi = 1,
+	.has_fbc = 1,
+	GEN_DEFAULT_PIPEOFFSETS,
+	IVB_CURSOR_OFFSETS,
+};
+
 /*
  * Make sure any device matches here are from most specific to most
  * general.  For example, since the Quanta match is based on the subsystem
@@ -392,7 +405,8 @@ static const struct intel_device_info intel_cherryview_info = {
 	INTEL_BDW_GT12D_IDS(&intel_broadwell_d_info),	\
 	INTEL_BDW_GT3M_IDS(&intel_broadwell_gt3m_info),	\
 	INTEL_BDW_GT3D_IDS(&intel_broadwell_gt3d_info), \
-	INTEL_CHV_IDS(&intel_cherryview_info)
+	INTEL_CHV_IDS(&intel_cherryview_info),	\
+	INTEL_SKL_IDS(&intel_skylake_info)
 
 static const struct pci_device_id pciidlist[] = {		/* aka */
 	INTEL_PCI_IDS,
@@ -449,7 +463,7 @@ void intel_detect_pch(struct drm_device *dev)
 				dev_priv->pch_type = PCH_LPT;
 				DRM_DEBUG_KMS("Found LynxPoint PCH\n");
 				WARN_ON(!IS_HASWELL(dev));
-				WARN_ON(IS_ULT(dev));
+				WARN_ON(IS_HSW_ULT(dev));
 			} else if (IS_BROADWELL(dev)) {
 				dev_priv->pch_type = PCH_LPT;
 				dev_priv->pch_id =
@@ -460,7 +474,15 @@ void intel_detect_pch(struct drm_device *dev)
 				dev_priv->pch_type = PCH_LPT;
 				DRM_DEBUG_KMS("Found LynxPoint LP PCH\n");
 				WARN_ON(!IS_HASWELL(dev));
-				WARN_ON(!IS_ULT(dev));
+				WARN_ON(!IS_HSW_ULT(dev));
+			} else if (id == INTEL_PCH_SPT_DEVICE_ID_TYPE) {
+				dev_priv->pch_type = PCH_SPT;
+				DRM_DEBUG_KMS("Found SunrisePoint PCH\n");
+				WARN_ON(!IS_SKYLAKE(dev));
+			} else if (id == INTEL_PCH_SPT_LP_DEVICE_ID_TYPE) {
+				dev_priv->pch_type = PCH_SPT;
+				DRM_DEBUG_KMS("Found SunrisePoint LP PCH\n");
+				WARN_ON(!IS_SKYLAKE(dev));
 			} else
 				continue;
 
@@ -529,10 +551,10 @@ static void intel_suspend_encoders(struct drm_i915_private *dev_priv)
 }
 
 static int intel_suspend_complete(struct drm_i915_private *dev_priv);
-static int intel_resume_prepare(struct drm_i915_private *dev_priv,
-				bool rpm_resume);
+static int vlv_resume_prepare(struct drm_i915_private *dev_priv,
+			      bool rpm_resume);
 
-static int i915_drm_freeze(struct drm_device *dev)
+static int i915_drm_suspend(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_crtc *crtc;
@@ -562,6 +584,8 @@ static int i915_drm_freeze(struct drm_device *dev)
 			return error;
 		}
 
+		intel_suspend_gt_powersave(dev);
+
 		/*
 		 * Disable CRTCs directly since we want to preserve sw state
 		 * for _thaw. Also, power gate the CRTC power wells.
@@ -573,16 +597,12 @@ static int i915_drm_freeze(struct drm_device *dev)
 
 		intel_dp_mst_suspend(dev);
 
-		flush_delayed_work(&dev_priv->rps.delayed_resume_work);
-
-		intel_runtime_pm_disable_interrupts(dev);
+		intel_runtime_pm_disable_interrupts(dev_priv);
 		intel_hpd_cancel_work(dev_priv);
 
 		intel_suspend_encoders(dev_priv);
 
-		intel_suspend_gt_powersave(dev);
-
-		intel_modeset_suspend_hw(dev);
+		intel_suspend_hw(dev);
 	}
 
 	i915_gem_suspend_gtt_mappings(dev);
@@ -608,7 +628,26 @@ static int i915_drm_freeze(struct drm_device *dev)
 	return 0;
 }
 
-int i915_suspend(struct drm_device *dev, pm_message_t state)
+static int i915_drm_suspend_late(struct drm_device *drm_dev)
+{
+	struct drm_i915_private *dev_priv = drm_dev->dev_private;
+	int ret;
+
+	ret = intel_suspend_complete(dev_priv);
+
+	if (ret) {
+		DRM_ERROR("Suspend complete failed: %d\n", ret);
+
+		return ret;
+	}
+
+	pci_disable_device(drm_dev->pdev);
+	pci_set_power_state(drm_dev->pdev, PCI_D3hot);
+
+	return 0;
+}
+
+int i915_suspend_legacy(struct drm_device *dev, pm_message_t state)
 {
 	int error;
 
@@ -618,48 +657,25 @@ int i915_suspend(struct drm_device *dev, pm_message_t state)
 		return -ENODEV;
 	}
 
-	if (state.event == PM_EVENT_PRETHAW)
-		return 0;
-
+	if (WARN_ON_ONCE(state.event != PM_EVENT_SUSPEND &&
+			 state.event != PM_EVENT_FREEZE))
+		return -EINVAL;
 
 	if (dev->switch_power_state == DRM_SWITCH_POWER_OFF)
 		return 0;
 
-	error = i915_drm_freeze(dev);
+	error = i915_drm_suspend(dev);
 	if (error)
 		return error;
 
-	if (state.event == PM_EVENT_SUSPEND) {
-		/* Shut down the device */
-		pci_disable_device(dev->pdev);
-		pci_set_power_state(dev->pdev, PCI_D3hot);
-	}
-
-	return 0;
-}
-
-static int i915_drm_thaw_early(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int ret;
-
-	ret = intel_resume_prepare(dev_priv, false);
-	if (ret)
-		DRM_ERROR("Resume prepare failed: %d,Continuing resume\n", ret);
-
-	intel_uncore_early_sanitize(dev, true);
-	intel_uncore_sanitize(dev);
-	intel_power_domains_init_hw(dev_priv);
-
-	return ret;
+	return i915_drm_suspend_late(dev);
 }
 
-static int __i915_drm_thaw(struct drm_device *dev, bool restore_gtt_mappings)
+static int i915_drm_resume(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	if (drm_core_check_feature(dev, DRIVER_MODESET) &&
-	    restore_gtt_mappings) {
+	if (drm_core_check_feature(dev, DRIVER_MODESET)) {
 		mutex_lock(&dev->struct_mutex);
 		i915_gem_restore_gtt_mappings(dev);
 		mutex_unlock(&dev->struct_mutex);
@@ -680,30 +696,29 @@ static int __i915_drm_thaw(struct drm_device *dev, bool restore_gtt_mappings)
 		}
 		mutex_unlock(&dev->struct_mutex);
 
-		intel_runtime_pm_restore_interrupts(dev);
+		/* We need working interrupts for modeset enabling ... */
+		intel_runtime_pm_enable_interrupts(dev_priv);
 
 		intel_modeset_init_hw(dev);
 
-		{
-			unsigned long irqflags;
-			spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-			if (dev_priv->display.hpd_irq_setup)
-				dev_priv->display.hpd_irq_setup(dev);
-			spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
-		}
+		spin_lock_irq(&dev_priv->irq_lock);
+		if (dev_priv->display.hpd_irq_setup)
+			dev_priv->display.hpd_irq_setup(dev);
+		spin_unlock_irq(&dev_priv->irq_lock);
 
-		intel_dp_mst_resume(dev);
 		drm_modeset_lock_all(dev);
 		intel_modeset_setup_hw_state(dev, true);
 		drm_modeset_unlock_all(dev);
 
+		intel_dp_mst_resume(dev);
+
 		/*
 		 * ... but also need to make sure that hotplug processing
 		 * doesn't cause havoc. Like in the driver load code we don't
 		 * bother with the tiny race here where we might loose hotplug
 		 * notifications.
 		 * */
-		intel_hpd_init(dev);
+		intel_hpd_init(dev_priv);
 		/* Config may have changed between suspend and resume */
 		drm_helper_hpd_irq_event(dev);
 	}
@@ -718,21 +733,15 @@ static int __i915_drm_thaw(struct drm_device *dev, bool restore_gtt_mappings)
 
 	intel_opregion_notify_adapter(dev, PCI_D0);
 
-	return 0;
-}
-
-static int i915_drm_thaw(struct drm_device *dev)
-{
-	if (drm_core_check_feature(dev, DRIVER_MODESET))
-		i915_check_and_clear_faults(dev);
+	drm_kms_helper_poll_enable(dev);
 
-	return __i915_drm_thaw(dev, true);
+	return 0;
 }
 
-static int i915_resume_early(struct drm_device *dev)
+static int i915_drm_resume_early(struct drm_device *dev)
 {
-	if (dev->switch_power_state == DRM_SWITCH_POWER_OFF)
-		return 0;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int ret = 0;
 
 	/*
 	 * We have a resume ordering issue with the snd-hda driver also
@@ -748,33 +757,34 @@ static int i915_resume_early(struct drm_device *dev)
 
 	pci_set_master(dev->pdev);
 
-	return i915_drm_thaw_early(dev);
+	if (IS_VALLEYVIEW(dev_priv))
+		ret = vlv_resume_prepare(dev_priv, false);
+	if (ret)
+		DRM_ERROR("Resume prepare failed: %d,Continuing resume\n", ret);
+
+	intel_uncore_early_sanitize(dev, true);
+
+	if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
+		hsw_disable_pc8(dev_priv);
+
+	intel_uncore_sanitize(dev);
+	intel_power_domains_init_hw(dev_priv);
+
+	return ret;
 }
 
-int i915_resume(struct drm_device *dev)
+int i915_resume_legacy(struct drm_device *dev)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret;
 
-	/*
-	 * Platforms with opregion should have sane BIOS, older ones (gen3 and
-	 * earlier) need to restore the GTT mappings since the BIOS might clear
-	 * all our scratch PTEs.
-	 */
-	ret = __i915_drm_thaw(dev, !dev_priv->opregion.header);
+	if (dev->switch_power_state == DRM_SWITCH_POWER_OFF)
+		return 0;
+
+	ret = i915_drm_resume_early(dev);
 	if (ret)
 		return ret;
 
-	drm_kms_helper_poll_enable(dev);
-	return 0;
-}
-
-static int i915_resume_legacy(struct drm_device *dev)
-{
-	i915_resume_early(dev);
-	i915_resume(dev);
-
-	return 0;
+	return i915_drm_resume(dev);
 }
 
 /**
@@ -801,6 +811,8 @@ int i915_reset(struct drm_device *dev)
 	if (!i915.reset)
 		return 0;
 
+	intel_reset_gt_powersave(dev);
+
 	mutex_lock(&dev->struct_mutex);
 
 	i915_gem_reset(dev);
@@ -820,6 +832,9 @@ int i915_reset(struct drm_device *dev)
 		}
 	}
 
+	if (i915_stop_ring_allow_warn(dev_priv))
+		pr_notice("drm/i915: Resetting chip after gpu hang\n");
+
 	if (ret) {
 		DRM_ERROR("Failed to reset chip: %i\n", ret);
 		mutex_unlock(&dev->struct_mutex);
@@ -840,10 +855,7 @@ int i915_reset(struct drm_device *dev)
 	 * was running at the time of the reset (i.e. we weren't VT
 	 * switched away).
 	 */
-	if (drm_core_check_feature(dev, DRIVER_MODESET) ||
-			!dev_priv->ums.mm_suspended) {
-		dev_priv->ums.mm_suspended = 0;
-
+	if (drm_core_check_feature(dev, DRIVER_MODESET)) {
 		/* Used to prevent gem_check_wedged returning -EAGAIN during gpu reset */
 		dev_priv->gpu_error.reload_in_reset = true;
 
@@ -870,7 +882,7 @@ int i915_reset(struct drm_device *dev)
 		 * of re-init after reset.
 		 */
 		if (INTEL_INFO(dev)->gen > 5)
-			intel_reset_gt_powersave(dev);
+			intel_enable_gt_powersave(dev);
 	} else {
 		mutex_unlock(&dev->struct_mutex);
 	}
@@ -923,15 +935,13 @@ static int i915_pm_suspend(struct device *dev)
 	if (drm_dev->switch_power_state == DRM_SWITCH_POWER_OFF)
 		return 0;
 
-	return i915_drm_freeze(drm_dev);
+	return i915_drm_suspend(drm_dev);
 }
 
 static int i915_pm_suspend_late(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
 	struct drm_device *drm_dev = pci_get_drvdata(pdev);
-	struct drm_i915_private *dev_priv = drm_dev->dev_private;
-	int ret;
 
 	/*
 	 * We have a suspedn ordering issue with the snd-hda driver also
@@ -945,16 +955,7 @@ static int i915_pm_suspend_late(struct device *dev)
 	if (drm_dev->switch_power_state == DRM_SWITCH_POWER_OFF)
 		return 0;
 
-	ret = intel_suspend_complete(dev_priv);
-
-	if (ret)
-		DRM_ERROR("Suspend complete failed: %d\n", ret);
-	else {
-		pci_disable_device(pdev);
-		pci_set_power_state(pdev, PCI_D3hot);
-	}
-
-	return ret;
+	return i915_drm_suspend_late(drm_dev);
 }
 
 static int i915_pm_resume_early(struct device *dev)
@@ -962,61 +963,21 @@ static int i915_pm_resume_early(struct device *dev)
 	struct pci_dev *pdev = to_pci_dev(dev);
 	struct drm_device *drm_dev = pci_get_drvdata(pdev);
 
-	return i915_resume_early(drm_dev);
-}
-
-static int i915_pm_resume(struct device *dev)
-{
-	struct pci_dev *pdev = to_pci_dev(dev);
-	struct drm_device *drm_dev = pci_get_drvdata(pdev);
-
-	return i915_resume(drm_dev);
-}
-
-static int i915_pm_freeze(struct device *dev)
-{
-	struct pci_dev *pdev = to_pci_dev(dev);
-	struct drm_device *drm_dev = pci_get_drvdata(pdev);
-
-	if (!drm_dev || !drm_dev->dev_private) {
-		dev_err(dev, "DRM not initialized, aborting suspend.\n");
-		return -ENODEV;
-	}
-
-	return i915_drm_freeze(drm_dev);
-}
-
-static int i915_pm_freeze_late(struct device *dev)
-{
-	struct pci_dev *pdev = to_pci_dev(dev);
-	struct drm_device *drm_dev = pci_get_drvdata(pdev);
-	struct drm_i915_private *dev_priv = drm_dev->dev_private;
-
-	return intel_suspend_complete(dev_priv);
-}
-
-static int i915_pm_thaw_early(struct device *dev)
-{
-	struct pci_dev *pdev = to_pci_dev(dev);
-	struct drm_device *drm_dev = pci_get_drvdata(pdev);
+	if (drm_dev->switch_power_state == DRM_SWITCH_POWER_OFF)
+		return 0;
 
-	return i915_drm_thaw_early(drm_dev);
+	return i915_drm_resume_early(drm_dev);
 }
 
-static int i915_pm_thaw(struct device *dev)
+static int i915_pm_resume(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
 	struct drm_device *drm_dev = pci_get_drvdata(pdev);
 
-	return i915_drm_thaw(drm_dev);
-}
-
-static int i915_pm_poweroff(struct device *dev)
-{
-	struct pci_dev *pdev = to_pci_dev(dev);
-	struct drm_device *drm_dev = pci_get_drvdata(pdev);
+	if (drm_dev->switch_power_state == DRM_SWITCH_POWER_OFF)
+		return 0;
 
-	return i915_drm_freeze(drm_dev);
+	return i915_drm_resume(drm_dev);
 }
 
 static int hsw_suspend_complete(struct drm_i915_private *dev_priv)
@@ -1026,25 +987,6 @@ static int hsw_suspend_complete(struct drm_i915_private *dev_priv)
 	return 0;
 }
 
-static int snb_resume_prepare(struct drm_i915_private *dev_priv,
-				bool rpm_resume)
-{
-	struct drm_device *dev = dev_priv->dev;
-
-	if (rpm_resume)
-		intel_init_pch_refclk(dev);
-
-	return 0;
-}
-
-static int hsw_resume_prepare(struct drm_i915_private *dev_priv,
-				bool rpm_resume)
-{
-	hsw_disable_pc8(dev_priv);
-
-	return 0;
-}
-
 /*
  * Save all Gunit registers that may be lost after a D3 and a subsequent
  * S0i[R123] transition. The list of registers needing a save/restore is
@@ -1449,18 +1391,13 @@ static int intel_runtime_suspend(struct device *device)
 	i915_gem_release_all_mmaps(dev_priv);
 	mutex_unlock(&dev->struct_mutex);
 
-	/*
-	 * rps.work can't be rearmed here, since we get here only after making
-	 * sure the GPU is idle and the RPS freq is set to the minimum. See
-	 * intel_mark_idle().
-	 */
-	cancel_work_sync(&dev_priv->rps.work);
-	intel_runtime_pm_disable_interrupts(dev);
+	intel_suspend_gt_powersave(dev);
+	intel_runtime_pm_disable_interrupts(dev_priv);
 
 	ret = intel_suspend_complete(dev_priv);
 	if (ret) {
 		DRM_ERROR("Runtime suspend failed, disabling it (%d)\n", ret);
-		intel_runtime_pm_restore_interrupts(dev);
+		intel_runtime_pm_enable_interrupts(dev_priv);
 
 		return ret;
 	}
@@ -1502,7 +1439,7 @@ static int intel_runtime_resume(struct device *device)
 	struct pci_dev *pdev = to_pci_dev(device);
 	struct drm_device *dev = pci_get_drvdata(pdev);
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	int ret;
+	int ret = 0;
 
 	if (WARN_ON_ONCE(!HAS_RUNTIME_PM(dev)))
 		return -ENODEV;
@@ -1512,7 +1449,13 @@ static int intel_runtime_resume(struct device *device)
 	intel_opregion_notify_adapter(dev, PCI_D0);
 	dev_priv->pm.suspended = false;
 
-	ret = intel_resume_prepare(dev_priv, true);
+	if (IS_GEN6(dev_priv))
+		intel_init_pch_refclk(dev);
+	else if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
+		hsw_disable_pc8(dev_priv);
+	else if (IS_VALLEYVIEW(dev_priv))
+		ret = vlv_resume_prepare(dev_priv, true);
+
 	/*
 	 * No point of rolling back things in case of an error, as the best
 	 * we can do is to hope that things will still work (and disable RPM).
@@ -1520,8 +1463,8 @@ static int intel_runtime_resume(struct device *device)
 	i915_gem_init_swizzling(dev);
 	gen6_update_ring_freq(dev);
 
-	intel_runtime_pm_restore_interrupts(dev);
-	intel_reset_gt_powersave(dev);
+	intel_runtime_pm_enable_interrupts(dev_priv);
+	intel_enable_gt_powersave(dev);
 
 	if (ret)
 		DRM_ERROR("Runtime resume failed, disabling it (%d)\n", ret);
@@ -1550,41 +1493,41 @@ static int intel_suspend_complete(struct drm_i915_private *dev_priv)
 	return ret;
 }
 
-/*
- * This function implements common functionality of runtime and system
- * resume sequence. Variable rpm_resume used for implementing different
- * code paths.
- */
-static int intel_resume_prepare(struct drm_i915_private *dev_priv,
-				bool rpm_resume)
-{
-	struct drm_device *dev = dev_priv->dev;
-	int ret;
-
-	if (IS_GEN6(dev))
-		ret = snb_resume_prepare(dev_priv, rpm_resume);
-	else if (IS_HASWELL(dev) || IS_BROADWELL(dev))
-		ret = hsw_resume_prepare(dev_priv, rpm_resume);
-	else if (IS_VALLEYVIEW(dev))
-		ret = vlv_resume_prepare(dev_priv, rpm_resume);
-	else
-		ret = 0;
-
-	return ret;
-}
-
 static const struct dev_pm_ops i915_pm_ops = {
+	/*
+	 * S0ix (via system suspend) and S3 event handlers [PMSG_SUSPEND,
+	 * PMSG_RESUME]
+	 */
 	.suspend = i915_pm_suspend,
 	.suspend_late = i915_pm_suspend_late,
 	.resume_early = i915_pm_resume_early,
 	.resume = i915_pm_resume,
-	.freeze = i915_pm_freeze,
-	.freeze_late = i915_pm_freeze_late,
-	.thaw_early = i915_pm_thaw_early,
-	.thaw = i915_pm_thaw,
-	.poweroff = i915_pm_poweroff,
+
+	/*
+	 * S4 event handlers
+	 * @freeze, @freeze_late    : called (1) before creating the
+	 *                            hibernation image [PMSG_FREEZE] and
+	 *                            (2) after rebooting, before restoring
+	 *                            the image [PMSG_QUIESCE]
+	 * @thaw, @thaw_early       : called (1) after creating the hibernation
+	 *                            image, before writing it [PMSG_THAW]
+	 *                            and (2) after failing to create or
+	 *                            restore the image [PMSG_RECOVER]
+	 * @poweroff, @poweroff_late: called after writing the hibernation
+	 *                            image, before rebooting [PMSG_HIBERNATE]
+	 * @restore, @restore_early : called after rebooting and restoring the
+	 *                            hibernation image [PMSG_RESTORE]
+	 */
+	.freeze = i915_pm_suspend,
+	.freeze_late = i915_pm_suspend_late,
+	.thaw_early = i915_pm_resume_early,
+	.thaw = i915_pm_resume,
+	.poweroff = i915_pm_suspend,
+	.poweroff_late = i915_pm_suspend_late,
 	.restore_early = i915_pm_resume_early,
 	.restore = i915_pm_resume,
+
+	/* S0ix (via runtime suspend) event handlers */
 	.runtime_suspend = intel_runtime_suspend,
 	.runtime_resume = intel_runtime_resume,
 };
@@ -1626,12 +1569,10 @@ static struct drm_driver driver = {
 	.set_busid = drm_pci_set_busid,
 
 	/* Used in place of i915_pm_ops for non-DRIVER_MODESET */
-	.suspend = i915_suspend,
+	.suspend = i915_suspend_legacy,
 	.resume = i915_resume_legacy,
 
 	.device_is_agp = i915_driver_device_is_agp,
-	.master_create = i915_master_create,
-	.master_destroy = i915_master_destroy,
 #if defined(CONFIG_DEBUG_FS)
 	.debugfs_init = i915_debugfs_init,
 	.debugfs_cleanup = i915_debugfs_cleanup,
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index 16a6f6d187a1..70d0f0f06f1a 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -55,7 +55,10 @@
 
 #define DRIVER_NAME		"i915"
 #define DRIVER_DESC		"Intel Graphics"
-#define DRIVER_DATE		"20140905"
+#define DRIVER_DATE		"20141121"
+
+#undef WARN_ON
+#define WARN_ON(x)		WARN(x, "WARN_ON(" #x ")")
 
 enum pipe {
 	INVALID_PIPE = -1,
@@ -76,6 +79,14 @@ enum transcoder {
 };
 #define transcoder_name(t) ((t) + 'A')
 
+/*
+ * This is the maximum (across all platforms) number of planes (primary +
+ * sprites) that can be active at the same time on one pipe.
+ *
+ * This value doesn't count the cursor plane.
+ */
+#define I915_MAX_PLANES	3
+
 enum plane {
 	PLANE_A = 0,
 	PLANE_B,
@@ -202,10 +213,15 @@ enum intel_dpll_id {
 	/* real shared dpll ids must be >= 0 */
 	DPLL_ID_PCH_PLL_A = 0,
 	DPLL_ID_PCH_PLL_B = 1,
+	/* hsw/bdw */
 	DPLL_ID_WRPLL1 = 0,
 	DPLL_ID_WRPLL2 = 1,
+	/* skl */
+	DPLL_ID_SKL_DPLL1 = 0,
+	DPLL_ID_SKL_DPLL2 = 1,
+	DPLL_ID_SKL_DPLL3 = 2,
 };
-#define I915_NUM_PLLS 2
+#define I915_NUM_PLLS 3
 
 struct intel_dpll_hw_state {
 	/* i9xx, pch plls */
@@ -216,16 +232,33 @@ struct intel_dpll_hw_state {
 
 	/* hsw, bdw */
 	uint32_t wrpll;
+
+	/* skl */
+	/*
+	 * DPLL_CTRL1 has 6 bits for each each this DPLL. We store those in
+	 * lower part of crtl1 and they get shifted into position when writing
+	 * the register.  This allows us to easily compare the state to share
+	 * the DPLL.
+	 */
+	uint32_t ctrl1;
+	/* HDMI only, 0 when used for DP */
+	uint32_t cfgcr1, cfgcr2;
+};
+
+struct intel_shared_dpll_config {
+	unsigned crtc_mask; /* mask of CRTCs sharing this PLL */
+	struct intel_dpll_hw_state hw_state;
 };
 
 struct intel_shared_dpll {
-	int refcount; /* count of number of CRTCs sharing this PLL */
+	struct intel_shared_dpll_config config;
+	struct intel_shared_dpll_config *new_config;
+
 	int active; /* count of number of active CRTCs (i.e. DPMS on) */
 	bool on; /* is the PLL actually active? Disabled during modeset */
 	const char *name;
 	/* should match the index in the dev_priv->shared_dplls array */
 	enum intel_dpll_id id;
-	struct intel_dpll_hw_state hw_state;
 	/* The mode_set hook is optional and should be used together with the
 	 * intel_prepare_shared_dpll function. */
 	void (*mode_set)(struct drm_i915_private *dev_priv,
@@ -239,6 +272,11 @@ struct intel_shared_dpll {
 			     struct intel_dpll_hw_state *hw_state);
 };
 
+#define SKL_DPLL0 0
+#define SKL_DPLL1 1
+#define SKL_DPLL2 2
+#define SKL_DPLL3 3
+
 /* Used by dp and fdi links */
 struct intel_link_m_n {
 	uint32_t	tu;
@@ -267,7 +305,6 @@ void intel_link_compute_m_n(int bpp, int nlanes,
 #define DRIVER_PATCHLEVEL	0
 
 #define WATCH_LISTS	0
-#define WATCH_GTT	0
 
 struct opregion_header;
 struct opregion_acpi;
@@ -290,12 +327,6 @@ struct intel_opregion {
 struct intel_overlay;
 struct intel_overlay_error_state;
 
-struct drm_local_map;
-
-struct drm_i915_master_private {
-	struct drm_local_map *sarea;
-	struct _drm_i915_sarea *sarea_priv;
-};
 #define I915_FENCE_REG_NONE -1
 #define I915_MAX_NUM_FENCES 32
 /* 32 fences + sign bit for FENCE_REG_NONE */
@@ -426,6 +457,7 @@ struct drm_i915_error_state {
 };
 
 struct intel_connector;
+struct intel_encoder;
 struct intel_crtc_config;
 struct intel_plane_config;
 struct intel_crtc;
@@ -452,7 +484,7 @@ struct drm_i915_display_funcs {
 	 * Returns true on success, false on failure.
 	 */
 	bool (*find_dpll)(const struct intel_limit *limit,
-			  struct drm_crtc *crtc,
+			  struct intel_crtc *crtc,
 			  int target, int refclk,
 			  struct dpll *match_clock,
 			  struct dpll *best_clock);
@@ -468,15 +500,14 @@ struct drm_i915_display_funcs {
 				struct intel_crtc_config *);
 	void (*get_plane_config)(struct intel_crtc *,
 				 struct intel_plane_config *);
-	int (*crtc_mode_set)(struct drm_crtc *crtc,
-			     int x, int y,
-			     struct drm_framebuffer *old_fb);
+	int (*crtc_compute_clock)(struct intel_crtc *crtc);
 	void (*crtc_enable)(struct drm_crtc *crtc);
 	void (*crtc_disable)(struct drm_crtc *crtc);
 	void (*off)(struct drm_crtc *crtc);
-	void (*write_eld)(struct drm_connector *connector,
-			  struct drm_crtc *crtc,
-			  struct drm_display_mode *mode);
+	void (*audio_codec_enable)(struct drm_connector *connector,
+				   struct intel_encoder *encoder,
+				   struct drm_display_mode *mode);
+	void (*audio_codec_disable)(struct intel_encoder *encoder);
 	void (*fdi_link_train)(struct drm_crtc *crtc);
 	void (*init_clock_gating)(struct drm_device *dev);
 	int (*queue_flip)(struct drm_device *dev, struct drm_crtc *crtc,
@@ -494,7 +525,7 @@ struct drm_i915_display_funcs {
 	/* display clock increase/decrease */
 	/* pll clock increase/decrease */
 
-	int (*setup_backlight)(struct intel_connector *connector);
+	int (*setup_backlight)(struct intel_connector *connector, enum pipe pipe);
 	uint32_t (*get_backlight)(struct intel_connector *connector);
 	void (*set_backlight)(struct intel_connector *connector,
 			      uint32_t level);
@@ -533,6 +564,7 @@ struct intel_uncore {
 
 	unsigned fw_rendercount;
 	unsigned fw_mediacount;
+	unsigned fw_blittercount;
 
 	struct timer_list force_wake_timer;
 };
@@ -551,6 +583,7 @@ struct intel_uncore {
 	func(is_ivybridge) sep \
 	func(is_valleyview) sep \
 	func(is_haswell) sep \
+	func(is_skylake) sep \
 	func(is_preliminary) sep \
 	func(has_fbc) sep \
 	func(has_pipe_cxsr) sep \
@@ -646,6 +679,7 @@ struct intel_context {
 	struct {
 		struct drm_i915_gem_object *state;
 		struct intel_ringbuffer *ringbuf;
+		int unpin_count;
 	} engine[I915_NUM_RINGS];
 
 	struct list_head link;
@@ -663,6 +697,18 @@ struct i915_fbc {
 
 	bool false_color;
 
+	/* Tracks whether the HW is actually enabled, not whether the feature is
+	 * possible. */
+	bool enabled;
+
+	/* On gen8 some rings cannont perform fbc clean operation so for now
+	 * we are doing this on SW with mmio.
+	 * This variable works in the opposite information direction
+	 * of ring->fbc_dirty telling software on frontbuffer tracking
+	 * to perform the cache clean on sw side.
+	 */
+	bool need_sw_cache_clean;
+
 	struct intel_fbc_work {
 		struct delayed_work work;
 		struct drm_crtc *crtc;
@@ -704,6 +750,7 @@ enum intel_pch {
 	PCH_IBX,	/* Ibexpeak PCH */
 	PCH_CPT,	/* Cougarpoint PCH */
 	PCH_LPT,	/* Lynxpoint PCH */
+	PCH_SPT,        /* Sunrisepoint PCH */
 	PCH_NOP,
 };
 
@@ -717,6 +764,7 @@ enum intel_sbi_destination {
 #define QUIRK_INVERT_BRIGHTNESS (1<<2)
 #define QUIRK_BACKLIGHT_PRESENT (1<<3)
 #define QUIRK_PIPEB_FORCE (1<<4)
+#define QUIRK_PIN_SWIZZLED_PAGES (1<<5)
 
 struct intel_fbdev;
 struct intel_fbc_work;
@@ -768,7 +816,6 @@ struct i915_suspend_saved_registers {
 	u32 saveBLC_HIST_CTL;
 	u32 saveBLC_PWM_CTL;
 	u32 saveBLC_PWM_CTL2;
-	u32 saveBLC_HIST_CTL_B;
 	u32 saveBLC_CPU_PWM_CTL;
 	u32 saveBLC_CPU_PWM_CTL2;
 	u32 saveFPB0;
@@ -877,6 +924,7 @@ struct i915_suspend_saved_registers {
 	u32 savePIPEB_LINK_N1;
 	u32 saveMCHBAR_RENDER_STANDBY;
 	u32 savePCH_PORT_HOTPLUG;
+	u16 saveGCDGMBUS;
 };
 
 struct vlv_s0ix_state {
@@ -947,8 +995,12 @@ struct intel_rps_ei {
 };
 
 struct intel_gen6_power_mgmt {
-	/* work and pm_iir are protected by dev_priv->irq_lock */
+	/*
+	 * work, interrupts_enabled and pm_iir are protected by
+	 * dev_priv->irq_lock
+	 */
 	struct work_struct work;
+	bool interrupts_enabled;
 	u32 pm_iir;
 
 	/* Frequencies are stored in potentially platform dependent multiples.
@@ -1071,31 +1123,6 @@ struct i915_power_domains {
 	struct i915_power_well *power_wells;
 };
 
-struct i915_dri1_state {
-	unsigned allow_batchbuffer : 1;
-	u32 __iomem *gfx_hws_cpu_addr;
-
-	unsigned int cpp;
-	int back_offset;
-	int front_offset;
-	int current_page;
-	int page_flipping;
-
-	uint32_t counter;
-};
-
-struct i915_ums_state {
-	/**
-	 * Flag if the X Server, and thus DRM, is not currently in
-	 * control of the device.
-	 *
-	 * This is set between LeaveVT and EnterVT.  It needs to be
-	 * replaced with a semaphore.  It also needs to be
-	 * transitioned away from for kernel modesetting.
-	 */
-	int mm_suspended;
-};
-
 #define MAX_L3_SLICES 2
 struct intel_l3_parity {
 	u32 *remap_info[MAX_L3_SLICES];
@@ -1357,6 +1384,49 @@ struct ilk_wm_values {
 	enum intel_ddb_partitioning partitioning;
 };
 
+struct skl_ddb_entry {
+	uint16_t start, end;	/* in number of blocks, 'end' is exclusive */
+};
+
+static inline uint16_t skl_ddb_entry_size(const struct skl_ddb_entry *entry)
+{
+	return entry->end - entry->start;
+}
+
+static inline bool skl_ddb_entry_equal(const struct skl_ddb_entry *e1,
+				       const struct skl_ddb_entry *e2)
+{
+	if (e1->start == e2->start && e1->end == e2->end)
+		return true;
+
+	return false;
+}
+
+struct skl_ddb_allocation {
+	struct skl_ddb_entry pipe[I915_MAX_PIPES];
+	struct skl_ddb_entry plane[I915_MAX_PIPES][I915_MAX_PLANES];
+	struct skl_ddb_entry cursor[I915_MAX_PIPES];
+};
+
+struct skl_wm_values {
+	bool dirty[I915_MAX_PIPES];
+	struct skl_ddb_allocation ddb;
+	uint32_t wm_linetime[I915_MAX_PIPES];
+	uint32_t plane[I915_MAX_PIPES][I915_MAX_PLANES][8];
+	uint32_t cursor[I915_MAX_PIPES][8];
+	uint32_t plane_trans[I915_MAX_PIPES][I915_MAX_PLANES];
+	uint32_t cursor_trans[I915_MAX_PIPES];
+};
+
+struct skl_wm_level {
+	bool plane_en[I915_MAX_PLANES];
+	bool cursor_en;
+	uint16_t plane_res_b[I915_MAX_PLANES];
+	uint8_t plane_res_l[I915_MAX_PLANES];
+	uint16_t cursor_res_b;
+	uint8_t cursor_res_l;
+};
+
 /*
  * This struct helps tracking the state needed for runtime PM, which puts the
  * device in PCI D3 state. Notice that when this happens, nothing on the
@@ -1369,7 +1439,7 @@ struct ilk_wm_values {
  *
  * Our driver uses the autosuspend delay feature, which means we'll only really
  * suspend if we stay with zero refcount for a certain amount of time. The
- * default value is currently very conservative (see intel_init_runtime_pm), but
+ * default value is currently very conservative (see intel_runtime_pm_enable), but
  * it can be changed with the standard runtime PM files from sysfs.
  *
  * The irqs_disabled variable becomes true exactly after we disable the IRQs and
@@ -1382,7 +1452,7 @@ struct ilk_wm_values {
  */
 struct i915_runtime_pm {
 	bool suspended;
-	bool _irqs_disabled;
+	bool irqs_enabled;
 };
 
 enum intel_pipe_crc_source {
@@ -1426,6 +1496,20 @@ struct i915_frontbuffer_tracking {
 	unsigned flip_bits;
 };
 
+struct i915_wa_reg {
+	u32 addr;
+	u32 value;
+	/* bitmask representing WA bits */
+	u32 mask;
+};
+
+#define I915_MAX_WA_REGS 16
+
+struct i915_workarounds {
+	struct i915_wa_reg reg[I915_MAX_WA_REGS];
+	u32 count;
+};
+
 struct drm_i915_private {
 	struct drm_device *dev;
 	struct kmem_cache *slab;
@@ -1505,11 +1589,13 @@ struct drm_i915_private {
 	struct intel_opregion opregion;
 	struct intel_vbt_data vbt;
 
+	bool preserve_bios_swizzle;
+
 	/* overlay */
 	struct intel_overlay *overlay;
 
 	/* backlight registers and fields in struct intel_panel */
-	spinlock_t backlight_lock;
+	struct mutex backlight_lock;
 
 	/* LVDS info */
 	bool no_aux_handshake;
@@ -1523,6 +1609,7 @@ struct drm_i915_private {
 
 	unsigned int fsb_freq, mem_freq, is_ddr3;
 	unsigned int vlv_cdclk_freq;
+	unsigned int hpll_freq;
 
 	/**
 	 * wq - Driver workqueue for GEM.
@@ -1568,19 +1655,7 @@ struct drm_i915_private {
 	struct intel_shared_dpll shared_dplls[I915_NUM_PLLS];
 	int dpio_phy_iosf_port[I915_NUM_PHYS_VLV];
 
-	/*
-	 * workarounds are currently applied at different places and
-	 * changes are being done to consolidate them so exact count is
-	 * not clear at this point, use a max value for now.
-	 */
-#define I915_MAX_WA_REGS  16
-	struct {
-		u32 addr;
-		u32 value;
-		/* bitmask representing WA bits */
-		u32 mask;
-	} intel_wa_regs[I915_MAX_WA_REGS];
-	u32 num_wa_regs;
+	struct i915_workarounds workarounds;
 
 	/* Reclocking support */
 	bool render_reclock_avail;
@@ -1644,9 +1719,25 @@ struct drm_i915_private {
 		uint16_t spr_latency[5];
 		/* cursor */
 		uint16_t cur_latency[5];
+		/*
+		 * Raw watermark memory latency values
+		 * for SKL for all 8 levels
+		 * in 1us units.
+		 */
+		uint16_t skl_latency[8];
+
+		/*
+		 * The skl_wm_values structure is a bit too big for stack
+		 * allocation, so we keep the staging struct where we store
+		 * intermediate results here instead.
+		 */
+		struct skl_wm_values skl_results;
 
 		/* current hardware state */
-		struct ilk_wm_values hw;
+		union {
+			struct ilk_wm_values hw;
+			struct skl_wm_values skl_hw;
+		};
 	} wm;
 
 	struct i915_runtime_pm pm;
@@ -1667,12 +1758,6 @@ struct drm_i915_private {
 
 	uint32_t bios_vgacntr;
 
-	/* Old dri1 support infrastructure, beware the dragons ya fools entering
-	 * here! */
-	struct i915_dri1_state dri1;
-	/* Old ums support infrastructure, same warning applies. */
-	struct i915_ums_state ums;
-
 	/* Abstract the submission mechanism (legacy ringbuffer or execlists) away */
 	struct {
 		int (*do_execbuf)(struct drm_device *dev, struct drm_file *file,
@@ -1830,8 +1915,6 @@ struct drm_i915_gem_object {
 	unsigned long gt_ro:1;
 	unsigned int cache_level:3;
 
-	unsigned int has_aliasing_ppgtt_mapping:1;
-	unsigned int has_global_gtt_mapping:1;
 	unsigned int has_dma_mapping:1;
 
 	unsigned int frontbuffer_bits:INTEL_FRONTBUFFER_BITS;
@@ -1864,10 +1947,10 @@ struct drm_i915_gem_object {
 	unsigned long user_pin_count;
 	struct drm_file *pin_filp;
 
-	/** for phy allocated objects */
-	struct drm_dma_handle *phys_handle;
-
 	union {
+		/** for phy allocated objects */
+		struct drm_dma_handle *phys_handle;
+
 		struct i915_gem_userptr {
 			uintptr_t ptr;
 			unsigned read_only :1;
@@ -2073,6 +2156,7 @@ struct drm_i915_cmd_table {
 #define IS_CHERRYVIEW(dev)	(INTEL_INFO(dev)->is_valleyview && IS_GEN8(dev))
 #define IS_HASWELL(dev)	(INTEL_INFO(dev)->is_haswell)
 #define IS_BROADWELL(dev)	(!INTEL_INFO(dev)->is_valleyview && IS_GEN8(dev))
+#define IS_SKYLAKE(dev)	(INTEL_INFO(dev)->is_skylake)
 #define IS_MOBILE(dev)		(INTEL_INFO(dev)->is_mobile)
 #define IS_HSW_EARLY_SDV(dev)	(IS_HASWELL(dev) && \
 				 (INTEL_DEVID(dev) & 0xFF00) == 0x0C00)
@@ -2080,9 +2164,10 @@ struct drm_i915_cmd_table {
 				 ((INTEL_DEVID(dev) & 0xf) == 0x2  || \
 				 (INTEL_DEVID(dev) & 0xf) == 0x6 || \
 				 (INTEL_DEVID(dev) & 0xf) == 0xe))
+#define IS_BDW_GT3(dev)		(IS_BROADWELL(dev) && \
+				 (INTEL_DEVID(dev) & 0x00F0) == 0x0020)
 #define IS_HSW_ULT(dev)		(IS_HASWELL(dev) && \
 				 (INTEL_DEVID(dev) & 0xFF00) == 0x0A00)
-#define IS_ULT(dev)		(IS_HSW_ULT(dev) || IS_BDW_ULT(dev))
 #define IS_HSW_GT3(dev)		(IS_HASWELL(dev) && \
 				 (INTEL_DEVID(dev) & 0x00F0) == 0x0020)
 /* ULX machines are also considered ULT. */
@@ -2103,6 +2188,7 @@ struct drm_i915_cmd_table {
 #define IS_GEN6(dev)	(INTEL_INFO(dev)->gen == 6)
 #define IS_GEN7(dev)	(INTEL_INFO(dev)->gen == 7)
 #define IS_GEN8(dev)	(INTEL_INFO(dev)->gen == 8)
+#define IS_GEN9(dev)	(INTEL_INFO(dev)->gen == 9)
 
 #define RENDER_RING		(1<<RCS)
 #define BSD_RING		(1<<VCS)
@@ -2115,13 +2201,11 @@ struct drm_i915_cmd_table {
 #define HAS_VEBOX(dev)		(INTEL_INFO(dev)->ring_mask & VEBOX_RING)
 #define HAS_LLC(dev)		(INTEL_INFO(dev)->has_llc)
 #define HAS_WT(dev)		((IS_HASWELL(dev) || IS_BROADWELL(dev)) && \
-				 to_i915(dev)->ellc_size)
+				 __I915__(dev)->ellc_size)
 #define I915_NEED_GFX_HWS(dev)	(INTEL_INFO(dev)->need_gfx_hws)
 
 #define HAS_HW_CONTEXTS(dev)	(INTEL_INFO(dev)->gen >= 6)
 #define HAS_LOGICAL_RING_CONTEXTS(dev)	(INTEL_INFO(dev)->gen >= 8)
-#define HAS_ALIASING_PPGTT(dev)	(INTEL_INFO(dev)->gen >= 6)
-#define HAS_PPGTT(dev)		(INTEL_INFO(dev)->gen >= 7 && !IS_GEN8(dev))
 #define USES_PPGTT(dev)		(i915.enable_ppgtt)
 #define USES_FULL_PPGTT(dev)	(i915.enable_ppgtt == 2)
 
@@ -2154,13 +2238,15 @@ struct drm_i915_cmd_table {
 #define HAS_PIPE_CXSR(dev) (INTEL_INFO(dev)->has_pipe_cxsr)
 #define HAS_FBC(dev) (INTEL_INFO(dev)->has_fbc)
 
-#define HAS_IPS(dev)		(IS_ULT(dev) || IS_BROADWELL(dev))
+#define HAS_IPS(dev)		(IS_HSW_ULT(dev) || IS_BROADWELL(dev))
 
 #define HAS_DDI(dev)		(INTEL_INFO(dev)->has_ddi)
 #define HAS_FPGA_DBG_UNCLAIMED(dev)	(INTEL_INFO(dev)->has_fpga_dbg)
 #define HAS_PSR(dev)		(IS_HASWELL(dev) || IS_BROADWELL(dev))
 #define HAS_RUNTIME_PM(dev)	(IS_GEN6(dev) || IS_HASWELL(dev) || \
 				 IS_BROADWELL(dev) || IS_VALLEYVIEW(dev))
+#define HAS_RC6(dev)		(INTEL_INFO(dev)->gen >= 6)
+#define HAS_RC6p(dev)		(INTEL_INFO(dev)->gen == 6 || IS_IVYBRIDGE(dev))
 
 #define INTEL_PCH_DEVICE_ID_MASK		0xff00
 #define INTEL_PCH_IBX_DEVICE_ID_TYPE		0x3b00
@@ -2168,8 +2254,11 @@ struct drm_i915_cmd_table {
 #define INTEL_PCH_PPT_DEVICE_ID_TYPE		0x1e00
 #define INTEL_PCH_LPT_DEVICE_ID_TYPE		0x8c00
 #define INTEL_PCH_LPT_LP_DEVICE_ID_TYPE		0x9c00
+#define INTEL_PCH_SPT_DEVICE_ID_TYPE		0xA100
+#define INTEL_PCH_SPT_LP_DEVICE_ID_TYPE		0x9D00
 
-#define INTEL_PCH_TYPE(dev) (to_i915(dev)->pch_type)
+#define INTEL_PCH_TYPE(dev) (__I915__(dev)->pch_type)
+#define HAS_PCH_SPT(dev) (INTEL_PCH_TYPE(dev) == PCH_SPT)
 #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)
@@ -2189,8 +2278,8 @@ struct drm_i915_cmd_table {
 extern const struct drm_ioctl_desc i915_ioctls[];
 extern int i915_max_ioctl;
 
-extern int i915_suspend(struct drm_device *dev, pm_message_t state);
-extern int i915_resume(struct drm_device *dev);
+extern int i915_suspend_legacy(struct drm_device *dev, pm_message_t state);
+extern int i915_resume_legacy(struct drm_device *dev);
 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);
 
@@ -2227,8 +2316,6 @@ struct i915_params {
 extern struct i915_params i915 __read_mostly;
 
 				/* 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 *);
 extern int i915_driver_open(struct drm_device *dev, struct drm_file *file);
@@ -2242,9 +2329,6 @@ extern int i915_driver_device_is_agp(struct drm_device * dev);
 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 intel_gpu_reset(struct drm_device *dev);
 extern int i915_reset(struct drm_device *dev);
 extern unsigned long i915_chipset_val(struct drm_i915_private *dev_priv);
@@ -2260,10 +2344,10 @@ __printf(3, 4)
 void i915_handle_error(struct drm_device *dev, bool wedged,
 		       const char *fmt, ...);
 
-void gen6_set_pm_mask(struct drm_i915_private *dev_priv, u32 pm_iir,
-							int new_delay);
-extern void intel_irq_init(struct drm_device *dev);
-extern void intel_hpd_init(struct drm_device *dev);
+extern void intel_irq_init(struct drm_i915_private *dev_priv);
+extern void intel_hpd_init(struct drm_i915_private *dev_priv);
+int intel_irq_install(struct drm_i915_private *dev_priv);
+void intel_irq_uninstall(struct drm_i915_private *dev_priv);
 
 extern void intel_uncore_sanitize(struct drm_device *dev);
 extern void intel_uncore_early_sanitize(struct drm_device *dev,
@@ -2283,10 +2367,19 @@ i915_disable_pipestat(struct drm_i915_private *dev_priv, enum pipe pipe,
 
 void valleyview_enable_display_irqs(struct drm_i915_private *dev_priv);
 void valleyview_disable_display_irqs(struct drm_i915_private *dev_priv);
+void
+ironlake_enable_display_irq(struct drm_i915_private *dev_priv, u32 mask);
+void
+ironlake_disable_display_irq(struct drm_i915_private *dev_priv, u32 mask);
+void ibx_display_interrupt_update(struct drm_i915_private *dev_priv,
+				  uint32_t interrupt_mask,
+				  uint32_t enabled_irq_mask);
+#define ibx_enable_display_interrupt(dev_priv, bits) \
+	ibx_display_interrupt_update((dev_priv), (bits), (bits))
+#define ibx_disable_display_interrupt(dev_priv, bits) \
+	ibx_display_interrupt_update((dev_priv), (bits), 0)
 
 /* i915_gem.c */
-int i915_gem_init_ioctl(struct drm_device *dev, void *data,
-			struct drm_file *file_priv);
 int i915_gem_create_ioctl(struct drm_device *dev, void *data,
 			  struct drm_file *file_priv);
 int i915_gem_pread_ioctl(struct drm_device *dev, void *data,
@@ -2333,10 +2426,6 @@ int i915_gem_throttle_ioctl(struct drm_device *dev, void *data,
 			    struct drm_file *file_priv);
 int i915_gem_madvise_ioctl(struct drm_device *dev, void *data,
 			   struct drm_file *file_priv);
-int i915_gem_entervt_ioctl(struct drm_device *dev, void *data,
-			   struct drm_file *file_priv);
-int i915_gem_leavevt_ioctl(struct drm_device *dev, void *data,
-			   struct drm_file *file_priv);
 int i915_gem_set_tiling(struct drm_device *dev, void *data,
 			struct drm_file *file_priv);
 int i915_gem_get_tiling(struct drm_device *dev, void *data,
@@ -2379,7 +2468,6 @@ int __must_check i915_vma_unbind(struct i915_vma *vma);
 int i915_gem_object_put_pages(struct drm_i915_gem_object *obj);
 void i915_gem_release_all_mmaps(struct drm_i915_private *dev_priv);
 void i915_gem_release_mmap(struct drm_i915_gem_object *obj);
-void i915_gem_lastclose(struct drm_device *dev);
 
 int i915_gem_obj_prepare_shmem_read(struct drm_i915_gem_object *obj,
 				    int *needs_clflush);
@@ -2486,6 +2574,11 @@ int __i915_add_request(struct intel_engine_cs *ring,
 		       u32 *seqno);
 #define i915_add_request(ring, seqno) \
 	__i915_add_request(ring, NULL, NULL, seqno)
+int __i915_wait_seqno(struct intel_engine_cs *ring, u32 seqno,
+			unsigned reset_counter,
+			bool interruptible,
+			s64 *timeout,
+			struct drm_i915_file_private *file_priv);
 int __must_check i915_wait_seqno(struct intel_engine_cs *ring,
 				 uint32_t seqno);
 int i915_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf);
@@ -2755,7 +2848,6 @@ static inline bool intel_gmbus_is_forced_bit(struct i2c_adapter *adapter)
 extern void intel_i2c_reset(struct drm_device *dev);
 
 /* intel_opregion.c */
-struct intel_encoder;
 #ifdef CONFIG_ACPI
 extern int intel_opregion_setup(struct drm_device *dev);
 extern void intel_opregion_init(struct drm_device *dev);
@@ -2793,7 +2885,6 @@ static inline void intel_unregister_dsm_handler(void) { return; }
 
 /* modesetting */
 extern void intel_modeset_init_hw(struct drm_device *dev);
-extern void intel_modeset_suspend_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);
@@ -2804,7 +2895,7 @@ extern void intel_modeset_setup_hw_state(struct drm_device *dev,
 extern void i915_redisable_vga(struct drm_device *dev);
 extern void i915_redisable_vga_power_on(struct drm_device *dev);
 extern bool intel_fbc_enabled(struct drm_device *dev);
-extern void gen8_fbc_sw_flush(struct drm_device *dev, u32 value);
+extern void bdw_fbc_sw_flush(struct drm_device *dev, u32 value);
 extern void intel_disable_fbc(struct drm_device *dev);
 extern bool ironlake_set_drps(struct drm_device *dev, u8 val);
 extern void intel_init_pch_refclk(struct drm_device *dev);
@@ -2842,8 +2933,8 @@ void gen6_gt_force_wake_get(struct drm_i915_private *dev_priv, int fw_engine);
 void gen6_gt_force_wake_put(struct drm_i915_private *dev_priv, int fw_engine);
 void assert_force_wake_inactive(struct drm_i915_private *dev_priv);
 
-int sandybridge_pcode_read(struct drm_i915_private *dev_priv, u8 mbox, u32 *val);
-int sandybridge_pcode_write(struct drm_i915_private *dev_priv, u8 mbox, u32 val);
+int sandybridge_pcode_read(struct drm_i915_private *dev_priv, u32 mbox, u32 *val);
+int sandybridge_pcode_write(struct drm_i915_private *dev_priv, u32 mbox, u32 val);
 
 /* intel_sideband.c */
 u32 vlv_punit_read(struct drm_i915_private *dev_priv, u8 addr);
@@ -2873,7 +2964,9 @@ int vlv_freq_opcode(struct drm_i915_private *dev_priv, int val);
 
 #define FORCEWAKE_RENDER	(1 << 0)
 #define FORCEWAKE_MEDIA		(1 << 1)
-#define FORCEWAKE_ALL		(FORCEWAKE_RENDER | FORCEWAKE_MEDIA)
+#define FORCEWAKE_BLITTER	(1 << 2)
+#define FORCEWAKE_ALL		(FORCEWAKE_RENDER | FORCEWAKE_MEDIA | \
+					FORCEWAKE_BLITTER)
 
 
 #define I915_READ8(reg)		dev_priv->uncore.funcs.mmio_readb(dev_priv, (reg), true)
@@ -2939,6 +3032,11 @@ static inline unsigned long msecs_to_jiffies_timeout(const unsigned int m)
 	return min_t(unsigned long, MAX_JIFFY_OFFSET, j + 1);
 }
 
+static inline unsigned long nsecs_to_jiffies_timeout(const u64 n)
+{
+        return min_t(u64, MAX_JIFFY_OFFSET, nsecs_to_jiffies64(n) + 1);
+}
+
 static inline unsigned long
 timespec_to_jiffies_timeout(const struct timespec *value)
 {
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index 28f91df2604d..52adcb680be3 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -160,33 +160,6 @@ i915_gem_object_is_inactive(struct drm_i915_gem_object *obj)
 }
 
 int
-i915_gem_init_ioctl(struct drm_device *dev, void *data,
-		    struct drm_file *file)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	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_setup_global_gtt(dev, args->gtt_start, args->gtt_end,
-				  args->gtt_end);
-	dev_priv->gtt.mappable_end = args->gtt_end;
-	mutex_unlock(&dev->struct_mutex);
-
-	return 0;
-}
-
-int
 i915_gem_get_aperture_ioctl(struct drm_device *dev, void *data,
 			    struct drm_file *file)
 {
@@ -208,40 +181,137 @@ i915_gem_get_aperture_ioctl(struct drm_device *dev, void *data,
 	return 0;
 }
 
-static void i915_gem_object_detach_phys(struct drm_i915_gem_object *obj)
+static int
+i915_gem_object_get_pages_phys(struct drm_i915_gem_object *obj)
 {
-	drm_dma_handle_t *phys = obj->phys_handle;
+	struct address_space *mapping = file_inode(obj->base.filp)->i_mapping;
+	char *vaddr = obj->phys_handle->vaddr;
+	struct sg_table *st;
+	struct scatterlist *sg;
+	int i;
 
-	if (!phys)
-		return;
+	if (WARN_ON(i915_gem_object_needs_bit17_swizzle(obj)))
+		return -EINVAL;
+
+	for (i = 0; i < obj->base.size / PAGE_SIZE; i++) {
+		struct page *page;
+		char *src;
+
+		page = shmem_read_mapping_page(mapping, i);
+		if (IS_ERR(page))
+			return PTR_ERR(page);
 
-	if (obj->madv == I915_MADV_WILLNEED) {
+		src = kmap_atomic(page);
+		memcpy(vaddr, src, PAGE_SIZE);
+		drm_clflush_virt_range(vaddr, PAGE_SIZE);
+		kunmap_atomic(src);
+
+		page_cache_release(page);
+		vaddr += PAGE_SIZE;
+	}
+
+	i915_gem_chipset_flush(obj->base.dev);
+
+	st = kmalloc(sizeof(*st), GFP_KERNEL);
+	if (st == NULL)
+		return -ENOMEM;
+
+	if (sg_alloc_table(st, 1, GFP_KERNEL)) {
+		kfree(st);
+		return -ENOMEM;
+	}
+
+	sg = st->sgl;
+	sg->offset = 0;
+	sg->length = obj->base.size;
+
+	sg_dma_address(sg) = obj->phys_handle->busaddr;
+	sg_dma_len(sg) = obj->base.size;
+
+	obj->pages = st;
+	obj->has_dma_mapping = true;
+	return 0;
+}
+
+static void
+i915_gem_object_put_pages_phys(struct drm_i915_gem_object *obj)
+{
+	int ret;
+
+	BUG_ON(obj->madv == __I915_MADV_PURGED);
+
+	ret = i915_gem_object_set_to_cpu_domain(obj, true);
+	if (ret) {
+		/* In the event of a disaster, abandon all caches and
+		 * hope for the best.
+		 */
+		WARN_ON(ret != -EIO);
+		obj->base.read_domains = obj->base.write_domain = I915_GEM_DOMAIN_CPU;
+	}
+
+	if (obj->madv == I915_MADV_DONTNEED)
+		obj->dirty = 0;
+
+	if (obj->dirty) {
 		struct address_space *mapping = file_inode(obj->base.filp)->i_mapping;
-		char *vaddr = phys->vaddr;
+		char *vaddr = obj->phys_handle->vaddr;
 		int i;
 
 		for (i = 0; i < obj->base.size / PAGE_SIZE; i++) {
-			struct page *page = shmem_read_mapping_page(mapping, i);
-			if (!IS_ERR(page)) {
-				char *dst = kmap_atomic(page);
-				memcpy(dst, vaddr, PAGE_SIZE);
-				drm_clflush_virt_range(dst, PAGE_SIZE);
-				kunmap_atomic(dst);
-
-				set_page_dirty(page);
+			struct page *page;
+			char *dst;
+
+			page = shmem_read_mapping_page(mapping, i);
+			if (IS_ERR(page))
+				continue;
+
+			dst = kmap_atomic(page);
+			drm_clflush_virt_range(vaddr, PAGE_SIZE);
+			memcpy(dst, vaddr, PAGE_SIZE);
+			kunmap_atomic(dst);
+
+			set_page_dirty(page);
+			if (obj->madv == I915_MADV_WILLNEED)
 				mark_page_accessed(page);
-				page_cache_release(page);
-			}
+			page_cache_release(page);
 			vaddr += PAGE_SIZE;
 		}
-		i915_gem_chipset_flush(obj->base.dev);
+		obj->dirty = 0;
 	}
 
-#ifdef CONFIG_X86
-	set_memory_wb((unsigned long)phys->vaddr, phys->size / PAGE_SIZE);
-#endif
-	drm_pci_free(obj->base.dev, phys);
-	obj->phys_handle = NULL;
+	sg_free_table(obj->pages);
+	kfree(obj->pages);
+
+	obj->has_dma_mapping = false;
+}
+
+static void
+i915_gem_object_release_phys(struct drm_i915_gem_object *obj)
+{
+	drm_pci_free(obj->base.dev, obj->phys_handle);
+}
+
+static const struct drm_i915_gem_object_ops i915_gem_phys_ops = {
+	.get_pages = i915_gem_object_get_pages_phys,
+	.put_pages = i915_gem_object_put_pages_phys,
+	.release = i915_gem_object_release_phys,
+};
+
+static int
+drop_pages(struct drm_i915_gem_object *obj)
+{
+	struct i915_vma *vma, *next;
+	int ret;
+
+	drm_gem_object_reference(&obj->base);
+	list_for_each_entry_safe(vma, next, &obj->vma_list, vma_link)
+		if (i915_vma_unbind(vma))
+			break;
+
+	ret = i915_gem_object_put_pages(obj);
+	drm_gem_object_unreference(&obj->base);
+
+	return ret;
 }
 
 int
@@ -249,9 +319,7 @@ i915_gem_object_attach_phys(struct drm_i915_gem_object *obj,
 			    int align)
 {
 	drm_dma_handle_t *phys;
-	struct address_space *mapping;
-	char *vaddr;
-	int i;
+	int ret;
 
 	if (obj->phys_handle) {
 		if ((unsigned long)obj->phys_handle->vaddr & (align -1))
@@ -266,41 +334,19 @@ i915_gem_object_attach_phys(struct drm_i915_gem_object *obj,
 	if (obj->base.filp == NULL)
 		return -EINVAL;
 
+	ret = drop_pages(obj);
+	if (ret)
+		return ret;
+
 	/* create a new object */
 	phys = drm_pci_alloc(obj->base.dev, obj->base.size, align);
 	if (!phys)
 		return -ENOMEM;
 
-	vaddr = phys->vaddr;
-#ifdef CONFIG_X86
-	set_memory_wc((unsigned long)vaddr, phys->size / PAGE_SIZE);
-#endif
-	mapping = file_inode(obj->base.filp)->i_mapping;
-	for (i = 0; i < obj->base.size / PAGE_SIZE; i++) {
-		struct page *page;
-		char *src;
-
-		page = shmem_read_mapping_page(mapping, i);
-		if (IS_ERR(page)) {
-#ifdef CONFIG_X86
-			set_memory_wb((unsigned long)phys->vaddr, phys->size / PAGE_SIZE);
-#endif
-			drm_pci_free(obj->base.dev, phys);
-			return PTR_ERR(page);
-		}
-
-		src = kmap_atomic(page);
-		memcpy(vaddr, src, PAGE_SIZE);
-		kunmap_atomic(src);
-
-		mark_page_accessed(page);
-		page_cache_release(page);
-
-		vaddr += PAGE_SIZE;
-	}
-
 	obj->phys_handle = phys;
-	return 0;
+	obj->ops = &i915_gem_phys_ops;
+
+	return i915_gem_object_get_pages(obj);
 }
 
 static int
@@ -311,6 +357,14 @@ i915_gem_phys_pwrite(struct drm_i915_gem_object *obj,
 	struct drm_device *dev = obj->base.dev;
 	void *vaddr = obj->phys_handle->vaddr + args->offset;
 	char __user *user_data = to_user_ptr(args->data_ptr);
+	int ret;
+
+	/* We manually control the domain here and pretend that it
+	 * remains coherent i.e. in the GTT domain, like shmem_pwrite.
+	 */
+	ret = i915_gem_object_wait_rendering(obj, false);
+	if (ret)
+		return ret;
 
 	if (__copy_from_user_inatomic_nocache(vaddr, user_data, args->size)) {
 		unsigned long unwritten;
@@ -326,6 +380,7 @@ i915_gem_phys_pwrite(struct drm_i915_gem_object *obj,
 			return -EFAULT;
 	}
 
+	drm_clflush_virt_range(vaddr, args->size);
 	i915_gem_chipset_flush(dev);
 	return 0;
 }
@@ -1046,11 +1101,6 @@ i915_gem_pwrite_ioctl(struct drm_device *dev, void *data,
 	 * pread/pwrite currently are reading and writing from the CPU
 	 * perspective, requiring manual detiling by the client.
 	 */
-	if (obj->phys_handle) {
-		ret = i915_gem_phys_pwrite(obj, args, file);
-		goto out;
-	}
-
 	if (obj->tiling_mode == I915_TILING_NONE &&
 	    obj->base.write_domain != I915_GEM_DOMAIN_CPU &&
 	    cpu_write_needs_clflush(obj)) {
@@ -1060,8 +1110,12 @@ i915_gem_pwrite_ioctl(struct drm_device *dev, void *data,
 		 * textures). Fallback to the shmem path in that case. */
 	}
 
-	if (ret == -EFAULT || ret == -ENOSPC)
-		ret = i915_gem_shmem_pwrite(dev, obj, args, file);
+	if (ret == -EFAULT || ret == -ENOSPC) {
+		if (obj->phys_handle)
+			ret = i915_gem_phys_pwrite(obj, args, file);
+		else
+			ret = i915_gem_shmem_pwrite(dev, obj, args, file);
+	}
 
 out:
 	drm_gem_object_unreference(&obj->base);
@@ -1134,7 +1188,7 @@ static bool can_wait_boost(struct drm_i915_file_private *file_priv)
 }
 
 /**
- * __wait_seqno - wait until execution of seqno has finished
+ * __i915_wait_seqno - wait until execution of seqno has finished
  * @ring: the ring expected to report seqno
  * @seqno: duh!
  * @reset_counter: reset sequence associated with the given seqno
@@ -1151,7 +1205,7 @@ static bool can_wait_boost(struct drm_i915_file_private *file_priv)
  * Returns 0 if the seqno was found within the alloted time. Else returns the
  * errno with remaining time filled in timeout argument.
  */
-static int __wait_seqno(struct intel_engine_cs *ring, u32 seqno,
+int __i915_wait_seqno(struct intel_engine_cs *ring, u32 seqno,
 			unsigned reset_counter,
 			bool interruptible,
 			s64 *timeout,
@@ -1171,7 +1225,8 @@ static int __wait_seqno(struct intel_engine_cs *ring, u32 seqno,
 	if (i915_seqno_passed(ring->get_seqno(ring, true), seqno))
 		return 0;
 
-	timeout_expire = timeout ? jiffies + nsecs_to_jiffies((u64)*timeout) : 0;
+	timeout_expire = timeout ?
+		jiffies + nsecs_to_jiffies_timeout((u64)*timeout) : 0;
 
 	if (INTEL_INFO(dev)->gen >= 6 && ring->id == RCS && can_wait_boost(file_priv)) {
 		gen6_rps_boost(dev_priv);
@@ -1247,6 +1302,16 @@ static int __wait_seqno(struct intel_engine_cs *ring, u32 seqno,
 		s64 tres = *timeout - (now - before);
 
 		*timeout = tres < 0 ? 0 : tres;
+
+		/*
+		 * Apparently ktime isn't accurate enough and occasionally has a
+		 * bit of mismatch in the jiffies<->nsecs<->ktime loop. So patch
+		 * things up to make the test happy. We allow up to 1 jiffy.
+		 *
+		 * This is a regrssion from the timespec->ktime conversion.
+		 */
+		if (ret == -ETIME && *timeout < jiffies_to_usecs(1)*1000)
+			*timeout = 0;
 	}
 
 	return ret;
@@ -1262,6 +1327,7 @@ i915_wait_seqno(struct intel_engine_cs *ring, uint32_t seqno)
 	struct drm_device *dev = ring->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	bool interruptible = dev_priv->mm.interruptible;
+	unsigned reset_counter;
 	int ret;
 
 	BUG_ON(!mutex_is_locked(&dev->struct_mutex));
@@ -1275,14 +1341,13 @@ i915_wait_seqno(struct intel_engine_cs *ring, uint32_t seqno)
 	if (ret)
 		return ret;
 
-	return __wait_seqno(ring, seqno,
-			    atomic_read(&dev_priv->gpu_error.reset_counter),
-			    interruptible, NULL, NULL);
+	reset_counter = atomic_read(&dev_priv->gpu_error.reset_counter);
+	return __i915_wait_seqno(ring, seqno, reset_counter, interruptible,
+				 NULL, NULL);
 }
 
 static int
-i915_gem_object_wait_rendering__tail(struct drm_i915_gem_object *obj,
-				     struct intel_engine_cs *ring)
+i915_gem_object_wait_rendering__tail(struct drm_i915_gem_object *obj)
 {
 	if (!obj->active)
 		return 0;
@@ -1319,7 +1384,7 @@ i915_gem_object_wait_rendering(struct drm_i915_gem_object *obj,
 	if (ret)
 		return ret;
 
-	return i915_gem_object_wait_rendering__tail(obj, ring);
+	return i915_gem_object_wait_rendering__tail(obj);
 }
 
 /* A nonblocking variant of the above wait. This is a highly dangerous routine
@@ -1354,12 +1419,13 @@ i915_gem_object_wait_rendering__nonblocking(struct drm_i915_gem_object *obj,
 
 	reset_counter = atomic_read(&dev_priv->gpu_error.reset_counter);
 	mutex_unlock(&dev->struct_mutex);
-	ret = __wait_seqno(ring, seqno, reset_counter, true, NULL, file_priv);
+	ret = __i915_wait_seqno(ring, seqno, reset_counter, true, NULL,
+				file_priv);
 	mutex_lock(&dev->struct_mutex);
 	if (ret)
 		return ret;
 
-	return i915_gem_object_wait_rendering__tail(obj, ring);
+	return i915_gem_object_wait_rendering__tail(obj);
 }
 
 /**
@@ -1466,6 +1532,16 @@ unlock:
  *
  * While the mapping holds a reference on the contents of the object, it doesn't
  * imply a ref on the object itself.
+ *
+ * IMPORTANT:
+ *
+ * DRM driver writers who look a this function as an example for how to do GEM
+ * mmap support, please don't implement mmap support like here. The modern way
+ * to implement DRM mmap support is with an mmap offset ioctl (like
+ * i915_gem_mmap_gtt) and then using the mmap syscall on the DRM fd directly.
+ * That way debug tooling like valgrind will understand what's going on, hiding
+ * the mmap call in a driver private ioctl will break that. The i915 driver only
+ * does cpu mmaps this way because we didn't know better.
  */
 int
 i915_gem_mmap_ioctl(struct drm_device *dev, void *data,
@@ -1945,7 +2021,14 @@ unsigned long
 i915_gem_shrink(struct drm_i915_private *dev_priv,
 		long target, unsigned flags)
 {
-	const bool purgeable_only = flags & I915_SHRINK_PURGEABLE;
+	const struct {
+		struct list_head *list;
+		unsigned int bit;
+	} phases[] = {
+		{ &dev_priv->mm.unbound_list, I915_SHRINK_UNBOUND },
+		{ &dev_priv->mm.bound_list, I915_SHRINK_BOUND },
+		{ NULL, 0 },
+	}, *phase;
 	unsigned long count = 0;
 
 	/*
@@ -1967,48 +2050,30 @@ i915_gem_shrink(struct drm_i915_private *dev_priv,
 	 * dev->struct_mutex and so we won't ever be able to observe an
 	 * object on the bound_list with a reference count equals 0.
 	 */
-	if (flags & I915_SHRINK_UNBOUND) {
+	for (phase = phases; phase->list; phase++) {
 		struct list_head still_in_list;
 
-		INIT_LIST_HEAD(&still_in_list);
-		while (count < target && !list_empty(&dev_priv->mm.unbound_list)) {
-			struct drm_i915_gem_object *obj;
-
-			obj = list_first_entry(&dev_priv->mm.unbound_list,
-					       typeof(*obj), global_list);
-			list_move_tail(&obj->global_list, &still_in_list);
-
-			if (!i915_gem_object_is_purgeable(obj) && purgeable_only)
-				continue;
-
-			drm_gem_object_reference(&obj->base);
-
-			if (i915_gem_object_put_pages(obj) == 0)
-				count += obj->base.size >> PAGE_SHIFT;
-
-			drm_gem_object_unreference(&obj->base);
-		}
-		list_splice(&still_in_list, &dev_priv->mm.unbound_list);
-	}
-
-	if (flags & I915_SHRINK_BOUND) {
-		struct list_head still_in_list;
+		if ((flags & phase->bit) == 0)
+			continue;
 
 		INIT_LIST_HEAD(&still_in_list);
-		while (count < target && !list_empty(&dev_priv->mm.bound_list)) {
+		while (count < target && !list_empty(phase->list)) {
 			struct drm_i915_gem_object *obj;
 			struct i915_vma *vma, *v;
 
-			obj = list_first_entry(&dev_priv->mm.bound_list,
+			obj = list_first_entry(phase->list,
 					       typeof(*obj), global_list);
 			list_move_tail(&obj->global_list, &still_in_list);
 
-			if (!i915_gem_object_is_purgeable(obj) && purgeable_only)
+			if (flags & I915_SHRINK_PURGEABLE &&
+			    !i915_gem_object_is_purgeable(obj))
 				continue;
 
 			drm_gem_object_reference(&obj->base);
 
-			list_for_each_entry_safe(vma, v, &obj->vma_list, vma_link)
+			/* For the unbound phase, this should be a no-op! */
+			list_for_each_entry_safe(vma, v,
+						 &obj->vma_list, vma_link)
 				if (i915_vma_unbind(vma))
 					break;
 
@@ -2017,7 +2082,7 @@ i915_gem_shrink(struct drm_i915_private *dev_priv,
 
 			drm_gem_object_unreference(&obj->base);
 		}
-		list_splice(&still_in_list, &dev_priv->mm.bound_list);
+		list_splice(&still_in_list, phase->list);
 	}
 
 	return count;
@@ -2122,6 +2187,10 @@ i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj)
 	if (i915_gem_object_needs_bit17_swizzle(obj))
 		i915_gem_object_do_bit_17_swizzle(obj);
 
+	if (obj->tiling_mode != I915_TILING_NONE &&
+	    dev_priv->quirks & QUIRK_PIN_SWIZZLED_PAGES)
+		i915_gem_object_pin_pages(obj);
+
 	return 0;
 
 err_pages:
@@ -2420,15 +2489,13 @@ int __i915_add_request(struct intel_engine_cs *ring,
 	ring->outstanding_lazy_seqno = 0;
 	ring->preallocated_lazy_request = NULL;
 
-	if (!dev_priv->ums.mm_suspended) {
-		i915_queue_hangcheck(ring->dev);
+	i915_queue_hangcheck(ring->dev);
 
-		cancel_delayed_work_sync(&dev_priv->mm.idle_work);
-		queue_delayed_work(dev_priv->wq,
-				   &dev_priv->mm.retire_work,
-				   round_jiffies_up_relative(HZ));
-		intel_mark_busy(dev_priv->dev);
-	}
+	cancel_delayed_work_sync(&dev_priv->mm.idle_work);
+	queue_delayed_work(dev_priv->wq,
+			   &dev_priv->mm.retire_work,
+			   round_jiffies_up_relative(HZ));
+	intel_mark_busy(dev_priv->dev);
 
 	if (out_seqno)
 		*out_seqno = request->seqno;
@@ -2495,12 +2562,20 @@ static void i915_set_reset_status(struct drm_i915_private *dev_priv,
 
 static void i915_gem_free_request(struct drm_i915_gem_request *request)
 {
+	struct intel_context *ctx = request->ctx;
+
 	list_del(&request->list);
 	i915_gem_request_remove_from_client(request);
 
-	if (request->ctx)
-		i915_gem_context_unreference(request->ctx);
+	if (ctx) {
+		if (i915.enable_execlists) {
+			struct intel_engine_cs *ring = request->ring;
 
+			if (ctx != ring->default_context)
+				intel_lr_context_unpin(ring, ctx);
+		}
+		i915_gem_context_unreference(ctx);
+	}
 	kfree(request);
 }
 
@@ -2555,6 +2630,23 @@ static void i915_gem_reset_ring_cleanup(struct drm_i915_private *dev_priv,
 	}
 
 	/*
+	 * Clear the execlists queue up before freeing the requests, as those
+	 * are the ones that keep the context and ringbuffer backing objects
+	 * pinned in place.
+	 */
+	while (!list_empty(&ring->execlist_queue)) {
+		struct intel_ctx_submit_request *submit_req;
+
+		submit_req = list_first_entry(&ring->execlist_queue,
+				struct intel_ctx_submit_request,
+				execlist_link);
+		list_del(&submit_req->execlist_link);
+		intel_runtime_pm_put(dev_priv);
+		i915_gem_context_unreference(submit_req->ctx);
+		kfree(submit_req);
+	}
+
+	/*
 	 * We must free the requests after all the corresponding objects have
 	 * been moved off active lists. Which is the same order as the normal
 	 * retire_requests function does. This is important if object hold
@@ -2571,18 +2663,6 @@ static void i915_gem_reset_ring_cleanup(struct drm_i915_private *dev_priv,
 		i915_gem_free_request(request);
 	}
 
-	while (!list_empty(&ring->execlist_queue)) {
-		struct intel_ctx_submit_request *submit_req;
-
-		submit_req = list_first_entry(&ring->execlist_queue,
-				struct intel_ctx_submit_request,
-				execlist_link);
-		list_del(&submit_req->execlist_link);
-		intel_runtime_pm_put(dev_priv);
-		i915_gem_context_unreference(submit_req->ctx);
-		kfree(submit_req);
-	}
-
 	/* These may not have been flush before the reset, do so now */
 	kfree(ring->preallocated_lazy_request);
 	ring->preallocated_lazy_request = NULL;
@@ -2719,6 +2799,15 @@ i915_gem_retire_requests(struct drm_device *dev)
 	for_each_ring(ring, dev_priv, i) {
 		i915_gem_retire_requests_ring(ring);
 		idle &= list_empty(&ring->request_list);
+		if (i915.enable_execlists) {
+			unsigned long flags;
+
+			spin_lock_irqsave(&ring->execlist_lock, flags);
+			idle &= list_empty(&ring->execlist_queue);
+			spin_unlock_irqrestore(&ring->execlist_lock, flags);
+
+			intel_execlists_retire_requests(ring);
+		}
 	}
 
 	if (idle)
@@ -2811,6 +2900,9 @@ i915_gem_wait_ioctl(struct drm_device *dev, void *data, struct drm_file *file)
 	u32 seqno = 0;
 	int ret = 0;
 
+	if (args->flags != 0)
+		return -EINVAL;
+
 	ret = i915_mutex_lock_interruptible(dev);
 	if (ret)
 		return ret;
@@ -2846,8 +2938,8 @@ i915_gem_wait_ioctl(struct drm_device *dev, void *data, struct drm_file *file)
 	reset_counter = atomic_read(&dev_priv->gpu_error.reset_counter);
 	mutex_unlock(&dev->struct_mutex);
 
-	return __wait_seqno(ring, seqno, reset_counter, true, &args->timeout_ns,
-			    file->driver_priv);
+	return __i915_wait_seqno(ring, seqno, reset_counter, true,
+				 &args->timeout_ns, file->driver_priv);
 
 out:
 	drm_gem_object_unreference(&obj->base);
@@ -3166,6 +3258,7 @@ static void i915_gem_write_fence(struct drm_device *dev, int reg,
 	     obj->stride, obj->tiling_mode);
 
 	switch (INTEL_INFO(dev)->gen) {
+	case 9:
 	case 8:
 	case 7:
 	case 6:
@@ -3384,46 +3477,6 @@ static bool i915_gem_valid_gtt_space(struct i915_vma *vma,
 	return true;
 }
 
-static void i915_gem_verify_gtt(struct drm_device *dev)
-{
-#if WATCH_GTT
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_i915_gem_object *obj;
-	int err = 0;
-
-	list_for_each_entry(obj, &dev_priv->mm.gtt_list, global_list) {
-		if (obj->gtt_space == NULL) {
-			printk(KERN_ERR "object found on GTT list with no space reserved\n");
-			err++;
-			continue;
-		}
-
-		if (obj->cache_level != obj->gtt_space->color) {
-			printk(KERN_ERR "object reserved space [%08lx, %08lx] with wrong color, cache_level=%x, color=%lx\n",
-			       i915_gem_obj_ggtt_offset(obj),
-			       i915_gem_obj_ggtt_offset(obj) + i915_gem_obj_ggtt_size(obj),
-			       obj->cache_level,
-			       obj->gtt_space->color);
-			err++;
-			continue;
-		}
-
-		if (!i915_gem_valid_gtt_space(dev,
-					      obj->gtt_space,
-					      obj->cache_level)) {
-			printk(KERN_ERR "invalid GTT space found at [%08lx, %08lx] - color=%x\n",
-			       i915_gem_obj_ggtt_offset(obj),
-			       i915_gem_obj_ggtt_offset(obj) + i915_gem_obj_ggtt_size(obj),
-			       obj->cache_level);
-			err++;
-			continue;
-		}
-	}
-
-	WARN_ON(err);
-#endif
-}
-
 /**
  * Finds free space in the GTT aperture and binds the object there.
  */
@@ -3514,25 +3567,10 @@ search_free:
 	list_move_tail(&obj->global_list, &dev_priv->mm.bound_list);
 	list_add_tail(&vma->mm_list, &vm->inactive_list);
 
-	if (i915_is_ggtt(vm)) {
-		bool mappable, fenceable;
-
-		fenceable = (vma->node.size == fence_size &&
-			     (vma->node.start & (fence_alignment - 1)) == 0);
-
-		mappable = (vma->node.start + obj->base.size <=
-			    dev_priv->gtt.mappable_end);
-
-		obj->map_and_fenceable = mappable && fenceable;
-	}
-
-	WARN_ON(flags & PIN_MAPPABLE && !obj->map_and_fenceable);
-
 	trace_i915_vma_bind(vma, flags);
 	vma->bind_vma(vma, obj->cache_level,
-		      flags & (PIN_MAPPABLE | PIN_GLOBAL) ? GLOBAL_BIND : 0);
+		      flags & PIN_GLOBAL ? GLOBAL_BIND : 0);
 
-	i915_gem_verify_gtt(dev);
 	return vma;
 
 err_remove_node:
@@ -3560,7 +3598,7 @@ i915_gem_clflush_object(struct drm_i915_gem_object *obj,
 	 * Stolen memory is always coherent with the GPU as it is explicitly
 	 * marked as wc by the system, or the system is cache-coherent.
 	 */
-	if (obj->stolen)
+	if (obj->stolen || obj->phys_handle)
 		return false;
 
 	/* If the GPU is snooping the contents of the CPU cache,
@@ -3739,7 +3777,7 @@ int i915_gem_object_set_cache_level(struct drm_i915_gem_object *obj,
 		list_for_each_entry(vma, &obj->vma_list, vma_link)
 			if (drm_mm_node_allocated(&vma->node))
 				vma->bind_vma(vma, cache_level,
-					      obj->has_global_gtt_mapping ? GLOBAL_BIND : 0);
+						vma->bound & GLOBAL_BIND);
 	}
 
 	list_for_each_entry(vma, &obj->vma_list, vma_link)
@@ -3769,7 +3807,6 @@ int i915_gem_object_set_cache_level(struct drm_i915_gem_object *obj,
 						    old_write_domain);
 	}
 
-	i915_gem_verify_gtt(dev);
 	return 0;
 }
 
@@ -4067,7 +4104,7 @@ i915_gem_ring_throttle(struct drm_device *dev, struct drm_file *file)
 	if (seqno == 0)
 		return 0;
 
-	ret = __wait_seqno(ring, seqno, reset_counter, true, NULL, NULL);
+	ret = __i915_wait_seqno(ring, seqno, reset_counter, true, NULL, NULL);
 	if (ret == 0)
 		queue_delayed_work(dev_priv->wq, &dev_priv->mm.retire_work, 0);
 
@@ -4101,6 +4138,7 @@ i915_gem_object_pin(struct drm_i915_gem_object *obj,
 {
 	struct drm_i915_private *dev_priv = obj->base.dev->dev_private;
 	struct i915_vma *vma;
+	unsigned bound;
 	int ret;
 
 	if (WARN_ON(vm == &dev_priv->mm.aliasing_ppgtt->base))
@@ -4109,6 +4147,9 @@ i915_gem_object_pin(struct drm_i915_gem_object *obj,
 	if (WARN_ON(flags & (PIN_GLOBAL | PIN_MAPPABLE) && !i915_is_ggtt(vm)))
 		return -EINVAL;
 
+	if (WARN_ON((flags & (PIN_MAPPABLE | PIN_GLOBAL)) == PIN_MAPPABLE))
+		return -EINVAL;
+
 	vma = i915_gem_obj_to_vma(obj, vm);
 	if (vma) {
 		if (WARN_ON(vma->pin_count == DRM_I915_GEM_OBJECT_MAX_PIN_COUNT))
@@ -4130,15 +4171,39 @@ i915_gem_object_pin(struct drm_i915_gem_object *obj,
 		}
 	}
 
+	bound = vma ? vma->bound : 0;
 	if (vma == NULL || !drm_mm_node_allocated(&vma->node)) {
 		vma = i915_gem_object_bind_to_vm(obj, vm, alignment, flags);
 		if (IS_ERR(vma))
 			return PTR_ERR(vma);
 	}
 
-	if (flags & PIN_GLOBAL && !obj->has_global_gtt_mapping)
+	if (flags & PIN_GLOBAL && !(vma->bound & GLOBAL_BIND))
 		vma->bind_vma(vma, obj->cache_level, GLOBAL_BIND);
 
+	if ((bound ^ vma->bound) & GLOBAL_BIND) {
+		bool mappable, fenceable;
+		u32 fence_size, fence_alignment;
+
+		fence_size = i915_gem_get_gtt_size(obj->base.dev,
+						   obj->base.size,
+						   obj->tiling_mode);
+		fence_alignment = i915_gem_get_gtt_alignment(obj->base.dev,
+							     obj->base.size,
+							     obj->tiling_mode,
+							     true);
+
+		fenceable = (vma->node.size == fence_size &&
+			     (vma->node.start & (fence_alignment - 1)) == 0);
+
+		mappable = (vma->node.start + obj->base.size <=
+			    dev_priv->gtt.mappable_end);
+
+		obj->map_and_fenceable = mappable && fenceable;
+	}
+
+	WARN_ON(flags & PIN_MAPPABLE && !obj->map_and_fenceable);
+
 	vma->pin_count++;
 	if (flags & PIN_MAPPABLE)
 		obj->pin_mappable |= true;
@@ -4193,7 +4258,7 @@ i915_gem_pin_ioctl(struct drm_device *dev, void *data,
 	struct drm_i915_gem_object *obj;
 	int ret;
 
-	if (INTEL_INFO(dev)->gen >= 6)
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
 		return -ENODEV;
 
 	ret = i915_mutex_lock_interruptible(dev);
@@ -4249,6 +4314,9 @@ i915_gem_unpin_ioctl(struct drm_device *dev, void *data,
 	struct drm_i915_gem_object *obj;
 	int ret;
 
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
 	ret = i915_mutex_lock_interruptible(dev);
 	if (ret)
 		return ret;
@@ -4326,6 +4394,7 @@ int
 i915_gem_madvise_ioctl(struct drm_device *dev, void *data,
 		       struct drm_file *file_priv)
 {
+	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_i915_gem_madvise *args = data;
 	struct drm_i915_gem_object *obj;
 	int ret;
@@ -4353,6 +4422,15 @@ i915_gem_madvise_ioctl(struct drm_device *dev, void *data,
 		goto out;
 	}
 
+	if (obj->pages &&
+	    obj->tiling_mode != I915_TILING_NONE &&
+	    dev_priv->quirks & QUIRK_PIN_SWIZZLED_PAGES) {
+		if (obj->madv == I915_MADV_WILLNEED)
+			i915_gem_object_unpin_pages(obj);
+		if (args->madv == I915_MADV_WILLNEED)
+			i915_gem_object_pin_pages(obj);
+	}
+
 	if (obj->madv != __I915_MADV_PURGED)
 		obj->madv = args->madv;
 
@@ -4495,8 +4573,6 @@ void i915_gem_free_object(struct drm_gem_object *gem_obj)
 		}
 	}
 
-	i915_gem_object_detach_phys(obj);
-
 	/* Stolen objects don't hold a ref, but do hold pin count. Fix that up
 	 * before progressing. */
 	if (obj->stolen)
@@ -4504,6 +4580,11 @@ void i915_gem_free_object(struct drm_gem_object *gem_obj)
 
 	WARN_ON(obj->frontbuffer_bits);
 
+	if (obj->pages && obj->madv == I915_MADV_WILLNEED &&
+	    dev_priv->quirks & QUIRK_PIN_SWIZZLED_PAGES &&
+	    obj->tiling_mode != I915_TILING_NONE)
+		i915_gem_object_unpin_pages(obj);
+
 	if (WARN_ON(obj->pages_pin_count))
 		obj->pages_pin_count = 0;
 	if (discard_backing_storage(obj))
@@ -4576,9 +4657,6 @@ i915_gem_suspend(struct drm_device *dev)
 	int ret = 0;
 
 	mutex_lock(&dev->struct_mutex);
-	if (dev_priv->ums.mm_suspended)
-		goto err;
-
 	ret = i915_gpu_idle(dev);
 	if (ret)
 		goto err;
@@ -4589,15 +4667,7 @@ i915_gem_suspend(struct drm_device *dev)
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		i915_gem_evict_everything(dev);
 
-	i915_kernel_lost_context(dev);
 	i915_gem_stop_ringbuffers(dev);
-
-	/* Hack!  Don't let anybody do execbuf while we don't control the chip.
-	 * We need to replace this with a semaphore, or something.
-	 * And not confound ums.mm_suspended!
-	 */
-	dev_priv->ums.mm_suspended = !drm_core_check_feature(dev,
-							     DRIVER_MODESET);
 	mutex_unlock(&dev->struct_mutex);
 
 	del_timer_sync(&dev_priv->gpu_error.hangcheck_timer);
@@ -4888,9 +4958,6 @@ int i915_gem_init(struct drm_device *dev)
 	}
 	mutex_unlock(&dev->struct_mutex);
 
-	/* 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 ret;
 }
 
@@ -4905,74 +4972,6 @@ i915_gem_cleanup_ringbuffer(struct drm_device *dev)
 		dev_priv->gt.cleanup_ring(ring);
 }
 
-int
-i915_gem_entervt_ioctl(struct drm_device *dev, void *data,
-		       struct drm_file *file_priv)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int ret;
-
-	if (drm_core_check_feature(dev, DRIVER_MODESET))
-		return 0;
-
-	if (i915_reset_in_progress(&dev_priv->gpu_error)) {
-		DRM_ERROR("Reenabling wedged hardware, good luck\n");
-		atomic_set(&dev_priv->gpu_error.reset_counter, 0);
-	}
-
-	mutex_lock(&dev->struct_mutex);
-	dev_priv->ums.mm_suspended = 0;
-
-	ret = i915_gem_init_hw(dev);
-	if (ret != 0) {
-		mutex_unlock(&dev->struct_mutex);
-		return ret;
-	}
-
-	BUG_ON(!list_empty(&dev_priv->gtt.base.active_list));
-
-	ret = drm_irq_install(dev, dev->pdev->irq);
-	if (ret)
-		goto cleanup_ringbuffer;
-	mutex_unlock(&dev->struct_mutex);
-
-	return 0;
-
-cleanup_ringbuffer:
-	i915_gem_cleanup_ringbuffer(dev);
-	dev_priv->ums.mm_suspended = 1;
-	mutex_unlock(&dev->struct_mutex);
-
-	return ret;
-}
-
-int
-i915_gem_leavevt_ioctl(struct drm_device *dev, void *data,
-		       struct drm_file *file_priv)
-{
-	if (drm_core_check_feature(dev, DRIVER_MODESET))
-		return 0;
-
-	mutex_lock(&dev->struct_mutex);
-	drm_irq_uninstall(dev);
-	mutex_unlock(&dev->struct_mutex);
-
-	return i915_gem_suspend(dev);
-}
-
-void
-i915_gem_lastclose(struct drm_device *dev)
-{
-	int ret;
-
-	if (drm_core_check_feature(dev, DRIVER_MODESET))
-		return;
-
-	ret = i915_gem_suspend(dev);
-	if (ret)
-		DRM_ERROR("failed to idle hardware: %d\n", ret);
-}
-
 static void
 init_ring_lists(struct intel_engine_cs *ring)
 {
@@ -5119,6 +5118,15 @@ int i915_gem_open(struct drm_device *dev, struct drm_file *file)
 	return ret;
 }
 
+/**
+ * i915_gem_track_fb - update frontbuffer tracking
+ * old: current GEM buffer for the frontbuffer slots
+ * new: new GEM buffer for the frontbuffer slots
+ * frontbuffer_bits: bitmask of frontbuffer slots
+ *
+ * This updates the frontbuffer tracking bits @frontbuffer_bits by clearing them
+ * from @old and setting them in @new. Both @old and @new can be NULL.
+ */
 void i915_gem_track_fb(struct drm_i915_gem_object *old,
 		       struct drm_i915_gem_object *new,
 		       unsigned frontbuffer_bits)
@@ -5302,7 +5310,7 @@ i915_gem_shrinker_oom(struct notifier_block *nb, unsigned long event, void *ptr)
 	struct drm_device *dev = dev_priv->dev;
 	struct drm_i915_gem_object *obj;
 	unsigned long timeout = msecs_to_jiffies(5000) + 1;
-	unsigned long pinned, bound, unbound, freed;
+	unsigned long pinned, bound, unbound, freed_pages;
 	bool was_interruptible;
 	bool unlock;
 
@@ -5319,7 +5327,7 @@ i915_gem_shrinker_oom(struct notifier_block *nb, unsigned long event, void *ptr)
 	was_interruptible = dev_priv->mm.interruptible;
 	dev_priv->mm.interruptible = false;
 
-	freed = i915_gem_shrink_all(dev_priv);
+	freed_pages = i915_gem_shrink_all(dev_priv);
 
 	dev_priv->mm.interruptible = was_interruptible;
 
@@ -5350,14 +5358,15 @@ i915_gem_shrinker_oom(struct notifier_block *nb, unsigned long event, void *ptr)
 	if (unlock)
 		mutex_unlock(&dev->struct_mutex);
 
-	pr_info("Purging GPU memory, %lu bytes freed, %lu bytes still pinned.\n",
-		freed, pinned);
+	if (freed_pages || unbound || bound)
+		pr_info("Purging GPU memory, %lu bytes freed, %lu bytes still pinned.\n",
+			freed_pages << PAGE_SHIFT, pinned);
 	if (unbound || bound)
 		pr_err("%lu and %lu bytes still available in the "
 		       "bound and unbound GPU page lists.\n",
 		       bound, unbound);
 
-	*(unsigned long *)ptr += freed;
+	*(unsigned long *)ptr += freed_pages;
 	return NOTIFY_DONE;
 }
 
diff --git a/drivers/gpu/drm/i915/i915_gem_context.c b/drivers/gpu/drm/i915/i915_gem_context.c
index a5221d8f1580..d011ec82ef1e 100644
--- a/drivers/gpu/drm/i915/i915_gem_context.c
+++ b/drivers/gpu/drm/i915/i915_gem_context.c
@@ -88,6 +88,7 @@
 #include <drm/drmP.h>
 #include <drm/i915_drm.h>
 #include "i915_drv.h"
+#include "i915_trace.h"
 
 /* This is a HW constraint. The value below is the largest known requirement
  * I've seen in a spec to date, and that was a workaround for a non-shipping
@@ -137,6 +138,8 @@ void i915_gem_context_free(struct kref *ctx_ref)
 	struct intel_context *ctx = container_of(ctx_ref,
 						 typeof(*ctx), ref);
 
+	trace_i915_context_free(ctx);
+
 	if (i915.enable_execlists)
 		intel_lr_context_free(ctx);
 
@@ -274,6 +277,8 @@ i915_gem_create_context(struct drm_device *dev,
 		ctx->ppgtt = ppgtt;
 	}
 
+	trace_i915_context_create(ctx);
+
 	return ctx;
 
 err_unpin:
@@ -468,7 +473,12 @@ mi_set_context(struct intel_engine_cs *ring,
 	       u32 hw_flags)
 {
 	u32 flags = hw_flags | MI_MM_SPACE_GTT;
-	int ret;
+	const int num_rings =
+		/* Use an extended w/a on ivb+ if signalling from other rings */
+		i915_semaphore_is_enabled(ring->dev) ?
+		hweight32(INTEL_INFO(ring->dev)->ring_mask) - 1 :
+		0;
+	int len, i, ret;
 
 	/* w/a: If Flush TLB Invalidation Mode is enabled, driver must do a TLB
 	 * invalidation prior to MI_SET_CONTEXT. On GEN6 we don't set the value
@@ -485,15 +495,31 @@ mi_set_context(struct intel_engine_cs *ring,
 	if (!IS_HASWELL(ring->dev) && INTEL_INFO(ring->dev)->gen < 8)
 		flags |= (MI_SAVE_EXT_STATE_EN | MI_RESTORE_EXT_STATE_EN);
 
-	ret = intel_ring_begin(ring, 6);
+
+	len = 4;
+	if (INTEL_INFO(ring->dev)->gen >= 7)
+		len += 2 + (num_rings ? 4*num_rings + 2 : 0);
+
+	ret = intel_ring_begin(ring, len);
 	if (ret)
 		return ret;
 
 	/* WaProgramMiArbOnOffAroundMiSetContext:ivb,vlv,hsw,bdw,chv */
-	if (INTEL_INFO(ring->dev)->gen >= 7)
+	if (INTEL_INFO(ring->dev)->gen >= 7) {
 		intel_ring_emit(ring, MI_ARB_ON_OFF | MI_ARB_DISABLE);
-	else
-		intel_ring_emit(ring, MI_NOOP);
+		if (num_rings) {
+			struct intel_engine_cs *signaller;
+
+			intel_ring_emit(ring, MI_LOAD_REGISTER_IMM(num_rings));
+			for_each_ring(signaller, to_i915(ring->dev), i) {
+				if (signaller == ring)
+					continue;
+
+				intel_ring_emit(ring, RING_PSMI_CTL(signaller->mmio_base));
+				intel_ring_emit(ring, _MASKED_BIT_ENABLE(GEN6_PSMI_SLEEP_MSG_DISABLE));
+			}
+		}
+	}
 
 	intel_ring_emit(ring, MI_NOOP);
 	intel_ring_emit(ring, MI_SET_CONTEXT);
@@ -505,10 +531,21 @@ mi_set_context(struct intel_engine_cs *ring,
 	 */
 	intel_ring_emit(ring, MI_NOOP);
 
-	if (INTEL_INFO(ring->dev)->gen >= 7)
+	if (INTEL_INFO(ring->dev)->gen >= 7) {
+		if (num_rings) {
+			struct intel_engine_cs *signaller;
+
+			intel_ring_emit(ring, MI_LOAD_REGISTER_IMM(num_rings));
+			for_each_ring(signaller, to_i915(ring->dev), i) {
+				if (signaller == ring)
+					continue;
+
+				intel_ring_emit(ring, RING_PSMI_CTL(signaller->mmio_base));
+				intel_ring_emit(ring, _MASKED_BIT_DISABLE(GEN6_PSMI_SLEEP_MSG_DISABLE));
+			}
+		}
 		intel_ring_emit(ring, MI_ARB_ON_OFF | MI_ARB_ENABLE);
-	else
-		intel_ring_emit(ring, MI_NOOP);
+	}
 
 	intel_ring_advance(ring);
 
@@ -522,6 +559,7 @@ static int do_switch(struct intel_engine_cs *ring,
 	struct intel_context *from = ring->last_context;
 	u32 hw_flags = 0;
 	bool uninitialized = false;
+	struct i915_vma *vma;
 	int ret, i;
 
 	if (from != NULL && ring == &dev_priv->ring[RCS]) {
@@ -548,6 +586,7 @@ static int do_switch(struct intel_engine_cs *ring,
 	from = ring->last_context;
 
 	if (to->ppgtt) {
+		trace_switch_mm(ring, to);
 		ret = to->ppgtt->switch_mm(to->ppgtt, ring);
 		if (ret)
 			goto unpin_out;
@@ -571,11 +610,10 @@ static int do_switch(struct intel_engine_cs *ring,
 	if (ret)
 		goto unpin_out;
 
-	if (!to->legacy_hw_ctx.rcs_state->has_global_gtt_mapping) {
-		struct i915_vma *vma = i915_gem_obj_to_vma(to->legacy_hw_ctx.rcs_state,
-							   &dev_priv->gtt.base);
-		vma->bind_vma(vma, to->legacy_hw_ctx.rcs_state->cache_level, GLOBAL_BIND);
-	}
+	vma = i915_gem_obj_to_ggtt(to->legacy_hw_ctx.rcs_state);
+	if (!(vma->bound & GLOBAL_BIND))
+		vma->bind_vma(vma, to->legacy_hw_ctx.rcs_state->cache_level,
+				GLOBAL_BIND);
 
 	if (!to->legacy_hw_ctx.initialized || i915_gem_context_is_default(to))
 		hw_flags |= MI_RESTORE_INHIBIT;
@@ -629,7 +667,7 @@ done:
 
 	if (uninitialized) {
 		if (ring->init_context) {
-			ret = ring->init_context(ring);
+			ret = ring->init_context(ring, to);
 			if (ret)
 				DRM_ERROR("ring init context: %d\n", ret);
 		}
diff --git a/drivers/gpu/drm/i915/i915_gem_execbuffer.c b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
index 1a0611bb576b..11738316394a 100644
--- a/drivers/gpu/drm/i915/i915_gem_execbuffer.c
+++ b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
@@ -357,12 +357,9 @@ i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
 	 * 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)) {
-		struct i915_vma *vma =
-			list_first_entry(&target_i915_obj->vma_list,
-					 typeof(*vma), vma_link);
-		vma->bind_vma(vma, target_i915_obj->cache_level, GLOBAL_BIND);
-	}
+	    !(target_vma->bound & GLOBAL_BIND)))
+		target_vma->bind_vma(target_vma, target_i915_obj->cache_level,
+				GLOBAL_BIND);
 
 	/* Validate that the target is in a valid r/w GPU domain */
 	if (unlikely(reloc->write_domain & (reloc->write_domain - 1))) {
@@ -531,7 +528,7 @@ i915_gem_execbuffer_reserve_vma(struct i915_vma *vma,
 
 	flags = 0;
 	if (entry->flags & __EXEC_OBJECT_NEEDS_MAP)
-		flags |= PIN_MAPPABLE;
+		flags |= PIN_GLOBAL | PIN_MAPPABLE;
 	if (entry->flags & EXEC_OBJECT_NEEDS_GTT)
 		flags |= PIN_GLOBAL;
 	if (entry->flags & __EXEC_OBJECT_NEEDS_BIAS)
@@ -1023,6 +1020,47 @@ i915_reset_gen7_sol_offsets(struct drm_device *dev,
 	return 0;
 }
 
+static int
+i915_emit_box(struct intel_engine_cs *ring,
+	      struct drm_clip_rect *box,
+	      int DR1, int DR4)
+{
+	int ret;
+
+	if (box->y2 <= box->y1 || box->x2 <= box->x1 ||
+	    box->y2 <= 0 || box->x2 <= 0) {
+		DRM_ERROR("Bad box %d,%d..%d,%d\n",
+			  box->x1, box->y1, box->x2, box->y2);
+		return -EINVAL;
+	}
+
+	if (INTEL_INFO(ring->dev)->gen >= 4) {
+		ret = intel_ring_begin(ring, 4);
+		if (ret)
+			return ret;
+
+		intel_ring_emit(ring, GFX_OP_DRAWRECT_INFO_I965);
+		intel_ring_emit(ring, (box->x1 & 0xffff) | box->y1 << 16);
+		intel_ring_emit(ring, ((box->x2 - 1) & 0xffff) | (box->y2 - 1) << 16);
+		intel_ring_emit(ring, DR4);
+	} else {
+		ret = intel_ring_begin(ring, 6);
+		if (ret)
+			return ret;
+
+		intel_ring_emit(ring, GFX_OP_DRAWRECT_INFO);
+		intel_ring_emit(ring, DR1);
+		intel_ring_emit(ring, (box->x1 & 0xffff) | box->y1 << 16);
+		intel_ring_emit(ring, ((box->x2 - 1) & 0xffff) | (box->y2 - 1) << 16);
+		intel_ring_emit(ring, DR4);
+		intel_ring_emit(ring, 0);
+	}
+	intel_ring_advance(ring);
+
+	return 0;
+}
+
+
 int
 i915_gem_ringbuffer_submission(struct drm_device *dev, struct drm_file *file,
 			       struct intel_engine_cs *ring,
@@ -1151,7 +1189,7 @@ i915_gem_ringbuffer_submission(struct drm_device *dev, struct drm_file *file,
 	exec_len = args->batch_len;
 	if (cliprects) {
 		for (i = 0; i < args->num_cliprects; i++) {
-			ret = i915_emit_box(dev, &cliprects[i],
+			ret = i915_emit_box(ring, &cliprects[i],
 					    args->DR1, args->DR4);
 			if (ret)
 				goto error;
@@ -1300,12 +1338,6 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 	if (ret)
 		goto pre_mutex_err;
 
-	if (dev_priv->ums.mm_suspended) {
-		mutex_unlock(&dev->struct_mutex);
-		ret = -EBUSY;
-		goto pre_mutex_err;
-	}
-
 	ctx = i915_gem_validate_context(dev, file, ring, ctx_id);
 	if (IS_ERR(ctx)) {
 		mutex_unlock(&dev->struct_mutex);
@@ -1368,17 +1400,19 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 				      batch_obj,
 				      args->batch_start_offset,
 				      file->is_master);
-		if (ret)
-			goto err;
-
-		/*
-		 * XXX: Actually do this when enabling batch copy...
-		 *
-		 * Set the DISPATCH_SECURE bit to remove the NON_SECURE bit
-		 * from MI_BATCH_BUFFER_START commands issued in the
-		 * dispatch_execbuffer implementations. We specifically don't
-		 * want that set when the command parser is enabled.
-		 */
+		if (ret) {
+			if (ret != -EACCES)
+				goto err;
+		} else {
+			/*
+			 * XXX: Actually do this when enabling batch copy...
+			 *
+			 * Set the DISPATCH_SECURE bit to remove the NON_SECURE bit
+			 * from MI_BATCH_BUFFER_START commands issued in the
+			 * dispatch_execbuffer implementations. We specifically don't
+			 * want that set when the command parser is enabled.
+			 */
+		}
 	}
 
 	/* snb/ivb/vlv conflate the "batch in ppgtt" bit with the "non-secure
diff --git a/drivers/gpu/drm/i915/i915_gem_gtt.c b/drivers/gpu/drm/i915/i915_gem_gtt.c
index 728938f02341..171f6eafdeee 100644
--- a/drivers/gpu/drm/i915/i915_gem_gtt.c
+++ b/drivers/gpu/drm/i915/i915_gem_gtt.c
@@ -35,13 +35,26 @@ static void chv_setup_private_ppat(struct drm_i915_private *dev_priv);
 
 static int sanitize_enable_ppgtt(struct drm_device *dev, int enable_ppgtt)
 {
-	if (enable_ppgtt == 0 || !HAS_ALIASING_PPGTT(dev))
+	bool has_aliasing_ppgtt;
+	bool has_full_ppgtt;
+
+	has_aliasing_ppgtt = INTEL_INFO(dev)->gen >= 6;
+	has_full_ppgtt = INTEL_INFO(dev)->gen >= 7;
+	if (IS_GEN8(dev))
+		has_full_ppgtt = false; /* XXX why? */
+
+	/*
+	 * We don't allow disabling PPGTT for gen9+ as it's a requirement for
+	 * execlists, the sole mechanism available to submit work.
+	 */
+	if (INTEL_INFO(dev)->gen < 9 &&
+	    (enable_ppgtt == 0 || !has_aliasing_ppgtt))
 		return 0;
 
 	if (enable_ppgtt == 1)
 		return 1;
 
-	if (enable_ppgtt == 2 && HAS_PPGTT(dev))
+	if (enable_ppgtt == 2 && has_full_ppgtt)
 		return 2;
 
 #ifdef CONFIG_INTEL_IOMMU
@@ -59,7 +72,7 @@ static int sanitize_enable_ppgtt(struct drm_device *dev, int enable_ppgtt)
 		return 0;
 	}
 
-	return HAS_ALIASING_PPGTT(dev) ? 1 : 0;
+	return has_aliasing_ppgtt ? 1 : 0;
 }
 
 
@@ -156,9 +169,6 @@ static gen6_gtt_pte_t byt_pte_encode(dma_addr_t addr,
 	gen6_gtt_pte_t pte = valid ? GEN6_PTE_VALID : 0;
 	pte |= GEN6_PTE_ADDR_ENCODE(addr);
 
-	/* Mark the page as writeable.  Other platforms don't have a
-	 * setting for read-only/writable, so this matches that behavior.
-	 */
 	if (!(flags & PTE_READ_ONLY))
 		pte |= BYT_PTE_WRITEABLE;
 
@@ -1092,7 +1102,7 @@ static int __hw_ppgtt_init(struct drm_device *dev, struct i915_hw_ppgtt *ppgtt)
 
 	if (INTEL_INFO(dev)->gen < 8)
 		return gen6_ppgtt_init(ppgtt);
-	else if (IS_GEN8(dev))
+	else if (IS_GEN8(dev) || IS_GEN9(dev))
 		return gen8_ppgtt_init(ppgtt, dev_priv->gtt.base.total);
 	else
 		BUG();
@@ -1166,6 +1176,8 @@ i915_ppgtt_create(struct drm_device *dev, struct drm_i915_file_private *fpriv)
 
 	ppgtt->file_priv = fpriv;
 
+	trace_i915_ppgtt_create(&ppgtt->base);
+
 	return ppgtt;
 }
 
@@ -1174,6 +1186,8 @@ void  i915_ppgtt_release(struct kref *kref)
 	struct i915_hw_ppgtt *ppgtt =
 		container_of(kref, struct i915_hw_ppgtt, ref);
 
+	trace_i915_ppgtt_release(&ppgtt->base);
+
 	/* vmas should already be unbound */
 	WARN_ON(!list_empty(&ppgtt->base.active_list));
 	WARN_ON(!list_empty(&ppgtt->base.inactive_list));
@@ -1258,7 +1272,7 @@ void i915_check_and_clear_faults(struct drm_device *dev)
 		fault_reg = I915_READ(RING_FAULT_REG(ring));
 		if (fault_reg & RING_FAULT_VALID) {
 			DRM_DEBUG_DRIVER("Unexpected fault\n"
-					 "\tAddr: 0x%08lx\\n"
+					 "\tAddr: 0x%08lx\n"
 					 "\tAddress space: %s\n"
 					 "\tSource ID: %d\n"
 					 "\tType: %d\n",
@@ -1328,7 +1342,7 @@ void i915_gem_restore_gtt_mappings(struct drm_device *dev)
 		 * Unfortunately above, we've just wiped out the mappings
 		 * without telling our object about it. So we need to fake it.
 		 */
-		obj->has_global_gtt_mapping = 0;
+		vma->bound &= ~GLOBAL_BIND;
 		vma->bind_vma(vma, obj->cache_level, GLOBAL_BIND);
 	}
 
@@ -1525,7 +1539,7 @@ static void i915_ggtt_bind_vma(struct i915_vma *vma,
 
 	BUG_ON(!i915_is_ggtt(vma->vm));
 	intel_gtt_insert_sg_entries(vma->obj->pages, entry, flags);
-	vma->obj->has_global_gtt_mapping = 1;
+	vma->bound = GLOBAL_BIND;
 }
 
 static void i915_ggtt_clear_range(struct i915_address_space *vm,
@@ -1544,7 +1558,7 @@ static void i915_ggtt_unbind_vma(struct i915_vma *vma)
 	const unsigned int size = vma->obj->base.size >> PAGE_SHIFT;
 
 	BUG_ON(!i915_is_ggtt(vma->vm));
-	vma->obj->has_global_gtt_mapping = 0;
+	vma->bound = 0;
 	intel_gtt_clear_range(first, size);
 }
 
@@ -1572,24 +1586,24 @@ static void ggtt_bind_vma(struct i915_vma *vma,
 	 * flags. At all other times, the GPU will use the aliasing PPGTT.
 	 */
 	if (!dev_priv->mm.aliasing_ppgtt || flags & GLOBAL_BIND) {
-		if (!obj->has_global_gtt_mapping ||
+		if (!(vma->bound & GLOBAL_BIND) ||
 		    (cache_level != obj->cache_level)) {
 			vma->vm->insert_entries(vma->vm, obj->pages,
 						vma->node.start,
 						cache_level, flags);
-			obj->has_global_gtt_mapping = 1;
+			vma->bound |= GLOBAL_BIND;
 		}
 	}
 
 	if (dev_priv->mm.aliasing_ppgtt &&
-	    (!obj->has_aliasing_ppgtt_mapping ||
+	    (!(vma->bound & LOCAL_BIND) ||
 	     (cache_level != obj->cache_level))) {
 		struct i915_hw_ppgtt *appgtt = dev_priv->mm.aliasing_ppgtt;
 		appgtt->base.insert_entries(&appgtt->base,
 					    vma->obj->pages,
 					    vma->node.start,
 					    cache_level, flags);
-		vma->obj->has_aliasing_ppgtt_mapping = 1;
+		vma->bound |= LOCAL_BIND;
 	}
 }
 
@@ -1599,21 +1613,21 @@ static void ggtt_unbind_vma(struct i915_vma *vma)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_i915_gem_object *obj = vma->obj;
 
-	if (obj->has_global_gtt_mapping) {
+	if (vma->bound & GLOBAL_BIND) {
 		vma->vm->clear_range(vma->vm,
 				     vma->node.start,
 				     obj->base.size,
 				     true);
-		obj->has_global_gtt_mapping = 0;
+		vma->bound &= ~GLOBAL_BIND;
 	}
 
-	if (obj->has_aliasing_ppgtt_mapping) {
+	if (vma->bound & LOCAL_BIND) {
 		struct i915_hw_ppgtt *appgtt = dev_priv->mm.aliasing_ppgtt;
 		appgtt->base.clear_range(&appgtt->base,
 					 vma->node.start,
 					 obj->base.size,
 					 true);
-		obj->has_aliasing_ppgtt_mapping = 0;
+		vma->bound &= ~LOCAL_BIND;
 	}
 }
 
@@ -1650,10 +1664,10 @@ static void i915_gtt_color_adjust(struct drm_mm_node *node,
 	}
 }
 
-int i915_gem_setup_global_gtt(struct drm_device *dev,
-			      unsigned long start,
-			      unsigned long mappable_end,
-			      unsigned long end)
+static int i915_gem_setup_global_gtt(struct drm_device *dev,
+				     unsigned long start,
+				     unsigned long mappable_end,
+				     unsigned long end)
 {
 	/* Let GEM Manage all of the aperture.
 	 *
@@ -1691,7 +1705,7 @@ int i915_gem_setup_global_gtt(struct drm_device *dev,
 			DRM_DEBUG_KMS("Reservation failed: %i\n", ret);
 			return ret;
 		}
-		obj->has_global_gtt_mapping = 1;
+		vma->bound |= GLOBAL_BIND;
 	}
 
 	dev_priv->gtt.base.start = start;
@@ -1764,7 +1778,6 @@ static int setup_scratch_page(struct drm_device *dev)
 	page = alloc_page(GFP_KERNEL | GFP_DMA32 | __GFP_ZERO);
 	if (page == NULL)
 		return -ENOMEM;
-	get_page(page);
 	set_pages_uc(page, 1);
 
 #ifdef CONFIG_INTEL_IOMMU
@@ -1789,7 +1802,6 @@ static void teardown_scratch_page(struct drm_device *dev)
 	set_pages_wb(page, 1);
 	pci_unmap_page(dev->pdev, dev_priv->gtt.base.scratch.addr,
 		       PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
-	put_page(page);
 	__free_page(page);
 }
 
@@ -1859,6 +1871,18 @@ static size_t chv_get_stolen_size(u16 gmch_ctrl)
 		return (gmch_ctrl - 0x17 + 9) << 22;
 }
 
+static size_t gen9_get_stolen_size(u16 gen9_gmch_ctl)
+{
+	gen9_gmch_ctl >>= BDW_GMCH_GMS_SHIFT;
+	gen9_gmch_ctl &= BDW_GMCH_GMS_MASK;
+
+	if (gen9_gmch_ctl < 0xf0)
+		return gen9_gmch_ctl << 25; /* 32 MB units */
+	else
+		/* 4MB increments starting at 0xf0 for 4MB */
+		return (gen9_gmch_ctl - 0xf0 + 1) << 22;
+}
+
 static int ggtt_probe_common(struct drm_device *dev,
 			     size_t gtt_size)
 {
@@ -1934,9 +1958,17 @@ static void chv_setup_private_ppat(struct drm_i915_private *dev_priv)
 	 * Only the snoop bit has meaning for CHV, the rest is
 	 * ignored.
 	 *
-	 * Note that the harware enforces snooping for all page
-	 * table accesses. The snoop bit is actually ignored for
-	 * PDEs.
+	 * The hardware will never snoop for certain types of accesses:
+	 * - CPU GTT (GMADR->GGTT->no snoop->memory)
+	 * - PPGTT page tables
+	 * - some other special cycles
+	 *
+	 * As with BDW, we also need to consider the following for GT accesses:
+	 * "For GGTT, there is NO pat_sel[2:0] from the entry,
+	 * so RTL will always use the value corresponding to
+	 * pat_sel = 000".
+	 * Which means we must set the snoop bit in PAT entry 0
+	 * in order to keep the global status page working.
 	 */
 	pat = GEN8_PPAT(0, CHV_PPAT_SNOOP) |
 	      GEN8_PPAT(1, 0) |
@@ -1971,7 +2003,10 @@ static int gen8_gmch_probe(struct drm_device *dev,
 
 	pci_read_config_word(dev->pdev, SNB_GMCH_CTRL, &snb_gmch_ctl);
 
-	if (IS_CHERRYVIEW(dev)) {
+	if (INTEL_INFO(dev)->gen >= 9) {
+		*stolen = gen9_get_stolen_size(snb_gmch_ctl);
+		gtt_size = gen8_get_total_gtt_size(snb_gmch_ctl);
+	} else if (IS_CHERRYVIEW(dev)) {
 		*stolen = chv_get_stolen_size(snb_gmch_ctl);
 		gtt_size = chv_get_total_gtt_size(snb_gmch_ctl);
 	} else {
@@ -2143,6 +2178,7 @@ static struct i915_vma *__i915_gem_vma_create(struct drm_i915_gem_object *obj,
 	vma->obj = obj;
 
 	switch (INTEL_INFO(vm->dev)->gen) {
+	case 9:
 	case 8:
 	case 7:
 	case 6:
diff --git a/drivers/gpu/drm/i915/i915_gem_gtt.h b/drivers/gpu/drm/i915/i915_gem_gtt.h
index d5c14af51e99..beaf4bcfdac8 100644
--- a/drivers/gpu/drm/i915/i915_gem_gtt.h
+++ b/drivers/gpu/drm/i915/i915_gem_gtt.h
@@ -123,6 +123,12 @@ struct i915_vma {
 	struct drm_i915_gem_object *obj;
 	struct i915_address_space *vm;
 
+	/** Flags and address space this VMA is bound to */
+#define GLOBAL_BIND	(1<<0)
+#define LOCAL_BIND	(1<<1)
+#define PTE_READ_ONLY	(1<<2)
+	unsigned int bound : 4;
+
 	/** This object's place on the active/inactive lists */
 	struct list_head mm_list;
 
@@ -155,8 +161,6 @@ struct i915_vma {
 	 * setting the valid PTE entries to a reserved scratch page. */
 	void (*unbind_vma)(struct i915_vma *vma);
 	/* Map an object into an address space with the given cache flags. */
-#define GLOBAL_BIND (1<<0)
-#define PTE_READ_ONLY (1<<1)
 	void (*bind_vma)(struct i915_vma *vma,
 			 enum i915_cache_level cache_level,
 			 u32 flags);
@@ -270,8 +274,6 @@ struct i915_hw_ppgtt {
 
 int i915_gem_gtt_init(struct drm_device *dev);
 void i915_gem_init_global_gtt(struct drm_device *dev);
-int i915_gem_setup_global_gtt(struct drm_device *dev, unsigned long start,
-			      unsigned long mappable_end, unsigned long end);
 void i915_global_gtt_cleanup(struct drm_device *dev);
 
 
diff --git a/drivers/gpu/drm/i915/i915_gem_render_state.c b/drivers/gpu/drm/i915/i915_gem_render_state.c
index a9a62d75aa57..98dcd94acba8 100644
--- a/drivers/gpu/drm/i915/i915_gem_render_state.c
+++ b/drivers/gpu/drm/i915/i915_gem_render_state.c
@@ -38,6 +38,8 @@ render_state_get_rodata(struct drm_device *dev, const int gen)
 		return &gen7_null_state;
 	case 8:
 		return &gen8_null_state;
+	case 9:
+		return &gen9_null_state;
 	}
 
 	return NULL;
diff --git a/drivers/gpu/drm/i915/i915_gem_stolen.c b/drivers/gpu/drm/i915/i915_gem_stolen.c
index 85fda6b803e4..a2045848bd1a 100644
--- a/drivers/gpu/drm/i915/i915_gem_stolen.c
+++ b/drivers/gpu/drm/i915/i915_gem_stolen.c
@@ -137,7 +137,11 @@ static unsigned long i915_stolen_to_physical(struct drm_device *dev)
 		r = devm_request_mem_region(dev->dev, base + 1,
 					    dev_priv->gtt.stolen_size - 1,
 					    "Graphics Stolen Memory");
-		if (r == NULL) {
+		/*
+		 * GEN3 firmware likes to smash pci bridges into the stolen
+		 * range. Apparently this works.
+		 */
+		if (r == NULL && !IS_GEN3(dev)) {
 			DRM_ERROR("conflict detected with stolen region: [0x%08x - 0x%08x]\n",
 				  base, base + (uint32_t)dev_priv->gtt.stolen_size);
 			base = 0;
@@ -533,7 +537,7 @@ i915_gem_object_create_stolen_for_preallocated(struct drm_device *dev,
 		}
 	}
 
-	obj->has_global_gtt_mapping = 1;
+	vma->bound |= GLOBAL_BIND;
 
 	list_add_tail(&obj->global_list, &dev_priv->mm.bound_list);
 	list_add_tail(&vma->mm_list, &ggtt->inactive_list);
diff --git a/drivers/gpu/drm/i915/i915_gem_tiling.c b/drivers/gpu/drm/i915/i915_gem_tiling.c
index 2b1eaa29ada4..4727a4e2c87c 100644
--- a/drivers/gpu/drm/i915/i915_gem_tiling.c
+++ b/drivers/gpu/drm/i915/i915_gem_tiling.c
@@ -102,22 +102,33 @@ i915_gem_detect_bit_6_swizzle(struct drm_device *dev)
 		swizzle_x = I915_BIT_6_SWIZZLE_NONE;
 		swizzle_y = I915_BIT_6_SWIZZLE_NONE;
 	} else if (INTEL_INFO(dev)->gen >= 6) {
-		uint32_t dimm_c0, dimm_c1;
-		dimm_c0 = I915_READ(MAD_DIMM_C0);
-		dimm_c1 = I915_READ(MAD_DIMM_C1);
-		dimm_c0 &= MAD_DIMM_A_SIZE_MASK | MAD_DIMM_B_SIZE_MASK;
-		dimm_c1 &= MAD_DIMM_A_SIZE_MASK | MAD_DIMM_B_SIZE_MASK;
-		/* Enable swizzling when the channels are populated with
-		 * identically sized dimms. We don't need to check the 3rd
-		 * channel because no cpu with gpu attached ships in that
-		 * configuration. Also, swizzling only makes sense for 2
-		 * channels anyway. */
-		if (dimm_c0 == dimm_c1) {
-			swizzle_x = I915_BIT_6_SWIZZLE_9_10;
-			swizzle_y = I915_BIT_6_SWIZZLE_9;
+		if (dev_priv->preserve_bios_swizzle) {
+			if (I915_READ(DISP_ARB_CTL) &
+			    DISP_TILE_SURFACE_SWIZZLING) {
+				swizzle_x = I915_BIT_6_SWIZZLE_9_10;
+				swizzle_y = I915_BIT_6_SWIZZLE_9;
+			} else {
+				swizzle_x = I915_BIT_6_SWIZZLE_NONE;
+				swizzle_y = I915_BIT_6_SWIZZLE_NONE;
+			}
 		} else {
-			swizzle_x = I915_BIT_6_SWIZZLE_NONE;
-			swizzle_y = I915_BIT_6_SWIZZLE_NONE;
+			uint32_t dimm_c0, dimm_c1;
+			dimm_c0 = I915_READ(MAD_DIMM_C0);
+			dimm_c1 = I915_READ(MAD_DIMM_C1);
+			dimm_c0 &= MAD_DIMM_A_SIZE_MASK | MAD_DIMM_B_SIZE_MASK;
+			dimm_c1 &= MAD_DIMM_A_SIZE_MASK | MAD_DIMM_B_SIZE_MASK;
+			/* Enable swizzling when the channels are populated
+			 * with identically sized dimms. We don't need to check
+			 * the 3rd channel because no cpu with gpu attached
+			 * ships in that configuration. Also, swizzling only
+			 * makes sense for 2 channels anyway. */
+			if (dimm_c0 == dimm_c1) {
+				swizzle_x = I915_BIT_6_SWIZZLE_9_10;
+				swizzle_y = I915_BIT_6_SWIZZLE_9;
+			} else {
+				swizzle_x = I915_BIT_6_SWIZZLE_NONE;
+				swizzle_y = I915_BIT_6_SWIZZLE_NONE;
+			}
 		}
 	} else if (IS_GEN5(dev)) {
 		/* On Ironlake whatever DRAM config, GPU always do
@@ -167,6 +178,15 @@ i915_gem_detect_bit_6_swizzle(struct drm_device *dev)
 			}
 			break;
 		}
+
+		/* check for L-shaped memory aka modified enhanced addressing */
+		if (IS_GEN4(dev)) {
+			uint32_t ddc2 = I915_READ(DCC2);
+
+			if (!(ddc2 & DCC2_MODIFIED_ENHANCED_DISABLE))
+				dev_priv->quirks |= QUIRK_PIN_SWIZZLED_PAGES;
+		}
+
 		if (dcc == 0xffffffff) {
 			DRM_ERROR("Couldn't read from MCHBAR.  "
 				  "Disabling tiling.\n");
@@ -369,6 +389,15 @@ i915_gem_set_tiling(struct drm_device *dev, void *data,
 			ret = i915_gem_object_ggtt_unbind(obj);
 
 		if (ret == 0) {
+			if (obj->pages &&
+			    obj->madv == I915_MADV_WILLNEED &&
+			    dev_priv->quirks & QUIRK_PIN_SWIZZLED_PAGES) {
+				if (args->tiling_mode == I915_TILING_NONE)
+					i915_gem_object_unpin_pages(obj);
+				if (obj->tiling_mode == I915_TILING_NONE)
+					i915_gem_object_pin_pages(obj);
+			}
+
 			obj->fence_dirty =
 				obj->last_fenced_seqno ||
 				obj->fence_reg != I915_FENCE_REG_NONE;
@@ -434,6 +463,7 @@ i915_gem_get_tiling(struct drm_device *dev, void *data,
 	}
 
 	/* Hide bit 17 from the user -- see comment in i915_gem_set_tiling */
+	args->phys_swizzle_mode = args->swizzle_mode;
 	if (args->swizzle_mode == I915_BIT_6_SWIZZLE_9_17)
 		args->swizzle_mode = I915_BIT_6_SWIZZLE_9;
 	if (args->swizzle_mode == I915_BIT_6_SWIZZLE_9_10_17)
diff --git a/drivers/gpu/drm/i915/i915_gpu_error.c b/drivers/gpu/drm/i915/i915_gpu_error.c
index 2c87a797213f..cdaee6ce05f8 100644
--- a/drivers/gpu/drm/i915/i915_gpu_error.c
+++ b/drivers/gpu/drm/i915/i915_gpu_error.c
@@ -242,11 +242,15 @@ static const char *hangcheck_action_to_str(enum intel_ring_hangcheck_action a)
 
 static void i915_ring_error_state(struct drm_i915_error_state_buf *m,
 				  struct drm_device *dev,
-				  struct drm_i915_error_ring *ring)
+				  struct drm_i915_error_state *error,
+				  int ring_idx)
 {
+	struct drm_i915_error_ring *ring = &error->ring[ring_idx];
+
 	if (!ring->valid)
 		return;
 
+	err_printf(m, "%s command stream:\n", ring_str(ring_idx));
 	err_printf(m, "  HEAD: 0x%08x\n", ring->head);
 	err_printf(m, "  TAIL: 0x%08x\n", ring->tail);
 	err_printf(m, "  CTL: 0x%08x\n", ring->ctl);
@@ -388,10 +392,8 @@ int i915_error_state_to_str(struct drm_i915_error_state_buf *m,
 	if (INTEL_INFO(dev)->gen == 7)
 		err_printf(m, "ERR_INT: 0x%08x\n", error->err_int);
 
-	for (i = 0; i < ARRAY_SIZE(error->ring); i++) {
-		err_printf(m, "%s command stream:\n", ring_str(i));
-		i915_ring_error_state(m, dev, &error->ring[i]);
-	}
+	for (i = 0; i < ARRAY_SIZE(error->ring); i++)
+		i915_ring_error_state(m, dev, error, i);
 
 	for (i = 0; i < error->vm_count; i++) {
 		err_printf(m, "vm[%d]\n", i);
@@ -565,6 +567,7 @@ i915_error_object_create(struct drm_i915_private *dev_priv,
 			 struct i915_address_space *vm)
 {
 	struct drm_i915_error_object *dst;
+	struct i915_vma *vma = NULL;
 	int num_pages;
 	bool use_ggtt;
 	int i = 0;
@@ -585,16 +588,17 @@ i915_error_object_create(struct drm_i915_private *dev_priv,
 		dst->gtt_offset = -1;
 
 	reloc_offset = dst->gtt_offset;
+	if (i915_is_ggtt(vm))
+		vma = i915_gem_obj_to_ggtt(src);
 	use_ggtt = (src->cache_level == I915_CACHE_NONE &&
-		    i915_is_ggtt(vm) &&
-		    src->has_global_gtt_mapping &&
-		    reloc_offset + num_pages * PAGE_SIZE <= dev_priv->gtt.mappable_end);
+		   vma && (vma->bound & GLOBAL_BIND) &&
+		   reloc_offset + num_pages * PAGE_SIZE <= dev_priv->gtt.mappable_end);
 
 	/* Cannot access stolen address directly, try to use the aperture */
 	if (src->stolen) {
 		use_ggtt = true;
 
-		if (!src->has_global_gtt_mapping)
+		if (!(vma && vma->bound & GLOBAL_BIND))
 			goto unwind;
 
 		reloc_offset = i915_gem_obj_ggtt_offset(src);
@@ -765,6 +769,7 @@ static void i915_gem_record_fences(struct drm_device *dev,
 
 	/* Fences */
 	switch (INTEL_INFO(dev)->gen) {
+	case 9:
 	case 8:
 	case 7:
 	case 6:
@@ -804,9 +809,8 @@ static void gen8_record_semaphore_state(struct drm_i915_private *dev_priv,
 
 	if (!error->semaphore_obj)
 		error->semaphore_obj =
-			i915_error_object_create(dev_priv,
-						 dev_priv->semaphore_obj,
-						 &dev_priv->gtt.base);
+			i915_error_ggtt_object_create(dev_priv,
+						      dev_priv->semaphore_obj);
 
 	for_each_ring(to, dev_priv, i) {
 		int idx;
@@ -923,6 +927,7 @@ static void i915_record_ring_state(struct drm_device *dev,
 		ering->vm_info.gfx_mode = I915_READ(RING_MODE_GEN7(ring));
 
 		switch (INTEL_INFO(dev)->gen) {
+		case 9:
 		case 8:
 			for (i = 0; i < 4; i++) {
 				ering->vm_info.pdp[i] =
@@ -1238,7 +1243,8 @@ static void i915_error_capture_msg(struct drm_device *dev,
 	ecode = i915_error_generate_code(dev_priv, error, &ring_id);
 
 	len = scnprintf(error->error_msg, sizeof(error->error_msg),
-			"GPU HANG: ecode %d:0x%08x", ring_id, ecode);
+			"GPU HANG: ecode %d:%d:0x%08x",
+			INTEL_INFO(dev)->gen, ring_id, ecode);
 
 	if (ring_id != -1 && error->ring[ring_id].pid != -1)
 		len += scnprintf(error->error_msg + len,
@@ -1326,13 +1332,12 @@ void i915_error_state_get(struct drm_device *dev,
 			  struct i915_error_state_file_priv *error_priv)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	unsigned long flags;
 
-	spin_lock_irqsave(&dev_priv->gpu_error.lock, flags);
+	spin_lock_irq(&dev_priv->gpu_error.lock);
 	error_priv->error = dev_priv->gpu_error.first_error;
 	if (error_priv->error)
 		kref_get(&error_priv->error->ref);
-	spin_unlock_irqrestore(&dev_priv->gpu_error.lock, flags);
+	spin_unlock_irq(&dev_priv->gpu_error.lock);
 
 }
 
@@ -1346,12 +1351,11 @@ void i915_destroy_error_state(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_i915_error_state *error;
-	unsigned long flags;
 
-	spin_lock_irqsave(&dev_priv->gpu_error.lock, flags);
+	spin_lock_irq(&dev_priv->gpu_error.lock);
 	error = dev_priv->gpu_error.first_error;
 	dev_priv->gpu_error.first_error = NULL;
-	spin_unlock_irqrestore(&dev_priv->gpu_error.lock, flags);
+	spin_unlock_irq(&dev_priv->gpu_error.lock);
 
 	if (error)
 		kref_put(&error->ref, i915_error_state_free);
@@ -1389,6 +1393,7 @@ void i915_get_extra_instdone(struct drm_device *dev, uint32_t *instdone)
 		WARN_ONCE(1, "Unsupported platform\n");
 	case 7:
 	case 8:
+	case 9:
 		instdone[0] = I915_READ(GEN7_INSTDONE_1);
 		instdone[1] = I915_READ(GEN7_SC_INSTDONE);
 		instdone[2] = I915_READ(GEN7_SAMPLER_INSTDONE);
diff --git a/drivers/gpu/drm/i915/i915_ioc32.c b/drivers/gpu/drm/i915/i915_ioc32.c
index 2e0613e26251..176de6322e4d 100644
--- a/drivers/gpu/drm/i915/i915_ioc32.c
+++ b/drivers/gpu/drm/i915/i915_ioc32.c
@@ -189,7 +189,6 @@ static 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>.
@@ -218,4 +217,3 @@ 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 f66392b6e287..996c2931c499 100644
--- a/drivers/gpu/drm/i915/i915_irq.c
+++ b/drivers/gpu/drm/i915/i915_irq.c
@@ -37,6 +37,14 @@
 #include "i915_trace.h"
 #include "intel_drv.h"
 
+/**
+ * DOC: interrupt handling
+ *
+ * These functions provide the basic support for enabling and disabling the
+ * interrupt handling support. There's a lot more functionality in i915_irq.c
+ * and related files, but that will be described in separate chapters.
+ */
+
 static const u32 hpd_ibx[] = {
 	[HPD_CRT] = SDE_CRT_HOTPLUG,
 	[HPD_SDVO_B] = SDE_SDVOB_HOTPLUG,
@@ -118,20 +126,22 @@ static const u32 hpd_status_i915[] = { /* i915 and valleyview are the same */
 
 #define GEN8_IRQ_INIT_NDX(type, which, imr_val, ier_val) do { \
 	GEN5_ASSERT_IIR_IS_ZERO(GEN8_##type##_IIR(which)); \
-	I915_WRITE(GEN8_##type##_IMR(which), (imr_val)); \
 	I915_WRITE(GEN8_##type##_IER(which), (ier_val)); \
-	POSTING_READ(GEN8_##type##_IER(which)); \
+	I915_WRITE(GEN8_##type##_IMR(which), (imr_val)); \
+	POSTING_READ(GEN8_##type##_IMR(which)); \
 } while (0)
 
 #define GEN5_IRQ_INIT(type, imr_val, ier_val) do { \
 	GEN5_ASSERT_IIR_IS_ZERO(type##IIR); \
-	I915_WRITE(type##IMR, (imr_val)); \
 	I915_WRITE(type##IER, (ier_val)); \
-	POSTING_READ(type##IER); \
+	I915_WRITE(type##IMR, (imr_val)); \
+	POSTING_READ(type##IMR); \
 } while (0)
 
+static void gen6_rps_irq_handler(struct drm_i915_private *dev_priv, u32 pm_iir);
+
 /* For display hotplug interrupt */
-static void
+void
 ironlake_enable_display_irq(struct drm_i915_private *dev_priv, u32 mask)
 {
 	assert_spin_locked(&dev_priv->irq_lock);
@@ -146,7 +156,7 @@ ironlake_enable_display_irq(struct drm_i915_private *dev_priv, u32 mask)
 	}
 }
 
-static void
+void
 ironlake_disable_display_irq(struct drm_i915_private *dev_priv, u32 mask)
 {
 	assert_spin_locked(&dev_priv->irq_lock);
@@ -192,71 +202,28 @@ void gen5_disable_gt_irq(struct drm_i915_private *dev_priv, uint32_t mask)
 	ilk_update_gt_irq(dev_priv, mask, 0);
 }
 
-/**
-  * snb_update_pm_irq - update GEN6_PMIMR
-  * @dev_priv: driver private
-  * @interrupt_mask: mask of interrupt bits to update
-  * @enabled_irq_mask: mask of interrupt bits to enable
-  */
-static void snb_update_pm_irq(struct drm_i915_private *dev_priv,
-			      uint32_t interrupt_mask,
-			      uint32_t enabled_irq_mask)
-{
-	uint32_t new_val;
-
-	assert_spin_locked(&dev_priv->irq_lock);
-
-	if (WARN_ON(!intel_irqs_enabled(dev_priv)))
-		return;
-
-	new_val = dev_priv->pm_irq_mask;
-	new_val &= ~interrupt_mask;
-	new_val |= (~enabled_irq_mask & interrupt_mask);
-
-	if (new_val != dev_priv->pm_irq_mask) {
-		dev_priv->pm_irq_mask = new_val;
-		I915_WRITE(GEN6_PMIMR, dev_priv->pm_irq_mask);
-		POSTING_READ(GEN6_PMIMR);
-	}
-}
-
-void gen6_enable_pm_irq(struct drm_i915_private *dev_priv, uint32_t mask)
+static u32 gen6_pm_iir(struct drm_i915_private *dev_priv)
 {
-	snb_update_pm_irq(dev_priv, mask, mask);
+	return INTEL_INFO(dev_priv)->gen >= 8 ? GEN8_GT_IIR(2) : GEN6_PMIIR;
 }
 
-void gen6_disable_pm_irq(struct drm_i915_private *dev_priv, uint32_t mask)
+static u32 gen6_pm_imr(struct drm_i915_private *dev_priv)
 {
-	snb_update_pm_irq(dev_priv, mask, 0);
+	return INTEL_INFO(dev_priv)->gen >= 8 ? GEN8_GT_IMR(2) : GEN6_PMIMR;
 }
 
-static bool ivb_can_enable_err_int(struct drm_device *dev)
+static u32 gen6_pm_ier(struct drm_i915_private *dev_priv)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_crtc *crtc;
-	enum pipe pipe;
-
-	assert_spin_locked(&dev_priv->irq_lock);
-
-	for_each_pipe(dev_priv, pipe) {
-		crtc = to_intel_crtc(dev_priv->pipe_to_crtc_mapping[pipe]);
-
-		if (crtc->cpu_fifo_underrun_disabled)
-			return false;
-	}
-
-	return true;
+	return INTEL_INFO(dev_priv)->gen >= 8 ? GEN8_GT_IER(2) : GEN6_PMIER;
 }
 
 /**
-  * bdw_update_pm_irq - update GT interrupt 2
+  * snb_update_pm_irq - update GEN6_PMIMR
   * @dev_priv: driver private
   * @interrupt_mask: mask of interrupt bits to update
   * @enabled_irq_mask: mask of interrupt bits to enable
-  *
-  * Copied from the snb function, updated with relevant register offsets
   */
-static void bdw_update_pm_irq(struct drm_i915_private *dev_priv,
+static void snb_update_pm_irq(struct drm_i915_private *dev_priv,
 			      uint32_t interrupt_mask,
 			      uint32_t enabled_irq_mask)
 {
@@ -264,144 +231,91 @@ static void bdw_update_pm_irq(struct drm_i915_private *dev_priv,
 
 	assert_spin_locked(&dev_priv->irq_lock);
 
-	if (WARN_ON(!intel_irqs_enabled(dev_priv)))
-		return;
-
 	new_val = dev_priv->pm_irq_mask;
 	new_val &= ~interrupt_mask;
 	new_val |= (~enabled_irq_mask & interrupt_mask);
 
 	if (new_val != dev_priv->pm_irq_mask) {
 		dev_priv->pm_irq_mask = new_val;
-		I915_WRITE(GEN8_GT_IMR(2), dev_priv->pm_irq_mask);
-		POSTING_READ(GEN8_GT_IMR(2));
+		I915_WRITE(gen6_pm_imr(dev_priv), dev_priv->pm_irq_mask);
+		POSTING_READ(gen6_pm_imr(dev_priv));
 	}
 }
 
-void gen8_enable_pm_irq(struct drm_i915_private *dev_priv, uint32_t mask)
+void gen6_enable_pm_irq(struct drm_i915_private *dev_priv, uint32_t mask)
 {
-	bdw_update_pm_irq(dev_priv, mask, mask);
+	if (WARN_ON(!intel_irqs_enabled(dev_priv)))
+		return;
+
+	snb_update_pm_irq(dev_priv, mask, mask);
 }
 
-void gen8_disable_pm_irq(struct drm_i915_private *dev_priv, uint32_t mask)
+static void __gen6_disable_pm_irq(struct drm_i915_private *dev_priv,
+				  uint32_t mask)
 {
-	bdw_update_pm_irq(dev_priv, mask, 0);
+	snb_update_pm_irq(dev_priv, mask, 0);
 }
 
-static bool cpt_can_enable_serr_int(struct drm_device *dev)
+void gen6_disable_pm_irq(struct drm_i915_private *dev_priv, uint32_t mask)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	enum pipe pipe;
-	struct intel_crtc *crtc;
-
-	assert_spin_locked(&dev_priv->irq_lock);
-
-	for_each_pipe(dev_priv, pipe) {
-		crtc = to_intel_crtc(dev_priv->pipe_to_crtc_mapping[pipe]);
-
-		if (crtc->pch_fifo_underrun_disabled)
-			return false;
-	}
+	if (WARN_ON(!intel_irqs_enabled(dev_priv)))
+		return;
 
-	return true;
+	__gen6_disable_pm_irq(dev_priv, mask);
 }
 
-void i9xx_check_fifo_underruns(struct drm_device *dev)
+void gen6_reset_rps_interrupts(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_crtc *crtc;
-	unsigned long flags;
-
-	spin_lock_irqsave(&dev_priv->irq_lock, flags);
-
-	for_each_intel_crtc(dev, crtc) {
-		u32 reg = PIPESTAT(crtc->pipe);
-		u32 pipestat;
-
-		if (crtc->cpu_fifo_underrun_disabled)
-			continue;
-
-		pipestat = I915_READ(reg) & 0xffff0000;
-		if ((pipestat & PIPE_FIFO_UNDERRUN_STATUS) == 0)
-			continue;
+	uint32_t reg = gen6_pm_iir(dev_priv);
 
-		I915_WRITE(reg, pipestat | PIPE_FIFO_UNDERRUN_STATUS);
-		POSTING_READ(reg);
-
-		DRM_ERROR("pipe %c underrun\n", pipe_name(crtc->pipe));
-	}
-
-	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
+	spin_lock_irq(&dev_priv->irq_lock);
+	I915_WRITE(reg, dev_priv->pm_rps_events);
+	I915_WRITE(reg, dev_priv->pm_rps_events);
+	POSTING_READ(reg);
+	spin_unlock_irq(&dev_priv->irq_lock);
 }
 
-static void i9xx_set_fifo_underrun_reporting(struct drm_device *dev,
-					     enum pipe pipe,
-					     bool enable, bool old)
+void gen6_enable_rps_interrupts(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 reg = PIPESTAT(pipe);
-	u32 pipestat = I915_READ(reg) & 0xffff0000;
-
-	assert_spin_locked(&dev_priv->irq_lock);
 
-	if (enable) {
-		I915_WRITE(reg, pipestat | PIPE_FIFO_UNDERRUN_STATUS);
-		POSTING_READ(reg);
-	} else {
-		if (old && pipestat & PIPE_FIFO_UNDERRUN_STATUS)
-			DRM_ERROR("pipe %c underrun\n", pipe_name(pipe));
-	}
-}
+	spin_lock_irq(&dev_priv->irq_lock);
 
-static void ironlake_set_fifo_underrun_reporting(struct drm_device *dev,
-						 enum pipe pipe, bool enable)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t bit = (pipe == PIPE_A) ? DE_PIPEA_FIFO_UNDERRUN :
-					  DE_PIPEB_FIFO_UNDERRUN;
+	WARN_ON(dev_priv->rps.pm_iir);
+	WARN_ON(I915_READ(gen6_pm_iir(dev_priv)) & dev_priv->pm_rps_events);
+	dev_priv->rps.interrupts_enabled = true;
+	I915_WRITE(gen6_pm_ier(dev_priv), I915_READ(gen6_pm_ier(dev_priv)) |
+				dev_priv->pm_rps_events);
+	gen6_enable_pm_irq(dev_priv, dev_priv->pm_rps_events);
 
-	if (enable)
-		ironlake_enable_display_irq(dev_priv, bit);
-	else
-		ironlake_disable_display_irq(dev_priv, bit);
+	spin_unlock_irq(&dev_priv->irq_lock);
 }
 
-static void ivybridge_set_fifo_underrun_reporting(struct drm_device *dev,
-						  enum pipe pipe,
-						  bool enable, bool old)
+void gen6_disable_rps_interrupts(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	if (enable) {
-		I915_WRITE(GEN7_ERR_INT, ERR_INT_FIFO_UNDERRUN(pipe));
 
-		if (!ivb_can_enable_err_int(dev))
-			return;
+	spin_lock_irq(&dev_priv->irq_lock);
+	dev_priv->rps.interrupts_enabled = false;
+	spin_unlock_irq(&dev_priv->irq_lock);
 
-		ironlake_enable_display_irq(dev_priv, DE_ERR_INT_IVB);
-	} else {
-		ironlake_disable_display_irq(dev_priv, DE_ERR_INT_IVB);
+	cancel_work_sync(&dev_priv->rps.work);
 
-		if (old &&
-		    I915_READ(GEN7_ERR_INT) & ERR_INT_FIFO_UNDERRUN(pipe)) {
-			DRM_ERROR("uncleared fifo underrun on pipe %c\n",
-				  pipe_name(pipe));
-		}
-	}
-}
+	spin_lock_irq(&dev_priv->irq_lock);
 
-static void broadwell_set_fifo_underrun_reporting(struct drm_device *dev,
-						  enum pipe pipe, bool enable)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	I915_WRITE(GEN6_PMINTRMSK, INTEL_INFO(dev_priv)->gen >= 8 ?
+		   ~GEN8_PMINTR_REDIRECT_TO_NON_DISP : ~0);
 
-	assert_spin_locked(&dev_priv->irq_lock);
+	__gen6_disable_pm_irq(dev_priv, dev_priv->pm_rps_events);
+	I915_WRITE(gen6_pm_ier(dev_priv), I915_READ(gen6_pm_ier(dev_priv)) &
+				~dev_priv->pm_rps_events);
+	I915_WRITE(gen6_pm_iir(dev_priv), dev_priv->pm_rps_events);
+	I915_WRITE(gen6_pm_iir(dev_priv), dev_priv->pm_rps_events);
 
-	if (enable)
-		dev_priv->de_irq_mask[pipe] &= ~GEN8_PIPE_FIFO_UNDERRUN;
-	else
-		dev_priv->de_irq_mask[pipe] |= GEN8_PIPE_FIFO_UNDERRUN;
-	I915_WRITE(GEN8_DE_PIPE_IMR(pipe), dev_priv->de_irq_mask[pipe]);
-	POSTING_READ(GEN8_DE_PIPE_IMR(pipe));
+	dev_priv->rps.pm_iir = 0;
+
+	spin_unlock_irq(&dev_priv->irq_lock);
 }
 
 /**
@@ -410,9 +324,9 @@ static void broadwell_set_fifo_underrun_reporting(struct drm_device *dev,
  * @interrupt_mask: mask of interrupt bits to update
  * @enabled_irq_mask: mask of interrupt bits to enable
  */
-static void ibx_display_interrupt_update(struct drm_i915_private *dev_priv,
-					 uint32_t interrupt_mask,
-					 uint32_t enabled_irq_mask)
+void ibx_display_interrupt_update(struct drm_i915_private *dev_priv,
+				  uint32_t interrupt_mask,
+				  uint32_t enabled_irq_mask)
 {
 	uint32_t sdeimr = I915_READ(SDEIMR);
 	sdeimr &= ~interrupt_mask;
@@ -426,160 +340,6 @@ static void ibx_display_interrupt_update(struct drm_i915_private *dev_priv,
 	I915_WRITE(SDEIMR, sdeimr);
 	POSTING_READ(SDEIMR);
 }
-#define ibx_enable_display_interrupt(dev_priv, bits) \
-	ibx_display_interrupt_update((dev_priv), (bits), (bits))
-#define ibx_disable_display_interrupt(dev_priv, bits) \
-	ibx_display_interrupt_update((dev_priv), (bits), 0)
-
-static void ibx_set_fifo_underrun_reporting(struct drm_device *dev,
-					    enum transcoder pch_transcoder,
-					    bool enable)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t bit = (pch_transcoder == TRANSCODER_A) ?
-		       SDE_TRANSA_FIFO_UNDER : SDE_TRANSB_FIFO_UNDER;
-
-	if (enable)
-		ibx_enable_display_interrupt(dev_priv, bit);
-	else
-		ibx_disable_display_interrupt(dev_priv, bit);
-}
-
-static void cpt_set_fifo_underrun_reporting(struct drm_device *dev,
-					    enum transcoder pch_transcoder,
-					    bool enable, bool old)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (enable) {
-		I915_WRITE(SERR_INT,
-			   SERR_INT_TRANS_FIFO_UNDERRUN(pch_transcoder));
-
-		if (!cpt_can_enable_serr_int(dev))
-			return;
-
-		ibx_enable_display_interrupt(dev_priv, SDE_ERROR_CPT);
-	} else {
-		ibx_disable_display_interrupt(dev_priv, SDE_ERROR_CPT);
-
-		if (old && I915_READ(SERR_INT) &
-		    SERR_INT_TRANS_FIFO_UNDERRUN(pch_transcoder)) {
-			DRM_ERROR("uncleared pch fifo underrun on pch transcoder %c\n",
-				  transcoder_name(pch_transcoder));
-		}
-	}
-}
-
-/**
- * intel_set_cpu_fifo_underrun_reporting - enable/disable FIFO underrun messages
- * @dev: drm device
- * @pipe: pipe
- * @enable: true if we want to report FIFO underrun errors, false otherwise
- *
- * This function makes us disable or enable CPU fifo underruns for a specific
- * pipe. Notice that on some Gens (e.g. IVB, HSW), disabling FIFO underrun
- * reporting for one pipe may also disable all the other CPU error interruts for
- * the other pipes, due to the fact that there's just one interrupt mask/enable
- * bit for all the pipes.
- *
- * Returns the previous state of underrun reporting.
- */
-static bool __intel_set_cpu_fifo_underrun_reporting(struct drm_device *dev,
-						    enum pipe pipe, bool enable)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc = dev_priv->pipe_to_crtc_mapping[pipe];
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	bool old;
-
-	assert_spin_locked(&dev_priv->irq_lock);
-
-	old = !intel_crtc->cpu_fifo_underrun_disabled;
-	intel_crtc->cpu_fifo_underrun_disabled = !enable;
-
-	if (HAS_GMCH_DISPLAY(dev))
-		i9xx_set_fifo_underrun_reporting(dev, pipe, enable, old);
-	else if (IS_GEN5(dev) || IS_GEN6(dev))
-		ironlake_set_fifo_underrun_reporting(dev, pipe, enable);
-	else if (IS_GEN7(dev))
-		ivybridge_set_fifo_underrun_reporting(dev, pipe, enable, old);
-	else if (IS_GEN8(dev))
-		broadwell_set_fifo_underrun_reporting(dev, pipe, enable);
-
-	return old;
-}
-
-bool intel_set_cpu_fifo_underrun_reporting(struct drm_device *dev,
-					   enum pipe pipe, bool enable)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	unsigned long flags;
-	bool ret;
-
-	spin_lock_irqsave(&dev_priv->irq_lock, flags);
-	ret = __intel_set_cpu_fifo_underrun_reporting(dev, pipe, enable);
-	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
-
-	return ret;
-}
-
-static bool __cpu_fifo_underrun_reporting_enabled(struct drm_device *dev,
-						  enum pipe pipe)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc = dev_priv->pipe_to_crtc_mapping[pipe];
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-
-	return !intel_crtc->cpu_fifo_underrun_disabled;
-}
-
-/**
- * intel_set_pch_fifo_underrun_reporting - enable/disable FIFO underrun messages
- * @dev: drm device
- * @pch_transcoder: the PCH transcoder (same as pipe on IVB and older)
- * @enable: true if we want to report FIFO underrun errors, false otherwise
- *
- * This function makes us disable or enable PCH fifo underruns for a specific
- * PCH transcoder. Notice that on some PCHs (e.g. CPT/PPT), disabling FIFO
- * underrun reporting for one transcoder may also disable all the other PCH
- * error interruts for the other transcoders, due to the fact that there's just
- * one interrupt mask/enable bit for all the transcoders.
- *
- * Returns the previous state of underrun reporting.
- */
-bool intel_set_pch_fifo_underrun_reporting(struct drm_device *dev,
-					   enum transcoder pch_transcoder,
-					   bool enable)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc = dev_priv->pipe_to_crtc_mapping[pch_transcoder];
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	unsigned long flags;
-	bool old;
-
-	/*
-	 * NOTE: Pre-LPT has a fixed cpu pipe -> pch transcoder mapping, but LPT
-	 * has only one pch transcoder A that all pipes can use. To avoid racy
-	 * pch transcoder -> pipe lookups from interrupt code simply store the
-	 * underrun statistics in crtc A. Since we never expose this anywhere
-	 * nor use it outside of the fifo underrun code here using the "wrong"
-	 * crtc on LPT won't cause issues.
-	 */
-
-	spin_lock_irqsave(&dev_priv->irq_lock, flags);
-
-	old = !intel_crtc->pch_fifo_underrun_disabled;
-	intel_crtc->pch_fifo_underrun_disabled = !enable;
-
-	if (HAS_PCH_IBX(dev))
-		ibx_set_fifo_underrun_reporting(dev, pch_transcoder, enable);
-	else
-		cpt_set_fifo_underrun_reporting(dev, pch_transcoder, enable, old);
-
-	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
-	return old;
-}
-
 
 static void
 __i915_enable_pipestat(struct drm_i915_private *dev_priv, enum pipe pipe,
@@ -589,6 +349,7 @@ __i915_enable_pipestat(struct drm_i915_private *dev_priv, enum pipe pipe,
 	u32 pipestat = I915_READ(reg) & PIPESTAT_INT_ENABLE_MASK;
 
 	assert_spin_locked(&dev_priv->irq_lock);
+	WARN_ON(!intel_irqs_enabled(dev_priv));
 
 	if (WARN_ONCE(enable_mask & ~PIPESTAT_INT_ENABLE_MASK ||
 		      status_mask & ~PIPESTAT_INT_STATUS_MASK,
@@ -615,6 +376,7 @@ __i915_disable_pipestat(struct drm_i915_private *dev_priv, enum pipe pipe,
 	u32 pipestat = I915_READ(reg) & PIPESTAT_INT_ENABLE_MASK;
 
 	assert_spin_locked(&dev_priv->irq_lock);
+	WARN_ON(!intel_irqs_enabled(dev_priv));
 
 	if (WARN_ONCE(enable_mask & ~PIPESTAT_INT_ENABLE_MASK ||
 		      status_mask & ~PIPESTAT_INT_STATUS_MASK,
@@ -694,19 +456,18 @@ i915_disable_pipestat(struct drm_i915_private *dev_priv, enum pipe pipe,
 static void i915_enable_asle_pipestat(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	unsigned long irqflags;
 
 	if (!dev_priv->opregion.asle || !IS_MOBILE(dev))
 		return;
 
-	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+	spin_lock_irq(&dev_priv->irq_lock);
 
 	i915_enable_pipestat(dev_priv, PIPE_B, PIPE_LEGACY_BLC_EVENT_STATUS);
 	if (INTEL_INFO(dev)->gen >= 4)
 		i915_enable_pipestat(dev_priv, PIPE_A,
 				     PIPE_LEGACY_BLC_EVENT_STATUS);
 
-	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+	spin_unlock_irq(&dev_priv->irq_lock);
 }
 
 /**
@@ -1094,18 +855,17 @@ static void i915_digport_work_func(struct work_struct *work)
 {
 	struct drm_i915_private *dev_priv =
 		container_of(work, struct drm_i915_private, dig_port_work);
-	unsigned long irqflags;
 	u32 long_port_mask, short_port_mask;
 	struct intel_digital_port *intel_dig_port;
 	int i, ret;
 	u32 old_bits = 0;
 
-	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+	spin_lock_irq(&dev_priv->irq_lock);
 	long_port_mask = dev_priv->long_hpd_port_mask;
 	dev_priv->long_hpd_port_mask = 0;
 	short_port_mask = dev_priv->short_hpd_port_mask;
 	dev_priv->short_hpd_port_mask = 0;
-	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+	spin_unlock_irq(&dev_priv->irq_lock);
 
 	for (i = 0; i < I915_MAX_PORTS; i++) {
 		bool valid = false;
@@ -1130,9 +890,9 @@ static void i915_digport_work_func(struct work_struct *work)
 	}
 
 	if (old_bits) {
-		spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+		spin_lock_irq(&dev_priv->irq_lock);
 		dev_priv->hpd_event_bits |= old_bits;
-		spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+		spin_unlock_irq(&dev_priv->irq_lock);
 		schedule_work(&dev_priv->hotplug_work);
 	}
 }
@@ -1151,7 +911,6 @@ static void i915_hotplug_work_func(struct work_struct *work)
 	struct intel_connector *intel_connector;
 	struct intel_encoder *intel_encoder;
 	struct drm_connector *connector;
-	unsigned long irqflags;
 	bool hpd_disabled = false;
 	bool changed = false;
 	u32 hpd_event_bits;
@@ -1159,7 +918,7 @@ static void i915_hotplug_work_func(struct work_struct *work)
 	mutex_lock(&mode_config->mutex);
 	DRM_DEBUG_KMS("running encoder hotplug functions\n");
 
-	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+	spin_lock_irq(&dev_priv->irq_lock);
 
 	hpd_event_bits = dev_priv->hpd_event_bits;
 	dev_priv->hpd_event_bits = 0;
@@ -1193,7 +952,7 @@ static void i915_hotplug_work_func(struct work_struct *work)
 				 msecs_to_jiffies(I915_REENABLE_HOTPLUG_DELAY));
 	}
 
-	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+	spin_unlock_irq(&dev_priv->irq_lock);
 
 	list_for_each_entry(connector, &mode_config->connector_list, head) {
 		intel_connector = to_intel_connector(connector);
@@ -1260,11 +1019,7 @@ static void notify_ring(struct drm_device *dev,
 
 	trace_i915_gem_request_complete(ring);
 
-	if (drm_core_check_feature(dev, DRIVER_MODESET))
-		intel_notify_mmio_flip(ring);
-
 	wake_up_all(&ring->irq_queue);
-	i915_queue_hangcheck(dev);
 }
 
 static u32 vlv_c0_residency(struct drm_i915_private *dev_priv,
@@ -1400,14 +1155,15 @@ static void gen6_pm_rps_work(struct work_struct *work)
 	int new_delay, adj;
 
 	spin_lock_irq(&dev_priv->irq_lock);
+	/* Speed up work cancelation during disabling rps interrupts. */
+	if (!dev_priv->rps.interrupts_enabled) {
+		spin_unlock_irq(&dev_priv->irq_lock);
+		return;
+	}
 	pm_iir = dev_priv->rps.pm_iir;
 	dev_priv->rps.pm_iir = 0;
-	if (INTEL_INFO(dev_priv->dev)->gen >= 8)
-		gen8_enable_pm_irq(dev_priv, dev_priv->pm_rps_events);
-	else {
-		/* Make sure not to corrupt PMIMR state used by ringbuffer */
-		gen6_enable_pm_irq(dev_priv, dev_priv->pm_rps_events);
-	}
+	/* Make sure not to corrupt PMIMR state used by ringbuffer on GEN6 */
+	gen6_enable_pm_irq(dev_priv, dev_priv->pm_rps_events);
 	spin_unlock_irq(&dev_priv->irq_lock);
 
 	/* Make sure we didn't queue anything we're not going to process. */
@@ -1488,7 +1244,6 @@ static void ivybridge_parity_work(struct work_struct *work)
 	u32 error_status, row, bank, subbank;
 	char *parity_event[6];
 	uint32_t misccpctl;
-	unsigned long flags;
 	uint8_t slice = 0;
 
 	/* We must turn off DOP level clock gating to access the L3 registers.
@@ -1547,9 +1302,9 @@ static void ivybridge_parity_work(struct work_struct *work)
 
 out:
 	WARN_ON(dev_priv->l3_parity.which_slice);
-	spin_lock_irqsave(&dev_priv->irq_lock, flags);
+	spin_lock_irq(&dev_priv->irq_lock);
 	gen5_enable_gt_irq(dev_priv, GT_PARITY_ERROR(dev_priv->dev));
-	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
+	spin_unlock_irq(&dev_priv->irq_lock);
 
 	mutex_unlock(&dev_priv->dev->struct_mutex);
 }
@@ -1601,28 +1356,13 @@ static void snb_gt_irq_handler(struct drm_device *dev,
 
 	if (gt_iir & (GT_BLT_CS_ERROR_INTERRUPT |
 		      GT_BSD_CS_ERROR_INTERRUPT |
-		      GT_RENDER_CS_MASTER_ERROR_INTERRUPT)) {
-		i915_handle_error(dev, false, "GT error interrupt 0x%08x",
-				  gt_iir);
-	}
+		      GT_RENDER_CS_MASTER_ERROR_INTERRUPT))
+		DRM_DEBUG("Command parser error, gt_iir 0x%08x\n", gt_iir);
 
 	if (gt_iir & GT_PARITY_ERROR(dev))
 		ivybridge_parity_error_irq_handler(dev, gt_iir);
 }
 
-static void gen8_rps_irq_handler(struct drm_i915_private *dev_priv, u32 pm_iir)
-{
-	if ((pm_iir & dev_priv->pm_rps_events) == 0)
-		return;
-
-	spin_lock(&dev_priv->irq_lock);
-	dev_priv->rps.pm_iir |= pm_iir & dev_priv->pm_rps_events;
-	gen8_disable_pm_irq(dev_priv, pm_iir & dev_priv->pm_rps_events);
-	spin_unlock(&dev_priv->irq_lock);
-
-	queue_work(dev_priv->wq, &dev_priv->rps.work);
-}
-
 static irqreturn_t gen8_gt_irq_handler(struct drm_device *dev,
 				       struct drm_i915_private *dev_priv,
 				       u32 master_ctl)
@@ -1684,7 +1424,7 @@ static irqreturn_t gen8_gt_irq_handler(struct drm_device *dev,
 			I915_WRITE(GEN8_GT_IIR(2),
 				   tmp & dev_priv->pm_rps_events);
 			ret = IRQ_HANDLED;
-			gen8_rps_irq_handler(dev_priv, tmp);
+			gen6_rps_irq_handler(dev_priv, tmp);
 		} else
 			DRM_ERROR("The master control interrupt lied (PM)!\n");
 	}
@@ -1898,7 +1638,7 @@ static void display_pipe_crc_irq_handler(struct drm_device *dev, enum pipe pipe,
 
 	if (!pipe_crc->entries) {
 		spin_unlock(&pipe_crc->lock);
-		DRM_ERROR("spurious interrupt\n");
+		DRM_DEBUG_KMS("spurious interrupt\n");
 		return;
 	}
 
@@ -1984,24 +1724,30 @@ static void i9xx_pipe_crc_irq_handler(struct drm_device *dev, enum pipe pipe)
  * the work queue. */
 static void gen6_rps_irq_handler(struct drm_i915_private *dev_priv, u32 pm_iir)
 {
+	/* TODO: RPS on GEN9+ is not supported yet. */
+	if (WARN_ONCE(INTEL_INFO(dev_priv)->gen >= 9,
+		      "GEN9+: unexpected RPS IRQ\n"))
+		return;
+
 	if (pm_iir & dev_priv->pm_rps_events) {
 		spin_lock(&dev_priv->irq_lock);
-		dev_priv->rps.pm_iir |= pm_iir & dev_priv->pm_rps_events;
 		gen6_disable_pm_irq(dev_priv, pm_iir & dev_priv->pm_rps_events);
+		if (dev_priv->rps.interrupts_enabled) {
+			dev_priv->rps.pm_iir |= pm_iir & dev_priv->pm_rps_events;
+			queue_work(dev_priv->wq, &dev_priv->rps.work);
+		}
 		spin_unlock(&dev_priv->irq_lock);
-
-		queue_work(dev_priv->wq, &dev_priv->rps.work);
 	}
 
+	if (INTEL_INFO(dev_priv)->gen >= 8)
+		return;
+
 	if (HAS_VEBOX(dev_priv->dev)) {
 		if (pm_iir & PM_VEBOX_USER_INTERRUPT)
 			notify_ring(dev_priv->dev, &dev_priv->ring[VECS]);
 
-		if (pm_iir & PM_VEBOX_CS_ERROR_INTERRUPT) {
-			i915_handle_error(dev_priv->dev, false,
-					  "VEBOX CS error interrupt 0x%08x",
-					  pm_iir);
-		}
+		if (pm_iir & PM_VEBOX_CS_ERROR_INTERRUPT)
+			DRM_DEBUG("Command parser error, pm_iir 0x%08x\n", pm_iir);
 	}
 }
 
@@ -2031,9 +1777,9 @@ static void valleyview_pipestat_irq_handler(struct drm_device *dev, u32 iir)
 		 * we need to be careful that we only handle what we want to
 		 * handle.
 		 */
-		mask = 0;
-		if (__cpu_fifo_underrun_reporting_enabled(dev, pipe))
-			mask |= PIPE_FIFO_UNDERRUN_STATUS;
+
+		/* fifo underruns are filterered in the underrun handler. */
+		mask = PIPE_FIFO_UNDERRUN_STATUS;
 
 		switch (pipe) {
 		case PIPE_A:
@@ -2078,9 +1824,8 @@ static void valleyview_pipestat_irq_handler(struct drm_device *dev, u32 iir)
 		if (pipe_stats[pipe] & PIPE_CRC_DONE_INTERRUPT_STATUS)
 			i9xx_pipe_crc_irq_handler(dev, pipe);
 
-		if (pipe_stats[pipe] & PIPE_FIFO_UNDERRUN_STATUS &&
-		    intel_set_cpu_fifo_underrun_reporting(dev, pipe, false))
-			DRM_ERROR("pipe %c underrun\n", pipe_name(pipe));
+		if (pipe_stats[pipe] & PIPE_FIFO_UNDERRUN_STATUS)
+			intel_cpu_fifo_underrun_irq_handler(dev_priv, pipe);
 	}
 
 	if (pipe_stats[0] & PIPE_GMBUS_INTERRUPT_STATUS)
@@ -2247,14 +1992,10 @@ static void ibx_irq_handler(struct drm_device *dev, u32 pch_iir)
 		DRM_DEBUG_DRIVER("PCH transcoder CRC error interrupt\n");
 
 	if (pch_iir & SDE_TRANSA_FIFO_UNDER)
-		if (intel_set_pch_fifo_underrun_reporting(dev, TRANSCODER_A,
-							  false))
-			DRM_ERROR("PCH transcoder A FIFO underrun\n");
+		intel_pch_fifo_underrun_irq_handler(dev_priv, TRANSCODER_A);
 
 	if (pch_iir & SDE_TRANSB_FIFO_UNDER)
-		if (intel_set_pch_fifo_underrun_reporting(dev, TRANSCODER_B,
-							  false))
-			DRM_ERROR("PCH transcoder B FIFO underrun\n");
+		intel_pch_fifo_underrun_irq_handler(dev_priv, TRANSCODER_B);
 }
 
 static void ivb_err_int_handler(struct drm_device *dev)
@@ -2267,12 +2008,8 @@ static void ivb_err_int_handler(struct drm_device *dev)
 		DRM_ERROR("Poison interrupt\n");
 
 	for_each_pipe(dev_priv, pipe) {
-		if (err_int & ERR_INT_FIFO_UNDERRUN(pipe)) {
-			if (intel_set_cpu_fifo_underrun_reporting(dev, pipe,
-								  false))
-				DRM_ERROR("Pipe %c FIFO underrun\n",
-					  pipe_name(pipe));
-		}
+		if (err_int & ERR_INT_FIFO_UNDERRUN(pipe))
+			intel_cpu_fifo_underrun_irq_handler(dev_priv, pipe);
 
 		if (err_int & ERR_INT_PIPE_CRC_DONE(pipe)) {
 			if (IS_IVYBRIDGE(dev))
@@ -2294,19 +2031,13 @@ static void cpt_serr_int_handler(struct drm_device *dev)
 		DRM_ERROR("PCH poison interrupt\n");
 
 	if (serr_int & SERR_INT_TRANS_A_FIFO_UNDERRUN)
-		if (intel_set_pch_fifo_underrun_reporting(dev, TRANSCODER_A,
-							  false))
-			DRM_ERROR("PCH transcoder A FIFO underrun\n");
+		intel_pch_fifo_underrun_irq_handler(dev_priv, TRANSCODER_A);
 
 	if (serr_int & SERR_INT_TRANS_B_FIFO_UNDERRUN)
-		if (intel_set_pch_fifo_underrun_reporting(dev, TRANSCODER_B,
-							  false))
-			DRM_ERROR("PCH transcoder B FIFO underrun\n");
+		intel_pch_fifo_underrun_irq_handler(dev_priv, TRANSCODER_B);
 
 	if (serr_int & SERR_INT_TRANS_C_FIFO_UNDERRUN)
-		if (intel_set_pch_fifo_underrun_reporting(dev, TRANSCODER_C,
-							  false))
-			DRM_ERROR("PCH transcoder C FIFO underrun\n");
+		intel_pch_fifo_underrun_irq_handler(dev_priv, TRANSCODER_C);
 
 	I915_WRITE(SERR_INT, serr_int);
 }
@@ -2372,9 +2103,7 @@ static void ilk_display_irq_handler(struct drm_device *dev, u32 de_iir)
 			intel_check_page_flip(dev, pipe);
 
 		if (de_iir & DE_PIPE_FIFO_UNDERRUN(pipe))
-			if (intel_set_cpu_fifo_underrun_reporting(dev, pipe, false))
-				DRM_ERROR("Pipe %c FIFO underrun\n",
-					  pipe_name(pipe));
+			intel_cpu_fifo_underrun_irq_handler(dev_priv, pipe);
 
 		if (de_iir & DE_PIPE_CRC_DONE(pipe))
 			i9xx_pipe_crc_irq_handler(dev, pipe);
@@ -2524,6 +2253,11 @@ static irqreturn_t gen8_irq_handler(int irq, void *arg)
 	irqreturn_t ret = IRQ_NONE;
 	uint32_t tmp = 0;
 	enum pipe pipe;
+	u32 aux_mask = GEN8_AUX_CHANNEL_A;
+
+	if (IS_GEN9(dev))
+		aux_mask |=  GEN9_AUX_CHANNEL_B | GEN9_AUX_CHANNEL_C |
+			GEN9_AUX_CHANNEL_D;
 
 	master_ctl = I915_READ(GEN8_MASTER_IRQ);
 	master_ctl &= ~GEN8_MASTER_IRQ_CONTROL;
@@ -2556,7 +2290,8 @@ static irqreturn_t gen8_irq_handler(int irq, void *arg)
 		if (tmp) {
 			I915_WRITE(GEN8_DE_PORT_IIR, tmp);
 			ret = IRQ_HANDLED;
-			if (tmp & GEN8_AUX_CHANNEL_A)
+
+			if (tmp & aux_mask)
 				dp_aux_irq_handler(dev);
 			else
 				DRM_ERROR("Unexpected DE Port interrupt\n");
@@ -2566,7 +2301,7 @@ static irqreturn_t gen8_irq_handler(int irq, void *arg)
 	}
 
 	for_each_pipe(dev_priv, pipe) {
-		uint32_t pipe_iir;
+		uint32_t pipe_iir, flip_done = 0, fault_errors = 0;
 
 		if (!(master_ctl & GEN8_DE_PIPE_IRQ(pipe)))
 			continue;
@@ -2575,11 +2310,17 @@ static irqreturn_t gen8_irq_handler(int irq, void *arg)
 		if (pipe_iir) {
 			ret = IRQ_HANDLED;
 			I915_WRITE(GEN8_DE_PIPE_IIR(pipe), pipe_iir);
+
 			if (pipe_iir & GEN8_PIPE_VBLANK &&
 			    intel_pipe_handle_vblank(dev, pipe))
 				intel_check_page_flip(dev, pipe);
 
-			if (pipe_iir & GEN8_PIPE_PRIMARY_FLIP_DONE) {
+			if (IS_GEN9(dev))
+				flip_done = pipe_iir & GEN9_PIPE_PLANE1_FLIP_DONE;
+			else
+				flip_done = pipe_iir & GEN8_PIPE_PRIMARY_FLIP_DONE;
+
+			if (flip_done) {
 				intel_prepare_page_flip(dev, pipe);
 				intel_finish_page_flip_plane(dev, pipe);
 			}
@@ -2587,18 +2328,20 @@ static irqreturn_t gen8_irq_handler(int irq, void *arg)
 			if (pipe_iir & GEN8_PIPE_CDCLK_CRC_DONE)
 				hsw_pipe_crc_irq_handler(dev, pipe);
 
-			if (pipe_iir & GEN8_PIPE_FIFO_UNDERRUN) {
-				if (intel_set_cpu_fifo_underrun_reporting(dev, pipe,
-									  false))
-					DRM_ERROR("Pipe %c FIFO underrun\n",
-						  pipe_name(pipe));
-			}
+			if (pipe_iir & GEN8_PIPE_FIFO_UNDERRUN)
+				intel_cpu_fifo_underrun_irq_handler(dev_priv,
+								    pipe);
+
+
+			if (IS_GEN9(dev))
+				fault_errors = pipe_iir & GEN9_DE_PIPE_IRQ_FAULT_ERRORS;
+			else
+				fault_errors = pipe_iir & GEN8_DE_PIPE_IRQ_FAULT_ERRORS;
 
-			if (pipe_iir & GEN8_DE_PIPE_IRQ_FAULT_ERRORS) {
+			if (fault_errors)
 				DRM_ERROR("Fault errors on pipe %c\n: 0x%08x",
 					  pipe_name(pipe),
 					  pipe_iir & GEN8_DE_PIPE_IRQ_FAULT_ERRORS);
-			}
 		} else
 			DRM_ERROR("The master control interrupt lied (DE PIPE)!\n");
 	}
@@ -2697,6 +2440,9 @@ static void i915_error_work_func(struct work_struct *work)
 		 * simulated reset via debugs, so get an RPM reference.
 		 */
 		intel_runtime_pm_get(dev_priv);
+
+		intel_prepare_reset(dev);
+
 		/*
 		 * All state reset _must_ be completed before we update the
 		 * reset counter, for otherwise waiters might miss the reset
@@ -2705,7 +2451,7 @@ static void i915_error_work_func(struct work_struct *work)
 		 */
 		ret = i915_reset(dev);
 
-		intel_display_handle_reset(dev);
+		intel_finish_reset(dev);
 
 		intel_runtime_pm_put(dev_priv);
 
@@ -3330,10 +3076,15 @@ static void i915_hangcheck_elapsed(unsigned long data)
 void i915_queue_hangcheck(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct timer_list *timer = &dev_priv->gpu_error.hangcheck_timer;
+
 	if (!i915.enable_hangcheck)
 		return;
 
-	mod_timer(&dev_priv->gpu_error.hangcheck_timer,
+	/* Don't continually defer the hangcheck, but make sure it is active */
+	if (timer_pending(timer))
+		return;
+	mod_timer(timer,
 		  round_jiffies_up(jiffies + DRM_I915_HANGCHECK_JIFFIES));
 }
 
@@ -3396,10 +3147,22 @@ static void ironlake_irq_reset(struct drm_device *dev)
 	ibx_irq_reset(dev);
 }
 
+static void vlv_display_irq_reset(struct drm_i915_private *dev_priv)
+{
+	enum pipe pipe;
+
+	I915_WRITE(PORT_HOTPLUG_EN, 0);
+	I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
+
+	for_each_pipe(dev_priv, pipe)
+		I915_WRITE(PIPESTAT(pipe), 0xffff);
+
+	GEN5_IRQ_RESET(VLV_);
+}
+
 static void valleyview_irq_preinstall(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	int pipe;
 
 	/* VLV magic */
 	I915_WRITE(VLV_IMR, 0);
@@ -3407,22 +3170,11 @@ static void valleyview_irq_preinstall(struct drm_device *dev)
 	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));
-
 	gen5_gt_irq_reset(dev);
 
-	I915_WRITE(DPINVGTT, 0xff);
+	I915_WRITE(DPINVGTT, DPINVGTT_STATUS_MASK);
 
-	I915_WRITE(PORT_HOTPLUG_EN, 0);
-	I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
-	for_each_pipe(dev_priv, 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);
+	vlv_display_irq_reset(dev_priv);
 }
 
 static void gen8_gt_irq_reset(struct drm_i915_private *dev_priv)
@@ -3444,8 +3196,8 @@ static void gen8_irq_reset(struct drm_device *dev)
 	gen8_gt_irq_reset(dev_priv);
 
 	for_each_pipe(dev_priv, pipe)
-		if (intel_display_power_enabled(dev_priv,
-						POWER_DOMAIN_PIPE(pipe)))
+		if (intel_display_power_is_enabled(dev_priv,
+						   POWER_DOMAIN_PIPE(pipe)))
 			GEN8_IRQ_RESET_NDX(DE_PIPE, pipe);
 
 	GEN5_IRQ_RESET(GEN8_DE_PORT_);
@@ -3457,21 +3209,19 @@ static void gen8_irq_reset(struct drm_device *dev)
 
 void gen8_irq_power_well_post_enable(struct drm_i915_private *dev_priv)
 {
-	unsigned long irqflags;
 	uint32_t extra_ier = GEN8_PIPE_VBLANK | GEN8_PIPE_FIFO_UNDERRUN;
 
-	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+	spin_lock_irq(&dev_priv->irq_lock);
 	GEN8_IRQ_INIT_NDX(DE_PIPE, PIPE_B, dev_priv->de_irq_mask[PIPE_B],
 			  ~dev_priv->de_irq_mask[PIPE_B] | extra_ier);
 	GEN8_IRQ_INIT_NDX(DE_PIPE, PIPE_C, dev_priv->de_irq_mask[PIPE_C],
 			  ~dev_priv->de_irq_mask[PIPE_C] | extra_ier);
-	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+	spin_unlock_irq(&dev_priv->irq_lock);
 }
 
 static void cherryview_irq_preinstall(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	int pipe;
 
 	I915_WRITE(GEN8_MASTER_IRQ, 0);
 	POSTING_READ(GEN8_MASTER_IRQ);
@@ -3480,20 +3230,9 @@ static void cherryview_irq_preinstall(struct drm_device *dev)
 
 	GEN5_IRQ_RESET(GEN8_PCU_);
 
-	POSTING_READ(GEN8_PCU_IIR);
-
 	I915_WRITE(DPINVGTT, DPINVGTT_STATUS_MASK_CHV);
 
-	I915_WRITE(PORT_HOTPLUG_EN, 0);
-	I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
-
-	for_each_pipe(dev_priv, pipe)
-		I915_WRITE(PIPESTAT(pipe), 0xffff);
-
-	I915_WRITE(VLV_IMR, 0xffffffff);
-	I915_WRITE(VLV_IER, 0x0);
-	I915_WRITE(VLV_IIR, 0xffffffff);
-	POSTING_READ(VLV_IIR);
+	vlv_display_irq_reset(dev_priv);
 }
 
 static void ibx_hpd_irq_setup(struct drm_device *dev)
@@ -3572,8 +3311,10 @@ static void gen5_gt_irq_postinstall(struct drm_device *dev)
 	GEN5_IRQ_INIT(GT, dev_priv->gt_irq_mask, gt_irqs);
 
 	if (INTEL_INFO(dev)->gen >= 6) {
-		pm_irqs |= dev_priv->pm_rps_events;
-
+		/*
+		 * RPS interrupts will get enabled/disabled on demand when RPS
+		 * itself is enabled/disabled.
+		 */
 		if (HAS_VEBOX(dev))
 			pm_irqs |= PM_VEBOX_USER_INTERRUPT;
 
@@ -3584,7 +3325,6 @@ static void gen5_gt_irq_postinstall(struct drm_device *dev)
 
 static int ironlake_irq_postinstall(struct drm_device *dev)
 {
-	unsigned long irqflags;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 display_mask, extra_mask;
 
@@ -3623,9 +3363,9 @@ static int ironlake_irq_postinstall(struct drm_device *dev)
 		 * spinlocking not required here for correctness since interrupt
 		 * setup is guaranteed to run in single-threaded context. But we
 		 * need it to make the assert_spin_locked happy. */
-		spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+		spin_lock_irq(&dev_priv->irq_lock);
 		ironlake_enable_display_irq(dev_priv, DE_PCU_EVENT);
-		spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+		spin_unlock_irq(&dev_priv->irq_lock);
 	}
 
 	return 0;
@@ -3635,45 +3375,51 @@ static void valleyview_display_irqs_install(struct drm_i915_private *dev_priv)
 {
 	u32 pipestat_mask;
 	u32 iir_mask;
+	enum pipe pipe;
 
 	pipestat_mask = PIPESTAT_INT_STATUS_MASK |
 			PIPE_FIFO_UNDERRUN_STATUS;
 
-	I915_WRITE(PIPESTAT(PIPE_A), pipestat_mask);
-	I915_WRITE(PIPESTAT(PIPE_B), pipestat_mask);
+	for_each_pipe(dev_priv, pipe)
+		I915_WRITE(PIPESTAT(pipe), pipestat_mask);
 	POSTING_READ(PIPESTAT(PIPE_A));
 
 	pipestat_mask = PLANE_FLIP_DONE_INT_STATUS_VLV |
 			PIPE_CRC_DONE_INTERRUPT_STATUS;
 
-	i915_enable_pipestat(dev_priv, PIPE_A, pipestat_mask |
-					       PIPE_GMBUS_INTERRUPT_STATUS);
-	i915_enable_pipestat(dev_priv, PIPE_B, pipestat_mask);
+	i915_enable_pipestat(dev_priv, PIPE_A, PIPE_GMBUS_INTERRUPT_STATUS);
+	for_each_pipe(dev_priv, pipe)
+		      i915_enable_pipestat(dev_priv, pipe, pipestat_mask);
 
 	iir_mask = I915_DISPLAY_PORT_INTERRUPT |
 		   I915_DISPLAY_PIPE_A_EVENT_INTERRUPT |
 		   I915_DISPLAY_PIPE_B_EVENT_INTERRUPT;
+	if (IS_CHERRYVIEW(dev_priv))
+		iir_mask |= I915_DISPLAY_PIPE_C_EVENT_INTERRUPT;
 	dev_priv->irq_mask &= ~iir_mask;
 
 	I915_WRITE(VLV_IIR, iir_mask);
 	I915_WRITE(VLV_IIR, iir_mask);
-	I915_WRITE(VLV_IMR, dev_priv->irq_mask);
 	I915_WRITE(VLV_IER, ~dev_priv->irq_mask);
-	POSTING_READ(VLV_IER);
+	I915_WRITE(VLV_IMR, dev_priv->irq_mask);
+	POSTING_READ(VLV_IMR);
 }
 
 static void valleyview_display_irqs_uninstall(struct drm_i915_private *dev_priv)
 {
 	u32 pipestat_mask;
 	u32 iir_mask;
+	enum pipe pipe;
 
 	iir_mask = I915_DISPLAY_PORT_INTERRUPT |
 		   I915_DISPLAY_PIPE_A_EVENT_INTERRUPT |
 		   I915_DISPLAY_PIPE_B_EVENT_INTERRUPT;
+	if (IS_CHERRYVIEW(dev_priv))
+		iir_mask |= I915_DISPLAY_PIPE_C_EVENT_INTERRUPT;
 
 	dev_priv->irq_mask |= iir_mask;
-	I915_WRITE(VLV_IER, ~dev_priv->irq_mask);
 	I915_WRITE(VLV_IMR, dev_priv->irq_mask);
+	I915_WRITE(VLV_IER, ~dev_priv->irq_mask);
 	I915_WRITE(VLV_IIR, iir_mask);
 	I915_WRITE(VLV_IIR, iir_mask);
 	POSTING_READ(VLV_IIR);
@@ -3681,14 +3427,15 @@ static void valleyview_display_irqs_uninstall(struct drm_i915_private *dev_priv)
 	pipestat_mask = PLANE_FLIP_DONE_INT_STATUS_VLV |
 			PIPE_CRC_DONE_INTERRUPT_STATUS;
 
-	i915_disable_pipestat(dev_priv, PIPE_A, pipestat_mask |
-					        PIPE_GMBUS_INTERRUPT_STATUS);
-	i915_disable_pipestat(dev_priv, PIPE_B, pipestat_mask);
+	i915_disable_pipestat(dev_priv, PIPE_A, PIPE_GMBUS_INTERRUPT_STATUS);
+	for_each_pipe(dev_priv, pipe)
+		i915_disable_pipestat(dev_priv, pipe, pipestat_mask);
 
 	pipestat_mask = PIPESTAT_INT_STATUS_MASK |
 			PIPE_FIFO_UNDERRUN_STATUS;
-	I915_WRITE(PIPESTAT(PIPE_A), pipestat_mask);
-	I915_WRITE(PIPESTAT(PIPE_B), pipestat_mask);
+
+	for_each_pipe(dev_priv, pipe)
+		I915_WRITE(PIPESTAT(pipe), pipestat_mask);
 	POSTING_READ(PIPESTAT(PIPE_A));
 }
 
@@ -3701,7 +3448,7 @@ void valleyview_enable_display_irqs(struct drm_i915_private *dev_priv)
 
 	dev_priv->display_irqs_enabled = true;
 
-	if (dev_priv->dev->irq_enabled)
+	if (intel_irqs_enabled(dev_priv))
 		valleyview_display_irqs_install(dev_priv);
 }
 
@@ -3714,34 +3461,36 @@ void valleyview_disable_display_irqs(struct drm_i915_private *dev_priv)
 
 	dev_priv->display_irqs_enabled = false;
 
-	if (dev_priv->dev->irq_enabled)
+	if (intel_irqs_enabled(dev_priv))
 		valleyview_display_irqs_uninstall(dev_priv);
 }
 
-static int valleyview_irq_postinstall(struct drm_device *dev)
+static void vlv_display_irq_postinstall(struct drm_i915_private *dev_priv)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	unsigned long irqflags;
-
 	dev_priv->irq_mask = ~0;
 
 	I915_WRITE(PORT_HOTPLUG_EN, 0);
 	POSTING_READ(PORT_HOTPLUG_EN);
 
-	I915_WRITE(VLV_IMR, dev_priv->irq_mask);
-	I915_WRITE(VLV_IER, ~dev_priv->irq_mask);
 	I915_WRITE(VLV_IIR, 0xffffffff);
-	POSTING_READ(VLV_IER);
+	I915_WRITE(VLV_IIR, 0xffffffff);
+	I915_WRITE(VLV_IER, ~dev_priv->irq_mask);
+	I915_WRITE(VLV_IMR, dev_priv->irq_mask);
+	POSTING_READ(VLV_IMR);
 
 	/* Interrupt setup is already guaranteed to be single-threaded, this is
 	 * just to make the assert_spin_locked check happy. */
-	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+	spin_lock_irq(&dev_priv->irq_lock);
 	if (dev_priv->display_irqs_enabled)
 		valleyview_display_irqs_install(dev_priv);
-	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+	spin_unlock_irq(&dev_priv->irq_lock);
+}
 
-	I915_WRITE(VLV_IIR, 0xffffffff);
-	I915_WRITE(VLV_IIR, 0xffffffff);
+static int valleyview_irq_postinstall(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	vlv_display_irq_postinstall(dev_priv);
 
 	gen5_gt_irq_postinstall(dev);
 
@@ -3777,30 +3526,45 @@ static void gen8_gt_irq_postinstall(struct drm_i915_private *dev_priv)
 	dev_priv->pm_irq_mask = 0xffffffff;
 	GEN8_IRQ_INIT_NDX(GT, 0, ~gt_interrupts[0], gt_interrupts[0]);
 	GEN8_IRQ_INIT_NDX(GT, 1, ~gt_interrupts[1], gt_interrupts[1]);
-	GEN8_IRQ_INIT_NDX(GT, 2, dev_priv->pm_irq_mask, dev_priv->pm_rps_events);
+	/*
+	 * RPS interrupts will get enabled/disabled on demand when RPS itself
+	 * is enabled/disabled.
+	 */
+	GEN8_IRQ_INIT_NDX(GT, 2, dev_priv->pm_irq_mask, 0);
 	GEN8_IRQ_INIT_NDX(GT, 3, ~gt_interrupts[3], gt_interrupts[3]);
 }
 
 static void gen8_de_irq_postinstall(struct drm_i915_private *dev_priv)
 {
-	uint32_t de_pipe_masked = GEN8_PIPE_PRIMARY_FLIP_DONE |
-		GEN8_PIPE_CDCLK_CRC_DONE |
-		GEN8_DE_PIPE_IRQ_FAULT_ERRORS;
-	uint32_t de_pipe_enables = de_pipe_masked | GEN8_PIPE_VBLANK |
-		GEN8_PIPE_FIFO_UNDERRUN;
+	uint32_t de_pipe_masked = GEN8_PIPE_CDCLK_CRC_DONE;
+	uint32_t de_pipe_enables;
 	int pipe;
+	u32 aux_en = GEN8_AUX_CHANNEL_A;
+
+	if (IS_GEN9(dev_priv)) {
+		de_pipe_masked |= GEN9_PIPE_PLANE1_FLIP_DONE |
+				  GEN9_DE_PIPE_IRQ_FAULT_ERRORS;
+		aux_en |= GEN9_AUX_CHANNEL_B | GEN9_AUX_CHANNEL_C |
+			GEN9_AUX_CHANNEL_D;
+	} else
+		de_pipe_masked |= GEN8_PIPE_PRIMARY_FLIP_DONE |
+				  GEN8_DE_PIPE_IRQ_FAULT_ERRORS;
+
+	de_pipe_enables = de_pipe_masked | GEN8_PIPE_VBLANK |
+					   GEN8_PIPE_FIFO_UNDERRUN;
+
 	dev_priv->de_irq_mask[PIPE_A] = ~de_pipe_masked;
 	dev_priv->de_irq_mask[PIPE_B] = ~de_pipe_masked;
 	dev_priv->de_irq_mask[PIPE_C] = ~de_pipe_masked;
 
 	for_each_pipe(dev_priv, pipe)
-		if (intel_display_power_enabled(dev_priv,
+		if (intel_display_power_is_enabled(dev_priv,
 				POWER_DOMAIN_PIPE(pipe)))
 			GEN8_IRQ_INIT_NDX(DE_PIPE, pipe,
 					  dev_priv->de_irq_mask[pipe],
 					  de_pipe_enables);
 
-	GEN5_IRQ_INIT(GEN8_DE_PORT_, ~GEN8_AUX_CHANNEL_A, GEN8_AUX_CHANNEL_A);
+	GEN5_IRQ_INIT(GEN8_DE_PORT_, ~aux_en, aux_en);
 }
 
 static int gen8_irq_postinstall(struct drm_device *dev)
@@ -3823,33 +3587,8 @@ static int gen8_irq_postinstall(struct drm_device *dev)
 static int cherryview_irq_postinstall(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 enable_mask = I915_DISPLAY_PORT_INTERRUPT |
-		I915_DISPLAY_PIPE_A_EVENT_INTERRUPT |
-		I915_DISPLAY_PIPE_B_EVENT_INTERRUPT |
-		I915_DISPLAY_PIPE_C_EVENT_INTERRUPT;
-	u32 pipestat_enable = PLANE_FLIP_DONE_INT_STATUS_VLV |
-		PIPE_CRC_DONE_INTERRUPT_STATUS;
-	unsigned long irqflags;
-	int pipe;
-
-	/*
-	 * Leave vblank interrupts masked initially.  enable/disable will
-	 * toggle them based on usage.
-	 */
-	dev_priv->irq_mask = ~enable_mask;
 
-	for_each_pipe(dev_priv, pipe)
-		I915_WRITE(PIPESTAT(pipe), 0xffff);
-
-	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-	i915_enable_pipestat(dev_priv, PIPE_A, PIPE_GMBUS_INTERRUPT_STATUS);
-	for_each_pipe(dev_priv, pipe)
-		i915_enable_pipestat(dev_priv, pipe, pipestat_enable);
-	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
-
-	I915_WRITE(VLV_IIR, 0xffffffff);
-	I915_WRITE(VLV_IMR, dev_priv->irq_mask);
-	I915_WRITE(VLV_IER, enable_mask);
+	vlv_display_irq_postinstall(dev_priv);
 
 	gen8_gt_irq_postinstall(dev_priv);
 
@@ -3869,41 +3608,39 @@ static void gen8_irq_uninstall(struct drm_device *dev)
 	gen8_irq_reset(dev);
 }
 
+static void vlv_display_irq_uninstall(struct drm_i915_private *dev_priv)
+{
+	/* Interrupt setup is already guaranteed to be single-threaded, this is
+	 * just to make the assert_spin_locked check happy. */
+	spin_lock_irq(&dev_priv->irq_lock);
+	if (dev_priv->display_irqs_enabled)
+		valleyview_display_irqs_uninstall(dev_priv);
+	spin_unlock_irq(&dev_priv->irq_lock);
+
+	vlv_display_irq_reset(dev_priv);
+
+	dev_priv->irq_mask = ~0;
+}
+
 static void valleyview_irq_uninstall(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	unsigned long irqflags;
-	int pipe;
 
 	if (!dev_priv)
 		return;
 
 	I915_WRITE(VLV_MASTER_IER, 0);
 
-	for_each_pipe(dev_priv, pipe)
-		I915_WRITE(PIPESTAT(pipe), 0xffff);
+	gen5_gt_irq_reset(dev);
 
 	I915_WRITE(HWSTAM, 0xffffffff);
-	I915_WRITE(PORT_HOTPLUG_EN, 0);
-	I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
-
-	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-	if (dev_priv->display_irqs_enabled)
-		valleyview_display_irqs_uninstall(dev_priv);
-	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
-
-	dev_priv->irq_mask = 0;
 
-	I915_WRITE(VLV_IIR, 0xffffffff);
-	I915_WRITE(VLV_IMR, 0xffffffff);
-	I915_WRITE(VLV_IER, 0x0);
-	POSTING_READ(VLV_IER);
+	vlv_display_irq_uninstall(dev_priv);
 }
 
 static void cherryview_irq_uninstall(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	int pipe;
 
 	if (!dev_priv)
 		return;
@@ -3911,44 +3648,11 @@ static void cherryview_irq_uninstall(struct drm_device *dev)
 	I915_WRITE(GEN8_MASTER_IRQ, 0);
 	POSTING_READ(GEN8_MASTER_IRQ);
 
-#define GEN8_IRQ_FINI_NDX(type, which)				\
-do {								\
-	I915_WRITE(GEN8_##type##_IMR(which), 0xffffffff);	\
-	I915_WRITE(GEN8_##type##_IER(which), 0);		\
-	I915_WRITE(GEN8_##type##_IIR(which), 0xffffffff);	\
-	POSTING_READ(GEN8_##type##_IIR(which));			\
-	I915_WRITE(GEN8_##type##_IIR(which), 0xffffffff);	\
-} while (0)
-
-#define GEN8_IRQ_FINI(type)				\
-do {							\
-	I915_WRITE(GEN8_##type##_IMR, 0xffffffff);	\
-	I915_WRITE(GEN8_##type##_IER, 0);		\
-	I915_WRITE(GEN8_##type##_IIR, 0xffffffff);	\
-	POSTING_READ(GEN8_##type##_IIR);		\
-	I915_WRITE(GEN8_##type##_IIR, 0xffffffff);	\
-} while (0)
-
-	GEN8_IRQ_FINI_NDX(GT, 0);
-	GEN8_IRQ_FINI_NDX(GT, 1);
-	GEN8_IRQ_FINI_NDX(GT, 2);
-	GEN8_IRQ_FINI_NDX(GT, 3);
-
-	GEN8_IRQ_FINI(PCU);
-
-#undef GEN8_IRQ_FINI
-#undef GEN8_IRQ_FINI_NDX
-
-	I915_WRITE(PORT_HOTPLUG_EN, 0);
-	I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
+	gen8_gt_irq_reset(dev_priv);
 
-	for_each_pipe(dev_priv, pipe)
-		I915_WRITE(PIPESTAT(pipe), 0xffff);
+	GEN5_IRQ_RESET(GEN8_PCU_);
 
-	I915_WRITE(VLV_IMR, 0xffffffff);
-	I915_WRITE(VLV_IER, 0x0);
-	I915_WRITE(VLV_IIR, 0xffffffff);
-	POSTING_READ(VLV_IIR);
+	vlv_display_irq_uninstall(dev_priv);
 }
 
 static void ironlake_irq_uninstall(struct drm_device *dev)
@@ -3976,7 +3680,6 @@ static void i8xx_irq_preinstall(struct drm_device * dev)
 static int i8xx_irq_postinstall(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	unsigned long irqflags;
 
 	I915_WRITE16(EMR,
 		     ~(I915_ERROR_PAGE_TABLE | I915_ERROR_MEMORY_REFRESH));
@@ -3999,10 +3702,10 @@ static int i8xx_irq_postinstall(struct drm_device *dev)
 
 	/* Interrupt setup is already guaranteed to be single-threaded, this is
 	 * just to make the assert_spin_locked check happy. */
-	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+	spin_lock_irq(&dev_priv->irq_lock);
 	i915_enable_pipestat(dev_priv, PIPE_A, PIPE_CRC_DONE_INTERRUPT_STATUS);
 	i915_enable_pipestat(dev_priv, PIPE_B, PIPE_CRC_DONE_INTERRUPT_STATUS);
-	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+	spin_unlock_irq(&dev_priv->irq_lock);
 
 	return 0;
 }
@@ -4047,7 +3750,6 @@ static irqreturn_t i8xx_irq_handler(int irq, void *arg)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u16 iir, new_iir;
 	u32 pipe_stats[2];
-	unsigned long irqflags;
 	int pipe;
 	u16 flip_mask =
 		I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |
@@ -4063,11 +3765,9 @@ static irqreturn_t i8xx_irq_handler(int irq, void *arg)
 		 * 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);
+		spin_lock(&dev_priv->irq_lock);
 		if (iir & I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT)
-			i915_handle_error(dev, false,
-					  "Command parser error, iir 0x%08x",
-					  iir);
+			DRM_DEBUG("Command parser error, iir 0x%08x\n", iir);
 
 		for_each_pipe(dev_priv, pipe) {
 			int reg = PIPESTAT(pipe);
@@ -4079,13 +3779,11 @@ static irqreturn_t i8xx_irq_handler(int irq, void *arg)
 			if (pipe_stats[pipe] & 0x8000ffff)
 				I915_WRITE(reg, pipe_stats[pipe]);
 		}
-		spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+		spin_unlock(&dev_priv->irq_lock);
 
 		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]);
 
@@ -4101,9 +3799,9 @@ static irqreturn_t i8xx_irq_handler(int irq, void *arg)
 			if (pipe_stats[pipe] & PIPE_CRC_DONE_INTERRUPT_STATUS)
 				i9xx_pipe_crc_irq_handler(dev, pipe);
 
-			if (pipe_stats[pipe] & PIPE_FIFO_UNDERRUN_STATUS &&
-			    intel_set_cpu_fifo_underrun_reporting(dev, pipe, false))
-				DRM_ERROR("pipe %c underrun\n", pipe_name(pipe));
+			if (pipe_stats[pipe] & PIPE_FIFO_UNDERRUN_STATUS)
+				intel_cpu_fifo_underrun_irq_handler(dev_priv,
+								    pipe);
 		}
 
 		iir = new_iir;
@@ -4149,7 +3847,6 @@ static int i915_irq_postinstall(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 enable_mask;
-	unsigned long irqflags;
 
 	I915_WRITE(EMR, ~(I915_ERROR_PAGE_TABLE | I915_ERROR_MEMORY_REFRESH));
 
@@ -4187,10 +3884,10 @@ static int i915_irq_postinstall(struct drm_device *dev)
 
 	/* Interrupt setup is already guaranteed to be single-threaded, this is
 	 * just to make the assert_spin_locked check happy. */
-	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+	spin_lock_irq(&dev_priv->irq_lock);
 	i915_enable_pipestat(dev_priv, PIPE_A, PIPE_CRC_DONE_INTERRUPT_STATUS);
 	i915_enable_pipestat(dev_priv, PIPE_B, PIPE_CRC_DONE_INTERRUPT_STATUS);
-	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+	spin_unlock_irq(&dev_priv->irq_lock);
 
 	return 0;
 }
@@ -4234,7 +3931,6 @@ static irqreturn_t i915_irq_handler(int irq, void *arg)
 	struct drm_device *dev = arg;
 	struct drm_i915_private *dev_priv = 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;
@@ -4250,11 +3946,9 @@ static irqreturn_t i915_irq_handler(int irq, void *arg)
 		 * 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);
+		spin_lock(&dev_priv->irq_lock);
 		if (iir & I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT)
-			i915_handle_error(dev, false,
-					  "Command parser error, iir 0x%08x",
-					  iir);
+			DRM_DEBUG("Command parser error, iir 0x%08x\n", iir);
 
 		for_each_pipe(dev_priv, pipe) {
 			int reg = PIPESTAT(pipe);
@@ -4266,7 +3960,7 @@ static irqreturn_t i915_irq_handler(int irq, void *arg)
 				irq_received = true;
 			}
 		}
-		spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+		spin_unlock(&dev_priv->irq_lock);
 
 		if (!irq_received)
 			break;
@@ -4297,9 +3991,9 @@ static irqreturn_t i915_irq_handler(int irq, void *arg)
 			if (pipe_stats[pipe] & PIPE_CRC_DONE_INTERRUPT_STATUS)
 				i9xx_pipe_crc_irq_handler(dev, pipe);
 
-			if (pipe_stats[pipe] & PIPE_FIFO_UNDERRUN_STATUS &&
-			    intel_set_cpu_fifo_underrun_reporting(dev, pipe, false))
-				DRM_ERROR("pipe %c underrun\n", pipe_name(pipe));
+			if (pipe_stats[pipe] & PIPE_FIFO_UNDERRUN_STATUS)
+				intel_cpu_fifo_underrun_irq_handler(dev_priv,
+								    pipe);
 		}
 
 		if (blc_event || (iir & I915_ASLE_INTERRUPT))
@@ -4324,8 +4018,6 @@ static irqreturn_t i915_irq_handler(int irq, void *arg)
 		iir = new_iir;
 	} while (iir & ~flip_mask);
 
-	i915_update_dri1_breadcrumb(dev);
-
 	return ret;
 }
 
@@ -4372,7 +4064,6 @@ static int i965_irq_postinstall(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 enable_mask;
 	u32 error_mask;
-	unsigned long irqflags;
 
 	/* Unmask the interrupts that we always want on. */
 	dev_priv->irq_mask = ~(I915_ASLE_INTERRUPT |
@@ -4393,11 +4084,11 @@ static int i965_irq_postinstall(struct drm_device *dev)
 
 	/* Interrupt setup is already guaranteed to be single-threaded, this is
 	 * just to make the assert_spin_locked check happy. */
-	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+	spin_lock_irq(&dev_priv->irq_lock);
 	i915_enable_pipestat(dev_priv, PIPE_A, PIPE_GMBUS_INTERRUPT_STATUS);
 	i915_enable_pipestat(dev_priv, PIPE_A, PIPE_CRC_DONE_INTERRUPT_STATUS);
 	i915_enable_pipestat(dev_priv, PIPE_B, PIPE_CRC_DONE_INTERRUPT_STATUS);
-	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+	spin_unlock_irq(&dev_priv->irq_lock);
 
 	/*
 	 * Enable some error detection, note the instruction error mask
@@ -4462,7 +4153,6 @@ static irqreturn_t i965_irq_handler(int irq, void *arg)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 iir, new_iir;
 	u32 pipe_stats[I915_MAX_PIPES];
-	unsigned long irqflags;
 	int ret = IRQ_NONE, pipe;
 	u32 flip_mask =
 		I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |
@@ -4479,11 +4169,9 @@ static irqreturn_t i965_irq_handler(int irq, void *arg)
 		 * 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);
+		spin_lock(&dev_priv->irq_lock);
 		if (iir & I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT)
-			i915_handle_error(dev, false,
-					  "Command parser error, iir 0x%08x",
-					  iir);
+			DRM_DEBUG("Command parser error, iir 0x%08x\n", iir);
 
 		for_each_pipe(dev_priv, pipe) {
 			int reg = PIPESTAT(pipe);
@@ -4497,7 +4185,7 @@ static irqreturn_t i965_irq_handler(int irq, void *arg)
 				irq_received = true;
 			}
 		}
-		spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+		spin_unlock(&dev_priv->irq_lock);
 
 		if (!irq_received)
 			break;
@@ -4527,9 +4215,8 @@ static irqreturn_t i965_irq_handler(int irq, void *arg)
 			if (pipe_stats[pipe] & PIPE_CRC_DONE_INTERRUPT_STATUS)
 				i9xx_pipe_crc_irq_handler(dev, pipe);
 
-			if (pipe_stats[pipe] & PIPE_FIFO_UNDERRUN_STATUS &&
-			    intel_set_cpu_fifo_underrun_reporting(dev, pipe, false))
-				DRM_ERROR("pipe %c underrun\n", pipe_name(pipe));
+			if (pipe_stats[pipe] & PIPE_FIFO_UNDERRUN_STATUS)
+				intel_cpu_fifo_underrun_irq_handler(dev_priv, pipe);
 		}
 
 		if (blc_event || (iir & I915_ASLE_INTERRUPT))
@@ -4556,8 +4243,6 @@ static irqreturn_t i965_irq_handler(int irq, void *arg)
 		iir = new_iir;
 	}
 
-	i915_update_dri1_breadcrumb(dev);
-
 	return ret;
 }
 
@@ -4584,19 +4269,18 @@ static void i965_irq_uninstall(struct drm_device * dev)
 	I915_WRITE(IIR, I915_READ(IIR));
 }
 
-static void intel_hpd_irq_reenable(struct work_struct *work)
+static void intel_hpd_irq_reenable_work(struct work_struct *work)
 {
 	struct drm_i915_private *dev_priv =
 		container_of(work, typeof(*dev_priv),
 			     hotplug_reenable_work.work);
 	struct drm_device *dev = dev_priv->dev;
 	struct drm_mode_config *mode_config = &dev->mode_config;
-	unsigned long irqflags;
 	int i;
 
 	intel_runtime_pm_get(dev_priv);
 
-	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+	spin_lock_irq(&dev_priv->irq_lock);
 	for (i = (HPD_NONE + 1); i < HPD_NUM_PINS; i++) {
 		struct drm_connector *connector;
 
@@ -4620,14 +4304,21 @@ static void intel_hpd_irq_reenable(struct work_struct *work)
 	}
 	if (dev_priv->display.hpd_irq_setup)
 		dev_priv->display.hpd_irq_setup(dev);
-	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+	spin_unlock_irq(&dev_priv->irq_lock);
 
 	intel_runtime_pm_put(dev_priv);
 }
 
-void intel_irq_init(struct drm_device *dev)
+/**
+ * intel_irq_init - initializes irq support
+ * @dev_priv: i915 device instance
+ *
+ * This function initializes all the irq support including work items, timers
+ * and all the vtables. It does not setup the interrupt itself though.
+ */
+void intel_irq_init(struct drm_i915_private *dev_priv)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_device *dev = dev_priv->dev;
 
 	INIT_WORK(&dev_priv->hotplug_work, i915_hotplug_work_func);
 	INIT_WORK(&dev_priv->dig_port_work, i915_digport_work_func);
@@ -4636,7 +4327,7 @@ void intel_irq_init(struct drm_device *dev)
 	INIT_WORK(&dev_priv->l3_parity.error_work, ivybridge_parity_work);
 
 	/* Let's track the enabled rps events */
-	if (IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev))
+	if (IS_VALLEYVIEW(dev_priv) && !IS_CHERRYVIEW(dev_priv))
 		/* WaGsvRC0ResidencyMethod:vlv */
 		dev_priv->pm_rps_events = GEN6_PM_RP_UP_EI_EXPIRED;
 	else
@@ -4646,17 +4337,14 @@ void intel_irq_init(struct drm_device *dev)
 		    i915_hangcheck_elapsed,
 		    (unsigned long) dev);
 	INIT_DELAYED_WORK(&dev_priv->hotplug_reenable_work,
-			  intel_hpd_irq_reenable);
+			  intel_hpd_irq_reenable_work);
 
 	pm_qos_add_request(&dev_priv->pm_qos, PM_QOS_CPU_DMA_LATENCY, PM_QOS_DEFAULT_VALUE);
 
-	/* Haven't installed the IRQ handler yet */
-	dev_priv->pm._irqs_disabled = true;
-
-	if (IS_GEN2(dev)) {
+	if (IS_GEN2(dev_priv)) {
 		dev->max_vblank_count = 0;
 		dev->driver->get_vblank_counter = i8xx_get_vblank_counter;
-	} else if (IS_G4X(dev) || INTEL_INFO(dev)->gen >= 5) {
+	} else if (IS_G4X(dev_priv) || INTEL_INFO(dev_priv)->gen >= 5) {
 		dev->max_vblank_count = 0xffffffff; /* full 32 bit counter */
 		dev->driver->get_vblank_counter = gm45_get_vblank_counter;
 	} else {
@@ -4669,7 +4357,7 @@ void intel_irq_init(struct drm_device *dev)
 	 * Gen2 doesn't have a hardware frame counter and so depends on
 	 * vblank interrupts to produce sane vblank seuquence numbers.
 	 */
-	if (!IS_GEN2(dev))
+	if (!IS_GEN2(dev_priv))
 		dev->vblank_disable_immediate = true;
 
 	if (drm_core_check_feature(dev, DRIVER_MODESET)) {
@@ -4677,7 +4365,7 @@ void intel_irq_init(struct drm_device *dev)
 		dev->driver->get_scanout_position = i915_get_crtc_scanoutpos;
 	}
 
-	if (IS_CHERRYVIEW(dev)) {
+	if (IS_CHERRYVIEW(dev_priv)) {
 		dev->driver->irq_handler = cherryview_irq_handler;
 		dev->driver->irq_preinstall = cherryview_irq_preinstall;
 		dev->driver->irq_postinstall = cherryview_irq_postinstall;
@@ -4685,7 +4373,7 @@ void intel_irq_init(struct drm_device *dev)
 		dev->driver->enable_vblank = valleyview_enable_vblank;
 		dev->driver->disable_vblank = valleyview_disable_vblank;
 		dev_priv->display.hpd_irq_setup = i915_hpd_irq_setup;
-	} else if (IS_VALLEYVIEW(dev)) {
+	} else if (IS_VALLEYVIEW(dev_priv)) {
 		dev->driver->irq_handler = valleyview_irq_handler;
 		dev->driver->irq_preinstall = valleyview_irq_preinstall;
 		dev->driver->irq_postinstall = valleyview_irq_postinstall;
@@ -4693,7 +4381,7 @@ void intel_irq_init(struct drm_device *dev)
 		dev->driver->enable_vblank = valleyview_enable_vblank;
 		dev->driver->disable_vblank = valleyview_disable_vblank;
 		dev_priv->display.hpd_irq_setup = i915_hpd_irq_setup;
-	} else if (IS_GEN8(dev)) {
+	} else if (INTEL_INFO(dev_priv)->gen >= 8) {
 		dev->driver->irq_handler = gen8_irq_handler;
 		dev->driver->irq_preinstall = gen8_irq_reset;
 		dev->driver->irq_postinstall = gen8_irq_postinstall;
@@ -4710,12 +4398,12 @@ void intel_irq_init(struct drm_device *dev)
 		dev->driver->disable_vblank = ironlake_disable_vblank;
 		dev_priv->display.hpd_irq_setup = ibx_hpd_irq_setup;
 	} else {
-		if (INTEL_INFO(dev)->gen == 2) {
+		if (INTEL_INFO(dev_priv)->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) {
+		} else if (INTEL_INFO(dev_priv)->gen == 3) {
 			dev->driver->irq_preinstall = i915_irq_preinstall;
 			dev->driver->irq_postinstall = i915_irq_postinstall;
 			dev->driver->irq_uninstall = i915_irq_uninstall;
@@ -4733,12 +4421,23 @@ void intel_irq_init(struct drm_device *dev)
 	}
 }
 
-void intel_hpd_init(struct drm_device *dev)
+/**
+ * intel_hpd_init - initializes and enables hpd support
+ * @dev_priv: i915 device instance
+ *
+ * This function enables the hotplug support. It requires that interrupts have
+ * already been enabled with intel_irq_init_hw(). From this point on hotplug and
+ * poll request can run concurrently to other code, so locking rules must be
+ * obeyed.
+ *
+ * This is a separate step from interrupt enabling to simplify the locking rules
+ * in the driver load and resume code.
+ */
+void intel_hpd_init(struct drm_i915_private *dev_priv)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_device *dev = dev_priv->dev;
 	struct drm_mode_config *mode_config = &dev->mode_config;
 	struct drm_connector *connector;
-	unsigned long irqflags;
 	int i;
 
 	for (i = 1; i < HPD_NUM_PINS; i++) {
@@ -4756,27 +4455,72 @@ void intel_hpd_init(struct drm_device *dev)
 
 	/* Interrupt setup is already guaranteed to be single-threaded, this is
 	 * just to make the assert_spin_locked checks happy. */
-	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+	spin_lock_irq(&dev_priv->irq_lock);
 	if (dev_priv->display.hpd_irq_setup)
 		dev_priv->display.hpd_irq_setup(dev);
-	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+	spin_unlock_irq(&dev_priv->irq_lock);
 }
 
-/* Disable interrupts so we can allow runtime PM. */
-void intel_runtime_pm_disable_interrupts(struct drm_device *dev)
+/**
+ * intel_irq_install - enables the hardware interrupt
+ * @dev_priv: i915 device instance
+ *
+ * This function enables the hardware interrupt handling, but leaves the hotplug
+ * handling still disabled. It is called after intel_irq_init().
+ *
+ * In the driver load and resume code we need working interrupts in a few places
+ * but don't want to deal with the hassle of concurrent probe and hotplug
+ * workers. Hence the split into this two-stage approach.
+ */
+int intel_irq_install(struct drm_i915_private *dev_priv)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	/*
+	 * We enable some interrupt sources in our postinstall hooks, so mark
+	 * interrupts as enabled _before_ actually enabling them to avoid
+	 * special cases in our ordering checks.
+	 */
+	dev_priv->pm.irqs_enabled = true;
+
+	return drm_irq_install(dev_priv->dev, dev_priv->dev->pdev->irq);
+}
 
-	dev->driver->irq_uninstall(dev);
-	dev_priv->pm._irqs_disabled = true;
+/**
+ * intel_irq_uninstall - finilizes all irq handling
+ * @dev_priv: i915 device instance
+ *
+ * This stops interrupt and hotplug handling and unregisters and frees all
+ * resources acquired in the init functions.
+ */
+void intel_irq_uninstall(struct drm_i915_private *dev_priv)
+{
+	drm_irq_uninstall(dev_priv->dev);
+	intel_hpd_cancel_work(dev_priv);
+	dev_priv->pm.irqs_enabled = false;
 }
 
-/* Restore interrupts so we can recover from runtime PM. */
-void intel_runtime_pm_restore_interrupts(struct drm_device *dev)
+/**
+ * intel_runtime_pm_disable_interrupts - runtime interrupt disabling
+ * @dev_priv: i915 device instance
+ *
+ * This function is used to disable interrupts at runtime, both in the runtime
+ * pm and the system suspend/resume code.
+ */
+void intel_runtime_pm_disable_interrupts(struct drm_i915_private *dev_priv)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	dev_priv->dev->driver->irq_uninstall(dev_priv->dev);
+	dev_priv->pm.irqs_enabled = false;
+}
 
-	dev_priv->pm._irqs_disabled = false;
-	dev->driver->irq_preinstall(dev);
-	dev->driver->irq_postinstall(dev);
+/**
+ * intel_runtime_pm_enable_interrupts - runtime interrupt enabling
+ * @dev_priv: i915 device instance
+ *
+ * This function is used to enable interrupts at runtime, both in the runtime
+ * pm and the system suspend/resume code.
+ */
+void intel_runtime_pm_enable_interrupts(struct drm_i915_private *dev_priv)
+{
+	dev_priv->pm.irqs_enabled = true;
+	dev_priv->dev->driver->irq_preinstall(dev_priv->dev);
+	dev_priv->dev->driver->irq_postinstall(dev_priv->dev);
 }
diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index c01e5f31430e..172de3b3433b 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -26,14 +26,25 @@
 #define _I915_REG_H_
 
 #define _PIPE(pipe, a, b) ((a) + (pipe)*((b)-(a)))
+#define _PLANE(plane, a, b) _PIPE(plane, a, b)
 #define _TRANSCODER(tran, a, b) ((a) + (tran)*((b)-(a)))
-
 #define _PORT(port, a, b) ((a) + (port)*((b)-(a)))
 #define _PIPE3(pipe, a, b, c) ((pipe) == PIPE_A ? (a) : \
 			       (pipe) == PIPE_B ? (b) : (c))
 
-#define _MASKED_BIT_ENABLE(a) (((a) << 16) | (a))
-#define _MASKED_BIT_DISABLE(a) ((a) << 16)
+#define _MASKED_FIELD(mask, value) ({					   \
+	if (__builtin_constant_p(mask))					   \
+		BUILD_BUG_ON_MSG(((mask) & 0xffff0000), "Incorrect mask"); \
+	if (__builtin_constant_p(value))				   \
+		BUILD_BUG_ON_MSG((value) & 0xffff0000, "Incorrect value"); \
+	if (__builtin_constant_p(mask) && __builtin_constant_p(value))	   \
+		BUILD_BUG_ON_MSG((value) & ~(mask),			   \
+				 "Incorrect value for mask");		   \
+	(mask) << 16 | (value); })
+#define _MASKED_BIT_ENABLE(a)	({ typeof(a) _a = (a); _MASKED_FIELD(_a, _a); })
+#define _MASKED_BIT_DISABLE(a)	(_MASKED_FIELD((a), 0))
+
+
 
 /* PCI config space */
 
@@ -74,15 +85,17 @@
 #define   I915_GC_RENDER_CLOCK_166_MHZ	(0 << 0)
 #define   I915_GC_RENDER_CLOCK_200_MHZ	(1 << 0)
 #define   I915_GC_RENDER_CLOCK_333_MHZ	(4 << 0)
+#define GCDGMBUS 0xcc
 #define PCI_LBPC 0xf4 /* legacy/combination backlight modes, also called LBB */
 
 
 /* Graphics reset regs */
-#define I965_GDRST 0xc0 /* PCI config register */
+#define I915_GDRST 0xc0 /* PCI config register */
 #define  GRDOM_FULL	(0<<2)
 #define  GRDOM_RENDER	(1<<2)
 #define  GRDOM_MEDIA	(3<<2)
 #define  GRDOM_MASK	(3<<2)
+#define  GRDOM_RESET_STATUS (1<<1)
 #define  GRDOM_RESET_ENABLE (1<<0)
 
 #define ILK_GDSR 0x2ca4 /* MCHBAR offset */
@@ -248,6 +261,16 @@
 #define   MI_DISPLAY_FLIP_IVB_SPRITE_B (3 << 19)
 #define   MI_DISPLAY_FLIP_IVB_PLANE_C  (4 << 19)
 #define   MI_DISPLAY_FLIP_IVB_SPRITE_C (5 << 19)
+/* SKL ones */
+#define   MI_DISPLAY_FLIP_SKL_PLANE_1_A	(0 << 8)
+#define   MI_DISPLAY_FLIP_SKL_PLANE_1_B	(1 << 8)
+#define   MI_DISPLAY_FLIP_SKL_PLANE_1_C	(2 << 8)
+#define   MI_DISPLAY_FLIP_SKL_PLANE_2_A	(4 << 8)
+#define   MI_DISPLAY_FLIP_SKL_PLANE_2_B	(5 << 8)
+#define   MI_DISPLAY_FLIP_SKL_PLANE_2_C	(6 << 8)
+#define   MI_DISPLAY_FLIP_SKL_PLANE_3_A	(7 << 8)
+#define   MI_DISPLAY_FLIP_SKL_PLANE_3_B	(8 << 8)
+#define   MI_DISPLAY_FLIP_SKL_PLANE_3_C	(9 << 8)
 #define MI_SEMAPHORE_MBOX	MI_INSTR(0x16, 1) /* gen6, gen7 */
 #define   MI_SEMAPHORE_GLOBAL_GTT    (1<<22)
 #define   MI_SEMAPHORE_UPDATE	    (1<<21)
@@ -314,6 +337,8 @@
 #define   MI_BATCH_GTT		    (2<<6) /* aliased with (1<<7) on gen4 */
 #define MI_BATCH_BUFFER_START_GEN8	MI_INSTR(0x31, 1)
 
+#define MI_PREDICATE_SRC0	(0x2400)
+#define MI_PREDICATE_SRC1	(0x2408)
 
 #define MI_PREDICATE_RESULT_2	(0x2214)
 #define  LOWER_SLICE_ENABLED	(1<<0)
@@ -370,6 +395,7 @@
 #define   PIPE_CONTROL_STORE_DATA_INDEX			(1<<21)
 #define   PIPE_CONTROL_CS_STALL				(1<<20)
 #define   PIPE_CONTROL_TLB_INVALIDATE			(1<<18)
+#define   PIPE_CONTROL_MEDIA_STATE_CLEAR		(1<<16)
 #define   PIPE_CONTROL_QW_WRITE				(1<<14)
 #define   PIPE_CONTROL_POST_SYNC_OP_MASK                (3<<14)
 #define   PIPE_CONTROL_DEPTH_STALL			(1<<13)
@@ -564,6 +590,7 @@ enum punit_power_well {
 #define PUNIT_REG_GPU_LFM			0xd3
 #define PUNIT_REG_GPU_FREQ_REQ			0xd4
 #define PUNIT_REG_GPU_FREQ_STS			0xd8
+#define   GPLLENABLE				(1<<4)
 #define   GENFREQSTATUS				(1<<0)
 #define PUNIT_REG_MEDIA_TURBO_FREQ_REQ		0xdc
 #define PUNIT_REG_CZ_TIMESTAMP			0xce
@@ -672,7 +699,7 @@ enum punit_power_well {
  * need to be accessed during AUX communication,
  *
  * Generally the common lane corresponds to the pipe and
- * the spline (PCS/TX) correponds to the port.
+ * the spline (PCS/TX) corresponds to the port.
  *
  * For dual channel PHY (VLV/CHV):
  *
@@ -796,6 +823,8 @@ enum punit_power_well {
 #define _VLV_PCS_DW0_CH1		0x8400
 #define   DPIO_PCS_TX_LANE2_RESET	(1<<16)
 #define   DPIO_PCS_TX_LANE1_RESET	(1<<7)
+#define   DPIO_LEFT_TXFIFO_RST_MASTER2	(1<<4)
+#define   DPIO_RIGHT_TXFIFO_RST_MASTER2	(1<<3)
 #define VLV_PCS_DW0(ch) _PORT(ch, _VLV_PCS_DW0_CH0, _VLV_PCS_DW0_CH1)
 
 #define _VLV_PCS01_DW0_CH0		0x200
@@ -836,12 +865,31 @@ enum punit_power_well {
 
 #define _VLV_PCS_DW9_CH0		0x8224
 #define _VLV_PCS_DW9_CH1		0x8424
+#define   DPIO_PCS_TX2MARGIN_MASK	(0x7<<13)
+#define   DPIO_PCS_TX2MARGIN_000	(0<<13)
+#define   DPIO_PCS_TX2MARGIN_101	(1<<13)
+#define   DPIO_PCS_TX1MARGIN_MASK	(0x7<<10)
+#define   DPIO_PCS_TX1MARGIN_000	(0<<10)
+#define   DPIO_PCS_TX1MARGIN_101	(1<<10)
 #define	VLV_PCS_DW9(ch) _PORT(ch, _VLV_PCS_DW9_CH0, _VLV_PCS_DW9_CH1)
 
+#define _VLV_PCS01_DW9_CH0		0x224
+#define _VLV_PCS23_DW9_CH0		0x424
+#define _VLV_PCS01_DW9_CH1		0x2624
+#define _VLV_PCS23_DW9_CH1		0x2824
+#define VLV_PCS01_DW9(ch) _PORT(ch, _VLV_PCS01_DW9_CH0, _VLV_PCS01_DW9_CH1)
+#define VLV_PCS23_DW9(ch) _PORT(ch, _VLV_PCS23_DW9_CH0, _VLV_PCS23_DW9_CH1)
+
 #define _CHV_PCS_DW10_CH0		0x8228
 #define _CHV_PCS_DW10_CH1		0x8428
 #define   DPIO_PCS_SWING_CALC_TX0_TX2	(1<<30)
 #define   DPIO_PCS_SWING_CALC_TX1_TX3	(1<<31)
+#define   DPIO_PCS_TX2DEEMP_MASK	(0xf<<24)
+#define   DPIO_PCS_TX2DEEMP_9P5		(0<<24)
+#define   DPIO_PCS_TX2DEEMP_6P0		(2<<24)
+#define   DPIO_PCS_TX1DEEMP_MASK	(0xf<<16)
+#define   DPIO_PCS_TX1DEEMP_9P5		(0<<16)
+#define   DPIO_PCS_TX1DEEMP_6P0		(2<<16)
 #define CHV_PCS_DW10(ch) _PORT(ch, _CHV_PCS_DW10_CH0, _CHV_PCS_DW10_CH1)
 
 #define _VLV_PCS01_DW10_CH0		0x0228
@@ -853,8 +901,18 @@ enum punit_power_well {
 
 #define _VLV_PCS_DW11_CH0		0x822c
 #define _VLV_PCS_DW11_CH1		0x842c
+#define   DPIO_LANEDESKEW_STRAP_OVRD	(1<<3)
+#define   DPIO_LEFT_TXFIFO_RST_MASTER	(1<<1)
+#define   DPIO_RIGHT_TXFIFO_RST_MASTER	(1<<0)
 #define VLV_PCS_DW11(ch) _PORT(ch, _VLV_PCS_DW11_CH0, _VLV_PCS_DW11_CH1)
 
+#define _VLV_PCS01_DW11_CH0		0x022c
+#define _VLV_PCS23_DW11_CH0		0x042c
+#define _VLV_PCS01_DW11_CH1		0x262c
+#define _VLV_PCS23_DW11_CH1		0x282c
+#define VLV_PCS01_DW11(ch) _PORT(ch, _VLV_PCS01_DW11_CH0, _VLV_PCS01_DW11_CH1)
+#define VLV_PCS23_DW11(ch) _PORT(ch, _VLV_PCS23_DW11_CH0, _VLV_PCS23_DW11_CH1)
+
 #define _VLV_PCS_DW12_CH0		0x8230
 #define _VLV_PCS_DW12_CH1		0x8430
 #define VLV_PCS_DW12(ch) _PORT(ch, _VLV_PCS_DW12_CH0, _VLV_PCS_DW12_CH1)
@@ -1071,6 +1129,7 @@ enum punit_power_well {
 #define GEN6_VERSYNC	(RING_SYNC_1(VEBOX_RING_BASE))
 #define GEN6_VEVSYNC	(RING_SYNC_2(VEBOX_RING_BASE))
 #define GEN6_NOSYNC 0
+#define RING_PSMI_CTL(base)	((base)+0x50)
 #define RING_MAX_IDLE(base)	((base)+0x54)
 #define RING_HWS_PGA(base)	((base)+0x80)
 #define RING_HWS_PGA_GEN6(base)	((base)+0x2080)
@@ -1237,7 +1296,7 @@ enum punit_power_well {
 #define   GEN6_WIZ_HASHING_8x8				GEN6_WIZ_HASHING(0, 0)
 #define   GEN6_WIZ_HASHING_8x4				GEN6_WIZ_HASHING(0, 1)
 #define   GEN6_WIZ_HASHING_16x4				GEN6_WIZ_HASHING(1, 0)
-#define   GEN6_WIZ_HASHING_MASK				(GEN6_WIZ_HASHING(1, 1) << 16)
+#define   GEN6_WIZ_HASHING_MASK				GEN6_WIZ_HASHING(1, 1)
 #define   GEN6_TD_FOUR_ROW_DISPATCH_DISABLE		(1 << 5)
 
 #define GFX_MODE	0x02520
@@ -1401,6 +1460,7 @@ enum punit_power_well {
 #define   GEN6_BLITTER_FBC_NOTIFY			(1<<3)
 
 #define GEN6_RC_SLEEP_PSMI_CONTROL	0x2050
+#define   GEN6_PSMI_SLEEP_MSG_DISABLE	(1 << 0)
 #define   GEN8_RC_SEMA_IDLE_MSG_DISABLE	(1 << 12)
 #define   GEN8_FF_DOP_CLOCK_GATE_DISABLE	(1<<10)
 
@@ -1999,6 +2059,8 @@ enum punit_power_well {
 #define DCC_ADDRESSING_MODE_MASK			(3 << 0)
 #define DCC_CHANNEL_XOR_DISABLE				(1 << 10)
 #define DCC_CHANNEL_XOR_BIT_17				(1 << 9)
+#define DCC2			0x10204
+#define DCC2_MODIFIED_ENHANCED_DISABLE			(1 << 20)
 
 /* Pineview MCH register contains DDR3 setting */
 #define CSHRDDR3CTL            0x101a8
@@ -2282,7 +2344,6 @@ enum punit_power_well {
 
 #define GEN6_GT_THREAD_STATUS_REG 0x13805c
 #define GEN6_GT_THREAD_STATUS_CORE_MASK 0x7
-#define GEN6_GT_THREAD_STATUS_CORE_MASK_HSW (0x7 | (0x07 << 16))
 
 #define GEN6_GT_PERF_STATUS	(MCHBAR_MIRROR_BASE_SNB + 0x5948)
 #define GEN6_RP_STATE_LIMITS	(MCHBAR_MIRROR_BASE_SNB + 0x5994)
@@ -2506,9 +2567,7 @@ enum punit_power_well {
 
 #define EDP_PSR_AUX_CTL(dev)			(EDP_PSR_BASE(dev) + 0x10)
 #define EDP_PSR_AUX_DATA1(dev)			(EDP_PSR_BASE(dev) + 0x14)
-#define   EDP_PSR_DPCD_COMMAND		0x80060000
 #define EDP_PSR_AUX_DATA2(dev)			(EDP_PSR_BASE(dev) + 0x18)
-#define   EDP_PSR_DPCD_NORMAL_OPERATION	(1<<24)
 #define EDP_PSR_AUX_DATA3(dev)			(EDP_PSR_BASE(dev) + 0x1c)
 #define EDP_PSR_AUX_DATA4(dev)			(EDP_PSR_BASE(dev) + 0x20)
 #define EDP_PSR_AUX_DATA5(dev)			(EDP_PSR_BASE(dev) + 0x24)
@@ -3645,6 +3704,7 @@ enum punit_power_well {
 #define   DP_AUX_CH_CTL_PRECHARGE_TEST	    (1 << 11)
 #define   DP_AUX_CH_CTL_BIT_CLOCK_2X_MASK    (0x7ff)
 #define   DP_AUX_CH_CTL_BIT_CLOCK_2X_SHIFT   0
+#define   DP_AUX_CH_CTL_SYNC_PULSE_SKL(c)   ((c) - 1)
 
 /*
  * Computing GMCH M and N values for the Display Port link
@@ -4024,17 +4084,18 @@ enum punit_power_well {
 #define   DSPFW_PLANEA_WM1_HI_MASK	(1<<0)
 
 /* drain latency register values*/
+#define DRAIN_LATENCY_PRECISION_16	16
 #define DRAIN_LATENCY_PRECISION_32	32
 #define DRAIN_LATENCY_PRECISION_64	64
 #define VLV_DDL(pipe)			(VLV_DISPLAY_BASE + 0x70050 + 4 * (pipe))
-#define DDL_CURSOR_PRECISION_64		(1<<31)
-#define DDL_CURSOR_PRECISION_32		(0<<31)
+#define DDL_CURSOR_PRECISION_HIGH	(1<<31)
+#define DDL_CURSOR_PRECISION_LOW	(0<<31)
 #define DDL_CURSOR_SHIFT		24
-#define DDL_SPRITE_PRECISION_64(sprite)	(1<<(15+8*(sprite)))
-#define DDL_SPRITE_PRECISION_32(sprite)	(0<<(15+8*(sprite)))
+#define DDL_SPRITE_PRECISION_HIGH(sprite)	(1<<(15+8*(sprite)))
+#define DDL_SPRITE_PRECISION_LOW(sprite)	(0<<(15+8*(sprite)))
 #define DDL_SPRITE_SHIFT(sprite)	(8+8*(sprite))
-#define DDL_PLANE_PRECISION_64		(1<<7)
-#define DDL_PLANE_PRECISION_32		(0<<7)
+#define DDL_PLANE_PRECISION_HIGH	(1<<7)
+#define DDL_PLANE_PRECISION_LOW		(0<<7)
 #define DDL_PLANE_SHIFT			0
 #define DRAIN_LATENCY_MASK		0x7f
 
@@ -4071,6 +4132,41 @@ enum punit_power_well {
 #define I965_CURSOR_MAX_WM	32
 #define I965_CURSOR_DFT_WM	8
 
+/* Watermark register definitions for SKL */
+#define CUR_WM_A_0		0x70140
+#define CUR_WM_B_0		0x71140
+#define PLANE_WM_1_A_0		0x70240
+#define PLANE_WM_1_B_0		0x71240
+#define PLANE_WM_2_A_0		0x70340
+#define PLANE_WM_2_B_0		0x71340
+#define PLANE_WM_TRANS_1_A_0	0x70268
+#define PLANE_WM_TRANS_1_B_0	0x71268
+#define PLANE_WM_TRANS_2_A_0	0x70368
+#define PLANE_WM_TRANS_2_B_0	0x71368
+#define CUR_WM_TRANS_A_0	0x70168
+#define CUR_WM_TRANS_B_0	0x71168
+#define   PLANE_WM_EN		(1 << 31)
+#define   PLANE_WM_LINES_SHIFT	14
+#define   PLANE_WM_LINES_MASK	0x1f
+#define   PLANE_WM_BLOCKS_MASK	0x3ff
+
+#define CUR_WM_0(pipe) _PIPE(pipe, CUR_WM_A_0, CUR_WM_B_0)
+#define CUR_WM(pipe, level) (CUR_WM_0(pipe) + ((4) * (level)))
+#define CUR_WM_TRANS(pipe) _PIPE(pipe, CUR_WM_TRANS_A_0, CUR_WM_TRANS_B_0)
+
+#define _PLANE_WM_1(pipe) _PIPE(pipe, PLANE_WM_1_A_0, PLANE_WM_1_B_0)
+#define _PLANE_WM_2(pipe) _PIPE(pipe, PLANE_WM_2_A_0, PLANE_WM_2_B_0)
+#define _PLANE_WM_BASE(pipe, plane)	\
+			_PLANE(plane, _PLANE_WM_1(pipe), _PLANE_WM_2(pipe))
+#define PLANE_WM(pipe, plane, level)	\
+			(_PLANE_WM_BASE(pipe, plane) + ((4) * (level)))
+#define _PLANE_WM_TRANS_1(pipe)	\
+			_PIPE(pipe, PLANE_WM_TRANS_1_A_0, PLANE_WM_TRANS_1_B_0)
+#define _PLANE_WM_TRANS_2(pipe)	\
+			_PIPE(pipe, PLANE_WM_TRANS_2_A_0, PLANE_WM_TRANS_2_B_0)
+#define PLANE_WM_TRANS(pipe, plane)	\
+		_PLANE(plane, _PLANE_WM_TRANS_1(pipe), _PLANE_WM_TRANS_2(pipe))
+
 /* define the Watermark register on Ironlake */
 #define WM0_PIPEA_ILK		0x45100
 #define  WM0_PIPE_PLANE_MASK	(0xffff<<16)
@@ -4177,6 +4273,7 @@ enum punit_power_well {
 #define   MCURSOR_PIPE_A	0x00
 #define   MCURSOR_PIPE_B	(1 << 28)
 #define   MCURSOR_GAMMA_ENABLE  (1 << 26)
+#define   CURSOR_ROTATE_180	(1<<15)
 #define   CURSOR_TRICKLE_FEED_DISABLE	(1 << 14)
 #define _CURABASE		0x70084
 #define _CURAPOS		0x70088
@@ -4240,9 +4337,11 @@ enum punit_power_well {
 #define   DISPPLANE_NO_LINE_DOUBLE		0
 #define   DISPPLANE_STEREO_POLARITY_FIRST	0
 #define   DISPPLANE_STEREO_POLARITY_SECOND	(1<<18)
-#define   DISPPLANE_ROTATE_180         (1<<15)
+#define   DISPPLANE_ALPHA_PREMULTIPLY		(1<<16) /* CHV pipe B */
+#define   DISPPLANE_ROTATE_180			(1<<15)
 #define   DISPPLANE_TRICKLE_FEED_DISABLE	(1<<14) /* Ironlake */
 #define   DISPPLANE_TILED			(1<<10)
+#define   DISPPLANE_MIRROR			(1<<8) /* CHV pipe B */
 #define _DSPAADDR				0x70184
 #define _DSPASTRIDE				0x70188
 #define _DSPAPOS				0x7018C /* reserved */
@@ -4263,6 +4362,24 @@ enum punit_power_well {
 #define DSPOFFSET(plane) _PIPE2(plane, _DSPAOFFSET)
 #define DSPSURFLIVE(plane) _PIPE2(plane, _DSPASURFLIVE)
 
+/* CHV pipe B blender and primary plane */
+#define _CHV_BLEND_A		0x60a00
+#define   CHV_BLEND_LEGACY		(0<<30)
+#define   CHV_BLEND_ANDROID		(1<<30)
+#define   CHV_BLEND_MPO			(2<<30)
+#define   CHV_BLEND_MASK		(3<<30)
+#define _CHV_CANVAS_A		0x60a04
+#define _PRIMPOS_A		0x60a08
+#define _PRIMSIZE_A		0x60a0c
+#define _PRIMCNSTALPHA_A	0x60a10
+#define   PRIM_CONST_ALPHA_ENABLE	(1<<31)
+
+#define CHV_BLEND(pipe) _TRANSCODER2(pipe, _CHV_BLEND_A)
+#define CHV_CANVAS(pipe) _TRANSCODER2(pipe, _CHV_CANVAS_A)
+#define PRIMPOS(plane) _TRANSCODER2(plane, _PRIMPOS_A)
+#define PRIMSIZE(plane) _TRANSCODER2(plane, _PRIMSIZE_A)
+#define PRIMCNSTALPHA(plane) _TRANSCODER2(plane, _PRIMCNSTALPHA_A)
+
 /* Display/Sprite base address macros */
 #define DISP_BASEADDR_MASK	(0xfffff000)
 #define I915_LO_DISPBASE(val)	(val & ~DISP_BASEADDR_MASK)
@@ -4464,6 +4581,7 @@ enum punit_power_well {
 #define   SP_FORMAT_RGBA1010102		(9<<26)
 #define   SP_FORMAT_RGBX8888		(0xe<<26)
 #define   SP_FORMAT_RGBA8888		(0xf<<26)
+#define   SP_ALPHA_PREMULTIPLY		(1<<23) /* CHV pipe B */
 #define   SP_SOURCE_KEY			(1<<22)
 #define   SP_YUV_BYTE_ORDER_MASK	(3<<16)
 #define   SP_YUV_ORDER_YUYV		(0<<16)
@@ -4472,6 +4590,7 @@ enum punit_power_well {
 #define   SP_YUV_ORDER_VYUY		(3<<16)
 #define   SP_ROTATE_180			(1<<15)
 #define   SP_TILED			(1<<10)
+#define   SP_MIRROR			(1<<8) /* CHV pipe B */
 #define _SPALINOFF		(VLV_DISPLAY_BASE + 0x72184)
 #define _SPASTRIDE		(VLV_DISPLAY_BASE + 0x72188)
 #define _SPAPOS			(VLV_DISPLAY_BASE + 0x7218c)
@@ -4482,6 +4601,7 @@ enum punit_power_well {
 #define _SPAKEYMAXVAL		(VLV_DISPLAY_BASE + 0x721a0)
 #define _SPATILEOFF		(VLV_DISPLAY_BASE + 0x721a4)
 #define _SPACONSTALPHA		(VLV_DISPLAY_BASE + 0x721a8)
+#define   SP_CONST_ALPHA_ENABLE		(1<<31)
 #define _SPAGAMC		(VLV_DISPLAY_BASE + 0x721f4)
 
 #define _SPBCNTR		(VLV_DISPLAY_BASE + 0x72280)
@@ -4510,6 +4630,195 @@ enum punit_power_well {
 #define SPCONSTALPHA(pipe, plane) _PIPE(pipe * 2 + plane, _SPACONSTALPHA, _SPBCONSTALPHA)
 #define SPGAMC(pipe, plane) _PIPE(pipe * 2 + plane, _SPAGAMC, _SPBGAMC)
 
+/*
+ * CHV pipe B sprite CSC
+ *
+ * |cr|   |c0 c1 c2|   |cr + cr_ioff|   |cr_ooff|
+ * |yg| = |c3 c4 c5| x |yg + yg_ioff| + |yg_ooff|
+ * |cb|   |c6 c7 c8|   |cb + cr_ioff|   |cb_ooff|
+ */
+#define SPCSCYGOFF(sprite)	(VLV_DISPLAY_BASE + 0x6d900 + (sprite) * 0x1000)
+#define SPCSCCBOFF(sprite)	(VLV_DISPLAY_BASE + 0x6d904 + (sprite) * 0x1000)
+#define SPCSCCROFF(sprite)	(VLV_DISPLAY_BASE + 0x6d908 + (sprite) * 0x1000)
+#define  SPCSC_OOFF(x)		(((x) & 0x7ff) << 16) /* s11 */
+#define  SPCSC_IOFF(x)		(((x) & 0x7ff) << 0) /* s11 */
+
+#define SPCSCC01(sprite)	(VLV_DISPLAY_BASE + 0x6d90c + (sprite) * 0x1000)
+#define SPCSCC23(sprite)	(VLV_DISPLAY_BASE + 0x6d910 + (sprite) * 0x1000)
+#define SPCSCC45(sprite)	(VLV_DISPLAY_BASE + 0x6d914 + (sprite) * 0x1000)
+#define SPCSCC67(sprite)	(VLV_DISPLAY_BASE + 0x6d918 + (sprite) * 0x1000)
+#define SPCSCC8(sprite)		(VLV_DISPLAY_BASE + 0x6d91c + (sprite) * 0x1000)
+#define  SPCSC_C1(x)		(((x) & 0x7fff) << 16) /* s3.12 */
+#define  SPCSC_C0(x)		(((x) & 0x7fff) << 0) /* s3.12 */
+
+#define SPCSCYGICLAMP(sprite)	(VLV_DISPLAY_BASE + 0x6d920 + (sprite) * 0x1000)
+#define SPCSCCBICLAMP(sprite)	(VLV_DISPLAY_BASE + 0x6d924 + (sprite) * 0x1000)
+#define SPCSCCRICLAMP(sprite)	(VLV_DISPLAY_BASE + 0x6d928 + (sprite) * 0x1000)
+#define  SPCSC_IMAX(x)		(((x) & 0x7ff) << 16) /* s11 */
+#define  SPCSC_IMIN(x)		(((x) & 0x7ff) << 0) /* s11 */
+
+#define SPCSCYGOCLAMP(sprite)	(VLV_DISPLAY_BASE + 0x6d92c + (sprite) * 0x1000)
+#define SPCSCCBOCLAMP(sprite)	(VLV_DISPLAY_BASE + 0x6d930 + (sprite) * 0x1000)
+#define SPCSCCROCLAMP(sprite)	(VLV_DISPLAY_BASE + 0x6d934 + (sprite) * 0x1000)
+#define  SPCSC_OMAX(x)		((x) << 16) /* u10 */
+#define  SPCSC_OMIN(x)		((x) << 0) /* u10 */
+
+/* Skylake plane registers */
+
+#define _PLANE_CTL_1_A				0x70180
+#define _PLANE_CTL_2_A				0x70280
+#define _PLANE_CTL_3_A				0x70380
+#define   PLANE_CTL_ENABLE			(1 << 31)
+#define   PLANE_CTL_PIPE_GAMMA_ENABLE		(1 << 30)
+#define   PLANE_CTL_FORMAT_MASK			(0xf << 24)
+#define   PLANE_CTL_FORMAT_YUV422		(  0 << 24)
+#define   PLANE_CTL_FORMAT_NV12			(  1 << 24)
+#define   PLANE_CTL_FORMAT_XRGB_2101010		(  2 << 24)
+#define   PLANE_CTL_FORMAT_XRGB_8888		(  4 << 24)
+#define   PLANE_CTL_FORMAT_XRGB_16161616F	(  6 << 24)
+#define   PLANE_CTL_FORMAT_AYUV			(  8 << 24)
+#define   PLANE_CTL_FORMAT_INDEXED		( 12 << 24)
+#define   PLANE_CTL_FORMAT_RGB_565		( 14 << 24)
+#define   PLANE_CTL_PIPE_CSC_ENABLE		(1 << 23)
+#define   PLANE_CTL_KEY_ENABLE_MASK		(0x3 << 21)
+#define   PLANE_CTL_KEY_ENABLE_SOURCE		(  1 << 21)
+#define   PLANE_CTL_KEY_ENABLE_DESTINATION	(  2 << 21)
+#define   PLANE_CTL_ORDER_BGRX			(0 << 20)
+#define   PLANE_CTL_ORDER_RGBX			(1 << 20)
+#define   PLANE_CTL_YUV422_ORDER_MASK		(0x3 << 16)
+#define   PLANE_CTL_YUV422_YUYV			(  0 << 16)
+#define   PLANE_CTL_YUV422_UYVY			(  1 << 16)
+#define   PLANE_CTL_YUV422_YVYU			(  2 << 16)
+#define   PLANE_CTL_YUV422_VYUY			(  3 << 16)
+#define   PLANE_CTL_DECOMPRESSION_ENABLE	(1 << 15)
+#define   PLANE_CTL_TRICKLE_FEED_DISABLE	(1 << 14)
+#define   PLANE_CTL_PLANE_GAMMA_DISABLE		(1 << 13)
+#define   PLANE_CTL_TILED_MASK			(0x7 << 10)
+#define   PLANE_CTL_TILED_LINEAR		(  0 << 10)
+#define   PLANE_CTL_TILED_X			(  1 << 10)
+#define   PLANE_CTL_TILED_Y			(  4 << 10)
+#define   PLANE_CTL_TILED_YF			(  5 << 10)
+#define   PLANE_CTL_ALPHA_MASK			(0x3 << 4)
+#define   PLANE_CTL_ALPHA_DISABLE		(  0 << 4)
+#define   PLANE_CTL_ALPHA_SW_PREMULTIPLY	(  2 << 4)
+#define   PLANE_CTL_ALPHA_HW_PREMULTIPLY	(  3 << 4)
+#define   PLANE_CTL_ROTATE_MASK			0x3
+#define   PLANE_CTL_ROTATE_0			0x0
+#define   PLANE_CTL_ROTATE_180			0x2
+#define _PLANE_STRIDE_1_A			0x70188
+#define _PLANE_STRIDE_2_A			0x70288
+#define _PLANE_STRIDE_3_A			0x70388
+#define _PLANE_POS_1_A				0x7018c
+#define _PLANE_POS_2_A				0x7028c
+#define _PLANE_POS_3_A				0x7038c
+#define _PLANE_SIZE_1_A				0x70190
+#define _PLANE_SIZE_2_A				0x70290
+#define _PLANE_SIZE_3_A				0x70390
+#define _PLANE_SURF_1_A				0x7019c
+#define _PLANE_SURF_2_A				0x7029c
+#define _PLANE_SURF_3_A				0x7039c
+#define _PLANE_OFFSET_1_A			0x701a4
+#define _PLANE_OFFSET_2_A			0x702a4
+#define _PLANE_OFFSET_3_A			0x703a4
+#define _PLANE_KEYVAL_1_A			0x70194
+#define _PLANE_KEYVAL_2_A			0x70294
+#define _PLANE_KEYMSK_1_A			0x70198
+#define _PLANE_KEYMSK_2_A			0x70298
+#define _PLANE_KEYMAX_1_A			0x701a0
+#define _PLANE_KEYMAX_2_A			0x702a0
+#define _PLANE_BUF_CFG_1_A			0x7027c
+#define _PLANE_BUF_CFG_2_A			0x7037c
+
+#define _PLANE_CTL_1_B				0x71180
+#define _PLANE_CTL_2_B				0x71280
+#define _PLANE_CTL_3_B				0x71380
+#define _PLANE_CTL_1(pipe)	_PIPE(pipe, _PLANE_CTL_1_A, _PLANE_CTL_1_B)
+#define _PLANE_CTL_2(pipe)	_PIPE(pipe, _PLANE_CTL_2_A, _PLANE_CTL_2_B)
+#define _PLANE_CTL_3(pipe)	_PIPE(pipe, _PLANE_CTL_3_A, _PLANE_CTL_3_B)
+#define PLANE_CTL(pipe, plane)	\
+	_PLANE(plane, _PLANE_CTL_1(pipe), _PLANE_CTL_2(pipe))
+
+#define _PLANE_STRIDE_1_B			0x71188
+#define _PLANE_STRIDE_2_B			0x71288
+#define _PLANE_STRIDE_3_B			0x71388
+#define _PLANE_STRIDE_1(pipe)	\
+	_PIPE(pipe, _PLANE_STRIDE_1_A, _PLANE_STRIDE_1_B)
+#define _PLANE_STRIDE_2(pipe)	\
+	_PIPE(pipe, _PLANE_STRIDE_2_A, _PLANE_STRIDE_2_B)
+#define _PLANE_STRIDE_3(pipe)	\
+	_PIPE(pipe, _PLANE_STRIDE_3_A, _PLANE_STRIDE_3_B)
+#define PLANE_STRIDE(pipe, plane)	\
+	_PLANE(plane, _PLANE_STRIDE_1(pipe), _PLANE_STRIDE_2(pipe))
+
+#define _PLANE_POS_1_B				0x7118c
+#define _PLANE_POS_2_B				0x7128c
+#define _PLANE_POS_3_B				0x7138c
+#define _PLANE_POS_1(pipe)	_PIPE(pipe, _PLANE_POS_1_A, _PLANE_POS_1_B)
+#define _PLANE_POS_2(pipe)	_PIPE(pipe, _PLANE_POS_2_A, _PLANE_POS_2_B)
+#define _PLANE_POS_3(pipe)	_PIPE(pipe, _PLANE_POS_3_A, _PLANE_POS_3_B)
+#define PLANE_POS(pipe, plane)	\
+	_PLANE(plane, _PLANE_POS_1(pipe), _PLANE_POS_2(pipe))
+
+#define _PLANE_SIZE_1_B				0x71190
+#define _PLANE_SIZE_2_B				0x71290
+#define _PLANE_SIZE_3_B				0x71390
+#define _PLANE_SIZE_1(pipe)	_PIPE(pipe, _PLANE_SIZE_1_A, _PLANE_SIZE_1_B)
+#define _PLANE_SIZE_2(pipe)	_PIPE(pipe, _PLANE_SIZE_2_A, _PLANE_SIZE_2_B)
+#define _PLANE_SIZE_3(pipe)	_PIPE(pipe, _PLANE_SIZE_3_A, _PLANE_SIZE_3_B)
+#define PLANE_SIZE(pipe, plane)	\
+	_PLANE(plane, _PLANE_SIZE_1(pipe), _PLANE_SIZE_2(pipe))
+
+#define _PLANE_SURF_1_B				0x7119c
+#define _PLANE_SURF_2_B				0x7129c
+#define _PLANE_SURF_3_B				0x7139c
+#define _PLANE_SURF_1(pipe)	_PIPE(pipe, _PLANE_SURF_1_A, _PLANE_SURF_1_B)
+#define _PLANE_SURF_2(pipe)	_PIPE(pipe, _PLANE_SURF_2_A, _PLANE_SURF_2_B)
+#define _PLANE_SURF_3(pipe)	_PIPE(pipe, _PLANE_SURF_3_A, _PLANE_SURF_3_B)
+#define PLANE_SURF(pipe, plane)	\
+	_PLANE(plane, _PLANE_SURF_1(pipe), _PLANE_SURF_2(pipe))
+
+#define _PLANE_OFFSET_1_B			0x711a4
+#define _PLANE_OFFSET_2_B			0x712a4
+#define _PLANE_OFFSET_1(pipe) _PIPE(pipe, _PLANE_OFFSET_1_A, _PLANE_OFFSET_1_B)
+#define _PLANE_OFFSET_2(pipe) _PIPE(pipe, _PLANE_OFFSET_2_A, _PLANE_OFFSET_2_B)
+#define PLANE_OFFSET(pipe, plane)	\
+	_PLANE(plane, _PLANE_OFFSET_1(pipe), _PLANE_OFFSET_2(pipe))
+
+#define _PLANE_KEYVAL_1_B			0x71194
+#define _PLANE_KEYVAL_2_B			0x71294
+#define _PLANE_KEYVAL_1(pipe) _PIPE(pipe, _PLANE_KEYVAL_1_A, _PLANE_KEYVAL_1_B)
+#define _PLANE_KEYVAL_2(pipe) _PIPE(pipe, _PLANE_KEYVAL_2_A, _PLANE_KEYVAL_2_B)
+#define PLANE_KEYVAL(pipe, plane)	\
+	_PLANE(plane, _PLANE_KEYVAL_1(pipe), _PLANE_KEYVAL_2(pipe))
+
+#define _PLANE_KEYMSK_1_B			0x71198
+#define _PLANE_KEYMSK_2_B			0x71298
+#define _PLANE_KEYMSK_1(pipe) _PIPE(pipe, _PLANE_KEYMSK_1_A, _PLANE_KEYMSK_1_B)
+#define _PLANE_KEYMSK_2(pipe) _PIPE(pipe, _PLANE_KEYMSK_2_A, _PLANE_KEYMSK_2_B)
+#define PLANE_KEYMSK(pipe, plane)	\
+	_PLANE(plane, _PLANE_KEYMSK_1(pipe), _PLANE_KEYMSK_2(pipe))
+
+#define _PLANE_KEYMAX_1_B			0x711a0
+#define _PLANE_KEYMAX_2_B			0x712a0
+#define _PLANE_KEYMAX_1(pipe) _PIPE(pipe, _PLANE_KEYMAX_1_A, _PLANE_KEYMAX_1_B)
+#define _PLANE_KEYMAX_2(pipe) _PIPE(pipe, _PLANE_KEYMAX_2_A, _PLANE_KEYMAX_2_B)
+#define PLANE_KEYMAX(pipe, plane)	\
+	_PLANE(plane, _PLANE_KEYMAX_1(pipe), _PLANE_KEYMAX_2(pipe))
+
+#define _PLANE_BUF_CFG_1_B			0x7127c
+#define _PLANE_BUF_CFG_2_B			0x7137c
+#define _PLANE_BUF_CFG_1(pipe)	\
+	_PIPE(pipe, _PLANE_BUF_CFG_1_A, _PLANE_BUF_CFG_1_B)
+#define _PLANE_BUF_CFG_2(pipe)	\
+	_PIPE(pipe, _PLANE_BUF_CFG_2_A, _PLANE_BUF_CFG_2_B)
+#define PLANE_BUF_CFG(pipe, plane)	\
+	_PLANE(plane, _PLANE_BUF_CFG_1(pipe), _PLANE_BUF_CFG_2(pipe))
+
+/* SKL new cursor registers */
+#define _CUR_BUF_CFG_A				0x7017c
+#define _CUR_BUF_CFG_B				0x7117c
+#define CUR_BUF_CFG(pipe)	_PIPE(pipe, _CUR_BUF_CFG_A, _CUR_BUF_CFG_B)
+
 /* VBIOS regs */
 #define VGACNTRL		0x71400
 # define VGA_DISP_DISABLE			(1 << 31)
@@ -4625,6 +4934,18 @@ enum punit_power_well {
 #define PF_VSCALE(pipe)		_PIPE(pipe, _PFA_VSCALE, _PFB_VSCALE)
 #define PF_HSCALE(pipe)		_PIPE(pipe, _PFA_HSCALE, _PFB_HSCALE)
 
+#define _PSA_CTL		0x68180
+#define _PSB_CTL		0x68980
+#define PS_ENABLE		(1<<31)
+#define _PSA_WIN_SZ		0x68174
+#define _PSB_WIN_SZ		0x68974
+#define _PSA_WIN_POS		0x68170
+#define _PSB_WIN_POS		0x68970
+
+#define PS_CTL(pipe)		_PIPE(pipe, _PSA_CTL, _PSB_CTL)
+#define PS_WIN_SZ(pipe)		_PIPE(pipe, _PSA_WIN_SZ, _PSB_WIN_SZ)
+#define PS_WIN_POS(pipe)	_PIPE(pipe, _PSA_WIN_POS, _PSB_WIN_POS)
+
 /* legacy palette */
 #define _LGC_PALETTE_A           0x4a000
 #define _LGC_PALETTE_B           0x4a800
@@ -4746,16 +5067,32 @@ enum punit_power_well {
 #define  GEN8_PIPE_SCAN_LINE_EVENT	(1 << 2)
 #define  GEN8_PIPE_VSYNC		(1 << 1)
 #define  GEN8_PIPE_VBLANK		(1 << 0)
+#define  GEN9_PIPE_CURSOR_FAULT		(1 << 11)
+#define  GEN9_PIPE_PLANE3_FAULT		(1 << 9)
+#define  GEN9_PIPE_PLANE2_FAULT		(1 << 8)
+#define  GEN9_PIPE_PLANE1_FAULT		(1 << 7)
+#define  GEN9_PIPE_PLANE3_FLIP_DONE	(1 << 5)
+#define  GEN9_PIPE_PLANE2_FLIP_DONE	(1 << 4)
+#define  GEN9_PIPE_PLANE1_FLIP_DONE	(1 << 3)
+#define  GEN9_PIPE_PLANE_FLIP_DONE(p)	(1 << (3 + p))
 #define GEN8_DE_PIPE_IRQ_FAULT_ERRORS \
 	(GEN8_PIPE_CURSOR_FAULT | \
 	 GEN8_PIPE_SPRITE_FAULT | \
 	 GEN8_PIPE_PRIMARY_FAULT)
+#define GEN9_DE_PIPE_IRQ_FAULT_ERRORS \
+	(GEN9_PIPE_CURSOR_FAULT | \
+	 GEN9_PIPE_PLANE3_FAULT | \
+	 GEN9_PIPE_PLANE2_FAULT | \
+	 GEN9_PIPE_PLANE1_FAULT)
 
 #define GEN8_DE_PORT_ISR 0x44440
 #define GEN8_DE_PORT_IMR 0x44444
 #define GEN8_DE_PORT_IIR 0x44448
 #define GEN8_DE_PORT_IER 0x4444c
 #define  GEN8_PORT_DP_A_HOTPLUG		(1 << 3)
+#define  GEN9_AUX_CHANNEL_D		(1 << 27)
+#define  GEN9_AUX_CHANNEL_C		(1 << 26)
+#define  GEN9_AUX_CHANNEL_B		(1 << 25)
 #define  GEN8_AUX_CHANNEL_A		(1 << 0)
 
 #define GEN8_DE_MISC_ISR 0x44460
@@ -4839,6 +5176,8 @@ enum punit_power_well {
 /* GEN8 chicken */
 #define HDC_CHICKEN0				0x7300
 #define  HDC_FORCE_NON_COHERENT			(1<<4)
+#define  HDC_DONOT_FETCH_MEM_WHEN_MASKED	(1<<11)
+#define  HDC_FENCE_DEST_SLM_DISABLE		(1<<14)
 
 /* WaCatErrorRejectionIssue */
 #define GEN7_SQ_CHICKEN_MBCUNIT_CONFIG		0x9030
@@ -5540,6 +5879,12 @@ enum punit_power_well {
 #define   VLV_GTLC_PW_MEDIA_STATUS_MASK		(1 << 5)
 #define   VLV_GTLC_PW_RENDER_STATUS_MASK	(1 << 7)
 #define  FORCEWAKE_MT				0xa188 /* multi-threaded */
+#define  FORCEWAKE_MEDIA_GEN9			0xa270
+#define  FORCEWAKE_RENDER_GEN9			0xa278
+#define  FORCEWAKE_BLITTER_GEN9			0xa188
+#define  FORCEWAKE_ACK_MEDIA_GEN9		0x0D88
+#define  FORCEWAKE_ACK_RENDER_GEN9		0x0D84
+#define  FORCEWAKE_ACK_BLITTER_GEN9		0x130044
 #define   FORCEWAKE_KERNEL			0x1
 #define   FORCEWAKE_USER			0x2
 #define  FORCEWAKE_MT_ACK			0x130040
@@ -5711,9 +6056,17 @@ enum punit_power_well {
 #define   GEN6_ENCODE_RC6_VID(mv)		(((mv) - 245) / 5)
 #define   GEN6_DECODE_RC6_VID(vids)		(((vids) * 5) + 245)
 #define   DISPLAY_IPS_CONTROL			0x19
+#define	  HSW_PCODE_DYNAMIC_DUTY_CYCLE_CONTROL	0x1A
 #define GEN6_PCODE_DATA				0x138128
 #define   GEN6_PCODE_FREQ_IA_RATIO_SHIFT	8
 #define   GEN6_PCODE_FREQ_RING_RATIO_SHIFT	16
+#define GEN6_PCODE_DATA1			0x13812C
+
+#define   GEN9_PCODE_READ_MEM_LATENCY		0x6
+#define   GEN9_MEM_LATENCY_LEVEL_MASK		0xFF
+#define   GEN9_MEM_LATENCY_LEVEL_1_5_SHIFT	8
+#define   GEN9_MEM_LATENCY_LEVEL_2_6_SHIFT	16
+#define   GEN9_MEM_LATENCY_LEVEL_3_7_SHIFT	24
 
 #define GEN6_GT_CORE_STATUS		0x138060
 #define   GEN6_CORE_CPD_STATE_MASK	(7<<4)
@@ -5751,6 +6104,9 @@ enum punit_power_well {
 #define   GEN7_SINGLE_SUBSCAN_DISPATCH_ENABLE	(1<<10)
 #define   GEN7_PSD_SINGLE_PORT_DISPATCH_ENABLE	(1<<3)
 
+#define GEN9_HALF_SLICE_CHICKEN5	0xe188
+#define   GEN9_DG_MIRROR_FIX_ENABLE	(1<<5)
+
 #define GEN8_ROW_CHICKEN		0xe4f0
 #define   PARTIAL_INSTRUCTION_SHOOTDOWN_DISABLE	(1<<8)
 #define   STALL_DOP_GATING_DISABLE		(1<<5)
@@ -5766,57 +6122,58 @@ enum punit_power_well {
 #define   GEN8_CENTROID_PIXEL_OPT_DIS	(1<<8)
 #define   GEN8_SAMPLER_POWER_BYPASS_DIS	(1<<1)
 
+/* Audio */
 #define G4X_AUD_VID_DID			(dev_priv->info.display_mmio_offset + 0x62020)
-#define INTEL_AUDIO_DEVCL		0x808629FB
-#define INTEL_AUDIO_DEVBLC		0x80862801
-#define INTEL_AUDIO_DEVCTG		0x80862802
+#define   INTEL_AUDIO_DEVCL		0x808629FB
+#define   INTEL_AUDIO_DEVBLC		0x80862801
+#define   INTEL_AUDIO_DEVCTG		0x80862802
 
 #define G4X_AUD_CNTL_ST			0x620B4
-#define G4X_ELDV_DEVCL_DEVBLC		(1 << 13)
-#define G4X_ELDV_DEVCTG			(1 << 14)
-#define G4X_ELD_ADDR			(0xf << 5)
-#define G4X_ELD_ACK			(1 << 4)
+#define   G4X_ELDV_DEVCL_DEVBLC		(1 << 13)
+#define   G4X_ELDV_DEVCTG		(1 << 14)
+#define   G4X_ELD_ADDR_MASK		(0xf << 5)
+#define   G4X_ELD_ACK			(1 << 4)
 #define G4X_HDMIW_HDMIEDID		0x6210C
 
-#define IBX_HDMIW_HDMIEDID_A		0xE2050
-#define IBX_HDMIW_HDMIEDID_B		0xE2150
+#define _IBX_HDMIW_HDMIEDID_A		0xE2050
+#define _IBX_HDMIW_HDMIEDID_B		0xE2150
 #define IBX_HDMIW_HDMIEDID(pipe) _PIPE(pipe, \
-					IBX_HDMIW_HDMIEDID_A, \
-					IBX_HDMIW_HDMIEDID_B)
-#define IBX_AUD_CNTL_ST_A		0xE20B4
-#define IBX_AUD_CNTL_ST_B		0xE21B4
+					_IBX_HDMIW_HDMIEDID_A, \
+					_IBX_HDMIW_HDMIEDID_B)
+#define _IBX_AUD_CNTL_ST_A		0xE20B4
+#define _IBX_AUD_CNTL_ST_B		0xE21B4
 #define IBX_AUD_CNTL_ST(pipe) _PIPE(pipe, \
-					IBX_AUD_CNTL_ST_A, \
-					IBX_AUD_CNTL_ST_B)
-#define IBX_ELD_BUFFER_SIZE		(0x1f << 10)
-#define IBX_ELD_ADDRESS			(0x1f << 5)
-#define IBX_ELD_ACK			(1 << 4)
+					_IBX_AUD_CNTL_ST_A, \
+					_IBX_AUD_CNTL_ST_B)
+#define   IBX_ELD_BUFFER_SIZE_MASK	(0x1f << 10)
+#define   IBX_ELD_ADDRESS_MASK		(0x1f << 5)
+#define   IBX_ELD_ACK			(1 << 4)
 #define IBX_AUD_CNTL_ST2		0xE20C0
-#define IBX_ELD_VALIDB			(1 << 0)
-#define IBX_CP_READYB			(1 << 1)
+#define   IBX_CP_READY(port)		((1 << 1) << (((port) - 1) * 4))
+#define   IBX_ELD_VALID(port)		((1 << 0) << (((port) - 1) * 4))
 
-#define CPT_HDMIW_HDMIEDID_A		0xE5050
-#define CPT_HDMIW_HDMIEDID_B		0xE5150
+#define _CPT_HDMIW_HDMIEDID_A		0xE5050
+#define _CPT_HDMIW_HDMIEDID_B		0xE5150
 #define CPT_HDMIW_HDMIEDID(pipe) _PIPE(pipe, \
-					CPT_HDMIW_HDMIEDID_A, \
-					CPT_HDMIW_HDMIEDID_B)
-#define CPT_AUD_CNTL_ST_A		0xE50B4
-#define CPT_AUD_CNTL_ST_B		0xE51B4
+					_CPT_HDMIW_HDMIEDID_A, \
+					_CPT_HDMIW_HDMIEDID_B)
+#define _CPT_AUD_CNTL_ST_A		0xE50B4
+#define _CPT_AUD_CNTL_ST_B		0xE51B4
 #define CPT_AUD_CNTL_ST(pipe) _PIPE(pipe, \
-					CPT_AUD_CNTL_ST_A, \
-					CPT_AUD_CNTL_ST_B)
+					_CPT_AUD_CNTL_ST_A, \
+					_CPT_AUD_CNTL_ST_B)
 #define CPT_AUD_CNTRL_ST2		0xE50C0
 
-#define VLV_HDMIW_HDMIEDID_A		(VLV_DISPLAY_BASE + 0x62050)
-#define VLV_HDMIW_HDMIEDID_B		(VLV_DISPLAY_BASE + 0x62150)
+#define _VLV_HDMIW_HDMIEDID_A		(VLV_DISPLAY_BASE + 0x62050)
+#define _VLV_HDMIW_HDMIEDID_B		(VLV_DISPLAY_BASE + 0x62150)
 #define VLV_HDMIW_HDMIEDID(pipe) _PIPE(pipe, \
-					VLV_HDMIW_HDMIEDID_A, \
-					VLV_HDMIW_HDMIEDID_B)
-#define VLV_AUD_CNTL_ST_A		(VLV_DISPLAY_BASE + 0x620B4)
-#define VLV_AUD_CNTL_ST_B		(VLV_DISPLAY_BASE + 0x621B4)
+					_VLV_HDMIW_HDMIEDID_A, \
+					_VLV_HDMIW_HDMIEDID_B)
+#define _VLV_AUD_CNTL_ST_A		(VLV_DISPLAY_BASE + 0x620B4)
+#define _VLV_AUD_CNTL_ST_B		(VLV_DISPLAY_BASE + 0x621B4)
 #define VLV_AUD_CNTL_ST(pipe) _PIPE(pipe, \
-					VLV_AUD_CNTL_ST_A, \
-					VLV_AUD_CNTL_ST_B)
+					_VLV_AUD_CNTL_ST_A, \
+					_VLV_AUD_CNTL_ST_B)
 #define VLV_AUD_CNTL_ST2		(VLV_DISPLAY_BASE + 0x620C0)
 
 /* These are the 4 32-bit write offset registers for each stream
@@ -5825,28 +6182,28 @@ enum punit_power_well {
  */
 #define GEN7_SO_WRITE_OFFSET(n)		(0x5280 + (n) * 4)
 
-#define IBX_AUD_CONFIG_A			0xe2000
-#define IBX_AUD_CONFIG_B			0xe2100
+#define _IBX_AUD_CONFIG_A		0xe2000
+#define _IBX_AUD_CONFIG_B		0xe2100
 #define IBX_AUD_CFG(pipe) _PIPE(pipe, \
-					IBX_AUD_CONFIG_A, \
-					IBX_AUD_CONFIG_B)
-#define CPT_AUD_CONFIG_A			0xe5000
-#define CPT_AUD_CONFIG_B			0xe5100
+					_IBX_AUD_CONFIG_A, \
+					_IBX_AUD_CONFIG_B)
+#define _CPT_AUD_CONFIG_A		0xe5000
+#define _CPT_AUD_CONFIG_B		0xe5100
 #define CPT_AUD_CFG(pipe) _PIPE(pipe, \
-					CPT_AUD_CONFIG_A, \
-					CPT_AUD_CONFIG_B)
-#define VLV_AUD_CONFIG_A		(VLV_DISPLAY_BASE + 0x62000)
-#define VLV_AUD_CONFIG_B		(VLV_DISPLAY_BASE + 0x62100)
+					_CPT_AUD_CONFIG_A, \
+					_CPT_AUD_CONFIG_B)
+#define _VLV_AUD_CONFIG_A		(VLV_DISPLAY_BASE + 0x62000)
+#define _VLV_AUD_CONFIG_B		(VLV_DISPLAY_BASE + 0x62100)
 #define VLV_AUD_CFG(pipe) _PIPE(pipe, \
-					VLV_AUD_CONFIG_A, \
-					VLV_AUD_CONFIG_B)
+					_VLV_AUD_CONFIG_A, \
+					_VLV_AUD_CONFIG_B)
 
 #define   AUD_CONFIG_N_VALUE_INDEX		(1 << 29)
 #define   AUD_CONFIG_N_PROG_ENABLE		(1 << 28)
 #define   AUD_CONFIG_UPPER_N_SHIFT		20
-#define   AUD_CONFIG_UPPER_N_VALUE		(0xff << 20)
+#define   AUD_CONFIG_UPPER_N_MASK		(0xff << 20)
 #define   AUD_CONFIG_LOWER_N_SHIFT		4
-#define   AUD_CONFIG_LOWER_N_VALUE		(0xfff << 4)
+#define   AUD_CONFIG_LOWER_N_MASK		(0xfff << 4)
 #define   AUD_CONFIG_PIXEL_CLOCK_HDMI_SHIFT	16
 #define   AUD_CONFIG_PIXEL_CLOCK_HDMI_MASK	(0xf << 16)
 #define   AUD_CONFIG_PIXEL_CLOCK_HDMI_25175	(0 << 16)
@@ -5862,52 +6219,44 @@ enum punit_power_well {
 #define   AUD_CONFIG_DISABLE_NCTS		(1 << 3)
 
 /* HSW Audio */
-#define   HSW_AUD_CONFIG_A		0x65000 /* Audio Configuration Transcoder A */
-#define   HSW_AUD_CONFIG_B		0x65100 /* Audio Configuration Transcoder B */
-#define   HSW_AUD_CFG(pipe) _PIPE(pipe, \
-					HSW_AUD_CONFIG_A, \
-					HSW_AUD_CONFIG_B)
-
-#define   HSW_AUD_MISC_CTRL_A		0x65010 /* Audio Misc Control Convert 1 */
-#define   HSW_AUD_MISC_CTRL_B		0x65110 /* Audio Misc Control Convert 2 */
-#define   HSW_AUD_MISC_CTRL(pipe) _PIPE(pipe, \
-					HSW_AUD_MISC_CTRL_A, \
-					HSW_AUD_MISC_CTRL_B)
-
-#define   HSW_AUD_DIP_ELD_CTRL_ST_A	0x650b4 /* Audio DIP and ELD Control State Transcoder A */
-#define   HSW_AUD_DIP_ELD_CTRL_ST_B	0x651b4 /* Audio DIP and ELD Control State Transcoder B */
-#define   HSW_AUD_DIP_ELD_CTRL(pipe) _PIPE(pipe, \
-					HSW_AUD_DIP_ELD_CTRL_ST_A, \
-					HSW_AUD_DIP_ELD_CTRL_ST_B)
+#define _HSW_AUD_CONFIG_A		0x65000
+#define _HSW_AUD_CONFIG_B		0x65100
+#define HSW_AUD_CFG(pipe) _PIPE(pipe, \
+					_HSW_AUD_CONFIG_A, \
+					_HSW_AUD_CONFIG_B)
+
+#define _HSW_AUD_MISC_CTRL_A		0x65010
+#define _HSW_AUD_MISC_CTRL_B		0x65110
+#define HSW_AUD_MISC_CTRL(pipe) _PIPE(pipe, \
+					_HSW_AUD_MISC_CTRL_A, \
+					_HSW_AUD_MISC_CTRL_B)
+
+#define _HSW_AUD_DIP_ELD_CTRL_ST_A	0x650b4
+#define _HSW_AUD_DIP_ELD_CTRL_ST_B	0x651b4
+#define HSW_AUD_DIP_ELD_CTRL(pipe) _PIPE(pipe, \
+					_HSW_AUD_DIP_ELD_CTRL_ST_A, \
+					_HSW_AUD_DIP_ELD_CTRL_ST_B)
 
 /* Audio Digital Converter */
-#define   HSW_AUD_DIG_CNVT_1		0x65080 /* Audio Converter 1 */
-#define   HSW_AUD_DIG_CNVT_2		0x65180 /* Audio Converter 1 */
-#define   AUD_DIG_CNVT(pipe) _PIPE(pipe, \
-					HSW_AUD_DIG_CNVT_1, \
-					HSW_AUD_DIG_CNVT_2)
-#define   DIP_PORT_SEL_MASK		0x3
-
-#define   HSW_AUD_EDID_DATA_A		0x65050
-#define   HSW_AUD_EDID_DATA_B		0x65150
-#define   HSW_AUD_EDID_DATA(pipe) _PIPE(pipe, \
-					HSW_AUD_EDID_DATA_A, \
-					HSW_AUD_EDID_DATA_B)
-
-#define   HSW_AUD_PIPE_CONV_CFG		0x6507c /* Audio pipe and converter configs */
-#define   HSW_AUD_PIN_ELD_CP_VLD	0x650c0 /* Audio ELD and CP Ready Status */
-#define   AUDIO_INACTIVE_C		(1<<11)
-#define   AUDIO_INACTIVE_B		(1<<7)
-#define   AUDIO_INACTIVE_A		(1<<3)
-#define   AUDIO_OUTPUT_ENABLE_A		(1<<2)
-#define   AUDIO_OUTPUT_ENABLE_B		(1<<6)
-#define   AUDIO_OUTPUT_ENABLE_C		(1<<10)
-#define   AUDIO_ELD_VALID_A		(1<<0)
-#define   AUDIO_ELD_VALID_B		(1<<4)
-#define   AUDIO_ELD_VALID_C		(1<<8)
-#define   AUDIO_CP_READY_A		(1<<1)
-#define   AUDIO_CP_READY_B		(1<<5)
-#define   AUDIO_CP_READY_C		(1<<9)
+#define _HSW_AUD_DIG_CNVT_1		0x65080
+#define _HSW_AUD_DIG_CNVT_2		0x65180
+#define AUD_DIG_CNVT(pipe) _PIPE(pipe, \
+					_HSW_AUD_DIG_CNVT_1, \
+					_HSW_AUD_DIG_CNVT_2)
+#define DIP_PORT_SEL_MASK		0x3
+
+#define _HSW_AUD_EDID_DATA_A		0x65050
+#define _HSW_AUD_EDID_DATA_B		0x65150
+#define HSW_AUD_EDID_DATA(pipe) _PIPE(pipe, \
+					_HSW_AUD_EDID_DATA_A, \
+					_HSW_AUD_EDID_DATA_B)
+
+#define HSW_AUD_PIPE_CONV_CFG		0x6507c
+#define HSW_AUD_PIN_ELD_CP_VLD		0x650c0
+#define   AUDIO_INACTIVE(trans)		((1 << 3) << ((trans) * 4))
+#define   AUDIO_OUTPUT_ENABLE(trans)	((1 << 2) << ((trans) * 4))
+#define   AUDIO_CP_READY(trans)		((1 << 1) << ((trans) * 4))
+#define   AUDIO_ELD_VALID(trans)	((1 << 0) << ((trans) * 4))
 
 /* HSW Power Wells */
 #define HSW_PWR_WELL_BIOS			0x45400 /* CTL1 */
@@ -6125,6 +6474,83 @@ enum punit_power_well {
 #define  LCPLL_CD_SOURCE_FCLK		(1<<21)
 #define  LCPLL_CD_SOURCE_FCLK_DONE	(1<<19)
 
+/*
+ * SKL Clocks
+ */
+
+/* CDCLK_CTL */
+#define CDCLK_CTL			0x46000
+#define  CDCLK_FREQ_SEL_MASK		(3<<26)
+#define  CDCLK_FREQ_450_432		(0<<26)
+#define  CDCLK_FREQ_540			(1<<26)
+#define  CDCLK_FREQ_337_308		(2<<26)
+#define  CDCLK_FREQ_675_617		(3<<26)
+#define  CDCLK_FREQ_DECIMAL_MASK	(0x7ff)
+
+/* LCPLL_CTL */
+#define LCPLL1_CTL		0x46010
+#define LCPLL2_CTL		0x46014
+#define  LCPLL_PLL_ENABLE	(1<<31)
+
+/* DPLL control1 */
+#define DPLL_CTRL1		0x6C058
+#define  DPLL_CTRL1_HDMI_MODE(id)		(1<<((id)*6+5))
+#define  DPLL_CTRL1_SSC(id)			(1<<((id)*6+4))
+#define  DPLL_CRTL1_LINK_RATE_MASK(id)		(7<<((id)*6+1))
+#define  DPLL_CRTL1_LINK_RATE_SHIFT(id)		((id)*6+1)
+#define  DPLL_CRTL1_LINK_RATE(linkrate, id)	((linkrate)<<((id)*6+1))
+#define  DPLL_CTRL1_OVERRIDE(id)		(1<<((id)*6))
+#define  DPLL_CRTL1_LINK_RATE_2700		0
+#define  DPLL_CRTL1_LINK_RATE_1350		1
+#define  DPLL_CRTL1_LINK_RATE_810		2
+#define  DPLL_CRTL1_LINK_RATE_1620		3
+#define  DPLL_CRTL1_LINK_RATE_1080		4
+#define  DPLL_CRTL1_LINK_RATE_2160		5
+
+/* DPLL control2 */
+#define DPLL_CTRL2				0x6C05C
+#define  DPLL_CTRL2_DDI_CLK_OFF(port)		(1<<(port+15))
+#define  DPLL_CTRL2_DDI_CLK_SEL_MASK(port)	(3<<((port)*3+1))
+#define  DPLL_CTRL2_DDI_CLK_SEL_SHIFT(port)    ((port)*3+1)
+#define  DPLL_CTRL2_DDI_CLK_SEL(clk, port)	(clk<<((port)*3+1))
+#define  DPLL_CTRL2_DDI_SEL_OVERRIDE(port)     (1<<((port)*3))
+
+/* DPLL Status */
+#define DPLL_STATUS	0x6C060
+#define  DPLL_LOCK(id) (1<<((id)*8))
+
+/* DPLL cfg */
+#define DPLL1_CFGCR1	0x6C040
+#define DPLL2_CFGCR1	0x6C048
+#define DPLL3_CFGCR1	0x6C050
+#define  DPLL_CFGCR1_FREQ_ENABLE	(1<<31)
+#define  DPLL_CFGCR1_DCO_FRACTION_MASK	(0x7fff<<9)
+#define  DPLL_CFGCR1_DCO_FRACTION(x)	(x<<9)
+#define  DPLL_CFGCR1_DCO_INTEGER_MASK	(0x1ff)
+
+#define DPLL1_CFGCR2	0x6C044
+#define DPLL2_CFGCR2	0x6C04C
+#define DPLL3_CFGCR2	0x6C054
+#define  DPLL_CFGCR2_QDIV_RATIO_MASK	(0xff<<8)
+#define  DPLL_CFGCR2_QDIV_RATIO(x)	(x<<8)
+#define  DPLL_CFGCR2_QDIV_MODE(x)	(x<<7)
+#define  DPLL_CFGCR2_KDIV_MASK		(3<<5)
+#define  DPLL_CFGCR2_KDIV(x)		(x<<5)
+#define  DPLL_CFGCR2_KDIV_5 (0<<5)
+#define  DPLL_CFGCR2_KDIV_2 (1<<5)
+#define  DPLL_CFGCR2_KDIV_3 (2<<5)
+#define  DPLL_CFGCR2_KDIV_1 (3<<5)
+#define  DPLL_CFGCR2_PDIV_MASK		(7<<2)
+#define  DPLL_CFGCR2_PDIV(x)		(x<<2)
+#define  DPLL_CFGCR2_PDIV_1 (0<<2)
+#define  DPLL_CFGCR2_PDIV_2 (1<<2)
+#define  DPLL_CFGCR2_PDIV_3 (2<<2)
+#define  DPLL_CFGCR2_PDIV_7 (4<<2)
+#define  DPLL_CFGCR2_CENTRAL_FREQ_MASK	(3)
+
+#define GET_CFG_CR1_REG(id) (DPLL1_CFGCR1 + (id - SKL_DPLL1) * 8)
+#define GET_CFG_CR2_REG(id) (DPLL1_CFGCR2 + (id - SKL_DPLL1) * 8)
+
 /* Please see hsw_read_dcomp() and hsw_write_dcomp() before using this register,
  * since on HSW we can't write to it using I915_WRITE. */
 #define D_COMP_HSW			(MCHBAR_MIRROR_BASE_SNB + 0x5F0C)
diff --git a/drivers/gpu/drm/i915/i915_suspend.c b/drivers/gpu/drm/i915/i915_suspend.c
index 043123c77a1f..26368822a33f 100644
--- a/drivers/gpu/drm/i915/i915_suspend.c
+++ b/drivers/gpu/drm/i915/i915_suspend.c
@@ -203,34 +203,19 @@ static void i915_save_display(struct drm_device *dev)
 		i915_save_display_reg(dev);
 
 	/* LVDS state */
-	if (HAS_PCH_SPLIT(dev)) {
-		dev_priv->regfile.savePP_CONTROL = I915_READ(PCH_PP_CONTROL);
-		if (HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev))
-			dev_priv->regfile.saveLVDS = I915_READ(PCH_LVDS);
-	} else if (IS_VALLEYVIEW(dev)) {
-		dev_priv->regfile.savePP_CONTROL = I915_READ(PP_CONTROL);
-		dev_priv->regfile.savePFIT_PGM_RATIOS = I915_READ(PFIT_PGM_RATIOS);
-
-		dev_priv->regfile.saveBLC_HIST_CTL =
-			I915_READ(VLV_BLC_HIST_CTL(PIPE_A));
-		dev_priv->regfile.saveBLC_HIST_CTL_B =
-			I915_READ(VLV_BLC_HIST_CTL(PIPE_B));
-	} else {
-		dev_priv->regfile.savePP_CONTROL = I915_READ(PP_CONTROL);
-		dev_priv->regfile.savePFIT_PGM_RATIOS = I915_READ(PFIT_PGM_RATIOS);
-		dev_priv->regfile.saveBLC_HIST_CTL = I915_READ(BLC_HIST_CTL);
-		if (IS_MOBILE(dev) && !IS_I830(dev))
-			dev_priv->regfile.saveLVDS = I915_READ(LVDS);
-	}
-
-	if (!IS_I830(dev) && !IS_845G(dev) && !HAS_PCH_SPLIT(dev))
-		dev_priv->regfile.savePFIT_CONTROL = I915_READ(PFIT_CONTROL);
+	if (HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev))
+		dev_priv->regfile.saveLVDS = I915_READ(PCH_LVDS);
+	else if (INTEL_INFO(dev)->gen <= 4 && IS_MOBILE(dev) && !IS_I830(dev))
+		dev_priv->regfile.saveLVDS = I915_READ(LVDS);
 
+	/* Panel power sequencer */
 	if (HAS_PCH_SPLIT(dev)) {
+		dev_priv->regfile.savePP_CONTROL = I915_READ(PCH_PP_CONTROL);
 		dev_priv->regfile.savePP_ON_DELAYS = I915_READ(PCH_PP_ON_DELAYS);
 		dev_priv->regfile.savePP_OFF_DELAYS = I915_READ(PCH_PP_OFF_DELAYS);
 		dev_priv->regfile.savePP_DIVISOR = I915_READ(PCH_PP_DIVISOR);
-	} else {
+	} else if (!IS_VALLEYVIEW(dev)) {
+		dev_priv->regfile.savePP_CONTROL = I915_READ(PP_CONTROL);
 		dev_priv->regfile.savePP_ON_DELAYS = I915_READ(PP_ON_DELAYS);
 		dev_priv->regfile.savePP_OFF_DELAYS = I915_READ(PP_OFF_DELAYS);
 		dev_priv->regfile.savePP_DIVISOR = I915_READ(PP_DIVISOR);
@@ -259,29 +244,19 @@ static void i915_restore_display(struct drm_device *dev)
 	if (drm_core_check_feature(dev, DRIVER_MODESET))
 		mask = ~LVDS_PORT_EN;
 
+	/* LVDS state */
 	if (HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev))
 		I915_WRITE(PCH_LVDS, dev_priv->regfile.saveLVDS & mask);
 	else if (INTEL_INFO(dev)->gen <= 4 && IS_MOBILE(dev) && !IS_I830(dev))
 		I915_WRITE(LVDS, dev_priv->regfile.saveLVDS & mask);
 
-	if (!IS_I830(dev) && !IS_845G(dev) && !HAS_PCH_SPLIT(dev))
-		I915_WRITE(PFIT_CONTROL, dev_priv->regfile.savePFIT_CONTROL);
-
+	/* Panel power sequencer */
 	if (HAS_PCH_SPLIT(dev)) {
 		I915_WRITE(PCH_PP_ON_DELAYS, dev_priv->regfile.savePP_ON_DELAYS);
 		I915_WRITE(PCH_PP_OFF_DELAYS, dev_priv->regfile.savePP_OFF_DELAYS);
 		I915_WRITE(PCH_PP_DIVISOR, dev_priv->regfile.savePP_DIVISOR);
 		I915_WRITE(PCH_PP_CONTROL, dev_priv->regfile.savePP_CONTROL);
-		I915_WRITE(RSTDBYCTL,
-			   dev_priv->regfile.saveMCHBAR_RENDER_STANDBY);
-	} else if (IS_VALLEYVIEW(dev)) {
-		I915_WRITE(VLV_BLC_HIST_CTL(PIPE_A),
-			   dev_priv->regfile.saveBLC_HIST_CTL);
-		I915_WRITE(VLV_BLC_HIST_CTL(PIPE_B),
-			   dev_priv->regfile.saveBLC_HIST_CTL);
-	} else {
-		I915_WRITE(PFIT_PGM_RATIOS, dev_priv->regfile.savePFIT_PGM_RATIOS);
-		I915_WRITE(BLC_HIST_CTL, dev_priv->regfile.saveBLC_HIST_CTL);
+	} else if (!IS_VALLEYVIEW(dev)) {
 		I915_WRITE(PP_ON_DELAYS, dev_priv->regfile.savePP_ON_DELAYS);
 		I915_WRITE(PP_OFF_DELAYS, dev_priv->regfile.savePP_OFF_DELAYS);
 		I915_WRITE(PP_DIVISOR, dev_priv->regfile.savePP_DIVISOR);
@@ -328,6 +303,10 @@ int i915_save_state(struct drm_device *dev)
 		}
 	}
 
+	if (IS_GEN4(dev))
+		pci_read_config_word(dev->pdev, GCDGMBUS,
+				     &dev_priv->regfile.saveGCDGMBUS);
+
 	/* Cache mode state */
 	if (INTEL_INFO(dev)->gen < 7)
 		dev_priv->regfile.saveCACHE_MODE_0 = I915_READ(CACHE_MODE_0);
@@ -356,6 +335,10 @@ int i915_restore_state(struct drm_device *dev)
 	mutex_lock(&dev->struct_mutex);
 
 	i915_gem_restore_fences(dev);
+
+	if (IS_GEN4(dev))
+		pci_write_config_word(dev->pdev, GCDGMBUS,
+				      dev_priv->regfile.saveGCDGMBUS);
 	i915_restore_display(dev);
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET)) {
@@ -368,6 +351,8 @@ int i915_restore_state(struct drm_device *dev)
 			I915_WRITE(_FDI_RXA_IMR, dev_priv->regfile.saveFDI_RXA_IMR);
 			I915_WRITE(_FDI_RXB_IMR, dev_priv->regfile.saveFDI_RXB_IMR);
 			I915_WRITE(PCH_PORT_HOTPLUG, dev_priv->regfile.savePCH_PORT_HOTPLUG);
+			I915_WRITE(RSTDBYCTL,
+				   dev_priv->regfile.saveMCHBAR_RENDER_STANDBY);
 		} else {
 			I915_WRITE(IER, dev_priv->regfile.saveIER);
 			I915_WRITE(IMR, dev_priv->regfile.saveIMR);
diff --git a/drivers/gpu/drm/i915/i915_sysfs.c b/drivers/gpu/drm/i915/i915_sysfs.c
index 503847f18fdd..4a5af695307e 100644
--- a/drivers/gpu/drm/i915/i915_sysfs.c
+++ b/drivers/gpu/drm/i915/i915_sysfs.c
@@ -139,8 +139,6 @@ 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
 };
 
@@ -148,6 +146,17 @@ static struct attribute_group rc6_attr_group = {
 	.name = power_group_name,
 	.attrs =  rc6_attrs
 };
+
+static struct attribute *rc6p_attrs[] = {
+	&dev_attr_rc6p_residency_ms.attr,
+	&dev_attr_rc6pp_residency_ms.attr,
+	NULL
+};
+
+static struct attribute_group rc6p_attr_group = {
+	.name = power_group_name,
+	.attrs =  rc6p_attrs
+};
 #endif
 
 static int l3_access_valid(struct drm_device *dev, loff_t offset)
@@ -595,12 +604,18 @@ void i915_setup_sysfs(struct drm_device *dev)
 	int ret;
 
 #ifdef CONFIG_PM
-	if (INTEL_INFO(dev)->gen >= 6) {
+	if (HAS_RC6(dev)) {
 		ret = sysfs_merge_group(&dev->primary->kdev->kobj,
 					&rc6_attr_group);
 		if (ret)
 			DRM_ERROR("RC6 residency sysfs setup failed\n");
 	}
+	if (HAS_RC6p(dev)) {
+		ret = sysfs_merge_group(&dev->primary->kdev->kobj,
+					&rc6p_attr_group);
+		if (ret)
+			DRM_ERROR("RC6p residency sysfs setup failed\n");
+	}
 #endif
 	if (HAS_L3_DPF(dev)) {
 		ret = device_create_bin_file(dev->primary->kdev, &dpf_attrs);
@@ -640,5 +655,6 @@ void i915_teardown_sysfs(struct drm_device *dev)
 	device_remove_bin_file(dev->primary->kdev,  &dpf_attrs);
 #ifdef CONFIG_PM
 	sysfs_unmerge_group(&dev->primary->kdev->kobj, &rc6_attr_group);
+	sysfs_unmerge_group(&dev->primary->kdev->kobj, &rc6p_attr_group);
 #endif
 }
diff --git a/drivers/gpu/drm/i915/i915_trace.h b/drivers/gpu/drm/i915/i915_trace.h
index f5aa0067755a..751d4ad14d62 100644
--- a/drivers/gpu/drm/i915/i915_trace.h
+++ b/drivers/gpu/drm/i915/i915_trace.h
@@ -587,6 +587,110 @@ TRACE_EVENT(intel_gpu_freq_change,
 	    TP_printk("new_freq=%u", __entry->freq)
 );
 
+/**
+ * DOC: i915_ppgtt_create and i915_ppgtt_release tracepoints
+ *
+ * With full ppgtt enabled each process using drm will allocate at least one
+ * translation table. With these traces it is possible to keep track of the
+ * allocation and of the lifetime of the tables; this can be used during
+ * testing/debug to verify that we are not leaking ppgtts.
+ * These traces identify the ppgtt through the vm pointer, which is also printed
+ * by the i915_vma_bind and i915_vma_unbind tracepoints.
+ */
+DECLARE_EVENT_CLASS(i915_ppgtt,
+	TP_PROTO(struct i915_address_space *vm),
+	TP_ARGS(vm),
+
+	TP_STRUCT__entry(
+			__field(struct i915_address_space *, vm)
+			__field(u32, dev)
+	),
+
+	TP_fast_assign(
+			__entry->vm = vm;
+			__entry->dev = vm->dev->primary->index;
+	),
+
+	TP_printk("dev=%u, vm=%p", __entry->dev, __entry->vm)
+)
+
+DEFINE_EVENT(i915_ppgtt, i915_ppgtt_create,
+	TP_PROTO(struct i915_address_space *vm),
+	TP_ARGS(vm)
+);
+
+DEFINE_EVENT(i915_ppgtt, i915_ppgtt_release,
+	TP_PROTO(struct i915_address_space *vm),
+	TP_ARGS(vm)
+);
+
+/**
+ * DOC: i915_context_create and i915_context_free tracepoints
+ *
+ * These tracepoints are used to track creation and deletion of contexts.
+ * If full ppgtt is enabled, they also print the address of the vm assigned to
+ * the context.
+ */
+DECLARE_EVENT_CLASS(i915_context,
+	TP_PROTO(struct intel_context *ctx),
+	TP_ARGS(ctx),
+
+	TP_STRUCT__entry(
+			__field(u32, dev)
+			__field(struct intel_context *, ctx)
+			__field(struct i915_address_space *, vm)
+	),
+
+	TP_fast_assign(
+			__entry->ctx = ctx;
+			__entry->vm = ctx->ppgtt ? &ctx->ppgtt->base : NULL;
+			__entry->dev = ctx->file_priv->dev_priv->dev->primary->index;
+	),
+
+	TP_printk("dev=%u, ctx=%p, ctx_vm=%p",
+		  __entry->dev, __entry->ctx, __entry->vm)
+)
+
+DEFINE_EVENT(i915_context, i915_context_create,
+	TP_PROTO(struct intel_context *ctx),
+	TP_ARGS(ctx)
+);
+
+DEFINE_EVENT(i915_context, i915_context_free,
+	TP_PROTO(struct intel_context *ctx),
+	TP_ARGS(ctx)
+);
+
+/**
+ * DOC: switch_mm tracepoint
+ *
+ * This tracepoint allows tracking of the mm switch, which is an important point
+ * in the lifetime of the vm in the legacy submission path. This tracepoint is
+ * called only if full ppgtt is enabled.
+ */
+TRACE_EVENT(switch_mm,
+	TP_PROTO(struct intel_engine_cs *ring, struct intel_context *to),
+
+	TP_ARGS(ring, to),
+
+	TP_STRUCT__entry(
+			__field(u32, ring)
+			__field(struct intel_context *, to)
+			__field(struct i915_address_space *, vm)
+			__field(u32, dev)
+	),
+
+	TP_fast_assign(
+			__entry->ring = ring->id;
+			__entry->to = to;
+			__entry->vm = to->ppgtt? &to->ppgtt->base : NULL;
+			__entry->dev = ring->dev->primary->index;
+	),
+
+	TP_printk("dev=%u, ring=%u, ctx=%p, ctx_vm=%p",
+		  __entry->dev, __entry->ring, __entry->to, __entry->vm)
+);
+
 #endif /* _I915_TRACE_H_ */
 
 /* This part must be outside protection */
diff --git a/drivers/gpu/drm/i915/i915_ums.c b/drivers/gpu/drm/i915/i915_ums.c
index 480da593e6c0..d10fe3e9c49f 100644
--- a/drivers/gpu/drm/i915/i915_ums.c
+++ b/drivers/gpu/drm/i915/i915_ums.c
@@ -270,6 +270,12 @@ void i915_save_display_reg(struct drm_device *dev)
 	}
 	/* FIXME: regfile.save TV & SDVO state */
 
+	/* Panel fitter */
+	if (!IS_I830(dev) && !IS_845G(dev) && !HAS_PCH_SPLIT(dev)) {
+		dev_priv->regfile.savePFIT_CONTROL = I915_READ(PFIT_CONTROL);
+		dev_priv->regfile.savePFIT_PGM_RATIOS = I915_READ(PFIT_PGM_RATIOS);
+	}
+
 	/* Backlight */
 	if (INTEL_INFO(dev)->gen <= 4)
 		pci_read_config_byte(dev->pdev, PCI_LBPC,
@@ -284,6 +290,7 @@ void i915_save_display_reg(struct drm_device *dev)
 		dev_priv->regfile.saveBLC_PWM_CTL = I915_READ(BLC_PWM_CTL);
 		if (INTEL_INFO(dev)->gen >= 4)
 			dev_priv->regfile.saveBLC_PWM_CTL2 = I915_READ(BLC_PWM_CTL2);
+		dev_priv->regfile.saveBLC_HIST_CTL = I915_READ(BLC_HIST_CTL);
 	}
 
 	return;
@@ -313,6 +320,13 @@ void i915_restore_display_reg(struct drm_device *dev)
 		if (INTEL_INFO(dev)->gen >= 4)
 			I915_WRITE(BLC_PWM_CTL2, dev_priv->regfile.saveBLC_PWM_CTL2);
 		I915_WRITE(BLC_PWM_CTL, dev_priv->regfile.saveBLC_PWM_CTL);
+		I915_WRITE(BLC_HIST_CTL, dev_priv->regfile.saveBLC_HIST_CTL);
+	}
+
+	/* Panel fitter */
+	if (!IS_I830(dev) && !IS_845G(dev) && !HAS_PCH_SPLIT(dev)) {
+		I915_WRITE(PFIT_PGM_RATIOS, dev_priv->regfile.savePFIT_PGM_RATIOS);
+		I915_WRITE(PFIT_CONTROL, dev_priv->regfile.savePFIT_CONTROL);
 	}
 
 	/* Display port ratios (must be done before clock is set) */
diff --git a/drivers/gpu/drm/i915/intel_audio.c b/drivers/gpu/drm/i915/intel_audio.c
new file mode 100644
index 000000000000..2c7ed5cb29c0
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_audio.c
@@ -0,0 +1,463 @@
+/*
+ * Copyright © 2014 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/kernel.h>
+
+#include <drm/drmP.h>
+#include <drm/drm_edid.h>
+#include "intel_drv.h"
+#include "i915_drv.h"
+
+/**
+ * DOC: High Definition Audio over HDMI and Display Port
+ *
+ * The graphics and audio drivers together support High Definition Audio over
+ * HDMI and Display Port. The audio programming sequences are divided into audio
+ * codec and controller enable and disable sequences. The graphics driver
+ * handles the audio codec sequences, while the audio driver handles the audio
+ * controller sequences.
+ *
+ * The disable sequences must be performed before disabling the transcoder or
+ * port. The enable sequences may only be performed after enabling the
+ * transcoder and port, and after completed link training.
+ *
+ * The codec and controller sequences could be done either parallel or serial,
+ * but generally the ELDV/PD change in the codec sequence indicates to the audio
+ * driver that the controller sequence should start. Indeed, most of the
+ * co-operation between the graphics and audio drivers is handled via audio
+ * related registers. (The notable exception is the power management, not
+ * covered here.)
+ */
+
+static const struct {
+	int clock;
+	u32 config;
+} hdmi_audio_clock[] = {
+	{ DIV_ROUND_UP(25200 * 1000, 1001), AUD_CONFIG_PIXEL_CLOCK_HDMI_25175 },
+	{ 25200, AUD_CONFIG_PIXEL_CLOCK_HDMI_25200 }, /* default per bspec */
+	{ 27000, AUD_CONFIG_PIXEL_CLOCK_HDMI_27000 },
+	{ 27000 * 1001 / 1000, AUD_CONFIG_PIXEL_CLOCK_HDMI_27027 },
+	{ 54000, AUD_CONFIG_PIXEL_CLOCK_HDMI_54000 },
+	{ 54000 * 1001 / 1000, AUD_CONFIG_PIXEL_CLOCK_HDMI_54054 },
+	{ DIV_ROUND_UP(74250 * 1000, 1001), AUD_CONFIG_PIXEL_CLOCK_HDMI_74176 },
+	{ 74250, AUD_CONFIG_PIXEL_CLOCK_HDMI_74250 },
+	{ DIV_ROUND_UP(148500 * 1000, 1001), AUD_CONFIG_PIXEL_CLOCK_HDMI_148352 },
+	{ 148500, AUD_CONFIG_PIXEL_CLOCK_HDMI_148500 },
+};
+
+/* get AUD_CONFIG_PIXEL_CLOCK_HDMI_* value for mode */
+static u32 audio_config_hdmi_pixel_clock(struct drm_display_mode *mode)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(hdmi_audio_clock); i++) {
+		if (mode->clock == hdmi_audio_clock[i].clock)
+			break;
+	}
+
+	if (i == ARRAY_SIZE(hdmi_audio_clock)) {
+		DRM_DEBUG_KMS("HDMI audio pixel clock setting for %d not found, falling back to defaults\n", mode->clock);
+		i = 1;
+	}
+
+	DRM_DEBUG_KMS("Configuring HDMI audio for pixel clock %d (0x%08x)\n",
+		      hdmi_audio_clock[i].clock,
+		      hdmi_audio_clock[i].config);
+
+	return hdmi_audio_clock[i].config;
+}
+
+static bool intel_eld_uptodate(struct drm_connector *connector,
+			       int reg_eldv, uint32_t bits_eldv,
+			       int reg_elda, uint32_t bits_elda,
+			       int reg_edid)
+{
+	struct drm_i915_private *dev_priv = connector->dev->dev_private;
+	uint8_t *eld = connector->eld;
+	uint32_t tmp;
+	int i;
+
+	tmp = I915_READ(reg_eldv);
+	tmp &= bits_eldv;
+
+	if (!tmp)
+		return false;
+
+	tmp = I915_READ(reg_elda);
+	tmp &= ~bits_elda;
+	I915_WRITE(reg_elda, tmp);
+
+	for (i = 0; i < drm_eld_size(eld) / 4; i++)
+		if (I915_READ(reg_edid) != *((uint32_t *)eld + i))
+			return false;
+
+	return true;
+}
+
+static void g4x_audio_codec_disable(struct intel_encoder *encoder)
+{
+	struct drm_i915_private *dev_priv = encoder->base.dev->dev_private;
+	uint32_t eldv, tmp;
+
+	DRM_DEBUG_KMS("Disable audio codec\n");
+
+	tmp = I915_READ(G4X_AUD_VID_DID);
+	if (tmp == INTEL_AUDIO_DEVBLC || tmp == INTEL_AUDIO_DEVCL)
+		eldv = G4X_ELDV_DEVCL_DEVBLC;
+	else
+		eldv = G4X_ELDV_DEVCTG;
+
+	/* Invalidate ELD */
+	tmp = I915_READ(G4X_AUD_CNTL_ST);
+	tmp &= ~eldv;
+	I915_WRITE(G4X_AUD_CNTL_ST, tmp);
+}
+
+static void g4x_audio_codec_enable(struct drm_connector *connector,
+				   struct intel_encoder *encoder,
+				   struct drm_display_mode *mode)
+{
+	struct drm_i915_private *dev_priv = connector->dev->dev_private;
+	uint8_t *eld = connector->eld;
+	uint32_t eldv;
+	uint32_t tmp;
+	int len, i;
+
+	DRM_DEBUG_KMS("Enable audio codec, %u bytes ELD\n", eld[2]);
+
+	tmp = I915_READ(G4X_AUD_VID_DID);
+	if (tmp == INTEL_AUDIO_DEVBLC || tmp == INTEL_AUDIO_DEVCL)
+		eldv = G4X_ELDV_DEVCL_DEVBLC;
+	else
+		eldv = G4X_ELDV_DEVCTG;
+
+	if (intel_eld_uptodate(connector,
+			       G4X_AUD_CNTL_ST, eldv,
+			       G4X_AUD_CNTL_ST, G4X_ELD_ADDR_MASK,
+			       G4X_HDMIW_HDMIEDID))
+		return;
+
+	tmp = I915_READ(G4X_AUD_CNTL_ST);
+	tmp &= ~(eldv | G4X_ELD_ADDR_MASK);
+	len = (tmp >> 9) & 0x1f;		/* ELD buffer size */
+	I915_WRITE(G4X_AUD_CNTL_ST, tmp);
+
+	len = min(drm_eld_size(eld) / 4, len);
+	DRM_DEBUG_DRIVER("ELD size %d\n", len);
+	for (i = 0; i < len; i++)
+		I915_WRITE(G4X_HDMIW_HDMIEDID, *((uint32_t *)eld + i));
+
+	tmp = I915_READ(G4X_AUD_CNTL_ST);
+	tmp |= eldv;
+	I915_WRITE(G4X_AUD_CNTL_ST, tmp);
+}
+
+static void hsw_audio_codec_disable(struct intel_encoder *encoder)
+{
+	struct drm_i915_private *dev_priv = encoder->base.dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->base.crtc);
+	enum pipe pipe = intel_crtc->pipe;
+	uint32_t tmp;
+
+	DRM_DEBUG_KMS("Disable audio codec on pipe %c\n", pipe_name(pipe));
+
+	/* Disable timestamps */
+	tmp = I915_READ(HSW_AUD_CFG(pipe));
+	tmp &= ~AUD_CONFIG_N_VALUE_INDEX;
+	tmp |= AUD_CONFIG_N_PROG_ENABLE;
+	tmp &= ~AUD_CONFIG_UPPER_N_MASK;
+	tmp &= ~AUD_CONFIG_LOWER_N_MASK;
+	if (intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_DISPLAYPORT))
+		tmp |= AUD_CONFIG_N_VALUE_INDEX;
+	I915_WRITE(HSW_AUD_CFG(pipe), tmp);
+
+	/* Invalidate ELD */
+	tmp = I915_READ(HSW_AUD_PIN_ELD_CP_VLD);
+	tmp &= ~AUDIO_ELD_VALID(pipe);
+	tmp &= ~AUDIO_OUTPUT_ENABLE(pipe);
+	I915_WRITE(HSW_AUD_PIN_ELD_CP_VLD, tmp);
+}
+
+static void hsw_audio_codec_enable(struct drm_connector *connector,
+				   struct intel_encoder *encoder,
+				   struct drm_display_mode *mode)
+{
+	struct drm_i915_private *dev_priv = connector->dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->base.crtc);
+	enum pipe pipe = intel_crtc->pipe;
+	const uint8_t *eld = connector->eld;
+	uint32_t tmp;
+	int len, i;
+
+	DRM_DEBUG_KMS("Enable audio codec on pipe %c, %u bytes ELD\n",
+		      pipe_name(pipe), drm_eld_size(eld));
+
+	/* Enable audio presence detect, invalidate ELD */
+	tmp = I915_READ(HSW_AUD_PIN_ELD_CP_VLD);
+	tmp |= AUDIO_OUTPUT_ENABLE(pipe);
+	tmp &= ~AUDIO_ELD_VALID(pipe);
+	I915_WRITE(HSW_AUD_PIN_ELD_CP_VLD, tmp);
+
+	/*
+	 * FIXME: We're supposed to wait for vblank here, but we have vblanks
+	 * disabled during the mode set. The proper fix would be to push the
+	 * rest of the setup into a vblank work item, queued here, but the
+	 * infrastructure is not there yet.
+	 */
+
+	/* Reset ELD write address */
+	tmp = I915_READ(HSW_AUD_DIP_ELD_CTRL(pipe));
+	tmp &= ~IBX_ELD_ADDRESS_MASK;
+	I915_WRITE(HSW_AUD_DIP_ELD_CTRL(pipe), tmp);
+
+	/* Up to 84 bytes of hw ELD buffer */
+	len = min(drm_eld_size(eld), 84);
+	for (i = 0; i < len / 4; i++)
+		I915_WRITE(HSW_AUD_EDID_DATA(pipe), *((uint32_t *)eld + i));
+
+	/* ELD valid */
+	tmp = I915_READ(HSW_AUD_PIN_ELD_CP_VLD);
+	tmp |= AUDIO_ELD_VALID(pipe);
+	I915_WRITE(HSW_AUD_PIN_ELD_CP_VLD, tmp);
+
+	/* Enable timestamps */
+	tmp = I915_READ(HSW_AUD_CFG(pipe));
+	tmp &= ~AUD_CONFIG_N_VALUE_INDEX;
+	tmp &= ~AUD_CONFIG_N_PROG_ENABLE;
+	tmp &= ~AUD_CONFIG_PIXEL_CLOCK_HDMI_MASK;
+	if (intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_DISPLAYPORT))
+		tmp |= AUD_CONFIG_N_VALUE_INDEX;
+	else
+		tmp |= audio_config_hdmi_pixel_clock(mode);
+	I915_WRITE(HSW_AUD_CFG(pipe), tmp);
+}
+
+static void ilk_audio_codec_disable(struct intel_encoder *encoder)
+{
+	struct drm_i915_private *dev_priv = encoder->base.dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->base.crtc);
+	struct intel_digital_port *intel_dig_port =
+		enc_to_dig_port(&encoder->base);
+	enum port port = intel_dig_port->port;
+	enum pipe pipe = intel_crtc->pipe;
+	uint32_t tmp, eldv;
+	int aud_config;
+	int aud_cntrl_st2;
+
+	DRM_DEBUG_KMS("Disable audio codec on port %c, pipe %c\n",
+		      port_name(port), pipe_name(pipe));
+
+	if (HAS_PCH_IBX(dev_priv->dev)) {
+		aud_config = IBX_AUD_CFG(pipe);
+		aud_cntrl_st2 = IBX_AUD_CNTL_ST2;
+	} else if (IS_VALLEYVIEW(dev_priv)) {
+		aud_config = VLV_AUD_CFG(pipe);
+		aud_cntrl_st2 = VLV_AUD_CNTL_ST2;
+	} else {
+		aud_config = CPT_AUD_CFG(pipe);
+		aud_cntrl_st2 = CPT_AUD_CNTRL_ST2;
+	}
+
+	/* Disable timestamps */
+	tmp = I915_READ(aud_config);
+	tmp &= ~AUD_CONFIG_N_VALUE_INDEX;
+	tmp |= AUD_CONFIG_N_PROG_ENABLE;
+	tmp &= ~AUD_CONFIG_UPPER_N_MASK;
+	tmp &= ~AUD_CONFIG_LOWER_N_MASK;
+	if (intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_DISPLAYPORT))
+		tmp |= AUD_CONFIG_N_VALUE_INDEX;
+	I915_WRITE(aud_config, tmp);
+
+	if (WARN_ON(!port)) {
+		eldv = IBX_ELD_VALID(PORT_B) | IBX_ELD_VALID(PORT_C) |
+			IBX_ELD_VALID(PORT_D);
+	} else {
+		eldv = IBX_ELD_VALID(port);
+	}
+
+	/* Invalidate ELD */
+	tmp = I915_READ(aud_cntrl_st2);
+	tmp &= ~eldv;
+	I915_WRITE(aud_cntrl_st2, tmp);
+}
+
+static void ilk_audio_codec_enable(struct drm_connector *connector,
+				   struct intel_encoder *encoder,
+				   struct drm_display_mode *mode)
+{
+	struct drm_i915_private *dev_priv = connector->dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->base.crtc);
+	struct intel_digital_port *intel_dig_port =
+		enc_to_dig_port(&encoder->base);
+	enum port port = intel_dig_port->port;
+	enum pipe pipe = intel_crtc->pipe;
+	uint8_t *eld = connector->eld;
+	uint32_t eldv;
+	uint32_t tmp;
+	int len, i;
+	int hdmiw_hdmiedid;
+	int aud_config;
+	int aud_cntl_st;
+	int aud_cntrl_st2;
+
+	DRM_DEBUG_KMS("Enable audio codec on port %c, pipe %c, %u bytes ELD\n",
+		      port_name(port), pipe_name(pipe), drm_eld_size(eld));
+
+	/*
+	 * FIXME: We're supposed to wait for vblank here, but we have vblanks
+	 * disabled during the mode set. The proper fix would be to push the
+	 * rest of the setup into a vblank work item, queued here, but the
+	 * infrastructure is not there yet.
+	 */
+
+	if (HAS_PCH_IBX(connector->dev)) {
+		hdmiw_hdmiedid = IBX_HDMIW_HDMIEDID(pipe);
+		aud_config = IBX_AUD_CFG(pipe);
+		aud_cntl_st = IBX_AUD_CNTL_ST(pipe);
+		aud_cntrl_st2 = IBX_AUD_CNTL_ST2;
+	} else if (IS_VALLEYVIEW(connector->dev)) {
+		hdmiw_hdmiedid = VLV_HDMIW_HDMIEDID(pipe);
+		aud_config = VLV_AUD_CFG(pipe);
+		aud_cntl_st = VLV_AUD_CNTL_ST(pipe);
+		aud_cntrl_st2 = VLV_AUD_CNTL_ST2;
+	} else {
+		hdmiw_hdmiedid = CPT_HDMIW_HDMIEDID(pipe);
+		aud_config = CPT_AUD_CFG(pipe);
+		aud_cntl_st = CPT_AUD_CNTL_ST(pipe);
+		aud_cntrl_st2 = CPT_AUD_CNTRL_ST2;
+	}
+
+	if (WARN_ON(!port)) {
+		eldv = IBX_ELD_VALID(PORT_B) | IBX_ELD_VALID(PORT_C) |
+			IBX_ELD_VALID(PORT_D);
+	} else {
+		eldv = IBX_ELD_VALID(port);
+	}
+
+	/* Invalidate ELD */
+	tmp = I915_READ(aud_cntrl_st2);
+	tmp &= ~eldv;
+	I915_WRITE(aud_cntrl_st2, tmp);
+
+	/* Reset ELD write address */
+	tmp = I915_READ(aud_cntl_st);
+	tmp &= ~IBX_ELD_ADDRESS_MASK;
+	I915_WRITE(aud_cntl_st, tmp);
+
+	/* Up to 84 bytes of hw ELD buffer */
+	len = min(drm_eld_size(eld), 84);
+	for (i = 0; i < len / 4; i++)
+		I915_WRITE(hdmiw_hdmiedid, *((uint32_t *)eld + i));
+
+	/* ELD valid */
+	tmp = I915_READ(aud_cntrl_st2);
+	tmp |= eldv;
+	I915_WRITE(aud_cntrl_st2, tmp);
+
+	/* Enable timestamps */
+	tmp = I915_READ(aud_config);
+	tmp &= ~AUD_CONFIG_N_VALUE_INDEX;
+	tmp &= ~AUD_CONFIG_N_PROG_ENABLE;
+	tmp &= ~AUD_CONFIG_PIXEL_CLOCK_HDMI_MASK;
+	if (intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_DISPLAYPORT))
+		tmp |= AUD_CONFIG_N_VALUE_INDEX;
+	else
+		tmp |= audio_config_hdmi_pixel_clock(mode);
+	I915_WRITE(aud_config, tmp);
+}
+
+/**
+ * intel_audio_codec_enable - Enable the audio codec for HD audio
+ * @intel_encoder: encoder on which to enable audio
+ *
+ * The enable sequences may only be performed after enabling the transcoder and
+ * port, and after completed link training.
+ */
+void intel_audio_codec_enable(struct intel_encoder *intel_encoder)
+{
+	struct drm_encoder *encoder = &intel_encoder->base;
+	struct intel_crtc *crtc = to_intel_crtc(encoder->crtc);
+	struct drm_display_mode *mode = &crtc->config.adjusted_mode;
+	struct drm_connector *connector;
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	connector = drm_select_eld(encoder, mode);
+	if (!connector)
+		return;
+
+	DRM_DEBUG_DRIVER("ELD on [CONNECTOR:%d:%s], [ENCODER:%d:%s]\n",
+			 connector->base.id,
+			 connector->name,
+			 connector->encoder->base.id,
+			 connector->encoder->name);
+
+	/* ELD Conn_Type */
+	connector->eld[5] &= ~(3 << 2);
+	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_DISPLAYPORT))
+		connector->eld[5] |= (1 << 2);
+
+	connector->eld[6] = drm_av_sync_delay(connector, mode) / 2;
+
+	if (dev_priv->display.audio_codec_enable)
+		dev_priv->display.audio_codec_enable(connector, intel_encoder, mode);
+}
+
+/**
+ * intel_audio_codec_disable - Disable the audio codec for HD audio
+ * @encoder: encoder on which to disable audio
+ *
+ * The disable sequences must be performed before disabling the transcoder or
+ * port.
+ */
+void intel_audio_codec_disable(struct intel_encoder *encoder)
+{
+	struct drm_device *dev = encoder->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (dev_priv->display.audio_codec_disable)
+		dev_priv->display.audio_codec_disable(encoder);
+}
+
+/**
+ * intel_init_audio - Set up chip specific audio functions
+ * @dev: drm device
+ */
+void intel_init_audio(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (IS_G4X(dev)) {
+		dev_priv->display.audio_codec_enable = g4x_audio_codec_enable;
+		dev_priv->display.audio_codec_disable = g4x_audio_codec_disable;
+	} else if (IS_VALLEYVIEW(dev)) {
+		dev_priv->display.audio_codec_enable = ilk_audio_codec_enable;
+		dev_priv->display.audio_codec_disable = ilk_audio_codec_disable;
+	} else if (IS_HASWELL(dev) || INTEL_INFO(dev)->gen >= 8) {
+		dev_priv->display.audio_codec_enable = hsw_audio_codec_enable;
+		dev_priv->display.audio_codec_disable = hsw_audio_codec_disable;
+	} else if (HAS_PCH_SPLIT(dev)) {
+		dev_priv->display.audio_codec_enable = ilk_audio_codec_enable;
+		dev_priv->display.audio_codec_disable = ilk_audio_codec_disable;
+	}
+}
diff --git a/drivers/gpu/drm/i915/intel_bios.h b/drivers/gpu/drm/i915/intel_bios.h
index 905999bee2ac..7603765c91fc 100644
--- a/drivers/gpu/drm/i915/intel_bios.h
+++ b/drivers/gpu/drm/i915/intel_bios.h
@@ -46,7 +46,7 @@ struct bdb_header {
 	u16 version;			/**< decimal */
 	u16 header_size;		/**< in bytes */
 	u16 bdb_size;			/**< in bytes */
-};
+} __packed;
 
 /* strictly speaking, this is a "skip" block, but it has interesting info */
 struct vbios_data {
@@ -252,7 +252,7 @@ union child_device_config {
 	/* This one should also be safe to use anywhere, even without version
 	 * checks. */
 	struct common_child_dev_config common;
-};
+} __packed;
 
 struct bdb_general_definitions {
 	/* DDC GPIO */
@@ -888,12 +888,12 @@ struct mipi_pps_data {
 	u16 bl_disable_delay;
 	u16 panel_off_delay;
 	u16 panel_power_cycle_delay;
-};
+} __packed;
 
 struct bdb_mipi_config {
 	struct mipi_config config[MAX_MIPI_CONFIGURATIONS];
 	struct mipi_pps_data pps[MAX_MIPI_CONFIGURATIONS];
-};
+} __packed;
 
 /* Block 53 contains MIPI sequences as needed by the panel
  * for enabling it. This block can be variable in size and
@@ -902,7 +902,7 @@ struct bdb_mipi_config {
 struct bdb_mipi_sequence {
 	u8 version;
 	u8 data[0];
-};
+} __packed;
 
 /* MIPI Sequnece Block definitions */
 enum mipi_seq {
diff --git a/drivers/gpu/drm/i915/intel_crt.c b/drivers/gpu/drm/i915/intel_crt.c
index 9212e6504e0f..a9af9a4866db 100644
--- a/drivers/gpu/drm/i915/intel_crt.c
+++ b/drivers/gpu/drm/i915/intel_crt.c
@@ -72,7 +72,7 @@ static bool intel_crt_get_hw_state(struct intel_encoder *encoder,
 	u32 tmp;
 
 	power_domain = intel_display_port_power_domain(encoder);
-	if (!intel_display_power_enabled(dev_priv, power_domain))
+	if (!intel_display_power_is_enabled(dev_priv, power_domain))
 		return false;
 
 	tmp = I915_READ(crt->adpa_reg);
@@ -775,7 +775,7 @@ static void intel_crt_reset(struct drm_connector *connector)
 		I915_WRITE(crt->adpa_reg, adpa);
 		POSTING_READ(crt->adpa_reg);
 
-		DRM_DEBUG_KMS("pch crt adpa set to 0x%x\n", adpa);
+		DRM_DEBUG_KMS("crt adpa set to 0x%x\n", adpa);
 		crt->force_hotplug_required = 1;
 	}
 
diff --git a/drivers/gpu/drm/i915/intel_ddi.c b/drivers/gpu/drm/i915/intel_ddi.c
index b63d4fa204a3..e6b45cd150d3 100644
--- a/drivers/gpu/drm/i915/intel_ddi.c
+++ b/drivers/gpu/drm/i915/intel_ddi.c
@@ -95,8 +95,8 @@ static const struct ddi_buf_trans bdw_ddi_translations_dp[] = {
 	{ 0x00BEFFFF, 0x00140006 },
 	{ 0x80B2CFFF, 0x001B0002 },
 	{ 0x00FFFFFF, 0x000E000A },
-	{ 0x00D75FFF, 0x00180004 },
-	{ 0x80CB2FFF, 0x001B0002 },
+	{ 0x00DB6FFF, 0x00160005 },
+	{ 0x80C71FFF, 0x001A0002 },
 	{ 0x00F7DFFF, 0x00180004 },
 	{ 0x80D75FFF, 0x001B0002 },
 };
@@ -127,6 +127,32 @@ static const struct ddi_buf_trans bdw_ddi_translations_hdmi[] = {
 	{ 0x80FFFFFF, 0x001B0002 },	/* 9:	1000	1000	0	*/
 };
 
+static const struct ddi_buf_trans skl_ddi_translations_dp[] = {
+	{ 0x00000018, 0x000000a0 },
+	{ 0x00004014, 0x00000098 },
+	{ 0x00006012, 0x00000088 },
+	{ 0x00008010, 0x00000080 },
+	{ 0x00000018, 0x00000098 },
+	{ 0x00004014, 0x00000088 },
+	{ 0x00006012, 0x00000080 },
+	{ 0x00000018, 0x00000088 },
+	{ 0x00004014, 0x00000080 },
+};
+
+static const struct ddi_buf_trans skl_ddi_translations_hdmi[] = {
+					/* Idx	NT mV   T mV    db  */
+	{ 0x00000018, 0x000000a0 },	/* 0:	400	400	0   */
+	{ 0x00004014, 0x00000098 },	/* 1:	400	600	3.5 */
+	{ 0x00006012, 0x00000088 },	/* 2:	400	800	6   */
+	{ 0x00000018, 0x0000003c },	/* 3:	450	450	0   */
+	{ 0x00000018, 0x00000098 },	/* 4:	600	600	0   */
+	{ 0x00003015, 0x00000088 },	/* 5:	600	800	2.5 */
+	{ 0x00005013, 0x00000080 },	/* 6:	600	1000	4.5 */
+	{ 0x00000018, 0x00000088 },	/* 7:	800	800	0   */
+	{ 0x00000096, 0x00000080 },	/* 8:	800	1000	2   */
+	{ 0x00000018, 0x00000080 },	/* 9:	1200	1200	0   */
+};
+
 enum port intel_ddi_get_encoder_port(struct intel_encoder *intel_encoder)
 {
 	struct drm_encoder *encoder = &intel_encoder->base;
@@ -169,7 +195,14 @@ static void intel_prepare_ddi_buffers(struct drm_device *dev, enum port port)
 	const struct ddi_buf_trans *ddi_translations_hdmi;
 	const struct ddi_buf_trans *ddi_translations;
 
-	if (IS_BROADWELL(dev)) {
+	if (IS_SKYLAKE(dev)) {
+		ddi_translations_fdi = NULL;
+		ddi_translations_dp = skl_ddi_translations_dp;
+		ddi_translations_edp = skl_ddi_translations_dp;
+		ddi_translations_hdmi = skl_ddi_translations_hdmi;
+		n_hdmi_entries = ARRAY_SIZE(skl_ddi_translations_hdmi);
+		hdmi_800mV_0dB = 7;
+	} else if (IS_BROADWELL(dev)) {
 		ddi_translations_fdi = bdw_ddi_translations_fdi;
 		ddi_translations_dp = bdw_ddi_translations_dp;
 		ddi_translations_edp = bdw_ddi_translations_edp;
@@ -208,7 +241,10 @@ static void intel_prepare_ddi_buffers(struct drm_device *dev, enum port port)
 			ddi_translations = ddi_translations_dp;
 		break;
 	case PORT_E:
-		ddi_translations = ddi_translations_fdi;
+		if (ddi_translations_fdi)
+			ddi_translations = ddi_translations_fdi;
+		else
+			ddi_translations = ddi_translations_dp;
 		break;
 	default:
 		BUG();
@@ -423,6 +459,27 @@ intel_ddi_get_crtc_encoder(struct drm_crtc *crtc)
 	return ret;
 }
 
+static struct intel_encoder *
+intel_ddi_get_crtc_new_encoder(struct intel_crtc *crtc)
+{
+	struct drm_device *dev = crtc->base.dev;
+	struct intel_encoder *intel_encoder, *ret = NULL;
+	int num_encoders = 0;
+
+	for_each_intel_encoder(dev, intel_encoder) {
+		if (intel_encoder->new_crtc == crtc) {
+			ret = intel_encoder;
+			num_encoders++;
+		}
+	}
+
+	WARN(num_encoders != 1, "%d encoders on crtc for pipe %c\n", num_encoders,
+	     pipe_name(crtc->pipe));
+
+	BUG_ON(ret == NULL);
+	return ret;
+}
+
 #define LC_FREQ 2700
 #define LC_FREQ_2K U64_C(LC_FREQ * 2000)
 
@@ -613,6 +670,111 @@ static int intel_ddi_calc_wrpll_link(struct drm_i915_private *dev_priv,
 	return (refclk * n * 100) / (p * r);
 }
 
+static int skl_calc_wrpll_link(struct drm_i915_private *dev_priv,
+			       uint32_t dpll)
+{
+	uint32_t cfgcr1_reg, cfgcr2_reg;
+	uint32_t cfgcr1_val, cfgcr2_val;
+	uint32_t p0, p1, p2, dco_freq;
+
+	cfgcr1_reg = GET_CFG_CR1_REG(dpll);
+	cfgcr2_reg = GET_CFG_CR2_REG(dpll);
+
+	cfgcr1_val = I915_READ(cfgcr1_reg);
+	cfgcr2_val = I915_READ(cfgcr2_reg);
+
+	p0 = cfgcr2_val & DPLL_CFGCR2_PDIV_MASK;
+	p2 = cfgcr2_val & DPLL_CFGCR2_KDIV_MASK;
+
+	if (cfgcr2_val &  DPLL_CFGCR2_QDIV_MODE(1))
+		p1 = (cfgcr2_val & DPLL_CFGCR2_QDIV_RATIO_MASK) >> 8;
+	else
+		p1 = 1;
+
+
+	switch (p0) {
+	case DPLL_CFGCR2_PDIV_1:
+		p0 = 1;
+		break;
+	case DPLL_CFGCR2_PDIV_2:
+		p0 = 2;
+		break;
+	case DPLL_CFGCR2_PDIV_3:
+		p0 = 3;
+		break;
+	case DPLL_CFGCR2_PDIV_7:
+		p0 = 7;
+		break;
+	}
+
+	switch (p2) {
+	case DPLL_CFGCR2_KDIV_5:
+		p2 = 5;
+		break;
+	case DPLL_CFGCR2_KDIV_2:
+		p2 = 2;
+		break;
+	case DPLL_CFGCR2_KDIV_3:
+		p2 = 3;
+		break;
+	case DPLL_CFGCR2_KDIV_1:
+		p2 = 1;
+		break;
+	}
+
+	dco_freq = (cfgcr1_val & DPLL_CFGCR1_DCO_INTEGER_MASK) * 24 * 1000;
+
+	dco_freq += (((cfgcr1_val & DPLL_CFGCR1_DCO_FRACTION_MASK) >> 9) * 24 *
+		1000) / 0x8000;
+
+	return dco_freq / (p0 * p1 * p2 * 5);
+}
+
+
+static void skl_ddi_clock_get(struct intel_encoder *encoder,
+				struct intel_crtc_config *pipe_config)
+{
+	struct drm_i915_private *dev_priv = encoder->base.dev->dev_private;
+	int link_clock = 0;
+	uint32_t dpll_ctl1, dpll;
+
+	dpll = pipe_config->ddi_pll_sel;
+
+	dpll_ctl1 = I915_READ(DPLL_CTRL1);
+
+	if (dpll_ctl1 & DPLL_CTRL1_HDMI_MODE(dpll)) {
+		link_clock = skl_calc_wrpll_link(dev_priv, dpll);
+	} else {
+		link_clock = dpll_ctl1 & DPLL_CRTL1_LINK_RATE_MASK(dpll);
+		link_clock >>= DPLL_CRTL1_LINK_RATE_SHIFT(dpll);
+
+		switch (link_clock) {
+		case DPLL_CRTL1_LINK_RATE_810:
+			link_clock = 81000;
+			break;
+		case DPLL_CRTL1_LINK_RATE_1350:
+			link_clock = 135000;
+			break;
+		case DPLL_CRTL1_LINK_RATE_2700:
+			link_clock = 270000;
+			break;
+		default:
+			WARN(1, "Unsupported link rate\n");
+			break;
+		}
+		link_clock *= 2;
+	}
+
+	pipe_config->port_clock = link_clock;
+
+	if (pipe_config->has_dp_encoder)
+		pipe_config->adjusted_mode.crtc_clock =
+			intel_dotclock_calculate(pipe_config->port_clock,
+						 &pipe_config->dp_m_n);
+	else
+		pipe_config->adjusted_mode.crtc_clock = pipe_config->port_clock;
+}
+
 static void hsw_ddi_clock_get(struct intel_encoder *encoder,
 			      struct intel_crtc_config *pipe_config)
 {
@@ -756,7 +918,7 @@ hsw_ddi_pll_select(struct intel_crtc *intel_crtc,
 		      WRPLL_DIVIDER_REFERENCE(r2) | WRPLL_DIVIDER_FEEDBACK(n2) |
 		      WRPLL_DIVIDER_POST(p);
 
-		intel_crtc->config.dpll_hw_state.wrpll = val;
+		intel_crtc->new_config->dpll_hw_state.wrpll = val;
 
 		pll = intel_get_shared_dpll(intel_crtc);
 		if (pll == NULL) {
@@ -765,12 +927,234 @@ hsw_ddi_pll_select(struct intel_crtc *intel_crtc,
 			return false;
 		}
 
-		intel_crtc->config.ddi_pll_sel = PORT_CLK_SEL_WRPLL(pll->id);
+		intel_crtc->new_config->ddi_pll_sel = PORT_CLK_SEL_WRPLL(pll->id);
 	}
 
 	return true;
 }
 
+struct skl_wrpll_params {
+	uint32_t        dco_fraction;
+	uint32_t        dco_integer;
+	uint32_t        qdiv_ratio;
+	uint32_t        qdiv_mode;
+	uint32_t        kdiv;
+	uint32_t        pdiv;
+	uint32_t        central_freq;
+};
+
+static void
+skl_ddi_calculate_wrpll(int clock /* in Hz */,
+			struct skl_wrpll_params *wrpll_params)
+{
+	uint64_t afe_clock = clock * 5; /* AFE Clock is 5x Pixel clock */
+	uint64_t dco_central_freq[3] = {8400000000ULL,
+					9000000000ULL,
+					9600000000ULL};
+	uint32_t min_dco_deviation = 400;
+	uint32_t min_dco_index = 3;
+	uint32_t P0[4] = {1, 2, 3, 7};
+	uint32_t P2[4] = {1, 2, 3, 5};
+	bool found = false;
+	uint32_t candidate_p = 0;
+	uint32_t candidate_p0[3] = {0}, candidate_p1[3] = {0};
+	uint32_t candidate_p2[3] = {0};
+	uint32_t dco_central_freq_deviation[3];
+	uint32_t i, P1, k, dco_count;
+	bool retry_with_odd = false;
+	uint64_t dco_freq;
+
+	/* Determine P0, P1 or P2 */
+	for (dco_count = 0; dco_count < 3; dco_count++) {
+		found = false;
+		candidate_p =
+			div64_u64(dco_central_freq[dco_count], afe_clock);
+		if (retry_with_odd == false)
+			candidate_p = (candidate_p % 2 == 0 ?
+				candidate_p : candidate_p + 1);
+
+		for (P1 = 1; P1 < candidate_p; P1++) {
+			for (i = 0; i < 4; i++) {
+				if (!(P0[i] != 1 || P1 == 1))
+					continue;
+
+				for (k = 0; k < 4; k++) {
+					if (P1 != 1 && P2[k] != 2)
+						continue;
+
+					if (candidate_p == P0[i] * P1 * P2[k]) {
+						/* Found possible P0, P1, P2 */
+						found = true;
+						candidate_p0[dco_count] = P0[i];
+						candidate_p1[dco_count] = P1;
+						candidate_p2[dco_count] = P2[k];
+						goto found;
+					}
+
+				}
+			}
+		}
+
+found:
+		if (found) {
+			dco_central_freq_deviation[dco_count] =
+				div64_u64(10000 *
+					  abs_diff((candidate_p * afe_clock),
+						   dco_central_freq[dco_count]),
+					  dco_central_freq[dco_count]);
+
+			if (dco_central_freq_deviation[dco_count] <
+				min_dco_deviation) {
+				min_dco_deviation =
+					dco_central_freq_deviation[dco_count];
+				min_dco_index = dco_count;
+			}
+		}
+
+		if (min_dco_index > 2 && dco_count == 2) {
+			retry_with_odd = true;
+			dco_count = 0;
+		}
+	}
+
+	if (min_dco_index > 2) {
+		WARN(1, "No valid values found for the given pixel clock\n");
+	} else {
+		 wrpll_params->central_freq = dco_central_freq[min_dco_index];
+
+		 switch (dco_central_freq[min_dco_index]) {
+		 case 9600000000ULL:
+			wrpll_params->central_freq = 0;
+			break;
+		 case 9000000000ULL:
+			wrpll_params->central_freq = 1;
+			break;
+		 case 8400000000ULL:
+			wrpll_params->central_freq = 3;
+		 }
+
+		 switch (candidate_p0[min_dco_index]) {
+		 case 1:
+			wrpll_params->pdiv = 0;
+			break;
+		 case 2:
+			wrpll_params->pdiv = 1;
+			break;
+		 case 3:
+			wrpll_params->pdiv = 2;
+			break;
+		 case 7:
+			wrpll_params->pdiv = 4;
+			break;
+		 default:
+			WARN(1, "Incorrect PDiv\n");
+		 }
+
+		 switch (candidate_p2[min_dco_index]) {
+		 case 5:
+			wrpll_params->kdiv = 0;
+			break;
+		 case 2:
+			wrpll_params->kdiv = 1;
+			break;
+		 case 3:
+			wrpll_params->kdiv = 2;
+			break;
+		 case 1:
+			wrpll_params->kdiv = 3;
+			break;
+		 default:
+			WARN(1, "Incorrect KDiv\n");
+		 }
+
+		 wrpll_params->qdiv_ratio = candidate_p1[min_dco_index];
+		 wrpll_params->qdiv_mode =
+			(wrpll_params->qdiv_ratio == 1) ? 0 : 1;
+
+		 dco_freq = candidate_p0[min_dco_index] *
+			 candidate_p1[min_dco_index] *
+			 candidate_p2[min_dco_index] * afe_clock;
+
+		/*
+		* Intermediate values are in Hz.
+		* Divide by MHz to match bsepc
+		*/
+		 wrpll_params->dco_integer = div_u64(dco_freq, (24 * MHz(1)));
+		 wrpll_params->dco_fraction =
+			 div_u64(((div_u64(dco_freq, 24) -
+				   wrpll_params->dco_integer * MHz(1)) * 0x8000), MHz(1));
+
+	}
+}
+
+
+static bool
+skl_ddi_pll_select(struct intel_crtc *intel_crtc,
+		   struct intel_encoder *intel_encoder,
+		   int clock)
+{
+	struct intel_shared_dpll *pll;
+	uint32_t ctrl1, cfgcr1, cfgcr2;
+
+	/*
+	 * See comment in intel_dpll_hw_state to understand why we always use 0
+	 * as the DPLL id in this function.
+	 */
+
+	ctrl1 = DPLL_CTRL1_OVERRIDE(0);
+
+	if (intel_encoder->type == INTEL_OUTPUT_HDMI) {
+		struct skl_wrpll_params wrpll_params = { 0, };
+
+		ctrl1 |= DPLL_CTRL1_HDMI_MODE(0);
+
+		skl_ddi_calculate_wrpll(clock * 1000, &wrpll_params);
+
+		cfgcr1 = DPLL_CFGCR1_FREQ_ENABLE |
+			 DPLL_CFGCR1_DCO_FRACTION(wrpll_params.dco_fraction) |
+			 wrpll_params.dco_integer;
+
+		cfgcr2 = DPLL_CFGCR2_QDIV_RATIO(wrpll_params.qdiv_ratio) |
+			 DPLL_CFGCR2_QDIV_MODE(wrpll_params.qdiv_mode) |
+			 DPLL_CFGCR2_KDIV(wrpll_params.kdiv) |
+			 DPLL_CFGCR2_PDIV(wrpll_params.pdiv) |
+			 wrpll_params.central_freq;
+	} else if (intel_encoder->type == INTEL_OUTPUT_DISPLAYPORT) {
+		struct drm_encoder *encoder = &intel_encoder->base;
+		struct intel_dp *intel_dp = enc_to_intel_dp(encoder);
+
+		switch (intel_dp->link_bw) {
+		case DP_LINK_BW_1_62:
+			ctrl1 |= DPLL_CRTL1_LINK_RATE(DPLL_CRTL1_LINK_RATE_810, 0);
+			break;
+		case DP_LINK_BW_2_7:
+			ctrl1 |= DPLL_CRTL1_LINK_RATE(DPLL_CRTL1_LINK_RATE_1350, 0);
+			break;
+		case DP_LINK_BW_5_4:
+			ctrl1 |= DPLL_CRTL1_LINK_RATE(DPLL_CRTL1_LINK_RATE_2700, 0);
+			break;
+		}
+
+		cfgcr1 = cfgcr2 = 0;
+	} else /* eDP */
+		return true;
+
+	intel_crtc->new_config->dpll_hw_state.ctrl1 = ctrl1;
+	intel_crtc->new_config->dpll_hw_state.cfgcr1 = cfgcr1;
+	intel_crtc->new_config->dpll_hw_state.cfgcr2 = cfgcr2;
+
+	pll = intel_get_shared_dpll(intel_crtc);
+	if (pll == NULL) {
+		DRM_DEBUG_DRIVER("failed to find PLL for pipe %c\n",
+				 pipe_name(intel_crtc->pipe));
+		return false;
+	}
+
+	/* shared DPLL id 0 is DPLL 1 */
+	intel_crtc->new_config->ddi_pll_sel = pll->id + 1;
+
+	return true;
+}
 
 /*
  * Tries to find a *shared* PLL for the CRTC and store it in
@@ -781,13 +1165,15 @@ hsw_ddi_pll_select(struct intel_crtc *intel_crtc,
  */
 bool intel_ddi_pll_select(struct intel_crtc *intel_crtc)
 {
-	struct drm_crtc *crtc = &intel_crtc->base;
-	struct intel_encoder *intel_encoder = intel_ddi_get_crtc_encoder(crtc);
-	int clock = intel_crtc->config.port_clock;
-
-	intel_put_shared_dpll(intel_crtc);
+	struct drm_device *dev = intel_crtc->base.dev;
+	struct intel_encoder *intel_encoder =
+		intel_ddi_get_crtc_new_encoder(intel_crtc);
+	int clock = intel_crtc->new_config->port_clock;
 
-	return hsw_ddi_pll_select(intel_crtc, intel_encoder, clock);
+	if (IS_SKYLAKE(dev))
+		return skl_ddi_pll_select(intel_crtc, intel_encoder, clock);
+	else
+		return hsw_ddi_pll_select(intel_crtc, intel_encoder, clock);
 }
 
 void intel_ddi_set_pipe_settings(struct drm_crtc *crtc)
@@ -962,7 +1348,7 @@ bool intel_ddi_connector_get_hw_state(struct intel_connector *intel_connector)
 	uint32_t tmp;
 
 	power_domain = intel_display_port_power_domain(intel_encoder);
-	if (!intel_display_power_enabled(dev_priv, power_domain))
+	if (!intel_display_power_is_enabled(dev_priv, power_domain))
 		return false;
 
 	if (!intel_encoder->get_hw_state(intel_encoder, &pipe))
@@ -1008,7 +1394,7 @@ bool intel_ddi_get_hw_state(struct intel_encoder *encoder,
 	int i;
 
 	power_domain = intel_display_port_power_domain(encoder);
-	if (!intel_display_power_enabled(dev_priv, power_domain))
+	if (!intel_display_power_is_enabled(dev_priv, power_domain))
 		return false;
 
 	tmp = I915_READ(DDI_BUF_CTL(port));
@@ -1079,27 +1465,53 @@ void intel_ddi_disable_pipe_clock(struct intel_crtc *intel_crtc)
 static void intel_ddi_pre_enable(struct intel_encoder *intel_encoder)
 {
 	struct drm_encoder *encoder = &intel_encoder->base;
-	struct drm_i915_private *dev_priv = encoder->dev->dev_private;
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *crtc = to_intel_crtc(encoder->crtc);
 	enum port port = intel_ddi_get_encoder_port(intel_encoder);
 	int type = intel_encoder->type;
 
-	if (crtc->config.has_audio) {
-		DRM_DEBUG_DRIVER("Audio on pipe %c on DDI\n",
-				 pipe_name(crtc->pipe));
-
-		/* write eld */
-		DRM_DEBUG_DRIVER("DDI audio: write eld information\n");
-		intel_write_eld(encoder, &crtc->config.adjusted_mode);
-	}
-
 	if (type == INTEL_OUTPUT_EDP) {
 		struct intel_dp *intel_dp = enc_to_intel_dp(encoder);
 		intel_edp_panel_on(intel_dp);
 	}
 
-	WARN_ON(crtc->config.ddi_pll_sel == PORT_CLK_SEL_NONE);
-	I915_WRITE(PORT_CLK_SEL(port), crtc->config.ddi_pll_sel);
+	if (IS_SKYLAKE(dev)) {
+		uint32_t dpll = crtc->config.ddi_pll_sel;
+		uint32_t val;
+
+		/*
+		 * DPLL0 is used for eDP and is the only "private" DPLL (as
+		 * opposed to shared) on SKL
+		 */
+		if (type == INTEL_OUTPUT_EDP) {
+			WARN_ON(dpll != SKL_DPLL0);
+
+			val = I915_READ(DPLL_CTRL1);
+
+			val &= ~(DPLL_CTRL1_HDMI_MODE(dpll) |
+				 DPLL_CTRL1_SSC(dpll) |
+				 DPLL_CRTL1_LINK_RATE_MASK(dpll));
+			val |= crtc->config.dpll_hw_state.ctrl1 << (dpll * 6);
+
+			I915_WRITE(DPLL_CTRL1, val);
+			POSTING_READ(DPLL_CTRL1);
+		}
+
+		/* DDI -> PLL mapping  */
+		val = I915_READ(DPLL_CTRL2);
+
+		val &= ~(DPLL_CTRL2_DDI_CLK_OFF(port) |
+			DPLL_CTRL2_DDI_CLK_SEL_MASK(port));
+		val |= (DPLL_CTRL2_DDI_CLK_SEL(dpll, port) |
+			DPLL_CTRL2_DDI_SEL_OVERRIDE(port));
+
+		I915_WRITE(DPLL_CTRL2, val);
+
+	} else {
+		WARN_ON(crtc->config.ddi_pll_sel == PORT_CLK_SEL_NONE);
+		I915_WRITE(PORT_CLK_SEL(port), crtc->config.ddi_pll_sel);
+	}
 
 	if (type == INTEL_OUTPUT_DISPLAYPORT || type == INTEL_OUTPUT_EDP) {
 		struct intel_dp *intel_dp = enc_to_intel_dp(encoder);
@@ -1109,7 +1521,7 @@ static void intel_ddi_pre_enable(struct intel_encoder *intel_encoder)
 		intel_dp_sink_dpms(intel_dp, DRM_MODE_DPMS_ON);
 		intel_dp_start_link_train(intel_dp);
 		intel_dp_complete_link_train(intel_dp);
-		if (port != PORT_A)
+		if (port != PORT_A || INTEL_INFO(dev)->gen >= 9)
 			intel_dp_stop_link_train(intel_dp);
 	} else if (type == INTEL_OUTPUT_HDMI) {
 		struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
@@ -1123,7 +1535,8 @@ static void intel_ddi_pre_enable(struct intel_encoder *intel_encoder)
 static void intel_ddi_post_disable(struct intel_encoder *intel_encoder)
 {
 	struct drm_encoder *encoder = &intel_encoder->base;
-	struct drm_i915_private *dev_priv = encoder->dev->dev_private;
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
 	enum port port = intel_ddi_get_encoder_port(intel_encoder);
 	int type = intel_encoder->type;
 	uint32_t val;
@@ -1151,7 +1564,11 @@ static void intel_ddi_post_disable(struct intel_encoder *intel_encoder)
 		intel_edp_panel_off(intel_dp);
 	}
 
-	I915_WRITE(PORT_CLK_SEL(port), PORT_CLK_SEL_NONE);
+	if (IS_SKYLAKE(dev))
+		I915_WRITE(DPLL_CTRL2, (I915_READ(DPLL_CTRL2) |
+					DPLL_CTRL2_DDI_CLK_OFF(port)));
+	else
+		I915_WRITE(PORT_CLK_SEL(port), PORT_CLK_SEL_NONE);
 }
 
 static void intel_enable_ddi(struct intel_encoder *intel_encoder)
@@ -1159,12 +1576,10 @@ static void intel_enable_ddi(struct intel_encoder *intel_encoder)
 	struct drm_encoder *encoder = &intel_encoder->base;
 	struct drm_crtc *crtc = encoder->crtc;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	int pipe = intel_crtc->pipe;
 	struct drm_device *dev = encoder->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	enum port port = intel_ddi_get_encoder_port(intel_encoder);
 	int type = intel_encoder->type;
-	uint32_t tmp;
 
 	if (type == INTEL_OUTPUT_HDMI) {
 		struct intel_digital_port *intel_dig_port =
@@ -1180,18 +1595,16 @@ static void intel_enable_ddi(struct intel_encoder *intel_encoder)
 	} else if (type == INTEL_OUTPUT_EDP) {
 		struct intel_dp *intel_dp = enc_to_intel_dp(encoder);
 
-		if (port == PORT_A)
+		if (port == PORT_A && INTEL_INFO(dev)->gen < 9)
 			intel_dp_stop_link_train(intel_dp);
 
 		intel_edp_backlight_on(intel_dp);
-		intel_edp_psr_enable(intel_dp);
+		intel_psr_enable(intel_dp);
 	}
 
 	if (intel_crtc->config.has_audio) {
 		intel_display_power_get(dev_priv, POWER_DOMAIN_AUDIO);
-		tmp = I915_READ(HSW_AUD_PIN_ELD_CP_VLD);
-		tmp |= ((AUDIO_OUTPUT_ENABLE_A | AUDIO_ELD_VALID_A) << (pipe * 4));
-		I915_WRITE(HSW_AUD_PIN_ELD_CP_VLD, tmp);
+		intel_audio_codec_enable(intel_encoder);
 	}
 }
 
@@ -1200,30 +1613,71 @@ static void intel_disable_ddi(struct intel_encoder *intel_encoder)
 	struct drm_encoder *encoder = &intel_encoder->base;
 	struct drm_crtc *crtc = encoder->crtc;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	int pipe = intel_crtc->pipe;
 	int type = intel_encoder->type;
 	struct drm_device *dev = encoder->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t tmp;
 
-	/* We can't touch HSW_AUD_PIN_ELD_CP_VLD uncionditionally because this
-	 * register is part of the power well on Haswell. */
 	if (intel_crtc->config.has_audio) {
-		tmp = I915_READ(HSW_AUD_PIN_ELD_CP_VLD);
-		tmp &= ~((AUDIO_OUTPUT_ENABLE_A | AUDIO_ELD_VALID_A) <<
-			 (pipe * 4));
-		I915_WRITE(HSW_AUD_PIN_ELD_CP_VLD, tmp);
+		intel_audio_codec_disable(intel_encoder);
 		intel_display_power_put(dev_priv, POWER_DOMAIN_AUDIO);
 	}
 
 	if (type == INTEL_OUTPUT_EDP) {
 		struct intel_dp *intel_dp = enc_to_intel_dp(encoder);
 
-		intel_edp_psr_disable(intel_dp);
+		intel_psr_disable(intel_dp);
 		intel_edp_backlight_off(intel_dp);
 	}
 }
 
+static int skl_get_cdclk_freq(struct drm_i915_private *dev_priv)
+{
+	uint32_t lcpll1 = I915_READ(LCPLL1_CTL);
+	uint32_t cdctl = I915_READ(CDCLK_CTL);
+	uint32_t linkrate;
+
+	if (!(lcpll1 & LCPLL_PLL_ENABLE)) {
+		WARN(1, "LCPLL1 not enabled\n");
+		return 24000; /* 24MHz is the cd freq with NSSC ref */
+	}
+
+	if ((cdctl & CDCLK_FREQ_SEL_MASK) == CDCLK_FREQ_540)
+		return 540000;
+
+	linkrate = (I915_READ(DPLL_CTRL1) &
+		    DPLL_CRTL1_LINK_RATE_MASK(SKL_DPLL0)) >> 1;
+
+	if (linkrate == DPLL_CRTL1_LINK_RATE_2160 ||
+	    linkrate == DPLL_CRTL1_LINK_RATE_1080) {
+		/* vco 8640 */
+		switch (cdctl & CDCLK_FREQ_SEL_MASK) {
+		case CDCLK_FREQ_450_432:
+			return 432000;
+		case CDCLK_FREQ_337_308:
+			return 308570;
+		case CDCLK_FREQ_675_617:
+			return 617140;
+		default:
+			WARN(1, "Unknown cd freq selection\n");
+		}
+	} else {
+		/* vco 8100 */
+		switch (cdctl & CDCLK_FREQ_SEL_MASK) {
+		case CDCLK_FREQ_450_432:
+			return 450000;
+		case CDCLK_FREQ_337_308:
+			return 337500;
+		case CDCLK_FREQ_675_617:
+			return 675000;
+		default:
+			WARN(1, "Unknown cd freq selection\n");
+		}
+	}
+
+	/* error case, do as if DPLL0 isn't enabled */
+	return 24000;
+}
+
 static int bdw_get_cdclk_freq(struct drm_i915_private *dev_priv)
 {
 	uint32_t lcpll = I915_READ(LCPLL_CTL);
@@ -1255,7 +1709,7 @@ static int hsw_get_cdclk_freq(struct drm_i915_private *dev_priv)
 		return 450000;
 	else if (freq == LCPLL_CLK_FREQ_450)
 		return 450000;
-	else if (IS_ULT(dev))
+	else if (IS_HSW_ULT(dev))
 		return 337500;
 	else
 		return 540000;
@@ -1265,6 +1719,9 @@ int intel_ddi_get_cdclk_freq(struct drm_i915_private *dev_priv)
 {
 	struct drm_device *dev = dev_priv->dev;
 
+	if (IS_SKYLAKE(dev))
+		return skl_get_cdclk_freq(dev_priv);
+
 	if (IS_BROADWELL(dev))
 		return bdw_get_cdclk_freq(dev_priv);
 
@@ -1275,7 +1732,7 @@ int intel_ddi_get_cdclk_freq(struct drm_i915_private *dev_priv)
 static void hsw_ddi_pll_enable(struct drm_i915_private *dev_priv,
 			       struct intel_shared_dpll *pll)
 {
-	I915_WRITE(WRPLL_CTL(pll->id), pll->hw_state.wrpll);
+	I915_WRITE(WRPLL_CTL(pll->id), pll->config.hw_state.wrpll);
 	POSTING_READ(WRPLL_CTL(pll->id));
 	udelay(20);
 }
@@ -1296,7 +1753,7 @@ static bool hsw_ddi_pll_get_hw_state(struct drm_i915_private *dev_priv,
 {
 	uint32_t val;
 
-	if (!intel_display_power_enabled(dev_priv, POWER_DOMAIN_PLLS))
+	if (!intel_display_power_is_enabled(dev_priv, POWER_DOMAIN_PLLS))
 		return false;
 
 	val = I915_READ(WRPLL_CTL(pll->id));
@@ -1326,26 +1783,156 @@ static void hsw_shared_dplls_init(struct drm_i915_private *dev_priv)
 	}
 }
 
+static const char * const skl_ddi_pll_names[] = {
+	"DPLL 1",
+	"DPLL 2",
+	"DPLL 3",
+};
+
+struct skl_dpll_regs {
+	u32 ctl, cfgcr1, cfgcr2;
+};
+
+/* this array is indexed by the *shared* pll id */
+static const struct skl_dpll_regs skl_dpll_regs[3] = {
+	{
+		/* DPLL 1 */
+		.ctl = LCPLL2_CTL,
+		.cfgcr1 = DPLL1_CFGCR1,
+		.cfgcr2 = DPLL1_CFGCR2,
+	},
+	{
+		/* DPLL 2 */
+		.ctl = WRPLL_CTL1,
+		.cfgcr1 = DPLL2_CFGCR1,
+		.cfgcr2 = DPLL2_CFGCR2,
+	},
+	{
+		/* DPLL 3 */
+		.ctl = WRPLL_CTL2,
+		.cfgcr1 = DPLL3_CFGCR1,
+		.cfgcr2 = DPLL3_CFGCR2,
+	},
+};
+
+static void skl_ddi_pll_enable(struct drm_i915_private *dev_priv,
+			       struct intel_shared_dpll *pll)
+{
+	uint32_t val;
+	unsigned int dpll;
+	const struct skl_dpll_regs *regs = skl_dpll_regs;
+
+	/* DPLL0 is not part of the shared DPLLs, so pll->id is 0 for DPLL1 */
+	dpll = pll->id + 1;
+
+	val = I915_READ(DPLL_CTRL1);
+
+	val &= ~(DPLL_CTRL1_HDMI_MODE(dpll) | DPLL_CTRL1_SSC(dpll) |
+		 DPLL_CRTL1_LINK_RATE_MASK(dpll));
+	val |= pll->config.hw_state.ctrl1 << (dpll * 6);
+
+	I915_WRITE(DPLL_CTRL1, val);
+	POSTING_READ(DPLL_CTRL1);
+
+	I915_WRITE(regs[pll->id].cfgcr1, pll->config.hw_state.cfgcr1);
+	I915_WRITE(regs[pll->id].cfgcr2, pll->config.hw_state.cfgcr2);
+	POSTING_READ(regs[pll->id].cfgcr1);
+	POSTING_READ(regs[pll->id].cfgcr2);
+
+	/* the enable bit is always bit 31 */
+	I915_WRITE(regs[pll->id].ctl,
+		   I915_READ(regs[pll->id].ctl) | LCPLL_PLL_ENABLE);
+
+	if (wait_for(I915_READ(DPLL_STATUS) & DPLL_LOCK(dpll), 5))
+		DRM_ERROR("DPLL %d not locked\n", dpll);
+}
+
+static void skl_ddi_pll_disable(struct drm_i915_private *dev_priv,
+				struct intel_shared_dpll *pll)
+{
+	const struct skl_dpll_regs *regs = skl_dpll_regs;
+
+	/* the enable bit is always bit 31 */
+	I915_WRITE(regs[pll->id].ctl,
+		   I915_READ(regs[pll->id].ctl) & ~LCPLL_PLL_ENABLE);
+	POSTING_READ(regs[pll->id].ctl);
+}
+
+static bool skl_ddi_pll_get_hw_state(struct drm_i915_private *dev_priv,
+				     struct intel_shared_dpll *pll,
+				     struct intel_dpll_hw_state *hw_state)
+{
+	uint32_t val;
+	unsigned int dpll;
+	const struct skl_dpll_regs *regs = skl_dpll_regs;
+
+	if (!intel_display_power_is_enabled(dev_priv, POWER_DOMAIN_PLLS))
+		return false;
+
+	/* DPLL0 is not part of the shared DPLLs, so pll->id is 0 for DPLL1 */
+	dpll = pll->id + 1;
+
+	val = I915_READ(regs[pll->id].ctl);
+	if (!(val & LCPLL_PLL_ENABLE))
+		return false;
+
+	val = I915_READ(DPLL_CTRL1);
+	hw_state->ctrl1 = (val >> (dpll * 6)) & 0x3f;
+
+	/* avoid reading back stale values if HDMI mode is not enabled */
+	if (val & DPLL_CTRL1_HDMI_MODE(dpll)) {
+		hw_state->cfgcr1 = I915_READ(regs[pll->id].cfgcr1);
+		hw_state->cfgcr2 = I915_READ(regs[pll->id].cfgcr2);
+	}
+
+	return true;
+}
+
+static void skl_shared_dplls_init(struct drm_i915_private *dev_priv)
+{
+	int i;
+
+	dev_priv->num_shared_dpll = 3;
+
+	for (i = 0; i < dev_priv->num_shared_dpll; i++) {
+		dev_priv->shared_dplls[i].id = i;
+		dev_priv->shared_dplls[i].name = skl_ddi_pll_names[i];
+		dev_priv->shared_dplls[i].disable = skl_ddi_pll_disable;
+		dev_priv->shared_dplls[i].enable = skl_ddi_pll_enable;
+		dev_priv->shared_dplls[i].get_hw_state =
+			skl_ddi_pll_get_hw_state;
+	}
+}
+
 void intel_ddi_pll_init(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	uint32_t val = I915_READ(LCPLL_CTL);
 
-	hsw_shared_dplls_init(dev_priv);
-
-	/* The LCPLL register should be turned on by the BIOS. For now let's
-	 * just check its state and print errors in case something is wrong.
-	 * Don't even try to turn it on.
-	 */
+	if (IS_SKYLAKE(dev))
+		skl_shared_dplls_init(dev_priv);
+	else
+		hsw_shared_dplls_init(dev_priv);
 
 	DRM_DEBUG_KMS("CDCLK running at %dKHz\n",
 		      intel_ddi_get_cdclk_freq(dev_priv));
 
-	if (val & LCPLL_CD_SOURCE_FCLK)
-		DRM_ERROR("CDCLK source is not LCPLL\n");
+	if (IS_SKYLAKE(dev)) {
+		if (!(I915_READ(LCPLL1_CTL) & LCPLL_PLL_ENABLE))
+			DRM_ERROR("LCPLL1 is disabled\n");
+	} else {
+		/*
+		 * The LCPLL register should be turned on by the BIOS. For now
+		 * let's just check its state and print errors in case
+		 * something is wrong.  Don't even try to turn it on.
+		 */
 
-	if (val & LCPLL_PLL_DISABLE)
-		DRM_ERROR("LCPLL is disabled\n");
+		if (val & LCPLL_CD_SOURCE_FCLK)
+			DRM_ERROR("CDCLK source is not LCPLL\n");
+
+		if (val & LCPLL_PLL_DISABLE)
+			DRM_ERROR("LCPLL is disabled\n");
+	}
 }
 
 void intel_ddi_prepare_link_retrain(struct drm_encoder *encoder)
@@ -1440,7 +2027,9 @@ void intel_ddi_get_config(struct intel_encoder *encoder,
 	struct drm_i915_private *dev_priv = encoder->base.dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->base.crtc);
 	enum transcoder cpu_transcoder = intel_crtc->config.cpu_transcoder;
+	struct intel_hdmi *intel_hdmi;
 	u32 temp, flags = 0;
+	struct drm_device *dev = dev_priv->dev;
 
 	temp = I915_READ(TRANS_DDI_FUNC_CTL(cpu_transcoder));
 	if (temp & TRANS_DDI_PHSYNC)
@@ -1474,6 +2063,11 @@ void intel_ddi_get_config(struct intel_encoder *encoder,
 	switch (temp & TRANS_DDI_MODE_SELECT_MASK) {
 	case TRANS_DDI_MODE_SELECT_HDMI:
 		pipe_config->has_hdmi_sink = true;
+		intel_hdmi = enc_to_intel_hdmi(&encoder->base);
+
+		if (intel_hdmi->infoframe_enabled(&encoder->base))
+			pipe_config->has_infoframe = true;
+		break;
 	case TRANS_DDI_MODE_SELECT_DVI:
 	case TRANS_DDI_MODE_SELECT_FDI:
 		break;
@@ -1486,9 +2080,9 @@ void intel_ddi_get_config(struct intel_encoder *encoder,
 		break;
 	}
 
-	if (intel_display_power_enabled(dev_priv, POWER_DOMAIN_AUDIO)) {
+	if (intel_display_power_is_enabled(dev_priv, POWER_DOMAIN_AUDIO)) {
 		temp = I915_READ(HSW_AUD_PIN_ELD_CP_VLD);
-		if (temp & (AUDIO_OUTPUT_ENABLE_A << (intel_crtc->pipe * 4)))
+		if (temp & AUDIO_OUTPUT_ENABLE(intel_crtc->pipe))
 			pipe_config->has_audio = true;
 	}
 
@@ -1512,7 +2106,10 @@ void intel_ddi_get_config(struct intel_encoder *encoder,
 		dev_priv->vbt.edp_bpp = pipe_config->pipe_bpp;
 	}
 
-	hsw_ddi_clock_get(encoder, pipe_config);
+	if (INTEL_INFO(dev)->gen <= 8)
+		hsw_ddi_clock_get(encoder, pipe_config);
+	else
+		skl_ddi_clock_get(encoder, pipe_config);
 }
 
 static void intel_ddi_destroy(struct drm_encoder *encoder)
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index 9cb5c95d5898..fb3e3d429191 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -73,8 +73,6 @@ static const uint32_t intel_cursor_formats[] = {
 	DRM_FORMAT_ARGB8888,
 };
 
-static void intel_increase_pllclock(struct drm_device *dev,
-				    enum pipe pipe);
 static void intel_crtc_update_cursor(struct drm_crtc *crtc, bool on);
 
 static void i9xx_crtc_clock_get(struct intel_crtc *crtc,
@@ -96,8 +94,10 @@ static void intel_cpu_transcoder_set_m_n(struct intel_crtc *crtc,
 static void ironlake_set_pipeconf(struct drm_crtc *crtc);
 static void haswell_set_pipeconf(struct drm_crtc *crtc);
 static void intel_set_pipe_csc(struct drm_crtc *crtc);
-static void vlv_prepare_pll(struct intel_crtc *crtc);
-static void chv_prepare_pll(struct intel_crtc *crtc);
+static void vlv_prepare_pll(struct intel_crtc *crtc,
+			    const struct intel_crtc_config *pipe_config);
+static void chv_prepare_pll(struct intel_crtc *crtc,
+			    const struct intel_crtc_config *pipe_config);
 
 static struct intel_encoder *intel_find_encoder(struct intel_connector *connector, int pipe)
 {
@@ -408,25 +408,43 @@ static void vlv_clock(int refclk, intel_clock_t *clock)
 /**
  * Returns whether any output on the specified pipe is of the specified type
  */
-static bool intel_pipe_has_type(struct drm_crtc *crtc, int type)
+bool intel_pipe_has_type(struct intel_crtc *crtc, enum intel_output_type type)
 {
-	struct drm_device *dev = crtc->dev;
+	struct drm_device *dev = crtc->base.dev;
 	struct intel_encoder *encoder;
 
-	for_each_encoder_on_crtc(dev, crtc, encoder)
+	for_each_encoder_on_crtc(dev, &crtc->base, encoder)
 		if (encoder->type == type)
 			return true;
 
 	return false;
 }
 
-static const intel_limit_t *intel_ironlake_limit(struct drm_crtc *crtc,
+/**
+ * Returns whether any output on the specified pipe will have the specified
+ * type after a staged modeset is complete, i.e., the same as
+ * intel_pipe_has_type() but looking at encoder->new_crtc instead of
+ * encoder->crtc.
+ */
+static bool intel_pipe_will_have_type(struct intel_crtc *crtc, int type)
+{
+	struct drm_device *dev = crtc->base.dev;
+	struct intel_encoder *encoder;
+
+	for_each_intel_encoder(dev, encoder)
+		if (encoder->new_crtc == crtc && encoder->type == type)
+			return true;
+
+	return false;
+}
+
+static const intel_limit_t *intel_ironlake_limit(struct intel_crtc *crtc,
 						int refclk)
 {
-	struct drm_device *dev = crtc->dev;
+	struct drm_device *dev = crtc->base.dev;
 	const intel_limit_t *limit;
 
-	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS)) {
+	if (intel_pipe_will_have_type(crtc, INTEL_OUTPUT_LVDS)) {
 		if (intel_is_dual_link_lvds(dev)) {
 			if (refclk == 100000)
 				limit = &intel_limits_ironlake_dual_lvds_100m;
@@ -444,20 +462,20 @@ static const intel_limit_t *intel_ironlake_limit(struct drm_crtc *crtc,
 	return limit;
 }
 
-static const intel_limit_t *intel_g4x_limit(struct drm_crtc *crtc)
+static const intel_limit_t *intel_g4x_limit(struct intel_crtc *crtc)
 {
-	struct drm_device *dev = crtc->dev;
+	struct drm_device *dev = crtc->base.dev;
 	const intel_limit_t *limit;
 
-	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS)) {
+	if (intel_pipe_will_have_type(crtc, INTEL_OUTPUT_LVDS)) {
 		if (intel_is_dual_link_lvds(dev))
 			limit = &intel_limits_g4x_dual_channel_lvds;
 		else
 			limit = &intel_limits_g4x_single_channel_lvds;
-	} else if (intel_pipe_has_type(crtc, INTEL_OUTPUT_HDMI) ||
-		   intel_pipe_has_type(crtc, INTEL_OUTPUT_ANALOG)) {
+	} else if (intel_pipe_will_have_type(crtc, INTEL_OUTPUT_HDMI) ||
+		   intel_pipe_will_have_type(crtc, INTEL_OUTPUT_ANALOG)) {
 		limit = &intel_limits_g4x_hdmi;
-	} else if (intel_pipe_has_type(crtc, INTEL_OUTPUT_SDVO)) {
+	} else if (intel_pipe_will_have_type(crtc, INTEL_OUTPUT_SDVO)) {
 		limit = &intel_limits_g4x_sdvo;
 	} else /* The option is for other outputs */
 		limit = &intel_limits_i9xx_sdvo;
@@ -465,9 +483,9 @@ static const intel_limit_t *intel_g4x_limit(struct drm_crtc *crtc)
 	return limit;
 }
 
-static const intel_limit_t *intel_limit(struct drm_crtc *crtc, int refclk)
+static const intel_limit_t *intel_limit(struct intel_crtc *crtc, int refclk)
 {
-	struct drm_device *dev = crtc->dev;
+	struct drm_device *dev = crtc->base.dev;
 	const intel_limit_t *limit;
 
 	if (HAS_PCH_SPLIT(dev))
@@ -475,7 +493,7 @@ static const intel_limit_t *intel_limit(struct drm_crtc *crtc, int refclk)
 	else if (IS_G4X(dev)) {
 		limit = intel_g4x_limit(crtc);
 	} else if (IS_PINEVIEW(dev)) {
-		if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS))
+		if (intel_pipe_will_have_type(crtc, INTEL_OUTPUT_LVDS))
 			limit = &intel_limits_pineview_lvds;
 		else
 			limit = &intel_limits_pineview_sdvo;
@@ -484,14 +502,14 @@ static const intel_limit_t *intel_limit(struct drm_crtc *crtc, int refclk)
 	} else if (IS_VALLEYVIEW(dev)) {
 		limit = &intel_limits_vlv;
 	} else if (!IS_GEN2(dev)) {
-		if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS))
+		if (intel_pipe_will_have_type(crtc, INTEL_OUTPUT_LVDS))
 			limit = &intel_limits_i9xx_lvds;
 		else
 			limit = &intel_limits_i9xx_sdvo;
 	} else {
-		if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS))
+		if (intel_pipe_will_have_type(crtc, INTEL_OUTPUT_LVDS))
 			limit = &intel_limits_i8xx_lvds;
-		else if (intel_pipe_has_type(crtc, INTEL_OUTPUT_DVO))
+		else if (intel_pipe_will_have_type(crtc, INTEL_OUTPUT_DVO))
 			limit = &intel_limits_i8xx_dvo;
 		else
 			limit = &intel_limits_i8xx_dac;
@@ -578,15 +596,15 @@ static bool intel_PLL_is_valid(struct drm_device *dev,
 }
 
 static bool
-i9xx_find_best_dpll(const intel_limit_t *limit, struct drm_crtc *crtc,
+i9xx_find_best_dpll(const intel_limit_t *limit, struct intel_crtc *crtc,
 		    int target, int refclk, intel_clock_t *match_clock,
 		    intel_clock_t *best_clock)
 {
-	struct drm_device *dev = crtc->dev;
+	struct drm_device *dev = crtc->base.dev;
 	intel_clock_t clock;
 	int err = target;
 
-	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS)) {
+	if (intel_pipe_will_have_type(crtc, INTEL_OUTPUT_LVDS)) {
 		/*
 		 * For LVDS just rely on its current settings for dual-channel.
 		 * We haven't figured out how to reliably set up different
@@ -639,15 +657,15 @@ i9xx_find_best_dpll(const intel_limit_t *limit, struct drm_crtc *crtc,
 }
 
 static bool
-pnv_find_best_dpll(const intel_limit_t *limit, struct drm_crtc *crtc,
+pnv_find_best_dpll(const intel_limit_t *limit, struct intel_crtc *crtc,
 		   int target, int refclk, intel_clock_t *match_clock,
 		   intel_clock_t *best_clock)
 {
-	struct drm_device *dev = crtc->dev;
+	struct drm_device *dev = crtc->base.dev;
 	intel_clock_t clock;
 	int err = target;
 
-	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS)) {
+	if (intel_pipe_will_have_type(crtc, INTEL_OUTPUT_LVDS)) {
 		/*
 		 * For LVDS just rely on its current settings for dual-channel.
 		 * We haven't figured out how to reliably set up different
@@ -698,11 +716,11 @@ pnv_find_best_dpll(const intel_limit_t *limit, struct drm_crtc *crtc,
 }
 
 static bool
-g4x_find_best_dpll(const intel_limit_t *limit, struct drm_crtc *crtc,
+g4x_find_best_dpll(const intel_limit_t *limit, struct intel_crtc *crtc,
 		   int target, int refclk, intel_clock_t *match_clock,
 		   intel_clock_t *best_clock)
 {
-	struct drm_device *dev = crtc->dev;
+	struct drm_device *dev = crtc->base.dev;
 	intel_clock_t clock;
 	int max_n;
 	bool found;
@@ -710,7 +728,7 @@ g4x_find_best_dpll(const intel_limit_t *limit, struct drm_crtc *crtc,
 	int err_most = (target >> 8) + (target >> 9);
 	found = false;
 
-	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS)) {
+	if (intel_pipe_will_have_type(crtc, INTEL_OUTPUT_LVDS)) {
 		if (intel_is_dual_link_lvds(dev))
 			clock.p2 = limit->p2.p2_fast;
 		else
@@ -755,11 +773,11 @@ g4x_find_best_dpll(const intel_limit_t *limit, struct drm_crtc *crtc,
 }
 
 static bool
-vlv_find_best_dpll(const intel_limit_t *limit, struct drm_crtc *crtc,
+vlv_find_best_dpll(const intel_limit_t *limit, struct intel_crtc *crtc,
 		   int target, int refclk, intel_clock_t *match_clock,
 		   intel_clock_t *best_clock)
 {
-	struct drm_device *dev = crtc->dev;
+	struct drm_device *dev = crtc->base.dev;
 	intel_clock_t clock;
 	unsigned int bestppm = 1000000;
 	/* min update 19.2 MHz */
@@ -812,11 +830,11 @@ vlv_find_best_dpll(const intel_limit_t *limit, struct drm_crtc *crtc,
 }
 
 static bool
-chv_find_best_dpll(const intel_limit_t *limit, struct drm_crtc *crtc,
+chv_find_best_dpll(const intel_limit_t *limit, struct intel_crtc *crtc,
 		   int target, int refclk, intel_clock_t *match_clock,
 		   intel_clock_t *best_clock)
 {
-	struct drm_device *dev = crtc->dev;
+	struct drm_device *dev = crtc->base.dev;
 	intel_clock_t clock;
 	uint64_t m2;
 	int found = false;
@@ -889,60 +907,6 @@ enum transcoder intel_pipe_to_cpu_transcoder(struct drm_i915_private *dev_priv,
 	return intel_crtc->config.cpu_transcoder;
 }
 
-static void g4x_wait_for_vblank(struct drm_device *dev, int pipe)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 frame, frame_reg = PIPE_FRMCOUNT_GM45(pipe);
-
-	frame = I915_READ(frame_reg);
-
-	if (wait_for(I915_READ_NOTRACE(frame_reg) != frame, 50))
-		WARN(1, "vblank wait on pipe %c timed out\n",
-		     pipe_name(pipe));
-}
-
-/**
- * intel_wait_for_vblank - wait for vblank on a given pipe
- * @dev: drm device
- * @pipe: pipe to wait for
- *
- * Wait for vblank to occur on a given pipe.  Needed for various bits of
- * mode setting code.
- */
-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 (IS_G4X(dev) || INTEL_INFO(dev)->gen >= 5) {
-		g4x_wait_for_vblank(dev, pipe);
-		return;
-	}
-
-	/* Clear existing vblank status. Note this will clear any other
-	 * sticky status fields as well.
-	 *
-	 * This races with i915_driver_irq_handler() with the result
-	 * that either function could miss a vblank event.  Here it is not
-	 * fatal, as we will either wait upon the next vblank interrupt or
-	 * timeout.  Generally speaking intel_wait_for_vblank() is only
-	 * called during modeset at which time the GPU should be idle and
-	 * should *not* be performing page flips and thus not waiting on
-	 * vblanks...
-	 * Currently, the result of us stealing a vblank from the irq
-	 * handler is that a single frame will be skipped during swapbuffers.
-	 */
-	I915_WRITE(pipestat_reg,
-		   I915_READ(pipestat_reg) | PIPE_VBLANK_INTERRUPT_STATUS);
-
-	/* Wait for vblank interrupt bit to set */
-	if (wait_for(I915_READ(pipestat_reg) &
-		     PIPE_VBLANK_INTERRUPT_STATUS,
-		     50))
-		DRM_DEBUG_KMS("vblank wait on pipe %c timed out\n",
-			      pipe_name(pipe));
-}
-
 static bool pipe_dsl_stopped(struct drm_device *dev, enum pipe pipe)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -1189,8 +1153,8 @@ void assert_fdi_rx_pll(struct drm_i915_private *dev_priv,
 	     state_string(state), state_string(cur_state));
 }
 
-static void assert_panel_unlocked(struct drm_i915_private *dev_priv,
-				  enum pipe pipe)
+void assert_panel_unlocked(struct drm_i915_private *dev_priv,
+			   enum pipe pipe)
 {
 	struct drm_device *dev = dev_priv->dev;
 	int pp_reg;
@@ -1263,7 +1227,7 @@ void assert_pipe(struct drm_i915_private *dev_priv,
 	    (pipe == PIPE_B && dev_priv->quirks & QUIRK_PIPEB_FORCE))
 		state = true;
 
-	if (!intel_display_power_enabled(dev_priv,
+	if (!intel_display_power_is_enabled(dev_priv,
 				POWER_DOMAIN_TRANSCODER(cpu_transcoder))) {
 		cur_state = false;
 	} else {
@@ -1332,7 +1296,14 @@ static void assert_sprites_disabled(struct drm_i915_private *dev_priv,
 	int reg, sprite;
 	u32 val;
 
-	if (IS_VALLEYVIEW(dev)) {
+	if (INTEL_INFO(dev)->gen >= 9) {
+		for_each_sprite(pipe, sprite) {
+			val = I915_READ(PLANE_CTL(pipe, sprite));
+			WARN(val & PLANE_CTL_ENABLE,
+			     "plane %d assertion failure, should be off on pipe %c but is still active\n",
+			     sprite, pipe_name(pipe));
+		}
+	} else if (IS_VALLEYVIEW(dev)) {
 		for_each_sprite(pipe, sprite) {
 			reg = SPCNTR(pipe, sprite);
 			val = I915_READ(reg);
@@ -1533,12 +1504,13 @@ static void intel_init_dpio(struct drm_device *dev)
 	}
 }
 
-static void vlv_enable_pll(struct intel_crtc *crtc)
+static void vlv_enable_pll(struct intel_crtc *crtc,
+			   const struct intel_crtc_config *pipe_config)
 {
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int reg = DPLL(crtc->pipe);
-	u32 dpll = crtc->config.dpll_hw_state.dpll;
+	u32 dpll = pipe_config->dpll_hw_state.dpll;
 
 	assert_pipe_disabled(dev_priv, crtc->pipe);
 
@@ -1556,7 +1528,7 @@ static void vlv_enable_pll(struct intel_crtc *crtc)
 	if (wait_for(((I915_READ(reg) & DPLL_LOCK_VLV) == DPLL_LOCK_VLV), 1))
 		DRM_ERROR("DPLL %d failed to lock\n", crtc->pipe);
 
-	I915_WRITE(DPLL_MD(crtc->pipe), crtc->config.dpll_hw_state.dpll_md);
+	I915_WRITE(DPLL_MD(crtc->pipe), pipe_config->dpll_hw_state.dpll_md);
 	POSTING_READ(DPLL_MD(crtc->pipe));
 
 	/* We do this three times for luck */
@@ -1571,7 +1543,8 @@ static void vlv_enable_pll(struct intel_crtc *crtc)
 	udelay(150); /* wait for warmup */
 }
 
-static void chv_enable_pll(struct intel_crtc *crtc)
+static void chv_enable_pll(struct intel_crtc *crtc,
+			   const struct intel_crtc_config *pipe_config)
 {
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -1596,14 +1569,14 @@ static void chv_enable_pll(struct intel_crtc *crtc)
 	udelay(1);
 
 	/* Enable PLL */
-	I915_WRITE(DPLL(pipe), crtc->config.dpll_hw_state.dpll);
+	I915_WRITE(DPLL(pipe), pipe_config->dpll_hw_state.dpll);
 
 	/* Check PLL is locked */
 	if (wait_for(((I915_READ(DPLL(pipe)) & DPLL_LOCK_VLV) == DPLL_LOCK_VLV), 1))
 		DRM_ERROR("PLL %d failed to lock\n", pipe);
 
 	/* not sure when this should be written */
-	I915_WRITE(DPLL_MD(pipe), crtc->config.dpll_hw_state.dpll_md);
+	I915_WRITE(DPLL_MD(pipe), pipe_config->dpll_hw_state.dpll_md);
 	POSTING_READ(DPLL_MD(pipe));
 
 	mutex_unlock(&dev_priv->dpio_lock);
@@ -1616,7 +1589,7 @@ static int intel_num_dvo_pipes(struct drm_device *dev)
 
 	for_each_intel_crtc(dev, crtc)
 		count += crtc->active &&
-			intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_DVO);
+			intel_pipe_has_type(crtc, INTEL_OUTPUT_DVO);
 
 	return count;
 }
@@ -1695,7 +1668,7 @@ static void i9xx_disable_pll(struct intel_crtc *crtc)
 
 	/* Disable DVO 2x clock on both PLLs if necessary */
 	if (IS_I830(dev) &&
-	    intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_DVO) &&
+	    intel_pipe_has_type(crtc, INTEL_OUTPUT_DVO) &&
 	    intel_num_dvo_pipes(dev) == 1) {
 		I915_WRITE(DPLL(PIPE_B),
 			   I915_READ(DPLL(PIPE_B)) & ~DPLL_DVO_2X_MODE);
@@ -1806,7 +1779,7 @@ static void intel_prepare_shared_dpll(struct intel_crtc *crtc)
 	if (WARN_ON(pll == NULL))
 		return;
 
-	WARN_ON(!pll->refcount);
+	WARN_ON(!pll->config.crtc_mask);
 	if (pll->active == 0) {
 		DRM_DEBUG_DRIVER("setting up %s\n", pll->name);
 		WARN_ON(pll->on);
@@ -1833,7 +1806,7 @@ static void intel_enable_shared_dpll(struct intel_crtc *crtc)
 	if (WARN_ON(pll == NULL))
 		return;
 
-	if (WARN_ON(pll->refcount == 0))
+	if (WARN_ON(pll->config.crtc_mask == 0))
 		return;
 
 	DRM_DEBUG_KMS("enable %s (active %d, on? %d) for crtc %d\n",
@@ -1865,7 +1838,7 @@ static void intel_disable_shared_dpll(struct intel_crtc *crtc)
 	if (WARN_ON(pll == NULL))
 	       return;
 
-	if (WARN_ON(pll->refcount == 0))
+	if (WARN_ON(pll->config.crtc_mask == 0))
 		return;
 
 	DRM_DEBUG_KMS("disable %s (active %d, on? %d) for crtc %d\n",
@@ -1933,7 +1906,7 @@ static void ironlake_enable_pch_transcoder(struct drm_i915_private *dev_priv,
 	val &= ~TRANS_INTERLACE_MASK;
 	if ((pipeconf_val & PIPECONF_INTERLACE_MASK) == PIPECONF_INTERLACED_ILK)
 		if (HAS_PCH_IBX(dev_priv->dev) &&
-		    intel_pipe_has_type(crtc, INTEL_OUTPUT_SDVO))
+		    intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_SDVO))
 			val |= TRANS_LEGACY_INTERLACED_ILK;
 		else
 			val |= TRANS_INTERLACED;
@@ -2056,7 +2029,7 @@ static void intel_enable_pipe(struct intel_crtc *crtc)
 	 * need the check.
 	 */
 	if (!HAS_PCH_SPLIT(dev_priv->dev))
-		if (intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_DSI))
+		if (intel_pipe_has_type(crtc, INTEL_OUTPUT_DSI))
 			assert_dsi_pll_enabled(dev_priv);
 		else
 			assert_pll_enabled(dev_priv, pipe);
@@ -2221,11 +2194,13 @@ static int intel_align_height(struct drm_device *dev, int height, bool tiled)
 }
 
 int
-intel_pin_and_fence_fb_obj(struct drm_device *dev,
-			   struct drm_i915_gem_object *obj,
+intel_pin_and_fence_fb_obj(struct drm_plane *plane,
+			   struct drm_framebuffer *fb,
 			   struct intel_engine_cs *pipelined)
 {
+	struct drm_device *dev = fb->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_gem_object *obj = intel_fb_obj(fb);
 	u32 alignment;
 	int ret;
 
@@ -2233,7 +2208,9 @@ intel_pin_and_fence_fb_obj(struct drm_device *dev,
 
 	switch (obj->tiling_mode) {
 	case I915_TILING_NONE:
-		if (IS_BROADWATER(dev) || IS_CRESTLINE(dev))
+		if (INTEL_INFO(dev)->gen >= 9)
+			alignment = 256 * 1024;
+		else if (IS_BROADWATER(dev) || IS_CRESTLINE(dev))
 			alignment = 128 * 1024;
 		else if (INTEL_INFO(dev)->gen >= 4)
 			alignment = 4 * 1024;
@@ -2241,8 +2218,12 @@ intel_pin_and_fence_fb_obj(struct drm_device *dev,
 			alignment = 64 * 1024;
 		break;
 	case I915_TILING_X:
-		/* pin() will align the object as required by fence */
-		alignment = 0;
+		if (INTEL_INFO(dev)->gen >= 9)
+			alignment = 256 * 1024;
+		else {
+			/* pin() will align the object as required by fence */
+			alignment = 0;
+		}
 		break;
 	case I915_TILING_Y:
 		WARN(1, "Y tiled bo slipped through, driver bug!\n");
@@ -2402,6 +2383,7 @@ static void intel_find_plane_obj(struct intel_crtc *intel_crtc,
 				 struct intel_plane_config *plane_config)
 {
 	struct drm_device *dev = intel_crtc->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_crtc *c;
 	struct intel_crtc *i;
 	struct drm_i915_gem_object *obj;
@@ -2433,6 +2415,9 @@ static void intel_find_plane_obj(struct intel_crtc *intel_crtc,
 			continue;
 
 		if (i915_gem_obj_ggtt_offset(obj) == plane_config->base) {
+			if (obj->tiling_mode != I915_TILING_NONE)
+				dev_priv->preserve_bios_swizzle = true;
+
 			drm_framebuffer_reference(c->primary->fb);
 			intel_crtc->base.primary->fb = c->primary->fb;
 			obj->frontbuffer_bits |= INTEL_FRONTBUFFER_PRIMARY(intel_crtc->pipe);
@@ -2486,6 +2471,12 @@ static void i9xx_update_primary_plane(struct drm_crtc *crtc,
 			   ((intel_crtc->config.pipe_src_h - 1) << 16) |
 			   (intel_crtc->config.pipe_src_w - 1));
 		I915_WRITE(DSPPOS(plane), 0);
+	} else if (IS_CHERRYVIEW(dev) && plane == PLANE_B) {
+		I915_WRITE(PRIMSIZE(plane),
+			   ((intel_crtc->config.pipe_src_h - 1) << 16) |
+			   (intel_crtc->config.pipe_src_w - 1));
+		I915_WRITE(PRIMPOS(plane), 0);
+		I915_WRITE(PRIMCNSTALPHA(plane), 0);
 	}
 
 	switch (fb->pixel_format) {
@@ -2672,6 +2663,92 @@ static void ironlake_update_primary_plane(struct drm_crtc *crtc,
 	POSTING_READ(reg);
 }
 
+static void skylake_update_primary_plane(struct drm_crtc *crtc,
+					 struct drm_framebuffer *fb,
+					 int x, int y)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_framebuffer *intel_fb;
+	struct drm_i915_gem_object *obj;
+	int pipe = intel_crtc->pipe;
+	u32 plane_ctl, stride;
+
+	if (!intel_crtc->primary_enabled) {
+		I915_WRITE(PLANE_CTL(pipe, 0), 0);
+		I915_WRITE(PLANE_SURF(pipe, 0), 0);
+		POSTING_READ(PLANE_CTL(pipe, 0));
+		return;
+	}
+
+	plane_ctl = PLANE_CTL_ENABLE |
+		    PLANE_CTL_PIPE_GAMMA_ENABLE |
+		    PLANE_CTL_PIPE_CSC_ENABLE;
+
+	switch (fb->pixel_format) {
+	case DRM_FORMAT_RGB565:
+		plane_ctl |= PLANE_CTL_FORMAT_RGB_565;
+		break;
+	case DRM_FORMAT_XRGB8888:
+		plane_ctl |= PLANE_CTL_FORMAT_XRGB_8888;
+		break;
+	case DRM_FORMAT_XBGR8888:
+		plane_ctl |= PLANE_CTL_ORDER_RGBX;
+		plane_ctl |= PLANE_CTL_FORMAT_XRGB_8888;
+		break;
+	case DRM_FORMAT_XRGB2101010:
+		plane_ctl |= PLANE_CTL_FORMAT_XRGB_2101010;
+		break;
+	case DRM_FORMAT_XBGR2101010:
+		plane_ctl |= PLANE_CTL_ORDER_RGBX;
+		plane_ctl |= PLANE_CTL_FORMAT_XRGB_2101010;
+		break;
+	default:
+		BUG();
+	}
+
+	intel_fb = to_intel_framebuffer(fb);
+	obj = intel_fb->obj;
+
+	/*
+	 * The stride is either expressed as a multiple of 64 bytes chunks for
+	 * linear buffers or in number of tiles for tiled buffers.
+	 */
+	switch (obj->tiling_mode) {
+	case I915_TILING_NONE:
+		stride = fb->pitches[0] >> 6;
+		break;
+	case I915_TILING_X:
+		plane_ctl |= PLANE_CTL_TILED_X;
+		stride = fb->pitches[0] >> 9;
+		break;
+	default:
+		BUG();
+	}
+
+	plane_ctl |= PLANE_CTL_PLANE_GAMMA_DISABLE;
+	if (to_intel_plane(crtc->primary)->rotation == BIT(DRM_ROTATE_180))
+		plane_ctl |= PLANE_CTL_ROTATE_180;
+
+	I915_WRITE(PLANE_CTL(pipe, 0), plane_ctl);
+
+	DRM_DEBUG_KMS("Writing base %08lX %d,%d,%d,%d pitch=%d\n",
+		      i915_gem_obj_ggtt_offset(obj),
+		      x, y, fb->width, fb->height,
+		      fb->pitches[0]);
+
+	I915_WRITE(PLANE_POS(pipe, 0), 0);
+	I915_WRITE(PLANE_OFFSET(pipe, 0), (y << 16) | x);
+	I915_WRITE(PLANE_SIZE(pipe, 0),
+		   (intel_crtc->config.pipe_src_h - 1) << 16 |
+		   (intel_crtc->config.pipe_src_w - 1));
+	I915_WRITE(PLANE_STRIDE(pipe, 0), stride);
+	I915_WRITE(PLANE_SURF(pipe, 0), i915_gem_obj_ggtt_offset(obj));
+
+	POSTING_READ(PLANE_SURF(pipe, 0));
+}
+
 /* Assume fb object is pinned & idle & fenced and just update base pointers */
 static int
 intel_pipe_set_base_atomic(struct drm_crtc *crtc, struct drm_framebuffer *fb,
@@ -2682,32 +2759,16 @@ intel_pipe_set_base_atomic(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 
 	if (dev_priv->display.disable_fbc)
 		dev_priv->display.disable_fbc(dev);
-	intel_increase_pllclock(dev, to_intel_crtc(crtc)->pipe);
 
 	dev_priv->display.update_primary_plane(crtc, fb, x, y);
 
 	return 0;
 }
 
-void intel_display_handle_reset(struct drm_device *dev)
+static void intel_complete_page_flips(struct drm_device *dev)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_crtc *crtc;
 
-	/*
-	 * Flips in the rings have been nuked by the reset,
-	 * so complete all pending flips so that user space
-	 * will get its events and not get stuck.
-	 *
-	 * Also update the base address of all primary
-	 * planes to the the last fb to make sure we're
-	 * showing the correct fb after a reset.
-	 *
-	 * Need to make two loops over the crtcs so that we
-	 * don't try to grab a crtc mutex before the
-	 * pending_flip_queue really got woken up.
-	 */
-
 	for_each_crtc(dev, crtc) {
 		struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 		enum plane plane = intel_crtc->plane;
@@ -2715,6 +2776,12 @@ void intel_display_handle_reset(struct drm_device *dev)
 		intel_prepare_page_flip(dev, plane);
 		intel_finish_page_flip_plane(dev, plane);
 	}
+}
+
+static void intel_update_primary_planes(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc;
 
 	for_each_crtc(dev, crtc) {
 		struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
@@ -2734,6 +2801,79 @@ void intel_display_handle_reset(struct drm_device *dev)
 	}
 }
 
+void intel_prepare_reset(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = to_i915(dev);
+	struct intel_crtc *crtc;
+
+	/* no reset support for gen2 */
+	if (IS_GEN2(dev))
+		return;
+
+	/* reset doesn't touch the display */
+	if (INTEL_INFO(dev)->gen >= 5 || IS_G4X(dev))
+		return;
+
+	drm_modeset_lock_all(dev);
+
+	/*
+	 * Disabling the crtcs gracefully seems nicer. Also the
+	 * g33 docs say we should at least disable all the planes.
+	 */
+	for_each_intel_crtc(dev, crtc) {
+		if (crtc->active)
+			dev_priv->display.crtc_disable(&crtc->base);
+	}
+}
+
+void intel_finish_reset(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = to_i915(dev);
+
+	/*
+	 * Flips in the rings will be nuked by the reset,
+	 * so complete all pending flips so that user space
+	 * will get its events and not get stuck.
+	 */
+	intel_complete_page_flips(dev);
+
+	/* no reset support for gen2 */
+	if (IS_GEN2(dev))
+		return;
+
+	/* reset doesn't touch the display */
+	if (INTEL_INFO(dev)->gen >= 5 || IS_G4X(dev)) {
+		/*
+		 * Flips in the rings have been nuked by the reset,
+		 * so update the base address of all primary
+		 * planes to the the last fb to make sure we're
+		 * showing the correct fb after a reset.
+		 */
+		intel_update_primary_planes(dev);
+		return;
+	}
+
+	/*
+	 * The display has been reset as well,
+	 * so need a full re-initialization.
+	 */
+	intel_runtime_pm_disable_interrupts(dev_priv);
+	intel_runtime_pm_enable_interrupts(dev_priv);
+
+	intel_modeset_init_hw(dev);
+
+	spin_lock_irq(&dev_priv->irq_lock);
+	if (dev_priv->display.hpd_irq_setup)
+		dev_priv->display.hpd_irq_setup(dev);
+	spin_unlock_irq(&dev_priv->irq_lock);
+
+	intel_modeset_setup_hw_state(dev, true);
+
+	intel_hpd_init(dev_priv);
+
+	drm_modeset_unlock_all(dev);
+}
+
 static int
 intel_finish_fb(struct drm_framebuffer *old_fb)
 {
@@ -2762,20 +2902,58 @@ static bool intel_crtc_has_pending_flip(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);
-	unsigned long flags;
 	bool pending;
 
 	if (i915_reset_in_progress(&dev_priv->gpu_error) ||
 	    intel_crtc->reset_counter != atomic_read(&dev_priv->gpu_error.reset_counter))
 		return false;
 
-	spin_lock_irqsave(&dev->event_lock, flags);
+	spin_lock_irq(&dev->event_lock);
 	pending = to_intel_crtc(crtc)->unpin_work != NULL;
-	spin_unlock_irqrestore(&dev->event_lock, flags);
+	spin_unlock_irq(&dev->event_lock);
 
 	return pending;
 }
 
+static void intel_update_pipe_size(struct intel_crtc *crtc)
+{
+	struct drm_device *dev = crtc->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	const struct drm_display_mode *adjusted_mode;
+
+	if (!i915.fastboot)
+		return;
+
+	/*
+	 * Update pipe size and adjust fitter if needed: the reason for this is
+	 * that in compute_mode_changes we check the native mode (not the pfit
+	 * mode) to see if we can flip rather than do a full mode set. In the
+	 * fastboot case, we'll flip, but if we don't update the pipesrc and
+	 * pfit state, we'll end up with a big fb scanned out into the wrong
+	 * sized surface.
+	 *
+	 * To fix this properly, we need to hoist the checks up into
+	 * compute_mode_changes (or above), check the actual pfit state and
+	 * whether the platform allows pfit disable with pipe active, and only
+	 * then update the pipesrc and pfit state, even on the flip path.
+	 */
+
+	adjusted_mode = &crtc->config.adjusted_mode;
+
+	I915_WRITE(PIPESRC(crtc->pipe),
+		   ((adjusted_mode->crtc_hdisplay - 1) << 16) |
+		   (adjusted_mode->crtc_vdisplay - 1));
+	if (!crtc->config.pch_pfit.enabled &&
+	    (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS) ||
+	     intel_pipe_has_type(crtc, INTEL_OUTPUT_EDP))) {
+		I915_WRITE(PF_CTL(crtc->pipe), 0);
+		I915_WRITE(PF_WIN_POS(crtc->pipe), 0);
+		I915_WRITE(PF_WIN_SZ(crtc->pipe), 0);
+	}
+	crtc->config.pipe_src_w = adjusted_mode->crtc_hdisplay;
+	crtc->config.pipe_src_h = adjusted_mode->crtc_vdisplay;
+}
+
 static int
 intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 		    struct drm_framebuffer *fb)
@@ -2785,7 +2963,6 @@ intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	enum pipe pipe = intel_crtc->pipe;
 	struct drm_framebuffer *old_fb = crtc->primary->fb;
-	struct drm_i915_gem_object *obj = intel_fb_obj(fb);
 	struct drm_i915_gem_object *old_obj = intel_fb_obj(old_fb);
 	int ret;
 
@@ -2808,9 +2985,9 @@ intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 	}
 
 	mutex_lock(&dev->struct_mutex);
-	ret = intel_pin_and_fence_fb_obj(dev, obj, NULL);
+	ret = intel_pin_and_fence_fb_obj(crtc->primary, fb, NULL);
 	if (ret == 0)
-		i915_gem_track_fb(old_obj, obj,
+		i915_gem_track_fb(old_obj, intel_fb_obj(fb),
 				  INTEL_FRONTBUFFER_PRIMARY(pipe));
 	mutex_unlock(&dev->struct_mutex);
 	if (ret != 0) {
@@ -2818,37 +2995,6 @@ intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 		return ret;
 	}
 
-	/*
-	 * Update pipe size and adjust fitter if needed: the reason for this is
-	 * that in compute_mode_changes we check the native mode (not the pfit
-	 * mode) to see if we can flip rather than do a full mode set. In the
-	 * fastboot case, we'll flip, but if we don't update the pipesrc and
-	 * pfit state, we'll end up with a big fb scanned out into the wrong
-	 * sized surface.
-	 *
-	 * To fix this properly, we need to hoist the checks up into
-	 * compute_mode_changes (or above), check the actual pfit state and
-	 * whether the platform allows pfit disable with pipe active, and only
-	 * then update the pipesrc and pfit state, even on the flip path.
-	 */
-	if (i915.fastboot) {
-		const struct drm_display_mode *adjusted_mode =
-			&intel_crtc->config.adjusted_mode;
-
-		I915_WRITE(PIPESRC(intel_crtc->pipe),
-			   ((adjusted_mode->crtc_hdisplay - 1) << 16) |
-			   (adjusted_mode->crtc_vdisplay - 1));
-		if (!intel_crtc->config.pch_pfit.enabled &&
-		    (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS) ||
-		     intel_pipe_has_type(crtc, INTEL_OUTPUT_EDP))) {
-			I915_WRITE(PF_CTL(intel_crtc->pipe), 0);
-			I915_WRITE(PF_WIN_POS(intel_crtc->pipe), 0);
-			I915_WRITE(PF_WIN_SZ(intel_crtc->pipe), 0);
-		}
-		intel_crtc->config.pipe_src_w = adjusted_mode->crtc_hdisplay;
-		intel_crtc->config.pipe_src_h = adjusted_mode->crtc_vdisplay;
-	}
-
 	dev_priv->display.update_primary_plane(crtc, fb, x, y);
 
 	if (intel_crtc->active)
@@ -3472,14 +3618,13 @@ void intel_crtc_wait_for_pending_flips(struct drm_crtc *crtc)
 				       !intel_crtc_has_pending_flip(crtc),
 				       60*HZ) == 0)) {
 		struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-		unsigned long flags;
 
-		spin_lock_irqsave(&dev->event_lock, flags);
+		spin_lock_irq(&dev->event_lock);
 		if (intel_crtc->unpin_work) {
 			WARN_ONCE(1, "Removing stuck page flip\n");
 			page_flip_completed(intel_crtc);
 		}
-		spin_unlock_irqrestore(&dev->event_lock, flags);
+		spin_unlock_irq(&dev->event_lock);
 	}
 
 	if (crtc->primary->fb) {
@@ -3704,9 +3849,7 @@ static void ironlake_pch_enable(struct drm_crtc *crtc)
 	intel_fdi_normal_train(crtc);
 
 	/* For PCH DP, enable TRANS_DP_CTL */
-	if (HAS_PCH_CPT(dev) &&
-	    (intel_pipe_has_type(crtc, INTEL_OUTPUT_DISPLAYPORT) ||
-	     intel_pipe_has_type(crtc, INTEL_OUTPUT_EDP))) {
+	if (HAS_PCH_CPT(dev) && intel_crtc->config.has_dp_encoder) {
 		u32 bpc = (I915_READ(PIPECONF(pipe)) & PIPECONF_BPC_MASK) >> 5;
 		reg = TRANS_DP_CTL(pipe);
 		temp = I915_READ(reg);
@@ -3766,12 +3909,13 @@ void intel_put_shared_dpll(struct intel_crtc *crtc)
 	if (pll == NULL)
 		return;
 
-	if (pll->refcount == 0) {
-		WARN(1, "bad %s refcount\n", pll->name);
+	if (!(pll->config.crtc_mask & (1 << crtc->pipe))) {
+		WARN(1, "bad %s crtc mask\n", pll->name);
 		return;
 	}
 
-	if (--pll->refcount == 0) {
+	pll->config.crtc_mask &= ~(1 << crtc->pipe);
+	if (pll->config.crtc_mask == 0) {
 		WARN_ON(pll->on);
 		WARN_ON(pll->active);
 	}
@@ -3782,15 +3926,9 @@ void intel_put_shared_dpll(struct intel_crtc *crtc)
 struct intel_shared_dpll *intel_get_shared_dpll(struct intel_crtc *crtc)
 {
 	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
-	struct intel_shared_dpll *pll = intel_crtc_to_shared_dpll(crtc);
+	struct intel_shared_dpll *pll;
 	enum intel_dpll_id i;
 
-	if (pll) {
-		DRM_DEBUG_KMS("CRTC:%d dropping existing %s\n",
-			      crtc->base.base.id, pll->name);
-		intel_put_shared_dpll(crtc);
-	}
-
 	if (HAS_PCH_IBX(dev_priv->dev)) {
 		/* Ironlake PCH has a fixed PLL->PCH pipe mapping. */
 		i = (enum intel_dpll_id) crtc->pipe;
@@ -3799,7 +3937,7 @@ struct intel_shared_dpll *intel_get_shared_dpll(struct intel_crtc *crtc)
 		DRM_DEBUG_KMS("CRTC:%d using pre-allocated %s\n",
 			      crtc->base.base.id, pll->name);
 
-		WARN_ON(pll->refcount);
+		WARN_ON(pll->new_config->crtc_mask);
 
 		goto found;
 	}
@@ -3808,15 +3946,16 @@ struct intel_shared_dpll *intel_get_shared_dpll(struct intel_crtc *crtc)
 		pll = &dev_priv->shared_dplls[i];
 
 		/* Only want to check enabled timings first */
-		if (pll->refcount == 0)
+		if (pll->new_config->crtc_mask == 0)
 			continue;
 
-		if (memcmp(&crtc->config.dpll_hw_state, &pll->hw_state,
-			   sizeof(pll->hw_state)) == 0) {
-			DRM_DEBUG_KMS("CRTC:%d sharing existing %s (refcount %d, ative %d)\n",
-				      crtc->base.base.id,
-				      pll->name, pll->refcount, pll->active);
-
+		if (memcmp(&crtc->new_config->dpll_hw_state,
+			   &pll->new_config->hw_state,
+			   sizeof(pll->new_config->hw_state)) == 0) {
+			DRM_DEBUG_KMS("CRTC:%d sharing existing %s (crtc mask 0x%08x, ative %d)\n",
+				      crtc->base.base.id, pll->name,
+				      pll->new_config->crtc_mask,
+				      pll->active);
 			goto found;
 		}
 	}
@@ -3824,7 +3963,7 @@ struct intel_shared_dpll *intel_get_shared_dpll(struct intel_crtc *crtc)
 	/* Ok no matching timings, maybe there's a free one? */
 	for (i = 0; i < dev_priv->num_shared_dpll; i++) {
 		pll = &dev_priv->shared_dplls[i];
-		if (pll->refcount == 0) {
+		if (pll->new_config->crtc_mask == 0) {
 			DRM_DEBUG_KMS("CRTC:%d allocated %s\n",
 				      crtc->base.base.id, pll->name);
 			goto found;
@@ -3834,18 +3973,86 @@ struct intel_shared_dpll *intel_get_shared_dpll(struct intel_crtc *crtc)
 	return NULL;
 
 found:
-	if (pll->refcount == 0)
-		pll->hw_state = crtc->config.dpll_hw_state;
+	if (pll->new_config->crtc_mask == 0)
+		pll->new_config->hw_state = crtc->new_config->dpll_hw_state;
 
-	crtc->config.shared_dpll = i;
+	crtc->new_config->shared_dpll = i;
 	DRM_DEBUG_DRIVER("using %s for pipe %c\n", pll->name,
 			 pipe_name(crtc->pipe));
 
-	pll->refcount++;
+	pll->new_config->crtc_mask |= 1 << crtc->pipe;
 
 	return pll;
 }
 
+/**
+ * intel_shared_dpll_start_config - start a new PLL staged config
+ * @dev_priv: DRM device
+ * @clear_pipes: mask of pipes that will have their PLLs freed
+ *
+ * Starts a new PLL staged config, copying the current config but
+ * releasing the references of pipes specified in clear_pipes.
+ */
+static int intel_shared_dpll_start_config(struct drm_i915_private *dev_priv,
+					  unsigned clear_pipes)
+{
+	struct intel_shared_dpll *pll;
+	enum intel_dpll_id i;
+
+	for (i = 0; i < dev_priv->num_shared_dpll; i++) {
+		pll = &dev_priv->shared_dplls[i];
+
+		pll->new_config = kmemdup(&pll->config, sizeof pll->config,
+					  GFP_KERNEL);
+		if (!pll->new_config)
+			goto cleanup;
+
+		pll->new_config->crtc_mask &= ~clear_pipes;
+	}
+
+	return 0;
+
+cleanup:
+	while (--i >= 0) {
+		pll = &dev_priv->shared_dplls[i];
+		kfree(pll->new_config);
+		pll->new_config = NULL;
+	}
+
+	return -ENOMEM;
+}
+
+static void intel_shared_dpll_commit(struct drm_i915_private *dev_priv)
+{
+	struct intel_shared_dpll *pll;
+	enum intel_dpll_id i;
+
+	for (i = 0; i < dev_priv->num_shared_dpll; i++) {
+		pll = &dev_priv->shared_dplls[i];
+
+		WARN_ON(pll->new_config == &pll->config);
+
+		pll->config = *pll->new_config;
+		kfree(pll->new_config);
+		pll->new_config = NULL;
+	}
+}
+
+static void intel_shared_dpll_abort_config(struct drm_i915_private *dev_priv)
+{
+	struct intel_shared_dpll *pll;
+	enum intel_dpll_id i;
+
+	for (i = 0; i < dev_priv->num_shared_dpll; i++) {
+		pll = &dev_priv->shared_dplls[i];
+
+		WARN_ON(pll->new_config == &pll->config);
+
+		kfree(pll->new_config);
+		pll->new_config = NULL;
+	}
+}
+
 static void cpt_verify_modeset(struct drm_device *dev, int pipe)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -3860,6 +4067,19 @@ static void cpt_verify_modeset(struct drm_device *dev, int pipe)
 	}
 }
 
+static void skylake_pfit_enable(struct intel_crtc *crtc)
+{
+	struct drm_device *dev = crtc->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int pipe = crtc->pipe;
+
+	if (crtc->config.pch_pfit.enabled) {
+		I915_WRITE(PS_CTL(pipe), PS_ENABLE);
+		I915_WRITE(PS_WIN_POS(pipe), crtc->config.pch_pfit.pos);
+		I915_WRITE(PS_WIN_SZ(pipe), crtc->config.pch_pfit.size);
+	}
+}
+
 static void ironlake_pfit_enable(struct intel_crtc *crtc)
 {
 	struct drm_device *dev = crtc->base.dev;
@@ -3983,7 +4203,7 @@ static void intel_crtc_load_lut(struct drm_crtc *crtc)
 		return;
 
 	if (!HAS_PCH_SPLIT(dev_priv->dev)) {
-		if (intel_pipe_has_type(crtc, INTEL_OUTPUT_DSI))
+		if (intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_DSI))
 			assert_dsi_pll_enabled(dev_priv);
 		else
 			assert_pll_enabled(dev_priv, pipe);
@@ -4038,10 +4258,6 @@ static void intel_crtc_enable_planes(struct drm_crtc *crtc)
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int pipe = intel_crtc->pipe;
 
-	assert_vblank_disabled(crtc);
-
-	drm_vblank_on(dev, pipe);
-
 	intel_enable_primary_hw_plane(crtc->primary, crtc);
 	intel_enable_planes(crtc);
 	intel_crtc_update_cursor(crtc, true);
@@ -4087,10 +4303,6 @@ static void intel_crtc_disable_planes(struct drm_crtc *crtc)
 	 * consider this a flip to a NULL plane.
 	 */
 	intel_frontbuffer_flip(dev, INTEL_FRONTBUFFER_ALL_MASK(pipe));
-
-	drm_vblank_off(dev, pipe);
-
-	assert_vblank_disabled(crtc);
 }
 
 static void ironlake_crtc_enable(struct drm_crtc *crtc)
@@ -4123,8 +4335,8 @@ static void ironlake_crtc_enable(struct drm_crtc *crtc)
 
 	intel_crtc->active = true;
 
-	intel_set_cpu_fifo_underrun_reporting(dev, pipe, true);
-	intel_set_pch_fifo_underrun_reporting(dev, pipe, true);
+	intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, true);
+	intel_set_pch_fifo_underrun_reporting(dev_priv, pipe, true);
 
 	for_each_encoder_on_crtc(dev, crtc, encoder)
 		if (encoder->pre_enable)
@@ -4160,6 +4372,9 @@ static void ironlake_crtc_enable(struct drm_crtc *crtc)
 	if (HAS_PCH_CPT(dev))
 		cpt_verify_modeset(dev, intel_crtc->pipe);
 
+	assert_vblank_disabled(crtc);
+	drm_crtc_vblank_on(crtc);
+
 	intel_crtc_enable_planes(crtc);
 }
 
@@ -4235,19 +4450,23 @@ static void haswell_crtc_enable(struct drm_crtc *crtc)
 
 	intel_crtc->active = true;
 
-	intel_set_cpu_fifo_underrun_reporting(dev, pipe, true);
+	intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, true);
 	for_each_encoder_on_crtc(dev, crtc, encoder)
 		if (encoder->pre_enable)
 			encoder->pre_enable(encoder);
 
 	if (intel_crtc->config.has_pch_encoder) {
-		intel_set_pch_fifo_underrun_reporting(dev, TRANSCODER_A, true);
+		intel_set_pch_fifo_underrun_reporting(dev_priv, TRANSCODER_A,
+						      true);
 		dev_priv->display.fdi_link_train(crtc);
 	}
 
 	intel_ddi_enable_pipe_clock(intel_crtc);
 
-	ironlake_pfit_enable(intel_crtc);
+	if (IS_SKYLAKE(dev))
+		skylake_pfit_enable(intel_crtc);
+	else
+		ironlake_pfit_enable(intel_crtc);
 
 	/*
 	 * On ILK+ LUT must be loaded before the pipe is running but with
@@ -4272,12 +4491,30 @@ static void haswell_crtc_enable(struct drm_crtc *crtc)
 		intel_opregion_notify_encoder(encoder, true);
 	}
 
+	assert_vblank_disabled(crtc);
+	drm_crtc_vblank_on(crtc);
+
 	/* If we change the relative order between pipe/planes enabling, we need
 	 * to change the workaround. */
 	haswell_mode_set_planes_workaround(intel_crtc);
 	intel_crtc_enable_planes(crtc);
 }
 
+static void skylake_pfit_disable(struct intel_crtc *crtc)
+{
+	struct drm_device *dev = crtc->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int pipe = crtc->pipe;
+
+	/* To avoid upsetting the power well on haswell only disable the pfit if
+	 * it's in use. The hw state code will make sure we get this right. */
+	if (crtc->config.pch_pfit.enabled) {
+		I915_WRITE(PS_CTL(pipe), 0);
+		I915_WRITE(PS_WIN_POS(pipe), 0);
+		I915_WRITE(PS_WIN_SZ(pipe), 0);
+	}
+}
+
 static void ironlake_pfit_disable(struct intel_crtc *crtc)
 {
 	struct drm_device *dev = crtc->base.dev;
@@ -4307,11 +4544,14 @@ static void ironlake_crtc_disable(struct drm_crtc *crtc)
 
 	intel_crtc_disable_planes(crtc);
 
+	drm_crtc_vblank_off(crtc);
+	assert_vblank_disabled(crtc);
+
 	for_each_encoder_on_crtc(dev, crtc, encoder)
 		encoder->disable(encoder);
 
 	if (intel_crtc->config.has_pch_encoder)
-		intel_set_pch_fifo_underrun_reporting(dev, pipe, false);
+		intel_set_pch_fifo_underrun_reporting(dev_priv, pipe, false);
 
 	intel_disable_pipe(intel_crtc);
 
@@ -4368,13 +4608,17 @@ static void haswell_crtc_disable(struct drm_crtc *crtc)
 
 	intel_crtc_disable_planes(crtc);
 
+	drm_crtc_vblank_off(crtc);
+	assert_vblank_disabled(crtc);
+
 	for_each_encoder_on_crtc(dev, crtc, encoder) {
 		intel_opregion_notify_encoder(encoder, false);
 		encoder->disable(encoder);
 	}
 
 	if (intel_crtc->config.has_pch_encoder)
-		intel_set_pch_fifo_underrun_reporting(dev, TRANSCODER_A, false);
+		intel_set_pch_fifo_underrun_reporting(dev_priv, TRANSCODER_A,
+						      false);
 	intel_disable_pipe(intel_crtc);
 
 	if (intel_crtc->config.dp_encoder_is_mst)
@@ -4382,7 +4626,10 @@ static void haswell_crtc_disable(struct drm_crtc *crtc)
 
 	intel_ddi_disable_transcoder_func(dev_priv, cpu_transcoder);
 
-	ironlake_pfit_disable(intel_crtc);
+	if (IS_SKYLAKE(dev))
+		skylake_pfit_disable(intel_crtc);
+	else
+		ironlake_pfit_disable(intel_crtc);
 
 	intel_ddi_disable_pipe_clock(intel_crtc);
 
@@ -4508,20 +4755,6 @@ static unsigned long get_crtc_power_domains(struct drm_crtc *crtc)
 	return mask;
 }
 
-void intel_display_set_init_power(struct drm_i915_private *dev_priv,
-				  bool enable)
-{
-	if (dev_priv->power_domains.init_power_on == enable)
-		return;
-
-	if (enable)
-		intel_display_power_get(dev_priv, POWER_DOMAIN_INIT);
-	else
-		intel_display_power_put(dev_priv, POWER_DOMAIN_INIT);
-
-	dev_priv->power_domains.init_power_on = enable;
-}
-
 static void modeset_update_crtc_power_domains(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -4544,6 +4777,9 @@ static void modeset_update_crtc_power_domains(struct drm_device *dev)
 			intel_display_power_get(dev_priv, domain);
 	}
 
+	if (dev_priv->display.modeset_global_resources)
+		dev_priv->display.modeset_global_resources(dev);
+
 	for_each_intel_crtc(dev, crtc) {
 		enum intel_display_power_domain domain;
 
@@ -4575,7 +4811,7 @@ static void vlv_update_cdclk(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
 	dev_priv->vlv_cdclk_freq = dev_priv->display.get_display_clock_speed(dev);
-	DRM_DEBUG_DRIVER("Current CD clock rate: %d kHz",
+	DRM_DEBUG_DRIVER("Current CD clock rate: %d kHz\n",
 			 dev_priv->vlv_cdclk_freq);
 
 	/*
@@ -4614,10 +4850,9 @@ static void valleyview_set_cdclk(struct drm_device *dev, int cdclk)
 	mutex_unlock(&dev_priv->rps.hw_lock);
 
 	if (cdclk == 400000) {
-		u32 divider, vco;
+		u32 divider;
 
-		vco = valleyview_get_vco(dev_priv);
-		divider = DIV_ROUND_CLOSEST(vco << 1, cdclk) - 1;
+		divider = DIV_ROUND_CLOSEST(dev_priv->hpll_freq << 1, cdclk) - 1;
 
 		mutex_lock(&dev_priv->dpio_lock);
 		/* adjust cdclk divider */
@@ -4696,8 +4931,7 @@ static void cherryview_set_cdclk(struct drm_device *dev, int cdclk)
 static int valleyview_calc_cdclk(struct drm_i915_private *dev_priv,
 				 int max_pixclk)
 {
-	int vco = valleyview_get_vco(dev_priv);
-	int freq_320 = (vco <<  1) % 320000 != 0 ? 333333 : 320000;
+	int freq_320 = (dev_priv->hpll_freq <<  1) % 320000 != 0 ? 333333 : 320000;
 
 	/* FIXME: Punit isn't quite ready yet */
 	if (IS_CHERRYVIEW(dev_priv->dev))
@@ -4766,18 +5000,30 @@ static void valleyview_modeset_global_resources(struct drm_device *dev)
 	int req_cdclk = valleyview_calc_cdclk(dev_priv, max_pixclk);
 
 	if (req_cdclk != dev_priv->vlv_cdclk_freq) {
+		/*
+		 * FIXME: We can end up here with all power domains off, yet
+		 * with a CDCLK frequency other than the minimum. To account
+		 * for this take the PIPE-A power domain, which covers the HW
+		 * blocks needed for the following programming. This can be
+		 * removed once it's guaranteed that we get here either with
+		 * the minimum CDCLK set, or the required power domains
+		 * enabled.
+		 */
+		intel_display_power_get(dev_priv, POWER_DOMAIN_PIPE_A);
+
 		if (IS_CHERRYVIEW(dev))
 			cherryview_set_cdclk(dev, req_cdclk);
 		else
 			valleyview_set_cdclk(dev, req_cdclk);
-	}
 
-	modeset_update_crtc_power_domains(dev);
+		intel_display_power_put(dev_priv, POWER_DOMAIN_PIPE_A);
+	}
 }
 
 static void valleyview_crtc_enable(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct intel_encoder *encoder;
 	int pipe = intel_crtc->pipe;
@@ -4788,13 +5034,13 @@ static void valleyview_crtc_enable(struct drm_crtc *crtc)
 	if (intel_crtc->active)
 		return;
 
-	is_dsi = intel_pipe_has_type(crtc, INTEL_OUTPUT_DSI);
+	is_dsi = intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_DSI);
 
 	if (!is_dsi) {
 		if (IS_CHERRYVIEW(dev))
-			chv_prepare_pll(intel_crtc);
+			chv_prepare_pll(intel_crtc, &intel_crtc->config);
 		else
-			vlv_prepare_pll(intel_crtc);
+			vlv_prepare_pll(intel_crtc, &intel_crtc->config);
 	}
 
 	if (intel_crtc->config.has_dp_encoder)
@@ -4802,11 +5048,18 @@ static void valleyview_crtc_enable(struct drm_crtc *crtc)
 
 	intel_set_pipe_timings(intel_crtc);
 
+	if (IS_CHERRYVIEW(dev) && pipe == PIPE_B) {
+		struct drm_i915_private *dev_priv = dev->dev_private;
+
+		I915_WRITE(CHV_BLEND(pipe), CHV_BLEND_LEGACY);
+		I915_WRITE(CHV_CANVAS(pipe), 0);
+	}
+
 	i9xx_set_pipeconf(intel_crtc);
 
 	intel_crtc->active = true;
 
-	intel_set_cpu_fifo_underrun_reporting(dev, pipe, true);
+	intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, true);
 
 	for_each_encoder_on_crtc(dev, crtc, encoder)
 		if (encoder->pre_pll_enable)
@@ -4814,9 +5067,9 @@ static void valleyview_crtc_enable(struct drm_crtc *crtc)
 
 	if (!is_dsi) {
 		if (IS_CHERRYVIEW(dev))
-			chv_enable_pll(intel_crtc);
+			chv_enable_pll(intel_crtc, &intel_crtc->config);
 		else
-			vlv_enable_pll(intel_crtc);
+			vlv_enable_pll(intel_crtc, &intel_crtc->config);
 	}
 
 	for_each_encoder_on_crtc(dev, crtc, encoder)
@@ -4833,10 +5086,13 @@ static void valleyview_crtc_enable(struct drm_crtc *crtc)
 	for_each_encoder_on_crtc(dev, crtc, encoder)
 		encoder->enable(encoder);
 
+	assert_vblank_disabled(crtc);
+	drm_crtc_vblank_on(crtc);
+
 	intel_crtc_enable_planes(crtc);
 
 	/* Underruns don't raise interrupts, so check manually. */
-	i9xx_check_fifo_underruns(dev);
+	i9xx_check_fifo_underruns(dev_priv);
 }
 
 static void i9xx_set_pll_dividers(struct intel_crtc *crtc)
@@ -4851,6 +5107,7 @@ static void i9xx_set_pll_dividers(struct intel_crtc *crtc)
 static void i9xx_crtc_enable(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct intel_encoder *encoder;
 	int pipe = intel_crtc->pipe;
@@ -4872,7 +5129,7 @@ static void i9xx_crtc_enable(struct drm_crtc *crtc)
 	intel_crtc->active = true;
 
 	if (!IS_GEN2(dev))
-		intel_set_cpu_fifo_underrun_reporting(dev, pipe, true);
+		intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, true);
 
 	for_each_encoder_on_crtc(dev, crtc, encoder)
 		if (encoder->pre_enable)
@@ -4890,6 +5147,9 @@ static void i9xx_crtc_enable(struct drm_crtc *crtc)
 	for_each_encoder_on_crtc(dev, crtc, encoder)
 		encoder->enable(encoder);
 
+	assert_vblank_disabled(crtc);
+	drm_crtc_vblank_on(crtc);
+
 	intel_crtc_enable_planes(crtc);
 
 	/*
@@ -4900,10 +5160,10 @@ static void i9xx_crtc_enable(struct drm_crtc *crtc)
 	 * but leave the pipe running.
 	 */
 	if (IS_GEN2(dev))
-		intel_set_cpu_fifo_underrun_reporting(dev, pipe, true);
+		intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, true);
 
 	/* Underruns don't raise interrupts, so check manually. */
-	i9xx_check_fifo_underruns(dev);
+	i9xx_check_fifo_underruns(dev_priv);
 }
 
 static void i9xx_pfit_disable(struct intel_crtc *crtc)
@@ -4939,7 +5199,7 @@ static void i9xx_crtc_disable(struct drm_crtc *crtc)
 	 * but leave the pipe running.
 	 */
 	if (IS_GEN2(dev))
-		intel_set_cpu_fifo_underrun_reporting(dev, pipe, false);
+		intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, false);
 
 	/*
 	 * Vblank time updates from the shadow to live plane control register
@@ -4953,9 +5213,6 @@ static void i9xx_crtc_disable(struct drm_crtc *crtc)
 	intel_set_memory_cxsr(dev_priv, false);
 	intel_crtc_disable_planes(crtc);
 
-	for_each_encoder_on_crtc(dev, crtc, encoder)
-		encoder->disable(encoder);
-
 	/*
 	 * On gen2 planes are double buffered but the pipe isn't, so we must
 	 * wait for planes to fully turn off before disabling the pipe.
@@ -4964,6 +5221,12 @@ static void i9xx_crtc_disable(struct drm_crtc *crtc)
 	 */
 	intel_wait_for_vblank(dev, pipe);
 
+	drm_crtc_vblank_off(crtc);
+	assert_vblank_disabled(crtc);
+
+	for_each_encoder_on_crtc(dev, crtc, encoder)
+		encoder->disable(encoder);
+
 	intel_disable_pipe(intel_crtc);
 
 	i9xx_pfit_disable(intel_crtc);
@@ -4972,7 +5235,7 @@ static void i9xx_crtc_disable(struct drm_crtc *crtc)
 		if (encoder->post_disable)
 			encoder->post_disable(encoder);
 
-	if (!intel_pipe_has_type(crtc, INTEL_OUTPUT_DSI)) {
+	if (!intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_DSI)) {
 		if (IS_CHERRYVIEW(dev))
 			chv_disable_pll(dev_priv, pipe);
 		else if (IS_VALLEYVIEW(dev))
@@ -4982,7 +5245,7 @@ static void i9xx_crtc_disable(struct drm_crtc *crtc)
 	}
 
 	if (!IS_GEN2(dev))
-		intel_set_cpu_fifo_underrun_reporting(dev, pipe, false);
+		intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, false);
 
 	intel_crtc->active = false;
 	intel_update_watermarks(crtc);
@@ -4996,36 +5259,6 @@ static void i9xx_crtc_off(struct drm_crtc *crtc)
 {
 }
 
-static void intel_crtc_update_sarea(struct drm_crtc *crtc,
-				    bool enabled)
-{
-	struct drm_device *dev = crtc->dev;
-	struct drm_i915_master_private *master_priv;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	int pipe = intel_crtc->pipe;
-
-	if (!dev->primary->master)
-		return;
-
-	master_priv = dev->primary->master->driver_priv;
-	if (!master_priv->sarea_priv)
-		return;
-
-	switch (pipe) {
-	case 0:
-		master_priv->sarea_priv->pipeA_w = enabled ? crtc->mode.hdisplay : 0;
-		master_priv->sarea_priv->pipeA_h = enabled ? crtc->mode.vdisplay : 0;
-		break;
-	case 1:
-		master_priv->sarea_priv->pipeB_w = enabled ? crtc->mode.hdisplay : 0;
-		master_priv->sarea_priv->pipeB_h = enabled ? crtc->mode.vdisplay : 0;
-		break;
-	default:
-		DRM_ERROR("Can't update pipe %c in SAREA\n", pipe_name(pipe));
-		break;
-	}
-}
-
 /* Master function to enable/disable CRTC and corresponding power wells */
 void intel_crtc_control(struct drm_crtc *crtc, bool enable)
 {
@@ -5069,8 +5302,6 @@ void intel_crtc_update_dpms(struct drm_crtc *crtc)
 		enable |= intel_encoder->connectors_active;
 
 	intel_crtc_control(crtc, enable);
-
-	intel_crtc_update_sarea(crtc, enable);
 }
 
 static void intel_crtc_disable(struct drm_crtc *crtc)
@@ -5085,7 +5316,6 @@ static void intel_crtc_disable(struct drm_crtc *crtc)
 	WARN_ON(!crtc->enabled);
 
 	dev_priv->display.crtc_disable(crtc);
-	intel_crtc_update_sarea(crtc, false);
 	dev_priv->display.off(crtc);
 
 	if (crtc->primary->fb) {
@@ -5324,11 +5554,11 @@ static int intel_crtc_compute_config(struct intel_crtc *crtc,
 				     struct intel_crtc_config *pipe_config)
 {
 	struct drm_device *dev = crtc->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_display_mode *adjusted_mode = &pipe_config->adjusted_mode;
 
 	/* FIXME should check pixel clock limits on all platforms */
 	if (INTEL_INFO(dev)->gen < 4) {
-		struct drm_i915_private *dev_priv = dev->dev_private;
 		int clock_limit =
 			dev_priv->display.get_display_clock_speed(dev);
 
@@ -5355,7 +5585,7 @@ static int intel_crtc_compute_config(struct intel_crtc *crtc,
 	 * - LVDS dual channel mode
 	 * - Double wide pipe
 	 */
-	if ((intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_LVDS) &&
+	if ((intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS) &&
 	     intel_is_dual_link_lvds(dev)) || pipe_config->double_wide)
 		pipe_config->pipe_src_w &= ~1;
 
@@ -5377,13 +5607,6 @@ static int intel_crtc_compute_config(struct intel_crtc *crtc,
 	if (HAS_IPS(dev))
 		hsw_compute_ips_config(crtc, pipe_config);
 
-	/*
-	 * XXX: PCH/WRPLL clock sharing is done in ->mode_set, so make sure the
-	 * old clock survives for now.
-	 */
-	if (HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev) || HAS_DDI(dev))
-		pipe_config->shared_dpll = crtc->config.shared_dpll;
-
 	if (pipe_config->has_pch_encoder)
 		return ironlake_fdi_compute_config(crtc, pipe_config);
 
@@ -5393,7 +5616,6 @@ static int intel_crtc_compute_config(struct intel_crtc *crtc,
 static int valleyview_get_display_clock_speed(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	int vco = valleyview_get_vco(dev_priv);
 	u32 val;
 	int divider;
 
@@ -5401,6 +5623,9 @@ static int valleyview_get_display_clock_speed(struct drm_device *dev)
 	if (IS_CHERRYVIEW(dev))
 		return 400000;
 
+	if (dev_priv->hpll_freq == 0)
+		dev_priv->hpll_freq = valleyview_get_vco(dev_priv);
+
 	mutex_lock(&dev_priv->dpio_lock);
 	val = vlv_cck_read(dev_priv, CCK_DISPLAY_CLOCK_CONTROL);
 	mutex_unlock(&dev_priv->dpio_lock);
@@ -5411,7 +5636,7 @@ static int valleyview_get_display_clock_speed(struct drm_device *dev)
 	     (divider << DISPLAY_FREQUENCY_STATUS_SHIFT),
 	     "cdclk change in progress\n");
 
-	return DIV_ROUND_CLOSEST(vco << 1, divider + 1);
+	return DIV_ROUND_CLOSEST(dev_priv->hpll_freq << 1, divider + 1);
 }
 
 static int i945_get_display_clock_speed(struct drm_device *dev)
@@ -5543,15 +5768,15 @@ static inline bool intel_panel_use_ssc(struct drm_i915_private *dev_priv)
 		&& !(dev_priv->quirks & QUIRK_LVDS_SSC_DISABLE);
 }
 
-static int i9xx_get_refclk(struct drm_crtc *crtc, int num_connectors)
+static int i9xx_get_refclk(struct intel_crtc *crtc, int num_connectors)
 {
-	struct drm_device *dev = crtc->dev;
+	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int refclk;
 
 	if (IS_VALLEYVIEW(dev)) {
 		refclk = 100000;
-	} else if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS) &&
+	} else if (intel_pipe_will_have_type(crtc, INTEL_OUTPUT_LVDS) &&
 	    intel_panel_use_ssc(dev_priv) && num_connectors < 2) {
 		refclk = dev_priv->vbt.lvds_ssc_freq;
 		DRM_DEBUG_KMS("using SSC reference clock of %d kHz\n", refclk);
@@ -5581,24 +5806,24 @@ static void i9xx_update_pll_dividers(struct intel_crtc *crtc,
 	u32 fp, fp2 = 0;
 
 	if (IS_PINEVIEW(dev)) {
-		fp = pnv_dpll_compute_fp(&crtc->config.dpll);
+		fp = pnv_dpll_compute_fp(&crtc->new_config->dpll);
 		if (reduced_clock)
 			fp2 = pnv_dpll_compute_fp(reduced_clock);
 	} else {
-		fp = i9xx_dpll_compute_fp(&crtc->config.dpll);
+		fp = i9xx_dpll_compute_fp(&crtc->new_config->dpll);
 		if (reduced_clock)
 			fp2 = i9xx_dpll_compute_fp(reduced_clock);
 	}
 
-	crtc->config.dpll_hw_state.fp0 = fp;
+	crtc->new_config->dpll_hw_state.fp0 = fp;
 
 	crtc->lowfreq_avail = false;
-	if (intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_LVDS) &&
+	if (intel_pipe_will_have_type(crtc, INTEL_OUTPUT_LVDS) &&
 	    reduced_clock && i915.powersave) {
-		crtc->config.dpll_hw_state.fp1 = fp2;
+		crtc->new_config->dpll_hw_state.fp1 = fp2;
 		crtc->lowfreq_avail = true;
 	} else {
-		crtc->config.dpll_hw_state.fp1 = fp;
+		crtc->new_config->dpll_hw_state.fp1 = fp;
 	}
 }
 
@@ -5687,7 +5912,8 @@ void intel_dp_set_m_n(struct intel_crtc *crtc)
 						   &crtc->config.dp_m2_n2);
 }
 
-static void vlv_update_pll(struct intel_crtc *crtc)
+static void vlv_update_pll(struct intel_crtc *crtc,
+			   struct intel_crtc_config *pipe_config)
 {
 	u32 dpll, dpll_md;
 
@@ -5702,14 +5928,15 @@ static void vlv_update_pll(struct intel_crtc *crtc)
 	if (crtc->pipe == PIPE_B)
 		dpll |= DPLL_INTEGRATED_CRI_CLK_VLV;
 	dpll |= DPLL_VCO_ENABLE;
-	crtc->config.dpll_hw_state.dpll = dpll;
+	pipe_config->dpll_hw_state.dpll = dpll;
 
-	dpll_md = (crtc->config.pixel_multiplier - 1)
+	dpll_md = (pipe_config->pixel_multiplier - 1)
 		<< DPLL_MD_UDI_MULTIPLIER_SHIFT;
-	crtc->config.dpll_hw_state.dpll_md = dpll_md;
+	pipe_config->dpll_hw_state.dpll_md = dpll_md;
 }
 
-static void vlv_prepare_pll(struct intel_crtc *crtc)
+static void vlv_prepare_pll(struct intel_crtc *crtc,
+			    const struct intel_crtc_config *pipe_config)
 {
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -5720,11 +5947,11 @@ static void vlv_prepare_pll(struct intel_crtc *crtc)
 
 	mutex_lock(&dev_priv->dpio_lock);
 
-	bestn = crtc->config.dpll.n;
-	bestm1 = crtc->config.dpll.m1;
-	bestm2 = crtc->config.dpll.m2;
-	bestp1 = crtc->config.dpll.p1;
-	bestp2 = crtc->config.dpll.p2;
+	bestn = pipe_config->dpll.n;
+	bestm1 = pipe_config->dpll.m1;
+	bestm2 = pipe_config->dpll.m2;
+	bestp1 = pipe_config->dpll.p1;
+	bestp2 = pipe_config->dpll.p2;
 
 	/* See eDP HDMI DPIO driver vbios notes doc */
 
@@ -5761,17 +5988,16 @@ static void vlv_prepare_pll(struct intel_crtc *crtc)
 	vlv_dpio_write(dev_priv, pipe, VLV_PLL_DW3(pipe), mdiv);
 
 	/* Set HBR and RBR LPF coefficients */
-	if (crtc->config.port_clock == 162000 ||
-	    intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_ANALOG) ||
-	    intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_HDMI))
+	if (pipe_config->port_clock == 162000 ||
+	    intel_pipe_has_type(crtc, INTEL_OUTPUT_ANALOG) ||
+	    intel_pipe_has_type(crtc, INTEL_OUTPUT_HDMI))
 		vlv_dpio_write(dev_priv, pipe, VLV_PLL_DW10(pipe),
 				 0x009f0003);
 	else
 		vlv_dpio_write(dev_priv, pipe, VLV_PLL_DW10(pipe),
 				 0x00d0000f);
 
-	if (intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_EDP) ||
-	    intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_DISPLAYPORT)) {
+	if (crtc->config.has_dp_encoder) {
 		/* Use SSC source */
 		if (pipe == PIPE_A)
 			vlv_dpio_write(dev_priv, pipe, VLV_PLL_DW5(pipe),
@@ -5791,8 +6017,8 @@ static void vlv_prepare_pll(struct intel_crtc *crtc)
 
 	coreclk = vlv_dpio_read(dev_priv, pipe, VLV_PLL_DW7(pipe));
 	coreclk = (coreclk & 0x0000ff00) | 0x01c00000;
-	if (intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_DISPLAYPORT) ||
-	    intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_EDP))
+	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_DISPLAYPORT) ||
+	    intel_pipe_has_type(crtc, INTEL_OUTPUT_EDP))
 		coreclk |= 0x01000000;
 	vlv_dpio_write(dev_priv, pipe, VLV_PLL_DW7(pipe), coreclk);
 
@@ -5800,19 +6026,21 @@ static void vlv_prepare_pll(struct intel_crtc *crtc)
 	mutex_unlock(&dev_priv->dpio_lock);
 }
 
-static void chv_update_pll(struct intel_crtc *crtc)
+static void chv_update_pll(struct intel_crtc *crtc,
+			   struct intel_crtc_config *pipe_config)
 {
-	crtc->config.dpll_hw_state.dpll = DPLL_SSC_REF_CLOCK_CHV |
+	pipe_config->dpll_hw_state.dpll = DPLL_SSC_REF_CLOCK_CHV |
 		DPLL_REFA_CLK_ENABLE_VLV | DPLL_VGA_MODE_DIS |
 		DPLL_VCO_ENABLE;
 	if (crtc->pipe != PIPE_A)
-		crtc->config.dpll_hw_state.dpll |= DPLL_INTEGRATED_CRI_CLK_VLV;
+		pipe_config->dpll_hw_state.dpll |= DPLL_INTEGRATED_CRI_CLK_VLV;
 
-	crtc->config.dpll_hw_state.dpll_md =
-		(crtc->config.pixel_multiplier - 1) << DPLL_MD_UDI_MULTIPLIER_SHIFT;
+	pipe_config->dpll_hw_state.dpll_md =
+		(pipe_config->pixel_multiplier - 1) << DPLL_MD_UDI_MULTIPLIER_SHIFT;
 }
 
-static void chv_prepare_pll(struct intel_crtc *crtc)
+static void chv_prepare_pll(struct intel_crtc *crtc,
+			    const struct intel_crtc_config *pipe_config)
 {
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -5823,18 +6051,18 @@ static void chv_prepare_pll(struct intel_crtc *crtc)
 	u32 bestn, bestm1, bestm2, bestp1, bestp2, bestm2_frac;
 	int refclk;
 
-	bestn = crtc->config.dpll.n;
-	bestm2_frac = crtc->config.dpll.m2 & 0x3fffff;
-	bestm1 = crtc->config.dpll.m1;
-	bestm2 = crtc->config.dpll.m2 >> 22;
-	bestp1 = crtc->config.dpll.p1;
-	bestp2 = crtc->config.dpll.p2;
+	bestn = pipe_config->dpll.n;
+	bestm2_frac = pipe_config->dpll.m2 & 0x3fffff;
+	bestm1 = pipe_config->dpll.m1;
+	bestm2 = pipe_config->dpll.m2 >> 22;
+	bestp1 = pipe_config->dpll.p1;
+	bestp2 = pipe_config->dpll.p2;
 
 	/*
 	 * Enable Refclk and SSC
 	 */
 	I915_WRITE(dpll_reg,
-		   crtc->config.dpll_hw_state.dpll & ~DPLL_VCO_ENABLE);
+		   pipe_config->dpll_hw_state.dpll & ~DPLL_VCO_ENABLE);
 
 	mutex_lock(&dev_priv->dpio_lock);
 
@@ -5862,7 +6090,7 @@ static void chv_prepare_pll(struct intel_crtc *crtc)
 		       (2 << DPIO_CHV_FEEDFWD_GAIN_SHIFT));
 
 	/* Loop filter */
-	refclk = i9xx_get_refclk(&crtc->base, 0);
+	refclk = i9xx_get_refclk(crtc, 0);
 	loopfilter = 5 << DPIO_CHV_PROP_COEFF_SHIFT |
 		2 << DPIO_CHV_GAIN_CTRL_SHIFT;
 	if (refclk == 100000)
@@ -5882,6 +6110,53 @@ static void chv_prepare_pll(struct intel_crtc *crtc)
 	mutex_unlock(&dev_priv->dpio_lock);
 }
 
+/**
+ * vlv_force_pll_on - forcibly enable just the PLL
+ * @dev_priv: i915 private structure
+ * @pipe: pipe PLL to enable
+ * @dpll: PLL configuration
+ *
+ * Enable the PLL for @pipe using the supplied @dpll config. To be used
+ * in cases where we need the PLL enabled even when @pipe is not going to
+ * be enabled.
+ */
+void vlv_force_pll_on(struct drm_device *dev, enum pipe pipe,
+		      const struct dpll *dpll)
+{
+	struct intel_crtc *crtc =
+		to_intel_crtc(intel_get_crtc_for_pipe(dev, pipe));
+	struct intel_crtc_config pipe_config = {
+		.pixel_multiplier = 1,
+		.dpll = *dpll,
+	};
+
+	if (IS_CHERRYVIEW(dev)) {
+		chv_update_pll(crtc, &pipe_config);
+		chv_prepare_pll(crtc, &pipe_config);
+		chv_enable_pll(crtc, &pipe_config);
+	} else {
+		vlv_update_pll(crtc, &pipe_config);
+		vlv_prepare_pll(crtc, &pipe_config);
+		vlv_enable_pll(crtc, &pipe_config);
+	}
+}
+
+/**
+ * vlv_force_pll_off - forcibly disable just the PLL
+ * @dev_priv: i915 private structure
+ * @pipe: pipe PLL to disable
+ *
+ * Disable the PLL for @pipe. To be used in cases where we need
+ * the PLL enabled even when @pipe is not going to be enabled.
+ */
+void vlv_force_pll_off(struct drm_device *dev, enum pipe pipe)
+{
+	if (IS_CHERRYVIEW(dev))
+		chv_disable_pll(to_i915(dev), pipe);
+	else
+		vlv_disable_pll(to_i915(dev), pipe);
+}
+
 static void i9xx_update_pll(struct intel_crtc *crtc,
 			    intel_clock_t *reduced_clock,
 			    int num_connectors)
@@ -5890,29 +6165,29 @@ static void i9xx_update_pll(struct intel_crtc *crtc,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 dpll;
 	bool is_sdvo;
-	struct dpll *clock = &crtc->config.dpll;
+	struct dpll *clock = &crtc->new_config->dpll;
 
 	i9xx_update_pll_dividers(crtc, reduced_clock);
 
-	is_sdvo = intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_SDVO) ||
-		intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_HDMI);
+	is_sdvo = intel_pipe_will_have_type(crtc, INTEL_OUTPUT_SDVO) ||
+		intel_pipe_will_have_type(crtc, INTEL_OUTPUT_HDMI);
 
 	dpll = DPLL_VGA_MODE_DIS;
 
-	if (intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_LVDS))
+	if (intel_pipe_will_have_type(crtc, INTEL_OUTPUT_LVDS))
 		dpll |= DPLLB_MODE_LVDS;
 	else
 		dpll |= DPLLB_MODE_DAC_SERIAL;
 
 	if (IS_I945G(dev) || IS_I945GM(dev) || IS_G33(dev)) {
-		dpll |= (crtc->config.pixel_multiplier - 1)
+		dpll |= (crtc->new_config->pixel_multiplier - 1)
 			<< SDVO_MULTIPLIER_SHIFT_HIRES;
 	}
 
 	if (is_sdvo)
 		dpll |= DPLL_SDVO_HIGH_SPEED;
 
-	if (intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_DISPLAYPORT))
+	if (crtc->new_config->has_dp_encoder)
 		dpll |= DPLL_SDVO_HIGH_SPEED;
 
 	/* compute bitmask from p1 value */
@@ -5940,21 +6215,21 @@ static void i9xx_update_pll(struct intel_crtc *crtc,
 	if (INTEL_INFO(dev)->gen >= 4)
 		dpll |= (6 << PLL_LOAD_PULSE_PHASE_SHIFT);
 
-	if (crtc->config.sdvo_tv_clock)
+	if (crtc->new_config->sdvo_tv_clock)
 		dpll |= PLL_REF_INPUT_TVCLKINBC;
-	else if (intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_LVDS) &&
+	else if (intel_pipe_will_have_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;
-	crtc->config.dpll_hw_state.dpll = dpll;
+	crtc->new_config->dpll_hw_state.dpll = dpll;
 
 	if (INTEL_INFO(dev)->gen >= 4) {
-		u32 dpll_md = (crtc->config.pixel_multiplier - 1)
+		u32 dpll_md = (crtc->new_config->pixel_multiplier - 1)
 			<< DPLL_MD_UDI_MULTIPLIER_SHIFT;
-		crtc->config.dpll_hw_state.dpll_md = dpll_md;
+		crtc->new_config->dpll_hw_state.dpll_md = dpll_md;
 	}
 }
 
@@ -5965,13 +6240,13 @@ static void i8xx_update_pll(struct intel_crtc *crtc,
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 dpll;
-	struct dpll *clock = &crtc->config.dpll;
+	struct dpll *clock = &crtc->new_config->dpll;
 
 	i9xx_update_pll_dividers(crtc, reduced_clock);
 
 	dpll = DPLL_VGA_MODE_DIS;
 
-	if (intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_LVDS)) {
+	if (intel_pipe_will_have_type(crtc, INTEL_OUTPUT_LVDS)) {
 		dpll |= (1 << (clock->p1 - 1)) << DPLL_FPA01_P1_POST_DIV_SHIFT;
 	} else {
 		if (clock->p1 == 2)
@@ -5982,17 +6257,17 @@ static void i8xx_update_pll(struct intel_crtc *crtc,
 			dpll |= PLL_P2_DIVIDE_BY_4;
 	}
 
-	if (!IS_I830(dev) && intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_DVO))
+	if (!IS_I830(dev) && intel_pipe_will_have_type(crtc, INTEL_OUTPUT_DVO))
 		dpll |= DPLL_DVO_2X_MODE;
 
-	if (intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_LVDS) &&
+	if (intel_pipe_will_have_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;
-	crtc->config.dpll_hw_state.dpll = dpll;
+	crtc->new_config->dpll_hw_state.dpll = dpll;
 }
 
 static void intel_set_pipe_timings(struct intel_crtc *intel_crtc)
@@ -6016,7 +6291,7 @@ static void intel_set_pipe_timings(struct intel_crtc *intel_crtc)
 		crtc_vtotal -= 1;
 		crtc_vblank_end -= 1;
 
-		if (intel_pipe_has_type(&intel_crtc->base, INTEL_OUTPUT_SDVO))
+		if (intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_SDVO))
 			vsyncshift = (adjusted_mode->crtc_htotal - 1) / 2;
 		else
 			vsyncshift = adjusted_mode->crtc_hsync_start -
@@ -6174,7 +6449,7 @@ static void i9xx_set_pipeconf(struct intel_crtc *intel_crtc)
 
 	if (intel_crtc->config.adjusted_mode.flags & DRM_MODE_FLAG_INTERLACE) {
 		if (INTEL_INFO(dev)->gen < 4 ||
-		    intel_pipe_has_type(&intel_crtc->base, INTEL_OUTPUT_SDVO))
+		    intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_SDVO))
 			pipeconf |= PIPECONF_INTERLACE_W_FIELD_INDICATION;
 		else
 			pipeconf |= PIPECONF_INTERLACE_W_SYNC_SHIFT;
@@ -6188,13 +6463,10 @@ static void i9xx_set_pipeconf(struct intel_crtc *intel_crtc)
 	POSTING_READ(PIPECONF(intel_crtc->pipe));
 }
 
-static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
-			      int x, int y,
-			      struct drm_framebuffer *fb)
+static int i9xx_crtc_compute_clock(struct intel_crtc *crtc)
 {
-	struct drm_device *dev = crtc->dev;
+	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int refclk, num_connectors = 0;
 	intel_clock_t clock, reduced_clock;
 	bool ok, has_reduced_clock = false;
@@ -6202,7 +6474,10 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 	struct intel_encoder *encoder;
 	const intel_limit_t *limit;
 
-	for_each_encoder_on_crtc(dev, crtc, encoder) {
+	for_each_intel_encoder(dev, encoder) {
+		if (encoder->new_crtc != crtc)
+			continue;
+
 		switch (encoder->type) {
 		case INTEL_OUTPUT_LVDS:
 			is_lvds = true;
@@ -6210,6 +6485,8 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 		case INTEL_OUTPUT_DSI:
 			is_dsi = true;
 			break;
+		default:
+			break;
 		}
 
 		num_connectors++;
@@ -6218,7 +6495,7 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 	if (is_dsi)
 		return 0;
 
-	if (!intel_crtc->config.clock_set) {
+	if (!crtc->new_config->clock_set) {
 		refclk = i9xx_get_refclk(crtc, num_connectors);
 
 		/*
@@ -6229,7 +6506,7 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 		 */
 		limit = intel_limit(crtc, refclk);
 		ok = dev_priv->display.find_dpll(limit, crtc,
-						 intel_crtc->config.port_clock,
+						 crtc->new_config->port_clock,
 						 refclk, NULL, &clock);
 		if (!ok) {
 			DRM_ERROR("Couldn't find PLL settings for mode!\n");
@@ -6250,23 +6527,23 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 							    &reduced_clock);
 		}
 		/* Compat-code for transition, will disappear. */
-		intel_crtc->config.dpll.n = clock.n;
-		intel_crtc->config.dpll.m1 = clock.m1;
-		intel_crtc->config.dpll.m2 = clock.m2;
-		intel_crtc->config.dpll.p1 = clock.p1;
-		intel_crtc->config.dpll.p2 = clock.p2;
+		crtc->new_config->dpll.n = clock.n;
+		crtc->new_config->dpll.m1 = clock.m1;
+		crtc->new_config->dpll.m2 = clock.m2;
+		crtc->new_config->dpll.p1 = clock.p1;
+		crtc->new_config->dpll.p2 = clock.p2;
 	}
 
 	if (IS_GEN2(dev)) {
-		i8xx_update_pll(intel_crtc,
+		i8xx_update_pll(crtc,
 				has_reduced_clock ? &reduced_clock : NULL,
 				num_connectors);
 	} else if (IS_CHERRYVIEW(dev)) {
-		chv_update_pll(intel_crtc);
+		chv_update_pll(crtc, crtc->new_config);
 	} else if (IS_VALLEYVIEW(dev)) {
-		vlv_update_pll(intel_crtc);
+		vlv_update_pll(crtc, crtc->new_config);
 	} else {
-		i9xx_update_pll(intel_crtc,
+		i9xx_update_pll(crtc,
 				has_reduced_clock ? &reduced_clock : NULL,
 				num_connectors);
 	}
@@ -6432,8 +6709,8 @@ static bool i9xx_get_pipe_config(struct intel_crtc *crtc,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	uint32_t tmp;
 
-	if (!intel_display_power_enabled(dev_priv,
-					 POWER_DOMAIN_PIPE(crtc->pipe)))
+	if (!intel_display_power_is_enabled(dev_priv,
+					    POWER_DOMAIN_PIPE(crtc->pipe)))
 		return false;
 
 	pipe_config->cpu_transcoder = (enum transcoder) crtc->pipe;
@@ -6538,6 +6815,8 @@ static void ironlake_init_pch_refclk(struct drm_device *dev)
 			if (enc_to_dig_port(&encoder->base)->port == PORT_A)
 				has_cpu_edp = true;
 			break;
+		default:
+			break;
 		}
 	}
 
@@ -6842,6 +7121,8 @@ static void lpt_init_pch_refclk(struct drm_device *dev)
 		case INTEL_OUTPUT_ANALOG:
 			has_vga = true;
 			break;
+		default:
+			break;
 		}
 	}
 
@@ -6870,11 +7151,16 @@ static int ironlake_get_refclk(struct drm_crtc *crtc)
 	int num_connectors = 0;
 	bool is_lvds = false;
 
-	for_each_encoder_on_crtc(dev, crtc, encoder) {
+	for_each_intel_encoder(dev, encoder) {
+		if (encoder->new_crtc != to_intel_crtc(crtc))
+			continue;
+
 		switch (encoder->type) {
 		case INTEL_OUTPUT_LVDS:
 			is_lvds = true;
 			break;
+		default:
+			break;
 		}
 		num_connectors++;
 	}
@@ -7019,7 +7305,7 @@ static void haswell_set_pipeconf(struct drm_crtc *crtc)
 	I915_WRITE(GAMMA_MODE(intel_crtc->pipe), GAMMA_MODE_MODE_8BIT);
 	POSTING_READ(GAMMA_MODE(intel_crtc->pipe));
 
-	if (IS_BROADWELL(dev)) {
+	if (IS_BROADWELL(dev) || INTEL_INFO(dev)->gen >= 9) {
 		val = 0;
 
 		switch (intel_crtc->config.pipe_bpp) {
@@ -7054,18 +7340,12 @@ static bool ironlake_compute_clocks(struct drm_crtc *crtc,
 {
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_encoder *intel_encoder;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int refclk;
 	const intel_limit_t *limit;
 	bool ret, is_lvds = false;
 
-	for_each_encoder_on_crtc(dev, crtc, intel_encoder) {
-		switch (intel_encoder->type) {
-		case INTEL_OUTPUT_LVDS:
-			is_lvds = true;
-			break;
-		}
-	}
+	is_lvds = intel_pipe_will_have_type(intel_crtc, INTEL_OUTPUT_LVDS);
 
 	refclk = ironlake_get_refclk(crtc);
 
@@ -7074,9 +7354,9 @@ static bool ironlake_compute_clocks(struct drm_crtc *crtc,
 	 * refclk, or FALSE.  The returned values represent the clock equation:
 	 * reflck * (5 * (m1 + 2) + (m2 + 2)) / (n + 2) / p1 / p2.
 	 */
-	limit = intel_limit(crtc, refclk);
-	ret = dev_priv->display.find_dpll(limit, crtc,
-					  to_intel_crtc(crtc)->config.port_clock,
+	limit = intel_limit(intel_crtc, refclk);
+	ret = dev_priv->display.find_dpll(limit, intel_crtc,
+					  intel_crtc->new_config->port_clock,
 					  refclk, NULL, clock);
 	if (!ret)
 		return false;
@@ -7089,7 +7369,7 @@ static bool ironlake_compute_clocks(struct drm_crtc *crtc,
 		 * downclock feature.
 		*/
 		*has_reduced_clock =
-			dev_priv->display.find_dpll(limit, crtc,
+			dev_priv->display.find_dpll(limit, intel_crtc,
 						    dev_priv->lvds_downclock,
 						    refclk, clock,
 						    reduced_clock);
@@ -7126,7 +7406,10 @@ static uint32_t ironlake_compute_dpll(struct intel_crtc *intel_crtc,
 	int factor, num_connectors = 0;
 	bool is_lvds = false, is_sdvo = false;
 
-	for_each_encoder_on_crtc(dev, crtc, intel_encoder) {
+	for_each_intel_encoder(dev, intel_encoder) {
+		if (intel_encoder->new_crtc != to_intel_crtc(crtc))
+			continue;
+
 		switch (intel_encoder->type) {
 		case INTEL_OUTPUT_LVDS:
 			is_lvds = true;
@@ -7135,6 +7418,8 @@ static uint32_t ironlake_compute_dpll(struct intel_crtc *intel_crtc,
 		case INTEL_OUTPUT_HDMI:
 			is_sdvo = true;
 			break;
+		default:
+			break;
 		}
 
 		num_connectors++;
@@ -7147,10 +7432,10 @@ static uint32_t ironlake_compute_dpll(struct intel_crtc *intel_crtc,
 		     dev_priv->vbt.lvds_ssc_freq == 100000) ||
 		    (HAS_PCH_IBX(dev) && intel_is_dual_link_lvds(dev)))
 			factor = 25;
-	} else if (intel_crtc->config.sdvo_tv_clock)
+	} else if (intel_crtc->new_config->sdvo_tv_clock)
 		factor = 20;
 
-	if (ironlake_needs_fb_cb_tune(&intel_crtc->config.dpll, factor))
+	if (ironlake_needs_fb_cb_tune(&intel_crtc->new_config->dpll, factor))
 		*fp |= FP_CB_TUNE;
 
 	if (fp2 && (reduced_clock->m < factor * reduced_clock->n))
@@ -7163,20 +7448,20 @@ static uint32_t ironlake_compute_dpll(struct intel_crtc *intel_crtc,
 	else
 		dpll |= DPLLB_MODE_DAC_SERIAL;
 
-	dpll |= (intel_crtc->config.pixel_multiplier - 1)
+	dpll |= (intel_crtc->new_config->pixel_multiplier - 1)
 		<< PLL_REF_SDVO_HDMI_MULTIPLIER_SHIFT;
 
 	if (is_sdvo)
 		dpll |= DPLL_SDVO_HIGH_SPEED;
-	if (intel_crtc->config.has_dp_encoder)
+	if (intel_crtc->new_config->has_dp_encoder)
 		dpll |= DPLL_SDVO_HIGH_SPEED;
 
 	/* compute bitmask from p1 value */
-	dpll |= (1 << (intel_crtc->config.dpll.p1 - 1)) << DPLL_FPA01_P1_POST_DIV_SHIFT;
+	dpll |= (1 << (intel_crtc->new_config->dpll.p1 - 1)) << DPLL_FPA01_P1_POST_DIV_SHIFT;
 	/* also FPA1 */
-	dpll |= (1 << (intel_crtc->config.dpll.p1 - 1)) << DPLL_FPA1_P1_POST_DIV_SHIFT;
+	dpll |= (1 << (intel_crtc->new_config->dpll.p1 - 1)) << DPLL_FPA1_P1_POST_DIV_SHIFT;
 
-	switch (intel_crtc->config.dpll.p2) {
+	switch (intel_crtc->new_config->dpll.p2) {
 	case 5:
 		dpll |= DPLL_DAC_SERIAL_P2_CLOCK_DIV_5;
 		break;
@@ -7199,78 +7484,64 @@ static uint32_t ironlake_compute_dpll(struct intel_crtc *intel_crtc,
 	return dpll | DPLL_VCO_ENABLE;
 }
 
-static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
-				  int x, int y,
-				  struct drm_framebuffer *fb)
+static int ironlake_crtc_compute_clock(struct intel_crtc *crtc)
 {
-	struct drm_device *dev = crtc->dev;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	int num_connectors = 0;
+	struct drm_device *dev = crtc->base.dev;
 	intel_clock_t clock, reduced_clock;
 	u32 dpll = 0, fp = 0, fp2 = 0;
 	bool ok, has_reduced_clock = false;
 	bool is_lvds = false;
-	struct intel_encoder *encoder;
 	struct intel_shared_dpll *pll;
 
-	for_each_encoder_on_crtc(dev, crtc, encoder) {
-		switch (encoder->type) {
-		case INTEL_OUTPUT_LVDS:
-			is_lvds = true;
-			break;
-		}
-
-		num_connectors++;
-	}
+	is_lvds = intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS);
 
 	WARN(!(HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev)),
 	     "Unexpected PCH type %d\n", INTEL_PCH_TYPE(dev));
 
-	ok = ironlake_compute_clocks(crtc, &clock,
+	ok = ironlake_compute_clocks(&crtc->base, &clock,
 				     &has_reduced_clock, &reduced_clock);
-	if (!ok && !intel_crtc->config.clock_set) {
+	if (!ok && !crtc->new_config->clock_set) {
 		DRM_ERROR("Couldn't find PLL settings for mode!\n");
 		return -EINVAL;
 	}
 	/* Compat-code for transition, will disappear. */
-	if (!intel_crtc->config.clock_set) {
-		intel_crtc->config.dpll.n = clock.n;
-		intel_crtc->config.dpll.m1 = clock.m1;
-		intel_crtc->config.dpll.m2 = clock.m2;
-		intel_crtc->config.dpll.p1 = clock.p1;
-		intel_crtc->config.dpll.p2 = clock.p2;
+	if (!crtc->new_config->clock_set) {
+		crtc->new_config->dpll.n = clock.n;
+		crtc->new_config->dpll.m1 = clock.m1;
+		crtc->new_config->dpll.m2 = clock.m2;
+		crtc->new_config->dpll.p1 = clock.p1;
+		crtc->new_config->dpll.p2 = clock.p2;
 	}
 
 	/* CPU eDP is the only output that doesn't need a PCH PLL of its own. */
-	if (intel_crtc->config.has_pch_encoder) {
-		fp = i9xx_dpll_compute_fp(&intel_crtc->config.dpll);
+	if (crtc->new_config->has_pch_encoder) {
+		fp = i9xx_dpll_compute_fp(&crtc->new_config->dpll);
 		if (has_reduced_clock)
 			fp2 = i9xx_dpll_compute_fp(&reduced_clock);
 
-		dpll = ironlake_compute_dpll(intel_crtc,
+		dpll = ironlake_compute_dpll(crtc,
 					     &fp, &reduced_clock,
 					     has_reduced_clock ? &fp2 : NULL);
 
-		intel_crtc->config.dpll_hw_state.dpll = dpll;
-		intel_crtc->config.dpll_hw_state.fp0 = fp;
+		crtc->new_config->dpll_hw_state.dpll = dpll;
+		crtc->new_config->dpll_hw_state.fp0 = fp;
 		if (has_reduced_clock)
-			intel_crtc->config.dpll_hw_state.fp1 = fp2;
+			crtc->new_config->dpll_hw_state.fp1 = fp2;
 		else
-			intel_crtc->config.dpll_hw_state.fp1 = fp;
+			crtc->new_config->dpll_hw_state.fp1 = fp;
 
-		pll = intel_get_shared_dpll(intel_crtc);
+		pll = intel_get_shared_dpll(crtc);
 		if (pll == NULL) {
 			DRM_DEBUG_DRIVER("failed to find PLL for pipe %c\n",
-					 pipe_name(intel_crtc->pipe));
+					 pipe_name(crtc->pipe));
 			return -EINVAL;
 		}
-	} else
-		intel_put_shared_dpll(intel_crtc);
+	}
 
 	if (is_lvds && has_reduced_clock && i915.powersave)
-		intel_crtc->lowfreq_avail = true;
+		crtc->lowfreq_avail = true;
 	else
-		intel_crtc->lowfreq_avail = false;
+		crtc->lowfreq_avail = false;
 
 	return 0;
 }
@@ -7351,6 +7622,22 @@ static void ironlake_get_fdi_m_n_config(struct intel_crtc *crtc,
 				     &pipe_config->fdi_m_n, NULL);
 }
 
+static void skylake_get_pfit_config(struct intel_crtc *crtc,
+				    struct intel_crtc_config *pipe_config)
+{
+	struct drm_device *dev = crtc->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t tmp;
+
+	tmp = I915_READ(PS_CTL(crtc->pipe));
+
+	if (tmp & PS_ENABLE) {
+		pipe_config->pch_pfit.enabled = true;
+		pipe_config->pch_pfit.pos = I915_READ(PS_WIN_POS(crtc->pipe));
+		pipe_config->pch_pfit.size = I915_READ(PS_WIN_SZ(crtc->pipe));
+	}
+}
+
 static void ironlake_get_pfit_config(struct intel_crtc *crtc,
 				     struct intel_crtc_config *pipe_config)
 {
@@ -7442,8 +7729,8 @@ static bool ironlake_get_pipe_config(struct intel_crtc *crtc,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	uint32_t tmp;
 
-	if (!intel_display_power_enabled(dev_priv,
-					 POWER_DOMAIN_PIPE(crtc->pipe)))
+	if (!intel_display_power_is_enabled(dev_priv,
+					    POWER_DOMAIN_PIPE(crtc->pipe)))
 		return false;
 
 	pipe_config->cpu_transcoder = (enum transcoder) crtc->pipe;
@@ -7636,7 +7923,6 @@ static void hsw_disable_lcpll(struct drm_i915_private *dev_priv,
 static void hsw_restore_lcpll(struct drm_i915_private *dev_priv)
 {
 	uint32_t val;
-	unsigned long irqflags;
 
 	val = I915_READ(LCPLL_CTL);
 
@@ -7656,10 +7942,10 @@ static void hsw_restore_lcpll(struct drm_i915_private *dev_priv)
 	 * to call special forcewake code that doesn't touch runtime PM and
 	 * doesn't enable the forcewake delayed work.
 	 */
-	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
+	spin_lock_irq(&dev_priv->uncore.lock);
 	if (dev_priv->uncore.forcewake_count++ == 0)
 		dev_priv->uncore.funcs.force_wake_get(dev_priv, FORCEWAKE_ALL);
-	spin_unlock_irqrestore(&dev_priv->uncore.lock, irqflags);
+	spin_unlock_irq(&dev_priv->uncore.lock);
 
 	if (val & LCPLL_POWER_DOWN_ALLOW) {
 		val &= ~LCPLL_POWER_DOWN_ALLOW;
@@ -7690,10 +7976,10 @@ static void hsw_restore_lcpll(struct drm_i915_private *dev_priv)
 	}
 
 	/* See the big comment above. */
-	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
+	spin_lock_irq(&dev_priv->uncore.lock);
 	if (--dev_priv->uncore.forcewake_count == 0)
 		dev_priv->uncore.funcs.force_wake_put(dev_priv, FORCEWAKE_ALL);
-	spin_unlock_irqrestore(&dev_priv->uncore.lock, irqflags);
+	spin_unlock_irq(&dev_priv->uncore.lock);
 }
 
 /*
@@ -7755,28 +8041,36 @@ void hsw_disable_pc8(struct drm_i915_private *dev_priv)
 	intel_prepare_ddi(dev);
 }
 
-static void snb_modeset_global_resources(struct drm_device *dev)
+static int haswell_crtc_compute_clock(struct intel_crtc *crtc)
 {
-	modeset_update_crtc_power_domains(dev);
-}
+	if (!intel_ddi_pll_select(crtc))
+		return -EINVAL;
 
-static void haswell_modeset_global_resources(struct drm_device *dev)
-{
-	modeset_update_crtc_power_domains(dev);
+	crtc->lowfreq_avail = false;
+
+	return 0;
 }
 
-static int haswell_crtc_mode_set(struct drm_crtc *crtc,
-				 int x, int y,
-				 struct drm_framebuffer *fb)
+static void skylake_get_ddi_pll(struct drm_i915_private *dev_priv,
+				enum port port,
+				struct intel_crtc_config *pipe_config)
 {
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-
-	if (!intel_ddi_pll_select(intel_crtc))
-		return -EINVAL;
+	u32 temp;
 
-	intel_crtc->lowfreq_avail = false;
+	temp = I915_READ(DPLL_CTRL2) & DPLL_CTRL2_DDI_CLK_SEL_MASK(port);
+	pipe_config->ddi_pll_sel = temp >> (port * 3 + 1);
 
-	return 0;
+	switch (pipe_config->ddi_pll_sel) {
+	case SKL_DPLL1:
+		pipe_config->shared_dpll = DPLL_ID_SKL_DPLL1;
+		break;
+	case SKL_DPLL2:
+		pipe_config->shared_dpll = DPLL_ID_SKL_DPLL2;
+		break;
+	case SKL_DPLL3:
+		pipe_config->shared_dpll = DPLL_ID_SKL_DPLL3;
+		break;
+	}
 }
 
 static void haswell_get_ddi_pll(struct drm_i915_private *dev_priv,
@@ -7808,7 +8102,10 @@ static void haswell_get_ddi_port_state(struct intel_crtc *crtc,
 
 	port = (tmp & TRANS_DDI_PORT_MASK) >> TRANS_DDI_PORT_SHIFT;
 
-	haswell_get_ddi_pll(dev_priv, port, pipe_config);
+	if (IS_SKYLAKE(dev))
+		skylake_get_ddi_pll(dev_priv, port, pipe_config);
+	else
+		haswell_get_ddi_pll(dev_priv, port, pipe_config);
 
 	if (pipe_config->shared_dpll >= 0) {
 		pll = &dev_priv->shared_dplls[pipe_config->shared_dpll];
@@ -7822,7 +8119,8 @@ static void haswell_get_ddi_port_state(struct intel_crtc *crtc,
 	 * DDI E. So just check whether this pipe is wired to DDI E and whether
 	 * the PCH transcoder is on.
 	 */
-	if ((port == PORT_E) && I915_READ(LPT_TRANSCONF) & TRANS_ENABLE) {
+	if (INTEL_INFO(dev)->gen < 9 &&
+	    (port == PORT_E) && I915_READ(LPT_TRANSCONF) & TRANS_ENABLE) {
 		pipe_config->has_pch_encoder = true;
 
 		tmp = I915_READ(FDI_RX_CTL(PIPE_A));
@@ -7841,7 +8139,7 @@ static bool haswell_get_pipe_config(struct intel_crtc *crtc,
 	enum intel_display_power_domain pfit_domain;
 	uint32_t tmp;
 
-	if (!intel_display_power_enabled(dev_priv,
+	if (!intel_display_power_is_enabled(dev_priv,
 					 POWER_DOMAIN_PIPE(crtc->pipe)))
 		return false;
 
@@ -7870,7 +8168,7 @@ static bool haswell_get_pipe_config(struct intel_crtc *crtc,
 			pipe_config->cpu_transcoder = TRANSCODER_EDP;
 	}
 
-	if (!intel_display_power_enabled(dev_priv,
+	if (!intel_display_power_is_enabled(dev_priv,
 			POWER_DOMAIN_TRANSCODER(pipe_config->cpu_transcoder)))
 		return false;
 
@@ -7883,8 +8181,12 @@ static bool haswell_get_pipe_config(struct intel_crtc *crtc,
 	intel_get_pipe_timings(crtc, pipe_config);
 
 	pfit_domain = POWER_DOMAIN_PIPE_PANEL_FITTER(crtc->pipe);
-	if (intel_display_power_enabled(dev_priv, pfit_domain))
-		ironlake_get_pfit_config(crtc, pipe_config);
+	if (intel_display_power_is_enabled(dev_priv, pfit_domain)) {
+		if (IS_SKYLAKE(dev))
+			skylake_get_pfit_config(crtc, pipe_config);
+		else
+			ironlake_get_pfit_config(crtc, pipe_config);
+	}
 
 	if (IS_HASWELL(dev))
 		pipe_config->ips_enabled = hsw_crtc_supports_ips(crtc) &&
@@ -7900,314 +8202,6 @@ static bool haswell_get_pipe_config(struct intel_crtc *crtc,
 	return true;
 }
 
-static struct {
-	int clock;
-	u32 config;
-} hdmi_audio_clock[] = {
-	{ DIV_ROUND_UP(25200 * 1000, 1001), AUD_CONFIG_PIXEL_CLOCK_HDMI_25175 },
-	{ 25200, AUD_CONFIG_PIXEL_CLOCK_HDMI_25200 }, /* default per bspec */
-	{ 27000, AUD_CONFIG_PIXEL_CLOCK_HDMI_27000 },
-	{ 27000 * 1001 / 1000, AUD_CONFIG_PIXEL_CLOCK_HDMI_27027 },
-	{ 54000, AUD_CONFIG_PIXEL_CLOCK_HDMI_54000 },
-	{ 54000 * 1001 / 1000, AUD_CONFIG_PIXEL_CLOCK_HDMI_54054 },
-	{ DIV_ROUND_UP(74250 * 1000, 1001), AUD_CONFIG_PIXEL_CLOCK_HDMI_74176 },
-	{ 74250, AUD_CONFIG_PIXEL_CLOCK_HDMI_74250 },
-	{ DIV_ROUND_UP(148500 * 1000, 1001), AUD_CONFIG_PIXEL_CLOCK_HDMI_148352 },
-	{ 148500, AUD_CONFIG_PIXEL_CLOCK_HDMI_148500 },
-};
-
-/* get AUD_CONFIG_PIXEL_CLOCK_HDMI_* value for mode */
-static u32 audio_config_hdmi_pixel_clock(struct drm_display_mode *mode)
-{
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(hdmi_audio_clock); i++) {
-		if (mode->clock == hdmi_audio_clock[i].clock)
-			break;
-	}
-
-	if (i == ARRAY_SIZE(hdmi_audio_clock)) {
-		DRM_DEBUG_KMS("HDMI audio pixel clock setting for %d not found, falling back to defaults\n", mode->clock);
-		i = 1;
-	}
-
-	DRM_DEBUG_KMS("Configuring HDMI audio for pixel clock %d (0x%08x)\n",
-		      hdmi_audio_clock[i].clock,
-		      hdmi_audio_clock[i].config);
-
-	return hdmi_audio_clock[i].config;
-}
-
-static bool intel_eld_uptodate(struct drm_connector *connector,
-			       int reg_eldv, uint32_t bits_eldv,
-			       int reg_elda, uint32_t bits_elda,
-			       int reg_edid)
-{
-	struct drm_i915_private *dev_priv = connector->dev->dev_private;
-	uint8_t *eld = connector->eld;
-	uint32_t i;
-
-	i = I915_READ(reg_eldv);
-	i &= bits_eldv;
-
-	if (!eld[0])
-		return !i;
-
-	if (!i)
-		return false;
-
-	i = I915_READ(reg_elda);
-	i &= ~bits_elda;
-	I915_WRITE(reg_elda, i);
-
-	for (i = 0; i < eld[2]; i++)
-		if (I915_READ(reg_edid) != *((uint32_t *)eld + i))
-			return false;
-
-	return true;
-}
-
-static void g4x_write_eld(struct drm_connector *connector,
-			  struct drm_crtc *crtc,
-			  struct drm_display_mode *mode)
-{
-	struct drm_i915_private *dev_priv = connector->dev->dev_private;
-	uint8_t *eld = connector->eld;
-	uint32_t eldv;
-	uint32_t len;
-	uint32_t i;
-
-	i = I915_READ(G4X_AUD_VID_DID);
-
-	if (i == INTEL_AUDIO_DEVBLC || i == INTEL_AUDIO_DEVCL)
-		eldv = G4X_ELDV_DEVCL_DEVBLC;
-	else
-		eldv = G4X_ELDV_DEVCTG;
-
-	if (intel_eld_uptodate(connector,
-			       G4X_AUD_CNTL_ST, eldv,
-			       G4X_AUD_CNTL_ST, G4X_ELD_ADDR,
-			       G4X_HDMIW_HDMIEDID))
-		return;
-
-	i = I915_READ(G4X_AUD_CNTL_ST);
-	i &= ~(eldv | G4X_ELD_ADDR);
-	len = (i >> 9) & 0x1f;		/* ELD buffer size */
-	I915_WRITE(G4X_AUD_CNTL_ST, i);
-
-	if (!eld[0])
-		return;
-
-	len = min_t(uint8_t, eld[2], len);
-	DRM_DEBUG_DRIVER("ELD size %d\n", len);
-	for (i = 0; i < len; i++)
-		I915_WRITE(G4X_HDMIW_HDMIEDID, *((uint32_t *)eld + i));
-
-	i = I915_READ(G4X_AUD_CNTL_ST);
-	i |= eldv;
-	I915_WRITE(G4X_AUD_CNTL_ST, i);
-}
-
-static void haswell_write_eld(struct drm_connector *connector,
-			      struct drm_crtc *crtc,
-			      struct drm_display_mode *mode)
-{
-	struct drm_i915_private *dev_priv = connector->dev->dev_private;
-	uint8_t *eld = connector->eld;
-	uint32_t eldv;
-	uint32_t i;
-	int len;
-	int pipe = to_intel_crtc(crtc)->pipe;
-	int tmp;
-
-	int hdmiw_hdmiedid = HSW_AUD_EDID_DATA(pipe);
-	int aud_cntl_st = HSW_AUD_DIP_ELD_CTRL(pipe);
-	int aud_config = HSW_AUD_CFG(pipe);
-	int aud_cntrl_st2 = HSW_AUD_PIN_ELD_CP_VLD;
-
-	/* Audio output enable */
-	DRM_DEBUG_DRIVER("HDMI audio: enable codec\n");
-	tmp = I915_READ(aud_cntrl_st2);
-	tmp |= (AUDIO_OUTPUT_ENABLE_A << (pipe * 4));
-	I915_WRITE(aud_cntrl_st2, tmp);
-	POSTING_READ(aud_cntrl_st2);
-
-	assert_pipe_disabled(dev_priv, to_intel_crtc(crtc)->pipe);
-
-	/* Set ELD valid state */
-	tmp = I915_READ(aud_cntrl_st2);
-	DRM_DEBUG_DRIVER("HDMI audio: pin eld vld status=0x%08x\n", tmp);
-	tmp |= (AUDIO_ELD_VALID_A << (pipe * 4));
-	I915_WRITE(aud_cntrl_st2, tmp);
-	tmp = I915_READ(aud_cntrl_st2);
-	DRM_DEBUG_DRIVER("HDMI audio: eld vld status=0x%08x\n", tmp);
-
-	/* Enable HDMI mode */
-	tmp = I915_READ(aud_config);
-	DRM_DEBUG_DRIVER("HDMI audio: audio conf: 0x%08x\n", tmp);
-	/* clear N_programing_enable and N_value_index */
-	tmp &= ~(AUD_CONFIG_N_VALUE_INDEX | AUD_CONFIG_N_PROG_ENABLE);
-	I915_WRITE(aud_config, tmp);
-
-	DRM_DEBUG_DRIVER("ELD on pipe %c\n", pipe_name(pipe));
-
-	eldv = AUDIO_ELD_VALID_A << (pipe * 4);
-
-	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_DISPLAYPORT)) {
-		DRM_DEBUG_DRIVER("ELD: DisplayPort detected\n");
-		eld[5] |= (1 << 2);	/* Conn_Type, 0x1 = DisplayPort */
-		I915_WRITE(aud_config, AUD_CONFIG_N_VALUE_INDEX); /* 0x1 = DP */
-	} else {
-		I915_WRITE(aud_config, audio_config_hdmi_pixel_clock(mode));
-	}
-
-	if (intel_eld_uptodate(connector,
-			       aud_cntrl_st2, eldv,
-			       aud_cntl_st, IBX_ELD_ADDRESS,
-			       hdmiw_hdmiedid))
-		return;
-
-	i = I915_READ(aud_cntrl_st2);
-	i &= ~eldv;
-	I915_WRITE(aud_cntrl_st2, i);
-
-	if (!eld[0])
-		return;
-
-	i = I915_READ(aud_cntl_st);
-	i &= ~IBX_ELD_ADDRESS;
-	I915_WRITE(aud_cntl_st, i);
-	i = (i >> 29) & DIP_PORT_SEL_MASK;		/* DIP_Port_Select, 0x1 = PortB */
-	DRM_DEBUG_DRIVER("port num:%d\n", i);
-
-	len = min_t(uint8_t, eld[2], 21);	/* 84 bytes of hw ELD buffer */
-	DRM_DEBUG_DRIVER("ELD size %d\n", len);
-	for (i = 0; i < len; i++)
-		I915_WRITE(hdmiw_hdmiedid, *((uint32_t *)eld + i));
-
-	i = I915_READ(aud_cntrl_st2);
-	i |= eldv;
-	I915_WRITE(aud_cntrl_st2, i);
-
-}
-
-static void ironlake_write_eld(struct drm_connector *connector,
-			       struct drm_crtc *crtc,
-			       struct drm_display_mode *mode)
-{
-	struct drm_i915_private *dev_priv = connector->dev->dev_private;
-	uint8_t *eld = connector->eld;
-	uint32_t eldv;
-	uint32_t i;
-	int len;
-	int hdmiw_hdmiedid;
-	int aud_config;
-	int aud_cntl_st;
-	int aud_cntrl_st2;
-	int pipe = to_intel_crtc(crtc)->pipe;
-
-	if (HAS_PCH_IBX(connector->dev)) {
-		hdmiw_hdmiedid = IBX_HDMIW_HDMIEDID(pipe);
-		aud_config = IBX_AUD_CFG(pipe);
-		aud_cntl_st = IBX_AUD_CNTL_ST(pipe);
-		aud_cntrl_st2 = IBX_AUD_CNTL_ST2;
-	} else if (IS_VALLEYVIEW(connector->dev)) {
-		hdmiw_hdmiedid = VLV_HDMIW_HDMIEDID(pipe);
-		aud_config = VLV_AUD_CFG(pipe);
-		aud_cntl_st = VLV_AUD_CNTL_ST(pipe);
-		aud_cntrl_st2 = VLV_AUD_CNTL_ST2;
-	} else {
-		hdmiw_hdmiedid = CPT_HDMIW_HDMIEDID(pipe);
-		aud_config = CPT_AUD_CFG(pipe);
-		aud_cntl_st = CPT_AUD_CNTL_ST(pipe);
-		aud_cntrl_st2 = CPT_AUD_CNTRL_ST2;
-	}
-
-	DRM_DEBUG_DRIVER("ELD on pipe %c\n", pipe_name(pipe));
-
-	if (IS_VALLEYVIEW(connector->dev))  {
-		struct intel_encoder *intel_encoder;
-		struct intel_digital_port *intel_dig_port;
-
-		intel_encoder = intel_attached_encoder(connector);
-		intel_dig_port = enc_to_dig_port(&intel_encoder->base);
-		i = intel_dig_port->port;
-	} else {
-		i = I915_READ(aud_cntl_st);
-		i = (i >> 29) & DIP_PORT_SEL_MASK;
-		/* DIP_Port_Select, 0x1 = PortB */
-	}
-
-	if (!i) {
-		DRM_DEBUG_DRIVER("Audio directed to unknown port\n");
-		/* operate blindly on all ports */
-		eldv = IBX_ELD_VALIDB;
-		eldv |= IBX_ELD_VALIDB << 4;
-		eldv |= IBX_ELD_VALIDB << 8;
-	} else {
-		DRM_DEBUG_DRIVER("ELD on port %c\n", port_name(i));
-		eldv = IBX_ELD_VALIDB << ((i - 1) * 4);
-	}
-
-	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_DISPLAYPORT)) {
-		DRM_DEBUG_DRIVER("ELD: DisplayPort detected\n");
-		eld[5] |= (1 << 2);	/* Conn_Type, 0x1 = DisplayPort */
-		I915_WRITE(aud_config, AUD_CONFIG_N_VALUE_INDEX); /* 0x1 = DP */
-	} else {
-		I915_WRITE(aud_config, audio_config_hdmi_pixel_clock(mode));
-	}
-
-	if (intel_eld_uptodate(connector,
-			       aud_cntrl_st2, eldv,
-			       aud_cntl_st, IBX_ELD_ADDRESS,
-			       hdmiw_hdmiedid))
-		return;
-
-	i = I915_READ(aud_cntrl_st2);
-	i &= ~eldv;
-	I915_WRITE(aud_cntrl_st2, i);
-
-	if (!eld[0])
-		return;
-
-	i = I915_READ(aud_cntl_st);
-	i &= ~IBX_ELD_ADDRESS;
-	I915_WRITE(aud_cntl_st, i);
-
-	len = min_t(uint8_t, eld[2], 21);	/* 84 bytes of hw ELD buffer */
-	DRM_DEBUG_DRIVER("ELD size %d\n", len);
-	for (i = 0; i < len; i++)
-		I915_WRITE(hdmiw_hdmiedid, *((uint32_t *)eld + i));
-
-	i = I915_READ(aud_cntrl_st2);
-	i |= eldv;
-	I915_WRITE(aud_cntrl_st2, i);
-}
-
-void intel_write_eld(struct drm_encoder *encoder,
-		     struct drm_display_mode *mode)
-{
-	struct drm_crtc *crtc = encoder->crtc;
-	struct drm_connector *connector;
-	struct drm_device *dev = encoder->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	connector = drm_select_eld(encoder, mode);
-	if (!connector)
-		return;
-
-	DRM_DEBUG_DRIVER("ELD on [CONNECTOR:%d:%s], [ENCODER:%d:%s]\n",
-			 connector->base.id,
-			 connector->name,
-			 connector->encoder->base.id,
-			 connector->encoder->name);
-
-	connector->eld[6] = drm_av_sync_delay(connector, mode) / 2;
-
-	if (dev_priv->display.write_eld)
-		dev_priv->display.write_eld(connector, crtc, mode);
-}
-
 static void i845_update_cursor(struct drm_crtc *crtc, u32 base)
 {
 	struct drm_device *dev = crtc->dev;
@@ -8253,8 +8247,10 @@ static void i845_update_cursor(struct drm_crtc *crtc, u32 base)
 		intel_crtc->cursor_cntl = 0;
 	}
 
-	if (intel_crtc->cursor_base != base)
+	if (intel_crtc->cursor_base != base) {
 		I915_WRITE(_CURABASE, base);
+		intel_crtc->cursor_base = base;
+	}
 
 	if (intel_crtc->cursor_size != size) {
 		I915_WRITE(CURSIZE, size);
@@ -8294,9 +8290,13 @@ static void i9xx_update_cursor(struct drm_crtc *crtc, u32 base)
 				return;
 		}
 		cntl |= pipe << 28; /* Connect to correct pipe */
+
+		if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+			cntl |= CURSOR_PIPE_CSC_ENABLE;
 	}
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
-		cntl |= CURSOR_PIPE_CSC_ENABLE;
+
+	if (to_intel_plane(crtc->cursor)->rotation == BIT(DRM_ROTATE_180))
+		cntl |= CURSOR_ROTATE_180;
 
 	if (intel_crtc->cursor_cntl != cntl) {
 		I915_WRITE(CURCNTR(pipe), cntl);
@@ -8307,6 +8307,8 @@ static void i9xx_update_cursor(struct drm_crtc *crtc, u32 base)
 	/* and commit changes on next vblank */
 	I915_WRITE(CURBASE(pipe), base);
 	POSTING_READ(CURBASE(pipe));
+
+	intel_crtc->cursor_base = base;
 }
 
 /* If no-part of the cursor is visible on the framebuffer, then the GPU may hang... */
@@ -8353,11 +8355,17 @@ static void intel_crtc_update_cursor(struct drm_crtc *crtc,
 
 	I915_WRITE(CURPOS(pipe), pos);
 
+	/* ILK+ do this automagically */
+	if (HAS_GMCH_DISPLAY(dev) &&
+		to_intel_plane(crtc->cursor)->rotation == BIT(DRM_ROTATE_180)) {
+		base += (intel_crtc->cursor_height *
+			intel_crtc->cursor_width - 1) * 4;
+	}
+
 	if (IS_845G(dev) || IS_I865G(dev))
 		i845_update_cursor(crtc, base);
 	else
 		i9xx_update_cursor(crtc, base);
-	intel_crtc->cursor_base = base;
 }
 
 static bool cursor_size_ok(struct drm_device *dev,
@@ -8397,22 +8405,15 @@ static bool cursor_size_ok(struct drm_device *dev,
 	return true;
 }
 
-/*
- * intel_crtc_cursor_set_obj - Set cursor to specified GEM object
- *
- * Note that the object's reference will be consumed if the update fails.  If
- * the update succeeds, the reference of the old object (if any) will be
- * consumed.
- */
 static int intel_crtc_cursor_set_obj(struct drm_crtc *crtc,
 				     struct drm_i915_gem_object *obj,
 				     uint32_t width, uint32_t height)
 {
 	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	enum pipe pipe = intel_crtc->pipe;
-	unsigned old_width, stride;
+	unsigned old_width;
 	uint32_t addr;
 	int ret;
 
@@ -8424,30 +8425,11 @@ static int intel_crtc_cursor_set_obj(struct drm_crtc *crtc,
 		goto finish;
 	}
 
-	/* Check for which cursor types we support */
-	if (!cursor_size_ok(dev, width, height)) {
-		DRM_DEBUG("Cursor dimension not supported\n");
-		return -EINVAL;
-	}
-
-	stride = roundup_pow_of_two(width) * 4;
-	if (obj->base.size < stride * height) {
-		DRM_DEBUG_KMS("buffer is too small\n");
-		ret = -ENOMEM;
-		goto fail;
-	}
-
 	/* we only need to pin inside GTT if cursor is non-phy */
 	mutex_lock(&dev->struct_mutex);
 	if (!INTEL_INFO(dev)->cursor_needs_physical) {
 		unsigned alignment;
 
-		if (obj->tiling_mode) {
-			DRM_DEBUG_KMS("cursor cannot be tiled\n");
-			ret = -EINVAL;
-			goto fail_locked;
-		}
-
 		/*
 		 * Global gtt pte registers are special registers which actually
 		 * forward writes to a chunk of system memory. Which means that
@@ -8514,17 +8496,15 @@ static int intel_crtc_cursor_set_obj(struct drm_crtc *crtc,
 		if (old_width != width)
 			intel_update_watermarks(crtc);
 		intel_crtc_update_cursor(crtc, intel_crtc->cursor_bo != NULL);
-	}
 
-	intel_frontbuffer_flip(dev, INTEL_FRONTBUFFER_CURSOR(pipe));
+		intel_frontbuffer_flip(dev, INTEL_FRONTBUFFER_CURSOR(pipe));
+	}
 
 	return 0;
 fail_unpin:
 	i915_gem_object_unpin_from_display_plane(obj);
 fail_locked:
 	mutex_unlock(&dev->struct_mutex);
-fail:
-	drm_gem_object_unreference_unlocked(&obj->base);
 	return ret;
 }
 
@@ -8559,7 +8539,7 @@ __intel_framebuffer_create(struct drm_device *dev,
 
 	intel_fb = kzalloc(sizeof(*intel_fb), GFP_KERNEL);
 	if (!intel_fb) {
-		drm_gem_object_unreference_unlocked(&obj->base);
+		drm_gem_object_unreference(&obj->base);
 		return ERR_PTR(-ENOMEM);
 	}
 
@@ -8569,7 +8549,7 @@ __intel_framebuffer_create(struct drm_device *dev,
 
 	return &intel_fb->base;
 err:
-	drm_gem_object_unreference_unlocked(&obj->base);
+	drm_gem_object_unreference(&obj->base);
 	kfree(intel_fb);
 
 	return ERR_PTR(ret);
@@ -8702,6 +8682,9 @@ retry:
 		ret = drm_modeset_lock(&crtc->mutex, ctx);
 		if (ret)
 			goto fail_unlock;
+		ret = drm_modeset_lock(&crtc->primary->mutex, ctx);
+		if (ret)
+			goto fail_unlock;
 
 		old->dpms_mode = connector->dpms;
 		old->load_detect_temp = false;
@@ -8739,6 +8722,9 @@ retry:
 	ret = drm_modeset_lock(&crtc->mutex, ctx);
 	if (ret)
 		goto fail_unlock;
+	ret = drm_modeset_lock(&crtc->primary->mutex, ctx);
+	if (ret)
+		goto fail_unlock;
 	intel_encoder->new_crtc = to_intel_crtc(crtc);
 	to_intel_connector(connector)->new_encoder = intel_encoder;
 
@@ -9021,35 +9007,6 @@ struct drm_display_mode *intel_crtc_mode_get(struct drm_device *dev,
 	return mode;
 }
 
-static void intel_increase_pllclock(struct drm_device *dev,
-				    enum pipe pipe)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int dpll_reg = DPLL(pipe);
-	int dpll;
-
-	if (!HAS_GMCH_DISPLAY(dev))
-		return;
-
-	if (!dev_priv->lvds_downclock_avail)
-		return;
-
-	dpll = I915_READ(dpll_reg);
-	if (!HAS_PIPE_CXSR(dev) && (dpll & DISPLAY_RATE_SELECT_FPA1)) {
-		DRM_DEBUG_DRIVER("upclocking LVDS\n");
-
-		assert_panel_unlocked(dev_priv, pipe);
-
-		dpll &= ~DISPLAY_RATE_SELECT_FPA1;
-		I915_WRITE(dpll_reg, dpll);
-		intel_wait_for_vblank(dev, pipe);
-
-		dpll = I915_READ(dpll_reg);
-		if (dpll & DISPLAY_RATE_SELECT_FPA1)
-			DRM_DEBUG_DRIVER("failed to upclock LVDS!\n");
-	}
-}
-
 static void intel_decrease_pllclock(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
@@ -9125,199 +9082,16 @@ out:
 	intel_runtime_pm_put(dev_priv);
 }
 
-
-/**
- * intel_mark_fb_busy - mark given planes as busy
- * @dev: DRM device
- * @frontbuffer_bits: bits for the affected planes
- * @ring: optional ring for asynchronous commands
- *
- * This function gets called every time the screen contents change. It can be
- * used to keep e.g. the update rate at the nominal refresh rate with DRRS.
- */
-static void intel_mark_fb_busy(struct drm_device *dev,
-			       unsigned frontbuffer_bits,
-			       struct intel_engine_cs *ring)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	enum pipe pipe;
-
-	if (!i915.powersave)
-		return;
-
-	for_each_pipe(dev_priv, pipe) {
-		if (!(frontbuffer_bits & INTEL_FRONTBUFFER_ALL_MASK(pipe)))
-			continue;
-
-		intel_increase_pllclock(dev, pipe);
-		if (ring && intel_fbc_enabled(dev))
-			ring->fbc_dirty = true;
-	}
-}
-
-/**
- * intel_fb_obj_invalidate - invalidate frontbuffer object
- * @obj: GEM object to invalidate
- * @ring: set for asynchronous rendering
- *
- * This function gets called every time rendering on the given object starts and
- * frontbuffer caching (fbc, low refresh rate for DRRS, panel self refresh) must
- * be invalidated. If @ring is non-NULL any subsequent invalidation will be delayed
- * until the rendering completes or a flip on this frontbuffer plane is
- * scheduled.
- */
-void intel_fb_obj_invalidate(struct drm_i915_gem_object *obj,
-			     struct intel_engine_cs *ring)
-{
-	struct drm_device *dev = obj->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	WARN_ON(!mutex_is_locked(&dev->struct_mutex));
-
-	if (!obj->frontbuffer_bits)
-		return;
-
-	if (ring) {
-		mutex_lock(&dev_priv->fb_tracking.lock);
-		dev_priv->fb_tracking.busy_bits
-			|= obj->frontbuffer_bits;
-		dev_priv->fb_tracking.flip_bits
-			&= ~obj->frontbuffer_bits;
-		mutex_unlock(&dev_priv->fb_tracking.lock);
-	}
-
-	intel_mark_fb_busy(dev, obj->frontbuffer_bits, ring);
-
-	intel_edp_psr_invalidate(dev, obj->frontbuffer_bits);
-}
-
-/**
- * intel_frontbuffer_flush - flush frontbuffer
- * @dev: DRM device
- * @frontbuffer_bits: frontbuffer plane tracking bits
- *
- * This function gets called every time rendering on the given planes has
- * completed and frontbuffer caching can be started again. Flushes will get
- * delayed if they're blocked by some oustanding asynchronous rendering.
- *
- * Can be called without any locks held.
- */
-void intel_frontbuffer_flush(struct drm_device *dev,
-			     unsigned frontbuffer_bits)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	/* Delay flushing when rings are still busy.*/
-	mutex_lock(&dev_priv->fb_tracking.lock);
-	frontbuffer_bits &= ~dev_priv->fb_tracking.busy_bits;
-	mutex_unlock(&dev_priv->fb_tracking.lock);
-
-	intel_mark_fb_busy(dev, frontbuffer_bits, NULL);
-
-	intel_edp_psr_flush(dev, frontbuffer_bits);
-
-	/*
-	 * FIXME: Unconditional fbc flushing here is a rather gross hack and
-	 * needs to be reworked into a proper frontbuffer tracking scheme like
-	 * psr employs.
-	 */
-	if (IS_BROADWELL(dev))
-		gen8_fbc_sw_flush(dev, FBC_REND_CACHE_CLEAN);
-}
-
-/**
- * intel_fb_obj_flush - flush frontbuffer object
- * @obj: GEM object to flush
- * @retire: set when retiring asynchronous rendering
- *
- * This function gets called every time rendering on the given object has
- * completed and frontbuffer caching can be started again. If @retire is true
- * then any delayed flushes will be unblocked.
- */
-void intel_fb_obj_flush(struct drm_i915_gem_object *obj,
-			bool retire)
-{
-	struct drm_device *dev = obj->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	unsigned frontbuffer_bits;
-
-	WARN_ON(!mutex_is_locked(&dev->struct_mutex));
-
-	if (!obj->frontbuffer_bits)
-		return;
-
-	frontbuffer_bits = obj->frontbuffer_bits;
-
-	if (retire) {
-		mutex_lock(&dev_priv->fb_tracking.lock);
-		/* Filter out new bits since rendering started. */
-		frontbuffer_bits &= dev_priv->fb_tracking.busy_bits;
-
-		dev_priv->fb_tracking.busy_bits &= ~frontbuffer_bits;
-		mutex_unlock(&dev_priv->fb_tracking.lock);
-	}
-
-	intel_frontbuffer_flush(dev, frontbuffer_bits);
-}
-
-/**
- * intel_frontbuffer_flip_prepare - prepare asnychronous frontbuffer flip
- * @dev: DRM device
- * @frontbuffer_bits: frontbuffer plane tracking bits
- *
- * This function gets called after scheduling a flip on @obj. The actual
- * frontbuffer flushing will be delayed until completion is signalled with
- * intel_frontbuffer_flip_complete. If an invalidate happens in between this
- * flush will be cancelled.
- *
- * Can be called without any locks held.
- */
-void intel_frontbuffer_flip_prepare(struct drm_device *dev,
-				    unsigned frontbuffer_bits)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	mutex_lock(&dev_priv->fb_tracking.lock);
-	dev_priv->fb_tracking.flip_bits
-		|= frontbuffer_bits;
-	mutex_unlock(&dev_priv->fb_tracking.lock);
-}
-
-/**
- * intel_frontbuffer_flip_complete - complete asynchronous frontbuffer flush
- * @dev: DRM device
- * @frontbuffer_bits: frontbuffer plane tracking bits
- *
- * This function gets called after the flip has been latched and will complete
- * on the next vblank. It will execute the fush if it hasn't been cancalled yet.
- *
- * Can be called without any locks held.
- */
-void intel_frontbuffer_flip_complete(struct drm_device *dev,
-				     unsigned frontbuffer_bits)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	mutex_lock(&dev_priv->fb_tracking.lock);
-	/* Mask any cancelled flips. */
-	frontbuffer_bits &= dev_priv->fb_tracking.flip_bits;
-	dev_priv->fb_tracking.flip_bits &= ~frontbuffer_bits;
-	mutex_unlock(&dev_priv->fb_tracking.lock);
-
-	intel_frontbuffer_flush(dev, frontbuffer_bits);
-}
-
 static void intel_crtc_destroy(struct drm_crtc *crtc)
 {
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct drm_device *dev = crtc->dev;
 	struct intel_unpin_work *work;
-	unsigned long flags;
 
-	spin_lock_irqsave(&dev->event_lock, flags);
+	spin_lock_irq(&dev->event_lock);
 	work = intel_crtc->unpin_work;
 	intel_crtc->unpin_work = NULL;
-	spin_unlock_irqrestore(&dev->event_lock, flags);
+	spin_unlock_irq(&dev->event_lock);
 
 	if (work) {
 		cancel_work_sync(&work->work);
@@ -9363,6 +9137,10 @@ static void do_intel_finish_page_flip(struct drm_device *dev,
 	if (intel_crtc == NULL)
 		return;
 
+	/*
+	 * This is called both by irq handlers and the reset code (to complete
+	 * lost pageflips) so needs the full irqsave spinlocks.
+	 */
 	spin_lock_irqsave(&dev->event_lock, flags);
 	work = intel_crtc->unpin_work;
 
@@ -9448,7 +9226,12 @@ void intel_prepare_page_flip(struct drm_device *dev, int plane)
 		to_intel_crtc(dev_priv->plane_to_crtc_mapping[plane]);
 	unsigned long flags;
 
-	/* NB: An MMIO update of the plane base pointer will also
+
+	/*
+	 * This is called both by irq handlers and the reset code (to complete
+	 * lost pageflips) so needs the full irqsave spinlocks.
+	 *
+	 * NB: An MMIO update of the plane base pointer will also
 	 * generate a page-flip completion irq, i.e. every modeset
 	 * is also accompanied by a spurious intel_prepare_page_flip().
 	 */
@@ -9738,115 +9521,128 @@ static void intel_do_mmio_flip(struct intel_crtc *intel_crtc)
 	struct intel_framebuffer *intel_fb =
 		to_intel_framebuffer(intel_crtc->base.primary->fb);
 	struct drm_i915_gem_object *obj = intel_fb->obj;
+	bool atomic_update;
+	u32 start_vbl_count;
 	u32 dspcntr;
 	u32 reg;
 
 	intel_mark_page_flip_active(intel_crtc);
 
+	atomic_update = intel_pipe_update_start(intel_crtc, &start_vbl_count);
+
 	reg = DSPCNTR(intel_crtc->plane);
 	dspcntr = I915_READ(reg);
 
-	if (INTEL_INFO(dev)->gen >= 4) {
-		if (obj->tiling_mode != I915_TILING_NONE)
-			dspcntr |= DISPPLANE_TILED;
-		else
-			dspcntr &= ~DISPPLANE_TILED;
-	}
+	if (obj->tiling_mode != I915_TILING_NONE)
+		dspcntr |= DISPPLANE_TILED;
+	else
+		dspcntr &= ~DISPPLANE_TILED;
+
 	I915_WRITE(reg, dspcntr);
 
 	I915_WRITE(DSPSURF(intel_crtc->plane),
 		   intel_crtc->unpin_work->gtt_offset);
 	POSTING_READ(DSPSURF(intel_crtc->plane));
+
+	if (atomic_update)
+		intel_pipe_update_end(intel_crtc, start_vbl_count);
 }
 
-static int intel_postpone_flip(struct drm_i915_gem_object *obj)
+static void intel_mmio_flip_work_func(struct work_struct *work)
 {
+	struct intel_crtc *intel_crtc =
+		container_of(work, struct intel_crtc, mmio_flip.work);
 	struct intel_engine_cs *ring;
-	int ret;
+	uint32_t seqno;
 
-	lockdep_assert_held(&obj->base.dev->struct_mutex);
+	seqno = intel_crtc->mmio_flip.seqno;
+	ring = intel_crtc->mmio_flip.ring;
 
-	if (!obj->last_write_seqno)
-		return 0;
+	if (seqno)
+		WARN_ON(__i915_wait_seqno(ring, seqno,
+					  intel_crtc->reset_counter,
+					  false, NULL, NULL) != 0);
 
-	ring = obj->ring;
-
-	if (i915_seqno_passed(ring->get_seqno(ring, true),
-			      obj->last_write_seqno))
-		return 0;
-
-	ret = i915_gem_check_olr(ring, obj->last_write_seqno);
-	if (ret)
-		return ret;
-
-	if (WARN_ON(!ring->irq_get(ring)))
-		return 0;
-
-	return 1;
+	intel_do_mmio_flip(intel_crtc);
 }
 
-void intel_notify_mmio_flip(struct intel_engine_cs *ring)
+static int intel_queue_mmio_flip(struct drm_device *dev,
+				 struct drm_crtc *crtc,
+				 struct drm_framebuffer *fb,
+				 struct drm_i915_gem_object *obj,
+				 struct intel_engine_cs *ring,
+				 uint32_t flags)
 {
-	struct drm_i915_private *dev_priv = to_i915(ring->dev);
-	struct intel_crtc *intel_crtc;
-	unsigned long irq_flags;
-	u32 seqno;
-
-	seqno = ring->get_seqno(ring, false);
-
-	spin_lock_irqsave(&dev_priv->mmio_flip_lock, irq_flags);
-	for_each_intel_crtc(ring->dev, intel_crtc) {
-		struct intel_mmio_flip *mmio_flip;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 
-		mmio_flip = &intel_crtc->mmio_flip;
-		if (mmio_flip->seqno == 0)
-			continue;
+	intel_crtc->mmio_flip.seqno = obj->last_write_seqno;
+	intel_crtc->mmio_flip.ring = obj->ring;
 
-		if (ring->id != mmio_flip->ring_id)
-			continue;
+	schedule_work(&intel_crtc->mmio_flip.work);
 
-		if (i915_seqno_passed(seqno, mmio_flip->seqno)) {
-			intel_do_mmio_flip(intel_crtc);
-			mmio_flip->seqno = 0;
-			ring->irq_put(ring);
-		}
-	}
-	spin_unlock_irqrestore(&dev_priv->mmio_flip_lock, irq_flags);
+	return 0;
 }
 
-static int intel_queue_mmio_flip(struct drm_device *dev,
+static int intel_gen9_queue_flip(struct drm_device *dev,
 				 struct drm_crtc *crtc,
 				 struct drm_framebuffer *fb,
 				 struct drm_i915_gem_object *obj,
 				 struct intel_engine_cs *ring,
 				 uint32_t flags)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	unsigned long irq_flags;
+	uint32_t plane = 0, stride;
 	int ret;
 
-	if (WARN_ON(intel_crtc->mmio_flip.seqno))
-		return -EBUSY;
+	switch(intel_crtc->pipe) {
+	case PIPE_A:
+		plane = MI_DISPLAY_FLIP_SKL_PLANE_1_A;
+		break;
+	case PIPE_B:
+		plane = MI_DISPLAY_FLIP_SKL_PLANE_1_B;
+		break;
+	case PIPE_C:
+		plane = MI_DISPLAY_FLIP_SKL_PLANE_1_C;
+		break;
+	default:
+		WARN_ONCE(1, "unknown plane in flip command\n");
+		return -ENODEV;
+	}
 
-	ret = intel_postpone_flip(obj);
-	if (ret < 0)
-		return ret;
-	if (ret == 0) {
-		intel_do_mmio_flip(intel_crtc);
-		return 0;
+	switch (obj->tiling_mode) {
+	case I915_TILING_NONE:
+		stride = fb->pitches[0] >> 6;
+		break;
+	case I915_TILING_X:
+		stride = fb->pitches[0] >> 9;
+		break;
+	default:
+		WARN_ONCE(1, "unknown tiling in flip command\n");
+		return -ENODEV;
 	}
 
-	spin_lock_irqsave(&dev_priv->mmio_flip_lock, irq_flags);
-	intel_crtc->mmio_flip.seqno = obj->last_write_seqno;
-	intel_crtc->mmio_flip.ring_id = obj->ring->id;
-	spin_unlock_irqrestore(&dev_priv->mmio_flip_lock, irq_flags);
+	ret = intel_ring_begin(ring, 10);
+	if (ret)
+		return ret;
+
+	intel_ring_emit(ring, MI_LOAD_REGISTER_IMM(1));
+	intel_ring_emit(ring, DERRMR);
+	intel_ring_emit(ring, ~(DERRMR_PIPEA_PRI_FLIP_DONE |
+				DERRMR_PIPEB_PRI_FLIP_DONE |
+				DERRMR_PIPEC_PRI_FLIP_DONE));
+	intel_ring_emit(ring, MI_STORE_REGISTER_MEM_GEN8(1) |
+			      MI_SRM_LRM_GLOBAL_GTT);
+	intel_ring_emit(ring, DERRMR);
+	intel_ring_emit(ring, ring->scratch.gtt_offset + 256);
+	intel_ring_emit(ring, 0);
+
+	intel_ring_emit(ring, MI_DISPLAY_FLIP_I915 | plane);
+	intel_ring_emit(ring, stride << 6 | obj->tiling_mode);
+	intel_ring_emit(ring, intel_crtc->unpin_work->gtt_offset);
+
+	intel_mark_page_flip_active(intel_crtc);
+	__intel_ring_advance(ring);
 
-	/*
-	 * Double check to catch cases where irq fired before
-	 * mmio flip data was ready
-	 */
-	intel_notify_mmio_flip(obj->ring);
 	return 0;
 }
 
@@ -9905,18 +9701,19 @@ void intel_check_page_flip(struct drm_device *dev, int pipe)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_crtc *crtc = dev_priv->pipe_to_crtc_mapping[pipe];
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	unsigned long flags;
+
+	WARN_ON(!in_irq());
 
 	if (crtc == NULL)
 		return;
 
-	spin_lock_irqsave(&dev->event_lock, flags);
+	spin_lock(&dev->event_lock);
 	if (intel_crtc->unpin_work && __intel_pageflip_stall_check(dev, crtc)) {
 		WARN_ONCE(1, "Kicking stuck page flip: queued at %d, now %d\n",
 			 intel_crtc->unpin_work->flip_queued_vblank, drm_vblank_count(dev, pipe));
 		page_flip_completed(intel_crtc);
 	}
-	spin_unlock_irqrestore(&dev->event_lock, flags);
+	spin_unlock(&dev->event_lock);
 }
 
 static int intel_crtc_page_flip(struct drm_crtc *crtc,
@@ -9932,7 +9729,6 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
 	enum pipe pipe = intel_crtc->pipe;
 	struct intel_unpin_work *work;
 	struct intel_engine_cs *ring;
-	unsigned long flags;
 	int ret;
 
 	/*
@@ -9973,7 +9769,7 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
 		goto free_work;
 
 	/* We borrow the event spin lock for protecting unpin_work */
-	spin_lock_irqsave(&dev->event_lock, flags);
+	spin_lock_irq(&dev->event_lock);
 	if (intel_crtc->unpin_work) {
 		/* Before declaring the flip queue wedged, check if
 		 * the hardware completed the operation behind our backs.
@@ -9983,7 +9779,7 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
 			page_flip_completed(intel_crtc);
 		} else {
 			DRM_DEBUG_DRIVER("flip queue: crtc already busy\n");
-			spin_unlock_irqrestore(&dev->event_lock, flags);
+			spin_unlock_irq(&dev->event_lock);
 
 			drm_crtc_vblank_put(crtc);
 			kfree(work);
@@ -9991,7 +9787,7 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
 		}
 	}
 	intel_crtc->unpin_work = work;
-	spin_unlock_irqrestore(&dev->event_lock, flags);
+	spin_unlock_irq(&dev->event_lock);
 
 	if (atomic_read(&intel_crtc->unpin_work_count) >= 2)
 		flush_workqueue(dev_priv->wq);
@@ -10029,7 +9825,7 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
 		ring = &dev_priv->ring[RCS];
 	}
 
-	ret = intel_pin_and_fence_fb_obj(dev, obj, ring);
+	ret = intel_pin_and_fence_fb_obj(crtc->primary, fb, ring);
 	if (ret)
 		goto cleanup_pending;
 
@@ -10078,9 +9874,9 @@ cleanup_pending:
 	mutex_unlock(&dev->struct_mutex);
 
 cleanup:
-	spin_lock_irqsave(&dev->event_lock, flags);
+	spin_lock_irq(&dev->event_lock);
 	intel_crtc->unpin_work = NULL;
-	spin_unlock_irqrestore(&dev->event_lock, flags);
+	spin_unlock_irq(&dev->event_lock);
 
 	drm_crtc_vblank_put(crtc);
 free_work:
@@ -10091,9 +9887,9 @@ out_hang:
 		intel_crtc_wait_for_pending_flips(crtc);
 		ret = intel_pipe_set_base(crtc, crtc->x, crtc->y, fb);
 		if (ret == 0 && event) {
-			spin_lock_irqsave(&dev->event_lock, flags);
+			spin_lock_irq(&dev->event_lock);
 			drm_send_vblank_event(dev, pipe, event);
-			spin_unlock_irqrestore(&dev->event_lock, flags);
+			spin_unlock_irq(&dev->event_lock);
 		}
 	}
 	return ret;
@@ -10289,6 +10085,10 @@ static void intel_dump_pipe_config(struct intel_crtc *crtc,
 		      pipe_config->dp_m2_n2.link_n,
 		      pipe_config->dp_m2_n2.tu);
 
+	DRM_DEBUG_KMS("audio: %i, infoframes: %i\n",
+		      pipe_config->has_audio,
+		      pipe_config->has_infoframe);
+
 	DRM_DEBUG_KMS("requested mode:\n");
 	drm_mode_debug_printmodeline(&pipe_config->requested_mode);
 	DRM_DEBUG_KMS("adjusted mode:\n");
@@ -10350,6 +10150,48 @@ static bool check_encoder_cloning(struct intel_crtc *crtc)
 	return true;
 }
 
+static bool check_digital_port_conflicts(struct drm_device *dev)
+{
+	struct intel_connector *connector;
+	unsigned int used_ports = 0;
+
+	/*
+	 * Walk the connector list instead of the encoder
+	 * list to detect the problem on ddi platforms
+	 * where there's just one encoder per digital port.
+	 */
+	list_for_each_entry(connector,
+			    &dev->mode_config.connector_list, base.head) {
+		struct intel_encoder *encoder = connector->new_encoder;
+
+		if (!encoder)
+			continue;
+
+		WARN_ON(!encoder->new_crtc);
+
+		switch (encoder->type) {
+			unsigned int port_mask;
+		case INTEL_OUTPUT_UNKNOWN:
+			if (WARN_ON(!HAS_DDI(dev)))
+				break;
+		case INTEL_OUTPUT_DISPLAYPORT:
+		case INTEL_OUTPUT_HDMI:
+		case INTEL_OUTPUT_EDP:
+			port_mask = 1 << enc_to_dig_port(&encoder->base)->port;
+
+			/* the same port mustn't appear more than once */
+			if (used_ports & port_mask)
+				return false;
+
+			used_ports |= port_mask;
+		default:
+			break;
+		}
+	}
+
+	return true;
+}
+
 static struct intel_crtc_config *
 intel_modeset_pipe_config(struct drm_crtc *crtc,
 			  struct drm_framebuffer *fb,
@@ -10366,6 +10208,11 @@ intel_modeset_pipe_config(struct drm_crtc *crtc,
 		return ERR_PTR(-EINVAL);
 	}
 
+	if (!check_digital_port_conflicts(dev)) {
+		DRM_DEBUG_KMS("rejecting conflicting digital port configuration\n");
+		return ERR_PTR(-EINVAL);
+	}
+
 	pipe_config = kzalloc(sizeof(*pipe_config), GFP_KERNEL);
 	if (!pipe_config)
 		return ERR_PTR(-ENOMEM);
@@ -10571,10 +10418,13 @@ static bool intel_crtc_in_use(struct drm_crtc *crtc)
 static void
 intel_modeset_update_state(struct drm_device *dev, unsigned prepare_pipes)
 {
+	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_encoder *intel_encoder;
 	struct intel_crtc *intel_crtc;
 	struct drm_connector *connector;
 
+	intel_shared_dpll_commit(dev_priv);
+
 	for_each_intel_encoder(dev, intel_encoder) {
 		if (!intel_encoder->base.crtc)
 			continue;
@@ -10754,6 +10604,7 @@ intel_pipe_config_compare(struct drm_device *dev,
 	if ((INTEL_INFO(dev)->gen < 8 && !IS_HASWELL(dev)) ||
 	    IS_VALLEYVIEW(dev))
 		PIPE_CONF_CHECK_I(limited_color_range);
+	PIPE_CONF_CHECK_I(has_infoframe);
 
 	PIPE_CONF_CHECK_I(has_audio);
 
@@ -10810,6 +10661,9 @@ intel_pipe_config_compare(struct drm_device *dev,
 	PIPE_CONF_CHECK_X(dpll_hw_state.fp0);
 	PIPE_CONF_CHECK_X(dpll_hw_state.fp1);
 	PIPE_CONF_CHECK_X(dpll_hw_state.wrpll);
+	PIPE_CONF_CHECK_X(dpll_hw_state.ctrl1);
+	PIPE_CONF_CHECK_X(dpll_hw_state.cfgcr1);
+	PIPE_CONF_CHECK_X(dpll_hw_state.cfgcr2);
 
 	if (IS_G4X(dev) || INTEL_INFO(dev)->gen >= 5)
 		PIPE_CONF_CHECK_I(pipe_bpp);
@@ -10827,6 +10681,56 @@ intel_pipe_config_compare(struct drm_device *dev,
 	return true;
 }
 
+static void check_wm_state(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct skl_ddb_allocation hw_ddb, *sw_ddb;
+	struct intel_crtc *intel_crtc;
+	int plane;
+
+	if (INTEL_INFO(dev)->gen < 9)
+		return;
+
+	skl_ddb_get_hw_state(dev_priv, &hw_ddb);
+	sw_ddb = &dev_priv->wm.skl_hw.ddb;
+
+	for_each_intel_crtc(dev, intel_crtc) {
+		struct skl_ddb_entry *hw_entry, *sw_entry;
+		const enum pipe pipe = intel_crtc->pipe;
+
+		if (!intel_crtc->active)
+			continue;
+
+		/* planes */
+		for_each_plane(pipe, plane) {
+			hw_entry = &hw_ddb.plane[pipe][plane];
+			sw_entry = &sw_ddb->plane[pipe][plane];
+
+			if (skl_ddb_entry_equal(hw_entry, sw_entry))
+				continue;
+
+			DRM_ERROR("mismatch in DDB state pipe %c plane %d "
+				  "(expected (%u,%u), found (%u,%u))\n",
+				  pipe_name(pipe), plane + 1,
+				  sw_entry->start, sw_entry->end,
+				  hw_entry->start, hw_entry->end);
+		}
+
+		/* cursor */
+		hw_entry = &hw_ddb.cursor[pipe];
+		sw_entry = &sw_ddb->cursor[pipe];
+
+		if (skl_ddb_entry_equal(hw_entry, sw_entry))
+			continue;
+
+		DRM_ERROR("mismatch in DDB state pipe %c cursor "
+			  "(expected (%u,%u), found (%u,%u))\n",
+			  pipe_name(pipe),
+			  sw_entry->start, sw_entry->end,
+			  hw_entry->start, hw_entry->end);
+	}
+}
+
 static void
 check_connector_state(struct drm_device *dev)
 {
@@ -10993,9 +10897,9 @@ check_shared_dpll_state(struct drm_device *dev)
 
 		active = pll->get_hw_state(dev_priv, pll, &dpll_hw_state);
 
-		WARN(pll->active > pll->refcount,
+		WARN(pll->active > hweight32(pll->config.crtc_mask),
 		     "more active pll users than references: %i vs %i\n",
-		     pll->active, pll->refcount);
+		     pll->active, hweight32(pll->config.crtc_mask));
 		WARN(pll->active && !pll->on,
 		     "pll in active use but not on in sw tracking\n");
 		WARN(pll->on && !pll->active,
@@ -11013,11 +10917,11 @@ check_shared_dpll_state(struct drm_device *dev)
 		WARN(pll->active != active_crtcs,
 		     "pll active crtcs mismatch (expected %i, found %i)\n",
 		     pll->active, active_crtcs);
-		WARN(pll->refcount != enabled_crtcs,
+		WARN(hweight32(pll->config.crtc_mask) != enabled_crtcs,
 		     "pll enabled crtcs mismatch (expected %i, found %i)\n",
-		     pll->refcount, enabled_crtcs);
+		     hweight32(pll->config.crtc_mask), enabled_crtcs);
 
-		WARN(pll->on && memcmp(&pll->hw_state, &dpll_hw_state,
+		WARN(pll->on && memcmp(&pll->config.hw_state, &dpll_hw_state,
 				       sizeof(dpll_hw_state)),
 		     "pll hw state mismatch\n");
 	}
@@ -11026,6 +10930,7 @@ check_shared_dpll_state(struct drm_device *dev)
 void
 intel_modeset_check_state(struct drm_device *dev)
 {
+	check_wm_state(dev);
 	check_connector_state(dev);
 	check_encoder_state(dev);
 	check_crtc_state(dev);
@@ -11076,50 +10981,67 @@ static void update_scanline_offset(struct intel_crtc *crtc)
 
 		crtc->scanline_offset = vtotal - 1;
 	} else if (HAS_DDI(dev) &&
-		   intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_HDMI)) {
+		   intel_pipe_has_type(crtc, INTEL_OUTPUT_HDMI)) {
 		crtc->scanline_offset = 2;
 	} else
 		crtc->scanline_offset = 1;
 }
 
+static struct intel_crtc_config *
+intel_modeset_compute_config(struct drm_crtc *crtc,
+			     struct drm_display_mode *mode,
+			     struct drm_framebuffer *fb,
+			     unsigned *modeset_pipes,
+			     unsigned *prepare_pipes,
+			     unsigned *disable_pipes)
+{
+	struct intel_crtc_config *pipe_config = NULL;
+
+	intel_modeset_affected_pipes(crtc, modeset_pipes,
+				     prepare_pipes, disable_pipes);
+
+	if ((*modeset_pipes) == 0)
+		goto out;
+
+	/*
+	 * Note this needs changes when we start tracking multiple modes
+	 * and crtcs.  At that point we'll need to compute the whole config
+	 * (i.e. one pipe_config for each crtc) rather than just the one
+	 * for this crtc.
+	 */
+	pipe_config = intel_modeset_pipe_config(crtc, fb, mode);
+	if (IS_ERR(pipe_config)) {
+		goto out;
+	}
+	intel_dump_pipe_config(to_intel_crtc(crtc), pipe_config,
+			       "[modeset]");
+
+out:
+	return pipe_config;
+}
+
 static int __intel_set_mode(struct drm_crtc *crtc,
 			    struct drm_display_mode *mode,
-			    int x, int y, struct drm_framebuffer *fb)
+			    int x, int y, struct drm_framebuffer *fb,
+			    struct intel_crtc_config *pipe_config,
+			    unsigned modeset_pipes,
+			    unsigned prepare_pipes,
+			    unsigned disable_pipes)
 {
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_display_mode *saved_mode;
-	struct intel_crtc_config *pipe_config = NULL;
 	struct intel_crtc *intel_crtc;
-	unsigned disable_pipes, prepare_pipes, modeset_pipes;
 	int ret = 0;
 
 	saved_mode = kmalloc(sizeof(*saved_mode), GFP_KERNEL);
 	if (!saved_mode)
 		return -ENOMEM;
 
-	intel_modeset_affected_pipes(crtc, &modeset_pipes,
-				     &prepare_pipes, &disable_pipes);
-
 	*saved_mode = crtc->mode;
 
-	/* Hack: Because we don't (yet) support global modeset on multiple
-	 * crtcs, we don't keep track of the new mode for more than one crtc.
-	 * Hence simply check whether any bit is set in modeset_pipes in all the
-	 * pieces of code that are not yet converted to deal with mutliple crtcs
-	 * changing their mode at the same time. */
-	if (modeset_pipes) {
-		pipe_config = intel_modeset_pipe_config(crtc, fb, mode);
-		if (IS_ERR(pipe_config)) {
-			ret = PTR_ERR(pipe_config);
-			pipe_config = NULL;
-
-			goto out;
-		}
-		intel_dump_pipe_config(to_intel_crtc(crtc), pipe_config,
-				       "[modeset]");
+	if (modeset_pipes)
 		to_intel_crtc(crtc)->new_config = pipe_config;
-	}
 
 	/*
 	 * See if the config requires any additional preparation, e.g.
@@ -11135,6 +11057,22 @@ static int __intel_set_mode(struct drm_crtc *crtc,
 		prepare_pipes &= ~disable_pipes;
 	}
 
+	if (dev_priv->display.crtc_compute_clock) {
+		unsigned clear_pipes = modeset_pipes | disable_pipes;
+
+		ret = intel_shared_dpll_start_config(dev_priv, clear_pipes);
+		if (ret)
+			goto done;
+
+		for_each_intel_crtc_masked(dev, modeset_pipes, intel_crtc) {
+			ret = dev_priv->display.crtc_compute_clock(intel_crtc);
+			if (ret) {
+				intel_shared_dpll_abort_config(dev_priv);
+				goto done;
+			}
+		}
+	}
+
 	for_each_intel_crtc_masked(dev, disable_pipes, intel_crtc)
 		intel_crtc_disable(&intel_crtc->base);
 
@@ -11145,6 +11083,10 @@ static int __intel_set_mode(struct drm_crtc *crtc,
 
 	/* crtc->mode is already used by the ->mode_set callbacks, hence we need
 	 * to set it here already despite that we pass it down the callchain.
+	 *
+	 * Note we'll need to fix this up when we start tracking multiple
+	 * pipes; here we assume a single modeset_pipe and only track the
+	 * single crtc and mode.
 	 */
 	if (modeset_pipes) {
 		crtc->mode = *mode;
@@ -11166,8 +11108,7 @@ static int __intel_set_mode(struct drm_crtc *crtc,
 	 * update the the output configuration. */
 	intel_modeset_update_state(dev, prepare_pipes);
 
-	if (dev_priv->display.modeset_global_resources)
-		dev_priv->display.modeset_global_resources(dev);
+	modeset_update_crtc_power_domains(dev);
 
 	/* Set up the DPLL and any encoders state that needs to adjust or depend
 	 * on the DPLL.
@@ -11178,9 +11119,7 @@ static int __intel_set_mode(struct drm_crtc *crtc,
 		struct drm_i915_gem_object *obj = intel_fb_obj(fb);
 
 		mutex_lock(&dev->struct_mutex);
-		ret = intel_pin_and_fence_fb_obj(dev,
-						 obj,
-						 NULL);
+		ret = intel_pin_and_fence_fb_obj(crtc->primary, fb, NULL);
 		if (ret != 0) {
 			DRM_ERROR("pin & fence failed\n");
 			mutex_unlock(&dev->struct_mutex);
@@ -11195,11 +11134,6 @@ static int __intel_set_mode(struct drm_crtc *crtc,
 		crtc->primary->fb = fb;
 		crtc->x = x;
 		crtc->y = y;
-
-		ret = dev_priv->display.crtc_mode_set(&intel_crtc->base,
-						      x, y, fb);
-		if (ret)
-			goto done;
 	}
 
 	/* Now enable the clocks, plane, pipe, and connectors that we set up. */
@@ -11214,19 +11148,23 @@ done:
 	if (ret && crtc->enabled)
 		crtc->mode = *saved_mode;
 
-out:
 	kfree(pipe_config);
 	kfree(saved_mode);
 	return ret;
 }
 
-static int intel_set_mode(struct drm_crtc *crtc,
-			  struct drm_display_mode *mode,
-			  int x, int y, struct drm_framebuffer *fb)
+static int intel_set_mode_pipes(struct drm_crtc *crtc,
+				struct drm_display_mode *mode,
+				int x, int y, struct drm_framebuffer *fb,
+				struct intel_crtc_config *pipe_config,
+				unsigned modeset_pipes,
+				unsigned prepare_pipes,
+				unsigned disable_pipes)
 {
 	int ret;
 
-	ret = __intel_set_mode(crtc, mode, x, y, fb);
+	ret = __intel_set_mode(crtc, mode, x, y, fb, pipe_config, modeset_pipes,
+			       prepare_pipes, disable_pipes);
 
 	if (ret == 0)
 		intel_modeset_check_state(crtc->dev);
@@ -11234,6 +11172,26 @@ static int intel_set_mode(struct drm_crtc *crtc,
 	return ret;
 }
 
+static int intel_set_mode(struct drm_crtc *crtc,
+			  struct drm_display_mode *mode,
+			  int x, int y, struct drm_framebuffer *fb)
+{
+	struct intel_crtc_config *pipe_config;
+	unsigned modeset_pipes, prepare_pipes, disable_pipes;
+
+	pipe_config = intel_modeset_compute_config(crtc, mode, fb,
+						   &modeset_pipes,
+						   &prepare_pipes,
+						   &disable_pipes);
+
+	if (IS_ERR(pipe_config))
+		return PTR_ERR(pipe_config);
+
+	return intel_set_mode_pipes(crtc, mode, x, y, fb, pipe_config,
+				    modeset_pipes, prepare_pipes,
+				    disable_pipes);
+}
+
 void intel_crtc_restore_mode(struct drm_crtc *crtc)
 {
 	intel_set_mode(crtc, &crtc->mode, crtc->x, crtc->y, crtc->primary->fb);
@@ -11562,6 +11520,8 @@ static int intel_crtc_set_config(struct drm_mode_set *set)
 	struct drm_device *dev;
 	struct drm_mode_set save_set;
 	struct intel_set_config *config;
+	struct intel_crtc_config *pipe_config;
+	unsigned modeset_pipes, prepare_pipes, disable_pipes;
 	int ret;
 
 	BUG_ON(!set);
@@ -11607,9 +11567,38 @@ static int intel_crtc_set_config(struct drm_mode_set *set)
 	if (ret)
 		goto fail;
 
+	pipe_config = intel_modeset_compute_config(set->crtc, set->mode,
+						   set->fb,
+						   &modeset_pipes,
+						   &prepare_pipes,
+						   &disable_pipes);
+	if (IS_ERR(pipe_config)) {
+		ret = PTR_ERR(pipe_config);
+		goto fail;
+	} else if (pipe_config) {
+		if (pipe_config->has_audio !=
+		    to_intel_crtc(set->crtc)->config.has_audio)
+			config->mode_changed = true;
+
+		/*
+		 * Note we have an issue here with infoframes: current code
+		 * only updates them on the full mode set path per hw
+		 * requirements.  So here we should be checking for any
+		 * required changes and forcing a mode set.
+		 */
+	}
+
+	/* set_mode will free it in the mode_changed case */
+	if (!config->mode_changed)
+		kfree(pipe_config);
+
+	intel_update_pipe_size(to_intel_crtc(set->crtc));
+
 	if (config->mode_changed) {
-		ret = intel_set_mode(set->crtc, set->mode,
-				     set->x, set->y, set->fb);
+		ret = intel_set_mode_pipes(set->crtc, set->mode,
+					   set->x, set->y, set->fb, pipe_config,
+					   modeset_pipes, prepare_pipes,
+					   disable_pipes);
 	} else if (config->fb_changed) {
 		struct intel_crtc *intel_crtc = to_intel_crtc(set->crtc);
 
@@ -11679,7 +11668,7 @@ static bool ibx_pch_dpll_get_hw_state(struct drm_i915_private *dev_priv,
 {
 	uint32_t val;
 
-	if (!intel_display_power_enabled(dev_priv, POWER_DOMAIN_PLLS))
+	if (!intel_display_power_is_enabled(dev_priv, POWER_DOMAIN_PLLS))
 		return false;
 
 	val = I915_READ(PCH_DPLL(pll->id));
@@ -11693,8 +11682,8 @@ static bool ibx_pch_dpll_get_hw_state(struct drm_i915_private *dev_priv,
 static void ibx_pch_dpll_mode_set(struct drm_i915_private *dev_priv,
 				  struct intel_shared_dpll *pll)
 {
-	I915_WRITE(PCH_FP0(pll->id), pll->hw_state.fp0);
-	I915_WRITE(PCH_FP1(pll->id), pll->hw_state.fp1);
+	I915_WRITE(PCH_FP0(pll->id), pll->config.hw_state.fp0);
+	I915_WRITE(PCH_FP1(pll->id), pll->config.hw_state.fp1);
 }
 
 static void ibx_pch_dpll_enable(struct drm_i915_private *dev_priv,
@@ -11703,7 +11692,7 @@ static void ibx_pch_dpll_enable(struct drm_i915_private *dev_priv,
 	/* PCH refclock must be enabled first */
 	ibx_assert_pch_refclk_enabled(dev_priv);
 
-	I915_WRITE(PCH_DPLL(pll->id), pll->hw_state.dpll);
+	I915_WRITE(PCH_DPLL(pll->id), pll->config.hw_state.dpll);
 
 	/* Wait for the clocks to stabilize. */
 	POSTING_READ(PCH_DPLL(pll->id));
@@ -11714,7 +11703,7 @@ static void ibx_pch_dpll_enable(struct drm_i915_private *dev_priv,
 	 *
 	 * So write it again.
 	 */
-	I915_WRITE(PCH_DPLL(pll->id), pll->hw_state.dpll);
+	I915_WRITE(PCH_DPLL(pll->id), pll->config.hw_state.dpll);
 	POSTING_READ(PCH_DPLL(pll->id));
 	udelay(200);
 }
@@ -11813,161 +11802,195 @@ disable_unpin:
 }
 
 static int
-intel_primary_plane_setplane(struct drm_plane *plane, struct drm_crtc *crtc,
-			     struct drm_framebuffer *fb, int crtc_x, int crtc_y,
-			     unsigned int crtc_w, unsigned int crtc_h,
-			     uint32_t src_x, uint32_t src_y,
-			     uint32_t src_w, uint32_t src_h)
+intel_check_primary_plane(struct drm_plane *plane,
+			  struct intel_plane_state *state)
 {
+	struct drm_crtc *crtc = state->crtc;
+	struct drm_framebuffer *fb = state->fb;
+	struct drm_rect *dest = &state->dst;
+	struct drm_rect *src = &state->src;
+	const struct drm_rect *clip = &state->clip;
+
+	return drm_plane_helper_check_update(plane, crtc, fb,
+					     src, dest, clip,
+					     DRM_PLANE_HELPER_NO_SCALING,
+					     DRM_PLANE_HELPER_NO_SCALING,
+					     false, true, &state->visible);
+}
+
+static int
+intel_prepare_primary_plane(struct drm_plane *plane,
+			    struct intel_plane_state *state)
+{
+	struct drm_crtc *crtc = state->crtc;
+	struct drm_framebuffer *fb = state->fb;
 	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	enum pipe pipe = intel_crtc->pipe;
 	struct drm_i915_gem_object *obj = intel_fb_obj(fb);
 	struct drm_i915_gem_object *old_obj = intel_fb_obj(plane->fb);
-	struct drm_rect dest = {
-		/* integer pixels */
-		.x1 = crtc_x,
-		.y1 = crtc_y,
-		.x2 = crtc_x + crtc_w,
-		.y2 = crtc_y + crtc_h,
-	};
-	struct drm_rect src = {
-		/* 16.16 fixed point */
-		.x1 = src_x,
-		.y1 = src_y,
-		.x2 = src_x + src_w,
-		.y2 = src_y + src_h,
-	};
-	const struct drm_rect clip = {
-		/* integer pixels */
-		.x2 = intel_crtc->active ? intel_crtc->config.pipe_src_w : 0,
-		.y2 = intel_crtc->active ? intel_crtc->config.pipe_src_h : 0,
-	};
-	const struct {
-		int crtc_x, crtc_y;
-		unsigned int crtc_w, crtc_h;
-		uint32_t src_x, src_y, src_w, src_h;
-	} orig = {
-		.crtc_x = crtc_x,
-		.crtc_y = crtc_y,
-		.crtc_w = crtc_w,
-		.crtc_h = crtc_h,
-		.src_x = src_x,
-		.src_y = src_y,
-		.src_w = src_w,
-		.src_h = src_h,
-	};
-	struct intel_plane *intel_plane = to_intel_plane(plane);
-	bool visible;
 	int ret;
 
-	ret = drm_plane_helper_check_update(plane, crtc, fb,
-					    &src, &dest, &clip,
-					    DRM_PLANE_HELPER_NO_SCALING,
-					    DRM_PLANE_HELPER_NO_SCALING,
-					    false, true, &visible);
+	intel_crtc_wait_for_pending_flips(crtc);
 
-	if (ret)
-		return ret;
+	if (intel_crtc_has_pending_flip(crtc)) {
+		DRM_ERROR("pipe is still busy with an old pageflip\n");
+		return -EBUSY;
+	}
 
-	/*
-	 * If the CRTC isn't enabled, we're just pinning the framebuffer,
-	 * updating the fb pointer, and returning without touching the
-	 * hardware.  This allows us to later do a drmModeSetCrtc with fb=-1 to
-	 * turn on the display with all planes setup as desired.
-	 */
-	if (!crtc->enabled) {
+	if (old_obj != obj) {
 		mutex_lock(&dev->struct_mutex);
-
-		/*
-		 * If we already called setplane while the crtc was disabled,
-		 * we may have an fb pinned; unpin it.
-		 */
-		if (plane->fb)
-			intel_unpin_fb_obj(old_obj);
-
-		i915_gem_track_fb(old_obj, obj,
-				  INTEL_FRONTBUFFER_PRIMARY(intel_crtc->pipe));
-
-		/* Pin and return without programming hardware */
-		ret = intel_pin_and_fence_fb_obj(dev, obj, NULL);
+		ret = intel_pin_and_fence_fb_obj(plane, fb, NULL);
+		if (ret == 0)
+			i915_gem_track_fb(old_obj, obj,
+					  INTEL_FRONTBUFFER_PRIMARY(pipe));
 		mutex_unlock(&dev->struct_mutex);
-
-		return ret;
+		if (ret != 0) {
+			DRM_DEBUG_KMS("pin & fence failed\n");
+			return ret;
+		}
 	}
 
-	intel_crtc_wait_for_pending_flips(crtc);
+	return 0;
+}
 
-	/*
-	 * If clipping results in a non-visible primary plane, we'll disable
-	 * the primary plane.  Note that this is a bit different than what
-	 * happens if userspace explicitly disables the plane by passing fb=0
-	 * because plane->fb still gets set and pinned.
-	 */
-	if (!visible) {
-		mutex_lock(&dev->struct_mutex);
+static void
+intel_commit_primary_plane(struct drm_plane *plane,
+			   struct intel_plane_state *state)
+{
+	struct drm_crtc *crtc = state->crtc;
+	struct drm_framebuffer *fb = state->fb;
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	enum pipe pipe = intel_crtc->pipe;
+	struct drm_framebuffer *old_fb = plane->fb;
+	struct drm_i915_gem_object *obj = intel_fb_obj(fb);
+	struct drm_i915_gem_object *old_obj = intel_fb_obj(plane->fb);
+	struct intel_plane *intel_plane = to_intel_plane(plane);
+	struct drm_rect *src = &state->src;
 
+	crtc->primary->fb = fb;
+	crtc->x = src->x1 >> 16;
+	crtc->y = src->y1 >> 16;
+
+	intel_plane->crtc_x = state->orig_dst.x1;
+	intel_plane->crtc_y = state->orig_dst.y1;
+	intel_plane->crtc_w = drm_rect_width(&state->orig_dst);
+	intel_plane->crtc_h = drm_rect_height(&state->orig_dst);
+	intel_plane->src_x = state->orig_src.x1;
+	intel_plane->src_y = state->orig_src.y1;
+	intel_plane->src_w = drm_rect_width(&state->orig_src);
+	intel_plane->src_h = drm_rect_height(&state->orig_src);
+	intel_plane->obj = obj;
+
+	if (intel_crtc->active) {
 		/*
-		 * Try to pin the new fb first so that we can bail out if we
-		 * fail.
+		 * FBC does not work on some platforms for rotated
+		 * planes, so disable it when rotation is not 0 and
+		 * update it when rotation is set back to 0.
+		 *
+		 * FIXME: This is redundant with the fbc update done in
+		 * the primary plane enable function except that that
+		 * one is done too late. We eventually need to unify
+		 * this.
 		 */
-		if (plane->fb != fb) {
-			ret = intel_pin_and_fence_fb_obj(dev, obj, NULL);
-			if (ret) {
-				mutex_unlock(&dev->struct_mutex);
-				return ret;
-			}
+		if (intel_crtc->primary_enabled &&
+		    INTEL_INFO(dev)->gen <= 4 && !IS_G4X(dev) &&
+		    dev_priv->fbc.plane == intel_crtc->plane &&
+		    intel_plane->rotation != BIT(DRM_ROTATE_0)) {
+			intel_disable_fbc(dev);
 		}
 
-		i915_gem_track_fb(old_obj, obj,
-				  INTEL_FRONTBUFFER_PRIMARY(intel_crtc->pipe));
-
-		if (intel_crtc->primary_enabled)
-			intel_disable_primary_hw_plane(plane, crtc);
+		if (state->visible) {
+			bool was_enabled = intel_crtc->primary_enabled;
 
+			/* FIXME: kill this fastboot hack */
+			intel_update_pipe_size(intel_crtc);
 
-		if (plane->fb != fb)
-			if (plane->fb)
-				intel_unpin_fb_obj(old_obj);
+			intel_crtc->primary_enabled = true;
 
-		mutex_unlock(&dev->struct_mutex);
+			dev_priv->display.update_primary_plane(crtc, plane->fb,
+					crtc->x, crtc->y);
 
-	} else {
-		if (intel_crtc && intel_crtc->active &&
-		    intel_crtc->primary_enabled) {
 			/*
-			 * FBC does not work on some platforms for rotated
-			 * planes, so disable it when rotation is not 0 and
-			 * update it when rotation is set back to 0.
-			 *
-			 * FIXME: This is redundant with the fbc update done in
-			 * the primary plane enable function except that that
-			 * one is done too late. We eventually need to unify
-			 * this.
+			 * BDW signals flip done immediately if the plane
+			 * is disabled, even if the plane enable is already
+			 * armed to occur at the next vblank :(
 			 */
-			if (INTEL_INFO(dev)->gen <= 4 && !IS_G4X(dev) &&
-			    dev_priv->fbc.plane == intel_crtc->plane &&
-			    intel_plane->rotation != BIT(DRM_ROTATE_0)) {
-				intel_disable_fbc(dev);
-			}
+			if (IS_BROADWELL(dev) && !was_enabled)
+				intel_wait_for_vblank(dev, intel_crtc->pipe);
+		} else {
+			/*
+			 * If clipping results in a non-visible primary plane,
+			 * we'll disable the primary plane.  Note that this is
+			 * a bit different than what happens if userspace
+			 * explicitly disables the plane by passing fb=0
+			 * because plane->fb still gets set and pinned.
+			 */
+			intel_disable_primary_hw_plane(plane, crtc);
 		}
-		ret = intel_pipe_set_base(crtc, src.x1, src.y1, fb);
-		if (ret)
-			return ret;
 
-		if (!intel_crtc->primary_enabled)
-			intel_enable_primary_hw_plane(plane, crtc);
+		intel_frontbuffer_flip(dev, INTEL_FRONTBUFFER_PRIMARY(pipe));
+
+		mutex_lock(&dev->struct_mutex);
+		intel_update_fbc(dev);
+		mutex_unlock(&dev->struct_mutex);
 	}
 
-	intel_plane->crtc_x = orig.crtc_x;
-	intel_plane->crtc_y = orig.crtc_y;
-	intel_plane->crtc_w = orig.crtc_w;
-	intel_plane->crtc_h = orig.crtc_h;
-	intel_plane->src_x = orig.src_x;
-	intel_plane->src_y = orig.src_y;
-	intel_plane->src_w = orig.src_w;
-	intel_plane->src_h = orig.src_h;
-	intel_plane->obj = obj;
+	if (old_fb && old_fb != fb) {
+		if (intel_crtc->active)
+			intel_wait_for_vblank(dev, intel_crtc->pipe);
+
+		mutex_lock(&dev->struct_mutex);
+		intel_unpin_fb_obj(old_obj);
+		mutex_unlock(&dev->struct_mutex);
+	}
+}
+
+static int
+intel_primary_plane_setplane(struct drm_plane *plane, struct drm_crtc *crtc,
+			     struct drm_framebuffer *fb, int crtc_x, int crtc_y,
+			     unsigned int crtc_w, unsigned int crtc_h,
+			     uint32_t src_x, uint32_t src_y,
+			     uint32_t src_w, uint32_t src_h)
+{
+	struct intel_plane_state state;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int ret;
+
+	state.crtc = crtc;
+	state.fb = fb;
+
+	/* sample coordinates in 16.16 fixed point */
+	state.src.x1 = src_x;
+	state.src.x2 = src_x + src_w;
+	state.src.y1 = src_y;
+	state.src.y2 = src_y + src_h;
+
+	/* integer pixels */
+	state.dst.x1 = crtc_x;
+	state.dst.x2 = crtc_x + crtc_w;
+	state.dst.y1 = crtc_y;
+	state.dst.y2 = crtc_y + crtc_h;
+
+	state.clip.x1 = 0;
+	state.clip.y1 = 0;
+	state.clip.x2 = intel_crtc->active ? intel_crtc->config.pipe_src_w : 0;
+	state.clip.y2 = intel_crtc->active ? intel_crtc->config.pipe_src_h : 0;
+
+	state.orig_src = state.src;
+	state.orig_dst = state.dst;
+
+	ret = intel_check_primary_plane(plane, &state);
+	if (ret)
+		return ret;
+
+	ret = intel_prepare_primary_plane(plane, &state);
+	if (ret)
+		return ret;
+
+	intel_commit_primary_plane(plane, &state);
 
 	return 0;
 }
@@ -12046,51 +12069,92 @@ intel_cursor_plane_disable(struct drm_plane *plane)
 }
 
 static int
-intel_cursor_plane_update(struct drm_plane *plane, struct drm_crtc *crtc,
-			  struct drm_framebuffer *fb, int crtc_x, int crtc_y,
-			  unsigned int crtc_w, unsigned int crtc_h,
-			  uint32_t src_x, uint32_t src_y,
-			  uint32_t src_w, uint32_t src_h)
+intel_check_cursor_plane(struct drm_plane *plane,
+			 struct intel_plane_state *state)
 {
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	struct intel_framebuffer *intel_fb = to_intel_framebuffer(fb);
-	struct drm_i915_gem_object *obj = intel_fb->obj;
-	struct drm_rect dest = {
-		/* integer pixels */
-		.x1 = crtc_x,
-		.y1 = crtc_y,
-		.x2 = crtc_x + crtc_w,
-		.y2 = crtc_y + crtc_h,
-	};
-	struct drm_rect src = {
-		/* 16.16 fixed point */
-		.x1 = src_x,
-		.y1 = src_y,
-		.x2 = src_x + src_w,
-		.y2 = src_y + src_h,
-	};
-	const struct drm_rect clip = {
-		/* integer pixels */
-		.x2 = intel_crtc->active ? intel_crtc->config.pipe_src_w : 0,
-		.y2 = intel_crtc->active ? intel_crtc->config.pipe_src_h : 0,
-	};
-	bool visible;
+	struct drm_crtc *crtc = state->crtc;
+	struct drm_device *dev = crtc->dev;
+	struct drm_framebuffer *fb = state->fb;
+	struct drm_rect *dest = &state->dst;
+	struct drm_rect *src = &state->src;
+	const struct drm_rect *clip = &state->clip;
+	struct drm_i915_gem_object *obj = intel_fb_obj(fb);
+	int crtc_w, crtc_h;
+	unsigned stride;
 	int ret;
 
 	ret = drm_plane_helper_check_update(plane, crtc, fb,
-					    &src, &dest, &clip,
+					    src, dest, clip,
 					    DRM_PLANE_HELPER_NO_SCALING,
 					    DRM_PLANE_HELPER_NO_SCALING,
-					    true, true, &visible);
+					    true, true, &state->visible);
 	if (ret)
 		return ret;
 
-	crtc->cursor_x = crtc_x;
-	crtc->cursor_y = crtc_y;
+
+	/* if we want to turn off the cursor ignore width and height */
+	if (!obj)
+		return 0;
+
+	/* Check for which cursor types we support */
+	crtc_w = drm_rect_width(&state->orig_dst);
+	crtc_h = drm_rect_height(&state->orig_dst);
+	if (!cursor_size_ok(dev, crtc_w, crtc_h)) {
+		DRM_DEBUG("Cursor dimension not supported\n");
+		return -EINVAL;
+	}
+
+	stride = roundup_pow_of_two(crtc_w) * 4;
+	if (obj->base.size < stride * crtc_h) {
+		DRM_DEBUG_KMS("buffer is too small\n");
+		return -ENOMEM;
+	}
+
+	if (fb == crtc->cursor->fb)
+		return 0;
+
+	/* we only need to pin inside GTT if cursor is non-phy */
+	mutex_lock(&dev->struct_mutex);
+	if (!INTEL_INFO(dev)->cursor_needs_physical && obj->tiling_mode) {
+		DRM_DEBUG_KMS("cursor cannot be tiled\n");
+		ret = -EINVAL;
+	}
+	mutex_unlock(&dev->struct_mutex);
+
+	return ret;
+}
+
+static int
+intel_commit_cursor_plane(struct drm_plane *plane,
+			  struct intel_plane_state *state)
+{
+	struct drm_crtc *crtc = state->crtc;
+	struct drm_framebuffer *fb = state->fb;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_plane *intel_plane = to_intel_plane(plane);
+	struct intel_framebuffer *intel_fb = to_intel_framebuffer(fb);
+	struct drm_i915_gem_object *obj = intel_fb->obj;
+	int crtc_w, crtc_h;
+
+	crtc->cursor_x = state->orig_dst.x1;
+	crtc->cursor_y = state->orig_dst.y1;
+
+	intel_plane->crtc_x = state->orig_dst.x1;
+	intel_plane->crtc_y = state->orig_dst.y1;
+	intel_plane->crtc_w = drm_rect_width(&state->orig_dst);
+	intel_plane->crtc_h = drm_rect_height(&state->orig_dst);
+	intel_plane->src_x = state->orig_src.x1;
+	intel_plane->src_y = state->orig_src.y1;
+	intel_plane->src_w = drm_rect_width(&state->orig_src);
+	intel_plane->src_h = drm_rect_height(&state->orig_src);
+	intel_plane->obj = obj;
+
 	if (fb != crtc->cursor->fb) {
+		crtc_w = drm_rect_width(&state->orig_dst);
+		crtc_h = drm_rect_height(&state->orig_dst);
 		return intel_crtc_cursor_set_obj(crtc, obj, crtc_w, crtc_h);
 	} else {
-		intel_crtc_update_cursor(crtc, visible);
+		intel_crtc_update_cursor(crtc, state->visible);
 
 		intel_frontbuffer_flip(crtc->dev,
 				       INTEL_FRONTBUFFER_CURSOR(intel_crtc->pipe));
@@ -12098,10 +12162,53 @@ intel_cursor_plane_update(struct drm_plane *plane, struct drm_crtc *crtc,
 		return 0;
 	}
 }
+
+static int
+intel_cursor_plane_update(struct drm_plane *plane, struct drm_crtc *crtc,
+			  struct drm_framebuffer *fb, int crtc_x, int crtc_y,
+			  unsigned int crtc_w, unsigned int crtc_h,
+			  uint32_t src_x, uint32_t src_y,
+			  uint32_t src_w, uint32_t src_h)
+{
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_plane_state state;
+	int ret;
+
+	state.crtc = crtc;
+	state.fb = fb;
+
+	/* sample coordinates in 16.16 fixed point */
+	state.src.x1 = src_x;
+	state.src.x2 = src_x + src_w;
+	state.src.y1 = src_y;
+	state.src.y2 = src_y + src_h;
+
+	/* integer pixels */
+	state.dst.x1 = crtc_x;
+	state.dst.x2 = crtc_x + crtc_w;
+	state.dst.y1 = crtc_y;
+	state.dst.y2 = crtc_y + crtc_h;
+
+	state.clip.x1 = 0;
+	state.clip.y1 = 0;
+	state.clip.x2 = intel_crtc->active ? intel_crtc->config.pipe_src_w : 0;
+	state.clip.y2 = intel_crtc->active ? intel_crtc->config.pipe_src_h : 0;
+
+	state.orig_src = state.src;
+	state.orig_dst = state.dst;
+
+	ret = intel_check_cursor_plane(plane, &state);
+	if (ret)
+		return ret;
+
+	return intel_commit_cursor_plane(plane, &state);
+}
+
 static const struct drm_plane_funcs intel_cursor_plane_funcs = {
 	.update_plane = intel_cursor_plane_update,
 	.disable_plane = intel_cursor_plane_disable,
 	.destroy = intel_plane_destroy,
+	.set_property = intel_plane_set_property,
 };
 
 static struct drm_plane *intel_cursor_plane_create(struct drm_device *dev,
@@ -12117,12 +12224,26 @@ static struct drm_plane *intel_cursor_plane_create(struct drm_device *dev,
 	cursor->max_downscale = 1;
 	cursor->pipe = pipe;
 	cursor->plane = pipe;
+	cursor->rotation = BIT(DRM_ROTATE_0);
 
 	drm_universal_plane_init(dev, &cursor->base, 0,
 				 &intel_cursor_plane_funcs,
 				 intel_cursor_formats,
 				 ARRAY_SIZE(intel_cursor_formats),
 				 DRM_PLANE_TYPE_CURSOR);
+
+	if (INTEL_INFO(dev)->gen >= 4) {
+		if (!dev->mode_config.rotation_property)
+			dev->mode_config.rotation_property =
+				drm_mode_create_rotation_property(dev,
+							BIT(DRM_ROTATE_0) |
+							BIT(DRM_ROTATE_180));
+		if (dev->mode_config.rotation_property)
+			drm_object_attach_property(&cursor->base.base,
+				dev->mode_config.rotation_property,
+				cursor->rotation);
+	}
+
 	return &cursor->base;
 }
 
@@ -12178,6 +12299,8 @@ static void intel_crtc_init(struct drm_device *dev, int pipe)
 	dev_priv->plane_to_crtc_mapping[intel_crtc->plane] = &intel_crtc->base;
 	dev_priv->pipe_to_crtc_mapping[intel_crtc->pipe] = &intel_crtc->base;
 
+	INIT_WORK(&intel_crtc->mmio_flip.work, intel_mmio_flip_work_func);
+
 	drm_crtc_helper_add(&intel_crtc->base, &intel_helper_funcs);
 
 	WARN_ON(drm_crtc_index(&intel_crtc->base) != intel_crtc->pipe);
@@ -12198,7 +12321,7 @@ enum pipe intel_get_pipe_from_connector(struct intel_connector *connector)
 
 	WARN_ON(!drm_modeset_is_locked(&dev->mode_config.connection_mutex));
 
-	if (!encoder)
+	if (!encoder || WARN_ON(!encoder->crtc))
 		return INVALID_PIPE;
 
 	return to_intel_crtc(encoder->crtc)->pipe;
@@ -12286,7 +12409,10 @@ static bool intel_crt_present(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	if (IS_ULT(dev))
+	if (INTEL_INFO(dev)->gen >= 9)
+		return false;
+
+	if (IS_HSW_ULT(dev) || IS_BDW_ULT(dev))
 		return false;
 
 	if (IS_CHERRYVIEW(dev))
@@ -12430,7 +12556,7 @@ static void intel_setup_outputs(struct drm_device *dev)
 	if (SUPPORTS_TV(dev))
 		intel_tv_init(dev);
 
-	intel_edp_psr_init(dev);
+	intel_psr_init(dev);
 
 	for_each_intel_encoder(dev, encoder) {
 		encoder->base.possible_crtcs = encoder->crtc_mask;
@@ -12634,16 +12760,22 @@ static void intel_init_display(struct drm_device *dev)
 	if (HAS_DDI(dev)) {
 		dev_priv->display.get_pipe_config = haswell_get_pipe_config;
 		dev_priv->display.get_plane_config = ironlake_get_plane_config;
-		dev_priv->display.crtc_mode_set = haswell_crtc_mode_set;
+		dev_priv->display.crtc_compute_clock =
+			haswell_crtc_compute_clock;
 		dev_priv->display.crtc_enable = haswell_crtc_enable;
 		dev_priv->display.crtc_disable = haswell_crtc_disable;
 		dev_priv->display.off = ironlake_crtc_off;
-		dev_priv->display.update_primary_plane =
-			ironlake_update_primary_plane;
+		if (INTEL_INFO(dev)->gen >= 9)
+			dev_priv->display.update_primary_plane =
+				skylake_update_primary_plane;
+		else
+			dev_priv->display.update_primary_plane =
+				ironlake_update_primary_plane;
 	} else if (HAS_PCH_SPLIT(dev)) {
 		dev_priv->display.get_pipe_config = ironlake_get_pipe_config;
 		dev_priv->display.get_plane_config = ironlake_get_plane_config;
-		dev_priv->display.crtc_mode_set = ironlake_crtc_mode_set;
+		dev_priv->display.crtc_compute_clock =
+			ironlake_crtc_compute_clock;
 		dev_priv->display.crtc_enable = ironlake_crtc_enable;
 		dev_priv->display.crtc_disable = ironlake_crtc_disable;
 		dev_priv->display.off = ironlake_crtc_off;
@@ -12652,7 +12784,7 @@ static void intel_init_display(struct drm_device *dev)
 	} else if (IS_VALLEYVIEW(dev)) {
 		dev_priv->display.get_pipe_config = i9xx_get_pipe_config;
 		dev_priv->display.get_plane_config = i9xx_get_plane_config;
-		dev_priv->display.crtc_mode_set = i9xx_crtc_mode_set;
+		dev_priv->display.crtc_compute_clock = i9xx_crtc_compute_clock;
 		dev_priv->display.crtc_enable = valleyview_crtc_enable;
 		dev_priv->display.crtc_disable = i9xx_crtc_disable;
 		dev_priv->display.off = i9xx_crtc_off;
@@ -12661,7 +12793,7 @@ static void intel_init_display(struct drm_device *dev)
 	} else {
 		dev_priv->display.get_pipe_config = i9xx_get_pipe_config;
 		dev_priv->display.get_plane_config = i9xx_get_plane_config;
-		dev_priv->display.crtc_mode_set = i9xx_crtc_mode_set;
+		dev_priv->display.crtc_compute_clock = i9xx_crtc_compute_clock;
 		dev_priv->display.crtc_enable = i9xx_crtc_enable;
 		dev_priv->display.crtc_disable = i9xx_crtc_disable;
 		dev_priv->display.off = i9xx_crtc_off;
@@ -12698,31 +12830,20 @@ static void intel_init_display(struct drm_device *dev)
 		dev_priv->display.get_display_clock_speed =
 			i830_get_display_clock_speed;
 
-	if (IS_G4X(dev)) {
-		dev_priv->display.write_eld = g4x_write_eld;
-	} else if (IS_GEN5(dev)) {
+	if (IS_GEN5(dev)) {
 		dev_priv->display.fdi_link_train = ironlake_fdi_link_train;
-		dev_priv->display.write_eld = ironlake_write_eld;
 	} else if (IS_GEN6(dev)) {
 		dev_priv->display.fdi_link_train = gen6_fdi_link_train;
-		dev_priv->display.write_eld = ironlake_write_eld;
-		dev_priv->display.modeset_global_resources =
-			snb_modeset_global_resources;
 	} else if (IS_IVYBRIDGE(dev)) {
 		/* FIXME: detect B0+ stepping and use auto training */
 		dev_priv->display.fdi_link_train = ivb_manual_fdi_link_train;
-		dev_priv->display.write_eld = ironlake_write_eld;
 		dev_priv->display.modeset_global_resources =
 			ivb_modeset_global_resources;
 	} else if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
 		dev_priv->display.fdi_link_train = hsw_fdi_link_train;
-		dev_priv->display.write_eld = haswell_write_eld;
-		dev_priv->display.modeset_global_resources =
-			haswell_modeset_global_resources;
 	} else if (IS_VALLEYVIEW(dev)) {
 		dev_priv->display.modeset_global_resources =
 			valleyview_modeset_global_resources;
-		dev_priv->display.write_eld = ironlake_write_eld;
 	}
 
 	/* Default just returns -ENODEV to indicate unsupported */
@@ -12749,6 +12870,9 @@ static void intel_init_display(struct drm_device *dev)
 	case 8: /* FIXME(BDW): Check that the gen8 RCS flip works. */
 		dev_priv->display.queue_flip = intel_gen7_queue_flip;
 		break;
+	case 9:
+		dev_priv->display.queue_flip = intel_gen9_queue_flip;
+		break;
 	}
 
 	intel_panel_init_backlight_funcs(dev);
@@ -12953,11 +13077,6 @@ void intel_modeset_init_hw(struct drm_device *dev)
 	intel_enable_gt_powersave(dev);
 }
 
-void intel_modeset_suspend_hw(struct drm_device *dev)
-{
-	intel_suspend_hw(dev);
-}
-
 void intel_modeset_init(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -12983,6 +13102,7 @@ void intel_modeset_init(struct drm_device *dev)
 		return;
 
 	intel_init_display(dev);
+	intel_init_audio(dev);
 
 	if (IS_GEN2(dev)) {
 		dev->mode_config.max_width = 2048;
@@ -13293,7 +13413,7 @@ void i915_redisable_vga(struct drm_device *dev)
 	 * level, just check if the power well is enabled instead of trying to
 	 * follow the "don't touch the power well if we don't need it" policy
 	 * the rest of the driver uses. */
-	if (!intel_display_power_enabled(dev_priv, POWER_DOMAIN_VGA))
+	if (!intel_display_power_is_enabled(dev_priv, POWER_DOMAIN_VGA))
 		return;
 
 	i915_redisable_vga_power_on(dev);
@@ -13337,18 +13457,21 @@ static void intel_modeset_readout_hw_state(struct drm_device *dev)
 	for (i = 0; i < dev_priv->num_shared_dpll; i++) {
 		struct intel_shared_dpll *pll = &dev_priv->shared_dplls[i];
 
-		pll->on = pll->get_hw_state(dev_priv, pll, &pll->hw_state);
+		pll->on = pll->get_hw_state(dev_priv, pll,
+					    &pll->config.hw_state);
 		pll->active = 0;
+		pll->config.crtc_mask = 0;
 		for_each_intel_crtc(dev, crtc) {
-			if (crtc->active && intel_crtc_to_shared_dpll(crtc) == pll)
+			if (crtc->active && intel_crtc_to_shared_dpll(crtc) == pll) {
 				pll->active++;
+				pll->config.crtc_mask |= 1 << crtc->pipe;
+			}
 		}
-		pll->refcount = pll->active;
 
-		DRM_DEBUG_KMS("%s hw state readout: refcount %i, on %i\n",
-			      pll->name, pll->refcount, pll->on);
+		DRM_DEBUG_KMS("%s hw state readout: crtc_mask 0x%08x, on %i\n",
+			      pll->name, pll->config.crtc_mask, pll->on);
 
-		if (pll->refcount)
+		if (pll->config.crtc_mask)
 			intel_display_power_get(dev_priv, POWER_DOMAIN_PLLS);
 	}
 
@@ -13438,7 +13561,9 @@ void intel_modeset_setup_hw_state(struct drm_device *dev,
 		pll->on = false;
 	}
 
-	if (HAS_PCH_SPLIT(dev))
+	if (IS_GEN9(dev))
+		skl_wm_get_hw_state(dev);
+	else if (HAS_PCH_SPLIT(dev))
 		ilk_wm_get_hw_state(dev);
 
 	if (force_restore) {
@@ -13452,8 +13577,8 @@ void intel_modeset_setup_hw_state(struct drm_device *dev,
 			struct drm_crtc *crtc =
 				dev_priv->pipe_to_crtc_mapping[pipe];
 
-			__intel_set_mode(crtc, &crtc->mode, crtc->x, crtc->y,
-					 crtc->primary->fb);
+			intel_set_mode(crtc, &crtc->mode, crtc->x, crtc->y,
+				       crtc->primary->fb);
 		}
 	} else {
 		intel_modeset_update_staged_output_state(dev);
@@ -13464,6 +13589,7 @@ void intel_modeset_setup_hw_state(struct drm_device *dev,
 
 void intel_modeset_gem_init(struct drm_device *dev)
 {
+	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_crtc *c;
 	struct drm_i915_gem_object *obj;
 
@@ -13471,6 +13597,16 @@ void intel_modeset_gem_init(struct drm_device *dev)
 	intel_init_gt_powersave(dev);
 	mutex_unlock(&dev->struct_mutex);
 
+	/*
+	 * There may be no VBT; and if the BIOS enabled SSC we can
+	 * just keep using it to avoid unnecessary flicker.  Whereas if the
+	 * BIOS isn't using it, don't assume it will work even if the VBT
+	 * indicates as much.
+	 */
+	if (HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev))
+		dev_priv->vbt.lvds_use_ssc = !!(I915_READ(PCH_DREF_CONTROL) &
+						DREF_SSC1_ENABLE);
+
 	intel_modeset_init_hw(dev);
 
 	intel_setup_overlay(dev);
@@ -13486,7 +13622,9 @@ void intel_modeset_gem_init(struct drm_device *dev)
 		if (obj == NULL)
 			continue;
 
-		if (intel_pin_and_fence_fb_obj(dev, obj, NULL)) {
+		if (intel_pin_and_fence_fb_obj(c->primary,
+					       c->primary->fb,
+					       NULL)) {
 			DRM_ERROR("failed to pin boot fb on pipe %d\n",
 				  to_intel_crtc(c)->pipe);
 			drm_framebuffer_unreference(c->primary->fb);
@@ -13494,6 +13632,8 @@ void intel_modeset_gem_init(struct drm_device *dev)
 		}
 	}
 	mutex_unlock(&dev->struct_mutex);
+
+	intel_backlight_register(dev);
 }
 
 void intel_connector_unregister(struct intel_connector *intel_connector)
@@ -13509,14 +13649,16 @@ void intel_modeset_cleanup(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_connector *connector;
 
+	intel_disable_gt_powersave(dev);
+
+	intel_backlight_unregister(dev);
+
 	/*
 	 * Interrupts and polling as the first thing to avoid creating havoc.
-	 * Too much stuff here (turning of rps, connectors, ...) would
+	 * Too much stuff here (turning of connectors, ...) would
 	 * experience fancy races otherwise.
 	 */
-	drm_irq_uninstall(dev);
-	intel_hpd_cancel_work(dev_priv);
-	dev_priv->pm._irqs_disabled = true;
+	intel_irq_uninstall(dev_priv);
 
 	/*
 	 * Due to the hpd irq storm handling the hotplug work can re-arm the
@@ -13530,8 +13672,6 @@ void intel_modeset_cleanup(struct drm_device *dev)
 
 	intel_disable_fbc(dev);
 
-	intel_disable_gt_powersave(dev);
-
 	ironlake_teardown_rc6(dev);
 
 	mutex_unlock(&dev->struct_mutex);
@@ -13671,8 +13811,8 @@ intel_display_capture_error_state(struct drm_device *dev)
 
 	for_each_pipe(dev_priv, i) {
 		error->pipe[i].power_domain_on =
-			intel_display_power_enabled_unlocked(dev_priv,
-							   POWER_DOMAIN_PIPE(i));
+			__intel_display_power_is_enabled(dev_priv,
+							 POWER_DOMAIN_PIPE(i));
 		if (!error->pipe[i].power_domain_on)
 			continue;
 
@@ -13707,7 +13847,7 @@ intel_display_capture_error_state(struct drm_device *dev)
 		enum transcoder cpu_transcoder = transcoders[i];
 
 		error->transcoder[i].power_domain_on =
-			intel_display_power_enabled_unlocked(dev_priv,
+			__intel_display_power_is_enabled(dev_priv,
 				POWER_DOMAIN_TRANSCODER(cpu_transcoder));
 		if (!error->transcoder[i].power_domain_on)
 			continue;
@@ -13791,9 +13931,8 @@ void intel_modeset_preclose(struct drm_device *dev, struct drm_file *file)
 
 	for_each_intel_crtc(dev, crtc) {
 		struct intel_unpin_work *work;
-		unsigned long irqflags;
 
-		spin_lock_irqsave(&dev->event_lock, irqflags);
+		spin_lock_irq(&dev->event_lock);
 
 		work = crtc->unpin_work;
 
@@ -13803,6 +13942,6 @@ void intel_modeset_preclose(struct drm_device *dev, struct drm_file *file)
 			work->event = NULL;
 		}
 
-		spin_unlock_irqrestore(&dev->event_lock, irqflags);
+		spin_unlock_irq(&dev->event_lock);
 	}
 }
diff --git a/drivers/gpu/drm/i915/intel_dp.c b/drivers/gpu/drm/i915/intel_dp.c
index 4bcd91757321..5cecc20efa71 100644
--- a/drivers/gpu/drm/i915/intel_dp.c
+++ b/drivers/gpu/drm/i915/intel_dp.c
@@ -113,6 +113,9 @@ static struct intel_dp *intel_attached_dp(struct drm_connector *connector)
 static void intel_dp_link_down(struct intel_dp *intel_dp);
 static bool edp_panel_vdd_on(struct intel_dp *intel_dp);
 static void edp_panel_vdd_off(struct intel_dp *intel_dp, bool sync);
+static void vlv_init_panel_power_sequencer(struct intel_dp *intel_dp);
+static void vlv_steal_power_sequencer(struct drm_device *dev,
+				      enum pipe pipe);
 
 int
 intel_dp_max_link_bw(struct intel_dp *intel_dp)
@@ -224,8 +227,7 @@ intel_dp_mode_valid(struct drm_connector *connector,
 	return MODE_OK;
 }
 
-static uint32_t
-pack_aux(uint8_t *src, int src_bytes)
+uint32_t intel_dp_pack_aux(const uint8_t *src, int src_bytes)
 {
 	int	i;
 	uint32_t v = 0;
@@ -237,8 +239,7 @@ pack_aux(uint8_t *src, int src_bytes)
 	return v;
 }
 
-static void
-unpack_aux(uint32_t src, uint8_t *dst, int dst_bytes)
+void intel_dp_unpack_aux(uint32_t src, uint8_t *dst, int dst_bytes)
 {
 	int i;
 	if (dst_bytes > 4)
@@ -283,12 +284,10 @@ intel_hrawclk(struct drm_device *dev)
 
 static void
 intel_dp_init_panel_power_sequencer(struct drm_device *dev,
-				    struct intel_dp *intel_dp,
-				    struct edp_power_seq *out);
+				    struct intel_dp *intel_dp);
 static void
 intel_dp_init_panel_power_sequencer_registers(struct drm_device *dev,
-					      struct intel_dp *intel_dp,
-					      struct edp_power_seq *out);
+					      struct intel_dp *intel_dp);
 
 static void pps_lock(struct intel_dp *intel_dp)
 {
@@ -322,6 +321,66 @@ static void pps_unlock(struct intel_dp *intel_dp)
 	intel_display_power_put(dev_priv, power_domain);
 }
 
+static void
+vlv_power_sequencer_kick(struct intel_dp *intel_dp)
+{
+	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
+	struct drm_device *dev = intel_dig_port->base.base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	enum pipe pipe = intel_dp->pps_pipe;
+	bool pll_enabled;
+	uint32_t DP;
+
+	if (WARN(I915_READ(intel_dp->output_reg) & DP_PORT_EN,
+		 "skipping pipe %c power seqeuncer kick due to port %c being active\n",
+		 pipe_name(pipe), port_name(intel_dig_port->port)))
+		return;
+
+	DRM_DEBUG_KMS("kicking pipe %c power sequencer for port %c\n",
+		      pipe_name(pipe), port_name(intel_dig_port->port));
+
+	/* Preserve the BIOS-computed detected bit. This is
+	 * supposed to be read-only.
+	 */
+	DP = I915_READ(intel_dp->output_reg) & DP_DETECTED;
+	DP |= DP_VOLTAGE_0_4 | DP_PRE_EMPHASIS_0;
+	DP |= DP_PORT_WIDTH(1);
+	DP |= DP_LINK_TRAIN_PAT_1;
+
+	if (IS_CHERRYVIEW(dev))
+		DP |= DP_PIPE_SELECT_CHV(pipe);
+	else if (pipe == PIPE_B)
+		DP |= DP_PIPEB_SELECT;
+
+	pll_enabled = I915_READ(DPLL(pipe)) & DPLL_VCO_ENABLE;
+
+	/*
+	 * The DPLL for the pipe must be enabled for this to work.
+	 * So enable temporarily it if it's not already enabled.
+	 */
+	if (!pll_enabled)
+		vlv_force_pll_on(dev, pipe, IS_CHERRYVIEW(dev) ?
+				 &chv_dpll[0].dpll : &vlv_dpll[0].dpll);
+
+	/*
+	 * Similar magic as in intel_dp_enable_port().
+	 * We _must_ do this port enable + disable trick
+	 * to make this power seqeuencer lock onto the port.
+	 * Otherwise even VDD force bit won't work.
+	 */
+	I915_WRITE(intel_dp->output_reg, DP);
+	POSTING_READ(intel_dp->output_reg);
+
+	I915_WRITE(intel_dp->output_reg, DP | DP_PORT_EN);
+	POSTING_READ(intel_dp->output_reg);
+
+	I915_WRITE(intel_dp->output_reg, DP & ~DP_PORT_EN);
+	POSTING_READ(intel_dp->output_reg);
+
+	if (!pll_enabled)
+		vlv_force_pll_off(dev, pipe);
+}
+
 static enum pipe
 vlv_power_sequencer_pipe(struct intel_dp *intel_dp)
 {
@@ -330,10 +389,13 @@ vlv_power_sequencer_pipe(struct intel_dp *intel_dp)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_encoder *encoder;
 	unsigned int pipes = (1 << PIPE_A) | (1 << PIPE_B);
-	struct edp_power_seq power_seq;
+	enum pipe pipe;
 
 	lockdep_assert_held(&dev_priv->pps_mutex);
 
+	/* We should never land here with regular DP ports */
+	WARN_ON(!is_edp(intel_dp));
+
 	if (intel_dp->pps_pipe != INVALID_PIPE)
 		return intel_dp->pps_pipe;
 
@@ -359,18 +421,26 @@ vlv_power_sequencer_pipe(struct intel_dp *intel_dp)
 	 * are two power sequencers and up to two eDP ports.
 	 */
 	if (WARN_ON(pipes == 0))
-		return PIPE_A;
+		pipe = PIPE_A;
+	else
+		pipe = ffs(pipes) - 1;
 
-	intel_dp->pps_pipe = ffs(pipes) - 1;
+	vlv_steal_power_sequencer(dev, pipe);
+	intel_dp->pps_pipe = pipe;
 
 	DRM_DEBUG_KMS("picked pipe %c power sequencer for port %c\n",
 		      pipe_name(intel_dp->pps_pipe),
 		      port_name(intel_dig_port->port));
 
 	/* init power sequencer on this pipe and port */
-	intel_dp_init_panel_power_sequencer(dev, intel_dp, &power_seq);
-	intel_dp_init_panel_power_sequencer_registers(dev, intel_dp,
-						      &power_seq);
+	intel_dp_init_panel_power_sequencer(dev, intel_dp);
+	intel_dp_init_panel_power_sequencer_registers(dev, intel_dp);
+
+	/*
+	 * Even vdd force doesn't work until we've made
+	 * the power sequencer lock in on the port.
+	 */
+	vlv_power_sequencer_kick(intel_dp);
 
 	return intel_dp->pps_pipe;
 }
@@ -425,7 +495,6 @@ vlv_initial_power_sequencer_setup(struct intel_dp *intel_dp)
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
 	struct drm_device *dev = intel_dig_port->base.base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct edp_power_seq power_seq;
 	enum port port = intel_dig_port->port;
 
 	lockdep_assert_held(&dev_priv->pps_mutex);
@@ -453,9 +522,8 @@ vlv_initial_power_sequencer_setup(struct intel_dp *intel_dp)
 	DRM_DEBUG_KMS("initial power sequencer for port %c: pipe %c\n",
 		      port_name(port), pipe_name(intel_dp->pps_pipe));
 
-	intel_dp_init_panel_power_sequencer(dev, intel_dp, &power_seq);
-	intel_dp_init_panel_power_sequencer_registers(dev, intel_dp,
-						      &power_seq);
+	intel_dp_init_panel_power_sequencer(dev, intel_dp);
+	intel_dp_init_panel_power_sequencer_registers(dev, intel_dp);
 }
 
 void vlv_power_sequencer_reset(struct drm_i915_private *dev_priv)
@@ -550,6 +618,10 @@ static bool edp_have_panel_power(struct intel_dp *intel_dp)
 
 	lockdep_assert_held(&dev_priv->pps_mutex);
 
+	if (IS_VALLEYVIEW(dev) &&
+	    intel_dp->pps_pipe == INVALID_PIPE)
+		return false;
+
 	return (I915_READ(_pp_stat_reg(intel_dp)) & PP_ON) != 0;
 }
 
@@ -560,6 +632,10 @@ static bool edp_have_panel_vdd(struct intel_dp *intel_dp)
 
 	lockdep_assert_held(&dev_priv->pps_mutex);
 
+	if (IS_VALLEYVIEW(dev) &&
+	    intel_dp->pps_pipe == INVALID_PIPE)
+		return false;
+
 	return I915_READ(_pp_ctrl_reg(intel_dp)) & EDP_FORCE_VDD;
 }
 
@@ -661,6 +737,16 @@ static uint32_t vlv_get_aux_clock_divider(struct intel_dp *intel_dp, int index)
 	return index ? 0 : 100;
 }
 
+static uint32_t skl_get_aux_clock_divider(struct intel_dp *intel_dp, int index)
+{
+	/*
+	 * SKL doesn't need us to program the AUX clock divider (Hardware will
+	 * derive the clock from CDCLK automatically). We still implement the
+	 * get_aux_clock_divider vfunc to plug-in into the existing code.
+	 */
+	return index ? 0 : 1;
+}
+
 static uint32_t i9xx_get_aux_send_ctl(struct intel_dp *intel_dp,
 				      bool has_aux_irq,
 				      int send_bytes,
@@ -691,9 +777,24 @@ static uint32_t i9xx_get_aux_send_ctl(struct intel_dp *intel_dp,
 	       (aux_clock_divider << DP_AUX_CH_CTL_BIT_CLOCK_2X_SHIFT);
 }
 
+static uint32_t skl_get_aux_send_ctl(struct intel_dp *intel_dp,
+				      bool has_aux_irq,
+				      int send_bytes,
+				      uint32_t unused)
+{
+	return DP_AUX_CH_CTL_SEND_BUSY |
+	       DP_AUX_CH_CTL_DONE |
+	       (has_aux_irq ? DP_AUX_CH_CTL_INTERRUPT : 0) |
+	       DP_AUX_CH_CTL_TIME_OUT_ERROR |
+	       DP_AUX_CH_CTL_TIME_OUT_1600us |
+	       DP_AUX_CH_CTL_RECEIVE_ERROR |
+	       (send_bytes << DP_AUX_CH_CTL_MESSAGE_SIZE_SHIFT) |
+	       DP_AUX_CH_CTL_SYNC_PULSE_SKL(32);
+}
+
 static int
 intel_dp_aux_ch(struct intel_dp *intel_dp,
-		uint8_t *send, int send_bytes,
+		const uint8_t *send, int send_bytes,
 		uint8_t *recv, int recv_size)
 {
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
@@ -760,7 +861,8 @@ intel_dp_aux_ch(struct intel_dp *intel_dp,
 			/* Load the send data into the aux channel data registers */
 			for (i = 0; i < send_bytes; i += 4)
 				I915_WRITE(ch_data + i,
-					   pack_aux(send + i, send_bytes - i));
+					   intel_dp_pack_aux(send + i,
+							     send_bytes - i));
 
 			/* Send the command and wait for it to complete */
 			I915_WRITE(ch_ctl, send_ctl);
@@ -814,8 +916,8 @@ intel_dp_aux_ch(struct intel_dp *intel_dp,
 		recv_bytes = recv_size;
 
 	for (i = 0; i < recv_bytes; i += 4)
-		unpack_aux(I915_READ(ch_data + i),
-			   recv + i, recv_bytes - i);
+		intel_dp_unpack_aux(I915_READ(ch_data + i),
+				    recv + i, recv_bytes - i);
 
 	ret = recv_bytes;
 out:
@@ -925,7 +1027,16 @@ intel_dp_aux_init(struct intel_dp *intel_dp, struct intel_connector *connector)
 		BUG();
 	}
 
-	if (!HAS_DDI(dev))
+	/*
+	 * The AUX_CTL register is usually DP_CTL + 0x10.
+	 *
+	 * On Haswell and Broadwell though:
+	 *   - Both port A DDI_BUF_CTL and DDI_AUX_CTL are on the CPU
+	 *   - Port B/C/D AUX channels are on the PCH, DDI_BUF_CTL on the CPU
+	 *
+	 * Skylake moves AUX_CTL back next to DDI_BUF_CTL, on the CPU.
+	 */
+	if (!IS_HASWELL(dev) && !IS_BROADWELL(dev))
 		intel_dp->aux_ch_ctl_reg = intel_dp->output_reg + 0x10;
 
 	intel_dp->aux.name = name;
@@ -963,6 +1074,33 @@ intel_dp_connector_unregister(struct intel_connector *intel_connector)
 }
 
 static void
+skl_edp_set_pll_config(struct intel_crtc_config *pipe_config, int link_bw)
+{
+	u32 ctrl1;
+
+	pipe_config->ddi_pll_sel = SKL_DPLL0;
+	pipe_config->dpll_hw_state.cfgcr1 = 0;
+	pipe_config->dpll_hw_state.cfgcr2 = 0;
+
+	ctrl1 = DPLL_CTRL1_OVERRIDE(SKL_DPLL0);
+	switch (link_bw) {
+	case DP_LINK_BW_1_62:
+		ctrl1 |= DPLL_CRTL1_LINK_RATE(DPLL_CRTL1_LINK_RATE_810,
+					      SKL_DPLL0);
+		break;
+	case DP_LINK_BW_2_7:
+		ctrl1 |= DPLL_CRTL1_LINK_RATE(DPLL_CRTL1_LINK_RATE_1350,
+					      SKL_DPLL0);
+		break;
+	case DP_LINK_BW_5_4:
+		ctrl1 |= DPLL_CRTL1_LINK_RATE(DPLL_CRTL1_LINK_RATE_2700,
+					      SKL_DPLL0);
+		break;
+	}
+	pipe_config->dpll_hw_state.ctrl1 = ctrl1;
+}
+
+static void
 hsw_dp_set_ddi_pll_sel(struct intel_crtc_config *pipe_config, int link_bw)
 {
 	switch (link_bw) {
@@ -1139,7 +1277,9 @@ found:
 				&pipe_config->dp_m2_n2);
 	}
 
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+	if (IS_SKYLAKE(dev) && is_edp(intel_dp))
+		skl_edp_set_pll_config(pipe_config, intel_dp->link_bw);
+	else if (IS_HASWELL(dev) || IS_BROADWELL(dev))
 		hsw_dp_set_ddi_pll_sel(pipe_config, intel_dp->link_bw);
 	else
 		intel_dp_set_clock(encoder, pipe_config, intel_dp->link_bw);
@@ -1212,12 +1352,8 @@ static void intel_dp_prepare(struct intel_encoder *encoder)
 	intel_dp->DP |= DP_VOLTAGE_0_4 | DP_PRE_EMPHASIS_0;
 	intel_dp->DP |= DP_PORT_WIDTH(intel_dp->lane_count);
 
-	if (crtc->config.has_audio) {
-		DRM_DEBUG_DRIVER("Enabling DP audio on pipe %c\n",
-				 pipe_name(crtc->pipe));
+	if (crtc->config.has_audio)
 		intel_dp->DP |= DP_AUDIO_OUTPUT_ENABLE;
-		intel_write_eld(&encoder->base, adjusted_mode);
-	}
 
 	/* Split out the IBX/CPU vs CPT settings */
 
@@ -1367,6 +1503,7 @@ static bool edp_panel_vdd_on(struct intel_dp *intel_dp)
 	if (!is_edp(intel_dp))
 		return false;
 
+	cancel_delayed_work(&intel_dp->panel_vdd_work);
 	intel_dp->want_panel_vdd = true;
 
 	if (edp_have_panel_vdd(intel_dp))
@@ -1375,7 +1512,8 @@ static bool edp_panel_vdd_on(struct intel_dp *intel_dp)
 	power_domain = intel_display_port_power_domain(intel_encoder);
 	intel_display_power_get(dev_priv, power_domain);
 
-	DRM_DEBUG_KMS("Turning eDP VDD on\n");
+	DRM_DEBUG_KMS("Turning eDP port %c VDD on\n",
+		      port_name(intel_dig_port->port));
 
 	if (!edp_have_panel_power(intel_dp))
 		wait_panel_power_cycle(intel_dp);
@@ -1394,7 +1532,8 @@ static bool edp_panel_vdd_on(struct intel_dp *intel_dp)
 	 * If the panel wasn't on, delay before accessing aux channel
 	 */
 	if (!edp_have_panel_power(intel_dp)) {
-		DRM_DEBUG_KMS("eDP was not running\n");
+		DRM_DEBUG_KMS("eDP port %c panel power wasn't enabled\n",
+			      port_name(intel_dig_port->port));
 		msleep(intel_dp->panel_power_up_delay);
 	}
 
@@ -1419,7 +1558,8 @@ void intel_edp_panel_vdd_on(struct intel_dp *intel_dp)
 	vdd = edp_panel_vdd_on(intel_dp);
 	pps_unlock(intel_dp);
 
-	WARN(!vdd, "eDP VDD already requested on\n");
+	WARN(!vdd, "eDP port %c VDD already requested on\n",
+	     port_name(dp_to_dig_port(intel_dp)->port));
 }
 
 static void edp_panel_vdd_off_sync(struct intel_dp *intel_dp)
@@ -1440,7 +1580,8 @@ static void edp_panel_vdd_off_sync(struct intel_dp *intel_dp)
 	if (!edp_have_panel_vdd(intel_dp))
 		return;
 
-	DRM_DEBUG_KMS("Turning eDP VDD off\n");
+	DRM_DEBUG_KMS("Turning eDP port %c VDD off\n",
+		      port_name(intel_dig_port->port));
 
 	pp = ironlake_get_pp_control(intel_dp);
 	pp &= ~EDP_FORCE_VDD;
@@ -1501,7 +1642,8 @@ static void edp_panel_vdd_off(struct intel_dp *intel_dp, bool sync)
 	if (!is_edp(intel_dp))
 		return;
 
-	WARN(!intel_dp->want_panel_vdd, "eDP VDD not forced on");
+	WARN(!intel_dp->want_panel_vdd, "eDP port %c VDD not forced on",
+	     port_name(dp_to_dig_port(intel_dp)->port));
 
 	intel_dp->want_panel_vdd = false;
 
@@ -1511,40 +1653,25 @@ static void edp_panel_vdd_off(struct intel_dp *intel_dp, bool sync)
 		edp_panel_vdd_schedule_off(intel_dp);
 }
 
-/*
- * Must be paired with intel_edp_panel_vdd_on().
- * Nested calls to these functions are not allowed since
- * we drop the lock. Caller must use some higher level
- * locking to prevent nested calls from other threads.
- */
-static void intel_edp_panel_vdd_off(struct intel_dp *intel_dp, bool sync)
-{
-	if (!is_edp(intel_dp))
-		return;
-
-	pps_lock(intel_dp);
-	edp_panel_vdd_off(intel_dp, sync);
-	pps_unlock(intel_dp);
-}
-
-void intel_edp_panel_on(struct intel_dp *intel_dp)
+static void edp_panel_on(struct intel_dp *intel_dp)
 {
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 pp;
 	u32 pp_ctrl_reg;
 
+	lockdep_assert_held(&dev_priv->pps_mutex);
+
 	if (!is_edp(intel_dp))
 		return;
 
-	DRM_DEBUG_KMS("Turn eDP power on\n");
+	DRM_DEBUG_KMS("Turn eDP port %c panel power on\n",
+		      port_name(dp_to_dig_port(intel_dp)->port));
 
-	pps_lock(intel_dp);
-
-	if (edp_have_panel_power(intel_dp)) {
-		DRM_DEBUG_KMS("eDP power already on\n");
-		goto out;
-	}
+	if (WARN(edp_have_panel_power(intel_dp),
+		 "eDP port %c panel power already on\n",
+		 port_name(dp_to_dig_port(intel_dp)->port)))
+		return;
 
 	wait_panel_power_cycle(intel_dp);
 
@@ -1572,12 +1699,20 @@ void intel_edp_panel_on(struct intel_dp *intel_dp)
 		I915_WRITE(pp_ctrl_reg, pp);
 		POSTING_READ(pp_ctrl_reg);
 	}
+}
+
+void intel_edp_panel_on(struct intel_dp *intel_dp)
+{
+	if (!is_edp(intel_dp))
+		return;
 
- out:
+	pps_lock(intel_dp);
+	edp_panel_on(intel_dp);
 	pps_unlock(intel_dp);
 }
 
-void intel_edp_panel_off(struct intel_dp *intel_dp)
+
+static void edp_panel_off(struct intel_dp *intel_dp)
 {
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
 	struct intel_encoder *intel_encoder = &intel_dig_port->base;
@@ -1587,14 +1722,16 @@ void intel_edp_panel_off(struct intel_dp *intel_dp)
 	u32 pp;
 	u32 pp_ctrl_reg;
 
+	lockdep_assert_held(&dev_priv->pps_mutex);
+
 	if (!is_edp(intel_dp))
 		return;
 
-	DRM_DEBUG_KMS("Turn eDP power off\n");
-
-	pps_lock(intel_dp);
+	DRM_DEBUG_KMS("Turn eDP port %c panel power off\n",
+		      port_name(dp_to_dig_port(intel_dp)->port));
 
-	WARN(!intel_dp->want_panel_vdd, "Need VDD to turn off panel\n");
+	WARN(!intel_dp->want_panel_vdd, "Need eDP port %c VDD to turn off panel\n",
+	     port_name(dp_to_dig_port(intel_dp)->port));
 
 	pp = ironlake_get_pp_control(intel_dp);
 	/* We need to switch off panel power _and_ force vdd, for otherwise some
@@ -1615,7 +1752,15 @@ void intel_edp_panel_off(struct intel_dp *intel_dp)
 	/* We got a reference when we enabled the VDD. */
 	power_domain = intel_display_port_power_domain(intel_encoder);
 	intel_display_power_put(dev_priv, power_domain);
+}
+
+void intel_edp_panel_off(struct intel_dp *intel_dp)
+{
+	if (!is_edp(intel_dp))
+		return;
 
+	pps_lock(intel_dp);
+	edp_panel_off(intel_dp);
 	pps_unlock(intel_dp);
 }
 
@@ -1819,7 +1964,7 @@ static bool intel_dp_get_hw_state(struct intel_encoder *encoder,
 	u32 tmp;
 
 	power_domain = intel_display_port_power_domain(encoder);
-	if (!intel_display_power_enabled(dev_priv, power_domain))
+	if (!intel_display_power_is_enabled(dev_priv, power_domain))
 		return false;
 
 	tmp = I915_READ(intel_dp->output_reg);
@@ -1951,368 +2096,14 @@ static void intel_dp_get_config(struct intel_encoder *encoder,
 	}
 }
 
-static bool is_edp_psr(struct intel_dp *intel_dp)
-{
-	return intel_dp->psr_dpcd[0] & DP_PSR_IS_SUPPORTED;
-}
-
-static bool intel_edp_is_psr_enabled(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (!HAS_PSR(dev))
-		return false;
-
-	return I915_READ(EDP_PSR_CTL(dev)) & EDP_PSR_ENABLE;
-}
-
-static void intel_edp_psr_write_vsc(struct intel_dp *intel_dp,
-				    struct edp_vsc_psr *vsc_psr)
-{
-	struct intel_digital_port *dig_port = dp_to_dig_port(intel_dp);
-	struct drm_device *dev = dig_port->base.base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_crtc *crtc = to_intel_crtc(dig_port->base.base.crtc);
-	u32 ctl_reg = HSW_TVIDEO_DIP_CTL(crtc->config.cpu_transcoder);
-	u32 data_reg = HSW_TVIDEO_DIP_VSC_DATA(crtc->config.cpu_transcoder);
-	uint32_t *data = (uint32_t *) vsc_psr;
-	unsigned int i;
-
-	/* As per BSPec (Pipe Video Data Island Packet), we need to disable
-	   the video DIP being updated before program video DIP data buffer
-	   registers for DIP being updated. */
-	I915_WRITE(ctl_reg, 0);
-	POSTING_READ(ctl_reg);
-
-	for (i = 0; i < VIDEO_DIP_VSC_DATA_SIZE; i += 4) {
-		if (i < sizeof(struct edp_vsc_psr))
-			I915_WRITE(data_reg + i, *data++);
-		else
-			I915_WRITE(data_reg + i, 0);
-	}
-
-	I915_WRITE(ctl_reg, VIDEO_DIP_ENABLE_VSC_HSW);
-	POSTING_READ(ctl_reg);
-}
-
-static void intel_edp_psr_setup(struct intel_dp *intel_dp)
-{
-	struct drm_device *dev = intel_dp_to_dev(intel_dp);
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct edp_vsc_psr psr_vsc;
-
-	/* Prepare VSC packet as per EDP 1.3 spec, Table 3.10 */
-	memset(&psr_vsc, 0, sizeof(psr_vsc));
-	psr_vsc.sdp_header.HB0 = 0;
-	psr_vsc.sdp_header.HB1 = 0x7;
-	psr_vsc.sdp_header.HB2 = 0x2;
-	psr_vsc.sdp_header.HB3 = 0x8;
-	intel_edp_psr_write_vsc(intel_dp, &psr_vsc);
-
-	/* Avoid continuous PSR exit by masking memup and hpd */
-	I915_WRITE(EDP_PSR_DEBUG_CTL(dev), EDP_PSR_DEBUG_MASK_MEMUP |
-		   EDP_PSR_DEBUG_MASK_HPD | EDP_PSR_DEBUG_MASK_LPSP);
-}
-
-static void intel_edp_psr_enable_sink(struct intel_dp *intel_dp)
-{
-	struct intel_digital_port *dig_port = dp_to_dig_port(intel_dp);
-	struct drm_device *dev = dig_port->base.base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t aux_clock_divider;
-	int precharge = 0x3;
-	int msg_size = 5;       /* Header(4) + Message(1) */
-	bool only_standby = false;
-
-	aux_clock_divider = intel_dp->get_aux_clock_divider(intel_dp, 0);
-
-	if (IS_BROADWELL(dev) && dig_port->port != PORT_A)
-		only_standby = true;
-
-	/* Enable PSR in sink */
-	if (intel_dp->psr_dpcd[1] & DP_PSR_NO_TRAIN_ON_EXIT || only_standby)
-		drm_dp_dpcd_writeb(&intel_dp->aux, DP_PSR_EN_CFG,
-				   DP_PSR_ENABLE & ~DP_PSR_MAIN_LINK_ACTIVE);
-	else
-		drm_dp_dpcd_writeb(&intel_dp->aux, DP_PSR_EN_CFG,
-				   DP_PSR_ENABLE | DP_PSR_MAIN_LINK_ACTIVE);
-
-	/* Setup AUX registers */
-	I915_WRITE(EDP_PSR_AUX_DATA1(dev), EDP_PSR_DPCD_COMMAND);
-	I915_WRITE(EDP_PSR_AUX_DATA2(dev), EDP_PSR_DPCD_NORMAL_OPERATION);
-	I915_WRITE(EDP_PSR_AUX_CTL(dev),
-		   DP_AUX_CH_CTL_TIME_OUT_400us |
-		   (msg_size << DP_AUX_CH_CTL_MESSAGE_SIZE_SHIFT) |
-		   (precharge << DP_AUX_CH_CTL_PRECHARGE_2US_SHIFT) |
-		   (aux_clock_divider << DP_AUX_CH_CTL_BIT_CLOCK_2X_SHIFT));
-}
-
-static void intel_edp_psr_enable_source(struct intel_dp *intel_dp)
-{
-	struct intel_digital_port *dig_port = dp_to_dig_port(intel_dp);
-	struct drm_device *dev = dig_port->base.base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t max_sleep_time = 0x1f;
-	uint32_t idle_frames = 1;
-	uint32_t val = 0x0;
-	const uint32_t link_entry_time = EDP_PSR_MIN_LINK_ENTRY_TIME_8_LINES;
-	bool only_standby = false;
-
-	if (IS_BROADWELL(dev) && dig_port->port != PORT_A)
-		only_standby = true;
-
-	if (intel_dp->psr_dpcd[1] & DP_PSR_NO_TRAIN_ON_EXIT || only_standby) {
-		val |= EDP_PSR_LINK_STANDBY;
-		val |= EDP_PSR_TP2_TP3_TIME_0us;
-		val |= EDP_PSR_TP1_TIME_0us;
-		val |= EDP_PSR_SKIP_AUX_EXIT;
-		val |= IS_BROADWELL(dev) ? BDW_PSR_SINGLE_FRAME : 0;
-	} else
-		val |= EDP_PSR_LINK_DISABLE;
-
-	I915_WRITE(EDP_PSR_CTL(dev), val |
-		   (IS_BROADWELL(dev) ? 0 : link_entry_time) |
-		   max_sleep_time << EDP_PSR_MAX_SLEEP_TIME_SHIFT |
-		   idle_frames << EDP_PSR_IDLE_FRAME_SHIFT |
-		   EDP_PSR_ENABLE);
-}
-
-static bool intel_edp_psr_match_conditions(struct intel_dp *intel_dp)
-{
-	struct intel_digital_port *dig_port = dp_to_dig_port(intel_dp);
-	struct drm_device *dev = dig_port->base.base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc = dig_port->base.base.crtc;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-
-	lockdep_assert_held(&dev_priv->psr.lock);
-	WARN_ON(!drm_modeset_is_locked(&dev->mode_config.connection_mutex));
-	WARN_ON(!drm_modeset_is_locked(&crtc->mutex));
-
-	dev_priv->psr.source_ok = false;
-
-	if (IS_HASWELL(dev) && dig_port->port != PORT_A) {
-		DRM_DEBUG_KMS("HSW ties PSR to DDI A (eDP)\n");
-		return false;
-	}
-
-	if (!i915.enable_psr) {
-		DRM_DEBUG_KMS("PSR disable by flag\n");
-		return false;
-	}
-
-	/* Below limitations aren't valid for Broadwell */
-	if (IS_BROADWELL(dev))
-		goto out;
-
-	if (I915_READ(HSW_STEREO_3D_CTL(intel_crtc->config.cpu_transcoder)) &
-	    S3D_ENABLE) {
-		DRM_DEBUG_KMS("PSR condition failed: Stereo 3D is Enabled\n");
-		return false;
-	}
-
-	if (intel_crtc->config.adjusted_mode.flags & DRM_MODE_FLAG_INTERLACE) {
-		DRM_DEBUG_KMS("PSR condition failed: Interlaced is Enabled\n");
-		return false;
-	}
-
- out:
-	dev_priv->psr.source_ok = true;
-	return true;
-}
-
-static void intel_edp_psr_do_enable(struct intel_dp *intel_dp)
-{
-	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
-	struct drm_device *dev = intel_dig_port->base.base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	WARN_ON(I915_READ(EDP_PSR_CTL(dev)) & EDP_PSR_ENABLE);
-	WARN_ON(dev_priv->psr.active);
-	lockdep_assert_held(&dev_priv->psr.lock);
-
-	/* Enable PSR on the panel */
-	intel_edp_psr_enable_sink(intel_dp);
-
-	/* Enable PSR on the host */
-	intel_edp_psr_enable_source(intel_dp);
-
-	dev_priv->psr.active = true;
-}
-
-void intel_edp_psr_enable(struct intel_dp *intel_dp)
-{
-	struct drm_device *dev = intel_dp_to_dev(intel_dp);
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (!HAS_PSR(dev)) {
-		DRM_DEBUG_KMS("PSR not supported on this platform\n");
-		return;
-	}
-
-	if (!is_edp_psr(intel_dp)) {
-		DRM_DEBUG_KMS("PSR not supported by this panel\n");
-		return;
-	}
-
-	mutex_lock(&dev_priv->psr.lock);
-	if (dev_priv->psr.enabled) {
-		DRM_DEBUG_KMS("PSR already in use\n");
-		mutex_unlock(&dev_priv->psr.lock);
-		return;
-	}
-
-	dev_priv->psr.busy_frontbuffer_bits = 0;
-
-	/* Setup PSR once */
-	intel_edp_psr_setup(intel_dp);
-
-	if (intel_edp_psr_match_conditions(intel_dp))
-		dev_priv->psr.enabled = intel_dp;
-	mutex_unlock(&dev_priv->psr.lock);
-}
-
-void intel_edp_psr_disable(struct intel_dp *intel_dp)
-{
-	struct drm_device *dev = intel_dp_to_dev(intel_dp);
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	mutex_lock(&dev_priv->psr.lock);
-	if (!dev_priv->psr.enabled) {
-		mutex_unlock(&dev_priv->psr.lock);
-		return;
-	}
-
-	if (dev_priv->psr.active) {
-		I915_WRITE(EDP_PSR_CTL(dev),
-			   I915_READ(EDP_PSR_CTL(dev)) & ~EDP_PSR_ENABLE);
-
-		/* Wait till PSR is idle */
-		if (_wait_for((I915_READ(EDP_PSR_STATUS_CTL(dev)) &
-			       EDP_PSR_STATUS_STATE_MASK) == 0, 2000, 10))
-			DRM_ERROR("Timed out waiting for PSR Idle State\n");
-
-		dev_priv->psr.active = false;
-	} else {
-		WARN_ON(I915_READ(EDP_PSR_CTL(dev)) & EDP_PSR_ENABLE);
-	}
-
-	dev_priv->psr.enabled = NULL;
-	mutex_unlock(&dev_priv->psr.lock);
-
-	cancel_delayed_work_sync(&dev_priv->psr.work);
-}
-
-static void intel_edp_psr_work(struct work_struct *work)
-{
-	struct drm_i915_private *dev_priv =
-		container_of(work, typeof(*dev_priv), psr.work.work);
-	struct intel_dp *intel_dp = dev_priv->psr.enabled;
-
-	mutex_lock(&dev_priv->psr.lock);
-	intel_dp = dev_priv->psr.enabled;
-
-	if (!intel_dp)
-		goto unlock;
-
-	/*
-	 * The delayed work can race with an invalidate hence we need to
-	 * recheck. Since psr_flush first clears this and then reschedules we
-	 * won't ever miss a flush when bailing out here.
-	 */
-	if (dev_priv->psr.busy_frontbuffer_bits)
-		goto unlock;
-
-	intel_edp_psr_do_enable(intel_dp);
-unlock:
-	mutex_unlock(&dev_priv->psr.lock);
-}
-
-static void intel_edp_psr_do_exit(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (dev_priv->psr.active) {
-		u32 val = I915_READ(EDP_PSR_CTL(dev));
-
-		WARN_ON(!(val & EDP_PSR_ENABLE));
-
-		I915_WRITE(EDP_PSR_CTL(dev), val & ~EDP_PSR_ENABLE);
-
-		dev_priv->psr.active = false;
-	}
-
-}
-
-void intel_edp_psr_invalidate(struct drm_device *dev,
-			      unsigned frontbuffer_bits)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc;
-	enum pipe pipe;
-
-	mutex_lock(&dev_priv->psr.lock);
-	if (!dev_priv->psr.enabled) {
-		mutex_unlock(&dev_priv->psr.lock);
-		return;
-	}
-
-	crtc = dp_to_dig_port(dev_priv->psr.enabled)->base.base.crtc;
-	pipe = to_intel_crtc(crtc)->pipe;
-
-	intel_edp_psr_do_exit(dev);
-
-	frontbuffer_bits &= INTEL_FRONTBUFFER_ALL_MASK(pipe);
-
-	dev_priv->psr.busy_frontbuffer_bits |= frontbuffer_bits;
-	mutex_unlock(&dev_priv->psr.lock);
-}
-
-void intel_edp_psr_flush(struct drm_device *dev,
-			 unsigned frontbuffer_bits)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc;
-	enum pipe pipe;
-
-	mutex_lock(&dev_priv->psr.lock);
-	if (!dev_priv->psr.enabled) {
-		mutex_unlock(&dev_priv->psr.lock);
-		return;
-	}
-
-	crtc = dp_to_dig_port(dev_priv->psr.enabled)->base.base.crtc;
-	pipe = to_intel_crtc(crtc)->pipe;
-	dev_priv->psr.busy_frontbuffer_bits &= ~frontbuffer_bits;
-
-	/*
-	 * On Haswell sprite plane updates don't result in a psr invalidating
-	 * signal in the hardware. Which means we need to manually fake this in
-	 * software for all flushes, not just when we've seen a preceding
-	 * invalidation through frontbuffer rendering.
-	 */
-	if (IS_HASWELL(dev) &&
-	    (frontbuffer_bits & INTEL_FRONTBUFFER_SPRITE(pipe)))
-		intel_edp_psr_do_exit(dev);
-
-	if (!dev_priv->psr.active && !dev_priv->psr.busy_frontbuffer_bits)
-		schedule_delayed_work(&dev_priv->psr.work,
-				      msecs_to_jiffies(100));
-	mutex_unlock(&dev_priv->psr.lock);
-}
-
-void intel_edp_psr_init(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	INIT_DELAYED_WORK(&dev_priv->psr.work, intel_edp_psr_work);
-	mutex_init(&dev_priv->psr.lock);
-}
-
 static void intel_disable_dp(struct intel_encoder *encoder)
 {
 	struct intel_dp *intel_dp = enc_to_intel_dp(&encoder->base);
 	struct drm_device *dev = encoder->base.dev;
+	struct intel_crtc *crtc = to_intel_crtc(encoder->base.crtc);
+
+	if (crtc->config.has_audio)
+		intel_audio_codec_disable(encoder);
 
 	/* Make sure the panel is off before trying to change the mode. But also
 	 * ensure that we have vdd while we switch off the panel. */
@@ -2467,14 +2258,23 @@ static void intel_dp_enable_port(struct intel_dp *intel_dp)
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	intel_dp->DP |= DP_PORT_EN;
-
 	/* enable with pattern 1 (as per spec) */
 	_intel_dp_set_link_train(intel_dp, &intel_dp->DP,
 				 DP_TRAINING_PATTERN_1);
 
 	I915_WRITE(intel_dp->output_reg, intel_dp->DP);
 	POSTING_READ(intel_dp->output_reg);
+
+	/*
+	 * Magic for VLV/CHV. We _must_ first set up the register
+	 * without actually enabling the port, and then do another
+	 * write to enable the port. Otherwise link training will
+	 * fail when the power sequencer is freshly used for this port.
+	 */
+	intel_dp->DP |= DP_PORT_EN;
+
+	I915_WRITE(intel_dp->output_reg, intel_dp->DP);
+	POSTING_READ(intel_dp->output_reg);
 }
 
 static void intel_enable_dp(struct intel_encoder *encoder)
@@ -2482,19 +2282,38 @@ static void intel_enable_dp(struct intel_encoder *encoder)
 	struct intel_dp *intel_dp = enc_to_intel_dp(&encoder->base);
 	struct drm_device *dev = encoder->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *crtc = to_intel_crtc(encoder->base.crtc);
 	uint32_t dp_reg = I915_READ(intel_dp->output_reg);
 
 	if (WARN_ON(dp_reg & DP_PORT_EN))
 		return;
 
+	pps_lock(intel_dp);
+
+	if (IS_VALLEYVIEW(dev))
+		vlv_init_panel_power_sequencer(intel_dp);
+
 	intel_dp_enable_port(intel_dp);
-	intel_edp_panel_vdd_on(intel_dp);
-	intel_edp_panel_on(intel_dp);
-	intel_edp_panel_vdd_off(intel_dp, true);
+
+	edp_panel_vdd_on(intel_dp);
+	edp_panel_on(intel_dp);
+	edp_panel_vdd_off(intel_dp, true);
+
+	pps_unlock(intel_dp);
+
+	if (IS_VALLEYVIEW(dev))
+		vlv_wait_port_ready(dev_priv, dp_to_dig_port(intel_dp));
+
 	intel_dp_sink_dpms(intel_dp, DRM_MODE_DPMS_ON);
 	intel_dp_start_link_train(intel_dp);
 	intel_dp_complete_link_train(intel_dp);
 	intel_dp_stop_link_train(intel_dp);
+
+	if (crtc->config.has_audio) {
+		DRM_DEBUG_DRIVER("Enabling DP audio on pipe %c\n",
+				 pipe_name(crtc->pipe));
+		intel_audio_codec_enable(encoder);
+	}
 }
 
 static void g4x_enable_dp(struct intel_encoder *encoder)
@@ -2526,6 +2345,32 @@ static void g4x_pre_enable_dp(struct intel_encoder *encoder)
 	}
 }
 
+static void vlv_detach_power_sequencer(struct intel_dp *intel_dp)
+{
+	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
+	struct drm_i915_private *dev_priv = intel_dig_port->base.base.dev->dev_private;
+	enum pipe pipe = intel_dp->pps_pipe;
+	int pp_on_reg = VLV_PIPE_PP_ON_DELAYS(pipe);
+
+	edp_panel_vdd_off_sync(intel_dp);
+
+	/*
+	 * VLV seems to get confused when multiple power seqeuencers
+	 * have the same port selected (even if only one has power/vdd
+	 * enabled). The failure manifests as vlv_wait_port_ready() failing
+	 * CHV on the other hand doesn't seem to mind having the same port
+	 * selected in multiple power seqeuencers, but let's clear the
+	 * port select always when logically disconnecting a power sequencer
+	 * from a port.
+	 */
+	DRM_DEBUG_KMS("detaching pipe %c power sequencer from port %c\n",
+		      pipe_name(pipe), port_name(intel_dig_port->port));
+	I915_WRITE(pp_on_reg, 0);
+	POSTING_READ(pp_on_reg);
+
+	intel_dp->pps_pipe = INVALID_PIPE;
+}
+
 static void vlv_steal_power_sequencer(struct drm_device *dev,
 				      enum pipe pipe)
 {
@@ -2534,6 +2379,9 @@ static void vlv_steal_power_sequencer(struct drm_device *dev,
 
 	lockdep_assert_held(&dev_priv->pps_mutex);
 
+	if (WARN_ON(pipe != PIPE_A && pipe != PIPE_B))
+		return;
+
 	list_for_each_entry(encoder, &dev->mode_config.encoder_list,
 			    base.head) {
 		struct intel_dp *intel_dp;
@@ -2551,10 +2399,12 @@ static void vlv_steal_power_sequencer(struct drm_device *dev,
 		DRM_DEBUG_KMS("stealing pipe %c power sequencer from port %c\n",
 			      pipe_name(pipe), port_name(port));
 
-		/* make sure vdd is off before we steal it */
-		edp_panel_vdd_off_sync(intel_dp);
+		WARN(encoder->connectors_active,
+		     "stealing pipe %c power sequencer from active eDP port %c\n",
+		     pipe_name(pipe), port_name(port));
 
-		intel_dp->pps_pipe = INVALID_PIPE;
+		/* make sure vdd is off before we steal it */
+		vlv_detach_power_sequencer(intel_dp);
 	}
 }
 
@@ -2565,10 +2415,12 @@ static void vlv_init_panel_power_sequencer(struct intel_dp *intel_dp)
 	struct drm_device *dev = encoder->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *crtc = to_intel_crtc(encoder->base.crtc);
-	struct edp_power_seq power_seq;
 
 	lockdep_assert_held(&dev_priv->pps_mutex);
 
+	if (!is_edp(intel_dp))
+		return;
+
 	if (intel_dp->pps_pipe == crtc->pipe)
 		return;
 
@@ -2578,7 +2430,7 @@ static void vlv_init_panel_power_sequencer(struct intel_dp *intel_dp)
 	 * we still have control of it.
 	 */
 	if (intel_dp->pps_pipe != INVALID_PIPE)
-		edp_panel_vdd_off_sync(intel_dp);
+		vlv_detach_power_sequencer(intel_dp);
 
 	/*
 	 * We may be stealing the power
@@ -2593,9 +2445,8 @@ static void vlv_init_panel_power_sequencer(struct intel_dp *intel_dp)
 		      pipe_name(intel_dp->pps_pipe), port_name(intel_dig_port->port));
 
 	/* init power sequencer on this pipe and port */
-	intel_dp_init_panel_power_sequencer(dev, intel_dp, &power_seq);
-	intel_dp_init_panel_power_sequencer_registers(dev, intel_dp,
-						      &power_seq);
+	intel_dp_init_panel_power_sequencer(dev, intel_dp);
+	intel_dp_init_panel_power_sequencer_registers(dev, intel_dp);
 }
 
 static void vlv_pre_enable_dp(struct intel_encoder *encoder)
@@ -2624,15 +2475,7 @@ static void vlv_pre_enable_dp(struct intel_encoder *encoder)
 
 	mutex_unlock(&dev_priv->dpio_lock);
 
-	if (is_edp(intel_dp)) {
-		pps_lock(intel_dp);
-		vlv_init_panel_power_sequencer(intel_dp);
-		pps_unlock(intel_dp);
-	}
-
 	intel_enable_dp(encoder);
-
-	vlv_wait_port_ready(dev_priv, dport);
 }
 
 static void vlv_dp_pre_pll_enable(struct intel_encoder *encoder)
@@ -2680,6 +2523,15 @@ static void chv_pre_enable_dp(struct intel_encoder *encoder)
 
 	mutex_lock(&dev_priv->dpio_lock);
 
+	/* allow hardware to manage TX FIFO reset source */
+	val = vlv_dpio_read(dev_priv, pipe, VLV_PCS01_DW11(ch));
+	val &= ~DPIO_LANEDESKEW_STRAP_OVRD;
+	vlv_dpio_write(dev_priv, pipe, VLV_PCS01_DW11(ch), val);
+
+	val = vlv_dpio_read(dev_priv, pipe, VLV_PCS23_DW11(ch));
+	val &= ~DPIO_LANEDESKEW_STRAP_OVRD;
+	vlv_dpio_write(dev_priv, pipe, VLV_PCS23_DW11(ch), val);
+
 	/* Deassert soft data lane reset*/
 	val = vlv_dpio_read(dev_priv, pipe, VLV_PCS01_DW1(ch));
 	val |= CHV_PCS_REQ_SOFTRESET_EN;
@@ -2715,15 +2567,7 @@ static void chv_pre_enable_dp(struct intel_encoder *encoder)
 
 	mutex_unlock(&dev_priv->dpio_lock);
 
-	if (is_edp(intel_dp)) {
-		pps_lock(intel_dp);
-		vlv_init_panel_power_sequencer(intel_dp);
-		pps_unlock(intel_dp);
-	}
-
 	intel_enable_dp(encoder);
-
-	vlv_wait_port_ready(dev_priv, dport);
 }
 
 static void chv_dp_pre_pll_enable(struct intel_encoder *encoder)
@@ -2843,7 +2687,9 @@ intel_dp_voltage_max(struct intel_dp *intel_dp)
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	enum port port = dp_to_dig_port(intel_dp)->port;
 
-	if (IS_VALLEYVIEW(dev))
+	if (INTEL_INFO(dev)->gen >= 9)
+		return DP_TRAIN_VOLTAGE_SWING_LEVEL_2;
+	else if (IS_VALLEYVIEW(dev))
 		return DP_TRAIN_VOLTAGE_SWING_LEVEL_3;
 	else if (IS_GEN7(dev) && port == PORT_A)
 		return DP_TRAIN_VOLTAGE_SWING_LEVEL_2;
@@ -2859,7 +2705,18 @@ intel_dp_pre_emphasis_max(struct intel_dp *intel_dp, uint8_t voltage_swing)
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	enum port port = dp_to_dig_port(intel_dp)->port;
 
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
+	if (INTEL_INFO(dev)->gen >= 9) {
+		switch (voltage_swing & DP_TRAIN_VOLTAGE_SWING_MASK) {
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
+			return DP_TRAIN_PRE_EMPH_LEVEL_3;
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_1:
+			return DP_TRAIN_PRE_EMPH_LEVEL_2;
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_2:
+			return DP_TRAIN_PRE_EMPH_LEVEL_1;
+		default:
+			return DP_TRAIN_PRE_EMPH_LEVEL_0;
+		}
+	} else if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
 		switch (voltage_swing & DP_TRAIN_VOLTAGE_SWING_MASK) {
 		case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
 			return DP_TRAIN_PRE_EMPH_LEVEL_3;
@@ -3095,12 +2952,26 @@ static uint32_t intel_chv_signal_levels(struct intel_dp *intel_dp)
 	/* Clear calc init */
 	val = vlv_dpio_read(dev_priv, pipe, VLV_PCS01_DW10(ch));
 	val &= ~(DPIO_PCS_SWING_CALC_TX0_TX2 | DPIO_PCS_SWING_CALC_TX1_TX3);
+	val &= ~(DPIO_PCS_TX1DEEMP_MASK | DPIO_PCS_TX2DEEMP_MASK);
+	val |= DPIO_PCS_TX1DEEMP_9P5 | DPIO_PCS_TX2DEEMP_9P5;
 	vlv_dpio_write(dev_priv, pipe, VLV_PCS01_DW10(ch), val);
 
 	val = vlv_dpio_read(dev_priv, pipe, VLV_PCS23_DW10(ch));
 	val &= ~(DPIO_PCS_SWING_CALC_TX0_TX2 | DPIO_PCS_SWING_CALC_TX1_TX3);
+	val &= ~(DPIO_PCS_TX1DEEMP_MASK | DPIO_PCS_TX2DEEMP_MASK);
+	val |= DPIO_PCS_TX1DEEMP_9P5 | DPIO_PCS_TX2DEEMP_9P5;
 	vlv_dpio_write(dev_priv, pipe, VLV_PCS23_DW10(ch), val);
 
+	val = vlv_dpio_read(dev_priv, pipe, VLV_PCS01_DW9(ch));
+	val &= ~(DPIO_PCS_TX1MARGIN_MASK | DPIO_PCS_TX2MARGIN_MASK);
+	val |= DPIO_PCS_TX1MARGIN_000 | DPIO_PCS_TX2MARGIN_000;
+	vlv_dpio_write(dev_priv, pipe, VLV_PCS01_DW9(ch), val);
+
+	val = vlv_dpio_read(dev_priv, pipe, VLV_PCS23_DW9(ch));
+	val &= ~(DPIO_PCS_TX1MARGIN_MASK | DPIO_PCS_TX2MARGIN_MASK);
+	val |= DPIO_PCS_TX1MARGIN_000 | DPIO_PCS_TX2MARGIN_000;
+	vlv_dpio_write(dev_priv, pipe, VLV_PCS23_DW9(ch), val);
+
 	/* Program swing deemph */
 	for (i = 0; i < 4; i++) {
 		val = vlv_dpio_read(dev_priv, pipe, CHV_TX_DW4(ch, i));
@@ -3341,7 +3212,7 @@ intel_dp_set_signal_levels(struct intel_dp *intel_dp, uint32_t *DP)
 	uint32_t signal_levels, mask;
 	uint8_t train_set = intel_dp->train_set[0];
 
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
+	if (IS_HASWELL(dev) || IS_BROADWELL(dev) || INTEL_INFO(dev)->gen >= 9) {
 		signal_levels = intel_hsw_signal_levels(train_set);
 		mask = DDI_BUF_EMP_MASK;
 	} else if (IS_CHERRYVIEW(dev)) {
@@ -3605,7 +3476,6 @@ intel_dp_complete_link_train(struct intel_dp *intel_dp)
 
 		/* Try 5 times, then try clock recovery if that fails */
 		if (tries > 5) {
-			intel_dp_link_down(intel_dp);
 			intel_dp_start_link_train(intel_dp);
 			intel_dp_set_link_train(intel_dp, &DP,
 						training_pattern |
@@ -3763,8 +3633,6 @@ intel_dp_probe_oui(struct intel_dp *intel_dp)
 	if (!(intel_dp->dpcd[DP_DOWN_STREAM_PORT_COUNT] & DP_OUI_SUPPORT))
 		return;
 
-	intel_edp_panel_vdd_on(intel_dp);
-
 	if (intel_dp_dpcd_read_wake(&intel_dp->aux, DP_SINK_OUI, buf, 3) == 3)
 		DRM_DEBUG_KMS("Sink OUI: %02hx%02hx%02hx\n",
 			      buf[0], buf[1], buf[2]);
@@ -3772,8 +3640,6 @@ intel_dp_probe_oui(struct intel_dp *intel_dp)
 	if (intel_dp_dpcd_read_wake(&intel_dp->aux, DP_BRANCH_OUI, buf, 3) == 3)
 		DRM_DEBUG_KMS("Branch OUI: %02hx%02hx%02hx\n",
 			      buf[0], buf[1], buf[2]);
-
-	intel_edp_panel_vdd_off(intel_dp, false);
 }
 
 static bool
@@ -3787,7 +3653,6 @@ intel_dp_probe_mst(struct intel_dp *intel_dp)
 	if (intel_dp->dpcd[DP_DPCD_REV] < 0x12)
 		return false;
 
-	intel_edp_panel_vdd_on(intel_dp);
 	if (intel_dp_dpcd_read_wake(&intel_dp->aux, DP_MSTM_CAP, buf, 1)) {
 		if (buf[0] & DP_MST_CAP) {
 			DRM_DEBUG_KMS("Sink is MST capable\n");
@@ -3797,7 +3662,6 @@ intel_dp_probe_mst(struct intel_dp *intel_dp)
 			intel_dp->is_mst = false;
 		}
 	}
-	intel_edp_panel_vdd_off(intel_dp, false);
 
 	drm_dp_mst_topology_mgr_set_mst(&intel_dp->mst_mgr, intel_dp->is_mst);
 	return intel_dp->is_mst;
@@ -3809,26 +3673,48 @@ int intel_dp_sink_crc(struct intel_dp *intel_dp, u8 *crc)
 	struct drm_device *dev = intel_dig_port->base.base.dev;
 	struct intel_crtc *intel_crtc =
 		to_intel_crtc(intel_dig_port->base.base.crtc);
-	u8 buf[1];
+	u8 buf;
+	int test_crc_count;
+	int attempts = 6;
 
-	if (drm_dp_dpcd_readb(&intel_dp->aux, DP_TEST_SINK_MISC, buf) < 0)
+	if (drm_dp_dpcd_readb(&intel_dp->aux, DP_TEST_SINK_MISC, &buf) < 0)
 		return -EIO;
 
-	if (!(buf[0] & DP_TEST_CRC_SUPPORTED))
+	if (!(buf & DP_TEST_CRC_SUPPORTED))
 		return -ENOTTY;
 
+	if (drm_dp_dpcd_readb(&intel_dp->aux, DP_TEST_SINK, &buf) < 0)
+		return -EIO;
+
 	if (drm_dp_dpcd_writeb(&intel_dp->aux, DP_TEST_SINK,
-			       DP_TEST_SINK_START) < 0)
+				buf | DP_TEST_SINK_START) < 0)
+		return -EIO;
+
+	if (drm_dp_dpcd_readb(&intel_dp->aux, DP_TEST_SINK_MISC, &buf) < 0)
 		return -EIO;
+	test_crc_count = buf & DP_TEST_COUNT_MASK;
 
-	/* Wait 2 vblanks to be sure we will have the correct CRC value */
-	intel_wait_for_vblank(dev, intel_crtc->pipe);
-	intel_wait_for_vblank(dev, intel_crtc->pipe);
+	do {
+		if (drm_dp_dpcd_readb(&intel_dp->aux,
+				      DP_TEST_SINK_MISC, &buf) < 0)
+			return -EIO;
+		intel_wait_for_vblank(dev, intel_crtc->pipe);
+	} while (--attempts && (buf & DP_TEST_COUNT_MASK) == test_crc_count);
+
+	if (attempts == 0) {
+		DRM_DEBUG_KMS("Panel is unable to calculate CRC after 6 vblanks\n");
+		return -ETIMEDOUT;
+	}
 
 	if (drm_dp_dpcd_read(&intel_dp->aux, DP_TEST_CRC_R_CR, crc, 6) < 0)
 		return -EIO;
 
-	drm_dp_dpcd_writeb(&intel_dp->aux, DP_TEST_SINK, 0);
+	if (drm_dp_dpcd_readb(&intel_dp->aux, DP_TEST_SINK, &buf) < 0)
+		return -EIO;
+	if (drm_dp_dpcd_writeb(&intel_dp->aux, DP_TEST_SINK,
+			       buf & ~DP_TEST_SINK_START) < 0)
+		return -EIO;
+
 	return 0;
 }
 
@@ -4456,9 +4342,52 @@ static void intel_dp_encoder_suspend(struct intel_encoder *intel_encoder)
 	pps_unlock(intel_dp);
 }
 
+static void intel_edp_panel_vdd_sanitize(struct intel_dp *intel_dp)
+{
+	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
+	struct drm_device *dev = intel_dig_port->base.base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	enum intel_display_power_domain power_domain;
+
+	lockdep_assert_held(&dev_priv->pps_mutex);
+
+	if (!edp_have_panel_vdd(intel_dp))
+		return;
+
+	/*
+	 * The VDD bit needs a power domain reference, so if the bit is
+	 * already enabled when we boot or resume, grab this reference and
+	 * schedule a vdd off, so we don't hold on to the reference
+	 * indefinitely.
+	 */
+	DRM_DEBUG_KMS("VDD left on by BIOS, adjusting state tracking\n");
+	power_domain = intel_display_port_power_domain(&intel_dig_port->base);
+	intel_display_power_get(dev_priv, power_domain);
+
+	edp_panel_vdd_schedule_off(intel_dp);
+}
+
 static void intel_dp_encoder_reset(struct drm_encoder *encoder)
 {
-	intel_edp_panel_vdd_sanitize(to_intel_encoder(encoder));
+	struct intel_dp *intel_dp;
+
+	if (to_intel_encoder(encoder)->type != INTEL_OUTPUT_EDP)
+		return;
+
+	intel_dp = enc_to_intel_dp(encoder);
+
+	pps_lock(intel_dp);
+
+	/*
+	 * Read out the current power sequencer assignment,
+	 * in case the BIOS did something with it.
+	 */
+	if (IS_VALLEYVIEW(encoder->dev))
+		vlv_initial_power_sequencer_setup(intel_dp);
+
+	intel_edp_panel_vdd_sanitize(intel_dp);
+
+	pps_unlock(intel_dp);
 }
 
 static const struct drm_connector_funcs intel_dp_connector_funcs = {
@@ -4645,16 +4574,20 @@ static void intel_dp_init_panel_power_timestamps(struct intel_dp *intel_dp)
 
 static void
 intel_dp_init_panel_power_sequencer(struct drm_device *dev,
-				    struct intel_dp *intel_dp,
-				    struct edp_power_seq *out)
+				    struct intel_dp *intel_dp)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct edp_power_seq cur, vbt, spec, final;
+	struct edp_power_seq cur, vbt, spec,
+		*final = &intel_dp->pps_delays;
 	u32 pp_on, pp_off, pp_div, pp;
 	int pp_ctrl_reg, pp_on_reg, pp_off_reg, pp_div_reg;
 
 	lockdep_assert_held(&dev_priv->pps_mutex);
 
+	/* already initialized? */
+	if (final->t11_t12 != 0)
+		return;
+
 	if (HAS_PCH_SPLIT(dev)) {
 		pp_ctrl_reg = PCH_PP_CONTROL;
 		pp_on_reg = PCH_PP_ON_DELAYS;
@@ -4716,7 +4649,7 @@ intel_dp_init_panel_power_sequencer(struct drm_device *dev,
 
 	/* Use the max of the register settings and vbt. If both are
 	 * unset, fall back to the spec limits. */
-#define assign_final(field)	final.field = (max(cur.field, vbt.field) == 0 ? \
+#define assign_final(field)	final->field = (max(cur.field, vbt.field) == 0 ? \
 				       spec.field : \
 				       max(cur.field, vbt.field))
 	assign_final(t1_t3);
@@ -4726,7 +4659,7 @@ intel_dp_init_panel_power_sequencer(struct drm_device *dev,
 	assign_final(t11_t12);
 #undef assign_final
 
-#define get_delay(field)	(DIV_ROUND_UP(final.field, 10))
+#define get_delay(field)	(DIV_ROUND_UP(final->field, 10))
 	intel_dp->panel_power_up_delay = get_delay(t1_t3);
 	intel_dp->backlight_on_delay = get_delay(t8);
 	intel_dp->backlight_off_delay = get_delay(t9);
@@ -4740,21 +4673,18 @@ intel_dp_init_panel_power_sequencer(struct drm_device *dev,
 
 	DRM_DEBUG_KMS("backlight on delay %d, off delay %d\n",
 		      intel_dp->backlight_on_delay, intel_dp->backlight_off_delay);
-
-	if (out)
-		*out = final;
 }
 
 static void
 intel_dp_init_panel_power_sequencer_registers(struct drm_device *dev,
-					      struct intel_dp *intel_dp,
-					      struct edp_power_seq *seq)
+					      struct intel_dp *intel_dp)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 pp_on, pp_off, pp_div, port_sel = 0;
 	int div = HAS_PCH_SPLIT(dev) ? intel_pch_rawclk(dev) : intel_hrawclk(dev);
 	int pp_on_reg, pp_off_reg, pp_div_reg;
 	enum port port = dp_to_dig_port(intel_dp)->port;
+	const struct edp_power_seq *seq = &intel_dp->pps_delays;
 
 	lockdep_assert_held(&dev_priv->pps_mutex);
 
@@ -4837,7 +4767,7 @@ void intel_dp_set_drrs_state(struct drm_device *dev, int refresh_rate)
 	 * hard to tell without seeing the user of this function of this code.
 	 * Check locking and ordering once that lands.
 	 */
-	if (INTEL_INFO(dev)->gen < 8 && intel_edp_is_psr_enabled(dev)) {
+	if (INTEL_INFO(dev)->gen < 8 && intel_psr_is_enabled(dev)) {
 		DRM_DEBUG_KMS("DRRS is disabled as PSR is enabled\n");
 		return;
 	}
@@ -4940,40 +4870,8 @@ intel_dp_drrs_init(struct intel_digital_port *intel_dig_port,
 	return downclock_mode;
 }
 
-void intel_edp_panel_vdd_sanitize(struct intel_encoder *intel_encoder)
-{
-	struct drm_device *dev = intel_encoder->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_dp *intel_dp;
-	enum intel_display_power_domain power_domain;
-
-	if (intel_encoder->type != INTEL_OUTPUT_EDP)
-		return;
-
-	intel_dp = enc_to_intel_dp(&intel_encoder->base);
-
-	pps_lock(intel_dp);
-
-	if (!edp_have_panel_vdd(intel_dp))
-		goto out;
-	/*
-	 * The VDD bit needs a power domain reference, so if the bit is
-	 * already enabled when we boot or resume, grab this reference and
-	 * schedule a vdd off, so we don't hold on to the reference
-	 * indefinitely.
-	 */
-	DRM_DEBUG_KMS("VDD left on by BIOS, adjusting state tracking\n");
-	power_domain = intel_display_port_power_domain(intel_encoder);
-	intel_display_power_get(dev_priv, power_domain);
-
-	edp_panel_vdd_schedule_off(intel_dp);
- out:
-	pps_unlock(intel_dp);
-}
-
 static bool intel_edp_init_connector(struct intel_dp *intel_dp,
-				     struct intel_connector *intel_connector,
-				     struct edp_power_seq *power_seq)
+				     struct intel_connector *intel_connector)
 {
 	struct drm_connector *connector = &intel_connector->base;
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
@@ -4985,18 +4883,19 @@ static bool intel_edp_init_connector(struct intel_dp *intel_dp,
 	bool has_dpcd;
 	struct drm_display_mode *scan;
 	struct edid *edid;
+	enum pipe pipe = INVALID_PIPE;
 
 	intel_dp->drrs_state.type = DRRS_NOT_SUPPORTED;
 
 	if (!is_edp(intel_dp))
 		return true;
 
-	intel_edp_panel_vdd_sanitize(intel_encoder);
+	pps_lock(intel_dp);
+	intel_edp_panel_vdd_sanitize(intel_dp);
+	pps_unlock(intel_dp);
 
 	/* Cache DPCD and EDID for edp. */
-	intel_edp_panel_vdd_on(intel_dp);
 	has_dpcd = intel_dp_get_dpcd(intel_dp);
-	intel_edp_panel_vdd_off(intel_dp, false);
 
 	if (has_dpcd) {
 		if (intel_dp->dpcd[DP_DPCD_REV] >= 0x11)
@@ -5011,7 +4910,7 @@ static bool intel_edp_init_connector(struct intel_dp *intel_dp,
 
 	/* We now know it's not a ghost, init power sequence regs. */
 	pps_lock(intel_dp);
-	intel_dp_init_panel_power_sequencer_registers(dev, intel_dp, power_seq);
+	intel_dp_init_panel_power_sequencer_registers(dev, intel_dp);
 	pps_unlock(intel_dp);
 
 	mutex_lock(&dev->mode_config.mutex);
@@ -5053,11 +4952,30 @@ static bool intel_edp_init_connector(struct intel_dp *intel_dp,
 	if (IS_VALLEYVIEW(dev)) {
 		intel_dp->edp_notifier.notifier_call = edp_notify_handler;
 		register_reboot_notifier(&intel_dp->edp_notifier);
+
+		/*
+		 * Figure out the current pipe for the initial backlight setup.
+		 * If the current pipe isn't valid, try the PPS pipe, and if that
+		 * fails just assume pipe A.
+		 */
+		if (IS_CHERRYVIEW(dev))
+			pipe = DP_PORT_TO_PIPE_CHV(intel_dp->DP);
+		else
+			pipe = PORT_TO_PIPE(intel_dp->DP);
+
+		if (pipe != PIPE_A && pipe != PIPE_B)
+			pipe = intel_dp->pps_pipe;
+
+		if (pipe != PIPE_A && pipe != PIPE_B)
+			pipe = PIPE_A;
+
+		DRM_DEBUG_KMS("using pipe %c for initial backlight setup\n",
+			      pipe_name(pipe));
 	}
 
 	intel_panel_init(&intel_connector->panel, fixed_mode, downclock_mode);
 	intel_connector->panel.backlight_power = intel_edp_backlight_power;
-	intel_panel_setup_backlight(connector);
+	intel_panel_setup_backlight(connector, pipe);
 
 	return true;
 }
@@ -5072,13 +4990,14 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	struct drm_device *dev = intel_encoder->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	enum port port = intel_dig_port->port;
-	struct edp_power_seq power_seq = { 0 };
 	int type;
 
 	intel_dp->pps_pipe = INVALID_PIPE;
 
 	/* intel_dp vfuncs */
-	if (IS_VALLEYVIEW(dev))
+	if (INTEL_INFO(dev)->gen >= 9)
+		intel_dp->get_aux_clock_divider = skl_get_aux_clock_divider;
+	else if (IS_VALLEYVIEW(dev))
 		intel_dp->get_aux_clock_divider = vlv_get_aux_clock_divider;
 	else if (IS_HASWELL(dev) || IS_BROADWELL(dev))
 		intel_dp->get_aux_clock_divider = hsw_get_aux_clock_divider;
@@ -5087,7 +5006,10 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	else
 		intel_dp->get_aux_clock_divider = i9xx_get_aux_clock_divider;
 
-	intel_dp->get_aux_send_ctl = i9xx_get_aux_send_ctl;
+	if (INTEL_INFO(dev)->gen >= 9)
+		intel_dp->get_aux_send_ctl = skl_get_aux_send_ctl;
+	else
+		intel_dp->get_aux_send_ctl = i9xx_get_aux_send_ctl;
 
 	/* Preserve the current hw state. */
 	intel_dp->DP = I915_READ(intel_dp->output_reg);
@@ -5106,6 +5028,11 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	if (type == DRM_MODE_CONNECTOR_eDP)
 		intel_encoder->type = INTEL_OUTPUT_EDP;
 
+	/* eDP only on port B and/or C on vlv/chv */
+	if (WARN_ON(IS_VALLEYVIEW(dev) && is_edp(intel_dp) &&
+		    port != PORT_B && port != PORT_C))
+		return false;
+
 	DRM_DEBUG_KMS("Adding %s connector on port %c\n",
 			type == DRM_MODE_CONNECTOR_eDP ? "eDP" : "DP",
 			port_name(port));
@@ -5148,13 +5075,11 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 
 	if (is_edp(intel_dp)) {
 		pps_lock(intel_dp);
-		if (IS_VALLEYVIEW(dev)) {
+		intel_dp_init_panel_power_timestamps(intel_dp);
+		if (IS_VALLEYVIEW(dev))
 			vlv_initial_power_sequencer_setup(intel_dp);
-		} else {
-			intel_dp_init_panel_power_timestamps(intel_dp);
-			intel_dp_init_panel_power_sequencer(dev, intel_dp,
-							    &power_seq);
-		}
+		else
+			intel_dp_init_panel_power_sequencer(dev, intel_dp);
 		pps_unlock(intel_dp);
 	}
 
@@ -5168,7 +5093,7 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 		}
 	}
 
-	if (!intel_edp_init_connector(intel_dp, intel_connector, &power_seq)) {
+	if (!intel_edp_init_connector(intel_dp, intel_connector)) {
 		drm_dp_aux_unregister(&intel_dp->aux);
 		if (is_edp(intel_dp)) {
 			cancel_delayed_work_sync(&intel_dp->panel_vdd_work);
diff --git a/drivers/gpu/drm/i915/intel_dp_mst.c b/drivers/gpu/drm/i915/intel_dp_mst.c
index d9a7a7865f66..7f8c6a66680a 100644
--- a/drivers/gpu/drm/i915/intel_dp_mst.c
+++ b/drivers/gpu/drm/i915/intel_dp_mst.c
@@ -278,20 +278,12 @@ static int intel_dp_mst_get_ddc_modes(struct drm_connector *connector)
 }
 
 static enum drm_connector_status
-intel_mst_port_dp_detect(struct drm_connector *connector)
+intel_dp_mst_detect(struct drm_connector *connector, bool force)
 {
 	struct intel_connector *intel_connector = to_intel_connector(connector);
 	struct intel_dp *intel_dp = intel_connector->mst_port;
 
-	return drm_dp_mst_detect_port(&intel_dp->mst_mgr, intel_connector->port);
-}
-
-static enum drm_connector_status
-intel_dp_mst_detect(struct drm_connector *connector, bool force)
-{
-	enum drm_connector_status status;
-	status = intel_mst_port_dp_detect(connector);
-	return status;
+	return drm_dp_mst_detect_port(connector, &intel_dp->mst_mgr, intel_connector->port);
 }
 
 static int
@@ -393,7 +385,7 @@ static void intel_connector_remove_from_fbdev(struct intel_connector *connector)
 #endif
 }
 
-static struct drm_connector *intel_dp_add_mst_connector(struct drm_dp_mst_topology_mgr *mgr, struct drm_dp_mst_port *port, char *pathprop)
+static struct drm_connector *intel_dp_add_mst_connector(struct drm_dp_mst_topology_mgr *mgr, struct drm_dp_mst_port *port, const char *pathprop)
 {
 	struct intel_dp *intel_dp = container_of(mgr, struct intel_dp, mst_mgr);
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
@@ -422,6 +414,8 @@ static struct drm_connector *intel_dp_add_mst_connector(struct drm_dp_mst_topolo
 	intel_dp_add_properties(intel_dp, connector);
 
 	drm_object_attach_property(&connector->base, dev->mode_config.path_property, 0);
+	drm_object_attach_property(&connector->base, dev->mode_config.tile_property, 0);
+
 	drm_mode_connector_set_path_property(connector, pathprop);
 	drm_reinit_primary_mode_group(dev);
 	mutex_lock(&dev->mode_config.mutex);
diff --git a/drivers/gpu/drm/i915/intel_drv.h b/drivers/gpu/drm/i915/intel_drv.h
index ba715229a540..25fdbb16d4e0 100644
--- a/drivers/gpu/drm/i915/intel_drv.h
+++ b/drivers/gpu/drm/i915/intel_drv.h
@@ -34,6 +34,7 @@
 #include <drm/drm_crtc_helper.h>
 #include <drm/drm_fb_helper.h>
 #include <drm/drm_dp_mst_helper.h>
+#include <drm/drm_rect.h>
 
 #define DIV_ROUND_CLOSEST_ULL(ll, d)	\
 ({ unsigned long long _tmp = (ll)+(d)/2; do_div(_tmp, d); _tmp; })
@@ -93,18 +94,20 @@
 
 /* these are outputs from the chip - integrated only
    external chips are via DVO or SDVO output */
-#define INTEL_OUTPUT_UNUSED 0
-#define INTEL_OUTPUT_ANALOG 1
-#define INTEL_OUTPUT_DVO 2
-#define INTEL_OUTPUT_SDVO 3
-#define INTEL_OUTPUT_LVDS 4
-#define INTEL_OUTPUT_TVOUT 5
-#define INTEL_OUTPUT_HDMI 6
-#define INTEL_OUTPUT_DISPLAYPORT 7
-#define INTEL_OUTPUT_EDP 8
-#define INTEL_OUTPUT_DSI 9
-#define INTEL_OUTPUT_UNKNOWN 10
-#define INTEL_OUTPUT_DP_MST 11
+enum intel_output_type {
+	INTEL_OUTPUT_UNUSED = 0,
+	INTEL_OUTPUT_ANALOG = 1,
+	INTEL_OUTPUT_DVO = 2,
+	INTEL_OUTPUT_SDVO = 3,
+	INTEL_OUTPUT_LVDS = 4,
+	INTEL_OUTPUT_TVOUT = 5,
+	INTEL_OUTPUT_HDMI = 6,
+	INTEL_OUTPUT_DISPLAYPORT = 7,
+	INTEL_OUTPUT_EDP = 8,
+	INTEL_OUTPUT_DSI = 9,
+	INTEL_OUTPUT_UNKNOWN = 10,
+	INTEL_OUTPUT_DP_MST = 11,
+};
 
 #define INTEL_DVO_CHIP_NONE 0
 #define INTEL_DVO_CHIP_LVDS 1
@@ -135,7 +138,7 @@ struct intel_encoder {
 	 */
 	struct intel_crtc *new_crtc;
 
-	int type;
+	enum intel_output_type type;
 	unsigned int cloneable;
 	bool connectors_active;
 	void (*hot_plug)(struct intel_encoder *);
@@ -240,6 +243,17 @@ typedef struct dpll {
 	int	p;
 } intel_clock_t;
 
+struct intel_plane_state {
+	struct drm_crtc *crtc;
+	struct drm_framebuffer *fb;
+	struct drm_rect src;
+	struct drm_rect dst;
+	struct drm_rect clip;
+	struct drm_rect orig_src;
+	struct drm_rect orig_dst;
+	bool visible;
+};
+
 struct intel_plane_config {
 	bool tiled;
 	int size;
@@ -278,6 +292,9 @@ struct intel_crtc_config {
 	 * between pch encoders and cpu encoders. */
 	bool has_pch_encoder;
 
+	/* Are we sending infoframes on the attached port */
+	bool has_infoframe;
+
 	/* CPU Transcoder for the pipe. Currently this can only differ from the
 	 * pipe on Haswell (where we have a special eDP transcoder). */
 	enum transcoder cpu_transcoder;
@@ -326,7 +343,10 @@ struct intel_crtc_config {
 	/* Selected dpll when shared or DPLL_ID_PRIVATE. */
 	enum intel_dpll_id shared_dpll;
 
-	/* PORT_CLK_SEL for DDI ports. */
+	/*
+	 * - PORT_CLK_SEL for DDI ports on HSW/BDW.
+	 * - enum skl_dpll on SKL
+	 */
 	uint32_t ddi_pll_sel;
 
 	/* Actual register state of the dpll, for shared dpll cross-checking. */
@@ -387,7 +407,14 @@ struct intel_pipe_wm {
 
 struct intel_mmio_flip {
 	u32 seqno;
-	u32 ring_id;
+	struct intel_engine_cs *ring;
+	struct work_struct work;
+};
+
+struct skl_pipe_wm {
+	struct skl_wm_level wm[8];
+	struct skl_wm_level trans_wm;
+	uint32_t linetime;
 };
 
 struct intel_crtc {
@@ -437,6 +464,8 @@ struct intel_crtc {
 	struct {
 		/* watermarks currently being used  */
 		struct intel_pipe_wm active;
+		/* SKL wm values currently in use */
+		struct skl_pipe_wm skl_active;
 	} wm;
 
 	int scanline_offset;
@@ -529,6 +558,7 @@ struct intel_hdmi {
 	void (*set_infoframes)(struct drm_encoder *encoder,
 			       bool enable,
 			       struct drm_display_mode *adjusted_mode);
+	bool (*infoframe_enabled)(struct drm_encoder *encoder);
 };
 
 struct intel_dp_mst_encoder;
@@ -578,6 +608,7 @@ struct intel_dp {
 	 * this port. Only relevant on VLV/CHV.
 	 */
 	enum pipe pps_pipe;
+	struct edp_power_seq pps_delays;
 
 	bool use_tps3;
 	bool can_mst; /* this port supports mst */
@@ -734,32 +765,47 @@ hdmi_to_dig_port(struct intel_hdmi *intel_hdmi)
 	return container_of(intel_hdmi, struct intel_digital_port, hdmi);
 }
 
+/*
+ * Returns the number of planes for this pipe, ie the number of sprites + 1
+ * (primary plane). This doesn't count the cursor plane then.
+ */
+static inline unsigned int intel_num_planes(struct intel_crtc *crtc)
+{
+	return INTEL_INFO(crtc->base.dev)->num_sprites[crtc->pipe] + 1;
+}
 
-/* i915_irq.c */
-bool intel_set_cpu_fifo_underrun_reporting(struct drm_device *dev,
+/* intel_fifo_underrun.c */
+bool intel_set_cpu_fifo_underrun_reporting(struct drm_i915_private *dev_priv,
 					   enum pipe pipe, bool enable);
-bool intel_set_pch_fifo_underrun_reporting(struct drm_device *dev,
+bool intel_set_pch_fifo_underrun_reporting(struct drm_i915_private *dev_priv,
 					   enum transcoder pch_transcoder,
 					   bool enable);
+void intel_cpu_fifo_underrun_irq_handler(struct drm_i915_private *dev_priv,
+					 enum pipe pipe);
+void intel_pch_fifo_underrun_irq_handler(struct drm_i915_private *dev_priv,
+					 enum transcoder pch_transcoder);
+void i9xx_check_fifo_underruns(struct drm_i915_private *dev_priv);
+
+/* i915_irq.c */
 void gen5_enable_gt_irq(struct drm_i915_private *dev_priv, uint32_t mask);
 void gen5_disable_gt_irq(struct drm_i915_private *dev_priv, uint32_t mask);
 void gen6_enable_pm_irq(struct drm_i915_private *dev_priv, uint32_t mask);
 void gen6_disable_pm_irq(struct drm_i915_private *dev_priv, uint32_t mask);
-void gen8_enable_pm_irq(struct drm_i915_private *dev_priv, uint32_t mask);
-void gen8_disable_pm_irq(struct drm_i915_private *dev_priv, uint32_t mask);
-void intel_runtime_pm_disable_interrupts(struct drm_device *dev);
-void intel_runtime_pm_restore_interrupts(struct drm_device *dev);
+void gen6_reset_rps_interrupts(struct drm_device *dev);
+void gen6_enable_rps_interrupts(struct drm_device *dev);
+void gen6_disable_rps_interrupts(struct drm_device *dev);
+void intel_runtime_pm_disable_interrupts(struct drm_i915_private *dev_priv);
+void intel_runtime_pm_enable_interrupts(struct drm_i915_private *dev_priv);
 static inline bool intel_irqs_enabled(struct drm_i915_private *dev_priv)
 {
 	/*
 	 * We only use drm_irq_uninstall() at unload and VT switch, so
 	 * this is the only thing we need to check.
 	 */
-	return !dev_priv->pm._irqs_disabled;
+	return dev_priv->pm.irqs_enabled;
 }
 
 int intel_get_crtc_scanline(struct intel_crtc *crtc);
-void i9xx_check_fifo_underruns(struct drm_device *dev);
 void gen8_irq_power_well_post_enable(struct drm_i915_private *dev_priv);
 
 /* intel_crt.c */
@@ -792,11 +838,7 @@ void intel_ddi_clock_get(struct intel_encoder *encoder,
 			 struct intel_crtc_config *pipe_config);
 void intel_ddi_set_vc_payload_alloc(struct drm_crtc *crtc, bool state);
 
-/* intel_display.c */
-const char *intel_output_name(int output);
-bool intel_has_pending_fb_unpin(struct drm_device *dev);
-int intel_pch_rawclk(struct drm_device *dev);
-void intel_mark_busy(struct drm_device *dev);
+/* intel_frontbuffer.c */
 void intel_fb_obj_invalidate(struct drm_i915_gem_object *obj,
 			     struct intel_engine_cs *ring);
 void intel_frontbuffer_flip_prepare(struct drm_device *dev,
@@ -806,7 +848,7 @@ void intel_frontbuffer_flip_complete(struct drm_device *dev,
 void intel_frontbuffer_flush(struct drm_device *dev,
 			     unsigned frontbuffer_bits);
 /**
- * intel_frontbuffer_flip - prepare frontbuffer flip
+ * intel_frontbuffer_flip - synchronous frontbuffer flip
  * @dev: DRM device
  * @frontbuffer_bits: frontbuffer plane tracking bits
  *
@@ -824,6 +866,18 @@ void intel_frontbuffer_flip(struct drm_device *dev,
 }
 
 void intel_fb_obj_flush(struct drm_i915_gem_object *obj, bool retire);
+
+
+/* intel_audio.c */
+void intel_init_audio(struct drm_device *dev);
+void intel_audio_codec_enable(struct intel_encoder *encoder);
+void intel_audio_codec_disable(struct intel_encoder *encoder);
+
+/* intel_display.c */
+const char *intel_output_name(int output);
+bool intel_has_pending_fb_unpin(struct drm_device *dev);
+int intel_pch_rawclk(struct drm_device *dev);
+void intel_mark_busy(struct drm_device *dev);
 void intel_mark_idle(struct drm_device *dev);
 void intel_crtc_restore_mode(struct drm_crtc *crtc);
 void intel_crtc_control(struct drm_crtc *crtc, bool enable);
@@ -844,7 +898,12 @@ int intel_get_pipe_from_crtc_id(struct drm_device *dev, void *data,
 				struct drm_file *file_priv);
 enum transcoder intel_pipe_to_cpu_transcoder(struct drm_i915_private *dev_priv,
 					     enum pipe pipe);
-void intel_wait_for_vblank(struct drm_device *dev, int pipe);
+bool intel_pipe_has_type(struct intel_crtc *crtc, enum intel_output_type type);
+static inline void
+intel_wait_for_vblank(struct drm_device *dev, int pipe)
+{
+	drm_wait_one_vblank(dev, pipe);
+}
 int ironlake_get_lanes_required(int target_clock, int link_bw, int bpp);
 void vlv_wait_port_ready(struct drm_i915_private *dev_priv,
 			 struct intel_digital_port *dport);
@@ -854,8 +913,8 @@ bool intel_get_load_detect_pipe(struct drm_connector *connector,
 				struct drm_modeset_acquire_ctx *ctx);
 void intel_release_load_detect_pipe(struct drm_connector *connector,
 				    struct intel_load_detect_pipe *old);
-int intel_pin_and_fence_fb_obj(struct drm_device *dev,
-			       struct drm_i915_gem_object *obj,
+int intel_pin_and_fence_fb_obj(struct drm_plane *plane,
+			       struct drm_framebuffer *fb,
 			       struct intel_engine_cs *pipelined);
 void intel_unpin_fb_obj(struct drm_i915_gem_object *obj);
 struct drm_framebuffer *
@@ -877,7 +936,13 @@ void assert_shared_dpll(struct drm_i915_private *dev_priv,
 struct intel_shared_dpll *intel_get_shared_dpll(struct intel_crtc *crtc);
 void intel_put_shared_dpll(struct intel_crtc *crtc);
 
+void vlv_force_pll_on(struct drm_device *dev, enum pipe pipe,
+		      const struct dpll *dpll);
+void vlv_force_pll_off(struct drm_device *dev, enum pipe pipe);
+
 /* modesetting asserts */
+void assert_panel_unlocked(struct drm_i915_private *dev_priv,
+			   enum pipe pipe);
 void assert_pll(struct drm_i915_private *dev_priv,
 		enum pipe pipe, bool state);
 #define assert_pll_enabled(d, p) assert_pll(d, p, true)
@@ -889,13 +954,12 @@ void assert_fdi_rx_pll(struct drm_i915_private *dev_priv,
 void assert_pipe(struct drm_i915_private *dev_priv, enum pipe pipe, bool state);
 #define assert_pipe_enabled(d, p) assert_pipe(d, p, true)
 #define assert_pipe_disabled(d, p) assert_pipe(d, p, false)
-void intel_write_eld(struct drm_encoder *encoder,
-		     struct drm_display_mode *mode);
 unsigned long intel_gen4_compute_page_offset(int *x, int *y,
 					     unsigned int tiling_mode,
 					     unsigned int bpp,
 					     unsigned int pitch);
-void intel_display_handle_reset(struct drm_device *dev);
+void intel_prepare_reset(struct drm_device *dev);
+void intel_finish_reset(struct drm_device *dev);
 void hsw_enable_pc8(struct drm_i915_private *dev_priv);
 void hsw_disable_pc8(struct drm_i915_private *dev_priv);
 void intel_dp_get_m_n(struct intel_crtc *crtc,
@@ -908,7 +972,6 @@ ironlake_check_encoder_dotclock(const struct intel_crtc_config *pipe_config,
 bool intel_crtc_active(struct drm_crtc *crtc);
 void hsw_enable_ips(struct intel_crtc *crtc);
 void hsw_disable_ips(struct intel_crtc *crtc);
-void intel_display_set_init_power(struct drm_i915_private *dev, bool enable);
 enum intel_display_power_domain
 intel_display_port_power_domain(struct intel_encoder *intel_encoder);
 void intel_mode_from_pipe_config(struct drm_display_mode *mode,
@@ -936,25 +999,18 @@ bool intel_dp_hpd_pulse(struct intel_digital_port *intel_dig_port,
 void intel_edp_backlight_on(struct intel_dp *intel_dp);
 void intel_edp_backlight_off(struct intel_dp *intel_dp);
 void intel_edp_panel_vdd_on(struct intel_dp *intel_dp);
-void intel_edp_panel_vdd_sanitize(struct intel_encoder *intel_encoder);
 void intel_edp_panel_on(struct intel_dp *intel_dp);
 void intel_edp_panel_off(struct intel_dp *intel_dp);
-void intel_edp_psr_enable(struct intel_dp *intel_dp);
-void intel_edp_psr_disable(struct intel_dp *intel_dp);
 void intel_dp_set_drrs_state(struct drm_device *dev, int refresh_rate);
-void intel_edp_psr_invalidate(struct drm_device *dev,
-			      unsigned frontbuffer_bits);
-void intel_edp_psr_flush(struct drm_device *dev,
-			 unsigned frontbuffer_bits);
-void intel_edp_psr_init(struct drm_device *dev);
-
-int intel_dp_handle_hpd_irq(struct intel_digital_port *digport, bool long_hpd);
 void intel_dp_add_properties(struct intel_dp *intel_dp, struct drm_connector *connector);
 void intel_dp_mst_suspend(struct drm_device *dev);
 void intel_dp_mst_resume(struct drm_device *dev);
 int intel_dp_max_link_bw(struct intel_dp *intel_dp);
 void intel_dp_hot_plug(struct intel_encoder *intel_encoder);
 void vlv_power_sequencer_reset(struct drm_i915_private *dev_priv);
+uint32_t intel_dp_pack_aux(const uint8_t *src, int src_bytes);
+void intel_dp_unpack_aux(uint32_t src, uint8_t *dst, int dst_bytes);
+
 /* intel_dp_mst.c */
 int intel_dp_mst_encoder_init(struct intel_digital_port *intel_dig_port, int conn_id);
 void intel_dp_mst_encoder_cleanup(struct intel_digital_port *intel_dig_port);
@@ -1044,7 +1100,7 @@ void intel_gmch_panel_fitting(struct intel_crtc *crtc,
 			      int fitting_mode);
 void intel_panel_set_backlight_acpi(struct intel_connector *connector,
 				    u32 level, u32 max);
-int intel_panel_setup_backlight(struct drm_connector *connector);
+int intel_panel_setup_backlight(struct drm_connector *connector, enum pipe pipe);
 void intel_panel_enable_backlight(struct intel_connector *connector);
 void intel_panel_disable_backlight(struct intel_connector *connector);
 void intel_panel_destroy_backlight(struct drm_connector *connector);
@@ -1054,6 +1110,41 @@ extern struct drm_display_mode *intel_find_panel_downclock(
 				struct drm_device *dev,
 				struct drm_display_mode *fixed_mode,
 				struct drm_connector *connector);
+void intel_backlight_register(struct drm_device *dev);
+void intel_backlight_unregister(struct drm_device *dev);
+
+
+/* intel_psr.c */
+bool intel_psr_is_enabled(struct drm_device *dev);
+void intel_psr_enable(struct intel_dp *intel_dp);
+void intel_psr_disable(struct intel_dp *intel_dp);
+void intel_psr_invalidate(struct drm_device *dev,
+			      unsigned frontbuffer_bits);
+void intel_psr_flush(struct drm_device *dev,
+			 unsigned frontbuffer_bits);
+void intel_psr_init(struct drm_device *dev);
+
+/* intel_runtime_pm.c */
+int intel_power_domains_init(struct drm_i915_private *);
+void intel_power_domains_fini(struct drm_i915_private *);
+void intel_power_domains_init_hw(struct drm_i915_private *dev_priv);
+void intel_runtime_pm_enable(struct drm_i915_private *dev_priv);
+
+bool intel_display_power_is_enabled(struct drm_i915_private *dev_priv,
+				    enum intel_display_power_domain domain);
+bool __intel_display_power_is_enabled(struct drm_i915_private *dev_priv,
+				      enum intel_display_power_domain domain);
+void intel_display_power_get(struct drm_i915_private *dev_priv,
+			     enum intel_display_power_domain domain);
+void intel_display_power_put(struct drm_i915_private *dev_priv,
+			     enum intel_display_power_domain domain);
+void intel_aux_display_runtime_get(struct drm_i915_private *dev_priv);
+void intel_aux_display_runtime_put(struct drm_i915_private *dev_priv);
+void intel_runtime_pm_get(struct drm_i915_private *dev_priv);
+void intel_runtime_pm_get_noresume(struct drm_i915_private *dev_priv);
+void intel_runtime_pm_put(struct drm_i915_private *dev_priv);
+
+void intel_display_set_init_power(struct drm_i915_private *dev, bool enable);
 
 /* intel_pm.c */
 void intel_init_clock_gating(struct drm_device *dev);
@@ -1072,17 +1163,6 @@ bool intel_fbc_enabled(struct drm_device *dev);
 void intel_update_fbc(struct drm_device *dev);
 void intel_gpu_ips_init(struct drm_i915_private *dev_priv);
 void intel_gpu_ips_teardown(void);
-int intel_power_domains_init(struct drm_i915_private *);
-void intel_power_domains_remove(struct drm_i915_private *);
-bool intel_display_power_enabled(struct drm_i915_private *dev_priv,
-				 enum intel_display_power_domain domain);
-bool intel_display_power_enabled_unlocked(struct drm_i915_private *dev_priv,
-					  enum intel_display_power_domain domain);
-void intel_display_power_get(struct drm_i915_private *dev_priv,
-			     enum intel_display_power_domain domain);
-void intel_display_power_put(struct drm_i915_private *dev_priv,
-			     enum intel_display_power_domain domain);
-void intel_power_domains_init_hw(struct drm_i915_private *dev_priv);
 void intel_init_gt_powersave(struct drm_device *dev);
 void intel_cleanup_gt_powersave(struct drm_device *dev);
 void intel_enable_gt_powersave(struct drm_device *dev);
@@ -1093,14 +1173,10 @@ void ironlake_teardown_rc6(struct drm_device *dev);
 void gen6_update_ring_freq(struct drm_device *dev);
 void gen6_rps_idle(struct drm_i915_private *dev_priv);
 void gen6_rps_boost(struct drm_i915_private *dev_priv);
-void intel_aux_display_runtime_get(struct drm_i915_private *dev_priv);
-void intel_aux_display_runtime_put(struct drm_i915_private *dev_priv);
-void intel_runtime_pm_get(struct drm_i915_private *dev_priv);
-void intel_runtime_pm_get_noresume(struct drm_i915_private *dev_priv);
-void intel_runtime_pm_put(struct drm_i915_private *dev_priv);
-void intel_init_runtime_pm(struct drm_i915_private *dev_priv);
-void intel_fini_runtime_pm(struct drm_i915_private *dev_priv);
 void ilk_wm_get_hw_state(struct drm_device *dev);
+void skl_wm_get_hw_state(struct drm_device *dev);
+void skl_ddb_get_hw_state(struct drm_i915_private *dev_priv,
+			  struct skl_ddb_allocation *ddb /* out */);
 
 
 /* intel_sdvo.c */
@@ -1120,7 +1196,9 @@ int intel_sprite_set_colorkey(struct drm_device *dev, void *data,
 			      struct drm_file *file_priv);
 int intel_sprite_get_colorkey(struct drm_device *dev, void *data,
 			      struct drm_file *file_priv);
-
+bool intel_pipe_update_start(struct intel_crtc *crtc,
+			     uint32_t *start_vbl_count);
+void intel_pipe_update_end(struct intel_crtc *crtc, u32 start_vbl_count);
 
 /* intel_tv.c */
 void intel_tv_init(struct drm_device *dev);
diff --git a/drivers/gpu/drm/i915/intel_dsi.c b/drivers/gpu/drm/i915/intel_dsi.c
index 5bd9e09ad3c5..0b184079de14 100644
--- a/drivers/gpu/drm/i915/intel_dsi.c
+++ b/drivers/gpu/drm/i915/intel_dsi.c
@@ -344,7 +344,7 @@ static bool intel_dsi_get_hw_state(struct intel_encoder *encoder,
 	DRM_DEBUG_KMS("\n");
 
 	power_domain = intel_display_port_power_domain(encoder);
-	if (!intel_display_power_enabled(dev_priv, power_domain))
+	if (!intel_display_power_is_enabled(dev_priv, power_domain))
 		return false;
 
 	/* XXX: this only works for one DSI output */
diff --git a/drivers/gpu/drm/i915/intel_fbdev.c b/drivers/gpu/drm/i915/intel_fbdev.c
index 9b584f3fbb99..850cf7d6578c 100644
--- a/drivers/gpu/drm/i915/intel_fbdev.c
+++ b/drivers/gpu/drm/i915/intel_fbdev.c
@@ -119,25 +119,25 @@ static int intelfb_alloc(struct drm_fb_helper *helper,
 		goto out;
 	}
 
-	/* Flush everything out, we'll be doing GTT only from now on */
-	ret = intel_pin_and_fence_fb_obj(dev, obj, NULL);
-	if (ret) {
-		DRM_ERROR("failed to pin obj: %d\n", ret);
-		goto out_unref;
-	}
-
 	fb = __intel_framebuffer_create(dev, &mode_cmd, obj);
 	if (IS_ERR(fb)) {
 		ret = PTR_ERR(fb);
-		goto out_unpin;
+		goto out_unref;
+	}
+
+	/* Flush everything out, we'll be doing GTT only from now on */
+	ret = intel_pin_and_fence_fb_obj(NULL, fb, NULL);
+	if (ret) {
+		DRM_ERROR("failed to pin obj: %d\n", ret);
+		goto out_fb;
 	}
 
 	ifbdev->fb = to_intel_framebuffer(fb);
 
 	return 0;
 
-out_unpin:
-	i915_gem_object_ggtt_unpin(obj);
+out_fb:
+	drm_framebuffer_remove(fb);
 out_unref:
 	drm_gem_object_unreference(&obj->base);
 out:
@@ -324,6 +324,7 @@ intel_fb_helper_crtc(struct drm_fb_helper *fb_helper, struct drm_crtc *crtc)
 static bool intel_fb_initial_config(struct drm_fb_helper *fb_helper,
 				    struct drm_fb_helper_crtc **crtcs,
 				    struct drm_display_mode **modes,
+				    struct drm_fb_offset *offsets,
 				    bool *enabled, int width, int height)
 {
 	struct drm_device *dev = fb_helper->dev;
@@ -332,6 +333,8 @@ static bool intel_fb_initial_config(struct drm_fb_helper *fb_helper,
 	bool fallback = true;
 	int num_connectors_enabled = 0;
 	int num_connectors_detected = 0;
+	uint64_t conn_configured = 0, mask;
+	int pass = 0;
 
 	save_enabled = kcalloc(dev->mode_config.num_connector, sizeof(bool),
 			       GFP_KERNEL);
@@ -339,7 +342,8 @@ static bool intel_fb_initial_config(struct drm_fb_helper *fb_helper,
 		return false;
 
 	memcpy(save_enabled, enabled, dev->mode_config.num_connector);
-
+	mask = (1 << fb_helper->connector_count) - 1;
+retry:
 	for (i = 0; i < fb_helper->connector_count; i++) {
 		struct drm_fb_helper_connector *fb_conn;
 		struct drm_connector *connector;
@@ -349,12 +353,19 @@ static bool intel_fb_initial_config(struct drm_fb_helper *fb_helper,
 		fb_conn = fb_helper->connector_info[i];
 		connector = fb_conn->connector;
 
+		if (conn_configured & (1 << i))
+			continue;
+
+		if (pass == 0 && !connector->has_tile)
+			continue;
+
 		if (connector->status == connector_status_connected)
 			num_connectors_detected++;
 
 		if (!enabled[i]) {
 			DRM_DEBUG_KMS("connector %s not enabled, skipping\n",
 				      connector->name);
+			conn_configured |= (1 << i);
 			continue;
 		}
 
@@ -373,6 +384,7 @@ static bool intel_fb_initial_config(struct drm_fb_helper *fb_helper,
 			DRM_DEBUG_KMS("connector %s has no encoder or crtc, skipping\n",
 				      connector->name);
 			enabled[i] = false;
+			conn_configured |= (1 << i);
 			continue;
 		}
 
@@ -400,8 +412,8 @@ static bool intel_fb_initial_config(struct drm_fb_helper *fb_helper,
 
 		/* try for preferred next */
 		if (!modes[i]) {
-			DRM_DEBUG_KMS("looking for preferred mode on connector %s\n",
-				      connector->name);
+			DRM_DEBUG_KMS("looking for preferred mode on connector %s %d\n",
+				      connector->name, connector->has_tile);
 			modes[i] = drm_has_preferred_mode(fb_conn, width,
 							  height);
 		}
@@ -444,6 +456,12 @@ static bool intel_fb_initial_config(struct drm_fb_helper *fb_helper,
 			      modes[i]->flags & DRM_MODE_FLAG_INTERLACE ? "i" :"");
 
 		fallback = false;
+		conn_configured |= (1 << i);
+	}
+
+	if ((conn_configured & mask) != mask) {
+		pass++;
+		goto retry;
 	}
 
 	/*
diff --git a/drivers/gpu/drm/i915/intel_fifo_underrun.c b/drivers/gpu/drm/i915/intel_fifo_underrun.c
new file mode 100644
index 000000000000..77af512d2d35
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_fifo_underrun.c
@@ -0,0 +1,381 @@
+/*
+ * Copyright © 2014 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:
+ *    Daniel Vetter <daniel.vetter@ffwll.ch>
+ *
+ */
+
+#include "i915_drv.h"
+#include "intel_drv.h"
+
+/**
+ * DOC: fifo underrun handling
+ *
+ * The i915 driver checks for display fifo underruns using the interrupt signals
+ * provided by the hardware. This is enabled by default and fairly useful to
+ * debug display issues, especially watermark settings.
+ *
+ * If an underrun is detected this is logged into dmesg. To avoid flooding logs
+ * and occupying the cpu underrun interrupts are disabled after the first
+ * occurrence until the next modeset on a given pipe.
+ *
+ * Note that underrun detection on gmch platforms is a bit more ugly since there
+ * is no interrupt (despite that the signalling bit is in the PIPESTAT pipe
+ * interrupt register). Also on some other platforms underrun interrupts are
+ * shared, which means that if we detect an underrun we need to disable underrun
+ * reporting on all pipes.
+ *
+ * The code also supports underrun detection on the PCH transcoder.
+ */
+
+static bool ivb_can_enable_err_int(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *crtc;
+	enum pipe pipe;
+
+	assert_spin_locked(&dev_priv->irq_lock);
+
+	for_each_pipe(dev_priv, pipe) {
+		crtc = to_intel_crtc(dev_priv->pipe_to_crtc_mapping[pipe]);
+
+		if (crtc->cpu_fifo_underrun_disabled)
+			return false;
+	}
+
+	return true;
+}
+
+static bool cpt_can_enable_serr_int(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	enum pipe pipe;
+	struct intel_crtc *crtc;
+
+	assert_spin_locked(&dev_priv->irq_lock);
+
+	for_each_pipe(dev_priv, pipe) {
+		crtc = to_intel_crtc(dev_priv->pipe_to_crtc_mapping[pipe]);
+
+		if (crtc->pch_fifo_underrun_disabled)
+			return false;
+	}
+
+	return true;
+}
+
+/**
+ * i9xx_check_fifo_underruns - check for fifo underruns
+ * @dev_priv: i915 device instance
+ *
+ * This function checks for fifo underruns on GMCH platforms. This needs to be
+ * done manually on modeset to make sure that we catch all underruns since they
+ * do not generate an interrupt by themselves on these platforms.
+ */
+void i9xx_check_fifo_underruns(struct drm_i915_private *dev_priv)
+{
+	struct intel_crtc *crtc;
+
+	spin_lock_irq(&dev_priv->irq_lock);
+
+	for_each_intel_crtc(dev_priv->dev, crtc) {
+		u32 reg = PIPESTAT(crtc->pipe);
+		u32 pipestat;
+
+		if (crtc->cpu_fifo_underrun_disabled)
+			continue;
+
+		pipestat = I915_READ(reg) & 0xffff0000;
+		if ((pipestat & PIPE_FIFO_UNDERRUN_STATUS) == 0)
+			continue;
+
+		I915_WRITE(reg, pipestat | PIPE_FIFO_UNDERRUN_STATUS);
+		POSTING_READ(reg);
+
+		DRM_ERROR("pipe %c underrun\n", pipe_name(crtc->pipe));
+	}
+
+	spin_unlock_irq(&dev_priv->irq_lock);
+}
+
+static void i9xx_set_fifo_underrun_reporting(struct drm_device *dev,
+					     enum pipe pipe,
+					     bool enable, bool old)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 reg = PIPESTAT(pipe);
+	u32 pipestat = I915_READ(reg) & 0xffff0000;
+
+	assert_spin_locked(&dev_priv->irq_lock);
+
+	if (enable) {
+		I915_WRITE(reg, pipestat | PIPE_FIFO_UNDERRUN_STATUS);
+		POSTING_READ(reg);
+	} else {
+		if (old && pipestat & PIPE_FIFO_UNDERRUN_STATUS)
+			DRM_ERROR("pipe %c underrun\n", pipe_name(pipe));
+	}
+}
+
+static void ironlake_set_fifo_underrun_reporting(struct drm_device *dev,
+						 enum pipe pipe, bool enable)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t bit = (pipe == PIPE_A) ? DE_PIPEA_FIFO_UNDERRUN :
+					  DE_PIPEB_FIFO_UNDERRUN;
+
+	if (enable)
+		ironlake_enable_display_irq(dev_priv, bit);
+	else
+		ironlake_disable_display_irq(dev_priv, bit);
+}
+
+static void ivybridge_set_fifo_underrun_reporting(struct drm_device *dev,
+						  enum pipe pipe,
+						  bool enable, bool old)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	if (enable) {
+		I915_WRITE(GEN7_ERR_INT, ERR_INT_FIFO_UNDERRUN(pipe));
+
+		if (!ivb_can_enable_err_int(dev))
+			return;
+
+		ironlake_enable_display_irq(dev_priv, DE_ERR_INT_IVB);
+	} else {
+		ironlake_disable_display_irq(dev_priv, DE_ERR_INT_IVB);
+
+		if (old &&
+		    I915_READ(GEN7_ERR_INT) & ERR_INT_FIFO_UNDERRUN(pipe)) {
+			DRM_ERROR("uncleared fifo underrun on pipe %c\n",
+				  pipe_name(pipe));
+		}
+	}
+}
+
+static void broadwell_set_fifo_underrun_reporting(struct drm_device *dev,
+						  enum pipe pipe, bool enable)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	assert_spin_locked(&dev_priv->irq_lock);
+
+	if (enable)
+		dev_priv->de_irq_mask[pipe] &= ~GEN8_PIPE_FIFO_UNDERRUN;
+	else
+		dev_priv->de_irq_mask[pipe] |= GEN8_PIPE_FIFO_UNDERRUN;
+	I915_WRITE(GEN8_DE_PIPE_IMR(pipe), dev_priv->de_irq_mask[pipe]);
+	POSTING_READ(GEN8_DE_PIPE_IMR(pipe));
+}
+
+static void ibx_set_fifo_underrun_reporting(struct drm_device *dev,
+					    enum transcoder pch_transcoder,
+					    bool enable)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t bit = (pch_transcoder == TRANSCODER_A) ?
+		       SDE_TRANSA_FIFO_UNDER : SDE_TRANSB_FIFO_UNDER;
+
+	if (enable)
+		ibx_enable_display_interrupt(dev_priv, bit);
+	else
+		ibx_disable_display_interrupt(dev_priv, bit);
+}
+
+static void cpt_set_fifo_underrun_reporting(struct drm_device *dev,
+					    enum transcoder pch_transcoder,
+					    bool enable, bool old)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (enable) {
+		I915_WRITE(SERR_INT,
+			   SERR_INT_TRANS_FIFO_UNDERRUN(pch_transcoder));
+
+		if (!cpt_can_enable_serr_int(dev))
+			return;
+
+		ibx_enable_display_interrupt(dev_priv, SDE_ERROR_CPT);
+	} else {
+		ibx_disable_display_interrupt(dev_priv, SDE_ERROR_CPT);
+
+		if (old && I915_READ(SERR_INT) &
+		    SERR_INT_TRANS_FIFO_UNDERRUN(pch_transcoder)) {
+			DRM_ERROR("uncleared pch fifo underrun on pch transcoder %c\n",
+				  transcoder_name(pch_transcoder));
+		}
+	}
+}
+
+static bool __intel_set_cpu_fifo_underrun_reporting(struct drm_device *dev,
+						    enum pipe pipe, bool enable)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc = dev_priv->pipe_to_crtc_mapping[pipe];
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	bool old;
+
+	assert_spin_locked(&dev_priv->irq_lock);
+
+	old = !intel_crtc->cpu_fifo_underrun_disabled;
+	intel_crtc->cpu_fifo_underrun_disabled = !enable;
+
+	if (HAS_GMCH_DISPLAY(dev))
+		i9xx_set_fifo_underrun_reporting(dev, pipe, enable, old);
+	else if (IS_GEN5(dev) || IS_GEN6(dev))
+		ironlake_set_fifo_underrun_reporting(dev, pipe, enable);
+	else if (IS_GEN7(dev))
+		ivybridge_set_fifo_underrun_reporting(dev, pipe, enable, old);
+	else if (IS_GEN8(dev) || IS_GEN9(dev))
+		broadwell_set_fifo_underrun_reporting(dev, pipe, enable);
+
+	return old;
+}
+
+/**
+ * intel_set_cpu_fifo_underrun_reporting - set cpu fifo underrrun reporting state
+ * @dev_priv: i915 device instance
+ * @pipe: (CPU) pipe to set state for
+ * @enable: whether underruns should be reported or not
+ *
+ * This function sets the fifo underrun state for @pipe. It is used in the
+ * modeset code to avoid false positives since on many platforms underruns are
+ * expected when disabling or enabling the pipe.
+ *
+ * Notice that on some platforms disabling underrun reports for one pipe
+ * disables for all due to shared interrupts. Actual reporting is still per-pipe
+ * though.
+ *
+ * Returns the previous state of underrun reporting.
+ */
+bool intel_set_cpu_fifo_underrun_reporting(struct drm_i915_private *dev_priv,
+					   enum pipe pipe, bool enable)
+{
+	unsigned long flags;
+	bool ret;
+
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
+	ret = __intel_set_cpu_fifo_underrun_reporting(dev_priv->dev, pipe,
+						      enable);
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
+
+	return ret;
+}
+
+static bool
+__cpu_fifo_underrun_reporting_enabled(struct drm_i915_private *dev_priv,
+				      enum pipe pipe)
+{
+	struct drm_crtc *crtc = dev_priv->pipe_to_crtc_mapping[pipe];
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+
+	return !intel_crtc->cpu_fifo_underrun_disabled;
+}
+
+/**
+ * intel_set_pch_fifo_underrun_reporting - set PCH fifo underrun reporting state
+ * @dev_priv: i915 device instance
+ * @pch_transcoder: the PCH transcoder (same as pipe on IVB and older)
+ * @enable: whether underruns should be reported or not
+ *
+ * This function makes us disable or enable PCH fifo underruns for a specific
+ * PCH transcoder. Notice that on some PCHs (e.g. CPT/PPT), disabling FIFO
+ * underrun reporting for one transcoder may also disable all the other PCH
+ * error interruts for the other transcoders, due to the fact that there's just
+ * one interrupt mask/enable bit for all the transcoders.
+ *
+ * Returns the previous state of underrun reporting.
+ */
+bool intel_set_pch_fifo_underrun_reporting(struct drm_i915_private *dev_priv,
+					   enum transcoder pch_transcoder,
+					   bool enable)
+{
+	struct drm_crtc *crtc = dev_priv->pipe_to_crtc_mapping[pch_transcoder];
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	unsigned long flags;
+	bool old;
+
+	/*
+	 * NOTE: Pre-LPT has a fixed cpu pipe -> pch transcoder mapping, but LPT
+	 * has only one pch transcoder A that all pipes can use. To avoid racy
+	 * pch transcoder -> pipe lookups from interrupt code simply store the
+	 * underrun statistics in crtc A. Since we never expose this anywhere
+	 * nor use it outside of the fifo underrun code here using the "wrong"
+	 * crtc on LPT won't cause issues.
+	 */
+
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
+
+	old = !intel_crtc->pch_fifo_underrun_disabled;
+	intel_crtc->pch_fifo_underrun_disabled = !enable;
+
+	if (HAS_PCH_IBX(dev_priv->dev))
+		ibx_set_fifo_underrun_reporting(dev_priv->dev, pch_transcoder,
+						enable);
+	else
+		cpt_set_fifo_underrun_reporting(dev_priv->dev, pch_transcoder,
+						enable, old);
+
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
+	return old;
+}
+
+/**
+ * intel_pch_fifo_underrun_irq_handler - handle PCH fifo underrun interrupt
+ * @dev_priv: i915 device instance
+ * @pipe: (CPU) pipe to set state for
+ *
+ * This handles a CPU fifo underrun interrupt, generating an underrun warning
+ * into dmesg if underrun reporting is enabled and then disables the underrun
+ * interrupt to avoid an irq storm.
+ */
+void intel_cpu_fifo_underrun_irq_handler(struct drm_i915_private *dev_priv,
+					 enum pipe pipe)
+{
+	/* GMCH can't disable fifo underruns, filter them. */
+	if (HAS_GMCH_DISPLAY(dev_priv->dev) &&
+	    !__cpu_fifo_underrun_reporting_enabled(dev_priv, pipe))
+		return;
+
+	if (intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, false))
+		DRM_ERROR("CPU pipe %c FIFO underrun\n",
+			  pipe_name(pipe));
+}
+
+/**
+ * intel_pch_fifo_underrun_irq_handler - handle PCH fifo underrun interrupt
+ * @dev_priv: i915 device instance
+ * @pch_transcoder: the PCH transcoder (same as pipe on IVB and older)
+ *
+ * This handles a PCH fifo underrun interrupt, generating an underrun warning
+ * into dmesg if underrun reporting is enabled and then disables the underrun
+ * interrupt to avoid an irq storm.
+ */
+void intel_pch_fifo_underrun_irq_handler(struct drm_i915_private *dev_priv,
+					 enum transcoder pch_transcoder)
+{
+	if (intel_set_pch_fifo_underrun_reporting(dev_priv, pch_transcoder,
+						  false))
+		DRM_ERROR("PCH transcoder %c FIFO underrun\n",
+			  transcoder_name(pch_transcoder));
+}
diff --git a/drivers/gpu/drm/i915/intel_frontbuffer.c b/drivers/gpu/drm/i915/intel_frontbuffer.c
new file mode 100644
index 000000000000..79f6d72179c5
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_frontbuffer.c
@@ -0,0 +1,279 @@
+/*
+ * Copyright © 2014 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:
+ *	Daniel Vetter <daniel.vetter@ffwll.ch>
+ */
+
+/**
+ * DOC: frontbuffer tracking
+ *
+ * Many features require us to track changes to the currently active
+ * frontbuffer, especially rendering targeted at the frontbuffer.
+ *
+ * To be able to do so GEM tracks frontbuffers using a bitmask for all possible
+ * frontbuffer slots through i915_gem_track_fb(). The function in this file are
+ * then called when the contents of the frontbuffer are invalidated, when
+ * frontbuffer rendering has stopped again to flush out all the changes and when
+ * the frontbuffer is exchanged with a flip. Subsystems interested in
+ * frontbuffer changes (e.g. PSR, FBC, DRRS) should directly put their callbacks
+ * into the relevant places and filter for the frontbuffer slots that they are
+ * interested int.
+ *
+ * On a high level there are two types of powersaving features. The first one
+ * work like a special cache (FBC and PSR) and are interested when they should
+ * stop caching and when to restart caching. This is done by placing callbacks
+ * into the invalidate and the flush functions: At invalidate the caching must
+ * be stopped and at flush time it can be restarted. And maybe they need to know
+ * when the frontbuffer changes (e.g. when the hw doesn't initiate an invalidate
+ * and flush on its own) which can be achieved with placing callbacks into the
+ * flip functions.
+ *
+ * The other type of display power saving feature only cares about busyness
+ * (e.g. DRRS). In that case all three (invalidate, flush and flip) indicate
+ * busyness. There is no direct way to detect idleness. Instead an idle timer
+ * work delayed work should be started from the flush and flip functions and
+ * cancelled as soon as busyness is detected.
+ *
+ * Note that there's also an older frontbuffer activity tracking scheme which
+ * just tracks general activity. This is done by the various mark_busy and
+ * mark_idle functions. For display power management features using these
+ * functions is deprecated and should be avoided.
+ */
+
+#include <drm/drmP.h>
+
+#include "intel_drv.h"
+#include "i915_drv.h"
+
+static void intel_increase_pllclock(struct drm_device *dev,
+				    enum pipe pipe)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int dpll_reg = DPLL(pipe);
+	int dpll;
+
+	if (!HAS_GMCH_DISPLAY(dev))
+		return;
+
+	if (!dev_priv->lvds_downclock_avail)
+		return;
+
+	dpll = I915_READ(dpll_reg);
+	if (!HAS_PIPE_CXSR(dev) && (dpll & DISPLAY_RATE_SELECT_FPA1)) {
+		DRM_DEBUG_DRIVER("upclocking LVDS\n");
+
+		assert_panel_unlocked(dev_priv, pipe);
+
+		dpll &= ~DISPLAY_RATE_SELECT_FPA1;
+		I915_WRITE(dpll_reg, dpll);
+		intel_wait_for_vblank(dev, pipe);
+
+		dpll = I915_READ(dpll_reg);
+		if (dpll & DISPLAY_RATE_SELECT_FPA1)
+			DRM_DEBUG_DRIVER("failed to upclock LVDS!\n");
+	}
+}
+
+/**
+ * intel_mark_fb_busy - mark given planes as busy
+ * @dev: DRM device
+ * @frontbuffer_bits: bits for the affected planes
+ * @ring: optional ring for asynchronous commands
+ *
+ * This function gets called every time the screen contents change. It can be
+ * used to keep e.g. the update rate at the nominal refresh rate with DRRS.
+ */
+static void intel_mark_fb_busy(struct drm_device *dev,
+			       unsigned frontbuffer_bits,
+			       struct intel_engine_cs *ring)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	enum pipe pipe;
+
+	if (!i915.powersave)
+		return;
+
+	for_each_pipe(dev_priv, pipe) {
+		if (!(frontbuffer_bits & INTEL_FRONTBUFFER_ALL_MASK(pipe)))
+			continue;
+
+		intel_increase_pllclock(dev, pipe);
+		if (ring && intel_fbc_enabled(dev))
+			ring->fbc_dirty = true;
+	}
+}
+
+/**
+ * intel_fb_obj_invalidate - invalidate frontbuffer object
+ * @obj: GEM object to invalidate
+ * @ring: set for asynchronous rendering
+ *
+ * This function gets called every time rendering on the given object starts and
+ * frontbuffer caching (fbc, low refresh rate for DRRS, panel self refresh) must
+ * be invalidated. If @ring is non-NULL any subsequent invalidation will be delayed
+ * until the rendering completes or a flip on this frontbuffer plane is
+ * scheduled.
+ */
+void intel_fb_obj_invalidate(struct drm_i915_gem_object *obj,
+			     struct intel_engine_cs *ring)
+{
+	struct drm_device *dev = obj->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	WARN_ON(!mutex_is_locked(&dev->struct_mutex));
+
+	if (!obj->frontbuffer_bits)
+		return;
+
+	if (ring) {
+		mutex_lock(&dev_priv->fb_tracking.lock);
+		dev_priv->fb_tracking.busy_bits
+			|= obj->frontbuffer_bits;
+		dev_priv->fb_tracking.flip_bits
+			&= ~obj->frontbuffer_bits;
+		mutex_unlock(&dev_priv->fb_tracking.lock);
+	}
+
+	intel_mark_fb_busy(dev, obj->frontbuffer_bits, ring);
+
+	intel_psr_invalidate(dev, obj->frontbuffer_bits);
+}
+
+/**
+ * intel_frontbuffer_flush - flush frontbuffer
+ * @dev: DRM device
+ * @frontbuffer_bits: frontbuffer plane tracking bits
+ *
+ * This function gets called every time rendering on the given planes has
+ * completed and frontbuffer caching can be started again. Flushes will get
+ * delayed if they're blocked by some outstanding asynchronous rendering.
+ *
+ * Can be called without any locks held.
+ */
+void intel_frontbuffer_flush(struct drm_device *dev,
+			     unsigned frontbuffer_bits)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	/* Delay flushing when rings are still busy.*/
+	mutex_lock(&dev_priv->fb_tracking.lock);
+	frontbuffer_bits &= ~dev_priv->fb_tracking.busy_bits;
+	mutex_unlock(&dev_priv->fb_tracking.lock);
+
+	intel_mark_fb_busy(dev, frontbuffer_bits, NULL);
+
+	intel_psr_flush(dev, frontbuffer_bits);
+
+	/*
+	 * FIXME: Unconditional fbc flushing here is a rather gross hack and
+	 * needs to be reworked into a proper frontbuffer tracking scheme like
+	 * psr employs.
+	 */
+	if (dev_priv->fbc.need_sw_cache_clean) {
+		dev_priv->fbc.need_sw_cache_clean = false;
+		bdw_fbc_sw_flush(dev, FBC_REND_CACHE_CLEAN);
+	}
+}
+
+/**
+ * intel_fb_obj_flush - flush frontbuffer object
+ * @obj: GEM object to flush
+ * @retire: set when retiring asynchronous rendering
+ *
+ * This function gets called every time rendering on the given object has
+ * completed and frontbuffer caching can be started again. If @retire is true
+ * then any delayed flushes will be unblocked.
+ */
+void intel_fb_obj_flush(struct drm_i915_gem_object *obj,
+			bool retire)
+{
+	struct drm_device *dev = obj->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	unsigned frontbuffer_bits;
+
+	WARN_ON(!mutex_is_locked(&dev->struct_mutex));
+
+	if (!obj->frontbuffer_bits)
+		return;
+
+	frontbuffer_bits = obj->frontbuffer_bits;
+
+	if (retire) {
+		mutex_lock(&dev_priv->fb_tracking.lock);
+		/* Filter out new bits since rendering started. */
+		frontbuffer_bits &= dev_priv->fb_tracking.busy_bits;
+
+		dev_priv->fb_tracking.busy_bits &= ~frontbuffer_bits;
+		mutex_unlock(&dev_priv->fb_tracking.lock);
+	}
+
+	intel_frontbuffer_flush(dev, frontbuffer_bits);
+}
+
+/**
+ * intel_frontbuffer_flip_prepare - prepare asynchronous frontbuffer flip
+ * @dev: DRM device
+ * @frontbuffer_bits: frontbuffer plane tracking bits
+ *
+ * This function gets called after scheduling a flip on @obj. The actual
+ * frontbuffer flushing will be delayed until completion is signalled with
+ * intel_frontbuffer_flip_complete. If an invalidate happens in between this
+ * flush will be cancelled.
+ *
+ * Can be called without any locks held.
+ */
+void intel_frontbuffer_flip_prepare(struct drm_device *dev,
+				    unsigned frontbuffer_bits)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	mutex_lock(&dev_priv->fb_tracking.lock);
+	dev_priv->fb_tracking.flip_bits |= frontbuffer_bits;
+	/* Remove stale busy bits due to the old buffer. */
+	dev_priv->fb_tracking.busy_bits &= ~frontbuffer_bits;
+	mutex_unlock(&dev_priv->fb_tracking.lock);
+}
+
+/**
+ * intel_frontbuffer_flip_complete - complete asynchronous frontbuffer flip
+ * @dev: DRM device
+ * @frontbuffer_bits: frontbuffer plane tracking bits
+ *
+ * This function gets called after the flip has been latched and will complete
+ * on the next vblank. It will execute the flush if it hasn't been cancelled yet.
+ *
+ * Can be called without any locks held.
+ */
+void intel_frontbuffer_flip_complete(struct drm_device *dev,
+				     unsigned frontbuffer_bits)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	mutex_lock(&dev_priv->fb_tracking.lock);
+	/* Mask any cancelled flips. */
+	frontbuffer_bits &= dev_priv->fb_tracking.flip_bits;
+	dev_priv->fb_tracking.flip_bits &= ~frontbuffer_bits;
+	mutex_unlock(&dev_priv->fb_tracking.lock);
+
+	intel_frontbuffer_flush(dev, frontbuffer_bits);
+}
diff --git a/drivers/gpu/drm/i915/intel_hdmi.c b/drivers/gpu/drm/i915/intel_hdmi.c
index 29ec1535992d..3abc2000fce9 100644
--- a/drivers/gpu/drm/i915/intel_hdmi.c
+++ b/drivers/gpu/drm/i915/intel_hdmi.c
@@ -166,6 +166,19 @@ static void g4x_write_infoframe(struct drm_encoder *encoder,
 	POSTING_READ(VIDEO_DIP_CTL);
 }
 
+static bool g4x_infoframe_enabled(struct drm_encoder *encoder)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_digital_port *intel_dig_port = enc_to_dig_port(encoder);
+	u32 val = I915_READ(VIDEO_DIP_CTL);
+
+	if (VIDEO_DIP_PORT(intel_dig_port->port) == (val & VIDEO_DIP_PORT_MASK))
+		return val & VIDEO_DIP_ENABLE;
+
+	return false;
+}
+
 static void ibx_write_infoframe(struct drm_encoder *encoder,
 				enum hdmi_infoframe_type type,
 				const void *frame, ssize_t len)
@@ -204,6 +217,17 @@ static void ibx_write_infoframe(struct drm_encoder *encoder,
 	POSTING_READ(reg);
 }
 
+static bool ibx_infoframe_enabled(struct drm_encoder *encoder)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
+	int reg = TVIDEO_DIP_CTL(intel_crtc->pipe);
+	u32 val = I915_READ(reg);
+
+	return val & VIDEO_DIP_ENABLE;
+}
+
 static void cpt_write_infoframe(struct drm_encoder *encoder,
 				enum hdmi_infoframe_type type,
 				const void *frame, ssize_t len)
@@ -245,6 +269,17 @@ static void cpt_write_infoframe(struct drm_encoder *encoder,
 	POSTING_READ(reg);
 }
 
+static bool cpt_infoframe_enabled(struct drm_encoder *encoder)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
+	int reg = TVIDEO_DIP_CTL(intel_crtc->pipe);
+	u32 val = I915_READ(reg);
+
+	return val & VIDEO_DIP_ENABLE;
+}
+
 static void vlv_write_infoframe(struct drm_encoder *encoder,
 				enum hdmi_infoframe_type type,
 				const void *frame, ssize_t len)
@@ -283,6 +318,17 @@ static void vlv_write_infoframe(struct drm_encoder *encoder,
 	POSTING_READ(reg);
 }
 
+static bool vlv_infoframe_enabled(struct drm_encoder *encoder)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
+	int reg = VLV_TVIDEO_DIP_CTL(intel_crtc->pipe);
+	u32 val = I915_READ(reg);
+
+	return val & VIDEO_DIP_ENABLE;
+}
+
 static void hsw_write_infoframe(struct drm_encoder *encoder,
 				enum hdmi_infoframe_type type,
 				const void *frame, ssize_t len)
@@ -320,6 +366,18 @@ static void hsw_write_infoframe(struct drm_encoder *encoder,
 	POSTING_READ(ctl_reg);
 }
 
+static bool hsw_infoframe_enabled(struct drm_encoder *encoder)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
+	u32 ctl_reg = HSW_TVIDEO_DIP_CTL(intel_crtc->config.cpu_transcoder);
+	u32 val = I915_READ(ctl_reg);
+
+	return val & (VIDEO_DIP_ENABLE_AVI_HSW | VIDEO_DIP_ENABLE_SPD_HSW |
+		      VIDEO_DIP_ENABLE_VS_HSW);
+}
+
 /*
  * The data we write to the DIP data buffer registers is 1 byte bigger than the
  * HDMI infoframe size because of an ECC/reserved byte at position 3 (starting
@@ -661,14 +719,6 @@ static void intel_hdmi_prepare(struct intel_encoder *encoder)
 	if (crtc->config.has_hdmi_sink)
 		hdmi_val |= HDMI_MODE_SELECT_HDMI;
 
-	if (crtc->config.has_audio) {
-		WARN_ON(!crtc->config.has_hdmi_sink);
-		DRM_DEBUG_DRIVER("Enabling HDMI audio on pipe %c\n",
-				 pipe_name(crtc->pipe));
-		hdmi_val |= SDVO_AUDIO_ENABLE;
-		intel_write_eld(&encoder->base, adjusted_mode);
-	}
-
 	if (HAS_PCH_CPT(dev))
 		hdmi_val |= SDVO_PIPE_SEL_CPT(crtc->pipe);
 	else if (IS_CHERRYVIEW(dev))
@@ -690,7 +740,7 @@ static bool intel_hdmi_get_hw_state(struct intel_encoder *encoder,
 	u32 tmp;
 
 	power_domain = intel_display_port_power_domain(encoder);
-	if (!intel_display_power_enabled(dev_priv, power_domain))
+	if (!intel_display_power_is_enabled(dev_priv, power_domain))
 		return false;
 
 	tmp = I915_READ(intel_hdmi->hdmi_reg);
@@ -732,6 +782,9 @@ static void intel_hdmi_get_config(struct intel_encoder *encoder,
 	if (tmp & HDMI_MODE_SELECT_HDMI)
 		pipe_config->has_hdmi_sink = true;
 
+	if (intel_hdmi->infoframe_enabled(&encoder->base))
+		pipe_config->has_infoframe = true;
+
 	if (tmp & SDVO_AUDIO_ENABLE)
 		pipe_config->has_audio = true;
 
@@ -791,6 +844,13 @@ static void intel_enable_hdmi(struct intel_encoder *encoder)
 		I915_WRITE(intel_hdmi->hdmi_reg, temp);
 		POSTING_READ(intel_hdmi->hdmi_reg);
 	}
+
+	if (intel_crtc->config.has_audio) {
+		WARN_ON(!intel_crtc->config.has_hdmi_sink);
+		DRM_DEBUG_DRIVER("Enabling HDMI audio on pipe %c\n",
+				 pipe_name(intel_crtc->pipe));
+		intel_audio_codec_enable(encoder);
+	}
 }
 
 static void vlv_enable_hdmi(struct intel_encoder *encoder)
@@ -802,9 +862,13 @@ static void intel_disable_hdmi(struct intel_encoder *encoder)
 	struct drm_device *dev = encoder->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(&encoder->base);
+	struct intel_crtc *crtc = to_intel_crtc(encoder->base.crtc);
 	u32 temp;
 	u32 enable_bits = SDVO_ENABLE | SDVO_AUDIO_ENABLE;
 
+	if (crtc->config.has_audio)
+		intel_audio_codec_disable(encoder);
+
 	temp = I915_READ(intel_hdmi->hdmi_reg);
 
 	/* HW workaround for IBX, we need to move the port to transcoder A
@@ -922,6 +986,9 @@ bool intel_hdmi_compute_config(struct intel_encoder *encoder,
 
 	pipe_config->has_hdmi_sink = intel_hdmi->has_hdmi_sink;
 
+	if (pipe_config->has_hdmi_sink)
+		pipe_config->has_infoframe = true;
+
 	if (intel_hdmi->color_range_auto) {
 		/* See CEA-861-E - 5.1 Default Encoding Parameters */
 		if (pipe_config->has_hdmi_sink &&
@@ -1394,10 +1461,13 @@ static void chv_hdmi_post_disable(struct intel_encoder *encoder)
 static void chv_hdmi_pre_enable(struct intel_encoder *encoder)
 {
 	struct intel_digital_port *dport = enc_to_dig_port(&encoder->base);
+	struct intel_hdmi *intel_hdmi = &dport->hdmi;
 	struct drm_device *dev = encoder->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc =
 		to_intel_crtc(encoder->base.crtc);
+	struct drm_display_mode *adjusted_mode =
+		&intel_crtc->config.adjusted_mode;
 	enum dpio_channel ch = vlv_dport_to_channel(dport);
 	int pipe = intel_crtc->pipe;
 	int data, i;
@@ -1405,6 +1475,15 @@ static void chv_hdmi_pre_enable(struct intel_encoder *encoder)
 
 	mutex_lock(&dev_priv->dpio_lock);
 
+	/* allow hardware to manage TX FIFO reset source */
+	val = vlv_dpio_read(dev_priv, pipe, VLV_PCS01_DW11(ch));
+	val &= ~DPIO_LANEDESKEW_STRAP_OVRD;
+	vlv_dpio_write(dev_priv, pipe, VLV_PCS01_DW11(ch), val);
+
+	val = vlv_dpio_read(dev_priv, pipe, VLV_PCS23_DW11(ch));
+	val &= ~DPIO_LANEDESKEW_STRAP_OVRD;
+	vlv_dpio_write(dev_priv, pipe, VLV_PCS23_DW11(ch), val);
+
 	/* Deassert soft data lane reset*/
 	val = vlv_dpio_read(dev_priv, pipe, VLV_PCS01_DW1(ch));
 	val |= CHV_PCS_REQ_SOFTRESET_EN;
@@ -1441,12 +1520,26 @@ static void chv_hdmi_pre_enable(struct intel_encoder *encoder)
 	/* Clear calc init */
 	val = vlv_dpio_read(dev_priv, pipe, VLV_PCS01_DW10(ch));
 	val &= ~(DPIO_PCS_SWING_CALC_TX0_TX2 | DPIO_PCS_SWING_CALC_TX1_TX3);
+	val &= ~(DPIO_PCS_TX1DEEMP_MASK | DPIO_PCS_TX2DEEMP_MASK);
+	val |= DPIO_PCS_TX1DEEMP_9P5 | DPIO_PCS_TX2DEEMP_9P5;
 	vlv_dpio_write(dev_priv, pipe, VLV_PCS01_DW10(ch), val);
 
 	val = vlv_dpio_read(dev_priv, pipe, VLV_PCS23_DW10(ch));
 	val &= ~(DPIO_PCS_SWING_CALC_TX0_TX2 | DPIO_PCS_SWING_CALC_TX1_TX3);
+	val &= ~(DPIO_PCS_TX1DEEMP_MASK | DPIO_PCS_TX2DEEMP_MASK);
+	val |= DPIO_PCS_TX1DEEMP_9P5 | DPIO_PCS_TX2DEEMP_9P5;
 	vlv_dpio_write(dev_priv, pipe, VLV_PCS23_DW10(ch), val);
 
+	val = vlv_dpio_read(dev_priv, pipe, VLV_PCS01_DW9(ch));
+	val &= ~(DPIO_PCS_TX1MARGIN_MASK | DPIO_PCS_TX2MARGIN_MASK);
+	val |= DPIO_PCS_TX1MARGIN_000 | DPIO_PCS_TX2MARGIN_000;
+	vlv_dpio_write(dev_priv, pipe, VLV_PCS01_DW9(ch), val);
+
+	val = vlv_dpio_read(dev_priv, pipe, VLV_PCS23_DW9(ch));
+	val &= ~(DPIO_PCS_TX1MARGIN_MASK | DPIO_PCS_TX2MARGIN_MASK);
+	val |= DPIO_PCS_TX1MARGIN_000 | DPIO_PCS_TX2MARGIN_000;
+	vlv_dpio_write(dev_priv, pipe, VLV_PCS23_DW9(ch), val);
+
 	/* FIXME: Program the support xxx V-dB */
 	/* Use 800mV-0dB */
 	for (i = 0; i < 4; i++) {
@@ -1499,6 +1592,10 @@ static void chv_hdmi_pre_enable(struct intel_encoder *encoder)
 
 	mutex_unlock(&dev_priv->dpio_lock);
 
+	intel_hdmi->set_infoframes(&encoder->base,
+				   intel_crtc->config.has_hdmi_sink,
+				   adjusted_mode);
+
 	intel_enable_hdmi(encoder);
 
 	vlv_wait_port_ready(dev_priv, dport);
@@ -1593,18 +1690,23 @@ void intel_hdmi_init_connector(struct intel_digital_port *intel_dig_port,
 	if (IS_VALLEYVIEW(dev)) {
 		intel_hdmi->write_infoframe = vlv_write_infoframe;
 		intel_hdmi->set_infoframes = vlv_set_infoframes;
+		intel_hdmi->infoframe_enabled = vlv_infoframe_enabled;
 	} else if (IS_G4X(dev)) {
 		intel_hdmi->write_infoframe = g4x_write_infoframe;
 		intel_hdmi->set_infoframes = g4x_set_infoframes;
+		intel_hdmi->infoframe_enabled = g4x_infoframe_enabled;
 	} else if (HAS_DDI(dev)) {
 		intel_hdmi->write_infoframe = hsw_write_infoframe;
 		intel_hdmi->set_infoframes = hsw_set_infoframes;
+		intel_hdmi->infoframe_enabled = hsw_infoframe_enabled;
 	} else if (HAS_PCH_IBX(dev)) {
 		intel_hdmi->write_infoframe = ibx_write_infoframe;
 		intel_hdmi->set_infoframes = ibx_set_infoframes;
+		intel_hdmi->infoframe_enabled = ibx_infoframe_enabled;
 	} else {
 		intel_hdmi->write_infoframe = cpt_write_infoframe;
 		intel_hdmi->set_infoframes = cpt_set_infoframes;
+		intel_hdmi->infoframe_enabled = cpt_infoframe_enabled;
 	}
 
 	if (HAS_DDI(dev))
diff --git a/drivers/gpu/drm/i915/intel_lrc.c b/drivers/gpu/drm/i915/intel_lrc.c
index bafd38b5703e..e588376227ea 100644
--- a/drivers/gpu/drm/i915/intel_lrc.c
+++ b/drivers/gpu/drm/i915/intel_lrc.c
@@ -136,11 +136,10 @@
 #include <drm/i915_drm.h>
 #include "i915_drv.h"
 
+#define GEN9_LR_CONTEXT_RENDER_SIZE (22 * PAGE_SIZE)
 #define GEN8_LR_CONTEXT_RENDER_SIZE (20 * PAGE_SIZE)
 #define GEN8_LR_CONTEXT_OTHER_SIZE (2 * PAGE_SIZE)
 
-#define GEN8_LR_CONTEXT_ALIGN 4096
-
 #define RING_EXECLIST_QFULL		(1 << 0x2)
 #define RING_EXECLIST1_VALID		(1 << 0x3)
 #define RING_EXECLIST0_VALID		(1 << 0x4)
@@ -204,6 +203,9 @@ enum {
 };
 #define GEN8_CTX_ID_SHIFT 32
 
+static int intel_lr_context_pin(struct intel_engine_cs *ring,
+		struct intel_context *ctx);
+
 /**
  * intel_sanitize_enable_execlists() - sanitize i915.enable_execlists
  * @dev: DRM device.
@@ -219,6 +221,9 @@ int intel_sanitize_enable_execlists(struct drm_device *dev, int enable_execlists
 {
 	WARN_ON(i915.enable_ppgtt == -1);
 
+	if (INTEL_INFO(dev)->gen >= 9)
+		return 1;
+
 	if (enable_execlists == 0)
 		return 0;
 
@@ -275,7 +280,8 @@ static void execlists_elsp_write(struct intel_engine_cs *ring,
 				 struct drm_i915_gem_object *ctx_obj0,
 				 struct drm_i915_gem_object *ctx_obj1)
 {
-	struct drm_i915_private *dev_priv = ring->dev->dev_private;
+	struct drm_device *dev = ring->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
 	uint64_t temp = 0;
 	uint32_t desc[4];
 	unsigned long flags;
@@ -300,13 +306,18 @@ static void execlists_elsp_write(struct intel_engine_cs *ring,
 	 * Instead, we do the runtime_pm_get/put when creating/destroying requests.
 	 */
 	spin_lock_irqsave(&dev_priv->uncore.lock, flags);
-	if (IS_CHERRYVIEW(dev_priv->dev)) {
+	if (IS_CHERRYVIEW(dev) || INTEL_INFO(dev)->gen >= 9) {
 		if (dev_priv->uncore.fw_rendercount++ == 0)
 			dev_priv->uncore.funcs.force_wake_get(dev_priv,
 							      FORCEWAKE_RENDER);
 		if (dev_priv->uncore.fw_mediacount++ == 0)
 			dev_priv->uncore.funcs.force_wake_get(dev_priv,
 							      FORCEWAKE_MEDIA);
+		if (INTEL_INFO(dev)->gen >= 9) {
+			if (dev_priv->uncore.fw_blittercount++ == 0)
+				dev_priv->uncore.funcs.force_wake_get(dev_priv,
+							FORCEWAKE_BLITTER);
+		}
 	} else {
 		if (dev_priv->uncore.forcewake_count++ == 0)
 			dev_priv->uncore.funcs.force_wake_get(dev_priv,
@@ -325,13 +336,18 @@ static void execlists_elsp_write(struct intel_engine_cs *ring,
 
 	/* Release Force Wakeup (see the big comment above). */
 	spin_lock_irqsave(&dev_priv->uncore.lock, flags);
-	if (IS_CHERRYVIEW(dev_priv->dev)) {
+	if (IS_CHERRYVIEW(dev) || INTEL_INFO(dev)->gen >= 9) {
 		if (--dev_priv->uncore.fw_rendercount == 0)
 			dev_priv->uncore.funcs.force_wake_put(dev_priv,
 							      FORCEWAKE_RENDER);
 		if (--dev_priv->uncore.fw_mediacount == 0)
 			dev_priv->uncore.funcs.force_wake_put(dev_priv,
 							      FORCEWAKE_MEDIA);
+		if (INTEL_INFO(dev)->gen >= 9) {
+			if (--dev_priv->uncore.fw_blittercount == 0)
+				dev_priv->uncore.funcs.force_wake_put(dev_priv,
+							FORCEWAKE_BLITTER);
+		}
 	} else {
 		if (--dev_priv->uncore.forcewake_count == 0)
 			dev_priv->uncore.funcs.force_wake_put(dev_priv,
@@ -341,7 +357,9 @@ static void execlists_elsp_write(struct intel_engine_cs *ring,
 	spin_unlock_irqrestore(&dev_priv->uncore.lock, flags);
 }
 
-static int execlists_ctx_write_tail(struct drm_i915_gem_object *ctx_obj, u32 tail)
+static int execlists_update_context(struct drm_i915_gem_object *ctx_obj,
+				    struct drm_i915_gem_object *ring_obj,
+				    u32 tail)
 {
 	struct page *page;
 	uint32_t *reg_state;
@@ -350,43 +368,45 @@ static int execlists_ctx_write_tail(struct drm_i915_gem_object *ctx_obj, u32 tai
 	reg_state = kmap_atomic(page);
 
 	reg_state[CTX_RING_TAIL+1] = tail;
+	reg_state[CTX_RING_BUFFER_START+1] = i915_gem_obj_ggtt_offset(ring_obj);
 
 	kunmap_atomic(reg_state);
 
 	return 0;
 }
 
-static int execlists_submit_context(struct intel_engine_cs *ring,
-				    struct intel_context *to0, u32 tail0,
-				    struct intel_context *to1, u32 tail1)
+static void execlists_submit_contexts(struct intel_engine_cs *ring,
+				      struct intel_context *to0, u32 tail0,
+				      struct intel_context *to1, u32 tail1)
 {
-	struct drm_i915_gem_object *ctx_obj0;
+	struct drm_i915_gem_object *ctx_obj0 = to0->engine[ring->id].state;
+	struct intel_ringbuffer *ringbuf0 = to0->engine[ring->id].ringbuf;
 	struct drm_i915_gem_object *ctx_obj1 = NULL;
+	struct intel_ringbuffer *ringbuf1 = NULL;
 
-	ctx_obj0 = to0->engine[ring->id].state;
 	BUG_ON(!ctx_obj0);
 	WARN_ON(!i915_gem_obj_is_pinned(ctx_obj0));
+	WARN_ON(!i915_gem_obj_is_pinned(ringbuf0->obj));
 
-	execlists_ctx_write_tail(ctx_obj0, tail0);
+	execlists_update_context(ctx_obj0, ringbuf0->obj, tail0);
 
 	if (to1) {
+		ringbuf1 = to1->engine[ring->id].ringbuf;
 		ctx_obj1 = to1->engine[ring->id].state;
 		BUG_ON(!ctx_obj1);
 		WARN_ON(!i915_gem_obj_is_pinned(ctx_obj1));
+		WARN_ON(!i915_gem_obj_is_pinned(ringbuf1->obj));
 
-		execlists_ctx_write_tail(ctx_obj1, tail1);
+		execlists_update_context(ctx_obj1, ringbuf1->obj, tail1);
 	}
 
 	execlists_elsp_write(ring, ctx_obj0, ctx_obj1);
-
-	return 0;
 }
 
 static void execlists_context_unqueue(struct intel_engine_cs *ring)
 {
 	struct intel_ctx_submit_request *req0 = NULL, *req1 = NULL;
 	struct intel_ctx_submit_request *cursor = NULL, *tmp = NULL;
-	struct drm_i915_private *dev_priv = ring->dev->dev_private;
 
 	assert_spin_locked(&ring->execlist_lock);
 
@@ -403,7 +423,8 @@ static void execlists_context_unqueue(struct intel_engine_cs *ring)
 			 * will update tail past first request's workload */
 			cursor->elsp_submitted = req0->elsp_submitted;
 			list_del(&req0->execlist_link);
-			queue_work(dev_priv->wq, &req0->work);
+			list_add_tail(&req0->execlist_link,
+				&ring->execlist_retired_req_list);
 			req0 = cursor;
 		} else {
 			req1 = cursor;
@@ -413,9 +434,9 @@ static void execlists_context_unqueue(struct intel_engine_cs *ring)
 
 	WARN_ON(req1 && req1->elsp_submitted);
 
-	WARN_ON(execlists_submit_context(ring, req0->ctx, req0->tail,
-					 req1 ? req1->ctx : NULL,
-					 req1 ? req1->tail : 0));
+	execlists_submit_contexts(ring, req0->ctx, req0->tail,
+				  req1 ? req1->ctx : NULL,
+				  req1 ? req1->tail : 0);
 
 	req0->elsp_submitted++;
 	if (req1)
@@ -425,7 +446,6 @@ static void execlists_context_unqueue(struct intel_engine_cs *ring)
 static bool execlists_check_remove_request(struct intel_engine_cs *ring,
 					   u32 request_id)
 {
-	struct drm_i915_private *dev_priv = ring->dev->dev_private;
 	struct intel_ctx_submit_request *head_req;
 
 	assert_spin_locked(&ring->execlist_lock);
@@ -443,7 +463,8 @@ static bool execlists_check_remove_request(struct intel_engine_cs *ring,
 
 			if (--head_req->elsp_submitted <= 0) {
 				list_del(&head_req->execlist_link);
-				queue_work(dev_priv->wq, &head_req->work);
+				list_add_tail(&head_req->execlist_link,
+					&ring->execlist_retired_req_list);
 				return true;
 			}
 		}
@@ -512,22 +533,6 @@ void intel_execlists_handle_ctx_events(struct intel_engine_cs *ring)
 		   ((u32)ring->next_context_status_buffer & 0x07) << 8);
 }
 
-static void execlists_free_request_task(struct work_struct *work)
-{
-	struct intel_ctx_submit_request *req =
-		container_of(work, struct intel_ctx_submit_request, work);
-	struct drm_device *dev = req->ring->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	intel_runtime_pm_put(dev_priv);
-
-	mutex_lock(&dev->struct_mutex);
-	i915_gem_context_unreference(req->ctx);
-	mutex_unlock(&dev->struct_mutex);
-
-	kfree(req);
-}
-
 static int execlists_context_queue(struct intel_engine_cs *ring,
 				   struct intel_context *to,
 				   u32 tail)
@@ -542,9 +547,12 @@ static int execlists_context_queue(struct intel_engine_cs *ring,
 		return -ENOMEM;
 	req->ctx = to;
 	i915_gem_context_reference(req->ctx);
+
+	if (to != ring->default_context)
+		intel_lr_context_pin(ring, to);
+
 	req->ring = ring;
 	req->tail = tail;
-	INIT_WORK(&req->work, execlists_free_request_task);
 
 	intel_runtime_pm_get(dev_priv);
 
@@ -563,9 +571,10 @@ static int execlists_context_queue(struct intel_engine_cs *ring,
 
 		if (to == tail_req->ctx) {
 			WARN(tail_req->elsp_submitted != 0,
-			     "More than 2 already-submitted reqs queued\n");
+				"More than 2 already-submitted reqs queued\n");
 			list_del(&tail_req->execlist_link);
-			queue_work(dev_priv->wq, &tail_req->work);
+			list_add_tail(&tail_req->execlist_link,
+				&ring->execlist_retired_req_list);
 		}
 	}
 
@@ -733,6 +742,36 @@ int intel_execlists_submission(struct drm_device *dev, struct drm_file *file,
 	return 0;
 }
 
+void intel_execlists_retire_requests(struct intel_engine_cs *ring)
+{
+	struct intel_ctx_submit_request *req, *tmp;
+	struct drm_i915_private *dev_priv = ring->dev->dev_private;
+	unsigned long flags;
+	struct list_head retired_list;
+
+	WARN_ON(!mutex_is_locked(&ring->dev->struct_mutex));
+	if (list_empty(&ring->execlist_retired_req_list))
+		return;
+
+	INIT_LIST_HEAD(&retired_list);
+	spin_lock_irqsave(&ring->execlist_lock, flags);
+	list_replace_init(&ring->execlist_retired_req_list, &retired_list);
+	spin_unlock_irqrestore(&ring->execlist_lock, flags);
+
+	list_for_each_entry_safe(req, tmp, &retired_list, execlist_link) {
+		struct intel_context *ctx = req->ctx;
+		struct drm_i915_gem_object *ctx_obj =
+				ctx->engine[ring->id].state;
+
+		if (ctx_obj && (ctx != ring->default_context))
+			intel_lr_context_unpin(ring, ctx);
+		intel_runtime_pm_put(dev_priv);
+		i915_gem_context_unreference(req->ctx);
+		list_del(&req->execlist_link);
+		kfree(req);
+	}
+}
+
 void intel_logical_ring_stop(struct intel_engine_cs *ring)
 {
 	struct drm_i915_private *dev_priv = ring->dev->dev_private;
@@ -793,9 +832,55 @@ void intel_logical_ring_advance_and_submit(struct intel_ringbuffer *ringbuf)
 	execlists_context_queue(ring, ctx, ringbuf->tail);
 }
 
+static int intel_lr_context_pin(struct intel_engine_cs *ring,
+		struct intel_context *ctx)
+{
+	struct drm_i915_gem_object *ctx_obj = ctx->engine[ring->id].state;
+	struct intel_ringbuffer *ringbuf = ctx->engine[ring->id].ringbuf;
+	int ret = 0;
+
+	WARN_ON(!mutex_is_locked(&ring->dev->struct_mutex));
+	if (ctx->engine[ring->id].unpin_count++ == 0) {
+		ret = i915_gem_obj_ggtt_pin(ctx_obj,
+				GEN8_LR_CONTEXT_ALIGN, 0);
+		if (ret)
+			goto reset_unpin_count;
+
+		ret = intel_pin_and_map_ringbuffer_obj(ring->dev, ringbuf);
+		if (ret)
+			goto unpin_ctx_obj;
+	}
+
+	return ret;
+
+unpin_ctx_obj:
+	i915_gem_object_ggtt_unpin(ctx_obj);
+reset_unpin_count:
+	ctx->engine[ring->id].unpin_count = 0;
+
+	return ret;
+}
+
+void intel_lr_context_unpin(struct intel_engine_cs *ring,
+		struct intel_context *ctx)
+{
+	struct drm_i915_gem_object *ctx_obj = ctx->engine[ring->id].state;
+	struct intel_ringbuffer *ringbuf = ctx->engine[ring->id].ringbuf;
+
+	if (ctx_obj) {
+		WARN_ON(!mutex_is_locked(&ring->dev->struct_mutex));
+		if (--ctx->engine[ring->id].unpin_count == 0) {
+			intel_unpin_ringbuffer_obj(ringbuf);
+			i915_gem_object_ggtt_unpin(ctx_obj);
+		}
+	}
+}
+
 static int logical_ring_alloc_seqno(struct intel_engine_cs *ring,
 				    struct intel_context *ctx)
 {
+	int ret;
+
 	if (ring->outstanding_lazy_seqno)
 		return 0;
 
@@ -806,6 +891,14 @@ static int logical_ring_alloc_seqno(struct intel_engine_cs *ring,
 		if (request == NULL)
 			return -ENOMEM;
 
+		if (ctx != ring->default_context) {
+			ret = intel_lr_context_pin(ring, ctx);
+			if (ret) {
+				kfree(request);
+				return ret;
+			}
+		}
+
 		/* Hold a reference to the context this request belongs to
 		 * (we will need it when the time comes to emit/retire the
 		 * request).
@@ -991,6 +1084,44 @@ int intel_logical_ring_begin(struct intel_ringbuffer *ringbuf, int num_dwords)
 	return 0;
 }
 
+static int intel_logical_ring_workarounds_emit(struct intel_engine_cs *ring,
+					       struct intel_context *ctx)
+{
+	int ret, i;
+	struct intel_ringbuffer *ringbuf = ctx->engine[ring->id].ringbuf;
+	struct drm_device *dev = ring->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct i915_workarounds *w = &dev_priv->workarounds;
+
+	if (WARN_ON(w->count == 0))
+		return 0;
+
+	ring->gpu_caches_dirty = true;
+	ret = logical_ring_flush_all_caches(ringbuf);
+	if (ret)
+		return ret;
+
+	ret = intel_logical_ring_begin(ringbuf, w->count * 2 + 2);
+	if (ret)
+		return ret;
+
+	intel_logical_ring_emit(ringbuf, MI_LOAD_REGISTER_IMM(w->count));
+	for (i = 0; i < w->count; i++) {
+		intel_logical_ring_emit(ringbuf, w->reg[i].addr);
+		intel_logical_ring_emit(ringbuf, w->reg[i].value);
+	}
+	intel_logical_ring_emit(ringbuf, MI_NOOP);
+
+	intel_logical_ring_advance(ringbuf);
+
+	ring->gpu_caches_dirty = true;
+	ret = logical_ring_flush_all_caches(ringbuf);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
 static int gen8_init_common_ring(struct intel_engine_cs *ring)
 {
 	struct drm_device *dev = ring->dev;
@@ -1034,7 +1165,7 @@ static int gen8_init_render_ring(struct intel_engine_cs *ring)
 
 	I915_WRITE(INSTPM, _MASKED_BIT_ENABLE(INSTPM_FORCE_ORDERING));
 
-	return ret;
+	return init_workarounds_ring(ring);
 }
 
 static int gen8_emit_bb_start(struct intel_ringbuffer *ringbuf,
@@ -1063,7 +1194,7 @@ static bool gen8_logical_ring_get_irq(struct intel_engine_cs *ring)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	unsigned long flags;
 
-	if (!dev->irq_enabled)
+	if (WARN_ON(!intel_irqs_enabled(dev_priv)))
 		return false;
 
 	spin_lock_irqsave(&dev_priv->irq_lock, flags);
@@ -1214,11 +1345,13 @@ static int gen8_emit_request(struct intel_ringbuffer *ringbuf)
  */
 void intel_logical_ring_cleanup(struct intel_engine_cs *ring)
 {
-	struct drm_i915_private *dev_priv = ring->dev->dev_private;
+	struct drm_i915_private *dev_priv;
 
 	if (!intel_ring_initialized(ring))
 		return;
 
+	dev_priv = ring->dev->dev_private;
+
 	intel_logical_ring_stop(ring);
 	WARN_ON((I915_READ_MODE(ring) & MODE_IDLE) == 0);
 	ring->preallocated_lazy_request = NULL;
@@ -1248,6 +1381,7 @@ static int logical_ring_init(struct drm_device *dev, struct intel_engine_cs *rin
 	init_waitqueue_head(&ring->irq_queue);
 
 	INIT_LIST_HEAD(&ring->execlist_queue);
+	INIT_LIST_HEAD(&ring->execlist_retired_req_list);
 	spin_lock_init(&ring->execlist_lock);
 	ring->next_context_status_buffer = 0;
 
@@ -1282,6 +1416,7 @@ static int logical_render_ring_init(struct drm_device *dev)
 		ring->irq_keep_mask |= GT_RENDER_L3_PARITY_ERROR_INTERRUPT;
 
 	ring->init = gen8_init_render_ring;
+	ring->init_context = intel_logical_ring_workarounds_emit;
 	ring->cleanup = intel_fini_pipe_control;
 	ring->get_seqno = gen8_get_seqno;
 	ring->set_seqno = gen8_set_seqno;
@@ -1495,7 +1630,6 @@ populate_lr_context(struct intel_context *ctx, struct drm_i915_gem_object *ctx_o
 {
 	struct drm_device *dev = ring->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_i915_gem_object *ring_obj = ringbuf->obj;
 	struct i915_hw_ppgtt *ppgtt = ctx->ppgtt;
 	struct page *page;
 	uint32_t *reg_state;
@@ -1541,7 +1675,9 @@ populate_lr_context(struct intel_context *ctx, struct drm_i915_gem_object *ctx_o
 	reg_state[CTX_RING_TAIL] = RING_TAIL(ring->mmio_base);
 	reg_state[CTX_RING_TAIL+1] = 0;
 	reg_state[CTX_RING_BUFFER_START] = RING_START(ring->mmio_base);
-	reg_state[CTX_RING_BUFFER_START+1] = i915_gem_obj_ggtt_offset(ring_obj);
+	/* Ring buffer start address is not known until the buffer is pinned.
+	 * It is written to the context image in execlists_update_context()
+	 */
 	reg_state[CTX_RING_BUFFER_CONTROL] = RING_CTL(ring->mmio_base);
 	reg_state[CTX_RING_BUFFER_CONTROL+1] =
 			((ringbuf->size - PAGE_SIZE) & RING_NR_PAGES) | RING_VALID;
@@ -1617,12 +1753,18 @@ void intel_lr_context_free(struct intel_context *ctx)
 
 	for (i = 0; i < I915_NUM_RINGS; i++) {
 		struct drm_i915_gem_object *ctx_obj = ctx->engine[i].state;
-		struct intel_ringbuffer *ringbuf = ctx->engine[i].ringbuf;
 
 		if (ctx_obj) {
+			struct intel_ringbuffer *ringbuf =
+					ctx->engine[i].ringbuf;
+			struct intel_engine_cs *ring = ringbuf->ring;
+
+			if (ctx == ring->default_context) {
+				intel_unpin_ringbuffer_obj(ringbuf);
+				i915_gem_object_ggtt_unpin(ctx_obj);
+			}
 			intel_destroy_ringbuffer_obj(ringbuf);
 			kfree(ringbuf);
-			i915_gem_object_ggtt_unpin(ctx_obj);
 			drm_gem_object_unreference(&ctx_obj->base);
 		}
 	}
@@ -1632,11 +1774,14 @@ static uint32_t get_lr_context_size(struct intel_engine_cs *ring)
 {
 	int ret = 0;
 
-	WARN_ON(INTEL_INFO(ring->dev)->gen != 8);
+	WARN_ON(INTEL_INFO(ring->dev)->gen < 8);
 
 	switch (ring->id) {
 	case RCS:
-		ret = GEN8_LR_CONTEXT_RENDER_SIZE;
+		if (INTEL_INFO(ring->dev)->gen >= 9)
+			ret = GEN9_LR_CONTEXT_RENDER_SIZE;
+		else
+			ret = GEN8_LR_CONTEXT_RENDER_SIZE;
 		break;
 	case VCS:
 	case BCS:
@@ -1649,6 +1794,23 @@ static uint32_t get_lr_context_size(struct intel_engine_cs *ring)
 	return ret;
 }
 
+static void lrc_setup_hardware_status_page(struct intel_engine_cs *ring,
+		struct drm_i915_gem_object *default_ctx_obj)
+{
+	struct drm_i915_private *dev_priv = ring->dev->dev_private;
+
+	/* The status page is offset 0 from the default context object
+	 * in LRC mode. */
+	ring->status_page.gfx_addr = i915_gem_obj_ggtt_offset(default_ctx_obj);
+	ring->status_page.page_addr =
+			kmap(sg_page(default_ctx_obj->pages->sgl));
+	ring->status_page.obj = default_ctx_obj;
+
+	I915_WRITE(RING_HWS_PGA(ring->mmio_base),
+			(u32)ring->status_page.gfx_addr);
+	POSTING_READ(RING_HWS_PGA(ring->mmio_base));
+}
+
 /**
  * intel_lr_context_deferred_create() - create the LRC specific bits of a context
  * @ctx: LR context to create.
@@ -1660,11 +1822,12 @@ static uint32_t get_lr_context_size(struct intel_engine_cs *ring)
  * the creation is a deferred call: it's better to make sure first that we need to use
  * a given ring with the context.
  *
- * Return: non-zero on eror.
+ * Return: non-zero on error.
  */
 int intel_lr_context_deferred_create(struct intel_context *ctx,
 				     struct intel_engine_cs *ring)
 {
+	const bool is_global_default_ctx = (ctx == ring->default_context);
 	struct drm_device *dev = ring->dev;
 	struct drm_i915_gem_object *ctx_obj;
 	uint32_t context_size;
@@ -1684,21 +1847,22 @@ int intel_lr_context_deferred_create(struct intel_context *ctx,
 		return ret;
 	}
 
-	ret = i915_gem_obj_ggtt_pin(ctx_obj, GEN8_LR_CONTEXT_ALIGN, 0);
-	if (ret) {
-		DRM_DEBUG_DRIVER("Pin LRC backing obj failed: %d\n", ret);
-		drm_gem_object_unreference(&ctx_obj->base);
-		return ret;
+	if (is_global_default_ctx) {
+		ret = i915_gem_obj_ggtt_pin(ctx_obj, GEN8_LR_CONTEXT_ALIGN, 0);
+		if (ret) {
+			DRM_DEBUG_DRIVER("Pin LRC backing obj failed: %d\n",
+					ret);
+			drm_gem_object_unreference(&ctx_obj->base);
+			return ret;
+		}
 	}
 
 	ringbuf = kzalloc(sizeof(*ringbuf), GFP_KERNEL);
 	if (!ringbuf) {
 		DRM_DEBUG_DRIVER("Failed to allocate ringbuffer %s\n",
 				ring->name);
-		i915_gem_object_ggtt_unpin(ctx_obj);
-		drm_gem_object_unreference(&ctx_obj->base);
 		ret = -ENOMEM;
-		return ret;
+		goto error_unpin_ctx;
 	}
 
 	ringbuf->ring = ring;
@@ -1711,46 +1875,51 @@ int intel_lr_context_deferred_create(struct intel_context *ctx,
 	ringbuf->space = ringbuf->size;
 	ringbuf->last_retired_head = -1;
 
-	/* TODO: For now we put this in the mappable region so that we can reuse
-	 * the existing ringbuffer code which ioremaps it. When we start
-	 * creating many contexts, this will no longer work and we must switch
-	 * to a kmapish interface.
-	 */
-	ret = intel_alloc_ringbuffer_obj(dev, ringbuf);
-	if (ret) {
-		DRM_DEBUG_DRIVER("Failed to allocate ringbuffer obj %s: %d\n",
+	if (ringbuf->obj == NULL) {
+		ret = intel_alloc_ringbuffer_obj(dev, ringbuf);
+		if (ret) {
+			DRM_DEBUG_DRIVER(
+				"Failed to allocate ringbuffer obj %s: %d\n",
 				ring->name, ret);
-		goto error;
+			goto error_free_rbuf;
+		}
+
+		if (is_global_default_ctx) {
+			ret = intel_pin_and_map_ringbuffer_obj(dev, ringbuf);
+			if (ret) {
+				DRM_ERROR(
+					"Failed to pin and map ringbuffer %s: %d\n",
+					ring->name, ret);
+				goto error_destroy_rbuf;
+			}
+		}
+
 	}
 
 	ret = populate_lr_context(ctx, ctx_obj, ring, ringbuf);
 	if (ret) {
 		DRM_DEBUG_DRIVER("Failed to populate LRC: %d\n", ret);
-		intel_destroy_ringbuffer_obj(ringbuf);
 		goto error;
 	}
 
 	ctx->engine[ring->id].ringbuf = ringbuf;
 	ctx->engine[ring->id].state = ctx_obj;
 
-	if (ctx == ring->default_context) {
-		/* The status page is offset 0 from the default context object
-		 * in LRC mode. */
-		ring->status_page.gfx_addr = i915_gem_obj_ggtt_offset(ctx_obj);
-		ring->status_page.page_addr =
-				kmap(sg_page(ctx_obj->pages->sgl));
-		if (ring->status_page.page_addr == NULL)
-			return -ENOMEM;
-		ring->status_page.obj = ctx_obj;
-	}
+	if (ctx == ring->default_context)
+		lrc_setup_hardware_status_page(ring, ctx_obj);
 
 	if (ring->id == RCS && !ctx->rcs_initialized) {
+		if (ring->init_context) {
+			ret = ring->init_context(ring, ctx);
+			if (ret)
+				DRM_ERROR("ring init context: %d\n", ret);
+		}
+
 		ret = intel_lr_context_render_state_init(ring, ctx);
 		if (ret) {
 			DRM_ERROR("Init render state failed: %d\n", ret);
 			ctx->engine[ring->id].ringbuf = NULL;
 			ctx->engine[ring->id].state = NULL;
-			intel_destroy_ringbuffer_obj(ringbuf);
 			goto error;
 		}
 		ctx->rcs_initialized = true;
@@ -1759,8 +1928,15 @@ int intel_lr_context_deferred_create(struct intel_context *ctx,
 	return 0;
 
 error:
+	if (is_global_default_ctx)
+		intel_unpin_ringbuffer_obj(ringbuf);
+error_destroy_rbuf:
+	intel_destroy_ringbuffer_obj(ringbuf);
+error_free_rbuf:
 	kfree(ringbuf);
-	i915_gem_object_ggtt_unpin(ctx_obj);
+error_unpin_ctx:
+	if (is_global_default_ctx)
+		i915_gem_object_ggtt_unpin(ctx_obj);
 	drm_gem_object_unreference(&ctx_obj->base);
 	return ret;
 }
diff --git a/drivers/gpu/drm/i915/intel_lrc.h b/drivers/gpu/drm/i915/intel_lrc.h
index 33c3b4bf28c5..14b216b9be7f 100644
--- a/drivers/gpu/drm/i915/intel_lrc.h
+++ b/drivers/gpu/drm/i915/intel_lrc.h
@@ -24,6 +24,8 @@
 #ifndef _INTEL_LRC_H_
 #define _INTEL_LRC_H_
 
+#define GEN8_LR_CONTEXT_ALIGN 4096
+
 /* Execlists regs */
 #define RING_ELSP(ring)			((ring)->mmio_base+0x230)
 #define RING_EXECLIST_STATUS(ring)	((ring)->mmio_base+0x234)
@@ -67,6 +69,8 @@ int intel_lr_context_render_state_init(struct intel_engine_cs *ring,
 void intel_lr_context_free(struct intel_context *ctx);
 int intel_lr_context_deferred_create(struct intel_context *ctx,
 				     struct intel_engine_cs *ring);
+void intel_lr_context_unpin(struct intel_engine_cs *ring,
+		struct intel_context *ctx);
 
 /* Execlists */
 int intel_sanitize_enable_execlists(struct drm_device *dev, int enable_execlists);
@@ -104,11 +108,11 @@ struct intel_ctx_submit_request {
 	u32 tail;
 
 	struct list_head execlist_link;
-	struct work_struct work;
 
 	int elsp_submitted;
 };
 
 void intel_execlists_handle_ctx_events(struct intel_engine_cs *ring);
+void intel_execlists_retire_requests(struct intel_engine_cs *ring);
 
 #endif /* _INTEL_LRC_H_ */
diff --git a/drivers/gpu/drm/i915/intel_lvds.c b/drivers/gpu/drm/i915/intel_lvds.c
index c0bbf2172446..14654d628ca4 100644
--- a/drivers/gpu/drm/i915/intel_lvds.c
+++ b/drivers/gpu/drm/i915/intel_lvds.c
@@ -76,7 +76,7 @@ static bool intel_lvds_get_hw_state(struct intel_encoder *encoder,
 	u32 tmp;
 
 	power_domain = intel_display_port_power_domain(encoder);
-	if (!intel_display_power_enabled(dev_priv, power_domain))
+	if (!intel_display_power_is_enabled(dev_priv, power_domain))
 		return false;
 
 	tmp = I915_READ(lvds_encoder->reg);
@@ -1116,7 +1116,7 @@ out:
 	drm_connector_register(connector);
 
 	intel_panel_init(&intel_connector->panel, fixed_mode, downclock_mode);
-	intel_panel_setup_backlight(connector);
+	intel_panel_setup_backlight(connector, INVALID_PIPE);
 
 	return;
 
diff --git a/drivers/gpu/drm/i915/intel_panel.c b/drivers/gpu/drm/i915/intel_panel.c
index 41b3be217493..4d63839bd9b4 100644
--- a/drivers/gpu/drm/i915/intel_panel.c
+++ b/drivers/gpu/drm/i915/intel_panel.c
@@ -521,6 +521,9 @@ static u32 _vlv_get_backlight(struct drm_device *dev, enum pipe pipe)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
+	if (WARN_ON(pipe != PIPE_A && pipe != PIPE_B))
+		return 0;
+
 	return I915_READ(VLV_BLC_PWM_CTL(pipe)) & BACKLIGHT_DUTY_CYCLE_MASK;
 }
 
@@ -536,15 +539,17 @@ static u32 intel_panel_get_backlight(struct intel_connector *connector)
 {
 	struct drm_device *dev = connector->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 val;
-	unsigned long flags;
+	struct intel_panel *panel = &connector->panel;
+	u32 val = 0;
 
-	spin_lock_irqsave(&dev_priv->backlight_lock, flags);
+	mutex_lock(&dev_priv->backlight_lock);
 
-	val = dev_priv->display.get_backlight(connector);
-	val = intel_panel_compute_brightness(connector, val);
+	if (panel->backlight.enabled) {
+		val = dev_priv->display.get_backlight(connector);
+		val = intel_panel_compute_brightness(connector, val);
+	}
 
-	spin_unlock_irqrestore(&dev_priv->backlight_lock, flags);
+	mutex_unlock(&dev_priv->backlight_lock);
 
 	DRM_DEBUG_DRIVER("get backlight PWM = %d\n", val);
 	return val;
@@ -603,6 +608,9 @@ static void vlv_set_backlight(struct intel_connector *connector, u32 level)
 	enum pipe pipe = intel_get_pipe_from_connector(connector);
 	u32 tmp;
 
+	if (WARN_ON(pipe != PIPE_A && pipe != PIPE_B))
+		return;
+
 	tmp = I915_READ(VLV_BLC_PWM_CTL(pipe)) & ~BACKLIGHT_DUTY_CYCLE_MASK;
 	I915_WRITE(VLV_BLC_PWM_CTL(pipe), tmp | level);
 }
@@ -626,14 +634,12 @@ static void intel_panel_set_backlight(struct intel_connector *connector,
 	struct drm_device *dev = connector->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_panel *panel = &connector->panel;
-	enum pipe pipe = intel_get_pipe_from_connector(connector);
 	u32 hw_level;
-	unsigned long flags;
 
-	if (!panel->backlight.present || pipe == INVALID_PIPE)
+	if (!panel->backlight.present)
 		return;
 
-	spin_lock_irqsave(&dev_priv->backlight_lock, flags);
+	mutex_lock(&dev_priv->backlight_lock);
 
 	WARN_ON(panel->backlight.max == 0);
 
@@ -643,7 +649,7 @@ static void intel_panel_set_backlight(struct intel_connector *connector,
 	if (panel->backlight.enabled)
 		intel_panel_actually_set_backlight(connector, hw_level);
 
-	spin_unlock_irqrestore(&dev_priv->backlight_lock, flags);
+	mutex_unlock(&dev_priv->backlight_lock);
 }
 
 /* set backlight brightness to level in range [0..max], assuming hw min is
@@ -657,12 +663,17 @@ void intel_panel_set_backlight_acpi(struct intel_connector *connector,
 	struct intel_panel *panel = &connector->panel;
 	enum pipe pipe = intel_get_pipe_from_connector(connector);
 	u32 hw_level;
-	unsigned long flags;
 
+	/*
+	 * INVALID_PIPE may occur during driver init because
+	 * connection_mutex isn't held across the entire backlight
+	 * setup + modeset readout, and the BIOS can issue the
+	 * requests at any time.
+	 */
 	if (!panel->backlight.present || pipe == INVALID_PIPE)
 		return;
 
-	spin_lock_irqsave(&dev_priv->backlight_lock, flags);
+	mutex_lock(&dev_priv->backlight_lock);
 
 	WARN_ON(panel->backlight.max == 0);
 
@@ -678,7 +689,7 @@ void intel_panel_set_backlight_acpi(struct intel_connector *connector,
 	if (panel->backlight.enabled)
 		intel_panel_actually_set_backlight(connector, hw_level);
 
-	spin_unlock_irqrestore(&dev_priv->backlight_lock, flags);
+	mutex_unlock(&dev_priv->backlight_lock);
 }
 
 static void pch_disable_backlight(struct intel_connector *connector)
@@ -720,6 +731,9 @@ static void vlv_disable_backlight(struct intel_connector *connector)
 	enum pipe pipe = intel_get_pipe_from_connector(connector);
 	u32 tmp;
 
+	if (WARN_ON(pipe != PIPE_A && pipe != PIPE_B))
+		return;
+
 	intel_panel_actually_set_backlight(connector, 0);
 
 	tmp = I915_READ(VLV_BLC_PWM_CTL2(pipe));
@@ -731,10 +745,8 @@ void intel_panel_disable_backlight(struct intel_connector *connector)
 	struct drm_device *dev = connector->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_panel *panel = &connector->panel;
-	enum pipe pipe = intel_get_pipe_from_connector(connector);
-	unsigned long flags;
 
-	if (!panel->backlight.present || pipe == INVALID_PIPE)
+	if (!panel->backlight.present)
 		return;
 
 	/*
@@ -748,14 +760,14 @@ void intel_panel_disable_backlight(struct intel_connector *connector)
 		return;
 	}
 
-	spin_lock_irqsave(&dev_priv->backlight_lock, flags);
+	mutex_lock(&dev_priv->backlight_lock);
 
 	if (panel->backlight.device)
 		panel->backlight.device->props.power = FB_BLANK_POWERDOWN;
 	panel->backlight.enabled = false;
 	dev_priv->display.disable_backlight(connector);
 
-	spin_unlock_irqrestore(&dev_priv->backlight_lock, flags);
+	mutex_unlock(&dev_priv->backlight_lock);
 }
 
 static void bdw_enable_backlight(struct intel_connector *connector)
@@ -779,8 +791,9 @@ static void bdw_enable_backlight(struct intel_connector *connector)
 	if (panel->backlight.active_low_pwm)
 		pch_ctl1 |= BLM_PCH_POLARITY;
 
-	/* BDW always uses the pch pwm controls. */
-	pch_ctl1 |= BLM_PCH_OVERRIDE_ENABLE;
+	/* After LPT, override is the default. */
+	if (HAS_PCH_LPT(dev_priv))
+		pch_ctl1 |= BLM_PCH_OVERRIDE_ENABLE;
 
 	I915_WRITE(BLC_PWM_PCH_CTL1, pch_ctl1);
 	POSTING_READ(BLC_PWM_PCH_CTL1);
@@ -909,6 +922,9 @@ static void vlv_enable_backlight(struct intel_connector *connector)
 	enum pipe pipe = intel_get_pipe_from_connector(connector);
 	u32 ctl, ctl2;
 
+	if (WARN_ON(pipe != PIPE_A && pipe != PIPE_B))
+		return;
+
 	ctl2 = I915_READ(VLV_BLC_PWM_CTL2(pipe));
 	if (ctl2 & BLM_PWM_ENABLE) {
 		DRM_DEBUG_KMS("backlight already enabled\n");
@@ -936,14 +952,13 @@ void intel_panel_enable_backlight(struct intel_connector *connector)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_panel *panel = &connector->panel;
 	enum pipe pipe = intel_get_pipe_from_connector(connector);
-	unsigned long flags;
 
-	if (!panel->backlight.present || pipe == INVALID_PIPE)
+	if (!panel->backlight.present)
 		return;
 
 	DRM_DEBUG_KMS("pipe %c\n", pipe_name(pipe));
 
-	spin_lock_irqsave(&dev_priv->backlight_lock, flags);
+	mutex_lock(&dev_priv->backlight_lock);
 
 	WARN_ON(panel->backlight.max == 0);
 
@@ -961,7 +976,7 @@ void intel_panel_enable_backlight(struct intel_connector *connector)
 	if (panel->backlight.device)
 		panel->backlight.device->props.power = FB_BLANK_UNBLANK;
 
-	spin_unlock_irqrestore(&dev_priv->backlight_lock, flags);
+	mutex_unlock(&dev_priv->backlight_lock);
 }
 
 #if IS_ENABLED(CONFIG_BACKLIGHT_CLASS_DEVICE)
@@ -1030,6 +1045,9 @@ static int intel_backlight_device_register(struct intel_connector *connector)
 	if (WARN_ON(panel->backlight.device))
 		return -ENODEV;
 
+	if (!panel->backlight.present)
+		return 0;
+
 	WARN_ON(panel->backlight.max == 0);
 
 	memset(&props, 0, sizeof(props));
@@ -1065,6 +1083,10 @@ static int intel_backlight_device_register(struct intel_connector *connector)
 		panel->backlight.device = NULL;
 		return -ENODEV;
 	}
+
+	DRM_DEBUG_KMS("Connector %s backlight sysfs interface registered\n",
+		      connector->base.name);
+
 	return 0;
 }
 
@@ -1119,7 +1141,7 @@ static u32 get_backlight_min_vbt(struct intel_connector *connector)
 	return scale(min, 0, 255, 0, panel->backlight.max);
 }
 
-static int bdw_setup_backlight(struct intel_connector *connector)
+static int bdw_setup_backlight(struct intel_connector *connector, enum pipe unused)
 {
 	struct drm_device *dev = connector->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -1145,7 +1167,7 @@ static int bdw_setup_backlight(struct intel_connector *connector)
 	return 0;
 }
 
-static int pch_setup_backlight(struct intel_connector *connector)
+static int pch_setup_backlight(struct intel_connector *connector, enum pipe unused)
 {
 	struct drm_device *dev = connector->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -1172,7 +1194,7 @@ static int pch_setup_backlight(struct intel_connector *connector)
 	return 0;
 }
 
-static int i9xx_setup_backlight(struct intel_connector *connector)
+static int i9xx_setup_backlight(struct intel_connector *connector, enum pipe unused)
 {
 	struct drm_device *dev = connector->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -1204,7 +1226,7 @@ static int i9xx_setup_backlight(struct intel_connector *connector)
 	return 0;
 }
 
-static int i965_setup_backlight(struct intel_connector *connector)
+static int i965_setup_backlight(struct intel_connector *connector, enum pipe unused)
 {
 	struct drm_device *dev = connector->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -1234,37 +1256,40 @@ static int i965_setup_backlight(struct intel_connector *connector)
 	return 0;
 }
 
-static int vlv_setup_backlight(struct intel_connector *connector)
+static int vlv_setup_backlight(struct intel_connector *connector, enum pipe pipe)
 {
 	struct drm_device *dev = connector->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_panel *panel = &connector->panel;
-	enum pipe pipe;
+	enum pipe p;
 	u32 ctl, ctl2, val;
 
-	for_each_pipe(dev_priv, pipe) {
-		u32 cur_val = I915_READ(VLV_BLC_PWM_CTL(pipe));
+	for_each_pipe(dev_priv, p) {
+		u32 cur_val = I915_READ(VLV_BLC_PWM_CTL(p));
 
 		/* Skip if the modulation freq is already set */
 		if (cur_val & ~BACKLIGHT_DUTY_CYCLE_MASK)
 			continue;
 
 		cur_val &= BACKLIGHT_DUTY_CYCLE_MASK;
-		I915_WRITE(VLV_BLC_PWM_CTL(pipe), (0xf42 << 16) |
+		I915_WRITE(VLV_BLC_PWM_CTL(p), (0xf42 << 16) |
 			   cur_val);
 	}
 
-	ctl2 = I915_READ(VLV_BLC_PWM_CTL2(PIPE_A));
+	if (WARN_ON(pipe != PIPE_A && pipe != PIPE_B))
+		return -ENODEV;
+
+	ctl2 = I915_READ(VLV_BLC_PWM_CTL2(pipe));
 	panel->backlight.active_low_pwm = ctl2 & BLM_POLARITY_I965;
 
-	ctl = I915_READ(VLV_BLC_PWM_CTL(PIPE_A));
+	ctl = I915_READ(VLV_BLC_PWM_CTL(pipe));
 	panel->backlight.max = ctl >> 16;
 	if (!panel->backlight.max)
 		return -ENODEV;
 
 	panel->backlight.min = get_backlight_min_vbt(connector);
 
-	val = _vlv_get_backlight(dev, PIPE_A);
+	val = _vlv_get_backlight(dev, pipe);
 	panel->backlight.level = intel_panel_compute_brightness(connector, val);
 
 	panel->backlight.enabled = (ctl2 & BLM_PWM_ENABLE) &&
@@ -1273,13 +1298,12 @@ static int vlv_setup_backlight(struct intel_connector *connector)
 	return 0;
 }
 
-int intel_panel_setup_backlight(struct drm_connector *connector)
+int intel_panel_setup_backlight(struct drm_connector *connector, enum pipe pipe)
 {
 	struct drm_device *dev = connector->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_connector *intel_connector = to_intel_connector(connector);
 	struct intel_panel *panel = &intel_connector->panel;
-	unsigned long flags;
 	int ret;
 
 	if (!dev_priv->vbt.backlight.present) {
@@ -1292,9 +1316,9 @@ int intel_panel_setup_backlight(struct drm_connector *connector)
 	}
 
 	/* set level and max in panel struct */
-	spin_lock_irqsave(&dev_priv->backlight_lock, flags);
-	ret = dev_priv->display.setup_backlight(intel_connector);
-	spin_unlock_irqrestore(&dev_priv->backlight_lock, flags);
+	mutex_lock(&dev_priv->backlight_lock);
+	ret = dev_priv->display.setup_backlight(intel_connector, pipe);
+	mutex_unlock(&dev_priv->backlight_lock);
 
 	if (ret) {
 		DRM_DEBUG_KMS("failed to setup backlight for connector %s\n",
@@ -1302,15 +1326,12 @@ int intel_panel_setup_backlight(struct drm_connector *connector)
 		return ret;
 	}
 
-	intel_backlight_device_register(intel_connector);
-
 	panel->backlight.present = true;
 
-	DRM_DEBUG_KMS("backlight initialized, %s, brightness %u/%u, "
-		      "sysfs interface %sregistered\n",
+	DRM_DEBUG_KMS("Connector %s backlight initialized, %s, brightness %u/%u\n",
+		      connector->name,
 		      panel->backlight.enabled ? "enabled" : "disabled",
-		      panel->backlight.level, panel->backlight.max,
-		      panel->backlight.device ? "" : "not ");
+		      panel->backlight.level, panel->backlight.max);
 
 	return 0;
 }
@@ -1321,7 +1342,6 @@ void intel_panel_destroy_backlight(struct drm_connector *connector)
 	struct intel_panel *panel = &intel_connector->panel;
 
 	panel->backlight.present = false;
-	intel_backlight_device_unregister(intel_connector);
 }
 
 /* Set up chip specific backlight functions */
@@ -1329,7 +1349,7 @@ void intel_panel_init_backlight_funcs(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	if (IS_BROADWELL(dev)) {
+	if (IS_BROADWELL(dev) || (INTEL_INFO(dev)->gen >= 9)) {
 		dev_priv->display.setup_backlight = bdw_setup_backlight;
 		dev_priv->display.enable_backlight = bdw_enable_backlight;
 		dev_priv->display.disable_backlight = pch_disable_backlight;
@@ -1384,3 +1404,19 @@ void intel_panel_fini(struct intel_panel *panel)
 		drm_mode_destroy(intel_connector->base.dev,
 				panel->downclock_mode);
 }
+
+void intel_backlight_register(struct drm_device *dev)
+{
+	struct intel_connector *connector;
+
+	list_for_each_entry(connector, &dev->mode_config.connector_list, base.head)
+		intel_backlight_device_register(connector);
+}
+
+void intel_backlight_unregister(struct drm_device *dev)
+{
+	struct intel_connector *connector;
+
+	list_for_each_entry(connector, &dev->mode_config.connector_list, base.head)
+		intel_backlight_device_unregister(connector);
+}
diff --git a/drivers/gpu/drm/i915/intel_pm.c b/drivers/gpu/drm/i915/intel_pm.c
index ad2fd605f76b..964b28e3c630 100644
--- a/drivers/gpu/drm/i915/intel_pm.c
+++ b/drivers/gpu/drm/i915/intel_pm.c
@@ -30,9 +30,6 @@
 #include "intel_drv.h"
 #include "../../../platform/x86/intel_ips.h"
 #include <linux/module.h>
-#include <linux/vgaarb.h>
-#include <drm/i915_powerwell.h>
-#include <linux/pm_runtime.h>
 
 /**
  * RC6 is a special power stage which allows the GPU to enter an very
@@ -66,11 +63,37 @@
  * i915.i915_enable_fbc parameter
  */
 
+static void gen9_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	/*
+	 * WaDisableSDEUnitClockGating:skl
+	 * This seems to be a pre-production w/a.
+	 */
+	I915_WRITE(GEN8_UCGCTL6, I915_READ(GEN8_UCGCTL6) |
+		   GEN8_SDEUNIT_CLOCK_GATE_DISABLE);
+
+	/*
+	 * WaDisableDgMirrorFixInHalfSliceChicken5:skl
+	 * This is a pre-production w/a.
+	 */
+	I915_WRITE(GEN9_HALF_SLICE_CHICKEN5,
+		   I915_READ(GEN9_HALF_SLICE_CHICKEN5) &
+		   ~GEN9_DG_MIRROR_FIX_ENABLE);
+
+	/* Wa4x4STCOptimizationDisable:skl */
+	I915_WRITE(CACHE_MODE_1,
+		   _MASKED_BIT_ENABLE(GEN8_4x4_STC_OPTIMIZATION_DISABLE));
+}
+
 static void i8xx_disable_fbc(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 fbc_ctl;
 
+	dev_priv->fbc.enabled = false;
+
 	/* Disable compression */
 	fbc_ctl = I915_READ(FBC_CONTROL);
 	if ((fbc_ctl & FBC_CTL_EN) == 0)
@@ -99,6 +122,8 @@ static void i8xx_enable_fbc(struct drm_crtc *crtc)
 	int i;
 	u32 fbc_ctl;
 
+	dev_priv->fbc.enabled = true;
+
 	cfb_pitch = dev_priv->fbc.size / FBC_LL_SIZE;
 	if (fb->pitches[0] < cfb_pitch)
 		cfb_pitch = fb->pitches[0];
@@ -153,6 +178,8 @@ static void g4x_enable_fbc(struct drm_crtc *crtc)
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	u32 dpfc_ctl;
 
+	dev_priv->fbc.enabled = true;
+
 	dpfc_ctl = DPFC_CTL_PLANE(intel_crtc->plane) | DPFC_SR_EN;
 	if (drm_format_plane_cpp(fb->pixel_format, 0) == 2)
 		dpfc_ctl |= DPFC_CTL_LIMIT_2X;
@@ -173,6 +200,8 @@ static void g4x_disable_fbc(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 dpfc_ctl;
 
+	dev_priv->fbc.enabled = false;
+
 	/* Disable compression */
 	dpfc_ctl = I915_READ(DPFC_CONTROL);
 	if (dpfc_ctl & DPFC_CTL_EN) {
@@ -224,6 +253,8 @@ static void ironlake_enable_fbc(struct drm_crtc *crtc)
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	u32 dpfc_ctl;
 
+	dev_priv->fbc.enabled = true;
+
 	dpfc_ctl = DPFC_CTL_PLANE(intel_crtc->plane);
 	if (drm_format_plane_cpp(fb->pixel_format, 0) == 2)
 		dev_priv->fbc.threshold++;
@@ -264,6 +295,8 @@ static void ironlake_disable_fbc(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 dpfc_ctl;
 
+	dev_priv->fbc.enabled = false;
+
 	/* Disable compression */
 	dpfc_ctl = I915_READ(ILK_DPFC_CONTROL);
 	if (dpfc_ctl & DPFC_CTL_EN) {
@@ -290,6 +323,8 @@ static void gen7_enable_fbc(struct drm_crtc *crtc)
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	u32 dpfc_ctl;
 
+	dev_priv->fbc.enabled = true;
+
 	dpfc_ctl = IVB_DPFC_CTL_PLANE(intel_crtc->plane);
 	if (drm_format_plane_cpp(fb->pixel_format, 0) == 2)
 		dev_priv->fbc.threshold++;
@@ -339,19 +374,19 @@ 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);
+	return dev_priv->fbc.enabled;
 }
 
-void gen8_fbc_sw_flush(struct drm_device *dev, u32 value)
+void bdw_fbc_sw_flush(struct drm_device *dev, u32 value)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
 	if (!IS_GEN8(dev))
 		return;
 
+	if (!intel_fbc_enabled(dev))
+		return;
+
 	I915_WRITE(MSG_FBC_REND_STATE, value);
 }
 
@@ -1310,6 +1345,7 @@ static bool vlv_compute_drain_latency(struct drm_crtc *crtc,
 				      int *prec_mult,
 				      int *drain_latency)
 {
+	struct drm_device *dev = crtc->dev;
 	int entries;
 	int clock = to_intel_crtc(crtc)->config.adjusted_mode.crtc_clock;
 
@@ -1320,8 +1356,12 @@ static bool vlv_compute_drain_latency(struct drm_crtc *crtc,
 		return false;
 
 	entries = DIV_ROUND_UP(clock, 1000) * pixel_size;
-	*prec_mult = (entries > 128) ? DRAIN_LATENCY_PRECISION_64 :
-				       DRAIN_LATENCY_PRECISION_32;
+	if (IS_CHERRYVIEW(dev))
+		*prec_mult = (entries > 128) ? DRAIN_LATENCY_PRECISION_32 :
+					       DRAIN_LATENCY_PRECISION_16;
+	else
+		*prec_mult = (entries > 128) ? DRAIN_LATENCY_PRECISION_64 :
+					       DRAIN_LATENCY_PRECISION_32;
 	*drain_latency = (64 * (*prec_mult) * 4) / entries;
 
 	if (*drain_latency > DRAIN_LATENCY_MASK)
@@ -1340,15 +1380,18 @@ static bool vlv_compute_drain_latency(struct drm_crtc *crtc,
 
 static void vlv_update_drain_latency(struct drm_crtc *crtc)
 {
-	struct drm_i915_private *dev_priv = crtc->dev->dev_private;
+	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 pixel_size;
 	int drain_latency;
 	enum pipe pipe = intel_crtc->pipe;
 	int plane_prec, prec_mult, plane_dl;
+	const int high_precision = IS_CHERRYVIEW(dev) ?
+		DRAIN_LATENCY_PRECISION_32 : DRAIN_LATENCY_PRECISION_64;
 
-	plane_dl = I915_READ(VLV_DDL(pipe)) & ~(DDL_PLANE_PRECISION_64 |
-		   DRAIN_LATENCY_MASK | DDL_CURSOR_PRECISION_64 |
+	plane_dl = I915_READ(VLV_DDL(pipe)) & ~(DDL_PLANE_PRECISION_HIGH |
+		   DRAIN_LATENCY_MASK | DDL_CURSOR_PRECISION_HIGH |
 		   (DRAIN_LATENCY_MASK << DDL_CURSOR_SHIFT));
 
 	if (!intel_crtc_active(crtc)) {
@@ -1359,9 +1402,9 @@ static void vlv_update_drain_latency(struct drm_crtc *crtc)
 	/* Primary plane Drain Latency */
 	pixel_size = crtc->primary->fb->bits_per_pixel / 8;	/* BPP */
 	if (vlv_compute_drain_latency(crtc, pixel_size, &prec_mult, &drain_latency)) {
-		plane_prec = (prec_mult == DRAIN_LATENCY_PRECISION_64) ?
-					   DDL_PLANE_PRECISION_64 :
-					   DDL_PLANE_PRECISION_32;
+		plane_prec = (prec_mult == high_precision) ?
+					   DDL_PLANE_PRECISION_HIGH :
+					   DDL_PLANE_PRECISION_LOW;
 		plane_dl |= plane_prec | drain_latency;
 	}
 
@@ -1373,9 +1416,9 @@ static void vlv_update_drain_latency(struct drm_crtc *crtc)
 	/* Program cursor DL only if it is enabled */
 	if (intel_crtc->cursor_base &&
 	    vlv_compute_drain_latency(crtc, pixel_size, &prec_mult, &drain_latency)) {
-		plane_prec = (prec_mult == DRAIN_LATENCY_PRECISION_64) ?
-					   DDL_CURSOR_PRECISION_64 :
-					   DDL_CURSOR_PRECISION_32;
+		plane_prec = (prec_mult == high_precision) ?
+					   DDL_CURSOR_PRECISION_HIGH :
+					   DDL_CURSOR_PRECISION_LOW;
 		plane_dl |= plane_prec | (drain_latency << DDL_CURSOR_SHIFT);
 	}
 
@@ -1543,15 +1586,17 @@ static void valleyview_update_sprite_wm(struct drm_plane *plane,
 	int plane_prec;
 	int sprite_dl;
 	int prec_mult;
+	const int high_precision = IS_CHERRYVIEW(dev) ?
+		DRAIN_LATENCY_PRECISION_32 : DRAIN_LATENCY_PRECISION_64;
 
-	sprite_dl = I915_READ(VLV_DDL(pipe)) & ~(DDL_SPRITE_PRECISION_64(sprite) |
+	sprite_dl = I915_READ(VLV_DDL(pipe)) & ~(DDL_SPRITE_PRECISION_HIGH(sprite) |
 		    (DRAIN_LATENCY_MASK << DDL_SPRITE_SHIFT(sprite)));
 
 	if (enabled && vlv_compute_drain_latency(crtc, pixel_size, &prec_mult,
 						 &drain_latency)) {
-		plane_prec = (prec_mult == DRAIN_LATENCY_PRECISION_64) ?
-					   DDL_SPRITE_PRECISION_64(sprite) :
-					   DDL_SPRITE_PRECISION_32(sprite);
+		plane_prec = (prec_mult == high_precision) ?
+					   DDL_SPRITE_PRECISION_HIGH(sprite) :
+					   DDL_SPRITE_PRECISION_LOW(sprite);
 		sprite_dl |= plane_prec |
 			     (drain_latency << DDL_SPRITE_SHIFT(sprite));
 	}
@@ -1915,6 +1960,14 @@ static uint32_t ilk_wm_fbc(uint32_t pri_val, uint32_t horiz_pixels,
 	return DIV_ROUND_UP(pri_val * 64, horiz_pixels * bytes_per_pixel) + 2;
 }
 
+struct skl_pipe_wm_parameters {
+	bool active;
+	uint32_t pipe_htotal;
+	uint32_t pixel_rate; /* in KHz */
+	struct intel_plane_wm_parameters plane[I915_MAX_PLANES];
+	struct intel_plane_wm_parameters cursor;
+};
+
 struct ilk_pipe_wm_parameters {
 	bool active;
 	uint32_t pipe_htotal;
@@ -2226,11 +2279,82 @@ hsw_compute_linetime_wm(struct drm_device *dev, struct drm_crtc *crtc)
 	       PIPE_WM_LINETIME_TIME(linetime);
 }
 
-static void intel_read_wm_latency(struct drm_device *dev, uint16_t wm[5])
+static void intel_read_wm_latency(struct drm_device *dev, uint16_t wm[8])
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
+	if (IS_GEN9(dev)) {
+		uint32_t val;
+		int ret, i;
+		int level, max_level = ilk_wm_max_level(dev);
+
+		/* read the first set of memory latencies[0:3] */
+		val = 0; /* data0 to be programmed to 0 for first set */
+		mutex_lock(&dev_priv->rps.hw_lock);
+		ret = sandybridge_pcode_read(dev_priv,
+					     GEN9_PCODE_READ_MEM_LATENCY,
+					     &val);
+		mutex_unlock(&dev_priv->rps.hw_lock);
+
+		if (ret) {
+			DRM_ERROR("SKL Mailbox read error = %d\n", ret);
+			return;
+		}
+
+		wm[0] = val & GEN9_MEM_LATENCY_LEVEL_MASK;
+		wm[1] = (val >> GEN9_MEM_LATENCY_LEVEL_1_5_SHIFT) &
+				GEN9_MEM_LATENCY_LEVEL_MASK;
+		wm[2] = (val >> GEN9_MEM_LATENCY_LEVEL_2_6_SHIFT) &
+				GEN9_MEM_LATENCY_LEVEL_MASK;
+		wm[3] = (val >> GEN9_MEM_LATENCY_LEVEL_3_7_SHIFT) &
+				GEN9_MEM_LATENCY_LEVEL_MASK;
+
+		/* read the second set of memory latencies[4:7] */
+		val = 1; /* data0 to be programmed to 1 for second set */
+		mutex_lock(&dev_priv->rps.hw_lock);
+		ret = sandybridge_pcode_read(dev_priv,
+					     GEN9_PCODE_READ_MEM_LATENCY,
+					     &val);
+		mutex_unlock(&dev_priv->rps.hw_lock);
+		if (ret) {
+			DRM_ERROR("SKL Mailbox read error = %d\n", ret);
+			return;
+		}
+
+		wm[4] = val & GEN9_MEM_LATENCY_LEVEL_MASK;
+		wm[5] = (val >> GEN9_MEM_LATENCY_LEVEL_1_5_SHIFT) &
+				GEN9_MEM_LATENCY_LEVEL_MASK;
+		wm[6] = (val >> GEN9_MEM_LATENCY_LEVEL_2_6_SHIFT) &
+				GEN9_MEM_LATENCY_LEVEL_MASK;
+		wm[7] = (val >> GEN9_MEM_LATENCY_LEVEL_3_7_SHIFT) &
+				GEN9_MEM_LATENCY_LEVEL_MASK;
+
+		/*
+		 * punit doesn't take into account the read latency so we need
+		 * to add 2us to the various latency levels we retrieve from
+		 * the punit.
+		 *   - W0 is a bit special in that it's the only level that
+		 *   can't be disabled if we want to have display working, so
+		 *   we always add 2us there.
+		 *   - For levels >=1, punit returns 0us latency when they are
+		 *   disabled, so we respect that and don't add 2us then
+		 *
+		 * Additionally, if a level n (n > 1) has a 0us latency, all
+		 * levels m (m >= n) need to be disabled. We make sure to
+		 * sanitize the values out of the punit to satisfy this
+		 * requirement.
+		 */
+		wm[0] += 2;
+		for (level = 1; level <= max_level; level++)
+			if (wm[level] != 0)
+				wm[level] += 2;
+			else {
+				for (i = level + 1; i <= max_level; i++)
+					wm[i] = 0;
+
+				break;
+			}
+	} else if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
 		uint64_t sskpd = I915_READ64(MCH_SSKPD);
 
 		wm[0] = (sskpd >> 56) & 0xFF;
@@ -2278,7 +2402,9 @@ static void intel_fixup_cur_wm_latency(struct drm_device *dev, uint16_t wm[5])
 int ilk_wm_max_level(const struct drm_device *dev)
 {
 	/* how many WM levels are we expecting */
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+	if (IS_GEN9(dev))
+		return 7;
+	else if (IS_HASWELL(dev) || IS_BROADWELL(dev))
 		return 4;
 	else if (INTEL_INFO(dev)->gen >= 6)
 		return 3;
@@ -2288,7 +2414,7 @@ int ilk_wm_max_level(const struct drm_device *dev)
 
 static void intel_print_wm_latency(struct drm_device *dev,
 				   const char *name,
-				   const uint16_t wm[5])
+				   const uint16_t wm[8])
 {
 	int level, max_level = ilk_wm_max_level(dev);
 
@@ -2301,8 +2427,13 @@ static void intel_print_wm_latency(struct drm_device *dev,
 			continue;
 		}
 
-		/* WM1+ latency values in 0.5us units */
-		if (level > 0)
+		/*
+		 * - latencies are in us on gen9.
+		 * - before then, WM1+ latency values are in 0.5us units
+		 */
+		if (IS_GEN9(dev))
+			latency *= 10;
+		else if (level > 0)
 			latency *= 5;
 
 		DRM_DEBUG_KMS("%s WM%d latency %u (%u.%u usec)\n",
@@ -2370,6 +2501,14 @@ static void ilk_setup_wm_latency(struct drm_device *dev)
 		snb_wm_latency_quirk(dev);
 }
 
+static void skl_setup_wm_latency(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	intel_read_wm_latency(dev, dev_priv->wm.skl_latency);
+	intel_print_wm_latency(dev, "Gen9 Plane", dev_priv->wm.skl_latency);
+}
+
 static void ilk_compute_wm_parameters(struct drm_crtc *crtc,
 				      struct ilk_pipe_wm_parameters *p)
 {
@@ -2860,6 +2999,769 @@ static bool ilk_disable_lp_wm(struct drm_device *dev)
 	return _ilk_disable_lp_wm(dev_priv, WM_DIRTY_LP_ALL);
 }
 
+/*
+ * On gen9, we need to allocate Display Data Buffer (DDB) portions to the
+ * different active planes.
+ */
+
+#define SKL_DDB_SIZE		896	/* in blocks */
+
+static void
+skl_ddb_get_pipe_allocation_limits(struct drm_device *dev,
+				   struct drm_crtc *for_crtc,
+				   const struct intel_wm_config *config,
+				   const struct skl_pipe_wm_parameters *params,
+				   struct skl_ddb_entry *alloc /* out */)
+{
+	struct drm_crtc *crtc;
+	unsigned int pipe_size, ddb_size;
+	int nth_active_pipe;
+
+	if (!params->active) {
+		alloc->start = 0;
+		alloc->end = 0;
+		return;
+	}
+
+	ddb_size = SKL_DDB_SIZE;
+
+	ddb_size -= 4; /* 4 blocks for bypass path allocation */
+
+	nth_active_pipe = 0;
+	for_each_crtc(dev, crtc) {
+		if (!intel_crtc_active(crtc))
+			continue;
+
+		if (crtc == for_crtc)
+			break;
+
+		nth_active_pipe++;
+	}
+
+	pipe_size = ddb_size / config->num_pipes_active;
+	alloc->start = nth_active_pipe * ddb_size / config->num_pipes_active;
+	alloc->end = alloc->start + pipe_size;
+}
+
+static unsigned int skl_cursor_allocation(const struct intel_wm_config *config)
+{
+	if (config->num_pipes_active == 1)
+		return 32;
+
+	return 8;
+}
+
+static void skl_ddb_entry_init_from_hw(struct skl_ddb_entry *entry, u32 reg)
+{
+	entry->start = reg & 0x3ff;
+	entry->end = (reg >> 16) & 0x3ff;
+	if (entry->end)
+		entry->end += 1;
+}
+
+void skl_ddb_get_hw_state(struct drm_i915_private *dev_priv,
+			  struct skl_ddb_allocation *ddb /* out */)
+{
+	struct drm_device *dev = dev_priv->dev;
+	enum pipe pipe;
+	int plane;
+	u32 val;
+
+	for_each_pipe(dev_priv, pipe) {
+		for_each_plane(pipe, plane) {
+			val = I915_READ(PLANE_BUF_CFG(pipe, plane));
+			skl_ddb_entry_init_from_hw(&ddb->plane[pipe][plane],
+						   val);
+		}
+
+		val = I915_READ(CUR_BUF_CFG(pipe));
+		skl_ddb_entry_init_from_hw(&ddb->cursor[pipe], val);
+	}
+}
+
+static unsigned int
+skl_plane_relative_data_rate(const struct intel_plane_wm_parameters *p)
+{
+	return p->horiz_pixels * p->vert_pixels * p->bytes_per_pixel;
+}
+
+/*
+ * We don't overflow 32 bits. Worst case is 3 planes enabled, each fetching
+ * a 8192x4096@32bpp framebuffer:
+ *   3 * 4096 * 8192  * 4 < 2^32
+ */
+static unsigned int
+skl_get_total_relative_data_rate(struct intel_crtc *intel_crtc,
+				 const struct skl_pipe_wm_parameters *params)
+{
+	unsigned int total_data_rate = 0;
+	int plane;
+
+	for (plane = 0; plane < intel_num_planes(intel_crtc); plane++) {
+		const struct intel_plane_wm_parameters *p;
+
+		p = &params->plane[plane];
+		if (!p->enabled)
+			continue;
+
+		total_data_rate += skl_plane_relative_data_rate(p);
+	}
+
+	return total_data_rate;
+}
+
+static void
+skl_allocate_pipe_ddb(struct drm_crtc *crtc,
+		      const struct intel_wm_config *config,
+		      const struct skl_pipe_wm_parameters *params,
+		      struct skl_ddb_allocation *ddb /* out */)
+{
+	struct drm_device *dev = crtc->dev;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	enum pipe pipe = intel_crtc->pipe;
+	struct skl_ddb_entry *alloc = &ddb->pipe[pipe];
+	uint16_t alloc_size, start, cursor_blocks;
+	unsigned int total_data_rate;
+	int plane;
+
+	skl_ddb_get_pipe_allocation_limits(dev, crtc, config, params, alloc);
+	alloc_size = skl_ddb_entry_size(alloc);
+	if (alloc_size == 0) {
+		memset(ddb->plane[pipe], 0, sizeof(ddb->plane[pipe]));
+		memset(&ddb->cursor[pipe], 0, sizeof(ddb->cursor[pipe]));
+		return;
+	}
+
+	cursor_blocks = skl_cursor_allocation(config);
+	ddb->cursor[pipe].start = alloc->end - cursor_blocks;
+	ddb->cursor[pipe].end = alloc->end;
+
+	alloc_size -= cursor_blocks;
+	alloc->end -= cursor_blocks;
+
+	/*
+	 * Each active plane get a portion of the remaining space, in
+	 * proportion to the amount of data they need to fetch from memory.
+	 *
+	 * FIXME: we may not allocate every single block here.
+	 */
+	total_data_rate = skl_get_total_relative_data_rate(intel_crtc, params);
+
+	start = alloc->start;
+	for (plane = 0; plane < intel_num_planes(intel_crtc); plane++) {
+		const struct intel_plane_wm_parameters *p;
+		unsigned int data_rate;
+		uint16_t plane_blocks;
+
+		p = &params->plane[plane];
+		if (!p->enabled)
+			continue;
+
+		data_rate = skl_plane_relative_data_rate(p);
+
+		/*
+		 * promote the expression to 64 bits to avoid overflowing, the
+		 * result is < available as data_rate / total_data_rate < 1
+		 */
+		plane_blocks = div_u64((uint64_t)alloc_size * data_rate,
+				       total_data_rate);
+
+		ddb->plane[pipe][plane].start = start;
+		ddb->plane[pipe][plane].end = start + plane_blocks;
+
+		start += plane_blocks;
+	}
+
+}
+
+static uint32_t skl_pipe_pixel_rate(const struct intel_crtc_config *config)
+{
+	/* TODO: Take into account the scalers once we support them */
+	return config->adjusted_mode.crtc_clock;
+}
+
+/*
+ * The max latency should be 257 (max the punit can code is 255 and we add 2us
+ * for the read latency) and bytes_per_pixel should always be <= 8, so that
+ * should allow pixel_rate up to ~2 GHz which seems sufficient since max
+ * 2xcdclk is 1350 MHz and the pixel rate should never exceed that.
+*/
+static uint32_t skl_wm_method1(uint32_t pixel_rate, uint8_t bytes_per_pixel,
+			       uint32_t latency)
+{
+	uint32_t wm_intermediate_val, ret;
+
+	if (latency == 0)
+		return UINT_MAX;
+
+	wm_intermediate_val = latency * pixel_rate * bytes_per_pixel;
+	ret = DIV_ROUND_UP(wm_intermediate_val, 1000);
+
+	return ret;
+}
+
+static uint32_t skl_wm_method2(uint32_t pixel_rate, uint32_t pipe_htotal,
+			       uint32_t horiz_pixels, uint8_t bytes_per_pixel,
+			       uint32_t latency)
+{
+	uint32_t ret, plane_bytes_per_line, wm_intermediate_val;
+
+	if (latency == 0)
+		return UINT_MAX;
+
+	plane_bytes_per_line = horiz_pixels * bytes_per_pixel;
+	wm_intermediate_val = latency * pixel_rate;
+	ret = DIV_ROUND_UP(wm_intermediate_val, pipe_htotal * 1000) *
+				plane_bytes_per_line;
+
+	return ret;
+}
+
+static bool skl_ddb_allocation_changed(const struct skl_ddb_allocation *new_ddb,
+				       const struct intel_crtc *intel_crtc)
+{
+	struct drm_device *dev = intel_crtc->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	const struct skl_ddb_allocation *cur_ddb = &dev_priv->wm.skl_hw.ddb;
+	enum pipe pipe = intel_crtc->pipe;
+
+	if (memcmp(new_ddb->plane[pipe], cur_ddb->plane[pipe],
+		   sizeof(new_ddb->plane[pipe])))
+		return true;
+
+	if (memcmp(&new_ddb->cursor[pipe], &cur_ddb->cursor[pipe],
+		    sizeof(new_ddb->cursor[pipe])))
+		return true;
+
+	return false;
+}
+
+static void skl_compute_wm_global_parameters(struct drm_device *dev,
+					     struct intel_wm_config *config)
+{
+	struct drm_crtc *crtc;
+	struct drm_plane *plane;
+
+	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head)
+		config->num_pipes_active += intel_crtc_active(crtc);
+
+	/* FIXME: I don't think we need those two global parameters on SKL */
+	list_for_each_entry(plane, &dev->mode_config.plane_list, head) {
+		struct intel_plane *intel_plane = to_intel_plane(plane);
+
+		config->sprites_enabled |= intel_plane->wm.enabled;
+		config->sprites_scaled |= intel_plane->wm.scaled;
+	}
+}
+
+static void skl_compute_wm_pipe_parameters(struct drm_crtc *crtc,
+					   struct skl_pipe_wm_parameters *p)
+{
+	struct drm_device *dev = crtc->dev;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	enum pipe pipe = intel_crtc->pipe;
+	struct drm_plane *plane;
+	int i = 1; /* Index for sprite planes start */
+
+	p->active = intel_crtc_active(crtc);
+	if (p->active) {
+		p->pipe_htotal = intel_crtc->config.adjusted_mode.crtc_htotal;
+		p->pixel_rate = skl_pipe_pixel_rate(&intel_crtc->config);
+
+		/*
+		 * For now, assume primary and cursor planes are always enabled.
+		 */
+		p->plane[0].enabled = true;
+		p->plane[0].bytes_per_pixel =
+			crtc->primary->fb->bits_per_pixel / 8;
+		p->plane[0].horiz_pixels = intel_crtc->config.pipe_src_w;
+		p->plane[0].vert_pixels = intel_crtc->config.pipe_src_h;
+
+		p->cursor.enabled = true;
+		p->cursor.bytes_per_pixel = 4;
+		p->cursor.horiz_pixels = intel_crtc->cursor_width ?
+					 intel_crtc->cursor_width : 64;
+	}
+
+	list_for_each_entry(plane, &dev->mode_config.plane_list, head) {
+		struct intel_plane *intel_plane = to_intel_plane(plane);
+
+		if (intel_plane->pipe == pipe)
+			p->plane[i++] = intel_plane->wm;
+	}
+}
+
+static bool skl_compute_plane_wm(struct skl_pipe_wm_parameters *p,
+				 struct intel_plane_wm_parameters *p_params,
+				 uint16_t ddb_allocation,
+				 uint32_t mem_value,
+				 uint16_t *out_blocks, /* out */
+				 uint8_t *out_lines /* out */)
+{
+	uint32_t method1, method2, plane_bytes_per_line, res_blocks, res_lines;
+	uint32_t result_bytes;
+
+	if (mem_value == 0 || !p->active || !p_params->enabled)
+		return false;
+
+	method1 = skl_wm_method1(p->pixel_rate,
+				 p_params->bytes_per_pixel,
+				 mem_value);
+	method2 = skl_wm_method2(p->pixel_rate,
+				 p->pipe_htotal,
+				 p_params->horiz_pixels,
+				 p_params->bytes_per_pixel,
+				 mem_value);
+
+	plane_bytes_per_line = p_params->horiz_pixels *
+					p_params->bytes_per_pixel;
+
+	/* For now xtile and linear */
+	if (((ddb_allocation * 512) / plane_bytes_per_line) >= 1)
+		result_bytes = min(method1, method2);
+	else
+		result_bytes = method1;
+
+	res_blocks = DIV_ROUND_UP(result_bytes, 512) + 1;
+	res_lines = DIV_ROUND_UP(result_bytes, plane_bytes_per_line);
+
+	if (res_blocks > ddb_allocation || res_lines > 31)
+		return false;
+
+	*out_blocks = res_blocks;
+	*out_lines = res_lines;
+
+	return true;
+}
+
+static void skl_compute_wm_level(const struct drm_i915_private *dev_priv,
+				 struct skl_ddb_allocation *ddb,
+				 struct skl_pipe_wm_parameters *p,
+				 enum pipe pipe,
+				 int level,
+				 int num_planes,
+				 struct skl_wm_level *result)
+{
+	uint16_t latency = dev_priv->wm.skl_latency[level];
+	uint16_t ddb_blocks;
+	int i;
+
+	for (i = 0; i < num_planes; i++) {
+		ddb_blocks = skl_ddb_entry_size(&ddb->plane[pipe][i]);
+
+		result->plane_en[i] = skl_compute_plane_wm(p, &p->plane[i],
+						ddb_blocks,
+						latency,
+						&result->plane_res_b[i],
+						&result->plane_res_l[i]);
+	}
+
+	ddb_blocks = skl_ddb_entry_size(&ddb->cursor[pipe]);
+	result->cursor_en = skl_compute_plane_wm(p, &p->cursor, ddb_blocks,
+						 latency, &result->cursor_res_b,
+						 &result->cursor_res_l);
+}
+
+static uint32_t
+skl_compute_linetime_wm(struct drm_crtc *crtc, struct skl_pipe_wm_parameters *p)
+{
+	if (!intel_crtc_active(crtc))
+		return 0;
+
+	return DIV_ROUND_UP(8 * p->pipe_htotal * 1000, p->pixel_rate);
+
+}
+
+static void skl_compute_transition_wm(struct drm_crtc *crtc,
+				      struct skl_pipe_wm_parameters *params,
+				      struct skl_wm_level *trans_wm /* out */)
+{
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int i;
+
+	if (!params->active)
+		return;
+
+	/* Until we know more, just disable transition WMs */
+	for (i = 0; i < intel_num_planes(intel_crtc); i++)
+		trans_wm->plane_en[i] = false;
+	trans_wm->cursor_en = false;
+}
+
+static void skl_compute_pipe_wm(struct drm_crtc *crtc,
+				struct skl_ddb_allocation *ddb,
+				struct skl_pipe_wm_parameters *params,
+				struct skl_pipe_wm *pipe_wm)
+{
+	struct drm_device *dev = crtc->dev;
+	const struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int level, max_level = ilk_wm_max_level(dev);
+
+	for (level = 0; level <= max_level; level++) {
+		skl_compute_wm_level(dev_priv, ddb, params, intel_crtc->pipe,
+				     level, intel_num_planes(intel_crtc),
+				     &pipe_wm->wm[level]);
+	}
+	pipe_wm->linetime = skl_compute_linetime_wm(crtc, params);
+
+	skl_compute_transition_wm(crtc, params, &pipe_wm->trans_wm);
+}
+
+static void skl_compute_wm_results(struct drm_device *dev,
+				   struct skl_pipe_wm_parameters *p,
+				   struct skl_pipe_wm *p_wm,
+				   struct skl_wm_values *r,
+				   struct intel_crtc *intel_crtc)
+{
+	int level, max_level = ilk_wm_max_level(dev);
+	enum pipe pipe = intel_crtc->pipe;
+	uint32_t temp;
+	int i;
+
+	for (level = 0; level <= max_level; level++) {
+		for (i = 0; i < intel_num_planes(intel_crtc); i++) {
+			temp = 0;
+
+			temp |= p_wm->wm[level].plane_res_l[i] <<
+					PLANE_WM_LINES_SHIFT;
+			temp |= p_wm->wm[level].plane_res_b[i];
+			if (p_wm->wm[level].plane_en[i])
+				temp |= PLANE_WM_EN;
+
+			r->plane[pipe][i][level] = temp;
+		}
+
+		temp = 0;
+
+		temp |= p_wm->wm[level].cursor_res_l << PLANE_WM_LINES_SHIFT;
+		temp |= p_wm->wm[level].cursor_res_b;
+
+		if (p_wm->wm[level].cursor_en)
+			temp |= PLANE_WM_EN;
+
+		r->cursor[pipe][level] = temp;
+
+	}
+
+	/* transition WMs */
+	for (i = 0; i < intel_num_planes(intel_crtc); i++) {
+		temp = 0;
+		temp |= p_wm->trans_wm.plane_res_l[i] << PLANE_WM_LINES_SHIFT;
+		temp |= p_wm->trans_wm.plane_res_b[i];
+		if (p_wm->trans_wm.plane_en[i])
+			temp |= PLANE_WM_EN;
+
+		r->plane_trans[pipe][i] = temp;
+	}
+
+	temp = 0;
+	temp |= p_wm->trans_wm.cursor_res_l << PLANE_WM_LINES_SHIFT;
+	temp |= p_wm->trans_wm.cursor_res_b;
+	if (p_wm->trans_wm.cursor_en)
+		temp |= PLANE_WM_EN;
+
+	r->cursor_trans[pipe] = temp;
+
+	r->wm_linetime[pipe] = p_wm->linetime;
+}
+
+static void skl_ddb_entry_write(struct drm_i915_private *dev_priv, uint32_t reg,
+				const struct skl_ddb_entry *entry)
+{
+	if (entry->end)
+		I915_WRITE(reg, (entry->end - 1) << 16 | entry->start);
+	else
+		I915_WRITE(reg, 0);
+}
+
+static void skl_write_wm_values(struct drm_i915_private *dev_priv,
+				const struct skl_wm_values *new)
+{
+	struct drm_device *dev = dev_priv->dev;
+	struct intel_crtc *crtc;
+
+	list_for_each_entry(crtc, &dev->mode_config.crtc_list, base.head) {
+		int i, level, max_level = ilk_wm_max_level(dev);
+		enum pipe pipe = crtc->pipe;
+
+		if (!new->dirty[pipe])
+			continue;
+
+		I915_WRITE(PIPE_WM_LINETIME(pipe), new->wm_linetime[pipe]);
+
+		for (level = 0; level <= max_level; level++) {
+			for (i = 0; i < intel_num_planes(crtc); i++)
+				I915_WRITE(PLANE_WM(pipe, i, level),
+					   new->plane[pipe][i][level]);
+			I915_WRITE(CUR_WM(pipe, level),
+				   new->cursor[pipe][level]);
+		}
+		for (i = 0; i < intel_num_planes(crtc); i++)
+			I915_WRITE(PLANE_WM_TRANS(pipe, i),
+				   new->plane_trans[pipe][i]);
+		I915_WRITE(CUR_WM_TRANS(pipe), new->cursor_trans[pipe]);
+
+		for (i = 0; i < intel_num_planes(crtc); i++)
+			skl_ddb_entry_write(dev_priv,
+					    PLANE_BUF_CFG(pipe, i),
+					    &new->ddb.plane[pipe][i]);
+
+		skl_ddb_entry_write(dev_priv, CUR_BUF_CFG(pipe),
+				    &new->ddb.cursor[pipe]);
+	}
+}
+
+/*
+ * When setting up a new DDB allocation arrangement, we need to correctly
+ * sequence the times at which the new allocations for the pipes are taken into
+ * account or we'll have pipes fetching from space previously allocated to
+ * another pipe.
+ *
+ * Roughly the sequence looks like:
+ *  1. re-allocate the pipe(s) with the allocation being reduced and not
+ *     overlapping with a previous light-up pipe (another way to put it is:
+ *     pipes with their new allocation strickly included into their old ones).
+ *  2. re-allocate the other pipes that get their allocation reduced
+ *  3. allocate the pipes having their allocation increased
+ *
+ * Steps 1. and 2. are here to take care of the following case:
+ * - Initially DDB looks like this:
+ *     |   B    |   C    |
+ * - enable pipe A.
+ * - pipe B has a reduced DDB allocation that overlaps with the old pipe C
+ *   allocation
+ *     |  A  |  B  |  C  |
+ *
+ * We need to sequence the re-allocation: C, B, A (and not B, C, A).
+ */
+
+static void
+skl_wm_flush_pipe(struct drm_i915_private *dev_priv, enum pipe pipe, int pass)
+{
+	struct drm_device *dev = dev_priv->dev;
+	int plane;
+
+	DRM_DEBUG_KMS("flush pipe %c (pass %d)\n", pipe_name(pipe), pass);
+
+	for_each_plane(pipe, plane) {
+		I915_WRITE(PLANE_SURF(pipe, plane),
+			   I915_READ(PLANE_SURF(pipe, plane)));
+	}
+	I915_WRITE(CURBASE(pipe), I915_READ(CURBASE(pipe)));
+}
+
+static bool
+skl_ddb_allocation_included(const struct skl_ddb_allocation *old,
+			    const struct skl_ddb_allocation *new,
+			    enum pipe pipe)
+{
+	uint16_t old_size, new_size;
+
+	old_size = skl_ddb_entry_size(&old->pipe[pipe]);
+	new_size = skl_ddb_entry_size(&new->pipe[pipe]);
+
+	return old_size != new_size &&
+	       new->pipe[pipe].start >= old->pipe[pipe].start &&
+	       new->pipe[pipe].end <= old->pipe[pipe].end;
+}
+
+static void skl_flush_wm_values(struct drm_i915_private *dev_priv,
+				struct skl_wm_values *new_values)
+{
+	struct drm_device *dev = dev_priv->dev;
+	struct skl_ddb_allocation *cur_ddb, *new_ddb;
+	bool reallocated[I915_MAX_PIPES] = {false, false, false};
+	struct intel_crtc *crtc;
+	enum pipe pipe;
+
+	new_ddb = &new_values->ddb;
+	cur_ddb = &dev_priv->wm.skl_hw.ddb;
+
+	/*
+	 * First pass: flush the pipes with the new allocation contained into
+	 * the old space.
+	 *
+	 * We'll wait for the vblank on those pipes to ensure we can safely
+	 * re-allocate the freed space without this pipe fetching from it.
+	 */
+	for_each_intel_crtc(dev, crtc) {
+		if (!crtc->active)
+			continue;
+
+		pipe = crtc->pipe;
+
+		if (!skl_ddb_allocation_included(cur_ddb, new_ddb, pipe))
+			continue;
+
+		skl_wm_flush_pipe(dev_priv, pipe, 1);
+		intel_wait_for_vblank(dev, pipe);
+
+		reallocated[pipe] = true;
+	}
+
+
+	/*
+	 * Second pass: flush the pipes that are having their allocation
+	 * reduced, but overlapping with a previous allocation.
+	 *
+	 * Here as well we need to wait for the vblank to make sure the freed
+	 * space is not used anymore.
+	 */
+	for_each_intel_crtc(dev, crtc) {
+		if (!crtc->active)
+			continue;
+
+		pipe = crtc->pipe;
+
+		if (reallocated[pipe])
+			continue;
+
+		if (skl_ddb_entry_size(&new_ddb->pipe[pipe]) <
+		    skl_ddb_entry_size(&cur_ddb->pipe[pipe])) {
+			skl_wm_flush_pipe(dev_priv, pipe, 2);
+			intel_wait_for_vblank(dev, pipe);
+		}
+
+		reallocated[pipe] = true;
+	}
+
+	/*
+	 * Third pass: flush the pipes that got more space allocated.
+	 *
+	 * We don't need to actively wait for the update here, next vblank
+	 * will just get more DDB space with the correct WM values.
+	 */
+	for_each_intel_crtc(dev, crtc) {
+		if (!crtc->active)
+			continue;
+
+		pipe = crtc->pipe;
+
+		/*
+		 * At this point, only the pipes more space than before are
+		 * left to re-allocate.
+		 */
+		if (reallocated[pipe])
+			continue;
+
+		skl_wm_flush_pipe(dev_priv, pipe, 3);
+	}
+}
+
+static bool skl_update_pipe_wm(struct drm_crtc *crtc,
+			       struct skl_pipe_wm_parameters *params,
+			       struct intel_wm_config *config,
+			       struct skl_ddb_allocation *ddb, /* out */
+			       struct skl_pipe_wm *pipe_wm /* out */)
+{
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+
+	skl_compute_wm_pipe_parameters(crtc, params);
+	skl_allocate_pipe_ddb(crtc, config, params, ddb);
+	skl_compute_pipe_wm(crtc, ddb, params, pipe_wm);
+
+	if (!memcmp(&intel_crtc->wm.skl_active, pipe_wm, sizeof(*pipe_wm)))
+		return false;
+
+	intel_crtc->wm.skl_active = *pipe_wm;
+	return true;
+}
+
+static void skl_update_other_pipe_wm(struct drm_device *dev,
+				     struct drm_crtc *crtc,
+				     struct intel_wm_config *config,
+				     struct skl_wm_values *r)
+{
+	struct intel_crtc *intel_crtc;
+	struct intel_crtc *this_crtc = to_intel_crtc(crtc);
+
+	/*
+	 * If the WM update hasn't changed the allocation for this_crtc (the
+	 * crtc we are currently computing the new WM values for), other
+	 * enabled crtcs will keep the same allocation and we don't need to
+	 * recompute anything for them.
+	 */
+	if (!skl_ddb_allocation_changed(&r->ddb, this_crtc))
+		return;
+
+	/*
+	 * Otherwise, because of this_crtc being freshly enabled/disabled, the
+	 * other active pipes need new DDB allocation and WM values.
+	 */
+	list_for_each_entry(intel_crtc, &dev->mode_config.crtc_list,
+				base.head) {
+		struct skl_pipe_wm_parameters params = {};
+		struct skl_pipe_wm pipe_wm = {};
+		bool wm_changed;
+
+		if (this_crtc->pipe == intel_crtc->pipe)
+			continue;
+
+		if (!intel_crtc->active)
+			continue;
+
+		wm_changed = skl_update_pipe_wm(&intel_crtc->base,
+						&params, config,
+						&r->ddb, &pipe_wm);
+
+		/*
+		 * If we end up re-computing the other pipe WM values, it's
+		 * because it was really needed, so we expect the WM values to
+		 * be different.
+		 */
+		WARN_ON(!wm_changed);
+
+		skl_compute_wm_results(dev, &params, &pipe_wm, r, intel_crtc);
+		r->dirty[intel_crtc->pipe] = true;
+	}
+}
+
+static void skl_update_wm(struct drm_crtc *crtc)
+{
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct skl_pipe_wm_parameters params = {};
+	struct skl_wm_values *results = &dev_priv->wm.skl_results;
+	struct skl_pipe_wm pipe_wm = {};
+	struct intel_wm_config config = {};
+
+	memset(results, 0, sizeof(*results));
+
+	skl_compute_wm_global_parameters(dev, &config);
+
+	if (!skl_update_pipe_wm(crtc, &params, &config,
+				&results->ddb, &pipe_wm))
+		return;
+
+	skl_compute_wm_results(dev, &params, &pipe_wm, results, intel_crtc);
+	results->dirty[intel_crtc->pipe] = true;
+
+	skl_update_other_pipe_wm(dev, crtc, &config, results);
+	skl_write_wm_values(dev_priv, results);
+	skl_flush_wm_values(dev_priv, results);
+
+	/* store the new configuration */
+	dev_priv->wm.skl_hw = *results;
+}
+
+static void
+skl_update_sprite_wm(struct drm_plane *plane, struct drm_crtc *crtc,
+		     uint32_t sprite_width, uint32_t sprite_height,
+		     int pixel_size, bool enabled, bool scaled)
+{
+	struct intel_plane *intel_plane = to_intel_plane(plane);
+
+	intel_plane->wm.enabled = enabled;
+	intel_plane->wm.scaled = scaled;
+	intel_plane->wm.horiz_pixels = sprite_width;
+	intel_plane->wm.vert_pixels = sprite_height;
+	intel_plane->wm.bytes_per_pixel = pixel_size;
+
+	skl_update_wm(crtc);
+}
+
 static void ilk_update_wm(struct drm_crtc *crtc)
 {
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
@@ -2934,6 +3836,113 @@ ilk_update_sprite_wm(struct drm_plane *plane,
 	ilk_update_wm(crtc);
 }
 
+static void skl_pipe_wm_active_state(uint32_t val,
+				     struct skl_pipe_wm *active,
+				     bool is_transwm,
+				     bool is_cursor,
+				     int i,
+				     int level)
+{
+	bool is_enabled = (val & PLANE_WM_EN) != 0;
+
+	if (!is_transwm) {
+		if (!is_cursor) {
+			active->wm[level].plane_en[i] = is_enabled;
+			active->wm[level].plane_res_b[i] =
+					val & PLANE_WM_BLOCKS_MASK;
+			active->wm[level].plane_res_l[i] =
+					(val >> PLANE_WM_LINES_SHIFT) &
+						PLANE_WM_LINES_MASK;
+		} else {
+			active->wm[level].cursor_en = is_enabled;
+			active->wm[level].cursor_res_b =
+					val & PLANE_WM_BLOCKS_MASK;
+			active->wm[level].cursor_res_l =
+					(val >> PLANE_WM_LINES_SHIFT) &
+						PLANE_WM_LINES_MASK;
+		}
+	} else {
+		if (!is_cursor) {
+			active->trans_wm.plane_en[i] = is_enabled;
+			active->trans_wm.plane_res_b[i] =
+					val & PLANE_WM_BLOCKS_MASK;
+			active->trans_wm.plane_res_l[i] =
+					(val >> PLANE_WM_LINES_SHIFT) &
+						PLANE_WM_LINES_MASK;
+		} else {
+			active->trans_wm.cursor_en = is_enabled;
+			active->trans_wm.cursor_res_b =
+					val & PLANE_WM_BLOCKS_MASK;
+			active->trans_wm.cursor_res_l =
+					(val >> PLANE_WM_LINES_SHIFT) &
+						PLANE_WM_LINES_MASK;
+		}
+	}
+}
+
+static void skl_pipe_wm_get_hw_state(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct skl_wm_values *hw = &dev_priv->wm.skl_hw;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct skl_pipe_wm *active = &intel_crtc->wm.skl_active;
+	enum pipe pipe = intel_crtc->pipe;
+	int level, i, max_level;
+	uint32_t temp;
+
+	max_level = ilk_wm_max_level(dev);
+
+	hw->wm_linetime[pipe] = I915_READ(PIPE_WM_LINETIME(pipe));
+
+	for (level = 0; level <= max_level; level++) {
+		for (i = 0; i < intel_num_planes(intel_crtc); i++)
+			hw->plane[pipe][i][level] =
+					I915_READ(PLANE_WM(pipe, i, level));
+		hw->cursor[pipe][level] = I915_READ(CUR_WM(pipe, level));
+	}
+
+	for (i = 0; i < intel_num_planes(intel_crtc); i++)
+		hw->plane_trans[pipe][i] = I915_READ(PLANE_WM_TRANS(pipe, i));
+	hw->cursor_trans[pipe] = I915_READ(CUR_WM_TRANS(pipe));
+
+	if (!intel_crtc_active(crtc))
+		return;
+
+	hw->dirty[pipe] = true;
+
+	active->linetime = hw->wm_linetime[pipe];
+
+	for (level = 0; level <= max_level; level++) {
+		for (i = 0; i < intel_num_planes(intel_crtc); i++) {
+			temp = hw->plane[pipe][i][level];
+			skl_pipe_wm_active_state(temp, active, false,
+						false, i, level);
+		}
+		temp = hw->cursor[pipe][level];
+		skl_pipe_wm_active_state(temp, active, false, true, i, level);
+	}
+
+	for (i = 0; i < intel_num_planes(intel_crtc); i++) {
+		temp = hw->plane_trans[pipe][i];
+		skl_pipe_wm_active_state(temp, active, true, false, i, 0);
+	}
+
+	temp = hw->cursor_trans[pipe];
+	skl_pipe_wm_active_state(temp, active, true, true, i, 0);
+}
+
+void skl_wm_get_hw_state(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct skl_ddb_allocation *ddb = &dev_priv->wm.skl_hw.ddb;
+	struct drm_crtc *crtc;
+
+	skl_ddb_get_hw_state(dev_priv, ddb);
+	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head)
+		skl_pipe_wm_get_hw_state(crtc);
+}
+
 static void ilk_pipe_wm_get_hw_state(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
@@ -3442,7 +4451,7 @@ static void vlv_set_rps_idle(struct drm_i915_private *dev_priv)
 					dev_priv->rps.min_freq_softlimit);
 
 	if (wait_for(((vlv_punit_read(dev_priv, PUNIT_REG_GPU_FREQ_STS))
-				& GENFREQSTATUS) == 0, 5))
+				& GENFREQSTATUS) == 0, 100))
 		DRM_ERROR("timed out waiting for Punit\n");
 
 	vlv_force_gfx_clock(dev_priv, false);
@@ -3495,14 +4504,8 @@ void valleyview_set_rps(struct drm_device *dev, u8 val)
 		      "Odd GPU freq value\n"))
 		val &= ~1;
 
-	if (val != dev_priv->rps.cur_freq) {
-		DRM_DEBUG_DRIVER("GPU freq request from %d MHz (%u) to %d MHz (%u)\n",
-				 vlv_gpu_freq(dev_priv, dev_priv->rps.cur_freq),
-				 dev_priv->rps.cur_freq,
-				 vlv_gpu_freq(dev_priv, val), val);
-
+	if (val != dev_priv->rps.cur_freq)
 		vlv_punit_write(dev_priv, PUNIT_REG_GPU_FREQ_REQ, val);
-	}
 
 	I915_WRITE(GEN6_PMINTRMSK, gen6_rps_pm_mask(dev_priv, val));
 
@@ -3510,43 +4513,11 @@ void valleyview_set_rps(struct drm_device *dev, u8 val)
 	trace_intel_gpu_freq_change(vlv_gpu_freq(dev_priv, val));
 }
 
-static void gen8_disable_rps_interrupts(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	I915_WRITE(GEN6_PMINTRMSK, ~GEN8_PMINTR_REDIRECT_TO_NON_DISP);
-	I915_WRITE(GEN8_GT_IER(2), I915_READ(GEN8_GT_IER(2)) &
-				   ~dev_priv->pm_rps_events);
-	/* Complete PM interrupt masking here doesn't race with the rps work
-	 * item again unmasking PM interrupts because that is using a different
-	 * register (GEN8_GT_IMR(2)) to mask PM interrupts. The only risk is in
-	 * leaving stale bits in GEN8_GT_IIR(2) and GEN8_GT_IMR(2) which
-	 * gen8_enable_rps will clean up. */
-
-	spin_lock_irq(&dev_priv->irq_lock);
-	dev_priv->rps.pm_iir = 0;
-	spin_unlock_irq(&dev_priv->irq_lock);
-
-	I915_WRITE(GEN8_GT_IIR(2), dev_priv->pm_rps_events);
-}
-
-static void gen6_disable_rps_interrupts(struct drm_device *dev)
+static void gen9_disable_rps(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	I915_WRITE(GEN6_PMINTRMSK, 0xffffffff);
-	I915_WRITE(GEN6_PMIER, I915_READ(GEN6_PMIER) &
-				~dev_priv->pm_rps_events);
-	/* 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->irq_lock);
-	dev_priv->rps.pm_iir = 0;
-	spin_unlock_irq(&dev_priv->irq_lock);
-
-	I915_WRITE(GEN6_PMIIR, dev_priv->pm_rps_events);
+	I915_WRITE(GEN6_RC_CONTROL, 0);
 }
 
 static void gen6_disable_rps(struct drm_device *dev)
@@ -3555,11 +4526,6 @@ static void gen6_disable_rps(struct drm_device *dev)
 
 	I915_WRITE(GEN6_RC_CONTROL, 0);
 	I915_WRITE(GEN6_RPNSWREQ, 1 << 31);
-
-	if (IS_BROADWELL(dev))
-		gen8_disable_rps_interrupts(dev);
-	else
-		gen6_disable_rps_interrupts(dev);
 }
 
 static void cherryview_disable_rps(struct drm_device *dev)
@@ -3567,8 +4533,6 @@ static void cherryview_disable_rps(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
 	I915_WRITE(GEN6_RC_CONTROL, 0);
-
-	gen8_disable_rps_interrupts(dev);
 }
 
 static void valleyview_disable_rps(struct drm_device *dev)
@@ -3582,8 +4546,6 @@ static void valleyview_disable_rps(struct drm_device *dev)
 	I915_WRITE(GEN6_RC_CONTROL, 0);
 
 	gen6_gt_force_wake_put(dev_priv, FORCEWAKE_ALL);
-
-	gen6_disable_rps_interrupts(dev);
 }
 
 static void intel_print_rc6_info(struct drm_device *dev, u32 mode)
@@ -3594,10 +4556,15 @@ static void intel_print_rc6_info(struct drm_device *dev, u32 mode)
 		else
 			mode = 0;
 	}
-	DRM_DEBUG_KMS("Enabling RC6 states: RC6 %s, RC6p %s, RC6pp %s\n",
-		      (mode & GEN6_RC_CTL_RC6_ENABLE) ? "on" : "off",
-		      (mode & GEN6_RC_CTL_RC6p_ENABLE) ? "on" : "off",
-		      (mode & GEN6_RC_CTL_RC6pp_ENABLE) ? "on" : "off");
+	if (HAS_RC6p(dev))
+		DRM_DEBUG_KMS("Enabling RC6 states: RC6 %s RC6p %s RC6pp %s\n",
+			      (mode & GEN6_RC_CTL_RC6_ENABLE) ? "on" : "off",
+			      (mode & GEN6_RC_CTL_RC6p_ENABLE) ? "on" : "off",
+			      (mode & GEN6_RC_CTL_RC6pp_ENABLE) ? "on" : "off");
+
+	else
+		DRM_DEBUG_KMS("Enabling RC6 states: RC6 %s\n",
+			      (mode & GEN6_RC_CTL_RC6_ENABLE) ? "on" : "off");
 }
 
 static int sanitize_rc6_option(const struct drm_device *dev, int enable_rc6)
@@ -3614,7 +4581,7 @@ static int sanitize_rc6_option(const struct drm_device *dev, int enable_rc6)
 	if (enable_rc6 >= 0) {
 		int mask;
 
-		if (INTEL_INFO(dev)->gen == 6 || IS_IVYBRIDGE(dev))
+		if (HAS_RC6p(dev))
 			mask = INTEL_RC6_ENABLE | INTEL_RC6p_ENABLE |
 			       INTEL_RC6pp_ENABLE;
 		else
@@ -3642,54 +4609,92 @@ int intel_enable_rc6(const struct drm_device *dev)
 	return i915.enable_rc6;
 }
 
-static void gen8_enable_rps_interrupts(struct drm_device *dev)
+static void gen6_init_rps_frequencies(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t rp_state_cap;
+	u32 ddcc_status = 0;
+	int ret;
 
-	spin_lock_irq(&dev_priv->irq_lock);
-	WARN_ON(dev_priv->rps.pm_iir);
-	gen8_enable_pm_irq(dev_priv, dev_priv->pm_rps_events);
-	I915_WRITE(GEN8_GT_IIR(2), dev_priv->pm_rps_events);
-	spin_unlock_irq(&dev_priv->irq_lock);
-}
-
-static void gen6_enable_rps_interrupts(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	spin_lock_irq(&dev_priv->irq_lock);
-	WARN_ON(dev_priv->rps.pm_iir);
-	gen6_enable_pm_irq(dev_priv, dev_priv->pm_rps_events);
-	I915_WRITE(GEN6_PMIIR, dev_priv->pm_rps_events);
-	spin_unlock_irq(&dev_priv->irq_lock);
-}
-
-static void parse_rp_state_cap(struct drm_i915_private *dev_priv, u32 rp_state_cap)
-{
+	rp_state_cap = I915_READ(GEN6_RP_STATE_CAP);
 	/* All of these values are in units of 50MHz */
 	dev_priv->rps.cur_freq		= 0;
-	/* static values from HW: RP0 < RPe < RP1 < RPn (min_freq) */
-	dev_priv->rps.rp1_freq		= (rp_state_cap >>  8) & 0xff;
+	/* static values from HW: RP0 > RP1 > RPn (min_freq) */
 	dev_priv->rps.rp0_freq		= (rp_state_cap >>  0) & 0xff;
+	dev_priv->rps.rp1_freq		= (rp_state_cap >>  8) & 0xff;
 	dev_priv->rps.min_freq		= (rp_state_cap >> 16) & 0xff;
-	/* XXX: only BYT has a special efficient freq */
-	dev_priv->rps.efficient_freq	= dev_priv->rps.rp1_freq;
 	/* hw_max = RP0 until we check for overclocking */
 	dev_priv->rps.max_freq		= dev_priv->rps.rp0_freq;
 
+	dev_priv->rps.efficient_freq = dev_priv->rps.rp1_freq;
+	if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
+		ret = sandybridge_pcode_read(dev_priv,
+					HSW_PCODE_DYNAMIC_DUTY_CYCLE_CONTROL,
+					&ddcc_status);
+		if (0 == ret)
+			dev_priv->rps.efficient_freq =
+				(ddcc_status >> 8) & 0xff;
+	}
+
 	/* Preserve min/max settings in case of re-init */
 	if (dev_priv->rps.max_freq_softlimit == 0)
 		dev_priv->rps.max_freq_softlimit = dev_priv->rps.max_freq;
 
-	if (dev_priv->rps.min_freq_softlimit == 0)
-		dev_priv->rps.min_freq_softlimit = dev_priv->rps.min_freq;
+	if (dev_priv->rps.min_freq_softlimit == 0) {
+		if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+			dev_priv->rps.min_freq_softlimit =
+				/* max(RPe, 450 MHz) */
+				max(dev_priv->rps.efficient_freq, (u8) 9);
+		else
+			dev_priv->rps.min_freq_softlimit =
+				dev_priv->rps.min_freq;
+	}
+}
+
+static void gen9_enable_rps(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_engine_cs *ring;
+	uint32_t rc6_mask = 0;
+	int unused;
+
+	/* 1a: Software RC state - RC0 */
+	I915_WRITE(GEN6_RC_STATE, 0);
+
+	/* 1b: Get forcewake during program sequence. Although the driver
+	 * hasn't enabled a state yet where we need forcewake, BIOS may have.*/
+	gen6_gt_force_wake_get(dev_priv, FORCEWAKE_ALL);
+
+	/* 2a: Disable RC states. */
+	I915_WRITE(GEN6_RC_CONTROL, 0);
+
+	/* 2b: Program RC6 thresholds.*/
+	I915_WRITE(GEN6_RC6_WAKE_RATE_LIMIT, 54 << 16);
+	I915_WRITE(GEN6_RC_EVALUATION_INTERVAL, 125000); /* 12500 * 1280ns */
+	I915_WRITE(GEN6_RC_IDLE_HYSTERSIS, 25); /* 25 * 1280ns */
+	for_each_ring(ring, dev_priv, unused)
+		I915_WRITE(RING_MAX_IDLE(ring->mmio_base), 10);
+	I915_WRITE(GEN6_RC_SLEEP, 0);
+	I915_WRITE(GEN6_RC6_THRESHOLD, 37500); /* 37.5/125ms per EI */
+
+	/* 3a: Enable RC6 */
+	if (intel_enable_rc6(dev) & INTEL_RC6_ENABLE)
+		rc6_mask = GEN6_RC_CTL_RC6_ENABLE;
+	DRM_INFO("RC6 %s\n", (rc6_mask & GEN6_RC_CTL_RC6_ENABLE) ?
+			"on" : "off");
+	I915_WRITE(GEN6_RC_CONTROL, GEN6_RC_CTL_HW_ENABLE |
+				   GEN6_RC_CTL_EI_MODE(1) |
+				   rc6_mask);
+
+	gen6_gt_force_wake_put(dev_priv, FORCEWAKE_ALL);
+
 }
 
 static void gen8_enable_rps(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_engine_cs *ring;
-	uint32_t rc6_mask = 0, rp_state_cap;
+	uint32_t rc6_mask = 0;
 	int unused;
 
 	/* 1a: Software RC state - RC0 */
@@ -3702,8 +4707,8 @@ static void gen8_enable_rps(struct drm_device *dev)
 	/* 2a: Disable RC states. */
 	I915_WRITE(GEN6_RC_CONTROL, 0);
 
-	rp_state_cap = I915_READ(GEN6_RP_STATE_CAP);
-	parse_rp_state_cap(dev_priv, rp_state_cap);
+	/* Initialize rps frequencies */
+	gen6_init_rps_frequencies(dev);
 
 	/* 2b: Program RC6 thresholds.*/
 	I915_WRITE(GEN6_RC6_WAKE_RATE_LIMIT, 40 << 16);
@@ -3761,9 +4766,8 @@ static void gen8_enable_rps(struct drm_device *dev)
 
 	/* 6: Ring frequency + overclocking (our driver does this later */
 
-	gen6_set_rps(dev, (I915_READ(GEN6_GT_PERF_STATUS) & 0xff00) >> 8);
-
-	gen8_enable_rps_interrupts(dev);
+	dev_priv->rps.power = HIGH_POWER; /* force a reset */
+	gen6_set_rps(dev_priv->dev, dev_priv->rps.min_freq_softlimit);
 
 	gen6_gt_force_wake_put(dev_priv, FORCEWAKE_ALL);
 }
@@ -3772,7 +4776,6 @@ static void gen6_enable_rps(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_engine_cs *ring;
-	u32 rp_state_cap;
 	u32 rc6vids, pcu_mbox = 0, rc6_mask = 0;
 	u32 gtfifodbg;
 	int rc6_mode;
@@ -3796,9 +4799,8 @@ static void gen6_enable_rps(struct drm_device *dev)
 
 	gen6_gt_force_wake_get(dev_priv, FORCEWAKE_ALL);
 
-	rp_state_cap = I915_READ(GEN6_RP_STATE_CAP);
-
-	parse_rp_state_cap(dev_priv, rp_state_cap);
+	/* Initialize rps frequencies */
+	gen6_init_rps_frequencies(dev);
 
 	/* disable the counters and set deterministic thresholds */
 	I915_WRITE(GEN6_RC_CONTROL, 0);
@@ -3861,8 +4863,6 @@ static void gen6_enable_rps(struct drm_device *dev)
 	dev_priv->rps.power = HIGH_POWER; /* force a reset */
 	gen6_set_rps(dev_priv->dev, dev_priv->rps.min_freq_softlimit);
 
-	gen6_enable_rps_interrupts(dev);
-
 	rc6vids = 0;
 	ret = sandybridge_pcode_read(dev_priv, GEN6_PCODE_READ_RC6VIDS, &rc6vids);
 	if (IS_GEN6(dev) && ret) {
@@ -3915,9 +4915,9 @@ static void __gen6_update_ring_freq(struct drm_device *dev)
 	 * 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->rps.max_freq_softlimit; gpu_freq >= dev_priv->rps.min_freq_softlimit;
+	for (gpu_freq = dev_priv->rps.max_freq; gpu_freq >= dev_priv->rps.min_freq;
 	     gpu_freq--) {
-		int diff = dev_priv->rps.max_freq_softlimit - gpu_freq;
+		int diff = dev_priv->rps.max_freq - gpu_freq;
 		unsigned int ia_freq = 0, ring_freq = 0;
 
 		if (INTEL_INFO(dev)->gen >= 8) {
@@ -4072,12 +5072,15 @@ static void cherryview_setup_pctx(struct drm_device *dev)
 
 	pcbr = I915_READ(VLV_PCBR);
 	if ((pcbr >> VLV_PCBR_ADDR_SHIFT) == 0) {
+		DRM_DEBUG_DRIVER("BIOS didn't set up PCBR, fixing up\n");
 		paddr = (dev_priv->mm.stolen_base +
 			 (gtt->stolen_size - pctx_size));
 
 		pctx_paddr = (paddr & (~4095));
 		I915_WRITE(VLV_PCBR, pctx_paddr);
 	}
+
+	DRM_DEBUG_DRIVER("PCBR: 0x%08x\n", I915_READ(VLV_PCBR));
 }
 
 static void valleyview_setup_pctx(struct drm_device *dev)
@@ -4103,6 +5106,8 @@ static void valleyview_setup_pctx(struct drm_device *dev)
 		goto out;
 	}
 
+	DRM_DEBUG_DRIVER("BIOS didn't set up PCBR, fixing up\n");
+
 	/*
 	 * From the Gunit register HAS:
 	 * The Gfx driver is expected to program this register and ensure
@@ -4121,6 +5126,7 @@ static void valleyview_setup_pctx(struct drm_device *dev)
 	I915_WRITE(VLV_PCBR, pctx_paddr);
 
 out:
+	DRM_DEBUG_DRIVER("PCBR: 0x%08x\n", I915_READ(VLV_PCBR));
 	dev_priv->vlv_pctx = pctx;
 }
 
@@ -4157,7 +5163,7 @@ static void valleyview_init_gt_powersave(struct drm_device *dev)
 		dev_priv->mem_freq = 1333;
 		break;
 	}
-	DRM_DEBUG_DRIVER("DDR speed: %d MHz", dev_priv->mem_freq);
+	DRM_DEBUG_DRIVER("DDR speed: %d MHz\n", dev_priv->mem_freq);
 
 	dev_priv->rps.max_freq = valleyview_rps_max_freq(dev_priv);
 	dev_priv->rps.rp0_freq = dev_priv->rps.max_freq;
@@ -4199,7 +5205,10 @@ static void cherryview_init_gt_powersave(struct drm_device *dev)
 
 	mutex_lock(&dev_priv->rps.hw_lock);
 
-	val = vlv_punit_read(dev_priv, CCK_FUSE_REG);
+	mutex_lock(&dev_priv->dpio_lock);
+	val = vlv_cck_read(dev_priv, CCK_FUSE_REG);
+	mutex_unlock(&dev_priv->dpio_lock);
+
 	switch ((val >> 2) & 0x7) {
 	case 0:
 	case 1:
@@ -4223,7 +5232,7 @@ static void cherryview_init_gt_powersave(struct drm_device *dev)
 		dev_priv->mem_freq = 1600;
 		break;
 	}
-	DRM_DEBUG_DRIVER("DDR speed: %d MHz", dev_priv->mem_freq);
+	DRM_DEBUG_DRIVER("DDR speed: %d MHz\n", dev_priv->mem_freq);
 
 	dev_priv->rps.max_freq = cherryview_rps_max_freq(dev_priv);
 	dev_priv->rps.rp0_freq = dev_priv->rps.max_freq;
@@ -4309,8 +5318,6 @@ static void cherryview_enable_rps(struct drm_device *dev)
 	/* For now we assume BIOS is allocating and populating the PCBR  */
 	pcbr = I915_READ(VLV_PCBR);
 
-	DRM_DEBUG_DRIVER("PCBR offset : 0x%x\n", pcbr);
-
 	/* 3: Enable RC6 */
 	if ((intel_enable_rc6(dev) & INTEL_RC6_ENABLE) &&
 						(pcbr >> VLV_PCBR_ADDR_SHIFT))
@@ -4340,7 +5347,10 @@ static void cherryview_enable_rps(struct drm_device *dev)
 
 	val = vlv_punit_read(dev_priv, PUNIT_REG_GPU_FREQ_STS);
 
-	DRM_DEBUG_DRIVER("GPLL enabled? %s\n", val & 0x10 ? "yes" : "no");
+	/* RPS code assumes GPLL is used */
+	WARN_ONCE((val & GPLLENABLE) == 0, "GPLL not enabled\n");
+
+	DRM_DEBUG_DRIVER("GPLL enabled? %s\n", val & GPLLENABLE ? "yes" : "no");
 	DRM_DEBUG_DRIVER("GPU status: 0x%08x\n", val);
 
 	dev_priv->rps.cur_freq = (val >> 8) & 0xff;
@@ -4354,8 +5364,6 @@ static void cherryview_enable_rps(struct drm_device *dev)
 
 	valleyview_set_rps(dev_priv->dev, dev_priv->rps.efficient_freq);
 
-	gen8_enable_rps_interrupts(dev);
-
 	gen6_gt_force_wake_put(dev_priv, FORCEWAKE_ALL);
 }
 
@@ -4420,7 +5428,10 @@ static void valleyview_enable_rps(struct drm_device *dev)
 
 	val = vlv_punit_read(dev_priv, PUNIT_REG_GPU_FREQ_STS);
 
-	DRM_DEBUG_DRIVER("GPLL enabled? %s\n", val & 0x10 ? "yes" : "no");
+	/* RPS code assumes GPLL is used */
+	WARN_ONCE((val & GPLLENABLE) == 0, "GPLL not enabled\n");
+
+	DRM_DEBUG_DRIVER("GPLL enabled? %s\n", val & GPLLENABLE ? "yes" : "no");
 	DRM_DEBUG_DRIVER("GPU status: 0x%08x\n", val);
 
 	dev_priv->rps.cur_freq = (val >> 8) & 0xff;
@@ -4434,8 +5445,6 @@ static void valleyview_enable_rps(struct drm_device *dev)
 
 	valleyview_set_rps(dev_priv->dev, dev_priv->rps.efficient_freq);
 
-	gen6_enable_rps_interrupts(dev);
-
 	gen6_gt_force_wake_put(dev_priv, FORCEWAKE_ALL);
 }
 
@@ -5182,6 +6191,20 @@ void intel_cleanup_gt_powersave(struct drm_device *dev)
 		valleyview_cleanup_gt_powersave(dev);
 }
 
+static void gen6_suspend_rps(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	flush_delayed_work(&dev_priv->rps.delayed_resume_work);
+
+	/*
+	 * TODO: disable RPS interrupts on GEN9+ too once RPS support
+	 * is added for it.
+	 */
+	if (INTEL_INFO(dev)->gen < 9)
+		gen6_disable_rps_interrupts(dev);
+}
+
 /**
  * intel_suspend_gt_powersave - suspend PM work and helper threads
  * @dev: drm device
@@ -5194,12 +6217,10 @@ void intel_suspend_gt_powersave(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	/* Interrupts should be disabled already to avoid re-arming. */
-	WARN_ON(intel_irqs_enabled(dev_priv));
-
-	flush_delayed_work(&dev_priv->rps.delayed_resume_work);
+	if (INTEL_INFO(dev)->gen < 6)
+		return;
 
-	cancel_work_sync(&dev_priv->rps.work);
+	gen6_suspend_rps(dev);
 
 	/* Force GPU to min freq during suspend */
 	gen6_rps_idle(dev_priv);
@@ -5209,9 +6230,6 @@ void intel_disable_gt_powersave(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	/* Interrupts should be disabled already to avoid re-arming. */
-	WARN_ON(intel_irqs_enabled(dev_priv));
-
 	if (IS_IRONLAKE_M(dev)) {
 		ironlake_disable_drps(dev);
 		ironlake_disable_rc6(dev);
@@ -5219,12 +6237,15 @@ void intel_disable_gt_powersave(struct drm_device *dev)
 		intel_suspend_gt_powersave(dev);
 
 		mutex_lock(&dev_priv->rps.hw_lock);
-		if (IS_CHERRYVIEW(dev))
+		if (INTEL_INFO(dev)->gen >= 9)
+			gen9_disable_rps(dev);
+		else if (IS_CHERRYVIEW(dev))
 			cherryview_disable_rps(dev);
 		else if (IS_VALLEYVIEW(dev))
 			valleyview_disable_rps(dev);
 		else
 			gen6_disable_rps(dev);
+
 		dev_priv->rps.enabled = false;
 		mutex_unlock(&dev_priv->rps.hw_lock);
 	}
@@ -5239,10 +6260,19 @@ static void intel_gen6_powersave_work(struct work_struct *work)
 
 	mutex_lock(&dev_priv->rps.hw_lock);
 
+	/*
+	 * TODO: reset/enable RPS interrupts on GEN9+ too, once RPS support is
+	 * added for it.
+	 */
+	if (INTEL_INFO(dev)->gen < 9)
+		gen6_reset_rps_interrupts(dev);
+
 	if (IS_CHERRYVIEW(dev)) {
 		cherryview_enable_rps(dev);
 	} else if (IS_VALLEYVIEW(dev)) {
 		valleyview_enable_rps(dev);
+	} else if (INTEL_INFO(dev)->gen >= 9) {
+		gen9_enable_rps(dev);
 	} else if (IS_BROADWELL(dev)) {
 		gen8_enable_rps(dev);
 		__gen6_update_ring_freq(dev);
@@ -5251,6 +6281,10 @@ static void intel_gen6_powersave_work(struct work_struct *work)
 		__gen6_update_ring_freq(dev);
 	}
 	dev_priv->rps.enabled = true;
+
+	if (INTEL_INFO(dev)->gen < 9)
+		gen6_enable_rps_interrupts(dev);
+
 	mutex_unlock(&dev_priv->rps.hw_lock);
 
 	intel_runtime_pm_put(dev_priv);
@@ -5289,8 +6323,11 @@ void intel_reset_gt_powersave(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
+	if (INTEL_INFO(dev)->gen < 6)
+		return;
+
+	gen6_suspend_rps(dev);
 	dev_priv->rps.enabled = false;
-	intel_enable_gt_powersave(dev);
 }
 
 static void ibx_init_clock_gating(struct drm_device *dev)
@@ -5481,7 +6518,7 @@ static void gen6_init_clock_gating(struct drm_device *dev)
 	 * to keep in mind (see 3DSTATE_PS and 3DSTATE_WM).
 	 */
 	I915_WRITE(GEN6_GT_MODE,
-		   GEN6_WIZ_HASHING_MASK | GEN6_WIZ_HASHING_16x4);
+		   _MASKED_FIELD(GEN6_WIZ_HASHING_MASK, GEN6_WIZ_HASHING_16x4));
 
 	ilk_init_lp_watermarks(dev);
 
@@ -5609,16 +6646,6 @@ static void broadwell_init_clock_gating(struct drm_device *dev)
 	I915_WRITE(WM2_LP_ILK, 0);
 	I915_WRITE(WM1_LP_ILK, 0);
 
-	/* FIXME(BDW): Check all the w/a, some might only apply to
-	 * pre-production hw. */
-
-
-	I915_WRITE(GAMTARBMODE, _MASKED_BIT_ENABLE(ARB_MODE_BWGTLB_DISABLE));
-
-	I915_WRITE(_3D_CHICKEN3,
-		   _MASKED_BIT_ENABLE(_3D_CHICKEN_SDE_LIMIT_FIFO_POLY_DEPTH(2)));
-
-
 	/* WaSwitchSolVfFArbitrationPriority:bdw */
 	I915_WRITE(GAM_ECOCHK, I915_READ(GAM_ECOCHK) | HSW_ECOCHK_ARB_PRIO_SOL);
 
@@ -5689,7 +6716,7 @@ static void haswell_init_clock_gating(struct drm_device *dev)
 	 * to keep in mind (see 3DSTATE_PS and 3DSTATE_WM).
 	 */
 	I915_WRITE(GEN7_GT_MODE,
-		   GEN6_WIZ_HASHING_MASK | GEN6_WIZ_HASHING_16x4);
+		   _MASKED_FIELD(GEN6_WIZ_HASHING_MASK, GEN6_WIZ_HASHING_16x4));
 
 	/* WaSwitchSolVfFArbitrationPriority:hsw */
 	I915_WRITE(GAM_ECOCHK, I915_READ(GAM_ECOCHK) | HSW_ECOCHK_ARB_PRIO_SOL);
@@ -5786,7 +6813,7 @@ static void ivybridge_init_clock_gating(struct drm_device *dev)
 	 * to keep in mind (see 3DSTATE_PS and 3DSTATE_WM).
 	 */
 	I915_WRITE(GEN7_GT_MODE,
-		   GEN6_WIZ_HASHING_MASK | GEN6_WIZ_HASHING_16x4);
+		   _MASKED_FIELD(GEN6_WIZ_HASHING_MASK, GEN6_WIZ_HASHING_16x4));
 
 	snpcr = I915_READ(GEN6_MBCUNIT_SNPCR);
 	snpcr &= ~GEN6_MBC_SNPCR_MASK;
@@ -5899,18 +6926,6 @@ static void cherryview_init_clock_gating(struct drm_device *dev)
 	/* WaDisableSDEUnitClockGating:chv */
 	I915_WRITE(GEN8_UCGCTL6, I915_READ(GEN8_UCGCTL6) |
 		   GEN8_SDEUNIT_CLOCK_GATE_DISABLE);
-
-	/* WaDisableGunitClockGating:chv (pre-production hw) */
-	I915_WRITE(VLV_GUNIT_CLOCK_GATE, I915_READ(VLV_GUNIT_CLOCK_GATE) |
-		   GINT_DIS);
-
-	/* WaDisableFfDopClockGating:chv (pre-production hw) */
-	I915_WRITE(GEN6_RC_SLEEP_PSMI_CONTROL,
-		   _MASKED_BIT_ENABLE(GEN8_FF_DOP_CLOCK_GATE_DISABLE));
-
-	/* WaDisableDopClockGating:chv (pre-production hw) */
-	I915_WRITE(GEN6_UCGCTL1, I915_READ(GEN6_UCGCTL1) |
-		   GEN6_EU_TCUNIT_CLOCK_GATE_DISABLE);
 }
 
 static void g4x_init_clock_gating(struct drm_device *dev)
@@ -6036,1161 +7051,35 @@ void intel_suspend_hw(struct drm_device *dev)
 		lpt_suspend_hw(dev);
 }
 
-#define for_each_power_well(i, power_well, domain_mask, power_domains)	\
-	for (i = 0;							\
-	     i < (power_domains)->power_well_count &&			\
-		 ((power_well) = &(power_domains)->power_wells[i]);	\
-	     i++)							\
-		if ((power_well)->domains & (domain_mask))
-
-#define for_each_power_well_rev(i, power_well, domain_mask, power_domains) \
-	for (i = (power_domains)->power_well_count - 1;			 \
-	     i >= 0 && ((power_well) = &(power_domains)->power_wells[i]);\
-	     i--)							 \
-		if ((power_well)->domains & (domain_mask))
-
-/**
- * We should only use the power well if we explicitly asked the hardware to
- * enable it, so check if it's enabled and also check if we've requested it to
- * be enabled.
- */
-static bool hsw_power_well_enabled(struct drm_i915_private *dev_priv,
-				   struct i915_power_well *power_well)
-{
-	return I915_READ(HSW_PWR_WELL_DRIVER) ==
-		     (HSW_PWR_WELL_ENABLE_REQUEST | HSW_PWR_WELL_STATE_ENABLED);
-}
-
-bool intel_display_power_enabled_unlocked(struct drm_i915_private *dev_priv,
-					  enum intel_display_power_domain domain)
-{
-	struct i915_power_domains *power_domains;
-	struct i915_power_well *power_well;
-	bool is_enabled;
-	int i;
-
-	if (dev_priv->pm.suspended)
-		return false;
-
-	power_domains = &dev_priv->power_domains;
-
-	is_enabled = true;
-
-	for_each_power_well_rev(i, power_well, BIT(domain), power_domains) {
-		if (power_well->always_on)
-			continue;
-
-		if (!power_well->hw_enabled) {
-			is_enabled = false;
-			break;
-		}
-	}
-
-	return is_enabled;
-}
-
-bool intel_display_power_enabled(struct drm_i915_private *dev_priv,
-				 enum intel_display_power_domain domain)
+static void intel_init_fbc(struct drm_i915_private *dev_priv)
 {
-	struct i915_power_domains *power_domains;
-	bool ret;
-
-	power_domains = &dev_priv->power_domains;
-
-	mutex_lock(&power_domains->lock);
-	ret = intel_display_power_enabled_unlocked(dev_priv, domain);
-	mutex_unlock(&power_domains->lock);
-
-	return ret;
-}
-
-/*
- * Starting with Haswell, we have a "Power Down Well" that can be turned off
- * when not needed anymore. We have 4 registers that can request the power well
- * to be enabled, and it will only be disabled if none of the registers is
- * requesting it to be enabled.
- */
-static void hsw_power_well_post_enable(struct drm_i915_private *dev_priv)
-{
-	struct drm_device *dev = dev_priv->dev;
-
-	/*
-	 * After we re-enable the power well, if we touch VGA register 0x3d5
-	 * we'll get unclaimed register interrupts. This stops after we write
-	 * anything to the VGA MSR register. The vgacon module uses this
-	 * register all the time, so if we unbind our driver and, as a
-	 * consequence, bind vgacon, we'll get stuck in an infinite loop at
-	 * console_unlock(). So make here we touch the VGA MSR register, making
-	 * sure vgacon can keep working normally without triggering interrupts
-	 * and error messages.
-	 */
-	vga_get_uninterruptible(dev->pdev, VGA_RSRC_LEGACY_IO);
-	outb(inb(VGA_MSR_READ), VGA_MSR_WRITE);
-	vga_put(dev->pdev, VGA_RSRC_LEGACY_IO);
-
-	if (IS_BROADWELL(dev))
-		gen8_irq_power_well_post_enable(dev_priv);
-}
-
-static void hsw_set_power_well(struct drm_i915_private *dev_priv,
-			       struct i915_power_well *power_well, bool enable)
-{
-	bool is_enabled, enable_requested;
-	uint32_t tmp;
-
-	tmp = I915_READ(HSW_PWR_WELL_DRIVER);
-	is_enabled = tmp & HSW_PWR_WELL_STATE_ENABLED;
-	enable_requested = tmp & HSW_PWR_WELL_ENABLE_REQUEST;
-
-	if (enable) {
-		if (!enable_requested)
-			I915_WRITE(HSW_PWR_WELL_DRIVER,
-				   HSW_PWR_WELL_ENABLE_REQUEST);
-
-		if (!is_enabled) {
-			DRM_DEBUG_KMS("Enabling power well\n");
-			if (wait_for((I915_READ(HSW_PWR_WELL_DRIVER) &
-				      HSW_PWR_WELL_STATE_ENABLED), 20))
-				DRM_ERROR("Timeout enabling power well\n");
-		}
-
-		hsw_power_well_post_enable(dev_priv);
-	} else {
-		if (enable_requested) {
-			I915_WRITE(HSW_PWR_WELL_DRIVER, 0);
-			POSTING_READ(HSW_PWR_WELL_DRIVER);
-			DRM_DEBUG_KMS("Requesting to disable the power well\n");
-		}
-	}
-}
-
-static void hsw_power_well_sync_hw(struct drm_i915_private *dev_priv,
-				   struct i915_power_well *power_well)
-{
-	hsw_set_power_well(dev_priv, power_well, power_well->count > 0);
-
-	/*
-	 * We're taking over the BIOS, so clear any requests made by it since
-	 * the driver is in charge now.
-	 */
-	if (I915_READ(HSW_PWR_WELL_BIOS) & HSW_PWR_WELL_ENABLE_REQUEST)
-		I915_WRITE(HSW_PWR_WELL_BIOS, 0);
-}
-
-static void hsw_power_well_enable(struct drm_i915_private *dev_priv,
-				  struct i915_power_well *power_well)
-{
-	hsw_set_power_well(dev_priv, power_well, true);
-}
-
-static void hsw_power_well_disable(struct drm_i915_private *dev_priv,
-				   struct i915_power_well *power_well)
-{
-	hsw_set_power_well(dev_priv, power_well, false);
-}
-
-static void i9xx_always_on_power_well_noop(struct drm_i915_private *dev_priv,
-					   struct i915_power_well *power_well)
-{
-}
-
-static bool i9xx_always_on_power_well_enabled(struct drm_i915_private *dev_priv,
-					     struct i915_power_well *power_well)
-{
-	return true;
-}
-
-static void vlv_set_power_well(struct drm_i915_private *dev_priv,
-			       struct i915_power_well *power_well, bool enable)
-{
-	enum punit_power_well power_well_id = power_well->data;
-	u32 mask;
-	u32 state;
-	u32 ctrl;
-
-	mask = PUNIT_PWRGT_MASK(power_well_id);
-	state = enable ? PUNIT_PWRGT_PWR_ON(power_well_id) :
-			 PUNIT_PWRGT_PWR_GATE(power_well_id);
-
-	mutex_lock(&dev_priv->rps.hw_lock);
-
-#define COND \
-	((vlv_punit_read(dev_priv, PUNIT_REG_PWRGT_STATUS) & mask) == state)
-
-	if (COND)
-		goto out;
-
-	ctrl = vlv_punit_read(dev_priv, PUNIT_REG_PWRGT_CTRL);
-	ctrl &= ~mask;
-	ctrl |= state;
-	vlv_punit_write(dev_priv, PUNIT_REG_PWRGT_CTRL, ctrl);
-
-	if (wait_for(COND, 100))
-		DRM_ERROR("timout setting power well state %08x (%08x)\n",
-			  state,
-			  vlv_punit_read(dev_priv, PUNIT_REG_PWRGT_CTRL));
-
-#undef COND
-
-out:
-	mutex_unlock(&dev_priv->rps.hw_lock);
-}
-
-static void vlv_power_well_sync_hw(struct drm_i915_private *dev_priv,
-				   struct i915_power_well *power_well)
-{
-	vlv_set_power_well(dev_priv, power_well, power_well->count > 0);
-}
-
-static void vlv_power_well_enable(struct drm_i915_private *dev_priv,
-				  struct i915_power_well *power_well)
-{
-	vlv_set_power_well(dev_priv, power_well, true);
-}
-
-static void vlv_power_well_disable(struct drm_i915_private *dev_priv,
-				   struct i915_power_well *power_well)
-{
-	vlv_set_power_well(dev_priv, power_well, false);
-}
-
-static bool vlv_power_well_enabled(struct drm_i915_private *dev_priv,
-				   struct i915_power_well *power_well)
-{
-	int power_well_id = power_well->data;
-	bool enabled = false;
-	u32 mask;
-	u32 state;
-	u32 ctrl;
-
-	mask = PUNIT_PWRGT_MASK(power_well_id);
-	ctrl = PUNIT_PWRGT_PWR_ON(power_well_id);
-
-	mutex_lock(&dev_priv->rps.hw_lock);
-
-	state = vlv_punit_read(dev_priv, PUNIT_REG_PWRGT_STATUS) & mask;
-	/*
-	 * We only ever set the power-on and power-gate states, anything
-	 * else is unexpected.
-	 */
-	WARN_ON(state != PUNIT_PWRGT_PWR_ON(power_well_id) &&
-		state != PUNIT_PWRGT_PWR_GATE(power_well_id));
-	if (state == ctrl)
-		enabled = true;
-
-	/*
-	 * A transient state at this point would mean some unexpected party
-	 * is poking at the power controls too.
-	 */
-	ctrl = vlv_punit_read(dev_priv, PUNIT_REG_PWRGT_CTRL) & mask;
-	WARN_ON(ctrl != state);
-
-	mutex_unlock(&dev_priv->rps.hw_lock);
-
-	return enabled;
-}
-
-static void vlv_display_power_well_enable(struct drm_i915_private *dev_priv,
-					  struct i915_power_well *power_well)
-{
-	WARN_ON_ONCE(power_well->data != PUNIT_POWER_WELL_DISP2D);
-
-	vlv_set_power_well(dev_priv, power_well, true);
-
-	spin_lock_irq(&dev_priv->irq_lock);
-	valleyview_enable_display_irqs(dev_priv);
-	spin_unlock_irq(&dev_priv->irq_lock);
-
-	/*
-	 * During driver initialization/resume we can avoid restoring the
-	 * part of the HW/SW state that will be inited anyway explicitly.
-	 */
-	if (dev_priv->power_domains.initializing)
+	if (!HAS_FBC(dev_priv)) {
+		dev_priv->fbc.enabled = false;
 		return;
-
-	intel_hpd_init(dev_priv->dev);
-
-	i915_redisable_vga_power_on(dev_priv->dev);
-}
-
-static void vlv_display_power_well_disable(struct drm_i915_private *dev_priv,
-					   struct i915_power_well *power_well)
-{
-	WARN_ON_ONCE(power_well->data != PUNIT_POWER_WELL_DISP2D);
-
-	spin_lock_irq(&dev_priv->irq_lock);
-	valleyview_disable_display_irqs(dev_priv);
-	spin_unlock_irq(&dev_priv->irq_lock);
-
-	vlv_set_power_well(dev_priv, power_well, false);
-
-	vlv_power_sequencer_reset(dev_priv);
-}
-
-static void vlv_dpio_cmn_power_well_enable(struct drm_i915_private *dev_priv,
-					   struct i915_power_well *power_well)
-{
-	WARN_ON_ONCE(power_well->data != PUNIT_POWER_WELL_DPIO_CMN_BC);
-
-	/*
-	 * Enable the CRI clock source so we can get at the
-	 * display and the reference clock for VGA
-	 * hotplug / manual detection.
-	 */
-	I915_WRITE(DPLL(PIPE_B), I915_READ(DPLL(PIPE_B)) |
-		   DPLL_REFA_CLK_ENABLE_VLV | DPLL_INTEGRATED_CRI_CLK_VLV);
-	udelay(1); /* >10ns for cmnreset, >0ns for sidereset */
-
-	vlv_set_power_well(dev_priv, power_well, true);
-
-	/*
-	 * From VLV2A0_DP_eDP_DPIO_driver_vbios_notes_10.docx -
-	 *  6.	De-assert cmn_reset/side_reset. Same as VLV X0.
-	 *   a.	GUnit 0x2110 bit[0] set to 1 (def 0)
-	 *   b.	The other bits such as sfr settings / modesel may all
-	 *	be set to 0.
-	 *
-	 * This should only be done on init and resume from S3 with
-	 * both PLLs disabled, or we risk losing DPIO and PLL
-	 * synchronization.
-	 */
-	I915_WRITE(DPIO_CTL, I915_READ(DPIO_CTL) | DPIO_CMNRST);
-}
-
-static void vlv_dpio_cmn_power_well_disable(struct drm_i915_private *dev_priv,
-					    struct i915_power_well *power_well)
-{
-	enum pipe pipe;
-
-	WARN_ON_ONCE(power_well->data != PUNIT_POWER_WELL_DPIO_CMN_BC);
-
-	for_each_pipe(dev_priv, pipe)
-		assert_pll_disabled(dev_priv, pipe);
-
-	/* Assert common reset */
-	I915_WRITE(DPIO_CTL, I915_READ(DPIO_CTL) & ~DPIO_CMNRST);
-
-	vlv_set_power_well(dev_priv, power_well, false);
-}
-
-static void chv_dpio_cmn_power_well_enable(struct drm_i915_private *dev_priv,
-					   struct i915_power_well *power_well)
-{
-	enum dpio_phy phy;
-
-	WARN_ON_ONCE(power_well->data != PUNIT_POWER_WELL_DPIO_CMN_BC &&
-		     power_well->data != PUNIT_POWER_WELL_DPIO_CMN_D);
-
-	/*
-	 * Enable the CRI clock source so we can get at the
-	 * display and the reference clock for VGA
-	 * hotplug / manual detection.
-	 */
-	if (power_well->data == PUNIT_POWER_WELL_DPIO_CMN_BC) {
-		phy = DPIO_PHY0;
-		I915_WRITE(DPLL(PIPE_B), I915_READ(DPLL(PIPE_B)) |
-			   DPLL_REFA_CLK_ENABLE_VLV);
-		I915_WRITE(DPLL(PIPE_B), I915_READ(DPLL(PIPE_B)) |
-			   DPLL_REFA_CLK_ENABLE_VLV | DPLL_INTEGRATED_CRI_CLK_VLV);
-	} else {
-		phy = DPIO_PHY1;
-		I915_WRITE(DPLL(PIPE_C), I915_READ(DPLL(PIPE_C)) |
-			   DPLL_REFA_CLK_ENABLE_VLV | DPLL_INTEGRATED_CRI_CLK_VLV);
 	}
-	udelay(1); /* >10ns for cmnreset, >0ns for sidereset */
-	vlv_set_power_well(dev_priv, power_well, true);
 
-	/* Poll for phypwrgood signal */
-	if (wait_for(I915_READ(DISPLAY_PHY_STATUS) & PHY_POWERGOOD(phy), 1))
-		DRM_ERROR("Display PHY %d is not power up\n", phy);
-
-	I915_WRITE(DISPLAY_PHY_CONTROL, I915_READ(DISPLAY_PHY_CONTROL) |
-		   PHY_COM_LANE_RESET_DEASSERT(phy));
-}
-
-static void chv_dpio_cmn_power_well_disable(struct drm_i915_private *dev_priv,
-					    struct i915_power_well *power_well)
-{
-	enum dpio_phy phy;
-
-	WARN_ON_ONCE(power_well->data != PUNIT_POWER_WELL_DPIO_CMN_BC &&
-		     power_well->data != PUNIT_POWER_WELL_DPIO_CMN_D);
-
-	if (power_well->data == PUNIT_POWER_WELL_DPIO_CMN_BC) {
-		phy = DPIO_PHY0;
-		assert_pll_disabled(dev_priv, PIPE_A);
-		assert_pll_disabled(dev_priv, PIPE_B);
+	if (INTEL_INFO(dev_priv)->gen >= 7) {
+		dev_priv->display.fbc_enabled = ironlake_fbc_enabled;
+		dev_priv->display.enable_fbc = gen7_enable_fbc;
+		dev_priv->display.disable_fbc = ironlake_disable_fbc;
+	} else if (INTEL_INFO(dev_priv)->gen >= 5) {
+		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_priv)) {
+		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 {
-		phy = DPIO_PHY1;
-		assert_pll_disabled(dev_priv, PIPE_C);
-	}
-
-	I915_WRITE(DISPLAY_PHY_CONTROL, I915_READ(DISPLAY_PHY_CONTROL) &
-		   ~PHY_COM_LANE_RESET_DEASSERT(phy));
-
-	vlv_set_power_well(dev_priv, power_well, false);
-}
-
-static bool chv_pipe_power_well_enabled(struct drm_i915_private *dev_priv,
-					struct i915_power_well *power_well)
-{
-	enum pipe pipe = power_well->data;
-	bool enabled;
-	u32 state, ctrl;
-
-	mutex_lock(&dev_priv->rps.hw_lock);
-
-	state = vlv_punit_read(dev_priv, PUNIT_REG_DSPFREQ) & DP_SSS_MASK(pipe);
-	/*
-	 * We only ever set the power-on and power-gate states, anything
-	 * else is unexpected.
-	 */
-	WARN_ON(state != DP_SSS_PWR_ON(pipe) && state != DP_SSS_PWR_GATE(pipe));
-	enabled = state == DP_SSS_PWR_ON(pipe);
-
-	/*
-	 * A transient state at this point would mean some unexpected party
-	 * is poking at the power controls too.
-	 */
-	ctrl = vlv_punit_read(dev_priv, PUNIT_REG_DSPFREQ) & DP_SSC_MASK(pipe);
-	WARN_ON(ctrl << 16 != state);
-
-	mutex_unlock(&dev_priv->rps.hw_lock);
-
-	return enabled;
-}
-
-static void chv_set_pipe_power_well(struct drm_i915_private *dev_priv,
-				    struct i915_power_well *power_well,
-				    bool enable)
-{
-	enum pipe pipe = power_well->data;
-	u32 state;
-	u32 ctrl;
-
-	state = enable ? DP_SSS_PWR_ON(pipe) : DP_SSS_PWR_GATE(pipe);
-
-	mutex_lock(&dev_priv->rps.hw_lock);
-
-#define COND \
-	((vlv_punit_read(dev_priv, PUNIT_REG_DSPFREQ) & DP_SSS_MASK(pipe)) == state)
-
-	if (COND)
-		goto out;
-
-	ctrl = vlv_punit_read(dev_priv, PUNIT_REG_DSPFREQ);
-	ctrl &= ~DP_SSC_MASK(pipe);
-	ctrl |= enable ? DP_SSC_PWR_ON(pipe) : DP_SSC_PWR_GATE(pipe);
-	vlv_punit_write(dev_priv, PUNIT_REG_DSPFREQ, ctrl);
-
-	if (wait_for(COND, 100))
-		DRM_ERROR("timout setting power well state %08x (%08x)\n",
-			  state,
-			  vlv_punit_read(dev_priv, PUNIT_REG_DSPFREQ));
+		dev_priv->display.fbc_enabled = i8xx_fbc_enabled;
+		dev_priv->display.enable_fbc = i8xx_enable_fbc;
+		dev_priv->display.disable_fbc = i8xx_disable_fbc;
 
-#undef COND
-
-out:
-	mutex_unlock(&dev_priv->rps.hw_lock);
-}
-
-static void chv_pipe_power_well_sync_hw(struct drm_i915_private *dev_priv,
-					struct i915_power_well *power_well)
-{
-	chv_set_pipe_power_well(dev_priv, power_well, power_well->count > 0);
-}
-
-static void chv_pipe_power_well_enable(struct drm_i915_private *dev_priv,
-				       struct i915_power_well *power_well)
-{
-	WARN_ON_ONCE(power_well->data != PIPE_A &&
-		     power_well->data != PIPE_B &&
-		     power_well->data != PIPE_C);
-
-	chv_set_pipe_power_well(dev_priv, power_well, true);
-}
-
-static void chv_pipe_power_well_disable(struct drm_i915_private *dev_priv,
-					struct i915_power_well *power_well)
-{
-	WARN_ON_ONCE(power_well->data != PIPE_A &&
-		     power_well->data != PIPE_B &&
-		     power_well->data != PIPE_C);
-
-	chv_set_pipe_power_well(dev_priv, power_well, false);
-}
-
-static void check_power_well_state(struct drm_i915_private *dev_priv,
-				   struct i915_power_well *power_well)
-{
-	bool enabled = power_well->ops->is_enabled(dev_priv, power_well);
-
-	if (power_well->always_on || !i915.disable_power_well) {
-		if (!enabled)
-			goto mismatch;
-
-		return;
-	}
-
-	if (enabled != (power_well->count > 0))
-		goto mismatch;
-
-	return;
-
-mismatch:
-	WARN(1, "state mismatch for '%s' (always_on %d hw state %d use-count %d disable_power_well %d\n",
-		  power_well->name, power_well->always_on, enabled,
-		  power_well->count, i915.disable_power_well);
-}
-
-void intel_display_power_get(struct drm_i915_private *dev_priv,
-			     enum intel_display_power_domain domain)
-{
-	struct i915_power_domains *power_domains;
-	struct i915_power_well *power_well;
-	int i;
-
-	intel_runtime_pm_get(dev_priv);
-
-	power_domains = &dev_priv->power_domains;
-
-	mutex_lock(&power_domains->lock);
-
-	for_each_power_well(i, power_well, BIT(domain), power_domains) {
-		if (!power_well->count++) {
-			DRM_DEBUG_KMS("enabling %s\n", power_well->name);
-			power_well->ops->enable(dev_priv, power_well);
-			power_well->hw_enabled = true;
-		}
-
-		check_power_well_state(dev_priv, power_well);
-	}
-
-	power_domains->domain_use_count[domain]++;
-
-	mutex_unlock(&power_domains->lock);
-}
-
-void intel_display_power_put(struct drm_i915_private *dev_priv,
-			     enum intel_display_power_domain domain)
-{
-	struct i915_power_domains *power_domains;
-	struct i915_power_well *power_well;
-	int i;
-
-	power_domains = &dev_priv->power_domains;
-
-	mutex_lock(&power_domains->lock);
-
-	WARN_ON(!power_domains->domain_use_count[domain]);
-	power_domains->domain_use_count[domain]--;
-
-	for_each_power_well_rev(i, power_well, BIT(domain), power_domains) {
-		WARN_ON(!power_well->count);
-
-		if (!--power_well->count && i915.disable_power_well) {
-			DRM_DEBUG_KMS("disabling %s\n", power_well->name);
-			power_well->hw_enabled = false;
-			power_well->ops->disable(dev_priv, power_well);
-		}
-
-		check_power_well_state(dev_priv, power_well);
-	}
-
-	mutex_unlock(&power_domains->lock);
-
-	intel_runtime_pm_put(dev_priv);
-}
-
-static struct i915_power_domains *hsw_pwr;
-
-/* Display audio driver power well request */
-int i915_request_power_well(void)
-{
-	struct drm_i915_private *dev_priv;
-
-	if (!hsw_pwr)
-		return -ENODEV;
-
-	dev_priv = container_of(hsw_pwr, struct drm_i915_private,
-				power_domains);
-	intel_display_power_get(dev_priv, POWER_DOMAIN_AUDIO);
-	return 0;
-}
-EXPORT_SYMBOL_GPL(i915_request_power_well);
-
-/* Display audio driver power well release */
-int i915_release_power_well(void)
-{
-	struct drm_i915_private *dev_priv;
-
-	if (!hsw_pwr)
-		return -ENODEV;
-
-	dev_priv = container_of(hsw_pwr, struct drm_i915_private,
-				power_domains);
-	intel_display_power_put(dev_priv, POWER_DOMAIN_AUDIO);
-	return 0;
-}
-EXPORT_SYMBOL_GPL(i915_release_power_well);
-
-/*
- * Private interface for the audio driver to get CDCLK in kHz.
- *
- * Caller must request power well using i915_request_power_well() prior to
- * making the call.
- */
-int i915_get_cdclk_freq(void)
-{
-	struct drm_i915_private *dev_priv;
-
-	if (!hsw_pwr)
-		return -ENODEV;
-
-	dev_priv = container_of(hsw_pwr, struct drm_i915_private,
-				power_domains);
-
-	return intel_ddi_get_cdclk_freq(dev_priv);
-}
-EXPORT_SYMBOL_GPL(i915_get_cdclk_freq);
-
-
-#define POWER_DOMAIN_MASK (BIT(POWER_DOMAIN_NUM) - 1)
-
-#define HSW_ALWAYS_ON_POWER_DOMAINS (			\
-	BIT(POWER_DOMAIN_PIPE_A) |			\
-	BIT(POWER_DOMAIN_TRANSCODER_EDP) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_A_2_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_A_4_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_B_2_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_B_4_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_C_2_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_D_2_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_D_4_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_CRT) |			\
-	BIT(POWER_DOMAIN_PLLS) |			\
-	BIT(POWER_DOMAIN_INIT))
-#define HSW_DISPLAY_POWER_DOMAINS (				\
-	(POWER_DOMAIN_MASK & ~HSW_ALWAYS_ON_POWER_DOMAINS) |	\
-	BIT(POWER_DOMAIN_INIT))
-
-#define BDW_ALWAYS_ON_POWER_DOMAINS (			\
-	HSW_ALWAYS_ON_POWER_DOMAINS |			\
-	BIT(POWER_DOMAIN_PIPE_A_PANEL_FITTER))
-#define BDW_DISPLAY_POWER_DOMAINS (				\
-	(POWER_DOMAIN_MASK & ~BDW_ALWAYS_ON_POWER_DOMAINS) |	\
-	BIT(POWER_DOMAIN_INIT))
-
-#define VLV_ALWAYS_ON_POWER_DOMAINS	BIT(POWER_DOMAIN_INIT)
-#define VLV_DISPLAY_POWER_DOMAINS	POWER_DOMAIN_MASK
-
-#define VLV_DPIO_CMN_BC_POWER_DOMAINS (		\
-	BIT(POWER_DOMAIN_PORT_DDI_B_2_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_DDI_B_4_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_DDI_C_2_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_CRT) |		\
-	BIT(POWER_DOMAIN_INIT))
-
-#define VLV_DPIO_TX_B_LANES_01_POWER_DOMAINS (	\
-	BIT(POWER_DOMAIN_PORT_DDI_B_2_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_DDI_B_4_LANES) |	\
-	BIT(POWER_DOMAIN_INIT))
-
-#define VLV_DPIO_TX_B_LANES_23_POWER_DOMAINS (	\
-	BIT(POWER_DOMAIN_PORT_DDI_B_4_LANES) |	\
-	BIT(POWER_DOMAIN_INIT))
-
-#define VLV_DPIO_TX_C_LANES_01_POWER_DOMAINS (	\
-	BIT(POWER_DOMAIN_PORT_DDI_C_2_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |	\
-	BIT(POWER_DOMAIN_INIT))
-
-#define VLV_DPIO_TX_C_LANES_23_POWER_DOMAINS (	\
-	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |	\
-	BIT(POWER_DOMAIN_INIT))
-
-#define CHV_PIPE_A_POWER_DOMAINS (	\
-	BIT(POWER_DOMAIN_PIPE_A) |	\
-	BIT(POWER_DOMAIN_INIT))
-
-#define CHV_PIPE_B_POWER_DOMAINS (	\
-	BIT(POWER_DOMAIN_PIPE_B) |	\
-	BIT(POWER_DOMAIN_INIT))
-
-#define CHV_PIPE_C_POWER_DOMAINS (	\
-	BIT(POWER_DOMAIN_PIPE_C) |	\
-	BIT(POWER_DOMAIN_INIT))
-
-#define CHV_DPIO_CMN_BC_POWER_DOMAINS (		\
-	BIT(POWER_DOMAIN_PORT_DDI_B_2_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_DDI_B_4_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_DDI_C_2_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |	\
-	BIT(POWER_DOMAIN_INIT))
-
-#define CHV_DPIO_CMN_D_POWER_DOMAINS (		\
-	BIT(POWER_DOMAIN_PORT_DDI_D_2_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_DDI_D_4_LANES) |	\
-	BIT(POWER_DOMAIN_INIT))
-
-#define CHV_DPIO_TX_D_LANES_01_POWER_DOMAINS (	\
-	BIT(POWER_DOMAIN_PORT_DDI_D_2_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_DDI_D_4_LANES) |	\
-	BIT(POWER_DOMAIN_INIT))
-
-#define CHV_DPIO_TX_D_LANES_23_POWER_DOMAINS (	\
-	BIT(POWER_DOMAIN_PORT_DDI_D_4_LANES) |	\
-	BIT(POWER_DOMAIN_INIT))
-
-static const struct i915_power_well_ops i9xx_always_on_power_well_ops = {
-	.sync_hw = i9xx_always_on_power_well_noop,
-	.enable = i9xx_always_on_power_well_noop,
-	.disable = i9xx_always_on_power_well_noop,
-	.is_enabled = i9xx_always_on_power_well_enabled,
-};
-
-static const struct i915_power_well_ops chv_pipe_power_well_ops = {
-	.sync_hw = chv_pipe_power_well_sync_hw,
-	.enable = chv_pipe_power_well_enable,
-	.disable = chv_pipe_power_well_disable,
-	.is_enabled = chv_pipe_power_well_enabled,
-};
-
-static const struct i915_power_well_ops chv_dpio_cmn_power_well_ops = {
-	.sync_hw = vlv_power_well_sync_hw,
-	.enable = chv_dpio_cmn_power_well_enable,
-	.disable = chv_dpio_cmn_power_well_disable,
-	.is_enabled = vlv_power_well_enabled,
-};
-
-static struct i915_power_well i9xx_always_on_power_well[] = {
-	{
-		.name = "always-on",
-		.always_on = 1,
-		.domains = POWER_DOMAIN_MASK,
-		.ops = &i9xx_always_on_power_well_ops,
-	},
-};
-
-static const struct i915_power_well_ops hsw_power_well_ops = {
-	.sync_hw = hsw_power_well_sync_hw,
-	.enable = hsw_power_well_enable,
-	.disable = hsw_power_well_disable,
-	.is_enabled = hsw_power_well_enabled,
-};
-
-static struct i915_power_well hsw_power_wells[] = {
-	{
-		.name = "always-on",
-		.always_on = 1,
-		.domains = HSW_ALWAYS_ON_POWER_DOMAINS,
-		.ops = &i9xx_always_on_power_well_ops,
-	},
-	{
-		.name = "display",
-		.domains = HSW_DISPLAY_POWER_DOMAINS,
-		.ops = &hsw_power_well_ops,
-	},
-};
-
-static struct i915_power_well bdw_power_wells[] = {
-	{
-		.name = "always-on",
-		.always_on = 1,
-		.domains = BDW_ALWAYS_ON_POWER_DOMAINS,
-		.ops = &i9xx_always_on_power_well_ops,
-	},
-	{
-		.name = "display",
-		.domains = BDW_DISPLAY_POWER_DOMAINS,
-		.ops = &hsw_power_well_ops,
-	},
-};
-
-static const struct i915_power_well_ops vlv_display_power_well_ops = {
-	.sync_hw = vlv_power_well_sync_hw,
-	.enable = vlv_display_power_well_enable,
-	.disable = vlv_display_power_well_disable,
-	.is_enabled = vlv_power_well_enabled,
-};
-
-static const struct i915_power_well_ops vlv_dpio_cmn_power_well_ops = {
-	.sync_hw = vlv_power_well_sync_hw,
-	.enable = vlv_dpio_cmn_power_well_enable,
-	.disable = vlv_dpio_cmn_power_well_disable,
-	.is_enabled = vlv_power_well_enabled,
-};
-
-static const struct i915_power_well_ops vlv_dpio_power_well_ops = {
-	.sync_hw = vlv_power_well_sync_hw,
-	.enable = vlv_power_well_enable,
-	.disable = vlv_power_well_disable,
-	.is_enabled = vlv_power_well_enabled,
-};
-
-static struct i915_power_well vlv_power_wells[] = {
-	{
-		.name = "always-on",
-		.always_on = 1,
-		.domains = VLV_ALWAYS_ON_POWER_DOMAINS,
-		.ops = &i9xx_always_on_power_well_ops,
-	},
-	{
-		.name = "display",
-		.domains = VLV_DISPLAY_POWER_DOMAINS,
-		.data = PUNIT_POWER_WELL_DISP2D,
-		.ops = &vlv_display_power_well_ops,
-	},
-	{
-		.name = "dpio-tx-b-01",
-		.domains = VLV_DPIO_TX_B_LANES_01_POWER_DOMAINS |
-			   VLV_DPIO_TX_B_LANES_23_POWER_DOMAINS |
-			   VLV_DPIO_TX_C_LANES_01_POWER_DOMAINS |
-			   VLV_DPIO_TX_C_LANES_23_POWER_DOMAINS,
-		.ops = &vlv_dpio_power_well_ops,
-		.data = PUNIT_POWER_WELL_DPIO_TX_B_LANES_01,
-	},
-	{
-		.name = "dpio-tx-b-23",
-		.domains = VLV_DPIO_TX_B_LANES_01_POWER_DOMAINS |
-			   VLV_DPIO_TX_B_LANES_23_POWER_DOMAINS |
-			   VLV_DPIO_TX_C_LANES_01_POWER_DOMAINS |
-			   VLV_DPIO_TX_C_LANES_23_POWER_DOMAINS,
-		.ops = &vlv_dpio_power_well_ops,
-		.data = PUNIT_POWER_WELL_DPIO_TX_B_LANES_23,
-	},
-	{
-		.name = "dpio-tx-c-01",
-		.domains = VLV_DPIO_TX_B_LANES_01_POWER_DOMAINS |
-			   VLV_DPIO_TX_B_LANES_23_POWER_DOMAINS |
-			   VLV_DPIO_TX_C_LANES_01_POWER_DOMAINS |
-			   VLV_DPIO_TX_C_LANES_23_POWER_DOMAINS,
-		.ops = &vlv_dpio_power_well_ops,
-		.data = PUNIT_POWER_WELL_DPIO_TX_C_LANES_01,
-	},
-	{
-		.name = "dpio-tx-c-23",
-		.domains = VLV_DPIO_TX_B_LANES_01_POWER_DOMAINS |
-			   VLV_DPIO_TX_B_LANES_23_POWER_DOMAINS |
-			   VLV_DPIO_TX_C_LANES_01_POWER_DOMAINS |
-			   VLV_DPIO_TX_C_LANES_23_POWER_DOMAINS,
-		.ops = &vlv_dpio_power_well_ops,
-		.data = PUNIT_POWER_WELL_DPIO_TX_C_LANES_23,
-	},
-	{
-		.name = "dpio-common",
-		.domains = VLV_DPIO_CMN_BC_POWER_DOMAINS,
-		.data = PUNIT_POWER_WELL_DPIO_CMN_BC,
-		.ops = &vlv_dpio_cmn_power_well_ops,
-	},
-};
-
-static struct i915_power_well chv_power_wells[] = {
-	{
-		.name = "always-on",
-		.always_on = 1,
-		.domains = VLV_ALWAYS_ON_POWER_DOMAINS,
-		.ops = &i9xx_always_on_power_well_ops,
-	},
-#if 0
-	{
-		.name = "display",
-		.domains = VLV_DISPLAY_POWER_DOMAINS,
-		.data = PUNIT_POWER_WELL_DISP2D,
-		.ops = &vlv_display_power_well_ops,
-	},
-	{
-		.name = "pipe-a",
-		.domains = CHV_PIPE_A_POWER_DOMAINS,
-		.data = PIPE_A,
-		.ops = &chv_pipe_power_well_ops,
-	},
-	{
-		.name = "pipe-b",
-		.domains = CHV_PIPE_B_POWER_DOMAINS,
-		.data = PIPE_B,
-		.ops = &chv_pipe_power_well_ops,
-	},
-	{
-		.name = "pipe-c",
-		.domains = CHV_PIPE_C_POWER_DOMAINS,
-		.data = PIPE_C,
-		.ops = &chv_pipe_power_well_ops,
-	},
-#endif
-	{
-		.name = "dpio-common-bc",
-		/*
-		 * XXX: cmnreset for one PHY seems to disturb the other.
-		 * As a workaround keep both powered on at the same
-		 * time for now.
-		 */
-		.domains = CHV_DPIO_CMN_BC_POWER_DOMAINS | CHV_DPIO_CMN_D_POWER_DOMAINS,
-		.data = PUNIT_POWER_WELL_DPIO_CMN_BC,
-		.ops = &chv_dpio_cmn_power_well_ops,
-	},
-	{
-		.name = "dpio-common-d",
-		/*
-		 * XXX: cmnreset for one PHY seems to disturb the other.
-		 * As a workaround keep both powered on at the same
-		 * time for now.
-		 */
-		.domains = CHV_DPIO_CMN_BC_POWER_DOMAINS | CHV_DPIO_CMN_D_POWER_DOMAINS,
-		.data = PUNIT_POWER_WELL_DPIO_CMN_D,
-		.ops = &chv_dpio_cmn_power_well_ops,
-	},
-#if 0
-	{
-		.name = "dpio-tx-b-01",
-		.domains = VLV_DPIO_TX_B_LANES_01_POWER_DOMAINS |
-			   VLV_DPIO_TX_B_LANES_23_POWER_DOMAINS,
-		.ops = &vlv_dpio_power_well_ops,
-		.data = PUNIT_POWER_WELL_DPIO_TX_B_LANES_01,
-	},
-	{
-		.name = "dpio-tx-b-23",
-		.domains = VLV_DPIO_TX_B_LANES_01_POWER_DOMAINS |
-			   VLV_DPIO_TX_B_LANES_23_POWER_DOMAINS,
-		.ops = &vlv_dpio_power_well_ops,
-		.data = PUNIT_POWER_WELL_DPIO_TX_B_LANES_23,
-	},
-	{
-		.name = "dpio-tx-c-01",
-		.domains = VLV_DPIO_TX_C_LANES_01_POWER_DOMAINS |
-			   VLV_DPIO_TX_C_LANES_23_POWER_DOMAINS,
-		.ops = &vlv_dpio_power_well_ops,
-		.data = PUNIT_POWER_WELL_DPIO_TX_C_LANES_01,
-	},
-	{
-		.name = "dpio-tx-c-23",
-		.domains = VLV_DPIO_TX_C_LANES_01_POWER_DOMAINS |
-			   VLV_DPIO_TX_C_LANES_23_POWER_DOMAINS,
-		.ops = &vlv_dpio_power_well_ops,
-		.data = PUNIT_POWER_WELL_DPIO_TX_C_LANES_23,
-	},
-	{
-		.name = "dpio-tx-d-01",
-		.domains = CHV_DPIO_TX_D_LANES_01_POWER_DOMAINS |
-			   CHV_DPIO_TX_D_LANES_23_POWER_DOMAINS,
-		.ops = &vlv_dpio_power_well_ops,
-		.data = PUNIT_POWER_WELL_DPIO_TX_D_LANES_01,
-	},
-	{
-		.name = "dpio-tx-d-23",
-		.domains = CHV_DPIO_TX_D_LANES_01_POWER_DOMAINS |
-			   CHV_DPIO_TX_D_LANES_23_POWER_DOMAINS,
-		.ops = &vlv_dpio_power_well_ops,
-		.data = PUNIT_POWER_WELL_DPIO_TX_D_LANES_23,
-	},
-#endif
-};
-
-static struct i915_power_well *lookup_power_well(struct drm_i915_private *dev_priv,
-						 enum punit_power_well power_well_id)
-{
-	struct i915_power_domains *power_domains = &dev_priv->power_domains;
-	struct i915_power_well *power_well;
-	int i;
-
-	for_each_power_well(i, power_well, POWER_DOMAIN_MASK, power_domains) {
-		if (power_well->data == power_well_id)
-			return power_well;
-	}
-
-	return NULL;
-}
-
-#define set_power_wells(power_domains, __power_wells) ({		\
-	(power_domains)->power_wells = (__power_wells);			\
-	(power_domains)->power_well_count = ARRAY_SIZE(__power_wells);	\
-})
-
-int intel_power_domains_init(struct drm_i915_private *dev_priv)
-{
-	struct i915_power_domains *power_domains = &dev_priv->power_domains;
-
-	mutex_init(&power_domains->lock);
-
-	/*
-	 * The enabling order will be from lower to higher indexed wells,
-	 * the disabling order is reversed.
-	 */
-	if (IS_HASWELL(dev_priv->dev)) {
-		set_power_wells(power_domains, hsw_power_wells);
-		hsw_pwr = power_domains;
-	} else if (IS_BROADWELL(dev_priv->dev)) {
-		set_power_wells(power_domains, bdw_power_wells);
-		hsw_pwr = power_domains;
-	} else if (IS_CHERRYVIEW(dev_priv->dev)) {
-		set_power_wells(power_domains, chv_power_wells);
-	} else if (IS_VALLEYVIEW(dev_priv->dev)) {
-		set_power_wells(power_domains, vlv_power_wells);
-	} else {
-		set_power_wells(power_domains, i9xx_always_on_power_well);
-	}
-
-	return 0;
-}
-
-void intel_power_domains_remove(struct drm_i915_private *dev_priv)
-{
-	hsw_pwr = NULL;
-}
-
-static void intel_power_domains_resume(struct drm_i915_private *dev_priv)
-{
-	struct i915_power_domains *power_domains = &dev_priv->power_domains;
-	struct i915_power_well *power_well;
-	int i;
-
-	mutex_lock(&power_domains->lock);
-	for_each_power_well(i, power_well, POWER_DOMAIN_MASK, power_domains) {
-		power_well->ops->sync_hw(dev_priv, power_well);
-		power_well->hw_enabled = power_well->ops->is_enabled(dev_priv,
-								     power_well);
-	}
-	mutex_unlock(&power_domains->lock);
-}
-
-static void vlv_cmnlane_wa(struct drm_i915_private *dev_priv)
-{
-	struct i915_power_well *cmn =
-		lookup_power_well(dev_priv, PUNIT_POWER_WELL_DPIO_CMN_BC);
-	struct i915_power_well *disp2d =
-		lookup_power_well(dev_priv, PUNIT_POWER_WELL_DISP2D);
-
-	/* nothing to do if common lane is already off */
-	if (!cmn->ops->is_enabled(dev_priv, cmn))
-		return;
-
-	/* If the display might be already active skip this */
-	if (disp2d->ops->is_enabled(dev_priv, disp2d) &&
-	    I915_READ(DPIO_CTL) & DPIO_CMNRST)
-		return;
-
-	DRM_DEBUG_KMS("toggling display PHY side reset\n");
-
-	/* cmnlane needs DPLL registers */
-	disp2d->ops->enable(dev_priv, disp2d);
-
-	/*
-	 * From VLV2A0_DP_eDP_HDMI_DPIO_driver_vbios_notes_11.docx:
-	 * Need to assert and de-assert PHY SB reset by gating the
-	 * common lane power, then un-gating it.
-	 * Simply ungating isn't enough to reset the PHY enough to get
-	 * ports and lanes running.
-	 */
-	cmn->ops->disable(dev_priv, cmn);
-}
-
-void intel_power_domains_init_hw(struct drm_i915_private *dev_priv)
-{
-	struct drm_device *dev = dev_priv->dev;
-	struct i915_power_domains *power_domains = &dev_priv->power_domains;
-
-	power_domains->initializing = true;
-
-	if (IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev)) {
-		mutex_lock(&power_domains->lock);
-		vlv_cmnlane_wa(dev_priv);
-		mutex_unlock(&power_domains->lock);
-	}
-
-	/* For now, we need the power well to be always enabled. */
-	intel_display_set_init_power(dev_priv, true);
-	intel_power_domains_resume(dev_priv);
-	power_domains->initializing = false;
-}
-
-void intel_aux_display_runtime_get(struct drm_i915_private *dev_priv)
-{
-	intel_runtime_pm_get(dev_priv);
-}
-
-void intel_aux_display_runtime_put(struct drm_i915_private *dev_priv)
-{
-	intel_runtime_pm_put(dev_priv);
-}
-
-void intel_runtime_pm_get(struct drm_i915_private *dev_priv)
-{
-	struct drm_device *dev = dev_priv->dev;
-	struct device *device = &dev->pdev->dev;
-
-	if (!HAS_RUNTIME_PM(dev))
-		return;
-
-	pm_runtime_get_sync(device);
-	WARN(dev_priv->pm.suspended, "Device still suspended.\n");
-}
-
-void intel_runtime_pm_get_noresume(struct drm_i915_private *dev_priv)
-{
-	struct drm_device *dev = dev_priv->dev;
-	struct device *device = &dev->pdev->dev;
-
-	if (!HAS_RUNTIME_PM(dev))
-		return;
-
-	WARN(dev_priv->pm.suspended, "Getting nosync-ref while suspended.\n");
-	pm_runtime_get_noresume(device);
-}
-
-void intel_runtime_pm_put(struct drm_i915_private *dev_priv)
-{
-	struct drm_device *dev = dev_priv->dev;
-	struct device *device = &dev->pdev->dev;
-
-	if (!HAS_RUNTIME_PM(dev))
-		return;
-
-	pm_runtime_mark_last_busy(device);
-	pm_runtime_put_autosuspend(device);
-}
-
-void intel_init_runtime_pm(struct drm_i915_private *dev_priv)
-{
-	struct drm_device *dev = dev_priv->dev;
-	struct device *device = &dev->pdev->dev;
-
-	if (!HAS_RUNTIME_PM(dev))
-		return;
-
-	pm_runtime_set_active(device);
-
-	/*
-	 * RPM depends on RC6 to save restore the GT HW context, so make RC6 a
-	 * requirement.
-	 */
-	if (!intel_enable_rc6(dev)) {
-		DRM_INFO("RC6 disabled, disabling runtime PM support\n");
-		return;
+		/* This value was pulled out of someone's hat */
+		I915_WRITE(FBC_CONTROL, 500 << FBC_CTL_INTERVAL_SHIFT);
 	}
 
-	pm_runtime_set_autosuspend_delay(device, 10000); /* 10s */
-	pm_runtime_mark_last_busy(device);
-	pm_runtime_use_autosuspend(device);
-
-	pm_runtime_put_autosuspend(device);
-}
-
-void intel_fini_runtime_pm(struct drm_i915_private *dev_priv)
-{
-	struct drm_device *dev = dev_priv->dev;
-	struct device *device = &dev->pdev->dev;
-
-	if (!HAS_RUNTIME_PM(dev))
-		return;
-
-	if (!intel_enable_rc6(dev))
-		return;
-
-	/* Make sure we're not suspended first. */
-	pm_runtime_get_sync(device);
-	pm_runtime_disable(device);
+	dev_priv->fbc.enabled = dev_priv->display.fbc_enabled(dev_priv->dev);
 }
 
 /* Set up chip specific power management-related functions */
@@ -7198,28 +7087,7 @@ void intel_init_pm(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	if (HAS_FBC(dev)) {
-		if (INTEL_INFO(dev)->gen >= 7) {
-			dev_priv->display.fbc_enabled = ironlake_fbc_enabled;
-			dev_priv->display.enable_fbc = gen7_enable_fbc;
-			dev_priv->display.disable_fbc = ironlake_disable_fbc;
-		} else if (INTEL_INFO(dev)->gen >= 5) {
-			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 {
-			dev_priv->display.fbc_enabled = i8xx_fbc_enabled;
-			dev_priv->display.enable_fbc = i8xx_enable_fbc;
-			dev_priv->display.disable_fbc = i8xx_disable_fbc;
-
-			/* This value was pulled out of someone's hat */
-			I915_WRITE(FBC_CONTROL, 500 << FBC_CTL_INTERVAL_SHIFT);
-		}
-	}
+	intel_init_fbc(dev_priv);
 
 	/* For cxsr */
 	if (IS_PINEVIEW(dev))
@@ -7228,7 +7096,13 @@ void intel_init_pm(struct drm_device *dev)
 		i915_ironlake_get_mem_freq(dev);
 
 	/* For FIFO watermark updates */
-	if (HAS_PCH_SPLIT(dev)) {
+	if (INTEL_INFO(dev)->gen >= 9) {
+		skl_setup_wm_latency(dev);
+
+		dev_priv->display.init_clock_gating = gen9_init_clock_gating;
+		dev_priv->display.update_wm = skl_update_wm;
+		dev_priv->display.update_sprite_wm = skl_update_sprite_wm;
+	} else if (HAS_PCH_SPLIT(dev)) {
 		ilk_setup_wm_latency(dev);
 
 		if ((IS_GEN5(dev) && dev_priv->wm.pri_latency[1] &&
@@ -7309,7 +7183,7 @@ void intel_init_pm(struct drm_device *dev)
 	}
 }
 
-int sandybridge_pcode_read(struct drm_i915_private *dev_priv, u8 mbox, u32 *val)
+int sandybridge_pcode_read(struct drm_i915_private *dev_priv, u32 mbox, u32 *val)
 {
 	WARN_ON(!mutex_is_locked(&dev_priv->rps.hw_lock));
 
@@ -7319,6 +7193,7 @@ int sandybridge_pcode_read(struct drm_i915_private *dev_priv, u8 mbox, u32 *val)
 	}
 
 	I915_WRITE(GEN6_PCODE_DATA, *val);
+	I915_WRITE(GEN6_PCODE_DATA1, 0);
 	I915_WRITE(GEN6_PCODE_MAILBOX, GEN6_PCODE_READY | mbox);
 
 	if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) & GEN6_PCODE_READY) == 0,
@@ -7333,7 +7208,7 @@ int sandybridge_pcode_read(struct drm_i915_private *dev_priv, u8 mbox, u32 *val)
 	return 0;
 }
 
-int sandybridge_pcode_write(struct drm_i915_private *dev_priv, u8 mbox, u32 val)
+int sandybridge_pcode_write(struct drm_i915_private *dev_priv, u32 mbox, u32 val)
 {
 	WARN_ON(!mutex_is_locked(&dev_priv->rps.hw_lock));
 
@@ -7356,99 +7231,66 @@ int sandybridge_pcode_write(struct drm_i915_private *dev_priv, u8 mbox, u32 val)
 	return 0;
 }
 
-static int byt_gpu_freq(struct drm_i915_private *dev_priv, int val)
+static int vlv_gpu_freq_div(unsigned int czclk_freq)
 {
-	int div;
-
-	/* 4 x czclk */
-	switch (dev_priv->mem_freq) {
-	case 800:
-		div = 10;
-		break;
-	case 1066:
-		div = 12;
-		break;
-	case 1333:
-		div = 16;
-		break;
+	switch (czclk_freq) {
+	case 200:
+		return 10;
+	case 267:
+		return 12;
+	case 320:
+	case 333:
+		return 16;
+	case 400:
+		return 20;
 	default:
 		return -1;
 	}
+}
+
+static int byt_gpu_freq(struct drm_i915_private *dev_priv, int val)
+{
+	int div, czclk_freq = DIV_ROUND_CLOSEST(dev_priv->mem_freq, 4);
+
+	div = vlv_gpu_freq_div(czclk_freq);
+	if (div < 0)
+		return div;
 
-	return DIV_ROUND_CLOSEST(dev_priv->mem_freq * (val + 6 - 0xbd), 4 * div);
+	return DIV_ROUND_CLOSEST(czclk_freq * (val + 6 - 0xbd), div);
 }
 
 static int byt_freq_opcode(struct drm_i915_private *dev_priv, int val)
 {
-	int mul;
+	int mul, czclk_freq = DIV_ROUND_CLOSEST(dev_priv->mem_freq, 4);
 
-	/* 4 x czclk */
-	switch (dev_priv->mem_freq) {
-	case 800:
-		mul = 10;
-		break;
-	case 1066:
-		mul = 12;
-		break;
-	case 1333:
-		mul = 16;
-		break;
-	default:
-		return -1;
-	}
+	mul = vlv_gpu_freq_div(czclk_freq);
+	if (mul < 0)
+		return mul;
 
-	return DIV_ROUND_CLOSEST(4 * mul * val, dev_priv->mem_freq) + 0xbd - 6;
+	return DIV_ROUND_CLOSEST(mul * val, czclk_freq) + 0xbd - 6;
 }
 
 static int chv_gpu_freq(struct drm_i915_private *dev_priv, int val)
 {
-	int div, freq;
+	int div, czclk_freq = dev_priv->rps.cz_freq;
 
-	switch (dev_priv->rps.cz_freq) {
-	case 200:
-		div = 5;
-		break;
-	case 267:
-		div = 6;
-		break;
-	case 320:
-	case 333:
-	case 400:
-		div = 8;
-		break;
-	default:
-		return -1;
-	}
-
-	freq = (DIV_ROUND_CLOSEST((dev_priv->rps.cz_freq * val), 2 * div) / 2);
+	div = vlv_gpu_freq_div(czclk_freq) / 2;
+	if (div < 0)
+		return div;
 
-	return freq;
+	return DIV_ROUND_CLOSEST(czclk_freq * val, 2 * div) / 2;
 }
 
 static int chv_freq_opcode(struct drm_i915_private *dev_priv, int val)
 {
-	int mul, opcode;
+	int mul, czclk_freq = dev_priv->rps.cz_freq;
 
-	switch (dev_priv->rps.cz_freq) {
-	case 200:
-		mul = 5;
-		break;
-	case 267:
-		mul = 6;
-		break;
-	case 320:
-	case 333:
-	case 400:
-		mul = 8;
-		break;
-	default:
-		return -1;
-	}
+	mul = vlv_gpu_freq_div(czclk_freq) / 2;
+	if (mul < 0)
+		return mul;
 
 	/* CHV needs even values */
-	opcode = (DIV_ROUND_CLOSEST((val * 2 * mul), dev_priv->rps.cz_freq) * 2);
-
-	return opcode;
+	return DIV_ROUND_CLOSEST(val * 2 * mul, czclk_freq) * 2;
 }
 
 int vlv_gpu_freq(struct drm_i915_private *dev_priv, int val)
@@ -7485,5 +7327,4 @@ void intel_pm_setup(struct drm_device *dev)
 			  intel_gen6_powersave_work);
 
 	dev_priv->pm.suspended = false;
-	dev_priv->pm._irqs_disabled = false;
 }
diff --git a/drivers/gpu/drm/i915/intel_psr.c b/drivers/gpu/drm/i915/intel_psr.c
new file mode 100644
index 000000000000..716b8a961eea
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_psr.c
@@ -0,0 +1,481 @@
+/*
+ * Copyright © 2014 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.
+ */
+
+/**
+ * DOC: Panel Self Refresh (PSR/SRD)
+ *
+ * Since Haswell Display controller supports Panel Self-Refresh on display
+ * panels witch have a remote frame buffer (RFB) implemented according to PSR
+ * spec in eDP1.3. PSR feature allows the display to go to lower standby states
+ * when system is idle but display is on as it eliminates display refresh
+ * request to DDR memory completely as long as the frame buffer for that
+ * display is unchanged.
+ *
+ * Panel Self Refresh must be supported by both Hardware (source) and
+ * Panel (sink).
+ *
+ * PSR saves power by caching the framebuffer in the panel RFB, which allows us
+ * to power down the link and memory controller. For DSI panels the same idea
+ * is called "manual mode".
+ *
+ * The implementation uses the hardware-based PSR support which automatically
+ * enters/exits self-refresh mode. The hardware takes care of sending the
+ * required DP aux message and could even retrain the link (that part isn't
+ * enabled yet though). The hardware also keeps track of any frontbuffer
+ * changes to know when to exit self-refresh mode again. Unfortunately that
+ * part doesn't work too well, hence why the i915 PSR support uses the
+ * software frontbuffer tracking to make sure it doesn't miss a screen
+ * update. For this integration intel_psr_invalidate() and intel_psr_flush()
+ * get called by the frontbuffer tracking code. Note that because of locking
+ * issues the self-refresh re-enable code is done from a work queue, which
+ * must be correctly synchronized/cancelled when shutting down the pipe."
+ */
+
+#include <drm/drmP.h>
+
+#include "intel_drv.h"
+#include "i915_drv.h"
+
+static bool is_edp_psr(struct intel_dp *intel_dp)
+{
+	return intel_dp->psr_dpcd[0] & DP_PSR_IS_SUPPORTED;
+}
+
+bool intel_psr_is_enabled(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (!HAS_PSR(dev))
+		return false;
+
+	return I915_READ(EDP_PSR_CTL(dev)) & EDP_PSR_ENABLE;
+}
+
+static void intel_psr_write_vsc(struct intel_dp *intel_dp,
+				    struct edp_vsc_psr *vsc_psr)
+{
+	struct intel_digital_port *dig_port = dp_to_dig_port(intel_dp);
+	struct drm_device *dev = dig_port->base.base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *crtc = to_intel_crtc(dig_port->base.base.crtc);
+	u32 ctl_reg = HSW_TVIDEO_DIP_CTL(crtc->config.cpu_transcoder);
+	u32 data_reg = HSW_TVIDEO_DIP_VSC_DATA(crtc->config.cpu_transcoder);
+	uint32_t *data = (uint32_t *) vsc_psr;
+	unsigned int i;
+
+	/* As per BSPec (Pipe Video Data Island Packet), we need to disable
+	   the video DIP being updated before program video DIP data buffer
+	   registers for DIP being updated. */
+	I915_WRITE(ctl_reg, 0);
+	POSTING_READ(ctl_reg);
+
+	for (i = 0; i < VIDEO_DIP_VSC_DATA_SIZE; i += 4) {
+		if (i < sizeof(struct edp_vsc_psr))
+			I915_WRITE(data_reg + i, *data++);
+		else
+			I915_WRITE(data_reg + i, 0);
+	}
+
+	I915_WRITE(ctl_reg, VIDEO_DIP_ENABLE_VSC_HSW);
+	POSTING_READ(ctl_reg);
+}
+
+static void intel_psr_setup_vsc(struct intel_dp *intel_dp)
+{
+	struct edp_vsc_psr psr_vsc;
+
+	/* Prepare VSC packet as per EDP 1.3 spec, Table 3.10 */
+	memset(&psr_vsc, 0, sizeof(psr_vsc));
+	psr_vsc.sdp_header.HB0 = 0;
+	psr_vsc.sdp_header.HB1 = 0x7;
+	psr_vsc.sdp_header.HB2 = 0x2;
+	psr_vsc.sdp_header.HB3 = 0x8;
+	intel_psr_write_vsc(intel_dp, &psr_vsc);
+}
+
+static void intel_psr_enable_sink(struct intel_dp *intel_dp)
+{
+	struct intel_digital_port *dig_port = dp_to_dig_port(intel_dp);
+	struct drm_device *dev = dig_port->base.base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t aux_clock_divider;
+	int precharge = 0x3;
+	bool only_standby = false;
+	static const uint8_t aux_msg[] = {
+		[0] = DP_AUX_NATIVE_WRITE << 4,
+		[1] = DP_SET_POWER >> 8,
+		[2] = DP_SET_POWER & 0xff,
+		[3] = 1 - 1,
+		[4] = DP_SET_POWER_D0,
+	};
+	int i;
+
+	BUILD_BUG_ON(sizeof(aux_msg) > 20);
+
+	aux_clock_divider = intel_dp->get_aux_clock_divider(intel_dp, 0);
+
+	if (IS_BROADWELL(dev) && dig_port->port != PORT_A)
+		only_standby = true;
+
+	/* Enable PSR in sink */
+	if (intel_dp->psr_dpcd[1] & DP_PSR_NO_TRAIN_ON_EXIT || only_standby)
+		drm_dp_dpcd_writeb(&intel_dp->aux, DP_PSR_EN_CFG,
+				   DP_PSR_ENABLE & ~DP_PSR_MAIN_LINK_ACTIVE);
+	else
+		drm_dp_dpcd_writeb(&intel_dp->aux, DP_PSR_EN_CFG,
+				   DP_PSR_ENABLE | DP_PSR_MAIN_LINK_ACTIVE);
+
+	/* Setup AUX registers */
+	for (i = 0; i < sizeof(aux_msg); i += 4)
+		I915_WRITE(EDP_PSR_AUX_DATA1(dev) + i,
+			   intel_dp_pack_aux(&aux_msg[i], sizeof(aux_msg) - i));
+
+	I915_WRITE(EDP_PSR_AUX_CTL(dev),
+		   DP_AUX_CH_CTL_TIME_OUT_400us |
+		   (sizeof(aux_msg) << DP_AUX_CH_CTL_MESSAGE_SIZE_SHIFT) |
+		   (precharge << DP_AUX_CH_CTL_PRECHARGE_2US_SHIFT) |
+		   (aux_clock_divider << DP_AUX_CH_CTL_BIT_CLOCK_2X_SHIFT));
+}
+
+static void intel_psr_enable_source(struct intel_dp *intel_dp)
+{
+	struct intel_digital_port *dig_port = dp_to_dig_port(intel_dp);
+	struct drm_device *dev = dig_port->base.base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t max_sleep_time = 0x1f;
+	uint32_t idle_frames = 1;
+	uint32_t val = 0x0;
+	const uint32_t link_entry_time = EDP_PSR_MIN_LINK_ENTRY_TIME_8_LINES;
+	bool only_standby = false;
+
+	if (IS_BROADWELL(dev) && dig_port->port != PORT_A)
+		only_standby = true;
+
+	if (intel_dp->psr_dpcd[1] & DP_PSR_NO_TRAIN_ON_EXIT || only_standby) {
+		val |= EDP_PSR_LINK_STANDBY;
+		val |= EDP_PSR_TP2_TP3_TIME_0us;
+		val |= EDP_PSR_TP1_TIME_0us;
+		val |= EDP_PSR_SKIP_AUX_EXIT;
+		val |= IS_BROADWELL(dev) ? BDW_PSR_SINGLE_FRAME : 0;
+	} else
+		val |= EDP_PSR_LINK_DISABLE;
+
+	I915_WRITE(EDP_PSR_CTL(dev), val |
+		   (IS_BROADWELL(dev) ? 0 : link_entry_time) |
+		   max_sleep_time << EDP_PSR_MAX_SLEEP_TIME_SHIFT |
+		   idle_frames << EDP_PSR_IDLE_FRAME_SHIFT |
+		   EDP_PSR_ENABLE);
+}
+
+static bool intel_psr_match_conditions(struct intel_dp *intel_dp)
+{
+	struct intel_digital_port *dig_port = dp_to_dig_port(intel_dp);
+	struct drm_device *dev = dig_port->base.base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc = dig_port->base.base.crtc;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+
+	lockdep_assert_held(&dev_priv->psr.lock);
+	WARN_ON(!drm_modeset_is_locked(&dev->mode_config.connection_mutex));
+	WARN_ON(!drm_modeset_is_locked(&crtc->mutex));
+
+	dev_priv->psr.source_ok = false;
+
+	if (IS_HASWELL(dev) && dig_port->port != PORT_A) {
+		DRM_DEBUG_KMS("HSW ties PSR to DDI A (eDP)\n");
+		return false;
+	}
+
+	if (!i915.enable_psr) {
+		DRM_DEBUG_KMS("PSR disable by flag\n");
+		return false;
+	}
+
+	/* Below limitations aren't valid for Broadwell */
+	if (IS_BROADWELL(dev))
+		goto out;
+
+	if (I915_READ(HSW_STEREO_3D_CTL(intel_crtc->config.cpu_transcoder)) &
+	    S3D_ENABLE) {
+		DRM_DEBUG_KMS("PSR condition failed: Stereo 3D is Enabled\n");
+		return false;
+	}
+
+	if (intel_crtc->config.adjusted_mode.flags & DRM_MODE_FLAG_INTERLACE) {
+		DRM_DEBUG_KMS("PSR condition failed: Interlaced is Enabled\n");
+		return false;
+	}
+
+ out:
+	dev_priv->psr.source_ok = true;
+	return true;
+}
+
+static void intel_psr_do_enable(struct intel_dp *intel_dp)
+{
+	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
+	struct drm_device *dev = intel_dig_port->base.base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	WARN_ON(I915_READ(EDP_PSR_CTL(dev)) & EDP_PSR_ENABLE);
+	WARN_ON(dev_priv->psr.active);
+	lockdep_assert_held(&dev_priv->psr.lock);
+
+	/* Enable/Re-enable PSR on the host */
+	intel_psr_enable_source(intel_dp);
+
+	dev_priv->psr.active = true;
+}
+
+/**
+ * intel_psr_enable - Enable PSR
+ * @intel_dp: Intel DP
+ *
+ * This function can only be called after the pipe is fully trained and enabled.
+ */
+void intel_psr_enable(struct intel_dp *intel_dp)
+{
+	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
+	struct drm_device *dev = intel_dig_port->base.base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (!HAS_PSR(dev)) {
+		DRM_DEBUG_KMS("PSR not supported on this platform\n");
+		return;
+	}
+
+	if (!is_edp_psr(intel_dp)) {
+		DRM_DEBUG_KMS("PSR not supported by this panel\n");
+		return;
+	}
+
+	mutex_lock(&dev_priv->psr.lock);
+	if (dev_priv->psr.enabled) {
+		DRM_DEBUG_KMS("PSR already in use\n");
+		goto unlock;
+	}
+
+	if (!intel_psr_match_conditions(intel_dp))
+		goto unlock;
+
+	dev_priv->psr.busy_frontbuffer_bits = 0;
+
+	intel_psr_setup_vsc(intel_dp);
+
+	/* Avoid continuous PSR exit by masking memup and hpd */
+	I915_WRITE(EDP_PSR_DEBUG_CTL(dev), EDP_PSR_DEBUG_MASK_MEMUP |
+		   EDP_PSR_DEBUG_MASK_HPD | EDP_PSR_DEBUG_MASK_LPSP);
+
+	/* Enable PSR on the panel */
+	intel_psr_enable_sink(intel_dp);
+
+	dev_priv->psr.enabled = intel_dp;
+unlock:
+	mutex_unlock(&dev_priv->psr.lock);
+}
+
+/**
+ * intel_psr_disable - Disable PSR
+ * @intel_dp: Intel DP
+ *
+ * This function needs to be called before disabling pipe.
+ */
+void intel_psr_disable(struct intel_dp *intel_dp)
+{
+	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
+	struct drm_device *dev = intel_dig_port->base.base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	mutex_lock(&dev_priv->psr.lock);
+	if (!dev_priv->psr.enabled) {
+		mutex_unlock(&dev_priv->psr.lock);
+		return;
+	}
+
+	if (dev_priv->psr.active) {
+		I915_WRITE(EDP_PSR_CTL(dev),
+			   I915_READ(EDP_PSR_CTL(dev)) & ~EDP_PSR_ENABLE);
+
+		/* Wait till PSR is idle */
+		if (_wait_for((I915_READ(EDP_PSR_STATUS_CTL(dev)) &
+			       EDP_PSR_STATUS_STATE_MASK) == 0, 2000, 10))
+			DRM_ERROR("Timed out waiting for PSR Idle State\n");
+
+		dev_priv->psr.active = false;
+	} else {
+		WARN_ON(I915_READ(EDP_PSR_CTL(dev)) & EDP_PSR_ENABLE);
+	}
+
+	dev_priv->psr.enabled = NULL;
+	mutex_unlock(&dev_priv->psr.lock);
+
+	cancel_delayed_work_sync(&dev_priv->psr.work);
+}
+
+static void intel_psr_work(struct work_struct *work)
+{
+	struct drm_i915_private *dev_priv =
+		container_of(work, typeof(*dev_priv), psr.work.work);
+	struct intel_dp *intel_dp = dev_priv->psr.enabled;
+
+	/* We have to make sure PSR is ready for re-enable
+	 * otherwise it keeps disabled until next full enable/disable cycle.
+	 * PSR might take some time to get fully disabled
+	 * and be ready for re-enable.
+	 */
+	if (wait_for((I915_READ(EDP_PSR_STATUS_CTL(dev_priv->dev)) &
+		      EDP_PSR_STATUS_STATE_MASK) == 0, 50)) {
+		DRM_ERROR("Timed out waiting for PSR Idle for re-enable\n");
+		return;
+	}
+
+	mutex_lock(&dev_priv->psr.lock);
+	intel_dp = dev_priv->psr.enabled;
+
+	if (!intel_dp)
+		goto unlock;
+
+	/*
+	 * The delayed work can race with an invalidate hence we need to
+	 * recheck. Since psr_flush first clears this and then reschedules we
+	 * won't ever miss a flush when bailing out here.
+	 */
+	if (dev_priv->psr.busy_frontbuffer_bits)
+		goto unlock;
+
+	intel_psr_do_enable(intel_dp);
+unlock:
+	mutex_unlock(&dev_priv->psr.lock);
+}
+
+static void intel_psr_exit(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (dev_priv->psr.active) {
+		u32 val = I915_READ(EDP_PSR_CTL(dev));
+
+		WARN_ON(!(val & EDP_PSR_ENABLE));
+
+		I915_WRITE(EDP_PSR_CTL(dev), val & ~EDP_PSR_ENABLE);
+
+		dev_priv->psr.active = false;
+	}
+
+}
+
+/**
+ * intel_psr_invalidate - Invalidade PSR
+ * @dev: DRM device
+ * @frontbuffer_bits: frontbuffer plane tracking bits
+ *
+ * Since the hardware frontbuffer tracking has gaps we need to integrate
+ * with the software frontbuffer tracking. This function gets called every
+ * time frontbuffer rendering starts and a buffer gets dirtied. PSR must be
+ * disabled if the frontbuffer mask contains a buffer relevant to PSR.
+ *
+ * Dirty frontbuffers relevant to PSR are tracked in busy_frontbuffer_bits."
+ */
+void intel_psr_invalidate(struct drm_device *dev,
+			      unsigned frontbuffer_bits)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc;
+	enum pipe pipe;
+
+	mutex_lock(&dev_priv->psr.lock);
+	if (!dev_priv->psr.enabled) {
+		mutex_unlock(&dev_priv->psr.lock);
+		return;
+	}
+
+	crtc = dp_to_dig_port(dev_priv->psr.enabled)->base.base.crtc;
+	pipe = to_intel_crtc(crtc)->pipe;
+
+	intel_psr_exit(dev);
+
+	frontbuffer_bits &= INTEL_FRONTBUFFER_ALL_MASK(pipe);
+
+	dev_priv->psr.busy_frontbuffer_bits |= frontbuffer_bits;
+	mutex_unlock(&dev_priv->psr.lock);
+}
+
+/**
+ * intel_psr_flush - Flush PSR
+ * @dev: DRM device
+ * @frontbuffer_bits: frontbuffer plane tracking bits
+ *
+ * Since the hardware frontbuffer tracking has gaps we need to integrate
+ * with the software frontbuffer tracking. This function gets called every
+ * time frontbuffer rendering has completed and flushed out to memory. PSR
+ * can be enabled again if no other frontbuffer relevant to PSR is dirty.
+ *
+ * Dirty frontbuffers relevant to PSR are tracked in busy_frontbuffer_bits.
+ */
+void intel_psr_flush(struct drm_device *dev,
+			 unsigned frontbuffer_bits)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc;
+	enum pipe pipe;
+
+	mutex_lock(&dev_priv->psr.lock);
+	if (!dev_priv->psr.enabled) {
+		mutex_unlock(&dev_priv->psr.lock);
+		return;
+	}
+
+	crtc = dp_to_dig_port(dev_priv->psr.enabled)->base.base.crtc;
+	pipe = to_intel_crtc(crtc)->pipe;
+	dev_priv->psr.busy_frontbuffer_bits &= ~frontbuffer_bits;
+
+	/*
+	 * On Haswell sprite plane updates don't result in a psr invalidating
+	 * signal in the hardware. Which means we need to manually fake this in
+	 * software for all flushes, not just when we've seen a preceding
+	 * invalidation through frontbuffer rendering.
+	 */
+	if (IS_HASWELL(dev) &&
+	    (frontbuffer_bits & INTEL_FRONTBUFFER_SPRITE(pipe)))
+		intel_psr_exit(dev);
+
+	if (!dev_priv->psr.active && !dev_priv->psr.busy_frontbuffer_bits)
+		schedule_delayed_work(&dev_priv->psr.work,
+				      msecs_to_jiffies(100));
+	mutex_unlock(&dev_priv->psr.lock);
+}
+
+/**
+ * intel_psr_init - Init basic PSR work and mutex.
+ * @dev: DRM device
+ *
+ * This function is  called only once at driver load to initialize basic
+ * PSR stuff.
+ */
+void intel_psr_init(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	INIT_DELAYED_WORK(&dev_priv->psr.work, intel_psr_work);
+	mutex_init(&dev_priv->psr.lock);
+}
diff --git a/drivers/gpu/drm/i915/intel_renderstate.h b/drivers/gpu/drm/i915/intel_renderstate.h
index 6c792d3a9c9c..5bd69852752c 100644
--- a/drivers/gpu/drm/i915/intel_renderstate.h
+++ b/drivers/gpu/drm/i915/intel_renderstate.h
@@ -29,6 +29,7 @@
 extern const struct intel_renderstate_rodata gen6_null_state;
 extern const struct intel_renderstate_rodata gen7_null_state;
 extern const struct intel_renderstate_rodata gen8_null_state;
+extern const struct intel_renderstate_rodata gen9_null_state;
 
 #define RO_RENDERSTATE(_g)						\
 	const struct intel_renderstate_rodata gen ## _g ## _null_state = { \
diff --git a/drivers/gpu/drm/i915/intel_renderstate_gen8.c b/drivers/gpu/drm/i915/intel_renderstate_gen8.c
index 75ef1b5de45c..78011d73fa9f 100644
--- a/drivers/gpu/drm/i915/intel_renderstate_gen8.c
+++ b/drivers/gpu/drm/i915/intel_renderstate_gen8.c
@@ -1,16 +1,134 @@
 #include "intel_renderstate.h"
 
 static const u32 gen8_null_state_relocs[] = {
-	0x00000048,
-	0x00000050,
-	0x00000060,
-	0x000003ec,
+	0x00000798,
+	0x000007a4,
+	0x000007ac,
+	0x000007bc,
 	-1,
 };
 
 static const u32 gen8_null_state_batch[] = {
+	0x7a000004,
+	0x01000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
 	0x69040000,
-	0x61020001,
+	0x78140000,
+	0x04000000,
+	0x7820000a,
+	0x00000000,
+	0x00000000,
+	0x80000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78130002,
+	0x00000000,
+	0x00000000,
+	0x02001808,
+	0x781f0002,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78510009,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78100007,
+	0x00000000,
+	0x00000000,
+	0x00010000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x781b0007,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000800,
+	0x00000000,
+	0x78110008,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x781e0003,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x781d0007,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78120002,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78500003,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x781c0002,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x780c0000,
+	0x00000000,
+	0x78520003,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78300000,
+	0x08010040,
+	0x78310000,
+	0x1e000000,
+	0x78320000,
+	0x1e000000,
+	0x78330000,
+	0x1e000000,
+	0x79190002,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x791a0002,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x791b0002,
+	0x00000000,
 	0x00000000,
 	0x00000000,
 	0x79120000,
@@ -23,48 +141,435 @@ static const u32 gen8_null_state_batch[] = {
 	0x00000000,
 	0x79160000,
 	0x00000000,
-	0x6101000e,
-	0x00000001,
+	0x78150009,
 	0x00000000,
-	0x00000001,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78190009,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x781a0009,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78160009,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78170009,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78490001,
+	0x00000000,
+	0x00000000,
+	0x784a0000,
+	0x00000000,
+	0x784b0000,
+	0x00000004,
+	0x79170101,
+	0x00000000,
+	0x00000080,
+	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,
+	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,
+	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,
+	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,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x79180006,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x79180006,
+	0x20000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x79180006,
+	0x40000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x79180006,
+	0x60000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x6101000e,
 	0x00000001,	 /* reloc */
 	0x00000000,
+	0x00000000,
 	0x00000001,	 /* reloc */
 	0x00000000,
+	0x00000001,	 /* reloc */
 	0x00000000,
+	0x00000001,
 	0x00000000,
 	0x00000001,	 /* reloc */
 	0x00000000,
-	0xfffff001,
 	0x00001001,
-	0xfffff001,
 	0x00001001,
-	0x78230000,
-	0x000006e0,
-	0x78210000,
-	0x00000700,
-	0x78300000,
-	0x08010040,
-	0x78330000,
-	0x08000000,
-	0x78310000,
-	0x08000000,
-	0x78320000,
-	0x08000000,
-	0x78240000,
-	0x00000641,
-	0x780e0000,
-	0x00000601,
+	0x00000001,
+	0x00001001,
+	0x61020001,
+	0x00000000,
+	0x00000000,
+	0x79000002,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78050006,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x79040002,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x79040002,
+	0x40000000,
+	0x00000000,
+	0x00000000,
+	0x79040002,
+	0x80000000,
+	0x00000000,
+	0x00000000,
+	0x79040002,
+	0xc0000000,
+	0x00000000,
+	0x00000000,
+	0x79080001,
+	0x00000000,
+	0x00000000,
+	0x790a0001,
+	0x00000000,
+	0x00000000,
+	0x78060003,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78070003,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78040001,
+	0x00000000,
+	0x00000000,
+	0x79110000,
+	0x00000000,
 	0x780d0000,
 	0x00000000,
-	0x78180000,
-	0x00000001,
-	0x78520003,
+	0x79060000,
 	0x00000000,
+	0x7907001f,
 	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x78190009,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -75,7 +580,6 @@ static const u32 gen8_null_state_batch[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x781b0007,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -84,26 +588,22 @@ static const u32 gen8_null_state_batch[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x78270000,
 	0x00000000,
-	0x782c0000,
 	0x00000000,
-	0x781c0002,
 	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x78160009,
 	0x00000000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
+	0x7902000f,
 	0x00000000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x78110008,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -113,12 +613,10 @@ static const u32 gen8_null_state_batch[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x78290000,
 	0x00000000,
-	0x782e0000,
 	0x00000000,
-	0x781a0009,
 	0x00000000,
+	0x790c000f,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -128,7 +626,6 @@ static const u32 gen8_null_state_batch[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x781d0007,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -136,153 +633,153 @@ static const u32 gen8_null_state_batch[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
+	0x780a0003,
 	0x00000000,
-	0x78280000,
 	0x00000000,
-	0x782d0000,
 	0x00000000,
-	0x78260000,
 	0x00000000,
-	0x782b0000,
+	0x78080083,
+	0x00004000,
 	0x00000000,
-	0x78150009,
 	0x00000000,
 	0x00000000,
+	0x04004000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
+	0x08004000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
+	0x0c004000,
 	0x00000000,
 	0x00000000,
-	0x78100007,
 	0x00000000,
+	0x10004000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
+	0x14004000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
+	0x18004000,
 	0x00000000,
-	0x781e0003,
 	0x00000000,
 	0x00000000,
+	0x1c004000,
 	0x00000000,
 	0x00000000,
-	0x78120002,
 	0x00000000,
+	0x20004000,
 	0x00000000,
 	0x00000000,
-	0x781f0002,
-	0x30400820,
 	0x00000000,
+	0x24004000,
 	0x00000000,
-	0x78510009,
 	0x00000000,
 	0x00000000,
+	0x28004000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
+	0x2c004000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
+	0x30004000,
 	0x00000000,
 	0x00000000,
-	0x78500003,
-	0x00210000,
 	0x00000000,
+	0x34004000,
 	0x00000000,
 	0x00000000,
-	0x78130002,
 	0x00000000,
+	0x38004000,
 	0x00000000,
 	0x00000000,
-	0x782a0000,
-	0x00000480,
-	0x782f0000,
-	0x00000540,
-	0x78140000,
-	0x00000800,
-	0x78170009,
 	0x00000000,
+	0x3c004000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
+	0x40004000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
+	0x44004000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x7820000a,
-	0x00000580,
+	0x48004000,
 	0x00000000,
-	0x08080000,
 	0x00000000,
 	0x00000000,
-	0x1f000002,
-	0x00060000,
+	0x4c004000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
+	0x50004000,
 	0x00000000,
-	0x784d0000,
-	0x40000000,
-	0x784f0000,
-	0x80000100,
-	0x780f0000,
-	0x00000740,
-	0x78050006,
 	0x00000000,
 	0x00000000,
+	0x54004000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
+	0x58004000,
 	0x00000000,
 	0x00000000,
-	0x78070003,
 	0x00000000,
+	0x5c004000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x78060003,
+	0x60004000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
+	0x64004000,
 	0x00000000,
-	0x78040001,
 	0x00000000,
-	0x00000001,
-	0x79000002,
-	0xffffffff,
+	0x00000000,
+	0x68004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x6c004000,
+	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x78080003,
-	0x00006000,
-	0x000005e0,	 /* reloc */
+	0x70004000,
 	0x00000000,
 	0x00000000,
-	0x78090005,
+	0x00000000,
+	0x74004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x7c004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x80004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78090043,
 	0x02000000,
 	0x22220000,
-	0x02f60000,
-	0x11230000,
-	0x02850004,
-	0x11230000,
-	0x784b0000,
-	0x0000000f,
-	0x78490001,
 	0x00000000,
 	0x00000000,
-	0x7b000005,
 	0x00000000,
-	0x00000003,
 	0x00000000,
-	0x00000001,
 	0x00000000,
 	0x00000000,
-	0x05000000,	 /* cmds end */
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -297,8 +794,6 @@ static const u32 gen8_null_state_batch[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x000004c0,	 /* state start */
-	0x00000500,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -345,46 +840,65 @@ static const u32 gen8_null_state_batch[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
+	0x680b0001,
+	0x78260000,
+	0x00000000,
+	0x78270000,
+	0x00000000,
+	0x78280000,
+	0x00000000,
+	0x78290000,
+	0x00000000,
+	0x782a0000,
+	0x00000000,
+	0x780e0000,
+	0x00000dc1,
+	0x78240000,
+	0x00000e01,
+	0x784f0000,
+	0x80000100,
+	0x784d0000,
+	0x40000000,
+	0x782b0000,
+	0x00000000,
+	0x782c0000,
+	0x00000000,
+	0x782d0000,
 	0x00000000,
+	0x782e0000,
 	0x00000000,
+	0x782f0000,
 	0x00000000,
-	0x00000092,
+	0x780f0000,
 	0x00000000,
+	0x78230000,
+	0x00000e60,
+	0x78210000,
+	0x00000e80,
+	0x7b000005,
+	0x00000004,
+	0x00000001,
 	0x00000000,
+	0x00000001,
 	0x00000000,
 	0x00000000,
+	0x05000000,	 /* cmds end */
 	0x00000000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
+	0x00000000,	 /* state start */
+	0x00000000,
+	0x3f800000,
+	0x3f800000,
+	0x3f800000,
+	0x3f800000,
+	0x00000000,
+	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x0060005a,
-	0x21403ae8,
-	0x3a0000c0,
-	0x008d0040,
-	0x0060005a,
-	0x21603ae8,
-	0x3a0000c0,
-	0x008d0080,
-	0x0060005a,
-	0x21803ae8,
-	0x3a0000d0,
-	0x008d0040,
-	0x0060005a,
-	0x21a03ae8,
-	0x3a0000d0,
-	0x008d0080,
-	0x02800031,
-	0x2e0022e8,
-	0x0e000140,
-	0x08840001,
-	0x05800031,
-	0x200022e0,
-	0x0e000e00,
-	0x90031000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -410,38 +924,6 @@ static const u32 gen8_null_state_batch[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x06200000,
-	0x00000002,
-	0x06200000,
-	0x00000002,
-	0x06200000,
-	0x00000002,
-	0x06200000,
-	0x00000002,
-	0x06200000,
-	0x00000002,
-	0x06200000,
-	0x00000002,
-	0x06200000,
-	0x00000002,
-	0x06200000,
-	0x00000002,
-	0x06200000,
-	0x00000002,
-	0x06200000,
-	0x00000002,
-	0x06200000,
-	0x00000002,
-	0x06200000,
-	0x00000002,
-	0x06200000,
-	0x00000002,
-	0x06200000,
-	0x00000002,
-	0x06200000,
-	0x00000002,
-	0x06200000,
-	0x00000002,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -449,8 +931,6 @@ static const u32 gen8_null_state_batch[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-	0xf99a130c,
-	0x799a130c,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -466,9 +946,7 @@ static const u32 gen8_null_state_batch[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x3f800000,
 	0x00000000,
-	0x3f800000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
diff --git a/drivers/gpu/drm/i915/intel_renderstate_gen9.c b/drivers/gpu/drm/i915/intel_renderstate_gen9.c
new file mode 100644
index 000000000000..875075373807
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_renderstate_gen9.c
@@ -0,0 +1,974 @@
+#include "intel_renderstate.h"
+
+static const u32 gen9_null_state_relocs[] = {
+	0x000007a8,
+	0x000007b4,
+	0x000007bc,
+	0x000007cc,
+	-1,
+};
+
+static const u32 gen9_null_state_batch[] = {
+	0x7a000004,
+	0x01000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x69040300,
+	0x78140000,
+	0x04000000,
+	0x7820000a,
+	0x00000000,
+	0x00000000,
+	0x80000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78130002,
+	0x00000000,
+	0x00000000,
+	0x02001808,
+	0x781f0004,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78510009,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78100007,
+	0x00000000,
+	0x00000000,
+	0x00010000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x781b0007,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000800,
+	0x00000000,
+	0x78110008,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x781e0003,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x781d0009,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78120002,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78500003,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x781c0002,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x780c0000,
+	0x00000000,
+	0x78520003,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78300000,
+	0x08010040,
+	0x78310000,
+	0x1e000000,
+	0x78320000,
+	0x1e000000,
+	0x78330000,
+	0x1e000000,
+	0x79190002,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x791a0002,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x791b0002,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x79120000,
+	0x00000000,
+	0x79130000,
+	0x00000000,
+	0x79140000,
+	0x00000000,
+	0x79150000,
+	0x00000000,
+	0x79160000,
+	0x00000000,
+	0x78150009,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78190009,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x781a0009,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78160009,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78170009,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78490001,
+	0x00000000,
+	0x00000000,
+	0x784a0000,
+	0x00000000,
+	0x784b0000,
+	0x00000004,
+	0x79170101,
+	0x00000000,
+	0x00000080,
+	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,
+	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,
+	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,
+	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,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x79180006,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x79180006,
+	0x20000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x79180006,
+	0x40000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x79180006,
+	0x60000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x61010011,
+	0x00000001,	 /* reloc */
+	0x00000000,
+	0x00000000,
+	0x00000001,	 /* reloc */
+	0x00000000,
+	0x00000001,	 /* reloc */
+	0x00000000,
+	0x00000001,
+	0x00000000,
+	0x00000001,	 /* reloc */
+	0x00000000,
+	0x00001001,
+	0x00001001,
+	0x00000001,
+	0x00001001,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x61020001,
+	0x00000000,
+	0x00000000,
+	0x79000002,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78050006,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x79040002,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x79040002,
+	0x40000000,
+	0x00000000,
+	0x00000000,
+	0x79040002,
+	0x80000000,
+	0x00000000,
+	0x00000000,
+	0x79040002,
+	0xc0000000,
+	0x00000000,
+	0x00000000,
+	0x79080001,
+	0x00000000,
+	0x00000000,
+	0x790a0001,
+	0x00000000,
+	0x00000000,
+	0x78060003,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78070003,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78040001,
+	0x00000000,
+	0x00000000,
+	0x79110000,
+	0x00000000,
+	0x780d0000,
+	0x00000000,
+	0x79060000,
+	0x00000000,
+	0x7907001f,
+	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,
+	0x7902000f,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x790c000f,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x780a0003,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78080083,
+	0x00004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x04004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x08004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x0c004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x10004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x14004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x18004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x1c004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x20004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x24004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x28004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x2c004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x30004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x34004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x38004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x3c004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x40004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x44004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x48004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x4c004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x50004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x54004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x58004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x5c004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x60004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x64004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x68004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x6c004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x70004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x74004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x7c004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x80004000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78090043,
+	0x02000000,
+	0x22220000,
+	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,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x78550003,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x680b0001,
+	0x780e0000,
+	0x00000e01,
+	0x78240000,
+	0x00000e41,
+	0x784f0000,
+	0x80000100,
+	0x784d0000,
+	0x40000000,
+	0x782b0000,
+	0x00000000,
+	0x782c0000,
+	0x00000000,
+	0x782d0000,
+	0x00000000,
+	0x782e0000,
+	0x00000000,
+	0x782f0000,
+	0x00000000,
+	0x780f0000,
+	0x00000000,
+	0x78230000,
+	0x00000ea0,
+	0x78210000,
+	0x00000ec0,
+	0x78260000,
+	0x00000000,
+	0x78270000,
+	0x00000000,
+	0x78280000,
+	0x00000000,
+	0x78290000,
+	0x00000000,
+	0x782a0000,
+	0x00000000,
+	0x7b000005,
+	0x00000004,
+	0x00000001,
+	0x00000000,
+	0x00000001,
+	0x00000000,
+	0x00000000,
+	0x05000000,	 /* cmds end */
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,	 /* state start */
+	0x00000000,
+	0x3f800000,
+	0x3f800000,
+	0x3f800000,
+	0x3f800000,
+	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,	 /* state end */
+};
+
+RO_RENDERSTATE(9);
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.c b/drivers/gpu/drm/i915/intel_ringbuffer.c
index 0a80e419b589..c7bc93d28d84 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.c
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.c
@@ -362,12 +362,15 @@ gen7_render_ring_flush(struct intel_engine_cs *ring,
 		flags |= PIPE_CONTROL_VF_CACHE_INVALIDATE;
 		flags |= PIPE_CONTROL_CONST_CACHE_INVALIDATE;
 		flags |= PIPE_CONTROL_STATE_CACHE_INVALIDATE;
+		flags |= PIPE_CONTROL_MEDIA_STATE_CLEAR;
 		/*
 		 * TLB invalidate requires a post-sync write.
 		 */
 		flags |= PIPE_CONTROL_QW_WRITE;
 		flags |= PIPE_CONTROL_GLOBAL_GTT_IVB;
 
+		flags |= PIPE_CONTROL_STALL_AT_SCOREBOARD;
+
 		/* Workaround: we must issue a pipe_control with CS-stall bit
 		 * set before a pipe_control command that has the state cache
 		 * invalidate bit set. */
@@ -589,14 +592,10 @@ static int init_ring_common(struct intel_engine_cs *ring)
 		goto out;
 	}
 
-	if (!drm_core_check_feature(ring->dev, DRIVER_MODESET))
-		i915_kernel_lost_context(ring->dev);
-	else {
-		ringbuf->head = I915_READ_HEAD(ring);
-		ringbuf->tail = I915_READ_TAIL(ring) & TAIL_ADDR;
-		ringbuf->space = intel_ring_space(ringbuf);
-		ringbuf->last_retired_head = -1;
-	}
+	ringbuf->head = I915_READ_HEAD(ring);
+	ringbuf->tail = I915_READ_TAIL(ring) & TAIL_ADDR;
+	ringbuf->space = intel_ring_space(ringbuf);
+	ringbuf->last_retired_head = -1;
 
 	memset(&ring->hangcheck, 0, sizeof(ring->hangcheck));
 
@@ -665,76 +664,112 @@ err:
 	return ret;
 }
 
-static inline void intel_ring_emit_wa(struct intel_engine_cs *ring,
-				       u32 addr, u32 value)
+static int intel_ring_workarounds_emit(struct intel_engine_cs *ring,
+				       struct intel_context *ctx)
 {
+	int ret, i;
 	struct drm_device *dev = ring->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct i915_workarounds *w = &dev_priv->workarounds;
 
-	if (WARN_ON(dev_priv->num_wa_regs >= I915_MAX_WA_REGS))
-		return;
+	if (WARN_ON(w->count == 0))
+		return 0;
 
-	intel_ring_emit(ring, MI_LOAD_REGISTER_IMM(1));
-	intel_ring_emit(ring, addr);
-	intel_ring_emit(ring, value);
+	ring->gpu_caches_dirty = true;
+	ret = intel_ring_flush_all_caches(ring);
+	if (ret)
+		return ret;
 
-	dev_priv->intel_wa_regs[dev_priv->num_wa_regs].addr = addr;
-	dev_priv->intel_wa_regs[dev_priv->num_wa_regs].mask = value & 0xFFFF;
-	/* value is updated with the status of remaining bits of this
-	 * register when it is read from debugfs file
-	 */
-	dev_priv->intel_wa_regs[dev_priv->num_wa_regs].value = value;
-	dev_priv->num_wa_regs++;
+	ret = intel_ring_begin(ring, (w->count * 2 + 2));
+	if (ret)
+		return ret;
 
-	return;
+	intel_ring_emit(ring, MI_LOAD_REGISTER_IMM(w->count));
+	for (i = 0; i < w->count; i++) {
+		intel_ring_emit(ring, w->reg[i].addr);
+		intel_ring_emit(ring, w->reg[i].value);
+	}
+	intel_ring_emit(ring, MI_NOOP);
+
+	intel_ring_advance(ring);
+
+	ring->gpu_caches_dirty = true;
+	ret = intel_ring_flush_all_caches(ring);
+	if (ret)
+		return ret;
+
+	DRM_DEBUG_DRIVER("Number of Workarounds emitted: %d\n", w->count);
+
+	return 0;
 }
 
+static int wa_add(struct drm_i915_private *dev_priv,
+		  const u32 addr, const u32 mask, const u32 val)
+{
+	const u32 idx = dev_priv->workarounds.count;
+
+	if (WARN_ON(idx >= I915_MAX_WA_REGS))
+		return -ENOSPC;
+
+	dev_priv->workarounds.reg[idx].addr = addr;
+	dev_priv->workarounds.reg[idx].value = val;
+	dev_priv->workarounds.reg[idx].mask = mask;
+
+	dev_priv->workarounds.count++;
+
+	return 0;
+}
+
+#define WA_REG(addr, mask, val) { \
+		const int r = wa_add(dev_priv, (addr), (mask), (val)); \
+		if (r) \
+			return r; \
+	}
+
+#define WA_SET_BIT_MASKED(addr, mask) \
+	WA_REG(addr, (mask), _MASKED_BIT_ENABLE(mask))
+
+#define WA_CLR_BIT_MASKED(addr, mask) \
+	WA_REG(addr, (mask), _MASKED_BIT_DISABLE(mask))
+
+#define WA_SET_FIELD_MASKED(addr, mask, value) \
+	WA_REG(addr, mask, _MASKED_FIELD(mask, value))
+
+#define WA_SET_BIT(addr, mask) WA_REG(addr, mask, I915_READ(addr) | (mask))
+#define WA_CLR_BIT(addr, mask) WA_REG(addr, mask, I915_READ(addr) & ~(mask))
+
+#define WA_WRITE(addr, val) WA_REG(addr, 0xffffffff, val)
+
 static int bdw_init_workarounds(struct intel_engine_cs *ring)
 {
-	int ret;
 	struct drm_device *dev = ring->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	/*
-	 * workarounds applied in this fn are part of register state context,
-	 * they need to be re-initialized followed by gpu reset, suspend/resume,
-	 * module reload.
-	 */
-	dev_priv->num_wa_regs = 0;
-	memset(dev_priv->intel_wa_regs, 0, sizeof(dev_priv->intel_wa_regs));
-
-	/*
-	 * update the number of dwords required based on the
-	 * actual number of workarounds applied
-	 */
-	ret = intel_ring_begin(ring, 18);
-	if (ret)
-		return ret;
-
 	/* WaDisablePartialInstShootdown:bdw */
-	/* WaDisableThreadStallDopClockGating:bdw */
-	/* FIXME: Unclear whether we really need this on production bdw. */
-	intel_ring_emit_wa(ring, GEN8_ROW_CHICKEN,
-			   _MASKED_BIT_ENABLE(PARTIAL_INSTRUCTION_SHOOTDOWN_DISABLE
-					     | STALL_DOP_GATING_DISABLE));
+	/* WaDisableThreadStallDopClockGating:bdw (pre-production) */
+	WA_SET_BIT_MASKED(GEN8_ROW_CHICKEN,
+			  PARTIAL_INSTRUCTION_SHOOTDOWN_DISABLE |
+			  STALL_DOP_GATING_DISABLE);
 
-	/* WaDisableDopClockGating:bdw May not be needed for production */
-	intel_ring_emit_wa(ring, GEN7_ROW_CHICKEN2,
-			   _MASKED_BIT_ENABLE(DOP_CLOCK_GATING_DISABLE));
+	/* WaDisableDopClockGating:bdw */
+	WA_SET_BIT_MASKED(GEN7_ROW_CHICKEN2,
+			  DOP_CLOCK_GATING_DISABLE);
 
-	intel_ring_emit_wa(ring, HALF_SLICE_CHICKEN3,
-			   _MASKED_BIT_ENABLE(GEN8_SAMPLER_POWER_BYPASS_DIS));
+	WA_SET_BIT_MASKED(HALF_SLICE_CHICKEN3,
+			  GEN8_SAMPLER_POWER_BYPASS_DIS);
 
 	/* Use Force Non-Coherent whenever executing a 3D context. This is a
 	 * workaround for for a possible hang in the unlikely event a TLB
 	 * invalidation occurs during a PSD flush.
 	 */
-	intel_ring_emit_wa(ring, HDC_CHICKEN0,
-			   _MASKED_BIT_ENABLE(HDC_FORCE_NON_COHERENT));
+	/* WaDisableFenceDestinationToSLM:bdw (GT3 pre-production) */
+	WA_SET_BIT_MASKED(HDC_CHICKEN0,
+			  HDC_FORCE_NON_COHERENT |
+			  (IS_BDW_GT3(dev) ? HDC_FENCE_DEST_SLM_DISABLE : 0));
 
 	/* Wa4x4STCOptimizationDisable:bdw */
-	intel_ring_emit_wa(ring, CACHE_MODE_1,
-			   _MASKED_BIT_ENABLE(GEN8_4x4_STC_OPTIMIZATION_DISABLE));
+	WA_SET_BIT_MASKED(CACHE_MODE_1,
+			  GEN8_4x4_STC_OPTIMIZATION_DISABLE);
 
 	/*
 	 * BSpec recommends 8x4 when MSAA is used,
@@ -744,52 +779,51 @@ static int bdw_init_workarounds(struct intel_engine_cs *ring)
 	 * disable bit, which we don't touch here, but it's good
 	 * to keep in mind (see 3DSTATE_PS and 3DSTATE_WM).
 	 */
-	intel_ring_emit_wa(ring, GEN7_GT_MODE,
-			   GEN6_WIZ_HASHING_MASK | GEN6_WIZ_HASHING_16x4);
-
-	intel_ring_advance(ring);
-
-	DRM_DEBUG_DRIVER("Number of Workarounds applied: %d\n",
-			 dev_priv->num_wa_regs);
+	WA_SET_FIELD_MASKED(GEN7_GT_MODE,
+			    GEN6_WIZ_HASHING_MASK,
+			    GEN6_WIZ_HASHING_16x4);
 
 	return 0;
 }
 
 static int chv_init_workarounds(struct intel_engine_cs *ring)
 {
-	int ret;
 	struct drm_device *dev = ring->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	/*
-	 * workarounds applied in this fn are part of register state context,
-	 * they need to be re-initialized followed by gpu reset, suspend/resume,
-	 * module reload.
+	/* WaDisablePartialInstShootdown:chv */
+	/* WaDisableThreadStallDopClockGating:chv */
+	WA_SET_BIT_MASKED(GEN8_ROW_CHICKEN,
+			  PARTIAL_INSTRUCTION_SHOOTDOWN_DISABLE |
+			  STALL_DOP_GATING_DISABLE);
+
+	/* Use Force Non-Coherent whenever executing a 3D context. This is a
+	 * workaround for a possible hang in the unlikely event a TLB
+	 * invalidation occurs during a PSD flush.
 	 */
-	dev_priv->num_wa_regs = 0;
-	memset(dev_priv->intel_wa_regs, 0, sizeof(dev_priv->intel_wa_regs));
+	/* WaForceEnableNonCoherent:chv */
+	/* WaHdcDisableFetchWhenMasked:chv */
+	WA_SET_BIT_MASKED(HDC_CHICKEN0,
+			  HDC_FORCE_NON_COHERENT |
+			  HDC_DONOT_FETCH_MEM_WHEN_MASKED);
 
-	ret = intel_ring_begin(ring, 12);
-	if (ret)
-		return ret;
+	return 0;
+}
 
-	/* WaDisablePartialInstShootdown:chv */
-	intel_ring_emit_wa(ring, GEN8_ROW_CHICKEN,
-			   _MASKED_BIT_ENABLE(PARTIAL_INSTRUCTION_SHOOTDOWN_DISABLE));
+int init_workarounds_ring(struct intel_engine_cs *ring)
+{
+	struct drm_device *dev = ring->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	/* WaDisableThreadStallDopClockGating:chv */
-	intel_ring_emit_wa(ring, GEN8_ROW_CHICKEN,
-			   _MASKED_BIT_ENABLE(STALL_DOP_GATING_DISABLE));
+	WARN_ON(ring->id != RCS);
 
-	/* WaDisableDopClockGating:chv (pre-production hw) */
-	intel_ring_emit_wa(ring, GEN7_ROW_CHICKEN2,
-			   _MASKED_BIT_ENABLE(DOP_CLOCK_GATING_DISABLE));
+	dev_priv->workarounds.count = 0;
 
-	/* WaDisableSamplerPowerBypass:chv (pre-production hw) */
-	intel_ring_emit_wa(ring, HALF_SLICE_CHICKEN3,
-			   _MASKED_BIT_ENABLE(GEN8_SAMPLER_POWER_BYPASS_DIS));
+	if (IS_BROADWELL(dev))
+		return bdw_init_workarounds(ring);
 
-	intel_ring_advance(ring);
+	if (IS_CHERRYVIEW(dev))
+		return chv_init_workarounds(ring);
 
 	return 0;
 }
@@ -812,7 +846,7 @@ static int init_render_ring(struct intel_engine_cs *ring)
 	 *
 	 * WaDisableAsyncFlipPerfMode:snb,ivb,hsw,vlv,bdw,chv
 	 */
-	if (INTEL_INFO(dev)->gen >= 6)
+	if (INTEL_INFO(dev)->gen >= 6 && INTEL_INFO(dev)->gen < 9)
 		I915_WRITE(MI_MODE, _MASKED_BIT_ENABLE(ASYNC_FLIP_PERF_DISABLE));
 
 	/* Required for the hardware to program scanline values for waiting */
@@ -849,7 +883,7 @@ static int init_render_ring(struct intel_engine_cs *ring)
 	if (HAS_L3_DPF(dev))
 		I915_WRITE_IMR(ring, ~GT_PARITY_ERROR(dev));
 
-	return ret;
+	return init_workarounds_ring(ring);
 }
 
 static void render_ring_cleanup(struct intel_engine_cs *ring)
@@ -1186,7 +1220,7 @@ gen5_ring_get_irq(struct intel_engine_cs *ring)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	unsigned long flags;
 
-	if (!dev->irq_enabled)
+	if (WARN_ON(!intel_irqs_enabled(dev_priv)))
 		return false;
 
 	spin_lock_irqsave(&dev_priv->irq_lock, flags);
@@ -1217,7 +1251,7 @@ i9xx_ring_get_irq(struct intel_engine_cs *ring)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	unsigned long flags;
 
-	if (!dev->irq_enabled)
+	if (!intel_irqs_enabled(dev_priv))
 		return false;
 
 	spin_lock_irqsave(&dev_priv->irq_lock, flags);
@@ -1254,7 +1288,7 @@ i8xx_ring_get_irq(struct intel_engine_cs *ring)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	unsigned long flags;
 
-	if (!dev->irq_enabled)
+	if (!intel_irqs_enabled(dev_priv))
 		return false;
 
 	spin_lock_irqsave(&dev_priv->irq_lock, flags);
@@ -1388,8 +1422,8 @@ gen6_ring_get_irq(struct intel_engine_cs *ring)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	unsigned long flags;
 
-	if (!dev->irq_enabled)
-	       return false;
+	if (WARN_ON(!intel_irqs_enabled(dev_priv)))
+		return false;
 
 	spin_lock_irqsave(&dev_priv->irq_lock, flags);
 	if (ring->irq_refcount++ == 0) {
@@ -1431,7 +1465,7 @@ hsw_vebox_get_irq(struct intel_engine_cs *ring)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	unsigned long flags;
 
-	if (!dev->irq_enabled)
+	if (WARN_ON(!intel_irqs_enabled(dev_priv)))
 		return false;
 
 	spin_lock_irqsave(&dev_priv->irq_lock, flags);
@@ -1451,9 +1485,6 @@ hsw_vebox_put_irq(struct intel_engine_cs *ring)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	unsigned long flags;
 
-	if (!dev->irq_enabled)
-		return;
-
 	spin_lock_irqsave(&dev_priv->irq_lock, flags);
 	if (--ring->irq_refcount == 0) {
 		I915_WRITE_IMR(ring, ~0);
@@ -1469,7 +1500,7 @@ gen8_ring_get_irq(struct intel_engine_cs *ring)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	unsigned long flags;
 
-	if (!dev->irq_enabled)
+	if (WARN_ON(!intel_irqs_enabled(dev_priv)))
 		return false;
 
 	spin_lock_irqsave(&dev_priv->irq_lock, flags);
@@ -1694,13 +1725,42 @@ static int init_phys_status_page(struct intel_engine_cs *ring)
 	return 0;
 }
 
-void intel_destroy_ringbuffer_obj(struct intel_ringbuffer *ringbuf)
+void intel_unpin_ringbuffer_obj(struct intel_ringbuffer *ringbuf)
 {
-	if (!ringbuf->obj)
-		return;
-
 	iounmap(ringbuf->virtual_start);
+	ringbuf->virtual_start = NULL;
 	i915_gem_object_ggtt_unpin(ringbuf->obj);
+}
+
+int intel_pin_and_map_ringbuffer_obj(struct drm_device *dev,
+				     struct intel_ringbuffer *ringbuf)
+{
+	struct drm_i915_private *dev_priv = to_i915(dev);
+	struct drm_i915_gem_object *obj = ringbuf->obj;
+	int ret;
+
+	ret = i915_gem_obj_ggtt_pin(obj, PAGE_SIZE, PIN_MAPPABLE);
+	if (ret)
+		return ret;
+
+	ret = i915_gem_object_set_to_gtt_domain(obj, true);
+	if (ret) {
+		i915_gem_object_ggtt_unpin(obj);
+		return ret;
+	}
+
+	ringbuf->virtual_start = ioremap_wc(dev_priv->gtt.mappable_base +
+			i915_gem_obj_ggtt_offset(obj), ringbuf->size);
+	if (ringbuf->virtual_start == NULL) {
+		i915_gem_object_ggtt_unpin(obj);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+void intel_destroy_ringbuffer_obj(struct intel_ringbuffer *ringbuf)
+{
 	drm_gem_object_unreference(&ringbuf->obj->base);
 	ringbuf->obj = NULL;
 }
@@ -1708,12 +1768,7 @@ void intel_destroy_ringbuffer_obj(struct intel_ringbuffer *ringbuf)
 int intel_alloc_ringbuffer_obj(struct drm_device *dev,
 			       struct intel_ringbuffer *ringbuf)
 {
-	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct drm_i915_gem_object *obj;
-	int ret;
-
-	if (ringbuf->obj)
-		return 0;
 
 	obj = NULL;
 	if (!HAS_LLC(dev))
@@ -1726,30 +1781,9 @@ int intel_alloc_ringbuffer_obj(struct drm_device *dev,
 	/* mark ring buffers as read-only from GPU side by default */
 	obj->gt_ro = 1;
 
-	ret = i915_gem_obj_ggtt_pin(obj, PAGE_SIZE, PIN_MAPPABLE);
-	if (ret)
-		goto err_unref;
-
-	ret = i915_gem_object_set_to_gtt_domain(obj, true);
-	if (ret)
-		goto err_unpin;
-
-	ringbuf->virtual_start =
-		ioremap_wc(dev_priv->gtt.mappable_base + i915_gem_obj_ggtt_offset(obj),
-				ringbuf->size);
-	if (ringbuf->virtual_start == NULL) {
-		ret = -EINVAL;
-		goto err_unpin;
-	}
-
 	ringbuf->obj = obj;
-	return 0;
 
-err_unpin:
-	i915_gem_object_ggtt_unpin(obj);
-err_unref:
-	drm_gem_object_unreference(&obj->base);
-	return ret;
+	return 0;
 }
 
 static int intel_init_ring_buffer(struct drm_device *dev,
@@ -1786,10 +1820,21 @@ static int intel_init_ring_buffer(struct drm_device *dev,
 			goto error;
 	}
 
-	ret = intel_alloc_ringbuffer_obj(dev, ringbuf);
-	if (ret) {
-		DRM_ERROR("Failed to allocate ringbuffer %s: %d\n", ring->name, ret);
-		goto error;
+	if (ringbuf->obj == NULL) {
+		ret = intel_alloc_ringbuffer_obj(dev, ringbuf);
+		if (ret) {
+			DRM_ERROR("Failed to allocate ringbuffer %s: %d\n",
+					ring->name, ret);
+			goto error;
+		}
+
+		ret = intel_pin_and_map_ringbuffer_obj(dev, ringbuf);
+		if (ret) {
+			DRM_ERROR("Failed to pin and map ringbuffer %s: %d\n",
+					ring->name, ret);
+			intel_destroy_ringbuffer_obj(ringbuf);
+			goto error;
+		}
 	}
 
 	/* Workaround an erratum on the i830 which causes a hang if
@@ -1818,15 +1863,19 @@ error:
 
 void intel_cleanup_ring_buffer(struct intel_engine_cs *ring)
 {
-	struct drm_i915_private *dev_priv = to_i915(ring->dev);
-	struct intel_ringbuffer *ringbuf = ring->buffer;
+	struct drm_i915_private *dev_priv;
+	struct intel_ringbuffer *ringbuf;
 
 	if (!intel_ring_initialized(ring))
 		return;
 
+	dev_priv = to_i915(ring->dev);
+	ringbuf = ring->buffer;
+
 	intel_stop_ring_buffer(ring);
 	WARN_ON(!IS_GEN2(ring->dev) && (I915_READ_MODE(ring) & MODE_IDLE) == 0);
 
+	intel_unpin_ringbuffer_obj(ringbuf);
 	intel_destroy_ringbuffer_obj(ringbuf);
 	ring->preallocated_lazy_request = NULL;
 	ring->outstanding_lazy_seqno = 0;
@@ -1912,13 +1961,6 @@ static int ring_wait_for_space(struct intel_engine_cs *ring, int n)
 			break;
 		}
 
-		if (!drm_core_check_feature(dev, DRIVER_MODESET) &&
-		    dev->primary->master) {
-			struct drm_i915_master_private *master_priv = dev->primary->master->driver_priv;
-			if (master_priv->sarea_priv)
-				master_priv->sarea_priv->perf_boxes |= I915_BOX_WAIT;
-		}
-
 		msleep(1);
 
 		if (dev_priv->mm.interruptible && signal_pending(current)) {
@@ -2229,6 +2271,7 @@ static int gen6_ring_flush(struct intel_engine_cs *ring,
 			   u32 invalidate, u32 flush)
 {
 	struct drm_device *dev = ring->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
 	uint32_t cmd;
 	int ret;
 
@@ -2259,8 +2302,12 @@ static int gen6_ring_flush(struct intel_engine_cs *ring,
 	}
 	intel_ring_advance(ring);
 
-	if (IS_GEN7(dev) && !invalidate && flush)
-		return gen7_ring_fbc_flush(ring, FBC_REND_CACHE_CLEAN);
+	if (!invalidate && flush) {
+		if (IS_GEN7(dev))
+			return gen7_ring_fbc_flush(ring, FBC_REND_CACHE_CLEAN);
+		else if (IS_BROADWELL(dev))
+			dev_priv->fbc.need_sw_cache_clean = true;
+	}
 
 	return 0;
 }
@@ -2293,10 +2340,8 @@ int intel_init_render_ring_buffer(struct drm_device *dev)
 					dev_priv->semaphore_obj = obj;
 			}
 		}
-		if (IS_CHERRYVIEW(dev))
-			ring->init_context = chv_init_workarounds;
-		else
-			ring->init_context = bdw_init_workarounds;
+
+		ring->init_context = intel_ring_workarounds_emit;
 		ring->add_request = gen6_add_request;
 		ring->flush = gen8_render_ring_flush;
 		ring->irq_get = gen8_ring_get_irq;
@@ -2406,91 +2451,6 @@ int intel_init_render_ring_buffer(struct drm_device *dev)
 	return intel_init_ring_buffer(dev, ring);
 }
 
-int intel_render_ring_init_dri(struct drm_device *dev, u64 start, u32 size)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_engine_cs *ring = &dev_priv->ring[RCS];
-	struct intel_ringbuffer *ringbuf = ring->buffer;
-	int ret;
-
-	if (ringbuf == NULL) {
-		ringbuf = kzalloc(sizeof(*ringbuf), GFP_KERNEL);
-		if (!ringbuf)
-			return -ENOMEM;
-		ring->buffer = ringbuf;
-	}
-
-	ring->name = "render ring";
-	ring->id = RCS;
-	ring->mmio_base = RENDER_RING_BASE;
-
-	if (INTEL_INFO(dev)->gen >= 6) {
-		/* non-kms not supported on gen6+ */
-		ret = -ENODEV;
-		goto err_ringbuf;
-	}
-
-	/* 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;
-	ring->set_seqno = ring_set_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;
-
-	ring->dev = dev;
-	INIT_LIST_HEAD(&ring->active_list);
-	INIT_LIST_HEAD(&ring->request_list);
-
-	ringbuf->size = size;
-	ringbuf->effective_size = ringbuf->size;
-	if (IS_I830(ring->dev) || IS_845G(ring->dev))
-		ringbuf->effective_size -= 2 * CACHELINE_BYTES;
-
-	ringbuf->virtual_start = ioremap_wc(start, size);
-	if (ringbuf->virtual_start == NULL) {
-		DRM_ERROR("can not ioremap virtual address for"
-			  " ring buffer\n");
-		ret = -ENOMEM;
-		goto err_ringbuf;
-	}
-
-	if (!I915_NEED_GFX_HWS(dev)) {
-		ret = init_phys_status_page(ring);
-		if (ret)
-			goto err_vstart;
-	}
-
-	return 0;
-
-err_vstart:
-	iounmap(ringbuf->virtual_start);
-err_ringbuf:
-	kfree(ringbuf);
-	ring->buffer = NULL;
-	return ret;
-}
-
 int intel_init_bsd_ring_buffer(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.h b/drivers/gpu/drm/i915/intel_ringbuffer.h
index 96479c89f4bd..fe426cff598b 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.h
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.h
@@ -148,7 +148,8 @@ struct  intel_engine_cs {
 
 	int		(*init)(struct intel_engine_cs *ring);
 
-	int		(*init_context)(struct intel_engine_cs *ring);
+	int		(*init_context)(struct intel_engine_cs *ring,
+					struct intel_context *ctx);
 
 	void		(*write_tail)(struct intel_engine_cs *ring,
 				      u32 value);
@@ -235,6 +236,7 @@ struct  intel_engine_cs {
 	/* Execlists */
 	spinlock_t execlist_lock;
 	struct list_head execlist_queue;
+	struct list_head execlist_retired_req_list;
 	u8 next_context_status_buffer;
 	u32             irq_keep_mask; /* bitmask for interrupts that should not be masked */
 	int		(*emit_request)(struct intel_ringbuffer *ringbuf);
@@ -381,6 +383,9 @@ intel_write_status_page(struct intel_engine_cs *ring,
 #define I915_GEM_HWS_SCRATCH_INDEX	0x30
 #define I915_GEM_HWS_SCRATCH_ADDR (I915_GEM_HWS_SCRATCH_INDEX << MI_STORE_DWORD_INDEX_SHIFT)
 
+void intel_unpin_ringbuffer_obj(struct intel_ringbuffer *ringbuf);
+int intel_pin_and_map_ringbuffer_obj(struct drm_device *dev,
+				     struct intel_ringbuffer *ringbuf);
 void intel_destroy_ringbuffer_obj(struct intel_ringbuffer *ringbuf);
 int intel_alloc_ringbuffer_obj(struct drm_device *dev,
 			       struct intel_ringbuffer *ringbuf);
@@ -424,6 +429,8 @@ int intel_init_vebox_ring_buffer(struct drm_device *dev);
 u64 intel_ring_get_active_head(struct intel_engine_cs *ring);
 void intel_ring_setup_status_page(struct intel_engine_cs *ring);
 
+int init_workarounds_ring(struct intel_engine_cs *ring);
+
 static inline u32 intel_ring_get_tail(struct intel_ringbuffer *ringbuf)
 {
 	return ringbuf->tail;
@@ -441,7 +448,4 @@ static inline void i915_trace_irq_get(struct intel_engine_cs *ring, u32 seqno)
 		ring->trace_irq_seqno = seqno;
 }
 
-/* DRI warts */
-int intel_render_ring_init_dri(struct drm_device *dev, u64 start, u32 size);
-
 #endif /* _INTEL_RINGBUFFER_H_ */
diff --git a/drivers/gpu/drm/i915/intel_runtime_pm.c b/drivers/gpu/drm/i915/intel_runtime_pm.c
new file mode 100644
index 000000000000..f5a78d53e297
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_runtime_pm.c
@@ -0,0 +1,1406 @@
+/*
+ * Copyright © 2012-2014 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>
+ *    Daniel Vetter <daniel.vetter@ffwll.ch>
+ *
+ */
+
+#include <linux/pm_runtime.h>
+#include <linux/vgaarb.h>
+
+#include "i915_drv.h"
+#include "intel_drv.h"
+#include <drm/i915_powerwell.h>
+
+/**
+ * DOC: runtime pm
+ *
+ * The i915 driver supports dynamic enabling and disabling of entire hardware
+ * blocks at runtime. This is especially important on the display side where
+ * software is supposed to control many power gates manually on recent hardware,
+ * since on the GT side a lot of the power management is done by the hardware.
+ * But even there some manual control at the device level is required.
+ *
+ * Since i915 supports a diverse set of platforms with a unified codebase and
+ * hardware engineers just love to shuffle functionality around between power
+ * domains there's a sizeable amount of indirection required. This file provides
+ * generic functions to the driver for grabbing and releasing references for
+ * abstract power domains. It then maps those to the actual power wells
+ * present for a given platform.
+ */
+
+static struct i915_power_domains *hsw_pwr;
+
+#define for_each_power_well(i, power_well, domain_mask, power_domains)	\
+	for (i = 0;							\
+	     i < (power_domains)->power_well_count &&			\
+		 ((power_well) = &(power_domains)->power_wells[i]);	\
+	     i++)							\
+		if ((power_well)->domains & (domain_mask))
+
+#define for_each_power_well_rev(i, power_well, domain_mask, power_domains) \
+	for (i = (power_domains)->power_well_count - 1;			 \
+	     i >= 0 && ((power_well) = &(power_domains)->power_wells[i]);\
+	     i--)							 \
+		if ((power_well)->domains & (domain_mask))
+
+/*
+ * We should only use the power well if we explicitly asked the hardware to
+ * enable it, so check if it's enabled and also check if we've requested it to
+ * be enabled.
+ */
+static bool hsw_power_well_enabled(struct drm_i915_private *dev_priv,
+				   struct i915_power_well *power_well)
+{
+	return I915_READ(HSW_PWR_WELL_DRIVER) ==
+		     (HSW_PWR_WELL_ENABLE_REQUEST | HSW_PWR_WELL_STATE_ENABLED);
+}
+
+/**
+ * __intel_display_power_is_enabled - unlocked check for a power domain
+ * @dev_priv: i915 device instance
+ * @domain: power domain to check
+ *
+ * This is the unlocked version of intel_display_power_is_enabled() and should
+ * only be used from error capture and recovery code where deadlocks are
+ * possible.
+ *
+ * Returns:
+ * True when the power domain is enabled, false otherwise.
+ */
+bool __intel_display_power_is_enabled(struct drm_i915_private *dev_priv,
+				      enum intel_display_power_domain domain)
+{
+	struct i915_power_domains *power_domains;
+	struct i915_power_well *power_well;
+	bool is_enabled;
+	int i;
+
+	if (dev_priv->pm.suspended)
+		return false;
+
+	power_domains = &dev_priv->power_domains;
+
+	is_enabled = true;
+
+	for_each_power_well_rev(i, power_well, BIT(domain), power_domains) {
+		if (power_well->always_on)
+			continue;
+
+		if (!power_well->hw_enabled) {
+			is_enabled = false;
+			break;
+		}
+	}
+
+	return is_enabled;
+}
+
+/**
+ * intel_display_power_is_enabled - unlocked check for a power domain
+ * @dev_priv: i915 device instance
+ * @domain: power domain to check
+ *
+ * This function can be used to check the hw power domain state. It is mostly
+ * used in hardware state readout functions. Everywhere else code should rely
+ * upon explicit power domain reference counting to ensure that the hardware
+ * block is powered up before accessing it.
+ *
+ * Callers must hold the relevant modesetting locks to ensure that concurrent
+ * threads can't disable the power well while the caller tries to read a few
+ * registers.
+ *
+ * Returns:
+ * True when the power domain is enabled, false otherwise.
+ */
+bool intel_display_power_is_enabled(struct drm_i915_private *dev_priv,
+				    enum intel_display_power_domain domain)
+{
+	struct i915_power_domains *power_domains;
+	bool ret;
+
+	power_domains = &dev_priv->power_domains;
+
+	mutex_lock(&power_domains->lock);
+	ret = __intel_display_power_is_enabled(dev_priv, domain);
+	mutex_unlock(&power_domains->lock);
+
+	return ret;
+}
+
+/**
+ * intel_display_set_init_power - set the initial power domain state
+ * @dev_priv: i915 device instance
+ * @enable: whether to enable or disable the initial power domain state
+ *
+ * For simplicity our driver load/unload and system suspend/resume code assumes
+ * that all power domains are always enabled. This functions controls the state
+ * of this little hack. While the initial power domain state is enabled runtime
+ * pm is effectively disabled.
+ */
+void intel_display_set_init_power(struct drm_i915_private *dev_priv,
+				  bool enable)
+{
+	if (dev_priv->power_domains.init_power_on == enable)
+		return;
+
+	if (enable)
+		intel_display_power_get(dev_priv, POWER_DOMAIN_INIT);
+	else
+		intel_display_power_put(dev_priv, POWER_DOMAIN_INIT);
+
+	dev_priv->power_domains.init_power_on = enable;
+}
+
+/*
+ * Starting with Haswell, we have a "Power Down Well" that can be turned off
+ * when not needed anymore. We have 4 registers that can request the power well
+ * to be enabled, and it will only be disabled if none of the registers is
+ * requesting it to be enabled.
+ */
+static void hsw_power_well_post_enable(struct drm_i915_private *dev_priv)
+{
+	struct drm_device *dev = dev_priv->dev;
+
+	/*
+	 * After we re-enable the power well, if we touch VGA register 0x3d5
+	 * we'll get unclaimed register interrupts. This stops after we write
+	 * anything to the VGA MSR register. The vgacon module uses this
+	 * register all the time, so if we unbind our driver and, as a
+	 * consequence, bind vgacon, we'll get stuck in an infinite loop at
+	 * console_unlock(). So make here we touch the VGA MSR register, making
+	 * sure vgacon can keep working normally without triggering interrupts
+	 * and error messages.
+	 */
+	vga_get_uninterruptible(dev->pdev, VGA_RSRC_LEGACY_IO);
+	outb(inb(VGA_MSR_READ), VGA_MSR_WRITE);
+	vga_put(dev->pdev, VGA_RSRC_LEGACY_IO);
+
+	if (IS_BROADWELL(dev) || (INTEL_INFO(dev)->gen >= 9))
+		gen8_irq_power_well_post_enable(dev_priv);
+}
+
+static void hsw_set_power_well(struct drm_i915_private *dev_priv,
+			       struct i915_power_well *power_well, bool enable)
+{
+	bool is_enabled, enable_requested;
+	uint32_t tmp;
+
+	tmp = I915_READ(HSW_PWR_WELL_DRIVER);
+	is_enabled = tmp & HSW_PWR_WELL_STATE_ENABLED;
+	enable_requested = tmp & HSW_PWR_WELL_ENABLE_REQUEST;
+
+	if (enable) {
+		if (!enable_requested)
+			I915_WRITE(HSW_PWR_WELL_DRIVER,
+				   HSW_PWR_WELL_ENABLE_REQUEST);
+
+		if (!is_enabled) {
+			DRM_DEBUG_KMS("Enabling power well\n");
+			if (wait_for((I915_READ(HSW_PWR_WELL_DRIVER) &
+				      HSW_PWR_WELL_STATE_ENABLED), 20))
+				DRM_ERROR("Timeout enabling power well\n");
+			hsw_power_well_post_enable(dev_priv);
+		}
+
+	} else {
+		if (enable_requested) {
+			I915_WRITE(HSW_PWR_WELL_DRIVER, 0);
+			POSTING_READ(HSW_PWR_WELL_DRIVER);
+			DRM_DEBUG_KMS("Requesting to disable the power well\n");
+		}
+	}
+}
+
+static void hsw_power_well_sync_hw(struct drm_i915_private *dev_priv,
+				   struct i915_power_well *power_well)
+{
+	hsw_set_power_well(dev_priv, power_well, power_well->count > 0);
+
+	/*
+	 * We're taking over the BIOS, so clear any requests made by it since
+	 * the driver is in charge now.
+	 */
+	if (I915_READ(HSW_PWR_WELL_BIOS) & HSW_PWR_WELL_ENABLE_REQUEST)
+		I915_WRITE(HSW_PWR_WELL_BIOS, 0);
+}
+
+static void hsw_power_well_enable(struct drm_i915_private *dev_priv,
+				  struct i915_power_well *power_well)
+{
+	hsw_set_power_well(dev_priv, power_well, true);
+}
+
+static void hsw_power_well_disable(struct drm_i915_private *dev_priv,
+				   struct i915_power_well *power_well)
+{
+	hsw_set_power_well(dev_priv, power_well, false);
+}
+
+static void i9xx_always_on_power_well_noop(struct drm_i915_private *dev_priv,
+					   struct i915_power_well *power_well)
+{
+}
+
+static bool i9xx_always_on_power_well_enabled(struct drm_i915_private *dev_priv,
+					     struct i915_power_well *power_well)
+{
+	return true;
+}
+
+static void vlv_set_power_well(struct drm_i915_private *dev_priv,
+			       struct i915_power_well *power_well, bool enable)
+{
+	enum punit_power_well power_well_id = power_well->data;
+	u32 mask;
+	u32 state;
+	u32 ctrl;
+
+	mask = PUNIT_PWRGT_MASK(power_well_id);
+	state = enable ? PUNIT_PWRGT_PWR_ON(power_well_id) :
+			 PUNIT_PWRGT_PWR_GATE(power_well_id);
+
+	mutex_lock(&dev_priv->rps.hw_lock);
+
+#define COND \
+	((vlv_punit_read(dev_priv, PUNIT_REG_PWRGT_STATUS) & mask) == state)
+
+	if (COND)
+		goto out;
+
+	ctrl = vlv_punit_read(dev_priv, PUNIT_REG_PWRGT_CTRL);
+	ctrl &= ~mask;
+	ctrl |= state;
+	vlv_punit_write(dev_priv, PUNIT_REG_PWRGT_CTRL, ctrl);
+
+	if (wait_for(COND, 100))
+		DRM_ERROR("timout setting power well state %08x (%08x)\n",
+			  state,
+			  vlv_punit_read(dev_priv, PUNIT_REG_PWRGT_CTRL));
+
+#undef COND
+
+out:
+	mutex_unlock(&dev_priv->rps.hw_lock);
+}
+
+static void vlv_power_well_sync_hw(struct drm_i915_private *dev_priv,
+				   struct i915_power_well *power_well)
+{
+	vlv_set_power_well(dev_priv, power_well, power_well->count > 0);
+}
+
+static void vlv_power_well_enable(struct drm_i915_private *dev_priv,
+				  struct i915_power_well *power_well)
+{
+	vlv_set_power_well(dev_priv, power_well, true);
+}
+
+static void vlv_power_well_disable(struct drm_i915_private *dev_priv,
+				   struct i915_power_well *power_well)
+{
+	vlv_set_power_well(dev_priv, power_well, false);
+}
+
+static bool vlv_power_well_enabled(struct drm_i915_private *dev_priv,
+				   struct i915_power_well *power_well)
+{
+	int power_well_id = power_well->data;
+	bool enabled = false;
+	u32 mask;
+	u32 state;
+	u32 ctrl;
+
+	mask = PUNIT_PWRGT_MASK(power_well_id);
+	ctrl = PUNIT_PWRGT_PWR_ON(power_well_id);
+
+	mutex_lock(&dev_priv->rps.hw_lock);
+
+	state = vlv_punit_read(dev_priv, PUNIT_REG_PWRGT_STATUS) & mask;
+	/*
+	 * We only ever set the power-on and power-gate states, anything
+	 * else is unexpected.
+	 */
+	WARN_ON(state != PUNIT_PWRGT_PWR_ON(power_well_id) &&
+		state != PUNIT_PWRGT_PWR_GATE(power_well_id));
+	if (state == ctrl)
+		enabled = true;
+
+	/*
+	 * A transient state at this point would mean some unexpected party
+	 * is poking at the power controls too.
+	 */
+	ctrl = vlv_punit_read(dev_priv, PUNIT_REG_PWRGT_CTRL) & mask;
+	WARN_ON(ctrl != state);
+
+	mutex_unlock(&dev_priv->rps.hw_lock);
+
+	return enabled;
+}
+
+static void vlv_display_power_well_enable(struct drm_i915_private *dev_priv,
+					  struct i915_power_well *power_well)
+{
+	WARN_ON_ONCE(power_well->data != PUNIT_POWER_WELL_DISP2D);
+
+	vlv_set_power_well(dev_priv, power_well, true);
+
+	spin_lock_irq(&dev_priv->irq_lock);
+	valleyview_enable_display_irqs(dev_priv);
+	spin_unlock_irq(&dev_priv->irq_lock);
+
+	/*
+	 * During driver initialization/resume we can avoid restoring the
+	 * part of the HW/SW state that will be inited anyway explicitly.
+	 */
+	if (dev_priv->power_domains.initializing)
+		return;
+
+	intel_hpd_init(dev_priv);
+
+	i915_redisable_vga_power_on(dev_priv->dev);
+}
+
+static void vlv_display_power_well_disable(struct drm_i915_private *dev_priv,
+					   struct i915_power_well *power_well)
+{
+	WARN_ON_ONCE(power_well->data != PUNIT_POWER_WELL_DISP2D);
+
+	spin_lock_irq(&dev_priv->irq_lock);
+	valleyview_disable_display_irqs(dev_priv);
+	spin_unlock_irq(&dev_priv->irq_lock);
+
+	vlv_set_power_well(dev_priv, power_well, false);
+
+	vlv_power_sequencer_reset(dev_priv);
+}
+
+static void vlv_dpio_cmn_power_well_enable(struct drm_i915_private *dev_priv,
+					   struct i915_power_well *power_well)
+{
+	WARN_ON_ONCE(power_well->data != PUNIT_POWER_WELL_DPIO_CMN_BC);
+
+	/*
+	 * Enable the CRI clock source so we can get at the
+	 * display and the reference clock for VGA
+	 * hotplug / manual detection.
+	 */
+	I915_WRITE(DPLL(PIPE_B), I915_READ(DPLL(PIPE_B)) |
+		   DPLL_REFA_CLK_ENABLE_VLV | DPLL_INTEGRATED_CRI_CLK_VLV);
+	udelay(1); /* >10ns for cmnreset, >0ns for sidereset */
+
+	vlv_set_power_well(dev_priv, power_well, true);
+
+	/*
+	 * From VLV2A0_DP_eDP_DPIO_driver_vbios_notes_10.docx -
+	 *  6.	De-assert cmn_reset/side_reset. Same as VLV X0.
+	 *   a.	GUnit 0x2110 bit[0] set to 1 (def 0)
+	 *   b.	The other bits such as sfr settings / modesel may all
+	 *	be set to 0.
+	 *
+	 * This should only be done on init and resume from S3 with
+	 * both PLLs disabled, or we risk losing DPIO and PLL
+	 * synchronization.
+	 */
+	I915_WRITE(DPIO_CTL, I915_READ(DPIO_CTL) | DPIO_CMNRST);
+}
+
+static void vlv_dpio_cmn_power_well_disable(struct drm_i915_private *dev_priv,
+					    struct i915_power_well *power_well)
+{
+	enum pipe pipe;
+
+	WARN_ON_ONCE(power_well->data != PUNIT_POWER_WELL_DPIO_CMN_BC);
+
+	for_each_pipe(dev_priv, pipe)
+		assert_pll_disabled(dev_priv, pipe);
+
+	/* Assert common reset */
+	I915_WRITE(DPIO_CTL, I915_READ(DPIO_CTL) & ~DPIO_CMNRST);
+
+	vlv_set_power_well(dev_priv, power_well, false);
+}
+
+static void chv_dpio_cmn_power_well_enable(struct drm_i915_private *dev_priv,
+					   struct i915_power_well *power_well)
+{
+	enum dpio_phy phy;
+
+	WARN_ON_ONCE(power_well->data != PUNIT_POWER_WELL_DPIO_CMN_BC &&
+		     power_well->data != PUNIT_POWER_WELL_DPIO_CMN_D);
+
+	/*
+	 * Enable the CRI clock source so we can get at the
+	 * display and the reference clock for VGA
+	 * hotplug / manual detection.
+	 */
+	if (power_well->data == PUNIT_POWER_WELL_DPIO_CMN_BC) {
+		phy = DPIO_PHY0;
+		I915_WRITE(DPLL(PIPE_B), I915_READ(DPLL(PIPE_B)) |
+			   DPLL_REFA_CLK_ENABLE_VLV);
+		I915_WRITE(DPLL(PIPE_B), I915_READ(DPLL(PIPE_B)) |
+			   DPLL_REFA_CLK_ENABLE_VLV | DPLL_INTEGRATED_CRI_CLK_VLV);
+	} else {
+		phy = DPIO_PHY1;
+		I915_WRITE(DPLL(PIPE_C), I915_READ(DPLL(PIPE_C)) |
+			   DPLL_REFA_CLK_ENABLE_VLV | DPLL_INTEGRATED_CRI_CLK_VLV);
+	}
+	udelay(1); /* >10ns for cmnreset, >0ns for sidereset */
+	vlv_set_power_well(dev_priv, power_well, true);
+
+	/* Poll for phypwrgood signal */
+	if (wait_for(I915_READ(DISPLAY_PHY_STATUS) & PHY_POWERGOOD(phy), 1))
+		DRM_ERROR("Display PHY %d is not power up\n", phy);
+
+	I915_WRITE(DISPLAY_PHY_CONTROL, I915_READ(DISPLAY_PHY_CONTROL) |
+		   PHY_COM_LANE_RESET_DEASSERT(phy));
+}
+
+static void chv_dpio_cmn_power_well_disable(struct drm_i915_private *dev_priv,
+					    struct i915_power_well *power_well)
+{
+	enum dpio_phy phy;
+
+	WARN_ON_ONCE(power_well->data != PUNIT_POWER_WELL_DPIO_CMN_BC &&
+		     power_well->data != PUNIT_POWER_WELL_DPIO_CMN_D);
+
+	if (power_well->data == PUNIT_POWER_WELL_DPIO_CMN_BC) {
+		phy = DPIO_PHY0;
+		assert_pll_disabled(dev_priv, PIPE_A);
+		assert_pll_disabled(dev_priv, PIPE_B);
+	} else {
+		phy = DPIO_PHY1;
+		assert_pll_disabled(dev_priv, PIPE_C);
+	}
+
+	I915_WRITE(DISPLAY_PHY_CONTROL, I915_READ(DISPLAY_PHY_CONTROL) &
+		   ~PHY_COM_LANE_RESET_DEASSERT(phy));
+
+	vlv_set_power_well(dev_priv, power_well, false);
+}
+
+static bool chv_pipe_power_well_enabled(struct drm_i915_private *dev_priv,
+					struct i915_power_well *power_well)
+{
+	enum pipe pipe = power_well->data;
+	bool enabled;
+	u32 state, ctrl;
+
+	mutex_lock(&dev_priv->rps.hw_lock);
+
+	state = vlv_punit_read(dev_priv, PUNIT_REG_DSPFREQ) & DP_SSS_MASK(pipe);
+	/*
+	 * We only ever set the power-on and power-gate states, anything
+	 * else is unexpected.
+	 */
+	WARN_ON(state != DP_SSS_PWR_ON(pipe) && state != DP_SSS_PWR_GATE(pipe));
+	enabled = state == DP_SSS_PWR_ON(pipe);
+
+	/*
+	 * A transient state at this point would mean some unexpected party
+	 * is poking at the power controls too.
+	 */
+	ctrl = vlv_punit_read(dev_priv, PUNIT_REG_DSPFREQ) & DP_SSC_MASK(pipe);
+	WARN_ON(ctrl << 16 != state);
+
+	mutex_unlock(&dev_priv->rps.hw_lock);
+
+	return enabled;
+}
+
+static void chv_set_pipe_power_well(struct drm_i915_private *dev_priv,
+				    struct i915_power_well *power_well,
+				    bool enable)
+{
+	enum pipe pipe = power_well->data;
+	u32 state;
+	u32 ctrl;
+
+	state = enable ? DP_SSS_PWR_ON(pipe) : DP_SSS_PWR_GATE(pipe);
+
+	mutex_lock(&dev_priv->rps.hw_lock);
+
+#define COND \
+	((vlv_punit_read(dev_priv, PUNIT_REG_DSPFREQ) & DP_SSS_MASK(pipe)) == state)
+
+	if (COND)
+		goto out;
+
+	ctrl = vlv_punit_read(dev_priv, PUNIT_REG_DSPFREQ);
+	ctrl &= ~DP_SSC_MASK(pipe);
+	ctrl |= enable ? DP_SSC_PWR_ON(pipe) : DP_SSC_PWR_GATE(pipe);
+	vlv_punit_write(dev_priv, PUNIT_REG_DSPFREQ, ctrl);
+
+	if (wait_for(COND, 100))
+		DRM_ERROR("timout setting power well state %08x (%08x)\n",
+			  state,
+			  vlv_punit_read(dev_priv, PUNIT_REG_DSPFREQ));
+
+#undef COND
+
+out:
+	mutex_unlock(&dev_priv->rps.hw_lock);
+}
+
+static void chv_pipe_power_well_sync_hw(struct drm_i915_private *dev_priv,
+					struct i915_power_well *power_well)
+{
+	chv_set_pipe_power_well(dev_priv, power_well, power_well->count > 0);
+}
+
+static void chv_pipe_power_well_enable(struct drm_i915_private *dev_priv,
+				       struct i915_power_well *power_well)
+{
+	WARN_ON_ONCE(power_well->data != PIPE_A &&
+		     power_well->data != PIPE_B &&
+		     power_well->data != PIPE_C);
+
+	chv_set_pipe_power_well(dev_priv, power_well, true);
+
+	if (power_well->data == PIPE_A) {
+		spin_lock_irq(&dev_priv->irq_lock);
+		valleyview_enable_display_irqs(dev_priv);
+		spin_unlock_irq(&dev_priv->irq_lock);
+
+		/*
+		 * During driver initialization/resume we can avoid restoring the
+		 * part of the HW/SW state that will be inited anyway explicitly.
+		 */
+		if (dev_priv->power_domains.initializing)
+			return;
+
+		intel_hpd_init(dev_priv);
+
+		i915_redisable_vga_power_on(dev_priv->dev);
+	}
+}
+
+static void chv_pipe_power_well_disable(struct drm_i915_private *dev_priv,
+					struct i915_power_well *power_well)
+{
+	WARN_ON_ONCE(power_well->data != PIPE_A &&
+		     power_well->data != PIPE_B &&
+		     power_well->data != PIPE_C);
+
+	if (power_well->data == PIPE_A) {
+		spin_lock_irq(&dev_priv->irq_lock);
+		valleyview_disable_display_irqs(dev_priv);
+		spin_unlock_irq(&dev_priv->irq_lock);
+	}
+
+	chv_set_pipe_power_well(dev_priv, power_well, false);
+
+	if (power_well->data == PIPE_A)
+		vlv_power_sequencer_reset(dev_priv);
+}
+
+static void check_power_well_state(struct drm_i915_private *dev_priv,
+				   struct i915_power_well *power_well)
+{
+	bool enabled = power_well->ops->is_enabled(dev_priv, power_well);
+
+	if (power_well->always_on || !i915.disable_power_well) {
+		if (!enabled)
+			goto mismatch;
+
+		return;
+	}
+
+	if (enabled != (power_well->count > 0))
+		goto mismatch;
+
+	return;
+
+mismatch:
+	WARN(1, "state mismatch for '%s' (always_on %d hw state %d use-count %d disable_power_well %d\n",
+		  power_well->name, power_well->always_on, enabled,
+		  power_well->count, i915.disable_power_well);
+}
+
+/**
+ * intel_display_power_get - grab a power domain reference
+ * @dev_priv: i915 device instance
+ * @domain: power domain to reference
+ *
+ * This function grabs a power domain reference for @domain and ensures that the
+ * power domain and all its parents are powered up. Therefore users should only
+ * grab a reference to the innermost power domain they need.
+ *
+ * Any power domain reference obtained by this function must have a symmetric
+ * call to intel_display_power_put() to release the reference again.
+ */
+void intel_display_power_get(struct drm_i915_private *dev_priv,
+			     enum intel_display_power_domain domain)
+{
+	struct i915_power_domains *power_domains;
+	struct i915_power_well *power_well;
+	int i;
+
+	intel_runtime_pm_get(dev_priv);
+
+	power_domains = &dev_priv->power_domains;
+
+	mutex_lock(&power_domains->lock);
+
+	for_each_power_well(i, power_well, BIT(domain), power_domains) {
+		if (!power_well->count++) {
+			DRM_DEBUG_KMS("enabling %s\n", power_well->name);
+			power_well->ops->enable(dev_priv, power_well);
+			power_well->hw_enabled = true;
+		}
+
+		check_power_well_state(dev_priv, power_well);
+	}
+
+	power_domains->domain_use_count[domain]++;
+
+	mutex_unlock(&power_domains->lock);
+}
+
+/**
+ * intel_display_power_put - release a power domain reference
+ * @dev_priv: i915 device instance
+ * @domain: power domain to reference
+ *
+ * This function drops the power domain reference obtained by
+ * intel_display_power_get() and might power down the corresponding hardware
+ * block right away if this is the last reference.
+ */
+void intel_display_power_put(struct drm_i915_private *dev_priv,
+			     enum intel_display_power_domain domain)
+{
+	struct i915_power_domains *power_domains;
+	struct i915_power_well *power_well;
+	int i;
+
+	power_domains = &dev_priv->power_domains;
+
+	mutex_lock(&power_domains->lock);
+
+	WARN_ON(!power_domains->domain_use_count[domain]);
+	power_domains->domain_use_count[domain]--;
+
+	for_each_power_well_rev(i, power_well, BIT(domain), power_domains) {
+		WARN_ON(!power_well->count);
+
+		if (!--power_well->count && i915.disable_power_well) {
+			DRM_DEBUG_KMS("disabling %s\n", power_well->name);
+			power_well->hw_enabled = false;
+			power_well->ops->disable(dev_priv, power_well);
+		}
+
+		check_power_well_state(dev_priv, power_well);
+	}
+
+	mutex_unlock(&power_domains->lock);
+
+	intel_runtime_pm_put(dev_priv);
+}
+
+#define POWER_DOMAIN_MASK (BIT(POWER_DOMAIN_NUM) - 1)
+
+#define HSW_ALWAYS_ON_POWER_DOMAINS (			\
+	BIT(POWER_DOMAIN_PIPE_A) |			\
+	BIT(POWER_DOMAIN_TRANSCODER_EDP) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_A_2_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_A_4_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_B_2_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_B_4_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_C_2_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_D_2_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_D_4_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_CRT) |			\
+	BIT(POWER_DOMAIN_PLLS) |			\
+	BIT(POWER_DOMAIN_INIT))
+#define HSW_DISPLAY_POWER_DOMAINS (				\
+	(POWER_DOMAIN_MASK & ~HSW_ALWAYS_ON_POWER_DOMAINS) |	\
+	BIT(POWER_DOMAIN_INIT))
+
+#define BDW_ALWAYS_ON_POWER_DOMAINS (			\
+	HSW_ALWAYS_ON_POWER_DOMAINS |			\
+	BIT(POWER_DOMAIN_PIPE_A_PANEL_FITTER))
+#define BDW_DISPLAY_POWER_DOMAINS (				\
+	(POWER_DOMAIN_MASK & ~BDW_ALWAYS_ON_POWER_DOMAINS) |	\
+	BIT(POWER_DOMAIN_INIT))
+
+#define VLV_ALWAYS_ON_POWER_DOMAINS	BIT(POWER_DOMAIN_INIT)
+#define VLV_DISPLAY_POWER_DOMAINS	POWER_DOMAIN_MASK
+
+#define VLV_DPIO_CMN_BC_POWER_DOMAINS (		\
+	BIT(POWER_DOMAIN_PORT_DDI_B_2_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_B_4_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_C_2_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_CRT) |		\
+	BIT(POWER_DOMAIN_INIT))
+
+#define VLV_DPIO_TX_B_LANES_01_POWER_DOMAINS (	\
+	BIT(POWER_DOMAIN_PORT_DDI_B_2_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_B_4_LANES) |	\
+	BIT(POWER_DOMAIN_INIT))
+
+#define VLV_DPIO_TX_B_LANES_23_POWER_DOMAINS (	\
+	BIT(POWER_DOMAIN_PORT_DDI_B_4_LANES) |	\
+	BIT(POWER_DOMAIN_INIT))
+
+#define VLV_DPIO_TX_C_LANES_01_POWER_DOMAINS (	\
+	BIT(POWER_DOMAIN_PORT_DDI_C_2_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |	\
+	BIT(POWER_DOMAIN_INIT))
+
+#define VLV_DPIO_TX_C_LANES_23_POWER_DOMAINS (	\
+	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |	\
+	BIT(POWER_DOMAIN_INIT))
+
+#define CHV_PIPE_A_POWER_DOMAINS (	\
+	BIT(POWER_DOMAIN_PIPE_A) |	\
+	BIT(POWER_DOMAIN_INIT))
+
+#define CHV_PIPE_B_POWER_DOMAINS (	\
+	BIT(POWER_DOMAIN_PIPE_B) |	\
+	BIT(POWER_DOMAIN_INIT))
+
+#define CHV_PIPE_C_POWER_DOMAINS (	\
+	BIT(POWER_DOMAIN_PIPE_C) |	\
+	BIT(POWER_DOMAIN_INIT))
+
+#define CHV_DPIO_CMN_BC_POWER_DOMAINS (		\
+	BIT(POWER_DOMAIN_PORT_DDI_B_2_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_B_4_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_C_2_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |	\
+	BIT(POWER_DOMAIN_INIT))
+
+#define CHV_DPIO_CMN_D_POWER_DOMAINS (		\
+	BIT(POWER_DOMAIN_PORT_DDI_D_2_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_D_4_LANES) |	\
+	BIT(POWER_DOMAIN_INIT))
+
+#define CHV_DPIO_TX_D_LANES_01_POWER_DOMAINS (	\
+	BIT(POWER_DOMAIN_PORT_DDI_D_2_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_D_4_LANES) |	\
+	BIT(POWER_DOMAIN_INIT))
+
+#define CHV_DPIO_TX_D_LANES_23_POWER_DOMAINS (	\
+	BIT(POWER_DOMAIN_PORT_DDI_D_4_LANES) |	\
+	BIT(POWER_DOMAIN_INIT))
+
+static const struct i915_power_well_ops i9xx_always_on_power_well_ops = {
+	.sync_hw = i9xx_always_on_power_well_noop,
+	.enable = i9xx_always_on_power_well_noop,
+	.disable = i9xx_always_on_power_well_noop,
+	.is_enabled = i9xx_always_on_power_well_enabled,
+};
+
+static const struct i915_power_well_ops chv_pipe_power_well_ops = {
+	.sync_hw = chv_pipe_power_well_sync_hw,
+	.enable = chv_pipe_power_well_enable,
+	.disable = chv_pipe_power_well_disable,
+	.is_enabled = chv_pipe_power_well_enabled,
+};
+
+static const struct i915_power_well_ops chv_dpio_cmn_power_well_ops = {
+	.sync_hw = vlv_power_well_sync_hw,
+	.enable = chv_dpio_cmn_power_well_enable,
+	.disable = chv_dpio_cmn_power_well_disable,
+	.is_enabled = vlv_power_well_enabled,
+};
+
+static struct i915_power_well i9xx_always_on_power_well[] = {
+	{
+		.name = "always-on",
+		.always_on = 1,
+		.domains = POWER_DOMAIN_MASK,
+		.ops = &i9xx_always_on_power_well_ops,
+	},
+};
+
+static const struct i915_power_well_ops hsw_power_well_ops = {
+	.sync_hw = hsw_power_well_sync_hw,
+	.enable = hsw_power_well_enable,
+	.disable = hsw_power_well_disable,
+	.is_enabled = hsw_power_well_enabled,
+};
+
+static struct i915_power_well hsw_power_wells[] = {
+	{
+		.name = "always-on",
+		.always_on = 1,
+		.domains = HSW_ALWAYS_ON_POWER_DOMAINS,
+		.ops = &i9xx_always_on_power_well_ops,
+	},
+	{
+		.name = "display",
+		.domains = HSW_DISPLAY_POWER_DOMAINS,
+		.ops = &hsw_power_well_ops,
+	},
+};
+
+static struct i915_power_well bdw_power_wells[] = {
+	{
+		.name = "always-on",
+		.always_on = 1,
+		.domains = BDW_ALWAYS_ON_POWER_DOMAINS,
+		.ops = &i9xx_always_on_power_well_ops,
+	},
+	{
+		.name = "display",
+		.domains = BDW_DISPLAY_POWER_DOMAINS,
+		.ops = &hsw_power_well_ops,
+	},
+};
+
+static const struct i915_power_well_ops vlv_display_power_well_ops = {
+	.sync_hw = vlv_power_well_sync_hw,
+	.enable = vlv_display_power_well_enable,
+	.disable = vlv_display_power_well_disable,
+	.is_enabled = vlv_power_well_enabled,
+};
+
+static const struct i915_power_well_ops vlv_dpio_cmn_power_well_ops = {
+	.sync_hw = vlv_power_well_sync_hw,
+	.enable = vlv_dpio_cmn_power_well_enable,
+	.disable = vlv_dpio_cmn_power_well_disable,
+	.is_enabled = vlv_power_well_enabled,
+};
+
+static const struct i915_power_well_ops vlv_dpio_power_well_ops = {
+	.sync_hw = vlv_power_well_sync_hw,
+	.enable = vlv_power_well_enable,
+	.disable = vlv_power_well_disable,
+	.is_enabled = vlv_power_well_enabled,
+};
+
+static struct i915_power_well vlv_power_wells[] = {
+	{
+		.name = "always-on",
+		.always_on = 1,
+		.domains = VLV_ALWAYS_ON_POWER_DOMAINS,
+		.ops = &i9xx_always_on_power_well_ops,
+	},
+	{
+		.name = "display",
+		.domains = VLV_DISPLAY_POWER_DOMAINS,
+		.data = PUNIT_POWER_WELL_DISP2D,
+		.ops = &vlv_display_power_well_ops,
+	},
+	{
+		.name = "dpio-tx-b-01",
+		.domains = VLV_DPIO_TX_B_LANES_01_POWER_DOMAINS |
+			   VLV_DPIO_TX_B_LANES_23_POWER_DOMAINS |
+			   VLV_DPIO_TX_C_LANES_01_POWER_DOMAINS |
+			   VLV_DPIO_TX_C_LANES_23_POWER_DOMAINS,
+		.ops = &vlv_dpio_power_well_ops,
+		.data = PUNIT_POWER_WELL_DPIO_TX_B_LANES_01,
+	},
+	{
+		.name = "dpio-tx-b-23",
+		.domains = VLV_DPIO_TX_B_LANES_01_POWER_DOMAINS |
+			   VLV_DPIO_TX_B_LANES_23_POWER_DOMAINS |
+			   VLV_DPIO_TX_C_LANES_01_POWER_DOMAINS |
+			   VLV_DPIO_TX_C_LANES_23_POWER_DOMAINS,
+		.ops = &vlv_dpio_power_well_ops,
+		.data = PUNIT_POWER_WELL_DPIO_TX_B_LANES_23,
+	},
+	{
+		.name = "dpio-tx-c-01",
+		.domains = VLV_DPIO_TX_B_LANES_01_POWER_DOMAINS |
+			   VLV_DPIO_TX_B_LANES_23_POWER_DOMAINS |
+			   VLV_DPIO_TX_C_LANES_01_POWER_DOMAINS |
+			   VLV_DPIO_TX_C_LANES_23_POWER_DOMAINS,
+		.ops = &vlv_dpio_power_well_ops,
+		.data = PUNIT_POWER_WELL_DPIO_TX_C_LANES_01,
+	},
+	{
+		.name = "dpio-tx-c-23",
+		.domains = VLV_DPIO_TX_B_LANES_01_POWER_DOMAINS |
+			   VLV_DPIO_TX_B_LANES_23_POWER_DOMAINS |
+			   VLV_DPIO_TX_C_LANES_01_POWER_DOMAINS |
+			   VLV_DPIO_TX_C_LANES_23_POWER_DOMAINS,
+		.ops = &vlv_dpio_power_well_ops,
+		.data = PUNIT_POWER_WELL_DPIO_TX_C_LANES_23,
+	},
+	{
+		.name = "dpio-common",
+		.domains = VLV_DPIO_CMN_BC_POWER_DOMAINS,
+		.data = PUNIT_POWER_WELL_DPIO_CMN_BC,
+		.ops = &vlv_dpio_cmn_power_well_ops,
+	},
+};
+
+static struct i915_power_well chv_power_wells[] = {
+	{
+		.name = "always-on",
+		.always_on = 1,
+		.domains = VLV_ALWAYS_ON_POWER_DOMAINS,
+		.ops = &i9xx_always_on_power_well_ops,
+	},
+#if 0
+	{
+		.name = "display",
+		.domains = VLV_DISPLAY_POWER_DOMAINS,
+		.data = PUNIT_POWER_WELL_DISP2D,
+		.ops = &vlv_display_power_well_ops,
+	},
+#endif
+	{
+		.name = "pipe-a",
+		/*
+		 * FIXME: pipe A power well seems to be the new disp2d well.
+		 * At least all registers seem to be housed there. Figure
+		 * out if this a a temporary situation in pre-production
+		 * hardware or a permanent state of affairs.
+		 */
+		.domains = CHV_PIPE_A_POWER_DOMAINS | VLV_DISPLAY_POWER_DOMAINS,
+		.data = PIPE_A,
+		.ops = &chv_pipe_power_well_ops,
+	},
+#if 0
+	{
+		.name = "pipe-b",
+		.domains = CHV_PIPE_B_POWER_DOMAINS,
+		.data = PIPE_B,
+		.ops = &chv_pipe_power_well_ops,
+	},
+	{
+		.name = "pipe-c",
+		.domains = CHV_PIPE_C_POWER_DOMAINS,
+		.data = PIPE_C,
+		.ops = &chv_pipe_power_well_ops,
+	},
+#endif
+	{
+		.name = "dpio-common-bc",
+		/*
+		 * XXX: cmnreset for one PHY seems to disturb the other.
+		 * As a workaround keep both powered on at the same
+		 * time for now.
+		 */
+		.domains = CHV_DPIO_CMN_BC_POWER_DOMAINS | CHV_DPIO_CMN_D_POWER_DOMAINS,
+		.data = PUNIT_POWER_WELL_DPIO_CMN_BC,
+		.ops = &chv_dpio_cmn_power_well_ops,
+	},
+	{
+		.name = "dpio-common-d",
+		/*
+		 * XXX: cmnreset for one PHY seems to disturb the other.
+		 * As a workaround keep both powered on at the same
+		 * time for now.
+		 */
+		.domains = CHV_DPIO_CMN_BC_POWER_DOMAINS | CHV_DPIO_CMN_D_POWER_DOMAINS,
+		.data = PUNIT_POWER_WELL_DPIO_CMN_D,
+		.ops = &chv_dpio_cmn_power_well_ops,
+	},
+#if 0
+	{
+		.name = "dpio-tx-b-01",
+		.domains = VLV_DPIO_TX_B_LANES_01_POWER_DOMAINS |
+			   VLV_DPIO_TX_B_LANES_23_POWER_DOMAINS,
+		.ops = &vlv_dpio_power_well_ops,
+		.data = PUNIT_POWER_WELL_DPIO_TX_B_LANES_01,
+	},
+	{
+		.name = "dpio-tx-b-23",
+		.domains = VLV_DPIO_TX_B_LANES_01_POWER_DOMAINS |
+			   VLV_DPIO_TX_B_LANES_23_POWER_DOMAINS,
+		.ops = &vlv_dpio_power_well_ops,
+		.data = PUNIT_POWER_WELL_DPIO_TX_B_LANES_23,
+	},
+	{
+		.name = "dpio-tx-c-01",
+		.domains = VLV_DPIO_TX_C_LANES_01_POWER_DOMAINS |
+			   VLV_DPIO_TX_C_LANES_23_POWER_DOMAINS,
+		.ops = &vlv_dpio_power_well_ops,
+		.data = PUNIT_POWER_WELL_DPIO_TX_C_LANES_01,
+	},
+	{
+		.name = "dpio-tx-c-23",
+		.domains = VLV_DPIO_TX_C_LANES_01_POWER_DOMAINS |
+			   VLV_DPIO_TX_C_LANES_23_POWER_DOMAINS,
+		.ops = &vlv_dpio_power_well_ops,
+		.data = PUNIT_POWER_WELL_DPIO_TX_C_LANES_23,
+	},
+	{
+		.name = "dpio-tx-d-01",
+		.domains = CHV_DPIO_TX_D_LANES_01_POWER_DOMAINS |
+			   CHV_DPIO_TX_D_LANES_23_POWER_DOMAINS,
+		.ops = &vlv_dpio_power_well_ops,
+		.data = PUNIT_POWER_WELL_DPIO_TX_D_LANES_01,
+	},
+	{
+		.name = "dpio-tx-d-23",
+		.domains = CHV_DPIO_TX_D_LANES_01_POWER_DOMAINS |
+			   CHV_DPIO_TX_D_LANES_23_POWER_DOMAINS,
+		.ops = &vlv_dpio_power_well_ops,
+		.data = PUNIT_POWER_WELL_DPIO_TX_D_LANES_23,
+	},
+#endif
+};
+
+static struct i915_power_well *lookup_power_well(struct drm_i915_private *dev_priv,
+						 enum punit_power_well power_well_id)
+{
+	struct i915_power_domains *power_domains = &dev_priv->power_domains;
+	struct i915_power_well *power_well;
+	int i;
+
+	for_each_power_well(i, power_well, POWER_DOMAIN_MASK, power_domains) {
+		if (power_well->data == power_well_id)
+			return power_well;
+	}
+
+	return NULL;
+}
+
+#define set_power_wells(power_domains, __power_wells) ({		\
+	(power_domains)->power_wells = (__power_wells);			\
+	(power_domains)->power_well_count = ARRAY_SIZE(__power_wells);	\
+})
+
+/**
+ * intel_power_domains_init - initializes the power domain structures
+ * @dev_priv: i915 device instance
+ *
+ * Initializes the power domain structures for @dev_priv depending upon the
+ * supported platform.
+ */
+int intel_power_domains_init(struct drm_i915_private *dev_priv)
+{
+	struct i915_power_domains *power_domains = &dev_priv->power_domains;
+
+	mutex_init(&power_domains->lock);
+
+	/*
+	 * The enabling order will be from lower to higher indexed wells,
+	 * the disabling order is reversed.
+	 */
+	if (IS_HASWELL(dev_priv->dev)) {
+		set_power_wells(power_domains, hsw_power_wells);
+		hsw_pwr = power_domains;
+	} else if (IS_BROADWELL(dev_priv->dev)) {
+		set_power_wells(power_domains, bdw_power_wells);
+		hsw_pwr = power_domains;
+	} else if (IS_CHERRYVIEW(dev_priv->dev)) {
+		set_power_wells(power_domains, chv_power_wells);
+	} else if (IS_VALLEYVIEW(dev_priv->dev)) {
+		set_power_wells(power_domains, vlv_power_wells);
+	} else {
+		set_power_wells(power_domains, i9xx_always_on_power_well);
+	}
+
+	return 0;
+}
+
+static void intel_runtime_pm_disable(struct drm_i915_private *dev_priv)
+{
+	struct drm_device *dev = dev_priv->dev;
+	struct device *device = &dev->pdev->dev;
+
+	if (!HAS_RUNTIME_PM(dev))
+		return;
+
+	if (!intel_enable_rc6(dev))
+		return;
+
+	/* Make sure we're not suspended first. */
+	pm_runtime_get_sync(device);
+	pm_runtime_disable(device);
+}
+
+/**
+ * intel_power_domains_fini - finalizes the power domain structures
+ * @dev_priv: i915 device instance
+ *
+ * Finalizes the power domain structures for @dev_priv depending upon the
+ * supported platform. This function also disables runtime pm and ensures that
+ * the device stays powered up so that the driver can be reloaded.
+ */
+void intel_power_domains_fini(struct drm_i915_private *dev_priv)
+{
+	intel_runtime_pm_disable(dev_priv);
+
+	/* The i915.ko module is still not prepared to be loaded when
+	 * the power well is not enabled, so just enable it in case
+	 * we're going to unload/reload. */
+	intel_display_set_init_power(dev_priv, true);
+
+	hsw_pwr = NULL;
+}
+
+static void intel_power_domains_resume(struct drm_i915_private *dev_priv)
+{
+	struct i915_power_domains *power_domains = &dev_priv->power_domains;
+	struct i915_power_well *power_well;
+	int i;
+
+	mutex_lock(&power_domains->lock);
+	for_each_power_well(i, power_well, POWER_DOMAIN_MASK, power_domains) {
+		power_well->ops->sync_hw(dev_priv, power_well);
+		power_well->hw_enabled = power_well->ops->is_enabled(dev_priv,
+								     power_well);
+	}
+	mutex_unlock(&power_domains->lock);
+}
+
+static void vlv_cmnlane_wa(struct drm_i915_private *dev_priv)
+{
+	struct i915_power_well *cmn =
+		lookup_power_well(dev_priv, PUNIT_POWER_WELL_DPIO_CMN_BC);
+	struct i915_power_well *disp2d =
+		lookup_power_well(dev_priv, PUNIT_POWER_WELL_DISP2D);
+
+	/* If the display might be already active skip this */
+	if (cmn->ops->is_enabled(dev_priv, cmn) &&
+	    disp2d->ops->is_enabled(dev_priv, disp2d) &&
+	    I915_READ(DPIO_CTL) & DPIO_CMNRST)
+		return;
+
+	DRM_DEBUG_KMS("toggling display PHY side reset\n");
+
+	/* cmnlane needs DPLL registers */
+	disp2d->ops->enable(dev_priv, disp2d);
+
+	/*
+	 * From VLV2A0_DP_eDP_HDMI_DPIO_driver_vbios_notes_11.docx:
+	 * Need to assert and de-assert PHY SB reset by gating the
+	 * common lane power, then un-gating it.
+	 * Simply ungating isn't enough to reset the PHY enough to get
+	 * ports and lanes running.
+	 */
+	cmn->ops->disable(dev_priv, cmn);
+}
+
+/**
+ * intel_power_domains_init_hw - initialize hardware power domain state
+ * @dev_priv: i915 device instance
+ *
+ * This function initializes the hardware power domain state and enables all
+ * power domains using intel_display_set_init_power().
+ */
+void intel_power_domains_init_hw(struct drm_i915_private *dev_priv)
+{
+	struct drm_device *dev = dev_priv->dev;
+	struct i915_power_domains *power_domains = &dev_priv->power_domains;
+
+	power_domains->initializing = true;
+
+	if (IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev)) {
+		mutex_lock(&power_domains->lock);
+		vlv_cmnlane_wa(dev_priv);
+		mutex_unlock(&power_domains->lock);
+	}
+
+	/* For now, we need the power well to be always enabled. */
+	intel_display_set_init_power(dev_priv, true);
+	intel_power_domains_resume(dev_priv);
+	power_domains->initializing = false;
+}
+
+/**
+ * intel_aux_display_runtime_get - grab an auxilliary power domain reference
+ * @dev_priv: i915 device instance
+ *
+ * This function grabs a power domain reference for the auxiliary power domain
+ * (for access to the GMBUS and DP AUX blocks) and ensures that it and all its
+ * parents are powered up. Therefore users should only grab a reference to the
+ * innermost power domain they need.
+ *
+ * Any power domain reference obtained by this function must have a symmetric
+ * call to intel_aux_display_runtime_put() to release the reference again.
+ */
+void intel_aux_display_runtime_get(struct drm_i915_private *dev_priv)
+{
+	intel_runtime_pm_get(dev_priv);
+}
+
+/**
+ * intel_aux_display_runtime_put - release an auxilliary power domain reference
+ * @dev_priv: i915 device instance
+ *
+ * This function drops the auxilliary power domain reference obtained by
+ * intel_aux_display_runtime_get() and might power down the corresponding
+ * hardware block right away if this is the last reference.
+ */
+void intel_aux_display_runtime_put(struct drm_i915_private *dev_priv)
+{
+	intel_runtime_pm_put(dev_priv);
+}
+
+/**
+ * intel_runtime_pm_get - grab a runtime pm reference
+ * @dev_priv: i915 device instance
+ *
+ * This function grabs a device-level runtime pm reference (mostly used for GEM
+ * code to ensure the GTT or GT is on) and ensures that it is powered up.
+ *
+ * Any runtime pm reference obtained by this function must have a symmetric
+ * call to intel_runtime_pm_put() to release the reference again.
+ */
+void intel_runtime_pm_get(struct drm_i915_private *dev_priv)
+{
+	struct drm_device *dev = dev_priv->dev;
+	struct device *device = &dev->pdev->dev;
+
+	if (!HAS_RUNTIME_PM(dev))
+		return;
+
+	pm_runtime_get_sync(device);
+	WARN(dev_priv->pm.suspended, "Device still suspended.\n");
+}
+
+/**
+ * intel_runtime_pm_get_noresume - grab a runtime pm reference
+ * @dev_priv: i915 device instance
+ *
+ * This function grabs a device-level runtime pm reference (mostly used for GEM
+ * code to ensure the GTT or GT is on).
+ *
+ * It will _not_ power up the device but instead only check that it's powered
+ * on.  Therefore it is only valid to call this functions from contexts where
+ * the device is known to be powered up and where trying to power it up would
+ * result in hilarity and deadlocks. That pretty much means only the system
+ * suspend/resume code where this is used to grab runtime pm references for
+ * delayed setup down in work items.
+ *
+ * Any runtime pm reference obtained by this function must have a symmetric
+ * call to intel_runtime_pm_put() to release the reference again.
+ */
+void intel_runtime_pm_get_noresume(struct drm_i915_private *dev_priv)
+{
+	struct drm_device *dev = dev_priv->dev;
+	struct device *device = &dev->pdev->dev;
+
+	if (!HAS_RUNTIME_PM(dev))
+		return;
+
+	WARN(dev_priv->pm.suspended, "Getting nosync-ref while suspended.\n");
+	pm_runtime_get_noresume(device);
+}
+
+/**
+ * intel_runtime_pm_put - release a runtime pm reference
+ * @dev_priv: i915 device instance
+ *
+ * This function drops the device-level runtime pm reference obtained by
+ * intel_runtime_pm_get() and might power down the corresponding
+ * hardware block right away if this is the last reference.
+ */
+void intel_runtime_pm_put(struct drm_i915_private *dev_priv)
+{
+	struct drm_device *dev = dev_priv->dev;
+	struct device *device = &dev->pdev->dev;
+
+	if (!HAS_RUNTIME_PM(dev))
+		return;
+
+	pm_runtime_mark_last_busy(device);
+	pm_runtime_put_autosuspend(device);
+}
+
+/**
+ * intel_runtime_pm_enable - enable runtime pm
+ * @dev_priv: i915 device instance
+ *
+ * This function enables runtime pm at the end of the driver load sequence.
+ *
+ * Note that this function does currently not enable runtime pm for the
+ * subordinate display power domains. That is only done on the first modeset
+ * using intel_display_set_init_power().
+ */
+void intel_runtime_pm_enable(struct drm_i915_private *dev_priv)
+{
+	struct drm_device *dev = dev_priv->dev;
+	struct device *device = &dev->pdev->dev;
+
+	if (!HAS_RUNTIME_PM(dev))
+		return;
+
+	pm_runtime_set_active(device);
+
+	/*
+	 * RPM depends on RC6 to save restore the GT HW context, so make RC6 a
+	 * requirement.
+	 */
+	if (!intel_enable_rc6(dev)) {
+		DRM_INFO("RC6 disabled, disabling runtime PM support\n");
+		return;
+	}
+
+	pm_runtime_set_autosuspend_delay(device, 10000); /* 10s */
+	pm_runtime_mark_last_busy(device);
+	pm_runtime_use_autosuspend(device);
+
+	pm_runtime_put_autosuspend(device);
+}
+
+/* Display audio driver power well request */
+int i915_request_power_well(void)
+{
+	struct drm_i915_private *dev_priv;
+
+	if (!hsw_pwr)
+		return -ENODEV;
+
+	dev_priv = container_of(hsw_pwr, struct drm_i915_private,
+				power_domains);
+	intel_display_power_get(dev_priv, POWER_DOMAIN_AUDIO);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(i915_request_power_well);
+
+/* Display audio driver power well release */
+int i915_release_power_well(void)
+{
+	struct drm_i915_private *dev_priv;
+
+	if (!hsw_pwr)
+		return -ENODEV;
+
+	dev_priv = container_of(hsw_pwr, struct drm_i915_private,
+				power_domains);
+	intel_display_power_put(dev_priv, POWER_DOMAIN_AUDIO);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(i915_release_power_well);
+
+/*
+ * Private interface for the audio driver to get CDCLK in kHz.
+ *
+ * Caller must request power well using i915_request_power_well() prior to
+ * making the call.
+ */
+int i915_get_cdclk_freq(void)
+{
+	struct drm_i915_private *dev_priv;
+
+	if (!hsw_pwr)
+		return -ENODEV;
+
+	dev_priv = container_of(hsw_pwr, struct drm_i915_private,
+				power_domains);
+
+	return intel_ddi_get_cdclk_freq(dev_priv);
+}
+EXPORT_SYMBOL_GPL(i915_get_cdclk_freq);
diff --git a/drivers/gpu/drm/i915/intel_sdvo.c b/drivers/gpu/drm/i915/intel_sdvo.c
index 9350edd6728d..6d7a277458b5 100644
--- a/drivers/gpu/drm/i915/intel_sdvo.c
+++ b/drivers/gpu/drm/i915/intel_sdvo.c
@@ -1991,57 +1991,10 @@ static int intel_sdvo_get_modes(struct drm_connector *connector)
 	return !list_empty(&connector->probed_modes);
 }
 
-static void
-intel_sdvo_destroy_enhance_property(struct drm_connector *connector)
-{
-	struct intel_sdvo_connector *intel_sdvo_connector = to_intel_sdvo_connector(connector);
-	struct drm_device *dev = connector->dev;
-
-	if (intel_sdvo_connector->left)
-		drm_property_destroy(dev, intel_sdvo_connector->left);
-	if (intel_sdvo_connector->right)
-		drm_property_destroy(dev, intel_sdvo_connector->right);
-	if (intel_sdvo_connector->top)
-		drm_property_destroy(dev, intel_sdvo_connector->top);
-	if (intel_sdvo_connector->bottom)
-		drm_property_destroy(dev, intel_sdvo_connector->bottom);
-	if (intel_sdvo_connector->hpos)
-		drm_property_destroy(dev, intel_sdvo_connector->hpos);
-	if (intel_sdvo_connector->vpos)
-		drm_property_destroy(dev, intel_sdvo_connector->vpos);
-	if (intel_sdvo_connector->saturation)
-		drm_property_destroy(dev, intel_sdvo_connector->saturation);
-	if (intel_sdvo_connector->contrast)
-		drm_property_destroy(dev, intel_sdvo_connector->contrast);
-	if (intel_sdvo_connector->hue)
-		drm_property_destroy(dev, intel_sdvo_connector->hue);
-	if (intel_sdvo_connector->sharpness)
-		drm_property_destroy(dev, intel_sdvo_connector->sharpness);
-	if (intel_sdvo_connector->flicker_filter)
-		drm_property_destroy(dev, intel_sdvo_connector->flicker_filter);
-	if (intel_sdvo_connector->flicker_filter_2d)
-		drm_property_destroy(dev, intel_sdvo_connector->flicker_filter_2d);
-	if (intel_sdvo_connector->flicker_filter_adaptive)
-		drm_property_destroy(dev, intel_sdvo_connector->flicker_filter_adaptive);
-	if (intel_sdvo_connector->tv_luma_filter)
-		drm_property_destroy(dev, intel_sdvo_connector->tv_luma_filter);
-	if (intel_sdvo_connector->tv_chroma_filter)
-		drm_property_destroy(dev, intel_sdvo_connector->tv_chroma_filter);
-	if (intel_sdvo_connector->dot_crawl)
-		drm_property_destroy(dev, intel_sdvo_connector->dot_crawl);
-	if (intel_sdvo_connector->brightness)
-		drm_property_destroy(dev, intel_sdvo_connector->brightness);
-}
-
 static void intel_sdvo_destroy(struct drm_connector *connector)
 {
 	struct intel_sdvo_connector *intel_sdvo_connector = to_intel_sdvo_connector(connector);
 
-	if (intel_sdvo_connector->tv_format)
-		drm_property_destroy(connector->dev,
-				     intel_sdvo_connector->tv_format);
-
-	intel_sdvo_destroy_enhance_property(connector);
 	drm_connector_cleanup(connector);
 	kfree(intel_sdvo_connector);
 }
diff --git a/drivers/gpu/drm/i915/intel_sprite.c b/drivers/gpu/drm/i915/intel_sprite.c
index 07a74ef589bd..7d9c340f7693 100644
--- a/drivers/gpu/drm/i915/intel_sprite.c
+++ b/drivers/gpu/drm/i915/intel_sprite.c
@@ -37,6 +37,20 @@
 #include <drm/i915_drm.h>
 #include "i915_drv.h"
 
+static bool
+format_is_yuv(uint32_t format)
+{
+	switch (format) {
+	case DRM_FORMAT_YUYV:
+	case DRM_FORMAT_UYVY:
+	case DRM_FORMAT_VYUY:
+	case DRM_FORMAT_YVYU:
+		return true;
+	default:
+		return false;
+	}
+}
+
 static int usecs_to_scanlines(const struct drm_display_mode *mode, int usecs)
 {
 	/* paranoia */
@@ -46,7 +60,23 @@ static int usecs_to_scanlines(const struct drm_display_mode *mode, int usecs)
 	return DIV_ROUND_UP(usecs * mode->crtc_clock, 1000 * mode->crtc_htotal);
 }
 
-static bool intel_pipe_update_start(struct intel_crtc *crtc, uint32_t *start_vbl_count)
+/**
+ * intel_pipe_update_start() - start update of a set of display registers
+ * @crtc: the crtc of which the registers are going to be updated
+ * @start_vbl_count: vblank counter return pointer used for error checking
+ *
+ * Mark the start of an update to pipe registers that should be updated
+ * atomically regarding vblank. If the next vblank will happens within
+ * the next 100 us, this function waits until the vblank passes.
+ *
+ * After a successful call to this function, interrupts will be disabled
+ * until a subsequent call to intel_pipe_update_end(). That is done to
+ * avoid random delays. The value written to @start_vbl_count should be
+ * supplied to intel_pipe_update_end() for error checking.
+ *
+ * Return: true if the call was successful
+ */
+bool intel_pipe_update_start(struct intel_crtc *crtc, uint32_t *start_vbl_count)
 {
 	struct drm_device *dev = crtc->base.dev;
 	const struct drm_display_mode *mode = &crtc->config.adjusted_mode;
@@ -56,8 +86,6 @@ static bool intel_pipe_update_start(struct intel_crtc *crtc, uint32_t *start_vbl
 	wait_queue_head_t *wq = drm_crtc_vblank_waitqueue(&crtc->base);
 	DEFINE_WAIT(wait);
 
-	WARN_ON(!drm_modeset_is_locked(&crtc->base.mutex));
-
 	vblank_start = mode->crtc_vblank_start;
 	if (mode->flags & DRM_MODE_FLAG_INTERLACE)
 		vblank_start = DIV_ROUND_UP(vblank_start, 2);
@@ -112,7 +140,16 @@ static bool intel_pipe_update_start(struct intel_crtc *crtc, uint32_t *start_vbl
 	return true;
 }
 
-static void intel_pipe_update_end(struct intel_crtc *crtc, u32 start_vbl_count)
+/**
+ * intel_pipe_update_end() - end update of a set of display registers
+ * @crtc: the crtc of which the registers were updated
+ * @start_vbl_count: start vblank counter (used for error checking)
+ *
+ * Mark the end of an update started with intel_pipe_update_start(). This
+ * re-enables interrupts and verifies the update was actually completed
+ * before a vblank using the value of @start_vbl_count.
+ */
+void intel_pipe_update_end(struct intel_crtc *crtc, u32 start_vbl_count)
 {
 	struct drm_device *dev = crtc->base.dev;
 	enum pipe pipe = crtc->pipe;
@@ -139,6 +176,226 @@ static void intel_update_primary_plane(struct intel_crtc *crtc)
 }
 
 static void
+skl_update_plane(struct drm_plane *drm_plane, struct drm_crtc *crtc,
+		 struct drm_framebuffer *fb,
+		 struct drm_i915_gem_object *obj, int crtc_x, int crtc_y,
+		 unsigned int crtc_w, unsigned int crtc_h,
+		 uint32_t x, uint32_t y,
+		 uint32_t src_w, uint32_t src_h)
+{
+	struct drm_device *dev = drm_plane->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_plane *intel_plane = to_intel_plane(drm_plane);
+	const int pipe = intel_plane->pipe;
+	const int plane = intel_plane->plane + 1;
+	u32 plane_ctl, stride;
+	int pixel_size = drm_format_plane_cpp(fb->pixel_format, 0);
+
+	plane_ctl = I915_READ(PLANE_CTL(pipe, plane));
+
+	/* Mask out pixel format bits in case we change it */
+	plane_ctl &= ~PLANE_CTL_FORMAT_MASK;
+	plane_ctl &= ~PLANE_CTL_ORDER_RGBX;
+	plane_ctl &= ~PLANE_CTL_YUV422_ORDER_MASK;
+	plane_ctl &= ~PLANE_CTL_TILED_MASK;
+	plane_ctl &= ~PLANE_CTL_ALPHA_MASK;
+	plane_ctl &= ~PLANE_CTL_ROTATE_MASK;
+
+	/* Trickle feed has to be enabled */
+	plane_ctl &= ~PLANE_CTL_TRICKLE_FEED_DISABLE;
+
+	switch (fb->pixel_format) {
+	case DRM_FORMAT_RGB565:
+		plane_ctl |= PLANE_CTL_FORMAT_RGB_565;
+		break;
+	case DRM_FORMAT_XBGR8888:
+		plane_ctl |= PLANE_CTL_FORMAT_XRGB_8888 | PLANE_CTL_ORDER_RGBX;
+		break;
+	case DRM_FORMAT_XRGB8888:
+		plane_ctl |= PLANE_CTL_FORMAT_XRGB_8888;
+		break;
+	/*
+	 * XXX: For ARBG/ABGR formats we default to expecting scanout buffers
+	 * to be already pre-multiplied. We need to add a knob (or a different
+	 * DRM_FORMAT) for user-space to configure that.
+	 */
+	case DRM_FORMAT_ABGR8888:
+		plane_ctl |= PLANE_CTL_FORMAT_XRGB_8888 |
+			     PLANE_CTL_ORDER_RGBX |
+			     PLANE_CTL_ALPHA_SW_PREMULTIPLY;
+		break;
+	case DRM_FORMAT_ARGB8888:
+		plane_ctl |= PLANE_CTL_FORMAT_XRGB_8888 |
+			     PLANE_CTL_ALPHA_SW_PREMULTIPLY;
+		break;
+	case DRM_FORMAT_YUYV:
+		plane_ctl |= PLANE_CTL_FORMAT_YUV422 | PLANE_CTL_YUV422_YUYV;
+		break;
+	case DRM_FORMAT_YVYU:
+		plane_ctl |= PLANE_CTL_FORMAT_YUV422 | PLANE_CTL_YUV422_YVYU;
+		break;
+	case DRM_FORMAT_UYVY:
+		plane_ctl |= PLANE_CTL_FORMAT_YUV422 | PLANE_CTL_YUV422_UYVY;
+		break;
+	case DRM_FORMAT_VYUY:
+		plane_ctl |= PLANE_CTL_FORMAT_YUV422 | PLANE_CTL_YUV422_VYUY;
+		break;
+	default:
+		BUG();
+	}
+
+	switch (obj->tiling_mode) {
+	case I915_TILING_NONE:
+		stride = fb->pitches[0] >> 6;
+		break;
+	case I915_TILING_X:
+		plane_ctl |= PLANE_CTL_TILED_X;
+		stride = fb->pitches[0] >> 9;
+		break;
+	default:
+		BUG();
+	}
+	if (intel_plane->rotation == BIT(DRM_ROTATE_180))
+		plane_ctl |= PLANE_CTL_ROTATE_180;
+
+	plane_ctl |= PLANE_CTL_ENABLE;
+	plane_ctl |= PLANE_CTL_PIPE_CSC_ENABLE;
+
+	intel_update_sprite_watermarks(drm_plane, crtc, src_w, src_h,
+				       pixel_size, true,
+				       src_w != crtc_w || src_h != crtc_h);
+
+	/* Sizes are 0 based */
+	src_w--;
+	src_h--;
+	crtc_w--;
+	crtc_h--;
+
+	I915_WRITE(PLANE_OFFSET(pipe, plane), (y << 16) | x);
+	I915_WRITE(PLANE_STRIDE(pipe, plane), stride);
+	I915_WRITE(PLANE_POS(pipe, plane), (crtc_y << 16) | crtc_x);
+	I915_WRITE(PLANE_SIZE(pipe, plane), (crtc_h << 16) | crtc_w);
+	I915_WRITE(PLANE_CTL(pipe, plane), plane_ctl);
+	I915_WRITE(PLANE_SURF(pipe, plane), i915_gem_obj_ggtt_offset(obj));
+	POSTING_READ(PLANE_SURF(pipe, plane));
+}
+
+static void
+skl_disable_plane(struct drm_plane *drm_plane, struct drm_crtc *crtc)
+{
+	struct drm_device *dev = drm_plane->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_plane *intel_plane = to_intel_plane(drm_plane);
+	const int pipe = intel_plane->pipe;
+	const int plane = intel_plane->plane + 1;
+
+	I915_WRITE(PLANE_CTL(pipe, plane),
+		   I915_READ(PLANE_CTL(pipe, plane)) & ~PLANE_CTL_ENABLE);
+
+	/* Activate double buffered register update */
+	I915_WRITE(PLANE_CTL(pipe, plane), 0);
+	POSTING_READ(PLANE_CTL(pipe, plane));
+
+	intel_update_sprite_watermarks(drm_plane, crtc, 0, 0, 0, false, false);
+}
+
+static int
+skl_update_colorkey(struct drm_plane *drm_plane,
+		    struct drm_intel_sprite_colorkey *key)
+{
+	struct drm_device *dev = drm_plane->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_plane *intel_plane = to_intel_plane(drm_plane);
+	const int pipe = intel_plane->pipe;
+	const int plane = intel_plane->plane;
+	u32 plane_ctl;
+
+	I915_WRITE(PLANE_KEYVAL(pipe, plane), key->min_value);
+	I915_WRITE(PLANE_KEYMAX(pipe, plane), key->max_value);
+	I915_WRITE(PLANE_KEYMSK(pipe, plane), key->channel_mask);
+
+	plane_ctl = I915_READ(PLANE_CTL(pipe, plane));
+	plane_ctl &= ~PLANE_CTL_KEY_ENABLE_MASK;
+	if (key->flags & I915_SET_COLORKEY_DESTINATION)
+		plane_ctl |= PLANE_CTL_KEY_ENABLE_DESTINATION;
+	else if (key->flags & I915_SET_COLORKEY_SOURCE)
+		plane_ctl |= PLANE_CTL_KEY_ENABLE_SOURCE;
+	I915_WRITE(PLANE_CTL(pipe, plane), plane_ctl);
+
+	POSTING_READ(PLANE_CTL(pipe, plane));
+
+	return 0;
+}
+
+static void
+skl_get_colorkey(struct drm_plane *drm_plane,
+		 struct drm_intel_sprite_colorkey *key)
+{
+	struct drm_device *dev = drm_plane->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_plane *intel_plane = to_intel_plane(drm_plane);
+	const int pipe = intel_plane->pipe;
+	const int plane = intel_plane->plane;
+	u32 plane_ctl;
+
+	key->min_value = I915_READ(PLANE_KEYVAL(pipe, plane));
+	key->max_value = I915_READ(PLANE_KEYMAX(pipe, plane));
+	key->channel_mask = I915_READ(PLANE_KEYMSK(pipe, plane));
+
+	plane_ctl = I915_READ(PLANE_CTL(pipe, plane));
+
+	switch (plane_ctl & PLANE_CTL_KEY_ENABLE_MASK) {
+	case PLANE_CTL_KEY_ENABLE_DESTINATION:
+		key->flags = I915_SET_COLORKEY_DESTINATION;
+		break;
+	case PLANE_CTL_KEY_ENABLE_SOURCE:
+		key->flags = I915_SET_COLORKEY_SOURCE;
+		break;
+	default:
+		key->flags = I915_SET_COLORKEY_NONE;
+	}
+}
+
+static void
+chv_update_csc(struct intel_plane *intel_plane, uint32_t format)
+{
+	struct drm_i915_private *dev_priv = intel_plane->base.dev->dev_private;
+	int plane = intel_plane->plane;
+
+	/* Seems RGB data bypasses the CSC always */
+	if (!format_is_yuv(format))
+		return;
+
+	/*
+	 * BT.601 limited range YCbCr -> full range RGB
+	 *
+	 * |r|   | 6537 4769     0|   |cr  |
+	 * |g| = |-3330 4769 -1605| x |y-64|
+	 * |b|   |    0 4769  8263|   |cb  |
+	 *
+	 * Cb and Cr apparently come in as signed already, so no
+	 * need for any offset. For Y we need to remove the offset.
+	 */
+	I915_WRITE(SPCSCYGOFF(plane), SPCSC_OOFF(0) | SPCSC_IOFF(-64));
+	I915_WRITE(SPCSCCBOFF(plane), SPCSC_OOFF(0) | SPCSC_IOFF(0));
+	I915_WRITE(SPCSCCROFF(plane), SPCSC_OOFF(0) | SPCSC_IOFF(0));
+
+	I915_WRITE(SPCSCC01(plane), SPCSC_C1(4769) | SPCSC_C0(6537));
+	I915_WRITE(SPCSCC23(plane), SPCSC_C1(-3330) | SPCSC_C0(0));
+	I915_WRITE(SPCSCC45(plane), SPCSC_C1(-1605) | SPCSC_C0(4769));
+	I915_WRITE(SPCSCC67(plane), SPCSC_C1(4769) | SPCSC_C0(0));
+	I915_WRITE(SPCSCC8(plane), SPCSC_C0(8263));
+
+	I915_WRITE(SPCSCYGICLAMP(plane), SPCSC_IMAX(940) | SPCSC_IMIN(64));
+	I915_WRITE(SPCSCCBICLAMP(plane), SPCSC_IMAX(448) | SPCSC_IMIN(-448));
+	I915_WRITE(SPCSCCRICLAMP(plane), SPCSC_IMAX(448) | SPCSC_IMIN(-448));
+
+	I915_WRITE(SPCSCYGOCLAMP(plane), SPCSC_OMAX(1023) | SPCSC_OMIN(0));
+	I915_WRITE(SPCSCCBOCLAMP(plane), SPCSC_OMAX(1023) | SPCSC_OMIN(0));
+	I915_WRITE(SPCSCCROCLAMP(plane), SPCSC_OMAX(1023) | SPCSC_OMIN(0));
+}
+
+static void
 vlv_update_plane(struct drm_plane *dplane, struct drm_crtc *crtc,
 		 struct drm_framebuffer *fb,
 		 struct drm_i915_gem_object *obj, int crtc_x, int crtc_y,
@@ -249,6 +506,9 @@ vlv_update_plane(struct drm_plane *dplane, struct drm_crtc *crtc,
 
 	intel_update_primary_plane(intel_crtc);
 
+	if (IS_CHERRYVIEW(dev) && pipe == PIPE_B)
+		chv_update_csc(intel_plane, fb->pixel_format);
+
 	I915_WRITE(SPSTRIDE(pipe, plane), fb->pitches[0]);
 	I915_WRITE(SPPOS(pipe, plane), (crtc_y << 16) | crtc_x);
 
@@ -257,6 +517,8 @@ vlv_update_plane(struct drm_plane *dplane, struct drm_crtc *crtc,
 	else
 		I915_WRITE(SPLINOFF(pipe, plane), linear_offset);
 
+	I915_WRITE(SPCONSTALPHA(pipe, plane), 0);
+
 	I915_WRITE(SPSIZE(pipe, plane), (crtc_h << 16) | crtc_w);
 	I915_WRITE(SPCNTR(pipe, plane), sprctl);
 	I915_WRITE(SPSURF(pipe, plane), i915_gem_obj_ggtt_offset(obj) +
@@ -821,20 +1083,6 @@ ilk_get_colorkey(struct drm_plane *plane, struct drm_intel_sprite_colorkey *key)
 		key->flags = I915_SET_COLORKEY_NONE;
 }
 
-static bool
-format_is_yuv(uint32_t format)
-{
-	switch (format) {
-	case DRM_FORMAT_YUYV:
-	case DRM_FORMAT_UYVY:
-	case DRM_FORMAT_VYUY:
-	case DRM_FORMAT_YVYU:
-		return true;
-	default:
-		return false;
-	}
-}
-
 static bool colorkey_enabled(struct intel_plane *intel_plane)
 {
 	struct drm_intel_sprite_colorkey key;
@@ -845,57 +1093,23 @@ static bool colorkey_enabled(struct intel_plane *intel_plane)
 }
 
 static int
-intel_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
-		   struct drm_framebuffer *fb, int crtc_x, int crtc_y,
-		   unsigned int crtc_w, unsigned int crtc_h,
-		   uint32_t src_x, uint32_t src_y,
-		   uint32_t src_w, uint32_t src_h)
+intel_check_sprite_plane(struct drm_plane *plane,
+			 struct intel_plane_state *state)
 {
-	struct drm_device *dev = plane->dev;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_crtc *intel_crtc = to_intel_crtc(state->crtc);
 	struct intel_plane *intel_plane = to_intel_plane(plane);
-	enum pipe pipe = intel_crtc->pipe;
-	struct intel_framebuffer *intel_fb = to_intel_framebuffer(fb);
-	struct drm_i915_gem_object *obj = intel_fb->obj;
-	struct drm_i915_gem_object *old_obj = intel_plane->obj;
-	int ret;
-	bool primary_enabled;
-	bool visible;
+	struct drm_framebuffer *fb = state->fb;
+	struct drm_i915_gem_object *obj = intel_fb_obj(fb);
+	int crtc_x, crtc_y;
+	unsigned int crtc_w, crtc_h;
+	uint32_t src_x, src_y, src_w, src_h;
+	struct drm_rect *src = &state->src;
+	struct drm_rect *dst = &state->dst;
+	struct drm_rect *orig_src = &state->orig_src;
+	const struct drm_rect *clip = &state->clip;
 	int hscale, vscale;
 	int max_scale, min_scale;
 	int pixel_size = drm_format_plane_cpp(fb->pixel_format, 0);
-	struct drm_rect src = {
-		/* sample coordinates in 16.16 fixed point */
-		.x1 = src_x,
-		.x2 = src_x + src_w,
-		.y1 = src_y,
-		.y2 = src_y + src_h,
-	};
-	struct drm_rect dst = {
-		/* integer pixels */
-		.x1 = crtc_x,
-		.x2 = crtc_x + crtc_w,
-		.y1 = crtc_y,
-		.y2 = crtc_y + crtc_h,
-	};
-	const struct drm_rect clip = {
-		.x2 = intel_crtc->active ? intel_crtc->config.pipe_src_w : 0,
-		.y2 = intel_crtc->active ? intel_crtc->config.pipe_src_h : 0,
-	};
-	const struct {
-		int crtc_x, crtc_y;
-		unsigned int crtc_w, crtc_h;
-		uint32_t src_x, src_y, src_w, src_h;
-	} orig = {
-		.crtc_x = crtc_x,
-		.crtc_y = crtc_y,
-		.crtc_w = crtc_w,
-		.crtc_h = crtc_h,
-		.src_x = src_x,
-		.src_y = src_y,
-		.src_w = src_w,
-		.src_h = src_h,
-	};
 
 	/* Don't modify another pipe's plane */
 	if (intel_plane->pipe != intel_crtc->pipe) {
@@ -927,55 +1141,55 @@ intel_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 	max_scale = intel_plane->max_downscale << 16;
 	min_scale = intel_plane->can_scale ? 1 : (1 << 16);
 
-	drm_rect_rotate(&src, fb->width << 16, fb->height << 16,
+	drm_rect_rotate(src, fb->width << 16, fb->height << 16,
 			intel_plane->rotation);
 
-	hscale = drm_rect_calc_hscale_relaxed(&src, &dst, min_scale, max_scale);
+	hscale = drm_rect_calc_hscale_relaxed(src, dst, min_scale, max_scale);
 	BUG_ON(hscale < 0);
 
-	vscale = drm_rect_calc_vscale_relaxed(&src, &dst, min_scale, max_scale);
+	vscale = drm_rect_calc_vscale_relaxed(src, dst, min_scale, max_scale);
 	BUG_ON(vscale < 0);
 
-	visible = drm_rect_clip_scaled(&src, &dst, &clip, hscale, vscale);
+	state->visible =  drm_rect_clip_scaled(src, dst, clip, hscale, vscale);
 
-	crtc_x = dst.x1;
-	crtc_y = dst.y1;
-	crtc_w = drm_rect_width(&dst);
-	crtc_h = drm_rect_height(&dst);
+	crtc_x = dst->x1;
+	crtc_y = dst->y1;
+	crtc_w = drm_rect_width(dst);
+	crtc_h = drm_rect_height(dst);
 
-	if (visible) {
+	if (state->visible) {
 		/* check again in case clipping clamped the results */
-		hscale = drm_rect_calc_hscale(&src, &dst, min_scale, max_scale);
+		hscale = drm_rect_calc_hscale(src, dst, min_scale, max_scale);
 		if (hscale < 0) {
 			DRM_DEBUG_KMS("Horizontal scaling factor out of limits\n");
-			drm_rect_debug_print(&src, true);
-			drm_rect_debug_print(&dst, false);
+			drm_rect_debug_print(src, true);
+			drm_rect_debug_print(dst, false);
 
 			return hscale;
 		}
 
-		vscale = drm_rect_calc_vscale(&src, &dst, min_scale, max_scale);
+		vscale = drm_rect_calc_vscale(src, dst, min_scale, max_scale);
 		if (vscale < 0) {
 			DRM_DEBUG_KMS("Vertical scaling factor out of limits\n");
-			drm_rect_debug_print(&src, true);
-			drm_rect_debug_print(&dst, false);
+			drm_rect_debug_print(src, true);
+			drm_rect_debug_print(dst, false);
 
 			return vscale;
 		}
 
 		/* Make the source viewport size an exact multiple of the scaling factors. */
-		drm_rect_adjust_size(&src,
-				     drm_rect_width(&dst) * hscale - drm_rect_width(&src),
-				     drm_rect_height(&dst) * vscale - drm_rect_height(&src));
+		drm_rect_adjust_size(src,
+				     drm_rect_width(dst) * hscale - drm_rect_width(src),
+				     drm_rect_height(dst) * vscale - drm_rect_height(src));
 
-		drm_rect_rotate_inv(&src, fb->width << 16, fb->height << 16,
+		drm_rect_rotate_inv(src, fb->width << 16, fb->height << 16,
 				    intel_plane->rotation);
 
 		/* sanity check to make sure the src viewport wasn't enlarged */
-		WARN_ON(src.x1 < (int) src_x ||
-			src.y1 < (int) src_y ||
-			src.x2 > (int) (src_x + src_w) ||
-			src.y2 > (int) (src_y + src_h));
+		WARN_ON(src->x1 < (int) orig_src->x1 ||
+			src->y1 < (int) orig_src->y1 ||
+			src->x2 > (int) orig_src->x2 ||
+			src->y2 > (int) orig_src->y2);
 
 		/*
 		 * Hardware doesn't handle subpixel coordinates.
@@ -983,10 +1197,10 @@ intel_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 		 * increase the source viewport size, because that could
 		 * push the downscaling factor out of bounds.
 		 */
-		src_x = src.x1 >> 16;
-		src_w = drm_rect_width(&src) >> 16;
-		src_y = src.y1 >> 16;
-		src_h = drm_rect_height(&src) >> 16;
+		src_x = src->x1 >> 16;
+		src_w = drm_rect_width(src) >> 16;
+		src_y = src->y1 >> 16;
+		src_h = drm_rect_height(src) >> 16;
 
 		if (format_is_yuv(fb->pixel_format)) {
 			src_x &= ~1;
@@ -1000,12 +1214,12 @@ intel_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 				crtc_w &= ~1;
 
 			if (crtc_w == 0)
-				visible = false;
+				state->visible = false;
 		}
 	}
 
 	/* Check size restrictions when scaling */
-	if (visible && (src_w != crtc_w || src_h != crtc_h)) {
+	if (state->visible && (src_w != crtc_w || src_h != crtc_h)) {
 		unsigned int width_bytes;
 
 		WARN_ON(!intel_plane->can_scale);
@@ -1013,12 +1227,13 @@ intel_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 		/* FIXME interlacing min height is 6 */
 
 		if (crtc_w < 3 || crtc_h < 3)
-			visible = false;
+			state->visible = false;
 
 		if (src_w < 3 || src_h < 3)
-			visible = false;
+			state->visible = false;
 
-		width_bytes = ((src_x * pixel_size) & 63) + src_w * pixel_size;
+		width_bytes = ((src_x * pixel_size) & 63) +
+					src_w * pixel_size;
 
 		if (src_w > 2048 || src_h > 2048 ||
 		    width_bytes > 4096 || fb->pitches[0] > 4096) {
@@ -1027,42 +1242,90 @@ intel_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 		}
 	}
 
-	dst.x1 = crtc_x;
-	dst.x2 = crtc_x + crtc_w;
-	dst.y1 = crtc_y;
-	dst.y2 = crtc_y + crtc_h;
+	if (state->visible) {
+		src->x1 = src_x;
+		src->x2 = src_x + src_w;
+		src->y1 = src_y;
+		src->y2 = src_y + src_h;
+	}
 
-	/*
-	 * If the sprite is completely covering the primary plane,
-	 * we can disable the primary and save power.
-	 */
-	primary_enabled = !drm_rect_equals(&dst, &clip) || colorkey_enabled(intel_plane);
-	WARN_ON(!primary_enabled && !visible && intel_crtc->active);
+	dst->x1 = crtc_x;
+	dst->x2 = crtc_x + crtc_w;
+	dst->y1 = crtc_y;
+	dst->y2 = crtc_y + crtc_h;
 
-	mutex_lock(&dev->struct_mutex);
+	return 0;
+}
 
-	/* Note that this will apply the VT-d workaround for scanouts,
-	 * which is more restrictive than required for sprites. (The
-	 * primary plane requires 256KiB alignment with 64 PTE padding,
-	 * the sprite planes only require 128KiB alignment and 32 PTE padding.
-	 */
-	ret = intel_pin_and_fence_fb_obj(dev, obj, NULL);
+static int
+intel_prepare_sprite_plane(struct drm_plane *plane,
+			   struct intel_plane_state *state)
+{
+	struct drm_device *dev = plane->dev;
+	struct drm_crtc *crtc = state->crtc;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_plane *intel_plane = to_intel_plane(plane);
+	enum pipe pipe = intel_crtc->pipe;
+	struct drm_framebuffer *fb = state->fb;
+	struct drm_i915_gem_object *obj = intel_fb_obj(fb);
+	struct drm_i915_gem_object *old_obj = intel_plane->obj;
+	int ret;
 
-	i915_gem_track_fb(old_obj, obj,
-			  INTEL_FRONTBUFFER_SPRITE(pipe));
-	mutex_unlock(&dev->struct_mutex);
+	if (old_obj != obj) {
+		mutex_lock(&dev->struct_mutex);
 
-	if (ret)
-		return ret;
+		/* Note that this will apply the VT-d workaround for scanouts,
+		 * which is more restrictive than required for sprites. (The
+		 * primary plane requires 256KiB alignment with 64 PTE padding,
+		 * the sprite planes only require 128KiB alignment and 32 PTE
+		 * padding.
+		 */
+		ret = intel_pin_and_fence_fb_obj(plane, fb, NULL);
+		if (ret == 0)
+			i915_gem_track_fb(old_obj, obj,
+					  INTEL_FRONTBUFFER_SPRITE(pipe));
+		mutex_unlock(&dev->struct_mutex);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
 
-	intel_plane->crtc_x = orig.crtc_x;
-	intel_plane->crtc_y = orig.crtc_y;
-	intel_plane->crtc_w = orig.crtc_w;
-	intel_plane->crtc_h = orig.crtc_h;
-	intel_plane->src_x = orig.src_x;
-	intel_plane->src_y = orig.src_y;
-	intel_plane->src_w = orig.src_w;
-	intel_plane->src_h = orig.src_h;
+static void
+intel_commit_sprite_plane(struct drm_plane *plane,
+			  struct intel_plane_state *state)
+{
+	struct drm_device *dev = plane->dev;
+	struct drm_crtc *crtc = state->crtc;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_plane *intel_plane = to_intel_plane(plane);
+	enum pipe pipe = intel_crtc->pipe;
+	struct drm_framebuffer *fb = state->fb;
+	struct drm_i915_gem_object *obj = intel_fb_obj(fb);
+	struct drm_i915_gem_object *old_obj = intel_plane->obj;
+	int crtc_x, crtc_y;
+	unsigned int crtc_w, crtc_h;
+	uint32_t src_x, src_y, src_w, src_h;
+	struct drm_rect *dst = &state->dst;
+	const struct drm_rect *clip = &state->clip;
+	bool primary_enabled;
+
+	/*
+	 * If the sprite is completely covering the primary plane,
+	 * we can disable the primary and save power.
+	 */
+	primary_enabled = !drm_rect_equals(dst, clip) || colorkey_enabled(intel_plane);
+	WARN_ON(!primary_enabled && !state->visible && intel_crtc->active);
+
+	intel_plane->crtc_x = state->orig_dst.x1;
+	intel_plane->crtc_y = state->orig_dst.y1;
+	intel_plane->crtc_w = drm_rect_width(&state->orig_dst);
+	intel_plane->crtc_h = drm_rect_height(&state->orig_dst);
+	intel_plane->src_x = state->orig_src.x1;
+	intel_plane->src_y = state->orig_src.y1;
+	intel_plane->src_w = drm_rect_width(&state->orig_src);
+	intel_plane->src_h = drm_rect_height(&state->orig_src);
 	intel_plane->obj = obj;
 
 	if (intel_crtc->active) {
@@ -1076,12 +1339,22 @@ intel_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 		if (primary_was_enabled && !primary_enabled)
 			intel_pre_disable_primary(crtc);
 
-		if (visible)
+		if (state->visible) {
+			crtc_x = state->dst.x1;
+			crtc_y = state->dst.y1;
+			crtc_w = drm_rect_width(&state->dst);
+			crtc_h = drm_rect_height(&state->dst);
+			src_x = state->src.x1;
+			src_y = state->src.y1;
+			src_w = drm_rect_width(&state->src);
+			src_h = drm_rect_height(&state->src);
 			intel_plane->update_plane(plane, crtc, fb, obj,
 						  crtc_x, crtc_y, crtc_w, crtc_h,
 						  src_x, src_y, src_w, src_h);
-		else
+		} else {
 			intel_plane->disable_plane(plane, crtc);
+		}
+
 
 		intel_frontbuffer_flip(dev, INTEL_FRONTBUFFER_SPRITE(pipe));
 
@@ -1090,21 +1363,65 @@ intel_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 	}
 
 	/* Unpin old obj after new one is active to avoid ugliness */
-	if (old_obj) {
+	if (old_obj && old_obj != obj) {
+
 		/*
 		 * It's fairly common to simply update the position of
 		 * an existing object.  In that case, we don't need to
 		 * wait for vblank to avoid ugliness, we only need to
 		 * do the pin & ref bookkeeping.
 		 */
-		if (old_obj != obj && intel_crtc->active)
+		if (intel_crtc->active)
 			intel_wait_for_vblank(dev, intel_crtc->pipe);
 
 		mutex_lock(&dev->struct_mutex);
 		intel_unpin_fb_obj(old_obj);
 		mutex_unlock(&dev->struct_mutex);
 	}
+}
+
+static int
+intel_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
+		   struct drm_framebuffer *fb, int crtc_x, int crtc_y,
+		   unsigned int crtc_w, unsigned int crtc_h,
+		   uint32_t src_x, uint32_t src_y,
+		   uint32_t src_w, uint32_t src_h)
+{
+	struct intel_plane_state state;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int ret;
 
+	state.crtc = crtc;
+	state.fb = fb;
+
+	/* sample coordinates in 16.16 fixed point */
+	state.src.x1 = src_x;
+	state.src.x2 = src_x + src_w;
+	state.src.y1 = src_y;
+	state.src.y2 = src_y + src_h;
+
+	/* integer pixels */
+	state.dst.x1 = crtc_x;
+	state.dst.x2 = crtc_x + crtc_w;
+	state.dst.y1 = crtc_y;
+	state.dst.y2 = crtc_y + crtc_h;
+
+	state.clip.x1 = 0;
+	state.clip.y1 = 0;
+	state.clip.x2 = intel_crtc->active ? intel_crtc->config.pipe_src_w : 0;
+	state.clip.y2 = intel_crtc->active ? intel_crtc->config.pipe_src_h : 0;
+	state.orig_src = state.src;
+	state.orig_dst = state.dst;
+
+	ret = intel_check_sprite_plane(plane, &state);
+	if (ret)
+		return ret;
+
+	ret = intel_prepare_sprite_plane(plane, &state);
+	if (ret)
+		return ret;
+
+	intel_commit_sprite_plane(plane, &state);
 	return 0;
 }
 
@@ -1305,6 +1622,18 @@ static uint32_t vlv_plane_formats[] = {
 	DRM_FORMAT_VYUY,
 };
 
+static uint32_t skl_plane_formats[] = {
+	DRM_FORMAT_RGB565,
+	DRM_FORMAT_ABGR8888,
+	DRM_FORMAT_ARGB8888,
+	DRM_FORMAT_XBGR8888,
+	DRM_FORMAT_XRGB8888,
+	DRM_FORMAT_YUYV,
+	DRM_FORMAT_YVYU,
+	DRM_FORMAT_UYVY,
+	DRM_FORMAT_VYUY,
+};
+
 int
 intel_plane_init(struct drm_device *dev, enum pipe pipe, int plane)
 {
@@ -1368,7 +1697,21 @@ intel_plane_init(struct drm_device *dev, enum pipe pipe, int plane)
 			num_plane_formats = ARRAY_SIZE(snb_plane_formats);
 		}
 		break;
-
+	case 9:
+		/*
+		 * FIXME: Skylake planes can be scaled (with some restrictions),
+		 * but this is for another time.
+		 */
+		intel_plane->can_scale = false;
+		intel_plane->max_downscale = 1;
+		intel_plane->update_plane = skl_update_plane;
+		intel_plane->disable_plane = skl_disable_plane;
+		intel_plane->update_colorkey = skl_update_colorkey;
+		intel_plane->get_colorkey = skl_get_colorkey;
+
+		plane_formats = skl_plane_formats;
+		num_plane_formats = ARRAY_SIZE(skl_plane_formats);
+		break;
 	default:
 		kfree(intel_plane);
 		return -ENODEV;
diff --git a/drivers/gpu/drm/i915/intel_tv.c b/drivers/gpu/drm/i915/intel_tv.c
index c14341ca3ef9..6f5f59b880f5 100644
--- a/drivers/gpu/drm/i915/intel_tv.c
+++ b/drivers/gpu/drm/i915/intel_tv.c
@@ -1182,18 +1182,17 @@ intel_tv_detect_type(struct intel_tv *intel_tv,
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct drm_device *dev = encoder->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	unsigned long irqflags;
 	u32 tv_ctl, save_tv_ctl;
 	u32 tv_dac, save_tv_dac;
 	int type;
 
 	/* Disable TV interrupts around load detect or we'll recurse */
 	if (connector->polled & DRM_CONNECTOR_POLL_HPD) {
-		spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+		spin_lock_irq(&dev_priv->irq_lock);
 		i915_disable_pipestat(dev_priv, 0,
 				      PIPE_HOTPLUG_INTERRUPT_STATUS |
 				      PIPE_HOTPLUG_TV_INTERRUPT_STATUS);
-		spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+		spin_unlock_irq(&dev_priv->irq_lock);
 	}
 
 	save_tv_dac = tv_dac = I915_READ(TV_DAC);
@@ -1266,11 +1265,11 @@ intel_tv_detect_type(struct intel_tv *intel_tv,
 
 	/* Restore interrupt config */
 	if (connector->polled & DRM_CONNECTOR_POLL_HPD) {
-		spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+		spin_lock_irq(&dev_priv->irq_lock);
 		i915_enable_pipestat(dev_priv, 0,
 				     PIPE_HOTPLUG_INTERRUPT_STATUS |
 				     PIPE_HOTPLUG_TV_INTERRUPT_STATUS);
-		spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+		spin_unlock_irq(&dev_priv->irq_lock);
 	}
 
 	return type;
diff --git a/drivers/gpu/drm/i915/intel_uncore.c b/drivers/gpu/drm/i915/intel_uncore.c
index 918b76163965..46de8d75b4bf 100644
--- a/drivers/gpu/drm/i915/intel_uncore.c
+++ b/drivers/gpu/drm/i915/intel_uncore.c
@@ -43,23 +43,17 @@
 static void
 assert_device_not_suspended(struct drm_i915_private *dev_priv)
 {
-	WARN(HAS_RUNTIME_PM(dev_priv->dev) && dev_priv->pm.suspended,
-	     "Device suspended\n");
+	WARN_ONCE(HAS_RUNTIME_PM(dev_priv->dev) && dev_priv->pm.suspended,
+		  "Device suspended\n");
 }
 
 static void __gen6_gt_wait_for_thread_c0(struct drm_i915_private *dev_priv)
 {
-	u32 gt_thread_status_mask;
-
-	if (IS_HASWELL(dev_priv->dev))
-		gt_thread_status_mask = GEN6_GT_THREAD_STATUS_CORE_MASK_HSW;
-	else
-		gt_thread_status_mask = GEN6_GT_THREAD_STATUS_CORE_MASK;
-
 	/* w/a for a sporadic read returning 0 by waiting for the GT
 	 * thread to wake up.
 	 */
-	if (wait_for_atomic_us((__raw_i915_read32(dev_priv, GEN6_GT_THREAD_STATUS_REG) & gt_thread_status_mask) == 0, 500))
+	if (wait_for_atomic_us((__raw_i915_read32(dev_priv, GEN6_GT_THREAD_STATUS_REG) &
+				GEN6_GT_THREAD_STATUS_CORE_MASK) == 0, 500))
 		DRM_ERROR("GT thread status wait timed out\n");
 }
 
@@ -120,8 +114,7 @@ static void __gen7_gt_force_wake_mt_get(struct drm_i915_private *dev_priv,
 		DRM_ERROR("Timed out waiting for forcewake to ack request.\n");
 
 	/* WaRsForcewakeWaitTC0:ivb,hsw */
-	if (INTEL_INFO(dev_priv->dev)->gen < 8)
-		__gen6_gt_wait_for_thread_c0(dev_priv);
+	__gen6_gt_wait_for_thread_c0(dev_priv);
 }
 
 static void gen6_gt_check_fifodbg(struct drm_i915_private *dev_priv)
@@ -229,10 +222,6 @@ static void __vlv_force_wake_get(struct drm_i915_private *dev_priv,
 					FORCEWAKE_ACK_TIMEOUT_MS))
 			DRM_ERROR("Timed out: waiting for media to ack.\n");
 	}
-
-	/* WaRsForcewakeWaitTC0:vlv */
-	if (!IS_CHERRYVIEW(dev_priv->dev))
-		__gen6_gt_wait_for_thread_c0(dev_priv);
 }
 
 static void __vlv_force_wake_put(struct drm_i915_private *dev_priv,
@@ -299,6 +288,154 @@ static void vlv_force_wake_put(struct drm_i915_private *dev_priv, int fw_engine)
 	spin_unlock_irqrestore(&dev_priv->uncore.lock, irqflags);
 }
 
+static void __gen9_gt_force_wake_mt_reset(struct drm_i915_private *dev_priv)
+{
+	__raw_i915_write32(dev_priv, FORCEWAKE_RENDER_GEN9,
+			_MASKED_BIT_DISABLE(0xffff));
+
+	__raw_i915_write32(dev_priv, FORCEWAKE_MEDIA_GEN9,
+			_MASKED_BIT_DISABLE(0xffff));
+
+	__raw_i915_write32(dev_priv, FORCEWAKE_BLITTER_GEN9,
+			_MASKED_BIT_DISABLE(0xffff));
+}
+
+static void
+__gen9_force_wake_get(struct drm_i915_private *dev_priv, int fw_engine)
+{
+	/* Check for Render Engine */
+	if (FORCEWAKE_RENDER & fw_engine) {
+		if (wait_for_atomic((__raw_i915_read32(dev_priv,
+						FORCEWAKE_ACK_RENDER_GEN9) &
+						FORCEWAKE_KERNEL) == 0,
+					FORCEWAKE_ACK_TIMEOUT_MS))
+			DRM_ERROR("Timed out: Render forcewake old ack to clear.\n");
+
+		__raw_i915_write32(dev_priv, FORCEWAKE_RENDER_GEN9,
+				   _MASKED_BIT_ENABLE(FORCEWAKE_KERNEL));
+
+		if (wait_for_atomic((__raw_i915_read32(dev_priv,
+						FORCEWAKE_ACK_RENDER_GEN9) &
+						FORCEWAKE_KERNEL),
+					FORCEWAKE_ACK_TIMEOUT_MS))
+			DRM_ERROR("Timed out: waiting for Render to ack.\n");
+	}
+
+	/* Check for Media Engine */
+	if (FORCEWAKE_MEDIA & fw_engine) {
+		if (wait_for_atomic((__raw_i915_read32(dev_priv,
+						FORCEWAKE_ACK_MEDIA_GEN9) &
+						FORCEWAKE_KERNEL) == 0,
+					FORCEWAKE_ACK_TIMEOUT_MS))
+			DRM_ERROR("Timed out: Media forcewake old ack to clear.\n");
+
+		__raw_i915_write32(dev_priv, FORCEWAKE_MEDIA_GEN9,
+				   _MASKED_BIT_ENABLE(FORCEWAKE_KERNEL));
+
+		if (wait_for_atomic((__raw_i915_read32(dev_priv,
+						FORCEWAKE_ACK_MEDIA_GEN9) &
+						FORCEWAKE_KERNEL),
+					FORCEWAKE_ACK_TIMEOUT_MS))
+			DRM_ERROR("Timed out: waiting for Media to ack.\n");
+	}
+
+	/* Check for Blitter Engine */
+	if (FORCEWAKE_BLITTER & fw_engine) {
+		if (wait_for_atomic((__raw_i915_read32(dev_priv,
+						FORCEWAKE_ACK_BLITTER_GEN9) &
+						FORCEWAKE_KERNEL) == 0,
+					FORCEWAKE_ACK_TIMEOUT_MS))
+			DRM_ERROR("Timed out: Blitter forcewake old ack to clear.\n");
+
+		__raw_i915_write32(dev_priv, FORCEWAKE_BLITTER_GEN9,
+				   _MASKED_BIT_ENABLE(FORCEWAKE_KERNEL));
+
+		if (wait_for_atomic((__raw_i915_read32(dev_priv,
+						FORCEWAKE_ACK_BLITTER_GEN9) &
+						FORCEWAKE_KERNEL),
+					FORCEWAKE_ACK_TIMEOUT_MS))
+			DRM_ERROR("Timed out: waiting for Blitter to ack.\n");
+	}
+}
+
+static void
+__gen9_force_wake_put(struct drm_i915_private *dev_priv, int fw_engine)
+{
+	/* Check for Render Engine */
+	if (FORCEWAKE_RENDER & fw_engine)
+		__raw_i915_write32(dev_priv, FORCEWAKE_RENDER_GEN9,
+				_MASKED_BIT_DISABLE(FORCEWAKE_KERNEL));
+
+	/* Check for Media Engine */
+	if (FORCEWAKE_MEDIA & fw_engine)
+		__raw_i915_write32(dev_priv, FORCEWAKE_MEDIA_GEN9,
+				_MASKED_BIT_DISABLE(FORCEWAKE_KERNEL));
+
+	/* Check for Blitter Engine */
+	if (FORCEWAKE_BLITTER & fw_engine)
+		__raw_i915_write32(dev_priv, FORCEWAKE_BLITTER_GEN9,
+				_MASKED_BIT_DISABLE(FORCEWAKE_KERNEL));
+}
+
+static void
+gen9_force_wake_get(struct drm_i915_private *dev_priv, int fw_engine)
+{
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
+
+	if (FORCEWAKE_RENDER & fw_engine) {
+		if (dev_priv->uncore.fw_rendercount++ == 0)
+			dev_priv->uncore.funcs.force_wake_get(dev_priv,
+							FORCEWAKE_RENDER);
+	}
+
+	if (FORCEWAKE_MEDIA & fw_engine) {
+		if (dev_priv->uncore.fw_mediacount++ == 0)
+			dev_priv->uncore.funcs.force_wake_get(dev_priv,
+							FORCEWAKE_MEDIA);
+	}
+
+	if (FORCEWAKE_BLITTER & fw_engine) {
+		if (dev_priv->uncore.fw_blittercount++ == 0)
+			dev_priv->uncore.funcs.force_wake_get(dev_priv,
+							FORCEWAKE_BLITTER);
+	}
+
+	spin_unlock_irqrestore(&dev_priv->uncore.lock, irqflags);
+}
+
+static void
+gen9_force_wake_put(struct drm_i915_private *dev_priv, int fw_engine)
+{
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
+
+	if (FORCEWAKE_RENDER & fw_engine) {
+		WARN_ON(dev_priv->uncore.fw_rendercount == 0);
+		if (--dev_priv->uncore.fw_rendercount == 0)
+			dev_priv->uncore.funcs.force_wake_put(dev_priv,
+							FORCEWAKE_RENDER);
+	}
+
+	if (FORCEWAKE_MEDIA & fw_engine) {
+		WARN_ON(dev_priv->uncore.fw_mediacount == 0);
+		if (--dev_priv->uncore.fw_mediacount == 0)
+			dev_priv->uncore.funcs.force_wake_put(dev_priv,
+							FORCEWAKE_MEDIA);
+	}
+
+	if (FORCEWAKE_BLITTER & fw_engine) {
+		WARN_ON(dev_priv->uncore.fw_blittercount == 0);
+		if (--dev_priv->uncore.fw_blittercount == 0)
+			dev_priv->uncore.funcs.force_wake_put(dev_priv,
+							FORCEWAKE_BLITTER);
+	}
+
+	spin_unlock_irqrestore(&dev_priv->uncore.lock, irqflags);
+}
+
 static void gen6_force_wake_timer(unsigned long arg)
 {
 	struct drm_i915_private *dev_priv = (void *)arg;
@@ -337,6 +474,9 @@ void intel_uncore_forcewake_reset(struct drm_device *dev, bool restore)
 	if (IS_IVYBRIDGE(dev) || IS_HASWELL(dev) || IS_BROADWELL(dev))
 		__gen7_gt_force_wake_mt_reset(dev_priv);
 
+	if (IS_GEN9(dev))
+		__gen9_gt_force_wake_mt_reset(dev_priv);
+
 	if (restore) { /* If reset with a user forcewake, try to restore */
 		unsigned fw = 0;
 
@@ -346,6 +486,15 @@ void intel_uncore_forcewake_reset(struct drm_device *dev, bool restore)
 
 			if (dev_priv->uncore.fw_mediacount)
 				fw |= FORCEWAKE_MEDIA;
+		} else if (IS_GEN9(dev)) {
+			if (dev_priv->uncore.fw_rendercount)
+				fw |= FORCEWAKE_RENDER;
+
+			if (dev_priv->uncore.fw_mediacount)
+				fw |= FORCEWAKE_MEDIA;
+
+			if (dev_priv->uncore.fw_blittercount)
+				fw |= FORCEWAKE_BLITTER;
 		} else {
 			if (dev_priv->uncore.forcewake_count)
 				fw = FORCEWAKE_ALL;
@@ -363,7 +512,8 @@ void intel_uncore_forcewake_reset(struct drm_device *dev, bool restore)
 	spin_unlock_irqrestore(&dev_priv->uncore.lock, irqflags);
 }
 
-void intel_uncore_early_sanitize(struct drm_device *dev, bool restore_forcewake)
+static void __intel_uncore_early_sanitize(struct drm_device *dev,
+					  bool restore_forcewake)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
@@ -389,6 +539,12 @@ void intel_uncore_early_sanitize(struct drm_device *dev, bool restore_forcewake)
 	intel_uncore_forcewake_reset(dev, restore_forcewake);
 }
 
+void intel_uncore_early_sanitize(struct drm_device *dev, bool restore_forcewake)
+{
+	__intel_uncore_early_sanitize(dev, restore_forcewake);
+	i915_check_and_clear_faults(dev);
+}
+
 void intel_uncore_sanitize(struct drm_device *dev)
 {
 	/* BIOS often leaves RC6 enabled, but disable it for hw init */
@@ -410,6 +566,10 @@ void gen6_gt_force_wake_get(struct drm_i915_private *dev_priv, int fw_engine)
 
 	intel_runtime_pm_get(dev_priv);
 
+	/* Redirect to Gen9 specific routine */
+	if (IS_GEN9(dev_priv->dev))
+		return gen9_force_wake_get(dev_priv, fw_engine);
+
 	/* Redirect to VLV specific routine */
 	if (IS_VALLEYVIEW(dev_priv->dev))
 		return vlv_force_wake_get(dev_priv, fw_engine);
@@ -431,6 +591,12 @@ void gen6_gt_force_wake_put(struct drm_i915_private *dev_priv, int fw_engine)
 	if (!dev_priv->uncore.funcs.force_wake_put)
 		return;
 
+	/* Redirect to Gen9 specific routine */
+	if (IS_GEN9(dev_priv->dev)) {
+		gen9_force_wake_put(dev_priv, fw_engine);
+		goto out;
+	}
+
 	/* Redirect to VLV specific routine */
 	if (IS_VALLEYVIEW(dev_priv->dev)) {
 		vlv_force_wake_put(dev_priv, fw_engine);
@@ -504,6 +670,38 @@ void assert_force_wake_inactive(struct drm_i915_private *dev_priv)
 	 REG_RANGE((reg), 0x14000, 0x14400) || \
 	 REG_RANGE((reg), 0x22000, 0x24000))
 
+#define FORCEWAKE_GEN9_UNCORE_RANGE_OFFSET(reg) \
+	REG_RANGE((reg), 0xB00,  0x2000)
+
+#define FORCEWAKE_GEN9_RENDER_RANGE_OFFSET(reg) \
+	(REG_RANGE((reg), 0x2000, 0x2700) || \
+	 REG_RANGE((reg), 0x3000, 0x4000) || \
+	 REG_RANGE((reg), 0x5200, 0x8000) || \
+	 REG_RANGE((reg), 0x8140, 0x8160) || \
+	 REG_RANGE((reg), 0x8300, 0x8500) || \
+	 REG_RANGE((reg), 0x8C00, 0x8D00) || \
+	 REG_RANGE((reg), 0xB000, 0xB480) || \
+	 REG_RANGE((reg), 0xE000, 0xE900) || \
+	 REG_RANGE((reg), 0x24400, 0x24800))
+
+#define FORCEWAKE_GEN9_MEDIA_RANGE_OFFSET(reg) \
+	(REG_RANGE((reg), 0x8130, 0x8140) || \
+	 REG_RANGE((reg), 0x8800, 0x8A00) || \
+	 REG_RANGE((reg), 0xD000, 0xD800) || \
+	 REG_RANGE((reg), 0x12000, 0x14000) || \
+	 REG_RANGE((reg), 0x1A000, 0x1EA00) || \
+	 REG_RANGE((reg), 0x30000, 0x40000))
+
+#define FORCEWAKE_GEN9_COMMON_RANGE_OFFSET(reg) \
+	REG_RANGE((reg), 0x9400, 0x9800)
+
+#define FORCEWAKE_GEN9_BLITTER_RANGE_OFFSET(reg) \
+	((reg) < 0x40000 &&\
+	 !FORCEWAKE_GEN9_UNCORE_RANGE_OFFSET(reg) && \
+	 !FORCEWAKE_GEN9_RENDER_RANGE_OFFSET(reg) && \
+	 !FORCEWAKE_GEN9_MEDIA_RANGE_OFFSET(reg) && \
+	 !FORCEWAKE_GEN9_COMMON_RANGE_OFFSET(reg))
+
 static void
 ilk_dummy_write(struct drm_i915_private *dev_priv)
 {
@@ -634,6 +832,45 @@ chv_read##x(struct drm_i915_private *dev_priv, off_t reg, bool trace) { \
 	REG_READ_FOOTER; \
 }
 
+#define SKL_NEEDS_FORCE_WAKE(dev_priv, reg)	\
+	 ((reg) < 0x40000 && !FORCEWAKE_GEN9_UNCORE_RANGE_OFFSET(reg))
+
+#define __gen9_read(x) \
+static u##x \
+gen9_read##x(struct drm_i915_private *dev_priv, off_t reg, bool trace) { \
+	REG_READ_HEADER(x); \
+	if (!SKL_NEEDS_FORCE_WAKE((dev_priv), (reg))) { \
+		val = __raw_i915_read##x(dev_priv, reg); \
+	} else { \
+		unsigned fwengine = 0; \
+		if (FORCEWAKE_GEN9_RENDER_RANGE_OFFSET(reg)) { \
+			if (dev_priv->uncore.fw_rendercount == 0) \
+				fwengine = FORCEWAKE_RENDER; \
+		} else if (FORCEWAKE_GEN9_MEDIA_RANGE_OFFSET(reg)) { \
+			if (dev_priv->uncore.fw_mediacount == 0) \
+				fwengine = FORCEWAKE_MEDIA; \
+		} else if (FORCEWAKE_GEN9_COMMON_RANGE_OFFSET(reg)) { \
+			if (dev_priv->uncore.fw_rendercount == 0) \
+				fwengine |= FORCEWAKE_RENDER; \
+			if (dev_priv->uncore.fw_mediacount == 0) \
+				fwengine |= FORCEWAKE_MEDIA; \
+		} else { \
+			if (dev_priv->uncore.fw_blittercount == 0) \
+				fwengine = FORCEWAKE_BLITTER; \
+		} \
+		if (fwengine) \
+			dev_priv->uncore.funcs.force_wake_get(dev_priv, fwengine); \
+		val = __raw_i915_read##x(dev_priv, reg); \
+		if (fwengine) \
+			dev_priv->uncore.funcs.force_wake_put(dev_priv, fwengine); \
+	} \
+	REG_READ_FOOTER; \
+}
+
+__gen9_read(8)
+__gen9_read(16)
+__gen9_read(32)
+__gen9_read(64)
 __chv_read(8)
 __chv_read(16)
 __chv_read(32)
@@ -655,6 +892,7 @@ __gen4_read(16)
 __gen4_read(32)
 __gen4_read(64)
 
+#undef __gen9_read
 #undef __chv_read
 #undef __vlv_read
 #undef __gen6_read
@@ -792,6 +1030,69 @@ chv_write##x(struct drm_i915_private *dev_priv, off_t reg, u##x val, bool trace)
 	REG_WRITE_FOOTER; \
 }
 
+static const u32 gen9_shadowed_regs[] = {
+	RING_TAIL(RENDER_RING_BASE),
+	RING_TAIL(GEN6_BSD_RING_BASE),
+	RING_TAIL(VEBOX_RING_BASE),
+	RING_TAIL(BLT_RING_BASE),
+	FORCEWAKE_BLITTER_GEN9,
+	FORCEWAKE_RENDER_GEN9,
+	FORCEWAKE_MEDIA_GEN9,
+	GEN6_RPNSWREQ,
+	GEN6_RC_VIDEO_FREQ,
+	/* TODO: Other registers are not yet used */
+};
+
+static bool is_gen9_shadowed(struct drm_i915_private *dev_priv, u32 reg)
+{
+	int i;
+	for (i = 0; i < ARRAY_SIZE(gen9_shadowed_regs); i++)
+		if (reg == gen9_shadowed_regs[i])
+			return true;
+
+	return false;
+}
+
+#define __gen9_write(x) \
+static void \
+gen9_write##x(struct drm_i915_private *dev_priv, off_t reg, u##x val, \
+		bool trace) { \
+	REG_WRITE_HEADER; \
+	if (!SKL_NEEDS_FORCE_WAKE((dev_priv), (reg)) || \
+			is_gen9_shadowed(dev_priv, reg)) { \
+		__raw_i915_write##x(dev_priv, reg, val); \
+	} else { \
+		unsigned fwengine = 0; \
+		if (FORCEWAKE_GEN9_RENDER_RANGE_OFFSET(reg)) { \
+			if (dev_priv->uncore.fw_rendercount == 0) \
+				fwengine = FORCEWAKE_RENDER; \
+		} else if (FORCEWAKE_GEN9_MEDIA_RANGE_OFFSET(reg)) { \
+			if (dev_priv->uncore.fw_mediacount == 0) \
+				fwengine = FORCEWAKE_MEDIA; \
+		} else if (FORCEWAKE_GEN9_COMMON_RANGE_OFFSET(reg)) { \
+			if (dev_priv->uncore.fw_rendercount == 0) \
+				fwengine |= FORCEWAKE_RENDER; \
+			if (dev_priv->uncore.fw_mediacount == 0) \
+				fwengine |= FORCEWAKE_MEDIA; \
+		} else { \
+			if (dev_priv->uncore.fw_blittercount == 0) \
+				fwengine = FORCEWAKE_BLITTER; \
+		} \
+		if (fwengine) \
+			dev_priv->uncore.funcs.force_wake_get(dev_priv, \
+					fwengine); \
+		__raw_i915_write##x(dev_priv, reg, val); \
+		if (fwengine) \
+			dev_priv->uncore.funcs.force_wake_put(dev_priv, \
+					fwengine); \
+	} \
+	REG_WRITE_FOOTER; \
+}
+
+__gen9_write(8)
+__gen9_write(16)
+__gen9_write(32)
+__gen9_write(64)
 __chv_write(8)
 __chv_write(16)
 __chv_write(32)
@@ -817,6 +1118,7 @@ __gen4_write(16)
 __gen4_write(32)
 __gen4_write(64)
 
+#undef __gen9_write
 #undef __chv_write
 #undef __gen8_write
 #undef __hsw_write
@@ -826,6 +1128,22 @@ __gen4_write(64)
 #undef REG_WRITE_FOOTER
 #undef REG_WRITE_HEADER
 
+#define ASSIGN_WRITE_MMIO_VFUNCS(x) \
+do { \
+	dev_priv->uncore.funcs.mmio_writeb = x##_write8; \
+	dev_priv->uncore.funcs.mmio_writew = x##_write16; \
+	dev_priv->uncore.funcs.mmio_writel = x##_write32; \
+	dev_priv->uncore.funcs.mmio_writeq = x##_write64; \
+} while (0)
+
+#define ASSIGN_READ_MMIO_VFUNCS(x) \
+do { \
+	dev_priv->uncore.funcs.mmio_readb = x##_read8; \
+	dev_priv->uncore.funcs.mmio_readw = x##_read16; \
+	dev_priv->uncore.funcs.mmio_readl = x##_read32; \
+	dev_priv->uncore.funcs.mmio_readq = x##_read64; \
+} while (0)
+
 void intel_uncore_init(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -833,9 +1151,12 @@ void intel_uncore_init(struct drm_device *dev)
 	setup_timer(&dev_priv->uncore.force_wake_timer,
 		    gen6_force_wake_timer, (unsigned long)dev_priv);
 
-	intel_uncore_early_sanitize(dev, false);
+	__intel_uncore_early_sanitize(dev, false);
 
-	if (IS_VALLEYVIEW(dev)) {
+	if (IS_GEN9(dev)) {
+		dev_priv->uncore.funcs.force_wake_get = __gen9_force_wake_get;
+		dev_priv->uncore.funcs.force_wake_put = __gen9_force_wake_put;
+	} else if (IS_VALLEYVIEW(dev)) {
 		dev_priv->uncore.funcs.force_wake_get = __vlv_force_wake_get;
 		dev_priv->uncore.funcs.force_wake_put = __vlv_force_wake_put;
 	} else if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
@@ -881,77 +1202,52 @@ void intel_uncore_init(struct drm_device *dev)
 
 	switch (INTEL_INFO(dev)->gen) {
 	default:
+		WARN_ON(1);
+		return;
+	case 9:
+		ASSIGN_WRITE_MMIO_VFUNCS(gen9);
+		ASSIGN_READ_MMIO_VFUNCS(gen9);
+		break;
+	case 8:
 		if (IS_CHERRYVIEW(dev)) {
-			dev_priv->uncore.funcs.mmio_writeb  = chv_write8;
-			dev_priv->uncore.funcs.mmio_writew  = chv_write16;
-			dev_priv->uncore.funcs.mmio_writel  = chv_write32;
-			dev_priv->uncore.funcs.mmio_writeq  = chv_write64;
-			dev_priv->uncore.funcs.mmio_readb  = chv_read8;
-			dev_priv->uncore.funcs.mmio_readw  = chv_read16;
-			dev_priv->uncore.funcs.mmio_readl  = chv_read32;
-			dev_priv->uncore.funcs.mmio_readq  = chv_read64;
+			ASSIGN_WRITE_MMIO_VFUNCS(chv);
+			ASSIGN_READ_MMIO_VFUNCS(chv);
 
 		} else {
-			dev_priv->uncore.funcs.mmio_writeb  = gen8_write8;
-			dev_priv->uncore.funcs.mmio_writew  = gen8_write16;
-			dev_priv->uncore.funcs.mmio_writel  = gen8_write32;
-			dev_priv->uncore.funcs.mmio_writeq  = gen8_write64;
-			dev_priv->uncore.funcs.mmio_readb  = gen6_read8;
-			dev_priv->uncore.funcs.mmio_readw  = gen6_read16;
-			dev_priv->uncore.funcs.mmio_readl  = gen6_read32;
-			dev_priv->uncore.funcs.mmio_readq  = gen6_read64;
+			ASSIGN_WRITE_MMIO_VFUNCS(gen8);
+			ASSIGN_READ_MMIO_VFUNCS(gen6);
 		}
 		break;
 	case 7:
 	case 6:
 		if (IS_HASWELL(dev)) {
-			dev_priv->uncore.funcs.mmio_writeb  = hsw_write8;
-			dev_priv->uncore.funcs.mmio_writew  = hsw_write16;
-			dev_priv->uncore.funcs.mmio_writel  = hsw_write32;
-			dev_priv->uncore.funcs.mmio_writeq  = hsw_write64;
+			ASSIGN_WRITE_MMIO_VFUNCS(hsw);
 		} else {
-			dev_priv->uncore.funcs.mmio_writeb  = gen6_write8;
-			dev_priv->uncore.funcs.mmio_writew  = gen6_write16;
-			dev_priv->uncore.funcs.mmio_writel  = gen6_write32;
-			dev_priv->uncore.funcs.mmio_writeq  = gen6_write64;
+			ASSIGN_WRITE_MMIO_VFUNCS(gen6);
 		}
 
 		if (IS_VALLEYVIEW(dev)) {
-			dev_priv->uncore.funcs.mmio_readb  = vlv_read8;
-			dev_priv->uncore.funcs.mmio_readw  = vlv_read16;
-			dev_priv->uncore.funcs.mmio_readl  = vlv_read32;
-			dev_priv->uncore.funcs.mmio_readq  = vlv_read64;
+			ASSIGN_READ_MMIO_VFUNCS(vlv);
 		} else {
-			dev_priv->uncore.funcs.mmio_readb  = gen6_read8;
-			dev_priv->uncore.funcs.mmio_readw  = gen6_read16;
-			dev_priv->uncore.funcs.mmio_readl  = gen6_read32;
-			dev_priv->uncore.funcs.mmio_readq  = gen6_read64;
+			ASSIGN_READ_MMIO_VFUNCS(gen6);
 		}
 		break;
 	case 5:
-		dev_priv->uncore.funcs.mmio_writeb  = gen5_write8;
-		dev_priv->uncore.funcs.mmio_writew  = gen5_write16;
-		dev_priv->uncore.funcs.mmio_writel  = gen5_write32;
-		dev_priv->uncore.funcs.mmio_writeq  = gen5_write64;
-		dev_priv->uncore.funcs.mmio_readb  = gen5_read8;
-		dev_priv->uncore.funcs.mmio_readw  = gen5_read16;
-		dev_priv->uncore.funcs.mmio_readl  = gen5_read32;
-		dev_priv->uncore.funcs.mmio_readq  = gen5_read64;
+		ASSIGN_WRITE_MMIO_VFUNCS(gen5);
+		ASSIGN_READ_MMIO_VFUNCS(gen5);
 		break;
 	case 4:
 	case 3:
 	case 2:
-		dev_priv->uncore.funcs.mmio_writeb  = gen4_write8;
-		dev_priv->uncore.funcs.mmio_writew  = gen4_write16;
-		dev_priv->uncore.funcs.mmio_writel  = gen4_write32;
-		dev_priv->uncore.funcs.mmio_writeq  = gen4_write64;
-		dev_priv->uncore.funcs.mmio_readb  = gen4_read8;
-		dev_priv->uncore.funcs.mmio_readw  = gen4_read16;
-		dev_priv->uncore.funcs.mmio_readl  = gen4_read32;
-		dev_priv->uncore.funcs.mmio_readq  = gen4_read64;
+		ASSIGN_WRITE_MMIO_VFUNCS(gen4);
+		ASSIGN_READ_MMIO_VFUNCS(gen4);
 		break;
 	}
+
+	i915_check_and_clear_faults(dev);
 }
+#undef ASSIGN_WRITE_MMIO_VFUNCS
+#undef ASSIGN_READ_MMIO_VFUNCS
 
 void intel_uncore_fini(struct drm_device *dev)
 {
@@ -968,7 +1264,7 @@ static const struct register_whitelist {
 	/* supported gens, 0x10 for 4, 0x30 for 4 and 5, etc. */
 	uint32_t gen_bitmask;
 } whitelist[] = {
-	{ RING_TIMESTAMP(RENDER_RING_BASE), 8, GEN_RANGE(4, 8) },
+	{ RING_TIMESTAMP(RENDER_RING_BASE), 8, GEN_RANGE(4, 9) },
 };
 
 int i915_reg_read_ioctl(struct drm_device *dev,
@@ -1053,41 +1349,34 @@ int i915_get_reset_stats_ioctl(struct drm_device *dev,
 	return 0;
 }
 
-static int i965_reset_complete(struct drm_device *dev)
+static int i915_reset_complete(struct drm_device *dev)
 {
 	u8 gdrst;
-	pci_read_config_byte(dev->pdev, I965_GDRST, &gdrst);
-	return (gdrst & GRDOM_RESET_ENABLE) == 0;
+	pci_read_config_byte(dev->pdev, I915_GDRST, &gdrst);
+	return (gdrst & GRDOM_RESET_STATUS) == 0;
 }
 
-static int i965_do_reset(struct drm_device *dev)
+static int i915_do_reset(struct drm_device *dev)
 {
-	int ret;
-
-	/* FIXME: i965g/gm need a display save/restore for gpu reset. */
-	return -ENODEV;
+	/* assert reset for at least 20 usec */
+	pci_write_config_byte(dev->pdev, I915_GDRST, GRDOM_RESET_ENABLE);
+	udelay(20);
+	pci_write_config_byte(dev->pdev, I915_GDRST, 0);
 
-	/*
-	 * Set the domains we want to reset (GRDOM/bits 2 and 3) as
-	 * well as the reset bit (GR/bit 0).  Setting the GR bit
-	 * triggers the reset; when done, the hardware will clear it.
-	 */
-	pci_write_config_byte(dev->pdev, I965_GDRST,
-			      GRDOM_RENDER | GRDOM_RESET_ENABLE);
-	ret =  wait_for(i965_reset_complete(dev), 500);
-	if (ret)
-		return ret;
-
-	pci_write_config_byte(dev->pdev, I965_GDRST,
-			      GRDOM_MEDIA | GRDOM_RESET_ENABLE);
-
-	ret =  wait_for(i965_reset_complete(dev), 500);
-	if (ret)
-		return ret;
+	return wait_for(i915_reset_complete(dev), 500);
+}
 
-	pci_write_config_byte(dev->pdev, I965_GDRST, 0);
+static int g4x_reset_complete(struct drm_device *dev)
+{
+	u8 gdrst;
+	pci_read_config_byte(dev->pdev, I915_GDRST, &gdrst);
+	return (gdrst & GRDOM_RESET_ENABLE) == 0;
+}
 
-	return 0;
+static int g33_do_reset(struct drm_device *dev)
+{
+	pci_write_config_byte(dev->pdev, I915_GDRST, GRDOM_RESET_ENABLE);
+	return wait_for(g4x_reset_complete(dev), 500);
 }
 
 static int g4x_do_reset(struct drm_device *dev)
@@ -1095,9 +1384,9 @@ static int g4x_do_reset(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret;
 
-	pci_write_config_byte(dev->pdev, I965_GDRST,
+	pci_write_config_byte(dev->pdev, I915_GDRST,
 			      GRDOM_RENDER | GRDOM_RESET_ENABLE);
-	ret =  wait_for(i965_reset_complete(dev), 500);
+	ret =  wait_for(g4x_reset_complete(dev), 500);
 	if (ret)
 		return ret;
 
@@ -1105,9 +1394,9 @@ static int g4x_do_reset(struct drm_device *dev)
 	I915_WRITE(VDECCLK_GATE_D, I915_READ(VDECCLK_GATE_D) | VCP_UNIT_CLOCK_GATE_DISABLE);
 	POSTING_READ(VDECCLK_GATE_D);
 
-	pci_write_config_byte(dev->pdev, I965_GDRST,
+	pci_write_config_byte(dev->pdev, I915_GDRST,
 			      GRDOM_MEDIA | GRDOM_RESET_ENABLE);
-	ret =  wait_for(i965_reset_complete(dev), 500);
+	ret =  wait_for(g4x_reset_complete(dev), 500);
 	if (ret)
 		return ret;
 
@@ -1115,7 +1404,7 @@ static int g4x_do_reset(struct drm_device *dev)
 	I915_WRITE(VDECCLK_GATE_D, I915_READ(VDECCLK_GATE_D) & ~VCP_UNIT_CLOCK_GATE_DISABLE);
 	POSTING_READ(VDECCLK_GATE_D);
 
-	pci_write_config_byte(dev->pdev, I965_GDRST, 0);
+	pci_write_config_byte(dev->pdev, I915_GDRST, 0);
 
 	return 0;
 }
@@ -1173,8 +1462,10 @@ int intel_gpu_reset(struct drm_device *dev)
 		return ironlake_do_reset(dev);
 	else if (IS_G4X(dev))
 		return g4x_do_reset(dev);
-	else if (IS_GEN4(dev))
-		return i965_do_reset(dev);
+	else if (IS_G33(dev))
+		return g33_do_reset(dev);
+	else if (INTEL_INFO(dev)->gen >= 3)
+		return i915_do_reset(dev);
 	else
 		return -ENODEV;
 }
diff --git a/drivers/staging/imx-drm/Kconfig b/drivers/gpu/drm/imx/Kconfig
index 82fb758a29bc..ab31848e92cf 100644
--- a/drivers/staging/imx-drm/Kconfig
+++ b/drivers/gpu/drm/imx/Kconfig
@@ -6,6 +6,7 @@ config DRM_IMX
 	select DRM_GEM_CMA_HELPER
 	select DRM_KMS_CMA_HELPER
 	depends on DRM && (ARCH_MXC || ARCH_MULTIPLATFORM)
+	depends on IMX_IPUV3_CORE
 	help
 	  enable i.MX graphics support
 
@@ -40,11 +41,11 @@ config DRM_IMX_LDB
 	  found on i.MX53 and i.MX6 processors.
 
 config DRM_IMX_IPUV3
-	tristate "DRM Support for i.MX IPUv3"
+	tristate
 	depends on DRM_IMX
 	depends on IMX_IPUV3_CORE
-	help
-	  Choose this if you have a i.MX5 or i.MX6 processor.
+	default y if DRM_IMX=y
+	default m if DRM_IMX=m
 
 config DRM_IMX_HDMI
 	tristate "Freescale i.MX DRM HDMI"
diff --git a/drivers/staging/imx-drm/Makefile b/drivers/gpu/drm/imx/Makefile
index 582c438d8cbd..582c438d8cbd 100644
--- a/drivers/staging/imx-drm/Makefile
+++ b/drivers/gpu/drm/imx/Makefile
diff --git a/drivers/staging/imx-drm/imx-drm-core.c b/drivers/gpu/drm/imx/imx-drm-core.c
index 9cb222e2996f..b250130debc8 100644
--- a/drivers/staging/imx-drm/imx-drm-core.c
+++ b/drivers/gpu/drm/imx/imx-drm-core.c
@@ -24,13 +24,12 @@
 #include <drm/drm_crtc_helper.h>
 #include <drm/drm_gem_cma_helper.h>
 #include <drm/drm_fb_cma_helper.h>
+#include <drm/drm_plane_helper.h>
 
 #include "imx-drm.h"
 
 #define MAX_CRTC	4
 
-struct imx_drm_crtc;
-
 struct imx_drm_component {
 	struct device_node *of_node;
 	struct list_head list;
@@ -632,7 +631,8 @@ static int imx_drm_platform_probe(struct platform_device *pdev)
 				continue;
 			}
 
-			component_match_add(&pdev->dev, &match, compare_of, remote);
+			component_match_add(&pdev->dev, &match, compare_of,
+					    remote);
 			of_node_put(remote);
 		}
 		of_node_put(port);
@@ -691,7 +691,6 @@ static struct platform_driver imx_drm_pdrv = {
 	.probe		= imx_drm_platform_probe,
 	.remove		= imx_drm_platform_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "imx-drm",
 		.pm	= &imx_drm_pm_ops,
 		.of_match_table = imx_drm_dt_ids,
diff --git a/drivers/staging/imx-drm/imx-drm.h b/drivers/gpu/drm/imx/imx-drm.h
index 7453ae00c412..7453ae00c412 100644
--- a/drivers/staging/imx-drm/imx-drm.h
+++ b/drivers/gpu/drm/imx/imx-drm.h
diff --git a/drivers/staging/imx-drm/imx-hdmi.c b/drivers/gpu/drm/imx/imx-hdmi.c
index aaec6b2cdf56..ddc53e039530 100644
--- a/drivers/staging/imx-drm/imx-hdmi.c
+++ b/drivers/gpu/drm/imx/imx-hdmi.c
@@ -1754,7 +1754,6 @@ static struct platform_driver imx_hdmi_driver = {
 	.remove = imx_hdmi_platform_remove,
 	.driver = {
 		.name = "imx-hdmi",
-		.owner = THIS_MODULE,
 		.of_match_table = imx_hdmi_dt_ids,
 	},
 };
diff --git a/drivers/staging/imx-drm/imx-hdmi.h b/drivers/gpu/drm/imx/imx-hdmi.h
index 39b677689db6..39b677689db6 100644
--- a/drivers/staging/imx-drm/imx-hdmi.h
+++ b/drivers/gpu/drm/imx/imx-hdmi.h
diff --git a/drivers/staging/imx-drm/imx-ldb.c b/drivers/gpu/drm/imx/imx-ldb.c
index 4662e00b456a..c60460043e24 100644
--- a/drivers/staging/imx-drm/imx-ldb.c
+++ b/drivers/gpu/drm/imx/imx-ldb.c
@@ -11,11 +11,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., 51 Franklin Street, Fifth Floor, Boston,
- * MA 02110-1301, USA.
  */
 
 #include <linux/module.h>
@@ -604,7 +599,6 @@ static struct platform_driver imx_ldb_driver = {
 	.driver		= {
 		.of_match_table = imx_ldb_dt_ids,
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/staging/imx-drm/imx-tve.c b/drivers/gpu/drm/imx/imx-tve.c
index 42c651be6c20..a729f4f7074c 100644
--- a/drivers/staging/imx-drm/imx-tve.c
+++ b/drivers/gpu/drm/imx/imx-tve.c
@@ -11,11 +11,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., 51 Franklin Street, Fifth Floor, Boston,
- * MA 02110-1301, USA.
  */
 
 #include <linux/clk.h>
@@ -665,7 +660,8 @@ static int imx_tve_bind(struct device *dev, struct device *master, void *data)
 
 	ret = regmap_read(tve->regmap, TVE_COM_CONF_REG, &val);
 	if (ret < 0) {
-		dev_err(dev, "failed to read configuration register: %d\n", ret);
+		dev_err(dev, "failed to read configuration register: %d\n",
+			ret);
 		return ret;
 	}
 	if (val != 0x00100000) {
@@ -724,7 +720,6 @@ static struct platform_driver imx_tve_driver = {
 	.driver		= {
 		.of_match_table = imx_tve_dt_ids,
 		.name	= "imx-tve",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/staging/imx-drm/ipuv3-crtc.c b/drivers/gpu/drm/imx/ipuv3-crtc.c
index 11e84a251773..ebee59cb96d8 100644
--- a/drivers/staging/imx-drm/ipuv3-crtc.c
+++ b/drivers/gpu/drm/imx/ipuv3-crtc.c
@@ -11,11 +11,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., 51 Franklin Street, Fifth Floor, Boston,
- * MA 02110-1301, USA.
  */
 #include <linux/component.h>
 #include <linux/module.h>
diff --git a/drivers/staging/imx-drm/ipuv3-plane.c b/drivers/gpu/drm/imx/ipuv3-plane.c
index 944962b692bb..6987e16fe99b 100644
--- a/drivers/staging/imx-drm/ipuv3-plane.c
+++ b/drivers/gpu/drm/imx/ipuv3-plane.c
@@ -64,6 +64,7 @@ int ipu_plane_set_base(struct ipu_plane *ipu_plane, struct drm_framebuffer *fb,
 {
 	struct drm_gem_cma_object *cma_obj;
 	unsigned long eba;
+	int active;
 
 	cma_obj = drm_fb_cma_get_gem_obj(fb, 0);
 	if (!cma_obj) {
@@ -74,12 +75,17 @@ int ipu_plane_set_base(struct ipu_plane *ipu_plane, struct drm_framebuffer *fb,
 	dev_dbg(ipu_plane->base.dev->dev, "phys = %pad, x = %d, y = %d",
 		&cma_obj->paddr, x, y);
 
-	ipu_cpmem_set_stride(ipu_plane->ipu_ch, fb->pitches[0]);
-
 	eba = cma_obj->paddr + fb->offsets[0] +
 	      fb->pitches[0] * y + (fb->bits_per_pixel >> 3) * x;
-	ipu_cpmem_set_buffer(ipu_plane->ipu_ch, 0, eba);
-	ipu_cpmem_set_buffer(ipu_plane->ipu_ch, 1, eba);
+
+	if (ipu_plane->enabled) {
+		active = ipu_idmac_get_current_buffer(ipu_plane->ipu_ch);
+		ipu_cpmem_set_buffer(ipu_plane->ipu_ch, !active, eba);
+		ipu_idmac_select_buffer(ipu_plane->ipu_ch, !active);
+	} else {
+		ipu_cpmem_set_buffer(ipu_plane->ipu_ch, 0, eba);
+		ipu_cpmem_set_buffer(ipu_plane->ipu_ch, 1, eba);
+	}
 
 	/* cache offsets for subsequent pageflips */
 	ipu_plane->x = x;
@@ -137,6 +143,18 @@ int ipu_plane_mode_set(struct ipu_plane *ipu_plane, struct drm_crtc *crtc,
 	if (crtc_h < 2)
 		return -EINVAL;
 
+	/*
+	 * since we cannot touch active IDMAC channels, we do not support
+	 * resizing the enabled plane or changing its format
+	 */
+	if (ipu_plane->enabled) {
+		if (src_w != ipu_plane->w || src_h != ipu_plane->h ||
+		    fb->pixel_format != ipu_plane->base.fb->pixel_format)
+			return -EINVAL;
+
+		return ipu_plane_set_base(ipu_plane, fb, src_x, src_y);
+	}
+
 	switch (ipu_plane->dp_flow) {
 	case IPU_DP_FLOW_SYNC_BG:
 		ret = ipu_dp_setup_channel(ipu_plane->dp,
@@ -148,14 +166,22 @@ int ipu_plane_mode_set(struct ipu_plane *ipu_plane, struct drm_crtc *crtc,
 				ret);
 			return ret;
 		}
-		ipu_dp_set_global_alpha(ipu_plane->dp, 1, 0, 1);
+		ipu_dp_set_global_alpha(ipu_plane->dp, true, 0, true);
 		break;
 	case IPU_DP_FLOW_SYNC_FG:
 		ipu_dp_setup_channel(ipu_plane->dp,
 				ipu_drm_fourcc_to_colorspace(fb->pixel_format),
 				IPUV3_COLORSPACE_UNKNOWN);
 		ipu_dp_set_window_pos(ipu_plane->dp, crtc_x, crtc_y);
-		break;
+		/* Enable local alpha on partial plane */
+		switch (fb->pixel_format) {
+		case DRM_FORMAT_ARGB8888:
+		case DRM_FORMAT_ABGR8888:
+			ipu_dp_set_global_alpha(ipu_plane->dp, false, 0, false);
+			break;
+		default:
+			break;
+		}
 	}
 
 	ret = ipu_dmfc_init_channel(ipu_plane->dmfc, crtc_w);
@@ -181,11 +207,16 @@ int ipu_plane_mode_set(struct ipu_plane *ipu_plane, struct drm_crtc *crtc,
 		return ret;
 	}
 	ipu_cpmem_set_high_priority(ipu_plane->ipu_ch);
+	ipu_idmac_set_double_buffer(ipu_plane->ipu_ch, 1);
+	ipu_cpmem_set_stride(ipu_plane->ipu_ch, fb->pitches[0]);
 
 	ret = ipu_plane_set_base(ipu_plane, fb, src_x, src_y);
 	if (ret < 0)
 		return ret;
 
+	ipu_plane->w = src_w;
+	ipu_plane->h = src_h;
+
 	return 0;
 }
 
diff --git a/drivers/staging/imx-drm/ipuv3-plane.h b/drivers/gpu/drm/imx/ipuv3-plane.h
index c0aae5bcb5d4..af125fb40ef5 100644
--- a/drivers/staging/imx-drm/ipuv3-plane.h
+++ b/drivers/gpu/drm/imx/ipuv3-plane.h
@@ -26,6 +26,8 @@ struct ipu_plane {
 
 	int			x;
 	int			y;
+	int			w;
+	int			h;
 
 	bool			enabled;
 };
diff --git a/drivers/staging/imx-drm/parallel-display.c b/drivers/gpu/drm/imx/parallel-display.c
index 015a454b87e1..796c3c1c170a 100644
--- a/drivers/staging/imx-drm/parallel-display.c
+++ b/drivers/gpu/drm/imx/parallel-display.c
@@ -11,11 +11,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., 51 Franklin Street, Fifth Floor, Boston,
- * MA 02110-1301, USA.
  */
 
 #include <linux/component.h>
@@ -128,6 +123,10 @@ static void imx_pd_encoder_prepare(struct drm_encoder *encoder)
 
 static void imx_pd_encoder_commit(struct drm_encoder *encoder)
 {
+	struct imx_parallel_display *imxpd = enc_to_imxpd(encoder);
+
+	drm_panel_prepare(imxpd->panel);
+	drm_panel_enable(imxpd->panel);
 }
 
 static void imx_pd_encoder_mode_set(struct drm_encoder *encoder,
@@ -138,6 +137,10 @@ static void imx_pd_encoder_mode_set(struct drm_encoder *encoder,
 
 static void imx_pd_encoder_disable(struct drm_encoder *encoder)
 {
+	struct imx_parallel_display *imxpd = enc_to_imxpd(encoder);
+
+	drm_panel_disable(imxpd->panel);
+	drm_panel_unprepare(imxpd->panel);
 }
 
 static struct drm_connector_funcs imx_pd_connector_funcs = {
@@ -284,7 +287,6 @@ static struct platform_driver imx_pd_driver = {
 	.driver		= {
 		.of_match_table = imx_pd_dt_ids,
 		.name	= "imx-parallel-display",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/gpu/drm/mgag200/mgag200_mode.c b/drivers/gpu/drm/mgag200/mgag200_mode.c
index 83485ab81ce8..9872ba9abf1a 100644
--- a/drivers/gpu/drm/mgag200/mgag200_mode.c
+++ b/drivers/gpu/drm/mgag200/mgag200_mode.c
@@ -15,6 +15,7 @@
 
 #include <drm/drmP.h>
 #include <drm/drm_crtc_helper.h>
+#include <drm/drm_plane_helper.h>
 
 #include "mgag200_drv.h"
 
diff --git a/drivers/gpu/drm/msm/Kconfig b/drivers/gpu/drm/msm/Kconfig
index 9d907c526c94..5b2a1ff95d3d 100644
--- a/drivers/gpu/drm/msm/Kconfig
+++ b/drivers/gpu/drm/msm/Kconfig
@@ -3,6 +3,7 @@ config DRM_MSM
 	tristate "MSM DRM"
 	depends on DRM
 	depends on ARCH_QCOM || (ARM && COMPILE_TEST)
+	select REGULATOR
 	select DRM_KMS_HELPER
 	select DRM_PANEL
 	select SHMEM
diff --git a/drivers/gpu/drm/msm/Makefile b/drivers/gpu/drm/msm/Makefile
index 6283dcb96af5..143d988f8add 100644
--- a/drivers/gpu/drm/msm/Makefile
+++ b/drivers/gpu/drm/msm/Makefile
@@ -7,6 +7,7 @@ msm-y := \
 	adreno/adreno_device.o \
 	adreno/adreno_gpu.o \
 	adreno/a3xx_gpu.o \
+	adreno/a4xx_gpu.o \
 	hdmi/hdmi.o \
 	hdmi/hdmi_audio.o \
 	hdmi/hdmi_bridge.o \
@@ -24,12 +25,15 @@ msm-y := \
 	mdp/mdp4/mdp4_irq.o \
 	mdp/mdp4/mdp4_kms.o \
 	mdp/mdp4/mdp4_plane.o \
+	mdp/mdp5/mdp5_cfg.o \
+	mdp/mdp5/mdp5_ctl.o \
 	mdp/mdp5/mdp5_crtc.o \
 	mdp/mdp5/mdp5_encoder.o \
 	mdp/mdp5/mdp5_irq.o \
 	mdp/mdp5/mdp5_kms.o \
 	mdp/mdp5/mdp5_plane.o \
 	mdp/mdp5/mdp5_smp.o \
+	msm_atomic.o \
 	msm_drv.o \
 	msm_fb.o \
 	msm_gem.o \
diff --git a/drivers/gpu/drm/msm/adreno/a2xx.xml.h b/drivers/gpu/drm/msm/adreno/a2xx.xml.h
index a3104598c27f..22882cc0a573 100644
--- a/drivers/gpu/drm/msm/adreno/a2xx.xml.h
+++ b/drivers/gpu/drm/msm/adreno/a2xx.xml.h
@@ -11,10 +11,10 @@ The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/adreno.xml               (    364 bytes, from 2013-11-30 14:47:15)
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml  (   1453 bytes, from 2013-03-31 16:51:27)
 - /home/robclark/src/freedreno/envytools/rnndb/adreno/a2xx.xml          (  32901 bytes, from 2014-06-02 15:21:30)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_common.xml (   9859 bytes, from 2014-06-02 15:21:30)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_pm4.xml    (  14960 bytes, from 2014-07-27 17:22:13)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/a3xx.xml          (  58020 bytes, from 2014-08-01 12:22:48)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/a4xx.xml          (  41068 bytes, from 2014-08-01 12:22:48)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_common.xml (  10551 bytes, from 2014-11-13 22:44:30)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_pm4.xml    (  15053 bytes, from 2014-11-09 15:45:47)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/a3xx.xml          (  63169 bytes, from 2014-11-13 22:44:18)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/a4xx.xml          (  49097 bytes, from 2014-11-14 15:38:00)
 
 Copyright (C) 2013-2014 by the following authors:
 - Rob Clark <robdclark@gmail.com> (robclark)
@@ -926,11 +926,11 @@ static inline uint32_t A2XX_VGT_DRAW_INITIATOR_INDEX_SIZE(enum pc_di_index_size
 #define A2XX_VGT_DRAW_INITIATOR_NOT_EOP				0x00001000
 #define A2XX_VGT_DRAW_INITIATOR_SMALL_INDEX			0x00002000
 #define A2XX_VGT_DRAW_INITIATOR_PRE_DRAW_INITIATOR_ENABLE	0x00004000
-#define A2XX_VGT_DRAW_INITIATOR_NUM_INDICES__MASK		0xffff0000
-#define A2XX_VGT_DRAW_INITIATOR_NUM_INDICES__SHIFT		16
-static inline uint32_t A2XX_VGT_DRAW_INITIATOR_NUM_INDICES(uint32_t val)
+#define A2XX_VGT_DRAW_INITIATOR_NUM_INSTANCES__MASK		0xff000000
+#define A2XX_VGT_DRAW_INITIATOR_NUM_INSTANCES__SHIFT		24
+static inline uint32_t A2XX_VGT_DRAW_INITIATOR_NUM_INSTANCES(uint32_t val)
 {
-	return ((val) << A2XX_VGT_DRAW_INITIATOR_NUM_INDICES__SHIFT) & A2XX_VGT_DRAW_INITIATOR_NUM_INDICES__MASK;
+	return ((val) << A2XX_VGT_DRAW_INITIATOR_NUM_INSTANCES__SHIFT) & A2XX_VGT_DRAW_INITIATOR_NUM_INSTANCES__MASK;
 }
 
 #define REG_A2XX_VGT_IMMED_DATA					0x000021fd
@@ -1243,13 +1243,13 @@ static inline uint32_t A2XX_CLEAR_COLOR_ALPHA(uint32_t val)
 #define A2XX_PA_SU_POINT_SIZE_HEIGHT__SHIFT			0
 static inline uint32_t A2XX_PA_SU_POINT_SIZE_HEIGHT(float val)
 {
-	return ((((uint32_t)(val * 8.0))) << A2XX_PA_SU_POINT_SIZE_HEIGHT__SHIFT) & A2XX_PA_SU_POINT_SIZE_HEIGHT__MASK;
+	return ((((uint32_t)(val * 16.0))) << A2XX_PA_SU_POINT_SIZE_HEIGHT__SHIFT) & A2XX_PA_SU_POINT_SIZE_HEIGHT__MASK;
 }
 #define A2XX_PA_SU_POINT_SIZE_WIDTH__MASK			0xffff0000
 #define A2XX_PA_SU_POINT_SIZE_WIDTH__SHIFT			16
 static inline uint32_t A2XX_PA_SU_POINT_SIZE_WIDTH(float val)
 {
-	return ((((uint32_t)(val * 8.0))) << A2XX_PA_SU_POINT_SIZE_WIDTH__SHIFT) & A2XX_PA_SU_POINT_SIZE_WIDTH__MASK;
+	return ((((uint32_t)(val * 16.0))) << A2XX_PA_SU_POINT_SIZE_WIDTH__SHIFT) & A2XX_PA_SU_POINT_SIZE_WIDTH__MASK;
 }
 
 #define REG_A2XX_PA_SU_POINT_MINMAX				0x00002281
@@ -1257,13 +1257,13 @@ static inline uint32_t A2XX_PA_SU_POINT_SIZE_WIDTH(float val)
 #define A2XX_PA_SU_POINT_MINMAX_MIN__SHIFT			0
 static inline uint32_t A2XX_PA_SU_POINT_MINMAX_MIN(float val)
 {
-	return ((((uint32_t)(val * 8.0))) << A2XX_PA_SU_POINT_MINMAX_MIN__SHIFT) & A2XX_PA_SU_POINT_MINMAX_MIN__MASK;
+	return ((((uint32_t)(val * 16.0))) << A2XX_PA_SU_POINT_MINMAX_MIN__SHIFT) & A2XX_PA_SU_POINT_MINMAX_MIN__MASK;
 }
 #define A2XX_PA_SU_POINT_MINMAX_MAX__MASK			0xffff0000
 #define A2XX_PA_SU_POINT_MINMAX_MAX__SHIFT			16
 static inline uint32_t A2XX_PA_SU_POINT_MINMAX_MAX(float val)
 {
-	return ((((uint32_t)(val * 8.0))) << A2XX_PA_SU_POINT_MINMAX_MAX__SHIFT) & A2XX_PA_SU_POINT_MINMAX_MAX__MASK;
+	return ((((uint32_t)(val * 16.0))) << A2XX_PA_SU_POINT_MINMAX_MAX__SHIFT) & A2XX_PA_SU_POINT_MINMAX_MAX__MASK;
 }
 
 #define REG_A2XX_PA_SU_LINE_CNTL				0x00002282
@@ -1271,7 +1271,7 @@ static inline uint32_t A2XX_PA_SU_POINT_MINMAX_MAX(float val)
 #define A2XX_PA_SU_LINE_CNTL_WIDTH__SHIFT			0
 static inline uint32_t A2XX_PA_SU_LINE_CNTL_WIDTH(float val)
 {
-	return ((((uint32_t)(val * 8.0))) << A2XX_PA_SU_LINE_CNTL_WIDTH__SHIFT) & A2XX_PA_SU_LINE_CNTL_WIDTH__MASK;
+	return ((((uint32_t)(val * 16.0))) << A2XX_PA_SU_LINE_CNTL_WIDTH__SHIFT) & A2XX_PA_SU_LINE_CNTL_WIDTH__MASK;
 }
 
 #define REG_A2XX_PA_SC_LINE_STIPPLE				0x00002283
diff --git a/drivers/gpu/drm/msm/adreno/a3xx.xml.h b/drivers/gpu/drm/msm/adreno/a3xx.xml.h
index 82d015279b47..109e9a263daf 100644
--- a/drivers/gpu/drm/msm/adreno/a3xx.xml.h
+++ b/drivers/gpu/drm/msm/adreno/a3xx.xml.h
@@ -11,10 +11,10 @@ The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/adreno.xml               (    364 bytes, from 2013-11-30 14:47:15)
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml  (   1453 bytes, from 2013-03-31 16:51:27)
 - /home/robclark/src/freedreno/envytools/rnndb/adreno/a2xx.xml          (  32901 bytes, from 2014-06-02 15:21:30)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_common.xml (   9859 bytes, from 2014-06-02 15:21:30)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_pm4.xml    (  14960 bytes, from 2014-07-27 17:22:13)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/a3xx.xml          (  58020 bytes, from 2014-08-01 12:22:48)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/a4xx.xml          (  41068 bytes, from 2014-08-01 12:22:48)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_common.xml (  10551 bytes, from 2014-11-13 22:44:30)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_pm4.xml    (  15053 bytes, from 2014-11-09 15:45:47)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/a3xx.xml          (  63169 bytes, from 2014-11-13 22:44:18)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/a4xx.xml          (  49097 bytes, from 2014-11-14 15:38:00)
 
 Copyright (C) 2013-2014 by the following authors:
 - Rob Clark <robdclark@gmail.com> (robclark)
@@ -86,6 +86,14 @@ enum a3xx_vtx_fmt {
 	VFMT_NORM_USHORT_16_16 = 29,
 	VFMT_NORM_USHORT_16_16_16 = 30,
 	VFMT_NORM_USHORT_16_16_16_16 = 31,
+	VFMT_UINT_32 = 32,
+	VFMT_UINT_32_32 = 33,
+	VFMT_UINT_32_32_32 = 34,
+	VFMT_UINT_32_32_32_32 = 35,
+	VFMT_INT_32 = 36,
+	VFMT_INT_32_32 = 37,
+	VFMT_INT_32_32_32 = 38,
+	VFMT_INT_32_32_32_32 = 39,
 	VFMT_UBYTE_8 = 40,
 	VFMT_UBYTE_8_8 = 41,
 	VFMT_UBYTE_8_8_8 = 42,
@@ -112,7 +120,9 @@ enum a3xx_tex_fmt {
 	TFMT_NORM_USHORT_565 = 4,
 	TFMT_NORM_USHORT_5551 = 6,
 	TFMT_NORM_USHORT_4444 = 7,
+	TFMT_NORM_USHORT_Z16 = 9,
 	TFMT_NORM_UINT_X8Z24 = 10,
+	TFMT_FLOAT_Z32 = 11,
 	TFMT_NORM_UINT_NV12_UV_TILED = 17,
 	TFMT_NORM_UINT_NV12_Y_TILED = 19,
 	TFMT_NORM_UINT_NV12_UV = 21,
@@ -121,18 +131,38 @@ enum a3xx_tex_fmt {
 	TFMT_NORM_UINT_I420_U = 26,
 	TFMT_NORM_UINT_I420_V = 27,
 	TFMT_NORM_UINT_2_10_10_10 = 41,
+	TFMT_FLOAT_9_9_9_E5 = 42,
+	TFMT_FLOAT_10_11_11 = 43,
 	TFMT_NORM_UINT_A8 = 44,
 	TFMT_NORM_UINT_L8_A8 = 47,
 	TFMT_NORM_UINT_8 = 48,
 	TFMT_NORM_UINT_8_8 = 49,
 	TFMT_NORM_UINT_8_8_8 = 50,
 	TFMT_NORM_UINT_8_8_8_8 = 51,
+	TFMT_NORM_SINT_8_8 = 53,
+	TFMT_NORM_SINT_8_8_8_8 = 55,
+	TFMT_UINT_8_8 = 57,
+	TFMT_UINT_8_8_8_8 = 59,
+	TFMT_SINT_8_8 = 61,
+	TFMT_SINT_8_8_8_8 = 63,
 	TFMT_FLOAT_16 = 64,
 	TFMT_FLOAT_16_16 = 65,
 	TFMT_FLOAT_16_16_16_16 = 67,
+	TFMT_UINT_16 = 68,
+	TFMT_UINT_16_16 = 69,
+	TFMT_UINT_16_16_16_16 = 71,
+	TFMT_SINT_16 = 72,
+	TFMT_SINT_16_16 = 73,
+	TFMT_SINT_16_16_16_16 = 75,
 	TFMT_FLOAT_32 = 84,
 	TFMT_FLOAT_32_32 = 85,
 	TFMT_FLOAT_32_32_32_32 = 87,
+	TFMT_UINT_32 = 88,
+	TFMT_UINT_32_32 = 89,
+	TFMT_UINT_32_32_32_32 = 91,
+	TFMT_SINT_32 = 92,
+	TFMT_SINT_32_32 = 93,
+	TFMT_SINT_32_32_32_32 = 95,
 };
 
 enum a3xx_tex_fetchsize {
@@ -145,19 +175,34 @@ enum a3xx_tex_fetchsize {
 };
 
 enum a3xx_color_fmt {
+	RB_R5G6B5_UNORM = 0,
+	RB_R5G5B5A1_UNORM = 1,
+	RB_R4G4B4A4_UNORM = 3,
 	RB_R8G8B8_UNORM = 4,
 	RB_R8G8B8A8_UNORM = 8,
-	RB_Z16_UNORM = 12,
+	RB_R8G8B8A8_UINT = 10,
+	RB_R8G8B8A8_SINT = 11,
+	RB_R8G8_UNORM = 12,
+	RB_R8_UINT = 14,
+	RB_R8_SINT = 15,
+	RB_R10G10B10A2_UNORM = 16,
 	RB_A8_UNORM = 20,
+	RB_R8_UNORM = 21,
 	RB_R16G16B16A16_FLOAT = 27,
+	RB_R11G11B10_FLOAT = 28,
+	RB_R16_SINT = 40,
+	RB_R16G16_SINT = 41,
+	RB_R16G16B16A16_SINT = 43,
+	RB_R16_UINT = 44,
+	RB_R16G16_UINT = 45,
+	RB_R16G16B16A16_UINT = 47,
 	RB_R32G32B32A32_FLOAT = 51,
-};
-
-enum a3xx_color_swap {
-	WZYX = 0,
-	WXYZ = 1,
-	ZYXW = 2,
-	XYZW = 3,
+	RB_R32_SINT = 52,
+	RB_R32G32_SINT = 53,
+	RB_R32G32B32A32_SINT = 55,
+	RB_R32_UINT = 56,
+	RB_R32G32_UINT = 57,
+	RB_R32G32B32A32_UINT = 59,
 };
 
 enum a3xx_sp_perfcounter_select {
@@ -194,6 +239,11 @@ enum a3xx_rb_blend_opcode {
 	BLEND_MAX_DST_SRC = 4,
 };
 
+enum a3xx_intp_mode {
+	SMOOTH = 0,
+	FLAT = 1,
+};
+
 enum a3xx_tex_filter {
 	A3XX_TEX_NEAREST = 0,
 	A3XX_TEX_LINEAR = 1,
@@ -536,6 +586,10 @@ enum a3xx_tex_type {
 
 #define REG_A3XX_CP_MEQ_DATA					0x000001db
 
+#define REG_A3XX_CP_WFI_PEND_CTR				0x000001f5
+
+#define REG_A3XX_RBBM_PM_OVERRIDE2				0x0000039d
+
 #define REG_A3XX_CP_PERFCOUNTER_SELECT				0x00000445
 
 #define REG_A3XX_CP_HW_FAULT					0x0000045c
@@ -550,6 +604,12 @@ static inline uint32_t REG_A3XX_CP_PROTECT_REG(uint32_t i0) { return 0x00000460
 
 #define REG_A3XX_CP_AHB_FAULT					0x0000054d
 
+#define REG_A3XX_SQ_GPR_MANAGEMENT				0x00000d00
+
+#define REG_A3XX_SQ_INST_STORE_MANAGMENT			0x00000d02
+
+#define REG_A3XX_TP0_CHICKEN					0x00000e1e
+
 #define REG_A3XX_SP_GLOBAL_MEM_SIZE				0x00000e22
 
 #define REG_A3XX_SP_GLOBAL_MEM_ADDR				0x00000e23
@@ -632,13 +692,13 @@ static inline uint32_t A3XX_GRAS_CL_VPORT_ZSCALE(float val)
 #define A3XX_GRAS_SU_POINT_MINMAX_MIN__SHIFT			0
 static inline uint32_t A3XX_GRAS_SU_POINT_MINMAX_MIN(float val)
 {
-	return ((((uint32_t)(val * 8.0))) << A3XX_GRAS_SU_POINT_MINMAX_MIN__SHIFT) & A3XX_GRAS_SU_POINT_MINMAX_MIN__MASK;
+	return ((((uint32_t)(val * 16.0))) << A3XX_GRAS_SU_POINT_MINMAX_MIN__SHIFT) & A3XX_GRAS_SU_POINT_MINMAX_MIN__MASK;
 }
 #define A3XX_GRAS_SU_POINT_MINMAX_MAX__MASK			0xffff0000
 #define A3XX_GRAS_SU_POINT_MINMAX_MAX__SHIFT			16
 static inline uint32_t A3XX_GRAS_SU_POINT_MINMAX_MAX(float val)
 {
-	return ((((uint32_t)(val * 8.0))) << A3XX_GRAS_SU_POINT_MINMAX_MAX__SHIFT) & A3XX_GRAS_SU_POINT_MINMAX_MAX__MASK;
+	return ((((uint32_t)(val * 16.0))) << A3XX_GRAS_SU_POINT_MINMAX_MAX__SHIFT) & A3XX_GRAS_SU_POINT_MINMAX_MAX__MASK;
 }
 
 #define REG_A3XX_GRAS_SU_POINT_SIZE				0x00002069
@@ -646,7 +706,7 @@ static inline uint32_t A3XX_GRAS_SU_POINT_MINMAX_MAX(float val)
 #define A3XX_GRAS_SU_POINT_SIZE__SHIFT				0
 static inline uint32_t A3XX_GRAS_SU_POINT_SIZE(float val)
 {
-	return ((((uint32_t)(val * 8.0))) << A3XX_GRAS_SU_POINT_SIZE__SHIFT) & A3XX_GRAS_SU_POINT_SIZE__MASK;
+	return ((((int32_t)(val * 16.0))) << A3XX_GRAS_SU_POINT_SIZE__SHIFT) & A3XX_GRAS_SU_POINT_SIZE__MASK;
 }
 
 #define REG_A3XX_GRAS_SU_POLY_OFFSET_SCALE			0x0000206c
@@ -654,7 +714,7 @@ static inline uint32_t A3XX_GRAS_SU_POINT_SIZE(float val)
 #define A3XX_GRAS_SU_POLY_OFFSET_SCALE_VAL__SHIFT		0
 static inline uint32_t A3XX_GRAS_SU_POLY_OFFSET_SCALE_VAL(float val)
 {
-	return ((((uint32_t)(val * 28.0))) << A3XX_GRAS_SU_POLY_OFFSET_SCALE_VAL__SHIFT) & A3XX_GRAS_SU_POLY_OFFSET_SCALE_VAL__MASK;
+	return ((((int32_t)(val * 16384.0))) << A3XX_GRAS_SU_POLY_OFFSET_SCALE_VAL__SHIFT) & A3XX_GRAS_SU_POLY_OFFSET_SCALE_VAL__MASK;
 }
 
 #define REG_A3XX_GRAS_SU_POLY_OFFSET_OFFSET			0x0000206d
@@ -662,7 +722,7 @@ static inline uint32_t A3XX_GRAS_SU_POLY_OFFSET_SCALE_VAL(float val)
 #define A3XX_GRAS_SU_POLY_OFFSET_OFFSET__SHIFT			0
 static inline uint32_t A3XX_GRAS_SU_POLY_OFFSET_OFFSET(float val)
 {
-	return ((((uint32_t)(val * 28.0))) << A3XX_GRAS_SU_POLY_OFFSET_OFFSET__SHIFT) & A3XX_GRAS_SU_POLY_OFFSET_OFFSET__MASK;
+	return ((((int32_t)(val * 16384.0))) << A3XX_GRAS_SU_POLY_OFFSET_OFFSET__SHIFT) & A3XX_GRAS_SU_POLY_OFFSET_OFFSET__MASK;
 }
 
 #define REG_A3XX_GRAS_SU_MODE_CONTROL				0x00002070
@@ -673,7 +733,7 @@ static inline uint32_t A3XX_GRAS_SU_POLY_OFFSET_OFFSET(float val)
 #define A3XX_GRAS_SU_MODE_CONTROL_LINEHALFWIDTH__SHIFT		3
 static inline uint32_t A3XX_GRAS_SU_MODE_CONTROL_LINEHALFWIDTH(float val)
 {
-	return ((((uint32_t)(val * 4.0))) << A3XX_GRAS_SU_MODE_CONTROL_LINEHALFWIDTH__SHIFT) & A3XX_GRAS_SU_MODE_CONTROL_LINEHALFWIDTH__MASK;
+	return ((((int32_t)(val * 4.0))) << A3XX_GRAS_SU_MODE_CONTROL_LINEHALFWIDTH__SHIFT) & A3XX_GRAS_SU_MODE_CONTROL_LINEHALFWIDTH__MASK;
 }
 #define A3XX_GRAS_SU_MODE_CONTROL_POLY_OFFSET			0x00000800
 
@@ -863,6 +923,7 @@ static inline uint32_t A3XX_RB_MRT_BUF_INFO_COLOR_SWAP(enum a3xx_color_swap val)
 {
 	return ((val) << A3XX_RB_MRT_BUF_INFO_COLOR_SWAP__SHIFT) & A3XX_RB_MRT_BUF_INFO_COLOR_SWAP__MASK;
 }
+#define A3XX_RB_MRT_BUF_INFO_COLOR_SRGB				0x00004000
 #define A3XX_RB_MRT_BUF_INFO_COLOR_BUF_PITCH__MASK		0xfffe0000
 #define A3XX_RB_MRT_BUF_INFO_COLOR_BUF_PITCH__SHIFT		17
 static inline uint32_t A3XX_RB_MRT_BUF_INFO_COLOR_BUF_PITCH(uint32_t val)
@@ -1001,6 +1062,7 @@ static inline uint32_t A3XX_RB_COPY_CONTROL_FASTCLEAR(uint32_t val)
 {
 	return ((val) << A3XX_RB_COPY_CONTROL_FASTCLEAR__SHIFT) & A3XX_RB_COPY_CONTROL_FASTCLEAR__MASK;
 }
+#define A3XX_RB_COPY_CONTROL_UNK12				0x00001000
 #define A3XX_RB_COPY_CONTROL_GMEM_BASE__MASK			0xffffc000
 #define A3XX_RB_COPY_CONTROL_GMEM_BASE__SHIFT			14
 static inline uint32_t A3XX_RB_COPY_CONTROL_GMEM_BASE(uint32_t val)
@@ -1079,7 +1141,7 @@ static inline uint32_t A3XX_RB_DEPTH_CONTROL_ZFUNC(enum adreno_compare_func val)
 #define REG_A3XX_RB_DEPTH_CLEAR					0x00002101
 
 #define REG_A3XX_RB_DEPTH_INFO					0x00002102
-#define A3XX_RB_DEPTH_INFO_DEPTH_FORMAT__MASK			0x00000001
+#define A3XX_RB_DEPTH_INFO_DEPTH_FORMAT__MASK			0x00000003
 #define A3XX_RB_DEPTH_INFO_DEPTH_FORMAT__SHIFT			0
 static inline uint32_t A3XX_RB_DEPTH_INFO_DEPTH_FORMAT(enum adreno_rb_depth_format val)
 {
@@ -1265,6 +1327,7 @@ static inline uint32_t A3XX_PC_PRIM_VTX_CNTL_POLYMODE_BACK_PTYPE(enum adreno_pa_
 {
 	return ((val) << A3XX_PC_PRIM_VTX_CNTL_POLYMODE_BACK_PTYPE__SHIFT) & A3XX_PC_PRIM_VTX_CNTL_POLYMODE_BACK_PTYPE__MASK;
 }
+#define A3XX_PC_PRIM_VTX_CNTL_PRIMITIVE_RESTART			0x00100000
 #define A3XX_PC_PRIM_VTX_CNTL_PROVOKING_VTX_LAST		0x02000000
 #define A3XX_PC_PRIM_VTX_CNTL_PSIZE				0x04000000
 
@@ -1281,7 +1344,12 @@ static inline uint32_t A3XX_HLSQ_CONTROL_0_REG_FSTHREADSIZE(enum a3xx_threadsize
 #define A3XX_HLSQ_CONTROL_0_REG_SPSHADERRESTART			0x00000200
 #define A3XX_HLSQ_CONTROL_0_REG_RESERVED2			0x00000400
 #define A3XX_HLSQ_CONTROL_0_REG_CHUNKDISABLE			0x04000000
-#define A3XX_HLSQ_CONTROL_0_REG_CONSTSWITCHMODE			0x08000000
+#define A3XX_HLSQ_CONTROL_0_REG_CONSTMODE__MASK			0x08000000
+#define A3XX_HLSQ_CONTROL_0_REG_CONSTMODE__SHIFT		27
+static inline uint32_t A3XX_HLSQ_CONTROL_0_REG_CONSTMODE(uint32_t val)
+{
+	return ((val) << A3XX_HLSQ_CONTROL_0_REG_CONSTMODE__SHIFT) & A3XX_HLSQ_CONTROL_0_REG_CONSTMODE__MASK;
+}
 #define A3XX_HLSQ_CONTROL_0_REG_LAZYUPDATEDISABLE		0x10000000
 #define A3XX_HLSQ_CONTROL_0_REG_SPCONSTFULLUPDATE		0x20000000
 #define A3XX_HLSQ_CONTROL_0_REG_TPFULLUPDATE			0x40000000
@@ -1484,6 +1552,8 @@ static inline uint32_t A3XX_VFD_CONTROL_1_REGID4INST(uint32_t val)
 
 #define REG_A3XX_VFD_INDEX_OFFSET				0x00002245
 
+#define REG_A3XX_VFD_INDEX_OFFSET				0x00002245
+
 static inline uint32_t REG_A3XX_VFD_FETCH(uint32_t i0) { return 0x00002246 + 0x2*i0; }
 
 static inline uint32_t REG_A3XX_VFD_FETCH_INSTR_0(uint32_t i0) { return 0x00002246 + 0x2*i0; }
@@ -1537,6 +1607,7 @@ static inline uint32_t A3XX_VFD_DECODE_INSTR_REGID(uint32_t val)
 {
 	return ((val) << A3XX_VFD_DECODE_INSTR_REGID__SHIFT) & A3XX_VFD_DECODE_INSTR_REGID__MASK;
 }
+#define A3XX_VFD_DECODE_INSTR_INT				0x00100000
 #define A3XX_VFD_DECODE_INSTR_SWAP__MASK			0x00c00000
 #define A3XX_VFD_DECODE_INSTR_SWAP__SHIFT			22
 static inline uint32_t A3XX_VFD_DECODE_INSTR_SWAP(enum a3xx_color_swap val)
@@ -1604,6 +1675,102 @@ static inline uint32_t A3XX_VPC_PACK_NUMNONPOSVSVAR(uint32_t val)
 static inline uint32_t REG_A3XX_VPC_VARYING_INTERP(uint32_t i0) { return 0x00002282 + 0x1*i0; }
 
 static inline uint32_t REG_A3XX_VPC_VARYING_INTERP_MODE(uint32_t i0) { return 0x00002282 + 0x1*i0; }
+#define A3XX_VPC_VARYING_INTERP_MODE_C0__MASK			0x00000003
+#define A3XX_VPC_VARYING_INTERP_MODE_C0__SHIFT			0
+static inline uint32_t A3XX_VPC_VARYING_INTERP_MODE_C0(enum a3xx_intp_mode val)
+{
+	return ((val) << A3XX_VPC_VARYING_INTERP_MODE_C0__SHIFT) & A3XX_VPC_VARYING_INTERP_MODE_C0__MASK;
+}
+#define A3XX_VPC_VARYING_INTERP_MODE_C1__MASK			0x0000000c
+#define A3XX_VPC_VARYING_INTERP_MODE_C1__SHIFT			2
+static inline uint32_t A3XX_VPC_VARYING_INTERP_MODE_C1(enum a3xx_intp_mode val)
+{
+	return ((val) << A3XX_VPC_VARYING_INTERP_MODE_C1__SHIFT) & A3XX_VPC_VARYING_INTERP_MODE_C1__MASK;
+}
+#define A3XX_VPC_VARYING_INTERP_MODE_C2__MASK			0x00000030
+#define A3XX_VPC_VARYING_INTERP_MODE_C2__SHIFT			4
+static inline uint32_t A3XX_VPC_VARYING_INTERP_MODE_C2(enum a3xx_intp_mode val)
+{
+	return ((val) << A3XX_VPC_VARYING_INTERP_MODE_C2__SHIFT) & A3XX_VPC_VARYING_INTERP_MODE_C2__MASK;
+}
+#define A3XX_VPC_VARYING_INTERP_MODE_C3__MASK			0x000000c0
+#define A3XX_VPC_VARYING_INTERP_MODE_C3__SHIFT			6
+static inline uint32_t A3XX_VPC_VARYING_INTERP_MODE_C3(enum a3xx_intp_mode val)
+{
+	return ((val) << A3XX_VPC_VARYING_INTERP_MODE_C3__SHIFT) & A3XX_VPC_VARYING_INTERP_MODE_C3__MASK;
+}
+#define A3XX_VPC_VARYING_INTERP_MODE_C4__MASK			0x00000300
+#define A3XX_VPC_VARYING_INTERP_MODE_C4__SHIFT			8
+static inline uint32_t A3XX_VPC_VARYING_INTERP_MODE_C4(enum a3xx_intp_mode val)
+{
+	return ((val) << A3XX_VPC_VARYING_INTERP_MODE_C4__SHIFT) & A3XX_VPC_VARYING_INTERP_MODE_C4__MASK;
+}
+#define A3XX_VPC_VARYING_INTERP_MODE_C5__MASK			0x00000c00
+#define A3XX_VPC_VARYING_INTERP_MODE_C5__SHIFT			10
+static inline uint32_t A3XX_VPC_VARYING_INTERP_MODE_C5(enum a3xx_intp_mode val)
+{
+	return ((val) << A3XX_VPC_VARYING_INTERP_MODE_C5__SHIFT) & A3XX_VPC_VARYING_INTERP_MODE_C5__MASK;
+}
+#define A3XX_VPC_VARYING_INTERP_MODE_C6__MASK			0x00003000
+#define A3XX_VPC_VARYING_INTERP_MODE_C6__SHIFT			12
+static inline uint32_t A3XX_VPC_VARYING_INTERP_MODE_C6(enum a3xx_intp_mode val)
+{
+	return ((val) << A3XX_VPC_VARYING_INTERP_MODE_C6__SHIFT) & A3XX_VPC_VARYING_INTERP_MODE_C6__MASK;
+}
+#define A3XX_VPC_VARYING_INTERP_MODE_C7__MASK			0x0000c000
+#define A3XX_VPC_VARYING_INTERP_MODE_C7__SHIFT			14
+static inline uint32_t A3XX_VPC_VARYING_INTERP_MODE_C7(enum a3xx_intp_mode val)
+{
+	return ((val) << A3XX_VPC_VARYING_INTERP_MODE_C7__SHIFT) & A3XX_VPC_VARYING_INTERP_MODE_C7__MASK;
+}
+#define A3XX_VPC_VARYING_INTERP_MODE_C8__MASK			0x00030000
+#define A3XX_VPC_VARYING_INTERP_MODE_C8__SHIFT			16
+static inline uint32_t A3XX_VPC_VARYING_INTERP_MODE_C8(enum a3xx_intp_mode val)
+{
+	return ((val) << A3XX_VPC_VARYING_INTERP_MODE_C8__SHIFT) & A3XX_VPC_VARYING_INTERP_MODE_C8__MASK;
+}
+#define A3XX_VPC_VARYING_INTERP_MODE_C9__MASK			0x000c0000
+#define A3XX_VPC_VARYING_INTERP_MODE_C9__SHIFT			18
+static inline uint32_t A3XX_VPC_VARYING_INTERP_MODE_C9(enum a3xx_intp_mode val)
+{
+	return ((val) << A3XX_VPC_VARYING_INTERP_MODE_C9__SHIFT) & A3XX_VPC_VARYING_INTERP_MODE_C9__MASK;
+}
+#define A3XX_VPC_VARYING_INTERP_MODE_CA__MASK			0x00300000
+#define A3XX_VPC_VARYING_INTERP_MODE_CA__SHIFT			20
+static inline uint32_t A3XX_VPC_VARYING_INTERP_MODE_CA(enum a3xx_intp_mode val)
+{
+	return ((val) << A3XX_VPC_VARYING_INTERP_MODE_CA__SHIFT) & A3XX_VPC_VARYING_INTERP_MODE_CA__MASK;
+}
+#define A3XX_VPC_VARYING_INTERP_MODE_CB__MASK			0x00c00000
+#define A3XX_VPC_VARYING_INTERP_MODE_CB__SHIFT			22
+static inline uint32_t A3XX_VPC_VARYING_INTERP_MODE_CB(enum a3xx_intp_mode val)
+{
+	return ((val) << A3XX_VPC_VARYING_INTERP_MODE_CB__SHIFT) & A3XX_VPC_VARYING_INTERP_MODE_CB__MASK;
+}
+#define A3XX_VPC_VARYING_INTERP_MODE_CC__MASK			0x03000000
+#define A3XX_VPC_VARYING_INTERP_MODE_CC__SHIFT			24
+static inline uint32_t A3XX_VPC_VARYING_INTERP_MODE_CC(enum a3xx_intp_mode val)
+{
+	return ((val) << A3XX_VPC_VARYING_INTERP_MODE_CC__SHIFT) & A3XX_VPC_VARYING_INTERP_MODE_CC__MASK;
+}
+#define A3XX_VPC_VARYING_INTERP_MODE_CD__MASK			0x0c000000
+#define A3XX_VPC_VARYING_INTERP_MODE_CD__SHIFT			26
+static inline uint32_t A3XX_VPC_VARYING_INTERP_MODE_CD(enum a3xx_intp_mode val)
+{
+	return ((val) << A3XX_VPC_VARYING_INTERP_MODE_CD__SHIFT) & A3XX_VPC_VARYING_INTERP_MODE_CD__MASK;
+}
+#define A3XX_VPC_VARYING_INTERP_MODE_CE__MASK			0x30000000
+#define A3XX_VPC_VARYING_INTERP_MODE_CE__SHIFT			28
+static inline uint32_t A3XX_VPC_VARYING_INTERP_MODE_CE(enum a3xx_intp_mode val)
+{
+	return ((val) << A3XX_VPC_VARYING_INTERP_MODE_CE__SHIFT) & A3XX_VPC_VARYING_INTERP_MODE_CE__MASK;
+}
+#define A3XX_VPC_VARYING_INTERP_MODE_CF__MASK			0xc0000000
+#define A3XX_VPC_VARYING_INTERP_MODE_CF__SHIFT			30
+static inline uint32_t A3XX_VPC_VARYING_INTERP_MODE_CF(enum a3xx_intp_mode val)
+{
+	return ((val) << A3XX_VPC_VARYING_INTERP_MODE_CF__SHIFT) & A3XX_VPC_VARYING_INTERP_MODE_CF__MASK;
+}
 
 static inline uint32_t REG_A3XX_VPC_VARYING_PS_REPL(uint32_t i0) { return 0x00002286 + 0x1*i0; }
 
@@ -1928,6 +2095,8 @@ static inline uint32_t A3XX_SP_FS_MRT_REG_REGID(uint32_t val)
 	return ((val) << A3XX_SP_FS_MRT_REG_REGID__SHIFT) & A3XX_SP_FS_MRT_REG_REGID__MASK;
 }
 #define A3XX_SP_FS_MRT_REG_HALF_PRECISION			0x00000100
+#define A3XX_SP_FS_MRT_REG_SINT					0x00000400
+#define A3XX_SP_FS_MRT_REG_UINT					0x00000800
 
 static inline uint32_t REG_A3XX_SP_FS_IMAGE_OUTPUT(uint32_t i0) { return 0x000022f4 + 0x1*i0; }
 
@@ -1947,6 +2116,8 @@ static inline uint32_t A3XX_SP_FS_LENGTH_REG_SHADERLENGTH(uint32_t val)
 	return ((val) << A3XX_SP_FS_LENGTH_REG_SHADERLENGTH__SHIFT) & A3XX_SP_FS_LENGTH_REG_SHADERLENGTH__MASK;
 }
 
+#define REG_A3XX_PA_SC_AA_CONFIG				0x00002301
+
 #define REG_A3XX_TPL1_TP_VS_TEX_OFFSET				0x00002340
 #define A3XX_TPL1_TP_VS_TEX_OFFSET_SAMPLEROFFSET__MASK		0x000000ff
 #define A3XX_TPL1_TP_VS_TEX_OFFSET_SAMPLEROFFSET__SHIFT		0
@@ -2297,11 +2468,11 @@ static inline uint32_t A3XX_VGT_DRAW_INITIATOR_INDEX_SIZE(enum pc_di_index_size
 #define A3XX_VGT_DRAW_INITIATOR_NOT_EOP				0x00001000
 #define A3XX_VGT_DRAW_INITIATOR_SMALL_INDEX			0x00002000
 #define A3XX_VGT_DRAW_INITIATOR_PRE_DRAW_INITIATOR_ENABLE	0x00004000
-#define A3XX_VGT_DRAW_INITIATOR_NUM_INDICES__MASK		0xffff0000
-#define A3XX_VGT_DRAW_INITIATOR_NUM_INDICES__SHIFT		16
-static inline uint32_t A3XX_VGT_DRAW_INITIATOR_NUM_INDICES(uint32_t val)
+#define A3XX_VGT_DRAW_INITIATOR_NUM_INSTANCES__MASK		0xff000000
+#define A3XX_VGT_DRAW_INITIATOR_NUM_INSTANCES__SHIFT		24
+static inline uint32_t A3XX_VGT_DRAW_INITIATOR_NUM_INSTANCES(uint32_t val)
 {
-	return ((val) << A3XX_VGT_DRAW_INITIATOR_NUM_INDICES__SHIFT) & A3XX_VGT_DRAW_INITIATOR_NUM_INDICES__MASK;
+	return ((val) << A3XX_VGT_DRAW_INITIATOR_NUM_INSTANCES__SHIFT) & A3XX_VGT_DRAW_INITIATOR_NUM_INSTANCES__MASK;
 }
 
 #define REG_A3XX_VGT_IMMED_DATA					0x000021fd
@@ -2347,17 +2518,23 @@ static inline uint32_t A3XX_TEX_SAMP_0_COMPARE_FUNC(enum adreno_compare_func val
 #define A3XX_TEX_SAMP_0_UNNORM_COORDS				0x80000000
 
 #define REG_A3XX_TEX_SAMP_1					0x00000001
+#define A3XX_TEX_SAMP_1_LOD_BIAS__MASK				0x000007ff
+#define A3XX_TEX_SAMP_1_LOD_BIAS__SHIFT				0
+static inline uint32_t A3XX_TEX_SAMP_1_LOD_BIAS(float val)
+{
+	return ((((int32_t)(val * 64.0))) << A3XX_TEX_SAMP_1_LOD_BIAS__SHIFT) & A3XX_TEX_SAMP_1_LOD_BIAS__MASK;
+}
 #define A3XX_TEX_SAMP_1_MAX_LOD__MASK				0x003ff000
 #define A3XX_TEX_SAMP_1_MAX_LOD__SHIFT				12
 static inline uint32_t A3XX_TEX_SAMP_1_MAX_LOD(float val)
 {
-	return ((((uint32_t)(val * 12.0))) << A3XX_TEX_SAMP_1_MAX_LOD__SHIFT) & A3XX_TEX_SAMP_1_MAX_LOD__MASK;
+	return ((((uint32_t)(val * 64.0))) << A3XX_TEX_SAMP_1_MAX_LOD__SHIFT) & A3XX_TEX_SAMP_1_MAX_LOD__MASK;
 }
 #define A3XX_TEX_SAMP_1_MIN_LOD__MASK				0xffc00000
 #define A3XX_TEX_SAMP_1_MIN_LOD__SHIFT				22
 static inline uint32_t A3XX_TEX_SAMP_1_MIN_LOD(float val)
 {
-	return ((((uint32_t)(val * 12.0))) << A3XX_TEX_SAMP_1_MIN_LOD__SHIFT) & A3XX_TEX_SAMP_1_MIN_LOD__MASK;
+	return ((((uint32_t)(val * 64.0))) << A3XX_TEX_SAMP_1_MIN_LOD__SHIFT) & A3XX_TEX_SAMP_1_MIN_LOD__MASK;
 }
 
 #define REG_A3XX_TEX_CONST_0					0x00000000
@@ -2448,6 +2625,24 @@ static inline uint32_t A3XX_TEX_CONST_2_SWAP(enum a3xx_color_swap val)
 }
 
 #define REG_A3XX_TEX_CONST_3					0x00000003
+#define A3XX_TEX_CONST_3_LAYERSZ1__MASK				0x0000000f
+#define A3XX_TEX_CONST_3_LAYERSZ1__SHIFT			0
+static inline uint32_t A3XX_TEX_CONST_3_LAYERSZ1(uint32_t val)
+{
+	return ((val >> 12) << A3XX_TEX_CONST_3_LAYERSZ1__SHIFT) & A3XX_TEX_CONST_3_LAYERSZ1__MASK;
+}
+#define A3XX_TEX_CONST_3_DEPTH__MASK				0x0ffe0000
+#define A3XX_TEX_CONST_3_DEPTH__SHIFT				17
+static inline uint32_t A3XX_TEX_CONST_3_DEPTH(uint32_t val)
+{
+	return ((val) << A3XX_TEX_CONST_3_DEPTH__SHIFT) & A3XX_TEX_CONST_3_DEPTH__MASK;
+}
+#define A3XX_TEX_CONST_3_LAYERSZ2__MASK				0xf0000000
+#define A3XX_TEX_CONST_3_LAYERSZ2__SHIFT			28
+static inline uint32_t A3XX_TEX_CONST_3_LAYERSZ2(uint32_t val)
+{
+	return ((val >> 12) << A3XX_TEX_CONST_3_LAYERSZ2__SHIFT) & A3XX_TEX_CONST_3_LAYERSZ2__MASK;
+}
 
 
 #endif /* A3XX_XML */
diff --git a/drivers/gpu/drm/msm/adreno/a3xx_gpu.c b/drivers/gpu/drm/msm/adreno/a3xx_gpu.c
index 218c5b060398..b66c53bdc039 100644
--- a/drivers/gpu/drm/msm/adreno/a3xx_gpu.c
+++ b/drivers/gpu/drm/msm/adreno/a3xx_gpu.c
@@ -2,6 +2,8 @@
  * Copyright (C) 2013 Red Hat
  * Author: Rob Clark <robdclark@gmail.com>
  *
+ * Copyright (c) 2014 The Linux Foundation. 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.
@@ -406,6 +408,94 @@ static void a3xx_dump(struct msm_gpu *gpu)
 			gpu_read(gpu, REG_A3XX_RBBM_STATUS));
 	adreno_dump(gpu);
 }
+/* Register offset defines for A3XX */
+static const unsigned int a3xx_register_offsets[REG_ADRENO_REGISTER_MAX] = {
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_DEBUG, REG_AXXX_CP_DEBUG),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_ME_RAM_WADDR, REG_AXXX_CP_ME_RAM_WADDR),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_ME_RAM_DATA, REG_AXXX_CP_ME_RAM_DATA),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_PFP_UCODE_DATA,
+			REG_A3XX_CP_PFP_UCODE_DATA),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_PFP_UCODE_ADDR,
+			REG_A3XX_CP_PFP_UCODE_ADDR),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_WFI_PEND_CTR, REG_A3XX_CP_WFI_PEND_CTR),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_RB_BASE, REG_AXXX_CP_RB_BASE),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_RB_RPTR_ADDR, REG_AXXX_CP_RB_RPTR_ADDR),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_RB_RPTR, REG_AXXX_CP_RB_RPTR),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_RB_WPTR, REG_AXXX_CP_RB_WPTR),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_PROTECT_CTRL, REG_A3XX_CP_PROTECT_CTRL),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_ME_CNTL, REG_AXXX_CP_ME_CNTL),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_RB_CNTL, REG_AXXX_CP_RB_CNTL),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_IB1_BASE, REG_AXXX_CP_IB1_BASE),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_IB1_BUFSZ, REG_AXXX_CP_IB1_BUFSZ),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_IB2_BASE, REG_AXXX_CP_IB2_BASE),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_IB2_BUFSZ, REG_AXXX_CP_IB2_BUFSZ),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_TIMESTAMP, REG_AXXX_CP_SCRATCH_REG0),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_ME_RAM_RADDR, REG_AXXX_CP_ME_RAM_RADDR),
+	REG_ADRENO_DEFINE(REG_ADRENO_SCRATCH_ADDR, REG_AXXX_SCRATCH_ADDR),
+	REG_ADRENO_DEFINE(REG_ADRENO_SCRATCH_UMSK, REG_AXXX_SCRATCH_UMSK),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_ROQ_ADDR, REG_A3XX_CP_ROQ_ADDR),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_ROQ_DATA, REG_A3XX_CP_ROQ_DATA),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_MERCIU_ADDR, REG_A3XX_CP_MERCIU_ADDR),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_MERCIU_DATA, REG_A3XX_CP_MERCIU_DATA),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_MERCIU_DATA2, REG_A3XX_CP_MERCIU_DATA2),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_MEQ_ADDR, REG_A3XX_CP_MEQ_ADDR),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_MEQ_DATA, REG_A3XX_CP_MEQ_DATA),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_HW_FAULT, REG_A3XX_CP_HW_FAULT),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_PROTECT_STATUS,
+			REG_A3XX_CP_PROTECT_STATUS),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_STATUS, REG_A3XX_RBBM_STATUS),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_PERFCTR_CTL,
+			REG_A3XX_RBBM_PERFCTR_CTL),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_PERFCTR_LOAD_CMD0,
+			REG_A3XX_RBBM_PERFCTR_LOAD_CMD0),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_PERFCTR_LOAD_CMD1,
+			REG_A3XX_RBBM_PERFCTR_LOAD_CMD1),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_PERFCTR_PWR_1_LO,
+			REG_A3XX_RBBM_PERFCTR_PWR_1_LO),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_INT_0_MASK, REG_A3XX_RBBM_INT_0_MASK),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_INT_0_STATUS,
+			REG_A3XX_RBBM_INT_0_STATUS),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_AHB_ERROR_STATUS,
+			REG_A3XX_RBBM_AHB_ERROR_STATUS),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_AHB_CMD, REG_A3XX_RBBM_AHB_CMD),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_INT_CLEAR_CMD,
+			REG_A3XX_RBBM_INT_CLEAR_CMD),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_CLOCK_CTL, REG_A3XX_RBBM_CLOCK_CTL),
+	REG_ADRENO_DEFINE(REG_ADRENO_VPC_DEBUG_RAM_SEL,
+			REG_A3XX_VPC_VPC_DEBUG_RAM_SEL),
+	REG_ADRENO_DEFINE(REG_ADRENO_VPC_DEBUG_RAM_READ,
+			REG_A3XX_VPC_VPC_DEBUG_RAM_READ),
+	REG_ADRENO_DEFINE(REG_ADRENO_VSC_SIZE_ADDRESS,
+			REG_A3XX_VSC_SIZE_ADDRESS),
+	REG_ADRENO_DEFINE(REG_ADRENO_VFD_CONTROL_0, REG_A3XX_VFD_CONTROL_0),
+	REG_ADRENO_DEFINE(REG_ADRENO_VFD_INDEX_MAX, REG_A3XX_VFD_INDEX_MAX),
+	REG_ADRENO_DEFINE(REG_ADRENO_SP_VS_PVT_MEM_ADDR_REG,
+			REG_A3XX_SP_VS_PVT_MEM_ADDR_REG),
+	REG_ADRENO_DEFINE(REG_ADRENO_SP_FS_PVT_MEM_ADDR_REG,
+			REG_A3XX_SP_FS_PVT_MEM_ADDR_REG),
+	REG_ADRENO_DEFINE(REG_ADRENO_SP_VS_OBJ_START_REG,
+			REG_A3XX_SP_VS_OBJ_START_REG),
+	REG_ADRENO_DEFINE(REG_ADRENO_SP_FS_OBJ_START_REG,
+			REG_A3XX_SP_FS_OBJ_START_REG),
+	REG_ADRENO_DEFINE(REG_ADRENO_PA_SC_AA_CONFIG, REG_A3XX_PA_SC_AA_CONFIG),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_PM_OVERRIDE2,
+			REG_A3XX_RBBM_PM_OVERRIDE2),
+	REG_ADRENO_DEFINE(REG_ADRENO_SCRATCH_REG2, REG_AXXX_CP_SCRATCH_REG2),
+	REG_ADRENO_DEFINE(REG_ADRENO_SQ_GPR_MANAGEMENT,
+			REG_A3XX_SQ_GPR_MANAGEMENT),
+	REG_ADRENO_DEFINE(REG_ADRENO_SQ_INST_STORE_MANAGMENT,
+			REG_A3XX_SQ_INST_STORE_MANAGMENT),
+	REG_ADRENO_DEFINE(REG_ADRENO_TP0_CHICKEN, REG_A3XX_TP0_CHICKEN),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_RBBM_CTL, REG_A3XX_RBBM_RBBM_CTL),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_SW_RESET_CMD,
+			REG_A3XX_RBBM_SW_RESET_CMD),
+	REG_ADRENO_DEFINE(REG_ADRENO_UCHE_INVALIDATE0,
+			REG_A3XX_UCHE_CACHE_INVALIDATE0_REG),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_PERFCTR_LOAD_VALUE_LO,
+			REG_A3XX_RBBM_PERFCTR_LOAD_VALUE_LO),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_PERFCTR_LOAD_VALUE_HI,
+			REG_A3XX_RBBM_PERFCTR_LOAD_VALUE_HI),
+};
 
 static const struct adreno_gpu_funcs funcs = {
 	.base = {
@@ -463,6 +553,7 @@ struct msm_gpu *a3xx_gpu_init(struct drm_device *dev)
 	gpu->num_perfcntrs = ARRAY_SIZE(perfcntrs);
 
 	adreno_gpu->registers = a3xx_registers;
+	adreno_gpu->reg_offsets = a3xx_register_offsets;
 
 	ret = adreno_gpu_init(dev, pdev, adreno_gpu, &funcs);
 	if (ret)
diff --git a/drivers/gpu/drm/msm/adreno/a4xx.xml.h b/drivers/gpu/drm/msm/adreno/a4xx.xml.h
new file mode 100644
index 000000000000..5a24c416d2dd
--- /dev/null
+++ b/drivers/gpu/drm/msm/adreno/a4xx.xml.h
@@ -0,0 +1,2144 @@
+#ifndef A4XX_XML
+#define A4XX_XML
+
+/* Autogenerated file, DO NOT EDIT manually!
+
+This file was generated by the rules-ng-ng headergen tool in this git repository:
+http://github.com/freedreno/envytools/
+git clone https://github.com/freedreno/envytools.git
+
+The rules-ng-ng source files this header was generated from are:
+- /home/robclark/src/freedreno/envytools/rnndb/adreno.xml               (    364 bytes, from 2013-11-30 14:47:15)
+- /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml  (   1453 bytes, from 2013-03-31 16:51:27)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/a2xx.xml          (  32901 bytes, from 2014-06-02 15:21:30)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_common.xml (  10551 bytes, from 2014-11-13 22:44:30)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_pm4.xml    (  15053 bytes, from 2014-11-09 15:45:47)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/a3xx.xml          (  63169 bytes, from 2014-11-13 22:44:18)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/a4xx.xml          (  49097 bytes, from 2014-11-14 15:38:00)
+
+Copyright (C) 2013-2014 by the following authors:
+- Rob Clark <robdclark@gmail.com> (robclark)
+
+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.
+*/
+
+
+enum a4xx_color_fmt {
+	RB4_A8_UNORM = 1,
+	RB4_R5G6R5_UNORM = 14,
+	RB4_Z16_UNORM = 15,
+	RB4_R8G8B8_UNORM = 25,
+	RB4_R8G8B8A8_UNORM = 26,
+};
+
+enum a4xx_tile_mode {
+	TILE4_LINEAR = 0,
+	TILE4_3 = 3,
+};
+
+enum a4xx_rb_blend_opcode {
+	BLEND_DST_PLUS_SRC = 0,
+	BLEND_SRC_MINUS_DST = 1,
+	BLEND_DST_MINUS_SRC = 2,
+	BLEND_MIN_DST_SRC = 3,
+	BLEND_MAX_DST_SRC = 4,
+};
+
+enum a4xx_vtx_fmt {
+	VFMT4_FLOAT_32 = 1,
+	VFMT4_FLOAT_32_32 = 2,
+	VFMT4_FLOAT_32_32_32 = 3,
+	VFMT4_FLOAT_32_32_32_32 = 4,
+	VFMT4_FLOAT_16 = 5,
+	VFMT4_FLOAT_16_16 = 6,
+	VFMT4_FLOAT_16_16_16 = 7,
+	VFMT4_FLOAT_16_16_16_16 = 8,
+	VFMT4_FIXED_32 = 9,
+	VFMT4_FIXED_32_32 = 10,
+	VFMT4_FIXED_32_32_32 = 11,
+	VFMT4_FIXED_32_32_32_32 = 12,
+	VFMT4_SHORT_16 = 16,
+	VFMT4_SHORT_16_16 = 17,
+	VFMT4_SHORT_16_16_16 = 18,
+	VFMT4_SHORT_16_16_16_16 = 19,
+	VFMT4_USHORT_16 = 20,
+	VFMT4_USHORT_16_16 = 21,
+	VFMT4_USHORT_16_16_16 = 22,
+	VFMT4_USHORT_16_16_16_16 = 23,
+	VFMT4_NORM_SHORT_16 = 24,
+	VFMT4_NORM_SHORT_16_16 = 25,
+	VFMT4_NORM_SHORT_16_16_16 = 26,
+	VFMT4_NORM_SHORT_16_16_16_16 = 27,
+	VFMT4_NORM_USHORT_16 = 28,
+	VFMT4_NORM_USHORT_16_16 = 29,
+	VFMT4_NORM_USHORT_16_16_16 = 30,
+	VFMT4_NORM_USHORT_16_16_16_16 = 31,
+	VFMT4_UBYTE_8 = 40,
+	VFMT4_UBYTE_8_8 = 41,
+	VFMT4_UBYTE_8_8_8 = 42,
+	VFMT4_UBYTE_8_8_8_8 = 43,
+	VFMT4_NORM_UBYTE_8 = 44,
+	VFMT4_NORM_UBYTE_8_8 = 45,
+	VFMT4_NORM_UBYTE_8_8_8 = 46,
+	VFMT4_NORM_UBYTE_8_8_8_8 = 47,
+	VFMT4_BYTE_8 = 48,
+	VFMT4_BYTE_8_8 = 49,
+	VFMT4_BYTE_8_8_8 = 50,
+	VFMT4_BYTE_8_8_8_8 = 51,
+	VFMT4_NORM_BYTE_8 = 52,
+	VFMT4_NORM_BYTE_8_8 = 53,
+	VFMT4_NORM_BYTE_8_8_8 = 54,
+	VFMT4_NORM_BYTE_8_8_8_8 = 55,
+	VFMT4_UINT_10_10_10_2 = 60,
+	VFMT4_NORM_UINT_10_10_10_2 = 61,
+	VFMT4_INT_10_10_10_2 = 62,
+	VFMT4_NORM_INT_10_10_10_2 = 63,
+};
+
+enum a4xx_tex_fmt {
+	TFMT4_NORM_USHORT_565 = 11,
+	TFMT4_NORM_USHORT_5551 = 10,
+	TFMT4_NORM_USHORT_4444 = 8,
+	TFMT4_NORM_UINT_X8Z24 = 71,
+	TFMT4_NORM_UINT_2_10_10_10 = 33,
+	TFMT4_NORM_UINT_A8 = 3,
+	TFMT4_NORM_UINT_L8_A8 = 13,
+	TFMT4_NORM_UINT_8 = 4,
+	TFMT4_NORM_UINT_8_8_8_8 = 28,
+	TFMT4_FLOAT_16 = 20,
+	TFMT4_FLOAT_16_16 = 40,
+	TFMT4_FLOAT_16_16_16_16 = 53,
+	TFMT4_FLOAT_32 = 43,
+	TFMT4_FLOAT_32_32 = 56,
+	TFMT4_FLOAT_32_32_32_32 = 63,
+};
+
+enum a4xx_depth_format {
+	DEPTH4_NONE = 0,
+	DEPTH4_16 = 1,
+	DEPTH4_24_8 = 2,
+};
+
+enum a4xx_tex_filter {
+	A4XX_TEX_NEAREST = 0,
+	A4XX_TEX_LINEAR = 1,
+};
+
+enum a4xx_tex_clamp {
+	A4XX_TEX_REPEAT = 0,
+	A4XX_TEX_CLAMP_TO_EDGE = 1,
+	A4XX_TEX_MIRROR_REPEAT = 2,
+	A4XX_TEX_CLAMP_NONE = 3,
+};
+
+enum a4xx_tex_swiz {
+	A4XX_TEX_X = 0,
+	A4XX_TEX_Y = 1,
+	A4XX_TEX_Z = 2,
+	A4XX_TEX_W = 3,
+	A4XX_TEX_ZERO = 4,
+	A4XX_TEX_ONE = 5,
+};
+
+enum a4xx_tex_type {
+	A4XX_TEX_1D = 0,
+	A4XX_TEX_2D = 1,
+	A4XX_TEX_CUBE = 2,
+	A4XX_TEX_3D = 3,
+};
+
+#define A4XX_CGC_HLSQ_EARLY_CYC__MASK				0x00700000
+#define A4XX_CGC_HLSQ_EARLY_CYC__SHIFT				20
+static inline uint32_t A4XX_CGC_HLSQ_EARLY_CYC(uint32_t val)
+{
+	return ((val) << A4XX_CGC_HLSQ_EARLY_CYC__SHIFT) & A4XX_CGC_HLSQ_EARLY_CYC__MASK;
+}
+#define A4XX_INT0_RBBM_GPU_IDLE					0x00000001
+#define A4XX_INT0_RBBM_AHB_ERROR				0x00000002
+#define A4XX_INT0_RBBM_REG_TIMEOUT				0x00000004
+#define A4XX_INT0_RBBM_ME_MS_TIMEOUT				0x00000008
+#define A4XX_INT0_RBBM_PFP_MS_TIMEOUT				0x00000010
+#define A4XX_INT0_RBBM_ATB_BUS_OVERFLOW				0x00000020
+#define A4XX_INT0_VFD_ERROR					0x00000040
+#define A4XX_INT0_CP_SW_INT					0x00000080
+#define A4XX_INT0_CP_T0_PACKET_IN_IB				0x00000100
+#define A4XX_INT0_CP_OPCODE_ERROR				0x00000200
+#define A4XX_INT0_CP_RESERVED_BIT_ERROR				0x00000400
+#define A4XX_INT0_CP_HW_FAULT					0x00000800
+#define A4XX_INT0_CP_DMA					0x00001000
+#define A4XX_INT0_CP_IB2_INT					0x00002000
+#define A4XX_INT0_CP_IB1_INT					0x00004000
+#define A4XX_INT0_CP_RB_INT					0x00008000
+#define A4XX_INT0_CP_REG_PROTECT_FAULT				0x00010000
+#define A4XX_INT0_CP_RB_DONE_TS					0x00020000
+#define A4XX_INT0_CP_VS_DONE_TS					0x00040000
+#define A4XX_INT0_CP_PS_DONE_TS					0x00080000
+#define A4XX_INT0_CACHE_FLUSH_TS				0x00100000
+#define A4XX_INT0_CP_AHB_ERROR_HALT				0x00200000
+#define A4XX_INT0_MISC_HANG_DETECT				0x01000000
+#define A4XX_INT0_UCHE_OOB_ACCESS				0x02000000
+#define REG_A4XX_RB_GMEM_BASE_ADDR				0x00000cc0
+
+#define REG_A4XX_RB_PERFCTR_RB_SEL_0				0x00000cc7
+
+#define REG_A4XX_RB_PERFCTR_RB_SEL_1				0x00000cc8
+
+#define REG_A4XX_RB_PERFCTR_RB_SEL_2				0x00000cc9
+
+#define REG_A4XX_RB_PERFCTR_RB_SEL_3				0x00000cca
+
+#define REG_A4XX_RB_PERFCTR_RB_SEL_4				0x00000ccb
+
+#define REG_A4XX_RB_PERFCTR_RB_SEL_5				0x00000ccc
+
+#define REG_A4XX_RB_PERFCTR_RB_SEL_6				0x00000ccd
+
+#define REG_A4XX_RB_PERFCTR_RB_SEL_7				0x00000cce
+
+#define REG_A4XX_RB_PERFCTR_CCU_SEL_3				0x00000cd2
+
+#define REG_A4XX_RB_FRAME_BUFFER_DIMENSION			0x00000ce0
+#define A4XX_RB_FRAME_BUFFER_DIMENSION_WIDTH__MASK		0x00003fff
+#define A4XX_RB_FRAME_BUFFER_DIMENSION_WIDTH__SHIFT		0
+static inline uint32_t A4XX_RB_FRAME_BUFFER_DIMENSION_WIDTH(uint32_t val)
+{
+	return ((val) << A4XX_RB_FRAME_BUFFER_DIMENSION_WIDTH__SHIFT) & A4XX_RB_FRAME_BUFFER_DIMENSION_WIDTH__MASK;
+}
+#define A4XX_RB_FRAME_BUFFER_DIMENSION_HEIGHT__MASK		0x3fff0000
+#define A4XX_RB_FRAME_BUFFER_DIMENSION_HEIGHT__SHIFT		16
+static inline uint32_t A4XX_RB_FRAME_BUFFER_DIMENSION_HEIGHT(uint32_t val)
+{
+	return ((val) << A4XX_RB_FRAME_BUFFER_DIMENSION_HEIGHT__SHIFT) & A4XX_RB_FRAME_BUFFER_DIMENSION_HEIGHT__MASK;
+}
+
+#define REG_A4XX_RB_CLEAR_COLOR_DW0				0x000020cc
+
+#define REG_A4XX_RB_CLEAR_COLOR_DW1				0x000020cd
+
+#define REG_A4XX_RB_CLEAR_COLOR_DW2				0x000020ce
+
+#define REG_A4XX_RB_CLEAR_COLOR_DW3				0x000020cf
+
+#define REG_A4XX_RB_MODE_CONTROL				0x000020a0
+#define A4XX_RB_MODE_CONTROL_WIDTH__MASK			0x0000003f
+#define A4XX_RB_MODE_CONTROL_WIDTH__SHIFT			0
+static inline uint32_t A4XX_RB_MODE_CONTROL_WIDTH(uint32_t val)
+{
+	return ((val >> 5) << A4XX_RB_MODE_CONTROL_WIDTH__SHIFT) & A4XX_RB_MODE_CONTROL_WIDTH__MASK;
+}
+#define A4XX_RB_MODE_CONTROL_HEIGHT__MASK			0x00003f00
+#define A4XX_RB_MODE_CONTROL_HEIGHT__SHIFT			8
+static inline uint32_t A4XX_RB_MODE_CONTROL_HEIGHT(uint32_t val)
+{
+	return ((val >> 5) << A4XX_RB_MODE_CONTROL_HEIGHT__SHIFT) & A4XX_RB_MODE_CONTROL_HEIGHT__MASK;
+}
+
+#define REG_A4XX_RB_RENDER_CONTROL				0x000020a1
+#define A4XX_RB_RENDER_CONTROL_BINNING_PASS			0x00000001
+#define A4XX_RB_RENDER_CONTROL_DISABLE_COLOR_PIPE		0x00000020
+
+#define REG_A4XX_RB_MSAA_CONTROL				0x000020a2
+#define A4XX_RB_MSAA_CONTROL_DISABLE				0x00001000
+#define A4XX_RB_MSAA_CONTROL_SAMPLES__MASK			0x0000e000
+#define A4XX_RB_MSAA_CONTROL_SAMPLES__SHIFT			13
+static inline uint32_t A4XX_RB_MSAA_CONTROL_SAMPLES(uint32_t val)
+{
+	return ((val) << A4XX_RB_MSAA_CONTROL_SAMPLES__SHIFT) & A4XX_RB_MSAA_CONTROL_SAMPLES__MASK;
+}
+
+#define REG_A4XX_RB_MSAA_CONTROL2				0x000020a3
+#define A4XX_RB_MSAA_CONTROL2_MSAA_SAMPLES__MASK		0x00000380
+#define A4XX_RB_MSAA_CONTROL2_MSAA_SAMPLES__SHIFT		7
+static inline uint32_t A4XX_RB_MSAA_CONTROL2_MSAA_SAMPLES(uint32_t val)
+{
+	return ((val) << A4XX_RB_MSAA_CONTROL2_MSAA_SAMPLES__SHIFT) & A4XX_RB_MSAA_CONTROL2_MSAA_SAMPLES__MASK;
+}
+#define A4XX_RB_MSAA_CONTROL2_VARYING				0x00001000
+
+static inline uint32_t REG_A4XX_RB_MRT(uint32_t i0) { return 0x000020a4 + 0x5*i0; }
+
+static inline uint32_t REG_A4XX_RB_MRT_CONTROL(uint32_t i0) { return 0x000020a4 + 0x5*i0; }
+#define A4XX_RB_MRT_CONTROL_READ_DEST_ENABLE			0x00000008
+#define A4XX_RB_MRT_CONTROL_BLEND				0x00000010
+#define A4XX_RB_MRT_CONTROL_BLEND2				0x00000020
+#define A4XX_RB_MRT_CONTROL_FASTCLEAR				0x00000400
+#define A4XX_RB_MRT_CONTROL_B11					0x00000800
+#define A4XX_RB_MRT_CONTROL_COMPONENT_ENABLE__MASK		0x0f000000
+#define A4XX_RB_MRT_CONTROL_COMPONENT_ENABLE__SHIFT		24
+static inline uint32_t A4XX_RB_MRT_CONTROL_COMPONENT_ENABLE(uint32_t val)
+{
+	return ((val) << A4XX_RB_MRT_CONTROL_COMPONENT_ENABLE__SHIFT) & A4XX_RB_MRT_CONTROL_COMPONENT_ENABLE__MASK;
+}
+
+static inline uint32_t REG_A4XX_RB_MRT_BUF_INFO(uint32_t i0) { return 0x000020a5 + 0x5*i0; }
+#define A4XX_RB_MRT_BUF_INFO_COLOR_FORMAT__MASK			0x0000003f
+#define A4XX_RB_MRT_BUF_INFO_COLOR_FORMAT__SHIFT		0
+static inline uint32_t A4XX_RB_MRT_BUF_INFO_COLOR_FORMAT(enum a4xx_color_fmt val)
+{
+	return ((val) << A4XX_RB_MRT_BUF_INFO_COLOR_FORMAT__SHIFT) & A4XX_RB_MRT_BUF_INFO_COLOR_FORMAT__MASK;
+}
+#define A4XX_RB_MRT_BUF_INFO_DITHER_MODE__MASK			0x00000600
+#define A4XX_RB_MRT_BUF_INFO_DITHER_MODE__SHIFT			9
+static inline uint32_t A4XX_RB_MRT_BUF_INFO_DITHER_MODE(enum adreno_rb_dither_mode val)
+{
+	return ((val) << A4XX_RB_MRT_BUF_INFO_DITHER_MODE__SHIFT) & A4XX_RB_MRT_BUF_INFO_DITHER_MODE__MASK;
+}
+#define A4XX_RB_MRT_BUF_INFO_COLOR_SWAP__MASK			0x00001800
+#define A4XX_RB_MRT_BUF_INFO_COLOR_SWAP__SHIFT			11
+static inline uint32_t A4XX_RB_MRT_BUF_INFO_COLOR_SWAP(enum a3xx_color_swap val)
+{
+	return ((val) << A4XX_RB_MRT_BUF_INFO_COLOR_SWAP__SHIFT) & A4XX_RB_MRT_BUF_INFO_COLOR_SWAP__MASK;
+}
+#define A4XX_RB_MRT_BUF_INFO_COLOR_BUF_PITCH__MASK		0x007fc000
+#define A4XX_RB_MRT_BUF_INFO_COLOR_BUF_PITCH__SHIFT		14
+static inline uint32_t A4XX_RB_MRT_BUF_INFO_COLOR_BUF_PITCH(uint32_t val)
+{
+	return ((val >> 4) << A4XX_RB_MRT_BUF_INFO_COLOR_BUF_PITCH__SHIFT) & A4XX_RB_MRT_BUF_INFO_COLOR_BUF_PITCH__MASK;
+}
+
+static inline uint32_t REG_A4XX_RB_MRT_BASE(uint32_t i0) { return 0x000020a6 + 0x5*i0; }
+
+static inline uint32_t REG_A4XX_RB_MRT_CONTROL3(uint32_t i0) { return 0x000020a7 + 0x5*i0; }
+#define A4XX_RB_MRT_CONTROL3_STRIDE__MASK			0x0001fff8
+#define A4XX_RB_MRT_CONTROL3_STRIDE__SHIFT			3
+static inline uint32_t A4XX_RB_MRT_CONTROL3_STRIDE(uint32_t val)
+{
+	return ((val) << A4XX_RB_MRT_CONTROL3_STRIDE__SHIFT) & A4XX_RB_MRT_CONTROL3_STRIDE__MASK;
+}
+
+static inline uint32_t REG_A4XX_RB_MRT_BLEND_CONTROL(uint32_t i0) { return 0x000020a8 + 0x5*i0; }
+#define A4XX_RB_MRT_BLEND_CONTROL_RGB_SRC_FACTOR__MASK		0x0000001f
+#define A4XX_RB_MRT_BLEND_CONTROL_RGB_SRC_FACTOR__SHIFT		0
+static inline uint32_t A4XX_RB_MRT_BLEND_CONTROL_RGB_SRC_FACTOR(enum adreno_rb_blend_factor val)
+{
+	return ((val) << A4XX_RB_MRT_BLEND_CONTROL_RGB_SRC_FACTOR__SHIFT) & A4XX_RB_MRT_BLEND_CONTROL_RGB_SRC_FACTOR__MASK;
+}
+#define A4XX_RB_MRT_BLEND_CONTROL_RGB_BLEND_OPCODE__MASK	0x000000e0
+#define A4XX_RB_MRT_BLEND_CONTROL_RGB_BLEND_OPCODE__SHIFT	5
+static inline uint32_t A4XX_RB_MRT_BLEND_CONTROL_RGB_BLEND_OPCODE(enum a4xx_rb_blend_opcode val)
+{
+	return ((val) << A4XX_RB_MRT_BLEND_CONTROL_RGB_BLEND_OPCODE__SHIFT) & A4XX_RB_MRT_BLEND_CONTROL_RGB_BLEND_OPCODE__MASK;
+}
+#define A4XX_RB_MRT_BLEND_CONTROL_RGB_DEST_FACTOR__MASK		0x00001f00
+#define A4XX_RB_MRT_BLEND_CONTROL_RGB_DEST_FACTOR__SHIFT	8
+static inline uint32_t A4XX_RB_MRT_BLEND_CONTROL_RGB_DEST_FACTOR(enum adreno_rb_blend_factor val)
+{
+	return ((val) << A4XX_RB_MRT_BLEND_CONTROL_RGB_DEST_FACTOR__SHIFT) & A4XX_RB_MRT_BLEND_CONTROL_RGB_DEST_FACTOR__MASK;
+}
+#define A4XX_RB_MRT_BLEND_CONTROL_ALPHA_SRC_FACTOR__MASK	0x001f0000
+#define A4XX_RB_MRT_BLEND_CONTROL_ALPHA_SRC_FACTOR__SHIFT	16
+static inline uint32_t A4XX_RB_MRT_BLEND_CONTROL_ALPHA_SRC_FACTOR(enum adreno_rb_blend_factor val)
+{
+	return ((val) << A4XX_RB_MRT_BLEND_CONTROL_ALPHA_SRC_FACTOR__SHIFT) & A4XX_RB_MRT_BLEND_CONTROL_ALPHA_SRC_FACTOR__MASK;
+}
+#define A4XX_RB_MRT_BLEND_CONTROL_ALPHA_BLEND_OPCODE__MASK	0x00e00000
+#define A4XX_RB_MRT_BLEND_CONTROL_ALPHA_BLEND_OPCODE__SHIFT	21
+static inline uint32_t A4XX_RB_MRT_BLEND_CONTROL_ALPHA_BLEND_OPCODE(enum a4xx_rb_blend_opcode val)
+{
+	return ((val) << A4XX_RB_MRT_BLEND_CONTROL_ALPHA_BLEND_OPCODE__SHIFT) & A4XX_RB_MRT_BLEND_CONTROL_ALPHA_BLEND_OPCODE__MASK;
+}
+#define A4XX_RB_MRT_BLEND_CONTROL_ALPHA_DEST_FACTOR__MASK	0x1f000000
+#define A4XX_RB_MRT_BLEND_CONTROL_ALPHA_DEST_FACTOR__SHIFT	24
+static inline uint32_t A4XX_RB_MRT_BLEND_CONTROL_ALPHA_DEST_FACTOR(enum adreno_rb_blend_factor val)
+{
+	return ((val) << A4XX_RB_MRT_BLEND_CONTROL_ALPHA_DEST_FACTOR__SHIFT) & A4XX_RB_MRT_BLEND_CONTROL_ALPHA_DEST_FACTOR__MASK;
+}
+
+#define REG_A4XX_RB_ALPHA_CONTROL				0x000020f8
+#define A4XX_RB_ALPHA_CONTROL_ALPHA_TEST			0x00000100
+#define A4XX_RB_ALPHA_CONTROL_ALPHA_TEST_FUNC__MASK		0x00000e00
+#define A4XX_RB_ALPHA_CONTROL_ALPHA_TEST_FUNC__SHIFT		9
+static inline uint32_t A4XX_RB_ALPHA_CONTROL_ALPHA_TEST_FUNC(enum adreno_compare_func val)
+{
+	return ((val) << A4XX_RB_ALPHA_CONTROL_ALPHA_TEST_FUNC__SHIFT) & A4XX_RB_ALPHA_CONTROL_ALPHA_TEST_FUNC__MASK;
+}
+
+#define REG_A4XX_RB_FS_OUTPUT					0x000020f9
+#define A4XX_RB_FS_OUTPUT_ENABLE_COLOR_PIPE			0x00000001
+#define A4XX_RB_FS_OUTPUT_FAST_CLEAR				0x00000100
+#define A4XX_RB_FS_OUTPUT_SAMPLE_MASK__MASK			0xffff0000
+#define A4XX_RB_FS_OUTPUT_SAMPLE_MASK__SHIFT			16
+static inline uint32_t A4XX_RB_FS_OUTPUT_SAMPLE_MASK(uint32_t val)
+{
+	return ((val) << A4XX_RB_FS_OUTPUT_SAMPLE_MASK__SHIFT) & A4XX_RB_FS_OUTPUT_SAMPLE_MASK__MASK;
+}
+
+#define REG_A4XX_RB_RENDER_CONTROL3				0x000020fb
+#define A4XX_RB_RENDER_CONTROL3_COMPONENT_ENABLE__MASK		0x0000001f
+#define A4XX_RB_RENDER_CONTROL3_COMPONENT_ENABLE__SHIFT		0
+static inline uint32_t A4XX_RB_RENDER_CONTROL3_COMPONENT_ENABLE(uint32_t val)
+{
+	return ((val) << A4XX_RB_RENDER_CONTROL3_COMPONENT_ENABLE__SHIFT) & A4XX_RB_RENDER_CONTROL3_COMPONENT_ENABLE__MASK;
+}
+
+#define REG_A4XX_RB_COPY_CONTROL				0x000020fc
+#define A4XX_RB_COPY_CONTROL_MSAA_RESOLVE__MASK			0x00000003
+#define A4XX_RB_COPY_CONTROL_MSAA_RESOLVE__SHIFT		0
+static inline uint32_t A4XX_RB_COPY_CONTROL_MSAA_RESOLVE(enum a3xx_msaa_samples val)
+{
+	return ((val) << A4XX_RB_COPY_CONTROL_MSAA_RESOLVE__SHIFT) & A4XX_RB_COPY_CONTROL_MSAA_RESOLVE__MASK;
+}
+#define A4XX_RB_COPY_CONTROL_MODE__MASK				0x00000070
+#define A4XX_RB_COPY_CONTROL_MODE__SHIFT			4
+static inline uint32_t A4XX_RB_COPY_CONTROL_MODE(enum adreno_rb_copy_control_mode val)
+{
+	return ((val) << A4XX_RB_COPY_CONTROL_MODE__SHIFT) & A4XX_RB_COPY_CONTROL_MODE__MASK;
+}
+#define A4XX_RB_COPY_CONTROL_FASTCLEAR__MASK			0x00000f00
+#define A4XX_RB_COPY_CONTROL_FASTCLEAR__SHIFT			8
+static inline uint32_t A4XX_RB_COPY_CONTROL_FASTCLEAR(uint32_t val)
+{
+	return ((val) << A4XX_RB_COPY_CONTROL_FASTCLEAR__SHIFT) & A4XX_RB_COPY_CONTROL_FASTCLEAR__MASK;
+}
+#define A4XX_RB_COPY_CONTROL_GMEM_BASE__MASK			0xffffc000
+#define A4XX_RB_COPY_CONTROL_GMEM_BASE__SHIFT			14
+static inline uint32_t A4XX_RB_COPY_CONTROL_GMEM_BASE(uint32_t val)
+{
+	return ((val >> 14) << A4XX_RB_COPY_CONTROL_GMEM_BASE__SHIFT) & A4XX_RB_COPY_CONTROL_GMEM_BASE__MASK;
+}
+
+#define REG_A4XX_RB_COPY_DEST_BASE				0x000020fd
+#define A4XX_RB_COPY_DEST_BASE_BASE__MASK			0xfffffff0
+#define A4XX_RB_COPY_DEST_BASE_BASE__SHIFT			4
+static inline uint32_t A4XX_RB_COPY_DEST_BASE_BASE(uint32_t val)
+{
+	return ((val >> 4) << A4XX_RB_COPY_DEST_BASE_BASE__SHIFT) & A4XX_RB_COPY_DEST_BASE_BASE__MASK;
+}
+
+#define REG_A4XX_RB_COPY_DEST_PITCH				0x000020fe
+#define A4XX_RB_COPY_DEST_PITCH_PITCH__MASK			0xffffffff
+#define A4XX_RB_COPY_DEST_PITCH_PITCH__SHIFT			0
+static inline uint32_t A4XX_RB_COPY_DEST_PITCH_PITCH(uint32_t val)
+{
+	return ((val >> 5) << A4XX_RB_COPY_DEST_PITCH_PITCH__SHIFT) & A4XX_RB_COPY_DEST_PITCH_PITCH__MASK;
+}
+
+#define REG_A4XX_RB_COPY_DEST_INFO				0x000020ff
+#define A4XX_RB_COPY_DEST_INFO_FORMAT__MASK			0x000000fc
+#define A4XX_RB_COPY_DEST_INFO_FORMAT__SHIFT			2
+static inline uint32_t A4XX_RB_COPY_DEST_INFO_FORMAT(enum a4xx_color_fmt val)
+{
+	return ((val) << A4XX_RB_COPY_DEST_INFO_FORMAT__SHIFT) & A4XX_RB_COPY_DEST_INFO_FORMAT__MASK;
+}
+#define A4XX_RB_COPY_DEST_INFO_SWAP__MASK			0x00000300
+#define A4XX_RB_COPY_DEST_INFO_SWAP__SHIFT			8
+static inline uint32_t A4XX_RB_COPY_DEST_INFO_SWAP(enum a3xx_color_swap val)
+{
+	return ((val) << A4XX_RB_COPY_DEST_INFO_SWAP__SHIFT) & A4XX_RB_COPY_DEST_INFO_SWAP__MASK;
+}
+#define A4XX_RB_COPY_DEST_INFO_DITHER_MODE__MASK		0x00000c00
+#define A4XX_RB_COPY_DEST_INFO_DITHER_MODE__SHIFT		10
+static inline uint32_t A4XX_RB_COPY_DEST_INFO_DITHER_MODE(enum adreno_rb_dither_mode val)
+{
+	return ((val) << A4XX_RB_COPY_DEST_INFO_DITHER_MODE__SHIFT) & A4XX_RB_COPY_DEST_INFO_DITHER_MODE__MASK;
+}
+#define A4XX_RB_COPY_DEST_INFO_COMPONENT_ENABLE__MASK		0x0003c000
+#define A4XX_RB_COPY_DEST_INFO_COMPONENT_ENABLE__SHIFT		14
+static inline uint32_t A4XX_RB_COPY_DEST_INFO_COMPONENT_ENABLE(uint32_t val)
+{
+	return ((val) << A4XX_RB_COPY_DEST_INFO_COMPONENT_ENABLE__SHIFT) & A4XX_RB_COPY_DEST_INFO_COMPONENT_ENABLE__MASK;
+}
+#define A4XX_RB_COPY_DEST_INFO_ENDIAN__MASK			0x001c0000
+#define A4XX_RB_COPY_DEST_INFO_ENDIAN__SHIFT			18
+static inline uint32_t A4XX_RB_COPY_DEST_INFO_ENDIAN(enum adreno_rb_surface_endian val)
+{
+	return ((val) << A4XX_RB_COPY_DEST_INFO_ENDIAN__SHIFT) & A4XX_RB_COPY_DEST_INFO_ENDIAN__MASK;
+}
+#define A4XX_RB_COPY_DEST_INFO_TILE__MASK			0x03000000
+#define A4XX_RB_COPY_DEST_INFO_TILE__SHIFT			24
+static inline uint32_t A4XX_RB_COPY_DEST_INFO_TILE(enum a4xx_tile_mode val)
+{
+	return ((val) << A4XX_RB_COPY_DEST_INFO_TILE__SHIFT) & A4XX_RB_COPY_DEST_INFO_TILE__MASK;
+}
+
+#define REG_A4XX_RB_FS_OUTPUT_REG				0x00002100
+#define A4XX_RB_FS_OUTPUT_REG_COLOR_PIPE_ENABLE			0x00000001
+#define A4XX_RB_FS_OUTPUT_REG_FRAG_WRITES_Z			0x00000020
+
+#define REG_A4XX_RB_DEPTH_CONTROL				0x00002101
+#define A4XX_RB_DEPTH_CONTROL_FRAG_WRITES_Z			0x00000001
+#define A4XX_RB_DEPTH_CONTROL_Z_ENABLE				0x00000002
+#define A4XX_RB_DEPTH_CONTROL_Z_WRITE_ENABLE			0x00000004
+#define A4XX_RB_DEPTH_CONTROL_ZFUNC__MASK			0x00000070
+#define A4XX_RB_DEPTH_CONTROL_ZFUNC__SHIFT			4
+static inline uint32_t A4XX_RB_DEPTH_CONTROL_ZFUNC(enum adreno_compare_func val)
+{
+	return ((val) << A4XX_RB_DEPTH_CONTROL_ZFUNC__SHIFT) & A4XX_RB_DEPTH_CONTROL_ZFUNC__MASK;
+}
+#define A4XX_RB_DEPTH_CONTROL_BF_ENABLE				0x00000080
+#define A4XX_RB_DEPTH_CONTROL_EARLY_Z_DISABLE			0x00010000
+#define A4XX_RB_DEPTH_CONTROL_Z_TEST_ENABLE			0x80000000
+
+#define REG_A4XX_RB_DEPTH_CLEAR					0x00002102
+
+#define REG_A4XX_RB_DEPTH_INFO					0x00002103
+#define A4XX_RB_DEPTH_INFO_DEPTH_FORMAT__MASK			0x00000003
+#define A4XX_RB_DEPTH_INFO_DEPTH_FORMAT__SHIFT			0
+static inline uint32_t A4XX_RB_DEPTH_INFO_DEPTH_FORMAT(enum a4xx_depth_format val)
+{
+	return ((val) << A4XX_RB_DEPTH_INFO_DEPTH_FORMAT__SHIFT) & A4XX_RB_DEPTH_INFO_DEPTH_FORMAT__MASK;
+}
+#define A4XX_RB_DEPTH_INFO_DEPTH_BASE__MASK			0xfffff000
+#define A4XX_RB_DEPTH_INFO_DEPTH_BASE__SHIFT			12
+static inline uint32_t A4XX_RB_DEPTH_INFO_DEPTH_BASE(uint32_t val)
+{
+	return ((val >> 12) << A4XX_RB_DEPTH_INFO_DEPTH_BASE__SHIFT) & A4XX_RB_DEPTH_INFO_DEPTH_BASE__MASK;
+}
+
+#define REG_A4XX_RB_DEPTH_PITCH					0x00002104
+#define A4XX_RB_DEPTH_PITCH__MASK				0xffffffff
+#define A4XX_RB_DEPTH_PITCH__SHIFT				0
+static inline uint32_t A4XX_RB_DEPTH_PITCH(uint32_t val)
+{
+	return ((val >> 4) << A4XX_RB_DEPTH_PITCH__SHIFT) & A4XX_RB_DEPTH_PITCH__MASK;
+}
+
+#define REG_A4XX_RB_DEPTH_PITCH2				0x00002105
+#define A4XX_RB_DEPTH_PITCH2__MASK				0xffffffff
+#define A4XX_RB_DEPTH_PITCH2__SHIFT				0
+static inline uint32_t A4XX_RB_DEPTH_PITCH2(uint32_t val)
+{
+	return ((val >> 4) << A4XX_RB_DEPTH_PITCH2__SHIFT) & A4XX_RB_DEPTH_PITCH2__MASK;
+}
+
+#define REG_A4XX_RB_STENCIL_CONTROL				0x00002106
+#define A4XX_RB_STENCIL_CONTROL_STENCIL_ENABLE			0x00000001
+#define A4XX_RB_STENCIL_CONTROL_STENCIL_ENABLE_BF		0x00000002
+#define A4XX_RB_STENCIL_CONTROL_STENCIL_READ			0x00000004
+#define A4XX_RB_STENCIL_CONTROL_FUNC__MASK			0x00000700
+#define A4XX_RB_STENCIL_CONTROL_FUNC__SHIFT			8
+static inline uint32_t A4XX_RB_STENCIL_CONTROL_FUNC(enum adreno_compare_func val)
+{
+	return ((val) << A4XX_RB_STENCIL_CONTROL_FUNC__SHIFT) & A4XX_RB_STENCIL_CONTROL_FUNC__MASK;
+}
+#define A4XX_RB_STENCIL_CONTROL_FAIL__MASK			0x00003800
+#define A4XX_RB_STENCIL_CONTROL_FAIL__SHIFT			11
+static inline uint32_t A4XX_RB_STENCIL_CONTROL_FAIL(enum adreno_stencil_op val)
+{
+	return ((val) << A4XX_RB_STENCIL_CONTROL_FAIL__SHIFT) & A4XX_RB_STENCIL_CONTROL_FAIL__MASK;
+}
+#define A4XX_RB_STENCIL_CONTROL_ZPASS__MASK			0x0001c000
+#define A4XX_RB_STENCIL_CONTROL_ZPASS__SHIFT			14
+static inline uint32_t A4XX_RB_STENCIL_CONTROL_ZPASS(enum adreno_stencil_op val)
+{
+	return ((val) << A4XX_RB_STENCIL_CONTROL_ZPASS__SHIFT) & A4XX_RB_STENCIL_CONTROL_ZPASS__MASK;
+}
+#define A4XX_RB_STENCIL_CONTROL_ZFAIL__MASK			0x000e0000
+#define A4XX_RB_STENCIL_CONTROL_ZFAIL__SHIFT			17
+static inline uint32_t A4XX_RB_STENCIL_CONTROL_ZFAIL(enum adreno_stencil_op val)
+{
+	return ((val) << A4XX_RB_STENCIL_CONTROL_ZFAIL__SHIFT) & A4XX_RB_STENCIL_CONTROL_ZFAIL__MASK;
+}
+#define A4XX_RB_STENCIL_CONTROL_FUNC_BF__MASK			0x00700000
+#define A4XX_RB_STENCIL_CONTROL_FUNC_BF__SHIFT			20
+static inline uint32_t A4XX_RB_STENCIL_CONTROL_FUNC_BF(enum adreno_compare_func val)
+{
+	return ((val) << A4XX_RB_STENCIL_CONTROL_FUNC_BF__SHIFT) & A4XX_RB_STENCIL_CONTROL_FUNC_BF__MASK;
+}
+#define A4XX_RB_STENCIL_CONTROL_FAIL_BF__MASK			0x03800000
+#define A4XX_RB_STENCIL_CONTROL_FAIL_BF__SHIFT			23
+static inline uint32_t A4XX_RB_STENCIL_CONTROL_FAIL_BF(enum adreno_stencil_op val)
+{
+	return ((val) << A4XX_RB_STENCIL_CONTROL_FAIL_BF__SHIFT) & A4XX_RB_STENCIL_CONTROL_FAIL_BF__MASK;
+}
+#define A4XX_RB_STENCIL_CONTROL_ZPASS_BF__MASK			0x1c000000
+#define A4XX_RB_STENCIL_CONTROL_ZPASS_BF__SHIFT			26
+static inline uint32_t A4XX_RB_STENCIL_CONTROL_ZPASS_BF(enum adreno_stencil_op val)
+{
+	return ((val) << A4XX_RB_STENCIL_CONTROL_ZPASS_BF__SHIFT) & A4XX_RB_STENCIL_CONTROL_ZPASS_BF__MASK;
+}
+#define A4XX_RB_STENCIL_CONTROL_ZFAIL_BF__MASK			0xe0000000
+#define A4XX_RB_STENCIL_CONTROL_ZFAIL_BF__SHIFT			29
+static inline uint32_t A4XX_RB_STENCIL_CONTROL_ZFAIL_BF(enum adreno_stencil_op val)
+{
+	return ((val) << A4XX_RB_STENCIL_CONTROL_ZFAIL_BF__SHIFT) & A4XX_RB_STENCIL_CONTROL_ZFAIL_BF__MASK;
+}
+
+#define REG_A4XX_RB_STENCIL_CONTROL2				0x00002107
+#define A4XX_RB_STENCIL_CONTROL2_STENCIL_BUFFER			0x00000001
+
+#define REG_A4XX_RB_STENCILREFMASK				0x0000210b
+#define A4XX_RB_STENCILREFMASK_STENCILREF__MASK			0x000000ff
+#define A4XX_RB_STENCILREFMASK_STENCILREF__SHIFT		0
+static inline uint32_t A4XX_RB_STENCILREFMASK_STENCILREF(uint32_t val)
+{
+	return ((val) << A4XX_RB_STENCILREFMASK_STENCILREF__SHIFT) & A4XX_RB_STENCILREFMASK_STENCILREF__MASK;
+}
+#define A4XX_RB_STENCILREFMASK_STENCILMASK__MASK		0x0000ff00
+#define A4XX_RB_STENCILREFMASK_STENCILMASK__SHIFT		8
+static inline uint32_t A4XX_RB_STENCILREFMASK_STENCILMASK(uint32_t val)
+{
+	return ((val) << A4XX_RB_STENCILREFMASK_STENCILMASK__SHIFT) & A4XX_RB_STENCILREFMASK_STENCILMASK__MASK;
+}
+#define A4XX_RB_STENCILREFMASK_STENCILWRITEMASK__MASK		0x00ff0000
+#define A4XX_RB_STENCILREFMASK_STENCILWRITEMASK__SHIFT		16
+static inline uint32_t A4XX_RB_STENCILREFMASK_STENCILWRITEMASK(uint32_t val)
+{
+	return ((val) << A4XX_RB_STENCILREFMASK_STENCILWRITEMASK__SHIFT) & A4XX_RB_STENCILREFMASK_STENCILWRITEMASK__MASK;
+}
+
+#define REG_A4XX_RB_STENCILREFMASK_BF				0x0000210c
+#define A4XX_RB_STENCILREFMASK_BF_STENCILREF__MASK		0x000000ff
+#define A4XX_RB_STENCILREFMASK_BF_STENCILREF__SHIFT		0
+static inline uint32_t A4XX_RB_STENCILREFMASK_BF_STENCILREF(uint32_t val)
+{
+	return ((val) << A4XX_RB_STENCILREFMASK_BF_STENCILREF__SHIFT) & A4XX_RB_STENCILREFMASK_BF_STENCILREF__MASK;
+}
+#define A4XX_RB_STENCILREFMASK_BF_STENCILMASK__MASK		0x0000ff00
+#define A4XX_RB_STENCILREFMASK_BF_STENCILMASK__SHIFT		8
+static inline uint32_t A4XX_RB_STENCILREFMASK_BF_STENCILMASK(uint32_t val)
+{
+	return ((val) << A4XX_RB_STENCILREFMASK_BF_STENCILMASK__SHIFT) & A4XX_RB_STENCILREFMASK_BF_STENCILMASK__MASK;
+}
+#define A4XX_RB_STENCILREFMASK_BF_STENCILWRITEMASK__MASK	0x00ff0000
+#define A4XX_RB_STENCILREFMASK_BF_STENCILWRITEMASK__SHIFT	16
+static inline uint32_t A4XX_RB_STENCILREFMASK_BF_STENCILWRITEMASK(uint32_t val)
+{
+	return ((val) << A4XX_RB_STENCILREFMASK_BF_STENCILWRITEMASK__SHIFT) & A4XX_RB_STENCILREFMASK_BF_STENCILWRITEMASK__MASK;
+}
+
+#define REG_A4XX_RB_BIN_OFFSET					0x0000210d
+#define A4XX_RB_BIN_OFFSET_WINDOW_OFFSET_DISABLE		0x80000000
+#define A4XX_RB_BIN_OFFSET_X__MASK				0x00007fff
+#define A4XX_RB_BIN_OFFSET_X__SHIFT				0
+static inline uint32_t A4XX_RB_BIN_OFFSET_X(uint32_t val)
+{
+	return ((val) << A4XX_RB_BIN_OFFSET_X__SHIFT) & A4XX_RB_BIN_OFFSET_X__MASK;
+}
+#define A4XX_RB_BIN_OFFSET_Y__MASK				0x7fff0000
+#define A4XX_RB_BIN_OFFSET_Y__SHIFT				16
+static inline uint32_t A4XX_RB_BIN_OFFSET_Y(uint32_t val)
+{
+	return ((val) << A4XX_RB_BIN_OFFSET_Y__SHIFT) & A4XX_RB_BIN_OFFSET_Y__MASK;
+}
+
+#define REG_A4XX_RB_VPORT_Z_CLAMP_MAX_15			0x0000213f
+
+#define REG_A4XX_RBBM_HW_VERSION				0x00000000
+
+#define REG_A4XX_RBBM_HW_CONFIGURATION				0x00000002
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_CTL_TP(uint32_t i0) { return 0x00000004 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_CTL_TP_REG(uint32_t i0) { return 0x00000004 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_CTL2_TP(uint32_t i0) { return 0x00000008 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_CTL2_TP_REG(uint32_t i0) { return 0x00000008 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_HYST_TP(uint32_t i0) { return 0x0000000c + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_HYST_TP_REG(uint32_t i0) { return 0x0000000c + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_DELAY_TP(uint32_t i0) { return 0x00000010 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_DELAY_TP_REG(uint32_t i0) { return 0x00000010 + 0x1*i0; }
+
+#define REG_A4XX_RBBM_CLOCK_CTL_UCHE 				0x00000014
+
+#define REG_A4XX_RBBM_CLOCK_CTL2_UCHE				0x00000015
+
+#define REG_A4XX_RBBM_CLOCK_CTL3_UCHE				0x00000016
+
+#define REG_A4XX_RBBM_CLOCK_CTL4_UCHE				0x00000017
+
+#define REG_A4XX_RBBM_CLOCK_HYST_UCHE				0x00000018
+
+#define REG_A4XX_RBBM_CLOCK_DELAY_UCHE				0x00000019
+
+#define REG_A4XX_RBBM_CLOCK_MODE_GPC				0x0000001a
+
+#define REG_A4XX_RBBM_CLOCK_DELAY_GPC				0x0000001b
+
+#define REG_A4XX_RBBM_CLOCK_HYST_GPC				0x0000001c
+
+#define REG_A4XX_RBBM_CLOCK_CTL_TSE_RAS_RBBM			0x0000001d
+
+#define REG_A4XX_RBBM_CLOCK_HYST_TSE_RAS_RBBM			0x0000001e
+
+#define REG_A4XX_RBBM_CLOCK_DELAY_TSE_RAS_RBBM			0x0000001f
+
+#define REG_A4XX_RBBM_CLOCK_CTL					0x00000020
+
+#define REG_A4XX_RBBM_SP_HYST_CNT				0x00000021
+
+#define REG_A4XX_RBBM_SW_RESET_CMD				0x00000022
+
+#define REG_A4XX_RBBM_AHB_CTL0					0x00000023
+
+#define REG_A4XX_RBBM_AHB_CTL1					0x00000024
+
+#define REG_A4XX_RBBM_AHB_CMD					0x00000025
+
+#define REG_A4XX_RBBM_RB_SUB_BLOCK_SEL_CTL			0x00000026
+
+#define REG_A4XX_RBBM_RAM_ACC_63_32				0x00000028
+
+#define REG_A4XX_RBBM_WAIT_IDLE_CLOCKS_CTL			0x0000002b
+
+#define REG_A4XX_RBBM_INTERFACE_HANG_INT_CTL			0x0000002f
+
+#define REG_A4XX_RBBM_INTERFACE_HANG_MASK_CTL4			0x00000034
+
+#define REG_A4XX_RBBM_INT_CLEAR_CMD				0x00000036
+
+#define REG_A4XX_RBBM_INT_0_MASK				0x00000037
+
+#define REG_A4XX_RBBM_RBBM_CTL					0x0000003e
+
+#define REG_A4XX_RBBM_AHB_DEBUG_CTL				0x0000003f
+
+#define REG_A4XX_RBBM_VBIF_DEBUG_CTL				0x00000041
+
+#define REG_A4XX_RBBM_CLOCK_CTL2				0x00000042
+
+#define REG_A4XX_RBBM_BLOCK_SW_RESET_CMD			0x00000045
+
+#define REG_A4XX_RBBM_RESET_CYCLES				0x00000047
+
+#define REG_A4XX_RBBM_EXT_TRACE_BUS_CTL				0x00000049
+
+#define REG_A4XX_RBBM_CFG_DEBBUS_SEL_A				0x0000004a
+
+#define REG_A4XX_RBBM_CFG_DEBBUS_SEL_B				0x0000004b
+
+#define REG_A4XX_RBBM_CFG_DEBBUS_SEL_C				0x0000004c
+
+#define REG_A4XX_RBBM_CFG_DEBBUS_SEL_D				0x0000004d
+
+#define REG_A4XX_RBBM_PERFCTR_CP_0_LO				0x0000009c
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_CTL_SP(uint32_t i0) { return 0x00000068 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_CTL_SP_REG(uint32_t i0) { return 0x00000068 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_CTL2_SP(uint32_t i0) { return 0x0000006c + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_CTL2_SP_REG(uint32_t i0) { return 0x0000006c + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_HYST_SP(uint32_t i0) { return 0x00000070 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_HYST_SP_REG(uint32_t i0) { return 0x00000070 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_DELAY_SP(uint32_t i0) { return 0x00000074 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_DELAY_SP_REG(uint32_t i0) { return 0x00000074 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_CTL_RB(uint32_t i0) { return 0x00000078 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_CTL_RB_REG(uint32_t i0) { return 0x00000078 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_CTL2_RB(uint32_t i0) { return 0x0000007c + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_CTL2_RB_REG(uint32_t i0) { return 0x0000007c + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_CTL_MARB_CCU(uint32_t i0) { return 0x00000082 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_CTL_MARB_CCU_REG(uint32_t i0) { return 0x00000082 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_HYST_RB_MARB_CCU(uint32_t i0) { return 0x00000086 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_HYST_RB_MARB_CCU_REG(uint32_t i0) { return 0x00000086 + 0x1*i0; }
+
+#define REG_A4XX_RBBM_CLOCK_HYST_COM_DCOM			0x00000080
+
+#define REG_A4XX_RBBM_CLOCK_CTL_COM_DCOM			0x00000081
+
+#define REG_A4XX_RBBM_CLOCK_CTL_HLSQ				0x0000008a
+
+#define REG_A4XX_RBBM_CLOCK_HYST_HLSQ				0x0000008b
+
+#define REG_A4XX_RBBM_CLOCK_DELAY_HLSQ				0x0000008c
+
+#define REG_A4XX_RBBM_CLOCK_DELAY_COM_DCOM			0x0000008d
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_DELAY_RB_MARB_CCU_L1(uint32_t i0) { return 0x0000008e + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_RBBM_CLOCK_DELAY_RB_MARB_CCU_L1_REG(uint32_t i0) { return 0x0000008e + 0x1*i0; }
+
+#define REG_A4XX_RBBM_PERFCTR_PWR_1_LO				0x00000168
+
+#define REG_A4XX_RBBM_PERFCTR_CTL				0x00000170
+
+#define REG_A4XX_RBBM_PERFCTR_LOAD_CMD0				0x00000171
+
+#define REG_A4XX_RBBM_PERFCTR_LOAD_CMD1				0x00000172
+
+#define REG_A4XX_RBBM_PERFCTR_LOAD_CMD2				0x00000173
+
+#define REG_A4XX_RBBM_PERFCTR_LOAD_VALUE_LO			0x00000174
+
+#define REG_A4XX_RBBM_PERFCTR_LOAD_VALUE_HI			0x00000175
+
+#define REG_A4XX_RBBM_GPU_BUSY_MASKED				0x0000017a
+
+#define REG_A4XX_RBBM_INT_0_STATUS				0x0000017d
+
+#define REG_A4XX_RBBM_CLOCK_STATUS				0x00000182
+
+#define REG_A4XX_RBBM_AHB_STATUS				0x00000189
+
+#define REG_A4XX_RBBM_AHB_ME_SPLIT_STATUS			0x0000018c
+
+#define REG_A4XX_RBBM_AHB_PFP_SPLIT_STATUS			0x0000018d
+
+#define REG_A4XX_RBBM_AHB_ERROR_STATUS				0x0000018f
+
+#define REG_A4XX_RBBM_STATUS					0x00000191
+#define A4XX_RBBM_STATUS_HI_BUSY				0x00000001
+#define A4XX_RBBM_STATUS_CP_ME_BUSY				0x00000002
+#define A4XX_RBBM_STATUS_CP_PFP_BUSY				0x00000004
+#define A4XX_RBBM_STATUS_CP_NRT_BUSY				0x00004000
+#define A4XX_RBBM_STATUS_VBIF_BUSY				0x00008000
+#define A4XX_RBBM_STATUS_TSE_BUSY				0x00010000
+#define A4XX_RBBM_STATUS_RAS_BUSY				0x00020000
+#define A4XX_RBBM_STATUS_RB_BUSY				0x00040000
+#define A4XX_RBBM_STATUS_PC_DCALL_BUSY				0x00080000
+#define A4XX_RBBM_STATUS_PC_VSD_BUSY				0x00100000
+#define A4XX_RBBM_STATUS_VFD_BUSY				0x00200000
+#define A4XX_RBBM_STATUS_VPC_BUSY				0x00400000
+#define A4XX_RBBM_STATUS_UCHE_BUSY				0x00800000
+#define A4XX_RBBM_STATUS_SP_BUSY				0x01000000
+#define A4XX_RBBM_STATUS_TPL1_BUSY				0x02000000
+#define A4XX_RBBM_STATUS_MARB_BUSY				0x04000000
+#define A4XX_RBBM_STATUS_VSC_BUSY				0x08000000
+#define A4XX_RBBM_STATUS_ARB_BUSY				0x10000000
+#define A4XX_RBBM_STATUS_HLSQ_BUSY				0x20000000
+#define A4XX_RBBM_STATUS_GPU_BUSY_NOHC				0x40000000
+#define A4XX_RBBM_STATUS_GPU_BUSY				0x80000000
+
+#define REG_A4XX_RBBM_INTERFACE_RRDY_STATUS5			0x0000019f
+
+#define REG_A4XX_CP_SCRATCH_UMASK				0x00000228
+
+#define REG_A4XX_CP_SCRATCH_ADDR				0x00000229
+
+#define REG_A4XX_CP_RB_BASE					0x00000200
+
+#define REG_A4XX_CP_RB_CNTL					0x00000201
+
+#define REG_A4XX_CP_RB_WPTR					0x00000205
+
+#define REG_A4XX_CP_RB_RPTR_ADDR				0x00000203
+
+#define REG_A4XX_CP_RB_RPTR					0x00000204
+
+#define REG_A4XX_CP_IB1_BASE					0x00000206
+
+#define REG_A4XX_CP_IB1_BUFSZ					0x00000207
+
+#define REG_A4XX_CP_IB2_BASE					0x00000208
+
+#define REG_A4XX_CP_IB2_BUFSZ					0x00000209
+
+#define REG_A4XX_CP_ME_RB_DONE_DATA				0x00000217
+
+#define REG_A4XX_CP_QUEUE_THRESH2				0x00000219
+
+#define REG_A4XX_CP_MERCIU_SIZE					0x0000021b
+
+#define REG_A4XX_CP_ROQ_ADDR					0x0000021c
+
+#define REG_A4XX_CP_ROQ_DATA					0x0000021d
+
+#define REG_A4XX_CP_MEQ_ADDR 					0x0000021e
+
+#define REG_A4XX_CP_MEQ_DATA 					0x0000021f
+
+#define REG_A4XX_CP_MERCIU_ADDR					0x00000220
+
+#define REG_A4XX_CP_MERCIU_DATA					0x00000221
+
+#define REG_A4XX_CP_MERCIU_DATA2				0x00000222
+
+#define REG_A4XX_CP_PFP_UCODE_ADDR				0x00000223
+
+#define REG_A4XX_CP_PFP_UCODE_DATA				0x00000224
+
+#define REG_A4XX_CP_ME_RAM_WADDR				0x00000225
+
+#define REG_A4XX_CP_ME_RAM_RADDR				0x00000226
+
+#define REG_A4XX_CP_ME_RAM_DATA					0x00000227
+
+#define REG_A4XX_CP_PREEMPT					0x0000022a
+
+#define REG_A4XX_CP_CNTL					0x0000022c
+
+#define REG_A4XX_CP_ME_CNTL					0x0000022d
+
+#define REG_A4XX_CP_DEBUG					0x0000022e
+
+#define REG_A4XX_CP_DEBUG_ECO_CONTROL				0x00000231
+
+#define REG_A4XX_CP_DRAW_STATE_ADDR				0x00000232
+
+#define REG_A4XX_CP_PROTECT_REG_0				0x00000240
+
+static inline uint32_t REG_A4XX_CP_PROTECT(uint32_t i0) { return 0x00000240 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_CP_PROTECT_REG(uint32_t i0) { return 0x00000240 + 0x1*i0; }
+
+#define REG_A4XX_CP_PROTECT_CTRL				0x00000250
+
+#define REG_A4XX_CP_ST_BASE					0x000004c0
+
+#define REG_A4XX_CP_STQ_AVAIL					0x000004ce
+
+#define REG_A4XX_CP_MERCIU_STAT					0x000004d0
+
+#define REG_A4XX_CP_WFI_PEND_CTR				0x000004d2
+
+#define REG_A4XX_CP_HW_FAULT					0x000004d8
+
+#define REG_A4XX_CP_PROTECT_STATUS				0x000004da
+
+#define REG_A4XX_CP_EVENTS_IN_FLIGHT				0x000004dd
+
+#define REG_A4XX_CP_PERFCTR_CP_SEL_0				0x00000500
+
+#define REG_A4XX_CP_PERFCOMBINER_SELECT				0x0000050b
+
+static inline uint32_t REG_A4XX_CP_SCRATCH(uint32_t i0) { return 0x00000578 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_CP_SCRATCH_REG(uint32_t i0) { return 0x00000578 + 0x1*i0; }
+
+#define REG_A4XX_SP_VS_STATUS					0x00000ec0
+
+#define REG_A4XX_SP_PERFCTR_SP_SEL_11				0x00000ecf
+
+#define REG_A4XX_SP_SP_CTRL_REG					0x000022c0
+#define A4XX_SP_SP_CTRL_REG_BINNING_PASS			0x00080000
+
+#define REG_A4XX_SP_INSTR_CACHE_CTRL				0x000022c1
+
+#define REG_A4XX_SP_VS_CTRL_REG0				0x000022c4
+#define A4XX_SP_VS_CTRL_REG0_THREADMODE__MASK			0x00000001
+#define A4XX_SP_VS_CTRL_REG0_THREADMODE__SHIFT			0
+static inline uint32_t A4XX_SP_VS_CTRL_REG0_THREADMODE(enum a3xx_threadmode val)
+{
+	return ((val) << A4XX_SP_VS_CTRL_REG0_THREADMODE__SHIFT) & A4XX_SP_VS_CTRL_REG0_THREADMODE__MASK;
+}
+#define A4XX_SP_VS_CTRL_REG0_VARYING				0x00000002
+#define A4XX_SP_VS_CTRL_REG0_CACHEINVALID			0x00000004
+#define A4XX_SP_VS_CTRL_REG0_HALFREGFOOTPRINT__MASK		0x000003f0
+#define A4XX_SP_VS_CTRL_REG0_HALFREGFOOTPRINT__SHIFT		4
+static inline uint32_t A4XX_SP_VS_CTRL_REG0_HALFREGFOOTPRINT(uint32_t val)
+{
+	return ((val) << A4XX_SP_VS_CTRL_REG0_HALFREGFOOTPRINT__SHIFT) & A4XX_SP_VS_CTRL_REG0_HALFREGFOOTPRINT__MASK;
+}
+#define A4XX_SP_VS_CTRL_REG0_FULLREGFOOTPRINT__MASK		0x0003fc00
+#define A4XX_SP_VS_CTRL_REG0_FULLREGFOOTPRINT__SHIFT		10
+static inline uint32_t A4XX_SP_VS_CTRL_REG0_FULLREGFOOTPRINT(uint32_t val)
+{
+	return ((val) << A4XX_SP_VS_CTRL_REG0_FULLREGFOOTPRINT__SHIFT) & A4XX_SP_VS_CTRL_REG0_FULLREGFOOTPRINT__MASK;
+}
+#define A4XX_SP_VS_CTRL_REG0_INOUTREGOVERLAP__MASK		0x000c0000
+#define A4XX_SP_VS_CTRL_REG0_INOUTREGOVERLAP__SHIFT		18
+static inline uint32_t A4XX_SP_VS_CTRL_REG0_INOUTREGOVERLAP(uint32_t val)
+{
+	return ((val) << A4XX_SP_VS_CTRL_REG0_INOUTREGOVERLAP__SHIFT) & A4XX_SP_VS_CTRL_REG0_INOUTREGOVERLAP__MASK;
+}
+#define A4XX_SP_VS_CTRL_REG0_THREADSIZE__MASK			0x00100000
+#define A4XX_SP_VS_CTRL_REG0_THREADSIZE__SHIFT			20
+static inline uint32_t A4XX_SP_VS_CTRL_REG0_THREADSIZE(enum a3xx_threadsize val)
+{
+	return ((val) << A4XX_SP_VS_CTRL_REG0_THREADSIZE__SHIFT) & A4XX_SP_VS_CTRL_REG0_THREADSIZE__MASK;
+}
+#define A4XX_SP_VS_CTRL_REG0_SUPERTHREADMODE			0x00200000
+#define A4XX_SP_VS_CTRL_REG0_PIXLODENABLE			0x00400000
+
+#define REG_A4XX_SP_VS_CTRL_REG1				0x000022c5
+#define A4XX_SP_VS_CTRL_REG1_CONSTLENGTH__MASK			0x000000ff
+#define A4XX_SP_VS_CTRL_REG1_CONSTLENGTH__SHIFT			0
+static inline uint32_t A4XX_SP_VS_CTRL_REG1_CONSTLENGTH(uint32_t val)
+{
+	return ((val) << A4XX_SP_VS_CTRL_REG1_CONSTLENGTH__SHIFT) & A4XX_SP_VS_CTRL_REG1_CONSTLENGTH__MASK;
+}
+#define A4XX_SP_VS_CTRL_REG1_INITIALOUTSTANDING__MASK		0x7f000000
+#define A4XX_SP_VS_CTRL_REG1_INITIALOUTSTANDING__SHIFT		24
+static inline uint32_t A4XX_SP_VS_CTRL_REG1_INITIALOUTSTANDING(uint32_t val)
+{
+	return ((val) << A4XX_SP_VS_CTRL_REG1_INITIALOUTSTANDING__SHIFT) & A4XX_SP_VS_CTRL_REG1_INITIALOUTSTANDING__MASK;
+}
+
+#define REG_A4XX_SP_VS_PARAM_REG				0x000022c6
+#define A4XX_SP_VS_PARAM_REG_POSREGID__MASK			0x000000ff
+#define A4XX_SP_VS_PARAM_REG_POSREGID__SHIFT			0
+static inline uint32_t A4XX_SP_VS_PARAM_REG_POSREGID(uint32_t val)
+{
+	return ((val) << A4XX_SP_VS_PARAM_REG_POSREGID__SHIFT) & A4XX_SP_VS_PARAM_REG_POSREGID__MASK;
+}
+#define A4XX_SP_VS_PARAM_REG_PSIZEREGID__MASK			0x0000ff00
+#define A4XX_SP_VS_PARAM_REG_PSIZEREGID__SHIFT			8
+static inline uint32_t A4XX_SP_VS_PARAM_REG_PSIZEREGID(uint32_t val)
+{
+	return ((val) << A4XX_SP_VS_PARAM_REG_PSIZEREGID__SHIFT) & A4XX_SP_VS_PARAM_REG_PSIZEREGID__MASK;
+}
+#define A4XX_SP_VS_PARAM_REG_TOTALVSOUTVAR__MASK		0xfff00000
+#define A4XX_SP_VS_PARAM_REG_TOTALVSOUTVAR__SHIFT		20
+static inline uint32_t A4XX_SP_VS_PARAM_REG_TOTALVSOUTVAR(uint32_t val)
+{
+	return ((val) << A4XX_SP_VS_PARAM_REG_TOTALVSOUTVAR__SHIFT) & A4XX_SP_VS_PARAM_REG_TOTALVSOUTVAR__MASK;
+}
+
+static inline uint32_t REG_A4XX_SP_VS_OUT(uint32_t i0) { return 0x000022c7 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_SP_VS_OUT_REG(uint32_t i0) { return 0x000022c7 + 0x1*i0; }
+#define A4XX_SP_VS_OUT_REG_A_REGID__MASK			0x000001ff
+#define A4XX_SP_VS_OUT_REG_A_REGID__SHIFT			0
+static inline uint32_t A4XX_SP_VS_OUT_REG_A_REGID(uint32_t val)
+{
+	return ((val) << A4XX_SP_VS_OUT_REG_A_REGID__SHIFT) & A4XX_SP_VS_OUT_REG_A_REGID__MASK;
+}
+#define A4XX_SP_VS_OUT_REG_A_COMPMASK__MASK			0x00001e00
+#define A4XX_SP_VS_OUT_REG_A_COMPMASK__SHIFT			9
+static inline uint32_t A4XX_SP_VS_OUT_REG_A_COMPMASK(uint32_t val)
+{
+	return ((val) << A4XX_SP_VS_OUT_REG_A_COMPMASK__SHIFT) & A4XX_SP_VS_OUT_REG_A_COMPMASK__MASK;
+}
+#define A4XX_SP_VS_OUT_REG_B_REGID__MASK			0x01ff0000
+#define A4XX_SP_VS_OUT_REG_B_REGID__SHIFT			16
+static inline uint32_t A4XX_SP_VS_OUT_REG_B_REGID(uint32_t val)
+{
+	return ((val) << A4XX_SP_VS_OUT_REG_B_REGID__SHIFT) & A4XX_SP_VS_OUT_REG_B_REGID__MASK;
+}
+#define A4XX_SP_VS_OUT_REG_B_COMPMASK__MASK			0x1e000000
+#define A4XX_SP_VS_OUT_REG_B_COMPMASK__SHIFT			25
+static inline uint32_t A4XX_SP_VS_OUT_REG_B_COMPMASK(uint32_t val)
+{
+	return ((val) << A4XX_SP_VS_OUT_REG_B_COMPMASK__SHIFT) & A4XX_SP_VS_OUT_REG_B_COMPMASK__MASK;
+}
+
+static inline uint32_t REG_A4XX_SP_VS_VPC_DST(uint32_t i0) { return 0x000022d8 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_SP_VS_VPC_DST_REG(uint32_t i0) { return 0x000022d8 + 0x1*i0; }
+#define A4XX_SP_VS_VPC_DST_REG_OUTLOC0__MASK			0x000000ff
+#define A4XX_SP_VS_VPC_DST_REG_OUTLOC0__SHIFT			0
+static inline uint32_t A4XX_SP_VS_VPC_DST_REG_OUTLOC0(uint32_t val)
+{
+	return ((val) << A4XX_SP_VS_VPC_DST_REG_OUTLOC0__SHIFT) & A4XX_SP_VS_VPC_DST_REG_OUTLOC0__MASK;
+}
+#define A4XX_SP_VS_VPC_DST_REG_OUTLOC1__MASK			0x0000ff00
+#define A4XX_SP_VS_VPC_DST_REG_OUTLOC1__SHIFT			8
+static inline uint32_t A4XX_SP_VS_VPC_DST_REG_OUTLOC1(uint32_t val)
+{
+	return ((val) << A4XX_SP_VS_VPC_DST_REG_OUTLOC1__SHIFT) & A4XX_SP_VS_VPC_DST_REG_OUTLOC1__MASK;
+}
+#define A4XX_SP_VS_VPC_DST_REG_OUTLOC2__MASK			0x00ff0000
+#define A4XX_SP_VS_VPC_DST_REG_OUTLOC2__SHIFT			16
+static inline uint32_t A4XX_SP_VS_VPC_DST_REG_OUTLOC2(uint32_t val)
+{
+	return ((val) << A4XX_SP_VS_VPC_DST_REG_OUTLOC2__SHIFT) & A4XX_SP_VS_VPC_DST_REG_OUTLOC2__MASK;
+}
+#define A4XX_SP_VS_VPC_DST_REG_OUTLOC3__MASK			0xff000000
+#define A4XX_SP_VS_VPC_DST_REG_OUTLOC3__SHIFT			24
+static inline uint32_t A4XX_SP_VS_VPC_DST_REG_OUTLOC3(uint32_t val)
+{
+	return ((val) << A4XX_SP_VS_VPC_DST_REG_OUTLOC3__SHIFT) & A4XX_SP_VS_VPC_DST_REG_OUTLOC3__MASK;
+}
+
+#define REG_A4XX_SP_VS_OBJ_OFFSET_REG				0x000022e0
+#define A4XX_SP_VS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__MASK	0x01ff0000
+#define A4XX_SP_VS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__SHIFT	16
+static inline uint32_t A4XX_SP_VS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_SP_VS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__SHIFT) & A4XX_SP_VS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__MASK;
+}
+#define A4XX_SP_VS_OBJ_OFFSET_REG_SHADEROBJOFFSET__MASK		0xfe000000
+#define A4XX_SP_VS_OBJ_OFFSET_REG_SHADEROBJOFFSET__SHIFT	25
+static inline uint32_t A4XX_SP_VS_OBJ_OFFSET_REG_SHADEROBJOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_SP_VS_OBJ_OFFSET_REG_SHADEROBJOFFSET__SHIFT) & A4XX_SP_VS_OBJ_OFFSET_REG_SHADEROBJOFFSET__MASK;
+}
+
+#define REG_A4XX_SP_VS_OBJ_START				0x000022e1
+
+#define REG_A4XX_SP_VS_PVT_MEM_PARAM				0x000022e2
+
+#define REG_A4XX_SP_VS_PVT_MEM_ADDR				0x000022e3
+
+#define REG_A4XX_SP_VS_LENGTH_REG				0x000022e5
+
+#define REG_A4XX_SP_FS_CTRL_REG0				0x000022e8
+#define A4XX_SP_FS_CTRL_REG0_THREADMODE__MASK			0x00000001
+#define A4XX_SP_FS_CTRL_REG0_THREADMODE__SHIFT			0
+static inline uint32_t A4XX_SP_FS_CTRL_REG0_THREADMODE(enum a3xx_threadmode val)
+{
+	return ((val) << A4XX_SP_FS_CTRL_REG0_THREADMODE__SHIFT) & A4XX_SP_FS_CTRL_REG0_THREADMODE__MASK;
+}
+#define A4XX_SP_FS_CTRL_REG0_VARYING				0x00000002
+#define A4XX_SP_FS_CTRL_REG0_CACHEINVALID			0x00000004
+#define A4XX_SP_FS_CTRL_REG0_HALFREGFOOTPRINT__MASK		0x000003f0
+#define A4XX_SP_FS_CTRL_REG0_HALFREGFOOTPRINT__SHIFT		4
+static inline uint32_t A4XX_SP_FS_CTRL_REG0_HALFREGFOOTPRINT(uint32_t val)
+{
+	return ((val) << A4XX_SP_FS_CTRL_REG0_HALFREGFOOTPRINT__SHIFT) & A4XX_SP_FS_CTRL_REG0_HALFREGFOOTPRINT__MASK;
+}
+#define A4XX_SP_FS_CTRL_REG0_FULLREGFOOTPRINT__MASK		0x0003fc00
+#define A4XX_SP_FS_CTRL_REG0_FULLREGFOOTPRINT__SHIFT		10
+static inline uint32_t A4XX_SP_FS_CTRL_REG0_FULLREGFOOTPRINT(uint32_t val)
+{
+	return ((val) << A4XX_SP_FS_CTRL_REG0_FULLREGFOOTPRINT__SHIFT) & A4XX_SP_FS_CTRL_REG0_FULLREGFOOTPRINT__MASK;
+}
+#define A4XX_SP_FS_CTRL_REG0_INOUTREGOVERLAP__MASK		0x000c0000
+#define A4XX_SP_FS_CTRL_REG0_INOUTREGOVERLAP__SHIFT		18
+static inline uint32_t A4XX_SP_FS_CTRL_REG0_INOUTREGOVERLAP(uint32_t val)
+{
+	return ((val) << A4XX_SP_FS_CTRL_REG0_INOUTREGOVERLAP__SHIFT) & A4XX_SP_FS_CTRL_REG0_INOUTREGOVERLAP__MASK;
+}
+#define A4XX_SP_FS_CTRL_REG0_THREADSIZE__MASK			0x00100000
+#define A4XX_SP_FS_CTRL_REG0_THREADSIZE__SHIFT			20
+static inline uint32_t A4XX_SP_FS_CTRL_REG0_THREADSIZE(enum a3xx_threadsize val)
+{
+	return ((val) << A4XX_SP_FS_CTRL_REG0_THREADSIZE__SHIFT) & A4XX_SP_FS_CTRL_REG0_THREADSIZE__MASK;
+}
+#define A4XX_SP_FS_CTRL_REG0_SUPERTHREADMODE			0x00200000
+#define A4XX_SP_FS_CTRL_REG0_PIXLODENABLE			0x00400000
+
+#define REG_A4XX_SP_FS_CTRL_REG1				0x000022e9
+#define A4XX_SP_FS_CTRL_REG1_CONSTLENGTH__MASK			0x000000ff
+#define A4XX_SP_FS_CTRL_REG1_CONSTLENGTH__SHIFT			0
+static inline uint32_t A4XX_SP_FS_CTRL_REG1_CONSTLENGTH(uint32_t val)
+{
+	return ((val) << A4XX_SP_FS_CTRL_REG1_CONSTLENGTH__SHIFT) & A4XX_SP_FS_CTRL_REG1_CONSTLENGTH__MASK;
+}
+#define A4XX_SP_FS_CTRL_REG1_VARYING				0x00100000
+
+#define REG_A4XX_SP_FS_OBJ_OFFSET_REG				0x000022ea
+#define A4XX_SP_FS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__MASK	0x01ff0000
+#define A4XX_SP_FS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__SHIFT	16
+static inline uint32_t A4XX_SP_FS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_SP_FS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__SHIFT) & A4XX_SP_FS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__MASK;
+}
+#define A4XX_SP_FS_OBJ_OFFSET_REG_SHADEROBJOFFSET__MASK		0xfe000000
+#define A4XX_SP_FS_OBJ_OFFSET_REG_SHADEROBJOFFSET__SHIFT	25
+static inline uint32_t A4XX_SP_FS_OBJ_OFFSET_REG_SHADEROBJOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_SP_FS_OBJ_OFFSET_REG_SHADEROBJOFFSET__SHIFT) & A4XX_SP_FS_OBJ_OFFSET_REG_SHADEROBJOFFSET__MASK;
+}
+
+#define REG_A4XX_SP_FS_OBJ_START				0x000022eb
+
+#define REG_A4XX_SP_FS_PVT_MEM_PARAM				0x000022ec
+
+#define REG_A4XX_SP_FS_PVT_MEM_ADDR				0x000022ed
+
+#define REG_A4XX_SP_FS_LENGTH_REG				0x000022ef
+
+#define REG_A4XX_SP_FS_OUTPUT_REG				0x000022f0
+#define A4XX_SP_FS_OUTPUT_REG_DEPTH_ENABLE			0x00000080
+#define A4XX_SP_FS_OUTPUT_REG_DEPTH_REGID__MASK			0x0000ff00
+#define A4XX_SP_FS_OUTPUT_REG_DEPTH_REGID__SHIFT		8
+static inline uint32_t A4XX_SP_FS_OUTPUT_REG_DEPTH_REGID(uint32_t val)
+{
+	return ((val) << A4XX_SP_FS_OUTPUT_REG_DEPTH_REGID__SHIFT) & A4XX_SP_FS_OUTPUT_REG_DEPTH_REGID__MASK;
+}
+
+static inline uint32_t REG_A4XX_SP_FS_MRT(uint32_t i0) { return 0x000022f1 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_SP_FS_MRT_REG(uint32_t i0) { return 0x000022f1 + 0x1*i0; }
+#define A4XX_SP_FS_MRT_REG_REGID__MASK				0x000000ff
+#define A4XX_SP_FS_MRT_REG_REGID__SHIFT				0
+static inline uint32_t A4XX_SP_FS_MRT_REG_REGID(uint32_t val)
+{
+	return ((val) << A4XX_SP_FS_MRT_REG_REGID__SHIFT) & A4XX_SP_FS_MRT_REG_REGID__MASK;
+}
+#define A4XX_SP_FS_MRT_REG_HALF_PRECISION			0x00000100
+#define A4XX_SP_FS_MRT_REG_MRTFORMAT__MASK			0x0003f000
+#define A4XX_SP_FS_MRT_REG_MRTFORMAT__SHIFT			12
+static inline uint32_t A4XX_SP_FS_MRT_REG_MRTFORMAT(enum a4xx_color_fmt val)
+{
+	return ((val) << A4XX_SP_FS_MRT_REG_MRTFORMAT__SHIFT) & A4XX_SP_FS_MRT_REG_MRTFORMAT__MASK;
+}
+
+#define REG_A4XX_SP_HS_OBJ_OFFSET_REG				0x0000230d
+#define A4XX_SP_HS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__MASK	0x01ff0000
+#define A4XX_SP_HS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__SHIFT	16
+static inline uint32_t A4XX_SP_HS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_SP_HS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__SHIFT) & A4XX_SP_HS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__MASK;
+}
+#define A4XX_SP_HS_OBJ_OFFSET_REG_SHADEROBJOFFSET__MASK		0xfe000000
+#define A4XX_SP_HS_OBJ_OFFSET_REG_SHADEROBJOFFSET__SHIFT	25
+static inline uint32_t A4XX_SP_HS_OBJ_OFFSET_REG_SHADEROBJOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_SP_HS_OBJ_OFFSET_REG_SHADEROBJOFFSET__SHIFT) & A4XX_SP_HS_OBJ_OFFSET_REG_SHADEROBJOFFSET__MASK;
+}
+
+#define REG_A4XX_SP_DS_OBJ_OFFSET_REG				0x00002334
+#define A4XX_SP_DS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__MASK	0x01ff0000
+#define A4XX_SP_DS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__SHIFT	16
+static inline uint32_t A4XX_SP_DS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_SP_DS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__SHIFT) & A4XX_SP_DS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__MASK;
+}
+#define A4XX_SP_DS_OBJ_OFFSET_REG_SHADEROBJOFFSET__MASK		0xfe000000
+#define A4XX_SP_DS_OBJ_OFFSET_REG_SHADEROBJOFFSET__SHIFT	25
+static inline uint32_t A4XX_SP_DS_OBJ_OFFSET_REG_SHADEROBJOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_SP_DS_OBJ_OFFSET_REG_SHADEROBJOFFSET__SHIFT) & A4XX_SP_DS_OBJ_OFFSET_REG_SHADEROBJOFFSET__MASK;
+}
+
+#define REG_A4XX_SP_GS_OBJ_OFFSET_REG				0x0000235b
+#define A4XX_SP_GS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__MASK	0x01ff0000
+#define A4XX_SP_GS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__SHIFT	16
+static inline uint32_t A4XX_SP_GS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_SP_GS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__SHIFT) & A4XX_SP_GS_OBJ_OFFSET_REG_CONSTOBJECTOFFSET__MASK;
+}
+#define A4XX_SP_GS_OBJ_OFFSET_REG_SHADEROBJOFFSET__MASK		0xfe000000
+#define A4XX_SP_GS_OBJ_OFFSET_REG_SHADEROBJOFFSET__SHIFT	25
+static inline uint32_t A4XX_SP_GS_OBJ_OFFSET_REG_SHADEROBJOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_SP_GS_OBJ_OFFSET_REG_SHADEROBJOFFSET__SHIFT) & A4XX_SP_GS_OBJ_OFFSET_REG_SHADEROBJOFFSET__MASK;
+}
+
+#define REG_A4XX_SP_GS_LENGTH_REG				0x00002360
+
+#define REG_A4XX_VPC_DEBUG_RAM_SEL				0x00000e60
+
+#define REG_A4XX_VPC_DEBUG_RAM_READ				0x00000e61
+
+#define REG_A4XX_VPC_DEBUG_ECO_CONTROL				0x00000e64
+
+#define REG_A4XX_VPC_PERFCTR_VPC_SEL_3				0x00000e68
+
+#define REG_A4XX_VPC_ATTR					0x00002140
+#define A4XX_VPC_ATTR_TOTALATTR__MASK				0x000001ff
+#define A4XX_VPC_ATTR_TOTALATTR__SHIFT				0
+static inline uint32_t A4XX_VPC_ATTR_TOTALATTR(uint32_t val)
+{
+	return ((val) << A4XX_VPC_ATTR_TOTALATTR__SHIFT) & A4XX_VPC_ATTR_TOTALATTR__MASK;
+}
+#define A4XX_VPC_ATTR_PSIZE					0x00000200
+#define A4XX_VPC_ATTR_THRDASSIGN__MASK				0x00003000
+#define A4XX_VPC_ATTR_THRDASSIGN__SHIFT				12
+static inline uint32_t A4XX_VPC_ATTR_THRDASSIGN(uint32_t val)
+{
+	return ((val) << A4XX_VPC_ATTR_THRDASSIGN__SHIFT) & A4XX_VPC_ATTR_THRDASSIGN__MASK;
+}
+#define A4XX_VPC_ATTR_ENABLE					0x02000000
+
+#define REG_A4XX_VPC_PACK					0x00002141
+#define A4XX_VPC_PACK_NUMBYPASSVAR__MASK			0x000000ff
+#define A4XX_VPC_PACK_NUMBYPASSVAR__SHIFT			0
+static inline uint32_t A4XX_VPC_PACK_NUMBYPASSVAR(uint32_t val)
+{
+	return ((val) << A4XX_VPC_PACK_NUMBYPASSVAR__SHIFT) & A4XX_VPC_PACK_NUMBYPASSVAR__MASK;
+}
+#define A4XX_VPC_PACK_NUMFPNONPOSVAR__MASK			0x0000ff00
+#define A4XX_VPC_PACK_NUMFPNONPOSVAR__SHIFT			8
+static inline uint32_t A4XX_VPC_PACK_NUMFPNONPOSVAR(uint32_t val)
+{
+	return ((val) << A4XX_VPC_PACK_NUMFPNONPOSVAR__SHIFT) & A4XX_VPC_PACK_NUMFPNONPOSVAR__MASK;
+}
+#define A4XX_VPC_PACK_NUMNONPOSVSVAR__MASK			0x00ff0000
+#define A4XX_VPC_PACK_NUMNONPOSVSVAR__SHIFT			16
+static inline uint32_t A4XX_VPC_PACK_NUMNONPOSVSVAR(uint32_t val)
+{
+	return ((val) << A4XX_VPC_PACK_NUMNONPOSVSVAR__SHIFT) & A4XX_VPC_PACK_NUMNONPOSVSVAR__MASK;
+}
+
+static inline uint32_t REG_A4XX_VPC_VARYING_INTERP(uint32_t i0) { return 0x00002142 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_VPC_VARYING_INTERP_MODE(uint32_t i0) { return 0x00002142 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_VPC_VARYING_PS_REPL(uint32_t i0) { return 0x0000214a + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_VPC_VARYING_PS_REPL_MODE(uint32_t i0) { return 0x0000214a + 0x1*i0; }
+
+#define REG_A4XX_VPC_SO_FLUSH_WADDR_3				0x0000216e
+
+#define REG_A4XX_VSC_BIN_SIZE					0x00000c00
+#define A4XX_VSC_BIN_SIZE_WIDTH__MASK				0x0000001f
+#define A4XX_VSC_BIN_SIZE_WIDTH__SHIFT				0
+static inline uint32_t A4XX_VSC_BIN_SIZE_WIDTH(uint32_t val)
+{
+	return ((val >> 5) << A4XX_VSC_BIN_SIZE_WIDTH__SHIFT) & A4XX_VSC_BIN_SIZE_WIDTH__MASK;
+}
+#define A4XX_VSC_BIN_SIZE_HEIGHT__MASK				0x000003e0
+#define A4XX_VSC_BIN_SIZE_HEIGHT__SHIFT				5
+static inline uint32_t A4XX_VSC_BIN_SIZE_HEIGHT(uint32_t val)
+{
+	return ((val >> 5) << A4XX_VSC_BIN_SIZE_HEIGHT__SHIFT) & A4XX_VSC_BIN_SIZE_HEIGHT__MASK;
+}
+
+#define REG_A4XX_VSC_SIZE_ADDRESS				0x00000c01
+
+#define REG_A4XX_VSC_SIZE_ADDRESS2				0x00000c02
+
+#define REG_A4XX_VSC_DEBUG_ECO_CONTROL				0x00000c03
+
+static inline uint32_t REG_A4XX_VSC_PIPE_CONFIG(uint32_t i0) { return 0x00000c08 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_VSC_PIPE_CONFIG_REG(uint32_t i0) { return 0x00000c08 + 0x1*i0; }
+#define A4XX_VSC_PIPE_CONFIG_REG_X__MASK			0x000003ff
+#define A4XX_VSC_PIPE_CONFIG_REG_X__SHIFT			0
+static inline uint32_t A4XX_VSC_PIPE_CONFIG_REG_X(uint32_t val)
+{
+	return ((val) << A4XX_VSC_PIPE_CONFIG_REG_X__SHIFT) & A4XX_VSC_PIPE_CONFIG_REG_X__MASK;
+}
+#define A4XX_VSC_PIPE_CONFIG_REG_Y__MASK			0x000ffc00
+#define A4XX_VSC_PIPE_CONFIG_REG_Y__SHIFT			10
+static inline uint32_t A4XX_VSC_PIPE_CONFIG_REG_Y(uint32_t val)
+{
+	return ((val) << A4XX_VSC_PIPE_CONFIG_REG_Y__SHIFT) & A4XX_VSC_PIPE_CONFIG_REG_Y__MASK;
+}
+#define A4XX_VSC_PIPE_CONFIG_REG_W__MASK			0x00f00000
+#define A4XX_VSC_PIPE_CONFIG_REG_W__SHIFT			20
+static inline uint32_t A4XX_VSC_PIPE_CONFIG_REG_W(uint32_t val)
+{
+	return ((val) << A4XX_VSC_PIPE_CONFIG_REG_W__SHIFT) & A4XX_VSC_PIPE_CONFIG_REG_W__MASK;
+}
+#define A4XX_VSC_PIPE_CONFIG_REG_H__MASK			0x0f000000
+#define A4XX_VSC_PIPE_CONFIG_REG_H__SHIFT			24
+static inline uint32_t A4XX_VSC_PIPE_CONFIG_REG_H(uint32_t val)
+{
+	return ((val) << A4XX_VSC_PIPE_CONFIG_REG_H__SHIFT) & A4XX_VSC_PIPE_CONFIG_REG_H__MASK;
+}
+
+static inline uint32_t REG_A4XX_VSC_PIPE_DATA_ADDRESS(uint32_t i0) { return 0x00000c10 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_VSC_PIPE_DATA_ADDRESS_REG(uint32_t i0) { return 0x00000c10 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_VSC_PIPE_DATA_LENGTH(uint32_t i0) { return 0x00000c18 + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_VSC_PIPE_DATA_LENGTH_REG(uint32_t i0) { return 0x00000c18 + 0x1*i0; }
+
+#define REG_A4XX_VSC_PIPE_PARTIAL_POSN_1			0x00000c41
+
+#define REG_A4XX_VSC_PERFCTR_VSC_SEL_0				0x00000c50
+
+#define REG_A4XX_VSC_PERFCTR_VSC_SEL_1				0x00000c51
+
+#define REG_A4XX_VFD_DEBUG_CONTROL				0x00000e40
+
+#define REG_A4XX_VFD_PERFCTR_VFD_SEL_7				0x00000e4a
+
+#define REG_A4XX_VFD_CONTROL_0					0x00002200
+#define A4XX_VFD_CONTROL_0_TOTALATTRTOVS__MASK			0x000000ff
+#define A4XX_VFD_CONTROL_0_TOTALATTRTOVS__SHIFT			0
+static inline uint32_t A4XX_VFD_CONTROL_0_TOTALATTRTOVS(uint32_t val)
+{
+	return ((val) << A4XX_VFD_CONTROL_0_TOTALATTRTOVS__SHIFT) & A4XX_VFD_CONTROL_0_TOTALATTRTOVS__MASK;
+}
+#define A4XX_VFD_CONTROL_0_BYPASSATTROVS__MASK			0x0001fe00
+#define A4XX_VFD_CONTROL_0_BYPASSATTROVS__SHIFT			9
+static inline uint32_t A4XX_VFD_CONTROL_0_BYPASSATTROVS(uint32_t val)
+{
+	return ((val) << A4XX_VFD_CONTROL_0_BYPASSATTROVS__SHIFT) & A4XX_VFD_CONTROL_0_BYPASSATTROVS__MASK;
+}
+#define A4XX_VFD_CONTROL_0_STRMDECINSTRCNT__MASK		0x03f00000
+#define A4XX_VFD_CONTROL_0_STRMDECINSTRCNT__SHIFT		20
+static inline uint32_t A4XX_VFD_CONTROL_0_STRMDECINSTRCNT(uint32_t val)
+{
+	return ((val) << A4XX_VFD_CONTROL_0_STRMDECINSTRCNT__SHIFT) & A4XX_VFD_CONTROL_0_STRMDECINSTRCNT__MASK;
+}
+#define A4XX_VFD_CONTROL_0_STRMFETCHINSTRCNT__MASK		0xfc000000
+#define A4XX_VFD_CONTROL_0_STRMFETCHINSTRCNT__SHIFT		26
+static inline uint32_t A4XX_VFD_CONTROL_0_STRMFETCHINSTRCNT(uint32_t val)
+{
+	return ((val) << A4XX_VFD_CONTROL_0_STRMFETCHINSTRCNT__SHIFT) & A4XX_VFD_CONTROL_0_STRMFETCHINSTRCNT__MASK;
+}
+
+#define REG_A4XX_VFD_CONTROL_1					0x00002201
+#define A4XX_VFD_CONTROL_1_MAXSTORAGE__MASK			0x0000ffff
+#define A4XX_VFD_CONTROL_1_MAXSTORAGE__SHIFT			0
+static inline uint32_t A4XX_VFD_CONTROL_1_MAXSTORAGE(uint32_t val)
+{
+	return ((val) << A4XX_VFD_CONTROL_1_MAXSTORAGE__SHIFT) & A4XX_VFD_CONTROL_1_MAXSTORAGE__MASK;
+}
+#define A4XX_VFD_CONTROL_1_REGID4VTX__MASK			0x00ff0000
+#define A4XX_VFD_CONTROL_1_REGID4VTX__SHIFT			16
+static inline uint32_t A4XX_VFD_CONTROL_1_REGID4VTX(uint32_t val)
+{
+	return ((val) << A4XX_VFD_CONTROL_1_REGID4VTX__SHIFT) & A4XX_VFD_CONTROL_1_REGID4VTX__MASK;
+}
+#define A4XX_VFD_CONTROL_1_REGID4INST__MASK			0xff000000
+#define A4XX_VFD_CONTROL_1_REGID4INST__SHIFT			24
+static inline uint32_t A4XX_VFD_CONTROL_1_REGID4INST(uint32_t val)
+{
+	return ((val) << A4XX_VFD_CONTROL_1_REGID4INST__SHIFT) & A4XX_VFD_CONTROL_1_REGID4INST__MASK;
+}
+
+#define REG_A4XX_VFD_CONTROL_2					0x00002202
+
+#define REG_A4XX_VFD_CONTROL_3					0x00002203
+
+#define REG_A4XX_VFD_CONTROL_4					0x00002204
+
+#define REG_A4XX_VFD_INDEX_OFFSET				0x00002208
+
+static inline uint32_t REG_A4XX_VFD_FETCH(uint32_t i0) { return 0x0000220a + 0x4*i0; }
+
+static inline uint32_t REG_A4XX_VFD_FETCH_INSTR_0(uint32_t i0) { return 0x0000220a + 0x4*i0; }
+#define A4XX_VFD_FETCH_INSTR_0_FETCHSIZE__MASK			0x0000007f
+#define A4XX_VFD_FETCH_INSTR_0_FETCHSIZE__SHIFT			0
+static inline uint32_t A4XX_VFD_FETCH_INSTR_0_FETCHSIZE(uint32_t val)
+{
+	return ((val) << A4XX_VFD_FETCH_INSTR_0_FETCHSIZE__SHIFT) & A4XX_VFD_FETCH_INSTR_0_FETCHSIZE__MASK;
+}
+#define A4XX_VFD_FETCH_INSTR_0_BUFSTRIDE__MASK			0x0001ff80
+#define A4XX_VFD_FETCH_INSTR_0_BUFSTRIDE__SHIFT			7
+static inline uint32_t A4XX_VFD_FETCH_INSTR_0_BUFSTRIDE(uint32_t val)
+{
+	return ((val) << A4XX_VFD_FETCH_INSTR_0_BUFSTRIDE__SHIFT) & A4XX_VFD_FETCH_INSTR_0_BUFSTRIDE__MASK;
+}
+#define A4XX_VFD_FETCH_INSTR_0_SWITCHNEXT			0x00080000
+#define A4XX_VFD_FETCH_INSTR_0_STEPRATE__MASK			0xff000000
+#define A4XX_VFD_FETCH_INSTR_0_STEPRATE__SHIFT			24
+static inline uint32_t A4XX_VFD_FETCH_INSTR_0_STEPRATE(uint32_t val)
+{
+	return ((val) << A4XX_VFD_FETCH_INSTR_0_STEPRATE__SHIFT) & A4XX_VFD_FETCH_INSTR_0_STEPRATE__MASK;
+}
+
+static inline uint32_t REG_A4XX_VFD_FETCH_INSTR_1(uint32_t i0) { return 0x0000220b + 0x4*i0; }
+
+static inline uint32_t REG_A4XX_VFD_FETCH_INSTR_2(uint32_t i0) { return 0x0000220c + 0x4*i0; }
+#define A4XX_VFD_FETCH_INSTR_2_SIZE__MASK			0xfffffff0
+#define A4XX_VFD_FETCH_INSTR_2_SIZE__SHIFT			4
+static inline uint32_t A4XX_VFD_FETCH_INSTR_2_SIZE(uint32_t val)
+{
+	return ((val >> 4) << A4XX_VFD_FETCH_INSTR_2_SIZE__SHIFT) & A4XX_VFD_FETCH_INSTR_2_SIZE__MASK;
+}
+
+static inline uint32_t REG_A4XX_VFD_FETCH_INSTR_3(uint32_t i0) { return 0x0000220d + 0x4*i0; }
+
+static inline uint32_t REG_A4XX_VFD_DECODE(uint32_t i0) { return 0x0000228a + 0x1*i0; }
+
+static inline uint32_t REG_A4XX_VFD_DECODE_INSTR(uint32_t i0) { return 0x0000228a + 0x1*i0; }
+#define A4XX_VFD_DECODE_INSTR_WRITEMASK__MASK			0x0000000f
+#define A4XX_VFD_DECODE_INSTR_WRITEMASK__SHIFT			0
+static inline uint32_t A4XX_VFD_DECODE_INSTR_WRITEMASK(uint32_t val)
+{
+	return ((val) << A4XX_VFD_DECODE_INSTR_WRITEMASK__SHIFT) & A4XX_VFD_DECODE_INSTR_WRITEMASK__MASK;
+}
+#define A4XX_VFD_DECODE_INSTR_CONSTFILL				0x00000010
+#define A4XX_VFD_DECODE_INSTR_FORMAT__MASK			0x00000fc0
+#define A4XX_VFD_DECODE_INSTR_FORMAT__SHIFT			6
+static inline uint32_t A4XX_VFD_DECODE_INSTR_FORMAT(enum a4xx_vtx_fmt val)
+{
+	return ((val) << A4XX_VFD_DECODE_INSTR_FORMAT__SHIFT) & A4XX_VFD_DECODE_INSTR_FORMAT__MASK;
+}
+#define A4XX_VFD_DECODE_INSTR_REGID__MASK			0x000ff000
+#define A4XX_VFD_DECODE_INSTR_REGID__SHIFT			12
+static inline uint32_t A4XX_VFD_DECODE_INSTR_REGID(uint32_t val)
+{
+	return ((val) << A4XX_VFD_DECODE_INSTR_REGID__SHIFT) & A4XX_VFD_DECODE_INSTR_REGID__MASK;
+}
+#define A4XX_VFD_DECODE_INSTR_SWAP__MASK			0x00c00000
+#define A4XX_VFD_DECODE_INSTR_SWAP__SHIFT			22
+static inline uint32_t A4XX_VFD_DECODE_INSTR_SWAP(enum a3xx_color_swap val)
+{
+	return ((val) << A4XX_VFD_DECODE_INSTR_SWAP__SHIFT) & A4XX_VFD_DECODE_INSTR_SWAP__MASK;
+}
+#define A4XX_VFD_DECODE_INSTR_SHIFTCNT__MASK			0x1f000000
+#define A4XX_VFD_DECODE_INSTR_SHIFTCNT__SHIFT			24
+static inline uint32_t A4XX_VFD_DECODE_INSTR_SHIFTCNT(uint32_t val)
+{
+	return ((val) << A4XX_VFD_DECODE_INSTR_SHIFTCNT__SHIFT) & A4XX_VFD_DECODE_INSTR_SHIFTCNT__MASK;
+}
+#define A4XX_VFD_DECODE_INSTR_LASTCOMPVALID			0x20000000
+#define A4XX_VFD_DECODE_INSTR_SWITCHNEXT			0x40000000
+
+#define REG_A4XX_TPL1_DEBUG_ECO_CONTROL				0x00000f00
+
+#define REG_A4XX_TPL1_PERFCTR_TP_SEL_7				0x00000f0b
+
+#define REG_A4XX_TPL1_TP_TEX_OFFSET				0x00002380
+
+#define REG_A4XX_TPL1_TP_CS_TEXMEMOBJ_BASE_ADDR			0x000023a6
+
+#define REG_A4XX_GRAS_TSE_STATUS				0x00000c80
+
+#define REG_A4XX_GRAS_DEBUG_ECO_CONTROL				0x00000c81
+
+#define REG_A4XX_GRAS_PERFCTR_TSE_SEL_0				0x00000c88
+
+#define REG_A4XX_GRAS_PERFCTR_TSE_SEL_3				0x00000c8b
+
+#define REG_A4XX_GRAS_CL_CLIP_CNTL				0x00002000
+
+#define REG_A4XX_GRAS_CLEAR_CNTL				0x00002003
+#define A4XX_GRAS_CLEAR_CNTL_NOT_FASTCLEAR			0x00000001
+
+#define REG_A4XX_GRAS_CL_GB_CLIP_ADJ				0x00002004
+#define A4XX_GRAS_CL_GB_CLIP_ADJ_HORZ__MASK			0x000003ff
+#define A4XX_GRAS_CL_GB_CLIP_ADJ_HORZ__SHIFT			0
+static inline uint32_t A4XX_GRAS_CL_GB_CLIP_ADJ_HORZ(uint32_t val)
+{
+	return ((val) << A4XX_GRAS_CL_GB_CLIP_ADJ_HORZ__SHIFT) & A4XX_GRAS_CL_GB_CLIP_ADJ_HORZ__MASK;
+}
+#define A4XX_GRAS_CL_GB_CLIP_ADJ_VERT__MASK			0x000ffc00
+#define A4XX_GRAS_CL_GB_CLIP_ADJ_VERT__SHIFT			10
+static inline uint32_t A4XX_GRAS_CL_GB_CLIP_ADJ_VERT(uint32_t val)
+{
+	return ((val) << A4XX_GRAS_CL_GB_CLIP_ADJ_VERT__SHIFT) & A4XX_GRAS_CL_GB_CLIP_ADJ_VERT__MASK;
+}
+
+#define REG_A4XX_GRAS_CL_VPORT_XOFFSET_0			0x00002008
+#define A4XX_GRAS_CL_VPORT_XOFFSET_0__MASK			0xffffffff
+#define A4XX_GRAS_CL_VPORT_XOFFSET_0__SHIFT			0
+static inline uint32_t A4XX_GRAS_CL_VPORT_XOFFSET_0(float val)
+{
+	return ((fui(val)) << A4XX_GRAS_CL_VPORT_XOFFSET_0__SHIFT) & A4XX_GRAS_CL_VPORT_XOFFSET_0__MASK;
+}
+
+#define REG_A4XX_GRAS_CL_VPORT_XSCALE_0				0x00002009
+#define A4XX_GRAS_CL_VPORT_XSCALE_0__MASK			0xffffffff
+#define A4XX_GRAS_CL_VPORT_XSCALE_0__SHIFT			0
+static inline uint32_t A4XX_GRAS_CL_VPORT_XSCALE_0(float val)
+{
+	return ((fui(val)) << A4XX_GRAS_CL_VPORT_XSCALE_0__SHIFT) & A4XX_GRAS_CL_VPORT_XSCALE_0__MASK;
+}
+
+#define REG_A4XX_GRAS_CL_VPORT_YOFFSET_0			0x0000200a
+#define A4XX_GRAS_CL_VPORT_YOFFSET_0__MASK			0xffffffff
+#define A4XX_GRAS_CL_VPORT_YOFFSET_0__SHIFT			0
+static inline uint32_t A4XX_GRAS_CL_VPORT_YOFFSET_0(float val)
+{
+	return ((fui(val)) << A4XX_GRAS_CL_VPORT_YOFFSET_0__SHIFT) & A4XX_GRAS_CL_VPORT_YOFFSET_0__MASK;
+}
+
+#define REG_A4XX_GRAS_CL_VPORT_YSCALE_0				0x0000200b
+#define A4XX_GRAS_CL_VPORT_YSCALE_0__MASK			0xffffffff
+#define A4XX_GRAS_CL_VPORT_YSCALE_0__SHIFT			0
+static inline uint32_t A4XX_GRAS_CL_VPORT_YSCALE_0(float val)
+{
+	return ((fui(val)) << A4XX_GRAS_CL_VPORT_YSCALE_0__SHIFT) & A4XX_GRAS_CL_VPORT_YSCALE_0__MASK;
+}
+
+#define REG_A4XX_GRAS_CL_VPORT_ZOFFSET_0			0x0000200c
+#define A4XX_GRAS_CL_VPORT_ZOFFSET_0__MASK			0xffffffff
+#define A4XX_GRAS_CL_VPORT_ZOFFSET_0__SHIFT			0
+static inline uint32_t A4XX_GRAS_CL_VPORT_ZOFFSET_0(float val)
+{
+	return ((fui(val)) << A4XX_GRAS_CL_VPORT_ZOFFSET_0__SHIFT) & A4XX_GRAS_CL_VPORT_ZOFFSET_0__MASK;
+}
+
+#define REG_A4XX_GRAS_CL_VPORT_ZSCALE_0				0x0000200d
+#define A4XX_GRAS_CL_VPORT_ZSCALE_0__MASK			0xffffffff
+#define A4XX_GRAS_CL_VPORT_ZSCALE_0__SHIFT			0
+static inline uint32_t A4XX_GRAS_CL_VPORT_ZSCALE_0(float val)
+{
+	return ((fui(val)) << A4XX_GRAS_CL_VPORT_ZSCALE_0__SHIFT) & A4XX_GRAS_CL_VPORT_ZSCALE_0__MASK;
+}
+
+#define REG_A4XX_GRAS_SU_POINT_MINMAX				0x00002070
+#define A4XX_GRAS_SU_POINT_MINMAX_MIN__MASK			0x0000ffff
+#define A4XX_GRAS_SU_POINT_MINMAX_MIN__SHIFT			0
+static inline uint32_t A4XX_GRAS_SU_POINT_MINMAX_MIN(float val)
+{
+	return ((((uint32_t)(val * 16.0))) << A4XX_GRAS_SU_POINT_MINMAX_MIN__SHIFT) & A4XX_GRAS_SU_POINT_MINMAX_MIN__MASK;
+}
+#define A4XX_GRAS_SU_POINT_MINMAX_MAX__MASK			0xffff0000
+#define A4XX_GRAS_SU_POINT_MINMAX_MAX__SHIFT			16
+static inline uint32_t A4XX_GRAS_SU_POINT_MINMAX_MAX(float val)
+{
+	return ((((uint32_t)(val * 16.0))) << A4XX_GRAS_SU_POINT_MINMAX_MAX__SHIFT) & A4XX_GRAS_SU_POINT_MINMAX_MAX__MASK;
+}
+
+#define REG_A4XX_GRAS_SU_POINT_SIZE				0x00002071
+#define A4XX_GRAS_SU_POINT_SIZE__MASK				0xffffffff
+#define A4XX_GRAS_SU_POINT_SIZE__SHIFT				0
+static inline uint32_t A4XX_GRAS_SU_POINT_SIZE(float val)
+{
+	return ((((int32_t)(val * 16.0))) << A4XX_GRAS_SU_POINT_SIZE__SHIFT) & A4XX_GRAS_SU_POINT_SIZE__MASK;
+}
+
+#define REG_A4XX_GRAS_ALPHA_CONTROL				0x00002073
+#define A4XX_GRAS_ALPHA_CONTROL_ALPHA_TEST_ENABLE		0x00000004
+
+#define REG_A4XX_GRAS_SU_POLY_OFFSET_SCALE			0x00002074
+#define A4XX_GRAS_SU_POLY_OFFSET_SCALE__MASK			0xffffffff
+#define A4XX_GRAS_SU_POLY_OFFSET_SCALE__SHIFT			0
+static inline uint32_t A4XX_GRAS_SU_POLY_OFFSET_SCALE(float val)
+{
+	return ((fui(val)) << A4XX_GRAS_SU_POLY_OFFSET_SCALE__SHIFT) & A4XX_GRAS_SU_POLY_OFFSET_SCALE__MASK;
+}
+
+#define REG_A4XX_GRAS_SU_POLY_OFFSET_OFFSET			0x00002075
+#define A4XX_GRAS_SU_POLY_OFFSET_OFFSET__MASK			0xffffffff
+#define A4XX_GRAS_SU_POLY_OFFSET_OFFSET__SHIFT			0
+static inline uint32_t A4XX_GRAS_SU_POLY_OFFSET_OFFSET(float val)
+{
+	return ((fui(val)) << A4XX_GRAS_SU_POLY_OFFSET_OFFSET__SHIFT) & A4XX_GRAS_SU_POLY_OFFSET_OFFSET__MASK;
+}
+
+#define REG_A4XX_GRAS_SC_EXTENT_WINDOW_TL			0x0000209f
+
+#define REG_A4XX_GRAS_SC_SCREEN_SCISSOR_TL			0x0000207c
+#define A4XX_GRAS_SC_SCREEN_SCISSOR_TL_WINDOW_OFFSET_DISABLE	0x80000000
+#define A4XX_GRAS_SC_SCREEN_SCISSOR_TL_X__MASK			0x00007fff
+#define A4XX_GRAS_SC_SCREEN_SCISSOR_TL_X__SHIFT			0
+static inline uint32_t A4XX_GRAS_SC_SCREEN_SCISSOR_TL_X(uint32_t val)
+{
+	return ((val) << A4XX_GRAS_SC_SCREEN_SCISSOR_TL_X__SHIFT) & A4XX_GRAS_SC_SCREEN_SCISSOR_TL_X__MASK;
+}
+#define A4XX_GRAS_SC_SCREEN_SCISSOR_TL_Y__MASK			0x7fff0000
+#define A4XX_GRAS_SC_SCREEN_SCISSOR_TL_Y__SHIFT			16
+static inline uint32_t A4XX_GRAS_SC_SCREEN_SCISSOR_TL_Y(uint32_t val)
+{
+	return ((val) << A4XX_GRAS_SC_SCREEN_SCISSOR_TL_Y__SHIFT) & A4XX_GRAS_SC_SCREEN_SCISSOR_TL_Y__MASK;
+}
+
+#define REG_A4XX_GRAS_SC_SCREEN_SCISSOR_BR			0x0000207d
+#define A4XX_GRAS_SC_SCREEN_SCISSOR_BR_WINDOW_OFFSET_DISABLE	0x80000000
+#define A4XX_GRAS_SC_SCREEN_SCISSOR_BR_X__MASK			0x00007fff
+#define A4XX_GRAS_SC_SCREEN_SCISSOR_BR_X__SHIFT			0
+static inline uint32_t A4XX_GRAS_SC_SCREEN_SCISSOR_BR_X(uint32_t val)
+{
+	return ((val) << A4XX_GRAS_SC_SCREEN_SCISSOR_BR_X__SHIFT) & A4XX_GRAS_SC_SCREEN_SCISSOR_BR_X__MASK;
+}
+#define A4XX_GRAS_SC_SCREEN_SCISSOR_BR_Y__MASK			0x7fff0000
+#define A4XX_GRAS_SC_SCREEN_SCISSOR_BR_Y__SHIFT			16
+static inline uint32_t A4XX_GRAS_SC_SCREEN_SCISSOR_BR_Y(uint32_t val)
+{
+	return ((val) << A4XX_GRAS_SC_SCREEN_SCISSOR_BR_Y__SHIFT) & A4XX_GRAS_SC_SCREEN_SCISSOR_BR_Y__MASK;
+}
+
+#define REG_A4XX_GRAS_SC_WINDOW_SCISSOR_BR			0x0000209c
+#define A4XX_GRAS_SC_WINDOW_SCISSOR_BR_WINDOW_OFFSET_DISABLE	0x80000000
+#define A4XX_GRAS_SC_WINDOW_SCISSOR_BR_X__MASK			0x00007fff
+#define A4XX_GRAS_SC_WINDOW_SCISSOR_BR_X__SHIFT			0
+static inline uint32_t A4XX_GRAS_SC_WINDOW_SCISSOR_BR_X(uint32_t val)
+{
+	return ((val) << A4XX_GRAS_SC_WINDOW_SCISSOR_BR_X__SHIFT) & A4XX_GRAS_SC_WINDOW_SCISSOR_BR_X__MASK;
+}
+#define A4XX_GRAS_SC_WINDOW_SCISSOR_BR_Y__MASK			0x7fff0000
+#define A4XX_GRAS_SC_WINDOW_SCISSOR_BR_Y__SHIFT			16
+static inline uint32_t A4XX_GRAS_SC_WINDOW_SCISSOR_BR_Y(uint32_t val)
+{
+	return ((val) << A4XX_GRAS_SC_WINDOW_SCISSOR_BR_Y__SHIFT) & A4XX_GRAS_SC_WINDOW_SCISSOR_BR_Y__MASK;
+}
+
+#define REG_A4XX_GRAS_SC_WINDOW_SCISSOR_TL			0x0000209d
+#define A4XX_GRAS_SC_WINDOW_SCISSOR_TL_WINDOW_OFFSET_DISABLE	0x80000000
+#define A4XX_GRAS_SC_WINDOW_SCISSOR_TL_X__MASK			0x00007fff
+#define A4XX_GRAS_SC_WINDOW_SCISSOR_TL_X__SHIFT			0
+static inline uint32_t A4XX_GRAS_SC_WINDOW_SCISSOR_TL_X(uint32_t val)
+{
+	return ((val) << A4XX_GRAS_SC_WINDOW_SCISSOR_TL_X__SHIFT) & A4XX_GRAS_SC_WINDOW_SCISSOR_TL_X__MASK;
+}
+#define A4XX_GRAS_SC_WINDOW_SCISSOR_TL_Y__MASK			0x7fff0000
+#define A4XX_GRAS_SC_WINDOW_SCISSOR_TL_Y__SHIFT			16
+static inline uint32_t A4XX_GRAS_SC_WINDOW_SCISSOR_TL_Y(uint32_t val)
+{
+	return ((val) << A4XX_GRAS_SC_WINDOW_SCISSOR_TL_Y__SHIFT) & A4XX_GRAS_SC_WINDOW_SCISSOR_TL_Y__MASK;
+}
+
+#define REG_A4XX_GRAS_DEPTH_CONTROL				0x00002077
+#define A4XX_GRAS_DEPTH_CONTROL_FORMAT__MASK			0x00000003
+#define A4XX_GRAS_DEPTH_CONTROL_FORMAT__SHIFT			0
+static inline uint32_t A4XX_GRAS_DEPTH_CONTROL_FORMAT(enum a4xx_depth_format val)
+{
+	return ((val) << A4XX_GRAS_DEPTH_CONTROL_FORMAT__SHIFT) & A4XX_GRAS_DEPTH_CONTROL_FORMAT__MASK;
+}
+
+#define REG_A4XX_GRAS_SU_MODE_CONTROL				0x00002078
+#define A4XX_GRAS_SU_MODE_CONTROL_CULL_FRONT			0x00000001
+#define A4XX_GRAS_SU_MODE_CONTROL_CULL_BACK			0x00000002
+#define A4XX_GRAS_SU_MODE_CONTROL_FRONT_CW			0x00000004
+#define A4XX_GRAS_SU_MODE_CONTROL_LINEHALFWIDTH__MASK		0x000007f8
+#define A4XX_GRAS_SU_MODE_CONTROL_LINEHALFWIDTH__SHIFT		3
+static inline uint32_t A4XX_GRAS_SU_MODE_CONTROL_LINEHALFWIDTH(float val)
+{
+	return ((((int32_t)(val * 4.0))) << A4XX_GRAS_SU_MODE_CONTROL_LINEHALFWIDTH__SHIFT) & A4XX_GRAS_SU_MODE_CONTROL_LINEHALFWIDTH__MASK;
+}
+#define A4XX_GRAS_SU_MODE_CONTROL_POLY_OFFSET			0x00000800
+#define A4XX_GRAS_SU_MODE_CONTROL_RENDERING_PASS		0x00100000
+
+#define REG_A4XX_GRAS_SC_CONTROL				0x0000207b
+#define A4XX_GRAS_SC_CONTROL_RENDER_MODE__MASK			0x0000000c
+#define A4XX_GRAS_SC_CONTROL_RENDER_MODE__SHIFT			2
+static inline uint32_t A4XX_GRAS_SC_CONTROL_RENDER_MODE(enum a3xx_render_mode val)
+{
+	return ((val) << A4XX_GRAS_SC_CONTROL_RENDER_MODE__SHIFT) & A4XX_GRAS_SC_CONTROL_RENDER_MODE__MASK;
+}
+#define A4XX_GRAS_SC_CONTROL_MSAA_SAMPLES__MASK			0x00000380
+#define A4XX_GRAS_SC_CONTROL_MSAA_SAMPLES__SHIFT		7
+static inline uint32_t A4XX_GRAS_SC_CONTROL_MSAA_SAMPLES(uint32_t val)
+{
+	return ((val) << A4XX_GRAS_SC_CONTROL_MSAA_SAMPLES__SHIFT) & A4XX_GRAS_SC_CONTROL_MSAA_SAMPLES__MASK;
+}
+#define A4XX_GRAS_SC_CONTROL_MSAA_DISABLE			0x00000800
+#define A4XX_GRAS_SC_CONTROL_RASTER_MODE__MASK			0x0000f000
+#define A4XX_GRAS_SC_CONTROL_RASTER_MODE__SHIFT			12
+static inline uint32_t A4XX_GRAS_SC_CONTROL_RASTER_MODE(uint32_t val)
+{
+	return ((val) << A4XX_GRAS_SC_CONTROL_RASTER_MODE__SHIFT) & A4XX_GRAS_SC_CONTROL_RASTER_MODE__MASK;
+}
+
+#define REG_A4XX_UCHE_CACHE_MODE_CONTROL			0x00000e80
+
+#define REG_A4XX_UCHE_TRAP_BASE_LO				0x00000e83
+
+#define REG_A4XX_UCHE_TRAP_BASE_HI				0x00000e84
+
+#define REG_A4XX_UCHE_CACHE_STATUS				0x00000e88
+
+#define REG_A4XX_UCHE_INVALIDATE0				0x00000e8a
+
+#define REG_A4XX_UCHE_INVALIDATE1				0x00000e8b
+
+#define REG_A4XX_UCHE_CACHE_WAYS_VFD				0x00000e8c
+
+#define REG_A4XX_UCHE_PERFCTR_UCHE_SEL_7			0x00000e95
+
+#define REG_A4XX_HLSQ_TIMEOUT_THRESHOLD				0x00000e00
+
+#define REG_A4XX_HLSQ_DEBUG_ECO_CONTROL				0x00000e04
+
+#define REG_A4XX_HLSQ_PERF_PIPE_MASK				0x00000e0e
+
+#define REG_A4XX_HLSQ_CONTROL_0_REG				0x000023c0
+#define A4XX_HLSQ_CONTROL_0_REG_FSTHREADSIZE__MASK		0x00000010
+#define A4XX_HLSQ_CONTROL_0_REG_FSTHREADSIZE__SHIFT		4
+static inline uint32_t A4XX_HLSQ_CONTROL_0_REG_FSTHREADSIZE(enum a3xx_threadsize val)
+{
+	return ((val) << A4XX_HLSQ_CONTROL_0_REG_FSTHREADSIZE__SHIFT) & A4XX_HLSQ_CONTROL_0_REG_FSTHREADSIZE__MASK;
+}
+#define A4XX_HLSQ_CONTROL_0_REG_FSSUPERTHREADENABLE		0x00000040
+#define A4XX_HLSQ_CONTROL_0_REG_SPSHADERRESTART			0x00000200
+#define A4XX_HLSQ_CONTROL_0_REG_RESERVED2			0x00000400
+#define A4XX_HLSQ_CONTROL_0_REG_CHUNKDISABLE			0x04000000
+#define A4XX_HLSQ_CONTROL_0_REG_CONSTMODE__MASK			0x08000000
+#define A4XX_HLSQ_CONTROL_0_REG_CONSTMODE__SHIFT		27
+static inline uint32_t A4XX_HLSQ_CONTROL_0_REG_CONSTMODE(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_CONTROL_0_REG_CONSTMODE__SHIFT) & A4XX_HLSQ_CONTROL_0_REG_CONSTMODE__MASK;
+}
+#define A4XX_HLSQ_CONTROL_0_REG_LAZYUPDATEDISABLE		0x10000000
+#define A4XX_HLSQ_CONTROL_0_REG_SPCONSTFULLUPDATE		0x20000000
+#define A4XX_HLSQ_CONTROL_0_REG_TPFULLUPDATE			0x40000000
+#define A4XX_HLSQ_CONTROL_0_REG_SINGLECONTEXT			0x80000000
+
+#define REG_A4XX_HLSQ_CONTROL_1_REG				0x000023c1
+#define A4XX_HLSQ_CONTROL_1_REG_VSTHREADSIZE__MASK		0x00000040
+#define A4XX_HLSQ_CONTROL_1_REG_VSTHREADSIZE__SHIFT		6
+static inline uint32_t A4XX_HLSQ_CONTROL_1_REG_VSTHREADSIZE(enum a3xx_threadsize val)
+{
+	return ((val) << A4XX_HLSQ_CONTROL_1_REG_VSTHREADSIZE__SHIFT) & A4XX_HLSQ_CONTROL_1_REG_VSTHREADSIZE__MASK;
+}
+#define A4XX_HLSQ_CONTROL_1_REG_VSSUPERTHREADENABLE		0x00000100
+#define A4XX_HLSQ_CONTROL_1_REG_RESERVED1			0x00000200
+#define A4XX_HLSQ_CONTROL_1_REG_ZWCOORD				0x02000000
+
+#define REG_A4XX_HLSQ_CONTROL_2_REG				0x000023c2
+#define A4XX_HLSQ_CONTROL_2_REG_PRIMALLOCTHRESHOLD__MASK	0xfc000000
+#define A4XX_HLSQ_CONTROL_2_REG_PRIMALLOCTHRESHOLD__SHIFT	26
+static inline uint32_t A4XX_HLSQ_CONTROL_2_REG_PRIMALLOCTHRESHOLD(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_CONTROL_2_REG_PRIMALLOCTHRESHOLD__SHIFT) & A4XX_HLSQ_CONTROL_2_REG_PRIMALLOCTHRESHOLD__MASK;
+}
+
+#define REG_A4XX_HLSQ_CONTROL_3_REG				0x000023c3
+#define A4XX_HLSQ_CONTROL_3_REG_REGID__MASK			0x000000ff
+#define A4XX_HLSQ_CONTROL_3_REG_REGID__SHIFT			0
+static inline uint32_t A4XX_HLSQ_CONTROL_3_REG_REGID(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_CONTROL_3_REG_REGID__SHIFT) & A4XX_HLSQ_CONTROL_3_REG_REGID__MASK;
+}
+
+#define REG_A4XX_HLSQ_VS_CONTROL_REG				0x000023c5
+#define A4XX_HLSQ_VS_CONTROL_REG_CONSTLENGTH__MASK		0x000000ff
+#define A4XX_HLSQ_VS_CONTROL_REG_CONSTLENGTH__SHIFT		0
+static inline uint32_t A4XX_HLSQ_VS_CONTROL_REG_CONSTLENGTH(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_VS_CONTROL_REG_CONSTLENGTH__SHIFT) & A4XX_HLSQ_VS_CONTROL_REG_CONSTLENGTH__MASK;
+}
+#define A4XX_HLSQ_VS_CONTROL_REG_CONSTOBJECTOFFSET__MASK	0x0000ff00
+#define A4XX_HLSQ_VS_CONTROL_REG_CONSTOBJECTOFFSET__SHIFT	8
+static inline uint32_t A4XX_HLSQ_VS_CONTROL_REG_CONSTOBJECTOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_VS_CONTROL_REG_CONSTOBJECTOFFSET__SHIFT) & A4XX_HLSQ_VS_CONTROL_REG_CONSTOBJECTOFFSET__MASK;
+}
+#define A4XX_HLSQ_VS_CONTROL_REG_SHADEROBJOFFSET__MASK		0x00fe0000
+#define A4XX_HLSQ_VS_CONTROL_REG_SHADEROBJOFFSET__SHIFT		17
+static inline uint32_t A4XX_HLSQ_VS_CONTROL_REG_SHADEROBJOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_VS_CONTROL_REG_SHADEROBJOFFSET__SHIFT) & A4XX_HLSQ_VS_CONTROL_REG_SHADEROBJOFFSET__MASK;
+}
+#define A4XX_HLSQ_VS_CONTROL_REG_INSTRLENGTH__MASK		0xff000000
+#define A4XX_HLSQ_VS_CONTROL_REG_INSTRLENGTH__SHIFT		24
+static inline uint32_t A4XX_HLSQ_VS_CONTROL_REG_INSTRLENGTH(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_VS_CONTROL_REG_INSTRLENGTH__SHIFT) & A4XX_HLSQ_VS_CONTROL_REG_INSTRLENGTH__MASK;
+}
+
+#define REG_A4XX_HLSQ_FS_CONTROL_REG				0x000023c6
+#define A4XX_HLSQ_FS_CONTROL_REG_CONSTLENGTH__MASK		0x000000ff
+#define A4XX_HLSQ_FS_CONTROL_REG_CONSTLENGTH__SHIFT		0
+static inline uint32_t A4XX_HLSQ_FS_CONTROL_REG_CONSTLENGTH(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_FS_CONTROL_REG_CONSTLENGTH__SHIFT) & A4XX_HLSQ_FS_CONTROL_REG_CONSTLENGTH__MASK;
+}
+#define A4XX_HLSQ_FS_CONTROL_REG_CONSTOBJECTOFFSET__MASK	0x0000ff00
+#define A4XX_HLSQ_FS_CONTROL_REG_CONSTOBJECTOFFSET__SHIFT	8
+static inline uint32_t A4XX_HLSQ_FS_CONTROL_REG_CONSTOBJECTOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_FS_CONTROL_REG_CONSTOBJECTOFFSET__SHIFT) & A4XX_HLSQ_FS_CONTROL_REG_CONSTOBJECTOFFSET__MASK;
+}
+#define A4XX_HLSQ_FS_CONTROL_REG_SHADEROBJOFFSET__MASK		0x00fe0000
+#define A4XX_HLSQ_FS_CONTROL_REG_SHADEROBJOFFSET__SHIFT		17
+static inline uint32_t A4XX_HLSQ_FS_CONTROL_REG_SHADEROBJOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_FS_CONTROL_REG_SHADEROBJOFFSET__SHIFT) & A4XX_HLSQ_FS_CONTROL_REG_SHADEROBJOFFSET__MASK;
+}
+#define A4XX_HLSQ_FS_CONTROL_REG_INSTRLENGTH__MASK		0xff000000
+#define A4XX_HLSQ_FS_CONTROL_REG_INSTRLENGTH__SHIFT		24
+static inline uint32_t A4XX_HLSQ_FS_CONTROL_REG_INSTRLENGTH(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_FS_CONTROL_REG_INSTRLENGTH__SHIFT) & A4XX_HLSQ_FS_CONTROL_REG_INSTRLENGTH__MASK;
+}
+
+#define REG_A4XX_HLSQ_HS_CONTROL_REG				0x000023c7
+#define A4XX_HLSQ_HS_CONTROL_REG_CONSTLENGTH__MASK		0x000000ff
+#define A4XX_HLSQ_HS_CONTROL_REG_CONSTLENGTH__SHIFT		0
+static inline uint32_t A4XX_HLSQ_HS_CONTROL_REG_CONSTLENGTH(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_HS_CONTROL_REG_CONSTLENGTH__SHIFT) & A4XX_HLSQ_HS_CONTROL_REG_CONSTLENGTH__MASK;
+}
+#define A4XX_HLSQ_HS_CONTROL_REG_CONSTOBJECTOFFSET__MASK	0x0000ff00
+#define A4XX_HLSQ_HS_CONTROL_REG_CONSTOBJECTOFFSET__SHIFT	8
+static inline uint32_t A4XX_HLSQ_HS_CONTROL_REG_CONSTOBJECTOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_HS_CONTROL_REG_CONSTOBJECTOFFSET__SHIFT) & A4XX_HLSQ_HS_CONTROL_REG_CONSTOBJECTOFFSET__MASK;
+}
+#define A4XX_HLSQ_HS_CONTROL_REG_SHADEROBJOFFSET__MASK		0x00fe0000
+#define A4XX_HLSQ_HS_CONTROL_REG_SHADEROBJOFFSET__SHIFT		17
+static inline uint32_t A4XX_HLSQ_HS_CONTROL_REG_SHADEROBJOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_HS_CONTROL_REG_SHADEROBJOFFSET__SHIFT) & A4XX_HLSQ_HS_CONTROL_REG_SHADEROBJOFFSET__MASK;
+}
+#define A4XX_HLSQ_HS_CONTROL_REG_INSTRLENGTH__MASK		0xff000000
+#define A4XX_HLSQ_HS_CONTROL_REG_INSTRLENGTH__SHIFT		24
+static inline uint32_t A4XX_HLSQ_HS_CONTROL_REG_INSTRLENGTH(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_HS_CONTROL_REG_INSTRLENGTH__SHIFT) & A4XX_HLSQ_HS_CONTROL_REG_INSTRLENGTH__MASK;
+}
+
+#define REG_A4XX_HLSQ_DS_CONTROL_REG				0x000023c8
+#define A4XX_HLSQ_DS_CONTROL_REG_CONSTLENGTH__MASK		0x000000ff
+#define A4XX_HLSQ_DS_CONTROL_REG_CONSTLENGTH__SHIFT		0
+static inline uint32_t A4XX_HLSQ_DS_CONTROL_REG_CONSTLENGTH(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_DS_CONTROL_REG_CONSTLENGTH__SHIFT) & A4XX_HLSQ_DS_CONTROL_REG_CONSTLENGTH__MASK;
+}
+#define A4XX_HLSQ_DS_CONTROL_REG_CONSTOBJECTOFFSET__MASK	0x0000ff00
+#define A4XX_HLSQ_DS_CONTROL_REG_CONSTOBJECTOFFSET__SHIFT	8
+static inline uint32_t A4XX_HLSQ_DS_CONTROL_REG_CONSTOBJECTOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_DS_CONTROL_REG_CONSTOBJECTOFFSET__SHIFT) & A4XX_HLSQ_DS_CONTROL_REG_CONSTOBJECTOFFSET__MASK;
+}
+#define A4XX_HLSQ_DS_CONTROL_REG_SHADEROBJOFFSET__MASK		0x00fe0000
+#define A4XX_HLSQ_DS_CONTROL_REG_SHADEROBJOFFSET__SHIFT		17
+static inline uint32_t A4XX_HLSQ_DS_CONTROL_REG_SHADEROBJOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_DS_CONTROL_REG_SHADEROBJOFFSET__SHIFT) & A4XX_HLSQ_DS_CONTROL_REG_SHADEROBJOFFSET__MASK;
+}
+#define A4XX_HLSQ_DS_CONTROL_REG_INSTRLENGTH__MASK		0xff000000
+#define A4XX_HLSQ_DS_CONTROL_REG_INSTRLENGTH__SHIFT		24
+static inline uint32_t A4XX_HLSQ_DS_CONTROL_REG_INSTRLENGTH(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_DS_CONTROL_REG_INSTRLENGTH__SHIFT) & A4XX_HLSQ_DS_CONTROL_REG_INSTRLENGTH__MASK;
+}
+
+#define REG_A4XX_HLSQ_GS_CONTROL_REG				0x000023c9
+#define A4XX_HLSQ_GS_CONTROL_REG_CONSTLENGTH__MASK		0x000000ff
+#define A4XX_HLSQ_GS_CONTROL_REG_CONSTLENGTH__SHIFT		0
+static inline uint32_t A4XX_HLSQ_GS_CONTROL_REG_CONSTLENGTH(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_GS_CONTROL_REG_CONSTLENGTH__SHIFT) & A4XX_HLSQ_GS_CONTROL_REG_CONSTLENGTH__MASK;
+}
+#define A4XX_HLSQ_GS_CONTROL_REG_CONSTOBJECTOFFSET__MASK	0x0000ff00
+#define A4XX_HLSQ_GS_CONTROL_REG_CONSTOBJECTOFFSET__SHIFT	8
+static inline uint32_t A4XX_HLSQ_GS_CONTROL_REG_CONSTOBJECTOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_GS_CONTROL_REG_CONSTOBJECTOFFSET__SHIFT) & A4XX_HLSQ_GS_CONTROL_REG_CONSTOBJECTOFFSET__MASK;
+}
+#define A4XX_HLSQ_GS_CONTROL_REG_SHADEROBJOFFSET__MASK		0x00fe0000
+#define A4XX_HLSQ_GS_CONTROL_REG_SHADEROBJOFFSET__SHIFT		17
+static inline uint32_t A4XX_HLSQ_GS_CONTROL_REG_SHADEROBJOFFSET(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_GS_CONTROL_REG_SHADEROBJOFFSET__SHIFT) & A4XX_HLSQ_GS_CONTROL_REG_SHADEROBJOFFSET__MASK;
+}
+#define A4XX_HLSQ_GS_CONTROL_REG_INSTRLENGTH__MASK		0xff000000
+#define A4XX_HLSQ_GS_CONTROL_REG_INSTRLENGTH__SHIFT		24
+static inline uint32_t A4XX_HLSQ_GS_CONTROL_REG_INSTRLENGTH(uint32_t val)
+{
+	return ((val) << A4XX_HLSQ_GS_CONTROL_REG_INSTRLENGTH__SHIFT) & A4XX_HLSQ_GS_CONTROL_REG_INSTRLENGTH__MASK;
+}
+
+#define REG_A4XX_HLSQ_UPDATE_CONTROL				0x000023db
+
+#define REG_A4XX_PC_BINNING_COMMAND				0x00000d00
+#define A4XX_PC_BINNING_COMMAND_BINNING_ENABLE			0x00000001
+
+#define REG_A4XX_PC_DRAWCALL_SETUP_OVERRIDE			0x00000d0c
+
+#define REG_A4XX_PC_PERFCTR_PC_SEL_0				0x00000d10
+
+#define REG_A4XX_PC_PERFCTR_PC_SEL_7				0x00000d17
+
+#define REG_A4XX_PC_BIN_BASE					0x000021c0
+
+#define REG_A4XX_PC_PRIM_VTX_CNTL				0x000021c4
+#define A4XX_PC_PRIM_VTX_CNTL_VAROUT				0x00000001
+#define A4XX_PC_PRIM_VTX_CNTL_PROVOKING_VTX_LAST		0x02000000
+#define A4XX_PC_PRIM_VTX_CNTL_PSIZE				0x04000000
+
+#define REG_A4XX_UNKNOWN_21C5					0x000021c5
+
+#define REG_A4XX_PC_RESTART_INDEX				0x000021c6
+
+#define REG_A4XX_PC_GS_PARAM					0x000021e5
+
+#define REG_A4XX_PC_HS_PARAM					0x000021e7
+
+#define REG_A4XX_VBIF_VERSION					0x00003000
+
+#define REG_A4XX_VBIF_CLKON					0x00003001
+#define A4XX_VBIF_CLKON_FORCE_ON_TESTBUS			0x00000001
+
+#define REG_A4XX_VBIF_ABIT_SORT					0x0000301c
+
+#define REG_A4XX_VBIF_ABIT_SORT_CONF				0x0000301d
+
+#define REG_A4XX_VBIF_GATE_OFF_WRREQ_EN				0x0000302a
+
+#define REG_A4XX_VBIF_IN_RD_LIM_CONF0				0x0000302c
+
+#define REG_A4XX_VBIF_IN_RD_LIM_CONF1				0x0000302d
+
+#define REG_A4XX_VBIF_IN_WR_LIM_CONF0				0x00003030
+
+#define REG_A4XX_VBIF_IN_WR_LIM_CONF1				0x00003031
+
+#define REG_A4XX_VBIF_ROUND_ROBIN_QOS_ARB			0x00003049
+
+#define REG_A4XX_UNKNOWN_0CC5					0x00000cc5
+
+#define REG_A4XX_UNKNOWN_0CC6					0x00000cc6
+
+#define REG_A4XX_UNKNOWN_0D01					0x00000d01
+
+#define REG_A4XX_UNKNOWN_0E05					0x00000e05
+
+#define REG_A4XX_UNKNOWN_0E42					0x00000e42
+
+#define REG_A4XX_UNKNOWN_0EC2					0x00000ec2
+
+#define REG_A4XX_UNKNOWN_0EC3					0x00000ec3
+
+#define REG_A4XX_UNKNOWN_0F03					0x00000f03
+
+#define REG_A4XX_UNKNOWN_2001					0x00002001
+
+#define REG_A4XX_UNKNOWN_209B					0x0000209b
+
+#define REG_A4XX_UNKNOWN_20EF					0x000020ef
+
+#define REG_A4XX_UNKNOWN_20F0					0x000020f0
+
+#define REG_A4XX_UNKNOWN_20F1					0x000020f1
+
+#define REG_A4XX_UNKNOWN_20F2					0x000020f2
+
+#define REG_A4XX_UNKNOWN_20F3					0x000020f3
+
+#define REG_A4XX_UNKNOWN_20F4					0x000020f4
+
+#define REG_A4XX_UNKNOWN_20F5					0x000020f5
+
+#define REG_A4XX_UNKNOWN_20F6					0x000020f6
+
+#define REG_A4XX_UNKNOWN_20F7					0x000020f7
+
+#define REG_A4XX_UNKNOWN_2152					0x00002152
+
+#define REG_A4XX_UNKNOWN_2153					0x00002153
+
+#define REG_A4XX_UNKNOWN_2154					0x00002154
+
+#define REG_A4XX_UNKNOWN_2155					0x00002155
+
+#define REG_A4XX_UNKNOWN_2156					0x00002156
+
+#define REG_A4XX_UNKNOWN_2157					0x00002157
+
+#define REG_A4XX_UNKNOWN_21C3					0x000021c3
+
+#define REG_A4XX_UNKNOWN_21E6					0x000021e6
+
+#define REG_A4XX_UNKNOWN_2209					0x00002209
+
+#define REG_A4XX_UNKNOWN_22D7					0x000022d7
+
+#define REG_A4XX_UNKNOWN_2381					0x00002381
+
+#define REG_A4XX_UNKNOWN_23A0					0x000023a0
+
+#define REG_A4XX_TEX_SAMP_0					0x00000000
+#define A4XX_TEX_SAMP_0_XY_MAG__MASK				0x00000006
+#define A4XX_TEX_SAMP_0_XY_MAG__SHIFT				1
+static inline uint32_t A4XX_TEX_SAMP_0_XY_MAG(enum a4xx_tex_filter val)
+{
+	return ((val) << A4XX_TEX_SAMP_0_XY_MAG__SHIFT) & A4XX_TEX_SAMP_0_XY_MAG__MASK;
+}
+#define A4XX_TEX_SAMP_0_XY_MIN__MASK				0x00000018
+#define A4XX_TEX_SAMP_0_XY_MIN__SHIFT				3
+static inline uint32_t A4XX_TEX_SAMP_0_XY_MIN(enum a4xx_tex_filter val)
+{
+	return ((val) << A4XX_TEX_SAMP_0_XY_MIN__SHIFT) & A4XX_TEX_SAMP_0_XY_MIN__MASK;
+}
+#define A4XX_TEX_SAMP_0_WRAP_S__MASK				0x000000e0
+#define A4XX_TEX_SAMP_0_WRAP_S__SHIFT				5
+static inline uint32_t A4XX_TEX_SAMP_0_WRAP_S(enum a4xx_tex_clamp val)
+{
+	return ((val) << A4XX_TEX_SAMP_0_WRAP_S__SHIFT) & A4XX_TEX_SAMP_0_WRAP_S__MASK;
+}
+#define A4XX_TEX_SAMP_0_WRAP_T__MASK				0x00000700
+#define A4XX_TEX_SAMP_0_WRAP_T__SHIFT				8
+static inline uint32_t A4XX_TEX_SAMP_0_WRAP_T(enum a4xx_tex_clamp val)
+{
+	return ((val) << A4XX_TEX_SAMP_0_WRAP_T__SHIFT) & A4XX_TEX_SAMP_0_WRAP_T__MASK;
+}
+#define A4XX_TEX_SAMP_0_WRAP_R__MASK				0x00003800
+#define A4XX_TEX_SAMP_0_WRAP_R__SHIFT				11
+static inline uint32_t A4XX_TEX_SAMP_0_WRAP_R(enum a4xx_tex_clamp val)
+{
+	return ((val) << A4XX_TEX_SAMP_0_WRAP_R__SHIFT) & A4XX_TEX_SAMP_0_WRAP_R__MASK;
+}
+
+#define REG_A4XX_TEX_SAMP_1					0x00000001
+#define A4XX_TEX_SAMP_1_COMPARE_FUNC__MASK			0x0000000e
+#define A4XX_TEX_SAMP_1_COMPARE_FUNC__SHIFT			1
+static inline uint32_t A4XX_TEX_SAMP_1_COMPARE_FUNC(enum adreno_compare_func val)
+{
+	return ((val) << A4XX_TEX_SAMP_1_COMPARE_FUNC__SHIFT) & A4XX_TEX_SAMP_1_COMPARE_FUNC__MASK;
+}
+#define A4XX_TEX_SAMP_1_MAX_LOD__MASK				0x000fff00
+#define A4XX_TEX_SAMP_1_MAX_LOD__SHIFT				8
+static inline uint32_t A4XX_TEX_SAMP_1_MAX_LOD(float val)
+{
+	return ((((uint32_t)(val * 64.0))) << A4XX_TEX_SAMP_1_MAX_LOD__SHIFT) & A4XX_TEX_SAMP_1_MAX_LOD__MASK;
+}
+#define A4XX_TEX_SAMP_1_MIN_LOD__MASK				0xfff00000
+#define A4XX_TEX_SAMP_1_MIN_LOD__SHIFT				20
+static inline uint32_t A4XX_TEX_SAMP_1_MIN_LOD(float val)
+{
+	return ((((uint32_t)(val * 64.0))) << A4XX_TEX_SAMP_1_MIN_LOD__SHIFT) & A4XX_TEX_SAMP_1_MIN_LOD__MASK;
+}
+
+#define REG_A4XX_TEX_CONST_0					0x00000000
+#define A4XX_TEX_CONST_0_TILED					0x00000001
+#define A4XX_TEX_CONST_0_SWIZ_X__MASK				0x00000070
+#define A4XX_TEX_CONST_0_SWIZ_X__SHIFT				4
+static inline uint32_t A4XX_TEX_CONST_0_SWIZ_X(enum a4xx_tex_swiz val)
+{
+	return ((val) << A4XX_TEX_CONST_0_SWIZ_X__SHIFT) & A4XX_TEX_CONST_0_SWIZ_X__MASK;
+}
+#define A4XX_TEX_CONST_0_SWIZ_Y__MASK				0x00000380
+#define A4XX_TEX_CONST_0_SWIZ_Y__SHIFT				7
+static inline uint32_t A4XX_TEX_CONST_0_SWIZ_Y(enum a4xx_tex_swiz val)
+{
+	return ((val) << A4XX_TEX_CONST_0_SWIZ_Y__SHIFT) & A4XX_TEX_CONST_0_SWIZ_Y__MASK;
+}
+#define A4XX_TEX_CONST_0_SWIZ_Z__MASK				0x00001c00
+#define A4XX_TEX_CONST_0_SWIZ_Z__SHIFT				10
+static inline uint32_t A4XX_TEX_CONST_0_SWIZ_Z(enum a4xx_tex_swiz val)
+{
+	return ((val) << A4XX_TEX_CONST_0_SWIZ_Z__SHIFT) & A4XX_TEX_CONST_0_SWIZ_Z__MASK;
+}
+#define A4XX_TEX_CONST_0_SWIZ_W__MASK				0x0000e000
+#define A4XX_TEX_CONST_0_SWIZ_W__SHIFT				13
+static inline uint32_t A4XX_TEX_CONST_0_SWIZ_W(enum a4xx_tex_swiz val)
+{
+	return ((val) << A4XX_TEX_CONST_0_SWIZ_W__SHIFT) & A4XX_TEX_CONST_0_SWIZ_W__MASK;
+}
+#define A4XX_TEX_CONST_0_FMT__MASK				0x1fc00000
+#define A4XX_TEX_CONST_0_FMT__SHIFT				22
+static inline uint32_t A4XX_TEX_CONST_0_FMT(enum a4xx_tex_fmt val)
+{
+	return ((val) << A4XX_TEX_CONST_0_FMT__SHIFT) & A4XX_TEX_CONST_0_FMT__MASK;
+}
+#define A4XX_TEX_CONST_0_TYPE__MASK				0x60000000
+#define A4XX_TEX_CONST_0_TYPE__SHIFT				29
+static inline uint32_t A4XX_TEX_CONST_0_TYPE(enum a4xx_tex_type val)
+{
+	return ((val) << A4XX_TEX_CONST_0_TYPE__SHIFT) & A4XX_TEX_CONST_0_TYPE__MASK;
+}
+
+#define REG_A4XX_TEX_CONST_1					0x00000001
+#define A4XX_TEX_CONST_1_HEIGHT__MASK				0x00007fff
+#define A4XX_TEX_CONST_1_HEIGHT__SHIFT				0
+static inline uint32_t A4XX_TEX_CONST_1_HEIGHT(uint32_t val)
+{
+	return ((val) << A4XX_TEX_CONST_1_HEIGHT__SHIFT) & A4XX_TEX_CONST_1_HEIGHT__MASK;
+}
+#define A4XX_TEX_CONST_1_WIDTH__MASK				0x1fff8000
+#define A4XX_TEX_CONST_1_WIDTH__SHIFT				15
+static inline uint32_t A4XX_TEX_CONST_1_WIDTH(uint32_t val)
+{
+	return ((val) << A4XX_TEX_CONST_1_WIDTH__SHIFT) & A4XX_TEX_CONST_1_WIDTH__MASK;
+}
+
+#define REG_A4XX_TEX_CONST_2					0x00000002
+#define A4XX_TEX_CONST_2_PITCH__MASK				0x3ffffe00
+#define A4XX_TEX_CONST_2_PITCH__SHIFT				9
+static inline uint32_t A4XX_TEX_CONST_2_PITCH(uint32_t val)
+{
+	return ((val) << A4XX_TEX_CONST_2_PITCH__SHIFT) & A4XX_TEX_CONST_2_PITCH__MASK;
+}
+#define A4XX_TEX_CONST_2_SWAP__MASK				0xc0000000
+#define A4XX_TEX_CONST_2_SWAP__SHIFT				30
+static inline uint32_t A4XX_TEX_CONST_2_SWAP(enum a3xx_color_swap val)
+{
+	return ((val) << A4XX_TEX_CONST_2_SWAP__SHIFT) & A4XX_TEX_CONST_2_SWAP__MASK;
+}
+
+#define REG_A4XX_TEX_CONST_3					0x00000003
+#define A4XX_TEX_CONST_3_LAYERSZ__MASK				0x0000000f
+#define A4XX_TEX_CONST_3_LAYERSZ__SHIFT				0
+static inline uint32_t A4XX_TEX_CONST_3_LAYERSZ(uint32_t val)
+{
+	return ((val >> 12) << A4XX_TEX_CONST_3_LAYERSZ__SHIFT) & A4XX_TEX_CONST_3_LAYERSZ__MASK;
+}
+
+#define REG_A4XX_TEX_CONST_4					0x00000004
+#define A4XX_TEX_CONST_4_BASE__MASK				0xffffffff
+#define A4XX_TEX_CONST_4_BASE__SHIFT				0
+static inline uint32_t A4XX_TEX_CONST_4_BASE(uint32_t val)
+{
+	return ((val) << A4XX_TEX_CONST_4_BASE__SHIFT) & A4XX_TEX_CONST_4_BASE__MASK;
+}
+
+#define REG_A4XX_TEX_CONST_5					0x00000005
+
+#define REG_A4XX_TEX_CONST_6					0x00000006
+
+#define REG_A4XX_TEX_CONST_7					0x00000007
+
+
+#endif /* A4XX_XML */
diff --git a/drivers/gpu/drm/msm/adreno/a4xx_gpu.c b/drivers/gpu/drm/msm/adreno/a4xx_gpu.c
new file mode 100644
index 000000000000..91221836c5ad
--- /dev/null
+++ b/drivers/gpu/drm/msm/adreno/a4xx_gpu.c
@@ -0,0 +1,604 @@
+/* Copyright (c) 2014 The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include "a4xx_gpu.h"
+#ifdef CONFIG_MSM_OCMEM
+#  include <soc/qcom/ocmem.h>
+#endif
+
+#define A4XX_INT0_MASK \
+	(A4XX_INT0_RBBM_AHB_ERROR |        \
+	 A4XX_INT0_RBBM_ATB_BUS_OVERFLOW | \
+	 A4XX_INT0_CP_T0_PACKET_IN_IB |    \
+	 A4XX_INT0_CP_OPCODE_ERROR |       \
+	 A4XX_INT0_CP_RESERVED_BIT_ERROR | \
+	 A4XX_INT0_CP_HW_FAULT |           \
+	 A4XX_INT0_CP_IB1_INT |            \
+	 A4XX_INT0_CP_IB2_INT |            \
+	 A4XX_INT0_CP_RB_INT |             \
+	 A4XX_INT0_CP_REG_PROTECT_FAULT |  \
+	 A4XX_INT0_CP_AHB_ERROR_HALT |     \
+	 A4XX_INT0_UCHE_OOB_ACCESS)
+
+extern bool hang_debug;
+static void a4xx_dump(struct msm_gpu *gpu);
+
+/*
+ * a4xx_enable_hwcg() - Program the clock control registers
+ * @device: The adreno device pointer
+ */
+static void a4xx_enable_hwcg(struct msm_gpu *gpu)
+{
+	struct adreno_gpu *adreno_gpu = to_adreno_gpu(gpu);
+	unsigned int i;
+	for (i = 0; i < 4; i++)
+		gpu_write(gpu, REG_A4XX_RBBM_CLOCK_CTL_TP(i), 0x02222202);
+	for (i = 0; i < 4; i++)
+		gpu_write(gpu, REG_A4XX_RBBM_CLOCK_CTL2_TP(i), 0x00002222);
+	for (i = 0; i < 4; i++)
+		gpu_write(gpu, REG_A4XX_RBBM_CLOCK_HYST_TP(i), 0x0E739CE7);
+	for (i = 0; i < 4; i++)
+		gpu_write(gpu, REG_A4XX_RBBM_CLOCK_DELAY_TP(i), 0x00111111);
+	for (i = 0; i < 4; i++)
+		gpu_write(gpu, REG_A4XX_RBBM_CLOCK_CTL_SP(i), 0x22222222);
+	for (i = 0; i < 4; i++)
+		gpu_write(gpu, REG_A4XX_RBBM_CLOCK_CTL2_SP(i), 0x00222222);
+	for (i = 0; i < 4; i++)
+		gpu_write(gpu, REG_A4XX_RBBM_CLOCK_HYST_SP(i), 0x00000104);
+	for (i = 0; i < 4; i++)
+		gpu_write(gpu, REG_A4XX_RBBM_CLOCK_DELAY_SP(i), 0x00000081);
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_CTL_UCHE, 0x22222222);
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_CTL2_UCHE, 0x02222222);
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_CTL3_UCHE, 0x00000000);
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_CTL4_UCHE, 0x00000000);
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_HYST_UCHE, 0x00004444);
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_DELAY_UCHE, 0x00001112);
+	for (i = 0; i < 4; i++)
+		gpu_write(gpu, REG_A4XX_RBBM_CLOCK_CTL_RB(i), 0x22222222);
+
+	/* Disable L1 clocking in A420 due to CCU issues with it */
+	for (i = 0; i < 4; i++) {
+		if (adreno_is_a420(adreno_gpu)) {
+			gpu_write(gpu, REG_A4XX_RBBM_CLOCK_CTL2_RB(i),
+					0x00002020);
+		} else {
+			gpu_write(gpu, REG_A4XX_RBBM_CLOCK_CTL2_RB(i),
+					0x00022020);
+		}
+	}
+
+	for (i = 0; i < 4; i++) {
+		gpu_write(gpu, REG_A4XX_RBBM_CLOCK_CTL_MARB_CCU(i),
+				0x00000922);
+	}
+
+	for (i = 0; i < 4; i++) {
+		gpu_write(gpu, REG_A4XX_RBBM_CLOCK_HYST_RB_MARB_CCU(i),
+				0x00000000);
+	}
+
+	for (i = 0; i < 4; i++) {
+		gpu_write(gpu, REG_A4XX_RBBM_CLOCK_DELAY_RB_MARB_CCU_L1(i),
+				0x00000001);
+	}
+
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_MODE_GPC, 0x02222222);
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_HYST_GPC, 0x04100104);
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_DELAY_GPC, 0x00022222);
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_CTL_COM_DCOM, 0x00000022);
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_HYST_COM_DCOM, 0x0000010F);
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_DELAY_COM_DCOM, 0x00000022);
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_CTL_TSE_RAS_RBBM, 0x00222222);
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_HYST_TSE_RAS_RBBM, 0x00004104);
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_DELAY_TSE_RAS_RBBM, 0x00000222);
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_CTL_HLSQ , 0x00000000);
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_HYST_HLSQ, 0x00000000);
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_DELAY_HLSQ, 0x00020000);
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_CTL, 0xAAAAAAAA);
+	gpu_write(gpu, REG_A4XX_RBBM_CLOCK_CTL2, 0);
+}
+
+static void a4xx_me_init(struct msm_gpu *gpu)
+{
+	struct msm_ringbuffer *ring = gpu->rb;
+
+	OUT_PKT3(ring, CP_ME_INIT, 17);
+	OUT_RING(ring, 0x000003f7);
+	OUT_RING(ring, 0x00000000);
+	OUT_RING(ring, 0x00000000);
+	OUT_RING(ring, 0x00000000);
+	OUT_RING(ring, 0x00000080);
+	OUT_RING(ring, 0x00000100);
+	OUT_RING(ring, 0x00000180);
+	OUT_RING(ring, 0x00006600);
+	OUT_RING(ring, 0x00000150);
+	OUT_RING(ring, 0x0000014e);
+	OUT_RING(ring, 0x00000154);
+	OUT_RING(ring, 0x00000001);
+	OUT_RING(ring, 0x00000000);
+	OUT_RING(ring, 0x00000000);
+	OUT_RING(ring, 0x00000000);
+	OUT_RING(ring, 0x00000000);
+	OUT_RING(ring, 0x00000000);
+
+	gpu->funcs->flush(gpu);
+	gpu->funcs->idle(gpu);
+}
+
+static int a4xx_hw_init(struct msm_gpu *gpu)
+{
+	struct adreno_gpu *adreno_gpu = to_adreno_gpu(gpu);
+	struct a4xx_gpu *a4xx_gpu = to_a4xx_gpu(adreno_gpu);
+	uint32_t *ptr, len;
+	int i, ret;
+
+	if (adreno_is_a4xx(adreno_gpu)) {
+		gpu_write(gpu, REG_A4XX_VBIF_ABIT_SORT, 0x0001001F);
+		gpu_write(gpu, REG_A4XX_VBIF_ABIT_SORT_CONF, 0x000000A4);
+		gpu_write(gpu, REG_A4XX_VBIF_GATE_OFF_WRREQ_EN, 0x00000001);
+		gpu_write(gpu, REG_A4XX_VBIF_IN_RD_LIM_CONF0, 0x18181818);
+		gpu_write(gpu, REG_A4XX_VBIF_IN_RD_LIM_CONF1, 0x00000018);
+		gpu_write(gpu, REG_A4XX_VBIF_IN_WR_LIM_CONF0, 0x18181818);
+		gpu_write(gpu, REG_A4XX_VBIF_IN_WR_LIM_CONF1, 0x00000018);
+		gpu_write(gpu, REG_A4XX_VBIF_ROUND_ROBIN_QOS_ARB, 0x00000003);
+	} else {
+		BUG();
+	}
+
+	/* Make all blocks contribute to the GPU BUSY perf counter */
+	gpu_write(gpu, REG_A4XX_RBBM_GPU_BUSY_MASKED, 0xffffffff);
+
+	/* Tune the hystersis counters for SP and CP idle detection */
+	gpu_write(gpu, REG_A4XX_RBBM_SP_HYST_CNT, 0x10);
+	gpu_write(gpu, REG_A4XX_RBBM_WAIT_IDLE_CLOCKS_CTL, 0x10);
+
+	 /* Enable the RBBM error reporting bits */
+	gpu_write(gpu, REG_A4XX_RBBM_AHB_CTL0, 0x00000001);
+
+	/* Enable AHB error reporting*/
+	gpu_write(gpu, REG_A4XX_RBBM_AHB_CTL1, 0xa6ffffff);
+
+	/* Enable power counters*/
+	gpu_write(gpu, REG_A4XX_RBBM_RBBM_CTL, 0x00000030);
+
+	/*
+	 * Turn on hang detection - this spews a lot of useful information
+	 * into the RBBM registers on a hang:
+	 */
+	gpu_write(gpu, REG_A4XX_RBBM_INTERFACE_HANG_INT_CTL,
+			(1 << 30) | 0xFFFF);
+
+	gpu_write(gpu, REG_A4XX_RB_GMEM_BASE_ADDR,
+			(unsigned int)(a4xx_gpu->ocmem_base >> 14));
+
+	/* Turn on performance counters: */
+	gpu_write(gpu, REG_A4XX_RBBM_PERFCTR_CTL, 0x01);
+
+	/* Disable L2 bypass to avoid UCHE out of bounds errors */
+	gpu_write(gpu, REG_A4XX_UCHE_TRAP_BASE_LO, 0xffff0000);
+	gpu_write(gpu, REG_A4XX_UCHE_TRAP_BASE_HI, 0xffff0000);
+
+	gpu_write(gpu, REG_A4XX_CP_DEBUG, (1 << 25) |
+			(adreno_is_a420(adreno_gpu) ? (1 << 29) : 0));
+
+	a4xx_enable_hwcg(gpu);
+
+	/*
+	 * For A420 set RBBM_CLOCK_DELAY_HLSQ.CGC_HLSQ_TP_EARLY_CYC >= 2
+	 * due to timing issue with HLSQ_TP_CLK_EN
+	 */
+	if (adreno_is_a420(adreno_gpu)) {
+		unsigned int val;
+		val = gpu_read(gpu, REG_A4XX_RBBM_CLOCK_DELAY_HLSQ);
+		val &= ~A4XX_CGC_HLSQ_EARLY_CYC__MASK;
+		val |= 2 << A4XX_CGC_HLSQ_EARLY_CYC__SHIFT;
+		gpu_write(gpu, REG_A4XX_RBBM_CLOCK_DELAY_HLSQ, val);
+	}
+
+	ret = adreno_hw_init(gpu);
+	if (ret)
+		return ret;
+
+	/* setup access protection: */
+	gpu_write(gpu, REG_A4XX_CP_PROTECT_CTRL, 0x00000007);
+
+	/* RBBM registers */
+	gpu_write(gpu, REG_A4XX_CP_PROTECT(0), 0x62000010);
+	gpu_write(gpu, REG_A4XX_CP_PROTECT(1), 0x63000020);
+	gpu_write(gpu, REG_A4XX_CP_PROTECT(2), 0x64000040);
+	gpu_write(gpu, REG_A4XX_CP_PROTECT(3), 0x65000080);
+	gpu_write(gpu, REG_A4XX_CP_PROTECT(4), 0x66000100);
+	gpu_write(gpu, REG_A4XX_CP_PROTECT(5), 0x64000200);
+
+	/* CP registers */
+	gpu_write(gpu, REG_A4XX_CP_PROTECT(6), 0x67000800);
+	gpu_write(gpu, REG_A4XX_CP_PROTECT(7), 0x64001600);
+
+
+	/* RB registers */
+	gpu_write(gpu, REG_A4XX_CP_PROTECT(8), 0x60003300);
+
+	/* HLSQ registers */
+	gpu_write(gpu, REG_A4XX_CP_PROTECT(9), 0x60003800);
+
+	/* VPC registers */
+	gpu_write(gpu, REG_A4XX_CP_PROTECT(10), 0x61003980);
+
+	/* SMMU registers */
+	gpu_write(gpu, REG_A4XX_CP_PROTECT(11), 0x6e010000);
+
+	gpu_write(gpu, REG_A4XX_RBBM_INT_0_MASK, A4XX_INT0_MASK);
+
+	ret = adreno_hw_init(gpu);
+	if (ret)
+		return ret;
+
+	/* Load PM4: */
+	ptr = (uint32_t *)(adreno_gpu->pm4->data);
+	len = adreno_gpu->pm4->size / 4;
+	DBG("loading PM4 ucode version: %u", ptr[0]);
+	gpu_write(gpu, REG_A4XX_CP_ME_RAM_WADDR, 0);
+	for (i = 1; i < len; i++)
+		gpu_write(gpu, REG_A4XX_CP_ME_RAM_DATA, ptr[i]);
+
+	/* Load PFP: */
+	ptr = (uint32_t *)(adreno_gpu->pfp->data);
+	len = adreno_gpu->pfp->size / 4;
+	DBG("loading PFP ucode version: %u", ptr[0]);
+
+	gpu_write(gpu, REG_A4XX_CP_PFP_UCODE_ADDR, 0);
+	for (i = 1; i < len; i++)
+		gpu_write(gpu, REG_A4XX_CP_PFP_UCODE_DATA, ptr[i]);
+
+	/* clear ME_HALT to start micro engine */
+	gpu_write(gpu, REG_A4XX_CP_ME_CNTL, 0);
+
+	a4xx_me_init(gpu);
+	return 0;
+}
+
+static void a4xx_recover(struct msm_gpu *gpu)
+{
+	/* dump registers before resetting gpu, if enabled: */
+	if (hang_debug)
+		a4xx_dump(gpu);
+
+	gpu_write(gpu, REG_A4XX_RBBM_SW_RESET_CMD, 1);
+	gpu_read(gpu, REG_A4XX_RBBM_SW_RESET_CMD);
+	gpu_write(gpu, REG_A4XX_RBBM_SW_RESET_CMD, 0);
+	adreno_recover(gpu);
+}
+
+static void a4xx_destroy(struct msm_gpu *gpu)
+{
+	struct adreno_gpu *adreno_gpu = to_adreno_gpu(gpu);
+	struct a4xx_gpu *a4xx_gpu = to_a4xx_gpu(adreno_gpu);
+
+	DBG("%s", gpu->name);
+
+	adreno_gpu_cleanup(adreno_gpu);
+
+#ifdef CONFIG_MSM_OCMEM
+	if (a4xx_gpu->ocmem_base)
+		ocmem_free(OCMEM_GRAPHICS, a4xx_gpu->ocmem_hdl);
+#endif
+
+	kfree(a4xx_gpu);
+}
+
+static void a4xx_idle(struct msm_gpu *gpu)
+{
+	/* wait for ringbuffer to drain: */
+	adreno_idle(gpu);
+
+	/* then wait for GPU to finish: */
+	if (spin_until(!(gpu_read(gpu, REG_A4XX_RBBM_STATUS) &
+					A4XX_RBBM_STATUS_GPU_BUSY)))
+		DRM_ERROR("%s: timeout waiting for GPU to idle!\n", gpu->name);
+
+	/* TODO maybe we need to reset GPU here to recover from hang? */
+}
+
+static irqreturn_t a4xx_irq(struct msm_gpu *gpu)
+{
+	uint32_t status;
+
+	status = gpu_read(gpu, REG_A4XX_RBBM_INT_0_STATUS);
+	DBG("%s: Int status %08x", gpu->name, status);
+
+	gpu_write(gpu, REG_A4XX_RBBM_INT_CLEAR_CMD, status);
+
+	msm_gpu_retire(gpu);
+
+	return IRQ_HANDLED;
+}
+
+static const unsigned int a4xx_registers[] = {
+	/* RBBM */
+	0x0000, 0x0002, 0x0004, 0x0021, 0x0023, 0x0024, 0x0026, 0x0026,
+	0x0028, 0x002B, 0x002E, 0x0034, 0x0037, 0x0044, 0x0047, 0x0066,
+	0x0068, 0x0095, 0x009C, 0x0170, 0x0174, 0x01AF,
+	/* CP */
+	0x0200, 0x0233, 0x0240, 0x0250, 0x04C0, 0x04DD, 0x0500, 0x050B,
+	0x0578, 0x058F,
+	/* VSC */
+	0x0C00, 0x0C03, 0x0C08, 0x0C41, 0x0C50, 0x0C51,
+	/* GRAS */
+	0x0C80, 0x0C81, 0x0C88, 0x0C8F,
+	/* RB */
+	0x0CC0, 0x0CC0, 0x0CC4, 0x0CD2,
+	/* PC */
+	0x0D00, 0x0D0C, 0x0D10, 0x0D17, 0x0D20, 0x0D23,
+	/* VFD */
+	0x0E40, 0x0E4A,
+	/* VPC */
+	0x0E60, 0x0E61, 0x0E63, 0x0E68,
+	/* UCHE */
+	0x0E80, 0x0E84, 0x0E88, 0x0E95,
+	/* VMIDMT */
+	0x1000, 0x1000, 0x1002, 0x1002, 0x1004, 0x1004, 0x1008, 0x100A,
+	0x100C, 0x100D, 0x100F, 0x1010, 0x1012, 0x1016, 0x1024, 0x1024,
+	0x1027, 0x1027, 0x1100, 0x1100, 0x1102, 0x1102, 0x1104, 0x1104,
+	0x1110, 0x1110, 0x1112, 0x1116, 0x1124, 0x1124, 0x1300, 0x1300,
+	0x1380, 0x1380,
+	/* GRAS CTX 0 */
+	0x2000, 0x2004, 0x2008, 0x2067, 0x2070, 0x2078, 0x207B, 0x216E,
+	/* PC CTX 0 */
+	0x21C0, 0x21C6, 0x21D0, 0x21D0, 0x21D9, 0x21D9, 0x21E5, 0x21E7,
+	/* VFD CTX 0 */
+	0x2200, 0x2204, 0x2208, 0x22A9,
+	/* GRAS CTX 1 */
+	0x2400, 0x2404, 0x2408, 0x2467, 0x2470, 0x2478, 0x247B, 0x256E,
+	/* PC CTX 1 */
+	0x25C0, 0x25C6, 0x25D0, 0x25D0, 0x25D9, 0x25D9, 0x25E5, 0x25E7,
+	/* VFD CTX 1 */
+	0x2600, 0x2604, 0x2608, 0x26A9,
+	/* XPU */
+	0x2C00, 0x2C01, 0x2C10, 0x2C10, 0x2C12, 0x2C16, 0x2C1D, 0x2C20,
+	0x2C28, 0x2C28, 0x2C30, 0x2C30, 0x2C32, 0x2C36, 0x2C40, 0x2C40,
+	0x2C50, 0x2C50, 0x2C52, 0x2C56, 0x2C80, 0x2C80, 0x2C94, 0x2C95,
+	/* VBIF */
+	0x3000, 0x3007, 0x300C, 0x3014, 0x3018, 0x301D, 0x3020, 0x3022,
+	0x3024, 0x3026, 0x3028, 0x302A, 0x302C, 0x302D, 0x3030, 0x3031,
+	0x3034, 0x3036, 0x3038, 0x3038, 0x303C, 0x303D, 0x3040, 0x3040,
+	0x3049, 0x3049, 0x3058, 0x3058, 0x305B, 0x3061, 0x3064, 0x3068,
+	0x306C, 0x306D, 0x3080, 0x3088, 0x308B, 0x308C, 0x3090, 0x3094,
+	0x3098, 0x3098, 0x309C, 0x309C, 0x30C0, 0x30C0, 0x30C8, 0x30C8,
+	0x30D0, 0x30D0, 0x30D8, 0x30D8, 0x30E0, 0x30E0, 0x3100, 0x3100,
+	0x3108, 0x3108, 0x3110, 0x3110, 0x3118, 0x3118, 0x3120, 0x3120,
+	0x3124, 0x3125, 0x3129, 0x3129, 0x3131, 0x3131, 0x330C, 0x330C,
+	0x3310, 0x3310, 0x3400, 0x3401, 0x3410, 0x3410, 0x3412, 0x3416,
+	0x341D, 0x3420, 0x3428, 0x3428, 0x3430, 0x3430, 0x3432, 0x3436,
+	0x3440, 0x3440, 0x3450, 0x3450, 0x3452, 0x3456, 0x3480, 0x3480,
+	0x3494, 0x3495, 0x4000, 0x4000, 0x4002, 0x4002, 0x4004, 0x4004,
+	0x4008, 0x400A, 0x400C, 0x400D, 0x400F, 0x4012, 0x4014, 0x4016,
+	0x401D, 0x401D, 0x4020, 0x4027, 0x4060, 0x4062, 0x4200, 0x4200,
+	0x4300, 0x4300, 0x4400, 0x4400, 0x4500, 0x4500, 0x4800, 0x4802,
+	0x480F, 0x480F, 0x4811, 0x4811, 0x4813, 0x4813, 0x4815, 0x4816,
+	0x482B, 0x482B, 0x4857, 0x4857, 0x4883, 0x4883, 0x48AF, 0x48AF,
+	0x48C5, 0x48C5, 0x48E5, 0x48E5, 0x4905, 0x4905, 0x4925, 0x4925,
+	0x4945, 0x4945, 0x4950, 0x4950, 0x495B, 0x495B, 0x4980, 0x498E,
+	0x4B00, 0x4B00, 0x4C00, 0x4C00, 0x4D00, 0x4D00, 0x4E00, 0x4E00,
+	0x4E80, 0x4E80, 0x4F00, 0x4F00, 0x4F08, 0x4F08, 0x4F10, 0x4F10,
+	0x4F18, 0x4F18, 0x4F20, 0x4F20, 0x4F30, 0x4F30, 0x4F60, 0x4F60,
+	0x4F80, 0x4F81, 0x4F88, 0x4F89, 0x4FEE, 0x4FEE, 0x4FF3, 0x4FF3,
+	0x6000, 0x6001, 0x6008, 0x600F, 0x6014, 0x6016, 0x6018, 0x601B,
+	0x61FD, 0x61FD, 0x623C, 0x623C, 0x6380, 0x6380, 0x63A0, 0x63A0,
+	0x63C0, 0x63C1, 0x63C8, 0x63C9, 0x63D0, 0x63D4, 0x63D6, 0x63D6,
+	0x63EE, 0x63EE, 0x6400, 0x6401, 0x6408, 0x640F, 0x6414, 0x6416,
+	0x6418, 0x641B, 0x65FD, 0x65FD, 0x663C, 0x663C, 0x6780, 0x6780,
+	0x67A0, 0x67A0, 0x67C0, 0x67C1, 0x67C8, 0x67C9, 0x67D0, 0x67D4,
+	0x67D6, 0x67D6, 0x67EE, 0x67EE, 0x6800, 0x6801, 0x6808, 0x680F,
+	0x6814, 0x6816, 0x6818, 0x681B, 0x69FD, 0x69FD, 0x6A3C, 0x6A3C,
+	0x6B80, 0x6B80, 0x6BA0, 0x6BA0, 0x6BC0, 0x6BC1, 0x6BC8, 0x6BC9,
+	0x6BD0, 0x6BD4, 0x6BD6, 0x6BD6, 0x6BEE, 0x6BEE,
+	~0 /* sentinel */
+};
+
+#ifdef CONFIG_DEBUG_FS
+static void a4xx_show(struct msm_gpu *gpu, struct seq_file *m)
+{
+	gpu->funcs->pm_resume(gpu);
+
+	seq_printf(m, "status:   %08x\n",
+			gpu_read(gpu, REG_A4XX_RBBM_STATUS));
+	gpu->funcs->pm_suspend(gpu);
+
+	adreno_show(gpu, m);
+
+}
+#endif
+
+/* Register offset defines for A4XX, in order of enum adreno_regs */
+static const unsigned int a4xx_register_offsets[REG_ADRENO_REGISTER_MAX] = {
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_DEBUG, REG_A4XX_CP_DEBUG),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_ME_RAM_WADDR, REG_A4XX_CP_ME_RAM_WADDR),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_ME_RAM_DATA, REG_A4XX_CP_ME_RAM_DATA),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_PFP_UCODE_DATA,
+			REG_A4XX_CP_PFP_UCODE_DATA),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_PFP_UCODE_ADDR,
+			REG_A4XX_CP_PFP_UCODE_ADDR),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_WFI_PEND_CTR, REG_A4XX_CP_WFI_PEND_CTR),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_RB_BASE, REG_A4XX_CP_RB_BASE),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_RB_RPTR_ADDR, REG_A4XX_CP_RB_RPTR_ADDR),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_RB_RPTR, REG_A4XX_CP_RB_RPTR),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_RB_WPTR, REG_A4XX_CP_RB_WPTR),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_PROTECT_CTRL, REG_A4XX_CP_PROTECT_CTRL),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_ME_CNTL, REG_A4XX_CP_ME_CNTL),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_RB_CNTL, REG_A4XX_CP_RB_CNTL),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_IB1_BASE, REG_A4XX_CP_IB1_BASE),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_IB1_BUFSZ, REG_A4XX_CP_IB1_BUFSZ),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_IB2_BASE, REG_A4XX_CP_IB2_BASE),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_IB2_BUFSZ, REG_A4XX_CP_IB2_BUFSZ),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_TIMESTAMP, REG_AXXX_CP_SCRATCH_REG0),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_ME_RAM_RADDR, REG_A4XX_CP_ME_RAM_RADDR),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_ROQ_ADDR, REG_A4XX_CP_ROQ_ADDR),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_ROQ_DATA, REG_A4XX_CP_ROQ_DATA),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_MERCIU_ADDR, REG_A4XX_CP_MERCIU_ADDR),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_MERCIU_DATA, REG_A4XX_CP_MERCIU_DATA),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_MERCIU_DATA2, REG_A4XX_CP_MERCIU_DATA2),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_MEQ_ADDR, REG_A4XX_CP_MEQ_ADDR),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_MEQ_DATA, REG_A4XX_CP_MEQ_DATA),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_HW_FAULT, REG_A4XX_CP_HW_FAULT),
+	REG_ADRENO_DEFINE(REG_ADRENO_CP_PROTECT_STATUS,
+			REG_A4XX_CP_PROTECT_STATUS),
+	REG_ADRENO_DEFINE(REG_ADRENO_SCRATCH_ADDR, REG_A4XX_CP_SCRATCH_ADDR),
+	REG_ADRENO_DEFINE(REG_ADRENO_SCRATCH_UMSK, REG_A4XX_CP_SCRATCH_UMASK),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_STATUS, REG_A4XX_RBBM_STATUS),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_PERFCTR_CTL,
+			REG_A4XX_RBBM_PERFCTR_CTL),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_PERFCTR_LOAD_CMD0,
+			REG_A4XX_RBBM_PERFCTR_LOAD_CMD0),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_PERFCTR_LOAD_CMD1,
+			REG_A4XX_RBBM_PERFCTR_LOAD_CMD1),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_PERFCTR_LOAD_CMD2,
+			REG_A4XX_RBBM_PERFCTR_LOAD_CMD2),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_PERFCTR_PWR_1_LO,
+			REG_A4XX_RBBM_PERFCTR_PWR_1_LO),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_INT_0_MASK, REG_A4XX_RBBM_INT_0_MASK),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_INT_0_STATUS,
+			REG_A4XX_RBBM_INT_0_STATUS),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_AHB_ERROR_STATUS,
+			REG_A4XX_RBBM_AHB_ERROR_STATUS),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_AHB_CMD, REG_A4XX_RBBM_AHB_CMD),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_CLOCK_CTL, REG_A4XX_RBBM_CLOCK_CTL),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_AHB_ME_SPLIT_STATUS,
+			REG_A4XX_RBBM_AHB_ME_SPLIT_STATUS),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_AHB_PFP_SPLIT_STATUS,
+			REG_A4XX_RBBM_AHB_PFP_SPLIT_STATUS),
+	REG_ADRENO_DEFINE(REG_ADRENO_VPC_DEBUG_RAM_SEL,
+			REG_A4XX_VPC_DEBUG_RAM_SEL),
+	REG_ADRENO_DEFINE(REG_ADRENO_VPC_DEBUG_RAM_READ,
+			REG_A4XX_VPC_DEBUG_RAM_READ),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_INT_CLEAR_CMD,
+			REG_A4XX_RBBM_INT_CLEAR_CMD),
+	REG_ADRENO_DEFINE(REG_ADRENO_VSC_SIZE_ADDRESS,
+			REG_A4XX_VSC_SIZE_ADDRESS),
+	REG_ADRENO_DEFINE(REG_ADRENO_VFD_CONTROL_0, REG_A4XX_VFD_CONTROL_0),
+	REG_ADRENO_DEFINE(REG_ADRENO_SP_VS_PVT_MEM_ADDR_REG,
+			REG_A4XX_SP_VS_PVT_MEM_ADDR),
+	REG_ADRENO_DEFINE(REG_ADRENO_SP_FS_PVT_MEM_ADDR_REG,
+			REG_A4XX_SP_FS_PVT_MEM_ADDR),
+	REG_ADRENO_DEFINE(REG_ADRENO_SP_VS_OBJ_START_REG,
+			REG_A4XX_SP_VS_OBJ_START),
+	REG_ADRENO_DEFINE(REG_ADRENO_SP_FS_OBJ_START_REG,
+			REG_A4XX_SP_FS_OBJ_START),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_RBBM_CTL, REG_A4XX_RBBM_RBBM_CTL),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_SW_RESET_CMD,
+			REG_A4XX_RBBM_SW_RESET_CMD),
+	REG_ADRENO_DEFINE(REG_ADRENO_UCHE_INVALIDATE0,
+			REG_A4XX_UCHE_INVALIDATE0),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_PERFCTR_LOAD_VALUE_LO,
+			REG_A4XX_RBBM_PERFCTR_LOAD_VALUE_LO),
+	REG_ADRENO_DEFINE(REG_ADRENO_RBBM_PERFCTR_LOAD_VALUE_HI,
+			REG_A4XX_RBBM_PERFCTR_LOAD_VALUE_HI),
+};
+
+static void a4xx_dump(struct msm_gpu *gpu)
+{
+	adreno_dump(gpu);
+	printk("status:   %08x\n",
+			gpu_read(gpu, REG_A4XX_RBBM_STATUS));
+	adreno_dump(gpu);
+}
+
+static const struct adreno_gpu_funcs funcs = {
+	.base = {
+		.get_param = adreno_get_param,
+		.hw_init = a4xx_hw_init,
+		.pm_suspend = msm_gpu_pm_suspend,
+		.pm_resume = msm_gpu_pm_resume,
+		.recover = a4xx_recover,
+		.last_fence = adreno_last_fence,
+		.submit = adreno_submit,
+		.flush = adreno_flush,
+		.idle = a4xx_idle,
+		.irq = a4xx_irq,
+		.destroy = a4xx_destroy,
+#ifdef CONFIG_DEBUG_FS
+		.show = a4xx_show,
+#endif
+	},
+};
+
+struct msm_gpu *a4xx_gpu_init(struct drm_device *dev)
+{
+	struct a4xx_gpu *a4xx_gpu = NULL;
+	struct adreno_gpu *adreno_gpu;
+	struct msm_gpu *gpu;
+	struct msm_drm_private *priv = dev->dev_private;
+	struct platform_device *pdev = priv->gpu_pdev;
+	int ret;
+
+	if (!pdev) {
+		dev_err(dev->dev, "no a4xx device\n");
+		ret = -ENXIO;
+		goto fail;
+	}
+
+	a4xx_gpu = kzalloc(sizeof(*a4xx_gpu), GFP_KERNEL);
+	if (!a4xx_gpu) {
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	adreno_gpu = &a4xx_gpu->base;
+	gpu = &adreno_gpu->base;
+
+	a4xx_gpu->pdev = pdev;
+
+	gpu->perfcntrs = NULL;
+	gpu->num_perfcntrs = 0;
+
+	adreno_gpu->registers = a4xx_registers;
+	adreno_gpu->reg_offsets = a4xx_register_offsets;
+
+	ret = adreno_gpu_init(dev, pdev, adreno_gpu, &funcs);
+	if (ret)
+		goto fail;
+
+	/* if needed, allocate gmem: */
+	if (adreno_is_a4xx(adreno_gpu)) {
+#ifdef CONFIG_MSM_OCMEM
+		/* TODO this is different/missing upstream: */
+		struct ocmem_buf *ocmem_hdl =
+				ocmem_allocate(OCMEM_GRAPHICS, adreno_gpu->gmem);
+
+		a4xx_gpu->ocmem_hdl = ocmem_hdl;
+		a4xx_gpu->ocmem_base = ocmem_hdl->addr;
+		adreno_gpu->gmem = ocmem_hdl->len;
+		DBG("using %dK of OCMEM at 0x%08x", adreno_gpu->gmem / 1024,
+				a4xx_gpu->ocmem_base);
+#endif
+	}
+
+	if (!gpu->mmu) {
+		/* TODO we think it is possible to configure the GPU to
+		 * restrict access to VRAM carveout.  But the required
+		 * registers are unknown.  For now just bail out and
+		 * limp along with just modesetting.  If it turns out
+		 * to not be possible to restrict access, then we must
+		 * implement a cmdstream validator.
+		 */
+		dev_err(dev->dev, "No memory protection without IOMMU\n");
+		ret = -ENXIO;
+		goto fail;
+	}
+
+	return gpu;
+
+fail:
+	if (a4xx_gpu)
+		a4xx_destroy(&a4xx_gpu->base.base);
+
+	return ERR_PTR(ret);
+}
diff --git a/drivers/gpu/drm/msm/adreno/a4xx_gpu.h b/drivers/gpu/drm/msm/adreno/a4xx_gpu.h
new file mode 100644
index 000000000000..01247204ac92
--- /dev/null
+++ b/drivers/gpu/drm/msm/adreno/a4xx_gpu.h
@@ -0,0 +1,34 @@
+/* Copyright (c) 2014 The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#ifndef __A4XX_GPU_H__
+#define __A4XX_GPU_H__
+
+#include "adreno_gpu.h"
+
+/* arrg, somehow fb.h is getting pulled in: */
+#undef ROP_COPY
+#undef ROP_XOR
+
+#include "a4xx.xml.h"
+
+struct a4xx_gpu {
+	struct adreno_gpu base;
+	struct platform_device *pdev;
+
+	/* if OCMEM is used for GMEM: */
+	uint32_t ocmem_base;
+	void *ocmem_hdl;
+};
+#define to_a4xx_gpu(x) container_of(x, struct a4xx_gpu, base)
+
+#endif /* __A4XX_GPU_H__ */
diff --git a/drivers/gpu/drm/msm/adreno/adreno_common.xml.h b/drivers/gpu/drm/msm/adreno/adreno_common.xml.h
index cc341bc62b51..a4b33af9338d 100644
--- a/drivers/gpu/drm/msm/adreno/adreno_common.xml.h
+++ b/drivers/gpu/drm/msm/adreno/adreno_common.xml.h
@@ -11,10 +11,10 @@ The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/adreno.xml               (    364 bytes, from 2013-11-30 14:47:15)
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml  (   1453 bytes, from 2013-03-31 16:51:27)
 - /home/robclark/src/freedreno/envytools/rnndb/adreno/a2xx.xml          (  32901 bytes, from 2014-06-02 15:21:30)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_common.xml (   9859 bytes, from 2014-06-02 15:21:30)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_pm4.xml    (  14960 bytes, from 2014-07-27 17:22:13)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/a3xx.xml          (  58020 bytes, from 2014-08-01 12:22:48)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/a4xx.xml          (  41068 bytes, from 2014-08-01 12:22:48)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_common.xml (  10551 bytes, from 2014-11-13 22:44:30)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_pm4.xml    (  15053 bytes, from 2014-11-09 15:45:47)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/a3xx.xml          (  63169 bytes, from 2014-11-13 22:44:18)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/a4xx.xml          (  49097 bytes, from 2014-11-14 15:38:00)
 
 Copyright (C) 2013-2014 by the following authors:
 - Rob Clark <robdclark@gmail.com> (robclark)
@@ -105,6 +105,7 @@ enum adreno_rb_dither_mode {
 enum adreno_rb_depth_format {
 	DEPTHX_16 = 0,
 	DEPTHX_24_8 = 1,
+	DEPTHX_32 = 2,
 };
 
 enum adreno_rb_copy_control_mode {
@@ -132,6 +133,7 @@ enum a3xx_threadmode {
 };
 
 enum a3xx_instrbuffermode {
+	CACHE = 0,
 	BUFFER = 1,
 };
 
@@ -140,6 +142,13 @@ enum a3xx_threadsize {
 	FOUR_QUADS = 1,
 };
 
+enum a3xx_color_swap {
+	WZYX = 0,
+	WXYZ = 1,
+	ZYXW = 2,
+	XYZW = 3,
+};
+
 #define REG_AXXX_CP_RB_BASE					0x000001c0
 
 #define REG_AXXX_CP_RB_CNTL					0x000001c1
diff --git a/drivers/gpu/drm/msm/adreno/adreno_device.c b/drivers/gpu/drm/msm/adreno/adreno_device.c
index 7ab85af3a7db..be83dee83d08 100644
--- a/drivers/gpu/drm/msm/adreno/adreno_device.c
+++ b/drivers/gpu/drm/msm/adreno/adreno_device.c
@@ -2,6 +2,8 @@
  * Copyright (C) 2013-2014 Red Hat
  * Author: Rob Clark <robdclark@gmail.com>
  *
+ * Copyright (c) 2014 The Linux Foundation. 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.
@@ -28,6 +30,7 @@ MODULE_PARM_DESC(hang_debug, "Dump registers when hang is detected (can be slow!
 module_param_named(hang_debug, hang_debug, bool, 0600);
 
 struct msm_gpu *a3xx_gpu_init(struct drm_device *dev);
+struct msm_gpu *a4xx_gpu_init(struct drm_device *dev);
 
 static const struct adreno_info gpulist[] = {
 	{
@@ -54,6 +57,14 @@ static const struct adreno_info gpulist[] = {
 		.pfpfw = "a330_pfp.fw",
 		.gmem  = SZ_1M,
 		.init  = a3xx_gpu_init,
+	}, {
+		.rev   = ADRENO_REV(4, 2, 0, ANY_ID),
+		.revn  = 420,
+		.name  = "A420",
+		.pm4fw = "a420_pm4.fw",
+		.pfpfw = "a420_pfp.fw",
+		.gmem  = (SZ_1M + SZ_512K),
+		.init  = a4xx_gpu_init,
 	},
 };
 
@@ -61,6 +72,8 @@ MODULE_FIRMWARE("a300_pm4.fw");
 MODULE_FIRMWARE("a300_pfp.fw");
 MODULE_FIRMWARE("a330_pm4.fw");
 MODULE_FIRMWARE("a330_pfp.fw");
+MODULE_FIRMWARE("a420_pm4.fw");
+MODULE_FIRMWARE("a420_pfp.fw");
 
 static inline bool _rev_match(uint8_t entry, uint8_t id)
 {
diff --git a/drivers/gpu/drm/msm/adreno/adreno_gpu.c b/drivers/gpu/drm/msm/adreno/adreno_gpu.c
index 6afa29167fee..94a5bee69fe7 100644
--- a/drivers/gpu/drm/msm/adreno/adreno_gpu.c
+++ b/drivers/gpu/drm/msm/adreno/adreno_gpu.c
@@ -2,6 +2,8 @@
  * Copyright (C) 2013 Red Hat
  * Author: Rob Clark <robdclark@gmail.com>
  *
+ * Copyright (c) 2014 The Linux Foundation. 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.
@@ -63,19 +65,21 @@ int adreno_hw_init(struct msm_gpu *gpu)
 	}
 
 	/* Setup REG_CP_RB_CNTL: */
-	gpu_write(gpu, REG_AXXX_CP_RB_CNTL,
+	adreno_gpu_write(adreno_gpu, REG_ADRENO_CP_RB_CNTL,
 			/* size is log2(quad-words): */
 			AXXX_CP_RB_CNTL_BUFSZ(ilog2(gpu->rb->size / 8)) |
 			AXXX_CP_RB_CNTL_BLKSZ(ilog2(RB_BLKSIZE / 8)));
 
 	/* Setup ringbuffer address: */
-	gpu_write(gpu, REG_AXXX_CP_RB_BASE, gpu->rb_iova);
-	gpu_write(gpu, REG_AXXX_CP_RB_RPTR_ADDR, rbmemptr(adreno_gpu, rptr));
+	adreno_gpu_write(adreno_gpu, REG_ADRENO_CP_RB_BASE, gpu->rb_iova);
+	adreno_gpu_write(adreno_gpu, REG_ADRENO_CP_RB_RPTR_ADDR,
+			rbmemptr(adreno_gpu, rptr));
 
 	/* Setup scratch/timestamp: */
-	gpu_write(gpu, REG_AXXX_SCRATCH_ADDR, rbmemptr(adreno_gpu, fence));
+	adreno_gpu_write(adreno_gpu, REG_ADRENO_SCRATCH_ADDR,
+			rbmemptr(adreno_gpu, fence));
 
-	gpu_write(gpu, REG_AXXX_SCRATCH_UMSK, 0x1);
+	adreno_gpu_write(adreno_gpu, REG_ADRENO_SCRATCH_UMSK, 0x1);
 
 	return 0;
 }
@@ -151,7 +155,7 @@ int adreno_submit(struct msm_gpu *gpu, struct msm_gem_submit *submit,
 	OUT_PKT0(ring, REG_AXXX_CP_SCRATCH_REG2, 1);
 	OUT_RING(ring, submit->fence);
 
-	if (adreno_is_a3xx(adreno_gpu)) {
+	if (adreno_is_a3xx(adreno_gpu) || adreno_is_a4xx(adreno_gpu)) {
 		/* Flush HLSQ lazy updates to make sure there is nothing
 		 * pending for indirect loads after the timestamp has
 		 * passed:
@@ -188,12 +192,13 @@ int adreno_submit(struct msm_gpu *gpu, struct msm_gem_submit *submit,
 
 void adreno_flush(struct msm_gpu *gpu)
 {
+	struct adreno_gpu *adreno_gpu = to_adreno_gpu(gpu);
 	uint32_t wptr = get_wptr(gpu->rb);
 
 	/* ensure writes to ringbuffer have hit system memory: */
 	mb();
 
-	gpu_write(gpu, REG_AXXX_CP_RB_WPTR, wptr);
+	adreno_gpu_write(adreno_gpu, REG_ADRENO_CP_RB_WPTR, wptr);
 }
 
 void adreno_idle(struct msm_gpu *gpu)
@@ -319,6 +324,12 @@ int adreno_gpu_init(struct drm_device *drm, struct platform_device *pdev,
 	DBG("fast_rate=%u, slow_rate=%u, bus_freq=%u",
 			gpu->fast_rate, gpu->slow_rate, gpu->bus_freq);
 
+	ret = msm_gpu_init(drm, pdev, &adreno_gpu->base, &funcs->base,
+			adreno_gpu->info->name, "kgsl_3d0_reg_memory", "kgsl_3d0_irq",
+			RB_SIZE);
+	if (ret)
+		return ret;
+
 	ret = request_firmware(&adreno_gpu->pm4, adreno_gpu->info->pm4fw, drm->dev);
 	if (ret) {
 		dev_err(drm->dev, "failed to load %s PM4 firmware: %d\n",
@@ -333,12 +344,6 @@ int adreno_gpu_init(struct drm_device *drm, struct platform_device *pdev,
 		return ret;
 	}
 
-	ret = msm_gpu_init(drm, pdev, &adreno_gpu->base, &funcs->base,
-			adreno_gpu->info->name, "kgsl_3d0_reg_memory", "kgsl_3d0_irq",
-			RB_SIZE);
-	if (ret)
-		return ret;
-
 	mmu = gpu->mmu;
 	if (mmu) {
 		ret = mmu->funcs->attach(mmu, iommu_ports,
@@ -381,9 +386,7 @@ void adreno_gpu_cleanup(struct adreno_gpu *gpu)
 			msm_gem_put_iova(gpu->memptrs_bo, gpu->base.id);
 		drm_gem_object_unreference(gpu->memptrs_bo);
 	}
-	if (gpu->pm4)
-		release_firmware(gpu->pm4);
-	if (gpu->pfp)
-		release_firmware(gpu->pfp);
+	release_firmware(gpu->pm4);
+	release_firmware(gpu->pfp);
 	msm_gpu_cleanup(&gpu->base);
 }
diff --git a/drivers/gpu/drm/msm/adreno/adreno_gpu.h b/drivers/gpu/drm/msm/adreno/adreno_gpu.h
index 52f051579753..a0cc30977e67 100644
--- a/drivers/gpu/drm/msm/adreno/adreno_gpu.h
+++ b/drivers/gpu/drm/msm/adreno/adreno_gpu.h
@@ -2,6 +2,8 @@
  * Copyright (C) 2013 Red Hat
  * Author: Rob Clark <robdclark@gmail.com>
  *
+ * Copyright (c) 2014 The Linux Foundation. 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.
@@ -25,6 +27,81 @@
 #include "adreno_common.xml.h"
 #include "adreno_pm4.xml.h"
 
+#define REG_ADRENO_DEFINE(_offset, _reg) [_offset] = (_reg) + 1
+/**
+ * adreno_regs: List of registers that are used in across all
+ * 3D devices. Each device type has different offset value for the same
+ * register, so an array of register offsets are declared for every device
+ * and are indexed by the enumeration values defined in this enum
+ */
+enum adreno_regs {
+	REG_ADRENO_CP_DEBUG,
+	REG_ADRENO_CP_ME_RAM_WADDR,
+	REG_ADRENO_CP_ME_RAM_DATA,
+	REG_ADRENO_CP_PFP_UCODE_DATA,
+	REG_ADRENO_CP_PFP_UCODE_ADDR,
+	REG_ADRENO_CP_WFI_PEND_CTR,
+	REG_ADRENO_CP_RB_BASE,
+	REG_ADRENO_CP_RB_RPTR_ADDR,
+	REG_ADRENO_CP_RB_RPTR,
+	REG_ADRENO_CP_RB_WPTR,
+	REG_ADRENO_CP_PROTECT_CTRL,
+	REG_ADRENO_CP_ME_CNTL,
+	REG_ADRENO_CP_RB_CNTL,
+	REG_ADRENO_CP_IB1_BASE,
+	REG_ADRENO_CP_IB1_BUFSZ,
+	REG_ADRENO_CP_IB2_BASE,
+	REG_ADRENO_CP_IB2_BUFSZ,
+	REG_ADRENO_CP_TIMESTAMP,
+	REG_ADRENO_CP_ME_RAM_RADDR,
+	REG_ADRENO_CP_ROQ_ADDR,
+	REG_ADRENO_CP_ROQ_DATA,
+	REG_ADRENO_CP_MERCIU_ADDR,
+	REG_ADRENO_CP_MERCIU_DATA,
+	REG_ADRENO_CP_MERCIU_DATA2,
+	REG_ADRENO_CP_MEQ_ADDR,
+	REG_ADRENO_CP_MEQ_DATA,
+	REG_ADRENO_CP_HW_FAULT,
+	REG_ADRENO_CP_PROTECT_STATUS,
+	REG_ADRENO_SCRATCH_ADDR,
+	REG_ADRENO_SCRATCH_UMSK,
+	REG_ADRENO_SCRATCH_REG2,
+	REG_ADRENO_RBBM_STATUS,
+	REG_ADRENO_RBBM_PERFCTR_CTL,
+	REG_ADRENO_RBBM_PERFCTR_LOAD_CMD0,
+	REG_ADRENO_RBBM_PERFCTR_LOAD_CMD1,
+	REG_ADRENO_RBBM_PERFCTR_LOAD_CMD2,
+	REG_ADRENO_RBBM_PERFCTR_PWR_1_LO,
+	REG_ADRENO_RBBM_INT_0_MASK,
+	REG_ADRENO_RBBM_INT_0_STATUS,
+	REG_ADRENO_RBBM_AHB_ERROR_STATUS,
+	REG_ADRENO_RBBM_PM_OVERRIDE2,
+	REG_ADRENO_RBBM_AHB_CMD,
+	REG_ADRENO_RBBM_INT_CLEAR_CMD,
+	REG_ADRENO_RBBM_SW_RESET_CMD,
+	REG_ADRENO_RBBM_CLOCK_CTL,
+	REG_ADRENO_RBBM_AHB_ME_SPLIT_STATUS,
+	REG_ADRENO_RBBM_AHB_PFP_SPLIT_STATUS,
+	REG_ADRENO_VPC_DEBUG_RAM_SEL,
+	REG_ADRENO_VPC_DEBUG_RAM_READ,
+	REG_ADRENO_VSC_SIZE_ADDRESS,
+	REG_ADRENO_VFD_CONTROL_0,
+	REG_ADRENO_VFD_INDEX_MAX,
+	REG_ADRENO_SP_VS_PVT_MEM_ADDR_REG,
+	REG_ADRENO_SP_FS_PVT_MEM_ADDR_REG,
+	REG_ADRENO_SP_VS_OBJ_START_REG,
+	REG_ADRENO_SP_FS_OBJ_START_REG,
+	REG_ADRENO_PA_SC_AA_CONFIG,
+	REG_ADRENO_SQ_GPR_MANAGEMENT,
+	REG_ADRENO_SQ_INST_STORE_MANAGMENT,
+	REG_ADRENO_TP0_CHICKEN,
+	REG_ADRENO_RBBM_RBBM_CTL,
+	REG_ADRENO_UCHE_INVALIDATE0,
+	REG_ADRENO_RBBM_PERFCTR_LOAD_VALUE_LO,
+	REG_ADRENO_RBBM_PERFCTR_LOAD_VALUE_HI,
+	REG_ADRENO_REGISTER_MAX,
+};
+
 struct adreno_rev {
 	uint8_t  core;
 	uint8_t  major;
@@ -76,6 +153,13 @@ struct adreno_gpu {
 	struct adreno_rbmemptrs *memptrs;
 	struct drm_gem_object *memptrs_bo;
 	uint32_t memptrs_iova;
+
+	/*
+	 * Register offsets are different between some GPUs.
+	 * GPU specific offsets will be exported by GPU specific
+	 * code (a3xx_gpu.c) and stored in this common location.
+	 */
+	const unsigned int *reg_offsets;
 };
 #define to_adreno_gpu(x) container_of(x, struct adreno_gpu, base)
 
@@ -128,6 +212,16 @@ static inline bool adreno_is_a330v2(struct adreno_gpu *gpu)
 	return adreno_is_a330(gpu) && (gpu->rev.patchid > 0);
 }
 
+static inline bool adreno_is_a4xx(struct adreno_gpu *gpu)
+{
+	return (gpu->revn >= 400) && (gpu->revn < 500);
+}
+
+static inline int adreno_is_a420(struct adreno_gpu *gpu)
+{
+	return gpu->revn == 420;
+}
+
 int adreno_get_param(struct msm_gpu *gpu, uint32_t param, uint64_t *value);
 int adreno_hw_init(struct msm_gpu *gpu);
 uint32_t adreno_last_fence(struct msm_gpu *gpu);
@@ -171,5 +265,37 @@ OUT_PKT3(struct msm_ringbuffer *ring, uint8_t opcode, uint16_t cnt)
 	OUT_RING(ring, CP_TYPE3_PKT | ((cnt-1) << 16) | ((opcode & 0xFF) << 8));
 }
 
+/*
+ * adreno_checkreg_off() - Checks the validity of a register enum
+ * @gpu:		Pointer to struct adreno_gpu
+ * @offset_name:	The register enum that is checked
+ */
+static inline bool adreno_reg_check(struct adreno_gpu *gpu,
+		enum adreno_regs offset_name)
+{
+	if (offset_name >= REG_ADRENO_REGISTER_MAX ||
+			!gpu->reg_offsets[offset_name]) {
+		BUG();
+	}
+	return true;
+}
+
+static inline u32 adreno_gpu_read(struct adreno_gpu *gpu,
+		enum adreno_regs offset_name)
+{
+	u32 reg = gpu->reg_offsets[offset_name];
+	u32 val = 0;
+	if(adreno_reg_check(gpu,offset_name))
+		val = gpu_read(&gpu->base, reg - 1);
+	return val;
+}
+
+static inline void adreno_gpu_write(struct adreno_gpu *gpu,
+		enum adreno_regs offset_name, u32 data)
+{
+	u32 reg = gpu->reg_offsets[offset_name];
+	if(adreno_reg_check(gpu, offset_name))
+		gpu_write(&gpu->base, reg - 1, data);
+}
 
 #endif /* __ADRENO_GPU_H__ */
diff --git a/drivers/gpu/drm/msm/adreno/adreno_pm4.xml.h b/drivers/gpu/drm/msm/adreno/adreno_pm4.xml.h
index 6ef43f66c30a..6a75cee94d81 100644
--- a/drivers/gpu/drm/msm/adreno/adreno_pm4.xml.h
+++ b/drivers/gpu/drm/msm/adreno/adreno_pm4.xml.h
@@ -11,10 +11,10 @@ The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/adreno.xml               (    364 bytes, from 2013-11-30 14:47:15)
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml  (   1453 bytes, from 2013-03-31 16:51:27)
 - /home/robclark/src/freedreno/envytools/rnndb/adreno/a2xx.xml          (  32901 bytes, from 2014-06-02 15:21:30)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_common.xml (   9859 bytes, from 2014-06-02 15:21:30)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_pm4.xml    (  14960 bytes, from 2014-07-27 17:22:13)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/a3xx.xml          (  58020 bytes, from 2014-08-01 12:22:48)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/a4xx.xml          (  41068 bytes, from 2014-08-01 12:22:48)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_common.xml (  10551 bytes, from 2014-11-13 22:44:30)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_pm4.xml    (  15053 bytes, from 2014-11-09 15:45:47)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/a3xx.xml          (  63169 bytes, from 2014-11-13 22:44:18)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/a4xx.xml          (  49097 bytes, from 2014-11-14 15:38:00)
 
 Copyright (C) 2013-2014 by the following authors:
 - Rob Clark <robdclark@gmail.com> (robclark)
@@ -157,6 +157,7 @@ enum adreno_pm4_type3_packets {
 	CP_IM_STORE = 44,
 	CP_SET_DRAW_INIT_FLAGS = 75,
 	CP_SET_PROTECTED_MODE = 95,
+	CP_BOOTSTRAP_UCODE = 111,
 	CP_LOAD_STATE = 48,
 	CP_COND_INDIRECT_BUFFER_PFE = 58,
 	CP_COND_INDIRECT_BUFFER_PFD = 50,
@@ -278,11 +279,11 @@ static inline uint32_t CP_DRAW_INDX_1_INDEX_SIZE(enum pc_di_index_size val)
 #define CP_DRAW_INDX_1_NOT_EOP					0x00001000
 #define CP_DRAW_INDX_1_SMALL_INDEX				0x00002000
 #define CP_DRAW_INDX_1_PRE_DRAW_INITIATOR_ENABLE		0x00004000
-#define CP_DRAW_INDX_1_NUM_INDICES__MASK			0xffff0000
-#define CP_DRAW_INDX_1_NUM_INDICES__SHIFT			16
-static inline uint32_t CP_DRAW_INDX_1_NUM_INDICES(uint32_t val)
+#define CP_DRAW_INDX_1_NUM_INSTANCES__MASK			0xff000000
+#define CP_DRAW_INDX_1_NUM_INSTANCES__SHIFT			24
+static inline uint32_t CP_DRAW_INDX_1_NUM_INSTANCES(uint32_t val)
 {
-	return ((val) << CP_DRAW_INDX_1_NUM_INDICES__SHIFT) & CP_DRAW_INDX_1_NUM_INDICES__MASK;
+	return ((val) << CP_DRAW_INDX_1_NUM_INSTANCES__SHIFT) & CP_DRAW_INDX_1_NUM_INSTANCES__MASK;
 }
 
 #define REG_CP_DRAW_INDX_2					0x00000002
@@ -293,20 +294,20 @@ static inline uint32_t CP_DRAW_INDX_2_NUM_INDICES(uint32_t val)
 	return ((val) << CP_DRAW_INDX_2_NUM_INDICES__SHIFT) & CP_DRAW_INDX_2_NUM_INDICES__MASK;
 }
 
-#define REG_CP_DRAW_INDX_2					0x00000002
-#define CP_DRAW_INDX_2_INDX_BASE__MASK				0xffffffff
-#define CP_DRAW_INDX_2_INDX_BASE__SHIFT				0
-static inline uint32_t CP_DRAW_INDX_2_INDX_BASE(uint32_t val)
+#define REG_CP_DRAW_INDX_3					0x00000003
+#define CP_DRAW_INDX_3_INDX_BASE__MASK				0xffffffff
+#define CP_DRAW_INDX_3_INDX_BASE__SHIFT				0
+static inline uint32_t CP_DRAW_INDX_3_INDX_BASE(uint32_t val)
 {
-	return ((val) << CP_DRAW_INDX_2_INDX_BASE__SHIFT) & CP_DRAW_INDX_2_INDX_BASE__MASK;
+	return ((val) << CP_DRAW_INDX_3_INDX_BASE__SHIFT) & CP_DRAW_INDX_3_INDX_BASE__MASK;
 }
 
-#define REG_CP_DRAW_INDX_2					0x00000002
-#define CP_DRAW_INDX_2_INDX_SIZE__MASK				0xffffffff
-#define CP_DRAW_INDX_2_INDX_SIZE__SHIFT				0
-static inline uint32_t CP_DRAW_INDX_2_INDX_SIZE(uint32_t val)
+#define REG_CP_DRAW_INDX_4					0x00000004
+#define CP_DRAW_INDX_4_INDX_SIZE__MASK				0xffffffff
+#define CP_DRAW_INDX_4_INDX_SIZE__SHIFT				0
+static inline uint32_t CP_DRAW_INDX_4_INDX_SIZE(uint32_t val)
 {
-	return ((val) << CP_DRAW_INDX_2_INDX_SIZE__SHIFT) & CP_DRAW_INDX_2_INDX_SIZE__MASK;
+	return ((val) << CP_DRAW_INDX_4_INDX_SIZE__SHIFT) & CP_DRAW_INDX_4_INDX_SIZE__MASK;
 }
 
 #define REG_CP_DRAW_INDX_2_0					0x00000000
@@ -345,11 +346,11 @@ static inline uint32_t CP_DRAW_INDX_2_1_INDEX_SIZE(enum pc_di_index_size val)
 #define CP_DRAW_INDX_2_1_NOT_EOP				0x00001000
 #define CP_DRAW_INDX_2_1_SMALL_INDEX				0x00002000
 #define CP_DRAW_INDX_2_1_PRE_DRAW_INITIATOR_ENABLE		0x00004000
-#define CP_DRAW_INDX_2_1_NUM_INDICES__MASK			0xffff0000
-#define CP_DRAW_INDX_2_1_NUM_INDICES__SHIFT			16
-static inline uint32_t CP_DRAW_INDX_2_1_NUM_INDICES(uint32_t val)
+#define CP_DRAW_INDX_2_1_NUM_INSTANCES__MASK			0xff000000
+#define CP_DRAW_INDX_2_1_NUM_INSTANCES__SHIFT			24
+static inline uint32_t CP_DRAW_INDX_2_1_NUM_INSTANCES(uint32_t val)
 {
-	return ((val) << CP_DRAW_INDX_2_1_NUM_INDICES__SHIFT) & CP_DRAW_INDX_2_1_NUM_INDICES__MASK;
+	return ((val) << CP_DRAW_INDX_2_1_NUM_INSTANCES__SHIFT) & CP_DRAW_INDX_2_1_NUM_INSTANCES__MASK;
 }
 
 #define REG_CP_DRAW_INDX_2_2					0x00000002
@@ -388,11 +389,11 @@ static inline uint32_t CP_DRAW_INDX_OFFSET_0_INDEX_SIZE(enum pc_di_index_size va
 #define CP_DRAW_INDX_OFFSET_0_NOT_EOP				0x00001000
 #define CP_DRAW_INDX_OFFSET_0_SMALL_INDEX			0x00002000
 #define CP_DRAW_INDX_OFFSET_0_PRE_DRAW_INITIATOR_ENABLE		0x00004000
-#define CP_DRAW_INDX_OFFSET_0_NUM_INDICES__MASK			0xffff0000
-#define CP_DRAW_INDX_OFFSET_0_NUM_INDICES__SHIFT		16
-static inline uint32_t CP_DRAW_INDX_OFFSET_0_NUM_INDICES(uint32_t val)
+#define CP_DRAW_INDX_OFFSET_0_NUM_INSTANCES__MASK		0xffff0000
+#define CP_DRAW_INDX_OFFSET_0_NUM_INSTANCES__SHIFT		16
+static inline uint32_t CP_DRAW_INDX_OFFSET_0_NUM_INSTANCES(uint32_t val)
 {
-	return ((val) << CP_DRAW_INDX_OFFSET_0_NUM_INDICES__SHIFT) & CP_DRAW_INDX_OFFSET_0_NUM_INDICES__MASK;
+	return ((val) << CP_DRAW_INDX_OFFSET_0_NUM_INSTANCES__SHIFT) & CP_DRAW_INDX_OFFSET_0_NUM_INSTANCES__MASK;
 }
 
 #define REG_CP_DRAW_INDX_OFFSET_1				0x00000001
@@ -405,20 +406,22 @@ static inline uint32_t CP_DRAW_INDX_OFFSET_2_NUM_INDICES(uint32_t val)
 	return ((val) << CP_DRAW_INDX_OFFSET_2_NUM_INDICES__SHIFT) & CP_DRAW_INDX_OFFSET_2_NUM_INDICES__MASK;
 }
 
-#define REG_CP_DRAW_INDX_OFFSET_2				0x00000002
-#define CP_DRAW_INDX_OFFSET_2_INDX_BASE__MASK			0xffffffff
-#define CP_DRAW_INDX_OFFSET_2_INDX_BASE__SHIFT			0
-static inline uint32_t CP_DRAW_INDX_OFFSET_2_INDX_BASE(uint32_t val)
+#define REG_CP_DRAW_INDX_OFFSET_3				0x00000003
+
+#define REG_CP_DRAW_INDX_OFFSET_4				0x00000004
+#define CP_DRAW_INDX_OFFSET_4_INDX_BASE__MASK			0xffffffff
+#define CP_DRAW_INDX_OFFSET_4_INDX_BASE__SHIFT			0
+static inline uint32_t CP_DRAW_INDX_OFFSET_4_INDX_BASE(uint32_t val)
 {
-	return ((val) << CP_DRAW_INDX_OFFSET_2_INDX_BASE__SHIFT) & CP_DRAW_INDX_OFFSET_2_INDX_BASE__MASK;
+	return ((val) << CP_DRAW_INDX_OFFSET_4_INDX_BASE__SHIFT) & CP_DRAW_INDX_OFFSET_4_INDX_BASE__MASK;
 }
 
-#define REG_CP_DRAW_INDX_OFFSET_2				0x00000002
-#define CP_DRAW_INDX_OFFSET_2_INDX_SIZE__MASK			0xffffffff
-#define CP_DRAW_INDX_OFFSET_2_INDX_SIZE__SHIFT			0
-static inline uint32_t CP_DRAW_INDX_OFFSET_2_INDX_SIZE(uint32_t val)
+#define REG_CP_DRAW_INDX_OFFSET_5				0x00000005
+#define CP_DRAW_INDX_OFFSET_5_INDX_SIZE__MASK			0xffffffff
+#define CP_DRAW_INDX_OFFSET_5_INDX_SIZE__SHIFT			0
+static inline uint32_t CP_DRAW_INDX_OFFSET_5_INDX_SIZE(uint32_t val)
 {
-	return ((val) << CP_DRAW_INDX_OFFSET_2_INDX_SIZE__SHIFT) & CP_DRAW_INDX_OFFSET_2_INDX_SIZE__MASK;
+	return ((val) << CP_DRAW_INDX_OFFSET_5_INDX_SIZE__SHIFT) & CP_DRAW_INDX_OFFSET_5_INDX_SIZE__MASK;
 }
 
 #define REG_CP_SET_DRAW_STATE_0					0x00000000
diff --git a/drivers/gpu/drm/msm/dsi/dsi.xml.h b/drivers/gpu/drm/msm/dsi/dsi.xml.h
index e965898dfda6..448438b759b4 100644
--- a/drivers/gpu/drm/msm/dsi/dsi.xml.h
+++ b/drivers/gpu/drm/msm/dsi/dsi.xml.h
@@ -10,12 +10,12 @@ git clone https://github.com/freedreno/envytools.git
 The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/msm.xml                 (    647 bytes, from 2013-11-30 14:45:35)
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml (   1453 bytes, from 2013-03-31 16:51:27)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20457 bytes, from 2014-08-01 12:22:48)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1615 bytes, from 2014-07-17 15:34:33)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  22517 bytes, from 2014-07-17 15:34:33)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20136 bytes, from 2014-10-31 16:51:39)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1940 bytes, from 2014-10-31 16:51:39)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  23963 bytes, from 2014-10-31 16:51:46)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/dsi.xml             (  11712 bytes, from 2013-08-17 17:13:43)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/sfpb.xml            (    344 bytes, from 2013-08-11 19:26:32)
-- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-08-01 12:23:53)
+- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-10-31 16:48:57)
 - /home/robclark/src/freedreno/envytools/rnndb/hdmi/qfprom.xml         (    600 bytes, from 2013-07-05 19:21:12)
 - /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-07-17 15:33:30)
 
diff --git a/drivers/gpu/drm/msm/dsi/mmss_cc.xml.h b/drivers/gpu/drm/msm/dsi/mmss_cc.xml.h
index f2bdda957205..c102a7f074ac 100644
--- a/drivers/gpu/drm/msm/dsi/mmss_cc.xml.h
+++ b/drivers/gpu/drm/msm/dsi/mmss_cc.xml.h
@@ -10,12 +10,12 @@ git clone https://github.com/freedreno/envytools.git
 The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/msm.xml                 (    647 bytes, from 2013-11-30 14:45:35)
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml (   1453 bytes, from 2013-03-31 16:51:27)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20457 bytes, from 2014-08-01 12:22:48)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1615 bytes, from 2014-07-17 15:34:33)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  22517 bytes, from 2014-07-17 15:34:33)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20136 bytes, from 2014-10-31 16:51:39)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1940 bytes, from 2014-10-31 16:51:39)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  23963 bytes, from 2014-10-31 16:51:46)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/dsi.xml             (  11712 bytes, from 2013-08-17 17:13:43)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/sfpb.xml            (    344 bytes, from 2013-08-11 19:26:32)
-- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-08-01 12:23:53)
+- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-10-31 16:48:57)
 - /home/robclark/src/freedreno/envytools/rnndb/hdmi/qfprom.xml         (    600 bytes, from 2013-07-05 19:21:12)
 - /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-07-17 15:33:30)
 
diff --git a/drivers/gpu/drm/msm/dsi/sfpb.xml.h b/drivers/gpu/drm/msm/dsi/sfpb.xml.h
index e5b071ffd865..a900134bdf33 100644
--- a/drivers/gpu/drm/msm/dsi/sfpb.xml.h
+++ b/drivers/gpu/drm/msm/dsi/sfpb.xml.h
@@ -10,12 +10,12 @@ git clone https://github.com/freedreno/envytools.git
 The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/msm.xml                 (    647 bytes, from 2013-11-30 14:45:35)
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml (   1453 bytes, from 2013-03-31 16:51:27)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20457 bytes, from 2014-08-01 12:22:48)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1615 bytes, from 2014-07-17 15:34:33)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  22517 bytes, from 2014-07-17 15:34:33)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20136 bytes, from 2014-10-31 16:51:39)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1940 bytes, from 2014-10-31 16:51:39)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  23963 bytes, from 2014-10-31 16:51:46)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/dsi.xml             (  11712 bytes, from 2013-08-17 17:13:43)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/sfpb.xml            (    344 bytes, from 2013-08-11 19:26:32)
-- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-08-01 12:23:53)
+- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-10-31 16:48:57)
 - /home/robclark/src/freedreno/envytools/rnndb/hdmi/qfprom.xml         (    600 bytes, from 2013-07-05 19:21:12)
 - /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-07-17 15:33:30)
 
diff --git a/drivers/gpu/drm/msm/hdmi/hdmi.c b/drivers/gpu/drm/msm/hdmi/hdmi.c
index 9d00dcba6959..062c68725376 100644
--- a/drivers/gpu/drm/msm/hdmi/hdmi.c
+++ b/drivers/gpu/drm/msm/hdmi/hdmi.c
@@ -15,6 +15,7 @@
  * this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <linux/of_irq.h>
 #include "hdmi.h"
 
 void hdmi_set_mode(struct hdmi *hdmi, bool power_on)
@@ -39,7 +40,7 @@ void hdmi_set_mode(struct hdmi *hdmi, bool power_on)
 			power_on ? "Enable" : "Disable", ctrl);
 }
 
-irqreturn_t hdmi_irq(int irq, void *dev_id)
+static irqreturn_t hdmi_irq(int irq, void *dev_id)
 {
 	struct hdmi *hdmi = dev_id;
 
@@ -54,9 +55,8 @@ irqreturn_t hdmi_irq(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-void hdmi_destroy(struct kref *kref)
+static void hdmi_destroy(struct hdmi *hdmi)
 {
-	struct hdmi *hdmi = container_of(kref, struct hdmi, refcount);
 	struct hdmi_phy *phy = hdmi->phy;
 
 	if (phy)
@@ -68,37 +68,24 @@ void hdmi_destroy(struct kref *kref)
 	platform_set_drvdata(hdmi->pdev, NULL);
 }
 
-/* initialize connector */
-struct hdmi *hdmi_init(struct drm_device *dev, struct drm_encoder *encoder)
+/* construct hdmi at bind/probe time, grab all the resources.  If
+ * we are to EPROBE_DEFER we want to do it here, rather than later
+ * at modeset_init() time
+ */
+static struct hdmi *hdmi_init(struct platform_device *pdev)
 {
+	struct hdmi_platform_config *config = pdev->dev.platform_data;
 	struct hdmi *hdmi = NULL;
-	struct msm_drm_private *priv = dev->dev_private;
-	struct platform_device *pdev = priv->hdmi_pdev;
-	struct hdmi_platform_config *config;
 	int i, ret;
 
-	if (!pdev) {
-		dev_err(dev->dev, "no hdmi device\n");
-		ret = -ENXIO;
-		goto fail;
-	}
-
-	config = pdev->dev.platform_data;
-
-	hdmi = kzalloc(sizeof(*hdmi), GFP_KERNEL);
+	hdmi = devm_kzalloc(&pdev->dev, sizeof(*hdmi), GFP_KERNEL);
 	if (!hdmi) {
 		ret = -ENOMEM;
 		goto fail;
 	}
 
-	kref_init(&hdmi->refcount);
-
-	hdmi->dev = dev;
 	hdmi->pdev = pdev;
 	hdmi->config = config;
-	hdmi->encoder = encoder;
-
-	hdmi_audio_infoframe_init(&hdmi->audio.infoframe);
 
 	/* not sure about which phy maps to which msm.. probably I miss some */
 	if (config->phy_init)
@@ -108,7 +95,7 @@ struct hdmi *hdmi_init(struct drm_device *dev, struct drm_encoder *encoder)
 
 	if (IS_ERR(hdmi->phy)) {
 		ret = PTR_ERR(hdmi->phy);
-		dev_err(dev->dev, "failed to load phy: %d\n", ret);
+		dev_err(&pdev->dev, "failed to load phy: %d\n", ret);
 		hdmi->phy = NULL;
 		goto fail;
 	}
@@ -127,7 +114,7 @@ struct hdmi *hdmi_init(struct drm_device *dev, struct drm_encoder *encoder)
 				config->hpd_reg_names[i]);
 		if (IS_ERR(reg)) {
 			ret = PTR_ERR(reg);
-			dev_err(dev->dev, "failed to get hpd regulator: %s (%d)\n",
+			dev_err(&pdev->dev, "failed to get hpd regulator: %s (%d)\n",
 					config->hpd_reg_names[i], ret);
 			goto fail;
 		}
@@ -143,7 +130,7 @@ struct hdmi *hdmi_init(struct drm_device *dev, struct drm_encoder *encoder)
 				config->pwr_reg_names[i]);
 		if (IS_ERR(reg)) {
 			ret = PTR_ERR(reg);
-			dev_err(dev->dev, "failed to get pwr regulator: %s (%d)\n",
+			dev_err(&pdev->dev, "failed to get pwr regulator: %s (%d)\n",
 					config->pwr_reg_names[i], ret);
 			goto fail;
 		}
@@ -158,7 +145,7 @@ struct hdmi *hdmi_init(struct drm_device *dev, struct drm_encoder *encoder)
 		clk = devm_clk_get(&pdev->dev, config->hpd_clk_names[i]);
 		if (IS_ERR(clk)) {
 			ret = PTR_ERR(clk);
-			dev_err(dev->dev, "failed to get hpd clk: %s (%d)\n",
+			dev_err(&pdev->dev, "failed to get hpd clk: %s (%d)\n",
 					config->hpd_clk_names[i], ret);
 			goto fail;
 		}
@@ -173,7 +160,7 @@ struct hdmi *hdmi_init(struct drm_device *dev, struct drm_encoder *encoder)
 		clk = devm_clk_get(&pdev->dev, config->pwr_clk_names[i]);
 		if (IS_ERR(clk)) {
 			ret = PTR_ERR(clk);
-			dev_err(dev->dev, "failed to get pwr clk: %s (%d)\n",
+			dev_err(&pdev->dev, "failed to get pwr clk: %s (%d)\n",
 					config->pwr_clk_names[i], ret);
 			goto fail;
 		}
@@ -184,11 +171,40 @@ struct hdmi *hdmi_init(struct drm_device *dev, struct drm_encoder *encoder)
 	hdmi->i2c = hdmi_i2c_init(hdmi);
 	if (IS_ERR(hdmi->i2c)) {
 		ret = PTR_ERR(hdmi->i2c);
-		dev_err(dev->dev, "failed to get i2c: %d\n", ret);
+		dev_err(&pdev->dev, "failed to get i2c: %d\n", ret);
 		hdmi->i2c = NULL;
 		goto fail;
 	}
 
+	return hdmi;
+
+fail:
+	if (hdmi)
+		hdmi_destroy(hdmi);
+
+	return ERR_PTR(ret);
+}
+
+/* Second part of initialization, the drm/kms level modeset_init,
+ * constructs/initializes mode objects, etc, is called from master
+ * driver (not hdmi sub-device's probe/bind!)
+ *
+ * Any resource (regulator/clk/etc) which could be missing at boot
+ * should be handled in hdmi_init() so that failure happens from
+ * hdmi sub-device's probe.
+ */
+int hdmi_modeset_init(struct hdmi *hdmi,
+		struct drm_device *dev, struct drm_encoder *encoder)
+{
+	struct msm_drm_private *priv = dev->dev_private;
+	struct platform_device *pdev = hdmi->pdev;
+	int ret;
+
+	hdmi->dev = dev;
+	hdmi->encoder = encoder;
+
+	hdmi_audio_infoframe_init(&hdmi->audio.infoframe);
+
 	hdmi->bridge = hdmi_bridge_init(hdmi);
 	if (IS_ERR(hdmi->bridge)) {
 		ret = PTR_ERR(hdmi->bridge);
@@ -205,22 +221,20 @@ struct hdmi *hdmi_init(struct drm_device *dev, struct drm_encoder *encoder)
 		goto fail;
 	}
 
-	if (!config->shared_irq) {
-		hdmi->irq = platform_get_irq(pdev, 0);
-		if (hdmi->irq < 0) {
-			ret = hdmi->irq;
-			dev_err(dev->dev, "failed to get irq: %d\n", ret);
-			goto fail;
-		}
+	hdmi->irq = irq_of_parse_and_map(pdev->dev.of_node, 0);
+	if (hdmi->irq < 0) {
+		ret = hdmi->irq;
+		dev_err(dev->dev, "failed to get irq: %d\n", ret);
+		goto fail;
+	}
 
-		ret = devm_request_threaded_irq(&pdev->dev, hdmi->irq,
-				NULL, hdmi_irq, IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
-				"hdmi_isr", hdmi);
-		if (ret < 0) {
-			dev_err(dev->dev, "failed to request IRQ%u: %d\n",
-					hdmi->irq, ret);
-			goto fail;
-		}
+	ret = devm_request_irq(&pdev->dev, hdmi->irq,
+			hdmi_irq, IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
+			"hdmi_isr", hdmi);
+	if (ret < 0) {
+		dev_err(dev->dev, "failed to request IRQ%u: %d\n",
+				hdmi->irq, ret);
+		goto fail;
 	}
 
 	encoder->bridge = hdmi->bridge;
@@ -230,19 +244,20 @@ struct hdmi *hdmi_init(struct drm_device *dev, struct drm_encoder *encoder)
 
 	platform_set_drvdata(pdev, hdmi);
 
-	return hdmi;
+	return 0;
 
 fail:
-	if (hdmi) {
-		/* bridge/connector are normally destroyed by drm: */
-		if (hdmi->bridge)
-			hdmi->bridge->funcs->destroy(hdmi->bridge);
-		if (hdmi->connector)
-			hdmi->connector->funcs->destroy(hdmi->connector);
-		hdmi_destroy(&hdmi->refcount);
+	/* bridge/connector are normally destroyed by drm: */
+	if (hdmi->bridge) {
+		hdmi->bridge->funcs->destroy(hdmi->bridge);
+		hdmi->bridge = NULL;
+	}
+	if (hdmi->connector) {
+		hdmi->connector->funcs->destroy(hdmi->connector);
+		hdmi->connector = NULL;
 	}
 
-	return ERR_PTR(ret);
+	return ret;
 }
 
 /*
@@ -251,13 +266,6 @@ fail:
 
 #include <linux/of_gpio.h>
 
-static void set_hdmi_pdev(struct drm_device *dev,
-		struct platform_device *pdev)
-{
-	struct msm_drm_private *priv = dev->dev_private;
-	priv->hdmi_pdev = pdev;
-}
-
 #ifdef CONFIG_OF
 static int get_gpio(struct device *dev, struct device_node *of_node, const char *name)
 {
@@ -278,7 +286,10 @@ static int get_gpio(struct device *dev, struct device_node *of_node, const char
 
 static int hdmi_bind(struct device *dev, struct device *master, void *data)
 {
+	struct drm_device *drm = dev_get_drvdata(master);
+	struct msm_drm_private *priv = drm->dev_private;
 	static struct hdmi_platform_config config = {};
+	struct hdmi *hdmi;
 #ifdef CONFIG_OF
 	struct device_node *of_node = dev->of_node;
 
@@ -298,7 +309,6 @@ static int hdmi_bind(struct device *dev, struct device *master, void *data)
 		config.hpd_clk_cnt   = ARRAY_SIZE(hpd_clk_names);
 		config.pwr_clk_names = pwr_clk_names;
 		config.pwr_clk_cnt   = ARRAY_SIZE(pwr_clk_names);
-		config.shared_irq    = true;
 	} else if (of_device_is_compatible(of_node, "qcom,hdmi-tx-8960")) {
 		static const char *hpd_clk_names[] = {"core_clk", "master_iface_clk", "slave_iface_clk"};
 		static const char *hpd_reg_names[] = {"core-vdda", "hdmi-mux"};
@@ -369,14 +379,22 @@ static int hdmi_bind(struct device *dev, struct device *master, void *data)
 	}
 #endif
 	dev->platform_data = &config;
-	set_hdmi_pdev(dev_get_drvdata(master), to_platform_device(dev));
+	hdmi = hdmi_init(to_platform_device(dev));
+	if (IS_ERR(hdmi))
+		return PTR_ERR(hdmi);
+	priv->hdmi = hdmi;
 	return 0;
 }
 
 static void hdmi_unbind(struct device *dev, struct device *master,
 		void *data)
 {
-	set_hdmi_pdev(dev_get_drvdata(master), NULL);
+	struct drm_device *drm = dev_get_drvdata(master);
+	struct msm_drm_private *priv = drm->dev_private;
+	if (priv->hdmi) {
+		hdmi_destroy(priv->hdmi);
+		priv->hdmi = NULL;
+	}
 }
 
 static const struct component_ops hdmi_ops = {
diff --git a/drivers/gpu/drm/msm/hdmi/hdmi.h b/drivers/gpu/drm/msm/hdmi/hdmi.h
index b981995410b5..43e654f751b7 100644
--- a/drivers/gpu/drm/msm/hdmi/hdmi.h
+++ b/drivers/gpu/drm/msm/hdmi/hdmi.h
@@ -38,8 +38,6 @@ struct hdmi_audio {
 };
 
 struct hdmi {
-	struct kref refcount;
-
 	struct drm_device *dev;
 	struct platform_device *pdev;
 
@@ -97,13 +95,9 @@ struct hdmi_platform_config {
 	/* gpio's: */
 	int ddc_clk_gpio, ddc_data_gpio, hpd_gpio, mux_en_gpio, mux_sel_gpio;
 	int mux_lpm_gpio;
-
-	/* older devices had their own irq, mdp5+ it is shared w/ mdp: */
-	bool shared_irq;
 };
 
 void hdmi_set_mode(struct hdmi *hdmi, bool power_on);
-void hdmi_destroy(struct kref *kref);
 
 static inline void hdmi_write(struct hdmi *hdmi, u32 reg, u32 data)
 {
@@ -115,17 +109,6 @@ static inline u32 hdmi_read(struct hdmi *hdmi, u32 reg)
 	return msm_readl(hdmi->mmio + reg);
 }
 
-static inline struct hdmi * hdmi_reference(struct hdmi *hdmi)
-{
-	kref_get(&hdmi->refcount);
-	return hdmi;
-}
-
-static inline void hdmi_unreference(struct hdmi *hdmi)
-{
-	kref_put(&hdmi->refcount, hdmi_destroy);
-}
-
 /*
  * The phy appears to be different, for example between 8960 and 8x60,
  * so split the phy related functions out and load the correct one at
diff --git a/drivers/gpu/drm/msm/hdmi/hdmi.xml.h b/drivers/gpu/drm/msm/hdmi/hdmi.xml.h
index 76fd0cfc6558..5b0844befbab 100644
--- a/drivers/gpu/drm/msm/hdmi/hdmi.xml.h
+++ b/drivers/gpu/drm/msm/hdmi/hdmi.xml.h
@@ -10,12 +10,12 @@ git clone https://github.com/freedreno/envytools.git
 The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/msm.xml                 (    647 bytes, from 2013-11-30 14:45:35)
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml (   1453 bytes, from 2013-03-31 16:51:27)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20457 bytes, from 2014-08-01 12:22:48)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1615 bytes, from 2014-07-17 15:34:33)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  22517 bytes, from 2014-07-17 15:34:33)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20136 bytes, from 2014-10-31 16:51:39)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1940 bytes, from 2014-10-31 16:51:39)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  23963 bytes, from 2014-10-31 16:51:46)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/dsi.xml             (  11712 bytes, from 2013-08-17 17:13:43)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/sfpb.xml            (    344 bytes, from 2013-08-11 19:26:32)
-- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-08-01 12:23:53)
+- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-10-31 16:48:57)
 - /home/robclark/src/freedreno/envytools/rnndb/hdmi/qfprom.xml         (    600 bytes, from 2013-07-05 19:21:12)
 - /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-07-17 15:33:30)
 
diff --git a/drivers/gpu/drm/msm/hdmi/hdmi_bridge.c b/drivers/gpu/drm/msm/hdmi/hdmi_bridge.c
index f6cf745c249e..6902ad6da710 100644
--- a/drivers/gpu/drm/msm/hdmi/hdmi_bridge.c
+++ b/drivers/gpu/drm/msm/hdmi/hdmi_bridge.c
@@ -26,7 +26,6 @@ struct hdmi_bridge {
 static void hdmi_bridge_destroy(struct drm_bridge *bridge)
 {
 	struct hdmi_bridge *hdmi_bridge = to_hdmi_bridge(bridge);
-	hdmi_unreference(hdmi_bridge->hdmi);
 	drm_bridge_cleanup(bridge);
 	kfree(hdmi_bridge);
 }
@@ -218,7 +217,7 @@ struct drm_bridge *hdmi_bridge_init(struct hdmi *hdmi)
 		goto fail;
 	}
 
-	hdmi_bridge->hdmi = hdmi_reference(hdmi);
+	hdmi_bridge->hdmi = hdmi;
 
 	bridge = &hdmi_bridge->base;
 
diff --git a/drivers/gpu/drm/msm/hdmi/hdmi_connector.c b/drivers/gpu/drm/msm/hdmi/hdmi_connector.c
index 4aca2a3c667c..b4e70e0e3cfa 100644
--- a/drivers/gpu/drm/msm/hdmi/hdmi_connector.c
+++ b/drivers/gpu/drm/msm/hdmi/hdmi_connector.c
@@ -141,6 +141,15 @@ static int hpd_enable(struct hdmi_connector *hdmi_connector)
 	uint32_t hpd_ctrl;
 	int i, ret;
 
+	for (i = 0; i < config->hpd_reg_cnt; i++) {
+		ret = regulator_enable(hdmi->hpd_regs[i]);
+		if (ret) {
+			dev_err(dev->dev, "failed to enable hpd regulator: %s (%d)\n",
+					config->hpd_reg_names[i], ret);
+			goto fail;
+		}
+	}
+
 	ret = gpio_config(hdmi, true);
 	if (ret) {
 		dev_err(dev->dev, "failed to configure GPIOs: %d\n", ret);
@@ -164,15 +173,6 @@ static int hpd_enable(struct hdmi_connector *hdmi_connector)
 		}
 	}
 
-	for (i = 0; i < config->hpd_reg_cnt; i++) {
-		ret = regulator_enable(hdmi->hpd_regs[i]);
-		if (ret) {
-			dev_err(dev->dev, "failed to enable hpd regulator: %s (%d)\n",
-					config->hpd_reg_names[i], ret);
-			goto fail;
-		}
-	}
-
 	hdmi_set_mode(hdmi, false);
 	phy->funcs->reset(phy);
 	hdmi_set_mode(hdmi, true);
@@ -200,7 +200,7 @@ fail:
 	return ret;
 }
 
-static int hdp_disable(struct hdmi_connector *hdmi_connector)
+static void hdp_disable(struct hdmi_connector *hdmi_connector)
 {
 	struct hdmi *hdmi = hdmi_connector->hdmi;
 	const struct hdmi_platform_config *config = hdmi->config;
@@ -212,28 +212,19 @@ static int hdp_disable(struct hdmi_connector *hdmi_connector)
 
 	hdmi_set_mode(hdmi, false);
 
-	for (i = 0; i < config->hpd_reg_cnt; i++) {
-		ret = regulator_disable(hdmi->hpd_regs[i]);
-		if (ret) {
-			dev_err(dev->dev, "failed to disable hpd regulator: %s (%d)\n",
-					config->hpd_reg_names[i], ret);
-			goto fail;
-		}
-	}
-
 	for (i = 0; i < config->hpd_clk_cnt; i++)
 		clk_disable_unprepare(hdmi->hpd_clks[i]);
 
 	ret = gpio_config(hdmi, false);
-	if (ret) {
-		dev_err(dev->dev, "failed to unconfigure GPIOs: %d\n", ret);
-		goto fail;
-	}
+	if (ret)
+		dev_warn(dev->dev, "failed to unconfigure GPIOs: %d\n", ret);
 
-	return 0;
-
-fail:
-	return ret;
+	for (i = 0; i < config->hpd_reg_cnt; i++) {
+		ret = regulator_disable(hdmi->hpd_regs[i]);
+		if (ret)
+			dev_warn(dev->dev, "failed to disable hpd regulator: %s (%d)\n",
+					config->hpd_reg_names[i], ret);
+	}
 }
 
 static void
@@ -260,11 +251,11 @@ void hdmi_connector_irq(struct drm_connector *connector)
 			(hpd_int_status & HDMI_HPD_INT_STATUS_INT)) {
 		bool detected = !!(hpd_int_status & HDMI_HPD_INT_STATUS_CABLE_DETECTED);
 
-		DBG("status=%04x, ctrl=%04x", hpd_int_status, hpd_int_ctrl);
-
-		/* ack the irq: */
+		/* ack & disable (temporarily) HPD events: */
 		hdmi_write(hdmi, REG_HDMI_HPD_INT_CTRL,
-				hpd_int_ctrl | HDMI_HPD_INT_CTRL_INT_ACK);
+			HDMI_HPD_INT_CTRL_INT_ACK);
+
+		DBG("status=%04x, ctrl=%04x", hpd_int_status, hpd_int_ctrl);
 
 		/* detect disconnect if we are connected or visa versa: */
 		hpd_int_ctrl = HDMI_HPD_INT_CTRL_INT_EN;
@@ -330,8 +321,6 @@ static void hdmi_connector_destroy(struct drm_connector *connector)
 	drm_connector_unregister(connector);
 	drm_connector_cleanup(connector);
 
-	hdmi_unreference(hdmi_connector->hdmi);
-
 	kfree(hdmi_connector);
 }
 
@@ -401,6 +390,9 @@ static const struct drm_connector_funcs hdmi_connector_funcs = {
 	.detect = hdmi_connector_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.destroy = hdmi_connector_destroy,
+	.reset = drm_atomic_helper_connector_reset,
+	.atomic_duplicate_state = drm_atomic_helper_connector_duplicate_state,
+	.atomic_destroy_state = drm_atomic_helper_connector_destroy_state,
 };
 
 static const struct drm_connector_helper_funcs hdmi_connector_helper_funcs = {
@@ -422,7 +414,7 @@ struct drm_connector *hdmi_connector_init(struct hdmi *hdmi)
 		goto fail;
 	}
 
-	hdmi_connector->hdmi = hdmi_reference(hdmi);
+	hdmi_connector->hdmi = hdmi;
 	INIT_WORK(&hdmi_connector->hpd_work, hotplug_work);
 
 	connector = &hdmi_connector->base;
diff --git a/drivers/gpu/drm/msm/hdmi/hdmi_phy_8960.c b/drivers/gpu/drm/msm/hdmi/hdmi_phy_8960.c
index f408b69486a8..eeed006eed13 100644
--- a/drivers/gpu/drm/msm/hdmi/hdmi_phy_8960.c
+++ b/drivers/gpu/drm/msm/hdmi/hdmi_phy_8960.c
@@ -510,7 +510,7 @@ struct hdmi_phy *hdmi_phy_8960_init(struct hdmi *hdmi)
 
 #ifdef CONFIG_COMMON_CLK
 	phy_8960->pll_hw.init = &pll_init;
-	phy_8960->pll = devm_clk_register(hdmi->dev->dev, &phy_8960->pll_hw);
+	phy_8960->pll = devm_clk_register(&hdmi->pdev->dev, &phy_8960->pll_hw);
 	if (IS_ERR(phy_8960->pll)) {
 		ret = PTR_ERR(phy_8960->pll);
 		phy_8960->pll = NULL;
diff --git a/drivers/gpu/drm/msm/hdmi/qfprom.xml.h b/drivers/gpu/drm/msm/hdmi/qfprom.xml.h
index d53c29327df9..29bd796797de 100644
--- a/drivers/gpu/drm/msm/hdmi/qfprom.xml.h
+++ b/drivers/gpu/drm/msm/hdmi/qfprom.xml.h
@@ -10,12 +10,12 @@ git clone https://github.com/freedreno/envytools.git
 The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/msm.xml                 (    647 bytes, from 2013-11-30 14:45:35)
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml (   1453 bytes, from 2013-03-31 16:51:27)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20457 bytes, from 2014-08-01 12:22:48)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1615 bytes, from 2014-07-17 15:34:33)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  22517 bytes, from 2014-07-17 15:34:33)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20136 bytes, from 2014-10-31 16:51:39)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1940 bytes, from 2014-10-31 16:51:39)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  23963 bytes, from 2014-10-31 16:51:46)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/dsi.xml             (  11712 bytes, from 2013-08-17 17:13:43)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/sfpb.xml            (    344 bytes, from 2013-08-11 19:26:32)
-- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-08-01 12:23:53)
+- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-10-31 16:48:57)
 - /home/robclark/src/freedreno/envytools/rnndb/hdmi/qfprom.xml         (    600 bytes, from 2013-07-05 19:21:12)
 - /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-07-17 15:33:30)
 
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4.xml.h b/drivers/gpu/drm/msm/mdp/mdp4/mdp4.xml.h
index 03c0bd9cd5b9..a4a7f8c7122a 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4.xml.h
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4.xml.h
@@ -10,12 +10,12 @@ git clone https://github.com/freedreno/envytools.git
 The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/msm.xml                 (    647 bytes, from 2013-11-30 14:45:35)
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml (   1453 bytes, from 2013-03-31 16:51:27)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20457 bytes, from 2014-08-01 12:22:48)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1615 bytes, from 2014-07-17 15:34:33)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  22517 bytes, from 2014-07-17 15:34:33)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20136 bytes, from 2014-10-31 16:51:39)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1940 bytes, from 2014-10-31 16:51:39)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  23963 bytes, from 2014-10-31 16:51:46)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/dsi.xml             (  11712 bytes, from 2013-08-17 17:13:43)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/sfpb.xml            (    344 bytes, from 2013-08-11 19:26:32)
-- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-08-01 12:23:53)
+- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-10-31 16:48:57)
 - /home/robclark/src/freedreno/envytools/rnndb/hdmi/qfprom.xml         (    600 bytes, from 2013-07-05 19:21:12)
 - /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-07-17 15:33:30)
 
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_crtc.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_crtc.c
index 7d00f7fb5773..3449213f1e76 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_crtc.c
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_crtc.c
@@ -25,8 +25,6 @@
 struct mdp4_crtc {
 	struct drm_crtc base;
 	char name[8];
-	struct drm_plane *plane;
-	struct drm_plane *planes[8];
 	int id;
 	int ovlp;
 	enum mdp4_dma dma;
@@ -52,25 +50,11 @@ struct mdp4_crtc {
 
 	/* if there is a pending flip, these will be non-null: */
 	struct drm_pending_vblank_event *event;
-	struct msm_fence_cb pageflip_cb;
 
 #define PENDING_CURSOR 0x1
 #define PENDING_FLIP   0x2
 	atomic_t pending;
 
-	/* the fb that we logically (from PoV of KMS API) hold a ref
-	 * to.  Which we may not yet be scanning out (we may still
-	 * be scanning out previous in case of page_flip while waiting
-	 * for gpu rendering to complete:
-	 */
-	struct drm_framebuffer *fb;
-
-	/* the fb that we currently hold a scanout ref to: */
-	struct drm_framebuffer *scanout_fb;
-
-	/* for unref'ing framebuffers after scanout completes: */
-	struct drm_flip_work unref_fb_work;
-
 	/* for unref'ing cursor bo's after scanout completes: */
 	struct drm_flip_work unref_cursor_work;
 
@@ -97,15 +81,14 @@ static void crtc_flush(struct drm_crtc *crtc)
 {
 	struct mdp4_crtc *mdp4_crtc = to_mdp4_crtc(crtc);
 	struct mdp4_kms *mdp4_kms = get_kms(crtc);
-	uint32_t i, flush = 0;
+	struct drm_plane *plane;
+	uint32_t flush = 0;
 
-	for (i = 0; i < ARRAY_SIZE(mdp4_crtc->planes); i++) {
-		struct drm_plane *plane = mdp4_crtc->planes[i];
-		if (plane) {
-			enum mdp4_pipe pipe_id = mdp4_plane_pipe(plane);
-			flush |= pipe2flush(pipe_id);
-		}
+	drm_atomic_crtc_for_each_plane(plane, crtc) {
+		enum mdp4_pipe pipe_id = mdp4_plane_pipe(plane);
+		flush |= pipe2flush(pipe_id);
 	}
+
 	flush |= ovlp2flush(mdp4_crtc->ovlp);
 
 	DBG("%s: flush=%08x", mdp4_crtc->name, flush);
@@ -113,47 +96,6 @@ static void crtc_flush(struct drm_crtc *crtc)
 	mdp4_write(mdp4_kms, REG_MDP4_OVERLAY_FLUSH, flush);
 }
 
-static void update_fb(struct drm_crtc *crtc, struct drm_framebuffer *new_fb)
-{
-	struct mdp4_crtc *mdp4_crtc = to_mdp4_crtc(crtc);
-	struct drm_framebuffer *old_fb = mdp4_crtc->fb;
-
-	/* grab reference to incoming scanout fb: */
-	drm_framebuffer_reference(new_fb);
-	mdp4_crtc->base.primary->fb = new_fb;
-	mdp4_crtc->fb = new_fb;
-
-	if (old_fb)
-		drm_flip_work_queue(&mdp4_crtc->unref_fb_work, old_fb);
-}
-
-/* unlike update_fb(), take a ref to the new scanout fb *before* updating
- * plane, then call this.  Needed to ensure we don't unref the buffer that
- * is actually still being scanned out.
- *
- * Note that this whole thing goes away with atomic.. since we can defer
- * calling into driver until rendering is done.
- */
-static void update_scanout(struct drm_crtc *crtc, struct drm_framebuffer *fb)
-{
-	struct mdp4_crtc *mdp4_crtc = to_mdp4_crtc(crtc);
-
-	/* flush updates, to make sure hw is updated to new scanout fb,
-	 * so that we can safely queue unref to current fb (ie. next
-	 * vblank we know hw is done w/ previous scanout_fb).
-	 */
-	crtc_flush(crtc);
-
-	if (mdp4_crtc->scanout_fb)
-		drm_flip_work_queue(&mdp4_crtc->unref_fb_work,
-				mdp4_crtc->scanout_fb);
-
-	mdp4_crtc->scanout_fb = fb;
-
-	/* enable vblank to complete flip: */
-	request_pending(crtc, PENDING_FLIP);
-}
-
 /* if file!=NULL, this is preclose potential cancel-flip path */
 static void complete_flip(struct drm_crtc *crtc, struct drm_file *file)
 {
@@ -171,38 +113,13 @@ static void complete_flip(struct drm_crtc *crtc, struct drm_file *file)
 		 */
 		if (!file || (event->base.file_priv == file)) {
 			mdp4_crtc->event = NULL;
+			DBG("%s: send event: %p", mdp4_crtc->name, event);
 			drm_send_vblank_event(dev, mdp4_crtc->id, event);
 		}
 	}
 	spin_unlock_irqrestore(&dev->event_lock, flags);
 }
 
-static void pageflip_cb(struct msm_fence_cb *cb)
-{
-	struct mdp4_crtc *mdp4_crtc =
-		container_of(cb, struct mdp4_crtc, pageflip_cb);
-	struct drm_crtc *crtc = &mdp4_crtc->base;
-	struct drm_framebuffer *fb = crtc->primary->fb;
-
-	if (!fb)
-		return;
-
-	drm_framebuffer_reference(fb);
-	mdp4_plane_set_scanout(mdp4_crtc->plane, fb);
-	update_scanout(crtc, fb);
-}
-
-static void unref_fb_worker(struct drm_flip_work *work, void *val)
-{
-	struct mdp4_crtc *mdp4_crtc =
-		container_of(work, struct mdp4_crtc, unref_fb_work);
-	struct drm_device *dev = mdp4_crtc->base.dev;
-
-	mutex_lock(&dev->mode_config.mutex);
-	drm_framebuffer_unreference(val);
-	mutex_unlock(&dev->mode_config.mutex);
-}
-
 static void unref_cursor_worker(struct drm_flip_work *work, void *val)
 {
 	struct mdp4_crtc *mdp4_crtc =
@@ -218,7 +135,6 @@ static void mdp4_crtc_destroy(struct drm_crtc *crtc)
 	struct mdp4_crtc *mdp4_crtc = to_mdp4_crtc(crtc);
 
 	drm_crtc_cleanup(crtc);
-	drm_flip_work_cleanup(&mdp4_crtc->unref_fb_work);
 	drm_flip_work_cleanup(&mdp4_crtc->unref_cursor_work);
 
 	kfree(mdp4_crtc);
@@ -251,57 +167,70 @@ static bool mdp4_crtc_mode_fixup(struct drm_crtc *crtc,
 	return true;
 }
 
-static void blend_setup(struct drm_crtc *crtc)
+/* statically (for now) map planes to mixer stage (z-order): */
+static const int idxs[] = {
+		[VG1]  = 1,
+		[VG2]  = 2,
+		[RGB1] = 0,
+		[RGB2] = 0,
+		[RGB3] = 0,
+		[VG3]  = 3,
+		[VG4]  = 4,
+
+};
+
+/* setup mixer config, for which we need to consider all crtc's and
+ * the planes attached to them
+ *
+ * TODO may possibly need some extra locking here
+ */
+static void setup_mixer(struct mdp4_kms *mdp4_kms)
 {
-	struct mdp4_crtc *mdp4_crtc = to_mdp4_crtc(crtc);
-	struct mdp4_kms *mdp4_kms = get_kms(crtc);
-	int i, ovlp = mdp4_crtc->ovlp;
+	struct drm_mode_config *config = &mdp4_kms->dev->mode_config;
+	struct drm_crtc *crtc;
 	uint32_t mixer_cfg = 0;
 	static const enum mdp_mixer_stage_id stages[] = {
 			STAGE_BASE, STAGE0, STAGE1, STAGE2, STAGE3,
 	};
-	/* statically (for now) map planes to mixer stage (z-order): */
-	static const int idxs[] = {
-			[VG1]  = 1,
-			[VG2]  = 2,
-			[RGB1] = 0,
-			[RGB2] = 0,
-			[RGB3] = 0,
-			[VG3]  = 3,
-			[VG4]  = 4,
 
-	};
-	bool alpha[4]= { false, false, false, false };
+	list_for_each_entry(crtc, &config->crtc_list, head) {
+		struct mdp4_crtc *mdp4_crtc = to_mdp4_crtc(crtc);
+		struct drm_plane *plane;
 
-	/* Don't rely on value read back from hw, but instead use our
-	 * own shadowed value.  Possibly disable/reenable looses the
-	 * previous value and goes back to power-on default?
-	 */
-	mixer_cfg = mdp4_kms->mixer_cfg;
+		drm_atomic_crtc_for_each_plane(plane, crtc) {
+			enum mdp4_pipe pipe_id = mdp4_plane_pipe(plane);
+			int idx = idxs[pipe_id];
+			mixer_cfg = mixercfg(mixer_cfg, mdp4_crtc->mixer,
+					pipe_id, stages[idx]);
+		}
+	}
+
+	mdp4_write(mdp4_kms, REG_MDP4_LAYERMIXER_IN_CFG, mixer_cfg);
+}
+
+static void blend_setup(struct drm_crtc *crtc)
+{
+	struct mdp4_crtc *mdp4_crtc = to_mdp4_crtc(crtc);
+	struct mdp4_kms *mdp4_kms = get_kms(crtc);
+	struct drm_plane *plane;
+	int i, ovlp = mdp4_crtc->ovlp;
+	bool alpha[4]= { false, false, false, false };
 
 	mdp4_write(mdp4_kms, REG_MDP4_OVLP_TRANSP_LOW0(ovlp), 0);
 	mdp4_write(mdp4_kms, REG_MDP4_OVLP_TRANSP_LOW1(ovlp), 0);
 	mdp4_write(mdp4_kms, REG_MDP4_OVLP_TRANSP_HIGH0(ovlp), 0);
 	mdp4_write(mdp4_kms, REG_MDP4_OVLP_TRANSP_HIGH1(ovlp), 0);
 
-	for (i = 0; i < ARRAY_SIZE(mdp4_crtc->planes); i++) {
-		struct drm_plane *plane = mdp4_crtc->planes[i];
-		if (plane) {
-			enum mdp4_pipe pipe_id = mdp4_plane_pipe(plane);
-			int idx = idxs[pipe_id];
-			if (idx > 0) {
-				const struct mdp_format *format =
+	drm_atomic_crtc_for_each_plane(plane, crtc) {
+		enum mdp4_pipe pipe_id = mdp4_plane_pipe(plane);
+		int idx = idxs[pipe_id];
+		if (idx > 0) {
+			const struct mdp_format *format =
 					to_mdp_format(msm_framebuffer_format(plane->fb));
-				alpha[idx-1] = format->alpha_enable;
-			}
-			mixer_cfg = mixercfg(mixer_cfg, mdp4_crtc->mixer,
-					pipe_id, stages[idx]);
+			alpha[idx-1] = format->alpha_enable;
 		}
 	}
 
-	/* this shouldn't happen.. and seems to cause underflow: */
-	WARN_ON(!mixer_cfg);
-
 	for (i = 0; i < 4; i++) {
 		uint32_t op;
 
@@ -324,22 +253,21 @@ static void blend_setup(struct drm_crtc *crtc)
 		mdp4_write(mdp4_kms, REG_MDP4_OVLP_STAGE_TRANSP_HIGH1(ovlp, i), 0);
 	}
 
-	mdp4_kms->mixer_cfg = mixer_cfg;
-	mdp4_write(mdp4_kms, REG_MDP4_LAYERMIXER_IN_CFG, mixer_cfg);
+	setup_mixer(mdp4_kms);
 }
 
-static int mdp4_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)
+static void mdp4_crtc_mode_set_nofb(struct drm_crtc *crtc)
 {
 	struct mdp4_crtc *mdp4_crtc = to_mdp4_crtc(crtc);
 	struct mdp4_kms *mdp4_kms = get_kms(crtc);
 	enum mdp4_dma dma = mdp4_crtc->dma;
-	int ret, ovlp = mdp4_crtc->ovlp;
+	int ovlp = mdp4_crtc->ovlp;
+	struct drm_display_mode *mode;
 
-	mode = adjusted_mode;
+	if (WARN_ON(!crtc->state))
+		return;
+
+	mode = &crtc->state->adjusted_mode;
 
 	DBG("%s: set mode: %d:\"%s\" %d %d %d %d %d %d %d %d %d %d 0x%x 0x%x",
 			mdp4_crtc->name, mode->base.id, mode->name,
@@ -350,28 +278,13 @@ static int mdp4_crtc_mode_set(struct drm_crtc *crtc,
 			mode->vsync_end, mode->vtotal,
 			mode->type, mode->flags);
 
-	/* grab extra ref for update_scanout() */
-	drm_framebuffer_reference(crtc->primary->fb);
-
-	ret = mdp4_plane_mode_set(mdp4_crtc->plane, crtc, crtc->primary->fb,
-			0, 0, mode->hdisplay, mode->vdisplay,
-			x << 16, y << 16,
-			mode->hdisplay << 16, mode->vdisplay << 16);
-	if (ret) {
-		drm_framebuffer_unreference(crtc->primary->fb);
-		dev_err(crtc->dev->dev, "%s: failed to set mode on plane: %d\n",
-				mdp4_crtc->name, ret);
-		return ret;
-	}
-
 	mdp4_write(mdp4_kms, REG_MDP4_DMA_SRC_SIZE(dma),
 			MDP4_DMA_SRC_SIZE_WIDTH(mode->hdisplay) |
 			MDP4_DMA_SRC_SIZE_HEIGHT(mode->vdisplay));
 
 	/* take data from pipe: */
 	mdp4_write(mdp4_kms, REG_MDP4_DMA_SRC_BASE(dma), 0);
-	mdp4_write(mdp4_kms, REG_MDP4_DMA_SRC_STRIDE(dma),
-			crtc->primary->fb->pitches[0]);
+	mdp4_write(mdp4_kms, REG_MDP4_DMA_SRC_STRIDE(dma), 0);
 	mdp4_write(mdp4_kms, REG_MDP4_DMA_DST_SIZE(dma),
 			MDP4_DMA_DST_SIZE_WIDTH(0) |
 			MDP4_DMA_DST_SIZE_HEIGHT(0));
@@ -380,8 +293,7 @@ static int mdp4_crtc_mode_set(struct drm_crtc *crtc,
 	mdp4_write(mdp4_kms, REG_MDP4_OVLP_SIZE(ovlp),
 			MDP4_OVLP_SIZE_WIDTH(mode->hdisplay) |
 			MDP4_OVLP_SIZE_HEIGHT(mode->vdisplay));
-	mdp4_write(mdp4_kms, REG_MDP4_OVLP_STRIDE(ovlp),
-			crtc->primary->fb->pitches[0]);
+	mdp4_write(mdp4_kms, REG_MDP4_OVLP_STRIDE(ovlp), 0);
 
 	mdp4_write(mdp4_kms, REG_MDP4_OVLP_CFG(ovlp), 1);
 
@@ -390,11 +302,6 @@ static int mdp4_crtc_mode_set(struct drm_crtc *crtc,
 		mdp4_write(mdp4_kms, REG_MDP4_DMA_E_QUANT(1), 0x00ff0000);
 		mdp4_write(mdp4_kms, REG_MDP4_DMA_E_QUANT(2), 0x00ff0000);
 	}
-
-	update_fb(crtc, crtc->primary->fb);
-	update_scanout(crtc, crtc->primary->fb);
-
-	return 0;
 }
 
 static void mdp4_crtc_prepare(struct drm_crtc *crtc)
@@ -416,60 +323,42 @@ static void mdp4_crtc_commit(struct drm_crtc *crtc)
 	drm_crtc_vblank_put(crtc);
 }
 
-static int mdp4_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
-		struct drm_framebuffer *old_fb)
+static void mdp4_crtc_load_lut(struct drm_crtc *crtc)
 {
-	struct mdp4_crtc *mdp4_crtc = to_mdp4_crtc(crtc);
-	struct drm_plane *plane = mdp4_crtc->plane;
-	struct drm_display_mode *mode = &crtc->mode;
-	int ret;
-
-	/* grab extra ref for update_scanout() */
-	drm_framebuffer_reference(crtc->primary->fb);
-
-	ret = mdp4_plane_mode_set(plane, crtc, crtc->primary->fb,
-			0, 0, mode->hdisplay, mode->vdisplay,
-			x << 16, y << 16,
-			mode->hdisplay << 16, mode->vdisplay << 16);
-	if (ret) {
-		drm_framebuffer_unreference(crtc->primary->fb);
-		return ret;
-	}
-
-	update_fb(crtc, crtc->primary->fb);
-	update_scanout(crtc, crtc->primary->fb);
+}
 
+static int mdp4_crtc_atomic_check(struct drm_crtc *crtc,
+		struct drm_crtc_state *state)
+{
+	struct mdp4_crtc *mdp4_crtc = to_mdp4_crtc(crtc);
+	DBG("%s: check", mdp4_crtc->name);
+	// TODO anything else to check?
 	return 0;
 }
 
-static void mdp4_crtc_load_lut(struct drm_crtc *crtc)
+static void mdp4_crtc_atomic_begin(struct drm_crtc *crtc)
 {
+	struct mdp4_crtc *mdp4_crtc = to_mdp4_crtc(crtc);
+	DBG("%s: begin", mdp4_crtc->name);
 }
 
-static int mdp4_crtc_page_flip(struct drm_crtc *crtc,
-		struct drm_framebuffer *new_fb,
-		struct drm_pending_vblank_event *event,
-		uint32_t page_flip_flags)
+static void mdp4_crtc_atomic_flush(struct drm_crtc *crtc)
 {
 	struct mdp4_crtc *mdp4_crtc = to_mdp4_crtc(crtc);
 	struct drm_device *dev = crtc->dev;
-	struct drm_gem_object *obj;
 	unsigned long flags;
 
-	if (mdp4_crtc->event) {
-		dev_err(dev->dev, "already pending flip!\n");
-		return -EBUSY;
-	}
+	DBG("%s: event: %p", mdp4_crtc->name, crtc->state->event);
 
-	obj = msm_framebuffer_bo(new_fb, 0);
+	WARN_ON(mdp4_crtc->event);
 
 	spin_lock_irqsave(&dev->event_lock, flags);
-	mdp4_crtc->event = event;
+	mdp4_crtc->event = crtc->state->event;
 	spin_unlock_irqrestore(&dev->event_lock, flags);
 
-	update_fb(crtc, new_fb);
-
-	return msm_gem_queue_inactive_cb(obj, &mdp4_crtc->pageflip_cb);
+	blend_setup(crtc);
+	crtc_flush(crtc);
+	request_pending(crtc, PENDING_FLIP);
 }
 
 static int mdp4_crtc_set_property(struct drm_crtc *crtc,
@@ -607,22 +496,29 @@ static int mdp4_crtc_cursor_move(struct drm_crtc *crtc, int x, int y)
 }
 
 static const struct drm_crtc_funcs mdp4_crtc_funcs = {
-	.set_config = drm_crtc_helper_set_config,
+	.set_config = drm_atomic_helper_set_config,
 	.destroy = mdp4_crtc_destroy,
-	.page_flip = mdp4_crtc_page_flip,
+	.page_flip = drm_atomic_helper_page_flip,
 	.set_property = mdp4_crtc_set_property,
 	.cursor_set = mdp4_crtc_cursor_set,
 	.cursor_move = mdp4_crtc_cursor_move,
+	.reset = drm_atomic_helper_crtc_reset,
+	.atomic_duplicate_state = drm_atomic_helper_crtc_duplicate_state,
+	.atomic_destroy_state = drm_atomic_helper_crtc_destroy_state,
 };
 
 static const struct drm_crtc_helper_funcs mdp4_crtc_helper_funcs = {
 	.dpms = mdp4_crtc_dpms,
 	.mode_fixup = mdp4_crtc_mode_fixup,
-	.mode_set = mdp4_crtc_mode_set,
+	.mode_set_nofb = mdp4_crtc_mode_set_nofb,
+	.mode_set = drm_helper_crtc_mode_set,
+	.mode_set_base = drm_helper_crtc_mode_set_base,
 	.prepare = mdp4_crtc_prepare,
 	.commit = mdp4_crtc_commit,
-	.mode_set_base = mdp4_crtc_mode_set_base,
 	.load_lut = mdp4_crtc_load_lut,
+	.atomic_check = mdp4_crtc_atomic_check,
+	.atomic_begin = mdp4_crtc_atomic_begin,
+	.atomic_flush = mdp4_crtc_atomic_flush,
 };
 
 static void mdp4_crtc_vblank_irq(struct mdp_irq *irq, uint32_t irqstatus)
@@ -638,7 +534,6 @@ static void mdp4_crtc_vblank_irq(struct mdp_irq *irq, uint32_t irqstatus)
 
 	if (pending & PENDING_FLIP) {
 		complete_flip(crtc, NULL);
-		drm_flip_work_commit(&mdp4_crtc->unref_fb_work, priv->wq);
 	}
 
 	if (pending & PENDING_CURSOR) {
@@ -663,7 +558,8 @@ uint32_t mdp4_crtc_vblank(struct drm_crtc *crtc)
 
 void mdp4_crtc_cancel_pending_flip(struct drm_crtc *crtc, struct drm_file *file)
 {
-	DBG("cancel: %p", file);
+	struct mdp4_crtc *mdp4_crtc = to_mdp4_crtc(crtc);
+	DBG("%s: cancel: %p", mdp4_crtc->name, file);
 	complete_flip(crtc, file);
 }
 
@@ -717,35 +613,6 @@ void mdp4_crtc_set_intf(struct drm_crtc *crtc, enum mdp4_intf intf, int mixer)
 	mdp4_write(mdp4_kms, REG_MDP4_DISP_INTF_SEL, intf_sel);
 }
 
-static void set_attach(struct drm_crtc *crtc, enum mdp4_pipe pipe_id,
-		struct drm_plane *plane)
-{
-	struct mdp4_crtc *mdp4_crtc = to_mdp4_crtc(crtc);
-
-	BUG_ON(pipe_id >= ARRAY_SIZE(mdp4_crtc->planes));
-
-	if (mdp4_crtc->planes[pipe_id] == plane)
-		return;
-
-	mdp4_crtc->planes[pipe_id] = plane;
-	blend_setup(crtc);
-	if (mdp4_crtc->enabled && (plane != mdp4_crtc->plane))
-		crtc_flush(crtc);
-}
-
-void mdp4_crtc_attach(struct drm_crtc *crtc, struct drm_plane *plane)
-{
-	set_attach(crtc, mdp4_plane_pipe(plane), plane);
-}
-
-void mdp4_crtc_detach(struct drm_crtc *crtc, struct drm_plane *plane)
-{
-	/* don't actually detatch our primary plane: */
-	if (to_mdp4_crtc(crtc)->plane == plane)
-		return;
-	set_attach(crtc, mdp4_plane_pipe(plane), NULL);
-}
-
 static const char *dma_names[] = {
 		"DMA_P", "DMA_S", "DMA_E",
 };
@@ -757,17 +624,13 @@ struct drm_crtc *mdp4_crtc_init(struct drm_device *dev,
 {
 	struct drm_crtc *crtc = NULL;
 	struct mdp4_crtc *mdp4_crtc;
-	int ret;
 
 	mdp4_crtc = kzalloc(sizeof(*mdp4_crtc), GFP_KERNEL);
-	if (!mdp4_crtc) {
-		ret = -ENOMEM;
-		goto fail;
-	}
+	if (!mdp4_crtc)
+		return ERR_PTR(-ENOMEM);
 
 	crtc = &mdp4_crtc->base;
 
-	mdp4_crtc->plane = plane;
 	mdp4_crtc->id = id;
 
 	mdp4_crtc->ovlp = ovlp_id;
@@ -784,26 +647,14 @@ struct drm_crtc *mdp4_crtc_init(struct drm_device *dev,
 
 	spin_lock_init(&mdp4_crtc->cursor.lock);
 
-	ret = drm_flip_work_init(&mdp4_crtc->unref_fb_work, 16,
-			"unref fb", unref_fb_worker);
-	if (ret)
-		goto fail;
-
-	ret = drm_flip_work_init(&mdp4_crtc->unref_cursor_work, 64,
+	drm_flip_work_init(&mdp4_crtc->unref_cursor_work,
 			"unref cursor", unref_cursor_worker);
 
-	INIT_FENCE_CB(&mdp4_crtc->pageflip_cb, pageflip_cb);
-
 	drm_crtc_init_with_planes(dev, crtc, plane, NULL, &mdp4_crtc_funcs);
 	drm_crtc_helper_add(crtc, &mdp4_crtc_helper_funcs);
+	plane->crtc = crtc;
 
-	mdp4_plane_install_properties(mdp4_crtc->plane, &crtc->base);
+	mdp4_plane_install_properties(plane, &crtc->base);
 
 	return crtc;
-
-fail:
-	if (crtc)
-		mdp4_crtc_destroy(crtc);
-
-	return ERR_PTR(ret);
 }
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.c
index 79d804e61cc4..a62109e4ae0d 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.c
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.c
@@ -228,7 +228,6 @@ static int modeset_init(struct mdp4_kms *mdp4_kms)
 	struct drm_encoder *encoder;
 	struct drm_connector *connector;
 	struct drm_panel *panel;
-	struct hdmi *hdmi;
 	int ret;
 
 	/* construct non-private planes: */
@@ -326,11 +325,13 @@ static int modeset_init(struct mdp4_kms *mdp4_kms)
 	priv->crtcs[priv->num_crtcs++] = crtc;
 	priv->encoders[priv->num_encoders++] = encoder;
 
-	hdmi = hdmi_init(dev, encoder);
-	if (IS_ERR(hdmi)) {
-		ret = PTR_ERR(hdmi);
-		dev_err(dev->dev, "failed to initialize HDMI: %d\n", ret);
-		goto fail;
+	if (priv->hdmi) {
+		/* Construct bridge/connector for HDMI: */
+		ret = hdmi_modeset_init(priv->hdmi, dev, encoder);
+		if (ret) {
+			dev_err(dev->dev, "failed to initialize HDMI: %d\n", ret);
+			goto fail;
+		}
 	}
 
 	return 0;
@@ -381,6 +382,10 @@ struct msm_kms *mdp4_kms_init(struct drm_device *dev)
 	if (IS_ERR(mdp4_kms->dsi_pll_vddio))
 		mdp4_kms->dsi_pll_vddio = NULL;
 
+	/* NOTE: driver for this regulator still missing upstream.. use
+	 * _get_exclusive() and ignore the error if it does not exist
+	 * (and hope that the bootloader left it on for us)
+	 */
 	mdp4_kms->vdd = devm_regulator_get_exclusive(&pdev->dev, "vdd");
 	if (IS_ERR(mdp4_kms->vdd))
 		mdp4_kms->vdd = NULL;
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.h b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.h
index 9ff6e7ccfe90..cbd77bc626d5 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.h
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.h
@@ -32,13 +32,6 @@ struct mdp4_kms {
 
 	int rev;
 
-	/* Shadow value for MDP4_LAYERMIXER_IN_CFG.. since setup for all
-	 * crtcs/encoders is in one shared register, we need to update it
-	 * via read/modify/write.  But to avoid getting confused by power-
-	 * on-default values after resume, use this shadow value instead:
-	 */
-	uint32_t mixer_cfg;
-
 	/* mapper-id used to request GEM buffer mapped for scanout: */
 	int id;
 
@@ -194,14 +187,6 @@ uint32_t mdp4_get_formats(enum mdp4_pipe pipe_id, uint32_t *pixel_formats,
 
 void mdp4_plane_install_properties(struct drm_plane *plane,
 		struct drm_mode_object *obj);
-void mdp4_plane_set_scanout(struct drm_plane *plane,
-		struct drm_framebuffer *fb);
-int mdp4_plane_mode_set(struct drm_plane *plane,
-		struct drm_crtc *crtc, struct drm_framebuffer *fb,
-		int crtc_x, int crtc_y,
-		unsigned int crtc_w, unsigned int crtc_h,
-		uint32_t src_x, uint32_t src_y,
-		uint32_t src_w, uint32_t src_h);
 enum mdp4_pipe mdp4_plane_pipe(struct drm_plane *plane);
 struct drm_plane *mdp4_plane_init(struct drm_device *dev,
 		enum mdp4_pipe pipe_id, bool private_plane);
@@ -210,8 +195,6 @@ uint32_t mdp4_crtc_vblank(struct drm_crtc *crtc);
 void mdp4_crtc_cancel_pending_flip(struct drm_crtc *crtc, struct drm_file *file);
 void mdp4_crtc_set_config(struct drm_crtc *crtc, uint32_t config);
 void mdp4_crtc_set_intf(struct drm_crtc *crtc, enum mdp4_intf intf, int mixer);
-void mdp4_crtc_attach(struct drm_crtc *crtc, struct drm_plane *plane);
-void mdp4_crtc_detach(struct drm_crtc *crtc, struct drm_plane *plane);
 struct drm_crtc *mdp4_crtc_init(struct drm_device *dev,
 		struct drm_plane *plane, int id, int ovlp_id,
 		enum mdp4_dma dma_id);
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lvds_connector.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lvds_connector.c
index 310034688c15..4ddc28e1275b 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lvds_connector.c
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lvds_connector.c
@@ -98,6 +98,9 @@ static const struct drm_connector_funcs mdp4_lvds_connector_funcs = {
 	.detect = mdp4_lvds_connector_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.destroy = mdp4_lvds_connector_destroy,
+	.reset = drm_atomic_helper_connector_reset,
+	.atomic_duplicate_state = drm_atomic_helper_connector_duplicate_state,
+	.atomic_destroy_state = drm_atomic_helper_connector_destroy_state,
 };
 
 static const struct drm_connector_helper_funcs mdp4_lvds_connector_helper_funcs = {
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_plane.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_plane.c
index 66f33dba1ebb..1e5ebe83647d 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_plane.c
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_plane.c
@@ -31,47 +31,26 @@ struct mdp4_plane {
 };
 #define to_mdp4_plane(x) container_of(x, struct mdp4_plane, base)
 
-static struct mdp4_kms *get_kms(struct drm_plane *plane)
-{
-	struct msm_drm_private *priv = plane->dev->dev_private;
-	return to_mdp4_kms(to_mdp_kms(priv->kms));
-}
-
-static int mdp4_plane_update(struct drm_plane *plane,
+static void mdp4_plane_set_scanout(struct drm_plane *plane,
+		struct drm_framebuffer *fb);
+static int mdp4_plane_mode_set(struct drm_plane *plane,
 		struct drm_crtc *crtc, struct drm_framebuffer *fb,
 		int crtc_x, int crtc_y,
 		unsigned int crtc_w, unsigned int crtc_h,
 		uint32_t src_x, uint32_t src_y,
-		uint32_t src_w, uint32_t src_h)
-{
-	struct mdp4_plane *mdp4_plane = to_mdp4_plane(plane);
-
-	mdp4_plane->enabled = true;
-
-	if (plane->fb)
-		drm_framebuffer_unreference(plane->fb);
-
-	drm_framebuffer_reference(fb);
-
-	return mdp4_plane_mode_set(plane, crtc, fb,
-			crtc_x, crtc_y, crtc_w, crtc_h,
-			src_x, src_y, src_w, src_h);
-}
+		uint32_t src_w, uint32_t src_h);
 
-static int mdp4_plane_disable(struct drm_plane *plane)
+static struct mdp4_kms *get_kms(struct drm_plane *plane)
 {
-	struct mdp4_plane *mdp4_plane = to_mdp4_plane(plane);
-	DBG("%s: disable", mdp4_plane->name);
-	if (plane->crtc)
-		mdp4_crtc_detach(plane->crtc, plane);
-	return 0;
+	struct msm_drm_private *priv = plane->dev->dev_private;
+	return to_mdp4_kms(to_mdp_kms(priv->kms));
 }
 
 static void mdp4_plane_destroy(struct drm_plane *plane)
 {
 	struct mdp4_plane *mdp4_plane = to_mdp4_plane(plane);
 
-	mdp4_plane_disable(plane);
+	drm_plane_helper_disable(plane);
 	drm_plane_cleanup(plane);
 
 	kfree(mdp4_plane);
@@ -92,19 +71,75 @@ int mdp4_plane_set_property(struct drm_plane *plane,
 }
 
 static const struct drm_plane_funcs mdp4_plane_funcs = {
-		.update_plane = mdp4_plane_update,
-		.disable_plane = mdp4_plane_disable,
+		.update_plane = drm_atomic_helper_update_plane,
+		.disable_plane = drm_atomic_helper_disable_plane,
 		.destroy = mdp4_plane_destroy,
 		.set_property = mdp4_plane_set_property,
+		.reset = drm_atomic_helper_plane_reset,
+		.atomic_duplicate_state = drm_atomic_helper_plane_duplicate_state,
+		.atomic_destroy_state = drm_atomic_helper_plane_destroy_state,
 };
 
-void mdp4_plane_set_scanout(struct drm_plane *plane,
+static int mdp4_plane_prepare_fb(struct drm_plane *plane,
+		struct drm_framebuffer *fb)
+{
+	struct mdp4_plane *mdp4_plane = to_mdp4_plane(plane);
+	struct mdp4_kms *mdp4_kms = get_kms(plane);
+
+	DBG("%s: prepare: FB[%u]", mdp4_plane->name, fb->base.id);
+	return msm_framebuffer_prepare(fb, mdp4_kms->id);
+}
+
+static void mdp4_plane_cleanup_fb(struct drm_plane *plane,
+		struct drm_framebuffer *fb)
+{
+	struct mdp4_plane *mdp4_plane = to_mdp4_plane(plane);
+	struct mdp4_kms *mdp4_kms = get_kms(plane);
+
+	DBG("%s: cleanup: FB[%u]", mdp4_plane->name, fb->base.id);
+	msm_framebuffer_cleanup(fb, mdp4_kms->id);
+}
+
+
+static int mdp4_plane_atomic_check(struct drm_plane *plane,
+		struct drm_plane_state *state)
+{
+	return 0;
+}
+
+static void mdp4_plane_atomic_update(struct drm_plane *plane,
+				     struct drm_plane_state *old_state)
+{
+	struct drm_plane_state *state = plane->state;
+	int ret;
+
+	ret = mdp4_plane_mode_set(plane,
+			state->crtc, state->fb,
+			state->crtc_x, state->crtc_y,
+			state->crtc_w, state->crtc_h,
+			state->src_x,  state->src_y,
+			state->src_w, state->src_h);
+	/* atomic_check should have ensured that this doesn't fail */
+	WARN_ON(ret < 0);
+}
+
+static const struct drm_plane_helper_funcs mdp4_plane_helper_funcs = {
+		.prepare_fb = mdp4_plane_prepare_fb,
+		.cleanup_fb = mdp4_plane_cleanup_fb,
+		.atomic_check = mdp4_plane_atomic_check,
+		.atomic_update = mdp4_plane_atomic_update,
+};
+
+static void mdp4_plane_set_scanout(struct drm_plane *plane,
 		struct drm_framebuffer *fb)
 {
 	struct mdp4_plane *mdp4_plane = to_mdp4_plane(plane);
 	struct mdp4_kms *mdp4_kms = get_kms(plane);
 	enum mdp4_pipe pipe = mdp4_plane->pipe;
-	uint32_t iova;
+	uint32_t iova = msm_framebuffer_iova(fb, mdp4_kms->id, 0);
+
+	DBG("%s: set_scanout: %08x (%u)", mdp4_plane->name,
+			iova, fb->pitches[0]);
 
 	mdp4_write(mdp4_kms, REG_MDP4_PIPE_SRC_STRIDE_A(pipe),
 			MDP4_PIPE_SRC_STRIDE_A_P0(fb->pitches[0]) |
@@ -114,7 +149,6 @@ void mdp4_plane_set_scanout(struct drm_plane *plane,
 			MDP4_PIPE_SRC_STRIDE_B_P2(fb->pitches[2]) |
 			MDP4_PIPE_SRC_STRIDE_B_P3(fb->pitches[3]));
 
-	msm_gem_get_iova(msm_framebuffer_bo(fb, 0), mdp4_kms->id, &iova);
 	mdp4_write(mdp4_kms, REG_MDP4_PIPE_SRCP0_BASE(pipe), iova);
 
 	plane->fb = fb;
@@ -122,7 +156,7 @@ void mdp4_plane_set_scanout(struct drm_plane *plane,
 
 #define MDP4_VG_PHASE_STEP_DEFAULT	0x20000000
 
-int mdp4_plane_mode_set(struct drm_plane *plane,
+static int mdp4_plane_mode_set(struct drm_plane *plane,
 		struct drm_crtc *crtc, struct drm_framebuffer *fb,
 		int crtc_x, int crtc_y,
 		unsigned int crtc_w, unsigned int crtc_h,
@@ -137,6 +171,11 @@ int mdp4_plane_mode_set(struct drm_plane *plane,
 	uint32_t phasex_step = MDP4_VG_PHASE_STEP_DEFAULT;
 	uint32_t phasey_step = MDP4_VG_PHASE_STEP_DEFAULT;
 
+	if (!(crtc && fb)) {
+		DBG("%s: disabled!", mdp4_plane->name);
+		return 0;
+	}
+
 	/* src values are in Q16 fixed point, convert to integer: */
 	src_x = src_x >> 16;
 	src_y = src_y >> 16;
@@ -197,9 +236,6 @@ int mdp4_plane_mode_set(struct drm_plane *plane,
 	mdp4_write(mdp4_kms, REG_MDP4_PIPE_PHASEX_STEP(pipe), phasex_step);
 	mdp4_write(mdp4_kms, REG_MDP4_PIPE_PHASEY_STEP(pipe), phasey_step);
 
-	/* TODO detach from old crtc (if we had more than one) */
-	mdp4_crtc_attach(crtc, plane);
-
 	return 0;
 }
 
@@ -239,9 +275,12 @@ struct drm_plane *mdp4_plane_init(struct drm_device *dev,
 			ARRAY_SIZE(mdp4_plane->formats));
 
 	type = private_plane ? DRM_PLANE_TYPE_PRIMARY : DRM_PLANE_TYPE_OVERLAY;
-	drm_universal_plane_init(dev, plane, 0xff, &mdp4_plane_funcs,
-				 mdp4_plane->formats, mdp4_plane->nformats,
-				 type);
+	ret = drm_universal_plane_init(dev, plane, 0xff, &mdp4_plane_funcs,
+				 mdp4_plane->formats, mdp4_plane->nformats, type);
+	if (ret)
+		goto fail;
+
+	drm_plane_helper_add(plane, &mdp4_plane_helper_funcs);
 
 	mdp4_plane_install_properties(plane, &plane->base);
 
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5.xml.h b/drivers/gpu/drm/msm/mdp/mdp5/mdp5.xml.h
index 67f4f896ba8c..e87ef5512cb0 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5.xml.h
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5.xml.h
@@ -10,14 +10,14 @@ git clone https://github.com/freedreno/envytools.git
 The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/msm.xml                 (    647 bytes, from 2013-11-30 14:45:35)
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml (   1453 bytes, from 2013-03-31 16:51:27)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  17996 bytes, from 2013-12-01 19:10:31)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1615 bytes, from 2013-11-30 15:00:52)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  22517 bytes, from 2014-06-25 12:55:02)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20136 bytes, from 2014-10-31 16:51:39)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1940 bytes, from 2014-10-31 16:51:39)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  23963 bytes, from 2014-10-31 16:51:46)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/dsi.xml             (  11712 bytes, from 2013-08-17 17:13:43)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/sfpb.xml            (    344 bytes, from 2013-08-11 19:26:32)
-- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1544 bytes, from 2013-08-16 19:17:05)
+- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-10-31 16:48:57)
 - /home/robclark/src/freedreno/envytools/rnndb/hdmi/qfprom.xml         (    600 bytes, from 2013-07-05 19:21:12)
-- /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-06-25 12:53:44)
+- /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-07-17 15:33:30)
 
 Copyright (C) 2013-2014 by the following authors:
 - Rob Clark <robdclark@gmail.com> (robclark)
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_cfg.c b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_cfg.c
new file mode 100644
index 000000000000..b0a44310cf2a
--- /dev/null
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_cfg.c
@@ -0,0 +1,207 @@
+/*
+ * Copyright (c) 2014 The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "mdp5_kms.h"
+#include "mdp5_cfg.h"
+
+struct mdp5_cfg_handler {
+	int revision;
+	struct mdp5_cfg config;
+};
+
+/* mdp5_cfg must be exposed (used in mdp5.xml.h) */
+const struct mdp5_cfg_hw *mdp5_cfg = NULL;
+
+const struct mdp5_cfg_hw msm8x74_config = {
+	.name = "msm8x74",
+	.smp = {
+		.mmb_count = 22,
+		.mmb_size = 4096,
+	},
+	.ctl = {
+		.count = 5,
+		.base = { 0x00600, 0x00700, 0x00800, 0x00900, 0x00a00 },
+	},
+	.pipe_vig = {
+		.count = 3,
+		.base = { 0x01200, 0x01600, 0x01a00 },
+	},
+	.pipe_rgb = {
+		.count = 3,
+		.base = { 0x01e00, 0x02200, 0x02600 },
+	},
+	.pipe_dma = {
+		.count = 2,
+		.base = { 0x02a00, 0x02e00 },
+	},
+	.lm = {
+		.count = 5,
+		.base = { 0x03200, 0x03600, 0x03a00, 0x03e00, 0x04200 },
+		.nb_stages = 5,
+	},
+	.dspp = {
+		.count = 3,
+		.base = { 0x04600, 0x04a00, 0x04e00 },
+	},
+	.ad = {
+		.count = 2,
+		.base = { 0x13100, 0x13300 }, /* NOTE: no ad in v1.0 */
+	},
+	.intf = {
+		.count = 4,
+		.base = { 0x12500, 0x12700, 0x12900, 0x12b00 },
+	},
+	.max_clk = 200000000,
+};
+
+const struct mdp5_cfg_hw apq8084_config = {
+	.name = "apq8084",
+	.smp = {
+		.mmb_count = 44,
+		.mmb_size = 8192,
+		.reserved_state[0] = GENMASK(7, 0),	/* first 8 MMBs */
+		.reserved[CID_RGB0] = 2,
+		.reserved[CID_RGB1] = 2,
+		.reserved[CID_RGB2] = 2,
+		.reserved[CID_RGB3] = 2,
+	},
+	.ctl = {
+		.count = 5,
+		.base = { 0x00600, 0x00700, 0x00800, 0x00900, 0x00a00 },
+	},
+	.pipe_vig = {
+		.count = 4,
+		.base = { 0x01200, 0x01600, 0x01a00, 0x01e00 },
+	},
+	.pipe_rgb = {
+		.count = 4,
+		.base = { 0x02200, 0x02600, 0x02a00, 0x02e00 },
+	},
+	.pipe_dma = {
+		.count = 2,
+		.base = { 0x03200, 0x03600 },
+	},
+	.lm = {
+		.count = 6,
+		.base = { 0x03a00, 0x03e00, 0x04200, 0x04600, 0x04a00, 0x04e00 },
+		.nb_stages = 5,
+	},
+	.dspp = {
+		.count = 4,
+		.base = { 0x05200, 0x05600, 0x05a00, 0x05e00 },
+
+	},
+	.ad = {
+		.count = 3,
+		.base = { 0x13500, 0x13700, 0x13900 },
+	},
+	.intf = {
+		.count = 5,
+		.base = { 0x12500, 0x12700, 0x12900, 0x12b00, 0x12d00 },
+	},
+	.max_clk = 320000000,
+};
+
+static const struct mdp5_cfg_handler cfg_handlers[] = {
+	{ .revision = 0, .config = { .hw = &msm8x74_config } },
+	{ .revision = 2, .config = { .hw = &msm8x74_config } },
+	{ .revision = 3, .config = { .hw = &apq8084_config } },
+};
+
+
+static struct mdp5_cfg_platform *mdp5_get_config(struct platform_device *dev);
+
+const struct mdp5_cfg_hw *mdp5_cfg_get_hw_config(struct mdp5_cfg_handler *cfg_handler)
+{
+	return cfg_handler->config.hw;
+}
+
+struct mdp5_cfg *mdp5_cfg_get_config(struct mdp5_cfg_handler *cfg_handler)
+{
+	return &cfg_handler->config;
+}
+
+int mdp5_cfg_get_hw_rev(struct mdp5_cfg_handler *cfg_handler)
+{
+	return cfg_handler->revision;
+}
+
+void mdp5_cfg_destroy(struct mdp5_cfg_handler *cfg_handler)
+{
+	kfree(cfg_handler);
+}
+
+struct mdp5_cfg_handler *mdp5_cfg_init(struct mdp5_kms *mdp5_kms,
+		uint32_t major, uint32_t minor)
+{
+	struct drm_device *dev = mdp5_kms->dev;
+	struct platform_device *pdev = dev->platformdev;
+	struct mdp5_cfg_handler *cfg_handler;
+	struct mdp5_cfg_platform *pconfig;
+	int i, ret = 0;
+
+	cfg_handler = kzalloc(sizeof(*cfg_handler), GFP_KERNEL);
+	if (unlikely(!cfg_handler)) {
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	if (major != 1) {
+		dev_err(dev->dev, "unexpected MDP major version: v%d.%d\n",
+				major, minor);
+		ret = -ENXIO;
+		goto fail;
+	}
+
+	/* only after mdp5_cfg global pointer's init can we access the hw */
+	for (i = 0; i < ARRAY_SIZE(cfg_handlers); i++) {
+		if (cfg_handlers[i].revision != minor)
+			continue;
+		mdp5_cfg = cfg_handlers[i].config.hw;
+
+		break;
+	}
+	if (unlikely(!mdp5_cfg)) {
+		dev_err(dev->dev, "unexpected MDP minor revision: v%d.%d\n",
+				major, minor);
+		ret = -ENXIO;
+		goto fail;
+	}
+
+	cfg_handler->revision = minor;
+	cfg_handler->config.hw = mdp5_cfg;
+
+	pconfig = mdp5_get_config(pdev);
+	memcpy(&cfg_handler->config.platform, pconfig, sizeof(*pconfig));
+
+	DBG("MDP5: %s hw config selected", mdp5_cfg->name);
+
+	return cfg_handler;
+
+fail:
+	if (cfg_handler)
+		mdp5_cfg_destroy(cfg_handler);
+
+	return NULL;
+}
+
+static struct mdp5_cfg_platform *mdp5_get_config(struct platform_device *dev)
+{
+	static struct mdp5_cfg_platform config = {};
+#ifdef CONFIG_OF
+	/* TODO */
+#endif
+	config.iommu = iommu_domain_alloc(&platform_bus_type);
+
+	return &config;
+}
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_cfg.h b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_cfg.h
new file mode 100644
index 000000000000..dba4d52cceeb
--- /dev/null
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_cfg.h
@@ -0,0 +1,91 @@
+/*
+ * Copyright (c) 2014 The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __MDP5_CFG_H__
+#define __MDP5_CFG_H__
+
+#include "msm_drv.h"
+
+/*
+ * mdp5_cfg
+ *
+ * This module configures the dynamic offsets used by mdp5.xml.h
+ * (initialized in mdp5_cfg.c)
+ */
+extern const struct mdp5_cfg_hw *mdp5_cfg;
+
+#define MAX_CTL			8
+#define MAX_BASES		8
+#define MAX_SMP_BLOCKS		44
+#define MAX_CLIENTS		32
+
+typedef DECLARE_BITMAP(mdp5_smp_state_t, MAX_SMP_BLOCKS);
+
+#define MDP5_SUB_BLOCK_DEFINITION \
+	int count; \
+	uint32_t base[MAX_BASES]
+
+struct mdp5_sub_block {
+	MDP5_SUB_BLOCK_DEFINITION;
+};
+
+struct mdp5_lm_block {
+	MDP5_SUB_BLOCK_DEFINITION;
+	uint32_t nb_stages;		/* number of stages per blender */
+};
+
+struct mdp5_smp_block {
+	int mmb_count;			/* number of SMP MMBs */
+	int mmb_size;			/* MMB: size in bytes */
+	mdp5_smp_state_t reserved_state;/* SMP MMBs statically allocated */
+	int reserved[MAX_CLIENTS];	/* # of MMBs allocated per client */
+};
+
+struct mdp5_cfg_hw {
+	char  *name;
+
+	struct mdp5_smp_block smp;
+	struct mdp5_sub_block ctl;
+	struct mdp5_sub_block pipe_vig;
+	struct mdp5_sub_block pipe_rgb;
+	struct mdp5_sub_block pipe_dma;
+	struct mdp5_lm_block  lm;
+	struct mdp5_sub_block dspp;
+	struct mdp5_sub_block ad;
+	struct mdp5_sub_block intf;
+
+	uint32_t max_clk;
+};
+
+/* platform config data (ie. from DT, or pdata) */
+struct mdp5_cfg_platform {
+	struct iommu_domain *iommu;
+};
+
+struct mdp5_cfg {
+	const struct mdp5_cfg_hw *hw;
+	struct mdp5_cfg_platform platform;
+};
+
+struct mdp5_kms;
+struct mdp5_cfg_handler;
+
+const struct mdp5_cfg_hw *mdp5_cfg_get_hw_config(struct mdp5_cfg_handler *cfg_hnd);
+struct mdp5_cfg *mdp5_cfg_get_config(struct mdp5_cfg_handler *cfg_hnd);
+int mdp5_cfg_get_hw_rev(struct mdp5_cfg_handler *cfg_hnd);
+
+struct mdp5_cfg_handler *mdp5_cfg_init(struct mdp5_kms *mdp5_kms,
+		uint32_t major, uint32_t minor);
+void mdp5_cfg_destroy(struct mdp5_cfg_handler *cfg_hnd);
+
+#endif /* __MDP5_CFG_H__ */
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_crtc.c b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_crtc.c
index ebe2e60f3ab1..f021f960a8a2 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_crtc.c
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_crtc.c
@@ -1,4 +1,5 @@
 /*
+ * Copyright (c) 2014 The Linux Foundation. All rights reserved.
  * Copyright (C) 2013 Red Hat
  * Author: Rob Clark <robdclark@gmail.com>
  *
@@ -17,43 +18,35 @@
 
 #include "mdp5_kms.h"
 
+#include <linux/sort.h>
 #include <drm/drm_mode.h>
 #include "drm_crtc.h"
 #include "drm_crtc_helper.h"
 #include "drm_flip_work.h"
 
+#define SSPP_MAX	(SSPP_RGB3 + 1) /* TODO: Add SSPP_MAX in mdp5.xml.h */
+
 struct mdp5_crtc {
 	struct drm_crtc base;
 	char name[8];
-	struct drm_plane *plane;
-	struct drm_plane *planes[8];
 	int id;
 	bool enabled;
 
-	/* which mixer/encoder we route output to: */
-	int mixer;
+	/* layer mixer used for this CRTC (+ its lock): */
+#define GET_LM_ID(crtc_id)	((crtc_id == 3) ? 5 : crtc_id)
+	int lm;
+	spinlock_t lm_lock;	/* protect REG_MDP5_LM_* registers */
+
+	/* CTL used for this CRTC: */
+	struct mdp5_ctl *ctl;
 
 	/* if there is a pending flip, these will be non-null: */
 	struct drm_pending_vblank_event *event;
-	struct msm_fence_cb pageflip_cb;
 
 #define PENDING_CURSOR 0x1
 #define PENDING_FLIP   0x2
 	atomic_t pending;
 
-	/* the fb that we logically (from PoV of KMS API) hold a ref
-	 * to.  Which we may not yet be scanning out (we may still
-	 * be scanning out previous in case of page_flip while waiting
-	 * for gpu rendering to complete:
-	 */
-	struct drm_framebuffer *fb;
-
-	/* the fb that we currently hold a scanout ref to: */
-	struct drm_framebuffer *scanout_fb;
-
-	/* for unref'ing framebuffers after scanout completes: */
-	struct drm_flip_work unref_fb_work;
-
 	struct mdp_irq vblank;
 	struct mdp_irq err;
 };
@@ -73,67 +66,38 @@ static void request_pending(struct drm_crtc *crtc, uint32_t pending)
 	mdp_irq_register(&get_kms(crtc)->base, &mdp5_crtc->vblank);
 }
 
-static void crtc_flush(struct drm_crtc *crtc)
-{
-	struct mdp5_crtc *mdp5_crtc = to_mdp5_crtc(crtc);
-	struct mdp5_kms *mdp5_kms = get_kms(crtc);
-	int id = mdp5_crtc->id;
-	uint32_t i, flush = 0;
-
-	for (i = 0; i < ARRAY_SIZE(mdp5_crtc->planes); i++) {
-		struct drm_plane *plane = mdp5_crtc->planes[i];
-		if (plane) {
-			enum mdp5_pipe pipe = mdp5_plane_pipe(plane);
-			flush |= pipe2flush(pipe);
-		}
-	}
-	flush |= mixer2flush(mdp5_crtc->id);
-	flush |= MDP5_CTL_FLUSH_CTL;
-
-	DBG("%s: flush=%08x", mdp5_crtc->name, flush);
-
-	mdp5_write(mdp5_kms, REG_MDP5_CTL_FLUSH(id), flush);
-}
+#define mdp5_lm_get_flush(lm)	mdp_ctl_flush_mask_lm(lm)
 
-static void update_fb(struct drm_crtc *crtc, struct drm_framebuffer *new_fb)
+static void crtc_flush(struct drm_crtc *crtc, u32 flush_mask)
 {
 	struct mdp5_crtc *mdp5_crtc = to_mdp5_crtc(crtc);
-	struct drm_framebuffer *old_fb = mdp5_crtc->fb;
 
-	/* grab reference to incoming scanout fb: */
-	drm_framebuffer_reference(new_fb);
-	mdp5_crtc->base.primary->fb = new_fb;
-	mdp5_crtc->fb = new_fb;
-
-	if (old_fb)
-		drm_flip_work_queue(&mdp5_crtc->unref_fb_work, old_fb);
+	DBG("%s: flush=%08x", mdp5_crtc->name, flush_mask);
+	mdp5_ctl_commit(mdp5_crtc->ctl, flush_mask);
 }
 
-/* unlike update_fb(), take a ref to the new scanout fb *before* updating
- * plane, then call this.  Needed to ensure we don't unref the buffer that
- * is actually still being scanned out.
- *
- * Note that this whole thing goes away with atomic.. since we can defer
- * calling into driver until rendering is done.
+/*
+ * flush updates, to make sure hw is updated to new scanout fb,
+ * so that we can safely queue unref to current fb (ie. next
+ * vblank we know hw is done w/ previous scanout_fb).
  */
-static void update_scanout(struct drm_crtc *crtc, struct drm_framebuffer *fb)
+static void crtc_flush_all(struct drm_crtc *crtc)
 {
 	struct mdp5_crtc *mdp5_crtc = to_mdp5_crtc(crtc);
+	struct drm_plane *plane;
+	uint32_t flush_mask = 0;
 
-	/* flush updates, to make sure hw is updated to new scanout fb,
-	 * so that we can safely queue unref to current fb (ie. next
-	 * vblank we know hw is done w/ previous scanout_fb).
-	 */
-	crtc_flush(crtc);
-
-	if (mdp5_crtc->scanout_fb)
-		drm_flip_work_queue(&mdp5_crtc->unref_fb_work,
-				mdp5_crtc->scanout_fb);
+	/* we could have already released CTL in the disable path: */
+	if (!mdp5_crtc->ctl)
+		return;
 
-	mdp5_crtc->scanout_fb = fb;
+	drm_atomic_crtc_for_each_plane(plane, crtc) {
+		flush_mask |= mdp5_plane_get_flush(plane);
+	}
+	flush_mask |= mdp5_ctl_get_flush(mdp5_crtc->ctl);
+	flush_mask |= mdp5_lm_get_flush(mdp5_crtc->lm);
 
-	/* enable vblank to complete flip: */
-	request_pending(crtc, PENDING_FLIP);
+	crtc_flush(crtc, flush_mask);
 }
 
 /* if file!=NULL, this is preclose potential cancel-flip path */
@@ -142,7 +106,8 @@ static void complete_flip(struct drm_crtc *crtc, struct drm_file *file)
 	struct mdp5_crtc *mdp5_crtc = to_mdp5_crtc(crtc);
 	struct drm_device *dev = crtc->dev;
 	struct drm_pending_vblank_event *event;
-	unsigned long flags, i;
+	struct drm_plane *plane;
+	unsigned long flags;
 
 	spin_lock_irqsave(&dev->event_lock, flags);
 	event = mdp5_crtc->event;
@@ -153,50 +118,22 @@ static void complete_flip(struct drm_crtc *crtc, struct drm_file *file)
 		 */
 		if (!file || (event->base.file_priv == file)) {
 			mdp5_crtc->event = NULL;
+			DBG("%s: send event: %p", mdp5_crtc->name, event);
 			drm_send_vblank_event(dev, mdp5_crtc->id, event);
 		}
 	}
 	spin_unlock_irqrestore(&dev->event_lock, flags);
 
-	for (i = 0; i < ARRAY_SIZE(mdp5_crtc->planes); i++) {
-		struct drm_plane *plane = mdp5_crtc->planes[i];
-		if (plane)
-			mdp5_plane_complete_flip(plane);
+	drm_atomic_crtc_for_each_plane(plane, crtc) {
+		mdp5_plane_complete_flip(plane);
 	}
 }
 
-static void pageflip_cb(struct msm_fence_cb *cb)
-{
-	struct mdp5_crtc *mdp5_crtc =
-		container_of(cb, struct mdp5_crtc, pageflip_cb);
-	struct drm_crtc *crtc = &mdp5_crtc->base;
-	struct drm_framebuffer *fb = mdp5_crtc->fb;
-
-	if (!fb)
-		return;
-
-	drm_framebuffer_reference(fb);
-	mdp5_plane_set_scanout(mdp5_crtc->plane, fb);
-	update_scanout(crtc, fb);
-}
-
-static void unref_fb_worker(struct drm_flip_work *work, void *val)
-{
-	struct mdp5_crtc *mdp5_crtc =
-		container_of(work, struct mdp5_crtc, unref_fb_work);
-	struct drm_device *dev = mdp5_crtc->base.dev;
-
-	mutex_lock(&dev->mode_config.mutex);
-	drm_framebuffer_unreference(val);
-	mutex_unlock(&dev->mode_config.mutex);
-}
-
 static void mdp5_crtc_destroy(struct drm_crtc *crtc)
 {
 	struct mdp5_crtc *mdp5_crtc = to_mdp5_crtc(crtc);
 
 	drm_crtc_cleanup(crtc);
-	drm_flip_work_cleanup(&mdp5_crtc->unref_fb_work);
 
 	kfree(mdp5_crtc);
 }
@@ -214,6 +151,8 @@ static void mdp5_crtc_dpms(struct drm_crtc *crtc, int mode)
 			mdp5_enable(mdp5_kms);
 			mdp_irq_register(&mdp5_kms->base, &mdp5_crtc->err);
 		} else {
+			/* set STAGE_UNUSED for all layers */
+			mdp5_ctl_blend(mdp5_crtc->ctl, mdp5_crtc->lm, 0x00000000);
 			mdp_irq_unregister(&mdp5_kms->base, &mdp5_crtc->err);
 			mdp5_disable(mdp5_kms);
 		}
@@ -228,54 +167,78 @@ static bool mdp5_crtc_mode_fixup(struct drm_crtc *crtc,
 	return true;
 }
 
+/*
+ * blend_setup() - blend all the planes of a CRTC
+ *
+ * When border is enabled, the border color will ALWAYS be the base layer.
+ * Therefore, the first plane (private RGB pipe) will start at STAGE0.
+ * If disabled, the first plane starts at STAGE_BASE.
+ *
+ * Note:
+ * Border is not enabled here because the private plane is exactly
+ * the CRTC resolution.
+ */
 static void blend_setup(struct drm_crtc *crtc)
 {
 	struct mdp5_crtc *mdp5_crtc = to_mdp5_crtc(crtc);
 	struct mdp5_kms *mdp5_kms = get_kms(crtc);
-	int id = mdp5_crtc->id;
+	struct drm_plane *plane;
+	const struct mdp5_cfg_hw *hw_cfg;
+	uint32_t lm = mdp5_crtc->lm, blend_cfg = 0;
+	unsigned long flags;
+#define blender(stage)	((stage) - STAGE_BASE)
 
-	/*
-	 * Hard-coded setup for now until I figure out how the
-	 * layer-mixer works
-	 */
+	hw_cfg = mdp5_cfg_get_hw_config(mdp5_kms->cfg);
 
-	/* LM[id]: */
-	mdp5_write(mdp5_kms, REG_MDP5_LM_BLEND_COLOR_OUT(id),
-			MDP5_LM_BLEND_COLOR_OUT_STAGE0_FG_ALPHA);
-	mdp5_write(mdp5_kms, REG_MDP5_LM_BLEND_OP_MODE(id, 0),
-			MDP5_LM_BLEND_OP_MODE_FG_ALPHA(FG_CONST) |
-			MDP5_LM_BLEND_OP_MODE_BG_ALPHA(FG_PIXEL) |
-			MDP5_LM_BLEND_OP_MODE_BG_INV_ALPHA);
-	mdp5_write(mdp5_kms, REG_MDP5_LM_BLEND_FG_ALPHA(id, 0), 0xff);
-	mdp5_write(mdp5_kms, REG_MDP5_LM_BLEND_BG_ALPHA(id, 0), 0x00);
-
-	/* NOTE: seems that LM[n] and CTL[m], we do not need n==m.. but
-	 * we want to be setting CTL[m].LAYER[n].  Not sure what the
-	 * point of having CTL[m].LAYER[o] (for o!=n).. maybe that is
-	 * used when chaining up mixers for high resolution displays?
-	 */
+	spin_lock_irqsave(&mdp5_crtc->lm_lock, flags);
 
-	/* CTL[id]: */
-	mdp5_write(mdp5_kms, REG_MDP5_CTL_LAYER_REG(id, 0),
-			MDP5_CTL_LAYER_REG_RGB0(STAGE0) |
-			MDP5_CTL_LAYER_REG_BORDER_COLOR);
-	mdp5_write(mdp5_kms, REG_MDP5_CTL_LAYER_REG(id, 1), 0);
-	mdp5_write(mdp5_kms, REG_MDP5_CTL_LAYER_REG(id, 2), 0);
-	mdp5_write(mdp5_kms, REG_MDP5_CTL_LAYER_REG(id, 3), 0);
-	mdp5_write(mdp5_kms, REG_MDP5_CTL_LAYER_REG(id, 4), 0);
+	/* ctl could be released already when we are shutting down: */
+	if (!mdp5_crtc->ctl)
+		goto out;
+
+	drm_atomic_crtc_for_each_plane(plane, crtc) {
+		enum mdp_mixer_stage_id stage =
+			to_mdp5_plane_state(plane->state)->stage;
+
+		/*
+		 * Note: This cannot happen with current implementation but
+		 * we need to check this condition once z property is added
+		 */
+		BUG_ON(stage > hw_cfg->lm.nb_stages);
+
+		/* LM */
+		mdp5_write(mdp5_kms,
+				REG_MDP5_LM_BLEND_OP_MODE(lm, blender(stage)),
+				MDP5_LM_BLEND_OP_MODE_FG_ALPHA(FG_CONST) |
+				MDP5_LM_BLEND_OP_MODE_BG_ALPHA(BG_CONST));
+		mdp5_write(mdp5_kms, REG_MDP5_LM_BLEND_FG_ALPHA(lm,
+				blender(stage)), 0xff);
+		mdp5_write(mdp5_kms, REG_MDP5_LM_BLEND_BG_ALPHA(lm,
+				blender(stage)), 0x00);
+		/* CTL */
+		blend_cfg |= mdp_ctl_blend_mask(mdp5_plane_pipe(plane), stage);
+		DBG("%s: blending pipe %s on stage=%d", mdp5_crtc->name,
+				pipe2name(mdp5_plane_pipe(plane)), stage);
+	}
+
+	DBG("%s: lm%d: blend config = 0x%08x", mdp5_crtc->name, lm, blend_cfg);
+	mdp5_ctl_blend(mdp5_crtc->ctl, lm, blend_cfg);
+
+out:
+	spin_unlock_irqrestore(&mdp5_crtc->lm_lock, flags);
 }
 
-static int mdp5_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)
+static void mdp5_crtc_mode_set_nofb(struct drm_crtc *crtc)
 {
 	struct mdp5_crtc *mdp5_crtc = to_mdp5_crtc(crtc);
 	struct mdp5_kms *mdp5_kms = get_kms(crtc);
-	int ret;
+	unsigned long flags;
+	struct drm_display_mode *mode;
 
-	mode = adjusted_mode;
+	if (WARN_ON(!crtc->state))
+		return;
+
+	mode = &crtc->state->adjusted_mode;
 
 	DBG("%s: set mode: %d:\"%s\" %d %d %d %d %d %d %d %d %d %d 0x%x 0x%x",
 			mdp5_crtc->name, mode->base.id, mode->name,
@@ -286,28 +249,11 @@ static int mdp5_crtc_mode_set(struct drm_crtc *crtc,
 			mode->vsync_end, mode->vtotal,
 			mode->type, mode->flags);
 
-	/* grab extra ref for update_scanout() */
-	drm_framebuffer_reference(crtc->primary->fb);
-
-	ret = mdp5_plane_mode_set(mdp5_crtc->plane, crtc, crtc->primary->fb,
-			0, 0, mode->hdisplay, mode->vdisplay,
-			x << 16, y << 16,
-			mode->hdisplay << 16, mode->vdisplay << 16);
-	if (ret) {
-		drm_framebuffer_unreference(crtc->primary->fb);
-		dev_err(crtc->dev->dev, "%s: failed to set mode on plane: %d\n",
-				mdp5_crtc->name, ret);
-		return ret;
-	}
-
-	mdp5_write(mdp5_kms, REG_MDP5_LM_OUT_SIZE(mdp5_crtc->id),
+	spin_lock_irqsave(&mdp5_crtc->lm_lock, flags);
+	mdp5_write(mdp5_kms, REG_MDP5_LM_OUT_SIZE(mdp5_crtc->lm),
 			MDP5_LM_OUT_SIZE_WIDTH(mode->hdisplay) |
 			MDP5_LM_OUT_SIZE_HEIGHT(mode->vdisplay));
-
-	update_fb(crtc, crtc->primary->fb);
-	update_scanout(crtc, crtc->primary->fb);
-
-	return 0;
+	spin_unlock_irqrestore(&mdp5_crtc->lm_lock, flags);
 }
 
 static void mdp5_crtc_prepare(struct drm_crtc *crtc)
@@ -321,66 +267,114 @@ static void mdp5_crtc_prepare(struct drm_crtc *crtc)
 
 static void mdp5_crtc_commit(struct drm_crtc *crtc)
 {
+	struct mdp5_crtc *mdp5_crtc = to_mdp5_crtc(crtc);
+	DBG("%s", mdp5_crtc->name);
 	mdp5_crtc_dpms(crtc, DRM_MODE_DPMS_ON);
-	crtc_flush(crtc);
+	crtc_flush_all(crtc);
 	/* drop the ref to mdp clk's that we got in prepare: */
 	mdp5_disable(get_kms(crtc));
 }
 
-static int mdp5_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
-		struct drm_framebuffer *old_fb)
+static void mdp5_crtc_load_lut(struct drm_crtc *crtc)
+{
+}
+
+struct plane_state {
+	struct drm_plane *plane;
+	struct mdp5_plane_state *state;
+};
+
+static int pstate_cmp(const void *a, const void *b)
+{
+	struct plane_state *pa = (struct plane_state *)a;
+	struct plane_state *pb = (struct plane_state *)b;
+	return pa->state->zpos - pb->state->zpos;
+}
+
+static int mdp5_crtc_atomic_check(struct drm_crtc *crtc,
+		struct drm_crtc_state *state)
 {
 	struct mdp5_crtc *mdp5_crtc = to_mdp5_crtc(crtc);
-	struct drm_plane *plane = mdp5_crtc->plane;
-	struct drm_display_mode *mode = &crtc->mode;
-	int ret;
-
-	/* grab extra ref for update_scanout() */
-	drm_framebuffer_reference(crtc->primary->fb);
-
-	ret = mdp5_plane_mode_set(plane, crtc, crtc->primary->fb,
-			0, 0, mode->hdisplay, mode->vdisplay,
-			x << 16, y << 16,
-			mode->hdisplay << 16, mode->vdisplay << 16);
-	if (ret) {
-		drm_framebuffer_unreference(crtc->primary->fb);
-		return ret;
+	struct mdp5_kms *mdp5_kms = get_kms(crtc);
+	struct drm_plane *plane;
+	struct drm_device *dev = crtc->dev;
+	struct plane_state pstates[STAGE3 + 1];
+	int cnt = 0, i;
+
+	DBG("%s: check", mdp5_crtc->name);
+
+	/* request a free CTL, if none is already allocated for this CRTC */
+	if (state->enable && !mdp5_crtc->ctl) {
+		mdp5_crtc->ctl = mdp5_ctlm_request(mdp5_kms->ctlm, crtc);
+		if (WARN_ON(!mdp5_crtc->ctl))
+			return -EINVAL;
 	}
 
-	update_fb(crtc, crtc->primary->fb);
-	update_scanout(crtc, crtc->primary->fb);
+	/* verify that there are not too many planes attached to crtc
+	 * and that we don't have conflicting mixer stages:
+	 */
+	drm_atomic_crtc_state_for_each_plane(plane, state) {
+		struct drm_plane_state *pstate;
+
+		if (cnt >= ARRAY_SIZE(pstates)) {
+			dev_err(dev->dev, "too many planes!\n");
+			return -EINVAL;
+		}
+
+		pstate = state->state->plane_states[drm_plane_index(plane)];
+
+		/* plane might not have changed, in which case take
+		 * current state:
+		 */
+		if (!pstate)
+			pstate = plane->state;
+
+		pstates[cnt].plane = plane;
+		pstates[cnt].state = to_mdp5_plane_state(pstate);
+
+		cnt++;
+	}
+
+	sort(pstates, cnt, sizeof(pstates[0]), pstate_cmp, NULL);
+
+	for (i = 0; i < cnt; i++) {
+		pstates[i].state->stage = STAGE_BASE + i;
+		DBG("%s: assign pipe %s on stage=%d", mdp5_crtc->name,
+				pipe2name(mdp5_plane_pipe(pstates[i].plane)),
+				pstates[i].state->stage);
+	}
 
 	return 0;
 }
 
-static void mdp5_crtc_load_lut(struct drm_crtc *crtc)
+static void mdp5_crtc_atomic_begin(struct drm_crtc *crtc)
 {
+	struct mdp5_crtc *mdp5_crtc = to_mdp5_crtc(crtc);
+	DBG("%s: begin", mdp5_crtc->name);
 }
 
-static int mdp5_crtc_page_flip(struct drm_crtc *crtc,
-		struct drm_framebuffer *new_fb,
-		struct drm_pending_vblank_event *event,
-		uint32_t page_flip_flags)
+static void mdp5_crtc_atomic_flush(struct drm_crtc *crtc)
 {
 	struct mdp5_crtc *mdp5_crtc = to_mdp5_crtc(crtc);
 	struct drm_device *dev = crtc->dev;
-	struct drm_gem_object *obj;
 	unsigned long flags;
 
-	if (mdp5_crtc->event) {
-		dev_err(dev->dev, "already pending flip!\n");
-		return -EBUSY;
-	}
+	DBG("%s: event: %p", mdp5_crtc->name, crtc->state->event);
 
-	obj = msm_framebuffer_bo(new_fb, 0);
+	WARN_ON(mdp5_crtc->event);
 
 	spin_lock_irqsave(&dev->event_lock, flags);
-	mdp5_crtc->event = event;
+	mdp5_crtc->event = crtc->state->event;
 	spin_unlock_irqrestore(&dev->event_lock, flags);
 
-	update_fb(crtc, new_fb);
+	blend_setup(crtc);
+	crtc_flush_all(crtc);
+	request_pending(crtc, PENDING_FLIP);
 
-	return msm_gem_queue_inactive_cb(obj, &mdp5_crtc->pageflip_cb);
+	if (mdp5_crtc->ctl && !crtc->state->enable) {
+		mdp5_ctl_release(mdp5_crtc->ctl);
+		mdp5_crtc->ctl = NULL;
+	}
 }
 
 static int mdp5_crtc_set_property(struct drm_crtc *crtc,
@@ -391,27 +385,33 @@ static int mdp5_crtc_set_property(struct drm_crtc *crtc,
 }
 
 static const struct drm_crtc_funcs mdp5_crtc_funcs = {
-	.set_config = drm_crtc_helper_set_config,
+	.set_config = drm_atomic_helper_set_config,
 	.destroy = mdp5_crtc_destroy,
-	.page_flip = mdp5_crtc_page_flip,
+	.page_flip = drm_atomic_helper_page_flip,
 	.set_property = mdp5_crtc_set_property,
+	.reset = drm_atomic_helper_crtc_reset,
+	.atomic_duplicate_state = drm_atomic_helper_crtc_duplicate_state,
+	.atomic_destroy_state = drm_atomic_helper_crtc_destroy_state,
 };
 
 static const struct drm_crtc_helper_funcs mdp5_crtc_helper_funcs = {
 	.dpms = mdp5_crtc_dpms,
 	.mode_fixup = mdp5_crtc_mode_fixup,
-	.mode_set = mdp5_crtc_mode_set,
+	.mode_set_nofb = mdp5_crtc_mode_set_nofb,
+	.mode_set = drm_helper_crtc_mode_set,
+	.mode_set_base = drm_helper_crtc_mode_set_base,
 	.prepare = mdp5_crtc_prepare,
 	.commit = mdp5_crtc_commit,
-	.mode_set_base = mdp5_crtc_mode_set_base,
 	.load_lut = mdp5_crtc_load_lut,
+	.atomic_check = mdp5_crtc_atomic_check,
+	.atomic_begin = mdp5_crtc_atomic_begin,
+	.atomic_flush = mdp5_crtc_atomic_flush,
 };
 
 static void mdp5_crtc_vblank_irq(struct mdp_irq *irq, uint32_t irqstatus)
 {
 	struct mdp5_crtc *mdp5_crtc = container_of(irq, struct mdp5_crtc, vblank);
 	struct drm_crtc *crtc = &mdp5_crtc->base;
-	struct msm_drm_private *priv = crtc->dev->dev_private;
 	unsigned pending;
 
 	mdp_irq_unregister(&get_kms(crtc)->base, &mdp5_crtc->vblank);
@@ -420,16 +420,14 @@ static void mdp5_crtc_vblank_irq(struct mdp_irq *irq, uint32_t irqstatus)
 
 	if (pending & PENDING_FLIP) {
 		complete_flip(crtc, NULL);
-		drm_flip_work_commit(&mdp5_crtc->unref_fb_work, priv->wq);
 	}
 }
 
 static void mdp5_crtc_err_irq(struct mdp_irq *irq, uint32_t irqstatus)
 {
 	struct mdp5_crtc *mdp5_crtc = container_of(irq, struct mdp5_crtc, err);
-	struct drm_crtc *crtc = &mdp5_crtc->base;
+
 	DBG("%s: error: %08x", mdp5_crtc->name, irqstatus);
-	crtc_flush(crtc);
 }
 
 uint32_t mdp5_crtc_vblank(struct drm_crtc *crtc)
@@ -450,19 +448,16 @@ void mdp5_crtc_set_intf(struct drm_crtc *crtc, int intf,
 {
 	struct mdp5_crtc *mdp5_crtc = to_mdp5_crtc(crtc);
 	struct mdp5_kms *mdp5_kms = get_kms(crtc);
-	static const enum mdp5_intfnum intfnum[] = {
-			INTF0, INTF1, INTF2, INTF3,
-	};
+	uint32_t flush_mask = 0;
 	uint32_t intf_sel;
+	unsigned long flags;
 
 	/* now that we know what irq's we want: */
 	mdp5_crtc->err.irqmask = intf2err(intf);
 	mdp5_crtc->vblank.irqmask = intf2vblank(intf);
+	mdp_irq_update(&mdp5_kms->base);
 
-	/* when called from modeset_init(), skip the rest until later: */
-	if (!mdp5_kms)
-		return;
-
+	spin_lock_irqsave(&mdp5_kms->resource_lock, flags);
 	intf_sel = mdp5_read(mdp5_kms, REG_MDP5_DISP_INTF_SEL);
 
 	switch (intf) {
@@ -487,45 +482,25 @@ void mdp5_crtc_set_intf(struct drm_crtc *crtc, int intf,
 		break;
 	}
 
-	blend_setup(crtc);
+	mdp5_write(mdp5_kms, REG_MDP5_DISP_INTF_SEL, intf_sel);
+	spin_unlock_irqrestore(&mdp5_kms->resource_lock, flags);
 
 	DBG("%s: intf_sel=%08x", mdp5_crtc->name, intf_sel);
+	mdp5_ctl_set_intf(mdp5_crtc->ctl, intf);
+	flush_mask |= mdp5_ctl_get_flush(mdp5_crtc->ctl);
+	flush_mask |= mdp5_lm_get_flush(mdp5_crtc->lm);
 
-	mdp5_write(mdp5_kms, REG_MDP5_DISP_INTF_SEL, intf_sel);
-	mdp5_write(mdp5_kms, REG_MDP5_CTL_OP(mdp5_crtc->id),
-			MDP5_CTL_OP_MODE(MODE_NONE) |
-			MDP5_CTL_OP_INTF_NUM(intfnum[intf]));
-
-	crtc_flush(crtc);
+	crtc_flush(crtc, flush_mask);
 }
 
-static void set_attach(struct drm_crtc *crtc, enum mdp5_pipe pipe_id,
-		struct drm_plane *plane)
+int mdp5_crtc_get_lm(struct drm_crtc *crtc)
 {
 	struct mdp5_crtc *mdp5_crtc = to_mdp5_crtc(crtc);
 
-	BUG_ON(pipe_id >= ARRAY_SIZE(mdp5_crtc->planes));
-
-	if (mdp5_crtc->planes[pipe_id] == plane)
-		return;
-
-	mdp5_crtc->planes[pipe_id] = plane;
-	blend_setup(crtc);
-	if (mdp5_crtc->enabled && (plane != mdp5_crtc->plane))
-		crtc_flush(crtc);
-}
+	if (WARN_ON(!crtc))
+		return -EINVAL;
 
-void mdp5_crtc_attach(struct drm_crtc *crtc, struct drm_plane *plane)
-{
-	set_attach(crtc, mdp5_plane_pipe(plane), plane);
-}
-
-void mdp5_crtc_detach(struct drm_crtc *crtc, struct drm_plane *plane)
-{
-	/* don't actually detatch our primary plane: */
-	if (to_mdp5_crtc(crtc)->plane == plane)
-		return;
-	set_attach(crtc, mdp5_plane_pipe(plane), NULL);
+	return mdp5_crtc->lm;
 }
 
 /* initialize crtc */
@@ -534,18 +509,17 @@ struct drm_crtc *mdp5_crtc_init(struct drm_device *dev,
 {
 	struct drm_crtc *crtc = NULL;
 	struct mdp5_crtc *mdp5_crtc;
-	int ret;
 
 	mdp5_crtc = kzalloc(sizeof(*mdp5_crtc), GFP_KERNEL);
-	if (!mdp5_crtc) {
-		ret = -ENOMEM;
-		goto fail;
-	}
+	if (!mdp5_crtc)
+		return ERR_PTR(-ENOMEM);
 
 	crtc = &mdp5_crtc->base;
 
-	mdp5_crtc->plane = plane;
 	mdp5_crtc->id = id;
+	mdp5_crtc->lm = GET_LM_ID(id);
+
+	spin_lock_init(&mdp5_crtc->lm_lock);
 
 	mdp5_crtc->vblank.irq = mdp5_crtc_vblank_irq;
 	mdp5_crtc->err.irq = mdp5_crtc_err_irq;
@@ -553,23 +527,11 @@ struct drm_crtc *mdp5_crtc_init(struct drm_device *dev,
 	snprintf(mdp5_crtc->name, sizeof(mdp5_crtc->name), "%s:%d",
 			pipe2name(mdp5_plane_pipe(plane)), id);
 
-	ret = drm_flip_work_init(&mdp5_crtc->unref_fb_work, 16,
-			"unref fb", unref_fb_worker);
-	if (ret)
-		goto fail;
-
-	INIT_FENCE_CB(&mdp5_crtc->pageflip_cb, pageflip_cb);
-
 	drm_crtc_init_with_planes(dev, crtc, plane, NULL, &mdp5_crtc_funcs);
 	drm_crtc_helper_add(crtc, &mdp5_crtc_helper_funcs);
+	plane->crtc = crtc;
 
-	mdp5_plane_install_properties(mdp5_crtc->plane, &crtc->base);
+	mdp5_plane_install_properties(plane, &crtc->base);
 
 	return crtc;
-
-fail:
-	if (crtc)
-		mdp5_crtc_destroy(crtc);
-
-	return ERR_PTR(ret);
 }
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_ctl.c b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_ctl.c
new file mode 100644
index 000000000000..dea4505ac963
--- /dev/null
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_ctl.c
@@ -0,0 +1,322 @@
+/*
+ * Copyright (c) 2014 The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "mdp5_kms.h"
+#include "mdp5_ctl.h"
+
+/*
+ * CTL - MDP Control Pool Manager
+ *
+ * Controls are shared between all CRTCs.
+ *
+ * They are intended to be used for data path configuration.
+ * The top level register programming describes the complete data path for
+ * a specific data path ID - REG_MDP5_CTL_*(<id>, ...)
+ *
+ * Hardware capabilities determine the number of concurrent data paths
+ *
+ * In certain use cases (high-resolution dual pipe), one single CTL can be
+ * shared across multiple CRTCs.
+ *
+ * Because the number of CTLs can be less than the number of CRTCs,
+ * CTLs are dynamically allocated from a pool of CTLs, only once a CRTC is
+ * requested by the client (in mdp5_crtc_mode_set()).
+ */
+
+struct mdp5_ctl {
+	struct mdp5_ctl_manager *ctlm;
+
+	u32 id;
+
+	/* whether this CTL has been allocated or not: */
+	bool busy;
+
+	/* memory output connection (@see mdp5_ctl_mode): */
+	u32 mode;
+
+	/* REG_MDP5_CTL_*(<id>) registers access info + lock: */
+	spinlock_t hw_lock;
+	u32 reg_offset;
+
+	/* flush mask used to commit CTL registers */
+	u32 flush_mask;
+
+	bool cursor_on;
+
+	struct drm_crtc *crtc;
+};
+
+struct mdp5_ctl_manager {
+	struct drm_device *dev;
+
+	/* number of CTL / Layer Mixers in this hw config: */
+	u32 nlm;
+	u32 nctl;
+
+	/* pool of CTLs + lock to protect resource allocation (ctls[i].busy) */
+	spinlock_t pool_lock;
+	struct mdp5_ctl ctls[MAX_CTL];
+};
+
+static inline
+struct mdp5_kms *get_kms(struct mdp5_ctl_manager *ctl_mgr)
+{
+	struct msm_drm_private *priv = ctl_mgr->dev->dev_private;
+
+	return to_mdp5_kms(to_mdp_kms(priv->kms));
+}
+
+static inline
+void ctl_write(struct mdp5_ctl *ctl, u32 reg, u32 data)
+{
+	struct mdp5_kms *mdp5_kms = get_kms(ctl->ctlm);
+
+	(void)ctl->reg_offset; /* TODO use this instead of mdp5_write */
+	mdp5_write(mdp5_kms, reg, data);
+}
+
+static inline
+u32 ctl_read(struct mdp5_ctl *ctl, u32 reg)
+{
+	struct mdp5_kms *mdp5_kms = get_kms(ctl->ctlm);
+
+	(void)ctl->reg_offset; /* TODO use this instead of mdp5_write */
+	return mdp5_read(mdp5_kms, reg);
+}
+
+
+int mdp5_ctl_set_intf(struct mdp5_ctl *ctl, enum mdp5_intf intf)
+{
+	unsigned long flags;
+	static const enum mdp5_intfnum intfnum[] = {
+			INTF0, INTF1, INTF2, INTF3,
+	};
+
+	spin_lock_irqsave(&ctl->hw_lock, flags);
+	ctl_write(ctl, REG_MDP5_CTL_OP(ctl->id),
+			MDP5_CTL_OP_MODE(ctl->mode) |
+			MDP5_CTL_OP_INTF_NUM(intfnum[intf]));
+	spin_unlock_irqrestore(&ctl->hw_lock, flags);
+
+	return 0;
+}
+
+int mdp5_ctl_set_cursor(struct mdp5_ctl *ctl, bool enable)
+{
+	struct mdp5_ctl_manager *ctl_mgr = ctl->ctlm;
+	unsigned long flags;
+	u32 blend_cfg;
+	int lm;
+
+	lm = mdp5_crtc_get_lm(ctl->crtc);
+	if (unlikely(WARN_ON(lm < 0))) {
+		dev_err(ctl_mgr->dev->dev, "CTL %d cannot find LM: %d",
+				ctl->id, lm);
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(&ctl->hw_lock, flags);
+
+	blend_cfg = ctl_read(ctl, REG_MDP5_CTL_LAYER_REG(ctl->id, lm));
+
+	if (enable)
+		blend_cfg |=  MDP5_CTL_LAYER_REG_CURSOR_OUT;
+	else
+		blend_cfg &= ~MDP5_CTL_LAYER_REG_CURSOR_OUT;
+
+	ctl_write(ctl, REG_MDP5_CTL_LAYER_REG(ctl->id, lm), blend_cfg);
+
+	spin_unlock_irqrestore(&ctl->hw_lock, flags);
+
+	ctl->cursor_on = enable;
+
+	return 0;
+}
+
+
+int mdp5_ctl_blend(struct mdp5_ctl *ctl, u32 lm, u32 blend_cfg)
+{
+	unsigned long flags;
+
+	if (ctl->cursor_on)
+		blend_cfg |=  MDP5_CTL_LAYER_REG_CURSOR_OUT;
+	else
+		blend_cfg &= ~MDP5_CTL_LAYER_REG_CURSOR_OUT;
+
+	spin_lock_irqsave(&ctl->hw_lock, flags);
+	ctl_write(ctl, REG_MDP5_CTL_LAYER_REG(ctl->id, lm), blend_cfg);
+	spin_unlock_irqrestore(&ctl->hw_lock, flags);
+
+	return 0;
+}
+
+int mdp5_ctl_commit(struct mdp5_ctl *ctl, u32 flush_mask)
+{
+	struct mdp5_ctl_manager *ctl_mgr = ctl->ctlm;
+	unsigned long flags;
+
+	if (flush_mask & MDP5_CTL_FLUSH_CURSOR_DUMMY) {
+		int lm = mdp5_crtc_get_lm(ctl->crtc);
+
+		if (unlikely(WARN_ON(lm < 0))) {
+			dev_err(ctl_mgr->dev->dev, "CTL %d cannot find LM: %d",
+					ctl->id, lm);
+			return -EINVAL;
+		}
+
+		/* for current targets, cursor bit is the same as LM bit */
+		flush_mask |= mdp_ctl_flush_mask_lm(lm);
+	}
+
+	spin_lock_irqsave(&ctl->hw_lock, flags);
+	ctl_write(ctl, REG_MDP5_CTL_FLUSH(ctl->id), flush_mask);
+	spin_unlock_irqrestore(&ctl->hw_lock, flags);
+
+	return 0;
+}
+
+u32 mdp5_ctl_get_flush(struct mdp5_ctl *ctl)
+{
+	return ctl->flush_mask;
+}
+
+void mdp5_ctl_release(struct mdp5_ctl *ctl)
+{
+	struct mdp5_ctl_manager *ctl_mgr = ctl->ctlm;
+	unsigned long flags;
+
+	if (unlikely(WARN_ON(ctl->id >= MAX_CTL) || !ctl->busy)) {
+		dev_err(ctl_mgr->dev->dev, "CTL %d in bad state (%d)",
+				ctl->id, ctl->busy);
+		return;
+	}
+
+	spin_lock_irqsave(&ctl_mgr->pool_lock, flags);
+	ctl->busy = false;
+	spin_unlock_irqrestore(&ctl_mgr->pool_lock, flags);
+
+	DBG("CTL %d released", ctl->id);
+}
+
+/*
+ * mdp5_ctl_request() - CTL dynamic allocation
+ *
+ * Note: Current implementation considers that we can only have one CRTC per CTL
+ *
+ * @return first free CTL
+ */
+struct mdp5_ctl *mdp5_ctlm_request(struct mdp5_ctl_manager *ctl_mgr,
+		struct drm_crtc *crtc)
+{
+	struct mdp5_ctl *ctl = NULL;
+	unsigned long flags;
+	int c;
+
+	spin_lock_irqsave(&ctl_mgr->pool_lock, flags);
+
+	for (c = 0; c < ctl_mgr->nctl; c++)
+		if (!ctl_mgr->ctls[c].busy)
+			break;
+
+	if (unlikely(c >= ctl_mgr->nctl)) {
+		dev_err(ctl_mgr->dev->dev, "No more CTL available!");
+		goto unlock;
+	}
+
+	ctl = &ctl_mgr->ctls[c];
+
+	ctl->crtc = crtc;
+	ctl->busy = true;
+	DBG("CTL %d allocated", ctl->id);
+
+unlock:
+	spin_unlock_irqrestore(&ctl_mgr->pool_lock, flags);
+	return ctl;
+}
+
+void mdp5_ctlm_hw_reset(struct mdp5_ctl_manager *ctl_mgr)
+{
+	unsigned long flags;
+	int c;
+
+	for (c = 0; c < ctl_mgr->nctl; c++) {
+		struct mdp5_ctl *ctl = &ctl_mgr->ctls[c];
+
+		spin_lock_irqsave(&ctl->hw_lock, flags);
+		ctl_write(ctl, REG_MDP5_CTL_OP(ctl->id), 0);
+		spin_unlock_irqrestore(&ctl->hw_lock, flags);
+	}
+}
+
+void mdp5_ctlm_destroy(struct mdp5_ctl_manager *ctl_mgr)
+{
+	kfree(ctl_mgr);
+}
+
+struct mdp5_ctl_manager *mdp5_ctlm_init(struct drm_device *dev,
+		void __iomem *mmio_base, const struct mdp5_cfg_hw *hw_cfg)
+{
+	struct mdp5_ctl_manager *ctl_mgr;
+	const struct mdp5_sub_block *ctl_cfg = &hw_cfg->ctl;
+	unsigned long flags;
+	int c, ret;
+
+	ctl_mgr = kzalloc(sizeof(*ctl_mgr), GFP_KERNEL);
+	if (!ctl_mgr) {
+		dev_err(dev->dev, "failed to allocate CTL manager\n");
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	if (unlikely(WARN_ON(ctl_cfg->count > MAX_CTL))) {
+		dev_err(dev->dev, "Increase static pool size to at least %d\n",
+				ctl_cfg->count);
+		ret = -ENOSPC;
+		goto fail;
+	}
+
+	/* initialize the CTL manager: */
+	ctl_mgr->dev = dev;
+	ctl_mgr->nlm = hw_cfg->lm.count;
+	ctl_mgr->nctl = ctl_cfg->count;
+	spin_lock_init(&ctl_mgr->pool_lock);
+
+	/* initialize each CTL of the pool: */
+	spin_lock_irqsave(&ctl_mgr->pool_lock, flags);
+	for (c = 0; c < ctl_mgr->nctl; c++) {
+		struct mdp5_ctl *ctl = &ctl_mgr->ctls[c];
+
+		if (WARN_ON(!ctl_cfg->base[c])) {
+			dev_err(dev->dev, "CTL_%d: base is null!\n", c);
+			ret = -EINVAL;
+			goto fail;
+		}
+		ctl->ctlm = ctl_mgr;
+		ctl->id = c;
+		ctl->mode = MODE_NONE;
+		ctl->reg_offset = ctl_cfg->base[c];
+		ctl->flush_mask = MDP5_CTL_FLUSH_CTL;
+		ctl->busy = false;
+		spin_lock_init(&ctl->hw_lock);
+	}
+	spin_unlock_irqrestore(&ctl_mgr->pool_lock, flags);
+	DBG("Pool of %d CTLs created.", ctl_mgr->nctl);
+
+	return ctl_mgr;
+
+fail:
+	if (ctl_mgr)
+		mdp5_ctlm_destroy(ctl_mgr);
+
+	return ERR_PTR(ret);
+}
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_ctl.h b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_ctl.h
new file mode 100644
index 000000000000..1018519b6af2
--- /dev/null
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_ctl.h
@@ -0,0 +1,122 @@
+/*
+ * Copyright (c) 2014 The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __MDP5_CTL_H__
+#define __MDP5_CTL_H__
+
+#include "msm_drv.h"
+
+/*
+ * CTL Manager prototypes:
+ * mdp5_ctlm_init() returns a ctlm (CTL Manager) handler,
+ * which is then used to call the other mdp5_ctlm_*(ctlm, ...) functions.
+ */
+struct mdp5_ctl_manager;
+struct mdp5_ctl_manager *mdp5_ctlm_init(struct drm_device *dev,
+		void __iomem *mmio_base, const struct mdp5_cfg_hw *hw_cfg);
+void mdp5_ctlm_hw_reset(struct mdp5_ctl_manager *ctlm);
+void mdp5_ctlm_destroy(struct mdp5_ctl_manager *ctlm);
+
+/*
+ * CTL prototypes:
+ * mdp5_ctl_request(ctlm, ...) returns a ctl (CTL resource) handler,
+ * which is then used to call the other mdp5_ctl_*(ctl, ...) functions.
+ */
+struct mdp5_ctl *mdp5_ctlm_request(struct mdp5_ctl_manager *ctlm, struct drm_crtc *crtc);
+
+int mdp5_ctl_set_intf(struct mdp5_ctl *ctl, enum mdp5_intf intf);
+
+int mdp5_ctl_set_cursor(struct mdp5_ctl *ctl, bool enable);
+
+/* @blend_cfg: see LM blender config definition below */
+int mdp5_ctl_blend(struct mdp5_ctl *ctl, u32 lm, u32 blend_cfg);
+
+/* @flush_mask: see CTL flush masks definitions below */
+int mdp5_ctl_commit(struct mdp5_ctl *ctl, u32 flush_mask);
+u32 mdp5_ctl_get_flush(struct mdp5_ctl *ctl);
+
+void mdp5_ctl_release(struct mdp5_ctl *ctl);
+
+/*
+ * blend_cfg (LM blender config):
+ *
+ * The function below allows the caller of mdp5_ctl_blend() to specify how pipes
+ * are being blended according to their stage (z-order), through @blend_cfg arg.
+ */
+static inline u32 mdp_ctl_blend_mask(enum mdp5_pipe pipe,
+		enum mdp_mixer_stage_id stage)
+{
+	switch (pipe) {
+	case SSPP_VIG0: return MDP5_CTL_LAYER_REG_VIG0(stage);
+	case SSPP_VIG1: return MDP5_CTL_LAYER_REG_VIG1(stage);
+	case SSPP_VIG2: return MDP5_CTL_LAYER_REG_VIG2(stage);
+	case SSPP_RGB0: return MDP5_CTL_LAYER_REG_RGB0(stage);
+	case SSPP_RGB1: return MDP5_CTL_LAYER_REG_RGB1(stage);
+	case SSPP_RGB2: return MDP5_CTL_LAYER_REG_RGB2(stage);
+	case SSPP_DMA0: return MDP5_CTL_LAYER_REG_DMA0(stage);
+	case SSPP_DMA1: return MDP5_CTL_LAYER_REG_DMA1(stage);
+	case SSPP_VIG3: return MDP5_CTL_LAYER_REG_VIG3(stage);
+	case SSPP_RGB3: return MDP5_CTL_LAYER_REG_RGB3(stage);
+	default:	return 0;
+	}
+}
+
+/*
+ * flush_mask (CTL flush masks):
+ *
+ * The following functions allow each DRM entity to get and store
+ * their own flush mask.
+ * Once stored, these masks will then be accessed through each DRM's
+ * interface and used by the caller of mdp5_ctl_commit() to specify
+ * which block(s) need to be flushed through @flush_mask parameter.
+ */
+
+#define MDP5_CTL_FLUSH_CURSOR_DUMMY	0x80000000
+
+static inline u32 mdp_ctl_flush_mask_cursor(int cursor_id)
+{
+	/* TODO: use id once multiple cursor support is present */
+	(void)cursor_id;
+
+	return MDP5_CTL_FLUSH_CURSOR_DUMMY;
+}
+
+static inline u32 mdp_ctl_flush_mask_lm(int lm)
+{
+	switch (lm) {
+	case 0:  return MDP5_CTL_FLUSH_LM0;
+	case 1:  return MDP5_CTL_FLUSH_LM1;
+	case 2:  return MDP5_CTL_FLUSH_LM2;
+	case 5:  return MDP5_CTL_FLUSH_LM5;
+	default: return 0;
+	}
+}
+
+static inline u32 mdp_ctl_flush_mask_pipe(enum mdp5_pipe pipe)
+{
+	switch (pipe) {
+	case SSPP_VIG0: return MDP5_CTL_FLUSH_VIG0;
+	case SSPP_VIG1: return MDP5_CTL_FLUSH_VIG1;
+	case SSPP_VIG2: return MDP5_CTL_FLUSH_VIG2;
+	case SSPP_RGB0: return MDP5_CTL_FLUSH_RGB0;
+	case SSPP_RGB1: return MDP5_CTL_FLUSH_RGB1;
+	case SSPP_RGB2: return MDP5_CTL_FLUSH_RGB2;
+	case SSPP_DMA0: return MDP5_CTL_FLUSH_DMA0;
+	case SSPP_DMA1: return MDP5_CTL_FLUSH_DMA1;
+	case SSPP_VIG3: return MDP5_CTL_FLUSH_VIG3;
+	case SSPP_RGB3: return MDP5_CTL_FLUSH_RGB3;
+	default:        return 0;
+	}
+}
+
+#endif /* __MDP5_CTL_H__ */
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_encoder.c b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_encoder.c
index edec7bfaa952..0254bfdeb92f 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_encoder.c
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_encoder.c
@@ -24,6 +24,7 @@ struct mdp5_encoder {
 	struct drm_encoder base;
 	int intf;
 	enum mdp5_intf intf_id;
+	spinlock_t intf_lock;	/* protect REG_MDP5_INTF_* registers */
 	bool enabled;
 	uint32_t bsc;
 };
@@ -115,6 +116,7 @@ static void mdp5_encoder_dpms(struct drm_encoder *encoder, int mode)
 	struct mdp5_kms *mdp5_kms = get_kms(encoder);
 	int intf = mdp5_encoder->intf;
 	bool enabled = (mode == DRM_MODE_DPMS_ON);
+	unsigned long flags;
 
 	DBG("mode=%d", mode);
 
@@ -123,9 +125,24 @@ static void mdp5_encoder_dpms(struct drm_encoder *encoder, int mode)
 
 	if (enabled) {
 		bs_set(mdp5_encoder, 1);
+		spin_lock_irqsave(&mdp5_encoder->intf_lock, flags);
 		mdp5_write(mdp5_kms, REG_MDP5_INTF_TIMING_ENGINE_EN(intf), 1);
+		spin_unlock_irqrestore(&mdp5_encoder->intf_lock, flags);
 	} else {
+		spin_lock_irqsave(&mdp5_encoder->intf_lock, flags);
 		mdp5_write(mdp5_kms, REG_MDP5_INTF_TIMING_ENGINE_EN(intf), 0);
+		spin_unlock_irqrestore(&mdp5_encoder->intf_lock, flags);
+
+		/*
+		 * Wait for a vsync so we know the ENABLE=0 latched before
+		 * the (connector) source of the vsync's gets disabled,
+		 * otherwise we end up in a funny state if we re-enable
+		 * before the disable latches, which results that some of
+		 * the settings changes for the new modeset (like new
+		 * scanout buffer) don't latch properly..
+		 */
+		mdp_irq_wait(&mdp5_kms->base, intf2vblank(intf));
+
 		bs_set(mdp5_encoder, 0);
 	}
 
@@ -150,6 +167,7 @@ static void mdp5_encoder_mode_set(struct drm_encoder *encoder,
 	uint32_t display_v_start, display_v_end;
 	uint32_t hsync_start_x, hsync_end_x;
 	uint32_t format;
+	unsigned long flags;
 
 	mode = adjusted_mode;
 
@@ -180,6 +198,8 @@ static void mdp5_encoder_mode_set(struct drm_encoder *encoder,
 	display_v_start = (mode->vtotal - mode->vsync_start) * mode->htotal + dtv_hsync_skew;
 	display_v_end = vsync_period - ((mode->vsync_start - mode->vdisplay) * mode->htotal) + dtv_hsync_skew - 1;
 
+	spin_lock_irqsave(&mdp5_encoder->intf_lock, flags);
+
 	mdp5_write(mdp5_kms, REG_MDP5_INTF_HSYNC_CTL(intf),
 			MDP5_INTF_HSYNC_CTL_PULSEW(mode->hsync_end - mode->hsync_start) |
 			MDP5_INTF_HSYNC_CTL_PERIOD(mode->htotal));
@@ -201,6 +221,8 @@ static void mdp5_encoder_mode_set(struct drm_encoder *encoder,
 	mdp5_write(mdp5_kms, REG_MDP5_INTF_ACTIVE_VEND_F0(intf), 0);
 	mdp5_write(mdp5_kms, REG_MDP5_INTF_PANEL_FORMAT(intf), format);
 	mdp5_write(mdp5_kms, REG_MDP5_INTF_FRAME_LINE_COUNT_EN(intf), 0x3);  /* frame+line? */
+
+	spin_unlock_irqrestore(&mdp5_encoder->intf_lock, flags);
 }
 
 static void mdp5_encoder_prepare(struct drm_encoder *encoder)
@@ -242,6 +264,8 @@ struct drm_encoder *mdp5_encoder_init(struct drm_device *dev, int intf,
 	mdp5_encoder->intf_id = intf_id;
 	encoder = &mdp5_encoder->base;
 
+	spin_lock_init(&mdp5_encoder->intf_lock);
+
 	drm_encoder_init(dev, encoder, &mdp5_encoder_funcs,
 			 DRM_MODE_ENCODER_TMDS);
 	drm_encoder_helper_add(encoder, &mdp5_encoder_helper_funcs);
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_irq.c b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_irq.c
index f2b985bc2adf..70ac81edd40f 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_irq.c
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_irq.c
@@ -15,6 +15,8 @@
  * this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <linux/irqdomain.h>
+#include <linux/irq.h>
 
 #include "msm_drv.h"
 #include "mdp5_kms.h"
@@ -88,11 +90,17 @@ irqreturn_t mdp5_irq(struct msm_kms *kms)
 
 	VERB("intr=%08x", intr);
 
-	if (intr & MDP5_HW_INTR_STATUS_INTR_MDP)
+	if (intr & MDP5_HW_INTR_STATUS_INTR_MDP) {
 		mdp5_irq_mdp(mdp_kms);
+		intr &= ~MDP5_HW_INTR_STATUS_INTR_MDP;
+	}
 
-	if (intr & MDP5_HW_INTR_STATUS_INTR_HDMI)
-		hdmi_irq(0, mdp5_kms->hdmi);
+	while (intr) {
+		irq_hw_number_t hwirq = fls(intr) - 1;
+		generic_handle_irq(irq_find_mapping(
+				mdp5_kms->irqcontroller.domain, hwirq));
+		intr &= ~(1 << hwirq);
+	}
 
 	return IRQ_HANDLED;
 }
@@ -109,3 +117,82 @@ void mdp5_disable_vblank(struct msm_kms *kms, struct drm_crtc *crtc)
 	mdp_update_vblank_mask(to_mdp_kms(kms),
 			mdp5_crtc_vblank(crtc), false);
 }
+
+/*
+ * interrupt-controller implementation, so sub-blocks (hdmi/eDP/dsi/etc)
+ * can register to get their irq's delivered
+ */
+
+#define VALID_IRQS  (MDP5_HW_INTR_STATUS_INTR_DSI0 | \
+		MDP5_HW_INTR_STATUS_INTR_DSI1 | \
+		MDP5_HW_INTR_STATUS_INTR_HDMI | \
+		MDP5_HW_INTR_STATUS_INTR_EDP)
+
+static void mdp5_hw_mask_irq(struct irq_data *irqd)
+{
+	struct mdp5_kms *mdp5_kms = irq_data_get_irq_chip_data(irqd);
+	smp_mb__before_atomic();
+	clear_bit(irqd->hwirq, &mdp5_kms->irqcontroller.enabled_mask);
+	smp_mb__after_atomic();
+}
+
+static void mdp5_hw_unmask_irq(struct irq_data *irqd)
+{
+	struct mdp5_kms *mdp5_kms = irq_data_get_irq_chip_data(irqd);
+	smp_mb__before_atomic();
+	set_bit(irqd->hwirq, &mdp5_kms->irqcontroller.enabled_mask);
+	smp_mb__after_atomic();
+}
+
+static struct irq_chip mdp5_hw_irq_chip = {
+	.name		= "mdp5",
+	.irq_mask	= mdp5_hw_mask_irq,
+	.irq_unmask	= mdp5_hw_unmask_irq,
+};
+
+static int mdp5_hw_irqdomain_map(struct irq_domain *d,
+		unsigned int irq, irq_hw_number_t hwirq)
+{
+	struct mdp5_kms *mdp5_kms = d->host_data;
+
+	if (!(VALID_IRQS & (1 << hwirq)))
+		return -EPERM;
+
+	irq_set_chip_and_handler(irq, &mdp5_hw_irq_chip, handle_level_irq);
+	irq_set_chip_data(irq, mdp5_kms);
+	set_irq_flags(irq, IRQF_VALID);
+
+	return 0;
+}
+
+static struct irq_domain_ops mdp5_hw_irqdomain_ops = {
+	.map = mdp5_hw_irqdomain_map,
+	.xlate = irq_domain_xlate_onecell,
+};
+
+
+int mdp5_irq_domain_init(struct mdp5_kms *mdp5_kms)
+{
+	struct device *dev = mdp5_kms->dev->dev;
+	struct irq_domain *d;
+
+	d = irq_domain_add_linear(dev->of_node, 32,
+			&mdp5_hw_irqdomain_ops, mdp5_kms);
+	if (!d) {
+		dev_err(dev, "mdp5 irq domain add failed\n");
+		return -ENXIO;
+	}
+
+	mdp5_kms->irqcontroller.enabled_mask = 0;
+	mdp5_kms->irqcontroller.domain = d;
+
+	return 0;
+}
+
+void mdp5_irq_domain_fini(struct mdp5_kms *mdp5_kms)
+{
+	if (mdp5_kms->irqcontroller.domain) {
+		irq_domain_remove(mdp5_kms->irqcontroller.domain);
+		mdp5_kms->irqcontroller.domain = NULL;
+	}
+}
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_kms.c b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_kms.c
index 31a2c6331a1d..9f01a4f21af2 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_kms.c
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_kms.c
@@ -1,4 +1,5 @@
 /*
+ * Copyright (c) 2014, The Linux Foundation. All rights reserved.
  * Copyright (C) 2013 Red Hat
  * Author: Rob Clark <robdclark@gmail.com>
  *
@@ -24,145 +25,11 @@ static const char *iommu_ports[] = {
 		"mdp_0",
 };
 
-static struct mdp5_platform_config *mdp5_get_config(struct platform_device *dev);
-
-const struct mdp5_config *mdp5_cfg;
-
-static const struct mdp5_config msm8x74_config = {
-	.name = "msm8x74",
-	.ctl = {
-		.count = 5,
-		.base = { 0x00600, 0x00700, 0x00800, 0x00900, 0x00a00 },
-	},
-	.pipe_vig = {
-		.count = 3,
-		.base = { 0x01200, 0x01600, 0x01a00 },
-	},
-	.pipe_rgb = {
-		.count = 3,
-		.base = { 0x01e00, 0x02200, 0x02600 },
-	},
-	.pipe_dma = {
-		.count = 2,
-		.base = { 0x02a00, 0x02e00 },
-	},
-	.lm = {
-		.count = 5,
-		.base = { 0x03200, 0x03600, 0x03a00, 0x03e00, 0x04200 },
-	},
-	.dspp = {
-		.count = 3,
-		.base = { 0x04600, 0x04a00, 0x04e00 },
-	},
-	.ad = {
-		.count = 2,
-		.base = { 0x13100, 0x13300 }, /* NOTE: no ad in v1.0 */
-	},
-	.intf = {
-		.count = 4,
-		.base = { 0x12500, 0x12700, 0x12900, 0x12b00 },
-	},
-};
-
-static const struct mdp5_config apq8084_config = {
-	.name = "apq8084",
-	.ctl = {
-		.count = 5,
-		.base = { 0x00600, 0x00700, 0x00800, 0x00900, 0x00a00 },
-	},
-	.pipe_vig = {
-		.count = 4,
-		.base = { 0x01200, 0x01600, 0x01a00, 0x01e00 },
-	},
-	.pipe_rgb = {
-		.count = 4,
-		.base = { 0x02200, 0x02600, 0x02a00, 0x02e00 },
-	},
-	.pipe_dma = {
-		.count = 2,
-		.base = { 0x03200, 0x03600 },
-	},
-	.lm = {
-		.count = 6,
-		.base = { 0x03a00, 0x03e00, 0x04200, 0x04600, 0x04a00, 0x04e00 },
-	},
-	.dspp = {
-		.count = 4,
-		.base = { 0x05200, 0x05600, 0x05a00, 0x05e00 },
-
-	},
-	.ad = {
-		.count = 3,
-		.base = { 0x13500, 0x13700, 0x13900 },
-	},
-	.intf = {
-		.count = 5,
-		.base = { 0x12500, 0x12700, 0x12900, 0x12b00, 0x12d00 },
-	},
-};
-
-struct mdp5_config_entry {
-	int revision;
-	const struct mdp5_config *config;
-};
-
-static const struct mdp5_config_entry mdp5_configs[] = {
-	{ .revision = 0, .config = &msm8x74_config },
-	{ .revision = 2, .config = &msm8x74_config },
-	{ .revision = 3, .config = &apq8084_config },
-};
-
-static int mdp5_select_hw_cfg(struct msm_kms *kms)
-{
-	struct mdp5_kms *mdp5_kms = to_mdp5_kms(to_mdp_kms(kms));
-	struct drm_device *dev = mdp5_kms->dev;
-	uint32_t version, major, minor;
-	int i, ret = 0;
-
-	mdp5_enable(mdp5_kms);
-	version = mdp5_read(mdp5_kms, REG_MDP5_MDP_VERSION);
-	mdp5_disable(mdp5_kms);
-
-	major = FIELD(version, MDP5_MDP_VERSION_MAJOR);
-	minor = FIELD(version, MDP5_MDP_VERSION_MINOR);
-
-	DBG("found MDP5 version v%d.%d", major, minor);
-
-	if (major != 1) {
-		dev_err(dev->dev, "unexpected MDP major version: v%d.%d\n",
-				major, minor);
-		ret = -ENXIO;
-		goto out;
-	}
-
-	mdp5_kms->rev = minor;
-
-	/* only after mdp5_cfg global pointer's init can we access the hw */
-	for (i = 0; i < ARRAY_SIZE(mdp5_configs); i++) {
-		if (mdp5_configs[i].revision != minor)
-			continue;
-		mdp5_kms->hw_cfg = mdp5_cfg = mdp5_configs[i].config;
-		break;
-	}
-	if (unlikely(!mdp5_kms->hw_cfg)) {
-		dev_err(dev->dev, "unexpected MDP minor revision: v%d.%d\n",
-				major, minor);
-		ret = -ENXIO;
-		goto out;
-	}
-
-	DBG("MDP5: %s config selected", mdp5_kms->hw_cfg->name);
-
-	return 0;
-out:
-	return ret;
-}
-
 static int mdp5_hw_init(struct msm_kms *kms)
 {
 	struct mdp5_kms *mdp5_kms = to_mdp5_kms(to_mdp_kms(kms));
 	struct drm_device *dev = mdp5_kms->dev;
-	int i;
+	unsigned long flags;
 
 	pm_runtime_get_sync(dev->dev);
 
@@ -190,10 +57,11 @@ static int mdp5_hw_init(struct msm_kms *kms)
 	 * care.
 	 */
 
+	spin_lock_irqsave(&mdp5_kms->resource_lock, flags);
 	mdp5_write(mdp5_kms, REG_MDP5_DISP_INTF_SEL, 0);
+	spin_unlock_irqrestore(&mdp5_kms->resource_lock, flags);
 
-	for (i = 0; i < mdp5_kms->hw_cfg->ctl.count; i++)
-		mdp5_write(mdp5_kms, REG_MDP5_CTL_OP(i), 0);
+	mdp5_ctlm_hw_reset(mdp5_kms->ctlm);
 
 	pm_runtime_put_sync(dev->dev);
 
@@ -221,10 +89,20 @@ static void mdp5_destroy(struct msm_kms *kms)
 	struct mdp5_kms *mdp5_kms = to_mdp5_kms(to_mdp_kms(kms));
 	struct msm_mmu *mmu = mdp5_kms->mmu;
 
+	mdp5_irq_domain_fini(mdp5_kms);
+
 	if (mmu) {
 		mmu->funcs->detach(mmu, iommu_ports, ARRAY_SIZE(iommu_ports));
 		mmu->funcs->destroy(mmu);
 	}
+
+	if (mdp5_kms->ctlm)
+		mdp5_ctlm_destroy(mdp5_kms->ctlm);
+	if (mdp5_kms->smp)
+		mdp5_smp_destroy(mdp5_kms->smp);
+	if (mdp5_kms->cfg)
+		mdp5_cfg_destroy(mdp5_kms->cfg);
+
 	kfree(mdp5_kms);
 }
 
@@ -274,17 +152,31 @@ static int modeset_init(struct mdp5_kms *mdp5_kms)
 	static const enum mdp5_pipe crtcs[] = {
 			SSPP_RGB0, SSPP_RGB1, SSPP_RGB2, SSPP_RGB3,
 	};
+	static const enum mdp5_pipe pub_planes[] = {
+			SSPP_VIG0, SSPP_VIG1, SSPP_VIG2, SSPP_VIG3,
+	};
 	struct drm_device *dev = mdp5_kms->dev;
 	struct msm_drm_private *priv = dev->dev_private;
 	struct drm_encoder *encoder;
+	const struct mdp5_cfg_hw *hw_cfg;
 	int i, ret;
 
-	/* construct CRTCs: */
-	for (i = 0; i < mdp5_kms->hw_cfg->pipe_rgb.count; i++) {
+	hw_cfg = mdp5_cfg_get_hw_config(mdp5_kms->cfg);
+
+	/* register our interrupt-controller for hdmi/eDP/dsi/etc
+	 * to use for irqs routed through mdp:
+	 */
+	ret = mdp5_irq_domain_init(mdp5_kms);
+	if (ret)
+		goto fail;
+
+	/* construct CRTCs and their private planes: */
+	for (i = 0; i < hw_cfg->pipe_rgb.count; i++) {
 		struct drm_plane *plane;
 		struct drm_crtc *crtc;
 
-		plane = mdp5_plane_init(dev, crtcs[i], true);
+		plane = mdp5_plane_init(dev, crtcs[i], true,
+				hw_cfg->pipe_rgb.base[i]);
 		if (IS_ERR(plane)) {
 			ret = PTR_ERR(plane);
 			dev_err(dev->dev, "failed to construct plane for %s (%d)\n",
@@ -302,6 +194,20 @@ static int modeset_init(struct mdp5_kms *mdp5_kms)
 		priv->crtcs[priv->num_crtcs++] = crtc;
 	}
 
+	/* Construct public planes: */
+	for (i = 0; i < hw_cfg->pipe_vig.count; i++) {
+		struct drm_plane *plane;
+
+		plane = mdp5_plane_init(dev, pub_planes[i], false,
+				hw_cfg->pipe_vig.base[i]);
+		if (IS_ERR(plane)) {
+			ret = PTR_ERR(plane);
+			dev_err(dev->dev, "failed to construct %s plane: %d\n",
+					pipe2name(pub_planes[i]), ret);
+			goto fail;
+		}
+	}
+
 	/* Construct encoder for HDMI: */
 	encoder = mdp5_encoder_init(dev, 3, INTF_HDMI);
 	if (IS_ERR(encoder)) {
@@ -310,25 +216,16 @@ static int modeset_init(struct mdp5_kms *mdp5_kms)
 		goto fail;
 	}
 
-	/* NOTE: the vsync and error irq's are actually associated with
-	 * the INTF/encoder.. the easiest way to deal with this (ie. what
-	 * we do now) is assume a fixed relationship between crtc's and
-	 * encoders.  I'm not sure if there is ever a need to more freely
-	 * assign crtcs to encoders, but if there is then we need to take
-	 * care of error and vblank irq's that the crtc has registered,
-	 * and also update user-requested vblank_mask.
-	 */
-	encoder->possible_crtcs = BIT(0);
-	mdp5_crtc_set_intf(priv->crtcs[0], 3, INTF_HDMI);
-
+	encoder->possible_crtcs = (1 << priv->num_crtcs) - 1;;
 	priv->encoders[priv->num_encoders++] = encoder;
 
 	/* Construct bridge/connector for HDMI: */
-	mdp5_kms->hdmi = hdmi_init(dev, encoder);
-	if (IS_ERR(mdp5_kms->hdmi)) {
-		ret = PTR_ERR(mdp5_kms->hdmi);
-		dev_err(dev->dev, "failed to initialize HDMI: %d\n", ret);
-		goto fail;
+	if (priv->hdmi) {
+		ret = hdmi_modeset_init(priv->hdmi, dev, encoder);
+		if (ret) {
+			dev_err(dev->dev, "failed to initialize HDMI: %d\n", ret);
+			goto fail;
+		}
 	}
 
 	return 0;
@@ -337,6 +234,21 @@ fail:
 	return ret;
 }
 
+static void read_hw_revision(struct mdp5_kms *mdp5_kms,
+		uint32_t *major, uint32_t *minor)
+{
+	uint32_t version;
+
+	mdp5_enable(mdp5_kms);
+	version = mdp5_read(mdp5_kms, REG_MDP5_MDP_VERSION);
+	mdp5_disable(mdp5_kms);
+
+	*major = FIELD(version, MDP5_MDP_VERSION_MAJOR);
+	*minor = FIELD(version, MDP5_MDP_VERSION_MINOR);
+
+	DBG("MDP5 version v%d.%d", *major, *minor);
+}
+
 static int get_clk(struct platform_device *pdev, struct clk **clkp,
 		const char *name)
 {
@@ -353,10 +265,11 @@ static int get_clk(struct platform_device *pdev, struct clk **clkp,
 struct msm_kms *mdp5_kms_init(struct drm_device *dev)
 {
 	struct platform_device *pdev = dev->platformdev;
-	struct mdp5_platform_config *config = mdp5_get_config(pdev);
+	struct mdp5_cfg *config;
 	struct mdp5_kms *mdp5_kms;
 	struct msm_kms *kms = NULL;
 	struct msm_mmu *mmu;
+	uint32_t major, minor;
 	int i, ret;
 
 	mdp5_kms = kzalloc(sizeof(*mdp5_kms), GFP_KERNEL);
@@ -366,12 +279,13 @@ struct msm_kms *mdp5_kms_init(struct drm_device *dev)
 		goto fail;
 	}
 
+	spin_lock_init(&mdp5_kms->resource_lock);
+
 	mdp_kms_init(&mdp5_kms->base, &kms_funcs);
 
 	kms = &mdp5_kms->base.base;
 
 	mdp5_kms->dev = dev;
-	mdp5_kms->smp_blk_cnt = config->smp_blk_cnt;
 
 	mdp5_kms->mmio = msm_ioremap(pdev, "mdp_phys", "MDP5");
 	if (IS_ERR(mdp5_kms->mmio)) {
@@ -416,24 +330,52 @@ struct msm_kms *mdp5_kms_init(struct drm_device *dev)
 	if (ret)
 		goto fail;
 
-	ret = clk_set_rate(mdp5_kms->src_clk, config->max_clk);
+	/* we need to set a default rate before enabling.  Set a safe
+	 * rate first, then figure out hw revision, and then set a
+	 * more optimal rate:
+	 */
+	clk_set_rate(mdp5_kms->src_clk, 200000000);
 
-	ret = mdp5_select_hw_cfg(kms);
-	if (ret)
+	read_hw_revision(mdp5_kms, &major, &minor);
+
+	mdp5_kms->cfg = mdp5_cfg_init(mdp5_kms, major, minor);
+	if (IS_ERR(mdp5_kms->cfg)) {
+		ret = PTR_ERR(mdp5_kms->cfg);
+		mdp5_kms->cfg = NULL;
 		goto fail;
+	}
+
+	config = mdp5_cfg_get_config(mdp5_kms->cfg);
+
+	/* TODO: compute core clock rate at runtime */
+	clk_set_rate(mdp5_kms->src_clk, config->hw->max_clk);
+
+	mdp5_kms->smp = mdp5_smp_init(mdp5_kms->dev, &config->hw->smp);
+	if (IS_ERR(mdp5_kms->smp)) {
+		ret = PTR_ERR(mdp5_kms->smp);
+		mdp5_kms->smp = NULL;
+		goto fail;
+	}
+
+	mdp5_kms->ctlm = mdp5_ctlm_init(dev, mdp5_kms->mmio, config->hw);
+	if (IS_ERR(mdp5_kms->ctlm)) {
+		ret = PTR_ERR(mdp5_kms->ctlm);
+		mdp5_kms->ctlm = NULL;
+		goto fail;
+	}
 
 	/* make sure things are off before attaching iommu (bootloader could
 	 * have left things on, in which case we'll start getting faults if
 	 * we don't disable):
 	 */
 	mdp5_enable(mdp5_kms);
-	for (i = 0; i < mdp5_kms->hw_cfg->intf.count; i++)
+	for (i = 0; i < config->hw->intf.count; i++)
 		mdp5_write(mdp5_kms, REG_MDP5_INTF_TIMING_ENGINE_EN(i), 0);
 	mdp5_disable(mdp5_kms);
 	mdelay(16);
 
-	if (config->iommu) {
-		mmu = msm_iommu_new(&pdev->dev, config->iommu);
+	if (config->platform.iommu) {
+		mmu = msm_iommu_new(&pdev->dev, config->platform.iommu);
 		if (IS_ERR(mmu)) {
 			ret = PTR_ERR(mmu);
 			dev_err(dev->dev, "failed to init iommu: %d\n", ret);
@@ -474,18 +416,3 @@ fail:
 		mdp5_destroy(kms);
 	return ERR_PTR(ret);
 }
-
-static struct mdp5_platform_config *mdp5_get_config(struct platform_device *dev)
-{
-	static struct mdp5_platform_config config = {};
-#ifdef CONFIG_OF
-	/* TODO */
-#endif
-	config.iommu = iommu_domain_alloc(&platform_bus_type);
-	/* TODO hard-coded in downstream mdss, but should it be? */
-	config.max_clk = 200000000;
-	/* TODO get from DT: */
-	config.smp_blk_cnt = 22;
-
-	return &config;
-}
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_kms.h b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_kms.h
index 5bf340dd0f00..dd69c77c0d64 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_kms.h
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_kms.h
@@ -21,25 +21,9 @@
 #include "msm_drv.h"
 #include "msm_kms.h"
 #include "mdp/mdp_kms.h"
-/* dynamic offsets used by mdp5.xml.h (initialized in mdp5_kms.c) */
-#define MDP5_MAX_BASES		8
-struct mdp5_sub_block {
-	int	count;
-	uint32_t base[MDP5_MAX_BASES];
-};
-struct mdp5_config {
-	char  *name;
-	struct mdp5_sub_block ctl;
-	struct mdp5_sub_block pipe_vig;
-	struct mdp5_sub_block pipe_rgb;
-	struct mdp5_sub_block pipe_dma;
-	struct mdp5_sub_block lm;
-	struct mdp5_sub_block dspp;
-	struct mdp5_sub_block ad;
-	struct mdp5_sub_block intf;
-};
-extern const struct mdp5_config *mdp5_cfg;
+#include "mdp5_cfg.h"	/* must be included before mdp5.xml.h */
 #include "mdp5.xml.h"
+#include "mdp5_ctl.h"
 #include "mdp5_smp.h"
 
 struct mdp5_kms {
@@ -47,17 +31,14 @@ struct mdp5_kms {
 
 	struct drm_device *dev;
 
-	int rev;
-	const struct mdp5_config *hw_cfg;
+	struct mdp5_cfg_handler *cfg;
 
 	/* mapper-id used to request GEM buffer mapped for scanout: */
 	int id;
 	struct msm_mmu *mmu;
 
-	/* for tracking smp allocation amongst pipes: */
-	mdp5_smp_state_t smp_state;
-	struct mdp5_client_smp_state smp_client_state[CID_MAX];
-	int smp_blk_cnt;
+	struct mdp5_smp *smp;
+	struct mdp5_ctl_manager *ctlm;
 
 	/* io/register spaces: */
 	void __iomem *mmio, *vbif;
@@ -71,18 +52,47 @@ struct mdp5_kms {
 	struct clk *lut_clk;
 	struct clk *vsync_clk;
 
-	struct hdmi *hdmi;
+	/*
+	 * lock to protect access to global resources: ie., following register:
+	 *	- REG_MDP5_DISP_INTF_SEL
+	 */
+	spinlock_t resource_lock;
 
 	struct mdp_irq error_handler;
+
+	struct {
+		volatile unsigned long enabled_mask;
+		struct irq_domain *domain;
+	} irqcontroller;
 };
 #define to_mdp5_kms(x) container_of(x, struct mdp5_kms, base)
 
-/* platform config data (ie. from DT, or pdata) */
-struct mdp5_platform_config {
-	struct iommu_domain *iommu;
-	uint32_t max_clk;
-	int smp_blk_cnt;
+struct mdp5_plane_state {
+	struct drm_plane_state base;
+
+	/* "virtual" zpos.. we calculate actual mixer-stage at runtime
+	 * by sorting the attached planes by zpos and then assigning
+	 * mixer stage lowest to highest.  Private planes get default
+	 * zpos of zero, and public planes a unique value that is
+	 * greater than zero.  This way, things work out if a naive
+	 * userspace assigns planes to a crtc without setting zpos.
+	 */
+	int zpos;
+
+	/* the actual mixer stage, calculated in crtc->atomic_check()
+	 * NOTE: this should move to mdp5_crtc_state, when that exists
+	 */
+	enum mdp_mixer_stage_id stage;
+
+	/* some additional transactional status to help us know in the
+	 * apply path whether we need to update SMP allocation, and
+	 * whether current update is still pending:
+	 */
+	bool mode_changed : 1;
+	bool pending : 1;
 };
+#define to_mdp5_plane_state(x) \
+		container_of(x, struct mdp5_plane_state, base)
 
 static inline void mdp5_write(struct mdp5_kms *mdp5_kms, u32 reg, u32 data)
 {
@@ -107,23 +117,6 @@ static inline const char *pipe2name(enum mdp5_pipe pipe)
 	return names[pipe];
 }
 
-static inline uint32_t pipe2flush(enum mdp5_pipe pipe)
-{
-	switch (pipe) {
-	case SSPP_VIG0: return MDP5_CTL_FLUSH_VIG0;
-	case SSPP_VIG1: return MDP5_CTL_FLUSH_VIG1;
-	case SSPP_VIG2: return MDP5_CTL_FLUSH_VIG2;
-	case SSPP_RGB0: return MDP5_CTL_FLUSH_RGB0;
-	case SSPP_RGB1: return MDP5_CTL_FLUSH_RGB1;
-	case SSPP_RGB2: return MDP5_CTL_FLUSH_RGB2;
-	case SSPP_DMA0: return MDP5_CTL_FLUSH_DMA0;
-	case SSPP_DMA1: return MDP5_CTL_FLUSH_DMA1;
-	case SSPP_VIG3: return MDP5_CTL_FLUSH_VIG3;
-	case SSPP_RGB3: return MDP5_CTL_FLUSH_RGB3;
-	default:        return 0;
-	}
-}
-
 static inline int pipe2nclients(enum mdp5_pipe pipe)
 {
 	switch (pipe) {
@@ -137,34 +130,6 @@ static inline int pipe2nclients(enum mdp5_pipe pipe)
 	}
 }
 
-static inline enum mdp5_client_id pipe2client(enum mdp5_pipe pipe, int plane)
-{
-	WARN_ON(plane >= pipe2nclients(pipe));
-	switch (pipe) {
-	case SSPP_VIG0: return CID_VIG0_Y + plane;
-	case SSPP_VIG1: return CID_VIG1_Y + plane;
-	case SSPP_VIG2: return CID_VIG2_Y + plane;
-	case SSPP_RGB0: return CID_RGB0;
-	case SSPP_RGB1: return CID_RGB1;
-	case SSPP_RGB2: return CID_RGB2;
-	case SSPP_DMA0: return CID_DMA0_Y + plane;
-	case SSPP_DMA1: return CID_DMA1_Y + plane;
-	case SSPP_VIG3: return CID_VIG3_Y + plane;
-	case SSPP_RGB3: return CID_RGB3;
-	default:        return CID_UNUSED;
-	}
-}
-
-static inline uint32_t mixer2flush(int lm)
-{
-	switch (lm) {
-	case 0:  return MDP5_CTL_FLUSH_LM0;
-	case 1:  return MDP5_CTL_FLUSH_LM1;
-	case 2:  return MDP5_CTL_FLUSH_LM2;
-	default: return 0;
-	}
-}
-
 static inline uint32_t intf2err(int intf)
 {
 	switch (intf) {
@@ -197,6 +162,8 @@ void mdp5_irq_uninstall(struct msm_kms *kms);
 irqreturn_t mdp5_irq(struct msm_kms *kms);
 int mdp5_enable_vblank(struct msm_kms *kms, struct drm_crtc *crtc);
 void mdp5_disable_vblank(struct msm_kms *kms, struct drm_crtc *crtc);
+int mdp5_irq_domain_init(struct mdp5_kms *mdp5_kms);
+void mdp5_irq_domain_fini(struct mdp5_kms *mdp5_kms);
 
 static inline
 uint32_t mdp5_get_formats(enum mdp5_pipe pipe, uint32_t *pixel_formats,
@@ -210,26 +177,18 @@ uint32_t mdp5_get_formats(enum mdp5_pipe pipe, uint32_t *pixel_formats,
 
 void mdp5_plane_install_properties(struct drm_plane *plane,
 		struct drm_mode_object *obj);
-void mdp5_plane_set_scanout(struct drm_plane *plane,
-		struct drm_framebuffer *fb);
-int mdp5_plane_mode_set(struct drm_plane *plane,
-		struct drm_crtc *crtc, struct drm_framebuffer *fb,
-		int crtc_x, int crtc_y,
-		unsigned int crtc_w, unsigned int crtc_h,
-		uint32_t src_x, uint32_t src_y,
-		uint32_t src_w, uint32_t src_h);
+uint32_t mdp5_plane_get_flush(struct drm_plane *plane);
 void mdp5_plane_complete_flip(struct drm_plane *plane);
 enum mdp5_pipe mdp5_plane_pipe(struct drm_plane *plane);
 struct drm_plane *mdp5_plane_init(struct drm_device *dev,
-		enum mdp5_pipe pipe, bool private_plane);
+		enum mdp5_pipe pipe, bool private_plane, uint32_t reg_offset);
 
 uint32_t mdp5_crtc_vblank(struct drm_crtc *crtc);
 
+int mdp5_crtc_get_lm(struct drm_crtc *crtc);
 void mdp5_crtc_cancel_pending_flip(struct drm_crtc *crtc, struct drm_file *file);
 void mdp5_crtc_set_intf(struct drm_crtc *crtc, int intf,
 		enum mdp5_intf intf_id);
-void mdp5_crtc_attach(struct drm_crtc *crtc, struct drm_plane *plane);
-void mdp5_crtc_detach(struct drm_crtc *crtc, struct drm_plane *plane);
 struct drm_crtc *mdp5_crtc_init(struct drm_device *dev,
 		struct drm_plane *plane, int id);
 
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_plane.c b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_plane.c
index f3daec4412ad..26e5fdea6594 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_plane.c
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_plane.c
@@ -1,4 +1,5 @@
 /*
+ * Copyright (c) 2014 The Linux Foundation. All rights reserved.
  * Copyright (C) 2013 Red Hat
  * Author: Rob Clark <robdclark@gmail.com>
  *
@@ -17,6 +18,7 @@
 
 #include "mdp5_kms.h"
 
+#define MAX_PLANE	4
 
 struct mdp5_plane {
 	struct drm_plane base;
@@ -24,6 +26,11 @@ struct mdp5_plane {
 
 	enum mdp5_pipe pipe;
 
+	spinlock_t pipe_lock;	/* protect REG_MDP5_PIPE_* registers */
+	uint32_t reg_offset;
+
+	uint32_t flush_mask;	/* used to commit pipe registers */
+
 	uint32_t nformats;
 	uint32_t formats[32];
 
@@ -31,31 +38,24 @@ struct mdp5_plane {
 };
 #define to_mdp5_plane(x) container_of(x, struct mdp5_plane, base)
 
+static int mdp5_plane_mode_set(struct drm_plane *plane,
+		struct drm_crtc *crtc, struct drm_framebuffer *fb,
+		int crtc_x, int crtc_y,
+		unsigned int crtc_w, unsigned int crtc_h,
+		uint32_t src_x, uint32_t src_y,
+		uint32_t src_w, uint32_t src_h);
+static void set_scanout_locked(struct drm_plane *plane,
+		struct drm_framebuffer *fb);
+
 static struct mdp5_kms *get_kms(struct drm_plane *plane)
 {
 	struct msm_drm_private *priv = plane->dev->dev_private;
 	return to_mdp5_kms(to_mdp_kms(priv->kms));
 }
 
-static int mdp5_plane_update(struct drm_plane *plane,
-		struct drm_crtc *crtc, struct drm_framebuffer *fb,
-		int crtc_x, int crtc_y,
-		unsigned int crtc_w, unsigned int crtc_h,
-		uint32_t src_x, uint32_t src_y,
-		uint32_t src_w, uint32_t src_h)
+static bool plane_enabled(struct drm_plane_state *state)
 {
-	struct mdp5_plane *mdp5_plane = to_mdp5_plane(plane);
-
-	mdp5_plane->enabled = true;
-
-	if (plane->fb)
-		drm_framebuffer_unreference(plane->fb);
-
-	drm_framebuffer_reference(fb);
-
-	return mdp5_plane_mode_set(plane, crtc, fb,
-			crtc_x, crtc_y, crtc_w, crtc_h,
-			src_x, src_y, src_w, src_h);
+	return state->fb && state->crtc;
 }
 
 static int mdp5_plane_disable(struct drm_plane *plane)
@@ -63,21 +63,13 @@ static int mdp5_plane_disable(struct drm_plane *plane)
 	struct mdp5_plane *mdp5_plane = to_mdp5_plane(plane);
 	struct mdp5_kms *mdp5_kms = get_kms(plane);
 	enum mdp5_pipe pipe = mdp5_plane->pipe;
-	int i;
 
 	DBG("%s: disable", mdp5_plane->name);
 
-	/* update our SMP request to zero (release all our blks): */
-	for (i = 0; i < pipe2nclients(pipe); i++)
-		mdp5_smp_request(mdp5_kms, pipe2client(pipe, i), 0);
-
-	/* TODO detaching now will cause us not to get the last
-	 * vblank and mdp5_smp_commit().. so other planes will
-	 * still see smp blocks previously allocated to us as
-	 * in-use..
-	 */
-	if (plane->crtc)
-		mdp5_crtc_detach(plane->crtc, plane);
+	if (mdp5_kms) {
+		/* Release the memory we requested earlier from the SMP: */
+		mdp5_smp_release(mdp5_kms->smp, pipe);
+	}
 
 	return 0;
 }
@@ -85,11 +77,8 @@ static int mdp5_plane_disable(struct drm_plane *plane)
 static void mdp5_plane_destroy(struct drm_plane *plane)
 {
 	struct mdp5_plane *mdp5_plane = to_mdp5_plane(plane);
-	struct msm_drm_private *priv = plane->dev->dev_private;
-
-	if (priv->kms)
-		mdp5_plane_disable(plane);
 
+	drm_plane_helper_disable(plane);
 	drm_plane_cleanup(plane);
 
 	kfree(mdp5_plane);
@@ -109,109 +98,186 @@ int mdp5_plane_set_property(struct drm_plane *plane,
 	return -EINVAL;
 }
 
+static void mdp5_plane_reset(struct drm_plane *plane)
+{
+	struct mdp5_plane_state *mdp5_state;
+
+	if (plane->state && plane->state->fb)
+		drm_framebuffer_unreference(plane->state->fb);
+
+	kfree(to_mdp5_plane_state(plane->state));
+	mdp5_state = kzalloc(sizeof(*mdp5_state), GFP_KERNEL);
+
+	if (plane->type == DRM_PLANE_TYPE_PRIMARY) {
+		mdp5_state->zpos = 0;
+	} else {
+		mdp5_state->zpos = 1 + drm_plane_index(plane);
+	}
+
+	plane->state = &mdp5_state->base;
+}
+
+static struct drm_plane_state *
+mdp5_plane_duplicate_state(struct drm_plane *plane)
+{
+	struct mdp5_plane_state *mdp5_state;
+
+	if (WARN_ON(!plane->state))
+		return NULL;
+
+	mdp5_state = kmemdup(to_mdp5_plane_state(plane->state),
+			sizeof(*mdp5_state), GFP_KERNEL);
+
+	if (mdp5_state && mdp5_state->base.fb)
+		drm_framebuffer_reference(mdp5_state->base.fb);
+
+	mdp5_state->mode_changed = false;
+	mdp5_state->pending = false;
+
+	return &mdp5_state->base;
+}
+
+static void mdp5_plane_destroy_state(struct drm_plane *plane,
+		struct drm_plane_state *state)
+{
+	if (state->fb)
+		drm_framebuffer_unreference(state->fb);
+
+	kfree(to_mdp5_plane_state(state));
+}
+
 static const struct drm_plane_funcs mdp5_plane_funcs = {
-		.update_plane = mdp5_plane_update,
-		.disable_plane = mdp5_plane_disable,
+		.update_plane = drm_atomic_helper_update_plane,
+		.disable_plane = drm_atomic_helper_disable_plane,
 		.destroy = mdp5_plane_destroy,
 		.set_property = mdp5_plane_set_property,
+		.reset = mdp5_plane_reset,
+		.atomic_duplicate_state = mdp5_plane_duplicate_state,
+		.atomic_destroy_state = mdp5_plane_destroy_state,
 };
 
-void mdp5_plane_set_scanout(struct drm_plane *plane,
+static int mdp5_plane_prepare_fb(struct drm_plane *plane,
 		struct drm_framebuffer *fb)
 {
 	struct mdp5_plane *mdp5_plane = to_mdp5_plane(plane);
 	struct mdp5_kms *mdp5_kms = get_kms(plane);
-	enum mdp5_pipe pipe = mdp5_plane->pipe;
-	uint32_t nplanes = drm_format_num_planes(fb->pixel_format);
-	uint32_t iova[4];
-	int i;
-
-	for (i = 0; i < nplanes; i++) {
-		struct drm_gem_object *bo = msm_framebuffer_bo(fb, i);
-		msm_gem_get_iova(bo, mdp5_kms->id, &iova[i]);
-	}
-	for (; i < 4; i++)
-		iova[i] = 0;
 
-	mdp5_write(mdp5_kms, REG_MDP5_PIPE_SRC_STRIDE_A(pipe),
-			MDP5_PIPE_SRC_STRIDE_A_P0(fb->pitches[0]) |
-			MDP5_PIPE_SRC_STRIDE_A_P1(fb->pitches[1]));
-
-	mdp5_write(mdp5_kms, REG_MDP5_PIPE_SRC_STRIDE_B(pipe),
-			MDP5_PIPE_SRC_STRIDE_B_P2(fb->pitches[2]) |
-			MDP5_PIPE_SRC_STRIDE_B_P3(fb->pitches[3]));
-
-	mdp5_write(mdp5_kms, REG_MDP5_PIPE_SRC0_ADDR(pipe), iova[0]);
-	mdp5_write(mdp5_kms, REG_MDP5_PIPE_SRC1_ADDR(pipe), iova[1]);
-	mdp5_write(mdp5_kms, REG_MDP5_PIPE_SRC2_ADDR(pipe), iova[2]);
-	mdp5_write(mdp5_kms, REG_MDP5_PIPE_SRC3_ADDR(pipe), iova[3]);
-
-	plane->fb = fb;
+	DBG("%s: prepare: FB[%u]", mdp5_plane->name, fb->base.id);
+	return msm_framebuffer_prepare(fb, mdp5_kms->id);
 }
 
-/* NOTE: looks like if horizontal decimation is used (if we supported that)
- * then the width used to calculate SMP block requirements is the post-
- * decimated width.  Ie. SMP buffering sits downstream of decimation (which
- * presumably happens during the dma from scanout buffer).
- */
-static int request_smp_blocks(struct drm_plane *plane, uint32_t format,
-		uint32_t nplanes, uint32_t width)
+static void mdp5_plane_cleanup_fb(struct drm_plane *plane,
+		struct drm_framebuffer *fb)
 {
-	struct drm_device *dev = plane->dev;
 	struct mdp5_plane *mdp5_plane = to_mdp5_plane(plane);
 	struct mdp5_kms *mdp5_kms = get_kms(plane);
-	enum mdp5_pipe pipe = mdp5_plane->pipe;
-	int i, hsub, nlines, nblks, ret;
 
-	hsub = drm_format_horz_chroma_subsampling(format);
+	DBG("%s: cleanup: FB[%u]", mdp5_plane->name, fb->base.id);
+	msm_framebuffer_cleanup(fb, mdp5_kms->id);
+}
 
-	/* different if BWC (compressed framebuffer?) enabled: */
-	nlines = 2;
+static int mdp5_plane_atomic_check(struct drm_plane *plane,
+		struct drm_plane_state *state)
+{
+	struct mdp5_plane *mdp5_plane = to_mdp5_plane(plane);
+	struct drm_plane_state *old_state = plane->state;
 
-	for (i = 0, nblks = 0; i < nplanes; i++) {
-		int n, fetch_stride, cpp;
+	DBG("%s: check (%d -> %d)", mdp5_plane->name,
+			plane_enabled(old_state), plane_enabled(state));
 
-		cpp = drm_format_plane_cpp(format, i);
-		fetch_stride = width * cpp / (i ? hsub : 1);
+	if (plane_enabled(state) && plane_enabled(old_state)) {
+		/* we cannot change SMP block configuration during scanout: */
+		bool full_modeset = false;
+		if (state->fb->pixel_format != old_state->fb->pixel_format) {
+			DBG("%s: pixel_format change!", mdp5_plane->name);
+			full_modeset = true;
+		}
+		if (state->src_w != old_state->src_w) {
+			DBG("%s: src_w change!", mdp5_plane->name);
+			full_modeset = true;
+		}
+		if (to_mdp5_plane_state(old_state)->pending) {
+			DBG("%s: still pending!", mdp5_plane->name);
+			full_modeset = true;
+		}
+		if (full_modeset) {
+			struct drm_crtc_state *crtc_state =
+					drm_atomic_get_crtc_state(state->state, state->crtc);
+			crtc_state->mode_changed = true;
+			to_mdp5_plane_state(state)->mode_changed = true;
+		}
+	} else {
+		to_mdp5_plane_state(state)->mode_changed = true;
+	}
 
-		n = DIV_ROUND_UP(fetch_stride * nlines, SMP_BLK_SIZE);
+	return 0;
+}
 
-		/* for hw rev v1.00 */
-		if (mdp5_kms->rev == 0)
-			n = roundup_pow_of_two(n);
+static void mdp5_plane_atomic_update(struct drm_plane *plane,
+				     struct drm_plane_state *old_state)
+{
+	struct mdp5_plane *mdp5_plane = to_mdp5_plane(plane);
+	struct drm_plane_state *state = plane->state;
 
-		DBG("%s[%d]: request %d SMP blocks", mdp5_plane->name, i, n);
-		ret = mdp5_smp_request(mdp5_kms, pipe2client(pipe, i), n);
-		if (ret) {
-			dev_err(dev->dev, "Could not allocate %d SMP blocks: %d\n",
-					n, ret);
-			return ret;
-		}
+	DBG("%s: update", mdp5_plane->name);
 
-		nblks += n;
+	if (!plane_enabled(state)) {
+		to_mdp5_plane_state(state)->pending = true;
+		mdp5_plane_disable(plane);
+	} else if (to_mdp5_plane_state(state)->mode_changed) {
+		int ret;
+		to_mdp5_plane_state(state)->pending = true;
+		ret = mdp5_plane_mode_set(plane,
+				state->crtc, state->fb,
+				state->crtc_x, state->crtc_y,
+				state->crtc_w, state->crtc_h,
+				state->src_x,  state->src_y,
+				state->src_w, state->src_h);
+		/* atomic_check should have ensured that this doesn't fail */
+		WARN_ON(ret < 0);
+	} else {
+		unsigned long flags;
+		spin_lock_irqsave(&mdp5_plane->pipe_lock, flags);
+		set_scanout_locked(plane, state->fb);
+		spin_unlock_irqrestore(&mdp5_plane->pipe_lock, flags);
 	}
-
-	/* in success case, return total # of blocks allocated: */
-	return nblks;
 }
 
-static void set_fifo_thresholds(struct drm_plane *plane, int nblks)
+static const struct drm_plane_helper_funcs mdp5_plane_helper_funcs = {
+		.prepare_fb = mdp5_plane_prepare_fb,
+		.cleanup_fb = mdp5_plane_cleanup_fb,
+		.atomic_check = mdp5_plane_atomic_check,
+		.atomic_update = mdp5_plane_atomic_update,
+};
+
+static void set_scanout_locked(struct drm_plane *plane,
+		struct drm_framebuffer *fb)
 {
 	struct mdp5_plane *mdp5_plane = to_mdp5_plane(plane);
 	struct mdp5_kms *mdp5_kms = get_kms(plane);
 	enum mdp5_pipe pipe = mdp5_plane->pipe;
-	uint32_t val;
 
-	/* 1/4 of SMP pool that is being fetched */
-	val = (nblks * SMP_ENTRIES_PER_BLK) / 4;
+	mdp5_write(mdp5_kms, REG_MDP5_PIPE_SRC_STRIDE_A(pipe),
+			MDP5_PIPE_SRC_STRIDE_A_P0(fb->pitches[0]) |
+			MDP5_PIPE_SRC_STRIDE_A_P1(fb->pitches[1]));
 
-	mdp5_write(mdp5_kms, REG_MDP5_PIPE_REQPRIO_FIFO_WM_0(pipe), val * 1);
-	mdp5_write(mdp5_kms, REG_MDP5_PIPE_REQPRIO_FIFO_WM_1(pipe), val * 2);
-	mdp5_write(mdp5_kms, REG_MDP5_PIPE_REQPRIO_FIFO_WM_2(pipe), val * 3);
+	mdp5_write(mdp5_kms, REG_MDP5_PIPE_SRC_STRIDE_B(pipe),
+			MDP5_PIPE_SRC_STRIDE_B_P2(fb->pitches[2]) |
+			MDP5_PIPE_SRC_STRIDE_B_P3(fb->pitches[3]));
+
+	mdp5_write(mdp5_kms, REG_MDP5_PIPE_SRC0_ADDR(pipe),
+			msm_framebuffer_iova(fb, mdp5_kms->id, 0));
+	mdp5_write(mdp5_kms, REG_MDP5_PIPE_SRC1_ADDR(pipe),
+			msm_framebuffer_iova(fb, mdp5_kms->id, 1));
+	mdp5_write(mdp5_kms, REG_MDP5_PIPE_SRC2_ADDR(pipe),
+			msm_framebuffer_iova(fb, mdp5_kms->id, 2));
+	mdp5_write(mdp5_kms, REG_MDP5_PIPE_SRC3_ADDR(pipe),
+			msm_framebuffer_iova(fb, mdp5_kms->id, 4));
 
+	plane->fb = fb;
 }
 
-int mdp5_plane_mode_set(struct drm_plane *plane,
+static int mdp5_plane_mode_set(struct drm_plane *plane,
 		struct drm_crtc *crtc, struct drm_framebuffer *fb,
 		int crtc_x, int crtc_y,
 		unsigned int crtc_w, unsigned int crtc_h,
@@ -225,7 +291,8 @@ int mdp5_plane_mode_set(struct drm_plane *plane,
 	uint32_t nplanes, config = 0;
 	uint32_t phasex_step = 0, phasey_step = 0;
 	uint32_t hdecm = 0, vdecm = 0;
-	int i, nblks;
+	unsigned long flags;
+	int ret;
 
 	nplanes = drm_format_num_planes(fb->pixel_format);
 
@@ -243,12 +310,11 @@ int mdp5_plane_mode_set(struct drm_plane *plane,
 			fb->base.id, src_x, src_y, src_w, src_h,
 			crtc->base.id, crtc_x, crtc_y, crtc_w, crtc_h);
 
-	/*
-	 * Calculate and request required # of smp blocks:
-	 */
-	nblks = request_smp_blocks(plane, fb->pixel_format, nplanes, src_w);
-	if (nblks < 0)
-		return nblks;
+	/* Request some memory from the SMP: */
+	ret = mdp5_smp_request(mdp5_kms->smp,
+			mdp5_plane->pipe, fb->pixel_format, src_w);
+	if (ret)
+		return ret;
 
 	/*
 	 * Currently we update the hw for allocations/requests immediately,
@@ -256,8 +322,7 @@ int mdp5_plane_mode_set(struct drm_plane *plane,
 	 * would move into atomic->check_plane_state(), while updating the
 	 * hw would remain here:
 	 */
-	for (i = 0; i < pipe2nclients(pipe); i++)
-		mdp5_smp_configure(mdp5_kms, pipe2client(pipe, i));
+	mdp5_smp_configure(mdp5_kms->smp, pipe);
 
 	if (src_w != crtc_w) {
 		config |= MDP5_PIPE_SCALE_CONFIG_SCALEX_EN;
@@ -269,6 +334,8 @@ int mdp5_plane_mode_set(struct drm_plane *plane,
 		/* TODO calc phasey_step, vdecm */
 	}
 
+	spin_lock_irqsave(&mdp5_plane->pipe_lock, flags);
+
 	mdp5_write(mdp5_kms, REG_MDP5_PIPE_SRC_IMG_SIZE(pipe),
 			MDP5_PIPE_SRC_IMG_SIZE_WIDTH(src_w) |
 			MDP5_PIPE_SRC_IMG_SIZE_HEIGHT(src_h));
@@ -289,8 +356,6 @@ int mdp5_plane_mode_set(struct drm_plane *plane,
 			MDP5_PIPE_OUT_XY_X(crtc_x) |
 			MDP5_PIPE_OUT_XY_Y(crtc_y));
 
-	mdp5_plane_set_scanout(plane, fb);
-
 	format = to_mdp_format(msm_framebuffer_format(fb));
 
 	mdp5_write(mdp5_kms, REG_MDP5_PIPE_SRC_FORMAT(pipe),
@@ -330,22 +395,24 @@ int mdp5_plane_mode_set(struct drm_plane *plane,
 			MDP5_PIPE_SCALE_CONFIG_SCALEX_MAX_FILTER(SCALE_FILTER_NEAREST) |
 			MDP5_PIPE_SCALE_CONFIG_SCALEY_MAX_FILTER(SCALE_FILTER_NEAREST));
 
-	set_fifo_thresholds(plane, nblks);
+	set_scanout_locked(plane, fb);
 
-	/* TODO detach from old crtc (if we had more than one) */
-	mdp5_crtc_attach(crtc, plane);
+	spin_unlock_irqrestore(&mdp5_plane->pipe_lock, flags);
 
-	return 0;
+	return ret;
 }
 
 void mdp5_plane_complete_flip(struct drm_plane *plane)
 {
 	struct mdp5_kms *mdp5_kms = get_kms(plane);
-	enum mdp5_pipe pipe = to_mdp5_plane(plane)->pipe;
-	int i;
+	struct mdp5_plane *mdp5_plane = to_mdp5_plane(plane);
+	enum mdp5_pipe pipe = mdp5_plane->pipe;
+
+	DBG("%s: complete flip", mdp5_plane->name);
 
-	for (i = 0; i < pipe2nclients(pipe); i++)
-		mdp5_smp_commit(mdp5_kms, pipe2client(pipe, i));
+	mdp5_smp_commit(mdp5_kms->smp, pipe);
+
+	to_mdp5_plane_state(plane->state)->pending = false;
 }
 
 enum mdp5_pipe mdp5_plane_pipe(struct drm_plane *plane)
@@ -354,9 +421,16 @@ enum mdp5_pipe mdp5_plane_pipe(struct drm_plane *plane)
 	return mdp5_plane->pipe;
 }
 
+uint32_t mdp5_plane_get_flush(struct drm_plane *plane)
+{
+	struct mdp5_plane *mdp5_plane = to_mdp5_plane(plane);
+
+	return mdp5_plane->flush_mask;
+}
+
 /* initialize plane */
 struct drm_plane *mdp5_plane_init(struct drm_device *dev,
-		enum mdp5_pipe pipe, bool private_plane)
+		enum mdp5_pipe pipe, bool private_plane, uint32_t reg_offset)
 {
 	struct drm_plane *plane = NULL;
 	struct mdp5_plane *mdp5_plane;
@@ -377,10 +451,18 @@ struct drm_plane *mdp5_plane_init(struct drm_device *dev,
 	mdp5_plane->nformats = mdp5_get_formats(pipe, mdp5_plane->formats,
 			ARRAY_SIZE(mdp5_plane->formats));
 
+	mdp5_plane->flush_mask = mdp_ctl_flush_mask_pipe(pipe);
+	mdp5_plane->reg_offset = reg_offset;
+	spin_lock_init(&mdp5_plane->pipe_lock);
+
 	type = private_plane ? DRM_PLANE_TYPE_PRIMARY : DRM_PLANE_TYPE_OVERLAY;
-	drm_universal_plane_init(dev, plane, 0xff, &mdp5_plane_funcs,
+	ret = drm_universal_plane_init(dev, plane, 0xff, &mdp5_plane_funcs,
 				 mdp5_plane->formats, mdp5_plane->nformats,
 				 type);
+	if (ret)
+		goto fail;
+
+	drm_plane_helper_add(plane, &mdp5_plane_helper_funcs);
 
 	mdp5_plane_install_properties(plane, &plane->base);
 
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_smp.c b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_smp.c
index 2d0236b963a6..bf551885e019 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_smp.c
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_smp.c
@@ -1,4 +1,5 @@
 /*
+ * Copyright (c) 2014, The Linux Foundation. All rights reserved.
  * Copyright (C) 2013 Red Hat
  * Author: Rob Clark <robdclark@gmail.com>
  *
@@ -29,8 +30,11 @@
  * Based on the size of the attached scanout buffer, a certain # of
  * blocks must be allocated to that client out of the shared pool.
  *
- * For each block, it can be either free, or pending/in-use by a
- * client.  The updates happen in three steps:
+ * In some hw, some blocks are statically allocated for certain pipes
+ * and CANNOT be re-allocated (eg: MMB0 and MMB1 both tied to RGB0).
+ *
+ * For each block that can be dynamically allocated, it can be either
+ * free, or pending/in-use by a client. The updates happen in three steps:
  *
  *  1) mdp5_smp_request():
  *     When plane scanout is setup, calculate required number of
@@ -61,21 +65,68 @@
  * inuse and pending state of all clients..
  */
 
-static DEFINE_SPINLOCK(smp_lock);
+struct mdp5_smp {
+	struct drm_device *dev;
+
+	int blk_cnt;
+	int blk_size;
+
+	spinlock_t state_lock;
+	mdp5_smp_state_t state; /* to track smp allocation amongst pipes: */
+
+	struct mdp5_client_smp_state client_state[CID_MAX];
+};
 
+static inline
+struct mdp5_kms *get_kms(struct mdp5_smp *smp)
+{
+	struct msm_drm_private *priv = smp->dev->dev_private;
+
+	return to_mdp5_kms(to_mdp_kms(priv->kms));
+}
+
+static inline enum mdp5_client_id pipe2client(enum mdp5_pipe pipe, int plane)
+{
+	WARN_ON(plane >= pipe2nclients(pipe));
+	switch (pipe) {
+	case SSPP_VIG0: return CID_VIG0_Y + plane;
+	case SSPP_VIG1: return CID_VIG1_Y + plane;
+	case SSPP_VIG2: return CID_VIG2_Y + plane;
+	case SSPP_RGB0: return CID_RGB0;
+	case SSPP_RGB1: return CID_RGB1;
+	case SSPP_RGB2: return CID_RGB2;
+	case SSPP_DMA0: return CID_DMA0_Y + plane;
+	case SSPP_DMA1: return CID_DMA1_Y + plane;
+	case SSPP_VIG3: return CID_VIG3_Y + plane;
+	case SSPP_RGB3: return CID_RGB3;
+	default:        return CID_UNUSED;
+	}
+}
 
 /* step #1: update # of blocks pending for the client: */
-int mdp5_smp_request(struct mdp5_kms *mdp5_kms,
+static int smp_request_block(struct mdp5_smp *smp,
 		enum mdp5_client_id cid, int nblks)
 {
-	struct mdp5_client_smp_state *ps = &mdp5_kms->smp_client_state[cid];
-	int i, ret, avail, cur_nblks, cnt = mdp5_kms->smp_blk_cnt;
+	struct mdp5_kms *mdp5_kms = get_kms(smp);
+	const struct mdp5_cfg_hw *hw_cfg;
+	struct mdp5_client_smp_state *ps = &smp->client_state[cid];
+	int i, ret, avail, cur_nblks, cnt = smp->blk_cnt;
+	int reserved;
 	unsigned long flags;
 
-	spin_lock_irqsave(&smp_lock, flags);
+	hw_cfg = mdp5_cfg_get_hw_config(mdp5_kms->cfg);
+	reserved = hw_cfg->smp.reserved[cid];
+
+	spin_lock_irqsave(&smp->state_lock, flags);
 
-	avail = cnt - bitmap_weight(mdp5_kms->smp_state, cnt);
+	nblks -= reserved;
+	if (reserved)
+		DBG("%d MMBs allocated (%d reserved)", nblks, reserved);
+
+	avail = cnt - bitmap_weight(smp->state, cnt);
 	if (nblks > avail) {
+		dev_err(mdp5_kms->dev->dev, "out of blks (req=%d > avail=%d)\n",
+				nblks, avail);
 		ret = -ENOSPC;
 		goto fail;
 	}
@@ -84,9 +135,9 @@ int mdp5_smp_request(struct mdp5_kms *mdp5_kms,
 	if (nblks > cur_nblks) {
 		/* grow the existing pending reservation: */
 		for (i = cur_nblks; i < nblks; i++) {
-			int blk = find_first_zero_bit(mdp5_kms->smp_state, cnt);
+			int blk = find_first_zero_bit(smp->state, cnt);
 			set_bit(blk, ps->pending);
-			set_bit(blk, mdp5_kms->smp_state);
+			set_bit(blk, smp->state);
 		}
 	} else {
 		/* shrink the existing pending reservation: */
@@ -98,15 +149,88 @@ int mdp5_smp_request(struct mdp5_kms *mdp5_kms,
 	}
 
 fail:
-	spin_unlock_irqrestore(&smp_lock, flags);
+	spin_unlock_irqrestore(&smp->state_lock, flags);
+	return 0;
+}
+
+static void set_fifo_thresholds(struct mdp5_smp *smp,
+		enum mdp5_pipe pipe, int nblks)
+{
+	struct mdp5_kms *mdp5_kms = get_kms(smp);
+	u32 smp_entries_per_blk = smp->blk_size / (128 / BITS_PER_BYTE);
+	u32 val;
+
+	/* 1/4 of SMP pool that is being fetched */
+	val = (nblks * smp_entries_per_blk) / 4;
+
+	mdp5_write(mdp5_kms, REG_MDP5_PIPE_REQPRIO_FIFO_WM_0(pipe), val * 1);
+	mdp5_write(mdp5_kms, REG_MDP5_PIPE_REQPRIO_FIFO_WM_1(pipe), val * 2);
+	mdp5_write(mdp5_kms, REG_MDP5_PIPE_REQPRIO_FIFO_WM_2(pipe), val * 3);
+}
+
+/*
+ * NOTE: looks like if horizontal decimation is used (if we supported that)
+ * then the width used to calculate SMP block requirements is the post-
+ * decimated width.  Ie. SMP buffering sits downstream of decimation (which
+ * presumably happens during the dma from scanout buffer).
+ */
+int mdp5_smp_request(struct mdp5_smp *smp, enum mdp5_pipe pipe, u32 fmt, u32 width)
+{
+	struct mdp5_kms *mdp5_kms = get_kms(smp);
+	struct drm_device *dev = mdp5_kms->dev;
+	int rev = mdp5_cfg_get_hw_rev(mdp5_kms->cfg);
+	int i, hsub, nplanes, nlines, nblks, ret;
+
+	nplanes = drm_format_num_planes(fmt);
+	hsub = drm_format_horz_chroma_subsampling(fmt);
+
+	/* different if BWC (compressed framebuffer?) enabled: */
+	nlines = 2;
+
+	for (i = 0, nblks = 0; i < nplanes; i++) {
+		int n, fetch_stride, cpp;
+
+		cpp = drm_format_plane_cpp(fmt, i);
+		fetch_stride = width * cpp / (i ? hsub : 1);
+
+		n = DIV_ROUND_UP(fetch_stride * nlines, smp->blk_size);
+
+		/* for hw rev v1.00 */
+		if (rev == 0)
+			n = roundup_pow_of_two(n);
+
+		DBG("%s[%d]: request %d SMP blocks", pipe2name(pipe), i, n);
+		ret = smp_request_block(smp, pipe2client(pipe, i), n);
+		if (ret) {
+			dev_err(dev->dev, "Cannot allocate %d SMP blocks: %d\n",
+					n, ret);
+			return ret;
+		}
+
+		nblks += n;
+	}
+
+	set_fifo_thresholds(smp, pipe, nblks);
+
 	return 0;
 }
 
-static void update_smp_state(struct mdp5_kms *mdp5_kms,
+/* Release SMP blocks for all clients of the pipe */
+void mdp5_smp_release(struct mdp5_smp *smp, enum mdp5_pipe pipe)
+{
+	int i, nblks;
+
+	for (i = 0, nblks = 0; i < pipe2nclients(pipe); i++)
+		smp_request_block(smp, pipe2client(pipe, i), 0);
+	set_fifo_thresholds(smp, pipe, 0);
+}
+
+static void update_smp_state(struct mdp5_smp *smp,
 		enum mdp5_client_id cid, mdp5_smp_state_t *assigned)
 {
-	int cnt = mdp5_kms->smp_blk_cnt;
-	uint32_t blk, val;
+	struct mdp5_kms *mdp5_kms = get_kms(smp);
+	int cnt = smp->blk_cnt;
+	u32 blk, val;
 
 	for_each_set_bit(blk, *assigned, cnt) {
 		int idx = blk / 3;
@@ -135,39 +259,80 @@ static void update_smp_state(struct mdp5_kms *mdp5_kms,
 }
 
 /* step #2: configure hw for union(pending, inuse): */
-void mdp5_smp_configure(struct mdp5_kms *mdp5_kms, enum mdp5_client_id cid)
+void mdp5_smp_configure(struct mdp5_smp *smp, enum mdp5_pipe pipe)
 {
-	struct mdp5_client_smp_state *ps = &mdp5_kms->smp_client_state[cid];
-	int cnt = mdp5_kms->smp_blk_cnt;
+	int cnt = smp->blk_cnt;
 	mdp5_smp_state_t assigned;
+	int i;
 
-	bitmap_or(assigned, ps->inuse, ps->pending, cnt);
-	update_smp_state(mdp5_kms, cid, &assigned);
+	for (i = 0; i < pipe2nclients(pipe); i++) {
+		enum mdp5_client_id cid = pipe2client(pipe, i);
+		struct mdp5_client_smp_state *ps = &smp->client_state[cid];
+
+		bitmap_or(assigned, ps->inuse, ps->pending, cnt);
+		update_smp_state(smp, cid, &assigned);
+	}
 }
 
 /* step #3: after vblank, copy pending -> inuse: */
-void mdp5_smp_commit(struct mdp5_kms *mdp5_kms, enum mdp5_client_id cid)
+void mdp5_smp_commit(struct mdp5_smp *smp, enum mdp5_pipe pipe)
 {
-	struct mdp5_client_smp_state *ps = &mdp5_kms->smp_client_state[cid];
-	int cnt = mdp5_kms->smp_blk_cnt;
+	int cnt = smp->blk_cnt;
 	mdp5_smp_state_t released;
+	int i;
+
+	for (i = 0; i < pipe2nclients(pipe); i++) {
+		enum mdp5_client_id cid = pipe2client(pipe, i);
+		struct mdp5_client_smp_state *ps = &smp->client_state[cid];
+
+		/*
+		 * Figure out if there are any blocks we where previously
+		 * using, which can be released and made available to other
+		 * clients:
+		 */
+		if (bitmap_andnot(released, ps->inuse, ps->pending, cnt)) {
+			unsigned long flags;
+
+			spin_lock_irqsave(&smp->state_lock, flags);
+			/* clear released blocks: */
+			bitmap_andnot(smp->state, smp->state, released, cnt);
+			spin_unlock_irqrestore(&smp->state_lock, flags);
 
-	/*
-	 * Figure out if there are any blocks we where previously
-	 * using, which can be released and made available to other
-	 * clients:
-	 */
-	if (bitmap_andnot(released, ps->inuse, ps->pending, cnt)) {
-		unsigned long flags;
-
-		spin_lock_irqsave(&smp_lock, flags);
-		/* clear released blocks: */
-		bitmap_andnot(mdp5_kms->smp_state, mdp5_kms->smp_state,
-				released, cnt);
-		spin_unlock_irqrestore(&smp_lock, flags);
-
-		update_smp_state(mdp5_kms, CID_UNUSED, &released);
+			update_smp_state(smp, CID_UNUSED, &released);
+		}
+
+		bitmap_copy(ps->inuse, ps->pending, cnt);
 	}
+}
+
+void mdp5_smp_destroy(struct mdp5_smp *smp)
+{
+	kfree(smp);
+}
+
+struct mdp5_smp *mdp5_smp_init(struct drm_device *dev, const struct mdp5_smp_block *cfg)
+{
+	struct mdp5_smp *smp = NULL;
+	int ret;
+
+	smp = kzalloc(sizeof(*smp), GFP_KERNEL);
+	if (unlikely(!smp)) {
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	smp->dev = dev;
+	smp->blk_cnt = cfg->mmb_count;
+	smp->blk_size = cfg->mmb_size;
+
+	/* statically tied MMBs cannot be re-allocated: */
+	bitmap_copy(smp->state, cfg->reserved_state, smp->blk_cnt);
+	spin_lock_init(&smp->state_lock);
+
+	return smp;
+fail:
+	if (smp)
+		mdp5_smp_destroy(smp);
 
-	bitmap_copy(ps->inuse, ps->pending, cnt);
+	return ERR_PTR(ret);
 }
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_smp.h b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_smp.h
index 0ab739e1a1dd..e47179f63585 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_smp.h
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_smp.h
@@ -1,4 +1,5 @@
 /*
+ * Copyright (c) 2014, The Linux Foundation. All rights reserved.
  * Copyright (C) 2013 Red Hat
  * Author: Rob Clark <robdclark@gmail.com>
  *
@@ -20,22 +21,26 @@
 
 #include "msm_drv.h"
 
-#define MAX_SMP_BLOCKS  22
-#define SMP_BLK_SIZE    4096
-#define SMP_ENTRIES_PER_BLK (SMP_BLK_SIZE / 16)
-
-typedef DECLARE_BITMAP(mdp5_smp_state_t, MAX_SMP_BLOCKS);
-
 struct mdp5_client_smp_state {
 	mdp5_smp_state_t inuse;
 	mdp5_smp_state_t pending;
 };
 
 struct mdp5_kms;
+struct mdp5_smp;
+
+/*
+ * SMP module prototypes:
+ * mdp5_smp_init() returns a SMP @handler,
+ * which is then used to call the other mdp5_smp_*(handler, ...) functions.
+ */
 
-int mdp5_smp_request(struct mdp5_kms *mdp5_kms, enum mdp5_client_id cid, int nblks);
-void mdp5_smp_configure(struct mdp5_kms *mdp5_kms, enum mdp5_client_id cid);
-void mdp5_smp_commit(struct mdp5_kms *mdp5_kms, enum mdp5_client_id cid);
+struct mdp5_smp *mdp5_smp_init(struct drm_device *dev, const struct mdp5_smp_block *cfg);
+void  mdp5_smp_destroy(struct mdp5_smp *smp);
 
+int  mdp5_smp_request(struct mdp5_smp *smp, enum mdp5_pipe pipe, u32 fmt, u32 width);
+void mdp5_smp_configure(struct mdp5_smp *smp, enum mdp5_pipe pipe);
+void mdp5_smp_commit(struct mdp5_smp *smp, enum mdp5_pipe pipe);
+void mdp5_smp_release(struct mdp5_smp *smp, enum mdp5_pipe pipe);
 
 #endif /* __MDP5_SMP_H__ */
diff --git a/drivers/gpu/drm/msm/mdp/mdp_kms.c b/drivers/gpu/drm/msm/mdp/mdp_kms.c
index 03455b64a245..2a731722d840 100644
--- a/drivers/gpu/drm/msm/mdp/mdp_kms.c
+++ b/drivers/gpu/drm/msm/mdp/mdp_kms.c
@@ -42,7 +42,10 @@ static void update_irq(struct mdp_kms *mdp_kms)
 	mdp_kms->funcs->set_irqmask(mdp_kms, irqmask);
 }
 
-static void update_irq_unlocked(struct mdp_kms *mdp_kms)
+/* if an mdp_irq's irqmask has changed, such as when mdp5 crtc<->encoder
+ * link changes, this must be called to figure out the new global irqmask
+ */
+void mdp_irq_update(struct mdp_kms *mdp_kms)
 {
 	unsigned long flags;
 	spin_lock_irqsave(&list_lock, flags);
@@ -122,7 +125,7 @@ void mdp_irq_register(struct mdp_kms *mdp_kms, struct mdp_irq *irq)
 	spin_unlock_irqrestore(&list_lock, flags);
 
 	if (needs_update)
-		update_irq_unlocked(mdp_kms);
+		mdp_irq_update(mdp_kms);
 }
 
 void mdp_irq_unregister(struct mdp_kms *mdp_kms, struct mdp_irq *irq)
@@ -141,5 +144,5 @@ void mdp_irq_unregister(struct mdp_kms *mdp_kms, struct mdp_irq *irq)
 	spin_unlock_irqrestore(&list_lock, flags);
 
 	if (needs_update)
-		update_irq_unlocked(mdp_kms);
+		mdp_irq_update(mdp_kms);
 }
diff --git a/drivers/gpu/drm/msm/mdp/mdp_kms.h b/drivers/gpu/drm/msm/mdp/mdp_kms.h
index 99557b5ad4fd..b268ce95d394 100644
--- a/drivers/gpu/drm/msm/mdp/mdp_kms.h
+++ b/drivers/gpu/drm/msm/mdp/mdp_kms.h
@@ -75,7 +75,7 @@ void mdp_update_vblank_mask(struct mdp_kms *mdp_kms, uint32_t mask, bool enable)
 void mdp_irq_wait(struct mdp_kms *mdp_kms, uint32_t irqmask);
 void mdp_irq_register(struct mdp_kms *mdp_kms, struct mdp_irq *irq);
 void mdp_irq_unregister(struct mdp_kms *mdp_kms, struct mdp_irq *irq);
-
+void mdp_irq_update(struct mdp_kms *mdp_kms);
 
 /*
  * pixel format helpers:
diff --git a/drivers/gpu/drm/msm/msm_atomic.c b/drivers/gpu/drm/msm/msm_atomic.c
new file mode 100644
index 000000000000..191968256c58
--- /dev/null
+++ b/drivers/gpu/drm/msm/msm_atomic.c
@@ -0,0 +1,230 @@
+/*
+ * Copyright (C) 2014 Red Hat
+ * Author: Rob Clark <robdclark@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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "msm_drv.h"
+#include "msm_kms.h"
+#include "msm_gem.h"
+
+struct msm_commit {
+	struct drm_atomic_state *state;
+	uint32_t fence;
+	struct msm_fence_cb fence_cb;
+	uint32_t crtc_mask;
+};
+
+static void fence_cb(struct msm_fence_cb *cb);
+
+/* block until specified crtcs are no longer pending update, and
+ * atomically mark them as pending update
+ */
+static int start_atomic(struct msm_drm_private *priv, uint32_t crtc_mask)
+{
+	int ret;
+
+	spin_lock(&priv->pending_crtcs_event.lock);
+	ret = wait_event_interruptible_locked(priv->pending_crtcs_event,
+			!(priv->pending_crtcs & crtc_mask));
+	if (ret == 0) {
+		DBG("start: %08x", crtc_mask);
+		priv->pending_crtcs |= crtc_mask;
+	}
+	spin_unlock(&priv->pending_crtcs_event.lock);
+
+	return ret;
+}
+
+/* clear specified crtcs (no longer pending update)
+ */
+static void end_atomic(struct msm_drm_private *priv, uint32_t crtc_mask)
+{
+	spin_lock(&priv->pending_crtcs_event.lock);
+	DBG("end: %08x", crtc_mask);
+	priv->pending_crtcs &= ~crtc_mask;
+	wake_up_all_locked(&priv->pending_crtcs_event);
+	spin_unlock(&priv->pending_crtcs_event.lock);
+}
+
+static struct msm_commit *new_commit(struct drm_atomic_state *state)
+{
+	struct msm_commit *c = kzalloc(sizeof(*c), GFP_KERNEL);
+
+	if (!c)
+		return NULL;
+
+	c->state = state;
+	/* TODO we might need a way to indicate to run the cb on a
+	 * different wq so wait_for_vblanks() doesn't block retiring
+	 * bo's..
+	 */
+	INIT_FENCE_CB(&c->fence_cb, fence_cb);
+
+	return c;
+}
+
+/* The (potentially) asynchronous part of the commit.  At this point
+ * nothing can fail short of armageddon.
+ */
+static void complete_commit(struct msm_commit *c)
+{
+	struct drm_atomic_state *state = c->state;
+	struct drm_device *dev = state->dev;
+
+	drm_atomic_helper_commit_pre_planes(dev, state);
+
+	drm_atomic_helper_commit_planes(dev, state);
+
+	drm_atomic_helper_commit_post_planes(dev, state);
+
+	/* NOTE: _wait_for_vblanks() only waits for vblank on
+	 * enabled CRTCs.  So we end up faulting when disabling
+	 * due to (potentially) unref'ing the outgoing fb's
+	 * before the vblank when the disable has latched.
+	 *
+	 * But if it did wait on disabled (or newly disabled)
+	 * CRTCs, that would be racy (ie. we could have missed
+	 * the irq.  We need some way to poll for pipe shut
+	 * down.  Or just live with occasionally hitting the
+	 * timeout in the CRTC disable path (which really should
+	 * not be critical path)
+	 */
+
+	drm_atomic_helper_wait_for_vblanks(dev, state);
+
+	drm_atomic_helper_cleanup_planes(dev, state);
+
+	drm_atomic_state_free(state);
+
+	end_atomic(dev->dev_private, c->crtc_mask);
+
+	kfree(c);
+}
+
+static void fence_cb(struct msm_fence_cb *cb)
+{
+	struct msm_commit *c =
+			container_of(cb, struct msm_commit, fence_cb);
+	complete_commit(c);
+}
+
+static void add_fb(struct msm_commit *c, struct drm_framebuffer *fb)
+{
+	struct drm_gem_object *obj = msm_framebuffer_bo(fb, 0);
+	c->fence = max(c->fence, msm_gem_fence(to_msm_bo(obj), MSM_PREP_READ));
+}
+
+
+/**
+ * drm_atomic_helper_commit - commit validated state object
+ * @dev: DRM device
+ * @state: the driver state object
+ * @async: asynchronous commit
+ *
+ * This function commits a with drm_atomic_helper_check() pre-validated state
+ * object. This can still fail when e.g. the framebuffer reservation fails. For
+ * now this doesn't implement asynchronous commits.
+ *
+ * RETURNS
+ * Zero for success or -errno.
+ */
+int msm_atomic_commit(struct drm_device *dev,
+		struct drm_atomic_state *state, bool async)
+{
+	int nplanes = dev->mode_config.num_total_plane;
+	int ncrtcs = dev->mode_config.num_crtc;
+	struct msm_commit *c;
+	int i, ret;
+
+	ret = drm_atomic_helper_prepare_planes(dev, state);
+	if (ret)
+		return ret;
+
+	c = new_commit(state);
+	if (!c)
+		return -ENOMEM;
+
+	/*
+	 * Figure out what crtcs we have:
+	 */
+	for (i = 0; i < ncrtcs; i++) {
+		struct drm_crtc *crtc = state->crtcs[i];
+		if (!crtc)
+			continue;
+		c->crtc_mask |= (1 << drm_crtc_index(crtc));
+	}
+
+	/*
+	 * Figure out what fence to wait for:
+	 */
+	for (i = 0; i < nplanes; i++) {
+		struct drm_plane *plane = state->planes[i];
+		struct drm_plane_state *new_state = state->plane_states[i];
+
+		if (!plane)
+			continue;
+
+		if ((plane->state->fb != new_state->fb) && new_state->fb)
+			add_fb(c, new_state->fb);
+	}
+
+	/*
+	 * Wait for pending updates on any of the same crtc's and then
+	 * mark our set of crtc's as busy:
+	 */
+	ret = start_atomic(dev->dev_private, c->crtc_mask);
+	if (ret)
+		return ret;
+
+	/*
+	 * This is the point of no return - everything below never fails except
+	 * when the hw goes bonghits. Which means we can commit the new state on
+	 * the software side now.
+	 */
+
+	drm_atomic_helper_swap_state(dev, state);
+
+	/*
+	 * Everything below can be run asynchronously without the need to grab
+	 * any modeset locks at all under one conditions: It must be guaranteed
+	 * that the asynchronous work has either been cancelled (if the driver
+	 * supports it, which at least requires that the framebuffers get
+	 * cleaned up with drm_atomic_helper_cleanup_planes()) or completed
+	 * before the new state gets committed on the software side with
+	 * drm_atomic_helper_swap_state().
+	 *
+	 * This scheme allows new atomic state updates to be prepared and
+	 * checked in parallel to the asynchronous completion of the previous
+	 * update. Which is important since compositors need to figure out the
+	 * composition of the next frame right after having submitted the
+	 * current layout.
+	 */
+
+	if (async) {
+		msm_queue_fence_cb(dev, &c->fence_cb, c->fence);
+		return 0;
+	}
+
+	ret = msm_wait_fence_interruptable(dev, c->fence, NULL);
+	if (ret) {
+		WARN_ON(ret);  // TODO unswap state back?  or??
+		kfree(c);
+		return ret;
+	}
+
+	complete_commit(c);
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/msm/msm_drv.c b/drivers/gpu/drm/msm/msm_drv.c
index b67ef5985125..9a61546a0b05 100644
--- a/drivers/gpu/drm/msm/msm_drv.c
+++ b/drivers/gpu/drm/msm/msm_drv.c
@@ -29,6 +29,8 @@ static void msm_fb_output_poll_changed(struct drm_device *dev)
 static const struct drm_mode_config_funcs mode_config_funcs = {
 	.fb_create = msm_framebuffer_create,
 	.output_poll_changed = msm_fb_output_poll_changed,
+	.atomic_check = drm_atomic_helper_check,
+	.atomic_commit = msm_atomic_commit,
 };
 
 int msm_register_mmu(struct drm_device *dev, struct msm_mmu *mmu)
@@ -191,6 +193,7 @@ static int msm_load(struct drm_device *dev, unsigned long flags)
 
 	priv->wq = alloc_ordered_workqueue("msm", 0);
 	init_waitqueue_head(&priv->fence_event);
+	init_waitqueue_head(&priv->pending_crtcs_event);
 
 	INIT_LIST_HEAD(&priv->inactive_list);
 	INIT_LIST_HEAD(&priv->fence_cbs);
@@ -294,6 +297,8 @@ static int msm_load(struct drm_device *dev, unsigned long flags)
 		goto fail;
 	}
 
+	drm_mode_config_reset(dev);
+
 #ifdef CONFIG_DRM_MSM_FBDEV
 	priv->fbdev = msm_fbdev_init(dev);
 #endif
@@ -619,6 +624,26 @@ int msm_wait_fence_interruptable(struct drm_device *dev, uint32_t fence,
 	return ret;
 }
 
+int msm_queue_fence_cb(struct drm_device *dev,
+		struct msm_fence_cb *cb, uint32_t fence)
+{
+	struct msm_drm_private *priv = dev->dev_private;
+	int ret = 0;
+
+	mutex_lock(&dev->struct_mutex);
+	if (!list_empty(&cb->work.entry)) {
+		ret = -EINVAL;
+	} else if (fence > priv->completed_fence) {
+		cb->fence = fence;
+		list_add_tail(&cb->work.entry, &priv->fence_cbs);
+	} else {
+		queue_work(priv->wq, &cb->work);
+	}
+	mutex_unlock(&dev->struct_mutex);
+
+	return ret;
+}
+
 /* called from workqueue */
 void msm_update_fence(struct drm_device *dev, uint32_t fence)
 {
@@ -832,6 +857,7 @@ static struct drm_driver msm_driver = {
 	.gem_prime_import_sg_table = msm_gem_prime_import_sg_table,
 	.gem_prime_vmap     = msm_gem_prime_vmap,
 	.gem_prime_vunmap   = msm_gem_prime_vunmap,
+	.gem_prime_mmap     = msm_gem_prime_mmap,
 #ifdef CONFIG_DEBUG_FS
 	.debugfs_init       = msm_debugfs_init,
 	.debugfs_cleanup    = msm_debugfs_cleanup,
@@ -987,7 +1013,6 @@ static struct platform_driver msm_platform_driver = {
 	.probe      = msm_pdev_probe,
 	.remove     = msm_pdev_remove,
 	.driver     = {
-		.owner  = THIS_MODULE,
 		.name   = "msm",
 		.of_match_table = dt_match,
 		.pm     = &msm_pm_ops,
diff --git a/drivers/gpu/drm/msm/msm_drv.h b/drivers/gpu/drm/msm/msm_drv.h
index 67f9d0a2332c..b69ef2d5a26c 100644
--- a/drivers/gpu/drm/msm/msm_drv.h
+++ b/drivers/gpu/drm/msm/msm_drv.h
@@ -32,15 +32,6 @@
 #include <linux/types.h>
 #include <asm/sizes.h>
 
-
-#if defined(CONFIG_COMPILE_TEST) && !defined(CONFIG_ARCH_QCOM)
-/* stubs we need for compile-test: */
-static inline struct device *msm_iommu_get_ctx(const char *ctx_name)
-{
-	return NULL;
-}
-#endif
-
 #ifndef CONFIG_OF
 #include <mach/board.h>
 #include <mach/socinfo.h>
@@ -48,7 +39,10 @@ static inline struct device *msm_iommu_get_ctx(const char *ctx_name)
 #endif
 
 #include <drm/drmP.h>
+#include <drm/drm_atomic.h>
+#include <drm/drm_atomic_helper.h>
 #include <drm/drm_crtc_helper.h>
+#include <drm/drm_plane_helper.h>
 #include <drm/drm_fb_helper.h>
 #include <drm/msm_drm.h>
 #include <drm/drm_gem.h>
@@ -75,7 +69,12 @@ struct msm_drm_private {
 	struct msm_kms *kms;
 
 	/* subordinate devices, if present: */
-	struct platform_device *hdmi_pdev, *gpu_pdev;
+	struct platform_device *gpu_pdev;
+
+	/* possibly this should be in the kms component, but it is
+	 * shared by both mdp4 and mdp5..
+	 */
+	struct hdmi *hdmi;
 
 	/* when we have more than one 'msm_gpu' these need to be an array: */
 	struct msm_gpu *gpu;
@@ -97,6 +96,10 @@ struct msm_drm_private {
 	/* callbacks deferred until bo is inactive: */
 	struct list_head fence_cbs;
 
+	/* crtcs pending async atomic updates: */
+	uint32_t pending_crtcs;
+	wait_queue_head_t pending_crtcs_event;
+
 	/* registered MMUs: */
 	unsigned int num_mmus;
 	struct msm_mmu *mmus[NUM_DOMAINS];
@@ -145,21 +148,29 @@ void __msm_fence_worker(struct work_struct *work);
 		(_cb)->func = _func;                         \
 	} while (0)
 
+int msm_atomic_commit(struct drm_device *dev,
+		struct drm_atomic_state *state, bool async);
+
 int msm_register_mmu(struct drm_device *dev, struct msm_mmu *mmu);
 
 int msm_wait_fence_interruptable(struct drm_device *dev, uint32_t fence,
 		struct timespec *timeout);
+int msm_queue_fence_cb(struct drm_device *dev,
+		struct msm_fence_cb *cb, uint32_t fence);
 void msm_update_fence(struct drm_device *dev, uint32_t fence);
 
 int msm_ioctl_gem_submit(struct drm_device *dev, void *data,
 		struct drm_file *file);
 
+int msm_gem_mmap_obj(struct drm_gem_object *obj,
+			struct vm_area_struct *vma);
 int msm_gem_mmap(struct file *filp, struct vm_area_struct *vma);
 int msm_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf);
 uint64_t msm_gem_mmap_offset(struct drm_gem_object *obj);
 int msm_gem_get_iova_locked(struct drm_gem_object *obj, int id,
 		uint32_t *iova);
 int msm_gem_get_iova(struct drm_gem_object *obj, int id, uint32_t *iova);
+uint32_t msm_gem_iova(struct drm_gem_object *obj, int id);
 struct page **msm_gem_get_pages(struct drm_gem_object *obj);
 void msm_gem_put_pages(struct drm_gem_object *obj);
 void msm_gem_put_iova(struct drm_gem_object *obj, int id);
@@ -170,6 +181,7 @@ int msm_gem_dumb_map_offset(struct drm_file *file, struct drm_device *dev,
 struct sg_table *msm_gem_prime_get_sg_table(struct drm_gem_object *obj);
 void *msm_gem_prime_vmap(struct drm_gem_object *obj);
 void msm_gem_prime_vunmap(struct drm_gem_object *obj, void *vaddr);
+int msm_gem_prime_mmap(struct drm_gem_object *obj, struct vm_area_struct *vma);
 struct drm_gem_object *msm_gem_prime_import_sg_table(struct drm_device *dev,
 		struct dma_buf_attachment *attach, struct sg_table *sg);
 int msm_gem_prime_pin(struct drm_gem_object *obj);
@@ -192,6 +204,9 @@ struct drm_gem_object *msm_gem_new(struct drm_device *dev,
 struct drm_gem_object *msm_gem_import(struct drm_device *dev,
 		uint32_t size, struct sg_table *sgt);
 
+int msm_framebuffer_prepare(struct drm_framebuffer *fb, int id);
+void msm_framebuffer_cleanup(struct drm_framebuffer *fb, int id);
+uint32_t msm_framebuffer_iova(struct drm_framebuffer *fb, int id, int plane);
 struct drm_gem_object *msm_framebuffer_bo(struct drm_framebuffer *fb, int plane);
 const struct msm_format *msm_framebuffer_format(struct drm_framebuffer *fb);
 struct drm_framebuffer *msm_framebuffer_init(struct drm_device *dev,
@@ -202,8 +217,8 @@ struct drm_framebuffer *msm_framebuffer_create(struct drm_device *dev,
 struct drm_fb_helper *msm_fbdev_init(struct drm_device *dev);
 
 struct hdmi;
-struct hdmi *hdmi_init(struct drm_device *dev, struct drm_encoder *encoder);
-irqreturn_t hdmi_irq(int irq, void *dev_id);
+int hdmi_modeset_init(struct hdmi *hdmi, struct drm_device *dev,
+		struct drm_encoder *encoder);
 void __init hdmi_register(void);
 void __exit hdmi_unregister(void);
 
diff --git a/drivers/gpu/drm/msm/msm_fb.c b/drivers/gpu/drm/msm/msm_fb.c
index 81bafdf19ab3..84dec161d836 100644
--- a/drivers/gpu/drm/msm/msm_fb.c
+++ b/drivers/gpu/drm/msm/msm_fb.c
@@ -24,7 +24,7 @@
 struct msm_framebuffer {
 	struct drm_framebuffer base;
 	const struct msm_format *format;
-	struct drm_gem_object *planes[2];
+	struct drm_gem_object *planes[3];
 };
 #define to_msm_framebuffer(x) container_of(x, struct msm_framebuffer, base)
 
@@ -87,6 +87,44 @@ void msm_framebuffer_describe(struct drm_framebuffer *fb, struct seq_file *m)
 }
 #endif
 
+/* prepare/pin all the fb's bo's for scanout.  Note that it is not valid
+ * to prepare an fb more multiple different initiator 'id's.  But that
+ * should be fine, since only the scanout (mdpN) side of things needs
+ * this, the gpu doesn't care about fb's.
+ */
+int msm_framebuffer_prepare(struct drm_framebuffer *fb, int id)
+{
+	struct msm_framebuffer *msm_fb = to_msm_framebuffer(fb);
+	int ret, i, n = drm_format_num_planes(fb->pixel_format);
+	uint32_t iova;
+
+	for (i = 0; i < n; i++) {
+		ret = msm_gem_get_iova(msm_fb->planes[i], id, &iova);
+		DBG("FB[%u]: iova[%d]: %08x (%d)", fb->base.id, i, iova, ret);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+void msm_framebuffer_cleanup(struct drm_framebuffer *fb, int id)
+{
+	struct msm_framebuffer *msm_fb = to_msm_framebuffer(fb);
+	int i, n = drm_format_num_planes(fb->pixel_format);
+
+	for (i = 0; i < n; i++)
+		msm_gem_put_iova(msm_fb->planes[i], id);
+}
+
+uint32_t msm_framebuffer_iova(struct drm_framebuffer *fb, int id, int plane)
+{
+	struct msm_framebuffer *msm_fb = to_msm_framebuffer(fb);
+	if (!msm_fb->planes[plane])
+		return 0;
+	return msm_gem_iova(msm_fb->planes[plane], id);
+}
+
 struct drm_gem_object *msm_framebuffer_bo(struct drm_framebuffer *fb, int plane)
 {
 	struct msm_framebuffer *msm_fb = to_msm_framebuffer(fb);
@@ -166,6 +204,11 @@ struct drm_framebuffer *msm_framebuffer_init(struct drm_device *dev,
 
 	msm_fb->format = format;
 
+	if (n > ARRAY_SIZE(msm_fb->planes)) {
+		ret = -EINVAL;
+		goto fail;
+	}
+
 	for (i = 0; i < n; i++) {
 		unsigned int width = mode_cmd->width / (i ? hsub : 1);
 		unsigned int height = mode_cmd->height / (i ? vsub : 1);
diff --git a/drivers/gpu/drm/msm/msm_fbdev.c b/drivers/gpu/drm/msm/msm_fbdev.c
index ab5bfd2d0ebf..1f3af13ccede 100644
--- a/drivers/gpu/drm/msm/msm_fbdev.c
+++ b/drivers/gpu/drm/msm/msm_fbdev.c
@@ -93,9 +93,6 @@ static int msm_fbdev_create(struct drm_fb_helper *helper,
 	uint32_t paddr;
 	int ret, size;
 
-	sizes->surface_bpp = 32;
-	sizes->surface_depth = 24;
-
 	DBG("create fbdev: %dx%d@%d (%dx%d)", sizes->surface_width,
 			sizes->surface_height, sizes->surface_bpp,
 			sizes->fb_width, sizes->fb_height);
@@ -193,8 +190,7 @@ fail_unlock:
 fail:
 
 	if (ret) {
-		if (fbi)
-			framebuffer_release(fbi);
+		framebuffer_release(fbi);
 		if (fb) {
 			drm_framebuffer_unregister_private(fb);
 			drm_framebuffer_remove(fb);
diff --git a/drivers/gpu/drm/msm/msm_gem.c b/drivers/gpu/drm/msm/msm_gem.c
index 4b1b82adabde..49dea4fb55ac 100644
--- a/drivers/gpu/drm/msm/msm_gem.c
+++ b/drivers/gpu/drm/msm/msm_gem.c
@@ -309,6 +309,7 @@ int msm_gem_get_iova_locked(struct drm_gem_object *obj, int id,
 	return ret;
 }
 
+/* get iova, taking a reference.  Should have a matching put */
 int msm_gem_get_iova(struct drm_gem_object *obj, int id, uint32_t *iova)
 {
 	struct msm_gem_object *msm_obj = to_msm_bo(obj);
@@ -328,6 +329,16 @@ int msm_gem_get_iova(struct drm_gem_object *obj, int id, uint32_t *iova)
 	return ret;
 }
 
+/* get iova without taking a reference, used in places where you have
+ * already done a 'msm_gem_get_iova()'.
+ */
+uint32_t msm_gem_iova(struct drm_gem_object *obj, int id)
+{
+	struct msm_gem_object *msm_obj = to_msm_bo(obj);
+	WARN_ON(!msm_obj->domain[id].iova);
+	return msm_obj->domain[id].iova;
+}
+
 void msm_gem_put_iova(struct drm_gem_object *obj, int id)
 {
 	// XXX TODO ..
@@ -397,23 +408,10 @@ void *msm_gem_vaddr(struct drm_gem_object *obj)
 int msm_gem_queue_inactive_cb(struct drm_gem_object *obj,
 		struct msm_fence_cb *cb)
 {
-	struct drm_device *dev = obj->dev;
-	struct msm_drm_private *priv = dev->dev_private;
 	struct msm_gem_object *msm_obj = to_msm_bo(obj);
-	int ret = 0;
-
-	mutex_lock(&dev->struct_mutex);
-	if (!list_empty(&cb->work.entry)) {
-		ret = -EINVAL;
-	} else if (is_active(msm_obj)) {
-		cb->fence = max(msm_obj->read_fence, msm_obj->write_fence);
-		list_add_tail(&cb->work.entry, &priv->fence_cbs);
-	} else {
-		queue_work(priv->wq, &cb->work);
-	}
-	mutex_unlock(&dev->struct_mutex);
-
-	return ret;
+	uint32_t fence = msm_gem_fence(msm_obj,
+			MSM_PREP_READ | MSM_PREP_WRITE);
+	return msm_queue_fence_cb(obj->dev, cb, fence);
 }
 
 void msm_gem_move_to_active(struct drm_gem_object *obj,
@@ -452,12 +450,8 @@ int msm_gem_cpu_prep(struct drm_gem_object *obj, uint32_t op,
 	int ret = 0;
 
 	if (is_active(msm_obj)) {
-		uint32_t fence = 0;
+		uint32_t fence = msm_gem_fence(msm_obj, op);
 
-		if (op & MSM_PREP_READ)
-			fence = msm_obj->write_fence;
-		if (op & MSM_PREP_WRITE)
-			fence = max(fence, msm_obj->read_fence);
 		if (op & MSM_PREP_NOSYNC)
 			timeout = NULL;
 
@@ -525,13 +519,11 @@ void msm_gem_free_object(struct drm_gem_object *obj)
 	for (id = 0; id < ARRAY_SIZE(msm_obj->domain); id++) {
 		struct msm_mmu *mmu = priv->mmus[id];
 		if (mmu && msm_obj->domain[id].iova) {
-			uint32_t offset = (uint32_t)mmap_offset(obj);
+			uint32_t offset = msm_obj->domain[id].iova;
 			mmu->funcs->unmap(mmu, offset, msm_obj->sgt, obj->size);
 		}
 	}
 
-	drm_gem_free_mmap_offset(obj);
-
 	if (obj->import_attach) {
 		if (msm_obj->vaddr)
 			dma_buf_vunmap(obj->import_attach->dmabuf, msm_obj->vaddr);
@@ -543,8 +535,7 @@ void msm_gem_free_object(struct drm_gem_object *obj)
 			drm_free_large(msm_obj->pages);
 
 	} else {
-		if (msm_obj->vaddr)
-			vunmap(msm_obj->vaddr);
+		vunmap(msm_obj->vaddr);
 		put_pages(obj);
 	}
 
diff --git a/drivers/gpu/drm/msm/msm_gem.h b/drivers/gpu/drm/msm/msm_gem.h
index bfb052688f8e..8fbbd0594c46 100644
--- a/drivers/gpu/drm/msm/msm_gem.h
+++ b/drivers/gpu/drm/msm/msm_gem.h
@@ -70,6 +70,19 @@ static inline bool is_active(struct msm_gem_object *msm_obj)
 	return msm_obj->gpu != NULL;
 }
 
+static inline uint32_t msm_gem_fence(struct msm_gem_object *msm_obj,
+		uint32_t op)
+{
+	uint32_t fence = 0;
+
+	if (op & MSM_PREP_READ)
+		fence = msm_obj->write_fence;
+	if (op & MSM_PREP_WRITE)
+		fence = max(fence, msm_obj->read_fence);
+
+	return fence;
+}
+
 #define MAX_CMDS 4
 
 /* Created per submit-ioctl, to track bo's and cmdstream bufs, etc,
diff --git a/drivers/gpu/drm/msm/msm_gem_prime.c b/drivers/gpu/drm/msm/msm_gem_prime.c
index ad772fe36115..dd7a7ab603e2 100644
--- a/drivers/gpu/drm/msm/msm_gem_prime.c
+++ b/drivers/gpu/drm/msm/msm_gem_prime.c
@@ -37,6 +37,19 @@ void msm_gem_prime_vunmap(struct drm_gem_object *obj, void *vaddr)
 	/* TODO msm_gem_vunmap() */
 }
 
+int msm_gem_prime_mmap(struct drm_gem_object *obj, struct vm_area_struct *vma)
+{
+	int ret;
+
+	mutex_lock(&obj->dev->struct_mutex);
+	ret = drm_gem_mmap_obj(obj, obj->size, vma);
+	mutex_unlock(&obj->dev->struct_mutex);
+	if (ret < 0)
+		return ret;
+
+	return msm_gem_mmap_obj(vma->vm_private_data, vma);
+}
+
 struct drm_gem_object *msm_gem_prime_import_sg_table(struct drm_device *dev,
 		struct dma_buf_attachment *attach, struct sg_table *sg)
 {
diff --git a/drivers/gpu/drm/nouveau/Makefile b/drivers/gpu/drm/nouveau/Makefile
index 12c24c8abf7f..6461e3565afe 100644
--- a/drivers/gpu/drm/nouveau/Makefile
+++ b/drivers/gpu/drm/nouveau/Makefile
@@ -41,17 +41,28 @@ nouveau-y += core/subdev/bios/extdev.o
 nouveau-y += core/subdev/bios/fan.o
 nouveau-y += core/subdev/bios/gpio.o
 nouveau-y += core/subdev/bios/i2c.o
+nouveau-y += core/subdev/bios/image.o
 nouveau-y += core/subdev/bios/init.o
 nouveau-y += core/subdev/bios/mxm.o
+nouveau-y += core/subdev/bios/npde.o
+nouveau-y += core/subdev/bios/pcir.o
 nouveau-y += core/subdev/bios/perf.o
 nouveau-y += core/subdev/bios/pll.o
+nouveau-y += core/subdev/bios/pmu.o
 nouveau-y += core/subdev/bios/ramcfg.o
 nouveau-y += core/subdev/bios/rammap.o
+nouveau-y += core/subdev/bios/shadow.o
+nouveau-y += core/subdev/bios/shadowacpi.o
+nouveau-y += core/subdev/bios/shadowof.o
+nouveau-y += core/subdev/bios/shadowpci.o
+nouveau-y += core/subdev/bios/shadowramin.o
+nouveau-y += core/subdev/bios/shadowrom.o
 nouveau-y += core/subdev/bios/timing.o
 nouveau-y += core/subdev/bios/therm.o
 nouveau-y += core/subdev/bios/vmap.o
 nouveau-y += core/subdev/bios/volt.o
 nouveau-y += core/subdev/bios/xpio.o
+nouveau-y += core/subdev/bios/M0203.o
 nouveau-y += core/subdev/bios/M0205.o
 nouveau-y += core/subdev/bios/M0209.o
 nouveau-y += core/subdev/bios/P0260.o
@@ -86,6 +97,7 @@ nouveau-y += core/subdev/devinit/nva3.o
 nouveau-y += core/subdev/devinit/nvaf.o
 nouveau-y += core/subdev/devinit/nvc0.o
 nouveau-y += core/subdev/devinit/gm107.o
+nouveau-y += core/subdev/devinit/gm204.o
 nouveau-y += core/subdev/fb/base.o
 nouveau-y += core/subdev/fb/nv04.o
 nouveau-y += core/subdev/fb/nv10.o
@@ -129,6 +141,7 @@ nouveau-y += core/subdev/fb/ramgk20a.o
 nouveau-y += core/subdev/fb/ramgm107.o
 nouveau-y += core/subdev/fb/sddr2.o
 nouveau-y += core/subdev/fb/sddr3.o
+nouveau-y += core/subdev/fb/gddr3.o
 nouveau-y += core/subdev/fb/gddr5.o
 nouveau-y += core/subdev/fuse/base.o
 nouveau-y += core/subdev/fuse/g80.o
@@ -147,6 +160,7 @@ nouveau-y += core/subdev/i2c/bit.o
 nouveau-y += core/subdev/i2c/pad.o
 nouveau-y += core/subdev/i2c/padnv04.o
 nouveau-y += core/subdev/i2c/padnv94.o
+nouveau-y += core/subdev/i2c/padgm204.o
 nouveau-y += core/subdev/i2c/nv04.o
 nouveau-y += core/subdev/i2c/nv4e.o
 nouveau-y += core/subdev/i2c/nv50.o
@@ -154,6 +168,7 @@ nouveau-y += core/subdev/i2c/nv94.o
 nouveau-y += core/subdev/i2c/nvd0.o
 nouveau-y += core/subdev/i2c/gf117.o
 nouveau-y += core/subdev/i2c/nve0.o
+nouveau-y += core/subdev/i2c/gm204.o
 nouveau-y += core/subdev/ibus/nvc0.o
 nouveau-y += core/subdev/ibus/nve0.o
 nouveau-y += core/subdev/ibus/gk20a.o
@@ -211,6 +226,7 @@ nouveau-y += core/subdev/vm/nvc0.o
 nouveau-y += core/subdev/volt/base.o
 nouveau-y += core/subdev/volt/gpio.o
 nouveau-y += core/subdev/volt/nv40.o
+nouveau-y += core/subdev/volt/gk20a.o
 
 nouveau-y += core/engine/falcon.o
 nouveau-y += core/engine/xtensa.o
@@ -254,6 +270,7 @@ nouveau-y += core/engine/disp/nvd0.o
 nouveau-y += core/engine/disp/nve0.o
 nouveau-y += core/engine/disp/nvf0.o
 nouveau-y += core/engine/disp/gm107.o
+nouveau-y += core/engine/disp/gm204.o
 nouveau-y += core/engine/disp/dacnv50.o
 nouveau-y += core/engine/disp/dport.o
 nouveau-y += core/engine/disp/hdanva3.o
@@ -266,6 +283,7 @@ nouveau-y += core/engine/disp/piornv50.o
 nouveau-y += core/engine/disp/sornv50.o
 nouveau-y += core/engine/disp/sornv94.o
 nouveau-y += core/engine/disp/sornvd0.o
+nouveau-y += core/engine/disp/sorgm204.o
 nouveau-y += core/engine/disp/vga.o
 nouveau-y += core/engine/fifo/base.o
 nouveau-y += core/engine/fifo/nv04.o
diff --git a/drivers/gpu/drm/nouveau/core/core/handle.c b/drivers/gpu/drm/nouveau/core/core/handle.c
index a490b805d7e3..13f816cb08bd 100644
--- a/drivers/gpu/drm/nouveau/core/core/handle.c
+++ b/drivers/gpu/drm/nouveau/core/core/handle.c
@@ -222,116 +222,3 @@ nouveau_handle_put(struct nouveau_handle *handle)
 	if (handle)
 		nouveau_namedb_put(handle);
 }
-
-int
-nouveau_handle_new(struct nouveau_object *client, u32 _parent, u32 _handle,
-		   u16 _oclass, void *data, u32 size,
-		   struct nouveau_object **pobject)
-{
-	struct nouveau_object *parent = NULL;
-	struct nouveau_object *engctx = NULL;
-	struct nouveau_object *object = NULL;
-	struct nouveau_object *engine;
-	struct nouveau_oclass *oclass;
-	struct nouveau_handle *handle;
-	int ret;
-
-	/* lookup parent object and ensure it *is* a parent */
-	parent = nouveau_handle_ref(client, _parent);
-	if (!parent) {
-		nv_error(client, "parent 0x%08x not found\n", _parent);
-		return -ENOENT;
-	}
-
-	if (!nv_iclass(parent, NV_PARENT_CLASS)) {
-		nv_error(parent, "cannot have children\n");
-		ret = -EINVAL;
-		goto fail_class;
-	}
-
-	/* check that parent supports the requested subclass */
-	ret = nouveau_parent_sclass(parent, _oclass, &engine, &oclass);
-	if (ret) {
-		nv_debug(parent, "illegal class 0x%04x\n", _oclass);
-		goto fail_class;
-	}
-
-	/* make sure engine init has been completed *before* any objects
-	 * it controls are created - the constructors may depend on
-	 * state calculated at init (ie. default context construction)
-	 */
-	if (engine) {
-		ret = nouveau_object_inc(engine);
-		if (ret)
-			goto fail_class;
-	}
-
-	/* if engine requires it, create a context object to insert
-	 * between the parent and its children (eg. PGRAPH context)
-	 */
-	if (engine && nv_engine(engine)->cclass) {
-		ret = nouveau_object_ctor(parent, engine,
-					  nv_engine(engine)->cclass,
-					  data, size, &engctx);
-		if (ret)
-			goto fail_engctx;
-	} else {
-		nouveau_object_ref(parent, &engctx);
-	}
-
-	/* finally, create new object and bind it to its handle */
-	ret = nouveau_object_ctor(engctx, engine, oclass, data, size, &object);
-	*pobject = object;
-	if (ret)
-		goto fail_ctor;
-
-	ret = nouveau_object_inc(object);
-	if (ret)
-		goto fail_init;
-
-	ret = nouveau_handle_create(parent, _parent, _handle, object, &handle);
-	if (ret)
-		goto fail_handle;
-
-	ret = nouveau_handle_init(handle);
-	if (ret)
-		nouveau_handle_destroy(handle);
-
-fail_handle:
-	nouveau_object_dec(object, false);
-fail_init:
-	nouveau_object_ref(NULL, &object);
-fail_ctor:
-	nouveau_object_ref(NULL, &engctx);
-fail_engctx:
-	if (engine)
-		nouveau_object_dec(engine, false);
-fail_class:
-	nouveau_object_ref(NULL, &parent);
-	return ret;
-}
-
-int
-nouveau_handle_del(struct nouveau_object *client, u32 _parent, u32 _handle)
-{
-	struct nouveau_object *parent = NULL;
-	struct nouveau_object *namedb = NULL;
-	struct nouveau_handle *handle = NULL;
-
-	parent = nouveau_handle_ref(client, _parent);
-	if (!parent)
-		return -ENOENT;
-
-	namedb = nv_pclass(parent, NV_NAMEDB_CLASS);
-	if (namedb) {
-		handle = nouveau_namedb_get(nv_namedb(namedb), _handle);
-		if (handle) {
-			nouveau_namedb_put(handle);
-			nouveau_handle_fini(handle, false);
-			nouveau_handle_destroy(handle);
-		}
-	}
-
-	nouveau_object_ref(NULL, &parent);
-	return handle ? 0 : -EINVAL;
-}
diff --git a/drivers/gpu/drm/nouveau/core/engine/device/base.c b/drivers/gpu/drm/nouveau/core/engine/device/base.c
index 0ef5a5713182..137e0b0faeae 100644
--- a/drivers/gpu/drm/nouveau/core/engine/device/base.c
+++ b/drivers/gpu/drm/nouveau/core/engine/device/base.c
@@ -29,6 +29,7 @@
 #include <nvif/unpack.h>
 #include <nvif/class.h>
 
+#include <subdev/bios.h>
 #include <subdev/fb.h>
 #include <subdev/instmem.h>
 
@@ -138,7 +139,7 @@ nouveau_devobj_info(struct nouveau_object *object, void *data, u32 size)
 	}
 
 	args->v0.chipset  = device->chipset;
-	args->v0.revision = device->chipset >= 0x10 ? nv_rd32(device, 0) : 0x00;
+	args->v0.revision = device->chiprev;
 	if (pfb)  args->v0.ram_size = args->v0.ram_user = pfb->ram->size;
 	else      args->v0.ram_size = args->v0.ram_user = 0;
 	if (imem) args->v0.ram_user = args->v0.ram_user - imem->reserved;
@@ -222,6 +223,7 @@ static const u64 disable_map[] = {
 	[NVDEV_SUBDEV_VOLT]	= NV_DEVICE_V0_DISABLE_CORE,
 	[NVDEV_SUBDEV_THERM]	= NV_DEVICE_V0_DISABLE_CORE,
 	[NVDEV_SUBDEV_PWR]	= NV_DEVICE_V0_DISABLE_CORE,
+	[NVDEV_SUBDEV_FUSE]	= NV_DEVICE_V0_DISABLE_CORE,
 	[NVDEV_ENGINE_DMAOBJ]	= NV_DEVICE_V0_DISABLE_CORE,
 	[NVDEV_ENGINE_PERFMON]  = NV_DEVICE_V0_DISABLE_CORE,
 	[NVDEV_ENGINE_FIFO]	= NV_DEVICE_V0_DISABLE_FIFO,
@@ -235,6 +237,7 @@ static const u64 disable_map[] = {
 	[NVDEV_ENGINE_PPP]	= NV_DEVICE_V0_DISABLE_PPP,
 	[NVDEV_ENGINE_COPY0]	= NV_DEVICE_V0_DISABLE_COPY0,
 	[NVDEV_ENGINE_COPY1]	= NV_DEVICE_V0_DISABLE_COPY1,
+	[NVDEV_ENGINE_COPY2]	= NV_DEVICE_V0_DISABLE_COPY1,
 	[NVDEV_ENGINE_VIC]	= NV_DEVICE_V0_DISABLE_VIC,
 	[NVDEV_ENGINE_VENC]	= NV_DEVICE_V0_DISABLE_VENC,
 	[NVDEV_ENGINE_DISP]	= NV_DEVICE_V0_DISABLE_DISP,
@@ -352,12 +355,14 @@ nouveau_devobj_ctor(struct nouveau_object *parent,
 		/* determine chipset and derive architecture from it */
 		if ((boot0 & 0x1f000000) > 0) {
 			device->chipset = (boot0 & 0x1ff00000) >> 20;
+			device->chiprev = (boot0 & 0x000000ff);
 			switch (device->chipset & 0x1f0) {
 			case 0x010: {
 				if (0x461 & (1 << (device->chipset & 0xf)))
 					device->card_type = NV_10;
 				else
 					device->card_type = NV_11;
+				device->chiprev = 0x00;
 				break;
 			}
 			case 0x020: device->card_type = NV_20; break;
@@ -373,7 +378,8 @@ nouveau_devobj_ctor(struct nouveau_object *parent,
 			case 0x0e0:
 			case 0x0f0:
 			case 0x100: device->card_type = NV_E0; break;
-			case 0x110: device->card_type = GM100; break;
+			case 0x110:
+			case 0x120: device->card_type = GM100; break;
 			default:
 				break;
 			}
@@ -427,6 +433,10 @@ nouveau_devobj_ctor(struct nouveau_object *parent,
 		}
 
 		nv_debug(device, "crystal freq: %dKHz\n", device->crystal);
+	} else
+	if ( (args->v0.disable & NV_DEVICE_V0_DISABLE_IDENTIFY)) {
+		device->cname = "NULL";
+		device->oclass[NVDEV_SUBDEV_VBIOS] = &nouveau_bios_oclass;
 	}
 
 	if (!(args->v0.disable & NV_DEVICE_V0_DISABLE_MMIO) &&
diff --git a/drivers/gpu/drm/nouveau/core/engine/device/gm100.c b/drivers/gpu/drm/nouveau/core/engine/device/gm100.c
index 6295668e29a5..4e74a3376de8 100644
--- a/drivers/gpu/drm/nouveau/core/engine/device/gm100.c
+++ b/drivers/gpu/drm/nouveau/core/engine/device/gm100.c
@@ -98,6 +98,49 @@ gm100_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_ENGINE_PPP    ] = &nvc0_ppp_oclass;
 #endif
 		break;
+	case 0x124:
+		device->cname = "GM204";
+		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nve0_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_I2C    ] =  gm204_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] = &gm107_fuse_oclass;
+#if 0
+		/* looks to be some non-trivial changes */
+		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nve0_clock_oclass;
+		/* priv ring says no to 0x10eb14 writes */
+		device->oclass[NVDEV_SUBDEV_THERM  ] = &gm107_therm_oclass;
+#endif
+		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
+		device->oclass[NVDEV_SUBDEV_DEVINIT] =  gm204_devinit_oclass;
+		device->oclass[NVDEV_SUBDEV_MC     ] =  gk20a_mc_oclass;
+		device->oclass[NVDEV_SUBDEV_BUS    ] =  nvc0_bus_oclass;
+		device->oclass[NVDEV_SUBDEV_TIMER  ] = &gk20a_timer_oclass;
+		device->oclass[NVDEV_SUBDEV_FB     ] =  gm107_fb_oclass;
+		device->oclass[NVDEV_SUBDEV_LTC    ] =  gm107_ltc_oclass;
+		device->oclass[NVDEV_SUBDEV_IBUS   ] = &nve0_ibus_oclass;
+		device->oclass[NVDEV_SUBDEV_INSTMEM] =  nv50_instmem_oclass;
+		device->oclass[NVDEV_SUBDEV_VM     ] = &nvc0_vmmgr_oclass;
+		device->oclass[NVDEV_SUBDEV_BAR    ] = &nvc0_bar_oclass;
+		device->oclass[NVDEV_SUBDEV_PWR    ] =  nv108_pwr_oclass;
+#if 0
+		device->oclass[NVDEV_SUBDEV_VOLT   ] = &nv40_volt_oclass;
+#endif
+		device->oclass[NVDEV_ENGINE_DMAOBJ ] =  nvd0_dmaeng_oclass;
+#if 0
+		device->oclass[NVDEV_ENGINE_FIFO   ] =  nv108_fifo_oclass;
+		device->oclass[NVDEV_ENGINE_SW     ] =  nvc0_software_oclass;
+		device->oclass[NVDEV_ENGINE_GR     ] =  gm107_graph_oclass;
+#endif
+		device->oclass[NVDEV_ENGINE_DISP   ] =  gm204_disp_oclass;
+#if 0
+		device->oclass[NVDEV_ENGINE_COPY0  ] = &gm204_copy0_oclass;
+		device->oclass[NVDEV_ENGINE_COPY1  ] = &gm204_copy1_oclass;
+		device->oclass[NVDEV_ENGINE_COPY2  ] = &gm204_copy2_oclass;
+		device->oclass[NVDEV_ENGINE_BSP    ] = &nve0_bsp_oclass;
+		device->oclass[NVDEV_ENGINE_VP     ] = &nve0_vp_oclass;
+		device->oclass[NVDEV_ENGINE_PPP    ] = &nvc0_ppp_oclass;
+#endif
+		break;
 	default:
 		nv_fatal(device, "unknown Maxwell chipset\n");
 		return -EINVAL;
diff --git a/drivers/gpu/drm/nouveau/core/engine/device/nve0.c b/drivers/gpu/drm/nouveau/core/engine/device/nve0.c
index b1b2e484ecfa..674da1f095b2 100644
--- a/drivers/gpu/drm/nouveau/core/engine/device/nve0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/device/nve0.c
@@ -179,6 +179,7 @@ nve0_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_ENGINE_GR     ] =  gk20a_graph_oclass;
 		device->oclass[NVDEV_ENGINE_COPY2  ] = &nve0_copy2_oclass;
 		device->oclass[NVDEV_ENGINE_PERFMON] = &nve0_perfmon_oclass;
+		device->oclass[NVDEV_SUBDEV_VOLT   ] = &gk20a_volt_oclass;
 		break;
 	case 0xf0:
 		device->cname = "GK110";
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/dport.c b/drivers/gpu/drm/nouveau/core/engine/disp/dport.c
index 39890221b91c..16db08dfba6e 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/dport.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/dport.c
@@ -28,7 +28,7 @@
 #include <subdev/bios/init.h>
 #include <subdev/i2c.h>
 
-#include <engine/disp.h>
+#include "nv50.h"
 
 #include <nvif/class.h>
 
@@ -326,7 +326,7 @@ void
 nouveau_dp_train(struct work_struct *w)
 {
 	struct nvkm_output_dp *outp = container_of(w, typeof(*outp), lt.work);
-	struct nouveau_disp *disp = nouveau_disp(outp);
+	struct nv50_disp_priv *priv = (void *)nouveau_disp(outp);
 	const struct dp_rates *cfg = nouveau_dp_rates;
 	struct dp_state _dp = {
 		.outp = outp,
@@ -334,8 +334,11 @@ nouveau_dp_train(struct work_struct *w)
 	u32 datarate = 0;
 	int ret;
 
+	if (!outp->base.info.location && priv->sor.magic)
+		priv->sor.magic(&outp->base);
+
 	/* bring capabilities within encoder limits */
-	if (nv_mclass(disp) < GF110_DISP)
+	if (nv_mclass(priv) < GF110_DISP)
 		outp->dpcd[2] &= ~DPCD_RC02_TPS3_SUPPORTED;
 	if ((outp->dpcd[2] & 0x1f) > outp->base.info.dpconf.link_nr) {
 		outp->dpcd[2] &= ~DPCD_RC02_MAX_LANE_COUNT;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/gm107.c b/drivers/gpu/drm/nouveau/core/engine/disp/gm107.c
index b3df3fe2dc09..e2ad0543fb31 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/gm107.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/gm107.c
@@ -35,8 +35,8 @@
 
 static struct nouveau_oclass
 gm107_disp_sclass[] = {
-	{ GM107_DISP_CORE_CHANNEL_DMA, &nvd0_disp_mast_ofuncs.base },
-	{ GK110_DISP_BASE_CHANNEL_DMA, &nvd0_disp_sync_ofuncs.base },
+	{ GM107_DISP_CORE_CHANNEL_DMA, &nvd0_disp_core_ofuncs.base },
+	{ GK110_DISP_BASE_CHANNEL_DMA, &nvd0_disp_base_ofuncs.base },
 	{ GK104_DISP_OVERLAY_CONTROL_DMA, &nvd0_disp_ovly_ofuncs.base },
 	{ GK104_DISP_OVERLAY, &nvd0_disp_oimm_ofuncs.base },
 	{ GK104_DISP_CURSOR, &nvd0_disp_curs_ofuncs.base },
@@ -44,8 +44,8 @@ gm107_disp_sclass[] = {
 };
 
 static struct nouveau_oclass
-gm107_disp_base_oclass[] = {
-	{ GM107_DISP, &nvd0_disp_base_ofuncs },
+gm107_disp_main_oclass[] = {
+	{ GM107_DISP, &nvd0_disp_main_ofuncs },
 	{}
 };
 
@@ -72,7 +72,7 @@ gm107_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	nv_engine(priv)->sclass = gm107_disp_base_oclass;
+	nv_engine(priv)->sclass = gm107_disp_main_oclass;
 	nv_engine(priv)->cclass = &nv50_disp_cclass;
 	nv_subdev(priv)->intr = nvd0_disp_intr;
 	INIT_WORK(&priv->supervisor, nvd0_disp_intr_supervisor);
@@ -99,9 +99,9 @@ gm107_disp_oclass = &(struct nv50_disp_impl) {
 	},
 	.base.vblank = &nvd0_disp_vblank_func,
 	.base.outp =  nvd0_disp_outp_sclass,
-	.mthd.core = &nve0_disp_mast_mthd_chan,
-	.mthd.base = &nvd0_disp_sync_mthd_chan,
+	.mthd.core = &nve0_disp_core_mthd_chan,
+	.mthd.base = &nvd0_disp_base_mthd_chan,
 	.mthd.ovly = &nve0_disp_ovly_mthd_chan,
 	.mthd.prev = -0x020000,
-	.head.scanoutpos = nvd0_disp_base_scanoutpos,
+	.head.scanoutpos = nvd0_disp_main_scanoutpos,
 }.base.base;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/gm204.c b/drivers/gpu/drm/nouveau/core/engine/disp/gm204.c
new file mode 100644
index 000000000000..672ded79b2a9
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/gm204.c
@@ -0,0 +1,114 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include <engine/software.h>
+#include <engine/disp.h>
+
+#include <nvif/class.h>
+
+#include "nv50.h"
+
+/*******************************************************************************
+ * Base display object
+ ******************************************************************************/
+
+static struct nouveau_oclass
+gm204_disp_sclass[] = {
+	{ GM204_DISP_CORE_CHANNEL_DMA, &nvd0_disp_core_ofuncs.base },
+	{ GK110_DISP_BASE_CHANNEL_DMA, &nvd0_disp_base_ofuncs.base },
+	{ GK104_DISP_OVERLAY_CONTROL_DMA, &nvd0_disp_ovly_ofuncs.base },
+	{ GK104_DISP_OVERLAY, &nvd0_disp_oimm_ofuncs.base },
+	{ GK104_DISP_CURSOR, &nvd0_disp_curs_ofuncs.base },
+	{}
+};
+
+static struct nouveau_oclass
+gm204_disp_main_oclass[] = {
+	{ GM204_DISP, &nvd0_disp_main_ofuncs },
+	{}
+};
+
+/*******************************************************************************
+ * Display engine implementation
+ ******************************************************************************/
+
+static int
+gm204_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
+	       struct nouveau_oclass *oclass, void *data, u32 size,
+	       struct nouveau_object **pobject)
+{
+	struct nv50_disp_priv *priv;
+	int heads = nv_rd32(parent, 0x022448);
+	int ret;
+
+	ret = nouveau_disp_create(parent, engine, oclass, heads,
+				  "PDISP", "display", &priv);
+	*pobject = nv_object(priv);
+	if (ret)
+		return ret;
+
+	ret = nvkm_event_init(&nvd0_disp_chan_uevent, 1, 17, &priv->uevent);
+	if (ret)
+		return ret;
+
+	nv_engine(priv)->sclass = gm204_disp_main_oclass;
+	nv_engine(priv)->cclass = &nv50_disp_cclass;
+	nv_subdev(priv)->intr = nvd0_disp_intr;
+	INIT_WORK(&priv->supervisor, nvd0_disp_intr_supervisor);
+	priv->sclass = gm204_disp_sclass;
+	priv->head.nr = heads;
+	priv->dac.nr = 3;
+	priv->sor.nr = 4;
+	priv->dac.power = nv50_dac_power;
+	priv->dac.sense = nv50_dac_sense;
+	priv->sor.power = nv50_sor_power;
+	priv->sor.hda_eld = nvd0_hda_eld;
+	priv->sor.hdmi = nvd0_hdmi_ctrl;
+	priv->sor.magic = gm204_sor_magic;
+	return 0;
+}
+
+struct nouveau_oclass *
+gm204_disp_outp_sclass[] = {
+	&gm204_sor_dp_impl.base.base,
+	NULL
+};
+
+struct nouveau_oclass *
+gm204_disp_oclass = &(struct nv50_disp_impl) {
+	.base.base.handle = NV_ENGINE(DISP, 0x07),
+	.base.base.ofuncs = &(struct nouveau_ofuncs) {
+		.ctor = gm204_disp_ctor,
+		.dtor = _nouveau_disp_dtor,
+		.init = _nouveau_disp_init,
+		.fini = _nouveau_disp_fini,
+	},
+	.base.vblank = &nvd0_disp_vblank_func,
+	.base.outp =  gm204_disp_outp_sclass,
+	.mthd.core = &nve0_disp_core_mthd_chan,
+	.mthd.base = &nvd0_disp_base_mthd_chan,
+	.mthd.ovly = &nve0_disp_ovly_mthd_chan,
+	.mthd.prev = -0x020000,
+	.head.scanoutpos = nvd0_disp_main_scanoutpos,
+}.base.base;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nv50.c b/drivers/gpu/drm/nouveau/core/engine/disp/nv50.c
index 2df3a937037d..44a8290aaea5 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nv50.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nv50.c
@@ -88,12 +88,14 @@ nv50_disp_chan_uevent_fini(struct nvkm_event *event, int type, int index)
 {
 	struct nv50_disp_priv *priv = container_of(event, typeof(*priv), uevent);
 	nv_mask(priv, 0x610028, 0x00000001 << index, 0x00000000 << index);
+	nv_wr32(priv, 0x610020, 0x00000001 << index);
 }
 
 static void
 nv50_disp_chan_uevent_init(struct nvkm_event *event, int types, int index)
 {
 	struct nv50_disp_priv *priv = container_of(event, typeof(*priv), uevent);
+	nv_wr32(priv, 0x610020, 0x00000001 << index);
 	nv_mask(priv, 0x610028, 0x00000001 << index, 0x00000001 << index);
 }
 
@@ -374,7 +376,7 @@ nv50_disp_mthd_chan(struct nv50_disp_priv *priv, int debug, int head,
 }
 
 const struct nv50_disp_mthd_list
-nv50_disp_mast_mthd_base = {
+nv50_disp_core_mthd_base = {
 	.mthd = 0x0000,
 	.addr = 0x000000,
 	.data = {
@@ -387,7 +389,7 @@ nv50_disp_mast_mthd_base = {
 };
 
 static const struct nv50_disp_mthd_list
-nv50_disp_mast_mthd_dac = {
+nv50_disp_core_mthd_dac = {
 	.mthd = 0x0080,
 	.addr = 0x000008,
 	.data = {
@@ -399,7 +401,7 @@ nv50_disp_mast_mthd_dac = {
 };
 
 const struct nv50_disp_mthd_list
-nv50_disp_mast_mthd_sor = {
+nv50_disp_core_mthd_sor = {
 	.mthd = 0x0040,
 	.addr = 0x000008,
 	.data = {
@@ -409,7 +411,7 @@ nv50_disp_mast_mthd_sor = {
 };
 
 const struct nv50_disp_mthd_list
-nv50_disp_mast_mthd_pior = {
+nv50_disp_core_mthd_pior = {
 	.mthd = 0x0040,
 	.addr = 0x000008,
 	.data = {
@@ -419,7 +421,7 @@ nv50_disp_mast_mthd_pior = {
 };
 
 static const struct nv50_disp_mthd_list
-nv50_disp_mast_mthd_head = {
+nv50_disp_core_mthd_head = {
 	.mthd = 0x0400,
 	.addr = 0x000540,
 	.data = {
@@ -466,21 +468,21 @@ nv50_disp_mast_mthd_head = {
 };
 
 static const struct nv50_disp_mthd_chan
-nv50_disp_mast_mthd_chan = {
+nv50_disp_core_mthd_chan = {
 	.name = "Core",
 	.addr = 0x000000,
 	.data = {
-		{ "Global", 1, &nv50_disp_mast_mthd_base },
-		{    "DAC", 3, &nv50_disp_mast_mthd_dac  },
-		{    "SOR", 2, &nv50_disp_mast_mthd_sor  },
-		{   "PIOR", 3, &nv50_disp_mast_mthd_pior },
-		{   "HEAD", 2, &nv50_disp_mast_mthd_head },
+		{ "Global", 1, &nv50_disp_core_mthd_base },
+		{    "DAC", 3, &nv50_disp_core_mthd_dac  },
+		{    "SOR", 2, &nv50_disp_core_mthd_sor  },
+		{   "PIOR", 3, &nv50_disp_core_mthd_pior },
+		{   "HEAD", 2, &nv50_disp_core_mthd_head },
 		{}
 	}
 };
 
 int
-nv50_disp_mast_ctor(struct nouveau_object *parent,
+nv50_disp_core_ctor(struct nouveau_object *parent,
 		    struct nouveau_object *engine,
 		    struct nouveau_oclass *oclass, void *data, u32 size,
 		    struct nouveau_object **pobject)
@@ -509,7 +511,7 @@ nv50_disp_mast_ctor(struct nouveau_object *parent,
 }
 
 static int
-nv50_disp_mast_init(struct nouveau_object *object)
+nv50_disp_core_init(struct nouveau_object *object)
 {
 	struct nv50_disp_priv *priv = (void *)object->engine;
 	struct nv50_disp_dmac *mast = (void *)object;
@@ -546,7 +548,7 @@ nv50_disp_mast_init(struct nouveau_object *object)
 }
 
 static int
-nv50_disp_mast_fini(struct nouveau_object *object, bool suspend)
+nv50_disp_core_fini(struct nouveau_object *object, bool suspend)
 {
 	struct nv50_disp_priv *priv = (void *)object->engine;
 	struct nv50_disp_dmac *mast = (void *)object;
@@ -567,11 +569,11 @@ nv50_disp_mast_fini(struct nouveau_object *object, bool suspend)
 }
 
 struct nv50_disp_chan_impl
-nv50_disp_mast_ofuncs = {
-	.base.ctor = nv50_disp_mast_ctor,
+nv50_disp_core_ofuncs = {
+	.base.ctor = nv50_disp_core_ctor,
 	.base.dtor = nv50_disp_dmac_dtor,
-	.base.init = nv50_disp_mast_init,
-	.base.fini = nv50_disp_mast_fini,
+	.base.init = nv50_disp_core_init,
+	.base.fini = nv50_disp_core_fini,
 	.base.map  = nv50_disp_chan_map,
 	.base.ntfy = nv50_disp_chan_ntfy,
 	.base.rd32 = nv50_disp_chan_rd32,
@@ -586,7 +588,7 @@ nv50_disp_mast_ofuncs = {
  ******************************************************************************/
 
 static const struct nv50_disp_mthd_list
-nv50_disp_sync_mthd_base = {
+nv50_disp_base_mthd_base = {
 	.mthd = 0x0000,
 	.addr = 0x000000,
 	.data = {
@@ -611,7 +613,7 @@ nv50_disp_sync_mthd_base = {
 };
 
 const struct nv50_disp_mthd_list
-nv50_disp_sync_mthd_image = {
+nv50_disp_base_mthd_image = {
 	.mthd = 0x0400,
 	.addr = 0x000000,
 	.data = {
@@ -625,18 +627,18 @@ nv50_disp_sync_mthd_image = {
 };
 
 static const struct nv50_disp_mthd_chan
-nv50_disp_sync_mthd_chan = {
+nv50_disp_base_mthd_chan = {
 	.name = "Base",
 	.addr = 0x000540,
 	.data = {
-		{ "Global", 1, &nv50_disp_sync_mthd_base },
-		{  "Image", 2, &nv50_disp_sync_mthd_image },
+		{ "Global", 1, &nv50_disp_base_mthd_base },
+		{  "Image", 2, &nv50_disp_base_mthd_image },
 		{}
 	}
 };
 
 int
-nv50_disp_sync_ctor(struct nouveau_object *parent,
+nv50_disp_base_ctor(struct nouveau_object *parent,
 		    struct nouveau_object *engine,
 		    struct nouveau_oclass *oclass, void *data, u32 size,
 		    struct nouveau_object **pobject)
@@ -669,8 +671,8 @@ nv50_disp_sync_ctor(struct nouveau_object *parent,
 }
 
 struct nv50_disp_chan_impl
-nv50_disp_sync_ofuncs = {
-	.base.ctor = nv50_disp_sync_ctor,
+nv50_disp_base_ofuncs = {
+	.base.ctor = nv50_disp_base_ctor,
 	.base.dtor = nv50_disp_dmac_dtor,
 	.base.init = nv50_disp_dmac_init,
 	.base.fini = nv50_disp_dmac_fini,
@@ -942,7 +944,7 @@ nv50_disp_curs_ofuncs = {
  ******************************************************************************/
 
 int
-nv50_disp_base_scanoutpos(NV50_DISP_MTHD_V0)
+nv50_disp_main_scanoutpos(NV50_DISP_MTHD_V0)
 {
 	const u32 blanke = nv_rd32(priv, 0x610aec + (head * 0x540));
 	const u32 blanks = nv_rd32(priv, 0x610af4 + (head * 0x540));
@@ -974,7 +976,7 @@ nv50_disp_base_scanoutpos(NV50_DISP_MTHD_V0)
 }
 
 int
-nv50_disp_base_mthd(struct nouveau_object *object, u32 mthd,
+nv50_disp_main_mthd(struct nouveau_object *object, u32 mthd,
 		    void *data, u32 size)
 {
 	const struct nv50_disp_impl *impl = (void *)nv_oclass(object->engine);
@@ -1098,7 +1100,7 @@ nv50_disp_base_mthd(struct nouveau_object *object, u32 mthd,
 }
 
 int
-nv50_disp_base_ctor(struct nouveau_object *parent,
+nv50_disp_main_ctor(struct nouveau_object *parent,
 		    struct nouveau_object *engine,
 		    struct nouveau_oclass *oclass, void *data, u32 size,
 		    struct nouveau_object **pobject)
@@ -1118,7 +1120,7 @@ nv50_disp_base_ctor(struct nouveau_object *parent,
 }
 
 void
-nv50_disp_base_dtor(struct nouveau_object *object)
+nv50_disp_main_dtor(struct nouveau_object *object)
 {
 	struct nv50_disp_base *base = (void *)object;
 	nouveau_ramht_ref(NULL, &base->ramht);
@@ -1126,7 +1128,7 @@ nv50_disp_base_dtor(struct nouveau_object *object)
 }
 
 static int
-nv50_disp_base_init(struct nouveau_object *object)
+nv50_disp_main_init(struct nouveau_object *object)
 {
 	struct nv50_disp_priv *priv = (void *)object->engine;
 	struct nv50_disp_base *base = (void *)object;
@@ -1194,7 +1196,7 @@ nv50_disp_base_init(struct nouveau_object *object)
 }
 
 static int
-nv50_disp_base_fini(struct nouveau_object *object, bool suspend)
+nv50_disp_main_fini(struct nouveau_object *object, bool suspend)
 {
 	struct nv50_disp_priv *priv = (void *)object->engine;
 	struct nv50_disp_base *base = (void *)object;
@@ -1207,25 +1209,25 @@ nv50_disp_base_fini(struct nouveau_object *object, bool suspend)
 }
 
 struct nouveau_ofuncs
-nv50_disp_base_ofuncs = {
-	.ctor = nv50_disp_base_ctor,
-	.dtor = nv50_disp_base_dtor,
-	.init = nv50_disp_base_init,
-	.fini = nv50_disp_base_fini,
-	.mthd = nv50_disp_base_mthd,
+nv50_disp_main_ofuncs = {
+	.ctor = nv50_disp_main_ctor,
+	.dtor = nv50_disp_main_dtor,
+	.init = nv50_disp_main_init,
+	.fini = nv50_disp_main_fini,
+	.mthd = nv50_disp_main_mthd,
 	.ntfy = nouveau_disp_ntfy,
 };
 
 static struct nouveau_oclass
-nv50_disp_base_oclass[] = {
-	{ NV50_DISP, &nv50_disp_base_ofuncs },
+nv50_disp_main_oclass[] = {
+	{ NV50_DISP, &nv50_disp_main_ofuncs },
 	{}
 };
 
 static struct nouveau_oclass
 nv50_disp_sclass[] = {
-	{ NV50_DISP_CORE_CHANNEL_DMA, &nv50_disp_mast_ofuncs.base },
-	{ NV50_DISP_BASE_CHANNEL_DMA, &nv50_disp_sync_ofuncs.base },
+	{ NV50_DISP_CORE_CHANNEL_DMA, &nv50_disp_core_ofuncs.base },
+	{ NV50_DISP_BASE_CHANNEL_DMA, &nv50_disp_base_ofuncs.base },
 	{ NV50_DISP_OVERLAY_CHANNEL_DMA, &nv50_disp_ovly_ofuncs.base },
 	{ NV50_DISP_OVERLAY, &nv50_disp_oimm_ofuncs.base },
 	{ NV50_DISP_CURSOR, &nv50_disp_curs_ofuncs.base },
@@ -1974,7 +1976,7 @@ nv50_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	nv_engine(priv)->sclass = nv50_disp_base_oclass;
+	nv_engine(priv)->sclass = nv50_disp_main_oclass;
 	nv_engine(priv)->cclass = &nv50_disp_cclass;
 	nv_subdev(priv)->intr = nv50_disp_intr;
 	INIT_WORK(&priv->supervisor, nv50_disp_intr_supervisor);
@@ -2007,9 +2009,9 @@ nv50_disp_oclass = &(struct nv50_disp_impl) {
 	},
 	.base.vblank = &nv50_disp_vblank_func,
 	.base.outp =  nv50_disp_outp_sclass,
-	.mthd.core = &nv50_disp_mast_mthd_chan,
-	.mthd.base = &nv50_disp_sync_mthd_chan,
+	.mthd.core = &nv50_disp_core_mthd_chan,
+	.mthd.base = &nv50_disp_base_mthd_chan,
 	.mthd.ovly = &nv50_disp_ovly_mthd_chan,
 	.mthd.prev = 0x000004,
-	.head.scanoutpos = nv50_disp_base_scanoutpos,
+	.head.scanoutpos = nv50_disp_main_scanoutpos,
 }.base.base;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nv50.h b/drivers/gpu/drm/nouveau/core/engine/disp/nv50.h
index 5279feefec06..7f08078ee925 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nv50.h
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nv50.h
@@ -42,6 +42,7 @@ struct nv50_disp_priv {
 		int (*hda_eld)(NV50_DISP_MTHD_V1);
 		int (*hdmi)(NV50_DISP_MTHD_V1);
 		u32 lvdsconf;
+		void (*magic)(struct nvkm_output *);
 	} sor;
 	struct {
 		int nr;
@@ -63,10 +64,10 @@ struct nv50_disp_impl {
 	} head;
 };
 
-int nv50_disp_base_scanoutpos(NV50_DISP_MTHD_V0);
-int nv50_disp_base_mthd(struct nouveau_object *, u32, void *, u32);
+int nv50_disp_main_scanoutpos(NV50_DISP_MTHD_V0);
+int nv50_disp_main_mthd(struct nouveau_object *, u32, void *, u32);
 
-int nvd0_disp_base_scanoutpos(NV50_DISP_MTHD_V0);
+int nvd0_disp_main_scanoutpos(NV50_DISP_MTHD_V0);
 
 int nv50_dac_power(NV50_DISP_MTHD_V1);
 int nv50_dac_sense(NV50_DISP_MTHD_V1);
@@ -169,18 +170,18 @@ struct nv50_disp_mthd_chan {
 	} data[];
 };
 
-extern struct nv50_disp_chan_impl nv50_disp_mast_ofuncs;
-int nv50_disp_mast_ctor(struct nouveau_object *, struct nouveau_object *,
+extern struct nv50_disp_chan_impl nv50_disp_core_ofuncs;
+int nv50_disp_core_ctor(struct nouveau_object *, struct nouveau_object *,
 			struct nouveau_oclass *, void *, u32,
 			struct nouveau_object **);
-extern const struct nv50_disp_mthd_list nv50_disp_mast_mthd_base;
-extern const struct nv50_disp_mthd_list nv50_disp_mast_mthd_sor;
-extern const struct nv50_disp_mthd_list nv50_disp_mast_mthd_pior;
-extern struct nv50_disp_chan_impl nv50_disp_sync_ofuncs;
-int nv50_disp_sync_ctor(struct nouveau_object *, struct nouveau_object *,
+extern const struct nv50_disp_mthd_list nv50_disp_core_mthd_base;
+extern const struct nv50_disp_mthd_list nv50_disp_core_mthd_sor;
+extern const struct nv50_disp_mthd_list nv50_disp_core_mthd_pior;
+extern struct nv50_disp_chan_impl nv50_disp_base_ofuncs;
+int nv50_disp_base_ctor(struct nouveau_object *, struct nouveau_object *,
 			struct nouveau_oclass *, void *, u32,
 			struct nouveau_object **);
-extern const struct nv50_disp_mthd_list nv50_disp_sync_mthd_image;
+extern const struct nv50_disp_mthd_list nv50_disp_base_mthd_image;
 extern struct nv50_disp_chan_impl nv50_disp_ovly_ofuncs;
 int nv50_disp_ovly_ctor(struct nouveau_object *, struct nouveau_object *,
 			struct nouveau_oclass *, void *, u32,
@@ -194,12 +195,12 @@ extern struct nv50_disp_chan_impl nv50_disp_curs_ofuncs;
 int nv50_disp_curs_ctor(struct nouveau_object *, struct nouveau_object *,
 			struct nouveau_oclass *, void *, u32,
 			struct nouveau_object **);
-extern struct nouveau_ofuncs nv50_disp_base_ofuncs;
-int  nv50_disp_base_ctor(struct nouveau_object *, struct nouveau_object *,
+extern struct nouveau_ofuncs nv50_disp_main_ofuncs;
+int  nv50_disp_main_ctor(struct nouveau_object *, struct nouveau_object *,
 			 struct nouveau_oclass *, void *, u32,
 			 struct nouveau_object **);
-void nv50_disp_base_dtor(struct nouveau_object *);
-extern struct nouveau_omthds nv50_disp_base_omthds[];
+void nv50_disp_main_dtor(struct nouveau_object *);
+extern struct nouveau_omthds nv50_disp_main_omthds[];
 extern struct nouveau_oclass nv50_disp_cclass;
 void nv50_disp_mthd_chan(struct nv50_disp_priv *, int debug, int head,
 			 const struct nv50_disp_mthd_chan *);
@@ -207,31 +208,31 @@ void nv50_disp_intr_supervisor(struct work_struct *);
 void nv50_disp_intr(struct nouveau_subdev *);
 extern const struct nvkm_event_func nv50_disp_vblank_func;
 
-extern const struct nv50_disp_mthd_chan nv84_disp_mast_mthd_chan;
-extern const struct nv50_disp_mthd_list nv84_disp_mast_mthd_dac;
-extern const struct nv50_disp_mthd_list nv84_disp_mast_mthd_head;
-extern const struct nv50_disp_mthd_chan nv84_disp_sync_mthd_chan;
+extern const struct nv50_disp_mthd_chan nv84_disp_core_mthd_chan;
+extern const struct nv50_disp_mthd_list nv84_disp_core_mthd_dac;
+extern const struct nv50_disp_mthd_list nv84_disp_core_mthd_head;
+extern const struct nv50_disp_mthd_chan nv84_disp_base_mthd_chan;
 extern const struct nv50_disp_mthd_chan nv84_disp_ovly_mthd_chan;
 
-extern const struct nv50_disp_mthd_chan nv94_disp_mast_mthd_chan;
+extern const struct nv50_disp_mthd_chan nv94_disp_core_mthd_chan;
 
-extern struct nv50_disp_chan_impl nvd0_disp_mast_ofuncs;
-extern const struct nv50_disp_mthd_list nvd0_disp_mast_mthd_base;
-extern const struct nv50_disp_mthd_list nvd0_disp_mast_mthd_dac;
-extern const struct nv50_disp_mthd_list nvd0_disp_mast_mthd_sor;
-extern const struct nv50_disp_mthd_list nvd0_disp_mast_mthd_pior;
-extern struct nv50_disp_chan_impl nvd0_disp_sync_ofuncs;
+extern struct nv50_disp_chan_impl nvd0_disp_core_ofuncs;
+extern const struct nv50_disp_mthd_list nvd0_disp_core_mthd_base;
+extern const struct nv50_disp_mthd_list nvd0_disp_core_mthd_dac;
+extern const struct nv50_disp_mthd_list nvd0_disp_core_mthd_sor;
+extern const struct nv50_disp_mthd_list nvd0_disp_core_mthd_pior;
+extern struct nv50_disp_chan_impl nvd0_disp_base_ofuncs;
 extern struct nv50_disp_chan_impl nvd0_disp_ovly_ofuncs;
-extern const struct nv50_disp_mthd_chan nvd0_disp_sync_mthd_chan;
+extern const struct nv50_disp_mthd_chan nvd0_disp_base_mthd_chan;
 extern struct nv50_disp_chan_impl nvd0_disp_oimm_ofuncs;
 extern struct nv50_disp_chan_impl nvd0_disp_curs_ofuncs;
-extern struct nouveau_ofuncs nvd0_disp_base_ofuncs;
+extern struct nouveau_ofuncs nvd0_disp_main_ofuncs;
 extern struct nouveau_oclass nvd0_disp_cclass;
 void nvd0_disp_intr_supervisor(struct work_struct *);
 void nvd0_disp_intr(struct nouveau_subdev *);
 extern const struct nvkm_event_func nvd0_disp_vblank_func;
 
-extern const struct nv50_disp_mthd_chan nve0_disp_mast_mthd_chan;
+extern const struct nv50_disp_mthd_chan nve0_disp_core_mthd_chan;
 extern const struct nv50_disp_mthd_chan nve0_disp_ovly_mthd_chan;
 
 extern struct nvkm_output_dp_impl nv50_pior_dp_impl;
@@ -242,6 +243,10 @@ int nv94_sor_dp_lnk_pwr(struct nvkm_output_dp *, int);
 extern struct nouveau_oclass *nv94_disp_outp_sclass[];
 
 extern struct nvkm_output_dp_impl nvd0_sor_dp_impl;
+int nvd0_sor_dp_lnk_ctl(struct nvkm_output_dp *, int, int, bool);
 extern struct nouveau_oclass *nvd0_disp_outp_sclass[];
 
+void gm204_sor_magic(struct nvkm_output *outp);
+extern struct nvkm_output_dp_impl gm204_sor_dp_impl;
+
 #endif
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nv84.c b/drivers/gpu/drm/nouveau/core/engine/disp/nv84.c
index d36284715b2a..13eff5e4ee51 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nv84.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nv84.c
@@ -34,7 +34,7 @@
  ******************************************************************************/
 
 const struct nv50_disp_mthd_list
-nv84_disp_mast_mthd_dac = {
+nv84_disp_core_mthd_dac = {
 	.mthd = 0x0080,
 	.addr = 0x000008,
 	.data = {
@@ -46,7 +46,7 @@ nv84_disp_mast_mthd_dac = {
 };
 
 const struct nv50_disp_mthd_list
-nv84_disp_mast_mthd_head = {
+nv84_disp_core_mthd_head = {
 	.mthd = 0x0400,
 	.addr = 0x000540,
 	.data = {
@@ -98,15 +98,15 @@ nv84_disp_mast_mthd_head = {
 };
 
 const struct nv50_disp_mthd_chan
-nv84_disp_mast_mthd_chan = {
+nv84_disp_core_mthd_chan = {
 	.name = "Core",
 	.addr = 0x000000,
 	.data = {
-		{ "Global", 1, &nv50_disp_mast_mthd_base },
-		{    "DAC", 3, &nv84_disp_mast_mthd_dac  },
-		{    "SOR", 2, &nv50_disp_mast_mthd_sor  },
-		{   "PIOR", 3, &nv50_disp_mast_mthd_pior },
-		{   "HEAD", 2, &nv84_disp_mast_mthd_head },
+		{ "Global", 1, &nv50_disp_core_mthd_base },
+		{    "DAC", 3, &nv84_disp_core_mthd_dac  },
+		{    "SOR", 2, &nv50_disp_core_mthd_sor  },
+		{   "PIOR", 3, &nv50_disp_core_mthd_pior },
+		{   "HEAD", 2, &nv84_disp_core_mthd_head },
 		{}
 	}
 };
@@ -116,7 +116,7 @@ nv84_disp_mast_mthd_chan = {
  ******************************************************************************/
 
 static const struct nv50_disp_mthd_list
-nv84_disp_sync_mthd_base = {
+nv84_disp_base_mthd_base = {
 	.mthd = 0x0000,
 	.addr = 0x000000,
 	.data = {
@@ -146,12 +146,12 @@ nv84_disp_sync_mthd_base = {
 };
 
 const struct nv50_disp_mthd_chan
-nv84_disp_sync_mthd_chan = {
+nv84_disp_base_mthd_chan = {
 	.name = "Base",
 	.addr = 0x000540,
 	.data = {
-		{ "Global", 1, &nv84_disp_sync_mthd_base },
-		{  "Image", 2, &nv50_disp_sync_mthd_image },
+		{ "Global", 1, &nv84_disp_base_mthd_base },
+		{  "Image", 2, &nv50_disp_base_mthd_image },
 		{}
 	}
 };
@@ -204,8 +204,8 @@ nv84_disp_ovly_mthd_chan = {
 
 static struct nouveau_oclass
 nv84_disp_sclass[] = {
-	{ G82_DISP_CORE_CHANNEL_DMA, &nv50_disp_mast_ofuncs.base },
-	{ G82_DISP_BASE_CHANNEL_DMA, &nv50_disp_sync_ofuncs.base },
+	{ G82_DISP_CORE_CHANNEL_DMA, &nv50_disp_core_ofuncs.base },
+	{ G82_DISP_BASE_CHANNEL_DMA, &nv50_disp_base_ofuncs.base },
 	{ G82_DISP_OVERLAY_CHANNEL_DMA, &nv50_disp_ovly_ofuncs.base },
 	{ G82_DISP_OVERLAY, &nv50_disp_oimm_ofuncs.base },
 	{ G82_DISP_CURSOR, &nv50_disp_curs_ofuncs.base },
@@ -213,8 +213,8 @@ nv84_disp_sclass[] = {
 };
 
 static struct nouveau_oclass
-nv84_disp_base_oclass[] = {
-	{ G82_DISP, &nv50_disp_base_ofuncs },
+nv84_disp_main_oclass[] = {
+	{ G82_DISP, &nv50_disp_main_ofuncs },
 	{}
 };
 
@@ -240,7 +240,7 @@ nv84_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	nv_engine(priv)->sclass = nv84_disp_base_oclass;
+	nv_engine(priv)->sclass = nv84_disp_main_oclass;
 	nv_engine(priv)->cclass = &nv50_disp_cclass;
 	nv_subdev(priv)->intr = nv50_disp_intr;
 	INIT_WORK(&priv->supervisor, nv50_disp_intr_supervisor);
@@ -268,9 +268,9 @@ nv84_disp_oclass = &(struct nv50_disp_impl) {
 	},
 	.base.vblank = &nv50_disp_vblank_func,
 	.base.outp =  nv50_disp_outp_sclass,
-	.mthd.core = &nv84_disp_mast_mthd_chan,
-	.mthd.base = &nv84_disp_sync_mthd_chan,
+	.mthd.core = &nv84_disp_core_mthd_chan,
+	.mthd.base = &nv84_disp_base_mthd_chan,
 	.mthd.ovly = &nv84_disp_ovly_mthd_chan,
 	.mthd.prev = 0x000004,
-	.head.scanoutpos = nv50_disp_base_scanoutpos,
+	.head.scanoutpos = nv50_disp_main_scanoutpos,
 }.base.base;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nv94.c b/drivers/gpu/drm/nouveau/core/engine/disp/nv94.c
index a117064002b1..2bb7ac5cd0e6 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nv94.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nv94.c
@@ -34,7 +34,7 @@
  ******************************************************************************/
 
 const struct nv50_disp_mthd_list
-nv94_disp_mast_mthd_sor = {
+nv94_disp_core_mthd_sor = {
 	.mthd = 0x0040,
 	.addr = 0x000008,
 	.data = {
@@ -44,15 +44,15 @@ nv94_disp_mast_mthd_sor = {
 };
 
 const struct nv50_disp_mthd_chan
-nv94_disp_mast_mthd_chan = {
+nv94_disp_core_mthd_chan = {
 	.name = "Core",
 	.addr = 0x000000,
 	.data = {
-		{ "Global", 1, &nv50_disp_mast_mthd_base },
-		{    "DAC", 3, &nv84_disp_mast_mthd_dac  },
-		{    "SOR", 4, &nv94_disp_mast_mthd_sor  },
-		{   "PIOR", 3, &nv50_disp_mast_mthd_pior },
-		{   "HEAD", 2, &nv84_disp_mast_mthd_head },
+		{ "Global", 1, &nv50_disp_core_mthd_base },
+		{    "DAC", 3, &nv84_disp_core_mthd_dac  },
+		{    "SOR", 4, &nv94_disp_core_mthd_sor  },
+		{   "PIOR", 3, &nv50_disp_core_mthd_pior },
+		{   "HEAD", 2, &nv84_disp_core_mthd_head },
 		{}
 	}
 };
@@ -63,8 +63,8 @@ nv94_disp_mast_mthd_chan = {
 
 static struct nouveau_oclass
 nv94_disp_sclass[] = {
-	{ GT206_DISP_CORE_CHANNEL_DMA, &nv50_disp_mast_ofuncs.base },
-	{ GT200_DISP_BASE_CHANNEL_DMA, &nv50_disp_sync_ofuncs.base },
+	{ GT206_DISP_CORE_CHANNEL_DMA, &nv50_disp_core_ofuncs.base },
+	{ GT200_DISP_BASE_CHANNEL_DMA, &nv50_disp_base_ofuncs.base },
 	{ GT200_DISP_OVERLAY_CHANNEL_DMA, &nv50_disp_ovly_ofuncs.base },
 	{ G82_DISP_OVERLAY, &nv50_disp_oimm_ofuncs.base },
 	{ G82_DISP_CURSOR, &nv50_disp_curs_ofuncs.base },
@@ -72,8 +72,8 @@ nv94_disp_sclass[] = {
 };
 
 static struct nouveau_oclass
-nv94_disp_base_oclass[] = {
-	{ GT206_DISP, &nv50_disp_base_ofuncs },
+nv94_disp_main_oclass[] = {
+	{ GT206_DISP, &nv50_disp_main_ofuncs },
 	{}
 };
 
@@ -99,7 +99,7 @@ nv94_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	nv_engine(priv)->sclass = nv94_disp_base_oclass;
+	nv_engine(priv)->sclass = nv94_disp_main_oclass;
 	nv_engine(priv)->cclass = &nv50_disp_cclass;
 	nv_subdev(priv)->intr = nv50_disp_intr;
 	INIT_WORK(&priv->supervisor, nv50_disp_intr_supervisor);
@@ -134,9 +134,9 @@ nv94_disp_oclass = &(struct nv50_disp_impl) {
 	},
 	.base.vblank = &nv50_disp_vblank_func,
 	.base.outp =  nv94_disp_outp_sclass,
-	.mthd.core = &nv94_disp_mast_mthd_chan,
-	.mthd.base = &nv84_disp_sync_mthd_chan,
+	.mthd.core = &nv94_disp_core_mthd_chan,
+	.mthd.base = &nv84_disp_base_mthd_chan,
 	.mthd.ovly = &nv84_disp_ovly_mthd_chan,
 	.mthd.prev = 0x000004,
-	.head.scanoutpos = nv50_disp_base_scanoutpos,
+	.head.scanoutpos = nv50_disp_main_scanoutpos,
 }.base.base;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nva0.c b/drivers/gpu/drm/nouveau/core/engine/disp/nva0.c
index c67e68aadd45..b32456c9494f 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nva0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nva0.c
@@ -80,8 +80,8 @@ nva0_disp_ovly_mthd_chan = {
 
 static struct nouveau_oclass
 nva0_disp_sclass[] = {
-	{ GT200_DISP_CORE_CHANNEL_DMA, &nv50_disp_mast_ofuncs.base },
-	{ GT200_DISP_BASE_CHANNEL_DMA, &nv50_disp_sync_ofuncs.base },
+	{ GT200_DISP_CORE_CHANNEL_DMA, &nv50_disp_core_ofuncs.base },
+	{ GT200_DISP_BASE_CHANNEL_DMA, &nv50_disp_base_ofuncs.base },
 	{ GT200_DISP_OVERLAY_CHANNEL_DMA, &nv50_disp_ovly_ofuncs.base },
 	{ G82_DISP_OVERLAY, &nv50_disp_oimm_ofuncs.base },
 	{ G82_DISP_CURSOR, &nv50_disp_curs_ofuncs.base },
@@ -89,8 +89,8 @@ nva0_disp_sclass[] = {
 };
 
 static struct nouveau_oclass
-nva0_disp_base_oclass[] = {
-	{ GT200_DISP, &nv50_disp_base_ofuncs },
+nva0_disp_main_oclass[] = {
+	{ GT200_DISP, &nv50_disp_main_ofuncs },
 	{}
 };
 
@@ -116,7 +116,7 @@ nva0_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	nv_engine(priv)->sclass = nva0_disp_base_oclass;
+	nv_engine(priv)->sclass = nva0_disp_main_oclass;
 	nv_engine(priv)->cclass = &nv50_disp_cclass;
 	nv_subdev(priv)->intr = nv50_disp_intr;
 	INIT_WORK(&priv->supervisor, nv50_disp_intr_supervisor);
@@ -144,9 +144,9 @@ nva0_disp_oclass = &(struct nv50_disp_impl) {
 	},
 	.base.vblank = &nv50_disp_vblank_func,
 	.base.outp =  nv50_disp_outp_sclass,
-	.mthd.core = &nv84_disp_mast_mthd_chan,
-	.mthd.base = &nv84_disp_sync_mthd_chan,
+	.mthd.core = &nv84_disp_core_mthd_chan,
+	.mthd.base = &nv84_disp_base_mthd_chan,
 	.mthd.ovly = &nva0_disp_ovly_mthd_chan,
 	.mthd.prev = 0x000004,
-	.head.scanoutpos = nv50_disp_base_scanoutpos,
+	.head.scanoutpos = nv50_disp_main_scanoutpos,
 }.base.base;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nva3.c b/drivers/gpu/drm/nouveau/core/engine/disp/nva3.c
index 22969f355aae..951d79f9b781 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nva3.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nva3.c
@@ -35,8 +35,8 @@
 
 static struct nouveau_oclass
 nva3_disp_sclass[] = {
-	{ GT214_DISP_CORE_CHANNEL_DMA, &nv50_disp_mast_ofuncs.base },
-	{ GT214_DISP_BASE_CHANNEL_DMA, &nv50_disp_sync_ofuncs.base },
+	{ GT214_DISP_CORE_CHANNEL_DMA, &nv50_disp_core_ofuncs.base },
+	{ GT214_DISP_BASE_CHANNEL_DMA, &nv50_disp_base_ofuncs.base },
 	{ GT214_DISP_OVERLAY_CHANNEL_DMA, &nv50_disp_ovly_ofuncs.base },
 	{ GT214_DISP_OVERLAY, &nv50_disp_oimm_ofuncs.base },
 	{ GT214_DISP_CURSOR, &nv50_disp_curs_ofuncs.base },
@@ -44,8 +44,8 @@ nva3_disp_sclass[] = {
 };
 
 static struct nouveau_oclass
-nva3_disp_base_oclass[] = {
-	{ GT214_DISP, &nv50_disp_base_ofuncs },
+nva3_disp_main_oclass[] = {
+	{ GT214_DISP, &nv50_disp_main_ofuncs },
 	{}
 };
 
@@ -71,7 +71,7 @@ nva3_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	nv_engine(priv)->sclass = nva3_disp_base_oclass;
+	nv_engine(priv)->sclass = nva3_disp_main_oclass;
 	nv_engine(priv)->cclass = &nv50_disp_cclass;
 	nv_subdev(priv)->intr = nv50_disp_intr;
 	INIT_WORK(&priv->supervisor, nv50_disp_intr_supervisor);
@@ -100,9 +100,9 @@ nva3_disp_oclass = &(struct nv50_disp_impl) {
 	},
 	.base.vblank = &nv50_disp_vblank_func,
 	.base.outp =  nv94_disp_outp_sclass,
-	.mthd.core = &nv94_disp_mast_mthd_chan,
-	.mthd.base = &nv84_disp_sync_mthd_chan,
+	.mthd.core = &nv94_disp_core_mthd_chan,
+	.mthd.base = &nv84_disp_base_mthd_chan,
 	.mthd.ovly = &nv84_disp_ovly_mthd_chan,
 	.mthd.prev = 0x000004,
-	.head.scanoutpos = nv50_disp_base_scanoutpos,
+	.head.scanoutpos = nv50_disp_main_scanoutpos,
 }.base.base;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c b/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c
index 747e64bb9c06..181a2d57e356 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c
@@ -51,12 +51,14 @@ nvd0_disp_chan_uevent_fini(struct nvkm_event *event, int type, int index)
 {
 	struct nv50_disp_priv *priv = container_of(event, typeof(*priv), uevent);
 	nv_mask(priv, 0x610090, 0x00000001 << index, 0x00000000 << index);
+	nv_wr32(priv, 0x61008c, 0x00000001 << index);
 }
 
 static void
 nvd0_disp_chan_uevent_init(struct nvkm_event *event, int types, int index)
 {
 	struct nv50_disp_priv *priv = container_of(event, typeof(*priv), uevent);
+	nv_wr32(priv, 0x61008c, 0x00000001 << index);
 	nv_mask(priv, 0x610090, 0x00000001 << index, 0x00000001 << index);
 }
 
@@ -151,7 +153,7 @@ nvd0_disp_dmac_fini(struct nouveau_object *object, bool suspend)
  ******************************************************************************/
 
 const struct nv50_disp_mthd_list
-nvd0_disp_mast_mthd_base = {
+nvd0_disp_core_mthd_base = {
 	.mthd = 0x0000,
 	.addr = 0x000000,
 	.data = {
@@ -164,7 +166,7 @@ nvd0_disp_mast_mthd_base = {
 };
 
 const struct nv50_disp_mthd_list
-nvd0_disp_mast_mthd_dac = {
+nvd0_disp_core_mthd_dac = {
 	.mthd = 0x0020,
 	.addr = 0x000020,
 	.data = {
@@ -177,7 +179,7 @@ nvd0_disp_mast_mthd_dac = {
 };
 
 const struct nv50_disp_mthd_list
-nvd0_disp_mast_mthd_sor = {
+nvd0_disp_core_mthd_sor = {
 	.mthd = 0x0020,
 	.addr = 0x000020,
 	.data = {
@@ -190,7 +192,7 @@ nvd0_disp_mast_mthd_sor = {
 };
 
 const struct nv50_disp_mthd_list
-nvd0_disp_mast_mthd_pior = {
+nvd0_disp_core_mthd_pior = {
 	.mthd = 0x0020,
 	.addr = 0x000020,
 	.data = {
@@ -203,7 +205,7 @@ nvd0_disp_mast_mthd_pior = {
 };
 
 static const struct nv50_disp_mthd_list
-nvd0_disp_mast_mthd_head = {
+nvd0_disp_core_mthd_head = {
 	.mthd = 0x0300,
 	.addr = 0x000300,
 	.data = {
@@ -277,21 +279,21 @@ nvd0_disp_mast_mthd_head = {
 };
 
 static const struct nv50_disp_mthd_chan
-nvd0_disp_mast_mthd_chan = {
+nvd0_disp_core_mthd_chan = {
 	.name = "Core",
 	.addr = 0x000000,
 	.data = {
-		{ "Global", 1, &nvd0_disp_mast_mthd_base },
-		{    "DAC", 3, &nvd0_disp_mast_mthd_dac  },
-		{    "SOR", 8, &nvd0_disp_mast_mthd_sor  },
-		{   "PIOR", 4, &nvd0_disp_mast_mthd_pior },
-		{   "HEAD", 4, &nvd0_disp_mast_mthd_head },
+		{ "Global", 1, &nvd0_disp_core_mthd_base },
+		{    "DAC", 3, &nvd0_disp_core_mthd_dac  },
+		{    "SOR", 8, &nvd0_disp_core_mthd_sor  },
+		{   "PIOR", 4, &nvd0_disp_core_mthd_pior },
+		{   "HEAD", 4, &nvd0_disp_core_mthd_head },
 		{}
 	}
 };
 
 static int
-nvd0_disp_mast_init(struct nouveau_object *object)
+nvd0_disp_core_init(struct nouveau_object *object)
 {
 	struct nv50_disp_priv *priv = (void *)object->engine;
 	struct nv50_disp_dmac *mast = (void *)object;
@@ -322,7 +324,7 @@ nvd0_disp_mast_init(struct nouveau_object *object)
 }
 
 static int
-nvd0_disp_mast_fini(struct nouveau_object *object, bool suspend)
+nvd0_disp_core_fini(struct nouveau_object *object, bool suspend)
 {
 	struct nv50_disp_priv *priv = (void *)object->engine;
 	struct nv50_disp_dmac *mast = (void *)object;
@@ -344,11 +346,11 @@ nvd0_disp_mast_fini(struct nouveau_object *object, bool suspend)
 }
 
 struct nv50_disp_chan_impl
-nvd0_disp_mast_ofuncs = {
-	.base.ctor = nv50_disp_mast_ctor,
+nvd0_disp_core_ofuncs = {
+	.base.ctor = nv50_disp_core_ctor,
 	.base.dtor = nv50_disp_dmac_dtor,
-	.base.init = nvd0_disp_mast_init,
-	.base.fini = nvd0_disp_mast_fini,
+	.base.init = nvd0_disp_core_init,
+	.base.fini = nvd0_disp_core_fini,
 	.base.ntfy = nv50_disp_chan_ntfy,
 	.base.map  = nv50_disp_chan_map,
 	.base.rd32 = nv50_disp_chan_rd32,
@@ -363,7 +365,7 @@ nvd0_disp_mast_ofuncs = {
  ******************************************************************************/
 
 static const struct nv50_disp_mthd_list
-nvd0_disp_sync_mthd_base = {
+nvd0_disp_base_mthd_base = {
 	.mthd = 0x0000,
 	.addr = 0x000000,
 	.data = {
@@ -413,7 +415,7 @@ nvd0_disp_sync_mthd_base = {
 };
 
 static const struct nv50_disp_mthd_list
-nvd0_disp_sync_mthd_image = {
+nvd0_disp_base_mthd_image = {
 	.mthd = 0x0400,
 	.addr = 0x000400,
 	.data = {
@@ -427,19 +429,19 @@ nvd0_disp_sync_mthd_image = {
 };
 
 const struct nv50_disp_mthd_chan
-nvd0_disp_sync_mthd_chan = {
+nvd0_disp_base_mthd_chan = {
 	.name = "Base",
 	.addr = 0x001000,
 	.data = {
-		{ "Global", 1, &nvd0_disp_sync_mthd_base },
-		{  "Image", 2, &nvd0_disp_sync_mthd_image },
+		{ "Global", 1, &nvd0_disp_base_mthd_base },
+		{  "Image", 2, &nvd0_disp_base_mthd_image },
 		{}
 	}
 };
 
 struct nv50_disp_chan_impl
-nvd0_disp_sync_ofuncs = {
-	.base.ctor = nv50_disp_sync_ctor,
+nvd0_disp_base_ofuncs = {
+	.base.ctor = nv50_disp_base_ctor,
 	.base.dtor = nv50_disp_dmac_dtor,
 	.base.init = nvd0_disp_dmac_init,
 	.base.fini = nvd0_disp_dmac_fini,
@@ -624,7 +626,7 @@ nvd0_disp_curs_ofuncs = {
  ******************************************************************************/
 
 int
-nvd0_disp_base_scanoutpos(NV50_DISP_MTHD_V0)
+nvd0_disp_main_scanoutpos(NV50_DISP_MTHD_V0)
 {
 	const u32 total  = nv_rd32(priv, 0x640414 + (head * 0x300));
 	const u32 blanke = nv_rd32(priv, 0x64041c + (head * 0x300));
@@ -656,7 +658,7 @@ nvd0_disp_base_scanoutpos(NV50_DISP_MTHD_V0)
 }
 
 static int
-nvd0_disp_base_init(struct nouveau_object *object)
+nvd0_disp_main_init(struct nouveau_object *object)
 {
 	struct nv50_disp_priv *priv = (void *)object->engine;
 	struct nv50_disp_base *base = (void *)object;
@@ -725,7 +727,7 @@ nvd0_disp_base_init(struct nouveau_object *object)
 }
 
 static int
-nvd0_disp_base_fini(struct nouveau_object *object, bool suspend)
+nvd0_disp_main_fini(struct nouveau_object *object, bool suspend)
 {
 	struct nv50_disp_priv *priv = (void *)object->engine;
 	struct nv50_disp_base *base = (void *)object;
@@ -737,25 +739,25 @@ nvd0_disp_base_fini(struct nouveau_object *object, bool suspend)
 }
 
 struct nouveau_ofuncs
-nvd0_disp_base_ofuncs = {
-	.ctor = nv50_disp_base_ctor,
-	.dtor = nv50_disp_base_dtor,
-	.init = nvd0_disp_base_init,
-	.fini = nvd0_disp_base_fini,
-	.mthd = nv50_disp_base_mthd,
+nvd0_disp_main_ofuncs = {
+	.ctor = nv50_disp_main_ctor,
+	.dtor = nv50_disp_main_dtor,
+	.init = nvd0_disp_main_init,
+	.fini = nvd0_disp_main_fini,
+	.mthd = nv50_disp_main_mthd,
 	.ntfy = nouveau_disp_ntfy,
 };
 
 static struct nouveau_oclass
-nvd0_disp_base_oclass[] = {
-	{ GF110_DISP, &nvd0_disp_base_ofuncs },
+nvd0_disp_main_oclass[] = {
+	{ GF110_DISP, &nvd0_disp_main_ofuncs },
 	{}
 };
 
 static struct nouveau_oclass
 nvd0_disp_sclass[] = {
-	{ GF110_DISP_CORE_CHANNEL_DMA, &nvd0_disp_mast_ofuncs.base },
-	{ GF110_DISP_BASE_CHANNEL_DMA, &nvd0_disp_sync_ofuncs.base },
+	{ GF110_DISP_CORE_CHANNEL_DMA, &nvd0_disp_core_ofuncs.base },
+	{ GF110_DISP_BASE_CHANNEL_DMA, &nvd0_disp_base_ofuncs.base },
 	{ GF110_DISP_OVERLAY_CONTROL_DMA, &nvd0_disp_ovly_ofuncs.base },
 	{ GF110_DISP_OVERLAY, &nvd0_disp_oimm_ofuncs.base },
 	{ GF110_DISP_CURSOR, &nvd0_disp_curs_ofuncs.base },
@@ -1055,6 +1057,9 @@ nvd0_disp_intr_unk2_2(struct nv50_disp_priv *priv, int head)
 
 		if (nvkm_output_dp_train(outp, pclk, true))
 			ERR("link not trained before attach\n");
+	} else {
+		if (priv->sor.magic)
+			priv->sor.magic(outp);
 	}
 
 	exec_clkcmp(priv, head, 0, pclk, &conf);
@@ -1063,10 +1068,18 @@ nvd0_disp_intr_unk2_2(struct nv50_disp_priv *priv, int head)
 		addr = 0x612280 + (ffs(outp->info.or) - 1) * 0x800;
 		data = 0x00000000;
 	} else {
-		if (outp->info.type == DCB_OUTPUT_DP)
-			nvd0_disp_intr_unk2_2_tu(priv, head, &outp->info);
 		addr = 0x612300 + (ffs(outp->info.or) - 1) * 0x800;
 		data = (conf & 0x0100) ? 0x00000101 : 0x00000000;
+		switch (outp->info.type) {
+		case DCB_OUTPUT_TMDS:
+			nv_mask(priv, addr, 0x007c0000, 0x00280000);
+			break;
+		case DCB_OUTPUT_DP:
+			nvd0_disp_intr_unk2_2_tu(priv, head, &outp->info);
+			break;
+		default:
+			break;
+		}
 	}
 
 	nv_mask(priv, addr, 0x00000707, data);
@@ -1259,7 +1272,7 @@ nvd0_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	nv_engine(priv)->sclass = nvd0_disp_base_oclass;
+	nv_engine(priv)->sclass = nvd0_disp_main_oclass;
 	nv_engine(priv)->cclass = &nv50_disp_cclass;
 	nv_subdev(priv)->intr = nvd0_disp_intr;
 	INIT_WORK(&priv->supervisor, nvd0_disp_intr_supervisor);
@@ -1292,9 +1305,9 @@ nvd0_disp_oclass = &(struct nv50_disp_impl) {
 	},
 	.base.vblank = &nvd0_disp_vblank_func,
 	.base.outp =  nvd0_disp_outp_sclass,
-	.mthd.core = &nvd0_disp_mast_mthd_chan,
-	.mthd.base = &nvd0_disp_sync_mthd_chan,
+	.mthd.core = &nvd0_disp_core_mthd_chan,
+	.mthd.base = &nvd0_disp_base_mthd_chan,
 	.mthd.ovly = &nvd0_disp_ovly_mthd_chan,
 	.mthd.prev = -0x020000,
-	.head.scanoutpos = nvd0_disp_base_scanoutpos,
+	.head.scanoutpos = nvd0_disp_main_scanoutpos,
 }.base.base;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nve0.c b/drivers/gpu/drm/nouveau/core/engine/disp/nve0.c
index db144b2cf06b..55debec7e68f 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nve0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nve0.c
@@ -34,7 +34,7 @@
  ******************************************************************************/
 
 static const struct nv50_disp_mthd_list
-nve0_disp_mast_mthd_head = {
+nve0_disp_core_mthd_head = {
 	.mthd = 0x0300,
 	.addr = 0x000300,
 	.data = {
@@ -113,15 +113,15 @@ nve0_disp_mast_mthd_head = {
 };
 
 const struct nv50_disp_mthd_chan
-nve0_disp_mast_mthd_chan = {
+nve0_disp_core_mthd_chan = {
 	.name = "Core",
 	.addr = 0x000000,
 	.data = {
-		{ "Global", 1, &nvd0_disp_mast_mthd_base },
-		{    "DAC", 3, &nvd0_disp_mast_mthd_dac  },
-		{    "SOR", 8, &nvd0_disp_mast_mthd_sor  },
-		{   "PIOR", 4, &nvd0_disp_mast_mthd_pior },
-		{   "HEAD", 4, &nve0_disp_mast_mthd_head },
+		{ "Global", 1, &nvd0_disp_core_mthd_base },
+		{    "DAC", 3, &nvd0_disp_core_mthd_dac  },
+		{    "SOR", 8, &nvd0_disp_core_mthd_sor  },
+		{   "PIOR", 4, &nvd0_disp_core_mthd_pior },
+		{   "HEAD", 4, &nve0_disp_core_mthd_head },
 		{}
 	}
 };
@@ -200,8 +200,8 @@ nve0_disp_ovly_mthd_chan = {
 
 static struct nouveau_oclass
 nve0_disp_sclass[] = {
-	{ GK104_DISP_CORE_CHANNEL_DMA, &nvd0_disp_mast_ofuncs.base },
-	{ GK104_DISP_BASE_CHANNEL_DMA, &nvd0_disp_sync_ofuncs.base },
+	{ GK104_DISP_CORE_CHANNEL_DMA, &nvd0_disp_core_ofuncs.base },
+	{ GK104_DISP_BASE_CHANNEL_DMA, &nvd0_disp_base_ofuncs.base },
 	{ GK104_DISP_OVERLAY_CONTROL_DMA, &nvd0_disp_ovly_ofuncs.base },
 	{ GK104_DISP_OVERLAY, &nvd0_disp_oimm_ofuncs.base },
 	{ GK104_DISP_CURSOR, &nvd0_disp_curs_ofuncs.base },
@@ -209,8 +209,8 @@ nve0_disp_sclass[] = {
 };
 
 static struct nouveau_oclass
-nve0_disp_base_oclass[] = {
-	{ GK104_DISP, &nvd0_disp_base_ofuncs },
+nve0_disp_main_oclass[] = {
+	{ GK104_DISP, &nvd0_disp_main_ofuncs },
 	{}
 };
 
@@ -237,7 +237,7 @@ nve0_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	nv_engine(priv)->sclass = nve0_disp_base_oclass;
+	nv_engine(priv)->sclass = nve0_disp_main_oclass;
 	nv_engine(priv)->cclass = &nv50_disp_cclass;
 	nv_subdev(priv)->intr = nvd0_disp_intr;
 	INIT_WORK(&priv->supervisor, nvd0_disp_intr_supervisor);
@@ -264,9 +264,9 @@ nve0_disp_oclass = &(struct nv50_disp_impl) {
 	},
 	.base.vblank = &nvd0_disp_vblank_func,
 	.base.outp =  nvd0_disp_outp_sclass,
-	.mthd.core = &nve0_disp_mast_mthd_chan,
-	.mthd.base = &nvd0_disp_sync_mthd_chan,
+	.mthd.core = &nve0_disp_core_mthd_chan,
+	.mthd.base = &nvd0_disp_base_mthd_chan,
 	.mthd.ovly = &nve0_disp_ovly_mthd_chan,
 	.mthd.prev = -0x020000,
-	.head.scanoutpos = nvd0_disp_base_scanoutpos,
+	.head.scanoutpos = nvd0_disp_main_scanoutpos,
 }.base.base;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nvf0.c b/drivers/gpu/drm/nouveau/core/engine/disp/nvf0.c
index 402d7d67d806..3e7e2d28744c 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nvf0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nvf0.c
@@ -35,8 +35,8 @@
 
 static struct nouveau_oclass
 nvf0_disp_sclass[] = {
-	{ GK110_DISP_CORE_CHANNEL_DMA, &nvd0_disp_mast_ofuncs.base },
-	{ GK110_DISP_BASE_CHANNEL_DMA, &nvd0_disp_sync_ofuncs.base },
+	{ GK110_DISP_CORE_CHANNEL_DMA, &nvd0_disp_core_ofuncs.base },
+	{ GK110_DISP_BASE_CHANNEL_DMA, &nvd0_disp_base_ofuncs.base },
 	{ GK104_DISP_OVERLAY_CONTROL_DMA, &nvd0_disp_ovly_ofuncs.base },
 	{ GK104_DISP_OVERLAY, &nvd0_disp_oimm_ofuncs.base },
 	{ GK104_DISP_CURSOR, &nvd0_disp_curs_ofuncs.base },
@@ -44,8 +44,8 @@ nvf0_disp_sclass[] = {
 };
 
 static struct nouveau_oclass
-nvf0_disp_base_oclass[] = {
-	{ GK110_DISP, &nvd0_disp_base_ofuncs },
+nvf0_disp_main_oclass[] = {
+	{ GK110_DISP, &nvd0_disp_main_ofuncs },
 	{}
 };
 
@@ -72,7 +72,7 @@ nvf0_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	nv_engine(priv)->sclass = nvf0_disp_base_oclass;
+	nv_engine(priv)->sclass = nvf0_disp_main_oclass;
 	nv_engine(priv)->cclass = &nv50_disp_cclass;
 	nv_subdev(priv)->intr = nvd0_disp_intr;
 	INIT_WORK(&priv->supervisor, nvd0_disp_intr_supervisor);
@@ -99,9 +99,9 @@ nvf0_disp_oclass = &(struct nv50_disp_impl) {
 	},
 	.base.vblank = &nvd0_disp_vblank_func,
 	.base.outp =  nvd0_disp_outp_sclass,
-	.mthd.core = &nve0_disp_mast_mthd_chan,
-	.mthd.base = &nvd0_disp_sync_mthd_chan,
+	.mthd.core = &nve0_disp_core_mthd_chan,
+	.mthd.base = &nvd0_disp_base_mthd_chan,
 	.mthd.ovly = &nve0_disp_ovly_mthd_chan,
 	.mthd.prev = -0x020000,
-	.head.scanoutpos = nvd0_disp_base_scanoutpos,
+	.head.scanoutpos = nvd0_disp_main_scanoutpos,
 }.base.base;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/outp.c b/drivers/gpu/drm/nouveau/core/engine/disp/outp.c
index a5ff00a9cedc..bbd9b6fdc90f 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/outp.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/outp.c
@@ -85,7 +85,10 @@ nvkm_output_create_(struct nouveau_object *parent,
 	    dcbE->sorconf.link : 0, dcbE->connector, dcbE->i2c_index,
 	    dcbE->bus, dcbE->heads);
 
-	outp->port = i2c->find(i2c, outp->info.i2c_index);
+	if (outp->info.type != DCB_OUTPUT_DP)
+		outp->port = i2c->find(i2c, NV_I2C_PORT(outp->info.i2c_index));
+	else
+		outp->port = i2c->find(i2c, NV_I2C_AUX(outp->info.i2c_index));
 	outp->edid = outp->port;
 
 	data = nvbios_connEp(bios, outp->info.connector, &ver, &hdr, &connE);
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/sorgm204.c b/drivers/gpu/drm/nouveau/core/engine/disp/sorgm204.c
new file mode 100644
index 000000000000..0b4fad39e9a6
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/sorgm204.c
@@ -0,0 +1,144 @@
+/*
+ * 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 <core/os.h>
+
+#include <subdev/bios.h>
+#include <subdev/bios/dcb.h>
+#include <subdev/bios/dp.h>
+#include <subdev/bios/init.h>
+#include <subdev/timer.h>
+
+#include "nv50.h"
+
+static inline u32
+gm204_sor_soff(struct nvkm_output_dp *outp)
+{
+	return (ffs(outp->base.info.or) - 1) * 0x800;
+}
+
+static inline u32
+gm204_sor_loff(struct nvkm_output_dp *outp)
+{
+	return gm204_sor_soff(outp) + !(outp->base.info.sorconf.link & 1) * 0x80;
+}
+
+void
+gm204_sor_magic(struct nvkm_output *outp)
+{
+	struct nv50_disp_priv *priv = (void *)nouveau_disp(outp);
+	const u32 soff = outp->or * 0x100;
+	const u32 data = outp->or + 1;
+	if (outp->info.sorconf.link & 1)
+		nv_mask(priv, 0x612308 + soff, 0x0000001f, 0x00000000 | data);
+	if (outp->info.sorconf.link & 2)
+		nv_mask(priv, 0x612388 + soff, 0x0000001f, 0x00000010 | data);
+}
+
+static inline u32
+gm204_sor_dp_lane_map(struct nv50_disp_priv *priv, u8 lane)
+{
+	return lane * 0x08;
+}
+
+static int
+gm204_sor_dp_pattern(struct nvkm_output_dp *outp, int pattern)
+{
+	struct nv50_disp_priv *priv = (void *)nouveau_disp(outp);
+	const u32 soff = gm204_sor_soff(outp);
+	const u32 data = 0x01010101 * pattern;
+	if (outp->base.info.sorconf.link & 1)
+		nv_mask(priv, 0x61c110 + soff, 0x0f0f0f0f, data);
+	else
+		nv_mask(priv, 0x61c12c + soff, 0x0f0f0f0f, data);
+	return 0;
+}
+
+static int
+gm204_sor_dp_lnk_pwr(struct nvkm_output_dp *outp, int nr)
+{
+	struct nv50_disp_priv *priv = (void *)nouveau_disp(outp);
+	const u32 soff = gm204_sor_soff(outp);
+	const u32 loff = gm204_sor_loff(outp);
+	u32 mask = 0, i;
+
+	for (i = 0; i < nr; i++)
+		mask |= 1 << (gm204_sor_dp_lane_map(priv, i) >> 3);
+
+	nv_mask(priv, 0x61c130 + loff, 0x0000000f, mask);
+	nv_mask(priv, 0x61c034 + soff, 0x80000000, 0x80000000);
+	nv_wait(priv, 0x61c034 + soff, 0x80000000, 0x00000000);
+	return 0;
+}
+
+static int
+gm204_sor_dp_drv_ctl(struct nvkm_output_dp *outp, int ln, int vs, int pe, int pc)
+{
+	struct nv50_disp_priv *priv = (void *)nouveau_disp(outp);
+	struct nouveau_bios *bios = nouveau_bios(priv);
+	const u32 shift = gm204_sor_dp_lane_map(priv, ln);
+	const u32 loff = gm204_sor_loff(outp);
+	u32 addr, data[4];
+	u8  ver, hdr, cnt, len;
+	struct nvbios_dpout info;
+	struct nvbios_dpcfg ocfg;
+
+	addr = nvbios_dpout_match(bios, outp->base.info.hasht,
+					outp->base.info.hashm,
+				 &ver, &hdr, &cnt, &len, &info);
+	if (!addr)
+		return -ENODEV;
+
+	addr = nvbios_dpcfg_match(bios, addr, pc, vs, pe,
+				 &ver, &hdr, &cnt, &len, &ocfg);
+	if (!addr)
+		return -EINVAL;
+
+	data[0] = nv_rd32(priv, 0x61c118 + loff) & ~(0x000000ff << shift);
+	data[1] = nv_rd32(priv, 0x61c120 + loff) & ~(0x000000ff << shift);
+	data[2] = nv_rd32(priv, 0x61c130 + loff);
+	if ((data[2] & 0x0000ff00) < (ocfg.tx_pu << 8) || ln == 0)
+		data[2] = (data[2] & ~0x0000ff00) | (ocfg.tx_pu << 8);
+	nv_wr32(priv, 0x61c118 + loff, data[0] | (ocfg.dc << shift));
+	nv_wr32(priv, 0x61c120 + loff, data[1] | (ocfg.pe << shift));
+	nv_wr32(priv, 0x61c130 + loff, data[2] | (ocfg.tx_pu << 8));
+	data[3] = nv_rd32(priv, 0x61c13c + loff) & ~(0x000000ff << shift);
+	nv_wr32(priv, 0x61c13c + loff, data[3] | (ocfg.pc << shift));
+	return 0;
+}
+
+struct nvkm_output_dp_impl
+gm204_sor_dp_impl = {
+	.base.base.handle = DCB_OUTPUT_DP,
+	.base.base.ofuncs = &(struct nouveau_ofuncs) {
+		.ctor = _nvkm_output_dp_ctor,
+		.dtor = _nvkm_output_dp_dtor,
+		.init = _nvkm_output_dp_init,
+		.fini = _nvkm_output_dp_fini,
+	},
+	.pattern = gm204_sor_dp_pattern,
+	.lnk_pwr = gm204_sor_dp_lnk_pwr,
+	.lnk_ctl = nvd0_sor_dp_lnk_ctl,
+	.drv_ctl = gm204_sor_dp_drv_ctl,
+};
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/sornvd0.c b/drivers/gpu/drm/nouveau/core/engine/disp/sornvd0.c
index 7b7bbc3e459e..fdab2939070c 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/sornvd0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/sornvd0.c
@@ -60,7 +60,7 @@ nvd0_sor_dp_pattern(struct nvkm_output_dp *outp, int pattern)
 	return 0;
 }
 
-static int
+int
 nvd0_sor_dp_lnk_ctl(struct nvkm_output_dp *outp, int nr, int bw, bool ef)
 {
 	struct nv50_disp_priv *priv = (void *)nouveau_disp(outp);
diff --git a/drivers/gpu/drm/nouveau/core/engine/dmaobj/nvd0.c b/drivers/gpu/drm/nouveau/core/engine/dmaobj/nvd0.c
index 3fc4f0b0eaca..19f5f6522962 100644
--- a/drivers/gpu/drm/nouveau/core/engine/dmaobj/nvd0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/dmaobj/nvd0.c
@@ -51,6 +51,7 @@ nvd0_dmaobj_bind(struct nouveau_dmaobj *dmaobj,
 		case GK104_DISP_CORE_CHANNEL_DMA:
 		case GK110_DISP_CORE_CHANNEL_DMA:
 		case GM107_DISP_CORE_CHANNEL_DMA:
+		case GM204_DISP_CORE_CHANNEL_DMA:
 		case GF110_DISP_BASE_CHANNEL_DMA:
 		case GK104_DISP_BASE_CHANNEL_DMA:
 		case GK110_DISP_BASE_CHANNEL_DMA:
diff --git a/drivers/gpu/drm/nouveau/core/engine/fifo/nve0.c b/drivers/gpu/drm/nouveau/core/engine/fifo/nve0.c
index f8734eb74eaa..6a8db7c80bd1 100644
--- a/drivers/gpu/drm/nouveau/core/engine/fifo/nve0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/fifo/nve0.c
@@ -792,7 +792,7 @@ nve0_fifo_intr_fault(struct nve0_fifo_priv *priv, int unit)
 	nouveau_engctx_put(engctx);
 }
 
-static const struct nouveau_bitfield nve0_fifo_pbdma_intr[] = {
+static const struct nouveau_bitfield nve0_fifo_pbdma_intr_0[] = {
 	{ 0x00000001, "MEMREQ" },
 	{ 0x00000002, "MEMACK_TIMEOUT" },
 	{ 0x00000004, "MEMACK_EXTRA" },
@@ -827,9 +827,10 @@ static const struct nouveau_bitfield nve0_fifo_pbdma_intr[] = {
 };
 
 static void
-nve0_fifo_intr_pbdma(struct nve0_fifo_priv *priv, int unit)
+nve0_fifo_intr_pbdma_0(struct nve0_fifo_priv *priv, int unit)
 {
-	u32 stat = nv_rd32(priv, 0x040108 + (unit * 0x2000));
+	u32 mask = nv_rd32(priv, 0x04010c + (unit * 0x2000));
+	u32 stat = nv_rd32(priv, 0x040108 + (unit * 0x2000)) & mask;
 	u32 addr = nv_rd32(priv, 0x0400c0 + (unit * 0x2000));
 	u32 data = nv_rd32(priv, 0x0400c4 + (unit * 0x2000));
 	u32 chid = nv_rd32(priv, 0x040120 + (unit * 0x2000)) & 0xfff;
@@ -840,11 +841,12 @@ nve0_fifo_intr_pbdma(struct nve0_fifo_priv *priv, int unit)
 	if (stat & 0x00800000) {
 		if (!nve0_fifo_swmthd(priv, chid, mthd, data))
 			show &= ~0x00800000;
+		nv_wr32(priv, 0x0400c0 + (unit * 0x2000), 0x80600008);
 	}
 
 	if (show) {
 		nv_error(priv, "PBDMA%d:", unit);
-		nouveau_bitfield_print(nve0_fifo_pbdma_intr, show);
+		nouveau_bitfield_print(nve0_fifo_pbdma_intr_0, show);
 		pr_cont("\n");
 		nv_error(priv,
 			 "PBDMA%d: ch %d [%s] subc %d mthd 0x%04x data 0x%08x\n",
@@ -853,10 +855,37 @@ nve0_fifo_intr_pbdma(struct nve0_fifo_priv *priv, int unit)
 			 subc, mthd, data);
 	}
 
-	nv_wr32(priv, 0x0400c0 + (unit * 0x2000), 0x80600008);
 	nv_wr32(priv, 0x040108 + (unit * 0x2000), stat);
 }
 
+static const struct nouveau_bitfield nve0_fifo_pbdma_intr_1[] = {
+	{ 0x00000001, "HCE_RE_ILLEGAL_OP" },
+	{ 0x00000002, "HCE_RE_ALIGNB" },
+	{ 0x00000004, "HCE_PRIV" },
+	{ 0x00000008, "HCE_ILLEGAL_MTHD" },
+	{ 0x00000010, "HCE_ILLEGAL_CLASS" },
+	{}
+};
+
+static void
+nve0_fifo_intr_pbdma_1(struct nve0_fifo_priv *priv, int unit)
+{
+	u32 mask = nv_rd32(priv, 0x04014c + (unit * 0x2000));
+	u32 stat = nv_rd32(priv, 0x040148 + (unit * 0x2000)) & mask;
+	u32 chid = nv_rd32(priv, 0x040120 + (unit * 0x2000)) & 0xfff;
+
+	if (stat) {
+		nv_error(priv, "PBDMA%d:", unit);
+		nouveau_bitfield_print(nve0_fifo_pbdma_intr_1, stat);
+		pr_cont("\n");
+		nv_error(priv, "PBDMA%d: ch %d %08x %08x\n", unit, chid,
+			 nv_rd32(priv, 0x040150 + (unit * 0x2000)),
+			 nv_rd32(priv, 0x040154 + (unit * 0x2000)));
+	}
+
+	nv_wr32(priv, 0x040148 + (unit * 0x2000), stat);
+}
+
 static void
 nve0_fifo_intr_runlist(struct nve0_fifo_priv *priv)
 {
@@ -939,7 +968,8 @@ nve0_fifo_intr(struct nouveau_subdev *subdev)
 		u32 mask = nv_rd32(priv, 0x0025a0);
 		while (mask) {
 			u32 unit = __ffs(mask);
-			nve0_fifo_intr_pbdma(priv, unit);
+			nve0_fifo_intr_pbdma_0(priv, unit);
+			nve0_fifo_intr_pbdma_1(priv, unit);
 			nv_wr32(priv, 0x0025a0, (1 << unit));
 			mask &= ~(1 << unit);
 		}
@@ -1022,6 +1052,12 @@ nve0_fifo_init(struct nouveau_object *object)
 		nv_wr32(priv, 0x04010c + (i * 0x2000), 0xfffffeff); /* INTREN */
 	}
 
+	/* PBDMA[n].HCE */
+	for (i = 0; i < priv->spoon_nr; i++) {
+		nv_wr32(priv, 0x040148 + (i * 0x2000), 0xffffffff); /* INTR */
+		nv_wr32(priv, 0x04014c + (i * 0x2000), 0xffffffff); /* INTREN */
+	}
+
 	nv_wr32(priv, 0x002254, 0x10000000 | priv->user.bar.offset >> 12);
 
 	nv_wr32(priv, 0x002100, 0xffffffff);
diff --git a/drivers/gpu/drm/nouveau/core/engine/graph/nvc0.c b/drivers/gpu/drm/nouveau/core/engine/graph/nvc0.c
index 30fd1dc64f93..17251e4b9e86 100644
--- a/drivers/gpu/drm/nouveau/core/engine/graph/nvc0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/graph/nvc0.c
@@ -1557,7 +1557,7 @@ nvc0_graph_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 		    nvc0_graph_ctor_fw(priv, "fuc409d", &priv->fuc409d) ||
 		    nvc0_graph_ctor_fw(priv, "fuc41ac", &priv->fuc41ac) ||
 		    nvc0_graph_ctor_fw(priv, "fuc41ad", &priv->fuc41ad))
-			return -EINVAL;
+			return -ENODEV;
 		priv->firmware = true;
 	}
 
diff --git a/drivers/gpu/drm/nouveau/core/include/core/device.h b/drivers/gpu/drm/nouveau/core/include/core/device.h
index 1d9d893929bb..2ec2e50d3676 100644
--- a/drivers/gpu/drm/nouveau/core/include/core/device.h
+++ b/drivers/gpu/drm/nouveau/core/include/core/device.h
@@ -16,6 +16,7 @@ enum nv_subdev_type {
 	 * to during POST.
 	 */
 	NVDEV_SUBDEV_DEVINIT,
+	NVDEV_SUBDEV_IBUS,
 	NVDEV_SUBDEV_GPIO,
 	NVDEV_SUBDEV_I2C,
 	NVDEV_SUBDEV_DEVINIT_LAST = NVDEV_SUBDEV_I2C,
@@ -31,7 +32,6 @@ enum nv_subdev_type {
 	NVDEV_SUBDEV_TIMER,
 	NVDEV_SUBDEV_FB,
 	NVDEV_SUBDEV_LTC,
-	NVDEV_SUBDEV_IBUS,
 	NVDEV_SUBDEV_INSTMEM,
 	NVDEV_SUBDEV_VM,
 	NVDEV_SUBDEV_BAR,
@@ -92,6 +92,7 @@ struct nouveau_device {
 		GM100    = 0x110,
 	} card_type;
 	u32 chipset;
+	u8  chiprev;
 	u32 crystal;
 
 	struct nouveau_oclass *oclass[NVDEV_SUBDEV_NR];
@@ -158,6 +159,12 @@ nv_device_is_pci(struct nouveau_device *device)
 	return device->pdev != NULL;
 }
 
+static inline bool
+nv_device_is_cpu_coherent(struct nouveau_device *device)
+{
+	return (!IS_ENABLED(CONFIG_ARM) && nv_device_is_pci(device));
+}
+
 static inline struct device *
 nv_device_base(struct nouveau_device *device)
 {
diff --git a/drivers/gpu/drm/nouveau/core/include/core/handle.h b/drivers/gpu/drm/nouveau/core/include/core/handle.h
index ceb67d770875..d22a59138a9b 100644
--- a/drivers/gpu/drm/nouveau/core/include/core/handle.h
+++ b/drivers/gpu/drm/nouveau/core/include/core/handle.h
@@ -23,11 +23,6 @@ void nouveau_handle_destroy(struct nouveau_handle *);
 int  nouveau_handle_init(struct nouveau_handle *);
 int  nouveau_handle_fini(struct nouveau_handle *, bool suspend);
 
-int  nouveau_handle_new(struct nouveau_object *, u32 parent, u32 handle,
-			u16 oclass, void *data, u32 size,
-			struct nouveau_object **);
-int  nouveau_handle_del(struct nouveau_object *, u32 parent, u32 handle);
-
 struct nouveau_object *
 nouveau_handle_ref(struct nouveau_object *, u32 name);
 
diff --git a/drivers/gpu/drm/nouveau/core/include/core/object.h b/drivers/gpu/drm/nouveau/core/include/core/object.h
index d7039482d6fd..2e2afa502c99 100644
--- a/drivers/gpu/drm/nouveau/core/include/core/object.h
+++ b/drivers/gpu/drm/nouveau/core/include/core/object.h
@@ -203,21 +203,4 @@ nv_memcmp(void *obj, u32 addr, const char *str, u32 len)
 	return 0;
 }
 
-#include <core/handle.h>
-
-static inline int
-nouveau_object_new(struct nouveau_object *client, u32 parent, u32 handle,
-		   u16 oclass, void *data, u32 size,
-		   struct nouveau_object **pobject)
-{
-	return nouveau_handle_new(client, parent, handle, oclass,
-				  data, size, pobject);
-}
-
-static inline int
-nouveau_object_del(struct nouveau_object *client, u32 parent, u32 handle)
-{
-	return nouveau_handle_del(client, parent, handle);
-}
-
 #endif
diff --git a/drivers/gpu/drm/nouveau/core/include/engine/disp.h b/drivers/gpu/drm/nouveau/core/include/engine/disp.h
index 7a64f347b385..fc307f1317ff 100644
--- a/drivers/gpu/drm/nouveau/core/include/engine/disp.h
+++ b/drivers/gpu/drm/nouveau/core/include/engine/disp.h
@@ -31,5 +31,6 @@ extern struct nouveau_oclass *nvd0_disp_oclass;
 extern struct nouveau_oclass *nve0_disp_oclass;
 extern struct nouveau_oclass *nvf0_disp_oclass;
 extern struct nouveau_oclass *gm107_disp_oclass;
+extern struct nouveau_oclass *gm204_disp_oclass;
 
 #endif
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/bios/M0203.h b/drivers/gpu/drm/nouveau/core/include/subdev/bios/M0203.h
new file mode 100644
index 000000000000..1f84d3612dd8
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/bios/M0203.h
@@ -0,0 +1,31 @@
+#ifndef __NVBIOS_M0203_H__
+#define __NVBIOS_M0203_H__
+
+struct nvbios_M0203T {
+#define M0203T_TYPE_RAMCFG 0x00
+	u8  type;
+	u16 pointer;
+};
+
+u32 nvbios_M0203Te(struct nouveau_bios *, u8 *ver, u8 *hdr, u8 *cnt, u8 *len);
+u32 nvbios_M0203Tp(struct nouveau_bios *, u8 *ver, u8 *hdr, u8 *cnt, u8 *len,
+		   struct nvbios_M0203T *);
+
+struct nvbios_M0203E {
+#define M0203E_TYPE_DDR2  0x0
+#define M0203E_TYPE_DDR3  0x1
+#define M0203E_TYPE_GDDR3 0x2
+#define M0203E_TYPE_GDDR5 0x3
+#define M0203E_TYPE_SKIP  0xf
+	u8 type;
+	u8 strap;
+	u8 group;
+};
+
+u32 nvbios_M0203Ee(struct nouveau_bios *, int idx, u8 *ver, u8 *hdr);
+u32 nvbios_M0203Ep(struct nouveau_bios *, int idx, u8 *ver, u8 *hdr,
+		   struct nvbios_M0203E *);
+u32 nvbios_M0203Em(struct nouveau_bios *, u8 ramcfg, u8 *ver, u8 *hdr,
+		   struct nvbios_M0203E *);
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/bios/i2c.h b/drivers/gpu/drm/nouveau/core/include/subdev/bios/i2c.h
index 10b57a19a7de..c9bb112895af 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/bios/i2c.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/bios/i2c.h
@@ -4,11 +4,14 @@
 struct nouveau_bios;
 
 enum dcb_i2c_type {
-	DCB_I2C_NV04_BIT = 0,
-	DCB_I2C_NV4E_BIT = 4,
-	DCB_I2C_NVIO_BIT = 5,
-	DCB_I2C_NVIO_AUX = 6,
-	DCB_I2C_UNUSED = 0xff
+	/* matches bios type field prior to ccb 4.1 */
+	DCB_I2C_NV04_BIT = 0x00,
+	DCB_I2C_NV4E_BIT = 0x04,
+	DCB_I2C_NVIO_BIT = 0x05,
+	DCB_I2C_NVIO_AUX = 0x06,
+	/* made up - mostly */
+	DCB_I2C_PMGR     = 0x80,
+	DCB_I2C_UNUSED   = 0xff
 };
 
 struct dcb_i2c_entry {
@@ -16,6 +19,7 @@ struct dcb_i2c_entry {
 	u8 drive;
 	u8 sense;
 	u8 share;
+	u8 auxch;
 };
 
 u16 dcb_i2c_table(struct nouveau_bios *, u8 *ver, u8 *hdr, u8 *cnt, u8 *len);
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/bios/image.h b/drivers/gpu/drm/nouveau/core/include/subdev/bios/image.h
new file mode 100644
index 000000000000..3348b4580843
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/bios/image.h
@@ -0,0 +1,13 @@
+#ifndef __NVBIOS_IMAGE_H__
+#define __NVBIOS_IMAGE_H__
+
+struct nvbios_image {
+	u32  base;
+	u32  size;
+	u8   type;
+	bool last;
+};
+
+bool nvbios_image(struct nouveau_bios *, int, struct nvbios_image *);
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/bios/npde.h b/drivers/gpu/drm/nouveau/core/include/subdev/bios/npde.h
new file mode 100644
index 000000000000..b18413d951e5
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/bios/npde.h
@@ -0,0 +1,12 @@
+#ifndef __NVBIOS_NPDE_H__
+#define __NVBIOS_NPDE_H__
+
+struct nvbios_npdeT {
+	u32 image_size;
+	bool last;
+};
+
+u32 nvbios_npdeTe(struct nouveau_bios *, u32);
+u32 nvbios_npdeTp(struct nouveau_bios *, u32, struct nvbios_npdeT *);
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/bios/pcir.h b/drivers/gpu/drm/nouveau/core/include/subdev/bios/pcir.h
new file mode 100644
index 000000000000..3d634a06dca1
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/bios/pcir.h
@@ -0,0 +1,18 @@
+#ifndef __NVBIOS_PCIR_H__
+#define __NVBIOS_PCIR_H__
+
+struct nvbios_pcirT {
+	u16 vendor_id;
+	u16 device_id;
+	u8  class_code[3];
+	u32 image_size;
+	u16 image_rev;
+	u8  image_type;
+	bool last;
+};
+
+u32 nvbios_pcirTe(struct nouveau_bios *, u32, u8 *ver, u16 *hdr);
+u32 nvbios_pcirTp(struct nouveau_bios *, u32, u8 *ver, u16 *hdr,
+		  struct nvbios_pcirT *);
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/bios/pmu.h b/drivers/gpu/drm/nouveau/core/include/subdev/bios/pmu.h
new file mode 100644
index 000000000000..9de593deaea8
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/bios/pmu.h
@@ -0,0 +1,37 @@
+#ifndef __NVBIOS_PMU_H__
+#define __NVBIOS_PMU_H__
+
+struct nvbios_pmuT {
+};
+
+u32 nvbios_pmuTe(struct nouveau_bios *, u8 *ver, u8 *hdr, u8 *cnt, u8 *len);
+u32 nvbios_pmuTp(struct nouveau_bios *, u8 *ver, u8 *hdr, u8 *cnt, u8 *len,
+		 struct nvbios_pmuT *);
+
+struct nvbios_pmuE {
+	u8  type;
+	u32 data;
+};
+
+u32 nvbios_pmuEe(struct nouveau_bios *, int idx, u8 *ver, u8 *hdr);
+u32 nvbios_pmuEp(struct nouveau_bios *, int idx, u8 *ver, u8 *hdr,
+		 struct nvbios_pmuE *);
+
+struct nvbios_pmuR {
+	u32 boot_addr_pmu;
+	u32 boot_addr;
+	u32 boot_size;
+	u32 code_addr_pmu;
+	u32 code_addr;
+	u32 code_size;
+	u32 init_addr_pmu;
+
+	u32 data_addr_pmu;
+	u32 data_addr;
+	u32 data_size;
+	u32 args_addr_pmu;
+};
+
+bool nvbios_pmuRm(struct nouveau_bios *, u8 type, struct nvbios_pmuR *);
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/bios/ramcfg.h b/drivers/gpu/drm/nouveau/core/include/subdev/bios/ramcfg.h
index a685bbd04568..4a0e0ceb41ba 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/bios/ramcfg.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/bios/ramcfg.h
@@ -43,8 +43,9 @@ struct nvbios_ramcfg {
 			unsigned ramcfg_10_02_08:1;
 			unsigned ramcfg_10_02_10:1;
 			unsigned ramcfg_10_02_20:1;
-			unsigned ramcfg_10_02_40:1;
+			unsigned ramcfg_10_DLLoff:1;
 			unsigned ramcfg_10_03_0f:4;
+			unsigned ramcfg_10_04_01:1;
 			unsigned ramcfg_10_05:8;
 			unsigned ramcfg_10_06:8;
 			unsigned ramcfg_10_07:8;
@@ -95,9 +96,29 @@ struct nvbios_ramcfg {
 	union {
 		struct {
 			unsigned timing_10_WR:8;
+			unsigned timing_10_WTR:8;
 			unsigned timing_10_CL:8;
+			unsigned timing_10_RC:8;
+			/*empty: 4 */
+			unsigned timing_10_RFC:8;        /* Byte 5 */
+			/*empty: 6 */
+			unsigned timing_10_RAS:8;        /* Byte 7 */
+			/*empty: 8 */
+			unsigned timing_10_RP:8;         /* Byte 9 */
+			unsigned timing_10_RCDRD:8;
+			unsigned timing_10_RCDWR:8;
+			unsigned timing_10_RRD:8;
+			unsigned timing_10_13:8;
 			unsigned timing_10_ODT:3;
+			/* empty: 15 */
+			unsigned timing_10_16:8;
+			/* empty: 17 */
+			unsigned timing_10_18:8;
 			unsigned timing_10_CWL:8;
+			unsigned timing_10_20:8;
+			unsigned timing_10_21:8;
+			/* empty: 22, 23 */
+			unsigned timing_10_24:8;
 		};
 		struct {
 			unsigned timing_20_2e_03:2;
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/devinit.h b/drivers/gpu/drm/nouveau/core/include/subdev/devinit.h
index e292271a84e4..e007a9d44683 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/devinit.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/devinit.h
@@ -30,5 +30,6 @@ extern struct nouveau_oclass *nva3_devinit_oclass;
 extern struct nouveau_oclass *nvaf_devinit_oclass;
 extern struct nouveau_oclass *nvc0_devinit_oclass;
 extern struct nouveau_oclass *gm107_devinit_oclass;
+extern struct nouveau_oclass *gm204_devinit_oclass;
 
 #endif
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/i2c.h b/drivers/gpu/drm/nouveau/core/include/subdev/i2c.h
index 1b937c2c25ae..d94ccacb40bf 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/i2c.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/i2c.h
@@ -8,6 +8,8 @@
 #include <subdev/bios/i2c.h>
 
 #define NV_I2C_PORT(n)    (0x00 + (n))
+#define NV_I2C_AUX(n)     (0x10 + (n))
+#define NV_I2C_EXT(n)     (0x20 + (n))
 #define NV_I2C_DEFAULT(n) (0x80 + (n))
 
 #define NV_I2C_TYPE_DCBI2C(n) (0x0000 | (n))
@@ -89,6 +91,7 @@ extern struct nouveau_oclass *nv94_i2c_oclass;
 extern struct nouveau_oclass *nvd0_i2c_oclass;
 extern struct nouveau_oclass *gf117_i2c_oclass;
 extern struct nouveau_oclass *nve0_i2c_oclass;
+extern struct nouveau_oclass *gm204_i2c_oclass;
 
 static inline int
 nv_rdi2cr(struct nouveau_i2c_port *port, u8 addr, u8 reg)
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/pwr.h b/drivers/gpu/drm/nouveau/core/include/subdev/pwr.h
index bf3d1f611333..f2427bf5aeed 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/pwr.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/pwr.h
@@ -48,6 +48,8 @@ void nouveau_memx_wait(struct nouveau_memx *,
 		       u32 addr, u32 mask, u32 data, u32 nsec);
 void nouveau_memx_nsec(struct nouveau_memx *, u32 nsec);
 void nouveau_memx_wait_vblank(struct nouveau_memx *);
+void nouveau_memx_train(struct nouveau_memx *);
+int  nouveau_memx_train_result(struct nouveau_pwr *, u32 *, int);
 void nouveau_memx_block(struct nouveau_memx *);
 void nouveau_memx_unblock(struct nouveau_memx *);
 
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/volt.h b/drivers/gpu/drm/nouveau/core/include/subdev/volt.h
index 820b62ffd75b..67db5e58880d 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/volt.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/volt.h
@@ -52,6 +52,7 @@ int  _nouveau_volt_init(struct nouveau_object *);
 #define _nouveau_volt_fini _nouveau_subdev_fini
 
 extern struct nouveau_oclass nv40_volt_oclass;
+extern struct nouveau_oclass gk20a_volt_oclass;
 
 int nouveau_voltgpio_init(struct nouveau_volt *);
 int nouveau_voltgpio_get(struct nouveau_volt *);
diff --git a/drivers/gpu/drm/nouveau/core/os.h b/drivers/gpu/drm/nouveau/core/os.h
index ccfa21d72ddc..bdd05ee7ec72 100644
--- a/drivers/gpu/drm/nouveau/core/os.h
+++ b/drivers/gpu/drm/nouveau/core/os.h
@@ -23,6 +23,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/power_supply.h>
 #include <linux/clk.h>
+#include <linux/regulator/consumer.h>
 
 #include <asm/unaligned.h>
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/M0203.c b/drivers/gpu/drm/nouveau/core/subdev/bios/M0203.c
new file mode 100644
index 000000000000..28906b16d4e5
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/M0203.c
@@ -0,0 +1,129 @@
+/*
+ * Copyright 2014 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include <subdev/bios.h>
+#include <subdev/bios/bit.h>
+#include <subdev/bios/M0203.h>
+
+u32
+nvbios_M0203Te(struct nouveau_bios *bios, u8 *ver, u8 *hdr, u8 *cnt, u8 *len)
+{
+	struct bit_entry bit_M;
+	u32 data = 0x00000000;
+
+	if (!bit_entry(bios, 'M', &bit_M)) {
+		if (bit_M.version == 2 && bit_M.length > 0x04)
+			data = nv_ro16(bios, bit_M.offset + 0x03);
+		if (data) {
+			*ver = nv_ro08(bios, data + 0x00);
+			switch (*ver) {
+			case 0x10:
+				*hdr = nv_ro08(bios, data + 0x01);
+				*len = nv_ro08(bios, data + 0x02);
+				*cnt = nv_ro08(bios, data + 0x03);
+				return data;
+			default:
+				break;
+			}
+		}
+	}
+
+	return 0x00000000;
+}
+
+u32
+nvbios_M0203Tp(struct nouveau_bios *bios, u8 *ver, u8 *hdr, u8 *cnt, u8 *len,
+	       struct nvbios_M0203T *info)
+{
+	u32 data = nvbios_M0203Te(bios, ver, hdr, cnt, len);
+	memset(info, 0x00, sizeof(*info));
+	switch (!!data * *ver) {
+	case 0x10:
+		info->type    = nv_ro08(bios, data + 0x04);
+		info->pointer = nv_ro16(bios, data + 0x05);
+		break;
+	default:
+		break;
+	}
+	return data;
+}
+
+u32
+nvbios_M0203Ee(struct nouveau_bios *bios, int idx, u8 *ver, u8 *hdr)
+{
+	u8  cnt, len;
+	u32 data = nvbios_M0203Te(bios, ver, hdr, &cnt, &len);
+	if (data && idx < cnt) {
+		data = data + *hdr + idx * len;
+		*hdr = len;
+		return data;
+	}
+	return 0x00000000;
+}
+
+u32
+nvbios_M0203Ep(struct nouveau_bios *bios, int idx, u8 *ver, u8 *hdr,
+	       struct nvbios_M0203E *info)
+{
+	u32 data = nvbios_M0203Ee(bios, idx, ver, hdr);
+	memset(info, 0x00, sizeof(*info));
+	switch (!!data * *ver) {
+	case 0x10:
+		info->type  = (nv_ro08(bios, data + 0x00) & 0x0f) >> 0;
+		info->strap = (nv_ro08(bios, data + 0x00) & 0xf0) >> 4;
+		info->group = (nv_ro08(bios, data + 0x01) & 0x0f) >> 0;
+		return data;
+	default:
+		break;
+	}
+	return 0x00000000;
+}
+
+u32
+nvbios_M0203Em(struct nouveau_bios *bios, u8 ramcfg, u8 *ver, u8 *hdr,
+	       struct nvbios_M0203E *info)
+{
+	struct nvbios_M0203T M0203T;
+	u8  cnt, len, idx = 0xff;
+	u32 data;
+
+	if (!nvbios_M0203Tp(bios, ver, hdr, &cnt, &len, &M0203T)) {
+		nv_warn(bios, "M0203T not found\n");
+		return 0x00000000;
+	}
+
+	while ((data = nvbios_M0203Ep(bios, ++idx, ver, hdr, info))) {
+		switch (M0203T.type) {
+		case M0203T_TYPE_RAMCFG:
+			if (info->strap != ramcfg)
+				continue;
+			return data;
+		default:
+			nv_warn(bios, "M0203T type %02x\n", M0203T.type);
+			return 0x00000000;
+		}
+	}
+
+	return data;
+}
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/base.c b/drivers/gpu/drm/nouveau/core/subdev/bios/base.c
index d45704a2c2df..7df3a273553d 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/base.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/base.c
@@ -31,6 +31,8 @@
 #include <subdev/bios/bmp.h>
 #include <subdev/bios/bit.h>
 
+#include "priv.h"
+
 u8
 nvbios_checksum(const u8 *data, int size)
 {
@@ -56,362 +58,21 @@ nvbios_findstr(const u8 *data, int size, const char *str, int len)
 	return 0;
 }
 
-#if defined(__powerpc__)
-static void
-nouveau_bios_shadow_of(struct nouveau_bios *bios)
+int
+nvbios_extend(struct nouveau_bios *bios, u32 length)
 {
-	struct pci_dev *pdev = nv_device(bios)->pdev;
-	struct device_node *dn;
-	const u32 *data;
-	int size;
-
-	dn = pci_device_to_OF_node(pdev);
-	if (!dn) {
-		nv_info(bios, "Unable to get the OF node\n");
-		return;
-	}
-
-	data = of_get_property(dn, "NVDA,BMP", &size);
-	if (data && size) {
-		bios->size = size;
-		bios->data = kmalloc(bios->size, GFP_KERNEL);
-		if (bios->data)
-			memcpy(bios->data, data, size);
-	}
-}
-#endif
-
-static void
-nouveau_bios_shadow_pramin(struct nouveau_bios *bios)
-{
-	struct nouveau_device *device = nv_device(bios);
-	u64 addr = 0;
-	u32 bar0 = 0;
-	int i;
-
-	if (device->card_type >= NV_50) {
-		if (device->card_type >= NV_C0 && device->card_type < GM100) {
-			if (nv_rd32(bios, 0x022500) & 0x00000001)
-				return;
-		} else
-		if (device->card_type >= GM100) {
-			if (nv_rd32(bios, 0x021c04) & 0x00000001)
-				return;
-		}
-
-		addr = nv_rd32(bios, 0x619f04);
-		if (!(addr & 0x00000008)) {
-			nv_debug(bios, "... not enabled\n");
-			return;
+	if (bios->size < length) {
+		u8 *prev = bios->data;
+		if (!(bios->data = kmalloc(length, GFP_KERNEL))) {
+			bios->data = prev;
+			return -ENOMEM;
 		}
-		if ( (addr & 0x00000003) != 1) {
-			nv_debug(bios, "... not in vram\n");
-			return;
-		}
-
-		addr = (addr & 0xffffff00) << 8;
-		if (!addr) {
-			addr  = (u64)nv_rd32(bios, 0x001700) << 16;
-			addr += 0xf0000;
-		}
-
-		bar0 = nv_mask(bios, 0x001700, 0xffffffff, addr >> 16);
-	}
-
-	/* bail if no rom signature */
-	if (nv_rd08(bios, 0x700000) != 0x55 ||
-	    nv_rd08(bios, 0x700001) != 0xaa)
-		goto out;
-
-	bios->size = nv_rd08(bios, 0x700002) * 512;
-	if (!bios->size)
-		goto out;
-
-	bios->data = kmalloc(bios->size, GFP_KERNEL);
-	if (bios->data) {
-		for (i = 0; i < bios->size; i++)
-			nv_wo08(bios, i, nv_rd08(bios, 0x700000 + i));
-	}
-
-out:
-	if (device->card_type >= NV_50)
-		nv_wr32(bios, 0x001700, bar0);
-}
-
-static void
-nouveau_bios_shadow_prom(struct nouveau_bios *bios)
-{
-	struct nouveau_device *device = nv_device(bios);
-	u32 pcireg, access;
-	u16 pcir;
-	int i;
-
-	/* there is no prom on nv4x IGP's */
-	if (device->card_type == NV_40 && device->chipset >= 0x4c)
-		return;
-
-	/* enable access to rom */
-	if (device->card_type >= NV_50)
-		pcireg = 0x088050;
-	else
-		pcireg = 0x001850;
-	access = nv_mask(bios, pcireg, 0x00000001, 0x00000000);
-
-	/* WARNING: PROM accesses should always be 32-bits aligned. Other
-	 * accesses work on most chipset but do not on Kepler chipsets
-	 */
-
-	/* bail if no rom signature, with a workaround for a PROM reading
-	 * issue on some chipsets.  the first read after a period of
-	 * inactivity returns the wrong result, so retry the first header
-	 * byte a few times before giving up as a workaround
-	 */
-	i = 16;
-	do {
-		u32 data = le32_to_cpu(nv_rd32(bios, 0x300000)) & 0xffff;
-		if (data == 0xaa55)
-			break;
-	} while (i--);
-
-	if (!i)
-		goto out;
-
-	/* read entire bios image to system memory */
-	bios->size = (le32_to_cpu(nv_rd32(bios, 0x300000)) >> 16) & 0xff;
-	bios->size = bios->size * 512;
-	if (!bios->size)
-		goto out;
-
-	bios->data = kmalloc(bios->size, GFP_KERNEL);
-	if (!bios->data)
-		goto out;
-
-	for (i = 0; i < bios->size; i += 4)
-		((u32 *)bios->data)[i/4] = nv_rd32(bios, 0x300000 + i);
-
-	/* check the PCI record header */
-	pcir = nv_ro16(bios, 0x0018);
-	if (bios->data[pcir + 0] != 'P' ||
-	    bios->data[pcir + 1] != 'C' ||
-	    bios->data[pcir + 2] != 'I' ||
-	    bios->data[pcir + 3] != 'R') {
-		bios->size = 0;
-		kfree(bios->data);
-	}
-
-out:
-	/* disable access to rom */
-	nv_wr32(bios, pcireg, access);
-}
-
-#if defined(CONFIG_ACPI) && defined(CONFIG_X86)
-int nouveau_acpi_get_bios_chunk(uint8_t *bios, int offset, int len);
-bool nouveau_acpi_rom_supported(struct pci_dev *pdev);
-#else
-static inline bool
-nouveau_acpi_rom_supported(struct pci_dev *pdev) {
-	return false;
-}
-
-static inline int
-nouveau_acpi_get_bios_chunk(uint8_t *bios, int offset, int len) {
-	return -EINVAL;
-}
-#endif
-
-static void
-nouveau_bios_shadow_acpi(struct nouveau_bios *bios)
-{
-	struct pci_dev *pdev = nv_device(bios)->pdev;
-	int ret, cnt, i;
-
-	if (!nouveau_acpi_rom_supported(pdev)) {
-		bios->data = NULL;
-		return;
-	}
-
-	bios->size = 0;
-	bios->data = kmalloc(4096, GFP_KERNEL);
-	if (bios->data) {
-		if (nouveau_acpi_get_bios_chunk(bios->data, 0, 4096) == 4096)
-			bios->size = bios->data[2] * 512;
-		kfree(bios->data);
+		memcpy(bios->data, prev, bios->size);
+		bios->size = length;
+		kfree(prev);
+		return 1;
 	}
-
-	if (!bios->size)
-		return;
-
-	bios->data = kmalloc(bios->size, GFP_KERNEL);
-	if (bios->data) {
-		/* disobey the acpi spec - much faster on at least w530 ... */
-		ret = nouveau_acpi_get_bios_chunk(bios->data, 0, bios->size);
-		if (ret != bios->size ||
-		    nvbios_checksum(bios->data, bios->size)) {
-			/* ... that didn't work, ok, i'll be good now */
-			for (i = 0; i < bios->size; i += cnt) {
-				cnt = min((bios->size - i), (u32)4096);
-				ret = nouveau_acpi_get_bios_chunk(bios->data, i, cnt);
-				if (ret != cnt)
-					break;
-			}
-		}
-	}
-}
-
-static void
-nouveau_bios_shadow_pci(struct nouveau_bios *bios)
-{
-	struct pci_dev *pdev = nv_device(bios)->pdev;
-	size_t size;
-
-	if (!pci_enable_rom(pdev)) {
-		void __iomem *rom = pci_map_rom(pdev, &size);
-		if (rom && size) {
-			bios->data = kmalloc(size, GFP_KERNEL);
-			if (bios->data) {
-				memcpy_fromio(bios->data, rom, size);
-				bios->size = size;
-			}
-		}
-		if (rom)
-			pci_unmap_rom(pdev, rom);
-
-		pci_disable_rom(pdev);
-	}
-}
-
-static void
-nouveau_bios_shadow_platform(struct nouveau_bios *bios)
-{
-	struct pci_dev *pdev = nv_device(bios)->pdev;
-	size_t size;
-
-	void __iomem *rom = pci_platform_rom(pdev, &size);
-	if (rom && size) {
-		bios->data = kmalloc(size, GFP_KERNEL);
-		if (bios->data) {
-			memcpy_fromio(bios->data, rom, size);
-			bios->size = size;
-		}
-	}
-}
-
-static int
-nouveau_bios_score(struct nouveau_bios *bios, const bool writeable)
-{
-	if (bios->size < 3 || !bios->data || bios->data[0] != 0x55 ||
-			bios->data[1] != 0xAA) {
-		nv_info(bios, "... signature not found\n");
-		return 0;
-	}
-
-	if (nvbios_checksum(bios->data,
-			min_t(u32, bios->data[2] * 512, bios->size))) {
-		nv_info(bios, "... checksum invalid\n");
-		/* if a ro image is somewhat bad, it's probably all rubbish */
-		return writeable ? 2 : 1;
-	}
-
-	nv_info(bios, "... appears to be valid\n");
-	return 3;
-}
-
-struct methods {
-	const char desc[16];
-	void (*shadow)(struct nouveau_bios *);
-	const bool rw;
-	int score;
-	u32 size;
-	u8 *data;
-};
-
-static int
-nouveau_bios_shadow(struct nouveau_bios *bios)
-{
-	struct methods shadow_methods[] = {
-#if defined(__powerpc__)
-		{ "OpenFirmware", nouveau_bios_shadow_of, true, 0, 0, NULL },
-#endif
-		{ "PRAMIN", nouveau_bios_shadow_pramin, true, 0, 0, NULL },
-		{ "PROM", nouveau_bios_shadow_prom, false, 0, 0, NULL },
-		{ "ACPI", nouveau_bios_shadow_acpi, true, 0, 0, NULL },
-		{ "PCIROM", nouveau_bios_shadow_pci, true, 0, 0, NULL },
-		{ "PLATFORM", nouveau_bios_shadow_platform, true, 0, 0, NULL },
-		{}
-	};
-	struct methods *mthd, *best;
-	const struct firmware *fw;
-	const char *optarg;
-	int optlen, ret;
-	char *source;
-
-	optarg = nouveau_stropt(nv_device(bios)->cfgopt, "NvBios", &optlen);
-	source = optarg ? kstrndup(optarg, optlen, GFP_KERNEL) : NULL;
-	if (source) {
-		/* try to match one of the built-in methods */
-		mthd = shadow_methods;
-		do {
-			if (strcasecmp(source, mthd->desc))
-				continue;
-			nv_info(bios, "source: %s\n", mthd->desc);
-
-			mthd->shadow(bios);
-			mthd->score = nouveau_bios_score(bios, mthd->rw);
-			if (mthd->score) {
-				kfree(source);
-				return 0;
-			}
-		} while ((++mthd)->shadow);
-
-		/* attempt to load firmware image */
-		ret = request_firmware(&fw, source, &nv_device(bios)->pdev->dev);
-		if (ret == 0) {
-			bios->size = fw->size;
-			bios->data = kmemdup(fw->data, fw->size, GFP_KERNEL);
-			release_firmware(fw);
-
-			nv_info(bios, "image: %s\n", source);
-			if (nouveau_bios_score(bios, 1)) {
-				kfree(source);
-				return 0;
-			}
-
-			kfree(bios->data);
-			bios->data = NULL;
-		}
-
-		nv_error(bios, "source \'%s\' invalid\n", source);
-		kfree(source);
-	}
-
-	mthd = shadow_methods;
-	do {
-		nv_info(bios, "checking %s for image...\n", mthd->desc);
-		mthd->shadow(bios);
-		mthd->score = nouveau_bios_score(bios, mthd->rw);
-		mthd->size = bios->size;
-		mthd->data = bios->data;
-		bios->data = NULL;
-	} while (mthd->score != 3 && (++mthd)->shadow);
-
-	mthd = shadow_methods;
-	best = mthd;
-	do {
-		if (mthd->score > best->score) {
-			kfree(best->data);
-			best = mthd;
-		}
-	} while ((++mthd)->shadow);
-
-	if (best->score) {
-		nv_info(bios, "using image from %s\n", best->desc);
-		bios->size = best->size;
-		bios->data = best->data;
-		return 0;
-	}
-
-	nv_error(bios, "unable to locate usable image\n");
-	return -EINVAL;
+	return 0;
 }
 
 static u8
@@ -472,7 +133,7 @@ nouveau_bios_ctor(struct nouveau_object *parent,
 	if (ret)
 		return ret;
 
-	ret = nouveau_bios_shadow(bios);
+	ret = nvbios_shadow(bios);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/dcb.c b/drivers/gpu/drm/nouveau/core/subdev/bios/dcb.c
index bd8d348385b3..96099aff8b41 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/dcb.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/dcb.c
@@ -42,7 +42,7 @@ dcb_table(struct nouveau_bios *bios, u8 *ver, u8 *hdr, u8 *cnt, u8 *len)
 
 	*ver = nv_ro08(bios, dcb);
 
-	if (*ver >= 0x41) {
+	if (*ver >= 0x42) {
 		nv_warn(bios, "DCB version 0x%02x unknown\n", *ver);
 		return 0x0000;
 	} else
@@ -157,17 +157,20 @@ dcb_outp_parse(struct nouveau_bios *bios, u8 idx, u8 *ver, u8 *len,
 					break;
 				}
 
-				switch (conf & 0x0f000000) {
-				case 0x0f000000:
-					outp->dpconf.link_nr = 4;
-					break;
-				case 0x03000000:
-					outp->dpconf.link_nr = 2;
-					break;
-				case 0x01000000:
-				default:
-					outp->dpconf.link_nr = 1;
-					break;
+				outp->dpconf.link_nr = (conf & 0x0f000000) >> 24;
+				if (*ver < 0x41) {
+					switch (outp->dpconf.link_nr) {
+					case 0x0f:
+						outp->dpconf.link_nr = 4;
+						break;
+					case 0x03:
+						outp->dpconf.link_nr = 2;
+						break;
+					case 0x01:
+					default:
+						outp->dpconf.link_nr = 1;
+						break;
+					}
 				}
 
 				/* fall-through... */
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/disp.c b/drivers/gpu/drm/nouveau/core/subdev/bios/disp.c
index 7f16e52d9bea..51f355599694 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/disp.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/disp.c
@@ -40,6 +40,7 @@ nvbios_disp_table(struct nouveau_bios *bios,
 				switch (*ver) {
 				case 0x20:
 				case 0x21:
+				case 0x22:
 					*hdr = nv_ro08(bios, data + 0x01);
 					*len = nv_ro08(bios, data + 0x02);
 					*cnt = nv_ro08(bios, data + 0x03);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/dp.c b/drivers/gpu/drm/nouveau/core/subdev/bios/dp.c
index f309dd657250..cef53f81f12b 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/dp.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/dp.c
@@ -41,6 +41,7 @@ nvbios_dp_table(struct nouveau_bios *bios, u8 *ver, u8 *hdr, u8 *cnt, u8 *len)
 				case 0x21:
 				case 0x30:
 				case 0x40:
+				case 0x41:
 					*hdr = nv_ro08(bios, data + 0x01);
 					*len = nv_ro08(bios, data + 0x02);
 					*cnt = nv_ro08(bios, data + 0x03);
@@ -70,6 +71,7 @@ nvbios_dpout_entry(struct nouveau_bios *bios, u8 idx,
 			*cnt = nv_ro08(bios, outp + 0x04);
 			break;
 		case 0x40:
+		case 0x41:
 			*hdr = nv_ro08(bios, data + 0x04);
 			*cnt = 0;
 			*len = 0;
@@ -108,6 +110,7 @@ nvbios_dpout_parse(struct nouveau_bios *bios, u8 idx,
 				info->script[4] = nv_ro16(bios, data + 0x10);
 			break;
 		case 0x40:
+		case 0x41:
 			info->flags     = nv_ro08(bios, data + 0x04);
 			info->script[0] = nv_ro16(bios, data + 0x05);
 			info->script[1] = nv_ro16(bios, data + 0x07);
@@ -172,10 +175,11 @@ nvbios_dpcfg_parse(struct nouveau_bios *bios, u16 outp, u8 idx,
 			break;
 		case 0x30:
 		case 0x40:
+		case 0x41:
 			info->pc    = nv_ro08(bios, data + 0x00);
 			info->dc    = nv_ro08(bios, data + 0x01);
 			info->pe    = nv_ro08(bios, data + 0x02);
-			info->tx_pu = nv_ro08(bios, data + 0x03);
+			info->tx_pu = nv_ro08(bios, data + 0x03) & 0x0f;
 			break;
 		default:
 			data = 0x0000;
@@ -194,6 +198,10 @@ nvbios_dpcfg_match(struct nouveau_bios *bios, u16 outp, u8 pc, u8 vs, u8 pe,
 	u16 data;
 
 	if (*ver >= 0x30) {
+		/*XXX: there's a second set of these on at least 4.1, that
+		 *     i've witnessed nvidia using instead of the first
+		 *     on gm204.  figure out what/why
+		 */
 		const u8 vsoff[] = { 0, 4, 7, 9 };
 		idx = (pc * 10) + vsoff[vs] + pe;
 	} else {
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/extdev.c b/drivers/gpu/drm/nouveau/core/subdev/bios/extdev.c
index b2a676e53580..49285d4f7ca5 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/extdev.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/extdev.c
@@ -90,7 +90,7 @@ nvbios_extdev_find(struct nouveau_bios *bios, enum nvbios_extdev_type type,
 	u16 entry;
 
 	i = 0;
-	while (!(entry = nvbios_extdev_entry(bios, i++, &ver, &len))) {
+	while ((entry = nvbios_extdev_entry(bios, i++, &ver, &len))) {
 		extdev_parse_entry(bios, entry, func);
 		if (func->type == type)
 			return 0;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/i2c.c b/drivers/gpu/drm/nouveau/core/subdev/bios/i2c.c
index cfb9288c6d28..282320ba9264 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/i2c.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/i2c.c
@@ -39,6 +39,11 @@ dcb_i2c_table(struct nouveau_bios *bios, u8 *ver, u8 *hdr, u8 *cnt, u8 *len)
 			i2c = nv_ro16(bios, dcb + 4);
 	}
 
+	if (i2c && *ver >= 0x42) {
+		nv_warn(bios, "ccb %02x not supported\n", *ver);
+		return 0x0000;
+	}
+
 	if (i2c && *ver >= 0x30) {
 		*ver = nv_ro08(bios, i2c + 0);
 		*hdr = nv_ro08(bios, i2c + 1);
@@ -70,14 +75,25 @@ dcb_i2c_parse(struct nouveau_bios *bios, u8 idx, struct dcb_i2c_entry *info)
 	u8  ver, len;
 	u16 ent = dcb_i2c_entry(bios, idx, &ver, &len);
 	if (ent) {
-		info->type  = nv_ro08(bios, ent + 3);
-		info->share = DCB_I2C_UNUSED;
-		if (ver < 0x30) {
-			info->type &= 0x07;
+		if (ver >= 0x41) {
+			if (!(nv_ro32(bios, ent) & 0x80000000))
+				info->type = DCB_I2C_UNUSED;
+			else
+				info->type = DCB_I2C_PMGR;
+		} else
+		if (ver >= 0x30) {
+			info->type = nv_ro08(bios, ent + 0x03);
+		} else {
+			info->type = nv_ro08(bios, ent + 0x03) & 0x07;
 			if (info->type == 0x07)
 				info->type = DCB_I2C_UNUSED;
 		}
 
+		info->drive = DCB_I2C_UNUSED;
+		info->sense = DCB_I2C_UNUSED;
+		info->share = DCB_I2C_UNUSED;
+		info->auxch = DCB_I2C_UNUSED;
+
 		switch (info->type) {
 		case DCB_I2C_NV04_BIT:
 			info->drive = nv_ro08(bios, ent + 0);
@@ -87,12 +103,23 @@ dcb_i2c_parse(struct nouveau_bios *bios, u8 idx, struct dcb_i2c_entry *info)
 			info->drive = nv_ro08(bios, ent + 1);
 			return 0;
 		case DCB_I2C_NVIO_BIT:
-		case DCB_I2C_NVIO_AUX:
 			info->drive = nv_ro08(bios, ent + 0) & 0x0f;
-			if (nv_ro08(bios, ent + 1) & 0x01) {
-				info->share  = nv_ro08(bios, ent + 1) >> 1;
-				info->share &= 0x0f;
-			}
+			if (nv_ro08(bios, ent + 1) & 0x01)
+				info->share = nv_ro08(bios, ent + 1) >> 1;
+			return 0;
+		case DCB_I2C_NVIO_AUX:
+			info->auxch = nv_ro08(bios, ent + 0) & 0x0f;
+			if (nv_ro08(bios, ent + 1) & 0x01)
+					info->share = info->auxch;
+			return 0;
+		case DCB_I2C_PMGR:
+			info->drive = (nv_ro16(bios, ent + 0) & 0x01f) >> 0;
+			if (info->drive == 0x1f)
+				info->drive = DCB_I2C_UNUSED;
+			info->auxch = (nv_ro16(bios, ent + 0) & 0x3e0) >> 5;
+			if (info->auxch == 0x1f)
+				info->auxch = DCB_I2C_UNUSED;
+			info->share = info->auxch;
 			return 0;
 		case DCB_I2C_UNUSED:
 			return 0;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/image.c b/drivers/gpu/drm/nouveau/core/subdev/bios/image.c
new file mode 100644
index 000000000000..373f9a564ac9
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/image.c
@@ -0,0 +1,78 @@
+/*
+ * Copyright 2014 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 <subdev/bios.h>
+#include <subdev/bios/image.h>
+#include <subdev/bios/pcir.h>
+#include <subdev/bios/npde.h>
+
+static bool
+nvbios_imagen(struct nouveau_bios *bios, struct nvbios_image *image)
+{
+	struct nvbios_pcirT pcir;
+	struct nvbios_npdeT npde;
+	u8  ver;
+	u16 hdr;
+	u32 data;
+
+	switch ((data = nv_ro16(bios, image->base + 0x00))) {
+	case 0xaa55:
+	case 0xbb77:
+	case 0x4e56: /* NV */
+		break;
+	default:
+		nv_debug(bios, "%08x: ROM signature (%04x) unknown\n",
+			 image->base, data);
+		return false;
+	}
+
+	if (!(data = nvbios_pcirTp(bios, image->base, &ver, &hdr, &pcir)))
+		return false;
+	image->size = pcir.image_size;
+	image->type = pcir.image_type;
+	image->last = pcir.last;
+
+	if (image->type != 0x70) {
+		if (!(data = nvbios_npdeTp(bios, image->base, &npde)))
+			return true;
+		image->size = npde.image_size;
+		image->last = npde.last;
+	} else {
+		image->last = true;
+	}
+
+	return true;
+}
+
+bool
+nvbios_image(struct nouveau_bios *bios, int idx, struct nvbios_image *image)
+{
+	memset(image, 0x00, sizeof(*image));
+	do {
+		image->base += image->size;
+		if (image->last || !nvbios_imagen(bios, image))
+			return false;
+	} while(idx--);
+	return true;
+}
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/init.c b/drivers/gpu/drm/nouveau/core/subdev/bios/init.c
index 626380f9e4c0..c6579ef32cd1 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/init.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/init.c
@@ -255,6 +255,8 @@ init_i2c(struct nvbios_init *init, int index)
 		}
 
 		index = init->outp->i2c_index;
+		if (init->outp->type == DCB_OUTPUT_DP)
+			index += NV_I2C_AUX(0);
 	}
 
 	return i2c->find(i2c, index);
@@ -278,7 +280,7 @@ init_wri2cr(struct nvbios_init *init, u8 index, u8 addr, u8 reg, u8 val)
 	return -ENODEV;
 }
 
-static int
+static u8
 init_rdauxr(struct nvbios_init *init, u32 addr)
 {
 	struct nouveau_i2c_port *port = init_i2c(init, -2);
@@ -286,20 +288,24 @@ init_rdauxr(struct nvbios_init *init, u32 addr)
 
 	if (port && init_exec(init)) {
 		int ret = nv_rdaux(port, addr, &data, 1);
-		if (ret)
-			return ret;
-		return data;
+		if (ret == 0)
+			return data;
+		trace("auxch read failed with %d\n", ret);
 	}
 
-	return -ENODEV;
+	return 0x00;
 }
 
 static int
 init_wrauxr(struct nvbios_init *init, u32 addr, u8 data)
 {
 	struct nouveau_i2c_port *port = init_i2c(init, -2);
-	if (port && init_exec(init))
-		return nv_wraux(port, addr, &data, 1);
+	if (port && init_exec(init)) {
+		int ret = nv_wraux(port, addr, &data, 1);
+		if (ret)
+			trace("auxch write failed with %d\n", ret);
+		return ret;
+	}
 	return -ENODEV;
 }
 
@@ -838,6 +844,40 @@ init_io_or(struct nvbios_init *init)
 }
 
 /**
+ * INIT_ANDN_REG - opcode 0x47
+ *
+ */
+static void
+init_andn_reg(struct nvbios_init *init)
+{
+	struct nouveau_bios *bios = init->bios;
+	u32  reg = nv_ro32(bios, init->offset + 1);
+	u32 mask = nv_ro32(bios, init->offset + 5);
+
+	trace("ANDN_REG\tR[0x%06x] &= ~0x%08x\n", reg, mask);
+	init->offset += 9;
+
+	init_mask(init, reg, mask, 0);
+}
+
+/**
+ * INIT_OR_REG - opcode 0x48
+ *
+ */
+static void
+init_or_reg(struct nvbios_init *init)
+{
+	struct nouveau_bios *bios = init->bios;
+	u32  reg = nv_ro32(bios, init->offset + 1);
+	u32 mask = nv_ro32(bios, init->offset + 5);
+
+	trace("OR_REG\tR[0x%06x] |= 0x%08x\n", reg, mask);
+	init->offset += 9;
+
+	init_mask(init, reg, 0, mask);
+}
+
+/**
  * INIT_INDEX_ADDRESS_LATCHED - opcode 0x49
  *
  */
@@ -2068,6 +2108,8 @@ static struct nvbios_init_opcode {
 	[0x3a] = { init_dp_condition },
 	[0x3b] = { init_io_mask_or },
 	[0x3c] = { init_io_or },
+	[0x47] = { init_andn_reg },
+	[0x48] = { init_or_reg },
 	[0x49] = { init_idx_addr_latched },
 	[0x4a] = { init_io_restrict_pll2 },
 	[0x4b] = { init_pll2 },
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/npde.c b/drivers/gpu/drm/nouveau/core/subdev/bios/npde.c
new file mode 100644
index 000000000000..d694716a166c
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/npde.c
@@ -0,0 +1,59 @@
+/*
+ * Copyright 2014 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 <subdev/bios.h>
+#include <subdev/bios/npde.h>
+#include <subdev/bios/pcir.h>
+
+u32
+nvbios_npdeTe(struct nouveau_bios *bios, u32 base)
+{
+	struct nvbios_pcirT pcir;
+	u8  ver; u16 hdr;
+	u32 data = nvbios_pcirTp(bios, base, &ver, &hdr, &pcir);
+	if (data = (data + hdr + 0x0f) & ~0x0f, data) {
+		switch (nv_ro32(bios, data + 0x00)) {
+		case 0x4544504e: /* NPDE */
+			break;
+		default:
+			nv_debug(bios, "%08x: NPDE signature (%08x) unknown\n",
+				 data, nv_ro32(bios, data + 0x00));
+			data = 0;
+			break;
+		}
+	}
+	return data;
+}
+
+u32
+nvbios_npdeTp(struct nouveau_bios *bios, u32 base, struct nvbios_npdeT *info)
+{
+	u32 data = nvbios_npdeTe(bios, base);
+	memset(info, 0x00, sizeof(*info));
+	if (data) {
+		info->image_size = nv_ro16(bios, data + 0x08) * 512;
+		info->last = nv_ro08(bios, data + 0x0a) & 0x80;
+	}
+	return data;
+}
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/pcir.c b/drivers/gpu/drm/nouveau/core/subdev/bios/pcir.c
new file mode 100644
index 000000000000..91dae26bc50f
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/pcir.c
@@ -0,0 +1,69 @@
+/*
+ * Copyright 2014 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 <subdev/bios.h>
+#include <subdev/bios/pcir.h>
+
+u32
+nvbios_pcirTe(struct nouveau_bios *bios, u32 base, u8 *ver, u16 *hdr)
+{
+	u32 data = nv_ro16(bios, base + 0x18);
+	if (data) {
+		data += base;
+		switch (nv_ro32(bios, data + 0x00)) {
+		case 0x52494350: /* PCIR */
+		case 0x53494752: /* RGIS */
+		case 0x5344504e: /* NPDS */
+			*hdr = nv_ro16(bios, data + 0x0a);
+			*ver = nv_ro08(bios, data + 0x0c);
+			break;
+		default:
+			nv_debug(bios, "%08x: PCIR signature (%08x) unknown\n",
+				 data, nv_ro32(bios, data + 0x00));
+			data = 0;
+			break;
+		}
+	}
+	return data;
+}
+
+u32
+nvbios_pcirTp(struct nouveau_bios *bios, u32 base, u8 *ver, u16 *hdr,
+	      struct nvbios_pcirT *info)
+{
+	u32 data = nvbios_pcirTe(bios, base, ver, hdr);
+	memset(info, 0x00, sizeof(*info));
+	if (data) {
+		info->vendor_id = nv_ro16(bios, data + 0x04);
+		info->device_id = nv_ro16(bios, data + 0x06);
+		info->class_code[0] = nv_ro08(bios, data + 0x0d);
+		info->class_code[1] = nv_ro08(bios, data + 0x0e);
+		info->class_code[2] = nv_ro08(bios, data + 0x0f);
+		info->image_size = nv_ro16(bios, data + 0x10) * 512;
+		info->image_rev = nv_ro16(bios, data + 0x12);
+		info->image_type = nv_ro08(bios, data + 0x14);
+		info->last = nv_ro08(bios, data + 0x15) & 0x80;
+	}
+	return data;
+}
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/pmu.c b/drivers/gpu/drm/nouveau/core/subdev/bios/pmu.c
new file mode 100644
index 000000000000..66c56ba07d1b
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/pmu.c
@@ -0,0 +1,135 @@
+/*
+ * Copyright 2014 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 <subdev/bios.h>
+#include <subdev/bios/bit.h>
+#include <subdev/bios/image.h>
+#include <subdev/bios/pmu.h>
+
+static u32
+weirdo_pointer(struct nouveau_bios *bios, u32 data)
+{
+	struct nvbios_image image;
+	int idx = 0;
+	if (nvbios_image(bios, idx++, &image)) {
+		data -= image.size;
+		while (nvbios_image(bios, idx++, &image)) {
+			if (image.type == 0xe0)
+				return image.base + data;
+		}
+	}
+	return 0;
+}
+
+u32
+nvbios_pmuTe(struct nouveau_bios *bios, u8 *ver, u8 *hdr, u8 *cnt, u8 *len)
+{
+	struct bit_entry bit_p;
+	u32 data = 0;
+
+	if (!bit_entry(bios, 'p', &bit_p)) {
+		if (bit_p.version == 2 && bit_p.length >= 4)
+			data = nv_ro32(bios, bit_p.offset + 0x00);
+		if ((data = weirdo_pointer(bios, data))) {
+			*ver = nv_ro08(bios, data + 0x00); /* maybe? */
+			*hdr = nv_ro08(bios, data + 0x01);
+			*len = nv_ro08(bios, data + 0x02);
+			*cnt = nv_ro08(bios, data + 0x03);
+		}
+	}
+
+	return data;
+}
+
+u32
+nvbios_pmuTp(struct nouveau_bios *bios, u8 *ver, u8 *hdr, u8 *cnt, u8 *len,
+	     struct nvbios_pmuT *info)
+{
+	u32 data = nvbios_pmuTe(bios, ver, hdr, cnt, len);
+	memset(info, 0x00, sizeof(*info));
+	switch (!!data * *ver) {
+	default:
+		break;
+	}
+	return data;
+}
+
+u32
+nvbios_pmuEe(struct nouveau_bios *bios, int idx, u8 *ver, u8 *hdr)
+{
+	u8  cnt, len;
+	u32 data = nvbios_pmuTe(bios, ver, hdr, &cnt, &len);
+	if (data && idx < cnt) {
+		data = data + *hdr + (idx * len);
+		*hdr = len;
+		return data;
+	}
+	return 0;
+}
+
+u32
+nvbios_pmuEp(struct nouveau_bios *bios, int idx, u8 *ver, u8 *hdr,
+	     struct nvbios_pmuE *info)
+{
+	u32 data = nvbios_pmuEe(bios, idx, ver, hdr);
+	memset(info, 0x00, sizeof(*info));
+	switch (!!data * *ver) {
+	default:
+		info->type = nv_ro08(bios, data + 0x00);
+		info->data = nv_ro32(bios, data + 0x02);
+		break;
+	}
+	return data;
+}
+
+bool
+nvbios_pmuRm(struct nouveau_bios *bios, u8 type, struct nvbios_pmuR *info)
+{
+	struct nvbios_pmuE pmuE;
+	u8  ver, hdr, idx = 0;
+	u32 data;
+	memset(info, 0x00, sizeof(*info));
+	while ((data = nvbios_pmuEp(bios, idx++, &ver, &hdr, &pmuE))) {
+		if ( pmuE.type == type &&
+		    (data = weirdo_pointer(bios, pmuE.data))) {
+			info->init_addr_pmu = nv_ro32(bios, data + 0x08);
+			info->args_addr_pmu = nv_ro32(bios, data + 0x0c);
+			info->boot_addr     = data + 0x30;
+			info->boot_addr_pmu = nv_ro32(bios, data + 0x10) +
+					      nv_ro32(bios, data + 0x18);
+			info->boot_size     = nv_ro32(bios, data + 0x1c) -
+					      nv_ro32(bios, data + 0x18);
+			info->code_addr     = info->boot_addr + info->boot_size;
+			info->code_addr_pmu = info->boot_addr_pmu +
+					      info->boot_size;
+			info->code_size     = nv_ro32(bios, data + 0x20);
+			info->data_addr     = data + 0x30 +
+					      nv_ro32(bios, data + 0x24);
+			info->data_addr_pmu = nv_ro32(bios, data + 0x28);
+			info->data_size     = nv_ro32(bios, data + 0x2c);
+			return true;
+		}
+	}
+	return false;
+}
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/priv.h b/drivers/gpu/drm/nouveau/core/subdev/bios/priv.h
new file mode 100644
index 000000000000..187d225bd1e9
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/priv.h
@@ -0,0 +1,25 @@
+#ifndef __NVKM_BIOS_PRIV_H__
+#define __NVKM_BIOS_PRIV_H__
+
+#include <subdev/bios.h>
+
+struct nvbios_source {
+	const char *name;
+	void *(*init)(struct nouveau_bios *, const char *);
+	void  (*fini)(void *);
+	u32   (*read)(void *, u32 offset, u32 length, struct nouveau_bios *);
+	bool rw;
+};
+
+int nvbios_extend(struct nouveau_bios *, u32 length);
+int nvbios_shadow(struct nouveau_bios *);
+
+extern const struct nvbios_source nvbios_rom;
+extern const struct nvbios_source nvbios_ramin;
+extern const struct nvbios_source nvbios_acpi_fast;
+extern const struct nvbios_source nvbios_acpi_slow;
+extern const struct nvbios_source nvbios_pcirom;
+extern const struct nvbios_source nvbios_platform;
+extern const struct nvbios_source nvbios_of;
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/ramcfg.c b/drivers/gpu/drm/nouveau/core/subdev/bios/ramcfg.c
index 6c401f70ab99..1623c8dfe797 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/ramcfg.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/ramcfg.c
@@ -25,6 +25,7 @@
 #include <subdev/bios.h>
 #include <subdev/bios/bit.h>
 #include <subdev/bios/ramcfg.h>
+#include <subdev/bios/M0203.h>
 
 static u8
 nvbios_ramcfg_strap(struct nouveau_subdev *subdev)
@@ -54,12 +55,22 @@ nvbios_ramcfg_index(struct nouveau_subdev *subdev)
 	u8 strap = nvbios_ramcfg_strap(subdev);
 	u32 xlat = 0x00000000;
 	struct bit_entry bit_M;
+	struct nvbios_M0203E M0203E;
+	u8 ver, hdr;
 
 	if (!bit_entry(bios, 'M', &bit_M)) {
 		if (bit_M.version == 1 && bit_M.length >= 5)
 			xlat = nv_ro16(bios, bit_M.offset + 3);
-		if (bit_M.version == 2 && bit_M.length >= 3)
+		if (bit_M.version == 2 && bit_M.length >= 3) {
+			/*XXX: is M ever shorter than this?
+			 *     if not - what is xlat used for now?
+			 *     also - sigh..
+			 */
+			if (bit_M.length >= 7 &&
+			    nvbios_M0203Em(bios, strap, &ver, &hdr, &M0203E))
+				return M0203E.group;
 			xlat = nv_ro16(bios, bit_M.offset + 1);
+		}
 	}
 
 	if (xlat)
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/rammap.c b/drivers/gpu/drm/nouveau/core/subdev/bios/rammap.c
index 585e69331ccc..c5685228c322 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/rammap.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/rammap.c
@@ -162,8 +162,9 @@ nvbios_rammapSp(struct nouveau_bios *bios, u32 data,
 		p->ramcfg_10_02_08 = (nv_ro08(bios, data + 0x02) & 0x08) >> 3;
 		p->ramcfg_10_02_10 = (nv_ro08(bios, data + 0x02) & 0x10) >> 4;
 		p->ramcfg_10_02_20 = (nv_ro08(bios, data + 0x02) & 0x20) >> 5;
-		p->ramcfg_10_02_40 = (nv_ro08(bios, data + 0x02) & 0x40) >> 6;
+		p->ramcfg_10_DLLoff = (nv_ro08(bios, data + 0x02) & 0x40) >> 6;
 		p->ramcfg_10_03_0f = (nv_ro08(bios, data + 0x03) & 0x0f) >> 0;
+		p->ramcfg_10_04_01 = (nv_ro08(bios, data + 0x04) & 0x01) >> 0;
 		p->ramcfg_10_05    = (nv_ro08(bios, data + 0x05) & 0xff) >> 0;
 		p->ramcfg_10_06    = (nv_ro08(bios, data + 0x06) & 0xff) >> 0;
 		p->ramcfg_10_07    = (nv_ro08(bios, data + 0x07) & 0xff) >> 0;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/shadow.c b/drivers/gpu/drm/nouveau/core/subdev/bios/shadow.c
new file mode 100644
index 000000000000..bb9e0018d936
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/shadow.c
@@ -0,0 +1,270 @@
+/*
+ * Copyright 2014 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 "priv.h"
+#include <core/option.h>
+#include <subdev/bios/image.h>
+
+struct shadow {
+	struct nouveau_oclass base;
+	u32 skip;
+	const struct nvbios_source *func;
+	void *data;
+	u32 size;
+	int score;
+};
+
+static bool
+shadow_fetch(struct nouveau_bios *bios, u32 upto)
+{
+	struct shadow *mthd = (void *)nv_object(bios)->oclass;
+	const u32 limit = (upto + 3) & ~3;
+	const u32 start = bios->size;
+	void *data = mthd->data;
+	if (nvbios_extend(bios, limit) > 0) {
+		u32 read = mthd->func->read(data, start, limit - start, bios);
+		bios->size = start + read;
+	}
+	return bios->size >= limit;
+}
+
+static u8
+shadow_rd08(struct nouveau_object *object, u64 addr)
+{
+	struct nouveau_bios *bios = (void *)object;
+	if (shadow_fetch(bios, addr + 1))
+		return bios->data[addr];
+	return 0x00;
+}
+
+static u16
+shadow_rd16(struct nouveau_object *object, u64 addr)
+{
+	struct nouveau_bios *bios = (void *)object;
+	if (shadow_fetch(bios, addr + 2))
+		return get_unaligned_le16(&bios->data[addr]);
+	return 0x0000;
+}
+
+static u32
+shadow_rd32(struct nouveau_object *object, u64 addr)
+{
+	struct nouveau_bios *bios = (void *)object;
+	if (shadow_fetch(bios, addr + 4))
+		return get_unaligned_le32(&bios->data[addr]);
+	return 0x00000000;
+}
+
+static struct nouveau_oclass
+shadow_class = {
+	.handle = NV_SUBDEV(VBIOS, 0x00),
+	.ofuncs = &(struct nouveau_ofuncs) {
+		.rd08 = shadow_rd08,
+		.rd16 = shadow_rd16,
+		.rd32 = shadow_rd32,
+	},
+};
+
+static int
+shadow_image(struct nouveau_bios *bios, int idx, struct shadow *mthd)
+{
+	struct nvbios_image image;
+	int score = 1;
+
+	if (!nvbios_image(bios, idx, &image)) {
+		nv_debug(bios, "image %d invalid\n", idx);
+		return 0;
+	}
+	nv_debug(bios, "%08x: type %02x, %d bytes\n",
+		 image.base, image.type, image.size);
+
+	if (!shadow_fetch(bios, image.size)) {
+		nv_debug(bios, "%08x: fetch failed\n", image.base);
+		return 0;
+	}
+
+	switch (image.type) {
+	case 0x00:
+		if (nvbios_checksum(&bios->data[image.base], image.size)) {
+			nv_debug(bios, "%08x: checksum failed\n", image.base);
+			if (mthd->func->rw)
+				score += 1;
+			score += 1;
+		} else {
+			score += 3;
+		}
+		break;
+	default:
+		score += 3;
+		break;
+	}
+
+	if (!image.last)
+		score += shadow_image(bios, idx + 1, mthd);
+	return score;
+}
+
+static int
+shadow_score(struct nouveau_bios *bios, struct shadow *mthd)
+{
+	struct nouveau_oclass *oclass = nv_object(bios)->oclass;
+	int score;
+	nv_object(bios)->oclass = &mthd->base;
+	score = shadow_image(bios, 0, mthd);
+	nv_object(bios)->oclass = oclass;
+	return score;
+
+}
+
+static int
+shadow_method(struct nouveau_bios *bios, struct shadow *mthd, const char *name)
+{
+	const struct nvbios_source *func = mthd->func;
+	if (func->name) {
+		nv_debug(bios, "trying %s...\n", name ? name : func->name);
+		if (func->init) {
+			mthd->data = func->init(bios, name);
+			if (IS_ERR(mthd->data)) {
+				mthd->data = NULL;
+				return 0;
+			}
+		}
+		mthd->score = shadow_score(bios, mthd);
+		if (func->fini)
+			func->fini(mthd->data);
+		nv_debug(bios, "scored %d\n", mthd->score);
+		mthd->data = bios->data;
+		mthd->size = bios->size;
+		bios->data  = NULL;
+		bios->size  = 0;
+	}
+	return mthd->score;
+}
+
+static u32
+shadow_fw_read(void *data, u32 offset, u32 length, struct nouveau_bios *bios)
+{
+	const struct firmware *fw = data;
+	if (offset + length <= fw->size) {
+		memcpy(bios->data + offset, fw->data + offset, length);
+		return length;
+	}
+	return 0;
+}
+
+static void *
+shadow_fw_init(struct nouveau_bios *bios, const char *name)
+{
+	struct device *dev = &nv_device(bios)->pdev->dev;
+	const struct firmware *fw;
+	int ret = request_firmware(&fw, name, dev);
+	if (ret)
+		return ERR_PTR(-ENOENT);
+	return (void *)fw;
+}
+
+static const struct nvbios_source
+shadow_fw = {
+	.name = "firmware",
+	.init = shadow_fw_init,
+	.fini = (void(*)(void *))release_firmware,
+	.read = shadow_fw_read,
+	.rw = false,
+};
+
+int
+nvbios_shadow(struct nouveau_bios *bios)
+{
+	struct shadow mthds[] = {
+		{ shadow_class, 0, &nvbios_of },
+		{ shadow_class, 0, &nvbios_ramin },
+		{ shadow_class, 0, &nvbios_rom },
+		{ shadow_class, 0, &nvbios_acpi_fast },
+		{ shadow_class, 4, &nvbios_acpi_slow },
+		{ shadow_class, 1, &nvbios_pcirom },
+		{ shadow_class, 1, &nvbios_platform },
+		{ shadow_class }
+	}, *mthd = mthds, *best = NULL;
+	const char *optarg;
+	char *source;
+	int optlen;
+
+	/* handle user-specified bios source */
+	optarg = nouveau_stropt(nv_device(bios)->cfgopt, "NvBios", &optlen);
+	source = optarg ? kstrndup(optarg, optlen, GFP_KERNEL) : NULL;
+	if (source) {
+		/* try to match one of the built-in methods */
+		for (mthd = mthds; mthd->func; mthd++) {
+			if (mthd->func->name &&
+			    !strcasecmp(source, mthd->func->name)) {
+				best = mthd;
+				if (shadow_method(bios, mthd, NULL))
+					break;
+			}
+		}
+
+		/* otherwise, attempt to load as firmware */
+		if (!best && (best = mthd)) {
+			mthd->func = &shadow_fw;
+			shadow_method(bios, mthd, source);
+			mthd->func = NULL;
+		}
+
+		if (!best->score) {
+			nv_error(bios, "%s invalid\n", source);
+			kfree(source);
+			source = NULL;
+		}
+	}
+
+	/* scan all potential bios sources, looking for best image */
+	if (!best || !best->score) {
+		for (mthd = mthds, best = mthd; mthd->func; mthd++) {
+			if (!mthd->skip || best->score < mthd->skip) {
+				if (shadow_method(bios, mthd, NULL)) {
+					if (mthd->score > best->score)
+						best = mthd;
+				}
+			}
+		}
+	}
+
+	/* cleanup the ones we didn't use */
+	for (mthd = mthds; mthd->func; mthd++) {
+		if (mthd != best)
+			kfree(mthd->data);
+	}
+
+	if (!best->score) {
+		nv_fatal(bios, "unable to locate usable image\n");
+		return -EINVAL;
+	}
+
+	nv_info(bios, "using image from %s\n", best->func ?
+		best->func->name : source);
+	bios->data = best->data;
+	bios->size = best->size;
+	kfree(source);
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/shadowacpi.c b/drivers/gpu/drm/nouveau/core/subdev/bios/shadowacpi.c
new file mode 100644
index 000000000000..bc130c12ec06
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/shadowacpi.c
@@ -0,0 +1,111 @@
+/*
+ * 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.
+ *
+ */
+
+#include "priv.h"
+
+#if defined(CONFIG_ACPI) && defined(CONFIG_X86)
+int nouveau_acpi_get_bios_chunk(uint8_t *bios, int offset, int len);
+bool nouveau_acpi_rom_supported(struct pci_dev *pdev);
+#else
+static inline bool
+nouveau_acpi_rom_supported(struct pci_dev *pdev)
+{
+	return false;
+}
+
+static inline int
+nouveau_acpi_get_bios_chunk(uint8_t *bios, int offset, int len)
+{
+	return -EINVAL;
+}
+#endif
+
+/* This version of the shadow function disobeys the ACPI spec and tries
+ * to fetch in units of more than 4KiB at a time.  This is a LOT faster
+ * on some systems, such as Lenovo W530.
+ */
+static u32
+acpi_read_fast(void *data, u32 offset, u32 length, struct nouveau_bios *bios)
+{
+	u32 limit = (offset + length + 0xfff) & ~0xfff;
+	u32 start = offset & ~0x00000fff;
+	u32 fetch = limit - start;
+
+	if (nvbios_extend(bios, limit) > 0) {
+		int ret = nouveau_acpi_get_bios_chunk(bios->data, start, fetch);
+		if (ret == fetch)
+			return fetch;
+	}
+
+	return 0;
+}
+
+/* Other systems, such as the one in fdo#55948, will report a success
+ * but only return 4KiB of data.  The common bios fetching logic will
+ * detect an invalid image, and fall back to this version of the read
+ * function.
+ */
+static u32
+acpi_read_slow(void *data, u32 offset, u32 length, struct nouveau_bios *bios)
+{
+	u32 limit = (offset + length + 0xfff) & ~0xfff;
+	u32 start = offset & ~0xfff;
+	u32 fetch = 0;
+
+	if (nvbios_extend(bios, limit) > 0) {
+		while (start + fetch < limit) {
+			int ret = nouveau_acpi_get_bios_chunk(bios->data,
+							      start + fetch,
+							      0x1000);
+			if (ret != 0x1000)
+				break;
+			fetch += 0x1000;
+		}
+	}
+
+	return fetch;
+}
+
+static void *
+acpi_init(struct nouveau_bios *bios, const char *name)
+{
+	if (!nouveau_acpi_rom_supported(nv_device(bios)->pdev))
+		return ERR_PTR(-ENODEV);
+	return NULL;
+}
+
+const struct nvbios_source
+nvbios_acpi_fast = {
+	.name = "ACPI",
+	.init = acpi_init,
+	.read = acpi_read_fast,
+	.rw = false,
+};
+
+const struct nvbios_source
+nvbios_acpi_slow = {
+	.name = "ACPI",
+	.init = acpi_init,
+	.read = acpi_read_slow,
+	.rw = false,
+};
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/shadowof.c b/drivers/gpu/drm/nouveau/core/subdev/bios/shadowof.c
new file mode 100644
index 000000000000..3abe487a6025
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/shadowof.c
@@ -0,0 +1,71 @@
+/*
+ * 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.
+ *
+ */
+
+#include "priv.h"
+
+#if defined(__powerpc__)
+struct priv {
+	const void __iomem *data;
+	int size;
+};
+
+static u32
+of_read(void *data, u32 offset, u32 length, struct nouveau_bios *bios)
+{
+	struct priv *priv = data;
+	if (offset + length <= priv->size) {
+		memcpy_fromio(bios->data + offset, priv->data + offset, length);
+		return length;
+	}
+	return 0;
+}
+
+static void *
+of_init(struct nouveau_bios *bios, const char *name)
+{
+	struct pci_dev *pdev = nv_device(bios)->pdev;
+	struct device_node *dn;
+	struct priv *priv;
+	if (!(dn = pci_device_to_OF_node(pdev)))
+		return ERR_PTR(-ENODEV);
+	if (!(priv = kzalloc(sizeof(*priv), GFP_KERNEL)))
+		return ERR_PTR(-ENOMEM);
+	if ((priv->data = of_get_property(dn, "NVDA,BMP", &priv->size)))
+		return priv;
+	kfree(priv);
+	return ERR_PTR(-EINVAL);
+}
+
+const struct nvbios_source
+nvbios_of = {
+	.name = "OpenFirmware",
+	.init = of_init,
+	.fini = (void(*)(void *))kfree,
+	.read = of_read,
+	.rw = false,
+};
+#else
+const struct nvbios_source
+nvbios_of = {
+};
+#endif
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/shadowpci.c b/drivers/gpu/drm/nouveau/core/subdev/bios/shadowpci.c
new file mode 100644
index 000000000000..1d0389c0abef
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/shadowpci.c
@@ -0,0 +1,108 @@
+/*
+ * 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.
+ *
+ */
+
+#include "priv.h"
+
+struct priv {
+	struct pci_dev *pdev;
+	void __iomem *rom;
+	size_t size;
+};
+
+static u32
+pcirom_read(void *data, u32 offset, u32 length, struct nouveau_bios *bios)
+{
+	struct priv *priv = data;
+	if (offset + length <= priv->size) {
+		memcpy_fromio(bios->data + offset, priv->rom + offset, length);
+		return length;
+	}
+	return 0;
+}
+
+static void
+pcirom_fini(void *data)
+{
+	struct priv *priv = data;
+	pci_unmap_rom(priv->pdev, priv->rom);
+	pci_disable_rom(priv->pdev);
+	kfree(priv);
+}
+
+static void *
+pcirom_init(struct nouveau_bios *bios, const char *name)
+{
+	struct pci_dev *pdev = nv_device(bios)->pdev;
+	struct priv *priv = NULL;
+	int ret;
+
+	if (!(ret = pci_enable_rom(pdev))) {
+		if (ret = -ENOMEM,
+		    (priv = kmalloc(sizeof(*priv), GFP_KERNEL))) {
+			if (ret = -EFAULT,
+			    (priv->rom = pci_map_rom(pdev, &priv->size))) {
+				priv->pdev = pdev;
+				return priv;
+			}
+			kfree(priv);
+		}
+		pci_disable_rom(pdev);
+	}
+
+	return ERR_PTR(ret);
+}
+
+const struct nvbios_source
+nvbios_pcirom = {
+	.name = "PCIROM",
+	.init = pcirom_init,
+	.fini = pcirom_fini,
+	.read = pcirom_read,
+	.rw = true,
+};
+
+static void *
+platform_init(struct nouveau_bios *bios, const char *name)
+{
+	struct pci_dev *pdev = nv_device(bios)->pdev;
+	struct priv *priv;
+	int ret = -ENOMEM;
+
+	if ((priv = kmalloc(sizeof(*priv), GFP_KERNEL))) {
+		if (ret = -ENODEV,
+		    (priv->rom = pci_platform_rom(pdev, &priv->size)))
+			return priv;
+		kfree(priv);
+	}
+
+	return ERR_PTR(ret);
+}
+
+const struct nvbios_source
+nvbios_platform = {
+	.name = "PLATFORM",
+	.init = platform_init,
+	.fini = (void(*)(void *))kfree,
+	.read = pcirom_read,
+	.rw = true,
+};
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/shadowramin.c b/drivers/gpu/drm/nouveau/core/subdev/bios/shadowramin.c
new file mode 100644
index 000000000000..5e58bba0dd5c
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/shadowramin.c
@@ -0,0 +1,112 @@
+/*
+ * 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.
+ *
+ */
+
+#include "priv.h"
+
+struct priv {
+	struct nouveau_bios *bios;
+	u32 bar0;
+};
+
+static u32
+pramin_read(void *data, u32 offset, u32 length, struct nouveau_bios *bios)
+{
+	u32 i;
+	if (offset + length <= 0x00100000) {
+		for (i = offset; i < offset + length; i += 4)
+			*(u32 *)&bios->data[i] = nv_rd32(bios, 0x700000 + i);
+		return length;
+	}
+	return 0;
+}
+
+static void
+pramin_fini(void *data)
+{
+	struct priv *priv = data;
+	nv_wr32(priv->bios, 0x001700, priv->bar0);
+	kfree(priv);
+}
+
+static void *
+pramin_init(struct nouveau_bios *bios, const char *name)
+{
+	struct priv *priv = NULL;
+	u64 addr = 0;
+
+	/* PRAMIN always potentially available prior to nv50 */
+	if (nv_device(bios)->card_type < NV_50)
+		return NULL;
+
+	/* we can't get the bios image pointer without PDISP */
+	if (nv_device(bios)->card_type >= GM100)
+		addr = nv_rd32(bios, 0x021c04);
+	else
+	if (nv_device(bios)->card_type >= NV_C0)
+		addr = nv_rd32(bios, 0x022500);
+	if (addr & 0x00000001) {
+		nv_debug(bios, "... display disabled\n");
+		return ERR_PTR(-ENODEV);
+	}
+
+	/* check that the window is enabled and in vram, particularly
+	 * important as we don't want to be touching vram on an
+	 * uninitialised board
+	 */
+	addr = nv_rd32(bios, 0x619f04);
+	if (!(addr & 0x00000008)) {
+		nv_debug(bios, "... not enabled\n");
+		return ERR_PTR(-ENODEV);
+	}
+	if ( (addr & 0x00000003) != 1) {
+		nv_debug(bios, "... not in vram\n");
+		return ERR_PTR(-ENODEV);
+	}
+
+	/* some alternate method inherited from xf86-video-nv... */
+	addr = (addr & 0xffffff00) << 8;
+	if (!addr) {
+		addr  = (u64)nv_rd32(bios, 0x001700) << 16;
+		addr += 0xf0000;
+	}
+
+	/* modify bar0 PRAMIN window to cover the bios image */
+	if (!(priv = kmalloc(sizeof(*priv), GFP_KERNEL))) {
+		nv_error(bios, "... out of memory\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	priv->bios = bios;
+	priv->bar0 = nv_rd32(bios, 0x001700);
+	nv_wr32(bios, 0x001700, addr >> 16);
+	return priv;
+}
+
+const struct nvbios_source
+nvbios_ramin = {
+	.name = "PRAMIN",
+	.init = pramin_init,
+	.fini = pramin_fini,
+	.read = pramin_read,
+	.rw = true,
+};
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/shadowrom.c b/drivers/gpu/drm/nouveau/core/subdev/bios/shadowrom.c
new file mode 100644
index 000000000000..b7992bc3ffa5
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/shadowrom.c
@@ -0,0 +1,69 @@
+/*
+ * 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.
+ *
+ */
+
+#include "priv.h"
+
+static u32
+prom_read(void *data, u32 offset, u32 length, struct nouveau_bios *bios)
+{
+	u32 i;
+	if (offset + length <= 0x00100000) {
+		for (i = offset; i < offset + length; i += 4)
+			*(u32 *)&bios->data[i] = nv_rd32(bios, 0x300000 + i);
+		return length;
+	}
+	return 0;
+}
+
+static void
+prom_fini(void *data)
+{
+	struct nouveau_bios *bios = data;
+	if (nv_device(bios)->card_type < NV_50)
+		nv_mask(bios, 0x001850, 0x00000001, 0x00000001);
+	else
+		nv_mask(bios, 0x088050, 0x00000001, 0x00000001);
+}
+
+static void *
+prom_init(struct nouveau_bios *bios, const char *name)
+{
+	if (nv_device(bios)->card_type < NV_50) {
+		if (nv_device(bios)->card_type == NV_40 &&
+		    nv_device(bios)->chipset >= 0x4c)
+			return ERR_PTR(-ENODEV);
+		nv_mask(bios, 0x001850, 0x00000001, 0x00000000);
+	} else {
+		nv_mask(bios, 0x088050, 0x00000001, 0x00000000);
+	}
+	return bios;
+}
+
+const struct nvbios_source
+nvbios_rom = {
+	.name = "PROM",
+	.init = prom_init,
+	.fini = prom_fini,
+	.read = prom_read,
+	.rw = false,
+};
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/timing.c b/drivers/gpu/drm/nouveau/core/subdev/bios/timing.c
index 46d955eb51eb..8521eca1ed9c 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/timing.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/timing.c
@@ -93,10 +93,44 @@ nvbios_timingEp(struct nouveau_bios *bios, int idx,
 	p->timing_hdr = *hdr;
 	switch (!!data * *ver) {
 	case 0x10:
-		p->timing_10_WR = nv_ro08(bios, data + 0x00);
-		p->timing_10_CL = nv_ro08(bios, data + 0x02);
-		p->timing_10_ODT = nv_ro08(bios, data + 0x0e) & 0x07;
-		p->timing_10_CWL = nv_ro08(bios, data + 0x13);
+		p->timing_10_WR    = nv_ro08(bios, data + 0x00);
+		p->timing_10_WTR   = nv_ro08(bios, data + 0x01);
+		p->timing_10_CL    = nv_ro08(bios, data + 0x02);
+		p->timing_10_RC    = nv_ro08(bios, data + 0x03);
+		p->timing_10_RFC   = nv_ro08(bios, data + 0x05);
+		p->timing_10_RAS   = nv_ro08(bios, data + 0x07);
+		p->timing_10_RP    = nv_ro08(bios, data + 0x09);
+		p->timing_10_RCDRD = nv_ro08(bios, data + 0x0a);
+		p->timing_10_RCDWR = nv_ro08(bios, data + 0x0b);
+		p->timing_10_RRD   = nv_ro08(bios, data + 0x0c);
+		p->timing_10_13    = nv_ro08(bios, data + 0x0d);
+		p->timing_10_ODT   = nv_ro08(bios, data + 0x0e) & 0x07;
+
+		p->timing_10_24  = 0xff;
+		p->timing_10_21  = 0;
+		p->timing_10_20  = 0;
+		p->timing_10_CWL = 0;
+		p->timing_10_18  = 0;
+		p->timing_10_16  = 0;
+
+		switch (min_t(u8, *hdr, 25)) {
+		case 25:
+			p->timing_10_24  = nv_ro08(bios, data + 0x18);
+		case 24:
+		case 23:
+		case 22:
+			p->timing_10_21  = nv_ro08(bios, data + 0x15);
+		case 21:
+			p->timing_10_20  = nv_ro08(bios, data + 0x14);
+		case 20:
+			p->timing_10_CWL = nv_ro08(bios, data + 0x13);
+		case 19:
+			p->timing_10_18  = nv_ro08(bios, data + 0x12);
+		case 18:
+		case 17:
+			p->timing_10_16  = nv_ro08(bios, data + 0x10);
+		}
+
 		break;
 	case 0x20:
 		p->timing[0] = nv_ro32(bios, data + 0x00);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/clock/gk20a.c b/drivers/gpu/drm/nouveau/core/subdev/clock/gk20a.c
index 425a8d5e9129..fb4fad374bdd 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/clock/gk20a.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/clock/gk20a.c
@@ -109,7 +109,7 @@ struct gk20a_clk_pllg_params {
 };
 
 static const struct gk20a_clk_pllg_params gk20a_pllg_params = {
-	.min_vco = 1000, .max_vco = 1700,
+	.min_vco = 1000, .max_vco = 2064,
 	.min_u = 12, .max_u = 38,
 	.min_m = 1, .max_m = 255,
 	.min_n = 8, .max_n = 255,
@@ -470,76 +470,91 @@ gk20a_pstates[] = {
 	{
 		.base = {
 			.domain[nv_clk_src_gpc] = 72000,
+			.voltage = 0,
 		},
 	},
 	{
 		.base = {
 			.domain[nv_clk_src_gpc] = 108000,
+			.voltage = 1,
 		},
 	},
 	{
 		.base = {
 			.domain[nv_clk_src_gpc] = 180000,
+			.voltage = 2,
 		},
 	},
 	{
 		.base = {
 			.domain[nv_clk_src_gpc] = 252000,
+			.voltage = 3,
 		},
 	},
 	{
 		.base = {
 			.domain[nv_clk_src_gpc] = 324000,
+			.voltage = 4,
 		},
 	},
 	{
 		.base = {
 			.domain[nv_clk_src_gpc] = 396000,
+			.voltage = 5,
 		},
 	},
 	{
 		.base = {
 			.domain[nv_clk_src_gpc] = 468000,
+			.voltage = 6,
 		},
 	},
 	{
 		.base = {
 			.domain[nv_clk_src_gpc] = 540000,
+			.voltage = 7,
 		},
 	},
 	{
 		.base = {
 			.domain[nv_clk_src_gpc] = 612000,
+			.voltage = 8,
 		},
 	},
 	{
 		.base = {
 			.domain[nv_clk_src_gpc] = 648000,
+			.voltage = 9,
 		},
 	},
 	{
 		.base = {
 			.domain[nv_clk_src_gpc] = 684000,
+			.voltage = 10,
 		},
 	},
 	{
 		.base = {
 			.domain[nv_clk_src_gpc] = 708000,
+			.voltage = 11,
 		},
 	},
 	{
 		.base = {
 			.domain[nv_clk_src_gpc] = 756000,
+			.voltage = 12,
 		},
 	},
 	{
 		.base = {
 			.domain[nv_clk_src_gpc] = 804000,
+			.voltage = 13,
 		},
 	},
 	{
 		.base = {
 			.domain[nv_clk_src_gpc] = 852000,
+			.voltage = 14,
 		},
 	},
 };
diff --git a/drivers/gpu/drm/nouveau/core/subdev/clock/nva3.c b/drivers/gpu/drm/nouveau/core/subdev/clock/nva3.c
index 094551d8ad9b..07ad01247675 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/clock/nva3.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/clock/nva3.c
@@ -510,7 +510,7 @@ nva3_clock_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	int ret;
 
 	ret = nouveau_clock_create(parent, engine, oclass, nva3_domain, NULL, 0,
-				   false, &priv);
+				   true, &priv);
 	*pobject = nv_object(priv);
 	if (ret)
 		return ret;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/devinit/base.c b/drivers/gpu/drm/nouveau/core/subdev/devinit/base.c
index 239acfe876c3..0e45cee82463 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/devinit/base.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/devinit/base.c
@@ -24,8 +24,6 @@
 
 #include <core/option.h>
 
-#include <subdev/bios.h>
-#include <subdev/bios/init.h>
 #include <subdev/vga.h>
 
 #include "priv.h"
@@ -56,7 +54,7 @@ _nouveau_devinit_init(struct nouveau_object *object)
 	if (ret)
 		return ret;
 
-	ret = nvbios_init(&devinit->base, devinit->post);
+	ret = impl->post(&devinit->base, devinit->post);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/devinit/gm107.c b/drivers/gpu/drm/nouveau/core/subdev/devinit/gm107.c
index c69bc7f54e37..4ba43d6a1ec8 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/devinit/gm107.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/devinit/gm107.c
@@ -24,7 +24,7 @@
 
 #include "nv50.h"
 
-static u64
+u64
 gm107_devinit_disable(struct nouveau_devinit *devinit)
 {
 	struct nv50_devinit_priv *priv = (void *)devinit;
@@ -53,4 +53,5 @@ gm107_devinit_oclass = &(struct nouveau_devinit_impl) {
 	},
 	.pll_set = nvc0_devinit_pll_set,
 	.disable = gm107_devinit_disable,
+	.post = nvbios_init,
 }.base;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/devinit/gm204.c b/drivers/gpu/drm/nouveau/core/subdev/devinit/gm204.c
new file mode 100644
index 000000000000..e44a86662a2a
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/devinit/gm204.c
@@ -0,0 +1,173 @@
+/*
+ * Copyright 2013 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include <subdev/bios.h>
+#include <subdev/bios/bit.h>
+#include <subdev/bios/pmu.h>
+
+#include "nv50.h"
+
+static void
+pmu_code(struct nv50_devinit_priv *priv, u32 pmu, u32 img, u32 len, bool sec)
+{
+	struct nouveau_bios *bios = nouveau_bios(priv);
+	int i;
+
+	nv_wr32(priv, 0x10a180, 0x01000000 | (sec ? 0x10000000 : 0) | pmu);
+	for (i = 0; i < len; i += 4) {
+		if ((i & 0xff) == 0)
+			nv_wr32(priv, 0x10a188, (pmu + i) >> 8);
+		nv_wr32(priv, 0x10a184, nv_ro32(bios, img + i));
+	}
+
+	while (i & 0xff) {
+		nv_wr32(priv, 0x10a184, 0x00000000);
+		i += 4;
+	}
+}
+
+static void
+pmu_data(struct nv50_devinit_priv *priv, u32 pmu, u32 img, u32 len)
+{
+	struct nouveau_bios *bios = nouveau_bios(priv);
+	int i;
+
+	nv_wr32(priv, 0x10a1c0, 0x01000000 | pmu);
+	for (i = 0; i < len; i += 4)
+		nv_wr32(priv, 0x10a1c4, nv_ro32(bios, img + i));
+}
+
+static u32
+pmu_args(struct nv50_devinit_priv *priv, u32 argp, u32 argi)
+{
+	nv_wr32(priv, 0x10a1c0, argp);
+	nv_wr32(priv, 0x10a1c0, nv_rd32(priv, 0x10a1c4) + argi);
+	return nv_rd32(priv, 0x10a1c4);
+}
+
+static void
+pmu_exec(struct nv50_devinit_priv *priv, u32 init_addr)
+{
+	nv_wr32(priv, 0x10a104, init_addr);
+	nv_wr32(priv, 0x10a10c, 0x00000000);
+	nv_wr32(priv, 0x10a100, 0x00000002);
+}
+
+static int
+pmu_load(struct nv50_devinit_priv *priv, u8 type, bool post,
+	 u32 *init_addr_pmu, u32 *args_addr_pmu)
+{
+	struct nouveau_bios *bios = nouveau_bios(priv);
+	struct nvbios_pmuR pmu;
+
+	if (!nvbios_pmuRm(bios, type, &pmu)) {
+		nv_error(priv, "VBIOS PMU fuc %02x not found\n", type);
+		return -EINVAL;
+	}
+
+	if (!post)
+		return 0;
+
+	pmu_code(priv, pmu.boot_addr_pmu, pmu.boot_addr, pmu.boot_size, false);
+	pmu_code(priv, pmu.code_addr_pmu, pmu.code_addr, pmu.code_size, true);
+	pmu_data(priv, pmu.data_addr_pmu, pmu.data_addr, pmu.data_size);
+
+	if (init_addr_pmu) {
+		*init_addr_pmu = pmu.init_addr_pmu;
+		*args_addr_pmu = pmu.args_addr_pmu;
+		return 0;
+	}
+
+	return pmu_exec(priv, pmu.init_addr_pmu), 0;
+}
+
+static int
+gm204_devinit_post(struct nouveau_subdev *subdev, bool post)
+{
+	struct nv50_devinit_priv *priv = (void *)nouveau_devinit(subdev);
+	struct nouveau_bios *bios = nouveau_bios(priv);
+	struct bit_entry bit_I;
+	u32 init, args;
+	int ret;
+
+	if (bit_entry(bios, 'I', &bit_I) || bit_I.version != 1 ||
+					    bit_I.length < 0x1c) {
+		nv_error(priv, "VBIOS PMU init data not found\n");
+		return -EINVAL;
+	}
+
+	/* reset PMU and load init table parser ucode */
+	if (post) {
+		nv_mask(priv, 0x000200, 0x00002000, 0x00000000);
+		nv_mask(priv, 0x000200, 0x00002000, 0x00002000);
+		nv_rd32(priv, 0x000200);
+		while (nv_rd32(priv, 0x10a10c) & 0x00000006) {
+		}
+	}
+
+	ret = pmu_load(priv, 0x04, post, &init, &args);
+	if (ret)
+		return ret;
+
+	/* upload first chunk of init data */
+	if (post) {
+		u32 pmu = pmu_args(priv, args + 0x08, 0x08);
+		u32 img = nv_ro16(bios, bit_I.offset + 0x14);
+		u32 len = nv_ro16(bios, bit_I.offset + 0x16);
+		pmu_data(priv, pmu, img, len);
+	}
+
+	/* upload second chunk of init data */
+	if (post) {
+		u32 pmu = pmu_args(priv, args + 0x08, 0x10);
+		u32 img = nv_ro16(bios, bit_I.offset + 0x18);
+		u32 len = nv_ro16(bios, bit_I.offset + 0x1a);
+		pmu_data(priv, pmu, img, len);
+	}
+
+	/* execute init tables */
+	if (post) {
+		nv_wr32(priv, 0x10a040, 0x00005000);
+		pmu_exec(priv, init);
+		while (!(nv_rd32(priv, 0x10a040) & 0x00002000)) {
+		}
+	}
+
+	/* load and execute some other ucode image (bios therm?) */
+	return pmu_load(priv, 0x01, post, NULL, NULL);
+}
+
+struct nouveau_oclass *
+gm204_devinit_oclass = &(struct nouveau_devinit_impl) {
+	.base.handle = NV_SUBDEV(DEVINIT, 0x07),
+	.base.ofuncs = &(struct nouveau_ofuncs) {
+		.ctor = nv50_devinit_ctor,
+		.dtor = _nouveau_devinit_dtor,
+		.init = nv50_devinit_init,
+		.fini = _nouveau_devinit_fini,
+	},
+	.pll_set = nvc0_devinit_pll_set,
+	.disable = gm107_devinit_disable,
+	.post = gm204_devinit_post,
+}.base;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/devinit/nv04.c b/drivers/gpu/drm/nouveau/core/subdev/devinit/nv04.c
index 052ad690b468..65651c50f6ea 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/devinit/nv04.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/devinit/nv04.c
@@ -464,4 +464,5 @@ nv04_devinit_oclass = &(struct nouveau_devinit_impl) {
 	},
 	.meminit = nv04_devinit_meminit,
 	.pll_set = nv04_devinit_pll_set,
+	.post = nvbios_init,
 }.base;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/devinit/nv05.c b/drivers/gpu/drm/nouveau/core/subdev/devinit/nv05.c
index 4a19c10e5178..a2007a3efc4d 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/devinit/nv05.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/devinit/nv05.c
@@ -136,4 +136,5 @@ nv05_devinit_oclass = &(struct nouveau_devinit_impl) {
 	},
 	.meminit = nv05_devinit_meminit,
 	.pll_set = nv04_devinit_pll_set,
+	.post = nvbios_init,
 }.base;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/devinit/nv10.c b/drivers/gpu/drm/nouveau/core/subdev/devinit/nv10.c
index 3b8d657da279..178b46f79b50 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/devinit/nv10.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/devinit/nv10.c
@@ -107,4 +107,5 @@ nv10_devinit_oclass = &(struct nouveau_devinit_impl) {
 	},
 	.meminit = nv10_devinit_meminit,
 	.pll_set = nv04_devinit_pll_set,
+	.post = nvbios_init,
 }.base;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/devinit/nv1a.c b/drivers/gpu/drm/nouveau/core/subdev/devinit/nv1a.c
index 526d0c6faacd..995dd97af3e9 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/devinit/nv1a.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/devinit/nv1a.c
@@ -34,4 +34,5 @@ nv1a_devinit_oclass = &(struct nouveau_devinit_impl) {
 		.fini = nv04_devinit_fini,
 	},
 	.pll_set = nv04_devinit_pll_set,
+	.post = nvbios_init,
 }.base;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/devinit/nv20.c b/drivers/gpu/drm/nouveau/core/subdev/devinit/nv20.c
index 04bc9732644c..915089fb46f7 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/devinit/nv20.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/devinit/nv20.c
@@ -71,4 +71,5 @@ nv20_devinit_oclass = &(struct nouveau_devinit_impl) {
 	},
 	.meminit = nv20_devinit_meminit,
 	.pll_set = nv04_devinit_pll_set,
+	.post = nvbios_init,
 }.base;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/devinit/nv50.c b/drivers/gpu/drm/nouveau/core/subdev/devinit/nv50.c
index b46c62a1d5d8..968334d1dca4 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/devinit/nv50.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/devinit/nv50.c
@@ -26,6 +26,7 @@
 #include <subdev/bios/dcb.h>
 #include <subdev/bios/disp.h>
 #include <subdev/bios/init.h>
+#include <subdev/ibus.h>
 #include <subdev/vga.h>
 
 #include "nv50.h"
@@ -91,6 +92,7 @@ int
 nv50_devinit_init(struct nouveau_object *object)
 {
 	struct nouveau_bios *bios = nouveau_bios(object);
+	struct nouveau_ibus *ibus = nouveau_ibus(object);
 	struct nv50_devinit_priv *priv = (void *)object;
 	struct nvbios_outp info;
 	struct dcb_output outp;
@@ -105,6 +107,13 @@ nv50_devinit_init(struct nouveau_object *object)
 		}
 	}
 
+	/* some boards appear to require certain priv register timeouts
+	 * to be bumped before runing devinit scripts.  not a clue why
+	 * the vbios engineers didn't make the scripts just work...
+	 */
+	if (priv->base.post && ibus)
+		nv_ofuncs(ibus)->init(nv_object(ibus));
+
 	ret = nouveau_devinit_init(&priv->base);
 	if (ret)
 		return ret;
@@ -160,4 +169,5 @@ nv50_devinit_oclass = &(struct nouveau_devinit_impl) {
 	},
 	.pll_set = nv50_devinit_pll_set,
 	.disable = nv50_devinit_disable,
+	.post = nvbios_init,
 }.base;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/devinit/nv50.h b/drivers/gpu/drm/nouveau/core/subdev/devinit/nv50.h
index 51d5076333ec..f412bb7f780e 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/devinit/nv50.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/devinit/nv50.h
@@ -18,4 +18,6 @@ int  nva3_devinit_pll_set(struct nouveau_devinit *, u32, u32);
 
 int  nvc0_devinit_pll_set(struct nouveau_devinit *, u32, u32);
 
+u64  gm107_devinit_disable(struct nouveau_devinit *);
+
 #endif
diff --git a/drivers/gpu/drm/nouveau/core/subdev/devinit/nv84.c b/drivers/gpu/drm/nouveau/core/subdev/devinit/nv84.c
index 787422505d87..a7c80ded77cd 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/devinit/nv84.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/devinit/nv84.c
@@ -60,4 +60,5 @@ nv84_devinit_oclass = &(struct nouveau_devinit_impl) {
 	},
 	.pll_set = nv50_devinit_pll_set,
 	.disable = nv84_devinit_disable,
+	.post = nvbios_init,
 }.base;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/devinit/nv98.c b/drivers/gpu/drm/nouveau/core/subdev/devinit/nv98.c
index 2b0e963fc6f0..a773253a17f6 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/devinit/nv98.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/devinit/nv98.c
@@ -59,4 +59,5 @@ nv98_devinit_oclass = &(struct nouveau_devinit_impl) {
 	},
 	.pll_set = nv50_devinit_pll_set,
 	.disable = nv98_devinit_disable,
+	.post = nvbios_init,
 }.base;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/devinit/nva3.c b/drivers/gpu/drm/nouveau/core/subdev/devinit/nva3.c
index 006cf348bda7..b9cd9e53f760 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/devinit/nva3.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/devinit/nva3.c
@@ -142,4 +142,5 @@ nva3_devinit_oclass = &(struct nouveau_devinit_impl) {
 	.pll_set = nva3_devinit_pll_set,
 	.disable = nva3_devinit_disable,
 	.mmio    = nva3_devinit_mmio,
+	.post = nvbios_init,
 }.base;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/devinit/nvaf.c b/drivers/gpu/drm/nouveau/core/subdev/devinit/nvaf.c
index 4fc68d27eff3..3729846a8e5c 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/devinit/nvaf.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/devinit/nvaf.c
@@ -60,4 +60,5 @@ nvaf_devinit_oclass = &(struct nouveau_devinit_impl) {
 	},
 	.pll_set = nva3_devinit_pll_set,
 	.disable = nvaf_devinit_disable,
+	.post = nvbios_init,
 }.base;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/devinit/nvc0.c b/drivers/gpu/drm/nouveau/core/subdev/devinit/nvc0.c
index 30c765747eea..80bd7f5eda3d 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/devinit/nvc0.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/devinit/nvc0.c
@@ -115,4 +115,5 @@ nvc0_devinit_oclass = &(struct nouveau_devinit_impl) {
 	},
 	.pll_set = nvc0_devinit_pll_set,
 	.disable = nvc0_devinit_disable,
+	.post = nvbios_init,
 }.base;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/devinit/priv.h b/drivers/gpu/drm/nouveau/core/subdev/devinit/priv.h
index f0e8683ad840..cbcd51852472 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/devinit/priv.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/devinit/priv.h
@@ -3,6 +3,7 @@
 
 #include <subdev/bios.h>
 #include <subdev/bios/pll.h>
+#include <subdev/bios/init.h>
 #include <subdev/clock/pll.h>
 #include <subdev/devinit.h>
 
@@ -12,6 +13,7 @@ struct nouveau_devinit_impl {
 	int  (*pll_set)(struct nouveau_devinit *, u32 type, u32 freq);
 	u64  (*disable)(struct nouveau_devinit *);
 	u32  (*mmio)(struct nouveau_devinit *, u32);
+	int  (*post)(struct nouveau_subdev *, bool);
 };
 
 #define nouveau_devinit_create(p,e,o,d)                                        \
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/base.c b/drivers/gpu/drm/nouveau/core/subdev/fb/base.c
index f009d8a39d9d..c866148c440f 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/base.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/base.c
@@ -23,37 +23,30 @@
  */
 
 #include <subdev/bios.h>
-#include <subdev/bios/bit.h>
+#include <subdev/bios/M0203.h>
 
 #include "priv.h"
 
 int
 nouveau_fb_bios_memtype(struct nouveau_bios *bios)
 {
-	struct bit_entry M;
-	u8 ramcfg;
-
-	ramcfg = (nv_rd32(bios, 0x101000) & 0x0000003c) >> 2;
-	if (!bit_entry(bios, 'M', &M) && M.version == 2 && M.length >= 5) {
-		u16 table   = nv_ro16(bios, M.offset + 3);
-		u8  version = nv_ro08(bios, table + 0);
-		u8  header  = nv_ro08(bios, table + 1);
-		u8  record  = nv_ro08(bios, table + 2);
-		u8  entries = nv_ro08(bios, table + 3);
-		if (table && version == 0x10 && ramcfg < entries) {
-			u16 entry = table + header + (ramcfg * record);
-			switch (nv_ro08(bios, entry) & 0x0f) {
-			case 0: return NV_MEM_TYPE_DDR2;
-			case 1: return NV_MEM_TYPE_DDR3;
-			case 2: return NV_MEM_TYPE_GDDR3;
-			case 3: return NV_MEM_TYPE_GDDR5;
-			default:
-				break;
-			}
-
+	const u8 ramcfg = (nv_rd32(bios, 0x101000) & 0x0000003c) >> 2;
+	struct nvbios_M0203E M0203E;
+	u8 ver, hdr;
+
+	if (nvbios_M0203Em(bios, ramcfg, &ver, &hdr, &M0203E)) {
+		switch (M0203E.type) {
+		case M0203E_TYPE_DDR2 : return NV_MEM_TYPE_DDR2;
+		case M0203E_TYPE_DDR3 : return NV_MEM_TYPE_DDR3;
+		case M0203E_TYPE_GDDR3: return NV_MEM_TYPE_GDDR3;
+		case M0203E_TYPE_GDDR5: return NV_MEM_TYPE_GDDR5;
+		default:
+			nv_warn(bios, "M0203E type %02x\n", M0203E.type);
+			return NV_MEM_TYPE_UNKNOWN;
 		}
 	}
 
+	nv_warn(bios, "M0203E not matched!\n");
 	return NV_MEM_TYPE_UNKNOWN;
 }
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/gddr3.c b/drivers/gpu/drm/nouveau/core/subdev/fb/gddr3.c
new file mode 100644
index 000000000000..d85a25d027ee
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/gddr3.c
@@ -0,0 +1,117 @@
+/*
+ * Copyright 2013 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs <bskeggs@redhat.com>
+ * 	    Roy Spliet <rspliet@eclipso.eu>
+ */
+
+#include <subdev/bios.h>
+#include "priv.h"
+
+struct ramxlat {
+	int id;
+	u8 enc;
+};
+
+static inline int
+ramxlat(const struct ramxlat *xlat, int id)
+{
+	while (xlat->id >= 0) {
+		if (xlat->id == id)
+			return xlat->enc;
+		xlat++;
+	}
+	return -EINVAL;
+}
+
+static const struct ramxlat
+ramgddr3_cl_lo[] = {
+	{ 7, 7 }, { 8, 0 }, { 9, 1 }, { 10, 2 }, { 11, 3 },
+	/* the below are mentioned in some, but not all, gddr3 docs */
+	{ 12, 4 }, { 13, 5 }, { 14, 6 },
+	/* XXX: Per Samsung docs, are these used? They overlap with Qimonda */
+	/* { 4, 4 }, { 5, 5 }, { 6, 6 }, { 12, 8 }, { 13, 9 }, { 14, 10 },
+	 * { 15, 11 }, */
+	{ -1 }
+};
+
+static const struct ramxlat
+ramgddr3_cl_hi[] = {
+	{ 10, 2 }, { 11, 3 }, { 12, 4 }, { 13, 5 }, { 14, 6 }, { 15, 7 },
+	{ 16, 0 }, { 17, 1 },
+	{ -1 }
+};
+
+static const struct ramxlat
+ramgddr3_wr_lo[] = {
+	{ 5, 2 }, { 7, 4 }, { 8, 5 }, { 9, 6 }, { 10, 7 },
+	{ 11, 0 },
+	/* the below are mentioned in some, but not all, gddr3 docs */
+	{ 4, 1 }, { 6, 3 }, { 12, 1 }, { 13 , 2 },
+	{ -1 }
+};
+
+int
+nouveau_gddr3_calc(struct nouveau_ram *ram)
+{
+	int CL, WR, CWL, DLL = 0, ODT = 0, hi;
+
+	switch (ram->next->bios.timing_ver) {
+	case 0x10:
+		CWL = ram->next->bios.timing_10_CWL;
+		CL  = ram->next->bios.timing_10_CL;
+		WR  = ram->next->bios.timing_10_WR;
+		DLL = !ram->next->bios.ramcfg_10_DLLoff;
+		ODT = ram->next->bios.timing_10_ODT;
+		break;
+	case 0x20:
+		CWL = (ram->next->bios.timing[1] & 0x00000f80) >> 7;
+		CL  = (ram->next->bios.timing[1] & 0x0000001f) >> 0;
+		WR  = (ram->next->bios.timing[2] & 0x007f0000) >> 16;
+		/* XXX: Get these values from the VBIOS instead */
+		DLL = !(ram->mr[1] & 0x1);
+		ODT =  (ram->mr[1] & 0x004) >> 2 |
+		       (ram->mr[1] & 0x040) >> 5 |
+		       (ram->mr[1] & 0x200) >> 7;
+		break;
+	default:
+		return -ENOSYS;
+	}
+
+	hi = ram->mr[2] & 0x1;
+	CL  = ramxlat(hi ? ramgddr3_cl_hi : ramgddr3_cl_lo, CL);
+	WR  = ramxlat(ramgddr3_wr_lo, WR);
+	if (CL < 0 || CWL < 1 || CWL > 7 || WR < 0)
+		return -EINVAL;
+
+	ram->mr[0] &= ~0xf74;
+	ram->mr[0] |= (CWL & 0x07) << 9;
+	ram->mr[0] |= (CL & 0x07) << 4;
+	ram->mr[0] |= (CL & 0x08) >> 1;
+
+	ram->mr[1] &= ~0x3fc;
+	ram->mr[1] |= (ODT & 0x03) << 2;
+	ram->mr[1] |= (ODT & 0x03) << 8;
+	ram->mr[1] |= (WR  & 0x03) << 4;
+	ram->mr[1] |= (WR  & 0x04) << 5;
+	ram->mr[1] |= !DLL << 6;
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/priv.h b/drivers/gpu/drm/nouveau/core/subdev/fb/priv.h
index 60322e906dd4..283863f7aa9b 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/priv.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/priv.h
@@ -37,6 +37,7 @@ extern struct nouveau_oclass gm107_ram_oclass;
 
 int nouveau_sddr2_calc(struct nouveau_ram *ram);
 int nouveau_sddr3_calc(struct nouveau_ram *ram);
+int nouveau_gddr3_calc(struct nouveau_ram *ram);
 int nouveau_gddr5_calc(struct nouveau_ram *ram, bool nuts);
 
 #define nouveau_fb_create(p,e,c,d)                                             \
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/ramfuc.h b/drivers/gpu/drm/nouveau/core/subdev/fb/ramfuc.h
index d1fbbe4b00a2..0ac7256443bb 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/ramfuc.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/ramfuc.h
@@ -141,6 +141,20 @@ ramfuc_wait_vblank(struct ramfuc *ram)
 }
 
 static inline void
+ramfuc_train(struct ramfuc *ram)
+{
+	nouveau_memx_train(ram->memx);
+}
+
+static inline int
+ramfuc_train_result(struct nouveau_fb *pfb, u32 *result, u32 rsize)
+{
+	struct nouveau_pwr *ppwr = nouveau_pwr(pfb);
+
+	return nouveau_memx_train_result(ppwr, result, rsize);
+}
+
+static inline void
 ramfuc_block(struct ramfuc *ram)
 {
 	nouveau_memx_block(ram->memx);
@@ -162,6 +176,8 @@ ramfuc_unblock(struct ramfuc *ram)
 #define ram_wait(s,r,m,d,n)  ramfuc_wait(&(s)->base, (r), (m), (d), (n))
 #define ram_nsec(s,n)        ramfuc_nsec(&(s)->base, (n))
 #define ram_wait_vblank(s)   ramfuc_wait_vblank(&(s)->base)
+#define ram_train(s)         ramfuc_train(&(s)->base)
+#define ram_train_result(s,r,l) ramfuc_train_result((s), (r), (l))
 #define ram_block(s)         ramfuc_block(&(s)->base)
 #define ram_unblock(s)       ramfuc_unblock(&(s)->base)
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/ramnva3.c b/drivers/gpu/drm/nouveau/core/subdev/fb/ramnva3.c
index 3601deca0bd5..3b38a538845d 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/ramnva3.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/ramnva3.c
@@ -20,86 +20,512 @@
  * OTHER DEALINGS IN THE SOFTWARE.
  *
  * Authors: Ben Skeggs
+ * 	    Roy Spliet <rspliet@eclipso.eu>
  */
 
 #include <subdev/bios.h>
 #include <subdev/bios/bit.h>
 #include <subdev/bios/pll.h>
 #include <subdev/bios/rammap.h>
+#include <subdev/bios/M0205.h>
 #include <subdev/bios/timing.h>
 
 #include <subdev/clock/nva3.h>
 #include <subdev/clock/pll.h>
 
+#include <subdev/gpio.h>
+
+#include <subdev/timer.h>
+
+#include <engine/fifo.h>
+
 #include <core/option.h>
 
 #include "ramfuc.h"
 
 #include "nv50.h"
 
+/* XXX: Remove when memx gains GPIO support */
+extern int nv50_gpio_location(int line, u32 *reg, u32 *shift);
+
 struct nva3_ramfuc {
 	struct ramfuc base;
+	struct ramfuc_reg r_0x001610;
+	struct ramfuc_reg r_0x001700;
+	struct ramfuc_reg r_0x002504;
 	struct ramfuc_reg r_0x004000;
 	struct ramfuc_reg r_0x004004;
 	struct ramfuc_reg r_0x004018;
 	struct ramfuc_reg r_0x004128;
 	struct ramfuc_reg r_0x004168;
+	struct ramfuc_reg r_0x100080;
 	struct ramfuc_reg r_0x100200;
 	struct ramfuc_reg r_0x100210;
 	struct ramfuc_reg r_0x100220[9];
+	struct ramfuc_reg r_0x100264;
 	struct ramfuc_reg r_0x1002d0;
 	struct ramfuc_reg r_0x1002d4;
 	struct ramfuc_reg r_0x1002dc;
 	struct ramfuc_reg r_0x10053c;
 	struct ramfuc_reg r_0x1005a0;
 	struct ramfuc_reg r_0x1005a4;
+	struct ramfuc_reg r_0x100700;
 	struct ramfuc_reg r_0x100714;
 	struct ramfuc_reg r_0x100718;
 	struct ramfuc_reg r_0x10071c;
+	struct ramfuc_reg r_0x100720;
 	struct ramfuc_reg r_0x100760;
 	struct ramfuc_reg r_0x1007a0;
 	struct ramfuc_reg r_0x1007e0;
+	struct ramfuc_reg r_0x100da0;
 	struct ramfuc_reg r_0x10f804;
 	struct ramfuc_reg r_0x1110e0;
 	struct ramfuc_reg r_0x111100;
 	struct ramfuc_reg r_0x111104;
+	struct ramfuc_reg r_0x1111e0;
+	struct ramfuc_reg r_0x111400;
 	struct ramfuc_reg r_0x611200;
 	struct ramfuc_reg r_mr[4];
+	struct ramfuc_reg r_gpioFBVREF;
+};
+
+struct nva3_ltrain {
+	enum {
+		NVA3_TRAIN_UNKNOWN,
+		NVA3_TRAIN_UNSUPPORTED,
+		NVA3_TRAIN_ONCE,
+		NVA3_TRAIN_EXEC,
+		NVA3_TRAIN_DONE
+	} state;
+	u32 r_100720;
+	u32 r_1111e0;
+	u32 r_111400;
+	struct nouveau_mem *mem;
 };
 
 struct nva3_ram {
 	struct nouveau_ram base;
 	struct nva3_ramfuc fuc;
+	struct nva3_ltrain ltrain;
 };
 
+void
+nva3_link_train_calc(u32 *vals, struct nva3_ltrain *train)
+{
+	int i, lo, hi;
+	u8 median[8], bins[4] = {0, 0, 0, 0}, bin = 0, qty = 0;
+
+	for (i = 0; i < 8; i++) {
+		for (lo = 0; lo < 0x40; lo++) {
+			if (!(vals[lo] & 0x80000000))
+				continue;
+			if (vals[lo] & (0x101 << i))
+				break;
+		}
+
+		if (lo == 0x40)
+			return;
+
+		for (hi = lo + 1; hi < 0x40; hi++) {
+			if (!(vals[lo] & 0x80000000))
+				continue;
+			if (!(vals[hi] & (0x101 << i))) {
+				hi--;
+				break;
+			}
+		}
+
+		median[i] = ((hi - lo) >> 1) + lo;
+		bins[(median[i] & 0xf0) >> 4]++;
+		median[i] += 0x30;
+	}
+
+	/* Find the best value for 0x1111e0 */
+	for (i = 0; i < 4; i++) {
+		if (bins[i] > qty) {
+			bin = i + 3;
+			qty = bins[i];
+		}
+	}
+
+	train->r_100720 = 0;
+	for (i = 0; i < 8; i++) {
+		median[i] = max(median[i], (u8) (bin << 4));
+		median[i] = min(median[i], (u8) ((bin << 4) | 0xf));
+
+		train->r_100720 |= ((median[i] & 0x0f) << (i << 2));
+	}
+
+	train->r_1111e0 = 0x02000000 | (bin * 0x101);
+	train->r_111400 = 0x0;
+}
+
+/*
+ * Link training for (at least) DDR3
+ */
+int
+nva3_link_train(struct nouveau_fb *pfb)
+{
+	struct nouveau_bios *bios = nouveau_bios(pfb);
+	struct nva3_ram *ram = (void *)pfb->ram;
+	struct nouveau_clock *clk = nouveau_clock(pfb);
+	struct nva3_ltrain *train = &ram->ltrain;
+	struct nouveau_device *device = nv_device(pfb);
+	struct nva3_ramfuc *fuc = &ram->fuc;
+	u32 *result, r1700;
+	int ret, i;
+	struct nvbios_M0205T M0205T = { 0 };
+	u8 ver, hdr, cnt, len, snr, ssz;
+	unsigned int clk_current;
+	unsigned long flags;
+	unsigned long *f = &flags;
+
+	if (nouveau_boolopt(device->cfgopt, "NvMemExec", true) != true)
+		return -ENOSYS;
+
+	/* XXX: Multiple partitions? */
+	result = kmalloc(64 * sizeof(u32), GFP_KERNEL);
+	if (!result)
+		return -ENOMEM;
+
+	train->state = NVA3_TRAIN_EXEC;
+
+	/* Clock speeds for training and back */
+	nvbios_M0205Tp(bios, &ver, &hdr, &cnt, &len, &snr, &ssz, &M0205T);
+	if (M0205T.freq == 0)
+		return -ENOENT;
+
+	clk_current = clk->read(clk, nv_clk_src_mem);
+
+	ret = nva3_clock_pre(clk, f);
+	if (ret)
+		goto out;
+
+	/* First: clock up/down */
+	ret = ram->base.calc(pfb, (u32) M0205T.freq * 1000);
+	if (ret)
+		goto out;
+
+	/* Do this *after* calc, eliminates write in script */
+	nv_wr32(pfb, 0x111400, 0x00000000);
+	/* XXX: Magic writes that improve train reliability? */
+	nv_mask(pfb, 0x100674, 0x0000ffff, 0x00000000);
+	nv_mask(pfb, 0x1005e4, 0x0000ffff, 0x00000000);
+	nv_mask(pfb, 0x100b0c, 0x000000ff, 0x00000000);
+	nv_wr32(pfb, 0x100c04, 0x00000400);
+
+	/* Now the training script */
+	r1700 = ram_rd32(fuc, 0x001700);
+
+	ram_mask(fuc, 0x100200, 0x00000800, 0x00000000);
+	ram_wr32(fuc, 0x611200, 0x3300);
+	ram_wait_vblank(fuc);
+	ram_wait(fuc, 0x611200, 0x00000003, 0x00000000, 500000);
+	ram_mask(fuc, 0x001610, 0x00000083, 0x00000003);
+	ram_mask(fuc, 0x100080, 0x00000020, 0x00000000);
+	ram_mask(fuc, 0x10f804, 0x80000000, 0x00000000);
+	ram_wr32(fuc, 0x001700, 0x00000000);
+
+	ram_train(fuc);
+
+	/* Reset */
+	ram_mask(fuc, 0x10f804, 0x80000000, 0x80000000);
+	ram_wr32(fuc, 0x10053c, 0x0);
+	ram_wr32(fuc, 0x100720, train->r_100720);
+	ram_wr32(fuc, 0x1111e0, train->r_1111e0);
+	ram_wr32(fuc, 0x111400, train->r_111400);
+	ram_nuke(fuc, 0x100080);
+	ram_mask(fuc, 0x100080, 0x00000020, 0x00000020);
+	ram_nsec(fuc, 1000);
+
+	ram_wr32(fuc, 0x001700, r1700);
+	ram_mask(fuc, 0x001610, 0x00000083, 0x00000080);
+	ram_wr32(fuc, 0x611200, 0x3330);
+	ram_mask(fuc, 0x100200, 0x00000800, 0x00000800);
+
+	ram_exec(fuc, true);
+
+	ram->base.calc(pfb, clk_current);
+	ram_exec(fuc, true);
+
+	/* Post-processing, avoids flicker */
+	nv_mask(pfb, 0x616308, 0x10, 0x10);
+	nv_mask(pfb, 0x616b08, 0x10, 0x10);
+
+	nva3_clock_post(clk, f);
+
+	ram_train_result(pfb, result, 64);
+	for (i = 0; i < 64; i++)
+		nv_debug(pfb, "Train: %08x", result[i]);
+	nva3_link_train_calc(result, train);
+
+	nv_debug(pfb, "Train: %08x %08x %08x", train->r_100720,
+			train->r_1111e0, train->r_111400);
+
+	kfree(result);
+
+	train->state = NVA3_TRAIN_DONE;
+
+	return ret;
+
+out:
+	if(ret == -EBUSY)
+		f = NULL;
+
+	train->state = NVA3_TRAIN_UNSUPPORTED;
+
+	nva3_clock_post(clk, f);
+	return ret;
+}
+
+int
+nva3_link_train_init(struct nouveau_fb *pfb)
+{
+	static const u32 pattern[16] = {
+		0xaaaaaaaa, 0xcccccccc, 0xdddddddd, 0xeeeeeeee,
+		0x00000000, 0x11111111, 0x44444444, 0xdddddddd,
+		0x33333333, 0x55555555, 0x77777777, 0x66666666,
+		0x99999999, 0x88888888, 0xeeeeeeee, 0xbbbbbbbb,
+	};
+	struct nouveau_bios *bios = nouveau_bios(pfb);
+	struct nva3_ram *ram = (void *)pfb->ram;
+	struct nva3_ltrain *train = &ram->ltrain;
+	struct nouveau_mem *mem;
+	struct nvbios_M0205E M0205E;
+	u8 ver, hdr, cnt, len;
+	u32 r001700;
+	int ret, i = 0;
+
+	train->state = NVA3_TRAIN_UNSUPPORTED;
+
+	/* We support type "5"
+	 * XXX: training pattern table appears to be unused for this routine */
+	if (!nvbios_M0205Ep(bios, i, &ver, &hdr, &cnt, &len, &M0205E))
+		return -ENOENT;
+
+	if (M0205E.type != 5)
+		return 0;
+
+	train->state = NVA3_TRAIN_ONCE;
+
+	ret = pfb->ram->get(pfb, 0x8000, 0x10000, 0, 0x800, &ram->ltrain.mem);
+	if (ret)
+		return ret;
+
+	mem = ram->ltrain.mem;
+
+	nv_wr32(pfb, 0x100538, 0x10000000 | (mem->offset >> 16));
+	nv_wr32(pfb, 0x1005a8, 0x0000ffff);
+	nv_mask(pfb, 0x10f800, 0x00000001, 0x00000001);
+
+	for (i = 0; i < 0x30; i++) {
+		nv_wr32(pfb, 0x10f8c0, (i << 8) | i);
+		nv_wr32(pfb, 0x10f900, pattern[i % 16]);
+	}
+
+	for (i = 0; i < 0x30; i++) {
+		nv_wr32(pfb, 0x10f8e0, (i << 8) | i);
+		nv_wr32(pfb, 0x10f920, pattern[i % 16]);
+	}
+
+	/* And upload the pattern */
+	r001700 = nv_rd32(pfb, 0x1700);
+	nv_wr32(pfb, 0x1700, mem->offset >> 16);
+	for (i = 0; i < 16; i++)
+		nv_wr32(pfb, 0x700000 + (i << 2), pattern[i]);
+	for (i = 0; i < 16; i++)
+		nv_wr32(pfb, 0x700100 + (i << 2), pattern[i]);
+	nv_wr32(pfb, 0x1700, r001700);
+
+	train->r_100720 = nv_rd32(pfb, 0x100720);
+	train->r_1111e0 = nv_rd32(pfb, 0x1111e0);
+	train->r_111400 = nv_rd32(pfb, 0x111400);
+
+	return 0;
+}
+
+void
+nva3_link_train_fini(struct nouveau_fb *pfb)
+{
+	struct nva3_ram *ram = (void *)pfb->ram;
+
+	if (ram->ltrain.mem)
+		pfb->ram->put(pfb, &ram->ltrain.mem);
+}
+
+/*
+ * RAM reclocking
+ */
+#define T(t) cfg->timing_10_##t
+static int
+nva3_ram_timing_calc(struct nouveau_fb *pfb, u32 *timing)
+{
+	struct nva3_ram *ram = (void *)pfb->ram;
+	struct nvbios_ramcfg *cfg = &ram->base.target.bios;
+	int tUNK_base, tUNK_40_0, prevCL;
+	u32 cur2, cur3, cur7, cur8;
+
+	cur2 = nv_rd32(pfb, 0x100228);
+	cur3 = nv_rd32(pfb, 0x10022c);
+	cur7 = nv_rd32(pfb, 0x10023c);
+	cur8 = nv_rd32(pfb, 0x100240);
+
+
+	switch ((!T(CWL)) * ram->base.type) {
+	case NV_MEM_TYPE_DDR2:
+		T(CWL) = T(CL) - 1;
+		break;
+	case NV_MEM_TYPE_GDDR3:
+		T(CWL) = ((cur2 & 0xff000000) >> 24) + 1;
+		break;
+	}
+
+	prevCL = (cur3 & 0x000000ff) + 1;
+	tUNK_base = ((cur7 & 0x00ff0000) >> 16) - prevCL;
+
+	timing[0] = (T(RP) << 24 | T(RAS) << 16 | T(RFC) << 8 | T(RC));
+	timing[1] = (T(WR) + 1 + T(CWL)) << 24 |
+		    max_t(u8,T(18), 1) << 16 |
+		    (T(WTR) + 1 + T(CWL)) << 8 |
+		    (5 + T(CL) - T(CWL));
+	timing[2] = (T(CWL) - 1) << 24 |
+		    (T(RRD) << 16) |
+		    (T(RCDWR) << 8) |
+		    T(RCDRD);
+	timing[3] = (cur3 & 0x00ff0000) |
+		    (0x30 + T(CL)) << 24 |
+		    (0xb + T(CL)) << 8 |
+		    (T(CL) - 1);
+	timing[4] = T(20) << 24 |
+		    T(21) << 16 |
+		    T(13) << 8 |
+		    T(13);
+	timing[5] = T(RFC) << 24 |
+		    max_t(u8,T(RCDRD), T(RCDWR)) << 16 |
+		    max_t(u8, (T(CWL) + 6), (T(CL) + 2)) << 8 |
+		    T(RP);
+	timing[6] = (0x5a + T(CL)) << 16 |
+		    max_t(u8, 1, (6 - T(CL) + T(CWL))) << 8 |
+		    (0x50 + T(CL) - T(CWL));
+	timing[7] = (cur7 & 0xff000000) |
+		    ((tUNK_base + T(CL)) << 16) |
+		    0x202;
+	timing[8] = cur8 & 0xffffff00;
+
+	switch (ram->base.type) {
+	case NV_MEM_TYPE_DDR2:
+	case NV_MEM_TYPE_GDDR3:
+		tUNK_40_0 = prevCL - (cur8 & 0xff);
+		if (tUNK_40_0 > 0)
+			timing[8] |= T(CL);
+		break;
+	default:
+		break;
+	}
+
+	nv_debug(pfb, "Entry: 220: %08x %08x %08x %08x\n",
+			timing[0], timing[1], timing[2], timing[3]);
+	nv_debug(pfb, "  230: %08x %08x %08x %08x\n",
+			timing[4], timing[5], timing[6], timing[7]);
+	nv_debug(pfb, "  240: %08x\n", timing[8]);
+	return 0;
+}
+#undef T
+
+static void
+nouveau_sddr2_dll_reset(struct nva3_ramfuc *fuc)
+{
+	ram_mask(fuc, mr[0], 0x100, 0x100);
+	ram_nsec(fuc, 1000);
+	ram_mask(fuc, mr[0], 0x100, 0x000);
+	ram_nsec(fuc, 1000);
+}
+
+static void
+nouveau_sddr3_dll_disable(struct nva3_ramfuc *fuc, u32 *mr)
+{
+	u32 mr1_old = ram_rd32(fuc, mr[1]);
+
+	if (!(mr1_old & 0x1)) {
+		ram_wr32(fuc, 0x1002d4, 0x00000001);
+		ram_wr32(fuc, mr[1], mr[1]);
+		ram_nsec(fuc, 1000);
+	}
+}
+
+static void
+nouveau_gddr3_dll_disable(struct nva3_ramfuc *fuc, u32 *mr)
+{
+	u32 mr1_old = ram_rd32(fuc, mr[1]);
+
+	if (!(mr1_old & 0x40)) {
+		ram_wr32(fuc, mr[1], mr[1]);
+		ram_nsec(fuc, 1000);
+	}
+}
+
+static void
+nva3_ram_lock_pll(struct nva3_ramfuc *fuc, struct nva3_clock_info *mclk)
+{
+	ram_wr32(fuc, 0x004004, mclk->pll);
+	ram_mask(fuc, 0x004000, 0x00000001, 0x00000001);
+	ram_mask(fuc, 0x004000, 0x00000010, 0x00000000);
+	ram_wait(fuc, 0x004000, 0x00020000, 0x00020000, 64000);
+	ram_mask(fuc, 0x004000, 0x00000010, 0x00000010);
+}
+
+static void
+nva3_ram_fbvref(struct nva3_ramfuc *fuc, u32 val)
+{
+	struct nouveau_gpio *gpio = nouveau_gpio(fuc->base.pfb);
+	struct dcb_gpio_func func;
+	u32 reg, sh, gpio_val;
+	int ret;
+
+	if (gpio->get(gpio, 0, 0x2e, DCB_GPIO_UNUSED) != val) {
+		ret = gpio->find(gpio, 0, 0x2e, DCB_GPIO_UNUSED, &func);
+		if (ret)
+			return;
+
+		nv50_gpio_location(func.line, &reg, &sh);
+		gpio_val = ram_rd32(fuc, gpioFBVREF);
+		if (gpio_val & (8 << sh))
+			val = !val;
+
+		ram_mask(fuc, gpioFBVREF, (0x3 << sh), ((val | 0x2) << sh));
+		ram_nsec(fuc, 20000);
+	}
+}
+
 static int
 nva3_ram_calc(struct nouveau_fb *pfb, u32 freq)
 {
 	struct nouveau_bios *bios = nouveau_bios(pfb);
 	struct nva3_ram *ram = (void *)pfb->ram;
 	struct nva3_ramfuc *fuc = &ram->fuc;
+	struct nva3_ltrain *train = &ram->ltrain;
 	struct nva3_clock_info mclk;
 	struct nouveau_ram_data *next;
 	u8  ver, hdr, cnt, len, strap;
 	u32 data;
-	u32 r004018, r100760, ctrl;
+	u32 r004018, r100760, r100da0, r111100, ctrl;
 	u32 unk714, unk718, unk71c;
 	int ret, i;
+	u32 timing[9];
+	bool pll2pll;
 
 	next = &ram->base.target;
 	next->freq = freq;
 	ram->base.next = next;
 
+	if (ram->ltrain.state == NVA3_TRAIN_ONCE)
+		nva3_link_train(pfb);
+
 	/* lookup memory config data relevant to the target frequency */
 	i = 0;
-	while ((data = nvbios_rammapEp(bios, i++, &ver, &hdr, &cnt, &len,
-				      &next->bios))) {
-		if (freq / 1000 >= next->bios.rammap_min &&
-		    freq / 1000 <= next->bios.rammap_max)
-			break;
-	}
-
-	if (!data || ver != 0x10 || hdr < 0x0e) {
+	data = nvbios_rammapEm(bios, freq / 1000, &ver, &hdr, &cnt, &len,
+				      &next->bios);
+	if (!data || ver != 0x10 || hdr < 0x05) {
 		nv_error(pfb, "invalid/missing rammap entry\n");
 		return -EINVAL;
 	}
@@ -113,7 +539,7 @@ nva3_ram_calc(struct nouveau_fb *pfb, u32 freq)
 
 	data = nvbios_rammapSp(bios, data, ver, hdr, cnt, len, strap,
 			       &ver, &hdr, &next->bios);
-	if (!data || ver != 0x10 || hdr < 0x0e) {
+	if (!data || ver != 0x10 || hdr < 0x09) {
 		nv_error(pfb, "invalid/missing ramcfg entry\n");
 		return -EINVAL;
 	}
@@ -123,7 +549,7 @@ nva3_ram_calc(struct nouveau_fb *pfb, u32 freq)
 		data = nvbios_timingEp(bios, next->bios.ramcfg_timing,
 				       &ver, &hdr, &cnt, &len,
 				       &next->bios);
-		if (!data || ver != 0x10 || hdr < 0x19) {
+		if (!data || ver != 0x10 || hdr < 0x17) {
 			nv_error(pfb, "invalid/missing timing entry\n");
 			return -EINVAL;
 		}
@@ -135,53 +561,99 @@ nva3_ram_calc(struct nouveau_fb *pfb, u32 freq)
 		return ret;
 	}
 
+	nva3_ram_timing_calc(pfb, timing);
+
 	ret = ram_init(fuc, pfb);
 	if (ret)
 		return ret;
 
+	/* Determine ram-specific MR values */
+	ram->base.mr[0] = ram_rd32(fuc, mr[0]);
+	ram->base.mr[1] = ram_rd32(fuc, mr[1]);
+	ram->base.mr[2] = ram_rd32(fuc, mr[2]);
+
+	switch (ram->base.type) {
+	case NV_MEM_TYPE_DDR2:
+		ret = nouveau_sddr2_calc(&ram->base);
+		break;
+	case NV_MEM_TYPE_DDR3:
+		ret = nouveau_sddr3_calc(&ram->base);
+		break;
+	case NV_MEM_TYPE_GDDR3:
+		ret = nouveau_gddr3_calc(&ram->base);
+		break;
+	default:
+		ret = -ENOSYS;
+		break;
+	}
+
+	if (ret)
+		return ret;
+
 	/* XXX: where the fuck does 750MHz come from? */
 	if (freq <= 750000) {
 		r004018 = 0x10000000;
 		r100760 = 0x22222222;
+		r100da0 = 0x00000010;
 	} else {
 		r004018 = 0x00000000;
 		r100760 = 0x00000000;
+		r100da0 = 0x00000000;
 	}
 
+	if (!next->bios.ramcfg_10_DLLoff)
+		r004018 |= 0x00004000;
+
+	/* pll2pll requires to switch to a safe clock first */
 	ctrl = ram_rd32(fuc, 0x004000);
-	if (ctrl & 0x00000008) {
-		if (mclk.pll) {
-			ram_mask(fuc, 0x004128, 0x00000101, 0x00000101);
-			ram_wr32(fuc, 0x004004, mclk.pll);
-			ram_wr32(fuc, 0x004000, (ctrl |= 0x00000001));
-			ram_wr32(fuc, 0x004000, (ctrl &= 0xffffffef));
-			ram_wait(fuc, 0x004000, 0x00020000, 0x00020000, 64000);
-			ram_wr32(fuc, 0x004000, (ctrl |= 0x00000010));
-			ram_wr32(fuc, 0x004018, 0x00005000 | r004018);
-			ram_wr32(fuc, 0x004000, (ctrl |= 0x00000004));
-		}
-	} else {
-		u32 ssel = 0x00000101;
-		if (mclk.clk)
-			ssel |= mclk.clk;
-		else
-			ssel |= 0x00080000; /* 324MHz, shouldn't matter... */
-		ram_mask(fuc, 0x004168, 0x003f3141, ctrl);
-	}
+	pll2pll = (!(ctrl & 0x00000008)) && mclk.pll;
 
+	/* Pre, NVIDIA does this outside the script */
 	if (next->bios.ramcfg_10_02_10) {
 		ram_mask(fuc, 0x111104, 0x00000600, 0x00000000);
 	} else {
 		ram_mask(fuc, 0x111100, 0x40000000, 0x40000000);
 		ram_mask(fuc, 0x111104, 0x00000180, 0x00000000);
 	}
+	/* Always disable this bit during reclock */
+	ram_mask(fuc, 0x100200, 0x00000800, 0x00000000);
+
+	/* If switching from non-pll to pll, lock before disabling FB */
+	if (mclk.pll && !pll2pll) {
+		ram_mask(fuc, 0x004128, 0x003f3141, mclk.clk | 0x00000101);
+		nva3_ram_lock_pll(fuc, &mclk);
+	}
+
+	/* Start with disabling some CRTCs and PFIFO? */
+	ram_wait_vblank(fuc);
+	ram_wr32(fuc, 0x611200, 0x3300);
+	ram_mask(fuc, 0x002504, 0x1, 0x1);
+	ram_nsec(fuc, 10000);
+	ram_wait(fuc, 0x002504, 0x10, 0x10, 20000); /* XXX: or longer? */
+	ram_block(fuc);
+	ram_nsec(fuc, 2000);
+
+	if (!next->bios.ramcfg_10_02_10) {
+		if (ram->base.type == NV_MEM_TYPE_GDDR3)
+			ram_mask(fuc, 0x111100, 0x04020000, 0x00020000);
+		else
+			ram_mask(fuc, 0x111100, 0x04020000, 0x04020000);
+	}
+
+	/* If we're disabling the DLL, do it now */
+	switch (next->bios.ramcfg_10_DLLoff * ram->base.type) {
+	case NV_MEM_TYPE_DDR3:
+		nouveau_sddr3_dll_disable(fuc, ram->base.mr);
+		break;
+	case NV_MEM_TYPE_GDDR3:
+		nouveau_gddr3_dll_disable(fuc, ram->base.mr);
+		break;
+	}
 
-	if (!next->bios.rammap_10_04_02)
-		ram_mask(fuc, 0x100200, 0x00000800, 0x00000000);
-	ram_wr32(fuc, 0x611200, 0x00003300);
-	if (!next->bios.ramcfg_10_02_10)
-		ram_wr32(fuc, 0x111100, 0x4c020000); /*XXX*/
+	if (fuc->r_gpioFBVREF.addr && next->bios.timing_10_ODT)
+		nva3_ram_fbvref(fuc, 0);
 
+	/* Brace RAM for impact */
 	ram_wr32(fuc, 0x1002d4, 0x00000001);
 	ram_wr32(fuc, 0x1002d0, 0x00000001);
 	ram_wr32(fuc, 0x1002d0, 0x00000001);
@@ -189,24 +661,38 @@ nva3_ram_calc(struct nouveau_fb *pfb, u32 freq)
 	ram_wr32(fuc, 0x1002dc, 0x00000001);
 	ram_nsec(fuc, 2000);
 
-	ctrl = ram_rd32(fuc, 0x004000);
-	if (!(ctrl & 0x00000008) && mclk.pll) {
-		ram_wr32(fuc, 0x004000, (ctrl |=  0x00000008));
+	if (nv_device(pfb)->chipset == 0xa3 && freq <= 500000)
+		ram_mask(fuc, 0x100700, 0x00000006, 0x00000006);
+
+	/* Fiddle with clocks */
+	/* There's 4 scenario's
+	 * pll->pll: first switch to a 324MHz clock, set up new PLL, switch
+	 * clk->pll: Set up new PLL, switch
+	 * pll->clk: Set up clock, switch
+	 * clk->clk: Overwrite ctrl and other bits, switch */
+
+	/* Switch to regular clock - 324MHz */
+	if (pll2pll) {
+		ram_mask(fuc, 0x004000, 0x00000004, 0x00000004);
+		ram_mask(fuc, 0x004168, 0x003f3141, 0x00083101);
+		ram_mask(fuc, 0x004000, 0x00000008, 0x00000008);
 		ram_mask(fuc, 0x1110e0, 0x00088000, 0x00088000);
 		ram_wr32(fuc, 0x004018, 0x00001000);
-		ram_wr32(fuc, 0x004000, (ctrl &= ~0x00000001));
-		ram_wr32(fuc, 0x004004, mclk.pll);
-		ram_wr32(fuc, 0x004000, (ctrl |=  0x00000001));
-		udelay(64);
-		ram_wr32(fuc, 0x004018, 0x00005000 | r004018);
-		udelay(20);
-	} else
-	if (!mclk.pll) {
-		ram_mask(fuc, 0x004168, 0x003f3040, mclk.clk);
-		ram_wr32(fuc, 0x004000, (ctrl |= 0x00000008));
+		nva3_ram_lock_pll(fuc, &mclk);
+	}
+
+	if (mclk.pll) {
+		ram_mask(fuc, 0x004000, 0x00000105, 0x00000105);
+		ram_wr32(fuc, 0x004018, 0x00001000 | r004018);
+		ram_wr32(fuc, 0x100da0, r100da0);
+	} else {
+		ram_mask(fuc, 0x004168, 0x003f3141, mclk.clk | 0x00000101);
+		ram_mask(fuc, 0x004000, 0x00000108, 0x00000008);
 		ram_mask(fuc, 0x1110e0, 0x00088000, 0x00088000);
-		ram_wr32(fuc, 0x004018, 0x0000d000 | r004018);
+		ram_wr32(fuc, 0x004018, 0x00009000 | r004018);
+		ram_wr32(fuc, 0x100da0, r100da0);
 	}
+	ram_nsec(fuc, 20000);
 
 	if (next->bios.rammap_10_04_08) {
 		ram_wr32(fuc, 0x1005a0, next->bios.ramcfg_10_06 << 16 |
@@ -220,6 +706,12 @@ nva3_ram_calc(struct nouveau_fb *pfb, u32 freq)
 					0x80000000);
 		ram_mask(fuc, 0x10053c, 0x00001000, 0x00000000);
 	} else {
+		if (train->state == NVA3_TRAIN_DONE) {
+			ram_wr32(fuc, 0x100080, 0x1020);
+			ram_mask(fuc, 0x111400, 0xffffffff, train->r_111400);
+			ram_mask(fuc, 0x1111e0, 0xffffffff, train->r_1111e0);
+			ram_mask(fuc, 0x100720, 0xffffffff, train->r_100720);
+		}
 		ram_mask(fuc, 0x10053c, 0x00001000, 0x00001000);
 		ram_mask(fuc, 0x10f804, 0x80000000, 0x00000000);
 		ram_mask(fuc, 0x100760, 0x22222222, r100760);
@@ -227,65 +719,131 @@ nva3_ram_calc(struct nouveau_fb *pfb, u32 freq)
 		ram_mask(fuc, 0x1007e0, 0x22222222, r100760);
 	}
 
+	if (nv_device(pfb)->chipset == 0xa3 && freq > 500000) {
+		ram_mask(fuc, 0x100700, 0x00000006, 0x00000000);
+	}
+
+	/* Final switch */
 	if (mclk.pll) {
 		ram_mask(fuc, 0x1110e0, 0x00088000, 0x00011000);
-		ram_wr32(fuc, 0x004000, (ctrl &= ~0x00000008));
+		ram_mask(fuc, 0x004000, 0x00000008, 0x00000000);
 	}
 
-	/*XXX: LEAVE */
 	ram_wr32(fuc, 0x1002dc, 0x00000000);
 	ram_wr32(fuc, 0x1002d4, 0x00000001);
 	ram_wr32(fuc, 0x100210, 0x80000000);
-	ram_nsec(fuc, 1000);
-	ram_nsec(fuc, 1000);
+	ram_nsec(fuc, 2000);
 
-	ram_mask(fuc, mr[2], 0x00000000, 0x00000000);
-	ram_nsec(fuc, 1000);
-	ram_nuke(fuc, mr[0]);
-	ram_mask(fuc, mr[0], 0x00000000, 0x00000000);
-	ram_nsec(fuc, 1000);
+	/* Set RAM MR parameters and timings */
+	for (i = 2; i >= 0; i--) {
+		if (ram_rd32(fuc, mr[i]) != ram->base.mr[i]) {
+			ram_wr32(fuc, mr[i], ram->base.mr[i]);
+			ram_nsec(fuc, 1000);
+		}
+	}
 
-	ram_mask(fuc, 0x100220[3], 0x00000000, 0x00000000);
-	ram_mask(fuc, 0x100220[1], 0x00000000, 0x00000000);
-	ram_mask(fuc, 0x100220[6], 0x00000000, 0x00000000);
-	ram_mask(fuc, 0x100220[7], 0x00000000, 0x00000000);
-	ram_mask(fuc, 0x100220[2], 0x00000000, 0x00000000);
-	ram_mask(fuc, 0x100220[4], 0x00000000, 0x00000000);
-	ram_mask(fuc, 0x100220[5], 0x00000000, 0x00000000);
-	ram_mask(fuc, 0x100220[0], 0x00000000, 0x00000000);
-	ram_mask(fuc, 0x100220[8], 0x00000000, 0x00000000);
+	ram_wr32(fuc, 0x100220[3], timing[3]);
+	ram_wr32(fuc, 0x100220[1], timing[1]);
+	ram_wr32(fuc, 0x100220[6], timing[6]);
+	ram_wr32(fuc, 0x100220[7], timing[7]);
+	ram_wr32(fuc, 0x100220[2], timing[2]);
+	ram_wr32(fuc, 0x100220[4], timing[4]);
+	ram_wr32(fuc, 0x100220[5], timing[5]);
+	ram_wr32(fuc, 0x100220[0], timing[0]);
+	ram_wr32(fuc, 0x100220[8], timing[8]);
 
+	/* Misc */
 	ram_mask(fuc, 0x100200, 0x00001000, !next->bios.ramcfg_10_02_08 << 12);
 
-	unk714 = ram_rd32(fuc, 0x100714) & ~0xf0000010;
-	unk718 = ram_rd32(fuc, 0x100718) & ~0x00000100;
-	unk71c = ram_rd32(fuc, 0x10071c) & ~0x00000100;
+	/* XXX: A lot of "chipset"/"ram type" specific stuff...? */
+	unk714  = ram_rd32(fuc, 0x100714) & ~0xf0000130;
+	unk718  = ram_rd32(fuc, 0x100718) & ~0x00000100;
+	unk71c  = ram_rd32(fuc, 0x10071c) & ~0x00000100;
+	r111100 = ram_rd32(fuc, 0x111100) & ~0x3a800000;
+
+	if (next->bios.ramcfg_10_02_04) {
+		switch (ram->base.type) {
+		case NV_MEM_TYPE_DDR3:
+			if (nv_device(pfb)->chipset != 0xa8)
+				r111100 |= 0x00000004;
+			/* no break */
+		case NV_MEM_TYPE_DDR2:
+			r111100 |= 0x08000000;
+			break;
+		default:
+			break;
+		}
+	} else {
+		switch (ram->base.type) {
+		case NV_MEM_TYPE_DDR2:
+			r111100 |= 0x1a800000;
+			unk714  |= 0x00000010;
+			break;
+		case NV_MEM_TYPE_DDR3:
+			if (nv_device(pfb)->chipset == 0xa8) {
+				r111100 |=  0x08000000;
+			} else {
+				r111100 &= ~0x00000004;
+				r111100 |=  0x12800000;
+			}
+			unk714  |= 0x00000010;
+			break;
+		case NV_MEM_TYPE_GDDR3:
+			r111100 |= 0x30000000;
+			unk714  |= 0x00000020;
+			break;
+		default:
+			break;
+		}
+	}
+
+	unk714 |= (next->bios.ramcfg_10_04_01) << 8;
+
 	if (next->bios.ramcfg_10_02_20)
 		unk714 |= 0xf0000000;
-	if (!next->bios.ramcfg_10_02_04)
-		unk714 |= 0x00000010;
-	ram_wr32(fuc, 0x100714, unk714);
-
+	if (next->bios.ramcfg_10_02_02)
+		unk718 |= 0x00000100;
 	if (next->bios.ramcfg_10_02_01)
 		unk71c |= 0x00000100;
-	ram_wr32(fuc, 0x10071c, unk71c);
+	if (next->bios.timing_10_24 != 0xff) {
+		unk718 &= ~0xf0000000;
+		unk718 |= next->bios.timing_10_24 << 28;
+	}
+	if (next->bios.ramcfg_10_02_10)
+		r111100 &= ~0x04020000;
 
-	if (next->bios.ramcfg_10_02_02)
-		unk718 |= 0x00000100;
-	ram_wr32(fuc, 0x100718, unk718);
+	ram_mask(fuc, 0x100714, 0xffffffff, unk714);
+	ram_mask(fuc, 0x10071c, 0xffffffff, unk71c);
+	ram_mask(fuc, 0x100718, 0xffffffff, unk718);
+	ram_mask(fuc, 0x111100, 0xffffffff, r111100);
 
-	if (next->bios.ramcfg_10_02_10)
-		ram_wr32(fuc, 0x111100, 0x48000000); /*XXX*/
+	if (fuc->r_gpioFBVREF.addr && !next->bios.timing_10_ODT)
+		nva3_ram_fbvref(fuc, 1);
 
-	ram_mask(fuc, mr[0], 0x100, 0x100);
-	ram_nsec(fuc, 1000);
-	ram_mask(fuc, mr[0], 0x100, 0x000);
-	ram_nsec(fuc, 1000);
+	/* Reset DLL */
+	if (!next->bios.ramcfg_10_DLLoff)
+		nouveau_sddr2_dll_reset(fuc);
 
-	ram_nsec(fuc, 2000);
-	ram_nsec(fuc, 12000);
+	if (ram->base.type == NV_MEM_TYPE_GDDR3) {
+		ram_nsec(fuc, 31000);
+	} else {
+		ram_nsec(fuc, 14000);
+	}
+
+	if (ram->base.type == NV_MEM_TYPE_DDR3) {
+		ram_wr32(fuc, 0x100264, 0x1);
+		ram_nsec(fuc, 2000);
+	}
 
-	ram_wr32(fuc, 0x611200, 0x00003330);
+	ram_nuke(fuc, 0x100700);
+	ram_mask(fuc, 0x100700, 0x01000000, 0x01000000);
+	ram_mask(fuc, 0x100700, 0x01000000, 0x00000000);
+
+	/* Re-enable FB */
+	ram_unblock(fuc);
+	ram_wr32(fuc, 0x611200, 0x3330);
+
+	/* Post fiddlings */
 	if (next->bios.rammap_10_04_02)
 		ram_mask(fuc, 0x100200, 0x00000800, 0x00000800);
 	if (next->bios.ramcfg_10_02_10) {
@@ -313,7 +871,22 @@ nva3_ram_prog(struct nouveau_fb *pfb)
 	struct nouveau_device *device = nv_device(pfb);
 	struct nva3_ram *ram = (void *)pfb->ram;
 	struct nva3_ramfuc *fuc = &ram->fuc;
-	ram_exec(fuc, nouveau_boolopt(device->cfgopt, "NvMemExec", true));
+	bool exec = nouveau_boolopt(device->cfgopt, "NvMemExec", true);
+
+	if (exec) {
+		nv_mask(pfb, 0x001534, 0x2, 0x2);
+
+		ram_exec(fuc, true);
+
+		/* Post-processing, avoids flicker */
+		nv_mask(pfb, 0x002504, 0x1, 0x0);
+		nv_mask(pfb, 0x001534, 0x2, 0x0);
+
+		nv_mask(pfb, 0x616308, 0x10, 0x10);
+		nv_mask(pfb, 0x616b08, 0x10, 0x10);
+	} else {
+		ram_exec(fuc, false);
+	}
 	return 0;
 }
 
@@ -330,38 +903,24 @@ nva3_ram_init(struct nouveau_object *object)
 {
 	struct nouveau_fb *pfb = (void *)object->parent;
 	struct nva3_ram   *ram = (void *)object;
-	int ret, i;
+	int ret;
 
 	ret = nouveau_ram_init(&ram->base);
 	if (ret)
 		return ret;
 
-	/* prepare for ddr link training, and load training patterns */
-	switch (ram->base.type) {
-	case NV_MEM_TYPE_DDR3: {
-		if (nv_device(pfb)->chipset == 0xa8) {
-			static const u32 pattern[16] = {
-				0xaaaaaaaa, 0xcccccccc, 0xdddddddd, 0xeeeeeeee,
-				0x00000000, 0x11111111, 0x44444444, 0xdddddddd,
-				0x33333333, 0x55555555, 0x77777777, 0x66666666,
-				0x99999999, 0x88888888, 0xeeeeeeee, 0xbbbbbbbb,
-			};
-
-			nv_wr32(pfb, 0x100538, 0x10001ff6); /*XXX*/
-			nv_wr32(pfb, 0x1005a8, 0x0000ffff);
-			nv_mask(pfb, 0x10f800, 0x00000001, 0x00000001);
-			for (i = 0; i < 0x30; i++) {
-				nv_wr32(pfb, 0x10f8c0, (i << 8) | i);
-				nv_wr32(pfb, 0x10f8e0, (i << 8) | i);
-				nv_wr32(pfb, 0x10f900, pattern[i % 16]);
-				nv_wr32(pfb, 0x10f920, pattern[i % 16]);
-			}
-		}
-	}
-		break;
-	default:
-		break;
-	}
+	nva3_link_train_init(pfb);
+
+	return 0;
+}
+
+static int
+nva3_ram_fini(struct nouveau_object *object, bool suspend)
+{
+	struct nouveau_fb *pfb = (void *)object->parent;
+
+	if (!suspend)
+		nva3_link_train_fini(pfb);
 
 	return 0;
 }
@@ -371,8 +930,12 @@ nva3_ram_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	      struct nouveau_oclass *oclass, void *data, u32 datasize,
 	      struct nouveau_object **pobject)
 {
+	struct nouveau_fb *pfb = nouveau_fb(parent);
+	struct nouveau_gpio *gpio = nouveau_gpio(pfb);
+	struct dcb_gpio_func func;
 	struct nva3_ram *ram;
 	int ret, i;
+	u32 reg, shift;
 
 	ret = nv50_ram_create(parent, engine, oclass, &ram);
 	*pobject = nv_object(ram);
@@ -380,7 +943,9 @@ nva3_ram_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 		return ret;
 
 	switch (ram->base.type) {
+	case NV_MEM_TYPE_DDR2:
 	case NV_MEM_TYPE_DDR3:
+	case NV_MEM_TYPE_GDDR3:
 		ram->base.calc = nva3_ram_calc;
 		ram->base.prog = nva3_ram_prog;
 		ram->base.tidy = nva3_ram_tidy;
@@ -390,31 +955,41 @@ nva3_ram_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 		return 0;
 	}
 
+	ram->fuc.r_0x001610 = ramfuc_reg(0x001610);
+	ram->fuc.r_0x001700 = ramfuc_reg(0x001700);
+	ram->fuc.r_0x002504 = ramfuc_reg(0x002504);
 	ram->fuc.r_0x004000 = ramfuc_reg(0x004000);
 	ram->fuc.r_0x004004 = ramfuc_reg(0x004004);
 	ram->fuc.r_0x004018 = ramfuc_reg(0x004018);
 	ram->fuc.r_0x004128 = ramfuc_reg(0x004128);
 	ram->fuc.r_0x004168 = ramfuc_reg(0x004168);
+	ram->fuc.r_0x100080 = ramfuc_reg(0x100080);
 	ram->fuc.r_0x100200 = ramfuc_reg(0x100200);
 	ram->fuc.r_0x100210 = ramfuc_reg(0x100210);
 	for (i = 0; i < 9; i++)
 		ram->fuc.r_0x100220[i] = ramfuc_reg(0x100220 + (i * 4));
+	ram->fuc.r_0x100264 = ramfuc_reg(0x100264);
 	ram->fuc.r_0x1002d0 = ramfuc_reg(0x1002d0);
 	ram->fuc.r_0x1002d4 = ramfuc_reg(0x1002d4);
 	ram->fuc.r_0x1002dc = ramfuc_reg(0x1002dc);
 	ram->fuc.r_0x10053c = ramfuc_reg(0x10053c);
 	ram->fuc.r_0x1005a0 = ramfuc_reg(0x1005a0);
 	ram->fuc.r_0x1005a4 = ramfuc_reg(0x1005a4);
+	ram->fuc.r_0x100700 = ramfuc_reg(0x100700);
 	ram->fuc.r_0x100714 = ramfuc_reg(0x100714);
 	ram->fuc.r_0x100718 = ramfuc_reg(0x100718);
 	ram->fuc.r_0x10071c = ramfuc_reg(0x10071c);
+	ram->fuc.r_0x100720 = ramfuc_reg(0x100720);
 	ram->fuc.r_0x100760 = ramfuc_stride(0x100760, 4, ram->base.part_mask);
 	ram->fuc.r_0x1007a0 = ramfuc_stride(0x1007a0, 4, ram->base.part_mask);
 	ram->fuc.r_0x1007e0 = ramfuc_stride(0x1007e0, 4, ram->base.part_mask);
+	ram->fuc.r_0x100da0 = ramfuc_stride(0x100da0, 4, ram->base.part_mask);
 	ram->fuc.r_0x10f804 = ramfuc_reg(0x10f804);
 	ram->fuc.r_0x1110e0 = ramfuc_stride(0x1110e0, 4, ram->base.part_mask);
 	ram->fuc.r_0x111100 = ramfuc_reg(0x111100);
 	ram->fuc.r_0x111104 = ramfuc_reg(0x111104);
+	ram->fuc.r_0x1111e0 = ramfuc_reg(0x1111e0);
+	ram->fuc.r_0x111400 = ramfuc_reg(0x111400);
 	ram->fuc.r_0x611200 = ramfuc_reg(0x611200);
 
 	if (ram->base.ranks > 1) {
@@ -429,6 +1004,12 @@ nva3_ram_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 		ram->fuc.r_mr[3] = ramfuc_reg(0x1002e4);
 	}
 
+	ret = gpio->find(gpio, 0, 0x2e, DCB_GPIO_UNUSED, &func);
+	if (ret == 0) {
+		nv50_gpio_location(func.line, &reg, &shift);
+		ram->fuc.r_gpioFBVREF = ramfuc_reg(reg);
+	}
+
 	return 0;
 }
 
@@ -438,6 +1019,6 @@ nva3_ram_oclass = {
 		.ctor = nva3_ram_ctor,
 		.dtor = _nouveau_ram_dtor,
 		.init = nva3_ram_init,
-		.fini = _nouveau_ram_fini,
+		.fini = nva3_ram_fini,
 	},
 };
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/sddr2.c b/drivers/gpu/drm/nouveau/core/subdev/fb/sddr2.c
index bb1eb8f3e639..252575f3aa29 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/sddr2.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/sddr2.c
@@ -66,7 +66,7 @@ nouveau_sddr2_calc(struct nouveau_ram *ram)
 	case 0x10:
 		CL  = ram->next->bios.timing_10_CL;
 		WR  = ram->next->bios.timing_10_WR;
-		DLL = !ram->next->bios.ramcfg_10_02_40;
+		DLL = !ram->next->bios.ramcfg_10_DLLoff;
 		ODT = ram->next->bios.timing_10_ODT & 3;
 		break;
 	case 0x20:
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/sddr3.c b/drivers/gpu/drm/nouveau/core/subdev/fb/sddr3.c
index 83949b11833a..a2dca4869e52 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/sddr3.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/sddr3.c
@@ -80,7 +80,7 @@ nouveau_sddr3_calc(struct nouveau_ram *ram)
 		CWL = ram->next->bios.timing_10_CWL;
 		CL  = ram->next->bios.timing_10_CL;
 		WR  = ram->next->bios.timing_10_WR;
-		DLL = !ram->next->bios.ramcfg_10_02_40;
+		DLL = !ram->next->bios.ramcfg_10_DLLoff;
 		ODT = ram->next->bios.timing_10_ODT;
 		break;
 	case 0x20:
diff --git a/drivers/gpu/drm/nouveau/core/subdev/gpio/nv50.c b/drivers/gpu/drm/nouveau/core/subdev/gpio/nv50.c
index 1864fa98e6b1..2e30d5a62d6e 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/gpio/nv50.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/gpio/nv50.c
@@ -54,7 +54,7 @@ nv50_gpio_reset(struct nouveau_gpio *gpio, u8 match)
 	}
 }
 
-static int
+int
 nv50_gpio_location(int line, u32 *reg, u32 *shift)
 {
 	const u32 nv50_gpio_reg[4] = { 0xe104, 0xe108, 0xe280, 0xe284 };
diff --git a/drivers/gpu/drm/nouveau/core/subdev/i2c/base.c b/drivers/gpu/drm/nouveau/core/subdev/i2c/base.c
index 2b1bf545e488..0dc605db7ec8 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/i2c/base.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/i2c/base.c
@@ -473,18 +473,56 @@ nouveau_i2c_extdev_sclass[] = {
 	nouveau_anx9805_sclass,
 };
 
+static void
+nouveau_i2c_create_port(struct nouveau_i2c *i2c, int index, u8 type,
+			struct dcb_i2c_entry *info)
+{
+	const struct nouveau_i2c_impl *impl = (void *)nv_oclass(i2c);
+	struct nouveau_oclass *oclass;
+	struct nouveau_object *parent;
+	struct nouveau_object *object;
+	int ret, pad;
+
+	if (info->share != DCB_I2C_UNUSED) {
+		pad    = info->share;
+		oclass = impl->pad_s;
+	} else {
+		if (type != DCB_I2C_NVIO_AUX)
+			pad = 0x100 + info->drive;
+		else
+			pad = 0x100 + info->auxch;
+		oclass = impl->pad_x;
+	}
+
+	ret = nouveau_object_ctor(NULL, nv_object(i2c), oclass, NULL, pad,
+				 &parent);
+	if (ret < 0)
+		return;
+
+	oclass = impl->sclass;
+	do {
+		ret = -EINVAL;
+		if (oclass->handle == type) {
+			ret = nouveau_object_ctor(parent, nv_object(i2c),
+						  oclass, info, index,
+						 &object);
+		}
+	} while (ret && (++oclass)->handle);
+
+	nouveau_object_ref(NULL, &parent);
+}
+
 int
 nouveau_i2c_create_(struct nouveau_object *parent,
 		    struct nouveau_object *engine,
 		    struct nouveau_oclass *oclass,
 		    int length, void **pobject)
 {
-	const struct nouveau_i2c_impl *impl = (void *)oclass;
 	struct nouveau_bios *bios = nouveau_bios(parent);
 	struct nouveau_i2c *i2c;
 	struct nouveau_object *object;
 	struct dcb_i2c_entry info;
-	int ret, i, j, index = -1, pad;
+	int ret, i, j, index = -1;
 	struct dcb_output outp;
 	u8  ver, hdr;
 	u32 data;
@@ -507,43 +545,40 @@ nouveau_i2c_create_(struct nouveau_object *parent,
 	INIT_LIST_HEAD(&i2c->ports);
 
 	while (!dcb_i2c_parse(bios, ++index, &info)) {
-		if (info.type == DCB_I2C_UNUSED)
+		switch (info.type) {
+		case DCB_I2C_NV04_BIT:
+		case DCB_I2C_NV4E_BIT:
+		case DCB_I2C_NVIO_BIT:
+			nouveau_i2c_create_port(i2c, NV_I2C_PORT(index),
+						info.type, &info);
+			break;
+		case DCB_I2C_NVIO_AUX:
+			nouveau_i2c_create_port(i2c, NV_I2C_AUX(index),
+						info.type, &info);
+			break;
+		case DCB_I2C_PMGR:
+			if (info.drive != DCB_I2C_UNUSED) {
+				nouveau_i2c_create_port(i2c, NV_I2C_PORT(index),
+							DCB_I2C_NVIO_BIT,
+							&info);
+			}
+			if (info.auxch != DCB_I2C_UNUSED) {
+				nouveau_i2c_create_port(i2c, NV_I2C_AUX(index),
+							DCB_I2C_NVIO_AUX,
+							&info);
+			}
+			break;
+		case DCB_I2C_UNUSED:
+		default:
 			continue;
-
-		if (info.share != DCB_I2C_UNUSED) {
-			if (info.type == DCB_I2C_NVIO_AUX)
-				pad = info.drive;
-			else
-				pad = info.share;
-			oclass = impl->pad_s;
-		} else {
-			pad = 0x100 + info.drive;
-			oclass = impl->pad_x;
 		}
-
-		ret = nouveau_object_ctor(NULL, *pobject, oclass,
-					  NULL, pad, &parent);
-		if (ret < 0)
-			continue;
-
-		oclass = impl->sclass;
-		do {
-			ret = -EINVAL;
-			if (oclass->handle == info.type) {
-				ret = nouveau_object_ctor(parent, *pobject,
-							  oclass, &info,
-							  index, &object);
-			}
-		} while (ret && (++oclass)->handle);
-
-		nouveau_object_ref(NULL, &parent);
 	}
 
 	/* in addition to the busses specified in the i2c table, there
 	 * may be ddc/aux channels hiding behind external tmds/dp/etc
 	 * transmitters.
 	 */
-	index = ((index + 0x0f) / 0x10) * 0x10;
+	index = NV_I2C_EXT(0);
 	i = -1;
 	while ((data = dcb_outp_parse(bios, ++i, &ver, &hdr, &outp))) {
 		if (!outp.location || !outp.extdev)
diff --git a/drivers/gpu/drm/nouveau/core/subdev/i2c/gm204.c b/drivers/gpu/drm/nouveau/core/subdev/i2c/gm204.c
new file mode 100644
index 000000000000..06a2b87ccbf1
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/i2c/gm204.c
@@ -0,0 +1,221 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include "nv50.h"
+
+#define AUX_DBG(fmt, args...) nv_debug(aux, "AUXCH(%d): " fmt, ch, ##args)
+#define AUX_ERR(fmt, args...) nv_error(aux, "AUXCH(%d): " fmt, ch, ##args)
+
+static void
+auxch_fini(struct nouveau_i2c *aux, int ch)
+{
+	nv_mask(aux, 0x00d954 + (ch * 0x50), 0x00310000, 0x00000000);
+}
+
+static int
+auxch_init(struct nouveau_i2c *aux, int ch)
+{
+	const u32 unksel = 1; /* nfi which to use, or if it matters.. */
+	const u32 ureq = unksel ? 0x00100000 : 0x00200000;
+	const u32 urep = unksel ? 0x01000000 : 0x02000000;
+	u32 ctrl, timeout;
+
+	/* wait up to 1ms for any previous transaction to be done... */
+	timeout = 1000;
+	do {
+		ctrl = nv_rd32(aux, 0x00d954 + (ch * 0x50));
+		udelay(1);
+		if (!timeout--) {
+			AUX_ERR("begin idle timeout 0x%08x\n", ctrl);
+			return -EBUSY;
+		}
+	} while (ctrl & 0x03010000);
+
+	/* set some magic, and wait up to 1ms for it to appear */
+	nv_mask(aux, 0x00d954 + (ch * 0x50), 0x00300000, ureq);
+	timeout = 1000;
+	do {
+		ctrl = nv_rd32(aux, 0x00d954 + (ch * 0x50));
+		udelay(1);
+		if (!timeout--) {
+			AUX_ERR("magic wait 0x%08x\n", ctrl);
+			auxch_fini(aux, ch);
+			return -EBUSY;
+		}
+	} while ((ctrl & 0x03000000) != urep);
+
+	return 0;
+}
+
+int
+gm204_aux(struct nouveau_i2c_port *base, bool retry,
+	 u8 type, u32 addr, u8 *data, u8 size)
+{
+	struct nouveau_i2c *aux = nouveau_i2c(base);
+	struct nv50_i2c_port *port = (void *)base;
+	u32 ctrl, stat, timeout, retries;
+	u32 xbuf[4] = {};
+	int ch = port->addr;
+	int ret, i;
+
+	AUX_DBG("%d: 0x%08x %d\n", type, addr, size);
+
+	ret = auxch_init(aux, ch);
+	if (ret)
+		goto out;
+
+	stat = nv_rd32(aux, 0x00d958 + (ch * 0x50));
+	if (!(stat & 0x10000000)) {
+		AUX_DBG("sink not detected\n");
+		ret = -ENXIO;
+		goto out;
+	}
+
+	if (!(type & 1)) {
+		memcpy(xbuf, data, size);
+		for (i = 0; i < 16; i += 4) {
+			AUX_DBG("wr 0x%08x\n", xbuf[i / 4]);
+			nv_wr32(aux, 0x00d930 + (ch * 0x50) + i, xbuf[i / 4]);
+		}
+	}
+
+	ctrl  = nv_rd32(aux, 0x00d954 + (ch * 0x50));
+	ctrl &= ~0x0001f0ff;
+	ctrl |= type << 12;
+	ctrl |= size - 1;
+	nv_wr32(aux, 0x00d950 + (ch * 0x50), addr);
+
+	/* (maybe) retry transaction a number of times on failure... */
+	for (retries = 0; !ret && retries < 32; retries++) {
+		/* reset, and delay a while if this is a retry */
+		nv_wr32(aux, 0x00d954 + (ch * 0x50), 0x80000000 | ctrl);
+		nv_wr32(aux, 0x00d954 + (ch * 0x50), 0x00000000 | ctrl);
+		if (retries)
+			udelay(400);
+
+		/* transaction request, wait up to 1ms for it to complete */
+		nv_wr32(aux, 0x00d954 + (ch * 0x50), 0x00010000 | ctrl);
+
+		timeout = 1000;
+		do {
+			ctrl = nv_rd32(aux, 0x00d954 + (ch * 0x50));
+			udelay(1);
+			if (!timeout--) {
+				AUX_ERR("tx req timeout 0x%08x\n", ctrl);
+				ret = -EIO;
+				goto out;
+			}
+		} while (ctrl & 0x00010000);
+		ret = 1;
+
+		/* read status, and check if transaction completed ok */
+		stat = nv_mask(aux, 0x00d958 + (ch * 0x50), 0, 0);
+		if ((stat & 0x000f0000) == 0x00080000 ||
+		    (stat & 0x000f0000) == 0x00020000)
+			ret = retry ? 0 : 1;
+		if ((stat & 0x00000100))
+			ret = -ETIMEDOUT;
+		if ((stat & 0x00000e00))
+			ret = -EIO;
+
+		AUX_DBG("%02d 0x%08x 0x%08x\n", retries, ctrl, stat);
+	}
+
+	if (type & 1) {
+		for (i = 0; i < 16; i += 4) {
+			xbuf[i / 4] = nv_rd32(aux, 0x00d940 + (ch * 0x50) + i);
+			AUX_DBG("rd 0x%08x\n", xbuf[i / 4]);
+		}
+		memcpy(data, xbuf, size);
+	}
+
+out:
+	auxch_fini(aux, ch);
+	return ret < 0 ? ret : (stat & 0x000f0000) >> 16;
+}
+
+static const struct nouveau_i2c_func
+gm204_aux_func = {
+	.aux       = gm204_aux,
+};
+
+int
+gm204_aux_port_ctor(struct nouveau_object *parent,
+		    struct nouveau_object *engine,
+		    struct nouveau_oclass *oclass, void *data, u32 index,
+		    struct nouveau_object **pobject)
+{
+	struct dcb_i2c_entry *info = data;
+	struct nv50_i2c_port *port;
+	int ret;
+
+	ret = nouveau_i2c_port_create(parent, engine, oclass, index,
+				      &nouveau_i2c_aux_algo, &gm204_aux_func,
+				      &port);
+	*pobject = nv_object(port);
+	if (ret)
+		return ret;
+
+	port->base.aux = info->auxch;
+	port->addr = info->auxch;
+	return 0;
+}
+
+struct nouveau_oclass
+gm204_i2c_sclass[] = {
+	{ .handle = NV_I2C_TYPE_DCBI2C(DCB_I2C_NVIO_BIT),
+	  .ofuncs = &(struct nouveau_ofuncs) {
+		  .ctor = nvd0_i2c_port_ctor,
+		  .dtor = _nouveau_i2c_port_dtor,
+		  .init = nv50_i2c_port_init,
+		  .fini = _nouveau_i2c_port_fini,
+	  },
+	},
+	{ .handle = NV_I2C_TYPE_DCBI2C(DCB_I2C_NVIO_AUX),
+	  .ofuncs = &(struct nouveau_ofuncs) {
+		  .ctor = gm204_aux_port_ctor,
+		  .dtor = _nouveau_i2c_port_dtor,
+		  .init = _nouveau_i2c_port_init,
+		  .fini = _nouveau_i2c_port_fini,
+	  },
+	},
+	{}
+};
+
+struct nouveau_oclass *
+gm204_i2c_oclass = &(struct nouveau_i2c_impl) {
+	.base.handle = NV_SUBDEV(I2C, 0x24),
+	.base.ofuncs = &(struct nouveau_ofuncs) {
+		.ctor = _nouveau_i2c_ctor,
+		.dtor = _nouveau_i2c_dtor,
+		.init = _nouveau_i2c_init,
+		.fini = _nouveau_i2c_fini,
+	},
+	.sclass = gm204_i2c_sclass,
+	.pad_x = &nv04_i2c_pad_oclass,
+	.pad_s = &gm204_i2c_pad_oclass,
+	.aux = 8,
+	.aux_stat = nve0_aux_stat,
+	.aux_mask = nve0_aux_mask,
+}.base;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/i2c/nv50.h b/drivers/gpu/drm/nouveau/core/subdev/i2c/nv50.h
index 5d2a77421c74..9ef965692fb1 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/i2c/nv50.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/i2c/nv50.h
@@ -10,8 +10,6 @@ struct nv50_i2c_priv {
 struct nv50_i2c_port {
 	struct nouveau_i2c_port base;
 	u32 addr;
-	u32 ctrl;
-	u32 data;
 	u32 state;
 };
 
@@ -29,4 +27,8 @@ int  nv94_aux_port_ctor(struct nouveau_object *, struct nouveau_object *,
 void nv94_i2c_acquire(struct nouveau_i2c_port *);
 void nv94_i2c_release(struct nouveau_i2c_port *);
 
+int  nvd0_i2c_port_ctor(struct nouveau_object *, struct nouveau_object *,
+			struct nouveau_oclass *, void *, u32,
+			struct nouveau_object **);
+
 #endif
diff --git a/drivers/gpu/drm/nouveau/core/subdev/i2c/nv94.c b/drivers/gpu/drm/nouveau/core/subdev/i2c/nv94.c
index f59c3a255462..e383ee81f4d2 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/i2c/nv94.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/i2c/nv94.c
@@ -214,10 +214,6 @@ nv94_i2c_port_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 
 	port->state = 7;
 	port->addr = nv50_i2c_addr[info->drive];
-	if (info->share != DCB_I2C_UNUSED) {
-		port->ctrl = 0x00e500 + (info->share * 0x50);
-		port->data = 0x0000e001;
-	}
 	return 0;
 }
 
@@ -242,13 +238,8 @@ nv94_aux_port_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	port->base.aux = info->drive;
-	port->addr = info->drive;
-	if (info->share != DCB_I2C_UNUSED) {
-		port->ctrl = 0x00e500 + (info->drive * 0x50);
-		port->data = 0x00002002;
-	}
-
+	port->base.aux = info->auxch;
+	port->addr = info->auxch;
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/i2c/nvd0.c b/drivers/gpu/drm/nouveau/core/subdev/i2c/nvd0.c
index 364ddb1c5f03..fd99380502ec 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/i2c/nvd0.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/i2c/nvd0.c
@@ -48,7 +48,7 @@ nvd0_i2c_func = {
 	.sense_sda = nvd0_i2c_sense_sda,
 };
 
-static int
+int
 nvd0_i2c_port_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 		   struct nouveau_oclass *oclass, void *data, u32 index,
 		   struct nouveau_object **pobject)
@@ -66,10 +66,6 @@ nvd0_i2c_port_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 
 	port->state = 0x00000007;
 	port->addr = 0x00d014 + (info->drive * 0x20);
-	if (info->share != DCB_I2C_UNUSED) {
-		port->ctrl = 0x00e500 + (info->share * 0x50);
-		port->data = 0x0000e001;
-	}
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/i2c/nve0.c b/drivers/gpu/drm/nouveau/core/subdev/i2c/nve0.c
index cae77e1ad8dc..25fe5c2d110e 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/i2c/nve0.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/i2c/nve0.c
@@ -24,7 +24,7 @@
 
 #include "nv50.h"
 
-static void
+void
 nve0_aux_stat(struct nouveau_i2c *i2c, u32 *hi, u32 *lo, u32 *rq, u32 *tx)
 {
 	u32 intr = nv_rd32(i2c, 0x00dc60);
@@ -38,7 +38,7 @@ nve0_aux_stat(struct nouveau_i2c *i2c, u32 *hi, u32 *lo, u32 *rq, u32 *tx)
 	nv_wr32(i2c, 0x00dc60, intr);
 }
 
-static void
+void
 nve0_aux_mask(struct nouveau_i2c *i2c, u32 type, u32 mask, u32 data)
 {
 	u32 temp = nv_rd32(i2c, 0x00dc68), i;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/i2c/padgm204.c b/drivers/gpu/drm/nouveau/core/subdev/i2c/padgm204.c
new file mode 100644
index 000000000000..f0e6fbbaa8cd
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/i2c/padgm204.c
@@ -0,0 +1,86 @@
+/*
+ * Copyright 2014 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 "pad.h"
+
+struct gm204_i2c_pad {
+	struct nvkm_i2c_pad base;
+	int addr;
+};
+
+static int
+gm204_i2c_pad_fini(struct nouveau_object *object, bool suspend)
+{
+	struct nouveau_i2c *i2c = (void *)object->engine;
+	struct gm204_i2c_pad *pad = (void *)object;
+	nv_mask(i2c, 0x00d97c + pad->addr, 0x00000001, 0x00000001);
+	return nvkm_i2c_pad_fini(&pad->base, suspend);
+}
+
+static int
+gm204_i2c_pad_init(struct nouveau_object *object)
+{
+	struct nouveau_i2c *i2c = (void *)object->engine;
+	struct gm204_i2c_pad *pad = (void *)object;
+
+	switch (nv_oclass(pad->base.next)->handle) {
+	case NV_I2C_TYPE_DCBI2C(DCB_I2C_NVIO_AUX):
+		nv_mask(i2c, 0x00d970 + pad->addr, 0x0000c003, 0x00000002);
+		break;
+	case NV_I2C_TYPE_DCBI2C(DCB_I2C_NVIO_BIT):
+	default:
+		nv_mask(i2c, 0x00d970 + pad->addr, 0x0000c003, 0x0000c001);
+		break;
+	}
+
+	nv_mask(i2c, 0x00d97c + pad->addr, 0x00000001, 0x00000000);
+	return nvkm_i2c_pad_init(&pad->base);
+}
+
+static int
+gm204_i2c_pad_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
+		  struct nouveau_oclass *oclass, void *data, u32 index,
+		  struct nouveau_object **pobject)
+{
+	struct gm204_i2c_pad *pad;
+	int ret;
+
+	ret = nvkm_i2c_pad_create(parent, engine, oclass, index, &pad);
+	*pobject = nv_object(pad);
+	if (ret)
+		return ret;
+
+	pad->addr = index * 0x50;;
+	return 0;
+}
+
+struct nouveau_oclass
+gm204_i2c_pad_oclass = {
+	.ofuncs = &(struct nouveau_ofuncs) {
+		.ctor = gm204_i2c_pad_ctor,
+		.dtor = _nvkm_i2c_pad_dtor,
+		.init = gm204_i2c_pad_init,
+		.fini = gm204_i2c_pad_fini,
+	},
+};
diff --git a/drivers/gpu/drm/nouveau/core/subdev/i2c/priv.h b/drivers/gpu/drm/nouveau/core/subdev/i2c/priv.h
index 780090b6425a..4fe7ae3fde4e 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/i2c/priv.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/i2c/priv.h
@@ -5,6 +5,7 @@
 
 extern struct nouveau_oclass nv04_i2c_pad_oclass;
 extern struct nouveau_oclass nv94_i2c_pad_oclass;
+extern struct nouveau_oclass gm204_i2c_pad_oclass;
 
 #define nouveau_i2c_port_create(p,e,o,i,a,f,d)                                 \
 	nouveau_i2c_port_create_((p), (e), (o), (i), (a), (f),                 \
@@ -82,4 +83,7 @@ struct nouveau_i2c_impl {
 void nv94_aux_stat(struct nouveau_i2c *, u32 *, u32 *, u32 *, u32 *);
 void nv94_aux_mask(struct nouveau_i2c *, u32, u32, u32);
 
+void nve0_aux_stat(struct nouveau_i2c *, u32 *, u32 *, u32 *, u32 *);
+void nve0_aux_mask(struct nouveau_i2c *, u32, u32, u32);
+
 #endif
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/memx.fuc b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/memx.fuc
index e89789a53b80..ec03f9a4290b 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/memx.fuc
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/memx.fuc
@@ -50,6 +50,7 @@ handler(WR32  , 0x0000, 0x0002, #memx_func_wr32)
 handler(WAIT  , 0x0004, 0x0000, #memx_func_wait)
 handler(DELAY , 0x0001, 0x0000, #memx_func_delay)
 handler(VBLANK, 0x0001, 0x0000, #memx_func_wait_vblank)
+handler(TRAIN , 0x0000, 0x0000, #memx_func_train)
 memx_func_tail:
 
 .equ #memx_func_size #memx_func_next - #memx_func_head
@@ -63,6 +64,10 @@ memx_ts_end:
 memx_data_head:
 .skip 0x0800
 memx_data_tail:
+
+memx_train_head:
+.skip 0x0100
+memx_train_tail:
 #endif
 
 /******************************************************************************
@@ -260,6 +265,101 @@ memx_func_delay:
 // description
 //
 // $r15 - current (memx)
+// $r4  - packet length
+// $r3  - opcode desciption
+// $r0  - zero
+memx_func_train:
+#if NVKM_PPWR_CHIPSET == GT215
+// $r5 - outer loop counter
+// $r6 - inner loop counter
+// $r7 - entry counter (#memx_train_head + $r7)
+	movw $r5 0x3
+	movw $r7 0x0
+
+// Read random memory to wake up... things
+	imm32($r9, 0x700000)
+	nv_rd32($r8,$r9)
+	movw $r14 0x2710
+	call(nsec)
+
+	memx_func_train_loop_outer:
+		mulu $r8 $r5 0x101
+		sethi $r8 0x02000000
+		imm32($r9, 0x1111e0)
+		nv_wr32($r9, $r8)
+		push $r5
+
+		movw $r6 0x0
+		memx_func_train_loop_inner:
+			movw $r8 0x1111
+			mulu $r9 $r6 $r8
+			shl b32 $r8 $r9 0x10
+			or $r8 $r9
+			imm32($r9, 0x100720)
+			nv_wr32($r9, $r8)
+
+			imm32($r9, 0x100080)
+			nv_rd32($r8, $r9)
+			or $r8 $r8 0x20
+			nv_wr32($r9, $r8)
+
+			imm32($r9, 0x10053c)
+			imm32($r8, 0x80003002)
+			nv_wr32($r9, $r8)
+
+			imm32($r14, 0x100560)
+			imm32($r13, 0x80000000)
+			add b32 $r12 $r13 0
+			imm32($r11, 0x001e8480)
+			call(wait)
+
+			// $r5 - inner inner loop counter
+			// $r9 - result
+			movw $r5 0
+			imm32($r9, 0x8300ffff)
+			memx_func_train_loop_4x:
+				imm32($r10, 0x100080)
+				nv_rd32($r8, $r10)
+				imm32($r11, 0xffffffdf)
+				and $r8 $r11
+				nv_wr32($r10, $r8)
+
+				imm32($r10, 0x10053c)
+				imm32($r8, 0x80003002)
+				nv_wr32($r10, $r8)
+
+				imm32($r14, 0x100560)
+				imm32($r13, 0x80000000)
+				mov b32 $r12 $r13
+				imm32($r11, 0x00002710)
+				call(wait)
+
+				nv_rd32($r13, $r14)
+				and $r9 $r9 $r13
+
+				add b32 $r5 1
+				cmp b16 $r5 0x4
+				bra l #memx_func_train_loop_4x
+
+			add b32 $r10 $r7 #memx_train_head
+			st b32 D[$r10 + 0] $r9
+			add b32 $r6 1
+			add b32 $r7 4
+
+			cmp b16 $r6 0x10
+			bra l #memx_func_train_loop_inner
+
+		pop $r5
+		add b32 $r5 1
+		cmp b16 $r5 7
+		bra l #memx_func_train_loop_outer
+
+#endif
+	ret
+
+// description
+//
+// $r15 - current (memx)
 // $r14 - sender process name
 // $r13 - message (exec)
 // $r12 - head of script
@@ -307,8 +407,19 @@ memx_exec:
 // $r11 - data1
 // $r0  - zero
 memx_info:
+	cmp b16 $r12 0x1
+	bra e #memx_info_train
+
+	memx_info_data:
 	mov $r12 #memx_data_head
 	mov $r11 #memx_data_tail - #memx_data_head
+	bra #memx_info_send
+
+	memx_info_train:
+	mov $r12 #memx_train_head
+	mov $r11 #memx_train_tail - #memx_train_head
+
+	memx_info_send:
 	call(send)
 	ret
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nv108.fuc.h b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nv108.fuc.h
index 4d278a96b2bb..713e11e2953d 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nv108.fuc.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nv108.fuc.h
@@ -46,8 +46,8 @@ uint32_t nv108_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x584d454d,
-	0x0000061c,
-	0x0000060e,
+	0x0000062d,
+	0x0000061f,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -68,8 +68,8 @@ uint32_t nv108_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x46524550,
-	0x00000620,
-	0x0000061e,
+	0x00000631,
+	0x0000062f,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -90,8 +90,8 @@ uint32_t nv108_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x5f433249,
-	0x00000a24,
-	0x000008cb,
+	0x00000a35,
+	0x000008dc,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -112,8 +112,8 @@ uint32_t nv108_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x54534554,
-	0x00000a45,
-	0x00000a26,
+	0x00000a56,
+	0x00000a37,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -134,8 +134,8 @@ uint32_t nv108_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x454c4449,
-	0x00000a50,
-	0x00000a4e,
+	0x00000a61,
+	0x00000a5f,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -246,13 +246,15 @@ uint32_t nv108_pwr_data[] = {
 	0x00010006,
 	0x00000000,
 	0x0000057b,
-/* 0x03b8: memx_func_tail */
-/* 0x03b8: memx_ts_start */
+	0x00000007,
 	0x00000000,
-/* 0x03bc: memx_ts_end */
+	0x000005c3,
+/* 0x03c4: memx_func_tail */
+/* 0x03c4: memx_ts_start */
 	0x00000000,
-/* 0x03c0: memx_data_head */
+/* 0x03c8: memx_ts_end */
 	0x00000000,
+/* 0x03cc: memx_data_head */
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -764,8 +766,75 @@ uint32_t nv108_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-/* 0x0bc0: memx_data_tail */
-/* 0x0bc0: i2c_scl_map */
+	0x00000000,
+/* 0x0bcc: memx_data_tail */
+/* 0x0bcc: memx_train_head */
+	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,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+/* 0x0ccc: memx_train_tail */
+/* 0x0ccc: i2c_scl_map */
 	0x00000400,
 	0x00000800,
 	0x00001000,
@@ -776,7 +845,7 @@ uint32_t nv108_pwr_data[] = {
 	0x00020000,
 	0x00040000,
 	0x00080000,
-/* 0x0be8: i2c_sda_map */
+/* 0x0cf4: i2c_sda_map */
 	0x00100000,
 	0x00200000,
 	0x00400000,
@@ -844,9 +913,6 @@ uint32_t nv108_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
 };
 
 uint32_t nv108_pwr_code[] = {
@@ -1215,10 +1281,10 @@ uint32_t nv108_pwr_code[] = {
 	0xf40464f0,
 	0x2c06f70b,
 	0xb50066cf,
-	0x00f8ee06,
+	0x00f8f106,
 /* 0x0500: memx_func_leave */
 	0x66cf2c06,
-	0xef06b500,
+	0xf206b500,
 	0xe4400406,
 	0x0006f607,
 /* 0x0512: memx_func_leave_wait */
@@ -1270,370 +1336,374 @@ uint32_t nv108_pwr_code[] = {
 	0x9800f800,
 	0x10b6001e,
 	0x005d7e04,
-/* 0x05c3: memx_exec */
-	0xf900f800,
-	0xb2d0f9e0,
-/* 0x05cb: memx_exec_next */
-	0x98b2b2c1,
-	0x10b60013,
-	0xf034e704,
-	0xe033e701,
-	0x0132b601,
-	0x980c30f0,
-	0x55f9de35,
-	0x1ef412a6,
-	0xee0b98e5,
-	0xbbef0c98,
-	0xc44b02cb,
-	0x00bbcf07,
-	0xe0fcd0fc,
-	0x0002c27e,
-/* 0x0602: memx_info */
-	0xc04c00f8,
+/* 0x05c3: memx_func_train */
+	0xf800f800,
+/* 0x05c5: memx_exec */
+	0xf9e0f900,
+	0xb2c1b2d0,
+/* 0x05cd: memx_exec_next */
+	0x001398b2,
+	0xe70410b6,
+	0xe701f034,
+	0xb601e033,
+	0x30f00132,
+	0xde35980c,
+	0x12a655f9,
+	0x98e51ef4,
+	0x0c98f10b,
+	0x02cbbbf2,
+	0xcf07c44b,
+	0xd0fc00bb,
+	0xc27ee0fc,
+	0x00f80002,
+/* 0x0604: memx_info */
+	0xf401c670,
+/* 0x060a: memx_info_data */
+	0xcc4c0c0b,
 	0x08004b03,
-	0x0002c27e,
-/* 0x060e: memx_recv */
-	0xd6b000f8,
-	0xb20bf401,
-	0xf400d6b0,
-	0x00f8eb0b,
-/* 0x061c: memx_init */
-/* 0x061e: perf_recv */
-	0x00f800f8,
-/* 0x0620: perf_init */
-/* 0x0622: i2c_drive_scl */
-	0x36b000f8,
-	0x0d0bf400,
-	0xf607e040,
-	0x04bd0001,
-/* 0x0632: i2c_drive_scl_lo */
-	0xe44000f8,
-	0x0001f607,
-	0x00f804bd,
-/* 0x063c: i2c_drive_sda */
-	0xf40036b0,
-	0xe0400d0b,
-	0x0002f607,
-	0x00f804bd,
-/* 0x064c: i2c_drive_sda_lo */
-	0xf607e440,
-	0x04bd0002,
-/* 0x0656: i2c_sense_scl */
-	0x32f400f8,
-	0x07c44301,
-	0xfd0033cf,
-	0x0bf40431,
-	0x0131f406,
-/* 0x0668: i2c_sense_scl_done */
-/* 0x066a: i2c_sense_sda */
-	0x32f400f8,
-	0x07c44301,
-	0xfd0033cf,
-	0x0bf40432,
-	0x0131f406,
-/* 0x067c: i2c_sense_sda_done */
-/* 0x067e: i2c_raise_scl */
-	0x40f900f8,
-	0x03089844,
-	0x06227e01,
-/* 0x0689: i2c_raise_scl_wait */
-	0x03e84e00,
-	0x00005d7e,
-	0x0006567e,
-	0xb60901f4,
-	0x1bf40142,
-/* 0x069d: i2c_raise_scl_done */
-	0xf840fcef,
-/* 0x06a1: i2c_start */
-	0x06567e00,
-	0x0d11f400,
-	0x00066a7e,
-	0xf40611f4,
-/* 0x06b2: i2c_start_rep */
-	0x00032e0e,
-	0x0006227e,
-	0x3c7e0103,
-	0x76bb0006,
-	0x0465b600,
-	0x659450f9,
-	0x0256bb04,
-	0x75fd50bd,
-	0x7e50fc04,
-	0xb600067e,
-	0x11f40464,
-/* 0x06dd: i2c_start_send */
-	0x7e00031d,
-	0x4e00063c,
-	0x5d7e1388,
-	0x00030000,
-	0x0006227e,
-	0x7e13884e,
-/* 0x06f7: i2c_start_out */
-	0xf800005d,
-/* 0x06f9: i2c_stop */
-	0x7e000300,
-	0x03000622,
-	0x063c7e00,
-	0x03e84e00,
-	0x00005d7e,
-	0x227e0103,
-	0x884e0006,
-	0x005d7e13,
+/* 0x0613: memx_info_train */
+	0x4c090ef4,
+	0x004b0bcc,
+/* 0x0619: memx_info_send */
+	0x02c27e01,
+/* 0x061f: memx_recv */
+	0xb000f800,
+	0x0bf401d6,
+	0x00d6b0a3,
+	0xf8dc0bf4,
+/* 0x062d: memx_init */
+/* 0x062f: perf_recv */
+	0xf800f800,
+/* 0x0631: perf_init */
+/* 0x0633: i2c_drive_scl */
+	0xb000f800,
+	0x0bf40036,
+	0x07e0400d,
+	0xbd0001f6,
+/* 0x0643: i2c_drive_scl_lo */
+	0x4000f804,
+	0x01f607e4,
+	0xf804bd00,
+/* 0x064d: i2c_drive_sda */
+	0x0036b000,
+	0x400d0bf4,
+	0x02f607e0,
+	0xf804bd00,
+/* 0x065d: i2c_drive_sda_lo */
+	0x07e44000,
+	0xbd0002f6,
+/* 0x0667: i2c_sense_scl */
+	0xf400f804,
+	0xc4430132,
+	0x0033cf07,
+	0xf40431fd,
+	0x31f4060b,
+/* 0x0679: i2c_sense_scl_done */
+/* 0x067b: i2c_sense_sda */
+	0xf400f801,
+	0xc4430132,
+	0x0033cf07,
+	0xf40432fd,
+	0x31f4060b,
+/* 0x068d: i2c_sense_sda_done */
+/* 0x068f: i2c_raise_scl */
+	0xf900f801,
+	0x08984440,
+	0x337e0103,
+/* 0x069a: i2c_raise_scl_wait */
+	0xe84e0006,
+	0x005d7e03,
+	0x06677e00,
+	0x0901f400,
+	0xf40142b6,
+/* 0x06ae: i2c_raise_scl_done */
+	0x40fcef1b,
+/* 0x06b2: i2c_start */
+	0x677e00f8,
+	0x11f40006,
+	0x067b7e0d,
+	0x0611f400,
+/* 0x06c3: i2c_start_rep */
+	0x032e0ef4,
+	0x06337e00,
 	0x7e010300,
-	0x4e00063c,
-	0x5d7e1388,
-	0x00f80000,
-/* 0x0728: i2c_bitw */
-	0x00063c7e,
-	0x7e03e84e,
-	0xbb00005d,
+	0xbb00064d,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x00067e7e,
+	0x00068f7e,
 	0xf40464b6,
-	0x884e1711,
-	0x005d7e13,
-	0x7e000300,
-	0x4e000622,
-	0x5d7e1388,
-/* 0x0766: i2c_bitw_out */
-	0x00f80000,
-/* 0x0768: i2c_bitr */
-	0x3c7e0103,
+/* 0x06ee: i2c_start_send */
+	0x00031d11,
+	0x00064d7e,
+	0x7e13884e,
+	0x0300005d,
+	0x06337e00,
+	0x13884e00,
+	0x00005d7e,
+/* 0x0708: i2c_start_out */
+/* 0x070a: i2c_stop */
+	0x000300f8,
+	0x0006337e,
+	0x4d7e0003,
 	0xe84e0006,
 	0x005d7e03,
-	0x0076bb00,
-	0xf90465b6,
-	0x04659450,
-	0xbd0256bb,
-	0x0475fd50,
-	0x7e7e50fc,
-	0x64b60006,
-	0x1a11f404,
-	0x00066a7e,
-	0x227e0003,
-	0x884e0006,
-	0x005d7e13,
-	0x013cf000,
-/* 0x07ab: i2c_bitr_done */
-	0xf80131f4,
-/* 0x07ad: i2c_get_byte */
-	0x04000500,
-/* 0x07b1: i2c_get_byte_next */
-	0x0154b608,
+	0x7e010300,
+	0x4e000633,
+	0x5d7e1388,
+	0x01030000,
+	0x00064d7e,
+	0x7e13884e,
+	0xf800005d,
+/* 0x0739: i2c_bitw */
+	0x064d7e00,
+	0x03e84e00,
+	0x00005d7e,
 	0xb60076bb,
 	0x50f90465,
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0x07687e50,
+	0x068f7e50,
 	0x0464b600,
-	0xfd2a11f4,
-	0x42b60553,
-	0xd81bf401,
-	0x76bb0103,
+	0x4e1711f4,
+	0x5d7e1388,
+	0x00030000,
+	0x0006337e,
+	0x7e13884e,
+/* 0x0777: i2c_bitw_out */
+	0xf800005d,
+/* 0x0779: i2c_bitr */
+	0x7e010300,
+	0x4e00064d,
+	0x5d7e03e8,
+	0x76bb0000,
 	0x0465b600,
 	0x659450f9,
 	0x0256bb04,
 	0x75fd50bd,
 	0x7e50fc04,
-	0xb6000728,
-/* 0x07fa: i2c_get_byte_done */
-	0x00f80464,
-/* 0x07fc: i2c_put_byte */
-/* 0x07fe: i2c_put_byte_next */
-	0x42b60804,
-	0x3854ff01,
-	0xb60076bb,
-	0x50f90465,
-	0xbb046594,
-	0x50bd0256,
-	0xfc0475fd,
-	0x07287e50,
-	0x0464b600,
-	0xb03411f4,
-	0x1bf40046,
-	0x0076bbd8,
+	0xb600068f,
+	0x11f40464,
+	0x067b7e1a,
+	0x7e000300,
+	0x4e000633,
+	0x5d7e1388,
+	0x3cf00000,
+	0x0131f401,
+/* 0x07bc: i2c_bitr_done */
+/* 0x07be: i2c_get_byte */
+	0x000500f8,
+/* 0x07c2: i2c_get_byte_next */
+	0x54b60804,
+	0x0076bb01,
+	0xf90465b6,
+	0x04659450,
+	0xbd0256bb,
+	0x0475fd50,
+	0x797e50fc,
+	0x64b60007,
+	0x2a11f404,
+	0xb60553fd,
+	0x1bf40142,
+	0xbb0103d8,
+	0x65b60076,
+	0x9450f904,
+	0x56bb0465,
+	0xfd50bd02,
+	0x50fc0475,
+	0x0007397e,
+/* 0x080b: i2c_get_byte_done */
+	0xf80464b6,
+/* 0x080d: i2c_put_byte */
+/* 0x080f: i2c_put_byte_next */
+	0xb6080400,
+	0x54ff0142,
+	0x0076bb38,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
-	0x687e50fc,
+	0x397e50fc,
 	0x64b60007,
-	0x0f11f404,
-	0xb00076bb,
-	0x1bf40136,
-	0x0132f406,
-/* 0x0854: i2c_put_byte_done */
-/* 0x0856: i2c_addr */
-	0x76bb00f8,
+	0x3411f404,
+	0xf40046b0,
+	0x76bbd81b,
 	0x0465b600,
 	0x659450f9,
 	0x0256bb04,
 	0x75fd50bd,
 	0x7e50fc04,
-	0xb60006a1,
+	0xb6000779,
 	0x11f40464,
-	0x2ec3e729,
-	0x0134b601,
-	0xbb0553fd,
+	0x0076bb0f,
+	0xf40136b0,
+	0x32f4061b,
+/* 0x0865: i2c_put_byte_done */
+/* 0x0867: i2c_addr */
+	0xbb00f801,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x0007fc7e,
-/* 0x089b: i2c_addr_done */
-	0xf80464b6,
-/* 0x089d: i2c_acquire_addr */
-	0xf8cec700,
-	0xb705e4b6,
-	0xf8d014e0,
-/* 0x08a9: i2c_acquire */
-	0x089d7e00,
-	0x00047e00,
-	0x03d9f000,
-	0x00002e7e,
-/* 0x08ba: i2c_release */
-	0x9d7e00f8,
+	0x0006b27e,
+	0xf40464b6,
+	0xc3e72911,
+	0x34b6012e,
+	0x0553fd01,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0x080d7e50,
+	0x0464b600,
+/* 0x08ac: i2c_addr_done */
+/* 0x08ae: i2c_acquire_addr */
+	0xcec700f8,
+	0x05e4b6f8,
+	0xd014e0b7,
+/* 0x08ba: i2c_acquire */
+	0xae7e00f8,
 	0x047e0008,
-	0xdaf00000,
+	0xd9f00000,
 	0x002e7e03,
-/* 0x08cb: i2c_recv */
-	0xf400f800,
-	0xc1c70132,
-	0x0214b6f8,
-	0xf52816b0,
-	0xb801371f,
-	0x000be813,
-	0xb8003298,
-	0x000bc013,
-	0xf4003198,
-	0xd0f90231,
-	0xd0f9e0f9,
-	0x000067f1,
-	0x100063f1,
-	0xbb016792,
+/* 0x08cb: i2c_release */
+	0x7e00f800,
+	0x7e0008ae,
+	0xf0000004,
+	0x2e7e03da,
+	0x00f80000,
+/* 0x08dc: i2c_recv */
+	0xc70132f4,
+	0x14b6f8c1,
+	0x2816b002,
+	0x01371ff5,
+	0x0cf413b8,
+	0x00329800,
+	0x0ccc13b8,
+	0x00319800,
+	0xf90231f4,
+	0xf9e0f9d0,
+	0x0067f1d0,
+	0x0063f100,
+	0x01679210,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0x08ba7e50,
+	0x0464b600,
+	0xd6b0d0fc,
+	0xb01bf500,
+	0xbb000500,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x0008a97e,
-	0xfc0464b6,
-	0x00d6b0d0,
-	0x00b01bf5,
-	0x76bb0005,
+	0x0008677e,
+	0xf50464b6,
+	0xc700cc11,
+	0x76bbe0c5,
 	0x0465b600,
 	0x659450f9,
 	0x0256bb04,
 	0x75fd50bd,
 	0x7e50fc04,
-	0xb6000856,
+	0xb600080d,
 	0x11f50464,
-	0xc5c700cc,
-	0x0076bbe0,
-	0xf90465b6,
-	0x04659450,
-	0xbd0256bb,
-	0x0475fd50,
-	0xfc7e50fc,
-	0x64b60007,
-	0xa911f504,
-	0xbb010500,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x0008567e,
-	0xf50464b6,
-	0xbb008711,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x0007ad7e,
-	0xf40464b6,
-	0x5bcb6711,
-	0x0076bbe0,
-	0xf90465b6,
-	0x04659450,
-	0xbd0256bb,
-	0x0475fd50,
-	0xf97e50fc,
-	0x64b60006,
-	0xbd5bb204,
-	0x410ef474,
-/* 0x09d0: i2c_recv_not_rd08 */
-	0xf401d6b0,
-	0x00053b1b,
-	0x0008567e,
-	0xc73211f4,
-	0xfc7ee0c5,
-	0x11f40007,
-	0x7e000528,
-	0xf4000856,
-	0xb5c71f11,
-	0x07fc7ee0,
-	0x1511f400,
-	0x0006f97e,
-	0xc5c774bd,
-	0x091bf408,
-	0xf40232f4,
-/* 0x0a0e: i2c_recv_not_wr08 */
-/* 0x0a0e: i2c_recv_done */
-	0xcec7030e,
-	0x08ba7ef8,
-	0xfce0fc00,
-	0x0912f4d0,
-	0xc27e7cb2,
-/* 0x0a22: i2c_recv_exit */
-	0x00f80002,
-/* 0x0a24: i2c_init */
-/* 0x0a26: test_recv */
-	0x584100f8,
-	0x0011cf04,
-	0x400110b6,
-	0x01f60458,
-	0xf104bd00,
-	0xf1d900e7,
-	0x7e134fe3,
-	0xf8000201,
-/* 0x0a45: test_init */
-	0x08004e00,
-	0x0002017e,
-/* 0x0a4e: idle_recv */
-	0x00f800f8,
-/* 0x0a50: idle */
-	0x410031f4,
-	0x11cf0454,
+	0x010500a9,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0x08677e50,
+	0x0464b600,
+	0x008711f5,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0x07be7e50,
+	0x0464b600,
+	0xcb6711f4,
+	0x76bbe05b,
+	0x0465b600,
+	0x659450f9,
+	0x0256bb04,
+	0x75fd50bd,
+	0x7e50fc04,
+	0xb600070a,
+	0x5bb20464,
+	0x0ef474bd,
+/* 0x09e1: i2c_recv_not_rd08 */
+	0x01d6b041,
+	0x053b1bf4,
+	0x08677e00,
+	0x3211f400,
+	0x7ee0c5c7,
+	0xf400080d,
+	0x00052811,
+	0x0008677e,
+	0xc71f11f4,
+	0x0d7ee0b5,
+	0x11f40008,
+	0x070a7e15,
+	0xc774bd00,
+	0x1bf408c5,
+	0x0232f409,
+/* 0x0a1f: i2c_recv_not_wr08 */
+/* 0x0a1f: i2c_recv_done */
+	0xc7030ef4,
+	0xcb7ef8ce,
+	0xe0fc0008,
+	0x12f4d0fc,
+	0x7e7cb209,
+/* 0x0a33: i2c_recv_exit */
+	0xf80002c2,
+/* 0x0a35: i2c_init */
+/* 0x0a37: test_recv */
+	0x4100f800,
+	0x11cf0458,
 	0x0110b600,
-	0xf6045440,
+	0xf6045840,
 	0x04bd0001,
-/* 0x0a64: idle_loop */
-	0x32f45801,
-/* 0x0a69: idle_proc */
-/* 0x0a69: idle_proc_exec */
-	0xb210f902,
-	0x02cb7e1e,
-	0xf410fc00,
-	0x31f40911,
-	0xf00ef402,
-/* 0x0a7c: idle_proc_next */
-	0xa65810b6,
-	0xe81bf41f,
-	0xf4e002f4,
-	0x0ef40028,
-	0x000000c6,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
+	0xd900e7f1,
+	0x134fe3f1,
+	0x0002017e,
+/* 0x0a56: test_init */
+	0x004e00f8,
+	0x02017e08,
+/* 0x0a5f: idle_recv */
+	0xf800f800,
+/* 0x0a61: idle */
+	0x0031f400,
+	0xcf045441,
+	0x10b60011,
+	0x04544001,
+	0xbd0001f6,
+/* 0x0a75: idle_loop */
+	0xf4580104,
+/* 0x0a7a: idle_proc */
+/* 0x0a7a: idle_proc_exec */
+	0x10f90232,
+	0xcb7e1eb2,
+	0x10fc0002,
+	0xf40911f4,
+	0x0ef40231,
+/* 0x0a8d: idle_proc_next */
+	0x5810b6f0,
+	0x1bf41fa6,
+	0xe002f4e8,
+	0xf40028f4,
+	0x0000c60e,
 	0x00000000,
 	0x00000000,
 	0x00000000,
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nva3.fuc.h b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nva3.fuc.h
index 64e97baabc3c..d1f9b6cb66d7 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nva3.fuc.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nva3.fuc.h
@@ -46,8 +46,8 @@ uint32_t nva3_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x584d454d,
-	0x000006e0,
-	0x000006d2,
+	0x00000842,
+	0x00000834,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -68,8 +68,8 @@ uint32_t nva3_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x46524550,
-	0x000006e4,
-	0x000006e2,
+	0x00000846,
+	0x00000844,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -90,8 +90,8 @@ uint32_t nva3_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x5f433249,
-	0x00000b14,
-	0x000009b7,
+	0x00000c76,
+	0x00000b19,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -112,8 +112,8 @@ uint32_t nva3_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x54534554,
-	0x00000b3d,
-	0x00000b16,
+	0x00000c9f,
+	0x00000c78,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -134,8 +134,8 @@ uint32_t nva3_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x454c4449,
-	0x00000b49,
-	0x00000b47,
+	0x00000cab,
+	0x00000ca9,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -246,13 +246,15 @@ uint32_t nva3_pwr_data[] = {
 	0x00010006,
 	0x00000000,
 	0x000005f8,
-/* 0x03b8: memx_func_tail */
-/* 0x03b8: memx_ts_start */
+	0x00000007,
 	0x00000000,
-/* 0x03bc: memx_ts_end */
+	0x0000067e,
+/* 0x03c4: memx_func_tail */
+/* 0x03c4: memx_ts_start */
 	0x00000000,
-/* 0x03c0: memx_data_head */
+/* 0x03c8: memx_ts_end */
 	0x00000000,
+/* 0x03cc: memx_data_head */
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -764,8 +766,75 @@ uint32_t nva3_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-/* 0x0bc0: memx_data_tail */
-/* 0x0bc0: i2c_scl_map */
+	0x00000000,
+/* 0x0bcc: memx_data_tail */
+/* 0x0bcc: memx_train_head */
+	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,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+/* 0x0ccc: memx_train_tail */
+/* 0x0ccc: i2c_scl_map */
 	0x00001000,
 	0x00004000,
 	0x00010000,
@@ -776,7 +845,7 @@ uint32_t nva3_pwr_data[] = {
 	0x01000000,
 	0x04000000,
 	0x10000000,
-/* 0x0be8: i2c_sda_map */
+/* 0x0cf4: i2c_sda_map */
 	0x00002000,
 	0x00008000,
 	0x00020000,
@@ -787,7 +856,7 @@ uint32_t nva3_pwr_data[] = {
 	0x02000000,
 	0x08000000,
 	0x20000000,
-/* 0x0c10: i2c_ctrl */
+/* 0x0d1c: i2c_ctrl */
 	0x0000e138,
 	0x0000e150,
 	0x0000e168,
@@ -845,9 +914,6 @@ uint32_t nva3_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
 };
 
 uint32_t nva3_pwr_code[] = {
@@ -1258,11 +1324,11 @@ uint32_t nva3_pwr_code[] = {
 	0x67f0f30b,
 	0x0664b62c,
 	0x800066cf,
-	0x00f8ee06,
+	0x00f8f106,
 /* 0x05a8: memx_func_leave */
 	0xb62c67f0,
 	0x66cf0664,
-	0xef068000,
+	0xf2068000,
 	0xf10467f0,
 	0xb607e407,
 	0x06d00604,
@@ -1323,408 +1389,479 @@ uint32_t nva3_pwr_code[] = {
 	0x9800f8a4,
 	0x10b6001e,
 	0x7f21f404,
-/* 0x067e: memx_exec */
-	0xe0f900f8,
-	0xc1b9d0f9,
-	0x02b2b902,
-/* 0x0688: memx_exec_next */
-	0xb6001398,
-	0x34e70410,
-	0x33e701f0,
-	0x32b601e0,
-	0x0c30f001,
-	0xf9de3598,
-	0x0612b855,
-	0x98e41ef4,
-	0x0c98ee0b,
-	0x02cbbbef,
-	0x07c4b7f1,
-	0xcf06b4b6,
-	0xd0fc00bb,
-	0x21f5e0fc,
+/* 0x067e: memx_func_train */
+	0x57f100f8,
+	0x77f10003,
+	0x97f10000,
+	0x93f00000,
+	0x029eb970,
+	0xb90421f4,
+	0xe7f102d8,
+	0x21f42710,
+/* 0x069d: memx_func_train_loop_outer */
+	0x0158e07f,
+	0x0083f101,
+	0xe097f102,
+	0x1193f011,
+	0x80f990f9,
+	0xe0fcd0fc,
+	0xf93f21f4,
+	0x0067f150,
+/* 0x06bd: memx_func_train_loop_inner */
+	0x1187f100,
+	0x9068ff11,
+	0xfd109894,
+	0x97f10589,
+	0x93f00720,
+	0xf990f910,
+	0xfcd0fc80,
+	0x3f21f4e0,
+	0x008097f1,
+	0xb91093f0,
+	0x21f4029e,
+	0x02d8b904,
+	0xf92088c5,
+	0xfc80f990,
+	0xf4e0fcd0,
+	0x97f13f21,
+	0x93f0053c,
+	0x0287f110,
+	0x0083f130,
+	0xf990f980,
+	0xfcd0fc80,
+	0x3f21f4e0,
+	0x0560e7f1,
+	0xf110e3f0,
+	0xf10000d7,
+	0x908000d3,
+	0xb7f100dc,
+	0xb3f08480,
+	0xa421f41e,
+	0x000057f1,
+	0xffff97f1,
+	0x830093f1,
+/* 0x073c: memx_func_train_loop_4x */
+	0x0080a7f1,
+	0xb910a3f0,
+	0x21f402ae,
+	0x02d8b904,
+	0xffdfb7f1,
+	0xffffb3f1,
+	0xf9048bfd,
+	0xfc80f9a0,
+	0xf4e0fcd0,
+	0xa7f13f21,
+	0xa3f0053c,
+	0x0287f110,
+	0x0083f130,
+	0xf9a0f980,
+	0xfcd0fc80,
+	0x3f21f4e0,
+	0x0560e7f1,
+	0xf110e3f0,
+	0xf10000d7,
+	0xb98000d3,
+	0xb7f102dc,
+	0xb3f02710,
+	0xa421f400,
+	0xf402eeb9,
+	0xddb90421,
+	0x949dff02,
+	0x700150b6,
+	0x1ef40456,
+	0xcc7aa092,
+	0x00a9800b,
+	0xb60160b6,
+	0x66700470,
+	0x001ef510,
+	0xb650fcff,
+	0x56700150,
+	0xd41ef507,
+/* 0x07cf: memx_exec */
+	0xf900f8fe,
+	0xb9d0f9e0,
+	0xb2b902c1,
+/* 0x07d9: memx_exec_next */
+	0x00139802,
+	0xe70410b6,
+	0xe701f034,
+	0xb601e033,
+	0x30f00132,
+	0xde35980c,
+	0x12b855f9,
+	0xe41ef406,
+	0x98f10b98,
+	0xcbbbf20c,
+	0xc4b7f102,
+	0x06b4b607,
+	0xfc00bbcf,
+	0xf5e0fcd0,
+	0xf8034221,
+/* 0x0815: memx_info */
+	0x01c67000,
+/* 0x081b: memx_info_data */
+	0xf10e0bf4,
+	0xf103ccc7,
+	0xf40800b7,
+/* 0x0826: memx_info_train */
+	0xc7f10b0e,
+	0xb7f10bcc,
+/* 0x082e: memx_info_send */
+	0x21f50100,
 	0x00f80342,
-/* 0x06c4: memx_info */
-	0x03c0c7f1,
-	0x0800b7f1,
-	0x034221f5,
-/* 0x06d2: memx_recv */
-	0xd6b000f8,
-	0xa90bf401,
-	0xf400d6b0,
-	0x00f8e90b,
-/* 0x06e0: memx_init */
-/* 0x06e2: perf_recv */
+/* 0x0834: memx_recv */
+	0xf401d6b0,
+	0xd6b0980b,
+	0xd80bf400,
+/* 0x0842: memx_init */
+	0x00f800f8,
+/* 0x0844: perf_recv */
+/* 0x0846: perf_init */
 	0x00f800f8,
-/* 0x06e4: perf_init */
-/* 0x06e6: i2c_drive_scl */
+/* 0x0848: i2c_drive_scl */
+	0xf40036b0,
+	0x07f1110b,
+	0x04b607e0,
+	0x0001d006,
+	0x00f804bd,
+/* 0x085c: i2c_drive_scl_lo */
+	0x07e407f1,
+	0xd00604b6,
+	0x04bd0001,
+/* 0x086a: i2c_drive_sda */
 	0x36b000f8,
 	0x110bf400,
 	0x07e007f1,
 	0xd00604b6,
-	0x04bd0001,
-/* 0x06fa: i2c_drive_scl_lo */
+	0x04bd0002,
+/* 0x087e: i2c_drive_sda_lo */
 	0x07f100f8,
 	0x04b607e4,
-	0x0001d006,
-	0x00f804bd,
-/* 0x0708: i2c_drive_sda */
-	0xf40036b0,
-	0x07f1110b,
-	0x04b607e0,
 	0x0002d006,
 	0x00f804bd,
-/* 0x071c: i2c_drive_sda_lo */
-	0x07e407f1,
-	0xd00604b6,
-	0x04bd0002,
-/* 0x072a: i2c_sense_scl */
-	0x32f400f8,
-	0xc437f101,
-	0x0634b607,
-	0xfd0033cf,
-	0x0bf40431,
-	0x0131f406,
-/* 0x0740: i2c_sense_scl_done */
-/* 0x0742: i2c_sense_sda */
-	0x32f400f8,
-	0xc437f101,
-	0x0634b607,
-	0xfd0033cf,
-	0x0bf40432,
-	0x0131f406,
-/* 0x0758: i2c_sense_sda_done */
-/* 0x075a: i2c_raise_scl */
-	0x40f900f8,
-	0x089847f1,
-	0xf50137f0,
-/* 0x0767: i2c_raise_scl_wait */
-	0xf106e621,
-	0xf403e8e7,
-	0x21f57f21,
-	0x01f4072a,
-	0x0142b609,
-/* 0x077b: i2c_raise_scl_done */
-	0xfcef1bf4,
-/* 0x077f: i2c_start */
-	0xf500f840,
-	0xf4072a21,
-	0x21f50d11,
-	0x11f40742,
-	0x300ef406,
-/* 0x0790: i2c_start_rep */
-	0xf50037f0,
-	0xf006e621,
-	0x21f50137,
-	0x76bb0708,
-	0x0465b600,
-	0x659450f9,
-	0x0256bb04,
-	0x75fd50bd,
-	0xf550fc04,
-	0xb6075a21,
-	0x11f40464,
-/* 0x07bd: i2c_start_send */
-	0x0037f01f,
-	0x070821f5,
-	0x1388e7f1,
-	0xf07f21f4,
-	0x21f50037,
-	0xe7f106e6,
-	0x21f41388,
-/* 0x07d9: i2c_start_out */
-/* 0x07db: i2c_stop */
-	0xf000f87f,
-	0x21f50037,
-	0x37f006e6,
-	0x0821f500,
-	0xe8e7f107,
+/* 0x088c: i2c_sense_scl */
+	0xf10132f4,
+	0xb607c437,
+	0x33cf0634,
+	0x0431fd00,
+	0xf4060bf4,
+/* 0x08a2: i2c_sense_scl_done */
+	0x00f80131,
+/* 0x08a4: i2c_sense_sda */
+	0xf10132f4,
+	0xb607c437,
+	0x33cf0634,
+	0x0432fd00,
+	0xf4060bf4,
+/* 0x08ba: i2c_sense_sda_done */
+	0x00f80131,
+/* 0x08bc: i2c_raise_scl */
+	0x47f140f9,
+	0x37f00898,
+	0x4821f501,
+/* 0x08c9: i2c_raise_scl_wait */
+	0xe8e7f108,
 	0x7f21f403,
-	0xf50137f0,
-	0xf106e621,
-	0xf41388e7,
-	0x37f07f21,
-	0x0821f501,
-	0x88e7f107,
-	0x7f21f413,
-/* 0x080e: i2c_bitw */
-	0x21f500f8,
-	0xe7f10708,
-	0x21f403e8,
-	0x0076bb7f,
-	0xf90465b6,
-	0x04659450,
-	0xbd0256bb,
-	0x0475fd50,
-	0x21f550fc,
-	0x64b6075a,
-	0x1811f404,
-	0x1388e7f1,
-	0xf07f21f4,
+	0x088c21f5,
+	0xb60901f4,
+	0x1bf40142,
+/* 0x08dd: i2c_raise_scl_done */
+	0xf840fcef,
+/* 0x08e1: i2c_start */
+	0x8c21f500,
+	0x0d11f408,
+	0x08a421f5,
+	0xf40611f4,
+/* 0x08f2: i2c_start_rep */
+	0x37f0300e,
+	0x4821f500,
+	0x0137f008,
+	0x086a21f5,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0xbc21f550,
+	0x0464b608,
+/* 0x091f: i2c_start_send */
+	0xf01f11f4,
 	0x21f50037,
-	0xe7f106e6,
+	0xe7f1086a,
 	0x21f41388,
-/* 0x084d: i2c_bitw_out */
-/* 0x084f: i2c_bitr */
-	0xf000f87f,
-	0x21f50137,
-	0xe7f10708,
-	0x21f403e8,
-	0x0076bb7f,
-	0xf90465b6,
-	0x04659450,
-	0xbd0256bb,
-	0x0475fd50,
-	0x21f550fc,
-	0x64b6075a,
-	0x1b11f404,
-	0x074221f5,
+	0x0037f07f,
+	0x084821f5,
+	0x1388e7f1,
+/* 0x093b: i2c_start_out */
+	0xf87f21f4,
+/* 0x093d: i2c_stop */
+	0x0037f000,
+	0x084821f5,
 	0xf50037f0,
-	0xf106e621,
+	0xf1086a21,
+	0xf403e8e7,
+	0x37f07f21,
+	0x4821f501,
+	0x88e7f108,
+	0x7f21f413,
+	0xf50137f0,
+	0xf1086a21,
 	0xf41388e7,
-	0x3cf07f21,
-	0x0131f401,
-/* 0x0894: i2c_bitr_done */
-/* 0x0896: i2c_get_byte */
-	0x57f000f8,
-	0x0847f000,
-/* 0x089c: i2c_get_byte_next */
-	0xbb0154b6,
+	0x00f87f21,
+/* 0x0970: i2c_bitw */
+	0x086a21f5,
+	0x03e8e7f1,
+	0xbb7f21f4,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x084f21f5,
+	0x08bc21f5,
 	0xf40464b6,
-	0x53fd2b11,
-	0x0142b605,
-	0xf0d81bf4,
-	0x76bb0137,
-	0x0465b600,
-	0x659450f9,
-	0x0256bb04,
-	0x75fd50bd,
-	0xf550fc04,
-	0xb6080e21,
-/* 0x08e6: i2c_get_byte_done */
-	0x00f80464,
-/* 0x08e8: i2c_put_byte */
-/* 0x08eb: i2c_put_byte_next */
-	0xb60847f0,
-	0x54ff0142,
-	0x0076bb38,
+	0xe7f11811,
+	0x21f41388,
+	0x0037f07f,
+	0x084821f5,
+	0x1388e7f1,
+/* 0x09af: i2c_bitw_out */
+	0xf87f21f4,
+/* 0x09b1: i2c_bitr */
+	0x0137f000,
+	0x086a21f5,
+	0x03e8e7f1,
+	0xbb7f21f4,
+	0x65b60076,
+	0x9450f904,
+	0x56bb0465,
+	0xfd50bd02,
+	0x50fc0475,
+	0x08bc21f5,
+	0xf40464b6,
+	0x21f51b11,
+	0x37f008a4,
+	0x4821f500,
+	0x88e7f108,
+	0x7f21f413,
+	0xf4013cf0,
+/* 0x09f6: i2c_bitr_done */
+	0x00f80131,
+/* 0x09f8: i2c_get_byte */
+	0xf00057f0,
+/* 0x09fe: i2c_get_byte_next */
+	0x54b60847,
+	0x0076bb01,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
 	0x21f550fc,
-	0x64b6080e,
-	0x3411f404,
-	0xf40046b0,
-	0x76bbd81b,
-	0x0465b600,
-	0x659450f9,
-	0x0256bb04,
-	0x75fd50bd,
-	0xf550fc04,
-	0xb6084f21,
-	0x11f40464,
-	0x0076bb0f,
-	0xf40136b0,
-	0x32f4061b,
-/* 0x0941: i2c_put_byte_done */
-/* 0x0943: i2c_addr */
-	0xbb00f801,
+	0x64b609b1,
+	0x2b11f404,
+	0xb60553fd,
+	0x1bf40142,
+	0x0137f0d8,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0x7021f550,
+	0x0464b609,
+/* 0x0a48: i2c_get_byte_done */
+/* 0x0a4a: i2c_put_byte */
+	0x47f000f8,
+/* 0x0a4d: i2c_put_byte_next */
+	0x0142b608,
+	0xbb3854ff,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x077f21f5,
+	0x097021f5,
 	0xf40464b6,
-	0xc3e72911,
-	0x34b6012e,
-	0x0553fd01,
+	0x46b03411,
+	0xd81bf400,
 	0xb60076bb,
 	0x50f90465,
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0xe821f550,
-	0x0464b608,
-/* 0x0988: i2c_addr_done */
-/* 0x098a: i2c_acquire_addr */
-	0xcec700f8,
-	0x02e4b6f8,
-	0x0c10e0b7,
-	0xf800ee98,
-/* 0x0999: i2c_acquire */
-	0x8a21f500,
-	0x0421f409,
-	0xf403d9f0,
-	0x00f83f21,
-/* 0x09a8: i2c_release */
-	0x098a21f5,
-	0xf00421f4,
-	0x21f403da,
-/* 0x09b7: i2c_recv */
-	0xf400f83f,
-	0xc1c70132,
-	0x0214b6f8,
-	0xf52816b0,
-	0xa0013a1f,
-	0x980be813,
-	0x13a00032,
-	0x31980bc0,
-	0x0231f400,
-	0xe0f9d0f9,
-	0x67f1d0f9,
-	0x63f10000,
-	0x67921000,
-	0x0076bb01,
-	0xf90465b6,
-	0x04659450,
-	0xbd0256bb,
-	0x0475fd50,
-	0x21f550fc,
-	0x64b60999,
-	0xb0d0fc04,
-	0x1bf500d6,
-	0x57f000b3,
+	0xb121f550,
+	0x0464b609,
+	0xbb0f11f4,
+	0x36b00076,
+	0x061bf401,
+/* 0x0aa3: i2c_put_byte_done */
+	0xf80132f4,
+/* 0x0aa5: i2c_addr */
 	0x0076bb00,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
 	0x21f550fc,
-	0x64b60943,
-	0xd011f504,
-	0xe0c5c700,
-	0xb60076bb,
-	0x50f90465,
-	0xbb046594,
-	0x50bd0256,
-	0xfc0475fd,
-	0xe821f550,
-	0x0464b608,
-	0x00ad11f5,
-	0xbb0157f0,
+	0x64b608e1,
+	0x2911f404,
+	0x012ec3e7,
+	0xfd0134b6,
+	0x76bb0553,
+	0x0465b600,
+	0x659450f9,
+	0x0256bb04,
+	0x75fd50bd,
+	0xf550fc04,
+	0xb60a4a21,
+/* 0x0aea: i2c_addr_done */
+	0x00f80464,
+/* 0x0aec: i2c_acquire_addr */
+	0xb6f8cec7,
+	0xe0b702e4,
+	0xee980d1c,
+/* 0x0afb: i2c_acquire */
+	0xf500f800,
+	0xf40aec21,
+	0xd9f00421,
+	0x3f21f403,
+/* 0x0b0a: i2c_release */
+	0x21f500f8,
+	0x21f40aec,
+	0x03daf004,
+	0xf83f21f4,
+/* 0x0b19: i2c_recv */
+	0x0132f400,
+	0xb6f8c1c7,
+	0x16b00214,
+	0x3a1ff528,
+	0xf413a001,
+	0x0032980c,
+	0x0ccc13a0,
+	0xf4003198,
+	0xd0f90231,
+	0xd0f9e0f9,
+	0x000067f1,
+	0x100063f1,
+	0xbb016792,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x094321f5,
-	0xf50464b6,
-	0xbb008a11,
+	0x0afb21f5,
+	0xfc0464b6,
+	0x00d6b0d0,
+	0x00b31bf5,
+	0xbb0057f0,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x089621f5,
-	0xf40464b6,
-	0x5bcb6a11,
-	0x0076bbe0,
+	0x0aa521f5,
+	0xf50464b6,
+	0xc700d011,
+	0x76bbe0c5,
+	0x0465b600,
+	0x659450f9,
+	0x0256bb04,
+	0x75fd50bd,
+	0xf550fc04,
+	0xb60a4a21,
+	0x11f50464,
+	0x57f000ad,
+	0x0076bb01,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
 	0x21f550fc,
-	0x64b607db,
-	0x025bb904,
-	0x0ef474bd,
-/* 0x0abd: i2c_recv_not_rd08 */
-	0x01d6b043,
-	0xf03d1bf4,
-	0x21f50057,
-	0x11f40943,
-	0xe0c5c733,
-	0x08e821f5,
-	0xf02911f4,
-	0x21f50057,
-	0x11f40943,
-	0xe0b5c71f,
-	0x08e821f5,
-	0xf51511f4,
-	0xbd07db21,
-	0x08c5c774,
-	0xf4091bf4,
-	0x0ef40232,
-/* 0x0afd: i2c_recv_not_wr08 */
-/* 0x0afd: i2c_recv_done */
-	0xf8cec703,
-	0x09a821f5,
-	0xd0fce0fc,
-	0xb90a12f4,
-	0x21f5027c,
-/* 0x0b12: i2c_recv_exit */
-	0x00f80342,
-/* 0x0b14: i2c_init */
-/* 0x0b16: test_recv */
-	0x17f100f8,
-	0x14b605d8,
-	0x0011cf06,
-	0xf10110b6,
-	0xb605d807,
-	0x01d00604,
-	0xf104bd00,
-	0xf1d900e7,
-	0xf5134fe3,
-	0xf8026221,
-/* 0x0b3d: test_init */
-	0x00e7f100,
-	0x6221f508,
-/* 0x0b47: idle_recv */
-	0xf800f802,
-/* 0x0b49: idle */
-	0x0031f400,
-	0x05d417f1,
+	0x64b60aa5,
+	0x8a11f504,
+	0x0076bb00,
+	0xf90465b6,
+	0x04659450,
+	0xbd0256bb,
+	0x0475fd50,
+	0x21f550fc,
+	0x64b609f8,
+	0x6a11f404,
+	0xbbe05bcb,
+	0x65b60076,
+	0x9450f904,
+	0x56bb0465,
+	0xfd50bd02,
+	0x50fc0475,
+	0x093d21f5,
+	0xb90464b6,
+	0x74bd025b,
+/* 0x0c1f: i2c_recv_not_rd08 */
+	0xb0430ef4,
+	0x1bf401d6,
+	0x0057f03d,
+	0x0aa521f5,
+	0xc73311f4,
+	0x21f5e0c5,
+	0x11f40a4a,
+	0x0057f029,
+	0x0aa521f5,
+	0xc71f11f4,
+	0x21f5e0b5,
+	0x11f40a4a,
+	0x3d21f515,
+	0xc774bd09,
+	0x1bf408c5,
+	0x0232f409,
+/* 0x0c5f: i2c_recv_not_wr08 */
+/* 0x0c5f: i2c_recv_done */
+	0xc7030ef4,
+	0x21f5f8ce,
+	0xe0fc0b0a,
+	0x12f4d0fc,
+	0x027cb90a,
+	0x034221f5,
+/* 0x0c74: i2c_recv_exit */
+/* 0x0c76: i2c_init */
+	0x00f800f8,
+/* 0x0c78: test_recv */
+	0x05d817f1,
 	0xcf0614b6,
 	0x10b60011,
-	0xd407f101,
+	0xd807f101,
 	0x0604b605,
 	0xbd0001d0,
-/* 0x0b65: idle_loop */
-	0x5817f004,
-/* 0x0b6b: idle_proc */
-/* 0x0b6b: idle_proc_exec */
-	0xf90232f4,
-	0x021eb910,
-	0x034b21f5,
-	0x11f410fc,
-	0x0231f409,
-/* 0x0b7f: idle_proc_next */
-	0xb6ef0ef4,
-	0x1fb85810,
-	0xe61bf406,
-	0xf4dd02f4,
-	0x0ef40028,
-	0x000000bb,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
+	0x00e7f104,
+	0x4fe3f1d9,
+	0x6221f513,
+/* 0x0c9f: test_init */
+	0xf100f802,
+	0xf50800e7,
+	0xf8026221,
+/* 0x0ca9: idle_recv */
+/* 0x0cab: idle */
+	0xf400f800,
+	0x17f10031,
+	0x14b605d4,
+	0x0011cf06,
+	0xf10110b6,
+	0xb605d407,
+	0x01d00604,
+/* 0x0cc7: idle_loop */
+	0xf004bd00,
+	0x32f45817,
+/* 0x0ccd: idle_proc */
+/* 0x0ccd: idle_proc_exec */
+	0xb910f902,
+	0x21f5021e,
+	0x10fc034b,
+	0xf40911f4,
+	0x0ef40231,
+/* 0x0ce1: idle_proc_next */
+	0x5810b6ef,
+	0xf4061fb8,
+	0x02f4e61b,
+	0x0028f4dd,
+	0x00bb0ef4,
 	0x00000000,
 	0x00000000,
 	0x00000000,
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvc0.fuc.h b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvc0.fuc.h
index ca30fa4011b5..90221d973f84 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvc0.fuc.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvc0.fuc.h
@@ -46,8 +46,8 @@ uint32_t nvc0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x584d454d,
-	0x0000074b,
-	0x0000073d,
+	0x0000075e,
+	0x00000750,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -68,8 +68,8 @@ uint32_t nvc0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x46524550,
-	0x0000074f,
-	0x0000074d,
+	0x00000762,
+	0x00000760,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -90,8 +90,8 @@ uint32_t nvc0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x5f433249,
-	0x00000b7f,
-	0x00000a22,
+	0x00000b92,
+	0x00000a35,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -112,8 +112,8 @@ uint32_t nvc0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x54534554,
-	0x00000ba8,
-	0x00000b81,
+	0x00000bbb,
+	0x00000b94,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -134,8 +134,8 @@ uint32_t nvc0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x454c4449,
-	0x00000bb4,
-	0x00000bb2,
+	0x00000bc7,
+	0x00000bc5,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -246,13 +246,15 @@ uint32_t nvc0_pwr_data[] = {
 	0x00010006,
 	0x00000000,
 	0x00000663,
-/* 0x03b8: memx_func_tail */
-/* 0x03b8: memx_ts_start */
+	0x00000007,
 	0x00000000,
-/* 0x03bc: memx_ts_end */
+	0x000006e9,
+/* 0x03c4: memx_func_tail */
+/* 0x03c4: memx_ts_start */
 	0x00000000,
-/* 0x03c0: memx_data_head */
+/* 0x03c8: memx_ts_end */
 	0x00000000,
+/* 0x03cc: memx_data_head */
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -764,8 +766,75 @@ uint32_t nvc0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-/* 0x0bc0: memx_data_tail */
-/* 0x0bc0: i2c_scl_map */
+	0x00000000,
+/* 0x0bcc: memx_data_tail */
+/* 0x0bcc: memx_train_head */
+	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,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+/* 0x0ccc: memx_train_tail */
+/* 0x0ccc: i2c_scl_map */
 	0x00001000,
 	0x00004000,
 	0x00010000,
@@ -776,7 +845,7 @@ uint32_t nvc0_pwr_data[] = {
 	0x01000000,
 	0x04000000,
 	0x10000000,
-/* 0x0be8: i2c_sda_map */
+/* 0x0cf4: i2c_sda_map */
 	0x00002000,
 	0x00008000,
 	0x00020000,
@@ -787,7 +856,7 @@ uint32_t nvc0_pwr_data[] = {
 	0x02000000,
 	0x08000000,
 	0x20000000,
-/* 0x0c10: i2c_ctrl */
+/* 0x0d1c: i2c_ctrl */
 	0x0000e138,
 	0x0000e150,
 	0x0000e168,
@@ -845,9 +914,6 @@ uint32_t nvc0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
 };
 
 uint32_t nvc0_pwr_code[] = {
@@ -1272,10 +1338,10 @@ uint32_t nvc0_pwr_code[] = {
 	0xcf0664b6,
 	0x06800066,
 /* 0x05db: memx_func_leave */
-	0xf000f8ee,
+	0xf000f8f1,
 	0x64b62c67,
 	0x0066cf06,
-	0xf0ef0680,
+	0xf0f20680,
 	0x07f10467,
 	0x04b607e4,
 	0x0006d006,
@@ -1350,382 +1416,450 @@ uint32_t nvc0_pwr_code[] = {
 	0x1e9800f8,
 	0x0410b600,
 	0xf87f21f4,
-/* 0x06e9: memx_exec */
-	0xf9e0f900,
-	0x02c1b9d0,
-/* 0x06f3: memx_exec_next */
-	0x9802b2b9,
-	0x10b60013,
-	0xf034e704,
-	0xe033e701,
-	0x0132b601,
-	0x980c30f0,
-	0x55f9de35,
-	0xf40612b8,
-	0x0b98e41e,
-	0xef0c98ee,
-	0xf102cbbb,
-	0xb607c4b7,
-	0xbbcf06b4,
-	0xfcd0fc00,
-	0x4221f5e0,
-/* 0x072f: memx_info */
-	0xf100f803,
-	0xf103c0c7,
-	0xf50800b7,
+/* 0x06e9: memx_func_train */
+/* 0x06eb: memx_exec */
+	0xf900f800,
+	0xb9d0f9e0,
+	0xb2b902c1,
+/* 0x06f5: memx_exec_next */
+	0x00139802,
+	0xe70410b6,
+	0xe701f034,
+	0xb601e033,
+	0x30f00132,
+	0xde35980c,
+	0x12b855f9,
+	0xe41ef406,
+	0x98f10b98,
+	0xcbbbf20c,
+	0xc4b7f102,
+	0x06b4b607,
+	0xfc00bbcf,
+	0xf5e0fcd0,
 	0xf8034221,
-/* 0x073d: memx_recv */
-	0x01d6b000,
-	0xb0a90bf4,
-	0x0bf400d6,
-/* 0x074b: memx_init */
-	0xf800f8e9,
-/* 0x074d: perf_recv */
-/* 0x074f: perf_init */
-	0xf800f800,
-/* 0x0751: i2c_drive_scl */
-	0x0036b000,
-	0xf1110bf4,
-	0xb607e007,
-	0x01d00604,
-	0xf804bd00,
-/* 0x0765: i2c_drive_scl_lo */
-	0xe407f100,
-	0x0604b607,
-	0xbd0001d0,
-/* 0x0773: i2c_drive_sda */
-	0xb000f804,
-	0x0bf40036,
-	0xe007f111,
-	0x0604b607,
-	0xbd0002d0,
-/* 0x0787: i2c_drive_sda_lo */
-	0xf100f804,
-	0xb607e407,
-	0x02d00604,
-	0xf804bd00,
-/* 0x0795: i2c_sense_scl */
-	0x0132f400,
-	0x07c437f1,
-	0xcf0634b6,
-	0x31fd0033,
-	0x060bf404,
-/* 0x07ab: i2c_sense_scl_done */
-	0xf80131f4,
-/* 0x07ad: i2c_sense_sda */
-	0x0132f400,
-	0x07c437f1,
-	0xcf0634b6,
-	0x32fd0033,
-	0x060bf404,
-/* 0x07c3: i2c_sense_sda_done */
-	0xf80131f4,
-/* 0x07c5: i2c_raise_scl */
-	0xf140f900,
-	0xf0089847,
-	0x21f50137,
-/* 0x07d2: i2c_raise_scl_wait */
-	0xe7f10751,
-	0x21f403e8,
-	0x9521f57f,
-	0x0901f407,
-	0xf40142b6,
-/* 0x07e6: i2c_raise_scl_done */
-	0x40fcef1b,
-/* 0x07ea: i2c_start */
-	0x21f500f8,
-	0x11f40795,
-	0xad21f50d,
-	0x0611f407,
-/* 0x07fb: i2c_start_rep */
-	0xf0300ef4,
-	0x21f50037,
-	0x37f00751,
-	0x7321f501,
-	0x0076bb07,
-	0xf90465b6,
-	0x04659450,
-	0xbd0256bb,
-	0x0475fd50,
-	0x21f550fc,
-	0x64b607c5,
-	0x1f11f404,
-/* 0x0828: i2c_start_send */
-	0xf50037f0,
-	0xf1077321,
-	0xf41388e7,
-	0x37f07f21,
-	0x5121f500,
-	0x88e7f107,
-	0x7f21f413,
-/* 0x0844: i2c_start_out */
-/* 0x0846: i2c_stop */
-	0x37f000f8,
-	0x5121f500,
-	0x0037f007,
-	0x077321f5,
-	0x03e8e7f1,
-	0xf07f21f4,
-	0x21f50137,
-	0xe7f10751,
-	0x21f41388,
-	0x0137f07f,
-	0x077321f5,
-	0x1388e7f1,
-	0xf87f21f4,
-/* 0x0879: i2c_bitw */
-	0x7321f500,
+/* 0x0731: memx_info */
+	0x01c67000,
+/* 0x0737: memx_info_data */
+	0xf10e0bf4,
+	0xf103ccc7,
+	0xf40800b7,
+/* 0x0742: memx_info_train */
+	0xc7f10b0e,
+	0xb7f10bcc,
+/* 0x074a: memx_info_send */
+	0x21f50100,
+	0x00f80342,
+/* 0x0750: memx_recv */
+	0xf401d6b0,
+	0xd6b0980b,
+	0xd80bf400,
+/* 0x075e: memx_init */
+	0x00f800f8,
+/* 0x0760: perf_recv */
+/* 0x0762: perf_init */
+	0x00f800f8,
+/* 0x0764: i2c_drive_scl */
+	0xf40036b0,
+	0x07f1110b,
+	0x04b607e0,
+	0x0001d006,
+	0x00f804bd,
+/* 0x0778: i2c_drive_scl_lo */
+	0x07e407f1,
+	0xd00604b6,
+	0x04bd0001,
+/* 0x0786: i2c_drive_sda */
+	0x36b000f8,
+	0x110bf400,
+	0x07e007f1,
+	0xd00604b6,
+	0x04bd0002,
+/* 0x079a: i2c_drive_sda_lo */
+	0x07f100f8,
+	0x04b607e4,
+	0x0002d006,
+	0x00f804bd,
+/* 0x07a8: i2c_sense_scl */
+	0xf10132f4,
+	0xb607c437,
+	0x33cf0634,
+	0x0431fd00,
+	0xf4060bf4,
+/* 0x07be: i2c_sense_scl_done */
+	0x00f80131,
+/* 0x07c0: i2c_sense_sda */
+	0xf10132f4,
+	0xb607c437,
+	0x33cf0634,
+	0x0432fd00,
+	0xf4060bf4,
+/* 0x07d6: i2c_sense_sda_done */
+	0x00f80131,
+/* 0x07d8: i2c_raise_scl */
+	0x47f140f9,
+	0x37f00898,
+	0x6421f501,
+/* 0x07e5: i2c_raise_scl_wait */
 	0xe8e7f107,
 	0x7f21f403,
+	0x07a821f5,
+	0xb60901f4,
+	0x1bf40142,
+/* 0x07f9: i2c_raise_scl_done */
+	0xf840fcef,
+/* 0x07fd: i2c_start */
+	0xa821f500,
+	0x0d11f407,
+	0x07c021f5,
+	0xf40611f4,
+/* 0x080e: i2c_start_rep */
+	0x37f0300e,
+	0x6421f500,
+	0x0137f007,
+	0x078621f5,
 	0xb60076bb,
 	0x50f90465,
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0xc521f550,
+	0xd821f550,
 	0x0464b607,
-	0xf11811f4,
-	0xf41388e7,
+/* 0x083b: i2c_start_send */
+	0xf01f11f4,
+	0x21f50037,
+	0xe7f10786,
+	0x21f41388,
+	0x0037f07f,
+	0x076421f5,
+	0x1388e7f1,
+/* 0x0857: i2c_start_out */
+	0xf87f21f4,
+/* 0x0859: i2c_stop */
+	0x0037f000,
+	0x076421f5,
+	0xf50037f0,
+	0xf1078621,
+	0xf403e8e7,
 	0x37f07f21,
-	0x5121f500,
+	0x6421f501,
 	0x88e7f107,
 	0x7f21f413,
-/* 0x08b8: i2c_bitw_out */
-/* 0x08ba: i2c_bitr */
-	0x37f000f8,
-	0x7321f501,
-	0xe8e7f107,
-	0x7f21f403,
-	0xb60076bb,
-	0x50f90465,
-	0xbb046594,
-	0x50bd0256,
-	0xfc0475fd,
-	0xc521f550,
-	0x0464b607,
-	0xf51b11f4,
-	0xf007ad21,
-	0x21f50037,
-	0xe7f10751,
+	0xf50137f0,
+	0xf1078621,
+	0xf41388e7,
+	0x00f87f21,
+/* 0x088c: i2c_bitw */
+	0x078621f5,
+	0x03e8e7f1,
+	0xbb7f21f4,
+	0x65b60076,
+	0x9450f904,
+	0x56bb0465,
+	0xfd50bd02,
+	0x50fc0475,
+	0x07d821f5,
+	0xf40464b6,
+	0xe7f11811,
 	0x21f41388,
-	0x013cf07f,
-/* 0x08ff: i2c_bitr_done */
-	0xf80131f4,
-/* 0x0901: i2c_get_byte */
-	0x0057f000,
-/* 0x0907: i2c_get_byte_next */
-	0xb60847f0,
-	0x76bb0154,
-	0x0465b600,
-	0x659450f9,
-	0x0256bb04,
-	0x75fd50bd,
-	0xf550fc04,
-	0xb608ba21,
-	0x11f40464,
-	0x0553fd2b,
-	0xf40142b6,
-	0x37f0d81b,
+	0x0037f07f,
+	0x076421f5,
+	0x1388e7f1,
+/* 0x08cb: i2c_bitw_out */
+	0xf87f21f4,
+/* 0x08cd: i2c_bitr */
+	0x0137f000,
+	0x078621f5,
+	0x03e8e7f1,
+	0xbb7f21f4,
+	0x65b60076,
+	0x9450f904,
+	0x56bb0465,
+	0xfd50bd02,
+	0x50fc0475,
+	0x07d821f5,
+	0xf40464b6,
+	0x21f51b11,
+	0x37f007c0,
+	0x6421f500,
+	0x88e7f107,
+	0x7f21f413,
+	0xf4013cf0,
+/* 0x0912: i2c_bitr_done */
+	0x00f80131,
+/* 0x0914: i2c_get_byte */
+	0xf00057f0,
+/* 0x091a: i2c_get_byte_next */
+	0x54b60847,
 	0x0076bb01,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
 	0x21f550fc,
-	0x64b60879,
-/* 0x0951: i2c_get_byte_done */
-/* 0x0953: i2c_put_byte */
-	0xf000f804,
-/* 0x0956: i2c_put_byte_next */
-	0x42b60847,
-	0x3854ff01,
+	0x64b608cd,
+	0x2b11f404,
+	0xb60553fd,
+	0x1bf40142,
+	0x0137f0d8,
 	0xb60076bb,
 	0x50f90465,
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0x7921f550,
+	0x8c21f550,
 	0x0464b608,
-	0xb03411f4,
-	0x1bf40046,
-	0x0076bbd8,
+/* 0x0964: i2c_get_byte_done */
+/* 0x0966: i2c_put_byte */
+	0x47f000f8,
+/* 0x0969: i2c_put_byte_next */
+	0x0142b608,
+	0xbb3854ff,
+	0x65b60076,
+	0x9450f904,
+	0x56bb0465,
+	0xfd50bd02,
+	0x50fc0475,
+	0x088c21f5,
+	0xf40464b6,
+	0x46b03411,
+	0xd81bf400,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0xcd21f550,
+	0x0464b608,
+	0xbb0f11f4,
+	0x36b00076,
+	0x061bf401,
+/* 0x09bf: i2c_put_byte_done */
+	0xf80132f4,
+/* 0x09c1: i2c_addr */
+	0x0076bb00,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
 	0x21f550fc,
-	0x64b608ba,
-	0x0f11f404,
-	0xb00076bb,
-	0x1bf40136,
-	0x0132f406,
-/* 0x09ac: i2c_put_byte_done */
-/* 0x09ae: i2c_addr */
-	0x76bb00f8,
+	0x64b607fd,
+	0x2911f404,
+	0x012ec3e7,
+	0xfd0134b6,
+	0x76bb0553,
 	0x0465b600,
 	0x659450f9,
 	0x0256bb04,
 	0x75fd50bd,
 	0xf550fc04,
-	0xb607ea21,
-	0x11f40464,
-	0x2ec3e729,
-	0x0134b601,
-	0xbb0553fd,
+	0xb6096621,
+/* 0x0a06: i2c_addr_done */
+	0x00f80464,
+/* 0x0a08: i2c_acquire_addr */
+	0xb6f8cec7,
+	0xe0b702e4,
+	0xee980d1c,
+/* 0x0a17: i2c_acquire */
+	0xf500f800,
+	0xf40a0821,
+	0xd9f00421,
+	0x3f21f403,
+/* 0x0a26: i2c_release */
+	0x21f500f8,
+	0x21f40a08,
+	0x03daf004,
+	0xf83f21f4,
+/* 0x0a35: i2c_recv */
+	0x0132f400,
+	0xb6f8c1c7,
+	0x16b00214,
+	0x3a1ff528,
+	0xf413a001,
+	0x0032980c,
+	0x0ccc13a0,
+	0xf4003198,
+	0xd0f90231,
+	0xd0f9e0f9,
+	0x000067f1,
+	0x100063f1,
+	0xbb016792,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x095321f5,
-/* 0x09f3: i2c_addr_done */
-	0xf80464b6,
-/* 0x09f5: i2c_acquire_addr */
-	0xf8cec700,
-	0xb702e4b6,
-	0x980c10e0,
-	0x00f800ee,
-/* 0x0a04: i2c_acquire */
-	0x09f521f5,
-	0xf00421f4,
-	0x21f403d9,
-/* 0x0a13: i2c_release */
-	0xf500f83f,
-	0xf409f521,
-	0xdaf00421,
-	0x3f21f403,
-/* 0x0a22: i2c_recv */
-	0x32f400f8,
-	0xf8c1c701,
-	0xb00214b6,
-	0x1ff52816,
-	0x13a0013a,
-	0x32980be8,
-	0xc013a000,
-	0x0031980b,
-	0xf90231f4,
-	0xf9e0f9d0,
-	0x0067f1d0,
-	0x0063f100,
-	0x01679210,
-	0xb60076bb,
-	0x50f90465,
-	0xbb046594,
-	0x50bd0256,
-	0xfc0475fd,
-	0x0421f550,
-	0x0464b60a,
-	0xd6b0d0fc,
-	0xb31bf500,
-	0x0057f000,
-	0xb60076bb,
-	0x50f90465,
-	0xbb046594,
-	0x50bd0256,
-	0xfc0475fd,
-	0xae21f550,
-	0x0464b609,
-	0x00d011f5,
-	0xbbe0c5c7,
+	0x0a1721f5,
+	0xfc0464b6,
+	0x00d6b0d0,
+	0x00b31bf5,
+	0xbb0057f0,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x095321f5,
+	0x09c121f5,
 	0xf50464b6,
-	0xf000ad11,
-	0x76bb0157,
+	0xc700d011,
+	0x76bbe0c5,
 	0x0465b600,
 	0x659450f9,
 	0x0256bb04,
 	0x75fd50bd,
 	0xf550fc04,
-	0xb609ae21,
+	0xb6096621,
 	0x11f50464,
-	0x76bb008a,
-	0x0465b600,
-	0x659450f9,
-	0x0256bb04,
-	0x75fd50bd,
-	0xf550fc04,
-	0xb6090121,
-	0x11f40464,
-	0xe05bcb6a,
-	0xb60076bb,
-	0x50f90465,
-	0xbb046594,
-	0x50bd0256,
-	0xfc0475fd,
-	0x4621f550,
-	0x0464b608,
-	0xbd025bb9,
-	0x430ef474,
-/* 0x0b28: i2c_recv_not_rd08 */
-	0xf401d6b0,
-	0x57f03d1b,
-	0xae21f500,
-	0x3311f409,
-	0xf5e0c5c7,
-	0xf4095321,
-	0x57f02911,
-	0xae21f500,
-	0x1f11f409,
-	0xf5e0b5c7,
-	0xf4095321,
-	0x21f51511,
-	0x74bd0846,
-	0xf408c5c7,
-	0x32f4091b,
-	0x030ef402,
-/* 0x0b68: i2c_recv_not_wr08 */
-/* 0x0b68: i2c_recv_done */
-	0xf5f8cec7,
-	0xfc0a1321,
-	0xf4d0fce0,
-	0x7cb90a12,
-	0x4221f502,
-/* 0x0b7d: i2c_recv_exit */
-/* 0x0b7f: i2c_init */
-	0xf800f803,
-/* 0x0b81: test_recv */
-	0xd817f100,
-	0x0614b605,
-	0xb60011cf,
-	0x07f10110,
-	0x04b605d8,
-	0x0001d006,
-	0xe7f104bd,
-	0xe3f1d900,
-	0x21f5134f,
-	0x00f80262,
-/* 0x0ba8: test_init */
-	0x0800e7f1,
-	0x026221f5,
-/* 0x0bb2: idle_recv */
+	0x57f000ad,
+	0x0076bb01,
+	0xf90465b6,
+	0x04659450,
+	0xbd0256bb,
+	0x0475fd50,
+	0x21f550fc,
+	0x64b609c1,
+	0x8a11f504,
+	0x0076bb00,
+	0xf90465b6,
+	0x04659450,
+	0xbd0256bb,
+	0x0475fd50,
+	0x21f550fc,
+	0x64b60914,
+	0x6a11f404,
+	0xbbe05bcb,
+	0x65b60076,
+	0x9450f904,
+	0x56bb0465,
+	0xfd50bd02,
+	0x50fc0475,
+	0x085921f5,
+	0xb90464b6,
+	0x74bd025b,
+/* 0x0b3b: i2c_recv_not_rd08 */
+	0xb0430ef4,
+	0x1bf401d6,
+	0x0057f03d,
+	0x09c121f5,
+	0xc73311f4,
+	0x21f5e0c5,
+	0x11f40966,
+	0x0057f029,
+	0x09c121f5,
+	0xc71f11f4,
+	0x21f5e0b5,
+	0x11f40966,
+	0x5921f515,
+	0xc774bd08,
+	0x1bf408c5,
+	0x0232f409,
+/* 0x0b7b: i2c_recv_not_wr08 */
+/* 0x0b7b: i2c_recv_done */
+	0xc7030ef4,
+	0x21f5f8ce,
+	0xe0fc0a26,
+	0x12f4d0fc,
+	0x027cb90a,
+	0x034221f5,
+/* 0x0b90: i2c_recv_exit */
+/* 0x0b92: i2c_init */
 	0x00f800f8,
-/* 0x0bb4: idle */
-	0xf10031f4,
-	0xb605d417,
-	0x11cf0614,
-	0x0110b600,
-	0x05d407f1,
-	0xd00604b6,
-	0x04bd0001,
-/* 0x0bd0: idle_loop */
-	0xf45817f0,
-/* 0x0bd6: idle_proc */
-/* 0x0bd6: idle_proc_exec */
-	0x10f90232,
-	0xf5021eb9,
-	0xfc034b21,
-	0x0911f410,
-	0xf40231f4,
-/* 0x0bea: idle_proc_next */
-	0x10b6ef0e,
-	0x061fb858,
-	0xf4e61bf4,
-	0x28f4dd02,
-	0xbb0ef400,
+/* 0x0b94: test_recv */
+	0x05d817f1,
+	0xcf0614b6,
+	0x10b60011,
+	0xd807f101,
+	0x0604b605,
+	0xbd0001d0,
+	0x00e7f104,
+	0x4fe3f1d9,
+	0x6221f513,
+/* 0x0bbb: test_init */
+	0xf100f802,
+	0xf50800e7,
+	0xf8026221,
+/* 0x0bc5: idle_recv */
+/* 0x0bc7: idle */
+	0xf400f800,
+	0x17f10031,
+	0x14b605d4,
+	0x0011cf06,
+	0xf10110b6,
+	0xb605d407,
+	0x01d00604,
+/* 0x0be3: idle_loop */
+	0xf004bd00,
+	0x32f45817,
+/* 0x0be9: idle_proc */
+/* 0x0be9: idle_proc_exec */
+	0xb910f902,
+	0x21f5021e,
+	0x10fc034b,
+	0xf40911f4,
+	0x0ef40231,
+/* 0x0bfd: idle_proc_next */
+	0x5810b6ef,
+	0xf4061fb8,
+	0x02f4e61b,
+	0x0028f4dd,
+	0x00bb0ef4,
+	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,
+	0x00000000,
 	0x00000000,
 };
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvd0.fuc.h b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvd0.fuc.h
index 12d86f72ad10..7e16aab44d85 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvd0.fuc.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvd0.fuc.h
@@ -46,8 +46,8 @@ uint32_t nvd0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x584d454d,
-	0x00000678,
-	0x0000066a,
+	0x0000068b,
+	0x0000067d,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -68,8 +68,8 @@ uint32_t nvd0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x46524550,
-	0x0000067c,
-	0x0000067a,
+	0x0000068f,
+	0x0000068d,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -90,8 +90,8 @@ uint32_t nvd0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x5f433249,
-	0x00000a97,
-	0x0000093a,
+	0x00000aaa,
+	0x0000094d,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -112,8 +112,8 @@ uint32_t nvd0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x54534554,
-	0x00000aba,
-	0x00000a99,
+	0x00000acd,
+	0x00000aac,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -134,8 +134,8 @@ uint32_t nvd0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x454c4449,
-	0x00000ac6,
-	0x00000ac4,
+	0x00000ad9,
+	0x00000ad7,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -246,13 +246,15 @@ uint32_t nvd0_pwr_data[] = {
 	0x00010006,
 	0x00000000,
 	0x000005d3,
-/* 0x03b8: memx_func_tail */
-/* 0x03b8: memx_ts_start */
+	0x00000007,
 	0x00000000,
-/* 0x03bc: memx_ts_end */
+	0x00000619,
+/* 0x03c4: memx_func_tail */
+/* 0x03c4: memx_ts_start */
 	0x00000000,
-/* 0x03c0: memx_data_head */
+/* 0x03c8: memx_ts_end */
 	0x00000000,
+/* 0x03cc: memx_data_head */
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -764,8 +766,75 @@ uint32_t nvd0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-/* 0x0bc0: memx_data_tail */
-/* 0x0bc0: i2c_scl_map */
+	0x00000000,
+/* 0x0bcc: memx_data_tail */
+/* 0x0bcc: memx_train_head */
+	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,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+/* 0x0ccc: memx_train_tail */
+/* 0x0ccc: i2c_scl_map */
 	0x00000400,
 	0x00000800,
 	0x00001000,
@@ -776,7 +845,7 @@ uint32_t nvd0_pwr_data[] = {
 	0x00020000,
 	0x00040000,
 	0x00080000,
-/* 0x0be8: i2c_sda_map */
+/* 0x0cf4: i2c_sda_map */
 	0x00100000,
 	0x00200000,
 	0x00400000,
@@ -844,9 +913,6 @@ uint32_t nvd0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
 };
 
 uint32_t nvd0_pwr_code[] = {
@@ -1236,11 +1302,11 @@ uint32_t nvd0_pwr_code[] = {
 	0x0bf40464,
 	0x2c67f0f6,
 	0x800066cf,
-	0x00f8ee06,
+	0x00f8f106,
 /* 0x0554: memx_func_leave */
 	0xcf2c67f0,
 	0x06800066,
-	0x0467f0ef,
+	0x0467f0f2,
 	0x07e407f1,
 	0xbd0006d0,
 /* 0x0569: memx_func_leave_wait */
@@ -1292,379 +1358,383 @@ uint32_t nvd0_pwr_code[] = {
 	0x1e9800f8,
 	0x0410b600,
 	0xf86721f4,
-/* 0x0619: memx_exec */
-	0xf9e0f900,
-	0x02c1b9d0,
-/* 0x0623: memx_exec_next */
-	0x9802b2b9,
-	0x10b60013,
-	0xf034e704,
-	0xe033e701,
-	0x0132b601,
-	0x980c30f0,
-	0x55f9de35,
-	0xf40612b8,
-	0x0b98e41e,
-	0xef0c98ee,
-	0xf102cbbb,
-	0xcf07c4b7,
-	0xd0fc00bb,
-	0x21f5e0fc,
-	0x00f802f1,
-/* 0x065c: memx_info */
-	0x03c0c7f1,
-	0x0800b7f1,
+/* 0x0619: memx_func_train */
+/* 0x061b: memx_exec */
+	0xf900f800,
+	0xb9d0f9e0,
+	0xb2b902c1,
+/* 0x0625: memx_exec_next */
+	0x00139802,
+	0xe70410b6,
+	0xe701f034,
+	0xb601e033,
+	0x30f00132,
+	0xde35980c,
+	0x12b855f9,
+	0xe41ef406,
+	0x98f10b98,
+	0xcbbbf20c,
+	0xc4b7f102,
+	0x00bbcf07,
+	0xe0fcd0fc,
 	0x02f121f5,
-/* 0x066a: memx_recv */
-	0xd6b000f8,
-	0xac0bf401,
-	0xf400d6b0,
-	0x00f8e90b,
-/* 0x0678: memx_init */
-/* 0x067a: perf_recv */
-	0x00f800f8,
-/* 0x067c: perf_init */
-/* 0x067e: i2c_drive_scl */
-	0x36b000f8,
-	0x0e0bf400,
-	0x07e007f1,
-	0xbd0001d0,
-/* 0x068f: i2c_drive_scl_lo */
-	0xf100f804,
-	0xd007e407,
+/* 0x065e: memx_info */
+	0xc67000f8,
+	0x0e0bf401,
+/* 0x0664: memx_info_data */
+	0x03ccc7f1,
+	0x0800b7f1,
+/* 0x066f: memx_info_train */
+	0xf10b0ef4,
+	0xf10bccc7,
+/* 0x0677: memx_info_send */
+	0xf50100b7,
+	0xf802f121,
+/* 0x067d: memx_recv */
+	0x01d6b000,
+	0xb09b0bf4,
+	0x0bf400d6,
+/* 0x068b: memx_init */
+	0xf800f8d8,
+/* 0x068d: perf_recv */
+/* 0x068f: perf_init */
+	0xf800f800,
+/* 0x0691: i2c_drive_scl */
+	0x0036b000,
+	0xf10e0bf4,
+	0xd007e007,
 	0x04bd0001,
-/* 0x069a: i2c_drive_sda */
-	0x36b000f8,
-	0x0e0bf400,
-	0x07e007f1,
-	0xbd0002d0,
-/* 0x06ab: i2c_drive_sda_lo */
-	0xf100f804,
-	0xd007e407,
+/* 0x06a2: i2c_drive_scl_lo */
+	0x07f100f8,
+	0x01d007e4,
+	0xf804bd00,
+/* 0x06ad: i2c_drive_sda */
+	0x0036b000,
+	0xf10e0bf4,
+	0xd007e007,
 	0x04bd0002,
-/* 0x06b6: i2c_sense_scl */
+/* 0x06be: i2c_drive_sda_lo */
+	0x07f100f8,
+	0x02d007e4,
+	0xf804bd00,
+/* 0x06c9: i2c_sense_scl */
+	0x0132f400,
+	0x07c437f1,
+	0xfd0033cf,
+	0x0bf40431,
+	0x0131f406,
+/* 0x06dc: i2c_sense_scl_done */
+/* 0x06de: i2c_sense_sda */
 	0x32f400f8,
 	0xc437f101,
 	0x0033cf07,
-	0xf40431fd,
+	0xf40432fd,
 	0x31f4060b,
-/* 0x06c9: i2c_sense_scl_done */
-/* 0x06cb: i2c_sense_sda */
-	0xf400f801,
-	0x37f10132,
-	0x33cf07c4,
-	0x0432fd00,
-	0xf4060bf4,
-/* 0x06de: i2c_sense_sda_done */
-	0x00f80131,
-/* 0x06e0: i2c_raise_scl */
-	0x47f140f9,
-	0x37f00898,
-	0x7e21f501,
-/* 0x06ed: i2c_raise_scl_wait */
-	0xe8e7f106,
-	0x6721f403,
-	0x06b621f5,
-	0xb60901f4,
-	0x1bf40142,
-/* 0x0701: i2c_raise_scl_done */
-	0xf840fcef,
-/* 0x0705: i2c_start */
-	0xb621f500,
-	0x0d11f406,
-	0x06cb21f5,
-	0xf40611f4,
-/* 0x0716: i2c_start_rep */
-	0x37f0300e,
-	0x7e21f500,
-	0x0137f006,
-	0x069a21f5,
-	0xb60076bb,
-	0x50f90465,
-	0xbb046594,
-	0x50bd0256,
-	0xfc0475fd,
-	0xe021f550,
-	0x0464b606,
-/* 0x0743: i2c_start_send */
-	0xf01f11f4,
-	0x21f50037,
-	0xe7f1069a,
-	0x21f41388,
-	0x0037f067,
-	0x067e21f5,
-	0x1388e7f1,
-/* 0x075f: i2c_start_out */
-	0xf86721f4,
-/* 0x0761: i2c_stop */
-	0x0037f000,
-	0x067e21f5,
-	0xf50037f0,
-	0xf1069a21,
-	0xf403e8e7,
-	0x37f06721,
-	0x7e21f501,
-	0x88e7f106,
-	0x6721f413,
-	0xf50137f0,
-	0xf1069a21,
-	0xf41388e7,
-	0x00f86721,
-/* 0x0794: i2c_bitw */
-	0x069a21f5,
+/* 0x06f1: i2c_sense_sda_done */
+/* 0x06f3: i2c_raise_scl */
+	0xf900f801,
+	0x9847f140,
+	0x0137f008,
+	0x069121f5,
+/* 0x0700: i2c_raise_scl_wait */
 	0x03e8e7f1,
-	0xbb6721f4,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x06e021f5,
-	0xf40464b6,
-	0xe7f11811,
-	0x21f41388,
-	0x0037f067,
-	0x067e21f5,
-	0x1388e7f1,
-/* 0x07d3: i2c_bitw_out */
-	0xf86721f4,
-/* 0x07d5: i2c_bitr */
-	0x0137f000,
-	0x069a21f5,
-	0x03e8e7f1,
-	0xbb6721f4,
+	0xf56721f4,
+	0xf406c921,
+	0x42b60901,
+	0xef1bf401,
+/* 0x0714: i2c_raise_scl_done */
+	0x00f840fc,
+/* 0x0718: i2c_start */
+	0x06c921f5,
+	0xf50d11f4,
+	0xf406de21,
+	0x0ef40611,
+/* 0x0729: i2c_start_rep */
+	0x0037f030,
+	0x069121f5,
+	0xf50137f0,
+	0xbb06ad21,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x06e021f5,
+	0x06f321f5,
 	0xf40464b6,
-	0x21f51b11,
-	0x37f006cb,
-	0x7e21f500,
+/* 0x0756: i2c_start_send */
+	0x37f01f11,
+	0xad21f500,
 	0x88e7f106,
 	0x6721f413,
-	0xf4013cf0,
-/* 0x081a: i2c_bitr_done */
-	0x00f80131,
-/* 0x081c: i2c_get_byte */
-	0xf00057f0,
-/* 0x0822: i2c_get_byte_next */
-	0x54b60847,
-	0x0076bb01,
-	0xf90465b6,
-	0x04659450,
-	0xbd0256bb,
-	0x0475fd50,
-	0x21f550fc,
-	0x64b607d5,
-	0x2b11f404,
-	0xb60553fd,
-	0x1bf40142,
-	0x0137f0d8,
+	0xf50037f0,
+	0xf1069121,
+	0xf41388e7,
+/* 0x0772: i2c_start_out */
+	0x00f86721,
+/* 0x0774: i2c_stop */
+	0xf50037f0,
+	0xf0069121,
+	0x21f50037,
+	0xe7f106ad,
+	0x21f403e8,
+	0x0137f067,
+	0x069121f5,
+	0x1388e7f1,
+	0xf06721f4,
+	0x21f50137,
+	0xe7f106ad,
+	0x21f41388,
+/* 0x07a7: i2c_bitw */
+	0xf500f867,
+	0xf106ad21,
+	0xf403e8e7,
+	0x76bb6721,
+	0x0465b600,
+	0x659450f9,
+	0x0256bb04,
+	0x75fd50bd,
+	0xf550fc04,
+	0xb606f321,
+	0x11f40464,
+	0x88e7f118,
+	0x6721f413,
+	0xf50037f0,
+	0xf1069121,
+	0xf41388e7,
+/* 0x07e6: i2c_bitw_out */
+	0x00f86721,
+/* 0x07e8: i2c_bitr */
+	0xf50137f0,
+	0xf106ad21,
+	0xf403e8e7,
+	0x76bb6721,
+	0x0465b600,
+	0x659450f9,
+	0x0256bb04,
+	0x75fd50bd,
+	0xf550fc04,
+	0xb606f321,
+	0x11f40464,
+	0xde21f51b,
+	0x0037f006,
+	0x069121f5,
+	0x1388e7f1,
+	0xf06721f4,
+	0x31f4013c,
+/* 0x082d: i2c_bitr_done */
+/* 0x082f: i2c_get_byte */
+	0xf000f801,
+	0x47f00057,
+/* 0x0835: i2c_get_byte_next */
+	0x0154b608,
 	0xb60076bb,
 	0x50f90465,
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0x9421f550,
+	0xe821f550,
 	0x0464b607,
-/* 0x086c: i2c_get_byte_done */
-/* 0x086e: i2c_put_byte */
-	0x47f000f8,
-/* 0x0871: i2c_put_byte_next */
-	0x0142b608,
-	0xbb3854ff,
+	0xfd2b11f4,
+	0x42b60553,
+	0xd81bf401,
+	0xbb0137f0,
+	0x65b60076,
+	0x9450f904,
+	0x56bb0465,
+	0xfd50bd02,
+	0x50fc0475,
+	0x07a721f5,
+/* 0x087f: i2c_get_byte_done */
+	0xf80464b6,
+/* 0x0881: i2c_put_byte */
+	0x0847f000,
+/* 0x0884: i2c_put_byte_next */
+	0xff0142b6,
+	0x76bb3854,
+	0x0465b600,
+	0x659450f9,
+	0x0256bb04,
+	0x75fd50bd,
+	0xf550fc04,
+	0xb607a721,
+	0x11f40464,
+	0x0046b034,
+	0xbbd81bf4,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x079421f5,
+	0x07e821f5,
 	0xf40464b6,
-	0x46b03411,
-	0xd81bf400,
+	0x76bb0f11,
+	0x0136b000,
+	0xf4061bf4,
+/* 0x08da: i2c_put_byte_done */
+	0x00f80132,
+/* 0x08dc: i2c_addr */
 	0xb60076bb,
 	0x50f90465,
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0xd521f550,
+	0x1821f550,
 	0x0464b607,
-	0xbb0f11f4,
-	0x36b00076,
-	0x061bf401,
-/* 0x08c7: i2c_put_byte_done */
-	0xf80132f4,
-/* 0x08c9: i2c_addr */
-	0x0076bb00,
+	0xe72911f4,
+	0xb6012ec3,
+	0x53fd0134,
+	0x0076bb05,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
 	0x21f550fc,
-	0x64b60705,
-	0x2911f404,
-	0x012ec3e7,
-	0xfd0134b6,
-	0x76bb0553,
-	0x0465b600,
-	0x659450f9,
-	0x0256bb04,
-	0x75fd50bd,
-	0xf550fc04,
-	0xb6086e21,
-/* 0x090e: i2c_addr_done */
-	0x00f80464,
-/* 0x0910: i2c_acquire_addr */
-	0xb6f8cec7,
-	0xe0b705e4,
-	0x00f8d014,
-/* 0x091c: i2c_acquire */
-	0x091021f5,
-	0xf00421f4,
-	0x21f403d9,
-/* 0x092b: i2c_release */
-	0xf500f833,
-	0xf4091021,
-	0xdaf00421,
+	0x64b60881,
+/* 0x0921: i2c_addr_done */
+/* 0x0923: i2c_acquire_addr */
+	0xc700f804,
+	0xe4b6f8ce,
+	0x14e0b705,
+/* 0x092f: i2c_acquire */
+	0xf500f8d0,
+	0xf4092321,
+	0xd9f00421,
 	0x3321f403,
-/* 0x093a: i2c_recv */
-	0x32f400f8,
-	0xf8c1c701,
-	0xb00214b6,
-	0x1ff52816,
-	0x13a0013a,
-	0x32980be8,
-	0xc013a000,
-	0x0031980b,
-	0xf90231f4,
-	0xf9e0f9d0,
-	0x0067f1d0,
-	0x0063f100,
-	0x01679210,
-	0xb60076bb,
-	0x50f90465,
-	0xbb046594,
-	0x50bd0256,
-	0xfc0475fd,
-	0x1c21f550,
-	0x0464b609,
-	0xd6b0d0fc,
-	0xb31bf500,
-	0x0057f000,
-	0xb60076bb,
-	0x50f90465,
-	0xbb046594,
-	0x50bd0256,
-	0xfc0475fd,
-	0xc921f550,
-	0x0464b608,
-	0x00d011f5,
-	0xbbe0c5c7,
+/* 0x093e: i2c_release */
+	0x21f500f8,
+	0x21f40923,
+	0x03daf004,
+	0xf83321f4,
+/* 0x094d: i2c_recv */
+	0x0132f400,
+	0xb6f8c1c7,
+	0x16b00214,
+	0x3a1ff528,
+	0xf413a001,
+	0x0032980c,
+	0x0ccc13a0,
+	0xf4003198,
+	0xd0f90231,
+	0xd0f9e0f9,
+	0x000067f1,
+	0x100063f1,
+	0xbb016792,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x086e21f5,
+	0x092f21f5,
+	0xfc0464b6,
+	0x00d6b0d0,
+	0x00b31bf5,
+	0xbb0057f0,
+	0x65b60076,
+	0x9450f904,
+	0x56bb0465,
+	0xfd50bd02,
+	0x50fc0475,
+	0x08dc21f5,
 	0xf50464b6,
-	0xf000ad11,
-	0x76bb0157,
+	0xc700d011,
+	0x76bbe0c5,
 	0x0465b600,
 	0x659450f9,
 	0x0256bb04,
 	0x75fd50bd,
 	0xf550fc04,
-	0xb608c921,
+	0xb6088121,
 	0x11f50464,
-	0x76bb008a,
-	0x0465b600,
-	0x659450f9,
-	0x0256bb04,
-	0x75fd50bd,
-	0xf550fc04,
-	0xb6081c21,
-	0x11f40464,
-	0xe05bcb6a,
-	0xb60076bb,
-	0x50f90465,
-	0xbb046594,
-	0x50bd0256,
-	0xfc0475fd,
-	0x6121f550,
-	0x0464b607,
-	0xbd025bb9,
-	0x430ef474,
-/* 0x0a40: i2c_recv_not_rd08 */
-	0xf401d6b0,
-	0x57f03d1b,
-	0xc921f500,
-	0x3311f408,
-	0xf5e0c5c7,
-	0xf4086e21,
-	0x57f02911,
-	0xc921f500,
-	0x1f11f408,
-	0xf5e0b5c7,
-	0xf4086e21,
-	0x21f51511,
-	0x74bd0761,
-	0xf408c5c7,
-	0x32f4091b,
-	0x030ef402,
-/* 0x0a80: i2c_recv_not_wr08 */
-/* 0x0a80: i2c_recv_done */
-	0xf5f8cec7,
-	0xfc092b21,
-	0xf4d0fce0,
-	0x7cb90a12,
-	0xf121f502,
-/* 0x0a95: i2c_recv_exit */
-/* 0x0a97: i2c_init */
+	0x57f000ad,
+	0x0076bb01,
+	0xf90465b6,
+	0x04659450,
+	0xbd0256bb,
+	0x0475fd50,
+	0x21f550fc,
+	0x64b608dc,
+	0x8a11f504,
+	0x0076bb00,
+	0xf90465b6,
+	0x04659450,
+	0xbd0256bb,
+	0x0475fd50,
+	0x21f550fc,
+	0x64b6082f,
+	0x6a11f404,
+	0xbbe05bcb,
+	0x65b60076,
+	0x9450f904,
+	0x56bb0465,
+	0xfd50bd02,
+	0x50fc0475,
+	0x077421f5,
+	0xb90464b6,
+	0x74bd025b,
+/* 0x0a53: i2c_recv_not_rd08 */
+	0xb0430ef4,
+	0x1bf401d6,
+	0x0057f03d,
+	0x08dc21f5,
+	0xc73311f4,
+	0x21f5e0c5,
+	0x11f40881,
+	0x0057f029,
+	0x08dc21f5,
+	0xc71f11f4,
+	0x21f5e0b5,
+	0x11f40881,
+	0x7421f515,
+	0xc774bd07,
+	0x1bf408c5,
+	0x0232f409,
+/* 0x0a93: i2c_recv_not_wr08 */
+/* 0x0a93: i2c_recv_done */
+	0xc7030ef4,
+	0x21f5f8ce,
+	0xe0fc093e,
+	0x12f4d0fc,
+	0x027cb90a,
+	0x02f121f5,
+/* 0x0aa8: i2c_recv_exit */
+/* 0x0aaa: i2c_init */
+	0x00f800f8,
+/* 0x0aac: test_recv */
+	0x05d817f1,
+	0xb60011cf,
+	0x07f10110,
+	0x01d005d8,
+	0xf104bd00,
+	0xf1d900e7,
+	0xf5134fe3,
+	0xf8022321,
+/* 0x0acd: test_init */
+	0x00e7f100,
+	0x2321f508,
+/* 0x0ad7: idle_recv */
 	0xf800f802,
-/* 0x0a99: test_recv */
-	0xd817f100,
-	0x0011cf05,
-	0xf10110b6,
-	0xd005d807,
-	0x04bd0001,
-	0xd900e7f1,
-	0x134fe3f1,
-	0x022321f5,
-/* 0x0aba: test_init */
-	0xe7f100f8,
-	0x21f50800,
-	0x00f80223,
-/* 0x0ac4: idle_recv */
-/* 0x0ac6: idle */
-	0x31f400f8,
-	0xd417f100,
-	0x0011cf05,
-	0xf10110b6,
-	0xd005d407,
-	0x04bd0001,
-/* 0x0adc: idle_loop */
-	0xf45817f0,
-/* 0x0ae2: idle_proc */
-/* 0x0ae2: idle_proc_exec */
-	0x10f90232,
-	0xf5021eb9,
-	0xfc02fa21,
-	0x0911f410,
-	0xf40231f4,
-/* 0x0af6: idle_proc_next */
-	0x10b6ef0e,
-	0x061fb858,
-	0xf4e61bf4,
-	0x28f4dd02,
-	0xc10ef400,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
+/* 0x0ad9: idle */
+	0x0031f400,
+	0x05d417f1,
+	0xb60011cf,
+	0x07f10110,
+	0x01d005d4,
+/* 0x0aef: idle_loop */
+	0xf004bd00,
+	0x32f45817,
+/* 0x0af5: idle_proc */
+/* 0x0af5: idle_proc_exec */
+	0xb910f902,
+	0x21f5021e,
+	0x10fc02fa,
+	0xf40911f4,
+	0x0ef40231,
+/* 0x0b09: idle_proc_next */
+	0x5810b6ef,
+	0xf4061fb8,
+	0x02f4e61b,
+	0x0028f4dd,
+	0x00c10ef4,
 	0x00000000,
 	0x00000000,
 	0x00000000,
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/os.h b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/os.h
index 522e3079f824..c8b06cb77e72 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/os.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/os.h
@@ -18,6 +18,10 @@
 #define MEMX_MSG_INFO 0
 #define MEMX_MSG_EXEC 1
 
+/* MEMX: info types */
+#define MEMX_INFO_DATA  0
+#define MEMX_INFO_TRAIN 1
+
 /* MEMX: script opcode definitions */
 #define MEMX_ENTER  1
 #define MEMX_LEAVE  2
@@ -25,6 +29,7 @@
 #define MEMX_WAIT   4
 #define MEMX_DELAY  5
 #define MEMX_VBLANK 6
+#define MEMX_TRAIN  7
 
 /* I2C_: message identifiers */
 #define I2C__MSG_RD08 0
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/memx.c b/drivers/gpu/drm/nouveau/core/subdev/pwr/memx.c
index 65eaa2546cad..7a9299d7159f 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/memx.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/memx.c
@@ -47,7 +47,8 @@ nouveau_memx_init(struct nouveau_pwr *ppwr, struct nouveau_memx **pmemx)
 	u32 reply[2];
 	int ret;
 
-	ret = ppwr->message(ppwr, reply, PROC_MEMX, MEMX_MSG_INFO, 0, 0);
+	ret = ppwr->message(ppwr, reply, PROC_MEMX, MEMX_MSG_INFO,
+					MEMX_INFO_DATA, 0);
 	if (ret)
 		return ret;
 
@@ -106,7 +107,7 @@ nouveau_memx_wait(struct nouveau_memx *memx,
 {
 	nv_debug(memx->ppwr, "R[%06x] & 0x%08x == 0x%08x, %d us\n",
 				addr, mask, data, nsec);
-	memx_cmd(memx, MEMX_WAIT, 4, (u32[]){ addr, ~mask, data, nsec });
+	memx_cmd(memx, MEMX_WAIT, 4, (u32[]){ addr, mask, data, nsec });
 	memx_out(memx); /* fuc can't handle multiple */
 }
 
@@ -152,6 +153,38 @@ nouveau_memx_wait_vblank(struct nouveau_memx *memx)
 }
 
 void
+nouveau_memx_train(struct nouveau_memx *memx)
+{
+	nv_debug(memx->ppwr, "   MEM TRAIN\n");
+	memx_cmd(memx, MEMX_TRAIN, 0, NULL);
+}
+
+int
+nouveau_memx_train_result(struct nouveau_pwr *ppwr, u32 *res, int rsize)
+{
+	u32 reply[2], base, size, i;
+	int ret;
+
+	ret = ppwr->message(ppwr, reply, PROC_MEMX, MEMX_MSG_INFO,
+					MEMX_INFO_TRAIN, 0);
+	if (ret)
+		return ret;
+
+	base = reply[0];
+	size = reply[1] >> 2;
+	if (size > rsize)
+		return -ENOMEM;
+
+	/* read the packet */
+	nv_wr32(ppwr, 0x10a1c0, 0x02000000 | base);
+
+	for (i = 0; i < size; i++)
+		res[i] = nv_rd32(ppwr, 0x10a1c4);
+
+	return 0;
+}
+
+void
 nouveau_memx_block(struct nouveau_memx *memx)
 {
 	nv_debug(memx->ppwr, "   HOST BLOCKED\n");
diff --git a/drivers/gpu/drm/nouveau/core/subdev/volt/base.c b/drivers/gpu/drm/nouveau/core/subdev/volt/base.c
index 32794a999106..26ccd8df193f 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/volt/base.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/volt/base.c
@@ -101,6 +101,41 @@ nouveau_volt_set_id(struct nouveau_volt *volt, u8 id, int condition)
 	return ret;
 }
 
+static void nouveau_volt_parse_bios(struct nouveau_bios *bios,
+		struct nouveau_volt *volt)
+{
+	struct nvbios_volt_entry ivid;
+	struct nvbios_volt info;
+	u8  ver, hdr, cnt, len;
+	u16 data;
+	int i;
+
+	data = nvbios_volt_parse(bios, &ver, &hdr, &cnt, &len, &info);
+	if (data && info.vidmask && info.base && info.step) {
+		for (i = 0; i < info.vidmask + 1; i++) {
+			if (info.base >= info.min &&
+				info.base <= info.max) {
+				volt->vid[volt->vid_nr].uv = info.base;
+				volt->vid[volt->vid_nr].vid = i;
+				volt->vid_nr++;
+			}
+			info.base += info.step;
+		}
+		volt->vid_mask = info.vidmask;
+	} else if (data && info.vidmask) {
+		for (i = 0; i < cnt; i++) {
+			data = nvbios_volt_entry_parse(bios, i, &ver, &hdr,
+							  &ivid);
+			if (data) {
+				volt->vid[volt->vid_nr].uv = ivid.voltage;
+				volt->vid[volt->vid_nr].vid = ivid.vid;
+				volt->vid_nr++;
+			}
+		}
+		volt->vid_mask = info.vidmask;
+	}
+}
+
 int
 _nouveau_volt_init(struct nouveau_object *object)
 {
@@ -136,10 +171,6 @@ nouveau_volt_create_(struct nouveau_object *parent,
 {
 	struct nouveau_bios *bios = nouveau_bios(parent);
 	struct nouveau_volt *volt;
-	struct nvbios_volt_entry ivid;
-	struct nvbios_volt info;
-	u8  ver, hdr, cnt, len;
-	u16 data;
 	int ret, i;
 
 	ret = nouveau_subdev_create_(parent, engine, oclass, 0, "VOLT",
@@ -152,31 +183,9 @@ nouveau_volt_create_(struct nouveau_object *parent,
 	volt->set = nouveau_volt_set;
 	volt->set_id = nouveau_volt_set_id;
 
-	data = nvbios_volt_parse(bios, &ver, &hdr, &cnt, &len, &info);
-	if (data && info.vidmask && info.base && info.step) {
-		for (i = 0; i < info.vidmask + 1; i++) {
-			if (info.base >= info.min &&
-			    info.base <= info.max) {
-				volt->vid[volt->vid_nr].uv = info.base;
-				volt->vid[volt->vid_nr].vid = i;
-				volt->vid_nr++;
-			}
-			info.base += info.step;
-		}
-		volt->vid_mask = info.vidmask;
-	} else
-	if (data && info.vidmask) {
-		for (i = 0; i < cnt; i++) {
-			data = nvbios_volt_entry_parse(bios, i, &ver, &hdr,
-						      &ivid);
-			if (data) {
-				volt->vid[volt->vid_nr].uv = ivid.voltage;
-				volt->vid[volt->vid_nr].vid = ivid.vid;
-				volt->vid_nr++;
-			}
-		}
-		volt->vid_mask = info.vidmask;
-	}
+	/* Assuming the non-bios device should build the voltage table later */
+	if (bios)
+		nouveau_volt_parse_bios(bios, volt);
 
 	if (volt->vid_nr) {
 		for (i = 0; i < volt->vid_nr; i++) {
diff --git a/drivers/gpu/drm/nouveau/core/subdev/volt/gk20a.c b/drivers/gpu/drm/nouveau/core/subdev/volt/gk20a.c
new file mode 100644
index 000000000000..717368ef31ac
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/volt/gk20a.c
@@ -0,0 +1,199 @@
+/*
+ * Copyright (c) 2014, NVIDIA CORPORATION. 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 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.
+ */
+
+#ifdef __KERNEL__
+#include <nouveau_platform.h>
+#endif
+#include <subdev/volt.h>
+
+struct cvb_coef {
+	int c0;
+	int c1;
+	int c2;
+	int c3;
+	int c4;
+	int c5;
+};
+
+struct gk20a_volt_priv {
+	struct nouveau_volt base;
+	struct regulator *vdd;
+};
+
+const struct cvb_coef gk20a_cvb_coef[] = {
+	/* MHz,        c0,     c1,   c2,    c3,     c4,   c5 */
+	/*  72 */ { 1209886, -36468,  515,   417, -13123,  203},
+	/* 108 */ { 1130804, -27659,  296,   298, -10834,  221},
+	/* 180 */ { 1162871, -27110,  247,   238, -10681,  268},
+	/* 252 */ { 1220458, -28654,  247,   179, -10376,  298},
+	/* 324 */ { 1280953, -30204,  247,   119,  -9766,  304},
+	/* 396 */ { 1344547, -31777,  247,   119,  -8545,  292},
+	/* 468 */ { 1420168, -34227,  269,    60,  -7172,  256},
+	/* 540 */ { 1490757, -35955,  274,    60,  -5188,  197},
+	/* 612 */ { 1599112, -42583,  398,     0,  -1831,  119},
+	/* 648 */ { 1366986, -16459, -274,     0,  -3204,   72},
+	/* 684 */ { 1391884, -17078, -274,   -60,  -1526,   30},
+	/* 708 */ { 1415522, -17497, -274,   -60,   -458,    0},
+	/* 756 */ { 1464061, -18331, -274,  -119,   1831,  -72},
+	/* 804 */ { 1524225, -20064, -254,  -119,   4272, -155},
+	/* 852 */ { 1608418, -21643, -269,     0,    763,  -48},
+};
+
+/**
+ * cvb_mv = ((c2 * speedo / s_scale + c1) * speedo / s_scale + c0)
+ */
+static inline int
+gk20a_volt_get_cvb_voltage(int speedo, int s_scale,
+		const struct cvb_coef *coef)
+{
+	int mv;
+
+	mv = DIV_ROUND_CLOSEST(coef->c2 * speedo, s_scale);
+	mv = DIV_ROUND_CLOSEST((mv + coef->c1) * speedo, s_scale) + coef->c0;
+	return mv;
+}
+
+/**
+ * cvb_t_mv =
+ * ((c2 * speedo / s_scale + c1) * speedo / s_scale + c0) +
+ * ((c3 * speedo / s_scale + c4 + c5 * T / t_scale) * T / t_scale)
+ */
+static inline int
+gk20a_volt_get_cvb_t_voltage(int speedo, int temp, int s_scale, int t_scale,
+		const struct cvb_coef *coef)
+{
+	int cvb_mv, mv;
+
+	cvb_mv = gk20a_volt_get_cvb_voltage(speedo, s_scale, coef);
+
+	mv = DIV_ROUND_CLOSEST(coef->c3 * speedo, s_scale) + coef->c4 +
+		DIV_ROUND_CLOSEST(coef->c5 * temp, t_scale);
+	mv = DIV_ROUND_CLOSEST(mv * temp, t_scale) + cvb_mv;
+	return mv;
+}
+
+static int
+gk20a_volt_calc_voltage(const struct cvb_coef *coef, int speedo)
+{
+	int mv;
+
+	mv = gk20a_volt_get_cvb_t_voltage(speedo, -10, 100, 10, coef);
+	mv = DIV_ROUND_UP(mv, 1000);
+
+	return mv * 1000;
+}
+
+static int
+gk20a_volt_vid_get(struct nouveau_volt *volt)
+{
+	struct gk20a_volt_priv *priv = (void *)volt;
+	int i, uv;
+
+	uv = regulator_get_voltage(priv->vdd);
+
+	for (i = 0; i < volt->vid_nr; i++)
+		if (volt->vid[i].uv >= uv)
+			return i;
+
+	return -EINVAL;
+}
+
+static int
+gk20a_volt_vid_set(struct nouveau_volt *volt, u8 vid)
+{
+	struct gk20a_volt_priv *priv = (void *)volt;
+
+	nv_debug(volt, "set voltage as %duv\n", volt->vid[vid].uv);
+	return regulator_set_voltage(priv->vdd, volt->vid[vid].uv, 1200000);
+}
+
+static int
+gk20a_volt_set_id(struct nouveau_volt *volt, u8 id, int condition)
+{
+	struct gk20a_volt_priv *priv = (void *)volt;
+	int prev_uv = regulator_get_voltage(priv->vdd);
+	int target_uv = volt->vid[id].uv;
+	int ret;
+
+	nv_debug(volt, "prev=%d, target=%d, condition=%d\n",
+			prev_uv, target_uv, condition);
+	if (!condition ||
+		(condition < 0 && target_uv < prev_uv) ||
+		(condition > 0 && target_uv > prev_uv)) {
+		ret = gk20a_volt_vid_set(volt, volt->vid[id].vid);
+	} else {
+		ret = 0;
+	}
+
+	return ret;
+}
+
+static int
+gk20a_volt_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
+	       struct nouveau_oclass *oclass, void *data, u32 size,
+	       struct nouveau_object **pobject)
+{
+	struct gk20a_volt_priv *priv;
+	struct nouveau_volt *volt;
+	struct nouveau_platform_device *plat;
+	int i, ret, uv;
+
+	ret = nouveau_volt_create(parent, engine, oclass, &priv);
+	*pobject = nv_object(priv);
+	if (ret)
+		return ret;
+
+	volt = &priv->base;
+
+	plat = nv_device_to_platform(nv_device(parent));
+
+	uv = regulator_get_voltage(plat->gpu->vdd);
+	nv_info(priv, "The default voltage is %duV\n", uv);
+
+	priv->vdd = plat->gpu->vdd;
+	priv->base.vid_get = gk20a_volt_vid_get;
+	priv->base.vid_set = gk20a_volt_vid_set;
+	priv->base.set_id = gk20a_volt_set_id;
+
+	volt->vid_nr = ARRAY_SIZE(gk20a_cvb_coef);
+	nv_debug(priv, "%s - vid_nr = %d\n", __func__, volt->vid_nr);
+	for (i = 0; i < volt->vid_nr; i++) {
+		volt->vid[i].vid = i;
+		volt->vid[i].uv = gk20a_volt_calc_voltage(&gk20a_cvb_coef[i],
+					plat->gpu_speedo);
+		nv_debug(priv, "%2d: vid=%d, uv=%d\n", i, volt->vid[i].vid,
+					volt->vid[i].uv);
+	}
+
+	return 0;
+}
+
+struct nouveau_oclass
+gk20a_volt_oclass = {
+	.handle = NV_SUBDEV(VOLT, 0xea),
+	.ofuncs = &(struct nouveau_ofuncs) {
+		.ctor = gk20a_volt_ctor,
+		.dtor = _nouveau_volt_dtor,
+		.init = _nouveau_volt_init,
+		.fini = _nouveau_volt_fini,
+	},
+};
diff --git a/drivers/gpu/drm/nouveau/dispnv04/crtc.c b/drivers/gpu/drm/nouveau/dispnv04/crtc.c
index fca6a1f9c20c..38402ade6835 100644
--- a/drivers/gpu/drm/nouveau/dispnv04/crtc.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/crtc.c
@@ -26,6 +26,7 @@
 
 #include <drm/drmP.h>
 #include <drm/drm_crtc_helper.h>
+#include <drm/drm_plane_helper.h>
 
 #include "nouveau_drm.h"
 #include "nouveau_reg.h"
@@ -613,7 +614,7 @@ nv_crtc_swap_fbs(struct drm_crtc *crtc, struct drm_framebuffer *old_fb)
 	struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
 	int ret;
 
-	ret = nouveau_bo_pin(nvfb->nvbo, TTM_PL_FLAG_VRAM);
+	ret = nouveau_bo_pin(nvfb->nvbo, TTM_PL_FLAG_VRAM, false);
 	if (ret == 0) {
 		if (disp->image[nv_crtc->index])
 			nouveau_bo_unpin(disp->image[nv_crtc->index]);
@@ -1129,7 +1130,7 @@ nv04_crtc_create(struct drm_device *dev, int crtc_num)
 	ret = nouveau_bo_new(dev, 64*64*4, 0x100, TTM_PL_FLAG_VRAM,
 			     0, 0x0000, NULL, NULL, &nv_crtc->cursor.nvbo);
 	if (!ret) {
-		ret = nouveau_bo_pin(nv_crtc->cursor.nvbo, TTM_PL_FLAG_VRAM);
+		ret = nouveau_bo_pin(nv_crtc->cursor.nvbo, TTM_PL_FLAG_VRAM, false);
 		if (!ret) {
 			ret = nouveau_bo_map(nv_crtc->cursor.nvbo);
 			if (ret)
diff --git a/drivers/gpu/drm/nouveau/dispnv04/overlay.c b/drivers/gpu/drm/nouveau/dispnv04/overlay.c
index 1e9056a8df94..9f2498571d09 100644
--- a/drivers/gpu/drm/nouveau/dispnv04/overlay.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/overlay.c
@@ -126,7 +126,7 @@ nv10_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 			return -ERANGE;
 	}
 
-	ret = nouveau_bo_pin(nv_fb->nvbo, TTM_PL_FLAG_VRAM);
+	ret = nouveau_bo_pin(nv_fb->nvbo, TTM_PL_FLAG_VRAM, false);
 	if (ret)
 		return ret;
 
@@ -373,7 +373,7 @@ nv04_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 	if (crtc_w < src_w || crtc_h < src_h)
 		return -ERANGE;
 
-	ret = nouveau_bo_pin(nv_fb->nvbo, TTM_PL_FLAG_VRAM);
+	ret = nouveau_bo_pin(nv_fb->nvbo, TTM_PL_FLAG_VRAM, false);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_abi16.c b/drivers/gpu/drm/nouveau/nouveau_abi16.c
index a24faa5e2a2a..d39a15000068 100644
--- a/drivers/gpu/drm/nouveau/nouveau_abi16.c
+++ b/drivers/gpu/drm/nouveau/nouveau_abi16.c
@@ -308,7 +308,7 @@ nouveau_abi16_ioctl_channel_alloc(ABI16_IOCTL_ARGS)
 	ret = nouveau_gem_new(dev, PAGE_SIZE, 0, NOUVEAU_GEM_DOMAIN_GART,
 			      0, 0, &chan->ntfy);
 	if (ret == 0)
-		ret = nouveau_bo_pin(chan->ntfy, TTM_PL_FLAG_TT);
+		ret = nouveau_bo_pin(chan->ntfy, TTM_PL_FLAG_TT, false);
 	if (ret)
 		goto done;
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_bios.c b/drivers/gpu/drm/nouveau/nouveau_bios.c
index dae2c96deef8..7df6acc8bb34 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bios.c
+++ b/drivers/gpu/drm/nouveau/nouveau_bios.c
@@ -1258,7 +1258,7 @@ olddcb_table(struct drm_device *dev)
 		return NULL;
 	}
 
-	if (dcb[0] >= 0x41) {
+	if (dcb[0] >= 0x42) {
 		NV_WARN(drm, "DCB version 0x%02x unknown\n", dcb[0]);
 		return NULL;
 	} else
@@ -1481,18 +1481,22 @@ parse_dcb20_entry(struct drm_device *dev, struct dcb_table *dcb,
 			entry->dpconf.link_bw = 540000;
 			break;
 		}
-		switch ((conf & 0x0f000000) >> 24) {
-		case 0xf:
-			entry->dpconf.link_nr = 4;
-			break;
-		case 0x3:
-			entry->dpconf.link_nr = 2;
-			break;
-		default:
-			entry->dpconf.link_nr = 1;
-			break;
+		entry->dpconf.link_nr = (conf & 0x0f000000) >> 24;
+		if (dcb->version < 0x41) {
+			switch (entry->dpconf.link_nr) {
+			case 0xf:
+				entry->dpconf.link_nr = 4;
+				break;
+			case 0x3:
+				entry->dpconf.link_nr = 2;
+				break;
+			default:
+				entry->dpconf.link_nr = 1;
+				break;
+			}
 		}
 		link = entry->dpconf.sor.link;
+		entry->i2c_index += NV_I2C_AUX(0);
 		break;
 	case DCB_OUTPUT_TMDS:
 		if (dcb->version >= 0x40) {
diff --git a/drivers/gpu/drm/nouveau/nouveau_bo.c b/drivers/gpu/drm/nouveau/nouveau_bo.c
index 3d474ac03f88..21ec561edc99 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bo.c
+++ b/drivers/gpu/drm/nouveau/nouveau_bo.c
@@ -214,6 +214,9 @@ nouveau_bo_new(struct drm_device *dev, int size, int align,
 	nvbo->tile_flags = tile_flags;
 	nvbo->bo.bdev = &drm->ttm.bdev;
 
+	if (!nv_device_is_cpu_coherent(nvkm_device(&drm->device)))
+		nvbo->force_coherent = flags & TTM_PL_FLAG_UNCACHED;
+
 	nvbo->page_shift = 12;
 	if (drm->client.vm) {
 		if (!(flags & TTM_PL_FLAG_TT) && size > 256 * 1024)
@@ -291,8 +294,9 @@ void
 nouveau_bo_placement_set(struct nouveau_bo *nvbo, uint32_t type, uint32_t busy)
 {
 	struct ttm_placement *pl = &nvbo->placement;
-	uint32_t flags = TTM_PL_MASK_CACHING |
-		(nvbo->pin_refcnt ? TTM_PL_FLAG_NO_EVICT : 0);
+	uint32_t flags = (nvbo->force_coherent ? TTM_PL_FLAG_UNCACHED :
+						 TTM_PL_MASK_CACHING) |
+			 (nvbo->pin_refcnt ? TTM_PL_FLAG_NO_EVICT : 0);
 
 	pl->placement = nvbo->placements;
 	set_placement_list(nvbo->placements, &pl->num_placement,
@@ -306,42 +310,75 @@ nouveau_bo_placement_set(struct nouveau_bo *nvbo, uint32_t type, uint32_t busy)
 }
 
 int
-nouveau_bo_pin(struct nouveau_bo *nvbo, uint32_t memtype)
+nouveau_bo_pin(struct nouveau_bo *nvbo, uint32_t memtype, bool contig)
 {
 	struct nouveau_drm *drm = nouveau_bdev(nvbo->bo.bdev);
 	struct ttm_buffer_object *bo = &nvbo->bo;
+	bool force = false, evict = false;
 	int ret;
 
 	ret = ttm_bo_reserve(bo, false, false, false, NULL);
 	if (ret)
-		goto out;
+		return ret;
 
-	if (nvbo->pin_refcnt && !(memtype & (1 << bo->mem.mem_type))) {
-		NV_ERROR(drm, "bo %p pinned elsewhere: 0x%08x vs 0x%08x\n", bo,
-			 1 << bo->mem.mem_type, memtype);
-		ret = -EINVAL;
-		goto out;
+	if (drm->device.info.family >= NV_DEVICE_INFO_V0_TESLA &&
+	    memtype == TTM_PL_FLAG_VRAM && contig) {
+		if (nvbo->tile_flags & NOUVEAU_GEM_TILE_NONCONTIG) {
+			if (bo->mem.mem_type == TTM_PL_VRAM) {
+				struct nouveau_mem *mem = bo->mem.mm_node;
+				if (!list_is_singular(&mem->regions))
+					evict = true;
+			}
+			nvbo->tile_flags &= ~NOUVEAU_GEM_TILE_NONCONTIG;
+			force = true;
+		}
 	}
 
-	if (nvbo->pin_refcnt++)
+	if (nvbo->pin_refcnt) {
+		if (!(memtype & (1 << bo->mem.mem_type)) || evict) {
+			NV_ERROR(drm, "bo %p pinned elsewhere: "
+				      "0x%08x vs 0x%08x\n", bo,
+				 1 << bo->mem.mem_type, memtype);
+			ret = -EBUSY;
+		}
+		nvbo->pin_refcnt++;
 		goto out;
+	}
 
+	if (evict) {
+		nouveau_bo_placement_set(nvbo, TTM_PL_FLAG_TT, 0);
+		ret = nouveau_bo_validate(nvbo, false, false);
+		if (ret)
+			goto out;
+	}
+
+	nvbo->pin_refcnt++;
 	nouveau_bo_placement_set(nvbo, memtype, 0);
 
+	/* drop pin_refcnt temporarily, so we don't trip the assertion
+	 * in nouveau_bo_move() that makes sure we're not trying to
+	 * move a pinned buffer
+	 */
+	nvbo->pin_refcnt--;
 	ret = nouveau_bo_validate(nvbo, false, false);
-	if (ret == 0) {
-		switch (bo->mem.mem_type) {
-		case TTM_PL_VRAM:
-			drm->gem.vram_available -= bo->mem.size;
-			break;
-		case TTM_PL_TT:
-			drm->gem.gart_available -= bo->mem.size;
-			break;
-		default:
-			break;
-		}
+	if (ret)
+		goto out;
+	nvbo->pin_refcnt++;
+
+	switch (bo->mem.mem_type) {
+	case TTM_PL_VRAM:
+		drm->gem.vram_available -= bo->mem.size;
+		break;
+	case TTM_PL_TT:
+		drm->gem.gart_available -= bo->mem.size;
+		break;
+	default:
+		break;
 	}
+
 out:
+	if (force && ret)
+		nvbo->tile_flags |= NOUVEAU_GEM_TILE_NONCONTIG;
 	ttm_bo_unreserve(bo);
 	return ret;
 }
@@ -392,7 +429,14 @@ nouveau_bo_map(struct nouveau_bo *nvbo)
 	if (ret)
 		return ret;
 
-	ret = ttm_bo_kmap(&nvbo->bo, 0, nvbo->bo.mem.num_pages, &nvbo->kmap);
+	/*
+	 * TTM buffers allocated using the DMA API already have a mapping, let's
+	 * use it instead.
+	 */
+	if (!nvbo->force_coherent)
+		ret = ttm_bo_kmap(&nvbo->bo, 0, nvbo->bo.mem.num_pages,
+				  &nvbo->kmap);
+
 	ttm_bo_unreserve(&nvbo->bo);
 	return ret;
 }
@@ -400,10 +444,57 @@ nouveau_bo_map(struct nouveau_bo *nvbo)
 void
 nouveau_bo_unmap(struct nouveau_bo *nvbo)
 {
-	if (nvbo)
+	if (!nvbo)
+		return;
+
+	/*
+	 * TTM buffers allocated using the DMA API already had a coherent
+	 * mapping which we used, no need to unmap.
+	 */
+	if (!nvbo->force_coherent)
 		ttm_bo_kunmap(&nvbo->kmap);
 }
 
+void
+nouveau_bo_sync_for_device(struct nouveau_bo *nvbo)
+{
+	struct nouveau_drm *drm = nouveau_bdev(nvbo->bo.bdev);
+	struct nouveau_device *device = nvkm_device(&drm->device);
+	struct ttm_dma_tt *ttm_dma = (struct ttm_dma_tt *)nvbo->bo.ttm;
+	int i;
+
+	if (!ttm_dma)
+		return;
+
+	/* Don't waste time looping if the object is coherent */
+	if (nvbo->force_coherent)
+		return;
+
+	for (i = 0; i < ttm_dma->ttm.num_pages; i++)
+		dma_sync_single_for_device(nv_device_base(device),
+			ttm_dma->dma_address[i], PAGE_SIZE, DMA_TO_DEVICE);
+}
+
+void
+nouveau_bo_sync_for_cpu(struct nouveau_bo *nvbo)
+{
+	struct nouveau_drm *drm = nouveau_bdev(nvbo->bo.bdev);
+	struct nouveau_device *device = nvkm_device(&drm->device);
+	struct ttm_dma_tt *ttm_dma = (struct ttm_dma_tt *)nvbo->bo.ttm;
+	int i;
+
+	if (!ttm_dma)
+		return;
+
+	/* Don't waste time looping if the object is coherent */
+	if (nvbo->force_coherent)
+		return;
+
+	for (i = 0; i < ttm_dma->ttm.num_pages; i++)
+		dma_sync_single_for_cpu(nv_device_base(device),
+			ttm_dma->dma_address[i], PAGE_SIZE, DMA_FROM_DEVICE);
+}
+
 int
 nouveau_bo_validate(struct nouveau_bo *nvbo, bool interruptible,
 		    bool no_wait_gpu)
@@ -415,15 +506,41 @@ nouveau_bo_validate(struct nouveau_bo *nvbo, bool interruptible,
 	if (ret)
 		return ret;
 
+	nouveau_bo_sync_for_device(nvbo);
+
 	return 0;
 }
 
+static inline void *
+_nouveau_bo_mem_index(struct nouveau_bo *nvbo, unsigned index, void *mem, u8 sz)
+{
+	struct ttm_dma_tt *dma_tt;
+	u8 *m = mem;
+
+	index *= sz;
+
+	if (m) {
+		/* kmap'd address, return the corresponding offset */
+		m += index;
+	} else {
+		/* DMA-API mapping, lookup the right address */
+		dma_tt = (struct ttm_dma_tt *)nvbo->bo.ttm;
+		m = dma_tt->cpu_address[index / PAGE_SIZE];
+		m += index % PAGE_SIZE;
+	}
+
+	return m;
+}
+#define nouveau_bo_mem_index(o, i, m) _nouveau_bo_mem_index(o, i, m, sizeof(*m))
+
 u16
 nouveau_bo_rd16(struct nouveau_bo *nvbo, unsigned index)
 {
 	bool is_iomem;
 	u16 *mem = ttm_kmap_obj_virtual(&nvbo->kmap, &is_iomem);
-	mem = &mem[index];
+
+	mem = nouveau_bo_mem_index(nvbo, index, mem);
+
 	if (is_iomem)
 		return ioread16_native((void __force __iomem *)mem);
 	else
@@ -435,7 +552,9 @@ nouveau_bo_wr16(struct nouveau_bo *nvbo, unsigned index, u16 val)
 {
 	bool is_iomem;
 	u16 *mem = ttm_kmap_obj_virtual(&nvbo->kmap, &is_iomem);
-	mem = &mem[index];
+
+	mem = nouveau_bo_mem_index(nvbo, index, mem);
+
 	if (is_iomem)
 		iowrite16_native(val, (void __force __iomem *)mem);
 	else
@@ -447,7 +566,9 @@ nouveau_bo_rd32(struct nouveau_bo *nvbo, unsigned index)
 {
 	bool is_iomem;
 	u32 *mem = ttm_kmap_obj_virtual(&nvbo->kmap, &is_iomem);
-	mem = &mem[index];
+
+	mem = nouveau_bo_mem_index(nvbo, index, mem);
+
 	if (is_iomem)
 		return ioread32_native((void __force __iomem *)mem);
 	else
@@ -459,7 +580,9 @@ nouveau_bo_wr32(struct nouveau_bo *nvbo, unsigned index, u32 val)
 {
 	bool is_iomem;
 	u32 *mem = ttm_kmap_obj_virtual(&nvbo->kmap, &is_iomem);
-	mem = &mem[index];
+
+	mem = nouveau_bo_mem_index(nvbo, index, mem);
+
 	if (is_iomem)
 		iowrite32_native(val, (void __force __iomem *)mem);
 	else
@@ -1184,6 +1307,9 @@ nouveau_bo_move(struct ttm_buffer_object *bo, bool evict, bool intr,
 	struct nouveau_drm_tile *new_tile = NULL;
 	int ret = 0;
 
+	if (nvbo->pin_refcnt)
+		NV_WARN(drm, "Moving pinned object %p!\n", nvbo);
+
 	if (drm->device.info.family < NV_DEVICE_INFO_V0_TESLA) {
 		ret = nouveau_bo_vm_bind(bo, new_mem, &new_tile);
 		if (ret)
@@ -1376,6 +1502,14 @@ nouveau_ttm_tt_populate(struct ttm_tt *ttm)
 	dev = drm->dev;
 	pdev = nv_device_base(device);
 
+	/*
+	 * Objects matching this condition have been marked as force_coherent,
+	 * so use the DMA API for them.
+	 */
+	if (!nv_device_is_cpu_coherent(device) &&
+	    ttm->caching_state == tt_uncached)
+		return ttm_dma_populate(ttm_dma, dev->dev);
+
 #if __OS_HAS_AGP
 	if (drm->agp.stat == ENABLED) {
 		return ttm_agp_tt_populate(ttm);
@@ -1433,6 +1567,14 @@ nouveau_ttm_tt_unpopulate(struct ttm_tt *ttm)
 	dev = drm->dev;
 	pdev = nv_device_base(device);
 
+	/*
+	 * Objects matching this condition have been marked as force_coherent,
+	 * so use the DMA API for them.
+	 */
+	if (!nv_device_is_cpu_coherent(device) &&
+	    ttm->caching_state == tt_uncached)
+		ttm_dma_unpopulate(ttm_dma, dev->dev);
+
 #if __OS_HAS_AGP
 	if (drm->agp.stat == ENABLED) {
 		ttm_agp_tt_unpopulate(ttm);
diff --git a/drivers/gpu/drm/nouveau/nouveau_bo.h b/drivers/gpu/drm/nouveau/nouveau_bo.h
index 22d2c764d80b..072222efeeb7 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bo.h
+++ b/drivers/gpu/drm/nouveau/nouveau_bo.h
@@ -13,6 +13,7 @@ struct nouveau_bo {
 	u32 valid_domains;
 	struct ttm_place placements[3];
 	struct ttm_place busy_placements[3];
+	bool force_coherent;
 	struct ttm_bo_kmap_obj kmap;
 	struct list_head head;
 
@@ -72,7 +73,7 @@ int  nouveau_bo_new(struct drm_device *, int size, int align, u32 flags,
 		    u32 tile_mode, u32 tile_flags, struct sg_table *sg,
 		    struct reservation_object *robj,
 		    struct nouveau_bo **);
-int  nouveau_bo_pin(struct nouveau_bo *, u32 flags);
+int  nouveau_bo_pin(struct nouveau_bo *, u32 flags, bool contig);
 int  nouveau_bo_unpin(struct nouveau_bo *);
 int  nouveau_bo_map(struct nouveau_bo *);
 void nouveau_bo_unmap(struct nouveau_bo *);
@@ -84,6 +85,8 @@ void nouveau_bo_wr32(struct nouveau_bo *, unsigned index, u32 val);
 void nouveau_bo_fence(struct nouveau_bo *, struct nouveau_fence *, bool exclusive);
 int  nouveau_bo_validate(struct nouveau_bo *, bool interruptible,
 			 bool no_wait_gpu);
+void nouveau_bo_sync_for_device(struct nouveau_bo *nvbo);
+void nouveau_bo_sync_for_cpu(struct nouveau_bo *nvbo);
 
 struct nouveau_vma *
 nouveau_bo_vma_find(struct nouveau_bo *, struct nouveau_vm *);
diff --git a/drivers/gpu/drm/nouveau/nouveau_chan.c b/drivers/gpu/drm/nouveau/nouveau_chan.c
index fd3dbd59d73e..aff9099aae6c 100644
--- a/drivers/gpu/drm/nouveau/nouveau_chan.c
+++ b/drivers/gpu/drm/nouveau/nouveau_chan.c
@@ -102,14 +102,14 @@ nouveau_channel_prep(struct nouveau_drm *drm, struct nvif_device *device,
 	chan->drm = drm;
 
 	/* allocate memory for dma push buffer */
-	target = TTM_PL_FLAG_TT;
+	target = TTM_PL_FLAG_TT | TTM_PL_FLAG_UNCACHED;
 	if (nouveau_vram_pushbuf)
 		target = TTM_PL_FLAG_VRAM;
 
 	ret = nouveau_bo_new(drm->dev, size, 0, target, 0, 0, NULL, NULL,
 			    &chan->push.buffer);
 	if (ret == 0) {
-		ret = nouveau_bo_pin(chan->push.buffer, target);
+		ret = nouveau_bo_pin(chan->push.buffer, target, false);
 		if (ret == 0)
 			ret = nouveau_bo_map(chan->push.buffer);
 	}
@@ -285,7 +285,6 @@ nouveau_channel_init(struct nouveau_channel *chan, u32 vram, u32 gart)
 	struct nouveau_software_chan *swch;
 	struct nv_dma_v0 args = {};
 	int ret, i;
-	bool save;
 
 	nvif_object_map(chan->object);
 
@@ -387,11 +386,7 @@ nouveau_channel_init(struct nouveau_channel *chan, u32 vram, u32 gart)
 	}
 
 	/* initialise synchronisation */
-	save = cli->base.super;
-	cli->base.super = true; /* hack until fencenv50 fixed */
-	ret = nouveau_fence(chan->drm)->context_new(chan);
-	cli->base.super = save;
-	return ret;
+	return nouveau_fence(chan->drm)->context_new(chan);
 }
 
 int
diff --git a/drivers/gpu/drm/nouveau/nouveau_display.c b/drivers/gpu/drm/nouveau/nouveau_display.c
index a88e6927f571..f8042433752b 100644
--- a/drivers/gpu/drm/nouveau/nouveau_display.c
+++ b/drivers/gpu/drm/nouveau/nouveau_display.c
@@ -479,6 +479,7 @@ nouveau_display_create(struct drm_device *dev)
 
 	if (nouveau_modeset != 2 && drm->vbios.dcb.entries) {
 		static const u16 oclass[] = {
+			GM204_DISP,
 			GM107_DISP,
 			GK110_DISP,
 			GK104_DISP,
@@ -568,9 +569,10 @@ nouveau_display_suspend(struct drm_device *dev, bool runtime)
 
 	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
 		struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
-
-		nouveau_bo_unmap(nv_crtc->cursor.nvbo);
-		nouveau_bo_unpin(nv_crtc->cursor.nvbo);
+		if (nv_crtc->cursor.nvbo) {
+			nouveau_bo_unmap(nv_crtc->cursor.nvbo);
+			nouveau_bo_unpin(nv_crtc->cursor.nvbo);
+		}
 	}
 
 	return 0;
@@ -591,15 +593,17 @@ nouveau_display_resume(struct drm_device *dev, bool runtime)
 		if (!nouveau_fb || !nouveau_fb->nvbo)
 			continue;
 
-		ret = nouveau_bo_pin(nouveau_fb->nvbo, TTM_PL_FLAG_VRAM);
+		ret = nouveau_bo_pin(nouveau_fb->nvbo, TTM_PL_FLAG_VRAM, true);
 		if (ret)
 			NV_ERROR(drm, "Could not pin framebuffer\n");
 	}
 
 	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
 		struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
+		if (!nv_crtc->cursor.nvbo)
+			continue;
 
-		ret = nouveau_bo_pin(nv_crtc->cursor.nvbo, TTM_PL_FLAG_VRAM);
+		ret = nouveau_bo_pin(nv_crtc->cursor.nvbo, TTM_PL_FLAG_VRAM, true);
 		if (!ret)
 			ret = nouveau_bo_map(nv_crtc->cursor.nvbo);
 		if (ret)
@@ -630,9 +634,10 @@ nouveau_display_resume(struct drm_device *dev, bool runtime)
 
 	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
 		struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
-		u32 offset = nv_crtc->cursor.nvbo->bo.offset;
 
-		nv_crtc->cursor.set_offset(nv_crtc, offset);
+		if (!nv_crtc->cursor.nvbo)
+			continue;
+		nv_crtc->cursor.set_offset(nv_crtc, nv_crtc->cursor.nvbo->bo.offset);
 		nv_crtc->cursor.set_pos(nv_crtc, nv_crtc->cursor_saved_x,
 						 nv_crtc->cursor_saved_y);
 	}
@@ -710,7 +715,7 @@ nouveau_crtc_page_flip(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 		return -ENOMEM;
 
 	if (new_bo != old_bo) {
-		ret = nouveau_bo_pin(new_bo, TTM_PL_FLAG_VRAM);
+		ret = nouveau_bo_pin(new_bo, TTM_PL_FLAG_VRAM, true);
 		if (ret)
 			goto fail_free;
 	}
diff --git a/drivers/gpu/drm/nouveau/nouveau_drm.c b/drivers/gpu/drm/nouveau/nouveau_drm.c
index 62b97c4eef8d..65910e3aed0c 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drm.c
+++ b/drivers/gpu/drm/nouveau/nouveau_drm.c
@@ -613,26 +613,6 @@ fail_display:
 	return ret;
 }
 
-int nouveau_pmops_suspend(struct device *dev)
-{
-	struct pci_dev *pdev = to_pci_dev(dev);
-	struct drm_device *drm_dev = pci_get_drvdata(pdev);
-	int ret;
-
-	if (drm_dev->switch_power_state == DRM_SWITCH_POWER_OFF ||
-	    drm_dev->switch_power_state == DRM_SWITCH_POWER_DYNAMIC_OFF)
-		return 0;
-
-	ret = nouveau_do_suspend(drm_dev, false);
-	if (ret)
-		return ret;
-
-	pci_save_state(pdev);
-	pci_disable_device(pdev);
-	pci_set_power_state(pdev, PCI_D3hot);
-	return 0;
-}
-
 static int
 nouveau_do_resume(struct drm_device *dev, bool runtime)
 {
@@ -667,7 +647,29 @@ nouveau_do_resume(struct drm_device *dev, bool runtime)
 	return 0;
 }
 
-int nouveau_pmops_resume(struct device *dev)
+int
+nouveau_pmops_suspend(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct drm_device *drm_dev = pci_get_drvdata(pdev);
+	int ret;
+
+	if (drm_dev->switch_power_state == DRM_SWITCH_POWER_OFF ||
+	    drm_dev->switch_power_state == DRM_SWITCH_POWER_DYNAMIC_OFF)
+		return 0;
+
+	ret = nouveau_do_suspend(drm_dev, false);
+	if (ret)
+		return ret;
+
+	pci_save_state(pdev);
+	pci_disable_device(pdev);
+	pci_set_power_state(pdev, PCI_D3hot);
+	return 0;
+}
+
+int
+nouveau_pmops_resume(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
 	struct drm_device *drm_dev = pci_get_drvdata(pdev);
@@ -687,20 +689,122 @@ int nouveau_pmops_resume(struct device *dev)
 	return nouveau_do_resume(drm_dev, false);
 }
 
-static int nouveau_pmops_freeze(struct device *dev)
+static int
+nouveau_pmops_freeze(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
 	struct drm_device *drm_dev = pci_get_drvdata(pdev);
 	return nouveau_do_suspend(drm_dev, false);
 }
 
-static int nouveau_pmops_thaw(struct device *dev)
+static int
+nouveau_pmops_thaw(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
 	struct drm_device *drm_dev = pci_get_drvdata(pdev);
 	return nouveau_do_resume(drm_dev, false);
 }
 
+static int
+nouveau_pmops_runtime_suspend(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct drm_device *drm_dev = pci_get_drvdata(pdev);
+	int ret;
+
+	if (nouveau_runtime_pm == 0) {
+		pm_runtime_forbid(dev);
+		return -EBUSY;
+	}
+
+	/* are we optimus enabled? */
+	if (nouveau_runtime_pm == -1 && !nouveau_is_optimus() && !nouveau_is_v1_dsm()) {
+		DRM_DEBUG_DRIVER("failing to power off - not optimus\n");
+		pm_runtime_forbid(dev);
+		return -EBUSY;
+	}
+
+	nv_debug_level(SILENT);
+	drm_kms_helper_poll_disable(drm_dev);
+	vga_switcheroo_set_dynamic_switch(pdev, VGA_SWITCHEROO_OFF);
+	nouveau_switcheroo_optimus_dsm();
+	ret = nouveau_do_suspend(drm_dev, true);
+	pci_save_state(pdev);
+	pci_disable_device(pdev);
+	pci_ignore_hotplug(pdev);
+	pci_set_power_state(pdev, PCI_D3cold);
+	drm_dev->switch_power_state = DRM_SWITCH_POWER_DYNAMIC_OFF;
+	return ret;
+}
+
+static int
+nouveau_pmops_runtime_resume(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct drm_device *drm_dev = pci_get_drvdata(pdev);
+	struct nvif_device *device = &nouveau_drm(drm_dev)->device;
+	int ret;
+
+	if (nouveau_runtime_pm == 0)
+		return -EINVAL;
+
+	pci_set_power_state(pdev, PCI_D0);
+	pci_restore_state(pdev);
+	ret = pci_enable_device(pdev);
+	if (ret)
+		return ret;
+	pci_set_master(pdev);
+
+	ret = nouveau_do_resume(drm_dev, true);
+	drm_kms_helper_poll_enable(drm_dev);
+	/* do magic */
+	nvif_mask(device, 0x88488, (1 << 25), (1 << 25));
+	vga_switcheroo_set_dynamic_switch(pdev, VGA_SWITCHEROO_ON);
+	drm_dev->switch_power_state = DRM_SWITCH_POWER_ON;
+	nv_debug_level(NORMAL);
+	return ret;
+}
+
+static int
+nouveau_pmops_runtime_idle(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct drm_device *drm_dev = pci_get_drvdata(pdev);
+	struct nouveau_drm *drm = nouveau_drm(drm_dev);
+	struct drm_crtc *crtc;
+
+	if (nouveau_runtime_pm == 0) {
+		pm_runtime_forbid(dev);
+		return -EBUSY;
+	}
+
+	/* are we optimus enabled? */
+	if (nouveau_runtime_pm == -1 && !nouveau_is_optimus() && !nouveau_is_v1_dsm()) {
+		DRM_DEBUG_DRIVER("failing to power off - not optimus\n");
+		pm_runtime_forbid(dev);
+		return -EBUSY;
+	}
+
+	/* if we have a hdmi audio device - make sure it has a driver loaded */
+	if (drm->hdmi_device) {
+		if (!drm->hdmi_device->driver) {
+			DRM_DEBUG_DRIVER("failing to power off - no HDMI audio driver loaded\n");
+			pm_runtime_mark_last_busy(dev);
+			return -EBUSY;
+		}
+	}
+
+	list_for_each_entry(crtc, &drm->dev->mode_config.crtc_list, head) {
+		if (crtc->enabled) {
+			DRM_DEBUG_DRIVER("failing to power off - crtc active\n");
+			return -EBUSY;
+		}
+	}
+	pm_runtime_mark_last_busy(dev);
+	pm_runtime_autosuspend(dev);
+	/* we don't want the main rpm_idle to call suspend - we want to autosuspend */
+	return 1;
+}
 
 static int
 nouveau_drm_open(struct drm_device *dev, struct drm_file *fpriv)
@@ -907,104 +1011,6 @@ nouveau_drm_pci_table[] = {
 	{}
 };
 
-static int nouveau_pmops_runtime_suspend(struct device *dev)
-{
-	struct pci_dev *pdev = to_pci_dev(dev);
-	struct drm_device *drm_dev = pci_get_drvdata(pdev);
-	int ret;
-
-	if (nouveau_runtime_pm == 0) {
-		pm_runtime_forbid(dev);
-		return -EBUSY;
-	}
-
-	/* are we optimus enabled? */
-	if (nouveau_runtime_pm == -1 && !nouveau_is_optimus() && !nouveau_is_v1_dsm()) {
-		DRM_DEBUG_DRIVER("failing to power off - not optimus\n");
-		pm_runtime_forbid(dev);
-		return -EBUSY;
-	}
-
-	nv_debug_level(SILENT);
-	drm_kms_helper_poll_disable(drm_dev);
-	vga_switcheroo_set_dynamic_switch(pdev, VGA_SWITCHEROO_OFF);
-	nouveau_switcheroo_optimus_dsm();
-	ret = nouveau_do_suspend(drm_dev, true);
-	pci_save_state(pdev);
-	pci_disable_device(pdev);
-	pci_ignore_hotplug(pdev);
-	pci_set_power_state(pdev, PCI_D3cold);
-	drm_dev->switch_power_state = DRM_SWITCH_POWER_DYNAMIC_OFF;
-	return ret;
-}
-
-static int nouveau_pmops_runtime_resume(struct device *dev)
-{
-	struct pci_dev *pdev = to_pci_dev(dev);
-	struct drm_device *drm_dev = pci_get_drvdata(pdev);
-	struct nvif_device *device = &nouveau_drm(drm_dev)->device;
-	int ret;
-
-	if (nouveau_runtime_pm == 0)
-		return -EINVAL;
-
-	pci_set_power_state(pdev, PCI_D0);
-	pci_restore_state(pdev);
-	ret = pci_enable_device(pdev);
-	if (ret)
-		return ret;
-	pci_set_master(pdev);
-
-	ret = nouveau_do_resume(drm_dev, true);
-	drm_kms_helper_poll_enable(drm_dev);
-	/* do magic */
-	nvif_mask(device, 0x88488, (1 << 25), (1 << 25));
-	vga_switcheroo_set_dynamic_switch(pdev, VGA_SWITCHEROO_ON);
-	drm_dev->switch_power_state = DRM_SWITCH_POWER_ON;
-	nv_debug_level(NORMAL);
-	return ret;
-}
-
-static int nouveau_pmops_runtime_idle(struct device *dev)
-{
-	struct pci_dev *pdev = to_pci_dev(dev);
-	struct drm_device *drm_dev = pci_get_drvdata(pdev);
-	struct nouveau_drm *drm = nouveau_drm(drm_dev);
-	struct drm_crtc *crtc;
-
-	if (nouveau_runtime_pm == 0) {
-		pm_runtime_forbid(dev);
-		return -EBUSY;
-	}
-
-	/* are we optimus enabled? */
-	if (nouveau_runtime_pm == -1 && !nouveau_is_optimus() && !nouveau_is_v1_dsm()) {
-		DRM_DEBUG_DRIVER("failing to power off - not optimus\n");
-		pm_runtime_forbid(dev);
-		return -EBUSY;
-	}
-
-	/* if we have a hdmi audio device - make sure it has a driver loaded */
-	if (drm->hdmi_device) {
-		if (!drm->hdmi_device->driver) {
-			DRM_DEBUG_DRIVER("failing to power off - no HDMI audio driver loaded\n");
-			pm_runtime_mark_last_busy(dev);
-			return -EBUSY;
-		}
-	}
-
-	list_for_each_entry(crtc, &drm->dev->mode_config.crtc_list, head) {
-		if (crtc->enabled) {
-			DRM_DEBUG_DRIVER("failing to power off - crtc active\n");
-			return -EBUSY;
-		}
-	}
-	pm_runtime_mark_last_busy(dev);
-	pm_runtime_autosuspend(dev);
-	/* we don't want the main rpm_idle to call suspend - we want to autosuspend */
-	return 1;
-}
-
 static void nouveau_display_options(void)
 {
 	DRM_DEBUG_DRIVER("Loading Nouveau with parameters:\n");
diff --git a/drivers/gpu/drm/nouveau/nouveau_fbcon.c b/drivers/gpu/drm/nouveau/nouveau_fbcon.c
index 593ef8a2a069..3ed12a8cfc91 100644
--- a/drivers/gpu/drm/nouveau/nouveau_fbcon.c
+++ b/drivers/gpu/drm/nouveau/nouveau_fbcon.c
@@ -341,7 +341,7 @@ nouveau_fbcon_create(struct drm_fb_helper *helper,
 		goto out;
 	}
 
-	ret = nouveau_bo_pin(nvbo, TTM_PL_FLAG_VRAM);
+	ret = nouveau_bo_pin(nvbo, TTM_PL_FLAG_VRAM, false);
 	if (ret) {
 		NV_ERROR(drm, "failed to pin fb: %d\n", ret);
 		goto out_unref;
@@ -498,6 +498,23 @@ nouveau_fbcon_set_suspend_work(struct work_struct *work)
 	console_unlock();
 }
 
+void
+nouveau_fbcon_set_suspend(struct drm_device *dev, int state)
+{
+	struct nouveau_drm *drm = nouveau_drm(dev);
+	if (drm->fbcon) {
+		if (state == FBINFO_STATE_RUNNING) {
+			schedule_work(&drm->fbcon->work);
+			return;
+		}
+		flush_work(&drm->fbcon->work);
+		console_lock();
+		fb_set_suspend(drm->fbcon->helper.fbdev, state);
+		nouveau_fbcon_accel_save_disable(dev);
+		console_unlock();
+	}
+}
+
 int
 nouveau_fbcon_init(struct drm_device *dev)
 {
@@ -557,20 +574,3 @@ nouveau_fbcon_fini(struct drm_device *dev)
 	kfree(drm->fbcon);
 	drm->fbcon = NULL;
 }
-
-void
-nouveau_fbcon_set_suspend(struct drm_device *dev, int state)
-{
-	struct nouveau_drm *drm = nouveau_drm(dev);
-	if (drm->fbcon) {
-		if (state == FBINFO_STATE_RUNNING) {
-			schedule_work(&drm->fbcon->work);
-			return;
-		}
-		flush_work(&drm->fbcon->work);
-		console_lock();
-		fb_set_suspend(drm->fbcon->helper.fbdev, state);
-		nouveau_fbcon_accel_save_disable(dev);
-		console_unlock();
-	}
-}
diff --git a/drivers/gpu/drm/nouveau/nouveau_gem.c b/drivers/gpu/drm/nouveau/nouveau_gem.c
index 36951ee4b157..42c34babc2e5 100644
--- a/drivers/gpu/drm/nouveau/nouveau_gem.c
+++ b/drivers/gpu/drm/nouveau/nouveau_gem.c
@@ -867,6 +867,7 @@ nouveau_gem_ioctl_cpu_prep(struct drm_device *dev, void *data,
 		else
 			ret = lret;
 	}
+	nouveau_bo_sync_for_cpu(nvbo);
 	drm_gem_object_unreference_unlocked(gem);
 
 	return ret;
@@ -876,6 +877,17 @@ int
 nouveau_gem_ioctl_cpu_fini(struct drm_device *dev, void *data,
 			   struct drm_file *file_priv)
 {
+	struct drm_nouveau_gem_cpu_fini *req = data;
+	struct drm_gem_object *gem;
+	struct nouveau_bo *nvbo;
+
+	gem = drm_gem_object_lookup(dev, file_priv, req->handle);
+	if (!gem)
+		return -ENOENT;
+	nvbo = nouveau_gem_object(gem);
+
+	nouveau_bo_sync_for_device(nvbo);
+	drm_gem_object_unreference_unlocked(gem);
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_platform.c b/drivers/gpu/drm/nouveau/nouveau_platform.c
index 246a824c16ca..b307bbedd4c4 100644
--- a/drivers/gpu/drm/nouveau/nouveau_platform.c
+++ b/drivers/gpu/drm/nouveau/nouveau_platform.c
@@ -27,6 +27,7 @@
 #include <linux/of.h>
 #include <linux/reset.h>
 #include <linux/regulator/consumer.h>
+#include <soc/tegra/fuse.h>
 #include <soc/tegra/pmc.h>
 
 #include "nouveau_drm.h"
@@ -128,6 +129,7 @@ static int nouveau_platform_probe(struct platform_device *pdev)
 	}
 
 	device->gpu = gpu;
+	device->gpu_speedo = tegra_sku_info.gpu_speedo_value;
 
 	err = drm_dev_register(drm, 0);
 	if (err < 0)
diff --git a/drivers/gpu/drm/nouveau/nouveau_platform.h b/drivers/gpu/drm/nouveau/nouveau_platform.h
index 91f66504900e..58c28b5653d5 100644
--- a/drivers/gpu/drm/nouveau/nouveau_platform.h
+++ b/drivers/gpu/drm/nouveau/nouveau_platform.h
@@ -41,6 +41,8 @@ struct nouveau_platform_device {
 	struct nouveau_device device;
 
 	struct nouveau_platform_gpu *gpu;
+
+	int gpu_speedo;
 };
 
 #define nv_device_to_platform(d)                                               \
diff --git a/drivers/gpu/drm/nouveau/nouveau_prime.c b/drivers/gpu/drm/nouveau/nouveau_prime.c
index 228226ab27fc..dd32ad6db53d 100644
--- a/drivers/gpu/drm/nouveau/nouveau_prime.c
+++ b/drivers/gpu/drm/nouveau/nouveau_prime.c
@@ -93,7 +93,7 @@ int nouveau_gem_prime_pin(struct drm_gem_object *obj)
 	int ret;
 
 	/* pin buffer into GTT */
-	ret = nouveau_bo_pin(nvbo, TTM_PL_FLAG_TT);
+	ret = nouveau_bo_pin(nvbo, TTM_PL_FLAG_TT, false);
 	if (ret)
 		return -EINVAL;
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_ttm.c b/drivers/gpu/drm/nouveau/nouveau_ttm.c
index 753a6def61e7..3d1cfcb96b6b 100644
--- a/drivers/gpu/drm/nouveau/nouveau_ttm.c
+++ b/drivers/gpu/drm/nouveau/nouveau_ttm.c
@@ -28,6 +28,7 @@
 #include "nouveau_ttm.h"
 #include "nouveau_gem.h"
 
+#include "drm_legacy.h"
 static int
 nouveau_vram_manager_init(struct ttm_mem_type_manager *man, unsigned long psize)
 {
@@ -281,7 +282,7 @@ nouveau_ttm_mmap(struct file *filp, struct vm_area_struct *vma)
 	struct nouveau_drm *drm = nouveau_drm(file_priv->minor->dev);
 
 	if (unlikely(vma->vm_pgoff < DRM_FILE_PAGE_OFFSET))
-		return -EINVAL;
+		return drm_legacy_mmap(filp, vma);
 
 	return ttm_bo_mmap(filp, vma, &drm->ttm.bdev);
 }
diff --git a/drivers/gpu/drm/nouveau/nv17_fence.c b/drivers/gpu/drm/nouveau/nv17_fence.c
index 40b461c7d5c5..57860cfa1de5 100644
--- a/drivers/gpu/drm/nouveau/nv17_fence.c
+++ b/drivers/gpu/drm/nouveau/nv17_fence.c
@@ -131,7 +131,7 @@ nv17_fence_create(struct nouveau_drm *drm)
 	ret = nouveau_bo_new(drm->dev, 4096, 0x1000, TTM_PL_FLAG_VRAM,
 			     0, 0x0000, NULL, NULL, &priv->bo);
 	if (!ret) {
-		ret = nouveau_bo_pin(priv->bo, TTM_PL_FLAG_VRAM);
+		ret = nouveau_bo_pin(priv->bo, TTM_PL_FLAG_VRAM, false);
 		if (!ret) {
 			ret = nouveau_bo_map(priv->bo);
 			if (ret)
diff --git a/drivers/gpu/drm/nouveau/nv50_display.c b/drivers/gpu/drm/nouveau/nv50_display.c
index eb8b36714fa1..490b90866baf 100644
--- a/drivers/gpu/drm/nouveau/nv50_display.c
+++ b/drivers/gpu/drm/nouveau/nv50_display.c
@@ -26,6 +26,7 @@
 
 #include <drm/drmP.h>
 #include <drm/drm_crtc_helper.h>
+#include <drm/drm_plane_helper.h>
 #include <drm/drm_dp_helper.h>
 
 #include <nvif/class.h>
@@ -65,15 +66,29 @@ static int
 nv50_chan_create(struct nvif_object *disp, const u32 *oclass, u8 head,
 		 void *data, u32 size, struct nv50_chan *chan)
 {
+	const u32 handle = (oclass[0] << 16) | head;
+	u32 sclass[8];
+	int ret, i;
+
+	ret = nvif_object_sclass(disp, sclass, ARRAY_SIZE(sclass));
+	WARN_ON(ret > ARRAY_SIZE(sclass));
+	if (ret < 0)
+		return ret;
+
 	while (oclass[0]) {
-		int ret = nvif_object_init(disp, NULL, (oclass[0] << 16) | head,
-					   oclass[0], data, size,
-					  &chan->user);
-		if (oclass++, ret == 0) {
-			nvif_object_map(&chan->user);
-			return ret;
+		for (i = 0; i < ARRAY_SIZE(sclass); i++) {
+			if (sclass[i] == oclass[0]) {
+				ret = nvif_object_init(disp, NULL, handle,
+						       oclass[0], data, size,
+						       &chan->user);
+				if (ret == 0)
+					nvif_object_map(&chan->user);
+				return ret;
+			}
 		}
+		oclass++;
 	}
+
 	return -ENOSYS;
 }
 
@@ -110,6 +125,7 @@ nv50_pioc_create(struct nvif_object *disp, const u32 *oclass, u8 head,
 
 struct nv50_curs {
 	struct nv50_pioc base;
+	struct nouveau_bo *image;
 };
 
 static int
@@ -265,6 +281,7 @@ nv50_core_create(struct nvif_object *disp, u64 syncbuf, struct nv50_mast *core)
 		.pushbuf = 0xb0007d00,
 	};
 	static const u32 oclass[] = {
+		GM204_DISP_CORE_CHANNEL_DMA,
 		GM107_DISP_CORE_CHANNEL_DMA,
 		GK110_DISP_CORE_CHANNEL_DMA,
 		GK104_DISP_CORE_CHANNEL_DMA,
@@ -424,8 +441,21 @@ evo_kick(u32 *push, void *evoc)
 	mutex_unlock(&dmac->lock);
 }
 
+#if 1
 #define evo_mthd(p,m,s) *((p)++) = (((s) << 18) | (m))
 #define evo_data(p,d)   *((p)++) = (d)
+#else
+#define evo_mthd(p,m,s) do {                                                   \
+	const u32 _m = (m), _s = (s);                                          \
+	printk(KERN_ERR "%04x %d %s\n", _m, _s, __func__);                     \
+	*((p)++) = ((_s << 18) | _m);                                          \
+} while(0)
+#define evo_data(p,d) do {                                                     \
+	const u32 _d = (d);                                                    \
+	printk(KERN_ERR "\t%08x\n", _d);                                       \
+	*((p)++) = _d;                                                         \
+} while(0)
+#endif
 
 static bool
 evo_sync_wait(void *data)
@@ -887,23 +917,24 @@ static void
 nv50_crtc_cursor_show(struct nouveau_crtc *nv_crtc)
 {
 	struct nv50_mast *mast = nv50_mast(nv_crtc->base.dev);
+	struct nv50_curs *curs = nv50_curs(&nv_crtc->base);
 	u32 *push = evo_wait(mast, 16);
 	if (push) {
 		if (nv50_vers(mast) < G82_DISP_CORE_CHANNEL_DMA) {
 			evo_mthd(push, 0x0880 + (nv_crtc->index * 0x400), 2);
 			evo_data(push, 0x85000000);
-			evo_data(push, nv_crtc->cursor.nvbo->bo.offset >> 8);
+			evo_data(push, curs->image->bo.offset >> 8);
 		} else
 		if (nv50_vers(mast) < GF110_DISP_CORE_CHANNEL_DMA) {
 			evo_mthd(push, 0x0880 + (nv_crtc->index * 0x400), 2);
 			evo_data(push, 0x85000000);
-			evo_data(push, nv_crtc->cursor.nvbo->bo.offset >> 8);
+			evo_data(push, curs->image->bo.offset >> 8);
 			evo_mthd(push, 0x089c + (nv_crtc->index * 0x400), 1);
 			evo_data(push, mast->base.vram.handle);
 		} else {
 			evo_mthd(push, 0x0480 + (nv_crtc->index * 0x300), 2);
 			evo_data(push, 0x85000000);
-			evo_data(push, nv_crtc->cursor.nvbo->bo.offset >> 8);
+			evo_data(push, curs->image->bo.offset >> 8);
 			evo_mthd(push, 0x048c + (nv_crtc->index * 0x300), 1);
 			evo_data(push, mast->base.vram.handle);
 		}
@@ -940,8 +971,9 @@ static void
 nv50_crtc_cursor_show_hide(struct nouveau_crtc *nv_crtc, bool show, bool update)
 {
 	struct nv50_mast *mast = nv50_mast(nv_crtc->base.dev);
+	struct nv50_curs *curs = nv50_curs(&nv_crtc->base);
 
-	if (show)
+	if (show && curs->image)
 		nv50_crtc_cursor_show(nv_crtc);
 	else
 		nv50_crtc_cursor_hide(nv_crtc);
@@ -1041,7 +1073,7 @@ nv50_crtc_commit(struct drm_crtc *crtc)
 		evo_kick(push, mast);
 	}
 
-	nv50_crtc_cursor_show_hide(nv_crtc, nv_crtc->cursor.visible, true);
+	nv50_crtc_cursor_show_hide(nv_crtc, true, true);
 	nv50_display_flip_next(crtc, crtc->primary->fb, NULL, 1);
 }
 
@@ -1060,7 +1092,7 @@ nv50_crtc_swap_fbs(struct drm_crtc *crtc, struct drm_framebuffer *old_fb)
 	struct nv50_head *head = nv50_head(crtc);
 	int ret;
 
-	ret = nouveau_bo_pin(nvfb->nvbo, TTM_PL_FLAG_VRAM);
+	ret = nouveau_bo_pin(nvfb->nvbo, TTM_PL_FLAG_VRAM, true);
 	if (ret == 0) {
 		if (head->image)
 			nouveau_bo_unpin(head->image);
@@ -1241,13 +1273,13 @@ nv50_crtc_cursor_set(struct drm_crtc *crtc, struct drm_file *file_priv,
 		     uint32_t handle, uint32_t width, uint32_t height)
 {
 	struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
+	struct nv50_curs *curs = nv50_curs(crtc);
 	struct drm_device *dev = crtc->dev;
-	struct drm_gem_object *gem;
-	struct nouveau_bo *nvbo;
-	bool visible = (handle != 0);
-	int i, ret = 0;
+	struct drm_gem_object *gem = NULL;
+	struct nouveau_bo *nvbo = NULL;
+	int ret = 0;
 
-	if (visible) {
+	if (handle) {
 		if (width != 64 || height != 64)
 			return -EINVAL;
 
@@ -1256,23 +1288,17 @@ nv50_crtc_cursor_set(struct drm_crtc *crtc, struct drm_file *file_priv,
 			return -ENOENT;
 		nvbo = nouveau_gem_object(gem);
 
-		ret = nouveau_bo_map(nvbo);
-		if (ret == 0) {
-			for (i = 0; i < 64 * 64; i++) {
-				u32 v = nouveau_bo_rd32(nvbo, i);
-				nouveau_bo_wr32(nv_crtc->cursor.nvbo, i, v);
-			}
-			nouveau_bo_unmap(nvbo);
-		}
-
-		drm_gem_object_unreference_unlocked(gem);
+		ret = nouveau_bo_pin(nvbo, TTM_PL_FLAG_VRAM, true);
 	}
 
-	if (visible != nv_crtc->cursor.visible) {
-		nv50_crtc_cursor_show_hide(nv_crtc, visible, true);
-		nv_crtc->cursor.visible = visible;
+	if (ret == 0) {
+		if (curs->image)
+			nouveau_bo_unpin(curs->image);
+		nouveau_bo_ref(nvbo, &curs->image);
 	}
+	drm_gem_object_unreference_unlocked(gem);
 
+	nv50_crtc_cursor_show_hide(nv_crtc, true, true);
 	return ret;
 }
 
@@ -1327,10 +1353,10 @@ nv50_crtc_destroy(struct drm_crtc *crtc)
 		nouveau_bo_unpin(head->image);
 	nouveau_bo_ref(NULL, &head->image);
 
-	nouveau_bo_unmap(nv_crtc->cursor.nvbo);
-	if (nv_crtc->cursor.nvbo)
-		nouveau_bo_unpin(nv_crtc->cursor.nvbo);
-	nouveau_bo_ref(NULL, &nv_crtc->cursor.nvbo);
+	/*XXX: ditto */
+	if (head->curs.image)
+		nouveau_bo_unpin(head->curs.image);
+	nouveau_bo_ref(NULL, &head->curs.image);
 
 	nouveau_bo_unmap(nv_crtc->lut.nvbo);
 	if (nv_crtc->lut.nvbo)
@@ -1362,16 +1388,6 @@ static const struct drm_crtc_funcs nv50_crtc_func = {
 	.page_flip = nouveau_crtc_page_flip,
 };
 
-static void
-nv50_cursor_set_pos(struct nouveau_crtc *nv_crtc, int x, int y)
-{
-}
-
-static void
-nv50_cursor_set_offset(struct nouveau_crtc *nv_crtc, uint32_t offset)
-{
-}
-
 static int
 nv50_crtc_create(struct drm_device *dev, int index)
 {
@@ -1390,8 +1406,6 @@ nv50_crtc_create(struct drm_device *dev, int index)
 	head->base.set_color_vibrance = nv50_crtc_set_color_vibrance;
 	head->base.color_vibrance = 50;
 	head->base.vibrant_hue = 0;
-	head->base.cursor.set_offset = nv50_cursor_set_offset;
-	head->base.cursor.set_pos = nv50_cursor_set_pos;
 	for (i = 0; i < 256; i++) {
 		head->base.lut.r[i] = i << 8;
 		head->base.lut.g[i] = i << 8;
@@ -1406,7 +1420,7 @@ nv50_crtc_create(struct drm_device *dev, int index)
 	ret = nouveau_bo_new(dev, 8192, 0x100, TTM_PL_FLAG_VRAM,
 			     0, 0x0000, NULL, NULL, &head->base.lut.nvbo);
 	if (!ret) {
-		ret = nouveau_bo_pin(head->base.lut.nvbo, TTM_PL_FLAG_VRAM);
+		ret = nouveau_bo_pin(head->base.lut.nvbo, TTM_PL_FLAG_VRAM, true);
 		if (!ret) {
 			ret = nouveau_bo_map(head->base.lut.nvbo);
 			if (ret)
@@ -1426,22 +1440,6 @@ nv50_crtc_create(struct drm_device *dev, int index)
 	if (ret)
 		goto out;
 
-	ret = nouveau_bo_new(dev, 64 * 64 * 4, 0x100, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, NULL, NULL, &head->base.cursor.nvbo);
-	if (!ret) {
-		ret = nouveau_bo_pin(head->base.cursor.nvbo, TTM_PL_FLAG_VRAM);
-		if (!ret) {
-			ret = nouveau_bo_map(head->base.cursor.nvbo);
-			if (ret)
-				nouveau_bo_unpin(head->base.lut.nvbo);
-		}
-		if (ret)
-			nouveau_bo_ref(NULL, &head->base.cursor.nvbo);
-	}
-
-	if (ret)
-		goto out;
-
 	/* allocate page flip / sync resources */
 	ret = nv50_base_create(disp->disp, index, disp->sync->bo.offset,
 			      &head->sync);
@@ -1701,7 +1699,8 @@ nv50_audio_mode_set(struct drm_encoder *encoder, struct drm_display_mode *mode)
 	drm_edid_to_eld(&nv_connector->base, nv_connector->edid);
 	memcpy(args.data, nv_connector->base.eld, sizeof(args.data));
 
-	nvif_mthd(disp->disp, 0, &args, sizeof(args.base) + args.data[2] * 4);
+	nvif_mthd(disp->disp, 0, &args,
+		  sizeof(args.base) + drm_eld_size(args.data));
 }
 
 static void
@@ -2373,11 +2372,6 @@ nv50_fb_ctor(struct drm_framebuffer *fb)
 	u8 kind = nouveau_bo_tile_layout(nvbo) >> 8;
 	u8 tile = nvbo->tile_mode;
 
-	if (nvbo->tile_flags & NOUVEAU_GEM_TILE_NONCONTIG) {
-		NV_ERROR(drm, "framebuffer requires contiguous bo\n");
-		return -EINVAL;
-	}
-
 	if (drm->device.info.chipset >= 0xc0)
 		tile >>= 4; /* yep.. */
 
@@ -2491,7 +2485,7 @@ nv50_display_create(struct drm_device *dev)
 	ret = nouveau_bo_new(dev, 4096, 0x1000, TTM_PL_FLAG_VRAM,
 			     0, 0x0000, NULL, NULL, &disp->sync);
 	if (!ret) {
-		ret = nouveau_bo_pin(disp->sync, TTM_PL_FLAG_VRAM);
+		ret = nouveau_bo_pin(disp->sync, TTM_PL_FLAG_VRAM, true);
 		if (!ret) {
 			ret = nouveau_bo_map(disp->sync);
 			if (ret)
diff --git a/drivers/gpu/drm/nouveau/nv50_fence.c b/drivers/gpu/drm/nouveau/nv50_fence.c
index 22d242b37962..a82d9ea7c6fd 100644
--- a/drivers/gpu/drm/nouveau/nv50_fence.c
+++ b/drivers/gpu/drm/nouveau/nv50_fence.c
@@ -102,7 +102,7 @@ nv50_fence_create(struct nouveau_drm *drm)
 	ret = nouveau_bo_new(drm->dev, 4096, 0x1000, TTM_PL_FLAG_VRAM,
 			     0, 0x0000, NULL, NULL, &priv->bo);
 	if (!ret) {
-		ret = nouveau_bo_pin(priv->bo, TTM_PL_FLAG_VRAM);
+		ret = nouveau_bo_pin(priv->bo, TTM_PL_FLAG_VRAM, false);
 		if (!ret) {
 			ret = nouveau_bo_map(priv->bo);
 			if (ret)
diff --git a/drivers/gpu/drm/nouveau/nv84_fence.c b/drivers/gpu/drm/nouveau/nv84_fence.c
index d6c6c87c3f07..cb5b88938d45 100644
--- a/drivers/gpu/drm/nouveau/nv84_fence.c
+++ b/drivers/gpu/drm/nouveau/nv84_fence.c
@@ -234,7 +234,7 @@ nv84_fence_create(struct nouveau_drm *drm)
 	ret = nouveau_bo_new(drm->dev, 16 * priv->base.contexts, 0,
 			     TTM_PL_FLAG_VRAM, 0, 0, NULL, NULL, &priv->bo);
 	if (ret == 0) {
-		ret = nouveau_bo_pin(priv->bo, TTM_PL_FLAG_VRAM);
+		ret = nouveau_bo_pin(priv->bo, TTM_PL_FLAG_VRAM, false);
 		if (ret == 0) {
 			ret = nouveau_bo_map(priv->bo);
 			if (ret)
@@ -246,10 +246,10 @@ nv84_fence_create(struct nouveau_drm *drm)
 
 	if (ret == 0)
 		ret = nouveau_bo_new(drm->dev, 16 * priv->base.contexts, 0,
-				     TTM_PL_FLAG_TT, 0, 0, NULL, NULL,
-				     &priv->bo_gart);
+				     TTM_PL_FLAG_TT | TTM_PL_FLAG_UNCACHED, 0,
+				     0, NULL, NULL, &priv->bo_gart);
 	if (ret == 0) {
-		ret = nouveau_bo_pin(priv->bo_gart, TTM_PL_FLAG_TT);
+		ret = nouveau_bo_pin(priv->bo_gart, TTM_PL_FLAG_TT, false);
 		if (ret == 0) {
 			ret = nouveau_bo_map(priv->bo_gart);
 			if (ret)
diff --git a/drivers/gpu/drm/nouveau/nvif/class.h b/drivers/gpu/drm/nouveau/nvif/class.h
index e5a27df0672b..4e308eacb27a 100644
--- a/drivers/gpu/drm/nouveau/nvif/class.h
+++ b/drivers/gpu/drm/nouveau/nvif/class.h
@@ -35,6 +35,7 @@
 #define GK104_DISP                                                   0x00009170
 #define GK110_DISP                                                   0x00009270
 #define GM107_DISP                                                   0x00009470
+#define GM204_DISP                                                   0x00009570
 
 #define NV50_DISP_CURSOR                                             0x0000507a
 #define G82_DISP_CURSOR                                              0x0000827a
@@ -65,6 +66,7 @@
 #define GK104_DISP_CORE_CHANNEL_DMA                                  0x0000917d
 #define GK110_DISP_CORE_CHANNEL_DMA                                  0x0000927d
 #define GM107_DISP_CORE_CHANNEL_DMA                                  0x0000947d
+#define GM204_DISP_CORE_CHANNEL_DMA                                  0x0000957d
 
 #define NV50_DISP_OVERLAY_CHANNEL_DMA                                0x0000507e
 #define G82_DISP_OVERLAY_CHANNEL_DMA                                 0x0000827e
@@ -131,6 +133,7 @@ struct nv_device_v0 {
 #define NV_DEVICE_V0_DISABLE_COPY1                        0x0000010000000000ULL
 #define NV_DEVICE_V0_DISABLE_VIC                          0x0000020000000000ULL
 #define NV_DEVICE_V0_DISABLE_VENC                         0x0000040000000000ULL
+#define NV_DEVICE_V0_DISABLE_COPY2                        0x0000080000000000ULL
 	__u64 disable;	/* disable particular subsystems */
 	__u64 debug0;	/* as above, but *internal* ids, and *NOT* ABI */
 };
diff --git a/drivers/gpu/drm/nouveau/nvif/client.c b/drivers/gpu/drm/nouveau/nvif/client.c
index 3c4df1fc26dc..3f7ac5bc8e03 100644
--- a/drivers/gpu/drm/nouveau/nvif/client.c
+++ b/drivers/gpu/drm/nouveau/nvif/client.c
@@ -62,6 +62,7 @@ nvif_drivers[] = {
 #else
 	&nvif_driver_drm,
 	&nvif_driver_lib,
+	&nvif_driver_null,
 #endif
 	NULL
 };
diff --git a/drivers/gpu/drm/nouveau/nvif/driver.h b/drivers/gpu/drm/nouveau/nvif/driver.h
index ac4bdb3ea506..8bd39e69229c 100644
--- a/drivers/gpu/drm/nouveau/nvif/driver.h
+++ b/drivers/gpu/drm/nouveau/nvif/driver.h
@@ -17,5 +17,6 @@ struct nvif_driver {
 extern const struct nvif_driver nvif_driver_nvkm;
 extern const struct nvif_driver nvif_driver_drm;
 extern const struct nvif_driver nvif_driver_lib;
+extern const struct nvif_driver nvif_driver_null;
 
 #endif
diff --git a/drivers/gpu/drm/omapdrm/omap_crtc.c b/drivers/gpu/drm/omapdrm/omap_crtc.c
index 2d28dc337cfb..b0566a1ca28f 100644
--- a/drivers/gpu/drm/omapdrm/omap_crtc.c
+++ b/drivers/gpu/drm/omapdrm/omap_crtc.c
@@ -20,6 +20,7 @@
 #include "omap_drv.h"
 
 #include <drm/drm_mode.h>
+#include <drm/drm_plane_helper.h>
 #include "drm_crtc.h"
 #include "drm_crtc_helper.h"
 
diff --git a/drivers/gpu/drm/omapdrm/omap_drv.c b/drivers/gpu/drm/omapdrm/omap_drv.c
index 862ba03c236c..8241ed9b353c 100644
--- a/drivers/gpu/drm/omapdrm/omap_drv.c
+++ b/drivers/gpu/drm/omapdrm/omap_drv.c
@@ -718,7 +718,6 @@ static const struct dev_pm_ops omapdrm_pm_ops = {
 static struct platform_driver pdev = {
 		.driver = {
 			.name = DRIVER_NAME,
-			.owner = THIS_MODULE,
 #ifdef CONFIG_PM
 			.pm = &omapdrm_pm_ops,
 #endif
diff --git a/drivers/gpu/drm/omapdrm/omap_gem.c b/drivers/gpu/drm/omapdrm/omap_gem.c
index e4849413ee80..aeb91ed653c9 100644
--- a/drivers/gpu/drm/omapdrm/omap_gem.c
+++ b/drivers/gpu/drm/omapdrm/omap_gem.c
@@ -612,8 +612,7 @@ int omap_gem_dumb_create(struct drm_file *file, struct drm_device *dev,
 {
 	union omap_gem_size gsize;
 
-	/* in case someone tries to feed us a completely bogus stride: */
-	args->pitch = align_pitch(args->pitch, args->width, args->bpp);
+	args->pitch = align_pitch(0, args->width, args->bpp);
 	args->size = PAGE_ALIGN(args->pitch * args->height);
 
 	gsize = (union omap_gem_size){
diff --git a/drivers/gpu/drm/omapdrm/omap_plane.c b/drivers/gpu/drm/omapdrm/omap_plane.c
index 891a4dc608af..ee8e2b3a117e 100644
--- a/drivers/gpu/drm/omapdrm/omap_plane.c
+++ b/drivers/gpu/drm/omapdrm/omap_plane.c
@@ -388,20 +388,15 @@ struct drm_plane *omap_plane_init(struct drm_device *dev,
 	struct drm_plane *plane = NULL;
 	struct omap_plane *omap_plane;
 	struct omap_overlay_info *info;
-	int ret;
 
 	DBG("%s: priv=%d", plane_names[id], private_plane);
 
 	omap_plane = kzalloc(sizeof(*omap_plane), GFP_KERNEL);
 	if (!omap_plane)
-		goto fail;
+		return NULL;
 
-	ret = drm_flip_work_init(&omap_plane->unpin_work, 16,
+	drm_flip_work_init(&omap_plane->unpin_work,
 			"unpin", unpin_worker);
-	if (ret) {
-		dev_err(dev->dev, "could not allocate unpin FIFO\n");
-		goto fail;
-	}
 
 	omap_plane->nformats = omap_framebuffer_get_formats(
 			omap_plane->formats, ARRAY_SIZE(omap_plane->formats),
@@ -443,10 +438,4 @@ struct drm_plane *omap_plane_init(struct drm_device *dev,
 		omap_plane->info.zorder = id;
 
 	return plane;
-
-fail:
-	if (plane)
-		omap_plane_destroy(plane);
-
-	return NULL;
 }
diff --git a/drivers/gpu/drm/panel/Kconfig b/drivers/gpu/drm/panel/Kconfig
index bee9f72b3a93..024e98ef8e4d 100644
--- a/drivers/gpu/drm/panel/Kconfig
+++ b/drivers/gpu/drm/panel/Kconfig
@@ -27,4 +27,17 @@ config DRM_PANEL_S6E8AA0
 	select DRM_MIPI_DSI
 	select VIDEOMODE_HELPERS
 
+config DRM_PANEL_SHARP_LQ101R1SX01
+	tristate "Sharp LQ101R1SX01 panel"
+	depends on OF
+	depends on DRM_MIPI_DSI
+	help
+	  Say Y here if you want to enable support for Sharp LQ101R1SX01
+	  TFT-LCD modules. The panel has a 2560x1600 resolution and uses
+	  24 bit RGB per pixel. It provides a dual MIPI DSI interface to
+	  the host and has a built-in LED backlight.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called panel-sharp-lq101r1sx01.
+
 endmenu
diff --git a/drivers/gpu/drm/panel/Makefile b/drivers/gpu/drm/panel/Makefile
index 8b929212fad7..4b2a0430804b 100644
--- a/drivers/gpu/drm/panel/Makefile
+++ b/drivers/gpu/drm/panel/Makefile
@@ -1,3 +1,4 @@
 obj-$(CONFIG_DRM_PANEL_SIMPLE) += panel-simple.o
 obj-$(CONFIG_DRM_PANEL_LD9040) += panel-ld9040.o
 obj-$(CONFIG_DRM_PANEL_S6E8AA0) += panel-s6e8aa0.o
+obj-$(CONFIG_DRM_PANEL_SHARP_LQ101R1SX01) += panel-sharp-lq101r1sx01.o
diff --git a/drivers/gpu/drm/panel/panel-ld9040.c b/drivers/gpu/drm/panel/panel-ld9040.c
index 42ac67b21e9f..08cf2c588c3d 100644
--- a/drivers/gpu/drm/panel/panel-ld9040.c
+++ b/drivers/gpu/drm/panel/panel-ld9040.c
@@ -145,7 +145,7 @@ static void ld9040_dcs_write(struct ld9040 *ctx, const u8 *data, size_t len)
 	if (ctx->error < 0 || len == 0)
 		return;
 
-	dev_dbg(ctx->dev, "writing dcs seq: %*ph\n", len, data);
+	dev_dbg(ctx->dev, "writing dcs seq: %*ph\n", (int)len, data);
 	ret = ld9040_spi_write_word(ctx, *data);
 
 	while (!ret && --len) {
@@ -154,8 +154,8 @@ static void ld9040_dcs_write(struct ld9040 *ctx, const u8 *data, size_t len)
 	}
 
 	if (ret) {
-		dev_err(ctx->dev, "error %d writing dcs seq: %*ph\n", ret, len,
-			data);
+		dev_err(ctx->dev, "error %d writing dcs seq: %*ph\n", ret,
+			(int)len, data);
 		ctx->error = ret;
 	}
 
@@ -336,17 +336,12 @@ static int ld9040_probe(struct spi_device *spi)
 	if (ret < 0)
 		return ret;
 
-	ctx->reset_gpio = devm_gpiod_get(dev, "reset");
+	ctx->reset_gpio = devm_gpiod_get(dev, "reset", GPIOD_OUT_HIGH);
 	if (IS_ERR(ctx->reset_gpio)) {
 		dev_err(dev, "cannot get reset-gpios %ld\n",
 			PTR_ERR(ctx->reset_gpio));
 		return PTR_ERR(ctx->reset_gpio);
 	}
-	ret = gpiod_direction_output(ctx->reset_gpio, 1);
-	if (ret < 0) {
-		dev_err(dev, "cannot configure reset-gpios %d\n", ret);
-		return ret;
-	}
 
 	spi->bits_per_word = 9;
 	ret = spi_setup(spi);
diff --git a/drivers/gpu/drm/panel/panel-s6e8aa0.c b/drivers/gpu/drm/panel/panel-s6e8aa0.c
index b5217fe37f02..144b2733e3d7 100644
--- a/drivers/gpu/drm/panel/panel-s6e8aa0.c
+++ b/drivers/gpu/drm/panel/panel-s6e8aa0.c
@@ -141,10 +141,10 @@ static void s6e8aa0_dcs_write(struct s6e8aa0 *ctx, const void *data, size_t len)
 	if (ctx->error < 0)
 		return;
 
-	ret = mipi_dsi_dcs_write(dsi, data, len);
+	ret = mipi_dsi_dcs_write_buffer(dsi, data, len);
 	if (ret < 0) {
-		dev_err(ctx->dev, "error %zd writing dcs seq: %*ph\n", ret, len,
-			data);
+		dev_err(ctx->dev, "error %zd writing dcs seq: %*ph\n", ret,
+			(int)len, data);
 		ctx->error = ret;
 	}
 }
@@ -800,27 +800,15 @@ static void s6e8aa0_panel_init(struct s6e8aa0 *ctx)
 }
 
 static void s6e8aa0_set_maximum_return_packet_size(struct s6e8aa0 *ctx,
-						   int size)
+						   u16 size)
 {
 	struct mipi_dsi_device *dsi = to_mipi_dsi_device(ctx->dev);
-	const struct mipi_dsi_host_ops *ops = dsi->host->ops;
-	u8 buf[] = {size, 0};
-	struct mipi_dsi_msg msg = {
-		.channel = dsi->channel,
-		.type = MIPI_DSI_SET_MAXIMUM_RETURN_PACKET_SIZE,
-		.tx_len = sizeof(buf),
-		.tx_buf = buf
-	};
 	int ret;
 
 	if (ctx->error < 0)
 		return;
 
-	if (!ops || !ops->transfer)
-		ret = -EIO;
-	else
-		ret = ops->transfer(dsi->host, &msg);
-
+	ret = mipi_dsi_set_maximum_return_packet_size(dsi, size);
 	if (ret < 0) {
 		dev_err(ctx->dev,
 			"error %d setting maximum return packet size to %d\n",
@@ -1019,17 +1007,12 @@ static int s6e8aa0_probe(struct mipi_dsi_device *dsi)
 		return ret;
 	}
 
-	ctx->reset_gpio = devm_gpiod_get(dev, "reset");
+	ctx->reset_gpio = devm_gpiod_get(dev, "reset", GPIOD_OUT_HIGH);
 	if (IS_ERR(ctx->reset_gpio)) {
 		dev_err(dev, "cannot get reset-gpios %ld\n",
 			PTR_ERR(ctx->reset_gpio));
 		return PTR_ERR(ctx->reset_gpio);
 	}
-	ret = gpiod_direction_output(ctx->reset_gpio, 1);
-	if (ret < 0) {
-		dev_err(dev, "cannot configure reset-gpios %d\n", ret);
-		return ret;
-	}
 
 	ctx->brightness = GAMMA_LEVEL_NUM - 1;
 
@@ -1069,7 +1052,6 @@ static struct mipi_dsi_driver s6e8aa0_driver = {
 	.remove = s6e8aa0_remove,
 	.driver = {
 		.name = "panel_s6e8aa0",
-		.owner = THIS_MODULE,
 		.of_match_table = s6e8aa0_of_match,
 	},
 };
diff --git a/drivers/gpu/drm/panel/panel-sharp-lq101r1sx01.c b/drivers/gpu/drm/panel/panel-sharp-lq101r1sx01.c
new file mode 100644
index 000000000000..9d81759d82fc
--- /dev/null
+++ b/drivers/gpu/drm/panel/panel-sharp-lq101r1sx01.c
@@ -0,0 +1,464 @@
+/*
+ * Copyright (C) 2014 NVIDIA 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.
+ */
+
+#include <linux/backlight.h>
+#include <linux/gpio/consumer.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/regulator/consumer.h>
+
+#include <drm/drmP.h>
+#include <drm/drm_crtc.h>
+#include <drm/drm_mipi_dsi.h>
+#include <drm/drm_panel.h>
+
+#include <video/mipi_display.h>
+
+#include <linux/host1x.h>
+
+struct sharp_panel {
+	struct drm_panel base;
+	/* the datasheet refers to them as DSI-LINK1 and DSI-LINK2 */
+	struct mipi_dsi_device *link1;
+	struct mipi_dsi_device *link2;
+
+	struct backlight_device *backlight;
+	struct regulator *supply;
+
+	bool prepared;
+	bool enabled;
+
+	const struct drm_display_mode *mode;
+};
+
+static inline struct sharp_panel *to_sharp_panel(struct drm_panel *panel)
+{
+	return container_of(panel, struct sharp_panel, base);
+}
+
+static int sharp_panel_write(struct sharp_panel *sharp, u16 offset, u8 value)
+{
+	u8 payload[3] = { offset >> 8, offset & 0xff, value };
+	struct mipi_dsi_device *dsi = sharp->link1;
+	ssize_t err;
+
+	err = mipi_dsi_generic_write(dsi, payload, sizeof(payload));
+	if (err < 0) {
+		dev_err(&dsi->dev, "failed to write %02x to %04x: %zd\n",
+			value, offset, err);
+		return err;
+	}
+
+	err = mipi_dsi_dcs_nop(dsi);
+	if (err < 0) {
+		dev_err(&dsi->dev, "failed to send DCS nop: %zd\n", err);
+		return err;
+	}
+
+	usleep_range(10, 20);
+
+	return 0;
+}
+
+static __maybe_unused int sharp_panel_read(struct sharp_panel *sharp,
+					   u16 offset, u8 *value)
+{
+	ssize_t err;
+
+	cpu_to_be16s(&offset);
+
+	err = mipi_dsi_generic_read(sharp->link1, &offset, sizeof(offset),
+				    value, sizeof(*value));
+	if (err < 0)
+		dev_err(&sharp->link1->dev, "failed to read from %04x: %zd\n",
+			offset, err);
+
+	return err;
+}
+
+static int sharp_panel_disable(struct drm_panel *panel)
+{
+	struct sharp_panel *sharp = to_sharp_panel(panel);
+
+	if (!sharp->enabled)
+		return 0;
+
+	if (sharp->backlight) {
+		sharp->backlight->props.power = FB_BLANK_POWERDOWN;
+		backlight_update_status(sharp->backlight);
+	}
+
+	sharp->enabled = false;
+
+	return 0;
+}
+
+static int sharp_panel_unprepare(struct drm_panel *panel)
+{
+	struct sharp_panel *sharp = to_sharp_panel(panel);
+	int err;
+
+	if (!sharp->prepared)
+		return 0;
+
+	err = mipi_dsi_dcs_set_display_off(sharp->link1);
+	if (err < 0)
+		dev_err(panel->dev, "failed to set display off: %d\n", err);
+
+	err = mipi_dsi_dcs_enter_sleep_mode(sharp->link1);
+	if (err < 0)
+		dev_err(panel->dev, "failed to enter sleep mode: %d\n", err);
+
+	msleep(120);
+
+	regulator_disable(sharp->supply);
+
+	sharp->prepared = false;
+
+	return 0;
+}
+
+static int sharp_setup_symmetrical_split(struct mipi_dsi_device *left,
+					 struct mipi_dsi_device *right,
+					 const struct drm_display_mode *mode)
+{
+	int err;
+
+	err = mipi_dsi_dcs_set_column_address(left, 0, mode->hdisplay / 2 - 1);
+	if (err < 0) {
+		dev_err(&left->dev, "failed to set column address: %d\n", err);
+		return err;
+	}
+
+	err = mipi_dsi_dcs_set_page_address(left, 0, mode->vdisplay - 1);
+	if (err < 0) {
+		dev_err(&left->dev, "failed to set page address: %d\n", err);
+		return err;
+	}
+
+	err = mipi_dsi_dcs_set_column_address(right, mode->hdisplay / 2,
+					      mode->hdisplay - 1);
+	if (err < 0) {
+		dev_err(&right->dev, "failed to set column address: %d\n", err);
+		return err;
+	}
+
+	err = mipi_dsi_dcs_set_page_address(right, 0, mode->vdisplay - 1);
+	if (err < 0) {
+		dev_err(&right->dev, "failed to set page address: %d\n", err);
+		return err;
+	}
+
+	return 0;
+}
+
+static int sharp_panel_prepare(struct drm_panel *panel)
+{
+	struct sharp_panel *sharp = to_sharp_panel(panel);
+	u8 format = MIPI_DCS_PIXEL_FMT_24BIT;
+	int err;
+
+	if (sharp->prepared)
+		return 0;
+
+	err = regulator_enable(sharp->supply);
+	if (err < 0)
+		return err;
+
+	usleep_range(10000, 20000);
+
+	err = mipi_dsi_dcs_soft_reset(sharp->link1);
+	if (err < 0) {
+		dev_err(panel->dev, "soft reset failed: %d\n", err);
+		goto poweroff;
+	}
+
+	msleep(120);
+
+	err = mipi_dsi_dcs_exit_sleep_mode(sharp->link1);
+	if (err < 0) {
+		dev_err(panel->dev, "failed to exit sleep mode: %d\n", err);
+		goto poweroff;
+	}
+
+	/*
+	 * The MIPI DCS specification mandates this delay only between the
+	 * exit_sleep_mode and enter_sleep_mode commands, so it isn't strictly
+	 * necessary here.
+	 */
+	/*
+	msleep(120);
+	*/
+
+	/* set left-right mode */
+	err = sharp_panel_write(sharp, 0x1000, 0x2a);
+	if (err < 0) {
+		dev_err(panel->dev, "failed to set left-right mode: %d\n", err);
+		goto poweroff;
+	}
+
+	/* enable command mode */
+	err = sharp_panel_write(sharp, 0x1001, 0x01);
+	if (err < 0) {
+		dev_err(panel->dev, "failed to enable command mode: %d\n", err);
+		goto poweroff;
+	}
+
+	err = mipi_dsi_dcs_set_pixel_format(sharp->link1, format);
+	if (err < 0) {
+		dev_err(panel->dev, "failed to set pixel format: %d\n", err);
+		goto poweroff;
+	}
+
+	/*
+	 * TODO: The device supports both left-right and even-odd split
+	 * configurations, but this driver currently supports only the left-
+	 * right split. To support a different mode a mechanism needs to be
+	 * put in place to communicate the configuration back to the DSI host
+	 * controller.
+	 */
+	err = sharp_setup_symmetrical_split(sharp->link1, sharp->link2,
+					    sharp->mode);
+	if (err < 0) {
+		dev_err(panel->dev, "failed to set up symmetrical split: %d\n",
+			err);
+		goto poweroff;
+	}
+
+	err = mipi_dsi_dcs_set_display_on(sharp->link1);
+	if (err < 0) {
+		dev_err(panel->dev, "failed to set display on: %d\n", err);
+		goto poweroff;
+	}
+
+	sharp->prepared = true;
+
+	return 0;
+
+poweroff:
+	regulator_disable(sharp->supply);
+	return err;
+}
+
+static int sharp_panel_enable(struct drm_panel *panel)
+{
+	struct sharp_panel *sharp = to_sharp_panel(panel);
+
+	if (sharp->enabled)
+		return 0;
+
+	if (sharp->backlight) {
+		sharp->backlight->props.power = FB_BLANK_UNBLANK;
+		backlight_update_status(sharp->backlight);
+	}
+
+	sharp->enabled = true;
+
+	return 0;
+}
+
+static const struct drm_display_mode default_mode = {
+	.clock = 278000,
+	.hdisplay = 2560,
+	.hsync_start = 2560 + 128,
+	.hsync_end = 2560 + 128 + 64,
+	.htotal = 2560 + 128 + 64 + 64,
+	.vdisplay = 1600,
+	.vsync_start = 1600 + 4,
+	.vsync_end = 1600 + 4 + 8,
+	.vtotal = 1600 + 4 + 8 + 32,
+	.vrefresh = 60,
+};
+
+static int sharp_panel_get_modes(struct drm_panel *panel)
+{
+	struct drm_display_mode *mode;
+
+	mode = drm_mode_duplicate(panel->drm, &default_mode);
+	if (!mode) {
+		dev_err(panel->drm->dev, "failed to add mode %ux%ux@%u\n",
+			default_mode.hdisplay, default_mode.vdisplay,
+			default_mode.vrefresh);
+		return -ENOMEM;
+	}
+
+	drm_mode_set_name(mode);
+
+	drm_mode_probed_add(panel->connector, mode);
+
+	panel->connector->display_info.width_mm = 217;
+	panel->connector->display_info.height_mm = 136;
+
+	return 1;
+}
+
+static const struct drm_panel_funcs sharp_panel_funcs = {
+	.disable = sharp_panel_disable,
+	.unprepare = sharp_panel_unprepare,
+	.prepare = sharp_panel_prepare,
+	.enable = sharp_panel_enable,
+	.get_modes = sharp_panel_get_modes,
+};
+
+static const struct of_device_id sharp_of_match[] = {
+	{ .compatible = "sharp,lq101r1sx01", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, sharp_of_match);
+
+static int sharp_panel_add(struct sharp_panel *sharp)
+{
+	struct device_node *np;
+	int err;
+
+	sharp->mode = &default_mode;
+
+	sharp->supply = devm_regulator_get(&sharp->link1->dev, "power");
+	if (IS_ERR(sharp->supply))
+		return PTR_ERR(sharp->supply);
+
+	np = of_parse_phandle(sharp->link1->dev.of_node, "backlight", 0);
+	if (np) {
+		sharp->backlight = of_find_backlight_by_node(np);
+		of_node_put(np);
+
+		if (!sharp->backlight)
+			return -EPROBE_DEFER;
+	}
+
+	drm_panel_init(&sharp->base);
+	sharp->base.funcs = &sharp_panel_funcs;
+	sharp->base.dev = &sharp->link1->dev;
+
+	err = drm_panel_add(&sharp->base);
+	if (err < 0)
+		goto put_backlight;
+
+	return 0;
+
+put_backlight:
+	if (sharp->backlight)
+		put_device(&sharp->backlight->dev);
+
+	return err;
+}
+
+static void sharp_panel_del(struct sharp_panel *sharp)
+{
+	if (sharp->base.dev)
+		drm_panel_remove(&sharp->base);
+
+	if (sharp->backlight)
+		put_device(&sharp->backlight->dev);
+
+	if (sharp->link2)
+		put_device(&sharp->link2->dev);
+}
+
+static int sharp_panel_probe(struct mipi_dsi_device *dsi)
+{
+	struct mipi_dsi_device *secondary = NULL;
+	struct sharp_panel *sharp;
+	struct device_node *np;
+	int err;
+
+	dsi->lanes = 4;
+	dsi->format = MIPI_DSI_FMT_RGB888;
+	dsi->mode_flags = MIPI_DSI_MODE_LPM;
+
+	/* Find DSI-LINK1 */
+	np = of_parse_phandle(dsi->dev.of_node, "link2", 0);
+	if (np) {
+		secondary = of_find_mipi_dsi_device_by_node(np);
+		of_node_put(np);
+
+		if (!secondary)
+			return -EPROBE_DEFER;
+	}
+
+	/* register a panel for only the DSI-LINK1 interface */
+	if (secondary) {
+		sharp = devm_kzalloc(&dsi->dev, sizeof(*sharp), GFP_KERNEL);
+		if (!sharp) {
+			put_device(&secondary->dev);
+			return -ENOMEM;
+		}
+
+		mipi_dsi_set_drvdata(dsi, sharp);
+
+		sharp->link2 = secondary;
+		sharp->link1 = dsi;
+
+		err = sharp_panel_add(sharp);
+		if (err < 0) {
+			put_device(&secondary->dev);
+			return err;
+		}
+	}
+
+	err = mipi_dsi_attach(dsi);
+	if (err < 0) {
+		if (secondary)
+			sharp_panel_del(sharp);
+
+		return err;
+	}
+
+	return 0;
+}
+
+static int sharp_panel_remove(struct mipi_dsi_device *dsi)
+{
+	struct sharp_panel *sharp = mipi_dsi_get_drvdata(dsi);
+	int err;
+
+	/* only detach from host for the DSI-LINK2 interface */
+	if (!sharp) {
+		mipi_dsi_detach(dsi);
+		return 0;
+	}
+
+	err = sharp_panel_disable(&sharp->base);
+	if (err < 0)
+		dev_err(&dsi->dev, "failed to disable panel: %d\n", err);
+
+	err = mipi_dsi_detach(dsi);
+	if (err < 0)
+		dev_err(&dsi->dev, "failed to detach from DSI host: %d\n", err);
+
+	drm_panel_detach(&sharp->base);
+	sharp_panel_del(sharp);
+
+	return 0;
+}
+
+static void sharp_panel_shutdown(struct mipi_dsi_device *dsi)
+{
+	struct sharp_panel *sharp = mipi_dsi_get_drvdata(dsi);
+
+	/* nothing to do for DSI-LINK2 */
+	if (!sharp)
+		return;
+
+	sharp_panel_disable(&sharp->base);
+}
+
+static struct mipi_dsi_driver sharp_panel_driver = {
+	.driver = {
+		.name = "panel-sharp-lq101r1sx01",
+		.of_match_table = sharp_of_match,
+	},
+	.probe = sharp_panel_probe,
+	.remove = sharp_panel_remove,
+	.shutdown = sharp_panel_shutdown,
+};
+module_mipi_dsi_driver(sharp_panel_driver);
+
+MODULE_AUTHOR("Thierry Reding <treding@nvidia.com>");
+MODULE_DESCRIPTION("Sharp LQ101R1SX01 panel driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/gpu/drm/panel/panel-simple.c b/drivers/gpu/drm/panel/panel-simple.c
index 23de22f8c820..e95385bf8356 100644
--- a/drivers/gpu/drm/panel/panel-simple.c
+++ b/drivers/gpu/drm/panel/panel-simple.c
@@ -247,21 +247,14 @@ static int panel_simple_probe(struct device *dev, const struct panel_desc *desc)
 	if (IS_ERR(panel->supply))
 		return PTR_ERR(panel->supply);
 
-	panel->enable_gpio = devm_gpiod_get_optional(dev, "enable");
+	panel->enable_gpio = devm_gpiod_get_optional(dev, "enable",
+						     GPIOD_OUT_LOW);
 	if (IS_ERR(panel->enable_gpio)) {
 		err = PTR_ERR(panel->enable_gpio);
 		dev_err(dev, "failed to request GPIO: %d\n", err);
 		return err;
 	}
 
-	if (panel->enable_gpio) {
-		err = gpiod_direction_output(panel->enable_gpio, 0);
-		if (err < 0) {
-			dev_err(dev, "failed to setup GPIO: %d\n", err);
-			return err;
-		}
-	}
-
 	backlight = of_parse_phandle(dev->of_node, "backlight", 0);
 	if (backlight) {
 		panel->backlight = of_find_backlight_by_node(backlight);
@@ -376,6 +369,29 @@ static const struct panel_desc auo_b101xtn01 = {
 	},
 };
 
+static const struct drm_display_mode auo_b116xw03_mode = {
+	.clock = 70589,
+	.hdisplay = 1366,
+	.hsync_start = 1366 + 40,
+	.hsync_end = 1366 + 40 + 40,
+	.htotal = 1366 + 40 + 40 + 32,
+	.vdisplay = 768,
+	.vsync_start = 768 + 10,
+	.vsync_end = 768 + 10 + 12,
+	.vtotal = 768 + 10 + 12 + 6,
+	.vrefresh = 60,
+};
+
+static const struct panel_desc auo_b116xw03 = {
+	.modes = &auo_b116xw03_mode,
+	.num_modes = 1,
+	.bpc = 6,
+	.size = {
+		.width = 256,
+		.height = 144,
+	},
+};
+
 static const struct drm_display_mode auo_b133xtn01_mode = {
 	.clock = 69500,
 	.hdisplay = 1366,
@@ -415,6 +431,7 @@ static const struct drm_display_mode auo_b133htn01_mode = {
 static const struct panel_desc auo_b133htn01 = {
 	.modes = &auo_b133htn01_mode,
 	.num_modes = 1,
+	.bpc = 6,
 	.size = {
 		.width = 293,
 		.height = 165,
@@ -536,22 +553,92 @@ static const struct drm_display_mode foxlink_fl500wvr00_a0t_mode = {
 static const struct panel_desc foxlink_fl500wvr00_a0t = {
 	.modes = &foxlink_fl500wvr00_a0t_mode,
 	.num_modes = 1,
+	.bpc = 8,
 	.size = {
 		.width = 108,
 		.height = 65,
 	},
 };
 
-static const struct drm_display_mode innolux_n116bge_mode = {
+static const struct drm_display_mode hannstar_hsd070pww1_mode = {
+	.clock = 71100,
+	.hdisplay = 1280,
+	.hsync_start = 1280 + 1,
+	.hsync_end = 1280 + 1 + 158,
+	.htotal = 1280 + 1 + 158 + 1,
+	.vdisplay = 800,
+	.vsync_start = 800 + 1,
+	.vsync_end = 800 + 1 + 21,
+	.vtotal = 800 + 1 + 21 + 1,
+	.vrefresh = 60,
+};
+
+static const struct panel_desc hannstar_hsd070pww1 = {
+	.modes = &hannstar_hsd070pww1_mode,
+	.num_modes = 1,
+	.bpc = 6,
+	.size = {
+		.width = 151,
+		.height = 94,
+	},
+};
+
+static const struct drm_display_mode hitachi_tx23d38vm0caa_mode = {
+	.clock = 33333,
+	.hdisplay = 800,
+	.hsync_start = 800 + 85,
+	.hsync_end = 800 + 85 + 86,
+	.htotal = 800 + 85 + 86 + 85,
+	.vdisplay = 480,
+	.vsync_start = 480 + 16,
+	.vsync_end = 480 + 16 + 13,
+	.vtotal = 480 + 16 + 13 + 16,
+	.vrefresh = 60,
+};
+
+static const struct panel_desc hitachi_tx23d38vm0caa = {
+	.modes = &hitachi_tx23d38vm0caa_mode,
+	.num_modes = 1,
+	.bpc = 6,
+	.size = {
+		.width = 195,
+		.height = 117,
+	},
+};
+
+static const struct drm_display_mode innolux_g121i1_l01_mode = {
 	.clock = 71000,
+	.hdisplay = 1280,
+	.hsync_start = 1280 + 64,
+	.hsync_end = 1280 + 64 + 32,
+	.htotal = 1280 + 64 + 32 + 64,
+	.vdisplay = 800,
+	.vsync_start = 800 + 9,
+	.vsync_end = 800 + 9 + 6,
+	.vtotal = 800 + 9 + 6 + 9,
+	.vrefresh = 60,
+};
+
+static const struct panel_desc innolux_g121i1_l01 = {
+	.modes = &innolux_g121i1_l01_mode,
+	.num_modes = 1,
+	.bpc = 6,
+	.size = {
+		.width = 261,
+		.height = 163,
+	},
+};
+
+static const struct drm_display_mode innolux_n116bge_mode = {
+	.clock = 76420,
 	.hdisplay = 1366,
-	.hsync_start = 1366 + 64,
-	.hsync_end = 1366 + 64 + 6,
-	.htotal = 1366 + 64 + 6 + 64,
+	.hsync_start = 1366 + 136,
+	.hsync_end = 1366 + 136 + 30,
+	.htotal = 1366 + 136 + 30 + 60,
 	.vdisplay = 768,
 	.vsync_start = 768 + 8,
-	.vsync_end = 768 + 8 + 4,
-	.vtotal = 768 + 8 + 4 + 8,
+	.vsync_end = 768 + 8 + 12,
+	.vtotal = 768 + 8 + 12 + 12,
 	.vrefresh = 60,
 	.flags = DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC,
 };
@@ -643,6 +730,9 @@ static const struct of_device_id platform_of_match[] = {
 		.compatible = "auo,b101xtn01",
 		.data = &auo_b101xtn01,
 	}, {
+		.compatible = "auo,b116xw03",
+		.data = &auo_b116xw03,
+	}, {
 		.compatible = "auo,b133htn01",
 		.data = &auo_b133htn01,
 	}, {
@@ -667,6 +757,15 @@ static const struct of_device_id platform_of_match[] = {
 		.compatible = "foxlink,fl500wvr00-a0t",
 		.data = &foxlink_fl500wvr00_a0t,
 	}, {
+		.compatible = "hannstar,hsd070pww1",
+		.data = &hannstar_hsd070pww1,
+	}, {
+		.compatible = "hit,tx23d38vm0caa",
+		.data = &hitachi_tx23d38vm0caa
+	}, {
+		.compatible ="innolux,g121i1-l01",
+		.data = &innolux_g121i1_l01
+	}, {
 		.compatible = "innolux,n116bge",
 		.data = &innolux_n116bge,
 	}, {
@@ -708,7 +807,6 @@ static void panel_simple_platform_shutdown(struct platform_device *pdev)
 static struct platform_driver panel_simple_platform_driver = {
 	.driver = {
 		.name = "panel-simple",
-		.owner = THIS_MODULE,
 		.of_match_table = platform_of_match,
 	},
 	.probe = panel_simple_platform_probe,
@@ -741,6 +839,7 @@ static const struct panel_desc_dsi lg_ld070wx3_sl01 = {
 	.desc = {
 		.modes = &lg_ld070wx3_sl01_mode,
 		.num_modes = 1,
+		.bpc = 8,
 		.size = {
 			.width = 94,
 			.height = 151,
@@ -768,6 +867,7 @@ static const struct panel_desc_dsi lg_lh500wx1_sd03 = {
 	.desc = {
 		.modes = &lg_lh500wx1_sd03_mode,
 		.num_modes = 1,
+		.bpc = 8,
 		.size = {
 			.width = 62,
 			.height = 110,
@@ -795,6 +895,7 @@ static const struct panel_desc_dsi panasonic_vvx10f004b00 = {
 	.desc = {
 		.modes = &panasonic_vvx10f004b00_mode,
 		.num_modes = 1,
+		.bpc = 8,
 		.size = {
 			.width = 217,
 			.height = 136,
@@ -864,7 +965,6 @@ static void panel_simple_dsi_shutdown(struct mipi_dsi_device *dsi)
 static struct mipi_dsi_driver panel_simple_dsi_driver = {
 	.driver = {
 		.name = "panel-simple-dsi",
-		.owner = THIS_MODULE,
 		.of_match_table = dsi_of_match,
 	},
 	.probe = panel_simple_dsi_probe,
diff --git a/drivers/gpu/drm/qxl/qxl_display.c b/drivers/gpu/drm/qxl/qxl_display.c
index 0d1396266857..4a0a8b29b0a1 100644
--- a/drivers/gpu/drm/qxl/qxl_display.c
+++ b/drivers/gpu/drm/qxl/qxl_display.c
@@ -29,6 +29,7 @@
 #include "qxl_drv.h"
 #include "qxl_object.h"
 #include "drm_crtc_helper.h"
+#include <drm/drm_plane_helper.h>
 
 static bool qxl_head_enabled(struct qxl_head *head)
 {
@@ -100,14 +101,37 @@ static int qxl_display_copy_rom_client_monitors_config(struct qxl_device *qdev)
 	return 0;
 }
 
+static void qxl_update_offset_props(struct qxl_device *qdev)
+{
+	struct drm_device *dev = qdev->ddev;
+	struct drm_connector *connector;
+	struct qxl_output *output;
+	struct qxl_head *head;
+
+	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
+		output = drm_connector_to_qxl_output(connector);
+
+		head = &qdev->client_monitors_config->heads[output->index];
+
+		drm_object_property_set_value(&connector->base,
+			dev->mode_config.suggested_x_property, head->x);
+		drm_object_property_set_value(&connector->base,
+			dev->mode_config.suggested_y_property, head->y);
+	}
+}
+
 void qxl_display_read_client_monitors_config(struct qxl_device *qdev)
 {
 
+	struct drm_device *dev = qdev->ddev;
 	while (qxl_display_copy_rom_client_monitors_config(qdev)) {
 		qxl_io_log(qdev, "failed crc check for client_monitors_config,"
 				 " retrying\n");
 	}
 
+	drm_modeset_lock_all(dev);
+	qxl_update_offset_props(qdev);
+	drm_modeset_unlock_all(dev);
 	if (!drm_helper_hpd_irq_event(qdev->ddev)) {
 		/* notify that the monitor configuration changed, to
 		   adjust at the arbitrary resolution */
@@ -568,7 +592,6 @@ static int qxl_crtc_mode_set(struct drm_crtc *crtc,
 {
 	struct drm_device *dev = crtc->dev;
 	struct qxl_device *qdev = dev->dev_private;
-	struct qxl_mode *m = (void *)mode->private;
 	struct qxl_framebuffer *qfb;
 	struct qxl_bo *bo, *old_bo = NULL;
 	struct qxl_crtc *qcrtc = to_qxl_crtc(crtc);
@@ -586,12 +609,6 @@ static int qxl_crtc_mode_set(struct drm_crtc *crtc,
 	}
 	qfb = to_qxl_framebuffer(crtc->primary->fb);
 	bo = gem_to_qxl_bo(qfb->obj);
-	if (!m)
-		/* and do we care? */
-		DRM_DEBUG("%dx%d: not a native mode\n", x, y);
-	else
-		DRM_DEBUG("%dx%d: qxl id %d\n",
-			  mode->hdisplay, mode->vdisplay, m->id);
 	DRM_DEBUG("+%d+%d (%d,%d) => (%d,%d)\n",
 		  x, y,
 		  mode->hdisplay, mode->vdisplay,
@@ -951,6 +968,10 @@ static int qdev_output_init(struct drm_device *dev, int num_output)
 
 	drm_object_attach_property(&connector->base,
 				   qdev->hotplug_mode_update_property, 0);
+	drm_object_attach_property(&connector->base,
+				   dev->mode_config.suggested_x_property, 0);
+	drm_object_attach_property(&connector->base,
+				   dev->mode_config.suggested_y_property, 0);
 	drm_connector_register(connector);
 	return 0;
 }
@@ -1064,6 +1085,7 @@ int qxl_modeset_init(struct qxl_device *qdev)
 
 	qdev->ddev->mode_config.fb_base = qdev->vram_base;
 
+	drm_mode_create_suggested_offset_properties(qdev->ddev);
 	qxl_mode_create_hotplug_mode_update_property(qdev);
 
 	for (i = 0 ; i < qxl_num_crtc; ++i) {
diff --git a/drivers/gpu/drm/qxl/qxl_release.c b/drivers/gpu/drm/qxl/qxl_release.c
index 446e71ca36cb..d9b25684ac98 100644
--- a/drivers/gpu/drm/qxl/qxl_release.c
+++ b/drivers/gpu/drm/qxl/qxl_release.c
@@ -264,7 +264,8 @@ int qxl_release_reserve_list(struct qxl_release *release, bool no_intr)
 	if (list_is_singular(&release->bos))
 		return 0;
 
-	ret = ttm_eu_reserve_buffers(&release->ticket, &release->bos, !no_intr);
+	ret = ttm_eu_reserve_buffers(&release->ticket, &release->bos,
+				     !no_intr, NULL);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpu/drm/r128/r128_state.c b/drivers/gpu/drm/r128/r128_state.c
index 575e986f82a7..8fd2d9f58f77 100644
--- a/drivers/gpu/drm/r128/r128_state.c
+++ b/drivers/gpu/drm/r128/r128_state.c
@@ -905,7 +905,7 @@ static int r128_cce_dispatch_write_span(struct drm_device *dev,
 	if (IS_ERR(buffer))
 		return PTR_ERR(buffer);
 
-	mask_size = depth->n * sizeof(u8);
+	mask_size = depth->n;
 	if (depth->mask) {
 		mask = memdup_user(depth->mask, mask_size);
 		if (IS_ERR(mask)) {
@@ -1010,7 +1010,7 @@ static int r128_cce_dispatch_write_pixels(struct drm_device *dev,
 	}
 
 	if (depth->mask) {
-		mask_size = depth->n * sizeof(u8);
+		mask_size = depth->n;
 		mask = memdup_user(depth->mask, mask_size);
 		if (IS_ERR(mask)) {
 			kfree(x);
diff --git a/drivers/gpu/drm/radeon/Makefile b/drivers/gpu/drm/radeon/Makefile
index d01b87991422..12bc21219a0e 100644
--- a/drivers/gpu/drm/radeon/Makefile
+++ b/drivers/gpu/drm/radeon/Makefile
@@ -80,7 +80,8 @@ radeon-y += radeon_device.o radeon_asic.o radeon_kms.o \
 	r600_dpm.o rs780_dpm.o rv6xx_dpm.o rv770_dpm.o rv730_dpm.o rv740_dpm.o \
 	rv770_smc.o cypress_dpm.o btc_dpm.o sumo_dpm.o sumo_smc.o trinity_dpm.o \
 	trinity_smc.o ni_dpm.o si_smc.o si_dpm.o kv_smc.o kv_dpm.o ci_smc.o \
-	ci_dpm.o dce6_afmt.o radeon_vm.o radeon_ucode.o radeon_ib.o radeon_mn.o
+	ci_dpm.o dce6_afmt.o radeon_vm.o radeon_ucode.o radeon_ib.o radeon_mn.o \
+	radeon_sync.o
 
 # add async DMA block
 radeon-y += \
@@ -104,6 +105,7 @@ radeon-y += \
 	radeon_vce.o \
 	vce_v1_0.o \
 	vce_v2_0.o \
+	radeon_kfd.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 30d242b25078..d59ec491dbb9 100644
--- a/drivers/gpu/drm/radeon/atombios_crtc.c
+++ b/drivers/gpu/drm/radeon/atombios_crtc.c
@@ -2039,6 +2039,7 @@ int atombios_crtc_mode_set(struct drm_crtc *crtc,
 	atombios_crtc_set_base(crtc, x, y, old_fb);
 	atombios_overscan_setup(crtc, mode, adjusted_mode);
 	atombios_scaler_setup(crtc);
+	radeon_cursor_reset(crtc);
 	/* update the hw version fpr dpm */
 	radeon_crtc->hw_mode = *adjusted_mode;
 
diff --git a/drivers/gpu/drm/radeon/ci_dpm.c b/drivers/gpu/drm/radeon/ci_dpm.c
index 11a55e9dad7f..f373a81ba3d5 100644
--- a/drivers/gpu/drm/radeon/ci_dpm.c
+++ b/drivers/gpu/drm/radeon/ci_dpm.c
@@ -46,15 +46,15 @@
 static const struct ci_pt_defaults defaults_hawaii_xt =
 {
 	1, 0xF, 0xFD, 0x19, 5, 0x14, 0, 0xB0000,
-	{ 0x84,  0x0,   0x0,   0x7F,  0x0,   0x0,   0x5A,  0x60,  0x51,  0x8E,  0x79,  0x6B,  0x5F,  0x90,  0x79  },
-	{ 0x1EA, 0x1EA, 0x1EA, 0x224, 0x224, 0x224, 0x24F, 0x24F, 0x24F, 0x28E, 0x28E, 0x28E, 0x2BC, 0x2BC, 0x2BC }
+	{ 0x2E,  0x00,  0x00,  0x88,  0x00,  0x00,  0x72,  0x60,  0x51,  0xA7,  0x79,  0x6B,  0x90,  0xBD,  0x79  },
+	{ 0x217, 0x217, 0x217, 0x242, 0x242, 0x242, 0x269, 0x269, 0x269, 0x2A1, 0x2A1, 0x2A1, 0x2C9, 0x2C9, 0x2C9 }
 };
 
 static const struct ci_pt_defaults defaults_hawaii_pro =
 {
 	1, 0xF, 0xFD, 0x19, 5, 0x14, 0, 0x65062,
-	{ 0x93,  0x0,   0x0,   0x97,  0x0,   0x0,   0x6B,  0x60,  0x51,  0x95,  0x79,  0x6B,  0x5F,  0x90,  0x79  },
-	{ 0x1EA, 0x1EA, 0x1EA, 0x224, 0x224, 0x224, 0x24F, 0x24F, 0x24F, 0x28E, 0x28E, 0x28E, 0x2BC, 0x2BC, 0x2BC }
+	{ 0x2E,  0x00,  0x00,  0x88,  0x00,  0x00,  0x72,  0x60,  0x51,  0xA7,  0x79,  0x6B,  0x90,  0xBD,  0x79  },
+	{ 0x217, 0x217, 0x217, 0x242, 0x242, 0x242, 0x269, 0x269, 0x269, 0x2A1, 0x2A1, 0x2A1, 0x2C9, 0x2C9, 0x2C9 }
 };
 
 static const struct ci_pt_defaults defaults_bonaire_xt =
@@ -184,6 +184,9 @@ static int ci_set_overdrive_target_tdp(struct radeon_device *rdev,
 				       u32 target_tdp);
 static int ci_update_uvd_dpm(struct radeon_device *rdev, bool gate);
 
+static PPSMC_Result ci_send_msg_to_smc_with_parameter(struct radeon_device *rdev,
+						      PPSMC_Msg msg, u32 parameter);
+
 static struct ci_power_info *ci_get_pi(struct radeon_device *rdev)
 {
         struct ci_power_info *pi = rdev->pm.dpm.priv;
@@ -249,7 +252,10 @@ static void ci_initialize_powertune_defaults(struct radeon_device *rdev)
 
 	if (pi->caps_power_containment) {
 		pi->caps_cac = true;
-		pi->enable_bapm_feature = true;
+		if (rdev->family == CHIP_HAWAII)
+			pi->enable_bapm_feature = false;
+		else
+			pi->enable_bapm_feature = true;
 		pi->enable_tdc_limit_feature = true;
 		pi->enable_pkg_pwr_tracking_feature = true;
 	}
@@ -352,6 +358,21 @@ static int ci_populate_dw8(struct radeon_device *rdev)
 	return 0;
 }
 
+static int ci_populate_fuzzy_fan(struct radeon_device *rdev)
+{
+	struct ci_power_info *pi = ci_get_pi(rdev);
+
+	if ((rdev->pm.dpm.fan.fan_output_sensitivity & (1 << 15)) ||
+	    (rdev->pm.dpm.fan.fan_output_sensitivity == 0))
+		rdev->pm.dpm.fan.fan_output_sensitivity =
+			rdev->pm.dpm.fan.default_fan_output_sensitivity;
+
+	pi->smc_powertune_table.FuzzyFan_PwmSetDelta =
+		cpu_to_be16(rdev->pm.dpm.fan.fan_output_sensitivity);
+
+	return 0;
+}
+
 static int ci_min_max_v_gnbl_pm_lid_from_bapm_vddc(struct radeon_device *rdev)
 {
 	struct ci_power_info *pi = ci_get_pi(rdev);
@@ -477,6 +498,9 @@ static int ci_populate_pm_base(struct radeon_device *rdev)
 		ret = ci_populate_dw8(rdev);
 		if (ret)
 			return ret;
+		ret = ci_populate_fuzzy_fan(rdev);
+		if (ret)
+			return ret;
 		ret = ci_min_max_v_gnbl_pm_lid_from_bapm_vddc(rdev);
 		if (ret)
 			return ret;
@@ -690,6 +714,25 @@ static int ci_enable_smc_cac(struct radeon_device *rdev, bool enable)
 	return ret;
 }
 
+static int ci_enable_thermal_based_sclk_dpm(struct radeon_device *rdev,
+					    bool enable)
+{
+	struct ci_power_info *pi = ci_get_pi(rdev);
+	PPSMC_Result smc_result = PPSMC_Result_OK;
+
+	if (pi->thermal_sclk_dpm_enabled) {
+		if (enable)
+			smc_result = ci_send_msg_to_smc(rdev, PPSMC_MSG_ENABLE_THERMAL_DPM);
+		else
+			smc_result = ci_send_msg_to_smc(rdev, PPSMC_MSG_DISABLE_THERMAL_DPM);
+	}
+
+	if (smc_result == PPSMC_Result_OK)
+		return 0;
+	else
+		return -EINVAL;
+}
+
 static int ci_power_control_set_level(struct radeon_device *rdev)
 {
 	struct ci_power_info *pi = ci_get_pi(rdev);
@@ -700,13 +743,11 @@ static int ci_power_control_set_level(struct radeon_device *rdev)
 	int ret = 0;
 	bool adjust_polarity = false; /* ??? */
 
-	if (pi->caps_power_containment &&
-	    (pi->power_containment_features & POWERCONTAINMENT_FEATURE_BAPM)) {
+	if (pi->caps_power_containment) {
 		adjust_percent = adjust_polarity ?
 			rdev->pm.dpm.tdp_adjustment : (-1 * rdev->pm.dpm.tdp_adjustment);
 		target_tdp = ((100 + adjust_percent) *
 			      (s32)cac_tdp_table->configurable_tdp) / 100;
-		target_tdp *= 256;
 
 		ret = ci_set_overdrive_target_tdp(rdev, (u32)target_tdp);
 	}
@@ -814,7 +855,7 @@ static void ci_apply_state_adjust_rules(struct radeon_device *rdev,
 	}
 }
 
-static int ci_set_thermal_temperature_range(struct radeon_device *rdev,
+static int ci_thermal_set_temperature_range(struct radeon_device *rdev,
 					    int min_temp, int max_temp)
 {
 	int low_temp = 0 * 1000;
@@ -850,6 +891,350 @@ static int ci_set_thermal_temperature_range(struct radeon_device *rdev,
 	return 0;
 }
 
+static int ci_thermal_enable_alert(struct radeon_device *rdev,
+				   bool enable)
+{
+	u32 thermal_int = RREG32_SMC(CG_THERMAL_INT);
+	PPSMC_Result result;
+
+	if (enable) {
+		thermal_int &= ~(THERM_INT_MASK_HIGH | THERM_INT_MASK_LOW);
+		WREG32_SMC(CG_THERMAL_INT, thermal_int);
+		rdev->irq.dpm_thermal = false;
+		result = ci_send_msg_to_smc(rdev, PPSMC_MSG_Thermal_Cntl_Enable);
+		if (result != PPSMC_Result_OK) {
+			DRM_DEBUG_KMS("Could not enable thermal interrupts.\n");
+			return -EINVAL;
+		}
+	} else {
+		thermal_int |= THERM_INT_MASK_HIGH | THERM_INT_MASK_LOW;
+		WREG32_SMC(CG_THERMAL_INT, thermal_int);
+		rdev->irq.dpm_thermal = true;
+		result = ci_send_msg_to_smc(rdev, PPSMC_MSG_Thermal_Cntl_Disable);
+		if (result != PPSMC_Result_OK) {
+			DRM_DEBUG_KMS("Could not disable thermal interrupts.\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static void ci_fan_ctrl_set_static_mode(struct radeon_device *rdev, u32 mode)
+{
+	struct ci_power_info *pi = ci_get_pi(rdev);
+	u32 tmp;
+
+	if (pi->fan_ctrl_is_in_default_mode) {
+		tmp = (RREG32_SMC(CG_FDO_CTRL2) & FDO_PWM_MODE_MASK) >> FDO_PWM_MODE_SHIFT;
+		pi->fan_ctrl_default_mode = tmp;
+		tmp = (RREG32_SMC(CG_FDO_CTRL2) & TMIN_MASK) >> TMIN_SHIFT;
+		pi->t_min = tmp;
+		pi->fan_ctrl_is_in_default_mode = false;
+	}
+
+	tmp = RREG32_SMC(CG_FDO_CTRL2) & ~TMIN_MASK;
+	tmp |= TMIN(0);
+	WREG32_SMC(CG_FDO_CTRL2, tmp);
+
+	tmp = RREG32_SMC(CG_FDO_CTRL2) & ~FDO_PWM_MODE_MASK;
+	tmp |= FDO_PWM_MODE(mode);
+	WREG32_SMC(CG_FDO_CTRL2, tmp);
+}
+
+static int ci_thermal_setup_fan_table(struct radeon_device *rdev)
+{
+	struct ci_power_info *pi = ci_get_pi(rdev);
+	SMU7_Discrete_FanTable fan_table = { FDO_MODE_HARDWARE };
+	u32 duty100;
+	u32 t_diff1, t_diff2, pwm_diff1, pwm_diff2;
+	u16 fdo_min, slope1, slope2;
+	u32 reference_clock, tmp;
+	int ret;
+	u64 tmp64;
+
+	if (!pi->fan_table_start) {
+		rdev->pm.dpm.fan.ucode_fan_control = false;
+		return 0;
+	}
+
+	duty100 = (RREG32_SMC(CG_FDO_CTRL1) & FMAX_DUTY100_MASK) >> FMAX_DUTY100_SHIFT;
+
+	if (duty100 == 0) {
+		rdev->pm.dpm.fan.ucode_fan_control = false;
+		return 0;
+	}
+
+	tmp64 = (u64)rdev->pm.dpm.fan.pwm_min * duty100;
+	do_div(tmp64, 10000);
+	fdo_min = (u16)tmp64;
+
+	t_diff1 = rdev->pm.dpm.fan.t_med - rdev->pm.dpm.fan.t_min;
+	t_diff2 = rdev->pm.dpm.fan.t_high - rdev->pm.dpm.fan.t_med;
+
+	pwm_diff1 = rdev->pm.dpm.fan.pwm_med - rdev->pm.dpm.fan.pwm_min;
+	pwm_diff2 = rdev->pm.dpm.fan.pwm_high - rdev->pm.dpm.fan.pwm_med;
+
+	slope1 = (u16)((50 + ((16 * duty100 * pwm_diff1) / t_diff1)) / 100);
+	slope2 = (u16)((50 + ((16 * duty100 * pwm_diff2) / t_diff2)) / 100);
+
+	fan_table.TempMin = cpu_to_be16((50 + rdev->pm.dpm.fan.t_min) / 100);
+	fan_table.TempMed = cpu_to_be16((50 + rdev->pm.dpm.fan.t_med) / 100);
+	fan_table.TempMax = cpu_to_be16((50 + rdev->pm.dpm.fan.t_max) / 100);
+
+	fan_table.Slope1 = cpu_to_be16(slope1);
+	fan_table.Slope2 = cpu_to_be16(slope2);
+
+	fan_table.FdoMin = cpu_to_be16(fdo_min);
+
+	fan_table.HystDown = cpu_to_be16(rdev->pm.dpm.fan.t_hyst);
+
+	fan_table.HystUp = cpu_to_be16(1);
+
+	fan_table.HystSlope = cpu_to_be16(1);
+
+	fan_table.TempRespLim = cpu_to_be16(5);
+
+	reference_clock = radeon_get_xclk(rdev);
+
+	fan_table.RefreshPeriod = cpu_to_be32((rdev->pm.dpm.fan.cycle_delay *
+					       reference_clock) / 1600);
+
+	fan_table.FdoMax = cpu_to_be16((u16)duty100);
+
+	tmp = (RREG32_SMC(CG_MULT_THERMAL_CTRL) & TEMP_SEL_MASK) >> TEMP_SEL_SHIFT;
+	fan_table.TempSrc = (uint8_t)tmp;
+
+	ret = ci_copy_bytes_to_smc(rdev,
+				   pi->fan_table_start,
+				   (u8 *)(&fan_table),
+				   sizeof(fan_table),
+				   pi->sram_end);
+
+	if (ret) {
+		DRM_ERROR("Failed to load fan table to the SMC.");
+		rdev->pm.dpm.fan.ucode_fan_control = false;
+	}
+
+	return 0;
+}
+
+static int ci_fan_ctrl_start_smc_fan_control(struct radeon_device *rdev)
+{
+	struct ci_power_info *pi = ci_get_pi(rdev);
+	PPSMC_Result ret;
+
+	if (pi->caps_od_fuzzy_fan_control_support) {
+		ret = ci_send_msg_to_smc_with_parameter(rdev,
+							PPSMC_StartFanControl,
+							FAN_CONTROL_FUZZY);
+		if (ret != PPSMC_Result_OK)
+			return -EINVAL;
+		ret = ci_send_msg_to_smc_with_parameter(rdev,
+							PPSMC_MSG_SetFanPwmMax,
+							rdev->pm.dpm.fan.default_max_fan_pwm);
+		if (ret != PPSMC_Result_OK)
+			return -EINVAL;
+	} else {
+		ret = ci_send_msg_to_smc_with_parameter(rdev,
+							PPSMC_StartFanControl,
+							FAN_CONTROL_TABLE);
+		if (ret != PPSMC_Result_OK)
+			return -EINVAL;
+	}
+
+	return 0;
+}
+
+#if 0
+static int ci_fan_ctrl_stop_smc_fan_control(struct radeon_device *rdev)
+{
+	PPSMC_Result ret;
+
+	ret = ci_send_msg_to_smc(rdev, PPSMC_StopFanControl);
+	if (ret == PPSMC_Result_OK)
+		return 0;
+	else
+		return -EINVAL;
+}
+
+static int ci_fan_ctrl_get_fan_speed_percent(struct radeon_device *rdev,
+					     u32 *speed)
+{
+	u32 duty, duty100;
+	u64 tmp64;
+
+	if (rdev->pm.no_fan)
+		return -ENOENT;
+
+	duty100 = (RREG32_SMC(CG_FDO_CTRL1) & FMAX_DUTY100_MASK) >> FMAX_DUTY100_SHIFT;
+	duty = (RREG32_SMC(CG_THERMAL_STATUS) & FDO_PWM_DUTY_MASK) >> FDO_PWM_DUTY_SHIFT;
+
+	if (duty100 == 0)
+		return -EINVAL;
+
+	tmp64 = (u64)duty * 100;
+	do_div(tmp64, duty100);
+	*speed = (u32)tmp64;
+
+	if (*speed > 100)
+		*speed = 100;
+
+	return 0;
+}
+
+static int ci_fan_ctrl_set_fan_speed_percent(struct radeon_device *rdev,
+					     u32 speed)
+{
+	u32 tmp;
+	u32 duty, duty100;
+	u64 tmp64;
+
+	if (rdev->pm.no_fan)
+		return -ENOENT;
+
+	if (speed > 100)
+		return -EINVAL;
+
+	if (rdev->pm.dpm.fan.ucode_fan_control)
+		ci_fan_ctrl_stop_smc_fan_control(rdev);
+
+	duty100 = (RREG32_SMC(CG_FDO_CTRL1) & FMAX_DUTY100_MASK) >> FMAX_DUTY100_SHIFT;
+
+	if (duty100 == 0)
+		return -EINVAL;
+
+	tmp64 = (u64)speed * duty100;
+	do_div(tmp64, 100);
+	duty = (u32)tmp64;
+
+	tmp = RREG32_SMC(CG_FDO_CTRL0) & ~FDO_STATIC_DUTY_MASK;
+	tmp |= FDO_STATIC_DUTY(duty);
+	WREG32_SMC(CG_FDO_CTRL0, tmp);
+
+	ci_fan_ctrl_set_static_mode(rdev, FDO_PWM_MODE_STATIC);
+
+	return 0;
+}
+
+static int ci_fan_ctrl_get_fan_speed_rpm(struct radeon_device *rdev,
+					 u32 *speed)
+{
+	u32 tach_period;
+	u32 xclk = radeon_get_xclk(rdev);
+
+	if (rdev->pm.no_fan)
+		return -ENOENT;
+
+	if (rdev->pm.fan_pulses_per_revolution == 0)
+		return -ENOENT;
+
+	tach_period = (RREG32_SMC(CG_TACH_STATUS) & TACH_PERIOD_MASK) >> TACH_PERIOD_SHIFT;
+	if (tach_period == 0)
+		return -ENOENT;
+
+	*speed = 60 * xclk * 10000 / tach_period;
+
+	return 0;
+}
+
+static int ci_fan_ctrl_set_fan_speed_rpm(struct radeon_device *rdev,
+					 u32 speed)
+{
+	u32 tach_period, tmp;
+	u32 xclk = radeon_get_xclk(rdev);
+
+	if (rdev->pm.no_fan)
+		return -ENOENT;
+
+	if (rdev->pm.fan_pulses_per_revolution == 0)
+		return -ENOENT;
+
+	if ((speed < rdev->pm.fan_min_rpm) ||
+	    (speed > rdev->pm.fan_max_rpm))
+		return -EINVAL;
+
+	if (rdev->pm.dpm.fan.ucode_fan_control)
+		ci_fan_ctrl_stop_smc_fan_control(rdev);
+
+	tach_period = 60 * xclk * 10000 / (8 * speed);
+	tmp = RREG32_SMC(CG_TACH_CTRL) & ~TARGET_PERIOD_MASK;
+	tmp |= TARGET_PERIOD(tach_period);
+	WREG32_SMC(CG_TACH_CTRL, tmp);
+
+	ci_fan_ctrl_set_static_mode(rdev, FDO_PWM_MODE_STATIC_RPM);
+
+	return 0;
+}
+#endif
+
+static void ci_fan_ctrl_set_default_mode(struct radeon_device *rdev)
+{
+	struct ci_power_info *pi = ci_get_pi(rdev);
+	u32 tmp;
+
+	if (!pi->fan_ctrl_is_in_default_mode) {
+		tmp = RREG32_SMC(CG_FDO_CTRL2) & ~FDO_PWM_MODE_MASK;
+		tmp |= FDO_PWM_MODE(pi->fan_ctrl_default_mode);
+		WREG32_SMC(CG_FDO_CTRL2, tmp);
+
+		tmp = RREG32_SMC(CG_FDO_CTRL2) & ~TMIN_MASK;
+		tmp |= TMIN(pi->t_min);
+		WREG32_SMC(CG_FDO_CTRL2, tmp);
+		pi->fan_ctrl_is_in_default_mode = true;
+	}
+}
+
+static void ci_thermal_start_smc_fan_control(struct radeon_device *rdev)
+{
+	if (rdev->pm.dpm.fan.ucode_fan_control) {
+		ci_fan_ctrl_start_smc_fan_control(rdev);
+		ci_fan_ctrl_set_static_mode(rdev, FDO_PWM_MODE_STATIC);
+	}
+}
+
+static void ci_thermal_initialize(struct radeon_device *rdev)
+{
+	u32 tmp;
+
+	if (rdev->pm.fan_pulses_per_revolution) {
+		tmp = RREG32_SMC(CG_TACH_CTRL) & ~EDGE_PER_REV_MASK;
+		tmp |= EDGE_PER_REV(rdev->pm.fan_pulses_per_revolution -1);
+		WREG32_SMC(CG_TACH_CTRL, tmp);
+	}
+
+	tmp = RREG32_SMC(CG_FDO_CTRL2) & ~TACH_PWM_RESP_RATE_MASK;
+	tmp |= TACH_PWM_RESP_RATE(0x28);
+	WREG32_SMC(CG_FDO_CTRL2, tmp);
+}
+
+static int ci_thermal_start_thermal_controller(struct radeon_device *rdev)
+{
+	int ret;
+
+	ci_thermal_initialize(rdev);
+	ret = ci_thermal_set_temperature_range(rdev, R600_TEMP_RANGE_MIN, R600_TEMP_RANGE_MAX);
+	if (ret)
+		return ret;
+	ret = ci_thermal_enable_alert(rdev, true);
+	if (ret)
+		return ret;
+	if (rdev->pm.dpm.fan.ucode_fan_control) {
+		ret = ci_thermal_setup_fan_table(rdev);
+		if (ret)
+			return ret;
+		ci_thermal_start_smc_fan_control(rdev);
+	}
+
+	return 0;
+}
+
+static void ci_thermal_stop_thermal_controller(struct radeon_device *rdev)
+{
+	if (!rdev->pm.no_fan)
+		ci_fan_ctrl_set_default_mode(rdev);
+}
+
 #if 0
 static int ci_read_smc_soft_register(struct radeon_device *rdev,
 				     u16 reg_offset, u32 *value)
@@ -1253,7 +1638,7 @@ static int ci_dpm_force_state_sclk(struct radeon_device *rdev, u32 n)
 
 	if (!pi->sclk_dpm_key_disabled) {
 		PPSMC_Result smc_result =
-			ci_send_msg_to_smc_with_parameter(rdev, PPSMC_MSG_DPM_ForceState, n);
+			ci_send_msg_to_smc_with_parameter(rdev, PPSMC_MSG_SCLKDPM_SetEnabledMask, 1 << n);
 		if (smc_result != PPSMC_Result_OK)
 			return -EINVAL;
 	}
@@ -1267,7 +1652,7 @@ static int ci_dpm_force_state_mclk(struct radeon_device *rdev, u32 n)
 
 	if (!pi->mclk_dpm_key_disabled) {
 		PPSMC_Result smc_result =
-			ci_send_msg_to_smc_with_parameter(rdev, PPSMC_MSG_MCLKDPM_ForceState, n);
+			ci_send_msg_to_smc_with_parameter(rdev, PPSMC_MSG_MCLKDPM_SetEnabledMask, 1 << n);
 		if (smc_result != PPSMC_Result_OK)
 			return -EINVAL;
 	}
@@ -2042,6 +2427,33 @@ static int ci_force_switch_to_arb_f0(struct radeon_device *rdev)
 	return ni_copy_and_switch_arb_sets(rdev, tmp, MC_CG_ARB_FREQ_F0);
 }
 
+static void ci_register_patching_mc_arb(struct radeon_device *rdev,
+					const u32 engine_clock,
+					const u32 memory_clock,
+					u32 *dram_timimg2)
+{
+	bool patch;
+	u32 tmp, tmp2;
+
+	tmp = RREG32(MC_SEQ_MISC0);
+	patch = ((tmp & 0x0000f00) == 0x300) ? true : false;
+
+	if (patch &&
+	    ((rdev->pdev->device == 0x67B0) ||
+	     (rdev->pdev->device == 0x67B1))) {
+		if ((memory_clock > 100000) && (memory_clock <= 125000)) {
+			tmp2 = (((0x31 * engine_clock) / 125000) - 1) & 0xff;
+			*dram_timimg2 &= ~0x00ff0000;
+			*dram_timimg2 |= tmp2 << 16;
+		} else if ((memory_clock > 125000) && (memory_clock <= 137500)) {
+			tmp2 = (((0x36 * engine_clock) / 137500) - 1) & 0xff;
+			*dram_timimg2 &= ~0x00ff0000;
+			*dram_timimg2 |= tmp2 << 16;
+		}
+	}
+}
+
+
 static int ci_populate_memory_timing_parameters(struct radeon_device *rdev,
 						u32 sclk,
 						u32 mclk,
@@ -2057,6 +2469,8 @@ static int ci_populate_memory_timing_parameters(struct radeon_device *rdev,
 	dram_timing2 = RREG32(MC_ARB_DRAM_TIMING2);
 	burst_time = RREG32(MC_ARB_BURST_TIME) & STATE0_MASK;
 
+	ci_register_patching_mc_arb(rdev, sclk, mclk, &dram_timing2);
+
 	arb_regs->McArbDramTiming  = cpu_to_be32(dram_timing);
 	arb_regs->McArbDramTiming2 = cpu_to_be32(dram_timing2);
 	arb_regs->McArbBurstTime = (u8)burst_time;
@@ -2351,10 +2765,10 @@ static int ci_calculate_mclk_params(struct radeon_device *rdev,
 		u32 tmp;
 		u32 reference_clock = rdev->clock.mpll.reference_freq;
 
-		if (pi->mem_gddr5)
-			freq_nom = memory_clock * 4;
+		if (mpll_param.qdr == 1)
+			freq_nom = memory_clock * 4 * (1 << mpll_param.post_div);
 		else
-			freq_nom = memory_clock * 2;
+			freq_nom = memory_clock * 2 * (1 << mpll_param.post_div);
 
 		tmp = (freq_nom / reference_clock);
 		tmp = tmp * tmp;
@@ -2434,7 +2848,6 @@ static int ci_populate_single_memory_level(struct radeon_device *rdev,
 						      &memory_level->MinVddcPhases);
 
 	memory_level->EnabledForThrottle = 1;
-	memory_level->EnabledForActivity = 1;
 	memory_level->UpH = 0;
 	memory_level->DownH = 100;
 	memory_level->VoltageDownH = 0;
@@ -2767,7 +3180,6 @@ static int ci_populate_single_graphic_level(struct radeon_device *rdev,
 
 	graphic_level->CcPwrDynRm = 0;
 	graphic_level->CcPwrDynRm1 = 0;
-	graphic_level->EnabledForActivity = 1;
 	graphic_level->EnabledForThrottle = 1;
 	graphic_level->UpH = 0;
 	graphic_level->DownH = 0;
@@ -2816,10 +3228,13 @@ static int ci_populate_all_graphic_levels(struct radeon_device *rdev)
 						       &pi->smc_state_table.GraphicsLevel[i]);
 		if (ret)
 			return ret;
+		if (i > 1)
+			pi->smc_state_table.GraphicsLevel[i].DeepSleepDivId = 0;
 		if (i == (dpm_table->sclk_table.count - 1))
 			pi->smc_state_table.GraphicsLevel[i].DisplayWatermark =
 				PPSMC_DISPLAY_WATERMARK_HIGH;
 	}
+	pi->smc_state_table.GraphicsLevel[0].EnabledForActivity = 1;
 
 	pi->smc_state_table.GraphicsDpmLevelCount = (u8)dpm_table->sclk_table.count;
 	pi->dpm_level_enable_mask.sclk_dpm_enable_mask =
@@ -2863,6 +3278,16 @@ static int ci_populate_all_memory_levels(struct radeon_device *rdev)
 			return ret;
 	}
 
+	pi->smc_state_table.MemoryLevel[0].EnabledForActivity = 1;
+
+	if ((dpm_table->mclk_table.count >= 2) &&
+	    ((rdev->pdev->device == 0x67B0) || (rdev->pdev->device == 0x67B1))) {
+		pi->smc_state_table.MemoryLevel[1].MinVddc =
+			pi->smc_state_table.MemoryLevel[0].MinVddc;
+		pi->smc_state_table.MemoryLevel[1].MinVddcPhases =
+			pi->smc_state_table.MemoryLevel[0].MinVddcPhases;
+	}
+
 	pi->smc_state_table.MemoryLevel[0].ActivityLevel = cpu_to_be16(0x1F);
 
 	pi->smc_state_table.MemoryDpmLevelCount = (u8)dpm_table->mclk_table.count;
@@ -2919,9 +3344,14 @@ static int ci_setup_default_pcie_tables(struct radeon_device *rdev)
 				  &pi->dpm_table.pcie_speed_table,
 				  SMU7_MAX_LEVELS_LINK);
 
-	ci_setup_pcie_table_entry(&pi->dpm_table.pcie_speed_table, 0,
-				  pi->pcie_gen_powersaving.min,
-				  pi->pcie_lane_powersaving.min);
+	if (rdev->family == CHIP_BONAIRE)
+		ci_setup_pcie_table_entry(&pi->dpm_table.pcie_speed_table, 0,
+					  pi->pcie_gen_powersaving.min,
+					  pi->pcie_lane_powersaving.max);
+	else
+		ci_setup_pcie_table_entry(&pi->dpm_table.pcie_speed_table, 0,
+					  pi->pcie_gen_powersaving.min,
+					  pi->pcie_lane_powersaving.min);
 	ci_setup_pcie_table_entry(&pi->dpm_table.pcie_speed_table, 1,
 				  pi->pcie_gen_performance.min,
 				  pi->pcie_lane_performance.min);
@@ -2988,19 +3418,21 @@ static int ci_setup_default_dpm_tables(struct radeon_device *rdev)
 		     allowed_sclk_vddc_table->entries[i].clk)) {
 			pi->dpm_table.sclk_table.dpm_levels[pi->dpm_table.sclk_table.count].value =
 				allowed_sclk_vddc_table->entries[i].clk;
-			pi->dpm_table.sclk_table.dpm_levels[pi->dpm_table.sclk_table.count].enabled = true;
+			pi->dpm_table.sclk_table.dpm_levels[pi->dpm_table.sclk_table.count].enabled =
+				(i == 0) ? true : false;
 			pi->dpm_table.sclk_table.count++;
 		}
 	}
 
 	pi->dpm_table.mclk_table.count = 0;
 	for (i = 0; i < allowed_mclk_table->count; i++) {
-		if ((i==0) ||
+		if ((i == 0) ||
 		    (pi->dpm_table.mclk_table.dpm_levels[pi->dpm_table.mclk_table.count-1].value !=
 		     allowed_mclk_table->entries[i].clk)) {
 			pi->dpm_table.mclk_table.dpm_levels[pi->dpm_table.mclk_table.count].value =
 				allowed_mclk_table->entries[i].clk;
-			pi->dpm_table.mclk_table.dpm_levels[pi->dpm_table.mclk_table.count].enabled = true;
+			pi->dpm_table.mclk_table.dpm_levels[pi->dpm_table.mclk_table.count].enabled =
+				(i == 0) ? true : false;
 			pi->dpm_table.mclk_table.count++;
 		}
 	}
@@ -3166,7 +3598,7 @@ static int ci_init_smc_table(struct radeon_device *rdev)
 	table->VddcVddciDelta = 4000;
 	table->PhaseResponseTime = 0;
 	table->MemoryThermThrottleEnable = 1;
-	table->PCIeBootLinkLevel = 0;
+	table->PCIeBootLinkLevel = pi->dpm_table.pcie_speed_table.count - 1;
 	table->PCIeGenInterval = 1;
 	if (pi->voltage_control == CISLANDS_VOLTAGE_CONTROL_BY_SVID2)
 		table->SVI2Enable  = 1;
@@ -3320,6 +3752,8 @@ static int ci_upload_dpm_level_enable_mask(struct radeon_device *rdev)
 	struct ci_power_info *pi = ci_get_pi(rdev);
 	PPSMC_Result result;
 
+	ci_apply_disp_minimum_voltage_request(rdev);
+
 	if (!pi->sclk_dpm_key_disabled) {
 		if (pi->dpm_level_enable_mask.sclk_dpm_enable_mask) {
 			result = ci_send_msg_to_smc_with_parameter(rdev,
@@ -3339,7 +3773,7 @@ static int ci_upload_dpm_level_enable_mask(struct radeon_device *rdev)
 				return -EINVAL;
 		}
 	}
-
+#if 0
 	if (!pi->pcie_dpm_key_disabled) {
 		if (pi->dpm_level_enable_mask.pcie_dpm_enable_mask) {
 			result = ci_send_msg_to_smc_with_parameter(rdev,
@@ -3349,9 +3783,7 @@ static int ci_upload_dpm_level_enable_mask(struct radeon_device *rdev)
 				return -EINVAL;
 		}
 	}
-
-	ci_apply_disp_minimum_voltage_request(rdev);
-
+#endif
 	return 0;
 }
 
@@ -3377,7 +3809,7 @@ static void ci_find_dpm_states_clocks_in_dpm_table(struct radeon_device *rdev,
 		pi->need_update_smu7_dpm_table |= DPMTABLE_OD_UPDATE_SCLK;
 	} else {
 		/* XXX check display min clock requirements */
-		if (0 != CISLAND_MINIMUM_ENGINE_CLOCK)
+		if (CISLAND_MINIMUM_ENGINE_CLOCK != CISLAND_MINIMUM_ENGINE_CLOCK)
 			pi->need_update_smu7_dpm_table |= DPMTABLE_UPDATE_SCLK;
 	}
 
@@ -3707,62 +4139,61 @@ int ci_dpm_force_performance_level(struct radeon_device *rdev,
 				   enum radeon_dpm_forced_level level)
 {
 	struct ci_power_info *pi = ci_get_pi(rdev);
-	PPSMC_Result smc_result;
 	u32 tmp, levels, i;
 	int ret;
 
 	if (level == RADEON_DPM_FORCED_LEVEL_HIGH) {
-		if ((!pi->sclk_dpm_key_disabled) &&
-		    pi->dpm_level_enable_mask.sclk_dpm_enable_mask) {
+		if ((!pi->pcie_dpm_key_disabled) &&
+		    pi->dpm_level_enable_mask.pcie_dpm_enable_mask) {
 			levels = 0;
-			tmp = pi->dpm_level_enable_mask.sclk_dpm_enable_mask;
+			tmp = pi->dpm_level_enable_mask.pcie_dpm_enable_mask;
 			while (tmp >>= 1)
 				levels++;
 			if (levels) {
-				ret = ci_dpm_force_state_sclk(rdev, levels);
+				ret = ci_dpm_force_state_pcie(rdev, level);
 				if (ret)
 					return ret;
 				for (i = 0; i < rdev->usec_timeout; i++) {
-					tmp = (RREG32_SMC(TARGET_AND_CURRENT_PROFILE_INDEX) &
-					       CURR_SCLK_INDEX_MASK) >> CURR_SCLK_INDEX_SHIFT;
+					tmp = (RREG32_SMC(TARGET_AND_CURRENT_PROFILE_INDEX_1) &
+					       CURR_PCIE_INDEX_MASK) >> CURR_PCIE_INDEX_SHIFT;
 					if (tmp == levels)
 						break;
 					udelay(1);
 				}
 			}
 		}
-		if ((!pi->mclk_dpm_key_disabled) &&
-		    pi->dpm_level_enable_mask.mclk_dpm_enable_mask) {
+		if ((!pi->sclk_dpm_key_disabled) &&
+		    pi->dpm_level_enable_mask.sclk_dpm_enable_mask) {
 			levels = 0;
-			tmp = pi->dpm_level_enable_mask.mclk_dpm_enable_mask;
+			tmp = pi->dpm_level_enable_mask.sclk_dpm_enable_mask;
 			while (tmp >>= 1)
 				levels++;
 			if (levels) {
-				ret = ci_dpm_force_state_mclk(rdev, levels);
+				ret = ci_dpm_force_state_sclk(rdev, levels);
 				if (ret)
 					return ret;
 				for (i = 0; i < rdev->usec_timeout; i++) {
 					tmp = (RREG32_SMC(TARGET_AND_CURRENT_PROFILE_INDEX) &
-					       CURR_MCLK_INDEX_MASK) >> CURR_MCLK_INDEX_SHIFT;
+					       CURR_SCLK_INDEX_MASK) >> CURR_SCLK_INDEX_SHIFT;
 					if (tmp == levels)
 						break;
 					udelay(1);
 				}
 			}
 		}
-		if ((!pi->pcie_dpm_key_disabled) &&
-		    pi->dpm_level_enable_mask.pcie_dpm_enable_mask) {
+		if ((!pi->mclk_dpm_key_disabled) &&
+		    pi->dpm_level_enable_mask.mclk_dpm_enable_mask) {
 			levels = 0;
-			tmp = pi->dpm_level_enable_mask.pcie_dpm_enable_mask;
+			tmp = pi->dpm_level_enable_mask.mclk_dpm_enable_mask;
 			while (tmp >>= 1)
 				levels++;
 			if (levels) {
-				ret = ci_dpm_force_state_pcie(rdev, level);
+				ret = ci_dpm_force_state_mclk(rdev, levels);
 				if (ret)
 					return ret;
 				for (i = 0; i < rdev->usec_timeout; i++) {
-					tmp = (RREG32_SMC(TARGET_AND_CURRENT_PROFILE_INDEX_1) &
-					       CURR_PCIE_INDEX_MASK) >> CURR_PCIE_INDEX_SHIFT;
+					tmp = (RREG32_SMC(TARGET_AND_CURRENT_PROFILE_INDEX) &
+					       CURR_MCLK_INDEX_MASK) >> CURR_MCLK_INDEX_SHIFT;
 					if (tmp == levels)
 						break;
 					udelay(1);
@@ -3816,21 +4247,17 @@ int ci_dpm_force_performance_level(struct radeon_device *rdev,
 			}
 		}
 	} else if (level == RADEON_DPM_FORCED_LEVEL_AUTO) {
-		if (!pi->sclk_dpm_key_disabled) {
-			smc_result = ci_send_msg_to_smc(rdev, PPSMC_MSG_NoForcedLevel);
-			if (smc_result != PPSMC_Result_OK)
-				return -EINVAL;
-		}
-		if (!pi->mclk_dpm_key_disabled) {
-			smc_result = ci_send_msg_to_smc(rdev, PPSMC_MSG_MCLKDPM_NoForcedLevel);
-			if (smc_result != PPSMC_Result_OK)
-				return -EINVAL;
-		}
 		if (!pi->pcie_dpm_key_disabled) {
-			smc_result = ci_send_msg_to_smc(rdev, PPSMC_MSG_PCIeDPM_UnForceLevel);
+			PPSMC_Result smc_result;
+
+			smc_result = ci_send_msg_to_smc(rdev,
+							PPSMC_MSG_PCIeDPM_UnForceLevel);
 			if (smc_result != PPSMC_Result_OK)
 				return -EINVAL;
 		}
+		ret = ci_upload_dpm_level_enable_mask(rdev);
+		if (ret)
+			return ret;
 	}
 
 	rdev->pm.dpm.forced_level = level;
@@ -4036,6 +4463,96 @@ static int ci_copy_vbios_mc_reg_table(const struct atom_mc_reg_table *table,
 	return 0;
 }
 
+static int ci_register_patching_mc_seq(struct radeon_device *rdev,
+				       struct ci_mc_reg_table *table)
+{
+	u8 i, k;
+	u32 tmp;
+	bool patch;
+
+	tmp = RREG32(MC_SEQ_MISC0);
+	patch = ((tmp & 0x0000f00) == 0x300) ? true : false;
+
+	if (patch &&
+	    ((rdev->pdev->device == 0x67B0) ||
+	     (rdev->pdev->device == 0x67B1))) {
+		for (i = 0; i < table->last; i++) {
+			if (table->last >= SMU7_DISCRETE_MC_REGISTER_ARRAY_SIZE)
+				return -EINVAL;
+			switch(table->mc_reg_address[i].s1 >> 2) {
+			case MC_SEQ_MISC1:
+				for (k = 0; k < table->num_entries; k++) {
+					if ((table->mc_reg_table_entry[k].mclk_max == 125000) ||
+					    (table->mc_reg_table_entry[k].mclk_max == 137500))
+						table->mc_reg_table_entry[k].mc_data[i] =
+							(table->mc_reg_table_entry[k].mc_data[i] & 0xFFFFFFF8) |
+							0x00000007;
+				}
+				break;
+			case MC_SEQ_WR_CTL_D0:
+				for (k = 0; k < table->num_entries; k++) {
+					if ((table->mc_reg_table_entry[k].mclk_max == 125000) ||
+					    (table->mc_reg_table_entry[k].mclk_max == 137500))
+						table->mc_reg_table_entry[k].mc_data[i] =
+							(table->mc_reg_table_entry[k].mc_data[i] & 0xFFFF0F00) |
+							0x0000D0DD;
+				}
+				break;
+			case MC_SEQ_WR_CTL_D1:
+				for (k = 0; k < table->num_entries; k++) {
+					if ((table->mc_reg_table_entry[k].mclk_max == 125000) ||
+					    (table->mc_reg_table_entry[k].mclk_max == 137500))
+						table->mc_reg_table_entry[k].mc_data[i] =
+							(table->mc_reg_table_entry[k].mc_data[i] & 0xFFFF0F00) |
+							0x0000D0DD;
+				}
+				break;
+			case MC_SEQ_WR_CTL_2:
+				for (k = 0; k < table->num_entries; k++) {
+					if ((table->mc_reg_table_entry[k].mclk_max == 125000) ||
+					    (table->mc_reg_table_entry[k].mclk_max == 137500))
+						table->mc_reg_table_entry[k].mc_data[i] = 0;
+				}
+				break;
+			case MC_SEQ_CAS_TIMING:
+				for (k = 0; k < table->num_entries; k++) {
+					if (table->mc_reg_table_entry[k].mclk_max == 125000)
+						table->mc_reg_table_entry[k].mc_data[i] =
+							(table->mc_reg_table_entry[k].mc_data[i] & 0xFFE0FE0F) |
+							0x000C0140;
+					else if (table->mc_reg_table_entry[k].mclk_max == 137500)
+						table->mc_reg_table_entry[k].mc_data[i] =
+							(table->mc_reg_table_entry[k].mc_data[i] & 0xFFE0FE0F) |
+							0x000C0150;
+				}
+				break;
+			case MC_SEQ_MISC_TIMING:
+				for (k = 0; k < table->num_entries; k++) {
+					if (table->mc_reg_table_entry[k].mclk_max == 125000)
+						table->mc_reg_table_entry[k].mc_data[i] =
+							(table->mc_reg_table_entry[k].mc_data[i] & 0xFFFFFFE0) |
+							0x00000030;
+					else if (table->mc_reg_table_entry[k].mclk_max == 137500)
+						table->mc_reg_table_entry[k].mc_data[i] =
+							(table->mc_reg_table_entry[k].mc_data[i] & 0xFFFFFFE0) |
+							0x00000035;
+				}
+				break;
+			default:
+				break;
+			}
+		}
+
+		WREG32(MC_SEQ_IO_DEBUG_INDEX, 3);
+		tmp = RREG32(MC_SEQ_IO_DEBUG_DATA);
+		tmp = (tmp & 0xFFF8FFFF) | (1 << 16);
+		WREG32(MC_SEQ_IO_DEBUG_INDEX, 3);
+		WREG32(MC_SEQ_IO_DEBUG_DATA, tmp);
+	}
+
+	return 0;
+}
+
 static int ci_initialize_mc_reg_table(struct radeon_device *rdev)
 {
 	struct ci_power_info *pi = ci_get_pi(rdev);
@@ -4079,6 +4596,10 @@ static int ci_initialize_mc_reg_table(struct radeon_device *rdev)
 
 	ci_set_s0_mc_reg_index(ci_table);
 
+	ret = ci_register_patching_mc_seq(rdev, ci_table);
+	if (ret)
+		goto init_mc_done;
+
 	ret = ci_set_mc_special_registers(rdev, ci_table);
 	if (ret)
 		goto init_mc_done;
@@ -4675,36 +5196,51 @@ int ci_dpm_enable(struct radeon_device *rdev)
 		return ret;
 	}
 
+	ret = ci_power_control_set_level(rdev);
+	if (ret) {
+		DRM_ERROR("ci_power_control_set_level failed\n");
+		return ret;
+	}
+
 	ci_enable_auto_throttle_source(rdev, RADEON_DPM_AUTO_THROTTLE_SRC_THERMAL, true);
 
+	ret = ci_enable_thermal_based_sclk_dpm(rdev, true);
+	if (ret) {
+		DRM_ERROR("ci_enable_thermal_based_sclk_dpm failed\n");
+		return ret;
+	}
+
+	ci_thermal_start_thermal_controller(rdev);
+
 	ci_update_current_ps(rdev, boot_ps);
 
 	return 0;
 }
 
-int ci_dpm_late_enable(struct radeon_device *rdev)
+static int ci_set_temperature_range(struct radeon_device *rdev)
 {
 	int ret;
 
-	if (rdev->irq.installed &&
-	    r600_is_internal_thermal_sensor(rdev->pm.int_thermal_type)) {
-#if 0
-		PPSMC_Result result;
-#endif
-		ret = ci_set_thermal_temperature_range(rdev, R600_TEMP_RANGE_MIN, R600_TEMP_RANGE_MAX);
-		if (ret) {
-			DRM_ERROR("ci_set_thermal_temperature_range failed\n");
-			return ret;
-		}
-		rdev->irq.dpm_thermal = true;
-		radeon_irq_set(rdev);
-#if 0
-		result = ci_send_msg_to_smc(rdev, PPSMC_MSG_EnableThermalInterrupt);
+	ret = ci_thermal_enable_alert(rdev, false);
+	if (ret)
+		return ret;
+	ret = ci_thermal_set_temperature_range(rdev, R600_TEMP_RANGE_MIN, R600_TEMP_RANGE_MAX);
+	if (ret)
+		return ret;
+	ret = ci_thermal_enable_alert(rdev, true);
+	if (ret)
+		return ret;
 
-		if (result != PPSMC_Result_OK)
-			DRM_DEBUG_KMS("Could not enable thermal interrupts.\n");
-#endif
-	}
+	return ret;
+}
+
+int ci_dpm_late_enable(struct radeon_device *rdev)
+{
+	int ret;
+
+	ret = ci_set_temperature_range(rdev);
+	if (ret)
+		return ret;
 
 	ci_dpm_powergate_uvd(rdev, true);
 
@@ -4721,6 +5257,8 @@ void ci_dpm_disable(struct radeon_device *rdev)
 	if (!ci_is_smc_running(rdev))
 		return;
 
+	ci_thermal_stop_thermal_controller(rdev);
+
 	if (pi->thermal_protection)
 		ci_enable_thermal_protection(rdev, false);
 	ci_enable_power_containment(rdev, false);
@@ -4729,12 +5267,13 @@ void ci_dpm_disable(struct radeon_device *rdev)
 	ci_enable_spread_spectrum(rdev, false);
 	ci_enable_auto_throttle_source(rdev, RADEON_DPM_AUTO_THROTTLE_SRC_THERMAL, false);
 	ci_stop_dpm(rdev);
-	ci_enable_ds_master_switch(rdev, true);
+	ci_enable_ds_master_switch(rdev, false);
 	ci_enable_ulv(rdev, false);
 	ci_clear_vc(rdev);
 	ci_reset_to_default(rdev);
 	ci_dpm_stop_smc(rdev);
 	ci_force_switch_to_arb_f0(rdev);
+	ci_enable_thermal_based_sclk_dpm(rdev, false);
 
 	ci_update_current_ps(rdev, boot_ps);
 }
@@ -4804,11 +5343,6 @@ int ci_dpm_set_power_state(struct radeon_device *rdev)
 	return 0;
 }
 
-int ci_dpm_power_control_set_level(struct radeon_device *rdev)
-{
-	return ci_power_control_set_level(rdev);
-}
-
 void ci_dpm_reset_asic(struct radeon_device *rdev)
 {
 	ci_set_boot_state(rdev);
@@ -5068,6 +5602,8 @@ void ci_dpm_fini(struct radeon_device *rdev)
 int ci_dpm_init(struct radeon_device *rdev)
 {
 	int index = GetIndexIntoMasterTable(DATA, ASIC_InternalSS_Info);
+	SMU7_Discrete_DpmTable  *dpm_table;
+	struct radeon_gpio_rec gpio;
 	u16 data_offset, size;
 	u8 frev, crev;
 	struct ci_power_info *pi;
@@ -5137,6 +5673,7 @@ int ci_dpm_init(struct radeon_device *rdev)
 	pi->sclk_dpm_key_disabled = 0;
 	pi->mclk_dpm_key_disabled = 0;
 	pi->pcie_dpm_key_disabled = 0;
+	pi->thermal_sclk_dpm_enabled = 0;
 
 	/* mclk dpm is unstable on some R7 260X cards with the old mc ucode */
 	if ((rdev->pdev->device == 0x6658) &&
@@ -5201,6 +5738,55 @@ int ci_dpm_init(struct radeon_device *rdev)
 
 	pi->uvd_enabled = false;
 
+	dpm_table = &pi->smc_state_table;
+
+	gpio = radeon_atombios_lookup_gpio(rdev, VDDC_VRHOT_GPIO_PINID);
+	if (gpio.valid) {
+		dpm_table->VRHotGpio = gpio.shift;
+		rdev->pm.dpm.platform_caps |= ATOM_PP_PLATFORM_CAP_REGULATOR_HOT;
+	} else {
+		dpm_table->VRHotGpio = CISLANDS_UNUSED_GPIO_PIN;
+		rdev->pm.dpm.platform_caps &= ~ATOM_PP_PLATFORM_CAP_REGULATOR_HOT;
+	}
+
+	gpio = radeon_atombios_lookup_gpio(rdev, PP_AC_DC_SWITCH_GPIO_PINID);
+	if (gpio.valid) {
+		dpm_table->AcDcGpio = gpio.shift;
+		rdev->pm.dpm.platform_caps |= ATOM_PP_PLATFORM_CAP_HARDWAREDC;
+	} else {
+		dpm_table->AcDcGpio = CISLANDS_UNUSED_GPIO_PIN;
+		rdev->pm.dpm.platform_caps &= ~ATOM_PP_PLATFORM_CAP_HARDWAREDC;
+	}
+
+	gpio = radeon_atombios_lookup_gpio(rdev, VDDC_PCC_GPIO_PINID);
+	if (gpio.valid) {
+		u32 tmp = RREG32_SMC(CNB_PWRMGT_CNTL);
+
+		switch (gpio.shift) {
+		case 0:
+			tmp &= ~GNB_SLOW_MODE_MASK;
+			tmp |= GNB_SLOW_MODE(1);
+			break;
+		case 1:
+			tmp &= ~GNB_SLOW_MODE_MASK;
+			tmp |= GNB_SLOW_MODE(2);
+			break;
+		case 2:
+			tmp |= GNB_SLOW;
+			break;
+		case 3:
+			tmp |= FORCE_NB_PS1;
+			break;
+		case 4:
+			tmp |= DPM_ENABLED;
+			break;
+		default:
+			DRM_ERROR("Invalid PCC GPIO: %u!\n", gpio.shift);
+			break;
+		}
+		WREG32_SMC(CNB_PWRMGT_CNTL, tmp);
+	}
+
 	pi->voltage_control = CISLANDS_VOLTAGE_CONTROL_NONE;
 	pi->vddci_control = CISLANDS_VOLTAGE_CONTROL_NONE;
 	pi->mvdd_control = CISLANDS_VOLTAGE_CONTROL_NONE;
@@ -5262,6 +5848,8 @@ int ci_dpm_init(struct radeon_device *rdev)
 		rdev->pm.dpm.dyn_state.max_clock_voltage_on_dc =
 			rdev->pm.dpm.dyn_state.max_clock_voltage_on_ac;
 
+	pi->fan_ctrl_is_in_default_mode = true;
+
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/radeon/ci_dpm.h b/drivers/gpu/drm/radeon/ci_dpm.h
index 93bbed977ffb..84e3d3bcf9f3 100644
--- a/drivers/gpu/drm/radeon/ci_dpm.h
+++ b/drivers/gpu/drm/radeon/ci_dpm.h
@@ -33,6 +33,8 @@
 
 #define CISLANDS_MAX_HARDWARE_POWERLEVELS 2
 
+#define CISLANDS_UNUSED_GPIO_PIN 0x7F
+
 struct ci_pl {
 	u32 mclk;
 	u32 sclk;
@@ -237,6 +239,7 @@ struct ci_power_info {
 	u32 sclk_dpm_key_disabled;
 	u32 mclk_dpm_key_disabled;
 	u32 pcie_dpm_key_disabled;
+	u32 thermal_sclk_dpm_enabled;
 	struct ci_pcie_perf_range pcie_gen_performance;
 	struct ci_pcie_perf_range pcie_lane_performance;
 	struct ci_pcie_perf_range pcie_gen_powersaving;
@@ -264,6 +267,7 @@ struct ci_power_info {
 	bool caps_automatic_dc_transition;
 	bool caps_sclk_throttle_low_notification;
 	bool caps_dynamic_ac_timing;
+	bool caps_od_fuzzy_fan_control_support;
 	/* flags */
 	bool thermal_protection;
 	bool pcie_performance_request;
@@ -285,6 +289,10 @@ struct ci_power_info {
 	struct ci_ps current_ps;
 	struct radeon_ps requested_rps;
 	struct ci_ps requested_ps;
+	/* fan control */
+	bool fan_ctrl_is_in_default_mode;
+	u32 t_min;
+	u32 fan_ctrl_default_mode;
 };
 
 #define CISLANDS_VOLTAGE_CONTROL_NONE                   0x0
diff --git a/drivers/gpu/drm/radeon/ci_smc.c b/drivers/gpu/drm/radeon/ci_smc.c
index b630edc2fd0c..e78bcad7a43e 100644
--- a/drivers/gpu/drm/radeon/ci_smc.c
+++ b/drivers/gpu/drm/radeon/ci_smc.c
@@ -129,7 +129,7 @@ void ci_reset_smc(struct radeon_device *rdev)
 
 int ci_program_jump_on_start(struct radeon_device *rdev)
 {
-	static u8 data[] = { 0xE0, 0x00, 0x80, 0x40 };
+	static const u8 data[] = { 0xE0, 0x00, 0x80, 0x40 };
 
 	return ci_copy_bytes_to_smc(rdev, 0x0, data, 4, sizeof(data)+1);
 }
diff --git a/drivers/gpu/drm/radeon/cik.c b/drivers/gpu/drm/radeon/cik.c
index 89c01fa6dd8e..6dcde3798b45 100644
--- a/drivers/gpu/drm/radeon/cik.c
+++ b/drivers/gpu/drm/radeon/cik.c
@@ -32,6 +32,7 @@
 #include "cik_blit_shaders.h"
 #include "radeon_ucode.h"
 #include "clearstate_ci.h"
+#include "radeon_kfd.h"
 
 MODULE_FIRMWARE("radeon/BONAIRE_pfp.bin");
 MODULE_FIRMWARE("radeon/BONAIRE_me.bin");
@@ -1563,6 +1564,8 @@ static const u32 godavari_golden_registers[] =
 
 static void cik_init_golden_registers(struct radeon_device *rdev)
 {
+	/* Some of the registers might be dependent on GRBM_GFX_INDEX */
+	mutex_lock(&rdev->grbm_idx_mutex);
 	switch (rdev->family) {
 	case CHIP_BONAIRE:
 		radeon_program_register_sequence(rdev,
@@ -1637,6 +1640,7 @@ static void cik_init_golden_registers(struct radeon_device *rdev)
 	default:
 		break;
 	}
+	mutex_unlock(&rdev->grbm_idx_mutex);
 }
 
 /**
@@ -1806,7 +1810,7 @@ int ci_mc_load_microcode(struct radeon_device *rdev)
 {
 	const __be32 *fw_data = NULL;
 	const __le32 *new_fw_data = NULL;
-	u32 running, blackout = 0;
+	u32 running, blackout = 0, tmp;
 	u32 *io_mc_regs = NULL;
 	const __le32 *new_io_mc_regs = NULL;
 	int i, regs_size, ucode_size;
@@ -1866,6 +1870,15 @@ int ci_mc_load_microcode(struct radeon_device *rdev)
 				WREG32(MC_SEQ_IO_DEBUG_DATA, io_mc_regs[(i << 1) + 1]);
 			}
 		}
+
+		tmp = RREG32(MC_SEQ_MISC0);
+		if ((rdev->pdev->device == 0x6649) && ((tmp & 0xff00) == 0x5600)) {
+			WREG32(MC_SEQ_IO_DEBUG_INDEX, 5);
+			WREG32(MC_SEQ_IO_DEBUG_DATA, 0x00000023);
+			WREG32(MC_SEQ_IO_DEBUG_INDEX, 9);
+			WREG32(MC_SEQ_IO_DEBUG_DATA, 0x000001f0);
+		}
+
 		/* load the MC ucode */
 		for (i = 0; i < ucode_size; i++) {
 			if (rdev->new_fw)
@@ -3419,6 +3432,7 @@ static void cik_setup_rb(struct radeon_device *rdev,
 	u32 disabled_rbs = 0;
 	u32 enabled_rbs = 0;
 
+	mutex_lock(&rdev->grbm_idx_mutex);
 	for (i = 0; i < se_num; i++) {
 		for (j = 0; j < sh_per_se; j++) {
 			cik_select_se_sh(rdev, i, j);
@@ -3430,6 +3444,7 @@ static void cik_setup_rb(struct radeon_device *rdev,
 		}
 	}
 	cik_select_se_sh(rdev, 0xffffffff, 0xffffffff);
+	mutex_unlock(&rdev->grbm_idx_mutex);
 
 	mask = 1;
 	for (i = 0; i < max_rb_num_per_se * se_num; i++) {
@@ -3440,6 +3455,7 @@ static void cik_setup_rb(struct radeon_device *rdev,
 
 	rdev->config.cik.backend_enable_mask = enabled_rbs;
 
+	mutex_lock(&rdev->grbm_idx_mutex);
 	for (i = 0; i < se_num; i++) {
 		cik_select_se_sh(rdev, i, 0xffffffff);
 		data = 0;
@@ -3467,6 +3483,7 @@ static void cik_setup_rb(struct radeon_device *rdev,
 		WREG32(PA_SC_RASTER_CONFIG, data);
 	}
 	cik_select_se_sh(rdev, 0xffffffff, 0xffffffff);
+	mutex_unlock(&rdev->grbm_idx_mutex);
 }
 
 /**
@@ -3684,6 +3701,12 @@ static void cik_gpu_init(struct radeon_device *rdev)
 	/* set HW defaults for 3D engine */
 	WREG32(CP_MEQ_THRESHOLDS, MEQ1_START(0x30) | MEQ2_START(0x60));
 
+	mutex_lock(&rdev->grbm_idx_mutex);
+	/*
+	 * making sure that the following register writes will be broadcasted
+	 * to all the shaders
+	 */
+	cik_select_se_sh(rdev, 0xffffffff, 0xffffffff);
 	WREG32(SX_DEBUG_1, 0x20);
 
 	WREG32(TA_CNTL_AUX, 0x00010000);
@@ -3739,6 +3762,7 @@ static void cik_gpu_init(struct radeon_device *rdev)
 
 	WREG32(PA_CL_ENHANCE, CLIP_VTX_REORDER_ENA | NUM_CLIP_SEQ(3));
 	WREG32(PA_SC_ENHANCE, ENABLE_PA_SC_OUT_OF_ORDER);
+	mutex_unlock(&rdev->grbm_idx_mutex);
 
 	udelay(50);
 }
@@ -3970,31 +3994,27 @@ struct radeon_fence *cik_copy_cpdma(struct radeon_device *rdev,
 				    unsigned num_gpu_pages,
 				    struct reservation_object *resv)
 {
-	struct radeon_semaphore *sem = NULL;
 	struct radeon_fence *fence;
+	struct radeon_sync sync;
 	int ring_index = rdev->asic->copy.blit_ring_index;
 	struct radeon_ring *ring = &rdev->ring[ring_index];
 	u32 size_in_bytes, cur_size_in_bytes, control;
 	int i, num_loops;
 	int r = 0;
 
-	r = radeon_semaphore_create(rdev, &sem);
-	if (r) {
-		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		return ERR_PTR(r);
-	}
+	radeon_sync_create(&sync);
 
 	size_in_bytes = (num_gpu_pages << RADEON_GPU_PAGE_SHIFT);
 	num_loops = DIV_ROUND_UP(size_in_bytes, 0x1fffff);
 	r = radeon_ring_lock(rdev, ring, num_loops * 7 + 18);
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		radeon_semaphore_free(rdev, &sem, NULL);
+		radeon_sync_free(rdev, &sync, NULL);
 		return ERR_PTR(r);
 	}
 
-	radeon_semaphore_sync_resv(rdev, sem, resv, false);
-	radeon_semaphore_sync_rings(rdev, sem, ring->idx);
+	radeon_sync_resv(rdev, &sync, resv, false);
+	radeon_sync_rings(rdev, &sync, ring->idx);
 
 	for (i = 0; i < num_loops; i++) {
 		cur_size_in_bytes = size_in_bytes;
@@ -4018,12 +4038,12 @@ struct radeon_fence *cik_copy_cpdma(struct radeon_device *rdev,
 	r = radeon_fence_emit(rdev, &fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
-		radeon_semaphore_free(rdev, &sem, NULL);
+		radeon_sync_free(rdev, &sync, NULL);
 		return ERR_PTR(r);
 	}
 
 	radeon_ring_unlock_commit(rdev, ring, false);
-	radeon_semaphore_free(rdev, &sem, fence);
+	radeon_sync_free(rdev, &sync, fence);
 
 	return fence;
 }
@@ -4046,6 +4066,7 @@ struct radeon_fence *cik_copy_cpdma(struct radeon_device *rdev,
 void cik_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 {
 	struct radeon_ring *ring = &rdev->ring[ib->ring];
+	unsigned vm_id = ib->vm ? ib->vm->ids[ib->ring].id : 0;
 	u32 header, control = INDIRECT_BUFFER_VALID;
 
 	if (ib->is_const_ib) {
@@ -4074,8 +4095,7 @@ void cik_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 		header = PACKET3(PACKET3_INDIRECT_BUFFER, 2);
 	}
 
-	control |= ib->length_dw |
-		(ib->vm ? (ib->vm->id << 24) : 0);
+	control |= ib->length_dw | (vm_id << 24);
 
 	radeon_ring_write(ring, header);
 	radeon_ring_write(ring,
@@ -4675,12 +4695,11 @@ static int cik_mec_init(struct radeon_device *rdev)
 	/*
 	 * KV:    2 MEC, 4 Pipes/MEC, 8 Queues/Pipe - 64 Queues total
 	 * CI/KB: 1 MEC, 4 Pipes/MEC, 8 Queues/Pipe - 32 Queues total
+	 * Nonetheless, we assign only 1 pipe because all other pipes will
+	 * be handled by KFD
 	 */
-	if (rdev->family == CHIP_KAVERI)
-		rdev->mec.num_mec = 2;
-	else
-		rdev->mec.num_mec = 1;
-	rdev->mec.num_pipe = 4;
+	rdev->mec.num_mec = 1;
+	rdev->mec.num_pipe = 1;
 	rdev->mec.num_queue = rdev->mec.num_mec * rdev->mec.num_pipe * 8;
 
 	if (rdev->mec.hpd_eop_obj == NULL) {
@@ -4822,28 +4841,24 @@ static int cik_cp_compute_resume(struct radeon_device *rdev)
 
 	/* init the pipes */
 	mutex_lock(&rdev->srbm_mutex);
-	for (i = 0; i < (rdev->mec.num_pipe * rdev->mec.num_mec); i++) {
-		int me = (i < 4) ? 1 : 2;
-		int pipe = (i < 4) ? i : (i - 4);
 
-		eop_gpu_addr = rdev->mec.hpd_eop_gpu_addr + (i * MEC_HPD_SIZE * 2);
+	eop_gpu_addr = rdev->mec.hpd_eop_gpu_addr;
 
-		cik_srbm_select(rdev, me, pipe, 0, 0);
+	cik_srbm_select(rdev, 0, 0, 0, 0);
 
-		/* write the EOP addr */
-		WREG32(CP_HPD_EOP_BASE_ADDR, eop_gpu_addr >> 8);
-		WREG32(CP_HPD_EOP_BASE_ADDR_HI, upper_32_bits(eop_gpu_addr) >> 8);
+	/* write the EOP addr */
+	WREG32(CP_HPD_EOP_BASE_ADDR, eop_gpu_addr >> 8);
+	WREG32(CP_HPD_EOP_BASE_ADDR_HI, upper_32_bits(eop_gpu_addr) >> 8);
 
-		/* set the VMID assigned */
-		WREG32(CP_HPD_EOP_VMID, 0);
+	/* set the VMID assigned */
+	WREG32(CP_HPD_EOP_VMID, 0);
+
+	/* set the EOP size, register value is 2^(EOP_SIZE+1) dwords */
+	tmp = RREG32(CP_HPD_EOP_CONTROL);
+	tmp &= ~EOP_SIZE_MASK;
+	tmp |= order_base_2(MEC_HPD_SIZE / 8);
+	WREG32(CP_HPD_EOP_CONTROL, tmp);
 
-		/* set the EOP size, register value is 2^(EOP_SIZE+1) dwords */
-		tmp = RREG32(CP_HPD_EOP_CONTROL);
-		tmp &= ~EOP_SIZE_MASK;
-		tmp |= order_base_2(MEC_HPD_SIZE / 8);
-		WREG32(CP_HPD_EOP_CONTROL, tmp);
-	}
-	cik_srbm_select(rdev, 0, 0, 0, 0);
 	mutex_unlock(&rdev->srbm_mutex);
 
 	/* init the queues.  Just two for now. */
@@ -5897,8 +5912,13 @@ int cik_ib_parse(struct radeon_device *rdev, struct radeon_ib *ib)
  */
 int cik_vm_init(struct radeon_device *rdev)
 {
-	/* number of VMs */
-	rdev->vm_manager.nvm = 16;
+	/*
+	 * number of VMs
+	 * VMID 0 is reserved for System
+	 * radeon graphics/compute will use VMIDs 1-7
+	 * amdkfd will use VMIDs 8-15
+	 */
+	rdev->vm_manager.nvm = RADEON_NUM_OF_VMIDS;
 	/* base offset of vram pages */
 	if (rdev->flags & RADEON_IS_IGP) {
 		u64 tmp = RREG32(MC_VM_FB_OFFSET);
@@ -5958,26 +5978,23 @@ static void cik_vm_decode_fault(struct radeon_device *rdev,
  * Update the page table base and flush the VM TLB
  * using the CP (CIK).
  */
-void cik_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
+void cik_vm_flush(struct radeon_device *rdev, struct radeon_ring *ring,
+		  unsigned vm_id, uint64_t pd_addr)
 {
-	struct radeon_ring *ring = &rdev->ring[ridx];
-	int usepfp = (ridx == RADEON_RING_TYPE_GFX_INDEX);
-
-	if (vm == NULL)
-		return;
+	int usepfp = (ring->idx == RADEON_RING_TYPE_GFX_INDEX);
 
 	radeon_ring_write(ring, PACKET3(PACKET3_WRITE_DATA, 3));
 	radeon_ring_write(ring, (WRITE_DATA_ENGINE_SEL(usepfp) |
 				 WRITE_DATA_DST_SEL(0)));
-	if (vm->id < 8) {
+	if (vm_id < 8) {
 		radeon_ring_write(ring,
-				  (VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (vm->id << 2)) >> 2);
+				  (VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (vm_id << 2)) >> 2);
 	} else {
 		radeon_ring_write(ring,
-				  (VM_CONTEXT8_PAGE_TABLE_BASE_ADDR + ((vm->id - 8) << 2)) >> 2);
+				  (VM_CONTEXT8_PAGE_TABLE_BASE_ADDR + ((vm_id - 8) << 2)) >> 2);
 	}
 	radeon_ring_write(ring, 0);
-	radeon_ring_write(ring, vm->pd_gpu_addr >> 12);
+	radeon_ring_write(ring, pd_addr >> 12);
 
 	/* update SH_MEM_* regs */
 	radeon_ring_write(ring, PACKET3(PACKET3_WRITE_DATA, 3));
@@ -5985,7 +6002,7 @@ void cik_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
 				 WRITE_DATA_DST_SEL(0)));
 	radeon_ring_write(ring, SRBM_GFX_CNTL >> 2);
 	radeon_ring_write(ring, 0);
-	radeon_ring_write(ring, VMID(vm->id));
+	radeon_ring_write(ring, VMID(vm_id));
 
 	radeon_ring_write(ring, PACKET3(PACKET3_WRITE_DATA, 6));
 	radeon_ring_write(ring, (WRITE_DATA_ENGINE_SEL(usepfp) |
@@ -6006,7 +6023,7 @@ void cik_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
 	radeon_ring_write(ring, VMID(0));
 
 	/* HDP flush */
-	cik_hdp_flush_cp_ring_emit(rdev, ridx);
+	cik_hdp_flush_cp_ring_emit(rdev, ring->idx);
 
 	/* bits 0-15 are the VM contexts0-15 */
 	radeon_ring_write(ring, PACKET3(PACKET3_WRITE_DATA, 3));
@@ -6014,7 +6031,7 @@ void cik_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
 				 WRITE_DATA_DST_SEL(0)));
 	radeon_ring_write(ring, VM_INVALIDATE_REQUEST >> 2);
 	radeon_ring_write(ring, 0);
-	radeon_ring_write(ring, 1 << vm->id);
+	radeon_ring_write(ring, 1 << vm_id);
 
 	/* compute doesn't have PFP */
 	if (usepfp) {
@@ -6059,6 +6076,7 @@ static void cik_wait_for_rlc_serdes(struct radeon_device *rdev)
 	u32 i, j, k;
 	u32 mask;
 
+	mutex_lock(&rdev->grbm_idx_mutex);
 	for (i = 0; i < rdev->config.cik.max_shader_engines; i++) {
 		for (j = 0; j < rdev->config.cik.max_sh_per_se; j++) {
 			cik_select_se_sh(rdev, i, j);
@@ -6070,6 +6088,7 @@ static void cik_wait_for_rlc_serdes(struct radeon_device *rdev)
 		}
 	}
 	cik_select_se_sh(rdev, 0xffffffff, 0xffffffff);
+	mutex_unlock(&rdev->grbm_idx_mutex);
 
 	mask = SE_MASTER_BUSY_MASK | GC_MASTER_BUSY | TC0_MASTER_BUSY | TC1_MASTER_BUSY;
 	for (k = 0; k < rdev->usec_timeout; k++) {
@@ -6204,10 +6223,12 @@ static int cik_rlc_resume(struct radeon_device *rdev)
 	WREG32(RLC_LB_CNTR_INIT, 0);
 	WREG32(RLC_LB_CNTR_MAX, 0x00008000);
 
+	mutex_lock(&rdev->grbm_idx_mutex);
 	cik_select_se_sh(rdev, 0xffffffff, 0xffffffff);
 	WREG32(RLC_LB_INIT_CU_MASK, 0xffffffff);
 	WREG32(RLC_LB_PARAMS, 0x00600408);
 	WREG32(RLC_LB_CNTL, 0x80000004);
+	mutex_unlock(&rdev->grbm_idx_mutex);
 
 	WREG32(RLC_MC_CNTL, 0);
 	WREG32(RLC_UCODE_CNTL, 0);
@@ -6274,11 +6295,13 @@ static void cik_enable_cgcg(struct radeon_device *rdev, bool enable)
 
 		tmp = cik_halt_rlc(rdev);
 
+		mutex_lock(&rdev->grbm_idx_mutex);
 		cik_select_se_sh(rdev, 0xffffffff, 0xffffffff);
 		WREG32(RLC_SERDES_WR_CU_MASTER_MASK, 0xffffffff);
 		WREG32(RLC_SERDES_WR_NONCU_MASTER_MASK, 0xffffffff);
 		tmp2 = BPM_ADDR_MASK | CGCG_OVERRIDE_0 | CGLS_ENABLE;
 		WREG32(RLC_SERDES_WR_CTRL, tmp2);
+		mutex_unlock(&rdev->grbm_idx_mutex);
 
 		cik_update_rlc(rdev, tmp);
 
@@ -6314,17 +6337,20 @@ static void cik_enable_mgcg(struct radeon_device *rdev, bool enable)
 		}
 
 		orig = data = RREG32(RLC_CGTT_MGCG_OVERRIDE);
+		data |= 0x00000001;
 		data &= 0xfffffffd;
 		if (orig != data)
 			WREG32(RLC_CGTT_MGCG_OVERRIDE, data);
 
 		tmp = cik_halt_rlc(rdev);
 
+		mutex_lock(&rdev->grbm_idx_mutex);
 		cik_select_se_sh(rdev, 0xffffffff, 0xffffffff);
 		WREG32(RLC_SERDES_WR_CU_MASTER_MASK, 0xffffffff);
 		WREG32(RLC_SERDES_WR_NONCU_MASTER_MASK, 0xffffffff);
 		data = BPM_ADDR_MASK | MGCG_OVERRIDE_0;
 		WREG32(RLC_SERDES_WR_CTRL, data);
+		mutex_unlock(&rdev->grbm_idx_mutex);
 
 		cik_update_rlc(rdev, tmp);
 
@@ -6345,7 +6371,7 @@ static void cik_enable_mgcg(struct radeon_device *rdev, bool enable)
 		}
 	} else {
 		orig = data = RREG32(RLC_CGTT_MGCG_OVERRIDE);
-		data |= 0x00000002;
+		data |= 0x00000003;
 		if (orig != data)
 			WREG32(RLC_CGTT_MGCG_OVERRIDE, data);
 
@@ -6368,11 +6394,13 @@ static void cik_enable_mgcg(struct radeon_device *rdev, bool enable)
 
 		tmp = cik_halt_rlc(rdev);
 
+		mutex_lock(&rdev->grbm_idx_mutex);
 		cik_select_se_sh(rdev, 0xffffffff, 0xffffffff);
 		WREG32(RLC_SERDES_WR_CU_MASTER_MASK, 0xffffffff);
 		WREG32(RLC_SERDES_WR_NONCU_MASTER_MASK, 0xffffffff);
 		data = BPM_ADDR_MASK | MGCG_OVERRIDE_1;
 		WREG32(RLC_SERDES_WR_CTRL, data);
+		mutex_unlock(&rdev->grbm_idx_mutex);
 
 		cik_update_rlc(rdev, tmp);
 	}
@@ -6801,10 +6829,12 @@ static u32 cik_get_cu_active_bitmap(struct radeon_device *rdev, u32 se, u32 sh)
 	u32 mask = 0, tmp, tmp1;
 	int i;
 
+	mutex_lock(&rdev->grbm_idx_mutex);
 	cik_select_se_sh(rdev, se, sh);
 	tmp = RREG32(CC_GC_SHADER_ARRAY_CONFIG);
 	tmp1 = RREG32(GC_USER_SHADER_ARRAY_CONFIG);
 	cik_select_se_sh(rdev, 0xffffffff, 0xffffffff);
+	mutex_unlock(&rdev->grbm_idx_mutex);
 
 	tmp &= 0xffff0000;
 
@@ -7288,8 +7318,7 @@ static int cik_irq_init(struct radeon_device *rdev)
 int cik_irq_set(struct radeon_device *rdev)
 {
 	u32 cp_int_cntl;
-	u32 cp_m1p0, cp_m1p1, cp_m1p2, cp_m1p3;
-	u32 cp_m2p0, cp_m2p1, cp_m2p2, cp_m2p3;
+	u32 cp_m1p0;
 	u32 crtc1 = 0, crtc2 = 0, crtc3 = 0, crtc4 = 0, crtc5 = 0, crtc6 = 0;
 	u32 hpd1, hpd2, hpd3, hpd4, hpd5, hpd6;
 	u32 grbm_int_cntl = 0;
@@ -7323,13 +7352,6 @@ int cik_irq_set(struct radeon_device *rdev)
 	dma_cntl1 = RREG32(SDMA0_CNTL + SDMA1_REGISTER_OFFSET) & ~TRAP_ENABLE;
 
 	cp_m1p0 = RREG32(CP_ME1_PIPE0_INT_CNTL) & ~TIME_STAMP_INT_ENABLE;
-	cp_m1p1 = RREG32(CP_ME1_PIPE1_INT_CNTL) & ~TIME_STAMP_INT_ENABLE;
-	cp_m1p2 = RREG32(CP_ME1_PIPE2_INT_CNTL) & ~TIME_STAMP_INT_ENABLE;
-	cp_m1p3 = RREG32(CP_ME1_PIPE3_INT_CNTL) & ~TIME_STAMP_INT_ENABLE;
-	cp_m2p0 = RREG32(CP_ME2_PIPE0_INT_CNTL) & ~TIME_STAMP_INT_ENABLE;
-	cp_m2p1 = RREG32(CP_ME2_PIPE1_INT_CNTL) & ~TIME_STAMP_INT_ENABLE;
-	cp_m2p2 = RREG32(CP_ME2_PIPE2_INT_CNTL) & ~TIME_STAMP_INT_ENABLE;
-	cp_m2p3 = RREG32(CP_ME2_PIPE3_INT_CNTL) & ~TIME_STAMP_INT_ENABLE;
 
 	if (rdev->flags & RADEON_IS_IGP)
 		thermal_int = RREG32_SMC(CG_THERMAL_INT_CTRL) &
@@ -7351,33 +7373,6 @@ int cik_irq_set(struct radeon_device *rdev)
 			case 0:
 				cp_m1p0 |= TIME_STAMP_INT_ENABLE;
 				break;
-			case 1:
-				cp_m1p1 |= TIME_STAMP_INT_ENABLE;
-				break;
-			case 2:
-				cp_m1p2 |= TIME_STAMP_INT_ENABLE;
-				break;
-			case 3:
-				cp_m1p2 |= TIME_STAMP_INT_ENABLE;
-				break;
-			default:
-				DRM_DEBUG("si_irq_set: sw int cp1 invalid pipe %d\n", ring->pipe);
-				break;
-			}
-		} else if (ring->me == 2) {
-			switch (ring->pipe) {
-			case 0:
-				cp_m2p0 |= TIME_STAMP_INT_ENABLE;
-				break;
-			case 1:
-				cp_m2p1 |= TIME_STAMP_INT_ENABLE;
-				break;
-			case 2:
-				cp_m2p2 |= TIME_STAMP_INT_ENABLE;
-				break;
-			case 3:
-				cp_m2p2 |= TIME_STAMP_INT_ENABLE;
-				break;
 			default:
 				DRM_DEBUG("si_irq_set: sw int cp1 invalid pipe %d\n", ring->pipe);
 				break;
@@ -7394,33 +7389,6 @@ int cik_irq_set(struct radeon_device *rdev)
 			case 0:
 				cp_m1p0 |= TIME_STAMP_INT_ENABLE;
 				break;
-			case 1:
-				cp_m1p1 |= TIME_STAMP_INT_ENABLE;
-				break;
-			case 2:
-				cp_m1p2 |= TIME_STAMP_INT_ENABLE;
-				break;
-			case 3:
-				cp_m1p2 |= TIME_STAMP_INT_ENABLE;
-				break;
-			default:
-				DRM_DEBUG("si_irq_set: sw int cp2 invalid pipe %d\n", ring->pipe);
-				break;
-			}
-		} else if (ring->me == 2) {
-			switch (ring->pipe) {
-			case 0:
-				cp_m2p0 |= TIME_STAMP_INT_ENABLE;
-				break;
-			case 1:
-				cp_m2p1 |= TIME_STAMP_INT_ENABLE;
-				break;
-			case 2:
-				cp_m2p2 |= TIME_STAMP_INT_ENABLE;
-				break;
-			case 3:
-				cp_m2p2 |= TIME_STAMP_INT_ENABLE;
-				break;
 			default:
 				DRM_DEBUG("si_irq_set: sw int cp2 invalid pipe %d\n", ring->pipe);
 				break;
@@ -7509,13 +7477,6 @@ int cik_irq_set(struct radeon_device *rdev)
 	WREG32(SDMA0_CNTL + SDMA1_REGISTER_OFFSET, dma_cntl1);
 
 	WREG32(CP_ME1_PIPE0_INT_CNTL, cp_m1p0);
-	WREG32(CP_ME1_PIPE1_INT_CNTL, cp_m1p1);
-	WREG32(CP_ME1_PIPE2_INT_CNTL, cp_m1p2);
-	WREG32(CP_ME1_PIPE3_INT_CNTL, cp_m1p3);
-	WREG32(CP_ME2_PIPE0_INT_CNTL, cp_m2p0);
-	WREG32(CP_ME2_PIPE1_INT_CNTL, cp_m2p1);
-	WREG32(CP_ME2_PIPE2_INT_CNTL, cp_m2p2);
-	WREG32(CP_ME2_PIPE3_INT_CNTL, cp_m2p3);
 
 	WREG32(GRBM_INT_CNTL, grbm_int_cntl);
 
@@ -7832,6 +7793,10 @@ restart_ih:
 	while (rptr != wptr) {
 		/* wptr/rptr are in bytes! */
 		ring_index = rptr / 4;
+
+		radeon_kfd_interrupt(rdev,
+				(const void *) &rdev->ih.ring[ring_index]);
+
 		src_id =  le32_to_cpu(rdev->ih.ring[ring_index]) & 0xff;
 		src_data = le32_to_cpu(rdev->ih.ring[ring_index + 1]) & 0xfffffff;
 		ring_id = le32_to_cpu(rdev->ih.ring[ring_index + 2]) & 0xff;
@@ -8521,6 +8486,10 @@ static int cik_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_kfd_resume(rdev);
+	if (r)
+		return r;
+
 	return 0;
 }
 
@@ -8569,6 +8538,7 @@ int cik_resume(struct radeon_device *rdev)
  */
 int cik_suspend(struct radeon_device *rdev)
 {
+	radeon_kfd_suspend(rdev);
 	radeon_pm_suspend(rdev);
 	dce6_audio_fini(rdev);
 	radeon_vm_manager_fini(rdev);
diff --git a/drivers/gpu/drm/radeon/cik_reg.h b/drivers/gpu/drm/radeon/cik_reg.h
index ca1bb6133580..79c45e8a536b 100644
--- a/drivers/gpu/drm/radeon/cik_reg.h
+++ b/drivers/gpu/drm/radeon/cik_reg.h
@@ -147,4 +147,140 @@
 
 #define CIK_LB_DESKTOP_HEIGHT                     0x6b0c
 
+#define CP_HQD_IQ_RPTR					0xC970u
+#define AQL_ENABLE					(1U << 0)
+
+#define IDLE					(1 << 2)
+
+struct cik_mqd {
+	uint32_t header;
+	uint32_t compute_dispatch_initiator;
+	uint32_t compute_dim_x;
+	uint32_t compute_dim_y;
+	uint32_t compute_dim_z;
+	uint32_t compute_start_x;
+	uint32_t compute_start_y;
+	uint32_t compute_start_z;
+	uint32_t compute_num_thread_x;
+	uint32_t compute_num_thread_y;
+	uint32_t compute_num_thread_z;
+	uint32_t compute_pipelinestat_enable;
+	uint32_t compute_perfcount_enable;
+	uint32_t compute_pgm_lo;
+	uint32_t compute_pgm_hi;
+	uint32_t compute_tba_lo;
+	uint32_t compute_tba_hi;
+	uint32_t compute_tma_lo;
+	uint32_t compute_tma_hi;
+	uint32_t compute_pgm_rsrc1;
+	uint32_t compute_pgm_rsrc2;
+	uint32_t compute_vmid;
+	uint32_t compute_resource_limits;
+	uint32_t compute_static_thread_mgmt_se0;
+	uint32_t compute_static_thread_mgmt_se1;
+	uint32_t compute_tmpring_size;
+	uint32_t compute_static_thread_mgmt_se2;
+	uint32_t compute_static_thread_mgmt_se3;
+	uint32_t compute_restart_x;
+	uint32_t compute_restart_y;
+	uint32_t compute_restart_z;
+	uint32_t compute_thread_trace_enable;
+	uint32_t compute_misc_reserved;
+	uint32_t compute_user_data_0;
+	uint32_t compute_user_data_1;
+	uint32_t compute_user_data_2;
+	uint32_t compute_user_data_3;
+	uint32_t compute_user_data_4;
+	uint32_t compute_user_data_5;
+	uint32_t compute_user_data_6;
+	uint32_t compute_user_data_7;
+	uint32_t compute_user_data_8;
+	uint32_t compute_user_data_9;
+	uint32_t compute_user_data_10;
+	uint32_t compute_user_data_11;
+	uint32_t compute_user_data_12;
+	uint32_t compute_user_data_13;
+	uint32_t compute_user_data_14;
+	uint32_t compute_user_data_15;
+	uint32_t cp_compute_csinvoc_count_lo;
+	uint32_t cp_compute_csinvoc_count_hi;
+	uint32_t cp_mqd_base_addr_lo;
+	uint32_t cp_mqd_base_addr_hi;
+	uint32_t cp_hqd_active;
+	uint32_t cp_hqd_vmid;
+	uint32_t cp_hqd_persistent_state;
+	uint32_t cp_hqd_pipe_priority;
+	uint32_t cp_hqd_queue_priority;
+	uint32_t cp_hqd_quantum;
+	uint32_t cp_hqd_pq_base_lo;
+	uint32_t cp_hqd_pq_base_hi;
+	uint32_t cp_hqd_pq_rptr;
+	uint32_t cp_hqd_pq_rptr_report_addr_lo;
+	uint32_t cp_hqd_pq_rptr_report_addr_hi;
+	uint32_t cp_hqd_pq_wptr_poll_addr_lo;
+	uint32_t cp_hqd_pq_wptr_poll_addr_hi;
+	uint32_t cp_hqd_pq_doorbell_control;
+	uint32_t cp_hqd_pq_wptr;
+	uint32_t cp_hqd_pq_control;
+	uint32_t cp_hqd_ib_base_addr_lo;
+	uint32_t cp_hqd_ib_base_addr_hi;
+	uint32_t cp_hqd_ib_rptr;
+	uint32_t cp_hqd_ib_control;
+	uint32_t cp_hqd_iq_timer;
+	uint32_t cp_hqd_iq_rptr;
+	uint32_t cp_hqd_dequeue_request;
+	uint32_t cp_hqd_dma_offload;
+	uint32_t cp_hqd_sema_cmd;
+	uint32_t cp_hqd_msg_type;
+	uint32_t cp_hqd_atomic0_preop_lo;
+	uint32_t cp_hqd_atomic0_preop_hi;
+	uint32_t cp_hqd_atomic1_preop_lo;
+	uint32_t cp_hqd_atomic1_preop_hi;
+	uint32_t cp_hqd_hq_status0;
+	uint32_t cp_hqd_hq_control0;
+	uint32_t cp_mqd_control;
+	uint32_t cp_mqd_query_time_lo;
+	uint32_t cp_mqd_query_time_hi;
+	uint32_t cp_mqd_connect_start_time_lo;
+	uint32_t cp_mqd_connect_start_time_hi;
+	uint32_t cp_mqd_connect_end_time_lo;
+	uint32_t cp_mqd_connect_end_time_hi;
+	uint32_t cp_mqd_connect_end_wf_count;
+	uint32_t cp_mqd_connect_end_pq_rptr;
+	uint32_t cp_mqd_connect_end_pq_wptr;
+	uint32_t cp_mqd_connect_end_ib_rptr;
+	uint32_t reserved_96;
+	uint32_t reserved_97;
+	uint32_t reserved_98;
+	uint32_t reserved_99;
+	uint32_t iqtimer_pkt_header;
+	uint32_t iqtimer_pkt_dw0;
+	uint32_t iqtimer_pkt_dw1;
+	uint32_t iqtimer_pkt_dw2;
+	uint32_t iqtimer_pkt_dw3;
+	uint32_t iqtimer_pkt_dw4;
+	uint32_t iqtimer_pkt_dw5;
+	uint32_t iqtimer_pkt_dw6;
+	uint32_t reserved_108;
+	uint32_t reserved_109;
+	uint32_t reserved_110;
+	uint32_t reserved_111;
+	uint32_t queue_doorbell_id0;
+	uint32_t queue_doorbell_id1;
+	uint32_t queue_doorbell_id2;
+	uint32_t queue_doorbell_id3;
+	uint32_t queue_doorbell_id4;
+	uint32_t queue_doorbell_id5;
+	uint32_t queue_doorbell_id6;
+	uint32_t queue_doorbell_id7;
+	uint32_t queue_doorbell_id8;
+	uint32_t queue_doorbell_id9;
+	uint32_t queue_doorbell_id10;
+	uint32_t queue_doorbell_id11;
+	uint32_t queue_doorbell_id12;
+	uint32_t queue_doorbell_id13;
+	uint32_t queue_doorbell_id14;
+	uint32_t queue_doorbell_id15;
+};
+
 #endif
diff --git a/drivers/gpu/drm/radeon/cik_sdma.c b/drivers/gpu/drm/radeon/cik_sdma.c
index d748963af08b..dde5c7e29eb2 100644
--- a/drivers/gpu/drm/radeon/cik_sdma.c
+++ b/drivers/gpu/drm/radeon/cik_sdma.c
@@ -134,7 +134,7 @@ void cik_sdma_ring_ib_execute(struct radeon_device *rdev,
 			      struct radeon_ib *ib)
 {
 	struct radeon_ring *ring = &rdev->ring[ib->ring];
-	u32 extra_bits = (ib->vm ? ib->vm->id : 0) & 0xf;
+	u32 extra_bits = (ib->vm ? ib->vm->ids[ib->ring].id : 0) & 0xf;
 
 	if (rdev->wb.enabled) {
 		u32 next_rptr = ring->wptr + 5;
@@ -541,31 +541,27 @@ struct radeon_fence *cik_copy_dma(struct radeon_device *rdev,
 				  unsigned num_gpu_pages,
 				  struct reservation_object *resv)
 {
-	struct radeon_semaphore *sem = NULL;
 	struct radeon_fence *fence;
+	struct radeon_sync sync;
 	int ring_index = rdev->asic->copy.dma_ring_index;
 	struct radeon_ring *ring = &rdev->ring[ring_index];
 	u32 size_in_bytes, cur_size_in_bytes;
 	int i, num_loops;
 	int r = 0;
 
-	r = radeon_semaphore_create(rdev, &sem);
-	if (r) {
-		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		return ERR_PTR(r);
-	}
+	radeon_sync_create(&sync);
 
 	size_in_bytes = (num_gpu_pages << RADEON_GPU_PAGE_SHIFT);
 	num_loops = DIV_ROUND_UP(size_in_bytes, 0x1fffff);
 	r = radeon_ring_lock(rdev, ring, num_loops * 7 + 14);
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		radeon_semaphore_free(rdev, &sem, NULL);
+		radeon_sync_free(rdev, &sync, NULL);
 		return ERR_PTR(r);
 	}
 
-	radeon_semaphore_sync_resv(rdev, sem, resv, false);
-	radeon_semaphore_sync_rings(rdev, sem, ring->idx);
+	radeon_sync_resv(rdev, &sync, resv, false);
+	radeon_sync_rings(rdev, &sync, ring->idx);
 
 	for (i = 0; i < num_loops; i++) {
 		cur_size_in_bytes = size_in_bytes;
@@ -586,12 +582,12 @@ struct radeon_fence *cik_copy_dma(struct radeon_device *rdev,
 	r = radeon_fence_emit(rdev, &fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
-		radeon_semaphore_free(rdev, &sem, NULL);
+		radeon_sync_free(rdev, &sync, NULL);
 		return ERR_PTR(r);
 	}
 
 	radeon_ring_unlock_commit(rdev, ring, false);
-	radeon_semaphore_free(rdev, &sem, fence);
+	radeon_sync_free(rdev, &sync, fence);
 
 	return fence;
 }
@@ -904,25 +900,21 @@ void cik_sdma_vm_pad_ib(struct radeon_ib *ib)
  * Update the page table base and flush the VM TLB
  * using sDMA (CIK).
  */
-void cik_dma_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
+void cik_dma_vm_flush(struct radeon_device *rdev, struct radeon_ring *ring,
+		      unsigned vm_id, uint64_t pd_addr)
 {
-	struct radeon_ring *ring = &rdev->ring[ridx];
-
-	if (vm == NULL)
-		return;
-
 	radeon_ring_write(ring, SDMA_PACKET(SDMA_OPCODE_SRBM_WRITE, 0, 0xf000));
-	if (vm->id < 8) {
-		radeon_ring_write(ring, (VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (vm->id << 2)) >> 2);
+	if (vm_id < 8) {
+		radeon_ring_write(ring, (VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (vm_id << 2)) >> 2);
 	} else {
-		radeon_ring_write(ring, (VM_CONTEXT8_PAGE_TABLE_BASE_ADDR + ((vm->id - 8) << 2)) >> 2);
+		radeon_ring_write(ring, (VM_CONTEXT8_PAGE_TABLE_BASE_ADDR + ((vm_id - 8) << 2)) >> 2);
 	}
-	radeon_ring_write(ring, vm->pd_gpu_addr >> 12);
+	radeon_ring_write(ring, pd_addr >> 12);
 
 	/* update SH_MEM_* regs */
 	radeon_ring_write(ring, SDMA_PACKET(SDMA_OPCODE_SRBM_WRITE, 0, 0xf000));
 	radeon_ring_write(ring, SRBM_GFX_CNTL >> 2);
-	radeon_ring_write(ring, VMID(vm->id));
+	radeon_ring_write(ring, VMID(vm_id));
 
 	radeon_ring_write(ring, SDMA_PACKET(SDMA_OPCODE_SRBM_WRITE, 0, 0xf000));
 	radeon_ring_write(ring, SH_MEM_BASES >> 2);
@@ -945,11 +937,11 @@ void cik_dma_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm
 	radeon_ring_write(ring, VMID(0));
 
 	/* flush HDP */
-	cik_sdma_hdp_flush_ring_emit(rdev, ridx);
+	cik_sdma_hdp_flush_ring_emit(rdev, ring->idx);
 
 	/* flush TLB */
 	radeon_ring_write(ring, SDMA_PACKET(SDMA_OPCODE_SRBM_WRITE, 0, 0xf000));
 	radeon_ring_write(ring, VM_INVALIDATE_REQUEST >> 2);
-	radeon_ring_write(ring, 1 << vm->id);
+	radeon_ring_write(ring, 1 << vm_id);
 }
 
diff --git a/drivers/gpu/drm/radeon/cikd.h b/drivers/gpu/drm/radeon/cikd.h
index 0c6e1b55d968..ba85986febea 100644
--- a/drivers/gpu/drm/radeon/cikd.h
+++ b/drivers/gpu/drm/radeon/cikd.h
@@ -30,6 +30,8 @@
 #define CIK_RB_BITMAP_WIDTH_PER_SH     2
 #define HAWAII_RB_BITMAP_WIDTH_PER_SH  4
 
+#define RADEON_NUM_OF_VMIDS	8
+
 /* DIDT IND registers */
 #define DIDT_SQ_CTRL0                                     0x0
 #       define DIDT_CTRL_EN                               (1 << 0)
@@ -184,7 +186,10 @@
 #define		DIG_THERM_DPM(x)			((x) << 14)
 #define		DIG_THERM_DPM_MASK			0x003FC000
 #define		DIG_THERM_DPM_SHIFT			14
-
+#define	CG_THERMAL_STATUS				0xC0300008
+#define		FDO_PWM_DUTY(x)				((x) << 9)
+#define		FDO_PWM_DUTY_MASK			(0xff << 9)
+#define		FDO_PWM_DUTY_SHIFT			9
 #define	CG_THERMAL_INT					0xC030000C
 #define		CI_DIG_THERM_INTH(x)			((x) << 8)
 #define		CI_DIG_THERM_INTH_MASK			0x0000FF00
@@ -194,7 +199,10 @@
 #define		CI_DIG_THERM_INTL_SHIFT			16
 #define 	THERM_INT_MASK_HIGH			(1 << 24)
 #define 	THERM_INT_MASK_LOW			(1 << 25)
-
+#define	CG_MULT_THERMAL_CTRL				0xC0300010
+#define		TEMP_SEL(x)				((x) << 20)
+#define		TEMP_SEL_MASK				(0xff << 20)
+#define		TEMP_SEL_SHIFT				20
 #define	CG_MULT_THERMAL_STATUS				0xC0300014
 #define		ASIC_MAX_TEMP(x)			((x) << 0)
 #define		ASIC_MAX_TEMP_MASK			0x000001ff
@@ -203,6 +211,36 @@
 #define		CTF_TEMP_MASK				0x0003fe00
 #define		CTF_TEMP_SHIFT				9
 
+#define	CG_FDO_CTRL0					0xC0300064
+#define		FDO_STATIC_DUTY(x)			((x) << 0)
+#define		FDO_STATIC_DUTY_MASK			0x000000FF
+#define		FDO_STATIC_DUTY_SHIFT			0
+#define	CG_FDO_CTRL1					0xC0300068
+#define		FMAX_DUTY100(x)				((x) << 0)
+#define		FMAX_DUTY100_MASK			0x000000FF
+#define		FMAX_DUTY100_SHIFT			0
+#define	CG_FDO_CTRL2					0xC030006C
+#define		TMIN(x)					((x) << 0)
+#define		TMIN_MASK				0x000000FF
+#define		TMIN_SHIFT				0
+#define		FDO_PWM_MODE(x)				((x) << 11)
+#define		FDO_PWM_MODE_MASK			(7 << 11)
+#define		FDO_PWM_MODE_SHIFT			11
+#define		TACH_PWM_RESP_RATE(x)			((x) << 25)
+#define		TACH_PWM_RESP_RATE_MASK			(0x7f << 25)
+#define		TACH_PWM_RESP_RATE_SHIFT		25
+#define CG_TACH_CTRL                                    0xC0300070
+#       define EDGE_PER_REV(x)                          ((x) << 0)
+#       define EDGE_PER_REV_MASK                        (0x7 << 0)
+#       define EDGE_PER_REV_SHIFT                       0
+#       define TARGET_PERIOD(x)                         ((x) << 3)
+#       define TARGET_PERIOD_MASK                       0xfffffff8
+#       define TARGET_PERIOD_SHIFT                      3
+#define CG_TACH_STATUS                                  0xC0300074
+#       define TACH_PERIOD(x)                           ((x) << 0)
+#       define TACH_PERIOD_MASK                         0xffffffff
+#       define TACH_PERIOD_SHIFT                        0
+
 #define CG_ECLK_CNTL                                    0xC05000AC
 #       define ECLK_DIVIDER_MASK                        0x7f
 #       define ECLK_DIR_CNTL_EN                         (1 << 8)
@@ -1137,6 +1175,9 @@
 #define			SH_MEM_ALIGNMENT_MODE_UNALIGNED			3
 #define		DEFAULT_MTYPE(x)				((x) << 4)
 #define		APE1_MTYPE(x)					((x) << 7)
+/* valid for both DEFAULT_MTYPE and APE1_MTYPE */
+#define	MTYPE_CACHED					0
+#define	MTYPE_NONCACHED					3
 
 #define	SX_DEBUG_1					0x9060
 
@@ -1447,6 +1488,16 @@
 #define CP_HQD_ACTIVE                                     0xC91C
 #define CP_HQD_VMID                                       0xC920
 
+#define CP_HQD_PERSISTENT_STATE				0xC924u
+#define	DEFAULT_CP_HQD_PERSISTENT_STATE			(0x33U << 8)
+
+#define CP_HQD_PIPE_PRIORITY				0xC928u
+#define CP_HQD_QUEUE_PRIORITY				0xC92Cu
+#define CP_HQD_QUANTUM					0xC930u
+#define	QUANTUM_EN					1U
+#define	QUANTUM_SCALE_1MS				(1U << 4)
+#define	QUANTUM_DURATION(x)				((x) << 8)
+
 #define CP_HQD_PQ_BASE                                    0xC934
 #define CP_HQD_PQ_BASE_HI                                 0xC938
 #define CP_HQD_PQ_RPTR                                    0xC93C
@@ -1474,12 +1525,32 @@
 #define		PRIV_STATE      			(1 << 30)
 #define		KMD_QUEUE      				(1 << 31)
 
-#define CP_HQD_DEQUEUE_REQUEST                          0xC974
+#define CP_HQD_IB_BASE_ADDR				0xC95Cu
+#define CP_HQD_IB_BASE_ADDR_HI			0xC960u
+#define CP_HQD_IB_RPTR					0xC964u
+#define CP_HQD_IB_CONTROL				0xC968u
+#define	IB_ATC_EN					(1U << 23)
+#define	DEFAULT_MIN_IB_AVAIL_SIZE			(3U << 20)
+
+#define CP_HQD_DEQUEUE_REQUEST			0xC974
+#define	DEQUEUE_REQUEST_DRAIN				1
+#define DEQUEUE_REQUEST_RESET				2
 
 #define CP_MQD_CONTROL                                  0xC99C
 #define		MQD_VMID(x)				((x) << 0)
 #define		MQD_VMID_MASK      			(0xf << 0)
 
+#define CP_HQD_SEMA_CMD					0xC97Cu
+#define CP_HQD_MSG_TYPE					0xC980u
+#define CP_HQD_ATOMIC0_PREOP_LO			0xC984u
+#define CP_HQD_ATOMIC0_PREOP_HI			0xC988u
+#define CP_HQD_ATOMIC1_PREOP_LO			0xC98Cu
+#define CP_HQD_ATOMIC1_PREOP_HI			0xC990u
+#define CP_HQD_HQ_SCHEDULER0			0xC994u
+#define CP_HQD_HQ_SCHEDULER1			0xC998u
+
+#define SH_STATIC_MEM_CONFIG			0x9604u
+
 #define DB_RENDER_CONTROL                               0x28000
 
 #define PA_SC_RASTER_CONFIG                             0x28350
@@ -2069,4 +2140,20 @@
 #define VCE_CMD_IB_AUTO		0x00000005
 #define VCE_CMD_SEMAPHORE	0x00000006
 
+#define ATC_VMID0_PASID_MAPPING					0x339Cu
+#define	ATC_VMID_PASID_MAPPING_UPDATE_STATUS	0x3398u
+#define	ATC_VMID_PASID_MAPPING_VALID				(1U << 31)
+
+#define ATC_VM_APERTURE0_CNTL					0x3310u
+#define	ATS_ACCESS_MODE_NEVER						0
+#define	ATS_ACCESS_MODE_ALWAYS						1
+
+#define ATC_VM_APERTURE0_CNTL2					0x3318u
+#define ATC_VM_APERTURE0_HIGH_ADDR				0x3308u
+#define ATC_VM_APERTURE0_LOW_ADDR				0x3300u
+#define ATC_VM_APERTURE1_CNTL					0x3314u
+#define ATC_VM_APERTURE1_CNTL2					0x331Cu
+#define ATC_VM_APERTURE1_HIGH_ADDR				0x330Cu
+#define ATC_VM_APERTURE1_LOW_ADDR				0x3304u
+
 #endif
diff --git a/drivers/gpu/drm/radeon/evergreen_cs.c b/drivers/gpu/drm/radeon/evergreen_cs.c
index 5c8b358f9fba..924b1b7ab455 100644
--- a/drivers/gpu/drm/radeon/evergreen_cs.c
+++ b/drivers/gpu/drm/radeon/evergreen_cs.c
@@ -35,7 +35,7 @@
 #define MIN(a,b)                   (((a)<(b))?(a):(b))
 
 int r600_dma_cs_next_reloc(struct radeon_cs_parser *p,
-			   struct radeon_cs_reloc **cs_reloc);
+			   struct radeon_bo_list **cs_reloc);
 struct evergreen_cs_track {
 	u32			group_size;
 	u32			nbanks;
@@ -1094,7 +1094,7 @@ static int evergreen_cs_parse_packet0(struct radeon_cs_parser *p,
 static int evergreen_cs_check_reg(struct radeon_cs_parser *p, u32 reg, u32 idx)
 {
 	struct evergreen_cs_track *track = (struct evergreen_cs_track *)p->track;
-	struct radeon_cs_reloc *reloc;
+	struct radeon_bo_list *reloc;
 	u32 last_reg;
 	u32 m, i, tmp, *ib;
 	int r;
@@ -1792,7 +1792,7 @@ static bool evergreen_is_safe_reg(struct radeon_cs_parser *p, u32 reg, u32 idx)
 static int evergreen_packet3_check(struct radeon_cs_parser *p,
 				   struct radeon_cs_packet *pkt)
 {
-	struct radeon_cs_reloc *reloc;
+	struct radeon_bo_list *reloc;
 	struct evergreen_cs_track *track;
 	volatile u32 *ib;
 	unsigned idx;
@@ -2661,7 +2661,7 @@ int evergreen_cs_parse(struct radeon_cs_parser *p)
 			p->track = NULL;
 			return r;
 		}
-	} while (p->idx < p->chunks[p->chunk_ib_idx].length_dw);
+	} while (p->idx < p->chunk_ib->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]);
@@ -2684,8 +2684,8 @@ int evergreen_cs_parse(struct radeon_cs_parser *p)
  **/
 int evergreen_dma_cs_parse(struct radeon_cs_parser *p)
 {
-	struct radeon_cs_chunk *ib_chunk = &p->chunks[p->chunk_ib_idx];
-	struct radeon_cs_reloc *src_reloc, *dst_reloc, *dst2_reloc;
+	struct radeon_cs_chunk *ib_chunk = p->chunk_ib;
+	struct radeon_bo_list *src_reloc, *dst_reloc, *dst2_reloc;
 	u32 header, cmd, count, sub_cmd;
 	volatile u32 *ib = p->ib.ptr;
 	u32 idx;
@@ -3100,7 +3100,7 @@ int evergreen_dma_cs_parse(struct radeon_cs_parser *p)
 			DRM_ERROR("Unknown packet type %d at %d !\n", cmd, idx);
 			return -EINVAL;
 		}
-	} while (p->idx < p->chunks[p->chunk_ib_idx].length_dw);
+	} while (p->idx < p->chunk_ib->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]);
diff --git a/drivers/gpu/drm/radeon/evergreen_dma.c b/drivers/gpu/drm/radeon/evergreen_dma.c
index 66bcfadeedd1..96535aa8659c 100644
--- a/drivers/gpu/drm/radeon/evergreen_dma.c
+++ b/drivers/gpu/drm/radeon/evergreen_dma.c
@@ -110,31 +110,27 @@ struct radeon_fence *evergreen_copy_dma(struct radeon_device *rdev,
 					unsigned num_gpu_pages,
 					struct reservation_object *resv)
 {
-	struct radeon_semaphore *sem = NULL;
 	struct radeon_fence *fence;
+	struct radeon_sync sync;
 	int ring_index = rdev->asic->copy.dma_ring_index;
 	struct radeon_ring *ring = &rdev->ring[ring_index];
 	u32 size_in_dw, cur_size_in_dw;
 	int i, num_loops;
 	int r = 0;
 
-	r = radeon_semaphore_create(rdev, &sem);
-	if (r) {
-		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		return ERR_PTR(r);
-	}
+	radeon_sync_create(&sync);
 
 	size_in_dw = (num_gpu_pages << RADEON_GPU_PAGE_SHIFT) / 4;
 	num_loops = DIV_ROUND_UP(size_in_dw, 0xfffff);
 	r = radeon_ring_lock(rdev, ring, num_loops * 5 + 11);
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		radeon_semaphore_free(rdev, &sem, NULL);
+		radeon_sync_free(rdev, &sync, NULL);
 		return ERR_PTR(r);
 	}
 
-	radeon_semaphore_sync_resv(rdev, sem, resv, false);
-	radeon_semaphore_sync_rings(rdev, sem, ring->idx);
+	radeon_sync_resv(rdev, &sync, resv, false);
+	radeon_sync_rings(rdev, &sync, ring->idx);
 
 	for (i = 0; i < num_loops; i++) {
 		cur_size_in_dw = size_in_dw;
@@ -153,12 +149,12 @@ struct radeon_fence *evergreen_copy_dma(struct radeon_device *rdev,
 	r = radeon_fence_emit(rdev, &fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
-		radeon_semaphore_free(rdev, &sem, NULL);
+		radeon_sync_free(rdev, &sync, NULL);
 		return ERR_PTR(r);
 	}
 
 	radeon_ring_unlock_commit(rdev, ring, false);
-	radeon_semaphore_free(rdev, &sem, fence);
+	radeon_sync_free(rdev, &sync, fence);
 
 	return fence;
 }
diff --git a/drivers/gpu/drm/radeon/mkregtable.c b/drivers/gpu/drm/radeon/mkregtable.c
index 4a85bb644e24..b928c17bdeed 100644
--- a/drivers/gpu/drm/radeon/mkregtable.c
+++ b/drivers/gpu/drm/radeon/mkregtable.c
@@ -347,7 +347,7 @@ static inline void list_splice_tail_init(struct list_head *list,
  * list_entry - get the struct for this entry
  * @ptr:	the &struct list_head pointer.
  * @type:	the type of the struct this is embedded in.
- * @member:	the name of the list_struct within the struct.
+ * @member:	the name of the list_head within the struct.
  */
 #define list_entry(ptr, type, member) \
 	container_of(ptr, type, member)
@@ -356,7 +356,7 @@ static inline void list_splice_tail_init(struct list_head *list,
  * list_first_entry - get the first element from a list
  * @ptr:	the list head to take the element from.
  * @type:	the type of the struct this is embedded in.
- * @member:	the name of the list_struct within the struct.
+ * @member:	the name of the list_head within the struct.
  *
  * Note, that list is expected to be not empty.
  */
@@ -406,7 +406,7 @@ static inline void list_splice_tail_init(struct list_head *list,
  * list_for_each_entry	-	iterate over list of given type
  * @pos:	the type * to use as a loop cursor.
  * @head:	the head for your list.
- * @member:	the name of the list_struct within the struct.
+ * @member:	the name of the list_head within the struct.
  */
 #define list_for_each_entry(pos, head, member)				\
 	for (pos = list_entry((head)->next, typeof(*pos), member);	\
@@ -417,7 +417,7 @@ static inline void list_splice_tail_init(struct list_head *list,
  * list_for_each_entry_reverse - iterate backwards over list of given type.
  * @pos:	the type * to use as a loop cursor.
  * @head:	the head for your list.
- * @member:	the name of the list_struct within the struct.
+ * @member:	the name of the list_head within the struct.
  */
 #define list_for_each_entry_reverse(pos, head, member)			\
 	for (pos = list_entry((head)->prev, typeof(*pos), member);	\
@@ -428,7 +428,7 @@ static inline void list_splice_tail_init(struct list_head *list,
  * list_prepare_entry - prepare a pos entry for use in list_for_each_entry_continue()
  * @pos:	the type * to use as a start point
  * @head:	the head of the list
- * @member:	the name of the list_struct within the struct.
+ * @member:	the name of the list_head within the struct.
  *
  * Prepares a pos entry for use as a start point in list_for_each_entry_continue().
  */
@@ -439,7 +439,7 @@ static inline void list_splice_tail_init(struct list_head *list,
  * list_for_each_entry_continue - continue iteration over list of given type
  * @pos:	the type * to use as a loop cursor.
  * @head:	the head for your list.
- * @member:	the name of the list_struct within the struct.
+ * @member:	the name of the list_head within the struct.
  *
  * Continue to iterate over list of given type, continuing after
  * the current position.
@@ -453,7 +453,7 @@ static inline void list_splice_tail_init(struct list_head *list,
  * list_for_each_entry_continue_reverse - iterate backwards from the given point
  * @pos:	the type * to use as a loop cursor.
  * @head:	the head for your list.
- * @member:	the name of the list_struct within the struct.
+ * @member:	the name of the list_head within the struct.
  *
  * Start to iterate over list of given type backwards, continuing after
  * the current position.
@@ -467,7 +467,7 @@ static inline void list_splice_tail_init(struct list_head *list,
  * list_for_each_entry_from - iterate over list of given type from the current point
  * @pos:	the type * to use as a loop cursor.
  * @head:	the head for your list.
- * @member:	the name of the list_struct within the struct.
+ * @member:	the name of the list_head within the struct.
  *
  * Iterate over list of given type, continuing from current position.
  */
@@ -480,7 +480,7 @@ static inline void list_splice_tail_init(struct list_head *list,
  * @pos:	the type * to use as a loop cursor.
  * @n:		another type * to use as temporary storage
  * @head:	the head for your list.
- * @member:	the name of the list_struct within the struct.
+ * @member:	the name of the list_head within the struct.
  */
 #define list_for_each_entry_safe(pos, n, head, member)			\
 	for (pos = list_entry((head)->next, typeof(*pos), member),	\
@@ -493,7 +493,7 @@ static inline void list_splice_tail_init(struct list_head *list,
  * @pos:	the type * to use as a loop cursor.
  * @n:		another type * to use as temporary storage
  * @head:	the head for your list.
- * @member:	the name of the list_struct within the struct.
+ * @member:	the name of the list_head within the struct.
  *
  * Iterate over list of given type, continuing after current point,
  * safe against removal of list entry.
@@ -509,7 +509,7 @@ static inline void list_splice_tail_init(struct list_head *list,
  * @pos:	the type * to use as a loop cursor.
  * @n:		another type * to use as temporary storage
  * @head:	the head for your list.
- * @member:	the name of the list_struct within the struct.
+ * @member:	the name of the list_head within the struct.
  *
  * Iterate over list of given type from current point, safe against
  * removal of list entry.
@@ -524,7 +524,7 @@ static inline void list_splice_tail_init(struct list_head *list,
  * @pos:	the type * to use as a loop cursor.
  * @n:		another type * to use as temporary storage
  * @head:	the head for your list.
- * @member:	the name of the list_struct within the struct.
+ * @member:	the name of the list_head within the struct.
  *
  * Iterate backwards over list of given type, safe against removal
  * of list entry.
diff --git a/drivers/gpu/drm/radeon/ni.c b/drivers/gpu/drm/radeon/ni.c
index 3faee58946dd..360de9f1f491 100644
--- a/drivers/gpu/drm/radeon/ni.c
+++ b/drivers/gpu/drm/radeon/ni.c
@@ -1373,6 +1373,7 @@ void cayman_fence_ring_emit(struct radeon_device *rdev,
 void cayman_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 {
 	struct radeon_ring *ring = &rdev->ring[ib->ring];
+	unsigned vm_id = ib->vm ? ib->vm->ids[ib->ring].id : 0;
 	u32 cp_coher_cntl = PACKET3_FULL_CACHE_ENA | PACKET3_TC_ACTION_ENA |
 		PACKET3_SH_ACTION_ENA;
 
@@ -1395,15 +1396,14 @@ void cayman_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 #endif
 			  (ib->gpu_addr & 0xFFFFFFFC));
 	radeon_ring_write(ring, upper_32_bits(ib->gpu_addr) & 0xFF);
-	radeon_ring_write(ring, ib->length_dw | 
-			  (ib->vm ? (ib->vm->id << 24) : 0));
+	radeon_ring_write(ring, ib->length_dw | (vm_id << 24));
 
 	/* flush read cache over gart for this vmid */
 	radeon_ring_write(ring, PACKET3(PACKET3_SURFACE_SYNC, 3));
 	radeon_ring_write(ring, PACKET3_ENGINE_ME | cp_coher_cntl);
 	radeon_ring_write(ring, 0xFFFFFFFF);
 	radeon_ring_write(ring, 0);
-	radeon_ring_write(ring, ((ib->vm ? ib->vm->id : 0) << 24) | 10); /* poll interval */
+	radeon_ring_write(ring, (vm_id << 24) | 10); /* poll interval */
 }
 
 static void cayman_cp_enable(struct radeon_device *rdev, bool enable)
@@ -2502,15 +2502,11 @@ void cayman_vm_decode_fault(struct radeon_device *rdev,
  * Update the page table base and flush the VM TLB
  * using the CP (cayman-si).
  */
-void cayman_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
+void cayman_vm_flush(struct radeon_device *rdev, struct radeon_ring *ring,
+		     unsigned vm_id, uint64_t pd_addr)
 {
-	struct radeon_ring *ring = &rdev->ring[ridx];
-
-	if (vm == NULL)
-		return;
-
-	radeon_ring_write(ring, PACKET0(VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (vm->id << 2), 0));
-	radeon_ring_write(ring, vm->pd_gpu_addr >> 12);
+	radeon_ring_write(ring, PACKET0(VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (vm_id << 2), 0));
+	radeon_ring_write(ring, pd_addr >> 12);
 
 	/* flush hdp cache */
 	radeon_ring_write(ring, PACKET0(HDP_MEM_COHERENCY_FLUSH_CNTL, 0));
@@ -2518,7 +2514,7 @@ void cayman_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
 
 	/* bits 0-7 are the VM contexts0-7 */
 	radeon_ring_write(ring, PACKET0(VM_INVALIDATE_REQUEST, 0));
-	radeon_ring_write(ring, 1 << vm->id);
+	radeon_ring_write(ring, 1 << vm_id);
 
 	/* sync PFP to ME, otherwise we might get invalid PFP reads */
 	radeon_ring_write(ring, PACKET3(PACKET3_PFP_SYNC_ME, 0));
diff --git a/drivers/gpu/drm/radeon/ni_dma.c b/drivers/gpu/drm/radeon/ni_dma.c
index f26f0a9fb522..50f88611ff60 100644
--- a/drivers/gpu/drm/radeon/ni_dma.c
+++ b/drivers/gpu/drm/radeon/ni_dma.c
@@ -123,6 +123,7 @@ void cayman_dma_ring_ib_execute(struct radeon_device *rdev,
 				struct radeon_ib *ib)
 {
 	struct radeon_ring *ring = &rdev->ring[ib->ring];
+	unsigned vm_id = ib->vm ? ib->vm->ids[ib->ring].id : 0;
 
 	if (rdev->wb.enabled) {
 		u32 next_rptr = ring->wptr + 4;
@@ -140,7 +141,7 @@ void cayman_dma_ring_ib_execute(struct radeon_device *rdev,
 	 */
 	while ((ring->wptr & 7) != 5)
 		radeon_ring_write(ring, DMA_PACKET(DMA_PACKET_NOP, 0, 0, 0));
-	radeon_ring_write(ring, DMA_IB_PACKET(DMA_PACKET_INDIRECT_BUFFER, ib->vm ? ib->vm->id : 0, 0));
+	radeon_ring_write(ring, DMA_IB_PACKET(DMA_PACKET_INDIRECT_BUFFER, vm_id, 0));
 	radeon_ring_write(ring, (ib->gpu_addr & 0xFFFFFFE0));
 	radeon_ring_write(ring, (ib->length_dw << 12) | (upper_32_bits(ib->gpu_addr) & 0xFF));
 
@@ -446,16 +447,12 @@ void cayman_dma_vm_pad_ib(struct radeon_ib *ib)
 		ib->ptr[ib->length_dw++] = DMA_PACKET(DMA_PACKET_NOP, 0, 0, 0);
 }
 
-void cayman_dma_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
+void cayman_dma_vm_flush(struct radeon_device *rdev, struct radeon_ring *ring,
+			 unsigned vm_id, uint64_t pd_addr)
 {
-	struct radeon_ring *ring = &rdev->ring[ridx];
-
-	if (vm == NULL)
-		return;
-
 	radeon_ring_write(ring, DMA_PACKET(DMA_PACKET_SRBM_WRITE, 0, 0, 0));
-	radeon_ring_write(ring, (0xf << 16) | ((VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (vm->id << 2)) >> 2));
-	radeon_ring_write(ring, vm->pd_gpu_addr >> 12);
+	radeon_ring_write(ring, (0xf << 16) | ((VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (vm_id << 2)) >> 2));
+	radeon_ring_write(ring, pd_addr >> 12);
 
 	/* flush hdp cache */
 	radeon_ring_write(ring, DMA_PACKET(DMA_PACKET_SRBM_WRITE, 0, 0, 0));
@@ -465,6 +462,6 @@ void cayman_dma_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm
 	/* bits 0-7 are the VM contexts0-7 */
 	radeon_ring_write(ring, DMA_PACKET(DMA_PACKET_SRBM_WRITE, 0, 0, 0));
 	radeon_ring_write(ring, (0xf << 16) | (VM_INVALIDATE_REQUEST >> 2));
-	radeon_ring_write(ring, 1 << vm->id);
+	radeon_ring_write(ring, 1 << vm_id);
 }
 
diff --git a/drivers/gpu/drm/radeon/ppsmc.h b/drivers/gpu/drm/radeon/ppsmc.h
index 5670b8291285..7e5724a12f8b 100644
--- a/drivers/gpu/drm/radeon/ppsmc.h
+++ b/drivers/gpu/drm/radeon/ppsmc.h
@@ -56,6 +56,14 @@
 #define PPSMC_STATEFLAG_DEEPSLEEP_THROTTLE 0x20
 #define PPSMC_STATEFLAG_DEEPSLEEP_BYPASS   0x40
 
+#define FDO_MODE_HARDWARE 0
+#define FDO_MODE_PIECE_WISE_LINEAR 1
+
+enum FAN_CONTROL {
+	FAN_CONTROL_FUZZY,
+	FAN_CONTROL_TABLE
+};
+
 #define PPSMC_Result_OK             ((uint8_t)0x01)
 #define PPSMC_Result_Failed         ((uint8_t)0xFF)
 
@@ -79,6 +87,8 @@ typedef uint8_t PPSMC_Result;
 #define PPSMC_MSG_DisableCac                ((uint8_t)0x54)
 #define PPSMC_TDPClampingActive             ((uint8_t)0x59)
 #define PPSMC_TDPClampingInactive           ((uint8_t)0x5A)
+#define PPSMC_StartFanControl               ((uint8_t)0x5B)
+#define PPSMC_StopFanControl                ((uint8_t)0x5C)
 #define PPSMC_MSG_NoDisplay                 ((uint8_t)0x5D)
 #define PPSMC_MSG_HasDisplay                ((uint8_t)0x5E)
 #define PPSMC_MSG_UVDPowerOFF               ((uint8_t)0x60)
@@ -106,6 +116,7 @@ typedef uint8_t PPSMC_Result;
 #define PPSMC_MSG_SAMUDPM_SetEnabledMask      ((uint16_t) 0x130)
 #define PPSMC_MSG_MCLKDPM_ForceState          ((uint16_t) 0x131)
 #define PPSMC_MSG_MCLKDPM_NoForcedLevel       ((uint16_t) 0x132)
+#define PPSMC_MSG_Thermal_Cntl_Disable        ((uint16_t) 0x133)
 #define PPSMC_MSG_Voltage_Cntl_Disable        ((uint16_t) 0x135)
 #define PPSMC_MSG_PCIeDPM_Enable              ((uint16_t) 0x136)
 #define PPSMC_MSG_PCIeDPM_Disable             ((uint16_t) 0x13d)
@@ -149,6 +160,10 @@ typedef uint8_t PPSMC_Result;
 #define PPSMC_MSG_MASTER_DeepSleep_ON         ((uint16_t) 0x18F)
 #define PPSMC_MSG_MASTER_DeepSleep_OFF        ((uint16_t) 0x190)
 #define PPSMC_MSG_Remove_DC_Clamp             ((uint16_t) 0x191)
+#define PPSMC_MSG_SetFanPwmMax                ((uint16_t) 0x19A)
+
+#define PPSMC_MSG_ENABLE_THERMAL_DPM          ((uint16_t) 0x19C)
+#define PPSMC_MSG_DISABLE_THERMAL_DPM         ((uint16_t) 0x19D)
 
 #define PPSMC_MSG_API_GetSclkFrequency        ((uint16_t) 0x200)
 #define PPSMC_MSG_API_GetMclkFrequency        ((uint16_t) 0x201)
@@ -157,10 +172,11 @@ typedef uint8_t PPSMC_Result;
 #define PPSMC_MSG_DPM_Config                ((uint32_t) 0x102)
 #define PPSMC_MSG_DPM_ForceState            ((uint32_t) 0x104)
 #define PPSMC_MSG_PG_SIMD_Config            ((uint32_t) 0x108)
-#define PPSMC_MSG_DPM_N_LevelsDisabled      ((uint32_t) 0x112)
+#define PPSMC_MSG_Thermal_Cntl_Enable       ((uint32_t) 0x10a)
 #define PPSMC_MSG_Voltage_Cntl_Enable       ((uint32_t) 0x109)
 #define PPSMC_MSG_VCEPowerOFF               ((uint32_t) 0x10e)
 #define PPSMC_MSG_VCEPowerON                ((uint32_t) 0x10f)
+#define PPSMC_MSG_DPM_N_LevelsDisabled      ((uint32_t) 0x112)
 #define PPSMC_MSG_DCE_RemoveVoltageAdjustment   ((uint32_t) 0x11d)
 #define PPSMC_MSG_DCE_AllowVoltageAdjustment    ((uint32_t) 0x11e)
 #define PPSMC_MSG_EnableBAPM                ((uint32_t) 0x120)
diff --git a/drivers/gpu/drm/radeon/pptable.h b/drivers/gpu/drm/radeon/pptable.h
index 2d532996c697..4c2eec49dadc 100644
--- a/drivers/gpu/drm/radeon/pptable.h
+++ b/drivers/gpu/drm/radeon/pptable.h
@@ -96,6 +96,14 @@ typedef struct _ATOM_PPLIB_FANTABLE2
     USHORT  usTMax;                          // The max temperature
 } ATOM_PPLIB_FANTABLE2;
 
+typedef struct _ATOM_PPLIB_FANTABLE3
+{
+	ATOM_PPLIB_FANTABLE2 basicTable2;
+	UCHAR ucFanControlMode;
+	USHORT usFanPWMMax;
+	USHORT usFanOutputSensitivity;
+} ATOM_PPLIB_FANTABLE3;
+
 typedef struct _ATOM_PPLIB_EXTENDEDHEADER
 {
     USHORT  usSize;
diff --git a/drivers/gpu/drm/radeon/r100.c b/drivers/gpu/drm/radeon/r100.c
index b53b31a7b76f..74f06d540591 100644
--- a/drivers/gpu/drm/radeon/r100.c
+++ b/drivers/gpu/drm/radeon/r100.c
@@ -1254,7 +1254,7 @@ int r100_reloc_pitch_offset(struct radeon_cs_parser *p,
 	int r;
 	u32 tile_flags = 0;
 	u32 tmp;
-	struct radeon_cs_reloc *reloc;
+	struct radeon_bo_list *reloc;
 	u32 value;
 
 	r = radeon_cs_packet_next_reloc(p, &reloc, 0);
@@ -1293,7 +1293,7 @@ int r100_packet3_load_vbpntr(struct radeon_cs_parser *p,
 			     int idx)
 {
 	unsigned c, i;
-	struct radeon_cs_reloc *reloc;
+	struct radeon_bo_list *reloc;
 	struct r100_cs_track *track;
 	int r = 0;
 	volatile uint32_t *ib;
@@ -1542,7 +1542,7 @@ static int r100_packet0_check(struct radeon_cs_parser *p,
 			      struct radeon_cs_packet *pkt,
 			      unsigned idx, unsigned reg)
 {
-	struct radeon_cs_reloc *reloc;
+	struct radeon_bo_list *reloc;
 	struct r100_cs_track *track;
 	volatile uint32_t *ib;
 	uint32_t tmp;
@@ -1901,7 +1901,7 @@ int r100_cs_track_check_pkt3_indx_buffer(struct radeon_cs_parser *p,
 static int r100_packet3_check(struct radeon_cs_parser *p,
 			      struct radeon_cs_packet *pkt)
 {
-	struct radeon_cs_reloc *reloc;
+	struct radeon_bo_list *reloc;
 	struct r100_cs_track *track;
 	unsigned idx;
 	volatile uint32_t *ib;
@@ -2061,7 +2061,7 @@ int r100_cs_parse(struct radeon_cs_parser *p)
 		}
 		if (r)
 			return r;
-	} while (p->idx < p->chunks[p->chunk_ib_idx].length_dw);
+	} while (p->idx < p->chunk_ib->length_dw);
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/radeon/r200.c b/drivers/gpu/drm/radeon/r200.c
index 732d4938aab7..c70e6d5bcd19 100644
--- a/drivers/gpu/drm/radeon/r200.c
+++ b/drivers/gpu/drm/radeon/r200.c
@@ -146,7 +146,7 @@ int r200_packet0_check(struct radeon_cs_parser *p,
 		       struct radeon_cs_packet *pkt,
 		       unsigned idx, unsigned reg)
 {
-	struct radeon_cs_reloc *reloc;
+	struct radeon_bo_list *reloc;
 	struct r100_cs_track *track;
 	volatile uint32_t *ib;
 	uint32_t tmp;
diff --git a/drivers/gpu/drm/radeon/r300.c b/drivers/gpu/drm/radeon/r300.c
index 1bc4704034ce..064ad5569cca 100644
--- a/drivers/gpu/drm/radeon/r300.c
+++ b/drivers/gpu/drm/radeon/r300.c
@@ -598,7 +598,7 @@ static int r300_packet0_check(struct radeon_cs_parser *p,
 		struct radeon_cs_packet *pkt,
 		unsigned idx, unsigned reg)
 {
-	struct radeon_cs_reloc *reloc;
+	struct radeon_bo_list *reloc;
 	struct r100_cs_track *track;
 	volatile uint32_t *ib;
 	uint32_t tmp, tile_flags = 0;
@@ -1142,7 +1142,7 @@ fail:
 static int r300_packet3_check(struct radeon_cs_parser *p,
 			      struct radeon_cs_packet *pkt)
 {
-	struct radeon_cs_reloc *reloc;
+	struct radeon_bo_list *reloc;
 	struct r100_cs_track *track;
 	volatile uint32_t *ib;
 	unsigned idx;
@@ -1283,7 +1283,7 @@ int r300_cs_parse(struct radeon_cs_parser *p)
 		if (r) {
 			return r;
 		}
-	} while (p->idx < p->chunks[p->chunk_ib_idx].length_dw);
+	} while (p->idx < p->chunk_ib->length_dw);
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/radeon/r600.c b/drivers/gpu/drm/radeon/r600.c
index 56b02927cd3d..ef5d6066fa5b 100644
--- a/drivers/gpu/drm/radeon/r600.c
+++ b/drivers/gpu/drm/radeon/r600.c
@@ -2889,31 +2889,27 @@ struct radeon_fence *r600_copy_cpdma(struct radeon_device *rdev,
 				     unsigned num_gpu_pages,
 				     struct reservation_object *resv)
 {
-	struct radeon_semaphore *sem = NULL;
 	struct radeon_fence *fence;
+	struct radeon_sync sync;
 	int ring_index = rdev->asic->copy.blit_ring_index;
 	struct radeon_ring *ring = &rdev->ring[ring_index];
 	u32 size_in_bytes, cur_size_in_bytes, tmp;
 	int i, num_loops;
 	int r = 0;
 
-	r = radeon_semaphore_create(rdev, &sem);
-	if (r) {
-		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		return ERR_PTR(r);
-	}
+	radeon_sync_create(&sync);
 
 	size_in_bytes = (num_gpu_pages << RADEON_GPU_PAGE_SHIFT);
 	num_loops = DIV_ROUND_UP(size_in_bytes, 0x1fffff);
 	r = radeon_ring_lock(rdev, ring, num_loops * 6 + 24);
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		radeon_semaphore_free(rdev, &sem, NULL);
+		radeon_sync_free(rdev, &sync, NULL);
 		return ERR_PTR(r);
 	}
 
-	radeon_semaphore_sync_resv(rdev, sem, resv, false);
-	radeon_semaphore_sync_rings(rdev, sem, ring->idx);
+	radeon_sync_resv(rdev, &sync, resv, false);
+	radeon_sync_rings(rdev, &sync, ring->idx);
 
 	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
 	radeon_ring_write(ring, (WAIT_UNTIL - PACKET3_SET_CONFIG_REG_OFFSET) >> 2);
@@ -2942,12 +2938,12 @@ struct radeon_fence *r600_copy_cpdma(struct radeon_device *rdev,
 	r = radeon_fence_emit(rdev, &fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
-		radeon_semaphore_free(rdev, &sem, NULL);
+		radeon_sync_free(rdev, &sync, NULL);
 		return ERR_PTR(r);
 	}
 
 	radeon_ring_unlock_commit(rdev, ring, false);
-	radeon_semaphore_free(rdev, &sem, fence);
+	radeon_sync_free(rdev, &sync, fence);
 
 	return fence;
 }
diff --git a/drivers/gpu/drm/radeon/r600_cs.c b/drivers/gpu/drm/radeon/r600_cs.c
index c47537a1ddba..acc1f99c84d9 100644
--- a/drivers/gpu/drm/radeon/r600_cs.c
+++ b/drivers/gpu/drm/radeon/r600_cs.c
@@ -969,7 +969,7 @@ static int r600_cs_parse_packet0(struct radeon_cs_parser *p,
 static int r600_cs_check_reg(struct radeon_cs_parser *p, u32 reg, u32 idx)
 {
 	struct r600_cs_track *track = (struct r600_cs_track *)p->track;
-	struct radeon_cs_reloc *reloc;
+	struct radeon_bo_list *reloc;
 	u32 m, i, tmp, *ib;
 	int r;
 
@@ -1626,7 +1626,7 @@ static bool r600_is_safe_reg(struct radeon_cs_parser *p, u32 reg, u32 idx)
 static int r600_packet3_check(struct radeon_cs_parser *p,
 				struct radeon_cs_packet *pkt)
 {
-	struct radeon_cs_reloc *reloc;
+	struct radeon_bo_list *reloc;
 	struct r600_cs_track *track;
 	volatile u32 *ib;
 	unsigned idx;
@@ -2316,7 +2316,7 @@ int r600_cs_parse(struct radeon_cs_parser *p)
 			p->track = NULL;
 			return r;
 		}
-	} while (p->idx < p->chunks[p->chunk_ib_idx].length_dw);
+	} while (p->idx < p->chunk_ib->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]);
@@ -2351,10 +2351,10 @@ static void r600_cs_parser_fini(struct radeon_cs_parser *parser, int error)
 
 static int r600_cs_parser_relocs_legacy(struct radeon_cs_parser *p)
 {
-	if (p->chunk_relocs_idx == -1) {
+	if (p->chunk_relocs == NULL) {
 		return 0;
 	}
-	p->relocs = kzalloc(sizeof(struct radeon_cs_reloc), GFP_KERNEL);
+	p->relocs = kzalloc(sizeof(struct radeon_bo_list), GFP_KERNEL);
 	if (p->relocs == NULL) {
 		return -ENOMEM;
 	}
@@ -2398,7 +2398,7 @@ int r600_cs_legacy(struct drm_device *dev, void *data, struct drm_file *filp,
 	/* Copy the packet into the IB, the parser will read from the
 	 * input memory (cached) and write to the IB (which can be
 	 * uncached). */
-	ib_chunk = &parser.chunks[parser.chunk_ib_idx];
+	ib_chunk = parser.chunk_ib;
 	parser.ib.length_dw = ib_chunk->length_dw;
 	*l = parser.ib.length_dw;
 	if (copy_from_user(ib, ib_chunk->user_ptr, ib_chunk->length_dw * 4)) {
@@ -2435,24 +2435,24 @@ void r600_cs_legacy_init(void)
  * GPU offset using the provided start.
  **/
 int r600_dma_cs_next_reloc(struct radeon_cs_parser *p,
-			   struct radeon_cs_reloc **cs_reloc)
+			   struct radeon_bo_list **cs_reloc)
 {
 	struct radeon_cs_chunk *relocs_chunk;
 	unsigned idx;
 
 	*cs_reloc = NULL;
-	if (p->chunk_relocs_idx == -1) {
+	if (p->chunk_relocs == NULL) {
 		DRM_ERROR("No relocation chunk !\n");
 		return -EINVAL;
 	}
-	relocs_chunk = &p->chunks[p->chunk_relocs_idx];
+	relocs_chunk = p->chunk_relocs;
 	idx = p->dma_reloc_idx;
 	if (idx >= p->nrelocs) {
 		DRM_ERROR("Relocs at %d after relocations chunk end %d !\n",
 			  idx, p->nrelocs);
 		return -EINVAL;
 	}
-	*cs_reloc = p->relocs_ptr[idx];
+	*cs_reloc = &p->relocs[idx];
 	p->dma_reloc_idx++;
 	return 0;
 }
@@ -2472,8 +2472,8 @@ int r600_dma_cs_next_reloc(struct radeon_cs_parser *p,
  **/
 int r600_dma_cs_parse(struct radeon_cs_parser *p)
 {
-	struct radeon_cs_chunk *ib_chunk = &p->chunks[p->chunk_ib_idx];
-	struct radeon_cs_reloc *src_reloc, *dst_reloc;
+	struct radeon_cs_chunk *ib_chunk = p->chunk_ib;
+	struct radeon_bo_list *src_reloc, *dst_reloc;
 	u32 header, cmd, count, tiled;
 	volatile u32 *ib = p->ib.ptr;
 	u32 idx, idx_value;
@@ -2619,7 +2619,7 @@ int r600_dma_cs_parse(struct radeon_cs_parser *p)
 			DRM_ERROR("Unknown packet type %d at %d !\n", cmd, idx);
 			return -EINVAL;
 		}
-	} while (p->idx < p->chunks[p->chunk_ib_idx].length_dw);
+	} while (p->idx < p->chunk_ib->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]);
diff --git a/drivers/gpu/drm/radeon/r600_dma.c b/drivers/gpu/drm/radeon/r600_dma.c
index cf0df45d455e..d2dd29ab24fa 100644
--- a/drivers/gpu/drm/radeon/r600_dma.c
+++ b/drivers/gpu/drm/radeon/r600_dma.c
@@ -441,31 +441,27 @@ struct radeon_fence *r600_copy_dma(struct radeon_device *rdev,
 				   unsigned num_gpu_pages,
 				   struct reservation_object *resv)
 {
-	struct radeon_semaphore *sem = NULL;
 	struct radeon_fence *fence;
+	struct radeon_sync sync;
 	int ring_index = rdev->asic->copy.dma_ring_index;
 	struct radeon_ring *ring = &rdev->ring[ring_index];
 	u32 size_in_dw, cur_size_in_dw;
 	int i, num_loops;
 	int r = 0;
 
-	r = radeon_semaphore_create(rdev, &sem);
-	if (r) {
-		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		return ERR_PTR(r);
-	}
+	radeon_sync_create(&sync);
 
 	size_in_dw = (num_gpu_pages << RADEON_GPU_PAGE_SHIFT) / 4;
 	num_loops = DIV_ROUND_UP(size_in_dw, 0xFFFE);
 	r = radeon_ring_lock(rdev, ring, num_loops * 4 + 8);
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		radeon_semaphore_free(rdev, &sem, NULL);
+		radeon_sync_free(rdev, &sync, NULL);
 		return ERR_PTR(r);
 	}
 
-	radeon_semaphore_sync_resv(rdev, sem, resv, false);
-	radeon_semaphore_sync_rings(rdev, sem, ring->idx);
+	radeon_sync_resv(rdev, &sync, resv, false);
+	radeon_sync_rings(rdev, &sync, ring->idx);
 
 	for (i = 0; i < num_loops; i++) {
 		cur_size_in_dw = size_in_dw;
@@ -484,12 +480,12 @@ struct radeon_fence *r600_copy_dma(struct radeon_device *rdev,
 	r = radeon_fence_emit(rdev, &fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
-		radeon_semaphore_free(rdev, &sem, NULL);
+		radeon_sync_free(rdev, &sync, NULL);
 		return ERR_PTR(r);
 	}
 
 	radeon_ring_unlock_commit(rdev, ring, false);
-	radeon_semaphore_free(rdev, &sem, fence);
+	radeon_sync_free(rdev, &sync, fence);
 
 	return fence;
 }
diff --git a/drivers/gpu/drm/radeon/r600_dpm.c b/drivers/gpu/drm/radeon/r600_dpm.c
index b5c73df8e202..843b65f46ece 100644
--- a/drivers/gpu/drm/radeon/r600_dpm.c
+++ b/drivers/gpu/drm/radeon/r600_dpm.c
@@ -811,6 +811,7 @@ union power_info {
 union fan_info {
 	struct _ATOM_PPLIB_FANTABLE fan;
 	struct _ATOM_PPLIB_FANTABLE2 fan2;
+	struct _ATOM_PPLIB_FANTABLE3 fan3;
 };
 
 static int r600_parse_clk_voltage_dep_table(struct radeon_clock_voltage_dependency_table *radeon_table,
@@ -900,6 +901,14 @@ int r600_parse_extended_power_table(struct radeon_device *rdev)
 			else
 				rdev->pm.dpm.fan.t_max = 10900;
 			rdev->pm.dpm.fan.cycle_delay = 100000;
+			if (fan_info->fan.ucFanTableFormat >= 3) {
+				rdev->pm.dpm.fan.control_mode = fan_info->fan3.ucFanControlMode;
+				rdev->pm.dpm.fan.default_max_fan_pwm =
+					le16_to_cpu(fan_info->fan3.usFanPWMMax);
+				rdev->pm.dpm.fan.default_fan_output_sensitivity = 4836;
+				rdev->pm.dpm.fan.fan_output_sensitivity =
+					le16_to_cpu(fan_info->fan3.usFanOutputSensitivity);
+			}
 			rdev->pm.dpm.fan.ucode_fan_control = true;
 		}
 	}
diff --git a/drivers/gpu/drm/radeon/r600_dpm.h b/drivers/gpu/drm/radeon/r600_dpm.h
index 46b9d2a03018..bd499d749bc9 100644
--- a/drivers/gpu/drm/radeon/r600_dpm.h
+++ b/drivers/gpu/drm/radeon/r600_dpm.h
@@ -96,6 +96,9 @@
 #define R600_TEMP_RANGE_MIN (90 * 1000)
 #define R600_TEMP_RANGE_MAX (120 * 1000)
 
+#define FDO_PWM_MODE_STATIC  1
+#define FDO_PWM_MODE_STATIC_RPM 5
+
 enum r600_power_level {
 	R600_POWER_LEVEL_LOW = 0,
 	R600_POWER_LEVEL_MEDIUM = 1,
diff --git a/drivers/gpu/drm/radeon/radeon.h b/drivers/gpu/drm/radeon/radeon.h
index a9717b3fbf1b..54529b837afa 100644
--- a/drivers/gpu/drm/radeon/radeon.h
+++ b/drivers/gpu/drm/radeon/radeon.h
@@ -150,9 +150,6 @@ extern int radeon_backlight;
 /* number of hw syncs before falling back on blocking */
 #define RADEON_NUM_SYNCS			4
 
-/* number of hw syncs before falling back on blocking */
-#define RADEON_NUM_SYNCS			4
-
 /* hardcode those limit for now */
 #define RADEON_VA_IB_OFFSET			(1 << 20)
 #define RADEON_VA_RESERVED_SIZE			(8 << 20)
@@ -363,14 +360,15 @@ struct radeon_fence_driver {
 };
 
 struct radeon_fence {
-	struct fence base;
+	struct fence		base;
 
-	struct radeon_device		*rdev;
-	uint64_t			seq;
+	struct radeon_device	*rdev;
+	uint64_t		seq;
 	/* RB, DMA, etc. */
-	unsigned			ring;
+	unsigned		ring;
+	bool			is_vm_update;
 
-	wait_queue_t			fence_wake;
+	wait_queue_t		fence_wake;
 };
 
 int radeon_fence_driver_start_ring(struct radeon_device *rdev, int ring);
@@ -452,12 +450,22 @@ struct radeon_mman {
 #endif
 };
 
+struct radeon_bo_list {
+	struct radeon_bo		*robj;
+	struct ttm_validate_buffer	tv;
+	uint64_t			gpu_offset;
+	unsigned			prefered_domains;
+	unsigned			allowed_domains;
+	uint32_t			tiling_flags;
+};
+
 /* bo virtual address in a specific vm */
 struct radeon_bo_va {
 	/* protected by bo being reserved */
 	struct list_head		bo_list;
 	uint32_t			flags;
 	uint64_t			addr;
+	struct radeon_fence		*last_pt_update;
 	unsigned			ref_count;
 
 	/* protected by vm mutex */
@@ -474,7 +482,7 @@ struct radeon_bo {
 	struct list_head		list;
 	/* Protected by tbo.reserved */
 	u32				initial_domain;
-	struct ttm_place		placements[3];
+	struct ttm_place		placements[4];
 	struct ttm_placement		placement;
 	struct ttm_buffer_object	tbo;
 	struct ttm_bo_kmap_obj		kmap;
@@ -576,10 +584,9 @@ int radeon_mode_dumb_mmap(struct drm_file *filp,
  * Semaphores.
  */
 struct radeon_semaphore {
-	struct radeon_sa_bo		*sa_bo;
-	signed				waiters;
-	uint64_t			gpu_addr;
-	struct radeon_fence		*sync_to[RADEON_NUM_RINGS];
+	struct radeon_sa_bo	*sa_bo;
+	signed			waiters;
+	uint64_t		gpu_addr;
 };
 
 int radeon_semaphore_create(struct radeon_device *rdev,
@@ -588,20 +595,33 @@ bool radeon_semaphore_emit_signal(struct radeon_device *rdev, int ring,
 				  struct radeon_semaphore *semaphore);
 bool radeon_semaphore_emit_wait(struct radeon_device *rdev, int ring,
 				struct radeon_semaphore *semaphore);
-void radeon_semaphore_sync_fence(struct radeon_semaphore *semaphore,
-				 struct radeon_fence *fence);
-int radeon_semaphore_sync_resv(struct radeon_device *rdev,
-			       struct radeon_semaphore *semaphore,
-			       struct reservation_object *resv,
-			       bool shared);
-int radeon_semaphore_sync_rings(struct radeon_device *rdev,
-				struct radeon_semaphore *semaphore,
-				int waiting_ring);
 void radeon_semaphore_free(struct radeon_device *rdev,
 			   struct radeon_semaphore **semaphore,
 			   struct radeon_fence *fence);
 
 /*
+ * Synchronization
+ */
+struct radeon_sync {
+	struct radeon_semaphore *semaphores[RADEON_NUM_SYNCS];
+	struct radeon_fence	*sync_to[RADEON_NUM_RINGS];
+	struct radeon_fence	*last_vm_update;
+};
+
+void radeon_sync_create(struct radeon_sync *sync);
+void radeon_sync_fence(struct radeon_sync *sync,
+		       struct radeon_fence *fence);
+int radeon_sync_resv(struct radeon_device *rdev,
+		     struct radeon_sync *sync,
+		     struct reservation_object *resv,
+		     bool shared);
+int radeon_sync_rings(struct radeon_device *rdev,
+		      struct radeon_sync *sync,
+		      int waiting_ring);
+void radeon_sync_free(struct radeon_device *rdev, struct radeon_sync *sync,
+		      struct radeon_fence *fence);
+
+/*
  * GART structures, functions & helpers
  */
 struct radeon_mc;
@@ -701,6 +721,10 @@ struct radeon_doorbell {
 
 int radeon_doorbell_get(struct radeon_device *rdev, u32 *page);
 void radeon_doorbell_free(struct radeon_device *rdev, u32 doorbell);
+void radeon_doorbell_get_kfd_info(struct radeon_device *rdev,
+				  phys_addr_t *aperture_base,
+				  size_t *aperture_size,
+				  size_t *start_offset);
 
 /*
  * IRQS.
@@ -814,7 +838,7 @@ struct radeon_ib {
 	struct radeon_fence		*fence;
 	struct radeon_vm		*vm;
 	bool				is_const_ib;
-	struct radeon_semaphore		*semaphore;
+	struct radeon_sync		sync;
 };
 
 struct radeon_ring {
@@ -891,33 +915,40 @@ struct radeon_vm_pt {
 	uint64_t			addr;
 };
 
+struct radeon_vm_id {
+	unsigned		id;
+	uint64_t		pd_gpu_addr;
+	/* last flushed PD/PT update */
+	struct radeon_fence	*flushed_updates;
+	/* last use of vmid */
+	struct radeon_fence	*last_id_use;
+};
+
 struct radeon_vm {
-	struct rb_root			va;
-	unsigned			id;
+	struct mutex		mutex;
+
+	struct rb_root		va;
+
+	/* protecting invalidated and freed */
+	spinlock_t		status_lock;
 
 	/* BOs moved, but not yet updated in the PT */
-	struct list_head		invalidated;
+	struct list_head	invalidated;
 
 	/* BOs freed, but not yet updated in the PT */
-	struct list_head		freed;
+	struct list_head	freed;
 
 	/* contains the page directory */
-	struct radeon_bo		*page_directory;
-	uint64_t			pd_gpu_addr;
-	unsigned			max_pde_used;
+	struct radeon_bo	*page_directory;
+	unsigned		max_pde_used;
 
 	/* array of page tables, one for each page directory entry */
-	struct radeon_vm_pt		*page_tables;
+	struct radeon_vm_pt	*page_tables;
 
-	struct radeon_bo_va		*ib_bo_va;
+	struct radeon_bo_va	*ib_bo_va;
 
-	struct mutex			mutex;
-	/* last fence for cs using this vm */
-	struct radeon_fence		*fence;
-	/* last flush or NULL if we still need to flush */
-	struct radeon_fence		*last_flush;
-	/* last use of vmid */
-	struct radeon_fence		*last_id_use;
+	/* for id and flush management per ring */
+	struct radeon_vm_id	ids[RADEON_NUM_RINGS];
 };
 
 struct radeon_vm_manager {
@@ -1025,19 +1056,7 @@ void cayman_dma_fini(struct radeon_device *rdev);
 /*
  * CS.
  */
-struct radeon_cs_reloc {
-	struct drm_gem_object		*gobj;
-	struct radeon_bo		*robj;
-	struct ttm_validate_buffer	tv;
-	uint64_t			gpu_offset;
-	unsigned			prefered_domains;
-	unsigned			allowed_domains;
-	uint32_t			tiling_flags;
-	uint32_t			handle;
-};
-
 struct radeon_cs_chunk {
-	uint32_t		chunk_id;
 	uint32_t		length_dw;
 	uint32_t		*kdata;
 	void __user		*user_ptr;
@@ -1055,16 +1074,15 @@ struct radeon_cs_parser {
 	unsigned		idx;
 	/* relocations */
 	unsigned		nrelocs;
-	struct radeon_cs_reloc	*relocs;
-	struct radeon_cs_reloc	**relocs_ptr;
-	struct radeon_cs_reloc	*vm_bos;
+	struct radeon_bo_list	*relocs;
+	struct radeon_bo_list	*vm_bos;
 	struct list_head	validated;
 	unsigned		dma_reloc_idx;
 	/* indices of various chunks */
-	int			chunk_ib_idx;
-	int			chunk_relocs_idx;
-	int			chunk_flags_idx;
-	int			chunk_const_ib_idx;
+	struct radeon_cs_chunk  *chunk_ib;
+	struct radeon_cs_chunk  *chunk_relocs;
+	struct radeon_cs_chunk  *chunk_flags;
+	struct radeon_cs_chunk  *chunk_const_ib;
 	struct radeon_ib	ib;
 	struct radeon_ib	const_ib;
 	void			*track;
@@ -1078,7 +1096,7 @@ struct radeon_cs_parser {
 
 static inline u32 radeon_get_ib_value(struct radeon_cs_parser *p, int idx)
 {
-	struct radeon_cs_chunk *ibc = &p->chunks[p->chunk_ib_idx];
+	struct radeon_cs_chunk *ibc = p->chunk_ib;
 
 	if (ibc->kdata)
 		return ibc->kdata[idx];
@@ -1490,6 +1508,10 @@ struct radeon_dpm_fan {
 	u8 t_hyst;
 	u32 cycle_delay;
 	u16 t_max;
+	u8 control_mode;
+	u16 default_max_fan_pwm;
+	u16 default_fan_output_sensitivity;
+	u16 fan_output_sensitivity;
 	bool ucode_fan_control;
 };
 
@@ -1623,6 +1645,11 @@ struct radeon_pm {
 	/* internal thermal controller on rv6xx+ */
 	enum radeon_int_thermal_type int_thermal_type;
 	struct device	        *int_hwmon_dev;
+	/* fan control parameters */
+	bool                    no_fan;
+	u8                      fan_pulses_per_revolution;
+	u8                      fan_min_rpm;
+	u8                      fan_max_rpm;
 	/* dpm */
 	bool                    dpm_enabled;
 	struct radeon_dpm       dpm;
@@ -1785,7 +1812,8 @@ struct radeon_asic_ring {
 	void (*hdp_flush)(struct radeon_device *rdev, struct radeon_ring *ring);
 	bool (*emit_semaphore)(struct radeon_device *rdev, struct radeon_ring *cp,
 			       struct radeon_semaphore *semaphore, bool emit_wait);
-	void (*vm_flush)(struct radeon_device *rdev, int ridx, struct radeon_vm *vm);
+	void (*vm_flush)(struct radeon_device *rdev, struct radeon_ring *ring,
+			 unsigned vm_id, uint64_t pd_addr);
 
 	/* testing functions */
 	int (*ring_test)(struct radeon_device *rdev, struct radeon_ring *cp);
@@ -2388,6 +2416,8 @@ struct radeon_device {
 	struct radeon_atcs		atcs;
 	/* srbm instance registers */
 	struct mutex			srbm_mutex;
+	/* GRBM index mutex. Protects concurrents access to GRBM index */
+	struct mutex			grbm_idx_mutex;
 	/* clock, powergating flags */
 	u32 cg_flags;
 	u32 pg_flags;
@@ -2400,6 +2430,10 @@ struct radeon_device {
 	u64 vram_pin_size;
 	u64 gart_pin_size;
 
+	/* amdkfd interface */
+	struct kfd_dev		*kfd;
+	struct radeon_sa_manager	kfd_bo;
+
 	struct mutex	mn_lock;
 	DECLARE_HASHTABLE(mn_hash, 7);
 };
@@ -2831,7 +2865,7 @@ static inline void radeon_ring_write(struct radeon_ring *ring, uint32_t v)
 #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_ring_vm_flush(rdev, r, vm) (rdev)->asic->ring[(r)]->vm_flush((rdev), (r), (vm))
+#define radeon_ring_vm_flush(rdev, r, vm_id, pd_addr) (rdev)->asic->ring[(r)->idx]->vm_flush((rdev), (r), (vm_id), (pd_addr))
 #define radeon_ring_get_rptr(rdev, r) (rdev)->asic->ring[(r)->idx]->get_rptr((rdev), (r))
 #define radeon_ring_get_wptr(rdev, r) (rdev)->asic->ring[(r)->idx]->get_wptr((rdev), (r))
 #define radeon_ring_set_wptr(rdev, r) (rdev)->asic->ring[(r)->idx]->set_wptr((rdev), (r))
@@ -2940,14 +2974,14 @@ int radeon_vm_manager_init(struct radeon_device *rdev);
 void radeon_vm_manager_fini(struct radeon_device *rdev);
 int radeon_vm_init(struct radeon_device *rdev, struct radeon_vm *vm);
 void radeon_vm_fini(struct radeon_device *rdev, struct radeon_vm *vm);
-struct radeon_cs_reloc *radeon_vm_get_bos(struct radeon_device *rdev,
+struct radeon_bo_list *radeon_vm_get_bos(struct radeon_device *rdev,
 					  struct radeon_vm *vm,
                                           struct list_head *head);
 struct radeon_fence *radeon_vm_grab_id(struct radeon_device *rdev,
 				       struct radeon_vm *vm, int ring);
 void radeon_vm_flush(struct radeon_device *rdev,
                      struct radeon_vm *vm,
-                     int ring);
+		     int ring, struct radeon_fence *fence);
 void radeon_vm_fence(struct radeon_device *rdev,
 		     struct radeon_vm *vm,
 		     struct radeon_fence *fence);
@@ -3054,7 +3088,7 @@ bool radeon_cs_packet_next_is_pkt3_nop(struct radeon_cs_parser *p);
 void radeon_cs_dump_packet(struct radeon_cs_parser *p,
 			   struct radeon_cs_packet *pkt);
 int radeon_cs_packet_next_reloc(struct radeon_cs_parser *p,
-				struct radeon_cs_reloc **cs_reloc,
+				struct radeon_bo_list **cs_reloc,
 				int nomm);
 int r600_cs_common_vline_parse(struct radeon_cs_parser *p,
 			       uint32_t *vline_start_end,
diff --git a/drivers/gpu/drm/radeon/radeon_asic.h b/drivers/gpu/drm/radeon/radeon_asic.h
index d8ace5b28a5b..2a45d548d5ec 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.h
+++ b/drivers/gpu/drm/radeon/radeon_asic.h
@@ -599,7 +599,8 @@ 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);
 void cayman_vm_fini(struct radeon_device *rdev);
-void cayman_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm);
+void cayman_vm_flush(struct radeon_device *rdev, struct radeon_ring *ring,
+		     unsigned vm_id, uint64_t pd_addr);
 uint32_t cayman_vm_page_flags(struct radeon_device *rdev, uint32_t flags);
 int evergreen_ib_parse(struct radeon_device *rdev, struct radeon_ib *ib);
 int evergreen_dma_ib_parse(struct radeon_device *rdev, struct radeon_ib *ib);
@@ -624,7 +625,8 @@ void cayman_dma_vm_set_pages(struct radeon_device *rdev,
 			     uint32_t incr, uint32_t flags);
 void cayman_dma_vm_pad_ib(struct radeon_ib *ib);
 
-void cayman_dma_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm);
+void cayman_dma_vm_flush(struct radeon_device *rdev, struct radeon_ring *ring,
+			 unsigned vm_id, uint64_t pd_addr);
 
 u32 cayman_gfx_get_rptr(struct radeon_device *rdev,
 			struct radeon_ring *ring);
@@ -699,7 +701,8 @@ int si_irq_set(struct radeon_device *rdev);
 int si_irq_process(struct radeon_device *rdev);
 int si_vm_init(struct radeon_device *rdev);
 void si_vm_fini(struct radeon_device *rdev);
-void si_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm);
+void si_vm_flush(struct radeon_device *rdev, struct radeon_ring *ring,
+		 unsigned vm_id, uint64_t pd_addr);
 int si_ib_parse(struct radeon_device *rdev, struct radeon_ib *ib);
 struct radeon_fence *si_copy_dma(struct radeon_device *rdev,
 				 uint64_t src_offset, uint64_t dst_offset,
@@ -721,7 +724,8 @@ void si_dma_vm_set_pages(struct radeon_device *rdev,
 			 uint64_t addr, unsigned count,
 			 uint32_t incr, uint32_t flags);
 
-void si_dma_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm);
+void si_dma_vm_flush(struct radeon_device *rdev, struct radeon_ring *ring,
+		     unsigned vm_id, uint64_t pd_addr);
 u32 si_get_xclk(struct radeon_device *rdev);
 uint64_t si_get_gpu_clock_counter(struct radeon_device *rdev);
 int si_set_uvd_clocks(struct radeon_device *rdev, u32 vclk, u32 dclk);
@@ -793,7 +797,8 @@ int cik_irq_set(struct radeon_device *rdev);
 int cik_irq_process(struct radeon_device *rdev);
 int cik_vm_init(struct radeon_device *rdev);
 void cik_vm_fini(struct radeon_device *rdev);
-void cik_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm);
+void cik_vm_flush(struct radeon_device *rdev, struct radeon_ring *ring,
+		  unsigned vm_id, uint64_t pd_addr);
 
 void cik_sdma_vm_copy_pages(struct radeon_device *rdev,
 			    struct radeon_ib *ib,
@@ -811,7 +816,8 @@ void cik_sdma_vm_set_pages(struct radeon_device *rdev,
 			   uint32_t incr, uint32_t flags);
 void cik_sdma_vm_pad_ib(struct radeon_ib *ib);
 
-void cik_dma_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm);
+void cik_dma_vm_flush(struct radeon_device *rdev, struct radeon_ring *ring,
+		      unsigned vm_id, uint64_t pd_addr);
 int cik_ib_parse(struct radeon_device *rdev, struct radeon_ib *ib);
 u32 cik_gfx_get_rptr(struct radeon_device *rdev,
 		     struct radeon_ring *ring);
diff --git a/drivers/gpu/drm/radeon/radeon_atombios.c b/drivers/gpu/drm/radeon/radeon_atombios.c
index df69b92ba164..dbc94f300297 100644
--- a/drivers/gpu/drm/radeon/radeon_atombios.c
+++ b/drivers/gpu/drm/radeon/radeon_atombios.c
@@ -196,8 +196,8 @@ void radeon_atombios_i2c_init(struct radeon_device *rdev)
 	}
 }
 
-static struct radeon_gpio_rec radeon_lookup_gpio(struct radeon_device *rdev,
-						 u8 id)
+struct radeon_gpio_rec radeon_atombios_lookup_gpio(struct radeon_device *rdev,
+						   u8 id)
 {
 	struct atom_context *ctx = rdev->mode_info.atom_context;
 	struct radeon_gpio_rec gpio;
@@ -221,6 +221,7 @@ static struct radeon_gpio_rec radeon_lookup_gpio(struct radeon_device *rdev,
 			if (id == pin->ucGPIO_ID) {
 				gpio.id = pin->ucGPIO_ID;
 				gpio.reg = le16_to_cpu(pin->usGpioPin_AIndex) * 4;
+				gpio.shift = pin->ucGpioPinBitShift;
 				gpio.mask = (1 << pin->ucGpioPinBitShift);
 				gpio.valid = true;
 				break;
@@ -801,7 +802,7 @@ bool radeon_get_atom_connector_info_from_object_table(struct drm_device *dev)
 								hpd_record =
 									(ATOM_HPD_INT_RECORD *)
 									record;
-								gpio = radeon_lookup_gpio(rdev,
+								gpio = radeon_atombios_lookup_gpio(rdev,
 											  hpd_record->ucHPDIntGPIOID);
 								hpd = radeon_atom_get_hpd_info_from_gpio(rdev, &gpio);
 								hpd.plugged_state = hpd_record->ucPlugged_PinState;
@@ -2128,7 +2129,7 @@ static int radeon_atombios_parse_power_table_1_3(struct radeon_device *rdev)
 				rdev->pm.power_state[state_index].clock_info[0].voltage.type =
 					VOLTAGE_GPIO;
 				rdev->pm.power_state[state_index].clock_info[0].voltage.gpio =
-					radeon_lookup_gpio(rdev,
+					radeon_atombios_lookup_gpio(rdev,
 							   power_info->info.asPowerPlayInfo[i].ucVoltageDropIndex);
 				if (misc & ATOM_PM_MISCINFO_VOLTAGE_DROP_ACTIVE_HIGH)
 					rdev->pm.power_state[state_index].clock_info[0].voltage.active_high =
@@ -2164,7 +2165,7 @@ static int radeon_atombios_parse_power_table_1_3(struct radeon_device *rdev)
 				rdev->pm.power_state[state_index].clock_info[0].voltage.type =
 					VOLTAGE_GPIO;
 				rdev->pm.power_state[state_index].clock_info[0].voltage.gpio =
-					radeon_lookup_gpio(rdev,
+					radeon_atombios_lookup_gpio(rdev,
 							   power_info->info_2.asPowerPlayInfo[i].ucVoltageDropIndex);
 				if (misc & ATOM_PM_MISCINFO_VOLTAGE_DROP_ACTIVE_HIGH)
 					rdev->pm.power_state[state_index].clock_info[0].voltage.active_high =
@@ -2200,7 +2201,7 @@ static int radeon_atombios_parse_power_table_1_3(struct radeon_device *rdev)
 				rdev->pm.power_state[state_index].clock_info[0].voltage.type =
 					VOLTAGE_GPIO;
 				rdev->pm.power_state[state_index].clock_info[0].voltage.gpio =
-					radeon_lookup_gpio(rdev,
+					radeon_atombios_lookup_gpio(rdev,
 							   power_info->info_3.asPowerPlayInfo[i].ucVoltageDropIndex);
 				if (misc & ATOM_PM_MISCINFO_VOLTAGE_DROP_ACTIVE_HIGH)
 					rdev->pm.power_state[state_index].clock_info[0].voltage.active_high =
@@ -2248,6 +2249,14 @@ static void radeon_atombios_add_pplib_thermal_controller(struct radeon_device *r
 
 	/* add the i2c bus for thermal/fan chip */
 	if (controller->ucType > 0) {
+		if (controller->ucFanParameters & ATOM_PP_FANPARAMETERS_NOFAN)
+			rdev->pm.no_fan = true;
+		rdev->pm.fan_pulses_per_revolution =
+			controller->ucFanParameters & ATOM_PP_FANPARAMETERS_TACHOMETER_PULSES_PER_REVOLUTION_MASK;
+		if (rdev->pm.fan_pulses_per_revolution) {
+			rdev->pm.fan_min_rpm = controller->ucFanMinRPM;
+			rdev->pm.fan_max_rpm = controller->ucFanMaxRPM;
+		}
 		if (controller->ucType == ATOM_PP_THERMALCONTROLLER_RV6xx) {
 			DRM_INFO("Internal thermal controller %s fan control\n",
 				 (controller->ucFanParameters &
diff --git a/drivers/gpu/drm/radeon/radeon_cs.c b/drivers/gpu/drm/radeon/radeon_cs.c
index 6f377de099f9..c830863bc98a 100644
--- a/drivers/gpu/drm/radeon/radeon_cs.c
+++ b/drivers/gpu/drm/radeon/radeon_cs.c
@@ -77,22 +77,18 @@ static int radeon_cs_parser_relocs(struct radeon_cs_parser *p)
 	struct drm_device *ddev = p->rdev->ddev;
 	struct radeon_cs_chunk *chunk;
 	struct radeon_cs_buckets buckets;
-	unsigned i, j;
-	bool duplicate, need_mmap_lock = false;
+	unsigned i;
+	bool need_mmap_lock = false;
 	int r;
 
-	if (p->chunk_relocs_idx == -1) {
+	if (p->chunk_relocs == NULL) {
 		return 0;
 	}
-	chunk = &p->chunks[p->chunk_relocs_idx];
+	chunk = p->chunk_relocs;
 	p->dma_reloc_idx = 0;
 	/* FIXME: we assume that each relocs use 4 dwords */
 	p->nrelocs = chunk->length_dw / 4;
-	p->relocs_ptr = kcalloc(p->nrelocs, sizeof(void *), GFP_KERNEL);
-	if (p->relocs_ptr == NULL) {
-		return -ENOMEM;
-	}
-	p->relocs = kcalloc(p->nrelocs, sizeof(struct radeon_cs_reloc), GFP_KERNEL);
+	p->relocs = kcalloc(p->nrelocs, sizeof(struct radeon_bo_list), GFP_KERNEL);
 	if (p->relocs == NULL) {
 		return -ENOMEM;
 	}
@@ -101,31 +97,17 @@ static int radeon_cs_parser_relocs(struct radeon_cs_parser *p)
 
 	for (i = 0; i < p->nrelocs; i++) {
 		struct drm_radeon_cs_reloc *r;
+		struct drm_gem_object *gobj;
 		unsigned priority;
 
-		duplicate = false;
 		r = (struct drm_radeon_cs_reloc *)&chunk->kdata[i*4];
-		for (j = 0; j < i; j++) {
-			if (r->handle == p->relocs[j].handle) {
-				p->relocs_ptr[i] = &p->relocs[j];
-				duplicate = true;
-				break;
-			}
-		}
-		if (duplicate) {
-			p->relocs[i].handle = 0;
-			continue;
-		}
-
-		p->relocs[i].gobj = drm_gem_object_lookup(ddev, p->filp,
-							  r->handle);
-		if (p->relocs[i].gobj == NULL) {
+		gobj = drm_gem_object_lookup(ddev, p->filp, r->handle);
+		if (gobj == NULL) {
 			DRM_ERROR("gem object lookup failed 0x%x\n",
 				  r->handle);
 			return -ENOENT;
 		}
-		p->relocs_ptr[i] = &p->relocs[i];
-		p->relocs[i].robj = gem_to_radeon_bo(p->relocs[i].gobj);
+		p->relocs[i].robj = gem_to_radeon_bo(gobj);
 
 		/* The userspace buffer priorities are from 0 to 15. A higher
 		 * number means the buffer is more important.
@@ -184,7 +166,6 @@ static int radeon_cs_parser_relocs(struct radeon_cs_parser *p)
 
 		p->relocs[i].tv.bo = &p->relocs[i].robj->tbo;
 		p->relocs[i].tv.shared = !r->write_domain;
-		p->relocs[i].handle = r->handle;
 
 		radeon_cs_buckets_add(&buckets, &p->relocs[i].tv.head,
 				      priority);
@@ -251,15 +232,15 @@ 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)
 {
-	struct radeon_cs_reloc *reloc;
+	struct radeon_bo_list *reloc;
 	int r;
 
 	list_for_each_entry(reloc, &p->validated, tv.head) {
 		struct reservation_object *resv;
 
 		resv = reloc->robj->tbo.resv;
-		r = radeon_semaphore_sync_resv(p->rdev, p->ib.semaphore, resv,
-					       reloc->tv.shared);
+		r = radeon_sync_resv(p->rdev, &p->ib.sync, resv,
+				     reloc->tv.shared);
 		if (r)
 			return r;
 	}
@@ -282,13 +263,11 @@ int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
 	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;
-	p->chunk_const_ib_idx = -1;
+	p->chunk_ib = NULL;
+	p->chunk_relocs = NULL;
+	p->chunk_flags = NULL;
+	p->chunk_const_ib = NULL;
 	p->chunks_array = kcalloc(cs->num_chunks, sizeof(uint64_t), GFP_KERNEL);
 	if (p->chunks_array == NULL) {
 		return -ENOMEM;
@@ -315,24 +294,23 @@ int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
 			return -EFAULT;
 		}
 		p->chunks[i].length_dw = user_chunk.length_dw;
-		p->chunks[i].chunk_id = user_chunk.chunk_id;
-		if (p->chunks[i].chunk_id == RADEON_CHUNK_ID_RELOCS) {
-			p->chunk_relocs_idx = i;
+		if (user_chunk.chunk_id == RADEON_CHUNK_ID_RELOCS) {
+			p->chunk_relocs = &p->chunks[i];
 		}
-		if (p->chunks[i].chunk_id == RADEON_CHUNK_ID_IB) {
-			p->chunk_ib_idx = i;
+		if (user_chunk.chunk_id == RADEON_CHUNK_ID_IB) {
+			p->chunk_ib = &p->chunks[i];
 			/* zero length IB isn't useful */
 			if (p->chunks[i].length_dw == 0)
 				return -EINVAL;
 		}
-		if (p->chunks[i].chunk_id == RADEON_CHUNK_ID_CONST_IB) {
-			p->chunk_const_ib_idx = i;
+		if (user_chunk.chunk_id == RADEON_CHUNK_ID_CONST_IB) {
+			p->chunk_const_ib = &p->chunks[i];
 			/* zero length CONST IB isn't useful */
 			if (p->chunks[i].length_dw == 0)
 				return -EINVAL;
 		}
-		if (p->chunks[i].chunk_id == RADEON_CHUNK_ID_FLAGS) {
-			p->chunk_flags_idx = i;
+		if (user_chunk.chunk_id == RADEON_CHUNK_ID_FLAGS) {
+			p->chunk_flags = &p->chunks[i];
 			/* zero length flags aren't useful */
 			if (p->chunks[i].length_dw == 0)
 				return -EINVAL;
@@ -341,10 +319,10 @@ int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
 		size = p->chunks[i].length_dw;
 		cdata = (void __user *)(unsigned long)user_chunk.chunk_data;
 		p->chunks[i].user_ptr = cdata;
-		if (p->chunks[i].chunk_id == RADEON_CHUNK_ID_CONST_IB)
+		if (user_chunk.chunk_id == RADEON_CHUNK_ID_CONST_IB)
 			continue;
 
-		if (p->chunks[i].chunk_id == RADEON_CHUNK_ID_IB) {
+		if (user_chunk.chunk_id == RADEON_CHUNK_ID_IB) {
 			if (!p->rdev || !(p->rdev->flags & RADEON_IS_AGP))
 				continue;
 		}
@@ -357,7 +335,7 @@ int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
 		if (copy_from_user(p->chunks[i].kdata, cdata, size)) {
 			return -EFAULT;
 		}
-		if (p->chunks[i].chunk_id == RADEON_CHUNK_ID_FLAGS) {
+		if (user_chunk.chunk_id == RADEON_CHUNK_ID_FLAGS) {
 			p->cs_flags = p->chunks[i].kdata[0];
 			if (p->chunks[i].length_dw > 1)
 				ring = p->chunks[i].kdata[1];
@@ -398,8 +376,8 @@ int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
 static int cmp_size_smaller_first(void *priv, struct list_head *a,
 				  struct list_head *b)
 {
-	struct radeon_cs_reloc *la = list_entry(a, struct radeon_cs_reloc, tv.head);
-	struct radeon_cs_reloc *lb = list_entry(b, struct radeon_cs_reloc, tv.head);
+	struct radeon_bo_list *la = list_entry(a, struct radeon_bo_list, tv.head);
+	struct radeon_bo_list *lb = list_entry(b, struct radeon_bo_list, tv.head);
 
 	/* Sort A before B if A is smaller. */
 	return (int)la->robj->tbo.num_pages - (int)lb->robj->tbo.num_pages;
@@ -440,13 +418,15 @@ static void radeon_cs_parser_fini(struct radeon_cs_parser *parser, int error, bo
 
 	if (parser->relocs != NULL) {
 		for (i = 0; i < parser->nrelocs; i++) {
-			if (parser->relocs[i].gobj)
-				drm_gem_object_unreference_unlocked(parser->relocs[i].gobj);
+			struct radeon_bo *bo = parser->relocs[i].robj;
+			if (bo == NULL)
+				continue;
+
+			drm_gem_object_unreference_unlocked(&bo->gem_base);
 		}
 	}
 	kfree(parser->track);
 	kfree(parser->relocs);
-	kfree(parser->relocs_ptr);
 	drm_free_large(parser->vm_bos);
 	for (i = 0; i < parser->nchunks; i++)
 		drm_free_large(parser->chunks[i].kdata);
@@ -461,7 +441,7 @@ static int radeon_cs_ib_chunk(struct radeon_device *rdev,
 {
 	int r;
 
-	if (parser->chunk_ib_idx == -1)
+	if (parser->chunk_ib == NULL)
 		return 0;
 
 	if (parser->cs_flags & RADEON_CS_USE_VM)
@@ -521,10 +501,6 @@ static int radeon_bo_vm_update_pte(struct radeon_cs_parser *p,
 	for (i = 0; i < p->nrelocs; i++) {
 		struct radeon_bo *bo;
 
-		/* ignore duplicates */
-		if (p->relocs_ptr[i] != &p->relocs[i])
-			continue;
-
 		bo = p->relocs[i].robj;
 		bo_va = radeon_vm_bo_find(vm, bo);
 		if (bo_va == NULL) {
@@ -535,6 +511,8 @@ static int radeon_bo_vm_update_pte(struct radeon_cs_parser *p,
 		r = radeon_vm_bo_update(rdev, bo_va, &bo->tbo.mem);
 		if (r)
 			return r;
+
+		radeon_sync_fence(&p->ib.sync, bo_va->last_pt_update);
 	}
 
 	return radeon_vm_clear_invalids(rdev, vm);
@@ -547,7 +525,7 @@ static int radeon_cs_ib_vm_chunk(struct radeon_device *rdev,
 	struct radeon_vm *vm = &fpriv->vm;
 	int r;
 
-	if (parser->chunk_ib_idx == -1)
+	if (parser->chunk_ib == NULL)
 		return 0;
 	if ((parser->cs_flags & RADEON_CS_USE_VM) == 0)
 		return 0;
@@ -579,10 +557,9 @@ static int radeon_cs_ib_vm_chunk(struct radeon_device *rdev,
 			DRM_ERROR("Failed to sync rings: %i\n", r);
 		goto out;
 	}
-	radeon_semaphore_sync_fence(parser->ib.semaphore, vm->fence);
 
 	if ((rdev->family >= CHIP_TAHITI) &&
-	    (parser->chunk_const_ib_idx != -1)) {
+	    (parser->chunk_const_ib != NULL)) {
 		r = radeon_ib_schedule(rdev, &parser->ib, &parser->const_ib, true);
 	} else {
 		r = radeon_ib_schedule(rdev, &parser->ib, NULL, true);
@@ -609,7 +586,7 @@ static int radeon_cs_ib_fill(struct radeon_device *rdev, struct radeon_cs_parser
 	struct radeon_vm *vm = NULL;
 	int r;
 
-	if (parser->chunk_ib_idx == -1)
+	if (parser->chunk_ib == NULL)
 		return 0;
 
 	if (parser->cs_flags & RADEON_CS_USE_VM) {
@@ -617,8 +594,8 @@ static int radeon_cs_ib_fill(struct radeon_device *rdev, struct radeon_cs_parser
 		vm = &fpriv->vm;
 
 		if ((rdev->family >= CHIP_TAHITI) &&
-		    (parser->chunk_const_ib_idx != -1)) {
-			ib_chunk = &parser->chunks[parser->chunk_const_ib_idx];
+		    (parser->chunk_const_ib != NULL)) {
+			ib_chunk = parser->chunk_const_ib;
 			if (ib_chunk->length_dw > RADEON_IB_VM_MAX_SIZE) {
 				DRM_ERROR("cs IB CONST too big: %d\n", ib_chunk->length_dw);
 				return -EINVAL;
@@ -637,13 +614,13 @@ static int radeon_cs_ib_fill(struct radeon_device *rdev, struct radeon_cs_parser
 				return -EFAULT;
 		}
 
-		ib_chunk = &parser->chunks[parser->chunk_ib_idx];
+		ib_chunk = parser->chunk_ib;
 		if (ib_chunk->length_dw > RADEON_IB_VM_MAX_SIZE) {
 			DRM_ERROR("cs IB too big: %d\n", ib_chunk->length_dw);
 			return -EINVAL;
 		}
 	}
-	ib_chunk = &parser->chunks[parser->chunk_ib_idx];
+	ib_chunk = parser->chunk_ib;
 
 	r =  radeon_ib_get(rdev, parser->ring, &parser->ib,
 			   vm, ib_chunk->length_dw * 4);
@@ -735,7 +712,7 @@ int radeon_cs_packet_parse(struct radeon_cs_parser *p,
 			   struct radeon_cs_packet *pkt,
 			   unsigned idx)
 {
-	struct radeon_cs_chunk *ib_chunk = &p->chunks[p->chunk_ib_idx];
+	struct radeon_cs_chunk *ib_chunk = p->chunk_ib;
 	struct radeon_device *rdev = p->rdev;
 	uint32_t header;
 
@@ -829,7 +806,7 @@ void radeon_cs_dump_packet(struct radeon_cs_parser *p,
  * GPU offset using the provided start.
  **/
 int radeon_cs_packet_next_reloc(struct radeon_cs_parser *p,
-				struct radeon_cs_reloc **cs_reloc,
+				struct radeon_bo_list **cs_reloc,
 				int nomm)
 {
 	struct radeon_cs_chunk *relocs_chunk;
@@ -837,12 +814,12 @@ int radeon_cs_packet_next_reloc(struct radeon_cs_parser *p,
 	unsigned idx;
 	int r;
 
-	if (p->chunk_relocs_idx == -1) {
+	if (p->chunk_relocs == NULL) {
 		DRM_ERROR("No relocation chunk !\n");
 		return -EINVAL;
 	}
 	*cs_reloc = NULL;
-	relocs_chunk = &p->chunks[p->chunk_relocs_idx];
+	relocs_chunk = p->chunk_relocs;
 	r = radeon_cs_packet_parse(p, &p3reloc, p->idx);
 	if (r)
 		return r;
@@ -868,6 +845,6 @@ int radeon_cs_packet_next_reloc(struct radeon_cs_parser *p,
 			(u64)relocs_chunk->kdata[idx + 3] << 32;
 		(*cs_reloc)->gpu_offset |= relocs_chunk->kdata[idx + 0];
 	} else
-		*cs_reloc = p->relocs_ptr[(idx / 4)];
+		*cs_reloc = &p->relocs[(idx / 4)];
 	return 0;
 }
diff --git a/drivers/gpu/drm/radeon/radeon_cursor.c b/drivers/gpu/drm/radeon/radeon_cursor.c
index 9630e8d95fb4..45e54060ee97 100644
--- a/drivers/gpu/drm/radeon/radeon_cursor.c
+++ b/drivers/gpu/drm/radeon/radeon_cursor.c
@@ -117,106 +117,7 @@ static void radeon_show_cursor(struct drm_crtc *crtc)
 	}
 }
 
-static void radeon_set_cursor(struct drm_crtc *crtc, struct drm_gem_object *obj,
-			      uint64_t gpu_addr)
-{
-	struct radeon_crtc *radeon_crtc = to_radeon_crtc(crtc);
-	struct radeon_device *rdev = crtc->dev->dev_private;
-
-	if (ASIC_IS_DCE4(rdev)) {
-		WREG32(EVERGREEN_CUR_SURFACE_ADDRESS_HIGH + radeon_crtc->crtc_offset,
-		       upper_32_bits(gpu_addr));
-		WREG32(EVERGREEN_CUR_SURFACE_ADDRESS + radeon_crtc->crtc_offset,
-		       gpu_addr & 0xffffffff);
-	} else if (ASIC_IS_AVIVO(rdev)) {
-		if (rdev->family >= CHIP_RV770) {
-			if (radeon_crtc->crtc_id)
-				WREG32(R700_D2CUR_SURFACE_ADDRESS_HIGH, upper_32_bits(gpu_addr));
-			else
-				WREG32(R700_D1CUR_SURFACE_ADDRESS_HIGH, upper_32_bits(gpu_addr));
-		}
-		WREG32(AVIVO_D1CUR_SURFACE_ADDRESS + radeon_crtc->crtc_offset,
-		       gpu_addr & 0xffffffff);
-	} else {
-		radeon_crtc->legacy_cursor_offset = gpu_addr - radeon_crtc->legacy_display_base_addr;
-		/* offset is from DISP(2)_BASE_ADDRESS */
-		WREG32(RADEON_CUR_OFFSET + radeon_crtc->crtc_offset, radeon_crtc->legacy_cursor_offset);
-	}
-}
-
-int radeon_crtc_cursor_set(struct drm_crtc *crtc,
-			   struct drm_file *file_priv,
-			   uint32_t handle,
-			   uint32_t width,
-			   uint32_t height)
-{
-	struct radeon_crtc *radeon_crtc = to_radeon_crtc(crtc);
-	struct radeon_device *rdev = crtc->dev->dev_private;
-	struct drm_gem_object *obj;
-	struct radeon_bo *robj;
-	uint64_t gpu_addr;
-	int ret;
-
-	if (!handle) {
-		/* turn off cursor */
-		radeon_hide_cursor(crtc);
-		obj = NULL;
-		goto unpin;
-	}
-
-	if ((width > radeon_crtc->max_cursor_width) ||
-	    (height > radeon_crtc->max_cursor_height)) {
-		DRM_ERROR("bad cursor width or height %d x %d\n", width, height);
-		return -EINVAL;
-	}
-
-	obj = drm_gem_object_lookup(crtc->dev, file_priv, handle);
-	if (!obj) {
-		DRM_ERROR("Cannot find cursor object %x for crtc %d\n", handle, radeon_crtc->crtc_id);
-		return -ENOENT;
-	}
-
-	robj = gem_to_radeon_bo(obj);
-	ret = radeon_bo_reserve(robj, false);
-	if (unlikely(ret != 0))
-		goto fail;
-	/* Only 27 bit offset for legacy cursor */
-	ret = radeon_bo_pin_restricted(robj, RADEON_GEM_DOMAIN_VRAM,
-				       ASIC_IS_AVIVO(rdev) ? 0 : 1 << 27,
-				       &gpu_addr);
-	radeon_bo_unreserve(robj);
-	if (ret)
-		goto fail;
-
-	radeon_crtc->cursor_width = width;
-	radeon_crtc->cursor_height = height;
-
-	radeon_lock_cursor(crtc, true);
-	radeon_set_cursor(crtc, obj, gpu_addr);
-	radeon_show_cursor(crtc);
-	radeon_lock_cursor(crtc, false);
-
-unpin:
-	if (radeon_crtc->cursor_bo) {
-		robj = gem_to_radeon_bo(radeon_crtc->cursor_bo);
-		ret = radeon_bo_reserve(robj, false);
-		if (likely(ret == 0)) {
-			radeon_bo_unpin(robj);
-			radeon_bo_unreserve(robj);
-		}
-		drm_gem_object_unreference_unlocked(radeon_crtc->cursor_bo);
-	}
-
-	radeon_crtc->cursor_bo = obj;
-	return 0;
-fail:
-	drm_gem_object_unreference_unlocked(obj);
-
-	return ret;
-}
-
-int radeon_crtc_cursor_move(struct drm_crtc *crtc,
-			    int x, int y)
+static int radeon_cursor_move_locked(struct drm_crtc *crtc, int x, int y)
 {
 	struct radeon_crtc *radeon_crtc = to_radeon_crtc(crtc);
 	struct radeon_device *rdev = crtc->dev->dev_private;
@@ -281,7 +182,6 @@ int radeon_crtc_cursor_move(struct drm_crtc *crtc,
 		}
 	}
 
-	radeon_lock_cursor(crtc, true);
 	if (ASIC_IS_DCE4(rdev)) {
 		WREG32(EVERGREEN_CUR_POSITION + radeon_crtc->crtc_offset, (x << 16) | y);
 		WREG32(EVERGREEN_CUR_HOT_SPOT + radeon_crtc->crtc_offset, (xorigin << 16) | yorigin);
@@ -308,7 +208,173 @@ int radeon_crtc_cursor_move(struct drm_crtc *crtc,
 		WREG32(RADEON_CUR_OFFSET + radeon_crtc->crtc_offset, (radeon_crtc->legacy_cursor_offset +
 								      (yorigin * 256)));
 	}
+
+	radeon_crtc->cursor_x = x;
+	radeon_crtc->cursor_y = y;
+
+	return 0;
+}
+
+int radeon_crtc_cursor_move(struct drm_crtc *crtc,
+			    int x, int y)
+{
+	int ret;
+
+	radeon_lock_cursor(crtc, true);
+	ret = radeon_cursor_move_locked(crtc, x, y);
 	radeon_lock_cursor(crtc, false);
 
+	return ret;
+}
+
+static int radeon_set_cursor(struct drm_crtc *crtc, struct drm_gem_object *obj)
+{
+	struct radeon_crtc *radeon_crtc = to_radeon_crtc(crtc);
+	struct radeon_device *rdev = crtc->dev->dev_private;
+	struct radeon_bo *robj = gem_to_radeon_bo(obj);
+	uint64_t gpu_addr;
+	int ret;
+
+	ret = radeon_bo_reserve(robj, false);
+	if (unlikely(ret != 0))
+		goto fail;
+	/* Only 27 bit offset for legacy cursor */
+	ret = radeon_bo_pin_restricted(robj, RADEON_GEM_DOMAIN_VRAM,
+				       ASIC_IS_AVIVO(rdev) ? 0 : 1 << 27,
+				       &gpu_addr);
+	radeon_bo_unreserve(robj);
+	if (ret)
+		goto fail;
+
+	if (ASIC_IS_DCE4(rdev)) {
+		WREG32(EVERGREEN_CUR_SURFACE_ADDRESS_HIGH + radeon_crtc->crtc_offset,
+		       upper_32_bits(gpu_addr));
+		WREG32(EVERGREEN_CUR_SURFACE_ADDRESS + radeon_crtc->crtc_offset,
+		       gpu_addr & 0xffffffff);
+	} else if (ASIC_IS_AVIVO(rdev)) {
+		if (rdev->family >= CHIP_RV770) {
+			if (radeon_crtc->crtc_id)
+				WREG32(R700_D2CUR_SURFACE_ADDRESS_HIGH, upper_32_bits(gpu_addr));
+			else
+				WREG32(R700_D1CUR_SURFACE_ADDRESS_HIGH, upper_32_bits(gpu_addr));
+		}
+		WREG32(AVIVO_D1CUR_SURFACE_ADDRESS + radeon_crtc->crtc_offset,
+		       gpu_addr & 0xffffffff);
+	} else {
+		radeon_crtc->legacy_cursor_offset = gpu_addr - radeon_crtc->legacy_display_base_addr;
+		/* offset is from DISP(2)_BASE_ADDRESS */
+		WREG32(RADEON_CUR_OFFSET + radeon_crtc->crtc_offset, radeon_crtc->legacy_cursor_offset);
+	}
+
 	return 0;
+
+fail:
+	drm_gem_object_unreference_unlocked(obj);
+
+	return ret;
+}
+
+int radeon_crtc_cursor_set2(struct drm_crtc *crtc,
+			    struct drm_file *file_priv,
+			    uint32_t handle,
+			    uint32_t width,
+			    uint32_t height,
+			    int32_t hot_x,
+			    int32_t hot_y)
+{
+	struct radeon_crtc *radeon_crtc = to_radeon_crtc(crtc);
+	struct drm_gem_object *obj;
+	int ret;
+
+	if (!handle) {
+		/* turn off cursor */
+		radeon_hide_cursor(crtc);
+		obj = NULL;
+		goto unpin;
+	}
+
+	if ((width > radeon_crtc->max_cursor_width) ||
+	    (height > radeon_crtc->max_cursor_height)) {
+		DRM_ERROR("bad cursor width or height %d x %d\n", width, height);
+		return -EINVAL;
+	}
+
+	obj = drm_gem_object_lookup(crtc->dev, file_priv, handle);
+	if (!obj) {
+		DRM_ERROR("Cannot find cursor object %x for crtc %d\n", handle, radeon_crtc->crtc_id);
+		return -ENOENT;
+	}
+
+	radeon_crtc->cursor_width = width;
+	radeon_crtc->cursor_height = height;
+
+	radeon_lock_cursor(crtc, true);
+
+	if (hot_x != radeon_crtc->cursor_hot_x ||
+	    hot_y != radeon_crtc->cursor_hot_y) {
+		int x, y;
+
+		x = radeon_crtc->cursor_x + radeon_crtc->cursor_hot_x - hot_x;
+		y = radeon_crtc->cursor_y + radeon_crtc->cursor_hot_y - hot_y;
+
+		radeon_cursor_move_locked(crtc, x, y);
+
+		radeon_crtc->cursor_hot_x = hot_x;
+		radeon_crtc->cursor_hot_y = hot_y;
+	}
+
+	ret = radeon_set_cursor(crtc, obj);
+
+	if (ret)
+		DRM_ERROR("radeon_set_cursor returned %d, not changing cursor\n",
+			  ret);
+	else
+		radeon_show_cursor(crtc);
+
+	radeon_lock_cursor(crtc, false);
+
+unpin:
+	if (radeon_crtc->cursor_bo) {
+		struct radeon_bo *robj = gem_to_radeon_bo(radeon_crtc->cursor_bo);
+		ret = radeon_bo_reserve(robj, false);
+		if (likely(ret == 0)) {
+			radeon_bo_unpin(robj);
+			radeon_bo_unreserve(robj);
+		}
+		if (radeon_crtc->cursor_bo != obj)
+			drm_gem_object_unreference_unlocked(radeon_crtc->cursor_bo);
+	}
+
+	radeon_crtc->cursor_bo = obj;
+	return 0;
+}
+
+/**
+ * radeon_cursor_reset - Re-set the current cursor, if any.
+ *
+ * @crtc: drm crtc
+ *
+ * If the CRTC passed in currently has a cursor assigned, this function
+ * makes sure it's visible.
+ */
+void radeon_cursor_reset(struct drm_crtc *crtc)
+{
+	struct radeon_crtc *radeon_crtc = to_radeon_crtc(crtc);
+	int ret;
+
+	if (radeon_crtc->cursor_bo) {
+		radeon_lock_cursor(crtc, true);
+
+		radeon_cursor_move_locked(crtc, radeon_crtc->cursor_x,
+					  radeon_crtc->cursor_y);
+
+		ret = radeon_set_cursor(crtc, radeon_crtc->cursor_bo);
+		if (ret)
+			DRM_ERROR("radeon_set_cursor returned %d, not showing "
+				  "cursor\n", ret);
+		else
+			radeon_show_cursor(crtc);
+
+		radeon_lock_cursor(crtc, false);
+	}
 }
diff --git a/drivers/gpu/drm/radeon/radeon_device.c b/drivers/gpu/drm/radeon/radeon_device.c
index 995a8b1770dd..0ec65168f331 100644
--- a/drivers/gpu/drm/radeon/radeon_device.c
+++ b/drivers/gpu/drm/radeon/radeon_device.c
@@ -377,6 +377,37 @@ void radeon_doorbell_free(struct radeon_device *rdev, u32 doorbell)
 		__clear_bit(doorbell, rdev->doorbell.used);
 }
 
+/**
+ * radeon_doorbell_get_kfd_info - Report doorbell configuration required to
+ *                                setup KFD
+ *
+ * @rdev: radeon_device pointer
+ * @aperture_base: output returning doorbell aperture base physical address
+ * @aperture_size: output returning doorbell aperture size in bytes
+ * @start_offset: output returning # of doorbell bytes reserved for radeon.
+ *
+ * Radeon and the KFD share the doorbell aperture. Radeon sets it up,
+ * takes doorbells required for its own rings and reports the setup to KFD.
+ * Radeon reserved doorbells are at the start of the doorbell aperture.
+ */
+void radeon_doorbell_get_kfd_info(struct radeon_device *rdev,
+				  phys_addr_t *aperture_base,
+				  size_t *aperture_size,
+				  size_t *start_offset)
+{
+	/* The first num_doorbells are used by radeon.
+	 * KFD takes whatever's left in the aperture. */
+	if (rdev->doorbell.size > rdev->doorbell.num_doorbells * sizeof(u32)) {
+		*aperture_base = rdev->doorbell.base;
+		*aperture_size = rdev->doorbell.size;
+		*start_offset = rdev->doorbell.num_doorbells * sizeof(u32);
+	} else {
+		*aperture_base = 0;
+		*aperture_size = 0;
+		*start_offset = 0;
+	}
+}
+
 /*
  * radeon_wb_*()
  * Writeback is the the method by which the the GPU updates special pages
@@ -1273,6 +1304,7 @@ int radeon_device_init(struct radeon_device *rdev,
 	mutex_init(&rdev->pm.mutex);
 	mutex_init(&rdev->gpu_clock_mutex);
 	mutex_init(&rdev->srbm_mutex);
+	mutex_init(&rdev->grbm_idx_mutex);
 	init_rwsem(&rdev->pm.mclk_lock);
 	init_rwsem(&rdev->exclusive_lock);
 	init_waitqueue_head(&rdev->irq.vblank_queue);
diff --git a/drivers/gpu/drm/radeon/radeon_display.c b/drivers/gpu/drm/radeon/radeon_display.c
index 00ead8c2758a..102116902a07 100644
--- a/drivers/gpu/drm/radeon/radeon_display.c
+++ b/drivers/gpu/drm/radeon/radeon_display.c
@@ -32,6 +32,7 @@
 
 #include <linux/pm_runtime.h>
 #include <drm/drm_crtc_helper.h>
+#include <drm/drm_plane_helper.h>
 #include <drm/drm_edid.h>
 
 #include <linux/gcd.h>
@@ -634,7 +635,7 @@ radeon_crtc_set_config(struct drm_mode_set *set)
 	return ret;
 }
 static const struct drm_crtc_funcs radeon_crtc_funcs = {
-	.cursor_set = radeon_crtc_cursor_set,
+	.cursor_set2 = radeon_crtc_cursor_set2,
 	.cursor_move = radeon_crtc_cursor_move,
 	.gamma_set = radeon_crtc_gamma_set,
 	.set_config = radeon_crtc_set_config,
diff --git a/drivers/gpu/drm/radeon/radeon_drv.c b/drivers/gpu/drm/radeon/radeon_drv.c
index dcffa30ee2db..4f50fb0e3d93 100644
--- a/drivers/gpu/drm/radeon/radeon_drv.c
+++ b/drivers/gpu/drm/radeon/radeon_drv.c
@@ -41,6 +41,8 @@
 #include <drm/drm_gem.h>
 
 #include "drm_crtc_helper.h"
+#include "radeon_kfd.h"
+
 /*
  * KMS wrapper.
  * - 2.0.0 - initial interface
@@ -654,12 +656,15 @@ static int __init radeon_init(void)
 #endif
 	}
 
+	radeon_kfd_init();
+
 	/* let modprobe override vga console setting */
 	return drm_pci_init(driver, pdriver);
 }
 
 static void __exit radeon_exit(void)
 {
+	radeon_kfd_fini();
 	drm_pci_exit(driver, pdriver);
 	radeon_unregister_atpx_handler();
 }
diff --git a/drivers/gpu/drm/radeon/radeon_fb.c b/drivers/gpu/drm/radeon/radeon_fb.c
index 0ea1db83d573..29b9220ec399 100644
--- a/drivers/gpu/drm/radeon/radeon_fb.c
+++ b/drivers/gpu/drm/radeon/radeon_fb.c
@@ -48,10 +48,40 @@ struct radeon_fbdev {
 	struct radeon_device *rdev;
 };
 
+/**
+ * radeon_fb_helper_set_par - Hide cursor on CRTCs used by fbdev.
+ *
+ * @info: fbdev info
+ *
+ * This function hides the cursor on all CRTCs used by fbdev.
+ */
+static int radeon_fb_helper_set_par(struct fb_info *info)
+{
+	int ret;
+
+	ret = drm_fb_helper_set_par(info);
+
+	/* XXX: with universal plane support fbdev will automatically disable
+	 * all non-primary planes (including the cursor)
+	 */
+	if (ret == 0) {
+		struct drm_fb_helper *fb_helper = info->par;
+		int i;
+
+		for (i = 0; i < fb_helper->crtc_count; i++) {
+			struct drm_crtc *crtc = fb_helper->crtc_info[i].mode_set.crtc;
+
+			radeon_crtc_cursor_set2(crtc, NULL, 0, 0, 0, 0, 0);
+		}
+	}
+
+	return ret;
+}
+
 static struct fb_ops radeonfb_ops = {
 	.owner = THIS_MODULE,
 	.fb_check_var = drm_fb_helper_check_var,
-	.fb_set_par = drm_fb_helper_set_par,
+	.fb_set_par = radeon_fb_helper_set_par,
 	.fb_fillrect = cfb_fillrect,
 	.fb_copyarea = cfb_copyarea,
 	.fb_imageblit = cfb_imageblit,
diff --git a/drivers/gpu/drm/radeon/radeon_fence.c b/drivers/gpu/drm/radeon/radeon_fence.c
index 995167025282..d13d1b5a859f 100644
--- a/drivers/gpu/drm/radeon/radeon_fence.c
+++ b/drivers/gpu/drm/radeon/radeon_fence.c
@@ -140,6 +140,7 @@ int radeon_fence_emit(struct radeon_device *rdev,
 	(*fence)->rdev = rdev;
 	(*fence)->seq = seq;
 	(*fence)->ring = ring;
+	(*fence)->is_vm_update = false;
 	fence_init(&(*fence)->base, &radeon_fence_ops,
 		   &rdev->fence_queue.lock, rdev->fence_context + ring, seq);
 	radeon_fence_ring_emit(rdev, ring, *fence);
diff --git a/drivers/gpu/drm/radeon/radeon_gem.c b/drivers/gpu/drm/radeon/radeon_gem.c
index c194497aa586..a46f73737994 100644
--- a/drivers/gpu/drm/radeon/radeon_gem.c
+++ b/drivers/gpu/drm/radeon/radeon_gem.c
@@ -518,6 +518,68 @@ out:
 	return r;
 }
 
+/**
+ * radeon_gem_va_update_vm -update the bo_va in its VM
+ *
+ * @rdev: radeon_device pointer
+ * @bo_va: bo_va to update
+ *
+ * Update the bo_va directly after setting it's address. Errors are not
+ * vital here, so they are not reported back to userspace.
+ */
+static void radeon_gem_va_update_vm(struct radeon_device *rdev,
+				    struct radeon_bo_va *bo_va)
+{
+	struct ttm_validate_buffer tv, *entry;
+	struct radeon_bo_list *vm_bos;
+	struct ww_acquire_ctx ticket;
+	struct list_head list;
+	unsigned domain;
+	int r;
+
+	INIT_LIST_HEAD(&list);
+
+	tv.bo = &bo_va->bo->tbo;
+	tv.shared = true;
+	list_add(&tv.head, &list);
+
+	vm_bos = radeon_vm_get_bos(rdev, bo_va->vm, &list);
+	if (!vm_bos)
+		return;
+
+	r = ttm_eu_reserve_buffers(&ticket, &list, true, NULL);
+	if (r)
+		goto error_free;
+
+	list_for_each_entry(entry, &list, head) {
+		domain = radeon_mem_type_to_domain(entry->bo->mem.mem_type);
+		/* if anything is swapped out don't swap it in here,
+		   just abort and wait for the next CS */
+		if (domain == RADEON_GEM_DOMAIN_CPU)
+			goto error_unreserve;
+	}
+
+	mutex_lock(&bo_va->vm->mutex);
+	r = radeon_vm_clear_freed(rdev, bo_va->vm);
+	if (r)
+		goto error_unlock;
+
+	if (bo_va->it.start)
+		r = radeon_vm_bo_update(rdev, bo_va, &bo_va->bo->tbo.mem);
+
+error_unlock:
+	mutex_unlock(&bo_va->vm->mutex);
+
+error_unreserve:
+	ttm_eu_backoff_reservation(&ticket, &list);
+
+error_free:
+	drm_free_large(vm_bos);
+
+	if (r)
+		DRM_ERROR("Couldn't update BO_VA (%d)\n", r);
+}
+
 int radeon_gem_va_ioctl(struct drm_device *dev, void *data,
 			  struct drm_file *filp)
 {
@@ -601,6 +663,7 @@ int radeon_gem_va_ioctl(struct drm_device *dev, void *data,
 		if (bo_va->it.start) {
 			args->operation = RADEON_VA_RESULT_VA_EXIST;
 			args->offset = bo_va->it.start * RADEON_GPU_PAGE_SIZE;
+			radeon_bo_unreserve(rbo);
 			goto out;
 		}
 		r = radeon_vm_bo_set_addr(rdev, bo_va, args->offset, args->flags);
@@ -611,12 +674,13 @@ int radeon_gem_va_ioctl(struct drm_device *dev, void *data,
 	default:
 		break;
 	}
+	if (!r)
+		radeon_gem_va_update_vm(rdev, bo_va);
 	args->operation = RADEON_VA_RESULT_OK;
 	if (r) {
 		args->operation = RADEON_VA_RESULT_ERROR;
 	}
 out:
-	radeon_bo_unreserve(rbo);
 	drm_gem_object_unreference_unlocked(gobj);
 	return r;
 }
diff --git a/drivers/gpu/drm/radeon/radeon_ib.c b/drivers/gpu/drm/radeon/radeon_ib.c
index 3f39fcca4d07..c39ce1f05703 100644
--- a/drivers/gpu/drm/radeon/radeon_ib.c
+++ b/drivers/gpu/drm/radeon/radeon_ib.c
@@ -64,10 +64,7 @@ int radeon_ib_get(struct radeon_device *rdev, int ring,
 		return r;
 	}
 
-	r = radeon_semaphore_create(rdev, &ib->semaphore);
-	if (r) {
-		return r;
-	}
+	radeon_sync_create(&ib->sync);
 
 	ib->ring = ring;
 	ib->fence = NULL;
@@ -96,7 +93,7 @@ int radeon_ib_get(struct radeon_device *rdev, int ring,
  */
 void radeon_ib_free(struct radeon_device *rdev, struct radeon_ib *ib)
 {
-	radeon_semaphore_free(rdev, &ib->semaphore, ib->fence);
+	radeon_sync_free(rdev, &ib->sync, ib->fence);
 	radeon_sa_bo_free(rdev, &ib->sa_bo, ib->fence);
 	radeon_fence_unref(&ib->fence);
 }
@@ -145,11 +142,11 @@ int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib,
 	if (ib->vm) {
 		struct radeon_fence *vm_id_fence;
 		vm_id_fence = radeon_vm_grab_id(rdev, ib->vm, ib->ring);
-		radeon_semaphore_sync_fence(ib->semaphore, vm_id_fence);
+		radeon_sync_fence(&ib->sync, vm_id_fence);
 	}
 
 	/* sync with other rings */
-	r = radeon_semaphore_sync_rings(rdev, ib->semaphore, ib->ring);
+	r = radeon_sync_rings(rdev, &ib->sync, ib->ring);
 	if (r) {
 		dev_err(rdev->dev, "failed to sync rings (%d)\n", r);
 		radeon_ring_unlock_undo(rdev, ring);
@@ -157,11 +154,12 @@ int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib,
 	}
 
 	if (ib->vm)
-		radeon_vm_flush(rdev, ib->vm, ib->ring);
+		radeon_vm_flush(rdev, ib->vm, ib->ring,
+				ib->sync.last_vm_update);
 
 	if (const_ib) {
 		radeon_ring_ib_execute(rdev, const_ib->ring, const_ib);
-		radeon_semaphore_free(rdev, &const_ib->semaphore, NULL);
+		radeon_sync_free(rdev, &const_ib->sync, NULL);
 	}
 	radeon_ring_ib_execute(rdev, ib->ring, ib);
 	r = radeon_fence_emit(rdev, &ib->fence, ib->ring);
diff --git a/drivers/gpu/drm/radeon/radeon_kfd.c b/drivers/gpu/drm/radeon/radeon_kfd.c
new file mode 100644
index 000000000000..242fd8b1b221
--- /dev/null
+++ b/drivers/gpu/drm/radeon/radeon_kfd.c
@@ -0,0 +1,616 @@
+/*
+ * Copyright 2014 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.
+ */
+
+#include <linux/module.h>
+#include <linux/fdtable.h>
+#include <linux/uaccess.h>
+#include <drm/drmP.h>
+#include "radeon.h"
+#include "cikd.h"
+#include "cik_reg.h"
+#include "radeon_kfd.h"
+#include "radeon_ucode.h"
+#include <linux/firmware.h>
+
+#define CIK_PIPE_PER_MEC	(4)
+
+struct kgd_mem {
+	struct radeon_sa_bo *sa_bo;
+	uint64_t gpu_addr;
+	void *ptr;
+};
+
+static int init_sa_manager(struct kgd_dev *kgd, unsigned int size);
+static void fini_sa_manager(struct kgd_dev *kgd);
+
+static int allocate_mem(struct kgd_dev *kgd, size_t size, size_t alignment,
+		enum kgd_memory_pool pool, struct kgd_mem **mem);
+
+static void free_mem(struct kgd_dev *kgd, struct kgd_mem *mem);
+
+static uint64_t get_vmem_size(struct kgd_dev *kgd);
+static uint64_t get_gpu_clock_counter(struct kgd_dev *kgd);
+
+static uint32_t get_max_engine_clock_in_mhz(struct kgd_dev *kgd);
+static uint16_t get_fw_version(struct kgd_dev *kgd, enum kgd_engine_type type);
+
+/*
+ * Register access functions
+ */
+
+static void kgd_program_sh_mem_settings(struct kgd_dev *kgd, uint32_t vmid,
+		uint32_t sh_mem_config,	uint32_t sh_mem_ape1_base,
+		uint32_t sh_mem_ape1_limit, uint32_t sh_mem_bases);
+
+static int kgd_set_pasid_vmid_mapping(struct kgd_dev *kgd, unsigned int pasid,
+					unsigned int vmid);
+
+static int kgd_init_memory(struct kgd_dev *kgd);
+
+static int kgd_init_pipeline(struct kgd_dev *kgd, uint32_t pipe_id,
+				uint32_t hpd_size, uint64_t hpd_gpu_addr);
+
+static int kgd_hqd_load(struct kgd_dev *kgd, void *mqd, uint32_t pipe_id,
+			uint32_t queue_id, uint32_t __user *wptr);
+
+static bool kgd_hqd_is_occupies(struct kgd_dev *kgd, uint64_t queue_address,
+				uint32_t pipe_id, uint32_t queue_id);
+
+static int kgd_hqd_destroy(struct kgd_dev *kgd, uint32_t reset_type,
+				unsigned int timeout, uint32_t pipe_id,
+				uint32_t queue_id);
+
+static const struct kfd2kgd_calls kfd2kgd = {
+	.init_sa_manager = init_sa_manager,
+	.fini_sa_manager = fini_sa_manager,
+	.allocate_mem = allocate_mem,
+	.free_mem = free_mem,
+	.get_vmem_size = get_vmem_size,
+	.get_gpu_clock_counter = get_gpu_clock_counter,
+	.get_max_engine_clock_in_mhz = get_max_engine_clock_in_mhz,
+	.program_sh_mem_settings = kgd_program_sh_mem_settings,
+	.set_pasid_vmid_mapping = kgd_set_pasid_vmid_mapping,
+	.init_memory = kgd_init_memory,
+	.init_pipeline = kgd_init_pipeline,
+	.hqd_load = kgd_hqd_load,
+	.hqd_is_occupies = kgd_hqd_is_occupies,
+	.hqd_destroy = kgd_hqd_destroy,
+	.get_fw_version = get_fw_version
+};
+
+static const struct kgd2kfd_calls *kgd2kfd;
+
+bool radeon_kfd_init(void)
+{
+	bool (*kgd2kfd_init_p)(unsigned, const struct kfd2kgd_calls*,
+				const struct kgd2kfd_calls**);
+
+	kgd2kfd_init_p = symbol_request(kgd2kfd_init);
+
+	if (kgd2kfd_init_p == NULL)
+		return false;
+
+	if (!kgd2kfd_init_p(KFD_INTERFACE_VERSION, &kfd2kgd, &kgd2kfd)) {
+		symbol_put(kgd2kfd_init);
+		kgd2kfd = NULL;
+
+		return false;
+	}
+
+	return true;
+}
+
+void radeon_kfd_fini(void)
+{
+	if (kgd2kfd) {
+		kgd2kfd->exit();
+		symbol_put(kgd2kfd_init);
+	}
+}
+
+void radeon_kfd_device_probe(struct radeon_device *rdev)
+{
+	if (kgd2kfd)
+		rdev->kfd = kgd2kfd->probe((struct kgd_dev *)rdev, rdev->pdev);
+}
+
+void radeon_kfd_device_init(struct radeon_device *rdev)
+{
+	if (rdev->kfd) {
+		struct kgd2kfd_shared_resources gpu_resources = {
+			.compute_vmid_bitmap = 0xFF00,
+
+			.first_compute_pipe = 1,
+			.compute_pipe_count = 8 - 1,
+		};
+
+		radeon_doorbell_get_kfd_info(rdev,
+				&gpu_resources.doorbell_physical_address,
+				&gpu_resources.doorbell_aperture_size,
+				&gpu_resources.doorbell_start_offset);
+
+		kgd2kfd->device_init(rdev->kfd, &gpu_resources);
+	}
+}
+
+void radeon_kfd_device_fini(struct radeon_device *rdev)
+{
+	if (rdev->kfd) {
+		kgd2kfd->device_exit(rdev->kfd);
+		rdev->kfd = NULL;
+	}
+}
+
+void radeon_kfd_interrupt(struct radeon_device *rdev, const void *ih_ring_entry)
+{
+	if (rdev->kfd)
+		kgd2kfd->interrupt(rdev->kfd, ih_ring_entry);
+}
+
+void radeon_kfd_suspend(struct radeon_device *rdev)
+{
+	if (rdev->kfd)
+		kgd2kfd->suspend(rdev->kfd);
+}
+
+int radeon_kfd_resume(struct radeon_device *rdev)
+{
+	int r = 0;
+
+	if (rdev->kfd)
+		r = kgd2kfd->resume(rdev->kfd);
+
+	return r;
+}
+
+static u32 pool_to_domain(enum kgd_memory_pool p)
+{
+	switch (p) {
+	case KGD_POOL_FRAMEBUFFER: return RADEON_GEM_DOMAIN_VRAM;
+	default: return RADEON_GEM_DOMAIN_GTT;
+	}
+}
+
+static int init_sa_manager(struct kgd_dev *kgd, unsigned int size)
+{
+	struct radeon_device *rdev = (struct radeon_device *)kgd;
+	int r;
+
+	BUG_ON(kgd == NULL);
+
+	r = radeon_sa_bo_manager_init(rdev, &rdev->kfd_bo,
+				      size,
+				      RADEON_GPU_PAGE_SIZE,
+				      RADEON_GEM_DOMAIN_GTT,
+				      RADEON_GEM_GTT_WC);
+
+	if (r)
+		return r;
+
+	r = radeon_sa_bo_manager_start(rdev, &rdev->kfd_bo);
+	if (r)
+		radeon_sa_bo_manager_fini(rdev, &rdev->kfd_bo);
+
+	return r;
+}
+
+static void fini_sa_manager(struct kgd_dev *kgd)
+{
+	struct radeon_device *rdev = (struct radeon_device *)kgd;
+
+	BUG_ON(kgd == NULL);
+
+	radeon_sa_bo_manager_suspend(rdev, &rdev->kfd_bo);
+	radeon_sa_bo_manager_fini(rdev, &rdev->kfd_bo);
+}
+
+static int allocate_mem(struct kgd_dev *kgd, size_t size, size_t alignment,
+		enum kgd_memory_pool pool, struct kgd_mem **mem)
+{
+	struct radeon_device *rdev = (struct radeon_device *)kgd;
+	u32 domain;
+	int r;
+
+	BUG_ON(kgd == NULL);
+
+	domain = pool_to_domain(pool);
+	if (domain != RADEON_GEM_DOMAIN_GTT) {
+		dev_err(rdev->dev,
+			"Only allowed to allocate gart memory for kfd\n");
+		return -EINVAL;
+	}
+
+	*mem = kmalloc(sizeof(struct kgd_mem), GFP_KERNEL);
+	if ((*mem) == NULL)
+		return -ENOMEM;
+
+	r = radeon_sa_bo_new(rdev, &rdev->kfd_bo, &(*mem)->sa_bo, size,
+				alignment);
+	if (r) {
+		dev_err(rdev->dev, "failed to get memory for kfd (%d)\n", r);
+		return r;
+	}
+
+	(*mem)->ptr = radeon_sa_bo_cpu_addr((*mem)->sa_bo);
+	(*mem)->gpu_addr = radeon_sa_bo_gpu_addr((*mem)->sa_bo);
+
+	return 0;
+}
+
+static void free_mem(struct kgd_dev *kgd, struct kgd_mem *mem)
+{
+	struct radeon_device *rdev = (struct radeon_device *)kgd;
+
+	BUG_ON(kgd == NULL);
+
+	radeon_sa_bo_free(rdev, &mem->sa_bo, NULL);
+	kfree(mem);
+}
+
+static uint64_t get_vmem_size(struct kgd_dev *kgd)
+{
+	struct radeon_device *rdev = (struct radeon_device *)kgd;
+
+	BUG_ON(kgd == NULL);
+
+	return rdev->mc.real_vram_size;
+}
+
+static uint64_t get_gpu_clock_counter(struct kgd_dev *kgd)
+{
+	struct radeon_device *rdev = (struct radeon_device *)kgd;
+
+	return rdev->asic->get_gpu_clock_counter(rdev);
+}
+
+static uint32_t get_max_engine_clock_in_mhz(struct kgd_dev *kgd)
+{
+	struct radeon_device *rdev = (struct radeon_device *)kgd;
+
+	/* The sclk is in quantas of 10kHz */
+	return rdev->pm.dpm.dyn_state.max_clock_voltage_on_ac.sclk / 100;
+}
+
+static inline struct radeon_device *get_radeon_device(struct kgd_dev *kgd)
+{
+	return (struct radeon_device *)kgd;
+}
+
+static void write_register(struct kgd_dev *kgd, uint32_t offset, uint32_t value)
+{
+	struct radeon_device *rdev = get_radeon_device(kgd);
+
+	writel(value, (void __iomem *)(rdev->rmmio + offset));
+}
+
+static uint32_t read_register(struct kgd_dev *kgd, uint32_t offset)
+{
+	struct radeon_device *rdev = get_radeon_device(kgd);
+
+	return readl((void __iomem *)(rdev->rmmio + offset));
+}
+
+static void lock_srbm(struct kgd_dev *kgd, uint32_t mec, uint32_t pipe,
+			uint32_t queue, uint32_t vmid)
+{
+	struct radeon_device *rdev = get_radeon_device(kgd);
+	uint32_t value = PIPEID(pipe) | MEID(mec) | VMID(vmid) | QUEUEID(queue);
+
+	mutex_lock(&rdev->srbm_mutex);
+	write_register(kgd, SRBM_GFX_CNTL, value);
+}
+
+static void unlock_srbm(struct kgd_dev *kgd)
+{
+	struct radeon_device *rdev = get_radeon_device(kgd);
+
+	write_register(kgd, SRBM_GFX_CNTL, 0);
+	mutex_unlock(&rdev->srbm_mutex);
+}
+
+static void acquire_queue(struct kgd_dev *kgd, uint32_t pipe_id,
+				uint32_t queue_id)
+{
+	uint32_t mec = (++pipe_id / CIK_PIPE_PER_MEC) + 1;
+	uint32_t pipe = (pipe_id % CIK_PIPE_PER_MEC);
+
+	lock_srbm(kgd, mec, pipe, queue_id, 0);
+}
+
+static void release_queue(struct kgd_dev *kgd)
+{
+	unlock_srbm(kgd);
+}
+
+static void kgd_program_sh_mem_settings(struct kgd_dev *kgd, uint32_t vmid,
+					uint32_t sh_mem_config,
+					uint32_t sh_mem_ape1_base,
+					uint32_t sh_mem_ape1_limit,
+					uint32_t sh_mem_bases)
+{
+	lock_srbm(kgd, 0, 0, 0, vmid);
+
+	write_register(kgd, SH_MEM_CONFIG, sh_mem_config);
+	write_register(kgd, SH_MEM_APE1_BASE, sh_mem_ape1_base);
+	write_register(kgd, SH_MEM_APE1_LIMIT, sh_mem_ape1_limit);
+	write_register(kgd, SH_MEM_BASES, sh_mem_bases);
+
+	unlock_srbm(kgd);
+}
+
+static int kgd_set_pasid_vmid_mapping(struct kgd_dev *kgd, unsigned int pasid,
+					unsigned int vmid)
+{
+	/*
+	 * We have to assume that there is no outstanding mapping.
+	 * The ATC_VMID_PASID_MAPPING_UPDATE_STATUS bit could be 0
+	 * because a mapping is in progress or because a mapping finished and
+	 * the SW cleared it.
+	 * So the protocol is to always wait & clear.
+	 */
+	uint32_t pasid_mapping = (pasid == 0) ? 0 :
+				(uint32_t)pasid | ATC_VMID_PASID_MAPPING_VALID;
+
+	write_register(kgd, ATC_VMID0_PASID_MAPPING + vmid*sizeof(uint32_t),
+			pasid_mapping);
+
+	while (!(read_register(kgd, ATC_VMID_PASID_MAPPING_UPDATE_STATUS) &
+								(1U << vmid)))
+		cpu_relax();
+	write_register(kgd, ATC_VMID_PASID_MAPPING_UPDATE_STATUS, 1U << vmid);
+
+	return 0;
+}
+
+static int kgd_init_memory(struct kgd_dev *kgd)
+{
+	/*
+	 * Configure apertures:
+	 * LDS:         0x60000000'00000000 - 0x60000001'00000000 (4GB)
+	 * Scratch:     0x60000001'00000000 - 0x60000002'00000000 (4GB)
+	 * GPUVM:       0x60010000'00000000 - 0x60020000'00000000 (1TB)
+	 */
+	int i;
+	uint32_t sh_mem_bases = PRIVATE_BASE(0x6000) | SHARED_BASE(0x6000);
+
+	for (i = 8; i < 16; i++) {
+		uint32_t sh_mem_config;
+
+		lock_srbm(kgd, 0, 0, 0, i);
+
+		sh_mem_config = ALIGNMENT_MODE(SH_MEM_ALIGNMENT_MODE_UNALIGNED);
+		sh_mem_config |= DEFAULT_MTYPE(MTYPE_NONCACHED);
+
+		write_register(kgd, SH_MEM_CONFIG, sh_mem_config);
+
+		write_register(kgd, SH_MEM_BASES, sh_mem_bases);
+
+		/* Scratch aperture is not supported for now. */
+		write_register(kgd, SH_STATIC_MEM_CONFIG, 0);
+
+		/* APE1 disabled for now. */
+		write_register(kgd, SH_MEM_APE1_BASE, 1);
+		write_register(kgd, SH_MEM_APE1_LIMIT, 0);
+
+		unlock_srbm(kgd);
+	}
+
+	return 0;
+}
+
+static int kgd_init_pipeline(struct kgd_dev *kgd, uint32_t pipe_id,
+				uint32_t hpd_size, uint64_t hpd_gpu_addr)
+{
+	uint32_t mec = (++pipe_id / CIK_PIPE_PER_MEC) + 1;
+	uint32_t pipe = (pipe_id % CIK_PIPE_PER_MEC);
+
+	lock_srbm(kgd, mec, pipe, 0, 0);
+	write_register(kgd, CP_HPD_EOP_BASE_ADDR,
+			lower_32_bits(hpd_gpu_addr >> 8));
+	write_register(kgd, CP_HPD_EOP_BASE_ADDR_HI,
+			upper_32_bits(hpd_gpu_addr >> 8));
+	write_register(kgd, CP_HPD_EOP_VMID, 0);
+	write_register(kgd, CP_HPD_EOP_CONTROL, hpd_size);
+	unlock_srbm(kgd);
+
+	return 0;
+}
+
+static inline struct cik_mqd *get_mqd(void *mqd)
+{
+	return (struct cik_mqd *)mqd;
+}
+
+static int kgd_hqd_load(struct kgd_dev *kgd, void *mqd, uint32_t pipe_id,
+			uint32_t queue_id, uint32_t __user *wptr)
+{
+	uint32_t wptr_shadow, is_wptr_shadow_valid;
+	struct cik_mqd *m;
+
+	m = get_mqd(mqd);
+
+	is_wptr_shadow_valid = !get_user(wptr_shadow, wptr);
+
+	acquire_queue(kgd, pipe_id, queue_id);
+	write_register(kgd, CP_MQD_BASE_ADDR, m->cp_mqd_base_addr_lo);
+	write_register(kgd, CP_MQD_BASE_ADDR_HI, m->cp_mqd_base_addr_hi);
+	write_register(kgd, CP_MQD_CONTROL, m->cp_mqd_control);
+
+	write_register(kgd, CP_HQD_PQ_BASE, m->cp_hqd_pq_base_lo);
+	write_register(kgd, CP_HQD_PQ_BASE_HI, m->cp_hqd_pq_base_hi);
+	write_register(kgd, CP_HQD_PQ_CONTROL, m->cp_hqd_pq_control);
+
+	write_register(kgd, CP_HQD_IB_CONTROL, m->cp_hqd_ib_control);
+	write_register(kgd, CP_HQD_IB_BASE_ADDR, m->cp_hqd_ib_base_addr_lo);
+	write_register(kgd, CP_HQD_IB_BASE_ADDR_HI, m->cp_hqd_ib_base_addr_hi);
+
+	write_register(kgd, CP_HQD_IB_RPTR, m->cp_hqd_ib_rptr);
+
+	write_register(kgd, CP_HQD_PERSISTENT_STATE,
+			m->cp_hqd_persistent_state);
+	write_register(kgd, CP_HQD_SEMA_CMD, m->cp_hqd_sema_cmd);
+	write_register(kgd, CP_HQD_MSG_TYPE, m->cp_hqd_msg_type);
+
+	write_register(kgd, CP_HQD_ATOMIC0_PREOP_LO,
+			m->cp_hqd_atomic0_preop_lo);
+
+	write_register(kgd, CP_HQD_ATOMIC0_PREOP_HI,
+			m->cp_hqd_atomic0_preop_hi);
+
+	write_register(kgd, CP_HQD_ATOMIC1_PREOP_LO,
+			m->cp_hqd_atomic1_preop_lo);
+
+	write_register(kgd, CP_HQD_ATOMIC1_PREOP_HI,
+			m->cp_hqd_atomic1_preop_hi);
+
+	write_register(kgd, CP_HQD_PQ_RPTR_REPORT_ADDR,
+			m->cp_hqd_pq_rptr_report_addr_lo);
+
+	write_register(kgd, CP_HQD_PQ_RPTR_REPORT_ADDR_HI,
+			m->cp_hqd_pq_rptr_report_addr_hi);
+
+	write_register(kgd, CP_HQD_PQ_RPTR, m->cp_hqd_pq_rptr);
+
+	write_register(kgd, CP_HQD_PQ_WPTR_POLL_ADDR,
+			m->cp_hqd_pq_wptr_poll_addr_lo);
+
+	write_register(kgd, CP_HQD_PQ_WPTR_POLL_ADDR_HI,
+			m->cp_hqd_pq_wptr_poll_addr_hi);
+
+	write_register(kgd, CP_HQD_PQ_DOORBELL_CONTROL,
+			m->cp_hqd_pq_doorbell_control);
+
+	write_register(kgd, CP_HQD_VMID, m->cp_hqd_vmid);
+
+	write_register(kgd, CP_HQD_QUANTUM, m->cp_hqd_quantum);
+
+	write_register(kgd, CP_HQD_PIPE_PRIORITY, m->cp_hqd_pipe_priority);
+	write_register(kgd, CP_HQD_QUEUE_PRIORITY, m->cp_hqd_queue_priority);
+
+	write_register(kgd, CP_HQD_IQ_RPTR, m->cp_hqd_iq_rptr);
+
+	if (is_wptr_shadow_valid)
+		write_register(kgd, CP_HQD_PQ_WPTR, wptr_shadow);
+
+	write_register(kgd, CP_HQD_ACTIVE, m->cp_hqd_active);
+	release_queue(kgd);
+
+	return 0;
+}
+
+static bool kgd_hqd_is_occupies(struct kgd_dev *kgd, uint64_t queue_address,
+				uint32_t pipe_id, uint32_t queue_id)
+{
+	uint32_t act;
+	bool retval = false;
+	uint32_t low, high;
+
+	acquire_queue(kgd, pipe_id, queue_id);
+	act = read_register(kgd, CP_HQD_ACTIVE);
+	if (act) {
+		low = lower_32_bits(queue_address >> 8);
+		high = upper_32_bits(queue_address >> 8);
+
+		if (low == read_register(kgd, CP_HQD_PQ_BASE) &&
+				high == read_register(kgd, CP_HQD_PQ_BASE_HI))
+			retval = true;
+	}
+	release_queue(kgd);
+	return retval;
+}
+
+static int kgd_hqd_destroy(struct kgd_dev *kgd, uint32_t reset_type,
+				unsigned int timeout, uint32_t pipe_id,
+				uint32_t queue_id)
+{
+	uint32_t temp;
+
+	acquire_queue(kgd, pipe_id, queue_id);
+	write_register(kgd, CP_HQD_PQ_DOORBELL_CONTROL, 0);
+
+	write_register(kgd, CP_HQD_DEQUEUE_REQUEST, reset_type);
+
+	while (true) {
+		temp = read_register(kgd, CP_HQD_ACTIVE);
+		if (temp & 0x1)
+			break;
+		if (timeout == 0) {
+			pr_err("kfd: cp queue preemption time out (%dms)\n",
+				temp);
+			return -ETIME;
+		}
+		msleep(20);
+		timeout -= 20;
+	}
+
+	release_queue(kgd);
+	return 0;
+}
+
+static uint16_t get_fw_version(struct kgd_dev *kgd, enum kgd_engine_type type)
+{
+	struct radeon_device *rdev = (struct radeon_device *) kgd;
+	const union radeon_firmware_header *hdr;
+
+	BUG_ON(kgd == NULL || rdev->mec_fw == NULL);
+
+	switch (type) {
+	case KGD_ENGINE_PFP:
+		hdr = (const union radeon_firmware_header *) rdev->pfp_fw->data;
+		break;
+
+	case KGD_ENGINE_ME:
+		hdr = (const union radeon_firmware_header *) rdev->me_fw->data;
+		break;
+
+	case KGD_ENGINE_CE:
+		hdr = (const union radeon_firmware_header *) rdev->ce_fw->data;
+		break;
+
+	case KGD_ENGINE_MEC1:
+		hdr = (const union radeon_firmware_header *) rdev->mec_fw->data;
+		break;
+
+	case KGD_ENGINE_MEC2:
+		hdr = (const union radeon_firmware_header *)
+							rdev->mec2_fw->data;
+		break;
+
+	case KGD_ENGINE_RLC:
+		hdr = (const union radeon_firmware_header *) rdev->rlc_fw->data;
+		break;
+
+	case KGD_ENGINE_SDMA:
+		hdr = (const union radeon_firmware_header *)
+							rdev->sdma_fw->data;
+		break;
+
+	default:
+		return 0;
+	}
+
+	if (hdr == NULL)
+		return 0;
+
+	/* Only 12 bit in use*/
+	return hdr->common.ucode_version;
+}
diff --git a/drivers/gpu/drm/radeon/radeon_kfd.h b/drivers/gpu/drm/radeon/radeon_kfd.h
new file mode 100644
index 000000000000..f90e161ca507
--- /dev/null
+++ b/drivers/gpu/drm/radeon/radeon_kfd.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright 2014 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.
+ */
+
+/*
+ * radeon_kfd.h defines the private interface between the
+ * AMD kernel graphics drivers and the AMD KFD.
+ */
+
+#ifndef RADEON_KFD_H_INCLUDED
+#define RADEON_KFD_H_INCLUDED
+
+#include <linux/types.h>
+#include "../amd/include/kgd_kfd_interface.h"
+
+struct radeon_device;
+
+bool radeon_kfd_init(void);
+void radeon_kfd_fini(void);
+
+void radeon_kfd_suspend(struct radeon_device *rdev);
+int radeon_kfd_resume(struct radeon_device *rdev);
+void radeon_kfd_interrupt(struct radeon_device *rdev,
+			const void *ih_ring_entry);
+void radeon_kfd_device_probe(struct radeon_device *rdev);
+void radeon_kfd_device_init(struct radeon_device *rdev);
+void radeon_kfd_device_fini(struct radeon_device *rdev);
+
+#endif /* RADEON_KFD_H_INCLUDED */
diff --git a/drivers/gpu/drm/radeon/radeon_kms.c b/drivers/gpu/drm/radeon/radeon_kms.c
index 03586763ee86..3cf9c1fa6475 100644
--- a/drivers/gpu/drm/radeon/radeon_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_kms.c
@@ -34,6 +34,8 @@
 #include <linux/slab.h>
 #include <linux/pm_runtime.h>
 
+#include "radeon_kfd.h"
+
 #if defined(CONFIG_VGA_SWITCHEROO)
 bool radeon_has_atpx(void);
 #else
@@ -63,6 +65,8 @@ int radeon_driver_unload_kms(struct drm_device *dev)
 
 	pm_runtime_get_sync(dev->dev);
 
+	radeon_kfd_device_fini(rdev);
+
 	radeon_acpi_fini(rdev);
 	
 	radeon_modeset_fini(rdev);
@@ -142,6 +146,9 @@ int radeon_driver_load_kms(struct drm_device *dev, unsigned long flags)
 				"Error during ACPI methods call\n");
 	}
 
+	radeon_kfd_device_probe(rdev);
+	radeon_kfd_device_init(rdev);
+
 	if (radeon_is_px(dev)) {
 		pm_runtime_use_autosuspend(dev->dev);
 		pm_runtime_set_autosuspend_delay(dev->dev, 5000);
@@ -621,8 +628,6 @@ int radeon_driver_open_kms(struct drm_device *dev, struct drm_file *file_priv)
 						  RADEON_VA_IB_OFFSET,
 						  RADEON_VM_PAGE_READABLE |
 						  RADEON_VM_PAGE_SNOOPED);
-
-			radeon_bo_unreserve(rdev->ring_tmp_bo.bo);
 			if (r) {
 				radeon_vm_fini(rdev, vm);
 				kfree(fpriv);
diff --git a/drivers/gpu/drm/radeon/radeon_legacy_crtc.c b/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
index cafb1ccf2ec3..678b4386540d 100644
--- a/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
+++ b/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
@@ -1054,6 +1054,7 @@ static int radeon_crtc_mode_set(struct drm_crtc *crtc,
 			DRM_ERROR("Mode need scaling but only first crtc can do that.\n");
 		}
 	}
+	radeon_cursor_reset(crtc);
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/radeon/radeon_mode.h b/drivers/gpu/drm/radeon/radeon_mode.h
index 04db2fdd8692..390db897f322 100644
--- a/drivers/gpu/drm/radeon/radeon_mode.h
+++ b/drivers/gpu/drm/radeon/radeon_mode.h
@@ -321,6 +321,10 @@ struct radeon_crtc {
 	uint32_t crtc_offset;
 	struct drm_gem_object *cursor_bo;
 	uint64_t cursor_addr;
+	int cursor_x;
+	int cursor_y;
+	int cursor_hot_x;
+	int cursor_hot_y;
 	int cursor_width;
 	int cursor_height;
 	int max_cursor_width;
@@ -462,6 +466,7 @@ struct radeon_gpio_rec {
 	u8 id;
 	u32 reg;
 	u32 mask;
+	u32 shift;
 };
 
 struct radeon_hpd {
@@ -748,6 +753,8 @@ extern bool radeon_atombios_get_ppll_ss_info(struct radeon_device *rdev,
 extern bool radeon_atombios_get_asic_ss_info(struct radeon_device *rdev,
 					     struct radeon_atom_ss *ss,
 					     int id, u32 clock);
+extern struct radeon_gpio_rec radeon_atombios_lookup_gpio(struct radeon_device *rdev,
+							  u8 id);
 
 extern void radeon_compute_pll_legacy(struct radeon_pll *pll,
 				      uint64_t freq,
@@ -802,13 +809,16 @@ extern int radeon_crtc_set_base_atomic(struct drm_crtc *crtc,
 extern int radeon_crtc_do_set_base(struct drm_crtc *crtc,
 				   struct drm_framebuffer *fb,
 				   int x, int y, int atomic);
-extern int radeon_crtc_cursor_set(struct drm_crtc *crtc,
-				  struct drm_file *file_priv,
-				  uint32_t handle,
-				  uint32_t width,
-				  uint32_t height);
+extern int radeon_crtc_cursor_set2(struct drm_crtc *crtc,
+				   struct drm_file *file_priv,
+				   uint32_t handle,
+				   uint32_t width,
+				   uint32_t height,
+				   int32_t hot_x,
+				   int32_t hot_y);
 extern int radeon_crtc_cursor_move(struct drm_crtc *crtc,
 				   int x, int y);
+extern void radeon_cursor_reset(struct drm_crtc *crtc);
 
 extern int radeon_get_crtc_scanoutpos(struct drm_device *dev, int crtc,
 				      unsigned int flags,
diff --git a/drivers/gpu/drm/radeon/radeon_object.c b/drivers/gpu/drm/radeon/radeon_object.c
index 4c0d786d5c7a..86fc56434b28 100644
--- a/drivers/gpu/drm/radeon/radeon_object.c
+++ b/drivers/gpu/drm/radeon/radeon_object.c
@@ -99,22 +99,39 @@ void radeon_ttm_placement_from_domain(struct radeon_bo *rbo, u32 domain)
 
 	rbo->placement.placement = rbo->placements;
 	rbo->placement.busy_placement = rbo->placements;
-	if (domain & RADEON_GEM_DOMAIN_VRAM)
+	if (domain & RADEON_GEM_DOMAIN_VRAM) {
+		/* Try placing BOs which don't need CPU access outside of the
+		 * CPU accessible part of VRAM
+		 */
+		if ((rbo->flags & RADEON_GEM_NO_CPU_ACCESS) &&
+		    rbo->rdev->mc.visible_vram_size < rbo->rdev->mc.real_vram_size) {
+			rbo->placements[c].fpfn =
+				rbo->rdev->mc.visible_vram_size >> PAGE_SHIFT;
+			rbo->placements[c++].flags = TTM_PL_FLAG_WC |
+						     TTM_PL_FLAG_UNCACHED |
+						     TTM_PL_FLAG_VRAM;
+		}
+
+		rbo->placements[c].fpfn = 0;
 		rbo->placements[c++].flags = TTM_PL_FLAG_WC |
 					     TTM_PL_FLAG_UNCACHED |
 					     TTM_PL_FLAG_VRAM;
+	}
 
 	if (domain & RADEON_GEM_DOMAIN_GTT) {
 		if (rbo->flags & RADEON_GEM_GTT_UC) {
+			rbo->placements[c].fpfn = 0;
 			rbo->placements[c++].flags = TTM_PL_FLAG_UNCACHED |
 				TTM_PL_FLAG_TT;
 
 		} else if ((rbo->flags & RADEON_GEM_GTT_WC) ||
 			   (rbo->rdev->flags & RADEON_IS_AGP)) {
+			rbo->placements[c].fpfn = 0;
 			rbo->placements[c++].flags = TTM_PL_FLAG_WC |
 				TTM_PL_FLAG_UNCACHED |
 				TTM_PL_FLAG_TT;
 		} else {
+			rbo->placements[c].fpfn = 0;
 			rbo->placements[c++].flags = TTM_PL_FLAG_CACHED |
 						     TTM_PL_FLAG_TT;
 		}
@@ -122,30 +139,35 @@ void radeon_ttm_placement_from_domain(struct radeon_bo *rbo, u32 domain)
 
 	if (domain & RADEON_GEM_DOMAIN_CPU) {
 		if (rbo->flags & RADEON_GEM_GTT_UC) {
+			rbo->placements[c].fpfn = 0;
 			rbo->placements[c++].flags = TTM_PL_FLAG_UNCACHED |
 				TTM_PL_FLAG_SYSTEM;
 
 		} else if ((rbo->flags & RADEON_GEM_GTT_WC) ||
 		    rbo->rdev->flags & RADEON_IS_AGP) {
+			rbo->placements[c].fpfn = 0;
 			rbo->placements[c++].flags = TTM_PL_FLAG_WC |
 				TTM_PL_FLAG_UNCACHED |
 				TTM_PL_FLAG_SYSTEM;
 		} else {
+			rbo->placements[c].fpfn = 0;
 			rbo->placements[c++].flags = TTM_PL_FLAG_CACHED |
 						     TTM_PL_FLAG_SYSTEM;
 		}
 	}
-	if (!c)
+	if (!c) {
+		rbo->placements[c].fpfn = 0;
 		rbo->placements[c++].flags = TTM_PL_MASK_CACHING |
 					     TTM_PL_FLAG_SYSTEM;
+	}
 
 	rbo->placement.num_placement = c;
 	rbo->placement.num_busy_placement = c;
 
 	for (i = 0; i < c; ++i) {
-		rbo->placements[i].fpfn = 0;
 		if ((rbo->flags & RADEON_GEM_CPU_ACCESS) &&
-		    (rbo->placements[i].flags & TTM_PL_FLAG_VRAM))
+		    (rbo->placements[i].flags & TTM_PL_FLAG_VRAM) &&
+		    !rbo->placements[i].fpfn)
 			rbo->placements[i].lpfn =
 				rbo->rdev->mc.visible_vram_size >> PAGE_SHIFT;
 		else
@@ -157,9 +179,7 @@ void radeon_ttm_placement_from_domain(struct radeon_bo *rbo, u32 domain)
 	 * improve fragmentation quality.
 	 * 512kb was measured as the most optimal number.
 	 */
-	if (!((rbo->flags & RADEON_GEM_CPU_ACCESS) &&
-	      (rbo->placements[i].flags & TTM_PL_FLAG_VRAM)) &&
-	    rbo->tbo.mem.size > 512 * 1024) {
+	if (rbo->tbo.mem.size > 512 * 1024) {
 		for (i = 0; i < c; i++) {
 			rbo->placements[i].flags |= TTM_PL_FLAG_TOPDOWN;
 		}
@@ -489,19 +509,20 @@ int radeon_bo_list_validate(struct radeon_device *rdev,
 			    struct ww_acquire_ctx *ticket,
 			    struct list_head *head, int ring)
 {
-	struct radeon_cs_reloc *lobj;
-	struct radeon_bo *bo;
+	struct radeon_bo_list *lobj;
+	struct list_head duplicates;
 	int r;
 	u64 bytes_moved = 0, initial_bytes_moved;
 	u64 bytes_moved_threshold = radeon_bo_get_threshold_for_moves(rdev);
 
-	r = ttm_eu_reserve_buffers(ticket, head, true);
+	INIT_LIST_HEAD(&duplicates);
+	r = ttm_eu_reserve_buffers(ticket, head, true, &duplicates);
 	if (unlikely(r != 0)) {
 		return r;
 	}
 
 	list_for_each_entry(lobj, head, tv.head) {
-		bo = lobj->robj;
+		struct radeon_bo *bo = lobj->robj;
 		if (!bo->pin_count) {
 			u32 domain = lobj->prefered_domains;
 			u32 allowed = lobj->allowed_domains;
@@ -546,6 +567,12 @@ int radeon_bo_list_validate(struct radeon_device *rdev,
 		lobj->gpu_offset = radeon_bo_gpu_offset(bo);
 		lobj->tiling_flags = bo->tiling_flags;
 	}
+
+	list_for_each_entry(lobj, &duplicates, tv.head) {
+		lobj->gpu_offset = radeon_bo_gpu_offset(lobj->robj);
+		lobj->tiling_flags = lobj->robj->tiling_flags;
+	}
+
 	return 0;
 }
 
@@ -750,8 +777,8 @@ int radeon_bo_fault_reserve_notify(struct ttm_buffer_object *bo)
 {
 	struct radeon_device *rdev;
 	struct radeon_bo *rbo;
-	unsigned long offset, size;
-	int r;
+	unsigned long offset, size, lpfn;
+	int i, r;
 
 	if (!radeon_ttm_bo_is_radeon_bo(bo))
 		return 0;
@@ -768,7 +795,13 @@ int radeon_bo_fault_reserve_notify(struct ttm_buffer_object *bo)
 
 	/* hurrah the memory is not visible ! */
 	radeon_ttm_placement_from_domain(rbo, RADEON_GEM_DOMAIN_VRAM);
-	rbo->placements[0].lpfn = rdev->mc.visible_vram_size >> PAGE_SHIFT;
+	lpfn =	rdev->mc.visible_vram_size >> PAGE_SHIFT;
+	for (i = 0; i < rbo->placement.num_placement; i++) {
+		/* Force into visible VRAM */
+		if ((rbo->placements[i].flags & TTM_PL_FLAG_VRAM) &&
+		    (!rbo->placements[i].lpfn || rbo->placements[i].lpfn > lpfn))
+			rbo->placements[i].lpfn = lpfn;
+	}
 	r = ttm_bo_validate(bo, &rbo->placement, false, false);
 	if (unlikely(r == -ENOMEM)) {
 		radeon_ttm_placement_from_domain(rbo, RADEON_GEM_DOMAIN_GTT);
@@ -799,3 +832,22 @@ int radeon_bo_wait(struct radeon_bo *bo, u32 *mem_type, bool no_wait)
 	ttm_bo_unreserve(&bo->tbo);
 	return r;
 }
+
+/**
+ * radeon_bo_fence - add fence to buffer object
+ *
+ * @bo: buffer object in question
+ * @fence: fence to add
+ * @shared: true if fence should be added shared
+ *
+ */
+void radeon_bo_fence(struct radeon_bo *bo, struct radeon_fence *fence,
+                     bool shared)
+{
+	struct reservation_object *resv = bo->tbo.resv;
+
+	if (shared)
+		reservation_object_add_shared_fence(resv, &fence->base);
+	else
+		reservation_object_add_excl_fence(resv, &fence->base);
+}
diff --git a/drivers/gpu/drm/radeon/radeon_object.h b/drivers/gpu/drm/radeon/radeon_object.h
index 1b8ec7917154..3b0b377f76cb 100644
--- a/drivers/gpu/drm/radeon/radeon_object.h
+++ b/drivers/gpu/drm/radeon/radeon_object.h
@@ -155,6 +155,8 @@ extern void radeon_bo_move_notify(struct ttm_buffer_object *bo,
 				  struct ttm_mem_reg *new_mem);
 extern int radeon_bo_fault_reserve_notify(struct ttm_buffer_object *bo);
 extern int radeon_bo_get_surface_reg(struct radeon_bo *bo);
+extern void radeon_bo_fence(struct radeon_bo *bo, struct radeon_fence *fence,
+			    bool shared);
 
 /*
  * sub allocation
diff --git a/drivers/gpu/drm/radeon/radeon_semaphore.c b/drivers/gpu/drm/radeon/radeon_semaphore.c
index 6deb08f045b7..e6ad54cdfa62 100644
--- a/drivers/gpu/drm/radeon/radeon_semaphore.c
+++ b/drivers/gpu/drm/radeon/radeon_semaphore.c
@@ -34,15 +34,14 @@
 int radeon_semaphore_create(struct radeon_device *rdev,
 			    struct radeon_semaphore **semaphore)
 {
-	uint64_t *cpu_addr;
-	int i, r;
+	int r;
 
 	*semaphore = kmalloc(sizeof(struct radeon_semaphore), GFP_KERNEL);
 	if (*semaphore == NULL) {
 		return -ENOMEM;
 	}
-	r = radeon_sa_bo_new(rdev, &rdev->ring_tmp_bo, &(*semaphore)->sa_bo,
-			     8 * RADEON_NUM_SYNCS, 8);
+	r = radeon_sa_bo_new(rdev, &rdev->ring_tmp_bo,
+			     &(*semaphore)->sa_bo, 8, 8);
 	if (r) {
 		kfree(*semaphore);
 		*semaphore = NULL;
@@ -51,12 +50,7 @@ int radeon_semaphore_create(struct radeon_device *rdev,
 	(*semaphore)->waiters = 0;
 	(*semaphore)->gpu_addr = radeon_sa_bo_gpu_addr((*semaphore)->sa_bo);
 
-	cpu_addr = radeon_sa_bo_cpu_addr((*semaphore)->sa_bo);
-	for (i = 0; i < RADEON_NUM_SYNCS; ++i)
-		cpu_addr[i] = 0;
-
-	for (i = 0; i < RADEON_NUM_RINGS; ++i)
-		(*semaphore)->sync_to[i] = NULL;
+	*((uint64_t *)radeon_sa_bo_cpu_addr((*semaphore)->sa_bo)) = 0;
 
 	return 0;
 }
@@ -95,146 +89,6 @@ bool radeon_semaphore_emit_wait(struct radeon_device *rdev, int ridx,
 	return false;
 }
 
-/**
- * radeon_semaphore_sync_fence - use the semaphore to sync to a fence
- *
- * @semaphore: semaphore object to add fence to
- * @fence: fence to sync to
- *
- * Sync to the fence using this semaphore object
- */
-void radeon_semaphore_sync_fence(struct radeon_semaphore *semaphore,
-				 struct radeon_fence *fence)
-{
-        struct radeon_fence *other;
-
-        if (!fence)
-                return;
-
-        other = semaphore->sync_to[fence->ring];
-        semaphore->sync_to[fence->ring] = radeon_fence_later(fence, other);
-}
-
-/**
- * radeon_semaphore_sync_to - use the semaphore to sync to a reservation object
- *
- * @sema: semaphore object to add fence from reservation object to
- * @resv: reservation object with embedded fence
- * @shared: true if we should onyl sync to the exclusive fence
- *
- * Sync to the fence using this semaphore object
- */
-int radeon_semaphore_sync_resv(struct radeon_device *rdev,
-			       struct radeon_semaphore *sema,
-			       struct reservation_object *resv,
-			       bool shared)
-{
-	struct reservation_object_list *flist;
-	struct fence *f;
-	struct radeon_fence *fence;
-	unsigned i;
-	int r = 0;
-
-	/* always sync to the exclusive fence */
-	f = reservation_object_get_excl(resv);
-	fence = f ? to_radeon_fence(f) : NULL;
-	if (fence && fence->rdev == rdev)
-		radeon_semaphore_sync_fence(sema, fence);
-	else if (f)
-		r = fence_wait(f, true);
-
-	flist = reservation_object_get_list(resv);
-	if (shared || !flist || r)
-		return r;
-
-	for (i = 0; i < flist->shared_count; ++i) {
-		f = rcu_dereference_protected(flist->shared[i],
-					      reservation_object_held(resv));
-		fence = to_radeon_fence(f);
-		if (fence && fence->rdev == rdev)
-			radeon_semaphore_sync_fence(sema, fence);
-		else
-			r = fence_wait(f, true);
-
-		if (r)
-			break;
-	}
-	return r;
-}
-
-/**
- * radeon_semaphore_sync_rings - sync ring to all registered fences
- *
- * @rdev: radeon_device pointer
- * @semaphore: semaphore object to use for sync
- * @ring: ring that needs sync
- *
- * Ensure that all registered fences are signaled before letting
- * the ring continue. The caller must hold the ring lock.
- */
-int radeon_semaphore_sync_rings(struct radeon_device *rdev,
-				struct radeon_semaphore *semaphore,
-				int ring)
-{
-	unsigned count = 0;
-	int i, r;
-
-        for (i = 0; i < RADEON_NUM_RINGS; ++i) {
-		struct radeon_fence *fence = semaphore->sync_to[i];
-
-		/* check if we really need to sync */
-                if (!radeon_fence_need_sync(fence, ring))
-			continue;
-
-		/* prevent GPU deadlocks */
-		if (!rdev->ring[i].ready) {
-			dev_err(rdev->dev, "Syncing to a disabled ring!");
-			return -EINVAL;
-		}
-
-		if (++count > RADEON_NUM_SYNCS) {
-			/* not enough room, wait manually */
-			r = radeon_fence_wait(fence, false);
-			if (r)
-				return r;
-			continue;
-		}
-
-		/* allocate enough space for sync command */
-		r = radeon_ring_alloc(rdev, &rdev->ring[i], 16);
-		if (r) {
-			return r;
-		}
-
-		/* emit the signal semaphore */
-		if (!radeon_semaphore_emit_signal(rdev, i, semaphore)) {
-			/* signaling wasn't successful wait manually */
-			radeon_ring_undo(&rdev->ring[i]);
-			r = radeon_fence_wait(fence, false);
-			if (r)
-				return r;
-			continue;
-		}
-
-		/* we assume caller has already allocated space on waiters ring */
-		if (!radeon_semaphore_emit_wait(rdev, ring, semaphore)) {
-			/* waiting wasn't successful wait manually */
-			radeon_ring_undo(&rdev->ring[i]);
-			r = radeon_fence_wait(fence, false);
-			if (r)
-				return r;
-			continue;
-		}
-
-		radeon_ring_commit(rdev, &rdev->ring[i], false);
-		radeon_fence_note_sync(fence, ring);
-
-		semaphore->gpu_addr += 8;
-	}
-
-	return 0;
-}
-
 void radeon_semaphore_free(struct radeon_device *rdev,
 			   struct radeon_semaphore **semaphore,
 			   struct radeon_fence *fence)
diff --git a/drivers/gpu/drm/radeon/radeon_sync.c b/drivers/gpu/drm/radeon/radeon_sync.c
new file mode 100644
index 000000000000..02ac8a1de4ff
--- /dev/null
+++ b/drivers/gpu/drm/radeon/radeon_sync.c
@@ -0,0 +1,220 @@
+/*
+ * Copyright 2014 Advanced Micro Devices, Inc.
+ * All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sub license, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
+ * USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ */
+/*
+ * Authors:
+ *    Christian König <christian.koenig@amd.com>
+ */
+
+#include <drm/drmP.h>
+#include "radeon.h"
+#include "radeon_trace.h"
+
+/**
+ * radeon_sync_create - zero init sync object
+ *
+ * @sync: sync object to initialize
+ *
+ * Just clear the sync object for now.
+ */
+void radeon_sync_create(struct radeon_sync *sync)
+{
+	unsigned i;
+
+	for (i = 0; i < RADEON_NUM_SYNCS; ++i)
+		sync->semaphores[i] = NULL;
+
+	for (i = 0; i < RADEON_NUM_RINGS; ++i)
+		sync->sync_to[i] = NULL;
+
+	sync->last_vm_update = NULL;
+}
+
+/**
+ * radeon_sync_fence - use the semaphore to sync to a fence
+ *
+ * @sync: sync object to add fence to
+ * @fence: fence to sync to
+ *
+ * Sync to the fence using the semaphore objects
+ */
+void radeon_sync_fence(struct radeon_sync *sync,
+		       struct radeon_fence *fence)
+{
+	struct radeon_fence *other;
+
+	if (!fence)
+		return;
+
+	other = sync->sync_to[fence->ring];
+	sync->sync_to[fence->ring] = radeon_fence_later(fence, other);
+
+	if (fence->is_vm_update) {
+		other = sync->last_vm_update;
+		sync->last_vm_update = radeon_fence_later(fence, other);
+	}
+}
+
+/**
+ * radeon_sync_resv - use the semaphores to sync to a reservation object
+ *
+ * @sync: sync object to add fences from reservation object to
+ * @resv: reservation object with embedded fence
+ * @shared: true if we should only sync to the exclusive fence
+ *
+ * Sync to the fence using the semaphore objects
+ */
+int radeon_sync_resv(struct radeon_device *rdev,
+		     struct radeon_sync *sync,
+		     struct reservation_object *resv,
+		     bool shared)
+{
+	struct reservation_object_list *flist;
+	struct fence *f;
+	struct radeon_fence *fence;
+	unsigned i;
+	int r = 0;
+
+	/* always sync to the exclusive fence */
+	f = reservation_object_get_excl(resv);
+	fence = f ? to_radeon_fence(f) : NULL;
+	if (fence && fence->rdev == rdev)
+		radeon_sync_fence(sync, fence);
+	else if (f)
+		r = fence_wait(f, true);
+
+	flist = reservation_object_get_list(resv);
+	if (shared || !flist || r)
+		return r;
+
+	for (i = 0; i < flist->shared_count; ++i) {
+		f = rcu_dereference_protected(flist->shared[i],
+					      reservation_object_held(resv));
+		fence = to_radeon_fence(f);
+		if (fence && fence->rdev == rdev)
+			radeon_sync_fence(sync, fence);
+		else
+			r = fence_wait(f, true);
+
+		if (r)
+			break;
+	}
+	return r;
+}
+
+/**
+ * radeon_sync_rings - sync ring to all registered fences
+ *
+ * @rdev: radeon_device pointer
+ * @sync: sync object to use
+ * @ring: ring that needs sync
+ *
+ * Ensure that all registered fences are signaled before letting
+ * the ring continue. The caller must hold the ring lock.
+ */
+int radeon_sync_rings(struct radeon_device *rdev,
+		      struct radeon_sync *sync,
+		      int ring)
+{
+	unsigned count = 0;
+	int i, r;
+
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		struct radeon_fence *fence = sync->sync_to[i];
+		struct radeon_semaphore *semaphore;
+
+		/* check if we really need to sync */
+		if (!radeon_fence_need_sync(fence, ring))
+			continue;
+
+		/* prevent GPU deadlocks */
+		if (!rdev->ring[i].ready) {
+			dev_err(rdev->dev, "Syncing to a disabled ring!");
+			return -EINVAL;
+		}
+
+		if (count >= RADEON_NUM_SYNCS) {
+			/* not enough room, wait manually */
+			r = radeon_fence_wait(fence, false);
+			if (r)
+				return r;
+			continue;
+		}
+		r = radeon_semaphore_create(rdev, &semaphore);
+		if (r)
+			return r;
+
+		sync->semaphores[count++] = semaphore;
+
+		/* allocate enough space for sync command */
+		r = radeon_ring_alloc(rdev, &rdev->ring[i], 16);
+		if (r)
+			return r;
+
+		/* emit the signal semaphore */
+		if (!radeon_semaphore_emit_signal(rdev, i, semaphore)) {
+			/* signaling wasn't successful wait manually */
+			radeon_ring_undo(&rdev->ring[i]);
+			r = radeon_fence_wait(fence, false);
+			if (r)
+				return r;
+			continue;
+		}
+
+		/* we assume caller has already allocated space on waiters ring */
+		if (!radeon_semaphore_emit_wait(rdev, ring, semaphore)) {
+			/* waiting wasn't successful wait manually */
+			radeon_ring_undo(&rdev->ring[i]);
+			r = radeon_fence_wait(fence, false);
+			if (r)
+				return r;
+			continue;
+		}
+
+		radeon_ring_commit(rdev, &rdev->ring[i], false);
+		radeon_fence_note_sync(fence, ring);
+	}
+
+	return 0;
+}
+
+/**
+ * radeon_sync_free - free the sync object
+ *
+ * @rdev: radeon_device pointer
+ * @sync: sync object to use
+ * @fence: fence to use for the free
+ *
+ * Free the sync object by freeing all semaphores in it.
+ */
+void radeon_sync_free(struct radeon_device *rdev,
+		      struct radeon_sync *sync,
+		      struct radeon_fence *fence)
+{
+	unsigned i;
+
+	for (i = 0; i < RADEON_NUM_SYNCS; ++i)
+		radeon_semaphore_free(rdev, &sync->semaphores[i], fence);
+}
diff --git a/drivers/gpu/drm/radeon/radeon_trace.h b/drivers/gpu/drm/radeon/radeon_trace.h
index 9db74a96ef61..ce075cb08cb2 100644
--- a/drivers/gpu/drm/radeon/radeon_trace.h
+++ b/drivers/gpu/drm/radeon/radeon_trace.h
@@ -38,7 +38,7 @@ TRACE_EVENT(radeon_cs,
 
 	    TP_fast_assign(
 			   __entry->ring = p->ring;
-			   __entry->dw = p->chunks[p->chunk_ib_idx].length_dw;
+			   __entry->dw = p->chunk_ib->length_dw;
 			   __entry->fences = radeon_fence_count_emitted(
 				p->rdev, p->ring);
 			   ),
diff --git a/drivers/gpu/drm/radeon/radeon_ttm.c b/drivers/gpu/drm/radeon/radeon_ttm.c
index 8624979afb65..d02aa1d0f588 100644
--- a/drivers/gpu/drm/radeon/radeon_ttm.c
+++ b/drivers/gpu/drm/radeon/radeon_ttm.c
@@ -196,9 +196,32 @@ static void radeon_evict_flags(struct ttm_buffer_object *bo,
 	rbo = container_of(bo, struct radeon_bo, tbo);
 	switch (bo->mem.mem_type) {
 	case TTM_PL_VRAM:
-		if (rbo->rdev->ring[RADEON_RING_TYPE_GFX_INDEX].ready == false)
+		if (rbo->rdev->ring[radeon_copy_ring_index(rbo->rdev)].ready == false)
 			radeon_ttm_placement_from_domain(rbo, RADEON_GEM_DOMAIN_CPU);
-		else
+		else if (rbo->rdev->mc.visible_vram_size < rbo->rdev->mc.real_vram_size &&
+			 bo->mem.start < (rbo->rdev->mc.visible_vram_size >> PAGE_SHIFT)) {
+			unsigned fpfn = rbo->rdev->mc.visible_vram_size >> PAGE_SHIFT;
+			int i;
+
+			/* Try evicting to the CPU inaccessible part of VRAM
+			 * first, but only set GTT as busy placement, so this
+			 * BO will be evicted to GTT rather than causing other
+			 * BOs to be evicted from VRAM
+			 */
+			radeon_ttm_placement_from_domain(rbo, RADEON_GEM_DOMAIN_VRAM |
+							 RADEON_GEM_DOMAIN_GTT);
+			rbo->placement.num_busy_placement = 0;
+			for (i = 0; i < rbo->placement.num_placement; i++) {
+				if (rbo->placements[i].flags & TTM_PL_FLAG_VRAM) {
+					if (rbo->placements[0].fpfn < fpfn)
+						rbo->placements[0].fpfn = fpfn;
+				} else {
+					rbo->placement.busy_placement =
+						&rbo->placements[i];
+					rbo->placement.num_busy_placement = 1;
+				}
+			}
+		} else
 			radeon_ttm_placement_from_domain(rbo, RADEON_GEM_DOMAIN_GTT);
 		break;
 	case TTM_PL_TT:
diff --git a/drivers/gpu/drm/radeon/radeon_uvd.c b/drivers/gpu/drm/radeon/radeon_uvd.c
index 11b662469253..c10b2aec6450 100644
--- a/drivers/gpu/drm/radeon/radeon_uvd.c
+++ b/drivers/gpu/drm/radeon/radeon_uvd.c
@@ -488,12 +488,12 @@ static int radeon_uvd_cs_reloc(struct radeon_cs_parser *p,
 			       unsigned buf_sizes[], bool *has_msg_cmd)
 {
 	struct radeon_cs_chunk *relocs_chunk;
-	struct radeon_cs_reloc *reloc;
+	struct radeon_bo_list *reloc;
 	unsigned idx, cmd, offset;
 	uint64_t start, end;
 	int r;
 
-	relocs_chunk = &p->chunks[p->chunk_relocs_idx];
+	relocs_chunk = p->chunk_relocs;
 	offset = radeon_get_ib_value(p, data0);
 	idx = radeon_get_ib_value(p, data1);
 	if (idx >= relocs_chunk->length_dw) {
@@ -502,7 +502,7 @@ static int radeon_uvd_cs_reloc(struct radeon_cs_parser *p,
 		return -EINVAL;
 	}
 
-	reloc = p->relocs_ptr[(idx / 4)];
+	reloc = &p->relocs[(idx / 4)];
 	start = reloc->gpu_offset;
 	end = start + radeon_bo_size(reloc->robj);
 	start += offset;
@@ -610,13 +610,13 @@ int radeon_uvd_cs_parse(struct radeon_cs_parser *p)
 		[0x00000003]	=	2048,
 	};
 
-	if (p->chunks[p->chunk_ib_idx].length_dw % 16) {
+	if (p->chunk_ib->length_dw % 16) {
 		DRM_ERROR("UVD IB length (%d) not 16 dwords aligned!\n",
-			  p->chunks[p->chunk_ib_idx].length_dw);
+			  p->chunk_ib->length_dw);
 		return -EINVAL;
 	}
 
-	if (p->chunk_relocs_idx == -1) {
+	if (p->chunk_relocs == NULL) {
 		DRM_ERROR("No relocation chunk !\n");
 		return -EINVAL;
 	}
@@ -640,7 +640,7 @@ int radeon_uvd_cs_parse(struct radeon_cs_parser *p)
 			DRM_ERROR("Unknown packet type %d !\n", pkt.type);
 			return -EINVAL;
 		}
-	} while (p->idx < p->chunks[p->chunk_ib_idx].length_dw);
+	} while (p->idx < p->chunk_ib->length_dw);
 
 	if (!has_msg_cmd) {
 		DRM_ERROR("UVD-IBs need a msg command!\n");
diff --git a/drivers/gpu/drm/radeon/radeon_vce.c b/drivers/gpu/drm/radeon/radeon_vce.c
index 9e85757d5599..976fe432f4e2 100644
--- a/drivers/gpu/drm/radeon/radeon_vce.c
+++ b/drivers/gpu/drm/radeon/radeon_vce.c
@@ -453,11 +453,11 @@ int radeon_vce_cs_reloc(struct radeon_cs_parser *p, int lo, int hi,
 			unsigned size)
 {
 	struct radeon_cs_chunk *relocs_chunk;
-	struct radeon_cs_reloc *reloc;
+	struct radeon_bo_list *reloc;
 	uint64_t start, end, offset;
 	unsigned idx;
 
-	relocs_chunk = &p->chunks[p->chunk_relocs_idx];
+	relocs_chunk = p->chunk_relocs;
 	offset = radeon_get_ib_value(p, lo);
 	idx = radeon_get_ib_value(p, hi);
 
@@ -467,7 +467,7 @@ int radeon_vce_cs_reloc(struct radeon_cs_parser *p, int lo, int hi,
 		return -EINVAL;
 	}
 
-	reloc = p->relocs_ptr[(idx / 4)];
+	reloc = &p->relocs[(idx / 4)];
 	start = reloc->gpu_offset;
 	end = start + radeon_bo_size(reloc->robj);
 	start += offset;
@@ -534,7 +534,7 @@ int radeon_vce_cs_parse(struct radeon_cs_parser *p)
 	uint32_t *size = &tmp;
 	int i, r;
 
-	while (p->idx < p->chunks[p->chunk_ib_idx].length_dw) {
+	while (p->idx < p->chunk_ib->length_dw) {
 		uint32_t len = radeon_get_ib_value(p, p->idx);
 		uint32_t cmd = radeon_get_ib_value(p, p->idx + 1);
 
diff --git a/drivers/gpu/drm/radeon/radeon_vm.c b/drivers/gpu/drm/radeon/radeon_vm.c
index dfde266529e2..cde48c42b30a 100644
--- a/drivers/gpu/drm/radeon/radeon_vm.c
+++ b/drivers/gpu/drm/radeon/radeon_vm.c
@@ -125,41 +125,37 @@ void radeon_vm_manager_fini(struct radeon_device *rdev)
  * Add the page directory to the list of BOs to
  * validate for command submission (cayman+).
  */
-struct radeon_cs_reloc *radeon_vm_get_bos(struct radeon_device *rdev,
+struct radeon_bo_list *radeon_vm_get_bos(struct radeon_device *rdev,
 					  struct radeon_vm *vm,
 					  struct list_head *head)
 {
-	struct radeon_cs_reloc *list;
+	struct radeon_bo_list *list;
 	unsigned i, idx;
 
 	list = drm_malloc_ab(vm->max_pde_used + 2,
-			     sizeof(struct radeon_cs_reloc));
+			     sizeof(struct radeon_bo_list));
 	if (!list)
 		return NULL;
 
 	/* add the vm page table to the list */
-	list[0].gobj = NULL;
 	list[0].robj = vm->page_directory;
 	list[0].prefered_domains = RADEON_GEM_DOMAIN_VRAM;
 	list[0].allowed_domains = RADEON_GEM_DOMAIN_VRAM;
 	list[0].tv.bo = &vm->page_directory->tbo;
-	list[0].tv.shared = false;
+	list[0].tv.shared = true;
 	list[0].tiling_flags = 0;
-	list[0].handle = 0;
 	list_add(&list[0].tv.head, head);
 
 	for (i = 0, idx = 1; i <= vm->max_pde_used; i++) {
 		if (!vm->page_tables[i].bo)
 			continue;
 
-		list[idx].gobj = NULL;
 		list[idx].robj = vm->page_tables[i].bo;
 		list[idx].prefered_domains = RADEON_GEM_DOMAIN_VRAM;
 		list[idx].allowed_domains = RADEON_GEM_DOMAIN_VRAM;
 		list[idx].tv.bo = &list[idx].robj->tbo;
-		list[idx].tv.shared = false;
+		list[idx].tv.shared = true;
 		list[idx].tiling_flags = 0;
-		list[idx].handle = 0;
 		list_add(&list[idx++].tv.head, head);
 	}
 
@@ -182,15 +178,18 @@ struct radeon_fence *radeon_vm_grab_id(struct radeon_device *rdev,
 				       struct radeon_vm *vm, int ring)
 {
 	struct radeon_fence *best[RADEON_NUM_RINGS] = {};
+	struct radeon_vm_id *vm_id = &vm->ids[ring];
+
 	unsigned choices[2] = {};
 	unsigned i;
 
 	/* check if the id is still valid */
-	if (vm->last_id_use && vm->last_id_use == rdev->vm_manager.active[vm->id])
+	if (vm_id->id && vm_id->last_id_use &&
+	    vm_id->last_id_use == rdev->vm_manager.active[vm_id->id])
 		return NULL;
 
 	/* we definately need to flush */
-	radeon_fence_unref(&vm->last_flush);
+	vm_id->pd_gpu_addr = ~0ll;
 
 	/* skip over VMID 0, since it is the system VM */
 	for (i = 1; i < rdev->vm_manager.nvm; ++i) {
@@ -198,8 +197,8 @@ struct radeon_fence *radeon_vm_grab_id(struct radeon_device *rdev,
 
 		if (fence == NULL) {
 			/* found a free one */
-			vm->id = i;
-			trace_radeon_vm_grab_id(vm->id, ring);
+			vm_id->id = i;
+			trace_radeon_vm_grab_id(i, ring);
 			return NULL;
 		}
 
@@ -211,8 +210,8 @@ struct radeon_fence *radeon_vm_grab_id(struct radeon_device *rdev,
 
 	for (i = 0; i < 2; ++i) {
 		if (choices[i]) {
-			vm->id = choices[i];
-			trace_radeon_vm_grab_id(vm->id, ring);
+			vm_id->id = choices[i];
+			trace_radeon_vm_grab_id(choices[i], ring);
 			return rdev->vm_manager.active[choices[i]];
 		}
 	}
@@ -228,6 +227,7 @@ struct radeon_fence *radeon_vm_grab_id(struct radeon_device *rdev,
  * @rdev: radeon_device pointer
  * @vm: vm we want to flush
  * @ring: ring to use for flush
+ * @updates: last vm update that is waited for
  *
  * Flush the vm (cayman+).
  *
@@ -235,15 +235,21 @@ struct radeon_fence *radeon_vm_grab_id(struct radeon_device *rdev,
  */
 void radeon_vm_flush(struct radeon_device *rdev,
 		     struct radeon_vm *vm,
-		     int ring)
+		     int ring, struct radeon_fence *updates)
 {
 	uint64_t pd_addr = radeon_bo_gpu_offset(vm->page_directory);
+	struct radeon_vm_id *vm_id = &vm->ids[ring];
+
+	if (pd_addr != vm_id->pd_gpu_addr || !vm_id->flushed_updates ||
+	    radeon_fence_is_earlier(vm_id->flushed_updates, updates)) {
+
+		trace_radeon_vm_flush(pd_addr, ring, vm->ids[ring].id);
+		radeon_fence_unref(&vm_id->flushed_updates);
+		vm_id->flushed_updates = radeon_fence_ref(updates);
+		vm_id->pd_gpu_addr = pd_addr;
+		radeon_ring_vm_flush(rdev, &rdev->ring[ring],
+				     vm_id->id, vm_id->pd_gpu_addr);
 
-	/* if we can't remember our last VM flush then flush now! */
-	if (!vm->last_flush || pd_addr != vm->pd_gpu_addr) {
-		trace_radeon_vm_flush(pd_addr, ring, vm->id);
-		vm->pd_gpu_addr = pd_addr;
-		radeon_ring_vm_flush(rdev, ring, vm);
 	}
 }
 
@@ -263,18 +269,13 @@ void radeon_vm_fence(struct radeon_device *rdev,
 		     struct radeon_vm *vm,
 		     struct radeon_fence *fence)
 {
-	radeon_fence_unref(&vm->fence);
-	vm->fence = radeon_fence_ref(fence);
-
-	radeon_fence_unref(&rdev->vm_manager.active[vm->id]);
-	rdev->vm_manager.active[vm->id] = radeon_fence_ref(fence);
+	unsigned vm_id = vm->ids[fence->ring].id;
 
-	radeon_fence_unref(&vm->last_id_use);
-	vm->last_id_use = radeon_fence_ref(fence);
+	radeon_fence_unref(&rdev->vm_manager.active[vm_id]);
+	rdev->vm_manager.active[vm_id] = radeon_fence_ref(fence);
 
-        /* we just flushed the VM, remember that */
-        if (!vm->last_flush)
-                vm->last_flush = radeon_fence_ref(fence);
+	radeon_fence_unref(&vm->ids[fence->ring].last_id_use);
+	vm->ids[fence->ring].last_id_use = radeon_fence_ref(fence);
 }
 
 /**
@@ -387,35 +388,25 @@ static void radeon_vm_set_pages(struct radeon_device *rdev,
 static int radeon_vm_clear_bo(struct radeon_device *rdev,
 			      struct radeon_bo *bo)
 {
-        struct ttm_validate_buffer tv;
-        struct ww_acquire_ctx ticket;
-        struct list_head head;
 	struct radeon_ib ib;
 	unsigned entries;
 	uint64_t addr;
 	int r;
 
-        memset(&tv, 0, sizeof(tv));
-        tv.bo = &bo->tbo;
-	tv.shared = false;
-
-        INIT_LIST_HEAD(&head);
-        list_add(&tv.head, &head);
-
-        r = ttm_eu_reserve_buffers(&ticket, &head, true);
-        if (r)
+	r = radeon_bo_reserve(bo, false);
+	if (r)
 		return r;
 
-        r = ttm_bo_validate(&bo->tbo, &bo->placement, true, false);
-        if (r)
-                goto error;
+	r = ttm_bo_validate(&bo->tbo, &bo->placement, true, false);
+	if (r)
+		goto error_unreserve;
 
 	addr = radeon_bo_gpu_offset(bo);
 	entries = radeon_bo_size(bo) / 8;
 
 	r = radeon_ib_get(rdev, R600_RING_TYPE_DMA_INDEX, &ib, NULL, 256);
 	if (r)
-                goto error;
+		goto error_unreserve;
 
 	ib.length_dw = 0;
 
@@ -425,15 +416,16 @@ static int radeon_vm_clear_bo(struct radeon_device *rdev,
 
 	r = radeon_ib_schedule(rdev, &ib, NULL, false);
 	if (r)
-                goto error;
+		goto error_free;
 
-	ttm_eu_fence_buffer_objects(&ticket, &head, &ib.fence->base);
-	radeon_ib_free(rdev, &ib);
+	ib.fence->is_vm_update = true;
+	radeon_bo_fence(bo, ib.fence, false);
 
-	return 0;
+error_free:
+	radeon_ib_free(rdev, &ib);
 
-error:
-	ttm_eu_backoff_reservation(&ticket, &head);
+error_unreserve:
+	radeon_bo_unreserve(bo);
 	return r;
 }
 
@@ -449,7 +441,7 @@ error:
  * Validate and set the offset requested within the vm address space.
  * Returns 0 for success, error for failure.
  *
- * Object has to be reserved!
+ * Object has to be reserved and gets unreserved by this function!
  */
 int radeon_vm_bo_set_addr(struct radeon_device *rdev,
 			  struct radeon_bo_va *bo_va,
@@ -495,7 +487,9 @@ int radeon_vm_bo_set_addr(struct radeon_device *rdev,
 			tmp->vm = vm;
 			tmp->addr = bo_va->addr;
 			tmp->bo = radeon_bo_ref(bo_va->bo);
+			spin_lock(&vm->status_lock);
 			list_add(&tmp->vm_status, &vm->freed);
+			spin_unlock(&vm->status_lock);
 		}
 
 		interval_tree_remove(&bo_va->it, &vm->va);
@@ -575,7 +569,7 @@ int radeon_vm_bo_set_addr(struct radeon_device *rdev,
 	}
 
 	mutex_unlock(&vm->mutex);
-	return radeon_bo_reserve(bo_va->bo, false);
+	return 0;
 }
 
 /**
@@ -699,17 +693,15 @@ int radeon_vm_update_page_directory(struct radeon_device *rdev,
 	if (ib.length_dw != 0) {
 		radeon_asic_vm_pad_ib(rdev, &ib);
 
-		radeon_semaphore_sync_resv(rdev, ib.semaphore, pd->tbo.resv, false);
-		radeon_semaphore_sync_fence(ib.semaphore, vm->last_id_use);
+		radeon_sync_resv(rdev, &ib.sync, pd->tbo.resv, true);
 		WARN_ON(ib.length_dw > ndw);
 		r = radeon_ib_schedule(rdev, &ib, NULL, false);
 		if (r) {
 			radeon_ib_free(rdev, &ib);
 			return r;
 		}
-		radeon_fence_unref(&vm->fence);
-		vm->fence = radeon_fence_ref(ib.fence);
-		radeon_fence_unref(&vm->last_flush);
+		ib.fence->is_vm_update = true;
+		radeon_bo_fence(pd, ib.fence, false);
 	}
 	radeon_ib_free(rdev, &ib);
 
@@ -808,11 +800,11 @@ static void radeon_vm_frag_ptes(struct radeon_device *rdev,
  *
  * Global and local mutex must be locked!
  */
-static void radeon_vm_update_ptes(struct radeon_device *rdev,
-				  struct radeon_vm *vm,
-				  struct radeon_ib *ib,
-				  uint64_t start, uint64_t end,
-				  uint64_t dst, uint32_t flags)
+static int radeon_vm_update_ptes(struct radeon_device *rdev,
+				 struct radeon_vm *vm,
+				 struct radeon_ib *ib,
+				 uint64_t start, uint64_t end,
+				 uint64_t dst, uint32_t flags)
 {
 	uint64_t mask = RADEON_VM_PTE_COUNT - 1;
 	uint64_t last_pte = ~0, last_dst = ~0;
@@ -825,8 +817,12 @@ static void radeon_vm_update_ptes(struct radeon_device *rdev,
 		struct radeon_bo *pt = vm->page_tables[pt_idx].bo;
 		unsigned nptes;
 		uint64_t pte;
+		int r;
 
-		radeon_semaphore_sync_resv(rdev, ib->semaphore, pt->tbo.resv, false);
+		radeon_sync_resv(rdev, &ib->sync, pt->tbo.resv, true);
+		r = reservation_object_reserve_shared(pt->tbo.resv);
+		if (r)
+			return r;
 
 		if ((addr & ~mask) == (end & ~mask))
 			nptes = end - addr;
@@ -860,6 +856,33 @@ static void radeon_vm_update_ptes(struct radeon_device *rdev,
 				    last_pte + 8 * count,
 				    last_dst, flags);
 	}
+
+	return 0;
+}
+
+/**
+ * radeon_vm_fence_pts - fence page tables after an update
+ *
+ * @vm: requested vm
+ * @start: start of GPU address range
+ * @end: end of GPU address range
+ * @fence: fence to use
+ *
+ * Fence the page tables in the range @start - @end (cayman+).
+ *
+ * Global and local mutex must be locked!
+ */
+static void radeon_vm_fence_pts(struct radeon_vm *vm,
+				uint64_t start, uint64_t end,
+				struct radeon_fence *fence)
+{
+	unsigned i;
+
+	start >>= radeon_vm_block_size;
+	end >>= radeon_vm_block_size;
+
+	for (i = start; i <= end; ++i)
+		radeon_bo_fence(vm->page_tables[i].bo, fence, true);
 }
 
 /**
@@ -892,7 +915,9 @@ int radeon_vm_bo_update(struct radeon_device *rdev,
 		return -EINVAL;
 	}
 
+	spin_lock(&vm->status_lock);
 	list_del_init(&bo_va->vm_status);
+	spin_unlock(&vm->status_lock);
 
 	bo_va->flags &= ~RADEON_VM_PAGE_VALID;
 	bo_va->flags &= ~RADEON_VM_PAGE_SYSTEM;
@@ -961,23 +986,34 @@ int radeon_vm_bo_update(struct radeon_device *rdev,
 		return r;
 	ib.length_dw = 0;
 
-	radeon_vm_update_ptes(rdev, vm, &ib, bo_va->it.start,
-			      bo_va->it.last + 1, addr,
-			      radeon_vm_page_flags(bo_va->flags));
+	if (!(bo_va->flags & RADEON_VM_PAGE_VALID)) {
+		unsigned i;
+
+		for (i = 0; i < RADEON_NUM_RINGS; ++i)
+			radeon_sync_fence(&ib.sync, vm->ids[i].last_id_use);
+	}
+
+	r = radeon_vm_update_ptes(rdev, vm, &ib, bo_va->it.start,
+				  bo_va->it.last + 1, addr,
+				  radeon_vm_page_flags(bo_va->flags));
+	if (r) {
+		radeon_ib_free(rdev, &ib);
+		return r;
+	}
 
 	radeon_asic_vm_pad_ib(rdev, &ib);
 	WARN_ON(ib.length_dw > ndw);
 
-	radeon_semaphore_sync_fence(ib.semaphore, vm->fence);
 	r = radeon_ib_schedule(rdev, &ib, NULL, false);
 	if (r) {
 		radeon_ib_free(rdev, &ib);
 		return r;
 	}
-	radeon_fence_unref(&vm->fence);
-	vm->fence = radeon_fence_ref(ib.fence);
+	ib.fence->is_vm_update = true;
+	radeon_vm_fence_pts(vm, bo_va->it.start, bo_va->it.last + 1, ib.fence);
+	radeon_fence_unref(&bo_va->last_pt_update);
+	bo_va->last_pt_update = radeon_fence_ref(ib.fence);
 	radeon_ib_free(rdev, &ib);
-	radeon_fence_unref(&vm->last_flush);
 
 	return 0;
 }
@@ -996,16 +1032,25 @@ int radeon_vm_bo_update(struct radeon_device *rdev,
 int radeon_vm_clear_freed(struct radeon_device *rdev,
 			  struct radeon_vm *vm)
 {
-	struct radeon_bo_va *bo_va, *tmp;
+	struct radeon_bo_va *bo_va;
 	int r;
 
-	list_for_each_entry_safe(bo_va, tmp, &vm->freed, vm_status) {
+	spin_lock(&vm->status_lock);
+	while (!list_empty(&vm->freed)) {
+		bo_va = list_first_entry(&vm->freed,
+			struct radeon_bo_va, vm_status);
+		spin_unlock(&vm->status_lock);
+
 		r = radeon_vm_bo_update(rdev, bo_va, NULL);
 		radeon_bo_unref(&bo_va->bo);
+		radeon_fence_unref(&bo_va->last_pt_update);
 		kfree(bo_va);
 		if (r)
 			return r;
+
+		spin_lock(&vm->status_lock);
 	}
+	spin_unlock(&vm->status_lock);
 	return 0;
 
 }
@@ -1024,14 +1069,23 @@ int radeon_vm_clear_freed(struct radeon_device *rdev,
 int radeon_vm_clear_invalids(struct radeon_device *rdev,
 			     struct radeon_vm *vm)
 {
-	struct radeon_bo_va *bo_va, *tmp;
+	struct radeon_bo_va *bo_va;
 	int r;
 
-	list_for_each_entry_safe(bo_va, tmp, &vm->invalidated, vm_status) {
+	spin_lock(&vm->status_lock);
+	while (!list_empty(&vm->invalidated)) {
+		bo_va = list_first_entry(&vm->invalidated,
+			struct radeon_bo_va, vm_status);
+		spin_unlock(&vm->status_lock);
+
 		r = radeon_vm_bo_update(rdev, bo_va, NULL);
 		if (r)
 			return r;
+
+		spin_lock(&vm->status_lock);
 	}
+	spin_unlock(&vm->status_lock);
+
 	return 0;
 }
 
@@ -1054,14 +1108,17 @@ void radeon_vm_bo_rmv(struct radeon_device *rdev,
 
 	mutex_lock(&vm->mutex);
 	interval_tree_remove(&bo_va->it, &vm->va);
+	spin_lock(&vm->status_lock);
 	list_del(&bo_va->vm_status);
 
 	if (bo_va->addr) {
 		bo_va->bo = radeon_bo_ref(bo_va->bo);
 		list_add(&bo_va->vm_status, &vm->freed);
 	} else {
+		radeon_fence_unref(&bo_va->last_pt_update);
 		kfree(bo_va);
 	}
+	spin_unlock(&vm->status_lock);
 
 	mutex_unlock(&vm->mutex);
 }
@@ -1082,10 +1139,10 @@ void radeon_vm_bo_invalidate(struct radeon_device *rdev,
 
 	list_for_each_entry(bo_va, &bo->va, bo_list) {
 		if (bo_va->addr) {
-			mutex_lock(&bo_va->vm->mutex);
+			spin_lock(&bo_va->vm->status_lock);
 			list_del(&bo_va->vm_status);
 			list_add(&bo_va->vm_status, &bo_va->vm->invalidated);
-			mutex_unlock(&bo_va->vm->mutex);
+			spin_unlock(&bo_va->vm->status_lock);
 		}
 	}
 }
@@ -1103,15 +1160,17 @@ int radeon_vm_init(struct radeon_device *rdev, struct radeon_vm *vm)
 	const unsigned align = min(RADEON_VM_PTB_ALIGN_SIZE,
 		RADEON_VM_PTE_COUNT * 8);
 	unsigned pd_size, pd_entries, pts_size;
-	int r;
+	int i, r;
 
-	vm->id = 0;
 	vm->ib_bo_va = NULL;
-	vm->fence = NULL;
-	vm->last_flush = NULL;
-	vm->last_id_use = NULL;
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		vm->ids[i].id = 0;
+		vm->ids[i].flushed_updates = NULL;
+		vm->ids[i].last_id_use = NULL;
+	}
 	mutex_init(&vm->mutex);
 	vm->va = RB_ROOT;
+	spin_lock_init(&vm->status_lock);
 	INIT_LIST_HEAD(&vm->invalidated);
 	INIT_LIST_HEAD(&vm->freed);
 
@@ -1165,11 +1224,13 @@ void radeon_vm_fini(struct radeon_device *rdev, struct radeon_vm *vm)
 		if (!r) {
 			list_del_init(&bo_va->bo_list);
 			radeon_bo_unreserve(bo_va->bo);
+			radeon_fence_unref(&bo_va->last_pt_update);
 			kfree(bo_va);
 		}
 	}
 	list_for_each_entry_safe(bo_va, tmp, &vm->freed, vm_status) {
 		radeon_bo_unref(&bo_va->bo);
+		radeon_fence_unref(&bo_va->last_pt_update);
 		kfree(bo_va);
 	}
 
@@ -1179,9 +1240,10 @@ void radeon_vm_fini(struct radeon_device *rdev, struct radeon_vm *vm)
 
 	radeon_bo_unref(&vm->page_directory);
 
-	radeon_fence_unref(&vm->fence);
-	radeon_fence_unref(&vm->last_flush);
-	radeon_fence_unref(&vm->last_id_use);
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		radeon_fence_unref(&vm->ids[i].flushed_updates);
+		radeon_fence_unref(&vm->ids[i].last_id_use);
+	}
 
 	mutex_destroy(&vm->mutex);
 }
diff --git a/drivers/gpu/drm/radeon/rv770_dma.c b/drivers/gpu/drm/radeon/rv770_dma.c
index 7f34bad2e724..acff6e09cc40 100644
--- a/drivers/gpu/drm/radeon/rv770_dma.c
+++ b/drivers/gpu/drm/radeon/rv770_dma.c
@@ -44,31 +44,27 @@ struct radeon_fence *rv770_copy_dma(struct radeon_device *rdev,
 				    unsigned num_gpu_pages,
 				    struct reservation_object *resv)
 {
-	struct radeon_semaphore *sem = NULL;
 	struct radeon_fence *fence;
+	struct radeon_sync sync;
 	int ring_index = rdev->asic->copy.dma_ring_index;
 	struct radeon_ring *ring = &rdev->ring[ring_index];
 	u32 size_in_dw, cur_size_in_dw;
 	int i, num_loops;
 	int r = 0;
 
-	r = radeon_semaphore_create(rdev, &sem);
-	if (r) {
-		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		return ERR_PTR(r);
-	}
+	radeon_sync_create(&sync);
 
 	size_in_dw = (num_gpu_pages << RADEON_GPU_PAGE_SHIFT) / 4;
 	num_loops = DIV_ROUND_UP(size_in_dw, 0xFFFF);
 	r = radeon_ring_lock(rdev, ring, num_loops * 5 + 8);
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		radeon_semaphore_free(rdev, &sem, NULL);
+		radeon_sync_free(rdev, &sync, NULL);
 		return ERR_PTR(r);
 	}
 
-	radeon_semaphore_sync_resv(rdev, sem, resv, false);
-	radeon_semaphore_sync_rings(rdev, sem, ring->idx);
+	radeon_sync_resv(rdev, &sync, resv, false);
+	radeon_sync_rings(rdev, &sync, ring->idx);
 
 	for (i = 0; i < num_loops; i++) {
 		cur_size_in_dw = size_in_dw;
@@ -87,12 +83,12 @@ struct radeon_fence *rv770_copy_dma(struct radeon_device *rdev,
 	r = radeon_fence_emit(rdev, &fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
-		radeon_semaphore_free(rdev, &sem, NULL);
+		radeon_sync_free(rdev, &sync, NULL);
 		return ERR_PTR(r);
 	}
 
 	radeon_ring_unlock_commit(rdev, ring, false);
-	radeon_semaphore_free(rdev, &sem, fence);
+	radeon_sync_free(rdev, &sync, fence);
 
 	return fence;
 }
diff --git a/drivers/gpu/drm/radeon/si.c b/drivers/gpu/drm/radeon/si.c
index 7d5083dc4acb..60df444bd075 100644
--- a/drivers/gpu/drm/radeon/si.c
+++ b/drivers/gpu/drm/radeon/si.c
@@ -3365,6 +3365,7 @@ void si_fence_ring_emit(struct radeon_device *rdev,
 void si_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 {
 	struct radeon_ring *ring = &rdev->ring[ib->ring];
+	unsigned vm_id = ib->vm ? ib->vm->ids[ib->ring].id : 0;
 	u32 header;
 
 	if (ib->is_const_ib) {
@@ -3400,14 +3401,13 @@ void si_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 #endif
 			  (ib->gpu_addr & 0xFFFFFFFC));
 	radeon_ring_write(ring, upper_32_bits(ib->gpu_addr) & 0xFFFF);
-	radeon_ring_write(ring, ib->length_dw |
-			  (ib->vm ? (ib->vm->id << 24) : 0));
+	radeon_ring_write(ring, ib->length_dw | (vm_id << 24));
 
 	if (!ib->is_const_ib) {
 		/* flush read cache over gart for this vmid */
 		radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
 		radeon_ring_write(ring, (CP_COHER_CNTL2 - PACKET3_SET_CONFIG_REG_START) >> 2);
-		radeon_ring_write(ring, ib->vm ? ib->vm->id : 0);
+		radeon_ring_write(ring, vm_id);
 		radeon_ring_write(ring, PACKET3(PACKET3_SURFACE_SYNC, 3));
 		radeon_ring_write(ring, PACKET3_TCL1_ACTION_ENA |
 				  PACKET3_TC_ACTION_ENA |
@@ -5023,27 +5023,23 @@ static void si_vm_decode_fault(struct radeon_device *rdev,
 	       block, mc_id);
 }
 
-void si_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
+void si_vm_flush(struct radeon_device *rdev, struct radeon_ring *ring,
+		 unsigned vm_id, uint64_t pd_addr)
 {
-	struct radeon_ring *ring = &rdev->ring[ridx];
-
-	if (vm == NULL)
-		return;
-
 	/* write new base address */
 	radeon_ring_write(ring, PACKET3(PACKET3_WRITE_DATA, 3));
 	radeon_ring_write(ring, (WRITE_DATA_ENGINE_SEL(1) |
 				 WRITE_DATA_DST_SEL(0)));
 
-	if (vm->id < 8) {
+	if (vm_id < 8) {
 		radeon_ring_write(ring,
-				  (VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (vm->id << 2)) >> 2);
+				  (VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (vm_id << 2)) >> 2);
 	} else {
 		radeon_ring_write(ring,
-				  (VM_CONTEXT8_PAGE_TABLE_BASE_ADDR + ((vm->id - 8) << 2)) >> 2);
+				  (VM_CONTEXT8_PAGE_TABLE_BASE_ADDR + ((vm_id - 8) << 2)) >> 2);
 	}
 	radeon_ring_write(ring, 0);
-	radeon_ring_write(ring, vm->pd_gpu_addr >> 12);
+	radeon_ring_write(ring, pd_addr >> 12);
 
 	/* flush hdp cache */
 	radeon_ring_write(ring, PACKET3(PACKET3_WRITE_DATA, 3));
@@ -5059,7 +5055,7 @@ void si_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
 				 WRITE_DATA_DST_SEL(0)));
 	radeon_ring_write(ring, VM_INVALIDATE_REQUEST >> 2);
 	radeon_ring_write(ring, 0);
-	radeon_ring_write(ring, 1 << vm->id);
+	radeon_ring_write(ring, 1 << vm_id);
 
 	/* sync PFP to ME, otherwise we might get invalid PFP reads */
 	radeon_ring_write(ring, PACKET3(PACKET3_PFP_SYNC_ME, 0));
diff --git a/drivers/gpu/drm/radeon/si_dma.c b/drivers/gpu/drm/radeon/si_dma.c
index b58f12b762d7..f5cc777e1c5f 100644
--- a/drivers/gpu/drm/radeon/si_dma.c
+++ b/drivers/gpu/drm/radeon/si_dma.c
@@ -185,20 +185,17 @@ void si_dma_vm_set_pages(struct radeon_device *rdev,
 	}
 }
 
-void si_dma_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
-{
-	struct radeon_ring *ring = &rdev->ring[ridx];
-
-	if (vm == NULL)
-		return;
+void si_dma_vm_flush(struct radeon_device *rdev, struct radeon_ring *ring,
+		     unsigned vm_id, uint64_t pd_addr)
 
+{
 	radeon_ring_write(ring, DMA_PACKET(DMA_PACKET_SRBM_WRITE, 0, 0, 0, 0));
-	if (vm->id < 8) {
-		radeon_ring_write(ring, (0xf << 16) | ((VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (vm->id << 2)) >> 2));
+	if (vm_id < 8) {
+		radeon_ring_write(ring, (0xf << 16) | ((VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (vm_id << 2)) >> 2));
 	} else {
-		radeon_ring_write(ring, (0xf << 16) | ((VM_CONTEXT8_PAGE_TABLE_BASE_ADDR + ((vm->id - 8) << 2)) >> 2));
+		radeon_ring_write(ring, (0xf << 16) | ((VM_CONTEXT8_PAGE_TABLE_BASE_ADDR + ((vm_id - 8) << 2)) >> 2));
 	}
-	radeon_ring_write(ring, vm->pd_gpu_addr >> 12);
+	radeon_ring_write(ring, pd_addr >> 12);
 
 	/* flush hdp cache */
 	radeon_ring_write(ring, DMA_PACKET(DMA_PACKET_SRBM_WRITE, 0, 0, 0, 0));
@@ -208,7 +205,7 @@ void si_dma_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
 	/* bits 0-7 are the VM contexts0-7 */
 	radeon_ring_write(ring, DMA_PACKET(DMA_PACKET_SRBM_WRITE, 0, 0, 0, 0));
 	radeon_ring_write(ring, (0xf << 16) | (VM_INVALIDATE_REQUEST >> 2));
-	radeon_ring_write(ring, 1 << vm->id);
+	radeon_ring_write(ring, 1 << vm_id);
 }
 
 /**
@@ -229,31 +226,27 @@ struct radeon_fence *si_copy_dma(struct radeon_device *rdev,
 				 unsigned num_gpu_pages,
 				 struct reservation_object *resv)
 {
-	struct radeon_semaphore *sem = NULL;
 	struct radeon_fence *fence;
+	struct radeon_sync sync;
 	int ring_index = rdev->asic->copy.dma_ring_index;
 	struct radeon_ring *ring = &rdev->ring[ring_index];
 	u32 size_in_bytes, cur_size_in_bytes;
 	int i, num_loops;
 	int r = 0;
 
-	r = radeon_semaphore_create(rdev, &sem);
-	if (r) {
-		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		return ERR_PTR(r);
-	}
+	radeon_sync_create(&sync);
 
 	size_in_bytes = (num_gpu_pages << RADEON_GPU_PAGE_SHIFT);
 	num_loops = DIV_ROUND_UP(size_in_bytes, 0xfffff);
 	r = radeon_ring_lock(rdev, ring, num_loops * 5 + 11);
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		radeon_semaphore_free(rdev, &sem, NULL);
+		radeon_sync_free(rdev, &sync, NULL);
 		return ERR_PTR(r);
 	}
 
-	radeon_semaphore_sync_resv(rdev, sem, resv, false);
-	radeon_semaphore_sync_rings(rdev, sem, ring->idx);
+	radeon_sync_resv(rdev, &sync, resv, false);
+	radeon_sync_rings(rdev, &sync, ring->idx);
 
 	for (i = 0; i < num_loops; i++) {
 		cur_size_in_bytes = size_in_bytes;
@@ -272,12 +265,12 @@ struct radeon_fence *si_copy_dma(struct radeon_device *rdev,
 	r = radeon_fence_emit(rdev, &fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
-		radeon_semaphore_free(rdev, &sem, NULL);
+		radeon_sync_free(rdev, &sync, NULL);
 		return ERR_PTR(r);
 	}
 
 	radeon_ring_unlock_commit(rdev, ring, false);
-	radeon_semaphore_free(rdev, &sem, fence);
+	radeon_sync_free(rdev, &sync, fence);
 
 	return fence;
 }
diff --git a/drivers/gpu/drm/radeon/si_dpm.c b/drivers/gpu/drm/radeon/si_dpm.c
index 676e6c2ba90a..32e354b8b0ab 100644
--- a/drivers/gpu/drm/radeon/si_dpm.c
+++ b/drivers/gpu/drm/radeon/si_dpm.c
@@ -3398,6 +3398,15 @@ static int si_process_firmware_header(struct radeon_device *rdev)
 
 	ret = si_read_smc_sram_dword(rdev,
 				     SISLANDS_SMC_FIRMWARE_HEADER_LOCATION +
+				     SISLANDS_SMC_FIRMWARE_HEADER_fanTable,
+				     &tmp, si_pi->sram_end);
+	if (ret)
+		return ret;
+
+	si_pi->fan_table_start = tmp;
+
+	ret = si_read_smc_sram_dword(rdev,
+				     SISLANDS_SMC_FIRMWARE_HEADER_LOCATION +
 				     SISLANDS_SMC_FIRMWARE_HEADER_mcArbDramAutoRefreshTable,
 				     &tmp, si_pi->sram_end);
 	if (ret)
@@ -5817,8 +5826,33 @@ void si_dpm_setup_asic(struct radeon_device *rdev)
 	si_enable_acpi_power_management(rdev);
 }
 
-static int si_set_thermal_temperature_range(struct radeon_device *rdev,
-					int min_temp, int max_temp)
+static int si_thermal_enable_alert(struct radeon_device *rdev,
+				   bool enable)
+{
+	u32 thermal_int = RREG32(CG_THERMAL_INT);
+
+	if (enable) {
+		PPSMC_Result result;
+
+		thermal_int &= ~(THERM_INT_MASK_HIGH | THERM_INT_MASK_LOW);
+		WREG32(CG_THERMAL_INT, thermal_int);
+		rdev->irq.dpm_thermal = false;
+		result = si_send_msg_to_smc(rdev, PPSMC_MSG_EnableThermalInterrupt);
+		if (result != PPSMC_Result_OK) {
+			DRM_DEBUG_KMS("Could not enable thermal interrupts.\n");
+			return -EINVAL;
+		}
+	} else {
+		thermal_int |= THERM_INT_MASK_HIGH | THERM_INT_MASK_LOW;
+		WREG32(CG_THERMAL_INT, thermal_int);
+		rdev->irq.dpm_thermal = true;
+	}
+
+	return 0;
+}
+
+static int si_thermal_set_temperature_range(struct radeon_device *rdev,
+					    int min_temp, int max_temp)
 {
 	int low_temp = 0 * 1000;
 	int high_temp = 255 * 1000;
@@ -5842,6 +5876,309 @@ static int si_set_thermal_temperature_range(struct radeon_device *rdev,
 	return 0;
 }
 
+static void si_fan_ctrl_set_static_mode(struct radeon_device *rdev, u32 mode)
+{
+	struct si_power_info *si_pi = si_get_pi(rdev);
+	u32 tmp;
+
+	if (si_pi->fan_ctrl_is_in_default_mode) {
+		tmp = (RREG32(CG_FDO_CTRL2) & FDO_PWM_MODE_MASK) >> FDO_PWM_MODE_SHIFT;
+		si_pi->fan_ctrl_default_mode = tmp;
+		tmp = (RREG32(CG_FDO_CTRL2) & TMIN_MASK) >> TMIN_SHIFT;
+		si_pi->t_min = tmp;
+		si_pi->fan_ctrl_is_in_default_mode = false;
+	}
+
+	tmp = RREG32(CG_FDO_CTRL2) & ~TMIN_MASK;
+	tmp |= TMIN(0);
+	WREG32(CG_FDO_CTRL2, tmp);
+
+	tmp = RREG32(CG_FDO_CTRL2) & ~FDO_PWM_MODE_MASK;
+	tmp |= FDO_PWM_MODE(mode);
+	WREG32(CG_FDO_CTRL2, tmp);
+}
+
+static int si_thermal_setup_fan_table(struct radeon_device *rdev)
+{
+	struct si_power_info *si_pi = si_get_pi(rdev);
+	PP_SIslands_FanTable fan_table = { FDO_MODE_HARDWARE };
+	u32 duty100;
+	u32 t_diff1, t_diff2, pwm_diff1, pwm_diff2;
+	u16 fdo_min, slope1, slope2;
+	u32 reference_clock, tmp;
+	int ret;
+	u64 tmp64;
+
+	if (!si_pi->fan_table_start) {
+		rdev->pm.dpm.fan.ucode_fan_control = false;
+		return 0;
+	}
+
+	duty100 = (RREG32(CG_FDO_CTRL1) & FMAX_DUTY100_MASK) >> FMAX_DUTY100_SHIFT;
+
+	if (duty100 == 0) {
+		rdev->pm.dpm.fan.ucode_fan_control = false;
+		return 0;
+	}
+
+	tmp64 = (u64)rdev->pm.dpm.fan.pwm_min * duty100;
+	do_div(tmp64, 10000);
+	fdo_min = (u16)tmp64;
+
+	t_diff1 = rdev->pm.dpm.fan.t_med - rdev->pm.dpm.fan.t_min;
+	t_diff2 = rdev->pm.dpm.fan.t_high - rdev->pm.dpm.fan.t_med;
+
+	pwm_diff1 = rdev->pm.dpm.fan.pwm_med - rdev->pm.dpm.fan.pwm_min;
+	pwm_diff2 = rdev->pm.dpm.fan.pwm_high - rdev->pm.dpm.fan.pwm_med;
+
+	slope1 = (u16)((50 + ((16 * duty100 * pwm_diff1) / t_diff1)) / 100);
+	slope2 = (u16)((50 + ((16 * duty100 * pwm_diff2) / t_diff2)) / 100);
+
+	fan_table.slope1 = cpu_to_be16(slope1);
+	fan_table.slope2 = cpu_to_be16(slope2);
+
+	fan_table.fdo_min = cpu_to_be16(fdo_min);
+
+	fan_table.hys_down = cpu_to_be16(rdev->pm.dpm.fan.t_hyst);
+
+	fan_table.hys_up = cpu_to_be16(1);
+
+	fan_table.hys_slope = cpu_to_be16(1);
+
+	fan_table.temp_resp_lim = cpu_to_be16(5);
+
+	reference_clock = radeon_get_xclk(rdev);
+
+	fan_table.refresh_period = cpu_to_be32((rdev->pm.dpm.fan.cycle_delay *
+						reference_clock) / 1600);
+
+	fan_table.fdo_max = cpu_to_be16((u16)duty100);
+
+	tmp = (RREG32(CG_MULT_THERMAL_CTRL) & TEMP_SEL_MASK) >> TEMP_SEL_SHIFT;
+	fan_table.temp_src = (uint8_t)tmp;
+
+	ret = si_copy_bytes_to_smc(rdev,
+				   si_pi->fan_table_start,
+				   (u8 *)(&fan_table),
+				   sizeof(fan_table),
+				   si_pi->sram_end);
+
+	if (ret) {
+		DRM_ERROR("Failed to load fan table to the SMC.");
+		rdev->pm.dpm.fan.ucode_fan_control = false;
+	}
+
+	return 0;
+}
+
+static int si_fan_ctrl_start_smc_fan_control(struct radeon_device *rdev)
+{
+	PPSMC_Result ret;
+
+	ret = si_send_msg_to_smc(rdev, PPSMC_StartFanControl);
+	if (ret == PPSMC_Result_OK)
+		return 0;
+	else
+		return -EINVAL;
+}
+
+static int si_fan_ctrl_stop_smc_fan_control(struct radeon_device *rdev)
+{
+	PPSMC_Result ret;
+
+	ret = si_send_msg_to_smc(rdev, PPSMC_StopFanControl);
+	if (ret == PPSMC_Result_OK)
+		return 0;
+	else
+		return -EINVAL;
+}
+
+#if 0
+static int si_fan_ctrl_get_fan_speed_percent(struct radeon_device *rdev,
+					     u32 *speed)
+{
+	u32 duty, duty100;
+	u64 tmp64;
+
+	if (rdev->pm.no_fan)
+		return -ENOENT;
+
+	duty100 = (RREG32(CG_FDO_CTRL1) & FMAX_DUTY100_MASK) >> FMAX_DUTY100_SHIFT;
+	duty = (RREG32(CG_THERMAL_STATUS) & FDO_PWM_DUTY_MASK) >> FDO_PWM_DUTY_SHIFT;
+
+	if (duty100 == 0)
+		return -EINVAL;
+
+	tmp64 = (u64)duty * 100;
+	do_div(tmp64, duty100);
+	*speed = (u32)tmp64;
+
+	if (*speed > 100)
+		*speed = 100;
+
+	return 0;
+}
+
+static int si_fan_ctrl_set_fan_speed_percent(struct radeon_device *rdev,
+					     u32 speed)
+{
+	u32 tmp;
+	u32 duty, duty100;
+	u64 tmp64;
+
+	if (rdev->pm.no_fan)
+		return -ENOENT;
+
+	if (speed > 100)
+		return -EINVAL;
+
+	if (rdev->pm.dpm.fan.ucode_fan_control)
+		si_fan_ctrl_stop_smc_fan_control(rdev);
+
+	duty100 = (RREG32(CG_FDO_CTRL1) & FMAX_DUTY100_MASK) >> FMAX_DUTY100_SHIFT;
+
+	if (duty100 == 0)
+		return -EINVAL;
+
+	tmp64 = (u64)speed * duty100;
+	do_div(tmp64, 100);
+	duty = (u32)tmp64;
+
+	tmp = RREG32(CG_FDO_CTRL0) & ~FDO_STATIC_DUTY_MASK;
+	tmp |= FDO_STATIC_DUTY(duty);
+	WREG32(CG_FDO_CTRL0, tmp);
+
+	si_fan_ctrl_set_static_mode(rdev, FDO_PWM_MODE_STATIC);
+
+	return 0;
+}
+
+static int si_fan_ctrl_get_fan_speed_rpm(struct radeon_device *rdev,
+					 u32 *speed)
+{
+	u32 tach_period;
+	u32 xclk = radeon_get_xclk(rdev);
+
+	if (rdev->pm.no_fan)
+		return -ENOENT;
+
+	if (rdev->pm.fan_pulses_per_revolution == 0)
+		return -ENOENT;
+
+	tach_period = (RREG32(CG_TACH_STATUS) & TACH_PERIOD_MASK) >> TACH_PERIOD_SHIFT;
+	if (tach_period == 0)
+		return -ENOENT;
+
+	*speed = 60 * xclk * 10000 / tach_period;
+
+	return 0;
+}
+
+static int si_fan_ctrl_set_fan_speed_rpm(struct radeon_device *rdev,
+					 u32 speed)
+{
+	u32 tach_period, tmp;
+	u32 xclk = radeon_get_xclk(rdev);
+
+	if (rdev->pm.no_fan)
+		return -ENOENT;
+
+	if (rdev->pm.fan_pulses_per_revolution == 0)
+		return -ENOENT;
+
+	if ((speed < rdev->pm.fan_min_rpm) ||
+	    (speed > rdev->pm.fan_max_rpm))
+		return -EINVAL;
+
+	if (rdev->pm.dpm.fan.ucode_fan_control)
+		si_fan_ctrl_stop_smc_fan_control(rdev);
+
+	tach_period = 60 * xclk * 10000 / (8 * speed);
+	tmp = RREG32(CG_TACH_CTRL) & ~TARGET_PERIOD_MASK;
+	tmp |= TARGET_PERIOD(tach_period);
+	WREG32(CG_TACH_CTRL, tmp);
+
+	si_fan_ctrl_set_static_mode(rdev, FDO_PWM_MODE_STATIC_RPM);
+
+	return 0;
+}
+#endif
+
+static void si_fan_ctrl_set_default_mode(struct radeon_device *rdev)
+{
+	struct si_power_info *si_pi = si_get_pi(rdev);
+	u32 tmp;
+
+	if (!si_pi->fan_ctrl_is_in_default_mode) {
+		tmp = RREG32(CG_FDO_CTRL2) & ~FDO_PWM_MODE_MASK;
+		tmp |= FDO_PWM_MODE(si_pi->fan_ctrl_default_mode);
+		WREG32(CG_FDO_CTRL2, tmp);
+
+		tmp = RREG32(CG_FDO_CTRL2) & ~TMIN_MASK;
+		tmp |= TMIN(si_pi->t_min);
+		WREG32(CG_FDO_CTRL2, tmp);
+		si_pi->fan_ctrl_is_in_default_mode = true;
+	}
+}
+
+static void si_thermal_start_smc_fan_control(struct radeon_device *rdev)
+{
+	if (rdev->pm.dpm.fan.ucode_fan_control) {
+		si_fan_ctrl_start_smc_fan_control(rdev);
+		si_fan_ctrl_set_static_mode(rdev, FDO_PWM_MODE_STATIC);
+	}
+}
+
+static void si_thermal_initialize(struct radeon_device *rdev)
+{
+	u32 tmp;
+
+	if (rdev->pm.fan_pulses_per_revolution) {
+		tmp = RREG32(CG_TACH_CTRL) & ~EDGE_PER_REV_MASK;
+		tmp |= EDGE_PER_REV(rdev->pm.fan_pulses_per_revolution -1);
+		WREG32(CG_TACH_CTRL, tmp);
+	}
+
+	tmp = RREG32(CG_FDO_CTRL2) & ~TACH_PWM_RESP_RATE_MASK;
+	tmp |= TACH_PWM_RESP_RATE(0x28);
+	WREG32(CG_FDO_CTRL2, tmp);
+}
+
+static int si_thermal_start_thermal_controller(struct radeon_device *rdev)
+{
+	int ret;
+
+	si_thermal_initialize(rdev);
+	ret = si_thermal_set_temperature_range(rdev, R600_TEMP_RANGE_MIN, R600_TEMP_RANGE_MAX);
+	if (ret)
+		return ret;
+	ret = si_thermal_enable_alert(rdev, true);
+	if (ret)
+		return ret;
+	if (rdev->pm.dpm.fan.ucode_fan_control) {
+		ret = si_halt_smc(rdev);
+		if (ret)
+			return ret;
+		ret = si_thermal_setup_fan_table(rdev);
+		if (ret)
+			return ret;
+		ret = si_resume_smc(rdev);
+		if (ret)
+			return ret;
+		si_thermal_start_smc_fan_control(rdev);
+	}
+
+	return 0;
+}
+
+static void si_thermal_stop_thermal_controller(struct radeon_device *rdev)
+{
+	if (!rdev->pm.no_fan) {
+		si_fan_ctrl_set_default_mode(rdev);
+		si_fan_ctrl_stop_smc_fan_control(rdev);
+	}
+}
+
 int si_dpm_enable(struct radeon_device *rdev)
 {
 	struct rv7xx_power_info *pi = rv770_get_pi(rdev);
@@ -5954,31 +6291,39 @@ int si_dpm_enable(struct radeon_device *rdev)
 
 	si_enable_auto_throttle_source(rdev, RADEON_DPM_AUTO_THROTTLE_SRC_THERMAL, true);
 
+	si_thermal_start_thermal_controller(rdev);
+
 	ni_update_current_ps(rdev, boot_ps);
 
 	return 0;
 }
 
-int si_dpm_late_enable(struct radeon_device *rdev)
+static int si_set_temperature_range(struct radeon_device *rdev)
 {
 	int ret;
 
-	if (rdev->irq.installed &&
-	    r600_is_internal_thermal_sensor(rdev->pm.int_thermal_type)) {
-		PPSMC_Result result;
+	ret = si_thermal_enable_alert(rdev, false);
+	if (ret)
+		return ret;
+	ret = si_thermal_set_temperature_range(rdev, R600_TEMP_RANGE_MIN, R600_TEMP_RANGE_MAX);
+	if (ret)
+		return ret;
+	ret = si_thermal_enable_alert(rdev, true);
+	if (ret)
+		return ret;
 
-		ret = si_set_thermal_temperature_range(rdev, R600_TEMP_RANGE_MIN, R600_TEMP_RANGE_MAX);
-		if (ret)
-			return ret;
-		rdev->irq.dpm_thermal = true;
-		radeon_irq_set(rdev);
-		result = si_send_msg_to_smc(rdev, PPSMC_MSG_EnableThermalInterrupt);
+	return ret;
+}
 
-		if (result != PPSMC_Result_OK)
-			DRM_DEBUG_KMS("Could not enable thermal interrupts.\n");
-	}
+int si_dpm_late_enable(struct radeon_device *rdev)
+{
+	int ret;
 
-	return 0;
+	ret = si_set_temperature_range(rdev);
+	if (ret)
+		return ret;
+
+	return ret;
 }
 
 void si_dpm_disable(struct radeon_device *rdev)
@@ -5988,6 +6333,7 @@ void si_dpm_disable(struct radeon_device *rdev)
 
 	if (!si_is_smc_running(rdev))
 		return;
+	si_thermal_stop_thermal_controller(rdev);
 	si_disable_ulv(rdev);
 	si_clear_vc(rdev);
 	if (pi->thermal_protection)
@@ -6526,6 +6872,9 @@ int si_dpm_init(struct radeon_device *rdev)
 		rdev->pm.dpm.dyn_state.max_clock_voltage_on_dc =
 			rdev->pm.dpm.dyn_state.max_clock_voltage_on_ac;
 
+	si_pi->fan_ctrl_is_in_default_mode = true;
+	rdev->pm.dpm.fan.ucode_fan_control = false;
+
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/radeon/si_dpm.h b/drivers/gpu/drm/radeon/si_dpm.h
index 8b5c06a0832d..d16bb1b5f10f 100644
--- a/drivers/gpu/drm/radeon/si_dpm.h
+++ b/drivers/gpu/drm/radeon/si_dpm.h
@@ -182,6 +182,7 @@ struct si_power_info {
 	u32 dte_table_start;
 	u32 spll_table_start;
 	u32 papm_cfg_table_start;
+	u32 fan_table_start;
 	/* CAC stuff */
 	const struct si_cac_config_reg *cac_weights;
 	const struct si_cac_config_reg *lcac_config;
@@ -197,6 +198,10 @@ struct si_power_info {
 	/* SVI2 */
 	u8 svd_gpio_id;
 	u8 svc_gpio_id;
+	/* fan control */
+	bool fan_ctrl_is_in_default_mode;
+	u32 t_min;
+	u32 fan_ctrl_default_mode;
 };
 
 #define SISLANDS_INITIAL_STATE_ARB_INDEX    0
diff --git a/drivers/gpu/drm/radeon/si_smc.c b/drivers/gpu/drm/radeon/si_smc.c
index 73dbc79c959d..e5bb92f16775 100644
--- a/drivers/gpu/drm/radeon/si_smc.c
+++ b/drivers/gpu/drm/radeon/si_smc.c
@@ -135,7 +135,7 @@ void si_reset_smc(struct radeon_device *rdev)
 
 int si_program_jump_on_start(struct radeon_device *rdev)
 {
-	static u8 data[] = { 0x0E, 0x00, 0x40, 0x40 };
+	static const u8 data[] = { 0x0E, 0x00, 0x40, 0x40 };
 
 	return si_copy_bytes_to_smc(rdev, 0x0, data, 4, sizeof(data)+1);
 }
diff --git a/drivers/gpu/drm/radeon/sid.h b/drivers/gpu/drm/radeon/sid.h
index 6635da9ec986..4069be89e585 100644
--- a/drivers/gpu/drm/radeon/sid.h
+++ b/drivers/gpu/drm/radeon/sid.h
@@ -180,7 +180,10 @@
 #define		DIG_THERM_DPM(x)			((x) << 14)
 #define		DIG_THERM_DPM_MASK			0x003FC000
 #define		DIG_THERM_DPM_SHIFT			14
-
+#define	CG_THERMAL_STATUS				0x704
+#define		FDO_PWM_DUTY(x)				((x) << 9)
+#define		FDO_PWM_DUTY_MASK			(0xff << 9)
+#define		FDO_PWM_DUTY_SHIFT			9
 #define	CG_THERMAL_INT					0x708
 #define		DIG_THERM_INTH(x)			((x) << 8)
 #define		DIG_THERM_INTH_MASK			0x0000FF00
@@ -191,6 +194,10 @@
 #define 	THERM_INT_MASK_HIGH			(1 << 24)
 #define 	THERM_INT_MASK_LOW			(1 << 25)
 
+#define	CG_MULT_THERMAL_CTRL					0x710
+#define		TEMP_SEL(x)					((x) << 20)
+#define		TEMP_SEL_MASK					(0xff << 20)
+#define		TEMP_SEL_SHIFT					20
 #define	CG_MULT_THERMAL_STATUS					0x714
 #define		ASIC_MAX_TEMP(x)				((x) << 0)
 #define		ASIC_MAX_TEMP_MASK				0x000001ff
@@ -199,6 +206,37 @@
 #define		CTF_TEMP_MASK					0x0003fe00
 #define		CTF_TEMP_SHIFT					9
 
+#define	CG_FDO_CTRL0					0x754
+#define		FDO_STATIC_DUTY(x)			((x) << 0)
+#define		FDO_STATIC_DUTY_MASK			0x000000FF
+#define		FDO_STATIC_DUTY_SHIFT			0
+#define	CG_FDO_CTRL1					0x758
+#define		FMAX_DUTY100(x)				((x) << 0)
+#define		FMAX_DUTY100_MASK			0x000000FF
+#define		FMAX_DUTY100_SHIFT			0
+#define	CG_FDO_CTRL2					0x75C
+#define		TMIN(x)					((x) << 0)
+#define		TMIN_MASK				0x000000FF
+#define		TMIN_SHIFT				0
+#define		FDO_PWM_MODE(x)				((x) << 11)
+#define		FDO_PWM_MODE_MASK			(7 << 11)
+#define		FDO_PWM_MODE_SHIFT			11
+#define		TACH_PWM_RESP_RATE(x)			((x) << 25)
+#define		TACH_PWM_RESP_RATE_MASK			(0x7f << 25)
+#define		TACH_PWM_RESP_RATE_SHIFT		25
+
+#define CG_TACH_CTRL                                    0x770
+#       define EDGE_PER_REV(x)                          ((x) << 0)
+#       define EDGE_PER_REV_MASK                        (0x7 << 0)
+#       define EDGE_PER_REV_SHIFT                       0
+#       define TARGET_PERIOD(x)                         ((x) << 3)
+#       define TARGET_PERIOD_MASK                       0xfffffff8
+#       define TARGET_PERIOD_SHIFT                      3
+#define CG_TACH_STATUS                                  0x774
+#       define TACH_PERIOD(x)                           ((x) << 0)
+#       define TACH_PERIOD_MASK                         0xffffffff
+#       define TACH_PERIOD_SHIFT                        0
+
 #define GENERAL_PWRMGT                                  0x780
 #       define GLOBAL_PWRMGT_EN                         (1 << 0)
 #       define STATIC_PM_EN                             (1 << 1)
diff --git a/drivers/gpu/drm/radeon/sislands_smc.h b/drivers/gpu/drm/radeon/sislands_smc.h
index 623a0b1e2d9d..3c779838d9ab 100644
--- a/drivers/gpu/drm/radeon/sislands_smc.h
+++ b/drivers/gpu/drm/radeon/sislands_smc.h
@@ -245,6 +245,31 @@ typedef struct SISLANDS_SMC_STATETABLE SISLANDS_SMC_STATETABLE;
 #define SI_SMC_SOFT_REGISTER_svi_rework_gpio_id_svd   0x11c
 #define SI_SMC_SOFT_REGISTER_svi_rework_gpio_id_svc   0x120
 
+struct PP_SIslands_FanTable
+{
+	uint8_t  fdo_mode;
+	uint8_t  padding;
+	int16_t  temp_min;
+	int16_t  temp_med;
+	int16_t  temp_max;
+	int16_t  slope1;
+	int16_t  slope2;
+	int16_t  fdo_min;
+	int16_t  hys_up;
+	int16_t  hys_down;
+	int16_t  hys_slope;
+	int16_t  temp_resp_lim;
+	int16_t  temp_curr;
+	int16_t  slope_curr;
+	int16_t  pwm_curr;
+	uint32_t refresh_period;
+	int16_t  fdo_max;
+	uint8_t  temp_src;
+	int8_t  padding2;
+};
+
+typedef struct PP_SIslands_FanTable PP_SIslands_FanTable;
+
 #define SMC_SISLANDS_LKGE_LUT_NUM_OF_TEMP_ENTRIES 16
 #define SMC_SISLANDS_LKGE_LUT_NUM_OF_VOLT_ENTRIES 32
 
diff --git a/drivers/gpu/drm/radeon/smu7_discrete.h b/drivers/gpu/drm/radeon/smu7_discrete.h
index 82f70c90a9ee..0b0b404ff091 100644
--- a/drivers/gpu/drm/radeon/smu7_discrete.h
+++ b/drivers/gpu/drm/radeon/smu7_discrete.h
@@ -431,6 +431,31 @@ struct SMU7_Discrete_MCRegisters
 
 typedef struct SMU7_Discrete_MCRegisters SMU7_Discrete_MCRegisters;
 
+struct SMU7_Discrete_FanTable
+{
+	uint16_t FdoMode;
+	int16_t  TempMin;
+	int16_t  TempMed;
+	int16_t  TempMax;
+	int16_t  Slope1;
+	int16_t  Slope2;
+	int16_t  FdoMin;
+	int16_t  HystUp;
+	int16_t  HystDown;
+	int16_t  HystSlope;
+	int16_t  TempRespLim;
+	int16_t  TempCurr;
+	int16_t  SlopeCurr;
+	int16_t  PwmCurr;
+	uint32_t RefreshPeriod;
+	int16_t  FdoMax;
+	uint8_t  TempSrc;
+	int8_t   Padding;
+};
+
+typedef struct SMU7_Discrete_FanTable SMU7_Discrete_FanTable;
+
+
 struct SMU7_Discrete_PmFuses {
   // dw0-dw1
   uint8_t BapmVddCVidHiSidd[8];
@@ -462,7 +487,10 @@ struct SMU7_Discrete_PmFuses {
   uint8_t BapmVddCVidHiSidd2[8];
 
   // dw11-dw12
-  uint32_t Reserved6[2];
+  int16_t FuzzyFan_ErrorSetDelta;
+  int16_t FuzzyFan_ErrorRateSetDelta;
+  int16_t FuzzyFan_PwmSetDelta;
+  uint16_t CalcMeasPowerBlend;
 
   // dw13-dw16
   uint8_t GnbLPML[16];
diff --git a/drivers/gpu/drm/rcar-du/Kconfig b/drivers/gpu/drm/rcar-du/Kconfig
index c96f6089f8bf..2324a526de65 100644
--- a/drivers/gpu/drm/rcar-du/Kconfig
+++ b/drivers/gpu/drm/rcar-du/Kconfig
@@ -11,10 +11,17 @@ config DRM_RCAR_DU
 	  Choose this option if you have an R-Car chipset.
 	  If M is selected the module will be called rcar-du-drm.
 
+config DRM_RCAR_HDMI
+	bool "R-Car DU HDMI Encoder Support"
+	depends on DRM_RCAR_DU
+	depends on OF
+	help
+	  Enable support for external HDMI encoders.
+
 config DRM_RCAR_LVDS
 	bool "R-Car DU LVDS Encoder Support"
 	depends on DRM_RCAR_DU
 	depends on ARCH_R8A7790 || ARCH_R8A7791 || COMPILE_TEST
 	help
-	  Enable support the R-Car Display Unit embedded LVDS encoders
-	  (currently only on R8A7790).
+	  Enable support for the R-Car Display Unit embedded LVDS encoders
+	  (currently only on R8A7790 and R8A7791).
diff --git a/drivers/gpu/drm/rcar-du/Makefile b/drivers/gpu/drm/rcar-du/Makefile
index 12b8d4477835..05de1c4097af 100644
--- a/drivers/gpu/drm/rcar-du/Makefile
+++ b/drivers/gpu/drm/rcar-du/Makefile
@@ -7,6 +7,8 @@ rcar-du-drm-y := rcar_du_crtc.o \
 		 rcar_du_plane.o \
 		 rcar_du_vgacon.o
 
+rcar-du-drm-$(CONFIG_DRM_RCAR_HDMI)	+= rcar_du_hdmicon.o \
+					   rcar_du_hdmienc.o
 rcar-du-drm-$(CONFIG_DRM_RCAR_LVDS)	+= rcar_du_lvdsenc.o
 
 obj-$(CONFIG_DRM_RCAR_DU)		+= rcar-du-drm.o
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_crtc.c b/drivers/gpu/drm/rcar-du/rcar_du_crtc.c
index 148b50589181..23cc910951f4 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_crtc.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_crtc.c
@@ -19,6 +19,7 @@
 #include <drm/drm_crtc_helper.h>
 #include <drm/drm_fb_cma_helper.h>
 #include <drm/drm_gem_cma_helper.h>
+#include <drm/drm_plane_helper.h>
 
 #include "rcar_du_crtc.h"
 #include "rcar_du_drv.h"
@@ -585,7 +586,7 @@ int rcar_du_crtc_create(struct rcar_du_group *rgrp, unsigned int index)
 
 	if (irq < 0) {
 		dev_err(rcdu->dev, "no IRQ for CRTC %u\n", index);
-		return ret;
+		return irq;
 	}
 
 	ret = devm_request_irq(rcdu->dev, irq, rcar_du_crtc_irq, irqflags,
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_crtc.h b/drivers/gpu/drm/rcar-du/rcar_du_crtc.h
index e97ae502dec5..984e6083699f 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_crtc.h
+++ b/drivers/gpu/drm/rcar-du/rcar_du_crtc.h
@@ -15,7 +15,6 @@
 #define __RCAR_DU_CRTC_H__
 
 #include <linux/mutex.h>
-#include <linux/platform_data/rcar-du.h>
 
 #include <drm/drmP.h>
 #include <drm/drm_crtc.h>
@@ -41,6 +40,15 @@ struct rcar_du_crtc {
 
 #define to_rcar_crtc(c)	container_of(c, struct rcar_du_crtc, crtc)
 
+enum rcar_du_output {
+	RCAR_DU_OUTPUT_DPAD0,
+	RCAR_DU_OUTPUT_DPAD1,
+	RCAR_DU_OUTPUT_LVDS0,
+	RCAR_DU_OUTPUT_LVDS1,
+	RCAR_DU_OUTPUT_TCON,
+	RCAR_DU_OUTPUT_MAX,
+};
+
 int rcar_du_crtc_create(struct rcar_du_group *rgrp, unsigned int index);
 void rcar_du_crtc_enable_vblank(struct rcar_du_crtc *rcrtc, bool enable);
 void rcar_du_crtc_cancel_page_flip(struct rcar_du_crtc *rcrtc,
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_drv.c b/drivers/gpu/drm/rcar-du/rcar_du_drv.c
index d212efa6a495..7bfa09cf18d5 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_drv.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_drv.c
@@ -146,12 +146,11 @@ static int rcar_du_load(struct drm_device *dev, unsigned long flags)
 {
 	struct platform_device *pdev = dev->platformdev;
 	struct device_node *np = pdev->dev.of_node;
-	struct rcar_du_platform_data *pdata = pdev->dev.platform_data;
 	struct rcar_du_device *rcdu;
 	struct resource *mem;
 	int ret;
 
-	if (pdata == NULL && np == NULL) {
+	if (np == NULL) {
 		dev_err(dev->dev, "no platform data\n");
 		return -ENODEV;
 	}
@@ -163,7 +162,6 @@ static int rcar_du_load(struct drm_device *dev, unsigned long flags)
 	}
 
 	rcdu->dev = &pdev->dev;
-	rcdu->pdata = pdata;
 	rcdu->info = np ? of_match_device(rcar_du_of_table, rcdu->dev)->data
 		   : (void *)platform_get_device_id(pdev)->driver_data;
 	rcdu->ddev = dev;
@@ -330,7 +328,6 @@ static struct platform_driver rcar_du_platform_driver = {
 	.probe		= rcar_du_probe,
 	.remove		= rcar_du_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "rcar-du",
 		.pm	= &rcar_du_pm_ops,
 		.of_match_table = rcar_du_of_table,
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_drv.h b/drivers/gpu/drm/rcar-du/rcar_du_drv.h
index 8e494633c3b3..0a724669f02d 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_drv.h
+++ b/drivers/gpu/drm/rcar-du/rcar_du_drv.h
@@ -15,7 +15,6 @@
 #define __RCAR_DU_DRV_H__
 
 #include <linux/kernel.h>
-#include <linux/platform_data/rcar-du.h>
 
 #include "rcar_du_crtc.h"
 #include "rcar_du_group.h"
@@ -67,7 +66,6 @@ struct rcar_du_device_info {
 
 struct rcar_du_device {
 	struct device *dev;
-	const struct rcar_du_platform_data *pdata;
 	const struct rcar_du_device_info *info;
 
 	void __iomem *mmio;
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_encoder.c b/drivers/gpu/drm/rcar-du/rcar_du_encoder.c
index 7c0ec95915ef..34a122a39664 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_encoder.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_encoder.c
@@ -19,6 +19,8 @@
 
 #include "rcar_du_drv.h"
 #include "rcar_du_encoder.h"
+#include "rcar_du_hdmicon.h"
+#include "rcar_du_hdmienc.h"
 #include "rcar_du_kms.h"
 #include "rcar_du_lvdscon.h"
 #include "rcar_du_lvdsenc.h"
@@ -33,7 +35,7 @@ rcar_du_connector_best_encoder(struct drm_connector *connector)
 {
 	struct rcar_du_connector *rcon = to_rcar_connector(connector);
 
-	return &rcon->encoder->encoder;
+	return rcar_encoder_to_drm_encoder(rcon->encoder);
 }
 
 /* -----------------------------------------------------------------------------
@@ -142,10 +144,11 @@ static const struct drm_encoder_funcs encoder_funcs = {
 int rcar_du_encoder_init(struct rcar_du_device *rcdu,
 			 enum rcar_du_encoder_type type,
 			 enum rcar_du_output output,
-			 const struct rcar_du_encoder_data *data,
-			 struct device_node *np)
+			 struct device_node *enc_node,
+			 struct device_node *con_node)
 {
 	struct rcar_du_encoder *renc;
+	struct drm_encoder *encoder;
 	unsigned int encoder_type;
 	int ret;
 
@@ -154,6 +157,7 @@ int rcar_du_encoder_init(struct rcar_du_device *rcdu,
 		return -ENOMEM;
 
 	renc->output = output;
+	encoder = rcar_encoder_to_drm_encoder(renc);
 
 	switch (output) {
 	case RCAR_DU_OUTPUT_LVDS0:
@@ -175,6 +179,9 @@ int rcar_du_encoder_init(struct rcar_du_device *rcdu,
 	case RCAR_DU_ENCODER_LVDS:
 		encoder_type = DRM_MODE_ENCODER_LVDS;
 		break;
+	case RCAR_DU_ENCODER_HDMI:
+		encoder_type = DRM_MODE_ENCODER_TMDS;
+		break;
 	case RCAR_DU_ENCODER_NONE:
 	default:
 		/* No external encoder, use the internal encoder type. */
@@ -182,23 +189,35 @@ int rcar_du_encoder_init(struct rcar_du_device *rcdu,
 		break;
 	}
 
-	ret = drm_encoder_init(rcdu->ddev, &renc->encoder, &encoder_funcs,
-			       encoder_type);
-	if (ret < 0)
-		return ret;
+	if (type == RCAR_DU_ENCODER_HDMI) {
+		if (renc->lvds) {
+			dev_err(rcdu->dev,
+				"Chaining LVDS and HDMI encoders not supported\n");
+			return -EINVAL;
+		}
 
-	drm_encoder_helper_add(&renc->encoder, &encoder_helper_funcs);
+		ret = rcar_du_hdmienc_init(rcdu, renc, enc_node);
+		if (ret < 0)
+			return ret;
+	} else {
+		ret = drm_encoder_init(rcdu->ddev, encoder, &encoder_funcs,
+				       encoder_type);
+		if (ret < 0)
+			return ret;
 
-	switch (encoder_type) {
-	case DRM_MODE_ENCODER_LVDS: {
-		const struct rcar_du_panel_data *pdata =
-			data ? &data->connector.lvds.panel : NULL;
-		return rcar_du_lvds_connector_init(rcdu, renc, pdata, np);
+		drm_encoder_helper_add(encoder, &encoder_helper_funcs);
 	}
 
+	switch (encoder_type) {
+	case DRM_MODE_ENCODER_LVDS:
+		return rcar_du_lvds_connector_init(rcdu, renc, con_node);
+
 	case DRM_MODE_ENCODER_DAC:
 		return rcar_du_vga_connector_init(rcdu, renc);
 
+	case DRM_MODE_ENCODER_TMDS:
+		return rcar_du_hdmi_connector_init(rcdu, renc);
+
 	default:
 		return -EINVAL;
 	}
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_encoder.h b/drivers/gpu/drm/rcar-du/rcar_du_encoder.h
index bd624135ef1f..719b6f2a031c 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_encoder.h
+++ b/drivers/gpu/drm/rcar-du/rcar_du_encoder.h
@@ -14,21 +14,32 @@
 #ifndef __RCAR_DU_ENCODER_H__
 #define __RCAR_DU_ENCODER_H__
 
-#include <linux/platform_data/rcar-du.h>
-
 #include <drm/drm_crtc.h>
+#include <drm/drm_encoder_slave.h>
 
 struct rcar_du_device;
+struct rcar_du_hdmienc;
 struct rcar_du_lvdsenc;
 
+enum rcar_du_encoder_type {
+	RCAR_DU_ENCODER_UNUSED = 0,
+	RCAR_DU_ENCODER_NONE,
+	RCAR_DU_ENCODER_VGA,
+	RCAR_DU_ENCODER_LVDS,
+	RCAR_DU_ENCODER_HDMI,
+};
+
 struct rcar_du_encoder {
-	struct drm_encoder encoder;
+	struct drm_encoder_slave slave;
 	enum rcar_du_output output;
+	struct rcar_du_hdmienc *hdmi;
 	struct rcar_du_lvdsenc *lvds;
 };
 
 #define to_rcar_encoder(e) \
-	container_of(e, struct rcar_du_encoder, encoder)
+	container_of(e, struct rcar_du_encoder, slave.base)
+
+#define rcar_encoder_to_drm_encoder(e)	(&(e)->slave.base)
 
 struct rcar_du_connector {
 	struct drm_connector connector;
@@ -44,7 +55,7 @@ rcar_du_connector_best_encoder(struct drm_connector *connector);
 int rcar_du_encoder_init(struct rcar_du_device *rcdu,
 			 enum rcar_du_encoder_type type,
 			 enum rcar_du_output output,
-			 const struct rcar_du_encoder_data *data,
-			 struct device_node *np);
+			 struct device_node *enc_node,
+			 struct device_node *con_node);
 
 #endif /* __RCAR_DU_ENCODER_H__ */
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_hdmicon.c b/drivers/gpu/drm/rcar-du/rcar_du_hdmicon.c
new file mode 100644
index 000000000000..4d7d4dd46d26
--- /dev/null
+++ b/drivers/gpu/drm/rcar-du/rcar_du_hdmicon.c
@@ -0,0 +1,121 @@
+/*
+ * R-Car Display Unit HDMI Connector
+ *
+ * Copyright (C) 2014 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.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 <drm/drmP.h>
+#include <drm/drm_crtc.h>
+#include <drm/drm_crtc_helper.h>
+#include <drm/drm_encoder_slave.h>
+
+#include "rcar_du_drv.h"
+#include "rcar_du_encoder.h"
+#include "rcar_du_hdmicon.h"
+#include "rcar_du_kms.h"
+
+#define to_slave_funcs(e)	(to_rcar_encoder(e)->slave.slave_funcs)
+
+static int rcar_du_hdmi_connector_get_modes(struct drm_connector *connector)
+{
+	struct rcar_du_connector *con = to_rcar_connector(connector);
+	struct drm_encoder *encoder = rcar_encoder_to_drm_encoder(con->encoder);
+	struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
+
+	if (sfuncs->get_modes == NULL)
+		return 0;
+
+	return sfuncs->get_modes(encoder, connector);
+}
+
+static int rcar_du_hdmi_connector_mode_valid(struct drm_connector *connector,
+					     struct drm_display_mode *mode)
+{
+	struct rcar_du_connector *con = to_rcar_connector(connector);
+	struct drm_encoder *encoder = rcar_encoder_to_drm_encoder(con->encoder);
+	struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
+
+	if (sfuncs->mode_valid == NULL)
+		return MODE_OK;
+
+	return sfuncs->mode_valid(encoder, mode);
+}
+
+static const struct drm_connector_helper_funcs connector_helper_funcs = {
+	.get_modes = rcar_du_hdmi_connector_get_modes,
+	.mode_valid = rcar_du_hdmi_connector_mode_valid,
+	.best_encoder = rcar_du_connector_best_encoder,
+};
+
+static void rcar_du_hdmi_connector_destroy(struct drm_connector *connector)
+{
+	drm_connector_unregister(connector);
+	drm_connector_cleanup(connector);
+}
+
+static enum drm_connector_status
+rcar_du_hdmi_connector_detect(struct drm_connector *connector, bool force)
+{
+	struct rcar_du_connector *con = to_rcar_connector(connector);
+	struct drm_encoder *encoder = rcar_encoder_to_drm_encoder(con->encoder);
+	struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
+
+	if (sfuncs->detect == NULL)
+		return connector_status_unknown;
+
+	return sfuncs->detect(encoder, connector);
+}
+
+static const struct drm_connector_funcs connector_funcs = {
+	.dpms = drm_helper_connector_dpms,
+	.detect = rcar_du_hdmi_connector_detect,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.destroy = rcar_du_hdmi_connector_destroy,
+};
+
+int rcar_du_hdmi_connector_init(struct rcar_du_device *rcdu,
+				struct rcar_du_encoder *renc)
+{
+	struct drm_encoder *encoder = rcar_encoder_to_drm_encoder(renc);
+	struct rcar_du_connector *rcon;
+	struct drm_connector *connector;
+	int ret;
+
+	rcon = devm_kzalloc(rcdu->dev, sizeof(*rcon), GFP_KERNEL);
+	if (rcon == NULL)
+		return -ENOMEM;
+
+	connector = &rcon->connector;
+	connector->display_info.width_mm = 0;
+	connector->display_info.height_mm = 0;
+
+	ret = drm_connector_init(rcdu->ddev, connector, &connector_funcs,
+				 DRM_MODE_CONNECTOR_HDMIA);
+	if (ret < 0)
+		return ret;
+
+	drm_connector_helper_add(connector, &connector_helper_funcs);
+	ret = drm_connector_register(connector);
+	if (ret < 0)
+		return ret;
+
+	drm_helper_connector_dpms(connector, DRM_MODE_DPMS_OFF);
+	drm_object_property_set_value(&connector->base,
+		rcdu->ddev->mode_config.dpms_property, DRM_MODE_DPMS_OFF);
+
+	ret = drm_mode_connector_attach_encoder(connector, encoder);
+	if (ret < 0)
+		return ret;
+
+	connector->encoder = encoder;
+	rcon->encoder = renc;
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_hdmicon.h b/drivers/gpu/drm/rcar-du/rcar_du_hdmicon.h
new file mode 100644
index 000000000000..87daa949227f
--- /dev/null
+++ b/drivers/gpu/drm/rcar-du/rcar_du_hdmicon.h
@@ -0,0 +1,31 @@
+/*
+ * R-Car Display Unit HDMI Connector
+ *
+ * Copyright (C) 2014 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef __RCAR_DU_HDMICON_H__
+#define __RCAR_DU_HDMICON_H__
+
+struct rcar_du_device;
+struct rcar_du_encoder;
+
+#if IS_ENABLED(CONFIG_DRM_RCAR_HDMI)
+int rcar_du_hdmi_connector_init(struct rcar_du_device *rcdu,
+				struct rcar_du_encoder *renc);
+#else
+static inline int rcar_du_hdmi_connector_init(struct rcar_du_device *rcdu,
+					      struct rcar_du_encoder *renc)
+{
+	return -ENOSYS;
+}
+#endif
+
+#endif /* __RCAR_DU_HDMICON_H__ */
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_hdmienc.c b/drivers/gpu/drm/rcar-du/rcar_du_hdmienc.c
new file mode 100644
index 000000000000..359bc999a9c8
--- /dev/null
+++ b/drivers/gpu/drm/rcar-du/rcar_du_hdmienc.c
@@ -0,0 +1,151 @@
+/*
+ * R-Car Display Unit HDMI Encoder
+ *
+ * Copyright (C) 2014 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.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/slab.h>
+
+#include <drm/drmP.h>
+#include <drm/drm_crtc.h>
+#include <drm/drm_crtc_helper.h>
+#include <drm/drm_encoder_slave.h>
+
+#include "rcar_du_drv.h"
+#include "rcar_du_encoder.h"
+#include "rcar_du_hdmienc.h"
+
+struct rcar_du_hdmienc {
+	struct rcar_du_encoder *renc;
+	struct device *dev;
+	int dpms;
+};
+
+#define to_rcar_hdmienc(e)	(to_rcar_encoder(e)->hdmi)
+#define to_slave_funcs(e)	(to_rcar_encoder(e)->slave.slave_funcs)
+
+static void rcar_du_hdmienc_dpms(struct drm_encoder *encoder, int mode)
+{
+	struct rcar_du_hdmienc *hdmienc = to_rcar_hdmienc(encoder);
+	struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
+
+	if (hdmienc->dpms == mode)
+		return;
+
+	if (sfuncs->dpms)
+		sfuncs->dpms(encoder, mode);
+
+	hdmienc->dpms = mode;
+}
+
+static bool rcar_du_hdmienc_mode_fixup(struct drm_encoder *encoder,
+				       const struct drm_display_mode *mode,
+				       struct drm_display_mode *adjusted_mode)
+{
+	struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
+
+	if (sfuncs->mode_fixup == NULL)
+		return true;
+
+	return sfuncs->mode_fixup(encoder, mode, adjusted_mode);
+}
+
+static void rcar_du_hdmienc_mode_prepare(struct drm_encoder *encoder)
+{
+	rcar_du_hdmienc_dpms(encoder, DRM_MODE_DPMS_OFF);
+}
+
+static void rcar_du_hdmienc_mode_commit(struct drm_encoder *encoder)
+{
+	rcar_du_hdmienc_dpms(encoder, DRM_MODE_DPMS_ON);
+}
+
+static void rcar_du_hdmienc_mode_set(struct drm_encoder *encoder,
+				     struct drm_display_mode *mode,
+				     struct drm_display_mode *adjusted_mode)
+{
+	struct rcar_du_hdmienc *hdmienc = to_rcar_hdmienc(encoder);
+	struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
+
+	if (sfuncs->mode_set)
+		sfuncs->mode_set(encoder, mode, adjusted_mode);
+
+	rcar_du_crtc_route_output(encoder->crtc, hdmienc->renc->output);
+}
+
+static const struct drm_encoder_helper_funcs encoder_helper_funcs = {
+	.dpms = rcar_du_hdmienc_dpms,
+	.mode_fixup = rcar_du_hdmienc_mode_fixup,
+	.prepare = rcar_du_hdmienc_mode_prepare,
+	.commit = rcar_du_hdmienc_mode_commit,
+	.mode_set = rcar_du_hdmienc_mode_set,
+};
+
+static void rcar_du_hdmienc_cleanup(struct drm_encoder *encoder)
+{
+	struct rcar_du_hdmienc *hdmienc = to_rcar_hdmienc(encoder);
+
+	rcar_du_hdmienc_dpms(encoder, DRM_MODE_DPMS_OFF);
+
+	drm_encoder_cleanup(encoder);
+	put_device(hdmienc->dev);
+}
+
+static const struct drm_encoder_funcs encoder_funcs = {
+	.destroy = rcar_du_hdmienc_cleanup,
+};
+
+int rcar_du_hdmienc_init(struct rcar_du_device *rcdu,
+			 struct rcar_du_encoder *renc, struct device_node *np)
+{
+	struct drm_encoder *encoder = rcar_encoder_to_drm_encoder(renc);
+	struct drm_i2c_encoder_driver *driver;
+	struct i2c_client *i2c_slave;
+	struct rcar_du_hdmienc *hdmienc;
+	int ret;
+
+	hdmienc = devm_kzalloc(rcdu->dev, sizeof(*hdmienc), GFP_KERNEL);
+	if (hdmienc == NULL)
+		return -ENOMEM;
+
+	/* Locate the slave I2C device and driver. */
+	i2c_slave = of_find_i2c_device_by_node(np);
+	if (!i2c_slave || !i2c_get_clientdata(i2c_slave))
+		return -EPROBE_DEFER;
+
+	hdmienc->dev = &i2c_slave->dev;
+
+	if (hdmienc->dev->driver == NULL) {
+		ret = -EPROBE_DEFER;
+		goto error;
+	}
+
+	/* Initialize the slave encoder. */
+	driver = to_drm_i2c_encoder_driver(to_i2c_driver(hdmienc->dev->driver));
+	ret = driver->encoder_init(i2c_slave, rcdu->ddev, &renc->slave);
+	if (ret < 0)
+		goto error;
+
+	ret = drm_encoder_init(rcdu->ddev, encoder, &encoder_funcs,
+			       DRM_MODE_ENCODER_TMDS);
+	if (ret < 0)
+		goto error;
+
+	drm_encoder_helper_add(encoder, &encoder_helper_funcs);
+
+	renc->hdmi = hdmienc;
+	hdmienc->renc = renc;
+
+	return 0;
+
+error:
+	put_device(hdmienc->dev);
+	return ret;
+}
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_hdmienc.h b/drivers/gpu/drm/rcar-du/rcar_du_hdmienc.h
new file mode 100644
index 000000000000..2ff0128ac8e1
--- /dev/null
+++ b/drivers/gpu/drm/rcar-du/rcar_du_hdmienc.h
@@ -0,0 +1,35 @@
+/*
+ * R-Car Display Unit HDMI Encoder
+ *
+ * Copyright (C) 2014 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef __RCAR_DU_HDMIENC_H__
+#define __RCAR_DU_HDMIENC_H__
+
+#include <linux/module.h>
+
+struct device_node;
+struct rcar_du_device;
+struct rcar_du_encoder;
+
+#if IS_ENABLED(CONFIG_DRM_RCAR_HDMI)
+int rcar_du_hdmienc_init(struct rcar_du_device *rcdu,
+			 struct rcar_du_encoder *renc, struct device_node *np);
+#else
+static inline int rcar_du_hdmienc_init(struct rcar_du_device *rcdu,
+				       struct rcar_du_encoder *renc,
+				       struct device_node *np)
+{
+	return -ENOSYS;
+}
+#endif
+
+#endif /* __RCAR_DU_HDMIENC_H__ */
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_kms.c b/drivers/gpu/drm/rcar-du/rcar_du_kms.c
index 6c24ad7d03ef..0c5ee616b5a3 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_kms.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_kms.c
@@ -126,9 +126,9 @@ int rcar_du_dumb_create(struct drm_file *file, struct drm_device *dev,
 	else
 		align = 16 * args->bpp / 8;
 
-	args->pitch = roundup(max(args->pitch, min_pitch), align);
+	args->pitch = roundup(min_pitch, align);
 
-	return drm_gem_cma_dumb_create(file, dev, args);
+	return drm_gem_cma_dumb_create_internal(file, dev, args);
 }
 
 static struct drm_framebuffer *
@@ -190,49 +190,16 @@ static const struct drm_mode_config_funcs rcar_du_mode_config_funcs = {
 	.output_poll_changed = rcar_du_output_poll_changed,
 };
 
-static int rcar_du_encoders_init_pdata(struct rcar_du_device *rcdu)
-{
-	unsigned int num_encoders = 0;
-	unsigned int i;
-	int ret;
-
-	for (i = 0; i < rcdu->pdata->num_encoders; ++i) {
-		const struct rcar_du_encoder_data *pdata =
-			&rcdu->pdata->encoders[i];
-		const struct rcar_du_output_routing *route =
-			&rcdu->info->routes[pdata->output];
-
-		if (pdata->type == RCAR_DU_ENCODER_UNUSED)
-			continue;
-
-		if (pdata->output >= RCAR_DU_OUTPUT_MAX ||
-		    route->possible_crtcs == 0) {
-			dev_warn(rcdu->dev,
-				 "encoder %u references unexisting output %u, skipping\n",
-				 i, pdata->output);
-			continue;
-		}
-
-		ret = rcar_du_encoder_init(rcdu, pdata->type, pdata->output,
-					   pdata, NULL);
-		if (ret < 0)
-			return ret;
-
-		num_encoders++;
-	}
-
-	return num_encoders;
-}
-
-static int rcar_du_encoders_init_dt_one(struct rcar_du_device *rcdu,
-					enum rcar_du_output output,
-					struct of_endpoint *ep)
+static int rcar_du_encoders_init_one(struct rcar_du_device *rcdu,
+				     enum rcar_du_output output,
+				     struct of_endpoint *ep)
 {
 	static const struct {
 		const char *compatible;
 		enum rcar_du_encoder_type type;
 	} encoders[] = {
 		{ "adi,adv7123", RCAR_DU_ENCODER_VGA },
+		{ "adi,adv7511w", RCAR_DU_ENCODER_HDMI },
 		{ "thine,thc63lvdm83d", RCAR_DU_ENCODER_LVDS },
 	};
 
@@ -323,14 +290,14 @@ static int rcar_du_encoders_init_dt_one(struct rcar_du_device *rcdu,
 		connector = entity;
 	}
 
-	ret = rcar_du_encoder_init(rcdu, enc_type, output, NULL, connector);
+	ret = rcar_du_encoder_init(rcdu, enc_type, output, encoder, connector);
 	of_node_put(encoder);
 	of_node_put(connector);
 
 	return ret < 0 ? ret : 1;
 }
 
-static int rcar_du_encoders_init_dt(struct rcar_du_device *rcdu)
+static int rcar_du_encoders_init(struct rcar_du_device *rcdu)
 {
 	struct device_node *np = rcdu->dev->of_node;
 	struct device_node *prev = NULL;
@@ -377,7 +344,7 @@ static int rcar_du_encoders_init_dt(struct rcar_du_device *rcdu)
 		}
 
 		/* Process the output pipeline. */
-		ret = rcar_du_encoders_init_dt_one(rcdu, output, &ep);
+		ret = rcar_du_encoders_init_one(rcdu, output, &ep);
 		if (ret < 0) {
 			of_node_put(ep_node);
 			return ret;
@@ -442,11 +409,7 @@ int rcar_du_modeset_init(struct rcar_du_device *rcdu)
 	if (ret < 0)
 		return ret;
 
-	if (rcdu->pdata)
-		ret = rcar_du_encoders_init_pdata(rcdu);
-	else
-		ret = rcar_du_encoders_init_dt(rcdu);
-
+	ret = rcar_du_encoders_init(rcdu);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_lvdscon.c b/drivers/gpu/drm/rcar-du/rcar_du_lvdscon.c
index 115eed20db12..6d9811c052c4 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_lvdscon.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_lvdscon.c
@@ -27,7 +27,11 @@
 struct rcar_du_lvds_connector {
 	struct rcar_du_connector connector;
 
-	struct rcar_du_panel_data panel;
+	struct {
+		unsigned int width_mm;		/* Panel width in mm */
+		unsigned int height_mm;		/* Panel height in mm */
+		struct videomode mode;
+	} panel;
 };
 
 #define to_rcar_lvds_connector(c) \
@@ -78,31 +82,26 @@ static const struct drm_connector_funcs connector_funcs = {
 
 int rcar_du_lvds_connector_init(struct rcar_du_device *rcdu,
 				struct rcar_du_encoder *renc,
-				const struct rcar_du_panel_data *panel,
 				/* TODO const */ struct device_node *np)
 {
+	struct drm_encoder *encoder = rcar_encoder_to_drm_encoder(renc);
 	struct rcar_du_lvds_connector *lvdscon;
 	struct drm_connector *connector;
+	struct display_timing timing;
 	int ret;
 
 	lvdscon = devm_kzalloc(rcdu->dev, sizeof(*lvdscon), GFP_KERNEL);
 	if (lvdscon == NULL)
 		return -ENOMEM;
 
-	if (panel) {
-		lvdscon->panel = *panel;
-	} else {
-		struct display_timing timing;
-
-		ret = of_get_display_timing(np, "panel-timing", &timing);
-		if (ret < 0)
-			return ret;
+	ret = of_get_display_timing(np, "panel-timing", &timing);
+	if (ret < 0)
+		return ret;
 
-		videomode_from_timing(&timing, &lvdscon->panel.mode);
+	videomode_from_timing(&timing, &lvdscon->panel.mode);
 
-		of_property_read_u32(np, "width-mm", &lvdscon->panel.width_mm);
-		of_property_read_u32(np, "height-mm", &lvdscon->panel.height_mm);
-	}
+	of_property_read_u32(np, "width-mm", &lvdscon->panel.width_mm);
+	of_property_read_u32(np, "height-mm", &lvdscon->panel.height_mm);
 
 	connector = &lvdscon->connector.connector;
 	connector->display_info.width_mm = lvdscon->panel.width_mm;
@@ -122,11 +121,11 @@ int rcar_du_lvds_connector_init(struct rcar_du_device *rcdu,
 	drm_object_property_set_value(&connector->base,
 		rcdu->ddev->mode_config.dpms_property, DRM_MODE_DPMS_OFF);
 
-	ret = drm_mode_connector_attach_encoder(connector, &renc->encoder);
+	ret = drm_mode_connector_attach_encoder(connector, encoder);
 	if (ret < 0)
 		return ret;
 
-	connector->encoder = &renc->encoder;
+	connector->encoder = encoder;
 	lvdscon->connector.encoder = renc;
 
 	return 0;
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_lvdscon.h b/drivers/gpu/drm/rcar-du/rcar_du_lvdscon.h
index d11424d537f9..d4881ee0be7e 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_lvdscon.h
+++ b/drivers/gpu/drm/rcar-du/rcar_du_lvdscon.h
@@ -16,11 +16,9 @@
 
 struct rcar_du_device;
 struct rcar_du_encoder;
-struct rcar_du_panel_data;
 
 int rcar_du_lvds_connector_init(struct rcar_du_device *rcdu,
 				struct rcar_du_encoder *renc,
-				const struct rcar_du_panel_data *panel,
 				struct device_node *np);
 
 #endif /* __RCAR_DU_LVDSCON_H__ */
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_lvdsenc.h b/drivers/gpu/drm/rcar-du/rcar_du_lvdsenc.h
index 3303a55cec79..f65aabda0796 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_lvdsenc.h
+++ b/drivers/gpu/drm/rcar-du/rcar_du_lvdsenc.h
@@ -16,7 +16,6 @@
 
 #include <linux/io.h>
 #include <linux/module.h>
-#include <linux/platform_data/rcar-du.h>
 
 struct rcar_drm_crtc;
 struct rcar_du_lvdsenc;
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_vgacon.c b/drivers/gpu/drm/rcar-du/rcar_du_vgacon.c
index 564a723ede03..752747a5e920 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_vgacon.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_vgacon.c
@@ -52,6 +52,7 @@ static const struct drm_connector_funcs connector_funcs = {
 int rcar_du_vga_connector_init(struct rcar_du_device *rcdu,
 			       struct rcar_du_encoder *renc)
 {
+	struct drm_encoder *encoder = rcar_encoder_to_drm_encoder(renc);
 	struct rcar_du_connector *rcon;
 	struct drm_connector *connector;
 	int ret;
@@ -78,11 +79,11 @@ int rcar_du_vga_connector_init(struct rcar_du_device *rcdu,
 	drm_object_property_set_value(&connector->base,
 		rcdu->ddev->mode_config.dpms_property, DRM_MODE_DPMS_OFF);
 
-	ret = drm_mode_connector_attach_encoder(connector, &renc->encoder);
+	ret = drm_mode_connector_attach_encoder(connector, encoder);
 	if (ret < 0)
 		return ret;
 
-	connector->encoder = &renc->encoder;
+	connector->encoder = encoder;
 	rcon->encoder = renc;
 
 	return 0;
diff --git a/drivers/gpu/drm/rockchip/Kconfig b/drivers/gpu/drm/rockchip/Kconfig
new file mode 100644
index 000000000000..ca9f085efa92
--- /dev/null
+++ b/drivers/gpu/drm/rockchip/Kconfig
@@ -0,0 +1,17 @@
+config DRM_ROCKCHIP
+	tristate "DRM Support for Rockchip"
+	depends on DRM && ROCKCHIP_IOMMU
+	select DRM_KMS_HELPER
+	select DRM_KMS_FB_HELPER
+	select DRM_PANEL
+	select FB_CFB_FILLRECT
+	select FB_CFB_COPYAREA
+	select FB_CFB_IMAGEBLIT
+	select VT_HW_CONSOLE_BINDING if FRAMEBUFFER_CONSOLE
+	select VIDEOMODE_HELPERS
+	help
+	  Choose this option if you have a Rockchip soc chipset.
+	  This driver provides kernel mode setting and buffer
+	  management to userspace. This driver does not provide
+	  2D or 3D acceleration; acceleration is performed by other
+	  IP found on the SoC.
diff --git a/drivers/gpu/drm/rockchip/Makefile b/drivers/gpu/drm/rockchip/Makefile
new file mode 100644
index 000000000000..2cb0672f57ed
--- /dev/null
+++ b/drivers/gpu/drm/rockchip/Makefile
@@ -0,0 +1,8 @@
+#
+# Makefile for the drm device driver.  This driver provides support for the
+# Direct Rendering Infrastructure (DRI) in XFree86 4.1.0 and higher.
+
+rockchipdrm-y := rockchip_drm_drv.o rockchip_drm_fb.o rockchip_drm_fbdev.o \
+		rockchip_drm_gem.o
+
+obj-$(CONFIG_DRM_ROCKCHIP) += rockchipdrm.o rockchip_drm_vop.o
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_drv.c b/drivers/gpu/drm/rockchip/rockchip_drm_drv.c
new file mode 100644
index 000000000000..a798c7c71f91
--- /dev/null
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_drv.c
@@ -0,0 +1,551 @@
+/*
+ * Copyright (C) Fuzhou Rockchip Electronics Co.Ltd
+ * Author:Mark Yao <mark.yao@rock-chips.com>
+ *
+ * based on exynos_drm_drv.c
+ *
+ * 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 <asm/dma-iommu.h>
+
+#include <drm/drmP.h>
+#include <drm/drm_crtc_helper.h>
+#include <drm/drm_fb_helper.h>
+#include <linux/dma-mapping.h>
+#include <linux/pm_runtime.h>
+#include <linux/of_graph.h>
+#include <linux/component.h>
+
+#include "rockchip_drm_drv.h"
+#include "rockchip_drm_fb.h"
+#include "rockchip_drm_fbdev.h"
+#include "rockchip_drm_gem.h"
+
+#define DRIVER_NAME	"rockchip"
+#define DRIVER_DESC	"RockChip Soc DRM"
+#define DRIVER_DATE	"20140818"
+#define DRIVER_MAJOR	1
+#define DRIVER_MINOR	0
+
+/*
+ * Attach a (component) device to the shared drm dma mapping from master drm
+ * device.  This is used by the VOPs to map GEM buffers to a common DMA
+ * mapping.
+ */
+int rockchip_drm_dma_attach_device(struct drm_device *drm_dev,
+				   struct device *dev)
+{
+	struct dma_iommu_mapping *mapping = drm_dev->dev->archdata.mapping;
+	int ret;
+
+	ret = dma_set_coherent_mask(dev, DMA_BIT_MASK(32));
+	if (ret)
+		return ret;
+
+	dma_set_max_seg_size(dev, DMA_BIT_MASK(32));
+
+	return arm_iommu_attach_device(dev, mapping);
+}
+EXPORT_SYMBOL_GPL(rockchip_drm_dma_attach_device);
+
+void rockchip_drm_dma_detach_device(struct drm_device *drm_dev,
+				    struct device *dev)
+{
+	arm_iommu_detach_device(dev);
+}
+EXPORT_SYMBOL_GPL(rockchip_drm_dma_detach_device);
+
+int rockchip_register_crtc_funcs(struct drm_device *dev,
+				 const struct rockchip_crtc_funcs *crtc_funcs,
+				 int pipe)
+{
+	struct rockchip_drm_private *priv = dev->dev_private;
+
+	if (pipe > ROCKCHIP_MAX_CRTC)
+		return -EINVAL;
+
+	priv->crtc_funcs[pipe] = crtc_funcs;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rockchip_register_crtc_funcs);
+
+void rockchip_unregister_crtc_funcs(struct drm_device *dev, int pipe)
+{
+	struct rockchip_drm_private *priv = dev->dev_private;
+
+	if (pipe > ROCKCHIP_MAX_CRTC)
+		return;
+
+	priv->crtc_funcs[pipe] = NULL;
+}
+EXPORT_SYMBOL_GPL(rockchip_unregister_crtc_funcs);
+
+static struct drm_crtc *rockchip_crtc_from_pipe(struct drm_device *drm,
+						int pipe)
+{
+	struct drm_crtc *crtc;
+	int i = 0;
+
+	list_for_each_entry(crtc, &drm->mode_config.crtc_list, head)
+		if (i++ == pipe)
+			return crtc;
+
+	return NULL;
+}
+
+static int rockchip_drm_crtc_enable_vblank(struct drm_device *dev, int pipe)
+{
+	struct rockchip_drm_private *priv = dev->dev_private;
+	struct drm_crtc *crtc = rockchip_crtc_from_pipe(dev, pipe);
+
+	if (crtc && priv->crtc_funcs[pipe] &&
+	    priv->crtc_funcs[pipe]->enable_vblank)
+		return priv->crtc_funcs[pipe]->enable_vblank(crtc);
+
+	return 0;
+}
+
+static void rockchip_drm_crtc_disable_vblank(struct drm_device *dev, int pipe)
+{
+	struct rockchip_drm_private *priv = dev->dev_private;
+	struct drm_crtc *crtc = rockchip_crtc_from_pipe(dev, pipe);
+
+	if (crtc && priv->crtc_funcs[pipe] &&
+	    priv->crtc_funcs[pipe]->enable_vblank)
+		priv->crtc_funcs[pipe]->disable_vblank(crtc);
+}
+
+static int rockchip_drm_load(struct drm_device *drm_dev, unsigned long flags)
+{
+	struct rockchip_drm_private *private;
+	struct dma_iommu_mapping *mapping;
+	struct device *dev = drm_dev->dev;
+	int ret;
+
+	private = devm_kzalloc(drm_dev->dev, sizeof(*private), GFP_KERNEL);
+	if (!private)
+		return -ENOMEM;
+
+	drm_dev->dev_private = private;
+
+	drm_mode_config_init(drm_dev);
+
+	rockchip_drm_mode_config_init(drm_dev);
+
+	dev->dma_parms = devm_kzalloc(dev, sizeof(*dev->dma_parms),
+				      GFP_KERNEL);
+	if (!dev->dma_parms) {
+		ret = -ENOMEM;
+		goto err_config_cleanup;
+	}
+
+	/* TODO(djkurtz): fetch the mapping start/size from somewhere */
+	mapping = arm_iommu_create_mapping(&platform_bus_type, 0x00000000,
+					   SZ_2G);
+	if (IS_ERR(mapping)) {
+		ret = PTR_ERR(mapping);
+		goto err_config_cleanup;
+	}
+
+	ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(32));
+	if (ret)
+		goto err_release_mapping;
+
+	dma_set_max_seg_size(dev, DMA_BIT_MASK(32));
+
+	ret = arm_iommu_attach_device(dev, mapping);
+	if (ret)
+		goto err_release_mapping;
+
+	/* Try to bind all sub drivers. */
+	ret = component_bind_all(dev, drm_dev);
+	if (ret)
+		goto err_detach_device;
+
+	/* init kms poll for handling hpd */
+	drm_kms_helper_poll_init(drm_dev);
+
+	/*
+	 * enable drm irq mode.
+	 * - with irq_enabled = true, we can use the vblank feature.
+	 */
+	drm_dev->irq_enabled = true;
+
+	ret = drm_vblank_init(drm_dev, ROCKCHIP_MAX_CRTC);
+	if (ret)
+		goto err_kms_helper_poll_fini;
+
+	/*
+	 * with vblank_disable_allowed = true, vblank interrupt will be disabled
+	 * by drm timer once a current process gives up ownership of
+	 * vblank event.(after drm_vblank_put function is called)
+	 */
+	drm_dev->vblank_disable_allowed = true;
+
+	ret = rockchip_drm_fbdev_init(drm_dev);
+	if (ret)
+		goto err_vblank_cleanup;
+
+	return 0;
+err_vblank_cleanup:
+	drm_vblank_cleanup(drm_dev);
+err_kms_helper_poll_fini:
+	drm_kms_helper_poll_fini(drm_dev);
+	component_unbind_all(dev, drm_dev);
+err_detach_device:
+	arm_iommu_detach_device(dev);
+err_release_mapping:
+	arm_iommu_release_mapping(dev->archdata.mapping);
+err_config_cleanup:
+	drm_mode_config_cleanup(drm_dev);
+	drm_dev->dev_private = NULL;
+	return ret;
+}
+
+static int rockchip_drm_unload(struct drm_device *drm_dev)
+{
+	struct device *dev = drm_dev->dev;
+
+	rockchip_drm_fbdev_fini(drm_dev);
+	drm_vblank_cleanup(drm_dev);
+	drm_kms_helper_poll_fini(drm_dev);
+	component_unbind_all(dev, drm_dev);
+	arm_iommu_detach_device(dev);
+	arm_iommu_release_mapping(dev->archdata.mapping);
+	drm_mode_config_cleanup(drm_dev);
+	drm_dev->dev_private = NULL;
+
+	return 0;
+}
+
+void rockchip_drm_lastclose(struct drm_device *dev)
+{
+	struct rockchip_drm_private *priv = dev->dev_private;
+
+	drm_fb_helper_restore_fbdev_mode_unlocked(&priv->fbdev_helper);
+}
+
+static const struct file_operations rockchip_drm_driver_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.mmap = rockchip_gem_mmap,
+	.poll = drm_poll,
+	.read = drm_read,
+	.unlocked_ioctl = drm_ioctl,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl = drm_compat_ioctl,
+#endif
+	.release = drm_release,
+};
+
+const struct vm_operations_struct rockchip_drm_vm_ops = {
+	.open = drm_gem_vm_open,
+	.close = drm_gem_vm_close,
+};
+
+static struct drm_driver rockchip_drm_driver = {
+	.driver_features	= DRIVER_MODESET | DRIVER_GEM | DRIVER_PRIME,
+	.load			= rockchip_drm_load,
+	.unload			= rockchip_drm_unload,
+	.lastclose		= rockchip_drm_lastclose,
+	.get_vblank_counter	= drm_vblank_count,
+	.enable_vblank		= rockchip_drm_crtc_enable_vblank,
+	.disable_vblank		= rockchip_drm_crtc_disable_vblank,
+	.gem_vm_ops		= &rockchip_drm_vm_ops,
+	.gem_free_object	= rockchip_gem_free_object,
+	.dumb_create		= rockchip_gem_dumb_create,
+	.dumb_map_offset	= rockchip_gem_dumb_map_offset,
+	.dumb_destroy		= 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_import	= drm_gem_prime_import,
+	.gem_prime_export	= drm_gem_prime_export,
+	.gem_prime_get_sg_table	= rockchip_gem_prime_get_sg_table,
+	.gem_prime_vmap		= rockchip_gem_prime_vmap,
+	.gem_prime_vunmap	= rockchip_gem_prime_vunmap,
+	.gem_prime_mmap		= rockchip_gem_mmap_buf,
+	.fops			= &rockchip_drm_driver_fops,
+	.name	= DRIVER_NAME,
+	.desc	= DRIVER_DESC,
+	.date	= DRIVER_DATE,
+	.major	= DRIVER_MAJOR,
+	.minor	= DRIVER_MINOR,
+};
+
+#ifdef CONFIG_PM_SLEEP
+static int rockchip_drm_sys_suspend(struct device *dev)
+{
+	struct drm_device *drm = dev_get_drvdata(dev);
+	struct drm_connector *connector;
+
+	if (!drm)
+		return 0;
+
+	drm_modeset_lock_all(drm);
+	list_for_each_entry(connector, &drm->mode_config.connector_list, head) {
+		int old_dpms = connector->dpms;
+
+		if (connector->funcs->dpms)
+			connector->funcs->dpms(connector, DRM_MODE_DPMS_OFF);
+
+		/* Set the old mode back to the connector for resume */
+		connector->dpms = old_dpms;
+	}
+	drm_modeset_unlock_all(drm);
+
+	return 0;
+}
+
+static int rockchip_drm_sys_resume(struct device *dev)
+{
+	struct drm_device *drm = dev_get_drvdata(dev);
+	struct drm_connector *connector;
+	enum drm_connector_status status;
+	bool changed = false;
+
+	if (!drm)
+		return 0;
+
+	drm_modeset_lock_all(drm);
+	list_for_each_entry(connector, &drm->mode_config.connector_list, head) {
+		int desired_mode = connector->dpms;
+
+		/*
+		 * at suspend time, we save dpms to connector->dpms,
+		 * restore the old_dpms, and at current time, the connector
+		 * dpms status must be DRM_MODE_DPMS_OFF.
+		 */
+		connector->dpms = DRM_MODE_DPMS_OFF;
+
+		/*
+		 * If the connector has been disconnected during suspend,
+		 * disconnect it from the encoder and leave it off. We'll notify
+		 * userspace at the end.
+		 */
+		if (desired_mode == DRM_MODE_DPMS_ON) {
+			status = connector->funcs->detect(connector, true);
+			if (status == connector_status_disconnected) {
+				connector->encoder = NULL;
+				connector->status = status;
+				changed = true;
+				continue;
+			}
+		}
+		if (connector->funcs->dpms)
+			connector->funcs->dpms(connector, desired_mode);
+	}
+	drm_modeset_unlock_all(drm);
+
+	drm_helper_resume_force_mode(drm);
+
+	if (changed)
+		drm_kms_helper_hotplug_event(drm);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops rockchip_drm_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(rockchip_drm_sys_suspend,
+				rockchip_drm_sys_resume)
+};
+
+/*
+ * @node: device tree node containing encoder input ports
+ * @encoder: drm_encoder
+ */
+int rockchip_drm_encoder_get_mux_id(struct device_node *node,
+				    struct drm_encoder *encoder)
+{
+	struct device_node *ep = NULL;
+	struct drm_crtc *crtc = encoder->crtc;
+	struct of_endpoint endpoint;
+	struct device_node *port;
+	int ret;
+
+	if (!node || !crtc)
+		return -EINVAL;
+
+	do {
+		ep = of_graph_get_next_endpoint(node, ep);
+		if (!ep)
+			break;
+
+		port = of_graph_get_remote_port(ep);
+		of_node_put(port);
+		if (port == crtc->port) {
+			ret = of_graph_parse_endpoint(ep, &endpoint);
+			return ret ?: endpoint.id;
+		}
+	} while (ep);
+
+	return -EINVAL;
+}
+
+static int compare_of(struct device *dev, void *data)
+{
+	struct device_node *np = data;
+
+	return dev->of_node == np;
+}
+
+static void rockchip_add_endpoints(struct device *dev,
+				   struct component_match **match,
+				   struct device_node *port)
+{
+	struct device_node *ep, *remote;
+
+	for_each_child_of_node(port, ep) {
+		remote = of_graph_get_remote_port_parent(ep);
+		if (!remote || !of_device_is_available(remote)) {
+			of_node_put(remote);
+			continue;
+		} else if (!of_device_is_available(remote->parent)) {
+			dev_warn(dev, "parent device of %s is not available\n",
+				 remote->full_name);
+			of_node_put(remote);
+			continue;
+		}
+
+		component_match_add(dev, match, compare_of, remote);
+		of_node_put(remote);
+	}
+}
+
+static int rockchip_drm_bind(struct device *dev)
+{
+	struct drm_device *drm;
+	int ret;
+
+	drm = drm_dev_alloc(&rockchip_drm_driver, dev);
+	if (!drm)
+		return -ENOMEM;
+
+	ret = drm_dev_set_unique(drm, "%s", dev_name(dev));
+	if (ret)
+		goto err_free;
+
+	ret = drm_dev_register(drm, 0);
+	if (ret)
+		goto err_free;
+
+	dev_set_drvdata(dev, drm);
+
+	return 0;
+
+err_free:
+	drm_dev_unref(drm);
+	return ret;
+}
+
+static void rockchip_drm_unbind(struct device *dev)
+{
+	struct drm_device *drm = dev_get_drvdata(dev);
+
+	drm_dev_unregister(drm);
+	drm_dev_unref(drm);
+	dev_set_drvdata(dev, NULL);
+}
+
+static const struct component_master_ops rockchip_drm_ops = {
+	.bind = rockchip_drm_bind,
+	.unbind = rockchip_drm_unbind,
+};
+
+static int rockchip_drm_platform_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct component_match *match = NULL;
+	struct device_node *np = dev->of_node;
+	struct device_node *port;
+	int i;
+
+	if (!np)
+		return -ENODEV;
+	/*
+	 * Bind the crtc ports first, so that
+	 * drm_of_find_possible_crtcs called from encoder .bind callbacks
+	 * works as expected.
+	 */
+	for (i = 0;; i++) {
+		port = of_parse_phandle(np, "ports", i);
+		if (!port)
+			break;
+
+		if (!of_device_is_available(port->parent)) {
+			of_node_put(port);
+			continue;
+		}
+
+		component_match_add(dev, &match, compare_of, port->parent);
+		of_node_put(port);
+	}
+
+	if (i == 0) {
+		dev_err(dev, "missing 'ports' property\n");
+		return -ENODEV;
+	}
+
+	if (!match) {
+		dev_err(dev, "No available vop found for display-subsystem.\n");
+		return -ENODEV;
+	}
+	/*
+	 * For each bound crtc, bind the encoders attached to its
+	 * remote endpoint.
+	 */
+	for (i = 0;; i++) {
+		port = of_parse_phandle(np, "ports", i);
+		if (!port)
+			break;
+
+		if (!of_device_is_available(port->parent)) {
+			of_node_put(port);
+			continue;
+		}
+
+		rockchip_add_endpoints(dev, &match, port);
+		of_node_put(port);
+	}
+
+	return component_master_add_with_match(dev, &rockchip_drm_ops, match);
+}
+
+static int rockchip_drm_platform_remove(struct platform_device *pdev)
+{
+	component_master_del(&pdev->dev, &rockchip_drm_ops);
+
+	return 0;
+}
+
+static const struct of_device_id rockchip_drm_dt_ids[] = {
+	{ .compatible = "rockchip,display-subsystem", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, rockchip_drm_dt_ids);
+
+static struct platform_driver rockchip_drm_platform_driver = {
+	.probe = rockchip_drm_platform_probe,
+	.remove = rockchip_drm_platform_remove,
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "rockchip-drm",
+		.of_match_table = rockchip_drm_dt_ids,
+		.pm = &rockchip_drm_pm_ops,
+	},
+};
+
+module_platform_driver(rockchip_drm_platform_driver);
+
+MODULE_AUTHOR("Mark Yao <mark.yao@rock-chips.com>");
+MODULE_DESCRIPTION("ROCKCHIP DRM Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_drv.h b/drivers/gpu/drm/rockchip/rockchip_drm_drv.h
new file mode 100644
index 000000000000..dc4e5f03ac79
--- /dev/null
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_drv.h
@@ -0,0 +1,68 @@
+/*
+ * Copyright (C) Fuzhou Rockchip Electronics Co.Ltd
+ * Author:Mark Yao <mark.yao@rock-chips.com>
+ *
+ * based on exynos_drm_drv.h
+ *
+ * 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 _ROCKCHIP_DRM_DRV_H
+#define _ROCKCHIP_DRM_DRV_H
+
+#include <drm/drm_fb_helper.h>
+#include <drm/drm_gem.h>
+
+#include <linux/module.h>
+#include <linux/component.h>
+
+#define ROCKCHIP_MAX_FB_BUFFER	3
+#define ROCKCHIP_MAX_CONNECTOR	2
+#define ROCKCHIP_MAX_CRTC	2
+
+struct drm_device;
+struct drm_connector;
+
+/*
+ * Rockchip drm private crtc funcs.
+ * @enable_vblank: enable crtc vblank irq.
+ * @disable_vblank: disable crtc vblank irq.
+ */
+struct rockchip_crtc_funcs {
+	int (*enable_vblank)(struct drm_crtc *crtc);
+	void (*disable_vblank)(struct drm_crtc *crtc);
+};
+
+/*
+ * Rockchip drm private structure.
+ *
+ * @crtc: array of enabled CRTCs, used to map from "pipe" to drm_crtc.
+ * @num_pipe: number of pipes for this device.
+ */
+struct rockchip_drm_private {
+	struct drm_fb_helper fbdev_helper;
+	struct drm_gem_object *fbdev_bo;
+	const struct rockchip_crtc_funcs *crtc_funcs[ROCKCHIP_MAX_CRTC];
+};
+
+int rockchip_register_crtc_funcs(struct drm_device *dev,
+				 const struct rockchip_crtc_funcs *crtc_funcs,
+				 int pipe);
+void rockchip_unregister_crtc_funcs(struct drm_device *dev, int pipe);
+int rockchip_drm_encoder_get_mux_id(struct device_node *node,
+				    struct drm_encoder *encoder);
+int rockchip_drm_crtc_mode_config(struct drm_crtc *crtc, int connector_type,
+				  int out_mode);
+int rockchip_drm_dma_attach_device(struct drm_device *drm_dev,
+				   struct device *dev);
+void rockchip_drm_dma_detach_device(struct drm_device *drm_dev,
+				    struct device *dev);
+
+#endif /* _ROCKCHIP_DRM_DRV_H_ */
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_fb.c b/drivers/gpu/drm/rockchip/rockchip_drm_fb.c
new file mode 100644
index 000000000000..77d52893d40f
--- /dev/null
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_fb.c
@@ -0,0 +1,201 @@
+/*
+ * Copyright (C) Fuzhou Rockchip Electronics Co.Ltd
+ * Author:Mark Yao <mark.yao@rock-chips.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 <drm/drm.h>
+#include <drm/drmP.h>
+#include <drm/drm_fb_helper.h>
+#include <drm/drm_crtc_helper.h>
+
+#include "rockchip_drm_drv.h"
+#include "rockchip_drm_gem.h"
+
+#define to_rockchip_fb(x) container_of(x, struct rockchip_drm_fb, fb)
+
+struct rockchip_drm_fb {
+	struct drm_framebuffer fb;
+	struct drm_gem_object *obj[ROCKCHIP_MAX_FB_BUFFER];
+};
+
+struct drm_gem_object *rockchip_fb_get_gem_obj(struct drm_framebuffer *fb,
+					       unsigned int plane)
+{
+	struct rockchip_drm_fb *rk_fb = to_rockchip_fb(fb);
+
+	if (plane >= ROCKCHIP_MAX_FB_BUFFER)
+		return NULL;
+
+	return rk_fb->obj[plane];
+}
+EXPORT_SYMBOL_GPL(rockchip_fb_get_gem_obj);
+
+static void rockchip_drm_fb_destroy(struct drm_framebuffer *fb)
+{
+	struct rockchip_drm_fb *rockchip_fb = to_rockchip_fb(fb);
+	struct drm_gem_object *obj;
+	int i;
+
+	for (i = 0; i < ROCKCHIP_MAX_FB_BUFFER; i++) {
+		obj = rockchip_fb->obj[i];
+		if (obj)
+			drm_gem_object_unreference_unlocked(obj);
+	}
+
+	drm_framebuffer_cleanup(fb);
+	kfree(rockchip_fb);
+}
+
+static int rockchip_drm_fb_create_handle(struct drm_framebuffer *fb,
+					 struct drm_file *file_priv,
+					 unsigned int *handle)
+{
+	struct rockchip_drm_fb *rockchip_fb = to_rockchip_fb(fb);
+
+	return drm_gem_handle_create(file_priv,
+				     rockchip_fb->obj[0], handle);
+}
+
+static struct drm_framebuffer_funcs rockchip_drm_fb_funcs = {
+	.destroy	= rockchip_drm_fb_destroy,
+	.create_handle	= rockchip_drm_fb_create_handle,
+};
+
+static struct rockchip_drm_fb *
+rockchip_fb_alloc(struct drm_device *dev, struct drm_mode_fb_cmd2 *mode_cmd,
+		  struct drm_gem_object **obj, unsigned int num_planes)
+{
+	struct rockchip_drm_fb *rockchip_fb;
+	int ret;
+	int i;
+
+	rockchip_fb = kzalloc(sizeof(*rockchip_fb), GFP_KERNEL);
+	if (!rockchip_fb)
+		return ERR_PTR(-ENOMEM);
+
+	drm_helper_mode_fill_fb_struct(&rockchip_fb->fb, mode_cmd);
+
+	for (i = 0; i < num_planes; i++)
+		rockchip_fb->obj[i] = obj[i];
+
+	ret = drm_framebuffer_init(dev, &rockchip_fb->fb,
+				   &rockchip_drm_fb_funcs);
+	if (ret) {
+		dev_err(dev->dev, "Failed to initialize framebuffer: %d\n",
+			ret);
+		kfree(rockchip_fb);
+		return ERR_PTR(ret);
+	}
+
+	return rockchip_fb;
+}
+
+static struct drm_framebuffer *
+rockchip_user_fb_create(struct drm_device *dev, struct drm_file *file_priv,
+			struct drm_mode_fb_cmd2 *mode_cmd)
+{
+	struct rockchip_drm_fb *rockchip_fb;
+	struct drm_gem_object *objs[ROCKCHIP_MAX_FB_BUFFER];
+	struct drm_gem_object *obj;
+	unsigned int hsub;
+	unsigned int vsub;
+	int num_planes;
+	int ret;
+	int i;
+
+	hsub = drm_format_horz_chroma_subsampling(mode_cmd->pixel_format);
+	vsub = drm_format_vert_chroma_subsampling(mode_cmd->pixel_format);
+	num_planes = min(drm_format_num_planes(mode_cmd->pixel_format),
+			 ROCKCHIP_MAX_FB_BUFFER);
+
+	for (i = 0; i < num_planes; i++) {
+		unsigned int width = mode_cmd->width / (i ? hsub : 1);
+		unsigned int height = mode_cmd->height / (i ? vsub : 1);
+		unsigned int min_size;
+
+		obj = drm_gem_object_lookup(dev, file_priv,
+					    mode_cmd->handles[i]);
+		if (!obj) {
+			dev_err(dev->dev, "Failed to lookup GEM object\n");
+			ret = -ENXIO;
+			goto err_gem_object_unreference;
+		}
+
+		min_size = (height - 1) * mode_cmd->pitches[i] +
+			mode_cmd->offsets[i] +
+			width * drm_format_plane_cpp(mode_cmd->pixel_format, i);
+
+		if (obj->size < min_size) {
+			drm_gem_object_unreference_unlocked(obj);
+			ret = -EINVAL;
+			goto err_gem_object_unreference;
+		}
+		objs[i] = obj;
+	}
+
+	rockchip_fb = rockchip_fb_alloc(dev, mode_cmd, objs, i);
+	if (IS_ERR(rockchip_fb)) {
+		ret = PTR_ERR(rockchip_fb);
+		goto err_gem_object_unreference;
+	}
+
+	return &rockchip_fb->fb;
+
+err_gem_object_unreference:
+	for (i--; i >= 0; i--)
+		drm_gem_object_unreference_unlocked(objs[i]);
+	return ERR_PTR(ret);
+}
+
+static void rockchip_drm_output_poll_changed(struct drm_device *dev)
+{
+	struct rockchip_drm_private *private = dev->dev_private;
+	struct drm_fb_helper *fb_helper = &private->fbdev_helper;
+
+	drm_fb_helper_hotplug_event(fb_helper);
+}
+
+static const struct drm_mode_config_funcs rockchip_drm_mode_config_funcs = {
+	.fb_create = rockchip_user_fb_create,
+	.output_poll_changed = rockchip_drm_output_poll_changed,
+};
+
+struct drm_framebuffer *
+rockchip_drm_framebuffer_init(struct drm_device *dev,
+			      struct drm_mode_fb_cmd2 *mode_cmd,
+			      struct drm_gem_object *obj)
+{
+	struct rockchip_drm_fb *rockchip_fb;
+
+	rockchip_fb = rockchip_fb_alloc(dev, mode_cmd, &obj, 1);
+	if (IS_ERR(rockchip_fb))
+		return NULL;
+
+	return &rockchip_fb->fb;
+}
+
+void rockchip_drm_mode_config_init(struct drm_device *dev)
+{
+	dev->mode_config.min_width = 0;
+	dev->mode_config.min_height = 0;
+
+	/*
+	 * set max width and height as default value(4096x4096).
+	 * this value would be used to check framebuffer size limitation
+	 * at drm_mode_addfb().
+	 */
+	dev->mode_config.max_width = 4096;
+	dev->mode_config.max_height = 4096;
+
+	dev->mode_config.funcs = &rockchip_drm_mode_config_funcs;
+}
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_fb.h b/drivers/gpu/drm/rockchip/rockchip_drm_fb.h
new file mode 100644
index 000000000000..09574d48226f
--- /dev/null
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_fb.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (C) Fuzhou Rockchip Electronics Co.Ltd
+ * Author:Mark Yao <mark.yao@rock-chips.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 _ROCKCHIP_DRM_FB_H
+#define _ROCKCHIP_DRM_FB_H
+
+struct drm_framebuffer *
+rockchip_drm_framebuffer_init(struct drm_device *dev,
+			      struct drm_mode_fb_cmd2 *mode_cmd,
+			      struct drm_gem_object *obj);
+void rockchip_drm_framebuffer_fini(struct drm_framebuffer *fb);
+
+void rockchip_drm_mode_config_init(struct drm_device *dev);
+
+struct drm_gem_object *rockchip_fb_get_gem_obj(struct drm_framebuffer *fb,
+					       unsigned int plane);
+#endif /* _ROCKCHIP_DRM_FB_H */
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_fbdev.c b/drivers/gpu/drm/rockchip/rockchip_drm_fbdev.c
new file mode 100644
index 000000000000..a5d889a8716b
--- /dev/null
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_fbdev.c
@@ -0,0 +1,210 @@
+/*
+ * Copyright (C) Fuzhou Rockchip Electronics Co.Ltd
+ * Author:Mark Yao <mark.yao@rock-chips.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 <drm/drm.h>
+#include <drm/drmP.h>
+#include <drm/drm_fb_helper.h>
+#include <drm/drm_crtc_helper.h>
+
+#include "rockchip_drm_drv.h"
+#include "rockchip_drm_gem.h"
+#include "rockchip_drm_fb.h"
+
+#define PREFERRED_BPP		32
+#define to_drm_private(x) \
+		container_of(x, struct rockchip_drm_private, fbdev_helper)
+
+static int rockchip_fbdev_mmap(struct fb_info *info,
+			       struct vm_area_struct *vma)
+{
+	struct drm_fb_helper *helper = info->par;
+	struct rockchip_drm_private *private = to_drm_private(helper);
+
+	return rockchip_gem_mmap_buf(private->fbdev_bo, vma);
+}
+
+static struct fb_ops rockchip_drm_fbdev_ops = {
+	.owner		= THIS_MODULE,
+	.fb_mmap	= rockchip_fbdev_mmap,
+	.fb_fillrect	= cfb_fillrect,
+	.fb_copyarea	= cfb_copyarea,
+	.fb_imageblit	= cfb_imageblit,
+	.fb_check_var	= drm_fb_helper_check_var,
+	.fb_set_par	= drm_fb_helper_set_par,
+	.fb_blank	= drm_fb_helper_blank,
+	.fb_pan_display	= drm_fb_helper_pan_display,
+	.fb_setcmap	= drm_fb_helper_setcmap,
+};
+
+static int rockchip_drm_fbdev_create(struct drm_fb_helper *helper,
+				     struct drm_fb_helper_surface_size *sizes)
+{
+	struct rockchip_drm_private *private = to_drm_private(helper);
+	struct drm_mode_fb_cmd2 mode_cmd = { 0 };
+	struct drm_device *dev = helper->dev;
+	struct rockchip_gem_object *rk_obj;
+	struct drm_framebuffer *fb;
+	unsigned int bytes_per_pixel;
+	unsigned long offset;
+	struct fb_info *fbi;
+	size_t size;
+	int ret;
+
+	bytes_per_pixel = DIV_ROUND_UP(sizes->surface_bpp, 8);
+
+	mode_cmd.width = sizes->surface_width;
+	mode_cmd.height = sizes->surface_height;
+	mode_cmd.pitches[0] = sizes->surface_width * bytes_per_pixel;
+	mode_cmd.pixel_format = drm_mode_legacy_fb_format(sizes->surface_bpp,
+		sizes->surface_depth);
+
+	size = mode_cmd.pitches[0] * mode_cmd.height;
+
+	rk_obj = rockchip_gem_create_object(dev, size);
+	if (IS_ERR(rk_obj))
+		return -ENOMEM;
+
+	private->fbdev_bo = &rk_obj->base;
+
+	fbi = framebuffer_alloc(0, dev->dev);
+	if (!fbi) {
+		dev_err(dev->dev, "Failed to allocate framebuffer info.\n");
+		ret = -ENOMEM;
+		goto err_rockchip_gem_free_object;
+	}
+
+	helper->fb = rockchip_drm_framebuffer_init(dev, &mode_cmd,
+						   private->fbdev_bo);
+	if (IS_ERR(helper->fb)) {
+		dev_err(dev->dev, "Failed to allocate DRM framebuffer.\n");
+		ret = PTR_ERR(helper->fb);
+		goto err_framebuffer_release;
+	}
+
+	helper->fbdev = fbi;
+
+	fbi->par = helper;
+	fbi->flags = FBINFO_FLAG_DEFAULT;
+	fbi->fbops = &rockchip_drm_fbdev_ops;
+
+	ret = fb_alloc_cmap(&fbi->cmap, 256, 0);
+	if (ret) {
+		dev_err(dev->dev, "Failed to allocate color map.\n");
+		goto err_drm_framebuffer_unref;
+	}
+
+	fb = helper->fb;
+	drm_fb_helper_fill_fix(fbi, fb->pitches[0], fb->depth);
+	drm_fb_helper_fill_var(fbi, helper, fb->width, fb->height);
+
+	offset = fbi->var.xoffset * bytes_per_pixel;
+	offset += fbi->var.yoffset * fb->pitches[0];
+
+	dev->mode_config.fb_base = 0;
+	fbi->screen_base = rk_obj->kvaddr + offset;
+	fbi->screen_size = rk_obj->base.size;
+	fbi->fix.smem_len = rk_obj->base.size;
+
+	DRM_DEBUG_KMS("FB [%dx%d]-%d kvaddr=%p offset=%ld size=%d\n",
+		      fb->width, fb->height, fb->depth, rk_obj->kvaddr,
+		      offset, size);
+	return 0;
+
+err_drm_framebuffer_unref:
+	drm_framebuffer_unreference(helper->fb);
+err_framebuffer_release:
+	framebuffer_release(fbi);
+err_rockchip_gem_free_object:
+	rockchip_gem_free_object(&rk_obj->base);
+	return ret;
+}
+
+static const struct drm_fb_helper_funcs rockchip_drm_fb_helper_funcs = {
+	.fb_probe = rockchip_drm_fbdev_create,
+};
+
+int rockchip_drm_fbdev_init(struct drm_device *dev)
+{
+	struct rockchip_drm_private *private = dev->dev_private;
+	struct drm_fb_helper *helper;
+	unsigned int num_crtc;
+	int ret;
+
+	if (!dev->mode_config.num_crtc || !dev->mode_config.num_connector)
+		return -EINVAL;
+
+	num_crtc = dev->mode_config.num_crtc;
+
+	helper = &private->fbdev_helper;
+
+	drm_fb_helper_prepare(dev, helper, &rockchip_drm_fb_helper_funcs);
+
+	ret = drm_fb_helper_init(dev, helper, num_crtc, ROCKCHIP_MAX_CONNECTOR);
+	if (ret < 0) {
+		dev_err(dev->dev, "Failed to initialize drm fb helper - %d.\n",
+			ret);
+		return ret;
+	}
+
+	ret = drm_fb_helper_single_add_all_connectors(helper);
+	if (ret < 0) {
+		dev_err(dev->dev, "Failed to add connectors - %d.\n", ret);
+		goto err_drm_fb_helper_fini;
+	}
+
+	/* disable all the possible outputs/crtcs before entering KMS mode */
+	drm_helper_disable_unused_functions(dev);
+
+	ret = drm_fb_helper_initial_config(helper, PREFERRED_BPP);
+	if (ret < 0) {
+		dev_err(dev->dev, "Failed to set initial hw config - %d.\n",
+			ret);
+		goto err_drm_fb_helper_fini;
+	}
+
+	return 0;
+
+err_drm_fb_helper_fini:
+	drm_fb_helper_fini(helper);
+	return ret;
+}
+
+void rockchip_drm_fbdev_fini(struct drm_device *dev)
+{
+	struct rockchip_drm_private *private = dev->dev_private;
+	struct drm_fb_helper *helper;
+
+	helper = &private->fbdev_helper;
+
+	if (helper->fbdev) {
+		struct fb_info *info;
+		int ret;
+
+		info = helper->fbdev;
+		ret = unregister_framebuffer(info);
+		if (ret < 0)
+			DRM_DEBUG_KMS("failed unregister_framebuffer() - %d\n",
+				      ret);
+
+		if (info->cmap.len)
+			fb_dealloc_cmap(&info->cmap);
+
+		framebuffer_release(info);
+	}
+
+	if (helper->fb)
+		drm_framebuffer_unreference(helper->fb);
+
+	drm_fb_helper_fini(helper);
+}
diff --git a/drivers/staging/android/binder.h b/drivers/gpu/drm/rockchip/rockchip_drm_fbdev.h
index eb0834656dfe..50432e9b5b37 100644
--- a/drivers/staging/android/binder.h
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_fbdev.h
@@ -1,10 +1,6 @@
 /*
- * Copyright (C) 2008 Google, Inc.
- *
- * Based on, but no longer compatible with, the original
- * OpenBinder.org binder driver interface, which is:
- *
- * Copyright (c) 2005 Palmsource, Inc.
+ * Copyright (C) Fuzhou Rockchip Electronics Co.Ltd
+ * Author:Mark Yao <mark.yao@rock-chips.com>
  *
  * This software is licensed under the terms of the GNU General Public
  * License version 2, as published by the Free Software Foundation, and
@@ -14,17 +10,12 @@
  * but WITHOUT 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_BINDER_H
-#define _LINUX_BINDER_H
-
-#ifdef CONFIG_ANDROID_BINDER_IPC_32BIT
-#define BINDER_IPC_32BIT 1
-#endif
-
-#include "uapi/binder.h"
+#ifndef _ROCKCHIP_DRM_FBDEV_H
+#define _ROCKCHIP_DRM_FBDEV_H
 
-#endif /* _LINUX_BINDER_H */
+int rockchip_drm_fbdev_init(struct drm_device *dev);
+void rockchip_drm_fbdev_fini(struct drm_device *dev);
 
+#endif /* _ROCKCHIP_DRM_FBDEV_H */
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_gem.c b/drivers/gpu/drm/rockchip/rockchip_drm_gem.c
new file mode 100644
index 000000000000..bc98a227dc76
--- /dev/null
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_gem.c
@@ -0,0 +1,294 @@
+/*
+ * Copyright (C) Fuzhou Rockchip Electronics Co.Ltd
+ * Author:Mark Yao <mark.yao@rock-chips.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 <drm/drm.h>
+#include <drm/drmP.h>
+#include <drm/drm_gem.h>
+#include <drm/drm_vma_manager.h>
+
+#include <linux/dma-attrs.h>
+
+#include "rockchip_drm_drv.h"
+#include "rockchip_drm_gem.h"
+
+static int rockchip_gem_alloc_buf(struct rockchip_gem_object *rk_obj)
+{
+	struct drm_gem_object *obj = &rk_obj->base;
+	struct drm_device *drm = obj->dev;
+
+	init_dma_attrs(&rk_obj->dma_attrs);
+	dma_set_attr(DMA_ATTR_WRITE_COMBINE, &rk_obj->dma_attrs);
+
+	/* TODO(djkurtz): Use DMA_ATTR_NO_KERNEL_MAPPING except for fbdev */
+	rk_obj->kvaddr = dma_alloc_attrs(drm->dev, obj->size,
+					 &rk_obj->dma_addr, GFP_KERNEL,
+					 &rk_obj->dma_attrs);
+	if (IS_ERR(rk_obj->kvaddr)) {
+		int ret = PTR_ERR(rk_obj->kvaddr);
+
+		DRM_ERROR("failed to allocate %#x byte dma buffer, %d",
+			  obj->size, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void rockchip_gem_free_buf(struct rockchip_gem_object *rk_obj)
+{
+	struct drm_gem_object *obj = &rk_obj->base;
+	struct drm_device *drm = obj->dev;
+
+	dma_free_attrs(drm->dev, obj->size, rk_obj->kvaddr, rk_obj->dma_addr,
+		       &rk_obj->dma_attrs);
+}
+
+int rockchip_gem_mmap_buf(struct drm_gem_object *obj,
+			  struct vm_area_struct *vma)
+{
+	struct rockchip_gem_object *rk_obj = to_rockchip_obj(obj);
+	struct drm_device *drm = obj->dev;
+	unsigned long vm_size;
+
+	vma->vm_flags |= VM_IO | VM_DONTEXPAND | VM_DONTDUMP;
+	vm_size = vma->vm_end - vma->vm_start;
+
+	if (vm_size > obj->size)
+		return -EINVAL;
+
+	return dma_mmap_attrs(drm->dev, vma, rk_obj->kvaddr, rk_obj->dma_addr,
+			     obj->size, &rk_obj->dma_attrs);
+}
+
+/* drm driver mmap file operations */
+int rockchip_gem_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	struct drm_file *priv = filp->private_data;
+	struct drm_device *dev = priv->minor->dev;
+	struct drm_gem_object *obj;
+	struct drm_vma_offset_node *node;
+	int ret;
+
+	if (drm_device_is_unplugged(dev))
+		return -ENODEV;
+
+	mutex_lock(&dev->struct_mutex);
+
+	node = drm_vma_offset_exact_lookup(dev->vma_offset_manager,
+					   vma->vm_pgoff,
+					   vma_pages(vma));
+	if (!node) {
+		mutex_unlock(&dev->struct_mutex);
+		DRM_ERROR("failed to find vma node.\n");
+		return -EINVAL;
+	} else if (!drm_vma_node_is_allowed(node, filp)) {
+		mutex_unlock(&dev->struct_mutex);
+		return -EACCES;
+	}
+
+	obj = container_of(node, struct drm_gem_object, vma_node);
+	ret = rockchip_gem_mmap_buf(obj, vma);
+
+	mutex_unlock(&dev->struct_mutex);
+
+	return ret;
+}
+
+struct rockchip_gem_object *
+	rockchip_gem_create_object(struct drm_device *drm, unsigned int size)
+{
+	struct rockchip_gem_object *rk_obj;
+	struct drm_gem_object *obj;
+	int ret;
+
+	size = round_up(size, PAGE_SIZE);
+
+	rk_obj = kzalloc(sizeof(*rk_obj), GFP_KERNEL);
+	if (!rk_obj)
+		return ERR_PTR(-ENOMEM);
+
+	obj = &rk_obj->base;
+
+	drm_gem_private_object_init(drm, obj, size);
+
+	ret = rockchip_gem_alloc_buf(rk_obj);
+	if (ret)
+		goto err_free_rk_obj;
+
+	return rk_obj;
+
+err_free_rk_obj:
+	kfree(rk_obj);
+	return ERR_PTR(ret);
+}
+
+/*
+ * rockchip_gem_free_object - (struct drm_driver)->gem_free_object callback
+ * function
+ */
+void rockchip_gem_free_object(struct drm_gem_object *obj)
+{
+	struct rockchip_gem_object *rk_obj;
+
+	drm_gem_free_mmap_offset(obj);
+
+	rk_obj = to_rockchip_obj(obj);
+
+	rockchip_gem_free_buf(rk_obj);
+
+	kfree(rk_obj);
+}
+
+/*
+ * rockchip_gem_create_with_handle - allocate an object with the given
+ * size and create a gem handle on it
+ *
+ * returns a struct rockchip_gem_object* on success or ERR_PTR values
+ * on failure.
+ */
+static struct rockchip_gem_object *
+rockchip_gem_create_with_handle(struct drm_file *file_priv,
+				struct drm_device *drm, unsigned int size,
+				unsigned int *handle)
+{
+	struct rockchip_gem_object *rk_obj;
+	struct drm_gem_object *obj;
+	int ret;
+
+	rk_obj = rockchip_gem_create_object(drm, size);
+	if (IS_ERR(rk_obj))
+		return ERR_CAST(rk_obj);
+
+	obj = &rk_obj->base;
+
+	/*
+	 * allocate a id of idr table where the obj is registered
+	 * and handle has the id what user can see.
+	 */
+	ret = drm_gem_handle_create(file_priv, obj, handle);
+	if (ret)
+		goto err_handle_create;
+
+	/* drop reference from allocate - handle holds it now. */
+	drm_gem_object_unreference_unlocked(obj);
+
+	return rk_obj;
+
+err_handle_create:
+	rockchip_gem_free_object(obj);
+
+	return ERR_PTR(ret);
+}
+
+int rockchip_gem_dumb_map_offset(struct drm_file *file_priv,
+				 struct drm_device *dev, uint32_t handle,
+				 uint64_t *offset)
+{
+	struct drm_gem_object *obj;
+	int ret;
+
+	mutex_lock(&dev->struct_mutex);
+
+	obj = drm_gem_object_lookup(dev, file_priv, handle);
+	if (!obj) {
+		DRM_ERROR("failed to lookup gem object.\n");
+		ret = -EINVAL;
+		goto unlock;
+	}
+
+	ret = drm_gem_create_mmap_offset(obj);
+	if (ret)
+		goto out;
+
+	*offset = drm_vma_node_offset_addr(&obj->vma_node);
+	DRM_DEBUG_KMS("offset = 0x%llx\n", *offset);
+
+out:
+	drm_gem_object_unreference(obj);
+unlock:
+	mutex_unlock(&dev->struct_mutex);
+	return ret;
+}
+
+/*
+ * rockchip_gem_dumb_create - (struct drm_driver)->dumb_create callback
+ * function
+ *
+ * This aligns the pitch and size arguments to the minimum required. wrap
+ * this into your own function if you need bigger alignment.
+ */
+int rockchip_gem_dumb_create(struct drm_file *file_priv,
+			     struct drm_device *dev,
+			     struct drm_mode_create_dumb *args)
+{
+	struct rockchip_gem_object *rk_obj;
+	int min_pitch = DIV_ROUND_UP(args->width * args->bpp, 8);
+
+	/*
+	 * align to 64 bytes since Mali requires it.
+	 */
+	min_pitch = ALIGN(min_pitch, 64);
+
+	if (args->pitch < min_pitch)
+		args->pitch = min_pitch;
+
+	if (args->size < args->pitch * args->height)
+		args->size = args->pitch * args->height;
+
+	rk_obj = rockchip_gem_create_with_handle(file_priv, dev, args->size,
+						 &args->handle);
+
+	return PTR_ERR_OR_ZERO(rk_obj);
+}
+
+/*
+ * Allocate a sg_table for this GEM object.
+ * Note: Both the table's contents, and the sg_table itself must be freed by
+ *       the caller.
+ * Returns a pointer to the newly allocated sg_table, or an ERR_PTR() error.
+ */
+struct sg_table *rockchip_gem_prime_get_sg_table(struct drm_gem_object *obj)
+{
+	struct rockchip_gem_object *rk_obj = to_rockchip_obj(obj);
+	struct drm_device *drm = obj->dev;
+	struct sg_table *sgt;
+	int ret;
+
+	sgt = kzalloc(sizeof(*sgt), GFP_KERNEL);
+	if (!sgt)
+		return ERR_PTR(-ENOMEM);
+
+	ret = dma_get_sgtable_attrs(drm->dev, sgt, rk_obj->kvaddr,
+				    rk_obj->dma_addr, obj->size,
+				    &rk_obj->dma_attrs);
+	if (ret) {
+		DRM_ERROR("failed to allocate sgt, %d\n", ret);
+		kfree(sgt);
+		return ERR_PTR(ret);
+	}
+
+	return sgt;
+}
+
+void *rockchip_gem_prime_vmap(struct drm_gem_object *obj)
+{
+	struct rockchip_gem_object *rk_obj = to_rockchip_obj(obj);
+
+	return rk_obj->kvaddr;
+}
+
+void rockchip_gem_prime_vunmap(struct drm_gem_object *obj, void *vaddr)
+{
+	/* Nothing to do */
+}
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_gem.h b/drivers/gpu/drm/rockchip/rockchip_drm_gem.h
new file mode 100644
index 000000000000..67bcebe90003
--- /dev/null
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_gem.h
@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) Fuzhou Rockchip Electronics Co.Ltd
+ * Author:Mark Yao <mark.yao@rock-chips.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 _ROCKCHIP_DRM_GEM_H
+#define _ROCKCHIP_DRM_GEM_H
+
+#define to_rockchip_obj(x) container_of(x, struct rockchip_gem_object, base)
+
+struct rockchip_gem_object {
+	struct drm_gem_object base;
+	unsigned int flags;
+
+	void *kvaddr;
+	dma_addr_t dma_addr;
+	struct dma_attrs dma_attrs;
+};
+
+struct sg_table *rockchip_gem_prime_get_sg_table(struct drm_gem_object *obj);
+struct drm_gem_object *
+rockchip_gem_prime_import_sg_table(struct drm_device *dev, size_t size,
+				   struct sg_table *sgt);
+void *rockchip_gem_prime_vmap(struct drm_gem_object *obj);
+void rockchip_gem_prime_vunmap(struct drm_gem_object *obj, void *vaddr);
+
+/* drm driver mmap file operations */
+int rockchip_gem_mmap(struct file *filp, struct vm_area_struct *vma);
+
+/* mmap a gem object to userspace. */
+int rockchip_gem_mmap_buf(struct drm_gem_object *obj,
+			  struct vm_area_struct *vma);
+
+struct rockchip_gem_object *
+	rockchip_gem_create_object(struct drm_device *drm, unsigned int size);
+
+void rockchip_gem_free_object(struct drm_gem_object *obj);
+
+int rockchip_gem_dumb_create(struct drm_file *file_priv,
+			     struct drm_device *dev,
+			     struct drm_mode_create_dumb *args);
+int rockchip_gem_dumb_map_offset(struct drm_file *file_priv,
+				 struct drm_device *dev, uint32_t handle,
+				 uint64_t *offset);
+#endif /* _ROCKCHIP_DRM_GEM_H */
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_vop.c b/drivers/gpu/drm/rockchip/rockchip_drm_vop.c
new file mode 100644
index 000000000000..e7ca25b3fb38
--- /dev/null
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_vop.c
@@ -0,0 +1,1455 @@
+/*
+ * Copyright (C) Fuzhou Rockchip Electronics Co.Ltd
+ * Author:Mark Yao <mark.yao@rock-chips.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 <drm/drm.h>
+#include <drm/drmP.h>
+#include <drm/drm_crtc.h>
+#include <drm/drm_crtc_helper.h>
+#include <drm/drm_plane_helper.h>
+
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/component.h>
+
+#include <linux/reset.h>
+#include <linux/delay.h>
+
+#include "rockchip_drm_drv.h"
+#include "rockchip_drm_gem.h"
+#include "rockchip_drm_fb.h"
+#include "rockchip_drm_vop.h"
+
+#define VOP_REG(off, _mask, s) \
+		{.offset = off, \
+		 .mask = _mask, \
+		 .shift = s,}
+
+#define __REG_SET_RELAXED(x, off, mask, shift, v) \
+		vop_mask_write_relaxed(x, off, (mask) << shift, (v) << shift)
+#define __REG_SET_NORMAL(x, off, mask, shift, v) \
+		vop_mask_write(x, off, (mask) << shift, (v) << shift)
+
+#define REG_SET(x, base, reg, v, mode) \
+		__REG_SET_##mode(x, base + reg.offset, reg.mask, reg.shift, v)
+
+#define VOP_WIN_SET(x, win, name, v) \
+		REG_SET(x, win->base, win->phy->name, v, RELAXED)
+#define VOP_CTRL_SET(x, name, v) \
+		REG_SET(x, 0, (x)->data->ctrl->name, v, NORMAL)
+
+#define VOP_WIN_GET(x, win, name) \
+		vop_read_reg(x, win->base, &win->phy->name)
+
+#define VOP_WIN_GET_YRGBADDR(vop, win) \
+		vop_readl(vop, win->base + win->phy->yrgb_mst.offset)
+
+#define to_vop(x) container_of(x, struct vop, crtc)
+#define to_vop_win(x) container_of(x, struct vop_win, base)
+
+struct vop_win_state {
+	struct list_head head;
+	struct drm_framebuffer *fb;
+	dma_addr_t yrgb_mst;
+	struct drm_pending_vblank_event *event;
+};
+
+struct vop_win {
+	struct drm_plane base;
+	const struct vop_win_data *data;
+	struct vop *vop;
+
+	struct list_head pending;
+	struct vop_win_state *active;
+};
+
+struct vop {
+	struct drm_crtc crtc;
+	struct device *dev;
+	struct drm_device *drm_dev;
+	unsigned int dpms;
+
+	int connector_type;
+	int connector_out_mode;
+
+	/* mutex vsync_ work */
+	struct mutex vsync_mutex;
+	bool vsync_work_pending;
+
+	const struct vop_data *data;
+
+	uint32_t *regsbak;
+	void __iomem *regs;
+
+	/* physical map length of vop register */
+	uint32_t len;
+
+	/* one time only one process allowed to config the register */
+	spinlock_t reg_lock;
+	/* lock vop irq reg */
+	spinlock_t irq_lock;
+
+	unsigned int irq;
+
+	/* vop AHP clk */
+	struct clk *hclk;
+	/* vop dclk */
+	struct clk *dclk;
+	/* vop share memory frequency */
+	struct clk *aclk;
+
+	/* vop dclk reset */
+	struct reset_control *dclk_rst;
+
+	int pipe;
+
+	struct vop_win win[];
+};
+
+enum vop_data_format {
+	VOP_FMT_ARGB8888 = 0,
+	VOP_FMT_RGB888,
+	VOP_FMT_RGB565,
+	VOP_FMT_YUV420SP = 4,
+	VOP_FMT_YUV422SP,
+	VOP_FMT_YUV444SP,
+};
+
+struct vop_reg_data {
+	uint32_t offset;
+	uint32_t value;
+};
+
+struct vop_reg {
+	uint32_t offset;
+	uint32_t shift;
+	uint32_t mask;
+};
+
+struct vop_ctrl {
+	struct vop_reg standby;
+	struct vop_reg data_blank;
+	struct vop_reg gate_en;
+	struct vop_reg mmu_en;
+	struct vop_reg rgb_en;
+	struct vop_reg edp_en;
+	struct vop_reg hdmi_en;
+	struct vop_reg mipi_en;
+	struct vop_reg out_mode;
+	struct vop_reg dither_down;
+	struct vop_reg dither_up;
+	struct vop_reg pin_pol;
+
+	struct vop_reg htotal_pw;
+	struct vop_reg hact_st_end;
+	struct vop_reg vtotal_pw;
+	struct vop_reg vact_st_end;
+	struct vop_reg hpost_st_end;
+	struct vop_reg vpost_st_end;
+};
+
+struct vop_win_phy {
+	const uint32_t *data_formats;
+	uint32_t nformats;
+
+	struct vop_reg enable;
+	struct vop_reg format;
+	struct vop_reg act_info;
+	struct vop_reg dsp_info;
+	struct vop_reg dsp_st;
+	struct vop_reg yrgb_mst;
+	struct vop_reg uv_mst;
+	struct vop_reg yrgb_vir;
+	struct vop_reg uv_vir;
+
+	struct vop_reg dst_alpha_ctl;
+	struct vop_reg src_alpha_ctl;
+};
+
+struct vop_win_data {
+	uint32_t base;
+	const struct vop_win_phy *phy;
+	enum drm_plane_type type;
+};
+
+struct vop_data {
+	const struct vop_reg_data *init_table;
+	unsigned int table_size;
+	const struct vop_ctrl *ctrl;
+	const struct vop_win_data *win;
+	unsigned int win_size;
+};
+
+static const uint32_t formats_01[] = {
+	DRM_FORMAT_XRGB8888,
+	DRM_FORMAT_ARGB8888,
+	DRM_FORMAT_RGB888,
+	DRM_FORMAT_RGB565,
+	DRM_FORMAT_NV12,
+	DRM_FORMAT_NV16,
+	DRM_FORMAT_NV24,
+};
+
+static const uint32_t formats_234[] = {
+	DRM_FORMAT_XRGB8888,
+	DRM_FORMAT_ARGB8888,
+	DRM_FORMAT_RGB888,
+	DRM_FORMAT_RGB565,
+};
+
+static const struct vop_win_phy win01_data = {
+	.data_formats = formats_01,
+	.nformats = ARRAY_SIZE(formats_01),
+	.enable = VOP_REG(WIN0_CTRL0, 0x1, 0),
+	.format = VOP_REG(WIN0_CTRL0, 0x7, 1),
+	.act_info = VOP_REG(WIN0_ACT_INFO, 0x1fff1fff, 0),
+	.dsp_info = VOP_REG(WIN0_DSP_INFO, 0x0fff0fff, 0),
+	.dsp_st = VOP_REG(WIN0_DSP_ST, 0x1fff1fff, 0),
+	.yrgb_mst = VOP_REG(WIN0_YRGB_MST, 0xffffffff, 0),
+	.uv_mst = VOP_REG(WIN0_CBR_MST, 0xffffffff, 0),
+	.yrgb_vir = VOP_REG(WIN0_VIR, 0x3fff, 0),
+	.uv_vir = VOP_REG(WIN0_VIR, 0x3fff, 16),
+	.src_alpha_ctl = VOP_REG(WIN0_SRC_ALPHA_CTRL, 0xff, 0),
+	.dst_alpha_ctl = VOP_REG(WIN0_DST_ALPHA_CTRL, 0xff, 0),
+};
+
+static const struct vop_win_phy win23_data = {
+	.data_formats = formats_234,
+	.nformats = ARRAY_SIZE(formats_234),
+	.enable = VOP_REG(WIN2_CTRL0, 0x1, 0),
+	.format = VOP_REG(WIN2_CTRL0, 0x7, 1),
+	.dsp_info = VOP_REG(WIN2_DSP_INFO0, 0x0fff0fff, 0),
+	.dsp_st = VOP_REG(WIN2_DSP_ST0, 0x1fff1fff, 0),
+	.yrgb_mst = VOP_REG(WIN2_MST0, 0xffffffff, 0),
+	.yrgb_vir = VOP_REG(WIN2_VIR0_1, 0x1fff, 0),
+	.src_alpha_ctl = VOP_REG(WIN2_SRC_ALPHA_CTRL, 0xff, 0),
+	.dst_alpha_ctl = VOP_REG(WIN2_DST_ALPHA_CTRL, 0xff, 0),
+};
+
+static const struct vop_win_phy cursor_data = {
+	.data_formats = formats_234,
+	.nformats = ARRAY_SIZE(formats_234),
+	.enable = VOP_REG(HWC_CTRL0, 0x1, 0),
+	.format = VOP_REG(HWC_CTRL0, 0x7, 1),
+	.dsp_st = VOP_REG(HWC_DSP_ST, 0x1fff1fff, 0),
+	.yrgb_mst = VOP_REG(HWC_MST, 0xffffffff, 0),
+};
+
+static const struct vop_ctrl ctrl_data = {
+	.standby = VOP_REG(SYS_CTRL, 0x1, 22),
+	.gate_en = VOP_REG(SYS_CTRL, 0x1, 23),
+	.mmu_en = VOP_REG(SYS_CTRL, 0x1, 20),
+	.rgb_en = VOP_REG(SYS_CTRL, 0x1, 12),
+	.hdmi_en = VOP_REG(SYS_CTRL, 0x1, 13),
+	.edp_en = VOP_REG(SYS_CTRL, 0x1, 14),
+	.mipi_en = VOP_REG(SYS_CTRL, 0x1, 15),
+	.dither_down = VOP_REG(DSP_CTRL1, 0xf, 1),
+	.dither_up = VOP_REG(DSP_CTRL1, 0x1, 6),
+	.data_blank = VOP_REG(DSP_CTRL0, 0x1, 19),
+	.out_mode = VOP_REG(DSP_CTRL0, 0xf, 0),
+	.pin_pol = VOP_REG(DSP_CTRL0, 0xf, 4),
+	.htotal_pw = VOP_REG(DSP_HTOTAL_HS_END, 0x1fff1fff, 0),
+	.hact_st_end = VOP_REG(DSP_HACT_ST_END, 0x1fff1fff, 0),
+	.vtotal_pw = VOP_REG(DSP_VTOTAL_VS_END, 0x1fff1fff, 0),
+	.vact_st_end = VOP_REG(DSP_VACT_ST_END, 0x1fff1fff, 0),
+	.hpost_st_end = VOP_REG(POST_DSP_HACT_INFO, 0x1fff1fff, 0),
+	.vpost_st_end = VOP_REG(POST_DSP_VACT_INFO, 0x1fff1fff, 0),
+};
+
+static const struct vop_reg_data vop_init_reg_table[] = {
+	{SYS_CTRL, 0x00c00000},
+	{DSP_CTRL0, 0x00000000},
+	{WIN0_CTRL0, 0x00000080},
+	{WIN1_CTRL0, 0x00000080},
+};
+
+/*
+ * Note: rk3288 has a dedicated 'cursor' window, however, that window requires
+ * special support to get alpha blending working.  For now, just use overlay
+ * window 1 for the drm cursor.
+ */
+static const struct vop_win_data rk3288_vop_win_data[] = {
+	{ .base = 0x00, .phy = &win01_data, .type = DRM_PLANE_TYPE_PRIMARY },
+	{ .base = 0x40, .phy = &win01_data, .type = DRM_PLANE_TYPE_CURSOR },
+	{ .base = 0x00, .phy = &win23_data, .type = DRM_PLANE_TYPE_OVERLAY },
+	{ .base = 0x50, .phy = &win23_data, .type = DRM_PLANE_TYPE_OVERLAY },
+	{ .base = 0x00, .phy = &cursor_data, .type = DRM_PLANE_TYPE_OVERLAY },
+};
+
+static const struct vop_data rk3288_vop = {
+	.init_table = vop_init_reg_table,
+	.table_size = ARRAY_SIZE(vop_init_reg_table),
+	.ctrl = &ctrl_data,
+	.win = rk3288_vop_win_data,
+	.win_size = ARRAY_SIZE(rk3288_vop_win_data),
+};
+
+static const struct of_device_id vop_driver_dt_match[] = {
+	{ .compatible = "rockchip,rk3288-vop",
+	  .data = &rk3288_vop },
+	{},
+};
+
+static inline void vop_writel(struct vop *vop, uint32_t offset, uint32_t v)
+{
+	writel(v, vop->regs + offset);
+	vop->regsbak[offset >> 2] = v;
+}
+
+static inline uint32_t vop_readl(struct vop *vop, uint32_t offset)
+{
+	return readl(vop->regs + offset);
+}
+
+static inline uint32_t vop_read_reg(struct vop *vop, uint32_t base,
+				    const struct vop_reg *reg)
+{
+	return (vop_readl(vop, base + reg->offset) >> reg->shift) & reg->mask;
+}
+
+static inline void vop_cfg_done(struct vop *vop)
+{
+	writel(0x01, vop->regs + REG_CFG_DONE);
+}
+
+static inline void vop_mask_write(struct vop *vop, uint32_t offset,
+				  uint32_t mask, uint32_t v)
+{
+	if (mask) {
+		uint32_t cached_val = vop->regsbak[offset >> 2];
+
+		cached_val = (cached_val & ~mask) | v;
+		writel(cached_val, vop->regs + offset);
+		vop->regsbak[offset >> 2] = cached_val;
+	}
+}
+
+static inline void vop_mask_write_relaxed(struct vop *vop, uint32_t offset,
+					  uint32_t mask, uint32_t v)
+{
+	if (mask) {
+		uint32_t cached_val = vop->regsbak[offset >> 2];
+
+		cached_val = (cached_val & ~mask) | v;
+		writel_relaxed(cached_val, vop->regs + offset);
+		vop->regsbak[offset >> 2] = cached_val;
+	}
+}
+
+static enum vop_data_format vop_convert_format(uint32_t format)
+{
+	switch (format) {
+	case DRM_FORMAT_XRGB8888:
+	case DRM_FORMAT_ARGB8888:
+		return VOP_FMT_ARGB8888;
+	case DRM_FORMAT_RGB888:
+		return VOP_FMT_RGB888;
+	case DRM_FORMAT_RGB565:
+		return VOP_FMT_RGB565;
+	case DRM_FORMAT_NV12:
+		return VOP_FMT_YUV420SP;
+	case DRM_FORMAT_NV16:
+		return VOP_FMT_YUV422SP;
+	case DRM_FORMAT_NV24:
+		return VOP_FMT_YUV444SP;
+	default:
+		DRM_ERROR("unsupport format[%08x]\n", format);
+		return -EINVAL;
+	}
+}
+
+static bool is_alpha_support(uint32_t format)
+{
+	switch (format) {
+	case DRM_FORMAT_ARGB8888:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static void vop_enable(struct drm_crtc *crtc)
+{
+	struct vop *vop = to_vop(crtc);
+	int ret;
+
+	ret = clk_enable(vop->hclk);
+	if (ret < 0) {
+		dev_err(vop->dev, "failed to enable hclk - %d\n", ret);
+		return;
+	}
+
+	ret = clk_enable(vop->dclk);
+	if (ret < 0) {
+		dev_err(vop->dev, "failed to enable dclk - %d\n", ret);
+		goto err_disable_hclk;
+	}
+
+	ret = clk_enable(vop->aclk);
+	if (ret < 0) {
+		dev_err(vop->dev, "failed to enable aclk - %d\n", ret);
+		goto err_disable_dclk;
+	}
+
+	/*
+	 * Slave iommu shares power, irq and clock with vop.  It was associated
+	 * automatically with this master device via common driver code.
+	 * Now that we have enabled the clock we attach it to the shared drm
+	 * mapping.
+	 */
+	ret = rockchip_drm_dma_attach_device(vop->drm_dev, vop->dev);
+	if (ret) {
+		dev_err(vop->dev, "failed to attach dma mapping, %d\n", ret);
+		goto err_disable_aclk;
+	}
+
+	spin_lock(&vop->reg_lock);
+
+	VOP_CTRL_SET(vop, standby, 0);
+
+	spin_unlock(&vop->reg_lock);
+
+	enable_irq(vop->irq);
+
+	drm_vblank_on(vop->drm_dev, vop->pipe);
+
+	return;
+
+err_disable_aclk:
+	clk_disable(vop->aclk);
+err_disable_dclk:
+	clk_disable(vop->dclk);
+err_disable_hclk:
+	clk_disable(vop->hclk);
+}
+
+static void vop_disable(struct drm_crtc *crtc)
+{
+	struct vop *vop = to_vop(crtc);
+
+	drm_vblank_off(crtc->dev, vop->pipe);
+
+	disable_irq(vop->irq);
+
+	/*
+	 * TODO: Since standby doesn't take effect until the next vblank,
+	 * when we turn off dclk below, the vop is probably still active.
+	 */
+	spin_lock(&vop->reg_lock);
+
+	VOP_CTRL_SET(vop, standby, 1);
+
+	spin_unlock(&vop->reg_lock);
+	/*
+	 * disable dclk to stop frame scan, so we can safely detach iommu,
+	 */
+	clk_disable(vop->dclk);
+
+	rockchip_drm_dma_detach_device(vop->drm_dev, vop->dev);
+
+	clk_disable(vop->aclk);
+	clk_disable(vop->hclk);
+}
+
+/*
+ * Caller must hold vsync_mutex.
+ */
+static struct drm_framebuffer *vop_win_last_pending_fb(struct vop_win *vop_win)
+{
+	struct vop_win_state *last;
+	struct vop_win_state *active = vop_win->active;
+
+	if (list_empty(&vop_win->pending))
+		return active ? active->fb : NULL;
+
+	last = list_last_entry(&vop_win->pending, struct vop_win_state, head);
+	return last ? last->fb : NULL;
+}
+
+/*
+ * Caller must hold vsync_mutex.
+ */
+static int vop_win_queue_fb(struct vop_win *vop_win,
+			    struct drm_framebuffer *fb, dma_addr_t yrgb_mst,
+			    struct drm_pending_vblank_event *event)
+{
+	struct vop_win_state *state;
+
+	state = kzalloc(sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	state->fb = fb;
+	state->yrgb_mst = yrgb_mst;
+	state->event = event;
+
+	list_add_tail(&state->head, &vop_win->pending);
+
+	return 0;
+}
+
+static int vop_update_plane_event(struct drm_plane *plane,
+				  struct drm_crtc *crtc,
+				  struct drm_framebuffer *fb, int crtc_x,
+				  int crtc_y, unsigned int crtc_w,
+				  unsigned int crtc_h, uint32_t src_x,
+				  uint32_t src_y, uint32_t src_w,
+				  uint32_t src_h,
+				  struct drm_pending_vblank_event *event)
+{
+	struct vop_win *vop_win = to_vop_win(plane);
+	const struct vop_win_data *win = vop_win->data;
+	struct vop *vop = to_vop(crtc);
+	struct drm_gem_object *obj;
+	struct rockchip_gem_object *rk_obj;
+	unsigned long offset;
+	unsigned int actual_w;
+	unsigned int actual_h;
+	unsigned int dsp_stx;
+	unsigned int dsp_sty;
+	unsigned int y_vir_stride;
+	dma_addr_t yrgb_mst;
+	enum vop_data_format format;
+	uint32_t val;
+	bool is_alpha;
+	bool visible;
+	int ret;
+	struct drm_rect dest = {
+		.x1 = crtc_x,
+		.y1 = crtc_y,
+		.x2 = crtc_x + crtc_w,
+		.y2 = crtc_y + crtc_h,
+	};
+	struct drm_rect src = {
+		/* 16.16 fixed point */
+		.x1 = src_x,
+		.y1 = src_y,
+		.x2 = src_x + src_w,
+		.y2 = src_y + src_h,
+	};
+	const struct drm_rect clip = {
+		.x2 = crtc->mode.hdisplay,
+		.y2 = crtc->mode.vdisplay,
+	};
+	bool can_position = plane->type != DRM_PLANE_TYPE_PRIMARY;
+
+	ret = drm_plane_helper_check_update(plane, crtc, fb,
+					    &src, &dest, &clip,
+					    DRM_PLANE_HELPER_NO_SCALING,
+					    DRM_PLANE_HELPER_NO_SCALING,
+					    can_position, false, &visible);
+	if (ret)
+		return ret;
+
+	if (!visible)
+		return 0;
+
+	is_alpha = is_alpha_support(fb->pixel_format);
+	format = vop_convert_format(fb->pixel_format);
+	if (format < 0)
+		return format;
+
+	obj = rockchip_fb_get_gem_obj(fb, 0);
+	if (!obj) {
+		DRM_ERROR("fail to get rockchip gem object from framebuffer\n");
+		return -EINVAL;
+	}
+
+	rk_obj = to_rockchip_obj(obj);
+
+	actual_w = (src.x2 - src.x1) >> 16;
+	actual_h = (src.y2 - src.y1) >> 16;
+	crtc_x = max(0, crtc_x);
+	crtc_y = max(0, crtc_y);
+
+	dsp_stx = crtc_x + crtc->mode.htotal - crtc->mode.hsync_start;
+	dsp_sty = crtc_y + crtc->mode.vtotal - crtc->mode.vsync_start;
+
+	offset = (src.x1 >> 16) * (fb->bits_per_pixel >> 3);
+	offset += (src.y1 >> 16) * fb->pitches[0];
+	yrgb_mst = rk_obj->dma_addr + offset;
+
+	y_vir_stride = fb->pitches[0] / (fb->bits_per_pixel >> 3);
+
+	/*
+	 * If this plane update changes the plane's framebuffer, (or more
+	 * precisely, if this update has a different framebuffer than the last
+	 * update), enqueue it so we can track when it completes.
+	 *
+	 * Only when we discover that this update has completed, can we
+	 * unreference any previous framebuffers.
+	 */
+	mutex_lock(&vop->vsync_mutex);
+	if (fb != vop_win_last_pending_fb(vop_win)) {
+		ret = drm_vblank_get(plane->dev, vop->pipe);
+		if (ret) {
+			DRM_ERROR("failed to get vblank, %d\n", ret);
+			mutex_unlock(&vop->vsync_mutex);
+			return ret;
+		}
+
+		drm_framebuffer_reference(fb);
+
+		ret = vop_win_queue_fb(vop_win, fb, yrgb_mst, event);
+		if (ret) {
+			drm_vblank_put(plane->dev, vop->pipe);
+			mutex_unlock(&vop->vsync_mutex);
+			return ret;
+		}
+
+		vop->vsync_work_pending = true;
+	}
+	mutex_unlock(&vop->vsync_mutex);
+
+	spin_lock(&vop->reg_lock);
+
+	VOP_WIN_SET(vop, win, format, format);
+	VOP_WIN_SET(vop, win, yrgb_vir, y_vir_stride);
+	VOP_WIN_SET(vop, win, yrgb_mst, yrgb_mst);
+	val = (actual_h - 1) << 16;
+	val |= (actual_w - 1) & 0xffff;
+	VOP_WIN_SET(vop, win, act_info, val);
+	VOP_WIN_SET(vop, win, dsp_info, val);
+	val = (dsp_sty - 1) << 16;
+	val |= (dsp_stx - 1) & 0xffff;
+	VOP_WIN_SET(vop, win, dsp_st, val);
+
+	if (is_alpha) {
+		VOP_WIN_SET(vop, win, dst_alpha_ctl,
+			    DST_FACTOR_M0(ALPHA_SRC_INVERSE));
+		val = SRC_ALPHA_EN(1) | SRC_COLOR_M0(ALPHA_SRC_PRE_MUL) |
+			SRC_ALPHA_M0(ALPHA_STRAIGHT) |
+			SRC_BLEND_M0(ALPHA_PER_PIX) |
+			SRC_ALPHA_CAL_M0(ALPHA_NO_SATURATION) |
+			SRC_FACTOR_M0(ALPHA_ONE);
+		VOP_WIN_SET(vop, win, src_alpha_ctl, val);
+	} else {
+		VOP_WIN_SET(vop, win, src_alpha_ctl, SRC_ALPHA_EN(0));
+	}
+
+	VOP_WIN_SET(vop, win, enable, 1);
+
+	vop_cfg_done(vop);
+	spin_unlock(&vop->reg_lock);
+
+	return 0;
+}
+
+static int vop_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
+			    struct drm_framebuffer *fb, int crtc_x, int crtc_y,
+			    unsigned int crtc_w, unsigned int crtc_h,
+			    uint32_t src_x, uint32_t src_y, uint32_t src_w,
+			    uint32_t src_h)
+{
+	return vop_update_plane_event(plane, crtc, fb, crtc_x, crtc_y, crtc_w,
+				      crtc_h, src_x, src_y, src_w, src_h,
+				      NULL);
+}
+
+static int vop_update_primary_plane(struct drm_crtc *crtc,
+				    struct drm_pending_vblank_event *event)
+{
+	unsigned int crtc_w, crtc_h;
+
+	crtc_w = crtc->primary->fb->width - crtc->x;
+	crtc_h = crtc->primary->fb->height - crtc->y;
+
+	return vop_update_plane_event(crtc->primary, crtc, crtc->primary->fb,
+				      0, 0, crtc_w, crtc_h, crtc->x << 16,
+				      crtc->y << 16, crtc_w << 16,
+				      crtc_h << 16, event);
+}
+
+static int vop_disable_plane(struct drm_plane *plane)
+{
+	struct vop_win *vop_win = to_vop_win(plane);
+	const struct vop_win_data *win = vop_win->data;
+	struct vop *vop;
+	int ret;
+
+	if (!plane->crtc)
+		return 0;
+
+	vop = to_vop(plane->crtc);
+
+	ret = drm_vblank_get(plane->dev, vop->pipe);
+	if (ret) {
+		DRM_ERROR("failed to get vblank, %d\n", ret);
+		return ret;
+	}
+
+	mutex_lock(&vop->vsync_mutex);
+
+	ret = vop_win_queue_fb(vop_win, NULL, 0, NULL);
+	if (ret) {
+		drm_vblank_put(plane->dev, vop->pipe);
+		mutex_unlock(&vop->vsync_mutex);
+		return ret;
+	}
+
+	vop->vsync_work_pending = true;
+	mutex_unlock(&vop->vsync_mutex);
+
+	spin_lock(&vop->reg_lock);
+	VOP_WIN_SET(vop, win, enable, 0);
+	vop_cfg_done(vop);
+	spin_unlock(&vop->reg_lock);
+
+	return 0;
+}
+
+static void vop_plane_destroy(struct drm_plane *plane)
+{
+	vop_disable_plane(plane);
+	drm_plane_cleanup(plane);
+}
+
+static const struct drm_plane_funcs vop_plane_funcs = {
+	.update_plane = vop_update_plane,
+	.disable_plane = vop_disable_plane,
+	.destroy = vop_plane_destroy,
+};
+
+int rockchip_drm_crtc_mode_config(struct drm_crtc *crtc,
+				  int connector_type,
+				  int out_mode)
+{
+	struct vop *vop = to_vop(crtc);
+
+	vop->connector_type = connector_type;
+	vop->connector_out_mode = out_mode;
+
+	return 0;
+}
+
+static int vop_crtc_enable_vblank(struct drm_crtc *crtc)
+{
+	struct vop *vop = to_vop(crtc);
+	unsigned long flags;
+
+	if (vop->dpms != DRM_MODE_DPMS_ON)
+		return -EPERM;
+
+	spin_lock_irqsave(&vop->irq_lock, flags);
+
+	vop_mask_write(vop, INTR_CTRL0, FS_INTR_MASK, FS_INTR_EN(1));
+
+	spin_unlock_irqrestore(&vop->irq_lock, flags);
+
+	return 0;
+}
+
+static void vop_crtc_disable_vblank(struct drm_crtc *crtc)
+{
+	struct vop *vop = to_vop(crtc);
+	unsigned long flags;
+
+	if (vop->dpms != DRM_MODE_DPMS_ON)
+		return;
+	spin_lock_irqsave(&vop->irq_lock, flags);
+	vop_mask_write(vop, INTR_CTRL0, FS_INTR_MASK, FS_INTR_EN(0));
+	spin_unlock_irqrestore(&vop->irq_lock, flags);
+}
+
+static const struct rockchip_crtc_funcs private_crtc_funcs = {
+	.enable_vblank = vop_crtc_enable_vblank,
+	.disable_vblank = vop_crtc_disable_vblank,
+};
+
+static void vop_crtc_dpms(struct drm_crtc *crtc, int mode)
+{
+	struct vop *vop = to_vop(crtc);
+
+	DRM_DEBUG_KMS("crtc[%d] mode[%d]\n", crtc->base.id, mode);
+
+	if (vop->dpms == mode) {
+		DRM_DEBUG_KMS("desired dpms mode is same as previous one.\n");
+		return;
+	}
+
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+		vop_enable(crtc);
+		break;
+	case DRM_MODE_DPMS_STANDBY:
+	case DRM_MODE_DPMS_SUSPEND:
+	case DRM_MODE_DPMS_OFF:
+		vop_disable(crtc);
+		break;
+	default:
+		DRM_DEBUG_KMS("unspecified mode %d\n", mode);
+		break;
+	}
+
+	vop->dpms = mode;
+}
+
+static void vop_crtc_prepare(struct drm_crtc *crtc)
+{
+	vop_crtc_dpms(crtc, DRM_MODE_DPMS_ON);
+}
+
+static bool vop_crtc_mode_fixup(struct drm_crtc *crtc,
+				const struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode)
+{
+	if (adjusted_mode->htotal == 0 || adjusted_mode->vtotal == 0)
+		return false;
+
+	return true;
+}
+
+static int vop_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
+				  struct drm_framebuffer *old_fb)
+{
+	int ret;
+
+	crtc->x = x;
+	crtc->y = y;
+
+	ret = vop_update_primary_plane(crtc, NULL);
+	if (ret < 0) {
+		DRM_ERROR("fail to update plane\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int vop_crtc_mode_set(struct drm_crtc *crtc,
+			     struct drm_display_mode *mode,
+			     struct drm_display_mode *adjusted_mode,
+			     int x, int y, struct drm_framebuffer *fb)
+{
+	struct vop *vop = to_vop(crtc);
+	u16 hsync_len = adjusted_mode->hsync_end - adjusted_mode->hsync_start;
+	u16 hdisplay = adjusted_mode->hdisplay;
+	u16 htotal = adjusted_mode->htotal;
+	u16 hact_st = adjusted_mode->htotal - adjusted_mode->hsync_start;
+	u16 hact_end = hact_st + hdisplay;
+	u16 vdisplay = adjusted_mode->vdisplay;
+	u16 vtotal = adjusted_mode->vtotal;
+	u16 vsync_len = adjusted_mode->vsync_end - adjusted_mode->vsync_start;
+	u16 vact_st = adjusted_mode->vtotal - adjusted_mode->vsync_start;
+	u16 vact_end = vact_st + vdisplay;
+	int ret;
+	uint32_t val;
+
+	/*
+	 * disable dclk to stop frame scan, so that we can safe config mode and
+	 * enable iommu.
+	 */
+	clk_disable(vop->dclk);
+
+	switch (vop->connector_type) {
+	case DRM_MODE_CONNECTOR_LVDS:
+		VOP_CTRL_SET(vop, rgb_en, 1);
+		break;
+	case DRM_MODE_CONNECTOR_eDP:
+		VOP_CTRL_SET(vop, edp_en, 1);
+		break;
+	case DRM_MODE_CONNECTOR_HDMIA:
+		VOP_CTRL_SET(vop, hdmi_en, 1);
+		break;
+	default:
+		DRM_ERROR("unsupport connector_type[%d]\n",
+			  vop->connector_type);
+		return -EINVAL;
+	};
+	VOP_CTRL_SET(vop, out_mode, vop->connector_out_mode);
+
+	val = 0x8;
+	val |= (adjusted_mode->flags & DRM_MODE_FLAG_NHSYNC) ? 1 : 0;
+	val |= (adjusted_mode->flags & DRM_MODE_FLAG_NVSYNC) ? (1 << 1) : 0;
+	VOP_CTRL_SET(vop, pin_pol, val);
+
+	VOP_CTRL_SET(vop, htotal_pw, (htotal << 16) | hsync_len);
+	val = hact_st << 16;
+	val |= hact_end;
+	VOP_CTRL_SET(vop, hact_st_end, val);
+	VOP_CTRL_SET(vop, hpost_st_end, val);
+
+	VOP_CTRL_SET(vop, vtotal_pw, (vtotal << 16) | vsync_len);
+	val = vact_st << 16;
+	val |= vact_end;
+	VOP_CTRL_SET(vop, vact_st_end, val);
+	VOP_CTRL_SET(vop, vpost_st_end, val);
+
+	ret = vop_crtc_mode_set_base(crtc, x, y, fb);
+	if (ret)
+		return ret;
+
+	/*
+	 * reset dclk, take all mode config affect, so the clk would run in
+	 * correct frame.
+	 */
+	reset_control_assert(vop->dclk_rst);
+	usleep_range(10, 20);
+	reset_control_deassert(vop->dclk_rst);
+
+	clk_set_rate(vop->dclk, adjusted_mode->clock * 1000);
+	ret = clk_enable(vop->dclk);
+	if (ret < 0) {
+		dev_err(vop->dev, "failed to enable dclk - %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void vop_crtc_commit(struct drm_crtc *crtc)
+{
+}
+
+static const struct drm_crtc_helper_funcs vop_crtc_helper_funcs = {
+	.dpms = vop_crtc_dpms,
+	.prepare = vop_crtc_prepare,
+	.mode_fixup = vop_crtc_mode_fixup,
+	.mode_set = vop_crtc_mode_set,
+	.mode_set_base = vop_crtc_mode_set_base,
+	.commit = vop_crtc_commit,
+};
+
+static int vop_crtc_page_flip(struct drm_crtc *crtc,
+			      struct drm_framebuffer *fb,
+			      struct drm_pending_vblank_event *event,
+			      uint32_t page_flip_flags)
+{
+	struct vop *vop = to_vop(crtc);
+	struct drm_framebuffer *old_fb = crtc->primary->fb;
+	int ret;
+
+	/* when the page flip is requested, crtc's dpms should be on */
+	if (vop->dpms > DRM_MODE_DPMS_ON) {
+		DRM_DEBUG("failed page flip request at dpms[%d].\n", vop->dpms);
+		return 0;
+	}
+
+	crtc->primary->fb = fb;
+
+	ret = vop_update_primary_plane(crtc, event);
+	if (ret)
+		crtc->primary->fb = old_fb;
+
+	return ret;
+}
+
+static void vop_win_state_complete(struct vop_win *vop_win,
+				   struct vop_win_state *state)
+{
+	struct vop *vop = vop_win->vop;
+	struct drm_crtc *crtc = &vop->crtc;
+	struct drm_device *drm = crtc->dev;
+	unsigned long flags;
+
+	if (state->event) {
+		spin_lock_irqsave(&drm->event_lock, flags);
+		drm_send_vblank_event(drm, -1, state->event);
+		spin_unlock_irqrestore(&drm->event_lock, flags);
+	}
+
+	list_del(&state->head);
+	drm_vblank_put(crtc->dev, vop->pipe);
+}
+
+static void vop_crtc_destroy(struct drm_crtc *crtc)
+{
+	drm_crtc_cleanup(crtc);
+}
+
+static const struct drm_crtc_funcs vop_crtc_funcs = {
+	.set_config = drm_crtc_helper_set_config,
+	.page_flip = vop_crtc_page_flip,
+	.destroy = vop_crtc_destroy,
+};
+
+static bool vop_win_state_is_active(struct vop_win *vop_win,
+				    struct vop_win_state *state)
+{
+	bool active = false;
+
+	if (state->fb) {
+		dma_addr_t yrgb_mst;
+
+		/* check yrgb_mst to tell if pending_fb is now front */
+		yrgb_mst = VOP_WIN_GET_YRGBADDR(vop_win->vop, vop_win->data);
+
+		active = (yrgb_mst == state->yrgb_mst);
+	} else {
+		bool enabled;
+
+		/* if enable bit is clear, plane is now disabled */
+		enabled = VOP_WIN_GET(vop_win->vop, vop_win->data, enable);
+
+		active = (enabled == 0);
+	}
+
+	return active;
+}
+
+static void vop_win_state_destroy(struct vop_win_state *state)
+{
+	struct drm_framebuffer *fb = state->fb;
+
+	if (fb)
+		drm_framebuffer_unreference(fb);
+
+	kfree(state);
+}
+
+static void vop_win_update_state(struct vop_win *vop_win)
+{
+	struct vop_win_state *state, *n, *new_active = NULL;
+
+	/* Check if any pending states are now active */
+	list_for_each_entry(state, &vop_win->pending, head)
+		if (vop_win_state_is_active(vop_win, state)) {
+			new_active = state;
+			break;
+		}
+
+	if (!new_active)
+		return;
+
+	/*
+	 * Destroy any 'skipped' pending states - states that were queued
+	 * before the newly active state.
+	 */
+	list_for_each_entry_safe(state, n, &vop_win->pending, head) {
+		if (state == new_active)
+			break;
+		vop_win_state_complete(vop_win, state);
+		vop_win_state_destroy(state);
+	}
+
+	vop_win_state_complete(vop_win, new_active);
+
+	if (vop_win->active)
+		vop_win_state_destroy(vop_win->active);
+	vop_win->active = new_active;
+}
+
+static bool vop_win_has_pending_state(struct vop_win *vop_win)
+{
+	return !list_empty(&vop_win->pending);
+}
+
+static irqreturn_t vop_isr_thread(int irq, void *data)
+{
+	struct vop *vop = data;
+	const struct vop_data *vop_data = vop->data;
+	unsigned int i;
+
+	mutex_lock(&vop->vsync_mutex);
+
+	if (!vop->vsync_work_pending)
+		goto done;
+
+	vop->vsync_work_pending = false;
+
+	for (i = 0; i < vop_data->win_size; i++) {
+		struct vop_win *vop_win = &vop->win[i];
+
+		vop_win_update_state(vop_win);
+		if (vop_win_has_pending_state(vop_win))
+			vop->vsync_work_pending = true;
+	}
+
+done:
+	mutex_unlock(&vop->vsync_mutex);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t vop_isr(int irq, void *data)
+{
+	struct vop *vop = data;
+	uint32_t intr0_reg, active_irqs;
+	unsigned long flags;
+
+	/*
+	 * INTR_CTRL0 register has interrupt status, enable and clear bits, we
+	 * must hold irq_lock to avoid a race with enable/disable_vblank().
+	*/
+	spin_lock_irqsave(&vop->irq_lock, flags);
+	intr0_reg = vop_readl(vop, INTR_CTRL0);
+	active_irqs = intr0_reg & INTR_MASK;
+	/* Clear all active interrupt sources */
+	if (active_irqs)
+		vop_writel(vop, INTR_CTRL0,
+			   intr0_reg | (active_irqs << INTR_CLR_SHIFT));
+	spin_unlock_irqrestore(&vop->irq_lock, flags);
+
+	/* This is expected for vop iommu irqs, since the irq is shared */
+	if (!active_irqs)
+		return IRQ_NONE;
+
+	/* Only Frame Start Interrupt is enabled; other irqs are spurious. */
+	if (!(active_irqs & FS_INTR)) {
+		DRM_ERROR("Unknown VOP IRQs: %#02x\n", active_irqs);
+		return IRQ_NONE;
+	}
+
+	drm_handle_vblank(vop->drm_dev, vop->pipe);
+
+	return (vop->vsync_work_pending) ? IRQ_WAKE_THREAD : IRQ_HANDLED;
+}
+
+static int vop_create_crtc(struct vop *vop)
+{
+	const struct vop_data *vop_data = vop->data;
+	struct device *dev = vop->dev;
+	struct drm_device *drm_dev = vop->drm_dev;
+	struct drm_plane *primary = NULL, *cursor = NULL, *plane;
+	struct drm_crtc *crtc = &vop->crtc;
+	struct device_node *port;
+	int ret;
+	int i;
+
+	/*
+	 * Create drm_plane for primary and cursor planes first, since we need
+	 * to pass them to drm_crtc_init_with_planes, which sets the
+	 * "possible_crtcs" to the newly initialized crtc.
+	 */
+	for (i = 0; i < vop_data->win_size; i++) {
+		struct vop_win *vop_win = &vop->win[i];
+		const struct vop_win_data *win_data = vop_win->data;
+
+		if (win_data->type != DRM_PLANE_TYPE_PRIMARY &&
+		    win_data->type != DRM_PLANE_TYPE_CURSOR)
+			continue;
+
+		ret = drm_universal_plane_init(vop->drm_dev, &vop_win->base,
+					       0, &vop_plane_funcs,
+					       win_data->phy->data_formats,
+					       win_data->phy->nformats,
+					       win_data->type);
+		if (ret) {
+			DRM_ERROR("failed to initialize plane\n");
+			goto err_cleanup_planes;
+		}
+
+		plane = &vop_win->base;
+		if (plane->type == DRM_PLANE_TYPE_PRIMARY)
+			primary = plane;
+		else if (plane->type == DRM_PLANE_TYPE_CURSOR)
+			cursor = plane;
+	}
+
+	ret = drm_crtc_init_with_planes(drm_dev, crtc, primary, cursor,
+					&vop_crtc_funcs);
+	if (ret)
+		return ret;
+
+	drm_crtc_helper_add(crtc, &vop_crtc_helper_funcs);
+
+	/*
+	 * Create drm_planes for overlay windows with possible_crtcs restricted
+	 * to the newly created crtc.
+	 */
+	for (i = 0; i < vop_data->win_size; i++) {
+		struct vop_win *vop_win = &vop->win[i];
+		const struct vop_win_data *win_data = vop_win->data;
+		unsigned long possible_crtcs = 1 << drm_crtc_index(crtc);
+
+		if (win_data->type != DRM_PLANE_TYPE_OVERLAY)
+			continue;
+
+		ret = drm_universal_plane_init(vop->drm_dev, &vop_win->base,
+					       possible_crtcs,
+					       &vop_plane_funcs,
+					       win_data->phy->data_formats,
+					       win_data->phy->nformats,
+					       win_data->type);
+		if (ret) {
+			DRM_ERROR("failed to initialize overlay plane\n");
+			goto err_cleanup_crtc;
+		}
+	}
+
+	port = of_get_child_by_name(dev->of_node, "port");
+	if (!port) {
+		DRM_ERROR("no port node found in %s\n",
+			  dev->of_node->full_name);
+		goto err_cleanup_crtc;
+	}
+
+	crtc->port = port;
+	vop->pipe = drm_crtc_index(crtc);
+	rockchip_register_crtc_funcs(drm_dev, &private_crtc_funcs, vop->pipe);
+
+	return 0;
+
+err_cleanup_crtc:
+	drm_crtc_cleanup(crtc);
+err_cleanup_planes:
+	list_for_each_entry(plane, &drm_dev->mode_config.plane_list, head)
+		drm_plane_cleanup(plane);
+	return ret;
+}
+
+static void vop_destroy_crtc(struct vop *vop)
+{
+	struct drm_crtc *crtc = &vop->crtc;
+
+	rockchip_unregister_crtc_funcs(vop->drm_dev, vop->pipe);
+	of_node_put(crtc->port);
+	drm_crtc_cleanup(crtc);
+}
+
+static int vop_initial(struct vop *vop)
+{
+	const struct vop_data *vop_data = vop->data;
+	const struct vop_reg_data *init_table = vop_data->init_table;
+	struct reset_control *ahb_rst;
+	int i, ret;
+
+	vop->hclk = devm_clk_get(vop->dev, "hclk_vop");
+	if (IS_ERR(vop->hclk)) {
+		dev_err(vop->dev, "failed to get hclk source\n");
+		return PTR_ERR(vop->hclk);
+	}
+	vop->aclk = devm_clk_get(vop->dev, "aclk_vop");
+	if (IS_ERR(vop->aclk)) {
+		dev_err(vop->dev, "failed to get aclk source\n");
+		return PTR_ERR(vop->aclk);
+	}
+	vop->dclk = devm_clk_get(vop->dev, "dclk_vop");
+	if (IS_ERR(vop->dclk)) {
+		dev_err(vop->dev, "failed to get dclk source\n");
+		return PTR_ERR(vop->dclk);
+	}
+
+	ret = clk_prepare(vop->hclk);
+	if (ret < 0) {
+		dev_err(vop->dev, "failed to prepare hclk\n");
+		return ret;
+	}
+
+	ret = clk_prepare(vop->dclk);
+	if (ret < 0) {
+		dev_err(vop->dev, "failed to prepare dclk\n");
+		goto err_unprepare_hclk;
+	}
+
+	ret = clk_prepare(vop->aclk);
+	if (ret < 0) {
+		dev_err(vop->dev, "failed to prepare aclk\n");
+		goto err_unprepare_dclk;
+	}
+
+	/*
+	 * enable hclk, so that we can config vop register.
+	 */
+	ret = clk_enable(vop->hclk);
+	if (ret < 0) {
+		dev_err(vop->dev, "failed to prepare aclk\n");
+		goto err_unprepare_aclk;
+	}
+	/*
+	 * do hclk_reset, reset all vop registers.
+	 */
+	ahb_rst = devm_reset_control_get(vop->dev, "ahb");
+	if (IS_ERR(ahb_rst)) {
+		dev_err(vop->dev, "failed to get ahb reset\n");
+		ret = PTR_ERR(ahb_rst);
+		goto err_disable_hclk;
+	}
+	reset_control_assert(ahb_rst);
+	usleep_range(10, 20);
+	reset_control_deassert(ahb_rst);
+
+	memcpy(vop->regsbak, vop->regs, vop->len);
+
+	for (i = 0; i < vop_data->table_size; i++)
+		vop_writel(vop, init_table[i].offset, init_table[i].value);
+
+	for (i = 0; i < vop_data->win_size; i++) {
+		const struct vop_win_data *win = &vop_data->win[i];
+
+		VOP_WIN_SET(vop, win, enable, 0);
+	}
+
+	vop_cfg_done(vop);
+
+	/*
+	 * do dclk_reset, let all config take affect.
+	 */
+	vop->dclk_rst = devm_reset_control_get(vop->dev, "dclk");
+	if (IS_ERR(vop->dclk_rst)) {
+		dev_err(vop->dev, "failed to get dclk reset\n");
+		ret = PTR_ERR(vop->dclk_rst);
+		goto err_unprepare_aclk;
+	}
+	reset_control_assert(vop->dclk_rst);
+	usleep_range(10, 20);
+	reset_control_deassert(vop->dclk_rst);
+
+	clk_disable(vop->hclk);
+
+	vop->dpms = DRM_MODE_DPMS_OFF;
+
+	return 0;
+
+err_disable_hclk:
+	clk_disable(vop->hclk);
+err_unprepare_aclk:
+	clk_unprepare(vop->aclk);
+err_unprepare_dclk:
+	clk_unprepare(vop->dclk);
+err_unprepare_hclk:
+	clk_unprepare(vop->hclk);
+	return ret;
+}
+
+/*
+ * Initialize the vop->win array elements.
+ */
+static void vop_win_init(struct vop *vop)
+{
+	const struct vop_data *vop_data = vop->data;
+	unsigned int i;
+
+	for (i = 0; i < vop_data->win_size; i++) {
+		struct vop_win *vop_win = &vop->win[i];
+		const struct vop_win_data *win_data = &vop_data->win[i];
+
+		vop_win->data = win_data;
+		vop_win->vop = vop;
+		INIT_LIST_HEAD(&vop_win->pending);
+	}
+}
+
+static int vop_bind(struct device *dev, struct device *master, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	const struct of_device_id *of_id;
+	const struct vop_data *vop_data;
+	struct drm_device *drm_dev = data;
+	struct vop *vop;
+	struct resource *res;
+	size_t alloc_size;
+	int ret;
+
+	of_id = of_match_device(vop_driver_dt_match, dev);
+	vop_data = of_id->data;
+	if (!vop_data)
+		return -ENODEV;
+
+	/* Allocate vop struct and its vop_win array */
+	alloc_size = sizeof(*vop) + sizeof(*vop->win) * vop_data->win_size;
+	vop = devm_kzalloc(dev, alloc_size, GFP_KERNEL);
+	if (!vop)
+		return -ENOMEM;
+
+	vop->dev = dev;
+	vop->data = vop_data;
+	vop->drm_dev = drm_dev;
+	dev_set_drvdata(dev, vop);
+
+	vop_win_init(vop);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	vop->len = resource_size(res);
+	vop->regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(vop->regs))
+		return PTR_ERR(vop->regs);
+
+	vop->regsbak = devm_kzalloc(dev, vop->len, GFP_KERNEL);
+	if (!vop->regsbak)
+		return -ENOMEM;
+
+	ret = vop_initial(vop);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "cannot initial vop dev - err %d\n", ret);
+		return ret;
+	}
+
+	vop->irq = platform_get_irq(pdev, 0);
+	if (vop->irq < 0) {
+		dev_err(dev, "cannot find irq for vop\n");
+		return vop->irq;
+	}
+
+	spin_lock_init(&vop->reg_lock);
+	spin_lock_init(&vop->irq_lock);
+
+	mutex_init(&vop->vsync_mutex);
+
+	ret = devm_request_threaded_irq(dev, vop->irq, vop_isr, vop_isr_thread,
+					IRQF_SHARED, dev_name(dev), vop);
+	if (ret)
+		return ret;
+
+	/* IRQ is initially disabled; it gets enabled in power_on */
+	disable_irq(vop->irq);
+
+	ret = vop_create_crtc(vop);
+	if (ret)
+		return ret;
+
+	pm_runtime_enable(&pdev->dev);
+	return 0;
+}
+
+static void vop_unbind(struct device *dev, struct device *master, void *data)
+{
+	struct vop *vop = dev_get_drvdata(dev);
+
+	pm_runtime_disable(dev);
+	vop_destroy_crtc(vop);
+}
+
+static const struct component_ops vop_component_ops = {
+	.bind = vop_bind,
+	.unbind = vop_unbind,
+};
+
+static int vop_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+
+	if (!dev->of_node) {
+		dev_err(dev, "can't find vop devices\n");
+		return -ENODEV;
+	}
+
+	return component_add(dev, &vop_component_ops);
+}
+
+static int vop_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &vop_component_ops);
+
+	return 0;
+}
+
+struct platform_driver vop_platform_driver = {
+	.probe = vop_probe,
+	.remove = vop_remove,
+	.driver = {
+		.name = "rockchip-vop",
+		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(vop_driver_dt_match),
+	},
+};
+
+module_platform_driver(vop_platform_driver);
+
+MODULE_AUTHOR("Mark Yao <mark.yao@rock-chips.com>");
+MODULE_DESCRIPTION("ROCKCHIP VOP Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_vop.h b/drivers/gpu/drm/rockchip/rockchip_drm_vop.h
new file mode 100644
index 000000000000..63e9b3a084c5
--- /dev/null
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_vop.h
@@ -0,0 +1,201 @@
+/*
+ * Copyright (C) Fuzhou Rockchip Electronics Co.Ltd
+ * Author:Mark Yao <mark.yao@rock-chips.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 _ROCKCHIP_DRM_VOP_H
+#define _ROCKCHIP_DRM_VOP_H
+
+/* register definition */
+#define REG_CFG_DONE			0x0000
+#define VERSION_INFO			0x0004
+#define SYS_CTRL			0x0008
+#define SYS_CTRL1			0x000c
+#define DSP_CTRL0			0x0010
+#define DSP_CTRL1			0x0014
+#define DSP_BG				0x0018
+#define MCU_CTRL			0x001c
+#define INTR_CTRL0			0x0020
+#define INTR_CTRL1			0x0024
+#define WIN0_CTRL0			0x0030
+#define WIN0_CTRL1			0x0034
+#define WIN0_COLOR_KEY			0x0038
+#define WIN0_VIR			0x003c
+#define WIN0_YRGB_MST			0x0040
+#define WIN0_CBR_MST			0x0044
+#define WIN0_ACT_INFO			0x0048
+#define WIN0_DSP_INFO			0x004c
+#define WIN0_DSP_ST			0x0050
+#define WIN0_SCL_FACTOR_YRGB		0x0054
+#define WIN0_SCL_FACTOR_CBR		0x0058
+#define WIN0_SCL_OFFSET			0x005c
+#define WIN0_SRC_ALPHA_CTRL		0x0060
+#define WIN0_DST_ALPHA_CTRL		0x0064
+#define WIN0_FADING_CTRL		0x0068
+/* win1 register */
+#define WIN1_CTRL0			0x0070
+#define WIN1_CTRL1			0x0074
+#define WIN1_COLOR_KEY			0x0078
+#define WIN1_VIR			0x007c
+#define WIN1_YRGB_MST			0x0080
+#define WIN1_CBR_MST			0x0084
+#define WIN1_ACT_INFO			0x0088
+#define WIN1_DSP_INFO			0x008c
+#define WIN1_DSP_ST			0x0090
+#define WIN1_SCL_FACTOR_YRGB		0x0094
+#define WIN1_SCL_FACTOR_CBR		0x0098
+#define WIN1_SCL_OFFSET			0x009c
+#define WIN1_SRC_ALPHA_CTRL		0x00a0
+#define WIN1_DST_ALPHA_CTRL		0x00a4
+#define WIN1_FADING_CTRL		0x00a8
+/* win2 register */
+#define WIN2_CTRL0			0x00b0
+#define WIN2_CTRL1			0x00b4
+#define WIN2_VIR0_1			0x00b8
+#define WIN2_VIR2_3			0x00bc
+#define WIN2_MST0			0x00c0
+#define WIN2_DSP_INFO0			0x00c4
+#define WIN2_DSP_ST0			0x00c8
+#define WIN2_COLOR_KEY			0x00cc
+#define WIN2_MST1			0x00d0
+#define WIN2_DSP_INFO1			0x00d4
+#define WIN2_DSP_ST1			0x00d8
+#define WIN2_SRC_ALPHA_CTRL		0x00dc
+#define WIN2_MST2			0x00e0
+#define WIN2_DSP_INFO2			0x00e4
+#define WIN2_DSP_ST2			0x00e8
+#define WIN2_DST_ALPHA_CTRL		0x00ec
+#define WIN2_MST3			0x00f0
+#define WIN2_DSP_INFO3			0x00f4
+#define WIN2_DSP_ST3			0x00f8
+#define WIN2_FADING_CTRL		0x00fc
+/* win3 register */
+#define WIN3_CTRL0			0x0100
+#define WIN3_CTRL1			0x0104
+#define WIN3_VIR0_1			0x0108
+#define WIN3_VIR2_3			0x010c
+#define WIN3_MST0			0x0110
+#define WIN3_DSP_INFO0			0x0114
+#define WIN3_DSP_ST0			0x0118
+#define WIN3_COLOR_KEY			0x011c
+#define WIN3_MST1			0x0120
+#define WIN3_DSP_INFO1			0x0124
+#define WIN3_DSP_ST1			0x0128
+#define WIN3_SRC_ALPHA_CTRL		0x012c
+#define WIN3_MST2			0x0130
+#define WIN3_DSP_INFO2			0x0134
+#define WIN3_DSP_ST2			0x0138
+#define WIN3_DST_ALPHA_CTRL		0x013c
+#define WIN3_MST3			0x0140
+#define WIN3_DSP_INFO3			0x0144
+#define WIN3_DSP_ST3			0x0148
+#define WIN3_FADING_CTRL		0x014c
+/* hwc register */
+#define HWC_CTRL0			0x0150
+#define HWC_CTRL1			0x0154
+#define HWC_MST				0x0158
+#define HWC_DSP_ST			0x015c
+#define HWC_SRC_ALPHA_CTRL		0x0160
+#define HWC_DST_ALPHA_CTRL		0x0164
+#define HWC_FADING_CTRL			0x0168
+/* post process register */
+#define POST_DSP_HACT_INFO		0x0170
+#define POST_DSP_VACT_INFO		0x0174
+#define POST_SCL_FACTOR_YRGB		0x0178
+#define POST_SCL_CTRL			0x0180
+#define POST_DSP_VACT_INFO_F1		0x0184
+#define DSP_HTOTAL_HS_END		0x0188
+#define DSP_HACT_ST_END			0x018c
+#define DSP_VTOTAL_VS_END		0x0190
+#define DSP_VACT_ST_END			0x0194
+#define DSP_VS_ST_END_F1		0x0198
+#define DSP_VACT_ST_END_F1		0x019c
+/* register definition end */
+
+/* interrupt define */
+#define DSP_HOLD_VALID_INTR		(1 << 0)
+#define FS_INTR				(1 << 1)
+#define LINE_FLAG_INTR			(1 << 2)
+#define BUS_ERROR_INTR			(1 << 3)
+
+#define INTR_MASK			(DSP_HOLD_VALID_INTR | FS_INTR | \
+					 LINE_FLAG_INTR | BUS_ERROR_INTR)
+
+#define DSP_HOLD_VALID_INTR_EN(x)	((x) << 4)
+#define FS_INTR_EN(x)			((x) << 5)
+#define LINE_FLAG_INTR_EN(x)		((x) << 6)
+#define BUS_ERROR_INTR_EN(x)		((x) << 7)
+#define DSP_HOLD_VALID_INTR_MASK	(1 << 4)
+#define FS_INTR_MASK			(1 << 5)
+#define LINE_FLAG_INTR_MASK		(1 << 6)
+#define BUS_ERROR_INTR_MASK		(1 << 7)
+
+#define INTR_CLR_SHIFT			8
+#define DSP_HOLD_VALID_INTR_CLR		(1 << (INTR_CLR_SHIFT + 0))
+#define FS_INTR_CLR			(1 << (INTR_CLR_SHIFT + 1))
+#define LINE_FLAG_INTR_CLR		(1 << (INTR_CLR_SHIFT + 2))
+#define BUS_ERROR_INTR_CLR		(1 << (INTR_CLR_SHIFT + 3))
+
+#define DSP_LINE_NUM(x)			(((x) & 0x1fff) << 12)
+#define DSP_LINE_NUM_MASK		(0x1fff << 12)
+
+/* src alpha ctrl define */
+#define SRC_FADING_VALUE(x)		(((x) & 0xff) << 24)
+#define SRC_GLOBAL_ALPHA(x)		(((x) & 0xff) << 16)
+#define SRC_FACTOR_M0(x)		(((x) & 0x7) << 6)
+#define SRC_ALPHA_CAL_M0(x)		(((x) & 0x1) << 5)
+#define SRC_BLEND_M0(x)			(((x) & 0x3) << 3)
+#define SRC_ALPHA_M0(x)			(((x) & 0x1) << 2)
+#define SRC_COLOR_M0(x)			(((x) & 0x1) << 1)
+#define SRC_ALPHA_EN(x)			(((x) & 0x1) << 0)
+/* dst alpha ctrl define */
+#define DST_FACTOR_M0(x)		(((x) & 0x7) << 6)
+
+/*
+ * display output interface supported by rockchip lcdc
+ */
+#define ROCKCHIP_OUT_MODE_P888	0
+#define ROCKCHIP_OUT_MODE_P666	1
+#define ROCKCHIP_OUT_MODE_P565	2
+/* for use special outface */
+#define ROCKCHIP_OUT_MODE_AAAA	15
+
+enum alpha_mode {
+	ALPHA_STRAIGHT,
+	ALPHA_INVERSE,
+};
+
+enum global_blend_mode {
+	ALPHA_GLOBAL,
+	ALPHA_PER_PIX,
+	ALPHA_PER_PIX_GLOBAL,
+};
+
+enum alpha_cal_mode {
+	ALPHA_SATURATION,
+	ALPHA_NO_SATURATION,
+};
+
+enum color_mode {
+	ALPHA_SRC_PRE_MUL,
+	ALPHA_SRC_NO_PRE_MUL,
+};
+
+enum factor_mode {
+	ALPHA_ZERO,
+	ALPHA_ONE,
+	ALPHA_SRC,
+	ALPHA_SRC_INVERSE,
+	ALPHA_SRC_GLOBAL,
+};
+
+#endif /* _ROCKCHIP_DRM_VOP_H */
diff --git a/drivers/gpu/drm/shmobile/shmob_drm_crtc.c b/drivers/gpu/drm/shmobile/shmob_drm_crtc.c
index 0ddce4d046d9..859ccb658601 100644
--- a/drivers/gpu/drm/shmobile/shmob_drm_crtc.c
+++ b/drivers/gpu/drm/shmobile/shmob_drm_crtc.c
@@ -19,6 +19,7 @@
 #include <drm/drm_crtc_helper.h>
 #include <drm/drm_fb_cma_helper.h>
 #include <drm/drm_gem_cma_helper.h>
+#include <drm/drm_plane_helper.h>
 
 #include <video/sh_mobile_meram.h>
 
diff --git a/drivers/gpu/drm/shmobile/shmob_drm_drv.c b/drivers/gpu/drm/shmobile/shmob_drm_drv.c
index e62cbde81e50..666321de7b99 100644
--- a/drivers/gpu/drm/shmobile/shmob_drm_drv.c
+++ b/drivers/gpu/drm/shmobile/shmob_drm_drv.c
@@ -348,7 +348,6 @@ static struct platform_driver shmob_drm_platform_driver = {
 	.probe		= shmob_drm_probe,
 	.remove		= shmob_drm_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "shmob-drm",
 		.pm	= &shmob_drm_pm_ops,
 	},
diff --git a/drivers/gpu/drm/sti/Kconfig b/drivers/gpu/drm/sti/Kconfig
index ae8850f3e63b..d6d6b705b8c1 100644
--- a/drivers/gpu/drm/sti/Kconfig
+++ b/drivers/gpu/drm/sti/Kconfig
@@ -5,6 +5,7 @@ config DRM_STI
 	select DRM_KMS_HELPER
 	select DRM_GEM_CMA_HELPER
 	select DRM_KMS_CMA_HELPER
+	select FW_LOADER_USER_HELPER_FALLBACK
 	help
 	  Choose this option to enable DRM on STM stiH41x chipset
 
diff --git a/drivers/gpu/drm/sti/Makefile b/drivers/gpu/drm/sti/Makefile
index 04ac2ceef27f..6ba9d27c1b90 100644
--- a/drivers/gpu/drm/sti/Makefile
+++ b/drivers/gpu/drm/sti/Makefile
@@ -3,6 +3,7 @@ sticompositor-y := \
 	sti_mixer.o \
 	sti_gdp.o \
 	sti_vid.o \
+	sti_cursor.o \
 	sti_compositor.o \
 	sti_drm_crtc.o \
 	sti_drm_plane.o
@@ -18,4 +19,5 @@ obj-$(CONFIG_DRM_STI) = \
 	sti_hda.o \
 	sti_tvout.o \
 	sticompositor.o \
-	sti_drm_drv.o
\ No newline at end of file
+	sti_hqvdp.o \
+	sti_drm_drv.o
diff --git a/drivers/gpu/drm/sti/sti_compositor.c b/drivers/gpu/drm/sti/sti_compositor.c
index 390d93e9a06c..43215d3020fb 100644
--- a/drivers/gpu/drm/sti/sti_compositor.c
+++ b/drivers/gpu/drm/sti/sti_compositor.c
@@ -24,14 +24,16 @@
  * stiH407 compositor properties
  */
 struct sti_compositor_data stih407_compositor_data = {
-	.nb_subdev = 6,
+	.nb_subdev = 8,
 	.subdev_desc = {
+			{STI_CURSOR_SUBDEV, (int)STI_CURSOR, 0x000},
 			{STI_GPD_SUBDEV, (int)STI_GDP_0, 0x100},
 			{STI_GPD_SUBDEV, (int)STI_GDP_1, 0x200},
 			{STI_GPD_SUBDEV, (int)STI_GDP_2, 0x300},
 			{STI_GPD_SUBDEV, (int)STI_GDP_3, 0x400},
 			{STI_VID_SUBDEV, (int)STI_VID_0, 0x700},
-			{STI_MIXER_MAIN_SUBDEV, STI_MIXER_MAIN, 0xC00}
+			{STI_MIXER_MAIN_SUBDEV, STI_MIXER_MAIN, 0xC00},
+			{STI_MIXER_AUX_SUBDEV, STI_MIXER_AUX, 0xD00},
 	},
 };
 
@@ -67,11 +69,11 @@ static int sti_compositor_init_subdev(struct sti_compositor *compo,
 			break;
 		case STI_GPD_SUBDEV:
 		case STI_VID_SUBDEV:
+		case STI_CURSOR_SUBDEV:
 			compo->layer[layer_id++] =
 			    sti_layer_create(compo->dev, desc[i].id,
 					     compo->regs + desc[i].offset);
 			break;
-			/* case STI_CURSOR_SUBDEV : TODO */
 		default:
 			DRM_ERROR("Unknow subdev compoment type\n");
 			return 1;
@@ -102,33 +104,35 @@ static int sti_compositor_bind(struct device *dev, struct device *master,
 			enum sti_layer_type type = desc & STI_LAYER_TYPE_MASK;
 			enum drm_plane_type plane_type = DRM_PLANE_TYPE_OVERLAY;
 
-			if (compo->mixer[crtc])
+			if (crtc < compo->nb_mixers)
 				plane_type = DRM_PLANE_TYPE_PRIMARY;
 
 			switch (type) {
 			case STI_CUR:
 				cursor = sti_drm_plane_init(drm_dev,
 						compo->layer[i],
-						(1 << crtc) - 1,
-						DRM_PLANE_TYPE_CURSOR);
+						1, DRM_PLANE_TYPE_CURSOR);
 				break;
 			case STI_GDP:
 			case STI_VID:
 				primary = sti_drm_plane_init(drm_dev,
 						compo->layer[i],
-						(1 << crtc) - 1, plane_type);
+						(1 << compo->nb_mixers) - 1,
+						plane_type);
 				plane++;
 				break;
 			case STI_BCK:
+			case STI_VDP:
 				break;
 			}
 
 			/* The first planes are reserved for primary planes*/
-			if (compo->mixer[crtc]) {
+			if (crtc < compo->nb_mixers && primary) {
 				sti_drm_crtc_init(drm_dev, compo->mixer[crtc],
 						primary, cursor);
 				crtc++;
 				cursor = NULL;
+				primary = NULL;
 			}
 		}
 	}
@@ -267,7 +271,6 @@ static int sti_compositor_remove(struct platform_device *pdev)
 static struct platform_driver sti_compositor_driver = {
 	.driver = {
 		.name = "sti-compositor",
-		.owner = THIS_MODULE,
 		.of_match_table = compositor_of_match,
 	},
 	.probe = sti_compositor_probe,
diff --git a/drivers/gpu/drm/sti/sti_compositor.h b/drivers/gpu/drm/sti/sti_compositor.h
index 3ea19db72e0f..019eb44c62cc 100644
--- a/drivers/gpu/drm/sti/sti_compositor.h
+++ b/drivers/gpu/drm/sti/sti_compositor.h
@@ -64,7 +64,6 @@ struct sti_compositor_data {
  * @layer: array of layers
  * @nb_mixers: number of mixers for this compositor
  * @nb_layers: number of layers (GDP,VID,...) for this compositor
- * @enable: true if compositor is enable else false
  * @vtg_vblank_nb: callback for VTG VSYNC notification
  */
 struct sti_compositor {
@@ -83,7 +82,6 @@ struct sti_compositor {
 	struct sti_layer *layer[STI_MAX_LAYER];
 	int nb_mixers;
 	int nb_layers;
-	bool enable;
 	struct notifier_block vtg_vblank_nb;
 };
 
diff --git a/drivers/gpu/drm/sti/sti_cursor.c b/drivers/gpu/drm/sti/sti_cursor.c
new file mode 100644
index 000000000000..010eaee60bf7
--- /dev/null
+++ b/drivers/gpu/drm/sti/sti_cursor.c
@@ -0,0 +1,242 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2014
+ * Authors: Vincent Abriou <vincent.abriou@st.com>
+ *          Fabien Dessenne <fabien.dessenne@st.com>
+ *          for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+#include <drm/drmP.h>
+
+#include "sti_cursor.h"
+#include "sti_layer.h"
+#include "sti_vtg.h"
+
+/* Registers */
+#define CUR_CTL             0x00
+#define CUR_VPO             0x0C
+#define CUR_PML             0x14
+#define CUR_PMP             0x18
+#define CUR_SIZE            0x1C
+#define CUR_CML             0x20
+#define CUR_AWS             0x28
+#define CUR_AWE             0x2C
+
+#define CUR_CTL_CLUT_UPDATE BIT(1)
+
+#define STI_CURS_MIN_SIZE   1
+#define STI_CURS_MAX_SIZE   128
+
+/*
+ * pixmap dma buffer stucture
+ *
+ * @paddr:  physical address
+ * @size:   buffer size
+ * @base:   virtual address
+ */
+struct dma_pixmap {
+	dma_addr_t paddr;
+	size_t size;
+	void *base;
+};
+
+/**
+ * STI Cursor structure
+ *
+ * @layer:      layer structure
+ * @width:      cursor width
+ * @height:     cursor height
+ * @clut:       color look up table
+ * @clut_paddr: color look up table physical address
+ * @pixmap:     pixmap dma buffer (clut8-format cursor)
+ */
+struct sti_cursor {
+	struct sti_layer layer;
+	unsigned int width;
+	unsigned int height;
+	unsigned short *clut;
+	dma_addr_t clut_paddr;
+	struct dma_pixmap pixmap;
+};
+
+static const uint32_t cursor_supported_formats[] = {
+	DRM_FORMAT_ARGB8888,
+};
+
+#define to_sti_cursor(x) container_of(x, struct sti_cursor, layer)
+
+static const uint32_t *sti_cursor_get_formats(struct sti_layer *layer)
+{
+	return cursor_supported_formats;
+}
+
+static unsigned int sti_cursor_get_nb_formats(struct sti_layer *layer)
+{
+	return ARRAY_SIZE(cursor_supported_formats);
+}
+
+static void sti_cursor_argb8888_to_clut8(struct sti_layer *layer)
+{
+	struct sti_cursor *cursor = to_sti_cursor(layer);
+	u32 *src = layer->vaddr;
+	u8  *dst = cursor->pixmap.base;
+	unsigned int i, j;
+	u32 a, r, g, b;
+
+	for (i = 0; i < cursor->height; i++) {
+		for (j = 0; j < cursor->width; j++) {
+			/* Pick the 2 higher bits of each component */
+			a = (*src >> 30) & 3;
+			r = (*src >> 22) & 3;
+			g = (*src >> 14) & 3;
+			b = (*src >> 6) & 3;
+			*dst = a << 6 | r << 4 | g << 2 | b;
+			src++;
+			dst++;
+		}
+	}
+}
+
+static int sti_cursor_prepare_layer(struct sti_layer *layer, bool first_prepare)
+{
+	struct sti_cursor *cursor = to_sti_cursor(layer);
+	struct drm_display_mode *mode = layer->mode;
+	u32 y, x;
+	u32 val;
+
+	DRM_DEBUG_DRIVER("\n");
+
+	dev_dbg(layer->dev, "%s %s\n", __func__, sti_layer_to_str(layer));
+
+	if (layer->src_w < STI_CURS_MIN_SIZE ||
+	    layer->src_h < STI_CURS_MIN_SIZE ||
+	    layer->src_w > STI_CURS_MAX_SIZE ||
+	    layer->src_h > STI_CURS_MAX_SIZE) {
+		DRM_ERROR("Invalid cursor size (%dx%d)\n",
+				layer->src_w, layer->src_h);
+		return -EINVAL;
+	}
+
+	/* If the cursor size has changed, re-allocated the pixmap */
+	if (!cursor->pixmap.base ||
+	    (cursor->width != layer->src_w) ||
+	    (cursor->height != layer->src_h)) {
+		cursor->width = layer->src_w;
+		cursor->height = layer->src_h;
+
+		if (cursor->pixmap.base)
+			dma_free_writecombine(layer->dev,
+					      cursor->pixmap.size,
+					      cursor->pixmap.base,
+					      cursor->pixmap.paddr);
+
+		cursor->pixmap.size = cursor->width * cursor->height;
+
+		cursor->pixmap.base = dma_alloc_writecombine(layer->dev,
+							cursor->pixmap.size,
+							&cursor->pixmap.paddr,
+							GFP_KERNEL | GFP_DMA);
+		if (!cursor->pixmap.base) {
+			DRM_ERROR("Failed to allocate memory for pixmap\n");
+			return -ENOMEM;
+		}
+	}
+
+	/* Convert ARGB8888 to CLUT8 */
+	sti_cursor_argb8888_to_clut8(layer);
+
+	/* AWS and AWE depend on the mode */
+	y = sti_vtg_get_line_number(*mode, 0);
+	x = sti_vtg_get_pixel_number(*mode, 0);
+	val = y << 16 | x;
+	writel(val, layer->regs + CUR_AWS);
+	y = sti_vtg_get_line_number(*mode, mode->vdisplay - 1);
+	x = sti_vtg_get_pixel_number(*mode, mode->hdisplay - 1);
+	val = y << 16 | x;
+	writel(val, layer->regs + CUR_AWE);
+
+	if (first_prepare) {
+		/* Set and fetch CLUT */
+		writel(cursor->clut_paddr, layer->regs + CUR_CML);
+		writel(CUR_CTL_CLUT_UPDATE, layer->regs + CUR_CTL);
+	}
+
+	return 0;
+}
+
+static int sti_cursor_commit_layer(struct sti_layer *layer)
+{
+	struct sti_cursor *cursor = to_sti_cursor(layer);
+	struct drm_display_mode *mode = layer->mode;
+	u32 ydo, xdo;
+
+	dev_dbg(layer->dev, "%s %s\n", __func__, sti_layer_to_str(layer));
+
+	/* Set memory location, size, and position */
+	writel(cursor->pixmap.paddr, layer->regs + CUR_PML);
+	writel(cursor->width, layer->regs + CUR_PMP);
+	writel(cursor->height << 16 | cursor->width, layer->regs + CUR_SIZE);
+
+	ydo = sti_vtg_get_line_number(*mode, layer->dst_y);
+	xdo = sti_vtg_get_pixel_number(*mode, layer->dst_y);
+	writel((ydo << 16) | xdo, layer->regs + CUR_VPO);
+
+	return 0;
+}
+
+static int sti_cursor_disable_layer(struct sti_layer *layer)
+{
+	return 0;
+}
+
+static void sti_cursor_init(struct sti_layer *layer)
+{
+	struct sti_cursor *cursor = to_sti_cursor(layer);
+	unsigned short *base = cursor->clut;
+	unsigned int a, r, g, b;
+
+	/* Assign CLUT values, ARGB444 format */
+	for (a = 0; a < 4; a++)
+		for (r = 0; r < 4; r++)
+			for (g = 0; g < 4; g++)
+				for (b = 0; b < 4; b++)
+					*base++ = (a * 5) << 12 |
+						  (r * 5) << 8 |
+						  (g * 5) << 4 |
+						  (b * 5);
+}
+
+static const struct sti_layer_funcs cursor_ops = {
+	.get_formats = sti_cursor_get_formats,
+	.get_nb_formats = sti_cursor_get_nb_formats,
+	.init = sti_cursor_init,
+	.prepare = sti_cursor_prepare_layer,
+	.commit = sti_cursor_commit_layer,
+	.disable = sti_cursor_disable_layer,
+};
+
+struct sti_layer *sti_cursor_create(struct device *dev)
+{
+	struct sti_cursor *cursor;
+
+	cursor = devm_kzalloc(dev, sizeof(*cursor), GFP_KERNEL);
+	if (!cursor) {
+		DRM_ERROR("Failed to allocate memory for cursor\n");
+		return NULL;
+	}
+
+	/* Allocate clut buffer */
+	cursor->clut = dma_alloc_writecombine(dev,
+			0x100 * sizeof(unsigned short),
+			&cursor->clut_paddr,
+			GFP_KERNEL | GFP_DMA);
+
+	if (!cursor->clut) {
+		DRM_ERROR("Failed to allocate memory for cursor clut\n");
+		devm_kfree(dev, cursor);
+		return NULL;
+	}
+
+	cursor->layer.ops = &cursor_ops;
+
+	return (struct sti_layer *)cursor;
+}
diff --git a/drivers/gpu/drm/sti/sti_cursor.h b/drivers/gpu/drm/sti/sti_cursor.h
new file mode 100644
index 000000000000..3c9827404f27
--- /dev/null
+++ b/drivers/gpu/drm/sti/sti_cursor.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2013
+ * Authors: Vincent Abriou <vincent.abriou@st.com> for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#ifndef _STI_CURSOR_H_
+#define _STI_CURSOR_H_
+
+struct sti_layer *sti_cursor_create(struct device *dev);
+
+#endif
diff --git a/drivers/gpu/drm/sti/sti_drm_crtc.c b/drivers/gpu/drm/sti/sti_drm_crtc.c
index d2ae0c0e13be..4c651c200f20 100644
--- a/drivers/gpu/drm/sti/sti_drm_crtc.c
+++ b/drivers/gpu/drm/sti/sti_drm_crtc.c
@@ -10,6 +10,7 @@
 
 #include <drm/drmP.h>
 #include <drm/drm_crtc_helper.h>
+#include <drm/drm_plane_helper.h>
 
 #include "sti_compositor.h"
 #include "sti_drm_drv.h"
@@ -27,7 +28,7 @@ static void sti_drm_crtc_prepare(struct drm_crtc *crtc)
 	struct device *dev = mixer->dev;
 	struct sti_compositor *compo = dev_get_drvdata(dev);
 
-	compo->enable = true;
+	mixer->enabled = true;
 
 	/* Prepare and enable the compo IP clock */
 	if (mixer->id == STI_MIXER_MAIN) {
@@ -37,6 +38,8 @@ static void sti_drm_crtc_prepare(struct drm_crtc *crtc)
 		if (clk_prepare_enable(compo->clk_compo_aux))
 			DRM_INFO("Failed to prepare/enable compo_aux clk\n");
 	}
+
+	sti_mixer_clear_all_layers(mixer);
 }
 
 static void sti_drm_crtc_commit(struct drm_crtc *crtc)
@@ -61,6 +64,8 @@ static void sti_drm_crtc_commit(struct drm_crtc *crtc)
 	/* Enable layer on mixer */
 	if (sti_mixer_set_layer_status(mixer, layer, true))
 		DRM_ERROR("Can not enable layer at mixer\n");
+
+	drm_crtc_vblank_on(crtc);
 }
 
 static bool sti_drm_crtc_mode_fixup(struct drm_crtc *crtc,
@@ -143,7 +148,8 @@ sti_drm_crtc_mode_set(struct drm_crtc *crtc, struct drm_display_mode *mode,
 	w = crtc->primary->fb->width - x;
 	h = crtc->primary->fb->height - y;
 
-	return sti_layer_prepare(layer, crtc->primary->fb, &crtc->mode,
+	return sti_layer_prepare(layer, crtc,
+			crtc->primary->fb, &crtc->mode,
 			mixer->id, 0, 0, w, h, x, y, w, h);
 }
 
@@ -170,7 +176,8 @@ static int sti_drm_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
 	w = crtc->primary->fb->width - crtc->x;
 	h = crtc->primary->fb->height - crtc->y;
 
-	ret = sti_layer_prepare(layer, crtc->primary->fb, &crtc->mode,
+	ret = sti_layer_prepare(layer, crtc,
+				crtc->primary->fb, &crtc->mode,
 				mixer->id, 0, 0, w, h,
 				crtc->x, crtc->y, w, h);
 	if (ret) {
@@ -195,7 +202,7 @@ static void sti_drm_crtc_disable(struct drm_crtc *crtc)
 	struct sti_compositor *compo = dev_get_drvdata(dev);
 	struct sti_layer *layer;
 
-	if (!compo->enable)
+	if (!mixer->enabled)
 		return;
 
 	DRM_DEBUG_KMS("CRTC:%d (%s)\n", crtc->base.id, sti_mixer_to_str(mixer));
@@ -221,7 +228,7 @@ static void sti_drm_crtc_disable(struct drm_crtc *crtc)
 	/* Then disable layer itself */
 	sti_layer_disable(layer);
 
-	drm_vblank_off(crtc->dev, mixer->id);
+	drm_crtc_vblank_off(crtc);
 
 	/* Disable pixel clock and compo IP clocks */
 	if (mixer->id == STI_MIXER_MAIN) {
@@ -232,7 +239,7 @@ static void sti_drm_crtc_disable(struct drm_crtc *crtc)
 		clk_disable_unprepare(compo->clk_compo_aux);
 	}
 
-	compo->enable = false;
+	mixer->enabled = false;
 }
 
 static struct drm_crtc_helper_funcs sti_crtc_helper_funcs = {
@@ -363,7 +370,6 @@ void sti_drm_crtc_disable_vblank(struct drm_device *dev, int crtc)
 	struct sti_drm_private *priv = dev->dev_private;
 	struct sti_compositor *compo = priv->compo;
 	struct notifier_block *vtg_vblank_nb = &compo->vtg_vblank_nb;
-	unsigned long flags;
 
 	DRM_DEBUG_DRIVER("\n");
 
@@ -372,13 +378,10 @@ void sti_drm_crtc_disable_vblank(struct drm_device *dev, int crtc)
 		DRM_DEBUG_DRIVER("Warning: cannot unregister VTG notifier\n");
 
 	/* free the resources of the pending requests */
-	spin_lock_irqsave(&dev->event_lock, flags);
 	if (compo->mixer[crtc]->pending_event) {
 		drm_vblank_put(dev, crtc);
 		compo->mixer[crtc]->pending_event = NULL;
 	}
-	spin_unlock_irqrestore(&dev->event_lock, flags);
-
 }
 EXPORT_SYMBOL(sti_drm_crtc_disable_vblank);
 
@@ -398,6 +401,7 @@ bool sti_drm_crtc_is_main(struct drm_crtc *crtc)
 
 	return false;
 }
+EXPORT_SYMBOL(sti_drm_crtc_is_main);
 
 int sti_drm_crtc_init(struct drm_device *drm_dev, struct sti_mixer *mixer,
 		struct drm_plane *primary, struct drm_plane *cursor)
diff --git a/drivers/gpu/drm/sti/sti_drm_drv.c b/drivers/gpu/drm/sti/sti_drm_drv.c
index 223d93c3a05d..5239fa121726 100644
--- a/drivers/gpu/drm/sti/sti_drm_drv.c
+++ b/drivers/gpu/drm/sti/sti_drm_drv.c
@@ -67,8 +67,12 @@ static int sti_drm_load(struct drm_device *dev, unsigned long flags)
 	sti_drm_mode_config_init(dev);
 
 	ret = component_bind_all(dev->dev, dev);
-	if (ret)
+	if (ret) {
+		drm_kms_helper_poll_fini(dev);
+		drm_mode_config_cleanup(dev);
+		kfree(private);
 		return ret;
+	}
 
 	drm_helper_disable_unused_functions(dev);
 
@@ -184,7 +188,6 @@ static struct platform_driver sti_drm_master_driver = {
 	.probe = sti_drm_master_probe,
 	.remove = sti_drm_master_remove,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = DRIVER_NAME "__master",
 	},
 };
@@ -228,7 +231,6 @@ static struct platform_driver sti_drm_platform_driver = {
 	.probe = sti_drm_platform_probe,
 	.remove = sti_drm_platform_remove,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = DRIVER_NAME,
 		.of_match_table = sti_drm_dt_ids,
 	},
diff --git a/drivers/gpu/drm/sti/sti_drm_plane.c b/drivers/gpu/drm/sti/sti_drm_plane.c
index f4118d4cac22..bb6a29339e10 100644
--- a/drivers/gpu/drm/sti/sti_drm_plane.c
+++ b/drivers/gpu/drm/sti/sti_drm_plane.c
@@ -45,7 +45,8 @@ sti_drm_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 	}
 
 	/* src_x are in 16.16 format. */
-	res = sti_layer_prepare(layer, fb, &crtc->mode, mixer->id,
+	res = sti_layer_prepare(layer, crtc, fb,
+			&crtc->mode, mixer->id,
 			crtc_x, crtc_y, crtc_w, crtc_h,
 			src_x >> 16, src_y >> 16,
 			src_w >> 16, src_h >> 16);
@@ -193,3 +194,4 @@ struct drm_plane *sti_drm_plane_init(struct drm_device *dev,
 
 	return &layer->plane;
 }
+EXPORT_SYMBOL(sti_drm_plane_init);
diff --git a/drivers/gpu/drm/sti/sti_gdp.c b/drivers/gpu/drm/sti/sti_gdp.c
index 4e30b74559f5..32448d1d1e8f 100644
--- a/drivers/gpu/drm/sti/sti_gdp.c
+++ b/drivers/gpu/drm/sti/sti_gdp.c
@@ -73,7 +73,9 @@ struct sti_gdp_node {
 
 struct sti_gdp_node_list {
 	struct sti_gdp_node *top_field;
+	dma_addr_t top_field_paddr;
 	struct sti_gdp_node *btm_field;
+	dma_addr_t btm_field_paddr;
 };
 
 /**
@@ -81,6 +83,8 @@ struct sti_gdp_node_list {
  *
  * @layer:		layer structure
  * @clk_pix:            pixel clock for the current gdp
+ * @clk_main_parent:    gdp parent clock if main path used
+ * @clk_aux_parent:     gdp parent clock if aux path used
  * @vtg_field_nb:       callback for VTG FIELD (top or bottom) notification
  * @is_curr_top:        true if the current node processed is the top field
  * @node_list:		array of node list
@@ -88,6 +92,8 @@ struct sti_gdp_node_list {
 struct sti_gdp {
 	struct sti_layer layer;
 	struct clk *clk_pix;
+	struct clk *clk_main_parent;
+	struct clk *clk_aux_parent;
 	struct notifier_block vtg_field_nb;
 	bool is_curr_top;
 	struct sti_gdp_node_list node_list[GDP_NODE_NB_BANK];
@@ -168,7 +174,6 @@ static int sti_gdp_get_alpharange(int format)
 static struct sti_gdp_node_list *sti_gdp_get_free_nodes(struct sti_layer *layer)
 {
 	int hw_nvn;
-	void *virt_nvn;
 	struct sti_gdp *gdp = to_sti_gdp(layer);
 	unsigned int i;
 
@@ -176,11 +181,9 @@ static struct sti_gdp_node_list *sti_gdp_get_free_nodes(struct sti_layer *layer)
 	if (!hw_nvn)
 		goto end;
 
-	virt_nvn = dma_to_virt(layer->dev, (dma_addr_t) hw_nvn);
-
 	for (i = 0; i < GDP_NODE_NB_BANK; i++)
-		if ((virt_nvn != gdp->node_list[i].btm_field) &&
-		    (virt_nvn != gdp->node_list[i].top_field))
+		if ((hw_nvn != gdp->node_list[i].btm_field_paddr) &&
+		    (hw_nvn != gdp->node_list[i].top_field_paddr))
 			return &gdp->node_list[i];
 
 	/* in hazardious cases restart with the first node */
@@ -204,7 +207,6 @@ static
 struct sti_gdp_node_list *sti_gdp_get_current_nodes(struct sti_layer *layer)
 {
 	int hw_nvn;
-	void *virt_nvn;
 	struct sti_gdp *gdp = to_sti_gdp(layer);
 	unsigned int i;
 
@@ -212,11 +214,9 @@ struct sti_gdp_node_list *sti_gdp_get_current_nodes(struct sti_layer *layer)
 	if (!hw_nvn)
 		goto end;
 
-	virt_nvn = dma_to_virt(layer->dev, (dma_addr_t) hw_nvn);
-
 	for (i = 0; i < GDP_NODE_NB_BANK; i++)
-		if ((virt_nvn == gdp->node_list[i].btm_field) ||
-				(virt_nvn == gdp->node_list[i].top_field))
+		if ((hw_nvn == gdp->node_list[i].btm_field_paddr) ||
+				(hw_nvn == gdp->node_list[i].top_field_paddr))
 			return &gdp->node_list[i];
 
 end:
@@ -292,8 +292,8 @@ static int sti_gdp_prepare_layer(struct sti_layer *layer, bool first_prepare)
 
 	/* Same content and chained together */
 	memcpy(btm_field, top_field, sizeof(*btm_field));
-	top_field->gam_gdp_nvn = virt_to_dma(dev, btm_field);
-	btm_field->gam_gdp_nvn = virt_to_dma(dev, top_field);
+	top_field->gam_gdp_nvn = list->btm_field_paddr;
+	btm_field->gam_gdp_nvn = list->top_field_paddr;
 
 	/* Interlaced mode */
 	if (layer->mode->flags & DRM_MODE_FLAG_INTERLACE)
@@ -311,6 +311,17 @@ static int sti_gdp_prepare_layer(struct sti_layer *layer, bool first_prepare)
 
 		/* Set and enable gdp clock */
 		if (gdp->clk_pix) {
+			struct clk *clkp;
+			/* According to the mixer used, the gdp pixel clock
+			 * should have a different parent clock. */
+			if (layer->mixer_id == STI_MIXER_MAIN)
+				clkp = gdp->clk_main_parent;
+			else
+				clkp = gdp->clk_aux_parent;
+
+			if (clkp)
+				clk_set_parent(gdp->clk_pix, clkp);
+
 			res = clk_set_rate(gdp->clk_pix, rate);
 			if (res < 0) {
 				DRM_ERROR("Cannot set rate (%dHz) for gdp\n",
@@ -349,8 +360,8 @@ static int sti_gdp_commit_layer(struct sti_layer *layer)
 	struct sti_gdp_node *updated_top_node = updated_list->top_field;
 	struct sti_gdp_node *updated_btm_node = updated_list->btm_field;
 	struct sti_gdp *gdp = to_sti_gdp(layer);
-	u32 dma_updated_top = virt_to_dma(layer->dev, updated_top_node);
-	u32 dma_updated_btm = virt_to_dma(layer->dev, updated_btm_node);
+	u32 dma_updated_top = updated_list->top_field_paddr;
+	u32 dma_updated_btm = updated_list->btm_field_paddr;
 	struct sti_gdp_node_list *curr_list = sti_gdp_get_current_nodes(layer);
 
 	dev_dbg(layer->dev, "%s %s top/btm_node:0x%p/0x%p\n", __func__,
@@ -461,16 +472,16 @@ static void sti_gdp_init(struct sti_layer *layer)
 {
 	struct sti_gdp *gdp = to_sti_gdp(layer);
 	struct device_node *np = layer->dev->of_node;
-	dma_addr_t dma;
+	dma_addr_t dma_addr;
 	void *base;
 	unsigned int i, size;
 
 	/* Allocate all the nodes within a single memory page */
 	size = sizeof(struct sti_gdp_node) *
 	    GDP_NODE_PER_FIELD * GDP_NODE_NB_BANK;
-
 	base = dma_alloc_writecombine(layer->dev,
-			size, &dma, GFP_KERNEL | GFP_DMA);
+			size, &dma_addr, GFP_KERNEL | GFP_DMA);
+
 	if (!base) {
 		DRM_ERROR("Failed to allocate memory for GDP node\n");
 		return;
@@ -478,21 +489,26 @@ static void sti_gdp_init(struct sti_layer *layer)
 	memset(base, 0, size);
 
 	for (i = 0; i < GDP_NODE_NB_BANK; i++) {
-		if (virt_to_dma(layer->dev, base) & 0xF) {
+		if (dma_addr & 0xF) {
 			DRM_ERROR("Mem alignment failed\n");
 			return;
 		}
 		gdp->node_list[i].top_field = base;
+		gdp->node_list[i].top_field_paddr = dma_addr;
+
 		DRM_DEBUG_DRIVER("node[%d].top_field=%p\n", i, base);
 		base += sizeof(struct sti_gdp_node);
+		dma_addr += sizeof(struct sti_gdp_node);
 
-		if (virt_to_dma(layer->dev, base) & 0xF) {
+		if (dma_addr & 0xF) {
 			DRM_ERROR("Mem alignment failed\n");
 			return;
 		}
 		gdp->node_list[i].btm_field = base;
+		gdp->node_list[i].btm_field_paddr = dma_addr;
 		DRM_DEBUG_DRIVER("node[%d].btm_field=%p\n", i, base);
 		base += sizeof(struct sti_gdp_node);
+		dma_addr += sizeof(struct sti_gdp_node);
 	}
 
 	if (of_device_is_compatible(np, "st,stih407-compositor")) {
@@ -520,6 +536,14 @@ static void sti_gdp_init(struct sti_layer *layer)
 		gdp->clk_pix = devm_clk_get(layer->dev, clk_name);
 		if (IS_ERR(gdp->clk_pix))
 			DRM_ERROR("Cannot get %s clock\n", clk_name);
+
+		gdp->clk_main_parent = devm_clk_get(layer->dev, "main_parent");
+		if (IS_ERR(gdp->clk_main_parent))
+			DRM_ERROR("Cannot get main_parent clock\n");
+
+		gdp->clk_aux_parent = devm_clk_get(layer->dev, "aux_parent");
+		if (IS_ERR(gdp->clk_aux_parent))
+			DRM_ERROR("Cannot get aux_parent clock\n");
 	}
 }
 
diff --git a/drivers/gpu/drm/sti/sti_hdmi.c b/drivers/gpu/drm/sti/sti_hdmi.c
index b22968c08d1f..d032e024b0b8 100644
--- a/drivers/gpu/drm/sti/sti_hdmi.c
+++ b/drivers/gpu/drm/sti/sti_hdmi.c
@@ -130,8 +130,7 @@ static irqreturn_t hdmi_irq_thread(int irq, void *arg)
 
 	/* Hot plug/unplug IRQ */
 	if (hdmi->irq_status & HDMI_INT_HOT_PLUG) {
-		/* read gpio to get the status */
-		hdmi->hpd = gpio_get_value(hdmi->hpd_gpio);
+		hdmi->hpd = readl(hdmi->regs + HDMI_STA) & HDMI_STA_HOT_PLUG;
 		if (hdmi->drm_dev)
 			drm_helper_hpd_irq_event(hdmi->drm_dev);
 	}
@@ -273,31 +272,32 @@ static int hdmi_avi_infoframe_config(struct sti_hdmi *hdmi)
 	hdmi_write(hdmi, val, HDMI_SW_DI_CFG);
 
 	/* Infoframe header */
-	val = buffer[0x0];
-	val |= buffer[0x1] << 8;
-	val |= buffer[0x2] << 16;
+	val =  buffer[0];
+	val |= buffer[1] << 8;
+	val |= buffer[2] << 16;
 	hdmi_write(hdmi, val, HDMI_SW_DI_N_HEAD_WORD(HDMI_IFRAME_SLOT_AVI));
 
 	/* Infoframe packet bytes */
-	val = frame[0x0];
-	val |= frame[0x1] << 8;
-	val |= frame[0x2] << 16;
-	val |= frame[0x3] << 24;
+	val =  buffer[3];
+	val |= *(frame++) << 8;
+	val |= *(frame++) << 16;
+	val |= *(frame++) << 24;
 	hdmi_write(hdmi, val, HDMI_SW_DI_N_PKT_WORD0(HDMI_IFRAME_SLOT_AVI));
 
-	val = frame[0x4];
-	val |= frame[0x5] << 8;
-	val |= frame[0x6] << 16;
-	val |= frame[0x7] << 24;
+	val =  *(frame++);
+	val |= *(frame++) << 8;
+	val |= *(frame++) << 16;
+	val |= *(frame++) << 24;
 	hdmi_write(hdmi, val, HDMI_SW_DI_N_PKT_WORD1(HDMI_IFRAME_SLOT_AVI));
 
-	val = frame[0x8];
-	val |= frame[0x9] << 8;
-	val |= frame[0xA] << 16;
-	val |= frame[0xB] << 24;
+	val =  *(frame++);
+	val |= *(frame++) << 8;
+	val |= *(frame++) << 16;
+	val |= *(frame++) << 24;
 	hdmi_write(hdmi, val, HDMI_SW_DI_N_PKT_WORD2(HDMI_IFRAME_SLOT_AVI));
 
-	val = frame[0xC];
+	val = *(frame++);
+	val |= *(frame) << 8;
 	hdmi_write(hdmi, val, HDMI_SW_DI_N_PKT_WORD3(HDMI_IFRAME_SLOT_AVI));
 
 	/* Enable transmission slot for AVI infoframe
@@ -480,17 +480,15 @@ static const struct drm_bridge_funcs sti_hdmi_bridge_funcs = {
 
 static int sti_hdmi_connector_get_modes(struct drm_connector *connector)
 {
-	struct i2c_adapter *i2c_adap;
+	struct sti_hdmi_connector *hdmi_connector
+		= to_sti_hdmi_connector(connector);
+	struct sti_hdmi *hdmi = hdmi_connector->hdmi;
 	struct edid *edid;
 	int count;
 
 	DRM_DEBUG_DRIVER("\n");
 
-	i2c_adap = i2c_get_adapter(1);
-	if (!i2c_adap)
-		goto fail;
-
-	edid = drm_get_edid(connector, i2c_adap);
+	edid = drm_get_edid(connector, hdmi->ddc_adapt);
 	if (!edid)
 		goto fail;
 
@@ -603,29 +601,38 @@ static int sti_hdmi_bind(struct device *dev, struct device *master, void *data)
 	struct sti_hdmi_connector *connector;
 	struct drm_connector *drm_connector;
 	struct drm_bridge *bridge;
-	struct i2c_adapter *i2c_adap;
+	struct device_node *ddc;
 	int err;
 
-	i2c_adap = i2c_get_adapter(1);
-	if (!i2c_adap)
-		return -EPROBE_DEFER;
+	ddc = of_parse_phandle(dev->of_node, "ddc", 0);
+	if (ddc) {
+		hdmi->ddc_adapt = of_find_i2c_adapter_by_node(ddc);
+		if (!hdmi->ddc_adapt) {
+			err = -EPROBE_DEFER;
+			of_node_put(ddc);
+			return err;
+		}
+
+		of_node_put(ddc);
+	}
 
 	/* Set the drm device handle */
 	hdmi->drm_dev = drm_dev;
 
 	encoder = sti_hdmi_find_encoder(drm_dev);
 	if (!encoder)
-		return -ENOMEM;
+		goto err_adapt;
 
 	connector = devm_kzalloc(dev, sizeof(*connector), GFP_KERNEL);
 	if (!connector)
-		return -ENOMEM;
+		goto err_adapt;
+
 
 	connector->hdmi = hdmi;
 
 	bridge = devm_kzalloc(dev, sizeof(*bridge), GFP_KERNEL);
 	if (!bridge)
-		return -ENOMEM;
+		goto err_adapt;
 
 	bridge->driver_private = hdmi;
 	drm_bridge_init(drm_dev, bridge, &sti_hdmi_bridge_funcs);
@@ -662,6 +669,8 @@ err_sysfs:
 err_connector:
 	drm_bridge_cleanup(bridge);
 	drm_connector_cleanup(drm_connector);
+err_adapt:
+	put_device(&hdmi->ddc_adapt->dev);
 	return -EINVAL;
 }
 
@@ -757,13 +766,7 @@ static int sti_hdmi_probe(struct platform_device *pdev)
 		return PTR_ERR(hdmi->clk_audio);
 	}
 
-	hdmi->hpd_gpio = of_get_named_gpio(np, "hdmi,hpd-gpio", 0);
-	if (hdmi->hpd_gpio < 0) {
-		DRM_ERROR("Failed to get hdmi hpd-gpio\n");
-		return -EIO;
-	}
-
-	hdmi->hpd = gpio_get_value(hdmi->hpd_gpio);
+	hdmi->hpd = readl(hdmi->regs + HDMI_STA) & HDMI_STA_HOT_PLUG;
 
 	init_waitqueue_head(&hdmi->wait_event);
 
@@ -788,6 +791,11 @@ static int sti_hdmi_probe(struct platform_device *pdev)
 
 static int sti_hdmi_remove(struct platform_device *pdev)
 {
+	struct sti_hdmi *hdmi = dev_get_drvdata(&pdev->dev);
+
+	if (hdmi->ddc_adapt)
+		put_device(&hdmi->ddc_adapt->dev);
+
 	component_del(&pdev->dev, &sti_hdmi_ops);
 	return 0;
 }
diff --git a/drivers/gpu/drm/sti/sti_hdmi.h b/drivers/gpu/drm/sti/sti_hdmi.h
index 61bec6557ceb..3d22390e1f3b 100644
--- a/drivers/gpu/drm/sti/sti_hdmi.h
+++ b/drivers/gpu/drm/sti/sti_hdmi.h
@@ -14,6 +14,9 @@
 #define HDMI_STA           0x0010
 #define HDMI_STA_DLL_LCK   BIT(5)
 
+#define HDMI_STA_HOT_PLUG_SHIFT 4
+#define HDMI_STA_HOT_PLUG	(1 << HDMI_STA_HOT_PLUG_SHIFT)
+
 struct sti_hdmi;
 
 struct hdmi_phy_ops {
@@ -37,7 +40,6 @@ struct hdmi_phy_ops {
  * @irq_status: interrupt status register
  * @phy_ops: phy start/stop operations
  * @enabled: true if hdmi is enabled else false
- * @hpd_gpio: hdmi hot plug detect gpio number
  * @hpd: hot plug detect status
  * @wait_event: wait event
  * @event_received: wait event status
@@ -57,11 +59,11 @@ struct sti_hdmi {
 	u32 irq_status;
 	struct hdmi_phy_ops *phy_ops;
 	bool enabled;
-	int hpd_gpio;
 	bool hpd;
 	wait_queue_head_t wait_event;
 	bool event_received;
 	struct reset_control *reset;
+	struct i2c_adapter *ddc_adapt;
 };
 
 u32 hdmi_read(struct sti_hdmi *hdmi, int offset);
diff --git a/drivers/gpu/drm/sti/sti_hqvdp.c b/drivers/gpu/drm/sti/sti_hqvdp.c
new file mode 100644
index 000000000000..f3db05dab0ab
--- /dev/null
+++ b/drivers/gpu/drm/sti/sti_hqvdp.c
@@ -0,0 +1,1073 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2014
+ * Authors: Fabien Dessenne <fabien.dessenne@st.com> for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include <linux/clk.h>
+#include <linux/component.h>
+#include <linux/firmware.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/reset.h>
+
+#include <drm/drmP.h>
+
+#include "sti_drm_plane.h"
+#include "sti_hqvdp.h"
+#include "sti_hqvdp_lut.h"
+#include "sti_layer.h"
+#include "sti_vtg.h"
+
+/* Firmware name */
+#define HQVDP_FMW_NAME          "hqvdp-stih407.bin"
+
+/* Regs address */
+#define HQVDP_DMEM              0x00000000               /* 0x00000000 */
+#define HQVDP_PMEM              0x00040000               /* 0x00040000 */
+#define HQVDP_RD_PLUG           0x000E0000               /* 0x000E0000 */
+#define HQVDP_RD_PLUG_CONTROL   (HQVDP_RD_PLUG + 0x1000) /* 0x000E1000 */
+#define HQVDP_RD_PLUG_PAGE_SIZE (HQVDP_RD_PLUG + 0x1004) /* 0x000E1004 */
+#define HQVDP_RD_PLUG_MIN_OPC   (HQVDP_RD_PLUG + 0x1008) /* 0x000E1008 */
+#define HQVDP_RD_PLUG_MAX_OPC   (HQVDP_RD_PLUG + 0x100C) /* 0x000E100C */
+#define HQVDP_RD_PLUG_MAX_CHK   (HQVDP_RD_PLUG + 0x1010) /* 0x000E1010 */
+#define HQVDP_RD_PLUG_MAX_MSG   (HQVDP_RD_PLUG + 0x1014) /* 0x000E1014 */
+#define HQVDP_RD_PLUG_MIN_SPACE (HQVDP_RD_PLUG + 0x1018) /* 0x000E1018 */
+#define HQVDP_WR_PLUG           0x000E2000               /* 0x000E2000 */
+#define HQVDP_WR_PLUG_CONTROL   (HQVDP_WR_PLUG + 0x1000) /* 0x000E3000 */
+#define HQVDP_WR_PLUG_PAGE_SIZE (HQVDP_WR_PLUG + 0x1004) /* 0x000E3004 */
+#define HQVDP_WR_PLUG_MIN_OPC   (HQVDP_WR_PLUG + 0x1008) /* 0x000E3008 */
+#define HQVDP_WR_PLUG_MAX_OPC   (HQVDP_WR_PLUG + 0x100C) /* 0x000E300C */
+#define HQVDP_WR_PLUG_MAX_CHK   (HQVDP_WR_PLUG + 0x1010) /* 0x000E3010 */
+#define HQVDP_WR_PLUG_MAX_MSG   (HQVDP_WR_PLUG + 0x1014) /* 0x000E3014 */
+#define HQVDP_WR_PLUG_MIN_SPACE (HQVDP_WR_PLUG + 0x1018) /* 0x000E3018 */
+#define HQVDP_MBX               0x000E4000               /* 0x000E4000 */
+#define HQVDP_MBX_IRQ_TO_XP70   (HQVDP_MBX + 0x0000)     /* 0x000E4000 */
+#define HQVDP_MBX_INFO_HOST     (HQVDP_MBX + 0x0004)     /* 0x000E4004 */
+#define HQVDP_MBX_IRQ_TO_HOST   (HQVDP_MBX + 0x0008)     /* 0x000E4008 */
+#define HQVDP_MBX_INFO_XP70     (HQVDP_MBX + 0x000C)     /* 0x000E400C */
+#define HQVDP_MBX_SW_RESET_CTRL (HQVDP_MBX + 0x0010)     /* 0x000E4010 */
+#define HQVDP_MBX_STARTUP_CTRL1 (HQVDP_MBX + 0x0014)     /* 0x000E4014 */
+#define HQVDP_MBX_STARTUP_CTRL2 (HQVDP_MBX + 0x0018)     /* 0x000E4018 */
+#define HQVDP_MBX_GP_STATUS     (HQVDP_MBX + 0x001C)     /* 0x000E401C */
+#define HQVDP_MBX_NEXT_CMD      (HQVDP_MBX + 0x0020)     /* 0x000E4020 */
+#define HQVDP_MBX_CURRENT_CMD   (HQVDP_MBX + 0x0024)     /* 0x000E4024 */
+#define HQVDP_MBX_SOFT_VSYNC    (HQVDP_MBX + 0x0028)     /* 0x000E4028 */
+
+/* Plugs config */
+#define PLUG_CONTROL_ENABLE     0x00000001
+#define PLUG_PAGE_SIZE_256      0x00000002
+#define PLUG_MIN_OPC_8          0x00000003
+#define PLUG_MAX_OPC_64         0x00000006
+#define PLUG_MAX_CHK_2X         0x00000001
+#define PLUG_MAX_MSG_1X         0x00000000
+#define PLUG_MIN_SPACE_1        0x00000000
+
+/* SW reset CTRL */
+#define SW_RESET_CTRL_FULL      BIT(0)
+#define SW_RESET_CTRL_CORE      BIT(1)
+
+/* Startup ctrl 1 */
+#define STARTUP_CTRL1_RST_DONE  BIT(0)
+#define STARTUP_CTRL1_AUTH_IDLE BIT(2)
+
+/* Startup ctrl 2 */
+#define STARTUP_CTRL2_FETCH_EN  BIT(1)
+
+/* Info xP70 */
+#define INFO_XP70_FW_READY      BIT(15)
+#define INFO_XP70_FW_PROCESSING BIT(14)
+#define INFO_XP70_FW_INITQUEUES BIT(13)
+
+/* SOFT_VSYNC */
+#define SOFT_VSYNC_HW           0x00000000
+#define SOFT_VSYNC_SW_CMD       0x00000001
+#define SOFT_VSYNC_SW_CTRL_IRQ  0x00000003
+
+/* Reset & boot poll config */
+#define POLL_MAX_ATTEMPT        50
+#define POLL_DELAY_MS           20
+
+#define SCALE_FACTOR            8192
+#define SCALE_MAX_FOR_LEG_LUT_F 4096
+#define SCALE_MAX_FOR_LEG_LUT_E 4915
+#define SCALE_MAX_FOR_LEG_LUT_D 6654
+#define SCALE_MAX_FOR_LEG_LUT_C 8192
+
+enum sti_hvsrc_orient {
+	HVSRC_HORI,
+	HVSRC_VERT
+};
+
+/* Command structures */
+struct sti_hqvdp_top {
+	u32 config;
+	u32 mem_format;
+	u32 current_luma;
+	u32 current_enh_luma;
+	u32 current_right_luma;
+	u32 current_enh_right_luma;
+	u32 current_chroma;
+	u32 current_enh_chroma;
+	u32 current_right_chroma;
+	u32 current_enh_right_chroma;
+	u32 output_luma;
+	u32 output_chroma;
+	u32 luma_src_pitch;
+	u32 luma_enh_src_pitch;
+	u32 luma_right_src_pitch;
+	u32 luma_enh_right_src_pitch;
+	u32 chroma_src_pitch;
+	u32 chroma_enh_src_pitch;
+	u32 chroma_right_src_pitch;
+	u32 chroma_enh_right_src_pitch;
+	u32 luma_processed_pitch;
+	u32 chroma_processed_pitch;
+	u32 input_frame_size;
+	u32 input_viewport_ori;
+	u32 input_viewport_ori_right;
+	u32 input_viewport_size;
+	u32 left_view_border_width;
+	u32 right_view_border_width;
+	u32 left_view_3d_offset_width;
+	u32 right_view_3d_offset_width;
+	u32 side_stripe_color;
+	u32 crc_reset_ctrl;
+};
+
+/* Configs for interlaced : no IT, no pass thru, 3 fields */
+#define TOP_CONFIG_INTER_BTM            0x00000000
+#define TOP_CONFIG_INTER_TOP            0x00000002
+
+/* Config for progressive : no IT, no pass thru, 3 fields */
+#define TOP_CONFIG_PROGRESSIVE          0x00000001
+
+/* Default MemFormat: in=420_raster_dual out=444_raster;opaque Mem2Tv mode */
+#define TOP_MEM_FORMAT_DFLT             0x00018060
+
+/* Min/Max size */
+#define MAX_WIDTH                       0x1FFF
+#define MAX_HEIGHT                      0x0FFF
+#define MIN_WIDTH                       0x0030
+#define MIN_HEIGHT                      0x0010
+
+struct sti_hqvdp_vc1re {
+	u32 ctrl_prv_csdi;
+	u32 ctrl_cur_csdi;
+	u32 ctrl_nxt_csdi;
+	u32 ctrl_cur_fmd;
+	u32 ctrl_nxt_fmd;
+};
+
+struct sti_hqvdp_fmd {
+	u32 config;
+	u32 viewport_ori;
+	u32 viewport_size;
+	u32 next_next_luma;
+	u32 next_next_right_luma;
+	u32 next_next_next_luma;
+	u32 next_next_next_right_luma;
+	u32 threshold_scd;
+	u32 threshold_rfd;
+	u32 threshold_move;
+	u32 threshold_cfd;
+};
+
+struct sti_hqvdp_csdi {
+	u32 config;
+	u32 config2;
+	u32 dcdi_config;
+	u32 prev_luma;
+	u32 prev_enh_luma;
+	u32 prev_right_luma;
+	u32 prev_enh_right_luma;
+	u32 next_luma;
+	u32 next_enh_luma;
+	u32 next_right_luma;
+	u32 next_enh_right_luma;
+	u32 prev_chroma;
+	u32 prev_enh_chroma;
+	u32 prev_right_chroma;
+	u32 prev_enh_right_chroma;
+	u32 next_chroma;
+	u32 next_enh_chroma;
+	u32 next_right_chroma;
+	u32 next_enh_right_chroma;
+	u32 prev_motion;
+	u32 prev_right_motion;
+	u32 cur_motion;
+	u32 cur_right_motion;
+	u32 next_motion;
+	u32 next_right_motion;
+};
+
+/* Config for progressive: by pass */
+#define CSDI_CONFIG_PROG                0x00000000
+/* Config for directional deinterlacing without motion */
+#define CSDI_CONFIG_INTER_DIR           0x00000016
+/* Additional configs for fader, blender, motion,... deinterlace algorithms */
+#define CSDI_CONFIG2_DFLT               0x000001B3
+#define CSDI_DCDI_CONFIG_DFLT           0x00203803
+
+struct sti_hqvdp_hvsrc {
+	u32 hor_panoramic_ctrl;
+	u32 output_picture_size;
+	u32 init_horizontal;
+	u32 init_vertical;
+	u32 param_ctrl;
+	u32 yh_coef[NB_COEF];
+	u32 ch_coef[NB_COEF];
+	u32 yv_coef[NB_COEF];
+	u32 cv_coef[NB_COEF];
+	u32 hori_shift;
+	u32 vert_shift;
+};
+
+/* Default ParamCtrl: all controls enabled */
+#define HVSRC_PARAM_CTRL_DFLT           0xFFFFFFFF
+
+struct sti_hqvdp_iqi {
+	u32 config;
+	u32 demo_wind_size;
+	u32 pk_config;
+	u32 coeff0_coeff1;
+	u32 coeff2_coeff3;
+	u32 coeff4;
+	u32 pk_lut;
+	u32 pk_gain;
+	u32 pk_coring_level;
+	u32 cti_config;
+	u32 le_config;
+	u32 le_lut[64];
+	u32 con_bri;
+	u32 sat_gain;
+	u32 pxf_conf;
+	u32 default_color;
+};
+
+/* Default Config : IQI bypassed */
+#define IQI_CONFIG_DFLT                 0x00000001
+/* Default Contrast & Brightness gain = 256 */
+#define IQI_CON_BRI_DFLT                0x00000100
+/* Default Saturation gain = 256 */
+#define IQI_SAT_GAIN_DFLT               0x00000100
+/* Default PxfConf : P2I bypassed */
+#define IQI_PXF_CONF_DFLT               0x00000001
+
+struct sti_hqvdp_top_status {
+	u32 processing_time;
+	u32 input_y_crc;
+	u32 input_uv_crc;
+};
+
+struct sti_hqvdp_fmd_status {
+	u32 fmd_repeat_move_status;
+	u32 fmd_scene_count_status;
+	u32 cfd_sum;
+	u32 field_sum;
+	u32 next_y_fmd_crc;
+	u32 next_next_y_fmd_crc;
+	u32 next_next_next_y_fmd_crc;
+};
+
+struct sti_hqvdp_csdi_status {
+	u32 prev_y_csdi_crc;
+	u32 cur_y_csdi_crc;
+	u32 next_y_csdi_crc;
+	u32 prev_uv_csdi_crc;
+	u32 cur_uv_csdi_crc;
+	u32 next_uv_csdi_crc;
+	u32 y_csdi_crc;
+	u32 uv_csdi_crc;
+	u32 uv_cup_crc;
+	u32 mot_csdi_crc;
+	u32 mot_cur_csdi_crc;
+	u32 mot_prev_csdi_crc;
+};
+
+struct sti_hqvdp_hvsrc_status {
+	u32 y_hvsrc_crc;
+	u32 u_hvsrc_crc;
+	u32 v_hvsrc_crc;
+};
+
+struct sti_hqvdp_iqi_status {
+	u32 pxf_it_status;
+	u32 y_iqi_crc;
+	u32 u_iqi_crc;
+	u32 v_iqi_crc;
+};
+
+/* Main commands. We use 2 commands one being processed by the firmware, one
+ * ready to be fetched upon next Vsync*/
+#define NB_VDP_CMD	2
+
+struct sti_hqvdp_cmd {
+	struct sti_hqvdp_top top;
+	struct sti_hqvdp_vc1re vc1re;
+	struct sti_hqvdp_fmd fmd;
+	struct sti_hqvdp_csdi csdi;
+	struct sti_hqvdp_hvsrc hvsrc;
+	struct sti_hqvdp_iqi iqi;
+	struct sti_hqvdp_top_status top_status;
+	struct sti_hqvdp_fmd_status fmd_status;
+	struct sti_hqvdp_csdi_status csdi_status;
+	struct sti_hqvdp_hvsrc_status hvsrc_status;
+	struct sti_hqvdp_iqi_status iqi_status;
+};
+
+/*
+ * STI HQVDP structure
+ *
+ * @dev:               driver device
+ * @drm_dev:           the drm device
+ * @regs:              registers
+ * @layer:             layer structure for hqvdp it self
+ * @vid_plane:         VID plug used as link with compositor IP
+ * @clk:               IP clock
+ * @clk_pix_main:      pix main clock
+ * @reset:             reset control
+ * @vtg_nb:            notifier to handle VTG Vsync
+ * @btm_field_pending: is there any bottom field (interlaced frame) to display
+ * @curr_field_count:  number of field updates
+ * @last_field_count:  number of field updates since last fps measure
+ * @hqvdp_cmd:         buffer of commands
+ * @hqvdp_cmd_paddr:   physical address of hqvdp_cmd
+ * @vtg:               vtg for main data path
+ */
+struct sti_hqvdp {
+	struct device *dev;
+	struct drm_device *drm_dev;
+	void __iomem *regs;
+	struct sti_layer layer;
+	struct drm_plane *vid_plane;
+	struct clk *clk;
+	struct clk *clk_pix_main;
+	struct reset_control *reset;
+	struct notifier_block vtg_nb;
+	bool btm_field_pending;
+	unsigned int curr_field_count;
+	unsigned int last_field_count;
+	void *hqvdp_cmd;
+	dma_addr_t hqvdp_cmd_paddr;
+	struct sti_vtg *vtg;
+};
+
+#define to_sti_hqvdp(x) container_of(x, struct sti_hqvdp, layer)
+
+static const uint32_t hqvdp_supported_formats[] = {
+	DRM_FORMAT_NV12,
+};
+
+static const uint32_t *sti_hqvdp_get_formats(struct sti_layer *layer)
+{
+	return hqvdp_supported_formats;
+}
+
+static unsigned int sti_hqvdp_get_nb_formats(struct sti_layer *layer)
+{
+	return ARRAY_SIZE(hqvdp_supported_formats);
+}
+
+/**
+ * sti_hqvdp_get_free_cmd
+ * @hqvdp: hqvdp structure
+ *
+ * Look for a hqvdp_cmd that is not being used (or about to be used) by the FW.
+ *
+ * RETURNS:
+ * the offset of the command to be used.
+ * -1 in error cases
+ */
+static int sti_hqvdp_get_free_cmd(struct sti_hqvdp *hqvdp)
+{
+	int curr_cmd, next_cmd;
+	dma_addr_t cmd = hqvdp->hqvdp_cmd_paddr;
+	int i;
+
+	curr_cmd = readl(hqvdp->regs + HQVDP_MBX_CURRENT_CMD);
+	next_cmd = readl(hqvdp->regs + HQVDP_MBX_NEXT_CMD);
+
+	for (i = 0; i < NB_VDP_CMD; i++) {
+		if ((cmd != curr_cmd) && (cmd != next_cmd))
+			return i * sizeof(struct sti_hqvdp_cmd);
+		cmd += sizeof(struct sti_hqvdp_cmd);
+	}
+
+	return -1;
+}
+
+/**
+ * sti_hqvdp_get_curr_cmd
+ * @hqvdp: hqvdp structure
+ *
+ * Look for the hqvdp_cmd that is being used by the FW.
+ *
+ * RETURNS:
+ *  the offset of the command to be used.
+ * -1 in error cases
+ */
+static int sti_hqvdp_get_curr_cmd(struct sti_hqvdp *hqvdp)
+{
+	int curr_cmd;
+	dma_addr_t cmd = hqvdp->hqvdp_cmd_paddr;
+	unsigned int i;
+
+	curr_cmd = readl(hqvdp->regs + HQVDP_MBX_CURRENT_CMD);
+
+	for (i = 0; i < NB_VDP_CMD; i++) {
+		if (cmd == curr_cmd)
+			return i * sizeof(struct sti_hqvdp_cmd);
+
+		cmd += sizeof(struct sti_hqvdp_cmd);
+	}
+
+	return -1;
+}
+
+/**
+ * sti_hqvdp_update_hvsrc
+ * @orient: horizontal or vertical
+ * @scale:  scaling/zoom factor
+ * @hvsrc:  the structure containing the LUT coef
+ *
+ * Update the Y and C Lut coef, as well as the shift param
+ *
+ * RETURNS:
+ * None.
+ */
+static void sti_hqvdp_update_hvsrc(enum sti_hvsrc_orient orient, int scale,
+		struct sti_hqvdp_hvsrc *hvsrc)
+{
+	const int *coef_c, *coef_y;
+	int shift_c, shift_y;
+
+	/* Get the appropriate coef tables */
+	if (scale < SCALE_MAX_FOR_LEG_LUT_F) {
+		coef_y = coef_lut_f_y_legacy;
+		coef_c = coef_lut_f_c_legacy;
+		shift_y = SHIFT_LUT_F_Y_LEGACY;
+		shift_c = SHIFT_LUT_F_C_LEGACY;
+	} else if (scale < SCALE_MAX_FOR_LEG_LUT_E) {
+		coef_y = coef_lut_e_y_legacy;
+		coef_c = coef_lut_e_c_legacy;
+		shift_y = SHIFT_LUT_E_Y_LEGACY;
+		shift_c = SHIFT_LUT_E_C_LEGACY;
+	} else if (scale < SCALE_MAX_FOR_LEG_LUT_D) {
+		coef_y = coef_lut_d_y_legacy;
+		coef_c = coef_lut_d_c_legacy;
+		shift_y = SHIFT_LUT_D_Y_LEGACY;
+		shift_c = SHIFT_LUT_D_C_LEGACY;
+	} else if (scale < SCALE_MAX_FOR_LEG_LUT_C) {
+		coef_y = coef_lut_c_y_legacy;
+		coef_c = coef_lut_c_c_legacy;
+		shift_y = SHIFT_LUT_C_Y_LEGACY;
+		shift_c = SHIFT_LUT_C_C_LEGACY;
+	} else if (scale == SCALE_MAX_FOR_LEG_LUT_C) {
+		coef_y = coef_c = coef_lut_b;
+		shift_y = shift_c = SHIFT_LUT_B;
+	} else {
+		coef_y = coef_c = coef_lut_a_legacy;
+		shift_y = shift_c = SHIFT_LUT_A_LEGACY;
+	}
+
+	if (orient == HVSRC_HORI) {
+		hvsrc->hori_shift = (shift_c << 16) | shift_y;
+		memcpy(hvsrc->yh_coef, coef_y, sizeof(hvsrc->yh_coef));
+		memcpy(hvsrc->ch_coef, coef_c, sizeof(hvsrc->ch_coef));
+	} else {
+		hvsrc->vert_shift = (shift_c << 16) | shift_y;
+		memcpy(hvsrc->yv_coef, coef_y, sizeof(hvsrc->yv_coef));
+		memcpy(hvsrc->cv_coef, coef_c, sizeof(hvsrc->cv_coef));
+	}
+}
+
+/**
+ * sti_hqvdp_check_hw_scaling
+ * @layer: hqvdp layer
+ *
+ * Check if the HW is able to perform the scaling request
+ * The firmware scaling limitation is "CEIL(1/Zy) <= FLOOR(LFW)" where:
+ *   Zy = OutputHeight / InputHeight
+ *   LFW = (Tx * IPClock) / (MaxNbCycles * Cp)
+ *     Tx : Total video mode horizontal resolution
+ *     IPClock : HQVDP IP clock (Mhz)
+ *     MaxNbCycles: max(InputWidth, OutputWidth)
+ *     Cp: Video mode pixel clock (Mhz)
+ *
+ * RETURNS:
+ * True if the HW can scale.
+ */
+static bool sti_hqvdp_check_hw_scaling(struct sti_layer *layer)
+{
+	struct sti_hqvdp *hqvdp = to_sti_hqvdp(layer);
+	unsigned long lfw;
+	unsigned int inv_zy;
+
+	lfw = layer->mode->htotal * (clk_get_rate(hqvdp->clk) / 1000000);
+	lfw /= max(layer->src_w, layer->dst_w) * layer->mode->clock / 1000;
+
+	inv_zy = DIV_ROUND_UP(layer->src_h, layer->dst_h);
+
+	return (inv_zy <= lfw) ? true : false;
+}
+
+/**
+ * sti_hqvdp_prepare_layer
+ * @layer: hqvdp layer
+ * @first_prepare: true if it is the first time this function is called
+ *
+ * Prepares a command for the firmware
+ *
+ * RETURNS:
+ * 0 on success.
+ */
+static int sti_hqvdp_prepare_layer(struct sti_layer *layer, bool first_prepare)
+{
+	struct sti_hqvdp *hqvdp = to_sti_hqvdp(layer);
+	struct sti_hqvdp_cmd *cmd;
+	int scale_h, scale_v;
+	int cmd_offset;
+
+	dev_dbg(hqvdp->dev, "%s %s\n", __func__, sti_layer_to_str(layer));
+
+	/* prepare and commit VID plane */
+	hqvdp->vid_plane->funcs->update_plane(hqvdp->vid_plane,
+					layer->crtc, layer->fb,
+					layer->dst_x, layer->dst_y,
+					layer->dst_w, layer->dst_h,
+					layer->src_x, layer->src_y,
+					layer->src_w, layer->src_h);
+
+	cmd_offset = sti_hqvdp_get_free_cmd(hqvdp);
+	if (cmd_offset == -1) {
+		DRM_ERROR("No available hqvdp_cmd now\n");
+		return -EBUSY;
+	}
+	cmd = hqvdp->hqvdp_cmd + cmd_offset;
+
+	if (!sti_hqvdp_check_hw_scaling(layer)) {
+		DRM_ERROR("Scaling beyond HW capabilities\n");
+		return -EINVAL;
+	}
+
+	/* Static parameters, defaulting to progressive mode */
+	cmd->top.config = TOP_CONFIG_PROGRESSIVE;
+	cmd->top.mem_format = TOP_MEM_FORMAT_DFLT;
+	cmd->hvsrc.param_ctrl = HVSRC_PARAM_CTRL_DFLT;
+	cmd->csdi.config = CSDI_CONFIG_PROG;
+
+	/* VC1RE, FMD bypassed : keep everything set to 0
+	 * IQI/P2I bypassed */
+	cmd->iqi.config = IQI_CONFIG_DFLT;
+	cmd->iqi.con_bri = IQI_CON_BRI_DFLT;
+	cmd->iqi.sat_gain = IQI_SAT_GAIN_DFLT;
+	cmd->iqi.pxf_conf = IQI_PXF_CONF_DFLT;
+
+	/* Buffer planes address */
+	cmd->top.current_luma = (u32) layer->paddr + layer->offsets[0];
+	cmd->top.current_chroma = (u32) layer->paddr + layer->offsets[1];
+
+	/* Pitches */
+	cmd->top.luma_processed_pitch = cmd->top.luma_src_pitch =
+			layer->pitches[0];
+	cmd->top.chroma_processed_pitch = cmd->top.chroma_src_pitch =
+			layer->pitches[1];
+
+	/* Input / output size
+	 * Align to upper even value */
+	layer->dst_w = ALIGN(layer->dst_w, 2);
+	layer->dst_h = ALIGN(layer->dst_h, 2);
+
+	if ((layer->src_w > MAX_WIDTH) || (layer->src_w < MIN_WIDTH) ||
+	    (layer->src_h > MAX_HEIGHT) || (layer->src_h < MIN_HEIGHT) ||
+	    (layer->dst_w > MAX_WIDTH) || (layer->dst_w < MIN_WIDTH) ||
+	    (layer->dst_h > MAX_HEIGHT) || (layer->dst_h < MIN_HEIGHT)) {
+		DRM_ERROR("Invalid in/out size %dx%d -> %dx%d\n",
+				layer->src_w, layer->src_h,
+				layer->dst_w, layer->dst_h);
+		return -EINVAL;
+	}
+	cmd->top.input_viewport_size = cmd->top.input_frame_size =
+			layer->src_h << 16 | layer->src_w;
+	cmd->hvsrc.output_picture_size = layer->dst_h << 16 | layer->dst_w;
+	cmd->top.input_viewport_ori = layer->src_y << 16 | layer->src_x;
+
+	/* Handle interlaced */
+	if (layer->fb->flags & DRM_MODE_FB_INTERLACED) {
+		/* Top field to display */
+		cmd->top.config = TOP_CONFIG_INTER_TOP;
+
+		/* Update pitches and vert size */
+		cmd->top.input_frame_size = (layer->src_h / 2) << 16 |
+					     layer->src_w;
+		cmd->top.luma_processed_pitch *= 2;
+		cmd->top.luma_src_pitch *= 2;
+		cmd->top.chroma_processed_pitch *= 2;
+		cmd->top.chroma_src_pitch *= 2;
+
+		/* Enable directional deinterlacing processing */
+		cmd->csdi.config = CSDI_CONFIG_INTER_DIR;
+		cmd->csdi.config2 = CSDI_CONFIG2_DFLT;
+		cmd->csdi.dcdi_config = CSDI_DCDI_CONFIG_DFLT;
+	}
+
+	/* Update hvsrc lut coef */
+	scale_h = SCALE_FACTOR * layer->dst_w / layer->src_w;
+	sti_hqvdp_update_hvsrc(HVSRC_HORI, scale_h, &cmd->hvsrc);
+
+	scale_v = SCALE_FACTOR * layer->dst_h / layer->src_h;
+	sti_hqvdp_update_hvsrc(HVSRC_VERT, scale_v, &cmd->hvsrc);
+
+	if (first_prepare) {
+		/* Prevent VTG shutdown */
+		if (clk_prepare_enable(hqvdp->clk_pix_main)) {
+			DRM_ERROR("Failed to prepare/enable pix main clk\n");
+			return -ENXIO;
+		}
+
+		/* Register VTG Vsync callback to handle bottom fields */
+		if ((layer->fb->flags & DRM_MODE_FB_INTERLACED) &&
+				sti_vtg_register_client(hqvdp->vtg,
+					&hqvdp->vtg_nb, layer->mixer_id)) {
+			DRM_ERROR("Cannot register VTG notifier\n");
+			return -ENXIO;
+		}
+	}
+
+	return 0;
+}
+
+static int sti_hqvdp_commit_layer(struct sti_layer *layer)
+{
+	struct sti_hqvdp *hqvdp = to_sti_hqvdp(layer);
+	int cmd_offset;
+
+	dev_dbg(hqvdp->dev, "%s %s\n", __func__, sti_layer_to_str(layer));
+
+	cmd_offset = sti_hqvdp_get_free_cmd(hqvdp);
+	if (cmd_offset == -1) {
+		DRM_ERROR("No available hqvdp_cmd now\n");
+		return -EBUSY;
+	}
+
+	writel(hqvdp->hqvdp_cmd_paddr + cmd_offset,
+			hqvdp->regs + HQVDP_MBX_NEXT_CMD);
+
+	hqvdp->curr_field_count++;
+
+	/* Interlaced : get ready to display the bottom field at next Vsync */
+	if (layer->fb->flags & DRM_MODE_FB_INTERLACED)
+		hqvdp->btm_field_pending = true;
+
+	dev_dbg(hqvdp->dev, "%s Posted command:0x%x\n",
+			__func__, hqvdp->hqvdp_cmd_paddr + cmd_offset);
+
+	return 0;
+}
+
+static int sti_hqvdp_disable_layer(struct sti_layer *layer)
+{
+	struct sti_hqvdp *hqvdp = to_sti_hqvdp(layer);
+	int i;
+
+	DRM_DEBUG_DRIVER("%s\n", sti_layer_to_str(layer));
+
+	/* Unregister VTG Vsync callback */
+	if ((layer->fb->flags & DRM_MODE_FB_INTERLACED) &&
+		sti_vtg_unregister_client(hqvdp->vtg, &hqvdp->vtg_nb))
+		DRM_DEBUG_DRIVER("Warning: cannot unregister VTG notifier\n");
+
+	/* Set next cmd to NULL */
+	writel(0, hqvdp->regs + HQVDP_MBX_NEXT_CMD);
+
+	for (i = 0; i < POLL_MAX_ATTEMPT; i++) {
+		if (readl(hqvdp->regs + HQVDP_MBX_INFO_XP70)
+				& INFO_XP70_FW_READY)
+			break;
+		msleep(POLL_DELAY_MS);
+	}
+
+	/* VTG can stop now */
+	clk_disable_unprepare(hqvdp->clk_pix_main);
+
+	if (i == POLL_MAX_ATTEMPT) {
+		DRM_ERROR("XP70 could not revert to idle\n");
+		return -ENXIO;
+	}
+
+	/* disable VID plane */
+	hqvdp->vid_plane->funcs->disable_plane(hqvdp->vid_plane);
+
+	return 0;
+}
+
+/**
+ * sti_vdp_vtg_cb
+ * @nb: notifier block
+ * @evt: event message
+ * @data: private data
+ *
+ * Handle VTG Vsync event, display pending bottom field
+ *
+ * RETURNS:
+ * 0 on success.
+ */
+int sti_hqvdp_vtg_cb(struct notifier_block *nb, unsigned long evt, void *data)
+{
+	struct sti_hqvdp *hqvdp = container_of(nb, struct sti_hqvdp, vtg_nb);
+	int btm_cmd_offset, top_cmd_offest;
+	struct sti_hqvdp_cmd *btm_cmd, *top_cmd;
+
+	if ((evt != VTG_TOP_FIELD_EVENT) && (evt != VTG_BOTTOM_FIELD_EVENT)) {
+		DRM_DEBUG_DRIVER("Unknown event\n");
+		return 0;
+	}
+
+	if (hqvdp->btm_field_pending) {
+		/* Create the btm field command from the current one */
+		btm_cmd_offset = sti_hqvdp_get_free_cmd(hqvdp);
+		top_cmd_offest = sti_hqvdp_get_curr_cmd(hqvdp);
+		if ((btm_cmd_offset == -1) || (top_cmd_offest == -1)) {
+			DRM_ERROR("Cannot get cmds, skip btm field\n");
+			return -EBUSY;
+		}
+
+		btm_cmd = hqvdp->hqvdp_cmd + btm_cmd_offset;
+		top_cmd = hqvdp->hqvdp_cmd + top_cmd_offest;
+
+		memcpy(btm_cmd, top_cmd, sizeof(*btm_cmd));
+
+		btm_cmd->top.config = TOP_CONFIG_INTER_BTM;
+		btm_cmd->top.current_luma +=
+				btm_cmd->top.luma_src_pitch / 2;
+		btm_cmd->top.current_chroma +=
+				btm_cmd->top.chroma_src_pitch / 2;
+
+		/* Post the command to mailbox */
+		writel(hqvdp->hqvdp_cmd_paddr + btm_cmd_offset,
+				hqvdp->regs + HQVDP_MBX_NEXT_CMD);
+
+		hqvdp->curr_field_count++;
+		hqvdp->btm_field_pending = false;
+
+		dev_dbg(hqvdp->dev, "%s Posted command:0x%x\n",
+				__func__, hqvdp->hqvdp_cmd_paddr);
+	}
+
+	return 0;
+}
+
+static struct drm_plane *sti_hqvdp_find_vid(struct drm_device *dev, int id)
+{
+	struct drm_plane *plane;
+
+	list_for_each_entry(plane, &dev->mode_config.plane_list, head) {
+		struct sti_layer *layer = to_sti_layer(plane);
+
+		if (layer->desc == id)
+			return plane;
+	}
+
+	return NULL;
+}
+
+static void sti_hqvd_init(struct sti_layer *layer)
+{
+	struct sti_hqvdp *hqvdp = to_sti_hqvdp(layer);
+	int size;
+
+	/* find the plane macthing with vid 0 */
+	hqvdp->vid_plane = sti_hqvdp_find_vid(hqvdp->drm_dev, STI_VID_0);
+	if (!hqvdp->vid_plane) {
+		DRM_ERROR("Cannot find Main video layer\n");
+		return;
+	}
+
+	hqvdp->vtg_nb.notifier_call = sti_hqvdp_vtg_cb;
+
+	/* Allocate memory for the VDP commands */
+	size = NB_VDP_CMD * sizeof(struct sti_hqvdp_cmd);
+	hqvdp->hqvdp_cmd = dma_alloc_writecombine(hqvdp->dev, size,
+					 &hqvdp->hqvdp_cmd_paddr,
+					 GFP_KERNEL | GFP_DMA);
+	if (!hqvdp->hqvdp_cmd) {
+		DRM_ERROR("Failed to allocate memory for VDP cmd\n");
+		return;
+	}
+
+	memset(hqvdp->hqvdp_cmd, 0, size);
+}
+
+static const struct sti_layer_funcs hqvdp_ops = {
+	.get_formats = sti_hqvdp_get_formats,
+	.get_nb_formats = sti_hqvdp_get_nb_formats,
+	.init = sti_hqvd_init,
+	.prepare = sti_hqvdp_prepare_layer,
+	.commit = sti_hqvdp_commit_layer,
+	.disable = sti_hqvdp_disable_layer,
+};
+
+struct sti_layer *sti_hqvdp_create(struct device *dev)
+{
+	struct sti_hqvdp *hqvdp = dev_get_drvdata(dev);
+
+	hqvdp->layer.ops = &hqvdp_ops;
+
+	return &hqvdp->layer;
+}
+EXPORT_SYMBOL(sti_hqvdp_create);
+
+static void sti_hqvdp_init_plugs(struct sti_hqvdp *hqvdp)
+{
+	/* Configure Plugs (same for RD & WR) */
+	writel(PLUG_PAGE_SIZE_256, hqvdp->regs + HQVDP_RD_PLUG_PAGE_SIZE);
+	writel(PLUG_MIN_OPC_8, hqvdp->regs + HQVDP_RD_PLUG_MIN_OPC);
+	writel(PLUG_MAX_OPC_64, hqvdp->regs + HQVDP_RD_PLUG_MAX_OPC);
+	writel(PLUG_MAX_CHK_2X, hqvdp->regs + HQVDP_RD_PLUG_MAX_CHK);
+	writel(PLUG_MAX_MSG_1X, hqvdp->regs + HQVDP_RD_PLUG_MAX_MSG);
+	writel(PLUG_MIN_SPACE_1, hqvdp->regs + HQVDP_RD_PLUG_MIN_SPACE);
+	writel(PLUG_CONTROL_ENABLE, hqvdp->regs + HQVDP_RD_PLUG_CONTROL);
+
+	writel(PLUG_PAGE_SIZE_256, hqvdp->regs + HQVDP_WR_PLUG_PAGE_SIZE);
+	writel(PLUG_MIN_OPC_8, hqvdp->regs + HQVDP_WR_PLUG_MIN_OPC);
+	writel(PLUG_MAX_OPC_64, hqvdp->regs + HQVDP_WR_PLUG_MAX_OPC);
+	writel(PLUG_MAX_CHK_2X, hqvdp->regs + HQVDP_WR_PLUG_MAX_CHK);
+	writel(PLUG_MAX_MSG_1X, hqvdp->regs + HQVDP_WR_PLUG_MAX_MSG);
+	writel(PLUG_MIN_SPACE_1, hqvdp->regs + HQVDP_WR_PLUG_MIN_SPACE);
+	writel(PLUG_CONTROL_ENABLE, hqvdp->regs + HQVDP_WR_PLUG_CONTROL);
+}
+
+/**
+ * sti_hqvdp_start_xp70
+ * @firmware: firmware found
+ * @ctxt:     hqvdp structure
+ *
+ * Run the xP70 initialization sequence
+ */
+static void sti_hqvdp_start_xp70(const struct firmware *firmware, void *ctxt)
+{
+	struct sti_hqvdp *hqvdp = ctxt;
+	u32 *fw_rd_plug, *fw_wr_plug, *fw_pmem, *fw_dmem;
+	u8 *data;
+	int i;
+	struct fw_header {
+		int rd_size;
+		int wr_size;
+		int pmem_size;
+		int dmem_size;
+	} *header;
+
+	DRM_DEBUG_DRIVER("\n");
+	/* Check firmware parts */
+	if (!firmware) {
+		DRM_ERROR("Firmware not available\n");
+		return;
+	}
+
+	header = (struct fw_header *) firmware->data;
+	if (firmware->size < sizeof(*header)) {
+		DRM_ERROR("Invalid firmware size (%d)\n", firmware->size);
+		goto out;
+	}
+	if ((sizeof(*header) + header->rd_size + header->wr_size +
+		header->pmem_size + header->dmem_size) != firmware->size) {
+		DRM_ERROR("Invalid fmw structure (%d+%d+%d+%d+%d != %d)\n",
+			   sizeof(*header), header->rd_size, header->wr_size,
+			   header->pmem_size, header->dmem_size,
+			   firmware->size);
+		goto out;
+	}
+
+	data = (u8 *) firmware->data;
+	data += sizeof(*header);
+	fw_rd_plug = (void *) data;
+	data += header->rd_size;
+	fw_wr_plug = (void *) data;
+	data += header->wr_size;
+	fw_pmem = (void *) data;
+	data += header->pmem_size;
+	fw_dmem = (void *) data;
+
+	/* Enable clock */
+	if (clk_prepare_enable(hqvdp->clk))
+		DRM_ERROR("Failed to prepare/enable HQVDP clk\n");
+
+	/* Reset */
+	writel(SW_RESET_CTRL_FULL, hqvdp->regs + HQVDP_MBX_SW_RESET_CTRL);
+
+	for (i = 0; i < POLL_MAX_ATTEMPT; i++) {
+		if (readl(hqvdp->regs + HQVDP_MBX_STARTUP_CTRL1)
+				& STARTUP_CTRL1_RST_DONE)
+			break;
+		msleep(POLL_DELAY_MS);
+	}
+	if (i == POLL_MAX_ATTEMPT) {
+		DRM_ERROR("Could not reset\n");
+		goto out;
+	}
+
+	/* Init Read & Write plugs */
+	for (i = 0; i < header->rd_size / 4; i++)
+		writel(fw_rd_plug[i], hqvdp->regs + HQVDP_RD_PLUG + i * 4);
+	for (i = 0; i < header->wr_size / 4; i++)
+		writel(fw_wr_plug[i], hqvdp->regs + HQVDP_WR_PLUG + i * 4);
+
+	sti_hqvdp_init_plugs(hqvdp);
+
+	/* Authorize Idle Mode */
+	writel(STARTUP_CTRL1_AUTH_IDLE, hqvdp->regs + HQVDP_MBX_STARTUP_CTRL1);
+
+	/* Prevent VTG interruption during the boot */
+	writel(SOFT_VSYNC_SW_CTRL_IRQ, hqvdp->regs + HQVDP_MBX_SOFT_VSYNC);
+	writel(0, hqvdp->regs + HQVDP_MBX_NEXT_CMD);
+
+	/* Download PMEM & DMEM */
+	for (i = 0; i < header->pmem_size / 4; i++)
+		writel(fw_pmem[i], hqvdp->regs + HQVDP_PMEM + i * 4);
+	for (i = 0; i < header->dmem_size / 4; i++)
+		writel(fw_dmem[i], hqvdp->regs + HQVDP_DMEM + i * 4);
+
+	/* Enable fetch */
+	writel(STARTUP_CTRL2_FETCH_EN, hqvdp->regs + HQVDP_MBX_STARTUP_CTRL2);
+
+	/* Wait end of boot */
+	for (i = 0; i < POLL_MAX_ATTEMPT; i++) {
+		if (readl(hqvdp->regs + HQVDP_MBX_INFO_XP70)
+				& INFO_XP70_FW_READY)
+			break;
+		msleep(POLL_DELAY_MS);
+	}
+	if (i == POLL_MAX_ATTEMPT) {
+		DRM_ERROR("Could not boot\n");
+		goto out;
+	}
+
+	/* Launch Vsync */
+	writel(SOFT_VSYNC_HW, hqvdp->regs + HQVDP_MBX_SOFT_VSYNC);
+
+	DRM_INFO("HQVDP XP70 started\n");
+out:
+	release_firmware(firmware);
+}
+
+int sti_hqvdp_bind(struct device *dev, struct device *master, void *data)
+{
+	struct sti_hqvdp *hqvdp = dev_get_drvdata(dev);
+	struct drm_device *drm_dev = data;
+	struct sti_layer *layer;
+	int err;
+
+	DRM_DEBUG_DRIVER("\n");
+
+	hqvdp->drm_dev = drm_dev;
+
+	/* Request for firmware */
+	err = request_firmware_nowait(THIS_MODULE, FW_ACTION_HOTPLUG,
+				HQVDP_FMW_NAME,	hqvdp->dev,
+				GFP_KERNEL, hqvdp, sti_hqvdp_start_xp70);
+	if (err) {
+		DRM_ERROR("Can't get HQVDP firmware\n");
+		return err;
+	}
+
+	layer = sti_layer_create(hqvdp->dev, STI_HQVDP_0, hqvdp->regs);
+	if (!layer) {
+		DRM_ERROR("Can't create HQVDP plane\n");
+		return -ENOMEM;
+	}
+
+	sti_drm_plane_init(drm_dev, layer, 1, DRM_PLANE_TYPE_OVERLAY);
+
+	return 0;
+}
+
+static void sti_hqvdp_unbind(struct device *dev,
+		struct device *master, void *data)
+{
+	/* do nothing */
+}
+
+static const struct component_ops sti_hqvdp_ops = {
+	.bind = sti_hqvdp_bind,
+	.unbind = sti_hqvdp_unbind,
+};
+
+static int sti_hqvdp_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *vtg_np;
+	struct sti_hqvdp *hqvdp;
+	struct resource *res;
+
+	DRM_DEBUG_DRIVER("\n");
+
+	hqvdp = devm_kzalloc(dev, sizeof(*hqvdp), GFP_KERNEL);
+	if (!hqvdp) {
+		DRM_ERROR("Failed to allocate HQVDP context\n");
+		return -ENOMEM;
+	}
+
+	hqvdp->dev = dev;
+
+	/* Get Memory resources */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		DRM_ERROR("Get memory resource failed\n");
+		return -ENXIO;
+	}
+	hqvdp->regs = devm_ioremap(dev, res->start, resource_size(res));
+	if (hqvdp->regs == NULL) {
+		DRM_ERROR("Register mapping failed\n");
+		return -ENXIO;
+	}
+
+	/* Get clock resources */
+	hqvdp->clk = devm_clk_get(dev, "hqvdp");
+	hqvdp->clk_pix_main = devm_clk_get(dev, "pix_main");
+	if (IS_ERR(hqvdp->clk) || IS_ERR(hqvdp->clk)) {
+		DRM_ERROR("Cannot get clocks\n");
+		return -ENXIO;
+	}
+
+	/* Get reset resources */
+	hqvdp->reset = devm_reset_control_get(dev, "hqvdp");
+	if (!IS_ERR(hqvdp->reset))
+		reset_control_deassert(hqvdp->reset);
+
+	vtg_np = of_parse_phandle(pdev->dev.of_node, "st,vtg", 0);
+	if (vtg_np)
+		hqvdp->vtg = of_vtg_find(vtg_np);
+
+	platform_set_drvdata(pdev, hqvdp);
+
+	return component_add(&pdev->dev, &sti_hqvdp_ops);
+}
+
+static int sti_hqvdp_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &sti_hqvdp_ops);
+	return 0;
+}
+
+static struct of_device_id hqvdp_of_match[] = {
+	{ .compatible = "st,stih407-hqvdp", },
+	{ /* end node */ }
+};
+MODULE_DEVICE_TABLE(of, hqvdp_of_match);
+
+struct platform_driver sti_hqvdp_driver = {
+	.driver = {
+		.name = "sti-hqvdp",
+		.owner = THIS_MODULE,
+		.of_match_table = hqvdp_of_match,
+	},
+	.probe = sti_hqvdp_probe,
+	.remove = sti_hqvdp_remove,
+};
+
+module_platform_driver(sti_hqvdp_driver);
+
+MODULE_AUTHOR("Benjamin Gaignard <benjamin.gaignard@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics SoC DRM driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpu/drm/sti/sti_hqvdp.h b/drivers/gpu/drm/sti/sti_hqvdp.h
new file mode 100644
index 000000000000..cd5ecd0a6dea
--- /dev/null
+++ b/drivers/gpu/drm/sti/sti_hqvdp.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2014
+ * Authors: Fabien Dessenne <fabien.dessenne@st.com> for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#ifndef _STI_HQVDP_H_
+#define _STI_HQVDP_H_
+
+struct sti_layer *sti_hqvdp_create(struct device *dev);
+
+#endif
diff --git a/drivers/gpu/drm/sti/sti_hqvdp_lut.h b/drivers/gpu/drm/sti/sti_hqvdp_lut.h
new file mode 100644
index 000000000000..619af7f4384e
--- /dev/null
+++ b/drivers/gpu/drm/sti/sti_hqvdp_lut.h
@@ -0,0 +1,373 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2014
+ * Authors: Fabien Dessenne <fabien.dessenne@st.com> for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#ifndef _STI_HQVDP_LUT_H_
+#define _STI_HQVDP_LUT_H_
+
+#define NB_COEF                 128
+
+#define SHIFT_LUT_A_LEGACY      8
+#define SHIFT_LUT_B             8
+#define SHIFT_LUT_C_Y_LEGACY    8
+#define SHIFT_LUT_C_C_LEGACY    8
+#define SHIFT_LUT_D_Y_LEGACY    8
+#define SHIFT_LUT_D_C_LEGACY    8
+#define SHIFT_LUT_E_Y_LEGACY    8
+#define SHIFT_LUT_E_C_LEGACY    8
+#define SHIFT_LUT_F_Y_LEGACY    8
+#define SHIFT_LUT_F_C_LEGACY    8
+
+static const u32 coef_lut_a_legacy[NB_COEF] = {
+	0x0000ffff, 0x00010000, 0x000100ff, 0x00000000,
+	0x00000000, 0x00050000, 0xfffc00ff, 0x00000000,
+	0x00000000, 0x00090000, 0xfff900fe, 0x00000000,
+	0x00000000, 0x0010ffff, 0xfff600fb, 0x00000000,
+	0x00000000, 0x0017fffe, 0xfff400f7, 0x00000000,
+	0x00000000, 0x001ffffd, 0xfff200f2, 0x00000000,
+	0x00000000, 0x0027fffc, 0xfff100ec, 0x00000000,
+	0x00000000, 0x0030fffb, 0xfff000e5, 0x00000000,
+	0x00000000, 0x003afffa, 0xffee00de, 0x00000000,
+	0x00000000, 0x0044fff9, 0xffed00d6, 0x00000000,
+	0x00000000, 0x004efff8, 0xffed00cd, 0x00000000,
+	0x00000000, 0x0059fff6, 0xffed00c4, 0x00000000,
+	0x00000000, 0x0064fff5, 0xffed00ba, 0x00000000,
+	0x00000000, 0x006ffff3, 0xffee00b0, 0x00000000,
+	0x00000000, 0x007afff2, 0xffee00a6, 0x00000000,
+	0x00000000, 0x0085fff1, 0xffef009b, 0x00000000,
+	0x00000000, 0x0090fff0, 0xfff00090, 0x00000000,
+	0x00000000, 0x009bffef, 0xfff10085, 0x00000000,
+	0x00000000, 0x00a6ffee, 0xfff2007a, 0x00000000,
+	0x00000000, 0x00b0ffee, 0xfff3006f, 0x00000000,
+	0x00000000, 0x00baffed, 0xfff50064, 0x00000000,
+	0x00000000, 0x00c4ffed, 0xfff60059, 0x00000000,
+	0x00000000, 0x00cdffed, 0xfff8004e, 0x00000000,
+	0x00000000, 0x00d6ffed, 0xfff90044, 0x00000000,
+	0x00000000, 0x00deffee, 0xfffa003a, 0x00000000,
+	0x00000000, 0x00e5fff0, 0xfffb0030, 0x00000000,
+	0x00000000, 0x00ecfff1, 0xfffc0027, 0x00000000,
+	0x00000000, 0x00f2fff2, 0xfffd001f, 0x00000000,
+	0x00000000, 0x00f7fff4, 0xfffe0017, 0x00000000,
+	0x00000000, 0x00fbfff6, 0xffff0010, 0x00000000,
+	0x00000000, 0x00fefff9, 0x00000009, 0x00000000,
+	0x00000000, 0x00fffffc, 0x00000005, 0x00000000
+};
+
+static const u32 coef_lut_b[NB_COEF] = {
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000,
+	0x00000000, 0x00000000, 0x00000100, 0x00000000
+};
+
+static const u32 coef_lut_c_y_legacy[NB_COEF] = {
+	0x00060004, 0x0038ffe1, 0x003800be, 0x0006ffe1,
+	0x00050005, 0x0042ffe1, 0x003800b3, 0x0007ffe1,
+	0x00040006, 0x0046ffe1, 0x003300b2, 0x0008ffe2,
+	0x00030007, 0x004cffe1, 0x002e00b1, 0x0008ffe2,
+	0x00020006, 0x0051ffe2, 0x002900b0, 0x0009ffe3,
+	0x00010008, 0x0056ffe2, 0x002400ae, 0x0009ffe4,
+	0xffff0008, 0x005cffe3, 0x001f00ad, 0x000affe4,
+	0xfffe0008, 0x0062ffe4, 0x001a00ab, 0x000affe5,
+	0xfffd000a, 0x0066ffe5, 0x001500a8, 0x000bffe6,
+	0xfffc0009, 0x006bffe7, 0x001100a5, 0x000bffe8,
+	0xfffa000a, 0x0070ffe8, 0x000d00a3, 0x000bffe9,
+	0xfff9000b, 0x0076ffea, 0x0008009f, 0x000bffea,
+	0xfff7000b, 0x007affec, 0x0005009b, 0x000cffec,
+	0xfff6000b, 0x007effef, 0x00010098, 0x000cffed,
+	0xfff4000b, 0x0084fff1, 0xfffd0095, 0x000cffee,
+	0xfff3000b, 0x0088fff4, 0xfffa0090, 0x000cfff0,
+	0xfff1000b, 0x008dfff7, 0xfff7008d, 0x000bfff1,
+	0xfff0000c, 0x0090fffa, 0xfff40088, 0x000bfff3,
+	0xffee000c, 0x0095fffd, 0xfff10084, 0x000bfff4,
+	0xffed000c, 0x00980001, 0xffef007e, 0x000bfff6,
+	0xffec000c, 0x009b0005, 0xffec007a, 0x000bfff7,
+	0xffea000b, 0x009f0008, 0xffea0076, 0x000bfff9,
+	0xffe9000b, 0x00a3000d, 0xffe80070, 0x000afffa,
+	0xffe8000b, 0x00a50011, 0xffe7006b, 0x0009fffc,
+	0xffe6000b, 0x00a80015, 0xffe50066, 0x000afffd,
+	0xffe5000a, 0x00ab001a, 0xffe40062, 0x0008fffe,
+	0xffe4000a, 0x00ad001f, 0xffe3005c, 0x0008ffff,
+	0xffe40009, 0x00ae0024, 0xffe20056, 0x00080001,
+	0xffe30009, 0x00b00029, 0xffe20051, 0x00060002,
+	0xffe20008, 0x00b1002e, 0xffe1004c, 0x00070003,
+	0xffe20008, 0x00b20033, 0xffe10046, 0x00060004,
+	0xffe10007, 0x00b30038, 0xffe10042, 0x00050005
+};
+
+static const u32 coef_lut_c_c_legacy[NB_COEF] = {
+	0x0001fff3, 0x003afffb, 0x003a00a1, 0x0001fffb,
+	0x0001fff5, 0x0041fffb, 0x0038009a, 0x0001fffb,
+	0x0001fff5, 0x0046fffb, 0x00340099, 0x0001fffb,
+	0x0001fff7, 0x0049fffb, 0x00300098, 0x0001fffb,
+	0x0001fff9, 0x004cfffb, 0x002d0096, 0x0001fffb,
+	0x0001fffa, 0x004ffffc, 0x00290095, 0x0001fffb,
+	0x0001fff9, 0x0054fffd, 0x00250093, 0x0001fffc,
+	0x0001fffa, 0x0058fffd, 0x00220092, 0x0000fffc,
+	0x0001fffb, 0x005bfffe, 0x001f0090, 0x0000fffc,
+	0x0001fffd, 0x005effff, 0x001c008c, 0x0000fffd,
+	0x0001fffd, 0x00620000, 0x0019008a, 0x0000fffd,
+	0x0001fffe, 0x00660001, 0x00160088, 0xfffffffd,
+	0x0000fffe, 0x006a0003, 0x00130085, 0xfffffffe,
+	0x0000fffe, 0x006e0004, 0x00100083, 0xfffffffe,
+	0x0000fffe, 0x00710006, 0x000e007f, 0xffffffff,
+	0x0000fffe, 0x00750008, 0x000c007c, 0xfffeffff,
+	0xfffffffe, 0x0079000a, 0x000a0079, 0xfffeffff,
+	0xfffffffe, 0x007c000c, 0x00080075, 0xfffe0000,
+	0xffffffff, 0x007f000e, 0x00060071, 0xfffe0000,
+	0xfffeffff, 0x00830010, 0x0004006e, 0xfffe0000,
+	0xfffeffff, 0x00850013, 0x0003006a, 0xfffe0000,
+	0xfffdffff, 0x00880016, 0x00010066, 0xfffe0001,
+	0xfffd0000, 0x008a0019, 0x00000062, 0xfffd0001,
+	0xfffd0000, 0x008c001c, 0xffff005e, 0xfffd0001,
+	0xfffc0000, 0x0090001f, 0xfffe005b, 0xfffb0001,
+	0xfffc0000, 0x00920022, 0xfffd0058, 0xfffa0001,
+	0xfffc0001, 0x00930025, 0xfffd0054, 0xfff90001,
+	0xfffb0001, 0x00950029, 0xfffc004f, 0xfffa0001,
+	0xfffb0001, 0x0096002d, 0xfffb004c, 0xfff90001,
+	0xfffb0001, 0x00980030, 0xfffb0049, 0xfff70001,
+	0xfffb0001, 0x00990034, 0xfffb0046, 0xfff50001,
+	0xfffb0001, 0x009a0038, 0xfffb0041, 0xfff50001
+};
+
+static const u32 coef_lut_d_y_legacy[NB_COEF] = {
+	0xfff80009, 0x0046ffec, 0x004600a3, 0xfff8ffec,
+	0xfff70009, 0x004effed, 0x0044009d, 0xfff9ffeb,
+	0xfff6000a, 0x0052ffee, 0x003f009d, 0xfffaffea,
+	0xfff50009, 0x0057ffef, 0x003b009d, 0xfffbffe9,
+	0xfff50008, 0x005bfff0, 0x0037009c, 0xfffcffe9,
+	0xfff40008, 0x005ffff2, 0x0033009b, 0xfffcffe9,
+	0xfff30007, 0x0064fff3, 0x002f009b, 0xfffdffe8,
+	0xfff20007, 0x0068fff5, 0x002b0099, 0xfffeffe8,
+	0xfff10008, 0x006bfff7, 0x00270097, 0xffffffe8,
+	0xfff00007, 0x006ffff9, 0x00230097, 0xffffffe8,
+	0xffef0006, 0x0073fffb, 0x00200095, 0x0000ffe8,
+	0xffee0005, 0x0077fffe, 0x001c0093, 0x0000ffe9,
+	0xffee0005, 0x007a0000, 0x00180091, 0x0001ffe9,
+	0xffed0005, 0x007d0003, 0x0015008e, 0x0002ffe9,
+	0xffec0005, 0x00800006, 0x0012008b, 0x0002ffea,
+	0xffeb0004, 0x00840008, 0x000e008a, 0x0003ffea,
+	0xffeb0003, 0x0087000b, 0x000b0087, 0x0003ffeb,
+	0xffea0003, 0x008a000e, 0x00080084, 0x0004ffeb,
+	0xffea0002, 0x008b0012, 0x00060080, 0x0005ffec,
+	0xffe90002, 0x008e0015, 0x0003007d, 0x0005ffed,
+	0xffe90001, 0x00910018, 0x0000007a, 0x0005ffee,
+	0xffe90000, 0x0093001c, 0xfffe0077, 0x0005ffee,
+	0xffe80000, 0x00950020, 0xfffb0073, 0x0006ffef,
+	0xffe8ffff, 0x00970023, 0xfff9006f, 0x0007fff0,
+	0xffe8ffff, 0x00970027, 0xfff7006b, 0x0008fff1,
+	0xffe8fffe, 0x0099002b, 0xfff50068, 0x0007fff2,
+	0xffe8fffd, 0x009b002f, 0xfff30064, 0x0007fff3,
+	0xffe9fffc, 0x009b0033, 0xfff2005f, 0x0008fff4,
+	0xffe9fffc, 0x009c0037, 0xfff0005b, 0x0008fff5,
+	0xffe9fffb, 0x009d003b, 0xffef0057, 0x0009fff5,
+	0xffeafffa, 0x009d003f, 0xffee0052, 0x000afff6,
+	0xffebfff9, 0x009d0044, 0xffed004e, 0x0009fff7
+};
+
+static const u32 coef_lut_d_c_legacy[NB_COEF] = {
+	0xfffeffff, 0x003fffff, 0x003f0089, 0xfffeffff,
+	0xfffe0000, 0x00460000, 0x0042007d, 0xfffffffe,
+	0xfffe0000, 0x00490001, 0x003f007d, 0xfffffffd,
+	0xfffd0001, 0x004b0002, 0x003c007d, 0x0000fffc,
+	0xfffd0001, 0x004e0003, 0x0039007c, 0x0000fffc,
+	0xfffc0001, 0x00510005, 0x0036007c, 0x0000fffb,
+	0xfffc0001, 0x00540006, 0x0033007b, 0x0001fffa,
+	0xfffc0003, 0x00550008, 0x00310078, 0x0001fffa,
+	0xfffb0003, 0x00580009, 0x002e0078, 0x0001fffa,
+	0xfffb0002, 0x005b000b, 0x002b0077, 0x0002fff9,
+	0xfffa0003, 0x005e000d, 0x00280075, 0x0002fff9,
+	0xfffa0002, 0x0060000f, 0x00260074, 0x0002fff9,
+	0xfffa0004, 0x00610011, 0x00230072, 0x0002fff9,
+	0xfffa0004, 0x00640013, 0x00200070, 0x0002fff9,
+	0xfff90004, 0x00660015, 0x001e006e, 0x0003fff9,
+	0xfff90004, 0x00680017, 0x001c006c, 0x0003fff9,
+	0xfff90003, 0x006b0019, 0x0019006b, 0x0003fff9,
+	0xfff90003, 0x006c001c, 0x00170068, 0x0004fff9,
+	0xfff90003, 0x006e001e, 0x00150066, 0x0004fff9,
+	0xfff90002, 0x00700020, 0x00130064, 0x0004fffa,
+	0xfff90002, 0x00720023, 0x00110061, 0x0004fffa,
+	0xfff90002, 0x00740026, 0x000f0060, 0x0002fffa,
+	0xfff90002, 0x00750028, 0x000d005e, 0x0003fffa,
+	0xfff90002, 0x0077002b, 0x000b005b, 0x0002fffb,
+	0xfffa0001, 0x0078002e, 0x00090058, 0x0003fffb,
+	0xfffa0001, 0x00780031, 0x00080055, 0x0003fffc,
+	0xfffa0001, 0x007b0033, 0x00060054, 0x0001fffc,
+	0xfffb0000, 0x007c0036, 0x00050051, 0x0001fffc,
+	0xfffc0000, 0x007c0039, 0x0003004e, 0x0001fffd,
+	0xfffc0000, 0x007d003c, 0x0002004b, 0x0001fffd,
+	0xfffdffff, 0x007d003f, 0x00010049, 0x0000fffe,
+	0xfffeffff, 0x007d0042, 0x00000046, 0x0000fffe
+};
+
+static const u32 coef_lut_e_y_legacy[NB_COEF] = {
+	0xfff10001, 0x00490004, 0x00490083, 0xfff10004,
+	0xfff10000, 0x00500006, 0x004b007b, 0xfff10002,
+	0xfff10000, 0x00530007, 0x0048007b, 0xfff10001,
+	0xfff10000, 0x00550009, 0x0046007a, 0xfff10000,
+	0xfff1fffe, 0x0058000b, 0x0043007b, 0xfff2fffe,
+	0xfff1ffff, 0x005a000d, 0x0040007a, 0xfff2fffd,
+	0xfff1fffd, 0x005d000f, 0x003e007a, 0xfff2fffc,
+	0xfff1fffd, 0x005f0011, 0x003b0079, 0xfff3fffb,
+	0xfff1fffc, 0x00610013, 0x00390079, 0xfff3fffa,
+	0xfff1fffb, 0x00640015, 0x00360079, 0xfff3fff9,
+	0xfff1fffa, 0x00660017, 0x00340078, 0xfff4fff8,
+	0xfff1fffb, 0x00680019, 0x00310077, 0xfff4fff7,
+	0xfff2fff9, 0x006a001b, 0x002f0076, 0xfff5fff6,
+	0xfff2fff9, 0x006c001e, 0x002c0075, 0xfff5fff5,
+	0xfff2fff9, 0x006d0020, 0x002a0073, 0xfff6fff5,
+	0xfff3fff7, 0x00700022, 0x00270073, 0xfff6fff4,
+	0xfff3fff7, 0x00710025, 0x00250071, 0xfff7fff3,
+	0xfff4fff6, 0x00730027, 0x00220070, 0xfff7fff3,
+	0xfff5fff6, 0x0073002a, 0x0020006d, 0xfff9fff2,
+	0xfff5fff5, 0x0075002c, 0x001e006c, 0xfff9fff2,
+	0xfff6fff5, 0x0076002f, 0x001b006a, 0xfff9fff2,
+	0xfff7fff4, 0x00770031, 0x00190068, 0xfffbfff1,
+	0xfff8fff4, 0x00780034, 0x00170066, 0xfffafff1,
+	0xfff9fff3, 0x00790036, 0x00150064, 0xfffbfff1,
+	0xfffafff3, 0x00790039, 0x00130061, 0xfffcfff1,
+	0xfffbfff3, 0x0079003b, 0x0011005f, 0xfffdfff1,
+	0xfffcfff2, 0x007a003e, 0x000f005d, 0xfffdfff1,
+	0xfffdfff2, 0x007a0040, 0x000d005a, 0xfffffff1,
+	0xfffefff2, 0x007b0043, 0x000b0058, 0xfffefff1,
+	0x0000fff1, 0x007a0046, 0x00090055, 0x0000fff1,
+	0x0001fff1, 0x007b0048, 0x00070053, 0x0000fff1,
+	0x0002fff1, 0x007b004b, 0x00060050, 0x0000fff1
+};
+
+static const u32 coef_lut_e_c_legacy[NB_COEF] = {
+	0xfffa0001, 0x003f0010, 0x003f006d, 0xfffa0010,
+	0xfffb0002, 0x00440011, 0x00440062, 0xfffa000e,
+	0xfffb0001, 0x00460013, 0x00420062, 0xfffa000d,
+	0xfffb0000, 0x00480014, 0x00410062, 0xfffa000c,
+	0xfffb0001, 0x00490015, 0x003f0061, 0xfffb000b,
+	0xfffb0000, 0x004b0017, 0x003d0061, 0xfffb000a,
+	0xfffb0000, 0x004d0018, 0x003b0062, 0xfffb0008,
+	0xfffcffff, 0x004f001a, 0x00390061, 0xfffb0007,
+	0xfffc0000, 0x004f001c, 0x00380060, 0xfffb0006,
+	0xfffcffff, 0x0052001d, 0x00360060, 0xfffb0005,
+	0xfffdfffe, 0x0053001f, 0x00340060, 0xfffb0004,
+	0xfffdfffe, 0x00540021, 0x0032005e, 0xfffc0004,
+	0xfffeffff, 0x00550022, 0x0030005d, 0xfffc0003,
+	0xfffeffff, 0x00560024, 0x002f005c, 0xfffc0002,
+	0xfffffffd, 0x00580026, 0x002d005c, 0xfffc0001,
+	0xfffffffd, 0x005a0027, 0x002b005c, 0xfffc0000,
+	0x0000fffd, 0x005a0029, 0x0029005a, 0xfffd0000,
+	0x0000fffc, 0x005c002b, 0x0027005a, 0xfffdffff,
+	0x0001fffc, 0x005c002d, 0x00260058, 0xfffdffff,
+	0x0002fffc, 0x005c002f, 0x00240056, 0xfffffffe,
+	0x0003fffc, 0x005d0030, 0x00220055, 0xfffffffe,
+	0x0004fffc, 0x005e0032, 0x00210054, 0xfffefffd,
+	0x0004fffb, 0x00600034, 0x001f0053, 0xfffefffd,
+	0x0005fffb, 0x00600036, 0x001d0052, 0xfffffffc,
+	0x0006fffb, 0x00600038, 0x001c004f, 0x0000fffc,
+	0x0007fffb, 0x00610039, 0x001a004f, 0xfffffffc,
+	0x0008fffb, 0x0062003b, 0x0018004d, 0x0000fffb,
+	0x000afffb, 0x0061003d, 0x0017004b, 0x0000fffb,
+	0x000bfffb, 0x0061003f, 0x00150049, 0x0001fffb,
+	0x000cfffa, 0x00620041, 0x00140048, 0x0000fffb,
+	0x000dfffa, 0x00620042, 0x00130046, 0x0001fffb,
+	0x000efffa, 0x00620044, 0x00110044, 0x0002fffb
+};
+
+static const u32 coef_lut_f_y_legacy[NB_COEF] = {
+	0xfff6fff0, 0x00490012, 0x0049006e, 0xfff60012,
+	0xfff7fff1, 0x004e0013, 0x00490068, 0xfff60010,
+	0xfff7fff2, 0x004f0015, 0x00470067, 0xfff6000f,
+	0xfff7fff5, 0x004f0017, 0x00450065, 0xfff6000e,
+	0xfff8fff5, 0x00500018, 0x00440065, 0xfff6000c,
+	0xfff8fff6, 0x0051001a, 0x00420064, 0xfff6000b,
+	0xfff8fff6, 0x0052001c, 0x00400064, 0xfff6000a,
+	0xfff9fff6, 0x0054001d, 0x003e0064, 0xfff60008,
+	0xfff9fff8, 0x0054001f, 0x003c0063, 0xfff60007,
+	0xfffafff8, 0x00550021, 0x003a0062, 0xfff60006,
+	0xfffbfff7, 0x00560022, 0x00390062, 0xfff60005,
+	0xfffbfff8, 0x00570024, 0x00370061, 0xfff60004,
+	0xfffcfff8, 0x00580026, 0x00350060, 0xfff60003,
+	0xfffdfff8, 0x00590028, 0x0033005f, 0xfff60002,
+	0xfffdfff7, 0x005b002a, 0x0031005f, 0xfff60001,
+	0xfffefff7, 0x005c002c, 0x002f005e, 0xfff60000,
+	0xfffffff6, 0x005e002d, 0x002d005e, 0xfff6ffff,
+	0x0000fff6, 0x005e002f, 0x002c005c, 0xfff7fffe,
+	0x0001fff6, 0x005f0031, 0x002a005b, 0xfff7fffd,
+	0x0002fff6, 0x005f0033, 0x00280059, 0xfff8fffd,
+	0x0003fff6, 0x00600035, 0x00260058, 0xfff8fffc,
+	0x0004fff6, 0x00610037, 0x00240057, 0xfff8fffb,
+	0x0005fff6, 0x00620039, 0x00220056, 0xfff7fffb,
+	0x0006fff6, 0x0062003a, 0x00210055, 0xfff8fffa,
+	0x0007fff6, 0x0063003c, 0x001f0054, 0xfff8fff9,
+	0x0008fff6, 0x0064003e, 0x001d0054, 0xfff6fff9,
+	0x000afff6, 0x00640040, 0x001c0052, 0xfff6fff8,
+	0x000bfff6, 0x00640042, 0x001a0051, 0xfff6fff8,
+	0x000cfff6, 0x00650044, 0x00180050, 0xfff5fff8,
+	0x000efff6, 0x00650045, 0x0017004f, 0xfff5fff7,
+	0x000ffff6, 0x00670047, 0x0015004f, 0xfff2fff7,
+	0x0010fff6, 0x00680049, 0x0013004e, 0xfff1fff7
+};
+
+static const u32 coef_lut_f_c_legacy[NB_COEF] = {
+	0x0000fffb, 0x003a001a, 0x003a005d, 0x0000001a,
+	0x0001fffb, 0x003f001b, 0x00400051, 0x00000019,
+	0x0001fffc, 0x0040001c, 0x003f0051, 0x00000017,
+	0x0002fffb, 0x0042001d, 0x003e0051, 0xffff0016,
+	0x0002fffb, 0x0043001e, 0x003d0051, 0xffff0015,
+	0x0003fffc, 0x00430020, 0x003b0050, 0xffff0014,
+	0x0003fffb, 0x00450021, 0x003a0051, 0xfffe0013,
+	0x0004fffc, 0x00450022, 0x00390050, 0xfffe0012,
+	0x0005fffc, 0x00460023, 0x0038004f, 0xfffe0011,
+	0x0005fffb, 0x00480025, 0x00360050, 0xfffd0010,
+	0x0006fffc, 0x00480026, 0x0035004f, 0xfffd000f,
+	0x0006fffc, 0x00490027, 0x0034004f, 0xfffd000e,
+	0x0007fffd, 0x00490028, 0x0033004e, 0xfffd000d,
+	0x0008fffc, 0x004a002a, 0x0031004d, 0xfffd000d,
+	0x0009fffd, 0x004a002b, 0x0030004d, 0xfffc000c,
+	0x0009fffc, 0x004c002c, 0x002f004d, 0xfffc000b,
+	0x000afffc, 0x004c002e, 0x002e004c, 0xfffc000a,
+	0x000bfffc, 0x004d002f, 0x002c004c, 0xfffc0009,
+	0x000cfffc, 0x004d0030, 0x002b004a, 0xfffd0009,
+	0x000dfffd, 0x004d0031, 0x002a004a, 0xfffc0008,
+	0x000dfffd, 0x004e0033, 0x00280049, 0xfffd0007,
+	0x000efffd, 0x004f0034, 0x00270049, 0xfffc0006,
+	0x000ffffd, 0x004f0035, 0x00260048, 0xfffc0006,
+	0x0010fffd, 0x00500036, 0x00250048, 0xfffb0005,
+	0x0011fffe, 0x004f0038, 0x00230046, 0xfffc0005,
+	0x0012fffe, 0x00500039, 0x00220045, 0xfffc0004,
+	0x0013fffe, 0x0051003a, 0x00210045, 0xfffb0003,
+	0x0014ffff, 0x0050003b, 0x00200043, 0xfffc0003,
+	0x0015ffff, 0x0051003d, 0x001e0043, 0xfffb0002,
+	0x0016ffff, 0x0051003e, 0x001d0042, 0xfffb0002,
+	0x00170000, 0x0051003f, 0x001c0040, 0xfffc0001,
+	0x00190000, 0x00510040, 0x001b003f, 0xfffb0001
+};
+
+#endif
diff --git a/drivers/gpu/drm/sti/sti_layer.c b/drivers/gpu/drm/sti/sti_layer.c
index 06a587c4f1bb..899104f9d4bc 100644
--- a/drivers/gpu/drm/sti/sti_layer.c
+++ b/drivers/gpu/drm/sti/sti_layer.c
@@ -11,7 +11,9 @@
 #include <drm/drm_fb_cma_helper.h>
 
 #include "sti_compositor.h"
+#include "sti_cursor.h"
 #include "sti_gdp.h"
+#include "sti_hqvdp.h"
 #include "sti_layer.h"
 #include "sti_vid.h"
 
@@ -32,10 +34,13 @@ const char *sti_layer_to_str(struct sti_layer *layer)
 		return "VID1";
 	case STI_CURSOR:
 		return "CURSOR";
+	case STI_HQVDP_0:
+		return "HQVDP0";
 	default:
 		return "<UNKNOWN LAYER>";
 	}
 }
+EXPORT_SYMBOL(sti_layer_to_str);
 
 struct sti_layer *sti_layer_create(struct device *dev, int desc,
 				   void __iomem *baseaddr)
@@ -50,6 +55,12 @@ struct sti_layer *sti_layer_create(struct device *dev, int desc,
 	case STI_VID:
 		layer = sti_vid_create(dev);
 		break;
+	case STI_CUR:
+		layer = sti_cursor_create(dev);
+		break;
+	case STI_VDP:
+		layer = sti_hqvdp_create(dev);
+		break;
 	}
 
 	if (!layer) {
@@ -67,8 +78,11 @@ struct sti_layer *sti_layer_create(struct device *dev, int desc,
 
 	return layer;
 }
+EXPORT_SYMBOL(sti_layer_create);
 
-int sti_layer_prepare(struct sti_layer *layer, struct drm_framebuffer *fb,
+int sti_layer_prepare(struct sti_layer *layer,
+		      struct drm_crtc *crtc,
+		      struct drm_framebuffer *fb,
 		      struct drm_display_mode *mode, int mixer_id,
 		      int dest_x, int dest_y, int dest_w, int dest_h,
 		      int src_x, int src_y, int src_w, int src_h)
@@ -88,6 +102,7 @@ int sti_layer_prepare(struct sti_layer *layer, struct drm_framebuffer *fb,
 		return 1;
 	}
 
+	layer->crtc = crtc;
 	layer->fb = fb;
 	layer->mode = mode;
 	layer->mixer_id = mixer_id;
@@ -100,6 +115,7 @@ int sti_layer_prepare(struct sti_layer *layer, struct drm_framebuffer *fb,
 	layer->src_w = src_w;
 	layer->src_h = src_h;
 	layer->format = fb->pixel_format;
+	layer->vaddr = cma_obj->vaddr;
 	layer->paddr = cma_obj->paddr;
 	for (i = 0; i < 4; i++) {
 		layer->pitches[i] = fb->pitches[i];
diff --git a/drivers/gpu/drm/sti/sti_layer.h b/drivers/gpu/drm/sti/sti_layer.h
index 198c3774cc12..ceff497f557e 100644
--- a/drivers/gpu/drm/sti/sti_layer.h
+++ b/drivers/gpu/drm/sti/sti_layer.h
@@ -22,7 +22,8 @@ enum sti_layer_type {
 	STI_GDP = 1 << STI_LAYER_TYPE_SHIFT,
 	STI_VID = 2 << STI_LAYER_TYPE_SHIFT,
 	STI_CUR = 3 << STI_LAYER_TYPE_SHIFT,
-	STI_BCK = 4 << STI_LAYER_TYPE_SHIFT
+	STI_BCK = 4 << STI_LAYER_TYPE_SHIFT,
+	STI_VDP = 5 << STI_LAYER_TYPE_SHIFT
 };
 
 enum sti_layer_id_of_type {
@@ -39,6 +40,7 @@ enum sti_layer_desc {
 	STI_GDP_3       = STI_GDP | STI_ID_3,
 	STI_VID_0       = STI_VID | STI_ID_0,
 	STI_VID_1       = STI_VID | STI_ID_1,
+	STI_HQVDP_0     = STI_VDP | STI_ID_0,
 	STI_CURSOR      = STI_CUR,
 	STI_BACK        = STI_BCK
 };
@@ -67,6 +69,7 @@ struct sti_layer_funcs {
  *
  * @plane:              drm plane it is bound to (if any)
  * @fb:                 drm fb it is bound to
+ * @crtc:               crtc it is bound to
  * @mode:               display mode
  * @desc:               layer type & id
  * @device:		driver device
@@ -82,11 +85,13 @@ struct sti_layer_funcs {
  * @format:             format
  * @pitches:            pitch of 'planes' (eg: Y, U, V)
  * @offsets:            offset of 'planes'
+ * @vaddr:              virtual address of the input buffer
  * @paddr:              physical address of the input buffer
  */
 struct sti_layer {
 	struct drm_plane plane;
 	struct drm_framebuffer *fb;
+	struct drm_crtc *crtc;
 	struct drm_display_mode *mode;
 	enum sti_layer_desc desc;
 	struct device *dev;
@@ -102,12 +107,15 @@ struct sti_layer {
 	uint32_t format;
 	unsigned int pitches[4];
 	unsigned int offsets[4];
+	void *vaddr;
 	dma_addr_t paddr;
 };
 
 struct sti_layer *sti_layer_create(struct device *dev, int desc,
 			void __iomem *baseaddr);
-int sti_layer_prepare(struct sti_layer *layer, struct drm_framebuffer *fb,
+int sti_layer_prepare(struct sti_layer *layer,
+			struct drm_crtc *crtc,
+			struct drm_framebuffer *fb,
 			struct drm_display_mode *mode,
 			int mixer_id,
 			int dest_x, int dest_y,
diff --git a/drivers/gpu/drm/sti/sti_mixer.c b/drivers/gpu/drm/sti/sti_mixer.c
index 79f369db9fb6..13a4b84deab6 100644
--- a/drivers/gpu/drm/sti/sti_mixer.c
+++ b/drivers/gpu/drm/sti/sti_mixer.c
@@ -45,6 +45,7 @@ static const u32 mixerColorSpaceMatIdentity[] = {
 #define GAM_CTL_GDP1_MASK  BIT(4)
 #define GAM_CTL_GDP2_MASK  BIT(5)
 #define GAM_CTL_GDP3_MASK  BIT(6)
+#define GAM_CTL_CURSOR_MASK BIT(9)
 
 const char *sti_mixer_to_str(struct sti_mixer *mixer)
 {
@@ -122,11 +123,15 @@ int sti_mixer_set_layer_depth(struct sti_mixer *mixer, struct sti_layer *layer)
 		layer_id = GAM_DEPTH_GDP3_ID;
 		break;
 	case STI_VID_0:
+	case STI_HQVDP_0:
 		layer_id = GAM_DEPTH_VID0_ID;
 		break;
 	case STI_VID_1:
 		layer_id = GAM_DEPTH_VID1_ID;
 		break;
+	case STI_CURSOR:
+		/* no need to set depth for cursor */
+		return 0;
 	default:
 		DRM_ERROR("Unknown layer %d\n", layer->desc);
 		return 1;
@@ -185,9 +190,12 @@ static u32 sti_mixer_get_layer_mask(struct sti_layer *layer)
 	case STI_GDP_3:
 		return GAM_CTL_GDP3_MASK;
 	case STI_VID_0:
+	case STI_HQVDP_0:
 		return GAM_CTL_VID0_MASK;
 	case STI_VID_1:
 		return GAM_CTL_VID1_MASK;
+	case STI_CURSOR:
+		return GAM_CTL_CURSOR_MASK;
 	default:
 		return 0;
 	}
@@ -215,6 +223,15 @@ int sti_mixer_set_layer_status(struct sti_mixer *mixer,
 	return 0;
 }
 
+void sti_mixer_clear_all_layers(struct sti_mixer *mixer)
+{
+	u32 val;
+
+	DRM_DEBUG_DRIVER("%s clear all layer\n", sti_mixer_to_str(mixer));
+	val = sti_mixer_reg_read(mixer, GAM_MIXER_CTL) & 0xFFFF0000;
+	sti_mixer_reg_write(mixer, GAM_MIXER_CTL, val);
+}
+
 void sti_mixer_set_matrix(struct sti_mixer *mixer)
 {
 	unsigned int i;
diff --git a/drivers/gpu/drm/sti/sti_mixer.h b/drivers/gpu/drm/sti/sti_mixer.h
index 874372102e52..b97282182908 100644
--- a/drivers/gpu/drm/sti/sti_mixer.h
+++ b/drivers/gpu/drm/sti/sti_mixer.h
@@ -23,6 +23,7 @@
  * @id: id of the mixer
  * @drm_crtc: crtc object link to the mixer
  * @pending_event: set if a flip event is pending on crtc
+ * @enabled: to know if the mixer is active or not
  */
 struct sti_mixer {
 	struct device *dev;
@@ -30,6 +31,7 @@ struct sti_mixer {
 	int id;
 	struct drm_crtc	drm_crtc;
 	struct drm_pending_vblank_event *pending_event;
+	bool enabled;
 };
 
 const char *sti_mixer_to_str(struct sti_mixer *mixer);
@@ -39,6 +41,7 @@ struct sti_mixer *sti_mixer_create(struct device *dev, int id,
 
 int sti_mixer_set_layer_status(struct sti_mixer *mixer,
 		struct sti_layer *layer, bool status);
+void sti_mixer_clear_all_layers(struct sti_mixer *mixer);
 int sti_mixer_set_layer_depth(struct sti_mixer *mixer, struct sti_layer *layer);
 int sti_mixer_active_video_area(struct sti_mixer *mixer,
 		struct drm_display_mode *mode);
diff --git a/drivers/gpu/drm/sti/sti_tvout.c b/drivers/gpu/drm/sti/sti_tvout.c
index b8afe490356a..cb924aa2b321 100644
--- a/drivers/gpu/drm/sti/sti_tvout.c
+++ b/drivers/gpu/drm/sti/sti_tvout.c
@@ -16,6 +16,8 @@
 #include <drm/drmP.h>
 #include <drm/drm_crtc_helper.h>
 
+#include "sti_drm_crtc.h"
+
 /* glue registers */
 #define TVO_CSC_MAIN_M0                  0x000
 #define TVO_CSC_MAIN_M1                  0x004
@@ -96,7 +98,7 @@
 
 #define TVO_SYNC_HD_DCS_SHIFT            8
 
-#define ENCODER_MAIN_CRTC_MASK           BIT(0)
+#define ENCODER_CRTC_MASK                (BIT(0) | BIT(1))
 
 /* enum listing the supported output data format */
 enum sti_tvout_video_out_type {
@@ -149,14 +151,15 @@ static void tvout_write(struct sti_tvout *tvout, u32 val, int offset)
  * Set the clipping mode of a VIP
  *
  * @tvout: tvout structure
+ * @reg: register to set
  * @cr_r:
  * @y_g:
  * @cb_b:
  */
-static void tvout_vip_set_color_order(struct sti_tvout *tvout,
+static void tvout_vip_set_color_order(struct sti_tvout *tvout, int reg,
 				      u32 cr_r, u32 y_g, u32 cb_b)
 {
-	u32 val = tvout_read(tvout, TVO_VIP_HDMI);
+	u32 val = tvout_read(tvout, reg);
 
 	val &= ~(TVO_VIP_REORDER_MASK << TVO_VIP_REORDER_R_SHIFT);
 	val &= ~(TVO_VIP_REORDER_MASK << TVO_VIP_REORDER_G_SHIFT);
@@ -165,52 +168,58 @@ static void tvout_vip_set_color_order(struct sti_tvout *tvout,
 	val |= y_g << TVO_VIP_REORDER_G_SHIFT;
 	val |= cb_b << TVO_VIP_REORDER_B_SHIFT;
 
-	tvout_write(tvout, val, TVO_VIP_HDMI);
+	tvout_write(tvout, val, reg);
 }
 
 /**
  * Set the clipping mode of a VIP
  *
  * @tvout: tvout structure
+ * @reg: register to set
  * @range: clipping range
  */
-static void tvout_vip_set_clip_mode(struct sti_tvout *tvout, u32 range)
+static void tvout_vip_set_clip_mode(struct sti_tvout *tvout, int reg, u32 range)
 {
-	u32 val = tvout_read(tvout, TVO_VIP_HDMI);
+	u32 val = tvout_read(tvout, reg);
 
 	val &= ~(TVO_VIP_CLIP_MASK << TVO_VIP_CLIP_SHIFT);
 	val |= range << TVO_VIP_CLIP_SHIFT;
-	tvout_write(tvout, val, TVO_VIP_HDMI);
+	tvout_write(tvout, val, reg);
 }
 
 /**
  * Set the rounded value of a VIP
  *
  * @tvout: tvout structure
+ * @reg: register to set
  * @rnd: rounded val per component
  */
-static void tvout_vip_set_rnd(struct sti_tvout *tvout, u32 rnd)
+static void tvout_vip_set_rnd(struct sti_tvout *tvout, int reg, u32 rnd)
 {
-	u32 val = tvout_read(tvout, TVO_VIP_HDMI);
+	u32 val = tvout_read(tvout, reg);
 
 	val &= ~(TVO_VIP_RND_MASK << TVO_VIP_RND_SHIFT);
 	val |= rnd << TVO_VIP_RND_SHIFT;
-	tvout_write(tvout, val, TVO_VIP_HDMI);
+	tvout_write(tvout, val, reg);
 }
 
 /**
  * Select the VIP input
  *
  * @tvout: tvout structure
+ * @reg: register to set
+ * @main_path: main or auxiliary path
+ * @sel_input_logic_inverted: need to invert the logic
  * @sel_input: selected_input (main/aux + conv)
  */
 static void tvout_vip_set_sel_input(struct sti_tvout *tvout,
+				    int reg,
 				    bool main_path,
 				    bool sel_input_logic_inverted,
 				    enum sti_tvout_video_out_type video_out)
 {
 	u32 sel_input;
-	u32 val = tvout_read(tvout, TVO_VIP_HDMI);
+	u32 val = tvout_read(tvout, reg);
 
 	if (main_path)
 		sel_input = TVO_VIP_SEL_INPUT_MAIN;
@@ -232,22 +241,24 @@ static void tvout_vip_set_sel_input(struct sti_tvout *tvout,
 
 	val &= ~TVO_VIP_SEL_INPUT_MASK;
 	val |= sel_input;
-	tvout_write(tvout, val, TVO_VIP_HDMI);
+	tvout_write(tvout, val, reg);
 }
 
 /**
  * Select the input video signed or unsigned
  *
  * @tvout: tvout structure
+ * @reg: register to set
  * @in_vid_signed: used video input format
  */
-static void tvout_vip_set_in_vid_fmt(struct sti_tvout *tvout, u32 in_vid_fmt)
+static void tvout_vip_set_in_vid_fmt(struct sti_tvout *tvout,
+		int reg, u32 in_vid_fmt)
 {
-	u32 val = tvout_read(tvout, TVO_VIP_HDMI);
+	u32 val = tvout_read(tvout, reg);
 
 	val &= ~TVO_IN_FMT_SIGNED;
 	val |= in_vid_fmt;
-	tvout_write(tvout, val, TVO_MAIN_IN_VID_FORMAT);
+	tvout_write(tvout, val, reg);
 }
 
 /**
@@ -261,6 +272,7 @@ static void tvout_hdmi_start(struct sti_tvout *tvout, bool main_path)
 {
 	struct device_node *node = tvout->dev->of_node;
 	bool sel_input_logic_inverted = false;
+	u32 tvo_in_vid_format;
 
 	dev_dbg(tvout->dev, "%s\n", __func__);
 
@@ -268,33 +280,36 @@ static void tvout_hdmi_start(struct sti_tvout *tvout, bool main_path)
 		DRM_DEBUG_DRIVER("main vip for hdmi\n");
 		/* select the input sync for hdmi = VTG set 1 */
 		tvout_write(tvout, TVO_SYNC_MAIN_VTG_SET_1, TVO_HDMI_SYNC_SEL);
+		tvo_in_vid_format = TVO_MAIN_IN_VID_FORMAT;
 	} else {
 		DRM_DEBUG_DRIVER("aux vip for hdmi\n");
 		/* select the input sync for hdmi = VTG set 1 */
 		tvout_write(tvout, TVO_SYNC_AUX_VTG_SET_1, TVO_HDMI_SYNC_SEL);
+		tvo_in_vid_format = TVO_AUX_IN_VID_FORMAT;
 	}
 
 	/* set color channel order */
-	tvout_vip_set_color_order(tvout,
+	tvout_vip_set_color_order(tvout, TVO_VIP_HDMI,
 				  TVO_VIP_REORDER_CR_R_SEL,
 				  TVO_VIP_REORDER_Y_G_SEL,
 				  TVO_VIP_REORDER_CB_B_SEL);
 
 	/* set clipping mode (Limited range RGB/Y) */
-	tvout_vip_set_clip_mode(tvout, TVO_VIP_CLIP_LIMITED_RANGE_RGB_Y);
+	tvout_vip_set_clip_mode(tvout, TVO_VIP_HDMI,
+			TVO_VIP_CLIP_LIMITED_RANGE_RGB_Y);
 
 	/* set round mode (rounded to 8-bit per component) */
-	tvout_vip_set_rnd(tvout, TVO_VIP_RND_8BIT_ROUNDED);
+	tvout_vip_set_rnd(tvout, TVO_VIP_HDMI, TVO_VIP_RND_8BIT_ROUNDED);
 
 	if (of_device_is_compatible(node, "st,stih407-tvout")) {
 		/* set input video format */
-		tvout_vip_set_in_vid_fmt(tvout->regs + TVO_MAIN_IN_VID_FORMAT,
-					 TVO_IN_FMT_SIGNED);
+		tvout_vip_set_in_vid_fmt(tvout, tvo_in_vid_format,
+					TVO_IN_FMT_SIGNED);
 		sel_input_logic_inverted = true;
 	}
 
 	/* input selection */
-	tvout_vip_set_sel_input(tvout, main_path,
+	tvout_vip_set_sel_input(tvout, TVO_VIP_HDMI, main_path,
 			sel_input_logic_inverted, STI_TVOUT_VIDEO_OUT_RGB);
 }
 
@@ -309,48 +324,47 @@ static void tvout_hda_start(struct sti_tvout *tvout, bool main_path)
 {
 	struct device_node *node = tvout->dev->of_node;
 	bool sel_input_logic_inverted = false;
+	u32 tvo_in_vid_format;
+	int val;
 
 	dev_dbg(tvout->dev, "%s\n", __func__);
 
-	if (!main_path) {
-		DRM_ERROR("HD Analog on aux not implemented\n");
-		return;
+	if (main_path) {
+		val = TVO_SYNC_MAIN_VTG_SET_2 << TVO_SYNC_HD_DCS_SHIFT;
+		val |= TVO_SYNC_MAIN_VTG_SET_3;
+		tvout_write(tvout, val, TVO_HD_SYNC_SEL);
+		tvo_in_vid_format = TVO_MAIN_IN_VID_FORMAT;
+	} else {
+		val = TVO_SYNC_AUX_VTG_SET_2 << TVO_SYNC_HD_DCS_SHIFT;
+		val |= TVO_SYNC_AUX_VTG_SET_3;
+		tvout_write(tvout, val, TVO_HD_SYNC_SEL);
+		tvo_in_vid_format = TVO_AUX_IN_VID_FORMAT;
 	}
 
-	DRM_DEBUG_DRIVER("main vip for HDF\n");
-
 	/* set color channel order */
-	tvout_vip_set_color_order(tvout->regs + TVO_VIP_HDF,
+	tvout_vip_set_color_order(tvout, TVO_VIP_HDF,
 				  TVO_VIP_REORDER_CR_R_SEL,
 				  TVO_VIP_REORDER_Y_G_SEL,
 				  TVO_VIP_REORDER_CB_B_SEL);
 
-	/* set clipping mode (Limited range RGB/Y) */
-	tvout_vip_set_clip_mode(tvout->regs + TVO_VIP_HDF,
-				TVO_VIP_CLIP_LIMITED_RANGE_CB_CR);
+	/* set clipping mode (EAV/SAV clipping) */
+	tvout_vip_set_clip_mode(tvout, TVO_VIP_HDF, TVO_VIP_CLIP_EAV_SAV);
 
 	/* set round mode (rounded to 10-bit per component) */
-	tvout_vip_set_rnd(tvout->regs + TVO_VIP_HDF, TVO_VIP_RND_10BIT_ROUNDED);
+	tvout_vip_set_rnd(tvout, TVO_VIP_HDF, TVO_VIP_RND_10BIT_ROUNDED);
 
 	if (of_device_is_compatible(node, "st,stih407-tvout")) {
 		/* set input video format */
-		tvout_vip_set_in_vid_fmt(tvout, TVO_IN_FMT_SIGNED);
+		tvout_vip_set_in_vid_fmt(tvout,
+			tvo_in_vid_format, TVO_IN_FMT_SIGNED);
 		sel_input_logic_inverted = true;
 	}
 
 	/* Input selection */
-	tvout_vip_set_sel_input(tvout->regs + TVO_VIP_HDF,
-				main_path,
+	tvout_vip_set_sel_input(tvout, TVO_VIP_HDF, main_path,
 				sel_input_logic_inverted,
 				STI_TVOUT_VIDEO_OUT_YUV);
 
-	/* select the input sync for HD analog = VTG set 3
-	 * and HD DCS = VTG set 2 */
-	tvout_write(tvout,
-		(TVO_SYNC_MAIN_VTG_SET_2 << TVO_SYNC_HD_DCS_SHIFT)
-		| TVO_SYNC_MAIN_VTG_SET_3,
-		TVO_HD_SYNC_SEL);
-
 	/* power up HD DAC */
 	tvout_write(tvout, 0, TVO_HD_DAC_CFG_OFF);
 }
@@ -392,7 +406,7 @@ static void sti_hda_encoder_commit(struct drm_encoder *encoder)
 {
 	struct sti_tvout *tvout = to_sti_tvout(encoder);
 
-	tvout_hda_start(tvout, true);
+	tvout_hda_start(tvout, sti_drm_crtc_is_main(encoder->crtc));
 }
 
 static void sti_hda_encoder_disable(struct drm_encoder *encoder)
@@ -429,7 +443,7 @@ static struct drm_encoder *sti_tvout_create_hda_encoder(struct drm_device *dev,
 
 	drm_encoder = (struct drm_encoder *) encoder;
 
-	drm_encoder->possible_crtcs = ENCODER_MAIN_CRTC_MASK;
+	drm_encoder->possible_crtcs = ENCODER_CRTC_MASK;
 	drm_encoder->possible_clones = 1 << 0;
 
 	drm_encoder_init(dev, drm_encoder,
@@ -444,7 +458,7 @@ static void sti_hdmi_encoder_commit(struct drm_encoder *encoder)
 {
 	struct sti_tvout *tvout = to_sti_tvout(encoder);
 
-	tvout_hdmi_start(tvout, true);
+	tvout_hdmi_start(tvout, sti_drm_crtc_is_main(encoder->crtc));
 }
 
 static void sti_hdmi_encoder_disable(struct drm_encoder *encoder)
@@ -478,7 +492,7 @@ static struct drm_encoder *sti_tvout_create_hdmi_encoder(struct drm_device *dev,
 
 	drm_encoder = (struct drm_encoder *) encoder;
 
-	drm_encoder->possible_crtcs = ENCODER_MAIN_CRTC_MASK;
+	drm_encoder->possible_crtcs = ENCODER_CRTC_MASK;
 	drm_encoder->possible_clones = 1 << 1;
 
 	drm_encoder_init(dev, drm_encoder,
diff --git a/drivers/gpu/drm/sti/sti_vtg.c b/drivers/gpu/drm/sti/sti_vtg.c
index 740d6e347a62..9564f2568e2c 100644
--- a/drivers/gpu/drm/sti/sti_vtg.c
+++ b/drivers/gpu/drm/sti/sti_vtg.c
@@ -51,10 +51,19 @@
 #define VTG_TOP_V_HD_3      0x010C
 #define VTG_BOT_V_HD_3      0x0110
 
+#define VTG_H_HD_4          0x0120
+#define VTG_TOP_V_VD_4      0x0124
+#define VTG_BOT_V_VD_4      0x0128
+#define VTG_TOP_V_HD_4      0x012c
+#define VTG_BOT_V_HD_4      0x0130
+
 #define VTG_IRQ_BOTTOM      BIT(0)
 #define VTG_IRQ_TOP         BIT(1)
 #define VTG_IRQ_MASK        (VTG_IRQ_TOP | VTG_IRQ_BOTTOM)
 
+/* Delay introduced by the HDMI in nb of pixel */
+#define HDMI_DELAY          (6)
+
 /* delay introduced by the Arbitrary Waveform Generator in nb of pixels */
 #define AWG_DELAY_HD        (-9)
 #define AWG_DELAY_ED        (-8)
@@ -133,10 +142,10 @@ static void vtg_set_mode(struct sti_vtg *vtg,
 	writel(tmp, vtg->regs + VTG_VID_TFS);
 	writel(tmp, vtg->regs + VTG_VID_BFS);
 
-	/* prepare VTG set 1 and 2 for HDMI and VTG set 3 for HD DAC */
-	tmp = (mode->hsync_end - mode->hsync_start) << 16;
+	/* prepare VTG set 1 for HDMI */
+	tmp = (mode->hsync_end - mode->hsync_start + HDMI_DELAY) << 16;
+	tmp |= HDMI_DELAY;
 	writel(tmp, vtg->regs + VTG_H_HD_1);
-	writel(tmp, vtg->regs + VTG_H_HD_2);
 
 	tmp = (mode->vsync_end - mode->vsync_start + 1) << 16;
 	tmp |= 1;
@@ -146,6 +155,11 @@ static void vtg_set_mode(struct sti_vtg *vtg,
 	writel(0, vtg->regs + VTG_BOT_V_HD_1);
 
 	/* prepare VTG set 2 for for HD DCS */
+	tmp = (mode->hsync_end - mode->hsync_start) << 16;
+	writel(tmp, vtg->regs + VTG_H_HD_2);
+
+	tmp = (mode->vsync_end - mode->vsync_start + 1) << 16;
+	tmp |= 1;
 	writel(tmp, vtg->regs + VTG_TOP_V_VD_2);
 	writel(tmp, vtg->regs + VTG_BOT_V_VD_2);
 	writel(0, vtg->regs + VTG_TOP_V_HD_2);
@@ -166,6 +180,17 @@ static void vtg_set_mode(struct sti_vtg *vtg,
 	writel(tmp, vtg->regs + VTG_TOP_V_HD_3);
 	writel(tmp, vtg->regs + VTG_BOT_V_HD_3);
 
+	/* Prepare VTG set 4 for DVO */
+	tmp = (mode->hsync_end - mode->hsync_start) << 16;
+	writel(tmp, vtg->regs + VTG_H_HD_4);
+
+	tmp = (mode->vsync_end - mode->vsync_start + 1) << 16;
+	tmp |= 1;
+	writel(tmp, vtg->regs + VTG_TOP_V_VD_4);
+	writel(tmp, vtg->regs + VTG_BOT_V_VD_4);
+	writel(0, vtg->regs + VTG_TOP_V_HD_4);
+	writel(0, vtg->regs + VTG_BOT_V_HD_4);
+
 	/* mode */
 	writel(type, vtg->regs + VTG_MODE);
 }
diff --git a/drivers/gpu/drm/tegra/Kconfig b/drivers/gpu/drm/tegra/Kconfig
index 354ddb29231f..74d9d621453d 100644
--- a/drivers/gpu/drm/tegra/Kconfig
+++ b/drivers/gpu/drm/tegra/Kconfig
@@ -1,6 +1,7 @@
 config DRM_TEGRA
 	tristate "NVIDIA Tegra DRM"
 	depends on ARCH_TEGRA || (ARM && COMPILE_TEST)
+	depends on COMMON_CLK
 	depends on DRM
 	depends on RESET_CONTROLLER
 	select DRM_KMS_HELPER
diff --git a/drivers/gpu/drm/tegra/dc.c b/drivers/gpu/drm/tegra/dc.c
index 054a79f143ae..978993fa3a36 100644
--- a/drivers/gpu/drm/tegra/dc.c
+++ b/drivers/gpu/drm/tegra/dc.c
@@ -9,17 +9,23 @@
 
 #include <linux/clk.h>
 #include <linux/debugfs.h>
+#include <linux/iommu.h>
 #include <linux/reset.h>
 
+#include <soc/tegra/pmc.h>
+
 #include "dc.h"
 #include "drm.h"
 #include "gem.h"
 
+#include <drm/drm_plane_helper.h>
+
 struct tegra_dc_soc_info {
 	bool supports_interlacing;
 	bool supports_cursor;
 	bool supports_block_linear;
 	unsigned int pitch_align;
+	bool has_powergate;
 };
 
 struct tegra_plane {
@@ -32,6 +38,26 @@ static inline struct tegra_plane *to_tegra_plane(struct drm_plane *plane)
 	return container_of(plane, struct tegra_plane, base);
 }
 
+static void tegra_dc_window_commit(struct tegra_dc *dc, unsigned int index)
+{
+	u32 value = WIN_A_ACT_REQ << index;
+
+	tegra_dc_writel(dc, value << 8, DC_CMD_STATE_CONTROL);
+	tegra_dc_writel(dc, value, DC_CMD_STATE_CONTROL);
+}
+
+static void tegra_dc_cursor_commit(struct tegra_dc *dc)
+{
+	tegra_dc_writel(dc, CURSOR_ACT_REQ << 8, DC_CMD_STATE_CONTROL);
+	tegra_dc_writel(dc, CURSOR_ACT_REQ, DC_CMD_STATE_CONTROL);
+}
+
+static void tegra_dc_commit(struct tegra_dc *dc)
+{
+	tegra_dc_writel(dc, GENERAL_ACT_REQ << 8, DC_CMD_STATE_CONTROL);
+	tegra_dc_writel(dc, GENERAL_ACT_REQ, DC_CMD_STATE_CONTROL);
+}
+
 static unsigned int tegra_dc_format(uint32_t format, uint32_t *swap)
 {
 	/* assume no swapping of fetched data */
@@ -142,7 +168,7 @@ static int tegra_dc_setup_window(struct tegra_dc *dc, unsigned int index,
 				 const struct tegra_dc_window *window)
 {
 	unsigned h_offset, v_offset, h_size, v_size, h_dda, v_dda, bpp;
-	unsigned long value;
+	unsigned long value, flags;
 	bool yuv, planar;
 
 	/*
@@ -155,6 +181,8 @@ static int tegra_dc_setup_window(struct tegra_dc *dc, unsigned int index,
 	else
 		bpp = planar ? 1 : 2;
 
+	spin_lock_irqsave(&dc->lock, flags);
+
 	value = WINDOW_A_SELECT << index;
 	tegra_dc_writel(dc, value, DC_CMD_DISPLAY_WINDOW_HEADER);
 
@@ -247,6 +275,7 @@ static int tegra_dc_setup_window(struct tegra_dc *dc, unsigned int index,
 
 		case TEGRA_BO_TILING_MODE_BLOCK:
 			DRM_ERROR("hardware doesn't support block linear mode\n");
+			spin_unlock_irqrestore(&dc->lock, flags);
 			return -EINVAL;
 		}
 
@@ -303,17 +332,267 @@ static int tegra_dc_setup_window(struct tegra_dc *dc, unsigned int index,
 		break;
 	}
 
-	tegra_dc_writel(dc, WIN_A_UPDATE << index, DC_CMD_STATE_CONTROL);
-	tegra_dc_writel(dc, WIN_A_ACT_REQ << index, DC_CMD_STATE_CONTROL);
+	tegra_dc_window_commit(dc, index);
+
+	spin_unlock_irqrestore(&dc->lock, flags);
+
+	return 0;
+}
+
+static int tegra_window_plane_disable(struct drm_plane *plane)
+{
+	struct tegra_dc *dc = to_tegra_dc(plane->crtc);
+	struct tegra_plane *p = to_tegra_plane(plane);
+	unsigned long flags;
+	u32 value;
+
+	if (!plane->crtc)
+		return 0;
+
+	spin_lock_irqsave(&dc->lock, flags);
+
+	value = WINDOW_A_SELECT << p->index;
+	tegra_dc_writel(dc, value, DC_CMD_DISPLAY_WINDOW_HEADER);
+
+	value = tegra_dc_readl(dc, DC_WIN_WIN_OPTIONS);
+	value &= ~WIN_ENABLE;
+	tegra_dc_writel(dc, value, DC_WIN_WIN_OPTIONS);
+
+	tegra_dc_window_commit(dc, p->index);
+
+	spin_unlock_irqrestore(&dc->lock, flags);
 
 	return 0;
 }
 
-static int tegra_plane_update(struct drm_plane *plane, struct drm_crtc *crtc,
-			      struct drm_framebuffer *fb, int crtc_x,
-			      int crtc_y, unsigned int crtc_w,
-			      unsigned int crtc_h, uint32_t src_x,
-			      uint32_t src_y, uint32_t src_w, uint32_t src_h)
+static void tegra_plane_destroy(struct drm_plane *plane)
+{
+	struct tegra_plane *p = to_tegra_plane(plane);
+
+	drm_plane_cleanup(plane);
+	kfree(p);
+}
+
+static const u32 tegra_primary_plane_formats[] = {
+	DRM_FORMAT_XBGR8888,
+	DRM_FORMAT_XRGB8888,
+	DRM_FORMAT_RGB565,
+};
+
+static int tegra_primary_plane_update(struct drm_plane *plane,
+				      struct drm_crtc *crtc,
+				      struct drm_framebuffer *fb, int crtc_x,
+				      int crtc_y, unsigned int crtc_w,
+				      unsigned int crtc_h, uint32_t src_x,
+				      uint32_t src_y, uint32_t src_w,
+				      uint32_t src_h)
+{
+	struct tegra_bo *bo = tegra_fb_get_plane(fb, 0);
+	struct tegra_plane *p = to_tegra_plane(plane);
+	struct tegra_dc *dc = to_tegra_dc(crtc);
+	struct tegra_dc_window window;
+	int err;
+
+	memset(&window, 0, sizeof(window));
+	window.src.x = src_x >> 16;
+	window.src.y = src_y >> 16;
+	window.src.w = src_w >> 16;
+	window.src.h = src_h >> 16;
+	window.dst.x = crtc_x;
+	window.dst.y = crtc_y;
+	window.dst.w = crtc_w;
+	window.dst.h = crtc_h;
+	window.format = tegra_dc_format(fb->pixel_format, &window.swap);
+	window.bits_per_pixel = fb->bits_per_pixel;
+	window.bottom_up = tegra_fb_is_bottom_up(fb);
+
+	err = tegra_fb_get_tiling(fb, &window.tiling);
+	if (err < 0)
+		return err;
+
+	window.base[0] = bo->paddr + fb->offsets[0];
+	window.stride[0] = fb->pitches[0];
+
+	err = tegra_dc_setup_window(dc, p->index, &window);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+
+static void tegra_primary_plane_destroy(struct drm_plane *plane)
+{
+	tegra_window_plane_disable(plane);
+	tegra_plane_destroy(plane);
+}
+
+static const struct drm_plane_funcs tegra_primary_plane_funcs = {
+	.update_plane = tegra_primary_plane_update,
+	.disable_plane = tegra_window_plane_disable,
+	.destroy = tegra_primary_plane_destroy,
+};
+
+static struct drm_plane *tegra_dc_primary_plane_create(struct drm_device *drm,
+						       struct tegra_dc *dc)
+{
+	struct tegra_plane *plane;
+	unsigned int num_formats;
+	const u32 *formats;
+	int err;
+
+	plane = kzalloc(sizeof(*plane), GFP_KERNEL);
+	if (!plane)
+		return ERR_PTR(-ENOMEM);
+
+	num_formats = ARRAY_SIZE(tegra_primary_plane_formats);
+	formats = tegra_primary_plane_formats;
+
+	err = drm_universal_plane_init(drm, &plane->base, 1 << dc->pipe,
+				       &tegra_primary_plane_funcs, formats,
+				       num_formats, DRM_PLANE_TYPE_PRIMARY);
+	if (err < 0) {
+		kfree(plane);
+		return ERR_PTR(err);
+	}
+
+	return &plane->base;
+}
+
+static const u32 tegra_cursor_plane_formats[] = {
+	DRM_FORMAT_RGBA8888,
+};
+
+static int tegra_cursor_plane_update(struct drm_plane *plane,
+				     struct drm_crtc *crtc,
+				     struct drm_framebuffer *fb, int crtc_x,
+				     int crtc_y, unsigned int crtc_w,
+				     unsigned int crtc_h, uint32_t src_x,
+				     uint32_t src_y, uint32_t src_w,
+				     uint32_t src_h)
+{
+	struct tegra_bo *bo = tegra_fb_get_plane(fb, 0);
+	struct tegra_dc *dc = to_tegra_dc(crtc);
+	u32 value = CURSOR_CLIP_DISPLAY;
+
+	/* scaling not supported for cursor */
+	if ((src_w >> 16 != crtc_w) || (src_h >> 16 != crtc_h))
+		return -EINVAL;
+
+	/* only square cursors supported */
+	if (src_w != src_h)
+		return -EINVAL;
+
+	switch (crtc_w) {
+	case 32:
+		value |= CURSOR_SIZE_32x32;
+		break;
+
+	case 64:
+		value |= CURSOR_SIZE_64x64;
+		break;
+
+	case 128:
+		value |= CURSOR_SIZE_128x128;
+		break;
+
+	case 256:
+		value |= CURSOR_SIZE_256x256;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	value |= (bo->paddr >> 10) & 0x3fffff;
+	tegra_dc_writel(dc, value, DC_DISP_CURSOR_START_ADDR);
+
+#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
+	value = (bo->paddr >> 32) & 0x3;
+	tegra_dc_writel(dc, value, DC_DISP_CURSOR_START_ADDR_HI);
+#endif
+
+	/* enable cursor and set blend mode */
+	value = tegra_dc_readl(dc, DC_DISP_DISP_WIN_OPTIONS);
+	value |= CURSOR_ENABLE;
+	tegra_dc_writel(dc, value, DC_DISP_DISP_WIN_OPTIONS);
+
+	value = tegra_dc_readl(dc, DC_DISP_BLEND_CURSOR_CONTROL);
+	value &= ~CURSOR_DST_BLEND_MASK;
+	value &= ~CURSOR_SRC_BLEND_MASK;
+	value |= CURSOR_MODE_NORMAL;
+	value |= CURSOR_DST_BLEND_NEG_K1_TIMES_SRC;
+	value |= CURSOR_SRC_BLEND_K1_TIMES_SRC;
+	value |= CURSOR_ALPHA;
+	tegra_dc_writel(dc, value, DC_DISP_BLEND_CURSOR_CONTROL);
+
+	/* position the cursor */
+	value = (crtc_y & 0x3fff) << 16 | (crtc_x & 0x3fff);
+	tegra_dc_writel(dc, value, DC_DISP_CURSOR_POSITION);
+
+	/* apply changes */
+	tegra_dc_cursor_commit(dc);
+	tegra_dc_commit(dc);
+
+	return 0;
+}
+
+static int tegra_cursor_plane_disable(struct drm_plane *plane)
+{
+	struct tegra_dc *dc = to_tegra_dc(plane->crtc);
+	u32 value;
+
+	if (!plane->crtc)
+		return 0;
+
+	value = tegra_dc_readl(dc, DC_DISP_DISP_WIN_OPTIONS);
+	value &= ~CURSOR_ENABLE;
+	tegra_dc_writel(dc, value, DC_DISP_DISP_WIN_OPTIONS);
+
+	tegra_dc_cursor_commit(dc);
+	tegra_dc_commit(dc);
+
+	return 0;
+}
+
+static const struct drm_plane_funcs tegra_cursor_plane_funcs = {
+	.update_plane = tegra_cursor_plane_update,
+	.disable_plane = tegra_cursor_plane_disable,
+	.destroy = tegra_plane_destroy,
+};
+
+static struct drm_plane *tegra_dc_cursor_plane_create(struct drm_device *drm,
+						      struct tegra_dc *dc)
+{
+	struct tegra_plane *plane;
+	unsigned int num_formats;
+	const u32 *formats;
+	int err;
+
+	plane = kzalloc(sizeof(*plane), GFP_KERNEL);
+	if (!plane)
+		return ERR_PTR(-ENOMEM);
+
+	num_formats = ARRAY_SIZE(tegra_cursor_plane_formats);
+	formats = tegra_cursor_plane_formats;
+
+	err = drm_universal_plane_init(drm, &plane->base, 1 << dc->pipe,
+				       &tegra_cursor_plane_funcs, formats,
+				       num_formats, DRM_PLANE_TYPE_CURSOR);
+	if (err < 0) {
+		kfree(plane);
+		return ERR_PTR(err);
+	}
+
+	return &plane->base;
+}
+
+static int tegra_overlay_plane_update(struct drm_plane *plane,
+				      struct drm_crtc *crtc,
+				      struct drm_framebuffer *fb, int crtc_x,
+				      int crtc_y, unsigned int crtc_w,
+				      unsigned int crtc_h, uint32_t src_x,
+				      uint32_t src_y, uint32_t src_w,
+				      uint32_t src_h)
 {
 	struct tegra_plane *p = to_tegra_plane(plane);
 	struct tegra_dc *dc = to_tegra_dc(crtc);
@@ -359,44 +638,19 @@ static int tegra_plane_update(struct drm_plane *plane, struct drm_crtc *crtc,
 	return tegra_dc_setup_window(dc, p->index, &window);
 }
 
-static int tegra_plane_disable(struct drm_plane *plane)
+static void tegra_overlay_plane_destroy(struct drm_plane *plane)
 {
-	struct tegra_dc *dc = to_tegra_dc(plane->crtc);
-	struct tegra_plane *p = to_tegra_plane(plane);
-	unsigned long value;
-
-	if (!plane->crtc)
-		return 0;
-
-	value = WINDOW_A_SELECT << p->index;
-	tegra_dc_writel(dc, value, DC_CMD_DISPLAY_WINDOW_HEADER);
-
-	value = tegra_dc_readl(dc, DC_WIN_WIN_OPTIONS);
-	value &= ~WIN_ENABLE;
-	tegra_dc_writel(dc, value, DC_WIN_WIN_OPTIONS);
-
-	tegra_dc_writel(dc, WIN_A_UPDATE << p->index, DC_CMD_STATE_CONTROL);
-	tegra_dc_writel(dc, WIN_A_ACT_REQ << p->index, DC_CMD_STATE_CONTROL);
-
-	return 0;
+	tegra_window_plane_disable(plane);
+	tegra_plane_destroy(plane);
 }
 
-static void tegra_plane_destroy(struct drm_plane *plane)
-{
-	struct tegra_plane *p = to_tegra_plane(plane);
-
-	tegra_plane_disable(plane);
-	drm_plane_cleanup(plane);
-	kfree(p);
-}
-
-static const struct drm_plane_funcs tegra_plane_funcs = {
-	.update_plane = tegra_plane_update,
-	.disable_plane = tegra_plane_disable,
-	.destroy = tegra_plane_destroy,
+static const struct drm_plane_funcs tegra_overlay_plane_funcs = {
+	.update_plane = tegra_overlay_plane_update,
+	.disable_plane = tegra_window_plane_disable,
+	.destroy = tegra_overlay_plane_destroy,
 };
 
-static const uint32_t plane_formats[] = {
+static const uint32_t tegra_overlay_plane_formats[] = {
 	DRM_FORMAT_XBGR8888,
 	DRM_FORMAT_XRGB8888,
 	DRM_FORMAT_RGB565,
@@ -406,27 +660,44 @@ static const uint32_t plane_formats[] = {
 	DRM_FORMAT_YUV422,
 };
 
-static int tegra_dc_add_planes(struct drm_device *drm, struct tegra_dc *dc)
+static struct drm_plane *tegra_dc_overlay_plane_create(struct drm_device *drm,
+						       struct tegra_dc *dc,
+						       unsigned int index)
 {
-	unsigned int i;
-	int err = 0;
+	struct tegra_plane *plane;
+	unsigned int num_formats;
+	const u32 *formats;
+	int err;
 
-	for (i = 0; i < 2; i++) {
-		struct tegra_plane *plane;
+	plane = kzalloc(sizeof(*plane), GFP_KERNEL);
+	if (!plane)
+		return ERR_PTR(-ENOMEM);
 
-		plane = kzalloc(sizeof(*plane), GFP_KERNEL);
-		if (!plane)
-			return -ENOMEM;
+	plane->index = index;
 
-		plane->index = 1 + i;
+	num_formats = ARRAY_SIZE(tegra_overlay_plane_formats);
+	formats = tegra_overlay_plane_formats;
 
-		err = drm_plane_init(drm, &plane->base, 1 << dc->pipe,
-				     &tegra_plane_funcs, plane_formats,
-				     ARRAY_SIZE(plane_formats), false);
-		if (err < 0) {
-			kfree(plane);
-			return err;
-		}
+	err = drm_universal_plane_init(drm, &plane->base, 1 << dc->pipe,
+				       &tegra_overlay_plane_funcs, formats,
+				       num_formats, DRM_PLANE_TYPE_OVERLAY);
+	if (err < 0) {
+		kfree(plane);
+		return ERR_PTR(err);
+	}
+
+	return &plane->base;
+}
+
+static int tegra_dc_add_planes(struct drm_device *drm, struct tegra_dc *dc)
+{
+	struct drm_plane *plane;
+	unsigned int i;
+
+	for (i = 0; i < 2; i++) {
+		plane = tegra_dc_overlay_plane_create(drm, dc, 1 + i);
+		if (IS_ERR(plane))
+			return PTR_ERR(plane);
 	}
 
 	return 0;
@@ -438,14 +709,16 @@ static int tegra_dc_set_base(struct tegra_dc *dc, int x, int y,
 	struct tegra_bo *bo = tegra_fb_get_plane(fb, 0);
 	unsigned int h_offset = 0, v_offset = 0;
 	struct tegra_bo_tiling tiling;
+	unsigned long value, flags;
 	unsigned int format, swap;
-	unsigned long value;
 	int err;
 
 	err = tegra_fb_get_tiling(fb, &tiling);
 	if (err < 0)
 		return err;
 
+	spin_lock_irqsave(&dc->lock, flags);
+
 	tegra_dc_writel(dc, WINDOW_A_SELECT, DC_CMD_DISPLAY_WINDOW_HEADER);
 
 	value = fb->offsets[0] + y * fb->pitches[0] +
@@ -491,6 +764,7 @@ static int tegra_dc_set_base(struct tegra_dc *dc, int x, int y,
 
 		case TEGRA_BO_TILING_MODE_BLOCK:
 			DRM_ERROR("hardware doesn't support block linear mode\n");
+			spin_unlock_irqrestore(&dc->lock, flags);
 			return -EINVAL;
 		}
 
@@ -513,12 +787,12 @@ static int tegra_dc_set_base(struct tegra_dc *dc, int x, int y,
 	tegra_dc_writel(dc, h_offset, DC_WINBUF_ADDR_H_OFFSET);
 	tegra_dc_writel(dc, v_offset, DC_WINBUF_ADDR_V_OFFSET);
 
-	value = GENERAL_UPDATE | WIN_A_UPDATE;
-	tegra_dc_writel(dc, value, DC_CMD_STATE_CONTROL);
-
 	value = GENERAL_ACT_REQ | WIN_A_ACT_REQ;
+	tegra_dc_writel(dc, value << 8, DC_CMD_STATE_CONTROL);
 	tegra_dc_writel(dc, value, DC_CMD_STATE_CONTROL);
 
+	spin_unlock_irqrestore(&dc->lock, flags);
+
 	return 0;
 }
 
@@ -548,109 +822,6 @@ void tegra_dc_disable_vblank(struct tegra_dc *dc)
 	spin_unlock_irqrestore(&dc->lock, flags);
 }
 
-static int tegra_dc_cursor_set2(struct drm_crtc *crtc, struct drm_file *file,
-				uint32_t handle, uint32_t width,
-				uint32_t height, int32_t hot_x, int32_t hot_y)
-{
-	unsigned long value = CURSOR_CLIP_DISPLAY;
-	struct tegra_dc *dc = to_tegra_dc(crtc);
-	struct drm_gem_object *gem;
-	struct tegra_bo *bo = NULL;
-
-	if (!dc->soc->supports_cursor)
-		return -ENXIO;
-
-	if (width != height)
-		return -EINVAL;
-
-	switch (width) {
-	case 32:
-		value |= CURSOR_SIZE_32x32;
-		break;
-
-	case 64:
-		value |= CURSOR_SIZE_64x64;
-		break;
-
-	case 128:
-		value |= CURSOR_SIZE_128x128;
-
-	case 256:
-		value |= CURSOR_SIZE_256x256;
-		break;
-
-	default:
-		return -EINVAL;
-	}
-
-	if (handle) {
-		gem = drm_gem_object_lookup(crtc->dev, file, handle);
-		if (!gem)
-			return -ENOENT;
-
-		bo = to_tegra_bo(gem);
-	}
-
-	if (bo) {
-		unsigned long addr = (bo->paddr & 0xfffffc00) >> 10;
-#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
-		unsigned long high = (bo->paddr & 0xfffffffc) >> 32;
-#endif
-
-		tegra_dc_writel(dc, value | addr, DC_DISP_CURSOR_START_ADDR);
-
-#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
-		tegra_dc_writel(dc, high, DC_DISP_CURSOR_START_ADDR_HI);
-#endif
-
-		value = tegra_dc_readl(dc, DC_DISP_DISP_WIN_OPTIONS);
-		value |= CURSOR_ENABLE;
-		tegra_dc_writel(dc, value, DC_DISP_DISP_WIN_OPTIONS);
-
-		value = tegra_dc_readl(dc, DC_DISP_BLEND_CURSOR_CONTROL);
-		value &= ~CURSOR_DST_BLEND_MASK;
-		value &= ~CURSOR_SRC_BLEND_MASK;
-		value |= CURSOR_MODE_NORMAL;
-		value |= CURSOR_DST_BLEND_NEG_K1_TIMES_SRC;
-		value |= CURSOR_SRC_BLEND_K1_TIMES_SRC;
-		value |= CURSOR_ALPHA;
-		tegra_dc_writel(dc, value, DC_DISP_BLEND_CURSOR_CONTROL);
-	} else {
-		value = tegra_dc_readl(dc, DC_DISP_DISP_WIN_OPTIONS);
-		value &= ~CURSOR_ENABLE;
-		tegra_dc_writel(dc, value, DC_DISP_DISP_WIN_OPTIONS);
-	}
-
-	tegra_dc_writel(dc, CURSOR_ACT_REQ << 8, DC_CMD_STATE_CONTROL);
-	tegra_dc_writel(dc, CURSOR_ACT_REQ, DC_CMD_STATE_CONTROL);
-
-	tegra_dc_writel(dc, GENERAL_ACT_REQ << 8, DC_CMD_STATE_CONTROL);
-	tegra_dc_writel(dc, GENERAL_ACT_REQ, DC_CMD_STATE_CONTROL);
-
-	return 0;
-}
-
-static int tegra_dc_cursor_move(struct drm_crtc *crtc, int x, int y)
-{
-	struct tegra_dc *dc = to_tegra_dc(crtc);
-	unsigned long value;
-
-	if (!dc->soc->supports_cursor)
-		return -ENXIO;
-
-	value = ((y & 0x3fff) << 16) | (x & 0x3fff);
-	tegra_dc_writel(dc, value, DC_DISP_CURSOR_POSITION);
-
-	tegra_dc_writel(dc, CURSOR_ACT_REQ << 8, DC_CMD_STATE_CONTROL);
-	tegra_dc_writel(dc, CURSOR_ACT_REQ, DC_CMD_STATE_CONTROL);
-
-	/* XXX: only required on generations earlier than Tegra124? */
-	tegra_dc_writel(dc, GENERAL_ACT_REQ << 8, DC_CMD_STATE_CONTROL);
-	tegra_dc_writel(dc, GENERAL_ACT_REQ, DC_CMD_STATE_CONTROL);
-
-	return 0;
-}
-
 static void tegra_dc_finish_page_flip(struct tegra_dc *dc)
 {
 	struct drm_device *drm = dc->base.dev;
@@ -658,23 +829,32 @@ static void tegra_dc_finish_page_flip(struct tegra_dc *dc)
 	unsigned long flags, base;
 	struct tegra_bo *bo;
 
-	if (!dc->event)
+	spin_lock_irqsave(&drm->event_lock, flags);
+
+	if (!dc->event) {
+		spin_unlock_irqrestore(&drm->event_lock, flags);
 		return;
+	}
 
 	bo = tegra_fb_get_plane(crtc->primary->fb, 0);
 
+	spin_lock_irqsave(&dc->lock, flags);
+
 	/* check if new start address has been latched */
+	tegra_dc_writel(dc, WINDOW_A_SELECT, DC_CMD_DISPLAY_WINDOW_HEADER);
 	tegra_dc_writel(dc, READ_MUX, DC_CMD_STATE_ACCESS);
 	base = tegra_dc_readl(dc, DC_WINBUF_START_ADDR);
 	tegra_dc_writel(dc, 0, DC_CMD_STATE_ACCESS);
 
+	spin_unlock_irqrestore(&dc->lock, flags);
+
 	if (base == bo->paddr + crtc->primary->fb->offsets[0]) {
-		spin_lock_irqsave(&drm->event_lock, flags);
-		drm_send_vblank_event(drm, dc->pipe, dc->event);
-		drm_vblank_put(drm, dc->pipe);
+		drm_crtc_send_vblank_event(crtc, dc->event);
+		drm_crtc_vblank_put(crtc);
 		dc->event = NULL;
-		spin_unlock_irqrestore(&drm->event_lock, flags);
 	}
+
+	spin_unlock_irqrestore(&drm->event_lock, flags);
 }
 
 void tegra_dc_cancel_page_flip(struct drm_crtc *crtc, struct drm_file *file)
@@ -687,7 +867,7 @@ void tegra_dc_cancel_page_flip(struct drm_crtc *crtc, struct drm_file *file)
 
 	if (dc->event && dc->event->base.file_priv == file) {
 		dc->event->base.destroy(&dc->event->base);
-		drm_vblank_put(drm, dc->pipe);
+		drm_crtc_vblank_put(crtc);
 		dc->event = NULL;
 	}
 
@@ -697,16 +877,16 @@ void tegra_dc_cancel_page_flip(struct drm_crtc *crtc, struct drm_file *file)
 static int tegra_dc_page_flip(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 			      struct drm_pending_vblank_event *event, uint32_t page_flip_flags)
 {
+	unsigned int pipe = drm_crtc_index(crtc);
 	struct tegra_dc *dc = to_tegra_dc(crtc);
-	struct drm_device *drm = crtc->dev;
 
 	if (dc->event)
 		return -EBUSY;
 
 	if (event) {
-		event->pipe = dc->pipe;
+		event->pipe = pipe;
 		dc->event = event;
-		drm_vblank_get(drm, dc->pipe);
+		drm_crtc_vblank_get(crtc);
 	}
 
 	tegra_dc_set_base(dc, 0, 0, fb);
@@ -727,8 +907,6 @@ static void tegra_dc_destroy(struct drm_crtc *crtc)
 }
 
 static const struct drm_crtc_funcs tegra_crtc_funcs = {
-	.cursor_set2 = tegra_dc_cursor_set2,
-	.cursor_move = tegra_dc_cursor_move,
 	.page_flip = tegra_dc_page_flip,
 	.set_config = drm_crtc_helper_set_config,
 	.destroy = tegra_dc_destroy,
@@ -736,12 +914,13 @@ static const struct drm_crtc_funcs tegra_crtc_funcs = {
 
 static void tegra_crtc_disable(struct drm_crtc *crtc)
 {
+	struct tegra_dc *dc = to_tegra_dc(crtc);
 	struct drm_device *drm = crtc->dev;
 	struct drm_plane *plane;
 
 	drm_for_each_legacy_plane(plane, &drm->mode_config.plane_list) {
 		if (plane->crtc == crtc) {
-			tegra_plane_disable(plane);
+			tegra_window_plane_disable(plane);
 			plane->crtc = NULL;
 
 			if (plane->fb) {
@@ -752,6 +931,7 @@ static void tegra_crtc_disable(struct drm_crtc *crtc)
 	}
 
 	drm_crtc_vblank_off(crtc);
+	tegra_dc_commit(dc);
 }
 
 static bool tegra_crtc_mode_fixup(struct drm_crtc *crtc,
@@ -934,15 +1114,9 @@ static void tegra_crtc_prepare(struct drm_crtc *crtc)
 static void tegra_crtc_commit(struct drm_crtc *crtc)
 {
 	struct tegra_dc *dc = to_tegra_dc(crtc);
-	unsigned long value;
-
-	value = GENERAL_UPDATE | WIN_A_UPDATE;
-	tegra_dc_writel(dc, value, DC_CMD_STATE_CONTROL);
-
-	value = GENERAL_ACT_REQ | WIN_A_ACT_REQ;
-	tegra_dc_writel(dc, value, DC_CMD_STATE_CONTROL);
 
 	drm_crtc_vblank_on(crtc);
+	tegra_dc_commit(dc);
 }
 
 static void tegra_crtc_load_lut(struct drm_crtc *crtc)
@@ -977,7 +1151,7 @@ static irqreturn_t tegra_dc_irq(int irq, void *data)
 		/*
 		dev_dbg(dc->dev, "%s(): vertical blank\n", __func__);
 		*/
-		drm_handle_vblank(dc->base.dev, dc->pipe);
+		drm_crtc_handle_vblank(&dc->base);
 		tegra_dc_finish_page_flip(dc);
 	}
 
@@ -996,7 +1170,7 @@ static int tegra_dc_show_regs(struct seq_file *s, void *data)
 	struct tegra_dc *dc = node->info_ent->data;
 
 #define DUMP_REG(name)						\
-	seq_printf(s, "%-40s %#05x %08lx\n", #name, name,	\
+	seq_printf(s, "%-40s %#05x %08x\n", #name, name,	\
 		   tegra_dc_readl(dc, name))
 
 	DUMP_REG(DC_CMD_GENERAL_INCR_SYNCPT);
@@ -1284,9 +1458,40 @@ static int tegra_dc_init(struct host1x_client *client)
 	struct drm_device *drm = dev_get_drvdata(client->parent);
 	struct tegra_dc *dc = host1x_client_to_dc(client);
 	struct tegra_drm *tegra = drm->dev_private;
+	struct drm_plane *primary = NULL;
+	struct drm_plane *cursor = NULL;
 	int err;
 
-	drm_crtc_init(drm, &dc->base, &tegra_crtc_funcs);
+	if (tegra->domain) {
+		err = iommu_attach_device(tegra->domain, dc->dev);
+		if (err < 0) {
+			dev_err(dc->dev, "failed to attach to domain: %d\n",
+				err);
+			return err;
+		}
+
+		dc->domain = tegra->domain;
+	}
+
+	primary = tegra_dc_primary_plane_create(drm, dc);
+	if (IS_ERR(primary)) {
+		err = PTR_ERR(primary);
+		goto cleanup;
+	}
+
+	if (dc->soc->supports_cursor) {
+		cursor = tegra_dc_cursor_plane_create(drm, dc);
+		if (IS_ERR(cursor)) {
+			err = PTR_ERR(cursor);
+			goto cleanup;
+		}
+	}
+
+	err = drm_crtc_init_with_planes(drm, &dc->base, primary, cursor,
+					&tegra_crtc_funcs);
+	if (err < 0)
+		goto cleanup;
+
 	drm_mode_crtc_set_gamma_size(&dc->base, 256);
 	drm_crtc_helper_add(&dc->base, &tegra_crtc_helper_funcs);
 
@@ -1300,12 +1505,12 @@ static int tegra_dc_init(struct host1x_client *client)
 	err = tegra_dc_rgb_init(drm, dc);
 	if (err < 0 && err != -ENODEV) {
 		dev_err(dc->dev, "failed to initialize RGB output: %d\n", err);
-		return err;
+		goto cleanup;
 	}
 
 	err = tegra_dc_add_planes(drm, dc);
 	if (err < 0)
-		return err;
+		goto cleanup;
 
 	if (IS_ENABLED(CONFIG_DEBUG_FS)) {
 		err = tegra_dc_debugfs_init(dc, drm->primary);
@@ -1318,10 +1523,24 @@ static int tegra_dc_init(struct host1x_client *client)
 	if (err < 0) {
 		dev_err(dc->dev, "failed to request IRQ#%u: %d\n", dc->irq,
 			err);
-		return err;
+		goto cleanup;
 	}
 
 	return 0;
+
+cleanup:
+	if (cursor)
+		drm_plane_cleanup(cursor);
+
+	if (primary)
+		drm_plane_cleanup(primary);
+
+	if (tegra->domain) {
+		iommu_detach_device(tegra->domain, dc->dev);
+		dc->domain = NULL;
+	}
+
+	return err;
 }
 
 static int tegra_dc_exit(struct host1x_client *client)
@@ -1343,6 +1562,11 @@ static int tegra_dc_exit(struct host1x_client *client)
 		return err;
 	}
 
+	if (dc->domain) {
+		iommu_detach_device(dc->domain, dc->dev);
+		dc->domain = NULL;
+	}
+
 	return 0;
 }
 
@@ -1356,6 +1580,7 @@ static const struct tegra_dc_soc_info tegra20_dc_soc_info = {
 	.supports_cursor = false,
 	.supports_block_linear = false,
 	.pitch_align = 8,
+	.has_powergate = false,
 };
 
 static const struct tegra_dc_soc_info tegra30_dc_soc_info = {
@@ -1363,6 +1588,7 @@ static const struct tegra_dc_soc_info tegra30_dc_soc_info = {
 	.supports_cursor = false,
 	.supports_block_linear = false,
 	.pitch_align = 8,
+	.has_powergate = false,
 };
 
 static const struct tegra_dc_soc_info tegra114_dc_soc_info = {
@@ -1370,6 +1596,7 @@ static const struct tegra_dc_soc_info tegra114_dc_soc_info = {
 	.supports_cursor = false,
 	.supports_block_linear = false,
 	.pitch_align = 64,
+	.has_powergate = true,
 };
 
 static const struct tegra_dc_soc_info tegra124_dc_soc_info = {
@@ -1377,6 +1604,7 @@ static const struct tegra_dc_soc_info tegra124_dc_soc_info = {
 	.supports_cursor = true,
 	.supports_block_linear = true,
 	.pitch_align = 64,
+	.has_powergate = true,
 };
 
 static const struct of_device_id tegra_dc_of_match[] = {
@@ -1384,6 +1612,9 @@ static const struct of_device_id tegra_dc_of_match[] = {
 		.compatible = "nvidia,tegra124-dc",
 		.data = &tegra124_dc_soc_info,
 	}, {
+		.compatible = "nvidia,tegra114-dc",
+		.data = &tegra114_dc_soc_info,
+	}, {
 		.compatible = "nvidia,tegra30-dc",
 		.data = &tegra30_dc_soc_info,
 	}, {
@@ -1466,9 +1697,34 @@ static int tegra_dc_probe(struct platform_device *pdev)
 		return PTR_ERR(dc->rst);
 	}
 
-	err = clk_prepare_enable(dc->clk);
-	if (err < 0)
-		return err;
+	if (dc->soc->has_powergate) {
+		if (dc->pipe == 0)
+			dc->powergate = TEGRA_POWERGATE_DIS;
+		else
+			dc->powergate = TEGRA_POWERGATE_DISB;
+
+		err = tegra_powergate_sequence_power_up(dc->powergate, dc->clk,
+							dc->rst);
+		if (err < 0) {
+			dev_err(&pdev->dev, "failed to power partition: %d\n",
+				err);
+			return err;
+		}
+	} else {
+		err = clk_prepare_enable(dc->clk);
+		if (err < 0) {
+			dev_err(&pdev->dev, "failed to enable clock: %d\n",
+				err);
+			return err;
+		}
+
+		err = reset_control_deassert(dc->rst);
+		if (err < 0) {
+			dev_err(&pdev->dev, "failed to deassert reset: %d\n",
+				err);
+			return err;
+		}
+	}
 
 	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	dc->regs = devm_ioremap_resource(&pdev->dev, regs);
@@ -1522,6 +1778,10 @@ static int tegra_dc_remove(struct platform_device *pdev)
 	}
 
 	reset_control_assert(dc->rst);
+
+	if (dc->soc->has_powergate)
+		tegra_powergate_power_off(dc->powergate);
+
 	clk_disable_unprepare(dc->clk);
 
 	return 0;
diff --git a/drivers/gpu/drm/tegra/drm.c b/drivers/gpu/drm/tegra/drm.c
index 59736bb810cd..d4f827593dfa 100644
--- a/drivers/gpu/drm/tegra/drm.c
+++ b/drivers/gpu/drm/tegra/drm.c
@@ -8,6 +8,7 @@
  */
 
 #include <linux/host1x.h>
+#include <linux/iommu.h>
 
 #include "drm.h"
 #include "gem.h"
@@ -33,6 +34,17 @@ static int tegra_drm_load(struct drm_device *drm, unsigned long flags)
 	if (!tegra)
 		return -ENOMEM;
 
+	if (iommu_present(&platform_bus_type)) {
+		tegra->domain = iommu_domain_alloc(&platform_bus_type);
+		if (IS_ERR(tegra->domain)) {
+			err = PTR_ERR(tegra->domain);
+			goto free;
+		}
+
+		DRM_DEBUG("IOMMU context initialized\n");
+		drm_mm_init(&tegra->mm, 0, SZ_2G);
+	}
+
 	mutex_init(&tegra->clients_lock);
 	INIT_LIST_HEAD(&tegra->clients);
 	drm->dev_private = tegra;
@@ -42,13 +54,13 @@ static int tegra_drm_load(struct drm_device *drm, unsigned long flags)
 
 	err = tegra_drm_fb_prepare(drm);
 	if (err < 0)
-		return err;
+		goto config;
 
 	drm_kms_helper_poll_init(drm);
 
 	err = host1x_device_init(device);
 	if (err < 0)
-		return err;
+		goto fbdev;
 
 	/*
 	 * We don't use the drm_irq_install() helpers provided by the DRM
@@ -59,18 +71,37 @@ static int tegra_drm_load(struct drm_device *drm, unsigned long flags)
 
 	err = drm_vblank_init(drm, drm->mode_config.num_crtc);
 	if (err < 0)
-		return err;
+		goto device;
 
 	err = tegra_drm_fb_init(drm);
 	if (err < 0)
-		return err;
+		goto vblank;
 
 	return 0;
+
+vblank:
+	drm_vblank_cleanup(drm);
+device:
+	host1x_device_exit(device);
+fbdev:
+	drm_kms_helper_poll_fini(drm);
+	tegra_drm_fb_free(drm);
+config:
+	drm_mode_config_cleanup(drm);
+
+	if (tegra->domain) {
+		iommu_domain_free(tegra->domain);
+		drm_mm_takedown(&tegra->mm);
+	}
+free:
+	kfree(tegra);
+	return err;
 }
 
 static int tegra_drm_unload(struct drm_device *drm)
 {
 	struct host1x_device *device = to_host1x_device(drm->dev);
+	struct tegra_drm *tegra = drm->dev_private;
 	int err;
 
 	drm_kms_helper_poll_fini(drm);
@@ -82,6 +113,13 @@ static int tegra_drm_unload(struct drm_device *drm)
 	if (err < 0)
 		return err;
 
+	if (tegra->domain) {
+		iommu_domain_free(tegra->domain);
+		drm_mm_takedown(&tegra->mm);
+	}
+
+	kfree(tegra);
+
 	return 0;
 }
 
@@ -656,24 +694,28 @@ static const struct file_operations tegra_drm_fops = {
 	.llseek = noop_llseek,
 };
 
-static struct drm_crtc *tegra_crtc_from_pipe(struct drm_device *drm, int pipe)
+static struct drm_crtc *tegra_crtc_from_pipe(struct drm_device *drm,
+					     unsigned int pipe)
 {
 	struct drm_crtc *crtc;
 
 	list_for_each_entry(crtc, &drm->mode_config.crtc_list, head) {
-		struct tegra_dc *dc = to_tegra_dc(crtc);
-
-		if (dc->pipe == pipe)
+		if (pipe == drm_crtc_index(crtc))
 			return crtc;
 	}
 
 	return NULL;
 }
 
-static u32 tegra_drm_get_vblank_counter(struct drm_device *dev, int crtc)
+static u32 tegra_drm_get_vblank_counter(struct drm_device *drm, int pipe)
 {
+	struct drm_crtc *crtc = tegra_crtc_from_pipe(drm, pipe);
+
+	if (!crtc)
+		return 0;
+
 	/* TODO: implement real hardware counter using syncpoints */
-	return drm_vblank_count(dev, crtc);
+	return drm_crtc_vblank_count(crtc);
 }
 
 static int tegra_drm_enable_vblank(struct drm_device *drm, int pipe)
diff --git a/drivers/gpu/drm/tegra/drm.h b/drivers/gpu/drm/tegra/drm.h
index e89c70fa82d5..3a3b2e7b5b3f 100644
--- a/drivers/gpu/drm/tegra/drm.h
+++ b/drivers/gpu/drm/tegra/drm.h
@@ -39,6 +39,9 @@ struct tegra_fbdev {
 struct tegra_drm {
 	struct drm_device *drm;
 
+	struct iommu_domain *domain;
+	struct drm_mm mm;
+
 	struct mutex clients_lock;
 	struct list_head clients;
 
@@ -101,6 +104,7 @@ struct tegra_dc {
 	spinlock_t lock;
 
 	struct drm_crtc base;
+	int powergate;
 	int pipe;
 
 	struct clk *clk;
@@ -120,6 +124,8 @@ struct tegra_dc {
 	struct drm_pending_vblank_event *event;
 
 	const struct tegra_dc_soc_info *soc;
+
+	struct iommu_domain *domain;
 };
 
 static inline struct tegra_dc *
@@ -133,16 +139,15 @@ static inline struct tegra_dc *to_tegra_dc(struct drm_crtc *crtc)
 	return crtc ? container_of(crtc, struct tegra_dc, base) : NULL;
 }
 
-static inline void tegra_dc_writel(struct tegra_dc *dc, unsigned long value,
-				   unsigned long reg)
+static inline void tegra_dc_writel(struct tegra_dc *dc, u32 value,
+				   unsigned long offset)
 {
-	writel(value, dc->regs + (reg << 2));
+	writel(value, dc->regs + (offset << 2));
 }
 
-static inline unsigned long tegra_dc_readl(struct tegra_dc *dc,
-					   unsigned long reg)
+static inline u32 tegra_dc_readl(struct tegra_dc *dc, unsigned long offset)
 {
-	return readl(dc->regs + (reg << 2));
+	return readl(dc->regs + (offset << 2));
 }
 
 struct tegra_dc_window {
@@ -287,6 +292,7 @@ bool tegra_fb_is_bottom_up(struct drm_framebuffer *framebuffer);
 int tegra_fb_get_tiling(struct drm_framebuffer *framebuffer,
 			struct tegra_bo_tiling *tiling);
 int tegra_drm_fb_prepare(struct drm_device *drm);
+void tegra_drm_fb_free(struct drm_device *drm);
 int tegra_drm_fb_init(struct drm_device *drm);
 void tegra_drm_fb_exit(struct drm_device *drm);
 #ifdef CONFIG_DRM_TEGRA_FBDEV
diff --git a/drivers/gpu/drm/tegra/dsi.c b/drivers/gpu/drm/tegra/dsi.c
index f7874458926a..33f67fd601c6 100644
--- a/drivers/gpu/drm/tegra/dsi.c
+++ b/drivers/gpu/drm/tegra/dsi.c
@@ -11,6 +11,7 @@
 #include <linux/host1x.h>
 #include <linux/module.h>
 #include <linux/of.h>
+#include <linux/of_platform.h>
 #include <linux/platform_device.h>
 #include <linux/reset.h>
 
@@ -26,9 +27,6 @@
 #include "dsi.h"
 #include "mipi-phy.h"
 
-#define DSI_VIDEO_FIFO_DEPTH (1920 / 4)
-#define DSI_HOST_FIFO_DEPTH 64
-
 struct tegra_dsi {
 	struct host1x_client client;
 	struct tegra_output output;
@@ -54,6 +52,13 @@ struct tegra_dsi {
 
 	struct regulator *vdd;
 	bool enabled;
+
+	unsigned int video_fifo_depth;
+	unsigned int host_fifo_depth;
+
+	/* for ganged-mode support */
+	struct tegra_dsi *master;
+	struct tegra_dsi *slave;
 };
 
 static inline struct tegra_dsi *
@@ -318,6 +323,21 @@ static const u32 pkt_seq_video_non_burst_sync_events[NUM_PKT_SEQ] = {
 	[11] = PKT_ID0(MIPI_DSI_BLANKING_PACKET) | PKT_LEN0(4),
 };
 
+static const u32 pkt_seq_command_mode[NUM_PKT_SEQ] = {
+	[ 0] = 0,
+	[ 1] = 0,
+	[ 2] = 0,
+	[ 3] = 0,
+	[ 4] = 0,
+	[ 5] = 0,
+	[ 6] = PKT_ID0(MIPI_DSI_DCS_LONG_WRITE) | PKT_LEN0(3) | PKT_LP,
+	[ 7] = 0,
+	[ 8] = 0,
+	[ 9] = 0,
+	[10] = PKT_ID0(MIPI_DSI_DCS_LONG_WRITE) | PKT_LEN0(5) | PKT_LP,
+	[11] = 0,
+};
+
 static int tegra_dsi_set_phy_timing(struct tegra_dsi *dsi)
 {
 	struct mipi_dphy_timing timing;
@@ -329,7 +349,7 @@ static int tegra_dsi_set_phy_timing(struct tegra_dsi *dsi)
 	if (rate < 0)
 		return rate;
 
-	period = DIV_ROUND_CLOSEST(1000000000UL, rate * 2);
+	period = DIV_ROUND_CLOSEST(NSEC_PER_SEC, rate * 2);
 
 	err = mipi_dphy_timing_get_default(&timing, period);
 	if (err < 0)
@@ -369,6 +389,9 @@ static int tegra_dsi_set_phy_timing(struct tegra_dsi *dsi)
 		DSI_TIMING_FIELD(timing.tago, period, 1);
 	tegra_dsi_writel(dsi, value, DSI_BTA_TIMING);
 
+	if (dsi->slave)
+		return tegra_dsi_set_phy_timing(dsi->slave);
+
 	return 0;
 }
 
@@ -426,26 +449,59 @@ static int tegra_dsi_get_format(enum mipi_dsi_pixel_format format,
 	return 0;
 }
 
-static int tegra_output_dsi_enable(struct tegra_output *output)
+static void tegra_dsi_ganged_enable(struct tegra_dsi *dsi, unsigned int start,
+				    unsigned int size)
+{
+	u32 value;
+
+	tegra_dsi_writel(dsi, start, DSI_GANGED_MODE_START);
+	tegra_dsi_writel(dsi, size << 16 | size, DSI_GANGED_MODE_SIZE);
+
+	value = DSI_GANGED_MODE_CONTROL_ENABLE;
+	tegra_dsi_writel(dsi, value, DSI_GANGED_MODE_CONTROL);
+}
+
+static void tegra_dsi_enable(struct tegra_dsi *dsi)
+{
+	u32 value;
+
+	value = tegra_dsi_readl(dsi, DSI_POWER_CONTROL);
+	value |= DSI_POWER_CONTROL_ENABLE;
+	tegra_dsi_writel(dsi, value, DSI_POWER_CONTROL);
+
+	if (dsi->slave)
+		tegra_dsi_enable(dsi->slave);
+}
+
+static unsigned int tegra_dsi_get_lanes(struct tegra_dsi *dsi)
+{
+	if (dsi->master)
+		return dsi->master->lanes + dsi->lanes;
+
+	if (dsi->slave)
+		return dsi->lanes + dsi->slave->lanes;
+
+	return dsi->lanes;
+}
+
+static int tegra_dsi_configure(struct tegra_dsi *dsi, unsigned int pipe,
+			       const struct drm_display_mode *mode)
 {
-	struct tegra_dc *dc = to_tegra_dc(output->encoder.crtc);
-	struct drm_display_mode *mode = &dc->base.mode;
 	unsigned int hact, hsw, hbp, hfp, i, mul, div;
-	struct tegra_dsi *dsi = to_dsi(output);
 	enum tegra_dsi_format format;
-	unsigned long value;
 	const u32 *pkt_seq;
+	u32 value;
 	int err;
 
-	if (dsi->enabled)
-		return 0;
-
 	if (dsi->flags & MIPI_DSI_MODE_VIDEO_SYNC_PULSE) {
 		DRM_DEBUG_KMS("Non-burst video mode with sync pulses\n");
 		pkt_seq = pkt_seq_video_non_burst_sync_pulses;
-	} else {
+	} else if (dsi->flags & MIPI_DSI_MODE_VIDEO) {
 		DRM_DEBUG_KMS("Non-burst video mode with sync events\n");
 		pkt_seq = pkt_seq_video_non_burst_sync_events;
+	} else {
+		DRM_DEBUG_KMS("Command mode\n");
+		pkt_seq = pkt_seq_command_mode;
 	}
 
 	err = tegra_dsi_get_muldiv(dsi->format, &mul, &div);
@@ -456,61 +512,136 @@ static int tegra_output_dsi_enable(struct tegra_output *output)
 	if (err < 0)
 		return err;
 
-	err = clk_enable(dsi->clk);
-	if (err < 0)
-		return err;
-
-	reset_control_deassert(dsi->rst);
-
 	value = DSI_CONTROL_CHANNEL(0) | DSI_CONTROL_FORMAT(format) |
 		DSI_CONTROL_LANES(dsi->lanes - 1) |
-		DSI_CONTROL_SOURCE(dc->pipe);
+		DSI_CONTROL_SOURCE(pipe);
 	tegra_dsi_writel(dsi, value, DSI_CONTROL);
 
-	tegra_dsi_writel(dsi, DSI_VIDEO_FIFO_DEPTH, DSI_MAX_THRESHOLD);
+	tegra_dsi_writel(dsi, dsi->video_fifo_depth, DSI_MAX_THRESHOLD);
 
-	value = DSI_HOST_CONTROL_HS | DSI_HOST_CONTROL_CS |
-		DSI_HOST_CONTROL_ECC;
+	value = DSI_HOST_CONTROL_HS;
 	tegra_dsi_writel(dsi, value, DSI_HOST_CONTROL);
 
 	value = tegra_dsi_readl(dsi, DSI_CONTROL);
+
 	if (dsi->flags & MIPI_DSI_CLOCK_NON_CONTINUOUS)
 		value |= DSI_CONTROL_HS_CLK_CTRL;
+
 	value &= ~DSI_CONTROL_TX_TRIG(3);
-	value &= ~DSI_CONTROL_DCS_ENABLE;
+
+	/* enable DCS commands for command mode */
+	if (dsi->flags & MIPI_DSI_MODE_VIDEO)
+		value &= ~DSI_CONTROL_DCS_ENABLE;
+	else
+		value |= DSI_CONTROL_DCS_ENABLE;
+
 	value |= DSI_CONTROL_VIDEO_ENABLE;
 	value &= ~DSI_CONTROL_HOST_ENABLE;
 	tegra_dsi_writel(dsi, value, DSI_CONTROL);
 
-	err = tegra_dsi_set_phy_timing(dsi);
-	if (err < 0)
-		return err;
-
 	for (i = 0; i < NUM_PKT_SEQ; i++)
 		tegra_dsi_writel(dsi, pkt_seq[i], DSI_PKT_SEQ_0_LO + i);
 
-	/* horizontal active pixels */
-	hact = mode->hdisplay * mul / div;
+	if (dsi->flags & MIPI_DSI_MODE_VIDEO) {
+		/* horizontal active pixels */
+		hact = mode->hdisplay * mul / div;
 
-	/* horizontal sync width */
-	hsw = (mode->hsync_end - mode->hsync_start) * mul / div;
-	hsw -= 10;
+		/* horizontal sync width */
+		hsw = (mode->hsync_end - mode->hsync_start) * mul / div;
+		hsw -= 10;
 
-	/* horizontal back porch */
-	hbp = (mode->htotal - mode->hsync_end) * mul / div;
-	hbp -= 14;
+		/* horizontal back porch */
+		hbp = (mode->htotal - mode->hsync_end) * mul / div;
+		hbp -= 14;
 
-	/* horizontal front porch */
-	hfp = (mode->hsync_start  - mode->hdisplay) * mul / div;
-	hfp -= 8;
+		/* horizontal front porch */
+		hfp = (mode->hsync_start - mode->hdisplay) * mul / div;
+		hfp -= 8;
 
-	tegra_dsi_writel(dsi, hsw << 16 | 0, DSI_PKT_LEN_0_1);
-	tegra_dsi_writel(dsi, hact << 16 | hbp, DSI_PKT_LEN_2_3);
-	tegra_dsi_writel(dsi, hfp, DSI_PKT_LEN_4_5);
-	tegra_dsi_writel(dsi, 0x0f0f << 16, DSI_PKT_LEN_6_7);
+		tegra_dsi_writel(dsi, hsw << 16 | 0, DSI_PKT_LEN_0_1);
+		tegra_dsi_writel(dsi, hact << 16 | hbp, DSI_PKT_LEN_2_3);
+		tegra_dsi_writel(dsi, hfp, DSI_PKT_LEN_4_5);
+		tegra_dsi_writel(dsi, 0x0f0f << 16, DSI_PKT_LEN_6_7);
 
-	/* set SOL delay */
-	tegra_dsi_writel(dsi, 8 * mul / div, DSI_SOL_DELAY);
+		/* set SOL delay (for non-burst mode only) */
+		tegra_dsi_writel(dsi, 8 * mul / div, DSI_SOL_DELAY);
+
+		/* TODO: implement ganged mode */
+	} else {
+		u16 bytes;
+
+		if (dsi->master || dsi->slave) {
+			/*
+			 * For ganged mode, assume symmetric left-right mode.
+			 */
+			bytes = 1 + (mode->hdisplay / 2) * mul / div;
+		} else {
+			/* 1 byte (DCS command) + pixel data */
+			bytes = 1 + mode->hdisplay * mul / div;
+		}
+
+		tegra_dsi_writel(dsi, 0, DSI_PKT_LEN_0_1);
+		tegra_dsi_writel(dsi, bytes << 16, DSI_PKT_LEN_2_3);
+		tegra_dsi_writel(dsi, bytes << 16, DSI_PKT_LEN_4_5);
+		tegra_dsi_writel(dsi, 0, DSI_PKT_LEN_6_7);
+
+		value = MIPI_DCS_WRITE_MEMORY_START << 8 |
+			MIPI_DCS_WRITE_MEMORY_CONTINUE;
+		tegra_dsi_writel(dsi, value, DSI_DCS_CMDS);
+
+		/* set SOL delay */
+		if (dsi->master || dsi->slave) {
+			unsigned int lanes = tegra_dsi_get_lanes(dsi);
+			unsigned long delay, bclk, bclk_ganged;
+
+			/* SOL to valid, valid to FIFO and FIFO write delay */
+			delay = 4 + 4 + 2;
+			delay = DIV_ROUND_UP(delay * mul, div * lanes);
+			/* FIFO read delay */
+			delay = delay + 6;
+
+			bclk = DIV_ROUND_UP(mode->htotal * mul, div * lanes);
+			bclk_ganged = DIV_ROUND_UP(bclk * lanes / 2, lanes);
+			value = bclk - bclk_ganged + delay + 20;
+		} else {
+			/* TODO: revisit for non-ganged mode */
+			value = 8 * mul / div;
+		}
+
+		tegra_dsi_writel(dsi, value, DSI_SOL_DELAY);
+	}
+
+	if (dsi->slave) {
+		err = tegra_dsi_configure(dsi->slave, pipe, mode);
+		if (err < 0)
+			return err;
+
+		/*
+		 * TODO: Support modes other than symmetrical left-right
+		 * split.
+		 */
+		tegra_dsi_ganged_enable(dsi, 0, mode->hdisplay / 2);
+		tegra_dsi_ganged_enable(dsi->slave, mode->hdisplay / 2,
+					mode->hdisplay / 2);
+	}
+
+	return 0;
+}
+
+static int tegra_output_dsi_enable(struct tegra_output *output)
+{
+	struct tegra_dc *dc = to_tegra_dc(output->encoder.crtc);
+	const struct drm_display_mode *mode = &dc->base.mode;
+	struct tegra_dsi *dsi = to_dsi(output);
+	u32 value;
+	int err;
+
+	if (dsi->enabled)
+		return 0;
+
+	err = tegra_dsi_configure(dsi, dc->pipe, mode);
+	if (err < 0)
+		return err;
 
 	/* enable display controller */
 	value = tegra_dc_readl(dc, DC_DISP_DISP_WIN_OPTIONS);
@@ -531,28 +662,79 @@ static int tegra_output_dsi_enable(struct tegra_output *output)
 	tegra_dc_writel(dc, GENERAL_ACT_REQ, DC_CMD_STATE_CONTROL);
 
 	/* enable DSI controller */
-	value = tegra_dsi_readl(dsi, DSI_POWER_CONTROL);
-	value |= DSI_POWER_CONTROL_ENABLE;
-	tegra_dsi_writel(dsi, value, DSI_POWER_CONTROL);
+	tegra_dsi_enable(dsi);
 
 	dsi->enabled = true;
 
 	return 0;
 }
 
+static int tegra_dsi_wait_idle(struct tegra_dsi *dsi, unsigned long timeout)
+{
+	u32 value;
+
+	timeout = jiffies + msecs_to_jiffies(timeout);
+
+	while (time_before(jiffies, timeout)) {
+		value = tegra_dsi_readl(dsi, DSI_STATUS);
+		if (value & DSI_STATUS_IDLE)
+			return 0;
+
+		usleep_range(1000, 2000);
+	}
+
+	return -ETIMEDOUT;
+}
+
+static void tegra_dsi_video_disable(struct tegra_dsi *dsi)
+{
+	u32 value;
+
+	value = tegra_dsi_readl(dsi, DSI_CONTROL);
+	value &= ~DSI_CONTROL_VIDEO_ENABLE;
+	tegra_dsi_writel(dsi, value, DSI_CONTROL);
+
+	if (dsi->slave)
+		tegra_dsi_video_disable(dsi->slave);
+}
+
+static void tegra_dsi_ganged_disable(struct tegra_dsi *dsi)
+{
+	tegra_dsi_writel(dsi, 0, DSI_GANGED_MODE_START);
+	tegra_dsi_writel(dsi, 0, DSI_GANGED_MODE_SIZE);
+	tegra_dsi_writel(dsi, 0, DSI_GANGED_MODE_CONTROL);
+}
+
+static void tegra_dsi_disable(struct tegra_dsi *dsi)
+{
+	u32 value;
+
+	if (dsi->slave) {
+		tegra_dsi_ganged_disable(dsi->slave);
+		tegra_dsi_ganged_disable(dsi);
+	}
+
+	value = tegra_dsi_readl(dsi, DSI_POWER_CONTROL);
+	value &= ~DSI_POWER_CONTROL_ENABLE;
+	tegra_dsi_writel(dsi, value, DSI_POWER_CONTROL);
+
+	if (dsi->slave)
+		tegra_dsi_disable(dsi->slave);
+
+	usleep_range(5000, 10000);
+}
+
 static int tegra_output_dsi_disable(struct tegra_output *output)
 {
 	struct tegra_dc *dc = to_tegra_dc(output->encoder.crtc);
 	struct tegra_dsi *dsi = to_dsi(output);
 	unsigned long value;
+	int err;
 
 	if (!dsi->enabled)
 		return 0;
 
-	/* disable DSI controller */
-	value = tegra_dsi_readl(dsi, DSI_POWER_CONTROL);
-	value &= ~DSI_POWER_CONTROL_ENABLE;
-	tegra_dsi_writel(dsi, value, DSI_POWER_CONTROL);
+	tegra_dsi_video_disable(dsi);
 
 	/*
 	 * The following accesses registers of the display controller, so make
@@ -576,39 +758,68 @@ static int tegra_output_dsi_disable(struct tegra_output *output)
 		tegra_dc_writel(dc, GENERAL_ACT_REQ, DC_CMD_STATE_CONTROL);
 	}
 
-	clk_disable(dsi->clk);
+	err = tegra_dsi_wait_idle(dsi, 100);
+	if (err < 0)
+		dev_dbg(dsi->dev, "failed to idle DSI: %d\n", err);
+
+	tegra_dsi_disable(dsi);
 
 	dsi->enabled = false;
 
 	return 0;
 }
 
+static void tegra_dsi_set_timeout(struct tegra_dsi *dsi, unsigned long bclk,
+				  unsigned int vrefresh)
+{
+	unsigned int timeout;
+	u32 value;
+
+	/* one frame high-speed transmission timeout */
+	timeout = (bclk / vrefresh) / 512;
+	value = DSI_TIMEOUT_LRX(0x2000) | DSI_TIMEOUT_HTX(timeout);
+	tegra_dsi_writel(dsi, value, DSI_TIMEOUT_0);
+
+	/* 2 ms peripheral timeout for panel */
+	timeout = 2 * bclk / 512 * 1000;
+	value = DSI_TIMEOUT_PR(timeout) | DSI_TIMEOUT_TA(0x2000);
+	tegra_dsi_writel(dsi, value, DSI_TIMEOUT_1);
+
+	value = DSI_TALLY_TA(0) | DSI_TALLY_LRX(0) | DSI_TALLY_HTX(0);
+	tegra_dsi_writel(dsi, value, DSI_TO_TALLY);
+
+	if (dsi->slave)
+		tegra_dsi_set_timeout(dsi->slave, bclk, vrefresh);
+}
+
 static int tegra_output_dsi_setup_clock(struct tegra_output *output,
 					struct clk *clk, unsigned long pclk,
 					unsigned int *divp)
 {
 	struct tegra_dc *dc = to_tegra_dc(output->encoder.crtc);
 	struct drm_display_mode *mode = &dc->base.mode;
-	unsigned int timeout, mul, div, vrefresh;
 	struct tegra_dsi *dsi = to_dsi(output);
-	unsigned long bclk, plld, value;
+	unsigned int mul, div, vrefresh, lanes;
+	unsigned long bclk, plld;
 	int err;
 
+	lanes = tegra_dsi_get_lanes(dsi);
+
 	err = tegra_dsi_get_muldiv(dsi->format, &mul, &div);
 	if (err < 0)
 		return err;
 
-	DRM_DEBUG_KMS("mul: %u, div: %u, lanes: %u\n", mul, div, dsi->lanes);
+	DRM_DEBUG_KMS("mul: %u, div: %u, lanes: %u\n", mul, div, lanes);
 	vrefresh = drm_mode_vrefresh(mode);
 	DRM_DEBUG_KMS("vrefresh: %u\n", vrefresh);
 
 	/* compute byte clock */
-	bclk = (pclk * mul) / (div * dsi->lanes);
+	bclk = (pclk * mul) / (div * lanes);
 
 	/*
 	 * Compute bit clock and round up to the next MHz.
 	 */
-	plld = DIV_ROUND_UP(bclk * 8, 1000000) * 1000000;
+	plld = DIV_ROUND_UP(bclk * 8, USEC_PER_SEC) * USEC_PER_SEC;
 
 	/*
 	 * We divide the frequency by two here, but we make up for that by
@@ -640,25 +851,17 @@ static int tegra_output_dsi_setup_clock(struct tegra_output *output,
 	 * not working properly otherwise. Perhaps the PLLs cannot generate
 	 * frequencies sufficiently high.
 	 */
-	*divp = ((8 * mul) / (div * dsi->lanes)) - 2;
+	*divp = ((8 * mul) / (div * lanes)) - 2;
 
 	/*
 	 * XXX: Move the below somewhere else so that we don't need to have
 	 * access to the vrefresh in this function?
 	 */
+	tegra_dsi_set_timeout(dsi, bclk, vrefresh);
 
-	/* one frame high-speed transmission timeout */
-	timeout = (bclk / vrefresh) / 512;
-	value = DSI_TIMEOUT_LRX(0x2000) | DSI_TIMEOUT_HTX(timeout);
-	tegra_dsi_writel(dsi, value, DSI_TIMEOUT_0);
-
-	/* 2 ms peripheral timeout for panel */
-	timeout = 2 * bclk / 512 * 1000;
-	value = DSI_TIMEOUT_PR(timeout) | DSI_TIMEOUT_TA(0x2000);
-	tegra_dsi_writel(dsi, value, DSI_TIMEOUT_1);
-
-	value = DSI_TALLY_TA(0) | DSI_TALLY_LRX(0) | DSI_TALLY_HTX(0);
-	tegra_dsi_writel(dsi, value, DSI_TO_TALLY);
+	err = tegra_dsi_set_phy_timing(dsi);
+	if (err < 0)
+		return err;
 
 	return 0;
 }
@@ -695,7 +898,7 @@ static int tegra_dsi_pad_enable(struct tegra_dsi *dsi)
 
 static int tegra_dsi_pad_calibrate(struct tegra_dsi *dsi)
 {
-	unsigned long value;
+	u32 value;
 
 	tegra_dsi_writel(dsi, 0, DSI_PAD_CONTROL_0);
 	tegra_dsi_writel(dsi, 0, DSI_PAD_CONTROL_1);
@@ -720,14 +923,17 @@ static int tegra_dsi_init(struct host1x_client *client)
 	struct tegra_dsi *dsi = host1x_client_to_dsi(client);
 	int err;
 
-	dsi->output.type = TEGRA_OUTPUT_DSI;
-	dsi->output.dev = client->dev;
-	dsi->output.ops = &dsi_ops;
-
-	err = tegra_output_init(drm, &dsi->output);
-	if (err < 0) {
-		dev_err(client->dev, "output setup failed: %d\n", err);
-		return err;
+	/* Gangsters must not register their own outputs. */
+	if (!dsi->master) {
+		dsi->output.type = TEGRA_OUTPUT_DSI;
+		dsi->output.dev = client->dev;
+		dsi->output.ops = &dsi_ops;
+
+		err = tegra_output_init(drm, &dsi->output);
+		if (err < 0) {
+			dev_err(client->dev, "output setup failed: %d\n", err);
+			return err;
+		}
 	}
 
 	if (IS_ENABLED(CONFIG_DEBUG_FS)) {
@@ -736,12 +942,6 @@ static int tegra_dsi_init(struct host1x_client *client)
 			dev_err(dsi->dev, "debugfs setup failed: %d\n", err);
 	}
 
-	err = tegra_dsi_pad_calibrate(dsi);
-	if (err < 0) {
-		dev_err(dsi->dev, "MIPI calibration failed: %d\n", err);
-		return err;
-	}
-
 	return 0;
 }
 
@@ -756,16 +956,20 @@ static int tegra_dsi_exit(struct host1x_client *client)
 			dev_err(dsi->dev, "debugfs cleanup failed: %d\n", err);
 	}
 
-	err = tegra_output_disable(&dsi->output);
-	if (err < 0) {
-		dev_err(client->dev, "output failed to disable: %d\n", err);
-		return err;
-	}
-
-	err = tegra_output_exit(&dsi->output);
-	if (err < 0) {
-		dev_err(client->dev, "output cleanup failed: %d\n", err);
-		return err;
+	if (!dsi->master) {
+		err = tegra_output_disable(&dsi->output);
+		if (err < 0) {
+			dev_err(client->dev, "output failed to disable: %d\n",
+				err);
+			return err;
+		}
+
+		err = tegra_output_exit(&dsi->output);
+		if (err < 0) {
+			dev_err(client->dev, "output cleanup failed: %d\n",
+				err);
+			return err;
+		}
 	}
 
 	return 0;
@@ -792,20 +996,324 @@ static int tegra_dsi_setup_clocks(struct tegra_dsi *dsi)
 	return 0;
 }
 
+static const char * const error_report[16] = {
+	"SoT Error",
+	"SoT Sync Error",
+	"EoT Sync Error",
+	"Escape Mode Entry Command Error",
+	"Low-Power Transmit Sync Error",
+	"Peripheral Timeout Error",
+	"False Control Error",
+	"Contention Detected",
+	"ECC Error, single-bit",
+	"ECC Error, multi-bit",
+	"Checksum Error",
+	"DSI Data Type Not Recognized",
+	"DSI VC ID Invalid",
+	"Invalid Transmission Length",
+	"Reserved",
+	"DSI Protocol Violation",
+};
+
+static ssize_t tegra_dsi_read_response(struct tegra_dsi *dsi,
+				       const struct mipi_dsi_msg *msg,
+				       size_t count)
+{
+	u8 *rx = msg->rx_buf;
+	unsigned int i, j, k;
+	size_t size = 0;
+	u16 errors;
+	u32 value;
+
+	/* read and parse packet header */
+	value = tegra_dsi_readl(dsi, DSI_RD_DATA);
+
+	switch (value & 0x3f) {
+	case MIPI_DSI_RX_ACKNOWLEDGE_AND_ERROR_REPORT:
+		errors = (value >> 8) & 0xffff;
+		dev_dbg(dsi->dev, "Acknowledge and error report: %04x\n",
+			errors);
+		for (i = 0; i < ARRAY_SIZE(error_report); i++)
+			if (errors & BIT(i))
+				dev_dbg(dsi->dev, "  %2u: %s\n", i,
+					error_report[i]);
+		break;
+
+	case MIPI_DSI_RX_DCS_SHORT_READ_RESPONSE_1BYTE:
+		rx[0] = (value >> 8) & 0xff;
+		size = 1;
+		break;
+
+	case MIPI_DSI_RX_DCS_SHORT_READ_RESPONSE_2BYTE:
+		rx[0] = (value >>  8) & 0xff;
+		rx[1] = (value >> 16) & 0xff;
+		size = 2;
+		break;
+
+	case MIPI_DSI_RX_DCS_LONG_READ_RESPONSE:
+		size = ((value >> 8) & 0xff00) | ((value >> 8) & 0xff);
+		break;
+
+	case MIPI_DSI_RX_GENERIC_LONG_READ_RESPONSE:
+		size = ((value >> 8) & 0xff00) | ((value >> 8) & 0xff);
+		break;
+
+	default:
+		dev_err(dsi->dev, "unhandled response type: %02x\n",
+			value & 0x3f);
+		return -EPROTO;
+	}
+
+	size = min(size, msg->rx_len);
+
+	if (msg->rx_buf && size > 0) {
+		for (i = 0, j = 0; i < count - 1; i++, j += 4) {
+			u8 *rx = msg->rx_buf + j;
+
+			value = tegra_dsi_readl(dsi, DSI_RD_DATA);
+
+			for (k = 0; k < 4 && (j + k) < msg->rx_len; k++)
+				rx[j + k] = (value >> (k << 3)) & 0xff;
+		}
+	}
+
+	return size;
+}
+
+static int tegra_dsi_transmit(struct tegra_dsi *dsi, unsigned long timeout)
+{
+	tegra_dsi_writel(dsi, DSI_TRIGGER_HOST, DSI_TRIGGER);
+
+	timeout = jiffies + msecs_to_jiffies(timeout);
+
+	while (time_before(jiffies, timeout)) {
+		u32 value = tegra_dsi_readl(dsi, DSI_TRIGGER);
+		if ((value & DSI_TRIGGER_HOST) == 0)
+			return 0;
+
+		usleep_range(1000, 2000);
+	}
+
+	DRM_DEBUG_KMS("timeout waiting for transmission to complete\n");
+	return -ETIMEDOUT;
+}
+
+static int tegra_dsi_wait_for_response(struct tegra_dsi *dsi,
+				       unsigned long timeout)
+{
+	timeout = jiffies + msecs_to_jiffies(250);
+
+	while (time_before(jiffies, timeout)) {
+		u32 value = tegra_dsi_readl(dsi, DSI_STATUS);
+		u8 count = value & 0x1f;
+
+		if (count > 0)
+			return count;
+
+		usleep_range(1000, 2000);
+	}
+
+	DRM_DEBUG_KMS("peripheral returned no data\n");
+	return -ETIMEDOUT;
+}
+
+static void tegra_dsi_writesl(struct tegra_dsi *dsi, unsigned long offset,
+			      const void *buffer, size_t size)
+{
+	const u8 *buf = buffer;
+	size_t i, j;
+	u32 value;
+
+	for (j = 0; j < size; j += 4) {
+		value = 0;
+
+		for (i = 0; i < 4 && j + i < size; i++)
+			value |= buf[j + i] << (i << 3);
+
+		tegra_dsi_writel(dsi, value, DSI_WR_DATA);
+	}
+}
+
+static ssize_t tegra_dsi_host_transfer(struct mipi_dsi_host *host,
+				       const struct mipi_dsi_msg *msg)
+{
+	struct tegra_dsi *dsi = host_to_tegra(host);
+	struct mipi_dsi_packet packet;
+	const u8 *header;
+	size_t count;
+	ssize_t err;
+	u32 value;
+
+	err = mipi_dsi_create_packet(&packet, msg);
+	if (err < 0)
+		return err;
+
+	header = packet.header;
+
+	/* maximum FIFO depth is 1920 words */
+	if (packet.size > dsi->video_fifo_depth * 4)
+		return -ENOSPC;
+
+	/* reset underflow/overflow flags */
+	value = tegra_dsi_readl(dsi, DSI_STATUS);
+	if (value & (DSI_STATUS_UNDERFLOW | DSI_STATUS_OVERFLOW)) {
+		value = DSI_HOST_CONTROL_FIFO_RESET;
+		tegra_dsi_writel(dsi, value, DSI_HOST_CONTROL);
+		usleep_range(10, 20);
+	}
+
+	value = tegra_dsi_readl(dsi, DSI_POWER_CONTROL);
+	value |= DSI_POWER_CONTROL_ENABLE;
+	tegra_dsi_writel(dsi, value, DSI_POWER_CONTROL);
+
+	usleep_range(5000, 10000);
+
+	value = DSI_HOST_CONTROL_CRC_RESET | DSI_HOST_CONTROL_TX_TRIG_HOST |
+		DSI_HOST_CONTROL_CS | DSI_HOST_CONTROL_ECC;
+
+	if ((msg->flags & MIPI_DSI_MSG_USE_LPM) == 0)
+		value |= DSI_HOST_CONTROL_HS;
+
+	/*
+	 * The host FIFO has a maximum of 64 words, so larger transmissions
+	 * need to use the video FIFO.
+	 */
+	if (packet.size > dsi->host_fifo_depth * 4)
+		value |= DSI_HOST_CONTROL_FIFO_SEL;
+
+	tegra_dsi_writel(dsi, value, DSI_HOST_CONTROL);
+
+	/*
+	 * For reads and messages with explicitly requested ACK, generate a
+	 * BTA sequence after the transmission of the packet.
+	 */
+	if ((msg->flags & MIPI_DSI_MSG_REQ_ACK) ||
+	    (msg->rx_buf && msg->rx_len > 0)) {
+		value = tegra_dsi_readl(dsi, DSI_HOST_CONTROL);
+		value |= DSI_HOST_CONTROL_PKT_BTA;
+		tegra_dsi_writel(dsi, value, DSI_HOST_CONTROL);
+	}
+
+	value = DSI_CONTROL_LANES(0) | DSI_CONTROL_HOST_ENABLE;
+	tegra_dsi_writel(dsi, value, DSI_CONTROL);
+
+	/* write packet header, ECC is generated by hardware */
+	value = header[2] << 16 | header[1] << 8 | header[0];
+	tegra_dsi_writel(dsi, value, DSI_WR_DATA);
+
+	/* write payload (if any) */
+	if (packet.payload_length > 0)
+		tegra_dsi_writesl(dsi, DSI_WR_DATA, packet.payload,
+				  packet.payload_length);
+
+	err = tegra_dsi_transmit(dsi, 250);
+	if (err < 0)
+		return err;
+
+	if ((msg->flags & MIPI_DSI_MSG_REQ_ACK) ||
+	    (msg->rx_buf && msg->rx_len > 0)) {
+		err = tegra_dsi_wait_for_response(dsi, 250);
+		if (err < 0)
+			return err;
+
+		count = err;
+
+		value = tegra_dsi_readl(dsi, DSI_RD_DATA);
+		switch (value) {
+		case 0x84:
+			/*
+			dev_dbg(dsi->dev, "ACK\n");
+			*/
+			break;
+
+		case 0x87:
+			/*
+			dev_dbg(dsi->dev, "ESCAPE\n");
+			*/
+			break;
+
+		default:
+			dev_err(dsi->dev, "unknown status: %08x\n", value);
+			break;
+		}
+
+		if (count > 1) {
+			err = tegra_dsi_read_response(dsi, msg, count);
+			if (err < 0)
+				dev_err(dsi->dev,
+					"failed to parse response: %zd\n",
+					err);
+			else {
+				/*
+				 * For read commands, return the number of
+				 * bytes returned by the peripheral.
+				 */
+				count = err;
+			}
+		}
+	} else {
+		/*
+		 * For write commands, we have transmitted the 4-byte header
+		 * plus the variable-length payload.
+		 */
+		count = 4 + packet.payload_length;
+	}
+
+	return count;
+}
+
+static int tegra_dsi_ganged_setup(struct tegra_dsi *dsi)
+{
+	struct clk *parent;
+	int err;
+
+	/* make sure both DSI controllers share the same PLL */
+	parent = clk_get_parent(dsi->slave->clk);
+	if (!parent)
+		return -EINVAL;
+
+	err = clk_set_parent(parent, dsi->clk_parent);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+
 static int tegra_dsi_host_attach(struct mipi_dsi_host *host,
 				 struct mipi_dsi_device *device)
 {
 	struct tegra_dsi *dsi = host_to_tegra(host);
-	struct tegra_output *output = &dsi->output;
 
 	dsi->flags = device->mode_flags;
 	dsi->format = device->format;
 	dsi->lanes = device->lanes;
 
-	output->panel = of_drm_find_panel(device->dev.of_node);
-	if (output->panel) {
-		if (output->connector.dev)
+	if (dsi->slave) {
+		int err;
+
+		dev_dbg(dsi->dev, "attaching dual-channel device %s\n",
+			dev_name(&device->dev));
+
+		err = tegra_dsi_ganged_setup(dsi);
+		if (err < 0) {
+			dev_err(dsi->dev, "failed to set up ganged mode: %d\n",
+				err);
+			return err;
+		}
+	}
+
+	/*
+	 * Slaves don't have a panel associated with them, so they provide
+	 * merely the second channel.
+	 */
+	if (!dsi->master) {
+		struct tegra_output *output = &dsi->output;
+
+		output->panel = of_drm_find_panel(device->dev.of_node);
+		if (output->panel && output->connector.dev) {
+			drm_panel_attach(output->panel, &output->connector);
 			drm_helper_hpd_irq_event(output->connector.dev);
+		}
 	}
 
 	return 0;
@@ -818,10 +1326,10 @@ static int tegra_dsi_host_detach(struct mipi_dsi_host *host,
 	struct tegra_output *output = &dsi->output;
 
 	if (output->panel && &device->dev == output->panel->dev) {
+		output->panel = NULL;
+
 		if (output->connector.dev)
 			drm_helper_hpd_irq_event(output->connector.dev);
-
-		output->panel = NULL;
 	}
 
 	return 0;
@@ -830,8 +1338,29 @@ static int tegra_dsi_host_detach(struct mipi_dsi_host *host,
 static const struct mipi_dsi_host_ops tegra_dsi_host_ops = {
 	.attach = tegra_dsi_host_attach,
 	.detach = tegra_dsi_host_detach,
+	.transfer = tegra_dsi_host_transfer,
 };
 
+static int tegra_dsi_ganged_probe(struct tegra_dsi *dsi)
+{
+	struct device_node *np;
+
+	np = of_parse_phandle(dsi->dev->of_node, "nvidia,ganged-mode", 0);
+	if (np) {
+		struct platform_device *gangster = of_find_device_by_node(np);
+
+		dsi->slave = platform_get_drvdata(gangster);
+		of_node_put(np);
+
+		if (!dsi->slave)
+			return -EPROBE_DEFER;
+
+		dsi->slave->master = dsi;
+	}
+
+	return 0;
+}
+
 static int tegra_dsi_probe(struct platform_device *pdev)
 {
 	struct tegra_dsi *dsi;
@@ -843,11 +1372,19 @@ static int tegra_dsi_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	dsi->output.dev = dsi->dev = &pdev->dev;
+	dsi->video_fifo_depth = 1920;
+	dsi->host_fifo_depth = 64;
+
+	err = tegra_dsi_ganged_probe(dsi);
+	if (err < 0)
+		return err;
 
 	err = tegra_output_probe(&dsi->output);
 	if (err < 0)
 		return err;
 
+	dsi->output.connector.polled = DRM_CONNECTOR_POLL_HPD;
+
 	/*
 	 * Assume these values by default. When a DSI peripheral driver
 	 * attaches to the DSI host, the parameters will be taken from
@@ -861,68 +1398,83 @@ static int tegra_dsi_probe(struct platform_device *pdev)
 	if (IS_ERR(dsi->rst))
 		return PTR_ERR(dsi->rst);
 
+	err = reset_control_deassert(dsi->rst);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to bring DSI out of reset: %d\n",
+			err);
+		return err;
+	}
+
 	dsi->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(dsi->clk)) {
 		dev_err(&pdev->dev, "cannot get DSI clock\n");
-		return PTR_ERR(dsi->clk);
+		err = PTR_ERR(dsi->clk);
+		goto reset;
 	}
 
 	err = clk_prepare_enable(dsi->clk);
 	if (err < 0) {
 		dev_err(&pdev->dev, "cannot enable DSI clock\n");
-		return err;
+		goto reset;
 	}
 
 	dsi->clk_lp = devm_clk_get(&pdev->dev, "lp");
 	if (IS_ERR(dsi->clk_lp)) {
 		dev_err(&pdev->dev, "cannot get low-power clock\n");
-		return PTR_ERR(dsi->clk_lp);
+		err = PTR_ERR(dsi->clk_lp);
+		goto disable_clk;
 	}
 
 	err = clk_prepare_enable(dsi->clk_lp);
 	if (err < 0) {
 		dev_err(&pdev->dev, "cannot enable low-power clock\n");
-		return err;
+		goto disable_clk;
 	}
 
 	dsi->clk_parent = devm_clk_get(&pdev->dev, "parent");
 	if (IS_ERR(dsi->clk_parent)) {
 		dev_err(&pdev->dev, "cannot get parent clock\n");
-		return PTR_ERR(dsi->clk_parent);
-	}
-
-	err = clk_prepare_enable(dsi->clk_parent);
-	if (err < 0) {
-		dev_err(&pdev->dev, "cannot enable parent clock\n");
-		return err;
+		err = PTR_ERR(dsi->clk_parent);
+		goto disable_clk_lp;
 	}
 
 	dsi->vdd = devm_regulator_get(&pdev->dev, "avdd-dsi-csi");
 	if (IS_ERR(dsi->vdd)) {
 		dev_err(&pdev->dev, "cannot get VDD supply\n");
-		return PTR_ERR(dsi->vdd);
+		err = PTR_ERR(dsi->vdd);
+		goto disable_clk_lp;
 	}
 
 	err = regulator_enable(dsi->vdd);
 	if (err < 0) {
 		dev_err(&pdev->dev, "cannot enable VDD supply\n");
-		return err;
+		goto disable_clk_lp;
 	}
 
 	err = tegra_dsi_setup_clocks(dsi);
 	if (err < 0) {
 		dev_err(&pdev->dev, "cannot setup clocks\n");
-		return err;
+		goto disable_vdd;
 	}
 
 	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	dsi->regs = devm_ioremap_resource(&pdev->dev, regs);
-	if (IS_ERR(dsi->regs))
-		return PTR_ERR(dsi->regs);
+	if (IS_ERR(dsi->regs)) {
+		err = PTR_ERR(dsi->regs);
+		goto disable_vdd;
+	}
 
 	dsi->mipi = tegra_mipi_request(&pdev->dev);
-	if (IS_ERR(dsi->mipi))
-		return PTR_ERR(dsi->mipi);
+	if (IS_ERR(dsi->mipi)) {
+		err = PTR_ERR(dsi->mipi);
+		goto disable_vdd;
+	}
+
+	err = tegra_dsi_pad_calibrate(dsi);
+	if (err < 0) {
+		dev_err(dsi->dev, "MIPI calibration failed: %d\n", err);
+		goto mipi_free;
+	}
 
 	dsi->host.ops = &tegra_dsi_host_ops;
 	dsi->host.dev = &pdev->dev;
@@ -930,7 +1482,7 @@ static int tegra_dsi_probe(struct platform_device *pdev)
 	err = mipi_dsi_host_register(&dsi->host);
 	if (err < 0) {
 		dev_err(&pdev->dev, "failed to register DSI host: %d\n", err);
-		return err;
+		goto mipi_free;
 	}
 
 	INIT_LIST_HEAD(&dsi->client.list);
@@ -941,12 +1493,26 @@ static int tegra_dsi_probe(struct platform_device *pdev)
 	if (err < 0) {
 		dev_err(&pdev->dev, "failed to register host1x client: %d\n",
 			err);
-		return err;
+		goto unregister;
 	}
 
 	platform_set_drvdata(pdev, dsi);
 
 	return 0;
+
+unregister:
+	mipi_dsi_host_unregister(&dsi->host);
+mipi_free:
+	tegra_mipi_free(dsi->mipi);
+disable_vdd:
+	regulator_disable(dsi->vdd);
+disable_clk_lp:
+	clk_disable_unprepare(dsi->clk_lp);
+disable_clk:
+	clk_disable_unprepare(dsi->clk);
+reset:
+	reset_control_assert(dsi->rst);
+	return err;
 }
 
 static int tegra_dsi_remove(struct platform_device *pdev)
@@ -965,7 +1531,6 @@ static int tegra_dsi_remove(struct platform_device *pdev)
 	tegra_mipi_free(dsi->mipi);
 
 	regulator_disable(dsi->vdd);
-	clk_disable_unprepare(dsi->clk_parent);
 	clk_disable_unprepare(dsi->clk_lp);
 	clk_disable_unprepare(dsi->clk);
 	reset_control_assert(dsi->rst);
diff --git a/drivers/gpu/drm/tegra/dsi.h b/drivers/gpu/drm/tegra/dsi.h
index 5ce610d08d77..bad1006a5150 100644
--- a/drivers/gpu/drm/tegra/dsi.h
+++ b/drivers/gpu/drm/tegra/dsi.h
@@ -21,9 +21,16 @@
 #define DSI_INT_STATUS			0x0d
 #define DSI_INT_MASK			0x0e
 #define DSI_HOST_CONTROL		0x0f
+#define DSI_HOST_CONTROL_FIFO_RESET	(1 << 21)
+#define DSI_HOST_CONTROL_CRC_RESET	(1 << 20)
+#define DSI_HOST_CONTROL_TX_TRIG_SOL	(0 << 12)
+#define DSI_HOST_CONTROL_TX_TRIG_FIFO	(1 << 12)
+#define DSI_HOST_CONTROL_TX_TRIG_HOST	(2 << 12)
 #define DSI_HOST_CONTROL_RAW		(1 << 6)
 #define DSI_HOST_CONTROL_HS		(1 << 5)
-#define DSI_HOST_CONTROL_BTA		(1 << 2)
+#define DSI_HOST_CONTROL_FIFO_SEL	(1 << 4)
+#define DSI_HOST_CONTROL_IMM_BTA	(1 << 3)
+#define DSI_HOST_CONTROL_PKT_BTA	(1 << 2)
 #define DSI_HOST_CONTROL_CS		(1 << 1)
 #define DSI_HOST_CONTROL_ECC		(1 << 0)
 #define DSI_CONTROL			0x10
@@ -39,9 +46,13 @@
 #define DSI_SOL_DELAY			0x11
 #define DSI_MAX_THRESHOLD		0x12
 #define DSI_TRIGGER			0x13
+#define DSI_TRIGGER_HOST		(1 << 1)
+#define DSI_TRIGGER_VIDEO		(1 << 0)
 #define DSI_TX_CRC			0x14
 #define DSI_STATUS			0x15
 #define DSI_STATUS_IDLE			(1 << 10)
+#define DSI_STATUS_UNDERFLOW		(1 <<  9)
+#define DSI_STATUS_OVERFLOW		(1 <<  8)
 #define DSI_INIT_SEQ_CONTROL		0x1a
 #define DSI_INIT_SEQ_DATA_0		0x1b
 #define DSI_INIT_SEQ_DATA_1		0x1c
@@ -104,6 +115,7 @@
 #define DSI_PAD_CONTROL_3		0x51
 #define DSI_PAD_CONTROL_4		0x52
 #define DSI_GANGED_MODE_CONTROL		0x53
+#define DSI_GANGED_MODE_CONTROL_ENABLE	(1 << 0)
 #define DSI_GANGED_MODE_START		0x54
 #define DSI_GANGED_MODE_SIZE		0x55
 #define DSI_RAW_DATA_BYTE_COUNT		0x56
diff --git a/drivers/gpu/drm/tegra/fb.c b/drivers/gpu/drm/tegra/fb.c
index 3513d12d5aa1..e9c715d89261 100644
--- a/drivers/gpu/drm/tegra/fb.c
+++ b/drivers/gpu/drm/tegra/fb.c
@@ -65,8 +65,12 @@ static void tegra_fb_destroy(struct drm_framebuffer *framebuffer)
 	for (i = 0; i < fb->num_planes; i++) {
 		struct tegra_bo *bo = fb->planes[i];
 
-		if (bo)
+		if (bo) {
+			if (bo->pages && bo->vaddr)
+				vunmap(bo->vaddr);
+
 			drm_gem_object_unreference_unlocked(&bo->gem);
+		}
 	}
 
 	drm_framebuffer_cleanup(framebuffer);
@@ -223,14 +227,16 @@ static int tegra_fbdev_probe(struct drm_fb_helper *helper,
 	info = framebuffer_alloc(0, drm->dev);
 	if (!info) {
 		dev_err(drm->dev, "failed to allocate framebuffer info\n");
-		tegra_bo_free_object(&bo->gem);
+		drm_gem_object_unreference_unlocked(&bo->gem);
 		return -ENOMEM;
 	}
 
 	fbdev->fb = tegra_fb_alloc(drm, &cmd, &bo, 1);
 	if (IS_ERR(fbdev->fb)) {
-		dev_err(drm->dev, "failed to allocate DRM framebuffer\n");
 		err = PTR_ERR(fbdev->fb);
+		dev_err(drm->dev, "failed to allocate DRM framebuffer: %d\n",
+			err);
+		drm_gem_object_unreference_unlocked(&bo->gem);
 		goto release;
 	}
 
@@ -254,6 +260,16 @@ static int tegra_fbdev_probe(struct drm_fb_helper *helper,
 	offset = info->var.xoffset * bytes_per_pixel +
 		 info->var.yoffset * fb->pitches[0];
 
+	if (bo->pages) {
+		bo->vaddr = vmap(bo->pages, bo->num_pages, VM_MAP,
+				 pgprot_writecombine(PAGE_KERNEL));
+		if (!bo->vaddr) {
+			dev_err(drm->dev, "failed to vmap() framebuffer\n");
+			err = -ENOMEM;
+			goto destroy;
+		}
+	}
+
 	drm->mode_config.fb_base = (resource_size_t)bo->paddr;
 	info->screen_base = (void __iomem *)bo->vaddr + offset;
 	info->screen_size = size;
@@ -289,6 +305,11 @@ static struct tegra_fbdev *tegra_fbdev_create(struct drm_device *drm)
 	return fbdev;
 }
 
+static void tegra_fbdev_free(struct tegra_fbdev *fbdev)
+{
+	kfree(fbdev);
+}
+
 static int tegra_fbdev_init(struct tegra_fbdev *fbdev,
 			    unsigned int preferred_bpp,
 			    unsigned int num_crtc,
@@ -299,19 +320,21 @@ static int tegra_fbdev_init(struct tegra_fbdev *fbdev,
 
 	err = drm_fb_helper_init(drm, &fbdev->base, num_crtc, max_connectors);
 	if (err < 0) {
-		dev_err(drm->dev, "failed to initialize DRM FB helper\n");
+		dev_err(drm->dev, "failed to initialize DRM FB helper: %d\n",
+			err);
 		return err;
 	}
 
 	err = drm_fb_helper_single_add_all_connectors(&fbdev->base);
 	if (err < 0) {
-		dev_err(drm->dev, "failed to add connectors\n");
+		dev_err(drm->dev, "failed to add connectors: %d\n", err);
 		goto fini;
 	}
 
 	err = drm_fb_helper_initial_config(&fbdev->base, preferred_bpp);
 	if (err < 0) {
-		dev_err(drm->dev, "failed to set initial configuration\n");
+		dev_err(drm->dev, "failed to set initial configuration: %d\n",
+			err);
 		goto fini;
 	}
 
@@ -322,7 +345,7 @@ fini:
 	return err;
 }
 
-static void tegra_fbdev_free(struct tegra_fbdev *fbdev)
+static void tegra_fbdev_exit(struct tegra_fbdev *fbdev)
 {
 	struct fb_info *info = fbdev->base.fbdev;
 
@@ -341,11 +364,11 @@ static void tegra_fbdev_free(struct tegra_fbdev *fbdev)
 
 	if (fbdev->fb) {
 		drm_framebuffer_unregister_private(&fbdev->fb->base);
-		tegra_fb_destroy(&fbdev->fb->base);
+		drm_framebuffer_remove(&fbdev->fb->base);
 	}
 
 	drm_fb_helper_fini(&fbdev->base);
-	kfree(fbdev);
+	tegra_fbdev_free(fbdev);
 }
 
 void tegra_fbdev_restore_mode(struct tegra_fbdev *fbdev)
@@ -393,6 +416,15 @@ int tegra_drm_fb_prepare(struct drm_device *drm)
 	return 0;
 }
 
+void tegra_drm_fb_free(struct drm_device *drm)
+{
+#ifdef CONFIG_DRM_TEGRA_FBDEV
+	struct tegra_drm *tegra = drm->dev_private;
+
+	tegra_fbdev_free(tegra->fbdev);
+#endif
+}
+
 int tegra_drm_fb_init(struct drm_device *drm)
 {
 #ifdef CONFIG_DRM_TEGRA_FBDEV
@@ -413,6 +445,6 @@ void tegra_drm_fb_exit(struct drm_device *drm)
 #ifdef CONFIG_DRM_TEGRA_FBDEV
 	struct tegra_drm *tegra = drm->dev_private;
 
-	tegra_fbdev_free(tegra->fbdev);
+	tegra_fbdev_exit(tegra->fbdev);
 #endif
 }
diff --git a/drivers/gpu/drm/tegra/gem.c b/drivers/gpu/drm/tegra/gem.c
index ce023fa3e8ae..8777b7f75791 100644
--- a/drivers/gpu/drm/tegra/gem.c
+++ b/drivers/gpu/drm/tegra/gem.c
@@ -14,6 +14,7 @@
  */
 
 #include <linux/dma-buf.h>
+#include <linux/iommu.h>
 #include <drm/tegra_drm.h>
 
 #include "drm.h"
@@ -91,13 +92,90 @@ static const struct host1x_bo_ops tegra_bo_ops = {
 	.kunmap = tegra_bo_kunmap,
 };
 
-static void tegra_bo_destroy(struct drm_device *drm, struct tegra_bo *bo)
+/*
+ * A generic iommu_map_sg() function is being reviewed and will hopefully be
+ * merged soon. At that point this function can be dropped in favour of the
+ * one provided by the IOMMU API.
+ */
+static ssize_t __iommu_map_sg(struct iommu_domain *domain, unsigned long iova,
+			      struct scatterlist *sg, unsigned int nents,
+			      int prot)
 {
-	dma_free_writecombine(drm->dev, bo->gem.size, bo->vaddr, bo->paddr);
+	struct scatterlist *s;
+	size_t offset = 0;
+	unsigned int i;
+	int err;
+
+	for_each_sg(sg, s, nents, i) {
+		phys_addr_t phys = page_to_phys(sg_page(s));
+		size_t length = s->offset + s->length;
+
+		err = iommu_map(domain, iova + offset, phys, length, prot);
+		if (err < 0) {
+			iommu_unmap(domain, iova, offset);
+			return err;
+		}
+
+		offset += length;
+	}
+
+	return offset;
 }
 
-struct tegra_bo *tegra_bo_create(struct drm_device *drm, unsigned int size,
-				 unsigned long flags)
+static int tegra_bo_iommu_map(struct tegra_drm *tegra, struct tegra_bo *bo)
+{
+	int prot = IOMMU_READ | IOMMU_WRITE;
+	ssize_t err;
+
+	if (bo->mm)
+		return -EBUSY;
+
+	bo->mm = kzalloc(sizeof(*bo->mm), GFP_KERNEL);
+	if (!bo->mm)
+		return -ENOMEM;
+
+	err = drm_mm_insert_node_generic(&tegra->mm, bo->mm, bo->gem.size,
+					 PAGE_SIZE, 0, 0, 0);
+	if (err < 0) {
+		dev_err(tegra->drm->dev, "out of I/O virtual memory: %zd\n",
+			err);
+		goto free;
+	}
+
+	bo->paddr = bo->mm->start;
+
+	err = __iommu_map_sg(tegra->domain, bo->paddr, bo->sgt->sgl,
+			     bo->sgt->nents, prot);
+	if (err < 0) {
+		dev_err(tegra->drm->dev, "failed to map buffer: %zd\n", err);
+		goto remove;
+	}
+
+	bo->size = err;
+
+	return 0;
+
+remove:
+	drm_mm_remove_node(bo->mm);
+free:
+	kfree(bo->mm);
+	return err;
+}
+
+static int tegra_bo_iommu_unmap(struct tegra_drm *tegra, struct tegra_bo *bo)
+{
+	if (!bo->mm)
+		return 0;
+
+	iommu_unmap(tegra->domain, bo->paddr, bo->size);
+	drm_mm_remove_node(bo->mm);
+	kfree(bo->mm);
+
+	return 0;
+}
+
+static struct tegra_bo *tegra_bo_alloc_object(struct drm_device *drm,
+					      size_t size)
 {
 	struct tegra_bo *bo;
 	int err;
@@ -109,22 +187,124 @@ struct tegra_bo *tegra_bo_create(struct drm_device *drm, unsigned int size,
 	host1x_bo_init(&bo->base, &tegra_bo_ops);
 	size = round_up(size, PAGE_SIZE);
 
-	bo->vaddr = dma_alloc_writecombine(drm->dev, size, &bo->paddr,
-					   GFP_KERNEL | __GFP_NOWARN);
-	if (!bo->vaddr) {
-		dev_err(drm->dev, "failed to allocate buffer with size %u\n",
-			size);
-		err = -ENOMEM;
-		goto err_dma;
-	}
-
 	err = drm_gem_object_init(drm, &bo->gem, size);
-	if (err)
-		goto err_init;
+	if (err < 0)
+		goto free;
 
 	err = drm_gem_create_mmap_offset(&bo->gem);
-	if (err)
-		goto err_mmap;
+	if (err < 0)
+		goto release;
+
+	return bo;
+
+release:
+	drm_gem_object_release(&bo->gem);
+free:
+	kfree(bo);
+	return ERR_PTR(err);
+}
+
+static void tegra_bo_free(struct drm_device *drm, struct tegra_bo *bo)
+{
+	if (bo->pages) {
+		drm_gem_put_pages(&bo->gem, bo->pages, true, true);
+		sg_free_table(bo->sgt);
+		kfree(bo->sgt);
+	} else if (bo->vaddr) {
+		dma_free_writecombine(drm->dev, bo->gem.size, bo->vaddr,
+				      bo->paddr);
+	}
+}
+
+static int tegra_bo_get_pages(struct drm_device *drm, struct tegra_bo *bo)
+{
+	struct scatterlist *s;
+	struct sg_table *sgt;
+	unsigned int i;
+
+	bo->pages = drm_gem_get_pages(&bo->gem);
+	if (IS_ERR(bo->pages))
+		return PTR_ERR(bo->pages);
+
+	bo->num_pages = bo->gem.size >> PAGE_SHIFT;
+
+	sgt = drm_prime_pages_to_sg(bo->pages, bo->num_pages);
+	if (IS_ERR(sgt))
+		goto put_pages;
+
+	/*
+	 * Fake up the SG table so that dma_map_sg() can be used to flush the
+	 * pages associated with it. Note that this relies on the fact that
+	 * the DMA API doesn't hook into IOMMU on Tegra, therefore mapping is
+	 * only cache maintenance.
+	 *
+	 * TODO: Replace this by drm_clflash_sg() once it can be implemented
+	 * without relying on symbols that are not exported.
+	 */
+	for_each_sg(sgt->sgl, s, sgt->nents, i)
+		sg_dma_address(s) = sg_phys(s);
+
+	if (dma_map_sg(drm->dev, sgt->sgl, sgt->nents, DMA_TO_DEVICE) == 0) {
+		sgt = ERR_PTR(-ENOMEM);
+		goto release_sgt;
+	}
+
+	bo->sgt = sgt;
+
+	return 0;
+
+release_sgt:
+	sg_free_table(sgt);
+	kfree(sgt);
+put_pages:
+	drm_gem_put_pages(&bo->gem, bo->pages, false, false);
+	return PTR_ERR(sgt);
+}
+
+static int tegra_bo_alloc(struct drm_device *drm, struct tegra_bo *bo)
+{
+	struct tegra_drm *tegra = drm->dev_private;
+	int err;
+
+	if (tegra->domain) {
+		err = tegra_bo_get_pages(drm, bo);
+		if (err < 0)
+			return err;
+
+		err = tegra_bo_iommu_map(tegra, bo);
+		if (err < 0) {
+			tegra_bo_free(drm, bo);
+			return err;
+		}
+	} else {
+		size_t size = bo->gem.size;
+
+		bo->vaddr = dma_alloc_writecombine(drm->dev, size, &bo->paddr,
+						   GFP_KERNEL | __GFP_NOWARN);
+		if (!bo->vaddr) {
+			dev_err(drm->dev,
+				"failed to allocate buffer of size %zu\n",
+				size);
+			return -ENOMEM;
+		}
+	}
+
+	return 0;
+}
+
+struct tegra_bo *tegra_bo_create(struct drm_device *drm, size_t size,
+				 unsigned long flags)
+{
+	struct tegra_bo *bo;
+	int err;
+
+	bo = tegra_bo_alloc_object(drm, size);
+	if (IS_ERR(bo))
+		return bo;
+
+	err = tegra_bo_alloc(drm, bo);
+	if (err < 0)
+		goto release;
 
 	if (flags & DRM_TEGRA_GEM_CREATE_TILED)
 		bo->tiling.mode = TEGRA_BO_TILING_MODE_TILED;
@@ -134,69 +314,52 @@ struct tegra_bo *tegra_bo_create(struct drm_device *drm, unsigned int size,
 
 	return bo;
 
-err_mmap:
+release:
 	drm_gem_object_release(&bo->gem);
-err_init:
-	tegra_bo_destroy(drm, bo);
-err_dma:
 	kfree(bo);
-
 	return ERR_PTR(err);
 }
 
 struct tegra_bo *tegra_bo_create_with_handle(struct drm_file *file,
 					     struct drm_device *drm,
-					     unsigned int size,
+					     size_t size,
 					     unsigned long flags,
-					     unsigned int *handle)
+					     u32 *handle)
 {
 	struct tegra_bo *bo;
-	int ret;
+	int err;
 
 	bo = tegra_bo_create(drm, size, flags);
 	if (IS_ERR(bo))
 		return bo;
 
-	ret = drm_gem_handle_create(file, &bo->gem, handle);
-	if (ret)
-		goto err;
+	err = drm_gem_handle_create(file, &bo->gem, handle);
+	if (err) {
+		tegra_bo_free_object(&bo->gem);
+		return ERR_PTR(err);
+	}
 
 	drm_gem_object_unreference_unlocked(&bo->gem);
 
 	return bo;
-
-err:
-	tegra_bo_free_object(&bo->gem);
-	return ERR_PTR(ret);
 }
 
 static struct tegra_bo *tegra_bo_import(struct drm_device *drm,
 					struct dma_buf *buf)
 {
+	struct tegra_drm *tegra = drm->dev_private;
 	struct dma_buf_attachment *attach;
 	struct tegra_bo *bo;
-	ssize_t size;
 	int err;
 
-	bo = kzalloc(sizeof(*bo), GFP_KERNEL);
-	if (!bo)
-		return ERR_PTR(-ENOMEM);
-
-	host1x_bo_init(&bo->base, &tegra_bo_ops);
-	size = round_up(buf->size, PAGE_SIZE);
-
-	err = drm_gem_object_init(drm, &bo->gem, size);
-	if (err < 0)
-		goto free;
-
-	err = drm_gem_create_mmap_offset(&bo->gem);
-	if (err < 0)
-		goto release;
+	bo = tegra_bo_alloc_object(drm, buf->size);
+	if (IS_ERR(bo))
+		return bo;
 
 	attach = dma_buf_attach(buf, drm->dev);
 	if (IS_ERR(attach)) {
 		err = PTR_ERR(attach);
-		goto free_mmap;
+		goto free;
 	}
 
 	get_dma_buf(buf);
@@ -212,12 +375,19 @@ static struct tegra_bo *tegra_bo_import(struct drm_device *drm,
 		goto detach;
 	}
 
-	if (bo->sgt->nents > 1) {
-		err = -EINVAL;
-		goto detach;
+	if (tegra->domain) {
+		err = tegra_bo_iommu_map(tegra, bo);
+		if (err < 0)
+			goto detach;
+	} else {
+		if (bo->sgt->nents > 1) {
+			err = -EINVAL;
+			goto detach;
+		}
+
+		bo->paddr = sg_dma_address(bo->sgt->sgl);
 	}
 
-	bo->paddr = sg_dma_address(bo->sgt->sgl);
 	bo->gem.import_attach = attach;
 
 	return bo;
@@ -228,47 +398,41 @@ detach:
 
 	dma_buf_detach(buf, attach);
 	dma_buf_put(buf);
-free_mmap:
-	drm_gem_free_mmap_offset(&bo->gem);
-release:
-	drm_gem_object_release(&bo->gem);
 free:
+	drm_gem_object_release(&bo->gem);
 	kfree(bo);
-
 	return ERR_PTR(err);
 }
 
 void tegra_bo_free_object(struct drm_gem_object *gem)
 {
+	struct tegra_drm *tegra = gem->dev->dev_private;
 	struct tegra_bo *bo = to_tegra_bo(gem);
 
+	if (tegra->domain)
+		tegra_bo_iommu_unmap(tegra, bo);
+
 	if (gem->import_attach) {
 		dma_buf_unmap_attachment(gem->import_attach, bo->sgt,
 					 DMA_TO_DEVICE);
 		drm_prime_gem_destroy(gem, NULL);
 	} else {
-		tegra_bo_destroy(gem->dev, bo);
+		tegra_bo_free(gem->dev, bo);
 	}
 
-	drm_gem_free_mmap_offset(gem);
 	drm_gem_object_release(gem);
-
 	kfree(bo);
 }
 
 int tegra_bo_dumb_create(struct drm_file *file, struct drm_device *drm,
 			 struct drm_mode_create_dumb *args)
 {
-	int min_pitch = DIV_ROUND_UP(args->width * args->bpp, 8);
+	unsigned int min_pitch = DIV_ROUND_UP(args->width * args->bpp, 8);
 	struct tegra_drm *tegra = drm->dev_private;
 	struct tegra_bo *bo;
 
-	min_pitch = round_up(min_pitch, tegra->pitch_align);
-	if (args->pitch < min_pitch)
-		args->pitch = min_pitch;
-
-	if (args->size < args->pitch * args->height)
-		args->size = args->pitch * args->height;
+	args->pitch = round_up(min_pitch, tegra->pitch_align);
+	args->size = args->pitch * args->height;
 
 	bo = tegra_bo_create_with_handle(file, drm, args->size, 0,
 					 &args->handle);
@@ -279,7 +443,7 @@ int tegra_bo_dumb_create(struct drm_file *file, struct drm_device *drm,
 }
 
 int tegra_bo_dumb_map_offset(struct drm_file *file, struct drm_device *drm,
-			     uint32_t handle, uint64_t *offset)
+			     u32 handle, u64 *offset)
 {
 	struct drm_gem_object *gem;
 	struct tegra_bo *bo;
@@ -304,7 +468,38 @@ int tegra_bo_dumb_map_offset(struct drm_file *file, struct drm_device *drm,
 	return 0;
 }
 
+static int tegra_bo_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
+{
+	struct drm_gem_object *gem = vma->vm_private_data;
+	struct tegra_bo *bo = to_tegra_bo(gem);
+	struct page *page;
+	pgoff_t offset;
+	int err;
+
+	if (!bo->pages)
+		return VM_FAULT_SIGBUS;
+
+	offset = ((unsigned long)vmf->virtual_address - vma->vm_start) >> PAGE_SHIFT;
+	page = bo->pages[offset];
+
+	err = vm_insert_page(vma, (unsigned long)vmf->virtual_address, page);
+	switch (err) {
+	case -EAGAIN:
+	case 0:
+	case -ERESTARTSYS:
+	case -EINTR:
+	case -EBUSY:
+		return VM_FAULT_NOPAGE;
+
+	case -ENOMEM:
+		return VM_FAULT_OOM;
+	}
+
+	return VM_FAULT_SIGBUS;
+}
+
 const struct vm_operations_struct tegra_bo_vm_ops = {
+	.fault = tegra_bo_fault,
 	.open = drm_gem_vm_open,
 	.close = drm_gem_vm_close,
 };
@@ -322,12 +517,30 @@ int tegra_drm_mmap(struct file *file, struct vm_area_struct *vma)
 	gem = vma->vm_private_data;
 	bo = to_tegra_bo(gem);
 
-	ret = remap_pfn_range(vma, vma->vm_start, bo->paddr >> PAGE_SHIFT,
-			      vma->vm_end - vma->vm_start, vma->vm_page_prot);
-	if (ret)
-		drm_gem_vm_close(vma);
+	if (!bo->pages) {
+		unsigned long vm_pgoff = vma->vm_pgoff;
+
+		vma->vm_flags &= ~VM_PFNMAP;
+		vma->vm_pgoff = 0;
+
+		ret = dma_mmap_writecombine(gem->dev->dev, vma, bo->vaddr,
+					    bo->paddr, gem->size);
+		if (ret) {
+			drm_gem_vm_close(vma);
+			return ret;
+		}
+
+		vma->vm_pgoff = vm_pgoff;
+	} else {
+		pgprot_t prot = vm_get_page_prot(vma->vm_flags);
+
+		vma->vm_flags |= VM_MIXEDMAP;
+		vma->vm_flags &= ~VM_PFNMAP;
 
-	return ret;
+		vma->vm_page_prot = pgprot_writecombine(prot);
+	}
+
+	return 0;
 }
 
 static struct sg_table *
@@ -342,21 +555,44 @@ tegra_gem_prime_map_dma_buf(struct dma_buf_attachment *attach,
 	if (!sgt)
 		return NULL;
 
-	if (sg_alloc_table(sgt, 1, GFP_KERNEL)) {
-		kfree(sgt);
-		return NULL;
-	}
+	if (bo->pages) {
+		struct scatterlist *sg;
+		unsigned int i;
 
-	sg_dma_address(sgt->sgl) = bo->paddr;
-	sg_dma_len(sgt->sgl) = gem->size;
+		if (sg_alloc_table(sgt, bo->num_pages, GFP_KERNEL))
+			goto free;
+
+		for_each_sg(sgt->sgl, sg, bo->num_pages, i)
+			sg_set_page(sg, bo->pages[i], PAGE_SIZE, 0);
+
+		if (dma_map_sg(attach->dev, sgt->sgl, sgt->nents, dir) == 0)
+			goto free;
+	} else {
+		if (sg_alloc_table(sgt, 1, GFP_KERNEL))
+			goto free;
+
+		sg_dma_address(sgt->sgl) = bo->paddr;
+		sg_dma_len(sgt->sgl) = gem->size;
+	}
 
 	return sgt;
+
+free:
+	sg_free_table(sgt);
+	kfree(sgt);
+	return NULL;
 }
 
 static void tegra_gem_prime_unmap_dma_buf(struct dma_buf_attachment *attach,
 					  struct sg_table *sgt,
 					  enum dma_data_direction dir)
 {
+	struct drm_gem_object *gem = attach->dmabuf->priv;
+	struct tegra_bo *bo = to_tegra_bo(gem);
+
+	if (bo->pages)
+		dma_unmap_sg(attach->dev, sgt->sgl, sgt->nents, dir);
+
 	sg_free_table(sgt);
 	kfree(sgt);
 }
diff --git a/drivers/gpu/drm/tegra/gem.h b/drivers/gpu/drm/tegra/gem.h
index 6538b56780c2..6c5f12ac0087 100644
--- a/drivers/gpu/drm/tegra/gem.h
+++ b/drivers/gpu/drm/tegra/gem.h
@@ -38,6 +38,12 @@ struct tegra_bo {
 	dma_addr_t paddr;
 	void *vaddr;
 
+	struct drm_mm_node *mm;
+	unsigned long num_pages;
+	struct page **pages;
+	/* size of IOMMU mapping */
+	size_t size;
+
 	struct tegra_bo_tiling tiling;
 };
 
@@ -46,18 +52,18 @@ static inline struct tegra_bo *to_tegra_bo(struct drm_gem_object *gem)
 	return container_of(gem, struct tegra_bo, gem);
 }
 
-struct tegra_bo *tegra_bo_create(struct drm_device *drm, unsigned int size,
+struct tegra_bo *tegra_bo_create(struct drm_device *drm, size_t size,
 				 unsigned long flags);
 struct tegra_bo *tegra_bo_create_with_handle(struct drm_file *file,
 					     struct drm_device *drm,
-					     unsigned int size,
+					     size_t size,
 					     unsigned long flags,
-					     unsigned int *handle);
+					     u32 *handle);
 void tegra_bo_free_object(struct drm_gem_object *gem);
 int tegra_bo_dumb_create(struct drm_file *file, struct drm_device *drm,
 			 struct drm_mode_create_dumb *args);
 int tegra_bo_dumb_map_offset(struct drm_file *file, struct drm_device *drm,
-			     uint32_t handle, uint64_t *offset);
+			     u32 handle, u64 *offset);
 
 int tegra_drm_mmap(struct file *file, struct vm_area_struct *vma);
 
diff --git a/drivers/gpu/drm/tegra/output.c b/drivers/gpu/drm/tegra/output.c
index 0c67d7eebc94..6a5c7b81fbc5 100644
--- a/drivers/gpu/drm/tegra/output.c
+++ b/drivers/gpu/drm/tegra/output.c
@@ -157,22 +157,18 @@ static bool tegra_encoder_mode_fixup(struct drm_encoder *encoder,
 
 static void tegra_encoder_prepare(struct drm_encoder *encoder)
 {
+	tegra_encoder_dpms(encoder, DRM_MODE_DPMS_OFF);
 }
 
 static void tegra_encoder_commit(struct drm_encoder *encoder)
 {
+	tegra_encoder_dpms(encoder, DRM_MODE_DPMS_ON);
 }
 
 static void tegra_encoder_mode_set(struct drm_encoder *encoder,
 				   struct drm_display_mode *mode,
 				   struct drm_display_mode *adjusted)
 {
-	struct tegra_output *output = encoder_to_output(encoder);
-	int err;
-
-	err = tegra_output_enable(output);
-	if (err < 0)
-		dev_err(encoder->dev->dev, "tegra_output_enable(): %d\n", err);
 }
 
 static const struct drm_encoder_helper_funcs encoder_helper_funcs = {
@@ -187,7 +183,8 @@ static irqreturn_t hpd_irq(int irq, void *data)
 {
 	struct tegra_output *output = data;
 
-	drm_helper_hpd_irq_event(output->connector.dev);
+	if (output->connector.dev)
+		drm_helper_hpd_irq_event(output->connector.dev);
 
 	return IRQ_HANDLED;
 }
@@ -259,6 +256,13 @@ int tegra_output_probe(struct tegra_output *output)
 		}
 
 		output->connector.polled = DRM_CONNECTOR_POLL_HPD;
+
+		/*
+		 * Disable the interrupt until the connector has been
+		 * initialized to avoid a race in the hotplug interrupt
+		 * handler.
+		 */
+		disable_irq(output->hpd_irq);
 	}
 
 	return 0;
@@ -324,10 +328,27 @@ int tegra_output_init(struct drm_device *drm, struct tegra_output *output)
 
 	output->encoder.possible_crtcs = 0x3;
 
+	/*
+	 * The connector is now registered and ready to receive hotplug events
+	 * so the hotplug interrupt can be enabled.
+	 */
+	if (gpio_is_valid(output->hpd_gpio))
+		enable_irq(output->hpd_irq);
+
 	return 0;
 }
 
 int tegra_output_exit(struct tegra_output *output)
 {
+	/*
+	 * The connector is going away, so the interrupt must be disabled to
+	 * prevent the hotplug interrupt handler from potentially crashing.
+	 */
+	if (gpio_is_valid(output->hpd_gpio))
+		disable_irq(output->hpd_irq);
+
+	if (output->panel)
+		drm_panel_detach(output->panel);
+
 	return 0;
 }
diff --git a/drivers/gpu/drm/tilcdc/tilcdc_crtc.c b/drivers/gpu/drm/tilcdc/tilcdc_crtc.c
index d642d4a02134..c73588483be0 100644
--- a/drivers/gpu/drm/tilcdc/tilcdc_crtc.c
+++ b/drivers/gpu/drm/tilcdc/tilcdc_crtc.c
@@ -16,6 +16,7 @@
  */
 
 #include "drm_flip_work.h"
+#include <drm/drm_plane_helper.h>
 
 #include "tilcdc_drv.h"
 #include "tilcdc_regs.h"
@@ -664,12 +665,8 @@ struct drm_crtc *tilcdc_crtc_create(struct drm_device *dev)
 	tilcdc_crtc->dpms = DRM_MODE_DPMS_OFF;
 	init_waitqueue_head(&tilcdc_crtc->frame_done_wq);
 
-	ret = drm_flip_work_init(&tilcdc_crtc->unref_work, 16,
+	drm_flip_work_init(&tilcdc_crtc->unref_work,
 			"unref", unref_worker);
-	if (ret) {
-		dev_err(dev->dev, "could not allocate unref FIFO\n");
-		goto fail;
-	}
 
 	ret = drm_crtc_init(dev, crtc, &tilcdc_crtc_funcs);
 	if (ret < 0)
diff --git a/drivers/gpu/drm/tilcdc/tilcdc_drv.c b/drivers/gpu/drm/tilcdc/tilcdc_drv.c
index 79a34cbd29f5..095fca91525c 100644
--- a/drivers/gpu/drm/tilcdc/tilcdc_drv.c
+++ b/drivers/gpu/drm/tilcdc/tilcdc_drv.c
@@ -58,8 +58,7 @@ static struct drm_framebuffer *tilcdc_fb_create(struct drm_device *dev,
 static void tilcdc_fb_output_poll_changed(struct drm_device *dev)
 {
 	struct tilcdc_drm_private *priv = dev->dev_private;
-	if (priv->fbdev)
-		drm_fbdev_cma_hotplug_event(priv->fbdev);
+	drm_fbdev_cma_hotplug_event(priv->fbdev);
 }
 
 static const struct drm_mode_config_funcs mode_config_funcs = {
@@ -645,7 +644,6 @@ static struct platform_driver tilcdc_platform_driver = {
 	.probe      = tilcdc_pdev_probe,
 	.remove     = tilcdc_pdev_remove,
 	.driver     = {
-		.owner  = THIS_MODULE,
 		.name   = "tilcdc",
 		.pm     = &tilcdc_pm_ops,
 		.of_match_table = tilcdc_of_match,
diff --git a/drivers/gpu/drm/ttm/ttm_bo_manager.c b/drivers/gpu/drm/ttm/ttm_bo_manager.c
index 964387fc5c8f..aa0bd054d3e9 100644
--- a/drivers/gpu/drm/ttm/ttm_bo_manager.c
+++ b/drivers/gpu/drm/ttm/ttm_bo_manager.c
@@ -55,6 +55,7 @@ static int ttm_bo_man_get_node(struct ttm_mem_type_manager *man,
 	struct ttm_range_manager *rman = (struct ttm_range_manager *) man->priv;
 	struct drm_mm *mm = &rman->mm;
 	struct drm_mm_node *node = NULL;
+	enum drm_mm_search_flags sflags = DRM_MM_SEARCH_BEST;
 	enum drm_mm_allocator_flags aflags = DRM_MM_CREATE_DEFAULT;
 	unsigned long lpfn;
 	int ret;
@@ -67,15 +68,16 @@ static int ttm_bo_man_get_node(struct ttm_mem_type_manager *man,
 	if (!node)
 		return -ENOMEM;
 
-	if (place->flags & TTM_PL_FLAG_TOPDOWN)
+	if (place->flags & TTM_PL_FLAG_TOPDOWN) {
+		sflags = DRM_MM_SEARCH_BELOW;
 		aflags = DRM_MM_CREATE_TOP;
+	}
 
 	spin_lock(&rman->lock);
 	ret = drm_mm_insert_node_in_range_generic(mm, node, mem->num_pages,
 					  mem->page_alignment, 0,
 					  place->fpfn, lpfn,
-					  DRM_MM_SEARCH_BEST,
-					  aflags);
+					  sflags, aflags);
 	spin_unlock(&rman->lock);
 
 	if (unlikely(ret)) {
diff --git a/drivers/gpu/drm/ttm/ttm_execbuf_util.c b/drivers/gpu/drm/ttm/ttm_execbuf_util.c
index 8ce508e76208..3820ae97a030 100644
--- a/drivers/gpu/drm/ttm/ttm_execbuf_util.c
+++ b/drivers/gpu/drm/ttm/ttm_execbuf_util.c
@@ -93,7 +93,8 @@ EXPORT_SYMBOL(ttm_eu_backoff_reservation);
  */
 
 int ttm_eu_reserve_buffers(struct ww_acquire_ctx *ticket,
-			   struct list_head *list, bool intr)
+			   struct list_head *list, bool intr,
+			   struct list_head *dups)
 {
 	struct ttm_bo_global *glob;
 	struct ttm_validate_buffer *entry;
@@ -117,6 +118,13 @@ int ttm_eu_reserve_buffers(struct ww_acquire_ctx *ticket,
 			__ttm_bo_unreserve(bo);
 
 			ret = -EBUSY;
+
+		} else if (ret == -EALREADY && dups) {
+			struct ttm_validate_buffer *safe = entry;
+			entry = list_prev_entry(entry, head);
+			list_del(&safe->head);
+			list_add(&safe->head, dups);
+			continue;
 		}
 
 		if (!ret) {
diff --git a/drivers/gpu/drm/ttm/ttm_page_alloc.c b/drivers/gpu/drm/ttm/ttm_page_alloc.c
index 09874d695188..025c429050c0 100644
--- a/drivers/gpu/drm/ttm/ttm_page_alloc.c
+++ b/drivers/gpu/drm/ttm/ttm_page_alloc.c
@@ -297,11 +297,12 @@ static void ttm_pool_update_free_locked(struct ttm_page_pool *pool,
  *
  * @pool: to free the pages from
  * @free_all: If set to true will free all pages in pool
- * @gfp: GFP flags.
+ * @use_static: Safe to use static buffer
  **/
 static int ttm_page_pool_free(struct ttm_page_pool *pool, unsigned nr_free,
-			      gfp_t gfp)
+			      bool use_static)
 {
+	static struct page *static_buf[NUM_PAGES_TO_ALLOC];
 	unsigned long irq_flags;
 	struct page *p;
 	struct page **pages_to_free;
@@ -311,7 +312,11 @@ static int ttm_page_pool_free(struct ttm_page_pool *pool, unsigned nr_free,
 	if (NUM_PAGES_TO_ALLOC < nr_free)
 		npages_to_free = NUM_PAGES_TO_ALLOC;
 
-	pages_to_free = kmalloc(npages_to_free * sizeof(struct page *), gfp);
+	if (use_static)
+		pages_to_free = static_buf;
+	else
+		pages_to_free = kmalloc(npages_to_free * sizeof(struct page *),
+					GFP_KERNEL);
 	if (!pages_to_free) {
 		pr_err("Failed to allocate memory for pool free operation\n");
 		return 0;
@@ -374,7 +379,8 @@ restart:
 	if (freed_pages)
 		ttm_pages_put(pages_to_free, freed_pages);
 out:
-	kfree(pages_to_free);
+	if (pages_to_free != static_buf)
+		kfree(pages_to_free);
 	return nr_free;
 }
 
@@ -383,8 +389,6 @@ out:
  *
  * XXX: (dchinner) Deadlock warning!
  *
- * We need to pass sc->gfp_mask to ttm_page_pool_free().
- *
  * This code is crying out for a shrinker per pool....
  */
 static unsigned long
@@ -407,8 +411,8 @@ ttm_pool_shrink_scan(struct shrinker *shrink, struct shrink_control *sc)
 		if (shrink_pages == 0)
 			break;
 		pool = &_manager->pools[(i + pool_offset)%NUM_POOLS];
-		shrink_pages = ttm_page_pool_free(pool, nr_free,
-						  sc->gfp_mask);
+		/* OK to use static buffer since global mutex is held. */
+		shrink_pages = ttm_page_pool_free(pool, nr_free, true);
 		freed += nr_free - shrink_pages;
 	}
 	mutex_unlock(&lock);
@@ -710,7 +714,7 @@ static void ttm_put_pages(struct page **pages, unsigned npages, int flags,
 	}
 	spin_unlock_irqrestore(&pool->lock, irq_flags);
 	if (npages)
-		ttm_page_pool_free(pool, npages, GFP_KERNEL);
+		ttm_page_pool_free(pool, npages, false);
 }
 
 /*
@@ -849,9 +853,9 @@ void ttm_page_alloc_fini(void)
 	pr_info("Finalizing pool allocator\n");
 	ttm_pool_mm_shrink_fini(_manager);
 
+	/* OK to use static buffer since global mutex is no longer used. */
 	for (i = 0; i < NUM_POOLS; ++i)
-		ttm_page_pool_free(&_manager->pools[i], FREE_ALL_PAGES,
-				   GFP_KERNEL);
+		ttm_page_pool_free(&_manager->pools[i], FREE_ALL_PAGES, true);
 
 	kobject_put(&_manager->kobj);
 	_manager = NULL;
diff --git a/drivers/gpu/drm/ttm/ttm_page_alloc_dma.c b/drivers/gpu/drm/ttm/ttm_page_alloc_dma.c
index c96db433f8af..01e1d27eb078 100644
--- a/drivers/gpu/drm/ttm/ttm_page_alloc_dma.c
+++ b/drivers/gpu/drm/ttm/ttm_page_alloc_dma.c
@@ -411,11 +411,12 @@ static void ttm_dma_page_put(struct dma_pool *pool, struct dma_page *d_page)
  *
  * @pool: to free the pages from
  * @nr_free: If set to true will free all pages in pool
- * @gfp: GFP flags.
+ * @use_static: Safe to use static buffer
  **/
 static unsigned ttm_dma_page_pool_free(struct dma_pool *pool, unsigned nr_free,
-				       gfp_t gfp)
+				       bool use_static)
 {
+	static struct page *static_buf[NUM_PAGES_TO_ALLOC];
 	unsigned long irq_flags;
 	struct dma_page *dma_p, *tmp;
 	struct page **pages_to_free;
@@ -432,7 +433,11 @@ static unsigned ttm_dma_page_pool_free(struct dma_pool *pool, unsigned nr_free,
 			 npages_to_free, nr_free);
 	}
 #endif
-	pages_to_free = kmalloc(npages_to_free * sizeof(struct page *), gfp);
+	if (use_static)
+		pages_to_free = static_buf;
+	else
+		pages_to_free = kmalloc(npages_to_free * sizeof(struct page *),
+					GFP_KERNEL);
 
 	if (!pages_to_free) {
 		pr_err("%s: Failed to allocate memory for pool free operation\n",
@@ -502,7 +507,8 @@ restart:
 	if (freed_pages)
 		ttm_dma_pages_put(pool, &d_pages, pages_to_free, freed_pages);
 out:
-	kfree(pages_to_free);
+	if (pages_to_free != static_buf)
+		kfree(pages_to_free);
 	return nr_free;
 }
 
@@ -531,7 +537,8 @@ static void ttm_dma_free_pool(struct device *dev, enum pool_type type)
 		if (pool->type != type)
 			continue;
 		/* Takes a spinlock.. */
-		ttm_dma_page_pool_free(pool, FREE_ALL_PAGES, GFP_KERNEL);
+		/* OK to use static buffer since global mutex is held. */
+		ttm_dma_page_pool_free(pool, FREE_ALL_PAGES, true);
 		WARN_ON(((pool->npages_in_use + pool->npages_free) != 0));
 		/* This code path is called after _all_ references to the
 		 * struct device has been dropped - so nobody should be
@@ -986,7 +993,7 @@ void ttm_dma_unpopulate(struct ttm_dma_tt *ttm_dma, struct device *dev)
 
 	/* shrink pool if necessary (only on !is_cached pools)*/
 	if (npages)
-		ttm_dma_page_pool_free(pool, npages, GFP_KERNEL);
+		ttm_dma_page_pool_free(pool, npages, false);
 	ttm->state = tt_unpopulated;
 }
 EXPORT_SYMBOL_GPL(ttm_dma_unpopulate);
@@ -996,8 +1003,6 @@ EXPORT_SYMBOL_GPL(ttm_dma_unpopulate);
  *
  * XXX: (dchinner) Deadlock warning!
  *
- * We need to pass sc->gfp_mask to ttm_dma_page_pool_free().
- *
  * I'm getting sadder as I hear more pathetical whimpers about needing per-pool
  * shrinkers
  */
@@ -1030,8 +1035,8 @@ ttm_dma_pool_shrink_scan(struct shrinker *shrink, struct shrink_control *sc)
 		if (++idx < pool_offset)
 			continue;
 		nr_free = shrink_pages;
-		shrink_pages = ttm_dma_page_pool_free(p->pool, nr_free,
-						      sc->gfp_mask);
+		/* OK to use static buffer since global mutex is held. */
+		shrink_pages = ttm_dma_page_pool_free(p->pool, nr_free, true);
 		freed += nr_free - shrink_pages;
 
 		pr_debug("%s: (%s:%d) Asked to shrink %d, have %d more to go\n",
diff --git a/drivers/gpu/drm/udl/Makefile b/drivers/gpu/drm/udl/Makefile
index 05c7481bfd40..195bcac0b6c8 100644
--- a/drivers/gpu/drm/udl/Makefile
+++ b/drivers/gpu/drm/udl/Makefile
@@ -1,6 +1,6 @@
 
 ccflags-y := -Iinclude/drm
 
-udl-y := udl_drv.o udl_modeset.o udl_connector.o udl_encoder.o udl_main.o udl_fb.o udl_transfer.o udl_gem.o
+udl-y := udl_drv.o udl_modeset.o udl_connector.o udl_encoder.o udl_main.o udl_fb.o udl_transfer.o udl_gem.o udl_dmabuf.o
 
 obj-$(CONFIG_DRM_UDL) := udl.o
diff --git a/drivers/gpu/drm/udl/udl_dmabuf.c b/drivers/gpu/drm/udl/udl_dmabuf.c
new file mode 100644
index 000000000000..ac8a66b4dfc2
--- /dev/null
+++ b/drivers/gpu/drm/udl/udl_dmabuf.c
@@ -0,0 +1,276 @@
+/*
+ * udl_dmabuf.c
+ *
+ * Copyright (c) 2014 The Chromium OS Authors
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <drm/drmP.h>
+#include "udl_drv.h"
+#include <linux/shmem_fs.h>
+#include <linux/dma-buf.h>
+
+struct udl_drm_dmabuf_attachment {
+	struct sg_table sgt;
+	enum dma_data_direction dir;
+	bool is_mapped;
+};
+
+static int udl_attach_dma_buf(struct dma_buf *dmabuf,
+			      struct device *dev,
+			      struct dma_buf_attachment *attach)
+{
+	struct udl_drm_dmabuf_attachment *udl_attach;
+
+	DRM_DEBUG_PRIME("[DEV:%s] size:%zd\n", dev_name(attach->dev),
+			attach->dmabuf->size);
+
+	udl_attach = kzalloc(sizeof(*udl_attach), GFP_KERNEL);
+	if (!udl_attach)
+		return -ENOMEM;
+
+	udl_attach->dir = DMA_NONE;
+	attach->priv = udl_attach;
+
+	return 0;
+}
+
+static void udl_detach_dma_buf(struct dma_buf *dmabuf,
+			       struct dma_buf_attachment *attach)
+{
+	struct udl_drm_dmabuf_attachment *udl_attach = attach->priv;
+	struct sg_table *sgt;
+
+	if (!udl_attach)
+		return;
+
+	DRM_DEBUG_PRIME("[DEV:%s] size:%zd\n", dev_name(attach->dev),
+			attach->dmabuf->size);
+
+	sgt = &udl_attach->sgt;
+
+	if (udl_attach->dir != DMA_NONE)
+		dma_unmap_sg(attach->dev, sgt->sgl, sgt->nents,
+				udl_attach->dir);
+
+	sg_free_table(sgt);
+	kfree(udl_attach);
+	attach->priv = NULL;
+}
+
+static struct sg_table *udl_map_dma_buf(struct dma_buf_attachment *attach,
+					enum dma_data_direction dir)
+{
+	struct udl_drm_dmabuf_attachment *udl_attach = attach->priv;
+	struct udl_gem_object *obj = to_udl_bo(attach->dmabuf->priv);
+	struct drm_device *dev = obj->base.dev;
+	struct scatterlist *rd, *wr;
+	struct sg_table *sgt = NULL;
+	unsigned int i;
+	int page_count;
+	int nents, ret;
+
+	DRM_DEBUG_PRIME("[DEV:%s] size:%zd dir=%d\n", dev_name(attach->dev),
+			attach->dmabuf->size, dir);
+
+	/* just return current sgt if already requested. */
+	if (udl_attach->dir == dir && udl_attach->is_mapped)
+		return &udl_attach->sgt;
+
+	if (!obj->pages) {
+		ret = udl_gem_get_pages(obj);
+		if (ret) {
+			DRM_ERROR("failed to map pages.\n");
+			return ERR_PTR(ret);
+		}
+	}
+
+	page_count = obj->base.size / PAGE_SIZE;
+	obj->sg = drm_prime_pages_to_sg(obj->pages, page_count);
+	if (IS_ERR(obj->sg)) {
+		DRM_ERROR("failed to allocate sgt.\n");
+		return ERR_CAST(obj->sg);
+	}
+
+	sgt = &udl_attach->sgt;
+
+	ret = sg_alloc_table(sgt, obj->sg->orig_nents, GFP_KERNEL);
+	if (ret) {
+		DRM_ERROR("failed to alloc sgt.\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	mutex_lock(&dev->struct_mutex);
+
+	rd = obj->sg->sgl;
+	wr = sgt->sgl;
+	for (i = 0; i < sgt->orig_nents; ++i) {
+		sg_set_page(wr, sg_page(rd), rd->length, rd->offset);
+		rd = sg_next(rd);
+		wr = sg_next(wr);
+	}
+
+	if (dir != DMA_NONE) {
+		nents = dma_map_sg(attach->dev, sgt->sgl, sgt->orig_nents, dir);
+		if (!nents) {
+			DRM_ERROR("failed to map sgl with iommu.\n");
+			sg_free_table(sgt);
+			sgt = ERR_PTR(-EIO);
+			goto err_unlock;
+		}
+	}
+
+	udl_attach->is_mapped = true;
+	udl_attach->dir = dir;
+	attach->priv = udl_attach;
+
+err_unlock:
+	mutex_unlock(&dev->struct_mutex);
+	return sgt;
+}
+
+static void udl_unmap_dma_buf(struct dma_buf_attachment *attach,
+			      struct sg_table *sgt,
+			      enum dma_data_direction dir)
+{
+	/* Nothing to do. */
+	DRM_DEBUG_PRIME("[DEV:%s] size:%zd dir:%d\n", dev_name(attach->dev),
+			attach->dmabuf->size, dir);
+}
+
+static void *udl_dmabuf_kmap(struct dma_buf *dma_buf, unsigned long page_num)
+{
+	/* TODO */
+
+	return NULL;
+}
+
+static void *udl_dmabuf_kmap_atomic(struct dma_buf *dma_buf,
+				    unsigned long page_num)
+{
+	/* TODO */
+
+	return NULL;
+}
+
+static void udl_dmabuf_kunmap(struct dma_buf *dma_buf,
+			      unsigned long page_num, void *addr)
+{
+	/* TODO */
+}
+
+static void udl_dmabuf_kunmap_atomic(struct dma_buf *dma_buf,
+				     unsigned long page_num,
+				     void *addr)
+{
+	/* TODO */
+}
+
+static int udl_dmabuf_mmap(struct dma_buf *dma_buf,
+			   struct vm_area_struct *vma)
+{
+	/* TODO */
+
+	return -EINVAL;
+}
+
+static struct dma_buf_ops udl_dmabuf_ops = {
+	.attach			= udl_attach_dma_buf,
+	.detach			= udl_detach_dma_buf,
+	.map_dma_buf		= udl_map_dma_buf,
+	.unmap_dma_buf		= udl_unmap_dma_buf,
+	.kmap			= udl_dmabuf_kmap,
+	.kmap_atomic		= udl_dmabuf_kmap_atomic,
+	.kunmap			= udl_dmabuf_kunmap,
+	.kunmap_atomic		= udl_dmabuf_kunmap_atomic,
+	.mmap			= udl_dmabuf_mmap,
+	.release		= drm_gem_dmabuf_release,
+};
+
+struct dma_buf *udl_gem_prime_export(struct drm_device *dev,
+				     struct drm_gem_object *obj, int flags)
+{
+	return dma_buf_export(obj, &udl_dmabuf_ops, obj->size, flags, NULL);
+}
+
+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;
+
+	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 */
+	get_device(dev->dev);
+	attach = dma_buf_attach(dma_buf, dev->dev);
+	if (IS_ERR(attach)) {
+		put_device(dev->dev);
+		return ERR_CAST(attach);
+	}
+
+	get_dma_buf(dma_buf);
+
+	sg = dma_buf_map_attachment(attach, DMA_BIDIRECTIONAL);
+	if (IS_ERR(sg)) {
+		ret = PTR_ERR(sg);
+		goto fail_detach;
+	}
+
+	ret = udl_prime_create(dev, dma_buf->size, sg, &uobj);
+	if (ret)
+		goto fail_unmap;
+
+	uobj->base.import_attach = attach;
+	uobj->flags = UDL_BO_WC;
+
+	return &uobj->base;
+
+fail_unmap:
+	dma_buf_unmap_attachment(attach, sg, DMA_BIDIRECTIONAL);
+fail_detach:
+	dma_buf_detach(dma_buf, attach);
+	dma_buf_put(dma_buf);
+	put_device(dev->dev);
+	return ERR_PTR(ret);
+}
diff --git a/drivers/gpu/drm/udl/udl_drv.c b/drivers/gpu/drm/udl/udl_drv.c
index 8607e9e513db..d5728ec85254 100644
--- a/drivers/gpu/drm/udl/udl_drv.c
+++ b/drivers/gpu/drm/udl/udl_drv.c
@@ -51,7 +51,9 @@ static struct drm_driver driver = {
 	.dumb_destroy = drm_gem_dumb_destroy,
 	.fops = &udl_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 = udl_gem_prime_export,
 	.gem_prime_import = udl_gem_prime_import,
 
 	.name = DRIVER_NAME,
diff --git a/drivers/gpu/drm/udl/udl_drv.h b/drivers/gpu/drm/udl/udl_drv.h
index c7490a2489a7..80adbac82bde 100644
--- a/drivers/gpu/drm/udl/udl_drv.h
+++ b/drivers/gpu/drm/udl/udl_drv.h
@@ -25,6 +25,9 @@
 #define DRIVER_MINOR		0
 #define DRIVER_PATCHLEVEL	1
 
+#define UDL_BO_CACHEABLE		(1 << 0)
+#define UDL_BO_WC		(1 << 1)
+
 struct udl_device;
 
 struct urb_node {
@@ -69,6 +72,7 @@ struct udl_gem_object {
 	struct page **pages;
 	void *vmapping;
 	struct sg_table *sg;
+	unsigned int flags;
 };
 
 #define to_udl_bo(x) container_of(x, struct udl_gem_object, base)
@@ -120,9 +124,13 @@ int udl_gem_mmap(struct drm_file *file_priv, struct drm_device *dev,
 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 dma_buf *udl_gem_prime_export(struct drm_device *dev,
+				     struct drm_gem_object *obj, int flags);
 struct drm_gem_object *udl_gem_prime_import(struct drm_device *dev,
 				struct dma_buf *dma_buf);
 
+int udl_gem_get_pages(struct udl_gem_object *obj);
+void udl_gem_put_pages(struct udl_gem_object *obj);
 int udl_gem_vmap(struct udl_gem_object *obj);
 void udl_gem_vunmap(struct udl_gem_object *obj);
 int udl_drm_gem_mmap(struct file *filp, struct vm_area_struct *vma);
diff --git a/drivers/gpu/drm/udl/udl_gem.c b/drivers/gpu/drm/udl/udl_gem.c
index 8044f5fb7c49..2a0a784ab6ee 100644
--- a/drivers/gpu/drm/udl/udl_gem.c
+++ b/drivers/gpu/drm/udl/udl_gem.c
@@ -25,6 +25,7 @@ struct udl_gem_object *udl_gem_alloc_object(struct drm_device *dev,
 		return NULL;
 	}
 
+	obj->flags = UDL_BO_CACHEABLE;
 	return obj;
 }
 
@@ -56,6 +57,23 @@ udl_gem_create(struct drm_file *file,
 	return 0;
 }
 
+static void update_vm_cache_attr(struct udl_gem_object *obj,
+				 struct vm_area_struct *vma)
+{
+	DRM_DEBUG_KMS("flags = 0x%x\n", obj->flags);
+
+	/* non-cacheable as default. */
+	if (obj->flags & UDL_BO_CACHEABLE) {
+		vma->vm_page_prot = vm_get_page_prot(vma->vm_flags);
+	} else if (obj->flags & UDL_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));
+	}
+}
+
 int udl_dumb_create(struct drm_file *file,
 		    struct drm_device *dev,
 		    struct drm_mode_create_dumb *args)
@@ -77,6 +95,8 @@ int udl_drm_gem_mmap(struct file *filp, struct vm_area_struct *vma)
 	vma->vm_flags &= ~VM_PFNMAP;
 	vma->vm_flags |= VM_MIXEDMAP;
 
+	update_vm_cache_attr(to_udl_bo(vma->vm_private_data), vma);
+
 	return ret;
 }
 
@@ -107,7 +127,7 @@ int udl_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 	}
 }
 
-static int udl_gem_get_pages(struct udl_gem_object *obj)
+int udl_gem_get_pages(struct udl_gem_object *obj)
 {
 	struct page **pages;
 
@@ -123,7 +143,7 @@ static int udl_gem_get_pages(struct udl_gem_object *obj)
 	return 0;
 }
 
-static void udl_gem_put_pages(struct udl_gem_object *obj)
+void udl_gem_put_pages(struct udl_gem_object *obj)
 {
 	if (obj->base.import_attach) {
 		drm_free_large(obj->pages);
@@ -164,8 +184,7 @@ void udl_gem_vunmap(struct udl_gem_object *obj)
 		return;
 	}
 
-	if (obj->vmapping)
-		vunmap(obj->vmapping);
+	vunmap(obj->vmapping);
 
 	udl_gem_put_pages(obj);
 }
@@ -220,73 +239,3 @@ 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;
-
-	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 */
-	get_device(dev->dev);
-	attach = dma_buf_attach(dma_buf, dev->dev);
-	if (IS_ERR(attach)) {
-		put_device(dev->dev);
-		return ERR_CAST(attach);
-	}
-
-	get_dma_buf(dma_buf);
-
-	sg = dma_buf_map_attachment(attach, DMA_BIDIRECTIONAL);
-	if (IS_ERR(sg)) {
-		ret = PTR_ERR(sg);
-		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);
-	dma_buf_put(dma_buf);
-	put_device(dev->dev);
-	return ERR_PTR(ret);
-}
diff --git a/drivers/gpu/drm/udl/udl_modeset.c b/drivers/gpu/drm/udl/udl_modeset.c
index dc145d320b25..1701f1dfb23f 100644
--- a/drivers/gpu/drm/udl/udl_modeset.c
+++ b/drivers/gpu/drm/udl/udl_modeset.c
@@ -14,6 +14,7 @@
 #include <drm/drmP.h>
 #include <drm/drm_crtc.h>
 #include <drm/drm_crtc_helper.h>
+#include <drm/drm_plane_helper.h>
 #include "udl_drv.h"
 
 /*
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
index 25f3c250fd98..7b5d22110f25 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
@@ -889,8 +889,7 @@ static int vmw_driver_unload(struct drm_device *dev)
 
 	if (dev_priv->ctx.res_ht_initialized)
 		drm_ht_remove(&dev_priv->ctx.res_ht);
-	if (dev_priv->ctx.cmd_bounce)
-		vfree(dev_priv->ctx.cmd_bounce);
+	vfree(dev_priv->ctx.cmd_bounce);
 	if (dev_priv->enable_fb) {
 		vmw_fb_close(dev_priv);
 		vmw_kms_restore_vga(dev_priv);
@@ -1063,8 +1062,12 @@ static long vmw_generic_ioctl(struct file *filp, unsigned int cmd,
 
 	vmaster = vmw_master_check(dev, file_priv, flags);
 	if (unlikely(IS_ERR(vmaster))) {
-		DRM_INFO("IOCTL ERROR %d\n", nr);
-		return PTR_ERR(vmaster);
+		ret = PTR_ERR(vmaster);
+
+		if (ret != -ERESTARTSYS)
+			DRM_INFO("IOCTL ERROR Command %d, Error %ld.\n",
+				 nr, ret);
+		return ret;
 	}
 
 	ret = ioctl_func(filp, cmd, arg);
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c b/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
index 596cd6dafd33..33176d05db35 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
@@ -2487,7 +2487,8 @@ int vmw_execbuf_process(struct drm_file *file_priv,
 	if (unlikely(ret != 0))
 		goto out_err_nores;
 
-	ret = ttm_eu_reserve_buffers(&ticket, &sw_context->validate_nodes, true);
+	ret = ttm_eu_reserve_buffers(&ticket, &sw_context->validate_nodes,
+				     true, NULL);
 	if (unlikely(ret != 0))
 		goto out_err;
 
@@ -2677,7 +2678,8 @@ void __vmw_execbuf_release_pinned_bo(struct vmw_private *dev_priv,
 	query_val.shared = false;
 	list_add_tail(&query_val.head, &validate_list);
 
-	ret = ttm_eu_reserve_buffers(&ticket, &validate_list, false);
+	ret = ttm_eu_reserve_buffers(&ticket, &validate_list,
+				     false, NULL);
 	if (unlikely(ret != 0)) {
 		vmw_execbuf_unpin_panic(dev_priv);
 		goto out_no_reserve;
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_fence.c b/drivers/gpu/drm/vmwgfx/vmwgfx_fence.c
index 197164fd7803..b7594cb758af 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_fence.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_fence.c
@@ -545,35 +545,19 @@ void vmw_fence_obj_flush(struct vmw_fence_obj *fence)
 
 static void vmw_fence_destroy(struct vmw_fence_obj *fence)
 {
-	struct vmw_fence_manager *fman = fman_from_fence(fence);
-
 	fence_free(&fence->base);
-
-	/*
-	 * Free kernel space accounting.
-	 */
-	ttm_mem_global_free(vmw_mem_glob(fman->dev_priv),
-			    fman->fence_size);
 }
 
 int vmw_fence_create(struct vmw_fence_manager *fman,
 		     uint32_t seqno,
 		     struct vmw_fence_obj **p_fence)
 {
-	struct ttm_mem_global *mem_glob = vmw_mem_glob(fman->dev_priv);
 	struct vmw_fence_obj *fence;
 	int ret;
 
-	ret = ttm_mem_global_alloc(mem_glob, fman->fence_size,
-				   false, false);
-	if (unlikely(ret != 0))
-		return ret;
-
 	fence = kzalloc(sizeof(*fence), GFP_KERNEL);
-	if (unlikely(fence == NULL)) {
-		ret = -ENOMEM;
-		goto out_no_object;
-	}
+	if (unlikely(fence == NULL))
+		return -ENOMEM;
 
 	ret = vmw_fence_obj_init(fman, fence, seqno,
 				 vmw_fence_destroy);
@@ -585,8 +569,6 @@ int vmw_fence_create(struct vmw_fence_manager *fman,
 
 out_err_init:
 	kfree(fence);
-out_no_object:
-	ttm_mem_global_free(mem_glob, fman->fence_size);
 	return ret;
 }
 
@@ -1105,6 +1087,8 @@ static int vmw_event_fence_action_create(struct drm_file *file_priv,
 	if (ret != 0)
 		goto out_no_queue;
 
+	return 0;
+
 out_no_queue:
 	event->base.destroy(&event->base);
 out_no_event:
@@ -1180,17 +1164,10 @@ int vmw_fence_event_ioctl(struct drm_device *dev, void *data,
 
 	BUG_ON(fence == NULL);
 
-	if (arg->flags & DRM_VMW_FE_FLAG_REQ_TIME)
-		ret = vmw_event_fence_action_create(file_priv, fence,
-						    arg->flags,
-						    arg->user_data,
-						    true);
-	else
-		ret = vmw_event_fence_action_create(file_priv, fence,
-						    arg->flags,
-						    arg->user_data,
-						    true);
-
+	ret = vmw_event_fence_action_create(file_priv, fence,
+					    arg->flags,
+					    arg->user_data,
+					    true);
 	if (unlikely(ret != 0)) {
 		if (ret != -ERESTARTSYS)
 			DRM_ERROR("Failed to attach event to fence.\n");
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
index 941a7bc0b791..3725b521d931 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
@@ -252,7 +252,7 @@ int vmw_du_crtc_cursor_set(struct drm_crtc *crtc, struct drm_file *file_priv,
 	ret = 0;
 out:
 	drm_modeset_unlock_all(dev_priv->dev);
-	drm_modeset_lock_crtc(crtc);
+	drm_modeset_lock_crtc(crtc, crtc->cursor);
 
 	return ret;
 }
@@ -281,7 +281,7 @@ int vmw_du_crtc_cursor_move(struct drm_crtc *crtc, int x, int y)
 				   du->cursor_y + du->hotspot_y);
 
 	drm_modeset_unlock_all(dev_priv->dev);
-	drm_modeset_lock_crtc(crtc);
+	drm_modeset_lock_crtc(crtc, crtc->cursor);
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c b/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c
index 15e185ae4c99..5c289f748ab4 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c
@@ -26,6 +26,7 @@
  **************************************************************************/
 
 #include "vmwgfx_kms.h"
+#include <drm/drm_plane_helper.h>
 
 
 #define vmw_crtc_to_ldu(x) \
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c b/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
index 026de7cea0f6..210ef15b1d09 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
@@ -1222,7 +1222,7 @@ vmw_resource_check_buffer(struct vmw_resource *res,
 	val_buf->bo = ttm_bo_reference(&res->backup->base);
 	val_buf->shared = false;
 	list_add_tail(&val_buf->head, &val_list);
-	ret = ttm_eu_reserve_buffers(NULL, &val_list, interruptible);
+	ret = ttm_eu_reserve_buffers(NULL, &val_list, interruptible, NULL);
 	if (unlikely(ret != 0))
 		goto out_no_reserve;
 
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_scrn.c b/drivers/gpu/drm/vmwgfx/vmwgfx_scrn.c
index b295463a60b3..7dc591d04d9a 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_scrn.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_scrn.c
@@ -26,6 +26,7 @@
  **************************************************************************/
 
 #include "vmwgfx_kms.h"
+#include <drm/drm_plane_helper.h>
 
 
 #define vmw_crtc_to_sou(x) \
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_shader.c b/drivers/gpu/drm/vmwgfx/vmwgfx_shader.c
index 8719fb3cccc9..6a4584a43aa6 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_shader.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_shader.c
@@ -198,7 +198,7 @@ static int vmw_gb_shader_bind(struct vmw_resource *res,
 	cmd->header.size = sizeof(cmd->body);
 	cmd->body.shid = res->id;
 	cmd->body.mobid = bo->mem.start;
-	cmd->body.offsetInBytes = 0;
+	cmd->body.offsetInBytes = res->backup_offset;
 	res->backup_dirty = false;
 	vmw_fifo_commit(dev_priv, sizeof(*cmd));
 
diff --git a/drivers/gpu/host1x/cdma.c b/drivers/gpu/host1x/cdma.c
index 3995255b16c7..5a8c8d55317a 100644
--- a/drivers/gpu/host1x/cdma.c
+++ b/drivers/gpu/host1x/cdma.c
@@ -97,7 +97,7 @@ fail:
 static void host1x_pushbuffer_push(struct push_buffer *pb, u32 op1, u32 op2)
 {
 	u32 pos = pb->pos;
-	u32 *p = (u32 *)((u32)pb->mapped + pos);
+	u32 *p = (u32 *)((void *)pb->mapped + pos);
 	WARN_ON(pos == pb->fence);
 	*(p++) = op1;
 	*(p++) = op2;
diff --git a/drivers/gpu/host1x/cdma.h b/drivers/gpu/host1x/cdma.h
index 313c4b784348..470087af8fe5 100644
--- a/drivers/gpu/host1x/cdma.h
+++ b/drivers/gpu/host1x/cdma.h
@@ -42,7 +42,7 @@ struct host1x_job;
  */
 
 struct push_buffer {
-	u32 *mapped;			/* mapped pushbuffer memory */
+	void *mapped;			/* mapped pushbuffer memory */
 	dma_addr_t phys;		/* physical address of pushbuffer */
 	u32 fence;			/* index we've written */
 	u32 pos;			/* index to write to */
diff --git a/drivers/gpu/host1x/hw/cdma_hw.c b/drivers/gpu/host1x/hw/cdma_hw.c
index 6b09b71940c2..305ea8f3382d 100644
--- a/drivers/gpu/host1x/hw/cdma_hw.c
+++ b/drivers/gpu/host1x/hw/cdma_hw.c
@@ -26,11 +26,11 @@
 #include "../debug.h"
 
 /*
- * Put the restart at the end of pushbuffer memor
+ * Put the restart at the end of pushbuffer memory
  */
 static void push_buffer_init(struct push_buffer *pb)
 {
-	*(pb->mapped + (pb->size_bytes >> 2)) = host1x_opcode_restart(0);
+	*(u32 *)(pb->mapped + pb->size_bytes) = host1x_opcode_restart(0);
 }
 
 /*
@@ -51,11 +51,11 @@ static void cdma_timeout_cpu_incr(struct host1x_cdma *cdma, u32 getptr,
 
 	/* NOP all the PB slots */
 	while (nr_slots--) {
-		u32 *p = (u32 *)((u32)pb->mapped + getptr);
+		u32 *p = (u32 *)(pb->mapped + getptr);
 		*(p++) = HOST1X_OPCODE_NOP;
 		*(p++) = HOST1X_OPCODE_NOP;
-		dev_dbg(host1x->dev, "%s: NOP at %#llx\n", __func__,
-			(u64)pb->phys + getptr);
+		dev_dbg(host1x->dev, "%s: NOP at %pad+%#x\n", __func__,
+			&pb->phys, getptr);
 		getptr = (getptr + 8) & (pb->size_bytes - 1);
 	}
 	wmb();
diff --git a/drivers/gpu/host1x/hw/channel_hw.c b/drivers/gpu/host1x/hw/channel_hw.c
index 4608257ab656..946c332c3906 100644
--- a/drivers/gpu/host1x/hw/channel_hw.c
+++ b/drivers/gpu/host1x/hw/channel_hw.c
@@ -32,6 +32,7 @@
 static void trace_write_gather(struct host1x_cdma *cdma, struct host1x_bo *bo,
 			       u32 offset, u32 words)
 {
+	struct device *dev = cdma_to_channel(cdma)->dev;
 	void *mem = NULL;
 
 	if (host1x_debug_trace_cmdbuf)
@@ -44,11 +45,14 @@ static void trace_write_gather(struct host1x_cdma *cdma, struct host1x_bo *bo,
 		 * of how much you can output to ftrace at once.
 		 */
 		for (i = 0; i < words; i += TRACE_MAX_LENGTH) {
-			trace_host1x_cdma_push_gather(
-				dev_name(cdma_to_channel(cdma)->dev),
-				(u32)bo, min(words - i, TRACE_MAX_LENGTH),
-				offset + i * sizeof(u32), mem);
+			u32 num_words = min(words - i, TRACE_MAX_LENGTH);
+			offset += i * sizeof(u32);
+
+			trace_host1x_cdma_push_gather(dev_name(dev), bo,
+						      num_words, offset,
+						      mem);
 		}
+
 		host1x_bo_munmap(bo, mem);
 	}
 }
diff --git a/drivers/gpu/host1x/hw/debug_hw.c b/drivers/gpu/host1x/hw/debug_hw.c
index f72c873eff81..791de9351eeb 100644
--- a/drivers/gpu/host1x/hw/debug_hw.c
+++ b/drivers/gpu/host1x/hw/debug_hw.c
@@ -163,8 +163,8 @@ static void show_channel_gathers(struct output *o, struct host1x_cdma *cdma)
 				continue;
 			}
 
-			host1x_debug_output(o, "    GATHER at %#llx+%04x, %d words\n",
-					    (u64)g->base, g->offset, g->words);
+			host1x_debug_output(o, "    GATHER at %pad+%#x, %d words\n",
+					    &g->base, g->offset, g->words);
 
 			show_gather(o, g->base + g->offset, g->words, cdma,
 				    g->base, mapped);
diff --git a/drivers/gpu/host1x/job.h b/drivers/gpu/host1x/job.h
index 33a697d6dcef..8b3c15df0660 100644
--- a/drivers/gpu/host1x/job.h
+++ b/drivers/gpu/host1x/job.h
@@ -23,7 +23,7 @@ struct host1x_job_gather {
 	u32 words;
 	dma_addr_t base;
 	struct host1x_bo *bo;
-	int offset;
+	u32 offset;
 	bool handled;
 };
 
diff --git a/drivers/gpu/host1x/mipi.c b/drivers/gpu/host1x/mipi.c
index 9882ea122024..fbc6ee6ca337 100644
--- a/drivers/gpu/host1x/mipi.c
+++ b/drivers/gpu/host1x/mipi.c
@@ -49,35 +49,47 @@
 #define MIPI_CAL_CONFIG_DSIC		0x10
 #define MIPI_CAL_CONFIG_DSID		0x11
 
+#define MIPI_CAL_CONFIG_DSIAB_CLK	0x19
+#define MIPI_CAL_CONFIG_DSICD_CLK	0x1a
+#define MIPI_CAL_CONFIG_CSIAB_CLK	0x1b
+#define MIPI_CAL_CONFIG_CSICD_CLK	0x1c
+#define MIPI_CAL_CONFIG_CSIE_CLK	0x1d
+
+/* for data and clock lanes */
 #define MIPI_CAL_CONFIG_SELECT		(1 << 21)
+
+/* for data lanes */
 #define MIPI_CAL_CONFIG_HSPDOS(x)	(((x) & 0x1f) << 16)
 #define MIPI_CAL_CONFIG_HSPUOS(x)	(((x) & 0x1f) <<  8)
 #define MIPI_CAL_CONFIG_TERMOS(x)	(((x) & 0x1f) <<  0)
 
+/* for clock lanes */
+#define MIPI_CAL_CONFIG_HSCLKPDOSD(x)	(((x) & 0x1f) <<  8)
+#define MIPI_CAL_CONFIG_HSCLKPUOSD(x)	(((x) & 0x1f) <<  0)
+
 #define MIPI_CAL_BIAS_PAD_CFG0		0x16
 #define MIPI_CAL_BIAS_PAD_PDVCLAMP	(1 << 1)
 #define MIPI_CAL_BIAS_PAD_E_VCLAMP_REF	(1 << 0)
 
 #define MIPI_CAL_BIAS_PAD_CFG1		0x17
+#define MIPI_CAL_BIAS_PAD_DRV_DN_REF(x) (((x) & 0x7) << 16)
 
 #define MIPI_CAL_BIAS_PAD_CFG2		0x18
 #define MIPI_CAL_BIAS_PAD_PDVREG	(1 << 1)
 
-static const struct module {
-	unsigned long reg;
-} modules[] = {
-	{ .reg = MIPI_CAL_CONFIG_CSIA },
-	{ .reg = MIPI_CAL_CONFIG_CSIB },
-	{ .reg = MIPI_CAL_CONFIG_CSIC },
-	{ .reg = MIPI_CAL_CONFIG_CSID },
-	{ .reg = MIPI_CAL_CONFIG_CSIE },
-	{ .reg = MIPI_CAL_CONFIG_DSIA },
-	{ .reg = MIPI_CAL_CONFIG_DSIB },
-	{ .reg = MIPI_CAL_CONFIG_DSIC },
-	{ .reg = MIPI_CAL_CONFIG_DSID },
+struct tegra_mipi_pad {
+	unsigned long data;
+	unsigned long clk;
+};
+
+struct tegra_mipi_soc {
+	bool has_clk_lane;
+	const struct tegra_mipi_pad *pads;
+	unsigned int num_pads;
 };
 
 struct tegra_mipi {
+	const struct tegra_mipi_soc *soc;
 	void __iomem *regs;
 	struct mutex lock;
 	struct clk *clk;
@@ -90,16 +102,16 @@ struct tegra_mipi_device {
 	unsigned long pads;
 };
 
-static inline unsigned long tegra_mipi_readl(struct tegra_mipi *mipi,
-					     unsigned long reg)
+static inline u32 tegra_mipi_readl(struct tegra_mipi *mipi,
+				   unsigned long offset)
 {
-	return readl(mipi->regs + (reg << 2));
+	return readl(mipi->regs + (offset << 2));
 }
 
-static inline void tegra_mipi_writel(struct tegra_mipi *mipi,
-				     unsigned long value, unsigned long reg)
+static inline void tegra_mipi_writel(struct tegra_mipi *mipi, u32 value,
+				     unsigned long offset)
 {
-	writel(value, mipi->regs + (reg << 2));
+	writel(value, mipi->regs + (offset << 2));
 }
 
 struct tegra_mipi_device *tegra_mipi_request(struct device *device)
@@ -117,36 +129,35 @@ struct tegra_mipi_device *tegra_mipi_request(struct device *device)
 
 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
 	if (!dev) {
-		of_node_put(args.np);
 		err = -ENOMEM;
 		goto out;
 	}
 
 	dev->pdev = of_find_device_by_node(args.np);
 	if (!dev->pdev) {
-		of_node_put(args.np);
 		err = -ENODEV;
 		goto free;
 	}
 
-	of_node_put(args.np);
-
 	dev->mipi = platform_get_drvdata(dev->pdev);
 	if (!dev->mipi) {
 		err = -EPROBE_DEFER;
-		goto pdev_put;
+		goto put;
 	}
 
+	of_node_put(args.np);
+
 	dev->pads = args.args[0];
 	dev->device = device;
 
 	return dev;
 
-pdev_put:
+put:
 	platform_device_put(dev->pdev);
 free:
 	kfree(dev);
 out:
+	of_node_put(args.np);
 	return ERR_PTR(err);
 }
 EXPORT_SYMBOL(tegra_mipi_request);
@@ -161,7 +172,7 @@ EXPORT_SYMBOL(tegra_mipi_free);
 static int tegra_mipi_wait(struct tegra_mipi *mipi)
 {
 	unsigned long timeout = jiffies + msecs_to_jiffies(250);
-	unsigned long value;
+	u32 value;
 
 	while (time_before(jiffies, timeout)) {
 		value = tegra_mipi_readl(mipi, MIPI_CAL_STATUS);
@@ -177,8 +188,9 @@ static int tegra_mipi_wait(struct tegra_mipi *mipi)
 
 int tegra_mipi_calibrate(struct tegra_mipi_device *device)
 {
-	unsigned long value;
+	const struct tegra_mipi_soc *soc = device->mipi->soc;
 	unsigned int i;
+	u32 value;
 	int err;
 
 	err = clk_enable(device->mipi->clk);
@@ -192,23 +204,35 @@ int tegra_mipi_calibrate(struct tegra_mipi_device *device)
 	value |= MIPI_CAL_BIAS_PAD_E_VCLAMP_REF;
 	tegra_mipi_writel(device->mipi, value, MIPI_CAL_BIAS_PAD_CFG0);
 
+	tegra_mipi_writel(device->mipi, MIPI_CAL_BIAS_PAD_DRV_DN_REF(2),
+			  MIPI_CAL_BIAS_PAD_CFG1);
+
 	value = tegra_mipi_readl(device->mipi, MIPI_CAL_BIAS_PAD_CFG2);
 	value &= ~MIPI_CAL_BIAS_PAD_PDVREG;
 	tegra_mipi_writel(device->mipi, value, MIPI_CAL_BIAS_PAD_CFG2);
 
-	for (i = 0; i < ARRAY_SIZE(modules); i++) {
-		if (device->pads & BIT(i))
-			value = MIPI_CAL_CONFIG_SELECT |
-				MIPI_CAL_CONFIG_HSPDOS(0) |
-				MIPI_CAL_CONFIG_HSPUOS(4) |
-				MIPI_CAL_CONFIG_TERMOS(5);
-		else
-			value = 0;
+	for (i = 0; i < soc->num_pads; i++) {
+		u32 clk = 0, data = 0;
+
+		if (device->pads & BIT(i)) {
+			data = MIPI_CAL_CONFIG_SELECT |
+			       MIPI_CAL_CONFIG_HSPDOS(0) |
+			       MIPI_CAL_CONFIG_HSPUOS(4) |
+			       MIPI_CAL_CONFIG_TERMOS(5);
+			clk = MIPI_CAL_CONFIG_SELECT |
+			      MIPI_CAL_CONFIG_HSCLKPDOSD(0) |
+			      MIPI_CAL_CONFIG_HSCLKPUOSD(4);
+		}
 
-		tegra_mipi_writel(device->mipi, value, modules[i].reg);
+		tegra_mipi_writel(device->mipi, data, soc->pads[i].data);
+
+		if (soc->has_clk_lane)
+			tegra_mipi_writel(device->mipi, clk, soc->pads[i].clk);
 	}
 
-	tegra_mipi_writel(device->mipi, MIPI_CAL_CTRL_START, MIPI_CAL_CTRL);
+	value = tegra_mipi_readl(device->mipi, MIPI_CAL_CTRL);
+	value |= MIPI_CAL_CTRL_START;
+	tegra_mipi_writel(device->mipi, value, MIPI_CAL_CTRL);
 
 	err = tegra_mipi_wait(device->mipi);
 
@@ -219,16 +243,63 @@ int tegra_mipi_calibrate(struct tegra_mipi_device *device)
 }
 EXPORT_SYMBOL(tegra_mipi_calibrate);
 
+static const struct tegra_mipi_pad tegra114_mipi_pads[] = {
+	{ .data = MIPI_CAL_CONFIG_CSIA },
+	{ .data = MIPI_CAL_CONFIG_CSIB },
+	{ .data = MIPI_CAL_CONFIG_CSIC },
+	{ .data = MIPI_CAL_CONFIG_CSID },
+	{ .data = MIPI_CAL_CONFIG_CSIE },
+	{ .data = MIPI_CAL_CONFIG_DSIA },
+	{ .data = MIPI_CAL_CONFIG_DSIB },
+	{ .data = MIPI_CAL_CONFIG_DSIC },
+	{ .data = MIPI_CAL_CONFIG_DSID },
+};
+
+static const struct tegra_mipi_soc tegra114_mipi_soc = {
+	.has_clk_lane = false,
+	.pads = tegra114_mipi_pads,
+	.num_pads = ARRAY_SIZE(tegra114_mipi_pads),
+};
+
+static const struct tegra_mipi_pad tegra124_mipi_pads[] = {
+	{ .data = MIPI_CAL_CONFIG_CSIA, .clk = MIPI_CAL_CONFIG_CSIAB_CLK },
+	{ .data = MIPI_CAL_CONFIG_CSIB, .clk = MIPI_CAL_CONFIG_CSIAB_CLK },
+	{ .data = MIPI_CAL_CONFIG_CSIC, .clk = MIPI_CAL_CONFIG_CSICD_CLK },
+	{ .data = MIPI_CAL_CONFIG_CSID, .clk = MIPI_CAL_CONFIG_CSICD_CLK },
+	{ .data = MIPI_CAL_CONFIG_CSIE, .clk = MIPI_CAL_CONFIG_CSIE_CLK },
+	{ .data = MIPI_CAL_CONFIG_DSIA, .clk = MIPI_CAL_CONFIG_DSIAB_CLK },
+	{ .data = MIPI_CAL_CONFIG_DSIB, .clk = MIPI_CAL_CONFIG_DSIAB_CLK },
+};
+
+static const struct tegra_mipi_soc tegra124_mipi_soc = {
+	.has_clk_lane = true,
+	.pads = tegra124_mipi_pads,
+	.num_pads = ARRAY_SIZE(tegra124_mipi_pads),
+};
+
+static struct of_device_id tegra_mipi_of_match[] = {
+	{ .compatible = "nvidia,tegra114-mipi", .data = &tegra114_mipi_soc },
+	{ .compatible = "nvidia,tegra124-mipi", .data = &tegra124_mipi_soc },
+	{ },
+};
+
 static int tegra_mipi_probe(struct platform_device *pdev)
 {
+	const struct of_device_id *match;
 	struct tegra_mipi *mipi;
 	struct resource *res;
 	int err;
 
+	match = of_match_node(tegra_mipi_of_match, pdev->dev.of_node);
+	if (!match)
+		return -ENODEV;
+
 	mipi = devm_kzalloc(&pdev->dev, sizeof(*mipi), GFP_KERNEL);
 	if (!mipi)
 		return -ENOMEM;
 
+	mipi->soc = match->data;
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	mipi->regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(mipi->regs))
@@ -260,11 +331,6 @@ static int tegra_mipi_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id tegra_mipi_of_match[] = {
-	{ .compatible = "nvidia,tegra114-mipi", },
-	{ },
-};
-
 struct platform_driver tegra_mipi_driver = {
 	.driver = {
 		.name = "tegra-mipi",
diff --git a/drivers/hid/Kconfig b/drivers/hid/Kconfig
index f42df4dd58d2..230b6f887cd8 100644
--- a/drivers/hid/Kconfig
+++ b/drivers/hid/Kconfig
@@ -371,6 +371,7 @@ config HID_LOGITECH_DJ
 	tristate "Logitech Unifying receivers full support"
 	depends on HIDRAW
 	depends on HID_LOGITECH
+	select HID_LOGITECH_HIDPP
 	---help---
 	Say Y if you want support for Logitech Unifying receivers and devices.
 	Unifying receivers are capable of pairing up to 6 Logitech compliant
@@ -378,6 +379,17 @@ config HID_LOGITECH_DJ
 	generic USB_HID driver and all incoming events will be multiplexed
 	into a single mouse and a single keyboard device.
 
+config HID_LOGITECH_HIDPP
+	tristate "Logitech HID++ devices support"
+	depends on HID_LOGITECH
+	---help---
+	Support for Logitech devices relyingon the HID++ Logitech specification
+
+	Say Y if you want support for Logitech devices relying on the HID++
+	specification. Such devices are the various Logitech Touchpads (T650,
+	T651, TK820), some mice (Zone Touch mouse), or even keyboards (Solar
+	Keayboard).
+
 config LOGITECH_FF
 	bool "Logitech force feedback support"
 	depends on HID_LOGITECH
@@ -613,6 +625,13 @@ config HID_PICOLCD_CIR
 	---help---
 	  Provide access to PicoLCD's CIR interface via remote control (LIRC).
 
+config HID_PLANTRONICS
+	tristate "Plantronics USB HID Driver"
+	default !EXPERT
+	depends on HID
+	---help---
+	Provides HID support for Plantronics telephony devices.
+
 config HID_PRIMAX
 	tristate "Primax non-fully HID-compliant devices"
 	depends on HID
@@ -629,7 +648,7 @@ config HID_ROCCAT
 	support for its special functionalities.
 
 config HID_SAITEK
-	tristate "Saitek non-fully HID-compliant devices"
+	tristate "Saitek (Mad Catz) non-fully HID-compliant devices"
 	depends on HID
 	---help---
 	Support for Saitek devices that are not fully compliant with the
@@ -637,6 +656,7 @@ config HID_SAITEK
 
 	Supported devices:
 	- PS1000 Dual Analog Pad
+	- R.A.T.9 Gaming Mouse
 	- R.A.T.7 Gaming Mouse
 	- M.M.O.7 Gaming Mouse
 
diff --git a/drivers/hid/Makefile b/drivers/hid/Makefile
index e2850d8af9ca..debd15b44b59 100644
--- a/drivers/hid/Makefile
+++ b/drivers/hid/Makefile
@@ -63,6 +63,7 @@ obj-$(CONFIG_HID_LCPOWER)       += hid-lcpower.o
 obj-$(CONFIG_HID_LENOVO)	+= hid-lenovo.o
 obj-$(CONFIG_HID_LOGITECH)	+= hid-logitech.o
 obj-$(CONFIG_HID_LOGITECH_DJ)	+= hid-logitech-dj.o
+obj-$(CONFIG_HID_LOGITECH_HIDPP)	+= hid-logitech-hidpp.o
 obj-$(CONFIG_HID_MAGICMOUSE)    += hid-magicmouse.o
 obj-$(CONFIG_HID_MICROSOFT)	+= hid-microsoft.o
 obj-$(CONFIG_HID_MONTEREY)	+= hid-monterey.o
@@ -94,6 +95,7 @@ ifdef CONFIG_DEBUG_FS
 hid-picolcd-y			+= hid-picolcd_debugfs.o
 endif
 
+obj-$(CONFIG_HID_PLANTRONICS)	+= hid-plantronics.o
 obj-$(CONFIG_HID_PRIMAX)	+= hid-primax.o
 obj-$(CONFIG_HID_ROCCAT)	+= hid-roccat.o hid-roccat-common.o \
 	hid-roccat-arvo.o hid-roccat-isku.o hid-roccat-kone.o \
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index 3402033fa52a..c3d0ac1a0988 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -702,6 +702,11 @@ static void hid_scan_collection(struct hid_parser *parser, unsigned type)
 	if (((parser->global.usage_page << 16) == HID_UP_SENSOR) &&
 	    type == HID_COLLECTION_PHYSICAL)
 		hid->group = HID_GROUP_SENSOR_HUB;
+
+	if (hid->vendor == USB_VENDOR_ID_MICROSOFT &&
+	    hid->product == USB_DEVICE_ID_MS_TYPE_COVER_3 &&
+	    hid->group == HID_GROUP_MULTITOUCH)
+		hid->group = HID_GROUP_GENERIC;
 }
 
 static int hid_scan_main(struct hid_parser *parser, struct hid_item *item)
@@ -780,22 +785,19 @@ static int hid_scan_report(struct hid_device *hid)
 		hid->group = HID_GROUP_MULTITOUCH_WIN_8;
 
 	/*
-	* Vendor specific handlings
-	*/
-	if ((hid->vendor == USB_VENDOR_ID_SYNAPTICS) &&
-	    (hid->group == HID_GROUP_GENERIC) &&
-	    /* only bind to the mouse interface of composite USB devices */
-	    (hid->bus != BUS_USB || hid->type == HID_TYPE_USBMOUSE))
-		/* hid-rmi should take care of them, not hid-generic */
-		hid->group = HID_GROUP_RMI;
-
-	/*
 	 * Vendor specific handlings
 	 */
 	switch (hid->vendor) {
 	case USB_VENDOR_ID_WACOM:
 		hid->group = HID_GROUP_WACOM;
 		break;
+	case USB_VENDOR_ID_SYNAPTICS:
+		if ((hid->group == HID_GROUP_GENERIC) &&
+		    (hid->bus != BUS_USB || hid->type == HID_TYPE_USBMOUSE))
+			/* hid-rmi should only bind to the mouse interface of
+			 * composite USB devices */
+			hid->group = HID_GROUP_RMI;
+		break;
 	}
 
 	vfree(parser);
@@ -1280,12 +1282,6 @@ void hid_output_report(struct hid_report *report, __u8 *data)
 }
 EXPORT_SYMBOL_GPL(hid_output_report);
 
-static int hid_report_len(struct hid_report *report)
-{
-	/* equivalent to DIV_ROUND_UP(report->size, 8) + !!(report->id > 0) */
-	return ((report->size - 1) >> 3) + 1 + (report->id > 0);
-}
-
 /*
  * Allocator for buffer that is going to be passed to hid_output_report()
  */
@@ -1822,6 +1818,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_S510_RECEIVER_2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_RECEIVER) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_HARMONY_PS3) },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_T651) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_DINOVO_DESKTOP) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_DINOVO_EDGE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_DINOVO_MINI) },
@@ -1862,6 +1859,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_DIGITAL_MEDIA_3K) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_WIRELESS_OPTICAL_DESKTOP_3_0) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_OFFICE_KB) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_3) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MONTEREY, USB_DEVICE_ID_GENIUS_KB29E) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MSI, USB_DEVICE_ID_MSI_GT683R_LED_PANEL) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_NTRIG, USB_DEVICE_ID_NTRIG_TOUCH_SCREEN) },
@@ -1887,6 +1885,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ORTEK, USB_DEVICE_ID_ORTEK_WKB2000) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_PENMOUNT, USB_DEVICE_ID_PENMOUNT_6000) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_PETALYNX, USB_DEVICE_ID_PETALYNX_MAXTER_REMOTE) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_PLANTRONICS, HID_ANY_ID) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_PRIMAX, USB_DEVICE_ID_PRIMAX_KEYBOARD) },
 #if IS_ENABLED(CONFIG_HID_ROCCAT)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_ARVO) },
@@ -1910,10 +1909,12 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_PS1000) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_RAT7) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_MMO7) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_MADCATZ, USB_DEVICE_ID_MADCATZ_RAT9) },
 #endif
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAMSUNG, USB_DEVICE_ID_SAMSUNG_IR_REMOTE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAMSUNG, USB_DEVICE_ID_SAMSUNG_WIRELESS_KBD_MOUSE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SKYCABLE, USB_DEVICE_ID_SKYCABLE_WIRELESS_PRESENTER) },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_SMK, USB_DEVICE_ID_SMK_PS3_BDREMOTE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_BUZZ_CONTROLLER) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_WIRELESS_BUZZ_CONTROLLER) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_PS3_BDREMOTE) },
@@ -2539,7 +2540,8 @@ int hid_add_device(struct hid_device *hdev)
 	 * Scan generic devices for group information
 	 */
 	if (hid_ignore_special_drivers ||
-	    !hid_match_id(hdev, hid_have_special_driver)) {
+	    (!hdev->group &&
+	     !hid_match_id(hdev, hid_have_special_driver))) {
 		ret = hid_scan_report(hdev);
 		if (ret)
 			hid_warn(hdev, "bad device descriptor (%d)\n", ret);
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index 7c863738e419..7460f3402298 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -300,6 +300,7 @@
 #define USB_DEVICE_ID_ELAN_TOUCHSCREEN	0x0089
 #define USB_DEVICE_ID_ELAN_TOUCHSCREEN_009B	0x009b
 #define USB_DEVICE_ID_ELAN_TOUCHSCREEN_0103	0x0103
+#define USB_DEVICE_ID_ELAN_TOUCHSCREEN_010c	0x010c
 #define USB_DEVICE_ID_ELAN_TOUCHSCREEN_016F	0x016f
 
 #define USB_VENDOR_ID_ELECOM		0x056e
@@ -578,6 +579,7 @@
 
 #define USB_VENDOR_ID_LOGITECH		0x046d
 #define USB_DEVICE_ID_LOGITECH_AUDIOHUB 0x0a0e
+#define USB_DEVICE_ID_LOGITECH_T651	0xb00c
 #define USB_DEVICE_ID_LOGITECH_RECEIVER	0xc101
 #define USB_DEVICE_ID_LOGITECH_HARMONY_FIRST  0xc110
 #define USB_DEVICE_ID_LOGITECH_HARMONY_LAST 0xc14f
@@ -620,6 +622,7 @@
 
 #define USB_VENDOR_ID_MADCATZ		0x0738
 #define USB_DEVICE_ID_MADCATZ_BEATPAD	0x4540
+#define USB_DEVICE_ID_MADCATZ_RAT9	0x1709
 
 #define USB_VENDOR_ID_MCC		0x09db
 #define USB_DEVICE_ID_MCC_PMD1024LS	0x0076
@@ -649,6 +652,7 @@
 #define USB_DEVICE_ID_MS_SURFACE_PRO_2   0x0799
 #define USB_DEVICE_ID_MS_TOUCH_COVER_2   0x07a7
 #define USB_DEVICE_ID_MS_TYPE_COVER_2    0x07a9
+#define USB_DEVICE_ID_MS_TYPE_COVER_3    0x07dc
 
 #define USB_VENDOR_ID_MOJO		0x8282
 #define USB_DEVICE_ID_RETRO_ADAPTER	0x3201
@@ -716,6 +720,8 @@
 #define USB_DEVICE_ID_ORTEK_PKB1700	0x1700
 #define USB_DEVICE_ID_ORTEK_WKB2000	0x2000
 
+#define USB_VENDOR_ID_PLANTRONICS	0x047f
+
 #define USB_VENDOR_ID_PANASONIC		0x04da
 #define USB_DEVICE_ID_PANABOARD_UBT780	0x1044
 #define USB_DEVICE_ID_PANABOARD_UBT880	0x104d
@@ -813,6 +819,9 @@
 #define USB_VENDOR_ID_SKYCABLE			0x1223
 #define	USB_DEVICE_ID_SKYCABLE_WIRELESS_PRESENTER	0x3F07
 
+#define USB_VENDOR_ID_SMK		0x0609
+#define USB_DEVICE_ID_SMK_PS3_BDREMOTE	0x0306
+
 #define USB_VENDOR_ID_SONY			0x054c
 #define USB_DEVICE_ID_SONY_VAIO_VGX_MOUSE	0x024b
 #define USB_DEVICE_ID_SONY_VAIO_VGP_MOUSE	0x0374
@@ -931,6 +940,9 @@
 #define USB_DEVICE_ID_VERNIER_CYCLOPS	0x0004
 #define USB_DEVICE_ID_VERNIER_LCSPEC	0x0006
 
+#define USB_VENDOR_ID_VTL		0x0306
+#define USB_DEVICE_ID_VTL_MULTITOUCH_FF3F	0xff3f
+
 #define USB_VENDOR_ID_WACOM		0x056a
 #define USB_DEVICE_ID_WACOM_GRAPHIRE_BLUETOOTH	0x81
 #define USB_DEVICE_ID_WACOM_INTUOS4_BLUETOOTH   0x00BD
diff --git a/drivers/hid/hid-input.c b/drivers/hid/hid-input.c
index 725f22ca47fc..e0a0f06ac5ef 100644
--- a/drivers/hid/hid-input.c
+++ b/drivers/hid/hid-input.c
@@ -872,7 +872,7 @@ static void hidinput_configure_usage(struct hid_input *hidinput, struct hid_fiel
 		case 0x2cb: map_key_clear(KEY_KBDINPUTASSIST_ACCEPT);	break;
 		case 0x2cc: map_key_clear(KEY_KBDINPUTASSIST_CANCEL);	break;
 
-		default:    goto ignore;
+		default: map_key_clear(KEY_UNKNOWN);
 		}
 		break;
 
@@ -1215,7 +1215,7 @@ static void hidinput_led_worker(struct work_struct *work)
 		return hid->ll_driver->request(hid, report, HID_REQ_SET_REPORT);
 
 	/* fall back to generic raw-output-report */
-	len = ((report->size - 1) >> 3) + 1 + (report->id > 0);
+	len = hid_report_len(report);
 	buf = hid_alloc_report_buf(report, GFP_KERNEL);
 	if (!buf)
 		return;
diff --git a/drivers/hid/hid-lenovo.c b/drivers/hid/hid-lenovo.c
index bf227f7679af..4c55f4d95798 100644
--- a/drivers/hid/hid-lenovo.c
+++ b/drivers/hid/hid-lenovo.c
@@ -62,7 +62,6 @@ static int lenovo_input_mapping_cptkbd(struct hid_device *hdev,
 	/* HID_UP_LNVENDOR = USB, HID_UP_MSVENDOR = BT */
 	if ((usage->hid & HID_USAGE_PAGE) == HID_UP_MSVENDOR ||
 	    (usage->hid & HID_USAGE_PAGE) == HID_UP_LNVENDOR) {
-		set_bit(EV_REP, hi->input->evbit);
 		switch (usage->hid & HID_USAGE) {
 		case 0x00f1: /* Fn-F4: Mic mute */
 			map_key_clear(KEY_MICMUTE);
@@ -85,13 +84,13 @@ static int lenovo_input_mapping_cptkbd(struct hid_device *hdev,
 		case 0x00f8: /* Fn-F11: View open applications (3 boxes) */
 			map_key_clear(KEY_SCALE);
 			return 1;
-		case 0x00fa: /* Fn-Esc: Fn-lock toggle */
-			map_key_clear(KEY_FN_ESC);
-			return 1;
-		case 0x00fb: /* Fn-F12: Open My computer (6 boxes) USB-only */
+		case 0x00f9: /* Fn-F12: Open My computer (6 boxes) USB-only */
 			/* NB: This mapping is invented in raw_event below */
 			map_key_clear(KEY_FILE);
 			return 1;
+		case 0x00fa: /* Fn-Esc: Fn-lock toggle */
+			map_key_clear(KEY_FN_ESC);
+			return 1;
 		}
 	}
 
@@ -207,8 +206,8 @@ static int lenovo_raw_event(struct hid_device *hdev,
 			&& data[0] == 0x15
 			&& data[1] == 0x94
 			&& data[2] == 0x01)) {
-		data[1] = 0x0;
-		data[2] = 0x4;
+		data[1] = 0x00;
+		data[2] = 0x01;
 	}
 
 	return 0;
diff --git a/drivers/hid/hid-logitech-dj.c b/drivers/hid/hid-logitech-dj.c
index 71f569292cab..c917ab61aafa 100644
--- a/drivers/hid/hid-logitech-dj.c
+++ b/drivers/hid/hid-logitech-dj.c
@@ -26,9 +26,104 @@
 #include <linux/hid.h>
 #include <linux/module.h>
 #include <linux/usb.h>
+#include <linux/kfifo.h>
 #include <asm/unaligned.h>
 #include "hid-ids.h"
-#include "hid-logitech-dj.h"
+
+#define DJ_MAX_PAIRED_DEVICES			6
+#define DJ_MAX_NUMBER_NOTIFICATIONS		8
+#define DJ_RECEIVER_INDEX			0
+#define DJ_DEVICE_INDEX_MIN			1
+#define DJ_DEVICE_INDEX_MAX			6
+
+#define DJREPORT_SHORT_LENGTH			15
+#define DJREPORT_LONG_LENGTH			32
+
+#define REPORT_ID_DJ_SHORT			0x20
+#define REPORT_ID_DJ_LONG			0x21
+
+#define REPORT_ID_HIDPP_SHORT			0x10
+#define REPORT_ID_HIDPP_LONG			0x11
+
+#define HIDPP_REPORT_SHORT_LENGTH		7
+#define HIDPP_REPORT_LONG_LENGTH		20
+
+#define HIDPP_RECEIVER_INDEX			0xff
+
+#define REPORT_TYPE_RFREPORT_FIRST		0x01
+#define REPORT_TYPE_RFREPORT_LAST		0x1F
+
+/* Command Switch to DJ mode */
+#define REPORT_TYPE_CMD_SWITCH			0x80
+#define CMD_SWITCH_PARAM_DEVBITFIELD		0x00
+#define CMD_SWITCH_PARAM_TIMEOUT_SECONDS	0x01
+#define TIMEOUT_NO_KEEPALIVE			0x00
+
+/* Command to Get the list of Paired devices */
+#define REPORT_TYPE_CMD_GET_PAIRED_DEVICES	0x81
+
+/* Device Paired Notification */
+#define REPORT_TYPE_NOTIF_DEVICE_PAIRED		0x41
+#define SPFUNCTION_MORE_NOTIF_EXPECTED		0x01
+#define SPFUNCTION_DEVICE_LIST_EMPTY		0x02
+#define DEVICE_PAIRED_PARAM_SPFUNCTION		0x00
+#define DEVICE_PAIRED_PARAM_EQUAD_ID_LSB	0x01
+#define DEVICE_PAIRED_PARAM_EQUAD_ID_MSB	0x02
+#define DEVICE_PAIRED_RF_REPORT_TYPE		0x03
+
+/* Device Un-Paired Notification */
+#define REPORT_TYPE_NOTIF_DEVICE_UNPAIRED	0x40
+
+
+/* Connection Status Notification */
+#define REPORT_TYPE_NOTIF_CONNECTION_STATUS	0x42
+#define CONNECTION_STATUS_PARAM_STATUS		0x00
+#define STATUS_LINKLOSS				0x01
+
+/* Error Notification */
+#define REPORT_TYPE_NOTIF_ERROR			0x7F
+#define NOTIF_ERROR_PARAM_ETYPE			0x00
+#define ETYPE_KEEPALIVE_TIMEOUT			0x01
+
+/* supported DJ HID && RF report types */
+#define REPORT_TYPE_KEYBOARD			0x01
+#define REPORT_TYPE_MOUSE			0x02
+#define REPORT_TYPE_CONSUMER_CONTROL		0x03
+#define REPORT_TYPE_SYSTEM_CONTROL		0x04
+#define REPORT_TYPE_MEDIA_CENTER		0x08
+#define REPORT_TYPE_LEDS			0x0E
+
+/* RF Report types bitfield */
+#define STD_KEYBOARD				0x00000002
+#define STD_MOUSE				0x00000004
+#define MULTIMEDIA				0x00000008
+#define POWER_KEYS				0x00000010
+#define MEDIA_CENTER				0x00000100
+#define KBD_LEDS				0x00004000
+
+struct dj_report {
+	u8 report_id;
+	u8 device_index;
+	u8 report_type;
+	u8 report_params[DJREPORT_SHORT_LENGTH - 3];
+};
+
+struct dj_receiver_dev {
+	struct hid_device *hdev;
+	struct dj_device *paired_dj_devices[DJ_MAX_PAIRED_DEVICES +
+					    DJ_DEVICE_INDEX_MIN];
+	struct work_struct work;
+	struct kfifo notif_fifo;
+	spinlock_t lock;
+	bool querying_devices;
+};
+
+struct dj_device {
+	struct hid_device *hdev;
+	struct dj_receiver_dev *dj_receiver_dev;
+	u32 reports_supported;
+	u8 device_index;
+};
 
 /* Keyboard descriptor (1) */
 static const char kbd_descriptor[] = {
@@ -156,6 +251,57 @@ static const char media_descriptor[] = {
 	0xc0,			/* EndCollection                       */
 };				/*                                     */
 
+/* HIDPP descriptor */
+static const char hidpp_descriptor[] = {
+	0x06, 0x00, 0xff,	/* Usage Page (Vendor Defined Page 1)  */
+	0x09, 0x01,		/* Usage (Vendor Usage 1)              */
+	0xa1, 0x01,		/* Collection (Application)            */
+	0x85, 0x10,		/*   Report ID (16)                    */
+	0x75, 0x08,		/*   Report Size (8)                   */
+	0x95, 0x06,		/*   Report Count (6)                  */
+	0x15, 0x00,		/*   Logical Minimum (0)               */
+	0x26, 0xff, 0x00,	/*   Logical Maximum (255)             */
+	0x09, 0x01,		/*   Usage (Vendor Usage 1)            */
+	0x81, 0x00,		/*   Input (Data,Arr,Abs)              */
+	0x09, 0x01,		/*   Usage (Vendor Usage 1)            */
+	0x91, 0x00,		/*   Output (Data,Arr,Abs)             */
+	0xc0,			/* End Collection                      */
+	0x06, 0x00, 0xff,	/* Usage Page (Vendor Defined Page 1)  */
+	0x09, 0x02,		/* Usage (Vendor Usage 2)              */
+	0xa1, 0x01,		/* Collection (Application)            */
+	0x85, 0x11,		/*   Report ID (17)                    */
+	0x75, 0x08,		/*   Report Size (8)                   */
+	0x95, 0x13,		/*   Report Count (19)                 */
+	0x15, 0x00,		/*   Logical Minimum (0)               */
+	0x26, 0xff, 0x00,	/*   Logical Maximum (255)             */
+	0x09, 0x02,		/*   Usage (Vendor Usage 2)            */
+	0x81, 0x00,		/*   Input (Data,Arr,Abs)              */
+	0x09, 0x02,		/*   Usage (Vendor Usage 2)            */
+	0x91, 0x00,		/*   Output (Data,Arr,Abs)             */
+	0xc0,			/* End Collection                      */
+	0x06, 0x00, 0xff,	/* Usage Page (Vendor Defined Page 1)  */
+	0x09, 0x04,		/* Usage (Vendor Usage 0x04)           */
+	0xa1, 0x01,		/* Collection (Application)            */
+	0x85, 0x20,		/*   Report ID (32)                    */
+	0x75, 0x08,		/*   Report Size (8)                   */
+	0x95, 0x0e,		/*   Report Count (14)                 */
+	0x15, 0x00,		/*   Logical Minimum (0)               */
+	0x26, 0xff, 0x00,	/*   Logical Maximum (255)             */
+	0x09, 0x41,		/*   Usage (Vendor Usage 0x41)         */
+	0x81, 0x00,		/*   Input (Data,Arr,Abs)              */
+	0x09, 0x41,		/*   Usage (Vendor Usage 0x41)         */
+	0x91, 0x00,		/*   Output (Data,Arr,Abs)             */
+	0x85, 0x21,		/*   Report ID (33)                    */
+	0x95, 0x1f,		/*   Report Count (31)                 */
+	0x15, 0x00,		/*   Logical Minimum (0)               */
+	0x26, 0xff, 0x00,	/*   Logical Maximum (255)             */
+	0x09, 0x42,		/*   Usage (Vendor Usage 0x42)         */
+	0x81, 0x00,		/*   Input (Data,Arr,Abs)              */
+	0x09, 0x42,		/*   Usage (Vendor Usage 0x42)         */
+	0x91, 0x00,		/*   Output (Data,Arr,Abs)             */
+	0xc0,			/* End Collection                      */
+};
+
 /* Maximum size of all defined hid reports in bytes (including report id) */
 #define MAX_REPORT_SIZE 8
 
@@ -165,7 +311,8 @@ static const char media_descriptor[] = {
 	 sizeof(mse_descriptor) +		\
 	 sizeof(consumer_descriptor) +		\
 	 sizeof(syscontrol_descriptor) +	\
-	 sizeof(media_descriptor))
+	 sizeof(media_descriptor) +	\
+	 sizeof(hidpp_descriptor))
 
 /* Number of possible hid report types that can be created by this driver.
  *
@@ -256,11 +403,15 @@ static void logi_dj_recv_add_djhid_device(struct dj_receiver_dev *djrcv_dev,
 	dj_hiddev->dev.parent = &djrcv_hdev->dev;
 	dj_hiddev->bus = BUS_USB;
 	dj_hiddev->vendor = le16_to_cpu(usbdev->descriptor.idVendor);
-	dj_hiddev->product = le16_to_cpu(usbdev->descriptor.idProduct);
+	dj_hiddev->product =
+		(dj_report->report_params[DEVICE_PAIRED_PARAM_EQUAD_ID_MSB]
+									<< 8) |
+		dj_report->report_params[DEVICE_PAIRED_PARAM_EQUAD_ID_LSB];
 	snprintf(dj_hiddev->name, sizeof(dj_hiddev->name),
-		"Logitech Unifying Device. Wireless PID:%02x%02x",
-		dj_report->report_params[DEVICE_PAIRED_PARAM_EQUAD_ID_MSB],
-		dj_report->report_params[DEVICE_PAIRED_PARAM_EQUAD_ID_LSB]);
+		"Logitech Unifying Device. Wireless PID:%04x",
+		dj_hiddev->product);
+
+	dj_hiddev->group = HID_GROUP_LOGITECH_DJ_DEVICE;
 
 	usb_make_path(usbdev, dj_hiddev->phys, sizeof(dj_hiddev->phys));
 	snprintf(tmpstr, sizeof(tmpstr), ":%d", dj_report->device_index);
@@ -422,6 +573,13 @@ static void logi_dj_recv_forward_report(struct dj_receiver_dev *djrcv_dev,
 	}
 }
 
+static void logi_dj_recv_forward_hidpp(struct dj_device *dj_dev, u8 *data,
+				       int size)
+{
+	/* We are called from atomic context (tasklet && djrcv->lock held) */
+	if (hid_input_report(dj_dev->hdev, HID_INPUT_REPORT, data, size, 1))
+		dbg_hid("hid_input_report error\n");
+}
 
 static int logi_dj_recv_send_report(struct dj_receiver_dev *djrcv_dev,
 				    struct dj_report *dj_report)
@@ -472,7 +630,9 @@ static int logi_dj_recv_query_paired_devices(struct dj_receiver_dev *djrcv_dev)
 static int logi_dj_recv_switch_to_dj_mode(struct dj_receiver_dev *djrcv_dev,
 					  unsigned timeout)
 {
+	struct hid_device *hdev = djrcv_dev->hdev;
 	struct dj_report *dj_report;
+	u8 *buf;
 	int retval;
 
 	dj_report = kzalloc(sizeof(struct dj_report), GFP_KERNEL);
@@ -484,7 +644,6 @@ static int logi_dj_recv_switch_to_dj_mode(struct dj_receiver_dev *djrcv_dev,
 	dj_report->report_params[CMD_SWITCH_PARAM_DEVBITFIELD] = 0x3F;
 	dj_report->report_params[CMD_SWITCH_PARAM_TIMEOUT_SECONDS] = (u8)timeout;
 	retval = logi_dj_recv_send_report(djrcv_dev, dj_report);
-	kfree(dj_report);
 
 	/*
 	 * Ugly sleep to work around a USB 3.0 bug when the receiver is still
@@ -493,6 +652,30 @@ static int logi_dj_recv_switch_to_dj_mode(struct dj_receiver_dev *djrcv_dev,
 	 */
 	msleep(50);
 
+	/*
+	 * Magical bits to set up hidpp notifications when the dj devices
+	 * are connected/disconnected.
+	 *
+	 * We can reuse dj_report because HIDPP_REPORT_SHORT_LENGTH is smaller
+	 * than DJREPORT_SHORT_LENGTH.
+	 */
+	buf = (u8 *)dj_report;
+
+	memset(buf, 0, HIDPP_REPORT_SHORT_LENGTH);
+
+	buf[0] = REPORT_ID_HIDPP_SHORT;
+	buf[1] = 0xFF;
+	buf[2] = 0x80;
+	buf[3] = 0x00;
+	buf[4] = 0x00;
+	buf[5] = 0x09;
+	buf[6] = 0x00;
+
+	hid_hw_raw_request(hdev, REPORT_ID_HIDPP_SHORT, buf,
+			HIDPP_REPORT_SHORT_LENGTH, HID_OUTPUT_REPORT,
+			HID_REQ_SET_REPORT);
+
+	kfree(dj_report);
 	return retval;
 }
 
@@ -509,6 +692,9 @@ static void logi_dj_ll_close(struct hid_device *hid)
 	dbg_hid("%s:%s\n", __func__, hid->phys);
 }
 
+static u8 unifying_name_query[]  = {0x10, 0xff, 0x83, 0xb5, 0x40, 0x00, 0x00};
+static u8 unifying_name_answer[] = {0x11, 0xff, 0x83, 0xb5};
+
 static int logi_dj_ll_raw_request(struct hid_device *hid,
 				  unsigned char reportnum, __u8 *buf,
 				  size_t count, unsigned char report_type,
@@ -519,6 +705,22 @@ static int logi_dj_ll_raw_request(struct hid_device *hid,
 	u8 *out_buf;
 	int ret;
 
+	if ((buf[0] == REPORT_ID_HIDPP_SHORT) ||
+	    (buf[0] == REPORT_ID_HIDPP_LONG)) {
+		if (count < 2)
+			return -EINVAL;
+
+		/* special case where we should not overwrite
+		 * the device_index */
+		if (count == 7 && !memcmp(buf, unifying_name_query,
+					  sizeof(unifying_name_query)))
+			buf[4] |= djdev->device_index - 1;
+		else
+			buf[1] = djdev->device_index;
+		return hid_hw_raw_request(djrcv_dev->hdev, reportnum, buf,
+				count, report_type, reqtype);
+	}
+
 	if (buf[0] != REPORT_TYPE_LEDS)
 		return -EINVAL;
 
@@ -597,6 +799,8 @@ static int logi_dj_ll_parse(struct hid_device *hid)
 			__func__, djdev->reports_supported);
 	}
 
+	rdcat(rdesc, &rsize, hidpp_descriptor, sizeof(hidpp_descriptor));
+
 	retval = hid_parse_report(hid, rdesc, rsize);
 	kfree(rdesc);
 
@@ -624,8 +828,7 @@ static struct hid_ll_driver logi_dj_ll_driver = {
 	.raw_request = logi_dj_ll_raw_request,
 };
 
-
-static int logi_dj_raw_event(struct hid_device *hdev,
+static int logi_dj_dj_event(struct hid_device *hdev,
 			     struct hid_report *report, u8 *data,
 			     int size)
 {
@@ -633,36 +836,24 @@ static int logi_dj_raw_event(struct hid_device *hdev,
 	struct dj_report *dj_report = (struct dj_report *) data;
 	unsigned long flags;
 
-	dbg_hid("%s, size:%d\n", __func__, size);
-
-	/* Here we receive all data coming from iface 2, there are 4 cases:
-	 *
-	 * 1) Data should continue its normal processing i.e. data does not
-	 * come from the DJ collection, in which case we do nothing and
-	 * return 0, so hid-core can continue normal processing (will forward
-	 * to associated hidraw device)
+	/*
+	 * Here we receive all data coming from iface 2, there are 3 cases:
 	 *
-	 * 2) Data is from DJ collection, and is intended for this driver i. e.
-	 * data contains arrival, departure, etc notifications, in which case
-	 * we queue them for delayed processing by the work queue. We return 1
-	 * to hid-core as no further processing is required from it.
+	 * 1) Data is intended for this driver i. e. data contains arrival,
+	 * departure, etc notifications, in which case we queue them for delayed
+	 * processing by the work queue. We return 1 to hid-core as no further
+	 * processing is required from it.
 	 *
-	 * 3) Data is from DJ collection, and informs a connection change,
-	 * if the change means rf link loss, then we must send a null report
-	 * to the upper layer to discard potentially pressed keys that may be
-	 * repeated forever by the input layer. Return 1 to hid-core as no
-	 * further processing is required.
+	 * 2) Data informs a connection change, if the change means rf link
+	 * loss, then we must send a null report to the upper layer to discard
+	 * potentially pressed keys that may be repeated forever by the input
+	 * layer. Return 1 to hid-core as no further processing is required.
 	 *
-	 * 4) Data is from DJ collection and is an actual input event from
-	 * a paired DJ device in which case we forward it to the correct hid
-	 * device (via hid_input_report() ) and return 1 so hid-core does not do
-	 * anything else with it.
+	 * 3) Data is an actual input event from a paired DJ device in which
+	 * case we forward it to the correct hid device (via hid_input_report()
+	 * ) and return 1 so hid-core does not anything else with it.
 	 */
 
-	/* case 1) */
-	if (data[0] != REPORT_ID_DJ_SHORT)
-		return false;
-
 	if ((dj_report->device_index < DJ_DEVICE_INDEX_MIN) ||
 	    (dj_report->device_index > DJ_DEVICE_INDEX_MAX)) {
 		/*
@@ -707,6 +898,80 @@ out:
 	return true;
 }
 
+static int logi_dj_hidpp_event(struct hid_device *hdev,
+			     struct hid_report *report, u8 *data,
+			     int size)
+{
+	struct dj_receiver_dev *djrcv_dev = hid_get_drvdata(hdev);
+	struct dj_report *dj_report = (struct dj_report *) data;
+	unsigned long flags;
+	u8 device_index = dj_report->device_index;
+
+	if (device_index == HIDPP_RECEIVER_INDEX) {
+		/* special case were the device wants to know its unifying
+		 * name */
+		if (size == HIDPP_REPORT_LONG_LENGTH &&
+		    !memcmp(data, unifying_name_answer,
+			    sizeof(unifying_name_answer)) &&
+		    ((data[4] & 0xF0) == 0x40))
+			device_index = (data[4] & 0x0F) + 1;
+		else
+			return false;
+	}
+
+	/*
+	 * Data is from the HID++ collection, in this case, we forward the
+	 * data to the corresponding child dj device and return 0 to hid-core
+	 * so he data also goes to the hidraw device of the receiver. This
+	 * allows a user space application to implement the full HID++ routing
+	 * via the receiver.
+	 */
+
+	if ((device_index < DJ_DEVICE_INDEX_MIN) ||
+	    (device_index > DJ_DEVICE_INDEX_MAX)) {
+		/*
+		 * Device index is wrong, bail out.
+		 * This driver can ignore safely the receiver notifications,
+		 * so ignore those reports too.
+		 */
+		dev_err(&hdev->dev, "%s: invalid device index:%d\n",
+				__func__, dj_report->device_index);
+		return false;
+	}
+
+	spin_lock_irqsave(&djrcv_dev->lock, flags);
+
+	if (!djrcv_dev->paired_dj_devices[device_index])
+		/* received an event for an unknown device, bail out */
+		goto out;
+
+	logi_dj_recv_forward_hidpp(djrcv_dev->paired_dj_devices[device_index],
+				   data, size);
+
+out:
+	spin_unlock_irqrestore(&djrcv_dev->lock, flags);
+
+	return false;
+}
+
+static int logi_dj_raw_event(struct hid_device *hdev,
+			     struct hid_report *report, u8 *data,
+			     int size)
+{
+	dbg_hid("%s, size:%d\n", __func__, size);
+
+	switch (data[0]) {
+	case REPORT_ID_DJ_SHORT:
+		return logi_dj_dj_event(hdev, report, data, size);
+	case REPORT_ID_HIDPP_SHORT:
+		/* intentional fallthrough */
+	case REPORT_ID_HIDPP_LONG:
+		return logi_dj_hidpp_event(hdev, report, data, size);
+	}
+
+	return false;
+}
+
 static int logi_dj_probe(struct hid_device *hdev,
 			 const struct hid_device_id *id)
 {
@@ -714,9 +979,6 @@ static int logi_dj_probe(struct hid_device *hdev,
 	struct dj_receiver_dev *djrcv_dev;
 	int retval;
 
-	if (is_dj_device((struct dj_device *)hdev->driver_data))
-		return -ENODEV;
-
 	dbg_hid("%s called for ifnum %d\n", __func__,
 		intf->cur_altsetting->desc.bInterfaceNumber);
 
@@ -869,22 +1131,6 @@ static void logi_dj_remove(struct hid_device *hdev)
 	hid_set_drvdata(hdev, NULL);
 }
 
-static int logi_djdevice_probe(struct hid_device *hdev,
-			 const struct hid_device_id *id)
-{
-	int ret;
-	struct dj_device *dj_dev = hdev->driver_data;
-
-	if (!is_dj_device(dj_dev))
-		return -ENODEV;
-
-	ret = hid_parse(hdev);
-	if (!ret)
-		ret = hid_hw_start(hdev, HID_CONNECT_DEFAULT);
-
-	return ret;
-}
-
 static const struct hid_device_id logi_dj_receivers[] = {
 	{HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH,
 		USB_DEVICE_ID_LOGITECH_UNIFYING_RECEIVER)},
@@ -906,51 +1152,8 @@ static struct hid_driver logi_djreceiver_driver = {
 #endif
 };
 
+module_hid_driver(logi_djreceiver_driver);
 
-static const struct hid_device_id logi_dj_devices[] = {
-	{HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH,
-		USB_DEVICE_ID_LOGITECH_UNIFYING_RECEIVER)},
-	{HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH,
-		USB_DEVICE_ID_LOGITECH_UNIFYING_RECEIVER_2)},
-	{}
-};
-
-static struct hid_driver logi_djdevice_driver = {
-	.name = "logitech-djdevice",
-	.id_table = logi_dj_devices,
-	.probe = logi_djdevice_probe,
-};
-
-
-static int __init logi_dj_init(void)
-{
-	int retval;
-
-	dbg_hid("Logitech-DJ:%s\n", __func__);
-
-	retval = hid_register_driver(&logi_djreceiver_driver);
-	if (retval)
-		return retval;
-
-	retval = hid_register_driver(&logi_djdevice_driver);
-	if (retval)
-		hid_unregister_driver(&logi_djreceiver_driver);
-
-	return retval;
-
-}
-
-static void __exit logi_dj_exit(void)
-{
-	dbg_hid("Logitech-DJ:%s\n", __func__);
-
-	hid_unregister_driver(&logi_djdevice_driver);
-	hid_unregister_driver(&logi_djreceiver_driver);
-
-}
-
-module_init(logi_dj_init);
-module_exit(logi_dj_exit);
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Logitech");
 MODULE_AUTHOR("Nestor Lopez Casado");
diff --git a/drivers/hid/hid-logitech-dj.h b/drivers/hid/hid-logitech-dj.h
deleted file mode 100644
index daeb0aa4bee9..000000000000
--- a/drivers/hid/hid-logitech-dj.h
+++ /dev/null
@@ -1,125 +0,0 @@
-#ifndef __HID_LOGITECH_DJ_H
-#define __HID_LOGITECH_DJ_H
-
-/*
- *  HID driver for Logitech Unifying receivers
- *
- *  Copyright (c) 2011 Logitech
- */
-
-/*
- * 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/kfifo.h>
-
-#define DJ_MAX_PAIRED_DEVICES			6
-#define DJ_MAX_NUMBER_NOTIFICATIONS		8
-#define DJ_RECEIVER_INDEX			0
-#define DJ_DEVICE_INDEX_MIN 			1
-#define DJ_DEVICE_INDEX_MAX 			6
-
-#define DJREPORT_SHORT_LENGTH			15
-#define DJREPORT_LONG_LENGTH			32
-
-#define REPORT_ID_DJ_SHORT			0x20
-#define REPORT_ID_DJ_LONG			0x21
-
-#define REPORT_TYPE_RFREPORT_FIRST		0x01
-#define REPORT_TYPE_RFREPORT_LAST		0x1F
-
-/* Command Switch to DJ mode */
-#define REPORT_TYPE_CMD_SWITCH			0x80
-#define CMD_SWITCH_PARAM_DEVBITFIELD		0x00
-#define CMD_SWITCH_PARAM_TIMEOUT_SECONDS	0x01
-#define TIMEOUT_NO_KEEPALIVE			0x00
-
-/* Command to Get the list of Paired devices */
-#define REPORT_TYPE_CMD_GET_PAIRED_DEVICES	0x81
-
-/* Device Paired Notification */
-#define REPORT_TYPE_NOTIF_DEVICE_PAIRED		0x41
-#define SPFUNCTION_MORE_NOTIF_EXPECTED		0x01
-#define SPFUNCTION_DEVICE_LIST_EMPTY		0x02
-#define DEVICE_PAIRED_PARAM_SPFUNCTION		0x00
-#define DEVICE_PAIRED_PARAM_EQUAD_ID_LSB	0x01
-#define DEVICE_PAIRED_PARAM_EQUAD_ID_MSB	0x02
-#define DEVICE_PAIRED_RF_REPORT_TYPE		0x03
-
-/* Device Un-Paired Notification */
-#define REPORT_TYPE_NOTIF_DEVICE_UNPAIRED	0x40
-
-
-/* Connection Status Notification */
-#define REPORT_TYPE_NOTIF_CONNECTION_STATUS	0x42
-#define CONNECTION_STATUS_PARAM_STATUS		0x00
-#define STATUS_LINKLOSS				0x01
-
-/* Error Notification */
-#define REPORT_TYPE_NOTIF_ERROR			0x7F
-#define NOTIF_ERROR_PARAM_ETYPE			0x00
-#define ETYPE_KEEPALIVE_TIMEOUT			0x01
-
-/* supported DJ HID && RF report types */
-#define REPORT_TYPE_KEYBOARD			0x01
-#define REPORT_TYPE_MOUSE			0x02
-#define REPORT_TYPE_CONSUMER_CONTROL		0x03
-#define REPORT_TYPE_SYSTEM_CONTROL		0x04
-#define REPORT_TYPE_MEDIA_CENTER		0x08
-#define REPORT_TYPE_LEDS			0x0E
-
-/* RF Report types bitfield */
-#define STD_KEYBOARD				0x00000002
-#define STD_MOUSE				0x00000004
-#define MULTIMEDIA				0x00000008
-#define POWER_KEYS				0x00000010
-#define MEDIA_CENTER				0x00000100
-#define KBD_LEDS				0x00004000
-
-struct dj_report {
-	u8 report_id;
-	u8 device_index;
-	u8 report_type;
-	u8 report_params[DJREPORT_SHORT_LENGTH - 3];
-};
-
-struct dj_receiver_dev {
-	struct hid_device *hdev;
-	struct dj_device *paired_dj_devices[DJ_MAX_PAIRED_DEVICES +
-					    DJ_DEVICE_INDEX_MIN];
-	struct work_struct work;
-	struct kfifo notif_fifo;
-	spinlock_t lock;
-	bool querying_devices;
-};
-
-struct dj_device {
-	struct hid_device *hdev;
-	struct dj_receiver_dev *dj_receiver_dev;
-	u32 reports_supported;
-	u8 device_index;
-};
-
-/**
- * is_dj_device - know if the given dj_device is not the receiver.
- * @dj_dev: the dj device to test
- *
- * This macro tests if a struct dj_device pointer is a device created
- * by the bus enumarator.
- */
-#define is_dj_device(dj_dev) \
-	(&(dj_dev)->dj_receiver_dev->hdev->dev == (dj_dev)->hdev->dev.parent)
-
-#endif
diff --git a/drivers/hid/hid-logitech-hidpp.c b/drivers/hid/hid-logitech-hidpp.c
new file mode 100644
index 000000000000..2f420c0b6609
--- /dev/null
+++ b/drivers/hid/hid-logitech-hidpp.c
@@ -0,0 +1,1241 @@
+/*
+ *  HIDPP protocol for Logitech Unifying receivers
+ *
+ *  Copyright (c) 2011 Logitech (c)
+ *  Copyright (c) 2012-2013 Google (c)
+ *  Copyright (c) 2013-2014 Red Hat 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 of the License.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/device.h>
+#include <linux/hid.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/sched.h>
+#include <linux/kfifo.h>
+#include <linux/input/mt.h>
+#include <asm/unaligned.h>
+#include "hid-ids.h"
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Benjamin Tissoires <benjamin.tissoires@gmail.com>");
+MODULE_AUTHOR("Nestor Lopez Casado <nlopezcasad@logitech.com>");
+
+#define REPORT_ID_HIDPP_SHORT			0x10
+#define REPORT_ID_HIDPP_LONG			0x11
+
+#define HIDPP_REPORT_SHORT_LENGTH		7
+#define HIDPP_REPORT_LONG_LENGTH		20
+
+#define HIDPP_QUIRK_CLASS_WTP			BIT(0)
+
+/* bits 1..20 are reserved for classes */
+#define HIDPP_QUIRK_DELAYED_INIT		BIT(21)
+#define HIDPP_QUIRK_WTP_PHYSICAL_BUTTONS	BIT(22)
+#define HIDPP_QUIRK_MULTI_INPUT			BIT(23)
+
+/*
+ * There are two hidpp protocols in use, the first version hidpp10 is known
+ * as register access protocol or RAP, the second version hidpp20 is known as
+ * feature access protocol or FAP
+ *
+ * Most older devices (including the Unifying usb receiver) use the RAP protocol
+ * where as most newer devices use the FAP protocol. Both protocols are
+ * compatible with the underlying transport, which could be usb, Unifiying, or
+ * bluetooth. The message lengths are defined by the hid vendor specific report
+ * descriptor for the HIDPP_SHORT report type (total message lenth 7 bytes) and
+ * the HIDPP_LONG report type (total message length 20 bytes)
+ *
+ * The RAP protocol uses both report types, whereas the FAP only uses HIDPP_LONG
+ * messages. The Unifying receiver itself responds to RAP messages (device index
+ * is 0xFF for the receiver), and all messages (short or long) with a device
+ * index between 1 and 6 are passed untouched to the corresponding paired
+ * Unifying device.
+ *
+ * The paired device can be RAP or FAP, it will receive the message untouched
+ * from the Unifiying receiver.
+ */
+
+struct fap {
+	u8 feature_index;
+	u8 funcindex_clientid;
+	u8 params[HIDPP_REPORT_LONG_LENGTH - 4U];
+};
+
+struct rap {
+	u8 sub_id;
+	u8 reg_address;
+	u8 params[HIDPP_REPORT_LONG_LENGTH - 4U];
+};
+
+struct hidpp_report {
+	u8 report_id;
+	u8 device_index;
+	union {
+		struct fap fap;
+		struct rap rap;
+		u8 rawbytes[sizeof(struct fap)];
+	};
+} __packed;
+
+struct hidpp_device {
+	struct hid_device *hid_dev;
+	struct mutex send_mutex;
+	void *send_receive_buf;
+	wait_queue_head_t wait;
+	bool answer_available;
+	u8 protocol_major;
+	u8 protocol_minor;
+
+	void *private_data;
+
+	struct work_struct work;
+	struct kfifo delayed_work_fifo;
+	atomic_t connected;
+	struct input_dev *delayed_input;
+
+	unsigned long quirks;
+};
+
+
+#define HIDPP_ERROR				0x8f
+#define HIDPP_ERROR_SUCCESS			0x00
+#define HIDPP_ERROR_INVALID_SUBID		0x01
+#define HIDPP_ERROR_INVALID_ADRESS		0x02
+#define HIDPP_ERROR_INVALID_VALUE		0x03
+#define HIDPP_ERROR_CONNECT_FAIL		0x04
+#define HIDPP_ERROR_TOO_MANY_DEVICES		0x05
+#define HIDPP_ERROR_ALREADY_EXISTS		0x06
+#define HIDPP_ERROR_BUSY			0x07
+#define HIDPP_ERROR_UNKNOWN_DEVICE		0x08
+#define HIDPP_ERROR_RESOURCE_ERROR		0x09
+#define HIDPP_ERROR_REQUEST_UNAVAILABLE		0x0a
+#define HIDPP_ERROR_INVALID_PARAM_VALUE		0x0b
+#define HIDPP_ERROR_WRONG_PIN_CODE		0x0c
+
+static void hidpp_connect_event(struct hidpp_device *hidpp_dev);
+
+static int __hidpp_send_report(struct hid_device *hdev,
+				struct hidpp_report *hidpp_report)
+{
+	int fields_count, ret;
+
+	switch (hidpp_report->report_id) {
+	case REPORT_ID_HIDPP_SHORT:
+		fields_count = HIDPP_REPORT_SHORT_LENGTH;
+		break;
+	case REPORT_ID_HIDPP_LONG:
+		fields_count = HIDPP_REPORT_LONG_LENGTH;
+		break;
+	default:
+		return -ENODEV;
+	}
+
+	/*
+	 * set the device_index as the receiver, it will be overwritten by
+	 * hid_hw_request if needed
+	 */
+	hidpp_report->device_index = 0xff;
+
+	ret = hid_hw_raw_request(hdev, hidpp_report->report_id,
+		(u8 *)hidpp_report, fields_count, HID_OUTPUT_REPORT,
+		HID_REQ_SET_REPORT);
+
+	return ret == fields_count ? 0 : -1;
+}
+
+/**
+ * hidpp_send_message_sync() returns 0 in case of success, and something else
+ * in case of a failure.
+ * - If ' something else' is positive, that means that an error has been raised
+ *   by the protocol itself.
+ * - If ' something else' is negative, that means that we had a classic error
+ *   (-ENOMEM, -EPIPE, etc...)
+ */
+static int hidpp_send_message_sync(struct hidpp_device *hidpp,
+	struct hidpp_report *message,
+	struct hidpp_report *response)
+{
+	int ret;
+
+	mutex_lock(&hidpp->send_mutex);
+
+	hidpp->send_receive_buf = response;
+	hidpp->answer_available = false;
+
+	/*
+	 * So that we can later validate the answer when it arrives
+	 * in hidpp_raw_event
+	 */
+	*response = *message;
+
+	ret = __hidpp_send_report(hidpp->hid_dev, message);
+
+	if (ret) {
+		dbg_hid("__hidpp_send_report returned err: %d\n", ret);
+		memset(response, 0, sizeof(struct hidpp_report));
+		goto exit;
+	}
+
+	if (!wait_event_timeout(hidpp->wait, hidpp->answer_available,
+				5*HZ)) {
+		dbg_hid("%s:timeout waiting for response\n", __func__);
+		memset(response, 0, sizeof(struct hidpp_report));
+		ret = -ETIMEDOUT;
+	}
+
+	if (response->report_id == REPORT_ID_HIDPP_SHORT &&
+	    response->fap.feature_index == HIDPP_ERROR) {
+		ret = response->fap.params[1];
+		dbg_hid("__hidpp_send_report got hidpp error %02X\n", ret);
+		goto exit;
+	}
+
+exit:
+	mutex_unlock(&hidpp->send_mutex);
+	return ret;
+
+}
+
+static int hidpp_send_fap_command_sync(struct hidpp_device *hidpp,
+	u8 feat_index, u8 funcindex_clientid, u8 *params, int param_count,
+	struct hidpp_report *response)
+{
+	struct hidpp_report *message;
+	int ret;
+
+	if (param_count > sizeof(message->fap.params))
+		return -EINVAL;
+
+	message = kzalloc(sizeof(struct hidpp_report), GFP_KERNEL);
+	if (!message)
+		return -ENOMEM;
+	message->report_id = REPORT_ID_HIDPP_LONG;
+	message->fap.feature_index = feat_index;
+	message->fap.funcindex_clientid = funcindex_clientid;
+	memcpy(&message->fap.params, params, param_count);
+
+	ret = hidpp_send_message_sync(hidpp, message, response);
+	kfree(message);
+	return ret;
+}
+
+static int hidpp_send_rap_command_sync(struct hidpp_device *hidpp_dev,
+	u8 report_id, u8 sub_id, u8 reg_address, u8 *params, int param_count,
+	struct hidpp_report *response)
+{
+	struct hidpp_report *message;
+	int ret;
+
+	if ((report_id != REPORT_ID_HIDPP_SHORT) &&
+	    (report_id != REPORT_ID_HIDPP_LONG))
+		return -EINVAL;
+
+	if (param_count > sizeof(message->rap.params))
+		return -EINVAL;
+
+	message = kzalloc(sizeof(struct hidpp_report), GFP_KERNEL);
+	if (!message)
+		return -ENOMEM;
+	message->report_id = report_id;
+	message->rap.sub_id = sub_id;
+	message->rap.reg_address = reg_address;
+	memcpy(&message->rap.params, params, param_count);
+
+	ret = hidpp_send_message_sync(hidpp_dev, message, response);
+	kfree(message);
+	return ret;
+}
+
+static void delayed_work_cb(struct work_struct *work)
+{
+	struct hidpp_device *hidpp = container_of(work, struct hidpp_device,
+							work);
+	hidpp_connect_event(hidpp);
+}
+
+static inline bool hidpp_match_answer(struct hidpp_report *question,
+		struct hidpp_report *answer)
+{
+	return (answer->fap.feature_index == question->fap.feature_index) &&
+	   (answer->fap.funcindex_clientid == question->fap.funcindex_clientid);
+}
+
+static inline bool hidpp_match_error(struct hidpp_report *question,
+		struct hidpp_report *answer)
+{
+	return (answer->fap.feature_index == HIDPP_ERROR) &&
+	    (answer->fap.funcindex_clientid == question->fap.feature_index) &&
+	    (answer->fap.params[0] == question->fap.funcindex_clientid);
+}
+
+static inline bool hidpp_report_is_connect_event(struct hidpp_report *report)
+{
+	return (report->report_id == REPORT_ID_HIDPP_SHORT) &&
+		(report->rap.sub_id == 0x41);
+}
+
+/* -------------------------------------------------------------------------- */
+/* HIDP++ 1.0 commands                                                        */
+/* -------------------------------------------------------------------------- */
+
+#define HIDPP_SET_REGISTER				0x80
+#define HIDPP_GET_REGISTER				0x81
+#define HIDPP_SET_LONG_REGISTER				0x82
+#define HIDPP_GET_LONG_REGISTER				0x83
+
+#define HIDPP_REG_PAIRING_INFORMATION			0xB5
+#define DEVICE_NAME					0x40
+
+static char *hidpp_get_unifying_name(struct hidpp_device *hidpp_dev)
+{
+	struct hidpp_report response;
+	int ret;
+	/* hid-logitech-dj is in charge of setting the right device index */
+	u8 params[1] = { DEVICE_NAME };
+	char *name;
+	int len;
+
+	ret = hidpp_send_rap_command_sync(hidpp_dev,
+					REPORT_ID_HIDPP_SHORT,
+					HIDPP_GET_LONG_REGISTER,
+					HIDPP_REG_PAIRING_INFORMATION,
+					params, 1, &response);
+	if (ret)
+		return NULL;
+
+	len = response.rap.params[1];
+
+	if (2 + len > sizeof(response.rap.params))
+		return NULL;
+
+	name = kzalloc(len + 1, GFP_KERNEL);
+	if (!name)
+		return NULL;
+
+	memcpy(name, &response.rap.params[2], len);
+	return name;
+}
+
+/* -------------------------------------------------------------------------- */
+/* 0x0000: Root                                                               */
+/* -------------------------------------------------------------------------- */
+
+#define HIDPP_PAGE_ROOT					0x0000
+#define HIDPP_PAGE_ROOT_IDX				0x00
+
+#define CMD_ROOT_GET_FEATURE				0x01
+#define CMD_ROOT_GET_PROTOCOL_VERSION			0x11
+
+static int hidpp_root_get_feature(struct hidpp_device *hidpp, u16 feature,
+	u8 *feature_index, u8 *feature_type)
+{
+	struct hidpp_report response;
+	int ret;
+	u8 params[2] = { feature >> 8, feature & 0x00FF };
+
+	ret = hidpp_send_fap_command_sync(hidpp,
+			HIDPP_PAGE_ROOT_IDX,
+			CMD_ROOT_GET_FEATURE,
+			params, 2, &response);
+	if (ret)
+		return ret;
+
+	*feature_index = response.fap.params[0];
+	*feature_type = response.fap.params[1];
+
+	return ret;
+}
+
+static int hidpp_root_get_protocol_version(struct hidpp_device *hidpp)
+{
+	struct hidpp_report response;
+	int ret;
+
+	ret = hidpp_send_fap_command_sync(hidpp,
+			HIDPP_PAGE_ROOT_IDX,
+			CMD_ROOT_GET_PROTOCOL_VERSION,
+			NULL, 0, &response);
+
+	if (ret == HIDPP_ERROR_INVALID_SUBID) {
+		hidpp->protocol_major = 1;
+		hidpp->protocol_minor = 0;
+		return 0;
+	}
+
+	/* the device might not be connected */
+	if (ret == HIDPP_ERROR_RESOURCE_ERROR)
+		return -EIO;
+
+	if (ret > 0) {
+		hid_err(hidpp->hid_dev, "%s: received protocol error 0x%02x\n",
+			__func__, ret);
+		return -EPROTO;
+	}
+	if (ret)
+		return ret;
+
+	hidpp->protocol_major = response.fap.params[0];
+	hidpp->protocol_minor = response.fap.params[1];
+
+	return ret;
+}
+
+static bool hidpp_is_connected(struct hidpp_device *hidpp)
+{
+	int ret;
+
+	ret = hidpp_root_get_protocol_version(hidpp);
+	if (!ret)
+		hid_dbg(hidpp->hid_dev, "HID++ %u.%u device connected.\n",
+			hidpp->protocol_major, hidpp->protocol_minor);
+	return ret == 0;
+}
+
+/* -------------------------------------------------------------------------- */
+/* 0x0005: GetDeviceNameType                                                  */
+/* -------------------------------------------------------------------------- */
+
+#define HIDPP_PAGE_GET_DEVICE_NAME_TYPE			0x0005
+
+#define CMD_GET_DEVICE_NAME_TYPE_GET_COUNT		0x01
+#define CMD_GET_DEVICE_NAME_TYPE_GET_DEVICE_NAME	0x11
+#define CMD_GET_DEVICE_NAME_TYPE_GET_TYPE		0x21
+
+static int hidpp_devicenametype_get_count(struct hidpp_device *hidpp,
+	u8 feature_index, u8 *nameLength)
+{
+	struct hidpp_report response;
+	int ret;
+
+	ret = hidpp_send_fap_command_sync(hidpp, feature_index,
+		CMD_GET_DEVICE_NAME_TYPE_GET_COUNT, NULL, 0, &response);
+
+	if (ret > 0) {
+		hid_err(hidpp->hid_dev, "%s: received protocol error 0x%02x\n",
+			__func__, ret);
+		return -EPROTO;
+	}
+	if (ret)
+		return ret;
+
+	*nameLength = response.fap.params[0];
+
+	return ret;
+}
+
+static int hidpp_devicenametype_get_device_name(struct hidpp_device *hidpp,
+	u8 feature_index, u8 char_index, char *device_name, int len_buf)
+{
+	struct hidpp_report response;
+	int ret, i;
+	int count;
+
+	ret = hidpp_send_fap_command_sync(hidpp, feature_index,
+		CMD_GET_DEVICE_NAME_TYPE_GET_DEVICE_NAME, &char_index, 1,
+		&response);
+
+	if (ret > 0) {
+		hid_err(hidpp->hid_dev, "%s: received protocol error 0x%02x\n",
+			__func__, ret);
+		return -EPROTO;
+	}
+	if (ret)
+		return ret;
+
+	if (response.report_id == REPORT_ID_HIDPP_LONG)
+		count = HIDPP_REPORT_LONG_LENGTH - 4;
+	else
+		count = HIDPP_REPORT_SHORT_LENGTH - 4;
+
+	if (len_buf < count)
+		count = len_buf;
+
+	for (i = 0; i < count; i++)
+		device_name[i] = response.fap.params[i];
+
+	return count;
+}
+
+static char *hidpp_get_device_name(struct hidpp_device *hidpp)
+{
+	u8 feature_type;
+	u8 feature_index;
+	u8 __name_length;
+	char *name;
+	unsigned index = 0;
+	int ret;
+
+	ret = hidpp_root_get_feature(hidpp, HIDPP_PAGE_GET_DEVICE_NAME_TYPE,
+		&feature_index, &feature_type);
+	if (ret)
+		return NULL;
+
+	ret = hidpp_devicenametype_get_count(hidpp, feature_index,
+		&__name_length);
+	if (ret)
+		return NULL;
+
+	name = kzalloc(__name_length + 1, GFP_KERNEL);
+	if (!name)
+		return NULL;
+
+	while (index < __name_length) {
+		ret = hidpp_devicenametype_get_device_name(hidpp,
+			feature_index, index, name + index,
+			__name_length - index);
+		if (ret <= 0) {
+			kfree(name);
+			return NULL;
+		}
+		index += ret;
+	}
+
+	return name;
+}
+
+/* -------------------------------------------------------------------------- */
+/* 0x6100: TouchPadRawXY                                                      */
+/* -------------------------------------------------------------------------- */
+
+#define HIDPP_PAGE_TOUCHPAD_RAW_XY			0x6100
+
+#define CMD_TOUCHPAD_GET_RAW_INFO			0x01
+#define CMD_TOUCHPAD_SET_RAW_REPORT_STATE		0x21
+
+#define EVENT_TOUCHPAD_RAW_XY				0x00
+
+#define TOUCHPAD_RAW_XY_ORIGIN_LOWER_LEFT		0x01
+#define TOUCHPAD_RAW_XY_ORIGIN_UPPER_LEFT		0x03
+
+struct hidpp_touchpad_raw_info {
+	u16 x_size;
+	u16 y_size;
+	u8 z_range;
+	u8 area_range;
+	u8 timestamp_unit;
+	u8 maxcontacts;
+	u8 origin;
+	u16 res;
+};
+
+struct hidpp_touchpad_raw_xy_finger {
+	u8 contact_type;
+	u8 contact_status;
+	u16 x;
+	u16 y;
+	u8 z;
+	u8 area;
+	u8 finger_id;
+};
+
+struct hidpp_touchpad_raw_xy {
+	u16 timestamp;
+	struct hidpp_touchpad_raw_xy_finger fingers[2];
+	u8 spurious_flag;
+	u8 end_of_frame;
+	u8 finger_count;
+	u8 button;
+};
+
+static int hidpp_touchpad_get_raw_info(struct hidpp_device *hidpp,
+	u8 feature_index, struct hidpp_touchpad_raw_info *raw_info)
+{
+	struct hidpp_report response;
+	int ret;
+	u8 *params = (u8 *)response.fap.params;
+
+	ret = hidpp_send_fap_command_sync(hidpp, feature_index,
+		CMD_TOUCHPAD_GET_RAW_INFO, NULL, 0, &response);
+
+	if (ret > 0) {
+		hid_err(hidpp->hid_dev, "%s: received protocol error 0x%02x\n",
+			__func__, ret);
+		return -EPROTO;
+	}
+	if (ret)
+		return ret;
+
+	raw_info->x_size = get_unaligned_be16(&params[0]);
+	raw_info->y_size = get_unaligned_be16(&params[2]);
+	raw_info->z_range = params[4];
+	raw_info->area_range = params[5];
+	raw_info->maxcontacts = params[7];
+	raw_info->origin = params[8];
+	/* res is given in unit per inch */
+	raw_info->res = get_unaligned_be16(&params[13]) * 2 / 51;
+
+	return ret;
+}
+
+static int hidpp_touchpad_set_raw_report_state(struct hidpp_device *hidpp_dev,
+		u8 feature_index, bool send_raw_reports,
+		bool sensor_enhanced_settings)
+{
+	struct hidpp_report response;
+
+	/*
+	 * Params:
+	 *   bit 0 - enable raw
+	 *   bit 1 - 16bit Z, no area
+	 *   bit 2 - enhanced sensitivity
+	 *   bit 3 - width, height (4 bits each) instead of area
+	 *   bit 4 - send raw + gestures (degrades smoothness)
+	 *   remaining bits - reserved
+	 */
+	u8 params = send_raw_reports | (sensor_enhanced_settings << 2);
+
+	return hidpp_send_fap_command_sync(hidpp_dev, feature_index,
+		CMD_TOUCHPAD_SET_RAW_REPORT_STATE, &params, 1, &response);
+}
+
+static void hidpp_touchpad_touch_event(u8 *data,
+	struct hidpp_touchpad_raw_xy_finger *finger)
+{
+	u8 x_m = data[0] << 2;
+	u8 y_m = data[2] << 2;
+
+	finger->x = x_m << 6 | data[1];
+	finger->y = y_m << 6 | data[3];
+
+	finger->contact_type = data[0] >> 6;
+	finger->contact_status = data[2] >> 6;
+
+	finger->z = data[4];
+	finger->area = data[5];
+	finger->finger_id = data[6] >> 4;
+}
+
+static void hidpp_touchpad_raw_xy_event(struct hidpp_device *hidpp_dev,
+		u8 *data, struct hidpp_touchpad_raw_xy *raw_xy)
+{
+	memset(raw_xy, 0, sizeof(struct hidpp_touchpad_raw_xy));
+	raw_xy->end_of_frame = data[8] & 0x01;
+	raw_xy->spurious_flag = (data[8] >> 1) & 0x01;
+	raw_xy->finger_count = data[15] & 0x0f;
+	raw_xy->button = (data[8] >> 2) & 0x01;
+
+	if (raw_xy->finger_count) {
+		hidpp_touchpad_touch_event(&data[2], &raw_xy->fingers[0]);
+		hidpp_touchpad_touch_event(&data[9], &raw_xy->fingers[1]);
+	}
+}
+
+/* ************************************************************************** */
+/*                                                                            */
+/* Device Support                                                             */
+/*                                                                            */
+/* ************************************************************************** */
+
+/* -------------------------------------------------------------------------- */
+/* Touchpad HID++ devices                                                     */
+/* -------------------------------------------------------------------------- */
+
+#define WTP_MANUAL_RESOLUTION				39
+
+struct wtp_data {
+	struct input_dev *input;
+	u16 x_size, y_size;
+	u8 finger_count;
+	u8 mt_feature_index;
+	u8 button_feature_index;
+	u8 maxcontacts;
+	bool flip_y;
+	unsigned int resolution;
+};
+
+static int wtp_input_mapping(struct hid_device *hdev, struct hid_input *hi,
+		struct hid_field *field, struct hid_usage *usage,
+		unsigned long **bit, int *max)
+{
+	struct hidpp_device *hidpp = hid_get_drvdata(hdev);
+
+	if ((hidpp->quirks & HIDPP_QUIRK_MULTI_INPUT) &&
+	    (field->application == HID_GD_KEYBOARD))
+		return 0;
+
+	return -1;
+}
+
+static void wtp_populate_input(struct hidpp_device *hidpp,
+		struct input_dev *input_dev, bool origin_is_hid_core)
+{
+	struct wtp_data *wd = hidpp->private_data;
+
+	if ((hidpp->quirks & HIDPP_QUIRK_MULTI_INPUT) && origin_is_hid_core)
+		/* this is the generic hid-input call */
+		return;
+
+	__set_bit(EV_ABS, input_dev->evbit);
+	__set_bit(EV_KEY, input_dev->evbit);
+	__clear_bit(EV_REL, input_dev->evbit);
+	__clear_bit(EV_LED, input_dev->evbit);
+
+	input_set_abs_params(input_dev, ABS_MT_POSITION_X, 0, wd->x_size, 0, 0);
+	input_abs_set_res(input_dev, ABS_MT_POSITION_X, wd->resolution);
+	input_set_abs_params(input_dev, ABS_MT_POSITION_Y, 0, wd->y_size, 0, 0);
+	input_abs_set_res(input_dev, ABS_MT_POSITION_Y, wd->resolution);
+
+	/* Max pressure is not given by the devices, pick one */
+	input_set_abs_params(input_dev, ABS_MT_PRESSURE, 0, 50, 0, 0);
+
+	input_set_capability(input_dev, EV_KEY, BTN_LEFT);
+
+	if (hidpp->quirks & HIDPP_QUIRK_WTP_PHYSICAL_BUTTONS)
+		input_set_capability(input_dev, EV_KEY, BTN_RIGHT);
+	else
+		__set_bit(INPUT_PROP_BUTTONPAD, input_dev->propbit);
+
+	input_mt_init_slots(input_dev, wd->maxcontacts, INPUT_MT_POINTER |
+		INPUT_MT_DROP_UNUSED);
+
+	wd->input = input_dev;
+}
+
+static void wtp_touch_event(struct wtp_data *wd,
+	struct hidpp_touchpad_raw_xy_finger *touch_report)
+{
+	int slot;
+
+	if (!touch_report->finger_id || touch_report->contact_type)
+		/* no actual data */
+		return;
+
+	slot = input_mt_get_slot_by_key(wd->input, touch_report->finger_id);
+
+	input_mt_slot(wd->input, slot);
+	input_mt_report_slot_state(wd->input, MT_TOOL_FINGER,
+					touch_report->contact_status);
+	if (touch_report->contact_status) {
+		input_event(wd->input, EV_ABS, ABS_MT_POSITION_X,
+				touch_report->x);
+		input_event(wd->input, EV_ABS, ABS_MT_POSITION_Y,
+				wd->flip_y ? wd->y_size - touch_report->y :
+					     touch_report->y);
+		input_event(wd->input, EV_ABS, ABS_MT_PRESSURE,
+				touch_report->area);
+	}
+}
+
+static void wtp_send_raw_xy_event(struct hidpp_device *hidpp,
+		struct hidpp_touchpad_raw_xy *raw)
+{
+	struct wtp_data *wd = hidpp->private_data;
+	int i;
+
+	for (i = 0; i < 2; i++)
+		wtp_touch_event(wd, &(raw->fingers[i]));
+
+	if (raw->end_of_frame &&
+	    !(hidpp->quirks & HIDPP_QUIRK_WTP_PHYSICAL_BUTTONS))
+		input_event(wd->input, EV_KEY, BTN_LEFT, raw->button);
+
+	if (raw->end_of_frame || raw->finger_count <= 2) {
+		input_mt_sync_frame(wd->input);
+		input_sync(wd->input);
+	}
+}
+
+static int wtp_mouse_raw_xy_event(struct hidpp_device *hidpp, u8 *data)
+{
+	struct wtp_data *wd = hidpp->private_data;
+	u8 c1_area = ((data[7] & 0xf) * (data[7] & 0xf) +
+		      (data[7] >> 4) * (data[7] >> 4)) / 2;
+	u8 c2_area = ((data[13] & 0xf) * (data[13] & 0xf) +
+		      (data[13] >> 4) * (data[13] >> 4)) / 2;
+	struct hidpp_touchpad_raw_xy raw = {
+		.timestamp = data[1],
+		.fingers = {
+			{
+				.contact_type = 0,
+				.contact_status = !!data[7],
+				.x = get_unaligned_le16(&data[3]),
+				.y = get_unaligned_le16(&data[5]),
+				.z = c1_area,
+				.area = c1_area,
+				.finger_id = data[2],
+			}, {
+				.contact_type = 0,
+				.contact_status = !!data[13],
+				.x = get_unaligned_le16(&data[9]),
+				.y = get_unaligned_le16(&data[11]),
+				.z = c2_area,
+				.area = c2_area,
+				.finger_id = data[8],
+			}
+		},
+		.finger_count = wd->maxcontacts,
+		.spurious_flag = 0,
+		.end_of_frame = (data[0] >> 7) == 0,
+		.button = data[0] & 0x01,
+	};
+
+	wtp_send_raw_xy_event(hidpp, &raw);
+
+	return 1;
+}
+
+static int wtp_raw_event(struct hid_device *hdev, u8 *data, int size)
+{
+	struct hidpp_device *hidpp = hid_get_drvdata(hdev);
+	struct wtp_data *wd = hidpp->private_data;
+	struct hidpp_report *report = (struct hidpp_report *)data;
+	struct hidpp_touchpad_raw_xy raw;
+
+	if (!wd || !wd->input)
+		return 1;
+
+	switch (data[0]) {
+	case 0x02:
+		if (hidpp->quirks & HIDPP_QUIRK_WTP_PHYSICAL_BUTTONS) {
+			input_event(wd->input, EV_KEY, BTN_LEFT,
+					!!(data[1] & 0x01));
+			input_event(wd->input, EV_KEY, BTN_RIGHT,
+					!!(data[1] & 0x02));
+			input_sync(wd->input);
+		} else {
+			if (size < 21)
+				return 1;
+			return wtp_mouse_raw_xy_event(hidpp, &data[7]);
+		}
+	case REPORT_ID_HIDPP_LONG:
+		if ((report->fap.feature_index != wd->mt_feature_index) ||
+		    (report->fap.funcindex_clientid != EVENT_TOUCHPAD_RAW_XY))
+			return 1;
+		hidpp_touchpad_raw_xy_event(hidpp, data + 4, &raw);
+
+		wtp_send_raw_xy_event(hidpp, &raw);
+		return 0;
+	}
+
+	return 0;
+}
+
+static int wtp_get_config(struct hidpp_device *hidpp)
+{
+	struct wtp_data *wd = hidpp->private_data;
+	struct hidpp_touchpad_raw_info raw_info = {0};
+	u8 feature_type;
+	int ret;
+
+	ret = hidpp_root_get_feature(hidpp, HIDPP_PAGE_TOUCHPAD_RAW_XY,
+		&wd->mt_feature_index, &feature_type);
+	if (ret)
+		/* means that the device is not powered up */
+		return ret;
+
+	ret = hidpp_touchpad_get_raw_info(hidpp, wd->mt_feature_index,
+		&raw_info);
+	if (ret)
+		return ret;
+
+	wd->x_size = raw_info.x_size;
+	wd->y_size = raw_info.y_size;
+	wd->maxcontacts = raw_info.maxcontacts;
+	wd->flip_y = raw_info.origin == TOUCHPAD_RAW_XY_ORIGIN_LOWER_LEFT;
+	wd->resolution = raw_info.res;
+	if (!wd->resolution)
+		wd->resolution = WTP_MANUAL_RESOLUTION;
+
+	return 0;
+}
+
+static int wtp_allocate(struct hid_device *hdev, const struct hid_device_id *id)
+{
+	struct hidpp_device *hidpp = hid_get_drvdata(hdev);
+	struct wtp_data *wd;
+
+	wd = devm_kzalloc(&hdev->dev, sizeof(struct wtp_data),
+			GFP_KERNEL);
+	if (!wd)
+		return -ENOMEM;
+
+	hidpp->private_data = wd;
+
+	return 0;
+};
+
+static void wtp_connect(struct hid_device *hdev, bool connected)
+{
+	struct hidpp_device *hidpp = hid_get_drvdata(hdev);
+	struct wtp_data *wd = hidpp->private_data;
+	int ret;
+
+	if (!connected)
+		return;
+
+	if (!wd->x_size) {
+		ret = wtp_get_config(hidpp);
+		if (ret) {
+			hid_err(hdev, "Can not get wtp config: %d\n", ret);
+			return;
+		}
+	}
+
+	hidpp_touchpad_set_raw_report_state(hidpp, wd->mt_feature_index,
+			true, true);
+}
+
+/* -------------------------------------------------------------------------- */
+/* Generic HID++ devices                                                      */
+/* -------------------------------------------------------------------------- */
+
+static int hidpp_input_mapping(struct hid_device *hdev, struct hid_input *hi,
+		struct hid_field *field, struct hid_usage *usage,
+		unsigned long **bit, int *max)
+{
+	struct hidpp_device *hidpp = hid_get_drvdata(hdev);
+
+	if (hidpp->quirks & HIDPP_QUIRK_CLASS_WTP)
+		return wtp_input_mapping(hdev, hi, field, usage, bit, max);
+
+	return 0;
+}
+
+static void hidpp_populate_input(struct hidpp_device *hidpp,
+		struct input_dev *input, bool origin_is_hid_core)
+{
+	if (hidpp->quirks & HIDPP_QUIRK_CLASS_WTP)
+		wtp_populate_input(hidpp, input, origin_is_hid_core);
+}
+
+static void hidpp_input_configured(struct hid_device *hdev,
+				struct hid_input *hidinput)
+{
+	struct hidpp_device *hidpp = hid_get_drvdata(hdev);
+	struct input_dev *input = hidinput->input;
+
+	hidpp_populate_input(hidpp, input, true);
+}
+
+static int hidpp_raw_hidpp_event(struct hidpp_device *hidpp, u8 *data,
+		int size)
+{
+	struct hidpp_report *question = hidpp->send_receive_buf;
+	struct hidpp_report *answer = hidpp->send_receive_buf;
+	struct hidpp_report *report = (struct hidpp_report *)data;
+
+	/*
+	 * If the mutex is locked then we have a pending answer from a
+	 * previoulsly sent command
+	 */
+	if (unlikely(mutex_is_locked(&hidpp->send_mutex))) {
+		/*
+		 * Check for a correct hidpp20 answer or the corresponding
+		 * error
+		 */
+		if (hidpp_match_answer(question, report) ||
+				hidpp_match_error(question, report)) {
+			*answer = *report;
+			hidpp->answer_available = true;
+			wake_up(&hidpp->wait);
+			/*
+			 * This was an answer to a command that this driver sent
+			 * We return 1 to hid-core to avoid forwarding the
+			 * command upstream as it has been treated by the driver
+			 */
+
+			return 1;
+		}
+	}
+
+	if (unlikely(hidpp_report_is_connect_event(report))) {
+		atomic_set(&hidpp->connected,
+				!(report->rap.params[0] & (1 << 6)));
+		if ((hidpp->quirks & HIDPP_QUIRK_DELAYED_INIT) &&
+		    (schedule_work(&hidpp->work) == 0))
+			dbg_hid("%s: connect event already queued\n", __func__);
+		return 1;
+	}
+
+	if (hidpp->quirks & HIDPP_QUIRK_CLASS_WTP)
+		return wtp_raw_event(hidpp->hid_dev, data, size);
+
+	return 0;
+}
+
+static int hidpp_raw_event(struct hid_device *hdev, struct hid_report *report,
+		u8 *data, int size)
+{
+	struct hidpp_device *hidpp = hid_get_drvdata(hdev);
+
+	switch (data[0]) {
+	case REPORT_ID_HIDPP_LONG:
+		if (size != HIDPP_REPORT_LONG_LENGTH) {
+			hid_err(hdev, "received hid++ report of bad size (%d)",
+				size);
+			return 1;
+		}
+		return hidpp_raw_hidpp_event(hidpp, data, size);
+	case REPORT_ID_HIDPP_SHORT:
+		if (size != HIDPP_REPORT_SHORT_LENGTH) {
+			hid_err(hdev, "received hid++ report of bad size (%d)",
+				size);
+			return 1;
+		}
+		return hidpp_raw_hidpp_event(hidpp, data, size);
+	}
+
+	if (hidpp->quirks & HIDPP_QUIRK_CLASS_WTP)
+		return wtp_raw_event(hdev, data, size);
+
+	return 0;
+}
+
+static void hidpp_overwrite_name(struct hid_device *hdev, bool use_unifying)
+{
+	struct hidpp_device *hidpp = hid_get_drvdata(hdev);
+	char *name;
+
+	if (use_unifying)
+		/*
+		 * the device is connected through an Unifying receiver, and
+		 * might not be already connected.
+		 * Ask the receiver for its name.
+		 */
+		name = hidpp_get_unifying_name(hidpp);
+	else
+		name = hidpp_get_device_name(hidpp);
+
+	if (!name)
+		hid_err(hdev, "unable to retrieve the name of the device");
+	else
+		snprintf(hdev->name, sizeof(hdev->name), "%s", name);
+
+	kfree(name);
+}
+
+static int hidpp_input_open(struct input_dev *dev)
+{
+	struct hid_device *hid = input_get_drvdata(dev);
+
+	return hid_hw_open(hid);
+}
+
+static void hidpp_input_close(struct input_dev *dev)
+{
+	struct hid_device *hid = input_get_drvdata(dev);
+
+	hid_hw_close(hid);
+}
+
+static struct input_dev *hidpp_allocate_input(struct hid_device *hdev)
+{
+	struct input_dev *input_dev = devm_input_allocate_device(&hdev->dev);
+
+	if (!input_dev)
+		return NULL;
+
+	input_set_drvdata(input_dev, hdev);
+	input_dev->open = hidpp_input_open;
+	input_dev->close = hidpp_input_close;
+
+	input_dev->name = hdev->name;
+	input_dev->phys = hdev->phys;
+	input_dev->uniq = hdev->uniq;
+	input_dev->id.bustype = hdev->bus;
+	input_dev->id.vendor  = hdev->vendor;
+	input_dev->id.product = hdev->product;
+	input_dev->id.version = hdev->version;
+	input_dev->dev.parent = &hdev->dev;
+
+	return input_dev;
+}
+
+static void hidpp_connect_event(struct hidpp_device *hidpp)
+{
+	struct hid_device *hdev = hidpp->hid_dev;
+	int ret = 0;
+	bool connected = atomic_read(&hidpp->connected);
+	struct input_dev *input;
+	char *name, *devm_name;
+
+	if (hidpp->quirks & HIDPP_QUIRK_CLASS_WTP)
+		wtp_connect(hdev, connected);
+
+	if (!connected || hidpp->delayed_input)
+		return;
+
+	if (!hidpp->protocol_major) {
+		ret = !hidpp_is_connected(hidpp);
+		if (ret) {
+			hid_err(hdev, "Can not get the protocol version.\n");
+			return;
+		}
+	}
+
+	/* the device is already connected, we can ask for its name and
+	 * protocol */
+	hid_info(hdev, "HID++ %u.%u device connected.\n",
+		 hidpp->protocol_major, hidpp->protocol_minor);
+
+	input = hidpp_allocate_input(hdev);
+	if (!input) {
+		hid_err(hdev, "cannot allocate new input device: %d\n", ret);
+		return;
+	}
+
+	name = hidpp_get_device_name(hidpp);
+	if (!name) {
+		hid_err(hdev, "unable to retrieve the name of the device");
+	} else {
+		devm_name = devm_kasprintf(&hdev->dev, GFP_KERNEL, "%s", name);
+		if (devm_name)
+			input->name = devm_name;
+		kfree(name);
+	}
+
+	hidpp_populate_input(hidpp, input, false);
+
+	ret = input_register_device(input);
+	if (ret)
+		input_free_device(input);
+
+	hidpp->delayed_input = input;
+}
+
+static int hidpp_probe(struct hid_device *hdev, const struct hid_device_id *id)
+{
+	struct hidpp_device *hidpp;
+	int ret;
+	bool connected;
+	unsigned int connect_mask = HID_CONNECT_DEFAULT;
+
+	hidpp = devm_kzalloc(&hdev->dev, sizeof(struct hidpp_device),
+			GFP_KERNEL);
+	if (!hidpp)
+		return -ENOMEM;
+
+	hidpp->hid_dev = hdev;
+	hid_set_drvdata(hdev, hidpp);
+
+	hidpp->quirks = id->driver_data;
+
+	if (hidpp->quirks & HIDPP_QUIRK_CLASS_WTP) {
+		ret = wtp_allocate(hdev, id);
+		if (ret)
+			goto wtp_allocate_fail;
+	}
+
+	INIT_WORK(&hidpp->work, delayed_work_cb);
+	mutex_init(&hidpp->send_mutex);
+	init_waitqueue_head(&hidpp->wait);
+
+	ret = hid_parse(hdev);
+	if (ret) {
+		hid_err(hdev, "%s:parse failed\n", __func__);
+		goto hid_parse_fail;
+	}
+
+	/* Allow incoming packets */
+	hid_device_io_start(hdev);
+
+	connected = hidpp_is_connected(hidpp);
+	if (id->group != HID_GROUP_LOGITECH_DJ_DEVICE) {
+		if (!connected) {
+			hid_err(hdev, "Device not connected");
+			hid_device_io_stop(hdev);
+			goto hid_parse_fail;
+		}
+
+		hid_info(hdev, "HID++ %u.%u device connected.\n",
+			 hidpp->protocol_major, hidpp->protocol_minor);
+	}
+
+	hidpp_overwrite_name(hdev, id->group == HID_GROUP_LOGITECH_DJ_DEVICE);
+	atomic_set(&hidpp->connected, connected);
+
+	if (connected && (hidpp->quirks & HIDPP_QUIRK_CLASS_WTP)) {
+		ret = wtp_get_config(hidpp);
+		if (ret)
+			goto hid_parse_fail;
+	}
+
+	/* Block incoming packets */
+	hid_device_io_stop(hdev);
+
+	if (hidpp->quirks & HIDPP_QUIRK_DELAYED_INIT)
+		connect_mask &= ~HID_CONNECT_HIDINPUT;
+
+	/* Re-enable hidinput for multi-input devices */
+	if (hidpp->quirks & HIDPP_QUIRK_MULTI_INPUT)
+		connect_mask |= HID_CONNECT_HIDINPUT;
+
+	ret = hid_hw_start(hdev, connect_mask);
+	if (ret) {
+		hid_err(hdev, "%s:hid_hw_start returned error\n", __func__);
+		goto hid_hw_start_fail;
+	}
+
+	if (hidpp->quirks & HIDPP_QUIRK_DELAYED_INIT) {
+		/* Allow incoming packets */
+		hid_device_io_start(hdev);
+
+		hidpp_connect_event(hidpp);
+	}
+
+	return ret;
+
+hid_hw_start_fail:
+hid_parse_fail:
+	cancel_work_sync(&hidpp->work);
+	mutex_destroy(&hidpp->send_mutex);
+wtp_allocate_fail:
+	hid_set_drvdata(hdev, NULL);
+	return ret;
+}
+
+static void hidpp_remove(struct hid_device *hdev)
+{
+	struct hidpp_device *hidpp = hid_get_drvdata(hdev);
+
+	cancel_work_sync(&hidpp->work);
+	mutex_destroy(&hidpp->send_mutex);
+	hid_hw_stop(hdev);
+}
+
+static const struct hid_device_id hidpp_devices[] = {
+	{ /* wireless touchpad */
+	  HID_DEVICE(BUS_USB, HID_GROUP_LOGITECH_DJ_DEVICE,
+		USB_VENDOR_ID_LOGITECH, 0x4011),
+	  .driver_data = HIDPP_QUIRK_CLASS_WTP | HIDPP_QUIRK_DELAYED_INIT |
+			 HIDPP_QUIRK_WTP_PHYSICAL_BUTTONS },
+	{ /* wireless touchpad T650 */
+	  HID_DEVICE(BUS_USB, HID_GROUP_LOGITECH_DJ_DEVICE,
+		USB_VENDOR_ID_LOGITECH, 0x4101),
+	  .driver_data = HIDPP_QUIRK_CLASS_WTP | HIDPP_QUIRK_DELAYED_INIT },
+	{ /* wireless touchpad T651 */
+	  HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_LOGITECH,
+		USB_DEVICE_ID_LOGITECH_T651),
+	  .driver_data = HIDPP_QUIRK_CLASS_WTP },
+	{ /* Keyboard TK820 */
+	  HID_DEVICE(BUS_USB, HID_GROUP_LOGITECH_DJ_DEVICE,
+		USB_VENDOR_ID_LOGITECH, 0x4102),
+	  .driver_data = HIDPP_QUIRK_DELAYED_INIT | HIDPP_QUIRK_MULTI_INPUT |
+			 HIDPP_QUIRK_CLASS_WTP },
+
+	{ HID_DEVICE(BUS_USB, HID_GROUP_LOGITECH_DJ_DEVICE,
+		USB_VENDOR_ID_LOGITECH, HID_ANY_ID)},
+	{}
+};
+
+MODULE_DEVICE_TABLE(hid, hidpp_devices);
+
+static struct hid_driver hidpp_driver = {
+	.name = "logitech-hidpp-device",
+	.id_table = hidpp_devices,
+	.probe = hidpp_probe,
+	.remove = hidpp_remove,
+	.raw_event = hidpp_raw_event,
+	.input_configured = hidpp_input_configured,
+	.input_mapping = hidpp_input_mapping,
+};
+
+module_hid_driver(hidpp_driver);
diff --git a/drivers/hid/hid-microsoft.c b/drivers/hid/hid-microsoft.c
index 8ba17a946f2a..cacda43f6a6f 100644
--- a/drivers/hid/hid-microsoft.c
+++ b/drivers/hid/hid-microsoft.c
@@ -274,6 +274,8 @@ static const struct hid_device_id ms_devices[] = {
 		.driver_data = MS_NOGET },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_COMFORT_MOUSE_4500),
 		.driver_data = MS_DUPLICATE_USAGES },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_3),
+		.driver_data = MS_HIDINPUT },
 
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_PRESENTER_8K_BT),
 		.driver_data = MS_PRESENTER },
diff --git a/drivers/hid/hid-multitouch.c b/drivers/hid/hid-multitouch.c
index 51e25b9407f2..f65e78b46999 100644
--- a/drivers/hid/hid-multitouch.c
+++ b/drivers/hid/hid-multitouch.c
@@ -67,6 +67,7 @@ MODULE_LICENSE("GPL");
 #define MT_QUIRK_IGNORE_DUPLICATES	(1 << 10)
 #define MT_QUIRK_HOVERING		(1 << 11)
 #define MT_QUIRK_CONTACT_CNT_ACCURATE	(1 << 12)
+#define MT_QUIRK_FORCE_GET_FEATURE	(1 << 13)
 
 #define MT_INPUTMODE_TOUCHSCREEN	0x02
 #define MT_INPUTMODE_TOUCHPAD		0x03
@@ -150,6 +151,7 @@ static void mt_post_parse(struct mt_device *td);
 #define MT_CLS_FLATFROG				0x0107
 #define MT_CLS_GENERALTOUCH_TWOFINGERS		0x0108
 #define MT_CLS_GENERALTOUCH_PWT_TENFINGERS	0x0109
+#define MT_CLS_VTL				0x0110
 
 #define MT_DEFAULT_MAXCONTACT	10
 #define MT_MAX_MAXCONTACT	250
@@ -255,6 +257,11 @@ static struct mt_class mt_classes[] = {
 		.sn_move = 2048,
 		.maxcontacts = 40,
 	},
+	{ .name = MT_CLS_VTL,
+		.quirks = MT_QUIRK_ALWAYS_VALID |
+			MT_QUIRK_CONTACT_CNT_ACCURATE |
+			MT_QUIRK_FORCE_GET_FEATURE,
+	},
 	{ }
 };
 
@@ -809,6 +816,9 @@ static void mt_set_input_mode(struct hid_device *hdev)
 	struct mt_device *td = hid_get_drvdata(hdev);
 	struct hid_report *r;
 	struct hid_report_enum *re;
+	struct mt_class *cls = &td->mtclass;
+	char *buf;
+	int report_len;
 
 	if (td->inputmode < 0)
 		return;
@@ -816,6 +826,18 @@ static void mt_set_input_mode(struct hid_device *hdev)
 	re = &(hdev->report_enum[HID_FEATURE_REPORT]);
 	r = re->report_id_hash[td->inputmode];
 	if (r) {
+		if (cls->quirks & MT_QUIRK_FORCE_GET_FEATURE) {
+			report_len = hid_report_len(r);
+			buf = hid_alloc_report_buf(r, GFP_KERNEL);
+			if (!buf) {
+				hid_err(hdev, "failed to allocate buffer for report\n");
+				return;
+			}
+			hid_hw_raw_request(hdev, r->id, buf, report_len,
+					   HID_FEATURE_REPORT,
+					   HID_REQ_GET_REPORT);
+			kfree(buf);
+		}
 		r->field[0]->value[td->inputmode_index] = td->inputmode_value;
 		hid_hw_request(hdev, r, HID_REQ_SET_REPORT);
 	}
@@ -1281,6 +1303,11 @@ static const struct hid_device_id mt_devices[] = {
 		MT_USB_DEVICE(USB_VENDOR_ID_UNITEC,
 			USB_DEVICE_ID_UNITEC_USB_TOUCH_0A19) },
 
+	/* VTL panels */
+	{ .driver_data = MT_CLS_VTL,
+		MT_USB_DEVICE(USB_VENDOR_ID_VTL,
+			USB_DEVICE_ID_VTL_MULTITOUCH_FF3F) },
+
 	/* Wistron panels */
 	{ .driver_data = MT_CLS_NSMU,
 		MT_USB_DEVICE(USB_VENDOR_ID_WISTRON,
diff --git a/drivers/hid/hid-plantronics.c b/drivers/hid/hid-plantronics.c
new file mode 100644
index 000000000000..2180e0789b76
--- /dev/null
+++ b/drivers/hid/hid-plantronics.c
@@ -0,0 +1,55 @@
+/*
+ *  Plantronics USB HID Driver
+ *
+ *  Copyright (c) 2014 JD Cole <jd.cole@plantronics.com>
+ *  Copyright (c) 2014 Terry Junge <terry.junge@plantronics.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 "hid-ids.h"
+
+#include <linux/hid.h>
+#include <linux/module.h>
+
+static int plantronics_input_mapping(struct hid_device *hdev,
+				     struct hid_input *hi,
+				     struct hid_field *field,
+				     struct hid_usage *usage,
+				     unsigned long **bit, int *max)
+{
+	if (field->application == HID_CP_CONSUMERCONTROL
+	    && (usage->hid & HID_USAGE_PAGE) == HID_UP_CONSUMER) {
+		hid_dbg(hdev, "usage: %08x (appl: %08x) - defaulted\n",
+			 usage->hid, field->application);
+		return 0;
+	}
+
+	hid_dbg(hdev, "usage: %08x (appl: %08x) - ignored\n",
+		usage->hid, field->application);
+
+	return -1;
+}
+
+static const struct hid_device_id plantronics_devices[] = {
+	{ HID_USB_DEVICE(USB_VENDOR_ID_PLANTRONICS, HID_ANY_ID) },
+	{ }
+};
+MODULE_DEVICE_TABLE(hid, plantronics_devices);
+
+static struct hid_driver plantronics_driver = {
+	.name = "plantronics",
+	.id_table = plantronics_devices,
+	.input_mapping = plantronics_input_mapping,
+};
+module_hid_driver(plantronics_driver);
+
+MODULE_AUTHOR("JD Cole <jd.cole@plantronics.com>");
+MODULE_AUTHOR("Terry Junge <terry.junge@plantronics.com>");
+MODULE_DESCRIPTION("Plantronics USB HID Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hid/hid-rmi.c b/drivers/hid/hid-rmi.c
index 3cccff73b9b9..b51200fe2f33 100644
--- a/drivers/hid/hid-rmi.c
+++ b/drivers/hid/hid-rmi.c
@@ -584,11 +584,15 @@ static int rmi_populate_f11(struct hid_device *hdev)
 	bool has_query10 = false;
 	bool has_query11;
 	bool has_query12;
+	bool has_query27;
+	bool has_query28;
+	bool has_query36 = false;
 	bool has_physical_props;
 	bool has_gestures;
 	bool has_rel;
+	bool has_data40 = false;
 	unsigned x_size, y_size;
-	u16 query12_offset;
+	u16 query_offset;
 
 	if (!data->f11.query_base_addr) {
 		hid_err(hdev, "No 2D sensor found, giving up.\n");
@@ -604,6 +608,8 @@ static int rmi_populate_f11(struct hid_device *hdev)
 	has_query9 = !!(buf[0] & BIT(3));
 	has_query11 = !!(buf[0] & BIT(4));
 	has_query12 = !!(buf[0] & BIT(5));
+	has_query27 = !!(buf[0] & BIT(6));
+	has_query28 = !!(buf[0] & BIT(7));
 
 	/* query 1 to get the max number of fingers */
 	ret = rmi_read(hdev, data->f11.query_base_addr + 1, buf);
@@ -626,43 +632,43 @@ static int rmi_populate_f11(struct hid_device *hdev)
 	has_rel = !!(buf[0] & BIT(3));
 	has_gestures = !!(buf[0] & BIT(5));
 
+	/*
+	 * At least 4 queries are guaranteed to be present in F11
+	 * +1 for query 5 which is present since absolute events are
+	 * reported and +1 for query 12.
+	 */
+	query_offset = 6;
+
+	if (has_rel)
+		++query_offset; /* query 6 is present */
+
 	if (has_gestures) {
 		/* query 8 to find out if query 10 exists */
-		ret = rmi_read(hdev, data->f11.query_base_addr + 8, buf);
+		ret = rmi_read(hdev,
+			data->f11.query_base_addr + query_offset + 1, buf);
 		if (ret) {
 			hid_err(hdev, "can not read gesture information: %d.\n",
 				ret);
 			return ret;
 		}
 		has_query10 = !!(buf[0] & BIT(2));
-	}
 
-	/*
-	 * At least 4 queries are guaranteed to be present in F11
-	 * +1 for query 5 which is present since absolute events are
-	 * reported and +1 for query 12.
-	 */
-	query12_offset = 6;
-
-	if (has_rel)
-		++query12_offset; /* query 6 is present */
-
-	if (has_gestures)
-		query12_offset += 2; /* query 7 and 8 are present */
+		query_offset += 2; /* query 7 and 8 are present */
+	}
 
 	if (has_query9)
-		++query12_offset;
+		++query_offset;
 
 	if (has_query10)
-		++query12_offset;
+		++query_offset;
 
 	if (has_query11)
-		++query12_offset;
+		++query_offset;
 
 	/* query 12 to know if the physical properties are reported */
 	if (has_query12) {
 		ret = rmi_read(hdev, data->f11.query_base_addr
-				+ query12_offset, buf);
+				+ query_offset, buf);
 		if (ret) {
 			hid_err(hdev, "can not get query 12: %d.\n", ret);
 			return ret;
@@ -670,9 +676,10 @@ static int rmi_populate_f11(struct hid_device *hdev)
 		has_physical_props = !!(buf[0] & BIT(5));
 
 		if (has_physical_props) {
+			query_offset += 1;
 			ret = rmi_read_block(hdev,
 					data->f11.query_base_addr
-						+ query12_offset + 1, buf, 4);
+						+ query_offset, buf, 4);
 			if (ret) {
 				hid_err(hdev, "can not read query 15-18: %d.\n",
 					ret);
@@ -687,9 +694,45 @@ static int rmi_populate_f11(struct hid_device *hdev)
 
 			hid_info(hdev, "%s: size in mm: %d x %d\n",
 				 __func__, data->x_size_mm, data->y_size_mm);
+
+			/*
+			 * query 15 - 18 contain the size of the sensor
+			 * and query 19 - 26 contain bezel dimensions
+			 */
+			query_offset += 12;
+		}
+	}
+
+	if (has_query27)
+		++query_offset;
+
+	if (has_query28) {
+		ret = rmi_read(hdev, data->f11.query_base_addr
+				+ query_offset, buf);
+		if (ret) {
+			hid_err(hdev, "can not get query 28: %d.\n", ret);
+			return ret;
+		}
+
+		has_query36 = !!(buf[0] & BIT(6));
+	}
+
+	if (has_query36) {
+		query_offset += 2;
+		ret = rmi_read(hdev, data->f11.query_base_addr
+				+ query_offset, buf);
+		if (ret) {
+			hid_err(hdev, "can not get query 36: %d.\n", ret);
+			return ret;
 		}
+
+		has_data40 = !!(buf[0] & BIT(5));
 	}
 
+
+	if (has_data40)
+		data->f11.report_size += data->max_fingers * 2;
+
 	/*
 	 * retrieve the ctrl registers
 	 * the ctrl register has a size of 20 but a fw bug split it into 16 + 4,
diff --git a/drivers/hid/hid-roccat-kone.c b/drivers/hid/hid-roccat-kone.c
index 6101816a7ddd..c29265055ac1 100644
--- a/drivers/hid/hid-roccat-kone.c
+++ b/drivers/hid/hid-roccat-kone.c
@@ -46,6 +46,7 @@ static void kone_profile_activated(struct kone_device *kone, uint new_profile)
 static void kone_profile_report(struct kone_device *kone, uint new_profile)
 {
 	struct kone_roccat_report roccat_report;
+
 	roccat_report.event = kone_mouse_event_switch_profile;
 	roccat_report.value = new_profile;
 	roccat_report.key = 0;
@@ -163,6 +164,7 @@ static int kone_set_settings(struct usb_device *usb_dev,
 		struct kone_settings const *settings)
 {
 	int retval;
+
 	retval = kone_send(usb_dev, kone_command_settings,
 			settings, sizeof(struct kone_settings));
 	if (retval)
@@ -387,7 +389,7 @@ static struct bin_attribute bin_attr_profile##number = {	\
 	.read = kone_sysfs_read_profilex,			\
 	.write = kone_sysfs_write_profilex,			\
 	.private = &profile_numbers[number-1],			\
-};
+}
 PROFILE_ATTR(1);
 PROFILE_ATTR(2);
 PROFILE_ATTR(3);
@@ -456,6 +458,7 @@ static ssize_t kone_sysfs_show_tcu(struct device *dev,
 static int kone_tcu_command(struct usb_device *usb_dev, int number)
 {
 	unsigned char value;
+
 	value = number;
 	return kone_send(usb_dev, kone_command_calibrate, &value, 1);
 }
@@ -697,10 +700,8 @@ static int kone_init_specials(struct hid_device *hdev)
 			== USB_INTERFACE_PROTOCOL_MOUSE) {
 
 		kone = kzalloc(sizeof(*kone), GFP_KERNEL);
-		if (!kone) {
-			hid_err(hdev, "can't alloc device descriptor\n");
+		if (!kone)
 			return -ENOMEM;
-		}
 		hid_set_drvdata(hdev, kone);
 
 		retval = kone_init_kone_device_struct(usb_dev, kone);
diff --git a/drivers/hid/hid-saitek.c b/drivers/hid/hid-saitek.c
index 69cca1476a0c..5632c54eadf0 100644
--- a/drivers/hid/hid-saitek.c
+++ b/drivers/hid/hid-saitek.c
@@ -7,7 +7,7 @@
  *  (This module is based on "hid-ortek".)
  *  Copyright (c) 2012 Andreas Hübner
  *
- *  R.A.T.7, M.M.O.7 (USB gaming mice):
+ *  R.A.T.7, R.A.T.9, M.M.O.7 (USB gaming mice):
  *  Fixes the mode button which cycles through three constantly pressed
  *  buttons. All three press events are mapped to one button and the
  *  missing release event is generated immediately.
@@ -179,6 +179,8 @@ static const struct hid_device_id saitek_devices[] = {
 		.driver_data = SAITEK_FIX_PS1000 },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_RAT7),
 		.driver_data = SAITEK_RELEASE_MODE_RAT7 },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_MADCATZ, USB_DEVICE_ID_MADCATZ_RAT9),
+		.driver_data = SAITEK_RELEASE_MODE_RAT7 },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_MMO7),
 		.driver_data = SAITEK_RELEASE_MODE_MMO7 },
 	{ }
diff --git a/drivers/hid/hid-sony.c b/drivers/hid/hid-sony.c
index bc4269e559f1..31e9d2561106 100644
--- a/drivers/hid/hid-sony.c
+++ b/drivers/hid/hid-sony.c
@@ -798,6 +798,12 @@ union sixaxis_output_report_01 {
 	__u8 buf[36];
 };
 
+#define DS4_REPORT_0x02_SIZE 37
+#define DS4_REPORT_0x05_SIZE 32
+#define DS4_REPORT_0x11_SIZE 78
+#define DS4_REPORT_0x81_SIZE 7
+#define SIXAXIS_REPORT_0xF2_SIZE 18
+
 static spinlock_t sony_dev_list_lock;
 static LIST_HEAD(sony_device_list);
 static DEFINE_IDA(sony_device_id_allocator);
@@ -811,6 +817,7 @@ struct sony_sc {
 	struct work_struct state_worker;
 	struct power_supply battery;
 	int device_id;
+	__u8 *output_report_dmabuf;
 
 #ifdef CONFIG_SONY_FF
 	__u8 left;
@@ -1142,9 +1149,20 @@ static int sixaxis_set_operational_usb(struct hid_device *hdev)
 
 static int sixaxis_set_operational_bt(struct hid_device *hdev)
 {
-	unsigned char buf[] = { 0xf4,  0x42, 0x03, 0x00, 0x00 };
-	return hid_hw_raw_request(hdev, buf[0], buf, sizeof(buf),
+	static const __u8 report[] = { 0xf4, 0x42, 0x03, 0x00, 0x00 };
+	__u8 *buf;
+	int ret;
+
+	buf = kmemdup(report, sizeof(report), GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	ret = hid_hw_raw_request(hdev, buf[0], buf, sizeof(report),
 				  HID_FEATURE_REPORT, HID_REQ_SET_REPORT);
+
+	kfree(buf);
+
+	return ret;
 }
 
 /*
@@ -1153,10 +1171,19 @@ static int sixaxis_set_operational_bt(struct hid_device *hdev)
  */
 static int dualshock4_set_operational_bt(struct hid_device *hdev)
 {
-	__u8 buf[37] = { 0 };
+	__u8 *buf;
+	int ret;
+
+	buf = kmalloc(DS4_REPORT_0x02_SIZE, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
 
-	return hid_hw_raw_request(hdev, 0x02, buf, sizeof(buf),
+	ret = hid_hw_raw_request(hdev, 0x02, buf, DS4_REPORT_0x02_SIZE,
 				HID_FEATURE_REPORT, HID_REQ_GET_REPORT);
+
+	kfree(buf);
+
+	return ret;
 }
 
 static void sixaxis_set_leds_from_id(int id, __u8 values[MAX_LEDS])
@@ -1471,9 +1498,7 @@ error_leds:
 
 static void sixaxis_state_worker(struct work_struct *work)
 {
-	struct sony_sc *sc = container_of(work, struct sony_sc, state_worker);
-	int n;
-	union sixaxis_output_report_01 report = {
+	static const union sixaxis_output_report_01 default_report = {
 		.buf = {
 			0x01,
 			0x00, 0xff, 0x00, 0xff, 0x00,
@@ -1485,20 +1510,27 @@ static void sixaxis_state_worker(struct work_struct *work)
 			0x00, 0x00, 0x00, 0x00, 0x00
 		}
 	};
+	struct sony_sc *sc = container_of(work, struct sony_sc, state_worker);
+	struct sixaxis_output_report *report =
+		(struct sixaxis_output_report *)sc->output_report_dmabuf;
+	int n;
+
+	/* Initialize the report with default values */
+	memcpy(report, &default_report, sizeof(struct sixaxis_output_report));
 
 #ifdef CONFIG_SONY_FF
-	report.data.rumble.right_motor_on = sc->right ? 1 : 0;
-	report.data.rumble.left_motor_force = sc->left;
+	report->rumble.right_motor_on = sc->right ? 1 : 0;
+	report->rumble.left_motor_force = sc->left;
 #endif
 
-	report.data.leds_bitmap |= sc->led_state[0] << 1;
-	report.data.leds_bitmap |= sc->led_state[1] << 2;
-	report.data.leds_bitmap |= sc->led_state[2] << 3;
-	report.data.leds_bitmap |= sc->led_state[3] << 4;
+	report->leds_bitmap |= sc->led_state[0] << 1;
+	report->leds_bitmap |= sc->led_state[1] << 2;
+	report->leds_bitmap |= sc->led_state[2] << 3;
+	report->leds_bitmap |= sc->led_state[3] << 4;
 
 	/* Set flag for all leds off, required for 3rd party INTEC controller */
-	if ((report.data.leds_bitmap & 0x1E) == 0)
-		report.data.leds_bitmap |= 0x20;
+	if ((report->leds_bitmap & 0x1E) == 0)
+		report->leds_bitmap |= 0x20;
 
 	/*
 	 * The LEDs in the report are indexed in reverse order to their
@@ -1511,28 +1543,30 @@ static void sixaxis_state_worker(struct work_struct *work)
 	 */
 	for (n = 0; n < 4; n++) {
 		if (sc->led_delay_on[n] || sc->led_delay_off[n]) {
-			report.data.led[3 - n].duty_off = sc->led_delay_off[n];
-			report.data.led[3 - n].duty_on = sc->led_delay_on[n];
+			report->led[3 - n].duty_off = sc->led_delay_off[n];
+			report->led[3 - n].duty_on = sc->led_delay_on[n];
 		}
 	}
 
-	hid_hw_raw_request(sc->hdev, report.data.report_id, report.buf,
-			sizeof(report), HID_OUTPUT_REPORT, HID_REQ_SET_REPORT);
+	hid_hw_raw_request(sc->hdev, report->report_id, (__u8 *)report,
+			sizeof(struct sixaxis_output_report),
+			HID_OUTPUT_REPORT, HID_REQ_SET_REPORT);
 }
 
 static void dualshock4_state_worker(struct work_struct *work)
 {
 	struct sony_sc *sc = container_of(work, struct sony_sc, state_worker);
 	struct hid_device *hdev = sc->hdev;
+	__u8 *buf = sc->output_report_dmabuf;
 	int offset;
 
-	__u8 buf[78] = { 0 };
-
 	if (sc->quirks & DUALSHOCK4_CONTROLLER_USB) {
+		memset(buf, 0, DS4_REPORT_0x05_SIZE);
 		buf[0] = 0x05;
 		buf[1] = 0xFF;
 		offset = 4;
 	} else {
+		memset(buf, 0, DS4_REPORT_0x11_SIZE);
 		buf[0] = 0x11;
 		buf[1] = 0xB0;
 		buf[3] = 0x0F;
@@ -1560,12 +1594,33 @@ static void dualshock4_state_worker(struct work_struct *work)
 	buf[offset++] = sc->led_delay_off[3];
 
 	if (sc->quirks & DUALSHOCK4_CONTROLLER_USB)
-		hid_hw_output_report(hdev, buf, 32);
+		hid_hw_output_report(hdev, buf, DS4_REPORT_0x05_SIZE);
 	else
-		hid_hw_raw_request(hdev, 0x11, buf, 78,
+		hid_hw_raw_request(hdev, 0x11, buf, DS4_REPORT_0x11_SIZE,
 				HID_OUTPUT_REPORT, HID_REQ_SET_REPORT);
 }
 
+static int sony_allocate_output_report(struct sony_sc *sc)
+{
+	if (sc->quirks & SIXAXIS_CONTROLLER)
+		sc->output_report_dmabuf =
+			kmalloc(sizeof(union sixaxis_output_report_01),
+				GFP_KERNEL);
+	else if (sc->quirks & DUALSHOCK4_CONTROLLER_BT)
+		sc->output_report_dmabuf = kmalloc(DS4_REPORT_0x11_SIZE,
+						GFP_KERNEL);
+	else if (sc->quirks & DUALSHOCK4_CONTROLLER_USB)
+		sc->output_report_dmabuf = kmalloc(DS4_REPORT_0x05_SIZE,
+						GFP_KERNEL);
+	else
+		return 0;
+
+	if (!sc->output_report_dmabuf)
+		return -ENOMEM;
+
+	return 0;
+}
+
 #ifdef CONFIG_SONY_FF
 static int sony_play_effect(struct input_dev *dev, void *data,
 			    struct ff_effect *effect)
@@ -1754,6 +1809,7 @@ static int sony_get_bt_devaddr(struct sony_sc *sc)
 
 static int sony_check_add(struct sony_sc *sc)
 {
+	__u8 *buf = NULL;
 	int n, ret;
 
 	if ((sc->quirks & DUALSHOCK4_CONTROLLER_BT) ||
@@ -1769,36 +1825,44 @@ static int sony_check_add(struct sony_sc *sc)
 			return 0;
 		}
 	} else if (sc->quirks & DUALSHOCK4_CONTROLLER_USB) {
-		__u8 buf[7];
+		buf = kmalloc(DS4_REPORT_0x81_SIZE, GFP_KERNEL);
+		if (!buf)
+			return -ENOMEM;
 
 		/*
 		 * The MAC address of a DS4 controller connected via USB can be
 		 * retrieved with feature report 0x81. The address begins at
 		 * offset 1.
 		 */
-		ret = hid_hw_raw_request(sc->hdev, 0x81, buf, sizeof(buf),
-				HID_FEATURE_REPORT, HID_REQ_GET_REPORT);
+		ret = hid_hw_raw_request(sc->hdev, 0x81, buf,
+				DS4_REPORT_0x81_SIZE, HID_FEATURE_REPORT,
+				HID_REQ_GET_REPORT);
 
-		if (ret != 7) {
+		if (ret != DS4_REPORT_0x81_SIZE) {
 			hid_err(sc->hdev, "failed to retrieve feature report 0x81 with the DualShock 4 MAC address\n");
-			return ret < 0 ? ret : -EINVAL;
+			ret = ret < 0 ? ret : -EINVAL;
+			goto out_free;
 		}
 
 		memcpy(sc->mac_address, &buf[1], sizeof(sc->mac_address));
 	} else if (sc->quirks & SIXAXIS_CONTROLLER_USB) {
-		__u8 buf[18];
+		buf = kmalloc(SIXAXIS_REPORT_0xF2_SIZE, GFP_KERNEL);
+		if (!buf)
+			return -ENOMEM;
 
 		/*
 		 * The MAC address of a Sixaxis controller connected via USB can
 		 * be retrieved with feature report 0xf2. The address begins at
 		 * offset 4.
 		 */
-		ret = hid_hw_raw_request(sc->hdev, 0xf2, buf, sizeof(buf),
-				HID_FEATURE_REPORT, HID_REQ_GET_REPORT);
+		ret = hid_hw_raw_request(sc->hdev, 0xf2, buf,
+				SIXAXIS_REPORT_0xF2_SIZE, HID_FEATURE_REPORT,
+				HID_REQ_GET_REPORT);
 
-		if (ret != 18) {
+		if (ret != SIXAXIS_REPORT_0xF2_SIZE) {
 			hid_err(sc->hdev, "failed to retrieve feature report 0xf2 with the Sixaxis MAC address\n");
-			return ret < 0 ? ret : -EINVAL;
+			ret = ret < 0 ? ret : -EINVAL;
+			goto out_free;
 		}
 
 		/*
@@ -1811,7 +1875,13 @@ static int sony_check_add(struct sony_sc *sc)
 		return 0;
 	}
 
-	return sony_check_add_dev_list(sc);
+	ret = sony_check_add_dev_list(sc);
+
+out_free:
+
+	kfree(buf);
+
+	return ret;
 }
 
 static int sony_set_device_id(struct sony_sc *sc)
@@ -1895,6 +1965,12 @@ static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		return ret;
 	}
 
+	ret = sony_allocate_output_report(sc);
+	if (ret < 0) {
+		hid_err(hdev, "failed to allocate the output report buffer\n");
+		goto err_stop;
+	}
+
 	ret = sony_set_device_id(sc);
 	if (ret < 0) {
 		hid_err(hdev, "failed to allocate the device id\n");
@@ -1984,6 +2060,7 @@ err_stop:
 	if (sc->quirks & SONY_BATTERY_SUPPORT)
 		sony_battery_remove(sc);
 	sony_cancel_work_sync(sc);
+	kfree(sc->output_report_dmabuf);
 	sony_remove_dev_list(sc);
 	sony_release_device_id(sc);
 	hid_hw_stop(hdev);
@@ -2004,6 +2081,8 @@ static void sony_remove(struct hid_device *hdev)
 
 	sony_cancel_work_sync(sc);
 
+	kfree(sc->output_report_dmabuf);
+
 	sony_remove_dev_list(sc);
 
 	sony_release_device_id(sc);
@@ -2034,6 +2113,9 @@ static const struct hid_device_id sony_devices[] = {
 	/* Logitech Harmony Adapter for PS3 */
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_HARMONY_PS3),
 		.driver_data = PS3REMOTE },
+	/* SMK-Link PS3 BD Remote Control */
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_SMK, USB_DEVICE_ID_SMK_PS3_BDREMOTE),
+		.driver_data = PS3REMOTE },
 	/* Sony Dualshock 4 controllers for PS4 */
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_PS4_CONTROLLER),
 		.driver_data = DUALSHOCK4_CONTROLLER_USB },
diff --git a/drivers/hid/i2c-hid/i2c-hid.c b/drivers/hid/i2c-hid/i2c-hid.c
index f09e70cafaf1..d32037cbf9db 100644
--- a/drivers/hid/i2c-hid/i2c-hid.c
+++ b/drivers/hid/i2c-hid/i2c-hid.c
@@ -137,6 +137,7 @@ struct i2c_hid {
 						   * descriptor. */
 	unsigned int		bufsize;	/* i2c buffer size */
 	char			*inbuf;		/* Input buffer */
+	char			*rawbuf;	/* Raw Input buffer */
 	char			*cmdbuf;	/* Command buffer */
 	char			*argsbuf;	/* Command arguments buffer */
 
@@ -369,7 +370,7 @@ static int i2c_hid_hwreset(struct i2c_client *client)
 static void i2c_hid_get_input(struct i2c_hid *ihid)
 {
 	int ret, ret_size;
-	int size = le16_to_cpu(ihid->hdesc.wMaxInputLength);
+	int size = ihid->bufsize;
 
 	ret = i2c_master_recv(ihid->client, ihid->inbuf, size);
 	if (ret != size) {
@@ -437,7 +438,7 @@ static void i2c_hid_init_report(struct hid_report *report, u8 *buffer,
 			report->id, buffer, size))
 		return;
 
-	i2c_hid_dbg(ihid, "report (len=%d): %*ph\n", size, size, ihid->inbuf);
+	i2c_hid_dbg(ihid, "report (len=%d): %*ph\n", size, size, buffer);
 
 	ret_size = buffer[0] | (buffer[1] << 8);
 
@@ -504,9 +505,11 @@ static void i2c_hid_find_max_report(struct hid_device *hid, unsigned int type,
 static void i2c_hid_free_buffers(struct i2c_hid *ihid)
 {
 	kfree(ihid->inbuf);
+	kfree(ihid->rawbuf);
 	kfree(ihid->argsbuf);
 	kfree(ihid->cmdbuf);
 	ihid->inbuf = NULL;
+	ihid->rawbuf = NULL;
 	ihid->cmdbuf = NULL;
 	ihid->argsbuf = NULL;
 	ihid->bufsize = 0;
@@ -522,10 +525,11 @@ static int i2c_hid_alloc_buffers(struct i2c_hid *ihid, size_t report_size)
 		       report_size; /* report */
 
 	ihid->inbuf = kzalloc(report_size, GFP_KERNEL);
+	ihid->rawbuf = kzalloc(report_size, GFP_KERNEL);
 	ihid->argsbuf = kzalloc(args_len, GFP_KERNEL);
 	ihid->cmdbuf = kzalloc(sizeof(union command) + args_len, GFP_KERNEL);
 
-	if (!ihid->inbuf || !ihid->argsbuf || !ihid->cmdbuf) {
+	if (!ihid->inbuf || !ihid->rawbuf || !ihid->argsbuf || !ihid->cmdbuf) {
 		i2c_hid_free_buffers(ihid);
 		return -ENOMEM;
 	}
@@ -552,12 +556,12 @@ static int i2c_hid_get_raw_report(struct hid_device *hid,
 
 	ret = i2c_hid_get_report(client,
 			report_type == HID_FEATURE_REPORT ? 0x03 : 0x01,
-			report_number, ihid->inbuf, ask_count);
+			report_number, ihid->rawbuf, ask_count);
 
 	if (ret < 0)
 		return ret;
 
-	ret_count = ihid->inbuf[0] | (ihid->inbuf[1] << 8);
+	ret_count = ihid->rawbuf[0] | (ihid->rawbuf[1] << 8);
 
 	if (ret_count <= 2)
 		return 0;
@@ -566,7 +570,7 @@ static int i2c_hid_get_raw_report(struct hid_device *hid,
 
 	/* The query buffer contains the size, dropping it in the reply */
 	count = min(count, ret_count - 2);
-	memcpy(buf, ihid->inbuf + 2, count);
+	memcpy(buf, ihid->rawbuf + 2, count);
 
 	return count;
 }
diff --git a/drivers/hid/usbhid/hid-core.c b/drivers/hid/usbhid/hid-core.c
index ca6849a0121e..bfbe1bedda7f 100644
--- a/drivers/hid/usbhid/hid-core.c
+++ b/drivers/hid/usbhid/hid-core.c
@@ -278,18 +278,20 @@ static void hid_irq_in(struct urb *urb)
 		usbhid->retry_delay = 0;
 		if ((hid->quirks & HID_QUIRK_ALWAYS_POLL) && !hid->open)
 			break;
-		hid_input_report(urb->context, HID_INPUT_REPORT,
-				 urb->transfer_buffer,
-				 urb->actual_length, 1);
-		/*
-		 * autosuspend refused while keys are pressed
-		 * because most keyboards don't wake up when
-		 * a key is released
-		 */
-		if (hid_check_keys_pressed(hid))
-			set_bit(HID_KEYS_PRESSED, &usbhid->iofl);
-		else
-			clear_bit(HID_KEYS_PRESSED, &usbhid->iofl);
+		if (!test_bit(HID_RESUME_RUNNING, &usbhid->iofl)) {
+			hid_input_report(urb->context, HID_INPUT_REPORT,
+					 urb->transfer_buffer,
+					 urb->actual_length, 1);
+			/*
+			 * autosuspend refused while keys are pressed
+			 * because most keyboards don't wake up when
+			 * a key is released
+			 */
+			if (hid_check_keys_pressed(hid))
+				set_bit(HID_KEYS_PRESSED, &usbhid->iofl);
+			else
+				clear_bit(HID_KEYS_PRESSED, &usbhid->iofl);
+		}
 		break;
 	case -EPIPE:		/* stall */
 		usbhid_mark_busy(usbhid);
@@ -338,8 +340,7 @@ static int hid_submit_out(struct hid_device *hid)
 	report = usbhid->out[usbhid->outtail].report;
 	raw_report = usbhid->out[usbhid->outtail].raw_report;
 
-	usbhid->urbout->transfer_buffer_length = ((report->size - 1) >> 3) +
-						 1 + (report->id > 0);
+	usbhid->urbout->transfer_buffer_length = hid_report_len(report);
 	usbhid->urbout->dev = hid_to_usb_dev(hid);
 	if (raw_report) {
 		memcpy(usbhid->outbuf, raw_report,
@@ -688,6 +689,7 @@ int usbhid_open(struct hid_device *hid)
 			goto done;
 		}
 		usbhid->intf->needs_remote_wakeup = 1;
+		set_bit(HID_RESUME_RUNNING, &usbhid->iofl);
 		res = hid_start_in(hid);
 		if (res) {
 			if (res != -ENOSPC) {
@@ -701,6 +703,15 @@ int usbhid_open(struct hid_device *hid)
 			}
 		}
 		usb_autopm_put_interface(usbhid->intf);
+
+		/*
+		 * In case events are generated while nobody was listening,
+		 * some are released when the device is re-opened.
+		 * Wait 50 msec for the queue to empty before allowing events
+		 * to go through hid.
+		 */
+		msleep(50);
+		clear_bit(HID_RESUME_RUNNING, &usbhid->iofl);
 	}
 done:
 	mutex_unlock(&hid_open_mut);
diff --git a/drivers/hid/usbhid/hid-quirks.c b/drivers/hid/usbhid/hid-quirks.c
index 552671ee7c5d..dc89be90b35e 100644
--- a/drivers/hid/usbhid/hid-quirks.c
+++ b/drivers/hid/usbhid/hid-quirks.c
@@ -73,11 +73,13 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_ELAN, USB_DEVICE_ID_ELAN_TOUCHSCREEN, HID_QUIRK_ALWAYS_POLL },
 	{ USB_VENDOR_ID_ELAN, USB_DEVICE_ID_ELAN_TOUCHSCREEN_009B, HID_QUIRK_ALWAYS_POLL },
 	{ USB_VENDOR_ID_ELAN, USB_DEVICE_ID_ELAN_TOUCHSCREEN_0103, HID_QUIRK_ALWAYS_POLL },
+	{ USB_VENDOR_ID_ELAN, USB_DEVICE_ID_ELAN_TOUCHSCREEN_010c, HID_QUIRK_ALWAYS_POLL },
 	{ USB_VENDOR_ID_ELAN, USB_DEVICE_ID_ELAN_TOUCHSCREEN_016F, HID_QUIRK_ALWAYS_POLL },
 	{ USB_VENDOR_ID_ELO, USB_DEVICE_ID_ELO_TS2700, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_FORMOSA, USB_DEVICE_ID_FORMOSA_IR_RECEIVER, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_FREESCALE, USB_DEVICE_ID_FREESCALE_MX28, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_MGE, USB_DEVICE_ID_MGE_UPS, HID_QUIRK_NOGET },
+	{ USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_3, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_MSI, USB_DEVICE_ID_MSI_GT683R_LED_PANEL, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_NEXIO, USB_DEVICE_ID_NEXIO_MULTITOUCH_PTI0750, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_NOVATEK, USB_DEVICE_ID_NOVATEK_MOUSE, HID_QUIRK_NO_INIT_REPORTS },
diff --git a/drivers/hid/usbhid/usbhid.h b/drivers/hid/usbhid/usbhid.h
index f633c24ce28b..807922b49aa4 100644
--- a/drivers/hid/usbhid/usbhid.h
+++ b/drivers/hid/usbhid/usbhid.h
@@ -52,6 +52,7 @@ struct usb_interface *usbhid_find_interface(int minor);
 #define HID_STARTED		8
 #define HID_KEYS_PRESSED	10
 #define HID_NO_BANDWIDTH	11
+#define HID_RESUME_RUNNING	12
 
 /*
  * USB-specific HID struct, to be pointed to
diff --git a/drivers/hid/wacom.h b/drivers/hid/wacom.h
index 0cc53440543a..7db432809e9e 100644
--- a/drivers/hid/wacom.h
+++ b/drivers/hid/wacom.h
@@ -140,7 +140,7 @@ extern const struct hid_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);
-int wacom_setup_input_capabilities(struct input_dev *input_dev,
+int wacom_setup_pentouch_input_capabilities(struct input_dev *input_dev,
 				   struct wacom_wac *wacom_wac);
 int wacom_setup_pad_input_capabilities(struct input_dev *input_dev,
 				       struct wacom_wac *wacom_wac);
diff --git a/drivers/hid/wacom_sys.c b/drivers/hid/wacom_sys.c
index 8593047bb726..654202941d30 100644
--- a/drivers/hid/wacom_sys.c
+++ b/drivers/hid/wacom_sys.c
@@ -13,6 +13,7 @@
 
 #include "wacom_wac.h"
 #include "wacom.h"
+#include <linux/input/mt.h>
 
 #define WAC_MSG_RETRIES		5
 
@@ -70,22 +71,15 @@ static int wacom_raw_event(struct hid_device *hdev, struct hid_report *report,
 static int wacom_open(struct input_dev *dev)
 {
 	struct wacom *wacom = input_get_drvdata(dev);
-	int retval;
-
-	mutex_lock(&wacom->lock);
-	retval = hid_hw_open(wacom->hdev);
-	mutex_unlock(&wacom->lock);
 
-	return retval;
+	return hid_hw_open(wacom->hdev);
 }
 
 static void wacom_close(struct input_dev *dev)
 {
 	struct wacom *wacom = input_get_drvdata(dev);
 
-	mutex_lock(&wacom->lock);
 	hid_hw_close(wacom->hdev);
-	mutex_unlock(&wacom->lock);
 }
 
 /*
@@ -192,9 +186,15 @@ static void wacom_usage_mapping(struct hid_device *hdev,
 	if (!pen && !finger)
 		return;
 
-	if (finger && !features->touch_max)
-		/* touch device at least supports one touch point */
-		features->touch_max = 1;
+	/*
+	 * Bamboo models do not support HID_DG_CONTACTMAX.
+	 * And, Bamboo Pen only descriptor contains touch.
+	 */
+	if (features->type != BAMBOO_PT) {
+		/* ISDv4 touch devices at least supports one touch point */
+		if (finger && !features->touch_max)
+			features->touch_max = 1;
+	}
 
 	switch (usage->hid) {
 	case HID_GD_X:
@@ -230,6 +230,21 @@ static void wacom_usage_mapping(struct hid_device *hdev,
 		wacom_wac_usage_mapping(hdev, field, usage);
 }
 
+static void wacom_post_parse_hid(struct hid_device *hdev,
+				 struct wacom_features *features)
+{
+	struct wacom *wacom = hid_get_drvdata(hdev);
+	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
+
+	if (features->type == HID_GENERIC) {
+		/* Any last-minute generic device setup */
+		if (features->touch_max > 1) {
+			input_mt_init_slots(wacom_wac->input, wacom_wac->features.touch_max,
+				    INPUT_MT_DIRECT);
+		}
+	}
+}
+
 static void wacom_parse_hid(struct hid_device *hdev,
 			   struct wacom_features *features)
 {
@@ -264,6 +279,8 @@ static void wacom_parse_hid(struct hid_device *hdev,
 				wacom_usage_mapping(hdev, hreport->field[i],
 						hreport->field[i]->usage + j);
 	}
+
+	wacom_post_parse_hid(hdev, features);
 }
 
 static int wacom_hid_set_device_mode(struct hid_device *hdev)
@@ -1129,7 +1146,7 @@ static void wacom_clean_inputs(struct wacom *wacom)
 			input_free_device(wacom->wacom_wac.input);
 	}
 	if (wacom->wacom_wac.pad_input) {
-		if (wacom->wacom_wac.input_registered)
+		if (wacom->wacom_wac.pad_registered)
 			input_unregister_device(wacom->wacom_wac.pad_input);
 		else
 			input_free_device(wacom->wacom_wac.pad_input);
@@ -1151,13 +1168,13 @@ static int wacom_register_inputs(struct wacom *wacom)
 	if (!input_dev || !pad_input_dev)
 		return -EINVAL;
 
-	error = wacom_setup_input_capabilities(input_dev, wacom_wac);
-	if (error)
-		return error;
-
-	error = input_register_device(input_dev);
-	if (error)
-		return error;
+	error = wacom_setup_pentouch_input_capabilities(input_dev, wacom_wac);
+	if (!error) {
+		error = input_register_device(input_dev);
+		if (error)
+			return error;
+		wacom_wac->input_registered = true;
+	}
 
 	error = wacom_setup_pad_input_capabilities(pad_input_dev, wacom_wac);
 	if (error) {
@@ -1169,22 +1186,23 @@ static int wacom_register_inputs(struct wacom *wacom)
 		error = input_register_device(pad_input_dev);
 		if (error)
 			goto fail_register_pad_input;
+		wacom_wac->pad_registered = true;
 
 		error = wacom_initialize_leds(wacom);
 		if (error)
 			goto fail_leds;
 	}
 
-	wacom_wac->input_registered = true;
-
 	return 0;
 
 fail_leds:
 	input_unregister_device(pad_input_dev);
 	pad_input_dev = NULL;
+	wacom_wac->pad_registered = false;
 fail_register_pad_input:
 	input_unregister_device(input_dev);
 	wacom_wac->input = NULL;
+	wacom_wac->input_registered = false;
 	return error;
 }
 
@@ -1321,12 +1339,6 @@ static void wacom_calculate_res(struct wacom_features *features)
 						    features->unitExpo);
 }
 
-static int wacom_hid_report_len(struct hid_report *report)
-{
-	/* equivalent to DIV_ROUND_UP(report->size, 8) + !!(report->id > 0) */
-	return ((report->size - 1) >> 3) + 1 + (report->id > 0);
-}
-
 static size_t wacom_compute_pktlen(struct hid_device *hdev)
 {
 	struct hid_report_enum *report_enum;
@@ -1336,7 +1348,7 @@ static size_t wacom_compute_pktlen(struct hid_device *hdev)
 	report_enum = hdev->report_enum + HID_INPUT_REPORT;
 
 	list_for_each_entry(report, &report_enum->report_list, list) {
-		size_t report_size = wacom_hid_report_len(report);
+		size_t report_size = hid_report_len(report);
 		if (report_size > size)
 			size = report_size;
 	}
diff --git a/drivers/hid/wacom_wac.c b/drivers/hid/wacom_wac.c
index 586b2405b0d4..ac7447c7b82e 100644
--- a/drivers/hid/wacom_wac.c
+++ b/drivers/hid/wacom_wac.c
@@ -25,6 +25,10 @@
 #define WACOM_INTUOS_RES	100
 #define WACOM_INTUOS3_RES	200
 
+/* Newer Cintiq and DTU have an offset between tablet and screen areas */
+#define WACOM_DTU_OFFSET	200
+#define WACOM_CINTIQ_OFFSET	400
+
 /*
  * Scale factor relating reported contact size to logical contact area.
  * 2^14/pi is a good approximation on Intuos5 and 3rd-gen Bamboo
@@ -600,8 +604,8 @@ static void wacom_intuos_general(struct wacom_wac *wacom)
 		}
 		input_report_abs(input, ABS_PRESSURE, t);
 		input_report_abs(input, ABS_TILT_X,
-				((data[7] << 1) & 0x7e) | (data[8] >> 7));
-		input_report_abs(input, ABS_TILT_Y, data[8] & 0x7f);
+				 (((data[7] << 1) & 0x7e) | (data[8] >> 7)) - 64);
+		input_report_abs(input, ABS_TILT_Y, (data[8] & 0x7f) - 64);
 		input_report_key(input, BTN_STYLUS, data[1] & 2);
 		input_report_key(input, BTN_STYLUS2, data[1] & 4);
 		input_report_key(input, BTN_TOUCH, t > 10);
@@ -612,8 +616,8 @@ static void wacom_intuos_general(struct wacom_wac *wacom)
 		input_report_abs(input, ABS_WHEEL,
 				(data[6] << 2) | ((data[7] >> 6) & 3));
 		input_report_abs(input, ABS_TILT_X,
-				((data[7] << 1) & 0x7e) | (data[8] >> 7));
-		input_report_abs(input, ABS_TILT_Y, data[8] & 0x7f);
+				 (((data[7] << 1) & 0x7e) | (data[8] >> 7)) - 64);
+		input_report_abs(input, ABS_TILT_Y, (data[8] & 0x7f) - 64);
 	}
 }
 
@@ -915,8 +919,8 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
 				input_report_key(input, BTN_EXTRA,  data[6] & 0x10);
 
 				input_report_abs(input, ABS_TILT_X,
-					((data[7] << 1) & 0x7e) | (data[8] >> 7));
-				input_report_abs(input, ABS_TILT_Y, data[8] & 0x7f);
+					(((data[7] << 1) & 0x7e) | (data[8] >> 7)) - 64);
+				input_report_abs(input, ABS_TILT_Y, (data[8] & 0x7f) - 64);
 			} else {
 				/* 2D mouse packet */
 				input_report_key(input, BTN_LEFT,   data[8] & 0x04);
@@ -1377,11 +1381,12 @@ static void wacom_wac_finger_usage_mapping(struct hid_device *hdev,
 {
 	struct wacom *wacom = hid_get_drvdata(hdev);
 	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
-	struct input_dev *input = wacom_wac->input;
+	struct wacom_features *features = &wacom_wac->features;
 	unsigned touch_max = wacom_wac->features.touch_max;
 
 	switch (usage->hid) {
 	case HID_GD_X:
+		features->last_slot_field = usage->hid;
 		if (touch_max == 1)
 			wacom_map_usage(wacom, usage, field, EV_ABS, ABS_X, 4);
 		else
@@ -1389,6 +1394,7 @@ static void wacom_wac_finger_usage_mapping(struct hid_device *hdev,
 					ABS_MT_POSITION_X, 4);
 		break;
 	case HID_GD_Y:
+		features->last_slot_field = usage->hid;
 		if (touch_max == 1)
 			wacom_map_usage(wacom, usage, field, EV_ABS, ABS_Y, 4);
 		else
@@ -1396,19 +1402,48 @@ static void wacom_wac_finger_usage_mapping(struct hid_device *hdev,
 					ABS_MT_POSITION_Y, 4);
 		break;
 	case HID_DG_CONTACTID:
-		input_mt_init_slots(input, wacom_wac->features.touch_max,
-			INPUT_MT_DIRECT);
+		features->last_slot_field = usage->hid;
 		break;
 	case HID_DG_INRANGE:
+		features->last_slot_field = usage->hid;
 		break;
 	case HID_DG_INVERT:
+		features->last_slot_field = usage->hid;
 		break;
 	case HID_DG_TIPSWITCH:
+		features->last_slot_field = usage->hid;
 		wacom_map_usage(wacom, usage, field, EV_KEY, BTN_TOUCH, 0);
 		break;
 	}
 }
 
+static void wacom_wac_finger_slot(struct wacom_wac *wacom_wac,
+		struct input_dev *input)
+{
+	struct hid_data *hid_data = &wacom_wac->hid_data;
+	bool mt = wacom_wac->features.touch_max > 1;
+	bool prox = hid_data->tipswitch &&
+		    !wacom_wac->shared->stylus_in_proximity;
+
+	if (mt) {
+		int slot;
+
+		slot = input_mt_get_slot_by_key(input, hid_data->id);
+		input_mt_slot(input, slot);
+		input_mt_report_slot_state(input, MT_TOOL_FINGER, prox);
+	}
+	else {
+		input_report_key(input, BTN_TOUCH, prox);
+	}
+
+	if (prox) {
+		input_report_abs(input, mt ? ABS_MT_POSITION_X : ABS_X,
+				 hid_data->x);
+		input_report_abs(input, mt ? ABS_MT_POSITION_Y : ABS_Y,
+				 hid_data->y);
+	}
+}
+
 static int wacom_wac_finger_event(struct hid_device *hdev,
 		struct hid_field *field, struct hid_usage *usage, __s32 value)
 {
@@ -1431,36 +1466,35 @@ static int wacom_wac_finger_event(struct hid_device *hdev,
 	}
 
 
+	if (usage->usage_index + 1 == field->report_count) {
+		if (usage->hid == wacom_wac->features.last_slot_field)
+			wacom_wac_finger_slot(wacom_wac, wacom_wac->input);
+	}
+
 	return 0;
 }
 
-static void wacom_wac_finger_mt_report(struct wacom_wac *wacom_wac,
-		struct input_dev *input, bool touch)
+static int wacom_wac_finger_count_touches(struct hid_device *hdev)
 {
-	int slot;
-	struct hid_data *hid_data = &wacom_wac->hid_data;
+	struct wacom *wacom = hid_get_drvdata(hdev);
+	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
+	struct input_dev *input = wacom_wac->input;
+	unsigned touch_max = wacom_wac->features.touch_max;
+	int count = 0;
+	int i;
 
-	slot = input_mt_get_slot_by_key(input, hid_data->id);
+	if (touch_max == 1)
+		return wacom_wac->hid_data.tipswitch &&
+		       !wacom_wac->shared->stylus_in_proximity;
 
-	input_mt_slot(input, slot);
-	input_mt_report_slot_state(input, MT_TOOL_FINGER, touch);
-	if (touch) {
-		input_report_abs(input, ABS_MT_POSITION_X, hid_data->x);
-		input_report_abs(input, ABS_MT_POSITION_Y, hid_data->y);
+	for (i = 0; i < input->mt->num_slots; i++) {
+		struct input_mt_slot *ps = &input->mt->slots[i];
+		int id = input_mt_get_value(ps, ABS_MT_TRACKING_ID);
+		if (id >= 0)
+			count++;
 	}
-	input_mt_sync_frame(input);
-}
 
-static void wacom_wac_finger_single_touch_report(struct wacom_wac *wacom_wac,
-		struct input_dev *input, bool touch)
-{
-	struct hid_data *hid_data = &wacom_wac->hid_data;
-
-	if (touch) {
-		input_report_abs(input, ABS_X, hid_data->x);
-		input_report_abs(input, ABS_Y, hid_data->y);
-	}
-	input_report_key(input, BTN_TOUCH, touch);
+	return count;
 }
 
 static void wacom_wac_finger_report(struct hid_device *hdev,
@@ -1469,24 +1503,23 @@ static void wacom_wac_finger_report(struct hid_device *hdev,
 	struct wacom *wacom = hid_get_drvdata(hdev);
 	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
 	struct input_dev *input = wacom_wac->input;
-	bool touch = wacom_wac->hid_data.tipswitch &&
-		     !wacom_wac->shared->stylus_in_proximity;
 	unsigned touch_max = wacom_wac->features.touch_max;
 
 	if (touch_max > 1)
-		wacom_wac_finger_mt_report(wacom_wac, input, touch);
-	else
-		wacom_wac_finger_single_touch_report(wacom_wac, input, touch);
+		input_mt_sync_frame(input);
+
 	input_sync(input);
 
 	/* keep touch state for pen event */
-	wacom_wac->shared->touch_down = touch;
+	wacom_wac->shared->touch_down = wacom_wac_finger_count_touches(hdev);
 }
 
 #define WACOM_PEN_FIELD(f)	(((f)->logical == HID_DG_STYLUS) || \
-				 ((f)->physical == HID_DG_STYLUS))
+				 ((f)->physical == HID_DG_STYLUS) || \
+				 ((f)->application == HID_DG_PEN))
 #define WACOM_FINGER_FIELD(f)	(((f)->logical == HID_DG_FINGER) || \
-				 ((f)->physical == HID_DG_FINGER))
+				 ((f)->physical == HID_DG_FINGER) || \
+				 ((f)->application == HID_DG_TOUCHSCREEN))
 
 void wacom_wac_usage_mapping(struct hid_device *hdev,
 		struct hid_field *field, struct hid_usage *usage)
@@ -1681,7 +1714,9 @@ static int wacom_bpt_pen(struct wacom_wac *wacom)
 	    return 0;
 
 	if (data[0] == WACOM_REPORT_USB) {
-		if (features->type == INTUOSHT && features->touch_max) {
+		if (features->type == INTUOSHT &&
+		    wacom->shared->touch_input &&
+		    features->touch_max) {
 			input_report_switch(wacom->shared->touch_input,
 					    SW_MUTE_DEVICE, data[8] & 0x40);
 			input_sync(wacom->shared->touch_input);
@@ -1774,7 +1809,8 @@ static int wacom_wireless_irq(struct wacom_wac *wacom, size_t len)
 		int pid, battery, ps_connected;
 
 		if ((wacom->shared->type == INTUOSHT) &&
-				wacom->shared->touch_max) {
+		    wacom->shared->touch_input &&
+		    wacom->shared->touch_max) {
 			input_report_switch(wacom->shared->touch_input,
 					SW_MUTE_DEVICE, data[5] & 0x40);
 			input_sync(wacom->shared->touch_input);
@@ -1838,6 +1874,7 @@ void wacom_wac_irq(struct wacom_wac *wacom_wac, size_t len)
 		break;
 
 	case DTUS:
+	case DTUSX:
 		sync = wacom_dtus_irq(wacom_wac);
 		break;
 
@@ -1926,8 +1963,10 @@ static void wacom_setup_cintiq(struct wacom_wac *wacom_wac)
 	input_set_abs_params(input_dev, ABS_DISTANCE,
 			     0, wacom_wac->features.distance_max, 0, 0);
 	input_set_abs_params(input_dev, ABS_WHEEL, 0, 1023, 0, 0);
-	input_set_abs_params(input_dev, ABS_TILT_X, 0, 127, 0, 0);
-	input_set_abs_params(input_dev, ABS_TILT_Y, 0, 127, 0, 0);
+	input_set_abs_params(input_dev, ABS_TILT_X, -64, 63, 0, 0);
+	input_abs_set_res(input_dev, ABS_TILT_X, 57);
+	input_set_abs_params(input_dev, ABS_TILT_Y, -64, 63, 0, 0);
+	input_abs_set_res(input_dev, ABS_TILT_Y, 57);
 }
 
 static void wacom_setup_intuos(struct wacom_wac *wacom_wac)
@@ -1947,6 +1986,7 @@ static void wacom_setup_intuos(struct wacom_wac *wacom_wac)
 	__set_bit(BTN_TOOL_LENS, input_dev->keybit);
 
 	input_set_abs_params(input_dev, ABS_RZ, -900, 899, 0, 0);
+	input_abs_set_res(input_dev, ABS_RZ, 287);
 	input_set_abs_params(input_dev, ABS_THROTTLE, -1023, 1023, 0, 0);
 }
 
@@ -2029,7 +2069,7 @@ static void wacom_abs_set_axis(struct input_dev *input_dev,
 	}
 }
 
-int wacom_setup_input_capabilities(struct input_dev *input_dev,
+int wacom_setup_pentouch_input_capabilities(struct input_dev *input_dev,
 				   struct wacom_wac *wacom_wac)
 {
 	struct wacom_features *features = &wacom_wac->features;
@@ -2047,9 +2087,6 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 
 	switch (features->type) {
 	case WACOM_MO:
-		input_set_abs_params(input_dev, ABS_WHEEL, 0, 71, 0, 0);
-		/* fall through */
-
 	case WACOM_G4:
 		/* fall through */
 
@@ -2092,6 +2129,7 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 
 	case WACOM_24HD:
 		input_set_abs_params(input_dev, ABS_Z, -900, 899, 0, 0);
+		input_abs_set_res(input_dev, ABS_Z, 287);
 		input_set_abs_params(input_dev, ABS_THROTTLE, 0, 71, 0, 0);
 		/* fall through */
 
@@ -2106,6 +2144,7 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 	case WACOM_BEE:
 	case CINTIQ:
 		input_set_abs_params(input_dev, ABS_Z, -900, 899, 0, 0);
+		input_abs_set_res(input_dev, ABS_Z, 287);
 
 		__set_bit(INPUT_PROP_DIRECT, input_dev->propbit);
 
@@ -2114,6 +2153,7 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 
 	case WACOM_13HD:
 		input_set_abs_params(input_dev, ABS_Z, -900, 899, 0, 0);
+		input_abs_set_res(input_dev, ABS_Z, 287);
 		__set_bit(INPUT_PROP_DIRECT, input_dev->propbit);
 		wacom_setup_cintiq(wacom_wac);
 		break;
@@ -2122,6 +2162,7 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 	case INTUOS3L:
 	case INTUOS3S:
 		input_set_abs_params(input_dev, ABS_Z, -900, 899, 0, 0);
+		input_abs_set_res(input_dev, ABS_Z, 287);
 		/* fall through */
 
 	case INTUOS:
@@ -2144,6 +2185,7 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 					      0, 0);
 
 			input_set_abs_params(input_dev, ABS_Z, -900, 899, 0, 0);
+			input_abs_set_res(input_dev, ABS_Z, 287);
 
 			wacom_setup_intuos(wacom_wac);
 		} else if (features->device_type == BTN_TOOL_FINGER) {
@@ -2162,6 +2204,7 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 	case INTUOS4L:
 	case INTUOS4S:
 		input_set_abs_params(input_dev, ABS_Z, -900, 899, 0, 0);
+		input_abs_set_res(input_dev, ABS_Z, 287);
 		wacom_setup_intuos(wacom_wac);
 
 		__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
@@ -2196,6 +2239,7 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 		/* fall through */
 
 	case DTUS:
+	case DTUSX:
 	case PL:
 	case DTU:
 		__set_bit(BTN_TOOL_PEN, input_dev->keybit);
@@ -2246,6 +2290,9 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 				__clear_bit(ABS_X, input_dev->absbit);
 				__clear_bit(ABS_Y, input_dev->absbit);
 				__clear_bit(BTN_TOUCH, input_dev->keybit);
+
+				/* PAD is setup by wacom_setup_pad_input_capabilities later */
+				return 1;
 			}
 		} else if (features->device_type == BTN_TOOL_PEN) {
 			__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
@@ -2261,6 +2308,7 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 
 	case CINTIQ_HYBRID:
 		input_set_abs_params(input_dev, ABS_Z, -900, 899, 0, 0);
+		input_abs_set_res(input_dev, ABS_Z, 287);
 		__set_bit(INPUT_PROP_DIRECT, input_dev->propbit);
 
 		wacom_setup_cintiq(wacom_wac);
@@ -2303,9 +2351,7 @@ int wacom_setup_pad_input_capabilities(struct input_dev *input_dev,
 
 	case WACOM_G4:
 		__set_bit(BTN_BACK, input_dev->keybit);
-		__set_bit(BTN_LEFT, input_dev->keybit);
 		__set_bit(BTN_FORWARD, input_dev->keybit);
-		__set_bit(BTN_RIGHT, input_dev->keybit);
 		input_set_capability(input_dev, EV_REL, REL_WHEEL);
 		break;
 
@@ -2402,7 +2448,7 @@ int wacom_setup_pad_input_capabilities(struct input_dev *input_dev,
 	case INTUOSPS:
 		/* touch interface does not have the pad device */
 		if (features->device_type != BTN_TOOL_PEN)
-			return 1;
+			return -ENODEV;
 
 		for (i = 0; i < 7; i++)
 			__set_bit(BTN_0 + i, input_dev->keybit);
@@ -2446,8 +2492,10 @@ int wacom_setup_pad_input_capabilities(struct input_dev *input_dev,
 	case INTUOSHT:
 	case BAMBOO_PT:
 		/* pad device is on the touch interface */
-		if (features->device_type != BTN_TOOL_FINGER)
-			return 1;
+		if ((features->device_type != BTN_TOOL_FINGER) ||
+		    /* Bamboo Pen only tablet does not have pad */
+		    ((features->type == BAMBOO_PT) && !features->touch_max))
+			return -ENODEV;
 
 		__clear_bit(ABS_MISC, input_dev->absbit);
 
@@ -2460,7 +2508,7 @@ int wacom_setup_pad_input_capabilities(struct input_dev *input_dev,
 
 	default:
 		/* no pad supported */
-		return 1;
+		return -ENODEV;
 	}
 	return 0;
 }
@@ -2664,11 +2712,13 @@ static const struct wacom_features wacom_features_0x317 =
 	  INTUOSPL, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, .touch_max = 16,
 	  .check_for_hid_type = true, .hid_type = HID_TYPE_USBNONE };
 static const struct wacom_features wacom_features_0xF4 =
-	{ "Wacom Cintiq 24HD", 104280, 65400, 2047, 63,
-	  WACOM_24HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200 };
+	{ "Wacom Cintiq 24HD", 104080, 65200, 2047, 63,
+	  WACOM_24HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	  WACOM_CINTIQ_OFFSET, WACOM_CINTIQ_OFFSET };
 static const struct wacom_features wacom_features_0xF8 =
-	{ "Wacom Cintiq 24HD touch", 104280, 65400, 2047, 63, /* Pen */
-	  WACOM_24HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200,
+	{ "Wacom Cintiq 24HD touch", 104080, 65200, 2047, 63, /* Pen */
+	  WACOM_24HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	  WACOM_CINTIQ_OFFSET, WACOM_CINTIQ_OFFSET,
 	  .oVid = USB_VENDOR_ID_WACOM, .oPid = 0xf6 };
 static const struct wacom_features wacom_features_0xF6 =
 	{ "Wacom Cintiq 24HD touch", .type = WACOM_24HDT, /* Touch */
@@ -2684,8 +2734,9 @@ static const struct wacom_features wacom_features_0xC6 =
 	{ "Wacom Cintiq 12WX", 53020, 33440, 1023, 63,
 	  WACOM_BEE, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
 static const struct wacom_features wacom_features_0x304 =
-	{ "Wacom Cintiq 13HD", 59352, 33648, 1023, 63,
-	  WACOM_13HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200 };
+	{ "Wacom Cintiq 13HD", 59152, 33448, 1023, 63,
+	  WACOM_13HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	  WACOM_CINTIQ_OFFSET, WACOM_CINTIQ_OFFSET };
 static const struct wacom_features wacom_features_0xC7 =
 	{ "Wacom DTU1931", 37832, 30305, 511, 0,
 	  PL, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
@@ -2697,28 +2748,38 @@ static const struct wacom_features wacom_features_0xF0 =
 	{ "Wacom DTU1631", 34623, 19553, 511, 0,
 	  DTU, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0xFB =
-	{ "Wacom DTU1031", 22096, 13960, 511, 0,
-	  DTUS, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	{ "Wacom DTU1031", 21896, 13760, 511, 0,
+	  DTUS, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  WACOM_DTU_OFFSET, WACOM_DTU_OFFSET };
+static const struct wacom_features wacom_features_0x32F =
+	{ "Wacom DTU1031X", 22472, 12728, 511, 0,
+	  DTUSX, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  WACOM_DTU_OFFSET, WACOM_DTU_OFFSET };
 static const struct wacom_features wacom_features_0x57 =
 	{ "Wacom DTK2241", 95640, 54060, 2047, 63,
-	  DTK, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200 };
+	  DTK, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	  WACOM_CINTIQ_OFFSET, WACOM_CINTIQ_OFFSET };
 static const struct wacom_features wacom_features_0x59 = /* Pen */
 	{ "Wacom DTH2242", 95640, 54060, 2047, 63,
-	  DTK, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200,
+	  DTK, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	  WACOM_CINTIQ_OFFSET, WACOM_CINTIQ_OFFSET,
 	  .oVid = USB_VENDOR_ID_WACOM, .oPid = 0x5D };
 static const struct wacom_features wacom_features_0x5D = /* Touch */
 	{ "Wacom DTH2242",       .type = WACOM_24HDT,
 	  .oVid = USB_VENDOR_ID_WACOM, .oPid = 0x59, .touch_max = 10,
 	  .check_for_hid_type = true, .hid_type = HID_TYPE_USBNONE };
 static const struct wacom_features wacom_features_0xCC =
-	{ "Wacom Cintiq 21UX2", 87000, 65400, 2047, 63,
-	  WACOM_21UX2, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200 };
+	{ "Wacom Cintiq 21UX2", 86800, 65200, 2047, 63,
+	  WACOM_21UX2, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	  WACOM_CINTIQ_OFFSET, WACOM_CINTIQ_OFFSET };
 static const struct wacom_features wacom_features_0xFA =
-	{ "Wacom Cintiq 22HD", 95640, 54060, 2047, 63,
-	  WACOM_22HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200 };
+	{ "Wacom Cintiq 22HD", 95440, 53860, 2047, 63,
+	  WACOM_22HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	  WACOM_CINTIQ_OFFSET, WACOM_CINTIQ_OFFSET };
 static const struct wacom_features wacom_features_0x5B =
-	{ "Wacom Cintiq 22HDT", 95640, 54060, 2047, 63,
-	  WACOM_22HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200,
+	{ "Wacom Cintiq 22HDT", 95440, 53860, 2047, 63,
+	  WACOM_22HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	  WACOM_CINTIQ_OFFSET, WACOM_CINTIQ_OFFSET,
 	  .oVid = USB_VENDOR_ID_WACOM, .oPid = 0x5e };
 static const struct wacom_features wacom_features_0x5E =
 	{ "Wacom Cintiq 22HDT", .type = WACOM_24HDT,
@@ -2863,21 +2924,27 @@ static const struct wacom_features wacom_features_0x6004 =
 	{ "ISD-V4", 12800, 8000, 255, 0,
 	  TABLETPC, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0x307 =
-	{ "Wacom ISDv5 307", 59352, 33648, 2047, 63,
-	  CINTIQ_HYBRID, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200,
+	{ "Wacom ISDv5 307", 59152, 33448, 2047, 63,
+	  CINTIQ_HYBRID, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	  WACOM_CINTIQ_OFFSET, WACOM_CINTIQ_OFFSET,
 	  .oVid = USB_VENDOR_ID_WACOM, .oPid = 0x309 };
 static const struct wacom_features wacom_features_0x309 =
 	{ "Wacom ISDv5 309", .type = WACOM_24HDT, /* Touch */
 	  .oVid = USB_VENDOR_ID_WACOM, .oPid = 0x0307, .touch_max = 10,
 	  .check_for_hid_type = true, .hid_type = HID_TYPE_USBNONE };
 static const struct wacom_features wacom_features_0x30A =
-	{ "Wacom ISDv5 30A", 59352, 33648, 2047, 63,
-	  CINTIQ_HYBRID, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200,
+	{ "Wacom ISDv5 30A", 59152, 33448, 2047, 63,
+	  CINTIQ_HYBRID, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	  WACOM_CINTIQ_OFFSET, WACOM_CINTIQ_OFFSET,
 	  .oVid = USB_VENDOR_ID_WACOM, .oPid = 0x30C };
 static const struct wacom_features wacom_features_0x30C =
 	{ "Wacom ISDv5 30C", .type = WACOM_24HDT, /* Touch */
 	  .oVid = USB_VENDOR_ID_WACOM, .oPid = 0x30A, .touch_max = 10,
 	  .check_for_hid_type = true, .hid_type = HID_TYPE_USBNONE };
+static const struct wacom_features wacom_features_0x323 =
+	{ "Wacom Intuos P M", 21600, 13500, 1023, 31,
+	  INTUOSHT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .check_for_hid_type = true, .hid_type = HID_TYPE_USBNONE };
 
 static const struct wacom_features wacom_features_HID_ANY_ID =
 	{ "Wacom HID", .type = HID_GENERIC };
@@ -3022,10 +3089,13 @@ const struct hid_device_id wacom_ids[] = {
 	{ USB_DEVICE_WACOM(0x314) },
 	{ USB_DEVICE_WACOM(0x315) },
 	{ USB_DEVICE_WACOM(0x317) },
+	{ USB_DEVICE_WACOM(0x323) },
+	{ USB_DEVICE_WACOM(0x32F) },
 	{ USB_DEVICE_WACOM(0x4001) },
 	{ USB_DEVICE_WACOM(0x4004) },
 	{ USB_DEVICE_WACOM(0x5000) },
 	{ USB_DEVICE_WACOM(0x5002) },
+	{ USB_DEVICE_LENOVO(0x6004) },
 
 	{ USB_DEVICE_WACOM(HID_ANY_ID) },
 	{ }
diff --git a/drivers/hid/wacom_wac.h b/drivers/hid/wacom_wac.h
index 0f0b85ec1322..bfad815cda8a 100644
--- a/drivers/hid/wacom_wac.h
+++ b/drivers/hid/wacom_wac.h
@@ -80,6 +80,7 @@ enum {
 	PL,
 	DTU,
 	DTUS,
+	DTUSX,
 	INTUOS,
 	INTUOS3S,
 	INTUOS3,
@@ -144,6 +145,7 @@ struct wacom_features {
 	int pktlen;
 	bool check_for_hid_type;
 	int hid_type;
+	int last_slot_field;
 };
 
 struct wacom_shared {
@@ -183,6 +185,7 @@ struct wacom_wac {
 	struct input_dev *input;
 	struct input_dev *pad_input;
 	bool input_registered;
+	bool pad_registered;
 	int pid;
 	int battery_capacity;
 	int num_contacts_left;
diff --git a/drivers/hsi/clients/nokia-modem.c b/drivers/hsi/clients/nokia-modem.c
index 363b780dacea..f0c21458962c 100644
--- a/drivers/hsi/clients/nokia-modem.c
+++ b/drivers/hsi/clients/nokia-modem.c
@@ -29,7 +29,7 @@
 #include <linux/of_gpio.h>
 #include <linux/hsi/ssi_protocol.h>
 
-static unsigned int pm;
+static unsigned int pm = 1;
 module_param(pm, int, 0400);
 MODULE_PARM_DESC(pm,
 	"Enable power management (0=disabled, 1=userland based [default])");
@@ -164,9 +164,9 @@ static int nokia_modem_probe(struct device *dev)
 	dev_set_drvdata(dev, modem);
 
 	irq = irq_of_parse_and_map(np, 0);
-	if (irq < 0) {
+	if (!irq) {
 		dev_err(dev, "Invalid rst_ind interrupt (%d)\n", irq);
-		return irq;
+		return -EINVAL;
 	}
 	modem->nokia_modem_rst_ind_irq = irq;
 	pflags = irq_get_trigger_type(irq);
@@ -174,7 +174,7 @@ static int nokia_modem_probe(struct device *dev)
 	tasklet_init(&modem->nokia_modem_rst_ind_tasklet,
 			do_nokia_modem_rst_ind_tasklet, (unsigned long)modem);
 	err = devm_request_irq(dev, irq, nokia_modem_rst_ind_isr,
-				IRQF_DISABLED | pflags, "modem_rst_ind", modem);
+				pflags, "modem_rst_ind", modem);
 	if (err < 0) {
 		dev_err(dev, "Request rst_ind irq(%d) failed (flags %d)\n",
 								irq, pflags);
diff --git a/drivers/hsi/controllers/omap_ssi.c b/drivers/hsi/controllers/omap_ssi.c
index 4d5b682fc6af..089c6c3feb3e 100644
--- a/drivers/hsi/controllers/omap_ssi.c
+++ b/drivers/hsi/controllers/omap_ssi.c
@@ -610,7 +610,6 @@ static struct platform_driver ssi_pdriver = {
 	.remove	= __exit_p(ssi_remove),
 	.driver	= {
 		.name	= "omap_ssi",
-		.owner	= THIS_MODULE,
 		.pm     = DEV_PM_OPS,
 		.of_match_table = omap_ssi_of_match,
 	},
diff --git a/drivers/hsi/controllers/omap_ssi_port.c b/drivers/hsi/controllers/omap_ssi_port.c
index d836cfe50513..1f8652b3de06 100644
--- a/drivers/hsi/controllers/omap_ssi_port.c
+++ b/drivers/hsi/controllers/omap_ssi_port.c
@@ -1118,8 +1118,7 @@ static int __init ssi_port_probe(struct platform_device *pd)
 	dev_dbg(&pd->dev, "init ssi port...\n");
 
 	if (!try_module_get(ssi->owner)) {
-		dev_err(&pd->dev, "could not increment parent module refcount (err=%d)\n",
-			err);
+		dev_err(&pd->dev, "could not increment parent module refcount\n");
 		return -ENODEV;
 	}
 
@@ -1385,7 +1384,6 @@ static struct platform_driver ssi_port_pdriver = {
 	.remove	= __exit_p(ssi_port_remove),
 	.driver	= {
 		.name	= "omap_ssi_port",
-		.owner	= THIS_MODULE,
 		.of_match_table = omap_ssi_port_of_match,
 		.pm	= DEV_PM_OPS,
 	},
diff --git a/drivers/hv/channel_mgmt.c b/drivers/hv/channel_mgmt.c
index 191a6a3ae6ca..2c59f030546b 100644
--- a/drivers/hv/channel_mgmt.c
+++ b/drivers/hv/channel_mgmt.c
@@ -216,9 +216,16 @@ static void vmbus_process_rescind_offer(struct work_struct *work)
 	unsigned long flags;
 	struct vmbus_channel *primary_channel;
 	struct vmbus_channel_relid_released msg;
+	struct device *dev;
+
+	if (channel->device_obj) {
+		dev = get_device(&channel->device_obj->device);
+		if (dev) {
+			vmbus_device_unregister(channel->device_obj);
+			put_device(dev);
+		}
+	}
 
-	if (channel->device_obj)
-		vmbus_device_unregister(channel->device_obj);
 	memset(&msg, 0, sizeof(struct vmbus_channel_relid_released));
 	msg.child_relid = channel->offermsg.child_relid;
 	msg.header.msgtype = CHANNELMSG_RELID_RELEASED;
diff --git a/drivers/hv/hv_balloon.c b/drivers/hv/hv_balloon.c
index 5e90c5d771a7..b958ded8ac7e 100644
--- a/drivers/hv/hv_balloon.c
+++ b/drivers/hv/hv_balloon.c
@@ -1087,10 +1087,12 @@ static void balloon_up(struct work_struct *dummy)
 	struct dm_balloon_response *bl_resp;
 	int alloc_unit;
 	int ret;
-	bool alloc_error = false;
+	bool alloc_error;
 	bool done = false;
 	int i;
 
+	/* The host balloons pages in 2M granularity. */
+	WARN_ON_ONCE(num_pages % PAGES_IN_2M != 0);
 
 	/*
 	 * We will attempt 2M allocations. However, if we fail to
@@ -1107,16 +1109,18 @@ static void balloon_up(struct work_struct *dummy)
 
 
 		num_pages -= num_ballooned;
+		alloc_error = false;
 		num_ballooned = alloc_balloon_pages(&dm_device, num_pages,
 						bl_resp, alloc_unit,
 						 &alloc_error);
 
-		if ((alloc_error) && (alloc_unit != 1)) {
+		if (alloc_unit != 1 && num_ballooned == 0) {
 			alloc_unit = 1;
 			continue;
 		}
 
-		if ((alloc_error) || (num_ballooned == num_pages)) {
+		if ((alloc_unit == 1 && alloc_error) ||
+			(num_ballooned == num_pages)) {
 			bl_resp->more_pages = 0;
 			done = true;
 			dm_device.state = DM_INITIALIZED;
diff --git a/drivers/hv/hv_kvp.c b/drivers/hv/hv_kvp.c
index 521c14625b3a..beb8105c0e7b 100644
--- a/drivers/hv/hv_kvp.c
+++ b/drivers/hv/hv_kvp.c
@@ -350,6 +350,7 @@ kvp_send_key(struct work_struct *dummy)
 	__u8 pool = kvp_transaction.kvp_msg->kvp_hdr.pool;
 	__u32 val32;
 	__u64 val64;
+	int rc;
 
 	msg = kzalloc(sizeof(*msg) + sizeof(struct hv_kvp_msg) , GFP_ATOMIC);
 	if (!msg)
@@ -446,7 +447,13 @@ kvp_send_key(struct work_struct *dummy)
 	}
 
 	msg->len = sizeof(struct hv_kvp_msg);
-	cn_netlink_send(msg, 0, 0, GFP_ATOMIC);
+	rc = cn_netlink_send(msg, 0, 0, GFP_ATOMIC);
+	if (rc) {
+		pr_debug("KVP: failed to communicate to the daemon: %d\n", rc);
+		if (cancel_delayed_work_sync(&kvp_work))
+			kvp_respond_to_host(message, HV_E_FAIL);
+	}
+
 	kfree(msg);
 
 	return;
diff --git a/drivers/hv/hv_snapshot.c b/drivers/hv/hv_snapshot.c
index 34f14fddb666..9d5e0d1efdb5 100644
--- a/drivers/hv/hv_snapshot.c
+++ b/drivers/hv/hv_snapshot.c
@@ -28,7 +28,7 @@
 #define VSS_MINOR  0
 #define VSS_VERSION    (VSS_MAJOR << 16 | VSS_MINOR)
 
-
+#define VSS_USERSPACE_TIMEOUT (msecs_to_jiffies(10 * 1000))
 
 /*
  * Global state maintained for transaction that is being processed.
@@ -55,12 +55,24 @@ static const char vss_name[] = "vss_kernel_module";
 static __u8 *recv_buffer;
 
 static void vss_send_op(struct work_struct *dummy);
+static void vss_timeout_func(struct work_struct *dummy);
+
+static DECLARE_DELAYED_WORK(vss_timeout_work, vss_timeout_func);
 static DECLARE_WORK(vss_send_op_work, vss_send_op);
 
 /*
  * Callback when data is received from user mode.
  */
 
+static void vss_timeout_func(struct work_struct *dummy)
+{
+	/*
+	 * Timeout waiting for userspace component to reply happened.
+	 */
+	pr_warn("VSS: timeout waiting for daemon to reply\n");
+	vss_respond_to_host(HV_E_FAIL);
+}
+
 static void
 vss_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
 {
@@ -76,13 +88,15 @@ vss_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
 		return;
 
 	}
-	vss_respond_to_host(vss_msg->error);
+	if (cancel_delayed_work_sync(&vss_timeout_work))
+		vss_respond_to_host(vss_msg->error);
 }
 
 
 static void vss_send_op(struct work_struct *dummy)
 {
 	int op = vss_transaction.msg->vss_hdr.operation;
+	int rc;
 	struct cn_msg *msg;
 	struct hv_vss_msg *vss_msg;
 
@@ -98,7 +112,12 @@ static void vss_send_op(struct work_struct *dummy)
 	vss_msg->vss_hdr.operation = op;
 	msg->len = sizeof(struct hv_vss_msg);
 
-	cn_netlink_send(msg, 0, 0, GFP_ATOMIC);
+	rc = cn_netlink_send(msg, 0, 0, GFP_ATOMIC);
+	if (rc) {
+		pr_warn("VSS: failed to communicate to the daemon: %d\n", rc);
+		if (cancel_delayed_work_sync(&vss_timeout_work))
+			vss_respond_to_host(HV_E_FAIL);
+	}
 	kfree(msg);
 
 	return;
@@ -223,6 +242,8 @@ void hv_vss_onchannelcallback(void *context)
 			case VSS_OP_FREEZE:
 			case VSS_OP_THAW:
 				schedule_work(&vss_send_op_work);
+				schedule_delayed_work(&vss_timeout_work,
+						      VSS_USERSPACE_TIMEOUT);
 				return;
 
 			case VSS_OP_HOT_BACKUP:
@@ -277,5 +298,6 @@ hv_vss_init(struct hv_util_service *srv)
 void hv_vss_deinit(void)
 {
 	cn_del_callback(&vss_id);
+	cancel_delayed_work_sync(&vss_timeout_work);
 	cancel_work_sync(&vss_send_op_work);
 }
diff --git a/drivers/hwmon/abituguru.c b/drivers/hwmon/abituguru.c
index 9c8a6bab8228..7a09c1615aa9 100644
--- a/drivers/hwmon/abituguru.c
+++ b/drivers/hwmon/abituguru.c
@@ -1547,7 +1547,6 @@ static SIMPLE_DEV_PM_OPS(abituguru_pm, abituguru_suspend, abituguru_resume);
 
 static struct platform_driver abituguru_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= ABIT_UGURU_NAME,
 		.pm	= ABIT_UGURU_PM,
 	},
diff --git a/drivers/hwmon/abituguru3.c b/drivers/hwmon/abituguru3.c
index 4ae74aa8cdc1..3d2a4ae92d1e 100644
--- a/drivers/hwmon/abituguru3.c
+++ b/drivers/hwmon/abituguru3.c
@@ -1167,7 +1167,6 @@ static SIMPLE_DEV_PM_OPS(abituguru3_pm, abituguru3_suspend, abituguru3_resume);
 
 static struct platform_driver abituguru3_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= ABIT_UGURU3_NAME,
 		.pm	= ABIT_UGURU3_PM
 	},
diff --git a/drivers/hwmon/abx500.c b/drivers/hwmon/abx500.c
index 769fe20ec938..13875968c844 100644
--- a/drivers/hwmon/abx500.c
+++ b/drivers/hwmon/abx500.c
@@ -474,7 +474,6 @@ static const struct of_device_id abx500_temp_match[] = {
 
 static struct platform_driver abx500_temp_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "abx500-temp",
 		.of_match_table = of_match_ptr(abx500_temp_match),
 	},
diff --git a/drivers/hwmon/applesmc.c b/drivers/hwmon/applesmc.c
index 3288f13d2d87..0af63da6b603 100644
--- a/drivers/hwmon/applesmc.c
+++ b/drivers/hwmon/applesmc.c
@@ -676,7 +676,6 @@ static struct platform_driver applesmc_driver = {
 	.probe = applesmc_probe,
 	.driver	= {
 		.name = "applesmc",
-		.owner = THIS_MODULE,
 		.pm = &applesmc_pm_ops,
 	},
 };
diff --git a/drivers/hwmon/coretemp.c b/drivers/hwmon/coretemp.c
index d76f0b70c6e0..5b7fec824f10 100644
--- a/drivers/hwmon/coretemp.c
+++ b/drivers/hwmon/coretemp.c
@@ -596,7 +596,6 @@ static int coretemp_remove(struct platform_device *pdev)
 
 static struct platform_driver coretemp_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = DRVNAME,
 	},
 	.probe = coretemp_probe,
diff --git a/drivers/hwmon/da9052-hwmon.c b/drivers/hwmon/da9052-hwmon.c
index 692b3f34d88c..c9832bfacfe5 100644
--- a/drivers/hwmon/da9052-hwmon.c
+++ b/drivers/hwmon/da9052-hwmon.c
@@ -282,7 +282,6 @@ static struct platform_driver da9052_hwmon_driver = {
 	.probe = da9052_hwmon_probe,
 	.driver = {
 		.name = "da9052-hwmon",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/hwmon/da9055-hwmon.c b/drivers/hwmon/da9055-hwmon.c
index 9916a3fb4bb9..f6e159cabe23 100644
--- a/drivers/hwmon/da9055-hwmon.c
+++ b/drivers/hwmon/da9055-hwmon.c
@@ -286,7 +286,6 @@ static struct platform_driver da9055_hwmon_driver = {
 	.probe = da9055_hwmon_probe,
 	.driver = {
 		.name = "da9055-hwmon",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/hwmon/dme1737.c b/drivers/hwmon/dme1737.c
index bea0a344fab5..8763c4a8280c 100644
--- a/drivers/hwmon/dme1737.c
+++ b/drivers/hwmon/dme1737.c
@@ -2732,7 +2732,6 @@ static int dme1737_isa_remove(struct platform_device *pdev)
 
 static struct platform_driver dme1737_isa_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "dme1737",
 	},
 	.probe = dme1737_isa_probe,
diff --git a/drivers/hwmon/f71805f.c b/drivers/hwmon/f71805f.c
index 9e57b77ecd34..facd05cda26d 100644
--- a/drivers/hwmon/f71805f.c
+++ b/drivers/hwmon/f71805f.c
@@ -1503,7 +1503,6 @@ static int f71805f_remove(struct platform_device *pdev)
 
 static struct platform_driver f71805f_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= DRVNAME,
 	},
 	.probe		= f71805f_probe,
diff --git a/drivers/hwmon/f71882fg.c b/drivers/hwmon/f71882fg.c
index 03d8592810bf..2e5c6f46e442 100644
--- a/drivers/hwmon/f71882fg.c
+++ b/drivers/hwmon/f71882fg.c
@@ -369,7 +369,6 @@ static int f71882fg_remove(struct platform_device *pdev);
 
 static struct platform_driver f71882fg_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= DRVNAME,
 	},
 	.probe		= f71882fg_probe,
diff --git a/drivers/hwmon/i5k_amb.c b/drivers/hwmon/i5k_amb.c
index 6c0080a3b902..6b3d1972cef7 100644
--- a/drivers/hwmon/i5k_amb.c
+++ b/drivers/hwmon/i5k_amb.c
@@ -581,7 +581,6 @@ static int i5k_amb_remove(struct platform_device *pdev)
 
 static struct platform_driver i5k_amb_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = DRVNAME,
 	},
 	.probe = i5k_amb_probe,
diff --git a/drivers/hwmon/ibmpowernv.c b/drivers/hwmon/ibmpowernv.c
index 7c2c7be182f2..febe8175d36c 100644
--- a/drivers/hwmon/ibmpowernv.c
+++ b/drivers/hwmon/ibmpowernv.c
@@ -313,7 +313,6 @@ static struct platform_driver ibmpowernv_driver = {
 	.probe		= ibmpowernv_probe,
 	.id_table	= opal_sensor_driver_ids,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= DRVNAME,
 	},
 };
diff --git a/drivers/hwmon/iio_hwmon.c b/drivers/hwmon/iio_hwmon.c
index 980175628563..17ae2eb26ce2 100644
--- a/drivers/hwmon/iio_hwmon.c
+++ b/drivers/hwmon/iio_hwmon.c
@@ -177,7 +177,6 @@ MODULE_DEVICE_TABLE(of, iio_hwmon_of_match);
 static struct platform_driver __refdata iio_hwmon_driver = {
 	.driver = {
 		.name = "iio_hwmon",
-		.owner = THIS_MODULE,
 		.of_match_table = iio_hwmon_of_match,
 	},
 	.probe = iio_hwmon_probe,
diff --git a/drivers/hwmon/it87.c b/drivers/hwmon/it87.c
index a327fd3402a7..409116c52cc5 100644
--- a/drivers/hwmon/it87.c
+++ b/drivers/hwmon/it87.c
@@ -498,7 +498,6 @@ static void it87_init_device(struct platform_device *pdev);
 
 static struct platform_driver it87_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= DRVNAME,
 	},
 	.probe	= it87_probe,
diff --git a/drivers/hwmon/jz4740-hwmon.c b/drivers/hwmon/jz4740-hwmon.c
index 7488e36809c8..df9b3447f2a8 100644
--- a/drivers/hwmon/jz4740-hwmon.c
+++ b/drivers/hwmon/jz4740-hwmon.c
@@ -172,7 +172,6 @@ static struct platform_driver jz4740_hwmon_driver = {
 	.remove = jz4740_hwmon_remove,
 	.driver = {
 		.name = "jz4740-hwmon",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/hwmon/lm75.c b/drivers/hwmon/lm75.c
index 6753fd940c76..fe41d5ae7cb2 100644
--- a/drivers/hwmon/lm75.c
+++ b/drivers/hwmon/lm75.c
@@ -177,6 +177,10 @@ static struct attribute *lm75_attrs[] = {
 };
 ATTRIBUTE_GROUPS(lm75);
 
+static const struct thermal_zone_of_device_ops lm75_of_thermal_ops = {
+	.get_temp = lm75_read_temp,
+};
+
 /*-----------------------------------------------------------------------*/
 
 /* device probe and removal */
@@ -296,10 +300,9 @@ lm75_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	if (IS_ERR(data->hwmon_dev))
 		return PTR_ERR(data->hwmon_dev);
 
-	data->tz = thermal_zone_of_sensor_register(data->hwmon_dev,
-						   0,
+	data->tz = thermal_zone_of_sensor_register(data->hwmon_dev, 0,
 						   data->hwmon_dev,
-						   lm75_read_temp, NULL);
+						   &lm75_of_thermal_ops);
 	if (IS_ERR(data->tz))
 		data->tz = NULL;
 
diff --git a/drivers/hwmon/lm78.c b/drivers/hwmon/lm78.c
index 759661c7d480..539efe4ad991 100644
--- a/drivers/hwmon/lm78.c
+++ b/drivers/hwmon/lm78.c
@@ -836,7 +836,6 @@ static int lm78_isa_probe(struct platform_device *pdev)
 
 static struct platform_driver lm78_isa_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= "lm78",
 	},
 	.probe		= lm78_isa_probe,
diff --git a/drivers/hwmon/max197.c b/drivers/hwmon/max197.c
index 82128ad79a91..cb0dcfda958c 100644
--- a/drivers/hwmon/max197.c
+++ b/drivers/hwmon/max197.c
@@ -334,7 +334,6 @@ MODULE_DEVICE_TABLE(platform, max197_device_ids);
 static struct platform_driver max197_driver = {
 	.driver = {
 		.name = "max197",
-		.owner = THIS_MODULE,
 	},
 	.probe = max197_probe,
 	.remove = max197_remove,
diff --git a/drivers/hwmon/mc13783-adc.c b/drivers/hwmon/mc13783-adc.c
index ae00e60d856c..0c02f40eb0c1 100644
--- a/drivers/hwmon/mc13783-adc.c
+++ b/drivers/hwmon/mc13783-adc.c
@@ -267,7 +267,6 @@ MODULE_DEVICE_TABLE(platform, mc13783_adc_idtable);
 static struct platform_driver mc13783_adc_driver = {
 	.remove		= mc13783_adc_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= DRIVER_NAME,
 	},
 	.id_table	= mc13783_adc_idtable,
diff --git a/drivers/hwmon/menf21bmc_hwmon.c b/drivers/hwmon/menf21bmc_hwmon.c
index afc6b58eaa62..c29a4c3c6b9e 100644
--- a/drivers/hwmon/menf21bmc_hwmon.c
+++ b/drivers/hwmon/menf21bmc_hwmon.c
@@ -219,7 +219,6 @@ static struct platform_driver menf21bmc_hwmon = {
 	.probe		= menf21bmc_hwmon_probe,
 	.driver		= {
 		.name		= DRV_NAME,
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/hwmon/nct6683.c b/drivers/hwmon/nct6683.c
index 7710f4694ba1..f3830db02d46 100644
--- a/drivers/hwmon/nct6683.c
+++ b/drivers/hwmon/nct6683.c
@@ -1282,7 +1282,6 @@ static const struct dev_pm_ops nct6683_dev_pm_ops = {
 
 static struct platform_driver nct6683_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= DRVNAME,
 		.pm	= NCT6683_DEV_PM_OPS,
 	},
diff --git a/drivers/hwmon/nct6775.c b/drivers/hwmon/nct6775.c
index dc0df57200cd..1be41177b620 100644
--- a/drivers/hwmon/nct6775.c
+++ b/drivers/hwmon/nct6775.c
@@ -4080,7 +4080,6 @@ static const struct dev_pm_ops nct6775_dev_pm_ops = {
 
 static struct platform_driver nct6775_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= DRVNAME,
 		.pm	= NCT6775_DEV_PM_OPS,
 	},
diff --git a/drivers/hwmon/ntc_thermistor.c b/drivers/hwmon/ntc_thermistor.c
index 4ff89b2482e4..112e4d45e4a0 100644
--- a/drivers/hwmon/ntc_thermistor.c
+++ b/drivers/hwmon/ntc_thermistor.c
@@ -486,6 +486,10 @@ static const struct attribute_group ntc_attr_group = {
 	.attrs = ntc_attributes,
 };
 
+static const struct thermal_zone_of_device_ops ntc_of_thermal_ops = {
+	.get_temp = ntc_read_temp,
+};
+
 static int ntc_thermistor_probe(struct platform_device *pdev)
 {
 	const struct of_device_id *of_id =
@@ -579,7 +583,7 @@ static int ntc_thermistor_probe(struct platform_device *pdev)
 								pdev_id->name);
 
 	data->tz = thermal_zone_of_sensor_register(data->dev, 0, data->dev,
-						ntc_read_temp, NULL);
+						   &ntc_of_thermal_ops);
 	if (IS_ERR(data->tz)) {
 		dev_dbg(&pdev->dev, "Failed to register to thermal fw.\n");
 		data->tz = NULL;
@@ -609,7 +613,6 @@ static int ntc_thermistor_remove(struct platform_device *pdev)
 static struct platform_driver ntc_thermistor_driver = {
 	.driver = {
 		.name = "ntc-thermistor",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(ntc_match),
 	},
 	.probe = ntc_thermistor_probe,
diff --git a/drivers/hwmon/pc87360.c b/drivers/hwmon/pc87360.c
index 145f674c1d87..d50fbf93a737 100644
--- a/drivers/hwmon/pc87360.c
+++ b/drivers/hwmon/pc87360.c
@@ -244,7 +244,6 @@ static struct pc87360_data *pc87360_update_device(struct device *dev);
 
 static struct platform_driver pc87360_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= "pc87360",
 	},
 	.probe		= pc87360_probe,
diff --git a/drivers/hwmon/pc87427.c b/drivers/hwmon/pc87427.c
index 9e4684e747ea..cb9fdd37bd0d 100644
--- a/drivers/hwmon/pc87427.c
+++ b/drivers/hwmon/pc87427.c
@@ -1153,7 +1153,6 @@ static int pc87427_remove(struct platform_device *pdev)
 
 static struct platform_driver pc87427_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= DRVNAME,
 	},
 	.probe		= pc87427_probe,
diff --git a/drivers/hwmon/s3c-hwmon.c b/drivers/hwmon/s3c-hwmon.c
index 0674c13bbd4b..0c4710d35d16 100644
--- a/drivers/hwmon/s3c-hwmon.c
+++ b/drivers/hwmon/s3c-hwmon.c
@@ -378,7 +378,6 @@ static int s3c_hwmon_remove(struct platform_device *dev)
 static struct platform_driver s3c_hwmon_driver = {
 	.driver	= {
 		.name		= "s3c-hwmon",
-		.owner		= THIS_MODULE,
 	},
 	.probe		= s3c_hwmon_probe,
 	.remove		= s3c_hwmon_remove,
diff --git a/drivers/hwmon/sch5627.c b/drivers/hwmon/sch5627.c
index 0cc99fd83e8e..19f85c0da270 100644
--- a/drivers/hwmon/sch5627.c
+++ b/drivers/hwmon/sch5627.c
@@ -591,7 +591,6 @@ error:
 
 static struct platform_driver sch5627_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= DRVNAME,
 	},
 	.probe		= sch5627_probe,
diff --git a/drivers/hwmon/sch5636.c b/drivers/hwmon/sch5636.c
index 547b5c952eff..131a2815dbda 100644
--- a/drivers/hwmon/sch5636.c
+++ b/drivers/hwmon/sch5636.c
@@ -521,7 +521,6 @@ error:
 
 static struct platform_driver sch5636_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= DRVNAME,
 	},
 	.probe		= sch5636_probe,
diff --git a/drivers/hwmon/sht15.c b/drivers/hwmon/sht15.c
index 97cd45a8432c..d4f0935daaa1 100644
--- a/drivers/hwmon/sht15.c
+++ b/drivers/hwmon/sht15.c
@@ -1087,7 +1087,6 @@ MODULE_DEVICE_TABLE(platform, sht15_device_ids);
 static struct platform_driver sht15_driver = {
 	.driver = {
 		.name = "sht15",
-		.owner = THIS_MODULE,
 	},
 	.probe = sht15_probe,
 	.remove = sht15_remove,
diff --git a/drivers/hwmon/sis5595.c b/drivers/hwmon/sis5595.c
index bf1d7893d51c..45a028fb8851 100644
--- a/drivers/hwmon/sis5595.c
+++ b/drivers/hwmon/sis5595.c
@@ -215,7 +215,6 @@ static void sis5595_init_device(struct sis5595_data *data);
 
 static struct platform_driver sis5595_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= "sis5595",
 	},
 	.probe		= sis5595_probe,
diff --git a/drivers/hwmon/smsc47b397.c b/drivers/hwmon/smsc47b397.c
index 221f0931bf1c..6bd200756560 100644
--- a/drivers/hwmon/smsc47b397.c
+++ b/drivers/hwmon/smsc47b397.c
@@ -219,7 +219,6 @@ static int smsc47b397_probe(struct platform_device *pdev);
 
 static struct platform_driver smsc47b397_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= DRVNAME,
 	},
 	.probe		= smsc47b397_probe,
diff --git a/drivers/hwmon/smsc47m1.c b/drivers/hwmon/smsc47m1.c
index d7485659acc5..5d323186d2c1 100644
--- a/drivers/hwmon/smsc47m1.c
+++ b/drivers/hwmon/smsc47m1.c
@@ -849,7 +849,6 @@ static int __exit smsc47m1_remove(struct platform_device *pdev)
 
 static struct platform_driver smsc47m1_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= DRVNAME,
 	},
 	.remove		= __exit_p(smsc47m1_remove),
diff --git a/drivers/hwmon/tmp102.c b/drivers/hwmon/tmp102.c
index 51719956cc03..ba9f478f64ee 100644
--- a/drivers/hwmon/tmp102.c
+++ b/drivers/hwmon/tmp102.c
@@ -158,6 +158,10 @@ ATTRIBUTE_GROUPS(tmp102);
 #define TMP102_CONFIG  (TMP102_CONF_TM | TMP102_CONF_EM | TMP102_CONF_CR1)
 #define TMP102_CONFIG_RD_ONLY (TMP102_CONF_R0 | TMP102_CONF_R1 | TMP102_CONF_AL)
 
+static const struct thermal_zone_of_device_ops tmp102_of_thermal_ops = {
+	.get_temp = tmp102_read_temp,
+};
+
 static int tmp102_probe(struct i2c_client *client,
 				  const struct i2c_device_id *id)
 {
@@ -215,7 +219,7 @@ static int tmp102_probe(struct i2c_client *client,
 	}
 	tmp102->hwmon_dev = hwmon_dev;
 	tmp102->tz = thermal_zone_of_sensor_register(hwmon_dev, 0, hwmon_dev,
-						     tmp102_read_temp, NULL);
+						     &tmp102_of_thermal_ops);
 	if (IS_ERR(tmp102->tz))
 		tmp102->tz = NULL;
 
diff --git a/drivers/hwmon/twl4030-madc-hwmon.c b/drivers/hwmon/twl4030-madc-hwmon.c
index 9a0e2b8e8b94..b5caf7fdb487 100644
--- a/drivers/hwmon/twl4030-madc-hwmon.c
+++ b/drivers/hwmon/twl4030-madc-hwmon.c
@@ -107,7 +107,6 @@ static struct platform_driver twl4030_madc_hwmon_driver = {
 	.probe = twl4030_madc_hwmon_probe,
 	.driver = {
 		   .name = "twl4030_madc_hwmon",
-		   .owner = THIS_MODULE,
 		   },
 };
 
diff --git a/drivers/hwmon/ultra45_env.c b/drivers/hwmon/ultra45_env.c
index 7d4658636064..f2816c7c918f 100644
--- a/drivers/hwmon/ultra45_env.c
+++ b/drivers/hwmon/ultra45_env.c
@@ -314,7 +314,6 @@ MODULE_DEVICE_TABLE(of, env_match);
 static struct platform_driver env_driver = {
 	.driver = {
 		.name = "ultra45_env",
-		.owner = THIS_MODULE,
 		.of_match_table = env_match,
 	},
 	.probe		= env_probe,
diff --git a/drivers/hwmon/vexpress.c b/drivers/hwmon/vexpress.c
index c53619086f33..cf1848b8fb32 100644
--- a/drivers/hwmon/vexpress.c
+++ b/drivers/hwmon/vexpress.c
@@ -247,7 +247,6 @@ static struct platform_driver vexpress_hwmon_driver = {
 	.probe = vexpress_hwmon_probe,
 	.driver	= {
 		.name = DRVNAME,
-		.owner = THIS_MODULE,
 		.of_match_table = vexpress_hwmon_of_match,
 	},
 };
diff --git a/drivers/hwmon/via-cputemp.c b/drivers/hwmon/via-cputemp.c
index 8df43c51de2c..ac91c07e3f90 100644
--- a/drivers/hwmon/via-cputemp.c
+++ b/drivers/hwmon/via-cputemp.c
@@ -205,7 +205,6 @@ static int via_cputemp_remove(struct platform_device *pdev)
 
 static struct platform_driver via_cputemp_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = DRVNAME,
 	},
 	.probe = via_cputemp_probe,
diff --git a/drivers/hwmon/via686a.c b/drivers/hwmon/via686a.c
index babd732b4e18..40dd93c8f9f4 100644
--- a/drivers/hwmon/via686a.c
+++ b/drivers/hwmon/via686a.c
@@ -670,7 +670,6 @@ static const struct attribute_group via686a_group = {
 
 static struct platform_driver via686a_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= "via686a",
 	},
 	.probe		= via686a_probe,
diff --git a/drivers/hwmon/vt1211.c b/drivers/hwmon/vt1211.c
index 3ea57c3504e2..3a6bfa51cb94 100644
--- a/drivers/hwmon/vt1211.c
+++ b/drivers/hwmon/vt1211.c
@@ -1233,7 +1233,6 @@ static int vt1211_remove(struct platform_device *pdev)
 
 static struct platform_driver vt1211_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name  = DRVNAME,
 	},
 	.probe  = vt1211_probe,
diff --git a/drivers/hwmon/vt8231.c b/drivers/hwmon/vt8231.c
index b3babe3326fb..cb69a8c2ed5b 100644
--- a/drivers/hwmon/vt8231.c
+++ b/drivers/hwmon/vt8231.c
@@ -759,7 +759,6 @@ static const struct attribute_group vt8231_group = {
 
 static struct platform_driver vt8231_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= "vt8231",
 	},
 	.probe	= vt8231_probe,
diff --git a/drivers/hwmon/w83627ehf.c b/drivers/hwmon/w83627ehf.c
index f0ab61db7a0d..b10353b31806 100644
--- a/drivers/hwmon/w83627ehf.c
+++ b/drivers/hwmon/w83627ehf.c
@@ -2705,7 +2705,6 @@ static const struct dev_pm_ops w83627ehf_dev_pm_ops = {
 
 static struct platform_driver w83627ehf_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= DRVNAME,
 		.pm	= W83627EHF_DEV_PM_OPS,
 	},
diff --git a/drivers/hwmon/w83627hf.c b/drivers/hwmon/w83627hf.c
index 2f55973a8c4c..721295b9a051 100644
--- a/drivers/hwmon/w83627hf.c
+++ b/drivers/hwmon/w83627hf.c
@@ -474,7 +474,6 @@ static const struct dev_pm_ops w83627hf_dev_pm_ops = {
 
 static struct platform_driver w83627hf_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= DRVNAME,
 		.pm	= W83627HF_DEV_PM_OPS,
 	},
diff --git a/drivers/hwmon/w83781d.c b/drivers/hwmon/w83781d.c
index 84911616d8c0..54848fdd181e 100644
--- a/drivers/hwmon/w83781d.c
+++ b/drivers/hwmon/w83781d.c
@@ -1839,7 +1839,6 @@ w83781d_isa_remove(struct platform_device *pdev)
 
 static struct platform_driver w83781d_isa_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "w83781d",
 	},
 	.probe = w83781d_isa_probe,
diff --git a/drivers/hwmon/wm831x-hwmon.c b/drivers/hwmon/wm831x-hwmon.c
index 3e6a3195cd11..a16cce72e4e2 100644
--- a/drivers/hwmon/wm831x-hwmon.c
+++ b/drivers/hwmon/wm831x-hwmon.c
@@ -154,7 +154,6 @@ static struct platform_driver wm831x_hwmon_driver = {
 	.probe = wm831x_hwmon_probe,
 	.driver = {
 		.name = "wm831x-hwmon",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/hwmon/wm8350-hwmon.c b/drivers/hwmon/wm8350-hwmon.c
index 90e3d918e597..31af438ffa88 100644
--- a/drivers/hwmon/wm8350-hwmon.c
+++ b/drivers/hwmon/wm8350-hwmon.c
@@ -93,7 +93,6 @@ static struct platform_driver wm8350_hwmon_driver = {
 	.probe = wm8350_hwmon_probe,
 	.driver = {
 		.name = "wm8350-hwmon",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/hwspinlock/omap_hwspinlock.c b/drivers/hwspinlock/omap_hwspinlock.c
index c1e2cd4d85fe..47a275c6ece1 100644
--- a/drivers/hwspinlock/omap_hwspinlock.c
+++ b/drivers/hwspinlock/omap_hwspinlock.c
@@ -179,7 +179,6 @@ static struct platform_driver omap_hwspinlock_driver = {
 	.remove		= omap_hwspinlock_remove,
 	.driver		= {
 		.name	= "omap_hwspinlock",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/hwspinlock/u8500_hsem.c b/drivers/hwspinlock/u8500_hsem.c
index 401c33bcdb45..e93eabbd660f 100644
--- a/drivers/hwspinlock/u8500_hsem.c
+++ b/drivers/hwspinlock/u8500_hsem.c
@@ -175,7 +175,6 @@ static struct platform_driver u8500_hsem_driver = {
 	.remove		= u8500_hsem_remove,
 	.driver		= {
 		.name	= "u8500_hsem",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/i2c/Kconfig b/drivers/i2c/Kconfig
index b51a402752c4..8c9e619f3026 100644
--- a/drivers/i2c/Kconfig
+++ b/drivers/i2c/Kconfig
@@ -110,6 +110,16 @@ config I2C_STUB
 
 	  If you don't know what to do here, definitely say N.
 
+config I2C_SLAVE
+	bool "I2C slave support"
+
+if I2C_SLAVE
+
+config I2C_SLAVE_EEPROM
+	tristate "I2C eeprom slave driver"
+
+endif
+
 config I2C_DEBUG_CORE
 	bool "I2C Core debugging messages"
 	help
diff --git a/drivers/i2c/Makefile b/drivers/i2c/Makefile
index 1722f50f2473..45095b3d16a9 100644
--- a/drivers/i2c/Makefile
+++ b/drivers/i2c/Makefile
@@ -9,6 +9,7 @@ obj-$(CONFIG_I2C_CHARDEV)	+= i2c-dev.o
 obj-$(CONFIG_I2C_MUX)		+= i2c-mux.o
 obj-y				+= algos/ busses/ muxes/
 obj-$(CONFIG_I2C_STUB)		+= i2c-stub.o
+obj-$(CONFIG_I2C_SLAVE_EEPROM)	+= i2c-slave-eeprom.o
 
 ccflags-$(CONFIG_I2C_DEBUG_CORE) := -DDEBUG
 CFLAGS_i2c-core.o := -Wno-deprecated-declarations
diff --git a/drivers/i2c/busses/Kconfig b/drivers/i2c/busses/Kconfig
index b4d135cc2f39..31e8308ba899 100644
--- a/drivers/i2c/busses/Kconfig
+++ b/drivers/i2c/busses/Kconfig
@@ -123,6 +123,7 @@ config I2C_I801
 	    Wildcat Point-LP (PCH)
 	    BayTrail (SOC)
 	    Sunrise Point-H (PCH)
+	    Sunrise Point-LP (PCH)
 
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-i801.
@@ -523,6 +524,16 @@ config I2C_IBM_IIC
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-ibm_iic.
 
+config I2C_IMG
+	tristate "Imagination Technologies I2C SCB Controller"
+	depends on MIPS || METAG || COMPILE_TEST
+	help
+	  Say Y here if you want to use the IMG I2C SCB controller,
+	  available on the TZ1090 and other IMG SoCs.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called i2c-img-scb.
+
 config I2C_IMX
 	tristate "IMX I2C interface"
 	depends on ARCH_MXC
@@ -553,6 +564,13 @@ config I2C_KEMPLD
 	  This driver can also be built as a module. If so, the module
 	  will be called i2c-kempld.
 
+config I2C_MESON
+	tristate "Amlogic Meson I2C controller"
+	depends on ARCH_MESON
+	help
+	  If you say yes to this option, support will be included for the
+	  I2C interface on the Amlogic Meson family of SoCs.
+
 config I2C_MPC
 	tristate "MPC107/824x/85xx/512x/52xx/83xx/86xx"
 	depends on PPC
@@ -702,7 +720,7 @@ config I2C_RIIC
 
 config I2C_RK3X
 	tristate "Rockchip RK3xxx I2C adapter"
-	depends on OF
+	depends on OF && COMMON_CLK
 	help
 	  Say Y here to include support for the I2C adapter in Rockchip RK3xxx
 	  SoCs.
@@ -735,6 +753,7 @@ config I2C_SH7760
 
 config I2C_SH_MOBILE
 	tristate "SuperH Mobile I2C Controller"
+	depends on HAS_DMA
 	depends on SUPERH || ARCH_SHMOBILE || COMPILE_TEST
 	help
 	  If you say yes to this option, support will be included for the
@@ -1054,4 +1073,15 @@ config SCx200_ACB
 	  This support is also available as a module.  If so, the module
 	  will be called scx200_acb.
 
+config I2C_OPAL
+	tristate "IBM OPAL I2C driver"
+	depends on PPC_POWERNV
+	default y
+	help
+	  This exposes the PowerNV platform i2c busses to the linux i2c layer,
+	  the driver is based on the OPAL interfaces.
+
+	  This driver can also be built as a module. If so, the module will be
+	  called as i2c-opal.
+
 endmenu
diff --git a/drivers/i2c/busses/Makefile b/drivers/i2c/busses/Makefile
index cdac7f15eab5..56388f658d2f 100644
--- a/drivers/i2c/busses/Makefile
+++ b/drivers/i2c/busses/Makefile
@@ -50,9 +50,11 @@ obj-$(CONFIG_I2C_GPIO)		+= i2c-gpio.o
 obj-$(CONFIG_I2C_HIGHLANDER)	+= i2c-highlander.o
 obj-$(CONFIG_I2C_HIX5HD2)	+= i2c-hix5hd2.o
 obj-$(CONFIG_I2C_IBM_IIC)	+= i2c-ibm_iic.o
+obj-$(CONFIG_I2C_IMG)		+= i2c-img-scb.o
 obj-$(CONFIG_I2C_IMX)		+= i2c-imx.o
 obj-$(CONFIG_I2C_IOP3XX)	+= i2c-iop3xx.o
 obj-$(CONFIG_I2C_KEMPLD)	+= i2c-kempld.o
+obj-$(CONFIG_I2C_MESON)		+= i2c-meson.o
 obj-$(CONFIG_I2C_MPC)		+= i2c-mpc.o
 obj-$(CONFIG_I2C_MV64XXX)	+= i2c-mv64xxx.o
 obj-$(CONFIG_I2C_MXS)		+= i2c-mxs.o
@@ -100,6 +102,7 @@ obj-$(CONFIG_I2C_ACORN)		+= i2c-acorn.o
 obj-$(CONFIG_I2C_BCM_KONA)	+= i2c-bcm-kona.o
 obj-$(CONFIG_I2C_CROS_EC_TUNNEL)	+= i2c-cros-ec-tunnel.o
 obj-$(CONFIG_I2C_ELEKTOR)	+= i2c-elektor.o
+obj-$(CONFIG_I2C_OPAL)		+= i2c-opal.o
 obj-$(CONFIG_I2C_PCA_ISA)	+= i2c-pca-isa.o
 obj-$(CONFIG_I2C_SIBYTE)	+= i2c-sibyte.o
 obj-$(CONFIG_SCx200_ACB)	+= scx200_acb.o
diff --git a/drivers/i2c/busses/i2c-at91.c b/drivers/i2c/busses/i2c-at91.c
index e05a672db3e5..636fd2efad88 100644
--- a/drivers/i2c/busses/i2c-at91.c
+++ b/drivers/i2c/busses/i2c-at91.c
@@ -31,10 +31,13 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/platform_data/dma-atmel.h>
+#include <linux/pm_runtime.h>
+#include <linux/pinctrl/consumer.h>
 
 #define DEFAULT_TWI_CLK_HZ		100000		/* max 400 Kbits/s */
 #define AT91_I2C_TIMEOUT	msecs_to_jiffies(100)	/* transfer timeout */
 #define AT91_I2C_DMA_THRESHOLD	8			/* enable DMA if transfer size is bigger than this threshold */
+#define AUTOSUSPEND_TIMEOUT		2000
 
 /* AT91 TWI register definitions */
 #define	AT91_TWI_CR		0x0000	/* Control Register */
@@ -72,7 +75,6 @@ struct at91_twi_pdata {
 	unsigned clk_max_div;
 	unsigned clk_offset;
 	bool has_unre_flag;
-	bool has_dma_support;
 	struct at_dma_slave dma_slave;
 };
 
@@ -481,6 +483,10 @@ static int at91_twi_xfer(struct i2c_adapter *adap, struct i2c_msg *msg, int num)
 
 	dev_dbg(&adap->dev, "at91_xfer: processing %d messages:\n", num);
 
+	ret = pm_runtime_get_sync(dev->dev);
+	if (ret < 0)
+		goto out;
+
 	/*
 	 * The hardware can handle at most two messages concatenated by a
 	 * repeated start via it's internal address feature.
@@ -488,18 +494,21 @@ static int at91_twi_xfer(struct i2c_adapter *adap, struct i2c_msg *msg, int num)
 	if (num > 2) {
 		dev_err(dev->dev,
 			"cannot handle more than two concatenated messages.\n");
-		return 0;
+		ret = 0;
+		goto out;
 	} else if (num == 2) {
 		int internal_address = 0;
 		int i;
 
 		if (msg->flags & I2C_M_RD) {
 			dev_err(dev->dev, "first transfer must be write.\n");
-			return -EINVAL;
+			ret = -EINVAL;
+			goto out;
 		}
 		if (msg->len > 3) {
 			dev_err(dev->dev, "first message size must be <= 3.\n");
-			return -EINVAL;
+			ret = -EINVAL;
+			goto out;
 		}
 
 		/* 1st msg is put into the internal address, start with 2nd */
@@ -523,7 +532,12 @@ static int at91_twi_xfer(struct i2c_adapter *adap, struct i2c_msg *msg, int num)
 
 	ret = at91_do_twi_transfer(dev);
 
-	return (ret < 0) ? ret : num;
+	ret = (ret < 0) ? ret : num;
+out:
+	pm_runtime_mark_last_busy(dev->dev);
+	pm_runtime_put_autosuspend(dev->dev);
+
+	return ret;
 }
 
 static u32 at91_twi_func(struct i2c_adapter *adapter)
@@ -541,35 +555,30 @@ static struct at91_twi_pdata at91rm9200_config = {
 	.clk_max_div = 5,
 	.clk_offset = 3,
 	.has_unre_flag = true,
-	.has_dma_support = false,
 };
 
 static struct at91_twi_pdata at91sam9261_config = {
 	.clk_max_div = 5,
 	.clk_offset = 4,
 	.has_unre_flag = false,
-	.has_dma_support = false,
 };
 
 static struct at91_twi_pdata at91sam9260_config = {
 	.clk_max_div = 7,
 	.clk_offset = 4,
 	.has_unre_flag = false,
-	.has_dma_support = false,
 };
 
 static struct at91_twi_pdata at91sam9g20_config = {
 	.clk_max_div = 7,
 	.clk_offset = 4,
 	.has_unre_flag = false,
-	.has_dma_support = false,
 };
 
 static struct at91_twi_pdata at91sam9g10_config = {
 	.clk_max_div = 7,
 	.clk_offset = 4,
 	.has_unre_flag = false,
-	.has_dma_support = false,
 };
 
 static const struct platform_device_id at91_twi_devtypes[] = {
@@ -598,7 +607,6 @@ static struct at91_twi_pdata at91sam9x5_config = {
 	.clk_max_div = 7,
 	.clk_offset = 4,
 	.has_unre_flag = false,
-	.has_dma_support = true,
 };
 
 static const struct of_device_id atmel_twi_dt_ids[] = {
@@ -627,30 +635,11 @@ static const struct of_device_id atmel_twi_dt_ids[] = {
 MODULE_DEVICE_TABLE(of, atmel_twi_dt_ids);
 #endif
 
-static bool filter(struct dma_chan *chan, void *pdata)
-{
-	struct at91_twi_pdata *sl_pdata = pdata;
-	struct at_dma_slave *sl;
-
-	if (!sl_pdata)
-		return false;
-
-	sl = &sl_pdata->dma_slave;
-	if (sl && (sl->dma_dev == chan->device->dev)) {
-		chan->private = sl;
-		return true;
-	} else {
-		return false;
-	}
-}
-
 static int at91_twi_configure_dma(struct at91_twi_dev *dev, u32 phy_addr)
 {
 	int ret = 0;
-	struct at91_twi_pdata *pdata = dev->pdata;
 	struct dma_slave_config slave_config;
 	struct at91_twi_dma *dma = &dev->dma;
-	dma_cap_mask_t mask;
 
 	memset(&slave_config, 0, sizeof(slave_config));
 	slave_config.src_addr = (dma_addr_t)phy_addr + AT91_TWI_RHR;
@@ -661,22 +650,17 @@ static int at91_twi_configure_dma(struct at91_twi_dev *dev, u32 phy_addr)
 	slave_config.dst_maxburst = 1;
 	slave_config.device_fc = false;
 
-	dma_cap_zero(mask);
-	dma_cap_set(DMA_SLAVE, mask);
-
-	dma->chan_tx = dma_request_slave_channel_compat(mask, filter, pdata,
-							dev->dev, "tx");
-	if (!dma->chan_tx) {
-		dev_err(dev->dev, "can't get a DMA channel for tx\n");
-		ret = -EBUSY;
+	dma->chan_tx = dma_request_slave_channel_reason(dev->dev, "tx");
+	if (IS_ERR(dma->chan_tx)) {
+		ret = PTR_ERR(dma->chan_tx);
+		dma->chan_tx = NULL;
 		goto error;
 	}
 
-	dma->chan_rx = dma_request_slave_channel_compat(mask, filter, pdata,
-							dev->dev, "rx");
-	if (!dma->chan_rx) {
-		dev_err(dev->dev, "can't get a DMA channel for rx\n");
-		ret = -EBUSY;
+	dma->chan_rx = dma_request_slave_channel_reason(dev->dev, "rx");
+	if (IS_ERR(dma->chan_rx)) {
+		ret = PTR_ERR(dma->chan_rx);
+		dma->chan_rx = NULL;
 		goto error;
 	}
 
@@ -697,6 +681,7 @@ static int at91_twi_configure_dma(struct at91_twi_dev *dev, u32 phy_addr)
 	sg_init_table(&dma->sg, 1);
 	dma->buf_mapped = false;
 	dma->xfer_in_progress = false;
+	dev->use_dma = true;
 
 	dev_info(dev->dev, "using %s (tx) and %s (rx) for DMA transfers\n",
 		 dma_chan_name(dma->chan_tx), dma_chan_name(dma->chan_rx));
@@ -704,7 +689,8 @@ static int at91_twi_configure_dma(struct at91_twi_dev *dev, u32 phy_addr)
 	return ret;
 
 error:
-	dev_info(dev->dev, "can't use DMA\n");
+	if (ret != -EPROBE_DEFER)
+		dev_info(dev->dev, "can't use DMA, error %d\n", ret);
 	if (dma->chan_rx)
 		dma_release_channel(dma->chan_rx);
 	if (dma->chan_tx)
@@ -772,9 +758,10 @@ static int at91_twi_probe(struct platform_device *pdev)
 	}
 	clk_prepare_enable(dev->clk);
 
-	if (dev->pdata->has_dma_support) {
-		if (at91_twi_configure_dma(dev, phy_addr) == 0)
-			dev->use_dma = true;
+	if (dev->dev->of_node) {
+		rc = at91_twi_configure_dma(dev, phy_addr);
+		if (rc == -EPROBE_DEFER)
+			return rc;
 	}
 
 	rc = of_property_read_u32(dev->dev->of_node, "clock-frequency",
@@ -795,11 +782,20 @@ static int at91_twi_probe(struct platform_device *pdev)
 	dev->adapter.timeout = AT91_I2C_TIMEOUT;
 	dev->adapter.dev.of_node = pdev->dev.of_node;
 
+	pm_runtime_set_autosuspend_delay(dev->dev, AUTOSUSPEND_TIMEOUT);
+	pm_runtime_use_autosuspend(dev->dev);
+	pm_runtime_set_active(dev->dev);
+	pm_runtime_enable(dev->dev);
+
 	rc = i2c_add_numbered_adapter(&dev->adapter);
 	if (rc) {
 		dev_err(dev->dev, "Adapter %s registration failed\n",
 			dev->adapter.name);
 		clk_disable_unprepare(dev->clk);
+
+		pm_runtime_disable(dev->dev);
+		pm_runtime_set_suspended(dev->dev);
+
 		return rc;
 	}
 
@@ -814,6 +810,9 @@ static int at91_twi_remove(struct platform_device *pdev)
 	i2c_del_adapter(&dev->adapter);
 	clk_disable_unprepare(dev->clk);
 
+	pm_runtime_disable(dev->dev);
+	pm_runtime_set_suspended(dev->dev);
+
 	return 0;
 }
 
@@ -823,7 +822,9 @@ static int at91_twi_runtime_suspend(struct device *dev)
 {
 	struct at91_twi_dev *twi_dev = dev_get_drvdata(dev);
 
-	clk_disable(twi_dev->clk);
+	clk_disable_unprepare(twi_dev->clk);
+
+	pinctrl_pm_select_sleep_state(dev);
 
 	return 0;
 }
@@ -832,10 +833,38 @@ static int at91_twi_runtime_resume(struct device *dev)
 {
 	struct at91_twi_dev *twi_dev = dev_get_drvdata(dev);
 
-	return clk_enable(twi_dev->clk);
+	pinctrl_pm_select_default_state(dev);
+
+	return clk_prepare_enable(twi_dev->clk);
+}
+
+static int at91_twi_suspend_noirq(struct device *dev)
+{
+	if (!pm_runtime_status_suspended(dev))
+		at91_twi_runtime_suspend(dev);
+
+	return 0;
+}
+
+static int at91_twi_resume_noirq(struct device *dev)
+{
+	int ret;
+
+	if (!pm_runtime_status_suspended(dev)) {
+		ret = at91_twi_runtime_resume(dev);
+		if (ret)
+			return ret;
+	}
+
+	pm_runtime_mark_last_busy(dev);
+	pm_request_autosuspend(dev);
+
+	return 0;
 }
 
 static const struct dev_pm_ops at91_twi_pm = {
+	.suspend_noirq	= at91_twi_suspend_noirq,
+	.resume_noirq	= at91_twi_resume_noirq,
 	.runtime_suspend	= at91_twi_runtime_suspend,
 	.runtime_resume		= at91_twi_runtime_resume,
 };
@@ -851,7 +880,6 @@ static struct platform_driver at91_twi_driver = {
 	.id_table	= at91_twi_devtypes,
 	.driver		= {
 		.name	= "at91_i2c",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(atmel_twi_dt_ids),
 		.pm	= at91_twi_pm_ops,
 	},
diff --git a/drivers/i2c/busses/i2c-au1550.c b/drivers/i2c/busses/i2c-au1550.c
index 6f8c0756e350..a6aae84e5706 100644
--- a/drivers/i2c/busses/i2c-au1550.c
+++ b/drivers/i2c/busses/i2c-au1550.c
@@ -413,7 +413,6 @@ static const struct dev_pm_ops i2c_au1550_pmops = {
 static struct platform_driver au1xpsc_smbus_driver = {
 	.driver = {
 		.name	= "au1xpsc_smbus",
-		.owner	= THIS_MODULE,
 		.pm	= AU1XPSC_SMBUS_PMOPS,
 	},
 	.probe		= i2c_au1550_probe,
diff --git a/drivers/i2c/busses/i2c-bcm-kona.c b/drivers/i2c/busses/i2c-bcm-kona.c
index 18a74a6751a9..2c9d9b1c8e64 100644
--- a/drivers/i2c/busses/i2c-bcm-kona.c
+++ b/drivers/i2c/busses/i2c-bcm-kona.c
@@ -895,7 +895,6 @@ MODULE_DEVICE_TABLE(of, bcm_kona_i2c_of_match);
 static struct platform_driver bcm_kona_i2c_driver = {
 	.driver = {
 		   .name = "bcm-kona-i2c",
-		   .owner = THIS_MODULE,
 		   .of_match_table = bcm_kona_i2c_of_match,
 		   },
 	.probe = bcm_kona_i2c_probe,
diff --git a/drivers/i2c/busses/i2c-bcm2835.c b/drivers/i2c/busses/i2c-bcm2835.c
index 4b8ecd0b3661..5d6feb937b9d 100644
--- a/drivers/i2c/busses/i2c-bcm2835.c
+++ b/drivers/i2c/busses/i2c-bcm2835.c
@@ -313,7 +313,6 @@ static struct platform_driver bcm2835_i2c_driver = {
 	.remove		= bcm2835_i2c_remove,
 	.driver		= {
 		.name	= "i2c-bcm2835",
-		.owner	= THIS_MODULE,
 		.of_match_table = bcm2835_i2c_of_match,
 	},
 };
diff --git a/drivers/i2c/busses/i2c-bfin-twi.c b/drivers/i2c/busses/i2c-bfin-twi.c
index 067c1615e968..af162b4c7a6d 100644
--- a/drivers/i2c/busses/i2c-bfin-twi.c
+++ b/drivers/i2c/busses/i2c-bfin-twi.c
@@ -717,7 +717,6 @@ static struct platform_driver i2c_bfin_twi_driver = {
 	.remove		= i2c_bfin_twi_remove,
 	.driver		= {
 		.name	= "i2c-bfin-twi",
-		.owner	= THIS_MODULE,
 		.pm	= I2C_BFIN_TWI_PM_OPS,
 	},
 };
diff --git a/drivers/i2c/busses/i2c-cadence.c b/drivers/i2c/busses/i2c-cadence.c
index c604f4c3ac0d..626f74ecd4be 100644
--- a/drivers/i2c/busses/i2c-cadence.c
+++ b/drivers/i2c/busses/i2c-cadence.c
@@ -901,7 +901,6 @@ MODULE_DEVICE_TABLE(of, cdns_i2c_of_match);
 static struct platform_driver cdns_i2c_drv = {
 	.driver = {
 		.name  = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = cdns_i2c_of_match,
 		.pm = &cdns_i2c_dev_pm_ops,
 	},
diff --git a/drivers/i2c/busses/i2c-cbus-gpio.c b/drivers/i2c/busses/i2c-cbus-gpio.c
index bdf040fd8675..b4f91e48948a 100644
--- a/drivers/i2c/busses/i2c-cbus-gpio.c
+++ b/drivers/i2c/busses/i2c-cbus-gpio.c
@@ -287,7 +287,6 @@ static struct platform_driver cbus_i2c_driver = {
 	.probe	= cbus_i2c_probe,
 	.remove	= cbus_i2c_remove,
 	.driver	= {
-		.owner	= THIS_MODULE,
 		.name	= "i2c-cbus-gpio",
 		.of_match_table = of_match_ptr(i2c_cbus_dt_ids),
 	},
diff --git a/drivers/i2c/busses/i2c-cpm.c b/drivers/i2c/busses/i2c-cpm.c
index 5bdbc71698d0..2d466538b2e2 100644
--- a/drivers/i2c/busses/i2c-cpm.c
+++ b/drivers/i2c/busses/i2c-cpm.c
@@ -716,7 +716,6 @@ static struct platform_driver cpm_i2c_driver = {
 	.remove		= cpm_i2c_remove,
 	.driver = {
 		.name = "fsl-i2c-cpm",
-		.owner = THIS_MODULE,
 		.of_match_table = cpm_i2c_match,
 	},
 };
diff --git a/drivers/i2c/busses/i2c-davinci.c b/drivers/i2c/busses/i2c-davinci.c
index 01f0cd87a4a5..6dc7ff5d3d9a 100644
--- a/drivers/i2c/busses/i2c-davinci.c
+++ b/drivers/i2c/busses/i2c-davinci.c
@@ -368,8 +368,7 @@ i2c_davinci_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg, int stop)
 		flag |= DAVINCI_I2C_MDR_STP;
 	davinci_i2c_write_reg(dev, DAVINCI_I2C_MDR_REG, flag);
 
-	r = wait_for_completion_interruptible_timeout(&dev->cmd_complete,
-						      dev->adapter.timeout);
+	r = wait_for_completion_timeout(&dev->cmd_complete, dev->adapter.timeout);
 	if (r == 0) {
 		dev_err(dev->dev, "controller timed out\n");
 		davinci_i2c_recover_bus(dev);
@@ -380,7 +379,6 @@ i2c_davinci_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg, int stop)
 	if (dev->buf_len) {
 		/* This should be 0 if all bytes were transferred
 		 * or dev->cmd_err denotes an error.
-		 * A signal may have aborted the transfer.
 		 */
 		if (r >= 0) {
 			dev_err(dev->dev, "abnormal termination buf_len=%i\n",
@@ -634,13 +632,17 @@ static int davinci_i2c_probe(struct platform_device *pdev)
 {
 	struct davinci_i2c_dev *dev;
 	struct i2c_adapter *adap;
-	struct resource *mem, *irq;
-	int r;
-
-	irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (!irq) {
-		dev_err(&pdev->dev, "no irq resource?\n");
-		return -ENODEV;
+	struct resource *mem;
+	int r, irq;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq <= 0) {
+		if (!irq)
+			irq = -ENXIO;
+		if (irq != -EPROBE_DEFER)
+			dev_err(&pdev->dev,
+				"can't get irq resource ret=%d\n", irq);
+		return irq;
 	}
 
 	dev = devm_kzalloc(&pdev->dev, sizeof(struct davinci_i2c_dev),
@@ -655,7 +657,7 @@ static int davinci_i2c_probe(struct platform_device *pdev)
 	init_completion(&dev->xfr_complete);
 #endif
 	dev->dev = &pdev->dev;
-	dev->irq = irq->start;
+	dev->irq = irq;
 	dev->pdata = dev_get_platdata(&pdev->dev);
 	platform_set_drvdata(pdev, dev);
 
@@ -787,7 +789,6 @@ static struct platform_driver davinci_i2c_driver = {
 	.remove		= davinci_i2c_remove,
 	.driver		= {
 		.name	= "i2c_davinci",
-		.owner	= THIS_MODULE,
 		.pm	= davinci_i2c_pm_ops,
 		.of_match_table = davinci_i2c_of_match,
 	},
diff --git a/drivers/i2c/busses/i2c-designware-platdrv.c b/drivers/i2c/busses/i2c-designware-platdrv.c
index 373dd4d47765..2b463c313e4e 100644
--- a/drivers/i2c/busses/i2c-designware-platdrv.c
+++ b/drivers/i2c/busses/i2c-designware-platdrv.c
@@ -327,7 +327,6 @@ static struct platform_driver dw_i2c_driver = {
 	.remove = dw_i2c_remove,
 	.driver		= {
 		.name	= "i2c_designware",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(dw_i2c_of_match),
 		.acpi_match_table = ACPI_PTR(dw_i2c_acpi_match),
 		.pm	= &dw_i2c_dev_pm_ops,
diff --git a/drivers/i2c/busses/i2c-efm32.c b/drivers/i2c/busses/i2c-efm32.c
index 10b8323b08d4..8eff62738877 100644
--- a/drivers/i2c/busses/i2c-efm32.c
+++ b/drivers/i2c/busses/i2c-efm32.c
@@ -473,7 +473,6 @@ static struct platform_driver efm32_i2c_driver = {
 
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = efm32_i2c_dt_ids,
 	},
 };
diff --git a/drivers/i2c/busses/i2c-exynos5.c b/drivers/i2c/busses/i2c-exynos5.c
index 81e6263cd7da..b29c7500461a 100644
--- a/drivers/i2c/busses/i2c-exynos5.c
+++ b/drivers/i2c/busses/i2c-exynos5.c
@@ -457,7 +457,7 @@ static irqreturn_t exynos5_i2c_irq(int irqno, void *dev_id)
 			goto stop;
 		} else if (int_status & HSI2C_INT_TIMEOUT) {
 			dev_dbg(i2c->dev, "Accessing device timed out\n");
-			i2c->state = -EAGAIN;
+			i2c->state = -ETIMEDOUT;
 			goto stop;
 		}
 	} else if (int_status & HSI2C_INT_I2C) {
@@ -476,7 +476,7 @@ static irqreturn_t exynos5_i2c_irq(int irqno, void *dev_id)
 			goto stop;
 		} else if (trans_status & HSI2C_TIMEOUT_AUTO) {
 			dev_dbg(i2c->dev, "Accessing device timed out\n");
-			i2c->state = -EAGAIN;
+			i2c->state = -ETIMEDOUT;
 			goto stop;
 		} else if (trans_status & HSI2C_TRANS_DONE) {
 			i2c->trans_done = 1;
@@ -861,7 +861,6 @@ static struct platform_driver exynos5_i2c_driver = {
 	.probe		= exynos5_i2c_probe,
 	.remove		= exynos5_i2c_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "exynos5-hsi2c",
 		.pm	= &exynos5_i2c_dev_pm_ops,
 		.of_match_table = exynos5_i2c_match,
diff --git a/drivers/i2c/busses/i2c-gpio.c b/drivers/i2c/busses/i2c-gpio.c
index 933f1e453e41..34cfc0ebdcb9 100644
--- a/drivers/i2c/busses/i2c-gpio.c
+++ b/drivers/i2c/busses/i2c-gpio.c
@@ -260,7 +260,6 @@ MODULE_DEVICE_TABLE(of, i2c_gpio_dt_ids);
 static struct platform_driver i2c_gpio_driver = {
 	.driver		= {
 		.name	= "i2c-gpio",
-		.owner	= THIS_MODULE,
 		.of_match_table	= of_match_ptr(i2c_gpio_dt_ids),
 	},
 	.probe		= i2c_gpio_probe,
diff --git a/drivers/i2c/busses/i2c-highlander.c b/drivers/i2c/busses/i2c-highlander.c
index 512fcfabc18e..56dc69e7349f 100644
--- a/drivers/i2c/busses/i2c-highlander.c
+++ b/drivers/i2c/busses/i2c-highlander.c
@@ -456,7 +456,6 @@ static int highlander_i2c_remove(struct platform_device *pdev)
 static struct platform_driver highlander_i2c_driver = {
 	.driver		= {
 		.name	= "i2c-highlander",
-		.owner	= THIS_MODULE,
 	},
 
 	.probe		= highlander_i2c_probe,
diff --git a/drivers/i2c/busses/i2c-i801.c b/drivers/i2c/busses/i2c-i801.c
index 6ab4f1cb21f3..8fafb254e42a 100644
--- a/drivers/i2c/busses/i2c-i801.c
+++ b/drivers/i2c/busses/i2c-i801.c
@@ -2,7 +2,7 @@
     Copyright (c) 1998 - 2002  Frodo Looijaard <frodol@dds.nl>,
     Philip Edelbrock <phil@netroedge.com>, and Mark D. Studebaker
     <mdsxyz123@yahoo.com>
-    Copyright (C) 2007 - 2012  Jean Delvare <jdelvare@suse.de>
+    Copyright (C) 2007 - 2014  Jean Delvare <jdelvare@suse.de>
     Copyright (C) 2010         Intel Corporation,
                                David Woodhouse <dwmw2@infradead.org>
 
@@ -59,6 +59,7 @@
  * Wildcat Point-LP (PCH)	0x9ca2	32	hard	yes	yes	yes
  * BayTrail (SOC)		0x0f12	32	hard	yes	yes	yes
  * Sunrise Point-H (PCH) 	0xa123  32	hard	yes	yes	yes
+ * Sunrise Point-LP (PCH)	0x9d23	32	hard	yes	yes	yes
  *
  * Features supported by this driver:
  * Software PEC				no
@@ -109,12 +110,16 @@
 
 /* PCI Address Constants */
 #define SMBBAR		4
+#define SMBPCICTL	0x004
 #define SMBPCISTS	0x006
 #define SMBHSTCFG	0x040
 
 /* Host status bits for SMBPCISTS */
 #define SMBPCISTS_INTS		0x08
 
+/* Control bits for SMBPCICTL */
+#define SMBPCICTL_INTDIS	0x0400
+
 /* Host configuration bits for SMBHSTCFG */
 #define SMBHSTCFG_HST_EN	1
 #define SMBHSTCFG_SMB_SMI_EN	2
@@ -182,6 +187,7 @@
 #define PCI_DEVICE_ID_INTEL_LYNXPOINT_LP_SMBUS		0x9c22
 #define PCI_DEVICE_ID_INTEL_WILDCATPOINT_LP_SMBUS	0x9ca2
 #define PCI_DEVICE_ID_INTEL_SUNRISEPOINT_H_SMBUS	0xa123
+#define PCI_DEVICE_ID_INTEL_SUNRISEPOINT_LP_SMBUS	0x9d23
 
 struct i801_mux_config {
 	char *gpio_chip;
@@ -371,6 +377,7 @@ static int i801_transaction(struct i801_priv *priv, int xact)
 {
 	int status;
 	int result;
+	const struct i2c_adapter *adap = &priv->adapter;
 
 	result = i801_check_pre(priv);
 	if (result < 0)
@@ -379,7 +386,14 @@ static int i801_transaction(struct i801_priv *priv, int xact)
 	if (priv->features & FEATURE_IRQ) {
 		outb_p(xact | SMBHSTCNT_INTREN | SMBHSTCNT_START,
 		       SMBHSTCNT(priv));
-		wait_event(priv->waitq, (status = priv->status));
+		result = wait_event_timeout(priv->waitq,
+					    (status = priv->status),
+					    adap->timeout);
+		if (!result) {
+			status = -ETIMEDOUT;
+			dev_warn(&priv->pci_dev->dev,
+				 "Timeout waiting for interrupt!\n");
+		}
 		priv->status = 0;
 		return i801_check_post(priv, status);
 	}
@@ -493,9 +507,6 @@ static irqreturn_t i801_isr(int irq, void *dev_id)
 		return IRQ_NONE;
 
 	status = inb_p(SMBHSTSTS(priv));
-	if (status != 0x42)
-		dev_dbg(&priv->pci_dev->dev, "irq: status = %02x\n", status);
-
 	if (status & SMBHSTSTS_BYTE_DONE)
 		i801_isr_byte_done(priv);
 
@@ -527,6 +538,7 @@ static int i801_block_transaction_byte_by_byte(struct i801_priv *priv,
 	int smbcmd;
 	int status;
 	int result;
+	const struct i2c_adapter *adap = &priv->adapter;
 
 	result = i801_check_pre(priv);
 	if (result < 0)
@@ -555,7 +567,14 @@ static int i801_block_transaction_byte_by_byte(struct i801_priv *priv,
 		priv->data = &data->block[1];
 
 		outb_p(priv->cmd | SMBHSTCNT_START, SMBHSTCNT(priv));
-		wait_event(priv->waitq, (status = priv->status));
+		result = wait_event_timeout(priv->waitq,
+					    (status = priv->status),
+					    adap->timeout);
+		if (!result) {
+			status = -ETIMEDOUT;
+			dev_warn(&priv->pci_dev->dev,
+				 "Timeout waiting for interrupt!\n");
+		}
 		priv->status = 0;
 		return i801_check_post(priv, status);
 	}
@@ -829,6 +848,7 @@ static const struct pci_device_id i801_ids[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_BAYTRAIL_SMBUS) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_BRASWELL_SMBUS) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_SUNRISEPOINT_H_SMBUS) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_SUNRISEPOINT_LP_SMBUS) },
 	{ 0, }
 };
 
@@ -1212,6 +1232,25 @@ static int i801_probe(struct pci_dev *dev, const struct pci_device_id *id)
 		outb_p(inb_p(SMBAUXCTL(priv)) &
 		       ~(SMBAUXCTL_CRC | SMBAUXCTL_E32B), SMBAUXCTL(priv));
 
+	/* Default timeout in interrupt mode: 200 ms */
+	priv->adapter.timeout = HZ / 5;
+
+	if (priv->features & FEATURE_IRQ) {
+		u16 pcictl, pcists;
+
+		/* Complain if an interrupt is already pending */
+		pci_read_config_word(priv->pci_dev, SMBPCISTS, &pcists);
+		if (pcists & SMBPCISTS_INTS)
+			dev_warn(&dev->dev, "An interrupt is pending!\n");
+
+		/* Check if interrupts have been disabled */
+		pci_read_config_word(priv->pci_dev, SMBPCICTL, &pcictl);
+		if (pcictl & SMBPCICTL_INTDIS) {
+			dev_info(&dev->dev, "Interrupts are disabled\n");
+			priv->features &= ~FEATURE_IRQ;
+		}
+	}
+
 	if (priv->features & FEATURE_IRQ) {
 		init_waitqueue_head(&priv->waitq);
 
@@ -1220,10 +1259,11 @@ static int i801_probe(struct pci_dev *dev, const struct pci_device_id *id)
 		if (err) {
 			dev_err(&dev->dev, "Failed to allocate irq %d: %d\n",
 				dev->irq, err);
-			goto exit_release;
+			priv->features &= ~FEATURE_IRQ;
 		}
-		dev_info(&dev->dev, "SMBus using PCI Interrupt\n");
 	}
+	dev_info(&dev->dev, "SMBus using %s\n",
+		 priv->features & FEATURE_IRQ ? "PCI interrupt" : "polling");
 
 	/* set up the sysfs linkage to our parent device */
 	priv->adapter.dev.parent = &dev->dev;
@@ -1250,7 +1290,6 @@ static int i801_probe(struct pci_dev *dev, const struct pci_device_id *id)
 exit_free_irq:
 	if (priv->features & FEATURE_IRQ)
 		free_irq(dev->irq, priv);
-exit_release:
 	pci_release_region(dev, SMBBAR);
 exit:
 	kfree(priv);
diff --git a/drivers/i2c/busses/i2c-ibm_iic.c b/drivers/i2c/busses/i2c-ibm_iic.c
index 274312c96b12..722f839cfa3c 100644
--- a/drivers/i2c/busses/i2c-ibm_iic.c
+++ b/drivers/i2c/busses/i2c-ibm_iic.c
@@ -802,7 +802,6 @@ static const struct of_device_id ibm_iic_match[] = {
 static struct platform_driver ibm_iic_driver = {
 	.driver = {
 		.name = "ibm-iic",
-		.owner = THIS_MODULE,
 		.of_match_table = ibm_iic_match,
 	},
 	.probe	= iic_probe,
diff --git a/drivers/i2c/busses/i2c-img-scb.c b/drivers/i2c/busses/i2c-img-scb.c
new file mode 100644
index 000000000000..0fcc1694c607
--- /dev/null
+++ b/drivers/i2c/busses/i2c-img-scb.c
@@ -0,0 +1,1412 @@
+/*
+ * I2C adapter for the IMG Serial Control Bus (SCB) IP block.
+ *
+ * Copyright (C) 2009, 2010, 2012, 2014 Imagination Technologies 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.
+ *
+ * There are three ways that this I2C controller can be driven:
+ *
+ * - Raw control of the SDA and SCK signals.
+ *
+ *   This corresponds to MODE_RAW, which takes control of the signals
+ *   directly for a certain number of clock cycles (the INT_TIMING
+ *   interrupt can be used for timing).
+ *
+ * - Atomic commands. A low level I2C symbol (such as generate
+ *   start/stop/ack/nack bit, generate byte, receive byte, and receive
+ *   ACK) is given to the hardware, with detection of completion by bits
+ *   in the LINESTAT register.
+ *
+ *   This mode of operation is used by MODE_ATOMIC, which uses an I2C
+ *   state machine in the interrupt handler to compose/react to I2C
+ *   transactions using atomic mode commands, and also by MODE_SEQUENCE,
+ *   which emits a simple fixed sequence of atomic mode commands.
+ *
+ *   Due to software control, the use of atomic commands usually results
+ *   in suboptimal use of the bus, with gaps between the I2C symbols while
+ *   the driver decides what to do next.
+ *
+ * - Automatic mode. A bus address, and whether to read/write is
+ *   specified, and the hardware takes care of the I2C state machine,
+ *   using a FIFO to send/receive bytes of data to an I2C slave. The
+ *   driver just has to keep the FIFO drained or filled in response to the
+ *   appropriate FIFO interrupts.
+ *
+ *   This corresponds to MODE_AUTOMATIC, which manages the FIFOs and deals
+ *   with control of repeated start bits between I2C messages.
+ *
+ *   Use of automatic mode and the FIFO can make much more efficient use
+ *   of the bus compared to individual atomic commands, with potentially
+ *   no wasted time between I2C symbols or I2C messages.
+ *
+ * In most cases MODE_AUTOMATIC is used, however if any of the messages in
+ * a transaction are zero byte writes (e.g. used by i2cdetect for probing
+ * the bus), MODE_ATOMIC must be used since automatic mode is normally
+ * started by the writing of data into the FIFO.
+ *
+ * The other modes are used in specific circumstances where MODE_ATOMIC and
+ * MODE_AUTOMATIC aren't appropriate. MODE_RAW is used to implement a bus
+ * recovery routine. MODE_SEQUENCE is used to reset the bus and make sure
+ * it is in a sane state.
+ *
+ * Notice that the driver implements a timer-based timeout mechanism.
+ * The reason for this mechanism is to reduce the number of interrupts
+ * received in automatic mode.
+ *
+ * The driver would get a slave event and transaction done interrupts for
+ * each atomic mode command that gets completed. However, these events are
+ * not needed in automatic mode, becase those atomic mode commands are
+ * managed automatically by the hardware.
+ *
+ * In practice, normal I2C transactions will be complete well before you
+ * get the timer interrupt, as the timer is re-scheduled during FIFO
+ * maintenance and disabled after the transaction is complete.
+ *
+ * In this way normal automatic mode operation isn't impacted by
+ * unnecessary interrupts, but the exceptional abort condition can still be
+ * detected (with a slight delay).
+ */
+
+#include <linux/bitops.h>
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/timer.h>
+
+/* Register offsets */
+
+#define SCB_STATUS_REG			0x00
+#define SCB_OVERRIDE_REG		0x04
+#define SCB_READ_ADDR_REG		0x08
+#define SCB_READ_COUNT_REG		0x0c
+#define SCB_WRITE_ADDR_REG		0x10
+#define SCB_READ_DATA_REG		0x14
+#define SCB_WRITE_DATA_REG		0x18
+#define SCB_FIFO_STATUS_REG		0x1c
+#define SCB_CONTROL_SOFT_RESET		0x1f
+#define SCB_CLK_SET_REG			0x3c
+#define SCB_INT_STATUS_REG		0x40
+#define SCB_INT_CLEAR_REG		0x44
+#define SCB_INT_MASK_REG		0x48
+#define SCB_CONTROL_REG			0x4c
+#define SCB_TIME_TPL_REG		0x50
+#define SCB_TIME_TPH_REG		0x54
+#define SCB_TIME_TP2S_REG		0x58
+#define SCB_TIME_TBI_REG		0x60
+#define SCB_TIME_TSL_REG		0x64
+#define SCB_TIME_TDL_REG		0x68
+#define SCB_TIME_TSDL_REG		0x6c
+#define SCB_TIME_TSDH_REG		0x70
+#define SCB_READ_XADDR_REG		0x74
+#define SCB_WRITE_XADDR_REG		0x78
+#define SCB_WRITE_COUNT_REG		0x7c
+#define SCB_CORE_REV_REG		0x80
+#define SCB_TIME_TCKH_REG		0x84
+#define SCB_TIME_TCKL_REG		0x88
+#define SCB_FIFO_FLUSH_REG		0x8c
+#define SCB_READ_FIFO_REG		0x94
+#define SCB_CLEAR_REG			0x98
+
+/* SCB_CONTROL_REG bits */
+
+#define SCB_CONTROL_CLK_ENABLE		0x1e0
+#define SCB_CONTROL_TRANSACTION_HALT	0x200
+
+#define FIFO_READ_FULL			BIT(0)
+#define FIFO_READ_EMPTY			BIT(1)
+#define FIFO_WRITE_FULL			BIT(2)
+#define FIFO_WRITE_EMPTY		BIT(3)
+
+/* SCB_CLK_SET_REG bits */
+#define SCB_FILT_DISABLE		BIT(31)
+#define SCB_FILT_BYPASS			BIT(30)
+#define SCB_FILT_INC_MASK		0x7f
+#define SCB_FILT_INC_SHIFT		16
+#define SCB_INC_MASK			0x7f
+#define SCB_INC_SHIFT			8
+
+/* SCB_INT_*_REG bits */
+
+#define INT_BUS_INACTIVE		BIT(0)
+#define INT_UNEXPECTED_START		BIT(1)
+#define INT_SCLK_LOW_TIMEOUT		BIT(2)
+#define INT_SDAT_LOW_TIMEOUT		BIT(3)
+#define INT_WRITE_ACK_ERR		BIT(4)
+#define INT_ADDR_ACK_ERR		BIT(5)
+#define INT_FIFO_FULL			BIT(9)
+#define INT_FIFO_FILLING		BIT(10)
+#define INT_FIFO_EMPTY			BIT(11)
+#define INT_FIFO_EMPTYING		BIT(12)
+#define INT_TRANSACTION_DONE		BIT(15)
+#define INT_SLAVE_EVENT			BIT(16)
+#define INT_TIMING			BIT(18)
+
+#define INT_FIFO_FULL_FILLING	(INT_FIFO_FULL  | INT_FIFO_FILLING)
+#define INT_FIFO_EMPTY_EMPTYING	(INT_FIFO_EMPTY | INT_FIFO_EMPTYING)
+
+/* Level interrupts need clearing after handling instead of before */
+#define INT_LEVEL			0x01e00
+
+/* Don't allow any interrupts while the clock may be off */
+#define INT_ENABLE_MASK_INACTIVE	0x00000
+
+/* Interrupt masks for the different driver modes */
+
+#define INT_ENABLE_MASK_RAW		INT_TIMING
+
+#define INT_ENABLE_MASK_ATOMIC		(INT_TRANSACTION_DONE | \
+					 INT_SLAVE_EVENT      | \
+					 INT_ADDR_ACK_ERR     | \
+					 INT_WRITE_ACK_ERR)
+
+#define INT_ENABLE_MASK_AUTOMATIC	(INT_SCLK_LOW_TIMEOUT | \
+					 INT_ADDR_ACK_ERR     | \
+					 INT_WRITE_ACK_ERR    | \
+					 INT_FIFO_FULL        | \
+					 INT_FIFO_FILLING     | \
+					 INT_FIFO_EMPTY       | \
+					 INT_FIFO_EMPTYING)
+
+#define INT_ENABLE_MASK_WAITSTOP	(INT_SLAVE_EVENT      | \
+					 INT_ADDR_ACK_ERR     | \
+					 INT_WRITE_ACK_ERR)
+
+/* SCB_STATUS_REG fields */
+
+#define LINESTAT_SCLK_LINE_STATUS	BIT(0)
+#define LINESTAT_SCLK_EN		BIT(1)
+#define LINESTAT_SDAT_LINE_STATUS	BIT(2)
+#define LINESTAT_SDAT_EN		BIT(3)
+#define LINESTAT_DET_START_STATUS	BIT(4)
+#define LINESTAT_DET_STOP_STATUS	BIT(5)
+#define LINESTAT_DET_ACK_STATUS		BIT(6)
+#define LINESTAT_DET_NACK_STATUS	BIT(7)
+#define LINESTAT_BUS_IDLE		BIT(8)
+#define LINESTAT_T_DONE_STATUS		BIT(9)
+#define LINESTAT_SCLK_OUT_STATUS	BIT(10)
+#define LINESTAT_SDAT_OUT_STATUS	BIT(11)
+#define LINESTAT_GEN_LINE_MASK_STATUS	BIT(12)
+#define LINESTAT_START_BIT_DET		BIT(13)
+#define LINESTAT_STOP_BIT_DET		BIT(14)
+#define LINESTAT_ACK_DET		BIT(15)
+#define LINESTAT_NACK_DET		BIT(16)
+#define LINESTAT_INPUT_HELD_V		BIT(17)
+#define LINESTAT_ABORT_DET		BIT(18)
+#define LINESTAT_ACK_OR_NACK_DET	(LINESTAT_ACK_DET | LINESTAT_NACK_DET)
+#define LINESTAT_INPUT_DATA		0xff000000
+#define LINESTAT_INPUT_DATA_SHIFT	24
+
+#define LINESTAT_CLEAR_SHIFT		13
+#define LINESTAT_LATCHED		(0x3f << LINESTAT_CLEAR_SHIFT)
+
+/* SCB_OVERRIDE_REG fields */
+
+#define OVERRIDE_SCLK_OVR		BIT(0)
+#define OVERRIDE_SCLKEN_OVR		BIT(1)
+#define OVERRIDE_SDAT_OVR		BIT(2)
+#define OVERRIDE_SDATEN_OVR		BIT(3)
+#define OVERRIDE_MASTER			BIT(9)
+#define OVERRIDE_LINE_OVR_EN		BIT(10)
+#define OVERRIDE_DIRECT			BIT(11)
+#define OVERRIDE_CMD_SHIFT		4
+#define OVERRIDE_CMD_MASK		0x1f
+#define OVERRIDE_DATA_SHIFT		24
+
+#define OVERRIDE_SCLK_DOWN		(OVERRIDE_LINE_OVR_EN | \
+					 OVERRIDE_SCLKEN_OVR)
+#define OVERRIDE_SCLK_UP		(OVERRIDE_LINE_OVR_EN | \
+					 OVERRIDE_SCLKEN_OVR | \
+					 OVERRIDE_SCLK_OVR)
+#define OVERRIDE_SDAT_DOWN		(OVERRIDE_LINE_OVR_EN | \
+					 OVERRIDE_SDATEN_OVR)
+#define OVERRIDE_SDAT_UP		(OVERRIDE_LINE_OVR_EN | \
+					 OVERRIDE_SDATEN_OVR | \
+					 OVERRIDE_SDAT_OVR)
+
+/* OVERRIDE_CMD values */
+
+#define CMD_PAUSE			0x00
+#define CMD_GEN_DATA			0x01
+#define CMD_GEN_START			0x02
+#define CMD_GEN_STOP			0x03
+#define CMD_GEN_ACK			0x04
+#define CMD_GEN_NACK			0x05
+#define CMD_RET_DATA			0x08
+#define CMD_RET_ACK			0x09
+
+/* Fixed timing values */
+
+#define TIMEOUT_TBI			0x0
+#define TIMEOUT_TSL			0xffff
+#define TIMEOUT_TDL			0x0
+
+/* Transaction timeout */
+
+#define IMG_I2C_TIMEOUT			(msecs_to_jiffies(1000))
+
+/*
+ * Worst incs are 1 (innacurate) and 16*256 (irregular).
+ * So a sensible inc is the logarithmic mean: 64 (2^6), which is
+ * in the middle of the valid range (0-127).
+ */
+#define SCB_OPT_INC		64
+
+/* Setup the clock enable filtering for 25 ns */
+#define SCB_FILT_GLITCH		25
+
+/*
+ * Bits to return from interrupt handler functions for different modes.
+ * This delays completion until we've finished with the registers, so that the
+ * function waiting for completion can safely disable the clock to save power.
+ */
+#define ISR_COMPLETE_M		BIT(31)
+#define ISR_FATAL_M		BIT(30)
+#define ISR_WAITSTOP		BIT(29)
+#define ISR_STATUS_M		0x0000ffff	/* contains +ve errno */
+#define ISR_COMPLETE(err)	(ISR_COMPLETE_M | (ISR_STATUS_M & (err)))
+#define ISR_FATAL(err)		(ISR_COMPLETE(err) | ISR_FATAL_M)
+
+#define REL_SOC_IP_SCB_2_2_1	0x00020201
+
+enum img_i2c_mode {
+	MODE_INACTIVE,
+	MODE_RAW,
+	MODE_ATOMIC,
+	MODE_AUTOMATIC,
+	MODE_SEQUENCE,
+	MODE_FATAL,
+	MODE_WAITSTOP,
+	MODE_SUSPEND,
+};
+
+/* Timing parameters for i2c modes (in ns) */
+struct img_i2c_timings {
+	const char *name;
+	unsigned int max_bitrate;
+	unsigned int tckh, tckl, tsdh, tsdl;
+	unsigned int tp2s, tpl, tph;
+};
+
+/* The timings array must be ordered from slower to faster */
+static struct img_i2c_timings timings[] = {
+	/* Standard mode */
+	{
+		.name = "standard",
+		.max_bitrate = 100000,
+		.tckh = 4000,
+		.tckl = 4700,
+		.tsdh = 4700,
+		.tsdl = 8700,
+		.tp2s = 4700,
+		.tpl = 4700,
+		.tph = 4000,
+	},
+	/* Fast mode */
+	{
+		.name = "fast",
+		.max_bitrate = 400000,
+		.tckh = 600,
+		.tckl = 1300,
+		.tsdh = 600,
+		.tsdl = 1200,
+		.tp2s = 1300,
+		.tpl = 600,
+		.tph = 600,
+	},
+};
+
+/* Reset dance */
+static u8 img_i2c_reset_seq[] = { CMD_GEN_START,
+				  CMD_GEN_DATA, 0xff,
+				  CMD_RET_ACK,
+				  CMD_GEN_START,
+				  CMD_GEN_STOP,
+				  0 };
+/* Just issue a stop (after an abort condition) */
+static u8 img_i2c_stop_seq[] = {  CMD_GEN_STOP,
+				  0 };
+
+/* We're interested in different interrupts depending on the mode */
+static unsigned int img_i2c_int_enable_by_mode[] = {
+	[MODE_INACTIVE]  = INT_ENABLE_MASK_INACTIVE,
+	[MODE_RAW]       = INT_ENABLE_MASK_RAW,
+	[MODE_ATOMIC]    = INT_ENABLE_MASK_ATOMIC,
+	[MODE_AUTOMATIC] = INT_ENABLE_MASK_AUTOMATIC,
+	[MODE_SEQUENCE]  = INT_ENABLE_MASK_ATOMIC,
+	[MODE_FATAL]     = 0,
+	[MODE_WAITSTOP]  = INT_ENABLE_MASK_WAITSTOP,
+	[MODE_SUSPEND]   = 0,
+};
+
+/* Atomic command names */
+static const char * const img_i2c_atomic_cmd_names[] = {
+	[CMD_PAUSE]	= "PAUSE",
+	[CMD_GEN_DATA]	= "GEN_DATA",
+	[CMD_GEN_START]	= "GEN_START",
+	[CMD_GEN_STOP]	= "GEN_STOP",
+	[CMD_GEN_ACK]	= "GEN_ACK",
+	[CMD_GEN_NACK]	= "GEN_NACK",
+	[CMD_RET_DATA]	= "RET_DATA",
+	[CMD_RET_ACK]	= "RET_ACK",
+};
+
+struct img_i2c {
+	struct i2c_adapter adap;
+
+	void __iomem *base;
+
+	/*
+	 * The scb core clock is used to get the input frequency, and to disable
+	 * it after every set of transactions to save some power.
+	 */
+	struct clk *scb_clk, *sys_clk;
+	unsigned int bitrate;
+	bool need_wr_rd_fence;
+
+	/* state */
+	struct completion msg_complete;
+	spinlock_t lock;	/* lock before doing anything with the state */
+	struct i2c_msg msg;
+
+	/* After the last transaction, wait for a stop bit */
+	bool last_msg;
+	int msg_status;
+
+	enum img_i2c_mode mode;
+	u32 int_enable;		/* depends on mode */
+	u32 line_status;	/* line status over command */
+
+	/*
+	 * To avoid slave event interrupts in automatic mode, use a timer to
+	 * poll the abort condition if we don't get an interrupt for too long.
+	 */
+	struct timer_list check_timer;
+	bool t_halt;
+
+	/* atomic mode state */
+	bool at_t_done;
+	bool at_slave_event;
+	int at_cur_cmd;
+	u8 at_cur_data;
+
+	/* Sequence: either reset or stop. See img_i2c_sequence. */
+	u8 *seq;
+
+	/* raw mode */
+	unsigned int raw_timeout;
+};
+
+static void img_i2c_writel(struct img_i2c *i2c, u32 offset, u32 value)
+{
+	writel(value, i2c->base + offset);
+}
+
+static u32 img_i2c_readl(struct img_i2c *i2c, u32 offset)
+{
+	return readl(i2c->base + offset);
+}
+
+/*
+ * The code to read from the master read fifo, and write to the master
+ * write fifo, checks a bit in an SCB register before every byte to
+ * ensure that the fifo is not full (write fifo) or empty (read fifo).
+ * Due to clock domain crossing inside the SCB block the updated value
+ * of this bit is only visible after 2 cycles.
+ *
+ * The scb_wr_rd_fence() function does 2 dummy writes (to the read-only
+ * revision register), and it's called after reading from or writing to the
+ * fifos to ensure that subsequent reads of the fifo status bits do not read
+ * stale values.
+ */
+static void img_i2c_wr_rd_fence(struct img_i2c *i2c)
+{
+	if (i2c->need_wr_rd_fence) {
+		img_i2c_writel(i2c, SCB_CORE_REV_REG, 0);
+		img_i2c_writel(i2c, SCB_CORE_REV_REG, 0);
+	}
+}
+
+static void img_i2c_switch_mode(struct img_i2c *i2c, enum img_i2c_mode mode)
+{
+	i2c->mode = mode;
+	i2c->int_enable = img_i2c_int_enable_by_mode[mode];
+	i2c->line_status = 0;
+}
+
+static void img_i2c_raw_op(struct img_i2c *i2c)
+{
+	i2c->raw_timeout = 0;
+	img_i2c_writel(i2c, SCB_OVERRIDE_REG,
+		OVERRIDE_SCLKEN_OVR |
+		OVERRIDE_SDATEN_OVR |
+		OVERRIDE_MASTER |
+		OVERRIDE_LINE_OVR_EN |
+		OVERRIDE_DIRECT |
+		((i2c->at_cur_cmd & OVERRIDE_CMD_MASK) << OVERRIDE_CMD_SHIFT) |
+		(i2c->at_cur_data << OVERRIDE_DATA_SHIFT));
+}
+
+static const char *img_i2c_atomic_op_name(unsigned int cmd)
+{
+	if (unlikely(cmd >= ARRAY_SIZE(img_i2c_atomic_cmd_names)))
+		return "UNKNOWN";
+	return img_i2c_atomic_cmd_names[cmd];
+}
+
+/* Send a single atomic mode command to the hardware */
+static void img_i2c_atomic_op(struct img_i2c *i2c, int cmd, u8 data)
+{
+	i2c->at_cur_cmd = cmd;
+	i2c->at_cur_data = data;
+
+	/* work around lack of data setup time when generating data */
+	if (cmd == CMD_GEN_DATA && i2c->mode == MODE_ATOMIC) {
+		u32 line_status = img_i2c_readl(i2c, SCB_STATUS_REG);
+
+		if (line_status & LINESTAT_SDAT_LINE_STATUS && !(data & 0x80)) {
+			/* hold the data line down for a moment */
+			img_i2c_switch_mode(i2c, MODE_RAW);
+			img_i2c_raw_op(i2c);
+			return;
+		}
+	}
+
+	dev_dbg(i2c->adap.dev.parent,
+		"atomic cmd=%s (%d) data=%#x\n",
+		img_i2c_atomic_op_name(cmd), cmd, data);
+	i2c->at_t_done = (cmd == CMD_RET_DATA || cmd == CMD_RET_ACK);
+	i2c->at_slave_event = false;
+	i2c->line_status = 0;
+
+	img_i2c_writel(i2c, SCB_OVERRIDE_REG,
+		((cmd & OVERRIDE_CMD_MASK) << OVERRIDE_CMD_SHIFT) |
+		OVERRIDE_MASTER |
+		OVERRIDE_DIRECT |
+		(data << OVERRIDE_DATA_SHIFT));
+}
+
+/* Start a transaction in atomic mode */
+static void img_i2c_atomic_start(struct img_i2c *i2c)
+{
+	img_i2c_switch_mode(i2c, MODE_ATOMIC);
+	img_i2c_writel(i2c, SCB_INT_MASK_REG, i2c->int_enable);
+	img_i2c_atomic_op(i2c, CMD_GEN_START, 0x00);
+}
+
+static void img_i2c_soft_reset(struct img_i2c *i2c)
+{
+	i2c->t_halt = false;
+	img_i2c_writel(i2c, SCB_CONTROL_REG, 0);
+	img_i2c_writel(i2c, SCB_CONTROL_REG,
+		       SCB_CONTROL_CLK_ENABLE | SCB_CONTROL_SOFT_RESET);
+}
+
+/* enable or release transaction halt for control of repeated starts */
+static void img_i2c_transaction_halt(struct img_i2c *i2c, bool t_halt)
+{
+	u32 val;
+
+	if (i2c->t_halt == t_halt)
+		return;
+	i2c->t_halt = t_halt;
+	val = img_i2c_readl(i2c, SCB_CONTROL_REG);
+	if (t_halt)
+		val |= SCB_CONTROL_TRANSACTION_HALT;
+	else
+		val &= ~SCB_CONTROL_TRANSACTION_HALT;
+	img_i2c_writel(i2c, SCB_CONTROL_REG, val);
+}
+
+/* Drain data from the FIFO into the buffer (automatic mode) */
+static void img_i2c_read_fifo(struct img_i2c *i2c)
+{
+	while (i2c->msg.len) {
+		u32 fifo_status;
+		u8 data;
+
+		fifo_status = img_i2c_readl(i2c, SCB_FIFO_STATUS_REG);
+		if (fifo_status & FIFO_READ_EMPTY)
+			break;
+
+		data = img_i2c_readl(i2c, SCB_READ_DATA_REG);
+		*i2c->msg.buf = data;
+
+		img_i2c_writel(i2c, SCB_READ_FIFO_REG, 0xff);
+		img_i2c_wr_rd_fence(i2c);
+		i2c->msg.len--;
+		i2c->msg.buf++;
+	}
+}
+
+/* Fill the FIFO with data from the buffer (automatic mode) */
+static void img_i2c_write_fifo(struct img_i2c *i2c)
+{
+	while (i2c->msg.len) {
+		u32 fifo_status;
+
+		fifo_status = img_i2c_readl(i2c, SCB_FIFO_STATUS_REG);
+		if (fifo_status & FIFO_WRITE_FULL)
+			break;
+
+		img_i2c_writel(i2c, SCB_WRITE_DATA_REG, *i2c->msg.buf);
+		img_i2c_wr_rd_fence(i2c);
+		i2c->msg.len--;
+		i2c->msg.buf++;
+	}
+
+	/* Disable fifo emptying interrupt if nothing more to write */
+	if (!i2c->msg.len)
+		i2c->int_enable &= ~INT_FIFO_EMPTYING;
+}
+
+/* Start a read transaction in automatic mode */
+static void img_i2c_read(struct img_i2c *i2c)
+{
+	img_i2c_switch_mode(i2c, MODE_AUTOMATIC);
+	if (!i2c->last_msg)
+		i2c->int_enable |= INT_SLAVE_EVENT;
+
+	img_i2c_writel(i2c, SCB_INT_MASK_REG, i2c->int_enable);
+	img_i2c_writel(i2c, SCB_READ_ADDR_REG, i2c->msg.addr);
+	img_i2c_writel(i2c, SCB_READ_COUNT_REG, i2c->msg.len);
+
+	img_i2c_transaction_halt(i2c, false);
+	mod_timer(&i2c->check_timer, jiffies + msecs_to_jiffies(1));
+}
+
+/* Start a write transaction in automatic mode */
+static void img_i2c_write(struct img_i2c *i2c)
+{
+	img_i2c_switch_mode(i2c, MODE_AUTOMATIC);
+	if (!i2c->last_msg)
+		i2c->int_enable |= INT_SLAVE_EVENT;
+
+	img_i2c_writel(i2c, SCB_WRITE_ADDR_REG, i2c->msg.addr);
+	img_i2c_writel(i2c, SCB_WRITE_COUNT_REG, i2c->msg.len);
+
+	img_i2c_transaction_halt(i2c, false);
+	mod_timer(&i2c->check_timer, jiffies + msecs_to_jiffies(1));
+	img_i2c_write_fifo(i2c);
+
+	/* img_i2c_write_fifo() may modify int_enable */
+	img_i2c_writel(i2c, SCB_INT_MASK_REG, i2c->int_enable);
+}
+
+/*
+ * Indicate that the transaction is complete. This is called from the
+ * ISR to wake up the waiting thread, after which the ISR must not
+ * access any more SCB registers.
+ */
+static void img_i2c_complete_transaction(struct img_i2c *i2c, int status)
+{
+	img_i2c_switch_mode(i2c, MODE_INACTIVE);
+	if (status) {
+		i2c->msg_status = status;
+		img_i2c_transaction_halt(i2c, false);
+	}
+	complete(&i2c->msg_complete);
+}
+
+static unsigned int img_i2c_raw_atomic_delay_handler(struct img_i2c *i2c,
+					u32 int_status, u32 line_status)
+{
+	/* Stay in raw mode for this, so we don't just loop infinitely */
+	img_i2c_atomic_op(i2c, i2c->at_cur_cmd, i2c->at_cur_data);
+	img_i2c_switch_mode(i2c, MODE_ATOMIC);
+	return 0;
+}
+
+static unsigned int img_i2c_raw(struct img_i2c *i2c, u32 int_status,
+				u32 line_status)
+{
+	if (int_status & INT_TIMING) {
+		if (i2c->raw_timeout == 0)
+			return img_i2c_raw_atomic_delay_handler(i2c,
+				int_status, line_status);
+		--i2c->raw_timeout;
+	}
+	return 0;
+}
+
+static unsigned int img_i2c_sequence(struct img_i2c *i2c, u32 int_status)
+{
+	static const unsigned int continue_bits[] = {
+		[CMD_GEN_START] = LINESTAT_START_BIT_DET,
+		[CMD_GEN_DATA]  = LINESTAT_INPUT_HELD_V,
+		[CMD_RET_ACK]   = LINESTAT_ACK_DET | LINESTAT_NACK_DET,
+		[CMD_RET_DATA]  = LINESTAT_INPUT_HELD_V,
+		[CMD_GEN_STOP]  = LINESTAT_STOP_BIT_DET,
+	};
+	int next_cmd = -1;
+	u8 next_data = 0x00;
+
+	if (int_status & INT_SLAVE_EVENT)
+		i2c->at_slave_event = true;
+	if (int_status & INT_TRANSACTION_DONE)
+		i2c->at_t_done = true;
+
+	if (!i2c->at_slave_event || !i2c->at_t_done)
+		return 0;
+
+	/* wait if no continue bits are set */
+	if (i2c->at_cur_cmd >= 0 &&
+	    i2c->at_cur_cmd < ARRAY_SIZE(continue_bits)) {
+		unsigned int cont_bits = continue_bits[i2c->at_cur_cmd];
+
+		if (cont_bits) {
+			cont_bits |= LINESTAT_ABORT_DET;
+			if (!(i2c->line_status & cont_bits))
+				return 0;
+		}
+	}
+
+	/* follow the sequence of commands in i2c->seq */
+	next_cmd = *i2c->seq;
+	/* stop on a nil */
+	if (!next_cmd) {
+		img_i2c_writel(i2c, SCB_OVERRIDE_REG, 0);
+		return ISR_COMPLETE(0);
+	}
+	/* when generating data, the next byte is the data */
+	if (next_cmd == CMD_GEN_DATA) {
+		++i2c->seq;
+		next_data = *i2c->seq;
+	}
+	++i2c->seq;
+	img_i2c_atomic_op(i2c, next_cmd, next_data);
+
+	return 0;
+}
+
+static void img_i2c_reset_start(struct img_i2c *i2c)
+{
+	/* Initiate the magic dance */
+	img_i2c_switch_mode(i2c, MODE_SEQUENCE);
+	img_i2c_writel(i2c, SCB_INT_MASK_REG, i2c->int_enable);
+	i2c->seq = img_i2c_reset_seq;
+	i2c->at_slave_event = true;
+	i2c->at_t_done = true;
+	i2c->at_cur_cmd = -1;
+
+	/* img_i2c_reset_seq isn't empty so the following won't fail */
+	img_i2c_sequence(i2c, 0);
+}
+
+static void img_i2c_stop_start(struct img_i2c *i2c)
+{
+	/* Initiate a stop bit sequence */
+	img_i2c_switch_mode(i2c, MODE_SEQUENCE);
+	img_i2c_writel(i2c, SCB_INT_MASK_REG, i2c->int_enable);
+	i2c->seq = img_i2c_stop_seq;
+	i2c->at_slave_event = true;
+	i2c->at_t_done = true;
+	i2c->at_cur_cmd = -1;
+
+	/* img_i2c_stop_seq isn't empty so the following won't fail */
+	img_i2c_sequence(i2c, 0);
+}
+
+static unsigned int img_i2c_atomic(struct img_i2c *i2c,
+				   u32 int_status,
+				   u32 line_status)
+{
+	int next_cmd = -1;
+	u8 next_data = 0x00;
+
+	if (int_status & INT_SLAVE_EVENT)
+		i2c->at_slave_event = true;
+	if (int_status & INT_TRANSACTION_DONE)
+		i2c->at_t_done = true;
+
+	if (!i2c->at_slave_event || !i2c->at_t_done)
+		goto next_atomic_cmd;
+	if (i2c->line_status & LINESTAT_ABORT_DET) {
+		dev_dbg(i2c->adap.dev.parent, "abort condition detected\n");
+		next_cmd = CMD_GEN_STOP;
+		i2c->msg_status = -EIO;
+		goto next_atomic_cmd;
+	}
+
+	/* i2c->at_cur_cmd may have completed */
+	switch (i2c->at_cur_cmd) {
+	case CMD_GEN_START:
+		next_cmd = CMD_GEN_DATA;
+		next_data = (i2c->msg.addr << 1);
+		if (i2c->msg.flags & I2C_M_RD)
+			next_data |= 0x1;
+		break;
+	case CMD_GEN_DATA:
+		if (i2c->line_status & LINESTAT_INPUT_HELD_V)
+			next_cmd = CMD_RET_ACK;
+		break;
+	case CMD_RET_ACK:
+		if (i2c->line_status & LINESTAT_ACK_DET) {
+			if (i2c->msg.len == 0) {
+				next_cmd = CMD_GEN_STOP;
+			} else if (i2c->msg.flags & I2C_M_RD) {
+				next_cmd = CMD_RET_DATA;
+			} else {
+				next_cmd = CMD_GEN_DATA;
+				next_data = *i2c->msg.buf;
+				--i2c->msg.len;
+				++i2c->msg.buf;
+			}
+		} else if (i2c->line_status & LINESTAT_NACK_DET) {
+			i2c->msg_status = -EIO;
+			next_cmd = CMD_GEN_STOP;
+		}
+		break;
+	case CMD_RET_DATA:
+		if (i2c->line_status & LINESTAT_INPUT_HELD_V) {
+			*i2c->msg.buf = (i2c->line_status &
+						LINESTAT_INPUT_DATA)
+					>> LINESTAT_INPUT_DATA_SHIFT;
+			--i2c->msg.len;
+			++i2c->msg.buf;
+			if (i2c->msg.len)
+				next_cmd = CMD_GEN_ACK;
+			else
+				next_cmd = CMD_GEN_NACK;
+		}
+		break;
+	case CMD_GEN_ACK:
+		if (i2c->line_status & LINESTAT_ACK_DET) {
+			next_cmd = CMD_RET_DATA;
+		} else {
+			i2c->msg_status = -EIO;
+			next_cmd = CMD_GEN_STOP;
+		}
+		break;
+	case CMD_GEN_NACK:
+		next_cmd = CMD_GEN_STOP;
+		break;
+	case CMD_GEN_STOP:
+		img_i2c_writel(i2c, SCB_OVERRIDE_REG, 0);
+		return ISR_COMPLETE(0);
+	default:
+		dev_err(i2c->adap.dev.parent, "bad atomic command %d\n",
+			i2c->at_cur_cmd);
+		i2c->msg_status = -EIO;
+		next_cmd = CMD_GEN_STOP;
+		break;
+	}
+
+next_atomic_cmd:
+	if (next_cmd != -1) {
+		/* don't actually stop unless we're the last transaction */
+		if (next_cmd == CMD_GEN_STOP && !i2c->msg_status &&
+						!i2c->last_msg)
+			return ISR_COMPLETE(0);
+		img_i2c_atomic_op(i2c, next_cmd, next_data);
+	}
+	return 0;
+}
+
+/*
+ * Timer function to check if something has gone wrong in automatic mode (so we
+ * don't have to handle so many interrupts just to catch an exception).
+ */
+static void img_i2c_check_timer(unsigned long arg)
+{
+	struct img_i2c *i2c = (struct img_i2c *)arg;
+	unsigned long flags;
+	unsigned int line_status;
+
+	spin_lock_irqsave(&i2c->lock, flags);
+	line_status = img_i2c_readl(i2c, SCB_STATUS_REG);
+
+	/* check for an abort condition */
+	if (line_status & LINESTAT_ABORT_DET) {
+		dev_dbg(i2c->adap.dev.parent,
+			"abort condition detected by check timer\n");
+		/* enable slave event interrupt mask to trigger irq */
+		img_i2c_writel(i2c, SCB_INT_MASK_REG,
+			       i2c->int_enable | INT_SLAVE_EVENT);
+	}
+
+	spin_unlock_irqrestore(&i2c->lock, flags);
+}
+
+static unsigned int img_i2c_auto(struct img_i2c *i2c,
+				 unsigned int int_status,
+				 unsigned int line_status)
+{
+	if (int_status & (INT_WRITE_ACK_ERR | INT_ADDR_ACK_ERR))
+		return ISR_COMPLETE(EIO);
+
+	if (line_status & LINESTAT_ABORT_DET) {
+		dev_dbg(i2c->adap.dev.parent, "abort condition detected\n");
+		/* empty the read fifo */
+		if ((i2c->msg.flags & I2C_M_RD) &&
+		    (int_status & INT_FIFO_FULL_FILLING))
+			img_i2c_read_fifo(i2c);
+		/* use atomic mode and try to force a stop bit */
+		i2c->msg_status = -EIO;
+		img_i2c_stop_start(i2c);
+		return 0;
+	}
+
+	/* Enable transaction halt on start bit */
+	if (!i2c->last_msg && i2c->line_status & LINESTAT_START_BIT_DET) {
+		img_i2c_transaction_halt(i2c, true);
+		/* we're no longer interested in the slave event */
+		i2c->int_enable &= ~INT_SLAVE_EVENT;
+	}
+
+	mod_timer(&i2c->check_timer, jiffies + msecs_to_jiffies(1));
+
+	if (i2c->msg.flags & I2C_M_RD) {
+		if (int_status & INT_FIFO_FULL_FILLING) {
+			img_i2c_read_fifo(i2c);
+			if (i2c->msg.len == 0)
+				return ISR_WAITSTOP;
+		}
+	} else {
+		if (int_status & INT_FIFO_EMPTY_EMPTYING) {
+			/*
+			 * The write fifo empty indicates that we're in the
+			 * last byte so it's safe to start a new write
+			 * transaction without losing any bytes from the
+			 * previous one.
+			 * see 2.3.7 Repeated Start Transactions.
+			 */
+			if ((int_status & INT_FIFO_EMPTY) &&
+			    i2c->msg.len == 0)
+				return ISR_WAITSTOP;
+			img_i2c_write_fifo(i2c);
+		}
+	}
+
+	return 0;
+}
+
+static irqreturn_t img_i2c_isr(int irq, void *dev_id)
+{
+	struct img_i2c *i2c = (struct img_i2c *)dev_id;
+	u32 int_status, line_status;
+	/* We handle transaction completion AFTER accessing registers */
+	unsigned int hret;
+
+	/* Read interrupt status register. */
+	int_status = img_i2c_readl(i2c, SCB_INT_STATUS_REG);
+	/* Clear detected interrupts. */
+	img_i2c_writel(i2c, SCB_INT_CLEAR_REG, int_status);
+
+	/*
+	 * Read line status and clear it until it actually is clear.  We have
+	 * to be careful not to lose any line status bits that get latched.
+	 */
+	line_status = img_i2c_readl(i2c, SCB_STATUS_REG);
+	if (line_status & LINESTAT_LATCHED) {
+		img_i2c_writel(i2c, SCB_CLEAR_REG,
+			      (line_status & LINESTAT_LATCHED)
+				>> LINESTAT_CLEAR_SHIFT);
+		img_i2c_wr_rd_fence(i2c);
+	}
+
+	spin_lock(&i2c->lock);
+
+	/* Keep track of line status bits received */
+	i2c->line_status &= ~LINESTAT_INPUT_DATA;
+	i2c->line_status |= line_status;
+
+	/*
+	 * Certain interrupts indicate that sclk low timeout is not
+	 * a problem. If any of these are set, just continue.
+	 */
+	if ((int_status & INT_SCLK_LOW_TIMEOUT) &&
+	    !(int_status & (INT_SLAVE_EVENT |
+			    INT_FIFO_EMPTY |
+			    INT_FIFO_FULL))) {
+		dev_crit(i2c->adap.dev.parent,
+			 "fatal: clock low timeout occurred %s addr 0x%02x\n",
+			 (i2c->msg.flags & I2C_M_RD) ? "reading" : "writing",
+			 i2c->msg.addr);
+		hret = ISR_FATAL(EIO);
+		goto out;
+	}
+
+	if (i2c->mode == MODE_ATOMIC)
+		hret = img_i2c_atomic(i2c, int_status, line_status);
+	else if (i2c->mode == MODE_AUTOMATIC)
+		hret = img_i2c_auto(i2c, int_status, line_status);
+	else if (i2c->mode == MODE_SEQUENCE)
+		hret = img_i2c_sequence(i2c, int_status);
+	else if (i2c->mode == MODE_WAITSTOP && (int_status & INT_SLAVE_EVENT) &&
+			 (line_status & LINESTAT_STOP_BIT_DET))
+		hret = ISR_COMPLETE(0);
+	else if (i2c->mode == MODE_RAW)
+		hret = img_i2c_raw(i2c, int_status, line_status);
+	else
+		hret = 0;
+
+	/* Clear detected level interrupts. */
+	img_i2c_writel(i2c, SCB_INT_CLEAR_REG, int_status & INT_LEVEL);
+
+out:
+	if (hret & ISR_WAITSTOP) {
+		/*
+		 * Only wait for stop on last message.
+		 * Also we may already have detected the stop bit.
+		 */
+		if (!i2c->last_msg || i2c->line_status & LINESTAT_STOP_BIT_DET)
+			hret = ISR_COMPLETE(0);
+		else
+			img_i2c_switch_mode(i2c, MODE_WAITSTOP);
+	}
+
+	/* now we've finished using regs, handle transaction completion */
+	if (hret & ISR_COMPLETE_M) {
+		int status = -(hret & ISR_STATUS_M);
+
+		img_i2c_complete_transaction(i2c, status);
+		if (hret & ISR_FATAL_M)
+			img_i2c_switch_mode(i2c, MODE_FATAL);
+	}
+
+	/* Enable interrupts (int_enable may be altered by changing mode) */
+	img_i2c_writel(i2c, SCB_INT_MASK_REG, i2c->int_enable);
+
+	spin_unlock(&i2c->lock);
+
+	return IRQ_HANDLED;
+}
+
+/* Force a bus reset sequence and wait for it to complete */
+static int img_i2c_reset_bus(struct img_i2c *i2c)
+{
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&i2c->lock, flags);
+	reinit_completion(&i2c->msg_complete);
+	img_i2c_reset_start(i2c);
+	spin_unlock_irqrestore(&i2c->lock, flags);
+
+	ret = wait_for_completion_timeout(&i2c->msg_complete, IMG_I2C_TIMEOUT);
+	if (ret == 0)
+		return -ETIMEDOUT;
+	return 0;
+}
+
+static int img_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
+			int num)
+{
+	struct img_i2c *i2c = i2c_get_adapdata(adap);
+	bool atomic = false;
+	int i, ret;
+
+	if (i2c->mode == MODE_SUSPEND) {
+		WARN(1, "refusing to service transaction in suspended state\n");
+		return -EIO;
+	}
+
+	if (i2c->mode == MODE_FATAL)
+		return -EIO;
+
+	for (i = 0; i < num; i++) {
+		if (likely(msgs[i].len))
+			continue;
+		/*
+		 * 0 byte reads are not possible because the slave could try
+		 * and pull the data line low, preventing a stop bit.
+		 */
+		if (unlikely(msgs[i].flags & I2C_M_RD))
+			return -EIO;
+		/*
+		 * 0 byte writes are possible and used for probing, but we
+		 * cannot do them in automatic mode, so use atomic mode
+		 * instead.
+		 */
+		atomic = true;
+	}
+
+	ret = clk_prepare_enable(i2c->scb_clk);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < num; i++) {
+		struct i2c_msg *msg = &msgs[i];
+		unsigned long flags;
+
+		spin_lock_irqsave(&i2c->lock, flags);
+
+		/*
+		 * Make a copy of the message struct. We mustn't modify the
+		 * original or we'll confuse drivers and i2c-dev.
+		 */
+		i2c->msg = *msg;
+		i2c->msg_status = 0;
+
+		/*
+		 * After the last message we must have waited for a stop bit.
+		 * Not waiting can cause problems when the clock is disabled
+		 * before the stop bit is sent, and the linux I2C interface
+		 * requires separate transfers not to joined with repeated
+		 * start.
+		 */
+		i2c->last_msg = (i == num - 1);
+		reinit_completion(&i2c->msg_complete);
+
+		if (atomic)
+			img_i2c_atomic_start(i2c);
+		else if (msg->flags & I2C_M_RD)
+			img_i2c_read(i2c);
+		else
+			img_i2c_write(i2c);
+		spin_unlock_irqrestore(&i2c->lock, flags);
+
+		ret = wait_for_completion_timeout(&i2c->msg_complete,
+						  IMG_I2C_TIMEOUT);
+		del_timer_sync(&i2c->check_timer);
+
+		if (ret == 0) {
+			dev_err(adap->dev.parent, "i2c transfer timed out\n");
+			i2c->msg_status = -ETIMEDOUT;
+			break;
+		}
+
+		if (i2c->msg_status)
+			break;
+	}
+
+	clk_disable_unprepare(i2c->scb_clk);
+
+	return i2c->msg_status ? i2c->msg_status : num;
+}
+
+static u32 img_i2c_func(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+}
+
+static const struct i2c_algorithm img_i2c_algo = {
+	.master_xfer = img_i2c_xfer,
+	.functionality = img_i2c_func,
+};
+
+static int img_i2c_init(struct img_i2c *i2c)
+{
+	unsigned int clk_khz, bitrate_khz, clk_period, tckh, tckl, tsdh;
+	unsigned int i, ret, data, prescale, inc, int_bitrate, filt;
+	struct img_i2c_timings timing;
+	u32 rev;
+
+	ret = clk_prepare_enable(i2c->scb_clk);
+	if (ret)
+		return ret;
+
+	rev = img_i2c_readl(i2c, SCB_CORE_REV_REG);
+	if ((rev & 0x00ffffff) < 0x00020200) {
+		dev_info(i2c->adap.dev.parent,
+			 "Unknown hardware revision (%d.%d.%d.%d)\n",
+			 (rev >> 24) & 0xff, (rev >> 16) & 0xff,
+			 (rev >> 8) & 0xff, rev & 0xff);
+		clk_disable_unprepare(i2c->scb_clk);
+		return -EINVAL;
+	}
+
+	if (rev == REL_SOC_IP_SCB_2_2_1) {
+		i2c->need_wr_rd_fence = true;
+		dev_info(i2c->adap.dev.parent, "fence quirk enabled");
+	}
+
+	bitrate_khz = i2c->bitrate / 1000;
+	clk_khz = clk_get_rate(i2c->scb_clk) / 1000;
+
+	/* Determine what mode we're in from the bitrate */
+	timing = timings[0];
+	for (i = 0; i < ARRAY_SIZE(timings); i++) {
+		if (i2c->bitrate <= timings[i].max_bitrate) {
+			timing = timings[i];
+			break;
+		}
+	}
+
+	/* Find the prescale that would give us that inc (approx delay = 0) */
+	prescale = SCB_OPT_INC * clk_khz / (256 * 16 * bitrate_khz);
+	prescale = clamp_t(unsigned int, prescale, 1, 8);
+	clk_khz /= prescale;
+
+	/* Setup the clock increment value */
+	inc = (256 * 16 * bitrate_khz) / clk_khz;
+
+	/*
+	 * The clock generation logic allows to filter glitches on the bus.
+	 * This filter is able to remove bus glitches shorter than 50ns.
+	 * If the clock enable rate is greater than 20 MHz, no filtering
+	 * is required, so we need to disable it.
+	 * If it's between the 20-40 MHz range, there's no need to divide
+	 * the clock to get a filter.
+	 */
+	if (clk_khz < 20000) {
+		filt = SCB_FILT_DISABLE;
+	} else if (clk_khz < 40000) {
+		filt = SCB_FILT_BYPASS;
+	} else {
+		/* Calculate filter clock */
+		filt = (64000 / ((clk_khz / 1000) * SCB_FILT_GLITCH));
+
+		/* Scale up if needed */
+		if (64000 % ((clk_khz / 1000) * SCB_FILT_GLITCH))
+			inc++;
+
+		if (filt > SCB_FILT_INC_MASK)
+			filt = SCB_FILT_INC_MASK;
+
+		filt = (filt & SCB_FILT_INC_MASK) << SCB_FILT_INC_SHIFT;
+	}
+	data = filt | ((inc & SCB_INC_MASK) << SCB_INC_SHIFT) | (prescale - 1);
+	img_i2c_writel(i2c, SCB_CLK_SET_REG, data);
+
+	/* Obtain the clock period of the fx16 clock in ns */
+	clk_period = (256 * 1000000) / (clk_khz * inc);
+
+	/* Calculate the bitrate in terms of internal clock pulses */
+	int_bitrate = 1000000 / (bitrate_khz * clk_period);
+	if ((1000000 % (bitrate_khz * clk_period)) >=
+	    ((bitrate_khz * clk_period) / 2))
+		int_bitrate++;
+
+	/* Setup TCKH value */
+	tckh = timing.tckh / clk_period;
+	if (timing.tckh % clk_period)
+		tckh++;
+
+	if (tckh > 0)
+		data = tckh - 1;
+	else
+		data = 0;
+
+	img_i2c_writel(i2c, SCB_TIME_TCKH_REG, data);
+
+	/* Setup TCKL value */
+	tckl = int_bitrate - tckh;
+
+	if (tckl > 0)
+		data = tckl - 1;
+	else
+		data = 0;
+
+	img_i2c_writel(i2c, SCB_TIME_TCKL_REG, data);
+
+	/* Setup TSDH value */
+	tsdh = timing.tsdh / clk_period;
+	if (timing.tsdh % clk_period)
+		tsdh++;
+
+	if (tsdh > 1)
+		data = tsdh - 1;
+	else
+		data = 0x01;
+	img_i2c_writel(i2c, SCB_TIME_TSDH_REG, data);
+
+	/* This value is used later */
+	tsdh = data;
+
+	/* Setup TPL value */
+	data = timing.tpl / clk_period;
+	if (data > 0)
+		--data;
+	img_i2c_writel(i2c, SCB_TIME_TPL_REG, data);
+
+	/* Setup TPH value */
+	data = timing.tph / clk_period;
+	if (data > 0)
+		--data;
+	img_i2c_writel(i2c, SCB_TIME_TPH_REG, data);
+
+	/* Setup TSDL value to TPL + TSDH + 2 */
+	img_i2c_writel(i2c, SCB_TIME_TSDL_REG, data + tsdh + 2);
+
+	/* Setup TP2S value */
+	data = timing.tp2s / clk_period;
+	if (data > 0)
+		--data;
+	img_i2c_writel(i2c, SCB_TIME_TP2S_REG, data);
+
+	img_i2c_writel(i2c, SCB_TIME_TBI_REG, TIMEOUT_TBI);
+	img_i2c_writel(i2c, SCB_TIME_TSL_REG, TIMEOUT_TSL);
+	img_i2c_writel(i2c, SCB_TIME_TDL_REG, TIMEOUT_TDL);
+
+	/* Take module out of soft reset and enable clocks */
+	img_i2c_soft_reset(i2c);
+
+	/* Disable all interrupts */
+	img_i2c_writel(i2c, SCB_INT_MASK_REG, 0);
+
+	/* Clear all interrupts */
+	img_i2c_writel(i2c, SCB_INT_CLEAR_REG, ~0);
+
+	/* Clear the scb_line_status events */
+	img_i2c_writel(i2c, SCB_CLEAR_REG, ~0);
+
+	/* Enable interrupts */
+	img_i2c_writel(i2c, SCB_INT_MASK_REG, i2c->int_enable);
+
+	/* Perform a synchronous sequence to reset the bus */
+	ret = img_i2c_reset_bus(i2c);
+
+	clk_disable_unprepare(i2c->scb_clk);
+
+	return ret;
+}
+
+static int img_i2c_probe(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct img_i2c *i2c;
+	struct resource *res;
+	int irq, ret;
+	u32 val;
+
+	i2c = devm_kzalloc(&pdev->dev, sizeof(struct img_i2c), GFP_KERNEL);
+	if (!i2c)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	i2c->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(i2c->base))
+		return PTR_ERR(i2c->base);
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "can't get irq number\n");
+		return irq;
+	}
+
+	i2c->sys_clk = devm_clk_get(&pdev->dev, "sys");
+	if (IS_ERR(i2c->sys_clk)) {
+		dev_err(&pdev->dev, "can't get system clock\n");
+		return PTR_ERR(i2c->sys_clk);
+	}
+
+	i2c->scb_clk = devm_clk_get(&pdev->dev, "scb");
+	if (IS_ERR(i2c->scb_clk)) {
+		dev_err(&pdev->dev, "can't get core clock\n");
+		return PTR_ERR(i2c->scb_clk);
+	}
+
+	ret = devm_request_irq(&pdev->dev, irq, img_i2c_isr, 0,
+			       pdev->name, i2c);
+	if (ret) {
+		dev_err(&pdev->dev, "can't request irq %d\n", irq);
+		return ret;
+	}
+
+	/* Set up the exception check timer */
+	init_timer(&i2c->check_timer);
+	i2c->check_timer.function = img_i2c_check_timer;
+	i2c->check_timer.data = (unsigned long)i2c;
+
+	i2c->bitrate = timings[0].max_bitrate;
+	if (!of_property_read_u32(node, "clock-frequency", &val))
+		i2c->bitrate = val;
+
+	i2c_set_adapdata(&i2c->adap, i2c);
+	i2c->adap.dev.parent = &pdev->dev;
+	i2c->adap.dev.of_node = node;
+	i2c->adap.owner = THIS_MODULE;
+	i2c->adap.algo = &img_i2c_algo;
+	i2c->adap.retries = 5;
+	i2c->adap.nr = pdev->id;
+	snprintf(i2c->adap.name, sizeof(i2c->adap.name), "IMG SCB I2C");
+
+	img_i2c_switch_mode(i2c, MODE_INACTIVE);
+	spin_lock_init(&i2c->lock);
+	init_completion(&i2c->msg_complete);
+
+	platform_set_drvdata(pdev, i2c);
+
+	ret = clk_prepare_enable(i2c->sys_clk);
+	if (ret)
+		return ret;
+
+	ret = img_i2c_init(i2c);
+	if (ret)
+		goto disable_clk;
+
+	ret = i2c_add_numbered_adapter(&i2c->adap);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to add adapter\n");
+		goto disable_clk;
+	}
+
+	return 0;
+
+disable_clk:
+	clk_disable_unprepare(i2c->sys_clk);
+	return ret;
+}
+
+static int img_i2c_remove(struct platform_device *dev)
+{
+	struct img_i2c *i2c = platform_get_drvdata(dev);
+
+	i2c_del_adapter(&i2c->adap);
+	clk_disable_unprepare(i2c->sys_clk);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int img_i2c_suspend(struct device *dev)
+{
+	struct img_i2c *i2c = dev_get_drvdata(dev);
+
+	img_i2c_switch_mode(i2c, MODE_SUSPEND);
+
+	clk_disable_unprepare(i2c->sys_clk);
+
+	return 0;
+}
+
+static int img_i2c_resume(struct device *dev)
+{
+	struct img_i2c *i2c = dev_get_drvdata(dev);
+	int ret;
+
+	ret = clk_prepare_enable(i2c->sys_clk);
+	if (ret)
+		return ret;
+
+	img_i2c_init(i2c);
+
+	return 0;
+}
+#endif /* CONFIG_PM_SLEEP */
+
+static SIMPLE_DEV_PM_OPS(img_i2c_pm, img_i2c_suspend, img_i2c_resume);
+
+static const struct of_device_id img_scb_i2c_match[] = {
+	{ .compatible = "img,scb-i2c" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, img_scb_i2c_match);
+
+static struct platform_driver img_scb_i2c_driver = {
+	.driver = {
+		.name		= "img-i2c-scb",
+		.of_match_table	= img_scb_i2c_match,
+		.pm		= &img_i2c_pm,
+	},
+	.probe = img_i2c_probe,
+	.remove = img_i2c_remove,
+};
+module_platform_driver(img_scb_i2c_driver);
+
+MODULE_AUTHOR("James Hogan <james.hogan@imgtec.com>");
+MODULE_DESCRIPTION("IMG host I2C driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/i2c/busses/i2c-imx.c b/drivers/i2c/busses/i2c-imx.c
index e9fb7cf78612..7f3a9fe9bf4e 100644
--- a/drivers/i2c/busses/i2c-imx.c
+++ b/drivers/i2c/busses/i2c-imx.c
@@ -32,22 +32,27 @@
 /** Includes *******************************************************************
 *******************************************************************************/
 
-#include <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/errno.h>
-#include <linux/err.h>
-#include <linux/interrupt.h>
+#include <linux/clk.h>
+#include <linux/completion.h>
 #include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
+#include <linux/dmapool.h>
+#include <linux/err.h>
+#include <linux/errno.h>
 #include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
 #include <linux/io.h>
-#include <linux/sched.h>
-#include <linux/platform_device.h>
-#include <linux/clk.h>
-#include <linux/slab.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
+#include <linux/of_dma.h>
 #include <linux/platform_data/i2c-imx.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
 
 /** Defines ********************************************************************
 *******************************************************************************/
@@ -58,6 +63,15 @@
 /* Default value */
 #define IMX_I2C_BIT_RATE	100000	/* 100kHz */
 
+/*
+ * Enable DMA if transfer byte size is bigger than this threshold.
+ * As the hardware request, it must bigger than 4 bytes.\
+ * I have set '16' here, maybe it's not the best but I think it's
+ * the appropriate.
+ */
+#define DMA_THRESHOLD	16
+#define DMA_TIMEOUT	1000
+
 /* IMX I2C registers:
  * the I2C register offset is different between SoCs,
  * to provid support for all these chips, split the
@@ -83,6 +97,7 @@
 #define I2SR_IBB	0x20
 #define I2SR_IAAS	0x40
 #define I2SR_ICF	0x80
+#define I2CR_DMAEN	0x02
 #define I2CR_RSTA	0x04
 #define I2CR_TXAK	0x08
 #define I2CR_MTX	0x10
@@ -169,6 +184,17 @@ struct imx_i2c_hwdata {
 	unsigned		i2cr_ien_opcode;
 };
 
+struct imx_i2c_dma {
+	struct dma_chan		*chan_tx;
+	struct dma_chan		*chan_rx;
+	struct dma_chan		*chan_using;
+	struct completion	cmd_complete;
+	dma_addr_t		dma_buf;
+	unsigned int		dma_len;
+	enum dma_transfer_direction dma_transfer_dir;
+	enum dma_data_direction dma_data_dir;
+};
+
 struct imx_i2c_struct {
 	struct i2c_adapter	adapter;
 	struct clk		*clk;
@@ -181,6 +207,8 @@ struct imx_i2c_struct {
 	unsigned int		cur_clk;
 	unsigned int		bitrate;
 	const struct imx_i2c_hwdata	*hwdata;
+
+	struct imx_i2c_dma	*dma;
 };
 
 static const struct imx_i2c_hwdata imx1_i2c_hwdata  = {
@@ -251,6 +279,138 @@ static inline unsigned char imx_i2c_read_reg(struct imx_i2c_struct *i2c_imx,
 	return readb(i2c_imx->base + (reg << i2c_imx->hwdata->regshift));
 }
 
+/* Functions for DMA support */
+static void i2c_imx_dma_request(struct imx_i2c_struct *i2c_imx,
+						dma_addr_t phy_addr)
+{
+	struct imx_i2c_dma *dma;
+	struct dma_slave_config dma_sconfig;
+	struct device *dev = &i2c_imx->adapter.dev;
+	int ret;
+
+	dma = devm_kzalloc(dev, sizeof(*dma), GFP_KERNEL);
+	if (!dma)
+		return;
+
+	dma->chan_tx = dma_request_slave_channel(dev, "tx");
+	if (!dma->chan_tx) {
+		dev_dbg(dev, "can't request DMA tx channel\n");
+		ret = -ENODEV;
+		goto fail_al;
+	}
+
+	dma_sconfig.dst_addr = phy_addr +
+				(IMX_I2C_I2DR << i2c_imx->hwdata->regshift);
+	dma_sconfig.dst_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
+	dma_sconfig.dst_maxburst = 1;
+	dma_sconfig.direction = DMA_MEM_TO_DEV;
+	ret = dmaengine_slave_config(dma->chan_tx, &dma_sconfig);
+	if (ret < 0) {
+		dev_dbg(dev, "can't configure tx channel\n");
+		goto fail_tx;
+	}
+
+	dma->chan_rx = dma_request_slave_channel(dev, "rx");
+	if (!dma->chan_rx) {
+		dev_dbg(dev, "can't request DMA rx channel\n");
+		ret = -ENODEV;
+		goto fail_tx;
+	}
+
+	dma_sconfig.src_addr = phy_addr +
+				(IMX_I2C_I2DR << i2c_imx->hwdata->regshift);
+	dma_sconfig.src_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
+	dma_sconfig.src_maxburst = 1;
+	dma_sconfig.direction = DMA_DEV_TO_MEM;
+	ret = dmaengine_slave_config(dma->chan_rx, &dma_sconfig);
+	if (ret < 0) {
+		dev_dbg(dev, "can't configure rx channel\n");
+		goto fail_rx;
+	}
+
+	i2c_imx->dma = dma;
+	init_completion(&dma->cmd_complete);
+	dev_info(dev, "using %s (tx) and %s (rx) for DMA transfers\n",
+		dma_chan_name(dma->chan_tx), dma_chan_name(dma->chan_rx));
+
+	return;
+
+fail_rx:
+	dma_release_channel(dma->chan_rx);
+fail_tx:
+	dma_release_channel(dma->chan_tx);
+fail_al:
+	devm_kfree(dev, dma);
+	dev_info(dev, "can't use DMA\n");
+}
+
+static void i2c_imx_dma_callback(void *arg)
+{
+	struct imx_i2c_struct *i2c_imx = (struct imx_i2c_struct *)arg;
+	struct imx_i2c_dma *dma = i2c_imx->dma;
+
+	dma_unmap_single(dma->chan_using->device->dev, dma->dma_buf,
+			dma->dma_len, dma->dma_data_dir);
+	complete(&dma->cmd_complete);
+}
+
+static int i2c_imx_dma_xfer(struct imx_i2c_struct *i2c_imx,
+					struct i2c_msg *msgs)
+{
+	struct imx_i2c_dma *dma = i2c_imx->dma;
+	struct dma_async_tx_descriptor *txdesc;
+	struct device *dev = &i2c_imx->adapter.dev;
+	struct device *chan_dev = dma->chan_using->device->dev;
+
+	dma->dma_buf = dma_map_single(chan_dev, msgs->buf,
+					dma->dma_len, dma->dma_data_dir);
+	if (dma_mapping_error(chan_dev, dma->dma_buf)) {
+		dev_err(dev, "DMA mapping failed\n");
+		goto err_map;
+	}
+
+	txdesc = dmaengine_prep_slave_single(dma->chan_using, dma->dma_buf,
+					dma->dma_len, dma->dma_transfer_dir,
+					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	if (!txdesc) {
+		dev_err(dev, "Not able to get desc for DMA xfer\n");
+		goto err_desc;
+	}
+
+	txdesc->callback = i2c_imx_dma_callback;
+	txdesc->callback_param = i2c_imx;
+	if (dma_submit_error(dmaengine_submit(txdesc))) {
+		dev_err(dev, "DMA submit failed\n");
+		goto err_submit;
+	}
+
+	dma_async_issue_pending(dma->chan_using);
+	return 0;
+
+err_submit:
+err_desc:
+	dma_unmap_single(chan_dev, dma->dma_buf,
+			dma->dma_len, dma->dma_data_dir);
+err_map:
+	return -EINVAL;
+}
+
+static void i2c_imx_dma_free(struct imx_i2c_struct *i2c_imx)
+{
+	struct imx_i2c_dma *dma = i2c_imx->dma;
+
+	dma->dma_buf = 0;
+	dma->dma_len = 0;
+
+	dma_release_channel(dma->chan_tx);
+	dma->chan_tx = NULL;
+
+	dma_release_channel(dma->chan_rx);
+	dma->chan_rx = NULL;
+
+	dma->chan_using = NULL;
+}
+
 /** Functions for IMX I2C adapter driver ***************************************
 *******************************************************************************/
 
@@ -382,6 +542,7 @@ static int i2c_imx_start(struct imx_i2c_struct *i2c_imx)
 	i2c_imx->stopped = 0;
 
 	temp |= I2CR_IIEN | I2CR_MTX | I2CR_TXAK;
+	temp &= ~I2CR_DMAEN;
 	imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2CR);
 	return result;
 }
@@ -395,6 +556,8 @@ static void i2c_imx_stop(struct imx_i2c_struct *i2c_imx)
 		dev_dbg(&i2c_imx->adapter.dev, "<%s>\n", __func__);
 		temp = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2CR);
 		temp &= ~(I2CR_MSTA | I2CR_MTX);
+		if (i2c_imx->dma)
+			temp &= ~I2CR_DMAEN;
 		imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2CR);
 	}
 	if (is_imx1_i2c(i2c_imx)) {
@@ -435,6 +598,155 @@ static irqreturn_t i2c_imx_isr(int irq, void *dev_id)
 	return IRQ_NONE;
 }
 
+static int i2c_imx_dma_write(struct imx_i2c_struct *i2c_imx,
+					struct i2c_msg *msgs)
+{
+	int result;
+	unsigned int temp = 0;
+	unsigned long orig_jiffies = jiffies;
+	struct imx_i2c_dma *dma = i2c_imx->dma;
+	struct device *dev = &i2c_imx->adapter.dev;
+
+	dma->chan_using = dma->chan_tx;
+	dma->dma_transfer_dir = DMA_MEM_TO_DEV;
+	dma->dma_data_dir = DMA_TO_DEVICE;
+	dma->dma_len = msgs->len - 1;
+	result = i2c_imx_dma_xfer(i2c_imx, msgs);
+	if (result)
+		return result;
+
+	temp = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2CR);
+	temp |= I2CR_DMAEN;
+	imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2CR);
+
+	/*
+	 * Write slave address.
+	 * The first byte must be transmitted by the CPU.
+	 */
+	imx_i2c_write_reg(msgs->addr << 1, i2c_imx, IMX_I2C_I2DR);
+	reinit_completion(&i2c_imx->dma->cmd_complete);
+	result = wait_for_completion_timeout(
+				&i2c_imx->dma->cmd_complete,
+				msecs_to_jiffies(DMA_TIMEOUT));
+	if (result <= 0) {
+		dmaengine_terminate_all(dma->chan_using);
+		return result ?: -ETIMEDOUT;
+	}
+
+	/* Waiting for transfer complete. */
+	while (1) {
+		temp = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2SR);
+		if (temp & I2SR_ICF)
+			break;
+		if (time_after(jiffies, orig_jiffies +
+				msecs_to_jiffies(DMA_TIMEOUT))) {
+			dev_dbg(dev, "<%s> Timeout\n", __func__);
+			return -ETIMEDOUT;
+		}
+		schedule();
+	}
+
+	temp = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2CR);
+	temp &= ~I2CR_DMAEN;
+	imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2CR);
+
+	/* The last data byte must be transferred by the CPU. */
+	imx_i2c_write_reg(msgs->buf[msgs->len-1],
+				i2c_imx, IMX_I2C_I2DR);
+	result = i2c_imx_trx_complete(i2c_imx);
+	if (result)
+		return result;
+
+	return i2c_imx_acked(i2c_imx);
+}
+
+static int i2c_imx_dma_read(struct imx_i2c_struct *i2c_imx,
+			struct i2c_msg *msgs, bool is_lastmsg)
+{
+	int result;
+	unsigned int temp;
+	unsigned long orig_jiffies = jiffies;
+	struct imx_i2c_dma *dma = i2c_imx->dma;
+	struct device *dev = &i2c_imx->adapter.dev;
+
+	temp = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2CR);
+	temp |= I2CR_DMAEN;
+	imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2CR);
+
+	dma->chan_using = dma->chan_rx;
+	dma->dma_transfer_dir = DMA_DEV_TO_MEM;
+	dma->dma_data_dir = DMA_FROM_DEVICE;
+	/* The last two data bytes must be transferred by the CPU. */
+	dma->dma_len = msgs->len - 2;
+	result = i2c_imx_dma_xfer(i2c_imx, msgs);
+	if (result)
+		return result;
+
+	reinit_completion(&i2c_imx->dma->cmd_complete);
+	result = wait_for_completion_timeout(
+				&i2c_imx->dma->cmd_complete,
+				msecs_to_jiffies(DMA_TIMEOUT));
+	if (result <= 0) {
+		dmaengine_terminate_all(dma->chan_using);
+		return result ?: -ETIMEDOUT;
+	}
+
+	/* waiting for transfer complete. */
+	while (1) {
+		temp = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2SR);
+		if (temp & I2SR_ICF)
+			break;
+		if (time_after(jiffies, orig_jiffies +
+				msecs_to_jiffies(DMA_TIMEOUT))) {
+			dev_dbg(dev, "<%s> Timeout\n", __func__);
+			return -ETIMEDOUT;
+		}
+		schedule();
+	}
+
+	temp = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2CR);
+	temp &= ~I2CR_DMAEN;
+	imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2CR);
+
+	/* read n-1 byte data */
+	temp = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2CR);
+	temp |= I2CR_TXAK;
+	imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2CR);
+
+	msgs->buf[msgs->len-2] = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2DR);
+	/* read n byte data */
+	result = i2c_imx_trx_complete(i2c_imx);
+	if (result)
+		return result;
+
+	if (is_lastmsg) {
+		/*
+		 * It must generate STOP before read I2DR to prevent
+		 * controller from generating another clock cycle
+		 */
+		dev_dbg(dev, "<%s> clear MSTA\n", __func__);
+		temp = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2CR);
+		temp &= ~(I2CR_MSTA | I2CR_MTX);
+		imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2CR);
+		i2c_imx_bus_busy(i2c_imx, 0);
+		i2c_imx->stopped = 1;
+	} else {
+		/*
+		 * For i2c master receiver repeat restart operation like:
+		 * read -> repeat MSTA -> read/write
+		 * The controller must set MTX before read the last byte in
+		 * the first read operation, otherwise the first read cost
+		 * one extra clock cycle.
+		 */
+		temp = readb(i2c_imx->base + IMX_I2C_I2CR);
+		temp |= I2CR_MTX;
+		writeb(temp, i2c_imx->base + IMX_I2C_I2CR);
+	}
+	msgs->buf[msgs->len-1] = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2DR);
+
+	return 0;
+}
+
 static int i2c_imx_write(struct imx_i2c_struct *i2c_imx, struct i2c_msg *msgs)
 {
 	int i, result;
@@ -504,6 +816,9 @@ static int i2c_imx_read(struct imx_i2c_struct *i2c_imx, struct i2c_msg *msgs, bo
 
 	dev_dbg(&i2c_imx->adapter.dev, "<%s> read data\n", __func__);
 
+	if (i2c_imx->dma && msgs->len >= DMA_THRESHOLD && !block_data)
+		return i2c_imx_dma_read(i2c_imx, msgs, is_lastmsg);
+
 	/* read data */
 	for (i = 0; i < msgs->len; i++) {
 		u8 len = 0;
@@ -618,8 +933,12 @@ static int i2c_imx_xfer(struct i2c_adapter *adapter,
 #endif
 		if (msgs[i].flags & I2C_M_RD)
 			result = i2c_imx_read(i2c_imx, &msgs[i], is_lastmsg);
-		else
-			result = i2c_imx_write(i2c_imx, &msgs[i]);
+		else {
+			if (i2c_imx->dma && msgs[i].len >= DMA_THRESHOLD)
+				result = i2c_imx_dma_write(i2c_imx, &msgs[i]);
+			else
+				result = i2c_imx_write(i2c_imx, &msgs[i]);
+		}
 		if (result)
 			goto fail0;
 	}
@@ -654,6 +973,7 @@ static int i2c_imx_probe(struct platform_device *pdev)
 	struct imxi2c_platform_data *pdata = dev_get_platdata(&pdev->dev);
 	void __iomem *base;
 	int irq, ret;
+	dma_addr_t phy_addr;
 
 	dev_dbg(&pdev->dev, "<%s>\n", __func__);
 
@@ -668,8 +988,8 @@ static int i2c_imx_probe(struct platform_device *pdev)
 	if (IS_ERR(base))
 		return PTR_ERR(base);
 
-	i2c_imx = devm_kzalloc(&pdev->dev, sizeof(struct imx_i2c_struct),
-				GFP_KERNEL);
+	phy_addr = (dma_addr_t)res->start;
+	i2c_imx = devm_kzalloc(&pdev->dev, sizeof(*i2c_imx), GFP_KERNEL);
 	if (!i2c_imx)
 		return -ENOMEM;
 
@@ -743,6 +1063,9 @@ static int i2c_imx_probe(struct platform_device *pdev)
 		i2c_imx->adapter.name);
 	dev_info(&i2c_imx->adapter.dev, "IMX I2C adapter registered\n");
 
+	/* Init DMA config if support*/
+	i2c_imx_dma_request(i2c_imx, phy_addr);
+
 	return 0;   /* Return OK */
 
 clk_disable:
@@ -758,6 +1081,9 @@ static int i2c_imx_remove(struct platform_device *pdev)
 	dev_dbg(&i2c_imx->adapter.dev, "adapter removed\n");
 	i2c_del_adapter(&i2c_imx->adapter);
 
+	if (i2c_imx->dma)
+		i2c_imx_dma_free(i2c_imx);
+
 	/* setup chip registers to defaults */
 	imx_i2c_write_reg(0, i2c_imx, IMX_I2C_IADR);
 	imx_i2c_write_reg(0, i2c_imx, IMX_I2C_IFDR);
@@ -772,7 +1098,6 @@ static struct platform_driver i2c_imx_driver = {
 	.remove = i2c_imx_remove,
 	.driver	= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = i2c_imx_dt_ids,
 	},
 	.id_table	= imx_i2c_devtype,
diff --git a/drivers/i2c/busses/i2c-iop3xx.c b/drivers/i2c/busses/i2c-iop3xx.c
index 3d16c2f60a5e..72d6161cf77c 100644
--- a/drivers/i2c/busses/i2c-iop3xx.c
+++ b/drivers/i2c/busses/i2c-iop3xx.c
@@ -516,7 +516,6 @@ static struct platform_driver iop3xx_i2c_driver = {
 	.probe		= iop3xx_i2c_probe,
 	.remove		= iop3xx_i2c_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "IOP3xx-I2C",
 	},
 };
diff --git a/drivers/i2c/busses/i2c-isch.c b/drivers/i2c/busses/i2c-isch.c
index 113293d275f6..c2f25f19d76f 100644
--- a/drivers/i2c/busses/i2c-isch.c
+++ b/drivers/i2c/busses/i2c-isch.c
@@ -309,7 +309,6 @@ static int smbus_sch_remove(struct platform_device *pdev)
 static struct platform_driver smbus_sch_driver = {
 	.driver = {
 		.name = "isch_smbus",
-		.owner = THIS_MODULE,
 	},
 	.probe		= smbus_sch_probe,
 	.remove		= smbus_sch_remove,
diff --git a/drivers/i2c/busses/i2c-kempld.c b/drivers/i2c/busses/i2c-kempld.c
index af8f65fb1c05..25993d2e64bf 100644
--- a/drivers/i2c/busses/i2c-kempld.c
+++ b/drivers/i2c/busses/i2c-kempld.c
@@ -394,7 +394,6 @@ static int kempld_i2c_resume(struct platform_device *pdev)
 static struct platform_driver kempld_i2c_driver = {
 	.driver = {
 		.name = "kempld-i2c",
-		.owner = THIS_MODULE,
 	},
 	.probe		= kempld_i2c_probe,
 	.remove		= kempld_i2c_remove,
diff --git a/drivers/i2c/busses/i2c-meson.c b/drivers/i2c/busses/i2c-meson.c
new file mode 100644
index 000000000000..5e176adca8e8
--- /dev/null
+++ b/drivers/i2c/busses/i2c-meson.c
@@ -0,0 +1,492 @@
+/*
+ * I2C bus driver for Amlogic Meson SoCs
+ *
+ * Copyright (C) 2014 Beniamino Galvani <b.galvani@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.
+ */
+
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/types.h>
+
+/* Meson I2C register map */
+#define REG_CTRL		0x00
+#define REG_SLAVE_ADDR		0x04
+#define REG_TOK_LIST0		0x08
+#define REG_TOK_LIST1		0x0c
+#define REG_TOK_WDATA0		0x10
+#define REG_TOK_WDATA1		0x14
+#define REG_TOK_RDATA0		0x18
+#define REG_TOK_RDATA1		0x1c
+
+/* Control register fields */
+#define REG_CTRL_START		BIT(0)
+#define REG_CTRL_ACK_IGNORE	BIT(1)
+#define REG_CTRL_STATUS		BIT(2)
+#define REG_CTRL_ERROR		BIT(3)
+#define REG_CTRL_CLKDIV_SHIFT	12
+#define REG_CTRL_CLKDIV_MASK	((BIT(10) - 1) << REG_CTRL_CLKDIV_SHIFT)
+
+#define I2C_TIMEOUT_MS		500
+#define DEFAULT_FREQ		100000
+
+enum {
+	TOKEN_END = 0,
+	TOKEN_START,
+	TOKEN_SLAVE_ADDR_WRITE,
+	TOKEN_SLAVE_ADDR_READ,
+	TOKEN_DATA,
+	TOKEN_DATA_LAST,
+	TOKEN_STOP,
+};
+
+enum {
+	STATE_IDLE,
+	STATE_READ,
+	STATE_WRITE,
+	STATE_STOP,
+};
+
+/**
+ * struct meson_i2c - Meson I2C device private data
+ *
+ * @adap:	I2C adapter instance
+ * @dev:	Pointer to device structure
+ * @regs:	Base address of the device memory mapped registers
+ * @clk:	Pointer to clock structure
+ * @irq:	IRQ number
+ * @msg:	Pointer to the current I2C message
+ * @state:	Current state in the driver state machine
+ * @last:	Flag set for the last message in the transfer
+ * @count:	Number of bytes to be sent/received in current transfer
+ * @pos:	Current position in the send/receive buffer
+ * @error:	Flag set when an error is received
+ * @lock:	To avoid race conditions between irq handler and xfer code
+ * @done:	Completion used to wait for transfer termination
+ * @frequency:	Operating frequency of I2C bus clock
+ * @tokens:	Sequence of tokens to be written to the device
+ * @num_tokens:	Number of tokens
+ */
+struct meson_i2c {
+	struct i2c_adapter	adap;
+	struct device		*dev;
+	void __iomem		*regs;
+	struct clk		*clk;
+	int			irq;
+
+	struct i2c_msg		*msg;
+	int			state;
+	bool			last;
+	int			count;
+	int			pos;
+	int			error;
+
+	spinlock_t		lock;
+	struct completion	done;
+	unsigned int		frequency;
+	u32			tokens[2];
+	int			num_tokens;
+};
+
+static void meson_i2c_set_mask(struct meson_i2c *i2c, int reg, u32 mask,
+			       u32 val)
+{
+	u32 data;
+
+	data = readl(i2c->regs + reg);
+	data &= ~mask;
+	data |= val & mask;
+	writel(data, i2c->regs + reg);
+}
+
+static void meson_i2c_reset_tokens(struct meson_i2c *i2c)
+{
+	i2c->tokens[0] = 0;
+	i2c->tokens[1] = 0;
+	i2c->num_tokens = 0;
+}
+
+static void meson_i2c_add_token(struct meson_i2c *i2c, int token)
+{
+	if (i2c->num_tokens < 8)
+		i2c->tokens[0] |= (token & 0xf) << (i2c->num_tokens * 4);
+	else
+		i2c->tokens[1] |= (token & 0xf) << ((i2c->num_tokens % 8) * 4);
+
+	i2c->num_tokens++;
+}
+
+static void meson_i2c_write_tokens(struct meson_i2c *i2c)
+{
+	writel(i2c->tokens[0], i2c->regs + REG_TOK_LIST0);
+	writel(i2c->tokens[1], i2c->regs + REG_TOK_LIST1);
+}
+
+static void meson_i2c_set_clk_div(struct meson_i2c *i2c)
+{
+	unsigned long clk_rate = clk_get_rate(i2c->clk);
+	unsigned int div;
+
+	div = DIV_ROUND_UP(clk_rate, i2c->frequency * 4);
+	meson_i2c_set_mask(i2c, REG_CTRL, REG_CTRL_CLKDIV_MASK,
+			   div << REG_CTRL_CLKDIV_SHIFT);
+
+	dev_dbg(i2c->dev, "%s: clk %lu, freq %u, div %u\n", __func__,
+		clk_rate, i2c->frequency, div);
+}
+
+static void meson_i2c_get_data(struct meson_i2c *i2c, char *buf, int len)
+{
+	u32 rdata0, rdata1;
+	int i;
+
+	rdata0 = readl(i2c->regs + REG_TOK_RDATA0);
+	rdata1 = readl(i2c->regs + REG_TOK_RDATA1);
+
+	dev_dbg(i2c->dev, "%s: data %08x %08x len %d\n", __func__,
+		rdata0, rdata1, len);
+
+	for (i = 0; i < min_t(int, 4, len); i++)
+		*buf++ = (rdata0 >> i * 8) & 0xff;
+
+	for (i = 4; i < min_t(int, 8, len); i++)
+		*buf++ = (rdata1 >> (i - 4) * 8) & 0xff;
+}
+
+static void meson_i2c_put_data(struct meson_i2c *i2c, char *buf, int len)
+{
+	u32 wdata0 = 0, wdata1 = 0;
+	int i;
+
+	for (i = 0; i < min_t(int, 4, len); i++)
+		wdata0 |= *buf++ << (i * 8);
+
+	for (i = 4; i < min_t(int, 8, len); i++)
+		wdata1 |= *buf++ << ((i - 4) * 8);
+
+	writel(wdata0, i2c->regs + REG_TOK_WDATA0);
+	writel(wdata0, i2c->regs + REG_TOK_WDATA1);
+
+	dev_dbg(i2c->dev, "%s: data %08x %08x len %d\n", __func__,
+		wdata0, wdata1, len);
+}
+
+static void meson_i2c_prepare_xfer(struct meson_i2c *i2c)
+{
+	bool write = !(i2c->msg->flags & I2C_M_RD);
+	int i;
+
+	i2c->count = min_t(int, i2c->msg->len - i2c->pos, 8);
+
+	for (i = 0; i < i2c->count - 1; i++)
+		meson_i2c_add_token(i2c, TOKEN_DATA);
+
+	if (i2c->count) {
+		if (write || i2c->pos + i2c->count < i2c->msg->len)
+			meson_i2c_add_token(i2c, TOKEN_DATA);
+		else
+			meson_i2c_add_token(i2c, TOKEN_DATA_LAST);
+	}
+
+	if (write)
+		meson_i2c_put_data(i2c, i2c->msg->buf + i2c->pos, i2c->count);
+}
+
+static void meson_i2c_stop(struct meson_i2c *i2c)
+{
+	dev_dbg(i2c->dev, "%s: last %d\n", __func__, i2c->last);
+
+	if (i2c->last) {
+		i2c->state = STATE_STOP;
+		meson_i2c_add_token(i2c, TOKEN_STOP);
+	} else {
+		i2c->state = STATE_IDLE;
+		complete_all(&i2c->done);
+	}
+}
+
+static irqreturn_t meson_i2c_irq(int irqno, void *dev_id)
+{
+	struct meson_i2c *i2c = dev_id;
+	unsigned int ctrl;
+
+	spin_lock(&i2c->lock);
+
+	meson_i2c_reset_tokens(i2c);
+	ctrl = readl(i2c->regs + REG_CTRL);
+
+	dev_dbg(i2c->dev, "irq: state %d, pos %d, count %d, ctrl %08x\n",
+		i2c->state, i2c->pos, i2c->count, ctrl);
+
+	if (ctrl & REG_CTRL_ERROR && i2c->state != STATE_IDLE) {
+		/*
+		 * The bit is set when the IGNORE_NAK bit is cleared
+		 * and the device didn't respond. In this case, the
+		 * I2C controller automatically generates a STOP
+		 * condition.
+		 */
+		dev_dbg(i2c->dev, "error bit set\n");
+		i2c->error = -ENXIO;
+		i2c->state = STATE_IDLE;
+		complete_all(&i2c->done);
+		goto out;
+	}
+
+	switch (i2c->state) {
+	case STATE_READ:
+		if (i2c->count > 0) {
+			meson_i2c_get_data(i2c, i2c->msg->buf + i2c->pos,
+					   i2c->count);
+			i2c->pos += i2c->count;
+		}
+
+		if (i2c->pos >= i2c->msg->len) {
+			meson_i2c_stop(i2c);
+			break;
+		}
+
+		meson_i2c_prepare_xfer(i2c);
+		break;
+	case STATE_WRITE:
+		i2c->pos += i2c->count;
+
+		if (i2c->pos >= i2c->msg->len) {
+			meson_i2c_stop(i2c);
+			break;
+		}
+
+		meson_i2c_prepare_xfer(i2c);
+		break;
+	case STATE_STOP:
+		i2c->state = STATE_IDLE;
+		complete_all(&i2c->done);
+		break;
+	case STATE_IDLE:
+		break;
+	}
+
+out:
+	if (i2c->state != STATE_IDLE) {
+		/* Restart the processing */
+		meson_i2c_write_tokens(i2c);
+		meson_i2c_set_mask(i2c, REG_CTRL, REG_CTRL_START, 0);
+		meson_i2c_set_mask(i2c, REG_CTRL, REG_CTRL_START,
+				   REG_CTRL_START);
+	}
+
+	spin_unlock(&i2c->lock);
+
+	return IRQ_HANDLED;
+}
+
+static void meson_i2c_do_start(struct meson_i2c *i2c, struct i2c_msg *msg)
+{
+	int token;
+
+	token = (msg->flags & I2C_M_RD) ? TOKEN_SLAVE_ADDR_READ :
+		TOKEN_SLAVE_ADDR_WRITE;
+
+	writel(msg->addr << 1, i2c->regs + REG_SLAVE_ADDR);
+	meson_i2c_add_token(i2c, TOKEN_START);
+	meson_i2c_add_token(i2c, token);
+}
+
+static int meson_i2c_xfer_msg(struct meson_i2c *i2c, struct i2c_msg *msg,
+			      int last)
+{
+	unsigned long time_left, flags;
+	int ret = 0;
+
+	i2c->msg = msg;
+	i2c->last = last;
+	i2c->pos = 0;
+	i2c->count = 0;
+	i2c->error = 0;
+
+	meson_i2c_reset_tokens(i2c);
+
+	flags = (msg->flags & I2C_M_IGNORE_NAK) ? REG_CTRL_ACK_IGNORE : 0;
+	meson_i2c_set_mask(i2c, REG_CTRL, REG_CTRL_ACK_IGNORE, flags);
+
+	if (!(msg->flags & I2C_M_NOSTART))
+		meson_i2c_do_start(i2c, msg);
+
+	i2c->state = (msg->flags & I2C_M_RD) ? STATE_READ : STATE_WRITE;
+	meson_i2c_prepare_xfer(i2c);
+	meson_i2c_write_tokens(i2c);
+	reinit_completion(&i2c->done);
+
+	/* Start the transfer */
+	meson_i2c_set_mask(i2c, REG_CTRL, REG_CTRL_START, REG_CTRL_START);
+
+	time_left = msecs_to_jiffies(I2C_TIMEOUT_MS);
+	time_left = wait_for_completion_timeout(&i2c->done, time_left);
+
+	/*
+	 * Protect access to i2c struct and registers from interrupt
+	 * handlers triggered by a transfer terminated after the
+	 * timeout period
+	 */
+	spin_lock_irqsave(&i2c->lock, flags);
+
+	/* Abort any active operation */
+	meson_i2c_set_mask(i2c, REG_CTRL, REG_CTRL_START, 0);
+
+	if (!time_left) {
+		i2c->state = STATE_IDLE;
+		ret = -ETIMEDOUT;
+	}
+
+	if (i2c->error)
+		ret = i2c->error;
+
+	spin_unlock_irqrestore(&i2c->lock, flags);
+
+	return ret;
+}
+
+static int meson_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
+			  int num)
+{
+	struct meson_i2c *i2c = adap->algo_data;
+	int i, ret = 0, count = 0;
+
+	clk_enable(i2c->clk);
+	meson_i2c_set_clk_div(i2c);
+
+	for (i = 0; i < num; i++) {
+		ret = meson_i2c_xfer_msg(i2c, msgs + i, i == num - 1);
+		if (ret)
+			break;
+		count++;
+	}
+
+	clk_disable(i2c->clk);
+
+	return ret ? ret : count;
+}
+
+static u32 meson_i2c_func(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+}
+
+static const struct i2c_algorithm meson_i2c_algorithm = {
+	.master_xfer	= meson_i2c_xfer,
+	.functionality	= meson_i2c_func,
+};
+
+static int meson_i2c_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct meson_i2c *i2c;
+	struct resource *mem;
+	int ret = 0;
+
+	i2c = devm_kzalloc(&pdev->dev, sizeof(struct meson_i2c), GFP_KERNEL);
+	if (!i2c)
+		return -ENOMEM;
+
+	if (of_property_read_u32(pdev->dev.of_node, "clock-frequency",
+				 &i2c->frequency))
+		i2c->frequency = DEFAULT_FREQ;
+
+	i2c->dev = &pdev->dev;
+	platform_set_drvdata(pdev, i2c);
+
+	spin_lock_init(&i2c->lock);
+	init_completion(&i2c->done);
+
+	i2c->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(i2c->clk)) {
+		dev_err(&pdev->dev, "can't get device clock\n");
+		return PTR_ERR(i2c->clk);
+	}
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	i2c->regs = devm_ioremap_resource(&pdev->dev, mem);
+	if (IS_ERR(i2c->regs))
+		return PTR_ERR(i2c->regs);
+
+	i2c->irq = platform_get_irq(pdev, 0);
+	if (i2c->irq < 0) {
+		dev_err(&pdev->dev, "can't find IRQ\n");
+		return i2c->irq;
+	}
+
+	ret = devm_request_irq(&pdev->dev, i2c->irq, meson_i2c_irq,
+			       0, dev_name(&pdev->dev), i2c);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "can't request IRQ\n");
+		return ret;
+	}
+
+	ret = clk_prepare(i2c->clk);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "can't prepare clock\n");
+		return ret;
+	}
+
+	strlcpy(i2c->adap.name, "Meson I2C adapter",
+		sizeof(i2c->adap.name));
+	i2c->adap.owner = THIS_MODULE;
+	i2c->adap.algo = &meson_i2c_algorithm;
+	i2c->adap.dev.parent = &pdev->dev;
+	i2c->adap.dev.of_node = np;
+	i2c->adap.algo_data = i2c;
+
+	/*
+	 * A transfer is triggered when START bit changes from 0 to 1.
+	 * Ensure that the bit is set to 0 after probe
+	 */
+	meson_i2c_set_mask(i2c, REG_CTRL, REG_CTRL_START, 0);
+
+	ret = i2c_add_adapter(&i2c->adap);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "can't register adapter\n");
+		clk_unprepare(i2c->clk);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int meson_i2c_remove(struct platform_device *pdev)
+{
+	struct meson_i2c *i2c = platform_get_drvdata(pdev);
+
+	i2c_del_adapter(&i2c->adap);
+	clk_unprepare(i2c->clk);
+
+	return 0;
+}
+
+static const struct of_device_id meson_i2c_match[] = {
+	{ .compatible = "amlogic,meson6-i2c" },
+	{ },
+};
+
+static struct platform_driver meson_i2c_driver = {
+	.probe   = meson_i2c_probe,
+	.remove  = meson_i2c_remove,
+	.driver  = {
+		.name  = "meson-i2c",
+		.of_match_table = meson_i2c_match,
+	},
+};
+
+module_platform_driver(meson_i2c_driver);
+
+MODULE_DESCRIPTION("Amlogic Meson I2C Bus driver");
+MODULE_AUTHOR("Beniamino Galvani <b.galvani@gmail.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/i2c/busses/i2c-mpc.c b/drivers/i2c/busses/i2c-mpc.c
index 0edf630b099a..c74cc2be613b 100644
--- a/drivers/i2c/busses/i2c-mpc.c
+++ b/drivers/i2c/busses/i2c-mpc.c
@@ -124,7 +124,7 @@ static void mpc_i2c_fixup(struct mpc_i2c *i2c)
 static int i2c_wait(struct mpc_i2c *i2c, unsigned timeout, int writing)
 {
 	unsigned long orig_jiffies = jiffies;
-	u32 x;
+	u32 cmd_err;
 	int result = 0;
 
 	if (!i2c->irq) {
@@ -133,11 +133,11 @@ static int i2c_wait(struct mpc_i2c *i2c, unsigned timeout, int writing)
 			if (time_after(jiffies, orig_jiffies + timeout)) {
 				dev_dbg(i2c->dev, "timeout\n");
 				writeccr(i2c, 0);
-				result = -EIO;
+				result = -ETIMEDOUT;
 				break;
 			}
 		}
-		x = readb(i2c->base + MPC_I2C_SR);
+		cmd_err = readb(i2c->base + MPC_I2C_SR);
 		writeb(0, i2c->base + MPC_I2C_SR);
 	} else {
 		/* Interrupt mode */
@@ -150,28 +150,28 @@ static int i2c_wait(struct mpc_i2c *i2c, unsigned timeout, int writing)
 			result = -ETIMEDOUT;
 		}
 
-		x = i2c->interrupt;
+		cmd_err = i2c->interrupt;
 		i2c->interrupt = 0;
 	}
 
 	if (result < 0)
 		return result;
 
-	if (!(x & CSR_MCF)) {
+	if (!(cmd_err & CSR_MCF)) {
 		dev_dbg(i2c->dev, "unfinished\n");
 		return -EIO;
 	}
 
-	if (x & CSR_MAL) {
+	if (cmd_err & CSR_MAL) {
 		dev_dbg(i2c->dev, "MAL\n");
-		return -EIO;
+		return -EAGAIN;
 	}
 
-	if (writing && (x & CSR_RXAK)) {
+	if (writing && (cmd_err & CSR_RXAK)) {
 		dev_dbg(i2c->dev, "No RXAK\n");
 		/* generate stop */
 		writeccr(i2c, CCR_MEN);
-		return -EIO;
+		return -ENXIO;
 	}
 	return 0;
 }
@@ -813,7 +813,6 @@ static struct platform_driver mpc_i2c_driver = {
 	.probe		= fsl_i2c_probe,
 	.remove		= fsl_i2c_remove,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = DRV_NAME,
 		.of_match_table = mpc_i2c_of_match,
 		.pm = MPC_I2C_PM_OPS,
diff --git a/drivers/i2c/busses/i2c-mv64xxx.c b/drivers/i2c/busses/i2c-mv64xxx.c
index 2f64273d3f2b..30059c1df2a3 100644
--- a/drivers/i2c/busses/i2c-mv64xxx.c
+++ b/drivers/i2c/busses/i2c-mv64xxx.c
@@ -30,12 +30,12 @@
 #define MV64XXX_I2C_BAUD_DIV_N(val)			(val & 0x7)
 #define MV64XXX_I2C_BAUD_DIV_M(val)			((val & 0xf) << 3)
 
-#define	MV64XXX_I2C_REG_CONTROL_ACK			0x00000004
-#define	MV64XXX_I2C_REG_CONTROL_IFLG			0x00000008
-#define	MV64XXX_I2C_REG_CONTROL_STOP			0x00000010
-#define	MV64XXX_I2C_REG_CONTROL_START			0x00000020
-#define	MV64XXX_I2C_REG_CONTROL_TWSIEN			0x00000040
-#define	MV64XXX_I2C_REG_CONTROL_INTEN			0x00000080
+#define	MV64XXX_I2C_REG_CONTROL_ACK			BIT(2)
+#define	MV64XXX_I2C_REG_CONTROL_IFLG			BIT(3)
+#define	MV64XXX_I2C_REG_CONTROL_STOP			BIT(4)
+#define	MV64XXX_I2C_REG_CONTROL_START			BIT(5)
+#define	MV64XXX_I2C_REG_CONTROL_TWSIEN			BIT(6)
+#define	MV64XXX_I2C_REG_CONTROL_INTEN			BIT(7)
 
 /* Ctlr status values */
 #define	MV64XXX_I2C_STATUS_BUS_ERR			0x00
@@ -68,19 +68,17 @@
 #define	MV64XXX_I2C_REG_BRIDGE_TIMING			0xe0
 
 /* Bridge Control values */
-#define	MV64XXX_I2C_BRIDGE_CONTROL_WR			0x00000001
-#define	MV64XXX_I2C_BRIDGE_CONTROL_RD			0x00000002
+#define	MV64XXX_I2C_BRIDGE_CONTROL_WR			BIT(0)
+#define	MV64XXX_I2C_BRIDGE_CONTROL_RD			BIT(1)
 #define	MV64XXX_I2C_BRIDGE_CONTROL_ADDR_SHIFT		2
-#define	MV64XXX_I2C_BRIDGE_CONTROL_ADDR_EXT		0x00001000
+#define	MV64XXX_I2C_BRIDGE_CONTROL_ADDR_EXT		BIT(12)
 #define	MV64XXX_I2C_BRIDGE_CONTROL_TX_SIZE_SHIFT	13
 #define	MV64XXX_I2C_BRIDGE_CONTROL_RX_SIZE_SHIFT	16
-#define	MV64XXX_I2C_BRIDGE_CONTROL_ENABLE		0x00080000
+#define	MV64XXX_I2C_BRIDGE_CONTROL_ENABLE		BIT(19)
+#define	MV64XXX_I2C_BRIDGE_CONTROL_REPEATED_START	BIT(20)
 
 /* Bridge Status values */
-#define	MV64XXX_I2C_BRIDGE_STATUS_ERROR			0x00000001
-#define	MV64XXX_I2C_STATUS_OFFLOAD_ERROR		0xf0000001
-#define	MV64XXX_I2C_STATUS_OFFLOAD_OK			0xf0000000
-
+#define	MV64XXX_I2C_BRIDGE_STATUS_ERROR			BIT(0)
 
 /* Driver states */
 enum {
@@ -99,14 +97,12 @@ enum {
 	MV64XXX_I2C_ACTION_INVALID,
 	MV64XXX_I2C_ACTION_CONTINUE,
 	MV64XXX_I2C_ACTION_SEND_RESTART,
-	MV64XXX_I2C_ACTION_OFFLOAD_RESTART,
 	MV64XXX_I2C_ACTION_SEND_ADDR_1,
 	MV64XXX_I2C_ACTION_SEND_ADDR_2,
 	MV64XXX_I2C_ACTION_SEND_DATA,
 	MV64XXX_I2C_ACTION_RCV_DATA,
 	MV64XXX_I2C_ACTION_RCV_DATA_STOP,
 	MV64XXX_I2C_ACTION_SEND_STOP,
-	MV64XXX_I2C_ACTION_OFFLOAD_SEND_STOP,
 };
 
 struct mv64xxx_i2c_regs {
@@ -193,75 +189,6 @@ mv64xxx_i2c_prepare_for_io(struct mv64xxx_i2c_data *drv_data,
 	}
 }
 
-static int mv64xxx_i2c_offload_msg(struct mv64xxx_i2c_data *drv_data)
-{
-	unsigned long data_reg_hi = 0;
-	unsigned long data_reg_lo = 0;
-	unsigned long ctrl_reg;
-	struct i2c_msg *msg = drv_data->msgs;
-
-	if (!drv_data->offload_enabled)
-		return -EOPNOTSUPP;
-
-	/* Only regular transactions can be offloaded */
-	if ((msg->flags & ~(I2C_M_TEN | I2C_M_RD)) != 0)
-		return -EINVAL;
-
-	/* Only 1-8 byte transfers can be offloaded */
-	if (msg->len < 1 || msg->len > 8)
-		return -EINVAL;
-
-	/* Build transaction */
-	ctrl_reg = MV64XXX_I2C_BRIDGE_CONTROL_ENABLE |
-		   (msg->addr << MV64XXX_I2C_BRIDGE_CONTROL_ADDR_SHIFT);
-
-	if ((msg->flags & I2C_M_TEN) != 0)
-		ctrl_reg |=  MV64XXX_I2C_BRIDGE_CONTROL_ADDR_EXT;
-
-	if ((msg->flags & I2C_M_RD) == 0) {
-		u8 local_buf[8] = { 0 };
-
-		memcpy(local_buf, msg->buf, msg->len);
-		data_reg_lo = cpu_to_le32(*((u32 *)local_buf));
-		data_reg_hi = cpu_to_le32(*((u32 *)(local_buf+4)));
-
-		ctrl_reg |= MV64XXX_I2C_BRIDGE_CONTROL_WR |
-		    (msg->len - 1) << MV64XXX_I2C_BRIDGE_CONTROL_TX_SIZE_SHIFT;
-
-		writel(data_reg_lo,
-			drv_data->reg_base + MV64XXX_I2C_REG_TX_DATA_LO);
-		writel(data_reg_hi,
-			drv_data->reg_base + MV64XXX_I2C_REG_TX_DATA_HI);
-
-	} else {
-		ctrl_reg |= MV64XXX_I2C_BRIDGE_CONTROL_RD |
-		    (msg->len - 1) << MV64XXX_I2C_BRIDGE_CONTROL_RX_SIZE_SHIFT;
-	}
-
-	/* Execute transaction */
-	writel(ctrl_reg, drv_data->reg_base + MV64XXX_I2C_REG_BRIDGE_CONTROL);
-
-	return 0;
-}
-
-static void
-mv64xxx_i2c_update_offload_data(struct mv64xxx_i2c_data *drv_data)
-{
-	struct i2c_msg *msg = drv_data->msg;
-
-	if (msg->flags & I2C_M_RD) {
-		u32 data_reg_lo = readl(drv_data->reg_base +
-				MV64XXX_I2C_REG_RX_DATA_LO);
-		u32 data_reg_hi = readl(drv_data->reg_base +
-				MV64XXX_I2C_REG_RX_DATA_HI);
-		u8 local_buf[8] = { 0 };
-
-		*((u32 *)local_buf) = le32_to_cpu(data_reg_lo);
-		*((u32 *)(local_buf+4)) = le32_to_cpu(data_reg_hi);
-		memcpy(msg->buf, local_buf, msg->len);
-	}
-
-}
 /*
  *****************************************************************************
  *
@@ -389,16 +316,6 @@ mv64xxx_i2c_fsm(struct mv64xxx_i2c_data *drv_data, u32 status)
 		drv_data->rc = -ENXIO;
 		break;
 
-	case MV64XXX_I2C_STATUS_OFFLOAD_OK:
-		if (drv_data->send_stop || drv_data->aborting) {
-			drv_data->action = MV64XXX_I2C_ACTION_OFFLOAD_SEND_STOP;
-			drv_data->state = MV64XXX_I2C_STATE_IDLE;
-		} else {
-			drv_data->action = MV64XXX_I2C_ACTION_OFFLOAD_RESTART;
-			drv_data->state = MV64XXX_I2C_STATE_WAITING_FOR_RESTART;
-		}
-		break;
-
 	default:
 		dev_err(&drv_data->adapter.dev,
 			"mv64xxx_i2c_fsm: Ctlr Error -- state: 0x%x, "
@@ -419,25 +336,15 @@ static void mv64xxx_i2c_send_start(struct mv64xxx_i2c_data *drv_data)
 	drv_data->aborting = 0;
 	drv_data->rc = 0;
 
-	/* Can we offload this msg ? */
-	if (mv64xxx_i2c_offload_msg(drv_data) < 0) {
-		/* No, switch to standard path */
-		mv64xxx_i2c_prepare_for_io(drv_data, drv_data->msgs);
-		writel(drv_data->cntl_bits | MV64XXX_I2C_REG_CONTROL_START,
-			drv_data->reg_base + drv_data->reg_offsets.control);
-	}
+	mv64xxx_i2c_prepare_for_io(drv_data, drv_data->msgs);
+	writel(drv_data->cntl_bits | MV64XXX_I2C_REG_CONTROL_START,
+	       drv_data->reg_base + drv_data->reg_offsets.control);
 }
 
 static void
 mv64xxx_i2c_do_action(struct mv64xxx_i2c_data *drv_data)
 {
 	switch(drv_data->action) {
-	case MV64XXX_I2C_ACTION_OFFLOAD_RESTART:
-		mv64xxx_i2c_update_offload_data(drv_data);
-		writel(0, drv_data->reg_base +	MV64XXX_I2C_REG_BRIDGE_CONTROL);
-		writel(0, drv_data->reg_base +
-			MV64XXX_I2C_REG_BRIDGE_INTR_CAUSE);
-		/* FALLTHRU */
 	case MV64XXX_I2C_ACTION_SEND_RESTART:
 		/* We should only get here if we have further messages */
 		BUG_ON(drv_data->num_msgs == 0);
@@ -518,16 +425,71 @@ mv64xxx_i2c_do_action(struct mv64xxx_i2c_data *drv_data)
 		drv_data->block = 0;
 		wake_up(&drv_data->waitq);
 		break;
+	}
+}
 
-	case MV64XXX_I2C_ACTION_OFFLOAD_SEND_STOP:
-		mv64xxx_i2c_update_offload_data(drv_data);
-		writel(0, drv_data->reg_base +	MV64XXX_I2C_REG_BRIDGE_CONTROL);
-		writel(0, drv_data->reg_base +
-			MV64XXX_I2C_REG_BRIDGE_INTR_CAUSE);
-		drv_data->block = 0;
-		wake_up(&drv_data->waitq);
-		break;
+static void
+mv64xxx_i2c_read_offload_rx_data(struct mv64xxx_i2c_data *drv_data,
+				 struct i2c_msg *msg)
+{
+	u32 buf[2];
+
+	buf[0] = readl(drv_data->reg_base + MV64XXX_I2C_REG_RX_DATA_LO);
+	buf[1] = readl(drv_data->reg_base + MV64XXX_I2C_REG_RX_DATA_HI);
+
+	memcpy(msg->buf, buf, msg->len);
+}
+
+static int
+mv64xxx_i2c_intr_offload(struct mv64xxx_i2c_data *drv_data)
+{
+	u32 cause, status;
+
+	cause = readl(drv_data->reg_base +
+		      MV64XXX_I2C_REG_BRIDGE_INTR_CAUSE);
+	if (!cause)
+		return IRQ_NONE;
+
+	status = readl(drv_data->reg_base +
+		       MV64XXX_I2C_REG_BRIDGE_STATUS);
+
+	if (status & MV64XXX_I2C_BRIDGE_STATUS_ERROR) {
+		drv_data->rc = -EIO;
+		goto out;
+	}
+
+	drv_data->rc = 0;
+
+	/*
+	 * Transaction is a one message read transaction, read data
+	 * for this message.
+	 */
+	if (drv_data->num_msgs == 1 && drv_data->msgs[0].flags & I2C_M_RD) {
+		mv64xxx_i2c_read_offload_rx_data(drv_data, drv_data->msgs);
+		drv_data->msgs++;
+		drv_data->num_msgs--;
+	}
+	/*
+	 * Transaction is a two messages write/read transaction, read
+	 * data for the second (read) message.
+	 */
+	else if (drv_data->num_msgs == 2 &&
+		 !(drv_data->msgs[0].flags & I2C_M_RD) &&
+		 drv_data->msgs[1].flags & I2C_M_RD) {
+		mv64xxx_i2c_read_offload_rx_data(drv_data, drv_data->msgs + 1);
+		drv_data->msgs += 2;
+		drv_data->num_msgs -= 2;
 	}
+
+out:
+	writel(0, drv_data->reg_base +	MV64XXX_I2C_REG_BRIDGE_CONTROL);
+	writel(0, drv_data->reg_base +
+	       MV64XXX_I2C_REG_BRIDGE_INTR_CAUSE);
+	drv_data->block = 0;
+
+	wake_up(&drv_data->waitq);
+
+	return IRQ_HANDLED;
 }
 
 static irqreturn_t
@@ -540,20 +502,9 @@ mv64xxx_i2c_intr(int irq, void *dev_id)
 
 	spin_lock_irqsave(&drv_data->lock, flags);
 
-	if (drv_data->offload_enabled) {
-		while (readl(drv_data->reg_base +
-				MV64XXX_I2C_REG_BRIDGE_INTR_CAUSE)) {
-			int reg_status = readl(drv_data->reg_base +
-					MV64XXX_I2C_REG_BRIDGE_STATUS);
-			if (reg_status & MV64XXX_I2C_BRIDGE_STATUS_ERROR)
-				status = MV64XXX_I2C_STATUS_OFFLOAD_ERROR;
-			else
-				status = MV64XXX_I2C_STATUS_OFFLOAD_OK;
-			mv64xxx_i2c_fsm(drv_data, status);
-			mv64xxx_i2c_do_action(drv_data);
-			rc = IRQ_HANDLED;
-		}
-	}
+	if (drv_data->offload_enabled)
+		rc = mv64xxx_i2c_intr_offload(drv_data);
+
 	while (readl(drv_data->reg_base + drv_data->reg_offsets.control) &
 						MV64XXX_I2C_REG_CONTROL_IFLG) {
 		status = readl(drv_data->reg_base + drv_data->reg_offsets.status);
@@ -635,6 +586,117 @@ mv64xxx_i2c_execute_msg(struct mv64xxx_i2c_data *drv_data, struct i2c_msg *msg,
 	return drv_data->rc;
 }
 
+static void
+mv64xxx_i2c_prepare_tx(struct mv64xxx_i2c_data *drv_data)
+{
+	struct i2c_msg *msg = drv_data->msgs;
+	u32 buf[2];
+
+	memcpy(buf, msg->buf, msg->len);
+
+	writel(buf[0], drv_data->reg_base + MV64XXX_I2C_REG_TX_DATA_LO);
+	writel(buf[1], drv_data->reg_base + MV64XXX_I2C_REG_TX_DATA_HI);
+}
+
+static int
+mv64xxx_i2c_offload_xfer(struct mv64xxx_i2c_data *drv_data)
+{
+	struct i2c_msg *msgs = drv_data->msgs;
+	int num = drv_data->num_msgs;
+	unsigned long ctrl_reg;
+	unsigned long flags;
+
+	spin_lock_irqsave(&drv_data->lock, flags);
+
+	/* Build transaction */
+	ctrl_reg = MV64XXX_I2C_BRIDGE_CONTROL_ENABLE |
+		(msgs[0].addr << MV64XXX_I2C_BRIDGE_CONTROL_ADDR_SHIFT);
+
+	if (msgs[0].flags & I2C_M_TEN)
+		ctrl_reg |= MV64XXX_I2C_BRIDGE_CONTROL_ADDR_EXT;
+
+	/* Single write message transaction */
+	if (num == 1 && !(msgs[0].flags & I2C_M_RD)) {
+		size_t len = msgs[0].len - 1;
+
+		ctrl_reg |= MV64XXX_I2C_BRIDGE_CONTROL_WR |
+			(len << MV64XXX_I2C_BRIDGE_CONTROL_TX_SIZE_SHIFT);
+		mv64xxx_i2c_prepare_tx(drv_data);
+	}
+	/* Single read message transaction */
+	else if (num == 1 && msgs[0].flags & I2C_M_RD) {
+		size_t len = msgs[0].len - 1;
+
+		ctrl_reg |= MV64XXX_I2C_BRIDGE_CONTROL_RD |
+			(len << MV64XXX_I2C_BRIDGE_CONTROL_RX_SIZE_SHIFT);
+	}
+	/*
+	 * Transaction with one write and one read message. This is
+	 * guaranteed by the mv64xx_i2c_can_offload() checks.
+	 */
+	else if (num == 2) {
+		size_t lentx = msgs[0].len - 1;
+		size_t lenrx = msgs[1].len - 1;
+
+		ctrl_reg |=
+			MV64XXX_I2C_BRIDGE_CONTROL_RD |
+			MV64XXX_I2C_BRIDGE_CONTROL_WR |
+			(lentx << MV64XXX_I2C_BRIDGE_CONTROL_TX_SIZE_SHIFT) |
+			(lenrx << MV64XXX_I2C_BRIDGE_CONTROL_RX_SIZE_SHIFT) |
+			MV64XXX_I2C_BRIDGE_CONTROL_REPEATED_START;
+		mv64xxx_i2c_prepare_tx(drv_data);
+	}
+
+	/* Execute transaction */
+	drv_data->block = 1;
+	writel(ctrl_reg, drv_data->reg_base + MV64XXX_I2C_REG_BRIDGE_CONTROL);
+	spin_unlock_irqrestore(&drv_data->lock, flags);
+
+	mv64xxx_i2c_wait_for_completion(drv_data);
+
+	return drv_data->rc;
+}
+
+static bool
+mv64xxx_i2c_valid_offload_sz(struct i2c_msg *msg)
+{
+	return msg->len <= 8 && msg->len >= 1;
+}
+
+static bool
+mv64xxx_i2c_can_offload(struct mv64xxx_i2c_data *drv_data)
+{
+	struct i2c_msg *msgs = drv_data->msgs;
+	int num = drv_data->num_msgs;
+
+	return false;
+
+	if (!drv_data->offload_enabled)
+		return false;
+
+	/*
+	 * We can offload a transaction consisting of a single
+	 * message, as long as the message has a length between 1 and
+	 * 8 bytes.
+	 */
+	if (num == 1 && mv64xxx_i2c_valid_offload_sz(msgs))
+		return true;
+
+	/*
+	 * We can offload a transaction consisting of two messages, if
+	 * the first is a write and a second is a read, and both have
+	 * a length between 1 and 8 bytes.
+	 */
+	if (num == 2 &&
+	    mv64xxx_i2c_valid_offload_sz(msgs) &&
+	    mv64xxx_i2c_valid_offload_sz(msgs + 1) &&
+	    !(msgs[0].flags & I2C_M_RD) &&
+	    msgs[1].flags & I2C_M_RD)
+		return true;
+
+	return false;
+}
+
 /*
  *****************************************************************************
  *
@@ -658,7 +720,11 @@ mv64xxx_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
 	drv_data->msgs = msgs;
 	drv_data->num_msgs = num;
 
-	rc = mv64xxx_i2c_execute_msg(drv_data, &msgs[0], num == 1);
+	if (mv64xxx_i2c_can_offload(drv_data))
+		rc = mv64xxx_i2c_offload_xfer(drv_data);
+	else
+		rc = mv64xxx_i2c_execute_msg(drv_data, &msgs[0], num == 1);
+
 	if (rc < 0)
 		ret = rc;
 
@@ -925,7 +991,6 @@ static struct platform_driver mv64xxx_i2c_driver = {
 	.probe	= mv64xxx_i2c_probe,
 	.remove	= mv64xxx_i2c_remove,
 	.driver	= {
-		.owner	= THIS_MODULE,
 		.name	= MV64XXX_I2C_CTLR_NAME,
 		.of_match_table = mv64xxx_i2c_of_match_table,
 	},
diff --git a/drivers/i2c/busses/i2c-mxs.c b/drivers/i2c/busses/i2c-mxs.c
index 07e1be6f8992..ff8b12c8d25f 100644
--- a/drivers/i2c/busses/i2c-mxs.c
+++ b/drivers/i2c/busses/i2c-mxs.c
@@ -811,7 +811,7 @@ static int mxs_i2c_probe(struct platform_device *pdev)
 	struct resource *res;
 	int err, irq;
 
-	i2c = devm_kzalloc(dev, sizeof(struct mxs_i2c_dev), GFP_KERNEL);
+	i2c = devm_kzalloc(dev, sizeof(*i2c), GFP_KERNEL);
 	if (!i2c)
 		return -ENOMEM;
 
@@ -893,7 +893,6 @@ static int mxs_i2c_remove(struct platform_device *pdev)
 static struct platform_driver mxs_i2c_driver = {
 	.driver = {
 		   .name = DRIVER_NAME,
-		   .owner = THIS_MODULE,
 		   .of_match_table = mxs_i2c_dt_ids,
 		   },
 	.probe = mxs_i2c_probe,
diff --git a/drivers/i2c/busses/i2c-ocores.c b/drivers/i2c/busses/i2c-ocores.c
index 2a4fe0b7cfb7..7249b5b1e5d0 100644
--- a/drivers/i2c/busses/i2c-ocores.c
+++ b/drivers/i2c/busses/i2c-ocores.c
@@ -480,7 +480,6 @@ static struct platform_driver ocores_i2c_driver = {
 	.probe   = ocores_i2c_probe,
 	.remove  = ocores_i2c_remove,
 	.driver  = {
-		.owner = THIS_MODULE,
 		.name = "ocores-i2c",
 		.of_match_table = ocores_i2c_match,
 		.pm = OCORES_I2C_PM,
diff --git a/drivers/i2c/busses/i2c-octeon.c b/drivers/i2c/busses/i2c-octeon.c
index 81042b08a947..6e75e016bffc 100644
--- a/drivers/i2c/busses/i2c-octeon.c
+++ b/drivers/i2c/busses/i2c-octeon.c
@@ -623,7 +623,6 @@ static struct platform_driver octeon_i2c_driver = {
 	.probe		= octeon_i2c_probe,
 	.remove		= octeon_i2c_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= DRV_NAME,
 		.of_match_table = octeon_i2c_match,
 	},
diff --git a/drivers/i2c/busses/i2c-omap.c b/drivers/i2c/busses/i2c-omap.c
index 0e650a0d0ad0..0e894193accf 100644
--- a/drivers/i2c/busses/i2c-omap.c
+++ b/drivers/i2c/busses/i2c-omap.c
@@ -54,6 +54,9 @@
 /* timeout for pm runtime autosuspend */
 #define OMAP_I2C_PM_TIMEOUT		1000	/* ms */
 
+/* timeout for making decision on bus free status */
+#define OMAP_I2C_BUS_FREE_TIMEOUT (msecs_to_jiffies(10))
+
 /* For OMAP3 I2C_IV has changed to I2C_WE (wakeup enable) */
 enum {
 	OMAP_I2C_REV_REG = 0,
@@ -98,7 +101,7 @@ enum {
 #define OMAP_I2C_STAT_ROVR	(1 << 11)	/* Receive overrun */
 #define OMAP_I2C_STAT_XUDF	(1 << 10)	/* Transmit underflow */
 #define OMAP_I2C_STAT_AAS	(1 << 9)	/* Address as slave */
-#define OMAP_I2C_STAT_AD0	(1 << 8)	/* Address zero */
+#define OMAP_I2C_STAT_BF	(1 << 8)	/* Bus Free */
 #define OMAP_I2C_STAT_XRDY	(1 << 4)	/* Transmit data ready */
 #define OMAP_I2C_STAT_RRDY	(1 << 3)	/* Receive data ready */
 #define OMAP_I2C_STAT_ARDY	(1 << 2)	/* Register access ready */
@@ -146,16 +149,20 @@ enum {
 #define OMAP_I2C_SCLH_HSSCLH	8
 
 /* I2C System Test Register (OMAP_I2C_SYSTEST): */
-#ifdef DEBUG
 #define OMAP_I2C_SYSTEST_ST_EN		(1 << 15)	/* System test enable */
 #define OMAP_I2C_SYSTEST_FREE		(1 << 14)	/* Free running mode */
 #define OMAP_I2C_SYSTEST_TMODE_MASK	(3 << 12)	/* Test mode select */
 #define OMAP_I2C_SYSTEST_TMODE_SHIFT	(12)		/* Test mode select */
+/* Functional mode */
+#define OMAP_I2C_SYSTEST_SCL_I_FUNC	(1 << 8)	/* SCL line input value */
+#define OMAP_I2C_SYSTEST_SCL_O_FUNC	(1 << 7)	/* SCL line output value */
+#define OMAP_I2C_SYSTEST_SDA_I_FUNC	(1 << 6)	/* SDA line input value */
+#define OMAP_I2C_SYSTEST_SDA_O_FUNC	(1 << 5)	/* SDA line output value */
+/* SDA/SCL IO mode */
 #define OMAP_I2C_SYSTEST_SCL_I		(1 << 3)	/* SCL line sense in */
 #define OMAP_I2C_SYSTEST_SCL_O		(1 << 2)	/* SCL line drive out */
 #define OMAP_I2C_SYSTEST_SDA_I		(1 << 1)	/* SDA line sense in */
 #define OMAP_I2C_SYSTEST_SDA_O		(1 << 0)	/* SDA line drive out */
-#endif
 
 /* OCP_SYSSTATUS bit definitions */
 #define SYSS_RESETDONE_MASK		(1 << 0)
@@ -202,6 +209,9 @@ struct omap_i2c_dev {
 						 */
 	u32			rev;
 	unsigned		b_hw:1;		/* bad h/w fixes */
+	unsigned		bb_valid:1;	/* true when BB-bit reflects
+						 * the I2C bus state
+						 */
 	unsigned		receiver:1;	/* true when we're in receiver mode */
 	u16			iestate;	/* Saved interrupt register */
 	u16			pscstate;
@@ -290,6 +300,12 @@ static void __omap_i2c_init(struct omap_i2c_dev *dev)
 	omap_i2c_write_reg(dev, OMAP_I2C_CON_REG, OMAP_I2C_CON_EN);
 
 	/*
+	 * NOTE: right after setting CON_EN, STAT_BB could be 0 while the
+	 * bus is busy. It will be changed to 1 on the next IP FCLK clock.
+	 * udelay(1) will be enough to fix that.
+	 */
+
+	/*
 	 * Don't write to this register if the IE state is 0 as it can
 	 * cause deadlock.
 	 */
@@ -328,7 +344,12 @@ static int omap_i2c_reset(struct omap_i2c_dev *dev)
 		/* SYSC register is cleared by the reset; rewrite it */
 		omap_i2c_write_reg(dev, OMAP_I2C_SYSC_REG, sysc);
 
+		if (dev->rev > OMAP_I2C_REV_ON_3430_3530) {
+			/* Schedule I2C-bus monitoring on the next transfer */
+			dev->bb_valid = 0;
+		}
 	}
+
 	return 0;
 }
 
@@ -441,6 +462,11 @@ static int omap_i2c_init(struct omap_i2c_dev *dev)
 	dev->scllstate = scll;
 	dev->sclhstate = sclh;
 
+	if (dev->rev <= OMAP_I2C_REV_ON_3430_3530) {
+		/* Not implemented */
+		dev->bb_valid = 1;
+	}
+
 	__omap_i2c_init(dev);
 
 	return 0;
@@ -465,6 +491,91 @@ static int omap_i2c_wait_for_bb(struct omap_i2c_dev *dev)
 	return 0;
 }
 
+/*
+ * Wait while BB-bit doesn't reflect the I2C bus state
+ *
+ * In a multimaster environment, after IP software reset, BB-bit value doesn't
+ * correspond to the current bus state. It may happen what BB-bit will be 0,
+ * while the bus is busy due to another I2C master activity.
+ * Here are BB-bit values after reset:
+ *     SDA   SCL   BB   NOTES
+ *       0     0    0   1, 2
+ *       1     0    0   1, 2
+ *       0     1    1
+ *       1     1    0   3
+ * Later, if IP detect SDA=0 and SCL=1 (ACK) or SDA 1->0 while SCL=1 (START)
+ * combinations on the bus, it set BB-bit to 1.
+ * If IP detect SDA 0->1 while SCL=1 (STOP) combination on the bus,
+ * it set BB-bit to 0 and BF to 1.
+ * BB and BF bits correctly tracks the bus state while IP is suspended
+ * BB bit became valid on the next FCLK clock after CON_EN bit set
+ *
+ * NOTES:
+ * 1. Any transfer started when BB=0 and bus is busy wouldn't be
+ *    completed by IP and results in controller timeout.
+ * 2. Any transfer started when BB=0 and SCL=0 results in IP
+ *    starting to drive SDA low. In that case IP corrupt data
+ *    on the bus.
+ * 3. Any transfer started in the middle of another master's transfer
+ *    results in unpredictable results and data corruption
+ */
+static int omap_i2c_wait_for_bb_valid(struct omap_i2c_dev *dev)
+{
+	unsigned long bus_free_timeout = 0;
+	unsigned long timeout;
+	int bus_free = 0;
+	u16 stat, systest;
+
+	if (dev->bb_valid)
+		return 0;
+
+	timeout = jiffies + OMAP_I2C_TIMEOUT;
+	while (1) {
+		stat = omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG);
+		/*
+		 * We will see BB or BF event in a case IP had detected any
+		 * activity on the I2C bus. Now IP correctly tracks the bus
+		 * state. BB-bit value is valid.
+		 */
+		if (stat & (OMAP_I2C_STAT_BB | OMAP_I2C_STAT_BF))
+			break;
+
+		/*
+		 * Otherwise, we must look signals on the bus to make
+		 * the right decision.
+		 */
+		systest = omap_i2c_read_reg(dev, OMAP_I2C_SYSTEST_REG);
+		if ((systest & OMAP_I2C_SYSTEST_SCL_I_FUNC) &&
+		    (systest & OMAP_I2C_SYSTEST_SDA_I_FUNC)) {
+			if (!bus_free) {
+				bus_free_timeout = jiffies +
+					OMAP_I2C_BUS_FREE_TIMEOUT;
+				bus_free = 1;
+			}
+
+			/*
+			 * SDA and SCL lines was high for 10 ms without bus
+			 * activity detected. The bus is free. Consider
+			 * BB-bit value is valid.
+			 */
+			if (time_after(jiffies, bus_free_timeout))
+				break;
+		} else {
+			bus_free = 0;
+		}
+
+		if (time_after(jiffies, timeout)) {
+			dev_warn(dev->dev, "timeout waiting for bus ready\n");
+			return -ETIMEDOUT;
+		}
+
+		msleep(1);
+	}
+
+	dev->bb_valid = 1;
+	return 0;
+}
+
 static void omap_i2c_resize_fifo(struct omap_i2c_dev *dev, u8 size, bool is_rx)
 {
 	u16		buf;
@@ -557,7 +668,11 @@ static int omap_i2c_xfer_msg(struct i2c_adapter *adap,
 
 	if (!dev->b_hw && stop)
 		w |= OMAP_I2C_CON_STP;
-
+	/*
+	 * NOTE: STAT_BB bit could became 1 here if another master occupy
+	 * the bus. IP successfully complete transfer when the bus will be
+	 * free again (BB reset to 0).
+	 */
 	omap_i2c_write_reg(dev, OMAP_I2C_CON_REG, w);
 
 	/*
@@ -600,13 +715,15 @@ static int omap_i2c_xfer_msg(struct i2c_adapter *adap,
 		return 0;
 
 	/* We have an error */
-	if (dev->cmd_err & (OMAP_I2C_STAT_AL | OMAP_I2C_STAT_ROVR |
-			    OMAP_I2C_STAT_XUDF)) {
+	if (dev->cmd_err & (OMAP_I2C_STAT_ROVR | OMAP_I2C_STAT_XUDF)) {
 		omap_i2c_reset(dev);
 		__omap_i2c_init(dev);
 		return -EIO;
 	}
 
+	if (dev->cmd_err & OMAP_I2C_STAT_AL)
+		return -EAGAIN;
+
 	if (dev->cmd_err & OMAP_I2C_STAT_NACK) {
 		if (msg->flags & I2C_M_IGNORE_NAK)
 			return 0;
@@ -635,6 +752,10 @@ omap_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
 	if (r < 0)
 		goto out;
 
+	r = omap_i2c_wait_for_bb_valid(dev);
+	if (r < 0)
+		goto out;
+
 	r = omap_i2c_wait_for_bb(dev);
 	if (r < 0)
 		goto out;
@@ -1332,7 +1453,6 @@ static struct platform_driver omap_i2c_driver = {
 	.remove		= omap_i2c_remove,
 	.driver		= {
 		.name	= "omap_i2c",
-		.owner	= THIS_MODULE,
 		.pm	= OMAP_I2C_PM_OPS,
 		.of_match_table = of_match_ptr(omap_i2c_of_match),
 	},
diff --git a/drivers/i2c/busses/i2c-opal.c b/drivers/i2c/busses/i2c-opal.c
new file mode 100644
index 000000000000..16f90b1a7508
--- /dev/null
+++ b/drivers/i2c/busses/i2c-opal.c
@@ -0,0 +1,294 @@
+/*
+ * IBM OPAL I2C driver
+ * Copyright (C) 2014 IBM
+ *
+ * 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.
+ */
+
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include <asm/firmware.h>
+#include <asm/opal.h>
+
+static int i2c_opal_translate_error(int rc)
+{
+	switch (rc) {
+	case OPAL_NO_MEM:
+		return -ENOMEM;
+	case OPAL_PARAMETER:
+		return -EINVAL;
+	case OPAL_I2C_ARBT_LOST:
+		return -EAGAIN;
+	case OPAL_I2C_TIMEOUT:
+		return -ETIMEDOUT;
+	case OPAL_I2C_NACK_RCVD:
+		return -ENXIO;
+	case OPAL_I2C_STOP_ERR:
+		return -EBUSY;
+	default:
+		return -EIO;
+	}
+}
+
+static int i2c_opal_send_request(u32 bus_id, struct opal_i2c_request *req)
+{
+	struct opal_msg msg;
+	int token, rc;
+
+	token = opal_async_get_token_interruptible();
+	if (token < 0) {
+		if (token != -ERESTARTSYS)
+			pr_err("Failed to get the async token\n");
+
+		return token;
+	}
+
+	rc = opal_i2c_request(token, bus_id, req);
+	if (rc != OPAL_ASYNC_COMPLETION) {
+		rc = i2c_opal_translate_error(rc);
+		goto exit;
+	}
+
+	rc = opal_async_wait_response(token, &msg);
+	if (rc)
+		goto exit;
+
+	rc = be64_to_cpu(msg.params[1]);
+	if (rc != OPAL_SUCCESS) {
+		rc = i2c_opal_translate_error(rc);
+		goto exit;
+	}
+
+exit:
+	opal_async_release_token(token);
+	return rc;
+}
+
+static int i2c_opal_master_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
+				int num)
+{
+	unsigned long opal_id = (unsigned long)adap->algo_data;
+	struct opal_i2c_request req;
+	int rc, i;
+
+	/* We only support fairly simple combinations here of one
+	 * or two messages
+	 */
+	memset(&req, 0, sizeof(req));
+	switch(num) {
+	case 0:
+		return 0;
+	case 1:
+		req.type = (msgs[0].flags & I2C_M_RD) ?
+			OPAL_I2C_RAW_READ : OPAL_I2C_RAW_WRITE;
+		req.addr = cpu_to_be16(msgs[0].addr);
+		req.size = cpu_to_be32(msgs[0].len);
+		req.buffer_ra = cpu_to_be64(__pa(msgs[0].buf));
+		break;
+	case 2:
+		/* For two messages, we basically support only simple
+		 * smbus transactions of a write plus a read. We might
+		 * want to allow also two writes but we'd have to bounce
+		 * the data into a single buffer.
+		 */
+		if ((msgs[0].flags & I2C_M_RD) || !(msgs[1].flags & I2C_M_RD))
+			return -EOPNOTSUPP;
+		if (msgs[0].len > 4)
+			return -EOPNOTSUPP;
+		if (msgs[0].addr != msgs[1].addr)
+			return -EOPNOTSUPP;
+		req.type = OPAL_I2C_SM_READ;
+		req.addr = cpu_to_be16(msgs[0].addr);
+		req.subaddr_sz = msgs[0].len;
+		for (i = 0; i < msgs[0].len; i++)
+			req.subaddr = (req.subaddr << 8) | msgs[0].buf[i];
+		req.subaddr = cpu_to_be32(req.subaddr);
+		req.size = cpu_to_be32(msgs[1].len);
+		req.buffer_ra = cpu_to_be64(__pa(msgs[1].buf));
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	rc = i2c_opal_send_request(opal_id, &req);
+	if (rc)
+		return rc;
+
+	return num;
+}
+
+static int i2c_opal_smbus_xfer(struct i2c_adapter *adap, u16 addr,
+			       unsigned short flags, char read_write,
+			       u8 command, int size, union i2c_smbus_data *data)
+{
+	unsigned long opal_id = (unsigned long)adap->algo_data;
+	struct opal_i2c_request req;
+	u8 local[2];
+	int rc;
+
+	memset(&req, 0, sizeof(req));
+
+	req.addr = cpu_to_be16(addr);
+	switch (size) {
+	case I2C_SMBUS_BYTE:
+		req.buffer_ra = cpu_to_be64(__pa(&data->byte));
+		req.size = cpu_to_be32(1);
+		/* Fall through */
+	case I2C_SMBUS_QUICK:
+		req.type = (read_write == I2C_SMBUS_READ) ?
+			OPAL_I2C_RAW_READ : OPAL_I2C_RAW_WRITE;
+		break;
+	case I2C_SMBUS_BYTE_DATA:
+		req.buffer_ra = cpu_to_be64(__pa(&data->byte));
+		req.size = cpu_to_be32(1);
+		req.subaddr = cpu_to_be32(command);
+		req.subaddr_sz = 1;
+		req.type = (read_write == I2C_SMBUS_READ) ?
+			OPAL_I2C_SM_READ : OPAL_I2C_SM_WRITE;
+		break;
+	case I2C_SMBUS_WORD_DATA:
+		if (!read_write) {
+			local[0] = data->word & 0xff;
+			local[1] = (data->word >> 8) & 0xff;
+		}
+		req.buffer_ra = cpu_to_be64(__pa(local));
+		req.size = cpu_to_be32(2);
+		req.subaddr = cpu_to_be32(command);
+		req.subaddr_sz = 1;
+		req.type = (read_write == I2C_SMBUS_READ) ?
+			OPAL_I2C_SM_READ : OPAL_I2C_SM_WRITE;
+		break;
+	case I2C_SMBUS_I2C_BLOCK_DATA:
+		req.buffer_ra = cpu_to_be64(__pa(&data->block[1]));
+		req.size = cpu_to_be32(data->block[0]);
+		req.subaddr = cpu_to_be32(command);
+		req.subaddr_sz = 1;
+		req.type = (read_write == I2C_SMBUS_READ) ?
+			OPAL_I2C_SM_READ : OPAL_I2C_SM_WRITE;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	rc = i2c_opal_send_request(opal_id, &req);
+	if (!rc && read_write && size == I2C_SMBUS_WORD_DATA) {
+		data->word = ((u16)local[1]) << 8;
+		data->word |= local[0];
+	}
+
+	return rc;
+}
+
+static u32 i2c_opal_func(struct i2c_adapter *adapter)
+{
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_QUICK | I2C_FUNC_SMBUS_BYTE |
+	       I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_WORD_DATA |
+	       I2C_FUNC_SMBUS_I2C_BLOCK;
+}
+
+static const struct i2c_algorithm i2c_opal_algo = {
+	.master_xfer	= i2c_opal_master_xfer,
+	.smbus_xfer	= i2c_opal_smbus_xfer,
+	.functionality	= i2c_opal_func,
+};
+
+static int i2c_opal_probe(struct platform_device *pdev)
+{
+	struct i2c_adapter	*adapter;
+	const char		*pname;
+	u32			opal_id;
+	int			rc;
+
+	if (!pdev->dev.of_node)
+		return -ENODEV;
+
+	rc = of_property_read_u32(pdev->dev.of_node, "ibm,opal-id", &opal_id);
+	if (rc) {
+		dev_err(&pdev->dev, "Missing ibm,opal-id property !\n");
+		return -EIO;
+	}
+
+	adapter = devm_kzalloc(&pdev->dev, sizeof(*adapter), GFP_KERNEL);
+	if (!adapter)
+		return -ENOMEM;
+
+	adapter->algo = &i2c_opal_algo;
+	adapter->algo_data = (void *)(unsigned long)opal_id;
+	adapter->dev.parent = &pdev->dev;
+	adapter->dev.of_node = of_node_get(pdev->dev.of_node);
+	pname = of_get_property(pdev->dev.of_node, "ibm,port-name", NULL);
+	if (pname)
+		strlcpy(adapter->name, pname, sizeof(adapter->name));
+	else
+		strlcpy(adapter->name, "opal", sizeof(adapter->name));
+
+	platform_set_drvdata(pdev, adapter);
+	rc = i2c_add_adapter(adapter);
+	if (rc)
+		dev_err(&pdev->dev, "Failed to register the i2c adapter\n");
+
+	return rc;
+}
+
+static int i2c_opal_remove(struct platform_device *pdev)
+{
+	struct i2c_adapter *adapter = platform_get_drvdata(pdev);
+
+	i2c_del_adapter(adapter);
+
+	return 0;
+}
+
+static const struct of_device_id i2c_opal_of_match[] = {
+	{
+		.compatible = "ibm,opal-i2c",
+	},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, i2c_opal_of_match);
+
+static struct platform_driver i2c_opal_driver = {
+	.probe	= i2c_opal_probe,
+	.remove	= i2c_opal_remove,
+	.driver	= {
+		.name		= "i2c-opal",
+		.of_match_table	= i2c_opal_of_match,
+	},
+};
+
+static int __init i2c_opal_init(void)
+{
+	if (!firmware_has_feature(FW_FEATURE_OPAL))
+		return -ENODEV;
+
+	return platform_driver_register(&i2c_opal_driver);
+}
+module_init(i2c_opal_init);
+
+static void __exit i2c_opal_exit(void)
+{
+	return platform_driver_unregister(&i2c_opal_driver);
+}
+module_exit(i2c_opal_exit);
+
+MODULE_AUTHOR("Neelesh Gupta <neelegup@linux.vnet.ibm.com>");
+MODULE_DESCRIPTION("IBM OPAL I2C driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/i2c/busses/i2c-parport-light.c b/drivers/i2c/busses/i2c-parport-light.c
index d1f625f923c7..1bcdd10b68b9 100644
--- a/drivers/i2c/busses/i2c-parport-light.c
+++ b/drivers/i2c/busses/i2c-parport-light.c
@@ -183,7 +183,6 @@ static int i2c_parport_remove(struct platform_device *pdev)
 
 static struct platform_driver i2c_parport_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= DRVNAME,
 	},
 	.probe		= i2c_parport_probe,
diff --git a/drivers/i2c/busses/i2c-pca-platform.c b/drivers/i2c/busses/i2c-pca-platform.c
index 845f12598e79..6336f02ec566 100644
--- a/drivers/i2c/busses/i2c-pca-platform.c
+++ b/drivers/i2c/busses/i2c-pca-platform.c
@@ -280,7 +280,6 @@ static struct platform_driver i2c_pca_pf_driver = {
 	.remove = i2c_pca_pf_remove,
 	.driver = {
 		.name = "i2c-pca-platform",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/i2c/busses/i2c-pmcmsp.c b/drivers/i2c/busses/i2c-pmcmsp.c
index 177834e2d841..44f03eed00dd 100644
--- a/drivers/i2c/busses/i2c-pmcmsp.c
+++ b/drivers/i2c/busses/i2c-pmcmsp.c
@@ -624,7 +624,6 @@ static struct platform_driver pmcmsptwi_driver = {
 	.remove	= pmcmsptwi_remove,
 	.driver = {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/i2c/busses/i2c-pnx.c b/drivers/i2c/busses/i2c-pnx.c
index dc7ff829ad78..e814a36d9b78 100644
--- a/drivers/i2c/busses/i2c-pnx.c
+++ b/drivers/i2c/busses/i2c-pnx.c
@@ -751,7 +751,6 @@ MODULE_DEVICE_TABLE(of, i2c_pnx_of_match);
 static struct platform_driver i2c_pnx_driver = {
 	.driver = {
 		.name = "pnx-i2c",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(i2c_pnx_of_match),
 		.pm = PNX_I2C_PM,
 	},
diff --git a/drivers/i2c/busses/i2c-puv3.c b/drivers/i2c/busses/i2c-puv3.c
index c83fc3ccdd2b..82b6f02544da 100644
--- a/drivers/i2c/busses/i2c-puv3.c
+++ b/drivers/i2c/busses/i2c-puv3.c
@@ -270,7 +270,6 @@ static struct platform_driver puv3_i2c_driver = {
 	.remove		= puv3_i2c_remove,
 	.driver		= {
 		.name	= "PKUnity-v3-I2C",
-		.owner	= THIS_MODULE,
 		.pm	= PUV3_I2C_PM,
 	}
 };
diff --git a/drivers/i2c/busses/i2c-pxa.c b/drivers/i2c/busses/i2c-pxa.c
index be671f7a0e06..d9c0d6a17ad6 100644
--- a/drivers/i2c/busses/i2c-pxa.c
+++ b/drivers/i2c/busses/i2c-pxa.c
@@ -885,7 +885,9 @@ static void i2c_pxa_irq_txempty(struct pxa_i2c *i2c, u32 isr)
 		return; /* ignore */
 	}
 
-	if (isr & ISR_BED) {
+	if ((isr & ISR_BED) &&
+		(!((i2c->msg->flags & I2C_M_IGNORE_NAK) &&
+			(isr & ISR_ACKNAK)))) {
 		int ret = BUS_ERROR;
 
 		/*
@@ -919,12 +921,14 @@ static void i2c_pxa_irq_txempty(struct pxa_i2c *i2c, u32 isr)
 		icr |= ICR_ALDIE | ICR_TB;
 
 		/*
-		 * If this is the last byte of the last message, send
-		 * a STOP.
+		 * If this is the last byte of the last message or last byte
+		 * of any message with I2C_M_STOP (e.g. SCCB), send a STOP.
 		 */
-		if (i2c->msg_ptr == i2c->msg->len &&
-		    i2c->msg_idx == i2c->msg_num - 1)
-			icr |= ICR_STOP;
+		if ((i2c->msg_ptr == i2c->msg->len) &&
+			((i2c->msg->flags & I2C_M_STOP) ||
+			(i2c->msg_idx == i2c->msg_num - 1)))
+				icr |= ICR_STOP;
+
 	} else if (i2c->msg_idx < i2c->msg_num - 1) {
 		/*
 		 * Next segment of the message.
@@ -1071,7 +1075,8 @@ static int i2c_pxa_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num
 
 static u32 i2c_pxa_functionality(struct i2c_adapter *adap)
 {
-	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL |
+		I2C_FUNC_PROTOCOL_MANGLING | I2C_FUNC_NOSTART;
 }
 
 static const struct i2c_algorithm i2c_pxa_algorithm = {
@@ -1328,7 +1333,6 @@ static struct platform_driver i2c_pxa_driver = {
 	.remove		= i2c_pxa_remove,
 	.driver		= {
 		.name	= "pxa2xx-i2c",
-		.owner	= THIS_MODULE,
 		.pm	= I2C_PXA_DEV_PM_OPS,
 		.of_match_table = i2c_pxa_dt_ids,
 	},
diff --git a/drivers/i2c/busses/i2c-qup.c b/drivers/i2c/busses/i2c-qup.c
index 092d89bd3224..4dad23bdffbe 100644
--- a/drivers/i2c/busses/i2c-qup.c
+++ b/drivers/i2c/busses/i2c-qup.c
@@ -764,7 +764,6 @@ static struct platform_driver qup_i2c_driver = {
 	.remove = qup_i2c_remove,
 	.driver = {
 		.name = "i2c_qup",
-		.owner = THIS_MODULE,
 		.pm = &qup_i2c_qup_pm_ops,
 		.of_match_table = qup_i2c_dt_match,
 	},
diff --git a/drivers/i2c/busses/i2c-rcar.c b/drivers/i2c/busses/i2c-rcar.c
index d826e82dd997..71a6e07eb7ab 100644
--- a/drivers/i2c/busses/i2c-rcar.c
+++ b/drivers/i2c/busses/i2c-rcar.c
@@ -48,6 +48,12 @@
 #define ICMAR	0x20	/* master address */
 #define ICRXTX	0x24	/* data port */
 
+/* ICSCR */
+#define SDBS	(1 << 3)	/* slave data buffer select */
+#define SIE	(1 << 2)	/* slave interface enable */
+#define GCAE	(1 << 1)	/* general call address enable */
+#define FNA	(1 << 0)	/* forced non acknowledgment */
+
 /* ICMCR */
 #define MDBS	(1 << 7)	/* non-fifo mode switch */
 #define FSCL	(1 << 6)	/* override SCL pin */
@@ -58,6 +64,15 @@
 #define FSB	(1 << 1)	/* force stop bit */
 #define ESG	(1 << 0)	/* en startbit gen */
 
+/* ICSSR (also for ICSIER) */
+#define GCAR	(1 << 6)	/* general call received */
+#define STM	(1 << 5)	/* slave transmit mode */
+#define SSR	(1 << 4)	/* stop received */
+#define SDE	(1 << 3)	/* slave data empty */
+#define SDT	(1 << 2)	/* slave data transmitted */
+#define SDR	(1 << 1)	/* slave data received */
+#define SAR	(1 << 0)	/* slave addr received */
+
 /* ICMSR (also for ICMIE) */
 #define MNR	(1 << 6)	/* nack received */
 #define MAL	(1 << 5)	/* arbitration lost */
@@ -103,6 +118,7 @@ struct rcar_i2c_priv {
 	u32 icccr;
 	u32 flags;
 	enum rcar_i2c_type devtype;
+	struct i2c_client *slave;
 };
 
 #define rcar_i2c_priv_to_dev(p)		((p)->adap.dev.parent)
@@ -126,15 +142,6 @@ static u32 rcar_i2c_read(struct rcar_i2c_priv *priv, int reg)
 
 static void rcar_i2c_init(struct rcar_i2c_priv *priv)
 {
-	/*
-	 * reset slave mode.
-	 * slave mode is not used on this driver
-	 */
-	rcar_i2c_write(priv, ICSIER, 0);
-	rcar_i2c_write(priv, ICSAR, 0);
-	rcar_i2c_write(priv, ICSCR, 0);
-	rcar_i2c_write(priv, ICSSR, 0);
-
 	/* reset master mode */
 	rcar_i2c_write(priv, ICMIER, 0);
 	rcar_i2c_write(priv, ICMCR, 0);
@@ -360,6 +367,63 @@ static int rcar_i2c_irq_recv(struct rcar_i2c_priv *priv, u32 msr)
 	return 0;
 }
 
+static bool rcar_i2c_slave_irq(struct rcar_i2c_priv *priv)
+{
+	u32 ssr_raw, ssr_filtered;
+	u8 value;
+
+	ssr_raw = rcar_i2c_read(priv, ICSSR) & 0xff;
+	ssr_filtered = ssr_raw & rcar_i2c_read(priv, ICSIER);
+
+	if (!ssr_filtered)
+		return false;
+
+	/* address detected */
+	if (ssr_filtered & SAR) {
+		/* read or write request */
+		if (ssr_raw & STM) {
+			i2c_slave_event(priv->slave, I2C_SLAVE_REQ_READ_START, &value);
+			rcar_i2c_write(priv, ICRXTX, value);
+			rcar_i2c_write(priv, ICSIER, SDE | SSR | SAR);
+		} else {
+			i2c_slave_event(priv->slave, I2C_SLAVE_REQ_WRITE_START, &value);
+			rcar_i2c_read(priv, ICRXTX);	/* dummy read */
+			rcar_i2c_write(priv, ICSIER, SDR | SSR | SAR);
+		}
+
+		rcar_i2c_write(priv, ICSSR, ~SAR & 0xff);
+	}
+
+	/* master sent stop */
+	if (ssr_filtered & SSR) {
+		i2c_slave_event(priv->slave, I2C_SLAVE_STOP, &value);
+		rcar_i2c_write(priv, ICSIER, SAR | SSR);
+		rcar_i2c_write(priv, ICSSR, ~SSR & 0xff);
+	}
+
+	/* master wants to write to us */
+	if (ssr_filtered & SDR) {
+		int ret;
+
+		value = rcar_i2c_read(priv, ICRXTX);
+		ret = i2c_slave_event(priv->slave, I2C_SLAVE_REQ_WRITE_END, &value);
+		/* Send NACK in case of error */
+		rcar_i2c_write(priv, ICSCR, SIE | SDBS | (ret < 0 ? FNA : 0));
+		i2c_slave_event(priv->slave, I2C_SLAVE_REQ_WRITE_START, &value);
+		rcar_i2c_write(priv, ICSSR, ~SDR & 0xff);
+	}
+
+	/* master wants to read from us */
+	if (ssr_filtered & SDE) {
+		i2c_slave_event(priv->slave, I2C_SLAVE_REQ_READ_END, &value);
+		i2c_slave_event(priv->slave, I2C_SLAVE_REQ_READ_START, &value);
+		rcar_i2c_write(priv, ICRXTX, value);
+		rcar_i2c_write(priv, ICSSR, ~SDE & 0xff);
+	}
+
+	return true;
+}
+
 static irqreturn_t rcar_i2c_irq(int irq, void *ptr)
 {
 	struct rcar_i2c_priv *priv = ptr;
@@ -369,6 +433,9 @@ static irqreturn_t rcar_i2c_irq(int irq, void *ptr)
 	/*-------------- spin lock -----------------*/
 	spin_lock(&priv->lock);
 
+	if (rcar_i2c_slave_irq(priv))
+		goto exit;
+
 	msr = rcar_i2c_read(priv, ICMSR);
 
 	/* Only handle interrupts that are currently enabled */
@@ -499,6 +566,43 @@ out:
 	return ret;
 }
 
+static int rcar_reg_slave(struct i2c_client *slave)
+{
+	struct rcar_i2c_priv *priv = i2c_get_adapdata(slave->adapter);
+
+	if (priv->slave)
+		return -EBUSY;
+
+	if (slave->flags & I2C_CLIENT_TEN)
+		return -EAFNOSUPPORT;
+
+	pm_runtime_forbid(rcar_i2c_priv_to_dev(priv));
+
+	priv->slave = slave;
+	rcar_i2c_write(priv, ICSAR, slave->addr);
+	rcar_i2c_write(priv, ICSSR, 0);
+	rcar_i2c_write(priv, ICSIER, SAR | SSR);
+	rcar_i2c_write(priv, ICSCR, SIE | SDBS);
+
+	return 0;
+}
+
+static int rcar_unreg_slave(struct i2c_client *slave)
+{
+	struct rcar_i2c_priv *priv = i2c_get_adapdata(slave->adapter);
+
+	WARN_ON(!priv->slave);
+
+	rcar_i2c_write(priv, ICSIER, 0);
+	rcar_i2c_write(priv, ICSCR, 0);
+
+	priv->slave = NULL;
+
+	pm_runtime_allow(rcar_i2c_priv_to_dev(priv));
+
+	return 0;
+}
+
 static u32 rcar_i2c_func(struct i2c_adapter *adap)
 {
 	/* This HW can't do SMBUS_QUICK and NOSTART */
@@ -508,6 +612,8 @@ static u32 rcar_i2c_func(struct i2c_adapter *adap)
 static const struct i2c_algorithm rcar_i2c_algo = {
 	.master_xfer	= rcar_i2c_master_xfer,
 	.functionality	= rcar_i2c_func,
+	.reg_slave	= rcar_reg_slave,
+	.unreg_slave	= rcar_unreg_slave,
 };
 
 static const struct of_device_id rcar_i2c_dt_ids[] = {
@@ -620,7 +726,6 @@ MODULE_DEVICE_TABLE(platform, rcar_i2c_id_table);
 static struct platform_driver rcar_i2c_driver = {
 	.driver	= {
 		.name	= "i2c-rcar",
-		.owner	= THIS_MODULE,
 		.of_match_table = rcar_i2c_dt_ids,
 	},
 	.probe		= rcar_i2c_probe,
diff --git a/drivers/i2c/busses/i2c-riic.c b/drivers/i2c/busses/i2c-riic.c
index af3b3d032a9f..d7e3af671543 100644
--- a/drivers/i2c/busses/i2c-riic.c
+++ b/drivers/i2c/busses/i2c-riic.c
@@ -414,7 +414,6 @@ static struct platform_driver riic_i2c_driver = {
 	.remove		= riic_i2c_remove,
 	.driver		= {
 		.name	= "i2c-riic",
-		.owner	= THIS_MODULE,
 		.of_match_table = riic_i2c_dt_ids,
 	},
 };
diff --git a/drivers/i2c/busses/i2c-rk3x.c b/drivers/i2c/busses/i2c-rk3x.c
index f486d0eac4d0..92462843db66 100644
--- a/drivers/i2c/busses/i2c-rk3x.c
+++ b/drivers/i2c/busses/i2c-rk3x.c
@@ -24,6 +24,7 @@
 #include <linux/wait.h>
 #include <linux/mfd/syscon.h>
 #include <linux/regmap.h>
+#include <linux/math64.h>
 
 
 /* Register Map */
@@ -97,6 +98,7 @@ struct rk3x_i2c {
 	/* Hardware resources */
 	void __iomem *regs;
 	struct clk *clk;
+	struct notifier_block clk_rate_nb;
 
 	/* Settings */
 	unsigned int scl_frequency;
@@ -428,18 +430,231 @@ out:
 	return IRQ_HANDLED;
 }
 
-static void rk3x_i2c_set_scl_rate(struct rk3x_i2c *i2c, unsigned long scl_rate)
+/**
+ * Calculate divider values for desired SCL frequency
+ *
+ * @clk_rate: I2C input clock rate
+ * @scl_rate: Desired SCL rate
+ * @div_low: Divider output for low
+ * @div_high: Divider output for high
+ *
+ * Returns: 0 on success, -EINVAL if the goal SCL rate is too slow. In that case
+ * a best-effort divider value is returned in divs. If the target rate is
+ * too high, we silently use the highest possible rate.
+ */
+static int rk3x_i2c_calc_divs(unsigned long clk_rate, unsigned long scl_rate,
+			      unsigned long *div_low, unsigned long *div_high)
 {
-	unsigned long i2c_rate = clk_get_rate(i2c->clk);
-	unsigned int div;
+	unsigned long min_low_ns, min_high_ns;
+	unsigned long max_data_hold_ns;
+	unsigned long data_hold_buffer_ns;
+	unsigned long max_low_ns, min_total_ns;
+
+	unsigned long clk_rate_khz, scl_rate_khz;
+
+	unsigned long min_low_div, min_high_div;
+	unsigned long max_low_div;
+
+	unsigned long min_div_for_hold, min_total_div;
+	unsigned long extra_div, extra_low_div, ideal_low_div;
+
+	int ret = 0;
+
+	/* Only support standard-mode and fast-mode */
+	if (WARN_ON(scl_rate > 400000))
+		scl_rate = 400000;
+
+	/* prevent scl_rate_khz from becoming 0 */
+	if (WARN_ON(scl_rate < 1000))
+		scl_rate = 1000;
+
+	/*
+	 * min_low_ns:  The minimum number of ns we need to hold low
+	 *		to meet i2c spec
+	 * min_high_ns: The minimum number of ns we need to hold high
+	 *		to meet i2c spec
+	 * max_low_ns:  The maximum number of ns we can hold low
+	 *		to meet i2c spec
+	 *
+	 * Note: max_low_ns should be (max data hold time * 2 - buffer)
+	 *	 This is because the i2c host on Rockchip holds the data line
+	 *	 for half the low time.
+	 */
+	if (scl_rate <= 100000) {
+		min_low_ns = 4700;
+		min_high_ns = 4000;
+		max_data_hold_ns = 3450;
+		data_hold_buffer_ns = 50;
+	} else {
+		min_low_ns = 1300;
+		min_high_ns = 600;
+		max_data_hold_ns = 900;
+		data_hold_buffer_ns = 50;
+	}
+	max_low_ns = max_data_hold_ns * 2 - data_hold_buffer_ns;
+	min_total_ns = min_low_ns + min_high_ns;
+
+	/* Adjust to avoid overflow */
+	clk_rate_khz = DIV_ROUND_UP(clk_rate, 1000);
+	scl_rate_khz = scl_rate / 1000;
+
+	/*
+	 * We need the total div to be >= this number
+	 * so we don't clock too fast.
+	 */
+	min_total_div = DIV_ROUND_UP(clk_rate_khz, scl_rate_khz * 8);
+
+	/* These are the min dividers needed for min hold times. */
+	min_low_div = DIV_ROUND_UP(clk_rate_khz * min_low_ns, 8 * 1000000);
+	min_high_div = DIV_ROUND_UP(clk_rate_khz * min_high_ns, 8 * 1000000);
+	min_div_for_hold = (min_low_div + min_high_div);
+
+	/*
+	 * This is the maximum divider so we don't go over the max.
+	 * We don't round up here (we round down) since this is a max.
+	 */
+	max_low_div = clk_rate_khz * max_low_ns / (8 * 1000000);
+
+	if (min_low_div > max_low_div) {
+		WARN_ONCE(true,
+			  "Conflicting, min_low_div %lu, max_low_div %lu\n",
+			  min_low_div, max_low_div);
+		max_low_div = min_low_div;
+	}
+
+	if (min_div_for_hold > min_total_div) {
+		/*
+		 * Time needed to meet hold requirements is important.
+		 * Just use that.
+		 */
+		*div_low = min_low_div;
+		*div_high = min_high_div;
+	} else {
+		/*
+		 * We've got to distribute some time among the low and high
+		 * so we don't run too fast.
+		 */
+		extra_div = min_total_div - min_div_for_hold;
+
+		/*
+		 * We'll try to split things up perfectly evenly,
+		 * biasing slightly towards having a higher div
+		 * for low (spend more time low).
+		 */
+		ideal_low_div = DIV_ROUND_UP(clk_rate_khz * min_low_ns,
+					     scl_rate_khz * 8 * min_total_ns);
+
+		/* Don't allow it to go over the max */
+		if (ideal_low_div > max_low_div)
+			ideal_low_div = max_low_div;
+
+		/*
+		 * Handle when the ideal low div is going to take up
+		 * more than we have.
+		 */
+		if (ideal_low_div > min_low_div + extra_div)
+			ideal_low_div = min_low_div + extra_div;
 
-	/* set DIV = DIVH = DIVL
-	 * SCL rate = (clk rate) / (8 * (DIVH + 1 + DIVL + 1))
-	 *          = (clk rate) / (16 * (DIV + 1))
+		/* Give low the "ideal" and give high whatever extra is left */
+		extra_low_div = ideal_low_div - min_low_div;
+		*div_low = ideal_low_div;
+		*div_high = min_high_div + (extra_div - extra_low_div);
+	}
+
+	/*
+	 * Adjust to the fact that the hardware has an implicit "+1".
+	 * NOTE: Above calculations always produce div_low > 0 and div_high > 0.
 	 */
-	div = DIV_ROUND_UP(i2c_rate, scl_rate * 16) - 1;
+	*div_low = *div_low - 1;
+	*div_high = *div_high - 1;
+
+	/* Maximum divider supported by hw is 0xffff */
+	if (*div_low > 0xffff) {
+		*div_low = 0xffff;
+		ret = -EINVAL;
+	}
+
+	if (*div_high > 0xffff) {
+		*div_high = 0xffff;
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static void rk3x_i2c_adapt_div(struct rk3x_i2c *i2c, unsigned long clk_rate)
+{
+	unsigned long div_low, div_high;
+	u64 t_low_ns, t_high_ns;
+	int ret;
+
+	ret = rk3x_i2c_calc_divs(clk_rate, i2c->scl_frequency, &div_low,
+				 &div_high);
+
+	WARN_ONCE(ret != 0, "Could not reach SCL freq %u", i2c->scl_frequency);
+
+	clk_enable(i2c->clk);
+	i2c_writel(i2c, (div_high << 16) | (div_low & 0xffff), REG_CLKDIV);
+	clk_disable(i2c->clk);
 
-	i2c_writel(i2c, (div << 16) | (div & 0xffff), REG_CLKDIV);
+	t_low_ns = div_u64(((u64)div_low + 1) * 8 * 1000000000, clk_rate);
+	t_high_ns = div_u64(((u64)div_high + 1) * 8 * 1000000000, clk_rate);
+	dev_dbg(i2c->dev,
+		"CLK %lukhz, Req %uns, Act low %lluns high %lluns\n",
+		clk_rate / 1000,
+		1000000000 / i2c->scl_frequency,
+		t_low_ns, t_high_ns);
+}
+
+/**
+ * rk3x_i2c_clk_notifier_cb - Clock rate change callback
+ * @nb:		Pointer to notifier block
+ * @event:	Notification reason
+ * @data:	Pointer to notification data object
+ *
+ * The callback checks whether a valid bus frequency can be generated after the
+ * change. If so, the change is acknowledged, otherwise the change is aborted.
+ * New dividers are written to the HW in the pre- or post change notification
+ * depending on the scaling direction.
+ *
+ * Code adapted from i2c-cadence.c.
+ *
+ * Return:	NOTIFY_STOP if the rate change should be aborted, NOTIFY_OK
+ *		to acknowedge the change, NOTIFY_DONE if the notification is
+ *		considered irrelevant.
+ */
+static int rk3x_i2c_clk_notifier_cb(struct notifier_block *nb, unsigned long
+				    event, void *data)
+{
+	struct clk_notifier_data *ndata = data;
+	struct rk3x_i2c *i2c = container_of(nb, struct rk3x_i2c, clk_rate_nb);
+	unsigned long div_low, div_high;
+
+	switch (event) {
+	case PRE_RATE_CHANGE:
+		if (rk3x_i2c_calc_divs(ndata->new_rate, i2c->scl_frequency,
+				      &div_low, &div_high) != 0) {
+			return NOTIFY_STOP;
+		}
+
+		/* scale up */
+		if (ndata->new_rate > ndata->old_rate)
+			rk3x_i2c_adapt_div(i2c, ndata->new_rate);
+
+		return NOTIFY_OK;
+	case POST_RATE_CHANGE:
+		/* scale down */
+		if (ndata->new_rate < ndata->old_rate)
+			rk3x_i2c_adapt_div(i2c, ndata->new_rate);
+		return NOTIFY_OK;
+	case ABORT_RATE_CHANGE:
+		/* scale up */
+		if (ndata->new_rate > ndata->old_rate)
+			rk3x_i2c_adapt_div(i2c, ndata->old_rate);
+		return NOTIFY_OK;
+	default:
+		return NOTIFY_DONE;
+	}
 }
 
 /**
@@ -536,9 +751,6 @@ static int rk3x_i2c_xfer(struct i2c_adapter *adap,
 
 	clk_enable(i2c->clk);
 
-	/* The clock rate might have changed, so setup the divider again */
-	rk3x_i2c_set_scl_rate(i2c, i2c->scl_frequency);
-
 	i2c->is_last_msg = false;
 
 	/*
@@ -624,6 +836,7 @@ static int rk3x_i2c_probe(struct platform_device *pdev)
 	int bus_nr;
 	u32 value;
 	int irq;
+	unsigned long clk_rate;
 
 	i2c = devm_kzalloc(&pdev->dev, sizeof(struct rk3x_i2c), GFP_KERNEL);
 	if (!i2c)
@@ -724,16 +937,28 @@ static int rk3x_i2c_probe(struct platform_device *pdev)
 		return ret;
 	}
 
+	i2c->clk_rate_nb.notifier_call = rk3x_i2c_clk_notifier_cb;
+	ret = clk_notifier_register(i2c->clk, &i2c->clk_rate_nb);
+	if (ret != 0) {
+		dev_err(&pdev->dev, "Unable to register clock notifier\n");
+		goto err_clk;
+	}
+
+	clk_rate = clk_get_rate(i2c->clk);
+	rk3x_i2c_adapt_div(i2c, clk_rate);
+
 	ret = i2c_add_adapter(&i2c->adap);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Could not register adapter\n");
-		goto err_clk;
+		goto err_clk_notifier;
 	}
 
 	dev_info(&pdev->dev, "Initialized RK3xxx I2C bus at %p\n", i2c->regs);
 
 	return 0;
 
+err_clk_notifier:
+	clk_notifier_unregister(i2c->clk, &i2c->clk_rate_nb);
 err_clk:
 	clk_unprepare(i2c->clk);
 	return ret;
@@ -744,6 +969,8 @@ static int rk3x_i2c_remove(struct platform_device *pdev)
 	struct rk3x_i2c *i2c = platform_get_drvdata(pdev);
 
 	i2c_del_adapter(&i2c->adap);
+
+	clk_notifier_unregister(i2c->clk, &i2c->clk_rate_nb);
 	clk_unprepare(i2c->clk);
 
 	return 0;
@@ -753,7 +980,6 @@ static struct platform_driver rk3x_i2c_driver = {
 	.probe   = rk3x_i2c_probe,
 	.remove  = rk3x_i2c_remove,
 	.driver  = {
-		.owner = THIS_MODULE,
 		.name  = "rk3x-i2c",
 		.of_match_table = rk3x_i2c_match,
 	},
diff --git a/drivers/i2c/busses/i2c-s3c2410.c b/drivers/i2c/busses/i2c-s3c2410.c
index 65244774bfa3..bff20a589621 100644
--- a/drivers/i2c/busses/i2c-s3c2410.c
+++ b/drivers/i2c/busses/i2c-s3c2410.c
@@ -35,6 +35,8 @@
 #include <linux/of.h>
 #include <linux/of_gpio.h>
 #include <linux/pinctrl/consumer.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
 
 #include <asm/irq.h>
 
@@ -87,6 +89,9 @@
 /* Max time to wait for bus to become idle after a xfer (in us) */
 #define S3C2410_IDLE_TIMEOUT	5000
 
+/* Exynos5 Sysreg offset */
+#define EXYNOS5_SYS_I2C_CFG	0x0234
+
 /* i2c controller state */
 enum s3c24xx_i2c_state {
 	STATE_IDLE,
@@ -123,6 +128,8 @@ struct s3c24xx_i2c {
 #if defined(CONFIG_ARM_S3C24XX_CPUFREQ)
 	struct notifier_block	freq_transition;
 #endif
+	struct regmap		*sysreg;
+	unsigned int		sys_i2c_cfg;
 };
 
 static struct platform_device_id s3c24xx_driver_ids[] = {
@@ -1071,6 +1078,7 @@ static void
 s3c24xx_i2c_parse_dt(struct device_node *np, struct s3c24xx_i2c *i2c)
 {
 	struct s3c2410_platform_i2c *pdata = i2c->pdata;
+	int id;
 
 	if (!np)
 		return;
@@ -1080,6 +1088,21 @@ s3c24xx_i2c_parse_dt(struct device_node *np, struct s3c24xx_i2c *i2c)
 	of_property_read_u32(np, "samsung,i2c-slave-addr", &pdata->slave_addr);
 	of_property_read_u32(np, "samsung,i2c-max-bus-freq",
 				(u32 *)&pdata->frequency);
+	/*
+	 * Exynos5's legacy i2c controller and new high speed i2c
+	 * controller have muxed interrupt sources. By default the
+	 * interrupts for 4-channel HS-I2C controller are enabled.
+	 * If nodes for first four channels of legacy i2c controller
+	 * are available then re-configure the interrupts via the
+	 * system register.
+	 */
+	id = of_alias_get_id(np, "i2c");
+	i2c->sysreg = syscon_regmap_lookup_by_phandle(np,
+			"samsung,sysreg-phandle");
+	if (IS_ERR(i2c->sysreg))
+		return;
+
+	regmap_update_bits(i2c->sysreg, EXYNOS5_SYS_I2C_CFG, BIT(id), 0);
 }
 #else
 static void
@@ -1260,6 +1283,9 @@ static int s3c24xx_i2c_suspend_noirq(struct device *dev)
 
 	i2c->suspended = 1;
 
+	if (!IS_ERR(i2c->sysreg))
+		regmap_read(i2c->sysreg, EXYNOS5_SYS_I2C_CFG, &i2c->sys_i2c_cfg);
+
 	return 0;
 }
 
@@ -1268,6 +1294,9 @@ static int s3c24xx_i2c_resume_noirq(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct s3c24xx_i2c *i2c = platform_get_drvdata(pdev);
 
+	if (!IS_ERR(i2c->sysreg))
+		regmap_write(i2c->sysreg, EXYNOS5_SYS_I2C_CFG, i2c->sys_i2c_cfg);
+
 	clk_prepare_enable(i2c->clk);
 	s3c24xx_i2c_init(i2c);
 	clk_disable_unprepare(i2c->clk);
@@ -1301,7 +1330,6 @@ static struct platform_driver s3c24xx_i2c_driver = {
 	.remove		= s3c24xx_i2c_remove,
 	.id_table	= s3c24xx_driver_ids,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "s3c-i2c",
 		.pm	= S3C24XX_DEV_PM_OPS,
 		.of_match_table = of_match_ptr(s3c24xx_i2c_match),
diff --git a/drivers/i2c/busses/i2c-sh7760.c b/drivers/i2c/busses/i2c-sh7760.c
index d76f3d9737ec..24968384b401 100644
--- a/drivers/i2c/busses/i2c-sh7760.c
+++ b/drivers/i2c/busses/i2c-sh7760.c
@@ -552,7 +552,6 @@ static int sh7760_i2c_remove(struct platform_device *pdev)
 static struct platform_driver sh7760_i2c_drv = {
 	.driver	= {
 		.name	= SH7760_I2C_DEVNAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= sh7760_i2c_probe,
 	.remove		= sh7760_i2c_remove,
diff --git a/drivers/i2c/busses/i2c-sh_mobile.c b/drivers/i2c/busses/i2c-sh_mobile.c
index 4855188747c9..440d5dbc8b5f 100644
--- a/drivers/i2c/busses/i2c-sh_mobile.c
+++ b/drivers/i2c/busses/i2c-sh_mobile.c
@@ -1,6 +1,8 @@
 /*
  * SuperH Mobile I2C Controller
  *
+ * Copyright (C) 2014 Wolfram Sang <wsa@sang-engineering.com>
+ *
  * Copyright (C) 2008 Magnus Damm
  *
  * Portions of the code based on out-of-tree driver i2c-sh7343.c
@@ -16,20 +18,22 @@
  * GNU General Public License for more details.
  */
 
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/dmaengine.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/i2c/i2c-sh_mobile.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/init.h>
-#include <linux/delay.h>
+#include <linux/of_device.h>
 #include <linux/platform_device.h>
-#include <linux/interrupt.h>
-#include <linux/i2c.h>
-#include <linux/err.h>
 #include <linux/pm_runtime.h>
-#include <linux/clk.h>
-#include <linux/io.h>
 #include <linux/slab.h>
-#include <linux/of_device.h>
-#include <linux/i2c/i2c-sh_mobile.h>
 
 /* Transmit operation:                                                      */
 /*                                                                          */
@@ -110,6 +114,7 @@ enum sh_mobile_i2c_op {
 	OP_TX_FIRST,
 	OP_TX,
 	OP_TX_STOP,
+	OP_TX_STOP_DATA,
 	OP_TX_TO_RX,
 	OP_RX,
 	OP_RX_STOP,
@@ -134,6 +139,12 @@ struct sh_mobile_i2c_data {
 	int pos;
 	int sr;
 	bool send_stop;
+
+	struct resource *res;
+	struct dma_chan *dma_tx;
+	struct dma_chan *dma_rx;
+	struct scatterlist sg;
+	enum dma_data_direction dma_direction;
 };
 
 struct sh_mobile_dt_config {
@@ -171,6 +182,8 @@ struct sh_mobile_dt_config {
 
 #define ICIC_ICCLB8		0x80
 #define ICIC_ICCHB8		0x40
+#define ICIC_TDMAE		0x20
+#define ICIC_RDMAE		0x10
 #define ICIC_ALE		0x08
 #define ICIC_TACKE		0x04
 #define ICIC_WAITE		0x02
@@ -277,6 +290,7 @@ static int sh_mobile_i2c_init(struct sh_mobile_i2c_data *pd)
 	else
 		pd->icic &= ~ICIC_ICCHB8;
 
+	dev_dbg(pd->dev, "timing values: L/H=0x%x/0x%x\n", pd->iccl, pd->icch);
 	return 0;
 }
 
@@ -332,8 +346,10 @@ static unsigned char i2c_op(struct sh_mobile_i2c_data *pd,
 	case OP_TX: /* write data */
 		iic_wr(pd, ICDR, data);
 		break;
-	case OP_TX_STOP: /* write data and issue a stop afterwards */
+	case OP_TX_STOP_DATA: /* write data and issue a stop afterwards */
 		iic_wr(pd, ICDR, data);
+		/* fallthrough */
+	case OP_TX_STOP: /* issue a stop */
 		iic_wr(pd, ICCR, pd->send_stop ? ICCR_ICE | ICCR_TRS
 					       : ICCR_ICE | ICCR_TRS | ICCR_BBSY);
 		break;
@@ -389,13 +405,17 @@ static int sh_mobile_i2c_isr_tx(struct sh_mobile_i2c_data *pd)
 {
 	unsigned char data;
 
-	if (pd->pos == pd->msg->len)
+	if (pd->pos == pd->msg->len) {
+		/* Send stop if we haven't yet (DMA case) */
+		if (pd->send_stop && (iic_rd(pd, ICCR) & ICCR_BBSY))
+			i2c_op(pd, OP_TX_STOP, 0);
 		return 1;
+	}
 
 	sh_mobile_i2c_get_data(pd, &data);
 
 	if (sh_mobile_i2c_is_last_byte(pd))
-		i2c_op(pd, OP_TX_STOP, data);
+		i2c_op(pd, OP_TX_STOP_DATA, data);
 	else if (sh_mobile_i2c_is_first_byte(pd))
 		i2c_op(pd, OP_TX_FIRST, data);
 	else
@@ -447,10 +467,9 @@ static int sh_mobile_i2c_isr_rx(struct sh_mobile_i2c_data *pd)
 
 static irqreturn_t sh_mobile_i2c_isr(int irq, void *dev_id)
 {
-	struct platform_device *dev = dev_id;
-	struct sh_mobile_i2c_data *pd = platform_get_drvdata(dev);
+	struct sh_mobile_i2c_data *pd = dev_id;
 	unsigned char sr;
-	int wakeup;
+	int wakeup = 0;
 
 	sr = iic_rd(pd, ICSR);
 	pd->sr |= sr; /* remember state */
@@ -459,15 +478,21 @@ static irqreturn_t sh_mobile_i2c_isr(int irq, void *dev_id)
 	       (pd->msg->flags & I2C_M_RD) ? "read" : "write",
 	       pd->pos, pd->msg->len);
 
-	if (sr & (ICSR_AL | ICSR_TACK)) {
+	/* Kick off TxDMA after preface was done */
+	if (pd->dma_direction == DMA_TO_DEVICE && pd->pos == 0)
+		iic_set_clr(pd, ICIC, ICIC_TDMAE, 0);
+	else if (sr & (ICSR_AL | ICSR_TACK))
 		/* don't interrupt transaction - continue to issue stop */
 		iic_wr(pd, ICSR, sr & ~(ICSR_AL | ICSR_TACK));
-		wakeup = 0;
-	} else if (pd->msg->flags & I2C_M_RD)
+	else if (pd->msg->flags & I2C_M_RD)
 		wakeup = sh_mobile_i2c_isr_rx(pd);
 	else
 		wakeup = sh_mobile_i2c_isr_tx(pd);
 
+	/* Kick off RxDMA after preface was done */
+	if (pd->dma_direction == DMA_FROM_DEVICE && pd->pos == 1)
+		iic_set_clr(pd, ICIC, ICIC_RDMAE, 0);
+
 	if (sr & ICSR_WAIT) /* TODO: add delay here to support slow acks */
 		iic_wr(pd, ICSR, sr & ~ICSR_WAIT);
 
@@ -482,6 +507,129 @@ static irqreturn_t sh_mobile_i2c_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+static void sh_mobile_i2c_dma_unmap(struct sh_mobile_i2c_data *pd)
+{
+	struct dma_chan *chan = pd->dma_direction == DMA_FROM_DEVICE
+				? pd->dma_rx : pd->dma_tx;
+
+	dma_unmap_single(chan->device->dev, sg_dma_address(&pd->sg),
+			 pd->msg->len, pd->dma_direction);
+
+	pd->dma_direction = DMA_NONE;
+}
+
+static void sh_mobile_i2c_cleanup_dma(struct sh_mobile_i2c_data *pd)
+{
+	if (pd->dma_direction == DMA_NONE)
+		return;
+	else if (pd->dma_direction == DMA_FROM_DEVICE)
+		dmaengine_terminate_all(pd->dma_rx);
+	else if (pd->dma_direction == DMA_TO_DEVICE)
+		dmaengine_terminate_all(pd->dma_tx);
+
+	sh_mobile_i2c_dma_unmap(pd);
+}
+
+static void sh_mobile_i2c_dma_callback(void *data)
+{
+	struct sh_mobile_i2c_data *pd = data;
+
+	sh_mobile_i2c_dma_unmap(pd);
+	pd->pos = pd->msg->len;
+
+	iic_set_clr(pd, ICIC, 0, ICIC_TDMAE | ICIC_RDMAE);
+}
+
+static struct dma_chan *sh_mobile_i2c_request_dma_chan(struct device *dev,
+				enum dma_transfer_direction dir, dma_addr_t port_addr)
+{
+	struct dma_chan *chan;
+	struct dma_slave_config cfg;
+	char *chan_name = dir == DMA_MEM_TO_DEV ? "tx" : "rx";
+	int ret;
+
+	chan = dma_request_slave_channel_reason(dev, chan_name);
+	if (IS_ERR(chan)) {
+		ret = PTR_ERR(chan);
+		dev_dbg(dev, "request_channel failed for %s (%d)\n", chan_name, ret);
+		return chan;
+	}
+
+	memset(&cfg, 0, sizeof(cfg));
+	cfg.direction = dir;
+	if (dir == DMA_MEM_TO_DEV) {
+		cfg.dst_addr = port_addr;
+		cfg.dst_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
+	} else {
+		cfg.src_addr = port_addr;
+		cfg.src_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
+	}
+
+	ret = dmaengine_slave_config(chan, &cfg);
+	if (ret) {
+		dev_dbg(dev, "slave_config failed for %s (%d)\n", chan_name, ret);
+		dma_release_channel(chan);
+		return ERR_PTR(ret);
+	}
+
+	dev_dbg(dev, "got DMA channel for %s\n", chan_name);
+	return chan;
+}
+
+static void sh_mobile_i2c_xfer_dma(struct sh_mobile_i2c_data *pd)
+{
+	bool read = pd->msg->flags & I2C_M_RD;
+	enum dma_data_direction dir = read ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
+	struct dma_chan *chan = read ? pd->dma_rx : pd->dma_tx;
+	struct dma_async_tx_descriptor *txdesc;
+	dma_addr_t dma_addr;
+	dma_cookie_t cookie;
+
+	if (PTR_ERR(chan) == -EPROBE_DEFER) {
+		if (read)
+			chan = pd->dma_rx = sh_mobile_i2c_request_dma_chan(pd->dev, DMA_DEV_TO_MEM,
+									   pd->res->start + ICDR);
+		else
+			chan = pd->dma_tx = sh_mobile_i2c_request_dma_chan(pd->dev, DMA_MEM_TO_DEV,
+									   pd->res->start + ICDR);
+	}
+
+	if (IS_ERR(chan))
+		return;
+
+	dma_addr = dma_map_single(chan->device->dev, pd->msg->buf, pd->msg->len, dir);
+	if (dma_mapping_error(pd->dev, dma_addr)) {
+		dev_dbg(pd->dev, "dma map failed, using PIO\n");
+		return;
+	}
+
+	sg_dma_len(&pd->sg) = pd->msg->len;
+	sg_dma_address(&pd->sg) = dma_addr;
+
+	pd->dma_direction = dir;
+
+	txdesc = dmaengine_prep_slave_sg(chan, &pd->sg, 1,
+					 read ? DMA_DEV_TO_MEM : DMA_MEM_TO_DEV,
+					 DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	if (!txdesc) {
+		dev_dbg(pd->dev, "dma prep slave sg failed, using PIO\n");
+		sh_mobile_i2c_cleanup_dma(pd);
+		return;
+	}
+
+	txdesc->callback = sh_mobile_i2c_dma_callback;
+	txdesc->callback_param = pd;
+
+	cookie = dmaengine_submit(txdesc);
+	if (dma_submit_error(cookie)) {
+		dev_dbg(pd->dev, "submitting dma failed, using PIO\n");
+		sh_mobile_i2c_cleanup_dma(pd);
+		return;
+	}
+
+	dma_async_issue_pending(chan);
+}
+
 static int start_ch(struct sh_mobile_i2c_data *pd, struct i2c_msg *usr_msg,
 		    bool do_init)
 {
@@ -506,6 +654,9 @@ static int start_ch(struct sh_mobile_i2c_data *pd, struct i2c_msg *usr_msg,
 	pd->pos = -1;
 	pd->sr = 0;
 
+	if (pd->msg->len > 8)
+		sh_mobile_i2c_xfer_dma(pd);
+
 	/* Enable all interrupts to begin with */
 	iic_wr(pd, ICIC, ICIC_DTEE | ICIC_WAITE | ICIC_ALE | ICIC_TACKE);
 	return 0;
@@ -589,6 +740,9 @@ static int sh_mobile_i2c_xfer(struct i2c_adapter *adapter,
 				       5 * HZ);
 		if (!k) {
 			dev_err(pd->dev, "Transfer request timed out\n");
+			if (pd->dma_direction != DMA_NONE)
+				sh_mobile_i2c_cleanup_dma(pd);
+
 			err = -ETIMEDOUT;
 			break;
 		}
@@ -622,22 +776,37 @@ static const struct sh_mobile_dt_config default_dt_config = {
 	.clks_per_count = 1,
 };
 
-static const struct sh_mobile_dt_config rcar_gen2_dt_config = {
+static const struct sh_mobile_dt_config fast_clock_dt_config = {
 	.clks_per_count = 2,
 };
 
 static const struct of_device_id sh_mobile_i2c_dt_ids[] = {
 	{ .compatible = "renesas,rmobile-iic", .data = &default_dt_config },
-	{ .compatible = "renesas,iic-r8a7790", .data = &rcar_gen2_dt_config },
-	{ .compatible = "renesas,iic-r8a7791", .data = &rcar_gen2_dt_config },
-	{ .compatible = "renesas,iic-r8a7792", .data = &rcar_gen2_dt_config },
-	{ .compatible = "renesas,iic-r8a7793", .data = &rcar_gen2_dt_config },
-	{ .compatible = "renesas,iic-r8a7794", .data = &rcar_gen2_dt_config },
+	{ .compatible = "renesas,iic-r8a73a4", .data = &fast_clock_dt_config },
+	{ .compatible = "renesas,iic-r8a7790", .data = &fast_clock_dt_config },
+	{ .compatible = "renesas,iic-r8a7791", .data = &fast_clock_dt_config },
+	{ .compatible = "renesas,iic-r8a7792", .data = &fast_clock_dt_config },
+	{ .compatible = "renesas,iic-r8a7793", .data = &fast_clock_dt_config },
+	{ .compatible = "renesas,iic-r8a7794", .data = &fast_clock_dt_config },
+	{ .compatible = "renesas,iic-sh73a0", .data = &fast_clock_dt_config },
 	{},
 };
 MODULE_DEVICE_TABLE(of, sh_mobile_i2c_dt_ids);
 
-static int sh_mobile_i2c_hook_irqs(struct platform_device *dev)
+static void sh_mobile_i2c_release_dma(struct sh_mobile_i2c_data *pd)
+{
+	if (!IS_ERR(pd->dma_tx)) {
+		dma_release_channel(pd->dma_tx);
+		pd->dma_tx = ERR_PTR(-EPROBE_DEFER);
+	}
+
+	if (!IS_ERR(pd->dma_rx)) {
+		dma_release_channel(pd->dma_rx);
+		pd->dma_rx = ERR_PTR(-EPROBE_DEFER);
+	}
+}
+
+static int sh_mobile_i2c_hook_irqs(struct platform_device *dev, struct sh_mobile_i2c_data *pd)
 {
 	struct resource *res;
 	resource_size_t n;
@@ -646,7 +815,7 @@ static int sh_mobile_i2c_hook_irqs(struct platform_device *dev)
 	while ((res = platform_get_resource(dev, IORESOURCE_IRQ, k))) {
 		for (n = res->start; n <= res->end; n++) {
 			ret = devm_request_irq(&dev->dev, n, sh_mobile_i2c_isr,
-					  0, dev_name(&dev->dev), dev);
+					  0, dev_name(&dev->dev), pd);
 			if (ret) {
 				dev_err(&dev->dev, "cannot request IRQ %pa\n", &n);
 				return ret;
@@ -677,7 +846,7 @@ static int sh_mobile_i2c_probe(struct platform_device *dev)
 		return PTR_ERR(pd->clk);
 	}
 
-	ret = sh_mobile_i2c_hook_irqs(dev);
+	ret = sh_mobile_i2c_hook_irqs(dev, pd);
 	if (ret)
 		return ret;
 
@@ -686,6 +855,7 @@ static int sh_mobile_i2c_probe(struct platform_device *dev)
 
 	res = platform_get_resource(dev, IORESOURCE_MEM, 0);
 
+	pd->res = res;
 	pd->reg = devm_ioremap_resource(&dev->dev, res);
 	if (IS_ERR(pd->reg))
 		return PTR_ERR(pd->reg);
@@ -723,6 +893,11 @@ static int sh_mobile_i2c_probe(struct platform_device *dev)
 	if (ret)
 		return ret;
 
+	/* Init DMA */
+	sg_init_table(&pd->sg, 1);
+	pd->dma_direction = DMA_NONE;
+	pd->dma_rx = pd->dma_tx = ERR_PTR(-EPROBE_DEFER);
+
 	/* Enable Runtime PM for this device.
 	 *
 	 * Also tell the Runtime PM core to ignore children
@@ -754,13 +929,12 @@ static int sh_mobile_i2c_probe(struct platform_device *dev)
 
 	ret = i2c_add_numbered_adapter(adap);
 	if (ret < 0) {
+		sh_mobile_i2c_release_dma(pd);
 		dev_err(&dev->dev, "cannot add numbered adapter\n");
 		return ret;
 	}
 
-	dev_info(&dev->dev,
-		 "I2C adapter %d with bus speed %lu Hz (L/H=0x%x/0x%x)\n",
-		 adap->nr, pd->bus_speed, pd->iccl, pd->icch);
+	dev_info(&dev->dev, "I2C adapter %d, bus speed %lu Hz\n", adap->nr, pd->bus_speed);
 
 	return 0;
 }
@@ -770,6 +944,7 @@ static int sh_mobile_i2c_remove(struct platform_device *dev)
 	struct sh_mobile_i2c_data *pd = platform_get_drvdata(dev);
 
 	i2c_del_adapter(&pd->adap);
+	sh_mobile_i2c_release_dma(pd);
 	pm_runtime_disable(&dev->dev);
 	return 0;
 }
@@ -794,7 +969,6 @@ static const struct dev_pm_ops sh_mobile_i2c_dev_pm_ops = {
 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,
 	},
@@ -806,16 +980,15 @@ static int __init sh_mobile_i2c_adap_init(void)
 {
 	return platform_driver_register(&sh_mobile_i2c_driver);
 }
+subsys_initcall(sh_mobile_i2c_adap_init);
 
 static void __exit sh_mobile_i2c_adap_exit(void)
 {
 	platform_driver_unregister(&sh_mobile_i2c_driver);
 }
-
-subsys_initcall(sh_mobile_i2c_adap_init);
 module_exit(sh_mobile_i2c_adap_exit);
 
 MODULE_DESCRIPTION("SuperH Mobile I2C Bus Controller driver");
-MODULE_AUTHOR("Magnus Damm");
+MODULE_AUTHOR("Magnus Damm and Wolfram Sang");
 MODULE_LICENSE("GPL v2");
 MODULE_ALIAS("platform:i2c-sh_mobile");
diff --git a/drivers/i2c/busses/i2c-simtec.c b/drivers/i2c/busses/i2c-simtec.c
index 15ac8395dcd3..b4685bb9b5d7 100644
--- a/drivers/i2c/busses/i2c-simtec.c
+++ b/drivers/i2c/busses/i2c-simtec.c
@@ -154,7 +154,6 @@ static int simtec_i2c_remove(struct platform_device *dev)
 static struct platform_driver simtec_i2c_driver = {
 	.driver		= {
 		.name		= "simtec-i2c",
-		.owner		= THIS_MODULE,
 	},
 	.probe		= simtec_i2c_probe,
 	.remove		= simtec_i2c_remove,
diff --git a/drivers/i2c/busses/i2c-sirf.c b/drivers/i2c/busses/i2c-sirf.c
index b1336d5f0531..1092d4eeeb54 100644
--- a/drivers/i2c/busses/i2c-sirf.c
+++ b/drivers/i2c/busses/i2c-sirf.c
@@ -451,7 +451,6 @@ MODULE_DEVICE_TABLE(of, sirfsoc_i2c_of_match);
 static struct platform_driver i2c_sirfsoc_driver = {
 	.driver = {
 		.name = "sirfsoc_i2c",
-		.owner = THIS_MODULE,
 #ifdef CONFIG_PM
 		.pm = &i2c_sirfsoc_pm_ops,
 #endif
diff --git a/drivers/i2c/busses/i2c-st.c b/drivers/i2c/busses/i2c-st.c
index 2e4eccd6599a..88057fad9dfe 100644
--- a/drivers/i2c/busses/i2c-st.c
+++ b/drivers/i2c/busses/i2c-st.c
@@ -863,7 +863,6 @@ MODULE_DEVICE_TABLE(of, st_i2c_match);
 static struct platform_driver st_i2c_driver = {
 	.driver = {
 		.name = "st-i2c",
-		.owner = THIS_MODULE,
 		.of_match_table = st_i2c_match,
 		.pm = ST_I2C_PM,
 	},
diff --git a/drivers/i2c/busses/i2c-stu300.c b/drivers/i2c/busses/i2c-stu300.c
index 6a44f37798c8..4885da9e9298 100644
--- a/drivers/i2c/busses/i2c-stu300.c
+++ b/drivers/i2c/busses/i2c-stu300.c
@@ -981,7 +981,6 @@ static const struct of_device_id stu300_dt_match[] = {
 static struct platform_driver stu300_i2c_driver = {
 	.driver = {
 		.name	= NAME,
-		.owner	= THIS_MODULE,
 		.pm	= STU300_I2C_PM,
 		.of_match_table = stu300_dt_match,
 	},
diff --git a/drivers/i2c/busses/i2c-sun6i-p2wi.c b/drivers/i2c/busses/i2c-sun6i-p2wi.c
index 4d75d4759709..7668e2e9b8fd 100644
--- a/drivers/i2c/busses/i2c-sun6i-p2wi.c
+++ b/drivers/i2c/busses/i2c-sun6i-p2wi.c
@@ -332,7 +332,6 @@ static struct platform_driver p2wi_driver = {
 	.probe	= p2wi_probe,
 	.remove	= p2wi_remove,
 	.driver	= {
-		.owner = THIS_MODULE,
 		.name = "i2c-sunxi-p2wi",
 		.of_match_table = p2wi_of_match_table,
 	},
diff --git a/drivers/i2c/busses/i2c-tegra.c b/drivers/i2c/busses/i2c-tegra.c
index efba1ebe16ba..28b87e683503 100644
--- a/drivers/i2c/busses/i2c-tegra.c
+++ b/drivers/i2c/busses/i2c-tegra.c
@@ -893,7 +893,6 @@ static struct platform_driver tegra_i2c_driver = {
 	.remove  = tegra_i2c_remove,
 	.driver  = {
 		.name  = "tegra-i2c",
-		.owner = THIS_MODULE,
 		.of_match_table = tegra_i2c_of_match,
 		.pm    = TEGRA_I2C_PM,
 	},
diff --git a/drivers/i2c/busses/i2c-versatile.c b/drivers/i2c/busses/i2c-versatile.c
index 6bb3a89a440f..240637f01d11 100644
--- a/drivers/i2c/busses/i2c-versatile.c
+++ b/drivers/i2c/busses/i2c-versatile.c
@@ -138,7 +138,6 @@ static struct platform_driver i2c_versatile_driver = {
 	.remove		= i2c_versatile_remove,
 	.driver		= {
 		.name	= "versatile-i2c",
-		.owner	= THIS_MODULE,
 		.of_match_table = i2c_versatile_match,
 	},
 };
diff --git a/drivers/i2c/busses/i2c-wmt.c b/drivers/i2c/busses/i2c-wmt.c
index f80a38c2072c..82ea34925489 100644
--- a/drivers/i2c/busses/i2c-wmt.c
+++ b/drivers/i2c/busses/i2c-wmt.c
@@ -462,7 +462,6 @@ static struct platform_driver wmt_i2c_driver = {
 	.remove		= wmt_i2c_remove,
 	.driver		= {
 		.name	= "wmt-i2c",
-		.owner	= THIS_MODULE,
 		.of_match_table = wmt_i2c_dt_ids,
 	},
 };
diff --git a/drivers/i2c/busses/i2c-xiic.c b/drivers/i2c/busses/i2c-xiic.c
index cc65ea0b818f..e8400042b358 100644
--- a/drivers/i2c/busses/i2c-xiic.c
+++ b/drivers/i2c/busses/i2c-xiic.c
@@ -46,6 +46,11 @@ enum xilinx_i2c_state {
 	STATE_START
 };
 
+enum xiic_endian {
+	LITTLE,
+	BIG
+};
+
 /**
  * struct xiic_i2c - Internal representation of the XIIC I2C bus
  * @base:	Memory base of the HW registers
@@ -70,6 +75,7 @@ struct xiic_i2c {
 	enum xilinx_i2c_state	state;
 	struct i2c_msg		*rx_msg;
 	int			rx_pos;
+	enum xiic_endian	endianness;
 };
 
 
@@ -170,29 +176,58 @@ struct xiic_i2c {
 static void xiic_start_xfer(struct xiic_i2c *i2c);
 static void __xiic_start_xfer(struct xiic_i2c *i2c);
 
+/*
+ * For the register read and write functions, a little-endian and big-endian
+ * version are necessary. Endianness is detected during the probe function.
+ * Only the least significant byte [doublet] of the register are ever
+ * accessed. This requires an offset of 3 [2] from the base address for
+ * big-endian systems.
+ */
+
 static inline void xiic_setreg8(struct xiic_i2c *i2c, int reg, u8 value)
 {
-	iowrite8(value, i2c->base + reg);
+	if (i2c->endianness == LITTLE)
+		iowrite8(value, i2c->base + reg);
+	else
+		iowrite8(value, i2c->base + reg + 3);
 }
 
 static inline u8 xiic_getreg8(struct xiic_i2c *i2c, int reg)
 {
-	return ioread8(i2c->base + reg);
+	u8 ret;
+
+	if (i2c->endianness == LITTLE)
+		ret = ioread8(i2c->base + reg);
+	else
+		ret = ioread8(i2c->base + reg + 3);
+	return ret;
 }
 
 static inline void xiic_setreg16(struct xiic_i2c *i2c, int reg, u16 value)
 {
-	iowrite16(value, i2c->base + reg);
+	if (i2c->endianness == LITTLE)
+		iowrite16(value, i2c->base + reg);
+	else
+		iowrite16be(value, i2c->base + reg + 2);
 }
 
 static inline void xiic_setreg32(struct xiic_i2c *i2c, int reg, int value)
 {
-	iowrite32(value, i2c->base + reg);
+	if (i2c->endianness == LITTLE)
+		iowrite32(value, i2c->base + reg);
+	else
+		iowrite32be(value, i2c->base + reg);
 }
 
 static inline int xiic_getreg32(struct xiic_i2c *i2c, int reg)
 {
-	return ioread32(i2c->base + reg);
+	u32 ret;
+
+	if (i2c->endianness == LITTLE)
+		ret = ioread32(i2c->base + reg);
+	else
+		ret = ioread32be(i2c->base + reg);
+	return ret;
 }
 
 static inline void xiic_irq_dis(struct xiic_i2c *i2c, u32 mask)
@@ -692,6 +727,7 @@ static int xiic_i2c_probe(struct platform_device *pdev)
 	struct resource *res;
 	int ret, irq;
 	u8 i;
+	u32 sr;
 
 	i2c = devm_kzalloc(&pdev->dev, sizeof(*i2c), GFP_KERNEL);
 	if (!i2c)
@@ -724,6 +760,18 @@ static int xiic_i2c_probe(struct platform_device *pdev)
 		return ret;
 	}
 
+	/*
+	 * Detect endianness
+	 * Try to reset the TX FIFO. Then check the EMPTY flag. If it is not
+	 * set, assume that the endianness was wrong and swap.
+	 */
+	i2c->endianness = LITTLE;
+	xiic_setreg32(i2c, XIIC_CR_REG_OFFSET, XIIC_CR_TX_FIFO_RESET_MASK);
+	/* Reset is cleared in xiic_reinit */
+	sr = xiic_getreg32(i2c, XIIC_SR_REG_OFFSET);
+	if (!(sr & XIIC_SR_TX_FIFO_EMPTY_MASK))
+		i2c->endianness = BIG;
+
 	xiic_reinit(i2c);
 
 	/* add i2c adapter to i2c tree */
@@ -767,7 +815,6 @@ static struct platform_driver xiic_i2c_driver = {
 	.probe   = xiic_i2c_probe,
 	.remove  = xiic_i2c_remove,
 	.driver  = {
-		.owner = THIS_MODULE,
 		.name = DRIVER_NAME,
 		.of_match_table = of_match_ptr(xiic_of_match),
 	},
diff --git a/drivers/i2c/busses/i2c-xlr.c b/drivers/i2c/busses/i2c-xlr.c
index 17f7352eca6b..8b36bcfd952d 100644
--- a/drivers/i2c/busses/i2c-xlr.c
+++ b/drivers/i2c/busses/i2c-xlr.c
@@ -263,7 +263,6 @@ static struct platform_driver xlr_i2c_driver = {
 	.remove = xlr_i2c_remove,
 	.driver = {
 		.name   = "xlr-i2cbus",
-		.owner  = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/i2c/busses/scx200_acb.c b/drivers/i2c/busses/scx200_acb.c
index 5153354b1a6b..0a7e410b6195 100644
--- a/drivers/i2c/busses/scx200_acb.c
+++ b/drivers/i2c/busses/scx200_acb.c
@@ -544,7 +544,6 @@ static int scx200_remove(struct platform_device *pdev)
 static struct platform_driver scx200_pci_driver = {
 	.driver = {
 		.name = "cs5535-smb",
-		.owner = THIS_MODULE,
 	},
 	.probe = scx200_probe,
 	.remove = scx200_remove,
diff --git a/drivers/i2c/i2c-core.c b/drivers/i2c/i2c-core.c
index 229a89e84b0f..39d25a8cb1ad 100644
--- a/drivers/i2c/i2c-core.c
+++ b/drivers/i2c/i2c-core.c
@@ -24,6 +24,7 @@
    (c) 2013  Wolfram Sang <wsa@the-dreams.de>
    I2C ACPI code Copyright (C) 2014 Intel Corp
    Author: Lan Tianyu <tianyu.lan@intel.com>
+   I2C slave support (c) 2014 by Wolfram Sang <wsa@sang-engineering.com>
  */
 
 #include <linux/module.h>
@@ -261,7 +262,7 @@ acpi_i2c_space_handler(u32 function, acpi_physical_address command,
 	struct acpi_resource *ares;
 	u32 accessor_type = function >> 16;
 	u8 action = function & ACPI_IO_MASK;
-	acpi_status ret = AE_OK;
+	acpi_status ret;
 	int status;
 
 	ret = acpi_buffer_to_resource(info->connection, info->length, &ares);
@@ -628,6 +629,17 @@ static int i2c_device_probe(struct device *dev)
 	if (!client)
 		return 0;
 
+	if (!client->irq && dev->of_node) {
+		int irq = of_irq_get(dev->of_node, 0);
+
+		if (irq == -EPROBE_DEFER)
+			return irq;
+		if (irq < 0)
+			irq = 0;
+
+		client->irq = irq;
+	}
+
 	driver = to_i2c_driver(dev->driver);
 	if (!driver->probe || !driver->id_table)
 		return -ENODEV;
@@ -1401,7 +1413,6 @@ static struct i2c_client *of_i2c_register_device(struct i2c_adapter *adap,
 		return ERR_PTR(-EINVAL);
 	}
 
-	info.irq = irq_of_parse_and_map(node, 0);
 	info.of_node = of_node_get(node);
 	info.archdata = &dev_ad;
 
@@ -1415,7 +1426,6 @@ static struct i2c_client *of_i2c_register_device(struct i2c_adapter *adap,
 		dev_err(&adap->dev, "of_i2c: Failure registering %s\n",
 			node->full_name);
 		of_node_put(node);
-		irq_dispose_mapping(info.irq);
 		return ERR_PTR(-EINVAL);
 	}
 	return result;
@@ -2962,6 +2972,54 @@ trace:
 }
 EXPORT_SYMBOL(i2c_smbus_xfer);
 
+int i2c_slave_register(struct i2c_client *client, i2c_slave_cb_t slave_cb)
+{
+	int ret;
+
+	if (!client || !slave_cb)
+		return -EINVAL;
+
+	if (!(client->flags & I2C_CLIENT_TEN)) {
+		/* Enforce stricter address checking */
+		ret = i2c_check_addr_validity(client->addr);
+		if (ret)
+			return ret;
+	}
+
+	if (!client->adapter->algo->reg_slave)
+		return -EOPNOTSUPP;
+
+	client->slave_cb = slave_cb;
+
+	i2c_lock_adapter(client->adapter);
+	ret = client->adapter->algo->reg_slave(client);
+	i2c_unlock_adapter(client->adapter);
+
+	if (ret)
+		client->slave_cb = NULL;
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(i2c_slave_register);
+
+int i2c_slave_unregister(struct i2c_client *client)
+{
+	int ret;
+
+	if (!client->adapter->algo->unreg_slave)
+		return -EOPNOTSUPP;
+
+	i2c_lock_adapter(client->adapter);
+	ret = client->adapter->algo->unreg_slave(client);
+	i2c_unlock_adapter(client->adapter);
+
+	if (ret == 0)
+		client->slave_cb = NULL;
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(i2c_slave_unregister);
+
 MODULE_AUTHOR("Simon G. Vogl <simon@tk.uni-linz.ac.at>");
 MODULE_DESCRIPTION("I2C-Bus main module");
 MODULE_LICENSE("GPL");
diff --git a/drivers/i2c/i2c-mux.c b/drivers/i2c/i2c-mux.c
index 2d0847b6be62..593f7ca9adc7 100644
--- a/drivers/i2c/i2c-mux.c
+++ b/drivers/i2c/i2c-mux.c
@@ -110,6 +110,7 @@ struct i2c_adapter *i2c_add_mux_adapter(struct i2c_adapter *parent,
 						 void *, u32))
 {
 	struct i2c_mux_priv *priv;
+	char symlink_name[20];
 	int ret;
 
 	priv = kzalloc(sizeof(struct i2c_mux_priv), GFP_KERNEL);
@@ -183,6 +184,12 @@ struct i2c_adapter *i2c_add_mux_adapter(struct i2c_adapter *parent,
 		return NULL;
 	}
 
+	WARN(sysfs_create_link(&priv->adap.dev.kobj, &mux_dev->kobj, "mux_device"),
+			       "can't create symlink to mux device\n");
+
+	snprintf(symlink_name, sizeof(symlink_name), "channel-%u", chan_id);
+	WARN(sysfs_create_link(&mux_dev->kobj, &priv->adap.dev.kobj, symlink_name),
+			       "can't create symlink for channel %u\n", chan_id);
 	dev_info(&parent->dev, "Added multiplexed i2c bus %d\n",
 		 i2c_adapter_id(&priv->adap));
 
@@ -193,7 +200,12 @@ EXPORT_SYMBOL_GPL(i2c_add_mux_adapter);
 void i2c_del_mux_adapter(struct i2c_adapter *adap)
 {
 	struct i2c_mux_priv *priv = adap->algo_data;
+	char symlink_name[20];
+
+	snprintf(symlink_name, sizeof(symlink_name), "channel-%u", priv->chan_id);
+	sysfs_remove_link(&adap->dev.parent->kobj, symlink_name);
 
+	sysfs_remove_link(&priv->adap.dev.kobj, "mux_device");
 	i2c_del_adapter(adap);
 	kfree(priv);
 }
diff --git a/drivers/i2c/i2c-slave-eeprom.c b/drivers/i2c/i2c-slave-eeprom.c
new file mode 100644
index 000000000000..6631400b5f02
--- /dev/null
+++ b/drivers/i2c/i2c-slave-eeprom.c
@@ -0,0 +1,170 @@
+/*
+ * I2C slave mode EEPROM simulator
+ *
+ * Copyright (C) 2014 by Wolfram Sang, Sang Engineering <wsa@sang-engineering.com>
+ * Copyright (C) 2014 by Renesas Electronics Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; version 2 of the License.
+ *
+ * Because most IP blocks can only detect one I2C slave address anyhow, this
+ * driver does not support simulating EEPROM types which take more than one
+ * address. It is prepared to simulate bigger EEPROMs with an internal 16 bit
+ * pointer, yet implementation is deferred until the need actually arises.
+ */
+
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/sysfs.h>
+
+struct eeprom_data {
+	struct bin_attribute bin;
+	bool first_write;
+	spinlock_t buffer_lock;
+	u8 buffer_idx;
+	u8 buffer[];
+};
+
+static int i2c_slave_eeprom_slave_cb(struct i2c_client *client,
+				     enum i2c_slave_event event, u8 *val)
+{
+	struct eeprom_data *eeprom = i2c_get_clientdata(client);
+
+	switch (event) {
+	case I2C_SLAVE_REQ_WRITE_END:
+		if (eeprom->first_write) {
+			eeprom->buffer_idx = *val;
+			eeprom->first_write = false;
+		} else {
+			spin_lock(&eeprom->buffer_lock);
+			eeprom->buffer[eeprom->buffer_idx++] = *val;
+			spin_unlock(&eeprom->buffer_lock);
+		}
+		break;
+
+	case I2C_SLAVE_REQ_READ_START:
+		spin_lock(&eeprom->buffer_lock);
+		*val = eeprom->buffer[eeprom->buffer_idx];
+		spin_unlock(&eeprom->buffer_lock);
+		break;
+
+	case I2C_SLAVE_REQ_READ_END:
+		eeprom->buffer_idx++;
+		break;
+
+	case I2C_SLAVE_STOP:
+		eeprom->first_write = true;
+		break;
+
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static ssize_t i2c_slave_eeprom_bin_read(struct file *filp, struct kobject *kobj,
+		struct bin_attribute *attr, char *buf, loff_t off, size_t count)
+{
+	struct eeprom_data *eeprom;
+	unsigned long flags;
+
+	if (off + count >= attr->size)
+		return -EFBIG;
+
+	eeprom = dev_get_drvdata(container_of(kobj, struct device, kobj));
+
+	spin_lock_irqsave(&eeprom->buffer_lock, flags);
+	memcpy(buf, &eeprom->buffer[off], count);
+	spin_unlock_irqrestore(&eeprom->buffer_lock, flags);
+
+	return count;
+}
+
+static ssize_t i2c_slave_eeprom_bin_write(struct file *filp, struct kobject *kobj,
+		struct bin_attribute *attr, char *buf, loff_t off, size_t count)
+{
+	struct eeprom_data *eeprom;
+	unsigned long flags;
+
+	if (off + count >= attr->size)
+		return -EFBIG;
+
+	eeprom = dev_get_drvdata(container_of(kobj, struct device, kobj));
+
+	spin_lock_irqsave(&eeprom->buffer_lock, flags);
+	memcpy(&eeprom->buffer[off], buf, count);
+	spin_unlock_irqrestore(&eeprom->buffer_lock, flags);
+
+	return count;
+}
+
+static int i2c_slave_eeprom_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct eeprom_data *eeprom;
+	int ret;
+	unsigned size = id->driver_data;
+
+	eeprom = devm_kzalloc(&client->dev, sizeof(struct eeprom_data) + size, GFP_KERNEL);
+	if (!eeprom)
+		return -ENOMEM;
+
+	eeprom->first_write = true;
+	spin_lock_init(&eeprom->buffer_lock);
+	i2c_set_clientdata(client, eeprom);
+
+	sysfs_bin_attr_init(&eeprom->bin);
+	eeprom->bin.attr.name = "slave-eeprom";
+	eeprom->bin.attr.mode = S_IRUSR | S_IWUSR;
+	eeprom->bin.read = i2c_slave_eeprom_bin_read;
+	eeprom->bin.write = i2c_slave_eeprom_bin_write;
+	eeprom->bin.size = size;
+
+	ret = sysfs_create_bin_file(&client->dev.kobj, &eeprom->bin);
+	if (ret)
+		return ret;
+
+	ret = i2c_slave_register(client, i2c_slave_eeprom_slave_cb);
+	if (ret) {
+		sysfs_remove_bin_file(&client->dev.kobj, &eeprom->bin);
+		return ret;
+	}
+
+	return 0;
+};
+
+static int i2c_slave_eeprom_remove(struct i2c_client *client)
+{
+	struct eeprom_data *eeprom = i2c_get_clientdata(client);
+
+	i2c_slave_unregister(client);
+	sysfs_remove_bin_file(&client->dev.kobj, &eeprom->bin);
+
+	return 0;
+}
+
+static const struct i2c_device_id i2c_slave_eeprom_id[] = {
+	{ "slave-24c02", 2048 / 8 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, i2c_slave_eeprom_id);
+
+static struct i2c_driver i2c_slave_eeprom_driver = {
+	.driver = {
+		.name = "i2c-slave-eeprom",
+		.owner = THIS_MODULE,
+	},
+	.probe = i2c_slave_eeprom_probe,
+	.remove = i2c_slave_eeprom_remove,
+	.id_table = i2c_slave_eeprom_id,
+};
+module_i2c_driver(i2c_slave_eeprom_driver);
+
+MODULE_AUTHOR("Wolfram Sang <wsa@sang-engineering.com>");
+MODULE_DESCRIPTION("I2C slave mode EEPROM simulator");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/i2c/muxes/i2c-arb-gpio-challenge.c b/drivers/i2c/muxes/i2c-arb-gpio-challenge.c
index 69afffa8f427..5cf1b60b69e2 100644
--- a/drivers/i2c/muxes/i2c-arb-gpio-challenge.c
+++ b/drivers/i2c/muxes/i2c-arb-gpio-challenge.c
@@ -235,7 +235,6 @@ static struct platform_driver i2c_arbitrator_driver = {
 	.probe	= i2c_arbitrator_probe,
 	.remove	= i2c_arbitrator_remove,
 	.driver	= {
-		.owner	= THIS_MODULE,
 		.name	= "i2c-arb-gpio-challenge",
 		.of_match_table = i2c_arbitrator_of_match,
 	},
diff --git a/drivers/i2c/muxes/i2c-mux-gpio.c b/drivers/i2c/muxes/i2c-mux-gpio.c
index d8989c823f50..f5798eb4076b 100644
--- a/drivers/i2c/muxes/i2c-mux-gpio.c
+++ b/drivers/i2c/muxes/i2c-mux-gpio.c
@@ -280,7 +280,6 @@ static struct platform_driver i2c_mux_gpio_driver = {
 	.probe	= i2c_mux_gpio_probe,
 	.remove	= i2c_mux_gpio_remove,
 	.driver	= {
-		.owner	= THIS_MODULE,
 		.name	= "i2c-mux-gpio",
 		.of_match_table = i2c_mux_gpio_of_match,
 	},
diff --git a/drivers/i2c/muxes/i2c-mux-pinctrl.c b/drivers/i2c/muxes/i2c-mux-pinctrl.c
index 4ff0ef3e07a6..b48378c4b40d 100644
--- a/drivers/i2c/muxes/i2c-mux-pinctrl.c
+++ b/drivers/i2c/muxes/i2c-mux-pinctrl.c
@@ -264,7 +264,6 @@ MODULE_DEVICE_TABLE(of, i2c_mux_pinctrl_of_match);
 static struct platform_driver i2c_mux_pinctrl_driver = {
 	.driver	= {
 		.name	= "i2c-mux-pinctrl",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(i2c_mux_pinctrl_of_match),
 	},
 	.probe	= i2c_mux_pinctrl_probe,
diff --git a/drivers/ide/au1xxx-ide.c b/drivers/ide/au1xxx-ide.c
index 07ea58084068..4d181a918d72 100644
--- a/drivers/ide/au1xxx-ide.c
+++ b/drivers/ide/au1xxx-ide.c
@@ -586,7 +586,6 @@ static int au_ide_remove(struct platform_device *dev)
 static struct platform_driver au1200_ide_driver = {
 	.driver = {
 		.name		= "au1200-ide",
-		.owner		= THIS_MODULE,
 	},
 	.probe 		= au_ide_probe,
 	.remove		= au_ide_remove,
diff --git a/drivers/ide/gayle.c b/drivers/ide/gayle.c
index 97a2f9dc75d8..901e6ebfeb96 100644
--- a/drivers/ide/gayle.c
+++ b/drivers/ide/gayle.c
@@ -179,7 +179,6 @@ static struct platform_driver amiga_gayle_ide_driver = {
 	.remove = __exit_p(amiga_gayle_ide_remove),
 	.driver   = {
 		.name	= "amiga-gayle-ide",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/ide/ide-floppy.c b/drivers/ide/ide-floppy.c
index 3d42043fec51..8c6363cdd208 100644
--- a/drivers/ide/ide-floppy.c
+++ b/drivers/ide/ide-floppy.c
@@ -487,7 +487,7 @@ static void ide_floppy_setup(ide_drive_t *drive)
 	 * it. It should be fixed as of version 1.9, but to be on the safe side
 	 * we'll leave the limitation below for the 2.2.x tree.
 	 */
-	if (!strncmp((char *)&id[ATA_ID_PROD], "IOMEGA ZIP 100 ATAPI", 20)) {
+	if (strstarts((char *)&id[ATA_ID_PROD], "IOMEGA ZIP 100 ATAPI")) {
 		drive->atapi_flags |= IDE_AFLAG_ZIP_DRIVE;
 		/* This value will be visible in the /proc/ide/hdx/settings */
 		drive->pc_delay = IDEFLOPPY_PC_DELAY;
@@ -498,7 +498,7 @@ static void ide_floppy_setup(ide_drive_t *drive)
 	 * Guess what? The IOMEGA Clik! drive also needs the above fix. It makes
 	 * nasty clicking noises without it, so please don't remove this.
 	 */
-	if (strncmp((char *)&id[ATA_ID_PROD], "IOMEGA Clik!", 11) == 0) {
+	if (strstarts((char *)&id[ATA_ID_PROD], "IOMEGA Clik!")) {
 		blk_queue_max_hw_sectors(drive->queue, 64);
 		drive->atapi_flags |= IDE_AFLAG_CLIK_DRIVE;
 		/* IOMEGA Clik! drives do not support lock/unlock commands */
diff --git a/drivers/ide/ide.c b/drivers/ide/ide.c
index 2ce6268a2734..e29b02ca9e91 100644
--- a/drivers/ide/ide.c
+++ b/drivers/ide/ide.c
@@ -101,8 +101,7 @@ void ide_device_put(ide_drive_t *drive)
 	struct device *host_dev = drive->hwif->host->dev[0];
 	struct module *module = host_dev ? host_dev->driver->owner : NULL;
 
-	if (module)
-		module_put(module);
+	module_put(module);
 #endif
 	put_device(&drive->gendev);
 }
diff --git a/drivers/ide/ide_platform.c b/drivers/ide/ide_platform.c
index d48de6de503e..2b43be3bca0a 100644
--- a/drivers/ide/ide_platform.c
+++ b/drivers/ide/ide_platform.c
@@ -125,7 +125,6 @@ static int plat_ide_remove(struct platform_device *pdev)
 static struct platform_driver platform_ide_driver = {
 	.driver = {
 		.name = "pata_platform",
-		.owner = THIS_MODULE,
 	},
 	.probe = plat_ide_probe,
 	.remove = plat_ide_remove,
diff --git a/drivers/ide/palm_bk3710.c b/drivers/ide/palm_bk3710.c
index ba20d18c0373..8012e43bf8f6 100644
--- a/drivers/ide/palm_bk3710.c
+++ b/drivers/ide/palm_bk3710.c
@@ -386,7 +386,6 @@ MODULE_ALIAS("platform:palm_bk3710");
 static struct platform_driver platform_bk_driver = {
 	.driver = {
 		.name = "palm_bk3710",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/ide/tx4938ide.c b/drivers/ide/tx4938ide.c
index 68edd4f58a28..40a3f55b08dd 100644
--- a/drivers/ide/tx4938ide.c
+++ b/drivers/ide/tx4938ide.c
@@ -198,7 +198,6 @@ static int __exit tx4938ide_remove(struct platform_device *pdev)
 static struct platform_driver tx4938ide_driver = {
 	.driver		= {
 		.name	= "tx4938ide",
-		.owner	= THIS_MODULE,
 	},
 	.remove = __exit_p(tx4938ide_remove),
 };
diff --git a/drivers/ide/tx4939ide.c b/drivers/ide/tx4939ide.c
index 4ecdee5eca83..67d4a7d4acc8 100644
--- a/drivers/ide/tx4939ide.c
+++ b/drivers/ide/tx4939ide.c
@@ -618,7 +618,6 @@ static int tx4939ide_resume(struct platform_device *dev)
 static struct platform_driver tx4939ide_driver = {
 	.driver = {
 		.name = MODNAME,
-		.owner = THIS_MODULE,
 	},
 	.remove = __exit_p(tx4939ide_remove),
 	.resume = tx4939ide_resume,
diff --git a/drivers/iio/accel/st_accel.h b/drivers/iio/accel/st_accel.h
index c3877630b2e4..fa9646034305 100644
--- a/drivers/iio/accel/st_accel.h
+++ b/drivers/iio/accel/st_accel.h
@@ -33,8 +33,7 @@ static const struct st_sensors_platform_data default_accel_pdata = {
 	.drdy_int_pin = 1,
 };
 
-int st_accel_common_probe(struct iio_dev *indio_dev,
-					struct st_sensors_platform_data *pdata);
+int st_accel_common_probe(struct iio_dev *indio_dev);
 void st_accel_common_remove(struct iio_dev *indio_dev);
 
 #ifdef CONFIG_IIO_BUFFER
diff --git a/drivers/iio/accel/st_accel_core.c b/drivers/iio/accel/st_accel_core.c
index 087864854c61..53f32629283a 100644
--- a/drivers/iio/accel/st_accel_core.c
+++ b/drivers/iio/accel/st_accel_core.c
@@ -161,7 +161,7 @@ static const struct iio_chan_spec st_accel_16bit_channels[] = {
 	IIO_CHAN_SOFT_TIMESTAMP(3)
 };
 
-static const struct st_sensors st_accel_sensors[] = {
+static const struct st_sensor_settings st_accel_sensors_settings[] = {
 	{
 		.wai = ST_ACCEL_1_WAI_EXP,
 		.sensors_supported = {
@@ -457,8 +457,7 @@ static const struct iio_trigger_ops st_accel_trigger_ops = {
 #define ST_ACCEL_TRIGGER_OPS NULL
 #endif
 
-int st_accel_common_probe(struct iio_dev *indio_dev,
-				struct st_sensors_platform_data *plat_data)
+int st_accel_common_probe(struct iio_dev *indio_dev)
 {
 	struct st_sensor_data *adata = iio_priv(indio_dev);
 	int irq = adata->get_irq_data_ready(indio_dev);
@@ -470,24 +469,25 @@ int st_accel_common_probe(struct iio_dev *indio_dev,
 	st_sensors_power_enable(indio_dev);
 
 	err = st_sensors_check_device_support(indio_dev,
-				ARRAY_SIZE(st_accel_sensors), st_accel_sensors);
+					ARRAY_SIZE(st_accel_sensors_settings),
+					st_accel_sensors_settings);
 	if (err < 0)
 		return err;
 
 	adata->num_data_channels = ST_ACCEL_NUMBER_DATA_CHANNELS;
-	adata->multiread_bit = adata->sensor->multi_read_bit;
-	indio_dev->channels = adata->sensor->ch;
+	adata->multiread_bit = adata->sensor_settings->multi_read_bit;
+	indio_dev->channels = adata->sensor_settings->ch;
 	indio_dev->num_channels = ST_SENSORS_NUMBER_ALL_CHANNELS;
 
 	adata->current_fullscale = (struct st_sensor_fullscale_avl *)
-						&adata->sensor->fs.fs_avl[0];
-	adata->odr = adata->sensor->odr.odr_avl[0].hz;
+					&adata->sensor_settings->fs.fs_avl[0];
+	adata->odr = adata->sensor_settings->odr.odr_avl[0].hz;
 
-	if (!plat_data)
-		plat_data =
+	if (!adata->dev->platform_data)
+		adata->dev->platform_data =
 			(struct st_sensors_platform_data *)&default_accel_pdata;
 
-	err = st_sensors_init_sensor(indio_dev, plat_data);
+	err = st_sensors_init_sensor(indio_dev, adata->dev->platform_data);
 	if (err < 0)
 		return err;
 
diff --git a/drivers/iio/accel/st_accel_i2c.c b/drivers/iio/accel/st_accel_i2c.c
index 7164aeff3ab1..c7246bdd30b9 100644
--- a/drivers/iio/accel/st_accel_i2c.c
+++ b/drivers/iio/accel/st_accel_i2c.c
@@ -79,12 +79,11 @@ static int st_accel_i2c_probe(struct i2c_client *client,
 		return -ENOMEM;
 
 	adata = iio_priv(indio_dev);
-	adata->dev = &client->dev;
 	st_sensors_of_i2c_probe(client, st_accel_of_match);
 
 	st_sensors_i2c_configure(indio_dev, client, adata);
 
-	err = st_accel_common_probe(indio_dev, client->dev.platform_data);
+	err = st_accel_common_probe(indio_dev);
 	if (err < 0)
 		return err;
 
diff --git a/drivers/iio/accel/st_accel_spi.c b/drivers/iio/accel/st_accel_spi.c
index 195639646e34..12ec29389e4b 100644
--- a/drivers/iio/accel/st_accel_spi.c
+++ b/drivers/iio/accel/st_accel_spi.c
@@ -29,11 +29,10 @@ static int st_accel_spi_probe(struct spi_device *spi)
 		return -ENOMEM;
 
 	adata = iio_priv(indio_dev);
-	adata->dev = &spi->dev;
 
 	st_sensors_spi_configure(indio_dev, spi, adata);
 
-	err = st_accel_common_probe(indio_dev, spi->dev.platform_data);
+	err = st_accel_common_probe(indio_dev);
 	if (err < 0)
 		return err;
 
diff --git a/drivers/iio/adc/Kconfig b/drivers/iio/adc/Kconfig
index bc4e787096e8..0f79e4725763 100644
--- a/drivers/iio/adc/Kconfig
+++ b/drivers/iio/adc/Kconfig
@@ -214,6 +214,20 @@ config NAU7802
 	  To compile this driver as a module, choose M here: the
 	  module will be called nau7802.
 
+config QCOM_SPMI_IADC
+	tristate "Qualcomm SPMI PMIC current ADC"
+	depends on SPMI
+	select REGMAP_SPMI
+	help
+	  This is the IIO Current ADC driver for Qualcomm QPNP IADC Chip.
+
+	  The driver supports single mode operation to read from one of two
+	  channels (external or internal). Hardware have additional
+	  channels internally used for gain and offset calibration.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called qcom-spmi-iadc.
+
 config ROCKCHIP_SARADC
 	tristate "Rockchip SARADC driver"
 	depends on ARCH_ROCKCHIP || (ARM && COMPILE_TEST)
diff --git a/drivers/iio/adc/Makefile b/drivers/iio/adc/Makefile
index f30093f5b67a..701fdb7c96aa 100644
--- a/drivers/iio/adc/Makefile
+++ b/drivers/iio/adc/Makefile
@@ -23,6 +23,7 @@ obj-$(CONFIG_MCP320X) += mcp320x.o
 obj-$(CONFIG_MCP3422) += mcp3422.o
 obj-$(CONFIG_MEN_Z188_ADC) += men_z188_adc.o
 obj-$(CONFIG_NAU7802) += nau7802.o
+obj-$(CONFIG_QCOM_SPMI_IADC) += qcom-spmi-iadc.o
 obj-$(CONFIG_ROCKCHIP_SARADC) += rockchip_saradc.o
 obj-$(CONFIG_TI_ADC081C) += ti-adc081c.o
 obj-$(CONFIG_TI_ADC128S052) += ti-adc128s052.o
diff --git a/drivers/iio/adc/exynos_adc.c b/drivers/iio/adc/exynos_adc.c
index 43620fd4c66a..3a2dbb3b4926 100644
--- a/drivers/iio/adc/exynos_adc.c
+++ b/drivers/iio/adc/exynos_adc.c
@@ -39,6 +39,8 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/machine.h>
 #include <linux/iio/driver.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
 
 /* S3C/EXYNOS4412/5250 ADC_V1 registers definitions */
 #define ADC_V1_CON(x)		((x) + 0x00)
@@ -90,11 +92,14 @@
 
 #define EXYNOS_ADC_TIMEOUT	(msecs_to_jiffies(100))
 
+#define EXYNOS_ADCV1_PHY_OFFSET	0x0718
+#define EXYNOS_ADCV2_PHY_OFFSET	0x0720
+
 struct exynos_adc {
 	struct exynos_adc_data	*data;
 	struct device		*dev;
 	void __iomem		*regs;
-	void __iomem		*enable_reg;
+	struct regmap		*pmu_map;
 	struct clk		*clk;
 	struct clk		*sclk;
 	unsigned int		irq;
@@ -110,6 +115,7 @@ struct exynos_adc_data {
 	int num_channels;
 	bool needs_sclk;
 	bool needs_adc_phy;
+	int phy_offset;
 	u32 mask;
 
 	void (*init_hw)(struct exynos_adc *info);
@@ -183,7 +189,7 @@ static void exynos_adc_v1_init_hw(struct exynos_adc *info)
 	u32 con1;
 
 	if (info->data->needs_adc_phy)
-		writel(1, info->enable_reg);
+		regmap_write(info->pmu_map, info->data->phy_offset, 1);
 
 	/* set default prescaler values and Enable prescaler */
 	con1 =  ADC_V1_CON_PRSCLV(49) | ADC_V1_CON_PRSCEN;
@@ -198,7 +204,7 @@ static void exynos_adc_v1_exit_hw(struct exynos_adc *info)
 	u32 con;
 
 	if (info->data->needs_adc_phy)
-		writel(0, info->enable_reg);
+		regmap_write(info->pmu_map, info->data->phy_offset, 0);
 
 	con = readl(ADC_V1_CON(info->regs));
 	con |= ADC_V1_CON_STANDBY;
@@ -225,6 +231,7 @@ static const struct exynos_adc_data exynos_adc_v1_data = {
 	.num_channels	= MAX_ADC_V1_CHANNELS,
 	.mask		= ADC_DATX_MASK,	/* 12 bit ADC resolution */
 	.needs_adc_phy	= true,
+	.phy_offset	= EXYNOS_ADCV1_PHY_OFFSET,
 
 	.init_hw	= exynos_adc_v1_init_hw,
 	.exit_hw	= exynos_adc_v1_exit_hw,
@@ -314,7 +321,7 @@ static void exynos_adc_v2_init_hw(struct exynos_adc *info)
 	u32 con1, con2;
 
 	if (info->data->needs_adc_phy)
-		writel(1, info->enable_reg);
+		regmap_write(info->pmu_map, info->data->phy_offset, 1);
 
 	con1 = ADC_V2_CON1_SOFT_RESET;
 	writel(con1, ADC_V2_CON1(info->regs));
@@ -332,7 +339,7 @@ static void exynos_adc_v2_exit_hw(struct exynos_adc *info)
 	u32 con;
 
 	if (info->data->needs_adc_phy)
-		writel(0, info->enable_reg);
+		regmap_write(info->pmu_map, info->data->phy_offset, 0);
 
 	con = readl(ADC_V2_CON1(info->regs));
 	con &= ~ADC_CON_EN_START;
@@ -362,6 +369,7 @@ static const struct exynos_adc_data exynos_adc_v2_data = {
 	.num_channels	= MAX_ADC_V2_CHANNELS,
 	.mask		= ADC_DATX_MASK, /* 12 bit ADC resolution */
 	.needs_adc_phy	= true,
+	.phy_offset	= EXYNOS_ADCV2_PHY_OFFSET,
 
 	.init_hw	= exynos_adc_v2_init_hw,
 	.exit_hw	= exynos_adc_v2_exit_hw,
@@ -374,6 +382,7 @@ static const struct exynos_adc_data exynos3250_adc_data = {
 	.mask		= ADC_DATX_MASK, /* 12 bit ADC resolution */
 	.needs_sclk	= true,
 	.needs_adc_phy	= true,
+	.phy_offset	= EXYNOS_ADCV1_PHY_OFFSET,
 
 	.init_hw	= exynos_adc_v2_init_hw,
 	.exit_hw	= exynos_adc_v2_exit_hw,
@@ -381,6 +390,35 @@ static const struct exynos_adc_data exynos3250_adc_data = {
 	.start_conv	= exynos_adc_v2_start_conv,
 };
 
+static void exynos_adc_exynos7_init_hw(struct exynos_adc *info)
+{
+	u32 con1, con2;
+
+	if (info->data->needs_adc_phy)
+		regmap_write(info->pmu_map, info->data->phy_offset, 1);
+
+	con1 = ADC_V2_CON1_SOFT_RESET;
+	writel(con1, ADC_V2_CON1(info->regs));
+
+	con2 = readl(ADC_V2_CON2(info->regs));
+	con2 &= ~ADC_V2_CON2_C_TIME(7);
+	con2 |= ADC_V2_CON2_C_TIME(0);
+	writel(con2, ADC_V2_CON2(info->regs));
+
+	/* Enable interrupts */
+	writel(1, ADC_V2_INT_EN(info->regs));
+}
+
+static const struct exynos_adc_data exynos7_adc_data = {
+	.num_channels	= MAX_ADC_V1_CHANNELS,
+	.mask		= ADC_DATX_MASK, /* 12 bit ADC resolution */
+
+	.init_hw	= exynos_adc_exynos7_init_hw,
+	.exit_hw	= exynos_adc_v2_exit_hw,
+	.clear_irq	= exynos_adc_v2_clear_irq,
+	.start_conv	= exynos_adc_v2_start_conv,
+};
+
 static const struct of_device_id exynos_adc_match[] = {
 	{
 		.compatible = "samsung,s3c2410-adc",
@@ -406,6 +444,9 @@ static const struct of_device_id exynos_adc_match[] = {
 	}, {
 		.compatible = "samsung,exynos3250-adc",
 		.data = &exynos3250_adc_data,
+	}, {
+		.compatible = "samsung,exynos7-adc",
+		.data = &exynos7_adc_data,
 	},
 	{},
 };
@@ -558,10 +599,13 @@ static int exynos_adc_probe(struct platform_device *pdev)
 
 
 	if (info->data->needs_adc_phy) {
-		mem = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-		info->enable_reg = devm_ioremap_resource(&pdev->dev, mem);
-		if (IS_ERR(info->enable_reg))
-			return PTR_ERR(info->enable_reg);
+		info->pmu_map = syscon_regmap_lookup_by_phandle(
+					pdev->dev.of_node,
+					"samsung,syscon-phandle");
+		if (IS_ERR(info->pmu_map)) {
+			dev_err(&pdev->dev, "syscon regmap lookup failed.\n");
+			return PTR_ERR(info->pmu_map);
+		}
 	}
 
 	irq = platform_get_irq(pdev, 0);
diff --git a/drivers/iio/adc/mcp320x.c b/drivers/iio/adc/mcp320x.c
index 28a086e48776..efbfd12a4bfd 100644
--- a/drivers/iio/adc/mcp320x.c
+++ b/drivers/iio/adc/mcp320x.c
@@ -1,9 +1,30 @@
 /*
  * Copyright (C) 2013 Oskar Andero <oskar.andero@gmail.com>
+ * Copyright (C) 2014 Rose Technology
+ * 	   Allan Bendorff Jensen <abj@rosetechnology.dk>
+ *	   Soren Andersen <san@rosetechnology.dk>
+ *
+ * Driver for following ADC chips from Microchip Technology's:
+ * 10 Bit converter
+ * MCP3001
+ * MCP3002
+ * MCP3004
+ * MCP3008
+ * ------------
+ * 12 bit converter
+ * MCP3201
+ * MCP3202
+ * MCP3204
+ * MCP3208
+ * ------------
  *
- * Driver for Microchip Technology's MCP3204 and MCP3208 ADC chips.
  * Datasheet can be found here:
- * http://ww1.microchip.com/downloads/en/devicedoc/21298c.pdf
+ * http://ww1.microchip.com/downloads/en/DeviceDoc/21293C.pdf  mcp3001
+ * http://ww1.microchip.com/downloads/en/DeviceDoc/21294E.pdf  mcp3002
+ * http://ww1.microchip.com/downloads/en/DeviceDoc/21295d.pdf  mcp3004/08
+ * http://ww1.microchip.com/downloads/en/DeviceDoc/21290D.pdf  mcp3201
+ * http://ww1.microchip.com/downloads/en/DeviceDoc/21034D.pdf  mcp3202
+ * http://ww1.microchip.com/downloads/en/DeviceDoc/21298c.pdf  mcp3204/08
  *
  * 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
@@ -11,19 +32,29 @@
  */
 
 #include <linux/err.h>
+#include <linux/delay.h>
 #include <linux/spi/spi.h>
 #include <linux/module.h>
 #include <linux/iio/iio.h>
 #include <linux/regulator/consumer.h>
 
-#define MCP_SINGLE_ENDED	(1 << 3)
-#define MCP_START_BIT		(1 << 4)
-
 enum {
+	mcp3001,
+	mcp3002,
+	mcp3004,
+	mcp3008,
+	mcp3201,
+	mcp3202,
 	mcp3204,
 	mcp3208,
 };
 
+struct mcp320x_chip_info {
+	const struct iio_chan_spec *channels;
+	unsigned int num_channels;
+	unsigned int resolution;
+};
+
 struct mcp320x {
 	struct spi_device *spi;
 	struct spi_message msg;
@@ -34,19 +65,69 @@ struct mcp320x {
 
 	struct regulator *reg;
 	struct mutex lock;
+	const struct mcp320x_chip_info *chip_info;
 };
 
-static int mcp320x_adc_conversion(struct mcp320x *adc, u8 msg)
+static int mcp320x_channel_to_tx_data(int device_index,
+			const unsigned int channel, bool differential)
+{
+	int start_bit = 1;
+
+	switch (device_index) {
+	case mcp3001:
+	case mcp3201:
+		return 0;
+	case mcp3002:
+	case mcp3202:
+		return ((start_bit << 4) | (!differential << 3) |
+							(channel << 2));
+	case mcp3004:
+	case mcp3204:
+	case mcp3008:
+	case mcp3208:
+		return ((start_bit << 6) | (!differential << 5) |
+							(channel << 2));
+	default:
+		return -EINVAL;
+	}
+}
+
+static int mcp320x_adc_conversion(struct mcp320x *adc, u8 channel,
+				  bool differential, int device_index)
 {
 	int ret;
 
-	adc->tx_buf = msg;
-	ret = spi_sync(adc->spi, &adc->msg);
-	if (ret < 0)
-		return ret;
+	adc->rx_buf[0] = 0;
+	adc->rx_buf[1] = 0;
+	adc->tx_buf = mcp320x_channel_to_tx_data(device_index,
+						channel, differential);
+
+	if (device_index != mcp3001 && device_index != mcp3201) {
+		ret = spi_sync(adc->spi, &adc->msg);
+		if (ret < 0)
+			return ret;
+	} else {
+		ret = spi_read(adc->spi, &adc->rx_buf, sizeof(adc->rx_buf));
+		if (ret < 0)
+			return ret;
+	}
 
-	return ((adc->rx_buf[0] & 0x3f) << 6)  |
-		(adc->rx_buf[1] >> 2);
+	switch (device_index) {
+	case mcp3001:
+		return (adc->rx_buf[0] << 5 | adc->rx_buf[1] >> 3);
+	case mcp3002:
+	case mcp3004:
+	case mcp3008:
+		return (adc->rx_buf[0] << 2 | adc->rx_buf[1] >> 6);
+	case mcp3201:
+		return (adc->rx_buf[0] << 7 | adc->rx_buf[1] >> 1);
+	case mcp3202:
+	case mcp3204:
+	case mcp3208:
+		return (adc->rx_buf[0] << 4 | adc->rx_buf[1] >> 4);
+	default:
+		return -EINVAL;
+	}
 }
 
 static int mcp320x_read_raw(struct iio_dev *indio_dev,
@@ -55,18 +136,17 @@ static int mcp320x_read_raw(struct iio_dev *indio_dev,
 {
 	struct mcp320x *adc = iio_priv(indio_dev);
 	int ret = -EINVAL;
+	int device_index = 0;
 
 	mutex_lock(&adc->lock);
 
+	device_index = spi_get_device_id(adc->spi)->driver_data;
+
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
-		if (channel->differential)
-			ret = mcp320x_adc_conversion(adc,
-				MCP_START_BIT | channel->address);
-		else
-			ret = mcp320x_adc_conversion(adc,
-				MCP_START_BIT | MCP_SINGLE_ENDED |
-				channel->address);
+		ret = mcp320x_adc_conversion(adc, channel->address,
+			channel->differential, device_index);
+
 		if (ret < 0)
 			goto out;
 
@@ -75,18 +155,15 @@ static int mcp320x_read_raw(struct iio_dev *indio_dev,
 		break;
 
 	case IIO_CHAN_INFO_SCALE:
-		/* Digital output code = (4096 * Vin) / Vref */
 		ret = regulator_get_voltage(adc->reg);
 		if (ret < 0)
 			goto out;
 
+		/* convert regulator output voltage to mV */
 		*val = ret / 1000;
-		*val2 = 12;
+		*val2 = adc->chip_info->resolution;
 		ret = IIO_VAL_FRACTIONAL_LOG2;
 		break;
-
-	default:
-		break;
 	}
 
 out:
@@ -117,6 +194,16 @@ out:
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) \
 	}
 
+static const struct iio_chan_spec mcp3201_channels[] = {
+	MCP320X_VOLTAGE_CHANNEL_DIFF(0),
+};
+
+static const struct iio_chan_spec mcp3202_channels[] = {
+	MCP320X_VOLTAGE_CHANNEL(0),
+	MCP320X_VOLTAGE_CHANNEL(1),
+	MCP320X_VOLTAGE_CHANNEL_DIFF(0),
+};
+
 static const struct iio_chan_spec mcp3204_channels[] = {
 	MCP320X_VOLTAGE_CHANNEL(0),
 	MCP320X_VOLTAGE_CHANNEL(1),
@@ -146,19 +233,46 @@ static const struct iio_info mcp320x_info = {
 	.driver_module = THIS_MODULE,
 };
 
-struct mcp3208_chip_info {
-	const struct iio_chan_spec *channels;
-	unsigned int num_channels;
-};
-
-static const struct mcp3208_chip_info mcp3208_chip_infos[] = {
+static const struct mcp320x_chip_info mcp320x_chip_infos[] = {
+	[mcp3001] = {
+		.channels = mcp3201_channels,
+		.num_channels = ARRAY_SIZE(mcp3201_channels),
+		.resolution = 10
+	},
+	[mcp3002] = {
+		.channels = mcp3202_channels,
+		.num_channels = ARRAY_SIZE(mcp3202_channels),
+		.resolution = 10
+	},
+	[mcp3004] = {
+		.channels = mcp3204_channels,
+		.num_channels = ARRAY_SIZE(mcp3204_channels),
+		.resolution = 10
+	},
+	[mcp3008] = {
+		.channels = mcp3208_channels,
+		.num_channels = ARRAY_SIZE(mcp3208_channels),
+		.resolution = 10
+	},
+	[mcp3201] = {
+		.channels = mcp3201_channels,
+		.num_channels = ARRAY_SIZE(mcp3201_channels),
+		.resolution = 12
+	},
+	[mcp3202] = {
+		.channels = mcp3202_channels,
+		.num_channels = ARRAY_SIZE(mcp3202_channels),
+		.resolution = 12
+	},
 	[mcp3204] = {
 		.channels = mcp3204_channels,
-		.num_channels = ARRAY_SIZE(mcp3204_channels)
+		.num_channels = ARRAY_SIZE(mcp3204_channels),
+		.resolution = 12
 	},
 	[mcp3208] = {
 		.channels = mcp3208_channels,
-		.num_channels = ARRAY_SIZE(mcp3208_channels)
+		.num_channels = ARRAY_SIZE(mcp3208_channels),
+		.resolution = 12
 	},
 };
 
@@ -166,7 +280,7 @@ static int mcp320x_probe(struct spi_device *spi)
 {
 	struct iio_dev *indio_dev;
 	struct mcp320x *adc;
-	const struct mcp3208_chip_info *chip_info;
+	const struct mcp320x_chip_info *chip_info;
 	int ret;
 
 	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*adc));
@@ -181,7 +295,7 @@ static int mcp320x_probe(struct spi_device *spi)
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->info = &mcp320x_info;
 
-	chip_info = &mcp3208_chip_infos[spi_get_device_id(spi)->driver_data];
+	chip_info = &mcp320x_chip_infos[spi_get_device_id(spi)->driver_data];
 	indio_dev->channels = chip_info->channels;
 	indio_dev->num_channels = chip_info->num_channels;
 
@@ -226,7 +340,45 @@ static int mcp320x_remove(struct spi_device *spi)
 	return 0;
 }
 
+#if defined(CONFIG_OF)
+static const struct of_device_id mcp320x_dt_ids[] = {
+	{
+		.compatible = "mcp3001",
+		.data = &mcp320x_chip_infos[mcp3001],
+	}, {
+		.compatible = "mcp3002",
+		.data = &mcp320x_chip_infos[mcp3002],
+	}, {
+		.compatible = "mcp3004",
+		.data = &mcp320x_chip_infos[mcp3004],
+	}, {
+		.compatible = "mcp3008",
+		.data = &mcp320x_chip_infos[mcp3008],
+	}, {
+		.compatible = "mcp3201",
+		.data = &mcp320x_chip_infos[mcp3201],
+	}, {
+		.compatible = "mcp3202",
+		.data = &mcp320x_chip_infos[mcp3202],
+	}, {
+		.compatible = "mcp3204",
+		.data = &mcp320x_chip_infos[mcp3204],
+	}, {
+		.compatible = "mcp3208",
+		.data = &mcp320x_chip_infos[mcp3208],
+	}, {
+	}
+};
+MODULE_DEVICE_TABLE(of, mcp320x_dt_ids);
+#endif
+
 static const struct spi_device_id mcp320x_id[] = {
+	{ "mcp3001", mcp3001 },
+	{ "mcp3002", mcp3002 },
+	{ "mcp3004", mcp3004 },
+	{ "mcp3008", mcp3008 },
+	{ "mcp3201", mcp3201 },
+	{ "mcp3202", mcp3202 },
 	{ "mcp3204", mcp3204 },
 	{ "mcp3208", mcp3208 },
 	{ }
@@ -245,5 +397,5 @@ static struct spi_driver mcp320x_driver = {
 module_spi_driver(mcp320x_driver);
 
 MODULE_AUTHOR("Oskar Andero <oskar.andero@gmail.com>");
-MODULE_DESCRIPTION("Microchip Technology MCP3204/08");
+MODULE_DESCRIPTION("Microchip Technology MCP3x01/02/04/08");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/qcom-spmi-iadc.c b/drivers/iio/adc/qcom-spmi-iadc.c
new file mode 100644
index 000000000000..b9666f2f5e51
--- /dev/null
+++ b/drivers/iio/adc/qcom-spmi-iadc.c
@@ -0,0 +1,595 @@
+/*
+ * Copyright (c) 2012-2014, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/bitops.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/iio/iio.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/mutex.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+
+/* IADC register and bit definition */
+#define IADC_REVISION2				0x1
+#define IADC_REVISION2_SUPPORTED_IADC		1
+
+#define IADC_PERPH_TYPE				0x4
+#define IADC_PERPH_TYPE_ADC			8
+
+#define IADC_PERPH_SUBTYPE			0x5
+#define IADC_PERPH_SUBTYPE_IADC			3
+
+#define IADC_STATUS1				0x8
+#define IADC_STATUS1_OP_MODE			4
+#define IADC_STATUS1_REQ_STS			BIT(1)
+#define IADC_STATUS1_EOC			BIT(0)
+#define IADC_STATUS1_REQ_STS_EOC_MASK		0x3
+
+#define IADC_MODE_CTL				0x40
+#define IADC_OP_MODE_SHIFT			3
+#define IADC_OP_MODE_NORMAL			0
+#define IADC_TRIM_EN				BIT(0)
+
+#define IADC_EN_CTL1				0x46
+#define IADC_EN_CTL1_SET			BIT(7)
+
+#define IADC_CH_SEL_CTL				0x48
+
+#define IADC_DIG_PARAM				0x50
+#define IADC_DIG_DEC_RATIO_SEL_SHIFT		2
+
+#define IADC_HW_SETTLE_DELAY			0x51
+
+#define IADC_CONV_REQ				0x52
+#define IADC_CONV_REQ_SET			BIT(7)
+
+#define IADC_FAST_AVG_CTL			0x5a
+#define IADC_FAST_AVG_EN			0x5b
+#define IADC_FAST_AVG_EN_SET			BIT(7)
+
+#define IADC_PERH_RESET_CTL3			0xda
+#define IADC_FOLLOW_WARM_RB			BIT(2)
+
+#define IADC_DATA				0x60	/* 16 bits */
+
+#define IADC_SEC_ACCESS				0xd0
+#define IADC_SEC_ACCESS_DATA			0xa5
+
+#define IADC_NOMINAL_RSENSE			0xf4
+#define IADC_NOMINAL_RSENSE_SIGN_MASK		BIT(7)
+
+#define IADC_REF_GAIN_MICRO_VOLTS		17857
+
+#define IADC_INT_RSENSE_DEVIATION		15625	/* nano Ohms per bit */
+
+#define IADC_INT_RSENSE_IDEAL_VALUE		10000	/* micro Ohms */
+#define IADC_INT_RSENSE_DEFAULT_VALUE		7800	/* micro Ohms */
+#define IADC_INT_RSENSE_DEFAULT_GF		9000	/* micro Ohms */
+#define IADC_INT_RSENSE_DEFAULT_SMIC		9700	/* micro Ohms */
+
+#define IADC_CONV_TIME_MIN_US			2000
+#define IADC_CONV_TIME_MAX_US			2100
+
+#define IADC_DEF_PRESCALING			0 /* 1:1 */
+#define IADC_DEF_DECIMATION			0 /* 512 */
+#define IADC_DEF_HW_SETTLE_TIME			0 /* 0 us */
+#define IADC_DEF_AVG_SAMPLES			0 /* 1 sample */
+
+/* IADC channel list */
+#define IADC_INT_RSENSE				0
+#define IADC_EXT_RSENSE				1
+#define IADC_GAIN_17P857MV			3
+#define IADC_EXT_OFFSET_CSP_CSN			5
+#define IADC_INT_OFFSET_CSP2_CSN2		6
+
+/**
+ * struct iadc_chip - IADC Current ADC device structure.
+ * @regmap: regmap for register read/write.
+ * @dev: This device pointer.
+ * @base: base offset for the ADC peripheral.
+ * @rsense: Values of the internal and external sense resister in micro Ohms.
+ * @poll_eoc: Poll for end of conversion instead of waiting for IRQ.
+ * @offset: Raw offset values for the internal and external channels.
+ * @gain: Raw gain of the channels.
+ * @lock: ADC lock for access to the peripheral.
+ * @complete: ADC notification after end of conversion interrupt is received.
+ */
+struct iadc_chip {
+	struct regmap	*regmap;
+	struct device	*dev;
+	u16		base;
+	bool		poll_eoc;
+	u32		rsense[2];
+	u16		offset[2];
+	u16		gain;
+	struct mutex	lock;
+	struct completion complete;
+};
+
+static int iadc_read(struct iadc_chip *iadc, u16 offset, u8 *data)
+{
+	unsigned int val;
+	int ret;
+
+	ret = regmap_read(iadc->regmap, iadc->base + offset, &val);
+	if (ret < 0)
+		return ret;
+
+	*data = val;
+	return 0;
+}
+
+static int iadc_write(struct iadc_chip *iadc, u16 offset, u8 data)
+{
+	return regmap_write(iadc->regmap, iadc->base + offset, data);
+}
+
+static int iadc_reset(struct iadc_chip *iadc)
+{
+	u8 data;
+	int ret;
+
+	ret = iadc_write(iadc, IADC_SEC_ACCESS, IADC_SEC_ACCESS_DATA);
+	if (ret < 0)
+		return ret;
+
+	ret = iadc_read(iadc, IADC_PERH_RESET_CTL3, &data);
+	if (ret < 0)
+		return ret;
+
+	ret = iadc_write(iadc, IADC_SEC_ACCESS, IADC_SEC_ACCESS_DATA);
+	if (ret < 0)
+		return ret;
+
+	data |= IADC_FOLLOW_WARM_RB;
+
+	return iadc_write(iadc, IADC_PERH_RESET_CTL3, data);
+}
+
+static int iadc_set_state(struct iadc_chip *iadc, bool state)
+{
+	return iadc_write(iadc, IADC_EN_CTL1, state ? IADC_EN_CTL1_SET : 0);
+}
+
+static void iadc_status_show(struct iadc_chip *iadc)
+{
+	u8 mode, sta1, chan, dig, en, req;
+	int ret;
+
+	ret = iadc_read(iadc, IADC_MODE_CTL, &mode);
+	if (ret < 0)
+		return;
+
+	ret = iadc_read(iadc, IADC_DIG_PARAM, &dig);
+	if (ret < 0)
+		return;
+
+	ret = iadc_read(iadc, IADC_CH_SEL_CTL, &chan);
+	if (ret < 0)
+		return;
+
+	ret = iadc_read(iadc, IADC_CONV_REQ, &req);
+	if (ret < 0)
+		return;
+
+	ret = iadc_read(iadc, IADC_STATUS1, &sta1);
+	if (ret < 0)
+		return;
+
+	ret = iadc_read(iadc, IADC_EN_CTL1, &en);
+	if (ret < 0)
+		return;
+
+	dev_err(iadc->dev,
+		"mode:%02x en:%02x chan:%02x dig:%02x req:%02x sta1:%02x\n",
+		mode, en, chan, dig, req, sta1);
+}
+
+static int iadc_configure(struct iadc_chip *iadc, int channel)
+{
+	u8 decim, mode;
+	int ret;
+
+	/* Mode selection */
+	mode = (IADC_OP_MODE_NORMAL << IADC_OP_MODE_SHIFT) | IADC_TRIM_EN;
+	ret = iadc_write(iadc, IADC_MODE_CTL, mode);
+	if (ret < 0)
+		return ret;
+
+	/* Channel selection */
+	ret = iadc_write(iadc, IADC_CH_SEL_CTL, channel);
+	if (ret < 0)
+		return ret;
+
+	/* Digital parameter setup */
+	decim = IADC_DEF_DECIMATION << IADC_DIG_DEC_RATIO_SEL_SHIFT;
+	ret = iadc_write(iadc, IADC_DIG_PARAM, decim);
+	if (ret < 0)
+		return ret;
+
+	/* HW settle time delay */
+	ret = iadc_write(iadc, IADC_HW_SETTLE_DELAY, IADC_DEF_HW_SETTLE_TIME);
+	if (ret < 0)
+		return ret;
+
+	ret = iadc_write(iadc, IADC_FAST_AVG_CTL, IADC_DEF_AVG_SAMPLES);
+	if (ret < 0)
+		return ret;
+
+	if (IADC_DEF_AVG_SAMPLES)
+		ret = iadc_write(iadc, IADC_FAST_AVG_EN, IADC_FAST_AVG_EN_SET);
+	else
+		ret = iadc_write(iadc, IADC_FAST_AVG_EN, 0);
+
+	if (ret < 0)
+		return ret;
+
+	if (!iadc->poll_eoc)
+		reinit_completion(&iadc->complete);
+
+	ret = iadc_set_state(iadc, true);
+	if (ret < 0)
+		return ret;
+
+	/* Request conversion */
+	return iadc_write(iadc, IADC_CONV_REQ, IADC_CONV_REQ_SET);
+}
+
+static int iadc_poll_wait_eoc(struct iadc_chip *iadc, unsigned int interval_us)
+{
+	unsigned int count, retry;
+	int ret;
+	u8 sta1;
+
+	retry = interval_us / IADC_CONV_TIME_MIN_US;
+
+	for (count = 0; count < retry; count++) {
+		ret = iadc_read(iadc, IADC_STATUS1, &sta1);
+		if (ret < 0)
+			return ret;
+
+		sta1 &= IADC_STATUS1_REQ_STS_EOC_MASK;
+		if (sta1 == IADC_STATUS1_EOC)
+			return 0;
+
+		usleep_range(IADC_CONV_TIME_MIN_US, IADC_CONV_TIME_MAX_US);
+	}
+
+	iadc_status_show(iadc);
+
+	return -ETIMEDOUT;
+}
+
+static int iadc_read_result(struct iadc_chip *iadc, u16 *data)
+{
+	return regmap_bulk_read(iadc->regmap, iadc->base + IADC_DATA, data, 2);
+}
+
+static int iadc_do_conversion(struct iadc_chip *iadc, int chan, u16 *data)
+{
+	unsigned int wait;
+	int ret;
+
+	ret = iadc_configure(iadc, chan);
+	if (ret < 0)
+		goto exit;
+
+	wait = BIT(IADC_DEF_AVG_SAMPLES) * IADC_CONV_TIME_MIN_US * 2;
+
+	if (iadc->poll_eoc) {
+		ret = iadc_poll_wait_eoc(iadc, wait);
+	} else {
+		ret = wait_for_completion_timeout(&iadc->complete, wait);
+		if (!ret)
+			ret = -ETIMEDOUT;
+		else
+			/* double check conversion status */
+			ret = iadc_poll_wait_eoc(iadc, IADC_CONV_TIME_MIN_US);
+	}
+
+	if (!ret)
+		ret = iadc_read_result(iadc, data);
+exit:
+	iadc_set_state(iadc, false);
+	if (ret < 0)
+		dev_err(iadc->dev, "conversion failed\n");
+
+	return ret;
+}
+
+static int iadc_read_raw(struct iio_dev *indio_dev,
+			 struct iio_chan_spec const *chan,
+			 int *val, int *val2, long mask)
+{
+	struct iadc_chip *iadc = iio_priv(indio_dev);
+	s32 isense_ua, vsense_uv;
+	u16 adc_raw, vsense_raw;
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		mutex_lock(&iadc->lock);
+		ret = iadc_do_conversion(iadc, chan->channel, &adc_raw);
+		mutex_unlock(&iadc->lock);
+		if (ret < 0)
+			return ret;
+
+		vsense_raw = adc_raw - iadc->offset[chan->channel];
+
+		vsense_uv = vsense_raw * IADC_REF_GAIN_MICRO_VOLTS;
+		vsense_uv /= (s32)iadc->gain - iadc->offset[chan->channel];
+
+		isense_ua = vsense_uv / iadc->rsense[chan->channel];
+
+		dev_dbg(iadc->dev, "off %d gain %d adc %d %duV I %duA\n",
+			iadc->offset[chan->channel], iadc->gain,
+			adc_raw, vsense_uv, isense_ua);
+
+		*val = isense_ua;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		*val = 0;
+		*val2 = 1000;
+		return IIO_VAL_INT_PLUS_MICRO;
+	}
+
+	return -EINVAL;
+}
+
+static const struct iio_info iadc_info = {
+	.read_raw = iadc_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static irqreturn_t iadc_isr(int irq, void *dev_id)
+{
+	struct iadc_chip *iadc = dev_id;
+
+	complete(&iadc->complete);
+
+	return IRQ_HANDLED;
+}
+
+static int iadc_update_offset(struct iadc_chip *iadc)
+{
+	int ret;
+
+	ret = iadc_do_conversion(iadc, IADC_GAIN_17P857MV, &iadc->gain);
+	if (ret < 0)
+		return ret;
+
+	ret = iadc_do_conversion(iadc, IADC_INT_OFFSET_CSP2_CSN2,
+				 &iadc->offset[IADC_INT_RSENSE]);
+	if (ret < 0)
+		return ret;
+
+	if (iadc->gain == iadc->offset[IADC_INT_RSENSE]) {
+		dev_err(iadc->dev, "error: internal offset == gain %d\n",
+			iadc->gain);
+		return -EINVAL;
+	}
+
+	ret = iadc_do_conversion(iadc, IADC_EXT_OFFSET_CSP_CSN,
+				 &iadc->offset[IADC_EXT_RSENSE]);
+	if (ret < 0)
+		return ret;
+
+	if (iadc->gain == iadc->offset[IADC_EXT_RSENSE]) {
+		dev_err(iadc->dev, "error: external offset == gain %d\n",
+			iadc->gain);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int iadc_version_check(struct iadc_chip *iadc)
+{
+	u8 val;
+	int ret;
+
+	ret = iadc_read(iadc, IADC_PERPH_TYPE, &val);
+	if (ret < 0)
+		return ret;
+
+	if (val < IADC_PERPH_TYPE_ADC) {
+		dev_err(iadc->dev, "%d is not ADC\n", val);
+		return -EINVAL;
+	}
+
+	ret = iadc_read(iadc, IADC_PERPH_SUBTYPE, &val);
+	if (ret < 0)
+		return ret;
+
+	if (val < IADC_PERPH_SUBTYPE_IADC) {
+		dev_err(iadc->dev, "%d is not IADC\n", val);
+		return -EINVAL;
+	}
+
+	ret = iadc_read(iadc, IADC_REVISION2, &val);
+	if (ret < 0)
+		return ret;
+
+	if (val < IADC_REVISION2_SUPPORTED_IADC) {
+		dev_err(iadc->dev, "revision %d not supported\n", val);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int iadc_rsense_read(struct iadc_chip *iadc, struct device_node *node)
+{
+	int ret, sign, int_sense;
+	u8 deviation;
+
+	ret = of_property_read_u32(node, "qcom,external-resistor-micro-ohms",
+				   &iadc->rsense[IADC_EXT_RSENSE]);
+	if (ret < 0)
+		iadc->rsense[IADC_EXT_RSENSE] = IADC_INT_RSENSE_IDEAL_VALUE;
+
+	if (!iadc->rsense[IADC_EXT_RSENSE]) {
+		dev_err(iadc->dev, "external resistor can't be zero Ohms");
+		return -EINVAL;
+	}
+
+	ret = iadc_read(iadc, IADC_NOMINAL_RSENSE, &deviation);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * Deviation value stored is an offset from 10 mili Ohms, bit 7 is
+	 * the sign, the remaining bits have an LSB of 15625 nano Ohms.
+	 */
+	sign = (deviation & IADC_NOMINAL_RSENSE_SIGN_MASK) ? -1 : 1;
+
+	deviation &= ~IADC_NOMINAL_RSENSE_SIGN_MASK;
+
+	/* Scale it to nono Ohms */
+	int_sense = IADC_INT_RSENSE_IDEAL_VALUE * 1000;
+	int_sense += sign * deviation * IADC_INT_RSENSE_DEVIATION;
+	int_sense /= 1000; /* micro Ohms */
+
+	iadc->rsense[IADC_INT_RSENSE] = int_sense;
+	return 0;
+}
+
+static const struct iio_chan_spec iadc_channels[] = {
+	{
+		.type = IIO_CURRENT,
+		.datasheet_name	= "INTERNAL_RSENSE",
+		.channel = 0,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+				      BIT(IIO_CHAN_INFO_SCALE),
+		.indexed = 1,
+	},
+	{
+		.type = IIO_CURRENT,
+		.datasheet_name	= "EXTERNAL_RSENSE",
+		.channel = 1,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+				      BIT(IIO_CHAN_INFO_SCALE),
+		.indexed = 1,
+	},
+};
+
+static int iadc_probe(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct device *dev = &pdev->dev;
+	struct iio_dev *indio_dev;
+	struct iadc_chip *iadc;
+	int ret, irq_eoc;
+	u32 res;
+
+	indio_dev = devm_iio_device_alloc(dev, sizeof(*iadc));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	iadc = iio_priv(indio_dev);
+	iadc->dev = dev;
+
+	iadc->regmap = dev_get_regmap(dev->parent, NULL);
+	if (!iadc->regmap)
+		return -ENODEV;
+
+	init_completion(&iadc->complete);
+	mutex_init(&iadc->lock);
+
+	ret = of_property_read_u32(node, "reg", &res);
+	if (ret < 0)
+		return -ENODEV;
+
+	iadc->base = res;
+
+	ret = iadc_version_check(iadc);
+	if (ret < 0)
+		return -ENODEV;
+
+	ret = iadc_rsense_read(iadc, node);
+	if (ret < 0)
+		return -ENODEV;
+
+	dev_dbg(iadc->dev, "sense resistors %d and %d micro Ohm\n",
+		iadc->rsense[IADC_INT_RSENSE],
+		iadc->rsense[IADC_EXT_RSENSE]);
+
+	irq_eoc = platform_get_irq(pdev, 0);
+	if (irq_eoc == -EPROBE_DEFER)
+		return irq_eoc;
+
+	if (irq_eoc < 0)
+		iadc->poll_eoc = true;
+
+	ret = iadc_reset(iadc);
+	if (ret < 0) {
+		dev_err(dev, "reset failed\n");
+		return ret;
+	}
+
+	if (!iadc->poll_eoc) {
+		ret = devm_request_irq(dev, irq_eoc, iadc_isr, 0,
+					"spmi-iadc", iadc);
+		if (!ret)
+			enable_irq_wake(irq_eoc);
+		else
+			return ret;
+	} else {
+		device_init_wakeup(iadc->dev, 1);
+	}
+
+	ret = iadc_update_offset(iadc);
+	if (ret < 0) {
+		dev_err(dev, "failed offset calibration\n");
+		return ret;
+	}
+
+	indio_dev->dev.parent = dev;
+	indio_dev->dev.of_node = node;
+	indio_dev->name = pdev->name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->info = &iadc_info;
+	indio_dev->channels = iadc_channels;
+	indio_dev->num_channels = ARRAY_SIZE(iadc_channels);
+
+	return devm_iio_device_register(dev, indio_dev);
+}
+
+static const struct of_device_id iadc_match_table[] = {
+	{ .compatible = "qcom,spmi-iadc" },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, iadc_match_table);
+
+static struct platform_driver iadc_driver = {
+	.driver = {
+		   .name = "qcom-spmi-iadc",
+		   .of_match_table = iadc_match_table,
+	},
+	.probe = iadc_probe,
+};
+
+module_platform_driver(iadc_driver);
+
+MODULE_ALIAS("platform:qcom-spmi-iadc");
+MODULE_DESCRIPTION("Qualcomm SPMI PMIC current ADC driver");
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Ivan T. Ivanov <iivanov@mm-sol.com>");
diff --git a/drivers/iio/adc/rockchip_saradc.c b/drivers/iio/adc/rockchip_saradc.c
index e074a0b03f28..8d4e019ea4ca 100644
--- a/drivers/iio/adc/rockchip_saradc.c
+++ b/drivers/iio/adc/rockchip_saradc.c
@@ -18,13 +18,13 @@
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/of.h>
+#include <linux/of_device.h>
 #include <linux/clk.h>
 #include <linux/completion.h>
 #include <linux/regulator/consumer.h>
 #include <linux/iio/iio.h>
 
 #define SARADC_DATA			0x00
-#define SARADC_DATA_MASK		0x3ff
 
 #define SARADC_STAS			0x04
 #define SARADC_STAS_BUSY		BIT(0)
@@ -38,15 +38,22 @@
 #define SARADC_DLY_PU_SOC		0x0c
 #define SARADC_DLY_PU_SOC_MASK		0x3f
 
-#define SARADC_BITS			10
 #define SARADC_TIMEOUT			msecs_to_jiffies(100)
 
+struct rockchip_saradc_data {
+	int				num_bits;
+	const struct iio_chan_spec	*channels;
+	int				num_channels;
+	unsigned long			clk_rate;
+};
+
 struct rockchip_saradc {
 	void __iomem		*regs;
 	struct clk		*pclk;
 	struct clk		*clk;
 	struct completion	completion;
 	struct regulator	*vref;
+	const struct rockchip_saradc_data *data;
 	u16			last_val;
 };
 
@@ -90,7 +97,7 @@ static int rockchip_saradc_read_raw(struct iio_dev *indio_dev,
 		}
 
 		*val = ret / 1000;
-		*val2 = SARADC_BITS;
+		*val2 = info->data->num_bits;
 		return IIO_VAL_FRACTIONAL_LOG2;
 	default:
 		return -EINVAL;
@@ -103,7 +110,7 @@ static irqreturn_t rockchip_saradc_isr(int irq, void *dev_id)
 
 	/* Read value */
 	info->last_val = readl_relaxed(info->regs + SARADC_DATA);
-	info->last_val &= SARADC_DATA_MASK;
+	info->last_val &= GENMASK(info->data->num_bits - 1, 0);
 
 	/* Clear irq & power down adc */
 	writel_relaxed(0, info->regs + SARADC_CTRL);
@@ -133,12 +140,44 @@ static const struct iio_chan_spec rockchip_saradc_iio_channels[] = {
 	ADC_CHANNEL(2, "adc2"),
 };
 
+static const struct rockchip_saradc_data saradc_data = {
+	.num_bits = 10,
+	.channels = rockchip_saradc_iio_channels,
+	.num_channels = ARRAY_SIZE(rockchip_saradc_iio_channels),
+	.clk_rate = 1000000,
+};
+
+static const struct iio_chan_spec rockchip_rk3066_tsadc_iio_channels[] = {
+	ADC_CHANNEL(0, "adc0"),
+	ADC_CHANNEL(1, "adc1"),
+};
+
+static const struct rockchip_saradc_data rk3066_tsadc_data = {
+	.num_bits = 12,
+	.channels = rockchip_rk3066_tsadc_iio_channels,
+	.num_channels = ARRAY_SIZE(rockchip_rk3066_tsadc_iio_channels),
+	.clk_rate = 50000,
+};
+
+static const struct of_device_id rockchip_saradc_match[] = {
+	{
+		.compatible = "rockchip,saradc",
+		.data = &saradc_data,
+	}, {
+		.compatible = "rockchip,rk3066-tsadc",
+		.data = &rk3066_tsadc_data,
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, rockchip_saradc_match);
+
 static int rockchip_saradc_probe(struct platform_device *pdev)
 {
 	struct rockchip_saradc *info = NULL;
 	struct device_node *np = pdev->dev.of_node;
 	struct iio_dev *indio_dev = NULL;
 	struct resource	*mem;
+	const struct of_device_id *match;
 	int ret;
 	int irq;
 
@@ -152,6 +191,9 @@ static int rockchip_saradc_probe(struct platform_device *pdev)
 	}
 	info = iio_priv(indio_dev);
 
+	match = of_match_device(rockchip_saradc_match, &pdev->dev);
+	info->data = match->data;
+
 	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	info->regs = devm_ioremap_resource(&pdev->dev, mem);
 	if (IS_ERR(info->regs))
@@ -192,10 +234,10 @@ static int rockchip_saradc_probe(struct platform_device *pdev)
 	}
 
 	/*
-	 * Use a default of 1MHz for the converter clock.
+	 * Use a default value for the converter clock.
 	 * This may become user-configurable in the future.
 	 */
-	ret = clk_set_rate(info->clk, 1000000);
+	ret = clk_set_rate(info->clk, info->data->clk_rate);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "failed to set adc clk rate, %d\n", ret);
 		return ret;
@@ -227,8 +269,8 @@ static int rockchip_saradc_probe(struct platform_device *pdev)
 	indio_dev->info = &rockchip_saradc_iio_info;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
-	indio_dev->channels = rockchip_saradc_iio_channels;
-	indio_dev->num_channels = ARRAY_SIZE(rockchip_saradc_iio_channels);
+	indio_dev->channels = info->data->channels;
+	indio_dev->num_channels = info->data->num_channels;
 
 	ret = iio_device_register(indio_dev);
 	if (ret)
@@ -296,18 +338,11 @@ static int rockchip_saradc_resume(struct device *dev)
 static SIMPLE_DEV_PM_OPS(rockchip_saradc_pm_ops,
 			 rockchip_saradc_suspend, rockchip_saradc_resume);
 
-static const struct of_device_id rockchip_saradc_match[] = {
-	{ .compatible = "rockchip,saradc" },
-	{},
-};
-MODULE_DEVICE_TABLE(of, rockchip_saradc_match);
-
 static struct platform_driver rockchip_saradc_driver = {
 	.probe		= rockchip_saradc_probe,
 	.remove		= rockchip_saradc_remove,
 	.driver		= {
 		.name	= "rockchip-saradc",
-		.owner	= THIS_MODULE,
 		.of_match_table = rockchip_saradc_match,
 		.pm	= &rockchip_saradc_pm_ops,
 	},
diff --git a/drivers/iio/adc/vf610_adc.c b/drivers/iio/adc/vf610_adc.c
index 4a10ae97dbf2..8ec353c01d98 100644
--- a/drivers/iio/adc/vf610_adc.c
+++ b/drivers/iio/adc/vf610_adc.c
@@ -91,7 +91,7 @@
 #define VF610_ADC_CAL			0x80
 
 /* Other field define */
-#define VF610_ADC_ADCHC(x)		((x) & 0xF)
+#define VF610_ADC_ADCHC(x)		((x) & 0x1F)
 #define VF610_ADC_AIEN			(0x1 << 7)
 #define VF610_ADC_CONV_DISABLE		0x1F
 #define VF610_ADC_HS_COCO0		0x1
@@ -153,6 +153,12 @@ struct vf610_adc {
 				BIT(IIO_CHAN_INFO_SAMP_FREQ),	\
 }
 
+#define VF610_ADC_TEMPERATURE_CHAN(_idx, _chan_type) {	\
+	.type = (_chan_type),	\
+	.channel = (_idx),		\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED),	\
+}
+
 static const struct iio_chan_spec vf610_adc_iio_channels[] = {
 	VF610_ADC_CHAN(0, IIO_VOLTAGE),
 	VF610_ADC_CHAN(1, IIO_VOLTAGE),
@@ -170,6 +176,7 @@ static const struct iio_chan_spec vf610_adc_iio_channels[] = {
 	VF610_ADC_CHAN(13, IIO_VOLTAGE),
 	VF610_ADC_CHAN(14, IIO_VOLTAGE),
 	VF610_ADC_CHAN(15, IIO_VOLTAGE),
+	VF610_ADC_TEMPERATURE_CHAN(26, IIO_TEMP),
 	/* sentinel */
 };
 
@@ -451,6 +458,7 @@ static int vf610_read_raw(struct iio_dev *indio_dev,
 
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
+	case IIO_CHAN_INFO_PROCESSED:
 		mutex_lock(&indio_dev->mlock);
 		reinit_completion(&info->completion);
 
@@ -468,7 +476,23 @@ static int vf610_read_raw(struct iio_dev *indio_dev,
 			return ret;
 		}
 
-		*val = info->value;
+		switch (chan->type) {
+		case IIO_VOLTAGE:
+			*val = info->value;
+			break;
+		case IIO_TEMP:
+			/*
+			* Calculate in degree Celsius times 1000
+			* Using sensor slope of 1.84 mV/°C and
+			* V at 25°C of 696 mV
+			*/
+			*val = 25000 - ((int)info->value - 864) * 1000000 / 1840;
+			break;
+		default:
+			mutex_unlock(&indio_dev->mlock);
+			return -EINVAL;
+		}
+
 		mutex_unlock(&indio_dev->mlock);
 		return IIO_VAL_INT;
 
@@ -569,9 +593,9 @@ static int vf610_adc_probe(struct platform_device *pdev)
 		return PTR_ERR(info->regs);
 
 	irq = platform_get_irq(pdev, 0);
-	if (irq <= 0) {
+	if (irq < 0) {
 		dev_err(&pdev->dev, "no irq resource?\n");
-		return -EINVAL;
+		return irq;
 	}
 
 	ret = devm_request_irq(info->dev, irq,
@@ -586,8 +610,7 @@ static int vf610_adc_probe(struct platform_device *pdev)
 	if (IS_ERR(info->clk)) {
 		dev_err(&pdev->dev, "failed getting clock, err = %ld\n",
 						PTR_ERR(info->clk));
-		ret = PTR_ERR(info->clk);
-		return ret;
+		return PTR_ERR(info->clk);
 	}
 
 	info->vref = devm_regulator_get(&pdev->dev, "vref");
@@ -681,17 +704,19 @@ static int vf610_adc_resume(struct device *dev)
 
 	ret = clk_prepare_enable(info->clk);
 	if (ret)
-		return ret;
+		goto disable_reg;
 
 	vf610_adc_hw_init(info);
 
 	return 0;
+
+disable_reg:
+	regulator_disable(info->vref);
+	return ret;
 }
 #endif
 
-static SIMPLE_DEV_PM_OPS(vf610_adc_pm_ops,
-			vf610_adc_suspend,
-			vf610_adc_resume);
+static SIMPLE_DEV_PM_OPS(vf610_adc_pm_ops, vf610_adc_suspend, vf610_adc_resume);
 
 static struct platform_driver vf610_adc_driver = {
 	.probe          = vf610_adc_probe,
diff --git a/drivers/iio/common/st_sensors/st_sensors_core.c b/drivers/iio/common/st_sensors/st_sensors_core.c
index 24cfe4e044f9..edd13d2b4121 100644
--- a/drivers/iio/common/st_sensors/st_sensors_core.c
+++ b/drivers/iio/common/st_sensors/st_sensors_core.c
@@ -44,18 +44,18 @@ st_sensors_write_data_with_mask_error:
 	return err;
 }
 
-static int st_sensors_match_odr(struct st_sensors *sensor,
+static int st_sensors_match_odr(struct st_sensor_settings *sensor_settings,
 			unsigned int odr, struct st_sensor_odr_avl *odr_out)
 {
 	int i, ret = -EINVAL;
 
 	for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) {
-		if (sensor->odr.odr_avl[i].hz == 0)
+		if (sensor_settings->odr.odr_avl[i].hz == 0)
 			goto st_sensors_match_odr_error;
 
-		if (sensor->odr.odr_avl[i].hz == odr) {
-			odr_out->hz = sensor->odr.odr_avl[i].hz;
-			odr_out->value = sensor->odr.odr_avl[i].value;
+		if (sensor_settings->odr.odr_avl[i].hz == odr) {
+			odr_out->hz = sensor_settings->odr.odr_avl[i].hz;
+			odr_out->value = sensor_settings->odr.odr_avl[i].value;
 			ret = 0;
 			break;
 		}
@@ -71,23 +71,26 @@ int st_sensors_set_odr(struct iio_dev *indio_dev, unsigned int odr)
 	struct st_sensor_odr_avl odr_out = {0, 0};
 	struct st_sensor_data *sdata = iio_priv(indio_dev);
 
-	err = st_sensors_match_odr(sdata->sensor, odr, &odr_out);
+	err = st_sensors_match_odr(sdata->sensor_settings, odr, &odr_out);
 	if (err < 0)
 		goto st_sensors_match_odr_error;
 
-	if ((sdata->sensor->odr.addr == sdata->sensor->pw.addr) &&
-			(sdata->sensor->odr.mask == sdata->sensor->pw.mask)) {
+	if ((sdata->sensor_settings->odr.addr ==
+					sdata->sensor_settings->pw.addr) &&
+				(sdata->sensor_settings->odr.mask ==
+					sdata->sensor_settings->pw.mask)) {
 		if (sdata->enabled == true) {
 			err = st_sensors_write_data_with_mask(indio_dev,
-				sdata->sensor->odr.addr,
-				sdata->sensor->odr.mask,
+				sdata->sensor_settings->odr.addr,
+				sdata->sensor_settings->odr.mask,
 				odr_out.value);
 		} else {
 			err = 0;
 		}
 	} else {
 		err = st_sensors_write_data_with_mask(indio_dev,
-			sdata->sensor->odr.addr, sdata->sensor->odr.mask,
+			sdata->sensor_settings->odr.addr,
+			sdata->sensor_settings->odr.mask,
 			odr_out.value);
 	}
 	if (err >= 0)
@@ -98,16 +101,16 @@ st_sensors_match_odr_error:
 }
 EXPORT_SYMBOL(st_sensors_set_odr);
 
-static int st_sensors_match_fs(struct st_sensors *sensor,
+static int st_sensors_match_fs(struct st_sensor_settings *sensor_settings,
 					unsigned int fs, int *index_fs_avl)
 {
 	int i, ret = -EINVAL;
 
 	for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
-		if (sensor->fs.fs_avl[i].num == 0)
+		if (sensor_settings->fs.fs_avl[i].num == 0)
 			goto st_sensors_match_odr_error;
 
-		if (sensor->fs.fs_avl[i].num == fs) {
+		if (sensor_settings->fs.fs_avl[i].num == fs) {
 			*index_fs_avl = i;
 			ret = 0;
 			break;
@@ -118,25 +121,24 @@ st_sensors_match_odr_error:
 	return ret;
 }
 
-static int st_sensors_set_fullscale(struct iio_dev *indio_dev,
-								unsigned int fs)
+static int st_sensors_set_fullscale(struct iio_dev *indio_dev, unsigned int fs)
 {
 	int err, i = 0;
 	struct st_sensor_data *sdata = iio_priv(indio_dev);
 
-	err = st_sensors_match_fs(sdata->sensor, fs, &i);
+	err = st_sensors_match_fs(sdata->sensor_settings, fs, &i);
 	if (err < 0)
 		goto st_accel_set_fullscale_error;
 
 	err = st_sensors_write_data_with_mask(indio_dev,
-				sdata->sensor->fs.addr,
-				sdata->sensor->fs.mask,
-				sdata->sensor->fs.fs_avl[i].value);
+				sdata->sensor_settings->fs.addr,
+				sdata->sensor_settings->fs.mask,
+				sdata->sensor_settings->fs.fs_avl[i].value);
 	if (err < 0)
 		goto st_accel_set_fullscale_error;
 
 	sdata->current_fullscale = (struct st_sensor_fullscale_avl *)
-						&sdata->sensor->fs.fs_avl[i];
+					&sdata->sensor_settings->fs.fs_avl[i];
 	return err;
 
 st_accel_set_fullscale_error:
@@ -153,10 +155,12 @@ int st_sensors_set_enable(struct iio_dev *indio_dev, bool enable)
 	struct st_sensor_data *sdata = iio_priv(indio_dev);
 
 	if (enable) {
-		tmp_value = sdata->sensor->pw.value_on;
-		if ((sdata->sensor->odr.addr == sdata->sensor->pw.addr) &&
-			(sdata->sensor->odr.mask == sdata->sensor->pw.mask)) {
-			err = st_sensors_match_odr(sdata->sensor,
+		tmp_value = sdata->sensor_settings->pw.value_on;
+		if ((sdata->sensor_settings->odr.addr ==
+					sdata->sensor_settings->pw.addr) &&
+				(sdata->sensor_settings->odr.mask ==
+					sdata->sensor_settings->pw.mask)) {
+			err = st_sensors_match_odr(sdata->sensor_settings,
 							sdata->odr, &odr_out);
 			if (err < 0)
 				goto set_enable_error;
@@ -164,8 +168,8 @@ int st_sensors_set_enable(struct iio_dev *indio_dev, bool enable)
 			found = true;
 		}
 		err = st_sensors_write_data_with_mask(indio_dev,
-				sdata->sensor->pw.addr,
-				sdata->sensor->pw.mask, tmp_value);
+				sdata->sensor_settings->pw.addr,
+				sdata->sensor_settings->pw.mask, tmp_value);
 		if (err < 0)
 			goto set_enable_error;
 
@@ -175,9 +179,9 @@ int st_sensors_set_enable(struct iio_dev *indio_dev, bool enable)
 			sdata->odr = odr_out.hz;
 	} else {
 		err = st_sensors_write_data_with_mask(indio_dev,
-				sdata->sensor->pw.addr,
-				sdata->sensor->pw.mask,
-				sdata->sensor->pw.value_off);
+				sdata->sensor_settings->pw.addr,
+				sdata->sensor_settings->pw.mask,
+				sdata->sensor_settings->pw.value_off);
 		if (err < 0)
 			goto set_enable_error;
 
@@ -194,8 +198,9 @@ int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable)
 	struct st_sensor_data *sdata = iio_priv(indio_dev);
 
 	return st_sensors_write_data_with_mask(indio_dev,
-				sdata->sensor->enable_axis.addr,
-				sdata->sensor->enable_axis.mask, axis_enable);
+				sdata->sensor_settings->enable_axis.addr,
+				sdata->sensor_settings->enable_axis.mask,
+				axis_enable);
 }
 EXPORT_SYMBOL(st_sensors_set_axis_enable);
 
@@ -236,13 +241,13 @@ void st_sensors_power_disable(struct iio_dev *indio_dev)
 EXPORT_SYMBOL(st_sensors_power_disable);
 
 static int st_sensors_set_drdy_int_pin(struct iio_dev *indio_dev,
-				       struct st_sensors_platform_data *pdata)
+					struct st_sensors_platform_data *pdata)
 {
 	struct st_sensor_data *sdata = iio_priv(indio_dev);
 
 	switch (pdata->drdy_int_pin) {
 	case 1:
-		if (sdata->sensor->drdy_irq.mask_int1 == 0) {
+		if (sdata->sensor_settings->drdy_irq.mask_int1 == 0) {
 			dev_err(&indio_dev->dev,
 					"DRDY on INT1 not available.\n");
 			return -EINVAL;
@@ -250,7 +255,7 @@ static int st_sensors_set_drdy_int_pin(struct iio_dev *indio_dev,
 		sdata->drdy_int_pin = 1;
 		break;
 	case 2:
-		if (sdata->sensor->drdy_irq.mask_int2 == 0) {
+		if (sdata->sensor_settings->drdy_irq.mask_int2 == 0) {
 			dev_err(&indio_dev->dev,
 					"DRDY on INT2 not available.\n");
 			return -EINVAL;
@@ -318,7 +323,7 @@ int st_sensors_init_sensor(struct iio_dev *indio_dev,
 
 	if (sdata->current_fullscale) {
 		err = st_sensors_set_fullscale(indio_dev,
-					       sdata->current_fullscale->num);
+						sdata->current_fullscale->num);
 		if (err < 0)
 			return err;
 	} else
@@ -330,7 +335,8 @@ int st_sensors_init_sensor(struct iio_dev *indio_dev,
 
 	/* set BDU */
 	err = st_sensors_write_data_with_mask(indio_dev,
-			sdata->sensor->bdu.addr, sdata->sensor->bdu.mask, true);
+					sdata->sensor_settings->bdu.addr,
+					sdata->sensor_settings->bdu.mask, true);
 	if (err < 0)
 		return err;
 
@@ -346,26 +352,28 @@ int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable)
 	u8 drdy_mask;
 	struct st_sensor_data *sdata = iio_priv(indio_dev);
 
-	if (!sdata->sensor->drdy_irq.addr)
+	if (!sdata->sensor_settings->drdy_irq.addr)
 		return 0;
 
 	/* Enable/Disable the interrupt generator 1. */
-	if (sdata->sensor->drdy_irq.ig1.en_addr > 0) {
+	if (sdata->sensor_settings->drdy_irq.ig1.en_addr > 0) {
 		err = st_sensors_write_data_with_mask(indio_dev,
-			sdata->sensor->drdy_irq.ig1.en_addr,
-			sdata->sensor->drdy_irq.ig1.en_mask, (int)enable);
+				sdata->sensor_settings->drdy_irq.ig1.en_addr,
+				sdata->sensor_settings->drdy_irq.ig1.en_mask,
+				(int)enable);
 		if (err < 0)
 			goto st_accel_set_dataready_irq_error;
 	}
 
 	if (sdata->drdy_int_pin == 1)
-		drdy_mask = sdata->sensor->drdy_irq.mask_int1;
+		drdy_mask = sdata->sensor_settings->drdy_irq.mask_int1;
 	else
-		drdy_mask = sdata->sensor->drdy_irq.mask_int2;
+		drdy_mask = sdata->sensor_settings->drdy_irq.mask_int2;
 
 	/* Enable/Disable the interrupt generator for data ready. */
 	err = st_sensors_write_data_with_mask(indio_dev,
-			sdata->sensor->drdy_irq.addr, drdy_mask, (int)enable);
+					sdata->sensor_settings->drdy_irq.addr,
+					drdy_mask, (int)enable);
 
 st_accel_set_dataready_irq_error:
 	return err;
@@ -378,8 +386,8 @@ int st_sensors_set_fullscale_by_gain(struct iio_dev *indio_dev, int scale)
 	struct st_sensor_data *sdata = iio_priv(indio_dev);
 
 	for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
-		if ((sdata->sensor->fs.fs_avl[i].gain == scale) &&
-				(sdata->sensor->fs.fs_avl[i].gain != 0)) {
+		if ((sdata->sensor_settings->fs.fs_avl[i].gain == scale) &&
+				(sdata->sensor_settings->fs.fs_avl[i].gain != 0)) {
 			err = 0;
 			break;
 		}
@@ -388,7 +396,7 @@ int st_sensors_set_fullscale_by_gain(struct iio_dev *indio_dev, int scale)
 		goto st_sensors_match_scale_error;
 
 	err = st_sensors_set_fullscale(indio_dev,
-					sdata->sensor->fs.fs_avl[i].num);
+				sdata->sensor_settings->fs.fs_avl[i].num);
 
 st_sensors_match_scale_error:
 	return err;
@@ -439,7 +447,7 @@ int st_sensors_read_info_raw(struct iio_dev *indio_dev,
 		if (err < 0)
 			goto out;
 
-		msleep((sdata->sensor->bootime * 1000) / sdata->odr);
+		msleep((sdata->sensor_settings->bootime * 1000) / sdata->odr);
 		err = st_sensors_read_axis_data(indio_dev, ch, val);
 		if (err < 0)
 			goto out;
@@ -456,7 +464,8 @@ out:
 EXPORT_SYMBOL(st_sensors_read_info_raw);
 
 int st_sensors_check_device_support(struct iio_dev *indio_dev,
-			int num_sensors_list, const struct st_sensors *sensors)
+			int num_sensors_list,
+			const struct st_sensor_settings *sensor_settings)
 {
 	u8 wai;
 	int i, n, err;
@@ -470,23 +479,24 @@ int st_sensors_check_device_support(struct iio_dev *indio_dev,
 	}
 
 	for (i = 0; i < num_sensors_list; i++) {
-		if (sensors[i].wai == wai)
+		if (sensor_settings[i].wai == wai)
 			break;
 	}
 	if (i == num_sensors_list)
 		goto device_not_supported;
 
-	for (n = 0; n < ARRAY_SIZE(sensors[i].sensors_supported); n++) {
+	for (n = 0; n < ARRAY_SIZE(sensor_settings[i].sensors_supported); n++) {
 		if (strcmp(indio_dev->name,
-				&sensors[i].sensors_supported[n][0]) == 0)
+				&sensor_settings[i].sensors_supported[n][0]) == 0)
 			break;
 	}
-	if (n == ARRAY_SIZE(sensors[i].sensors_supported)) {
+	if (n == ARRAY_SIZE(sensor_settings[i].sensors_supported)) {
 		dev_err(&indio_dev->dev, "device name and WhoAmI mismatch.\n");
 		goto sensor_name_mismatch;
 	}
 
-	sdata->sensor = (struct st_sensors *)&sensors[i];
+	sdata->sensor_settings =
+			(struct st_sensor_settings *)&sensor_settings[i];
 
 	return i;
 
@@ -508,11 +518,11 @@ ssize_t st_sensors_sysfs_sampling_frequency_avail(struct device *dev,
 
 	mutex_lock(&indio_dev->mlock);
 	for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) {
-		if (sdata->sensor->odr.odr_avl[i].hz == 0)
+		if (sdata->sensor_settings->odr.odr_avl[i].hz == 0)
 			break;
 
 		len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
-					sdata->sensor->odr.odr_avl[i].hz);
+				sdata->sensor_settings->odr.odr_avl[i].hz);
 	}
 	mutex_unlock(&indio_dev->mlock);
 	buf[len - 1] = '\n';
@@ -530,11 +540,11 @@ ssize_t st_sensors_sysfs_scale_avail(struct device *dev,
 
 	mutex_lock(&indio_dev->mlock);
 	for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
-		if (sdata->sensor->fs.fs_avl[i].num == 0)
+		if (sdata->sensor_settings->fs.fs_avl[i].num == 0)
 			break;
 
 		len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
-					sdata->sensor->fs.fs_avl[i].gain);
+				sdata->sensor_settings->fs.fs_avl[i].gain);
 	}
 	mutex_unlock(&indio_dev->mlock);
 	buf[len - 1] = '\n';
diff --git a/drivers/iio/common/st_sensors/st_sensors_i2c.c b/drivers/iio/common/st_sensors/st_sensors_i2c.c
index bb6f3085f57b..98cfee296d46 100644
--- a/drivers/iio/common/st_sensors/st_sensors_i2c.c
+++ b/drivers/iio/common/st_sensors/st_sensors_i2c.c
@@ -72,6 +72,7 @@ void st_sensors_i2c_configure(struct iio_dev *indio_dev,
 	indio_dev->dev.parent = &client->dev;
 	indio_dev->name = client->name;
 
+	sdata->dev = &client->dev;
 	sdata->tf = &st_sensors_tf_i2c;
 	sdata->get_irq_data_ready = st_sensors_i2c_get_irq;
 }
diff --git a/drivers/iio/common/st_sensors/st_sensors_spi.c b/drivers/iio/common/st_sensors/st_sensors_spi.c
index 251baf6abc25..78a6a1ab3ece 100644
--- a/drivers/iio/common/st_sensors/st_sensors_spi.c
+++ b/drivers/iio/common/st_sensors/st_sensors_spi.c
@@ -111,6 +111,7 @@ void st_sensors_spi_configure(struct iio_dev *indio_dev,
 	indio_dev->dev.parent = &spi->dev;
 	indio_dev->name = spi->modalias;
 
+	sdata->dev = &spi->dev;
 	sdata->tf = &st_sensors_tf_spi;
 	sdata->get_irq_data_ready = st_sensors_spi_get_irq;
 }
diff --git a/drivers/iio/gyro/st_gyro.h b/drivers/iio/gyro/st_gyro.h
index c197360c450b..5353d6328c54 100644
--- a/drivers/iio/gyro/st_gyro.h
+++ b/drivers/iio/gyro/st_gyro.h
@@ -30,8 +30,7 @@ static const struct st_sensors_platform_data gyro_pdata = {
 	.drdy_int_pin = 2,
 };
 
-int st_gyro_common_probe(struct iio_dev *indio_dev,
-					struct st_sensors_platform_data *pdata);
+int st_gyro_common_probe(struct iio_dev *indio_dev);
 void st_gyro_common_remove(struct iio_dev *indio_dev);
 
 #ifdef CONFIG_IIO_BUFFER
diff --git a/drivers/iio/gyro/st_gyro_core.c b/drivers/iio/gyro/st_gyro_core.c
index f156fc6c5c6c..f07a2336f7dc 100644
--- a/drivers/iio/gyro/st_gyro_core.c
+++ b/drivers/iio/gyro/st_gyro_core.c
@@ -103,7 +103,7 @@ static const struct iio_chan_spec st_gyro_16bit_channels[] = {
 	IIO_CHAN_SOFT_TIMESTAMP(3)
 };
 
-static const struct st_sensors st_gyro_sensors[] = {
+static const struct st_sensor_settings st_gyro_sensors_settings[] = {
 	{
 		.wai = ST_GYRO_1_WAI_EXP,
 		.sensors_supported = {
@@ -309,8 +309,7 @@ static const struct iio_trigger_ops st_gyro_trigger_ops = {
 #define ST_GYRO_TRIGGER_OPS NULL
 #endif
 
-int st_gyro_common_probe(struct iio_dev *indio_dev,
-					struct st_sensors_platform_data *pdata)
+int st_gyro_common_probe(struct iio_dev *indio_dev)
 {
 	struct st_sensor_data *gdata = iio_priv(indio_dev);
 	int irq = gdata->get_irq_data_ready(indio_dev);
@@ -322,20 +321,22 @@ int st_gyro_common_probe(struct iio_dev *indio_dev,
 	st_sensors_power_enable(indio_dev);
 
 	err = st_sensors_check_device_support(indio_dev,
-				ARRAY_SIZE(st_gyro_sensors), st_gyro_sensors);
+					ARRAY_SIZE(st_gyro_sensors_settings),
+					st_gyro_sensors_settings);
 	if (err < 0)
 		return err;
 
 	gdata->num_data_channels = ST_GYRO_NUMBER_DATA_CHANNELS;
-	gdata->multiread_bit = gdata->sensor->multi_read_bit;
-	indio_dev->channels = gdata->sensor->ch;
+	gdata->multiread_bit = gdata->sensor_settings->multi_read_bit;
+	indio_dev->channels = gdata->sensor_settings->ch;
 	indio_dev->num_channels = ST_SENSORS_NUMBER_ALL_CHANNELS;
 
 	gdata->current_fullscale = (struct st_sensor_fullscale_avl *)
-						&gdata->sensor->fs.fs_avl[0];
-	gdata->odr = gdata->sensor->odr.odr_avl[0].hz;
+					&gdata->sensor_settings->fs.fs_avl[0];
+	gdata->odr = gdata->sensor_settings->odr.odr_avl[0].hz;
 
-	err = st_sensors_init_sensor(indio_dev, pdata);
+	err = st_sensors_init_sensor(indio_dev,
+				(struct st_sensors_platform_data *)&gyro_pdata);
 	if (err < 0)
 		return err;
 
diff --git a/drivers/iio/gyro/st_gyro_i2c.c b/drivers/iio/gyro/st_gyro_i2c.c
index 8fa0ad2ef4ef..64480b16c689 100644
--- a/drivers/iio/gyro/st_gyro_i2c.c
+++ b/drivers/iio/gyro/st_gyro_i2c.c
@@ -67,13 +67,11 @@ static int st_gyro_i2c_probe(struct i2c_client *client,
 		return -ENOMEM;
 
 	gdata = iio_priv(indio_dev);
-	gdata->dev = &client->dev;
 	st_sensors_of_i2c_probe(client, st_gyro_of_match);
 
 	st_sensors_i2c_configure(indio_dev, client, gdata);
 
-	err = st_gyro_common_probe(indio_dev,
-				(struct st_sensors_platform_data *)&gyro_pdata);
+	err = st_gyro_common_probe(indio_dev);
 	if (err < 0)
 		return err;
 
diff --git a/drivers/iio/gyro/st_gyro_spi.c b/drivers/iio/gyro/st_gyro_spi.c
index b4ad3be26687..e59bead6bc3c 100644
--- a/drivers/iio/gyro/st_gyro_spi.c
+++ b/drivers/iio/gyro/st_gyro_spi.c
@@ -29,12 +29,10 @@ static int st_gyro_spi_probe(struct spi_device *spi)
 		return -ENOMEM;
 
 	gdata = iio_priv(indio_dev);
-	gdata->dev = &spi->dev;
 
 	st_sensors_spi_configure(indio_dev, spi, gdata);
 
-	err = st_gyro_common_probe(indio_dev,
-				(struct st_sensors_platform_data *)&gyro_pdata);
+	err = st_gyro_common_probe(indio_dev);
 	if (err < 0)
 		return err;
 
diff --git a/drivers/iio/humidity/Kconfig b/drivers/iio/humidity/Kconfig
index e116bd8dd0e4..4813b793b9f7 100644
--- a/drivers/iio/humidity/Kconfig
+++ b/drivers/iio/humidity/Kconfig
@@ -22,4 +22,14 @@ config SI7005
 	  To compile this driver as a module, choose M here: the module
 	  will be called si7005.
 
+config SI7020
+	tristate "Si7013/20/21 Relative Humidity and Temperature Sensors"
+	depends on I2C
+	help
+	  Say yes here to build support for the Silicon Labs Si7013/20/21
+	  Relative Humidity and Temperature Sensors.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called si7020.
+
 endmenu
diff --git a/drivers/iio/humidity/Makefile b/drivers/iio/humidity/Makefile
index e3f3d942e646..86e2d26e9f4d 100644
--- a/drivers/iio/humidity/Makefile
+++ b/drivers/iio/humidity/Makefile
@@ -4,3 +4,4 @@
 
 obj-$(CONFIG_DHT11) += dht11.o
 obj-$(CONFIG_SI7005) += si7005.o
+obj-$(CONFIG_SI7020) += si7020.o
diff --git a/drivers/iio/humidity/si7020.c b/drivers/iio/humidity/si7020.c
new file mode 100644
index 000000000000..b54164677b89
--- /dev/null
+++ b/drivers/iio/humidity/si7020.c
@@ -0,0 +1,161 @@
+/*
+ * si7020.c - Silicon Labs Si7013/20/21 Relative Humidity and Temp Sensors
+ * Copyright (c) 2013,2014  Uplogix, Inc.
+ * David Barksdale <dbarksdale@uplogix.com>
+ *
+ * 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.
+ */
+
+/*
+ * The Silicon Labs Si7013/20/21 Relative Humidity and Temperature Sensors
+ * are i2c devices which have an identical programming interface for
+ * measuring relative humidity and temperature. The Si7013 has an additional
+ * temperature input which this driver does not support.
+ *
+ * Data Sheets:
+ *   Si7013: http://www.silabs.com/Support%20Documents/TechnicalDocs/Si7013.pdf
+ *   Si7020: http://www.silabs.com/Support%20Documents/TechnicalDocs/Si7020.pdf
+ *   Si7021: http://www.silabs.com/Support%20Documents/TechnicalDocs/Si7021.pdf
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+/* Measure Relative Humidity, Hold Master Mode */
+#define SI7020CMD_RH_HOLD	0xE5
+/* Measure Temperature, Hold Master Mode */
+#define SI7020CMD_TEMP_HOLD	0xE3
+/* Software Reset */
+#define SI7020CMD_RESET		0xFE
+
+static int si7020_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan, int *val,
+			   int *val2, long mask)
+{
+	struct i2c_client *client = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		ret = i2c_smbus_read_word_data(client,
+					       chan->type == IIO_TEMP ?
+					       SI7020CMD_TEMP_HOLD :
+					       SI7020CMD_RH_HOLD);
+		if (ret < 0)
+			return ret;
+		*val = ret >> 2;
+		if (chan->type == IIO_HUMIDITYRELATIVE)
+			*val &= GENMASK(11, 0);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		if (chan->type == IIO_TEMP)
+			*val = 175720; /* = 175.72 * 1000 */
+		else
+			*val = 125 * 1000;
+		*val2 = 65536 >> 2;
+		return IIO_VAL_FRACTIONAL;
+	case IIO_CHAN_INFO_OFFSET:
+		/*
+		 * Since iio_convert_raw_to_processed_unlocked assumes offset
+		 * is an integer we have to round these values and lose
+		 * accuracy.
+		 * Relative humidity will be 0.0032959% too high and
+		 * temperature will be 0.00277344 degrees too high.
+		 * This is no big deal because it's within the accuracy of the
+		 * sensor.
+		 */
+		if (chan->type == IIO_TEMP)
+			*val = -4368; /* = -46.85 * (65536 >> 2) / 175.72 */
+		else
+			*val = -786; /* = -6 * (65536 >> 2) / 125 */
+		return IIO_VAL_INT;
+	default:
+		break;
+	}
+
+	return -EINVAL;
+}
+
+static const struct iio_chan_spec si7020_channels[] = {
+	{
+		.type = IIO_HUMIDITYRELATIVE,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_SCALE) | BIT(IIO_CHAN_INFO_OFFSET),
+	},
+	{
+		.type = IIO_TEMP,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_SCALE) | BIT(IIO_CHAN_INFO_OFFSET),
+	}
+};
+
+static const struct iio_info si7020_info = {
+	.read_raw = si7020_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static int si7020_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct iio_dev *indio_dev;
+	struct i2c_client **data;
+	int ret;
+
+	if (!i2c_check_functionality(client->adapter,
+				     I2C_FUNC_SMBUS_WRITE_BYTE |
+				     I2C_FUNC_SMBUS_READ_WORD_DATA))
+		return -ENODEV;
+
+	/* Reset device, loads default settings. */
+	ret = i2c_smbus_write_byte(client, SI7020CMD_RESET);
+	if (ret < 0)
+		return ret;
+	/* Wait the maximum power-up time after software reset. */
+	msleep(15);
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*client));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	data = iio_priv(indio_dev);
+	*data = client;
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->name = dev_name(&client->dev);
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->info = &si7020_info;
+	indio_dev->channels = si7020_channels;
+	indio_dev->num_channels = ARRAY_SIZE(si7020_channels);
+
+	return devm_iio_device_register(&client->dev, indio_dev);
+}
+
+static const struct i2c_device_id si7020_id[] = {
+	{ "si7020", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, si7020_id);
+
+static struct i2c_driver si7020_driver = {
+	.driver.name	= "si7020",
+	.probe		= si7020_probe,
+	.id_table	= si7020_id,
+};
+
+module_i2c_driver(si7020_driver);
+MODULE_DESCRIPTION("Silicon Labs Si7013/20/21 Relative Humidity and Temperature Sensors");
+MODULE_AUTHOR("David Barksdale <dbarksdale@uplogix.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/inkern.c b/drivers/iio/inkern.c
index f0846108d006..866fe904cba2 100644
--- a/drivers/iio/inkern.c
+++ b/drivers/iio/inkern.c
@@ -100,6 +100,28 @@ static int iio_dev_node_match(struct device *dev, void *data)
 	return dev->of_node == data && dev->type == &iio_device_type;
 }
 
+/**
+ * __of_iio_simple_xlate - translate iiospec to the IIO channel index
+ * @indio_dev:	pointer to the iio_dev structure
+ * @iiospec:	IIO specifier as found in the device tree
+ *
+ * This is simple translation function, suitable for the most 1:1 mapped
+ * channels in IIO chips. This function performs only one sanity check:
+ * whether IIO index is less than num_channels (that is specified in the
+ * iio_dev).
+ */
+static int __of_iio_simple_xlate(struct iio_dev *indio_dev,
+				const struct of_phandle_args *iiospec)
+{
+	if (!iiospec->args_count)
+		return 0;
+
+	if (iiospec->args[0] >= indio_dev->num_channels)
+		return -EINVAL;
+
+	return iiospec->args[0];
+}
+
 static int __of_iio_channel_get(struct iio_channel *channel,
 				struct device_node *np, int index)
 {
@@ -122,18 +144,19 @@ static int __of_iio_channel_get(struct iio_channel *channel,
 
 	indio_dev = dev_to_iio_dev(idev);
 	channel->indio_dev = indio_dev;
-	index = iiospec.args_count ? iiospec.args[0] : 0;
-	if (index >= indio_dev->num_channels) {
-		err = -EINVAL;
+	if (indio_dev->info->of_xlate)
+		index = indio_dev->info->of_xlate(indio_dev, &iiospec);
+	else
+		index = __of_iio_simple_xlate(indio_dev, &iiospec);
+	if (index < 0)
 		goto err_put;
-	}
 	channel->channel = &indio_dev->channels[index];
 
 	return 0;
 
 err_put:
 	iio_device_put(indio_dev);
-	return err;
+	return index;
 }
 
 static struct iio_channel *of_iio_channel_get(struct device_node *np, int index)
diff --git a/drivers/iio/magnetometer/st_magn.h b/drivers/iio/magnetometer/st_magn.h
index 694e33e0fb72..7e81d00ef0c3 100644
--- a/drivers/iio/magnetometer/st_magn.h
+++ b/drivers/iio/magnetometer/st_magn.h
@@ -18,8 +18,7 @@
 #define LSM303DLM_MAGN_DEV_NAME		"lsm303dlm_magn"
 #define LIS3MDL_MAGN_DEV_NAME		"lis3mdl"
 
-int st_magn_common_probe(struct iio_dev *indio_dev,
-					struct st_sensors_platform_data *pdata);
+int st_magn_common_probe(struct iio_dev *indio_dev);
 void st_magn_common_remove(struct iio_dev *indio_dev);
 
 #ifdef CONFIG_IIO_BUFFER
diff --git a/drivers/iio/magnetometer/st_magn_core.c b/drivers/iio/magnetometer/st_magn_core.c
index 68cae86dbd29..8ade473f99fe 100644
--- a/drivers/iio/magnetometer/st_magn_core.c
+++ b/drivers/iio/magnetometer/st_magn_core.c
@@ -149,7 +149,7 @@ static const struct iio_chan_spec st_magn_2_16bit_channels[] = {
 	IIO_CHAN_SOFT_TIMESTAMP(3)
 };
 
-static const struct st_sensors st_magn_sensors[] = {
+static const struct st_sensor_settings st_magn_sensors_settings[] = {
 	{
 		.wai = ST_MAGN_1_WAI_EXP,
 		.sensors_supported = {
@@ -361,8 +361,7 @@ static const struct iio_info magn_info = {
 	.write_raw = &st_magn_write_raw,
 };
 
-int st_magn_common_probe(struct iio_dev *indio_dev,
-					struct st_sensors_platform_data *pdata)
+int st_magn_common_probe(struct iio_dev *indio_dev)
 {
 	struct st_sensor_data *mdata = iio_priv(indio_dev);
 	int irq = mdata->get_irq_data_ready(indio_dev);
@@ -374,20 +373,21 @@ int st_magn_common_probe(struct iio_dev *indio_dev,
 	st_sensors_power_enable(indio_dev);
 
 	err = st_sensors_check_device_support(indio_dev,
-				ARRAY_SIZE(st_magn_sensors), st_magn_sensors);
+					ARRAY_SIZE(st_magn_sensors_settings),
+					st_magn_sensors_settings);
 	if (err < 0)
 		return err;
 
 	mdata->num_data_channels = ST_MAGN_NUMBER_DATA_CHANNELS;
-	mdata->multiread_bit = mdata->sensor->multi_read_bit;
-	indio_dev->channels = mdata->sensor->ch;
+	mdata->multiread_bit = mdata->sensor_settings->multi_read_bit;
+	indio_dev->channels = mdata->sensor_settings->ch;
 	indio_dev->num_channels = ST_SENSORS_NUMBER_ALL_CHANNELS;
 
 	mdata->current_fullscale = (struct st_sensor_fullscale_avl *)
-						&mdata->sensor->fs.fs_avl[0];
-	mdata->odr = mdata->sensor->odr.odr_avl[0].hz;
+					&mdata->sensor_settings->fs.fs_avl[0];
+	mdata->odr = mdata->sensor_settings->odr.odr_avl[0].hz;
 
-	err = st_sensors_init_sensor(indio_dev, pdata);
+	err = st_sensors_init_sensor(indio_dev, NULL);
 	if (err < 0)
 		return err;
 
diff --git a/drivers/iio/magnetometer/st_magn_i2c.c b/drivers/iio/magnetometer/st_magn_i2c.c
index 689250058442..92e5c15452a3 100644
--- a/drivers/iio/magnetometer/st_magn_i2c.c
+++ b/drivers/iio/magnetometer/st_magn_i2c.c
@@ -51,12 +51,11 @@ static int st_magn_i2c_probe(struct i2c_client *client,
 		return -ENOMEM;
 
 	mdata = iio_priv(indio_dev);
-	mdata->dev = &client->dev;
 	st_sensors_of_i2c_probe(client, st_magn_of_match);
 
 	st_sensors_i2c_configure(indio_dev, client, mdata);
 
-	err = st_magn_common_probe(indio_dev, NULL);
+	err = st_magn_common_probe(indio_dev);
 	if (err < 0)
 		return err;
 
diff --git a/drivers/iio/magnetometer/st_magn_spi.c b/drivers/iio/magnetometer/st_magn_spi.c
index a6143ea51dfc..7adacf160146 100644
--- a/drivers/iio/magnetometer/st_magn_spi.c
+++ b/drivers/iio/magnetometer/st_magn_spi.c
@@ -29,11 +29,10 @@ static int st_magn_spi_probe(struct spi_device *spi)
 		return -ENOMEM;
 
 	mdata = iio_priv(indio_dev);
-	mdata->dev = &spi->dev;
 
 	st_sensors_spi_configure(indio_dev, spi, mdata);
 
-	err = st_magn_common_probe(indio_dev, NULL);
+	err = st_magn_common_probe(indio_dev);
 	if (err < 0)
 		return err;
 
diff --git a/drivers/iio/pressure/Kconfig b/drivers/iio/pressure/Kconfig
index 15afbc919521..a3be53792072 100644
--- a/drivers/iio/pressure/Kconfig
+++ b/drivers/iio/pressure/Kconfig
@@ -5,6 +5,17 @@
 
 menu "Pressure sensors"
 
+config BMP280
+	tristate "Bosch Sensortec BMP280 pressure sensor driver"
+	depends on I2C
+	select REGMAP_I2C
+	help
+	 Say yes here to build support for Bosch Sensortec BMP280
+	 pressure and temperature sensor.
+
+	 To compile this driver as a module, choose M here: the module
+	 will be called bmp280.
+
 config HID_SENSOR_PRESS
 	depends on HID_SENSOR_HUB
 	select IIO_BUFFER
diff --git a/drivers/iio/pressure/Makefile b/drivers/iio/pressure/Makefile
index 90a37e85cf21..88011f2ae00e 100644
--- a/drivers/iio/pressure/Makefile
+++ b/drivers/iio/pressure/Makefile
@@ -3,6 +3,7 @@
 #
 
 # When adding new entries keep the list in alphabetical order
+obj-$(CONFIG_BMP280) += bmp280.o
 obj-$(CONFIG_HID_SENSOR_PRESS)   += hid-sensor-press.o
 obj-$(CONFIG_MPL115) += mpl115.o
 obj-$(CONFIG_MPL3115) += mpl3115.o
diff --git a/drivers/iio/pressure/bmp280.c b/drivers/iio/pressure/bmp280.c
new file mode 100644
index 000000000000..75038dacfff1
--- /dev/null
+++ b/drivers/iio/pressure/bmp280.c
@@ -0,0 +1,455 @@
+/*
+ * Copyright (c) 2014 Intel Corporation
+ *
+ * Driver for Bosch Sensortec BMP280 digital pressure sensor.
+ *
+ * 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) "bmp280: " fmt
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/acpi.h>
+#include <linux/regmap.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+#define BMP280_REG_TEMP_XLSB		0xFC
+#define BMP280_REG_TEMP_LSB		0xFB
+#define BMP280_REG_TEMP_MSB		0xFA
+#define BMP280_REG_PRESS_XLSB		0xF9
+#define BMP280_REG_PRESS_LSB		0xF8
+#define BMP280_REG_PRESS_MSB		0xF7
+
+#define BMP280_REG_CONFIG		0xF5
+#define BMP280_REG_CTRL_MEAS		0xF4
+#define BMP280_REG_STATUS		0xF3
+#define BMP280_REG_RESET		0xE0
+#define BMP280_REG_ID			0xD0
+
+#define BMP280_REG_COMP_TEMP_START	0x88
+#define BMP280_COMP_TEMP_REG_COUNT	6
+
+#define BMP280_REG_COMP_PRESS_START	0x8E
+#define BMP280_COMP_PRESS_REG_COUNT	18
+
+#define BMP280_FILTER_MASK		(BIT(4) | BIT(3) | BIT(2))
+#define BMP280_FILTER_OFF		0
+#define BMP280_FILTER_2X		BIT(2)
+#define BMP280_FILTER_4X		BIT(3)
+#define BMP280_FILTER_8X		(BIT(3) | BIT(2))
+#define BMP280_FILTER_16X		BIT(4)
+
+#define BMP280_OSRS_TEMP_MASK		(BIT(7) | BIT(6) | BIT(5))
+#define BMP280_OSRS_TEMP_SKIP		0
+#define BMP280_OSRS_TEMP_1X		BIT(5)
+#define BMP280_OSRS_TEMP_2X		BIT(6)
+#define BMP280_OSRS_TEMP_4X		(BIT(6) | BIT(5))
+#define BMP280_OSRS_TEMP_8X		BIT(7)
+#define BMP280_OSRS_TEMP_16X		(BIT(7) | BIT(5))
+
+#define BMP280_OSRS_PRESS_MASK		(BIT(4) | BIT(3) | BIT(2))
+#define BMP280_OSRS_PRESS_SKIP		0
+#define BMP280_OSRS_PRESS_1X		BIT(2)
+#define BMP280_OSRS_PRESS_2X		BIT(3)
+#define BMP280_OSRS_PRESS_4X		(BIT(3) | BIT(2))
+#define BMP280_OSRS_PRESS_8X		BIT(4)
+#define BMP280_OSRS_PRESS_16X		(BIT(4) | BIT(2))
+
+#define BMP280_MODE_MASK		(BIT(1) | BIT(0))
+#define BMP280_MODE_SLEEP		0
+#define BMP280_MODE_FORCED		BIT(0)
+#define BMP280_MODE_NORMAL		(BIT(1) | BIT(0))
+
+#define BMP280_CHIP_ID			0x58
+#define BMP280_SOFT_RESET_VAL		0xB6
+
+struct bmp280_data {
+	struct i2c_client *client;
+	struct mutex lock;
+	struct regmap *regmap;
+
+	/*
+	 * Carryover value from temperature conversion, used in pressure
+	 * calculation.
+	 */
+	s32 t_fine;
+};
+
+/* Compensation parameters. */
+struct bmp280_comp_temp {
+	u16 dig_t1;
+	s16 dig_t2, dig_t3;
+};
+
+struct bmp280_comp_press {
+	u16 dig_p1;
+	s16 dig_p2, dig_p3, dig_p4, dig_p5, dig_p6, dig_p7, dig_p8, dig_p9;
+};
+
+static const struct iio_chan_spec bmp280_channels[] = {
+	{
+		.type = IIO_PRESSURE,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED),
+	},
+	{
+		.type = IIO_TEMP,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED),
+	},
+};
+
+static bool bmp280_is_writeable_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case BMP280_REG_CONFIG:
+	case BMP280_REG_CTRL_MEAS:
+	case BMP280_REG_RESET:
+		return true;
+	default:
+		return false;
+	};
+}
+
+static bool bmp280_is_volatile_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case BMP280_REG_TEMP_XLSB:
+	case BMP280_REG_TEMP_LSB:
+	case BMP280_REG_TEMP_MSB:
+	case BMP280_REG_PRESS_XLSB:
+	case BMP280_REG_PRESS_LSB:
+	case BMP280_REG_PRESS_MSB:
+	case BMP280_REG_STATUS:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static const struct regmap_config bmp280_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.max_register = BMP280_REG_TEMP_XLSB,
+	.cache_type = REGCACHE_RBTREE,
+
+	.writeable_reg = bmp280_is_writeable_reg,
+	.volatile_reg = bmp280_is_volatile_reg,
+};
+
+static int bmp280_read_compensation_temp(struct bmp280_data *data,
+					 struct bmp280_comp_temp *comp)
+{
+	int ret;
+	__le16 buf[BMP280_COMP_TEMP_REG_COUNT / 2];
+
+	ret = regmap_bulk_read(data->regmap, BMP280_REG_COMP_TEMP_START,
+			       buf, BMP280_COMP_TEMP_REG_COUNT);
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"failed to read temperature calibration parameters\n");
+		return ret;
+	}
+
+	comp->dig_t1 = (u16) le16_to_cpu(buf[0]);
+	comp->dig_t2 = (s16) le16_to_cpu(buf[1]);
+	comp->dig_t3 = (s16) le16_to_cpu(buf[2]);
+
+	return 0;
+}
+
+static int bmp280_read_compensation_press(struct bmp280_data *data,
+					  struct bmp280_comp_press *comp)
+{
+	int ret;
+	__le16 buf[BMP280_COMP_PRESS_REG_COUNT / 2];
+
+	ret = regmap_bulk_read(data->regmap, BMP280_REG_COMP_PRESS_START,
+			       buf, BMP280_COMP_PRESS_REG_COUNT);
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"failed to read pressure calibration parameters\n");
+		return ret;
+	}
+
+	comp->dig_p1 = (u16) le16_to_cpu(buf[0]);
+	comp->dig_p2 = (s16) le16_to_cpu(buf[1]);
+	comp->dig_p3 = (s16) le16_to_cpu(buf[2]);
+	comp->dig_p4 = (s16) le16_to_cpu(buf[3]);
+	comp->dig_p5 = (s16) le16_to_cpu(buf[4]);
+	comp->dig_p6 = (s16) le16_to_cpu(buf[5]);
+	comp->dig_p7 = (s16) le16_to_cpu(buf[6]);
+	comp->dig_p8 = (s16) le16_to_cpu(buf[7]);
+	comp->dig_p9 = (s16) le16_to_cpu(buf[8]);
+
+	return 0;
+}
+
+/*
+ * Returns temperature in DegC, resolution is 0.01 DegC.  Output value of
+ * "5123" equals 51.23 DegC.  t_fine carries fine temperature as global
+ * value.
+ *
+ * Taken from datasheet, Section 3.11.3, "Compensation formula".
+ */
+static s32 bmp280_compensate_temp(struct bmp280_data *data,
+				  struct bmp280_comp_temp *comp,
+				  s32 adc_temp)
+{
+	s32 var1, var2, t;
+
+	var1 = (((adc_temp >> 3) - ((s32) comp->dig_t1 << 1)) *
+		((s32) comp->dig_t2)) >> 11;
+	var2 = (((((adc_temp >> 4) - ((s32) comp->dig_t1)) *
+		  ((adc_temp >> 4) - ((s32) comp->dig_t1))) >> 12) *
+		((s32) comp->dig_t3)) >> 14;
+
+	data->t_fine = var1 + var2;
+	t = (data->t_fine * 5 + 128) >> 8;
+
+	return t;
+}
+
+/*
+ * Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24
+ * integer bits and 8 fractional bits).  Output value of "24674867"
+ * represents 24674867/256 = 96386.2 Pa = 963.862 hPa
+ *
+ * Taken from datasheet, Section 3.11.3, "Compensation formula".
+ */
+static u32 bmp280_compensate_press(struct bmp280_data *data,
+				   struct bmp280_comp_press *comp,
+				   s32 adc_press)
+{
+	s64 var1, var2, p;
+
+	var1 = ((s64) data->t_fine) - 128000;
+	var2 = var1 * var1 * (s64) comp->dig_p6;
+	var2 = var2 + ((var1 * (s64) comp->dig_p5) << 17);
+	var2 = var2 + (((s64) comp->dig_p4) << 35);
+	var1 = ((var1 * var1 * (s64) comp->dig_p3) >> 8) +
+		((var1 * (s64) comp->dig_p2) << 12);
+	var1 = (((((s64) 1) << 47) + var1)) * ((s64) comp->dig_p1) >> 33;
+
+	if (var1 == 0)
+		return 0;
+
+	p = ((((s64) 1048576 - adc_press) << 31) - var2) * 3125;
+	p = div64_s64(p, var1);
+	var1 = (((s64) comp->dig_p9) * (p >> 13) * (p >> 13)) >> 25;
+	var2 = (((s64) comp->dig_p8) * p) >> 19;
+	p = ((p + var1 + var2) >> 8) + (((s64) comp->dig_p7) << 4);
+
+	return (u32) p;
+}
+
+static int bmp280_read_temp(struct bmp280_data *data,
+			    int *val)
+{
+	int ret;
+	__be32 tmp = 0;
+	s32 adc_temp, comp_temp;
+	struct bmp280_comp_temp comp;
+
+	ret = bmp280_read_compensation_temp(data, &comp);
+	if (ret < 0)
+		return ret;
+
+	ret = regmap_bulk_read(data->regmap, BMP280_REG_TEMP_MSB,
+			       (u8 *) &tmp, 3);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "failed to read temperature\n");
+		return ret;
+	}
+
+	adc_temp = be32_to_cpu(tmp) >> 12;
+	comp_temp = bmp280_compensate_temp(data, &comp, adc_temp);
+
+	/*
+	 * val might be NULL if we're called by the read_press routine,
+	 * who only cares about the carry over t_fine value.
+	 */
+	if (val) {
+		*val = comp_temp * 10;
+		return IIO_VAL_INT;
+	}
+
+	return 0;
+}
+
+static int bmp280_read_press(struct bmp280_data *data,
+			     int *val, int *val2)
+{
+	int ret;
+	__be32 tmp = 0;
+	s32 adc_press;
+	u32 comp_press;
+	struct bmp280_comp_press comp;
+
+	ret = bmp280_read_compensation_press(data, &comp);
+	if (ret < 0)
+		return ret;
+
+	/* Read and compensate temperature so we get a reading of t_fine. */
+	ret = bmp280_read_temp(data, NULL);
+	if (ret < 0)
+		return ret;
+
+	ret = regmap_bulk_read(data->regmap, BMP280_REG_PRESS_MSB,
+			       (u8 *) &tmp, 3);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "failed to read pressure\n");
+		return ret;
+	}
+
+	adc_press = be32_to_cpu(tmp) >> 12;
+	comp_press = bmp280_compensate_press(data, &comp, adc_press);
+
+	*val = comp_press;
+	*val2 = 256000;
+
+	return IIO_VAL_FRACTIONAL;
+}
+
+static int bmp280_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val, int *val2, long mask)
+{
+	int ret;
+	struct bmp280_data *data = iio_priv(indio_dev);
+
+	mutex_lock(&data->lock);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_PROCESSED:
+		switch (chan->type) {
+		case IIO_PRESSURE:
+			ret = bmp280_read_press(data, val, val2);
+			break;
+		case IIO_TEMP:
+			ret = bmp280_read_temp(data, val);
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	mutex_unlock(&data->lock);
+
+	return ret;
+}
+
+static const struct iio_info bmp280_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw = &bmp280_read_raw,
+};
+
+static int bmp280_chip_init(struct bmp280_data *data)
+{
+	int ret;
+
+	ret = regmap_update_bits(data->regmap, BMP280_REG_CTRL_MEAS,
+				 BMP280_OSRS_TEMP_MASK |
+				 BMP280_OSRS_PRESS_MASK |
+				 BMP280_MODE_MASK,
+				 BMP280_OSRS_TEMP_2X |
+				 BMP280_OSRS_PRESS_16X |
+				 BMP280_MODE_NORMAL);
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"failed to write config register\n");
+		return ret;
+	}
+
+	ret = regmap_update_bits(data->regmap, BMP280_REG_CONFIG,
+				 BMP280_FILTER_MASK,
+				 BMP280_FILTER_4X);
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"failed to write config register\n");
+		return ret;
+	}
+
+	return ret;
+}
+
+static int bmp280_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int ret;
+	struct iio_dev *indio_dev;
+	struct bmp280_data *data;
+	unsigned int chip_id;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	i2c_set_clientdata(client, indio_dev);
+	data = iio_priv(indio_dev);
+	mutex_init(&data->lock);
+	data->client = client;
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->name = id->name;
+	indio_dev->channels = bmp280_channels;
+	indio_dev->num_channels = ARRAY_SIZE(bmp280_channels);
+	indio_dev->info = &bmp280_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	data->regmap = devm_regmap_init_i2c(client, &bmp280_regmap_config);
+	if (IS_ERR(data->regmap)) {
+		dev_err(&client->dev, "failed to allocate register map\n");
+		return PTR_ERR(data->regmap);
+	}
+
+	ret = regmap_read(data->regmap, BMP280_REG_ID, &chip_id);
+	if (ret < 0)
+		return ret;
+	if (chip_id != BMP280_CHIP_ID) {
+		dev_err(&client->dev, "bad chip id.  expected %x got %x\n",
+			BMP280_CHIP_ID, chip_id);
+		return -EINVAL;
+	}
+
+	ret = bmp280_chip_init(data);
+	if (ret < 0)
+		return ret;
+
+	return devm_iio_device_register(&client->dev, indio_dev);
+}
+
+static const struct acpi_device_id bmp280_acpi_match[] = {
+	{"BMP0280", 0},
+	{ },
+};
+MODULE_DEVICE_TABLE(acpi, bmp280_acpi_match);
+
+static const struct i2c_device_id bmp280_id[] = {
+	{"bmp280", 0},
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, bmp280_id);
+
+static struct i2c_driver bmp280_driver = {
+	.driver = {
+		.name	= "bmp280",
+		.acpi_match_table = ACPI_PTR(bmp280_acpi_match),
+	},
+	.probe		= bmp280_probe,
+	.id_table	= bmp280_id,
+};
+module_i2c_driver(bmp280_driver);
+
+MODULE_AUTHOR("Vlad Dogaru <vlad.dogaru@intel.com>");
+MODULE_DESCRIPTION("Driver for Bosch Sensortec BMP280 pressure and temperature sensor");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/pressure/st_pressure.h b/drivers/iio/pressure/st_pressure.h
index 242943c0c4e4..f5f41490060b 100644
--- a/drivers/iio/pressure/st_pressure.h
+++ b/drivers/iio/pressure/st_pressure.h
@@ -26,8 +26,7 @@ static const struct st_sensors_platform_data default_press_pdata = {
 	.drdy_int_pin = 1,
 };
 
-int st_press_common_probe(struct iio_dev *indio_dev,
-					struct st_sensors_platform_data *pdata);
+int st_press_common_probe(struct iio_dev *indio_dev);
 void st_press_common_remove(struct iio_dev *indio_dev);
 
 #ifdef CONFIG_IIO_BUFFER
diff --git a/drivers/iio/pressure/st_pressure_buffer.c b/drivers/iio/pressure/st_pressure_buffer.c
index b37b1c9ac932..2ff53f222352 100644
--- a/drivers/iio/pressure/st_pressure_buffer.c
+++ b/drivers/iio/pressure/st_pressure_buffer.c
@@ -38,10 +38,10 @@ static int st_press_buffer_preenable(struct iio_dev *indio_dev)
 static int st_press_buffer_postenable(struct iio_dev *indio_dev)
 {
 	int err;
-	struct st_sensor_data *pdata = iio_priv(indio_dev);
+	struct st_sensor_data *press_data = iio_priv(indio_dev);
 
-	pdata->buffer_data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
-	if (pdata->buffer_data == NULL) {
+	press_data->buffer_data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
+	if (press_data->buffer_data == NULL) {
 		err = -ENOMEM;
 		goto allocate_memory_error;
 	}
@@ -53,7 +53,7 @@ static int st_press_buffer_postenable(struct iio_dev *indio_dev)
 	return err;
 
 st_press_buffer_postenable_error:
-	kfree(pdata->buffer_data);
+	kfree(press_data->buffer_data);
 allocate_memory_error:
 	return err;
 }
@@ -61,7 +61,7 @@ allocate_memory_error:
 static int st_press_buffer_predisable(struct iio_dev *indio_dev)
 {
 	int err;
-	struct st_sensor_data *pdata = iio_priv(indio_dev);
+	struct st_sensor_data *press_data = iio_priv(indio_dev);
 
 	err = iio_triggered_buffer_predisable(indio_dev);
 	if (err < 0)
@@ -70,7 +70,7 @@ static int st_press_buffer_predisable(struct iio_dev *indio_dev)
 	err = st_sensors_set_enable(indio_dev, false);
 
 st_press_buffer_predisable_error:
-	kfree(pdata->buffer_data);
+	kfree(press_data->buffer_data);
 	return err;
 }
 
diff --git a/drivers/iio/pressure/st_pressure_core.c b/drivers/iio/pressure/st_pressure_core.c
index 473d914ef470..97baf40d424b 100644
--- a/drivers/iio/pressure/st_pressure_core.c
+++ b/drivers/iio/pressure/st_pressure_core.c
@@ -175,7 +175,7 @@ static const struct iio_chan_spec st_press_lps001wp_channels[] = {
 	IIO_CHAN_SOFT_TIMESTAMP(1)
 };
 
-static const struct st_sensors st_press_sensors[] = {
+static const struct st_sensor_settings st_press_sensors_settings[] = {
 	{
 		.wai = ST_PRESS_LPS331AP_WAI_EXP,
 		.sensors_supported = {
@@ -333,7 +333,7 @@ static int st_press_read_raw(struct iio_dev *indio_dev,
 							int *val2, long mask)
 {
 	int err;
-	struct st_sensor_data *pdata = iio_priv(indio_dev);
+	struct st_sensor_data *press_data = iio_priv(indio_dev);
 
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
@@ -347,10 +347,10 @@ static int st_press_read_raw(struct iio_dev *indio_dev,
 
 		switch (ch->type) {
 		case IIO_PRESSURE:
-			*val2 = pdata->current_fullscale->gain;
+			*val2 = press_data->current_fullscale->gain;
 			break;
 		case IIO_TEMP:
-			*val2 = pdata->current_fullscale->gain2;
+			*val2 = press_data->current_fullscale->gain2;
 			break;
 		default:
 			err = -EINVAL;
@@ -371,7 +371,7 @@ static int st_press_read_raw(struct iio_dev *indio_dev,
 
 		return IIO_VAL_FRACTIONAL;
 	case IIO_CHAN_INFO_SAMP_FREQ:
-		*val = pdata->odr;
+		*val = press_data->odr;
 		return IIO_VAL_INT;
 	default:
 		return -EINVAL;
@@ -409,11 +409,10 @@ static const struct iio_trigger_ops st_press_trigger_ops = {
 #define ST_PRESS_TRIGGER_OPS NULL
 #endif
 
-int st_press_common_probe(struct iio_dev *indio_dev,
-				struct st_sensors_platform_data *plat_data)
+int st_press_common_probe(struct iio_dev *indio_dev)
 {
-	struct st_sensor_data *pdata = iio_priv(indio_dev);
-	int irq = pdata->get_irq_data_ready(indio_dev);
+	struct st_sensor_data *press_data = iio_priv(indio_dev);
+	int irq = press_data->get_irq_data_ready(indio_dev);
 	int err;
 
 	indio_dev->modes = INDIO_DIRECT_MODE;
@@ -422,28 +421,30 @@ int st_press_common_probe(struct iio_dev *indio_dev,
 	st_sensors_power_enable(indio_dev);
 
 	err = st_sensors_check_device_support(indio_dev,
-					      ARRAY_SIZE(st_press_sensors),
-					      st_press_sensors);
+					ARRAY_SIZE(st_press_sensors_settings),
+					st_press_sensors_settings);
 	if (err < 0)
 		return err;
 
-	pdata->num_data_channels = ST_PRESS_NUMBER_DATA_CHANNELS;
-	pdata->multiread_bit     = pdata->sensor->multi_read_bit;
-	indio_dev->channels      = pdata->sensor->ch;
-	indio_dev->num_channels  = pdata->sensor->num_ch;
+	press_data->num_data_channels = ST_PRESS_NUMBER_DATA_CHANNELS;
+	press_data->multiread_bit = press_data->sensor_settings->multi_read_bit;
+	indio_dev->channels = press_data->sensor_settings->ch;
+	indio_dev->num_channels = press_data->sensor_settings->num_ch;
 
-	if (pdata->sensor->fs.addr != 0)
-		pdata->current_fullscale = (struct st_sensor_fullscale_avl *)
-			&pdata->sensor->fs.fs_avl[0];
+	if (press_data->sensor_settings->fs.addr != 0)
+		press_data->current_fullscale =
+			(struct st_sensor_fullscale_avl *)
+				&press_data->sensor_settings->fs.fs_avl[0];
 
-	pdata->odr = pdata->sensor->odr.odr_avl[0].hz;
+	press_data->odr = press_data->sensor_settings->odr.odr_avl[0].hz;
 
 	/* Some devices don't support a data ready pin. */
-	if (!plat_data && pdata->sensor->drdy_irq.addr)
-		plat_data =
+	if (!press_data->dev->platform_data &&
+				press_data->sensor_settings->drdy_irq.addr)
+		press_data->dev->platform_data =
 			(struct st_sensors_platform_data *)&default_press_pdata;
 
-	err = st_sensors_init_sensor(indio_dev, plat_data);
+	err = st_sensors_init_sensor(indio_dev, press_data->dev->platform_data);
 	if (err < 0)
 		return err;
 
@@ -479,12 +480,12 @@ EXPORT_SYMBOL(st_press_common_probe);
 
 void st_press_common_remove(struct iio_dev *indio_dev)
 {
-	struct st_sensor_data *pdata = iio_priv(indio_dev);
+	struct st_sensor_data *press_data = iio_priv(indio_dev);
 
 	st_sensors_power_disable(indio_dev);
 
 	iio_device_unregister(indio_dev);
-	if (pdata->get_irq_data_ready(indio_dev) > 0)
+	if (press_data->get_irq_data_ready(indio_dev) > 0)
 		st_sensors_deallocate_trigger(indio_dev);
 
 	st_press_deallocate_ring(indio_dev);
diff --git a/drivers/iio/pressure/st_pressure_i2c.c b/drivers/iio/pressure/st_pressure_i2c.c
index acaf165260bb..137788bba4a3 100644
--- a/drivers/iio/pressure/st_pressure_i2c.c
+++ b/drivers/iio/pressure/st_pressure_i2c.c
@@ -43,20 +43,19 @@ static int st_press_i2c_probe(struct i2c_client *client,
 						const struct i2c_device_id *id)
 {
 	struct iio_dev *indio_dev;
-	struct st_sensor_data *pdata;
+	struct st_sensor_data *press_data;
 	int err;
 
-	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*pdata));
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*press_data));
 	if (!indio_dev)
 		return -ENOMEM;
 
-	pdata = iio_priv(indio_dev);
-	pdata->dev = &client->dev;
+	press_data = iio_priv(indio_dev);
 	st_sensors_of_i2c_probe(client, st_press_of_match);
 
-	st_sensors_i2c_configure(indio_dev, client, pdata);
+	st_sensors_i2c_configure(indio_dev, client, press_data);
 
-	err = st_press_common_probe(indio_dev, client->dev.platform_data);
+	err = st_press_common_probe(indio_dev);
 	if (err < 0)
 		return err;
 
diff --git a/drivers/iio/pressure/st_pressure_spi.c b/drivers/iio/pressure/st_pressure_spi.c
index f45d430ec529..1ffa6d4d349c 100644
--- a/drivers/iio/pressure/st_pressure_spi.c
+++ b/drivers/iio/pressure/st_pressure_spi.c
@@ -21,19 +21,18 @@
 static int st_press_spi_probe(struct spi_device *spi)
 {
 	struct iio_dev *indio_dev;
-	struct st_sensor_data *pdata;
+	struct st_sensor_data *press_data;
 	int err;
 
-	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*pdata));
+	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*press_data));
 	if (indio_dev == NULL)
 		return -ENOMEM;
 
-	pdata = iio_priv(indio_dev);
-	pdata->dev = &spi->dev;
+	press_data = iio_priv(indio_dev);
 
-	st_sensors_spi_configure(indio_dev, spi, pdata);
+	st_sensors_spi_configure(indio_dev, spi, press_data);
 
-	err = st_press_common_probe(indio_dev, spi->dev.platform_data);
+	err = st_press_common_probe(indio_dev);
 	if (err < 0)
 		return err;
 
diff --git a/drivers/iio/proximity/as3935.c b/drivers/iio/proximity/as3935.c
index 8349cc0fdf66..466aa4314667 100644
--- a/drivers/iio/proximity/as3935.c
+++ b/drivers/iio/proximity/as3935.c
@@ -95,7 +95,7 @@ static int as3935_read(struct as3935_state *st, unsigned int reg, int *val)
 	*val = ret;
 
 	return 0;
-};
+}
 
 static int as3935_write(struct as3935_state *st,
 				unsigned int reg,
@@ -107,7 +107,7 @@ static int as3935_write(struct as3935_state *st,
 	buf[1] = val;
 
 	return spi_write(st->spi, buf, 2);
-};
+}
 
 static ssize_t as3935_sensor_sensitivity_show(struct device *dev,
 					struct device_attribute *attr,
@@ -122,7 +122,7 @@ static ssize_t as3935_sensor_sensitivity_show(struct device *dev,
 	val = (val & AS3935_AFE_MASK) >> 1;
 
 	return sprintf(buf, "%d\n", val);
-};
+}
 
 static ssize_t as3935_sensor_sensitivity_store(struct device *dev,
 					struct device_attribute *attr,
@@ -142,7 +142,7 @@ static ssize_t as3935_sensor_sensitivity_store(struct device *dev,
 	as3935_write(st, AS3935_AFE_GAIN, val << 1);
 
 	return len;
-};
+}
 
 static IIO_DEVICE_ATTR(sensor_sensitivity, S_IRUGO | S_IWUSR,
 	as3935_sensor_sensitivity_show, as3935_sensor_sensitivity_store, 0);
@@ -214,7 +214,7 @@ err_read:
 	iio_trigger_notify_done(indio_dev->trig);
 
 	return IRQ_HANDLED;
-};
+}
 
 static const struct iio_trigger_ops iio_interrupt_trigger_ops = {
 	.owner = THIS_MODULE,
@@ -238,7 +238,7 @@ static void as3935_event_work(struct work_struct *work)
 		dev_warn(&st->spi->dev, "noise level is too high");
 		break;
 	}
-};
+}
 
 static irqreturn_t as3935_interrupt_handler(int irq, void *private)
 {
@@ -417,7 +417,7 @@ unregister_trigger:
 	iio_trigger_unregister(st->trig);
 
 	return ret;
-};
+}
 
 static int as3935_remove(struct spi_device *spi)
 {
@@ -429,7 +429,7 @@ static int as3935_remove(struct spi_device *spi)
 	iio_trigger_unregister(st->trig);
 
 	return 0;
-};
+}
 
 static const struct spi_device_id as3935_id[] = {
 	{"as3935", 0},
diff --git a/drivers/infiniband/Kconfig b/drivers/infiniband/Kconfig
index 77089399359b..b899531498eb 100644
--- a/drivers/infiniband/Kconfig
+++ b/drivers/infiniband/Kconfig
@@ -38,6 +38,17 @@ config INFINIBAND_USER_MEM
 	depends on INFINIBAND_USER_ACCESS != n
 	default y
 
+config INFINIBAND_ON_DEMAND_PAGING
+	bool "InfiniBand on-demand paging support"
+	depends on INFINIBAND_USER_MEM
+	select MMU_NOTIFIER
+	default y
+	---help---
+	  On demand paging support for the InfiniBand subsystem.
+	  Together with driver support this allows registration of
+	  memory regions without pinning their pages, fetching the
+	  pages on demand instead.
+
 config INFINIBAND_ADDR_TRANS
 	bool
 	depends on INFINIBAND
diff --git a/drivers/infiniband/core/Makefile b/drivers/infiniband/core/Makefile
index ffd0af6734af..acf736764445 100644
--- a/drivers/infiniband/core/Makefile
+++ b/drivers/infiniband/core/Makefile
@@ -11,6 +11,7 @@ obj-$(CONFIG_INFINIBAND_USER_ACCESS) +=	ib_uverbs.o ib_ucm.o \
 ib_core-y :=			packer.o ud_header.o verbs.o sysfs.o \
 				device.o fmr_pool.o cache.o netlink.o
 ib_core-$(CONFIG_INFINIBAND_USER_MEM) += umem.o
+ib_core-$(CONFIG_INFINIBAND_ON_DEMAND_PAGING) += umem_odp.o umem_rbtree.o
 
 ib_mad-y :=			mad.o smi.o agent.o mad_rmpp.o
 
diff --git a/drivers/infiniband/core/addr.c b/drivers/infiniband/core/addr.c
index 8172d37f9add..f80da50d84a5 100644
--- a/drivers/infiniband/core/addr.c
+++ b/drivers/infiniband/core/addr.c
@@ -176,8 +176,8 @@ static void set_timeout(unsigned long time)
 	unsigned long delay;
 
 	delay = time - jiffies;
-	if ((long)delay <= 0)
-		delay = 1;
+	if ((long)delay < 0)
+		delay = 0;
 
 	mod_delayed_work(addr_wq, &work, delay);
 }
diff --git a/drivers/infiniband/core/multicast.c b/drivers/infiniband/core/multicast.c
index d2360a8ef0b2..fa17b552ff78 100644
--- a/drivers/infiniband/core/multicast.c
+++ b/drivers/infiniband/core/multicast.c
@@ -525,17 +525,22 @@ static void join_handler(int status, struct ib_sa_mcmember_rec *rec,
 	if (status)
 		process_join_error(group, status);
 	else {
+		int mgids_changed, is_mgid0;
 		ib_find_pkey(group->port->dev->device, group->port->port_num,
 			     be16_to_cpu(rec->pkey), &pkey_index);
 
 		spin_lock_irq(&group->port->lock);
-		group->rec = *rec;
 		if (group->state == MCAST_BUSY &&
 		    group->pkey_index == MCAST_INVALID_PKEY_INDEX)
 			group->pkey_index = pkey_index;
-		if (!memcmp(&mgid0, &group->rec.mgid, sizeof mgid0)) {
+		mgids_changed = memcmp(&rec->mgid, &group->rec.mgid,
+				       sizeof(group->rec.mgid));
+		group->rec = *rec;
+		if (mgids_changed) {
 			rb_erase(&group->node, &group->port->table);
-			mcast_insert(group->port, group, 1);
+			is_mgid0 = !memcmp(&mgid0, &group->rec.mgid,
+					   sizeof(mgid0));
+			mcast_insert(group->port, group, is_mgid0);
 		}
 		spin_unlock_irq(&group->port->lock);
 	}
diff --git a/drivers/infiniband/core/umem.c b/drivers/infiniband/core/umem.c
index df0c4f605a21..aec7a6aa2951 100644
--- a/drivers/infiniband/core/umem.c
+++ b/drivers/infiniband/core/umem.c
@@ -39,6 +39,7 @@
 #include <linux/hugetlb.h>
 #include <linux/dma-attrs.h>
 #include <linux/slab.h>
+#include <rdma/ib_umem_odp.h>
 
 #include "uverbs.h"
 
@@ -69,6 +70,10 @@ static void __ib_umem_release(struct ib_device *dev, struct ib_umem *umem, int d
 
 /**
  * ib_umem_get - Pin and DMA map userspace memory.
+ *
+ * If access flags indicate ODP memory, avoid pinning. Instead, stores
+ * the mm for future page fault handling in conjunction with MMU notifiers.
+ *
  * @context: userspace context to pin memory for
  * @addr: userspace virtual address to start at
  * @size: length of region to pin
@@ -103,17 +108,30 @@ struct ib_umem *ib_umem_get(struct ib_ucontext *context, unsigned long addr,
 
 	umem->context   = context;
 	umem->length    = size;
-	umem->offset    = addr & ~PAGE_MASK;
+	umem->address   = addr;
 	umem->page_size = PAGE_SIZE;
 	umem->pid       = get_task_pid(current, PIDTYPE_PID);
 	/*
-	 * We ask for writable memory if any access flags other than
-	 * "remote read" are set.  "Local write" and "remote write"
+	 * We ask for writable memory if any of the following
+	 * access flags are set.  "Local write" and "remote write"
 	 * obviously require write access.  "Remote atomic" can do
 	 * things like fetch and add, which will modify memory, and
 	 * "MW bind" can change permissions by binding a window.
 	 */
-	umem->writable  = !!(access & ~IB_ACCESS_REMOTE_READ);
+	umem->writable  = !!(access &
+		(IB_ACCESS_LOCAL_WRITE   | IB_ACCESS_REMOTE_WRITE |
+		 IB_ACCESS_REMOTE_ATOMIC | IB_ACCESS_MW_BIND));
+
+	if (access & IB_ACCESS_ON_DEMAND) {
+		ret = ib_umem_odp_get(context, umem);
+		if (ret) {
+			kfree(umem);
+			return ERR_PTR(ret);
+		}
+		return umem;
+	}
+
+	umem->odp_data = NULL;
 
 	/* We assume the memory is from hugetlb until proved otherwise */
 	umem->hugetlb   = 1;
@@ -132,7 +150,7 @@ struct ib_umem *ib_umem_get(struct ib_ucontext *context, unsigned long addr,
 	if (!vma_list)
 		umem->hugetlb = 0;
 
-	npages = PAGE_ALIGN(size + umem->offset) >> PAGE_SHIFT;
+	npages = ib_umem_num_pages(umem);
 
 	down_write(&current->mm->mmap_sem);
 
@@ -235,6 +253,11 @@ void ib_umem_release(struct ib_umem *umem)
 	struct task_struct *task;
 	unsigned long diff;
 
+	if (umem->odp_data) {
+		ib_umem_odp_release(umem);
+		return;
+	}
+
 	__ib_umem_release(umem->context->device, umem, 1);
 
 	task = get_pid_task(umem->pid, PIDTYPE_PID);
@@ -246,7 +269,7 @@ void ib_umem_release(struct ib_umem *umem)
 	if (!mm)
 		goto out;
 
-	diff = PAGE_ALIGN(umem->length + umem->offset) >> PAGE_SHIFT;
+	diff = ib_umem_num_pages(umem);
 
 	/*
 	 * We may be called with the mm's mmap_sem already held.  This
@@ -283,6 +306,9 @@ int ib_umem_page_count(struct ib_umem *umem)
 	int n;
 	struct scatterlist *sg;
 
+	if (umem->odp_data)
+		return ib_umem_num_pages(umem);
+
 	shift = ilog2(umem->page_size);
 
 	n = 0;
@@ -292,3 +318,37 @@ int ib_umem_page_count(struct ib_umem *umem)
 	return n;
 }
 EXPORT_SYMBOL(ib_umem_page_count);
+
+/*
+ * Copy from the given ib_umem's pages to the given buffer.
+ *
+ * umem - the umem to copy from
+ * offset - offset to start copying from
+ * dst - destination buffer
+ * length - buffer length
+ *
+ * Returns 0 on success, or an error code.
+ */
+int ib_umem_copy_from(void *dst, struct ib_umem *umem, size_t offset,
+		      size_t length)
+{
+	size_t end = offset + length;
+	int ret;
+
+	if (offset > umem->length || length > umem->length - offset) {
+		pr_err("ib_umem_copy_from not in range. offset: %zd umem length: %zd end: %zd\n",
+		       offset, umem->length, end);
+		return -EINVAL;
+	}
+
+	ret = sg_pcopy_to_buffer(umem->sg_head.sgl, umem->nmap, dst, length,
+				 offset + ib_umem_offset(umem));
+
+	if (ret < 0)
+		return ret;
+	else if (ret != length)
+		return -EINVAL;
+	else
+		return 0;
+}
+EXPORT_SYMBOL(ib_umem_copy_from);
diff --git a/drivers/infiniband/core/umem_odp.c b/drivers/infiniband/core/umem_odp.c
new file mode 100644
index 000000000000..6095872549e7
--- /dev/null
+++ b/drivers/infiniband/core/umem_odp.c
@@ -0,0 +1,668 @@
+/*
+ * Copyright (c) 2014 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/types.h>
+#include <linux/sched.h>
+#include <linux/pid.h>
+#include <linux/slab.h>
+#include <linux/export.h>
+#include <linux/vmalloc.h>
+
+#include <rdma/ib_verbs.h>
+#include <rdma/ib_umem.h>
+#include <rdma/ib_umem_odp.h>
+
+static void ib_umem_notifier_start_account(struct ib_umem *item)
+{
+	mutex_lock(&item->odp_data->umem_mutex);
+
+	/* Only update private counters for this umem if it has them.
+	 * Otherwise skip it. All page faults will be delayed for this umem. */
+	if (item->odp_data->mn_counters_active) {
+		int notifiers_count = item->odp_data->notifiers_count++;
+
+		if (notifiers_count == 0)
+			/* Initialize the completion object for waiting on
+			 * notifiers. Since notifier_count is zero, no one
+			 * should be waiting right now. */
+			reinit_completion(&item->odp_data->notifier_completion);
+	}
+	mutex_unlock(&item->odp_data->umem_mutex);
+}
+
+static void ib_umem_notifier_end_account(struct ib_umem *item)
+{
+	mutex_lock(&item->odp_data->umem_mutex);
+
+	/* Only update private counters for this umem if it has them.
+	 * Otherwise skip it. All page faults will be delayed for this umem. */
+	if (item->odp_data->mn_counters_active) {
+		/*
+		 * This sequence increase will notify the QP page fault that
+		 * the page that is going to be mapped in the spte could have
+		 * been freed.
+		 */
+		++item->odp_data->notifiers_seq;
+		if (--item->odp_data->notifiers_count == 0)
+			complete_all(&item->odp_data->notifier_completion);
+	}
+	mutex_unlock(&item->odp_data->umem_mutex);
+}
+
+/* Account for a new mmu notifier in an ib_ucontext. */
+static void ib_ucontext_notifier_start_account(struct ib_ucontext *context)
+{
+	atomic_inc(&context->notifier_count);
+}
+
+/* Account for a terminating mmu notifier in an ib_ucontext.
+ *
+ * Must be called with the ib_ucontext->umem_rwsem semaphore unlocked, since
+ * the function takes the semaphore itself. */
+static void ib_ucontext_notifier_end_account(struct ib_ucontext *context)
+{
+	int zero_notifiers = atomic_dec_and_test(&context->notifier_count);
+
+	if (zero_notifiers &&
+	    !list_empty(&context->no_private_counters)) {
+		/* No currently running mmu notifiers. Now is the chance to
+		 * add private accounting to all previously added umems. */
+		struct ib_umem_odp *odp_data, *next;
+
+		/* Prevent concurrent mmu notifiers from working on the
+		 * no_private_counters list. */
+		down_write(&context->umem_rwsem);
+
+		/* Read the notifier_count again, with the umem_rwsem
+		 * semaphore taken for write. */
+		if (!atomic_read(&context->notifier_count)) {
+			list_for_each_entry_safe(odp_data, next,
+						 &context->no_private_counters,
+						 no_private_counters) {
+				mutex_lock(&odp_data->umem_mutex);
+				odp_data->mn_counters_active = true;
+				list_del(&odp_data->no_private_counters);
+				complete_all(&odp_data->notifier_completion);
+				mutex_unlock(&odp_data->umem_mutex);
+			}
+		}
+
+		up_write(&context->umem_rwsem);
+	}
+}
+
+static int ib_umem_notifier_release_trampoline(struct ib_umem *item, u64 start,
+					       u64 end, void *cookie) {
+	/*
+	 * Increase the number of notifiers running, to
+	 * prevent any further fault handling on this MR.
+	 */
+	ib_umem_notifier_start_account(item);
+	item->odp_data->dying = 1;
+	/* Make sure that the fact the umem is dying is out before we release
+	 * all pending page faults. */
+	smp_wmb();
+	complete_all(&item->odp_data->notifier_completion);
+	item->context->invalidate_range(item, ib_umem_start(item),
+					ib_umem_end(item));
+	return 0;
+}
+
+static void ib_umem_notifier_release(struct mmu_notifier *mn,
+				     struct mm_struct *mm)
+{
+	struct ib_ucontext *context = container_of(mn, struct ib_ucontext, mn);
+
+	if (!context->invalidate_range)
+		return;
+
+	ib_ucontext_notifier_start_account(context);
+	down_read(&context->umem_rwsem);
+	rbt_ib_umem_for_each_in_range(&context->umem_tree, 0,
+				      ULLONG_MAX,
+				      ib_umem_notifier_release_trampoline,
+				      NULL);
+	up_read(&context->umem_rwsem);
+}
+
+static int invalidate_page_trampoline(struct ib_umem *item, u64 start,
+				      u64 end, void *cookie)
+{
+	ib_umem_notifier_start_account(item);
+	item->context->invalidate_range(item, start, start + PAGE_SIZE);
+	ib_umem_notifier_end_account(item);
+	return 0;
+}
+
+static void ib_umem_notifier_invalidate_page(struct mmu_notifier *mn,
+					     struct mm_struct *mm,
+					     unsigned long address)
+{
+	struct ib_ucontext *context = container_of(mn, struct ib_ucontext, mn);
+
+	if (!context->invalidate_range)
+		return;
+
+	ib_ucontext_notifier_start_account(context);
+	down_read(&context->umem_rwsem);
+	rbt_ib_umem_for_each_in_range(&context->umem_tree, address,
+				      address + PAGE_SIZE,
+				      invalidate_page_trampoline, NULL);
+	up_read(&context->umem_rwsem);
+	ib_ucontext_notifier_end_account(context);
+}
+
+static int invalidate_range_start_trampoline(struct ib_umem *item, u64 start,
+					     u64 end, void *cookie)
+{
+	ib_umem_notifier_start_account(item);
+	item->context->invalidate_range(item, start, end);
+	return 0;
+}
+
+static void ib_umem_notifier_invalidate_range_start(struct mmu_notifier *mn,
+						    struct mm_struct *mm,
+						    unsigned long start,
+						    unsigned long end)
+{
+	struct ib_ucontext *context = container_of(mn, struct ib_ucontext, mn);
+
+	if (!context->invalidate_range)
+		return;
+
+	ib_ucontext_notifier_start_account(context);
+	down_read(&context->umem_rwsem);
+	rbt_ib_umem_for_each_in_range(&context->umem_tree, start,
+				      end,
+				      invalidate_range_start_trampoline, NULL);
+	up_read(&context->umem_rwsem);
+}
+
+static int invalidate_range_end_trampoline(struct ib_umem *item, u64 start,
+					   u64 end, void *cookie)
+{
+	ib_umem_notifier_end_account(item);
+	return 0;
+}
+
+static void ib_umem_notifier_invalidate_range_end(struct mmu_notifier *mn,
+						  struct mm_struct *mm,
+						  unsigned long start,
+						  unsigned long end)
+{
+	struct ib_ucontext *context = container_of(mn, struct ib_ucontext, mn);
+
+	if (!context->invalidate_range)
+		return;
+
+	down_read(&context->umem_rwsem);
+	rbt_ib_umem_for_each_in_range(&context->umem_tree, start,
+				      end,
+				      invalidate_range_end_trampoline, NULL);
+	up_read(&context->umem_rwsem);
+	ib_ucontext_notifier_end_account(context);
+}
+
+static struct mmu_notifier_ops ib_umem_notifiers = {
+	.release                    = ib_umem_notifier_release,
+	.invalidate_page            = ib_umem_notifier_invalidate_page,
+	.invalidate_range_start     = ib_umem_notifier_invalidate_range_start,
+	.invalidate_range_end       = ib_umem_notifier_invalidate_range_end,
+};
+
+int ib_umem_odp_get(struct ib_ucontext *context, struct ib_umem *umem)
+{
+	int ret_val;
+	struct pid *our_pid;
+	struct mm_struct *mm = get_task_mm(current);
+
+	if (!mm)
+		return -EINVAL;
+
+	/* Prevent creating ODP MRs in child processes */
+	rcu_read_lock();
+	our_pid = get_task_pid(current->group_leader, PIDTYPE_PID);
+	rcu_read_unlock();
+	put_pid(our_pid);
+	if (context->tgid != our_pid) {
+		ret_val = -EINVAL;
+		goto out_mm;
+	}
+
+	umem->hugetlb = 0;
+	umem->odp_data = kzalloc(sizeof(*umem->odp_data), GFP_KERNEL);
+	if (!umem->odp_data) {
+		ret_val = -ENOMEM;
+		goto out_mm;
+	}
+	umem->odp_data->umem = umem;
+
+	mutex_init(&umem->odp_data->umem_mutex);
+
+	init_completion(&umem->odp_data->notifier_completion);
+
+	umem->odp_data->page_list = vzalloc(ib_umem_num_pages(umem) *
+					    sizeof(*umem->odp_data->page_list));
+	if (!umem->odp_data->page_list) {
+		ret_val = -ENOMEM;
+		goto out_odp_data;
+	}
+
+	umem->odp_data->dma_list = vzalloc(ib_umem_num_pages(umem) *
+					  sizeof(*umem->odp_data->dma_list));
+	if (!umem->odp_data->dma_list) {
+		ret_val = -ENOMEM;
+		goto out_page_list;
+	}
+
+	/*
+	 * When using MMU notifiers, we will get a
+	 * notification before the "current" task (and MM) is
+	 * destroyed. We use the umem_rwsem semaphore to synchronize.
+	 */
+	down_write(&context->umem_rwsem);
+	context->odp_mrs_count++;
+	if (likely(ib_umem_start(umem) != ib_umem_end(umem)))
+		rbt_ib_umem_insert(&umem->odp_data->interval_tree,
+				   &context->umem_tree);
+	if (likely(!atomic_read(&context->notifier_count)))
+		umem->odp_data->mn_counters_active = true;
+	else
+		list_add(&umem->odp_data->no_private_counters,
+			 &context->no_private_counters);
+	downgrade_write(&context->umem_rwsem);
+
+	if (context->odp_mrs_count == 1) {
+		/*
+		 * Note that at this point, no MMU notifier is running
+		 * for this context!
+		 */
+		atomic_set(&context->notifier_count, 0);
+		INIT_HLIST_NODE(&context->mn.hlist);
+		context->mn.ops = &ib_umem_notifiers;
+		/*
+		 * Lock-dep detects a false positive for mmap_sem vs.
+		 * umem_rwsem, due to not grasping downgrade_write correctly.
+		 */
+		lockdep_off();
+		ret_val = mmu_notifier_register(&context->mn, mm);
+		lockdep_on();
+		if (ret_val) {
+			pr_err("Failed to register mmu_notifier %d\n", ret_val);
+			ret_val = -EBUSY;
+			goto out_mutex;
+		}
+	}
+
+	up_read(&context->umem_rwsem);
+
+	/*
+	 * Note that doing an mmput can cause a notifier for the relevant mm.
+	 * If the notifier is called while we hold the umem_rwsem, this will
+	 * cause a deadlock. Therefore, we release the reference only after we
+	 * released the semaphore.
+	 */
+	mmput(mm);
+	return 0;
+
+out_mutex:
+	up_read(&context->umem_rwsem);
+	vfree(umem->odp_data->dma_list);
+out_page_list:
+	vfree(umem->odp_data->page_list);
+out_odp_data:
+	kfree(umem->odp_data);
+out_mm:
+	mmput(mm);
+	return ret_val;
+}
+
+void ib_umem_odp_release(struct ib_umem *umem)
+{
+	struct ib_ucontext *context = umem->context;
+
+	/*
+	 * Ensure that no more pages are mapped in the umem.
+	 *
+	 * It is the driver's responsibility to ensure, before calling us,
+	 * that the hardware will not attempt to access the MR any more.
+	 */
+	ib_umem_odp_unmap_dma_pages(umem, ib_umem_start(umem),
+				    ib_umem_end(umem));
+
+	down_write(&context->umem_rwsem);
+	if (likely(ib_umem_start(umem) != ib_umem_end(umem)))
+		rbt_ib_umem_remove(&umem->odp_data->interval_tree,
+				   &context->umem_tree);
+	context->odp_mrs_count--;
+	if (!umem->odp_data->mn_counters_active) {
+		list_del(&umem->odp_data->no_private_counters);
+		complete_all(&umem->odp_data->notifier_completion);
+	}
+
+	/*
+	 * Downgrade the lock to a read lock. This ensures that the notifiers
+	 * (who lock the mutex for reading) will be able to finish, and we
+	 * will be able to enventually obtain the mmu notifiers SRCU. Note
+	 * that since we are doing it atomically, no other user could register
+	 * and unregister while we do the check.
+	 */
+	downgrade_write(&context->umem_rwsem);
+	if (!context->odp_mrs_count) {
+		struct task_struct *owning_process = NULL;
+		struct mm_struct *owning_mm        = NULL;
+
+		owning_process = get_pid_task(context->tgid,
+					      PIDTYPE_PID);
+		if (owning_process == NULL)
+			/*
+			 * The process is already dead, notifier were removed
+			 * already.
+			 */
+			goto out;
+
+		owning_mm = get_task_mm(owning_process);
+		if (owning_mm == NULL)
+			/*
+			 * The process' mm is already dead, notifier were
+			 * removed already.
+			 */
+			goto out_put_task;
+		mmu_notifier_unregister(&context->mn, owning_mm);
+
+		mmput(owning_mm);
+
+out_put_task:
+		put_task_struct(owning_process);
+	}
+out:
+	up_read(&context->umem_rwsem);
+
+	vfree(umem->odp_data->dma_list);
+	vfree(umem->odp_data->page_list);
+	kfree(umem->odp_data);
+	kfree(umem);
+}
+
+/*
+ * Map for DMA and insert a single page into the on-demand paging page tables.
+ *
+ * @umem: the umem to insert the page to.
+ * @page_index: index in the umem to add the page to.
+ * @page: the page struct to map and add.
+ * @access_mask: access permissions needed for this page.
+ * @current_seq: sequence number for synchronization with invalidations.
+ *               the sequence number is taken from
+ *               umem->odp_data->notifiers_seq.
+ *
+ * The function returns -EFAULT if the DMA mapping operation fails. It returns
+ * -EAGAIN if a concurrent invalidation prevents us from updating the page.
+ *
+ * The page is released via put_page even if the operation failed. For
+ * on-demand pinning, the page is released whenever it isn't stored in the
+ * umem.
+ */
+static int ib_umem_odp_map_dma_single_page(
+		struct ib_umem *umem,
+		int page_index,
+		u64 base_virt_addr,
+		struct page *page,
+		u64 access_mask,
+		unsigned long current_seq)
+{
+	struct ib_device *dev = umem->context->device;
+	dma_addr_t dma_addr;
+	int stored_page = 0;
+	int remove_existing_mapping = 0;
+	int ret = 0;
+
+	mutex_lock(&umem->odp_data->umem_mutex);
+	/*
+	 * Note: we avoid writing if seq is different from the initial seq, to
+	 * handle case of a racing notifier. This check also allows us to bail
+	 * early if we have a notifier running in parallel with us.
+	 */
+	if (ib_umem_mmu_notifier_retry(umem, current_seq)) {
+		ret = -EAGAIN;
+		goto out;
+	}
+	if (!(umem->odp_data->dma_list[page_index])) {
+		dma_addr = ib_dma_map_page(dev,
+					   page,
+					   0, PAGE_SIZE,
+					   DMA_BIDIRECTIONAL);
+		if (ib_dma_mapping_error(dev, dma_addr)) {
+			ret = -EFAULT;
+			goto out;
+		}
+		umem->odp_data->dma_list[page_index] = dma_addr | access_mask;
+		umem->odp_data->page_list[page_index] = page;
+		stored_page = 1;
+	} else if (umem->odp_data->page_list[page_index] == page) {
+		umem->odp_data->dma_list[page_index] |= access_mask;
+	} else {
+		pr_err("error: got different pages in IB device and from get_user_pages. IB device page: %p, gup page: %p\n",
+		       umem->odp_data->page_list[page_index], page);
+		/* Better remove the mapping now, to prevent any further
+		 * damage. */
+		remove_existing_mapping = 1;
+	}
+
+out:
+	mutex_unlock(&umem->odp_data->umem_mutex);
+
+	/* On Demand Paging - avoid pinning the page */
+	if (umem->context->invalidate_range || !stored_page)
+		put_page(page);
+
+	if (remove_existing_mapping && umem->context->invalidate_range) {
+		invalidate_page_trampoline(
+			umem,
+			base_virt_addr + (page_index * PAGE_SIZE),
+			base_virt_addr + ((page_index+1)*PAGE_SIZE),
+			NULL);
+		ret = -EAGAIN;
+	}
+
+	return ret;
+}
+
+/**
+ * ib_umem_odp_map_dma_pages - Pin and DMA map userspace memory in an ODP MR.
+ *
+ * Pins the range of pages passed in the argument, and maps them to
+ * DMA addresses. The DMA addresses of the mapped pages is updated in
+ * umem->odp_data->dma_list.
+ *
+ * Returns the number of pages mapped in success, negative error code
+ * for failure.
+ * An -EAGAIN error code is returned when a concurrent mmu notifier prevents
+ * the function from completing its task.
+ *
+ * @umem: the umem to map and pin
+ * @user_virt: the address from which we need to map.
+ * @bcnt: the minimal number of bytes to pin and map. The mapping might be
+ *        bigger due to alignment, and may also be smaller in case of an error
+ *        pinning or mapping a page. The actual pages mapped is returned in
+ *        the return value.
+ * @access_mask: bit mask of the requested access permissions for the given
+ *               range.
+ * @current_seq: the MMU notifiers sequance value for synchronization with
+ *               invalidations. the sequance number is read from
+ *               umem->odp_data->notifiers_seq before calling this function
+ */
+int ib_umem_odp_map_dma_pages(struct ib_umem *umem, u64 user_virt, u64 bcnt,
+			      u64 access_mask, unsigned long current_seq)
+{
+	struct task_struct *owning_process  = NULL;
+	struct mm_struct   *owning_mm       = NULL;
+	struct page       **local_page_list = NULL;
+	u64 off;
+	int j, k, ret = 0, start_idx, npages = 0;
+	u64 base_virt_addr;
+
+	if (access_mask == 0)
+		return -EINVAL;
+
+	if (user_virt < ib_umem_start(umem) ||
+	    user_virt + bcnt > ib_umem_end(umem))
+		return -EFAULT;
+
+	local_page_list = (struct page **)__get_free_page(GFP_KERNEL);
+	if (!local_page_list)
+		return -ENOMEM;
+
+	off = user_virt & (~PAGE_MASK);
+	user_virt = user_virt & PAGE_MASK;
+	base_virt_addr = user_virt;
+	bcnt += off; /* Charge for the first page offset as well. */
+
+	owning_process = get_pid_task(umem->context->tgid, PIDTYPE_PID);
+	if (owning_process == NULL) {
+		ret = -EINVAL;
+		goto out_no_task;
+	}
+
+	owning_mm = get_task_mm(owning_process);
+	if (owning_mm == NULL) {
+		ret = -EINVAL;
+		goto out_put_task;
+	}
+
+	start_idx = (user_virt - ib_umem_start(umem)) >> PAGE_SHIFT;
+	k = start_idx;
+
+	while (bcnt > 0) {
+		const size_t gup_num_pages =
+			min_t(size_t, ALIGN(bcnt, PAGE_SIZE) / PAGE_SIZE,
+			      PAGE_SIZE / sizeof(struct page *));
+
+		down_read(&owning_mm->mmap_sem);
+		/*
+		 * Note: this might result in redundent page getting. We can
+		 * avoid this by checking dma_list to be 0 before calling
+		 * get_user_pages. However, this make the code much more
+		 * complex (and doesn't gain us much performance in most use
+		 * cases).
+		 */
+		npages = get_user_pages(owning_process, owning_mm, user_virt,
+					gup_num_pages,
+					access_mask & ODP_WRITE_ALLOWED_BIT, 0,
+					local_page_list, NULL);
+		up_read(&owning_mm->mmap_sem);
+
+		if (npages < 0)
+			break;
+
+		bcnt -= min_t(size_t, npages << PAGE_SHIFT, bcnt);
+		user_virt += npages << PAGE_SHIFT;
+		for (j = 0; j < npages; ++j) {
+			ret = ib_umem_odp_map_dma_single_page(
+				umem, k, base_virt_addr, local_page_list[j],
+				access_mask, current_seq);
+			if (ret < 0)
+				break;
+			k++;
+		}
+
+		if (ret < 0) {
+			/* Release left over pages when handling errors. */
+			for (++j; j < npages; ++j)
+				put_page(local_page_list[j]);
+			break;
+		}
+	}
+
+	if (ret >= 0) {
+		if (npages < 0 && k == start_idx)
+			ret = npages;
+		else
+			ret = k - start_idx;
+	}
+
+	mmput(owning_mm);
+out_put_task:
+	put_task_struct(owning_process);
+out_no_task:
+	free_page((unsigned long)local_page_list);
+	return ret;
+}
+EXPORT_SYMBOL(ib_umem_odp_map_dma_pages);
+
+void ib_umem_odp_unmap_dma_pages(struct ib_umem *umem, u64 virt,
+				 u64 bound)
+{
+	int idx;
+	u64 addr;
+	struct ib_device *dev = umem->context->device;
+
+	virt  = max_t(u64, virt,  ib_umem_start(umem));
+	bound = min_t(u64, bound, ib_umem_end(umem));
+	/* Note that during the run of this function, the
+	 * notifiers_count of the MR is > 0, preventing any racing
+	 * faults from completion. We might be racing with other
+	 * invalidations, so we must make sure we free each page only
+	 * once. */
+	for (addr = virt; addr < bound; addr += (u64)umem->page_size) {
+		idx = (addr - ib_umem_start(umem)) / PAGE_SIZE;
+		mutex_lock(&umem->odp_data->umem_mutex);
+		if (umem->odp_data->page_list[idx]) {
+			struct page *page = umem->odp_data->page_list[idx];
+			struct page *head_page = compound_head(page);
+			dma_addr_t dma = umem->odp_data->dma_list[idx];
+			dma_addr_t dma_addr = dma & ODP_DMA_ADDR_MASK;
+
+			WARN_ON(!dma_addr);
+
+			ib_dma_unmap_page(dev, dma_addr, PAGE_SIZE,
+					  DMA_BIDIRECTIONAL);
+			if (dma & ODP_WRITE_ALLOWED_BIT)
+				/*
+				 * set_page_dirty prefers being called with
+				 * the page lock. However, MMU notifiers are
+				 * called sometimes with and sometimes without
+				 * the lock. We rely on the umem_mutex instead
+				 * to prevent other mmu notifiers from
+				 * continuing and allowing the page mapping to
+				 * be removed.
+				 */
+				set_page_dirty(head_page);
+			/* on demand pinning support */
+			if (!umem->context->invalidate_range)
+				put_page(page);
+			umem->odp_data->page_list[idx] = NULL;
+			umem->odp_data->dma_list[idx] = 0;
+		}
+		mutex_unlock(&umem->odp_data->umem_mutex);
+	}
+}
+EXPORT_SYMBOL(ib_umem_odp_unmap_dma_pages);
diff --git a/drivers/infiniband/core/umem_rbtree.c b/drivers/infiniband/core/umem_rbtree.c
new file mode 100644
index 000000000000..727d788448f5
--- /dev/null
+++ b/drivers/infiniband/core/umem_rbtree.c
@@ -0,0 +1,94 @@
+/*
+ * Copyright (c) 2014 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/kernel.h>
+#include <linux/module.h>
+#include <linux/interval_tree_generic.h>
+#include <linux/sched.h>
+#include <linux/gfp.h>
+#include <rdma/ib_umem_odp.h>
+
+/*
+ * The ib_umem list keeps track of memory regions for which the HW
+ * device request to receive notification when the related memory
+ * mapping is changed.
+ *
+ * ib_umem_lock protects the list.
+ */
+
+static inline u64 node_start(struct umem_odp_node *n)
+{
+	struct ib_umem_odp *umem_odp =
+			container_of(n, struct ib_umem_odp, interval_tree);
+
+	return ib_umem_start(umem_odp->umem);
+}
+
+/* Note that the representation of the intervals in the interval tree
+ * considers the ending point as contained in the interval, while the
+ * function ib_umem_end returns the first address which is not contained
+ * in the umem.
+ */
+static inline u64 node_last(struct umem_odp_node *n)
+{
+	struct ib_umem_odp *umem_odp =
+			container_of(n, struct ib_umem_odp, interval_tree);
+
+	return ib_umem_end(umem_odp->umem) - 1;
+}
+
+INTERVAL_TREE_DEFINE(struct umem_odp_node, rb, u64, __subtree_last,
+		     node_start, node_last, , rbt_ib_umem)
+
+/* @last is not a part of the interval. See comment for function
+ * node_last.
+ */
+int rbt_ib_umem_for_each_in_range(struct rb_root *root,
+				  u64 start, u64 last,
+				  umem_call_back cb,
+				  void *cookie)
+{
+	int ret_val = 0;
+	struct umem_odp_node *node;
+	struct ib_umem_odp *umem;
+
+	if (unlikely(start == last))
+		return ret_val;
+
+	for (node = rbt_ib_umem_iter_first(root, start, last - 1); node;
+			node = rbt_ib_umem_iter_next(node, start, last - 1)) {
+		umem = container_of(node, struct ib_umem_odp, interval_tree);
+		ret_val = cb(umem->umem, start, last, cookie) || ret_val;
+	}
+
+	return ret_val;
+}
diff --git a/drivers/infiniband/core/uverbs.h b/drivers/infiniband/core/uverbs.h
index 643c08a025a5..b716b0815644 100644
--- a/drivers/infiniband/core/uverbs.h
+++ b/drivers/infiniband/core/uverbs.h
@@ -258,5 +258,6 @@ IB_UVERBS_DECLARE_CMD(close_xrcd);
 
 IB_UVERBS_DECLARE_EX_CMD(create_flow);
 IB_UVERBS_DECLARE_EX_CMD(destroy_flow);
+IB_UVERBS_DECLARE_EX_CMD(query_device);
 
 #endif /* UVERBS_H */
diff --git a/drivers/infiniband/core/uverbs_cmd.c b/drivers/infiniband/core/uverbs_cmd.c
index 5ba2a86aab6a..532d8eba8b02 100644
--- a/drivers/infiniband/core/uverbs_cmd.c
+++ b/drivers/infiniband/core/uverbs_cmd.c
@@ -36,6 +36,7 @@
 #include <linux/file.h>
 #include <linux/fs.h>
 #include <linux/slab.h>
+#include <linux/sched.h>
 
 #include <asm/uaccess.h>
 
@@ -288,6 +289,9 @@ ssize_t ib_uverbs_get_context(struct ib_uverbs_file *file,
 	struct ib_uverbs_get_context_resp resp;
 	struct ib_udata                   udata;
 	struct ib_device                 *ibdev = file->device->ib_dev;
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+	struct ib_device_attr		  dev_attr;
+#endif
 	struct ib_ucontext		 *ucontext;
 	struct file			 *filp;
 	int ret;
@@ -325,8 +329,25 @@ ssize_t ib_uverbs_get_context(struct ib_uverbs_file *file,
 	INIT_LIST_HEAD(&ucontext->ah_list);
 	INIT_LIST_HEAD(&ucontext->xrcd_list);
 	INIT_LIST_HEAD(&ucontext->rule_list);
+	rcu_read_lock();
+	ucontext->tgid = get_task_pid(current->group_leader, PIDTYPE_PID);
+	rcu_read_unlock();
 	ucontext->closing = 0;
 
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+	ucontext->umem_tree = RB_ROOT;
+	init_rwsem(&ucontext->umem_rwsem);
+	ucontext->odp_mrs_count = 0;
+	INIT_LIST_HEAD(&ucontext->no_private_counters);
+
+	ret = ib_query_device(ibdev, &dev_attr);
+	if (ret)
+		goto err_free;
+	if (!(dev_attr.device_cap_flags & IB_DEVICE_ON_DEMAND_PAGING))
+		ucontext->invalidate_range = NULL;
+
+#endif
+
 	resp.num_comp_vectors = file->device->num_comp_vectors;
 
 	ret = get_unused_fd_flags(O_CLOEXEC);
@@ -371,6 +392,7 @@ err_fd:
 	put_unused_fd(resp.async_fd);
 
 err_free:
+	put_pid(ucontext->tgid);
 	ibdev->dealloc_ucontext(ucontext);
 
 err:
@@ -378,6 +400,52 @@ err:
 	return ret;
 }
 
+static void copy_query_dev_fields(struct ib_uverbs_file *file,
+				  struct ib_uverbs_query_device_resp *resp,
+				  struct ib_device_attr *attr)
+{
+	resp->fw_ver		= attr->fw_ver;
+	resp->node_guid		= file->device->ib_dev->node_guid;
+	resp->sys_image_guid	= attr->sys_image_guid;
+	resp->max_mr_size	= attr->max_mr_size;
+	resp->page_size_cap	= attr->page_size_cap;
+	resp->vendor_id		= attr->vendor_id;
+	resp->vendor_part_id	= attr->vendor_part_id;
+	resp->hw_ver		= attr->hw_ver;
+	resp->max_qp		= attr->max_qp;
+	resp->max_qp_wr		= attr->max_qp_wr;
+	resp->device_cap_flags	= attr->device_cap_flags;
+	resp->max_sge		= attr->max_sge;
+	resp->max_sge_rd	= attr->max_sge_rd;
+	resp->max_cq		= attr->max_cq;
+	resp->max_cqe		= attr->max_cqe;
+	resp->max_mr		= attr->max_mr;
+	resp->max_pd		= attr->max_pd;
+	resp->max_qp_rd_atom	= attr->max_qp_rd_atom;
+	resp->max_ee_rd_atom	= attr->max_ee_rd_atom;
+	resp->max_res_rd_atom	= attr->max_res_rd_atom;
+	resp->max_qp_init_rd_atom	= attr->max_qp_init_rd_atom;
+	resp->max_ee_init_rd_atom	= attr->max_ee_init_rd_atom;
+	resp->atomic_cap		= attr->atomic_cap;
+	resp->max_ee			= attr->max_ee;
+	resp->max_rdd			= attr->max_rdd;
+	resp->max_mw			= attr->max_mw;
+	resp->max_raw_ipv6_qp		= attr->max_raw_ipv6_qp;
+	resp->max_raw_ethy_qp		= attr->max_raw_ethy_qp;
+	resp->max_mcast_grp		= attr->max_mcast_grp;
+	resp->max_mcast_qp_attach	= attr->max_mcast_qp_attach;
+	resp->max_total_mcast_qp_attach	= attr->max_total_mcast_qp_attach;
+	resp->max_ah			= attr->max_ah;
+	resp->max_fmr			= attr->max_fmr;
+	resp->max_map_per_fmr		= attr->max_map_per_fmr;
+	resp->max_srq			= attr->max_srq;
+	resp->max_srq_wr		= attr->max_srq_wr;
+	resp->max_srq_sge		= attr->max_srq_sge;
+	resp->max_pkeys			= attr->max_pkeys;
+	resp->local_ca_ack_delay	= attr->local_ca_ack_delay;
+	resp->phys_port_cnt		= file->device->ib_dev->phys_port_cnt;
+}
+
 ssize_t ib_uverbs_query_device(struct ib_uverbs_file *file,
 			       const char __user *buf,
 			       int in_len, int out_len)
@@ -398,47 +466,7 @@ ssize_t ib_uverbs_query_device(struct ib_uverbs_file *file,
 		return ret;
 
 	memset(&resp, 0, sizeof resp);
-
-	resp.fw_ver 		       = attr.fw_ver;
-	resp.node_guid 		       = file->device->ib_dev->node_guid;
-	resp.sys_image_guid 	       = attr.sys_image_guid;
-	resp.max_mr_size 	       = attr.max_mr_size;
-	resp.page_size_cap 	       = attr.page_size_cap;
-	resp.vendor_id 		       = attr.vendor_id;
-	resp.vendor_part_id 	       = attr.vendor_part_id;
-	resp.hw_ver 		       = attr.hw_ver;
-	resp.max_qp 		       = attr.max_qp;
-	resp.max_qp_wr 		       = attr.max_qp_wr;
-	resp.device_cap_flags 	       = attr.device_cap_flags;
-	resp.max_sge 		       = attr.max_sge;
-	resp.max_sge_rd 	       = attr.max_sge_rd;
-	resp.max_cq 		       = attr.max_cq;
-	resp.max_cqe 		       = attr.max_cqe;
-	resp.max_mr 		       = attr.max_mr;
-	resp.max_pd 		       = attr.max_pd;
-	resp.max_qp_rd_atom 	       = attr.max_qp_rd_atom;
-	resp.max_ee_rd_atom 	       = attr.max_ee_rd_atom;
-	resp.max_res_rd_atom 	       = attr.max_res_rd_atom;
-	resp.max_qp_init_rd_atom       = attr.max_qp_init_rd_atom;
-	resp.max_ee_init_rd_atom       = attr.max_ee_init_rd_atom;
-	resp.atomic_cap 	       = attr.atomic_cap;
-	resp.max_ee 		       = attr.max_ee;
-	resp.max_rdd 		       = attr.max_rdd;
-	resp.max_mw 		       = attr.max_mw;
-	resp.max_raw_ipv6_qp 	       = attr.max_raw_ipv6_qp;
-	resp.max_raw_ethy_qp 	       = attr.max_raw_ethy_qp;
-	resp.max_mcast_grp 	       = attr.max_mcast_grp;
-	resp.max_mcast_qp_attach       = attr.max_mcast_qp_attach;
-	resp.max_total_mcast_qp_attach = attr.max_total_mcast_qp_attach;
-	resp.max_ah 		       = attr.max_ah;
-	resp.max_fmr 		       = attr.max_fmr;
-	resp.max_map_per_fmr 	       = attr.max_map_per_fmr;
-	resp.max_srq 		       = attr.max_srq;
-	resp.max_srq_wr 	       = attr.max_srq_wr;
-	resp.max_srq_sge 	       = attr.max_srq_sge;
-	resp.max_pkeys 		       = attr.max_pkeys;
-	resp.local_ca_ack_delay        = attr.local_ca_ack_delay;
-	resp.phys_port_cnt	       = file->device->ib_dev->phys_port_cnt;
+	copy_query_dev_fields(file, &resp, &attr);
 
 	if (copy_to_user((void __user *) (unsigned long) cmd.response,
 			 &resp, sizeof resp))
@@ -947,6 +975,18 @@ ssize_t ib_uverbs_reg_mr(struct ib_uverbs_file *file,
 		goto err_free;
 	}
 
+	if (cmd.access_flags & IB_ACCESS_ON_DEMAND) {
+		struct ib_device_attr attr;
+
+		ret = ib_query_device(pd->device, &attr);
+		if (ret || !(attr.device_cap_flags &
+				IB_DEVICE_ON_DEMAND_PAGING)) {
+			pr_debug("ODP support not available\n");
+			ret = -EINVAL;
+			goto err_put;
+		}
+	}
+
 	mr = pd->device->reg_user_mr(pd, cmd.start, cmd.length, cmd.hca_va,
 				     cmd.access_flags, &udata);
 	if (IS_ERR(mr)) {
@@ -3253,3 +3293,52 @@ ssize_t ib_uverbs_destroy_srq(struct ib_uverbs_file *file,
 
 	return ret ? ret : in_len;
 }
+
+int ib_uverbs_ex_query_device(struct ib_uverbs_file *file,
+			      struct ib_udata *ucore,
+			      struct ib_udata *uhw)
+{
+	struct ib_uverbs_ex_query_device_resp resp;
+	struct ib_uverbs_ex_query_device  cmd;
+	struct ib_device_attr attr;
+	struct ib_device *device;
+	int err;
+
+	device = file->device->ib_dev;
+	if (ucore->inlen < sizeof(cmd))
+		return -EINVAL;
+
+	err = ib_copy_from_udata(&cmd, ucore, sizeof(cmd));
+	if (err)
+		return err;
+
+	if (cmd.reserved)
+		return -EINVAL;
+
+	err = device->query_device(device, &attr);
+	if (err)
+		return err;
+
+	memset(&resp, 0, sizeof(resp));
+	copy_query_dev_fields(file, &resp.base, &attr);
+	resp.comp_mask = 0;
+
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+	if (cmd.comp_mask & IB_USER_VERBS_EX_QUERY_DEVICE_ODP) {
+		resp.odp_caps.general_caps = attr.odp_caps.general_caps;
+		resp.odp_caps.per_transport_caps.rc_odp_caps =
+			attr.odp_caps.per_transport_caps.rc_odp_caps;
+		resp.odp_caps.per_transport_caps.uc_odp_caps =
+			attr.odp_caps.per_transport_caps.uc_odp_caps;
+		resp.odp_caps.per_transport_caps.ud_odp_caps =
+			attr.odp_caps.per_transport_caps.ud_odp_caps;
+		resp.comp_mask |= IB_USER_VERBS_EX_QUERY_DEVICE_ODP;
+	}
+#endif
+
+	err = ib_copy_to_udata(ucore, &resp, sizeof(resp));
+	if (err)
+		return err;
+
+	return 0;
+}
diff --git a/drivers/infiniband/core/uverbs_main.c b/drivers/infiniband/core/uverbs_main.c
index 71ab83fde472..e6c23b9eab33 100644
--- a/drivers/infiniband/core/uverbs_main.c
+++ b/drivers/infiniband/core/uverbs_main.c
@@ -122,7 +122,8 @@ static int (*uverbs_ex_cmd_table[])(struct ib_uverbs_file *file,
 				    struct ib_udata *ucore,
 				    struct ib_udata *uhw) = {
 	[IB_USER_VERBS_EX_CMD_CREATE_FLOW]	= ib_uverbs_ex_create_flow,
-	[IB_USER_VERBS_EX_CMD_DESTROY_FLOW]	= ib_uverbs_ex_destroy_flow
+	[IB_USER_VERBS_EX_CMD_DESTROY_FLOW]	= ib_uverbs_ex_destroy_flow,
+	[IB_USER_VERBS_EX_CMD_QUERY_DEVICE]	= ib_uverbs_ex_query_device
 };
 
 static void ib_uverbs_add_one(struct ib_device *device);
@@ -296,6 +297,8 @@ static int ib_uverbs_cleanup_ucontext(struct ib_uverbs_file *file,
 		kfree(uobj);
 	}
 
+	put_pid(context->tgid);
+
 	return context->device->dealloc_ucontext(context);
 }
 
diff --git a/drivers/infiniband/core/verbs.c b/drivers/infiniband/core/verbs.c
index c2b89cc5dbca..f93eb8da7b5a 100644
--- a/drivers/infiniband/core/verbs.c
+++ b/drivers/infiniband/core/verbs.c
@@ -879,7 +879,8 @@ int ib_resolve_eth_l2_attrs(struct ib_qp *qp,
 		if (rdma_link_local_addr((struct in6_addr *)qp_attr->ah_attr.grh.dgid.raw)) {
 			rdma_get_ll_mac((struct in6_addr *)qp_attr->ah_attr.grh.dgid.raw, qp_attr->ah_attr.dmac);
 			rdma_get_ll_mac((struct in6_addr *)sgid.raw, qp_attr->smac);
-			qp_attr->vlan_id = rdma_get_vlan_id(&sgid);
+			if (!(*qp_attr_mask & IB_QP_VID))
+				qp_attr->vlan_id = rdma_get_vlan_id(&sgid);
 		} else {
 			ret = rdma_addr_find_dmac_by_grh(&sgid, &qp_attr->ah_attr.grh.dgid,
 					qp_attr->ah_attr.dmac, &qp_attr->vlan_id);
diff --git a/drivers/infiniband/hw/amso1100/c2_provider.c b/drivers/infiniband/hw/amso1100/c2_provider.c
index 2d5cbf4363e4..bdf3507810cb 100644
--- a/drivers/infiniband/hw/amso1100/c2_provider.c
+++ b/drivers/infiniband/hw/amso1100/c2_provider.c
@@ -476,7 +476,7 @@ static struct ib_mr *c2_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
 					 c2mr->umem->page_size,
 					 i,
 					 length,
-					 c2mr->umem->offset,
+					 ib_umem_offset(c2mr->umem),
 					 &kva,
 					 c2_convert_access(acc),
 					 c2mr);
diff --git a/drivers/infiniband/hw/cxgb4/cm.c b/drivers/infiniband/hw/cxgb4/cm.c
index 4b8c6116c058..9edc200b311d 100644
--- a/drivers/infiniband/hw/cxgb4/cm.c
+++ b/drivers/infiniband/hw/cxgb4/cm.c
@@ -1640,7 +1640,8 @@ static void process_mpa_request(struct c4iw_ep *ep, struct sk_buff *skb)
 		__state_set(&ep->com, MPA_REQ_RCVD);
 
 		/* drive upcall */
-		mutex_lock(&ep->parent_ep->com.mutex);
+		mutex_lock_nested(&ep->parent_ep->com.mutex,
+				  SINGLE_DEPTH_NESTING);
 		if (ep->parent_ep->com.state != DEAD) {
 			if (connect_request_upcall(ep))
 				abort_connection(ep, skb, GFP_KERNEL);
@@ -3126,6 +3127,8 @@ static int create_server6(struct c4iw_dev *dev, struct c4iw_listen_ep *ep)
 		err = c4iw_wait_for_reply(&ep->com.dev->rdev,
 					  &ep->com.wr_wait,
 					  0, 0, __func__);
+	else if (err > 0)
+		err = net_xmit_errno(err);
 	if (err)
 		pr_err("cxgb4_create_server6/filter failed err %d stid %d laddr %pI6 lport %d\n",
 		       err, ep->stid,
@@ -3159,6 +3162,8 @@ static int create_server4(struct c4iw_dev *dev, struct c4iw_listen_ep *ep)
 			err = c4iw_wait_for_reply(&ep->com.dev->rdev,
 						  &ep->com.wr_wait,
 						  0, 0, __func__);
+		else if (err > 0)
+			err = net_xmit_errno(err);
 	}
 	if (err)
 		pr_err("cxgb4_create_server/filter failed err %d stid %d laddr %pI4 lport %d\n"
diff --git a/drivers/infiniband/hw/cxgb4/device.c b/drivers/infiniband/hw/cxgb4/device.c
index 72f1f052e88c..eb5df4e62703 100644
--- a/drivers/infiniband/hw/cxgb4/device.c
+++ b/drivers/infiniband/hw/cxgb4/device.c
@@ -670,7 +670,7 @@ static int ep_open(struct inode *inode, struct file *file)
 	idr_for_each(&epd->devp->stid_idr, count_idrs, &count);
 	spin_unlock_irq(&epd->devp->lock);
 
-	epd->bufsize = count * 160;
+	epd->bufsize = count * 240;
 	epd->buf = vmalloc(epd->bufsize);
 	if (!epd->buf) {
 		ret = -ENOMEM;
diff --git a/drivers/infiniband/hw/cxgb4/mem.c b/drivers/infiniband/hw/cxgb4/mem.c
index 0744455cd88b..cb43c2299ac0 100644
--- a/drivers/infiniband/hw/cxgb4/mem.c
+++ b/drivers/infiniband/hw/cxgb4/mem.c
@@ -50,6 +50,13 @@ static int inline_threshold = C4IW_INLINE_THRESHOLD;
 module_param(inline_threshold, int, 0644);
 MODULE_PARM_DESC(inline_threshold, "inline vs dsgl threshold (default=128)");
 
+static int mr_exceeds_hw_limits(struct c4iw_dev *dev, u64 length)
+{
+	return (is_t4(dev->rdev.lldi.adapter_type) ||
+		is_t5(dev->rdev.lldi.adapter_type)) &&
+		length >= 8*1024*1024*1024ULL;
+}
+
 static int _c4iw_write_mem_dma_aligned(struct c4iw_rdev *rdev, u32 addr,
 				       u32 len, dma_addr_t data, int wait)
 {
@@ -369,9 +376,11 @@ static int register_mem(struct c4iw_dev *rhp, struct c4iw_pd *php,
 	int ret;
 
 	ret = write_tpt_entry(&rhp->rdev, 0, &stag, 1, mhp->attr.pdid,
-			      FW_RI_STAG_NSMR, mhp->attr.perms,
+			      FW_RI_STAG_NSMR, mhp->attr.len ?
+			      mhp->attr.perms : 0,
 			      mhp->attr.mw_bind_enable, mhp->attr.zbva,
-			      mhp->attr.va_fbo, mhp->attr.len, shift - 12,
+			      mhp->attr.va_fbo, mhp->attr.len ?
+			      mhp->attr.len : -1, shift - 12,
 			      mhp->attr.pbl_size, mhp->attr.pbl_addr);
 	if (ret)
 		return ret;
@@ -536,6 +545,11 @@ int c4iw_reregister_phys_mem(struct ib_mr *mr, int mr_rereg_mask,
 			return ret;
 	}
 
+	if (mr_exceeds_hw_limits(rhp, total_size)) {
+		kfree(page_list);
+		return -EINVAL;
+	}
+
 	ret = reregister_mem(rhp, php, &mh, shift, npages);
 	kfree(page_list);
 	if (ret)
@@ -596,6 +610,12 @@ struct ib_mr *c4iw_register_phys_mem(struct ib_pd *pd,
 	if (ret)
 		goto err;
 
+	if (mr_exceeds_hw_limits(rhp, total_size)) {
+		kfree(page_list);
+		ret = -EINVAL;
+		goto err;
+	}
+
 	ret = alloc_pbl(mhp, npages);
 	if (ret) {
 		kfree(page_list);
@@ -699,6 +719,10 @@ struct ib_mr *c4iw_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
 
 	php = to_c4iw_pd(pd);
 	rhp = php->rhp;
+
+	if (mr_exceeds_hw_limits(rhp, length))
+		return ERR_PTR(-EINVAL);
+
 	mhp = kzalloc(sizeof(*mhp), GFP_KERNEL);
 	if (!mhp)
 		return ERR_PTR(-ENOMEM);
diff --git a/drivers/infiniband/hw/cxgb4/qp.c b/drivers/infiniband/hw/cxgb4/qp.c
index 2ed3ece2b2ee..bb85d479e66e 100644
--- a/drivers/infiniband/hw/cxgb4/qp.c
+++ b/drivers/infiniband/hw/cxgb4/qp.c
@@ -1538,9 +1538,9 @@ err:
 	set_state(qhp, C4IW_QP_STATE_ERROR);
 	free = 1;
 	abort = 1;
-	wake_up(&qhp->wait);
 	BUG_ON(!ep);
 	flush_qp(qhp);
+	wake_up(&qhp->wait);
 out:
 	mutex_unlock(&qhp->mutex);
 
diff --git a/drivers/infiniband/hw/ehca/ehca_mrmw.c b/drivers/infiniband/hw/ehca/ehca_mrmw.c
index 3488e8c9fcb4..f914b30999f8 100644
--- a/drivers/infiniband/hw/ehca/ehca_mrmw.c
+++ b/drivers/infiniband/hw/ehca/ehca_mrmw.c
@@ -399,7 +399,7 @@ reg_user_mr_fallback:
 	pginfo.num_kpages = num_kpages;
 	pginfo.num_hwpages = num_hwpages;
 	pginfo.u.usr.region = e_mr->umem;
-	pginfo.next_hwpage = e_mr->umem->offset / hwpage_size;
+	pginfo.next_hwpage = ib_umem_offset(e_mr->umem) / hwpage_size;
 	pginfo.u.usr.next_sg = pginfo.u.usr.region->sg_head.sgl;
 	ret = ehca_reg_mr(shca, e_mr, (u64 *)virt, length, mr_access_flags,
 			  e_pd, &pginfo, &e_mr->ib.ib_mr.lkey,
diff --git a/drivers/infiniband/hw/ipath/ipath_mr.c b/drivers/infiniband/hw/ipath/ipath_mr.c
index 5e61e9bff697..c7278f6a8217 100644
--- a/drivers/infiniband/hw/ipath/ipath_mr.c
+++ b/drivers/infiniband/hw/ipath/ipath_mr.c
@@ -214,7 +214,7 @@ struct ib_mr *ipath_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
 	mr->mr.user_base = start;
 	mr->mr.iova = virt_addr;
 	mr->mr.length = length;
-	mr->mr.offset = umem->offset;
+	mr->mr.offset = ib_umem_offset(umem);
 	mr->mr.access_flags = mr_access_flags;
 	mr->mr.max_segs = n;
 	mr->umem = umem;
diff --git a/drivers/infiniband/hw/mlx4/mr.c b/drivers/infiniband/hw/mlx4/mr.c
index 8f9325cfc85d..c36ccbd9a644 100644
--- a/drivers/infiniband/hw/mlx4/mr.c
+++ b/drivers/infiniband/hw/mlx4/mr.c
@@ -223,7 +223,6 @@ int mlx4_ib_rereg_user_mr(struct ib_mr *mr, int flags,
 
 	if (flags & IB_MR_REREG_TRANS) {
 		int shift;
-		int err;
 		int n;
 
 		mlx4_mr_rereg_mem_cleanup(dev->dev, &mmr->mmr);
diff --git a/drivers/infiniband/hw/mlx5/Makefile b/drivers/infiniband/hw/mlx5/Makefile
index 4ea0135af484..27a70159e2ea 100644
--- a/drivers/infiniband/hw/mlx5/Makefile
+++ b/drivers/infiniband/hw/mlx5/Makefile
@@ -1,3 +1,4 @@
 obj-$(CONFIG_MLX5_INFINIBAND)	+= mlx5_ib.o
 
 mlx5_ib-y :=	main.o cq.o doorbell.o qp.o mem.o srq.o mr.o ah.o mad.o
+mlx5_ib-$(CONFIG_INFINIBAND_ON_DEMAND_PAGING) += odp.o
diff --git a/drivers/infiniband/hw/mlx5/main.c b/drivers/infiniband/hw/mlx5/main.c
index 1ba6c42e4df8..8a87404e9c76 100644
--- a/drivers/infiniband/hw/mlx5/main.c
+++ b/drivers/infiniband/hw/mlx5/main.c
@@ -244,6 +244,12 @@ static int mlx5_ib_query_device(struct ib_device *ibdev,
 					   props->max_mcast_grp;
 	props->max_map_per_fmr = INT_MAX; /* no limit in ConnectIB */
 
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+	if (dev->mdev->caps.gen.flags & MLX5_DEV_CAP_FLAG_ON_DMND_PG)
+		props->device_cap_flags |= IB_DEVICE_ON_DEMAND_PAGING;
+	props->odp_caps = dev->odp_caps;
+#endif
+
 out:
 	kfree(in_mad);
 	kfree(out_mad);
@@ -568,6 +574,10 @@ static struct ib_ucontext *mlx5_ib_alloc_ucontext(struct ib_device *ibdev,
 			goto out_count;
 	}
 
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+	context->ibucontext.invalidate_range = &mlx5_ib_invalidate_range;
+#endif
+
 	INIT_LIST_HEAD(&context->db_page_list);
 	mutex_init(&context->db_page_mutex);
 
@@ -858,7 +868,7 @@ static ssize_t show_reg_pages(struct device *device,
 	struct mlx5_ib_dev *dev =
 		container_of(device, struct mlx5_ib_dev, ib_dev.dev);
 
-	return sprintf(buf, "%d\n", dev->mdev->priv.reg_pages);
+	return sprintf(buf, "%d\n", atomic_read(&dev->mdev->priv.reg_pages));
 }
 
 static ssize_t show_hca(struct device *device, struct device_attribute *attr,
@@ -1321,6 +1331,8 @@ static void *mlx5_ib_add(struct mlx5_core_dev *mdev)
 		(1ull << IB_USER_VERBS_CMD_DESTROY_SRQ)		|
 		(1ull << IB_USER_VERBS_CMD_CREATE_XSRQ)		|
 		(1ull << IB_USER_VERBS_CMD_OPEN_QP);
+	dev->ib_dev.uverbs_ex_cmd_mask =
+		(1ull << IB_USER_VERBS_EX_CMD_QUERY_DEVICE);
 
 	dev->ib_dev.query_device	= mlx5_ib_query_device;
 	dev->ib_dev.query_port		= mlx5_ib_query_port;
@@ -1366,6 +1378,8 @@ static void *mlx5_ib_add(struct mlx5_core_dev *mdev)
 	dev->ib_dev.free_fast_reg_page_list  = mlx5_ib_free_fast_reg_page_list;
 	dev->ib_dev.check_mr_status	= mlx5_ib_check_mr_status;
 
+	mlx5_ib_internal_query_odp_caps(dev);
+
 	if (mdev->caps.gen.flags & MLX5_DEV_CAP_FLAG_XRC) {
 		dev->ib_dev.alloc_xrcd = mlx5_ib_alloc_xrcd;
 		dev->ib_dev.dealloc_xrcd = mlx5_ib_dealloc_xrcd;
@@ -1379,16 +1393,19 @@ static void *mlx5_ib_add(struct mlx5_core_dev *mdev)
 		goto err_eqs;
 
 	mutex_init(&dev->cap_mask_mutex);
-	spin_lock_init(&dev->mr_lock);
 
 	err = create_dev_resources(&dev->devr);
 	if (err)
 		goto err_eqs;
 
-	err = ib_register_device(&dev->ib_dev, NULL);
+	err = mlx5_ib_odp_init_one(dev);
 	if (err)
 		goto err_rsrc;
 
+	err = ib_register_device(&dev->ib_dev, NULL);
+	if (err)
+		goto err_odp;
+
 	err = create_umr_res(dev);
 	if (err)
 		goto err_dev;
@@ -1410,6 +1427,9 @@ err_umrc:
 err_dev:
 	ib_unregister_device(&dev->ib_dev);
 
+err_odp:
+	mlx5_ib_odp_remove_one(dev);
+
 err_rsrc:
 	destroy_dev_resources(&dev->devr);
 
@@ -1425,8 +1445,10 @@ err_dealloc:
 static void mlx5_ib_remove(struct mlx5_core_dev *mdev, void *context)
 {
 	struct mlx5_ib_dev *dev = context;
+
 	ib_unregister_device(&dev->ib_dev);
 	destroy_umrc_res(dev);
+	mlx5_ib_odp_remove_one(dev);
 	destroy_dev_resources(&dev->devr);
 	free_comp_eqs(dev);
 	ib_dealloc_device(&dev->ib_dev);
@@ -1440,15 +1462,30 @@ static struct mlx5_interface mlx5_ib_interface = {
 
 static int __init mlx5_ib_init(void)
 {
+	int err;
+
 	if (deprecated_prof_sel != 2)
 		pr_warn("prof_sel is deprecated for mlx5_ib, set it for mlx5_core\n");
 
-	return mlx5_register_interface(&mlx5_ib_interface);
+	err = mlx5_ib_odp_init();
+	if (err)
+		return err;
+
+	err = mlx5_register_interface(&mlx5_ib_interface);
+	if (err)
+		goto clean_odp;
+
+	return err;
+
+clean_odp:
+	mlx5_ib_odp_cleanup();
+	return err;
 }
 
 static void __exit mlx5_ib_cleanup(void)
 {
 	mlx5_unregister_interface(&mlx5_ib_interface);
+	mlx5_ib_odp_cleanup();
 }
 
 module_init(mlx5_ib_init);
diff --git a/drivers/infiniband/hw/mlx5/mem.c b/drivers/infiniband/hw/mlx5/mem.c
index dae07eae9507..b56e4c5593ee 100644
--- a/drivers/infiniband/hw/mlx5/mem.c
+++ b/drivers/infiniband/hw/mlx5/mem.c
@@ -32,6 +32,7 @@
 
 #include <linux/module.h>
 #include <rdma/ib_umem.h>
+#include <rdma/ib_umem_odp.h>
 #include "mlx5_ib.h"
 
 /* @umem: umem object to scan
@@ -57,6 +58,17 @@ void mlx5_ib_cont_pages(struct ib_umem *umem, u64 addr, int *count, int *shift,
 	int entry;
 	unsigned long page_shift = ilog2(umem->page_size);
 
+	/* With ODP we must always match OS page size. */
+	if (umem->odp_data) {
+		*count = ib_umem_page_count(umem);
+		*shift = PAGE_SHIFT;
+		*ncont = *count;
+		if (order)
+			*order = ilog2(roundup_pow_of_two(*count));
+
+		return;
+	}
+
 	addr = addr >> page_shift;
 	tmp = (unsigned long)addr;
 	m = find_first_bit(&tmp, sizeof(tmp));
@@ -108,8 +120,36 @@ void mlx5_ib_cont_pages(struct ib_umem *umem, u64 addr, int *count, int *shift,
 	*count = i;
 }
 
-void mlx5_ib_populate_pas(struct mlx5_ib_dev *dev, struct ib_umem *umem,
-			  int page_shift, __be64 *pas, int umr)
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+static u64 umem_dma_to_mtt(dma_addr_t umem_dma)
+{
+	u64 mtt_entry = umem_dma & ODP_DMA_ADDR_MASK;
+
+	if (umem_dma & ODP_READ_ALLOWED_BIT)
+		mtt_entry |= MLX5_IB_MTT_READ;
+	if (umem_dma & ODP_WRITE_ALLOWED_BIT)
+		mtt_entry |= MLX5_IB_MTT_WRITE;
+
+	return mtt_entry;
+}
+#endif
+
+/*
+ * Populate the given array with bus addresses from the umem.
+ *
+ * dev - mlx5_ib device
+ * umem - umem to use to fill the pages
+ * page_shift - determines the page size used in the resulting array
+ * offset - offset into the umem to start from,
+ *          only implemented for ODP umems
+ * num_pages - total number of pages to fill
+ * pas - bus addresses array to fill
+ * access_flags - access flags to set on all present pages.
+		  use enum mlx5_ib_mtt_access_flags for this.
+ */
+void __mlx5_ib_populate_pas(struct mlx5_ib_dev *dev, struct ib_umem *umem,
+			    int page_shift, size_t offset, size_t num_pages,
+			    __be64 *pas, int access_flags)
 {
 	unsigned long umem_page_shift = ilog2(umem->page_size);
 	int shift = page_shift - umem_page_shift;
@@ -120,6 +160,21 @@ void mlx5_ib_populate_pas(struct mlx5_ib_dev *dev, struct ib_umem *umem,
 	int len;
 	struct scatterlist *sg;
 	int entry;
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+	const bool odp = umem->odp_data != NULL;
+
+	if (odp) {
+		WARN_ON(shift != 0);
+		WARN_ON(access_flags != (MLX5_IB_MTT_READ | MLX5_IB_MTT_WRITE));
+
+		for (i = 0; i < num_pages; ++i) {
+			dma_addr_t pa = umem->odp_data->dma_list[offset + i];
+
+			pas[i] = cpu_to_be64(umem_dma_to_mtt(pa));
+		}
+		return;
+	}
+#endif
 
 	i = 0;
 	for_each_sg(umem->sg_head.sgl, sg, umem->nmap, entry) {
@@ -128,8 +183,7 @@ void mlx5_ib_populate_pas(struct mlx5_ib_dev *dev, struct ib_umem *umem,
 		for (k = 0; k < len; k++) {
 			if (!(i & mask)) {
 				cur = base + (k << umem_page_shift);
-				if (umr)
-					cur |= 3;
+				cur |= access_flags;
 
 				pas[i >> shift] = cpu_to_be64(cur);
 				mlx5_ib_dbg(dev, "pas[%d] 0x%llx\n",
@@ -142,6 +196,13 @@ void mlx5_ib_populate_pas(struct mlx5_ib_dev *dev, struct ib_umem *umem,
 	}
 }
 
+void mlx5_ib_populate_pas(struct mlx5_ib_dev *dev, struct ib_umem *umem,
+			  int page_shift, __be64 *pas, int access_flags)
+{
+	return __mlx5_ib_populate_pas(dev, umem, page_shift, 0,
+				      ib_umem_num_pages(umem), pas,
+				      access_flags);
+}
 int mlx5_ib_get_buf_offset(u64 addr, int page_shift, u32 *offset)
 {
 	u64 page_size;
diff --git a/drivers/infiniband/hw/mlx5/mlx5_ib.h b/drivers/infiniband/hw/mlx5/mlx5_ib.h
index 386780f0d1e1..83f22fe297c8 100644
--- a/drivers/infiniband/hw/mlx5/mlx5_ib.h
+++ b/drivers/infiniband/hw/mlx5/mlx5_ib.h
@@ -111,6 +111,8 @@ struct mlx5_ib_pd {
  */
 
 #define MLX5_IB_SEND_UMR_UNREG	IB_SEND_RESERVED_START
+#define MLX5_IB_SEND_UMR_FAIL_IF_FREE (IB_SEND_RESERVED_START << 1)
+#define MLX5_IB_SEND_UMR_UPDATE_MTT (IB_SEND_RESERVED_START << 2)
 #define MLX5_IB_QPT_REG_UMR	IB_QPT_RESERVED1
 #define MLX5_IB_WR_UMR		IB_WR_RESERVED1
 
@@ -147,6 +149,29 @@ enum {
 	MLX5_QP_EMPTY
 };
 
+/*
+ * Connect-IB can trigger up to four concurrent pagefaults
+ * per-QP.
+ */
+enum mlx5_ib_pagefault_context {
+	MLX5_IB_PAGEFAULT_RESPONDER_READ,
+	MLX5_IB_PAGEFAULT_REQUESTOR_READ,
+	MLX5_IB_PAGEFAULT_RESPONDER_WRITE,
+	MLX5_IB_PAGEFAULT_REQUESTOR_WRITE,
+	MLX5_IB_PAGEFAULT_CONTEXTS
+};
+
+static inline enum mlx5_ib_pagefault_context
+	mlx5_ib_get_pagefault_context(struct mlx5_pagefault *pagefault)
+{
+	return pagefault->flags & (MLX5_PFAULT_REQUESTOR | MLX5_PFAULT_WRITE);
+}
+
+struct mlx5_ib_pfault {
+	struct work_struct	work;
+	struct mlx5_pagefault	mpfault;
+};
+
 struct mlx5_ib_qp {
 	struct ib_qp		ibqp;
 	struct mlx5_core_qp	mqp;
@@ -192,6 +217,21 @@ struct mlx5_ib_qp {
 
 	/* Store signature errors */
 	bool			signature_en;
+
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+	/*
+	 * A flag that is true for QP's that are in a state that doesn't
+	 * allow page faults, and shouldn't schedule any more faults.
+	 */
+	int                     disable_page_faults;
+	/*
+	 * The disable_page_faults_lock protects a QP's disable_page_faults
+	 * field, allowing for a thread to atomically check whether the QP
+	 * allows page faults, and if so schedule a page fault.
+	 */
+	spinlock_t              disable_page_faults_lock;
+	struct mlx5_ib_pfault	pagefaults[MLX5_IB_PAGEFAULT_CONTEXTS];
+#endif
 };
 
 struct mlx5_ib_cq_buf {
@@ -206,6 +246,19 @@ enum mlx5_ib_qp_flags {
 	MLX5_IB_QP_SIGNATURE_HANDLING           = 1 << 1,
 };
 
+struct mlx5_umr_wr {
+	union {
+		u64			virt_addr;
+		u64			offset;
+	} target;
+	struct ib_pd		       *pd;
+	unsigned int			page_shift;
+	unsigned int			npages;
+	u32				length;
+	int				access_flags;
+	u32				mkey;
+};
+
 struct mlx5_shared_mr_info {
 	int mr_id;
 	struct ib_umem		*umem;
@@ -253,6 +306,13 @@ struct mlx5_ib_xrcd {
 	u32			xrcdn;
 };
 
+enum mlx5_ib_mtt_access_flags {
+	MLX5_IB_MTT_READ  = (1 << 0),
+	MLX5_IB_MTT_WRITE = (1 << 1),
+};
+
+#define MLX5_IB_MTT_PRESENT (MLX5_IB_MTT_READ | MLX5_IB_MTT_WRITE)
+
 struct mlx5_ib_mr {
 	struct ib_mr		ibmr;
 	struct mlx5_core_mr	mmr;
@@ -261,12 +321,11 @@ struct mlx5_ib_mr {
 	struct list_head	list;
 	int			order;
 	int			umred;
-	__be64			*pas;
-	dma_addr_t		dma;
 	int			npages;
 	struct mlx5_ib_dev     *dev;
 	struct mlx5_create_mkey_mbox_out out;
 	struct mlx5_core_sig_ctx    *sig;
+	int			live;
 };
 
 struct mlx5_ib_fast_reg_page_list {
@@ -372,11 +431,18 @@ struct mlx5_ib_dev {
 	struct umr_common		umrc;
 	/* sync used page count stats
 	 */
-	spinlock_t			mr_lock;
 	struct mlx5_ib_resources	devr;
 	struct mlx5_mr_cache		cache;
 	struct timer_list		delay_timer;
 	int				fill_delay;
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+	struct ib_odp_caps	odp_caps;
+	/*
+	 * Sleepable RCU that prevents destruction of MRs while they are still
+	 * being used by a page fault handler.
+	 */
+	struct srcu_struct      mr_srcu;
+#endif
 };
 
 static inline struct mlx5_ib_cq *to_mibcq(struct mlx5_core_cq *mcq)
@@ -490,6 +556,8 @@ int mlx5_ib_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 int mlx5_ib_post_recv(struct ib_qp *ibqp, struct ib_recv_wr *wr,
 		      struct ib_recv_wr **bad_wr);
 void *mlx5_get_send_wqe(struct mlx5_ib_qp *qp, int n);
+int mlx5_ib_read_user_wqe(struct mlx5_ib_qp *qp, int send, int wqe_index,
+			  void *buffer, u32 length);
 struct ib_cq *mlx5_ib_create_cq(struct ib_device *ibdev, int entries,
 				int vector, struct ib_ucontext *context,
 				struct ib_udata *udata);
@@ -502,6 +570,8 @@ struct ib_mr *mlx5_ib_get_dma_mr(struct ib_pd *pd, int acc);
 struct ib_mr *mlx5_ib_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
 				  u64 virt_addr, int access_flags,
 				  struct ib_udata *udata);
+int mlx5_ib_update_mtt(struct mlx5_ib_mr *mr, u64 start_page_index,
+		       int npages, int zap);
 int mlx5_ib_dereg_mr(struct ib_mr *ibmr);
 int mlx5_ib_destroy_mr(struct ib_mr *ibmr);
 struct ib_mr *mlx5_ib_create_mr(struct ib_pd *pd,
@@ -533,8 +603,11 @@ int mlx5_ib_init_fmr(struct mlx5_ib_dev *dev);
 void mlx5_ib_cleanup_fmr(struct mlx5_ib_dev *dev);
 void mlx5_ib_cont_pages(struct ib_umem *umem, u64 addr, int *count, int *shift,
 			int *ncont, int *order);
+void __mlx5_ib_populate_pas(struct mlx5_ib_dev *dev, struct ib_umem *umem,
+			    int page_shift, size_t offset, size_t num_pages,
+			    __be64 *pas, int access_flags);
 void mlx5_ib_populate_pas(struct mlx5_ib_dev *dev, struct ib_umem *umem,
-			  int page_shift, __be64 *pas, int umr);
+			  int page_shift, __be64 *pas, int access_flags);
 void mlx5_ib_copy_pas(u64 *old, u64 *new, int step, int num);
 int mlx5_ib_get_cqe_size(struct mlx5_ib_dev *dev, struct ib_cq *ibcq);
 int mlx5_mr_cache_init(struct mlx5_ib_dev *dev);
@@ -544,6 +617,38 @@ void mlx5_umr_cq_handler(struct ib_cq *cq, void *cq_context);
 int mlx5_ib_check_mr_status(struct ib_mr *ibmr, u32 check_mask,
 			    struct ib_mr_status *mr_status);
 
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+extern struct workqueue_struct *mlx5_ib_page_fault_wq;
+
+int mlx5_ib_internal_query_odp_caps(struct mlx5_ib_dev *dev);
+void mlx5_ib_mr_pfault_handler(struct mlx5_ib_qp *qp,
+			       struct mlx5_ib_pfault *pfault);
+void mlx5_ib_odp_create_qp(struct mlx5_ib_qp *qp);
+int mlx5_ib_odp_init_one(struct mlx5_ib_dev *ibdev);
+void mlx5_ib_odp_remove_one(struct mlx5_ib_dev *ibdev);
+int __init mlx5_ib_odp_init(void);
+void mlx5_ib_odp_cleanup(void);
+void mlx5_ib_qp_disable_pagefaults(struct mlx5_ib_qp *qp);
+void mlx5_ib_qp_enable_pagefaults(struct mlx5_ib_qp *qp);
+void mlx5_ib_invalidate_range(struct ib_umem *umem, unsigned long start,
+			      unsigned long end);
+
+#else /* CONFIG_INFINIBAND_ON_DEMAND_PAGING */
+static inline int mlx5_ib_internal_query_odp_caps(struct mlx5_ib_dev *dev)
+{
+	return 0;
+}
+
+static inline void mlx5_ib_odp_create_qp(struct mlx5_ib_qp *qp)		{}
+static inline int mlx5_ib_odp_init_one(struct mlx5_ib_dev *ibdev) { return 0; }
+static inline void mlx5_ib_odp_remove_one(struct mlx5_ib_dev *ibdev)	{}
+static inline int mlx5_ib_odp_init(void) { return 0; }
+static inline void mlx5_ib_odp_cleanup(void)				{}
+static inline void mlx5_ib_qp_disable_pagefaults(struct mlx5_ib_qp *qp) {}
+static inline void mlx5_ib_qp_enable_pagefaults(struct mlx5_ib_qp *qp)  {}
+
+#endif /* CONFIG_INFINIBAND_ON_DEMAND_PAGING */
+
 static inline void init_query_mad(struct ib_smp *mad)
 {
 	mad->base_version  = 1;
@@ -561,4 +666,7 @@ static inline u8 convert_access(int acc)
 	       MLX5_PERM_LOCAL_READ;
 }
 
+#define MLX5_MAX_UMR_SHIFT 16
+#define MLX5_MAX_UMR_PAGES (1 << MLX5_MAX_UMR_SHIFT)
+
 #endif /* MLX5_IB_H */
diff --git a/drivers/infiniband/hw/mlx5/mr.c b/drivers/infiniband/hw/mlx5/mr.c
index 5a80dd993761..32a28bd50b20 100644
--- a/drivers/infiniband/hw/mlx5/mr.c
+++ b/drivers/infiniband/hw/mlx5/mr.c
@@ -37,21 +37,34 @@
 #include <linux/export.h>
 #include <linux/delay.h>
 #include <rdma/ib_umem.h>
+#include <rdma/ib_umem_odp.h>
+#include <rdma/ib_verbs.h>
 #include "mlx5_ib.h"
 
 enum {
 	MAX_PENDING_REG_MR = 8,
 };
 
-enum {
-	MLX5_UMR_ALIGN	= 2048
-};
+#define MLX5_UMR_ALIGN 2048
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+static __be64 mlx5_ib_update_mtt_emergency_buffer[
+		MLX5_UMR_MTT_MIN_CHUNK_SIZE/sizeof(__be64)]
+	__aligned(MLX5_UMR_ALIGN);
+static DEFINE_MUTEX(mlx5_ib_update_mtt_emergency_buffer_mutex);
+#endif
+
+static int clean_mr(struct mlx5_ib_mr *mr);
 
-static __be64 *mr_align(__be64 *ptr, int align)
+static int destroy_mkey(struct mlx5_ib_dev *dev, struct mlx5_ib_mr *mr)
 {
-	unsigned long mask = align - 1;
+	int err = mlx5_core_destroy_mkey(dev->mdev, &mr->mmr);
 
-	return (__be64 *)(((unsigned long)ptr + mask) & ~mask);
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+	/* Wait until all page fault handlers using the mr complete. */
+	synchronize_srcu(&dev->mr_srcu);
+#endif
+
+	return err;
 }
 
 static int order2idx(struct mlx5_ib_dev *dev, int order)
@@ -146,7 +159,7 @@ static int add_keys(struct mlx5_ib_dev *dev, int c, int num)
 		mr->order = ent->order;
 		mr->umred = 1;
 		mr->dev = dev;
-		in->seg.status = 1 << 6;
+		in->seg.status = MLX5_MKEY_STATUS_FREE;
 		in->seg.xlt_oct_size = cpu_to_be32((npages + 1) / 2);
 		in->seg.qpn_mkey7_0 = cpu_to_be32(0xffffff << 8);
 		in->seg.flags = MLX5_ACCESS_MODE_MTT | MLX5_PERM_UMR_EN;
@@ -191,7 +204,7 @@ static void remove_keys(struct mlx5_ib_dev *dev, int c, int num)
 		ent->cur--;
 		ent->size--;
 		spin_unlock_irq(&ent->lock);
-		err = mlx5_core_destroy_mkey(dev->mdev, &mr->mmr);
+		err = destroy_mkey(dev, mr);
 		if (err)
 			mlx5_ib_warn(dev, "failed destroy mkey\n");
 		else
@@ -482,7 +495,7 @@ static void clean_keys(struct mlx5_ib_dev *dev, int c)
 		ent->cur--;
 		ent->size--;
 		spin_unlock_irq(&ent->lock);
-		err = mlx5_core_destroy_mkey(dev->mdev, &mr->mmr);
+		err = destroy_mkey(dev, mr);
 		if (err)
 			mlx5_ib_warn(dev, "failed destroy mkey\n");
 		else
@@ -668,7 +681,7 @@ static int get_octo_len(u64 addr, u64 len, int page_size)
 
 static int use_umr(int order)
 {
-	return order <= 17;
+	return order <= MLX5_MAX_UMR_SHIFT;
 }
 
 static void prep_umr_reg_wqe(struct ib_pd *pd, struct ib_send_wr *wr,
@@ -678,6 +691,7 @@ static void prep_umr_reg_wqe(struct ib_pd *pd, struct ib_send_wr *wr,
 {
 	struct mlx5_ib_dev *dev = to_mdev(pd->device);
 	struct ib_mr *mr = dev->umrc.mr;
+	struct mlx5_umr_wr *umrwr = (struct mlx5_umr_wr *)&wr->wr.fast_reg;
 
 	sg->addr = dma;
 	sg->length = ALIGN(sizeof(u64) * n, 64);
@@ -692,21 +706,24 @@ static void prep_umr_reg_wqe(struct ib_pd *pd, struct ib_send_wr *wr,
 		wr->num_sge = 0;
 
 	wr->opcode = MLX5_IB_WR_UMR;
-	wr->wr.fast_reg.page_list_len = n;
-	wr->wr.fast_reg.page_shift = page_shift;
-	wr->wr.fast_reg.rkey = key;
-	wr->wr.fast_reg.iova_start = virt_addr;
-	wr->wr.fast_reg.length = len;
-	wr->wr.fast_reg.access_flags = access_flags;
-	wr->wr.fast_reg.page_list = (struct ib_fast_reg_page_list *)pd;
+
+	umrwr->npages = n;
+	umrwr->page_shift = page_shift;
+	umrwr->mkey = key;
+	umrwr->target.virt_addr = virt_addr;
+	umrwr->length = len;
+	umrwr->access_flags = access_flags;
+	umrwr->pd = pd;
 }
 
 static void prep_umr_unreg_wqe(struct mlx5_ib_dev *dev,
 			       struct ib_send_wr *wr, u32 key)
 {
-	wr->send_flags = MLX5_IB_SEND_UMR_UNREG;
+	struct mlx5_umr_wr *umrwr = (struct mlx5_umr_wr *)&wr->wr.fast_reg;
+
+	wr->send_flags = MLX5_IB_SEND_UMR_UNREG | MLX5_IB_SEND_UMR_FAIL_IF_FREE;
 	wr->opcode = MLX5_IB_WR_UMR;
-	wr->wr.fast_reg.rkey = key;
+	umrwr->mkey = key;
 }
 
 void mlx5_umr_cq_handler(struct ib_cq *cq, void *cq_context)
@@ -742,7 +759,10 @@ static struct mlx5_ib_mr *reg_umr(struct ib_pd *pd, struct ib_umem *umem,
 	struct ib_send_wr wr, *bad;
 	struct mlx5_ib_mr *mr;
 	struct ib_sge sg;
-	int size = sizeof(u64) * npages;
+	int size;
+	__be64 *mr_pas;
+	__be64 *pas;
+	dma_addr_t dma;
 	int err = 0;
 	int i;
 
@@ -761,25 +781,31 @@ static struct mlx5_ib_mr *reg_umr(struct ib_pd *pd, struct ib_umem *umem,
 	if (!mr)
 		return ERR_PTR(-EAGAIN);
 
-	mr->pas = kmalloc(size + MLX5_UMR_ALIGN - 1, GFP_KERNEL);
-	if (!mr->pas) {
+	/* UMR copies MTTs in units of MLX5_UMR_MTT_ALIGNMENT bytes.
+	 * To avoid copying garbage after the pas array, we allocate
+	 * a little more. */
+	size = ALIGN(sizeof(u64) * npages, MLX5_UMR_MTT_ALIGNMENT);
+	mr_pas = kmalloc(size + MLX5_UMR_ALIGN - 1, GFP_KERNEL);
+	if (!mr_pas) {
 		err = -ENOMEM;
 		goto free_mr;
 	}
 
-	mlx5_ib_populate_pas(dev, umem, page_shift,
-			     mr_align(mr->pas, MLX5_UMR_ALIGN), 1);
+	pas = PTR_ALIGN(mr_pas, MLX5_UMR_ALIGN);
+	mlx5_ib_populate_pas(dev, umem, page_shift, pas, MLX5_IB_MTT_PRESENT);
+	/* Clear padding after the actual pages. */
+	memset(pas + npages, 0, size - npages * sizeof(u64));
 
-	mr->dma = dma_map_single(ddev, mr_align(mr->pas, MLX5_UMR_ALIGN), size,
-				 DMA_TO_DEVICE);
-	if (dma_mapping_error(ddev, mr->dma)) {
+	dma = dma_map_single(ddev, pas, size, DMA_TO_DEVICE);
+	if (dma_mapping_error(ddev, dma)) {
 		err = -ENOMEM;
 		goto free_pas;
 	}
 
 	memset(&wr, 0, sizeof(wr));
 	wr.wr_id = (u64)(unsigned long)&umr_context;
-	prep_umr_reg_wqe(pd, &wr, &sg, mr->dma, npages, mr->mmr.key, page_shift, virt_addr, len, access_flags);
+	prep_umr_reg_wqe(pd, &wr, &sg, dma, npages, mr->mmr.key, page_shift,
+			 virt_addr, len, access_flags);
 
 	mlx5_ib_init_umr_context(&umr_context);
 	down(&umrc->sem);
@@ -799,12 +825,14 @@ static struct mlx5_ib_mr *reg_umr(struct ib_pd *pd, struct ib_umem *umem,
 	mr->mmr.size = len;
 	mr->mmr.pd = to_mpd(pd)->pdn;
 
+	mr->live = 1;
+
 unmap_dma:
 	up(&umrc->sem);
-	dma_unmap_single(ddev, mr->dma, size, DMA_TO_DEVICE);
+	dma_unmap_single(ddev, dma, size, DMA_TO_DEVICE);
 
 free_pas:
-	kfree(mr->pas);
+	kfree(mr_pas);
 
 free_mr:
 	if (err) {
@@ -815,6 +843,128 @@ free_mr:
 	return mr;
 }
 
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+int mlx5_ib_update_mtt(struct mlx5_ib_mr *mr, u64 start_page_index, int npages,
+		       int zap)
+{
+	struct mlx5_ib_dev *dev = mr->dev;
+	struct device *ddev = dev->ib_dev.dma_device;
+	struct umr_common *umrc = &dev->umrc;
+	struct mlx5_ib_umr_context umr_context;
+	struct ib_umem *umem = mr->umem;
+	int size;
+	__be64 *pas;
+	dma_addr_t dma;
+	struct ib_send_wr wr, *bad;
+	struct mlx5_umr_wr *umrwr = (struct mlx5_umr_wr *)&wr.wr.fast_reg;
+	struct ib_sge sg;
+	int err = 0;
+	const int page_index_alignment = MLX5_UMR_MTT_ALIGNMENT / sizeof(u64);
+	const int page_index_mask = page_index_alignment - 1;
+	size_t pages_mapped = 0;
+	size_t pages_to_map = 0;
+	size_t pages_iter = 0;
+	int use_emergency_buf = 0;
+
+	/* UMR copies MTTs in units of MLX5_UMR_MTT_ALIGNMENT bytes,
+	 * so we need to align the offset and length accordingly */
+	if (start_page_index & page_index_mask) {
+		npages += start_page_index & page_index_mask;
+		start_page_index &= ~page_index_mask;
+	}
+
+	pages_to_map = ALIGN(npages, page_index_alignment);
+
+	if (start_page_index + pages_to_map > MLX5_MAX_UMR_PAGES)
+		return -EINVAL;
+
+	size = sizeof(u64) * pages_to_map;
+	size = min_t(int, PAGE_SIZE, size);
+	/* We allocate with GFP_ATOMIC to avoid recursion into page-reclaim
+	 * code, when we are called from an invalidation. The pas buffer must
+	 * be 2k-aligned for Connect-IB. */
+	pas = (__be64 *)get_zeroed_page(GFP_ATOMIC);
+	if (!pas) {
+		mlx5_ib_warn(dev, "unable to allocate memory during MTT update, falling back to slower chunked mechanism.\n");
+		pas = mlx5_ib_update_mtt_emergency_buffer;
+		size = MLX5_UMR_MTT_MIN_CHUNK_SIZE;
+		use_emergency_buf = 1;
+		mutex_lock(&mlx5_ib_update_mtt_emergency_buffer_mutex);
+		memset(pas, 0, size);
+	}
+	pages_iter = size / sizeof(u64);
+	dma = dma_map_single(ddev, pas, size, DMA_TO_DEVICE);
+	if (dma_mapping_error(ddev, dma)) {
+		mlx5_ib_err(dev, "unable to map DMA during MTT update.\n");
+		err = -ENOMEM;
+		goto free_pas;
+	}
+
+	for (pages_mapped = 0;
+	     pages_mapped < pages_to_map && !err;
+	     pages_mapped += pages_iter, start_page_index += pages_iter) {
+		dma_sync_single_for_cpu(ddev, dma, size, DMA_TO_DEVICE);
+
+		npages = min_t(size_t,
+			       pages_iter,
+			       ib_umem_num_pages(umem) - start_page_index);
+
+		if (!zap) {
+			__mlx5_ib_populate_pas(dev, umem, PAGE_SHIFT,
+					       start_page_index, npages, pas,
+					       MLX5_IB_MTT_PRESENT);
+			/* Clear padding after the pages brought from the
+			 * umem. */
+			memset(pas + npages, 0, size - npages * sizeof(u64));
+		}
+
+		dma_sync_single_for_device(ddev, dma, size, DMA_TO_DEVICE);
+
+		memset(&wr, 0, sizeof(wr));
+		wr.wr_id = (u64)(unsigned long)&umr_context;
+
+		sg.addr = dma;
+		sg.length = ALIGN(npages * sizeof(u64),
+				MLX5_UMR_MTT_ALIGNMENT);
+		sg.lkey = dev->umrc.mr->lkey;
+
+		wr.send_flags = MLX5_IB_SEND_UMR_FAIL_IF_FREE |
+				MLX5_IB_SEND_UMR_UPDATE_MTT;
+		wr.sg_list = &sg;
+		wr.num_sge = 1;
+		wr.opcode = MLX5_IB_WR_UMR;
+		umrwr->npages = sg.length / sizeof(u64);
+		umrwr->page_shift = PAGE_SHIFT;
+		umrwr->mkey = mr->mmr.key;
+		umrwr->target.offset = start_page_index;
+
+		mlx5_ib_init_umr_context(&umr_context);
+		down(&umrc->sem);
+		err = ib_post_send(umrc->qp, &wr, &bad);
+		if (err) {
+			mlx5_ib_err(dev, "UMR post send failed, err %d\n", err);
+		} else {
+			wait_for_completion(&umr_context.done);
+			if (umr_context.status != IB_WC_SUCCESS) {
+				mlx5_ib_err(dev, "UMR completion failed, code %d\n",
+					    umr_context.status);
+				err = -EFAULT;
+			}
+		}
+		up(&umrc->sem);
+	}
+	dma_unmap_single(ddev, dma, size, DMA_TO_DEVICE);
+
+free_pas:
+	if (!use_emergency_buf)
+		free_page((unsigned long)pas);
+	else
+		mutex_unlock(&mlx5_ib_update_mtt_emergency_buffer_mutex);
+
+	return err;
+}
+#endif
+
 static struct mlx5_ib_mr *reg_create(struct ib_pd *pd, u64 virt_addr,
 				     u64 length, struct ib_umem *umem,
 				     int npages, int page_shift,
@@ -825,6 +975,8 @@ static struct mlx5_ib_mr *reg_create(struct ib_pd *pd, u64 virt_addr,
 	struct mlx5_ib_mr *mr;
 	int inlen;
 	int err;
+	bool pg_cap = !!(dev->mdev->caps.gen.flags &
+			 MLX5_DEV_CAP_FLAG_ON_DMND_PG);
 
 	mr = kzalloc(sizeof(*mr), GFP_KERNEL);
 	if (!mr)
@@ -836,8 +988,12 @@ static struct mlx5_ib_mr *reg_create(struct ib_pd *pd, u64 virt_addr,
 		err = -ENOMEM;
 		goto err_1;
 	}
-	mlx5_ib_populate_pas(dev, umem, page_shift, in->pas, 0);
+	mlx5_ib_populate_pas(dev, umem, page_shift, in->pas,
+			     pg_cap ? MLX5_IB_MTT_PRESENT : 0);
 
+	/* The MLX5_MKEY_INBOX_PG_ACCESS bit allows setting the access flags
+	 * in the page list submitted with the command. */
+	in->flags = pg_cap ? cpu_to_be32(MLX5_MKEY_INBOX_PG_ACCESS) : 0;
 	in->seg.flags = convert_access(access_flags) |
 		MLX5_ACCESS_MODE_MTT;
 	in->seg.flags_pd = cpu_to_be32(to_mpd(pd)->pdn);
@@ -856,6 +1012,7 @@ static struct mlx5_ib_mr *reg_create(struct ib_pd *pd, u64 virt_addr,
 		goto err_2;
 	}
 	mr->umem = umem;
+	mr->live = 1;
 	kvfree(in);
 
 	mlx5_ib_dbg(dev, "mkey = 0x%x\n", mr->mmr.key);
@@ -910,6 +1067,10 @@ struct ib_mr *mlx5_ib_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
 			mlx5_ib_dbg(dev, "cache empty for order %d", order);
 			mr = NULL;
 		}
+	} else if (access_flags & IB_ACCESS_ON_DEMAND) {
+		err = -EINVAL;
+		pr_err("Got MR registration for ODP MR > 512MB, not supported for Connect-IB");
+		goto error;
 	}
 
 	if (!mr)
@@ -925,16 +1086,51 @@ struct ib_mr *mlx5_ib_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
 
 	mr->umem = umem;
 	mr->npages = npages;
-	spin_lock(&dev->mr_lock);
-	dev->mdev->priv.reg_pages += npages;
-	spin_unlock(&dev->mr_lock);
+	atomic_add(npages, &dev->mdev->priv.reg_pages);
 	mr->ibmr.lkey = mr->mmr.key;
 	mr->ibmr.rkey = mr->mmr.key;
 
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+	if (umem->odp_data) {
+		/*
+		 * This barrier prevents the compiler from moving the
+		 * setting of umem->odp_data->private to point to our
+		 * MR, before reg_umr finished, to ensure that the MR
+		 * initialization have finished before starting to
+		 * handle invalidations.
+		 */
+		smp_wmb();
+		mr->umem->odp_data->private = mr;
+		/*
+		 * Make sure we will see the new
+		 * umem->odp_data->private value in the invalidation
+		 * routines, before we can get page faults on the
+		 * MR. Page faults can happen once we put the MR in
+		 * the tree, below this line. Without the barrier,
+		 * there can be a fault handling and an invalidation
+		 * before umem->odp_data->private == mr is visible to
+		 * the invalidation handler.
+		 */
+		smp_wmb();
+	}
+#endif
+
 	return &mr->ibmr;
 
 error:
+	/*
+	 * Destroy the umem *before* destroying the MR, to ensure we
+	 * will not have any in-flight notifiers when destroying the
+	 * MR.
+	 *
+	 * As the MR is completely invalid to begin with, and this
+	 * error path is only taken if we can't push the mr entry into
+	 * the pagefault tree, this is safe.
+	 */
+
 	ib_umem_release(umem);
+	/* Kill the MR, and return an error code. */
+	clean_mr(mr);
 	return ERR_PTR(err);
 }
 
@@ -971,17 +1167,14 @@ error:
 	return err;
 }
 
-int mlx5_ib_dereg_mr(struct ib_mr *ibmr)
+static int clean_mr(struct mlx5_ib_mr *mr)
 {
-	struct mlx5_ib_dev *dev = to_mdev(ibmr->device);
-	struct mlx5_ib_mr *mr = to_mmr(ibmr);
-	struct ib_umem *umem = mr->umem;
-	int npages = mr->npages;
+	struct mlx5_ib_dev *dev = to_mdev(mr->ibmr.device);
 	int umred = mr->umred;
 	int err;
 
 	if (!umred) {
-		err = mlx5_core_destroy_mkey(dev->mdev, &mr->mmr);
+		err = destroy_mkey(dev, mr);
 		if (err) {
 			mlx5_ib_warn(dev, "failed to destroy mkey 0x%x (%d)\n",
 				     mr->mmr.key, err);
@@ -996,15 +1189,47 @@ int mlx5_ib_dereg_mr(struct ib_mr *ibmr)
 		free_cached_mr(dev, mr);
 	}
 
-	if (umem) {
+	if (!umred)
+		kfree(mr);
+
+	return 0;
+}
+
+int mlx5_ib_dereg_mr(struct ib_mr *ibmr)
+{
+	struct mlx5_ib_dev *dev = to_mdev(ibmr->device);
+	struct mlx5_ib_mr *mr = to_mmr(ibmr);
+	int npages = mr->npages;
+	struct ib_umem *umem = mr->umem;
+
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+	if (umem && umem->odp_data) {
+		/* Prevent new page faults from succeeding */
+		mr->live = 0;
+		/* Wait for all running page-fault handlers to finish. */
+		synchronize_srcu(&dev->mr_srcu);
+		/* Destroy all page mappings */
+		mlx5_ib_invalidate_range(umem, ib_umem_start(umem),
+					 ib_umem_end(umem));
+		/*
+		 * We kill the umem before the MR for ODP,
+		 * so that there will not be any invalidations in
+		 * flight, looking at the *mr struct.
+		 */
 		ib_umem_release(umem);
-		spin_lock(&dev->mr_lock);
-		dev->mdev->priv.reg_pages -= npages;
-		spin_unlock(&dev->mr_lock);
+		atomic_sub(npages, &dev->mdev->priv.reg_pages);
+
+		/* Avoid double-freeing the umem. */
+		umem = NULL;
 	}
+#endif
 
-	if (!umred)
-		kfree(mr);
+	clean_mr(mr);
+
+	if (umem) {
+		ib_umem_release(umem);
+		atomic_sub(npages, &dev->mdev->priv.reg_pages);
+	}
 
 	return 0;
 }
@@ -1028,7 +1253,7 @@ struct ib_mr *mlx5_ib_create_mr(struct ib_pd *pd,
 		goto err_free;
 	}
 
-	in->seg.status = 1 << 6; /* free */
+	in->seg.status = MLX5_MKEY_STATUS_FREE;
 	in->seg.xlt_oct_size = cpu_to_be32(ndescs);
 	in->seg.qpn_mkey7_0 = cpu_to_be32(0xffffff << 8);
 	in->seg.flags_pd = cpu_to_be32(to_mpd(pd)->pdn);
@@ -1113,7 +1338,7 @@ int mlx5_ib_destroy_mr(struct ib_mr *ibmr)
 		kfree(mr->sig);
 	}
 
-	err = mlx5_core_destroy_mkey(dev->mdev, &mr->mmr);
+	err = destroy_mkey(dev, mr);
 	if (err) {
 		mlx5_ib_warn(dev, "failed to destroy mkey 0x%x (%d)\n",
 			     mr->mmr.key, err);
@@ -1143,7 +1368,7 @@ struct ib_mr *mlx5_ib_alloc_fast_reg_mr(struct ib_pd *pd,
 		goto err_free;
 	}
 
-	in->seg.status = 1 << 6; /* free */
+	in->seg.status = MLX5_MKEY_STATUS_FREE;
 	in->seg.xlt_oct_size = cpu_to_be32((max_page_list_len + 1) / 2);
 	in->seg.qpn_mkey7_0 = cpu_to_be32(0xffffff << 8);
 	in->seg.flags = MLX5_PERM_UMR_EN | MLX5_ACCESS_MODE_MTT;
diff --git a/drivers/infiniband/hw/mlx5/odp.c b/drivers/infiniband/hw/mlx5/odp.c
new file mode 100644
index 000000000000..a2c541c4809a
--- /dev/null
+++ b/drivers/infiniband/hw/mlx5/odp.c
@@ -0,0 +1,798 @@
+/*
+ * Copyright (c) 2014 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 <rdma/ib_umem.h>
+#include <rdma/ib_umem_odp.h>
+
+#include "mlx5_ib.h"
+
+#define MAX_PREFETCH_LEN (4*1024*1024U)
+
+/* Timeout in ms to wait for an active mmu notifier to complete when handling
+ * a pagefault. */
+#define MMU_NOTIFIER_TIMEOUT 1000
+
+struct workqueue_struct *mlx5_ib_page_fault_wq;
+
+void mlx5_ib_invalidate_range(struct ib_umem *umem, unsigned long start,
+			      unsigned long end)
+{
+	struct mlx5_ib_mr *mr;
+	const u64 umr_block_mask = (MLX5_UMR_MTT_ALIGNMENT / sizeof(u64)) - 1;
+	u64 idx = 0, blk_start_idx = 0;
+	int in_block = 0;
+	u64 addr;
+
+	if (!umem || !umem->odp_data) {
+		pr_err("invalidation called on NULL umem or non-ODP umem\n");
+		return;
+	}
+
+	mr = umem->odp_data->private;
+
+	if (!mr || !mr->ibmr.pd)
+		return;
+
+	start = max_t(u64, ib_umem_start(umem), start);
+	end = min_t(u64, ib_umem_end(umem), end);
+
+	/*
+	 * Iteration one - zap the HW's MTTs. The notifiers_count ensures that
+	 * while we are doing the invalidation, no page fault will attempt to
+	 * overwrite the same MTTs.  Concurent invalidations might race us,
+	 * but they will write 0s as well, so no difference in the end result.
+	 */
+
+	for (addr = start; addr < end; addr += (u64)umem->page_size) {
+		idx = (addr - ib_umem_start(umem)) / PAGE_SIZE;
+		/*
+		 * Strive to write the MTTs in chunks, but avoid overwriting
+		 * non-existing MTTs. The huristic here can be improved to
+		 * estimate the cost of another UMR vs. the cost of bigger
+		 * UMR.
+		 */
+		if (umem->odp_data->dma_list[idx] &
+		    (ODP_READ_ALLOWED_BIT | ODP_WRITE_ALLOWED_BIT)) {
+			if (!in_block) {
+				blk_start_idx = idx;
+				in_block = 1;
+			}
+		} else {
+			u64 umr_offset = idx & umr_block_mask;
+
+			if (in_block && umr_offset == 0) {
+				mlx5_ib_update_mtt(mr, blk_start_idx,
+						   idx - blk_start_idx, 1);
+				in_block = 0;
+			}
+		}
+	}
+	if (in_block)
+		mlx5_ib_update_mtt(mr, blk_start_idx, idx - blk_start_idx + 1,
+				   1);
+
+	/*
+	 * We are now sure that the device will not access the
+	 * memory. We can safely unmap it, and mark it as dirty if
+	 * needed.
+	 */
+
+	ib_umem_odp_unmap_dma_pages(umem, start, end);
+}
+
+#define COPY_ODP_BIT_MLX_TO_IB(reg, ib_caps, field_name, bit_name) do {	\
+	if (be32_to_cpu(reg.field_name) & MLX5_ODP_SUPPORT_##bit_name)	\
+		ib_caps->field_name |= IB_ODP_SUPPORT_##bit_name;	\
+} while (0)
+
+int mlx5_ib_internal_query_odp_caps(struct mlx5_ib_dev *dev)
+{
+	int err;
+	struct mlx5_odp_caps hw_caps;
+	struct ib_odp_caps *caps = &dev->odp_caps;
+
+	memset(caps, 0, sizeof(*caps));
+
+	if (!(dev->mdev->caps.gen.flags & MLX5_DEV_CAP_FLAG_ON_DMND_PG))
+		return 0;
+
+	err = mlx5_query_odp_caps(dev->mdev, &hw_caps);
+	if (err)
+		goto out;
+
+	caps->general_caps = IB_ODP_SUPPORT;
+	COPY_ODP_BIT_MLX_TO_IB(hw_caps, caps, per_transport_caps.ud_odp_caps,
+			       SEND);
+	COPY_ODP_BIT_MLX_TO_IB(hw_caps, caps, per_transport_caps.rc_odp_caps,
+			       SEND);
+	COPY_ODP_BIT_MLX_TO_IB(hw_caps, caps, per_transport_caps.rc_odp_caps,
+			       RECV);
+	COPY_ODP_BIT_MLX_TO_IB(hw_caps, caps, per_transport_caps.rc_odp_caps,
+			       WRITE);
+	COPY_ODP_BIT_MLX_TO_IB(hw_caps, caps, per_transport_caps.rc_odp_caps,
+			       READ);
+
+out:
+	return err;
+}
+
+static struct mlx5_ib_mr *mlx5_ib_odp_find_mr_lkey(struct mlx5_ib_dev *dev,
+						   u32 key)
+{
+	u32 base_key = mlx5_base_mkey(key);
+	struct mlx5_core_mr *mmr = __mlx5_mr_lookup(dev->mdev, base_key);
+	struct mlx5_ib_mr *mr = container_of(mmr, struct mlx5_ib_mr, mmr);
+
+	if (!mmr || mmr->key != key || !mr->live)
+		return NULL;
+
+	return container_of(mmr, struct mlx5_ib_mr, mmr);
+}
+
+static void mlx5_ib_page_fault_resume(struct mlx5_ib_qp *qp,
+				      struct mlx5_ib_pfault *pfault,
+				      int error) {
+	struct mlx5_ib_dev *dev = to_mdev(qp->ibqp.pd->device);
+	int ret = mlx5_core_page_fault_resume(dev->mdev, qp->mqp.qpn,
+					      pfault->mpfault.flags,
+					      error);
+	if (ret)
+		pr_err("Failed to resolve the page fault on QP 0x%x\n",
+		       qp->mqp.qpn);
+}
+
+/*
+ * Handle a single data segment in a page-fault WQE.
+ *
+ * Returns number of pages retrieved on success. The caller will continue to
+ * the next data segment.
+ * Can return the following error codes:
+ * -EAGAIN to designate a temporary error. The caller will abort handling the
+ *  page fault and resolve it.
+ * -EFAULT when there's an error mapping the requested pages. The caller will
+ *  abort the page fault handling and possibly move the QP to an error state.
+ * On other errors the QP should also be closed with an error.
+ */
+static int pagefault_single_data_segment(struct mlx5_ib_qp *qp,
+					 struct mlx5_ib_pfault *pfault,
+					 u32 key, u64 io_virt, size_t bcnt,
+					 u32 *bytes_mapped)
+{
+	struct mlx5_ib_dev *mib_dev = to_mdev(qp->ibqp.pd->device);
+	int srcu_key;
+	unsigned int current_seq;
+	u64 start_idx;
+	int npages = 0, ret = 0;
+	struct mlx5_ib_mr *mr;
+	u64 access_mask = ODP_READ_ALLOWED_BIT;
+
+	srcu_key = srcu_read_lock(&mib_dev->mr_srcu);
+	mr = mlx5_ib_odp_find_mr_lkey(mib_dev, key);
+	/*
+	 * If we didn't find the MR, it means the MR was closed while we were
+	 * handling the ODP event. In this case we return -EFAULT so that the
+	 * QP will be closed.
+	 */
+	if (!mr || !mr->ibmr.pd) {
+		pr_err("Failed to find relevant mr for lkey=0x%06x, probably the MR was destroyed\n",
+		       key);
+		ret = -EFAULT;
+		goto srcu_unlock;
+	}
+	if (!mr->umem->odp_data) {
+		pr_debug("skipping non ODP MR (lkey=0x%06x) in page fault handler.\n",
+			 key);
+		if (bytes_mapped)
+			*bytes_mapped +=
+				(bcnt - pfault->mpfault.bytes_committed);
+		goto srcu_unlock;
+	}
+	if (mr->ibmr.pd != qp->ibqp.pd) {
+		pr_err("Page-fault with different PDs for QP and MR.\n");
+		ret = -EFAULT;
+		goto srcu_unlock;
+	}
+
+	current_seq = ACCESS_ONCE(mr->umem->odp_data->notifiers_seq);
+	/*
+	 * Ensure the sequence number is valid for some time before we call
+	 * gup.
+	 */
+	smp_rmb();
+
+	/*
+	 * Avoid branches - this code will perform correctly
+	 * in all iterations (in iteration 2 and above,
+	 * bytes_committed == 0).
+	 */
+	io_virt += pfault->mpfault.bytes_committed;
+	bcnt -= pfault->mpfault.bytes_committed;
+
+	start_idx = (io_virt - (mr->mmr.iova & PAGE_MASK)) >> PAGE_SHIFT;
+
+	if (mr->umem->writable)
+		access_mask |= ODP_WRITE_ALLOWED_BIT;
+	npages = ib_umem_odp_map_dma_pages(mr->umem, io_virt, bcnt,
+					   access_mask, current_seq);
+	if (npages < 0) {
+		ret = npages;
+		goto srcu_unlock;
+	}
+
+	if (npages > 0) {
+		mutex_lock(&mr->umem->odp_data->umem_mutex);
+		if (!ib_umem_mmu_notifier_retry(mr->umem, current_seq)) {
+			/*
+			 * No need to check whether the MTTs really belong to
+			 * this MR, since ib_umem_odp_map_dma_pages already
+			 * checks this.
+			 */
+			ret = mlx5_ib_update_mtt(mr, start_idx, npages, 0);
+		} else {
+			ret = -EAGAIN;
+		}
+		mutex_unlock(&mr->umem->odp_data->umem_mutex);
+		if (ret < 0) {
+			if (ret != -EAGAIN)
+				pr_err("Failed to update mkey page tables\n");
+			goto srcu_unlock;
+		}
+
+		if (bytes_mapped) {
+			u32 new_mappings = npages * PAGE_SIZE -
+				(io_virt - round_down(io_virt, PAGE_SIZE));
+			*bytes_mapped += min_t(u32, new_mappings, bcnt);
+		}
+	}
+
+srcu_unlock:
+	if (ret == -EAGAIN) {
+		if (!mr->umem->odp_data->dying) {
+			struct ib_umem_odp *odp_data = mr->umem->odp_data;
+			unsigned long timeout =
+				msecs_to_jiffies(MMU_NOTIFIER_TIMEOUT);
+
+			if (!wait_for_completion_timeout(
+					&odp_data->notifier_completion,
+					timeout)) {
+				pr_warn("timeout waiting for mmu notifier completion\n");
+			}
+		} else {
+			/* The MR is being killed, kill the QP as well. */
+			ret = -EFAULT;
+		}
+	}
+	srcu_read_unlock(&mib_dev->mr_srcu, srcu_key);
+	pfault->mpfault.bytes_committed = 0;
+	return ret ? ret : npages;
+}
+
+/**
+ * Parse a series of data segments for page fault handling.
+ *
+ * @qp the QP on which the fault occurred.
+ * @pfault contains page fault information.
+ * @wqe points at the first data segment in the WQE.
+ * @wqe_end points after the end of the WQE.
+ * @bytes_mapped receives the number of bytes that the function was able to
+ *               map. This allows the caller to decide intelligently whether
+ *               enough memory was mapped to resolve the page fault
+ *               successfully (e.g. enough for the next MTU, or the entire
+ *               WQE).
+ * @total_wqe_bytes receives the total data size of this WQE in bytes (minus
+ *                  the committed bytes).
+ *
+ * Returns the number of pages loaded if positive, zero for an empty WQE, or a
+ * negative error code.
+ */
+static int pagefault_data_segments(struct mlx5_ib_qp *qp,
+				   struct mlx5_ib_pfault *pfault, void *wqe,
+				   void *wqe_end, u32 *bytes_mapped,
+				   u32 *total_wqe_bytes, int receive_queue)
+{
+	int ret = 0, npages = 0;
+	u64 io_virt;
+	u32 key;
+	u32 byte_count;
+	size_t bcnt;
+	int inline_segment;
+
+	/* Skip SRQ next-WQE segment. */
+	if (receive_queue && qp->ibqp.srq)
+		wqe += sizeof(struct mlx5_wqe_srq_next_seg);
+
+	if (bytes_mapped)
+		*bytes_mapped = 0;
+	if (total_wqe_bytes)
+		*total_wqe_bytes = 0;
+
+	while (wqe < wqe_end) {
+		struct mlx5_wqe_data_seg *dseg = wqe;
+
+		io_virt = be64_to_cpu(dseg->addr);
+		key = be32_to_cpu(dseg->lkey);
+		byte_count = be32_to_cpu(dseg->byte_count);
+		inline_segment = !!(byte_count &  MLX5_INLINE_SEG);
+		bcnt	       = byte_count & ~MLX5_INLINE_SEG;
+
+		if (inline_segment) {
+			bcnt = bcnt & MLX5_WQE_INLINE_SEG_BYTE_COUNT_MASK;
+			wqe += ALIGN(sizeof(struct mlx5_wqe_inline_seg) + bcnt,
+				     16);
+		} else {
+			wqe += sizeof(*dseg);
+		}
+
+		/* receive WQE end of sg list. */
+		if (receive_queue && bcnt == 0 && key == MLX5_INVALID_LKEY &&
+		    io_virt == 0)
+			break;
+
+		if (!inline_segment && total_wqe_bytes) {
+			*total_wqe_bytes += bcnt - min_t(size_t, bcnt,
+					pfault->mpfault.bytes_committed);
+		}
+
+		/* A zero length data segment designates a length of 2GB. */
+		if (bcnt == 0)
+			bcnt = 1U << 31;
+
+		if (inline_segment || bcnt <= pfault->mpfault.bytes_committed) {
+			pfault->mpfault.bytes_committed -=
+				min_t(size_t, bcnt,
+				      pfault->mpfault.bytes_committed);
+			continue;
+		}
+
+		ret = pagefault_single_data_segment(qp, pfault, key, io_virt,
+						    bcnt, bytes_mapped);
+		if (ret < 0)
+			break;
+		npages += ret;
+	}
+
+	return ret < 0 ? ret : npages;
+}
+
+/*
+ * Parse initiator WQE. Advances the wqe pointer to point at the
+ * scatter-gather list, and set wqe_end to the end of the WQE.
+ */
+static int mlx5_ib_mr_initiator_pfault_handler(
+	struct mlx5_ib_qp *qp, struct mlx5_ib_pfault *pfault,
+	void **wqe, void **wqe_end, int wqe_length)
+{
+	struct mlx5_ib_dev *dev = to_mdev(qp->ibqp.pd->device);
+	struct mlx5_wqe_ctrl_seg *ctrl = *wqe;
+	u16 wqe_index = pfault->mpfault.wqe.wqe_index;
+	unsigned ds, opcode;
+#if defined(DEBUG)
+	u32 ctrl_wqe_index, ctrl_qpn;
+#endif
+
+	ds = be32_to_cpu(ctrl->qpn_ds) & MLX5_WQE_CTRL_DS_MASK;
+	if (ds * MLX5_WQE_DS_UNITS > wqe_length) {
+		mlx5_ib_err(dev, "Unable to read the complete WQE. ds = 0x%x, ret = 0x%x\n",
+			    ds, wqe_length);
+		return -EFAULT;
+	}
+
+	if (ds == 0) {
+		mlx5_ib_err(dev, "Got WQE with zero DS. wqe_index=%x, qpn=%x\n",
+			    wqe_index, qp->mqp.qpn);
+		return -EFAULT;
+	}
+
+#if defined(DEBUG)
+	ctrl_wqe_index = (be32_to_cpu(ctrl->opmod_idx_opcode) &
+			MLX5_WQE_CTRL_WQE_INDEX_MASK) >>
+			MLX5_WQE_CTRL_WQE_INDEX_SHIFT;
+	if (wqe_index != ctrl_wqe_index) {
+		mlx5_ib_err(dev, "Got WQE with invalid wqe_index. wqe_index=0x%x, qpn=0x%x ctrl->wqe_index=0x%x\n",
+			    wqe_index, qp->mqp.qpn,
+			    ctrl_wqe_index);
+		return -EFAULT;
+	}
+
+	ctrl_qpn = (be32_to_cpu(ctrl->qpn_ds) & MLX5_WQE_CTRL_QPN_MASK) >>
+		MLX5_WQE_CTRL_QPN_SHIFT;
+	if (qp->mqp.qpn != ctrl_qpn) {
+		mlx5_ib_err(dev, "Got WQE with incorrect QP number. wqe_index=0x%x, qpn=0x%x ctrl->qpn=0x%x\n",
+			    wqe_index, qp->mqp.qpn,
+			    ctrl_qpn);
+		return -EFAULT;
+	}
+#endif /* DEBUG */
+
+	*wqe_end = *wqe + ds * MLX5_WQE_DS_UNITS;
+	*wqe += sizeof(*ctrl);
+
+	opcode = be32_to_cpu(ctrl->opmod_idx_opcode) &
+		 MLX5_WQE_CTRL_OPCODE_MASK;
+	switch (qp->ibqp.qp_type) {
+	case IB_QPT_RC:
+		switch (opcode) {
+		case MLX5_OPCODE_SEND:
+		case MLX5_OPCODE_SEND_IMM:
+		case MLX5_OPCODE_SEND_INVAL:
+			if (!(dev->odp_caps.per_transport_caps.rc_odp_caps &
+			      IB_ODP_SUPPORT_SEND))
+				goto invalid_transport_or_opcode;
+			break;
+		case MLX5_OPCODE_RDMA_WRITE:
+		case MLX5_OPCODE_RDMA_WRITE_IMM:
+			if (!(dev->odp_caps.per_transport_caps.rc_odp_caps &
+			      IB_ODP_SUPPORT_WRITE))
+				goto invalid_transport_or_opcode;
+			*wqe += sizeof(struct mlx5_wqe_raddr_seg);
+			break;
+		case MLX5_OPCODE_RDMA_READ:
+			if (!(dev->odp_caps.per_transport_caps.rc_odp_caps &
+			      IB_ODP_SUPPORT_READ))
+				goto invalid_transport_or_opcode;
+			*wqe += sizeof(struct mlx5_wqe_raddr_seg);
+			break;
+		default:
+			goto invalid_transport_or_opcode;
+		}
+		break;
+	case IB_QPT_UD:
+		switch (opcode) {
+		case MLX5_OPCODE_SEND:
+		case MLX5_OPCODE_SEND_IMM:
+			if (!(dev->odp_caps.per_transport_caps.ud_odp_caps &
+			      IB_ODP_SUPPORT_SEND))
+				goto invalid_transport_or_opcode;
+			*wqe += sizeof(struct mlx5_wqe_datagram_seg);
+			break;
+		default:
+			goto invalid_transport_or_opcode;
+		}
+		break;
+	default:
+invalid_transport_or_opcode:
+		mlx5_ib_err(dev, "ODP fault on QP of an unsupported opcode or transport. transport: 0x%x opcode: 0x%x.\n",
+			    qp->ibqp.qp_type, opcode);
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+/*
+ * Parse responder WQE. Advances the wqe pointer to point at the
+ * scatter-gather list, and set wqe_end to the end of the WQE.
+ */
+static int mlx5_ib_mr_responder_pfault_handler(
+	struct mlx5_ib_qp *qp, struct mlx5_ib_pfault *pfault,
+	void **wqe, void **wqe_end, int wqe_length)
+{
+	struct mlx5_ib_dev *dev = to_mdev(qp->ibqp.pd->device);
+	struct mlx5_ib_wq *wq = &qp->rq;
+	int wqe_size = 1 << wq->wqe_shift;
+
+	if (qp->ibqp.srq) {
+		mlx5_ib_err(dev, "ODP fault on SRQ is not supported\n");
+		return -EFAULT;
+	}
+
+	if (qp->wq_sig) {
+		mlx5_ib_err(dev, "ODP fault with WQE signatures is not supported\n");
+		return -EFAULT;
+	}
+
+	if (wqe_size > wqe_length) {
+		mlx5_ib_err(dev, "Couldn't read all of the receive WQE's content\n");
+		return -EFAULT;
+	}
+
+	switch (qp->ibqp.qp_type) {
+	case IB_QPT_RC:
+		if (!(dev->odp_caps.per_transport_caps.rc_odp_caps &
+		      IB_ODP_SUPPORT_RECV))
+			goto invalid_transport_or_opcode;
+		break;
+	default:
+invalid_transport_or_opcode:
+		mlx5_ib_err(dev, "ODP fault on QP of an unsupported transport. transport: 0x%x\n",
+			    qp->ibqp.qp_type);
+		return -EFAULT;
+	}
+
+	*wqe_end = *wqe + wqe_size;
+
+	return 0;
+}
+
+static void mlx5_ib_mr_wqe_pfault_handler(struct mlx5_ib_qp *qp,
+					  struct mlx5_ib_pfault *pfault)
+{
+	struct mlx5_ib_dev *dev = to_mdev(qp->ibqp.pd->device);
+	int ret;
+	void *wqe, *wqe_end;
+	u32 bytes_mapped, total_wqe_bytes;
+	char *buffer = NULL;
+	int resume_with_error = 0;
+	u16 wqe_index = pfault->mpfault.wqe.wqe_index;
+	int requestor = pfault->mpfault.flags & MLX5_PFAULT_REQUESTOR;
+
+	buffer = (char *)__get_free_page(GFP_KERNEL);
+	if (!buffer) {
+		mlx5_ib_err(dev, "Error allocating memory for IO page fault handling.\n");
+		resume_with_error = 1;
+		goto resolve_page_fault;
+	}
+
+	ret = mlx5_ib_read_user_wqe(qp, requestor, wqe_index, buffer,
+				    PAGE_SIZE);
+	if (ret < 0) {
+		mlx5_ib_err(dev, "Failed reading a WQE following page fault, error=%x, wqe_index=%x, qpn=%x\n",
+			    -ret, wqe_index, qp->mqp.qpn);
+		resume_with_error = 1;
+		goto resolve_page_fault;
+	}
+
+	wqe = buffer;
+	if (requestor)
+		ret = mlx5_ib_mr_initiator_pfault_handler(qp, pfault, &wqe,
+							  &wqe_end, ret);
+	else
+		ret = mlx5_ib_mr_responder_pfault_handler(qp, pfault, &wqe,
+							  &wqe_end, ret);
+	if (ret < 0) {
+		resume_with_error = 1;
+		goto resolve_page_fault;
+	}
+
+	if (wqe >= wqe_end) {
+		mlx5_ib_err(dev, "ODP fault on invalid WQE.\n");
+		resume_with_error = 1;
+		goto resolve_page_fault;
+	}
+
+	ret = pagefault_data_segments(qp, pfault, wqe, wqe_end, &bytes_mapped,
+				      &total_wqe_bytes, !requestor);
+	if (ret == -EAGAIN) {
+		goto resolve_page_fault;
+	} else if (ret < 0 || total_wqe_bytes > bytes_mapped) {
+		mlx5_ib_err(dev, "Error getting user pages for page fault. Error: 0x%x\n",
+			    -ret);
+		resume_with_error = 1;
+		goto resolve_page_fault;
+	}
+
+resolve_page_fault:
+	mlx5_ib_page_fault_resume(qp, pfault, resume_with_error);
+	mlx5_ib_dbg(dev, "PAGE FAULT completed. QP 0x%x resume_with_error=%d, flags: 0x%x\n",
+		    qp->mqp.qpn, resume_with_error, pfault->mpfault.flags);
+
+	free_page((unsigned long)buffer);
+}
+
+static int pages_in_range(u64 address, u32 length)
+{
+	return (ALIGN(address + length, PAGE_SIZE) -
+		(address & PAGE_MASK)) >> PAGE_SHIFT;
+}
+
+static void mlx5_ib_mr_rdma_pfault_handler(struct mlx5_ib_qp *qp,
+					   struct mlx5_ib_pfault *pfault)
+{
+	struct mlx5_pagefault *mpfault = &pfault->mpfault;
+	u64 address;
+	u32 length;
+	u32 prefetch_len = mpfault->bytes_committed;
+	int prefetch_activated = 0;
+	u32 rkey = mpfault->rdma.r_key;
+	int ret;
+
+	/* The RDMA responder handler handles the page fault in two parts.
+	 * First it brings the necessary pages for the current packet
+	 * (and uses the pfault context), and then (after resuming the QP)
+	 * prefetches more pages. The second operation cannot use the pfault
+	 * context and therefore uses the dummy_pfault context allocated on
+	 * the stack */
+	struct mlx5_ib_pfault dummy_pfault = {};
+
+	dummy_pfault.mpfault.bytes_committed = 0;
+
+	mpfault->rdma.rdma_va += mpfault->bytes_committed;
+	mpfault->rdma.rdma_op_len -= min(mpfault->bytes_committed,
+					 mpfault->rdma.rdma_op_len);
+	mpfault->bytes_committed = 0;
+
+	address = mpfault->rdma.rdma_va;
+	length  = mpfault->rdma.rdma_op_len;
+
+	/* For some operations, the hardware cannot tell the exact message
+	 * length, and in those cases it reports zero. Use prefetch
+	 * logic. */
+	if (length == 0) {
+		prefetch_activated = 1;
+		length = mpfault->rdma.packet_size;
+		prefetch_len = min(MAX_PREFETCH_LEN, prefetch_len);
+	}
+
+	ret = pagefault_single_data_segment(qp, pfault, rkey, address, length,
+					    NULL);
+	if (ret == -EAGAIN) {
+		/* We're racing with an invalidation, don't prefetch */
+		prefetch_activated = 0;
+	} else if (ret < 0 || pages_in_range(address, length) > ret) {
+		mlx5_ib_page_fault_resume(qp, pfault, 1);
+		return;
+	}
+
+	mlx5_ib_page_fault_resume(qp, pfault, 0);
+
+	/* At this point, there might be a new pagefault already arriving in
+	 * the eq, switch to the dummy pagefault for the rest of the
+	 * processing. We're still OK with the objects being alive as the
+	 * work-queue is being fenced. */
+
+	if (prefetch_activated) {
+		ret = pagefault_single_data_segment(qp, &dummy_pfault, rkey,
+						    address,
+						    prefetch_len,
+						    NULL);
+		if (ret < 0) {
+			pr_warn("Prefetch failed (ret = %d, prefetch_activated = %d) for QPN %d, address: 0x%.16llx, length = 0x%.16x\n",
+				ret, prefetch_activated,
+				qp->ibqp.qp_num, address, prefetch_len);
+		}
+	}
+}
+
+void mlx5_ib_mr_pfault_handler(struct mlx5_ib_qp *qp,
+			       struct mlx5_ib_pfault *pfault)
+{
+	u8 event_subtype = pfault->mpfault.event_subtype;
+
+	switch (event_subtype) {
+	case MLX5_PFAULT_SUBTYPE_WQE:
+		mlx5_ib_mr_wqe_pfault_handler(qp, pfault);
+		break;
+	case MLX5_PFAULT_SUBTYPE_RDMA:
+		mlx5_ib_mr_rdma_pfault_handler(qp, pfault);
+		break;
+	default:
+		pr_warn("Invalid page fault event subtype: 0x%x\n",
+			event_subtype);
+		mlx5_ib_page_fault_resume(qp, pfault, 1);
+		break;
+	}
+}
+
+static void mlx5_ib_qp_pfault_action(struct work_struct *work)
+{
+	struct mlx5_ib_pfault *pfault = container_of(work,
+						     struct mlx5_ib_pfault,
+						     work);
+	enum mlx5_ib_pagefault_context context =
+		mlx5_ib_get_pagefault_context(&pfault->mpfault);
+	struct mlx5_ib_qp *qp = container_of(pfault, struct mlx5_ib_qp,
+					     pagefaults[context]);
+	mlx5_ib_mr_pfault_handler(qp, pfault);
+}
+
+void mlx5_ib_qp_disable_pagefaults(struct mlx5_ib_qp *qp)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&qp->disable_page_faults_lock, flags);
+	qp->disable_page_faults = 1;
+	spin_unlock_irqrestore(&qp->disable_page_faults_lock, flags);
+
+	/*
+	 * Note that at this point, we are guarenteed that no more
+	 * work queue elements will be posted to the work queue with
+	 * the QP we are closing.
+	 */
+	flush_workqueue(mlx5_ib_page_fault_wq);
+}
+
+void mlx5_ib_qp_enable_pagefaults(struct mlx5_ib_qp *qp)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&qp->disable_page_faults_lock, flags);
+	qp->disable_page_faults = 0;
+	spin_unlock_irqrestore(&qp->disable_page_faults_lock, flags);
+}
+
+static void mlx5_ib_pfault_handler(struct mlx5_core_qp *qp,
+				   struct mlx5_pagefault *pfault)
+{
+	/*
+	 * Note that we will only get one fault event per QP per context
+	 * (responder/initiator, read/write), until we resolve the page fault
+	 * with the mlx5_ib_page_fault_resume command. Since this function is
+	 * called from within the work element, there is no risk of missing
+	 * events.
+	 */
+	struct mlx5_ib_qp *mibqp = to_mibqp(qp);
+	enum mlx5_ib_pagefault_context context =
+		mlx5_ib_get_pagefault_context(pfault);
+	struct mlx5_ib_pfault *qp_pfault = &mibqp->pagefaults[context];
+
+	qp_pfault->mpfault = *pfault;
+
+	/* No need to stop interrupts here since we are in an interrupt */
+	spin_lock(&mibqp->disable_page_faults_lock);
+	if (!mibqp->disable_page_faults)
+		queue_work(mlx5_ib_page_fault_wq, &qp_pfault->work);
+	spin_unlock(&mibqp->disable_page_faults_lock);
+}
+
+void mlx5_ib_odp_create_qp(struct mlx5_ib_qp *qp)
+{
+	int i;
+
+	qp->disable_page_faults = 1;
+	spin_lock_init(&qp->disable_page_faults_lock);
+
+	qp->mqp.pfault_handler	= mlx5_ib_pfault_handler;
+
+	for (i = 0; i < MLX5_IB_PAGEFAULT_CONTEXTS; ++i)
+		INIT_WORK(&qp->pagefaults[i].work, mlx5_ib_qp_pfault_action);
+}
+
+int mlx5_ib_odp_init_one(struct mlx5_ib_dev *ibdev)
+{
+	int ret;
+
+	ret = init_srcu_struct(&ibdev->mr_srcu);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+void mlx5_ib_odp_remove_one(struct mlx5_ib_dev *ibdev)
+{
+	cleanup_srcu_struct(&ibdev->mr_srcu);
+}
+
+int __init mlx5_ib_odp_init(void)
+{
+	mlx5_ib_page_fault_wq =
+		create_singlethread_workqueue("mlx5_ib_page_faults");
+	if (!mlx5_ib_page_fault_wq)
+		return -ENOMEM;
+
+	return 0;
+}
+
+void mlx5_ib_odp_cleanup(void)
+{
+	destroy_workqueue(mlx5_ib_page_fault_wq);
+}
diff --git a/drivers/infiniband/hw/mlx5/qp.c b/drivers/infiniband/hw/mlx5/qp.c
index 1cae1c7132b4..be0cd358b080 100644
--- a/drivers/infiniband/hw/mlx5/qp.c
+++ b/drivers/infiniband/hw/mlx5/qp.c
@@ -70,15 +70,6 @@ static const u32 mlx5_ib_opcode[] = {
 	[MLX5_IB_WR_UMR]			= MLX5_OPCODE_UMR,
 };
 
-struct umr_wr {
-	u64				virt_addr;
-	struct ib_pd		       *pd;
-	unsigned int			page_shift;
-	unsigned int			npages;
-	u32				length;
-	int				access_flags;
-	u32				mkey;
-};
 
 static int is_qp0(enum ib_qp_type qp_type)
 {
@@ -110,6 +101,77 @@ void *mlx5_get_send_wqe(struct mlx5_ib_qp *qp, int n)
 	return get_wqe(qp, qp->sq.offset + (n << MLX5_IB_SQ_STRIDE));
 }
 
+/**
+ * mlx5_ib_read_user_wqe() - Copy a user-space WQE to kernel space.
+ *
+ * @qp: QP to copy from.
+ * @send: copy from the send queue when non-zero, use the receive queue
+ *	  otherwise.
+ * @wqe_index:  index to start copying from. For send work queues, the
+ *		wqe_index is in units of MLX5_SEND_WQE_BB.
+ *		For receive work queue, it is the number of work queue
+ *		element in the queue.
+ * @buffer: destination buffer.
+ * @length: maximum number of bytes to copy.
+ *
+ * Copies at least a single WQE, but may copy more data.
+ *
+ * Return: the number of bytes copied, or an error code.
+ */
+int mlx5_ib_read_user_wqe(struct mlx5_ib_qp *qp, int send, int wqe_index,
+			  void *buffer, u32 length)
+{
+	struct ib_device *ibdev = qp->ibqp.device;
+	struct mlx5_ib_dev *dev = to_mdev(ibdev);
+	struct mlx5_ib_wq *wq = send ? &qp->sq : &qp->rq;
+	size_t offset;
+	size_t wq_end;
+	struct ib_umem *umem = qp->umem;
+	u32 first_copy_length;
+	int wqe_length;
+	int ret;
+
+	if (wq->wqe_cnt == 0) {
+		mlx5_ib_dbg(dev, "mlx5_ib_read_user_wqe for a QP with wqe_cnt == 0. qp_type: 0x%x\n",
+			    qp->ibqp.qp_type);
+		return -EINVAL;
+	}
+
+	offset = wq->offset + ((wqe_index % wq->wqe_cnt) << wq->wqe_shift);
+	wq_end = wq->offset + (wq->wqe_cnt << wq->wqe_shift);
+
+	if (send && length < sizeof(struct mlx5_wqe_ctrl_seg))
+		return -EINVAL;
+
+	if (offset > umem->length ||
+	    (send && offset + sizeof(struct mlx5_wqe_ctrl_seg) > umem->length))
+		return -EINVAL;
+
+	first_copy_length = min_t(u32, offset + length, wq_end) - offset;
+	ret = ib_umem_copy_from(buffer, umem, offset, first_copy_length);
+	if (ret)
+		return ret;
+
+	if (send) {
+		struct mlx5_wqe_ctrl_seg *ctrl = buffer;
+		int ds = be32_to_cpu(ctrl->qpn_ds) & MLX5_WQE_CTRL_DS_MASK;
+
+		wqe_length = ds * MLX5_WQE_DS_UNITS;
+	} else {
+		wqe_length = 1 << wq->wqe_shift;
+	}
+
+	if (wqe_length <= first_copy_length)
+		return first_copy_length;
+
+	ret = ib_umem_copy_from(buffer + first_copy_length, umem, wq->offset,
+				wqe_length - first_copy_length);
+	if (ret)
+		return ret;
+
+	return wqe_length;
+}
+
 static void mlx5_ib_qp_event(struct mlx5_core_qp *qp, int type)
 {
 	struct ib_qp *ibqp = &to_mibqp(qp)->ibqp;
@@ -814,6 +876,8 @@ static int create_qp_common(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 	int inlen = sizeof(*in);
 	int err;
 
+	mlx5_ib_odp_create_qp(qp);
+
 	gen = &dev->mdev->caps.gen;
 	mutex_init(&qp->mutex);
 	spin_lock_init(&qp->sq.lock);
@@ -1098,11 +1162,13 @@ static void destroy_qp_common(struct mlx5_ib_dev *dev, struct mlx5_ib_qp *qp)
 	in = kzalloc(sizeof(*in), GFP_KERNEL);
 	if (!in)
 		return;
-	if (qp->state != IB_QPS_RESET)
+	if (qp->state != IB_QPS_RESET) {
+		mlx5_ib_qp_disable_pagefaults(qp);
 		if (mlx5_core_qp_modify(dev->mdev, to_mlx5_state(qp->state),
 					MLX5_QP_STATE_RST, in, sizeof(*in), &qp->mqp))
 			mlx5_ib_warn(dev, "mlx5_ib: modify QP %06x to RESET failed\n",
 				     qp->mqp.qpn);
+	}
 
 	get_cqs(qp, &send_cq, &recv_cq);
 
@@ -1650,6 +1716,15 @@ static int __mlx5_ib_modify_qp(struct ib_qp *ibqp,
 	if (mlx5_st < 0)
 		goto out;
 
+	/* If moving to a reset or error state, we must disable page faults on
+	 * this QP and flush all current page faults. Otherwise a stale page
+	 * fault may attempt to work on this QP after it is reset and moved
+	 * again to RTS, and may cause the driver and the device to get out of
+	 * sync. */
+	if (cur_state != IB_QPS_RESET && cur_state != IB_QPS_ERR &&
+	    (new_state == IB_QPS_RESET || new_state == IB_QPS_ERR))
+		mlx5_ib_qp_disable_pagefaults(qp);
+
 	optpar = ib_mask_to_mlx5_opt(attr_mask);
 	optpar &= opt_mask[mlx5_cur][mlx5_new][mlx5_st];
 	in->optparam = cpu_to_be32(optpar);
@@ -1659,6 +1734,9 @@ static int __mlx5_ib_modify_qp(struct ib_qp *ibqp,
 	if (err)
 		goto out;
 
+	if (cur_state == IB_QPS_RESET && new_state == IB_QPS_INIT)
+		mlx5_ib_qp_enable_pagefaults(qp);
+
 	qp->state = new_state;
 
 	if (attr_mask & IB_QP_ACCESS_FLAGS)
@@ -1848,37 +1926,70 @@ static void set_frwr_umr_segment(struct mlx5_wqe_umr_ctrl_seg *umr,
 	umr->mkey_mask = frwr_mkey_mask();
 }
 
+static __be64 get_umr_reg_mr_mask(void)
+{
+	u64 result;
+
+	result = MLX5_MKEY_MASK_LEN		|
+		 MLX5_MKEY_MASK_PAGE_SIZE	|
+		 MLX5_MKEY_MASK_START_ADDR	|
+		 MLX5_MKEY_MASK_PD		|
+		 MLX5_MKEY_MASK_LR		|
+		 MLX5_MKEY_MASK_LW		|
+		 MLX5_MKEY_MASK_KEY		|
+		 MLX5_MKEY_MASK_RR		|
+		 MLX5_MKEY_MASK_RW		|
+		 MLX5_MKEY_MASK_A		|
+		 MLX5_MKEY_MASK_FREE;
+
+	return cpu_to_be64(result);
+}
+
+static __be64 get_umr_unreg_mr_mask(void)
+{
+	u64 result;
+
+	result = MLX5_MKEY_MASK_FREE;
+
+	return cpu_to_be64(result);
+}
+
+static __be64 get_umr_update_mtt_mask(void)
+{
+	u64 result;
+
+	result = MLX5_MKEY_MASK_FREE;
+
+	return cpu_to_be64(result);
+}
+
 static void set_reg_umr_segment(struct mlx5_wqe_umr_ctrl_seg *umr,
 				struct ib_send_wr *wr)
 {
-	struct umr_wr *umrwr = (struct umr_wr *)&wr->wr.fast_reg;
-	u64 mask;
+	struct mlx5_umr_wr *umrwr = (struct mlx5_umr_wr *)&wr->wr.fast_reg;
 
 	memset(umr, 0, sizeof(*umr));
 
+	if (wr->send_flags & MLX5_IB_SEND_UMR_FAIL_IF_FREE)
+		umr->flags = MLX5_UMR_CHECK_FREE; /* fail if free */
+	else
+		umr->flags = MLX5_UMR_CHECK_NOT_FREE; /* fail if not free */
+
 	if (!(wr->send_flags & MLX5_IB_SEND_UMR_UNREG)) {
-		umr->flags = 1 << 5; /* fail if not free */
 		umr->klm_octowords = get_klm_octo(umrwr->npages);
-		mask =  MLX5_MKEY_MASK_LEN		|
-			MLX5_MKEY_MASK_PAGE_SIZE	|
-			MLX5_MKEY_MASK_START_ADDR	|
-			MLX5_MKEY_MASK_PD		|
-			MLX5_MKEY_MASK_LR		|
-			MLX5_MKEY_MASK_LW		|
-			MLX5_MKEY_MASK_KEY		|
-			MLX5_MKEY_MASK_RR		|
-			MLX5_MKEY_MASK_RW		|
-			MLX5_MKEY_MASK_A		|
-			MLX5_MKEY_MASK_FREE;
-		umr->mkey_mask = cpu_to_be64(mask);
+		if (wr->send_flags & MLX5_IB_SEND_UMR_UPDATE_MTT) {
+			umr->mkey_mask = get_umr_update_mtt_mask();
+			umr->bsf_octowords = get_klm_octo(umrwr->target.offset);
+			umr->flags |= MLX5_UMR_TRANSLATION_OFFSET_EN;
+		} else {
+			umr->mkey_mask = get_umr_reg_mr_mask();
+		}
 	} else {
-		umr->flags = 2 << 5; /* fail if free */
-		mask = MLX5_MKEY_MASK_FREE;
-		umr->mkey_mask = cpu_to_be64(mask);
+		umr->mkey_mask = get_umr_unreg_mr_mask();
 	}
 
 	if (!wr->num_sge)
-		umr->flags |= (1 << 7); /* inline */
+		umr->flags |= MLX5_UMR_INLINE;
 }
 
 static u8 get_umr_flags(int acc)
@@ -1895,7 +2006,7 @@ static void set_mkey_segment(struct mlx5_mkey_seg *seg, struct ib_send_wr *wr,
 {
 	memset(seg, 0, sizeof(*seg));
 	if (li) {
-		seg->status = 1 << 6;
+		seg->status = MLX5_MKEY_STATUS_FREE;
 		return;
 	}
 
@@ -1912,19 +2023,23 @@ static void set_mkey_segment(struct mlx5_mkey_seg *seg, struct ib_send_wr *wr,
 
 static void set_reg_mkey_segment(struct mlx5_mkey_seg *seg, struct ib_send_wr *wr)
 {
+	struct mlx5_umr_wr *umrwr = (struct mlx5_umr_wr *)&wr->wr.fast_reg;
+
 	memset(seg, 0, sizeof(*seg));
 	if (wr->send_flags & MLX5_IB_SEND_UMR_UNREG) {
-		seg->status = 1 << 6;
+		seg->status = MLX5_MKEY_STATUS_FREE;
 		return;
 	}
 
-	seg->flags = convert_access(wr->wr.fast_reg.access_flags);
-	seg->flags_pd = cpu_to_be32(to_mpd((struct ib_pd *)wr->wr.fast_reg.page_list)->pdn);
-	seg->start_addr = cpu_to_be64(wr->wr.fast_reg.iova_start);
-	seg->len = cpu_to_be64(wr->wr.fast_reg.length);
-	seg->log2_page_size = wr->wr.fast_reg.page_shift;
+	seg->flags = convert_access(umrwr->access_flags);
+	if (!(wr->send_flags & MLX5_IB_SEND_UMR_UPDATE_MTT)) {
+		seg->flags_pd = cpu_to_be32(to_mpd(umrwr->pd)->pdn);
+		seg->start_addr = cpu_to_be64(umrwr->target.virt_addr);
+	}
+	seg->len = cpu_to_be64(umrwr->length);
+	seg->log2_page_size = umrwr->page_shift;
 	seg->qpn_mkey7_0 = cpu_to_be32(0xffffff00 |
-				       mlx5_mkey_variant(wr->wr.fast_reg.rkey));
+				       mlx5_mkey_variant(umrwr->mkey));
 }
 
 static void set_frwr_pages(struct mlx5_wqe_data_seg *dseg,
@@ -2927,6 +3042,14 @@ int mlx5_ib_query_qp(struct ib_qp *ibqp, struct ib_qp_attr *qp_attr, int qp_attr
 	int mlx5_state;
 	int err = 0;
 
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+	/*
+	 * Wait for any outstanding page faults, in case the user frees memory
+	 * based upon this query's result.
+	 */
+	flush_workqueue(mlx5_ib_page_fault_wq);
+#endif
+
 	mutex_lock(&qp->mutex);
 	outb = kzalloc(sizeof(*outb), GFP_KERNEL);
 	if (!outb) {
diff --git a/drivers/infiniband/hw/nes/nes_verbs.c b/drivers/infiniband/hw/nes/nes_verbs.c
index fef067c959fc..c0d0296e7a00 100644
--- a/drivers/infiniband/hw/nes/nes_verbs.c
+++ b/drivers/infiniband/hw/nes/nes_verbs.c
@@ -2341,9 +2341,9 @@ static struct ib_mr *nes_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
 	nes_debug(NES_DBG_MR, "User base = 0x%lX, Virt base = 0x%lX, length = %u,"
 			" offset = %u, page size = %u.\n",
 			(unsigned long int)start, (unsigned long int)virt, (u32)length,
-			region->offset, region->page_size);
+			ib_umem_offset(region), region->page_size);
 
-	skip_pages = ((u32)region->offset) >> 12;
+	skip_pages = ((u32)ib_umem_offset(region)) >> 12;
 
 	if (ib_copy_from_udata(&req, udata, sizeof(req))) {
 		ib_umem_release(region);
@@ -2408,7 +2408,7 @@ static struct ib_mr *nes_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
 				region_length -= skip_pages << 12;
 				for (page_index = skip_pages; page_index < chunk_pages; page_index++) {
 					skip_pages = 0;
-					if ((page_count != 0) && (page_count<<12)-(region->offset&(4096-1)) >= region->length)
+					if ((page_count != 0) && (page_count << 12) - (ib_umem_offset(region) & (4096 - 1)) >= region->length)
 						goto enough_pages;
 					if ((page_count&0x01FF) == 0) {
 						if (page_count >= 1024 * 512) {
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_ah.c b/drivers/infiniband/hw/ocrdma/ocrdma_ah.c
index ac02ce4e8040..f3cc8c9e65ae 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_ah.c
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_ah.c
@@ -96,7 +96,6 @@ struct ib_ah *ocrdma_create_ah(struct ib_pd *ibpd, struct ib_ah_attr *attr)
 	struct ocrdma_pd *pd = get_ocrdma_pd(ibpd);
 	struct ocrdma_dev *dev = get_ocrdma_dev(ibpd->device);
 	union ib_gid sgid;
-	u8 zmac[ETH_ALEN];
 
 	if (!(attr->ah_flags & IB_AH_GRH))
 		return ERR_PTR(-EINVAL);
@@ -118,9 +117,7 @@ struct ib_ah *ocrdma_create_ah(struct ib_pd *ibpd, struct ib_ah_attr *attr)
 		goto av_conf_err;
 	}
 
-	memset(&zmac, 0, ETH_ALEN);
-	if (pd->uctx &&
-	    memcmp(attr->dmac, &zmac, ETH_ALEN)) {
+	if (pd->uctx) {
 		status = rdma_addr_find_dmac_by_grh(&sgid, &attr->grh.dgid,
                                         attr->dmac, &attr->vlan_id);
 		if (status) {
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
index 4c68305ee781..fb8d8c4dfbb9 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
@@ -805,7 +805,7 @@ struct ib_mr *ocrdma_reg_user_mr(struct ib_pd *ibpd, u64 start, u64 len,
 		goto umem_err;
 
 	mr->hwmr.pbe_size = mr->umem->page_size;
-	mr->hwmr.fbo = mr->umem->offset;
+	mr->hwmr.fbo = ib_umem_offset(mr->umem);
 	mr->hwmr.va = usr_addr;
 	mr->hwmr.len = len;
 	mr->hwmr.remote_wr = (acc & IB_ACCESS_REMOTE_WRITE) ? 1 : 0;
@@ -1410,6 +1410,8 @@ int ocrdma_query_qp(struct ib_qp *ibqp,
 	mutex_unlock(&dev->dev_lock);
 	if (status)
 		goto mbx_err;
+	if (qp->qp_type == IB_QPT_UD)
+		qp_attr->qkey = params.qkey;
 	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 =
diff --git a/drivers/infiniband/hw/qib/qib_mr.c b/drivers/infiniband/hw/qib/qib_mr.c
index 9bbb55347cc1..a77fb4fb14e4 100644
--- a/drivers/infiniband/hw/qib/qib_mr.c
+++ b/drivers/infiniband/hw/qib/qib_mr.c
@@ -258,7 +258,7 @@ struct ib_mr *qib_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
 	mr->mr.user_base = start;
 	mr->mr.iova = virt_addr;
 	mr->mr.length = length;
-	mr->mr.offset = umem->offset;
+	mr->mr.offset = ib_umem_offset(umem);
 	mr->mr.access_flags = mr_access_flags;
 	mr->umem = umem;
 
diff --git a/drivers/infiniband/ulp/ipoib/ipoib.h b/drivers/infiniband/ulp/ipoib/ipoib.h
index d7562beb5423..8ba80a6d3a46 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib.h
+++ b/drivers/infiniband/ulp/ipoib/ipoib.h
@@ -98,9 +98,15 @@ enum {
 
 	IPOIB_MCAST_FLAG_FOUND	  = 0,	/* used in set_multicast_list */
 	IPOIB_MCAST_FLAG_SENDONLY = 1,
-	IPOIB_MCAST_FLAG_BUSY	  = 2,	/* joining or already joined */
+	/*
+	 * For IPOIB_MCAST_FLAG_BUSY
+	 * When set, in flight join and mcast->mc is unreliable
+	 * When clear and mcast->mc IS_ERR_OR_NULL, need to restart or
+	 *   haven't started yet
+	 * When clear and mcast->mc is valid pointer, join was successful
+	 */
+	IPOIB_MCAST_FLAG_BUSY	  = 2,
 	IPOIB_MCAST_FLAG_ATTACHED = 3,
-	IPOIB_MCAST_JOIN_STARTED  = 4,
 
 	MAX_SEND_CQE		  = 16,
 	IPOIB_CM_COPYBREAK	  = 256,
@@ -317,6 +323,7 @@ struct ipoib_dev_priv {
 	struct list_head multicast_list;
 	struct rb_root multicast_tree;
 
+	struct workqueue_struct *wq;
 	struct delayed_work mcast_task;
 	struct work_struct carrier_on_task;
 	struct work_struct flush_light;
@@ -477,10 +484,10 @@ void ipoib_ib_dev_flush_heavy(struct work_struct *work);
 void ipoib_pkey_event(struct work_struct *work);
 void ipoib_ib_dev_cleanup(struct net_device *dev);
 
-int ipoib_ib_dev_open(struct net_device *dev, int flush);
+int ipoib_ib_dev_open(struct net_device *dev);
 int ipoib_ib_dev_up(struct net_device *dev);
-int ipoib_ib_dev_down(struct net_device *dev, int flush);
-int ipoib_ib_dev_stop(struct net_device *dev, int flush);
+int ipoib_ib_dev_down(struct net_device *dev);
+int ipoib_ib_dev_stop(struct net_device *dev);
 void ipoib_pkey_dev_check_presence(struct net_device *dev);
 
 int ipoib_dev_init(struct net_device *dev, struct ib_device *ca, int port);
@@ -492,7 +499,7 @@ void ipoib_mcast_send(struct net_device *dev, u8 *daddr, struct sk_buff *skb);
 
 void ipoib_mcast_restart_task(struct work_struct *work);
 int ipoib_mcast_start_thread(struct net_device *dev);
-int ipoib_mcast_stop_thread(struct net_device *dev, int flush);
+int ipoib_mcast_stop_thread(struct net_device *dev);
 
 void ipoib_mcast_dev_down(struct net_device *dev);
 void ipoib_mcast_dev_flush(struct net_device *dev);
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_cm.c b/drivers/infiniband/ulp/ipoib/ipoib_cm.c
index 933efcea0d03..56959adb6c7d 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_cm.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_cm.c
@@ -474,7 +474,7 @@ static int ipoib_cm_req_handler(struct ib_cm_id *cm_id, struct ib_cm_event *even
 	}
 
 	spin_lock_irq(&priv->lock);
-	queue_delayed_work(ipoib_workqueue,
+	queue_delayed_work(priv->wq,
 			   &priv->cm.stale_task, IPOIB_CM_RX_DELAY);
 	/* Add this entry to passive ids list head, but do not re-add it
 	 * if IB_EVENT_QP_LAST_WQE_REACHED has moved it to flush list. */
@@ -576,7 +576,7 @@ void ipoib_cm_handle_rx_wc(struct net_device *dev, struct ib_wc *wc)
 			spin_lock_irqsave(&priv->lock, flags);
 			list_splice_init(&priv->cm.rx_drain_list, &priv->cm.rx_reap_list);
 			ipoib_cm_start_rx_drain(priv);
-			queue_work(ipoib_workqueue, &priv->cm.rx_reap_task);
+			queue_work(priv->wq, &priv->cm.rx_reap_task);
 			spin_unlock_irqrestore(&priv->lock, flags);
 		} else
 			ipoib_warn(priv, "cm recv completion event with wrid %d (> %d)\n",
@@ -603,7 +603,7 @@ void ipoib_cm_handle_rx_wc(struct net_device *dev, struct ib_wc *wc)
 				spin_lock_irqsave(&priv->lock, flags);
 				list_move(&p->list, &priv->cm.rx_reap_list);
 				spin_unlock_irqrestore(&priv->lock, flags);
-				queue_work(ipoib_workqueue, &priv->cm.rx_reap_task);
+				queue_work(priv->wq, &priv->cm.rx_reap_task);
 			}
 			return;
 		}
@@ -827,7 +827,7 @@ void ipoib_cm_handle_tx_wc(struct net_device *dev, struct ib_wc *wc)
 
 		if (test_and_clear_bit(IPOIB_FLAG_INITIALIZED, &tx->flags)) {
 			list_move(&tx->list, &priv->cm.reap_list);
-			queue_work(ipoib_workqueue, &priv->cm.reap_task);
+			queue_work(priv->wq, &priv->cm.reap_task);
 		}
 
 		clear_bit(IPOIB_FLAG_OPER_UP, &tx->flags);
@@ -1255,7 +1255,7 @@ static int ipoib_cm_tx_handler(struct ib_cm_id *cm_id,
 
 		if (test_and_clear_bit(IPOIB_FLAG_INITIALIZED, &tx->flags)) {
 			list_move(&tx->list, &priv->cm.reap_list);
-			queue_work(ipoib_workqueue, &priv->cm.reap_task);
+			queue_work(priv->wq, &priv->cm.reap_task);
 		}
 
 		spin_unlock_irqrestore(&priv->lock, flags);
@@ -1284,7 +1284,7 @@ struct ipoib_cm_tx *ipoib_cm_create_tx(struct net_device *dev, struct ipoib_path
 	tx->dev = dev;
 	list_add(&tx->list, &priv->cm.start_list);
 	set_bit(IPOIB_FLAG_INITIALIZED, &tx->flags);
-	queue_work(ipoib_workqueue, &priv->cm.start_task);
+	queue_work(priv->wq, &priv->cm.start_task);
 	return tx;
 }
 
@@ -1295,7 +1295,7 @@ void ipoib_cm_destroy_tx(struct ipoib_cm_tx *tx)
 	if (test_and_clear_bit(IPOIB_FLAG_INITIALIZED, &tx->flags)) {
 		spin_lock_irqsave(&priv->lock, flags);
 		list_move(&tx->list, &priv->cm.reap_list);
-		queue_work(ipoib_workqueue, &priv->cm.reap_task);
+		queue_work(priv->wq, &priv->cm.reap_task);
 		ipoib_dbg(priv, "Reap connection for gid %pI6\n",
 			  tx->neigh->daddr + 4);
 		tx->neigh = NULL;
@@ -1417,7 +1417,7 @@ void ipoib_cm_skb_too_long(struct net_device *dev, struct sk_buff *skb,
 
 	skb_queue_tail(&priv->cm.skb_queue, skb);
 	if (e)
-		queue_work(ipoib_workqueue, &priv->cm.skb_task);
+		queue_work(priv->wq, &priv->cm.skb_task);
 }
 
 static void ipoib_cm_rx_reap(struct work_struct *work)
@@ -1450,7 +1450,7 @@ static void ipoib_cm_stale_task(struct work_struct *work)
 	}
 
 	if (!list_empty(&priv->cm.passive_ids))
-		queue_delayed_work(ipoib_workqueue,
+		queue_delayed_work(priv->wq,
 				   &priv->cm.stale_task, IPOIB_CM_RX_DELAY);
 	spin_unlock_irq(&priv->lock);
 }
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_ib.c b/drivers/infiniband/ulp/ipoib/ipoib_ib.c
index 72626c348174..fe65abb5150c 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_ib.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_ib.c
@@ -655,7 +655,7 @@ void ipoib_reap_ah(struct work_struct *work)
 	__ipoib_reap_ah(dev);
 
 	if (!test_bit(IPOIB_STOP_REAPER, &priv->flags))
-		queue_delayed_work(ipoib_workqueue, &priv->ah_reap_task,
+		queue_delayed_work(priv->wq, &priv->ah_reap_task,
 				   round_jiffies_relative(HZ));
 }
 
@@ -664,7 +664,7 @@ static void ipoib_ib_tx_timer_func(unsigned long ctx)
 	drain_tx_cq((struct net_device *)ctx);
 }
 
-int ipoib_ib_dev_open(struct net_device *dev, int flush)
+int ipoib_ib_dev_open(struct net_device *dev)
 {
 	struct ipoib_dev_priv *priv = netdev_priv(dev);
 	int ret;
@@ -696,7 +696,7 @@ int ipoib_ib_dev_open(struct net_device *dev, int flush)
 	}
 
 	clear_bit(IPOIB_STOP_REAPER, &priv->flags);
-	queue_delayed_work(ipoib_workqueue, &priv->ah_reap_task,
+	queue_delayed_work(priv->wq, &priv->ah_reap_task,
 			   round_jiffies_relative(HZ));
 
 	if (!test_and_set_bit(IPOIB_FLAG_INITIALIZED, &priv->flags))
@@ -706,7 +706,7 @@ int ipoib_ib_dev_open(struct net_device *dev, int flush)
 dev_stop:
 	if (!test_and_set_bit(IPOIB_FLAG_INITIALIZED, &priv->flags))
 		napi_enable(&priv->napi);
-	ipoib_ib_dev_stop(dev, flush);
+	ipoib_ib_dev_stop(dev);
 	return -1;
 }
 
@@ -738,7 +738,7 @@ int ipoib_ib_dev_up(struct net_device *dev)
 	return ipoib_mcast_start_thread(dev);
 }
 
-int ipoib_ib_dev_down(struct net_device *dev, int flush)
+int ipoib_ib_dev_down(struct net_device *dev)
 {
 	struct ipoib_dev_priv *priv = netdev_priv(dev);
 
@@ -747,7 +747,7 @@ int ipoib_ib_dev_down(struct net_device *dev, int flush)
 	clear_bit(IPOIB_FLAG_OPER_UP, &priv->flags);
 	netif_carrier_off(dev);
 
-	ipoib_mcast_stop_thread(dev, flush);
+	ipoib_mcast_stop_thread(dev);
 	ipoib_mcast_dev_flush(dev);
 
 	ipoib_flush_paths(dev);
@@ -807,7 +807,7 @@ void ipoib_drain_cq(struct net_device *dev)
 	local_bh_enable();
 }
 
-int ipoib_ib_dev_stop(struct net_device *dev, int flush)
+int ipoib_ib_dev_stop(struct net_device *dev)
 {
 	struct ipoib_dev_priv *priv = netdev_priv(dev);
 	struct ib_qp_attr qp_attr;
@@ -880,8 +880,7 @@ timeout:
 	/* Wait for all AHs to be reaped */
 	set_bit(IPOIB_STOP_REAPER, &priv->flags);
 	cancel_delayed_work(&priv->ah_reap_task);
-	if (flush)
-		flush_workqueue(ipoib_workqueue);
+	flush_workqueue(priv->wq);
 
 	begin = jiffies;
 
@@ -918,7 +917,7 @@ int ipoib_ib_dev_init(struct net_device *dev, struct ib_device *ca, int port)
 		    (unsigned long) dev);
 
 	if (dev->flags & IFF_UP) {
-		if (ipoib_ib_dev_open(dev, 1)) {
+		if (ipoib_ib_dev_open(dev)) {
 			ipoib_transport_dev_cleanup(dev);
 			return -ENODEV;
 		}
@@ -1040,12 +1039,12 @@ static void __ipoib_ib_dev_flush(struct ipoib_dev_priv *priv,
 	}
 
 	if (level >= IPOIB_FLUSH_NORMAL)
-		ipoib_ib_dev_down(dev, 0);
+		ipoib_ib_dev_down(dev);
 
 	if (level == IPOIB_FLUSH_HEAVY) {
 		if (test_bit(IPOIB_FLAG_INITIALIZED, &priv->flags))
-			ipoib_ib_dev_stop(dev, 0);
-		if (ipoib_ib_dev_open(dev, 0) != 0)
+			ipoib_ib_dev_stop(dev);
+		if (ipoib_ib_dev_open(dev) != 0)
 			return;
 		if (netif_queue_stopped(dev))
 			netif_start_queue(dev);
@@ -1097,7 +1096,7 @@ void ipoib_ib_dev_cleanup(struct net_device *dev)
 	 */
 	ipoib_flush_paths(dev);
 
-	ipoib_mcast_stop_thread(dev, 1);
+	ipoib_mcast_stop_thread(dev);
 	ipoib_mcast_dev_flush(dev);
 
 	ipoib_transport_dev_cleanup(dev);
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_main.c b/drivers/infiniband/ulp/ipoib/ipoib_main.c
index 58b5aa3b6f2d..6bad17d4d588 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_main.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_main.c
@@ -108,7 +108,7 @@ int ipoib_open(struct net_device *dev)
 
 	set_bit(IPOIB_FLAG_ADMIN_UP, &priv->flags);
 
-	if (ipoib_ib_dev_open(dev, 1)) {
+	if (ipoib_ib_dev_open(dev)) {
 		if (!test_bit(IPOIB_PKEY_ASSIGNED, &priv->flags))
 			return 0;
 		goto err_disable;
@@ -139,7 +139,7 @@ int ipoib_open(struct net_device *dev)
 	return 0;
 
 err_stop:
-	ipoib_ib_dev_stop(dev, 1);
+	ipoib_ib_dev_stop(dev);
 
 err_disable:
 	clear_bit(IPOIB_FLAG_ADMIN_UP, &priv->flags);
@@ -157,8 +157,8 @@ static int ipoib_stop(struct net_device *dev)
 
 	netif_stop_queue(dev);
 
-	ipoib_ib_dev_down(dev, 1);
-	ipoib_ib_dev_stop(dev, 0);
+	ipoib_ib_dev_down(dev);
+	ipoib_ib_dev_stop(dev);
 
 	if (!test_bit(IPOIB_FLAG_SUBINTERFACE, &priv->flags)) {
 		struct ipoib_dev_priv *cpriv;
@@ -839,7 +839,7 @@ static void ipoib_set_mcast_list(struct net_device *dev)
 		return;
 	}
 
-	queue_work(ipoib_workqueue, &priv->restart_task);
+	queue_work(priv->wq, &priv->restart_task);
 }
 
 static u32 ipoib_addr_hash(struct ipoib_neigh_hash *htbl, u8 *daddr)
@@ -954,7 +954,7 @@ static void ipoib_reap_neigh(struct work_struct *work)
 	__ipoib_reap_neigh(priv);
 
 	if (!test_bit(IPOIB_STOP_NEIGH_GC, &priv->flags))
-		queue_delayed_work(ipoib_workqueue, &priv->neigh_reap_task,
+		queue_delayed_work(priv->wq, &priv->neigh_reap_task,
 				   arp_tbl.gc_interval);
 }
 
@@ -1133,7 +1133,7 @@ static int ipoib_neigh_hash_init(struct ipoib_dev_priv *priv)
 
 	/* start garbage collection */
 	clear_bit(IPOIB_STOP_NEIGH_GC, &priv->flags);
-	queue_delayed_work(ipoib_workqueue, &priv->neigh_reap_task,
+	queue_delayed_work(priv->wq, &priv->neigh_reap_task,
 			   arp_tbl.gc_interval);
 
 	return 0;
@@ -1262,15 +1262,13 @@ int ipoib_dev_init(struct net_device *dev, struct ib_device *ca, int port)
 {
 	struct ipoib_dev_priv *priv = netdev_priv(dev);
 
-	if (ipoib_neigh_hash_init(priv) < 0)
-		goto out;
 	/* Allocate RX/TX "rings" to hold queued skbs */
 	priv->rx_ring =	kzalloc(ipoib_recvq_size * sizeof *priv->rx_ring,
 				GFP_KERNEL);
 	if (!priv->rx_ring) {
 		printk(KERN_WARNING "%s: failed to allocate RX ring (%d entries)\n",
 		       ca->name, ipoib_recvq_size);
-		goto out_neigh_hash_cleanup;
+		goto out;
 	}
 
 	priv->tx_ring = vzalloc(ipoib_sendq_size * sizeof *priv->tx_ring);
@@ -1285,16 +1283,24 @@ int ipoib_dev_init(struct net_device *dev, struct ib_device *ca, int port)
 	if (ipoib_ib_dev_init(dev, ca, port))
 		goto out_tx_ring_cleanup;
 
+	/*
+	 * Must be after ipoib_ib_dev_init so we can allocate a per
+	 * device wq there and use it here
+	 */
+	if (ipoib_neigh_hash_init(priv) < 0)
+		goto out_dev_uninit;
+
 	return 0;
 
+out_dev_uninit:
+	ipoib_ib_dev_cleanup(dev);
+
 out_tx_ring_cleanup:
 	vfree(priv->tx_ring);
 
 out_rx_ring_cleanup:
 	kfree(priv->rx_ring);
 
-out_neigh_hash_cleanup:
-	ipoib_neigh_hash_uninit(dev);
 out:
 	return -ENOMEM;
 }
@@ -1317,6 +1323,12 @@ void ipoib_dev_cleanup(struct net_device *dev)
 	}
 	unregister_netdevice_many(&head);
 
+	/*
+	 * Must be before ipoib_ib_dev_cleanup or we delete an in use
+	 * work queue
+	 */
+	ipoib_neigh_hash_uninit(dev);
+
 	ipoib_ib_dev_cleanup(dev);
 
 	kfree(priv->rx_ring);
@@ -1324,8 +1336,6 @@ void ipoib_dev_cleanup(struct net_device *dev)
 
 	priv->rx_ring = NULL;
 	priv->tx_ring = NULL;
-
-	ipoib_neigh_hash_uninit(dev);
 }
 
 static const struct header_ops ipoib_header_ops = {
@@ -1636,7 +1646,7 @@ register_failed:
 	/* Stop GC if started before flush */
 	set_bit(IPOIB_STOP_NEIGH_GC, &priv->flags);
 	cancel_delayed_work(&priv->neigh_reap_task);
-	flush_workqueue(ipoib_workqueue);
+	flush_workqueue(priv->wq);
 
 event_failed:
 	ipoib_dev_cleanup(priv->dev);
@@ -1707,7 +1717,7 @@ static void ipoib_remove_one(struct ib_device *device)
 		/* Stop GC */
 		set_bit(IPOIB_STOP_NEIGH_GC, &priv->flags);
 		cancel_delayed_work(&priv->neigh_reap_task);
-		flush_workqueue(ipoib_workqueue);
+		flush_workqueue(priv->wq);
 
 		unregister_netdev(priv->dev);
 		free_netdev(priv->dev);
@@ -1748,8 +1758,13 @@ static int __init ipoib_init_module(void)
 	 * unregister_netdev() and linkwatch_event take the rtnl lock,
 	 * so flush_scheduled_work() can deadlock during device
 	 * removal.
+	 *
+	 * In addition, bringing one device up and another down at the
+	 * same time can deadlock a single workqueue, so we have this
+	 * global fallback workqueue, but we also attempt to open a
+	 * per device workqueue each time we bring an interface up
 	 */
-	ipoib_workqueue = create_singlethread_workqueue("ipoib");
+	ipoib_workqueue = create_singlethread_workqueue("ipoib_flush");
 	if (!ipoib_workqueue) {
 		ret = -ENOMEM;
 		goto err_fs;
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_multicast.c b/drivers/infiniband/ulp/ipoib/ipoib_multicast.c
index ffb83b5f7e80..bc50dd0d0e4d 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_multicast.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_multicast.c
@@ -190,12 +190,6 @@ static int ipoib_mcast_join_finish(struct ipoib_mcast *mcast,
 		spin_unlock_irq(&priv->lock);
 		priv->tx_wr.wr.ud.remote_qkey = priv->qkey;
 		set_qkey = 1;
-
-		if (!ipoib_cm_admin_enabled(dev)) {
-			rtnl_lock();
-			dev_set_mtu(dev, min(priv->mcast_mtu, priv->admin_mtu));
-			rtnl_unlock();
-		}
 	}
 
 	if (!test_bit(IPOIB_MCAST_FLAG_SENDONLY, &mcast->flags)) {
@@ -277,16 +271,27 @@ ipoib_mcast_sendonly_join_complete(int status,
 	struct ipoib_mcast *mcast = multicast->context;
 	struct net_device *dev = mcast->dev;
 
+	/*
+	 * We have to take the mutex to force mcast_sendonly_join to
+	 * return from ib_sa_multicast_join and set mcast->mc to a
+	 * valid value.  Otherwise we were racing with ourselves in
+	 * that we might fail here, but get a valid return from
+	 * ib_sa_multicast_join after we had cleared mcast->mc here,
+	 * resulting in mis-matched joins and leaves and a deadlock
+	 */
+	mutex_lock(&mcast_mutex);
+
 	/* We trap for port events ourselves. */
 	if (status == -ENETRESET)
-		return 0;
+		goto out;
 
 	if (!status)
 		status = ipoib_mcast_join_finish(mcast, &multicast->rec);
 
 	if (status) {
 		if (mcast->logcount++ < 20)
-			ipoib_dbg_mcast(netdev_priv(dev), "multicast join failed for %pI6, status %d\n",
+			ipoib_dbg_mcast(netdev_priv(dev), "sendonly multicast "
+					"join failed for %pI6, status %d\n",
 					mcast->mcmember.mgid.raw, status);
 
 		/* Flush out any queued packets */
@@ -296,11 +301,15 @@ ipoib_mcast_sendonly_join_complete(int status,
 			dev_kfree_skb_any(skb_dequeue(&mcast->pkt_queue));
 		}
 		netif_tx_unlock_bh(dev);
-
-		/* Clear the busy flag so we try again */
-		status = test_and_clear_bit(IPOIB_MCAST_FLAG_BUSY,
-					    &mcast->flags);
 	}
+out:
+	clear_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags);
+	if (status)
+		mcast->mc = NULL;
+	complete(&mcast->done);
+	if (status == -ENETRESET)
+		status = 0;
+	mutex_unlock(&mcast_mutex);
 	return status;
 }
 
@@ -318,12 +327,14 @@ static int ipoib_mcast_sendonly_join(struct ipoib_mcast *mcast)
 	int ret = 0;
 
 	if (!test_bit(IPOIB_FLAG_OPER_UP, &priv->flags)) {
-		ipoib_dbg_mcast(priv, "device shutting down, no multicast joins\n");
+		ipoib_dbg_mcast(priv, "device shutting down, no sendonly "
+				"multicast joins\n");
 		return -ENODEV;
 	}
 
-	if (test_and_set_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags)) {
-		ipoib_dbg_mcast(priv, "multicast entry busy, skipping\n");
+	if (test_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags)) {
+		ipoib_dbg_mcast(priv, "multicast entry busy, skipping "
+				"sendonly join\n");
 		return -EBUSY;
 	}
 
@@ -331,6 +342,9 @@ static int ipoib_mcast_sendonly_join(struct ipoib_mcast *mcast)
 	rec.port_gid = priv->local_gid;
 	rec.pkey     = cpu_to_be16(priv->pkey);
 
+	mutex_lock(&mcast_mutex);
+	init_completion(&mcast->done);
+	set_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags);
 	mcast->mc = ib_sa_join_multicast(&ipoib_sa_client, priv->ca,
 					 priv->port, &rec,
 					 IB_SA_MCMEMBER_REC_MGID	|
@@ -343,12 +357,14 @@ static int ipoib_mcast_sendonly_join(struct ipoib_mcast *mcast)
 	if (IS_ERR(mcast->mc)) {
 		ret = PTR_ERR(mcast->mc);
 		clear_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags);
-		ipoib_warn(priv, "ib_sa_join_multicast failed (ret = %d)\n",
-			   ret);
+		complete(&mcast->done);
+		ipoib_warn(priv, "ib_sa_join_multicast for sendonly join "
+			   "failed (ret = %d)\n", ret);
 	} else {
-		ipoib_dbg_mcast(priv, "no multicast record for %pI6, starting join\n",
-				mcast->mcmember.mgid.raw);
+		ipoib_dbg_mcast(priv, "no multicast record for %pI6, starting "
+				"sendonly join\n", mcast->mcmember.mgid.raw);
 	}
+	mutex_unlock(&mcast_mutex);
 
 	return ret;
 }
@@ -359,18 +375,29 @@ void ipoib_mcast_carrier_on_task(struct work_struct *work)
 						   carrier_on_task);
 	struct ib_port_attr attr;
 
-	/*
-	 * Take rtnl_lock to avoid racing with ipoib_stop() and
-	 * turning the carrier back on while a device is being
-	 * removed.
-	 */
 	if (ib_query_port(priv->ca, priv->port, &attr) ||
 	    attr.state != IB_PORT_ACTIVE) {
 		ipoib_dbg(priv, "Keeping carrier off until IB port is active\n");
 		return;
 	}
 
-	rtnl_lock();
+	/*
+	 * Take rtnl_lock to avoid racing with ipoib_stop() and
+	 * turning the carrier back on while a device is being
+	 * removed.  However, ipoib_stop() will attempt to flush
+	 * the workqueue while holding the rtnl lock, so loop
+	 * on trylock until either we get the lock or we see
+	 * FLAG_ADMIN_UP go away as that signals that we are bailing
+	 * and can safely ignore the carrier on work.
+	 */
+	while (!rtnl_trylock()) {
+		if (!test_bit(IPOIB_FLAG_ADMIN_UP, &priv->flags))
+			return;
+		else
+			msleep(20);
+	}
+	if (!ipoib_cm_admin_enabled(priv->dev))
+		dev_set_mtu(priv->dev, min(priv->mcast_mtu, priv->admin_mtu));
 	netif_carrier_on(priv->dev);
 	rtnl_unlock();
 }
@@ -385,60 +412,63 @@ static int ipoib_mcast_join_complete(int status,
 	ipoib_dbg_mcast(priv, "join completion for %pI6 (status %d)\n",
 			mcast->mcmember.mgid.raw, status);
 
+	/*
+	 * We have to take the mutex to force mcast_join to
+	 * return from ib_sa_multicast_join and set mcast->mc to a
+	 * valid value.  Otherwise we were racing with ourselves in
+	 * that we might fail here, but get a valid return from
+	 * ib_sa_multicast_join after we had cleared mcast->mc here,
+	 * resulting in mis-matched joins and leaves and a deadlock
+	 */
+	mutex_lock(&mcast_mutex);
+
 	/* We trap for port events ourselves. */
-	if (status == -ENETRESET) {
-		status = 0;
+	if (status == -ENETRESET)
 		goto out;
-	}
 
 	if (!status)
 		status = ipoib_mcast_join_finish(mcast, &multicast->rec);
 
 	if (!status) {
 		mcast->backoff = 1;
-		mutex_lock(&mcast_mutex);
 		if (test_bit(IPOIB_MCAST_RUN, &priv->flags))
-			queue_delayed_work(ipoib_workqueue,
-					   &priv->mcast_task, 0);
-		mutex_unlock(&mcast_mutex);
+			queue_delayed_work(priv->wq, &priv->mcast_task, 0);
 
 		/*
-		 * Defer carrier on work to ipoib_workqueue to avoid a
+		 * Defer carrier on work to priv->wq to avoid a
 		 * deadlock on rtnl_lock here.
 		 */
 		if (mcast == priv->broadcast)
-			queue_work(ipoib_workqueue, &priv->carrier_on_task);
-
-		status = 0;
-		goto out;
-	}
-
-	if (mcast->logcount++ < 20) {
-		if (status == -ETIMEDOUT || status == -EAGAIN) {
-			ipoib_dbg_mcast(priv, "multicast join failed for %pI6, status %d\n",
-					mcast->mcmember.mgid.raw, status);
-		} else {
-			ipoib_warn(priv, "multicast join failed for %pI6, status %d\n",
-				   mcast->mcmember.mgid.raw, status);
+			queue_work(priv->wq, &priv->carrier_on_task);
+	} else {
+		if (mcast->logcount++ < 20) {
+			if (status == -ETIMEDOUT || status == -EAGAIN) {
+				ipoib_dbg_mcast(priv, "multicast join failed for %pI6, status %d\n",
+						mcast->mcmember.mgid.raw, status);
+			} else {
+				ipoib_warn(priv, "multicast join failed for %pI6, status %d\n",
+					   mcast->mcmember.mgid.raw, status);
+			}
 		}
-	}
-
-	mcast->backoff *= 2;
-	if (mcast->backoff > IPOIB_MAX_BACKOFF_SECONDS)
-		mcast->backoff = IPOIB_MAX_BACKOFF_SECONDS;
 
-	/* Clear the busy flag so we try again */
-	status = test_and_clear_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags);
-
-	mutex_lock(&mcast_mutex);
+		mcast->backoff *= 2;
+		if (mcast->backoff > IPOIB_MAX_BACKOFF_SECONDS)
+			mcast->backoff = IPOIB_MAX_BACKOFF_SECONDS;
+	}
+out:
 	spin_lock_irq(&priv->lock);
-	if (test_bit(IPOIB_MCAST_RUN, &priv->flags))
-		queue_delayed_work(ipoib_workqueue, &priv->mcast_task,
+	clear_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags);
+	if (status)
+		mcast->mc = NULL;
+	complete(&mcast->done);
+	if (status == -ENETRESET)
+		status = 0;
+	if (status && test_bit(IPOIB_MCAST_RUN, &priv->flags))
+		queue_delayed_work(priv->wq, &priv->mcast_task,
 				   mcast->backoff * HZ);
 	spin_unlock_irq(&priv->lock);
 	mutex_unlock(&mcast_mutex);
-out:
-	complete(&mcast->done);
+
 	return status;
 }
 
@@ -487,10 +517,9 @@ static void ipoib_mcast_join(struct net_device *dev, struct ipoib_mcast *mcast,
 		rec.hop_limit	  = priv->broadcast->mcmember.hop_limit;
 	}
 
-	set_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags);
+	mutex_lock(&mcast_mutex);
 	init_completion(&mcast->done);
-	set_bit(IPOIB_MCAST_JOIN_STARTED, &mcast->flags);
-
+	set_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags);
 	mcast->mc = ib_sa_join_multicast(&ipoib_sa_client, priv->ca, priv->port,
 					 &rec, comp_mask, GFP_KERNEL,
 					 ipoib_mcast_join_complete, mcast);
@@ -504,13 +533,11 @@ static void ipoib_mcast_join(struct net_device *dev, struct ipoib_mcast *mcast,
 		if (mcast->backoff > IPOIB_MAX_BACKOFF_SECONDS)
 			mcast->backoff = IPOIB_MAX_BACKOFF_SECONDS;
 
-		mutex_lock(&mcast_mutex);
 		if (test_bit(IPOIB_MCAST_RUN, &priv->flags))
-			queue_delayed_work(ipoib_workqueue,
-					   &priv->mcast_task,
+			queue_delayed_work(priv->wq, &priv->mcast_task,
 					   mcast->backoff * HZ);
-		mutex_unlock(&mcast_mutex);
 	}
+	mutex_unlock(&mcast_mutex);
 }
 
 void ipoib_mcast_join_task(struct work_struct *work)
@@ -547,8 +574,8 @@ void ipoib_mcast_join_task(struct work_struct *work)
 			ipoib_warn(priv, "failed to allocate broadcast group\n");
 			mutex_lock(&mcast_mutex);
 			if (test_bit(IPOIB_MCAST_RUN, &priv->flags))
-				queue_delayed_work(ipoib_workqueue,
-						   &priv->mcast_task, HZ);
+				queue_delayed_work(priv->wq, &priv->mcast_task,
+						   HZ);
 			mutex_unlock(&mcast_mutex);
 			return;
 		}
@@ -563,7 +590,8 @@ void ipoib_mcast_join_task(struct work_struct *work)
 	}
 
 	if (!test_bit(IPOIB_MCAST_FLAG_ATTACHED, &priv->broadcast->flags)) {
-		if (!test_bit(IPOIB_MCAST_FLAG_BUSY, &priv->broadcast->flags))
+		if (IS_ERR_OR_NULL(priv->broadcast->mc) &&
+		    !test_bit(IPOIB_MCAST_FLAG_BUSY, &priv->broadcast->flags))
 			ipoib_mcast_join(dev, priv->broadcast, 0);
 		return;
 	}
@@ -571,23 +599,33 @@ void ipoib_mcast_join_task(struct work_struct *work)
 	while (1) {
 		struct ipoib_mcast *mcast = NULL;
 
+		/*
+		 * Need the mutex so our flags are consistent, need the
+		 * priv->lock so we don't race with list removals in either
+		 * mcast_dev_flush or mcast_restart_task
+		 */
+		mutex_lock(&mcast_mutex);
 		spin_lock_irq(&priv->lock);
 		list_for_each_entry(mcast, &priv->multicast_list, list) {
-			if (!test_bit(IPOIB_MCAST_FLAG_SENDONLY, &mcast->flags)
-			    && !test_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags)
-			    && !test_bit(IPOIB_MCAST_FLAG_ATTACHED, &mcast->flags)) {
+			if (IS_ERR_OR_NULL(mcast->mc) &&
+			    !test_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags) &&
+			    !test_bit(IPOIB_MCAST_FLAG_ATTACHED, &mcast->flags)) {
 				/* Found the next unjoined group */
 				break;
 			}
 		}
 		spin_unlock_irq(&priv->lock);
+		mutex_unlock(&mcast_mutex);
 
 		if (&mcast->list == &priv->multicast_list) {
 			/* All done */
 			break;
 		}
 
-		ipoib_mcast_join(dev, mcast, 1);
+		if (test_bit(IPOIB_MCAST_FLAG_SENDONLY, &mcast->flags))
+			ipoib_mcast_sendonly_join(mcast);
+		else
+			ipoib_mcast_join(dev, mcast, 1);
 		return;
 	}
 
@@ -604,13 +642,13 @@ int ipoib_mcast_start_thread(struct net_device *dev)
 
 	mutex_lock(&mcast_mutex);
 	if (!test_and_set_bit(IPOIB_MCAST_RUN, &priv->flags))
-		queue_delayed_work(ipoib_workqueue, &priv->mcast_task, 0);
+		queue_delayed_work(priv->wq, &priv->mcast_task, 0);
 	mutex_unlock(&mcast_mutex);
 
 	return 0;
 }
 
-int ipoib_mcast_stop_thread(struct net_device *dev, int flush)
+int ipoib_mcast_stop_thread(struct net_device *dev)
 {
 	struct ipoib_dev_priv *priv = netdev_priv(dev);
 
@@ -621,8 +659,7 @@ int ipoib_mcast_stop_thread(struct net_device *dev, int flush)
 	cancel_delayed_work(&priv->mcast_task);
 	mutex_unlock(&mcast_mutex);
 
-	if (flush)
-		flush_workqueue(ipoib_workqueue);
+	flush_workqueue(priv->wq);
 
 	return 0;
 }
@@ -633,6 +670,9 @@ static int ipoib_mcast_leave(struct net_device *dev, struct ipoib_mcast *mcast)
 	int ret = 0;
 
 	if (test_and_clear_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags))
+		ipoib_warn(priv, "ipoib_mcast_leave on an in-flight join\n");
+
+	if (!IS_ERR_OR_NULL(mcast->mc))
 		ib_sa_free_multicast(mcast->mc);
 
 	if (test_and_clear_bit(IPOIB_MCAST_FLAG_ATTACHED, &mcast->flags)) {
@@ -685,6 +725,8 @@ void ipoib_mcast_send(struct net_device *dev, u8 *daddr, struct sk_buff *skb)
 		memcpy(mcast->mcmember.mgid.raw, mgid, sizeof (union ib_gid));
 		__ipoib_mcast_add(dev, mcast);
 		list_add_tail(&mcast->list, &priv->multicast_list);
+		if (!test_and_set_bit(IPOIB_MCAST_RUN, &priv->flags))
+			queue_delayed_work(priv->wq, &priv->mcast_task, 0);
 	}
 
 	if (!mcast->ah) {
@@ -698,8 +740,6 @@ void ipoib_mcast_send(struct net_device *dev, u8 *daddr, struct sk_buff *skb)
 		if (test_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags))
 			ipoib_dbg_mcast(priv, "no address vector, "
 					"but multicast join already started\n");
-		else if (test_bit(IPOIB_MCAST_FLAG_SENDONLY, &mcast->flags))
-			ipoib_mcast_sendonly_join(mcast);
 
 		/*
 		 * If lookup completes between here and out:, don't
@@ -759,9 +799,12 @@ void ipoib_mcast_dev_flush(struct net_device *dev)
 
 	spin_unlock_irqrestore(&priv->lock, flags);
 
-	/* seperate between the wait to the leave*/
+	/*
+	 * make sure the in-flight joins have finished before we attempt
+	 * to leave
+	 */
 	list_for_each_entry_safe(mcast, tmcast, &remove_list, list)
-		if (test_bit(IPOIB_MCAST_JOIN_STARTED, &mcast->flags))
+		if (test_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags))
 			wait_for_completion(&mcast->done);
 
 	list_for_each_entry_safe(mcast, tmcast, &remove_list, list) {
@@ -794,8 +837,6 @@ void ipoib_mcast_restart_task(struct work_struct *work)
 
 	ipoib_dbg_mcast(priv, "restarting multicast task\n");
 
-	ipoib_mcast_stop_thread(dev, 0);
-
 	local_irq_save(flags);
 	netif_addr_lock(dev);
 	spin_lock(&priv->lock);
@@ -880,14 +921,38 @@ void ipoib_mcast_restart_task(struct work_struct *work)
 	netif_addr_unlock(dev);
 	local_irq_restore(flags);
 
-	/* We have to cancel outside of the spinlock */
+	/*
+	 * make sure the in-flight joins have finished before we attempt
+	 * to leave
+	 */
+	list_for_each_entry_safe(mcast, tmcast, &remove_list, list)
+		if (test_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags))
+			wait_for_completion(&mcast->done);
+
+	/*
+	 * We have to cancel outside of the spinlock, but we have to
+	 * take the rtnl lock or else we race with the removal of
+	 * entries from the remove list in mcast_dev_flush as part
+	 * of ipoib_stop().  We detect the drop of the ADMIN_UP flag
+	 * to signal that we have hit this particular race, and we
+	 * return since we know we don't need to do anything else
+	 * anyway.
+	 */
+	while (!rtnl_trylock()) {
+		if (!test_bit(IPOIB_FLAG_ADMIN_UP, &priv->flags))
+			return;
+		else
+			msleep(20);
+	}
 	list_for_each_entry_safe(mcast, tmcast, &remove_list, list) {
 		ipoib_mcast_leave(mcast->dev, mcast);
 		ipoib_mcast_free(mcast);
 	}
-
-	if (test_bit(IPOIB_FLAG_ADMIN_UP, &priv->flags))
-		ipoib_mcast_start_thread(dev);
+	/*
+	 * Restart our join task if needed
+	 */
+	ipoib_mcast_start_thread(dev);
+	rtnl_unlock();
 }
 
 #ifdef CONFIG_INFINIBAND_IPOIB_DEBUG
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_verbs.c b/drivers/infiniband/ulp/ipoib/ipoib_verbs.c
index c56d5d44c53b..b72a753eb41d 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_verbs.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_verbs.c
@@ -145,10 +145,20 @@ int ipoib_transport_dev_init(struct net_device *dev, struct ib_device *ca)
 	int ret, size;
 	int i;
 
+	/*
+	 * the various IPoIB tasks assume they will never race against
+	 * themselves, so always use a single thread workqueue
+	 */
+	priv->wq = create_singlethread_workqueue("ipoib_wq");
+	if (!priv->wq) {
+		printk(KERN_WARNING "ipoib: failed to allocate device WQ\n");
+		return -ENODEV;
+	}
+
 	priv->pd = ib_alloc_pd(priv->ca);
 	if (IS_ERR(priv->pd)) {
 		printk(KERN_WARNING "%s: failed to allocate PD\n", ca->name);
-		return -ENODEV;
+		goto out_free_wq;
 	}
 
 	priv->mr = ib_get_dma_mr(priv->pd, IB_ACCESS_LOCAL_WRITE);
@@ -242,6 +252,10 @@ out_free_mr:
 
 out_free_pd:
 	ib_dealloc_pd(priv->pd);
+
+out_free_wq:
+	destroy_workqueue(priv->wq);
+	priv->wq = NULL;
 	return -ENODEV;
 }
 
@@ -270,6 +284,12 @@ void ipoib_transport_dev_cleanup(struct net_device *dev)
 
 	if (ib_dealloc_pd(priv->pd))
 		ipoib_warn(priv, "ib_dealloc_pd failed\n");
+
+	if (priv->wq) {
+		flush_workqueue(priv->wq);
+		destroy_workqueue(priv->wq);
+		priv->wq = NULL;
+	}
 }
 
 void ipoib_event(struct ib_event_handler *handler,
diff --git a/drivers/infiniband/ulp/iser/iscsi_iser.c b/drivers/infiniband/ulp/iser/iscsi_iser.c
index 20ca6a619476..6a594aac2290 100644
--- a/drivers/infiniband/ulp/iser/iscsi_iser.c
+++ b/drivers/infiniband/ulp/iser/iscsi_iser.c
@@ -97,7 +97,7 @@ module_param_named(pi_enable, iser_pi_enable, bool, 0644);
 MODULE_PARM_DESC(pi_enable, "Enable T10-PI offload support (default:disabled)");
 
 module_param_named(pi_guard, iser_pi_guard, int, 0644);
-MODULE_PARM_DESC(pi_guard, "T10-PI guard_type, 0:CRC|1:IP_CSUM (default:IP_CSUM)");
+MODULE_PARM_DESC(pi_guard, "T10-PI guard_type [deprecated]");
 
 static struct workqueue_struct *release_wq;
 struct iser_global ig;
@@ -164,18 +164,42 @@ iscsi_iser_pdu_alloc(struct iscsi_task *task, uint8_t opcode)
 	return 0;
 }
 
-int iser_initialize_task_headers(struct iscsi_task *task,
-						struct iser_tx_desc *tx_desc)
+/**
+ * iser_initialize_task_headers() - Initialize task headers
+ * @task:       iscsi task
+ * @tx_desc:    iser tx descriptor
+ *
+ * Notes:
+ * This routine may race with iser teardown flow for scsi
+ * error handling TMFs. So for TMF we should acquire the
+ * state mutex to avoid dereferencing the IB device which
+ * may have already been terminated.
+ */
+int
+iser_initialize_task_headers(struct iscsi_task *task,
+			     struct iser_tx_desc *tx_desc)
 {
-	struct iser_conn       *iser_conn   = task->conn->dd_data;
+	struct iser_conn *iser_conn = task->conn->dd_data;
 	struct iser_device *device = iser_conn->ib_conn.device;
 	struct iscsi_iser_task *iser_task = task->dd_data;
 	u64 dma_addr;
+	const bool mgmt_task = !task->sc && !in_interrupt();
+	int ret = 0;
+
+	if (unlikely(mgmt_task))
+		mutex_lock(&iser_conn->state_mutex);
+
+	if (unlikely(iser_conn->state != ISER_CONN_UP)) {
+		ret = -ENODEV;
+		goto out;
+	}
 
 	dma_addr = ib_dma_map_single(device->ib_device, (void *)tx_desc,
 				ISER_HEADERS_LEN, DMA_TO_DEVICE);
-	if (ib_dma_mapping_error(device->ib_device, dma_addr))
-		return -ENOMEM;
+	if (ib_dma_mapping_error(device->ib_device, dma_addr)) {
+		ret = -ENOMEM;
+		goto out;
+	}
 
 	tx_desc->dma_addr = dma_addr;
 	tx_desc->tx_sg[0].addr   = tx_desc->dma_addr;
@@ -183,7 +207,11 @@ int iser_initialize_task_headers(struct iscsi_task *task,
 	tx_desc->tx_sg[0].lkey   = device->mr->lkey;
 
 	iser_task->iser_conn = iser_conn;
-	return 0;
+out:
+	if (unlikely(mgmt_task))
+		mutex_unlock(&iser_conn->state_mutex);
+
+	return ret;
 }
 
 /**
@@ -199,9 +227,14 @@ static int
 iscsi_iser_task_init(struct iscsi_task *task)
 {
 	struct iscsi_iser_task *iser_task = task->dd_data;
+	int ret;
 
-	if (iser_initialize_task_headers(task, &iser_task->desc))
-			return -ENOMEM;
+	ret = iser_initialize_task_headers(task, &iser_task->desc);
+	if (ret) {
+		iser_err("Failed to init task %p, err = %d\n",
+			 iser_task, ret);
+		return ret;
+	}
 
 	/* mgmt task */
 	if (!task->sc)
@@ -508,8 +541,8 @@ iscsi_iser_conn_stop(struct iscsi_cls_conn *cls_conn, int flag)
 	 */
 	if (iser_conn) {
 		mutex_lock(&iser_conn->state_mutex);
-		iscsi_conn_stop(cls_conn, flag);
 		iser_conn_terminate(iser_conn);
+		iscsi_conn_stop(cls_conn, flag);
 
 		/* unbind */
 		iser_conn->iscsi_conn = NULL;
@@ -541,12 +574,13 @@ iscsi_iser_session_destroy(struct iscsi_cls_session *cls_session)
 static inline unsigned int
 iser_dif_prot_caps(int prot_caps)
 {
-	return ((prot_caps & IB_PROT_T10DIF_TYPE_1) ? SHOST_DIF_TYPE1_PROTECTION |
-						      SHOST_DIX_TYPE1_PROTECTION : 0) |
-	       ((prot_caps & IB_PROT_T10DIF_TYPE_2) ? SHOST_DIF_TYPE2_PROTECTION |
-						      SHOST_DIX_TYPE2_PROTECTION : 0) |
-	       ((prot_caps & IB_PROT_T10DIF_TYPE_3) ? SHOST_DIF_TYPE3_PROTECTION |
-						      SHOST_DIX_TYPE3_PROTECTION : 0);
+	return ((prot_caps & IB_PROT_T10DIF_TYPE_1) ?
+		SHOST_DIF_TYPE1_PROTECTION | SHOST_DIX_TYPE0_PROTECTION |
+		SHOST_DIX_TYPE1_PROTECTION : 0) |
+	       ((prot_caps & IB_PROT_T10DIF_TYPE_2) ?
+		SHOST_DIF_TYPE2_PROTECTION | SHOST_DIX_TYPE2_PROTECTION : 0) |
+	       ((prot_caps & IB_PROT_T10DIF_TYPE_3) ?
+		SHOST_DIF_TYPE3_PROTECTION | SHOST_DIX_TYPE3_PROTECTION : 0);
 }
 
 /**
@@ -569,6 +603,7 @@ iscsi_iser_session_create(struct iscsi_endpoint *ep,
 	struct Scsi_Host *shost;
 	struct iser_conn *iser_conn = NULL;
 	struct ib_conn *ib_conn;
+	u16 max_cmds;
 
 	shost = iscsi_host_alloc(&iscsi_iser_sht, 0, 0);
 	if (!shost)
@@ -586,26 +621,41 @@ iscsi_iser_session_create(struct iscsi_endpoint *ep,
 	 */
 	if (ep) {
 		iser_conn = ep->dd_data;
+		max_cmds = iser_conn->max_cmds;
+
+		mutex_lock(&iser_conn->state_mutex);
+		if (iser_conn->state != ISER_CONN_UP) {
+			iser_err("iser conn %p already started teardown\n",
+				 iser_conn);
+			mutex_unlock(&iser_conn->state_mutex);
+			goto free_host;
+		}
+
 		ib_conn = &iser_conn->ib_conn;
 		if (ib_conn->pi_support) {
 			u32 sig_caps = ib_conn->device->dev_attr.sig_prot_cap;
 
 			scsi_host_set_prot(shost, iser_dif_prot_caps(sig_caps));
-			if (iser_pi_guard)
-				scsi_host_set_guard(shost, SHOST_DIX_GUARD_IP);
-			else
-				scsi_host_set_guard(shost, SHOST_DIX_GUARD_CRC);
+			scsi_host_set_guard(shost, SHOST_DIX_GUARD_IP |
+						   SHOST_DIX_GUARD_CRC);
 		}
-	}
 
-	if (iscsi_host_add(shost, ep ?
-			   ib_conn->device->ib_device->dma_device : NULL))
-		goto free_host;
+		if (iscsi_host_add(shost,
+				   ib_conn->device->ib_device->dma_device)) {
+			mutex_unlock(&iser_conn->state_mutex);
+			goto free_host;
+		}
+		mutex_unlock(&iser_conn->state_mutex);
+	} else {
+		max_cmds = ISER_DEF_XMIT_CMDS_MAX;
+		if (iscsi_host_add(shost, NULL))
+			goto free_host;
+	}
 
-	if (cmds_max > ISER_DEF_XMIT_CMDS_MAX) {
+	if (cmds_max > max_cmds) {
 		iser_info("cmds_max changed from %u to %u\n",
-			  cmds_max, ISER_DEF_XMIT_CMDS_MAX);
-		cmds_max = ISER_DEF_XMIT_CMDS_MAX;
+			  cmds_max, max_cmds);
+		cmds_max = max_cmds;
 	}
 
 	cls_session = iscsi_session_setup(&iscsi_iser_transport, shost,
diff --git a/drivers/infiniband/ulp/iser/iscsi_iser.h b/drivers/infiniband/ulp/iser/iscsi_iser.h
index cd4174ca9a76..5ce26817e7e1 100644
--- a/drivers/infiniband/ulp/iser/iscsi_iser.h
+++ b/drivers/infiniband/ulp/iser/iscsi_iser.h
@@ -69,34 +69,31 @@
 
 #define DRV_NAME	"iser"
 #define PFX		DRV_NAME ": "
-#define DRV_VER		"1.4.8"
+#define DRV_VER		"1.5"
 
 #define iser_dbg(fmt, arg...)				 \
 	do {						 \
-		if (iser_debug_level > 2)		 \
+		if (unlikely(iser_debug_level > 2))	 \
 			printk(KERN_DEBUG PFX "%s: " fmt,\
 				__func__ , ## arg);	 \
 	} while (0)
 
 #define iser_warn(fmt, arg...)				\
 	do {						\
-		if (iser_debug_level > 0)		\
+		if (unlikely(iser_debug_level > 0))	\
 			pr_warn(PFX "%s: " fmt,		\
 				__func__ , ## arg);	\
 	} while (0)
 
 #define iser_info(fmt, arg...)				\
 	do {						\
-		if (iser_debug_level > 1)		\
+		if (unlikely(iser_debug_level > 1))	\
 			pr_info(PFX "%s: " fmt,		\
 				__func__ , ## arg);	\
 	} while (0)
 
-#define iser_err(fmt, arg...)				\
-	do {						\
-		printk(KERN_ERR PFX "%s: " fmt,		\
-		       __func__ , ## arg);		\
-	} while (0)
+#define iser_err(fmt, arg...) \
+	pr_err(PFX "%s: " fmt, __func__ , ## arg)
 
 #define SHIFT_4K	12
 #define SIZE_4K	(1ULL << SHIFT_4K)
@@ -144,6 +141,11 @@
 					ISER_MAX_TX_MISC_PDUS         + \
 					ISER_MAX_RX_MISC_PDUS)
 
+#define ISER_GET_MAX_XMIT_CMDS(send_wr) ((send_wr			\
+					 - ISER_MAX_TX_MISC_PDUS	\
+					 - ISER_MAX_RX_MISC_PDUS) /	\
+					 (1 + ISER_INFLIGHT_DATAOUTS))
+
 #define ISER_WC_BATCH_COUNT   16
 #define ISER_SIGNAL_CMD_COUNT 32
 
@@ -247,7 +249,6 @@ struct iscsi_endpoint;
  * @va:           MR start address (buffer va)
  * @len:          MR length
  * @mem_h:        pointer to registration context (FMR/Fastreg)
- * @is_mr:        indicates weather we registered the buffer
  */
 struct iser_mem_reg {
 	u32  lkey;
@@ -255,7 +256,6 @@ struct iser_mem_reg {
 	u64  va;
 	u64  len;
 	void *mem_h;
-	int  is_mr;
 };
 
 /**
@@ -323,8 +323,6 @@ struct iser_rx_desc {
 	char		             pad[ISER_RX_PAD_SIZE];
 } __attribute__((packed));
 
-#define ISER_MAX_CQ 4
-
 struct iser_conn;
 struct ib_conn;
 struct iscsi_iser_task;
@@ -375,7 +373,7 @@ struct iser_device {
 	struct list_head             ig_list;
 	int                          refcount;
 	int			     comps_used;
-	struct iser_comp	     comps[ISER_MAX_CQ];
+	struct iser_comp	     *comps;
 	int                          (*iser_alloc_rdma_reg_res)(struct ib_conn *ib_conn,
 								unsigned cmds_max);
 	void                         (*iser_free_rdma_reg_res)(struct ib_conn *ib_conn);
@@ -432,6 +430,7 @@ struct fast_reg_descriptor {
  * @cma_id:              rdma_cm connection maneger handle
  * @qp:                  Connection Queue-pair
  * @post_recv_buf_count: post receive counter
+ * @sig_count:           send work request signal count
  * @rx_wr:               receive work request for batch posts
  * @device:              reference to iser device
  * @comp:                iser completion context
@@ -452,6 +451,7 @@ struct ib_conn {
 	struct rdma_cm_id           *cma_id;
 	struct ib_qp	            *qp;
 	int                          post_recv_buf_count;
+	u8                           sig_count;
 	struct ib_recv_wr	     rx_wr[ISER_MIN_POSTED_RX];
 	struct iser_device          *device;
 	struct iser_comp	    *comp;
@@ -482,6 +482,7 @@ struct ib_conn {
  *                    to max number of post recvs
  * @qp_max_recv_dtos_mask: (qp_max_recv_dtos - 1)
  * @min_posted_rx:    (qp_max_recv_dtos >> 2)
+ * @max_cmds:         maximum cmds allowed for this connection
  * @name:             connection peer portal
  * @release_work:     deffered work for release job
  * @state_mutex:      protects iser onnection state
@@ -507,6 +508,7 @@ struct iser_conn {
 	unsigned		     qp_max_recv_dtos;
 	unsigned		     qp_max_recv_dtos_mask;
 	unsigned		     min_posted_rx;
+	u16                          max_cmds;
 	char 			     name[ISER_OBJECT_NAME_SIZE];
 	struct work_struct	     release_work;
 	struct mutex		     state_mutex;
diff --git a/drivers/infiniband/ulp/iser/iser_initiator.c b/drivers/infiniband/ulp/iser/iser_initiator.c
index 5a489ea63732..3821633f1065 100644
--- a/drivers/infiniband/ulp/iser/iser_initiator.c
+++ b/drivers/infiniband/ulp/iser/iser_initiator.c
@@ -369,7 +369,7 @@ static int iser_post_rx_bufs(struct iscsi_conn *conn, struct iscsi_hdr *req)
 	return 0;
 }
 
-static inline bool iser_signal_comp(int sig_count)
+static inline bool iser_signal_comp(u8 sig_count)
 {
 	return ((sig_count % ISER_SIGNAL_CMD_COUNT) == 0);
 }
@@ -388,7 +388,7 @@ int iser_send_command(struct iscsi_conn *conn,
 	struct iscsi_scsi_req *hdr = (struct iscsi_scsi_req *)task->hdr;
 	struct scsi_cmnd *sc  =  task->sc;
 	struct iser_tx_desc *tx_desc = &iser_task->desc;
-	static unsigned sig_count;
+	u8 sig_count = ++iser_conn->ib_conn.sig_count;
 
 	edtl = ntohl(hdr->data_length);
 
@@ -435,7 +435,7 @@ int iser_send_command(struct iscsi_conn *conn,
 	iser_task->status = ISER_TASK_STATUS_STARTED;
 
 	err = iser_post_send(&iser_conn->ib_conn, tx_desc,
-			     iser_signal_comp(++sig_count));
+			     iser_signal_comp(sig_count));
 	if (!err)
 		return 0;
 
diff --git a/drivers/infiniband/ulp/iser/iser_memory.c b/drivers/infiniband/ulp/iser/iser_memory.c
index 6c5ce357fba6..abce9339333f 100644
--- a/drivers/infiniband/ulp/iser/iser_memory.c
+++ b/drivers/infiniband/ulp/iser/iser_memory.c
@@ -73,7 +73,6 @@ static int iser_start_rdma_unaligned_sg(struct iscsi_iser_task *iser_task,
 
 	if (cmd_dir == ISER_DIR_OUT) {
 		/* copy the unaligned sg the buffer which is used for RDMA */
-		int i;
 		char *p, *from;
 
 		sgl = (struct scatterlist *)data->buf;
@@ -409,7 +408,6 @@ int iser_reg_rdma_mem_fmr(struct iscsi_iser_task *iser_task,
 		regd_buf->reg.rkey = device->mr->rkey;
 		regd_buf->reg.len  = ib_sg_dma_len(ibdev, &sg[0]);
 		regd_buf->reg.va   = ib_sg_dma_address(ibdev, &sg[0]);
-		regd_buf->reg.is_mr = 0;
 
 		iser_dbg("PHYSICAL Mem.register: lkey: 0x%08X rkey: 0x%08X  "
 			 "va: 0x%08lX sz: %ld]\n",
@@ -440,13 +438,13 @@ int iser_reg_rdma_mem_fmr(struct iscsi_iser_task *iser_task,
 	return 0;
 }
 
-static inline void
+static void
 iser_set_dif_domain(struct scsi_cmnd *sc, struct ib_sig_attrs *sig_attrs,
 		    struct ib_sig_domain *domain)
 {
 	domain->sig_type = IB_SIG_TYPE_T10_DIF;
-	domain->sig.dif.pi_interval = sc->device->sector_size;
-	domain->sig.dif.ref_tag = scsi_get_lba(sc) & 0xffffffff;
+	domain->sig.dif.pi_interval = scsi_prot_interval(sc);
+	domain->sig.dif.ref_tag = scsi_prot_ref_tag(sc);
 	/*
 	 * At the moment we hard code those, but in the future
 	 * we will take them from sc.
@@ -454,8 +452,7 @@ iser_set_dif_domain(struct scsi_cmnd *sc, struct ib_sig_attrs *sig_attrs,
 	domain->sig.dif.apptag_check_mask = 0xffff;
 	domain->sig.dif.app_escape = true;
 	domain->sig.dif.ref_escape = true;
-	if (scsi_get_prot_type(sc) == SCSI_PROT_DIF_TYPE1 ||
-	    scsi_get_prot_type(sc) == SCSI_PROT_DIF_TYPE2)
+	if (sc->prot_flags & SCSI_PROT_REF_INCREMENT)
 		domain->sig.dif.ref_remap = true;
 };
 
@@ -473,26 +470,16 @@ iser_set_sig_attrs(struct scsi_cmnd *sc, struct ib_sig_attrs *sig_attrs)
 	case SCSI_PROT_WRITE_STRIP:
 		sig_attrs->wire.sig_type = IB_SIG_TYPE_NONE;
 		iser_set_dif_domain(sc, sig_attrs, &sig_attrs->mem);
-		/*
-		 * At the moment we use this modparam to tell what is
-		 * the memory bg_type, in the future we will take it
-		 * from sc.
-		 */
-		sig_attrs->mem.sig.dif.bg_type = iser_pi_guard ? IB_T10DIF_CSUM :
-						 IB_T10DIF_CRC;
+		sig_attrs->mem.sig.dif.bg_type = sc->prot_flags & SCSI_PROT_IP_CHECKSUM ?
+						IB_T10DIF_CSUM : IB_T10DIF_CRC;
 		break;
 	case SCSI_PROT_READ_PASS:
 	case SCSI_PROT_WRITE_PASS:
 		iser_set_dif_domain(sc, sig_attrs, &sig_attrs->wire);
 		sig_attrs->wire.sig.dif.bg_type = IB_T10DIF_CRC;
 		iser_set_dif_domain(sc, sig_attrs, &sig_attrs->mem);
-		/*
-		 * At the moment we use this modparam to tell what is
-		 * the memory bg_type, in the future we will take it
-		 * from sc.
-		 */
-		sig_attrs->mem.sig.dif.bg_type = iser_pi_guard ? IB_T10DIF_CSUM :
-						 IB_T10DIF_CRC;
+		sig_attrs->mem.sig.dif.bg_type = sc->prot_flags & SCSI_PROT_IP_CHECKSUM ?
+						IB_T10DIF_CSUM : IB_T10DIF_CRC;
 		break;
 	default:
 		iser_err("Unsupported PI operation %d\n",
@@ -503,26 +490,28 @@ iser_set_sig_attrs(struct scsi_cmnd *sc, struct ib_sig_attrs *sig_attrs)
 	return 0;
 }
 
-static int
+static inline void
 iser_set_prot_checks(struct scsi_cmnd *sc, u8 *mask)
 {
-	switch (scsi_get_prot_type(sc)) {
-	case SCSI_PROT_DIF_TYPE0:
-		break;
-	case SCSI_PROT_DIF_TYPE1:
-	case SCSI_PROT_DIF_TYPE2:
-		*mask = ISER_CHECK_GUARD | ISER_CHECK_REFTAG;
-		break;
-	case SCSI_PROT_DIF_TYPE3:
-		*mask = ISER_CHECK_GUARD;
-		break;
-	default:
-		iser_err("Unsupported protection type %d\n",
-			 scsi_get_prot_type(sc));
-		return -EINVAL;
-	}
+	*mask = 0;
+	if (sc->prot_flags & SCSI_PROT_REF_CHECK)
+		*mask |= ISER_CHECK_REFTAG;
+	if (sc->prot_flags & SCSI_PROT_GUARD_CHECK)
+		*mask |= ISER_CHECK_GUARD;
+}
 
-	return 0;
+static void
+iser_inv_rkey(struct ib_send_wr *inv_wr, struct ib_mr *mr)
+{
+	u32 rkey;
+
+	memset(inv_wr, 0, sizeof(*inv_wr));
+	inv_wr->opcode = IB_WR_LOCAL_INV;
+	inv_wr->wr_id = ISER_FASTREG_LI_WRID;
+	inv_wr->ex.invalidate_rkey = mr->rkey;
+
+	rkey = ib_inc_rkey(mr->rkey);
+	ib_update_fast_reg_key(mr, rkey);
 }
 
 static int
@@ -536,26 +525,17 @@ iser_reg_sig_mr(struct iscsi_iser_task *iser_task,
 	struct ib_send_wr *bad_wr, *wr = NULL;
 	struct ib_sig_attrs sig_attrs;
 	int ret;
-	u32 key;
 
 	memset(&sig_attrs, 0, sizeof(sig_attrs));
 	ret = iser_set_sig_attrs(iser_task->sc, &sig_attrs);
 	if (ret)
 		goto err;
 
-	ret = iser_set_prot_checks(iser_task->sc, &sig_attrs.check_mask);
-	if (ret)
-		goto err;
+	iser_set_prot_checks(iser_task->sc, &sig_attrs.check_mask);
 
 	if (!(desc->reg_indicators & ISER_SIG_KEY_VALID)) {
-		memset(&inv_wr, 0, sizeof(inv_wr));
-		inv_wr.opcode = IB_WR_LOCAL_INV;
-		inv_wr.wr_id = ISER_FASTREG_LI_WRID;
-		inv_wr.ex.invalidate_rkey = pi_ctx->sig_mr->rkey;
+		iser_inv_rkey(&inv_wr, pi_ctx->sig_mr);
 		wr = &inv_wr;
-		/* Bump the key */
-		key = (u8)(pi_ctx->sig_mr->rkey & 0x000000FF);
-		ib_update_fast_reg_key(pi_ctx->sig_mr, ++key);
 	}
 
 	memset(&sig_wr, 0, sizeof(sig_wr));
@@ -585,12 +565,7 @@ iser_reg_sig_mr(struct iscsi_iser_task *iser_task,
 
 	sig_sge->lkey = pi_ctx->sig_mr->lkey;
 	sig_sge->addr = 0;
-	sig_sge->length = data_sge->length + prot_sge->length;
-	if (scsi_get_prot_op(iser_task->sc) == SCSI_PROT_WRITE_INSERT ||
-	    scsi_get_prot_op(iser_task->sc) == SCSI_PROT_READ_STRIP) {
-		sig_sge->length += (data_sge->length /
-				   iser_task->sc->device->sector_size) * 8;
-	}
+	sig_sge->length = scsi_transfer_length(iser_task->sc);
 
 	iser_dbg("sig_sge: addr: 0x%llx  length: %u lkey: 0x%x\n",
 		 sig_sge->addr, sig_sge->length,
@@ -613,7 +588,6 @@ static int iser_fast_reg_mr(struct iscsi_iser_task *iser_task,
 	struct ib_fast_reg_page_list *frpl;
 	struct ib_send_wr fastreg_wr, inv_wr;
 	struct ib_send_wr *bad_wr, *wr = NULL;
-	u8 key;
 	int ret, offset, size, plen;
 
 	/* if there a single dma entry, dma mr suffices */
@@ -645,14 +619,8 @@ static int iser_fast_reg_mr(struct iscsi_iser_task *iser_task,
 	}
 
 	if (!(desc->reg_indicators & ind)) {
-		memset(&inv_wr, 0, sizeof(inv_wr));
-		inv_wr.wr_id = ISER_FASTREG_LI_WRID;
-		inv_wr.opcode = IB_WR_LOCAL_INV;
-		inv_wr.ex.invalidate_rkey = mr->rkey;
+		iser_inv_rkey(&inv_wr, mr);
 		wr = &inv_wr;
-		/* Bump the key */
-		key = (u8)(mr->rkey & 0x000000FF);
-		ib_update_fast_reg_key(mr, ++key);
 	}
 
 	/* Prepare FASTREG WR */
@@ -770,15 +738,11 @@ int iser_reg_rdma_mem_fastreg(struct iscsi_iser_task *iser_task,
 		regd_buf->reg.rkey = desc->pi_ctx->sig_mr->rkey;
 		regd_buf->reg.va = sig_sge.addr;
 		regd_buf->reg.len = sig_sge.length;
-		regd_buf->reg.is_mr = 1;
 	} else {
-		if (desc) {
+		if (desc)
 			regd_buf->reg.rkey = desc->data_mr->rkey;
-			regd_buf->reg.is_mr = 1;
-		} else {
+		else
 			regd_buf->reg.rkey = device->mr->rkey;
-			regd_buf->reg.is_mr = 0;
-		}
 
 		regd_buf->reg.lkey = data_sge.lkey;
 		regd_buf->reg.va = data_sge.addr;
diff --git a/drivers/infiniband/ulp/iser/iser_verbs.c b/drivers/infiniband/ulp/iser/iser_verbs.c
index 67225bb82bb5..695a2704bd43 100644
--- a/drivers/infiniband/ulp/iser/iser_verbs.c
+++ b/drivers/infiniband/ulp/iser/iser_verbs.c
@@ -76,7 +76,7 @@ static void iser_event_handler(struct ib_event_handler *handler,
 static int iser_create_device_ib_res(struct iser_device *device)
 {
 	struct ib_device_attr *dev_attr = &device->dev_attr;
-	int ret, i;
+	int ret, i, max_cqe;
 
 	ret = ib_query_device(device->ib_device, dev_attr);
 	if (ret) {
@@ -104,11 +104,19 @@ static int iser_create_device_ib_res(struct iser_device *device)
 		return -1;
 	}
 
-	device->comps_used = min(ISER_MAX_CQ,
+	device->comps_used = min_t(int, num_online_cpus(),
 				 device->ib_device->num_comp_vectors);
-	iser_info("using %d CQs, device %s supports %d vectors\n",
+
+	device->comps = kcalloc(device->comps_used, sizeof(*device->comps),
+				GFP_KERNEL);
+	if (!device->comps)
+		goto comps_err;
+
+	max_cqe = min(ISER_MAX_CQ_LEN, dev_attr->max_cqe);
+
+	iser_info("using %d CQs, device %s supports %d vectors max_cqe %d\n",
 		  device->comps_used, device->ib_device->name,
-		  device->ib_device->num_comp_vectors);
+		  device->ib_device->num_comp_vectors, max_cqe);
 
 	device->pd = ib_alloc_pd(device->ib_device);
 	if (IS_ERR(device->pd))
@@ -122,7 +130,7 @@ static int iser_create_device_ib_res(struct iser_device *device)
 					iser_cq_callback,
 					iser_cq_event_callback,
 					(void *)comp,
-					ISER_MAX_CQ_LEN, i);
+					max_cqe, i);
 		if (IS_ERR(comp->cq)) {
 			comp->cq = NULL;
 			goto cq_err;
@@ -162,6 +170,8 @@ cq_err:
 	}
 	ib_dealloc_pd(device->pd);
 pd_err:
+	kfree(device->comps);
+comps_err:
 	iser_err("failed to allocate an IB resource\n");
 	return -1;
 }
@@ -187,6 +197,9 @@ static void iser_free_device_ib_res(struct iser_device *device)
 	(void)ib_dereg_mr(device->mr);
 	(void)ib_dealloc_pd(device->pd);
 
+	kfree(device->comps);
+	device->comps = NULL;
+
 	device->mr = NULL;
 	device->pd = NULL;
 }
@@ -425,7 +438,10 @@ void iser_free_fastreg_pool(struct ib_conn *ib_conn)
  */
 static int iser_create_ib_conn_res(struct ib_conn *ib_conn)
 {
+	struct iser_conn *iser_conn = container_of(ib_conn, struct iser_conn,
+						   ib_conn);
 	struct iser_device	*device;
+	struct ib_device_attr *dev_attr;
 	struct ib_qp_init_attr	init_attr;
 	int			ret = -ENOMEM;
 	int index, min_index = 0;
@@ -433,6 +449,7 @@ static int iser_create_ib_conn_res(struct ib_conn *ib_conn)
 	BUG_ON(ib_conn->device == NULL);
 
 	device = ib_conn->device;
+	dev_attr = &device->dev_attr;
 
 	memset(&init_attr, 0, sizeof init_attr);
 
@@ -460,8 +477,20 @@ static int iser_create_ib_conn_res(struct ib_conn *ib_conn)
 	if (ib_conn->pi_support) {
 		init_attr.cap.max_send_wr = ISER_QP_SIG_MAX_REQ_DTOS + 1;
 		init_attr.create_flags |= IB_QP_CREATE_SIGNATURE_EN;
+		iser_conn->max_cmds =
+			ISER_GET_MAX_XMIT_CMDS(ISER_QP_SIG_MAX_REQ_DTOS);
 	} else {
-		init_attr.cap.max_send_wr  = ISER_QP_MAX_REQ_DTOS + 1;
+		if (dev_attr->max_qp_wr > ISER_QP_MAX_REQ_DTOS) {
+			init_attr.cap.max_send_wr  = ISER_QP_MAX_REQ_DTOS + 1;
+			iser_conn->max_cmds =
+				ISER_GET_MAX_XMIT_CMDS(ISER_QP_MAX_REQ_DTOS);
+		} else {
+			init_attr.cap.max_send_wr = dev_attr->max_qp_wr;
+			iser_conn->max_cmds =
+				ISER_GET_MAX_XMIT_CMDS(dev_attr->max_qp_wr);
+			iser_dbg("device %s supports max_send_wr %d\n",
+				 device->ib_device->name, dev_attr->max_qp_wr);
+		}
 	}
 
 	ret = rdma_create_qp(ib_conn->cma_id, device->pd, &init_attr);
@@ -475,7 +504,11 @@ static int iser_create_ib_conn_res(struct ib_conn *ib_conn)
 	return ret;
 
 out_err:
+	mutex_lock(&ig.connlist_mutex);
+	ib_conn->comp->active_qps--;
+	mutex_unlock(&ig.connlist_mutex);
 	iser_err("unable to alloc mem or create resource, err %d\n", ret);
+
 	return ret;
 }
 
@@ -610,9 +643,11 @@ void iser_conn_release(struct iser_conn *iser_conn)
 	mutex_unlock(&ig.connlist_mutex);
 
 	mutex_lock(&iser_conn->state_mutex);
-	if (iser_conn->state != ISER_CONN_DOWN)
+	if (iser_conn->state != ISER_CONN_DOWN) {
 		iser_warn("iser conn %p state %d, expected state down.\n",
 			  iser_conn, iser_conn->state);
+		iser_conn->state = ISER_CONN_DOWN;
+	}
 	/*
 	 * In case we never got to bind stage, we still need to
 	 * release IB resources (which is safe to call more than once).
@@ -662,8 +697,10 @@ int iser_conn_terminate(struct iser_conn *iser_conn)
 
 		/* post an indication that all flush errors were consumed */
 		err = ib_post_send(ib_conn->qp, &ib_conn->beacon, &bad_wr);
-		if (err)
+		if (err) {
 			iser_err("conn %p failed to post beacon", ib_conn);
+			return 1;
+		}
 
 		wait_for_completion(&ib_conn->flush_comp);
 	}
@@ -846,20 +883,21 @@ static int iser_cma_handler(struct rdma_cm_id *cma_id, struct rdma_cm_event *eve
 		break;
 	case RDMA_CM_EVENT_DISCONNECTED:
 	case RDMA_CM_EVENT_ADDR_CHANGE:
-		iser_disconnected_handler(cma_id);
+	case RDMA_CM_EVENT_TIMEWAIT_EXIT:
+		iser_cleanup_handler(cma_id, false);
 		break;
 	case RDMA_CM_EVENT_DEVICE_REMOVAL:
 		/*
 		 * we *must* destroy the device as we cannot rely
 		 * on iscsid to be around to initiate error handling.
-		 * also implicitly destroy the cma_id.
+		 * also if we are not in state DOWN implicitly destroy
+		 * the cma_id.
 		 */
 		iser_cleanup_handler(cma_id, true);
-		iser_conn->ib_conn.cma_id = NULL;
-		ret = 1;
-		break;
-	case RDMA_CM_EVENT_TIMEWAIT_EXIT:
-		iser_cleanup_handler(cma_id, false);
+		if (iser_conn->state != ISER_CONN_DOWN) {
+			iser_conn->ib_conn.cma_id = NULL;
+			ret = 1;
+		}
 		break;
 	default:
 		iser_err("Unexpected RDMA CM event (%d)\n", event->event);
@@ -981,7 +1019,6 @@ int iser_reg_page_vec(struct ib_conn *ib_conn,
 	mem_reg->rkey  = mem->fmr->rkey;
 	mem_reg->len   = page_vec->length * SIZE_4K;
 	mem_reg->va    = io_addr;
-	mem_reg->is_mr = 1;
 	mem_reg->mem_h = (void *)mem;
 
 	mem_reg->va   += page_vec->offset;
@@ -1008,7 +1045,7 @@ void iser_unreg_mem_fmr(struct iscsi_iser_task *iser_task,
 	struct iser_mem_reg *reg = &iser_task->rdma_regd[cmd_dir].reg;
 	int ret;
 
-	if (!reg->is_mr)
+	if (!reg->mem_h)
 		return;
 
 	iser_dbg("PHYSICAL Mem.Unregister mem_h %p\n",reg->mem_h);
@@ -1028,11 +1065,10 @@ void iser_unreg_mem_fastreg(struct iscsi_iser_task *iser_task,
 	struct ib_conn *ib_conn = &iser_conn->ib_conn;
 	struct fast_reg_descriptor *desc = reg->mem_h;
 
-	if (!reg->is_mr)
+	if (!desc)
 		return;
 
 	reg->mem_h = NULL;
-	reg->is_mr = 0;
 	spin_lock_bh(&ib_conn->lock);
 	list_add_tail(&desc->list, &ib_conn->fastreg.pool);
 	spin_unlock_bh(&ib_conn->lock);
@@ -1049,7 +1085,7 @@ int iser_post_recvl(struct iser_conn *iser_conn)
 	sge.length = ISER_RX_LOGIN_SIZE;
 	sge.lkey   = ib_conn->device->mr->lkey;
 
-	rx_wr.wr_id   = (unsigned long)iser_conn->login_resp_buf;
+	rx_wr.wr_id   = (uintptr_t)iser_conn->login_resp_buf;
 	rx_wr.sg_list = &sge;
 	rx_wr.num_sge = 1;
 	rx_wr.next    = NULL;
@@ -1073,7 +1109,7 @@ int iser_post_recvm(struct iser_conn *iser_conn, int count)
 
 	for (rx_wr = ib_conn->rx_wr, i = 0; i < count; i++, rx_wr++) {
 		rx_desc		= &iser_conn->rx_descs[my_rx_head];
-		rx_wr->wr_id	= (unsigned long)rx_desc;
+		rx_wr->wr_id	= (uintptr_t)rx_desc;
 		rx_wr->sg_list	= &rx_desc->rx_sg;
 		rx_wr->num_sge	= 1;
 		rx_wr->next	= rx_wr + 1;
@@ -1110,7 +1146,7 @@ int iser_post_send(struct ib_conn *ib_conn, struct iser_tx_desc *tx_desc,
 				      DMA_TO_DEVICE);
 
 	send_wr.next	   = NULL;
-	send_wr.wr_id	   = (unsigned long)tx_desc;
+	send_wr.wr_id	   = (uintptr_t)tx_desc;
 	send_wr.sg_list	   = tx_desc->tx_sg;
 	send_wr.num_sge	   = tx_desc->num_sge;
 	send_wr.opcode	   = IB_WR_SEND;
@@ -1160,6 +1196,7 @@ static void
 iser_handle_comp_error(struct ib_conn *ib_conn,
 		       struct ib_wc *wc)
 {
+	void *wr_id = (void *)(uintptr_t)wc->wr_id;
 	struct iser_conn *iser_conn = container_of(ib_conn, struct iser_conn,
 						   ib_conn);
 
@@ -1168,8 +1205,8 @@ iser_handle_comp_error(struct ib_conn *ib_conn,
 			iscsi_conn_failure(iser_conn->iscsi_conn,
 					   ISCSI_ERR_CONN_FAILED);
 
-	if (is_iser_tx_desc(iser_conn, (void *)wc->wr_id)) {
-		struct iser_tx_desc *desc = (struct iser_tx_desc *)wc->wr_id;
+	if (is_iser_tx_desc(iser_conn, wr_id)) {
+		struct iser_tx_desc *desc = wr_id;
 
 		if (desc->type == ISCSI_TX_DATAOUT)
 			kmem_cache_free(ig.desc_cache, desc);
@@ -1193,14 +1230,14 @@ static void iser_handle_wc(struct ib_wc *wc)
 	struct iser_rx_desc *rx_desc;
 
 	ib_conn = wc->qp->qp_context;
-	if (wc->status == IB_WC_SUCCESS) {
+	if (likely(wc->status == IB_WC_SUCCESS)) {
 		if (wc->opcode == IB_WC_RECV) {
-			rx_desc = (struct iser_rx_desc *)wc->wr_id;
+			rx_desc = (struct iser_rx_desc *)(uintptr_t)wc->wr_id;
 			iser_rcv_completion(rx_desc, wc->byte_len,
 					    ib_conn);
 		} else
 		if (wc->opcode == IB_WC_SEND) {
-			tx_desc = (struct iser_tx_desc *)wc->wr_id;
+			tx_desc = (struct iser_tx_desc *)(uintptr_t)wc->wr_id;
 			iser_snd_completion(tx_desc, ib_conn);
 		} else {
 			iser_err("Unknown wc opcode %d\n", wc->opcode);
diff --git a/drivers/infiniband/ulp/isert/ib_isert.c b/drivers/infiniband/ulp/isert/ib_isert.c
index 10641b7816f4..dafb3c531f96 100644
--- a/drivers/infiniband/ulp/isert/ib_isert.c
+++ b/drivers/infiniband/ulp/isert/ib_isert.c
@@ -22,7 +22,6 @@
 #include <linux/socket.h>
 #include <linux/in.h>
 #include <linux/in6.h>
-#include <linux/llist.h>
 #include <rdma/ib_verbs.h>
 #include <rdma/rdma_cm.h>
 #include <target/target_core_base.h>
@@ -36,11 +35,17 @@
 #define	ISERT_MAX_CONN		8
 #define ISER_MAX_RX_CQ_LEN	(ISERT_QP_MAX_RECV_DTOS * ISERT_MAX_CONN)
 #define ISER_MAX_TX_CQ_LEN	(ISERT_QP_MAX_REQ_DTOS  * ISERT_MAX_CONN)
+#define ISER_MAX_CQ_LEN		(ISER_MAX_RX_CQ_LEN + ISER_MAX_TX_CQ_LEN + \
+				 ISERT_MAX_CONN)
+
+int isert_debug_level = 0;
+module_param_named(debug_level, isert_debug_level, int, 0644);
+MODULE_PARM_DESC(debug_level, "Enable debug tracing if > 0 (default:0)");
 
 static DEFINE_MUTEX(device_list_mutex);
 static LIST_HEAD(device_list);
-static struct workqueue_struct *isert_rx_wq;
 static struct workqueue_struct *isert_comp_wq;
+static struct workqueue_struct *isert_release_wq;
 
 static void
 isert_unmap_cmd(struct isert_cmd *isert_cmd, struct isert_conn *isert_conn);
@@ -54,19 +59,32 @@ isert_reg_rdma(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 	       struct isert_rdma_wr *wr);
 static int
 isert_put_response(struct iscsi_conn *conn, struct iscsi_cmd *cmd);
+static int
+isert_rdma_post_recvl(struct isert_conn *isert_conn);
+static int
+isert_rdma_accept(struct isert_conn *isert_conn);
+struct rdma_cm_id *isert_setup_id(struct isert_np *isert_np);
+
+static inline bool
+isert_prot_cmd(struct isert_conn *conn, struct se_cmd *cmd)
+{
+	return (conn->pi_support &&
+		cmd->prot_op != TARGET_PROT_NORMAL);
+}
+
 
 static void
 isert_qp_event_callback(struct ib_event *e, void *context)
 {
 	struct isert_conn *isert_conn = (struct isert_conn *)context;
 
-	pr_err("isert_qp_event_callback event: %d\n", e->event);
+	isert_err("conn %p event: %d\n", isert_conn, e->event);
 	switch (e->event) {
 	case IB_EVENT_COMM_EST:
 		rdma_notify(isert_conn->conn_cm_id, IB_EVENT_COMM_EST);
 		break;
 	case IB_EVENT_QP_LAST_WQE_REACHED:
-		pr_warn("Reached TX IB_EVENT_QP_LAST_WQE_REACHED:\n");
+		isert_warn("Reached TX IB_EVENT_QP_LAST_WQE_REACHED\n");
 		break;
 	default:
 		break;
@@ -80,39 +98,41 @@ isert_query_device(struct ib_device *ib_dev, struct ib_device_attr *devattr)
 
 	ret = ib_query_device(ib_dev, devattr);
 	if (ret) {
-		pr_err("ib_query_device() failed: %d\n", ret);
+		isert_err("ib_query_device() failed: %d\n", ret);
 		return ret;
 	}
-	pr_debug("devattr->max_sge: %d\n", devattr->max_sge);
-	pr_debug("devattr->max_sge_rd: %d\n", devattr->max_sge_rd);
+	isert_dbg("devattr->max_sge: %d\n", devattr->max_sge);
+	isert_dbg("devattr->max_sge_rd: %d\n", devattr->max_sge_rd);
 
 	return 0;
 }
 
 static int
-isert_conn_setup_qp(struct isert_conn *isert_conn, struct rdma_cm_id *cma_id,
-		    u8 protection)
+isert_conn_setup_qp(struct isert_conn *isert_conn, struct rdma_cm_id *cma_id)
 {
 	struct isert_device *device = isert_conn->conn_device;
 	struct ib_qp_init_attr attr;
-	int ret, index, min_index = 0;
+	struct isert_comp *comp;
+	int ret, i, min = 0;
 
 	mutex_lock(&device_list_mutex);
-	for (index = 0; index < device->cqs_used; index++)
-		if (device->cq_active_qps[index] <
-		    device->cq_active_qps[min_index])
-			min_index = index;
-	device->cq_active_qps[min_index]++;
-	pr_debug("isert_conn_setup_qp: Using min_index: %d\n", min_index);
+	for (i = 0; i < device->comps_used; i++)
+		if (device->comps[i].active_qps <
+		    device->comps[min].active_qps)
+			min = i;
+	comp = &device->comps[min];
+	comp->active_qps++;
+	isert_info("conn %p, using comp %p min_index: %d\n",
+		   isert_conn, comp, min);
 	mutex_unlock(&device_list_mutex);
 
 	memset(&attr, 0, sizeof(struct ib_qp_init_attr));
 	attr.event_handler = isert_qp_event_callback;
 	attr.qp_context = isert_conn;
-	attr.send_cq = device->dev_tx_cq[min_index];
-	attr.recv_cq = device->dev_rx_cq[min_index];
+	attr.send_cq = comp->cq;
+	attr.recv_cq = comp->cq;
 	attr.cap.max_send_wr = ISERT_QP_MAX_REQ_DTOS;
-	attr.cap.max_recv_wr = ISERT_QP_MAX_RECV_DTOS;
+	attr.cap.max_recv_wr = ISERT_QP_MAX_RECV_DTOS + 1;
 	/*
 	 * FIXME: Use devattr.max_sge - 2 for max_send_sge as
 	 * work-around for RDMA_READs with ConnectX-2.
@@ -126,29 +146,29 @@ isert_conn_setup_qp(struct isert_conn *isert_conn, struct rdma_cm_id *cma_id,
 	attr.cap.max_recv_sge = 1;
 	attr.sq_sig_type = IB_SIGNAL_REQ_WR;
 	attr.qp_type = IB_QPT_RC;
-	if (protection)
+	if (device->pi_capable)
 		attr.create_flags |= IB_QP_CREATE_SIGNATURE_EN;
 
-	pr_debug("isert_conn_setup_qp cma_id->device: %p\n",
-		 cma_id->device);
-	pr_debug("isert_conn_setup_qp conn_pd->device: %p\n",
-		 isert_conn->conn_pd->device);
-
 	ret = rdma_create_qp(cma_id, isert_conn->conn_pd, &attr);
 	if (ret) {
-		pr_err("rdma_create_qp failed for cma_id %d\n", ret);
-		return ret;
+		isert_err("rdma_create_qp failed for cma_id %d\n", ret);
+		goto err;
 	}
 	isert_conn->conn_qp = cma_id->qp;
-	pr_debug("rdma_create_qp() returned success >>>>>>>>>>>>>>>>>>>>>>>>>.\n");
 
 	return 0;
+err:
+	mutex_lock(&device_list_mutex);
+	comp->active_qps--;
+	mutex_unlock(&device_list_mutex);
+
+	return ret;
 }
 
 static void
 isert_cq_event_callback(struct ib_event *e, void *context)
 {
-	pr_debug("isert_cq_event_callback event: %d\n", e->event);
+	isert_dbg("event: %d\n", e->event);
 }
 
 static int
@@ -182,6 +202,7 @@ isert_alloc_rx_descriptors(struct isert_conn *isert_conn)
 	}
 
 	isert_conn->conn_rx_desc_head = 0;
+
 	return 0;
 
 dma_map_fail:
@@ -193,6 +214,8 @@ dma_map_fail:
 	kfree(isert_conn->conn_rx_descs);
 	isert_conn->conn_rx_descs = NULL;
 fail:
+	isert_err("conn %p failed to allocate rx descriptors\n", isert_conn);
+
 	return -ENOMEM;
 }
 
@@ -216,27 +239,23 @@ isert_free_rx_descriptors(struct isert_conn *isert_conn)
 	isert_conn->conn_rx_descs = NULL;
 }
 
-static void isert_cq_tx_work(struct work_struct *);
-static void isert_cq_tx_callback(struct ib_cq *, void *);
-static void isert_cq_rx_work(struct work_struct *);
-static void isert_cq_rx_callback(struct ib_cq *, void *);
+static void isert_cq_work(struct work_struct *);
+static void isert_cq_callback(struct ib_cq *, void *);
 
 static int
 isert_create_device_ib_res(struct isert_device *device)
 {
 	struct ib_device *ib_dev = device->ib_device;
-	struct isert_cq_desc *cq_desc;
 	struct ib_device_attr *dev_attr;
-	int ret = 0, i, j;
-	int max_rx_cqe, max_tx_cqe;
+	int ret = 0, i;
+	int max_cqe;
 
 	dev_attr = &device->dev_attr;
 	ret = isert_query_device(ib_dev, dev_attr);
 	if (ret)
 		return ret;
 
-	max_rx_cqe = min(ISER_MAX_RX_CQ_LEN, dev_attr->max_cqe);
-	max_tx_cqe = min(ISER_MAX_TX_CQ_LEN, dev_attr->max_cqe);
+	max_cqe = min(ISER_MAX_CQ_LEN, dev_attr->max_cqe);
 
 	/* asign function handlers */
 	if (dev_attr->device_cap_flags & IB_DEVICE_MEM_MGT_EXTENSIONS &&
@@ -254,55 +273,38 @@ isert_create_device_ib_res(struct isert_device *device)
 	device->pi_capable = dev_attr->device_cap_flags &
 			     IB_DEVICE_SIGNATURE_HANDOVER ? true : false;
 
-	device->cqs_used = min_t(int, num_online_cpus(),
-				 device->ib_device->num_comp_vectors);
-	device->cqs_used = min(ISERT_MAX_CQ, device->cqs_used);
-	pr_debug("Using %d CQs, device %s supports %d vectors support "
-		 "Fast registration %d pi_capable %d\n",
-		 device->cqs_used, device->ib_device->name,
-		 device->ib_device->num_comp_vectors, device->use_fastreg,
-		 device->pi_capable);
-	device->cq_desc = kzalloc(sizeof(struct isert_cq_desc) *
-				device->cqs_used, GFP_KERNEL);
-	if (!device->cq_desc) {
-		pr_err("Unable to allocate device->cq_desc\n");
+	device->comps_used = min(ISERT_MAX_CQ, min_t(int, num_online_cpus(),
+					device->ib_device->num_comp_vectors));
+	isert_info("Using %d CQs, %s supports %d vectors support "
+		   "Fast registration %d pi_capable %d\n",
+		   device->comps_used, device->ib_device->name,
+		   device->ib_device->num_comp_vectors, device->use_fastreg,
+		   device->pi_capable);
+
+	device->comps = kcalloc(device->comps_used, sizeof(struct isert_comp),
+				GFP_KERNEL);
+	if (!device->comps) {
+		isert_err("Unable to allocate completion contexts\n");
 		return -ENOMEM;
 	}
-	cq_desc = device->cq_desc;
-
-	for (i = 0; i < device->cqs_used; i++) {
-		cq_desc[i].device = device;
-		cq_desc[i].cq_index = i;
-
-		INIT_WORK(&cq_desc[i].cq_rx_work, isert_cq_rx_work);
-		device->dev_rx_cq[i] = ib_create_cq(device->ib_device,
-						isert_cq_rx_callback,
-						isert_cq_event_callback,
-						(void *)&cq_desc[i],
-						max_rx_cqe, i);
-		if (IS_ERR(device->dev_rx_cq[i])) {
-			ret = PTR_ERR(device->dev_rx_cq[i]);
-			device->dev_rx_cq[i] = NULL;
-			goto out_cq;
-		}
 
-		INIT_WORK(&cq_desc[i].cq_tx_work, isert_cq_tx_work);
-		device->dev_tx_cq[i] = ib_create_cq(device->ib_device,
-						isert_cq_tx_callback,
-						isert_cq_event_callback,
-						(void *)&cq_desc[i],
-						max_tx_cqe, i);
-		if (IS_ERR(device->dev_tx_cq[i])) {
-			ret = PTR_ERR(device->dev_tx_cq[i]);
-			device->dev_tx_cq[i] = NULL;
-			goto out_cq;
-		}
+	for (i = 0; i < device->comps_used; i++) {
+		struct isert_comp *comp = &device->comps[i];
 
-		ret = ib_req_notify_cq(device->dev_rx_cq[i], IB_CQ_NEXT_COMP);
-		if (ret)
+		comp->device = device;
+		INIT_WORK(&comp->work, isert_cq_work);
+		comp->cq = ib_create_cq(device->ib_device,
+					isert_cq_callback,
+					isert_cq_event_callback,
+					(void *)comp,
+					max_cqe, i);
+		if (IS_ERR(comp->cq)) {
+			ret = PTR_ERR(comp->cq);
+			comp->cq = NULL;
 			goto out_cq;
+		}
 
-		ret = ib_req_notify_cq(device->dev_tx_cq[i], IB_CQ_NEXT_COMP);
+		ret = ib_req_notify_cq(comp->cq, IB_CQ_NEXT_COMP);
 		if (ret)
 			goto out_cq;
 	}
@@ -310,19 +312,15 @@ isert_create_device_ib_res(struct isert_device *device)
 	return 0;
 
 out_cq:
-	for (j = 0; j < i; j++) {
-		cq_desc = &device->cq_desc[j];
+	for (i = 0; i < device->comps_used; i++) {
+		struct isert_comp *comp = &device->comps[i];
 
-		if (device->dev_rx_cq[j]) {
-			cancel_work_sync(&cq_desc->cq_rx_work);
-			ib_destroy_cq(device->dev_rx_cq[j]);
-		}
-		if (device->dev_tx_cq[j]) {
-			cancel_work_sync(&cq_desc->cq_tx_work);
-			ib_destroy_cq(device->dev_tx_cq[j]);
+		if (comp->cq) {
+			cancel_work_sync(&comp->work);
+			ib_destroy_cq(comp->cq);
 		}
 	}
-	kfree(device->cq_desc);
+	kfree(device->comps);
 
 	return ret;
 }
@@ -330,21 +328,18 @@ out_cq:
 static void
 isert_free_device_ib_res(struct isert_device *device)
 {
-	struct isert_cq_desc *cq_desc;
 	int i;
 
-	for (i = 0; i < device->cqs_used; i++) {
-		cq_desc = &device->cq_desc[i];
+	isert_info("device %p\n", device);
 
-		cancel_work_sync(&cq_desc->cq_rx_work);
-		cancel_work_sync(&cq_desc->cq_tx_work);
-		ib_destroy_cq(device->dev_rx_cq[i]);
-		ib_destroy_cq(device->dev_tx_cq[i]);
-		device->dev_rx_cq[i] = NULL;
-		device->dev_tx_cq[i] = NULL;
-	}
+	for (i = 0; i < device->comps_used; i++) {
+		struct isert_comp *comp = &device->comps[i];
 
-	kfree(device->cq_desc);
+		cancel_work_sync(&comp->work);
+		ib_destroy_cq(comp->cq);
+		comp->cq = NULL;
+	}
+	kfree(device->comps);
 }
 
 static void
@@ -352,6 +347,7 @@ isert_device_try_release(struct isert_device *device)
 {
 	mutex_lock(&device_list_mutex);
 	device->refcount--;
+	isert_info("device %p refcount %d\n", device, device->refcount);
 	if (!device->refcount) {
 		isert_free_device_ib_res(device);
 		list_del(&device->dev_node);
@@ -370,6 +366,8 @@ isert_device_find_by_ib_dev(struct rdma_cm_id *cma_id)
 	list_for_each_entry(device, &device_list, dev_node) {
 		if (device->ib_device->node_guid == cma_id->device->node_guid) {
 			device->refcount++;
+			isert_info("Found iser device %p refcount %d\n",
+				   device, device->refcount);
 			mutex_unlock(&device_list_mutex);
 			return device;
 		}
@@ -393,6 +391,8 @@ isert_device_find_by_ib_dev(struct rdma_cm_id *cma_id)
 
 	device->refcount++;
 	list_add_tail(&device->dev_node, &device_list);
+	isert_info("Created a new iser device %p refcount %d\n",
+		   device, device->refcount);
 	mutex_unlock(&device_list_mutex);
 
 	return device;
@@ -407,7 +407,7 @@ isert_conn_free_fastreg_pool(struct isert_conn *isert_conn)
 	if (list_empty(&isert_conn->conn_fr_pool))
 		return;
 
-	pr_debug("Freeing conn %p fastreg pool", isert_conn);
+	isert_info("Freeing conn %p fastreg pool", isert_conn);
 
 	list_for_each_entry_safe(fr_desc, tmp,
 				 &isert_conn->conn_fr_pool, list) {
@@ -425,87 +425,97 @@ isert_conn_free_fastreg_pool(struct isert_conn *isert_conn)
 	}
 
 	if (i < isert_conn->conn_fr_pool_size)
-		pr_warn("Pool still has %d regions registered\n",
+		isert_warn("Pool still has %d regions registered\n",
 			isert_conn->conn_fr_pool_size - i);
 }
 
 static int
+isert_create_pi_ctx(struct fast_reg_descriptor *desc,
+		    struct ib_device *device,
+		    struct ib_pd *pd)
+{
+	struct ib_mr_init_attr mr_init_attr;
+	struct pi_context *pi_ctx;
+	int ret;
+
+	pi_ctx = kzalloc(sizeof(*desc->pi_ctx), GFP_KERNEL);
+	if (!pi_ctx) {
+		isert_err("Failed to allocate pi context\n");
+		return -ENOMEM;
+	}
+
+	pi_ctx->prot_frpl = ib_alloc_fast_reg_page_list(device,
+					    ISCSI_ISER_SG_TABLESIZE);
+	if (IS_ERR(pi_ctx->prot_frpl)) {
+		isert_err("Failed to allocate prot frpl err=%ld\n",
+			  PTR_ERR(pi_ctx->prot_frpl));
+		ret = PTR_ERR(pi_ctx->prot_frpl);
+		goto err_pi_ctx;
+	}
+
+	pi_ctx->prot_mr = ib_alloc_fast_reg_mr(pd, ISCSI_ISER_SG_TABLESIZE);
+	if (IS_ERR(pi_ctx->prot_mr)) {
+		isert_err("Failed to allocate prot frmr err=%ld\n",
+			  PTR_ERR(pi_ctx->prot_mr));
+		ret = PTR_ERR(pi_ctx->prot_mr);
+		goto err_prot_frpl;
+	}
+	desc->ind |= ISERT_PROT_KEY_VALID;
+
+	memset(&mr_init_attr, 0, sizeof(mr_init_attr));
+	mr_init_attr.max_reg_descriptors = 2;
+	mr_init_attr.flags |= IB_MR_SIGNATURE_EN;
+	pi_ctx->sig_mr = ib_create_mr(pd, &mr_init_attr);
+	if (IS_ERR(pi_ctx->sig_mr)) {
+		isert_err("Failed to allocate signature enabled mr err=%ld\n",
+			  PTR_ERR(pi_ctx->sig_mr));
+		ret = PTR_ERR(pi_ctx->sig_mr);
+		goto err_prot_mr;
+	}
+
+	desc->pi_ctx = pi_ctx;
+	desc->ind |= ISERT_SIG_KEY_VALID;
+	desc->ind &= ~ISERT_PROTECTED;
+
+	return 0;
+
+err_prot_mr:
+	ib_dereg_mr(desc->pi_ctx->prot_mr);
+err_prot_frpl:
+	ib_free_fast_reg_page_list(desc->pi_ctx->prot_frpl);
+err_pi_ctx:
+	kfree(desc->pi_ctx);
+
+	return ret;
+}
+
+static int
 isert_create_fr_desc(struct ib_device *ib_device, struct ib_pd *pd,
-		     struct fast_reg_descriptor *fr_desc, u8 protection)
+		     struct fast_reg_descriptor *fr_desc)
 {
 	int ret;
 
 	fr_desc->data_frpl = ib_alloc_fast_reg_page_list(ib_device,
 							 ISCSI_ISER_SG_TABLESIZE);
 	if (IS_ERR(fr_desc->data_frpl)) {
-		pr_err("Failed to allocate data frpl err=%ld\n",
-		       PTR_ERR(fr_desc->data_frpl));
+		isert_err("Failed to allocate data frpl err=%ld\n",
+			  PTR_ERR(fr_desc->data_frpl));
 		return PTR_ERR(fr_desc->data_frpl);
 	}
 
 	fr_desc->data_mr = ib_alloc_fast_reg_mr(pd, ISCSI_ISER_SG_TABLESIZE);
 	if (IS_ERR(fr_desc->data_mr)) {
-		pr_err("Failed to allocate data frmr err=%ld\n",
-		       PTR_ERR(fr_desc->data_mr));
+		isert_err("Failed to allocate data frmr err=%ld\n",
+			  PTR_ERR(fr_desc->data_mr));
 		ret = PTR_ERR(fr_desc->data_mr);
 		goto err_data_frpl;
 	}
-	pr_debug("Create fr_desc %p page_list %p\n",
-		 fr_desc, fr_desc->data_frpl->page_list);
 	fr_desc->ind |= ISERT_DATA_KEY_VALID;
 
-	if (protection) {
-		struct ib_mr_init_attr mr_init_attr = {0};
-		struct pi_context *pi_ctx;
-
-		fr_desc->pi_ctx = kzalloc(sizeof(*fr_desc->pi_ctx), GFP_KERNEL);
-		if (!fr_desc->pi_ctx) {
-			pr_err("Failed to allocate pi context\n");
-			ret = -ENOMEM;
-			goto err_data_mr;
-		}
-		pi_ctx = fr_desc->pi_ctx;
-
-		pi_ctx->prot_frpl = ib_alloc_fast_reg_page_list(ib_device,
-						    ISCSI_ISER_SG_TABLESIZE);
-		if (IS_ERR(pi_ctx->prot_frpl)) {
-			pr_err("Failed to allocate prot frpl err=%ld\n",
-			       PTR_ERR(pi_ctx->prot_frpl));
-			ret = PTR_ERR(pi_ctx->prot_frpl);
-			goto err_pi_ctx;
-		}
-
-		pi_ctx->prot_mr = ib_alloc_fast_reg_mr(pd, ISCSI_ISER_SG_TABLESIZE);
-		if (IS_ERR(pi_ctx->prot_mr)) {
-			pr_err("Failed to allocate prot frmr err=%ld\n",
-			       PTR_ERR(pi_ctx->prot_mr));
-			ret = PTR_ERR(pi_ctx->prot_mr);
-			goto err_prot_frpl;
-		}
-		fr_desc->ind |= ISERT_PROT_KEY_VALID;
-
-		mr_init_attr.max_reg_descriptors = 2;
-		mr_init_attr.flags |= IB_MR_SIGNATURE_EN;
-		pi_ctx->sig_mr = ib_create_mr(pd, &mr_init_attr);
-		if (IS_ERR(pi_ctx->sig_mr)) {
-			pr_err("Failed to allocate signature enabled mr err=%ld\n",
-			       PTR_ERR(pi_ctx->sig_mr));
-			ret = PTR_ERR(pi_ctx->sig_mr);
-			goto err_prot_mr;
-		}
-		fr_desc->ind |= ISERT_SIG_KEY_VALID;
-	}
-	fr_desc->ind &= ~ISERT_PROTECTED;
+	isert_dbg("Created fr_desc %p\n", fr_desc);
 
 	return 0;
-err_prot_mr:
-	ib_dereg_mr(fr_desc->pi_ctx->prot_mr);
-err_prot_frpl:
-	ib_free_fast_reg_page_list(fr_desc->pi_ctx->prot_frpl);
-err_pi_ctx:
-	kfree(fr_desc->pi_ctx);
-err_data_mr:
-	ib_dereg_mr(fr_desc->data_mr);
+
 err_data_frpl:
 	ib_free_fast_reg_page_list(fr_desc->data_frpl);
 
@@ -513,7 +523,7 @@ err_data_frpl:
 }
 
 static int
-isert_conn_create_fastreg_pool(struct isert_conn *isert_conn, u8 pi_support)
+isert_conn_create_fastreg_pool(struct isert_conn *isert_conn)
 {
 	struct fast_reg_descriptor *fr_desc;
 	struct isert_device *device = isert_conn->conn_device;
@@ -531,16 +541,15 @@ isert_conn_create_fastreg_pool(struct isert_conn *isert_conn, u8 pi_support)
 	for (i = 0; i < tag_num; i++) {
 		fr_desc = kzalloc(sizeof(*fr_desc), GFP_KERNEL);
 		if (!fr_desc) {
-			pr_err("Failed to allocate fast_reg descriptor\n");
+			isert_err("Failed to allocate fast_reg descriptor\n");
 			ret = -ENOMEM;
 			goto err;
 		}
 
 		ret = isert_create_fr_desc(device->ib_device,
-					   isert_conn->conn_pd, fr_desc,
-					   pi_support);
+					   isert_conn->conn_pd, fr_desc);
 		if (ret) {
-			pr_err("Failed to create fastreg descriptor err=%d\n",
+			isert_err("Failed to create fastreg descriptor err=%d\n",
 			       ret);
 			kfree(fr_desc);
 			goto err;
@@ -550,7 +559,7 @@ isert_conn_create_fastreg_pool(struct isert_conn *isert_conn, u8 pi_support)
 		isert_conn->conn_fr_pool_size++;
 	}
 
-	pr_debug("Creating conn %p fastreg pool size=%d",
+	isert_dbg("Creating conn %p fastreg pool size=%d",
 		 isert_conn, isert_conn->conn_fr_pool_size);
 
 	return 0;
@@ -563,47 +572,45 @@ err:
 static int
 isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 {
-	struct iscsi_np *np = cma_id->context;
-	struct isert_np *isert_np = np->np_context;
+	struct isert_np *isert_np = cma_id->context;
+	struct iscsi_np *np = isert_np->np;
 	struct isert_conn *isert_conn;
 	struct isert_device *device;
 	struct ib_device *ib_dev = cma_id->device;
 	int ret = 0;
-	u8 pi_support;
 
 	spin_lock_bh(&np->np_thread_lock);
 	if (!np->enabled) {
 		spin_unlock_bh(&np->np_thread_lock);
-		pr_debug("iscsi_np is not enabled, reject connect request\n");
+		isert_dbg("iscsi_np is not enabled, reject connect request\n");
 		return rdma_reject(cma_id, NULL, 0);
 	}
 	spin_unlock_bh(&np->np_thread_lock);
 
-	pr_debug("Entering isert_connect_request cma_id: %p, context: %p\n",
+	isert_dbg("cma_id: %p, portal: %p\n",
 		 cma_id, cma_id->context);
 
 	isert_conn = kzalloc(sizeof(struct isert_conn), GFP_KERNEL);
 	if (!isert_conn) {
-		pr_err("Unable to allocate isert_conn\n");
+		isert_err("Unable to allocate isert_conn\n");
 		return -ENOMEM;
 	}
 	isert_conn->state = ISER_CONN_INIT;
 	INIT_LIST_HEAD(&isert_conn->conn_accept_node);
 	init_completion(&isert_conn->conn_login_comp);
+	init_completion(&isert_conn->login_req_comp);
 	init_completion(&isert_conn->conn_wait);
-	init_completion(&isert_conn->conn_wait_comp_err);
 	kref_init(&isert_conn->conn_kref);
 	mutex_init(&isert_conn->conn_mutex);
 	spin_lock_init(&isert_conn->conn_lock);
 	INIT_LIST_HEAD(&isert_conn->conn_fr_pool);
 
-	cma_id->context = isert_conn;
 	isert_conn->conn_cm_id = cma_id;
 
 	isert_conn->login_buf = kzalloc(ISCSI_DEF_MAX_RECV_SEG_LEN +
 					ISER_RX_LOGIN_SIZE, GFP_KERNEL);
 	if (!isert_conn->login_buf) {
-		pr_err("Unable to allocate isert_conn->login_buf\n");
+		isert_err("Unable to allocate isert_conn->login_buf\n");
 		ret = -ENOMEM;
 		goto out;
 	}
@@ -611,7 +618,7 @@ isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 	isert_conn->login_req_buf = isert_conn->login_buf;
 	isert_conn->login_rsp_buf = isert_conn->login_buf +
 				    ISCSI_DEF_MAX_RECV_SEG_LEN;
-	pr_debug("Set login_buf: %p login_req_buf: %p login_rsp_buf: %p\n",
+	isert_dbg("Set login_buf: %p login_req_buf: %p login_rsp_buf: %p\n",
 		 isert_conn->login_buf, isert_conn->login_req_buf,
 		 isert_conn->login_rsp_buf);
 
@@ -621,7 +628,7 @@ isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 
 	ret = ib_dma_mapping_error(ib_dev, isert_conn->login_req_dma);
 	if (ret) {
-		pr_err("ib_dma_mapping_error failed for login_req_dma: %d\n",
+		isert_err("ib_dma_mapping_error failed for login_req_dma: %d\n",
 		       ret);
 		isert_conn->login_req_dma = 0;
 		goto out_login_buf;
@@ -633,7 +640,7 @@ isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 
 	ret = ib_dma_mapping_error(ib_dev, isert_conn->login_rsp_dma);
 	if (ret) {
-		pr_err("ib_dma_mapping_error failed for login_rsp_dma: %d\n",
+		isert_err("ib_dma_mapping_error failed for login_rsp_dma: %d\n",
 		       ret);
 		isert_conn->login_rsp_dma = 0;
 		goto out_req_dma_map;
@@ -649,13 +656,13 @@ isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 	isert_conn->initiator_depth = min_t(u8,
 				event->param.conn.initiator_depth,
 				device->dev_attr.max_qp_init_rd_atom);
-	pr_debug("Using initiator_depth: %u\n", isert_conn->initiator_depth);
+	isert_dbg("Using initiator_depth: %u\n", isert_conn->initiator_depth);
 
 	isert_conn->conn_device = device;
 	isert_conn->conn_pd = ib_alloc_pd(isert_conn->conn_device->ib_device);
 	if (IS_ERR(isert_conn->conn_pd)) {
 		ret = PTR_ERR(isert_conn->conn_pd);
-		pr_err("ib_alloc_pd failed for conn %p: ret=%d\n",
+		isert_err("ib_alloc_pd failed for conn %p: ret=%d\n",
 		       isert_conn, ret);
 		goto out_pd;
 	}
@@ -664,20 +671,20 @@ isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 					   IB_ACCESS_LOCAL_WRITE);
 	if (IS_ERR(isert_conn->conn_mr)) {
 		ret = PTR_ERR(isert_conn->conn_mr);
-		pr_err("ib_get_dma_mr failed for conn %p: ret=%d\n",
+		isert_err("ib_get_dma_mr failed for conn %p: ret=%d\n",
 		       isert_conn, ret);
 		goto out_mr;
 	}
 
-	pi_support = np->tpg_np->tpg->tpg_attrib.t10_pi;
-	if (pi_support && !device->pi_capable) {
-		pr_err("Protection information requested but not supported, "
-		       "rejecting connect request\n");
-		ret = rdma_reject(cma_id, NULL, 0);
-		goto out_mr;
-	}
+	ret = isert_conn_setup_qp(isert_conn, cma_id);
+	if (ret)
+		goto out_conn_dev;
 
-	ret = isert_conn_setup_qp(isert_conn, cma_id, pi_support);
+	ret = isert_rdma_post_recvl(isert_conn);
+	if (ret)
+		goto out_conn_dev;
+
+	ret = isert_rdma_accept(isert_conn);
 	if (ret)
 		goto out_conn_dev;
 
@@ -685,7 +692,7 @@ isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 	list_add_tail(&isert_conn->conn_accept_node, &isert_np->np_accept_list);
 	mutex_unlock(&isert_np->np_accept_mutex);
 
-	pr_debug("isert_connect_request() up np_sem np: %p\n", np);
+	isert_info("np %p: Allow accept_np to continue\n", np);
 	up(&isert_np->np_sem);
 	return 0;
 
@@ -705,6 +712,7 @@ out_login_buf:
 	kfree(isert_conn->login_buf);
 out:
 	kfree(isert_conn);
+	rdma_reject(cma_id, NULL, 0);
 	return ret;
 }
 
@@ -713,24 +721,25 @@ isert_connect_release(struct isert_conn *isert_conn)
 {
 	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
 	struct isert_device *device = isert_conn->conn_device;
-	int cq_index;
 
-	pr_debug("Entering isert_connect_release(): >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+	isert_dbg("conn %p\n", isert_conn);
 
 	if (device && device->use_fastreg)
 		isert_conn_free_fastreg_pool(isert_conn);
 
+	isert_free_rx_descriptors(isert_conn);
+	rdma_destroy_id(isert_conn->conn_cm_id);
+
 	if (isert_conn->conn_qp) {
-		cq_index = ((struct isert_cq_desc *)
-			isert_conn->conn_qp->recv_cq->cq_context)->cq_index;
-		pr_debug("isert_connect_release: cq_index: %d\n", cq_index);
-		isert_conn->conn_device->cq_active_qps[cq_index]--;
+		struct isert_comp *comp = isert_conn->conn_qp->recv_cq->cq_context;
 
-		rdma_destroy_qp(isert_conn->conn_cm_id);
-	}
+		isert_dbg("dec completion context %p active_qps\n", comp);
+		mutex_lock(&device_list_mutex);
+		comp->active_qps--;
+		mutex_unlock(&device_list_mutex);
 
-	isert_free_rx_descriptors(isert_conn);
-	rdma_destroy_id(isert_conn->conn_cm_id);
+		ib_destroy_qp(isert_conn->conn_qp);
+	}
 
 	ib_dereg_mr(isert_conn->conn_mr);
 	ib_dealloc_pd(isert_conn->conn_pd);
@@ -747,16 +756,24 @@ isert_connect_release(struct isert_conn *isert_conn)
 
 	if (device)
 		isert_device_try_release(device);
-
-	pr_debug("Leaving isert_connect_release >>>>>>>>>>>>\n");
 }
 
 static void
 isert_connected_handler(struct rdma_cm_id *cma_id)
 {
-	struct isert_conn *isert_conn = cma_id->context;
+	struct isert_conn *isert_conn = cma_id->qp->qp_context;
 
-	kref_get(&isert_conn->conn_kref);
+	isert_info("conn %p\n", isert_conn);
+
+	if (!kref_get_unless_zero(&isert_conn->conn_kref)) {
+		isert_warn("conn %p connect_release is running\n", isert_conn);
+		return;
+	}
+
+	mutex_lock(&isert_conn->conn_mutex);
+	if (isert_conn->state != ISER_CONN_FULL_FEATURE)
+		isert_conn->state = ISER_CONN_UP;
+	mutex_unlock(&isert_conn->conn_mutex);
 }
 
 static void
@@ -765,8 +782,8 @@ isert_release_conn_kref(struct kref *kref)
 	struct isert_conn *isert_conn = container_of(kref,
 				struct isert_conn, conn_kref);
 
-	pr_debug("Calling isert_connect_release for final kref %s/%d\n",
-		 current->comm, current->pid);
+	isert_info("conn %p final kref %s/%d\n", isert_conn, current->comm,
+		   current->pid);
 
 	isert_connect_release(isert_conn);
 }
@@ -777,75 +794,111 @@ isert_put_conn(struct isert_conn *isert_conn)
 	kref_put(&isert_conn->conn_kref, isert_release_conn_kref);
 }
 
+/**
+ * isert_conn_terminate() - Initiate connection termination
+ * @isert_conn: isert connection struct
+ *
+ * Notes:
+ * In case the connection state is FULL_FEATURE, move state
+ * to TEMINATING and start teardown sequence (rdma_disconnect).
+ * In case the connection state is UP, complete flush as well.
+ *
+ * This routine must be called with conn_mutex held. Thus it is
+ * safe to call multiple times.
+ */
 static void
-isert_disconnect_work(struct work_struct *work)
+isert_conn_terminate(struct isert_conn *isert_conn)
 {
-	struct isert_conn *isert_conn = container_of(work,
-				struct isert_conn, conn_logout_work);
+	int err;
 
-	pr_debug("isert_disconnect_work(): >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
-	mutex_lock(&isert_conn->conn_mutex);
-	if (isert_conn->state == ISER_CONN_UP)
+	switch (isert_conn->state) {
+	case ISER_CONN_TERMINATING:
+		break;
+	case ISER_CONN_UP:
+	case ISER_CONN_FULL_FEATURE: /* FALLTHRU */
+		isert_info("Terminating conn %p state %d\n",
+			   isert_conn, isert_conn->state);
 		isert_conn->state = ISER_CONN_TERMINATING;
-
-	if (isert_conn->post_recv_buf_count == 0 &&
-	    atomic_read(&isert_conn->post_send_buf_count) == 0) {
-		mutex_unlock(&isert_conn->conn_mutex);
-		goto wake_up;
-	}
-	if (!isert_conn->conn_cm_id) {
-		mutex_unlock(&isert_conn->conn_mutex);
-		isert_put_conn(isert_conn);
-		return;
+		err = rdma_disconnect(isert_conn->conn_cm_id);
+		if (err)
+			isert_warn("Failed rdma_disconnect isert_conn %p\n",
+				   isert_conn);
+		break;
+	default:
+		isert_warn("conn %p teminating in state %d\n",
+			   isert_conn, isert_conn->state);
 	}
+}
 
-	if (isert_conn->disconnect) {
-		/* Send DREQ/DREP towards our initiator */
-		rdma_disconnect(isert_conn->conn_cm_id);
-	}
+static int
+isert_np_cma_handler(struct isert_np *isert_np,
+		     enum rdma_cm_event_type event)
+{
+	isert_dbg("isert np %p, handling event %d\n", isert_np, event);
 
-	mutex_unlock(&isert_conn->conn_mutex);
+	switch (event) {
+	case RDMA_CM_EVENT_DEVICE_REMOVAL:
+		isert_np->np_cm_id = NULL;
+		break;
+	case RDMA_CM_EVENT_ADDR_CHANGE:
+		isert_np->np_cm_id = isert_setup_id(isert_np);
+		if (IS_ERR(isert_np->np_cm_id)) {
+			isert_err("isert np %p setup id failed: %ld\n",
+				  isert_np, PTR_ERR(isert_np->np_cm_id));
+			isert_np->np_cm_id = NULL;
+		}
+		break;
+	default:
+		isert_err("isert np %p Unexpected event %d\n",
+			  isert_np, event);
+	}
 
-wake_up:
-	complete(&isert_conn->conn_wait);
+	return -1;
 }
 
 static int
-isert_disconnected_handler(struct rdma_cm_id *cma_id, bool disconnect)
+isert_disconnected_handler(struct rdma_cm_id *cma_id,
+			   enum rdma_cm_event_type event)
 {
+	struct isert_np *isert_np = cma_id->context;
 	struct isert_conn *isert_conn;
 
-	if (!cma_id->qp) {
-		struct isert_np *isert_np = cma_id->context;
+	if (isert_np->np_cm_id == cma_id)
+		return isert_np_cma_handler(cma_id->context, event);
 
-		isert_np->np_cm_id = NULL;
-		return -1;
-	}
+	isert_conn = cma_id->qp->qp_context;
 
-	isert_conn = (struct isert_conn *)cma_id->context;
+	mutex_lock(&isert_conn->conn_mutex);
+	isert_conn_terminate(isert_conn);
+	mutex_unlock(&isert_conn->conn_mutex);
 
-	isert_conn->disconnect = disconnect;
-	INIT_WORK(&isert_conn->conn_logout_work, isert_disconnect_work);
-	schedule_work(&isert_conn->conn_logout_work);
+	isert_info("conn %p completing conn_wait\n", isert_conn);
+	complete(&isert_conn->conn_wait);
 
 	return 0;
 }
 
+static void
+isert_connect_error(struct rdma_cm_id *cma_id)
+{
+	struct isert_conn *isert_conn = cma_id->qp->qp_context;
+
+	isert_put_conn(isert_conn);
+}
+
 static int
 isert_cma_handler(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 {
 	int ret = 0;
-	bool disconnect = false;
 
-	pr_debug("isert_cma_handler: event %d status %d conn %p id %p\n",
-		 event->event, event->status, cma_id->context, cma_id);
+	isert_info("event %d status %d id %p np %p\n", event->event,
+		   event->status, cma_id, cma_id->context);
 
 	switch (event->event) {
 	case RDMA_CM_EVENT_CONNECT_REQUEST:
 		ret = isert_connect_request(cma_id, event);
 		if (ret)
-			pr_err("isert_cma_handler failed RDMA_CM_EVENT: 0x%08x %d\n",
-				event->event, ret);
+			isert_err("failed handle connect request %d\n", ret);
 		break;
 	case RDMA_CM_EVENT_ESTABLISHED:
 		isert_connected_handler(cma_id);
@@ -853,13 +906,16 @@ isert_cma_handler(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 	case RDMA_CM_EVENT_ADDR_CHANGE:    /* FALLTHRU */
 	case RDMA_CM_EVENT_DISCONNECTED:   /* FALLTHRU */
 	case RDMA_CM_EVENT_DEVICE_REMOVAL: /* FALLTHRU */
-		disconnect = true;
 	case RDMA_CM_EVENT_TIMEWAIT_EXIT:  /* FALLTHRU */
-		ret = isert_disconnected_handler(cma_id, disconnect);
+		ret = isert_disconnected_handler(cma_id, event->event);
 		break;
+	case RDMA_CM_EVENT_REJECTED:       /* FALLTHRU */
+	case RDMA_CM_EVENT_UNREACHABLE:    /* FALLTHRU */
 	case RDMA_CM_EVENT_CONNECT_ERROR:
+		isert_connect_error(cma_id);
+		break;
 	default:
-		pr_err("Unhandled RDMA CMA event: %d\n", event->event);
+		isert_err("Unhandled RDMA CMA event: %d\n", event->event);
 		break;
 	}
 
@@ -876,7 +932,7 @@ isert_post_recv(struct isert_conn *isert_conn, u32 count)
 
 	for (rx_wr = isert_conn->conn_rx_wr, i = 0; i < count; i++, rx_wr++) {
 		rx_desc		= &isert_conn->conn_rx_descs[rx_head];
-		rx_wr->wr_id	= (unsigned long)rx_desc;
+		rx_wr->wr_id	= (uintptr_t)rx_desc;
 		rx_wr->sg_list	= &rx_desc->rx_sg;
 		rx_wr->num_sge	= 1;
 		rx_wr->next	= rx_wr + 1;
@@ -890,10 +946,10 @@ isert_post_recv(struct isert_conn *isert_conn, u32 count)
 	ret = ib_post_recv(isert_conn->conn_qp, isert_conn->conn_rx_wr,
 				&rx_wr_failed);
 	if (ret) {
-		pr_err("ib_post_recv() failed with ret: %d\n", ret);
+		isert_err("ib_post_recv() failed with ret: %d\n", ret);
 		isert_conn->post_recv_buf_count -= count;
 	} else {
-		pr_debug("isert_post_recv(): Posted %d RX buffers\n", count);
+		isert_dbg("isert_post_recv(): Posted %d RX buffers\n", count);
 		isert_conn->conn_rx_desc_head = rx_head;
 	}
 	return ret;
@@ -910,19 +966,15 @@ isert_post_send(struct isert_conn *isert_conn, struct iser_tx_desc *tx_desc)
 				      ISER_HEADERS_LEN, DMA_TO_DEVICE);
 
 	send_wr.next	= NULL;
-	send_wr.wr_id	= (unsigned long)tx_desc;
+	send_wr.wr_id	= (uintptr_t)tx_desc;
 	send_wr.sg_list	= tx_desc->tx_sg;
 	send_wr.num_sge	= tx_desc->num_sge;
 	send_wr.opcode	= IB_WR_SEND;
 	send_wr.send_flags = IB_SEND_SIGNALED;
 
-	atomic_inc(&isert_conn->post_send_buf_count);
-
 	ret = ib_post_send(isert_conn->conn_qp, &send_wr, &send_wr_failed);
-	if (ret) {
-		pr_err("ib_post_send() failed, ret: %d\n", ret);
-		atomic_dec(&isert_conn->post_send_buf_count);
-	}
+	if (ret)
+		isert_err("ib_post_send() failed, ret: %d\n", ret);
 
 	return ret;
 }
@@ -945,7 +997,7 @@ isert_create_send_desc(struct isert_conn *isert_conn,
 
 	if (tx_desc->tx_sg[0].lkey != isert_conn->conn_mr->lkey) {
 		tx_desc->tx_sg[0].lkey = isert_conn->conn_mr->lkey;
-		pr_debug("tx_desc %p lkey mismatch, fixing\n", tx_desc);
+		isert_dbg("tx_desc %p lkey mismatch, fixing\n", tx_desc);
 	}
 }
 
@@ -959,7 +1011,7 @@ isert_init_tx_hdrs(struct isert_conn *isert_conn,
 	dma_addr = ib_dma_map_single(ib_dev, (void *)tx_desc,
 			ISER_HEADERS_LEN, DMA_TO_DEVICE);
 	if (ib_dma_mapping_error(ib_dev, dma_addr)) {
-		pr_err("ib_dma_mapping_error() failed\n");
+		isert_err("ib_dma_mapping_error() failed\n");
 		return -ENOMEM;
 	}
 
@@ -968,40 +1020,24 @@ isert_init_tx_hdrs(struct isert_conn *isert_conn,
 	tx_desc->tx_sg[0].length = ISER_HEADERS_LEN;
 	tx_desc->tx_sg[0].lkey = isert_conn->conn_mr->lkey;
 
-	pr_debug("isert_init_tx_hdrs: Setup tx_sg[0].addr: 0x%llx length: %u"
-		 " lkey: 0x%08x\n", tx_desc->tx_sg[0].addr,
-		 tx_desc->tx_sg[0].length, tx_desc->tx_sg[0].lkey);
+	isert_dbg("Setup tx_sg[0].addr: 0x%llx length: %u lkey: 0x%x\n",
+		  tx_desc->tx_sg[0].addr, tx_desc->tx_sg[0].length,
+		  tx_desc->tx_sg[0].lkey);
 
 	return 0;
 }
 
 static void
 isert_init_send_wr(struct isert_conn *isert_conn, struct isert_cmd *isert_cmd,
-		   struct ib_send_wr *send_wr, bool coalesce)
+		   struct ib_send_wr *send_wr)
 {
 	struct iser_tx_desc *tx_desc = &isert_cmd->tx_desc;
 
 	isert_cmd->rdma_wr.iser_ib_op = ISER_IB_SEND;
-	send_wr->wr_id = (unsigned long)&isert_cmd->tx_desc;
+	send_wr->wr_id = (uintptr_t)&isert_cmd->tx_desc;
 	send_wr->opcode = IB_WR_SEND;
 	send_wr->sg_list = &tx_desc->tx_sg[0];
 	send_wr->num_sge = isert_cmd->tx_desc.num_sge;
-	/*
-	 * Coalesce send completion interrupts by only setting IB_SEND_SIGNALED
-	 * bit for every ISERT_COMP_BATCH_COUNT number of ib_post_send() calls.
-	 */
-	mutex_lock(&isert_conn->conn_mutex);
-	if (coalesce && isert_conn->state == ISER_CONN_UP &&
-	    ++isert_conn->conn_comp_batch < ISERT_COMP_BATCH_COUNT) {
-		tx_desc->llnode_active = true;
-		llist_add(&tx_desc->comp_llnode, &isert_conn->conn_comp_llist);
-		mutex_unlock(&isert_conn->conn_mutex);
-		return;
-	}
-	isert_conn->conn_comp_batch = 0;
-	tx_desc->comp_llnode_batch = llist_del_all(&isert_conn->conn_comp_llist);
-	mutex_unlock(&isert_conn->conn_mutex);
-
 	send_wr->send_flags = IB_SEND_SIGNALED;
 }
 
@@ -1017,22 +1053,21 @@ isert_rdma_post_recvl(struct isert_conn *isert_conn)
 	sge.length = ISER_RX_LOGIN_SIZE;
 	sge.lkey = isert_conn->conn_mr->lkey;
 
-	pr_debug("Setup sge: addr: %llx length: %d 0x%08x\n",
+	isert_dbg("Setup sge: addr: %llx length: %d 0x%08x\n",
 		sge.addr, sge.length, sge.lkey);
 
 	memset(&rx_wr, 0, sizeof(struct ib_recv_wr));
-	rx_wr.wr_id = (unsigned long)isert_conn->login_req_buf;
+	rx_wr.wr_id = (uintptr_t)isert_conn->login_req_buf;
 	rx_wr.sg_list = &sge;
 	rx_wr.num_sge = 1;
 
 	isert_conn->post_recv_buf_count++;
 	ret = ib_post_recv(isert_conn->conn_qp, &rx_wr, &rx_wr_fail);
 	if (ret) {
-		pr_err("ib_post_recv() failed: %d\n", ret);
+		isert_err("ib_post_recv() failed: %d\n", ret);
 		isert_conn->post_recv_buf_count--;
 	}
 
-	pr_debug("ib_post_recv(): returned success >>>>>>>>>>>>>>>>>>>>>>>>\n");
 	return ret;
 }
 
@@ -1072,13 +1107,9 @@ isert_put_login_tx(struct iscsi_conn *conn, struct iscsi_login *login,
 		if (login->login_complete) {
 			if (!conn->sess->sess_ops->SessionType &&
 			    isert_conn->conn_device->use_fastreg) {
-				/* Normal Session and fastreg is used */
-				u8 pi_support = login->np->tpg_np->tpg->tpg_attrib.t10_pi;
-
-				ret = isert_conn_create_fastreg_pool(isert_conn,
-								     pi_support);
+				ret = isert_conn_create_fastreg_pool(isert_conn);
 				if (ret) {
-					pr_err("Conn: %p failed to create"
+					isert_err("Conn: %p failed to create"
 					       " fastreg pool\n", isert_conn);
 					return ret;
 				}
@@ -1092,7 +1123,10 @@ isert_put_login_tx(struct iscsi_conn *conn, struct iscsi_login *login,
 			if (ret)
 				return ret;
 
-			isert_conn->state = ISER_CONN_UP;
+			/* Now we are in FULL_FEATURE phase */
+			mutex_lock(&isert_conn->conn_mutex);
+			isert_conn->state = ISER_CONN_FULL_FEATURE;
+			mutex_unlock(&isert_conn->conn_mutex);
 			goto post_send;
 		}
 
@@ -1109,18 +1143,17 @@ post_send:
 }
 
 static void
-isert_rx_login_req(struct iser_rx_desc *rx_desc, int rx_buflen,
-		   struct isert_conn *isert_conn)
+isert_rx_login_req(struct isert_conn *isert_conn)
 {
+	struct iser_rx_desc *rx_desc = (void *)isert_conn->login_req_buf;
+	int rx_buflen = isert_conn->login_req_len;
 	struct iscsi_conn *conn = isert_conn->conn;
 	struct iscsi_login *login = conn->conn_login;
 	int size;
 
-	if (!login) {
-		pr_err("conn->conn_login is NULL\n");
-		dump_stack();
-		return;
-	}
+	isert_info("conn %p\n", isert_conn);
+
+	WARN_ON_ONCE(!login);
 
 	if (login->first_request) {
 		struct iscsi_login_req *login_req =
@@ -1146,8 +1179,9 @@ isert_rx_login_req(struct iser_rx_desc *rx_desc, int rx_buflen,
 	memcpy(&login->req[0], (void *)&rx_desc->iscsi_header, ISCSI_HDR_LEN);
 
 	size = min(rx_buflen, MAX_KEY_VALUE_PAIRS);
-	pr_debug("Using login payload size: %d, rx_buflen: %d MAX_KEY_VALUE_PAIRS: %d\n",
-		 size, rx_buflen, MAX_KEY_VALUE_PAIRS);
+	isert_dbg("Using login payload size: %d, rx_buflen: %d "
+		  "MAX_KEY_VALUE_PAIRS: %d\n", size, rx_buflen,
+		  MAX_KEY_VALUE_PAIRS);
 	memcpy(login->req_buf, &rx_desc->data[0], size);
 
 	if (login->first_request) {
@@ -1166,7 +1200,7 @@ static struct iscsi_cmd
 
 	cmd = iscsit_allocate_cmd(conn, TASK_INTERRUPTIBLE);
 	if (!cmd) {
-		pr_err("Unable to allocate iscsi_cmd + isert_cmd\n");
+		isert_err("Unable to allocate iscsi_cmd + isert_cmd\n");
 		return NULL;
 	}
 	isert_cmd = iscsit_priv_cmd(cmd);
@@ -1209,8 +1243,8 @@ isert_handle_scsi_cmd(struct isert_conn *isert_conn,
 	sg = &cmd->se_cmd.t_data_sg[0];
 	sg_nents = max(1UL, DIV_ROUND_UP(imm_data_len, PAGE_SIZE));
 
-	pr_debug("Copying Immediate SG: %p sg_nents: %u from %p imm_data_len: %d\n",
-		 sg, sg_nents, &rx_desc->data[0], imm_data_len);
+	isert_dbg("Copying Immediate SG: %p sg_nents: %u from %p imm_data_len: %d\n",
+		  sg, sg_nents, &rx_desc->data[0], imm_data_len);
 
 	sg_copy_from_buffer(sg, sg_nents, &rx_desc->data[0], imm_data_len);
 
@@ -1254,13 +1288,15 @@ isert_handle_iscsi_dataout(struct isert_conn *isert_conn,
 	 * FIXME: Unexpected unsolicited_data out
 	 */
 	if (!cmd->unsolicited_data) {
-		pr_err("Received unexpected solicited data payload\n");
+		isert_err("Received unexpected solicited data payload\n");
 		dump_stack();
 		return -1;
 	}
 
-	pr_debug("Unsolicited DataOut unsol_data_len: %u, write_data_done: %u, data_length: %u\n",
-		 unsol_data_len, cmd->write_data_done, cmd->se_cmd.data_length);
+	isert_dbg("Unsolicited DataOut unsol_data_len: %u, "
+		  "write_data_done: %u, data_length: %u\n",
+		  unsol_data_len,  cmd->write_data_done,
+		  cmd->se_cmd.data_length);
 
 	sg_off = cmd->write_data_done / PAGE_SIZE;
 	sg_start = &cmd->se_cmd.t_data_sg[sg_off];
@@ -1270,12 +1306,13 @@ isert_handle_iscsi_dataout(struct isert_conn *isert_conn,
 	 * FIXME: Non page-aligned unsolicited_data out
 	 */
 	if (page_off) {
-		pr_err("Received unexpected non-page aligned data payload\n");
+		isert_err("unexpected non-page aligned data payload\n");
 		dump_stack();
 		return -1;
 	}
-	pr_debug("Copying DataOut: sg_start: %p, sg_off: %u sg_nents: %u from %p %u\n",
-		 sg_start, sg_off, sg_nents, &rx_desc->data[0], unsol_data_len);
+	isert_dbg("Copying DataOut: sg_start: %p, sg_off: %u "
+		  "sg_nents: %u from %p %u\n", sg_start, sg_off,
+		  sg_nents, &rx_desc->data[0], unsol_data_len);
 
 	sg_copy_from_buffer(sg_start, sg_nents, &rx_desc->data[0],
 			    unsol_data_len);
@@ -1322,8 +1359,8 @@ isert_handle_text_cmd(struct isert_conn *isert_conn, struct isert_cmd *isert_cmd
 
 	text_in = kzalloc(payload_length, GFP_KERNEL);
 	if (!text_in) {
-		pr_err("Unable to allocate text_in of payload_length: %u\n",
-		       payload_length);
+		isert_err("Unable to allocate text_in of payload_length: %u\n",
+			  payload_length);
 		return -ENOMEM;
 	}
 	cmd->text_in_ptr = text_in;
@@ -1348,8 +1385,8 @@ isert_rx_opcode(struct isert_conn *isert_conn, struct iser_rx_desc *rx_desc,
 
 	if (sess->sess_ops->SessionType &&
 	   (!(opcode & ISCSI_OP_TEXT) || !(opcode & ISCSI_OP_LOGOUT))) {
-		pr_err("Got illegal opcode: 0x%02x in SessionType=Discovery,"
-		       " ignoring\n", opcode);
+		isert_err("Got illegal opcode: 0x%02x in SessionType=Discovery,"
+			  " ignoring\n", opcode);
 		return 0;
 	}
 
@@ -1395,10 +1432,6 @@ isert_rx_opcode(struct isert_conn *isert_conn, struct iser_rx_desc *rx_desc,
 			break;
 
 		ret = iscsit_handle_logout_cmd(conn, cmd, (unsigned char *)hdr);
-		if (ret > 0)
-			wait_for_completion_timeout(&conn->conn_logout_comp,
-						    SECONDS_FOR_LOGOUT_COMP *
-						    HZ);
 		break;
 	case ISCSI_OP_TEXT:
 		cmd = isert_allocate_cmd(conn);
@@ -1410,7 +1443,7 @@ isert_rx_opcode(struct isert_conn *isert_conn, struct iser_rx_desc *rx_desc,
 					    rx_desc, (struct iscsi_text *)hdr);
 		break;
 	default:
-		pr_err("Got unknown iSCSI OpCode: 0x%02x\n", opcode);
+		isert_err("Got unknown iSCSI OpCode: 0x%02x\n", opcode);
 		dump_stack();
 		break;
 	}
@@ -1431,23 +1464,23 @@ isert_rx_do_work(struct iser_rx_desc *rx_desc, struct isert_conn *isert_conn)
 		if (iser_hdr->flags & ISER_RSV) {
 			read_stag = be32_to_cpu(iser_hdr->read_stag);
 			read_va = be64_to_cpu(iser_hdr->read_va);
-			pr_debug("ISER_RSV: read_stag: 0x%08x read_va: 0x%16llx\n",
-				 read_stag, (unsigned long long)read_va);
+			isert_dbg("ISER_RSV: read_stag: 0x%x read_va: 0x%llx\n",
+				  read_stag, (unsigned long long)read_va);
 		}
 		if (iser_hdr->flags & ISER_WSV) {
 			write_stag = be32_to_cpu(iser_hdr->write_stag);
 			write_va = be64_to_cpu(iser_hdr->write_va);
-			pr_debug("ISER_WSV: write__stag: 0x%08x write_va: 0x%16llx\n",
-				 write_stag, (unsigned long long)write_va);
+			isert_dbg("ISER_WSV: write_stag: 0x%x write_va: 0x%llx\n",
+				  write_stag, (unsigned long long)write_va);
 		}
 
-		pr_debug("ISER ISCSI_CTRL PDU\n");
+		isert_dbg("ISER ISCSI_CTRL PDU\n");
 		break;
 	case ISER_HELLO:
-		pr_err("iSER Hello message\n");
+		isert_err("iSER Hello message\n");
 		break;
 	default:
-		pr_warn("Unknown iSER hdr flags: 0x%02x\n", iser_hdr->flags);
+		isert_warn("Unknown iSER hdr flags: 0x%02x\n", iser_hdr->flags);
 		break;
 	}
 
@@ -1457,7 +1490,7 @@ isert_rx_do_work(struct iser_rx_desc *rx_desc, struct isert_conn *isert_conn)
 
 static void
 isert_rx_completion(struct iser_rx_desc *desc, struct isert_conn *isert_conn,
-		    unsigned long xfer_len)
+		    u32 xfer_len)
 {
 	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
 	struct iscsi_hdr *hdr;
@@ -1467,34 +1500,43 @@ isert_rx_completion(struct iser_rx_desc *desc, struct isert_conn *isert_conn,
 	if ((char *)desc == isert_conn->login_req_buf) {
 		rx_dma = isert_conn->login_req_dma;
 		rx_buflen = ISER_RX_LOGIN_SIZE;
-		pr_debug("ISER login_buf: Using rx_dma: 0x%llx, rx_buflen: %d\n",
+		isert_dbg("login_buf: Using rx_dma: 0x%llx, rx_buflen: %d\n",
 			 rx_dma, rx_buflen);
 	} else {
 		rx_dma = desc->dma_addr;
 		rx_buflen = ISER_RX_PAYLOAD_SIZE;
-		pr_debug("ISER req_buf: Using rx_dma: 0x%llx, rx_buflen: %d\n",
+		isert_dbg("req_buf: Using rx_dma: 0x%llx, rx_buflen: %d\n",
 			 rx_dma, rx_buflen);
 	}
 
 	ib_dma_sync_single_for_cpu(ib_dev, rx_dma, rx_buflen, DMA_FROM_DEVICE);
 
 	hdr = &desc->iscsi_header;
-	pr_debug("iSCSI opcode: 0x%02x, ITT: 0x%08x, flags: 0x%02x dlen: %d\n",
+	isert_dbg("iSCSI opcode: 0x%02x, ITT: 0x%08x, flags: 0x%02x dlen: %d\n",
 		 hdr->opcode, hdr->itt, hdr->flags,
 		 (int)(xfer_len - ISER_HEADERS_LEN));
 
-	if ((char *)desc == isert_conn->login_req_buf)
-		isert_rx_login_req(desc, xfer_len - ISER_HEADERS_LEN,
-				   isert_conn);
-	else
+	if ((char *)desc == isert_conn->login_req_buf) {
+		isert_conn->login_req_len = xfer_len - ISER_HEADERS_LEN;
+		if (isert_conn->conn) {
+			struct iscsi_login *login = isert_conn->conn->conn_login;
+
+			if (login && !login->first_request)
+				isert_rx_login_req(isert_conn);
+		}
+		mutex_lock(&isert_conn->conn_mutex);
+		complete(&isert_conn->login_req_comp);
+		mutex_unlock(&isert_conn->conn_mutex);
+	} else {
 		isert_rx_do_work(desc, isert_conn);
+	}
 
 	ib_dma_sync_single_for_device(ib_dev, rx_dma, rx_buflen,
 				      DMA_FROM_DEVICE);
 
 	isert_conn->post_recv_buf_count--;
-	pr_debug("iSERT: Decremented post_recv_buf_count: %d\n",
-		 isert_conn->post_recv_buf_count);
+	isert_dbg("Decremented post_recv_buf_count: %d\n",
+		  isert_conn->post_recv_buf_count);
 
 	if ((char *)desc == isert_conn->login_req_buf)
 		return;
@@ -1505,7 +1547,7 @@ isert_rx_completion(struct iser_rx_desc *desc, struct isert_conn *isert_conn,
 				ISERT_MIN_POSTED_RX);
 		err = isert_post_recv(isert_conn, count);
 		if (err) {
-			pr_err("isert_post_recv() count: %d failed, %d\n",
+			isert_err("isert_post_recv() count: %d failed, %d\n",
 			       count, err);
 		}
 	}
@@ -1534,12 +1576,12 @@ isert_map_data_buf(struct isert_conn *isert_conn, struct isert_cmd *isert_cmd,
 	data->dma_nents = ib_dma_map_sg(ib_dev, data->sg, data->nents,
 					data->dma_dir);
 	if (unlikely(!data->dma_nents)) {
-		pr_err("Cmd: unable to dma map SGs %p\n", sg);
+		isert_err("Cmd: unable to dma map SGs %p\n", sg);
 		return -EINVAL;
 	}
 
-	pr_debug("Mapped cmd: %p count: %u sg: %p sg_nents: %u rdma_len %d\n",
-		 isert_cmd, data->dma_nents, data->sg, data->nents, data->len);
+	isert_dbg("Mapped cmd: %p count: %u sg: %p sg_nents: %u rdma_len %d\n",
+		  isert_cmd, data->dma_nents, data->sg, data->nents, data->len);
 
 	return 0;
 }
@@ -1560,21 +1602,21 @@ isert_unmap_cmd(struct isert_cmd *isert_cmd, struct isert_conn *isert_conn)
 {
 	struct isert_rdma_wr *wr = &isert_cmd->rdma_wr;
 
-	pr_debug("isert_unmap_cmd: %p\n", isert_cmd);
+	isert_dbg("Cmd %p\n", isert_cmd);
 
 	if (wr->data.sg) {
-		pr_debug("isert_unmap_cmd: %p unmap_sg op\n", isert_cmd);
+		isert_dbg("Cmd %p unmap_sg op\n", isert_cmd);
 		isert_unmap_data_buf(isert_conn, &wr->data);
 	}
 
 	if (wr->send_wr) {
-		pr_debug("isert_unmap_cmd: %p free send_wr\n", isert_cmd);
+		isert_dbg("Cmd %p free send_wr\n", isert_cmd);
 		kfree(wr->send_wr);
 		wr->send_wr = NULL;
 	}
 
 	if (wr->ib_sge) {
-		pr_debug("isert_unmap_cmd: %p free ib_sge\n", isert_cmd);
+		isert_dbg("Cmd %p free ib_sge\n", isert_cmd);
 		kfree(wr->ib_sge);
 		wr->ib_sge = NULL;
 	}
@@ -1586,11 +1628,10 @@ isert_unreg_rdma(struct isert_cmd *isert_cmd, struct isert_conn *isert_conn)
 	struct isert_rdma_wr *wr = &isert_cmd->rdma_wr;
 	LIST_HEAD(unmap_list);
 
-	pr_debug("unreg_fastreg_cmd: %p\n", isert_cmd);
+	isert_dbg("Cmd %p\n", isert_cmd);
 
 	if (wr->fr_desc) {
-		pr_debug("unreg_fastreg_cmd: %p free fr_desc %p\n",
-			 isert_cmd, wr->fr_desc);
+		isert_dbg("Cmd %p free fr_desc %p\n", isert_cmd, wr->fr_desc);
 		if (wr->fr_desc->ind & ISERT_PROTECTED) {
 			isert_unmap_data_buf(isert_conn, &wr->prot);
 			wr->fr_desc->ind &= ~ISERT_PROTECTED;
@@ -1602,7 +1643,7 @@ isert_unreg_rdma(struct isert_cmd *isert_cmd, struct isert_conn *isert_conn)
 	}
 
 	if (wr->data.sg) {
-		pr_debug("unreg_fastreg_cmd: %p unmap_sg op\n", isert_cmd);
+		isert_dbg("Cmd %p unmap_sg op\n", isert_cmd);
 		isert_unmap_data_buf(isert_conn, &wr->data);
 	}
 
@@ -1618,7 +1659,7 @@ isert_put_cmd(struct isert_cmd *isert_cmd, bool comp_err)
 	struct iscsi_conn *conn = isert_conn->conn;
 	struct isert_device *device = isert_conn->conn_device;
 
-	pr_debug("Entering isert_put_cmd: %p\n", isert_cmd);
+	isert_dbg("Cmd %p\n", isert_cmd);
 
 	switch (cmd->iscsi_opcode) {
 	case ISCSI_OP_SCSI_CMD:
@@ -1668,7 +1709,7 @@ isert_put_cmd(struct isert_cmd *isert_cmd, bool comp_err)
 		 * associated cmd->se_cmd needs to be released.
 		 */
 		if (cmd->se_cmd.se_tfo != NULL) {
-			pr_debug("Calling transport_generic_free_cmd from"
+			isert_dbg("Calling transport_generic_free_cmd from"
 				 " isert_put_cmd for 0x%02x\n",
 				 cmd->iscsi_opcode);
 			transport_generic_free_cmd(&cmd->se_cmd, 0);
@@ -1687,7 +1728,7 @@ static void
 isert_unmap_tx_desc(struct iser_tx_desc *tx_desc, struct ib_device *ib_dev)
 {
 	if (tx_desc->dma_addr != 0) {
-		pr_debug("Calling ib_dma_unmap_single for tx_desc->dma_addr\n");
+		isert_dbg("unmap single for tx_desc->dma_addr\n");
 		ib_dma_unmap_single(ib_dev, tx_desc->dma_addr,
 				    ISER_HEADERS_LEN, DMA_TO_DEVICE);
 		tx_desc->dma_addr = 0;
@@ -1699,7 +1740,7 @@ isert_completion_put(struct iser_tx_desc *tx_desc, struct isert_cmd *isert_cmd,
 		     struct ib_device *ib_dev, bool comp_err)
 {
 	if (isert_cmd->pdu_buf_dma != 0) {
-		pr_debug("Calling ib_dma_unmap_single for isert_cmd->pdu_buf_dma\n");
+		isert_dbg("unmap single for isert_cmd->pdu_buf_dma\n");
 		ib_dma_unmap_single(ib_dev, isert_cmd->pdu_buf_dma,
 				    isert_cmd->pdu_buf_len, DMA_TO_DEVICE);
 		isert_cmd->pdu_buf_dma = 0;
@@ -1717,7 +1758,7 @@ isert_check_pi_status(struct se_cmd *se_cmd, struct ib_mr *sig_mr)
 
 	ret = ib_check_mr_status(sig_mr, IB_MR_CHECK_SIG_STATUS, &mr_status);
 	if (ret) {
-		pr_err("ib_check_mr_status failed, ret %d\n", ret);
+		isert_err("ib_check_mr_status failed, ret %d\n", ret);
 		goto fail_mr_status;
 	}
 
@@ -1740,12 +1781,12 @@ isert_check_pi_status(struct se_cmd *se_cmd, struct ib_mr *sig_mr)
 		do_div(sec_offset_err, block_size);
 		se_cmd->bad_sector = sec_offset_err + se_cmd->t_task_lba;
 
-		pr_err("isert: PI error found type %d at sector 0x%llx "
-		       "expected 0x%x vs actual 0x%x\n",
-		       mr_status.sig_err.err_type,
-		       (unsigned long long)se_cmd->bad_sector,
-		       mr_status.sig_err.expected,
-		       mr_status.sig_err.actual);
+		isert_err("PI error found type %d at sector 0x%llx "
+			  "expected 0x%x vs actual 0x%x\n",
+			  mr_status.sig_err.err_type,
+			  (unsigned long long)se_cmd->bad_sector,
+			  mr_status.sig_err.expected,
+			  mr_status.sig_err.actual);
 		ret = 1;
 	}
 
@@ -1801,7 +1842,7 @@ isert_completion_rdma_read(struct iser_tx_desc *tx_desc,
 	cmd->write_data_done = wr->data.len;
 	wr->send_wr_num = 0;
 
-	pr_debug("Cmd: %p RDMA_READ comp calling execute_cmd\n", isert_cmd);
+	isert_dbg("Cmd: %p RDMA_READ comp calling execute_cmd\n", isert_cmd);
 	spin_lock_bh(&cmd->istate_lock);
 	cmd->cmd_flags |= ICF_GOT_LAST_DATAOUT;
 	cmd->i_state = ISTATE_RECEIVED_LAST_DATAOUT;
@@ -1823,36 +1864,22 @@ isert_do_control_comp(struct work_struct *work)
 	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
 	struct iscsi_cmd *cmd = isert_cmd->iscsi_cmd;
 
+	isert_dbg("Cmd %p i_state %d\n", isert_cmd, cmd->i_state);
+
 	switch (cmd->i_state) {
 	case ISTATE_SEND_TASKMGTRSP:
-		pr_debug("Calling iscsit_tmr_post_handler >>>>>>>>>>>>>>>>>\n");
-
-		atomic_dec(&isert_conn->post_send_buf_count);
 		iscsit_tmr_post_handler(cmd, cmd->conn);
-
-		cmd->i_state = ISTATE_SENT_STATUS;
-		isert_completion_put(&isert_cmd->tx_desc, isert_cmd, ib_dev, false);
-		break;
-	case ISTATE_SEND_REJECT:
-		pr_debug("Got isert_do_control_comp ISTATE_SEND_REJECT: >>>\n");
-		atomic_dec(&isert_conn->post_send_buf_count);
-
+	case ISTATE_SEND_REJECT:   /* FALLTHRU */
+	case ISTATE_SEND_TEXTRSP:  /* FALLTHRU */
 		cmd->i_state = ISTATE_SENT_STATUS;
-		isert_completion_put(&isert_cmd->tx_desc, isert_cmd, ib_dev, false);
+		isert_completion_put(&isert_cmd->tx_desc, isert_cmd,
+				     ib_dev, false);
 		break;
 	case ISTATE_SEND_LOGOUTRSP:
-		pr_debug("Calling iscsit_logout_post_handler >>>>>>>>>>>>>>\n");
-
-		atomic_dec(&isert_conn->post_send_buf_count);
 		iscsit_logout_post_handler(cmd, cmd->conn);
 		break;
-	case ISTATE_SEND_TEXTRSP:
-		atomic_dec(&isert_conn->post_send_buf_count);
-		cmd->i_state = ISTATE_SENT_STATUS;
-		isert_completion_put(&isert_cmd->tx_desc, isert_cmd, ib_dev, false);
-		break;
 	default:
-		pr_err("Unknown do_control_comp i_state %d\n", cmd->i_state);
+		isert_err("Unknown i_state %d\n", cmd->i_state);
 		dump_stack();
 		break;
 	}
@@ -1865,7 +1892,6 @@ isert_response_completion(struct iser_tx_desc *tx_desc,
 			  struct ib_device *ib_dev)
 {
 	struct iscsi_cmd *cmd = isert_cmd->iscsi_cmd;
-	struct isert_rdma_wr *wr = &isert_cmd->rdma_wr;
 
 	if (cmd->i_state == ISTATE_SEND_TASKMGTRSP ||
 	    cmd->i_state == ISTATE_SEND_LOGOUTRSP ||
@@ -1878,267 +1904,151 @@ isert_response_completion(struct iser_tx_desc *tx_desc,
 		return;
 	}
 
-	/**
-	 * If send_wr_num is 0 this means that we got
-	 * RDMA completion and we cleared it and we should
-	 * simply decrement the response post. else the
-	 * response is incorporated in send_wr_num, just
-	 * sub it.
-	 **/
-	if (wr->send_wr_num)
-		atomic_sub(wr->send_wr_num, &isert_conn->post_send_buf_count);
-	else
-		atomic_dec(&isert_conn->post_send_buf_count);
-
 	cmd->i_state = ISTATE_SENT_STATUS;
 	isert_completion_put(tx_desc, isert_cmd, ib_dev, false);
 }
 
 static void
-__isert_send_completion(struct iser_tx_desc *tx_desc,
-		        struct isert_conn *isert_conn)
+isert_send_completion(struct iser_tx_desc *tx_desc,
+		      struct isert_conn *isert_conn)
 {
 	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
 	struct isert_cmd *isert_cmd = tx_desc->isert_cmd;
 	struct isert_rdma_wr *wr;
 
 	if (!isert_cmd) {
-		atomic_dec(&isert_conn->post_send_buf_count);
 		isert_unmap_tx_desc(tx_desc, ib_dev);
 		return;
 	}
 	wr = &isert_cmd->rdma_wr;
 
+	isert_dbg("Cmd %p iser_ib_op %d\n", isert_cmd, wr->iser_ib_op);
+
 	switch (wr->iser_ib_op) {
 	case ISER_IB_RECV:
-		pr_err("isert_send_completion: Got ISER_IB_RECV\n");
+		isert_err("Got ISER_IB_RECV\n");
 		dump_stack();
 		break;
 	case ISER_IB_SEND:
-		pr_debug("isert_send_completion: Got ISER_IB_SEND\n");
 		isert_response_completion(tx_desc, isert_cmd,
 					  isert_conn, ib_dev);
 		break;
 	case ISER_IB_RDMA_WRITE:
-		pr_debug("isert_send_completion: Got ISER_IB_RDMA_WRITE\n");
-		atomic_sub(wr->send_wr_num, &isert_conn->post_send_buf_count);
 		isert_completion_rdma_write(tx_desc, isert_cmd);
 		break;
 	case ISER_IB_RDMA_READ:
-		pr_debug("isert_send_completion: Got ISER_IB_RDMA_READ:\n");
-
-		atomic_sub(wr->send_wr_num, &isert_conn->post_send_buf_count);
 		isert_completion_rdma_read(tx_desc, isert_cmd);
 		break;
 	default:
-		pr_err("Unknown wr->iser_ib_op: 0x%02x\n", wr->iser_ib_op);
+		isert_err("Unknown wr->iser_ib_op: 0x%x\n", wr->iser_ib_op);
 		dump_stack();
 		break;
 	}
 }
 
-static void
-isert_send_completion(struct iser_tx_desc *tx_desc,
-		      struct isert_conn *isert_conn)
-{
-	struct llist_node *llnode = tx_desc->comp_llnode_batch;
-	struct iser_tx_desc *t;
-	/*
-	 * Drain coalesced completion llist starting from comp_llnode_batch
-	 * setup in isert_init_send_wr(), and then complete trailing tx_desc.
-	 */
-	while (llnode) {
-		t = llist_entry(llnode, struct iser_tx_desc, comp_llnode);
-		llnode = llist_next(llnode);
-		__isert_send_completion(t, isert_conn);
-	}
-	__isert_send_completion(tx_desc, isert_conn);
-}
-
-static void
-isert_cq_drain_comp_llist(struct isert_conn *isert_conn, struct ib_device *ib_dev)
+/**
+ * is_isert_tx_desc() - Indicate if the completion wr_id
+ *     is a TX descriptor or not.
+ * @isert_conn: iser connection
+ * @wr_id: completion WR identifier
+ *
+ * Since we cannot rely on wc opcode in FLUSH errors
+ * we must work around it by checking if the wr_id address
+ * falls in the iser connection rx_descs buffer. If so
+ * it is an RX descriptor, otherwize it is a TX.
+ */
+static inline bool
+is_isert_tx_desc(struct isert_conn *isert_conn, void *wr_id)
 {
-	struct llist_node *llnode;
-	struct isert_rdma_wr *wr;
-	struct iser_tx_desc *t;
+	void *start = isert_conn->conn_rx_descs;
+	int len = ISERT_QP_MAX_RECV_DTOS * sizeof(*isert_conn->conn_rx_descs);
 
-	mutex_lock(&isert_conn->conn_mutex);
-	llnode = llist_del_all(&isert_conn->conn_comp_llist);
-	isert_conn->conn_comp_batch = 0;
-	mutex_unlock(&isert_conn->conn_mutex);
-
-	while (llnode) {
-		t = llist_entry(llnode, struct iser_tx_desc, comp_llnode);
-		llnode = llist_next(llnode);
-		wr = &t->isert_cmd->rdma_wr;
-
-		/**
-		 * If send_wr_num is 0 this means that we got
-		 * RDMA completion and we cleared it and we should
-		 * simply decrement the response post. else the
-		 * response is incorporated in send_wr_num, just
-		 * sub it.
-		 **/
-		if (wr->send_wr_num)
-			atomic_sub(wr->send_wr_num,
-				   &isert_conn->post_send_buf_count);
-		else
-			atomic_dec(&isert_conn->post_send_buf_count);
+	if (wr_id >= start && wr_id < start + len)
+		return false;
 
-		isert_completion_put(t, t->isert_cmd, ib_dev, true);
-	}
+	return true;
 }
 
 static void
-isert_cq_tx_comp_err(struct iser_tx_desc *tx_desc, struct isert_conn *isert_conn)
+isert_cq_comp_err(struct isert_conn *isert_conn, struct ib_wc *wc)
 {
-	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
-	struct isert_cmd *isert_cmd = tx_desc->isert_cmd;
-	struct llist_node *llnode = tx_desc->comp_llnode_batch;
-	struct isert_rdma_wr *wr;
-	struct iser_tx_desc *t;
-
-	while (llnode) {
-		t = llist_entry(llnode, struct iser_tx_desc, comp_llnode);
-		llnode = llist_next(llnode);
-		wr = &t->isert_cmd->rdma_wr;
+	if (wc->wr_id == ISER_BEACON_WRID) {
+		isert_info("conn %p completing conn_wait_comp_err\n",
+			   isert_conn);
+		complete(&isert_conn->conn_wait_comp_err);
+	} else if (is_isert_tx_desc(isert_conn, (void *)(uintptr_t)wc->wr_id)) {
+		struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+		struct isert_cmd *isert_cmd;
+		struct iser_tx_desc *desc;
 
-		/**
-		 * If send_wr_num is 0 this means that we got
-		 * RDMA completion and we cleared it and we should
-		 * simply decrement the response post. else the
-		 * response is incorporated in send_wr_num, just
-		 * sub it.
-		 **/
-		if (wr->send_wr_num)
-			atomic_sub(wr->send_wr_num,
-				   &isert_conn->post_send_buf_count);
+		desc = (struct iser_tx_desc *)(uintptr_t)wc->wr_id;
+		isert_cmd = desc->isert_cmd;
+		if (!isert_cmd)
+			isert_unmap_tx_desc(desc, ib_dev);
 		else
-			atomic_dec(&isert_conn->post_send_buf_count);
-
-		isert_completion_put(t, t->isert_cmd, ib_dev, true);
-	}
-	tx_desc->comp_llnode_batch = NULL;
-
-	if (!isert_cmd)
-		isert_unmap_tx_desc(tx_desc, ib_dev);
-	else
-		isert_completion_put(tx_desc, isert_cmd, ib_dev, true);
-}
-
-static void
-isert_cq_rx_comp_err(struct isert_conn *isert_conn)
-{
-	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
-	struct iscsi_conn *conn = isert_conn->conn;
-
-	if (isert_conn->post_recv_buf_count)
-		return;
-
-	isert_cq_drain_comp_llist(isert_conn, ib_dev);
-
-	if (conn->sess) {
-		target_sess_cmd_list_set_waiting(conn->sess->se_sess);
-		target_wait_for_sess_cmds(conn->sess->se_sess);
+			isert_completion_put(desc, isert_cmd, ib_dev, true);
+	} else {
+		isert_conn->post_recv_buf_count--;
+		if (!isert_conn->post_recv_buf_count)
+			iscsit_cause_connection_reinstatement(isert_conn->conn, 0);
 	}
-
-	while (atomic_read(&isert_conn->post_send_buf_count))
-		msleep(3000);
-
-	mutex_lock(&isert_conn->conn_mutex);
-	isert_conn->state = ISER_CONN_DOWN;
-	mutex_unlock(&isert_conn->conn_mutex);
-
-	iscsit_cause_connection_reinstatement(isert_conn->conn, 0);
-
-	complete(&isert_conn->conn_wait_comp_err);
 }
 
 static void
-isert_cq_tx_work(struct work_struct *work)
+isert_handle_wc(struct ib_wc *wc)
 {
-	struct isert_cq_desc *cq_desc = container_of(work,
-				struct isert_cq_desc, cq_tx_work);
-	struct isert_device *device = cq_desc->device;
-	int cq_index = cq_desc->cq_index;
-	struct ib_cq *tx_cq = device->dev_tx_cq[cq_index];
 	struct isert_conn *isert_conn;
 	struct iser_tx_desc *tx_desc;
-	struct ib_wc wc;
-
-	while (ib_poll_cq(tx_cq, 1, &wc) == 1) {
-		tx_desc = (struct iser_tx_desc *)(unsigned long)wc.wr_id;
-		isert_conn = wc.qp->qp_context;
+	struct iser_rx_desc *rx_desc;
 
-		if (wc.status == IB_WC_SUCCESS) {
-			isert_send_completion(tx_desc, isert_conn);
+	isert_conn = wc->qp->qp_context;
+	if (likely(wc->status == IB_WC_SUCCESS)) {
+		if (wc->opcode == IB_WC_RECV) {
+			rx_desc = (struct iser_rx_desc *)(uintptr_t)wc->wr_id;
+			isert_rx_completion(rx_desc, isert_conn, wc->byte_len);
 		} else {
-			pr_debug("TX wc.status != IB_WC_SUCCESS >>>>>>>>>>>>>>\n");
-			pr_debug("TX wc.status: 0x%08x\n", wc.status);
-			pr_debug("TX wc.vendor_err: 0x%08x\n", wc.vendor_err);
-
-			if (wc.wr_id != ISER_FASTREG_LI_WRID) {
-				if (tx_desc->llnode_active)
-					continue;
-
-				atomic_dec(&isert_conn->post_send_buf_count);
-				isert_cq_tx_comp_err(tx_desc, isert_conn);
-			}
+			tx_desc = (struct iser_tx_desc *)(uintptr_t)wc->wr_id;
+			isert_send_completion(tx_desc, isert_conn);
 		}
-	}
-
-	ib_req_notify_cq(tx_cq, IB_CQ_NEXT_COMP);
-}
-
-static void
-isert_cq_tx_callback(struct ib_cq *cq, void *context)
-{
-	struct isert_cq_desc *cq_desc = (struct isert_cq_desc *)context;
+	} else {
+		if (wc->status != IB_WC_WR_FLUSH_ERR)
+			isert_err("wr id %llx status %d vend_err %x\n",
+				  wc->wr_id, wc->status, wc->vendor_err);
+		else
+			isert_dbg("flush error: wr id %llx\n", wc->wr_id);
 
-	queue_work(isert_comp_wq, &cq_desc->cq_tx_work);
+		if (wc->wr_id != ISER_FASTREG_LI_WRID)
+			isert_cq_comp_err(isert_conn, wc);
+	}
 }
 
 static void
-isert_cq_rx_work(struct work_struct *work)
+isert_cq_work(struct work_struct *work)
 {
-	struct isert_cq_desc *cq_desc = container_of(work,
-			struct isert_cq_desc, cq_rx_work);
-	struct isert_device *device = cq_desc->device;
-	int cq_index = cq_desc->cq_index;
-	struct ib_cq *rx_cq = device->dev_rx_cq[cq_index];
-	struct isert_conn *isert_conn;
-	struct iser_rx_desc *rx_desc;
-	struct ib_wc wc;
-	unsigned long xfer_len;
+	enum { isert_poll_budget = 65536 };
+	struct isert_comp *comp = container_of(work, struct isert_comp,
+					       work);
+	struct ib_wc *const wcs = comp->wcs;
+	int i, n, completed = 0;
 
-	while (ib_poll_cq(rx_cq, 1, &wc) == 1) {
-		rx_desc = (struct iser_rx_desc *)(unsigned long)wc.wr_id;
-		isert_conn = wc.qp->qp_context;
+	while ((n = ib_poll_cq(comp->cq, ARRAY_SIZE(comp->wcs), wcs)) > 0) {
+		for (i = 0; i < n; i++)
+			isert_handle_wc(&wcs[i]);
 
-		if (wc.status == IB_WC_SUCCESS) {
-			xfer_len = (unsigned long)wc.byte_len;
-			isert_rx_completion(rx_desc, isert_conn, xfer_len);
-		} else {
-			pr_debug("RX wc.status != IB_WC_SUCCESS >>>>>>>>>>>>>>\n");
-			if (wc.status != IB_WC_WR_FLUSH_ERR) {
-				pr_debug("RX wc.status: 0x%08x\n", wc.status);
-				pr_debug("RX wc.vendor_err: 0x%08x\n",
-					 wc.vendor_err);
-			}
-			isert_conn->post_recv_buf_count--;
-			isert_cq_rx_comp_err(isert_conn);
-		}
+		completed += n;
+		if (completed >= isert_poll_budget)
+			break;
 	}
 
-	ib_req_notify_cq(rx_cq, IB_CQ_NEXT_COMP);
+	ib_req_notify_cq(comp->cq, IB_CQ_NEXT_COMP);
 }
 
 static void
-isert_cq_rx_callback(struct ib_cq *cq, void *context)
+isert_cq_callback(struct ib_cq *cq, void *context)
 {
-	struct isert_cq_desc *cq_desc = (struct isert_cq_desc *)context;
+	struct isert_comp *comp = context;
 
-	queue_work(isert_rx_wq, &cq_desc->cq_rx_work);
+	queue_work(isert_comp_wq, &comp->work);
 }
 
 static int
@@ -2147,13 +2057,10 @@ isert_post_response(struct isert_conn *isert_conn, struct isert_cmd *isert_cmd)
 	struct ib_send_wr *wr_failed;
 	int ret;
 
-	atomic_inc(&isert_conn->post_send_buf_count);
-
 	ret = ib_post_send(isert_conn->conn_qp, &isert_cmd->tx_desc.send_wr,
 			   &wr_failed);
 	if (ret) {
-		pr_err("ib_post_send failed with %d\n", ret);
-		atomic_dec(&isert_conn->post_send_buf_count);
+		isert_err("ib_post_send failed with %d\n", ret);
 		return ret;
 	}
 	return ret;
@@ -2200,9 +2107,9 @@ isert_put_response(struct iscsi_conn *conn, struct iscsi_cmd *cmd)
 		isert_cmd->tx_desc.num_sge = 2;
 	}
 
-	isert_init_send_wr(isert_conn, isert_cmd, send_wr, false);
+	isert_init_send_wr(isert_conn, isert_cmd, send_wr);
 
-	pr_debug("Posting SCSI Response IB_WR_SEND >>>>>>>>>>>>>>>>>>>>>>\n");
+	isert_dbg("Posting SCSI Response\n");
 
 	return isert_post_response(isert_conn, isert_cmd);
 }
@@ -2231,8 +2138,16 @@ isert_get_sup_prot_ops(struct iscsi_conn *conn)
 	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
 	struct isert_device *device = isert_conn->conn_device;
 
-	if (device->pi_capable)
-		return TARGET_PROT_ALL;
+	if (conn->tpg->tpg_attrib.t10_pi) {
+		if (device->pi_capable) {
+			isert_info("conn %p PI offload enabled\n", isert_conn);
+			isert_conn->pi_support = true;
+			return TARGET_PROT_ALL;
+		}
+	}
+
+	isert_info("conn %p PI offload disabled\n", isert_conn);
+	isert_conn->pi_support = false;
 
 	return TARGET_PROT_NORMAL;
 }
@@ -2250,9 +2165,9 @@ isert_put_nopin(struct iscsi_cmd *cmd, struct iscsi_conn *conn,
 			       &isert_cmd->tx_desc.iscsi_header,
 			       nopout_response);
 	isert_init_tx_hdrs(isert_conn, &isert_cmd->tx_desc);
-	isert_init_send_wr(isert_conn, isert_cmd, send_wr, false);
+	isert_init_send_wr(isert_conn, isert_cmd, send_wr);
 
-	pr_debug("Posting NOPIN Response IB_WR_SEND >>>>>>>>>>>>>>>>>>>>>>\n");
+	isert_dbg("conn %p Posting NOPIN Response\n", isert_conn);
 
 	return isert_post_response(isert_conn, isert_cmd);
 }
@@ -2268,9 +2183,9 @@ isert_put_logout_rsp(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
 	iscsit_build_logout_rsp(cmd, conn, (struct iscsi_logout_rsp *)
 				&isert_cmd->tx_desc.iscsi_header);
 	isert_init_tx_hdrs(isert_conn, &isert_cmd->tx_desc);
-	isert_init_send_wr(isert_conn, isert_cmd, send_wr, false);
+	isert_init_send_wr(isert_conn, isert_cmd, send_wr);
 
-	pr_debug("Posting Logout Response IB_WR_SEND >>>>>>>>>>>>>>>>>>>>>>\n");
+	isert_dbg("conn %p Posting Logout Response\n", isert_conn);
 
 	return isert_post_response(isert_conn, isert_cmd);
 }
@@ -2286,9 +2201,9 @@ isert_put_tm_rsp(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
 	iscsit_build_task_mgt_rsp(cmd, conn, (struct iscsi_tm_rsp *)
 				  &isert_cmd->tx_desc.iscsi_header);
 	isert_init_tx_hdrs(isert_conn, &isert_cmd->tx_desc);
-	isert_init_send_wr(isert_conn, isert_cmd, send_wr, false);
+	isert_init_send_wr(isert_conn, isert_cmd, send_wr);
 
-	pr_debug("Posting Task Management Response IB_WR_SEND >>>>>>>>>>>>>>>>>>>>>>\n");
+	isert_dbg("conn %p Posting Task Management Response\n", isert_conn);
 
 	return isert_post_response(isert_conn, isert_cmd);
 }
@@ -2318,9 +2233,9 @@ isert_put_reject(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
 	tx_dsg->lkey	= isert_conn->conn_mr->lkey;
 	isert_cmd->tx_desc.num_sge = 2;
 
-	isert_init_send_wr(isert_conn, isert_cmd, send_wr, false);
+	isert_init_send_wr(isert_conn, isert_cmd, send_wr);
 
-	pr_debug("Posting Reject IB_WR_SEND >>>>>>>>>>>>>>>>>>>>>>\n");
+	isert_dbg("conn %p Posting Reject\n", isert_conn);
 
 	return isert_post_response(isert_conn, isert_cmd);
 }
@@ -2358,9 +2273,9 @@ isert_put_text_rsp(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
 		tx_dsg->lkey	= isert_conn->conn_mr->lkey;
 		isert_cmd->tx_desc.num_sge = 2;
 	}
-	isert_init_send_wr(isert_conn, isert_cmd, send_wr, false);
+	isert_init_send_wr(isert_conn, isert_cmd, send_wr);
 
-	pr_debug("Posting Text Response IB_WR_SEND >>>>>>>>>>>>>>>>>>>>>>\n");
+	isert_dbg("conn %p Text Reject\n", isert_conn);
 
 	return isert_post_response(isert_conn, isert_cmd);
 }
@@ -2383,30 +2298,31 @@ isert_build_rdma_wr(struct isert_conn *isert_conn, struct isert_cmd *isert_cmd,
 
 	send_wr->sg_list = ib_sge;
 	send_wr->num_sge = sg_nents;
-	send_wr->wr_id = (unsigned long)&isert_cmd->tx_desc;
+	send_wr->wr_id = (uintptr_t)&isert_cmd->tx_desc;
 	/*
 	 * Perform mapping of TCM scatterlist memory ib_sge dma_addr.
 	 */
 	for_each_sg(sg_start, tmp_sg, sg_nents, i) {
-		pr_debug("ISER RDMA from SGL dma_addr: 0x%16llx dma_len: %u, page_off: %u\n",
-			 (unsigned long long)tmp_sg->dma_address,
-			 tmp_sg->length, page_off);
+		isert_dbg("RDMA from SGL dma_addr: 0x%llx dma_len: %u, "
+			  "page_off: %u\n",
+			  (unsigned long long)tmp_sg->dma_address,
+			  tmp_sg->length, page_off);
 
 		ib_sge->addr = ib_sg_dma_address(ib_dev, tmp_sg) + page_off;
 		ib_sge->length = min_t(u32, data_left,
 				ib_sg_dma_len(ib_dev, tmp_sg) - page_off);
 		ib_sge->lkey = isert_conn->conn_mr->lkey;
 
-		pr_debug("RDMA ib_sge: addr: 0x%16llx  length: %u lkey: %08x\n",
-			 ib_sge->addr, ib_sge->length, ib_sge->lkey);
+		isert_dbg("RDMA ib_sge: addr: 0x%llx  length: %u lkey: %x\n",
+			  ib_sge->addr, ib_sge->length, ib_sge->lkey);
 		page_off = 0;
 		data_left -= ib_sge->length;
 		ib_sge++;
-		pr_debug("Incrementing ib_sge pointer to %p\n", ib_sge);
+		isert_dbg("Incrementing ib_sge pointer to %p\n", ib_sge);
 	}
 
-	pr_debug("Set outgoing sg_list: %p num_sg: %u from TCM SGLs\n",
-		 send_wr->sg_list, send_wr->num_sge);
+	isert_dbg("Set outgoing sg_list: %p num_sg: %u from TCM SGLs\n",
+		  send_wr->sg_list, send_wr->num_sge);
 
 	return sg_nents;
 }
@@ -2438,7 +2354,7 @@ isert_map_rdma(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 
 	ib_sge = kzalloc(sizeof(struct ib_sge) * data->nents, GFP_KERNEL);
 	if (!ib_sge) {
-		pr_warn("Unable to allocate ib_sge\n");
+		isert_warn("Unable to allocate ib_sge\n");
 		ret = -ENOMEM;
 		goto unmap_cmd;
 	}
@@ -2448,7 +2364,7 @@ isert_map_rdma(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 	wr->send_wr = kzalloc(sizeof(struct ib_send_wr) * wr->send_wr_num,
 				GFP_KERNEL);
 	if (!wr->send_wr) {
-		pr_debug("Unable to allocate wr->send_wr\n");
+		isert_dbg("Unable to allocate wr->send_wr\n");
 		ret = -ENOMEM;
 		goto unmap_cmd;
 	}
@@ -2512,9 +2428,9 @@ isert_map_fr_pagelist(struct ib_device *ib_dev,
 			chunk_start = start_addr;
 		end_addr = start_addr + ib_sg_dma_len(ib_dev, tmp_sg);
 
-		pr_debug("SGL[%d] dma_addr: 0x%16llx len: %u\n",
-			 i, (unsigned long long)tmp_sg->dma_address,
-			 tmp_sg->length);
+		isert_dbg("SGL[%d] dma_addr: 0x%llx len: %u\n",
+			  i, (unsigned long long)tmp_sg->dma_address,
+			  tmp_sg->length);
 
 		if ((end_addr & ~PAGE_MASK) && i < last_ent) {
 			new_chunk = 0;
@@ -2525,8 +2441,8 @@ isert_map_fr_pagelist(struct ib_device *ib_dev,
 		page = chunk_start & PAGE_MASK;
 		do {
 			fr_pl[n_pages++] = page;
-			pr_debug("Mapped page_list[%d] page_addr: 0x%16llx\n",
-				 n_pages - 1, page);
+			isert_dbg("Mapped page_list[%d] page_addr: 0x%llx\n",
+				  n_pages - 1, page);
 			page += PAGE_SIZE;
 		} while (page < end_addr);
 	}
@@ -2534,6 +2450,21 @@ isert_map_fr_pagelist(struct ib_device *ib_dev,
 	return n_pages;
 }
 
+static inline void
+isert_inv_rkey(struct ib_send_wr *inv_wr, struct ib_mr *mr)
+{
+	u32 rkey;
+
+	memset(inv_wr, 0, sizeof(*inv_wr));
+	inv_wr->wr_id = ISER_FASTREG_LI_WRID;
+	inv_wr->opcode = IB_WR_LOCAL_INV;
+	inv_wr->ex.invalidate_rkey = mr->rkey;
+
+	/* Bump the key */
+	rkey = ib_inc_rkey(mr->rkey);
+	ib_update_fast_reg_key(mr, rkey);
+}
+
 static int
 isert_fast_reg_mr(struct isert_conn *isert_conn,
 		  struct fast_reg_descriptor *fr_desc,
@@ -2548,15 +2479,13 @@ isert_fast_reg_mr(struct isert_conn *isert_conn,
 	struct ib_send_wr *bad_wr, *wr = NULL;
 	int ret, pagelist_len;
 	u32 page_off;
-	u8 key;
 
 	if (mem->dma_nents == 1) {
 		sge->lkey = isert_conn->conn_mr->lkey;
 		sge->addr = ib_sg_dma_address(ib_dev, &mem->sg[0]);
 		sge->length = ib_sg_dma_len(ib_dev, &mem->sg[0]);
-		pr_debug("%s:%d sge: addr: 0x%llx  length: %u lkey: %x\n",
-			 __func__, __LINE__, sge->addr, sge->length,
-			 sge->lkey);
+		isert_dbg("sge: addr: 0x%llx  length: %u lkey: %x\n",
+			 sge->addr, sge->length, sge->lkey);
 		return 0;
 	}
 
@@ -2572,21 +2501,15 @@ isert_fast_reg_mr(struct isert_conn *isert_conn,
 
 	page_off = mem->offset % PAGE_SIZE;
 
-	pr_debug("Use fr_desc %p sg_nents %d offset %u\n",
-		 fr_desc, mem->nents, mem->offset);
+	isert_dbg("Use fr_desc %p sg_nents %d offset %u\n",
+		  fr_desc, mem->nents, mem->offset);
 
 	pagelist_len = isert_map_fr_pagelist(ib_dev, mem->sg, mem->nents,
 					     &frpl->page_list[0]);
 
-	if (!(fr_desc->ind & ISERT_DATA_KEY_VALID)) {
-		memset(&inv_wr, 0, sizeof(inv_wr));
-		inv_wr.wr_id = ISER_FASTREG_LI_WRID;
-		inv_wr.opcode = IB_WR_LOCAL_INV;
-		inv_wr.ex.invalidate_rkey = mr->rkey;
+	if (!(fr_desc->ind & ind)) {
+		isert_inv_rkey(&inv_wr, mr);
 		wr = &inv_wr;
-		/* Bump the key */
-		key = (u8)(mr->rkey & 0x000000FF);
-		ib_update_fast_reg_key(mr, ++key);
 	}
 
 	/* Prepare FASTREG WR */
@@ -2608,7 +2531,7 @@ isert_fast_reg_mr(struct isert_conn *isert_conn,
 
 	ret = ib_post_send(isert_conn->conn_qp, wr, &bad_wr);
 	if (ret) {
-		pr_err("fast registration failed, ret:%d\n", ret);
+		isert_err("fast registration failed, ret:%d\n", ret);
 		return ret;
 	}
 	fr_desc->ind &= ~ind;
@@ -2617,9 +2540,8 @@ isert_fast_reg_mr(struct isert_conn *isert_conn,
 	sge->addr = frpl->page_list[0] + page_off;
 	sge->length = mem->len;
 
-	pr_debug("%s:%d sge: addr: 0x%llx  length: %u lkey: %x\n",
-		 __func__, __LINE__, sge->addr, sge->length,
-		 sge->lkey);
+	isert_dbg("sge: addr: 0x%llx  length: %u lkey: %x\n",
+		  sge->addr, sge->length, sge->lkey);
 
 	return ret;
 }
@@ -2665,7 +2587,7 @@ isert_set_sig_attrs(struct se_cmd *se_cmd, struct ib_sig_attrs *sig_attrs)
 		isert_set_dif_domain(se_cmd, sig_attrs, &sig_attrs->mem);
 		break;
 	default:
-		pr_err("Unsupported PI operation %d\n", se_cmd->prot_op);
+		isert_err("Unsupported PI operation %d\n", se_cmd->prot_op);
 		return -EINVAL;
 	}
 
@@ -2681,17 +2603,16 @@ isert_set_prot_checks(u8 prot_checks)
 }
 
 static int
-isert_reg_sig_mr(struct isert_conn *isert_conn, struct se_cmd *se_cmd,
-		 struct fast_reg_descriptor *fr_desc,
-		 struct ib_sge *data_sge, struct ib_sge *prot_sge,
-		 struct ib_sge *sig_sge)
+isert_reg_sig_mr(struct isert_conn *isert_conn,
+		 struct se_cmd *se_cmd,
+		 struct isert_rdma_wr *rdma_wr,
+		 struct fast_reg_descriptor *fr_desc)
 {
 	struct ib_send_wr sig_wr, inv_wr;
 	struct ib_send_wr *bad_wr, *wr = NULL;
 	struct pi_context *pi_ctx = fr_desc->pi_ctx;
 	struct ib_sig_attrs sig_attrs;
 	int ret;
-	u32 key;
 
 	memset(&sig_attrs, 0, sizeof(sig_attrs));
 	ret = isert_set_sig_attrs(se_cmd, &sig_attrs);
@@ -2701,26 +2622,20 @@ isert_reg_sig_mr(struct isert_conn *isert_conn, struct se_cmd *se_cmd,
 	sig_attrs.check_mask = isert_set_prot_checks(se_cmd->prot_checks);
 
 	if (!(fr_desc->ind & ISERT_SIG_KEY_VALID)) {
-		memset(&inv_wr, 0, sizeof(inv_wr));
-		inv_wr.opcode = IB_WR_LOCAL_INV;
-		inv_wr.wr_id = ISER_FASTREG_LI_WRID;
-		inv_wr.ex.invalidate_rkey = pi_ctx->sig_mr->rkey;
+		isert_inv_rkey(&inv_wr, pi_ctx->sig_mr);
 		wr = &inv_wr;
-		/* Bump the key */
-		key = (u8)(pi_ctx->sig_mr->rkey & 0x000000FF);
-		ib_update_fast_reg_key(pi_ctx->sig_mr, ++key);
 	}
 
 	memset(&sig_wr, 0, sizeof(sig_wr));
 	sig_wr.opcode = IB_WR_REG_SIG_MR;
 	sig_wr.wr_id = ISER_FASTREG_LI_WRID;
-	sig_wr.sg_list = data_sge;
+	sig_wr.sg_list = &rdma_wr->ib_sg[DATA];
 	sig_wr.num_sge = 1;
 	sig_wr.wr.sig_handover.access_flags = IB_ACCESS_LOCAL_WRITE;
 	sig_wr.wr.sig_handover.sig_attrs = &sig_attrs;
 	sig_wr.wr.sig_handover.sig_mr = pi_ctx->sig_mr;
 	if (se_cmd->t_prot_sg)
-		sig_wr.wr.sig_handover.prot = prot_sge;
+		sig_wr.wr.sig_handover.prot = &rdma_wr->ib_sg[PROT];
 
 	if (!wr)
 		wr = &sig_wr;
@@ -2729,39 +2644,98 @@ isert_reg_sig_mr(struct isert_conn *isert_conn, struct se_cmd *se_cmd,
 
 	ret = ib_post_send(isert_conn->conn_qp, wr, &bad_wr);
 	if (ret) {
-		pr_err("fast registration failed, ret:%d\n", ret);
+		isert_err("fast registration failed, ret:%d\n", ret);
 		goto err;
 	}
 	fr_desc->ind &= ~ISERT_SIG_KEY_VALID;
 
-	sig_sge->lkey = pi_ctx->sig_mr->lkey;
-	sig_sge->addr = 0;
-	sig_sge->length = se_cmd->data_length;
+	rdma_wr->ib_sg[SIG].lkey = pi_ctx->sig_mr->lkey;
+	rdma_wr->ib_sg[SIG].addr = 0;
+	rdma_wr->ib_sg[SIG].length = se_cmd->data_length;
 	if (se_cmd->prot_op != TARGET_PROT_DIN_STRIP &&
 	    se_cmd->prot_op != TARGET_PROT_DOUT_INSERT)
 		/*
 		 * We have protection guards on the wire
 		 * so we need to set a larget transfer
 		 */
-		sig_sge->length += se_cmd->prot_length;
+		rdma_wr->ib_sg[SIG].length += se_cmd->prot_length;
 
-	pr_debug("sig_sge: addr: 0x%llx  length: %u lkey: %x\n",
-		 sig_sge->addr, sig_sge->length,
-		 sig_sge->lkey);
+	isert_dbg("sig_sge: addr: 0x%llx  length: %u lkey: %x\n",
+		  rdma_wr->ib_sg[SIG].addr, rdma_wr->ib_sg[SIG].length,
+		  rdma_wr->ib_sg[SIG].lkey);
 err:
 	return ret;
 }
 
 static int
+isert_handle_prot_cmd(struct isert_conn *isert_conn,
+		      struct isert_cmd *isert_cmd,
+		      struct isert_rdma_wr *wr)
+{
+	struct isert_device *device = isert_conn->conn_device;
+	struct se_cmd *se_cmd = &isert_cmd->iscsi_cmd->se_cmd;
+	int ret;
+
+	if (!wr->fr_desc->pi_ctx) {
+		ret = isert_create_pi_ctx(wr->fr_desc,
+					  device->ib_device,
+					  isert_conn->conn_pd);
+		if (ret) {
+			isert_err("conn %p failed to allocate pi_ctx\n",
+				  isert_conn);
+			return ret;
+		}
+	}
+
+	if (se_cmd->t_prot_sg) {
+		ret = isert_map_data_buf(isert_conn, isert_cmd,
+					 se_cmd->t_prot_sg,
+					 se_cmd->t_prot_nents,
+					 se_cmd->prot_length,
+					 0, wr->iser_ib_op, &wr->prot);
+		if (ret) {
+			isert_err("conn %p failed to map protection buffer\n",
+				  isert_conn);
+			return ret;
+		}
+
+		memset(&wr->ib_sg[PROT], 0, sizeof(wr->ib_sg[PROT]));
+		ret = isert_fast_reg_mr(isert_conn, wr->fr_desc, &wr->prot,
+					ISERT_PROT_KEY_VALID, &wr->ib_sg[PROT]);
+		if (ret) {
+			isert_err("conn %p failed to fast reg mr\n",
+				  isert_conn);
+			goto unmap_prot_cmd;
+		}
+	}
+
+	ret = isert_reg_sig_mr(isert_conn, se_cmd, wr, wr->fr_desc);
+	if (ret) {
+		isert_err("conn %p failed to fast reg mr\n",
+			  isert_conn);
+		goto unmap_prot_cmd;
+	}
+	wr->fr_desc->ind |= ISERT_PROTECTED;
+
+	return 0;
+
+unmap_prot_cmd:
+	if (se_cmd->t_prot_sg)
+		isert_unmap_data_buf(isert_conn, &wr->prot);
+
+	return ret;
+}
+
+static int
 isert_reg_rdma(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 	       struct isert_rdma_wr *wr)
 {
 	struct se_cmd *se_cmd = &cmd->se_cmd;
 	struct isert_cmd *isert_cmd = iscsit_priv_cmd(cmd);
 	struct isert_conn *isert_conn = conn->context;
-	struct ib_sge data_sge;
-	struct ib_send_wr *send_wr;
 	struct fast_reg_descriptor *fr_desc = NULL;
+	struct ib_send_wr *send_wr;
+	struct ib_sge *ib_sg;
 	u32 offset;
 	int ret = 0;
 	unsigned long flags;
@@ -2775,8 +2749,7 @@ isert_reg_rdma(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 	if (ret)
 		return ret;
 
-	if (wr->data.dma_nents != 1 ||
-	    se_cmd->prot_op != TARGET_PROT_NORMAL) {
+	if (wr->data.dma_nents != 1 || isert_prot_cmd(isert_conn, se_cmd)) {
 		spin_lock_irqsave(&isert_conn->conn_lock, flags);
 		fr_desc = list_first_entry(&isert_conn->conn_fr_pool,
 					   struct fast_reg_descriptor, list);
@@ -2786,38 +2759,21 @@ isert_reg_rdma(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 	}
 
 	ret = isert_fast_reg_mr(isert_conn, fr_desc, &wr->data,
-				ISERT_DATA_KEY_VALID, &data_sge);
+				ISERT_DATA_KEY_VALID, &wr->ib_sg[DATA]);
 	if (ret)
 		goto unmap_cmd;
 
-	if (se_cmd->prot_op != TARGET_PROT_NORMAL) {
-		struct ib_sge prot_sge, sig_sge;
-
-		if (se_cmd->t_prot_sg) {
-			ret = isert_map_data_buf(isert_conn, isert_cmd,
-						 se_cmd->t_prot_sg,
-						 se_cmd->t_prot_nents,
-						 se_cmd->prot_length,
-						 0, wr->iser_ib_op, &wr->prot);
-			if (ret)
-				goto unmap_cmd;
-
-			ret = isert_fast_reg_mr(isert_conn, fr_desc, &wr->prot,
-						ISERT_PROT_KEY_VALID, &prot_sge);
-			if (ret)
-				goto unmap_prot_cmd;
-		}
-
-		ret = isert_reg_sig_mr(isert_conn, se_cmd, fr_desc,
-				       &data_sge, &prot_sge, &sig_sge);
+	if (isert_prot_cmd(isert_conn, se_cmd)) {
+		ret = isert_handle_prot_cmd(isert_conn, isert_cmd, wr);
 		if (ret)
-			goto unmap_prot_cmd;
+			goto unmap_cmd;
 
-		fr_desc->ind |= ISERT_PROTECTED;
-		memcpy(&wr->s_ib_sge, &sig_sge, sizeof(sig_sge));
-	} else
-		memcpy(&wr->s_ib_sge, &data_sge, sizeof(data_sge));
+		ib_sg = &wr->ib_sg[SIG];
+	} else {
+		ib_sg = &wr->ib_sg[DATA];
+	}
 
+	memcpy(&wr->s_ib_sge, ib_sg, sizeof(*ib_sg));
 	wr->ib_sge = &wr->s_ib_sge;
 	wr->send_wr_num = 1;
 	memset(&wr->s_send_wr, 0, sizeof(*send_wr));
@@ -2827,12 +2783,12 @@ isert_reg_rdma(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 	send_wr = &isert_cmd->rdma_wr.s_send_wr;
 	send_wr->sg_list = &wr->s_ib_sge;
 	send_wr->num_sge = 1;
-	send_wr->wr_id = (unsigned long)&isert_cmd->tx_desc;
+	send_wr->wr_id = (uintptr_t)&isert_cmd->tx_desc;
 	if (wr->iser_ib_op == ISER_IB_RDMA_WRITE) {
 		send_wr->opcode = IB_WR_RDMA_WRITE;
 		send_wr->wr.rdma.remote_addr = isert_cmd->read_va;
 		send_wr->wr.rdma.rkey = isert_cmd->read_stag;
-		send_wr->send_flags = se_cmd->prot_op == TARGET_PROT_NORMAL ?
+		send_wr->send_flags = !isert_prot_cmd(isert_conn, se_cmd) ?
 				      0 : IB_SEND_SIGNALED;
 	} else {
 		send_wr->opcode = IB_WR_RDMA_READ;
@@ -2842,9 +2798,7 @@ isert_reg_rdma(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 	}
 
 	return 0;
-unmap_prot_cmd:
-	if (se_cmd->t_prot_sg)
-		isert_unmap_data_buf(isert_conn, &wr->prot);
+
 unmap_cmd:
 	if (fr_desc) {
 		spin_lock_irqsave(&isert_conn->conn_lock, flags);
@@ -2867,16 +2821,17 @@ isert_put_datain(struct iscsi_conn *conn, struct iscsi_cmd *cmd)
 	struct ib_send_wr *wr_failed;
 	int rc;
 
-	pr_debug("Cmd: %p RDMA_WRITE data_length: %u\n",
+	isert_dbg("Cmd: %p RDMA_WRITE data_length: %u\n",
 		 isert_cmd, se_cmd->data_length);
+
 	wr->iser_ib_op = ISER_IB_RDMA_WRITE;
 	rc = device->reg_rdma_mem(conn, cmd, wr);
 	if (rc) {
-		pr_err("Cmd: %p failed to prepare RDMA res\n", isert_cmd);
+		isert_err("Cmd: %p failed to prepare RDMA res\n", isert_cmd);
 		return rc;
 	}
 
-	if (se_cmd->prot_op == TARGET_PROT_NORMAL) {
+	if (!isert_prot_cmd(isert_conn, se_cmd)) {
 		/*
 		 * Build isert_conn->tx_desc for iSCSI response PDU and attach
 		 */
@@ -2886,24 +2841,20 @@ isert_put_datain(struct iscsi_conn *conn, struct iscsi_cmd *cmd)
 				     &isert_cmd->tx_desc.iscsi_header);
 		isert_init_tx_hdrs(isert_conn, &isert_cmd->tx_desc);
 		isert_init_send_wr(isert_conn, isert_cmd,
-				   &isert_cmd->tx_desc.send_wr, false);
+				   &isert_cmd->tx_desc.send_wr);
 		isert_cmd->rdma_wr.s_send_wr.next = &isert_cmd->tx_desc.send_wr;
 		wr->send_wr_num += 1;
 	}
 
-	atomic_add(wr->send_wr_num, &isert_conn->post_send_buf_count);
-
 	rc = ib_post_send(isert_conn->conn_qp, wr->send_wr, &wr_failed);
-	if (rc) {
-		pr_warn("ib_post_send() failed for IB_WR_RDMA_WRITE\n");
-		atomic_sub(wr->send_wr_num, &isert_conn->post_send_buf_count);
-	}
+	if (rc)
+		isert_warn("ib_post_send() failed for IB_WR_RDMA_WRITE\n");
 
-	if (se_cmd->prot_op == TARGET_PROT_NORMAL)
-		pr_debug("Cmd: %p posted RDMA_WRITE + Response for iSER Data "
+	if (!isert_prot_cmd(isert_conn, se_cmd))
+		isert_dbg("Cmd: %p posted RDMA_WRITE + Response for iSER Data "
 			 "READ\n", isert_cmd);
 	else
-		pr_debug("Cmd: %p posted RDMA_WRITE for iSER Data READ\n",
+		isert_dbg("Cmd: %p posted RDMA_WRITE for iSER Data READ\n",
 			 isert_cmd);
 
 	return 1;
@@ -2920,23 +2871,20 @@ isert_get_dataout(struct iscsi_conn *conn, struct iscsi_cmd *cmd, bool recovery)
 	struct ib_send_wr *wr_failed;
 	int rc;
 
-	pr_debug("Cmd: %p RDMA_READ data_length: %u write_data_done: %u\n",
+	isert_dbg("Cmd: %p RDMA_READ data_length: %u write_data_done: %u\n",
 		 isert_cmd, se_cmd->data_length, cmd->write_data_done);
 	wr->iser_ib_op = ISER_IB_RDMA_READ;
 	rc = device->reg_rdma_mem(conn, cmd, wr);
 	if (rc) {
-		pr_err("Cmd: %p failed to prepare RDMA res\n", isert_cmd);
+		isert_err("Cmd: %p failed to prepare RDMA res\n", isert_cmd);
 		return rc;
 	}
 
-	atomic_add(wr->send_wr_num, &isert_conn->post_send_buf_count);
-
 	rc = ib_post_send(isert_conn->conn_qp, wr->send_wr, &wr_failed);
-	if (rc) {
-		pr_warn("ib_post_send() failed for IB_WR_RDMA_READ\n");
-		atomic_sub(wr->send_wr_num, &isert_conn->post_send_buf_count);
-	}
-	pr_debug("Cmd: %p posted RDMA_READ memory for ISER Data WRITE\n",
+	if (rc)
+		isert_warn("ib_post_send() failed for IB_WR_RDMA_READ\n");
+
+	isert_dbg("Cmd: %p posted RDMA_READ memory for ISER Data WRITE\n",
 		 isert_cmd);
 
 	return 0;
@@ -2952,7 +2900,7 @@ isert_immediate_queue(struct iscsi_conn *conn, struct iscsi_cmd *cmd, int state)
 		ret = isert_put_nopin(cmd, conn, false);
 		break;
 	default:
-		pr_err("Unknown immediate state: 0x%02x\n", state);
+		isert_err("Unknown immediate state: 0x%02x\n", state);
 		ret = -EINVAL;
 		break;
 	}
@@ -2963,15 +2911,14 @@ isert_immediate_queue(struct iscsi_conn *conn, struct iscsi_cmd *cmd, int state)
 static int
 isert_response_queue(struct iscsi_conn *conn, struct iscsi_cmd *cmd, int state)
 {
+	struct isert_conn *isert_conn = conn->context;
 	int ret;
 
 	switch (state) {
 	case ISTATE_SEND_LOGOUTRSP:
 		ret = isert_put_logout_rsp(cmd, conn);
-		if (!ret) {
-			pr_debug("Returning iSER Logout -EAGAIN\n");
-			ret = -EAGAIN;
-		}
+		if (!ret)
+			isert_conn->logout_posted = true;
 		break;
 	case ISTATE_SEND_NOPIN:
 		ret = isert_put_nopin(cmd, conn, true);
@@ -2993,7 +2940,7 @@ isert_response_queue(struct iscsi_conn *conn, struct iscsi_cmd *cmd, int state)
 		ret = isert_put_response(conn, cmd);
 		break;
 	default:
-		pr_err("Unknown response state: 0x%02x\n", state);
+		isert_err("Unknown response state: 0x%02x\n", state);
 		ret = -EINVAL;
 		break;
 	}
@@ -3001,27 +2948,64 @@ isert_response_queue(struct iscsi_conn *conn, struct iscsi_cmd *cmd, int state)
 	return ret;
 }
 
+struct rdma_cm_id *
+isert_setup_id(struct isert_np *isert_np)
+{
+	struct iscsi_np *np = isert_np->np;
+	struct rdma_cm_id *id;
+	struct sockaddr *sa;
+	int ret;
+
+	sa = (struct sockaddr *)&np->np_sockaddr;
+	isert_dbg("ksockaddr: %p, sa: %p\n", &np->np_sockaddr, sa);
+
+	id = rdma_create_id(isert_cma_handler, isert_np,
+			    RDMA_PS_TCP, IB_QPT_RC);
+	if (IS_ERR(id)) {
+		isert_err("rdma_create_id() failed: %ld\n", PTR_ERR(id));
+		ret = PTR_ERR(id);
+		goto out;
+	}
+	isert_dbg("id %p context %p\n", id, id->context);
+
+	ret = rdma_bind_addr(id, sa);
+	if (ret) {
+		isert_err("rdma_bind_addr() failed: %d\n", ret);
+		goto out_id;
+	}
+
+	ret = rdma_listen(id, ISERT_RDMA_LISTEN_BACKLOG);
+	if (ret) {
+		isert_err("rdma_listen() failed: %d\n", ret);
+		goto out_id;
+	}
+
+	return id;
+out_id:
+	rdma_destroy_id(id);
+out:
+	return ERR_PTR(ret);
+}
+
 static int
 isert_setup_np(struct iscsi_np *np,
 	       struct __kernel_sockaddr_storage *ksockaddr)
 {
 	struct isert_np *isert_np;
 	struct rdma_cm_id *isert_lid;
-	struct sockaddr *sa;
 	int ret;
 
 	isert_np = kzalloc(sizeof(struct isert_np), GFP_KERNEL);
 	if (!isert_np) {
-		pr_err("Unable to allocate struct isert_np\n");
+		isert_err("Unable to allocate struct isert_np\n");
 		return -ENOMEM;
 	}
 	sema_init(&isert_np->np_sem, 0);
 	mutex_init(&isert_np->np_accept_mutex);
 	INIT_LIST_HEAD(&isert_np->np_accept_list);
 	init_completion(&isert_np->np_login_comp);
+	isert_np->np = np;
 
-	sa = (struct sockaddr *)ksockaddr;
-	pr_debug("ksockaddr: %p, sa: %p\n", ksockaddr, sa);
 	/*
 	 * Setup the np->np_sockaddr from the passed sockaddr setup
 	 * in iscsi_target_configfs.c code..
@@ -3029,37 +3013,20 @@ isert_setup_np(struct iscsi_np *np,
 	memcpy(&np->np_sockaddr, ksockaddr,
 	       sizeof(struct __kernel_sockaddr_storage));
 
-	isert_lid = rdma_create_id(isert_cma_handler, np, RDMA_PS_TCP,
-				IB_QPT_RC);
+	isert_lid = isert_setup_id(isert_np);
 	if (IS_ERR(isert_lid)) {
-		pr_err("rdma_create_id() for isert_listen_handler failed: %ld\n",
-		       PTR_ERR(isert_lid));
 		ret = PTR_ERR(isert_lid);
 		goto out;
 	}
 
-	ret = rdma_bind_addr(isert_lid, sa);
-	if (ret) {
-		pr_err("rdma_bind_addr() for isert_lid failed: %d\n", ret);
-		goto out_lid;
-	}
-
-	ret = rdma_listen(isert_lid, ISERT_RDMA_LISTEN_BACKLOG);
-	if (ret) {
-		pr_err("rdma_listen() for isert_lid failed: %d\n", ret);
-		goto out_lid;
-	}
-
 	isert_np->np_cm_id = isert_lid;
 	np->np_context = isert_np;
-	pr_debug("Setup isert_lid->context: %p\n", isert_lid->context);
 
 	return 0;
 
-out_lid:
-	rdma_destroy_id(isert_lid);
 out:
 	kfree(isert_np);
+
 	return ret;
 }
 
@@ -3075,16 +3042,12 @@ isert_rdma_accept(struct isert_conn *isert_conn)
 	cp.retry_count = 7;
 	cp.rnr_retry_count = 7;
 
-	pr_debug("Before rdma_accept >>>>>>>>>>>>>>>>>>>>.\n");
-
 	ret = rdma_accept(cm_id, &cp);
 	if (ret) {
-		pr_err("rdma_accept() failed with: %d\n", ret);
+		isert_err("rdma_accept() failed with: %d\n", ret);
 		return ret;
 	}
 
-	pr_debug("After rdma_accept >>>>>>>>>>>>>>>>>>>>>.\n");
-
 	return 0;
 }
 
@@ -3094,7 +3057,15 @@ isert_get_login_rx(struct iscsi_conn *conn, struct iscsi_login *login)
 	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
 	int ret;
 
-	pr_debug("isert_get_login_rx before conn_login_comp conn: %p\n", conn);
+	isert_info("before login_req comp conn: %p\n", isert_conn);
+	ret = wait_for_completion_interruptible(&isert_conn->login_req_comp);
+	if (ret) {
+		isert_err("isert_conn %p interrupted before got login req\n",
+			  isert_conn);
+		return ret;
+	}
+	reinit_completion(&isert_conn->login_req_comp);
+
 	/*
 	 * For login requests after the first PDU, isert_rx_login_req() will
 	 * kick schedule_delayed_work(&conn->login_work) as the packet is
@@ -3104,11 +3075,15 @@ isert_get_login_rx(struct iscsi_conn *conn, struct iscsi_login *login)
 	if (!login->first_request)
 		return 0;
 
+	isert_rx_login_req(isert_conn);
+
+	isert_info("before conn_login_comp conn: %p\n", conn);
 	ret = wait_for_completion_interruptible(&isert_conn->conn_login_comp);
 	if (ret)
 		return ret;
 
-	pr_debug("isert_get_login_rx processing login->req: %p\n", login->req);
+	isert_info("processing login->req: %p\n", login->req);
+
 	return 0;
 }
 
@@ -3161,7 +3136,7 @@ accept_wait:
 	spin_lock_bh(&np->np_thread_lock);
 	if (np->np_thread_state >= ISCSI_NP_THREAD_RESET) {
 		spin_unlock_bh(&np->np_thread_lock);
-		pr_debug("np_thread_state %d for isert_accept_np\n",
+		isert_dbg("np_thread_state %d for isert_accept_np\n",
 			 np->np_thread_state);
 		/**
 		 * No point in stalling here when np_thread
@@ -3186,17 +3161,10 @@ accept_wait:
 	isert_conn->conn = conn;
 	max_accept = 0;
 
-	ret = isert_rdma_post_recvl(isert_conn);
-	if (ret)
-		return ret;
-
-	ret = isert_rdma_accept(isert_conn);
-	if (ret)
-		return ret;
-
 	isert_set_conn_info(np, conn, isert_conn);
 
-	pr_debug("Processing isert_accept_np: isert_conn: %p\n", isert_conn);
+	isert_dbg("Processing isert_conn: %p\n", isert_conn);
+
 	return 0;
 }
 
@@ -3204,25 +3172,103 @@ static void
 isert_free_np(struct iscsi_np *np)
 {
 	struct isert_np *isert_np = (struct isert_np *)np->np_context;
+	struct isert_conn *isert_conn, *n;
 
 	if (isert_np->np_cm_id)
 		rdma_destroy_id(isert_np->np_cm_id);
 
+	/*
+	 * FIXME: At this point we don't have a good way to insure
+	 * that at this point we don't have hanging connections that
+	 * completed RDMA establishment but didn't start iscsi login
+	 * process. So work-around this by cleaning up what ever piled
+	 * up in np_accept_list.
+	 */
+	mutex_lock(&isert_np->np_accept_mutex);
+	if (!list_empty(&isert_np->np_accept_list)) {
+		isert_info("Still have isert connections, cleaning up...\n");
+		list_for_each_entry_safe(isert_conn, n,
+					 &isert_np->np_accept_list,
+					 conn_accept_node) {
+			isert_info("cleaning isert_conn %p state (%d)\n",
+				   isert_conn, isert_conn->state);
+			isert_connect_release(isert_conn);
+		}
+	}
+	mutex_unlock(&isert_np->np_accept_mutex);
+
 	np->np_context = NULL;
 	kfree(isert_np);
 }
 
+static void isert_release_work(struct work_struct *work)
+{
+	struct isert_conn *isert_conn = container_of(work,
+						     struct isert_conn,
+						     release_work);
+
+	isert_info("Starting release conn %p\n", isert_conn);
+
+	wait_for_completion(&isert_conn->conn_wait);
+
+	mutex_lock(&isert_conn->conn_mutex);
+	isert_conn->state = ISER_CONN_DOWN;
+	mutex_unlock(&isert_conn->conn_mutex);
+
+	isert_info("Destroying conn %p\n", isert_conn);
+	isert_put_conn(isert_conn);
+}
+
+static void
+isert_wait4logout(struct isert_conn *isert_conn)
+{
+	struct iscsi_conn *conn = isert_conn->conn;
+
+	isert_info("conn %p\n", isert_conn);
+
+	if (isert_conn->logout_posted) {
+		isert_info("conn %p wait for conn_logout_comp\n", isert_conn);
+		wait_for_completion_timeout(&conn->conn_logout_comp,
+					    SECONDS_FOR_LOGOUT_COMP * HZ);
+	}
+}
+
+static void
+isert_wait4cmds(struct iscsi_conn *conn)
+{
+	isert_info("iscsi_conn %p\n", conn);
+
+	if (conn->sess) {
+		target_sess_cmd_list_set_waiting(conn->sess->se_sess);
+		target_wait_for_sess_cmds(conn->sess->se_sess);
+	}
+}
+
+static void
+isert_wait4flush(struct isert_conn *isert_conn)
+{
+	struct ib_recv_wr *bad_wr;
+
+	isert_info("conn %p\n", isert_conn);
+
+	init_completion(&isert_conn->conn_wait_comp_err);
+	isert_conn->beacon.wr_id = ISER_BEACON_WRID;
+	/* post an indication that all flush errors were consumed */
+	if (ib_post_recv(isert_conn->conn_qp, &isert_conn->beacon, &bad_wr)) {
+		isert_err("conn %p failed to post beacon", isert_conn);
+		return;
+	}
+
+	wait_for_completion(&isert_conn->conn_wait_comp_err);
+}
+
 static void isert_wait_conn(struct iscsi_conn *conn)
 {
 	struct isert_conn *isert_conn = conn->context;
 
-	pr_debug("isert_wait_conn: Starting \n");
+	isert_info("Starting conn %p\n", isert_conn);
 
 	mutex_lock(&isert_conn->conn_mutex);
-	if (isert_conn->conn_cm_id && !isert_conn->disconnect) {
-		pr_debug("Calling rdma_disconnect from isert_wait_conn\n");
-		rdma_disconnect(isert_conn->conn_cm_id);
-	}
 	/*
 	 * Only wait for conn_wait_comp_err if the isert_conn made it
 	 * into full feature phase..
@@ -3231,14 +3277,15 @@ static void isert_wait_conn(struct iscsi_conn *conn)
 		mutex_unlock(&isert_conn->conn_mutex);
 		return;
 	}
-	if (isert_conn->state == ISER_CONN_UP)
-		isert_conn->state = ISER_CONN_TERMINATING;
+	isert_conn_terminate(isert_conn);
 	mutex_unlock(&isert_conn->conn_mutex);
 
-	wait_for_completion(&isert_conn->conn_wait_comp_err);
+	isert_wait4cmds(conn);
+	isert_wait4flush(isert_conn);
+	isert_wait4logout(isert_conn);
 
-	wait_for_completion(&isert_conn->conn_wait);
-	isert_put_conn(isert_conn);
+	INIT_WORK(&isert_conn->release_work, isert_release_work);
+	queue_work(isert_release_wq, &isert_conn->release_work);
 }
 
 static void isert_free_conn(struct iscsi_conn *conn)
@@ -3273,35 +3320,39 @@ static int __init isert_init(void)
 {
 	int ret;
 
-	isert_rx_wq = alloc_workqueue("isert_rx_wq", 0, 0);
-	if (!isert_rx_wq) {
-		pr_err("Unable to allocate isert_rx_wq\n");
+	isert_comp_wq = alloc_workqueue("isert_comp_wq", 0, 0);
+	if (!isert_comp_wq) {
+		isert_err("Unable to allocate isert_comp_wq\n");
+		ret = -ENOMEM;
 		return -ENOMEM;
 	}
 
-	isert_comp_wq = alloc_workqueue("isert_comp_wq", 0, 0);
-	if (!isert_comp_wq) {
-		pr_err("Unable to allocate isert_comp_wq\n");
+	isert_release_wq = alloc_workqueue("isert_release_wq", WQ_UNBOUND,
+					WQ_UNBOUND_MAX_ACTIVE);
+	if (!isert_release_wq) {
+		isert_err("Unable to allocate isert_release_wq\n");
 		ret = -ENOMEM;
-		goto destroy_rx_wq;
+		goto destroy_comp_wq;
 	}
 
 	iscsit_register_transport(&iser_target_transport);
-	pr_debug("iSER_TARGET[0] - Loaded iser_target_transport\n");
+	isert_info("iSER_TARGET[0] - Loaded iser_target_transport\n");
+
 	return 0;
 
-destroy_rx_wq:
-	destroy_workqueue(isert_rx_wq);
+destroy_comp_wq:
+	destroy_workqueue(isert_comp_wq);
+
 	return ret;
 }
 
 static void __exit isert_exit(void)
 {
 	flush_scheduled_work();
+	destroy_workqueue(isert_release_wq);
 	destroy_workqueue(isert_comp_wq);
-	destroy_workqueue(isert_rx_wq);
 	iscsit_unregister_transport(&iser_target_transport);
-	pr_debug("iSER_TARGET[0] - Released iser_target_transport\n");
+	isert_info("iSER_TARGET[0] - Released iser_target_transport\n");
 }
 
 MODULE_DESCRIPTION("iSER-Target for mainline target infrastructure");
diff --git a/drivers/infiniband/ulp/isert/ib_isert.h b/drivers/infiniband/ulp/isert/ib_isert.h
index 04f51f7bf614..8dc8415d152d 100644
--- a/drivers/infiniband/ulp/isert/ib_isert.h
+++ b/drivers/infiniband/ulp/isert/ib_isert.h
@@ -4,9 +4,37 @@
 #include <rdma/ib_verbs.h>
 #include <rdma/rdma_cm.h>
 
+#define DRV_NAME	"isert"
+#define PFX		DRV_NAME ": "
+
+#define isert_dbg(fmt, arg...)				 \
+	do {						 \
+		if (unlikely(isert_debug_level > 2))	 \
+			printk(KERN_DEBUG PFX "%s: " fmt,\
+				__func__ , ## arg);	 \
+	} while (0)
+
+#define isert_warn(fmt, arg...)				\
+	do {						\
+		if (unlikely(isert_debug_level > 0))	\
+			pr_warn(PFX "%s: " fmt,         \
+				__func__ , ## arg);	\
+	} while (0)
+
+#define isert_info(fmt, arg...)				\
+	do {						\
+		if (unlikely(isert_debug_level > 1))	\
+			pr_info(PFX "%s: " fmt,         \
+				__func__ , ## arg);	\
+	} while (0)
+
+#define isert_err(fmt, arg...) \
+	pr_err(PFX "%s: " fmt, __func__ , ## arg)
+
 #define ISERT_RDMA_LISTEN_BACKLOG	10
 #define ISCSI_ISER_SG_TABLESIZE		256
 #define ISER_FASTREG_LI_WRID		0xffffffffffffffffULL
+#define ISER_BEACON_WRID               0xfffffffffffffffeULL
 
 enum isert_desc_type {
 	ISCSI_TX_CONTROL,
@@ -23,6 +51,7 @@ enum iser_ib_op_code {
 enum iser_conn_state {
 	ISER_CONN_INIT,
 	ISER_CONN_UP,
+	ISER_CONN_FULL_FEATURE,
 	ISER_CONN_TERMINATING,
 	ISER_CONN_DOWN,
 };
@@ -44,9 +73,6 @@ struct iser_tx_desc {
 	struct ib_sge	tx_sg[2];
 	int		num_sge;
 	struct isert_cmd *isert_cmd;
-	struct llist_node *comp_llnode_batch;
-	struct llist_node comp_llnode;
-	bool		llnode_active;
 	struct ib_send_wr send_wr;
 } __packed;
 
@@ -81,6 +107,12 @@ struct isert_data_buf {
 	enum dma_data_direction dma_dir;
 };
 
+enum {
+	DATA = 0,
+	PROT = 1,
+	SIG = 2,
+};
+
 struct isert_rdma_wr {
 	struct list_head	wr_list;
 	struct isert_cmd	*isert_cmd;
@@ -90,6 +122,7 @@ struct isert_rdma_wr {
 	int			send_wr_num;
 	struct ib_send_wr	*send_wr;
 	struct ib_send_wr	s_send_wr;
+	struct ib_sge		ib_sg[3];
 	struct isert_data_buf	data;
 	struct isert_data_buf	prot;
 	struct fast_reg_descriptor *fr_desc;
@@ -117,14 +150,15 @@ struct isert_device;
 struct isert_conn {
 	enum iser_conn_state	state;
 	int			post_recv_buf_count;
-	atomic_t		post_send_buf_count;
 	u32			responder_resources;
 	u32			initiator_depth;
+	bool			pi_support;
 	u32			max_sge;
 	char			*login_buf;
 	char			*login_req_buf;
 	char			*login_rsp_buf;
 	u64			login_req_dma;
+	int			login_req_len;
 	u64			login_rsp_dma;
 	unsigned int		conn_rx_desc_head;
 	struct iser_rx_desc	*conn_rx_descs;
@@ -132,13 +166,13 @@ struct isert_conn {
 	struct iscsi_conn	*conn;
 	struct list_head	conn_accept_node;
 	struct completion	conn_login_comp;
+	struct completion	login_req_comp;
 	struct iser_tx_desc	conn_login_tx_desc;
 	struct rdma_cm_id	*conn_cm_id;
 	struct ib_pd		*conn_pd;
 	struct ib_mr		*conn_mr;
 	struct ib_qp		*conn_qp;
 	struct isert_device	*conn_device;
-	struct work_struct	conn_logout_work;
 	struct mutex		conn_mutex;
 	struct completion	conn_wait;
 	struct completion	conn_wait_comp_err;
@@ -147,31 +181,38 @@ struct isert_conn {
 	int			conn_fr_pool_size;
 	/* lock to protect fastreg pool */
 	spinlock_t		conn_lock;
-#define ISERT_COMP_BATCH_COUNT	8
-	int			conn_comp_batch;
-	struct llist_head	conn_comp_llist;
-	bool                    disconnect;
+	struct work_struct	release_work;
+	struct ib_recv_wr       beacon;
+	bool                    logout_posted;
 };
 
 #define ISERT_MAX_CQ 64
 
-struct isert_cq_desc {
-	struct isert_device	*device;
-	int			cq_index;
-	struct work_struct	cq_rx_work;
-	struct work_struct	cq_tx_work;
+/**
+ * struct isert_comp - iSER completion context
+ *
+ * @device:     pointer to device handle
+ * @cq:         completion queue
+ * @wcs:        work completion array
+ * @active_qps: Number of active QPs attached
+ *              to completion context
+ * @work:       completion work handle
+ */
+struct isert_comp {
+	struct isert_device     *device;
+	struct ib_cq		*cq;
+	struct ib_wc		 wcs[16];
+	int                      active_qps;
+	struct work_struct	 work;
 };
 
 struct isert_device {
 	int			use_fastreg;
 	bool			pi_capable;
-	int			cqs_used;
 	int			refcount;
-	int			cq_active_qps[ISERT_MAX_CQ];
 	struct ib_device	*ib_device;
-	struct ib_cq		*dev_rx_cq[ISERT_MAX_CQ];
-	struct ib_cq		*dev_tx_cq[ISERT_MAX_CQ];
-	struct isert_cq_desc	*cq_desc;
+	struct isert_comp	*comps;
+	int                     comps_used;
 	struct list_head	dev_node;
 	struct ib_device_attr	dev_attr;
 	int			(*reg_rdma_mem)(struct iscsi_conn *conn,
@@ -182,6 +223,7 @@ struct isert_device {
 };
 
 struct isert_np {
+	struct iscsi_np         *np;
 	struct semaphore	np_sem;
 	struct rdma_cm_id	*np_cm_id;
 	struct mutex		np_accept_mutex;
diff --git a/drivers/infiniband/ulp/srp/ib_srp.c b/drivers/infiniband/ulp/srp/ib_srp.c
index 5461924c9f10..0747c0595a9d 100644
--- a/drivers/infiniband/ulp/srp/ib_srp.c
+++ b/drivers/infiniband/ulp/srp/ib_srp.c
@@ -2740,7 +2740,6 @@ static struct scsi_host_template srp_template = {
 	.info				= srp_target_info,
 	.queuecommand			= srp_queuecommand,
 	.change_queue_depth             = srp_change_queue_depth,
-	.change_queue_type              = scsi_change_queue_type,
 	.eh_abort_handler		= srp_abort,
 	.eh_device_reset_handler	= srp_reset_device,
 	.eh_host_reset_handler		= srp_reset_host,
@@ -2929,7 +2928,7 @@ static int srp_parse_options(const char *buf, struct srp_target_port *target)
 		return -ENOMEM;
 
 	sep_opt = options;
-	while ((p = strsep(&sep_opt, ",")) != NULL) {
+	while ((p = strsep(&sep_opt, ",\n")) != NULL) {
 		if (!*p)
 			continue;
 
diff --git a/drivers/infiniband/ulp/srpt/ib_srpt.c b/drivers/infiniband/ulp/srpt/ib_srpt.c
index dc829682701a..eb694ddad79f 100644
--- a/drivers/infiniband/ulp/srpt/ib_srpt.c
+++ b/drivers/infiniband/ulp/srpt/ib_srpt.c
@@ -1708,17 +1708,17 @@ static int srpt_handle_cmd(struct srpt_rdma_ch *ch,
 
 	switch (srp_cmd->task_attr) {
 	case SRP_CMD_SIMPLE_Q:
-		cmd->sam_task_attr = MSG_SIMPLE_TAG;
+		cmd->sam_task_attr = TCM_SIMPLE_TAG;
 		break;
 	case SRP_CMD_ORDERED_Q:
 	default:
-		cmd->sam_task_attr = MSG_ORDERED_TAG;
+		cmd->sam_task_attr = TCM_ORDERED_TAG;
 		break;
 	case SRP_CMD_HEAD_OF_Q:
-		cmd->sam_task_attr = MSG_HEAD_TAG;
+		cmd->sam_task_attr = TCM_HEAD_TAG;
 		break;
 	case SRP_CMD_ACA:
-		cmd->sam_task_attr = MSG_ACA_TAG;
+		cmd->sam_task_attr = TCM_ACA_TAG;
 		break;
 	}
 
@@ -1733,7 +1733,7 @@ static int srpt_handle_cmd(struct srpt_rdma_ch *ch,
 				       sizeof(srp_cmd->lun));
 	rc = target_submit_cmd(cmd, ch->sess, srp_cmd->cdb,
 			&send_ioctx->sense_data[0], unpacked_lun, data_len,
-			MSG_SIMPLE_TAG, dir, TARGET_SCF_ACK_KREF);
+			TCM_SIMPLE_TAG, dir, TARGET_SCF_ACK_KREF);
 	if (rc != 0) {
 		ret = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 		goto send_sense;
diff --git a/drivers/input/gameport/gameport.c b/drivers/input/gameport/gameport.c
index e29c04e2aff4..e853a2134680 100644
--- a/drivers/input/gameport/gameport.c
+++ b/drivers/input/gameport/gameport.c
@@ -527,14 +527,14 @@ EXPORT_SYMBOL(gameport_set_phys);
  */
 static void gameport_init_port(struct gameport *gameport)
 {
-	static atomic_t gameport_no = ATOMIC_INIT(0);
+	static atomic_t gameport_no = ATOMIC_INIT(-1);
 
 	__module_get(THIS_MODULE);
 
 	mutex_init(&gameport->drv_mutex);
 	device_initialize(&gameport->dev);
 	dev_set_name(&gameport->dev, "gameport%lu",
-			(unsigned long)atomic_inc_return(&gameport_no) - 1);
+			(unsigned long)atomic_inc_return(&gameport_no));
 	gameport->dev.bus = &gameport_bus;
 	gameport->dev.release = gameport_release_port;
 	if (gameport->parent)
diff --git a/drivers/input/input.c b/drivers/input/input.c
index 0f175f55782b..04217c2e345c 100644
--- a/drivers/input/input.c
+++ b/drivers/input/input.c
@@ -1775,7 +1775,7 @@ EXPORT_SYMBOL_GPL(input_class);
  */
 struct input_dev *input_allocate_device(void)
 {
-	static atomic_t input_no = ATOMIC_INIT(0);
+	static atomic_t input_no = ATOMIC_INIT(-1);
 	struct input_dev *dev;
 
 	dev = kzalloc(sizeof(struct input_dev), GFP_KERNEL);
@@ -1790,7 +1790,7 @@ struct input_dev *input_allocate_device(void)
 		INIT_LIST_HEAD(&dev->node);
 
 		dev_set_name(&dev->dev, "input%lu",
-			     (unsigned long) atomic_inc_return(&input_no) - 1);
+			     (unsigned long)atomic_inc_return(&input_no));
 
 		__module_get(THIS_MODULE);
 	}
diff --git a/drivers/input/joystick/xpad.c b/drivers/input/joystick/xpad.c
index fc55f0d15b70..3aa2f3f3da5b 100644
--- a/drivers/input/joystick/xpad.c
+++ b/drivers/input/joystick/xpad.c
@@ -886,8 +886,8 @@ static void xpad_led_set(struct led_classdev *led_cdev,
 
 static int xpad_led_probe(struct usb_xpad *xpad)
 {
-	static atomic_t led_seq	= ATOMIC_INIT(0);
-	long led_no;
+	static atomic_t led_seq	= ATOMIC_INIT(-1);
+	unsigned long led_no;
 	struct xpad_led *led;
 	struct led_classdev *led_cdev;
 	int error;
@@ -899,9 +899,9 @@ static int xpad_led_probe(struct usb_xpad *xpad)
 	if (!led)
 		return -ENOMEM;
 
-	led_no = (long)atomic_inc_return(&led_seq) - 1;
+	led_no = atomic_inc_return(&led_seq);
 
-	snprintf(led->name, sizeof(led->name), "xpad%ld", led_no);
+	snprintf(led->name, sizeof(led->name), "xpad%lu", led_no);
 	led->xpad = xpad;
 
 	led_cdev = &led->led_cdev;
diff --git a/drivers/input/keyboard/Kconfig b/drivers/input/keyboard/Kconfig
index a3958c63d7d5..96ee26c555e0 100644
--- a/drivers/input/keyboard/Kconfig
+++ b/drivers/input/keyboard/Kconfig
@@ -665,14 +665,14 @@ config KEYBOARD_CROS_EC
 	  To compile this driver as a module, choose M here: the
 	  module will be called cros_ec_keyb.
 
-config KEYBOARD_CAP1106
-	tristate "Microchip CAP1106 touch sensor"
+config KEYBOARD_CAP11XX
+	tristate "Microchip CAP11XX based touch sensors"
 	depends on OF && I2C
 	select REGMAP_I2C
 	help
-	  Say Y here to enable the CAP1106 touch sensor driver.
+	  Say Y here to enable the CAP11XX touch sensor driver.
 
 	  To compile this driver as a module, choose M here: the
-	  module will be called cap1106.
+	  module will be called cap11xx.
 
 endif
diff --git a/drivers/input/keyboard/Makefile b/drivers/input/keyboard/Makefile
index 0a3345634d79..febafa527eb6 100644
--- a/drivers/input/keyboard/Makefile
+++ b/drivers/input/keyboard/Makefile
@@ -11,7 +11,7 @@ obj-$(CONFIG_KEYBOARD_AMIGA)		+= amikbd.o
 obj-$(CONFIG_KEYBOARD_ATARI)		+= atakbd.o
 obj-$(CONFIG_KEYBOARD_ATKBD)		+= atkbd.o
 obj-$(CONFIG_KEYBOARD_BFIN)		+= bf54x-keys.o
-obj-$(CONFIG_KEYBOARD_CAP1106)		+= cap1106.o
+obj-$(CONFIG_KEYBOARD_CAP11XX)		+= cap11xx.o
 obj-$(CONFIG_KEYBOARD_CLPS711X)		+= clps711x-keypad.o
 obj-$(CONFIG_KEYBOARD_CROS_EC)		+= cros_ec_keyb.o
 obj-$(CONFIG_KEYBOARD_DAVINCI)		+= davinci_keyscan.o
diff --git a/drivers/input/keyboard/adp5520-keys.c b/drivers/input/keyboard/adp5520-keys.c
index 7f4a8b58efc1..db1004dad108 100644
--- a/drivers/input/keyboard/adp5520-keys.c
+++ b/drivers/input/keyboard/adp5520-keys.c
@@ -184,7 +184,6 @@ static int adp5520_keys_remove(struct platform_device *pdev)
 static struct platform_driver adp5520_keys_driver = {
 	.driver	= {
 		.name	= "adp5520-keys",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= adp5520_keys_probe,
 	.remove		= adp5520_keys_remove,
diff --git a/drivers/input/keyboard/amikbd.c b/drivers/input/keyboard/amikbd.c
index 096d6067ae1f..e04a3b4e55d6 100644
--- a/drivers/input/keyboard/amikbd.c
+++ b/drivers/input/keyboard/amikbd.c
@@ -45,6 +45,7 @@ MODULE_AUTHOR("Vojtech Pavlik <vojtech@ucw.cz>");
 MODULE_DESCRIPTION("Amiga keyboard driver");
 MODULE_LICENSE("GPL");
 
+#ifdef CONFIG_HW_CONSOLE
 static unsigned char amikbd_keycode[0x78] __initdata = {
 	[0]	 = KEY_GRAVE,
 	[1]	 = KEY_1,
@@ -144,6 +145,32 @@ static unsigned char amikbd_keycode[0x78] __initdata = {
 	[103]	 = KEY_RIGHTMETA
 };
 
+static void __init amikbd_init_console_keymaps(void)
+{
+	/* We can spare 512 bytes on stack for temp_map in init path. */
+	unsigned short temp_map[NR_KEYS];
+	int i, j;
+
+	for (i = 0; i < MAX_NR_KEYMAPS; i++) {
+		if (!key_maps[i])
+			continue;
+		memset(temp_map, 0, sizeof(temp_map));
+		for (j = 0; j < 0x78; j++) {
+			if (!amikbd_keycode[j])
+				continue;
+			temp_map[j] = key_maps[i][amikbd_keycode[j]];
+		}
+		for (j = 0; j < NR_KEYS; j++) {
+			if (!temp_map[j])
+				temp_map[j] = 0xf200;
+		}
+		memcpy(key_maps[i], temp_map, sizeof(temp_map));
+	}
+}
+#else /* !CONFIG_HW_CONSOLE */
+static inline void amikbd_init_console_keymaps(void) {}
+#endif /* !CONFIG_HW_CONSOLE */
+
 static const char *amikbd_messages[8] = {
 	[0] = KERN_ALERT "amikbd: Ctrl-Amiga-Amiga reset warning!!\n",
 	[1] = KERN_WARNING "amikbd: keyboard lost sync\n",
@@ -186,7 +213,7 @@ static irqreturn_t amikbd_interrupt(int irq, void *data)
 static int __init amikbd_probe(struct platform_device *pdev)
 {
 	struct input_dev *dev;
-	int i, j, err;
+	int i, err;
 
 	dev = input_allocate_device();
 	if (!dev) {
@@ -207,22 +234,8 @@ static int __init amikbd_probe(struct platform_device *pdev)
 	for (i = 0; i < 0x78; i++)
 		set_bit(i, dev->keybit);
 
-	for (i = 0; i < MAX_NR_KEYMAPS; i++) {
-		static u_short temp_map[NR_KEYS] __initdata;
-		if (!key_maps[i])
-			continue;
-		memset(temp_map, 0, sizeof(temp_map));
-		for (j = 0; j < 0x78; j++) {
-			if (!amikbd_keycode[j])
-				continue;
-			temp_map[j] = key_maps[i][amikbd_keycode[j]];
-		}
-		for (j = 0; j < NR_KEYS; j++) {
-			if (!temp_map[j])
-				temp_map[j] = 0xf200;
-		}
-		memcpy(key_maps[i], temp_map, sizeof(temp_map));
-	}
+	amikbd_init_console_keymaps();
+
 	ciaa.cra &= ~0x41;	 /* serial data in, turn off TA */
 	err = request_irq(IRQ_AMIGA_CIAA_SP, amikbd_interrupt, 0, "amikbd",
 			  dev);
@@ -255,7 +268,6 @@ static struct platform_driver amikbd_driver = {
 	.remove = __exit_p(amikbd_remove),
 	.driver   = {
 		.name	= "amiga-keyboard",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/input/keyboard/atkbd.c b/drivers/input/keyboard/atkbd.c
index 6f5d79569136..e27a25892db4 100644
--- a/drivers/input/keyboard/atkbd.c
+++ b/drivers/input/keyboard/atkbd.c
@@ -456,8 +456,9 @@ static irqreturn_t atkbd_interrupt(struct serio *serio, unsigned char data,
 
 	keycode = atkbd->keycode[code];
 
-	if (keycode != ATKBD_KEY_NULL)
-		input_event(dev, EV_MSC, MSC_SCAN, code);
+	if (!(atkbd->release && test_bit(code, atkbd->force_release_mask)))
+		if (keycode != ATKBD_KEY_NULL)
+			input_event(dev, EV_MSC, MSC_SCAN, code);
 
 	switch (keycode) {
 	case ATKBD_KEY_NULL:
@@ -511,6 +512,7 @@ static irqreturn_t atkbd_interrupt(struct serio *serio, unsigned char data,
 		input_sync(dev);
 
 		if (value && test_bit(code, atkbd->force_release_mask)) {
+			input_event(dev, EV_MSC, MSC_SCAN, code);
 			input_report_key(dev, keycode, 0);
 			input_sync(dev);
 		}
diff --git a/drivers/input/keyboard/bf54x-keys.c b/drivers/input/keyboard/bf54x-keys.c
index e6d46c5994d7..81b07dddae86 100644
--- a/drivers/input/keyboard/bf54x-keys.c
+++ b/drivers/input/keyboard/bf54x-keys.c
@@ -385,7 +385,6 @@ static int bfin_kpad_resume(struct platform_device *pdev)
 static struct platform_driver bfin_kpad_device_driver = {
 	.driver		= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= bfin_kpad_probe,
 	.remove		= bfin_kpad_remove,
diff --git a/drivers/input/keyboard/cap1106.c b/drivers/input/keyboard/cap1106.c
deleted file mode 100644
index d70b65a14ced..000000000000
--- a/drivers/input/keyboard/cap1106.c
+++ /dev/null
@@ -1,341 +0,0 @@
-/*
- * Input driver for Microchip CAP1106, 6 channel capacitive touch sensor
- *
- * http://www.microchip.com/wwwproducts/Devices.aspx?product=CAP1106
- *
- * (c) 2014 Daniel Mack <linux@zonque.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.
- */
-
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/interrupt.h>
-#include <linux/input.h>
-#include <linux/of_irq.h>
-#include <linux/regmap.h>
-#include <linux/i2c.h>
-#include <linux/gpio/consumer.h>
-
-#define CAP1106_REG_MAIN_CONTROL	0x00
-#define CAP1106_REG_MAIN_CONTROL_GAIN_SHIFT	(6)
-#define CAP1106_REG_MAIN_CONTROL_GAIN_MASK	(0xc0)
-#define CAP1106_REG_MAIN_CONTROL_DLSEEP		BIT(4)
-#define CAP1106_REG_GENERAL_STATUS	0x02
-#define CAP1106_REG_SENSOR_INPUT	0x03
-#define CAP1106_REG_NOISE_FLAG_STATUS	0x0a
-#define CAP1106_REG_SENOR_DELTA(X)	(0x10 + (X))
-#define CAP1106_REG_SENSITIVITY_CONTROL	0x1f
-#define CAP1106_REG_CONFIG		0x20
-#define CAP1106_REG_SENSOR_ENABLE	0x21
-#define CAP1106_REG_SENSOR_CONFIG	0x22
-#define CAP1106_REG_SENSOR_CONFIG2	0x23
-#define CAP1106_REG_SAMPLING_CONFIG	0x24
-#define CAP1106_REG_CALIBRATION		0x26
-#define CAP1106_REG_INT_ENABLE		0x27
-#define CAP1106_REG_REPEAT_RATE		0x28
-#define CAP1106_REG_MT_CONFIG		0x2a
-#define CAP1106_REG_MT_PATTERN_CONFIG	0x2b
-#define CAP1106_REG_MT_PATTERN		0x2d
-#define CAP1106_REG_RECALIB_CONFIG	0x2f
-#define CAP1106_REG_SENSOR_THRESH(X)	(0x30 + (X))
-#define CAP1106_REG_SENSOR_NOISE_THRESH	0x38
-#define CAP1106_REG_STANDBY_CHANNEL	0x40
-#define CAP1106_REG_STANDBY_CONFIG	0x41
-#define CAP1106_REG_STANDBY_SENSITIVITY	0x42
-#define CAP1106_REG_STANDBY_THRESH	0x43
-#define CAP1106_REG_CONFIG2		0x44
-#define CAP1106_REG_SENSOR_BASE_CNT(X)	(0x50 + (X))
-#define CAP1106_REG_SENSOR_CALIB	(0xb1 + (X))
-#define CAP1106_REG_SENSOR_CALIB_LSB1	0xb9
-#define CAP1106_REG_SENSOR_CALIB_LSB2	0xba
-#define CAP1106_REG_PRODUCT_ID		0xfd
-#define CAP1106_REG_MANUFACTURER_ID	0xfe
-#define CAP1106_REG_REVISION		0xff
-
-#define CAP1106_NUM_CHN 6
-#define CAP1106_PRODUCT_ID	0x55
-#define CAP1106_MANUFACTURER_ID	0x5d
-
-struct cap1106_priv {
-	struct regmap *regmap;
-	struct input_dev *idev;
-
-	/* config */
-	unsigned short keycodes[CAP1106_NUM_CHN];
-};
-
-static const struct reg_default cap1106_reg_defaults[] = {
-	{ CAP1106_REG_MAIN_CONTROL,		0x00 },
-	{ CAP1106_REG_GENERAL_STATUS,		0x00 },
-	{ CAP1106_REG_SENSOR_INPUT,		0x00 },
-	{ CAP1106_REG_NOISE_FLAG_STATUS,	0x00 },
-	{ CAP1106_REG_SENSITIVITY_CONTROL,	0x2f },
-	{ CAP1106_REG_CONFIG,			0x20 },
-	{ CAP1106_REG_SENSOR_ENABLE,		0x3f },
-	{ CAP1106_REG_SENSOR_CONFIG,		0xa4 },
-	{ CAP1106_REG_SENSOR_CONFIG2,		0x07 },
-	{ CAP1106_REG_SAMPLING_CONFIG,		0x39 },
-	{ CAP1106_REG_CALIBRATION,		0x00 },
-	{ CAP1106_REG_INT_ENABLE,		0x3f },
-	{ CAP1106_REG_REPEAT_RATE,		0x3f },
-	{ CAP1106_REG_MT_CONFIG,		0x80 },
-	{ CAP1106_REG_MT_PATTERN_CONFIG,	0x00 },
-	{ CAP1106_REG_MT_PATTERN,		0x3f },
-	{ CAP1106_REG_RECALIB_CONFIG,		0x8a },
-	{ CAP1106_REG_SENSOR_THRESH(0),		0x40 },
-	{ CAP1106_REG_SENSOR_THRESH(1),		0x40 },
-	{ CAP1106_REG_SENSOR_THRESH(2),		0x40 },
-	{ CAP1106_REG_SENSOR_THRESH(3),		0x40 },
-	{ CAP1106_REG_SENSOR_THRESH(4),		0x40 },
-	{ CAP1106_REG_SENSOR_THRESH(5),		0x40 },
-	{ CAP1106_REG_SENSOR_NOISE_THRESH,	0x01 },
-	{ CAP1106_REG_STANDBY_CHANNEL,		0x00 },
-	{ CAP1106_REG_STANDBY_CONFIG,		0x39 },
-	{ CAP1106_REG_STANDBY_SENSITIVITY,	0x02 },
-	{ CAP1106_REG_STANDBY_THRESH,		0x40 },
-	{ CAP1106_REG_CONFIG2,			0x40 },
-	{ CAP1106_REG_SENSOR_CALIB_LSB1,	0x00 },
-	{ CAP1106_REG_SENSOR_CALIB_LSB2,	0x00 },
-};
-
-static bool cap1106_volatile_reg(struct device *dev, unsigned int reg)
-{
-	switch (reg) {
-	case CAP1106_REG_MAIN_CONTROL:
-	case CAP1106_REG_SENSOR_INPUT:
-	case CAP1106_REG_SENOR_DELTA(0):
-	case CAP1106_REG_SENOR_DELTA(1):
-	case CAP1106_REG_SENOR_DELTA(2):
-	case CAP1106_REG_SENOR_DELTA(3):
-	case CAP1106_REG_SENOR_DELTA(4):
-	case CAP1106_REG_SENOR_DELTA(5):
-	case CAP1106_REG_PRODUCT_ID:
-	case CAP1106_REG_MANUFACTURER_ID:
-	case CAP1106_REG_REVISION:
-		return true;
-	}
-
-	return false;
-}
-
-static const struct regmap_config cap1106_regmap_config = {
-	.reg_bits = 8,
-	.val_bits = 8,
-
-	.max_register = CAP1106_REG_REVISION,
-	.reg_defaults = cap1106_reg_defaults,
-
-	.num_reg_defaults = ARRAY_SIZE(cap1106_reg_defaults),
-	.cache_type = REGCACHE_RBTREE,
-	.volatile_reg = cap1106_volatile_reg,
-};
-
-static irqreturn_t cap1106_thread_func(int irq_num, void *data)
-{
-	struct cap1106_priv *priv = data;
-	unsigned int status;
-	int ret, i;
-
-	/*
-	 * Deassert interrupt. This needs to be done before reading the status
-	 * registers, which will not carry valid values otherwise.
-	 */
-	ret = regmap_update_bits(priv->regmap, CAP1106_REG_MAIN_CONTROL, 1, 0);
-	if (ret < 0)
-		goto out;
-
-	ret = regmap_read(priv->regmap, CAP1106_REG_SENSOR_INPUT, &status);
-	if (ret < 0)
-		goto out;
-
-	for (i = 0; i < CAP1106_NUM_CHN; i++)
-		input_report_key(priv->idev, priv->keycodes[i],
-				 status & (1 << i));
-
-	input_sync(priv->idev);
-
-out:
-	return IRQ_HANDLED;
-}
-
-static int cap1106_set_sleep(struct cap1106_priv *priv, bool sleep)
-{
-	return regmap_update_bits(priv->regmap, CAP1106_REG_MAIN_CONTROL,
-				  CAP1106_REG_MAIN_CONTROL_DLSEEP,
-				  sleep ? CAP1106_REG_MAIN_CONTROL_DLSEEP : 0);
-}
-
-static int cap1106_input_open(struct input_dev *idev)
-{
-	struct cap1106_priv *priv = input_get_drvdata(idev);
-
-	return cap1106_set_sleep(priv, false);
-}
-
-static void cap1106_input_close(struct input_dev *idev)
-{
-	struct cap1106_priv *priv = input_get_drvdata(idev);
-
-	cap1106_set_sleep(priv, true);
-}
-
-static int cap1106_i2c_probe(struct i2c_client *i2c_client,
-			     const struct i2c_device_id *id)
-{
-	struct device *dev = &i2c_client->dev;
-	struct cap1106_priv *priv;
-	struct device_node *node;
-	int i, error, irq, gain = 0;
-	unsigned int val, rev;
-	u32 gain32, keycodes[CAP1106_NUM_CHN];
-
-	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
-	if (!priv)
-		return -ENOMEM;
-
-	priv->regmap = devm_regmap_init_i2c(i2c_client, &cap1106_regmap_config);
-	if (IS_ERR(priv->regmap))
-		return PTR_ERR(priv->regmap);
-
-	error = regmap_read(priv->regmap, CAP1106_REG_PRODUCT_ID, &val);
-	if (error)
-		return error;
-
-	if (val != CAP1106_PRODUCT_ID) {
-		dev_err(dev, "Product ID: Got 0x%02x, expected 0x%02x\n",
-			val, CAP1106_PRODUCT_ID);
-		return -ENODEV;
-	}
-
-	error = regmap_read(priv->regmap, CAP1106_REG_MANUFACTURER_ID, &val);
-	if (error)
-		return error;
-
-	if (val != CAP1106_MANUFACTURER_ID) {
-		dev_err(dev, "Manufacturer ID: Got 0x%02x, expected 0x%02x\n",
-			val, CAP1106_MANUFACTURER_ID);
-		return -ENODEV;
-	}
-
-	error = regmap_read(priv->regmap, CAP1106_REG_REVISION, &rev);
-	if (error < 0)
-		return error;
-
-	dev_info(dev, "CAP1106 detected, revision 0x%02x\n", rev);
-	i2c_set_clientdata(i2c_client, priv);
-	node = dev->of_node;
-
-	if (!of_property_read_u32(node, "microchip,sensor-gain", &gain32)) {
-		if (is_power_of_2(gain32) && gain32 <= 8)
-			gain = ilog2(gain32);
-		else
-			dev_err(dev, "Invalid sensor-gain value %d\n", gain32);
-	}
-
-	BUILD_BUG_ON(ARRAY_SIZE(keycodes) != ARRAY_SIZE(priv->keycodes));
-
-	/* Provide some useful defaults */
-	for (i = 0; i < ARRAY_SIZE(keycodes); i++)
-		keycodes[i] = KEY_A + i;
-
-	of_property_read_u32_array(node, "linux,keycodes",
-				   keycodes, ARRAY_SIZE(keycodes));
-
-	for (i = 0; i < ARRAY_SIZE(keycodes); i++)
-		priv->keycodes[i] = keycodes[i];
-
-	error = regmap_update_bits(priv->regmap, CAP1106_REG_MAIN_CONTROL,
-				   CAP1106_REG_MAIN_CONTROL_GAIN_MASK,
-				   gain << CAP1106_REG_MAIN_CONTROL_GAIN_SHIFT);
-	if (error)
-		return error;
-
-	/* Disable autorepeat. The Linux input system has its own handling. */
-	error = regmap_write(priv->regmap, CAP1106_REG_REPEAT_RATE, 0);
-	if (error)
-		return error;
-
-	priv->idev = devm_input_allocate_device(dev);
-	if (!priv->idev)
-		return -ENOMEM;
-
-	priv->idev->name = "CAP1106 capacitive touch sensor";
-	priv->idev->id.bustype = BUS_I2C;
-	priv->idev->evbit[0] = BIT_MASK(EV_KEY);
-
-	if (of_property_read_bool(node, "autorepeat"))
-		__set_bit(EV_REP, priv->idev->evbit);
-
-	for (i = 0; i < CAP1106_NUM_CHN; i++)
-		__set_bit(priv->keycodes[i], priv->idev->keybit);
-
-	__clear_bit(KEY_RESERVED, priv->idev->keybit);
-
-	priv->idev->keycode = priv->keycodes;
-	priv->idev->keycodesize = sizeof(priv->keycodes[0]);
-	priv->idev->keycodemax = ARRAY_SIZE(priv->keycodes);
-
-	priv->idev->id.vendor = CAP1106_MANUFACTURER_ID;
-	priv->idev->id.product = CAP1106_PRODUCT_ID;
-	priv->idev->id.version = rev;
-
-	priv->idev->open = cap1106_input_open;
-	priv->idev->close = cap1106_input_close;
-
-	input_set_drvdata(priv->idev, priv);
-
-	/*
-	 * Put the device in deep sleep mode for now.
-	 * ->open() will bring it back once the it is actually needed.
-	 */
-	cap1106_set_sleep(priv, true);
-
-	error = input_register_device(priv->idev);
-	if (error)
-		return error;
-
-	irq = irq_of_parse_and_map(node, 0);
-	if (!irq) {
-		dev_err(dev, "Unable to parse or map IRQ\n");
-		return -ENXIO;
-	}
-
-	error = devm_request_threaded_irq(dev, irq, NULL, cap1106_thread_func,
-					  IRQF_ONESHOT, dev_name(dev), priv);
-	if (error)
-		return error;
-
-	return 0;
-}
-
-static const struct of_device_id cap1106_dt_ids[] = {
-	{ .compatible = "microchip,cap1106", },
-	{}
-};
-MODULE_DEVICE_TABLE(of, cap1106_dt_ids);
-
-static const struct i2c_device_id cap1106_i2c_ids[] = {
-	{ "cap1106", 0 },
-	{}
-};
-MODULE_DEVICE_TABLE(i2c, cap1106_i2c_ids);
-
-static struct i2c_driver cap1106_i2c_driver = {
-	.driver = {
-		.name	= "cap1106",
-		.owner	= THIS_MODULE,
-		.of_match_table = cap1106_dt_ids,
-	},
-	.id_table	= cap1106_i2c_ids,
-	.probe		= cap1106_i2c_probe,
-};
-
-module_i2c_driver(cap1106_i2c_driver);
-
-MODULE_ALIAS("platform:cap1106");
-MODULE_DESCRIPTION("Microchip CAP1106 driver");
-MODULE_AUTHOR("Daniel Mack <linux@zonque.org>");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/input/keyboard/cap11xx.c b/drivers/input/keyboard/cap11xx.c
new file mode 100644
index 000000000000..4f59f0bab28f
--- /dev/null
+++ b/drivers/input/keyboard/cap11xx.c
@@ -0,0 +1,376 @@
+/*
+ * Input driver for Microchip CAP11xx based capacitive touch sensors
+ *
+ * (c) 2014 Daniel Mack <linux@zonque.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.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/input.h>
+#include <linux/of_irq.h>
+#include <linux/regmap.h>
+#include <linux/i2c.h>
+#include <linux/gpio/consumer.h>
+
+#define CAP11XX_REG_MAIN_CONTROL	0x00
+#define CAP11XX_REG_MAIN_CONTROL_GAIN_SHIFT	(6)
+#define CAP11XX_REG_MAIN_CONTROL_GAIN_MASK	(0xc0)
+#define CAP11XX_REG_MAIN_CONTROL_DLSEEP		BIT(4)
+#define CAP11XX_REG_GENERAL_STATUS	0x02
+#define CAP11XX_REG_SENSOR_INPUT	0x03
+#define CAP11XX_REG_NOISE_FLAG_STATUS	0x0a
+#define CAP11XX_REG_SENOR_DELTA(X)	(0x10 + (X))
+#define CAP11XX_REG_SENSITIVITY_CONTROL	0x1f
+#define CAP11XX_REG_CONFIG		0x20
+#define CAP11XX_REG_SENSOR_ENABLE	0x21
+#define CAP11XX_REG_SENSOR_CONFIG	0x22
+#define CAP11XX_REG_SENSOR_CONFIG2	0x23
+#define CAP11XX_REG_SAMPLING_CONFIG	0x24
+#define CAP11XX_REG_CALIBRATION		0x26
+#define CAP11XX_REG_INT_ENABLE		0x27
+#define CAP11XX_REG_REPEAT_RATE		0x28
+#define CAP11XX_REG_MT_CONFIG		0x2a
+#define CAP11XX_REG_MT_PATTERN_CONFIG	0x2b
+#define CAP11XX_REG_MT_PATTERN		0x2d
+#define CAP11XX_REG_RECALIB_CONFIG	0x2f
+#define CAP11XX_REG_SENSOR_THRESH(X)	(0x30 + (X))
+#define CAP11XX_REG_SENSOR_NOISE_THRESH	0x38
+#define CAP11XX_REG_STANDBY_CHANNEL	0x40
+#define CAP11XX_REG_STANDBY_CONFIG	0x41
+#define CAP11XX_REG_STANDBY_SENSITIVITY	0x42
+#define CAP11XX_REG_STANDBY_THRESH	0x43
+#define CAP11XX_REG_CONFIG2		0x44
+#define CAP11XX_REG_CONFIG2_ALT_POL	BIT(6)
+#define CAP11XX_REG_SENSOR_BASE_CNT(X)	(0x50 + (X))
+#define CAP11XX_REG_SENSOR_CALIB	(0xb1 + (X))
+#define CAP11XX_REG_SENSOR_CALIB_LSB1	0xb9
+#define CAP11XX_REG_SENSOR_CALIB_LSB2	0xba
+#define CAP11XX_REG_PRODUCT_ID		0xfd
+#define CAP11XX_REG_MANUFACTURER_ID	0xfe
+#define CAP11XX_REG_REVISION		0xff
+
+#define CAP11XX_MANUFACTURER_ID	0x5d
+
+struct cap11xx_priv {
+	struct regmap *regmap;
+	struct input_dev *idev;
+
+	/* config */
+	u32 keycodes[];
+};
+
+struct cap11xx_hw_model {
+	u8 product_id;
+	unsigned int num_channels;
+};
+
+enum {
+	CAP1106,
+	CAP1126,
+	CAP1188,
+};
+
+static const struct cap11xx_hw_model cap11xx_devices[] = {
+	[CAP1106] = { .product_id = 0x55, .num_channels = 6 },
+	[CAP1126] = { .product_id = 0x53, .num_channels = 6 },
+	[CAP1188] = { .product_id = 0x50, .num_channels = 8 },
+};
+
+static const struct reg_default cap11xx_reg_defaults[] = {
+	{ CAP11XX_REG_MAIN_CONTROL,		0x00 },
+	{ CAP11XX_REG_GENERAL_STATUS,		0x00 },
+	{ CAP11XX_REG_SENSOR_INPUT,		0x00 },
+	{ CAP11XX_REG_NOISE_FLAG_STATUS,	0x00 },
+	{ CAP11XX_REG_SENSITIVITY_CONTROL,	0x2f },
+	{ CAP11XX_REG_CONFIG,			0x20 },
+	{ CAP11XX_REG_SENSOR_ENABLE,		0x3f },
+	{ CAP11XX_REG_SENSOR_CONFIG,		0xa4 },
+	{ CAP11XX_REG_SENSOR_CONFIG2,		0x07 },
+	{ CAP11XX_REG_SAMPLING_CONFIG,		0x39 },
+	{ CAP11XX_REG_CALIBRATION,		0x00 },
+	{ CAP11XX_REG_INT_ENABLE,		0x3f },
+	{ CAP11XX_REG_REPEAT_RATE,		0x3f },
+	{ CAP11XX_REG_MT_CONFIG,		0x80 },
+	{ CAP11XX_REG_MT_PATTERN_CONFIG,	0x00 },
+	{ CAP11XX_REG_MT_PATTERN,		0x3f },
+	{ CAP11XX_REG_RECALIB_CONFIG,		0x8a },
+	{ CAP11XX_REG_SENSOR_THRESH(0),		0x40 },
+	{ CAP11XX_REG_SENSOR_THRESH(1),		0x40 },
+	{ CAP11XX_REG_SENSOR_THRESH(2),		0x40 },
+	{ CAP11XX_REG_SENSOR_THRESH(3),		0x40 },
+	{ CAP11XX_REG_SENSOR_THRESH(4),		0x40 },
+	{ CAP11XX_REG_SENSOR_THRESH(5),		0x40 },
+	{ CAP11XX_REG_SENSOR_NOISE_THRESH,	0x01 },
+	{ CAP11XX_REG_STANDBY_CHANNEL,		0x00 },
+	{ CAP11XX_REG_STANDBY_CONFIG,		0x39 },
+	{ CAP11XX_REG_STANDBY_SENSITIVITY,	0x02 },
+	{ CAP11XX_REG_STANDBY_THRESH,		0x40 },
+	{ CAP11XX_REG_CONFIG2,			0x40 },
+	{ CAP11XX_REG_SENSOR_CALIB_LSB1,	0x00 },
+	{ CAP11XX_REG_SENSOR_CALIB_LSB2,	0x00 },
+};
+
+static bool cap11xx_volatile_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case CAP11XX_REG_MAIN_CONTROL:
+	case CAP11XX_REG_SENSOR_INPUT:
+	case CAP11XX_REG_SENOR_DELTA(0):
+	case CAP11XX_REG_SENOR_DELTA(1):
+	case CAP11XX_REG_SENOR_DELTA(2):
+	case CAP11XX_REG_SENOR_DELTA(3):
+	case CAP11XX_REG_SENOR_DELTA(4):
+	case CAP11XX_REG_SENOR_DELTA(5):
+	case CAP11XX_REG_PRODUCT_ID:
+	case CAP11XX_REG_MANUFACTURER_ID:
+	case CAP11XX_REG_REVISION:
+		return true;
+	}
+
+	return false;
+}
+
+static const struct regmap_config cap11xx_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.max_register = CAP11XX_REG_REVISION,
+	.reg_defaults = cap11xx_reg_defaults,
+
+	.num_reg_defaults = ARRAY_SIZE(cap11xx_reg_defaults),
+	.cache_type = REGCACHE_RBTREE,
+	.volatile_reg = cap11xx_volatile_reg,
+};
+
+static irqreturn_t cap11xx_thread_func(int irq_num, void *data)
+{
+	struct cap11xx_priv *priv = data;
+	unsigned int status;
+	int ret, i;
+
+	/*
+	 * Deassert interrupt. This needs to be done before reading the status
+	 * registers, which will not carry valid values otherwise.
+	 */
+	ret = regmap_update_bits(priv->regmap, CAP11XX_REG_MAIN_CONTROL, 1, 0);
+	if (ret < 0)
+		goto out;
+
+	ret = regmap_read(priv->regmap, CAP11XX_REG_SENSOR_INPUT, &status);
+	if (ret < 0)
+		goto out;
+
+	for (i = 0; i < priv->idev->keycodemax; i++)
+		input_report_key(priv->idev, priv->keycodes[i],
+				 status & (1 << i));
+
+	input_sync(priv->idev);
+
+out:
+	return IRQ_HANDLED;
+}
+
+static int cap11xx_set_sleep(struct cap11xx_priv *priv, bool sleep)
+{
+	return regmap_update_bits(priv->regmap, CAP11XX_REG_MAIN_CONTROL,
+				  CAP11XX_REG_MAIN_CONTROL_DLSEEP,
+				  sleep ? CAP11XX_REG_MAIN_CONTROL_DLSEEP : 0);
+}
+
+static int cap11xx_input_open(struct input_dev *idev)
+{
+	struct cap11xx_priv *priv = input_get_drvdata(idev);
+
+	return cap11xx_set_sleep(priv, false);
+}
+
+static void cap11xx_input_close(struct input_dev *idev)
+{
+	struct cap11xx_priv *priv = input_get_drvdata(idev);
+
+	cap11xx_set_sleep(priv, true);
+}
+
+static int cap11xx_i2c_probe(struct i2c_client *i2c_client,
+			     const struct i2c_device_id *id)
+{
+	struct device *dev = &i2c_client->dev;
+	struct cap11xx_priv *priv;
+	struct device_node *node;
+	const struct cap11xx_hw_model *cap;
+	int i, error, irq, gain = 0;
+	unsigned int val, rev;
+	u32 gain32;
+
+	if (id->driver_data >= ARRAY_SIZE(cap11xx_devices)) {
+		dev_err(dev, "Invalid device ID %lu\n", id->driver_data);
+		return -EINVAL;
+	}
+
+	cap = &cap11xx_devices[id->driver_data];
+	if (!cap || !cap->num_channels) {
+		dev_err(dev, "Invalid device configuration\n");
+		return -EINVAL;
+	}
+
+	priv = devm_kzalloc(dev,
+			    sizeof(*priv) +
+				cap->num_channels * sizeof(priv->keycodes[0]),
+			    GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->regmap = devm_regmap_init_i2c(i2c_client, &cap11xx_regmap_config);
+	if (IS_ERR(priv->regmap))
+		return PTR_ERR(priv->regmap);
+
+	error = regmap_read(priv->regmap, CAP11XX_REG_PRODUCT_ID, &val);
+	if (error)
+		return error;
+
+	if (val != cap->product_id) {
+		dev_err(dev, "Product ID: Got 0x%02x, expected 0x%02x\n",
+			val, cap->product_id);
+		return -ENXIO;
+	}
+
+	error = regmap_read(priv->regmap, CAP11XX_REG_MANUFACTURER_ID, &val);
+	if (error)
+		return error;
+
+	if (val != CAP11XX_MANUFACTURER_ID) {
+		dev_err(dev, "Manufacturer ID: Got 0x%02x, expected 0x%02x\n",
+			val, CAP11XX_MANUFACTURER_ID);
+		return -ENXIO;
+	}
+
+	error = regmap_read(priv->regmap, CAP11XX_REG_REVISION, &rev);
+	if (error < 0)
+		return error;
+
+	dev_info(dev, "CAP11XX detected, revision 0x%02x\n", rev);
+	i2c_set_clientdata(i2c_client, priv);
+	node = dev->of_node;
+
+	if (!of_property_read_u32(node, "microchip,sensor-gain", &gain32)) {
+		if (is_power_of_2(gain32) && gain32 <= 8)
+			gain = ilog2(gain32);
+		else
+			dev_err(dev, "Invalid sensor-gain value %d\n", gain32);
+	}
+
+	if (of_property_read_bool(node, "microchip,irq-active-high")) {
+		error = regmap_update_bits(priv->regmap, CAP11XX_REG_CONFIG2,
+					   CAP11XX_REG_CONFIG2_ALT_POL, 0);
+		if (error)
+			return error;
+	}
+
+	/* Provide some useful defaults */
+	for (i = 0; i < cap->num_channels; i++)
+		priv->keycodes[i] = KEY_A + i;
+
+	of_property_read_u32_array(node, "linux,keycodes",
+				   priv->keycodes, cap->num_channels);
+
+	error = regmap_update_bits(priv->regmap, CAP11XX_REG_MAIN_CONTROL,
+				   CAP11XX_REG_MAIN_CONTROL_GAIN_MASK,
+				   gain << CAP11XX_REG_MAIN_CONTROL_GAIN_SHIFT);
+	if (error)
+		return error;
+
+	/* Disable autorepeat. The Linux input system has its own handling. */
+	error = regmap_write(priv->regmap, CAP11XX_REG_REPEAT_RATE, 0);
+	if (error)
+		return error;
+
+	priv->idev = devm_input_allocate_device(dev);
+	if (!priv->idev)
+		return -ENOMEM;
+
+	priv->idev->name = "CAP11XX capacitive touch sensor";
+	priv->idev->id.bustype = BUS_I2C;
+	priv->idev->evbit[0] = BIT_MASK(EV_KEY);
+
+	if (of_property_read_bool(node, "autorepeat"))
+		__set_bit(EV_REP, priv->idev->evbit);
+
+	for (i = 0; i < cap->num_channels; i++)
+		__set_bit(priv->keycodes[i], priv->idev->keybit);
+
+	__clear_bit(KEY_RESERVED, priv->idev->keybit);
+
+	priv->idev->keycode = priv->keycodes;
+	priv->idev->keycodesize = sizeof(priv->keycodes[0]);
+	priv->idev->keycodemax = cap->num_channels;
+
+	priv->idev->id.vendor = CAP11XX_MANUFACTURER_ID;
+	priv->idev->id.product = cap->product_id;
+	priv->idev->id.version = rev;
+
+	priv->idev->open = cap11xx_input_open;
+	priv->idev->close = cap11xx_input_close;
+
+	input_set_drvdata(priv->idev, priv);
+
+	/*
+	 * Put the device in deep sleep mode for now.
+	 * ->open() will bring it back once the it is actually needed.
+	 */
+	cap11xx_set_sleep(priv, true);
+
+	error = input_register_device(priv->idev);
+	if (error)
+		return error;
+
+	irq = irq_of_parse_and_map(node, 0);
+	if (!irq) {
+		dev_err(dev, "Unable to parse or map IRQ\n");
+		return -ENXIO;
+	}
+
+	error = devm_request_threaded_irq(dev, irq, NULL, cap11xx_thread_func,
+					  IRQF_ONESHOT, dev_name(dev), priv);
+	if (error)
+		return error;
+
+	return 0;
+}
+
+static const struct of_device_id cap11xx_dt_ids[] = {
+	{ .compatible = "microchip,cap1106", },
+	{ .compatible = "microchip,cap1126", },
+	{ .compatible = "microchip,cap1188", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, cap11xx_dt_ids);
+
+static const struct i2c_device_id cap11xx_i2c_ids[] = {
+	{ "cap1106", CAP1106 },
+	{ "cap1126", CAP1126 },
+	{ "cap1188", CAP1188 },
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, cap11xx_i2c_ids);
+
+static struct i2c_driver cap11xx_i2c_driver = {
+	.driver = {
+		.name	= "cap11xx",
+		.owner	= THIS_MODULE,
+		.of_match_table = cap11xx_dt_ids,
+	},
+	.id_table	= cap11xx_i2c_ids,
+	.probe		= cap11xx_i2c_probe,
+};
+
+module_i2c_driver(cap11xx_i2c_driver);
+
+MODULE_ALIAS("platform:cap11xx");
+MODULE_DESCRIPTION("Microchip CAP11XX driver");
+MODULE_AUTHOR("Daniel Mack <linux@zonque.org>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/input/keyboard/clps711x-keypad.c b/drivers/input/keyboard/clps711x-keypad.c
index 552b65c6e6b0..27ef29f8fe6a 100644
--- a/drivers/input/keyboard/clps711x-keypad.c
+++ b/drivers/input/keyboard/clps711x-keypad.c
@@ -194,7 +194,6 @@ MODULE_DEVICE_TABLE(of, clps711x_keypad_of_match);
 static struct platform_driver clps711x_keypad_driver = {
 	.driver	= {
 		.name		= "clps711x-keypad",
-		.owner		= THIS_MODULE,
 		.of_match_table	= clps711x_keypad_of_match,
 	},
 	.probe	= clps711x_keypad_probe,
diff --git a/drivers/input/keyboard/davinci_keyscan.c b/drivers/input/keyboard/davinci_keyscan.c
index 1559dc1cf951..f363d1d2907a 100644
--- a/drivers/input/keyboard/davinci_keyscan.c
+++ b/drivers/input/keyboard/davinci_keyscan.c
@@ -322,7 +322,6 @@ static int davinci_ks_remove(struct platform_device *pdev)
 static struct platform_driver davinci_ks_driver = {
 	.driver	= {
 		.name = "davinci_keyscan",
-		.owner = THIS_MODULE,
 	},
 	.remove	= davinci_ks_remove,
 };
diff --git a/drivers/input/keyboard/ep93xx_keypad.c b/drivers/input/keyboard/ep93xx_keypad.c
index e59876212b8c..f77b295e0123 100644
--- a/drivers/input/keyboard/ep93xx_keypad.c
+++ b/drivers/input/keyboard/ep93xx_keypad.c
@@ -373,7 +373,6 @@ static int ep93xx_keypad_remove(struct platform_device *pdev)
 static struct platform_driver ep93xx_keypad_driver = {
 	.driver		= {
 		.name	= "ep93xx-keypad",
-		.owner	= THIS_MODULE,
 		.pm	= &ep93xx_keypad_pm_ops,
 	},
 	.probe		= ep93xx_keypad_probe,
diff --git a/drivers/input/keyboard/goldfish_events.c b/drivers/input/keyboard/goldfish_events.c
index 69e854763370..907e4e278fce 100644
--- a/drivers/input/keyboard/goldfish_events.c
+++ b/drivers/input/keyboard/goldfish_events.c
@@ -181,7 +181,6 @@ static int events_probe(struct platform_device *pdev)
 static struct platform_driver events_driver = {
 	.probe	= events_probe,
 	.driver	= {
-		.owner	= THIS_MODULE,
 		.name	= "goldfish_events",
 	},
 };
diff --git a/drivers/input/keyboard/gpio_keys.c b/drivers/input/keyboard/gpio_keys.c
index 8c98e97f8e41..d4dd78a7d56b 100644
--- a/drivers/input/keyboard/gpio_keys.c
+++ b/drivers/input/keyboard/gpio_keys.c
@@ -29,6 +29,7 @@
 #include <linux/of.h>
 #include <linux/of_platform.h>
 #include <linux/of_gpio.h>
+#include <linux/of_irq.h>
 #include <linux/spinlock.h>
 
 struct gpio_button_data {
@@ -617,27 +618,31 @@ gpio_keys_get_devtree_pdata(struct device *dev)
 
 	i = 0;
 	for_each_child_of_node(node, pp) {
-		int gpio;
+		int gpio = -1;
 		enum of_gpio_flags flags;
 
-		if (!of_find_property(pp, "gpios", NULL)) {
-			pdata->nbuttons--;
-			dev_warn(dev, "Found button without gpios\n");
-			continue;
-		}
+		button = &pdata->buttons[i++];
 
-		gpio = of_get_gpio_flags(pp, 0, &flags);
-		if (gpio < 0) {
-			error = gpio;
-			if (error != -EPROBE_DEFER)
-				dev_err(dev,
-					"Failed to get gpio flags, error: %d\n",
-					error);
-			return ERR_PTR(error);
+		if (!of_find_property(pp, "gpios", NULL)) {
+			button->irq = irq_of_parse_and_map(pp, 0);
+			if (button->irq == 0) {
+				i--;
+				pdata->nbuttons--;
+				dev_warn(dev, "Found button without gpios or irqs\n");
+				continue;
+			}
+		} else {
+			gpio = of_get_gpio_flags(pp, 0, &flags);
+			if (gpio < 0) {
+				error = gpio;
+				if (error != -EPROBE_DEFER)
+					dev_err(dev,
+						"Failed to get gpio flags, error: %d\n",
+						error);
+				return ERR_PTR(error);
+			}
 		}
 
-		button = &pdata->buttons[i++];
-
 		button->gpio = gpio;
 		button->active_low = flags & OF_GPIO_ACTIVE_LOW;
 
@@ -835,7 +840,6 @@ static struct platform_driver gpio_keys_device_driver = {
 	.remove		= gpio_keys_remove,
 	.driver		= {
 		.name	= "gpio-keys",
-		.owner	= THIS_MODULE,
 		.pm	= &gpio_keys_pm_ops,
 		.of_match_table = of_match_ptr(gpio_keys_of_match),
 	}
diff --git a/drivers/input/keyboard/gpio_keys_polled.c b/drivers/input/keyboard/gpio_keys_polled.c
index c9c1c8ca7267..90df4df58b07 100644
--- a/drivers/input/keyboard/gpio_keys_polled.c
+++ b/drivers/input/keyboard/gpio_keys_polled.c
@@ -297,7 +297,6 @@ static struct platform_driver gpio_keys_polled_driver = {
 	.probe	= gpio_keys_polled_probe,
 	.driver	= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = gpio_keys_polled_of_match,
 	},
 };
diff --git a/drivers/input/keyboard/imx_keypad.c b/drivers/input/keyboard/imx_keypad.c
index 20a99c368d16..e53f232eda0e 100644
--- a/drivers/input/keyboard/imx_keypad.c
+++ b/drivers/input/keyboard/imx_keypad.c
@@ -580,7 +580,6 @@ static SIMPLE_DEV_PM_OPS(imx_kbd_pm_ops, imx_kbd_suspend, imx_kbd_resume);
 static struct platform_driver imx_keypad_driver = {
 	.driver		= {
 		.name	= "imx-keypad",
-		.owner	= THIS_MODULE,
 		.pm	= &imx_kbd_pm_ops,
 		.of_match_table = of_match_ptr(imx_keypad_of_match),
 	},
diff --git a/drivers/input/keyboard/jornada680_kbd.c b/drivers/input/keyboard/jornada680_kbd.c
index 0ba4428da24a..80c81278ad2c 100644
--- a/drivers/input/keyboard/jornada680_kbd.c
+++ b/drivers/input/keyboard/jornada680_kbd.c
@@ -237,7 +237,6 @@ static int jornada680kbd_probe(struct platform_device *pdev)
 static struct platform_driver jornada680kbd_driver = {
 	.driver	= {
 		.name	= "jornada680_kbd",
-		.owner	= THIS_MODULE,
 	},
 	.probe	= jornada680kbd_probe,
 };
diff --git a/drivers/input/keyboard/jornada720_kbd.c b/drivers/input/keyboard/jornada720_kbd.c
index cd729d485e98..421d9c55b0e8 100644
--- a/drivers/input/keyboard/jornada720_kbd.c
+++ b/drivers/input/keyboard/jornada720_kbd.c
@@ -167,7 +167,6 @@ MODULE_ALIAS("platform:jornada720_kbd");
 static struct platform_driver jornada720_kbd_driver = {
 	.driver  = {
 		.name    = "jornada720_kbd",
-		.owner	= THIS_MODULE,
 	 },
 	.probe   = jornada720_kbd_probe,
 	.remove  = jornada720_kbd_remove,
diff --git a/drivers/input/keyboard/lm8323.c b/drivers/input/keyboard/lm8323.c
index cb32e2b506b7..21bea52d4365 100644
--- a/drivers/input/keyboard/lm8323.c
+++ b/drivers/input/keyboard/lm8323.c
@@ -616,6 +616,8 @@ static ssize_t lm8323_set_disable(struct device *dev,
 	unsigned int i;
 
 	ret = kstrtouint(buf, 10, &i);
+	if (ret)
+		return ret;
 
 	mutex_lock(&lm->lock);
 	lm->kp_enabled = !i;
diff --git a/drivers/input/keyboard/lpc32xx-keys.c b/drivers/input/keyboard/lpc32xx-keys.c
index 8b1b01361ec6..265d641c40e2 100644
--- a/drivers/input/keyboard/lpc32xx-keys.c
+++ b/drivers/input/keyboard/lpc32xx-keys.c
@@ -66,7 +66,6 @@
 struct lpc32xx_kscan_drv {
 	struct input_dev *input;
 	struct clk *clk;
-	struct resource *iores;
 	void __iomem *kscan_base;
 	unsigned int irq;
 
@@ -188,32 +187,27 @@ static int lpc32xx_kscan_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	kscandat = kzalloc(sizeof(struct lpc32xx_kscan_drv), GFP_KERNEL);
-	if (!kscandat) {
-		dev_err(&pdev->dev, "failed to allocate memory\n");
+	kscandat = devm_kzalloc(&pdev->dev, sizeof(*kscandat),
+				GFP_KERNEL);
+	if (!kscandat)
 		return -ENOMEM;
-	}
 
 	error = lpc32xx_parse_dt(&pdev->dev, kscandat);
 	if (error) {
 		dev_err(&pdev->dev, "failed to parse device tree\n");
-		goto err_free_mem;
+		return error;
 	}
 
 	keymap_size = sizeof(kscandat->keymap[0]) *
 				(kscandat->matrix_sz << kscandat->row_shift);
-	kscandat->keymap = kzalloc(keymap_size, GFP_KERNEL);
-	if (!kscandat->keymap) {
-		dev_err(&pdev->dev, "could not allocate memory for keymap\n");
-		error = -ENOMEM;
-		goto err_free_mem;
-	}
+	kscandat->keymap = devm_kzalloc(&pdev->dev, keymap_size, GFP_KERNEL);
+	if (!kscandat->keymap)
+		return -ENOMEM;
 
-	kscandat->input = input = input_allocate_device();
+	kscandat->input = input = devm_input_allocate_device(&pdev->dev);
 	if (!input) {
 		dev_err(&pdev->dev, "failed to allocate input device\n");
-		error = -ENOMEM;
-		goto err_free_keymap;
+		return -ENOMEM;
 	}
 
 	/* Setup key input */
@@ -234,39 +228,26 @@ static int lpc32xx_kscan_probe(struct platform_device *pdev)
 					   kscandat->keymap, kscandat->input);
 	if (error) {
 		dev_err(&pdev->dev, "failed to build keymap\n");
-		goto err_free_input;
+		return error;
 	}
 
 	input_set_drvdata(kscandat->input, kscandat);
 
-	kscandat->iores = request_mem_region(res->start, resource_size(res),
-					     pdev->name);
-	if (!kscandat->iores) {
-		dev_err(&pdev->dev, "failed to request I/O memory\n");
-		error = -EBUSY;
-		goto err_free_input;
-	}
-
-	kscandat->kscan_base = ioremap(kscandat->iores->start,
-				       resource_size(kscandat->iores));
-	if (!kscandat->kscan_base) {
-		dev_err(&pdev->dev, "failed to remap I/O memory\n");
-		error = -EBUSY;
-		goto err_release_memregion;
-	}
+	kscandat->kscan_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(kscandat->kscan_base))
+		return PTR_ERR(kscandat->kscan_base);
 
 	/* Get the key scanner clock */
-	kscandat->clk = clk_get(&pdev->dev, NULL);
+	kscandat->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(kscandat->clk)) {
 		dev_err(&pdev->dev, "failed to get clock\n");
-		error = PTR_ERR(kscandat->clk);
-		goto err_unmap;
+		return PTR_ERR(kscandat->clk);
 	}
 
 	/* Configure the key scanner */
 	error = clk_prepare_enable(kscandat->clk);
 	if (error)
-		goto err_clk_put;
+		return error;
 
 	writel(kscandat->deb_clks, LPC32XX_KS_DEB(kscandat->kscan_base));
 	writel(kscandat->scan_delay, LPC32XX_KS_SCAN_CTL(kscandat->kscan_base));
@@ -277,52 +258,20 @@ static int lpc32xx_kscan_probe(struct platform_device *pdev)
 	writel(1, LPC32XX_KS_IRQ(kscandat->kscan_base));
 	clk_disable_unprepare(kscandat->clk);
 
-	error = request_irq(irq, lpc32xx_kscan_irq, 0, pdev->name, kscandat);
+	error = devm_request_irq(&pdev->dev, irq, lpc32xx_kscan_irq, 0,
+				 pdev->name, kscandat);
 	if (error) {
 		dev_err(&pdev->dev, "failed to request irq\n");
-		goto err_clk_put;
+		return error;
 	}
 
 	error = input_register_device(kscandat->input);
 	if (error) {
 		dev_err(&pdev->dev, "failed to register input device\n");
-		goto err_free_irq;
+		return error;
 	}
 
 	platform_set_drvdata(pdev, kscandat);
-	return 0;
-
-err_free_irq:
-	free_irq(irq, kscandat);
-err_clk_put:
-	clk_put(kscandat->clk);
-err_unmap:
-	iounmap(kscandat->kscan_base);
-err_release_memregion:
-	release_mem_region(kscandat->iores->start,
-			   resource_size(kscandat->iores));
-err_free_input:
-	input_free_device(kscandat->input);
-err_free_keymap:
-	kfree(kscandat->keymap);
-err_free_mem:
-	kfree(kscandat);
-
-	return error;
-}
-
-static int lpc32xx_kscan_remove(struct platform_device *pdev)
-{
-	struct lpc32xx_kscan_drv *kscandat = platform_get_drvdata(pdev);
-
-	free_irq(platform_get_irq(pdev, 0), kscandat);
-	clk_put(kscandat->clk);
-	iounmap(kscandat->kscan_base);
-	release_mem_region(kscandat->iores->start,
-			   resource_size(kscandat->iores));
-	input_unregister_device(kscandat->input);
-	kfree(kscandat->keymap);
-	kfree(kscandat);
 
 	return 0;
 }
@@ -378,10 +327,8 @@ MODULE_DEVICE_TABLE(of, lpc32xx_kscan_match);
 
 static struct platform_driver lpc32xx_kscan_driver = {
 	.probe		= lpc32xx_kscan_probe,
-	.remove		= lpc32xx_kscan_remove,
 	.driver		= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 		.pm	= &lpc32xx_kscan_pm_ops,
 		.of_match_table = lpc32xx_kscan_match,
 	}
diff --git a/drivers/input/keyboard/matrix_keypad.c b/drivers/input/keyboard/matrix_keypad.c
index e651fa692afe..b370a59cb759 100644
--- a/drivers/input/keyboard/matrix_keypad.c
+++ b/drivers/input/keyboard/matrix_keypad.c
@@ -565,7 +565,6 @@ static struct platform_driver matrix_keypad_driver = {
 	.remove		= matrix_keypad_remove,
 	.driver		= {
 		.name	= "matrix-keypad",
-		.owner	= THIS_MODULE,
 		.pm	= &matrix_keypad_pm_ops,
 		.of_match_table = of_match_ptr(matrix_keypad_dt_match),
 	},
diff --git a/drivers/input/keyboard/mpr121_touchkey.c b/drivers/input/keyboard/mpr121_touchkey.c
index 009c82256e89..3aa2ec45bcab 100644
--- a/drivers/input/keyboard/mpr121_touchkey.c
+++ b/drivers/input/keyboard/mpr121_touchkey.c
@@ -214,13 +214,14 @@ static int mpr_touchkey_probe(struct i2c_client *client,
 		return -EINVAL;
 	}
 
-	mpr121 = kzalloc(sizeof(struct mpr121_touchkey), GFP_KERNEL);
-	input_dev = input_allocate_device();
-	if (!mpr121 || !input_dev) {
-		dev_err(&client->dev, "Failed to allocate memory\n");
-		error = -ENOMEM;
-		goto err_free_mem;
-	}
+	mpr121 = devm_kzalloc(&client->dev, sizeof(*mpr121),
+			      GFP_KERNEL);
+	if (!mpr121)
+		return -ENOMEM;
+
+	input_dev = devm_input_allocate_device(&client->dev);
+	if (!input_dev)
+		return -ENOMEM;
 
 	mpr121->client = client;
 	mpr121->input_dev = input_dev;
@@ -243,44 +244,26 @@ static int mpr_touchkey_probe(struct i2c_client *client,
 	error = mpr121_phys_init(pdata, mpr121, client);
 	if (error) {
 		dev_err(&client->dev, "Failed to init register\n");
-		goto err_free_mem;
+		return error;
 	}
 
-	error = request_threaded_irq(client->irq, NULL,
+	error = devm_request_threaded_irq(&client->dev, client->irq, NULL,
 				     mpr_touchkey_interrupt,
 				     IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
 				     client->dev.driver->name, mpr121);
 	if (error) {
 		dev_err(&client->dev, "Failed to register interrupt\n");
-		goto err_free_mem;
+		return error;
 	}
 
 	error = input_register_device(input_dev);
 	if (error)
-		goto err_free_irq;
+		return error;
 
 	i2c_set_clientdata(client, mpr121);
 	device_init_wakeup(&client->dev, pdata->wakeup);
 
 	return 0;
-
-err_free_irq:
-	free_irq(client->irq, mpr121);
-err_free_mem:
-	input_free_device(input_dev);
-	kfree(mpr121);
-	return error;
-}
-
-static int mpr_touchkey_remove(struct i2c_client *client)
-{
-	struct mpr121_touchkey *mpr121 = i2c_get_clientdata(client);
-
-	free_irq(client->irq, mpr121);
-	input_unregister_device(mpr121->input_dev);
-	kfree(mpr121);
-
-	return 0;
 }
 
 #ifdef CONFIG_PM_SLEEP
@@ -327,7 +310,6 @@ static struct i2c_driver mpr_touchkey_driver = {
 	},
 	.id_table	= mpr121_id,
 	.probe		= mpr_touchkey_probe,
-	.remove		= mpr_touchkey_remove,
 };
 
 module_i2c_driver(mpr_touchkey_driver);
diff --git a/drivers/input/keyboard/nomadik-ske-keypad.c b/drivers/input/keyboard/nomadik-ske-keypad.c
index 63332e2f8628..c7d5b1666fc3 100644
--- a/drivers/input/keyboard/nomadik-ske-keypad.c
+++ b/drivers/input/keyboard/nomadik-ske-keypad.c
@@ -425,7 +425,6 @@ static SIMPLE_DEV_PM_OPS(ske_keypad_dev_pm_ops,
 static struct platform_driver ske_keypad_driver = {
 	.driver = {
 		.name = "nmk-ske-keypad",
-		.owner  = THIS_MODULE,
 		.pm = &ske_keypad_dev_pm_ops,
 	},
 	.remove = ske_keypad_remove,
diff --git a/drivers/input/keyboard/nspire-keypad.c b/drivers/input/keyboard/nspire-keypad.c
index b31064981e96..7abfd34eb87e 100644
--- a/drivers/input/keyboard/nspire-keypad.c
+++ b/drivers/input/keyboard/nspire-keypad.c
@@ -268,7 +268,6 @@ MODULE_DEVICE_TABLE(of, nspire_keypad_dt_match);
 static struct platform_driver nspire_keypad_driver = {
 	.driver = {
 		.name = "nspire-keypad",
-		.owner = THIS_MODULE,
 		.of_match_table = nspire_keypad_dt_match,
 	},
 	.probe = nspire_keypad_probe,
diff --git a/drivers/input/keyboard/omap-keypad.c b/drivers/input/keyboard/omap-keypad.c
index b1acc9852eb7..7502e46165fa 100644
--- a/drivers/input/keyboard/omap-keypad.c
+++ b/drivers/input/keyboard/omap-keypad.c
@@ -383,7 +383,6 @@ static struct platform_driver omap_kp_driver = {
 	.resume		= omap_kp_resume,
 	.driver		= {
 		.name	= "omap-keypad",
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(omap_kp_driver);
diff --git a/drivers/input/keyboard/omap4-keypad.c b/drivers/input/keyboard/omap4-keypad.c
index 024b7bdffe5b..b052afec9a11 100644
--- a/drivers/input/keyboard/omap4-keypad.c
+++ b/drivers/input/keyboard/omap4-keypad.c
@@ -460,7 +460,6 @@ static struct platform_driver omap4_keypad_driver = {
 	.remove		= omap4_keypad_remove,
 	.driver		= {
 		.name	= "omap4-keypad",
-		.owner	= THIS_MODULE,
 		.pm	= &omap4_keypad_pm_ops,
 		.of_match_table = omap_keypad_dt_match,
 	},
diff --git a/drivers/input/keyboard/pmic8xxx-keypad.c b/drivers/input/keyboard/pmic8xxx-keypad.c
index 80c6b0ef3fc8..32580afecc26 100644
--- a/drivers/input/keyboard/pmic8xxx-keypad.c
+++ b/drivers/input/keyboard/pmic8xxx-keypad.c
@@ -687,7 +687,6 @@ static struct platform_driver pmic8xxx_kp_driver = {
 	.probe		= pmic8xxx_kp_probe,
 	.driver		= {
 		.name = "pm8xxx-keypad",
-		.owner = THIS_MODULE,
 		.pm = &pm8xxx_kp_pm_ops,
 		.of_match_table = pm8xxx_match_table,
 	},
diff --git a/drivers/input/keyboard/pxa27x_keypad.c b/drivers/input/keyboard/pxa27x_keypad.c
index a15063bea700..a90d6bdc499e 100644
--- a/drivers/input/keyboard/pxa27x_keypad.c
+++ b/drivers/input/keyboard/pxa27x_keypad.c
@@ -741,37 +741,27 @@ static int pxa27x_keypad_probe(struct platform_device *pdev)
 		return -ENXIO;
 	}
 
-	keypad = kzalloc(sizeof(struct pxa27x_keypad), GFP_KERNEL);
-	input_dev = input_allocate_device();
-	if (!keypad || !input_dev) {
-		dev_err(&pdev->dev, "failed to allocate memory\n");
-		error = -ENOMEM;
-		goto failed_free;
-	}
+	keypad = devm_kzalloc(&pdev->dev, sizeof(*keypad),
+			      GFP_KERNEL);
+	if (!keypad)
+		return -ENOMEM;
+
+	input_dev = devm_input_allocate_device(&pdev->dev);
+	if (!input_dev)
+		return -ENOMEM;
 
 	keypad->pdata = pdata;
 	keypad->input_dev = input_dev;
 	keypad->irq = irq;
 
-	res = request_mem_region(res->start, resource_size(res), pdev->name);
-	if (res == NULL) {
-		dev_err(&pdev->dev, "failed to request I/O memory\n");
-		error = -EBUSY;
-		goto failed_free;
-	}
-
-	keypad->mmio_base = ioremap(res->start, resource_size(res));
-	if (keypad->mmio_base == NULL) {
-		dev_err(&pdev->dev, "failed to remap I/O memory\n");
-		error = -ENXIO;
-		goto failed_free_mem;
-	}
+	keypad->mmio_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(keypad->mmio_base))
+		return PTR_ERR(keypad->mmio_base);
 
-	keypad->clk = clk_get(&pdev->dev, NULL);
+	keypad->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(keypad->clk)) {
 		dev_err(&pdev->dev, "failed to get keypad clock\n");
-		error = PTR_ERR(keypad->clk);
-		goto failed_free_io;
+		return PTR_ERR(keypad->clk);
 	}
 
 	input_dev->name = pdev->name;
@@ -802,7 +792,7 @@ static int pxa27x_keypad_probe(struct platform_device *pdev)
 	}
 	if (error) {
 		dev_err(&pdev->dev, "failed to build keycode\n");
-		goto failed_put_clk;
+		return error;
 	}
 
 	keypad->row_shift = get_count_order(pdata->matrix_key_cols);
@@ -812,61 +802,26 @@ static int pxa27x_keypad_probe(struct platform_device *pdev)
 		input_dev->evbit[0] |= BIT_MASK(EV_REL);
 	}
 
-	error = request_irq(irq, pxa27x_keypad_irq_handler, 0,
-			    pdev->name, keypad);
+	error = devm_request_irq(&pdev->dev, irq, pxa27x_keypad_irq_handler,
+				 0, pdev->name, keypad);
 	if (error) {
 		dev_err(&pdev->dev, "failed to request IRQ\n");
-		goto failed_put_clk;
+		return error;
 	}
 
 	/* Register the input device */
 	error = input_register_device(input_dev);
 	if (error) {
 		dev_err(&pdev->dev, "failed to register input device\n");
-		goto failed_free_irq;
+		return error;
 	}
 
 	platform_set_drvdata(pdev, keypad);
 	device_init_wakeup(&pdev->dev, 1);
 
 	return 0;
-
-failed_free_irq:
-	free_irq(irq, keypad);
-failed_put_clk:
-	clk_put(keypad->clk);
-failed_free_io:
-	iounmap(keypad->mmio_base);
-failed_free_mem:
-	release_mem_region(res->start, resource_size(res));
-failed_free:
-	input_free_device(input_dev);
-	kfree(keypad);
-	return error;
 }
 
-static int pxa27x_keypad_remove(struct platform_device *pdev)
-{
-	struct pxa27x_keypad *keypad = platform_get_drvdata(pdev);
-	struct resource *res;
-
-	free_irq(keypad->irq, keypad);
-	clk_put(keypad->clk);
-
-	input_unregister_device(keypad->input_dev);
-	iounmap(keypad->mmio_base);
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	release_mem_region(res->start, resource_size(res));
-
-	kfree(keypad);
-
-	return 0;
-}
-
-/* work with hotplug and coldplug */
-MODULE_ALIAS("platform:pxa27x-keypad");
-
 #ifdef CONFIG_OF
 static const struct of_device_id pxa27x_keypad_dt_match[] = {
 	{ .compatible = "marvell,pxa27x-keypad" },
@@ -877,11 +832,9 @@ MODULE_DEVICE_TABLE(of, pxa27x_keypad_dt_match);
 
 static struct platform_driver pxa27x_keypad_driver = {
 	.probe		= pxa27x_keypad_probe,
-	.remove		= pxa27x_keypad_remove,
 	.driver		= {
 		.name	= "pxa27x-keypad",
 		.of_match_table = of_match_ptr(pxa27x_keypad_dt_match),
-		.owner	= THIS_MODULE,
 		.pm	= &pxa27x_keypad_pm_ops,
 	},
 };
@@ -889,3 +842,5 @@ module_platform_driver(pxa27x_keypad_driver);
 
 MODULE_DESCRIPTION("PXA27x Keypad Controller Driver");
 MODULE_LICENSE("GPL");
+/* work with hotplug and coldplug */
+MODULE_ALIAS("platform:pxa27x-keypad");
diff --git a/drivers/input/keyboard/pxa930_rotary.c b/drivers/input/keyboard/pxa930_rotary.c
index 374ca0246c8f..1cf5211fddaa 100644
--- a/drivers/input/keyboard/pxa930_rotary.c
+++ b/drivers/input/keyboard/pxa930_rotary.c
@@ -189,7 +189,6 @@ static int pxa930_rotary_remove(struct platform_device *pdev)
 static struct platform_driver pxa930_rotary_driver = {
 	.driver		= {
 		.name	= "pxa930-rotary",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= pxa930_rotary_probe,
 	.remove		= pxa930_rotary_remove,
diff --git a/drivers/input/keyboard/samsung-keypad.c b/drivers/input/keyboard/samsung-keypad.c
index c994e3bbd776..6b9fdf6cf8e8 100644
--- a/drivers/input/keyboard/samsung-keypad.c
+++ b/drivers/input/keyboard/samsung-keypad.c
@@ -602,7 +602,6 @@ static struct platform_driver samsung_keypad_driver = {
 	.remove		= samsung_keypad_remove,
 	.driver		= {
 		.name	= "samsung-keypad",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(samsung_keypad_dt_match),
 		.pm	= &samsung_keypad_pm_ops,
 	},
diff --git a/drivers/input/keyboard/spear-keyboard.c b/drivers/input/keyboard/spear-keyboard.c
index 258af10e5811..f42a543db043 100644
--- a/drivers/input/keyboard/spear-keyboard.c
+++ b/drivers/input/keyboard/spear-keyboard.c
@@ -385,7 +385,6 @@ static struct platform_driver spear_kbd_driver = {
 	.remove		= spear_kbd_remove,
 	.driver		= {
 		.name	= "keyboard",
-		.owner	= THIS_MODULE,
 		.pm	= &spear_kbd_pm_ops,
 		.of_match_table = of_match_ptr(spear_kbd_id_table),
 	},
diff --git a/drivers/input/keyboard/tc3589x-keypad.c b/drivers/input/keyboard/tc3589x-keypad.c
index ad7abae69078..8ff612d160b0 100644
--- a/drivers/input/keyboard/tc3589x-keypad.c
+++ b/drivers/input/keyboard/tc3589x-keypad.c
@@ -505,7 +505,6 @@ static SIMPLE_DEV_PM_OPS(tc3589x_keypad_dev_pm_ops,
 static struct platform_driver tc3589x_keypad_driver = {
 	.driver	= {
 		.name	= "tc3589x-keypad",
-		.owner	= THIS_MODULE,
 		.pm	= &tc3589x_keypad_dev_pm_ops,
 	},
 	.probe	= tc3589x_keypad_probe,
diff --git a/drivers/input/keyboard/tegra-kbc.c b/drivers/input/keyboard/tegra-kbc.c
index 9757a58bc897..f97c73bd14f8 100644
--- a/drivers/input/keyboard/tegra-kbc.c
+++ b/drivers/input/keyboard/tegra-kbc.c
@@ -822,7 +822,6 @@ static struct platform_driver tegra_kbc_driver = {
 	.probe		= tegra_kbc_probe,
 	.driver	= {
 		.name	= "tegra-kbc",
-		.owner  = THIS_MODULE,
 		.pm	= &tegra_kbc_pm_ops,
 		.of_match_table = tegra_kbc_of_match,
 	},
diff --git a/drivers/input/keyboard/twl4030_keypad.c b/drivers/input/keyboard/twl4030_keypad.c
index c5a11700a1bf..bbcccd67247d 100644
--- a/drivers/input/keyboard/twl4030_keypad.c
+++ b/drivers/input/keyboard/twl4030_keypad.c
@@ -459,7 +459,6 @@ static struct platform_driver twl4030_kp_driver = {
 	.probe		= twl4030_kp_probe,
 	.driver		= {
 		.name	= "twl4030_keypad",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(twl4030_keypad_dt_match_table),
 	},
 };
diff --git a/drivers/input/keyboard/w90p910_keypad.c b/drivers/input/keyboard/w90p910_keypad.c
index e8b9d94daae7..a1ff69c53102 100644
--- a/drivers/input/keyboard/w90p910_keypad.c
+++ b/drivers/input/keyboard/w90p910_keypad.c
@@ -258,7 +258,6 @@ static struct platform_driver w90p910_keypad_driver = {
 	.remove		= w90p910_keypad_remove,
 	.driver		= {
 		.name	= "nuc900-kpi",
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(w90p910_keypad_driver);
diff --git a/drivers/input/misc/88pm80x_onkey.c b/drivers/input/misc/88pm80x_onkey.c
index ee43e5b7c881..cf9908f1e5d5 100644
--- a/drivers/input/misc/88pm80x_onkey.c
+++ b/drivers/input/misc/88pm80x_onkey.c
@@ -153,7 +153,6 @@ static int pm80x_onkey_remove(struct platform_device *pdev)
 static struct platform_driver pm80x_onkey_driver = {
 	.driver = {
 		   .name = "88pm80x-onkey",
-		   .owner = THIS_MODULE,
 		   .pm = &pm80x_onkey_pm_ops,
 		   },
 	.probe = pm80x_onkey_probe,
diff --git a/drivers/input/misc/88pm860x_onkey.c b/drivers/input/misc/88pm860x_onkey.c
index 220ce0fa15d9..cc87443aa2ee 100644
--- a/drivers/input/misc/88pm860x_onkey.c
+++ b/drivers/input/misc/88pm860x_onkey.c
@@ -112,8 +112,7 @@ static int pm860x_onkey_probe(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int pm860x_onkey_suspend(struct device *dev)
+static int __maybe_unused pm860x_onkey_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct pm860x_chip *chip = dev_get_drvdata(pdev->dev.parent);
@@ -122,7 +121,7 @@ static int pm860x_onkey_suspend(struct device *dev)
 		chip->wakeup_flag |= 1 << PM8607_IRQ_ONKEY;
 	return 0;
 }
-static int pm860x_onkey_resume(struct device *dev)
+static int __maybe_unused pm860x_onkey_resume(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct pm860x_chip *chip = dev_get_drvdata(pdev->dev.parent);
@@ -131,14 +130,12 @@ static int pm860x_onkey_resume(struct device *dev)
 		chip->wakeup_flag &= ~(1 << PM8607_IRQ_ONKEY);
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(pm860x_onkey_pm_ops, pm860x_onkey_suspend, pm860x_onkey_resume);
 
 static struct platform_driver pm860x_onkey_driver = {
 	.driver		= {
 		.name	= "88pm860x-onkey",
-		.owner	= THIS_MODULE,
 		.pm	= &pm860x_onkey_pm_ops,
 	},
 	.probe		= pm860x_onkey_probe,
diff --git a/drivers/input/misc/ab8500-ponkey.c b/drivers/input/misc/ab8500-ponkey.c
index 95ef7dd6442d..1f7e15ca5fbe 100644
--- a/drivers/input/misc/ab8500-ponkey.c
+++ b/drivers/input/misc/ab8500-ponkey.c
@@ -123,7 +123,6 @@ static const struct of_device_id ab8500_ponkey_match[] = {
 static struct platform_driver ab8500_ponkey_driver = {
 	.driver		= {
 		.name	= "ab8500-poweron-key",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(ab8500_ponkey_match),
 	},
 	.probe		= ab8500_ponkey_probe,
diff --git a/drivers/input/misc/ad714x-i2c.c b/drivers/input/misc/ad714x-i2c.c
index e0f522516ef5..189bdc8e91a5 100644
--- a/drivers/input/misc/ad714x-i2c.c
+++ b/drivers/input/misc/ad714x-i2c.c
@@ -13,17 +13,15 @@
 #include <linux/pm.h>
 #include "ad714x.h"
 
-#ifdef CONFIG_PM_SLEEP
-static int ad714x_i2c_suspend(struct device *dev)
+static int __maybe_unused ad714x_i2c_suspend(struct device *dev)
 {
 	return ad714x_disable(i2c_get_clientdata(to_i2c_client(dev)));
 }
 
-static int ad714x_i2c_resume(struct device *dev)
+static int __maybe_unused ad714x_i2c_resume(struct device *dev)
 {
 	return ad714x_enable(i2c_get_clientdata(to_i2c_client(dev)));
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(ad714x_i2c_pm, ad714x_i2c_suspend, ad714x_i2c_resume);
 
diff --git a/drivers/input/misc/ad714x-spi.c b/drivers/input/misc/ad714x-spi.c
index 3a90b710e309..a79e50b58bf5 100644
--- a/drivers/input/misc/ad714x-spi.c
+++ b/drivers/input/misc/ad714x-spi.c
@@ -16,17 +16,15 @@
 #define AD714x_SPI_CMD_PREFIX      0xE000   /* bits 15:11 */
 #define AD714x_SPI_READ            BIT(10)
 
-#ifdef CONFIG_PM_SLEEP
-static int ad714x_spi_suspend(struct device *dev)
+static int __maybe_unused ad714x_spi_suspend(struct device *dev)
 {
 	return ad714x_disable(spi_get_drvdata(to_spi_device(dev)));
 }
 
-static int ad714x_spi_resume(struct device *dev)
+static int __maybe_unused ad714x_spi_resume(struct device *dev)
 {
 	return ad714x_enable(spi_get_drvdata(to_spi_device(dev)));
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(ad714x_spi_pm, ad714x_spi_suspend, ad714x_spi_resume);
 
diff --git a/drivers/input/misc/adxl34x-i2c.c b/drivers/input/misc/adxl34x-i2c.c
index 416f47ddcc90..470bfd6f0830 100644
--- a/drivers/input/misc/adxl34x-i2c.c
+++ b/drivers/input/misc/adxl34x-i2c.c
@@ -105,8 +105,7 @@ static int adxl34x_i2c_remove(struct i2c_client *client)
 	return adxl34x_remove(ac);
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int adxl34x_i2c_suspend(struct device *dev)
+static int __maybe_unused adxl34x_i2c_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct adxl34x *ac = i2c_get_clientdata(client);
@@ -116,7 +115,7 @@ static int adxl34x_i2c_suspend(struct device *dev)
 	return 0;
 }
 
-static int adxl34x_i2c_resume(struct device *dev)
+static int __maybe_unused adxl34x_i2c_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct adxl34x *ac = i2c_get_clientdata(client);
@@ -125,7 +124,6 @@ static int adxl34x_i2c_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(adxl34x_i2c_pm, adxl34x_i2c_suspend,
 			 adxl34x_i2c_resume);
diff --git a/drivers/input/misc/adxl34x-spi.c b/drivers/input/misc/adxl34x-spi.c
index 76dc0679d3b1..da6e76b58dab 100644
--- a/drivers/input/misc/adxl34x-spi.c
+++ b/drivers/input/misc/adxl34x-spi.c
@@ -94,8 +94,7 @@ static int adxl34x_spi_remove(struct spi_device *spi)
 	return adxl34x_remove(ac);
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int adxl34x_spi_suspend(struct device *dev)
+static int __maybe_unused adxl34x_spi_suspend(struct device *dev)
 {
 	struct spi_device *spi = to_spi_device(dev);
 	struct adxl34x *ac = spi_get_drvdata(spi);
@@ -105,7 +104,7 @@ static int adxl34x_spi_suspend(struct device *dev)
 	return 0;
 }
 
-static int adxl34x_spi_resume(struct device *dev)
+static int __maybe_unused adxl34x_spi_resume(struct device *dev)
 {
 	struct spi_device *spi = to_spi_device(dev);
 	struct adxl34x *ac = spi_get_drvdata(spi);
@@ -114,7 +113,6 @@ static int adxl34x_spi_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(adxl34x_spi_pm, adxl34x_spi_suspend,
 			 adxl34x_spi_resume);
diff --git a/drivers/input/misc/arizona-haptics.c b/drivers/input/misc/arizona-haptics.c
index ef2e281b0a43..4dbbed74c9e4 100644
--- a/drivers/input/misc/arizona-haptics.c
+++ b/drivers/input/misc/arizona-haptics.c
@@ -225,7 +225,6 @@ static struct platform_driver arizona_haptics_driver = {
 	.remove		= arizona_haptics_remove,
 	.driver		= {
 		.name	= "arizona-haptics",
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(arizona_haptics_driver);
diff --git a/drivers/input/misc/bfin_rotary.c b/drivers/input/misc/bfin_rotary.c
index e69d9bcb37e1..3f4351579372 100644
--- a/drivers/input/misc/bfin_rotary.c
+++ b/drivers/input/misc/bfin_rotary.c
@@ -256,7 +256,6 @@ static struct platform_driver bfin_rotary_device_driver = {
 	.remove		= bfin_rotary_remove,
 	.driver		= {
 		.name	= "bfin-rotary",
-		.owner	= THIS_MODULE,
 #ifdef CONFIG_PM
 		.pm	= &bfin_rotary_pm_ops,
 #endif
diff --git a/drivers/input/misc/cobalt_btns.c b/drivers/input/misc/cobalt_btns.c
index 3e11510ff82d..fbe72afc9347 100644
--- a/drivers/input/misc/cobalt_btns.c
+++ b/drivers/input/misc/cobalt_btns.c
@@ -157,7 +157,6 @@ static struct platform_driver cobalt_buttons_driver = {
 	.remove	= cobalt_buttons_remove,
 	.driver	= {
 		.name	= "Cobalt buttons",
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(cobalt_buttons_driver);
diff --git a/drivers/input/misc/da9052_onkey.c b/drivers/input/misc/da9052_onkey.c
index 184c8f21ab59..266e07fdc182 100644
--- a/drivers/input/misc/da9052_onkey.c
+++ b/drivers/input/misc/da9052_onkey.c
@@ -149,7 +149,6 @@ static struct platform_driver da9052_onkey_driver = {
 	.remove	= da9052_onkey_remove,
 	.driver = {
 		.name	= "da9052-onkey",
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(da9052_onkey_driver);
diff --git a/drivers/input/misc/da9055_onkey.c b/drivers/input/misc/da9055_onkey.c
index 4765799fef74..3251a9693f45 100644
--- a/drivers/input/misc/da9055_onkey.c
+++ b/drivers/input/misc/da9055_onkey.c
@@ -157,7 +157,6 @@ static struct platform_driver da9055_onkey_driver = {
 	.remove	= da9055_onkey_remove,
 	.driver = {
 		.name	= "da9055-onkey",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/input/misc/dm355evm_keys.c b/drivers/input/misc/dm355evm_keys.c
index 0eba94f581df..b6b7bd4e5462 100644
--- a/drivers/input/misc/dm355evm_keys.c
+++ b/drivers/input/misc/dm355evm_keys.c
@@ -263,7 +263,6 @@ static struct platform_driver dm355evm_keys_driver = {
 	.probe		= dm355evm_keys_probe,
 	.remove		= dm355evm_keys_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "dm355evm_keys",
 	},
 };
diff --git a/drivers/input/misc/drv260x.c b/drivers/input/misc/drv260x.c
index cab87f5ce6d3..a364e109ca7c 100644
--- a/drivers/input/misc/drv260x.c
+++ b/drivers/input/misc/drv260x.c
@@ -639,8 +639,7 @@ static int drv260x_probe(struct i2c_client *client,
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int drv260x_suspend(struct device *dev)
+static int __maybe_unused drv260x_suspend(struct device *dev)
 {
 	struct drv260x_data *haptics = dev_get_drvdata(dev);
 	int ret = 0;
@@ -672,7 +671,7 @@ out:
 	return ret;
 }
 
-static int drv260x_resume(struct device *dev)
+static int __maybe_unused drv260x_resume(struct device *dev)
 {
 	struct drv260x_data *haptics = dev_get_drvdata(dev);
 	int ret = 0;
@@ -702,7 +701,6 @@ out:
 	mutex_unlock(&haptics->input_dev->mutex);
 	return ret;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(drv260x_pm_ops, drv260x_suspend, drv260x_resume);
 
diff --git a/drivers/input/misc/drv2667.c b/drivers/input/misc/drv2667.c
index 0f437581cc04..a021744e608c 100644
--- a/drivers/input/misc/drv2667.c
+++ b/drivers/input/misc/drv2667.c
@@ -406,8 +406,7 @@ static int drv2667_probe(struct i2c_client *client,
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int drv2667_suspend(struct device *dev)
+static int __maybe_unused drv2667_suspend(struct device *dev)
 {
 	struct drv2667_data *haptics = dev_get_drvdata(dev);
 	int ret = 0;
@@ -436,7 +435,7 @@ out:
 	return ret;
 }
 
-static int drv2667_resume(struct device *dev)
+static int __maybe_unused drv2667_resume(struct device *dev)
 {
 	struct drv2667_data *haptics = dev_get_drvdata(dev);
 	int ret = 0;
@@ -464,7 +463,6 @@ out:
 	mutex_unlock(&haptics->input_dev->mutex);
 	return ret;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(drv2667_pm_ops, drv2667_suspend, drv2667_resume);
 
diff --git a/drivers/input/misc/gp2ap002a00f.c b/drivers/input/misc/gp2ap002a00f.c
index de21e317da32..0ac176d66a6f 100644
--- a/drivers/input/misc/gp2ap002a00f.c
+++ b/drivers/input/misc/gp2ap002a00f.c
@@ -225,8 +225,7 @@ static int gp2a_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int gp2a_suspend(struct device *dev)
+static int __maybe_unused gp2a_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct gp2a_data *dt = i2c_get_clientdata(client);
@@ -244,7 +243,7 @@ static int gp2a_suspend(struct device *dev)
 	return retval;
 }
 
-static int gp2a_resume(struct device *dev)
+static int __maybe_unused gp2a_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct gp2a_data *dt = i2c_get_clientdata(client);
@@ -261,7 +260,6 @@ static int gp2a_resume(struct device *dev)
 
 	return retval;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(gp2a_pm, gp2a_suspend, gp2a_resume);
 
diff --git a/drivers/input/misc/gpio-beeper.c b/drivers/input/misc/gpio-beeper.c
index 8886af63eae3..4817c5f0c3e4 100644
--- a/drivers/input/misc/gpio-beeper.c
+++ b/drivers/input/misc/gpio-beeper.c
@@ -112,7 +112,6 @@ MODULE_DEVICE_TABLE(of, gpio_beeper_of_match);
 static struct platform_driver gpio_beeper_platform_driver = {
 	.driver	= {
 		.name		= BEEPER_MODNAME,
-		.owner		= THIS_MODULE,
 		.of_match_table	= of_match_ptr(gpio_beeper_of_match),
 	},
 	.probe	= gpio_beeper_probe,
diff --git a/drivers/input/misc/gpio_tilt_polled.c b/drivers/input/misc/gpio_tilt_polled.c
index 1a81d9115226..f103b99d1852 100644
--- a/drivers/input/misc/gpio_tilt_polled.c
+++ b/drivers/input/misc/gpio_tilt_polled.c
@@ -199,7 +199,6 @@ static struct platform_driver gpio_tilt_polled_driver = {
 	.remove	= gpio_tilt_polled_remove,
 	.driver	= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/input/misc/ideapad_slidebar.c b/drivers/input/misc/ideapad_slidebar.c
index edfd6239f131..b0acb878d1cf 100644
--- a/drivers/input/misc/ideapad_slidebar.c
+++ b/drivers/input/misc/ideapad_slidebar.c
@@ -272,7 +272,6 @@ static int ideapad_remove(struct platform_device *pdev)
 static struct platform_driver slidebar_drv = {
 	.driver = {
 		.name = "ideapad_slidebar",
-		.owner = THIS_MODULE,
 	},
 	.remove = ideapad_remove,
 };
diff --git a/drivers/input/misc/ims-pcu.c b/drivers/input/misc/ims-pcu.c
index afed8e2b2f94..ac1fa5f44580 100644
--- a/drivers/input/misc/ims-pcu.c
+++ b/drivers/input/misc/ims-pcu.c
@@ -1851,7 +1851,7 @@ static int ims_pcu_identify_type(struct ims_pcu *pcu, u8 *device_id)
 
 static int ims_pcu_init_application_mode(struct ims_pcu *pcu)
 {
-	static atomic_t device_no = ATOMIC_INIT(0);
+	static atomic_t device_no = ATOMIC_INIT(-1);
 
 	const struct ims_pcu_device_info *info;
 	int error;
@@ -1882,7 +1882,7 @@ static int ims_pcu_init_application_mode(struct ims_pcu *pcu)
 	}
 
 	/* Device appears to be operable, complete initialization */
-	pcu->device_no = atomic_inc_return(&device_no) - 1;
+	pcu->device_no = atomic_inc_return(&device_no);
 
 	/*
 	 * PCU-B devices, both GEN_1 and GEN_2 do not have OFN sensor
diff --git a/drivers/input/misc/ixp4xx-beeper.c b/drivers/input/misc/ixp4xx-beeper.c
index ed8e5e8449d3..1fe149f3def2 100644
--- a/drivers/input/misc/ixp4xx-beeper.c
+++ b/drivers/input/misc/ixp4xx-beeper.c
@@ -168,7 +168,6 @@ static void ixp4xx_spkr_shutdown(struct platform_device *dev)
 static struct platform_driver ixp4xx_spkr_platform_driver = {
 	.driver		= {
 		.name	= "ixp4xx-beeper",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= ixp4xx_spkr_probe,
 	.remove		= ixp4xx_spkr_remove,
diff --git a/drivers/input/misc/kxtj9.c b/drivers/input/misc/kxtj9.c
index d708478bc5b5..6e29349da537 100644
--- a/drivers/input/misc/kxtj9.c
+++ b/drivers/input/misc/kxtj9.c
@@ -615,8 +615,7 @@ static int kxtj9_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int kxtj9_suspend(struct device *dev)
+static int __maybe_unused kxtj9_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct kxtj9_data *tj9 = i2c_get_clientdata(client);
@@ -631,7 +630,7 @@ static int kxtj9_suspend(struct device *dev)
 	return 0;
 }
 
-static int kxtj9_resume(struct device *dev)
+static int __maybe_unused kxtj9_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct kxtj9_data *tj9 = i2c_get_clientdata(client);
@@ -646,7 +645,6 @@ static int kxtj9_resume(struct device *dev)
 	mutex_unlock(&input_dev->mutex);
 	return retval;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(kxtj9_pm_ops, kxtj9_suspend, kxtj9_resume);
 
diff --git a/drivers/input/misc/m68kspkr.c b/drivers/input/misc/m68kspkr.c
index def21dc84522..312d63623038 100644
--- a/drivers/input/misc/m68kspkr.c
+++ b/drivers/input/misc/m68kspkr.c
@@ -100,7 +100,6 @@ static void m68kspkr_shutdown(struct platform_device *dev)
 static struct platform_driver m68kspkr_platform_driver = {
 	.driver		= {
 		.name	= "m68kspkr",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= m68kspkr_probe,
 	.remove		= m68kspkr_remove,
diff --git a/drivers/input/misc/max77693-haptic.c b/drivers/input/misc/max77693-haptic.c
index ef6a9d650d69..39e930c10ebb 100644
--- a/drivers/input/misc/max77693-haptic.c
+++ b/drivers/input/misc/max77693-haptic.c
@@ -309,8 +309,7 @@ static int max77693_haptic_probe(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int max77693_haptic_suspend(struct device *dev)
+static int __maybe_unused max77693_haptic_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct max77693_haptic *haptic = platform_get_drvdata(pdev);
@@ -323,7 +322,7 @@ static int max77693_haptic_suspend(struct device *dev)
 	return 0;
 }
 
-static int max77693_haptic_resume(struct device *dev)
+static int __maybe_unused max77693_haptic_resume(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct max77693_haptic *haptic = platform_get_drvdata(pdev);
@@ -335,7 +334,6 @@ static int max77693_haptic_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(max77693_haptic_pm_ops,
 			 max77693_haptic_suspend, max77693_haptic_resume);
@@ -343,7 +341,6 @@ static SIMPLE_DEV_PM_OPS(max77693_haptic_pm_ops,
 static struct platform_driver max77693_haptic_driver = {
 	.driver		= {
 		.name	= "max77693-haptic",
-		.owner	= THIS_MODULE,
 		.pm	= &max77693_haptic_pm_ops,
 	},
 	.probe		= max77693_haptic_probe,
diff --git a/drivers/input/misc/max8925_onkey.c b/drivers/input/misc/max8925_onkey.c
index 3809618e6a5d..7c49b8d23894 100644
--- a/drivers/input/misc/max8925_onkey.c
+++ b/drivers/input/misc/max8925_onkey.c
@@ -133,8 +133,7 @@ static int max8925_onkey_probe(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int max8925_onkey_suspend(struct device *dev)
+static int __maybe_unused max8925_onkey_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct max8925_onkey_info *info = platform_get_drvdata(pdev);
@@ -148,7 +147,7 @@ static int max8925_onkey_suspend(struct device *dev)
 	return 0;
 }
 
-static int max8925_onkey_resume(struct device *dev)
+static int __maybe_unused max8925_onkey_resume(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct max8925_onkey_info *info = platform_get_drvdata(pdev);
@@ -161,14 +160,12 @@ static int max8925_onkey_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(max8925_onkey_pm_ops, max8925_onkey_suspend, max8925_onkey_resume);
 
 static struct platform_driver max8925_onkey_driver = {
 	.driver		= {
 		.name	= "max8925-onkey",
-		.owner	= THIS_MODULE,
 		.pm	= &max8925_onkey_pm_ops,
 	},
 	.probe		= max8925_onkey_probe,
diff --git a/drivers/input/misc/max8997_haptic.c b/drivers/input/misc/max8997_haptic.c
index a363ebbd9cc0..d0f687281339 100644
--- a/drivers/input/misc/max8997_haptic.c
+++ b/drivers/input/misc/max8997_haptic.c
@@ -378,8 +378,7 @@ static int max8997_haptic_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int max8997_haptic_suspend(struct device *dev)
+static int __maybe_unused max8997_haptic_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct max8997_haptic *chip = platform_get_drvdata(pdev);
@@ -388,7 +387,6 @@ static int max8997_haptic_suspend(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(max8997_haptic_pm_ops, max8997_haptic_suspend, NULL);
 
@@ -401,7 +399,6 @@ MODULE_DEVICE_TABLE(i2c, max8997_haptic_id);
 static struct platform_driver max8997_haptic_driver = {
 	.driver	= {
 		.name	= "max8997-haptic",
-		.owner	= THIS_MODULE,
 		.pm	= &max8997_haptic_pm_ops,
 	},
 	.probe		= max8997_haptic_probe,
diff --git a/drivers/input/misc/mc13783-pwrbutton.c b/drivers/input/misc/mc13783-pwrbutton.c
index 0df6e8d8bd03..afdf8ef25ee8 100644
--- a/drivers/input/misc/mc13783-pwrbutton.c
+++ b/drivers/input/misc/mc13783-pwrbutton.c
@@ -258,7 +258,6 @@ static struct platform_driver mc13783_pwrbutton_driver = {
 	.remove		= mc13783_pwrbutton_remove,
 	.driver		= {
 		.name	= "mc13783-pwrbutton",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/input/misc/palmas-pwrbutton.c b/drivers/input/misc/palmas-pwrbutton.c
index f505ac3a8d87..1f9b5ee92746 100644
--- a/drivers/input/misc/palmas-pwrbutton.c
+++ b/drivers/input/misc/palmas-pwrbutton.c
@@ -260,7 +260,6 @@ static int palmas_pwron_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
 /**
  * palmas_pwron_suspend() - suspend handler
  * @dev:	power button device
@@ -269,7 +268,7 @@ static int palmas_pwron_remove(struct platform_device *pdev)
  *
  * Return: 0
  */
-static int palmas_pwron_suspend(struct device *dev)
+static int __maybe_unused palmas_pwron_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct palmas_pwron *pwron = platform_get_drvdata(pdev);
@@ -290,7 +289,7 @@ static int palmas_pwron_suspend(struct device *dev)
  *
  * Return: 0
  */
-static int palmas_pwron_resume(struct device *dev)
+static int __maybe_unused palmas_pwron_resume(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct palmas_pwron *pwron = platform_get_drvdata(pdev);
@@ -300,7 +299,6 @@ static int palmas_pwron_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(palmas_pwron_pm,
 			 palmas_pwron_suspend, palmas_pwron_resume);
@@ -319,7 +317,6 @@ static struct platform_driver palmas_pwron_driver = {
 	.remove	= palmas_pwron_remove,
 	.driver	= {
 		.name	= "palmas_pwrbutton",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(of_palmas_pwr_match),
 		.pm	= &palmas_pwron_pm,
 	},
diff --git a/drivers/input/misc/pcap_keys.c b/drivers/input/misc/pcap_keys.c
index cd230365166e..3b81daf67726 100644
--- a/drivers/input/misc/pcap_keys.c
+++ b/drivers/input/misc/pcap_keys.c
@@ -121,7 +121,6 @@ static struct platform_driver pcap_keys_device_driver = {
 	.remove		= pcap_keys_remove,
 	.driver		= {
 		.name	= "pcap-keys",
-		.owner	= THIS_MODULE,
 	}
 };
 module_platform_driver(pcap_keys_device_driver);
diff --git a/drivers/input/misc/pcspkr.c b/drivers/input/misc/pcspkr.c
index 674a2cfc3c0e..72b1fc3ab910 100644
--- a/drivers/input/misc/pcspkr.c
+++ b/drivers/input/misc/pcspkr.c
@@ -125,7 +125,6 @@ static const struct dev_pm_ops pcspkr_pm_ops = {
 static struct platform_driver pcspkr_platform_driver = {
 	.driver		= {
 		.name	= "pcspkr",
-		.owner	= THIS_MODULE,
 		.pm	= &pcspkr_pm_ops,
 	},
 	.probe		= pcspkr_probe,
diff --git a/drivers/input/misc/pm8xxx-vibrator.c b/drivers/input/misc/pm8xxx-vibrator.c
index 6a915ba31bba..5113877153d7 100644
--- a/drivers/input/misc/pm8xxx-vibrator.c
+++ b/drivers/input/misc/pm8xxx-vibrator.c
@@ -199,8 +199,7 @@ static int pm8xxx_vib_probe(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int pm8xxx_vib_suspend(struct device *dev)
+static int __maybe_unused pm8xxx_vib_suspend(struct device *dev)
 {
 	struct pm8xxx_vib *vib = dev_get_drvdata(dev);
 
@@ -209,7 +208,6 @@ static int pm8xxx_vib_suspend(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(pm8xxx_vib_pm_ops, pm8xxx_vib_suspend, NULL);
 
@@ -224,7 +222,6 @@ static struct platform_driver pm8xxx_vib_driver = {
 	.probe		= pm8xxx_vib_probe,
 	.driver		= {
 		.name	= "pm8xxx-vib",
-		.owner	= THIS_MODULE,
 		.pm	= &pm8xxx_vib_pm_ops,
 		.of_match_table = pm8xxx_vib_id_table,
 	},
diff --git a/drivers/input/misc/pmic8xxx-pwrkey.c b/drivers/input/misc/pmic8xxx-pwrkey.c
index c91e3d33aea9..c4ca20e63221 100644
--- a/drivers/input/misc/pmic8xxx-pwrkey.c
+++ b/drivers/input/misc/pmic8xxx-pwrkey.c
@@ -53,8 +53,7 @@ static irqreturn_t pwrkey_release_irq(int irq, void *_pwr)
 	return IRQ_HANDLED;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int pmic8xxx_pwrkey_suspend(struct device *dev)
+static int __maybe_unused pmic8xxx_pwrkey_suspend(struct device *dev)
 {
 	struct pmic8xxx_pwrkey *pwrkey = dev_get_drvdata(dev);
 
@@ -64,7 +63,7 @@ static int pmic8xxx_pwrkey_suspend(struct device *dev)
 	return 0;
 }
 
-static int pmic8xxx_pwrkey_resume(struct device *dev)
+static int __maybe_unused pmic8xxx_pwrkey_resume(struct device *dev)
 {
 	struct pmic8xxx_pwrkey *pwrkey = dev_get_drvdata(dev);
 
@@ -73,7 +72,6 @@ static int pmic8xxx_pwrkey_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(pm8xxx_pwr_key_pm_ops,
 		pmic8xxx_pwrkey_suspend, pmic8xxx_pwrkey_resume);
@@ -195,7 +193,6 @@ static struct platform_driver pmic8xxx_pwrkey_driver = {
 	.remove		= pmic8xxx_pwrkey_remove,
 	.driver		= {
 		.name	= "pm8xxx-pwrkey",
-		.owner	= THIS_MODULE,
 		.pm	= &pm8xxx_pwr_key_pm_ops,
 		.of_match_table = pm8xxx_pwr_key_id_table,
 	},
diff --git a/drivers/input/misc/pwm-beeper.c b/drivers/input/misc/pwm-beeper.c
index 8ef288e7c971..a28ee70ff158 100644
--- a/drivers/input/misc/pwm-beeper.c
+++ b/drivers/input/misc/pwm-beeper.c
@@ -144,8 +144,7 @@ static int pwm_beeper_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int pwm_beeper_suspend(struct device *dev)
+static int __maybe_unused pwm_beeper_suspend(struct device *dev)
 {
 	struct pwm_beeper *beeper = dev_get_drvdata(dev);
 
@@ -155,7 +154,7 @@ static int pwm_beeper_suspend(struct device *dev)
 	return 0;
 }
 
-static int pwm_beeper_resume(struct device *dev)
+static int __maybe_unused pwm_beeper_resume(struct device *dev)
 {
 	struct pwm_beeper *beeper = dev_get_drvdata(dev);
 
@@ -170,6 +169,7 @@ static int pwm_beeper_resume(struct device *dev)
 static SIMPLE_DEV_PM_OPS(pwm_beeper_pm_ops,
 			 pwm_beeper_suspend, pwm_beeper_resume);
 
+#ifdef CONFIG_PM_SLEEP
 #define PWM_BEEPER_PM_OPS (&pwm_beeper_pm_ops)
 #else
 #define PWM_BEEPER_PM_OPS NULL
@@ -187,7 +187,6 @@ static struct platform_driver pwm_beeper_driver = {
 	.remove = pwm_beeper_remove,
 	.driver = {
 		.name	= "pwm-beeper",
-		.owner	= THIS_MODULE,
 		.pm	= PWM_BEEPER_PM_OPS,
 		.of_match_table = of_match_ptr(pwm_beeper_match),
 	},
diff --git a/drivers/input/misc/rb532_button.c b/drivers/input/misc/rb532_button.c
index 83fff38b86b3..e956e81cd4e6 100644
--- a/drivers/input/misc/rb532_button.c
+++ b/drivers/input/misc/rb532_button.c
@@ -96,7 +96,6 @@ static struct platform_driver rb532_button_driver = {
 	.remove = rb532_button_remove,
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 	},
 };
 module_platform_driver(rb532_button_driver);
diff --git a/drivers/input/misc/retu-pwrbutton.c b/drivers/input/misc/retu-pwrbutton.c
index 4bff1aa9b0db..0c8ac60e2639 100644
--- a/drivers/input/misc/retu-pwrbutton.c
+++ b/drivers/input/misc/retu-pwrbutton.c
@@ -85,7 +85,6 @@ static struct platform_driver retu_pwrbutton_driver = {
 	.remove		= retu_pwrbutton_remove,
 	.driver		= {
 		.name	= "retu-pwrbutton",
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(retu_pwrbutton_driver);
diff --git a/drivers/input/misc/rotary_encoder.c b/drivers/input/misc/rotary_encoder.c
index 93558a1c7f70..f27f81ee84ed 100644
--- a/drivers/input/misc/rotary_encoder.c
+++ b/drivers/input/misc/rotary_encoder.c
@@ -325,7 +325,6 @@ static struct platform_driver rotary_encoder_driver = {
 	.remove		= rotary_encoder_remove,
 	.driver		= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(rotary_encoder_of_match),
 	}
 };
diff --git a/drivers/input/misc/sgi_btns.c b/drivers/input/misc/sgi_btns.c
index f10474937a64..7bbe79d89f5c 100644
--- a/drivers/input/misc/sgi_btns.c
+++ b/drivers/input/misc/sgi_btns.c
@@ -157,7 +157,6 @@ static struct platform_driver sgi_buttons_driver = {
 	.remove	= sgi_buttons_remove,
 	.driver	= {
 		.name	= "sgibtns",
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(sgi_buttons_driver);
diff --git a/drivers/input/misc/sirfsoc-onkey.c b/drivers/input/misc/sirfsoc-onkey.c
index fed5102e1802..9d5b89befe6f 100644
--- a/drivers/input/misc/sirfsoc-onkey.c
+++ b/drivers/input/misc/sirfsoc-onkey.c
@@ -179,8 +179,7 @@ static int sirfsoc_pwrc_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int sirfsoc_pwrc_resume(struct device *dev)
+static int __maybe_unused sirfsoc_pwrc_resume(struct device *dev)
 {
 	struct sirfsoc_pwrc_drvdata *pwrcdrv = dev_get_drvdata(dev);
 	struct input_dev *input = pwrcdrv->input;
@@ -196,7 +195,6 @@ static int sirfsoc_pwrc_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(sirfsoc_pwrc_pm_ops, NULL, sirfsoc_pwrc_resume);
 
@@ -205,7 +203,6 @@ static struct platform_driver sirfsoc_pwrc_driver = {
 	.remove		= sirfsoc_pwrc_remove,
 	.driver		= {
 		.name	= "sirfsoc-pwrc",
-		.owner	= THIS_MODULE,
 		.pm	= &sirfsoc_pwrc_pm_ops,
 		.of_match_table = sirfsoc_pwrc_of_match,
 	}
diff --git a/drivers/input/misc/soc_button_array.c b/drivers/input/misc/soc_button_array.c
index e097f1ab427f..79cc0f79896f 100644
--- a/drivers/input/misc/soc_button_array.c
+++ b/drivers/input/misc/soc_button_array.c
@@ -214,7 +214,6 @@ static struct platform_driver soc_button_driver = {
 	.remove		= soc_button_remove,
 	.driver		= {
 		.name = KBUILD_MODNAME,
-		.owner = THIS_MODULE,
 		.acpi_match_table = ACPI_PTR(soc_button_acpi_match),
 	},
 };
diff --git a/drivers/input/misc/sparcspkr.c b/drivers/input/misc/sparcspkr.c
index 179ff1cd6f6b..54116e544c96 100644
--- a/drivers/input/misc/sparcspkr.c
+++ b/drivers/input/misc/sparcspkr.c
@@ -257,7 +257,6 @@ static const struct of_device_id bbc_beep_match[] = {
 static struct platform_driver bbc_beep_driver = {
 	.driver = {
 		.name = "bbcbeep",
-		.owner = THIS_MODULE,
 		.of_match_table = bbc_beep_match,
 	},
 	.probe		= bbc_beep_probe,
@@ -337,7 +336,6 @@ static const struct of_device_id grover_beep_match[] = {
 static struct platform_driver grover_beep_driver = {
 	.driver = {
 		.name = "groverbeep",
-		.owner = THIS_MODULE,
 		.of_match_table = grover_beep_match,
 	},
 	.probe		= grover_beep_probe,
diff --git a/drivers/input/misc/twl4030-pwrbutton.c b/drivers/input/misc/twl4030-pwrbutton.c
index 8400a1a34d87..e98cc81a84c6 100644
--- a/drivers/input/misc/twl4030-pwrbutton.c
+++ b/drivers/input/misc/twl4030-pwrbutton.c
@@ -102,7 +102,6 @@ static struct platform_driver twl4030_pwrbutton_driver = {
 	.probe		= twl4030_pwrbutton_probe,
 	.driver		= {
 		.name	= "twl4030_pwrbutton",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(twl4030_pwrbutton_dt_match_table),
 	},
 };
diff --git a/drivers/input/misc/twl4030-vibra.c b/drivers/input/misc/twl4030-vibra.c
index 960ef2a70910..fc17b9592f54 100644
--- a/drivers/input/misc/twl4030-vibra.c
+++ b/drivers/input/misc/twl4030-vibra.c
@@ -157,8 +157,7 @@ static void twl4030_vibra_close(struct input_dev *input)
 }
 
 /*** Module ***/
-#ifdef CONFIG_PM_SLEEP
-static int twl4030_vibra_suspend(struct device *dev)
+static int __maybe_unused twl4030_vibra_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct vibra_info *info = platform_get_drvdata(pdev);
@@ -169,12 +168,11 @@ static int twl4030_vibra_suspend(struct device *dev)
 	return 0;
 }
 
-static int twl4030_vibra_resume(struct device *dev)
+static int __maybe_unused twl4030_vibra_resume(struct device *dev)
 {
 	vibra_disable_leds();
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(twl4030_vibra_pm_ops,
 			 twl4030_vibra_suspend, twl4030_vibra_resume);
@@ -253,7 +251,6 @@ static struct platform_driver twl4030_vibra_driver = {
 	.probe		= twl4030_vibra_probe,
 	.driver		= {
 		.name	= "twl4030-vibra",
-		.owner	= THIS_MODULE,
 		.pm	= &twl4030_vibra_pm_ops,
 	},
 };
diff --git a/drivers/input/misc/twl6040-vibra.c b/drivers/input/misc/twl6040-vibra.c
index 6d26eecc278c..0e0d094df2e6 100644
--- a/drivers/input/misc/twl6040-vibra.c
+++ b/drivers/input/misc/twl6040-vibra.c
@@ -236,8 +236,7 @@ static void twl6040_vibra_close(struct input_dev *input)
 	mutex_unlock(&info->mutex);
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int twl6040_vibra_suspend(struct device *dev)
+static int __maybe_unused twl6040_vibra_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct vibra_info *info = platform_get_drvdata(pdev);
@@ -251,7 +250,6 @@ static int twl6040_vibra_suspend(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(twl6040_vibra_pm_ops, twl6040_vibra_suspend, NULL);
 
@@ -388,7 +386,6 @@ static struct platform_driver twl6040_vibra_driver = {
 	.probe		= twl6040_vibra_probe,
 	.driver		= {
 		.name	= "twl6040-vibra",
-		.owner	= THIS_MODULE,
 		.pm	= &twl6040_vibra_pm_ops,
 	},
 };
diff --git a/drivers/input/misc/wistron_btns.c b/drivers/input/misc/wistron_btns.c
index 7b7add5061a5..e25f87ba19f6 100644
--- a/drivers/input/misc/wistron_btns.c
+++ b/drivers/input/misc/wistron_btns.c
@@ -1347,7 +1347,6 @@ static const struct dev_pm_ops wistron_pm_ops = {
 static struct platform_driver wistron_driver = {
 	.driver		= {
 		.name	= "wistron-bios",
-		.owner	= THIS_MODULE,
 #ifdef CONFIG_PM
 		.pm	= &wistron_pm_ops,
 #endif
diff --git a/drivers/input/misc/wm831x-on.c b/drivers/input/misc/wm831x-on.c
index 173b6dcca0da..59d4f7bcb4a3 100644
--- a/drivers/input/misc/wm831x-on.c
+++ b/drivers/input/misc/wm831x-on.c
@@ -138,7 +138,6 @@ static struct platform_driver wm831x_on_driver = {
 	.remove		= wm831x_on_remove,
 	.driver		= {
 		.name	= "wm831x-on",
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(wm831x_on_driver);
diff --git a/drivers/input/mouse/Kconfig b/drivers/input/mouse/Kconfig
index 366fc7ad5eb6..d8b46b0f2dbe 100644
--- a/drivers/input/mouse/Kconfig
+++ b/drivers/input/mouse/Kconfig
@@ -215,6 +215,36 @@ config MOUSE_CYAPA
 	  To compile this driver as a module, choose M here: the module will be
 	  called cyapa.
 
+config MOUSE_ELAN_I2C
+	tristate "ELAN I2C Touchpad support"
+	depends on I2C
+	help
+	  This driver adds support for Elan I2C/SMbus Trackpads.
+
+	  Say Y here if you have a ELAN I2C/SMbus Touchpad.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called elan_i2c.
+
+config MOUSE_ELAN_I2C_I2C
+	bool "Enable I2C support"
+	depends on MOUSE_ELAN_I2C
+	default y
+	help
+	   Say Y here if Elan Touchpad in your system is connected to
+	   a standard I2C controller.
+
+	   If unsure, say Y.
+
+config MOUSE_ELAN_I2C_SMBUS
+	bool "Enable SMbus support"
+	depends on MOUSE_ELAN_I2C
+	help
+	   Say Y here if Elan Touchpad in your system is connected to
+	   a SMbus adapter.
+
+	   If unsure, say Y.
+
 config MOUSE_INPORT
 	tristate "InPort/MS/ATIXL busmouse"
 	depends on ISA
diff --git a/drivers/input/mouse/Makefile b/drivers/input/mouse/Makefile
index dda507f8b3a2..560003dcac37 100644
--- a/drivers/input/mouse/Makefile
+++ b/drivers/input/mouse/Makefile
@@ -9,6 +9,7 @@ obj-$(CONFIG_MOUSE_APPLETOUCH)		+= appletouch.o
 obj-$(CONFIG_MOUSE_ATARI)		+= atarimouse.o
 obj-$(CONFIG_MOUSE_BCM5974)		+= bcm5974.o
 obj-$(CONFIG_MOUSE_CYAPA)		+= cyapa.o
+obj-$(CONFIG_MOUSE_ELAN_I2C)		+= elan_i2c.o
 obj-$(CONFIG_MOUSE_GPIO)		+= gpio_mouse.o
 obj-$(CONFIG_MOUSE_INPORT)		+= inport.o
 obj-$(CONFIG_MOUSE_LOGIBM)		+= logibm.o
@@ -34,3 +35,7 @@ psmouse-$(CONFIG_MOUSE_PS2_SENTELIC)	+= sentelic.o
 psmouse-$(CONFIG_MOUSE_PS2_TRACKPOINT)	+= trackpoint.o
 psmouse-$(CONFIG_MOUSE_PS2_TOUCHKIT)	+= touchkit_ps2.o
 psmouse-$(CONFIG_MOUSE_PS2_CYPRESS)	+= cypress_ps2.o
+
+elan_i2c-objs := elan_i2c_core.o
+elan_i2c-$(CONFIG_MOUSE_ELAN_I2C_I2C)	+= elan_i2c_i2c.o
+elan_i2c-$(CONFIG_MOUSE_ELAN_I2C_SMBUS)	+= elan_i2c_smbus.o
diff --git a/drivers/input/mouse/amimouse.c b/drivers/input/mouse/amimouse.c
index 62ec52b2e347..a7fd8f22ba56 100644
--- a/drivers/input/mouse/amimouse.c
+++ b/drivers/input/mouse/amimouse.c
@@ -141,7 +141,6 @@ static struct platform_driver amimouse_driver = {
 	.remove = __exit_p(amimouse_remove),
 	.driver   = {
 		.name	= "amiga-mouse",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/input/mouse/cyapa.c b/drivers/input/mouse/cyapa.c
index b409c3d7d4fb..1bece8cad46f 100644
--- a/drivers/input/mouse/cyapa.c
+++ b/drivers/input/mouse/cyapa.c
@@ -6,7 +6,7 @@
  *   Daniel Kurtz <djkurtz@chromium.org>
  *   Benson Leung <bleung@chromium.org>
  *
- * Copyright (C) 2011-2012 Cypress Semiconductor, Inc.
+ * Copyright (C) 2011-2014 Cypress Semiconductor, Inc.
  * Copyright (C) 2011-2012 Google, Inc.
  *
  * This file is subject to the terms and conditions of the GNU General Public
@@ -206,7 +206,6 @@ struct cyapa {
 	struct i2c_client *client;
 	struct input_dev *input;
 	char phys[32];	/* device physical location */
-	int irq;
 	bool irq_wake;  /* irq wake is enabled */
 	bool smbus;
 
@@ -422,8 +421,8 @@ static ssize_t cyapa_read_block(struct cyapa *cyapa, u8 cmd_idx, u8 *values)
  */
 static int cyapa_get_state(struct cyapa *cyapa)
 {
-	int ret;
 	u8 status[BL_STATUS_SIZE];
+	int error;
 
 	cyapa->state = CYAPA_STATE_NO_DEVICE;
 
@@ -433,18 +432,18 @@ static int cyapa_get_state(struct cyapa *cyapa)
 	 * If the device is in operation mode, this will be the DATA regs.
 	 *
 	 */
-	ret = cyapa_i2c_reg_read_block(cyapa, BL_HEAD_OFFSET, BL_STATUS_SIZE,
-				       status);
+	error = cyapa_i2c_reg_read_block(cyapa, BL_HEAD_OFFSET, BL_STATUS_SIZE,
+				         status);
 
 	/*
 	 * On smbus systems in OP mode, the i2c_reg_read will fail with
 	 * -ETIMEDOUT.  In this case, try again using the smbus equivalent
 	 * command.  This should return a BL_HEAD indicating CYAPA_STATE_OP.
 	 */
-	if (cyapa->smbus && (ret == -ETIMEDOUT || ret == -ENXIO))
-		ret = cyapa_read_block(cyapa, CYAPA_CMD_BL_STATUS, status);
+	if (cyapa->smbus && (error == -ETIMEDOUT || error == -ENXIO))
+		error = cyapa_read_block(cyapa, CYAPA_CMD_BL_STATUS, status);
 
-	if (ret != BL_STATUS_SIZE)
+	if (error != BL_STATUS_SIZE)
 		goto error;
 
 	if ((status[REG_OP_STATUS] & OP_STATUS_SRC) == OP_STATUS_SRC) {
@@ -454,7 +453,7 @@ static int cyapa_get_state(struct cyapa *cyapa)
 			cyapa->state = CYAPA_STATE_OP;
 			break;
 		default:
-			ret = -EAGAIN;
+			error = -EAGAIN;
 			goto error;
 		}
 	} else {
@@ -468,7 +467,7 @@ static int cyapa_get_state(struct cyapa *cyapa)
 
 	return 0;
 error:
-	return (ret < 0) ? ret : -EAGAIN;
+	return (error < 0) ? error : -EAGAIN;
 }
 
 /*
@@ -487,31 +486,31 @@ error:
  */
 static int cyapa_poll_state(struct cyapa *cyapa, unsigned int timeout)
 {
-	int ret;
+	int error;
 	int tries = timeout / 100;
 
-	ret = cyapa_get_state(cyapa);
-	while ((ret || cyapa->state >= CYAPA_STATE_BL_BUSY) && tries--) {
+	error = cyapa_get_state(cyapa);
+	while ((error || cyapa->state >= CYAPA_STATE_BL_BUSY) && tries--) {
 		msleep(100);
-		ret = cyapa_get_state(cyapa);
+		error = cyapa_get_state(cyapa);
 	}
-	return (ret == -EAGAIN || ret == -ETIMEDOUT) ? -ETIMEDOUT : ret;
+	return (error == -EAGAIN || error == -ETIMEDOUT) ? -ETIMEDOUT : error;
 }
 
 static int cyapa_bl_deactivate(struct cyapa *cyapa)
 {
-	int ret;
+	int error;
 
-	ret = cyapa_i2c_reg_write_block(cyapa, 0, sizeof(bl_deactivate),
-					bl_deactivate);
-	if (ret < 0)
-		return ret;
+	error = cyapa_i2c_reg_write_block(cyapa, 0, sizeof(bl_deactivate),
+					  bl_deactivate);
+	if (error)
+		return error;
 
 	/* wait for bootloader to switch to idle state; should take < 100ms */
 	msleep(100);
-	ret = cyapa_poll_state(cyapa, 500);
-	if (ret < 0)
-		return ret;
+	error = cyapa_poll_state(cyapa, 500);
+	if (error)
+		return error;
 	if (cyapa->state != CYAPA_STATE_BL_IDLE)
 		return -EAGAIN;
 	return 0;
@@ -532,11 +531,11 @@ static int cyapa_bl_deactivate(struct cyapa *cyapa)
  */
 static int cyapa_bl_exit(struct cyapa *cyapa)
 {
-	int ret;
+	int error;
 
-	ret = cyapa_i2c_reg_write_block(cyapa, 0, sizeof(bl_exit), bl_exit);
-	if (ret < 0)
-		return ret;
+	error = cyapa_i2c_reg_write_block(cyapa, 0, sizeof(bl_exit), bl_exit);
+	if (error)
+		return error;
 
 	/*
 	 * Wait for bootloader to exit, and operation mode to start.
@@ -548,9 +547,9 @@ static int cyapa_bl_exit(struct cyapa *cyapa)
 	 * updated to new firmware, it must first calibrate its sensors, which
 	 * can take up to an additional 2 seconds.
 	 */
-	ret = cyapa_poll_state(cyapa, 2000);
-	if (ret < 0)
-		return ret;
+	error = cyapa_poll_state(cyapa, 2000);
+	if (error < 0)
+		return error;
 	if (cyapa->state != CYAPA_STATE_OP)
 		return -EAGAIN;
 
@@ -577,10 +576,13 @@ static int cyapa_set_power_mode(struct cyapa *cyapa, u8 power_mode)
 	power = ret & ~PWR_MODE_MASK;
 	power |= power_mode & PWR_MODE_MASK;
 	ret = cyapa_write_byte(cyapa, CYAPA_CMD_POWER_MODE, power);
-	if (ret < 0)
+	if (ret < 0) {
 		dev_err(dev, "failed to set power_mode 0x%02x err = %d\n",
 			power_mode, ret);
-	return ret;
+		return ret;
+	}
+
+	return 0;
 }
 
 static int cyapa_get_query_data(struct cyapa *cyapa)
@@ -637,28 +639,28 @@ static int cyapa_check_is_operational(struct cyapa *cyapa)
 {
 	struct device *dev = &cyapa->client->dev;
 	static const char unique_str[] = "CYTRA";
-	int ret;
+	int error;
 
-	ret = cyapa_poll_state(cyapa, 2000);
-	if (ret < 0)
-		return ret;
+	error = cyapa_poll_state(cyapa, 2000);
+	if (error)
+		return error;
 	switch (cyapa->state) {
 	case CYAPA_STATE_BL_ACTIVE:
-		ret = cyapa_bl_deactivate(cyapa);
-		if (ret)
-			return ret;
+		error = cyapa_bl_deactivate(cyapa);
+		if (error)
+			return error;
 
 	/* Fallthrough state */
 	case CYAPA_STATE_BL_IDLE:
-		ret = cyapa_bl_exit(cyapa);
-		if (ret)
-			return ret;
+		error = cyapa_bl_exit(cyapa);
+		if (error)
+			return error;
 
 	/* Fallthrough state */
 	case CYAPA_STATE_OP:
-		ret = cyapa_get_query_data(cyapa);
-		if (ret < 0)
-			return ret;
+		error = cyapa_get_query_data(cyapa);
+		if (error)
+			return error;
 
 		/* only support firmware protocol gen3 */
 		if (cyapa->gen != CYAPA_GEN3) {
@@ -753,18 +755,42 @@ static u8 cyapa_check_adapter_functionality(struct i2c_client *client)
 	return ret;
 }
 
+static int cyapa_open(struct input_dev *input)
+{
+	struct cyapa *cyapa = input_get_drvdata(input);
+	struct i2c_client *client = cyapa->client;
+	int error;
+
+	error = cyapa_set_power_mode(cyapa, PWR_MODE_FULL_ACTIVE);
+	if (error) {
+		dev_err(&client->dev, "set active power failed: %d\n", error);
+		return error;
+	}
+
+	enable_irq(client->irq);
+	return 0;
+}
+
+static void cyapa_close(struct input_dev *input)
+{
+	struct cyapa *cyapa = input_get_drvdata(input);
+
+	disable_irq(cyapa->client->irq);
+	cyapa_set_power_mode(cyapa, PWR_MODE_OFF);
+}
+
 static int cyapa_create_input_dev(struct cyapa *cyapa)
 {
 	struct device *dev = &cyapa->client->dev;
-	int ret;
 	struct input_dev *input;
+	int error;
 
 	if (!cyapa->physical_size_x || !cyapa->physical_size_y)
 		return -EINVAL;
 
-	input = cyapa->input = input_allocate_device();
+	input = devm_input_allocate_device(dev);
 	if (!input) {
-		dev_err(dev, "allocate memory for input device failed\n");
+		dev_err(dev, "failed to allocate memory for input device.\n");
 		return -ENOMEM;
 	}
 
@@ -772,14 +798,17 @@ static int cyapa_create_input_dev(struct cyapa *cyapa)
 	input->phys = cyapa->phys;
 	input->id.bustype = BUS_I2C;
 	input->id.version = 1;
-	input->id.product = 0;  /* means any product in eventcomm. */
+	input->id.product = 0;  /* Means any product in eventcomm. */
 	input->dev.parent = &cyapa->client->dev;
 
+	input->open = cyapa_open;
+	input->close = cyapa_close;
+
 	input_set_drvdata(input, cyapa);
 
 	__set_bit(EV_ABS, input->evbit);
 
-	/* finger position */
+	/* Finger position */
 	input_set_abs_params(input, ABS_MT_POSITION_X, 0, cyapa->max_abs_x, 0,
 			     0);
 	input_set_abs_params(input, ABS_MT_POSITION_Y, 0, cyapa->max_abs_y, 0,
@@ -801,35 +830,25 @@ static int cyapa_create_input_dev(struct cyapa *cyapa)
 	if (cyapa->btn_capability == CAPABILITY_LEFT_BTN_MASK)
 		__set_bit(INPUT_PROP_BUTTONPAD, input->propbit);
 
-	/* handle pointer emulation and unused slots in core */
-	ret = input_mt_init_slots(input, CYAPA_MAX_MT_SLOTS,
-				  INPUT_MT_POINTER | INPUT_MT_DROP_UNUSED);
-	if (ret) {
-		dev_err(dev, "allocate memory for MT slots failed, %d\n", ret);
-		goto err_free_device;
+	/* Handle pointer emulation and unused slots in core */
+	error = input_mt_init_slots(input, CYAPA_MAX_MT_SLOTS,
+				    INPUT_MT_POINTER | INPUT_MT_DROP_UNUSED);
+	if (error) {
+		dev_err(dev, "failed to initialize MT slots: %d\n", error);
+		return error;
 	}
 
-	/* Register the device in input subsystem */
-	ret = input_register_device(input);
-	if (ret) {
-		dev_err(dev, "input device register failed, %d\n", ret);
-		goto err_free_device;
-	}
+	cyapa->input = input;
 	return 0;
-
-err_free_device:
-	input_free_device(input);
-	cyapa->input = NULL;
-	return ret;
 }
 
 static int cyapa_probe(struct i2c_client *client,
 		       const struct i2c_device_id *dev_id)
 {
-	int ret;
-	u8 adapter_func;
-	struct cyapa *cyapa;
 	struct device *dev = &client->dev;
+	struct cyapa *cyapa;
+	u8 adapter_func;
+	int error;
 
 	adapter_func = cyapa_check_adapter_functionality(client);
 	if (adapter_func == CYAPA_ADAPTER_FUNC_NONE) {
@@ -837,11 +856,9 @@ static int cyapa_probe(struct i2c_client *client,
 		return -EIO;
 	}
 
-	cyapa = kzalloc(sizeof(struct cyapa), GFP_KERNEL);
-	if (!cyapa) {
-		dev_err(dev, "allocate memory for cyapa failed\n");
+	cyapa = devm_kzalloc(dev, sizeof(struct cyapa), GFP_KERNEL);
+	if (!cyapa)
 		return -ENOMEM;
-	}
 
 	cyapa->gen = CYAPA_GEN3;
 	cyapa->client = client;
@@ -852,67 +869,61 @@ static int cyapa_probe(struct i2c_client *client,
 	/* i2c isn't supported, use smbus */
 	if (adapter_func == CYAPA_ADAPTER_FUNC_SMBUS)
 		cyapa->smbus = true;
+
 	cyapa->state = CYAPA_STATE_NO_DEVICE;
-	ret = cyapa_check_is_operational(cyapa);
-	if (ret) {
-		dev_err(dev, "device not operational, %d\n", ret);
-		goto err_mem_free;
-	}
 
-	ret = cyapa_create_input_dev(cyapa);
-	if (ret) {
-		dev_err(dev, "create input_dev instance failed, %d\n", ret);
-		goto err_mem_free;
+	error = cyapa_check_is_operational(cyapa);
+	if (error) {
+		dev_err(dev, "device not operational, %d\n", error);
+		return error;
 	}
 
-	ret = cyapa_set_power_mode(cyapa, PWR_MODE_FULL_ACTIVE);
-	if (ret) {
-		dev_err(dev, "set active power failed, %d\n", ret);
-		goto err_unregister_device;
+	/* Power down the device until we need it */
+	error = cyapa_set_power_mode(cyapa, PWR_MODE_OFF);
+	if (error) {
+		dev_err(dev, "failed to quiesce the device: %d\n", error);
+		return error;
 	}
 
-	cyapa->irq = client->irq;
-	ret = request_threaded_irq(cyapa->irq,
-				   NULL,
-				   cyapa_irq,
-				   IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
-				   "cyapa",
-				   cyapa);
-	if (ret) {
-		dev_err(dev, "IRQ request failed: %d\n, ", ret);
-		goto err_unregister_device;
+	error = cyapa_create_input_dev(cyapa);
+	if (error)
+		return error;
+
+	error = devm_request_threaded_irq(dev, client->irq,
+					  NULL, cyapa_irq,
+					  IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+					  "cyapa", cyapa);
+	if (error) {
+		dev_err(dev, "failed to request threaded irq: %d\n", error);
+		return error;
 	}
 
-	return 0;
+	/* Disable IRQ until the device is opened */
+	disable_irq(client->irq);
 
-err_unregister_device:
-	input_unregister_device(cyapa->input);
-err_mem_free:
-	kfree(cyapa);
-
-	return ret;
-}
-
-static int cyapa_remove(struct i2c_client *client)
-{
-	struct cyapa *cyapa = i2c_get_clientdata(client);
-
-	free_irq(cyapa->irq, cyapa);
-	input_unregister_device(cyapa->input);
-	cyapa_set_power_mode(cyapa, PWR_MODE_OFF);
-	kfree(cyapa);
+	/* Register the device in input subsystem */
+	error = input_register_device(cyapa->input);
+	if (error) {
+		dev_err(dev, "failed to register input device: %d\n", error);
+		return error;
+	}
 
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int cyapa_suspend(struct device *dev)
+static int __maybe_unused cyapa_suspend(struct device *dev)
 {
-	int ret;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct cyapa *cyapa = i2c_get_clientdata(client);
+	struct input_dev *input = cyapa->input;
 	u8 power_mode;
-	struct cyapa *cyapa = dev_get_drvdata(dev);
+	int error;
 
-	disable_irq(cyapa->irq);
+	error = mutex_lock_interruptible(&input->mutex);
+	if (error)
+		return error;
+
+	disable_irq(client->irq);
 
 	/*
 	 * Set trackpad device to idle mode if wakeup is allowed,
@@ -920,31 +931,44 @@ static int cyapa_suspend(struct device *dev)
 	 */
 	power_mode = device_may_wakeup(dev) ? PWR_MODE_IDLE
 					    : PWR_MODE_OFF;
-	ret = cyapa_set_power_mode(cyapa, power_mode);
-	if (ret < 0)
-		dev_err(dev, "set power mode failed, %d\n", ret);
+	error = cyapa_set_power_mode(cyapa, power_mode);
+	if (error)
+		dev_err(dev, "resume: set power mode to %d failed: %d\n",
+			 power_mode, error);
 
 	if (device_may_wakeup(dev))
-		cyapa->irq_wake = (enable_irq_wake(cyapa->irq) == 0);
+		cyapa->irq_wake = (enable_irq_wake(client->irq) == 0);
+
+	mutex_unlock(&input->mutex);
+
 	return 0;
 }
 
-static int cyapa_resume(struct device *dev)
+static int __maybe_unused cyapa_resume(struct device *dev)
 {
-	int ret;
-	struct cyapa *cyapa = dev_get_drvdata(dev);
+	struct i2c_client *client = to_i2c_client(dev);
+	struct cyapa *cyapa = i2c_get_clientdata(client);
+	struct input_dev *input = cyapa->input;
+	u8 power_mode;
+	int error;
+
+	mutex_lock(&input->mutex);
 
 	if (device_may_wakeup(dev) && cyapa->irq_wake)
-		disable_irq_wake(cyapa->irq);
+		disable_irq_wake(client->irq);
+
+	power_mode = input->users ? PWR_MODE_FULL_ACTIVE : PWR_MODE_OFF;
+	error = cyapa_set_power_mode(cyapa, PWR_MODE_FULL_ACTIVE);
+	if (error)
+		dev_warn(dev, "resume: set power mode to %d failed: %d\n",
+			 power_mode, error);
+
+	enable_irq(client->irq);
 
-	ret = cyapa_set_power_mode(cyapa, PWR_MODE_FULL_ACTIVE);
-	if (ret)
-		dev_warn(dev, "resume active power failed, %d\n", ret);
+	mutex_unlock(&input->mutex);
 
-	enable_irq(cyapa->irq);
 	return 0;
 }
-#endif /* CONFIG_PM_SLEEP */
 
 static SIMPLE_DEV_PM_OPS(cyapa_pm_ops, cyapa_suspend, cyapa_resume);
 
@@ -962,7 +986,6 @@ static struct i2c_driver cyapa_driver = {
 	},
 
 	.probe = cyapa_probe,
-	.remove = cyapa_remove,
 	.id_table = cyapa_id_table,
 };
 
diff --git a/drivers/input/mouse/elan_i2c.h b/drivers/input/mouse/elan_i2c.h
new file mode 100644
index 000000000000..2e838626205f
--- /dev/null
+++ b/drivers/input/mouse/elan_i2c.h
@@ -0,0 +1,86 @@
+/*
+ * Elan I2C/SMBus Touchpad driver
+ *
+ * Copyright (c) 2013 ELAN Microelectronics Corp.
+ *
+ * Author: 林政維 (Duson Lin) <dusonlin@emc.com.tw>
+ * Version: 1.5.5
+ *
+ * Based on cyapa driver:
+ * copyright (c) 2011-2012 Cypress Semiconductor, Inc.
+ * copyright (c) 2011-2012 Google, 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.
+ *
+ * Trademarks are the property of their respective owners.
+ */
+
+#ifndef _ELAN_I2C_H
+#define _ELAN_i2C_H
+
+#include <linux/types.h>
+
+#define ETP_ENABLE_ABS		0x0001
+#define ETP_ENABLE_CALIBRATE	0x0002
+#define ETP_DISABLE_CALIBRATE	0x0000
+#define ETP_DISABLE_POWER	0x0001
+
+/* IAP Firmware handling */
+#define ETP_FW_NAME		"elan_i2c.bin"
+#define ETP_IAP_START_ADDR	0x0083
+#define ETP_FW_IAP_PAGE_ERR	(1 << 5)
+#define ETP_FW_IAP_INTF_ERR	(1 << 4)
+#define ETP_FW_PAGE_SIZE	64
+#define ETP_FW_PAGE_COUNT	768
+#define ETP_FW_SIZE		(ETP_FW_PAGE_SIZE * ETP_FW_PAGE_COUNT)
+
+struct i2c_client;
+struct completion;
+
+enum tp_mode {
+	IAP_MODE = 1,
+	MAIN_MODE
+};
+
+struct elan_transport_ops {
+	int (*initialize)(struct i2c_client *client);
+	int (*sleep_control)(struct i2c_client *, bool sleep);
+	int (*power_control)(struct i2c_client *, bool enable);
+	int (*set_mode)(struct i2c_client *client, u8 mode);
+
+	int (*calibrate)(struct i2c_client *client);
+	int (*calibrate_result)(struct i2c_client *client, u8 *val);
+
+	int (*get_baseline_data)(struct i2c_client *client,
+				 bool max_baseliune, u8 *value);
+
+	int (*get_version)(struct i2c_client *client, bool iap, u8 *version);
+	int (*get_sm_version)(struct i2c_client *client, u8 *version);
+	int (*get_checksum)(struct i2c_client *client, bool iap, u16 *csum);
+	int (*get_product_id)(struct i2c_client *client, u8 *id);
+
+	int (*get_max)(struct i2c_client *client,
+		       unsigned int *max_x, unsigned int *max_y);
+	int (*get_resolution)(struct i2c_client *client,
+			      u8 *hw_res_x, u8 *hw_res_y);
+	int (*get_num_traces)(struct i2c_client *client,
+			      unsigned int *x_tracenum,
+			      unsigned int *y_tracenum);
+
+	int (*iap_get_mode)(struct i2c_client *client, enum tp_mode *mode);
+	int (*iap_reset)(struct i2c_client *client);
+
+	int (*prepare_fw_update)(struct i2c_client *client);
+	int (*write_fw_block)(struct i2c_client *client,
+			      const u8 *page, u16 checksum, int idx);
+	int (*finish_fw_update)(struct i2c_client *client,
+				struct completion *reset_done);
+
+	int (*get_report)(struct i2c_client *client, u8 *report);
+};
+
+extern const struct elan_transport_ops elan_smbus_ops, elan_i2c_ops;
+
+#endif /* _ELAN_I2C_H */
diff --git a/drivers/input/mouse/elan_i2c_core.c b/drivers/input/mouse/elan_i2c_core.c
new file mode 100644
index 000000000000..0cb2be48d537
--- /dev/null
+++ b/drivers/input/mouse/elan_i2c_core.c
@@ -0,0 +1,1137 @@
+/*
+ * Elan I2C/SMBus Touchpad driver
+ *
+ * Copyright (c) 2013 ELAN Microelectronics Corp.
+ *
+ * Author: 林政維 (Duson Lin) <dusonlin@emc.com.tw>
+ * Version: 1.5.5
+ *
+ * Based on cyapa driver:
+ * copyright (c) 2011-2012 Cypress Semiconductor, Inc.
+ * copyright (c) 2011-2012 Google, 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.
+ *
+ * Trademarks are the property of their respective owners.
+ */
+
+#include <linux/acpi.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/firmware.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/input/mt.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/input.h>
+#include <linux/uaccess.h>
+#include <linux/jiffies.h>
+#include <linux/completion.h>
+#include <linux/of.h>
+#include <linux/regulator/consumer.h>
+#include <asm/unaligned.h>
+
+#include "elan_i2c.h"
+
+#define DRIVER_NAME		"elan_i2c"
+#define ELAN_DRIVER_VERSION	"1.5.5"
+#define ETP_PRESSURE_OFFSET	25
+#define ETP_MAX_PRESSURE	255
+#define ETP_FWIDTH_REDUCE	90
+#define ETP_FINGER_WIDTH	15
+#define ETP_RETRY_COUNT		3
+
+#define ETP_MAX_FINGERS		5
+#define ETP_FINGER_DATA_LEN	5
+#define ETP_REPORT_ID		0x5D
+#define ETP_REPORT_ID_OFFSET	2
+#define ETP_TOUCH_INFO_OFFSET	3
+#define ETP_FINGER_DATA_OFFSET	4
+#define ETP_MAX_REPORT_LEN	34
+
+/* The main device structure */
+struct elan_tp_data {
+	struct i2c_client	*client;
+	struct input_dev	*input;
+	struct regulator	*vcc;
+
+	const struct elan_transport_ops *ops;
+
+	/* for fw update */
+	struct completion	fw_completion;
+	bool			in_fw_update;
+
+	struct mutex		sysfs_mutex;
+
+	unsigned int		max_x;
+	unsigned int		max_y;
+	unsigned int		width_x;
+	unsigned int		width_y;
+	unsigned int		x_res;
+	unsigned int		y_res;
+
+	u8			product_id;
+	u8			fw_version;
+	u8			sm_version;
+	u8			iap_version;
+	u16			fw_checksum;
+
+	u8			mode;
+
+	bool			irq_wake;
+
+	u8			min_baseline;
+	u8			max_baseline;
+	bool			baseline_ready;
+};
+
+static int elan_enable_power(struct elan_tp_data *data)
+{
+	int repeat = ETP_RETRY_COUNT;
+	int error;
+
+	error = regulator_enable(data->vcc);
+	if (error) {
+		dev_err(&data->client->dev,
+			"Failed to enable regulator: %d\n", error);
+		return error;
+	}
+
+	do {
+		error = data->ops->power_control(data->client, true);
+		if (error >= 0)
+			return 0;
+
+		msleep(30);
+	} while (--repeat > 0);
+
+	return error;
+}
+
+static int elan_disable_power(struct elan_tp_data *data)
+{
+	int repeat = ETP_RETRY_COUNT;
+	int error;
+
+	do {
+		error = data->ops->power_control(data->client, false);
+		if (!error) {
+			error = regulator_disable(data->vcc);
+			if (error) {
+				dev_err(&data->client->dev,
+					"Failed to disable regulator: %d\n",
+					error);
+				/* Attempt to power the chip back up */
+				data->ops->power_control(data->client, true);
+				break;
+			}
+
+			return 0;
+		}
+
+		msleep(30);
+	} while (--repeat > 0);
+
+	return error;
+}
+
+static int elan_sleep(struct elan_tp_data *data)
+{
+	int repeat = ETP_RETRY_COUNT;
+	int error;
+
+	do {
+		error = data->ops->sleep_control(data->client, true);
+		if (!error)
+			return 0;
+
+		msleep(30);
+	} while (--repeat > 0);
+
+	return error;
+}
+
+static int __elan_initialize(struct elan_tp_data *data)
+{
+	struct i2c_client *client = data->client;
+	int error;
+
+	error = data->ops->initialize(client);
+	if (error) {
+		dev_err(&client->dev, "device initialize failed: %d\n", error);
+		return error;
+	}
+
+	data->mode |= ETP_ENABLE_ABS;
+	error = data->ops->set_mode(client, data->mode);
+	if (error) {
+		dev_err(&client->dev,
+			"failed to switch to absolute mode: %d\n", error);
+		return error;
+	}
+
+	error = data->ops->sleep_control(client, false);
+	if (error) {
+		dev_err(&client->dev,
+			"failed to wake device up: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int elan_initialize(struct elan_tp_data *data)
+{
+	int repeat = ETP_RETRY_COUNT;
+	int error;
+
+	do {
+		error = __elan_initialize(data);
+		if (!error)
+			return 0;
+
+		repeat--;
+		msleep(30);
+	} while (--repeat > 0);
+
+	return error;
+}
+
+static int elan_query_device_info(struct elan_tp_data *data)
+{
+	int error;
+
+	error = data->ops->get_product_id(data->client, &data->product_id);
+	if (error)
+		return error;
+
+	error = data->ops->get_version(data->client, false, &data->fw_version);
+	if (error)
+		return error;
+
+	error = data->ops->get_checksum(data->client, false,
+					&data->fw_checksum);
+	if (error)
+		return error;
+
+	error = data->ops->get_sm_version(data->client, &data->sm_version);
+	if (error)
+		return error;
+
+	error = data->ops->get_version(data->client, true, &data->iap_version);
+	if (error)
+		return error;
+
+	return 0;
+}
+
+static unsigned int elan_convert_resolution(u8 val)
+{
+	/*
+	 * (value from firmware) * 10 + 790 = dpi
+	 *
+	 * We also have to convert dpi to dots/mm (*10/254 to avoid floating
+	 * point).
+	 */
+
+	return ((int)(char)val * 10 + 790) * 10 / 254;
+}
+
+static int elan_query_device_parameters(struct elan_tp_data *data)
+{
+	unsigned int x_traces, y_traces;
+	u8 hw_x_res, hw_y_res;
+	int error;
+
+	error = data->ops->get_max(data->client, &data->max_x, &data->max_y);
+	if (error)
+		return error;
+
+	error = data->ops->get_num_traces(data->client, &x_traces, &y_traces);
+	if (error)
+		return error;
+
+	data->width_x = data->max_x / x_traces;
+	data->width_y = data->max_y / y_traces;
+
+	error = data->ops->get_resolution(data->client, &hw_x_res, &hw_y_res);
+	if (error)
+		return error;
+
+	data->x_res = elan_convert_resolution(hw_x_res);
+	data->y_res = elan_convert_resolution(hw_y_res);
+
+	return 0;
+}
+
+/*
+ **********************************************************
+ * IAP firmware updater related routines
+ **********************************************************
+ */
+static int elan_write_fw_block(struct elan_tp_data *data,
+			       const u8 *page, u16 checksum, int idx)
+{
+	int retry = ETP_RETRY_COUNT;
+	int error;
+
+	do {
+		error = data->ops->write_fw_block(data->client,
+						  page, checksum, idx);
+		if (!error)
+			return 0;
+
+		dev_dbg(&data->client->dev,
+			"IAP retrying page %d (error: %d)\n", idx, error);
+	} while (--retry > 0);
+
+	return error;
+}
+
+static int __elan_update_firmware(struct elan_tp_data *data,
+				  const struct firmware *fw)
+{
+	struct i2c_client *client = data->client;
+	struct device *dev = &client->dev;
+	int i, j;
+	int error;
+	u16 iap_start_addr;
+	u16 boot_page_count;
+	u16 sw_checksum = 0, fw_checksum = 0;
+
+	error = data->ops->prepare_fw_update(client);
+	if (error)
+		return error;
+
+	iap_start_addr = get_unaligned_le16(&fw->data[ETP_IAP_START_ADDR * 2]);
+
+	boot_page_count = (iap_start_addr * 2) / ETP_FW_PAGE_SIZE;
+	for (i = boot_page_count; i < ETP_FW_PAGE_COUNT; i++) {
+		u16 checksum = 0;
+		const u8 *page = &fw->data[i * ETP_FW_PAGE_SIZE];
+
+		for (j = 0; j < ETP_FW_PAGE_SIZE; j += 2)
+			checksum += ((page[j + 1] << 8) | page[j]);
+
+		error = elan_write_fw_block(data, page, checksum, i);
+		if (error) {
+			dev_err(dev, "write page %d fail: %d\n", i, error);
+			return error;
+		}
+
+		sw_checksum += checksum;
+	}
+
+	/* Wait WDT reset and power on reset */
+	msleep(600);
+
+	error = data->ops->finish_fw_update(client, &data->fw_completion);
+	if (error)
+		return error;
+
+	error = data->ops->get_checksum(client, true, &fw_checksum);
+	if (error)
+		return error;
+
+	if (sw_checksum != fw_checksum) {
+		dev_err(dev, "checksum diff sw=[%04X], fw=[%04X]\n",
+			sw_checksum, fw_checksum);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int elan_update_firmware(struct elan_tp_data *data,
+				const struct firmware *fw)
+{
+	struct i2c_client *client = data->client;
+	int retval;
+
+	dev_dbg(&client->dev, "Starting firmware update....\n");
+
+	disable_irq(client->irq);
+	data->in_fw_update = true;
+
+	retval = __elan_update_firmware(data, fw);
+	if (retval) {
+		dev_err(&client->dev, "firmware update failed: %d\n", retval);
+		data->ops->iap_reset(client);
+	} else {
+		/* Reinitialize TP after fw is updated */
+		elan_initialize(data);
+		elan_query_device_info(data);
+	}
+
+	data->in_fw_update = false;
+	enable_irq(client->irq);
+
+	return retval;
+}
+
+/*
+ *******************************************************************
+ * SYSFS attributes
+ *******************************************************************
+ */
+static ssize_t elan_sysfs_read_fw_checksum(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct elan_tp_data *data = i2c_get_clientdata(client);
+
+	return sprintf(buf, "0x%04x\n", data->fw_checksum);
+}
+
+static ssize_t elan_sysfs_read_product_id(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct elan_tp_data *data = i2c_get_clientdata(client);
+
+	return sprintf(buf, "%d.0\n", data->product_id);
+}
+
+static ssize_t elan_sysfs_read_fw_ver(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct elan_tp_data *data = i2c_get_clientdata(client);
+
+	return sprintf(buf, "%d.0\n", data->fw_version);
+}
+
+static ssize_t elan_sysfs_read_sm_ver(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct elan_tp_data *data = i2c_get_clientdata(client);
+
+	return sprintf(buf, "%d.0\n", data->sm_version);
+}
+
+static ssize_t elan_sysfs_read_iap_ver(struct device *dev,
+				       struct device_attribute *attr,
+				       char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct elan_tp_data *data = i2c_get_clientdata(client);
+
+	return sprintf(buf, "%d.0\n", data->iap_version);
+}
+
+static ssize_t elan_sysfs_update_fw(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct elan_tp_data *data = i2c_get_clientdata(client);
+	const struct firmware *fw;
+	int error;
+
+	error = request_firmware(&fw, ETP_FW_NAME, dev);
+	if (error) {
+		dev_err(dev, "cannot load firmware %s: %d\n",
+			ETP_FW_NAME, error);
+		return error;
+	}
+
+	/* Firmware must be exactly PAGE_NUM * PAGE_SIZE bytes */
+	if (fw->size != ETP_FW_SIZE) {
+		dev_err(dev, "invalid firmware size = %zu, expected %d.\n",
+			fw->size, ETP_FW_SIZE);
+		error = -EBADF;
+		goto out_release_fw;
+	}
+
+	error = mutex_lock_interruptible(&data->sysfs_mutex);
+	if (error)
+		goto out_release_fw;
+
+	error = elan_update_firmware(data, fw);
+
+	mutex_unlock(&data->sysfs_mutex);
+
+out_release_fw:
+	release_firmware(fw);
+	return error ?: count;
+}
+
+static ssize_t calibrate_store(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct elan_tp_data *data = i2c_get_clientdata(client);
+	int tries = 20;
+	int retval;
+	int error;
+	u8 val[3];
+
+	retval = mutex_lock_interruptible(&data->sysfs_mutex);
+	if (retval)
+		return retval;
+
+	disable_irq(client->irq);
+
+	data->mode |= ETP_ENABLE_CALIBRATE;
+	retval = data->ops->set_mode(client, data->mode);
+	if (retval) {
+		dev_err(dev, "failed to enable calibration mode: %d\n",
+			retval);
+		goto out;
+	}
+
+	retval = data->ops->calibrate(client);
+	if (retval) {
+		dev_err(dev, "failed to start calibration: %d\n",
+			retval);
+		goto out_disable_calibrate;
+	}
+
+	val[0] = 0xff;
+	do {
+		/* Wait 250ms before checking if calibration has completed. */
+		msleep(250);
+
+		retval = data->ops->calibrate_result(client, val);
+		if (retval)
+			dev_err(dev, "failed to check calibration result: %d\n",
+				retval);
+		else if (val[0] == 0)
+			break; /* calibration done */
+
+	} while (--tries);
+
+	if (tries == 0) {
+		dev_err(dev, "failed to calibrate. Timeout.\n");
+		retval = -ETIMEDOUT;
+	}
+
+out_disable_calibrate:
+	data->mode &= ~ETP_ENABLE_CALIBRATE;
+	error = data->ops->set_mode(data->client, data->mode);
+	if (error) {
+		dev_err(dev, "failed to disable calibration mode: %d\n",
+			error);
+		if (!retval)
+			retval = error;
+	}
+out:
+	enable_irq(client->irq);
+	mutex_unlock(&data->sysfs_mutex);
+	return retval ?: count;
+}
+
+static ssize_t elan_sysfs_read_mode(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct elan_tp_data *data = i2c_get_clientdata(client);
+	int error;
+	enum tp_mode mode;
+
+	error = mutex_lock_interruptible(&data->sysfs_mutex);
+	if (error)
+		return error;
+
+	error = data->ops->iap_get_mode(data->client, &mode);
+
+	mutex_unlock(&data->sysfs_mutex);
+
+	if (error)
+		return error;
+
+	return sprintf(buf, "%d\n", (int)mode);
+}
+
+static DEVICE_ATTR(product_id, S_IRUGO, elan_sysfs_read_product_id, NULL);
+static DEVICE_ATTR(firmware_version, S_IRUGO, elan_sysfs_read_fw_ver, NULL);
+static DEVICE_ATTR(sample_version, S_IRUGO, elan_sysfs_read_sm_ver, NULL);
+static DEVICE_ATTR(iap_version, S_IRUGO, elan_sysfs_read_iap_ver, NULL);
+static DEVICE_ATTR(fw_checksum, S_IRUGO, elan_sysfs_read_fw_checksum, NULL);
+static DEVICE_ATTR(mode, S_IRUGO, elan_sysfs_read_mode, NULL);
+static DEVICE_ATTR(update_fw, S_IWUSR, NULL, elan_sysfs_update_fw);
+
+static DEVICE_ATTR_WO(calibrate);
+
+static struct attribute *elan_sysfs_entries[] = {
+	&dev_attr_product_id.attr,
+	&dev_attr_firmware_version.attr,
+	&dev_attr_sample_version.attr,
+	&dev_attr_iap_version.attr,
+	&dev_attr_fw_checksum.attr,
+	&dev_attr_calibrate.attr,
+	&dev_attr_mode.attr,
+	&dev_attr_update_fw.attr,
+	NULL,
+};
+
+static const struct attribute_group elan_sysfs_group = {
+	.attrs = elan_sysfs_entries,
+};
+
+static ssize_t acquire_store(struct device *dev, struct device_attribute *attr,
+			     const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct elan_tp_data *data = i2c_get_clientdata(client);
+	int error;
+	int retval;
+
+	retval = mutex_lock_interruptible(&data->sysfs_mutex);
+	if (retval)
+		return retval;
+
+	disable_irq(client->irq);
+
+	data->baseline_ready = false;
+
+	data->mode |= ETP_ENABLE_CALIBRATE;
+	retval = data->ops->set_mode(data->client, data->mode);
+	if (retval) {
+		dev_err(dev, "Failed to enable calibration mode to get baseline: %d\n",
+			retval);
+		goto out;
+	}
+
+	msleep(250);
+
+	retval = data->ops->get_baseline_data(data->client, true,
+					      &data->max_baseline);
+	if (retval) {
+		dev_err(dev, "Failed to read max baseline form device: %d\n",
+			retval);
+		goto out_disable_calibrate;
+	}
+
+	retval = data->ops->get_baseline_data(data->client, false,
+					      &data->min_baseline);
+	if (retval) {
+		dev_err(dev, "Failed to read min baseline form device: %d\n",
+			retval);
+		goto out_disable_calibrate;
+	}
+
+	data->baseline_ready = true;
+
+out_disable_calibrate:
+	data->mode &= ~ETP_ENABLE_CALIBRATE;
+	error = data->ops->set_mode(data->client, data->mode);
+	if (error) {
+		dev_err(dev, "Failed to disable calibration mode after acquiring baseline: %d\n",
+			error);
+		if (!retval)
+			retval = error;
+	}
+out:
+	enable_irq(client->irq);
+	mutex_unlock(&data->sysfs_mutex);
+	return retval ?: count;
+}
+
+static ssize_t min_show(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct elan_tp_data *data = i2c_get_clientdata(client);
+	int retval;
+
+	retval = mutex_lock_interruptible(&data->sysfs_mutex);
+	if (retval)
+		return retval;
+
+	if (!data->baseline_ready) {
+		retval = -ENODATA;
+		goto out;
+	}
+
+	retval = snprintf(buf, PAGE_SIZE, "%d", data->min_baseline);
+
+out:
+	mutex_unlock(&data->sysfs_mutex);
+	return retval;
+}
+
+static ssize_t max_show(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct elan_tp_data *data = i2c_get_clientdata(client);
+	int retval;
+
+	retval = mutex_lock_interruptible(&data->sysfs_mutex);
+	if (retval)
+		return retval;
+
+	if (!data->baseline_ready) {
+		retval = -ENODATA;
+		goto out;
+	}
+
+	retval = snprintf(buf, PAGE_SIZE, "%d", data->max_baseline);
+
+out:
+	mutex_unlock(&data->sysfs_mutex);
+	return retval;
+}
+
+
+static DEVICE_ATTR_WO(acquire);
+static DEVICE_ATTR_RO(min);
+static DEVICE_ATTR_RO(max);
+
+static struct attribute *elan_baseline_sysfs_entries[] = {
+	&dev_attr_acquire.attr,
+	&dev_attr_min.attr,
+	&dev_attr_max.attr,
+	NULL,
+};
+
+static const struct attribute_group elan_baseline_sysfs_group = {
+	.name = "baseline",
+	.attrs = elan_baseline_sysfs_entries,
+};
+
+static const struct attribute_group *elan_sysfs_groups[] = {
+	&elan_sysfs_group,
+	&elan_baseline_sysfs_group,
+	NULL
+};
+
+/*
+ ******************************************************************
+ * Elan isr functions
+ ******************************************************************
+ */
+static void elan_report_contact(struct elan_tp_data *data,
+				int contact_num, bool contact_valid,
+				u8 *finger_data)
+{
+	struct input_dev *input = data->input;
+	unsigned int pos_x, pos_y;
+	unsigned int pressure, mk_x, mk_y;
+	unsigned int area_x, area_y, major, minor, new_pressure;
+
+
+	if (contact_valid) {
+		pos_x = ((finger_data[0] & 0xf0) << 4) |
+						finger_data[1];
+		pos_y = ((finger_data[0] & 0x0f) << 8) |
+						finger_data[2];
+		mk_x = (finger_data[3] & 0x0f);
+		mk_y = (finger_data[3] >> 4);
+		pressure = finger_data[4];
+
+		if (pos_x > data->max_x || pos_y > data->max_y) {
+			dev_dbg(input->dev.parent,
+				"[%d] x=%d y=%d over max (%d, %d)",
+				contact_num, pos_x, pos_y,
+				data->max_x, data->max_y);
+			return;
+		}
+
+		/*
+		 * To avoid treating large finger as palm, let's reduce the
+		 * width x and y per trace.
+		 */
+		area_x = mk_x * (data->width_x - ETP_FWIDTH_REDUCE);
+		area_y = mk_y * (data->width_y - ETP_FWIDTH_REDUCE);
+
+		major = max(area_x, area_y);
+		minor = min(area_x, area_y);
+
+		new_pressure = pressure + ETP_PRESSURE_OFFSET;
+		if (new_pressure > ETP_MAX_PRESSURE)
+			new_pressure = ETP_MAX_PRESSURE;
+
+		input_mt_slot(input, contact_num);
+		input_mt_report_slot_state(input, MT_TOOL_FINGER, true);
+		input_report_abs(input, ABS_MT_POSITION_X, pos_x);
+		input_report_abs(input, ABS_MT_POSITION_Y, data->max_y - pos_y);
+		input_report_abs(input, ABS_MT_PRESSURE, new_pressure);
+		input_report_abs(input, ABS_TOOL_WIDTH, mk_x);
+		input_report_abs(input, ABS_MT_TOUCH_MAJOR, major);
+		input_report_abs(input, ABS_MT_TOUCH_MINOR, minor);
+	} else {
+		input_mt_slot(input, contact_num);
+		input_mt_report_slot_state(input, MT_TOOL_FINGER, false);
+	}
+}
+
+static void elan_report_absolute(struct elan_tp_data *data, u8 *packet)
+{
+	struct input_dev *input = data->input;
+	u8 *finger_data = &packet[ETP_FINGER_DATA_OFFSET];
+	int i;
+	u8 tp_info = packet[ETP_TOUCH_INFO_OFFSET];
+	bool contact_valid;
+
+	for (i = 0; i < ETP_MAX_FINGERS; i++) {
+		contact_valid = tp_info & (1U << (3 + i));
+		elan_report_contact(data, i, contact_valid, finger_data);
+
+		if (contact_valid)
+			finger_data += ETP_FINGER_DATA_LEN;
+	}
+
+	input_report_key(input, BTN_LEFT, tp_info & 0x01);
+	input_mt_report_pointer_emulation(input, true);
+	input_sync(input);
+}
+
+static irqreturn_t elan_isr(int irq, void *dev_id)
+{
+	struct elan_tp_data *data = dev_id;
+	struct device *dev = &data->client->dev;
+	int error;
+	u8 report[ETP_MAX_REPORT_LEN];
+
+	/*
+	 * When device is connected to i2c bus, when all IAP page writes
+	 * complete, the driver will receive interrupt and must read
+	 * 0000 to confirm that IAP is finished.
+	*/
+	if (data->in_fw_update) {
+		complete(&data->fw_completion);
+		goto out;
+	}
+
+	error = data->ops->get_report(data->client, report);
+	if (error)
+		goto out;
+
+	if (report[ETP_REPORT_ID_OFFSET] != ETP_REPORT_ID)
+		dev_err(dev, "invalid report id data (%x)\n",
+			report[ETP_REPORT_ID_OFFSET]);
+	else
+		elan_report_absolute(data, report);
+
+out:
+	return IRQ_HANDLED;
+}
+
+/*
+ ******************************************************************
+ * Elan initialization functions
+ ******************************************************************
+ */
+static int elan_setup_input_device(struct elan_tp_data *data)
+{
+	struct device *dev = &data->client->dev;
+	struct input_dev *input;
+	unsigned int max_width = max(data->width_x, data->width_y);
+	unsigned int min_width = min(data->width_x, data->width_y);
+	int error;
+
+	input = devm_input_allocate_device(dev);
+	if (!input)
+		return -ENOMEM;
+
+	input->name = "Elan Touchpad";
+	input->id.bustype = BUS_I2C;
+	input_set_drvdata(input, data);
+
+	error = input_mt_init_slots(input, ETP_MAX_FINGERS,
+				    INPUT_MT_POINTER | INPUT_MT_DROP_UNUSED);
+	if (error) {
+		dev_err(dev, "failed to initialize MT slots: %d\n", error);
+		return error;
+	}
+
+	__set_bit(EV_ABS, input->evbit);
+	__set_bit(INPUT_PROP_POINTER, input->propbit);
+	__set_bit(INPUT_PROP_BUTTONPAD, input->propbit);
+	__set_bit(BTN_LEFT, input->keybit);
+
+	/* Set up ST parameters */
+	input_set_abs_params(input, ABS_X, 0, data->max_x, 0, 0);
+	input_set_abs_params(input, ABS_Y, 0, data->max_y, 0, 0);
+	input_abs_set_res(input, ABS_X, data->x_res);
+	input_abs_set_res(input, ABS_Y, data->y_res);
+	input_set_abs_params(input, ABS_PRESSURE, 0, ETP_MAX_PRESSURE, 0, 0);
+	input_set_abs_params(input, ABS_TOOL_WIDTH, 0, ETP_FINGER_WIDTH, 0, 0);
+
+	/* And MT parameters */
+	input_set_abs_params(input, ABS_MT_POSITION_X, 0, data->max_x, 0, 0);
+	input_set_abs_params(input, ABS_MT_POSITION_Y, 0, data->max_y, 0, 0);
+	input_abs_set_res(input, ABS_MT_POSITION_X, data->x_res);
+	input_abs_set_res(input, ABS_MT_POSITION_Y, data->y_res);
+	input_set_abs_params(input, ABS_MT_PRESSURE, 0,
+			     ETP_MAX_PRESSURE, 0, 0);
+	input_set_abs_params(input, ABS_MT_TOUCH_MAJOR, 0,
+			     ETP_FINGER_WIDTH * max_width, 0, 0);
+	input_set_abs_params(input, ABS_MT_TOUCH_MINOR, 0,
+			     ETP_FINGER_WIDTH * min_width, 0, 0);
+
+	data->input = input;
+
+	return 0;
+}
+
+static void elan_disable_regulator(void *_data)
+{
+	struct elan_tp_data *data = _data;
+
+	regulator_disable(data->vcc);
+}
+
+static void elan_remove_sysfs_groups(void *_data)
+{
+	struct elan_tp_data *data = _data;
+
+	sysfs_remove_groups(&data->client->dev.kobj, elan_sysfs_groups);
+}
+
+static int elan_probe(struct i2c_client *client,
+		      const struct i2c_device_id *dev_id)
+{
+	const struct elan_transport_ops *transport_ops;
+	struct device *dev = &client->dev;
+	struct elan_tp_data *data;
+	unsigned long irqflags;
+	int error;
+
+	if (IS_ENABLED(CONFIG_MOUSE_ELAN_I2C_I2C) &&
+	    i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		transport_ops = &elan_i2c_ops;
+	} else if (IS_ENABLED(CONFIG_MOUSE_ELAN_I2C_SMBUS) &&
+		   i2c_check_functionality(client->adapter,
+					   I2C_FUNC_SMBUS_BYTE_DATA |
+						I2C_FUNC_SMBUS_BLOCK_DATA |
+						I2C_FUNC_SMBUS_I2C_BLOCK)) {
+		transport_ops = &elan_smbus_ops;
+	} else {
+		dev_err(dev, "not a supported I2C/SMBus adapter\n");
+		return -EIO;
+	}
+
+	data = devm_kzalloc(&client->dev, sizeof(struct elan_tp_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	i2c_set_clientdata(client, data);
+
+	data->ops = transport_ops;
+	data->client = client;
+	init_completion(&data->fw_completion);
+	mutex_init(&data->sysfs_mutex);
+
+	data->vcc = devm_regulator_get(&client->dev, "vcc");
+	if (IS_ERR(data->vcc)) {
+		error = PTR_ERR(data->vcc);
+		if (error != -EPROBE_DEFER)
+			dev_err(&client->dev,
+				"Failed to get 'vcc' regulator: %d\n",
+				error);
+		return error;
+	}
+
+	error = regulator_enable(data->vcc);
+	if (error) {
+		dev_err(&client->dev,
+			"Failed to enable regulator: %d\n", error);
+		return error;
+	}
+
+	error = devm_add_action(&client->dev,
+				elan_disable_regulator, data);
+	if (error) {
+		regulator_disable(data->vcc);
+		dev_err(&client->dev,
+			"Failed to add disable regulator action: %d\n",
+			error);
+		return error;
+	}
+
+	/* Initialize the touchpad. */
+	error = elan_initialize(data);
+	if (error)
+		return error;
+
+	error = elan_query_device_info(data);
+	if (error)
+		return error;
+
+	error = elan_query_device_parameters(data);
+	if (error)
+		return error;
+
+	dev_dbg(&client->dev,
+		"Elan Touchpad Information:\n"
+		"    Module product ID:  0x%04x\n"
+		"    Firmware Version:  0x%04x\n"
+		"    Sample Version:  0x%04x\n"
+		"    IAP Version:  0x%04x\n"
+		"    Max ABS X,Y:   %d,%d\n"
+		"    Width X,Y:   %d,%d\n"
+		"    Resolution X,Y:   %d,%d (dots/mm)\n",
+		data->product_id,
+		data->fw_version,
+		data->sm_version,
+		data->iap_version,
+		data->max_x, data->max_y,
+		data->width_x, data->width_y,
+		data->x_res, data->y_res);
+
+	/* Set up input device properties based on queried parameters. */
+	error = elan_setup_input_device(data);
+	if (error)
+		return error;
+
+	/*
+	 * Systems using device tree should set up interrupt via DTS,
+	 * the rest will use the default falling edge interrupts.
+	 */
+	irqflags = client->dev.of_node ? 0 : IRQF_TRIGGER_FALLING;
+
+	error = devm_request_threaded_irq(&client->dev, client->irq,
+					  NULL, elan_isr,
+					  irqflags | IRQF_ONESHOT,
+					  client->name, data);
+	if (error) {
+		dev_err(&client->dev, "cannot register irq=%d\n", client->irq);
+		return error;
+	}
+
+	error = sysfs_create_groups(&client->dev.kobj, elan_sysfs_groups);
+	if (error) {
+		dev_err(&client->dev, "failed to create sysfs attributes: %d\n",
+			error);
+		return error;
+	}
+
+	error = devm_add_action(&client->dev,
+				elan_remove_sysfs_groups, data);
+	if (error) {
+		elan_remove_sysfs_groups(data);
+		dev_err(&client->dev,
+			"Failed to add sysfs cleanup action: %d\n",
+			error);
+		return error;
+	}
+
+	error = input_register_device(data->input);
+	if (error) {
+		dev_err(&client->dev, "failed to register input device: %d\n",
+			error);
+		return error;
+	}
+
+	/*
+	 * Systems using device tree should set up wakeup via DTS,
+	 * the rest will configure device as wakeup source by default.
+	 */
+	if (!client->dev.of_node)
+		device_init_wakeup(&client->dev, true);
+
+	return 0;
+}
+
+static int __maybe_unused elan_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct elan_tp_data *data = i2c_get_clientdata(client);
+	int ret;
+
+	/*
+	 * We are taking the mutex to make sure sysfs operations are
+	 * complete before we attempt to bring the device into low[er]
+	 * power mode.
+	 */
+	ret = mutex_lock_interruptible(&data->sysfs_mutex);
+	if (ret)
+		return ret;
+
+	disable_irq(client->irq);
+
+	if (device_may_wakeup(dev)) {
+		ret = elan_sleep(data);
+		/* Enable wake from IRQ */
+		data->irq_wake = (enable_irq_wake(client->irq) == 0);
+	} else {
+		ret = elan_disable_power(data);
+	}
+
+	mutex_unlock(&data->sysfs_mutex);
+	return ret;
+}
+
+static int __maybe_unused elan_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct elan_tp_data *data = i2c_get_clientdata(client);
+	int error;
+
+	if (device_may_wakeup(dev) && data->irq_wake) {
+		disable_irq_wake(client->irq);
+		data->irq_wake = false;
+	}
+
+	error = elan_enable_power(data);
+	if (error)
+		dev_err(dev, "power up when resuming failed: %d\n", error);
+
+	error = elan_initialize(data);
+	if (error)
+		dev_err(dev, "initialize when resuming failed: %d\n", error);
+
+	enable_irq(data->client->irq);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(elan_pm_ops, elan_suspend, elan_resume);
+
+static const struct i2c_device_id elan_id[] = {
+	{ DRIVER_NAME, 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, elan_id);
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id elan_acpi_id[] = {
+	{ "ELAN0000", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(acpi, elan_acpi_id);
+#endif
+
+#ifdef CONFIG_OF
+static const struct of_device_id elan_of_match[] = {
+	{ .compatible = "elan,ekth3000" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, elan_of_match);
+#endif
+
+static struct i2c_driver elan_driver = {
+	.driver = {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+		.pm	= &elan_pm_ops,
+		.acpi_match_table = ACPI_PTR(elan_acpi_id),
+		.of_match_table = of_match_ptr(elan_of_match),
+	},
+	.probe		= elan_probe,
+	.id_table	= elan_id,
+};
+
+module_i2c_driver(elan_driver);
+
+MODULE_AUTHOR("Duson Lin <dusonlin@emc.com.tw>");
+MODULE_DESCRIPTION("Elan I2C/SMBus Touchpad driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(ELAN_DRIVER_VERSION);
diff --git a/drivers/input/mouse/elan_i2c_i2c.c b/drivers/input/mouse/elan_i2c_i2c.c
new file mode 100644
index 000000000000..97d4937fc244
--- /dev/null
+++ b/drivers/input/mouse/elan_i2c_i2c.c
@@ -0,0 +1,611 @@
+/*
+ * Elan I2C/SMBus Touchpad driver - I2C interface
+ *
+ * Copyright (c) 2013 ELAN Microelectronics Corp.
+ *
+ * Author: 林政維 (Duson Lin) <dusonlin@emc.com.tw>
+ * Version: 1.5.5
+ *
+ * Based on cyapa driver:
+ * copyright (c) 2011-2012 Cypress Semiconductor, Inc.
+ * copyright (c) 2011-2012 Google, 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.
+ *
+ * Trademarks are the property of their respective owners.
+ */
+
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/jiffies.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <asm/unaligned.h>
+
+#include "elan_i2c.h"
+
+/* Elan i2c commands */
+#define ETP_I2C_RESET			0x0100
+#define ETP_I2C_WAKE_UP			0x0800
+#define ETP_I2C_SLEEP			0x0801
+#define ETP_I2C_DESC_CMD		0x0001
+#define ETP_I2C_REPORT_DESC_CMD		0x0002
+#define ETP_I2C_STAND_CMD		0x0005
+#define ETP_I2C_UNIQUEID_CMD		0x0101
+#define ETP_I2C_FW_VERSION_CMD		0x0102
+#define ETP_I2C_SM_VERSION_CMD		0x0103
+#define ETP_I2C_XY_TRACENUM_CMD		0x0105
+#define ETP_I2C_MAX_X_AXIS_CMD		0x0106
+#define ETP_I2C_MAX_Y_AXIS_CMD		0x0107
+#define ETP_I2C_RESOLUTION_CMD		0x0108
+#define ETP_I2C_IAP_VERSION_CMD		0x0110
+#define ETP_I2C_SET_CMD			0x0300
+#define ETP_I2C_POWER_CMD		0x0307
+#define ETP_I2C_FW_CHECKSUM_CMD		0x030F
+#define ETP_I2C_IAP_CTRL_CMD		0x0310
+#define ETP_I2C_IAP_CMD			0x0311
+#define ETP_I2C_IAP_RESET_CMD		0x0314
+#define ETP_I2C_IAP_CHECKSUM_CMD	0x0315
+#define ETP_I2C_CALIBRATE_CMD		0x0316
+#define ETP_I2C_MAX_BASELINE_CMD	0x0317
+#define ETP_I2C_MIN_BASELINE_CMD	0x0318
+
+#define ETP_I2C_REPORT_LEN		34
+#define ETP_I2C_DESC_LENGTH		30
+#define ETP_I2C_REPORT_DESC_LENGTH	158
+#define ETP_I2C_INF_LENGTH		2
+#define ETP_I2C_IAP_PASSWORD		0x1EA5
+#define ETP_I2C_IAP_RESET		0xF0F0
+#define ETP_I2C_MAIN_MODE_ON		(1 << 9)
+#define ETP_I2C_IAP_REG_L		0x01
+#define ETP_I2C_IAP_REG_H		0x06
+
+static int elan_i2c_read_block(struct i2c_client *client,
+			       u16 reg, u8 *val, u16 len)
+{
+	__le16 buf[] = {
+		cpu_to_le16(reg),
+	};
+	struct i2c_msg msgs[] = {
+		{
+			.addr = client->addr,
+			.flags = client->flags & I2C_M_TEN,
+			.len = sizeof(buf),
+			.buf = (u8 *)buf,
+		},
+		{
+			.addr = client->addr,
+			.flags = (client->flags & I2C_M_TEN) | I2C_M_RD,
+			.len = len,
+			.buf = val,
+		}
+	};
+	int ret;
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	return ret == ARRAY_SIZE(msgs) ? 0 : (ret < 0 ? ret : -EIO);
+}
+
+static int elan_i2c_read_cmd(struct i2c_client *client, u16 reg, u8 *val)
+{
+	int retval;
+
+	retval = elan_i2c_read_block(client, reg, val, ETP_I2C_INF_LENGTH);
+	if (retval < 0) {
+		dev_err(&client->dev, "reading cmd (0x%04x) fail.\n", reg);
+		return retval;
+	}
+
+	return 0;
+}
+
+static int elan_i2c_write_cmd(struct i2c_client *client, u16 reg, u16 cmd)
+{
+	__le16 buf[] = {
+		cpu_to_le16(reg),
+		cpu_to_le16(cmd),
+	};
+	struct i2c_msg msg = {
+		.addr = client->addr,
+		.flags = client->flags & I2C_M_TEN,
+		.len = sizeof(buf),
+		.buf = (u8 *)buf,
+	};
+	int ret;
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	return ret == 1 ? 0 : (ret < 0 ? ret : -EIO);
+}
+
+static int elan_i2c_initialize(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	int error;
+	u8 val[256];
+
+	error = elan_i2c_write_cmd(client, ETP_I2C_STAND_CMD, ETP_I2C_RESET);
+	if (error) {
+		dev_err(dev, "device reset failed: %d\n", error);
+		return error;
+	}
+
+	/* Wait for the device to reset */
+	msleep(100);
+
+	/* get reset acknowledgement 0000 */
+	error = i2c_master_recv(client, val, ETP_I2C_INF_LENGTH);
+	if (error < 0) {
+		dev_err(dev, "failed to read reset response: %d\n", error);
+		return error;
+	}
+
+	error = elan_i2c_read_block(client, ETP_I2C_DESC_CMD,
+				    val, ETP_I2C_DESC_LENGTH);
+	if (error) {
+		dev_err(dev, "cannot get device descriptor: %d\n", error);
+		return error;
+	}
+
+	error = elan_i2c_read_block(client, ETP_I2C_REPORT_DESC_CMD,
+				    val, ETP_I2C_REPORT_DESC_LENGTH);
+	if (error) {
+		dev_err(dev, "fetching report descriptor failed.: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int elan_i2c_sleep_control(struct i2c_client *client, bool sleep)
+{
+	return elan_i2c_write_cmd(client, ETP_I2C_STAND_CMD,
+				  sleep ? ETP_I2C_SLEEP : ETP_I2C_WAKE_UP);
+}
+
+static int elan_i2c_power_control(struct i2c_client *client, bool enable)
+{
+	u8 val[2];
+	u16 reg;
+	int error;
+
+	error = elan_i2c_read_cmd(client, ETP_I2C_POWER_CMD, val);
+	if (error) {
+		dev_err(&client->dev,
+			"failed to read current power state: %d\n",
+			error);
+		return error;
+	}
+
+	reg = le16_to_cpup((__le16 *)val);
+	if (enable)
+		reg &= ~ETP_DISABLE_POWER;
+	else
+		reg |= ETP_DISABLE_POWER;
+
+	error = elan_i2c_write_cmd(client, ETP_I2C_POWER_CMD, reg);
+	if (error) {
+		dev_err(&client->dev,
+			"failed to write current power state: %d\n",
+			error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int elan_i2c_set_mode(struct i2c_client *client, u8 mode)
+{
+	return elan_i2c_write_cmd(client, ETP_I2C_SET_CMD, mode);
+}
+
+
+static int elan_i2c_calibrate(struct i2c_client *client)
+{
+	return elan_i2c_write_cmd(client, ETP_I2C_CALIBRATE_CMD, 1);
+}
+
+static int elan_i2c_calibrate_result(struct i2c_client *client, u8 *val)
+{
+	return elan_i2c_read_block(client, ETP_I2C_CALIBRATE_CMD, val, 1);
+}
+
+static int elan_i2c_get_baseline_data(struct i2c_client *client,
+				      bool max_baseline, u8 *value)
+{
+	int error;
+	u8 val[3];
+
+	error = elan_i2c_read_cmd(client,
+				  max_baseline ? ETP_I2C_MAX_BASELINE_CMD :
+						 ETP_I2C_MIN_BASELINE_CMD,
+				  val);
+	if (error)
+		return error;
+
+	*value = le16_to_cpup((__le16 *)val);
+
+	return 0;
+}
+
+static int elan_i2c_get_version(struct i2c_client *client,
+				bool iap, u8 *version)
+{
+	int error;
+	u8 val[3];
+
+	error = elan_i2c_read_cmd(client,
+				  iap ? ETP_I2C_IAP_VERSION_CMD :
+					ETP_I2C_FW_VERSION_CMD,
+				  val);
+	if (error) {
+		dev_err(&client->dev, "failed to get %s version: %d\n",
+			iap ? "IAP" : "FW", error);
+		return error;
+	}
+
+	*version = val[0];
+	return 0;
+}
+
+static int elan_i2c_get_sm_version(struct i2c_client *client, u8 *version)
+{
+	int error;
+	u8 val[3];
+
+	error = elan_i2c_read_cmd(client, ETP_I2C_SM_VERSION_CMD, val);
+	if (error) {
+		dev_err(&client->dev, "failed to get SM version: %d\n", error);
+		return error;
+	}
+
+	*version = val[0];
+	return 0;
+}
+
+static int elan_i2c_get_product_id(struct i2c_client *client, u8 *id)
+{
+	int error;
+	u8 val[3];
+
+	error = elan_i2c_read_cmd(client, ETP_I2C_UNIQUEID_CMD, val);
+	if (error) {
+		dev_err(&client->dev, "failed to get product ID: %d\n", error);
+		return error;
+	}
+
+	*id = val[0];
+	return 0;
+}
+
+static int elan_i2c_get_checksum(struct i2c_client *client,
+				 bool iap, u16 *csum)
+{
+	int error;
+	u8 val[3];
+
+	error = elan_i2c_read_cmd(client,
+				  iap ? ETP_I2C_IAP_CHECKSUM_CMD :
+					ETP_I2C_FW_CHECKSUM_CMD,
+				  val);
+	if (error) {
+		dev_err(&client->dev, "failed to get %s checksum: %d\n",
+			iap ? "IAP" : "FW", error);
+		return error;
+	}
+
+	*csum = le16_to_cpup((__le16 *)val);
+	return 0;
+}
+
+static int elan_i2c_get_max(struct i2c_client *client,
+			    unsigned int *max_x, unsigned int *max_y)
+{
+	int error;
+	u8 val[3];
+
+	error = elan_i2c_read_cmd(client, ETP_I2C_MAX_X_AXIS_CMD, val);
+	if (error) {
+		dev_err(&client->dev, "failed to get X dimension: %d\n", error);
+		return error;
+	}
+
+	*max_x = le16_to_cpup((__le16 *)val) & 0x0fff;
+
+	error = elan_i2c_read_cmd(client, ETP_I2C_MAX_Y_AXIS_CMD, val);
+	if (error) {
+		dev_err(&client->dev, "failed to get Y dimension: %d\n", error);
+		return error;
+	}
+
+	*max_y = le16_to_cpup((__le16 *)val) & 0x0fff;
+
+	return 0;
+}
+
+static int elan_i2c_get_resolution(struct i2c_client *client,
+				   u8 *hw_res_x, u8 *hw_res_y)
+{
+	int error;
+	u8 val[3];
+
+	error = elan_i2c_read_cmd(client, ETP_I2C_RESOLUTION_CMD, val);
+	if (error) {
+		dev_err(&client->dev, "failed to get resolution: %d\n", error);
+		return error;
+	}
+
+	*hw_res_x = val[0];
+	*hw_res_y = val[1];
+
+	return 0;
+}
+
+static int elan_i2c_get_num_traces(struct i2c_client *client,
+				   unsigned int *x_traces,
+				   unsigned int *y_traces)
+{
+	int error;
+	u8 val[3];
+
+	error = elan_i2c_read_cmd(client, ETP_I2C_XY_TRACENUM_CMD, val);
+	if (error) {
+		dev_err(&client->dev, "failed to get trace info: %d\n", error);
+		return error;
+	}
+
+	*x_traces = val[0] - 1;
+	*y_traces = val[1] - 1;
+
+	return 0;
+}
+
+static int elan_i2c_iap_get_mode(struct i2c_client *client, enum tp_mode *mode)
+{
+	int error;
+	u16 constant;
+	u8 val[3];
+
+	error = elan_i2c_read_cmd(client, ETP_I2C_IAP_CTRL_CMD, val);
+	if (error) {
+		dev_err(&client->dev,
+			"failed to read iap control register: %d\n",
+			error);
+		return error;
+	}
+
+	constant = le16_to_cpup((__le16 *)val);
+	dev_dbg(&client->dev, "iap control reg: 0x%04x.\n", constant);
+
+	*mode = (constant & ETP_I2C_MAIN_MODE_ON) ? MAIN_MODE : IAP_MODE;
+
+	return 0;
+}
+
+static int elan_i2c_iap_reset(struct i2c_client *client)
+{
+	int error;
+
+	error = elan_i2c_write_cmd(client, ETP_I2C_IAP_RESET_CMD,
+				   ETP_I2C_IAP_RESET);
+	if (error) {
+		dev_err(&client->dev, "cannot reset IC: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int elan_i2c_set_flash_key(struct i2c_client *client)
+{
+	int error;
+
+	error = elan_i2c_write_cmd(client, ETP_I2C_IAP_CMD,
+				   ETP_I2C_IAP_PASSWORD);
+	if (error) {
+		dev_err(&client->dev, "cannot set flash key: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int elan_i2c_prepare_fw_update(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	int error;
+	enum tp_mode mode;
+	u8 val[3];
+	u16 password;
+
+	/* Get FW in which mode	(IAP_MODE/MAIN_MODE)  */
+	error = elan_i2c_iap_get_mode(client, &mode);
+	if (error)
+		return error;
+
+	if (mode == IAP_MODE) {
+		/* Reset IC */
+		error = elan_i2c_iap_reset(client);
+		if (error)
+			return error;
+
+		msleep(30);
+	}
+
+	/* Set flash key*/
+	error = elan_i2c_set_flash_key(client);
+	if (error)
+		return error;
+
+	/* Wait for F/W IAP initialization */
+	msleep(mode == MAIN_MODE ? 100 : 30);
+
+	/* Check if we are in IAP mode or not */
+	error = elan_i2c_iap_get_mode(client, &mode);
+	if (error)
+		return error;
+
+	if (mode == MAIN_MODE) {
+		dev_err(dev, "wrong mode: %d\n", mode);
+		return -EIO;
+	}
+
+	/* Set flash key again */
+	error = elan_i2c_set_flash_key(client);
+	if (error)
+		return error;
+
+	/* Wait for F/W IAP initialization */
+	msleep(30);
+
+	/* read back to check we actually enabled successfully. */
+	error = elan_i2c_read_cmd(client, ETP_I2C_IAP_CMD, val);
+	if (error) {
+		dev_err(dev, "cannot read iap password: %d\n",
+			error);
+		return error;
+	}
+
+	password = le16_to_cpup((__le16 *)val);
+	if (password != ETP_I2C_IAP_PASSWORD) {
+		dev_err(dev, "wrong iap password: 0x%X\n", password);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int elan_i2c_write_fw_block(struct i2c_client *client,
+				   const u8 *page, u16 checksum, int idx)
+{
+	struct device *dev = &client->dev;
+	u8 page_store[ETP_FW_PAGE_SIZE + 4];
+	u8 val[3];
+	u16 result;
+	int ret, error;
+
+	page_store[0] = ETP_I2C_IAP_REG_L;
+	page_store[1] = ETP_I2C_IAP_REG_H;
+	memcpy(&page_store[2], page, ETP_FW_PAGE_SIZE);
+	/* recode checksum at last two bytes */
+	put_unaligned_le16(checksum, &page_store[ETP_FW_PAGE_SIZE + 2]);
+
+	ret = i2c_master_send(client, page_store, sizeof(page_store));
+	if (ret != sizeof(page_store)) {
+		error = ret < 0 ? ret : -EIO;
+		dev_err(dev, "Failed to write page %d: %d\n", idx, error);
+		return error;
+	}
+
+	/* Wait for F/W to update one page ROM data. */
+	msleep(20);
+
+	error = elan_i2c_read_cmd(client, ETP_I2C_IAP_CTRL_CMD, val);
+	if (error) {
+		dev_err(dev, "Failed to read IAP write result: %d\n", error);
+		return error;
+	}
+
+	result = le16_to_cpup((__le16 *)val);
+	if (result & (ETP_FW_IAP_PAGE_ERR | ETP_FW_IAP_INTF_ERR)) {
+		dev_err(dev, "IAP reports failed write: %04hx\n",
+			result);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int elan_i2c_finish_fw_update(struct i2c_client *client,
+				     struct completion *completion)
+{
+	struct device *dev = &client->dev;
+	long ret;
+	int error;
+	int len;
+	u8 buffer[ETP_I2C_INF_LENGTH];
+
+	reinit_completion(completion);
+	enable_irq(client->irq);
+
+	error = elan_i2c_write_cmd(client, ETP_I2C_STAND_CMD, ETP_I2C_RESET);
+	if (!error)
+		ret = wait_for_completion_interruptible_timeout(completion,
+							msecs_to_jiffies(300));
+	disable_irq(client->irq);
+
+	if (error) {
+		dev_err(dev, "device reset failed: %d\n", error);
+		return error;
+	} else if (ret == 0) {
+		dev_err(dev, "timeout waiting for device reset\n");
+		return -ETIMEDOUT;
+	} else if (ret < 0) {
+		error = ret;
+		dev_err(dev, "error waiting for device reset: %d\n", error);
+		return error;
+	}
+
+	len = i2c_master_recv(client, buffer, ETP_I2C_INF_LENGTH);
+	if (len != ETP_I2C_INF_LENGTH) {
+		error = len < 0 ? len : -EIO;
+		dev_err(dev, "failed to read INT signal: %d (%d)\n",
+			error, len);
+		return error;
+	}
+
+	return 0;
+}
+
+static int elan_i2c_get_report(struct i2c_client *client, u8 *report)
+{
+	int len;
+
+	len = i2c_master_recv(client, report, ETP_I2C_REPORT_LEN);
+	if (len < 0) {
+		dev_err(&client->dev, "failed to read report data: %d\n", len);
+		return len;
+	}
+
+	if (len != ETP_I2C_REPORT_LEN) {
+		dev_err(&client->dev,
+			"wrong report length (%d vs %d expected)\n",
+			len, ETP_I2C_REPORT_LEN);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+const struct elan_transport_ops elan_i2c_ops = {
+	.initialize		= elan_i2c_initialize,
+	.sleep_control		= elan_i2c_sleep_control,
+	.power_control		= elan_i2c_power_control,
+	.set_mode		= elan_i2c_set_mode,
+
+	.calibrate		= elan_i2c_calibrate,
+	.calibrate_result	= elan_i2c_calibrate_result,
+
+	.get_baseline_data	= elan_i2c_get_baseline_data,
+
+	.get_version		= elan_i2c_get_version,
+	.get_sm_version		= elan_i2c_get_sm_version,
+	.get_product_id		= elan_i2c_get_product_id,
+	.get_checksum		= elan_i2c_get_checksum,
+
+	.get_max		= elan_i2c_get_max,
+	.get_resolution		= elan_i2c_get_resolution,
+	.get_num_traces		= elan_i2c_get_num_traces,
+
+	.iap_get_mode		= elan_i2c_iap_get_mode,
+	.iap_reset		= elan_i2c_iap_reset,
+
+	.prepare_fw_update	= elan_i2c_prepare_fw_update,
+	.write_fw_block		= elan_i2c_write_fw_block,
+	.finish_fw_update	= elan_i2c_finish_fw_update,
+
+	.get_report		= elan_i2c_get_report,
+};
diff --git a/drivers/input/mouse/elan_i2c_smbus.c b/drivers/input/mouse/elan_i2c_smbus.c
new file mode 100644
index 000000000000..359bf8583d54
--- /dev/null
+++ b/drivers/input/mouse/elan_i2c_smbus.c
@@ -0,0 +1,514 @@
+/*
+ * Elan I2C/SMBus Touchpad driver - SMBus interface
+ *
+ * Copyright (c) 2013 ELAN Microelectronics Corp.
+ *
+ * Author: 林政維 (Duson Lin) <dusonlin@emc.com.tw>
+ * Version: 1.5.5
+ *
+ * Based on cyapa driver:
+ * copyright (c) 2011-2012 Cypress Semiconductor, Inc.
+ * copyright (c) 2011-2012 Google, 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.
+ *
+ * Trademarks are the property of their respective owners.
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+
+#include "elan_i2c.h"
+
+/* Elan SMbus commands */
+#define ETP_SMBUS_IAP_CMD		0x00
+#define ETP_SMBUS_ENABLE_TP		0x20
+#define ETP_SMBUS_SLEEP_CMD		0x21
+#define ETP_SMBUS_IAP_PASSWORD_WRITE	0x29
+#define ETP_SMBUS_IAP_PASSWORD_READ	0x80
+#define ETP_SMBUS_WRITE_FW_BLOCK	0x2A
+#define ETP_SMBUS_IAP_RESET_CMD		0x2B
+#define ETP_SMBUS_RANGE_CMD		0xA0
+#define ETP_SMBUS_FW_VERSION_CMD	0xA1
+#define ETP_SMBUS_XY_TRACENUM_CMD	0xA2
+#define ETP_SMBUS_SM_VERSION_CMD	0xA3
+#define ETP_SMBUS_UNIQUEID_CMD		0xA3
+#define ETP_SMBUS_RESOLUTION_CMD	0xA4
+#define ETP_SMBUS_HELLOPACKET_CMD	0xA7
+#define ETP_SMBUS_PACKET_QUERY		0xA8
+#define ETP_SMBUS_IAP_VERSION_CMD	0xAC
+#define ETP_SMBUS_IAP_CTRL_CMD		0xAD
+#define ETP_SMBUS_IAP_CHECKSUM_CMD	0xAE
+#define ETP_SMBUS_FW_CHECKSUM_CMD	0xAF
+#define ETP_SMBUS_MAX_BASELINE_CMD	0xC3
+#define ETP_SMBUS_MIN_BASELINE_CMD	0xC4
+#define ETP_SMBUS_CALIBRATE_QUERY	0xC5
+
+#define ETP_SMBUS_REPORT_LEN		32
+#define ETP_SMBUS_REPORT_OFFSET		2
+#define ETP_SMBUS_HELLOPACKET_LEN	5
+#define ETP_SMBUS_IAP_PASSWORD		0x1234
+#define ETP_SMBUS_IAP_MODE_ON		(1 << 6)
+
+static int elan_smbus_initialize(struct i2c_client *client)
+{
+	u8 check[ETP_SMBUS_HELLOPACKET_LEN] = { 0x55, 0x55, 0x55, 0x55, 0x55 };
+	u8 values[ETP_SMBUS_HELLOPACKET_LEN] = { 0, 0, 0, 0, 0 };
+	int len, error;
+
+	/* Get hello packet */
+	len = i2c_smbus_read_block_data(client,
+					ETP_SMBUS_HELLOPACKET_CMD, values);
+	if (len != ETP_SMBUS_HELLOPACKET_LEN) {
+		dev_err(&client->dev, "hello packet length fail: %d\n", len);
+		error = len < 0 ? len : -EIO;
+		return error;
+	}
+
+	/* compare hello packet */
+	if (memcmp(values, check, ETP_SMBUS_HELLOPACKET_LEN)) {
+		dev_err(&client->dev, "hello packet fail [%*px]\n",
+			ETP_SMBUS_HELLOPACKET_LEN, values);
+		return -ENXIO;
+	}
+
+	/* enable tp */
+	error = i2c_smbus_write_byte(client, ETP_SMBUS_ENABLE_TP);
+	if (error) {
+		dev_err(&client->dev, "failed to enable touchpad: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int elan_smbus_set_mode(struct i2c_client *client, u8 mode)
+{
+	u8 cmd[4] = { 0x00, 0x07, 0x00, mode };
+
+	return i2c_smbus_write_block_data(client, ETP_SMBUS_IAP_CMD,
+					  sizeof(cmd), cmd);
+}
+
+static int elan_smbus_sleep_control(struct i2c_client *client, bool sleep)
+{
+	if (sleep)
+		return i2c_smbus_write_byte(client, ETP_SMBUS_SLEEP_CMD);
+	else
+		return 0; /* XXX should we send ETP_SMBUS_ENABLE_TP here? */
+}
+
+static int elan_smbus_power_control(struct i2c_client *client, bool enable)
+{
+	return 0; /* A no-op */
+}
+
+static int elan_smbus_calibrate(struct i2c_client *client)
+{
+	u8 cmd[4] = { 0x00, 0x08, 0x00, 0x01 };
+
+	return i2c_smbus_write_block_data(client, ETP_SMBUS_IAP_CMD,
+					  sizeof(cmd), cmd);
+}
+
+static int elan_smbus_calibrate_result(struct i2c_client *client, u8 *val)
+{
+	int error;
+
+	error = i2c_smbus_read_block_data(client,
+					  ETP_SMBUS_CALIBRATE_QUERY, val);
+	if (error < 0)
+		return error;
+
+	return 0;
+}
+
+static int elan_smbus_get_baseline_data(struct i2c_client *client,
+					bool max_baseline, u8 *value)
+{
+	int error;
+	u8 val[3];
+
+	error = i2c_smbus_read_block_data(client,
+					  max_baseline ?
+						ETP_SMBUS_MAX_BASELINE_CMD :
+						ETP_SMBUS_MIN_BASELINE_CMD,
+					  val);
+	if (error < 0)
+		return error;
+
+	*value = be16_to_cpup((__be16 *)val);
+
+	return 0;
+}
+
+static int elan_smbus_get_version(struct i2c_client *client,
+				  bool iap, u8 *version)
+{
+	int error;
+	u8 val[3];
+
+	error = i2c_smbus_read_block_data(client,
+					  iap ? ETP_SMBUS_IAP_VERSION_CMD :
+						ETP_SMBUS_FW_VERSION_CMD,
+					  val);
+	if (error < 0) {
+		dev_err(&client->dev, "failed to get %s version: %d\n",
+			iap ? "IAP" : "FW", error);
+		return error;
+	}
+
+	*version = val[2];
+	return 0;
+}
+
+static int elan_smbus_get_sm_version(struct i2c_client *client, u8 *version)
+{
+	int error;
+	u8 val[3];
+
+	error = i2c_smbus_read_block_data(client,
+					  ETP_SMBUS_SM_VERSION_CMD, val);
+	if (error < 0) {
+		dev_err(&client->dev, "failed to get SM version: %d\n", error);
+		return error;
+	}
+
+	*version = val[0]; /* XXX Why 0 and not 2 as in IAP/FW versions? */
+	return 0;
+}
+
+static int elan_smbus_get_product_id(struct i2c_client *client, u8 *id)
+{
+	int error;
+	u8 val[3];
+
+	error = i2c_smbus_read_block_data(client,
+					  ETP_SMBUS_UNIQUEID_CMD, val);
+	if (error < 0) {
+		dev_err(&client->dev, "failed to get product ID: %d\n", error);
+		return error;
+	}
+
+	*id = val[1];
+	return 0;
+}
+
+static int elan_smbus_get_checksum(struct i2c_client *client,
+				   bool iap, u16 *csum)
+{
+	int error;
+	u8 val[3];
+
+	error = i2c_smbus_read_block_data(client,
+					  iap ? ETP_SMBUS_FW_CHECKSUM_CMD :
+						ETP_SMBUS_IAP_CHECKSUM_CMD,
+					  val);
+	if (error < 0) {
+		dev_err(&client->dev, "failed to get %s checksum: %d\n",
+			iap ? "IAP" : "FW", error);
+		return error;
+	}
+
+	*csum = be16_to_cpup((__be16 *)val);
+	return 0;
+}
+
+static int elan_smbus_get_max(struct i2c_client *client,
+			      unsigned int *max_x, unsigned int *max_y)
+{
+	int error;
+	u8 val[3];
+
+	error = i2c_smbus_read_block_data(client, ETP_SMBUS_RANGE_CMD, val);
+	if (error) {
+		dev_err(&client->dev, "failed to get dimensions: %d\n", error);
+		return error;
+	}
+
+	*max_x = (0x0f & val[0]) << 8 | val[1];
+	*max_y = (0xf0 & val[0]) << 4 | val[2];
+
+	return 0;
+}
+
+static int elan_smbus_get_resolution(struct i2c_client *client,
+				     u8 *hw_res_x, u8 *hw_res_y)
+{
+	int error;
+	u8 val[3];
+
+	error = i2c_smbus_read_block_data(client,
+					  ETP_SMBUS_RESOLUTION_CMD, val);
+	if (error) {
+		dev_err(&client->dev, "failed to get resolution: %d\n", error);
+		return error;
+	}
+
+	*hw_res_x = val[1] & 0x0F;
+	*hw_res_y = (val[1] & 0xF0) >> 4;
+
+	return 0;
+}
+
+static int elan_smbus_get_num_traces(struct i2c_client *client,
+				     unsigned int *x_traces,
+				     unsigned int *y_traces)
+{
+	int error;
+	u8 val[3];
+
+	error = i2c_smbus_read_block_data(client,
+					  ETP_SMBUS_XY_TRACENUM_CMD, val);
+	if (error) {
+		dev_err(&client->dev, "failed to get trace info: %d\n", error);
+		return error;
+	}
+
+	*x_traces = val[1] - 1;
+	*y_traces = val[2] - 1;
+
+	return 0;
+}
+
+static int elan_smbus_iap_get_mode(struct i2c_client *client,
+				   enum tp_mode *mode)
+{
+	int error;
+	u16 constant;
+	u8 val[3];
+
+	error = i2c_smbus_read_block_data(client, ETP_SMBUS_IAP_CTRL_CMD, val);
+	if (error < 0) {
+		dev_err(&client->dev, "failed to read iap ctrol register: %d\n",
+			error);
+		return error;
+	}
+
+	constant = be16_to_cpup((__be16 *)val);
+	dev_dbg(&client->dev, "iap control reg: 0x%04x.\n", constant);
+
+	*mode = (constant & ETP_SMBUS_IAP_MODE_ON) ? IAP_MODE : MAIN_MODE;
+
+	return 0;
+}
+
+static int elan_smbus_iap_reset(struct i2c_client *client)
+{
+	int error;
+
+	error = i2c_smbus_write_byte(client, ETP_SMBUS_IAP_RESET_CMD);
+	if (error) {
+		dev_err(&client->dev, "cannot reset IC: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int elan_smbus_set_flash_key(struct i2c_client *client)
+{
+	int error;
+	u8 cmd[4] = { 0x00, 0x0B, 0x00, 0x5A };
+
+	error = i2c_smbus_write_block_data(client, ETP_SMBUS_IAP_CMD,
+					   sizeof(cmd), cmd);
+	if (error) {
+		dev_err(&client->dev, "cannot set flash key: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int elan_smbus_prepare_fw_update(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	int len;
+	int error;
+	enum tp_mode mode;
+	u8 val[3];
+	u8 cmd[4] = {0x0F, 0x78, 0x00, 0x06};
+	u16 password;
+
+	/* Get FW in which mode	(IAP_MODE/MAIN_MODE)  */
+	error = elan_smbus_iap_get_mode(client, &mode);
+	if (error)
+		return error;
+
+	if (mode == MAIN_MODE) {
+
+		/* set flash key */
+		error = elan_smbus_set_flash_key(client);
+		if (error)
+			return error;
+
+		/* write iap password */
+		if (i2c_smbus_write_byte(client,
+					 ETP_SMBUS_IAP_PASSWORD_WRITE) < 0) {
+			dev_err(dev, "cannot write iap password\n");
+			return -EIO;
+		}
+
+		error = i2c_smbus_write_block_data(client, ETP_SMBUS_IAP_CMD,
+						   sizeof(cmd), cmd);
+		if (error) {
+			dev_err(dev, "failed to write iap password: %d\n",
+				error);
+			return error;
+		}
+
+		/*
+		 * Read back password to make sure we enabled flash
+		 * successfully.
+		 */
+		len = i2c_smbus_read_block_data(client,
+						ETP_SMBUS_IAP_PASSWORD_READ,
+						val);
+		if (len < sizeof(u16)) {
+			error = len < 0 ? len : -EIO;
+			dev_err(dev, "failed to read iap password: %d\n",
+				error);
+			return error;
+		}
+
+		password = be16_to_cpup((__be16 *)val);
+		if (password != ETP_SMBUS_IAP_PASSWORD) {
+			dev_err(dev, "wrong iap password = 0x%X\n", password);
+			return -EIO;
+		}
+
+		/* Wait 30ms for MAIN_MODE change to IAP_MODE */
+		msleep(30);
+	}
+
+	error = elan_smbus_set_flash_key(client);
+	if (error)
+		return error;
+
+	/* Reset IC */
+	error = elan_smbus_iap_reset(client);
+	if (error)
+		return error;
+
+	return 0;
+}
+
+
+static int elan_smbus_write_fw_block(struct i2c_client *client,
+				     const u8 *page, u16 checksum, int idx)
+{
+	struct device *dev = &client->dev;
+	int error;
+	u16 result;
+	u8 val[3];
+
+	/*
+	 * Due to the limitation of smbus protocol limiting
+	 * transfer to 32 bytes at a time, we must split block
+	 * in 2 transfers.
+	 */
+	error = i2c_smbus_write_block_data(client,
+					   ETP_SMBUS_WRITE_FW_BLOCK,
+					   ETP_FW_PAGE_SIZE / 2,
+					   page);
+	if (error) {
+		dev_err(dev, "Failed to write page %d (part %d): %d\n",
+			idx, 1, error);
+		return error;
+	}
+
+	error = i2c_smbus_write_block_data(client,
+					   ETP_SMBUS_WRITE_FW_BLOCK,
+					   ETP_FW_PAGE_SIZE / 2,
+					   page + ETP_FW_PAGE_SIZE / 2);
+	if (error) {
+		dev_err(dev, "Failed to write page %d (part %d): %d\n",
+			idx, 2, error);
+		return error;
+	}
+
+
+	/* Wait for F/W to update one page ROM data. */
+	usleep_range(8000, 10000);
+
+	error = i2c_smbus_read_block_data(client,
+					  ETP_SMBUS_IAP_CTRL_CMD, val);
+	if (error < 0) {
+		dev_err(dev, "Failed to read IAP write result: %d\n",
+			error);
+		return error;
+	}
+
+	result = be16_to_cpup((__be16 *)val);
+	if (result & (ETP_FW_IAP_PAGE_ERR | ETP_FW_IAP_INTF_ERR)) {
+		dev_err(dev, "IAP reports failed write: %04hx\n",
+			result);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int elan_smbus_get_report(struct i2c_client *client, u8 *report)
+{
+	int len;
+
+	len = i2c_smbus_read_block_data(client,
+					ETP_SMBUS_PACKET_QUERY,
+					&report[ETP_SMBUS_REPORT_OFFSET]);
+	if (len < 0) {
+		dev_err(&client->dev, "failed to read report data: %d\n", len);
+		return len;
+	}
+
+	if (len != ETP_SMBUS_REPORT_LEN) {
+		dev_err(&client->dev,
+			"wrong report length (%d vs %d expected)\n",
+			len, ETP_SMBUS_REPORT_LEN);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int elan_smbus_finish_fw_update(struct i2c_client *client,
+				       struct completion *fw_completion)
+{
+	/* No special handling unlike I2C transport */
+	return 0;
+}
+
+const struct elan_transport_ops elan_smbus_ops = {
+	.initialize		= elan_smbus_initialize,
+	.sleep_control		= elan_smbus_sleep_control,
+	.power_control		= elan_smbus_power_control,
+	.set_mode		= elan_smbus_set_mode,
+
+	.calibrate		= elan_smbus_calibrate,
+	.calibrate_result	= elan_smbus_calibrate_result,
+
+	.get_baseline_data	= elan_smbus_get_baseline_data,
+
+	.get_version		= elan_smbus_get_version,
+	.get_sm_version		= elan_smbus_get_sm_version,
+	.get_product_id		= elan_smbus_get_product_id,
+	.get_checksum		= elan_smbus_get_checksum,
+
+	.get_max		= elan_smbus_get_max,
+	.get_resolution		= elan_smbus_get_resolution,
+	.get_num_traces		= elan_smbus_get_num_traces,
+
+	.iap_get_mode		= elan_smbus_iap_get_mode,
+	.iap_reset		= elan_smbus_iap_reset,
+
+	.prepare_fw_update	= elan_smbus_prepare_fw_update,
+	.write_fw_block		= elan_smbus_write_fw_block,
+	.finish_fw_update	= elan_smbus_finish_fw_update,
+
+	.get_report		= elan_smbus_get_report,
+};
diff --git a/drivers/input/mouse/gpio_mouse.c b/drivers/input/mouse/gpio_mouse.c
index 8c7d94200bdb..ced07391304b 100644
--- a/drivers/input/mouse/gpio_mouse.c
+++ b/drivers/input/mouse/gpio_mouse.c
@@ -171,7 +171,6 @@ static struct platform_driver gpio_mouse_device_driver = {
 	.remove		= gpio_mouse_remove,
 	.driver		= {
 		.name	= "gpio_mouse",
-		.owner	= THIS_MODULE,
 	}
 };
 module_platform_driver(gpio_mouse_device_driver);
diff --git a/drivers/input/mouse/lifebook.h b/drivers/input/mouse/lifebook.h
index 4c4326c6f504..0baf02a70a99 100644
--- a/drivers/input/mouse/lifebook.h
+++ b/drivers/input/mouse/lifebook.h
@@ -16,14 +16,14 @@ void lifebook_module_init(void);
 int lifebook_detect(struct psmouse *psmouse, bool set_properties);
 int lifebook_init(struct psmouse *psmouse);
 #else
-inline void lifebook_module_init(void)
+static inline void lifebook_module_init(void)
 {
 }
-inline int lifebook_detect(struct psmouse *psmouse, bool set_properties)
+static inline int lifebook_detect(struct psmouse *psmouse, bool set_properties)
 {
 	return -ENOSYS;
 }
-inline int lifebook_init(struct psmouse *psmouse)
+static inline int lifebook_init(struct psmouse *psmouse)
 {
 	return -ENOSYS;
 }
diff --git a/drivers/input/mouse/navpoint.c b/drivers/input/mouse/navpoint.c
index 1ccc88af1f0b..d6e8f58a1de3 100644
--- a/drivers/input/mouse/navpoint.c
+++ b/drivers/input/mouse/navpoint.c
@@ -318,8 +318,7 @@ static int navpoint_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int navpoint_suspend(struct device *dev)
+static int __maybe_unused navpoint_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct navpoint *navpoint = platform_get_drvdata(pdev);
@@ -333,7 +332,7 @@ static int navpoint_suspend(struct device *dev)
 	return 0;
 }
 
-static int navpoint_resume(struct device *dev)
+static int __maybe_unused navpoint_resume(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct navpoint *navpoint = platform_get_drvdata(pdev);
@@ -346,7 +345,6 @@ static int navpoint_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(navpoint_pm_ops, navpoint_suspend, navpoint_resume);
 
@@ -355,7 +353,6 @@ static struct platform_driver navpoint_driver = {
 	.remove		= navpoint_remove,
 	.driver = {
 		.name	= "navpoint",
-		.owner	= THIS_MODULE,
 		.pm	= &navpoint_pm_ops,
 	},
 };
diff --git a/drivers/input/mouse/synaptics_i2c.c b/drivers/input/mouse/synaptics_i2c.c
index ad822608f6ee..878f18498f3b 100644
--- a/drivers/input/mouse/synaptics_i2c.c
+++ b/drivers/input/mouse/synaptics_i2c.c
@@ -614,8 +614,7 @@ static int synaptics_i2c_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int synaptics_i2c_suspend(struct device *dev)
+static int __maybe_unused synaptics_i2c_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct synaptics_i2c *touch = i2c_get_clientdata(client);
@@ -628,7 +627,7 @@ static int synaptics_i2c_suspend(struct device *dev)
 	return 0;
 }
 
-static int synaptics_i2c_resume(struct device *dev)
+static int __maybe_unused synaptics_i2c_resume(struct device *dev)
 {
 	int ret;
 	struct i2c_client *client = to_i2c_client(dev);
@@ -643,7 +642,6 @@ static int synaptics_i2c_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(synaptics_i2c_pm, synaptics_i2c_suspend,
 			 synaptics_i2c_resume);
diff --git a/drivers/input/serio/altera_ps2.c b/drivers/input/serio/altera_ps2.c
index 58781c8a8aec..131d7826dc6b 100644
--- a/drivers/input/serio/altera_ps2.c
+++ b/drivers/input/serio/altera_ps2.c
@@ -24,9 +24,7 @@
 
 struct ps2if {
 	struct serio *io;
-	struct resource *iomem_res;
 	void __iomem *base;
-	unsigned irq;
 };
 
 /*
@@ -83,16 +81,34 @@ static void altera_ps2_close(struct serio *io)
 static int altera_ps2_probe(struct platform_device *pdev)
 {
 	struct ps2if *ps2if;
+	struct resource *res;
 	struct serio *serio;
 	int error, irq;
 
-	ps2if = kzalloc(sizeof(struct ps2if), GFP_KERNEL);
-	serio = kzalloc(sizeof(struct serio), GFP_KERNEL);
-	if (!ps2if || !serio) {
-		error = -ENOMEM;
-		goto err_free_mem;
+	ps2if = devm_kzalloc(&pdev->dev, sizeof(struct ps2if), GFP_KERNEL);
+	if (!ps2if)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	ps2if->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(ps2if->base))
+		return PTR_ERR(ps2if->base);
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return -ENXIO;
+
+	error = devm_request_irq(&pdev->dev, irq, altera_ps2_rxint, 0,
+				 pdev->name, ps2if);
+	if (error) {
+		dev_err(&pdev->dev, "could not request IRQ %d\n", irq);
+		return error;
 	}
 
+	serio = kzalloc(sizeof(struct serio), GFP_KERNEL);
+	if (!serio)
+		return -ENOMEM;
+
 	serio->id.type		= SERIO_8042;
 	serio->write		= altera_ps2_write;
 	serio->open		= altera_ps2_open;
@@ -103,56 +119,12 @@ static int altera_ps2_probe(struct platform_device *pdev)
 	serio->dev.parent	= &pdev->dev;
 	ps2if->io		= serio;
 
-	ps2if->iomem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (ps2if->iomem_res == NULL) {
-		error = -ENOENT;
-		goto err_free_mem;
-	}
-
-
-	irq = platform_get_irq(pdev, 0);
-	if (irq < 0) {
-		error = -ENXIO;
-		goto err_free_mem;
-	}
-	ps2if->irq = irq;
-
-	if (!request_mem_region(ps2if->iomem_res->start,
-				resource_size(ps2if->iomem_res), pdev->name)) {
-		error = -EBUSY;
-		goto err_free_mem;
-	}
-
-	ps2if->base = ioremap(ps2if->iomem_res->start,
-			      resource_size(ps2if->iomem_res));
-	if (!ps2if->base) {
-		error = -ENOMEM;
-		goto err_free_res;
-	}
-
-	error = request_irq(ps2if->irq, altera_ps2_rxint, 0, pdev->name, ps2if);
-	if (error) {
-		dev_err(&pdev->dev, "could not allocate IRQ %d: %d\n",
-			ps2if->irq, error);
-		goto err_unmap;
-	}
-
-	dev_info(&pdev->dev, "base %p, irq %d\n", ps2if->base, ps2if->irq);
+	dev_info(&pdev->dev, "base %p, irq %d\n", ps2if->base, irq);
 
 	serio_register_port(ps2if->io);
 	platform_set_drvdata(pdev, ps2if);
 
 	return 0;
-
- err_unmap:
-	iounmap(ps2if->base);
- err_free_res:
-	release_mem_region(ps2if->iomem_res->start,
-			   resource_size(ps2if->iomem_res));
- err_free_mem:
-	kfree(ps2if);
-	kfree(serio);
-	return error;
 }
 
 /*
@@ -163,11 +135,6 @@ static int altera_ps2_remove(struct platform_device *pdev)
 	struct ps2if *ps2if = platform_get_drvdata(pdev);
 
 	serio_unregister_port(ps2if->io);
-	free_irq(ps2if->irq, ps2if);
-	iounmap(ps2if->base);
-	release_mem_region(ps2if->iomem_res->start,
-			   resource_size(ps2if->iomem_res));
-	kfree(ps2if);
 
 	return 0;
 }
@@ -189,7 +156,6 @@ static struct platform_driver altera_ps2_driver = {
 	.remove		= altera_ps2_remove,
 	.driver	= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(altera_ps2_match),
 	},
 };
diff --git a/drivers/input/serio/apbps2.c b/drivers/input/serio/apbps2.c
index 98be824544a5..45d4e08ca4f8 100644
--- a/drivers/input/serio/apbps2.c
+++ b/drivers/input/serio/apbps2.c
@@ -214,7 +214,6 @@ MODULE_DEVICE_TABLE(of, apbps2_of_match);
 static struct platform_driver apbps2_of_driver = {
 	.driver = {
 		.name = "grlib-apbps2",
-		.owner = THIS_MODULE,
 		.of_match_table = apbps2_of_match,
 	},
 	.probe = apbps2_of_probe,
diff --git a/drivers/input/serio/arc_ps2.c b/drivers/input/serio/arc_ps2.c
index 8024a6d7fccb..99e57a418753 100644
--- a/drivers/input/serio/arc_ps2.c
+++ b/drivers/input/serio/arc_ps2.c
@@ -266,7 +266,6 @@ MODULE_DEVICE_TABLE(of, arc_ps2_match);
 static struct platform_driver arc_ps2_driver = {
 	.driver	= {
 		.name		= "arc_ps2",
-		.owner		= THIS_MODULE,
 		.of_match_table	= of_match_ptr(arc_ps2_match),
 	},
 	.probe	= arc_ps2_probe,
diff --git a/drivers/input/serio/at32psif.c b/drivers/input/serio/at32psif.c
index 3290b287ac4b..2e4ff5bac754 100644
--- a/drivers/input/serio/at32psif.c
+++ b/drivers/input/serio/at32psif.c
@@ -352,7 +352,6 @@ static struct platform_driver psif_driver = {
 	.remove		= __exit_p(psif_remove),
 	.driver		= {
 		.name	= "atmel_psif",
-		.owner	= THIS_MODULE,
 		.pm	= &psif_pm_ops,
 	},
 };
diff --git a/drivers/input/serio/ct82c710.c b/drivers/input/serio/ct82c710.c
index cfe549d4eaa5..9c54c43c9749 100644
--- a/drivers/input/serio/ct82c710.c
+++ b/drivers/input/serio/ct82c710.c
@@ -209,7 +209,6 @@ static int ct82c710_remove(struct platform_device *dev)
 static struct platform_driver ct82c710_driver = {
 	.driver		= {
 		.name	= "ct82c710",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= ct82c710_probe,
 	.remove		= ct82c710_remove,
diff --git a/drivers/input/serio/i8042-sparcio.h b/drivers/input/serio/i8042-sparcio.h
index 93cb7912703c..afcd1c1a05b2 100644
--- a/drivers/input/serio/i8042-sparcio.h
+++ b/drivers/input/serio/i8042-sparcio.h
@@ -99,7 +99,6 @@ MODULE_DEVICE_TABLE(of, sparc_i8042_match);
 static struct platform_driver sparc_i8042_driver = {
 	.driver = {
 		.name = "i8042",
-		.owner = THIS_MODULE,
 		.of_match_table = sparc_i8042_match,
 	},
 	.probe		= sparc_i8042_probe,
diff --git a/drivers/input/serio/i8042-x86ia64io.h b/drivers/input/serio/i8042-x86ia64io.h
index faeeb1372462..c66d1b53843e 100644
--- a/drivers/input/serio/i8042-x86ia64io.h
+++ b/drivers/input/serio/i8042-x86ia64io.h
@@ -579,6 +579,16 @@ static const struct dmi_system_id __initconst i8042_dmi_nopnp_table[] = {
 		},
 	},
 	{
+		/*
+		 * Intel NUC D54250WYK - does not have i8042 controller but
+		 * declares PS/2 devices in DSDT.
+		 */
+		.matches = {
+			DMI_MATCH(DMI_BOARD_NAME, "D54250WYK"),
+			DMI_MATCH(DMI_BOARD_VENDOR, "Intel Corporation"),
+		},
+	},
+	{
 		/* MSI Wind U-100 */
 		.matches = {
 			DMI_MATCH(DMI_BOARD_NAME, "U-100"),
diff --git a/drivers/input/serio/i8042.c b/drivers/input/serio/i8042.c
index f5a98af3b325..924e4bf357fb 100644
--- a/drivers/input/serio/i8042.c
+++ b/drivers/input/serio/i8042.c
@@ -1463,7 +1463,6 @@ static int i8042_remove(struct platform_device *dev)
 static struct platform_driver i8042_driver = {
 	.driver		= {
 		.name	= "i8042",
-		.owner	= THIS_MODULE,
 #ifdef CONFIG_PM
 		.pm	= &i8042_pm_ops,
 #endif
diff --git a/drivers/input/serio/maceps2.c b/drivers/input/serio/maceps2.c
index bc85e1cc66d8..e365c5f4cbc9 100644
--- a/drivers/input/serio/maceps2.c
+++ b/drivers/input/serio/maceps2.c
@@ -162,7 +162,6 @@ static int maceps2_remove(struct platform_device *dev)
 static struct platform_driver maceps2_driver = {
 	.driver		= {
 		.name	= "maceps2",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= maceps2_probe,
 	.remove		= maceps2_remove,
diff --git a/drivers/input/serio/olpc_apsp.c b/drivers/input/serio/olpc_apsp.c
index d906f3ebc8c8..8e9a4209fcad 100644
--- a/drivers/input/serio/olpc_apsp.c
+++ b/drivers/input/serio/olpc_apsp.c
@@ -273,7 +273,6 @@ static struct platform_driver olpc_apsp_driver = {
 	.remove		= olpc_apsp_remove,
 	.driver		= {
 		.name	= "olpc-apsp",
-		.owner	= THIS_MODULE,
 		.of_match_table = olpc_apsp_dt_ids,
 	},
 };
diff --git a/drivers/input/serio/q40kbd.c b/drivers/input/serio/q40kbd.c
index 594256c38554..5a9d521510bf 100644
--- a/drivers/input/serio/q40kbd.c
+++ b/drivers/input/serio/q40kbd.c
@@ -186,7 +186,6 @@ static int q40kbd_remove(struct platform_device *pdev)
 static struct platform_driver q40kbd_driver = {
 	.driver		= {
 		.name	= "q40kbd",
-		.owner	= THIS_MODULE,
 	},
 	.remove		= q40kbd_remove,
 };
diff --git a/drivers/input/serio/rpckbd.c b/drivers/input/serio/rpckbd.c
index e462e7791bb8..8cf964736902 100644
--- a/drivers/input/serio/rpckbd.c
+++ b/drivers/input/serio/rpckbd.c
@@ -168,7 +168,6 @@ static struct platform_driver rpckbd_driver = {
 	.remove		= rpckbd_remove,
 	.driver		= {
 		.name	= "kart",
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(rpckbd_driver);
diff --git a/drivers/input/serio/serio.c b/drivers/input/serio/serio.c
index d399b8b0f000..a05a5179da32 100644
--- a/drivers/input/serio/serio.c
+++ b/drivers/input/serio/serio.c
@@ -514,7 +514,7 @@ static void serio_release_port(struct device *dev)
  */
 static void serio_init_port(struct serio *serio)
 {
-	static atomic_t serio_no = ATOMIC_INIT(0);
+	static atomic_t serio_no = ATOMIC_INIT(-1);
 
 	__module_get(THIS_MODULE);
 
@@ -525,7 +525,7 @@ static void serio_init_port(struct serio *serio)
 	mutex_init(&serio->drv_mutex);
 	device_initialize(&serio->dev);
 	dev_set_name(&serio->dev, "serio%lu",
-		     (unsigned long)atomic_inc_return(&serio_no) - 1);
+		     (unsigned long)atomic_inc_return(&serio_no));
 	serio->dev.bus = &serio_bus;
 	serio->dev.release = serio_release_port;
 	serio->dev.groups = serio_device_attr_groups;
diff --git a/drivers/input/serio/serio_raw.c b/drivers/input/serio/serio_raw.c
index c9a02fe57576..71ef5d65a0c6 100644
--- a/drivers/input/serio/serio_raw.c
+++ b/drivers/input/serio/serio_raw.c
@@ -292,7 +292,7 @@ static irqreturn_t serio_raw_interrupt(struct serio *serio, unsigned char data,
 
 static int serio_raw_connect(struct serio *serio, struct serio_driver *drv)
 {
-	static atomic_t serio_raw_no = ATOMIC_INIT(0);
+	static atomic_t serio_raw_no = ATOMIC_INIT(-1);
 	struct serio_raw *serio_raw;
 	int err;
 
@@ -303,7 +303,7 @@ static int serio_raw_connect(struct serio *serio, struct serio_driver *drv)
 	}
 
 	snprintf(serio_raw->name, sizeof(serio_raw->name),
-		 "serio_raw%ld", (long)atomic_inc_return(&serio_raw_no) - 1);
+		 "serio_raw%ld", (long)atomic_inc_return(&serio_raw_no));
 	kref_init(&serio_raw->kref);
 	INIT_LIST_HEAD(&serio_raw->client_list);
 	init_waitqueue_head(&serio_raw->wait);
diff --git a/drivers/input/serio/xilinx_ps2.c b/drivers/input/serio/xilinx_ps2.c
index e6cf52ebad87..5223cbf94262 100644
--- a/drivers/input/serio/xilinx_ps2.c
+++ b/drivers/input/serio/xilinx_ps2.c
@@ -363,7 +363,6 @@ MODULE_DEVICE_TABLE(of, xps2_of_match);
 static struct platform_driver xps2_of_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = xps2_of_match,
 	},
 	.probe		= xps2_of_probe,
diff --git a/drivers/input/touchscreen/88pm860x-ts.c b/drivers/input/touchscreen/88pm860x-ts.c
index 0d4a9fad4a78..251ff2aa0633 100644
--- a/drivers/input/touchscreen/88pm860x-ts.c
+++ b/drivers/input/touchscreen/88pm860x-ts.c
@@ -292,7 +292,6 @@ static int pm860x_touch_probe(struct platform_device *pdev)
 static struct platform_driver pm860x_touch_driver = {
 	.driver	= {
 		.name	= "88pm860x-touch",
-		.owner	= THIS_MODULE,
 	},
 	.probe	= pm860x_touch_probe,
 };
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index e1d8003d01f8..58917525126e 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -295,6 +295,19 @@ config TOUCHSCREEN_FUJITSU
 	  To compile this driver as a module, choose M here: the
 	  module will be called fujitsu-ts.
 
+config TOUCHSCREEN_GOODIX
+	tristate "Goodix I2C touchscreen"
+	depends on I2C && ACPI
+	help
+	  Say Y here if you have the Goodix touchscreen (such as one
+	  installed in Onda v975w tablets) connected to your
+	  system.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called goodix.
+
 config TOUCHSCREEN_ILI210X
 	tristate "Ilitek ILI210X based touchscreen"
 	depends on I2C
@@ -334,6 +347,18 @@ config TOUCHSCREEN_GUNZE
 	  To compile this driver as a module, choose M here: the
 	  module will be called gunze.
 
+config TOUCHSCREEN_ELAN
+	tristate "Elan eKTH I2C touchscreen"
+	depends on I2C
+	help
+	  Say Y here if you have an Elan eKTH I2C touchscreen
+	  connected to your system.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called elants_i2c.
+
 config TOUCHSCREEN_ELO
 	tristate "Elo serial touchscreens"
 	select SERIO
diff --git a/drivers/input/touchscreen/Makefile b/drivers/input/touchscreen/Makefile
index 090e61cc9171..0242fea2102a 100644
--- a/drivers/input/touchscreen/Makefile
+++ b/drivers/input/touchscreen/Makefile
@@ -31,9 +31,11 @@ obj-$(CONFIG_TOUCHSCREEN_EDT_FT5X06)	+= edt-ft5x06.o
 obj-$(CONFIG_TOUCHSCREEN_HAMPSHIRE)	+= hampshire.o
 obj-$(CONFIG_TOUCHSCREEN_GUNZE)		+= gunze.o
 obj-$(CONFIG_TOUCHSCREEN_EETI)		+= eeti_ts.o
+obj-$(CONFIG_TOUCHSCREEN_ELAN)		+= elants_i2c.o
 obj-$(CONFIG_TOUCHSCREEN_ELO)		+= elo.o
 obj-$(CONFIG_TOUCHSCREEN_EGALAX)	+= egalax_ts.o
 obj-$(CONFIG_TOUCHSCREEN_FUJITSU)	+= fujitsu_ts.o
+obj-$(CONFIG_TOUCHSCREEN_GOODIX)	+= goodix.o
 obj-$(CONFIG_TOUCHSCREEN_ILI210X)	+= ili210x.o
 obj-$(CONFIG_TOUCHSCREEN_INEXIO)	+= inexio.o
 obj-$(CONFIG_TOUCHSCREEN_INTEL_MID)	+= intel-mid-touch.o
diff --git a/drivers/input/touchscreen/ad7877.c b/drivers/input/touchscreen/ad7877.c
index 523865daa1d3..da4e5bb5e045 100644
--- a/drivers/input/touchscreen/ad7877.c
+++ b/drivers/input/touchscreen/ad7877.c
@@ -820,8 +820,7 @@ static int ad7877_remove(struct spi_device *spi)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int ad7877_suspend(struct device *dev)
+static int __maybe_unused ad7877_suspend(struct device *dev)
 {
 	struct ad7877 *ts = dev_get_drvdata(dev);
 
@@ -830,7 +829,7 @@ static int ad7877_suspend(struct device *dev)
 	return 0;
 }
 
-static int ad7877_resume(struct device *dev)
+static int __maybe_unused ad7877_resume(struct device *dev)
 {
 	struct ad7877 *ts = dev_get_drvdata(dev);
 
@@ -838,7 +837,6 @@ static int ad7877_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(ad7877_pm, ad7877_suspend, ad7877_resume);
 
diff --git a/drivers/input/touchscreen/ad7879.c b/drivers/input/touchscreen/ad7879.c
index 1eb9d3c20886..fec66ad80513 100644
--- a/drivers/input/touchscreen/ad7879.c
+++ b/drivers/input/touchscreen/ad7879.c
@@ -284,8 +284,7 @@ static void ad7879_close(struct input_dev* input)
 		__ad7879_disable(ts);
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int ad7879_suspend(struct device *dev)
+static int __maybe_unused ad7879_suspend(struct device *dev)
 {
 	struct ad7879 *ts = dev_get_drvdata(dev);
 
@@ -301,7 +300,7 @@ static int ad7879_suspend(struct device *dev)
 	return 0;
 }
 
-static int ad7879_resume(struct device *dev)
+static int __maybe_unused ad7879_resume(struct device *dev)
 {
 	struct ad7879 *ts = dev_get_drvdata(dev);
 
@@ -316,7 +315,6 @@ static int ad7879_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 SIMPLE_DEV_PM_OPS(ad7879_pm_ops, ad7879_suspend, ad7879_resume);
 EXPORT_SYMBOL(ad7879_pm_ops);
diff --git a/drivers/input/touchscreen/ads7846.c b/drivers/input/touchscreen/ads7846.c
index e57ba52bf484..e4eb8a6c658f 100644
--- a/drivers/input/touchscreen/ads7846.c
+++ b/drivers/input/touchscreen/ads7846.c
@@ -883,8 +883,7 @@ static irqreturn_t ads7846_irq(int irq, void *handle)
 	return IRQ_HANDLED;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int ads7846_suspend(struct device *dev)
+static int __maybe_unused ads7846_suspend(struct device *dev)
 {
 	struct ads7846 *ts = dev_get_drvdata(dev);
 
@@ -906,7 +905,7 @@ static int ads7846_suspend(struct device *dev)
 	return 0;
 }
 
-static int ads7846_resume(struct device *dev)
+static int __maybe_unused ads7846_resume(struct device *dev)
 {
 	struct ads7846 *ts = dev_get_drvdata(dev);
 
@@ -927,7 +926,6 @@ static int ads7846_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(ads7846_pm, ads7846_suspend, ads7846_resume);
 
diff --git a/drivers/input/touchscreen/atmel-wm97xx.c b/drivers/input/touchscreen/atmel-wm97xx.c
index 279c0e42b8a7..7ec0421c0dd8 100644
--- a/drivers/input/touchscreen/atmel-wm97xx.c
+++ b/drivers/input/touchscreen/atmel-wm97xx.c
@@ -425,7 +425,6 @@ static struct platform_driver atmel_wm97xx_driver = {
 	.remove		= __exit_p(atmel_wm97xx_remove),
 	.driver		= {
 		.name	= "wm97xx-touch",
-		.owner	= THIS_MODULE,
 		.pm	= &atmel_wm97xx_pm_ops,
 	},
 };
diff --git a/drivers/input/touchscreen/atmel_mxt_ts.c b/drivers/input/touchscreen/atmel_mxt_ts.c
index aaacf8bfa61f..bb070206223c 100644
--- a/drivers/input/touchscreen/atmel_mxt_ts.c
+++ b/drivers/input/touchscreen/atmel_mxt_ts.c
@@ -2244,8 +2244,7 @@ static int mxt_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int mxt_suspend(struct device *dev)
+static int __maybe_unused mxt_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct mxt_data *data = i2c_get_clientdata(client);
@@ -2261,7 +2260,7 @@ static int mxt_suspend(struct device *dev)
 	return 0;
 }
 
-static int mxt_resume(struct device *dev)
+static int __maybe_unused mxt_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct mxt_data *data = i2c_get_clientdata(client);
@@ -2276,7 +2275,6 @@ static int mxt_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(mxt_pm_ops, mxt_suspend, mxt_resume);
 
diff --git a/drivers/input/touchscreen/auo-pixcir-ts.c b/drivers/input/touchscreen/auo-pixcir-ts.c
index 7f3c94787787..40e02dd5b2f9 100644
--- a/drivers/input/touchscreen/auo-pixcir-ts.c
+++ b/drivers/input/touchscreen/auo-pixcir-ts.c
@@ -417,8 +417,7 @@ static void auo_pixcir_input_close(struct input_dev *dev)
 	return;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int auo_pixcir_suspend(struct device *dev)
+static int __maybe_unused auo_pixcir_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct auo_pixcir_ts *ts = i2c_get_clientdata(client);
@@ -450,7 +449,7 @@ unlock:
 	return ret;
 }
 
-static int auo_pixcir_resume(struct device *dev)
+static int __maybe_unused auo_pixcir_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct auo_pixcir_ts *ts = i2c_get_clientdata(client);
@@ -479,7 +478,6 @@ unlock:
 
 	return ret;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(auo_pixcir_pm_ops,
 			 auo_pixcir_suspend, auo_pixcir_resume);
diff --git a/drivers/input/touchscreen/cy8ctmg110_ts.c b/drivers/input/touchscreen/cy8ctmg110_ts.c
index 5bf1aeeea825..f2119ee0e21b 100644
--- a/drivers/input/touchscreen/cy8ctmg110_ts.c
+++ b/drivers/input/touchscreen/cy8ctmg110_ts.c
@@ -291,8 +291,7 @@ err_free_mem:
 	return err;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int cy8ctmg110_suspend(struct device *dev)
+static int __maybe_unused cy8ctmg110_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct cy8ctmg110 *ts = i2c_get_clientdata(client);
@@ -306,7 +305,7 @@ static int cy8ctmg110_suspend(struct device *dev)
 	return 0;
 }
 
-static int cy8ctmg110_resume(struct device *dev)
+static int __maybe_unused cy8ctmg110_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct cy8ctmg110 *ts = i2c_get_clientdata(client);
@@ -319,7 +318,6 @@ static int cy8ctmg110_resume(struct device *dev)
 	}
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(cy8ctmg110_pm, cy8ctmg110_suspend, cy8ctmg110_resume);
 
diff --git a/drivers/input/touchscreen/cyttsp_core.c b/drivers/input/touchscreen/cyttsp_core.c
index eee656f77a2e..5b74e8b84e79 100644
--- a/drivers/input/touchscreen/cyttsp_core.c
+++ b/drivers/input/touchscreen/cyttsp_core.c
@@ -472,8 +472,7 @@ static int cyttsp_disable(struct cyttsp *ts)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int cyttsp_suspend(struct device *dev)
+static int __maybe_unused cyttsp_suspend(struct device *dev)
 {
 	struct cyttsp *ts = dev_get_drvdata(dev);
 	int retval = 0;
@@ -491,7 +490,7 @@ static int cyttsp_suspend(struct device *dev)
 	return retval;
 }
 
-static int cyttsp_resume(struct device *dev)
+static int __maybe_unused cyttsp_resume(struct device *dev)
 {
 	struct cyttsp *ts = dev_get_drvdata(dev);
 
@@ -507,8 +506,6 @@ static int cyttsp_resume(struct device *dev)
 	return 0;
 }
 
-#endif
-
 SIMPLE_DEV_PM_OPS(cyttsp_pm_ops, cyttsp_suspend, cyttsp_resume);
 EXPORT_SYMBOL_GPL(cyttsp_pm_ops);
 
diff --git a/drivers/input/touchscreen/da9034-ts.c b/drivers/input/touchscreen/da9034-ts.c
index cf6f4b31db4d..8264822dc4b9 100644
--- a/drivers/input/touchscreen/da9034-ts.c
+++ b/drivers/input/touchscreen/da9034-ts.c
@@ -357,7 +357,6 @@ static int da9034_touch_probe(struct platform_device *pdev)
 static struct platform_driver da9034_touch_driver = {
 	.driver	= {
 		.name	= "da9034-touch",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= da9034_touch_probe,
 };
diff --git a/drivers/input/touchscreen/da9052_tsi.c b/drivers/input/touchscreen/da9052_tsi.c
index ab64d58c3ac0..5a013bb7bcad 100644
--- a/drivers/input/touchscreen/da9052_tsi.c
+++ b/drivers/input/touchscreen/da9052_tsi.c
@@ -337,7 +337,6 @@ static struct platform_driver da9052_tsi_driver = {
 	.remove	= da9052_ts_remove,
 	.driver	= {
 		.name	= "da9052-tsi",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/input/touchscreen/edt-ft5x06.c b/drivers/input/touchscreen/edt-ft5x06.c
index ee3434f1e949..3793fcc7e5db 100644
--- a/drivers/input/touchscreen/edt-ft5x06.c
+++ b/drivers/input/touchscreen/edt-ft5x06.c
@@ -1092,8 +1092,7 @@ static int edt_ft5x06_ts_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int edt_ft5x06_ts_suspend(struct device *dev)
+static int __maybe_unused edt_ft5x06_ts_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 
@@ -1103,7 +1102,7 @@ static int edt_ft5x06_ts_suspend(struct device *dev)
 	return 0;
 }
 
-static int edt_ft5x06_ts_resume(struct device *dev)
+static int __maybe_unused edt_ft5x06_ts_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 
@@ -1112,7 +1111,6 @@ static int edt_ft5x06_ts_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(edt_ft5x06_ts_pm_ops,
 			 edt_ft5x06_ts_suspend, edt_ft5x06_ts_resume);
diff --git a/drivers/input/touchscreen/eeti_ts.c b/drivers/input/touchscreen/eeti_ts.c
index b1884ddd7a84..09be6ced7151 100644
--- a/drivers/input/touchscreen/eeti_ts.c
+++ b/drivers/input/touchscreen/eeti_ts.c
@@ -264,8 +264,7 @@ static int eeti_ts_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int eeti_ts_suspend(struct device *dev)
+static int __maybe_unused eeti_ts_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct eeti_ts_priv *priv = i2c_get_clientdata(client);
@@ -284,7 +283,7 @@ static int eeti_ts_suspend(struct device *dev)
 	return 0;
 }
 
-static int eeti_ts_resume(struct device *dev)
+static int __maybe_unused eeti_ts_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct eeti_ts_priv *priv = i2c_get_clientdata(client);
@@ -302,7 +301,6 @@ static int eeti_ts_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(eeti_ts_pm, eeti_ts_suspend, eeti_ts_resume);
 
diff --git a/drivers/input/touchscreen/egalax_ts.c b/drivers/input/touchscreen/egalax_ts.c
index c8057847d71d..4c56299284ef 100644
--- a/drivers/input/touchscreen/egalax_ts.c
+++ b/drivers/input/touchscreen/egalax_ts.c
@@ -239,8 +239,7 @@ static const struct i2c_device_id egalax_ts_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, egalax_ts_id);
 
-#ifdef CONFIG_PM_SLEEP
-static int egalax_ts_suspend(struct device *dev)
+static int __maybe_unused egalax_ts_suspend(struct device *dev)
 {
 	static const u8 suspend_cmd[MAX_I2C_DATA_LEN] = {
 		0x3, 0x6, 0xa, 0x3, 0x36, 0x3f, 0x2, 0, 0, 0
@@ -252,13 +251,12 @@ static int egalax_ts_suspend(struct device *dev)
 	return ret > 0 ? 0 : ret;
 }
 
-static int egalax_ts_resume(struct device *dev)
+static int __maybe_unused egalax_ts_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 
 	return egalax_wake_up_device(client);
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(egalax_ts_pm_ops, egalax_ts_suspend, egalax_ts_resume);
 
diff --git a/drivers/input/touchscreen/elants_i2c.c b/drivers/input/touchscreen/elants_i2c.c
new file mode 100644
index 000000000000..a510f7ef9b66
--- /dev/null
+++ b/drivers/input/touchscreen/elants_i2c.c
@@ -0,0 +1,1271 @@
+/*
+ * Elan Microelectronics touch panels with I2C interface
+ *
+ * Copyright (C) 2014 Elan Microelectronics Corporation.
+ * Scott Liu <scott.liu@emc.com.tw>
+ *
+ * This code is partly based on hid-multitouch.c:
+ *
+ *  Copyright (c) 2010-2012 Stephane Chatty <chatty@enac.fr>
+ *  Copyright (c) 2010-2012 Benjamin Tissoires <benjamin.tissoires@gmail.com>
+ *  Copyright (c) 2010-2012 Ecole Nationale de l'Aviation Civile, France
+ *
+ *
+ * This code is partly based on i2c-hid.c:
+ *
+ * Copyright (c) 2012 Benjamin Tissoires <benjamin.tissoires@gmail.com>
+ * Copyright (c) 2012 Ecole Nationale de l'Aviation Civile, France
+ * Copyright (c) 2012 Red Hat, 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.
+ */
+
+#include <linux/module.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/async.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/uaccess.h>
+#include <linux/buffer_head.h>
+#include <linux/version.h>
+#include <linux/slab.h>
+#include <linux/firmware.h>
+#include <linux/version.h>
+#include <linux/input/mt.h>
+#include <linux/acpi.h>
+#include <linux/of.h>
+#include <asm/unaligned.h>
+
+/* Device, Driver information */
+#define DEVICE_NAME	"elants_i2c"
+#define DRV_VERSION	"1.0.9"
+
+/* Convert from rows or columns into resolution */
+#define ELAN_TS_RESOLUTION(n, m)   (((n) - 1) * (m))
+
+/* FW header data */
+#define HEADER_SIZE		4
+#define FW_HDR_TYPE		0
+#define FW_HDR_COUNT		1
+#define FW_HDR_LENGTH		2
+
+/* Buffer mode Queue Header information */
+#define QUEUE_HEADER_SINGLE	0x62
+#define QUEUE_HEADER_NORMAL	0X63
+#define QUEUE_HEADER_WAIT	0x64
+
+/* Command header definition */
+#define CMD_HEADER_WRITE	0x54
+#define CMD_HEADER_READ		0x53
+#define CMD_HEADER_6B_READ	0x5B
+#define CMD_HEADER_RESP		0x52
+#define CMD_HEADER_6B_RESP	0x9B
+#define CMD_HEADER_HELLO	0x55
+#define CMD_HEADER_REK		0x66
+
+/* FW position data */
+#define PACKET_SIZE		55
+#define MAX_CONTACT_NUM		10
+#define FW_POS_HEADER		0
+#define FW_POS_STATE		1
+#define FW_POS_TOTAL		2
+#define FW_POS_XY		3
+#define FW_POS_CHECKSUM		34
+#define FW_POS_WIDTH		35
+#define FW_POS_PRESSURE		45
+
+#define HEADER_REPORT_10_FINGER	0x62
+
+/* Header (4 bytes) plus 3 fill 10-finger packets */
+#define MAX_PACKET_SIZE		169
+
+#define BOOT_TIME_DELAY_MS	50
+
+/* FW read command, 0x53 0x?? 0x0, 0x01 */
+#define E_ELAN_INFO_FW_VER	0x00
+#define E_ELAN_INFO_BC_VER	0x10
+#define E_ELAN_INFO_TEST_VER	0xE0
+#define E_ELAN_INFO_FW_ID	0xF0
+#define E_INFO_OSR		0xD6
+#define E_INFO_PHY_SCAN		0xD7
+#define E_INFO_PHY_DRIVER	0xD8
+
+#define MAX_RETRIES		3
+#define MAX_FW_UPDATE_RETRIES	30
+
+#define ELAN_FW_PAGESIZE	132
+#define ELAN_FW_FILENAME	"elants_i2c.bin"
+
+/* calibration timeout definition */
+#define ELAN_CALI_TIMEOUT_MSEC	10000
+
+enum elants_state {
+	ELAN_STATE_NORMAL,
+	ELAN_WAIT_QUEUE_HEADER,
+	ELAN_WAIT_RECALIBRATION,
+};
+
+enum elants_iap_mode {
+	ELAN_IAP_OPERATIONAL,
+	ELAN_IAP_RECOVERY,
+};
+
+/* struct elants_data - represents state of Elan touchscreen device */
+struct elants_data {
+	struct i2c_client *client;
+	struct input_dev *input;
+
+	u16 fw_version;
+	u8 test_version;
+	u8 solution_version;
+	u8 bc_version;
+	u8 iap_version;
+	u16 hw_version;
+	unsigned int x_res;	/* resolution in units/mm */
+	unsigned int y_res;
+	unsigned int x_max;
+	unsigned int y_max;
+
+	enum elants_state state;
+	enum elants_iap_mode iap_mode;
+
+	/* Guards against concurrent access to the device via sysfs */
+	struct mutex sysfs_mutex;
+
+	u8 cmd_resp[HEADER_SIZE];
+	struct completion cmd_done;
+
+	u8 buf[MAX_PACKET_SIZE];
+
+	bool wake_irq_enabled;
+};
+
+static int elants_i2c_send(struct i2c_client *client,
+			   const void *data, size_t size)
+{
+	int ret;
+
+	ret = i2c_master_send(client, data, size);
+	if (ret == size)
+		return 0;
+
+	if (ret >= 0)
+		ret = -EIO;
+
+	dev_err(&client->dev, "%s failed (%*ph): %d\n",
+		__func__, (int)size, data, ret);
+
+	return ret;
+}
+
+static int elants_i2c_read(struct i2c_client *client, void *data, size_t size)
+{
+	int ret;
+
+	ret = i2c_master_recv(client, data, size);
+	if (ret == size)
+		return 0;
+
+	if (ret >= 0)
+		ret = -EIO;
+
+	dev_err(&client->dev, "%s failed: %d\n", __func__, ret);
+
+	return ret;
+}
+
+static int elants_i2c_execute_command(struct i2c_client *client,
+				      const u8 *cmd, size_t cmd_size,
+				      u8 *resp, size_t resp_size)
+{
+	struct i2c_msg msgs[2];
+	int ret;
+	u8 expected_response;
+
+	switch (cmd[0]) {
+	case CMD_HEADER_READ:
+		expected_response = CMD_HEADER_RESP;
+		break;
+
+	case CMD_HEADER_6B_READ:
+		expected_response = CMD_HEADER_6B_RESP;
+		break;
+
+	default:
+		dev_err(&client->dev, "%s: invalid command %*ph\n",
+			__func__, (int)cmd_size, cmd);
+		return -EINVAL;
+	}
+
+	msgs[0].addr = client->addr;
+	msgs[0].flags = client->flags & I2C_M_TEN;
+	msgs[0].len = cmd_size;
+	msgs[0].buf = (u8 *)cmd;
+
+	msgs[1].addr = client->addr;
+	msgs[1].flags = client->flags & I2C_M_TEN;
+	msgs[1].flags |= I2C_M_RD;
+	msgs[1].len = resp_size;
+	msgs[1].buf = resp;
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret < 0)
+		return ret;
+
+	if (ret != ARRAY_SIZE(msgs) || resp[FW_HDR_TYPE] != expected_response)
+		return -EIO;
+
+	return 0;
+}
+
+static int elants_i2c_calibrate(struct elants_data *ts)
+{
+	struct i2c_client *client = ts->client;
+	int ret, error;
+	static const u8 w_flashkey[] = { 0x54, 0xC0, 0xE1, 0x5A };
+	static const u8 rek[] = { 0x54, 0x29, 0x00, 0x01 };
+	static const u8 rek_resp[] = { CMD_HEADER_REK, 0x66, 0x66, 0x66 };
+
+	disable_irq(client->irq);
+
+	ts->state = ELAN_WAIT_RECALIBRATION;
+	reinit_completion(&ts->cmd_done);
+
+	elants_i2c_send(client, w_flashkey, sizeof(w_flashkey));
+	elants_i2c_send(client, rek, sizeof(rek));
+
+	enable_irq(client->irq);
+
+	ret = wait_for_completion_interruptible_timeout(&ts->cmd_done,
+				msecs_to_jiffies(ELAN_CALI_TIMEOUT_MSEC));
+
+	ts->state = ELAN_STATE_NORMAL;
+
+	if (ret <= 0) {
+		error = ret < 0 ? ret : -ETIMEDOUT;
+		dev_err(&client->dev,
+			"error while waiting for calibration to complete: %d\n",
+			error);
+		return error;
+	}
+
+	if (memcmp(rek_resp, ts->cmd_resp, sizeof(rek_resp))) {
+		dev_err(&client->dev,
+			"unexpected calibration response: %*ph\n",
+			(int)sizeof(ts->cmd_resp), ts->cmd_resp);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int elants_i2c_sw_reset(struct i2c_client *client)
+{
+	const u8 soft_rst_cmd[] = { 0x77, 0x77, 0x77, 0x77 };
+	int error;
+
+	error = elants_i2c_send(client, soft_rst_cmd,
+				sizeof(soft_rst_cmd));
+	if (error) {
+		dev_err(&client->dev, "software reset failed: %d\n", error);
+		return error;
+	}
+
+	/*
+	 * We should wait at least 10 msec (but no more than 40) before
+	 * sending fastboot or IAP command to the device.
+	 */
+	msleep(30);
+
+	return 0;
+}
+
+static u16 elants_i2c_parse_version(u8 *buf)
+{
+	return get_unaligned_be32(buf) >> 4;
+}
+
+static int elants_i2c_query_fw_id(struct elants_data *ts)
+{
+	struct i2c_client *client = ts->client;
+	int error, retry_cnt;
+	const u8 cmd[] = { CMD_HEADER_READ, E_ELAN_INFO_FW_ID, 0x00, 0x01 };
+	u8 resp[HEADER_SIZE];
+
+	for (retry_cnt = 0; retry_cnt < MAX_RETRIES; retry_cnt++) {
+		error = elants_i2c_execute_command(client, cmd, sizeof(cmd),
+						   resp, sizeof(resp));
+		if (!error) {
+			ts->hw_version = elants_i2c_parse_version(resp);
+			if (ts->hw_version != 0xffff)
+				return 0;
+		}
+
+		dev_dbg(&client->dev, "read fw id error=%d, buf=%*phC\n",
+			error, (int)sizeof(resp), resp);
+	}
+
+	dev_err(&client->dev,
+		"Failed to read fw id or fw id is invalid\n");
+
+	return -EINVAL;
+}
+
+static int elants_i2c_query_fw_version(struct elants_data *ts)
+{
+	struct i2c_client *client = ts->client;
+	int error, retry_cnt;
+	const u8 cmd[] = { CMD_HEADER_READ, E_ELAN_INFO_FW_VER, 0x00, 0x01 };
+	u8 resp[HEADER_SIZE];
+
+	for (retry_cnt = 0; retry_cnt < MAX_RETRIES; retry_cnt++) {
+		error = elants_i2c_execute_command(client, cmd, sizeof(cmd),
+						   resp, sizeof(resp));
+		if (!error) {
+			ts->fw_version = elants_i2c_parse_version(resp);
+			if (ts->fw_version != 0x0000 &&
+			    ts->fw_version != 0xffff)
+				return 0;
+		}
+
+		dev_dbg(&client->dev, "read fw version error=%d, buf=%*phC\n",
+			error, (int)sizeof(resp), resp);
+	}
+
+	dev_err(&client->dev,
+		"Failed to read fw version or fw version is invalid\n");
+
+	return -EINVAL;
+}
+
+static int elants_i2c_query_test_version(struct elants_data *ts)
+{
+	struct i2c_client *client = ts->client;
+	int error, retry_cnt;
+	u16 version;
+	const u8 cmd[] = { CMD_HEADER_READ, E_ELAN_INFO_TEST_VER, 0x00, 0x01 };
+	u8 resp[HEADER_SIZE];
+
+	for (retry_cnt = 0; retry_cnt < MAX_RETRIES; retry_cnt++) {
+		error = elants_i2c_execute_command(client, cmd, sizeof(cmd),
+						   resp, sizeof(resp));
+		if (!error) {
+			version = elants_i2c_parse_version(resp);
+			ts->test_version = version >> 8;
+			ts->solution_version = version & 0xff;
+
+			return 0;
+		}
+
+		dev_dbg(&client->dev,
+			"read test version error rc=%d, buf=%*phC\n",
+			error, (int)sizeof(resp), resp);
+	}
+
+	dev_err(&client->dev, "Failed to read test version\n");
+
+	return -EINVAL;
+}
+
+static int elants_i2c_query_bc_version(struct elants_data *ts)
+{
+	struct i2c_client *client = ts->client;
+	const u8 cmd[] = { CMD_HEADER_READ, E_ELAN_INFO_BC_VER, 0x00, 0x01 };
+	u8 resp[HEADER_SIZE];
+	u16 version;
+	int error;
+
+	error = elants_i2c_execute_command(client, cmd, sizeof(cmd),
+					   resp, sizeof(resp));
+	if (error) {
+		dev_err(&client->dev,
+			"read BC version error=%d, buf=%*phC\n",
+			error, (int)sizeof(resp), resp);
+		return error;
+	}
+
+	version = elants_i2c_parse_version(resp);
+	ts->bc_version = version >> 8;
+	ts->iap_version = version & 0xff;
+
+	return 0;
+}
+
+static int elants_i2c_query_ts_info(struct elants_data *ts)
+{
+	struct i2c_client *client = ts->client;
+	int error;
+	u8 resp[17];
+	u16 phy_x, phy_y, rows, cols, osr;
+	const u8 get_resolution_cmd[] = {
+		CMD_HEADER_6B_READ, 0x00, 0x00, 0x00, 0x00, 0x00
+	};
+	const u8 get_osr_cmd[] = {
+		CMD_HEADER_READ, E_INFO_OSR, 0x00, 0x01
+	};
+	const u8 get_physical_scan_cmd[] = {
+		CMD_HEADER_READ, E_INFO_PHY_SCAN, 0x00, 0x01
+	};
+	const u8 get_physical_drive_cmd[] = {
+		CMD_HEADER_READ, E_INFO_PHY_DRIVER, 0x00, 0x01
+	};
+
+	/* Get trace number */
+	error = elants_i2c_execute_command(client,
+					   get_resolution_cmd,
+					   sizeof(get_resolution_cmd),
+					   resp, sizeof(resp));
+	if (error) {
+		dev_err(&client->dev, "get resolution command failed: %d\n",
+			error);
+		return error;
+	}
+
+	rows = resp[2] + resp[6] + resp[10];
+	cols = resp[3] + resp[7] + resp[11];
+
+	/* Process mm_to_pixel information */
+	error = elants_i2c_execute_command(client,
+					   get_osr_cmd, sizeof(get_osr_cmd),
+					   resp, sizeof(resp));
+	if (error) {
+		dev_err(&client->dev, "get osr command failed: %d\n",
+			error);
+		return error;
+	}
+
+	osr = resp[3];
+
+	error = elants_i2c_execute_command(client,
+					   get_physical_scan_cmd,
+					   sizeof(get_physical_scan_cmd),
+					   resp, sizeof(resp));
+	if (error) {
+		dev_err(&client->dev, "get physical scan command failed: %d\n",
+			error);
+		return error;
+	}
+
+	phy_x = get_unaligned_be16(&resp[2]);
+
+	error = elants_i2c_execute_command(client,
+					   get_physical_drive_cmd,
+					   sizeof(get_physical_drive_cmd),
+					   resp, sizeof(resp));
+	if (error) {
+		dev_err(&client->dev, "get physical drive command failed: %d\n",
+			error);
+		return error;
+	}
+
+	phy_y = get_unaligned_be16(&resp[2]);
+
+	dev_dbg(&client->dev, "phy_x=%d, phy_y=%d\n", phy_x, phy_y);
+
+	if (rows == 0 || cols == 0 || osr == 0) {
+		dev_warn(&client->dev,
+			 "invalid trace number data: %d, %d, %d\n",
+			 rows, cols, osr);
+	} else {
+		/* translate trace number to TS resolution */
+		ts->x_max = ELAN_TS_RESOLUTION(rows, osr);
+		ts->x_res = DIV_ROUND_CLOSEST(ts->x_max, phy_x);
+		ts->y_max = ELAN_TS_RESOLUTION(cols, osr);
+		ts->y_res = DIV_ROUND_CLOSEST(ts->y_max, phy_y);
+	}
+
+	return 0;
+}
+
+static int elants_i2c_fastboot(struct i2c_client *client)
+{
+	const u8 boot_cmd[] = { 0x4D, 0x61, 0x69, 0x6E };
+	int error;
+
+	error = elants_i2c_send(client, boot_cmd, sizeof(boot_cmd));
+	if (error) {
+		dev_err(&client->dev, "boot failed: %d\n", error);
+		return error;
+	}
+
+	dev_dbg(&client->dev, "boot success -- 0x%x\n", client->addr);
+	return 0;
+}
+
+static int elants_i2c_initialize(struct elants_data *ts)
+{
+	struct i2c_client *client = ts->client;
+	int error, retry_cnt;
+	const u8 hello_packet[] = { 0x55, 0x55, 0x55, 0x55 };
+	const u8 recov_packet[] = { 0x55, 0x55, 0x80, 0x80 };
+	u8 buf[HEADER_SIZE];
+
+	for (retry_cnt = 0; retry_cnt < MAX_RETRIES; retry_cnt++) {
+		error = elants_i2c_sw_reset(client);
+		if (error) {
+			/* Continue initializing if it's the last try */
+			if (retry_cnt < MAX_RETRIES - 1)
+				continue;
+		}
+
+		error = elants_i2c_fastboot(client);
+		if (error) {
+			/* Continue initializing if it's the last try */
+			if (retry_cnt < MAX_RETRIES - 1)
+				continue;
+		}
+
+		/* Wait for Hello packet */
+		msleep(BOOT_TIME_DELAY_MS);
+
+		error = elants_i2c_read(client, buf, sizeof(buf));
+		if (error) {
+			dev_err(&client->dev,
+				"failed to read 'hello' packet: %d\n", error);
+		} else if (!memcmp(buf, hello_packet, sizeof(hello_packet))) {
+			ts->iap_mode = ELAN_IAP_OPERATIONAL;
+			break;
+		} else if (!memcmp(buf, recov_packet, sizeof(recov_packet))) {
+			/*
+			 * Setting error code will mark device
+			 * in recovery mode below.
+			 */
+			error = -EIO;
+			break;
+		} else {
+			error = -EINVAL;
+			dev_err(&client->dev,
+				"invalid 'hello' packet: %*ph\n",
+				(int)sizeof(buf), buf);
+		}
+	}
+
+	if (!error)
+		error = elants_i2c_query_fw_id(ts);
+	if (!error)
+		error = elants_i2c_query_fw_version(ts);
+
+	if (error) {
+		ts->iap_mode = ELAN_IAP_RECOVERY;
+	} else {
+		elants_i2c_query_test_version(ts);
+		elants_i2c_query_bc_version(ts);
+		elants_i2c_query_ts_info(ts);
+	}
+
+	return 0;
+}
+
+/*
+ * Firmware update interface.
+ */
+
+static int elants_i2c_fw_write_page(struct i2c_client *client,
+				    const void *page)
+{
+	const u8 ack_ok[] = { 0xaa, 0xaa };
+	u8 buf[2];
+	int retry;
+	int error;
+
+	for (retry = 0; retry < MAX_FW_UPDATE_RETRIES; retry++) {
+		error = elants_i2c_send(client, page, ELAN_FW_PAGESIZE);
+		if (error) {
+			dev_err(&client->dev,
+				"IAP Write Page failed: %d\n", error);
+			continue;
+		}
+
+		error = elants_i2c_read(client, buf, 2);
+		if (error) {
+			dev_err(&client->dev,
+				"IAP Ack read failed: %d\n", error);
+			return error;
+		}
+
+		if (!memcmp(buf, ack_ok, sizeof(ack_ok)))
+			return 0;
+
+		error = -EIO;
+		dev_err(&client->dev,
+			"IAP Get Ack Error [%02x:%02x]\n",
+			buf[0], buf[1]);
+	}
+
+	return error;
+}
+
+static int elants_i2c_do_update_firmware(struct i2c_client *client,
+					 const struct firmware *fw,
+					 bool force)
+{
+	const u8 enter_iap[] = { 0x45, 0x49, 0x41, 0x50 };
+	const u8 enter_iap2[] = { 0x54, 0x00, 0x12, 0x34 };
+	const u8 iap_ack[] = { 0x55, 0xaa, 0x33, 0xcc };
+	u8 buf[HEADER_SIZE];
+	u16 send_id;
+	int page, n_fw_pages;
+	int error;
+
+	/* Recovery mode detection! */
+	if (force) {
+		dev_dbg(&client->dev, "Recovery mode procedure\n");
+		error = elants_i2c_send(client, enter_iap2, sizeof(enter_iap2));
+	} else {
+		/* Start IAP Procedure */
+		dev_dbg(&client->dev, "Normal IAP procedure\n");
+		elants_i2c_sw_reset(client);
+
+		error = elants_i2c_send(client, enter_iap, sizeof(enter_iap));
+	}
+
+	if (error) {
+		dev_err(&client->dev, "failed to enter IAP mode: %d\n", error);
+		return error;
+	}
+
+	msleep(20);
+
+	/* check IAP state */
+	error = elants_i2c_read(client, buf, 4);
+	if (error) {
+		dev_err(&client->dev,
+			"failed to read IAP acknowledgement: %d\n",
+			error);
+		return error;
+	}
+
+	if (memcmp(buf, iap_ack, sizeof(iap_ack))) {
+		dev_err(&client->dev,
+			"failed to enter IAP: %*ph (expected %*ph)\n",
+			(int)sizeof(buf), buf, (int)sizeof(iap_ack), iap_ack);
+		return -EIO;
+	}
+
+	dev_info(&client->dev, "successfully entered IAP mode");
+
+	send_id = client->addr;
+	error = elants_i2c_send(client, &send_id, 1);
+	if (error) {
+		dev_err(&client->dev, "sending dummy byte failed: %d\n",
+			error);
+		return error;
+	}
+
+	/* Clear the last page of Master */
+	error = elants_i2c_send(client, fw->data, ELAN_FW_PAGESIZE);
+	if (error) {
+		dev_err(&client->dev, "clearing of the last page failed: %d\n",
+			error);
+		return error;
+	}
+
+	error = elants_i2c_read(client, buf, 2);
+	if (error) {
+		dev_err(&client->dev,
+			"failed to read ACK for clearing the last page: %d\n",
+			error);
+		return error;
+	}
+
+	n_fw_pages = fw->size / ELAN_FW_PAGESIZE;
+	dev_dbg(&client->dev, "IAP Pages = %d\n", n_fw_pages);
+
+	for (page = 0; page < n_fw_pages; page++) {
+		error = elants_i2c_fw_write_page(client,
+					fw->data + page * ELAN_FW_PAGESIZE);
+		if (error) {
+			dev_err(&client->dev,
+				"failed to write FW page %d: %d\n",
+				page, error);
+			return error;
+		}
+	}
+
+	/* Old iap needs to wait 200ms for WDT and rest is for hello packets */
+	msleep(300);
+
+	dev_info(&client->dev, "firmware update completed\n");
+	return 0;
+}
+
+static int elants_i2c_fw_update(struct elants_data *ts)
+{
+	struct i2c_client *client = ts->client;
+	const struct firmware *fw;
+	int error;
+
+	error = request_firmware(&fw, ELAN_FW_FILENAME, &client->dev);
+	if (error) {
+		dev_err(&client->dev, "failed to request firmware %s: %d\n",
+			ELAN_FW_FILENAME, error);
+		return error;
+	}
+
+	if (fw->size % ELAN_FW_PAGESIZE) {
+		dev_err(&client->dev, "invalid firmware length: %zu\n",
+			fw->size);
+		error = -EINVAL;
+		goto out;
+	}
+
+	disable_irq(client->irq);
+
+	error = elants_i2c_do_update_firmware(client, fw,
+					ts->iap_mode == ELAN_IAP_RECOVERY);
+	if (error) {
+		dev_err(&client->dev, "firmware update failed: %d\n", error);
+		ts->iap_mode = ELAN_IAP_RECOVERY;
+		goto out_enable_irq;
+	}
+
+	error = elants_i2c_initialize(ts);
+	if (error) {
+		dev_err(&client->dev,
+			"failed to initialize device after firmware update: %d\n",
+			error);
+		ts->iap_mode = ELAN_IAP_RECOVERY;
+		goto out_enable_irq;
+	}
+
+	ts->iap_mode = ELAN_IAP_OPERATIONAL;
+
+out_enable_irq:
+	ts->state = ELAN_STATE_NORMAL;
+	enable_irq(client->irq);
+	msleep(100);
+
+	if (!error)
+		elants_i2c_calibrate(ts);
+out:
+	release_firmware(fw);
+	return error;
+}
+
+/*
+ * Event reporting.
+ */
+
+static void elants_i2c_mt_event(struct elants_data *ts, u8 *buf)
+{
+	struct input_dev *input = ts->input;
+	unsigned int n_fingers;
+	u16 finger_state;
+	int i;
+
+	n_fingers = buf[FW_POS_STATE + 1] & 0x0f;
+	finger_state = ((buf[FW_POS_STATE + 1] & 0x30) << 4) |
+			buf[FW_POS_STATE];
+
+	dev_dbg(&ts->client->dev,
+		"n_fingers: %u, state: %04x\n",  n_fingers, finger_state);
+
+	for (i = 0; i < MAX_CONTACT_NUM && n_fingers; i++) {
+		if (finger_state & 1) {
+			unsigned int x, y, p, w;
+			u8 *pos;
+
+			pos = &buf[FW_POS_XY + i * 3];
+			x = (((u16)pos[0] & 0xf0) << 4) | pos[1];
+			y = (((u16)pos[0] & 0x0f) << 8) | pos[2];
+			p = buf[FW_POS_PRESSURE + i];
+			w = buf[FW_POS_WIDTH + i];
+
+			dev_dbg(&ts->client->dev, "i=%d x=%d y=%d p=%d w=%d\n",
+				i, x, y, p, w);
+
+			input_mt_slot(input, i);
+			input_mt_report_slot_state(input, MT_TOOL_FINGER, true);
+			input_event(input, EV_ABS, ABS_MT_POSITION_X, x);
+			input_event(input, EV_ABS, ABS_MT_POSITION_Y, y);
+			input_event(input, EV_ABS, ABS_MT_PRESSURE, p);
+			input_event(input, EV_ABS, ABS_MT_TOUCH_MAJOR, w);
+
+			n_fingers--;
+		}
+
+		finger_state >>= 1;
+	}
+
+	input_mt_sync_frame(input);
+	input_sync(input);
+}
+
+static u8 elants_i2c_calculate_checksum(u8 *buf)
+{
+	u8 checksum = 0;
+	u8 i;
+
+	for (i = 0; i < FW_POS_CHECKSUM; i++)
+		checksum += buf[i];
+
+	return checksum;
+}
+
+static void elants_i2c_event(struct elants_data *ts, u8 *buf)
+{
+	u8 checksum = elants_i2c_calculate_checksum(buf);
+
+	if (unlikely(buf[FW_POS_CHECKSUM] != checksum))
+		dev_warn(&ts->client->dev,
+			 "%s: invalid checksum for packet %02x: %02x vs. %02x\n",
+			 __func__, buf[FW_POS_HEADER],
+			 checksum, buf[FW_POS_CHECKSUM]);
+	else if (unlikely(buf[FW_POS_HEADER] != HEADER_REPORT_10_FINGER))
+		dev_warn(&ts->client->dev,
+			 "%s: unknown packet type: %02x\n",
+			 __func__, buf[FW_POS_HEADER]);
+	else
+		elants_i2c_mt_event(ts, buf);
+}
+
+static irqreturn_t elants_i2c_irq(int irq, void *_dev)
+{
+	const u8 wait_packet[] = { 0x64, 0x64, 0x64, 0x64 };
+	struct elants_data *ts = _dev;
+	struct i2c_client *client = ts->client;
+	int report_count, report_len;
+	int i;
+	int len;
+
+	len = i2c_master_recv(client, ts->buf, sizeof(ts->buf));
+	if (len < 0) {
+		dev_err(&client->dev, "%s: failed to read data: %d\n",
+			__func__, len);
+		goto out;
+	}
+
+	dev_dbg(&client->dev, "%s: packet %*ph\n",
+		__func__, HEADER_SIZE, ts->buf);
+
+	switch (ts->state) {
+	case ELAN_WAIT_RECALIBRATION:
+		if (ts->buf[FW_HDR_TYPE] == CMD_HEADER_REK) {
+			memcpy(ts->cmd_resp, ts->buf, sizeof(ts->cmd_resp));
+			complete(&ts->cmd_done);
+			ts->state = ELAN_STATE_NORMAL;
+		}
+		break;
+
+	case ELAN_WAIT_QUEUE_HEADER:
+		if (ts->buf[FW_HDR_TYPE] != QUEUE_HEADER_NORMAL)
+			break;
+
+		ts->state = ELAN_STATE_NORMAL;
+		/* fall through */
+
+	case ELAN_STATE_NORMAL:
+
+		switch (ts->buf[FW_HDR_TYPE]) {
+		case CMD_HEADER_HELLO:
+		case CMD_HEADER_RESP:
+		case CMD_HEADER_REK:
+			break;
+
+		case QUEUE_HEADER_WAIT:
+			if (memcmp(ts->buf, wait_packet, sizeof(wait_packet))) {
+				dev_err(&client->dev,
+					"invalid wait packet %*ph\n",
+					HEADER_SIZE, ts->buf);
+			} else {
+				ts->state = ELAN_WAIT_QUEUE_HEADER;
+				udelay(30);
+			}
+			break;
+
+		case QUEUE_HEADER_SINGLE:
+			elants_i2c_event(ts, &ts->buf[HEADER_SIZE]);
+			break;
+
+		case QUEUE_HEADER_NORMAL:
+			report_count = ts->buf[FW_HDR_COUNT];
+			if (report_count > 3) {
+				dev_err(&client->dev,
+					"too large report count: %*ph\n",
+					HEADER_SIZE, ts->buf);
+				break;
+			}
+
+			report_len = ts->buf[FW_HDR_LENGTH] / report_count;
+			if (report_len != PACKET_SIZE) {
+				dev_err(&client->dev,
+					"mismatching report length: %*ph\n",
+					HEADER_SIZE, ts->buf);
+				break;
+			}
+
+			for (i = 0; i < report_count; i++) {
+				u8 *buf = ts->buf + HEADER_SIZE +
+							i * PACKET_SIZE;
+				elants_i2c_event(ts, buf);
+			}
+			break;
+
+		default:
+			dev_err(&client->dev, "unknown packet %*ph\n",
+				HEADER_SIZE, ts->buf);
+			break;
+		}
+		break;
+	}
+
+out:
+	return IRQ_HANDLED;
+}
+
+/*
+ * sysfs interface
+ */
+static ssize_t calibrate_store(struct device *dev,
+			       struct device_attribute *attr,
+			      const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct elants_data *ts = i2c_get_clientdata(client);
+	int error;
+
+	error = mutex_lock_interruptible(&ts->sysfs_mutex);
+	if (error)
+		return error;
+
+	error = elants_i2c_calibrate(ts);
+
+	mutex_unlock(&ts->sysfs_mutex);
+	return error ?: count;
+}
+
+static ssize_t write_update_fw(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct elants_data *ts = i2c_get_clientdata(client);
+	int error;
+
+	error = mutex_lock_interruptible(&ts->sysfs_mutex);
+	if (error)
+		return error;
+
+	error = elants_i2c_fw_update(ts);
+	dev_dbg(dev, "firmware update result: %d\n", error);
+
+	mutex_unlock(&ts->sysfs_mutex);
+	return error ?: count;
+}
+
+static ssize_t show_iap_mode(struct device *dev,
+			     struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct elants_data *ts = i2c_get_clientdata(client);
+
+	return sprintf(buf, "%s\n",
+		       ts->iap_mode == ELAN_IAP_OPERATIONAL ?
+				"Normal" : "Recovery");
+}
+
+static DEVICE_ATTR(calibrate, S_IWUSR, NULL, calibrate_store);
+static DEVICE_ATTR(iap_mode, S_IRUGO, show_iap_mode, NULL);
+static DEVICE_ATTR(update_fw, S_IWUSR, NULL, write_update_fw);
+
+struct elants_version_attribute {
+	struct device_attribute dattr;
+	size_t field_offset;
+	size_t field_size;
+};
+
+#define __ELANTS_FIELD_SIZE(_field)					\
+	sizeof(((struct elants_data *)NULL)->_field)
+#define __ELANTS_VERIFY_SIZE(_field)					\
+	(BUILD_BUG_ON_ZERO(__ELANTS_FIELD_SIZE(_field) > 2) +		\
+	 __ELANTS_FIELD_SIZE(_field))
+#define ELANTS_VERSION_ATTR(_field)					\
+	struct elants_version_attribute elants_ver_attr_##_field = {	\
+		.dattr = __ATTR(_field, S_IRUGO,			\
+				elants_version_attribute_show, NULL),	\
+		.field_offset = offsetof(struct elants_data, _field),	\
+		.field_size = __ELANTS_VERIFY_SIZE(_field),		\
+	}
+
+static ssize_t elants_version_attribute_show(struct device *dev,
+					     struct device_attribute *dattr,
+					     char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct elants_data *ts = i2c_get_clientdata(client);
+	struct elants_version_attribute *attr =
+		container_of(dattr, struct elants_version_attribute, dattr);
+	u8 *field = (u8 *)((char *)ts + attr->field_offset);
+	unsigned int fmt_size;
+	unsigned int val;
+
+	if (attr->field_size == 1) {
+		val = *field;
+		fmt_size = 2; /* 2 HEX digits */
+	} else {
+		val = *(u16 *)field;
+		fmt_size = 4; /* 4 HEX digits */
+	}
+
+	return sprintf(buf, "%0*x\n", fmt_size, val);
+}
+
+static ELANTS_VERSION_ATTR(fw_version);
+static ELANTS_VERSION_ATTR(hw_version);
+static ELANTS_VERSION_ATTR(test_version);
+static ELANTS_VERSION_ATTR(solution_version);
+static ELANTS_VERSION_ATTR(bc_version);
+static ELANTS_VERSION_ATTR(iap_version);
+
+static struct attribute *elants_attributes[] = {
+	&dev_attr_calibrate.attr,
+	&dev_attr_update_fw.attr,
+	&dev_attr_iap_mode.attr,
+
+	&elants_ver_attr_fw_version.dattr.attr,
+	&elants_ver_attr_hw_version.dattr.attr,
+	&elants_ver_attr_test_version.dattr.attr,
+	&elants_ver_attr_solution_version.dattr.attr,
+	&elants_ver_attr_bc_version.dattr.attr,
+	&elants_ver_attr_iap_version.dattr.attr,
+	NULL
+};
+
+static struct attribute_group elants_attribute_group = {
+	.attrs = elants_attributes,
+};
+
+static void elants_i2c_remove_sysfs_group(void *_data)
+{
+	struct elants_data *ts = _data;
+
+	sysfs_remove_group(&ts->client->dev.kobj, &elants_attribute_group);
+}
+
+static int elants_i2c_probe(struct i2c_client *client,
+			    const struct i2c_device_id *id)
+{
+	union i2c_smbus_data dummy;
+	struct elants_data *ts;
+	unsigned long irqflags;
+	int error;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev,
+			"%s: i2c check functionality error\n", DEVICE_NAME);
+		return -ENXIO;
+	}
+
+	/* Make sure there is something at this address */
+	if (i2c_smbus_xfer(client->adapter, client->addr, 0,
+			I2C_SMBUS_READ, 0, I2C_SMBUS_BYTE, &dummy) < 0) {
+		dev_err(&client->dev, "nothing at this address\n");
+		return -ENXIO;
+	}
+
+	ts = devm_kzalloc(&client->dev, sizeof(struct elants_data), GFP_KERNEL);
+	if (!ts)
+		return -ENOMEM;
+
+	mutex_init(&ts->sysfs_mutex);
+	init_completion(&ts->cmd_done);
+
+	ts->client = client;
+	i2c_set_clientdata(client, ts);
+
+	error = elants_i2c_initialize(ts);
+	if (error) {
+		dev_err(&client->dev, "failed to initialize: %d\n", error);
+		return error;
+	}
+
+	ts->input = devm_input_allocate_device(&client->dev);
+	if (!ts->input) {
+		dev_err(&client->dev, "Failed to allocate input device\n");
+		return -ENOMEM;
+	}
+
+	ts->input->name = "Elan Touchscreen";
+	ts->input->id.bustype = BUS_I2C;
+
+	__set_bit(BTN_TOUCH, ts->input->keybit);
+	__set_bit(EV_ABS, ts->input->evbit);
+	__set_bit(EV_KEY, ts->input->evbit);
+
+	/* Single touch input params setup */
+	input_set_abs_params(ts->input, ABS_X, 0, ts->x_max, 0, 0);
+	input_set_abs_params(ts->input, ABS_Y, 0, ts->y_max, 0, 0);
+	input_set_abs_params(ts->input, ABS_PRESSURE, 0, 255, 0, 0);
+	input_abs_set_res(ts->input, ABS_X, ts->x_res);
+	input_abs_set_res(ts->input, ABS_Y, ts->y_res);
+
+	/* Multitouch input params setup */
+	error = input_mt_init_slots(ts->input, MAX_CONTACT_NUM,
+				    INPUT_MT_DIRECT | INPUT_MT_DROP_UNUSED);
+	if (error) {
+		dev_err(&client->dev,
+			"failed to initialize MT slots: %d\n", error);
+		return error;
+	}
+
+	input_set_abs_params(ts->input, ABS_MT_POSITION_X, 0, ts->x_max, 0, 0);
+	input_set_abs_params(ts->input, ABS_MT_POSITION_Y, 0, ts->y_max, 0, 0);
+	input_set_abs_params(ts->input, ABS_MT_TOUCH_MAJOR, 0, 255, 0, 0);
+	input_set_abs_params(ts->input, ABS_MT_PRESSURE, 0, 255, 0, 0);
+	input_abs_set_res(ts->input, ABS_MT_POSITION_X, ts->x_res);
+	input_abs_set_res(ts->input, ABS_MT_POSITION_Y, ts->y_res);
+
+	input_set_drvdata(ts->input, ts);
+
+	error = input_register_device(ts->input);
+	if (error) {
+		dev_err(&client->dev,
+			"unable to register input device: %d\n", error);
+		return error;
+	}
+
+	/*
+	 * Systems using device tree should set up interrupt via DTS,
+	 * the rest will use the default falling edge interrupts.
+	 */
+	irqflags = client->dev.of_node ? 0 : IRQF_TRIGGER_FALLING;
+
+	error = devm_request_threaded_irq(&client->dev, client->irq,
+					  NULL, elants_i2c_irq,
+					  irqflags | IRQF_ONESHOT,
+					  client->name, ts);
+	if (error) {
+		dev_err(&client->dev, "Failed to register interrupt\n");
+		return error;
+	}
+
+	/*
+	 * Systems using device tree should set up wakeup via DTS,
+	 * the rest will configure device as wakeup source by default.
+	 */
+	if (!client->dev.of_node)
+		device_init_wakeup(&client->dev, true);
+
+	error = sysfs_create_group(&client->dev.kobj, &elants_attribute_group);
+	if (error) {
+		dev_err(&client->dev, "failed to create sysfs attributes: %d\n",
+			error);
+		return error;
+	}
+
+	error = devm_add_action(&client->dev,
+				elants_i2c_remove_sysfs_group, ts);
+	if (error) {
+		elants_i2c_remove_sysfs_group(ts);
+		dev_err(&client->dev,
+			"Failed to add sysfs cleanup action: %d\n",
+			error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int __maybe_unused elants_i2c_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct elants_data *ts = i2c_get_clientdata(client);
+	const u8 set_sleep_cmd[] = { 0x54, 0x50, 0x00, 0x01 };
+	int retry_cnt;
+	int error;
+
+	/* Command not support in IAP recovery mode */
+	if (ts->iap_mode != ELAN_IAP_OPERATIONAL)
+		return -EBUSY;
+
+	disable_irq(client->irq);
+
+	for (retry_cnt = 0; retry_cnt < MAX_RETRIES; retry_cnt++) {
+		error = elants_i2c_send(client, set_sleep_cmd,
+					sizeof(set_sleep_cmd));
+		if (!error)
+			break;
+
+		dev_err(&client->dev, "suspend command failed: %d\n", error);
+	}
+
+	if (device_may_wakeup(dev))
+		ts->wake_irq_enabled = (enable_irq_wake(client->irq) == 0);
+
+	return 0;
+}
+
+static int __maybe_unused elants_i2c_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct elants_data *ts = i2c_get_clientdata(client);
+	const u8 set_active_cmd[] = { 0x54, 0x58, 0x00, 0x01 };
+	int retry_cnt;
+	int error;
+
+	if (device_may_wakeup(dev) && ts->wake_irq_enabled)
+		disable_irq_wake(client->irq);
+
+	for (retry_cnt = 0; retry_cnt < MAX_RETRIES; retry_cnt++) {
+		error = elants_i2c_send(client, set_active_cmd,
+					sizeof(set_active_cmd));
+		if (!error)
+			break;
+
+		dev_err(&client->dev, "resume command failed: %d\n", error);
+	}
+
+	ts->state = ELAN_STATE_NORMAL;
+	enable_irq(client->irq);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(elants_i2c_pm_ops,
+			 elants_i2c_suspend, elants_i2c_resume);
+
+static const struct i2c_device_id elants_i2c_id[] = {
+	{ DEVICE_NAME, 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, elants_i2c_id);
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id elants_acpi_id[] = {
+	{ "ELAN0001", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(acpi, elants_acpi_id);
+#endif
+
+#ifdef CONFIG_OF
+static const struct of_device_id elants_of_match[] = {
+	{ .compatible = "elan,ekth3500" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, elants_of_match);
+#endif
+
+static struct i2c_driver elants_i2c_driver = {
+	.probe = elants_i2c_probe,
+	.id_table = elants_i2c_id,
+	.driver = {
+		.name = DEVICE_NAME,
+		.owner = THIS_MODULE,
+		.pm = &elants_i2c_pm_ops,
+		.acpi_match_table = ACPI_PTR(elants_acpi_id),
+		.of_match_table = of_match_ptr(elants_of_match),
+	},
+};
+module_i2c_driver(elants_i2c_driver);
+
+MODULE_AUTHOR("Scott Liu <scott.liu@emc.com.tw>");
+MODULE_DESCRIPTION("Elan I2c Touchscreen driver");
+MODULE_VERSION(DRV_VERSION);
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/goodix.c b/drivers/input/touchscreen/goodix.c
new file mode 100644
index 000000000000..ca196689f025
--- /dev/null
+++ b/drivers/input/touchscreen/goodix.c
@@ -0,0 +1,395 @@
+/*
+ *  Driver for Goodix Touchscreens
+ *
+ *  Copyright (c) 2014 Red Hat Inc.
+ *
+ *  This code is based on gt9xx.c authored by andrew@goodix.com:
+ *
+ *  2010 - 2012 Goodix Technology.
+ */
+
+/*
+ * 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/i2c.h>
+#include <linux/input.h>
+#include <linux/input/mt.h>
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <asm/unaligned.h>
+
+struct goodix_ts_data {
+	struct i2c_client *client;
+	struct input_dev *input_dev;
+	int abs_x_max;
+	int abs_y_max;
+	unsigned int max_touch_num;
+	unsigned int int_trigger_type;
+};
+
+#define GOODIX_MAX_HEIGHT		4096
+#define GOODIX_MAX_WIDTH		4096
+#define GOODIX_INT_TRIGGER		1
+#define GOODIX_CONTACT_SIZE		8
+#define GOODIX_MAX_CONTACTS		10
+
+#define GOODIX_CONFIG_MAX_LENGTH	240
+
+/* Register defines */
+#define GOODIX_READ_COOR_ADDR		0x814E
+#define GOODIX_REG_CONFIG_DATA		0x8047
+#define GOODIX_REG_VERSION		0x8140
+
+#define RESOLUTION_LOC		1
+#define TRIGGER_LOC		6
+
+static const unsigned long goodix_irq_flags[] = {
+	IRQ_TYPE_EDGE_RISING,
+	IRQ_TYPE_EDGE_FALLING,
+	IRQ_TYPE_LEVEL_LOW,
+	IRQ_TYPE_LEVEL_HIGH,
+};
+
+/**
+ * goodix_i2c_read - read data from a register of the i2c slave device.
+ *
+ * @client: i2c device.
+ * @reg: the register to read from.
+ * @buf: raw write data buffer.
+ * @len: length of the buffer to write
+ */
+static int goodix_i2c_read(struct i2c_client *client,
+				u16 reg, u8 *buf, int len)
+{
+	struct i2c_msg msgs[2];
+	u16 wbuf = cpu_to_be16(reg);
+	int ret;
+
+	msgs[0].flags = 0;
+	msgs[0].addr  = client->addr;
+	msgs[0].len   = 2;
+	msgs[0].buf   = (u8 *) &wbuf;
+
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].addr  = client->addr;
+	msgs[1].len   = len;
+	msgs[1].buf   = buf;
+
+	ret = i2c_transfer(client->adapter, msgs, 2);
+	return ret < 0 ? ret : (ret != ARRAY_SIZE(msgs) ? -EIO : 0);
+}
+
+static int goodix_ts_read_input_report(struct goodix_ts_data *ts, u8 *data)
+{
+	int touch_num;
+	int error;
+
+	error = goodix_i2c_read(ts->client, GOODIX_READ_COOR_ADDR, data,
+				GOODIX_CONTACT_SIZE + 1);
+	if (error) {
+		dev_err(&ts->client->dev, "I2C transfer error: %d\n", error);
+		return error;
+	}
+
+	touch_num = data[0] & 0x0f;
+	if (touch_num > GOODIX_MAX_CONTACTS)
+		return -EPROTO;
+
+	if (touch_num > 1) {
+		data += 1 + GOODIX_CONTACT_SIZE;
+		error = goodix_i2c_read(ts->client,
+					GOODIX_READ_COOR_ADDR +
+						1 + GOODIX_CONTACT_SIZE,
+					data,
+					GOODIX_CONTACT_SIZE * (touch_num - 1));
+		if (error)
+			return error;
+	}
+
+	return touch_num;
+}
+
+static void goodix_ts_report_touch(struct goodix_ts_data *ts, u8 *coor_data)
+{
+	int id = coor_data[0] & 0x0F;
+	int input_x = get_unaligned_le16(&coor_data[1]);
+	int input_y = get_unaligned_le16(&coor_data[3]);
+	int input_w = get_unaligned_le16(&coor_data[5]);
+
+	input_mt_slot(ts->input_dev, id);
+	input_mt_report_slot_state(ts->input_dev, MT_TOOL_FINGER, true);
+	input_report_abs(ts->input_dev, ABS_MT_POSITION_X, input_x);
+	input_report_abs(ts->input_dev, ABS_MT_POSITION_Y, input_y);
+	input_report_abs(ts->input_dev, ABS_MT_TOUCH_MAJOR, input_w);
+	input_report_abs(ts->input_dev, ABS_MT_WIDTH_MAJOR, input_w);
+}
+
+/**
+ * goodix_process_events - Process incoming events
+ *
+ * @ts: our goodix_ts_data pointer
+ *
+ * Called when the IRQ is triggered. Read the current device state, and push
+ * the input events to the user space.
+ */
+static void goodix_process_events(struct goodix_ts_data *ts)
+{
+	u8  point_data[1 + GOODIX_CONTACT_SIZE * GOODIX_MAX_CONTACTS];
+	int touch_num;
+	int i;
+
+	touch_num = goodix_ts_read_input_report(ts, point_data);
+	if (touch_num < 0)
+		return;
+
+	for (i = 0; i < touch_num; i++)
+		goodix_ts_report_touch(ts,
+				&point_data[1 + GOODIX_CONTACT_SIZE * i]);
+
+	input_mt_sync_frame(ts->input_dev);
+	input_sync(ts->input_dev);
+}
+
+/**
+ * goodix_ts_irq_handler - The IRQ handler
+ *
+ * @irq: interrupt number.
+ * @dev_id: private data pointer.
+ */
+static irqreturn_t goodix_ts_irq_handler(int irq, void *dev_id)
+{
+	static const u8 end_cmd[] = {
+		GOODIX_READ_COOR_ADDR >> 8,
+		GOODIX_READ_COOR_ADDR & 0xff,
+		0
+	};
+	struct goodix_ts_data *ts = dev_id;
+
+	goodix_process_events(ts);
+
+	if (i2c_master_send(ts->client, end_cmd, sizeof(end_cmd)) < 0)
+		dev_err(&ts->client->dev, "I2C write end_cmd error\n");
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * goodix_read_config - Read the embedded configuration of the panel
+ *
+ * @ts: our goodix_ts_data pointer
+ *
+ * Must be called during probe
+ */
+static void goodix_read_config(struct goodix_ts_data *ts)
+{
+	u8 config[GOODIX_CONFIG_MAX_LENGTH];
+	int error;
+
+	error = goodix_i2c_read(ts->client, GOODIX_REG_CONFIG_DATA,
+			      config,
+			   GOODIX_CONFIG_MAX_LENGTH);
+	if (error) {
+		dev_warn(&ts->client->dev,
+			 "Error reading config (%d), using defaults\n",
+			 error);
+		ts->abs_x_max = GOODIX_MAX_WIDTH;
+		ts->abs_y_max = GOODIX_MAX_HEIGHT;
+		ts->int_trigger_type = GOODIX_INT_TRIGGER;
+		return;
+	}
+
+	ts->abs_x_max = get_unaligned_le16(&config[RESOLUTION_LOC]);
+	ts->abs_y_max = get_unaligned_le16(&config[RESOLUTION_LOC + 2]);
+	ts->int_trigger_type = (config[TRIGGER_LOC]) & 0x03;
+	if (!ts->abs_x_max || !ts->abs_y_max) {
+		dev_err(&ts->client->dev,
+			"Invalid config, using defaults\n");
+		ts->abs_x_max = GOODIX_MAX_WIDTH;
+		ts->abs_y_max = GOODIX_MAX_HEIGHT;
+	}
+}
+
+
+/**
+ * goodix_read_version - Read goodix touchscreen version
+ *
+ * @client: the i2c client
+ * @version: output buffer containing the version on success
+ */
+static int goodix_read_version(struct i2c_client *client, u16 *version)
+{
+	int error;
+	u8 buf[6];
+
+	error = goodix_i2c_read(client, GOODIX_REG_VERSION, buf, sizeof(buf));
+	if (error) {
+		dev_err(&client->dev, "read version failed: %d\n", error);
+		return error;
+	}
+
+	if (version)
+		*version = get_unaligned_le16(&buf[4]);
+
+	dev_info(&client->dev, "IC VERSION: %6ph\n", buf);
+
+	return 0;
+}
+
+/**
+ * goodix_i2c_test - I2C test function to check if the device answers.
+ *
+ * @client: the i2c client
+ */
+static int goodix_i2c_test(struct i2c_client *client)
+{
+	int retry = 0;
+	int error;
+	u8 test;
+
+	while (retry++ < 2) {
+		error = goodix_i2c_read(client, GOODIX_REG_CONFIG_DATA,
+					&test, 1);
+		if (!error)
+			return 0;
+
+		dev_err(&client->dev, "i2c test failed attempt %d: %d\n",
+			retry, error);
+		msleep(20);
+	}
+
+	return error;
+}
+
+/**
+ * goodix_request_input_dev - Allocate, populate and register the input device
+ *
+ * @ts: our goodix_ts_data pointer
+ *
+ * Must be called during probe
+ */
+static int goodix_request_input_dev(struct goodix_ts_data *ts)
+{
+	int error;
+
+	ts->input_dev = devm_input_allocate_device(&ts->client->dev);
+	if (!ts->input_dev) {
+		dev_err(&ts->client->dev, "Failed to allocate input device.");
+		return -ENOMEM;
+	}
+
+	ts->input_dev->evbit[0] = BIT_MASK(EV_SYN) |
+				  BIT_MASK(EV_KEY) |
+				  BIT_MASK(EV_ABS);
+
+	input_set_abs_params(ts->input_dev, ABS_MT_POSITION_X, 0,
+				ts->abs_x_max, 0, 0);
+	input_set_abs_params(ts->input_dev, ABS_MT_POSITION_Y, 0,
+				ts->abs_y_max, 0, 0);
+	input_set_abs_params(ts->input_dev, ABS_MT_WIDTH_MAJOR, 0, 255, 0, 0);
+	input_set_abs_params(ts->input_dev, ABS_MT_TOUCH_MAJOR, 0, 255, 0, 0);
+
+	input_mt_init_slots(ts->input_dev, GOODIX_MAX_CONTACTS,
+			    INPUT_MT_DIRECT | INPUT_MT_DROP_UNUSED);
+
+	ts->input_dev->name = "Goodix Capacitive TouchScreen";
+	ts->input_dev->phys = "input/ts";
+	ts->input_dev->id.bustype = BUS_I2C;
+	ts->input_dev->id.vendor = 0x0416;
+	ts->input_dev->id.product = 0x1001;
+	ts->input_dev->id.version = 10427;
+
+	error = input_register_device(ts->input_dev);
+	if (error) {
+		dev_err(&ts->client->dev,
+			"Failed to register input device: %d", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int goodix_ts_probe(struct i2c_client *client,
+			   const struct i2c_device_id *id)
+{
+	struct goodix_ts_data *ts;
+	unsigned long irq_flags;
+	int error;
+	u16 version_info;
+
+	dev_dbg(&client->dev, "I2C Address: 0x%02x\n", client->addr);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev, "I2C check functionality failed.\n");
+		return -ENXIO;
+	}
+
+	ts = devm_kzalloc(&client->dev, sizeof(*ts), GFP_KERNEL);
+	if (!ts)
+		return -ENOMEM;
+
+	ts->client = client;
+	i2c_set_clientdata(client, ts);
+
+	error = goodix_i2c_test(client);
+	if (error) {
+		dev_err(&client->dev, "I2C communication failure: %d\n", error);
+		return error;
+	}
+
+	error = goodix_read_version(client, &version_info);
+	if (error) {
+		dev_err(&client->dev, "Read version failed.\n");
+		return error;
+	}
+
+	goodix_read_config(ts);
+
+	error = goodix_request_input_dev(ts);
+	if (error)
+		return error;
+
+	irq_flags = goodix_irq_flags[ts->int_trigger_type] | IRQF_ONESHOT;
+	error = devm_request_threaded_irq(&ts->client->dev, client->irq,
+					  NULL, goodix_ts_irq_handler,
+					  irq_flags, client->name, ts);
+	if (error) {
+		dev_err(&client->dev, "request IRQ failed: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static const struct i2c_device_id goodix_ts_id[] = {
+	{ "GDIX1001:00", 0 },
+	{ }
+};
+
+static const struct acpi_device_id goodix_acpi_match[] = {
+	{ "GDIX1001", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(acpi, goodix_acpi_match);
+
+static struct i2c_driver goodix_ts_driver = {
+	.probe = goodix_ts_probe,
+	.id_table = goodix_ts_id,
+	.driver = {
+		.name = "Goodix-TS",
+		.owner = THIS_MODULE,
+		.acpi_match_table = goodix_acpi_match,
+	},
+};
+module_i2c_driver(goodix_ts_driver);
+
+MODULE_AUTHOR("Benjamin Tissoires <benjamin.tissoires@gmail.com>");
+MODULE_AUTHOR("Bastien Nocera <hadess@hadess.net>");
+MODULE_DESCRIPTION("Goodix touchscreen driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/input/touchscreen/ili210x.c b/drivers/input/touchscreen/ili210x.c
index 2a5089139818..da6dc819c846 100644
--- a/drivers/input/touchscreen/ili210x.c
+++ b/drivers/input/touchscreen/ili210x.c
@@ -311,8 +311,7 @@ static int ili210x_i2c_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int ili210x_i2c_suspend(struct device *dev)
+static int __maybe_unused ili210x_i2c_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 
@@ -322,7 +321,7 @@ static int ili210x_i2c_suspend(struct device *dev)
 	return 0;
 }
 
-static int ili210x_i2c_resume(struct device *dev)
+static int __maybe_unused ili210x_i2c_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 
@@ -331,7 +330,6 @@ static int ili210x_i2c_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(ili210x_i2c_pm,
 			 ili210x_i2c_suspend, ili210x_i2c_resume);
diff --git a/drivers/input/touchscreen/intel-mid-touch.c b/drivers/input/touchscreen/intel-mid-touch.c
index c38ca4a7e386..b4f0725a1c3d 100644
--- a/drivers/input/touchscreen/intel-mid-touch.c
+++ b/drivers/input/touchscreen/intel-mid-touch.c
@@ -644,7 +644,6 @@ static int mrstouch_probe(struct platform_device *pdev)
 static struct platform_driver mrstouch_driver = {
 	.driver = {
 		.name	= "pmic_touch",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= mrstouch_probe,
 };
diff --git a/drivers/input/touchscreen/ipaq-micro-ts.c b/drivers/input/touchscreen/ipaq-micro-ts.c
index 62c8976e616f..33c134820ef9 100644
--- a/drivers/input/touchscreen/ipaq-micro-ts.c
+++ b/drivers/input/touchscreen/ipaq-micro-ts.c
@@ -122,8 +122,7 @@ static int micro_ts_probe(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int micro_ts_suspend(struct device *dev)
+static int __maybe_unused micro_ts_suspend(struct device *dev)
 {
 	struct touchscreen_data *ts = dev_get_drvdata(dev);
 
@@ -132,7 +131,7 @@ static int micro_ts_suspend(struct device *dev)
 	return 0;
 }
 
-static int micro_ts_resume(struct device *dev)
+static int __maybe_unused micro_ts_resume(struct device *dev)
 {
 	struct touchscreen_data *ts = dev_get_drvdata(dev);
 	struct input_dev *input = ts->input;
@@ -146,7 +145,6 @@ static int micro_ts_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static const struct dev_pm_ops micro_ts_dev_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(micro_ts_suspend, micro_ts_resume)
diff --git a/drivers/input/touchscreen/jornada720_ts.c b/drivers/input/touchscreen/jornada720_ts.c
index 651ec71a5c68..ea3b6a5b83e6 100644
--- a/drivers/input/touchscreen/jornada720_ts.c
+++ b/drivers/input/touchscreen/jornada720_ts.c
@@ -148,7 +148,6 @@ static struct platform_driver jornada720_ts_driver = {
 	.probe		= jornada720_ts_probe,
 	.driver		= {
 		.name	= "jornada_ts",
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(jornada720_ts_driver);
diff --git a/drivers/input/touchscreen/lpc32xx_ts.c b/drivers/input/touchscreen/lpc32xx_ts.c
index bb47d3442a35..24d704cd9f88 100644
--- a/drivers/input/touchscreen/lpc32xx_ts.c
+++ b/drivers/input/touchscreen/lpc32xx_ts.c
@@ -396,7 +396,6 @@ static struct platform_driver lpc32xx_ts_driver = {
 	.remove		= lpc32xx_ts_remove,
 	.driver		= {
 		.name	= MOD_NAME,
-		.owner	= THIS_MODULE,
 		.pm	= LPC32XX_TS_PM_OPS,
 		.of_match_table = of_match_ptr(lpc32xx_tsc_of_match),
 	},
diff --git a/drivers/input/touchscreen/mc13783_ts.c b/drivers/input/touchscreen/mc13783_ts.c
index d6f099c47f84..913e25a994b4 100644
--- a/drivers/input/touchscreen/mc13783_ts.c
+++ b/drivers/input/touchscreen/mc13783_ts.c
@@ -243,7 +243,6 @@ static int mc13783_ts_remove(struct platform_device *pdev)
 static struct platform_driver mc13783_ts_driver = {
 	.remove		= mc13783_ts_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= MC13783_TS_NAME,
 	},
 };
diff --git a/drivers/input/touchscreen/mms114.c b/drivers/input/touchscreen/mms114.c
index 372bbf7658fe..67c0d31613d8 100644
--- a/drivers/input/touchscreen/mms114.c
+++ b/drivers/input/touchscreen/mms114.c
@@ -515,8 +515,7 @@ static int mms114_probe(struct i2c_client *client,
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int mms114_suspend(struct device *dev)
+static int __maybe_unused mms114_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct mms114_data *data = i2c_get_clientdata(client);
@@ -540,7 +539,7 @@ static int mms114_suspend(struct device *dev)
 	return 0;
 }
 
-static int mms114_resume(struct device *dev)
+static int __maybe_unused mms114_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct mms114_data *data = i2c_get_clientdata(client);
@@ -559,7 +558,6 @@ static int mms114_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(mms114_pm_ops, mms114_suspend, mms114_resume);
 
diff --git a/drivers/input/touchscreen/pcap_ts.c b/drivers/input/touchscreen/pcap_ts.c
index cff2376817e5..23a354a392ae 100644
--- a/drivers/input/touchscreen/pcap_ts.c
+++ b/drivers/input/touchscreen/pcap_ts.c
@@ -247,7 +247,6 @@ static struct platform_driver pcap_ts_driver = {
 	.remove		= pcap_ts_remove,
 	.driver		= {
 		.name	= "pcap-ts",
-		.owner	= THIS_MODULE,
 		.pm	= PCAP_TS_PM_OPS,
 	},
 };
diff --git a/drivers/input/touchscreen/pixcir_i2c_ts.c b/drivers/input/touchscreen/pixcir_i2c_ts.c
index fc49c75317d1..4fb5537fdd42 100644
--- a/drivers/input/touchscreen/pixcir_i2c_ts.c
+++ b/drivers/input/touchscreen/pixcir_i2c_ts.c
@@ -347,8 +347,7 @@ static void pixcir_input_close(struct input_dev *dev)
 	pixcir_stop(ts);
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int pixcir_i2c_ts_suspend(struct device *dev)
+static int __maybe_unused pixcir_i2c_ts_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct pixcir_i2c_ts_data *ts = i2c_get_clientdata(client);
@@ -377,7 +376,7 @@ unlock:
 	return ret;
 }
 
-static int pixcir_i2c_ts_resume(struct device *dev)
+static int __maybe_unused pixcir_i2c_ts_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct pixcir_i2c_ts_data *ts = i2c_get_clientdata(client);
@@ -405,7 +404,6 @@ unlock:
 
 	return ret;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(pixcir_dev_pm_ops,
 			 pixcir_i2c_ts_suspend, pixcir_i2c_ts_resume);
diff --git a/drivers/input/touchscreen/s3c2410_ts.c b/drivers/input/touchscreen/s3c2410_ts.c
index 5a69ded9b53c..bdfa27dc097b 100644
--- a/drivers/input/touchscreen/s3c2410_ts.c
+++ b/drivers/input/touchscreen/s3c2410_ts.c
@@ -422,7 +422,6 @@ MODULE_DEVICE_TABLE(platform, s3cts_driver_ids);
 static struct platform_driver s3c_ts_driver = {
 	.driver         = {
 		.name   = "samsung-ts",
-		.owner  = THIS_MODULE,
 #ifdef CONFIG_PM
 		.pm	= &s3c_ts_pmops,
 #endif
diff --git a/drivers/input/touchscreen/st1232.c b/drivers/input/touchscreen/st1232.c
index 3c0f57efe7b1..697e26e52d54 100644
--- a/drivers/input/touchscreen/st1232.c
+++ b/drivers/input/touchscreen/st1232.c
@@ -243,8 +243,7 @@ static int st1232_ts_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int st1232_ts_suspend(struct device *dev)
+static int __maybe_unused st1232_ts_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct st1232_ts_data *ts = i2c_get_clientdata(client);
@@ -259,7 +258,7 @@ static int st1232_ts_suspend(struct device *dev)
 	return 0;
 }
 
-static int st1232_ts_resume(struct device *dev)
+static int __maybe_unused st1232_ts_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct st1232_ts_data *ts = i2c_get_clientdata(client);
@@ -274,8 +273,6 @@ static int st1232_ts_resume(struct device *dev)
 	return 0;
 }
 
-#endif
-
 static SIMPLE_DEV_PM_OPS(st1232_ts_pm_ops,
 			 st1232_ts_suspend, st1232_ts_resume);
 
diff --git a/drivers/input/touchscreen/stmpe-ts.c b/drivers/input/touchscreen/stmpe-ts.c
index 42ce31afa259..2d5ff86b343f 100644
--- a/drivers/input/touchscreen/stmpe-ts.c
+++ b/drivers/input/touchscreen/stmpe-ts.c
@@ -384,7 +384,6 @@ static int stmpe_ts_remove(struct platform_device *pdev)
 static struct platform_driver stmpe_ts_driver = {
 	.driver = {
 		   .name = STMPE_TS_NAME,
-		   .owner = THIS_MODULE,
 		   },
 	.probe = stmpe_input_probe,
 	.remove = stmpe_ts_remove,
diff --git a/drivers/input/touchscreen/sun4i-ts.c b/drivers/input/touchscreen/sun4i-ts.c
index 2ba826024954..28a06749ae42 100644
--- a/drivers/input/touchscreen/sun4i-ts.c
+++ b/drivers/input/touchscreen/sun4i-ts.c
@@ -324,7 +324,6 @@ MODULE_DEVICE_TABLE(of, sun4i_ts_of_match);
 
 static struct platform_driver sun4i_ts_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= "sun4i-ts",
 		.of_match_table = of_match_ptr(sun4i_ts_of_match),
 	},
diff --git a/drivers/input/touchscreen/ti_am335x_tsc.c b/drivers/input/touchscreen/ti_am335x_tsc.c
index 2ce649520fe0..004f1346a957 100644
--- a/drivers/input/touchscreen/ti_am335x_tsc.c
+++ b/drivers/input/touchscreen/ti_am335x_tsc.c
@@ -518,7 +518,6 @@ static struct platform_driver ti_tsc_driver = {
 	.remove	= titsc_remove,
 	.driver	= {
 		.name   = "TI-am335x-tsc",
-		.owner	= THIS_MODULE,
 		.pm	= TITSC_PM_OPS,
 		.of_match_table = ti_tsc_dt_ids,
 	},
diff --git a/drivers/input/touchscreen/tps6507x-ts.c b/drivers/input/touchscreen/tps6507x-ts.c
index 94cde2cb1491..4ffd829d1990 100644
--- a/drivers/input/touchscreen/tps6507x-ts.c
+++ b/drivers/input/touchscreen/tps6507x-ts.c
@@ -314,7 +314,6 @@ static int tps6507x_ts_remove(struct platform_device *pdev)
 static struct platform_driver tps6507x_ts_driver = {
 	.driver = {
 		.name = "tps6507x-ts",
-		.owner = THIS_MODULE,
 	},
 	.probe = tps6507x_ts_probe,
 	.remove = tps6507x_ts_remove,
diff --git a/drivers/input/touchscreen/tsc2005.c b/drivers/input/touchscreen/tsc2005.c
index 52380b68ebdf..72657c579430 100644
--- a/drivers/input/touchscreen/tsc2005.c
+++ b/drivers/input/touchscreen/tsc2005.c
@@ -773,8 +773,7 @@ static int tsc2005_remove(struct spi_device *spi)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int tsc2005_suspend(struct device *dev)
+static int __maybe_unused tsc2005_suspend(struct device *dev)
 {
 	struct spi_device *spi = to_spi_device(dev);
 	struct tsc2005 *ts = spi_get_drvdata(spi);
@@ -791,7 +790,7 @@ static int tsc2005_suspend(struct device *dev)
 	return 0;
 }
 
-static int tsc2005_resume(struct device *dev)
+static int __maybe_unused tsc2005_resume(struct device *dev)
 {
 	struct spi_device *spi = to_spi_device(dev);
 	struct tsc2005 *ts = spi_get_drvdata(spi);
@@ -807,7 +806,6 @@ static int tsc2005_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(tsc2005_pm_ops, tsc2005_suspend, tsc2005_resume);
 
diff --git a/drivers/input/touchscreen/ucb1400_ts.c b/drivers/input/touchscreen/ucb1400_ts.c
index b46c55cd1bbb..c1e23cfc6155 100644
--- a/drivers/input/touchscreen/ucb1400_ts.c
+++ b/drivers/input/touchscreen/ucb1400_ts.c
@@ -406,8 +406,7 @@ static int ucb1400_ts_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int ucb1400_ts_suspend(struct device *dev)
+static int __maybe_unused ucb1400_ts_suspend(struct device *dev)
 {
 	struct ucb1400_ts *ucb = dev_get_platdata(dev);
 	struct input_dev *idev = ucb->ts_idev;
@@ -421,7 +420,7 @@ static int ucb1400_ts_suspend(struct device *dev)
 	return 0;
 }
 
-static int ucb1400_ts_resume(struct device *dev)
+static int __maybe_unused ucb1400_ts_resume(struct device *dev)
 {
 	struct ucb1400_ts *ucb = dev_get_platdata(dev);
 	struct input_dev *idev = ucb->ts_idev;
@@ -434,7 +433,6 @@ static int ucb1400_ts_resume(struct device *dev)
 	mutex_unlock(&idev->mutex);
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(ucb1400_ts_pm_ops,
 			 ucb1400_ts_suspend, ucb1400_ts_resume);
@@ -444,7 +442,6 @@ static struct platform_driver ucb1400_ts_driver = {
 	.remove	= ucb1400_ts_remove,
 	.driver	= {
 		.name	= "ucb1400_ts",
-		.owner	= THIS_MODULE,
 		.pm	= &ucb1400_ts_pm_ops,
 	},
 };
diff --git a/drivers/input/touchscreen/w90p910_ts.c b/drivers/input/touchscreen/w90p910_ts.c
index 003d0c3b5d08..da6004e97753 100644
--- a/drivers/input/touchscreen/w90p910_ts.c
+++ b/drivers/input/touchscreen/w90p910_ts.c
@@ -326,7 +326,6 @@ static struct platform_driver w90x900ts_driver = {
 	.remove		= w90x900ts_remove,
 	.driver		= {
 		.name	= "nuc900-ts",
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(w90x900ts_driver);
diff --git a/drivers/input/touchscreen/wacom_i2c.c b/drivers/input/touchscreen/wacom_i2c.c
index 7ccaa1b12b05..32f8ac003936 100644
--- a/drivers/input/touchscreen/wacom_i2c.c
+++ b/drivers/input/touchscreen/wacom_i2c.c
@@ -242,8 +242,7 @@ static int wacom_i2c_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int wacom_i2c_suspend(struct device *dev)
+static int __maybe_unused wacom_i2c_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 
@@ -252,7 +251,7 @@ static int wacom_i2c_suspend(struct device *dev)
 	return 0;
 }
 
-static int wacom_i2c_resume(struct device *dev)
+static int __maybe_unused wacom_i2c_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 
@@ -260,7 +259,6 @@ static int wacom_i2c_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(wacom_i2c_pm, wacom_i2c_suspend, wacom_i2c_resume);
 
diff --git a/drivers/input/touchscreen/wm831x-ts.c b/drivers/input/touchscreen/wm831x-ts.c
index 1b953a066b2c..1db0a1410929 100644
--- a/drivers/input/touchscreen/wm831x-ts.c
+++ b/drivers/input/touchscreen/wm831x-ts.c
@@ -392,7 +392,6 @@ static int wm831x_ts_remove(struct platform_device *pdev)
 static struct platform_driver wm831x_ts_driver = {
 	.driver = {
 		.name = "wm831x-touch",
-		.owner = THIS_MODULE,
 	},
 	.probe = wm831x_ts_probe,
 	.remove = wm831x_ts_remove,
diff --git a/drivers/input/touchscreen/zforce_ts.c b/drivers/input/touchscreen/zforce_ts.c
index 8ba48f5eff7b..19880c7385e3 100644
--- a/drivers/input/touchscreen/zforce_ts.c
+++ b/drivers/input/touchscreen/zforce_ts.c
@@ -602,8 +602,7 @@ static void zforce_input_close(struct input_dev *dev)
 	return;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int zforce_suspend(struct device *dev)
+static int __maybe_unused zforce_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct zforce_ts *ts = i2c_get_clientdata(client);
@@ -648,7 +647,7 @@ unlock:
 	return ret;
 }
 
-static int zforce_resume(struct device *dev)
+static int __maybe_unused zforce_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct zforce_ts *ts = i2c_get_clientdata(client);
@@ -685,7 +684,6 @@ unlock:
 
 	return ret;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(zforce_pm_ops, zforce_suspend, zforce_resume);
 
diff --git a/drivers/iommu/Kconfig b/drivers/iommu/Kconfig
index 6dbfbc209491..325188eef1c1 100644
--- a/drivers/iommu/Kconfig
+++ b/drivers/iommu/Kconfig
@@ -15,7 +15,7 @@ if IOMMU_SUPPORT
 
 config OF_IOMMU
        def_bool y
-       depends on OF
+       depends on OF && IOMMU_API
 
 config FSL_PAMU
 	bool "Freescale IOMMU support"
@@ -144,13 +144,26 @@ config OMAP_IOMMU
 	select IOMMU_API
 
 config OMAP_IOMMU_DEBUG
-       tristate "Export OMAP IOMMU internals in DebugFS"
-       depends on OMAP_IOMMU && DEBUG_FS
-       help
-         Select this to see extensive information about
-         the internal state of OMAP IOMMU in debugfs.
+	bool "Export OMAP IOMMU internals in DebugFS"
+	depends on OMAP_IOMMU && DEBUG_FS
+	---help---
+	  Select this to see extensive information about
+	  the internal state of OMAP IOMMU in debugfs.
+
+	  Say N unless you know you need this.
 
-         Say N unless you know you need this.
+config ROCKCHIP_IOMMU
+	bool "Rockchip IOMMU Support"
+	depends on ARM
+	depends on ARCH_ROCKCHIP || COMPILE_TEST
+	select IOMMU_API
+	select ARM_DMA_USE_IOMMU
+	help
+	  Support for IOMMUs found on Rockchip rk32xx SOCs.
+	  These IOMMUs allow virtualization of the address space used by most
+	  cores within the multimedia subsystem.
+	  Say Y here if you are using a Rockchip SoC that includes an IOMMU
+	  device.
 
 config TEGRA_IOMMU_GART
 	bool "Tegra GART IOMMU Support"
@@ -174,7 +187,7 @@ config TEGRA_IOMMU_SMMU
 
 config EXYNOS_IOMMU
 	bool "Exynos IOMMU Support"
-	depends on ARCH_EXYNOS
+	depends on ARCH_EXYNOS && ARM
 	select IOMMU_API
 	select ARM_DMA_USE_IOMMU
 	help
diff --git a/drivers/iommu/Makefile b/drivers/iommu/Makefile
index 16edef74b8ee..7b976f294a69 100644
--- a/drivers/iommu/Makefile
+++ b/drivers/iommu/Makefile
@@ -11,8 +11,8 @@ obj-$(CONFIG_INTEL_IOMMU) += iova.o intel-iommu.o
 obj-$(CONFIG_IPMMU_VMSA) += ipmmu-vmsa.o
 obj-$(CONFIG_IRQ_REMAP) += intel_irq_remapping.o irq_remapping.o
 obj-$(CONFIG_OMAP_IOMMU) += omap-iommu.o
-obj-$(CONFIG_OMAP_IOMMU) += omap-iommu2.o
 obj-$(CONFIG_OMAP_IOMMU_DEBUG) += omap-iommu-debug.o
+obj-$(CONFIG_ROCKCHIP_IOMMU) += rockchip-iommu.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/amd_iommu.c b/drivers/iommu/amd_iommu.c
index 2d84c9edf3b8..98024856df07 100644
--- a/drivers/iommu/amd_iommu.c
+++ b/drivers/iommu/amd_iommu.c
@@ -3411,6 +3411,8 @@ static bool amd_iommu_capable(enum iommu_cap cap)
 		return true;
 	case IOMMU_CAP_INTR_REMAP:
 		return (irq_remapping_enabled == 1);
+	case IOMMU_CAP_NOEXEC:
+		return false;
 	}
 
 	return false;
@@ -4069,7 +4071,7 @@ static int setup_ioapic_entry(int irq, struct IO_APIC_route_entry *entry,
 	int devid;
 	int ret;
 
-	cfg = irq_get_chip_data(irq);
+	cfg = irq_cfg(irq);
 	if (!cfg)
 		return -EINVAL;
 
@@ -4132,7 +4134,7 @@ static int set_affinity(struct irq_data *data, const struct cpumask *mask,
 	if (!config_enabled(CONFIG_SMP))
 		return -1;
 
-	cfg       = data->chip_data;
+	cfg       = irqd_cfg(data);
 	irq       = data->irq;
 	irte_info = &cfg->irq_2_irte;
 
@@ -4170,7 +4172,7 @@ static int free_irq(int irq)
 	struct irq_2_irte *irte_info;
 	struct irq_cfg *cfg;
 
-	cfg = irq_get_chip_data(irq);
+	cfg = irq_cfg(irq);
 	if (!cfg)
 		return -EINVAL;
 
@@ -4189,7 +4191,7 @@ static void compose_msi_msg(struct pci_dev *pdev,
 	struct irq_cfg *cfg;
 	union irte irte;
 
-	cfg = irq_get_chip_data(irq);
+	cfg = irq_cfg(irq);
 	if (!cfg)
 		return;
 
@@ -4218,7 +4220,7 @@ static int msi_alloc_irq(struct pci_dev *pdev, int irq, int nvec)
 	if (!pdev)
 		return -EINVAL;
 
-	cfg = irq_get_chip_data(irq);
+	cfg = irq_cfg(irq);
 	if (!cfg)
 		return -EINVAL;
 
@@ -4238,7 +4240,7 @@ static int msi_setup_irq(struct pci_dev *pdev, unsigned int irq,
 	if (!pdev)
 		return -EINVAL;
 
-	cfg = irq_get_chip_data(irq);
+	cfg = irq_cfg(irq);
 	if (!cfg)
 		return -EINVAL;
 
@@ -4261,7 +4263,7 @@ static int alloc_hpet_msi(unsigned int irq, unsigned int id)
 	struct irq_cfg *cfg;
 	int index, devid;
 
-	cfg = irq_get_chip_data(irq);
+	cfg = irq_cfg(irq);
 	if (!cfg)
 		return -EINVAL;
 
diff --git a/drivers/iommu/amd_iommu_v2.c b/drivers/iommu/amd_iommu_v2.c
index 90d734bbf467..90f70d0e1141 100644
--- a/drivers/iommu/amd_iommu_v2.c
+++ b/drivers/iommu/amd_iommu_v2.c
@@ -92,13 +92,6 @@ static spinlock_t state_lock;
 
 static struct workqueue_struct *iommu_wq;
 
-/*
- * Empty page table - Used between
- * mmu_notifier_invalidate_range_start and
- * mmu_notifier_invalidate_range_end
- */
-static u64 *empty_page_table;
-
 static void free_pasid_states(struct device_state *dev_state);
 
 static u16 device_id(struct pci_dev *pdev)
@@ -279,10 +272,8 @@ static void free_pasid_state(struct pasid_state *pasid_state)
 
 static void put_pasid_state(struct pasid_state *pasid_state)
 {
-	if (atomic_dec_and_test(&pasid_state->count)) {
-		put_device_state(pasid_state->device_state);
+	if (atomic_dec_and_test(&pasid_state->count))
 		wake_up(&pasid_state->wq);
-	}
 }
 
 static void put_pasid_state_wait(struct pasid_state *pasid_state)
@@ -291,9 +282,7 @@ static void put_pasid_state_wait(struct pasid_state *pasid_state)
 
 	prepare_to_wait(&pasid_state->wq, &wait, TASK_UNINTERRUPTIBLE);
 
-	if (atomic_dec_and_test(&pasid_state->count))
-		put_device_state(pasid_state->device_state);
-	else
+	if (!atomic_dec_and_test(&pasid_state->count))
 		schedule();
 
 	finish_wait(&pasid_state->wq, &wait);
@@ -418,46 +407,21 @@ static void mn_invalidate_page(struct mmu_notifier *mn,
 	__mn_flush_page(mn, address);
 }
 
-static void mn_invalidate_range_start(struct mmu_notifier *mn,
-				      struct mm_struct *mm,
-				      unsigned long start, unsigned long end)
-{
-	struct pasid_state *pasid_state;
-	struct device_state *dev_state;
-	unsigned long flags;
-
-	pasid_state = mn_to_state(mn);
-	dev_state   = pasid_state->device_state;
-
-	spin_lock_irqsave(&pasid_state->lock, flags);
-	if (pasid_state->mmu_notifier_count == 0) {
-		amd_iommu_domain_set_gcr3(dev_state->domain,
-					  pasid_state->pasid,
-					  __pa(empty_page_table));
-	}
-	pasid_state->mmu_notifier_count += 1;
-	spin_unlock_irqrestore(&pasid_state->lock, flags);
-}
-
-static void mn_invalidate_range_end(struct mmu_notifier *mn,
-				    struct mm_struct *mm,
-				    unsigned long start, unsigned long end)
+static void mn_invalidate_range(struct mmu_notifier *mn,
+				struct mm_struct *mm,
+				unsigned long start, unsigned long end)
 {
 	struct pasid_state *pasid_state;
 	struct device_state *dev_state;
-	unsigned long flags;
 
 	pasid_state = mn_to_state(mn);
 	dev_state   = pasid_state->device_state;
 
-	spin_lock_irqsave(&pasid_state->lock, flags);
-	pasid_state->mmu_notifier_count -= 1;
-	if (pasid_state->mmu_notifier_count == 0) {
-		amd_iommu_domain_set_gcr3(dev_state->domain,
-					  pasid_state->pasid,
-					  __pa(pasid_state->mm->pgd));
-	}
-	spin_unlock_irqrestore(&pasid_state->lock, flags);
+	if ((start ^ (end - 1)) < PAGE_SIZE)
+		amd_iommu_flush_page(dev_state->domain, pasid_state->pasid,
+				     start);
+	else
+		amd_iommu_flush_tlb(dev_state->domain, pasid_state->pasid);
 }
 
 static void mn_release(struct mmu_notifier *mn, struct mm_struct *mm)
@@ -482,8 +446,7 @@ static struct mmu_notifier_ops iommu_mn = {
 	.release		= mn_release,
 	.clear_flush_young      = mn_clear_flush_young,
 	.invalidate_page        = mn_invalidate_page,
-	.invalidate_range_start = mn_invalidate_range_start,
-	.invalidate_range_end   = mn_invalidate_range_end,
+	.invalidate_range       = mn_invalidate_range,
 };
 
 static void set_pri_tag_status(struct pasid_state *pasid_state,
@@ -513,45 +476,67 @@ static void finish_pri_tag(struct device_state *dev_state,
 	spin_unlock_irqrestore(&pasid_state->lock, flags);
 }
 
+static void handle_fault_error(struct fault *fault)
+{
+	int status;
+
+	if (!fault->dev_state->inv_ppr_cb) {
+		set_pri_tag_status(fault->state, fault->tag, PPR_INVALID);
+		return;
+	}
+
+	status = fault->dev_state->inv_ppr_cb(fault->dev_state->pdev,
+					      fault->pasid,
+					      fault->address,
+					      fault->flags);
+	switch (status) {
+	case AMD_IOMMU_INV_PRI_RSP_SUCCESS:
+		set_pri_tag_status(fault->state, fault->tag, PPR_SUCCESS);
+		break;
+	case AMD_IOMMU_INV_PRI_RSP_INVALID:
+		set_pri_tag_status(fault->state, fault->tag, PPR_INVALID);
+		break;
+	case AMD_IOMMU_INV_PRI_RSP_FAIL:
+		set_pri_tag_status(fault->state, fault->tag, PPR_FAILURE);
+		break;
+	default:
+		BUG();
+	}
+}
+
 static void do_fault(struct work_struct *work)
 {
 	struct fault *fault = container_of(work, struct fault, work);
-	int npages, write;
-	struct page *page;
+	struct mm_struct *mm;
+	struct vm_area_struct *vma;
+	u64 address;
+	int ret, write;
 
 	write = !!(fault->flags & PPR_FAULT_WRITE);
 
-	down_read(&fault->state->mm->mmap_sem);
-	npages = get_user_pages(NULL, fault->state->mm,
-				fault->address, 1, write, 0, &page, NULL);
-	up_read(&fault->state->mm->mmap_sem);
-
-	if (npages == 1) {
-		put_page(page);
-	} else if (fault->dev_state->inv_ppr_cb) {
-		int status;
-
-		status = fault->dev_state->inv_ppr_cb(fault->dev_state->pdev,
-						      fault->pasid,
-						      fault->address,
-						      fault->flags);
-		switch (status) {
-		case AMD_IOMMU_INV_PRI_RSP_SUCCESS:
-			set_pri_tag_status(fault->state, fault->tag, PPR_SUCCESS);
-			break;
-		case AMD_IOMMU_INV_PRI_RSP_INVALID:
-			set_pri_tag_status(fault->state, fault->tag, PPR_INVALID);
-			break;
-		case AMD_IOMMU_INV_PRI_RSP_FAIL:
-			set_pri_tag_status(fault->state, fault->tag, PPR_FAILURE);
-			break;
-		default:
-			BUG();
-		}
-	} else {
-		set_pri_tag_status(fault->state, fault->tag, PPR_INVALID);
+	mm = fault->state->mm;
+	address = fault->address;
+
+	down_read(&mm->mmap_sem);
+	vma = find_extend_vma(mm, address);
+	if (!vma || address < vma->vm_start) {
+		/* failed to get a vma in the right range */
+		up_read(&mm->mmap_sem);
+		handle_fault_error(fault);
+		goto out;
+	}
+
+	ret = handle_mm_fault(mm, vma, address, write);
+	if (ret & VM_FAULT_ERROR) {
+		/* failed to service fault */
+		up_read(&mm->mmap_sem);
+		handle_fault_error(fault);
+		goto out;
 	}
 
+	up_read(&mm->mmap_sem);
+
+out:
 	finish_pri_tag(fault->dev_state, fault->state, fault->tag);
 
 	put_pasid_state(fault->state);
@@ -954,18 +939,10 @@ static int __init amd_iommu_v2_init(void)
 	if (iommu_wq == NULL)
 		goto out;
 
-	ret = -ENOMEM;
-	empty_page_table = (u64 *)get_zeroed_page(GFP_KERNEL);
-	if (empty_page_table == NULL)
-		goto out_destroy_wq;
-
 	amd_iommu_register_ppr_notifier(&ppr_nb);
 
 	return 0;
 
-out_destroy_wq:
-	destroy_workqueue(iommu_wq);
-
 out:
 	return ret;
 }
@@ -999,8 +976,6 @@ static void __exit amd_iommu_v2_exit(void)
 	}
 
 	destroy_workqueue(iommu_wq);
-
-	free_page((unsigned long)empty_page_table);
 }
 
 module_init(amd_iommu_v2_init);
diff --git a/drivers/iommu/arm-smmu.c b/drivers/iommu/arm-smmu.c
index e393ae01b5d2..6cd47b75286f 100644
--- a/drivers/iommu/arm-smmu.c
+++ b/drivers/iommu/arm-smmu.c
@@ -404,9 +404,16 @@ struct arm_smmu_cfg {
 #define ARM_SMMU_CB_ASID(cfg)		((cfg)->cbndx)
 #define ARM_SMMU_CB_VMID(cfg)		((cfg)->cbndx + 1)
 
+enum arm_smmu_domain_stage {
+	ARM_SMMU_DOMAIN_S1 = 0,
+	ARM_SMMU_DOMAIN_S2,
+	ARM_SMMU_DOMAIN_NESTED,
+};
+
 struct arm_smmu_domain {
 	struct arm_smmu_device		*smmu;
 	struct arm_smmu_cfg		cfg;
+	enum arm_smmu_domain_stage	stage;
 	spinlock_t			lock;
 };
 
@@ -906,19 +913,46 @@ static int arm_smmu_init_domain_context(struct iommu_domain *domain,
 	if (smmu_domain->smmu)
 		goto out_unlock;
 
-	if (smmu->features & ARM_SMMU_FEAT_TRANS_NESTED) {
+	/*
+	 * Mapping the requested stage onto what we support is surprisingly
+	 * complicated, mainly because the spec allows S1+S2 SMMUs without
+	 * support for nested translation. That means we end up with the
+	 * following table:
+	 *
+	 * Requested        Supported        Actual
+	 *     S1               N              S1
+	 *     S1             S1+S2            S1
+	 *     S1               S2             S2
+	 *     S1               S1             S1
+	 *     N                N              N
+	 *     N              S1+S2            S2
+	 *     N                S2             S2
+	 *     N                S1             S1
+	 *
+	 * Note that you can't actually request stage-2 mappings.
+	 */
+	if (!(smmu->features & ARM_SMMU_FEAT_TRANS_S1))
+		smmu_domain->stage = ARM_SMMU_DOMAIN_S2;
+	if (!(smmu->features & ARM_SMMU_FEAT_TRANS_S2))
+		smmu_domain->stage = ARM_SMMU_DOMAIN_S1;
+
+	switch (smmu_domain->stage) {
+	case ARM_SMMU_DOMAIN_S1:
+		cfg->cbar = CBAR_TYPE_S1_TRANS_S2_BYPASS;
+		start = smmu->num_s2_context_banks;
+		break;
+	case ARM_SMMU_DOMAIN_NESTED:
 		/*
 		 * We will likely want to change this if/when KVM gets
 		 * involved.
 		 */
-		cfg->cbar = CBAR_TYPE_S1_TRANS_S2_BYPASS;
-		start = smmu->num_s2_context_banks;
-	} else if (smmu->features & ARM_SMMU_FEAT_TRANS_S1) {
-		cfg->cbar = CBAR_TYPE_S1_TRANS_S2_BYPASS;
-		start = smmu->num_s2_context_banks;
-	} else {
+	case ARM_SMMU_DOMAIN_S2:
 		cfg->cbar = CBAR_TYPE_S2_TRANS;
 		start = 0;
+		break;
+	default:
+		ret = -EINVAL;
+		goto out_unlock;
 	}
 
 	ret = __arm_smmu_alloc_bitmap(smmu->context_map, start,
@@ -1281,7 +1315,7 @@ static int arm_smmu_alloc_init_pte(struct arm_smmu_device *smmu, pmd_t *pmd,
 				   unsigned long pfn, int prot, int stage)
 {
 	pte_t *pte, *start;
-	pteval_t pteval = ARM_SMMU_PTE_PAGE | ARM_SMMU_PTE_AF | ARM_SMMU_PTE_XN;
+	pteval_t pteval = ARM_SMMU_PTE_PAGE | ARM_SMMU_PTE_AF;
 
 	if (pmd_none(*pmd)) {
 		/* Allocate a new set of tables */
@@ -1315,10 +1349,11 @@ static int arm_smmu_alloc_init_pte(struct arm_smmu_device *smmu, pmd_t *pmd,
 			pteval |= ARM_SMMU_PTE_MEMATTR_NC;
 	}
 
+	if (prot & IOMMU_NOEXEC)
+		pteval |= ARM_SMMU_PTE_XN;
+
 	/* If no access, create a faulting entry to avoid TLB fills */
-	if (prot & IOMMU_EXEC)
-		pteval &= ~ARM_SMMU_PTE_XN;
-	else if (!(prot & (IOMMU_READ | IOMMU_WRITE)))
+	if (!(prot & (IOMMU_READ | IOMMU_WRITE)))
 		pteval &= ~ARM_SMMU_PTE_PAGE;
 
 	pteval |= ARM_SMMU_PTE_SH_IS;
@@ -1568,6 +1603,8 @@ static bool arm_smmu_capable(enum iommu_cap cap)
 		return true;
 	case IOMMU_CAP_INTR_REMAP:
 		return true; /* MSIs are just memory writes */
+	case IOMMU_CAP_NOEXEC:
+		return true;
 	default:
 		return false;
 	}
@@ -1644,21 +1681,57 @@ static void arm_smmu_remove_device(struct device *dev)
 	iommu_group_remove_device(dev);
 }
 
+static int arm_smmu_domain_get_attr(struct iommu_domain *domain,
+				    enum iommu_attr attr, void *data)
+{
+	struct arm_smmu_domain *smmu_domain = domain->priv;
+
+	switch (attr) {
+	case DOMAIN_ATTR_NESTING:
+		*(int *)data = (smmu_domain->stage == ARM_SMMU_DOMAIN_NESTED);
+		return 0;
+	default:
+		return -ENODEV;
+	}
+}
+
+static int arm_smmu_domain_set_attr(struct iommu_domain *domain,
+				    enum iommu_attr attr, void *data)
+{
+	struct arm_smmu_domain *smmu_domain = domain->priv;
+
+	switch (attr) {
+	case DOMAIN_ATTR_NESTING:
+		if (smmu_domain->smmu)
+			return -EPERM;
+		if (*(int *)data)
+			smmu_domain->stage = ARM_SMMU_DOMAIN_NESTED;
+		else
+			smmu_domain->stage = ARM_SMMU_DOMAIN_S1;
+
+		return 0;
+	default:
+		return -ENODEV;
+	}
+}
+
 static const struct iommu_ops arm_smmu_ops = {
-	.capable	= arm_smmu_capable,
-	.domain_init	= arm_smmu_domain_init,
-	.domain_destroy	= arm_smmu_domain_destroy,
-	.attach_dev	= arm_smmu_attach_dev,
-	.detach_dev	= arm_smmu_detach_dev,
-	.map		= arm_smmu_map,
-	.unmap		= arm_smmu_unmap,
-	.map_sg		= default_iommu_map_sg,
-	.iova_to_phys	= arm_smmu_iova_to_phys,
-	.add_device	= arm_smmu_add_device,
-	.remove_device	= arm_smmu_remove_device,
-	.pgsize_bitmap	= (SECTION_SIZE |
-			   ARM_SMMU_PTE_CONT_SIZE |
-			   PAGE_SIZE),
+	.capable		= arm_smmu_capable,
+	.domain_init		= arm_smmu_domain_init,
+	.domain_destroy		= arm_smmu_domain_destroy,
+	.attach_dev		= arm_smmu_attach_dev,
+	.detach_dev		= arm_smmu_detach_dev,
+	.map			= arm_smmu_map,
+	.unmap			= arm_smmu_unmap,
+	.map_sg			= default_iommu_map_sg,
+	.iova_to_phys		= arm_smmu_iova_to_phys,
+	.add_device		= arm_smmu_add_device,
+	.remove_device		= arm_smmu_remove_device,
+	.domain_get_attr	= arm_smmu_domain_get_attr,
+	.domain_set_attr	= arm_smmu_domain_set_attr,
+	.pgsize_bitmap		= (SECTION_SIZE |
+				   ARM_SMMU_PTE_CONT_SIZE |
+				   PAGE_SIZE),
 };
 
 static void arm_smmu_device_reset(struct arm_smmu_device *smmu)
@@ -2063,7 +2136,6 @@ static int arm_smmu_device_remove(struct platform_device *pdev)
 
 static struct platform_driver arm_smmu_driver = {
 	.driver	= {
-		.owner		= THIS_MODULE,
 		.name		= "arm-smmu",
 		.of_match_table	= of_match_ptr(arm_smmu_of_match),
 	},
@@ -2073,8 +2145,20 @@ static struct platform_driver arm_smmu_driver = {
 
 static int __init arm_smmu_init(void)
 {
+	struct device_node *np;
 	int ret;
 
+	/*
+	 * Play nice with systems that don't have an ARM SMMU by checking that
+	 * an ARM SMMU exists in the system before proceeding with the driver
+	 * and IOMMU bus operation registration.
+	 */
+	np = of_find_matching_node(NULL, arm_smmu_of_match);
+	if (!np)
+		return 0;
+
+	of_node_put(np);
+
 	ret = platform_driver_register(&arm_smmu_driver);
 	if (ret)
 		return ret;
diff --git a/drivers/iommu/dmar.c b/drivers/iommu/dmar.c
index c5c61cabd6e3..9847613085e1 100644
--- a/drivers/iommu/dmar.c
+++ b/drivers/iommu/dmar.c
@@ -44,6 +44,14 @@
 
 #include "irq_remapping.h"
 
+typedef int (*dmar_res_handler_t)(struct acpi_dmar_header *, void *);
+struct dmar_res_callback {
+	dmar_res_handler_t	cb[ACPI_DMAR_TYPE_RESERVED];
+	void			*arg[ACPI_DMAR_TYPE_RESERVED];
+	bool			ignore_unhandled;
+	bool			print_entry;
+};
+
 /*
  * Assumptions:
  * 1) The hotplug framework guarentees that DMAR unit will be hot-added
@@ -62,11 +70,12 @@ LIST_HEAD(dmar_drhd_units);
 struct acpi_table_header * __initdata dmar_tbl;
 static acpi_size dmar_tbl_size;
 static int dmar_dev_scope_status = 1;
+static unsigned long dmar_seq_ids[BITS_TO_LONGS(DMAR_UNITS_SUPPORTED)];
 
 static int alloc_iommu(struct dmar_drhd_unit *drhd);
 static void free_iommu(struct intel_iommu *iommu);
 
-static void __init dmar_register_drhd_unit(struct dmar_drhd_unit *drhd)
+static void dmar_register_drhd_unit(struct dmar_drhd_unit *drhd)
 {
 	/*
 	 * add INCLUDE_ALL at the tail, so scan the list will find it at
@@ -344,24 +353,45 @@ static struct notifier_block dmar_pci_bus_nb = {
 	.priority = INT_MIN,
 };
 
+static struct dmar_drhd_unit *
+dmar_find_dmaru(struct acpi_dmar_hardware_unit *drhd)
+{
+	struct dmar_drhd_unit *dmaru;
+
+	list_for_each_entry_rcu(dmaru, &dmar_drhd_units, list)
+		if (dmaru->segment == drhd->segment &&
+		    dmaru->reg_base_addr == drhd->address)
+			return dmaru;
+
+	return NULL;
+}
+
 /**
  * dmar_parse_one_drhd - parses exactly one DMA remapping hardware definition
  * structure which uniquely represent one DMA remapping hardware unit
  * present in the platform
  */
-static int __init
-dmar_parse_one_drhd(struct acpi_dmar_header *header)
+static int dmar_parse_one_drhd(struct acpi_dmar_header *header, void *arg)
 {
 	struct acpi_dmar_hardware_unit *drhd;
 	struct dmar_drhd_unit *dmaru;
 	int ret = 0;
 
 	drhd = (struct acpi_dmar_hardware_unit *)header;
-	dmaru = kzalloc(sizeof(*dmaru), GFP_KERNEL);
+	dmaru = dmar_find_dmaru(drhd);
+	if (dmaru)
+		goto out;
+
+	dmaru = kzalloc(sizeof(*dmaru) + header->length, GFP_KERNEL);
 	if (!dmaru)
 		return -ENOMEM;
 
-	dmaru->hdr = header;
+	/*
+	 * If header is allocated from slab by ACPI _DSM method, we need to
+	 * copy the content because the memory buffer will be freed on return.
+	 */
+	dmaru->hdr = (void *)(dmaru + 1);
+	memcpy(dmaru->hdr, header, header->length);
 	dmaru->reg_base_addr = drhd->address;
 	dmaru->segment = drhd->segment;
 	dmaru->include_all = drhd->flags & 0x1; /* BIT0: INCLUDE_ALL */
@@ -381,6 +411,11 @@ dmar_parse_one_drhd(struct acpi_dmar_header *header)
 		return ret;
 	}
 	dmar_register_drhd_unit(dmaru);
+
+out:
+	if (arg)
+		(*(int *)arg)++;
+
 	return 0;
 }
 
@@ -393,7 +428,8 @@ static void dmar_free_drhd(struct dmar_drhd_unit *dmaru)
 	kfree(dmaru);
 }
 
-static int __init dmar_parse_one_andd(struct acpi_dmar_header *header)
+static int __init dmar_parse_one_andd(struct acpi_dmar_header *header,
+				      void *arg)
 {
 	struct acpi_dmar_andd *andd = (void *)header;
 
@@ -414,8 +450,7 @@ static int __init dmar_parse_one_andd(struct acpi_dmar_header *header)
 }
 
 #ifdef CONFIG_ACPI_NUMA
-static int __init
-dmar_parse_one_rhsa(struct acpi_dmar_header *header)
+static int dmar_parse_one_rhsa(struct acpi_dmar_header *header, void *arg)
 {
 	struct acpi_dmar_rhsa *rhsa;
 	struct dmar_drhd_unit *drhd;
@@ -442,6 +477,8 @@ dmar_parse_one_rhsa(struct acpi_dmar_header *header)
 
 	return 0;
 }
+#else
+#define	dmar_parse_one_rhsa		dmar_res_noop
 #endif
 
 static void __init
@@ -503,6 +540,52 @@ static int __init dmar_table_detect(void)
 	return (ACPI_SUCCESS(status) ? 1 : 0);
 }
 
+static int dmar_walk_remapping_entries(struct acpi_dmar_header *start,
+				       size_t len, struct dmar_res_callback *cb)
+{
+	int ret = 0;
+	struct acpi_dmar_header *iter, *next;
+	struct acpi_dmar_header *end = ((void *)start) + len;
+
+	for (iter = start; iter < end && ret == 0; iter = next) {
+		next = (void *)iter + iter->length;
+		if (iter->length == 0) {
+			/* Avoid looping forever on bad ACPI tables */
+			pr_debug(FW_BUG "Invalid 0-length structure\n");
+			break;
+		} else if (next > end) {
+			/* Avoid passing table end */
+			pr_warn(FW_BUG "record passes table end\n");
+			ret = -EINVAL;
+			break;
+		}
+
+		if (cb->print_entry)
+			dmar_table_print_dmar_entry(iter);
+
+		if (iter->type >= ACPI_DMAR_TYPE_RESERVED) {
+			/* continue for forward compatibility */
+			pr_debug("Unknown DMAR structure type %d\n",
+				 iter->type);
+		} else if (cb->cb[iter->type]) {
+			ret = cb->cb[iter->type](iter, cb->arg[iter->type]);
+		} else if (!cb->ignore_unhandled) {
+			pr_warn("No handler for DMAR structure type %d\n",
+				iter->type);
+			ret = -EINVAL;
+		}
+	}
+
+	return ret;
+}
+
+static inline int dmar_walk_dmar_table(struct acpi_table_dmar *dmar,
+				       struct dmar_res_callback *cb)
+{
+	return dmar_walk_remapping_entries((void *)(dmar + 1),
+			dmar->header.length - sizeof(*dmar), cb);
+}
+
 /**
  * parse_dmar_table - parses the DMA reporting table
  */
@@ -510,9 +593,18 @@ static int __init
 parse_dmar_table(void)
 {
 	struct acpi_table_dmar *dmar;
-	struct acpi_dmar_header *entry_header;
 	int ret = 0;
 	int drhd_count = 0;
+	struct dmar_res_callback cb = {
+		.print_entry = true,
+		.ignore_unhandled = true,
+		.arg[ACPI_DMAR_TYPE_HARDWARE_UNIT] = &drhd_count,
+		.cb[ACPI_DMAR_TYPE_HARDWARE_UNIT] = &dmar_parse_one_drhd,
+		.cb[ACPI_DMAR_TYPE_RESERVED_MEMORY] = &dmar_parse_one_rmrr,
+		.cb[ACPI_DMAR_TYPE_ROOT_ATS] = &dmar_parse_one_atsr,
+		.cb[ACPI_DMAR_TYPE_HARDWARE_AFFINITY] = &dmar_parse_one_rhsa,
+		.cb[ACPI_DMAR_TYPE_NAMESPACE] = &dmar_parse_one_andd,
+	};
 
 	/*
 	 * Do it again, earlier dmar_tbl mapping could be mapped with
@@ -536,51 +628,10 @@ parse_dmar_table(void)
 	}
 
 	pr_info("Host address width %d\n", dmar->width + 1);
-
-	entry_header = (struct acpi_dmar_header *)(dmar + 1);
-	while (((unsigned long)entry_header) <
-			(((unsigned long)dmar) + dmar_tbl->length)) {
-		/* Avoid looping forever on bad ACPI tables */
-		if (entry_header->length == 0) {
-			pr_warn("Invalid 0-length structure\n");
-			ret = -EINVAL;
-			break;
-		}
-
-		dmar_table_print_dmar_entry(entry_header);
-
-		switch (entry_header->type) {
-		case ACPI_DMAR_TYPE_HARDWARE_UNIT:
-			drhd_count++;
-			ret = dmar_parse_one_drhd(entry_header);
-			break;
-		case ACPI_DMAR_TYPE_RESERVED_MEMORY:
-			ret = dmar_parse_one_rmrr(entry_header);
-			break;
-		case ACPI_DMAR_TYPE_ROOT_ATS:
-			ret = dmar_parse_one_atsr(entry_header);
-			break;
-		case ACPI_DMAR_TYPE_HARDWARE_AFFINITY:
-#ifdef CONFIG_ACPI_NUMA
-			ret = dmar_parse_one_rhsa(entry_header);
-#endif
-			break;
-		case ACPI_DMAR_TYPE_NAMESPACE:
-			ret = dmar_parse_one_andd(entry_header);
-			break;
-		default:
-			pr_warn("Unknown DMAR structure type %d\n",
-				entry_header->type);
-			ret = 0; /* for forward compatibility */
-			break;
-		}
-		if (ret)
-			break;
-
-		entry_header = ((void *)entry_header + entry_header->length);
-	}
-	if (drhd_count == 0)
+	ret = dmar_walk_dmar_table(dmar, &cb);
+	if (ret == 0 && drhd_count == 0)
 		pr_warn(FW_BUG "No DRHD structure found in DMAR table\n");
+
 	return ret;
 }
 
@@ -778,76 +829,68 @@ static void warn_invalid_dmar(u64 addr, const char *message)
 		dmi_get_system_info(DMI_PRODUCT_VERSION));
 }
 
-static int __init check_zero_address(void)
+static int __ref
+dmar_validate_one_drhd(struct acpi_dmar_header *entry, void *arg)
 {
-	struct acpi_table_dmar *dmar;
-	struct acpi_dmar_header *entry_header;
 	struct acpi_dmar_hardware_unit *drhd;
+	void __iomem *addr;
+	u64 cap, ecap;
 
-	dmar = (struct acpi_table_dmar *)dmar_tbl;
-	entry_header = (struct acpi_dmar_header *)(dmar + 1);
-
-	while (((unsigned long)entry_header) <
-			(((unsigned long)dmar) + dmar_tbl->length)) {
-		/* Avoid looping forever on bad ACPI tables */
-		if (entry_header->length == 0) {
-			pr_warn("Invalid 0-length structure\n");
-			return 0;
-		}
+	drhd = (void *)entry;
+	if (!drhd->address) {
+		warn_invalid_dmar(0, "");
+		return -EINVAL;
+	}
 
-		if (entry_header->type == ACPI_DMAR_TYPE_HARDWARE_UNIT) {
-			void __iomem *addr;
-			u64 cap, ecap;
+	if (arg)
+		addr = ioremap(drhd->address, VTD_PAGE_SIZE);
+	else
+		addr = early_ioremap(drhd->address, VTD_PAGE_SIZE);
+	if (!addr) {
+		pr_warn("IOMMU: can't validate: %llx\n", drhd->address);
+		return -EINVAL;
+	}
 
-			drhd = (void *)entry_header;
-			if (!drhd->address) {
-				warn_invalid_dmar(0, "");
-				goto failed;
-			}
+	cap = dmar_readq(addr + DMAR_CAP_REG);
+	ecap = dmar_readq(addr + DMAR_ECAP_REG);
 
-			addr = early_ioremap(drhd->address, VTD_PAGE_SIZE);
-			if (!addr ) {
-				printk("IOMMU: can't validate: %llx\n", drhd->address);
-				goto failed;
-			}
-			cap = dmar_readq(addr + DMAR_CAP_REG);
-			ecap = dmar_readq(addr + DMAR_ECAP_REG);
-			early_iounmap(addr, VTD_PAGE_SIZE);
-			if (cap == (uint64_t)-1 && ecap == (uint64_t)-1) {
-				warn_invalid_dmar(drhd->address,
-						  " returns all ones");
-				goto failed;
-			}
-		}
+	if (arg)
+		iounmap(addr);
+	else
+		early_iounmap(addr, VTD_PAGE_SIZE);
 
-		entry_header = ((void *)entry_header + entry_header->length);
+	if (cap == (uint64_t)-1 && ecap == (uint64_t)-1) {
+		warn_invalid_dmar(drhd->address, " returns all ones");
+		return -EINVAL;
 	}
-	return 1;
 
-failed:
 	return 0;
 }
 
 int __init detect_intel_iommu(void)
 {
 	int ret;
+	struct dmar_res_callback validate_drhd_cb = {
+		.cb[ACPI_DMAR_TYPE_HARDWARE_UNIT] = &dmar_validate_one_drhd,
+		.ignore_unhandled = true,
+	};
 
 	down_write(&dmar_global_lock);
 	ret = dmar_table_detect();
 	if (ret)
-		ret = check_zero_address();
-	{
-		if (ret && !no_iommu && !iommu_detected && !dmar_disabled) {
-			iommu_detected = 1;
-			/* Make sure ACS will be enabled */
-			pci_request_acs();
-		}
+		ret = !dmar_walk_dmar_table((struct acpi_table_dmar *)dmar_tbl,
+					    &validate_drhd_cb);
+	if (ret && !no_iommu && !iommu_detected && !dmar_disabled) {
+		iommu_detected = 1;
+		/* Make sure ACS will be enabled */
+		pci_request_acs();
+	}
 
 #ifdef CONFIG_X86
-		if (ret)
-			x86_init.iommu.iommu_init = intel_iommu_init;
+	if (ret)
+		x86_init.iommu.iommu_init = intel_iommu_init;
 #endif
-	}
+
 	early_acpi_os_unmap_memory((void __iomem *)dmar_tbl, dmar_tbl_size);
 	dmar_tbl = NULL;
 	up_write(&dmar_global_lock);
@@ -931,11 +974,32 @@ out:
 	return err;
 }
 
+static int dmar_alloc_seq_id(struct intel_iommu *iommu)
+{
+	iommu->seq_id = find_first_zero_bit(dmar_seq_ids,
+					    DMAR_UNITS_SUPPORTED);
+	if (iommu->seq_id >= DMAR_UNITS_SUPPORTED) {
+		iommu->seq_id = -1;
+	} else {
+		set_bit(iommu->seq_id, dmar_seq_ids);
+		sprintf(iommu->name, "dmar%d", iommu->seq_id);
+	}
+
+	return iommu->seq_id;
+}
+
+static void dmar_free_seq_id(struct intel_iommu *iommu)
+{
+	if (iommu->seq_id >= 0) {
+		clear_bit(iommu->seq_id, dmar_seq_ids);
+		iommu->seq_id = -1;
+	}
+}
+
 static int alloc_iommu(struct dmar_drhd_unit *drhd)
 {
 	struct intel_iommu *iommu;
 	u32 ver, sts;
-	static int iommu_allocated = 0;
 	int agaw = 0;
 	int msagaw = 0;
 	int err;
@@ -949,13 +1013,16 @@ static int alloc_iommu(struct dmar_drhd_unit *drhd)
 	if (!iommu)
 		return -ENOMEM;
 
-	iommu->seq_id = iommu_allocated++;
-	sprintf (iommu->name, "dmar%d", iommu->seq_id);
+	if (dmar_alloc_seq_id(iommu) < 0) {
+		pr_err("IOMMU: failed to allocate seq_id\n");
+		err = -ENOSPC;
+		goto error;
+	}
 
 	err = map_iommu(iommu, drhd->reg_base_addr);
 	if (err) {
 		pr_err("IOMMU: failed to map %s\n", iommu->name);
-		goto error;
+		goto error_free_seq_id;
 	}
 
 	err = -EINVAL;
@@ -1005,9 +1072,11 @@ static int alloc_iommu(struct dmar_drhd_unit *drhd)
 
 	return 0;
 
- err_unmap:
+err_unmap:
 	unmap_iommu(iommu);
- error:
+error_free_seq_id:
+	dmar_free_seq_id(iommu);
+error:
 	kfree(iommu);
 	return err;
 }
@@ -1031,6 +1100,7 @@ static void free_iommu(struct intel_iommu *iommu)
 	if (iommu->reg)
 		unmap_iommu(iommu);
 
+	dmar_free_seq_id(iommu);
 	kfree(iommu);
 }
 
@@ -1661,12 +1731,17 @@ int __init dmar_ir_support(void)
 	return dmar->flags & 0x1;
 }
 
+/* Check whether DMAR units are in use */
+static inline bool dmar_in_use(void)
+{
+	return irq_remapping_enabled || intel_iommu_enabled;
+}
+
 static int __init dmar_free_unused_resources(void)
 {
 	struct dmar_drhd_unit *dmaru, *dmaru_n;
 
-	/* DMAR units are in use */
-	if (irq_remapping_enabled || intel_iommu_enabled)
+	if (dmar_in_use())
 		return 0;
 
 	if (dmar_dev_scope_status != 1 && !list_empty(&dmar_drhd_units))
@@ -1684,3 +1759,242 @@ static int __init dmar_free_unused_resources(void)
 
 late_initcall(dmar_free_unused_resources);
 IOMMU_INIT_POST(detect_intel_iommu);
+
+/*
+ * DMAR Hotplug Support
+ * For more details, please refer to Intel(R) Virtualization Technology
+ * for Directed-IO Architecture Specifiction, Rev 2.2, Section 8.8
+ * "Remapping Hardware Unit Hot Plug".
+ */
+static u8 dmar_hp_uuid[] = {
+	/* 0000 */    0xA6, 0xA3, 0xC1, 0xD8, 0x9B, 0xBE, 0x9B, 0x4C,
+	/* 0008 */    0x91, 0xBF, 0xC3, 0xCB, 0x81, 0xFC, 0x5D, 0xAF
+};
+
+/*
+ * Currently there's only one revision and BIOS will not check the revision id,
+ * so use 0 for safety.
+ */
+#define	DMAR_DSM_REV_ID			0
+#define	DMAR_DSM_FUNC_DRHD		1
+#define	DMAR_DSM_FUNC_ATSR		2
+#define	DMAR_DSM_FUNC_RHSA		3
+
+static inline bool dmar_detect_dsm(acpi_handle handle, int func)
+{
+	return acpi_check_dsm(handle, dmar_hp_uuid, DMAR_DSM_REV_ID, 1 << func);
+}
+
+static int dmar_walk_dsm_resource(acpi_handle handle, int func,
+				  dmar_res_handler_t handler, void *arg)
+{
+	int ret = -ENODEV;
+	union acpi_object *obj;
+	struct acpi_dmar_header *start;
+	struct dmar_res_callback callback;
+	static int res_type[] = {
+		[DMAR_DSM_FUNC_DRHD] = ACPI_DMAR_TYPE_HARDWARE_UNIT,
+		[DMAR_DSM_FUNC_ATSR] = ACPI_DMAR_TYPE_ROOT_ATS,
+		[DMAR_DSM_FUNC_RHSA] = ACPI_DMAR_TYPE_HARDWARE_AFFINITY,
+	};
+
+	if (!dmar_detect_dsm(handle, func))
+		return 0;
+
+	obj = acpi_evaluate_dsm_typed(handle, dmar_hp_uuid, DMAR_DSM_REV_ID,
+				      func, NULL, ACPI_TYPE_BUFFER);
+	if (!obj)
+		return -ENODEV;
+
+	memset(&callback, 0, sizeof(callback));
+	callback.cb[res_type[func]] = handler;
+	callback.arg[res_type[func]] = arg;
+	start = (struct acpi_dmar_header *)obj->buffer.pointer;
+	ret = dmar_walk_remapping_entries(start, obj->buffer.length, &callback);
+
+	ACPI_FREE(obj);
+
+	return ret;
+}
+
+static int dmar_hp_add_drhd(struct acpi_dmar_header *header, void *arg)
+{
+	int ret;
+	struct dmar_drhd_unit *dmaru;
+
+	dmaru = dmar_find_dmaru((struct acpi_dmar_hardware_unit *)header);
+	if (!dmaru)
+		return -ENODEV;
+
+	ret = dmar_ir_hotplug(dmaru, true);
+	if (ret == 0)
+		ret = dmar_iommu_hotplug(dmaru, true);
+
+	return ret;
+}
+
+static int dmar_hp_remove_drhd(struct acpi_dmar_header *header, void *arg)
+{
+	int i, ret;
+	struct device *dev;
+	struct dmar_drhd_unit *dmaru;
+
+	dmaru = dmar_find_dmaru((struct acpi_dmar_hardware_unit *)header);
+	if (!dmaru)
+		return 0;
+
+	/*
+	 * All PCI devices managed by this unit should have been destroyed.
+	 */
+	if (!dmaru->include_all && dmaru->devices && dmaru->devices_cnt)
+		for_each_active_dev_scope(dmaru->devices,
+					  dmaru->devices_cnt, i, dev)
+			return -EBUSY;
+
+	ret = dmar_ir_hotplug(dmaru, false);
+	if (ret == 0)
+		ret = dmar_iommu_hotplug(dmaru, false);
+
+	return ret;
+}
+
+static int dmar_hp_release_drhd(struct acpi_dmar_header *header, void *arg)
+{
+	struct dmar_drhd_unit *dmaru;
+
+	dmaru = dmar_find_dmaru((struct acpi_dmar_hardware_unit *)header);
+	if (dmaru) {
+		list_del_rcu(&dmaru->list);
+		synchronize_rcu();
+		dmar_free_drhd(dmaru);
+	}
+
+	return 0;
+}
+
+static int dmar_hotplug_insert(acpi_handle handle)
+{
+	int ret;
+	int drhd_count = 0;
+
+	ret = dmar_walk_dsm_resource(handle, DMAR_DSM_FUNC_DRHD,
+				     &dmar_validate_one_drhd, (void *)1);
+	if (ret)
+		goto out;
+
+	ret = dmar_walk_dsm_resource(handle, DMAR_DSM_FUNC_DRHD,
+				     &dmar_parse_one_drhd, (void *)&drhd_count);
+	if (ret == 0 && drhd_count == 0) {
+		pr_warn(FW_BUG "No DRHD structures in buffer returned by _DSM method\n");
+		goto out;
+	} else if (ret) {
+		goto release_drhd;
+	}
+
+	ret = dmar_walk_dsm_resource(handle, DMAR_DSM_FUNC_RHSA,
+				     &dmar_parse_one_rhsa, NULL);
+	if (ret)
+		goto release_drhd;
+
+	ret = dmar_walk_dsm_resource(handle, DMAR_DSM_FUNC_ATSR,
+				     &dmar_parse_one_atsr, NULL);
+	if (ret)
+		goto release_atsr;
+
+	ret = dmar_walk_dsm_resource(handle, DMAR_DSM_FUNC_DRHD,
+				     &dmar_hp_add_drhd, NULL);
+	if (!ret)
+		return 0;
+
+	dmar_walk_dsm_resource(handle, DMAR_DSM_FUNC_DRHD,
+			       &dmar_hp_remove_drhd, NULL);
+release_atsr:
+	dmar_walk_dsm_resource(handle, DMAR_DSM_FUNC_ATSR,
+			       &dmar_release_one_atsr, NULL);
+release_drhd:
+	dmar_walk_dsm_resource(handle, DMAR_DSM_FUNC_DRHD,
+			       &dmar_hp_release_drhd, NULL);
+out:
+	return ret;
+}
+
+static int dmar_hotplug_remove(acpi_handle handle)
+{
+	int ret;
+
+	ret = dmar_walk_dsm_resource(handle, DMAR_DSM_FUNC_ATSR,
+				     &dmar_check_one_atsr, NULL);
+	if (ret)
+		return ret;
+
+	ret = dmar_walk_dsm_resource(handle, DMAR_DSM_FUNC_DRHD,
+				     &dmar_hp_remove_drhd, NULL);
+	if (ret == 0) {
+		WARN_ON(dmar_walk_dsm_resource(handle, DMAR_DSM_FUNC_ATSR,
+					       &dmar_release_one_atsr, NULL));
+		WARN_ON(dmar_walk_dsm_resource(handle, DMAR_DSM_FUNC_DRHD,
+					       &dmar_hp_release_drhd, NULL));
+	} else {
+		dmar_walk_dsm_resource(handle, DMAR_DSM_FUNC_DRHD,
+				       &dmar_hp_add_drhd, NULL);
+	}
+
+	return ret;
+}
+
+static acpi_status dmar_get_dsm_handle(acpi_handle handle, u32 lvl,
+				       void *context, void **retval)
+{
+	acpi_handle *phdl = retval;
+
+	if (dmar_detect_dsm(handle, DMAR_DSM_FUNC_DRHD)) {
+		*phdl = handle;
+		return AE_CTRL_TERMINATE;
+	}
+
+	return AE_OK;
+}
+
+static int dmar_device_hotplug(acpi_handle handle, bool insert)
+{
+	int ret;
+	acpi_handle tmp = NULL;
+	acpi_status status;
+
+	if (!dmar_in_use())
+		return 0;
+
+	if (dmar_detect_dsm(handle, DMAR_DSM_FUNC_DRHD)) {
+		tmp = handle;
+	} else {
+		status = acpi_walk_namespace(ACPI_TYPE_DEVICE, handle,
+					     ACPI_UINT32_MAX,
+					     dmar_get_dsm_handle,
+					     NULL, NULL, &tmp);
+		if (ACPI_FAILURE(status)) {
+			pr_warn("Failed to locate _DSM method.\n");
+			return -ENXIO;
+		}
+	}
+	if (tmp == NULL)
+		return 0;
+
+	down_write(&dmar_global_lock);
+	if (insert)
+		ret = dmar_hotplug_insert(tmp);
+	else
+		ret = dmar_hotplug_remove(tmp);
+	up_write(&dmar_global_lock);
+
+	return ret;
+}
+
+int dmar_device_add(acpi_handle handle)
+{
+	return dmar_device_hotplug(handle, true);
+}
+
+int dmar_device_remove(acpi_handle handle)
+{
+	return dmar_device_hotplug(handle, false);
+}
diff --git a/drivers/iommu/exynos-iommu.c b/drivers/iommu/exynos-iommu.c
index 28372b85d8da..7ce52737c7a1 100644
--- a/drivers/iommu/exynos-iommu.c
+++ b/drivers/iommu/exynos-iommu.c
@@ -684,7 +684,6 @@ static const struct of_device_id sysmmu_of_match[] __initconst = {
 static struct platform_driver exynos_sysmmu_driver __refdata = {
 	.probe	= exynos_sysmmu_probe,
 	.driver	= {
-		.owner		= THIS_MODULE,
 		.name		= "exynos-sysmmu",
 		.of_match_table	= sysmmu_of_match,
 	}
diff --git a/drivers/iommu/fsl_pamu.c b/drivers/iommu/fsl_pamu.c
index 2b6ce9387af1..80ac68d884c5 100644
--- a/drivers/iommu/fsl_pamu.c
+++ b/drivers/iommu/fsl_pamu.c
@@ -1227,7 +1227,6 @@ static const struct of_device_id fsl_of_pamu_ids[] = {
 static struct platform_driver fsl_of_pamu_driver = {
 	.driver = {
 		.name = "fsl-of-pamu",
-		.owner = THIS_MODULE,
 	},
 	.probe = fsl_pamu_probe,
 };
diff --git a/drivers/iommu/intel-iommu.c b/drivers/iommu/intel-iommu.c
index 02cd26a17fe0..1232336b960e 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -195,6 +195,7 @@ static inline void set_root_present(struct root_entry *root)
 }
 static inline void set_root_value(struct root_entry *root, unsigned long value)
 {
+	root->val &= ~VTD_PAGE_MASK;
 	root->val |= value & VTD_PAGE_MASK;
 }
 
@@ -247,6 +248,7 @@ static inline void context_set_translation_type(struct context_entry *context,
 static inline void context_set_address_root(struct context_entry *context,
 					    unsigned long value)
 {
+	context->lo &= ~VTD_PAGE_MASK;
 	context->lo |= value & VTD_PAGE_MASK;
 }
 
@@ -328,17 +330,10 @@ static int hw_pass_through = 1;
 /* si_domain contains mulitple devices */
 #define DOMAIN_FLAG_STATIC_IDENTITY	(1 << 1)
 
-/* define the limit of IOMMUs supported in each domain */
-#ifdef	CONFIG_X86
-# define	IOMMU_UNITS_SUPPORTED	MAX_IO_APICS
-#else
-# define	IOMMU_UNITS_SUPPORTED	64
-#endif
-
 struct dmar_domain {
 	int	id;			/* domain id */
 	int	nid;			/* node id */
-	DECLARE_BITMAP(iommu_bmp, IOMMU_UNITS_SUPPORTED);
+	DECLARE_BITMAP(iommu_bmp, DMAR_UNITS_SUPPORTED);
 					/* bitmap of iommus this domain uses*/
 
 	struct list_head devices; 	/* all devices' list */
@@ -1132,8 +1127,11 @@ static int iommu_alloc_root_entry(struct intel_iommu *iommu)
 	unsigned long flags;
 
 	root = (struct root_entry *)alloc_pgtable_page(iommu->node);
-	if (!root)
+	if (!root) {
+		pr_err("IOMMU: allocating root entry for %s failed\n",
+			iommu->name);
 		return -ENOMEM;
+	}
 
 	__iommu_flush_cache(iommu, root, ROOT_SIZE);
 
@@ -1473,7 +1471,7 @@ static int iommu_init_domains(struct intel_iommu *iommu)
 	return 0;
 }
 
-static void free_dmar_iommu(struct intel_iommu *iommu)
+static void disable_dmar_iommu(struct intel_iommu *iommu)
 {
 	struct dmar_domain *domain;
 	int i;
@@ -1497,11 +1495,16 @@ static void free_dmar_iommu(struct intel_iommu *iommu)
 
 	if (iommu->gcmd & DMA_GCMD_TE)
 		iommu_disable_translation(iommu);
+}
 
-	kfree(iommu->domains);
-	kfree(iommu->domain_ids);
-	iommu->domains = NULL;
-	iommu->domain_ids = NULL;
+static void free_dmar_iommu(struct intel_iommu *iommu)
+{
+	if ((iommu->domains) && (iommu->domain_ids)) {
+		kfree(iommu->domains);
+		kfree(iommu->domain_ids);
+		iommu->domains = NULL;
+		iommu->domain_ids = NULL;
+	}
 
 	g_iommus[iommu->seq_id] = NULL;
 
@@ -1983,7 +1986,7 @@ static int __domain_mapping(struct dmar_domain *domain, unsigned long iov_pfn,
 {
 	struct dma_pte *first_pte = NULL, *pte = NULL;
 	phys_addr_t uninitialized_var(pteval);
-	unsigned long sg_res;
+	unsigned long sg_res = 0;
 	unsigned int largepage_lvl = 0;
 	unsigned long lvl_pages = 0;
 
@@ -1994,10 +1997,8 @@ static int __domain_mapping(struct dmar_domain *domain, unsigned long iov_pfn,
 
 	prot &= DMA_PTE_READ | DMA_PTE_WRITE | DMA_PTE_SNP;
 
-	if (sg)
-		sg_res = 0;
-	else {
-		sg_res = nr_pages + 1;
+	if (!sg) {
+		sg_res = nr_pages;
 		pteval = ((phys_addr_t)phys_pfn << VTD_PAGE_SHIFT) | prot;
 	}
 
@@ -2708,6 +2709,41 @@ static int __init iommu_prepare_static_identity_mapping(int hw)
 	return 0;
 }
 
+static void intel_iommu_init_qi(struct intel_iommu *iommu)
+{
+	/*
+	 * Start from the sane iommu hardware state.
+	 * If the queued invalidation is already initialized by us
+	 * (for example, while enabling interrupt-remapping) then
+	 * we got the things already rolling from a sane state.
+	 */
+	if (!iommu->qi) {
+		/*
+		 * Clear any previous faults.
+		 */
+		dmar_fault(-1, iommu);
+		/*
+		 * Disable queued invalidation if supported and already enabled
+		 * before OS handover.
+		 */
+		dmar_disable_qi(iommu);
+	}
+
+	if (dmar_enable_qi(iommu)) {
+		/*
+		 * Queued Invalidate not enabled, use Register Based Invalidate
+		 */
+		iommu->flush.flush_context = __iommu_flush_context;
+		iommu->flush.flush_iotlb = __iommu_flush_iotlb;
+		pr_info("IOMMU: %s using Register based invalidation\n",
+			iommu->name);
+	} else {
+		iommu->flush.flush_context = qi_flush_context;
+		iommu->flush.flush_iotlb = qi_flush_iotlb;
+		pr_info("IOMMU: %s using Queued invalidation\n", iommu->name);
+	}
+}
+
 static int __init init_dmars(void)
 {
 	struct dmar_drhd_unit *drhd;
@@ -2728,14 +2764,18 @@ static int __init init_dmars(void)
 		 * threaded kernel __init code path all other access are read
 		 * only
 		 */
-		if (g_num_of_iommus < IOMMU_UNITS_SUPPORTED) {
+		if (g_num_of_iommus < DMAR_UNITS_SUPPORTED) {
 			g_num_of_iommus++;
 			continue;
 		}
 		printk_once(KERN_ERR "intel-iommu: exceeded %d IOMMUs\n",
-			  IOMMU_UNITS_SUPPORTED);
+			  DMAR_UNITS_SUPPORTED);
 	}
 
+	/* Preallocate enough resources for IOMMU hot-addition */
+	if (g_num_of_iommus < DMAR_UNITS_SUPPORTED)
+		g_num_of_iommus = DMAR_UNITS_SUPPORTED;
+
 	g_iommus = kcalloc(g_num_of_iommus, sizeof(struct intel_iommu *),
 			GFP_KERNEL);
 	if (!g_iommus) {
@@ -2764,58 +2804,14 @@ static int __init init_dmars(void)
 		 * among all IOMMU's. Need to Split it later.
 		 */
 		ret = iommu_alloc_root_entry(iommu);
-		if (ret) {
-			printk(KERN_ERR "IOMMU: allocate root entry failed\n");
+		if (ret)
 			goto free_iommu;
-		}
 		if (!ecap_pass_through(iommu->ecap))
 			hw_pass_through = 0;
 	}
 
-	/*
-	 * Start from the sane iommu hardware state.
-	 */
-	for_each_active_iommu(iommu, drhd) {
-		/*
-		 * If the queued invalidation is already initialized by us
-		 * (for example, while enabling interrupt-remapping) then
-		 * we got the things already rolling from a sane state.
-		 */
-		if (iommu->qi)
-			continue;
-
-		/*
-		 * Clear any previous faults.
-		 */
-		dmar_fault(-1, iommu);
-		/*
-		 * Disable queued invalidation if supported and already enabled
-		 * before OS handover.
-		 */
-		dmar_disable_qi(iommu);
-	}
-
-	for_each_active_iommu(iommu, drhd) {
-		if (dmar_enable_qi(iommu)) {
-			/*
-			 * Queued Invalidate not enabled, use Register Based
-			 * Invalidate
-			 */
-			iommu->flush.flush_context = __iommu_flush_context;
-			iommu->flush.flush_iotlb = __iommu_flush_iotlb;
-			printk(KERN_INFO "IOMMU %d 0x%Lx: using Register based "
-			       "invalidation\n",
-				iommu->seq_id,
-			       (unsigned long long)drhd->reg_base_addr);
-		} else {
-			iommu->flush.flush_context = qi_flush_context;
-			iommu->flush.flush_iotlb = qi_flush_iotlb;
-			printk(KERN_INFO "IOMMU %d 0x%Lx: using Queued "
-			       "invalidation\n",
-				iommu->seq_id,
-			       (unsigned long long)drhd->reg_base_addr);
-		}
-	}
+	for_each_active_iommu(iommu, drhd)
+		intel_iommu_init_qi(iommu);
 
 	if (iommu_pass_through)
 		iommu_identity_mapping |= IDENTMAP_ALL;
@@ -2901,8 +2897,10 @@ static int __init init_dmars(void)
 	return 0;
 
 free_iommu:
-	for_each_active_iommu(iommu, drhd)
+	for_each_active_iommu(iommu, drhd) {
+		disable_dmar_iommu(iommu);
 		free_dmar_iommu(iommu);
+	}
 	kfree(deferred_flush);
 free_g_iommus:
 	kfree(g_iommus);
@@ -3682,7 +3680,7 @@ static inline void init_iommu_pm_ops(void) {}
 #endif	/* CONFIG_PM */
 
 
-int __init dmar_parse_one_rmrr(struct acpi_dmar_header *header)
+int __init dmar_parse_one_rmrr(struct acpi_dmar_header *header, void *arg)
 {
 	struct acpi_dmar_reserved_memory *rmrr;
 	struct dmar_rmrr_unit *rmrru;
@@ -3708,17 +3706,48 @@ int __init dmar_parse_one_rmrr(struct acpi_dmar_header *header)
 	return 0;
 }
 
-int __init dmar_parse_one_atsr(struct acpi_dmar_header *hdr)
+static struct dmar_atsr_unit *dmar_find_atsr(struct acpi_dmar_atsr *atsr)
+{
+	struct dmar_atsr_unit *atsru;
+	struct acpi_dmar_atsr *tmp;
+
+	list_for_each_entry_rcu(atsru, &dmar_atsr_units, list) {
+		tmp = (struct acpi_dmar_atsr *)atsru->hdr;
+		if (atsr->segment != tmp->segment)
+			continue;
+		if (atsr->header.length != tmp->header.length)
+			continue;
+		if (memcmp(atsr, tmp, atsr->header.length) == 0)
+			return atsru;
+	}
+
+	return NULL;
+}
+
+int dmar_parse_one_atsr(struct acpi_dmar_header *hdr, void *arg)
 {
 	struct acpi_dmar_atsr *atsr;
 	struct dmar_atsr_unit *atsru;
 
+	if (system_state != SYSTEM_BOOTING && !intel_iommu_enabled)
+		return 0;
+
 	atsr = container_of(hdr, struct acpi_dmar_atsr, header);
-	atsru = kzalloc(sizeof(*atsru), GFP_KERNEL);
+	atsru = dmar_find_atsr(atsr);
+	if (atsru)
+		return 0;
+
+	atsru = kzalloc(sizeof(*atsru) + hdr->length, GFP_KERNEL);
 	if (!atsru)
 		return -ENOMEM;
 
-	atsru->hdr = hdr;
+	/*
+	 * If memory is allocated from slab by ACPI _DSM method, we need to
+	 * copy the memory content because the memory buffer will be freed
+	 * on return.
+	 */
+	atsru->hdr = (void *)(atsru + 1);
+	memcpy(atsru->hdr, hdr, hdr->length);
 	atsru->include_all = atsr->flags & 0x1;
 	if (!atsru->include_all) {
 		atsru->devices = dmar_alloc_dev_scope((void *)(atsr + 1),
@@ -3741,6 +3770,138 @@ static void intel_iommu_free_atsr(struct dmar_atsr_unit *atsru)
 	kfree(atsru);
 }
 
+int dmar_release_one_atsr(struct acpi_dmar_header *hdr, void *arg)
+{
+	struct acpi_dmar_atsr *atsr;
+	struct dmar_atsr_unit *atsru;
+
+	atsr = container_of(hdr, struct acpi_dmar_atsr, header);
+	atsru = dmar_find_atsr(atsr);
+	if (atsru) {
+		list_del_rcu(&atsru->list);
+		synchronize_rcu();
+		intel_iommu_free_atsr(atsru);
+	}
+
+	return 0;
+}
+
+int dmar_check_one_atsr(struct acpi_dmar_header *hdr, void *arg)
+{
+	int i;
+	struct device *dev;
+	struct acpi_dmar_atsr *atsr;
+	struct dmar_atsr_unit *atsru;
+
+	atsr = container_of(hdr, struct acpi_dmar_atsr, header);
+	atsru = dmar_find_atsr(atsr);
+	if (!atsru)
+		return 0;
+
+	if (!atsru->include_all && atsru->devices && atsru->devices_cnt)
+		for_each_active_dev_scope(atsru->devices, atsru->devices_cnt,
+					  i, dev)
+			return -EBUSY;
+
+	return 0;
+}
+
+static int intel_iommu_add(struct dmar_drhd_unit *dmaru)
+{
+	int sp, ret = 0;
+	struct intel_iommu *iommu = dmaru->iommu;
+
+	if (g_iommus[iommu->seq_id])
+		return 0;
+
+	if (hw_pass_through && !ecap_pass_through(iommu->ecap)) {
+		pr_warn("IOMMU: %s doesn't support hardware pass through.\n",
+			iommu->name);
+		return -ENXIO;
+	}
+	if (!ecap_sc_support(iommu->ecap) &&
+	    domain_update_iommu_snooping(iommu)) {
+		pr_warn("IOMMU: %s doesn't support snooping.\n",
+			iommu->name);
+		return -ENXIO;
+	}
+	sp = domain_update_iommu_superpage(iommu) - 1;
+	if (sp >= 0 && !(cap_super_page_val(iommu->cap) & (1 << sp))) {
+		pr_warn("IOMMU: %s doesn't support large page.\n",
+			iommu->name);
+		return -ENXIO;
+	}
+
+	/*
+	 * Disable translation if already enabled prior to OS handover.
+	 */
+	if (iommu->gcmd & DMA_GCMD_TE)
+		iommu_disable_translation(iommu);
+
+	g_iommus[iommu->seq_id] = iommu;
+	ret = iommu_init_domains(iommu);
+	if (ret == 0)
+		ret = iommu_alloc_root_entry(iommu);
+	if (ret)
+		goto out;
+
+	if (dmaru->ignored) {
+		/*
+		 * we always have to disable PMRs or DMA may fail on this device
+		 */
+		if (force_on)
+			iommu_disable_protect_mem_regions(iommu);
+		return 0;
+	}
+
+	intel_iommu_init_qi(iommu);
+	iommu_flush_write_buffer(iommu);
+	ret = dmar_set_interrupt(iommu);
+	if (ret)
+		goto disable_iommu;
+
+	iommu_set_root_entry(iommu);
+	iommu->flush.flush_context(iommu, 0, 0, 0, DMA_CCMD_GLOBAL_INVL);
+	iommu->flush.flush_iotlb(iommu, 0, 0, 0, DMA_TLB_GLOBAL_FLUSH);
+	iommu_enable_translation(iommu);
+
+	if (si_domain) {
+		ret = iommu_attach_domain(si_domain, iommu);
+		if (ret < 0 || si_domain->id != ret)
+			goto disable_iommu;
+		domain_attach_iommu(si_domain, iommu);
+	}
+
+	iommu_disable_protect_mem_regions(iommu);
+	return 0;
+
+disable_iommu:
+	disable_dmar_iommu(iommu);
+out:
+	free_dmar_iommu(iommu);
+	return ret;
+}
+
+int dmar_iommu_hotplug(struct dmar_drhd_unit *dmaru, bool insert)
+{
+	int ret = 0;
+	struct intel_iommu *iommu = dmaru->iommu;
+
+	if (!intel_iommu_enabled)
+		return 0;
+	if (iommu == NULL)
+		return -EINVAL;
+
+	if (insert) {
+		ret = intel_iommu_add(dmaru);
+	} else {
+		disable_dmar_iommu(iommu);
+		free_dmar_iommu(iommu);
+	}
+
+	return ret;
+}
+
 static void intel_iommu_free_dmars(void)
 {
 	struct dmar_rmrr_unit *rmrru, *rmrr_n;
diff --git a/drivers/iommu/intel_irq_remapping.c b/drivers/iommu/intel_irq_remapping.c
index 7c80661b35c1..a55b207b9425 100644
--- a/drivers/iommu/intel_irq_remapping.c
+++ b/drivers/iommu/intel_irq_remapping.c
@@ -36,7 +36,6 @@ struct hpet_scope {
 
 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;
 
 /*
  * Lock ordering:
@@ -55,7 +54,7 @@ static int __init parse_ioapics_under_ir(void);
 
 static struct irq_2_iommu *irq_2_iommu(unsigned int irq)
 {
-	struct irq_cfg *cfg = irq_get_chip_data(irq);
+	struct irq_cfg *cfg = irq_cfg(irq);
 	return cfg ? &cfg->irq_2_iommu : NULL;
 }
 
@@ -86,7 +85,7 @@ 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);
-	struct irq_cfg *cfg = irq_get_chip_data(irq);
+	struct irq_cfg *cfg = irq_cfg(irq);
 	unsigned int mask = 0;
 	unsigned long flags;
 	int index;
@@ -154,7 +153,7 @@ static int map_irq_to_irte_handle(int irq, u16 *sub_handle)
 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);
-	struct irq_cfg *cfg = irq_get_chip_data(irq);
+	struct irq_cfg *cfg = irq_cfg(irq);
 	unsigned long flags;
 
 	if (!irq_iommu)
@@ -206,7 +205,7 @@ static struct intel_iommu *map_hpet_to_ir(u8 hpet_id)
 	int i;
 
 	for (i = 0; i < MAX_HPET_TBS; i++)
-		if (ir_hpet[i].id == hpet_id)
+		if (ir_hpet[i].id == hpet_id && ir_hpet[i].iommu)
 			return ir_hpet[i].iommu;
 	return NULL;
 }
@@ -216,7 +215,7 @@ static struct intel_iommu *map_ioapic_to_ir(int apic)
 	int i;
 
 	for (i = 0; i < MAX_IO_APICS; i++)
-		if (ir_ioapic[i].id == apic)
+		if (ir_ioapic[i].id == apic && ir_ioapic[i].iommu)
 			return ir_ioapic[i].iommu;
 	return NULL;
 }
@@ -325,7 +324,7 @@ static int set_ioapic_sid(struct irte *irte, int apic)
 
 	down_read(&dmar_global_lock);
 	for (i = 0; i < MAX_IO_APICS; i++) {
-		if (ir_ioapic[i].id == apic) {
+		if (ir_ioapic[i].iommu && ir_ioapic[i].id == apic) {
 			sid = (ir_ioapic[i].bus << 8) | ir_ioapic[i].devfn;
 			break;
 		}
@@ -352,7 +351,7 @@ static int set_hpet_sid(struct irte *irte, u8 id)
 
 	down_read(&dmar_global_lock);
 	for (i = 0; i < MAX_HPET_TBS; i++) {
-		if (ir_hpet[i].id == id) {
+		if (ir_hpet[i].iommu && ir_hpet[i].id == id) {
 			sid = (ir_hpet[i].bus << 8) | ir_hpet[i].devfn;
 			break;
 		}
@@ -473,17 +472,17 @@ static void iommu_set_irq_remapping(struct intel_iommu *iommu, int mode)
 	raw_spin_unlock_irqrestore(&iommu->register_lock, flags);
 }
 
-
-static int intel_setup_irq_remapping(struct intel_iommu *iommu, int mode)
+static int intel_setup_irq_remapping(struct intel_iommu *iommu)
 {
 	struct ir_table *ir_table;
 	struct page *pages;
 	unsigned long *bitmap;
 
-	ir_table = iommu->ir_table = kzalloc(sizeof(struct ir_table),
-					     GFP_ATOMIC);
+	if (iommu->ir_table)
+		return 0;
 
-	if (!iommu->ir_table)
+	ir_table = kzalloc(sizeof(struct ir_table), GFP_ATOMIC);
+	if (!ir_table)
 		return -ENOMEM;
 
 	pages = alloc_pages_node(iommu->node, GFP_ATOMIC | __GFP_ZERO,
@@ -492,24 +491,37 @@ static int intel_setup_irq_remapping(struct intel_iommu *iommu, int mode)
 	if (!pages) {
 		pr_err("IR%d: failed to allocate pages of order %d\n",
 		       iommu->seq_id, INTR_REMAP_PAGE_ORDER);
-		kfree(iommu->ir_table);
-		return -ENOMEM;
+		goto out_free_table;
 	}
 
 	bitmap = kcalloc(BITS_TO_LONGS(INTR_REMAP_TABLE_ENTRIES),
 			 sizeof(long), GFP_ATOMIC);
 	if (bitmap == NULL) {
 		pr_err("IR%d: failed to allocate bitmap\n", iommu->seq_id);
-		__free_pages(pages, INTR_REMAP_PAGE_ORDER);
-		kfree(ir_table);
-		return -ENOMEM;
+		goto out_free_pages;
 	}
 
 	ir_table->base = page_address(pages);
 	ir_table->bitmap = bitmap;
-
-	iommu_set_irq_remapping(iommu, mode);
+	iommu->ir_table = ir_table;
 	return 0;
+
+out_free_pages:
+	__free_pages(pages, INTR_REMAP_PAGE_ORDER);
+out_free_table:
+	kfree(ir_table);
+	return -ENOMEM;
+}
+
+static void intel_teardown_irq_remapping(struct intel_iommu *iommu)
+{
+	if (iommu && iommu->ir_table) {
+		free_pages((unsigned long)iommu->ir_table->base,
+			   INTR_REMAP_PAGE_ORDER);
+		kfree(iommu->ir_table->bitmap);
+		kfree(iommu->ir_table);
+		iommu->ir_table = NULL;
+	}
 }
 
 /*
@@ -666,9 +678,10 @@ static int __init intel_enable_irq_remapping(void)
 		if (!ecap_ir_support(iommu->ecap))
 			continue;
 
-		if (intel_setup_irq_remapping(iommu, eim))
+		if (intel_setup_irq_remapping(iommu))
 			goto error;
 
+		iommu_set_irq_remapping(iommu, eim);
 		setup = 1;
 	}
 
@@ -689,9 +702,11 @@ static int __init intel_enable_irq_remapping(void)
 	return eim ? IRQ_REMAP_X2APIC_MODE : IRQ_REMAP_XAPIC_MODE;
 
 error:
-	/*
-	 * handle error condition gracefully here!
-	 */
+	for_each_iommu(iommu, drhd)
+		if (ecap_ir_support(iommu->ecap)) {
+			iommu_disable_irq_remapping(iommu);
+			intel_teardown_irq_remapping(iommu);
+		}
 
 	if (x2apic_present)
 		pr_warn("Failed to enable irq remapping.  You are vulnerable to irq-injection attacks.\n");
@@ -699,12 +714,13 @@ error:
 	return -1;
 }
 
-static void ir_parse_one_hpet_scope(struct acpi_dmar_device_scope *scope,
-				      struct intel_iommu *iommu)
+static int ir_parse_one_hpet_scope(struct acpi_dmar_device_scope *scope,
+				   struct intel_iommu *iommu,
+				   struct acpi_dmar_hardware_unit *drhd)
 {
 	struct acpi_dmar_pci_path *path;
 	u8 bus;
-	int count;
+	int count, free = -1;
 
 	bus = scope->bus;
 	path = (struct acpi_dmar_pci_path *)(scope + 1);
@@ -720,19 +736,36 @@ static void ir_parse_one_hpet_scope(struct acpi_dmar_device_scope *scope,
 					   PCI_SECONDARY_BUS);
 		path++;
 	}
-	ir_hpet[ir_hpet_num].bus   = bus;
-	ir_hpet[ir_hpet_num].devfn = PCI_DEVFN(path->device, path->function);
-	ir_hpet[ir_hpet_num].iommu = iommu;
-	ir_hpet[ir_hpet_num].id    = scope->enumeration_id;
-	ir_hpet_num++;
+
+	for (count = 0; count < MAX_HPET_TBS; count++) {
+		if (ir_hpet[count].iommu == iommu &&
+		    ir_hpet[count].id == scope->enumeration_id)
+			return 0;
+		else if (ir_hpet[count].iommu == NULL && free == -1)
+			free = count;
+	}
+	if (free == -1) {
+		pr_warn("Exceeded Max HPET blocks\n");
+		return -ENOSPC;
+	}
+
+	ir_hpet[free].iommu = iommu;
+	ir_hpet[free].id    = scope->enumeration_id;
+	ir_hpet[free].bus   = bus;
+	ir_hpet[free].devfn = PCI_DEVFN(path->device, path->function);
+	pr_info("HPET id %d under DRHD base 0x%Lx\n",
+		scope->enumeration_id, drhd->address);
+
+	return 0;
 }
 
-static void ir_parse_one_ioapic_scope(struct acpi_dmar_device_scope *scope,
-				      struct intel_iommu *iommu)
+static int ir_parse_one_ioapic_scope(struct acpi_dmar_device_scope *scope,
+				     struct intel_iommu *iommu,
+				     struct acpi_dmar_hardware_unit *drhd)
 {
 	struct acpi_dmar_pci_path *path;
 	u8 bus;
-	int count;
+	int count, free = -1;
 
 	bus = scope->bus;
 	path = (struct acpi_dmar_pci_path *)(scope + 1);
@@ -749,54 +782,63 @@ static void ir_parse_one_ioapic_scope(struct acpi_dmar_device_scope *scope,
 		path++;
 	}
 
-	ir_ioapic[ir_ioapic_num].bus   = bus;
-	ir_ioapic[ir_ioapic_num].devfn = PCI_DEVFN(path->device, path->function);
-	ir_ioapic[ir_ioapic_num].iommu = iommu;
-	ir_ioapic[ir_ioapic_num].id    = scope->enumeration_id;
-	ir_ioapic_num++;
+	for (count = 0; count < MAX_IO_APICS; count++) {
+		if (ir_ioapic[count].iommu == iommu &&
+		    ir_ioapic[count].id == scope->enumeration_id)
+			return 0;
+		else if (ir_ioapic[count].iommu == NULL && free == -1)
+			free = count;
+	}
+	if (free == -1) {
+		pr_warn("Exceeded Max IO APICS\n");
+		return -ENOSPC;
+	}
+
+	ir_ioapic[free].bus   = bus;
+	ir_ioapic[free].devfn = PCI_DEVFN(path->device, path->function);
+	ir_ioapic[free].iommu = iommu;
+	ir_ioapic[free].id    = scope->enumeration_id;
+	pr_info("IOAPIC id %d under DRHD base  0x%Lx IOMMU %d\n",
+		scope->enumeration_id, drhd->address, iommu->seq_id);
+
+	return 0;
 }
 
 static int ir_parse_ioapic_hpet_scope(struct acpi_dmar_header *header,
 				      struct intel_iommu *iommu)
 {
+	int ret = 0;
 	struct acpi_dmar_hardware_unit *drhd;
 	struct acpi_dmar_device_scope *scope;
 	void *start, *end;
 
 	drhd = (struct acpi_dmar_hardware_unit *)header;
-
 	start = (void *)(drhd + 1);
 	end = ((void *)drhd) + header->length;
 
-	while (start < end) {
+	while (start < end && ret == 0) {
 		scope = start;
-		if (scope->entry_type == ACPI_DMAR_SCOPE_TYPE_IOAPIC) {
-			if (ir_ioapic_num == MAX_IO_APICS) {
-				printk(KERN_WARNING "Exceeded Max IO APICS\n");
-				return -1;
-			}
-
-			printk(KERN_INFO "IOAPIC id %d under DRHD base "
-			       " 0x%Lx IOMMU %d\n", scope->enumeration_id,
-			       drhd->address, iommu->seq_id);
+		if (scope->entry_type == ACPI_DMAR_SCOPE_TYPE_IOAPIC)
+			ret = ir_parse_one_ioapic_scope(scope, iommu, drhd);
+		else if (scope->entry_type == ACPI_DMAR_SCOPE_TYPE_HPET)
+			ret = ir_parse_one_hpet_scope(scope, iommu, drhd);
+		start += scope->length;
+	}
 
-			ir_parse_one_ioapic_scope(scope, iommu);
-		} else if (scope->entry_type == ACPI_DMAR_SCOPE_TYPE_HPET) {
-			if (ir_hpet_num == MAX_HPET_TBS) {
-				printk(KERN_WARNING "Exceeded Max HPET blocks\n");
-				return -1;
-			}
+	return ret;
+}
 
-			printk(KERN_INFO "HPET id %d under DRHD base"
-			       " 0x%Lx\n", scope->enumeration_id,
-			       drhd->address);
+static void ir_remove_ioapic_hpet_scope(struct intel_iommu *iommu)
+{
+	int i;
 
-			ir_parse_one_hpet_scope(scope, iommu);
-		}
-		start += scope->length;
-	}
+	for (i = 0; i < MAX_HPET_TBS; i++)
+		if (ir_hpet[i].iommu == iommu)
+			ir_hpet[i].iommu = NULL;
 
-	return 0;
+	for (i = 0; i < MAX_IO_APICS; i++)
+		if (ir_ioapic[i].iommu == iommu)
+			ir_ioapic[i].iommu = NULL;
 }
 
 /*
@@ -1008,7 +1050,7 @@ static int
 intel_ioapic_set_affinity(struct irq_data *data, const struct cpumask *mask,
 			  bool force)
 {
-	struct irq_cfg *cfg = data->chip_data;
+	struct irq_cfg *cfg = irqd_cfg(data);
 	unsigned int dest, irq = data->irq;
 	struct irte irte;
 	int err;
@@ -1063,7 +1105,7 @@ static void intel_compose_msi_msg(struct pci_dev *pdev,
 	u16 sub_handle = 0;
 	int ir_index;
 
-	cfg = irq_get_chip_data(irq);
+	cfg = irq_cfg(irq);
 
 	ir_index = map_irq_to_irte_handle(irq, &sub_handle);
 	BUG_ON(ir_index == -1);
@@ -1171,3 +1213,86 @@ struct irq_remap_ops intel_irq_remap_ops = {
 	.msi_setup_irq		= intel_msi_setup_irq,
 	.alloc_hpet_msi		= intel_alloc_hpet_msi,
 };
+
+/*
+ * Support of Interrupt Remapping Unit Hotplug
+ */
+static int dmar_ir_add(struct dmar_drhd_unit *dmaru, struct intel_iommu *iommu)
+{
+	int ret;
+	int eim = x2apic_enabled();
+
+	if (eim && !ecap_eim_support(iommu->ecap)) {
+		pr_info("DRHD %Lx: EIM not supported by DRHD, ecap %Lx\n",
+			iommu->reg_phys, iommu->ecap);
+		return -ENODEV;
+	}
+
+	if (ir_parse_ioapic_hpet_scope(dmaru->hdr, iommu)) {
+		pr_warn("DRHD %Lx: failed to parse managed IOAPIC/HPET\n",
+			iommu->reg_phys);
+		return -ENODEV;
+	}
+
+	/* TODO: check all IOAPICs are covered by IOMMU */
+
+	/* Setup Interrupt-remapping now. */
+	ret = intel_setup_irq_remapping(iommu);
+	if (ret) {
+		pr_err("DRHD %Lx: failed to allocate resource\n",
+		       iommu->reg_phys);
+		ir_remove_ioapic_hpet_scope(iommu);
+		return ret;
+	}
+
+	if (!iommu->qi) {
+		/* Clear previous faults. */
+		dmar_fault(-1, iommu);
+		iommu_disable_irq_remapping(iommu);
+		dmar_disable_qi(iommu);
+	}
+
+	/* Enable queued invalidation */
+	ret = dmar_enable_qi(iommu);
+	if (!ret) {
+		iommu_set_irq_remapping(iommu, eim);
+	} else {
+		pr_err("DRHD %Lx: failed to enable queued invalidation, ecap %Lx, ret %d\n",
+		       iommu->reg_phys, iommu->ecap, ret);
+		intel_teardown_irq_remapping(iommu);
+		ir_remove_ioapic_hpet_scope(iommu);
+	}
+
+	return ret;
+}
+
+int dmar_ir_hotplug(struct dmar_drhd_unit *dmaru, bool insert)
+{
+	int ret = 0;
+	struct intel_iommu *iommu = dmaru->iommu;
+
+	if (!irq_remapping_enabled)
+		return 0;
+	if (iommu == NULL)
+		return -EINVAL;
+	if (!ecap_ir_support(iommu->ecap))
+		return 0;
+
+	if (insert) {
+		if (!iommu->ir_table)
+			ret = dmar_ir_add(dmaru, iommu);
+	} else {
+		if (iommu->ir_table) {
+			if (!bitmap_empty(iommu->ir_table->bitmap,
+					  INTR_REMAP_TABLE_ENTRIES)) {
+				ret = -EBUSY;
+			} else {
+				iommu_disable_irq_remapping(iommu);
+				intel_teardown_irq_remapping(iommu);
+				ir_remove_ioapic_hpet_scope(iommu);
+			}
+		}
+	}
+
+	return ret;
+}
diff --git a/drivers/iommu/iommu.c b/drivers/iommu/iommu.c
index 02e4313e937c..f7718d73e984 100644
--- a/drivers/iommu/iommu.c
+++ b/drivers/iommu/iommu.c
@@ -737,7 +737,7 @@ static int add_iommu_group(struct device *dev, void *data)
 	const struct iommu_ops *ops = cb->ops;
 
 	if (!ops->add_device)
-		return -ENODEV;
+		return 0;
 
 	WARN_ON(dev->iommu_group);
 
@@ -1143,14 +1143,24 @@ size_t default_iommu_map_sg(struct iommu_domain *domain, unsigned long iova,
 {
 	struct scatterlist *s;
 	size_t mapped = 0;
-	unsigned int i;
+	unsigned int i, min_pagesz;
 	int ret;
 
-	for_each_sg(sg, s, nents, i) {
-		phys_addr_t phys = page_to_phys(sg_page(s));
+	if (unlikely(domain->ops->pgsize_bitmap == 0UL))
+		return 0;
 
-		/* We are mapping on page boundarys, so offset must be 0 */
-		if (s->offset)
+	min_pagesz = 1 << __ffs(domain->ops->pgsize_bitmap);
+
+	for_each_sg(sg, s, nents, i) {
+		phys_addr_t phys = page_to_phys(sg_page(s)) + s->offset;
+
+		/*
+		 * We are mapping on IOMMU page boundaries, so offset within
+		 * the page must be 0. However, the IOMMU may support pages
+		 * smaller than PAGE_SIZE, so s->offset may still represent
+		 * an offset of that boundary within the CPU page.
+		 */
+		if (!IS_ALIGNED(s->offset, min_pagesz))
 			goto out_err;
 
 		ret = iommu_map(domain, iova + mapped, phys, s->length, prot);
diff --git a/drivers/iommu/ipmmu-vmsa.c b/drivers/iommu/ipmmu-vmsa.c
index e509c58eee92..68dfb0fd5ee9 100644
--- a/drivers/iommu/ipmmu-vmsa.c
+++ b/drivers/iommu/ipmmu-vmsa.c
@@ -1185,7 +1185,7 @@ static int ipmmu_probe(struct platform_device *pdev)
 			       dev_name(&pdev->dev), mmu);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "failed to request IRQ %d\n", irq);
-		return irq;
+		return ret;
 	}
 
 	ipmmu_device_reset(mmu);
@@ -1222,7 +1222,6 @@ static int ipmmu_remove(struct platform_device *pdev)
 
 static struct platform_driver ipmmu_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "ipmmu-vmsa",
 	},
 	.probe = ipmmu_probe,
diff --git a/drivers/iommu/irq_remapping.c b/drivers/iommu/irq_remapping.c
index 2c3f5ad01098..89c4846683be 100644
--- a/drivers/iommu/irq_remapping.c
+++ b/drivers/iommu/irq_remapping.c
@@ -298,7 +298,7 @@ static int set_remapped_irq_affinity(struct irq_data *data,
 
 void free_remapped_irq(int irq)
 {
-	struct irq_cfg *cfg = irq_get_chip_data(irq);
+	struct irq_cfg *cfg = irq_cfg(irq);
 
 	if (!remap_ops || !remap_ops->free_irq)
 		return;
@@ -311,7 +311,7 @@ void compose_remapped_msi_msg(struct pci_dev *pdev,
 			      unsigned int irq, unsigned int dest,
 			      struct msi_msg *msg, u8 hpet_id)
 {
-	struct irq_cfg *cfg = irq_get_chip_data(irq);
+	struct irq_cfg *cfg = irq_cfg(irq);
 
 	if (!irq_remapped(cfg))
 		native_compose_msi_msg(pdev, irq, dest, msg, hpet_id);
@@ -364,7 +364,7 @@ static void ir_ack_apic_edge(struct irq_data *data)
 static void ir_ack_apic_level(struct irq_data *data)
 {
 	ack_APIC_irq();
-	eoi_ioapic_irq(data->irq, data->chip_data);
+	eoi_ioapic_irq(data->irq, irqd_cfg(data));
 }
 
 static void ir_print_prefix(struct irq_data *data, struct seq_file *p)
diff --git a/drivers/iommu/msm_iommu.c b/drivers/iommu/msm_iommu.c
index 1c7b78ecf3e3..e1b05379ca0e 100644
--- a/drivers/iommu/msm_iommu.c
+++ b/drivers/iommu/msm_iommu.c
@@ -73,8 +73,7 @@ fail:
 
 static void __disable_clocks(struct msm_iommu_drvdata *drvdata)
 {
-	if (drvdata->clk)
-		clk_disable(drvdata->clk);
+	clk_disable(drvdata->clk);
 	clk_disable(drvdata->pclk);
 }
 
diff --git a/drivers/iommu/msm_iommu_dev.c b/drivers/iommu/msm_iommu_dev.c
index 61def7cb5263..b6d01f97e537 100644
--- a/drivers/iommu/msm_iommu_dev.c
+++ b/drivers/iommu/msm_iommu_dev.c
@@ -131,7 +131,7 @@ static int msm_iommu_probe(struct platform_device *pdev)
 	struct clk *iommu_clk;
 	struct clk *iommu_pclk;
 	struct msm_iommu_drvdata *drvdata;
-	struct msm_iommu_dev *iommu_dev = pdev->dev.platform_data;
+	struct msm_iommu_dev *iommu_dev = dev_get_platdata(&pdev->dev);
 	void __iomem *regs_base;
 	int ret, irq, par;
 
@@ -224,8 +224,7 @@ static int msm_iommu_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, drvdata);
 
-	if (iommu_clk)
-		clk_disable(iommu_clk);
+	clk_disable(iommu_clk);
 
 	clk_disable(iommu_pclk);
 
@@ -264,7 +263,7 @@ static int msm_iommu_remove(struct platform_device *pdev)
 
 static int msm_iommu_ctx_probe(struct platform_device *pdev)
 {
-	struct msm_iommu_ctx_dev *c = pdev->dev.platform_data;
+	struct msm_iommu_ctx_dev *c = dev_get_platdata(&pdev->dev);
 	struct msm_iommu_drvdata *drvdata;
 	struct msm_iommu_ctx_drvdata *ctx_drvdata;
 	int i, ret;
@@ -323,8 +322,7 @@ static int msm_iommu_ctx_probe(struct platform_device *pdev)
 		SET_NSCFG(drvdata->base, mid, 3);
 	}
 
-	if (drvdata->clk)
-		clk_disable(drvdata->clk);
+	clk_disable(drvdata->clk);
 	clk_disable(drvdata->pclk);
 
 	dev_info(&pdev->dev, "context %s using bank %d\n", c->name, c->num);
diff --git a/drivers/iommu/of_iommu.c b/drivers/iommu/of_iommu.c
index e550ccb7634e..af1dc6a1c0a1 100644
--- a/drivers/iommu/of_iommu.c
+++ b/drivers/iommu/of_iommu.c
@@ -18,9 +18,14 @@
  */
 
 #include <linux/export.h>
+#include <linux/iommu.h>
 #include <linux/limits.h>
 #include <linux/of.h>
 #include <linux/of_iommu.h>
+#include <linux/slab.h>
+
+static const struct of_device_id __iommu_of_table_sentinel
+	__used __section(__iommu_of_table_end);
 
 /**
  * of_get_dma_window - Parse *dma-window property and returns 0 if found.
@@ -89,3 +94,87 @@ int of_get_dma_window(struct device_node *dn, const char *prefix, int index,
 	return 0;
 }
 EXPORT_SYMBOL_GPL(of_get_dma_window);
+
+struct of_iommu_node {
+	struct list_head list;
+	struct device_node *np;
+	struct iommu_ops *ops;
+};
+static LIST_HEAD(of_iommu_list);
+static DEFINE_SPINLOCK(of_iommu_lock);
+
+void of_iommu_set_ops(struct device_node *np, struct iommu_ops *ops)
+{
+	struct of_iommu_node *iommu = kzalloc(sizeof(*iommu), GFP_KERNEL);
+
+	if (WARN_ON(!iommu))
+		return;
+
+	INIT_LIST_HEAD(&iommu->list);
+	iommu->np = np;
+	iommu->ops = ops;
+	spin_lock(&of_iommu_lock);
+	list_add_tail(&iommu->list, &of_iommu_list);
+	spin_unlock(&of_iommu_lock);
+}
+
+struct iommu_ops *of_iommu_get_ops(struct device_node *np)
+{
+	struct of_iommu_node *node;
+	struct iommu_ops *ops = NULL;
+
+	spin_lock(&of_iommu_lock);
+	list_for_each_entry(node, &of_iommu_list, list)
+		if (node->np == np) {
+			ops = node->ops;
+			break;
+		}
+	spin_unlock(&of_iommu_lock);
+	return ops;
+}
+
+struct iommu_ops *of_iommu_configure(struct device *dev)
+{
+	struct of_phandle_args iommu_spec;
+	struct device_node *np;
+	struct iommu_ops *ops = NULL;
+	int idx = 0;
+
+	/*
+	 * We don't currently walk up the tree looking for a parent IOMMU.
+	 * See the `Notes:' section of
+	 * Documentation/devicetree/bindings/iommu/iommu.txt
+	 */
+	while (!of_parse_phandle_with_args(dev->of_node, "iommus",
+					   "#iommu-cells", idx,
+					   &iommu_spec)) {
+		np = iommu_spec.np;
+		ops = of_iommu_get_ops(np);
+
+		if (!ops || !ops->of_xlate || ops->of_xlate(dev, &iommu_spec))
+			goto err_put_node;
+
+		of_node_put(np);
+		idx++;
+	}
+
+	return ops;
+
+err_put_node:
+	of_node_put(np);
+	return NULL;
+}
+
+void __init of_iommu_init(void)
+{
+	struct device_node *np;
+	const struct of_device_id *match, *matches = &__iommu_of_table;
+
+	for_each_matching_node_and_match(np, matches, &match) {
+		const of_iommu_init_fn init_fn = match->data;
+
+		if (init_fn(np))
+			pr_err("Failed to initialise IOMMU %s\n",
+				of_node_full_name(np));
+	}
+}
diff --git a/drivers/iommu/omap-iommu-debug.c b/drivers/iommu/omap-iommu-debug.c
index 531658d17333..f3d20a2039d2 100644
--- a/drivers/iommu/omap-iommu-debug.c
+++ b/drivers/iommu/omap-iommu-debug.c
@@ -10,45 +10,35 @@
  * published by the Free Software Foundation.
  */
 
-#include <linux/module.h>
 #include <linux/err.h>
-#include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/uaccess.h>
-#include <linux/platform_device.h>
 #include <linux/debugfs.h>
-#include <linux/omap-iommu.h>
 #include <linux/platform_data/iommu-omap.h>
 
 #include "omap-iopgtable.h"
 #include "omap-iommu.h"
 
-#define MAXCOLUMN 100 /* for short messages */
-
 static DEFINE_MUTEX(iommu_debug_lock);
 
 static struct dentry *iommu_debug_root;
 
-static ssize_t debug_read_ver(struct file *file, char __user *userbuf,
-			      size_t count, loff_t *ppos)
+static inline bool is_omap_iommu_detached(struct omap_iommu *obj)
 {
-	u32 ver = omap_iommu_arch_version();
-	char buf[MAXCOLUMN], *p = buf;
-
-	p += sprintf(p, "H/W version: %d.%d\n", (ver >> 4) & 0xf , ver & 0xf);
-
-	return simple_read_from_buffer(userbuf, count, ppos, buf, p - buf);
+	return !obj->domain;
 }
 
 static ssize_t debug_read_regs(struct file *file, char __user *userbuf,
 			       size_t count, loff_t *ppos)
 {
-	struct device *dev = file->private_data;
-	struct omap_iommu *obj = dev_to_omap_iommu(dev);
+	struct omap_iommu *obj = file->private_data;
 	char *p, *buf;
 	ssize_t bytes;
 
+	if (is_omap_iommu_detached(obj))
+		return -EPERM;
+
 	buf = kmalloc(count, GFP_KERNEL);
 	if (!buf)
 		return -ENOMEM;
@@ -68,11 +58,13 @@ static ssize_t debug_read_regs(struct file *file, char __user *userbuf,
 static ssize_t debug_read_tlb(struct file *file, char __user *userbuf,
 			      size_t count, loff_t *ppos)
 {
-	struct device *dev = file->private_data;
-	struct omap_iommu *obj = dev_to_omap_iommu(dev);
+	struct omap_iommu *obj = file->private_data;
 	char *p, *buf;
 	ssize_t bytes, rest;
 
+	if (is_omap_iommu_detached(obj))
+		return -EPERM;
+
 	buf = kmalloc(count, GFP_KERNEL);
 	if (!buf)
 		return -ENOMEM;
@@ -93,133 +85,69 @@ static ssize_t debug_read_tlb(struct file *file, char __user *userbuf,
 	return bytes;
 }
 
-static ssize_t debug_write_pagetable(struct file *file,
-		     const char __user *userbuf, size_t count, loff_t *ppos)
+static void dump_ioptable(struct seq_file *s)
 {
-	struct iotlb_entry e;
-	struct cr_regs cr;
-	int err;
-	struct device *dev = file->private_data;
-	struct omap_iommu *obj = dev_to_omap_iommu(dev);
-	char buf[MAXCOLUMN], *p = buf;
-
-	count = min(count, sizeof(buf));
-
-	mutex_lock(&iommu_debug_lock);
-	if (copy_from_user(p, userbuf, count)) {
-		mutex_unlock(&iommu_debug_lock);
-		return -EFAULT;
-	}
-
-	sscanf(p, "%x %x", &cr.cam, &cr.ram);
-	if (!cr.cam || !cr.ram) {
-		mutex_unlock(&iommu_debug_lock);
-		return -EINVAL;
-	}
-
-	omap_iotlb_cr_to_e(&cr, &e);
-	err = omap_iopgtable_store_entry(obj, &e);
-	if (err)
-		dev_err(obj->dev, "%s: fail to store cr\n", __func__);
-
-	mutex_unlock(&iommu_debug_lock);
-	return count;
-}
-
-#define dump_ioptable_entry_one(lv, da, val)			\
-	({							\
-		int __err = 0;					\
-		ssize_t bytes;					\
-		const int maxcol = 22;				\
-		const char *str = "%d: %08x %08x\n";		\
-		bytes = snprintf(p, maxcol, str, lv, da, val);	\
-		p += bytes;					\
-		len -= bytes;					\
-		if (len < maxcol)				\
-			__err = -ENOMEM;			\
-		__err;						\
-	})
-
-static ssize_t dump_ioptable(struct omap_iommu *obj, char *buf, ssize_t len)
-{
-	int i;
-	u32 *iopgd;
-	char *p = buf;
+	int i, j;
+	u32 da;
+	u32 *iopgd, *iopte;
+	struct omap_iommu *obj = s->private;
 
 	spin_lock(&obj->page_table_lock);
 
 	iopgd = iopgd_offset(obj, 0);
 	for (i = 0; i < PTRS_PER_IOPGD; i++, iopgd++) {
-		int j, err;
-		u32 *iopte;
-		u32 da;
-
 		if (!*iopgd)
 			continue;
 
 		if (!(*iopgd & IOPGD_TABLE)) {
 			da = i << IOPGD_SHIFT;
-
-			err = dump_ioptable_entry_one(1, da, *iopgd);
-			if (err)
-				goto out;
+			seq_printf(s, "1: 0x%08x 0x%08x\n", da, *iopgd);
 			continue;
 		}
 
 		iopte = iopte_offset(iopgd, 0);
-
 		for (j = 0; j < PTRS_PER_IOPTE; j++, iopte++) {
 			if (!*iopte)
 				continue;
 
 			da = (i << IOPGD_SHIFT) + (j << IOPTE_SHIFT);
-			err = dump_ioptable_entry_one(2, da, *iopgd);
-			if (err)
-				goto out;
+			seq_printf(s, "2: 0x%08x 0x%08x\n", da, *iopte);
 		}
 	}
-out:
-	spin_unlock(&obj->page_table_lock);
 
-	return p - buf;
+	spin_unlock(&obj->page_table_lock);
 }
 
-static ssize_t debug_read_pagetable(struct file *file, char __user *userbuf,
-				    size_t count, loff_t *ppos)
+static int debug_read_pagetable(struct seq_file *s, void *data)
 {
-	struct device *dev = file->private_data;
-	struct omap_iommu *obj = dev_to_omap_iommu(dev);
-	char *p, *buf;
-	size_t bytes;
+	struct omap_iommu *obj = s->private;
 
-	buf = (char *)__get_free_page(GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
-	p = buf;
-
-	p += sprintf(p, "L: %8s %8s\n", "da:", "pa:");
-	p += sprintf(p, "-----------------------------------------\n");
+	if (is_omap_iommu_detached(obj))
+		return -EPERM;
 
 	mutex_lock(&iommu_debug_lock);
 
-	bytes = PAGE_SIZE - (p - buf);
-	p += dump_ioptable(obj, p, bytes);
-
-	bytes = simple_read_from_buffer(userbuf, count, ppos, buf, p - buf);
+	seq_printf(s, "L: %8s %8s\n", "da:", "pte:");
+	seq_puts(s, "--------------------------\n");
+	dump_ioptable(s);
 
 	mutex_unlock(&iommu_debug_lock);
-	free_page((unsigned long)buf);
 
-	return bytes;
+	return 0;
 }
 
-#define DEBUG_FOPS(name)						\
-	static const struct file_operations debug_##name##_fops = {	\
-		.open = simple_open,					\
-		.read = debug_read_##name,				\
-		.write = debug_write_##name,				\
-		.llseek = generic_file_llseek,				\
-	};
+#define DEBUG_SEQ_FOPS_RO(name)						       \
+	static int debug_open_##name(struct inode *inode, struct file *file)   \
+	{								       \
+		return single_open(file, debug_read_##name, inode->i_private); \
+	}								       \
+									       \
+	static const struct file_operations debug_##name##_fops = {	       \
+		.open		= debug_open_##name,			       \
+		.read		= seq_read,				       \
+		.llseek		= seq_lseek,				       \
+		.release	= single_release,			       \
+	}
 
 #define DEBUG_FOPS_RO(name)						\
 	static const struct file_operations debug_##name##_fops = {	\
@@ -228,103 +156,63 @@ static ssize_t debug_read_pagetable(struct file *file, char __user *userbuf,
 		.llseek = generic_file_llseek,				\
 	};
 
-DEBUG_FOPS_RO(ver);
 DEBUG_FOPS_RO(regs);
 DEBUG_FOPS_RO(tlb);
-DEBUG_FOPS(pagetable);
+DEBUG_SEQ_FOPS_RO(pagetable);
 
 #define __DEBUG_ADD_FILE(attr, mode)					\
 	{								\
 		struct dentry *dent;					\
-		dent = debugfs_create_file(#attr, mode, parent,		\
-					   dev, &debug_##attr##_fops);	\
+		dent = debugfs_create_file(#attr, mode, obj->debug_dir,	\
+					   obj, &debug_##attr##_fops);	\
 		if (!dent)						\
-			return -ENOMEM;					\
+			goto err;					\
 	}
 
-#define DEBUG_ADD_FILE(name) __DEBUG_ADD_FILE(name, 0600)
 #define DEBUG_ADD_FILE_RO(name) __DEBUG_ADD_FILE(name, 0400)
 
-static int iommu_debug_register(struct device *dev, void *data)
+void omap_iommu_debugfs_add(struct omap_iommu *obj)
 {
-	struct platform_device *pdev = to_platform_device(dev);
-	struct omap_iommu *obj = platform_get_drvdata(pdev);
-	struct omap_iommu_arch_data *arch_data;
-	struct dentry *d, *parent;
-
-	if (!obj || !obj->dev)
-		return -EINVAL;
-
-	arch_data = kzalloc(sizeof(*arch_data), GFP_KERNEL);
-	if (!arch_data)
-		return -ENOMEM;
-
-	arch_data->iommu_dev = obj;
+	struct dentry *d;
 
-	dev->archdata.iommu = arch_data;
+	if (!iommu_debug_root)
+		return;
 
-	d = debugfs_create_dir(obj->name, iommu_debug_root);
-	if (!d)
-		goto nomem;
-	parent = d;
+	obj->debug_dir = debugfs_create_dir(obj->name, iommu_debug_root);
+	if (!obj->debug_dir)
+		return;
 
-	d = debugfs_create_u8("nr_tlb_entries", 400, parent,
+	d = debugfs_create_u8("nr_tlb_entries", 0400, obj->debug_dir,
 			      (u8 *)&obj->nr_tlb_entries);
 	if (!d)
-		goto nomem;
+		return;
 
-	DEBUG_ADD_FILE_RO(ver);
 	DEBUG_ADD_FILE_RO(regs);
 	DEBUG_ADD_FILE_RO(tlb);
-	DEBUG_ADD_FILE(pagetable);
+	DEBUG_ADD_FILE_RO(pagetable);
 
-	return 0;
+	return;
 
-nomem:
-	kfree(arch_data);
-	return -ENOMEM;
+err:
+	debugfs_remove_recursive(obj->debug_dir);
 }
 
-static int iommu_debug_unregister(struct device *dev, void *data)
+void omap_iommu_debugfs_remove(struct omap_iommu *obj)
 {
-	if (!dev->archdata.iommu)
-		return 0;
-
-	kfree(dev->archdata.iommu);
+	if (!obj->debug_dir)
+		return;
 
-	dev->archdata.iommu = NULL;
-
-	return 0;
+	debugfs_remove_recursive(obj->debug_dir);
 }
 
-static int __init iommu_debug_init(void)
+void __init omap_iommu_debugfs_init(void)
 {
-	struct dentry *d;
-	int err;
-
-	d = debugfs_create_dir("iommu", NULL);
-	if (!d)
-		return -ENOMEM;
-	iommu_debug_root = d;
-
-	err = omap_foreach_iommu_device(d, iommu_debug_register);
-	if (err)
-		goto err_out;
-	return 0;
-
-err_out:
-	debugfs_remove_recursive(iommu_debug_root);
-	return err;
+	iommu_debug_root = debugfs_create_dir("omap_iommu", NULL);
+	if (!iommu_debug_root)
+		pr_err("can't create debugfs dir\n");
 }
-module_init(iommu_debug_init)
 
-static void __exit iommu_debugfs_exit(void)
+void __exit omap_iommu_debugfs_exit(void)
 {
-	debugfs_remove_recursive(iommu_debug_root);
-	omap_foreach_iommu_device(NULL, iommu_debug_unregister);
+	debugfs_remove(iommu_debug_root);
 }
-module_exit(iommu_debugfs_exit)
-
-MODULE_DESCRIPTION("omap iommu: debugfs interface");
-MODULE_AUTHOR("Hiroshi DOYU <Hiroshi.DOYU@nokia.com>");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/iommu/omap-iommu.c b/drivers/iommu/omap-iommu.c
index 18003c044454..bbb7dcef02d3 100644
--- a/drivers/iommu/omap-iommu.c
+++ b/drivers/iommu/omap-iommu.c
@@ -76,53 +76,23 @@ struct iotlb_lock {
 	short vict;
 };
 
-/* accommodate the difference between omap1 and omap2/3 */
-static const struct iommu_functions *arch_iommu;
-
 static struct platform_driver omap_iommu_driver;
 static struct kmem_cache *iopte_cachep;
 
 /**
- * omap_install_iommu_arch - Install archtecure specific iommu functions
- * @ops:	a pointer to architecture specific iommu functions
- *
- * There are several kind of iommu algorithm(tlb, pagetable) among
- * omap series. This interface installs such an iommu algorighm.
- **/
-int omap_install_iommu_arch(const struct iommu_functions *ops)
-{
-	if (arch_iommu)
-		return -EBUSY;
-
-	arch_iommu = ops;
-	return 0;
-}
-EXPORT_SYMBOL_GPL(omap_install_iommu_arch);
-
-/**
- * omap_uninstall_iommu_arch - Uninstall archtecure specific iommu functions
- * @ops:	a pointer to architecture specific iommu functions
- *
- * This interface uninstalls the iommu algorighm installed previously.
- **/
-void omap_uninstall_iommu_arch(const struct iommu_functions *ops)
-{
-	if (arch_iommu != ops)
-		pr_err("%s: not your arch\n", __func__);
-
-	arch_iommu = NULL;
-}
-EXPORT_SYMBOL_GPL(omap_uninstall_iommu_arch);
-
-/**
  * omap_iommu_save_ctx - Save registers for pm off-mode support
  * @dev:	client device
  **/
 void omap_iommu_save_ctx(struct device *dev)
 {
 	struct omap_iommu *obj = dev_to_omap_iommu(dev);
+	u32 *p = obj->ctx;
+	int i;
 
-	arch_iommu->save_ctx(obj);
+	for (i = 0; i < (MMU_REG_SIZE / sizeof(u32)); i++) {
+		p[i] = iommu_read_reg(obj, i * sizeof(u32));
+		dev_dbg(obj->dev, "%s\t[%02d] %08x\n", __func__, i, p[i]);
+	}
 }
 EXPORT_SYMBOL_GPL(omap_iommu_save_ctx);
 
@@ -133,28 +103,74 @@ EXPORT_SYMBOL_GPL(omap_iommu_save_ctx);
 void omap_iommu_restore_ctx(struct device *dev)
 {
 	struct omap_iommu *obj = dev_to_omap_iommu(dev);
+	u32 *p = obj->ctx;
+	int i;
 
-	arch_iommu->restore_ctx(obj);
+	for (i = 0; i < (MMU_REG_SIZE / sizeof(u32)); i++) {
+		iommu_write_reg(obj, p[i], i * sizeof(u32));
+		dev_dbg(obj->dev, "%s\t[%02d] %08x\n", __func__, i, p[i]);
+	}
 }
 EXPORT_SYMBOL_GPL(omap_iommu_restore_ctx);
 
-/**
- * omap_iommu_arch_version - Return running iommu arch version
- **/
-u32 omap_iommu_arch_version(void)
+static void __iommu_set_twl(struct omap_iommu *obj, bool on)
 {
-	return arch_iommu->version;
+	u32 l = iommu_read_reg(obj, MMU_CNTL);
+
+	if (on)
+		iommu_write_reg(obj, MMU_IRQ_TWL_MASK, MMU_IRQENABLE);
+	else
+		iommu_write_reg(obj, MMU_IRQ_TLB_MISS_MASK, MMU_IRQENABLE);
+
+	l &= ~MMU_CNTL_MASK;
+	if (on)
+		l |= (MMU_CNTL_MMU_EN | MMU_CNTL_TWL_EN);
+	else
+		l |= (MMU_CNTL_MMU_EN);
+
+	iommu_write_reg(obj, l, MMU_CNTL);
+}
+
+static int omap2_iommu_enable(struct omap_iommu *obj)
+{
+	u32 l, pa;
+
+	if (!obj->iopgd || !IS_ALIGNED((u32)obj->iopgd,  SZ_16K))
+		return -EINVAL;
+
+	pa = virt_to_phys(obj->iopgd);
+	if (!IS_ALIGNED(pa, SZ_16K))
+		return -EINVAL;
+
+	l = iommu_read_reg(obj, MMU_REVISION);
+	dev_info(obj->dev, "%s: version %d.%d\n", obj->name,
+		 (l >> 4) & 0xf, l & 0xf);
+
+	iommu_write_reg(obj, pa, MMU_TTB);
+
+	if (obj->has_bus_err_back)
+		iommu_write_reg(obj, MMU_GP_REG_BUS_ERR_BACK_EN, MMU_GP_REG);
+
+	__iommu_set_twl(obj, true);
+
+	return 0;
+}
+
+static void omap2_iommu_disable(struct omap_iommu *obj)
+{
+	u32 l = iommu_read_reg(obj, MMU_CNTL);
+
+	l &= ~MMU_CNTL_MASK;
+	iommu_write_reg(obj, l, MMU_CNTL);
+
+	dev_dbg(obj->dev, "%s is shutting down\n", obj->name);
 }
-EXPORT_SYMBOL_GPL(omap_iommu_arch_version);
 
 static int iommu_enable(struct omap_iommu *obj)
 {
 	int err;
 	struct platform_device *pdev = to_platform_device(obj->dev);
-	struct iommu_platform_data *pdata = pdev->dev.platform_data;
-
-	if (!arch_iommu)
-		return -ENODEV;
+	struct iommu_platform_data *pdata = dev_get_platdata(&pdev->dev);
 
 	if (pdata && pdata->deassert_reset) {
 		err = pdata->deassert_reset(pdev, pdata->reset_name);
@@ -166,7 +182,7 @@ static int iommu_enable(struct omap_iommu *obj)
 
 	pm_runtime_get_sync(obj->dev);
 
-	err = arch_iommu->enable(obj);
+	err = omap2_iommu_enable(obj);
 
 	return err;
 }
@@ -174,9 +190,9 @@ static int iommu_enable(struct omap_iommu *obj)
 static void iommu_disable(struct omap_iommu *obj)
 {
 	struct platform_device *pdev = to_platform_device(obj->dev);
-	struct iommu_platform_data *pdata = pdev->dev.platform_data;
+	struct iommu_platform_data *pdata = dev_get_platdata(&pdev->dev);
 
-	arch_iommu->disable(obj);
+	omap2_iommu_disable(obj);
 
 	pm_runtime_put_sync(obj->dev);
 
@@ -187,44 +203,51 @@ static void iommu_disable(struct omap_iommu *obj)
 /*
  *	TLB operations
  */
-void omap_iotlb_cr_to_e(struct cr_regs *cr, struct iotlb_entry *e)
-{
-	BUG_ON(!cr || !e);
-
-	arch_iommu->cr_to_e(cr, e);
-}
-EXPORT_SYMBOL_GPL(omap_iotlb_cr_to_e);
-
 static inline int iotlb_cr_valid(struct cr_regs *cr)
 {
 	if (!cr)
 		return -EINVAL;
 
-	return arch_iommu->cr_valid(cr);
-}
-
-static inline struct cr_regs *iotlb_alloc_cr(struct omap_iommu *obj,
-					     struct iotlb_entry *e)
-{
-	if (!e)
-		return NULL;
-
-	return arch_iommu->alloc_cr(obj, e);
+	return cr->cam & MMU_CAM_V;
 }
 
 static u32 iotlb_cr_to_virt(struct cr_regs *cr)
 {
-	return arch_iommu->cr_to_virt(cr);
+	u32 page_size = cr->cam & MMU_CAM_PGSZ_MASK;
+	u32 mask = get_cam_va_mask(cr->cam & page_size);
+
+	return cr->cam & mask;
 }
 
 static u32 get_iopte_attr(struct iotlb_entry *e)
 {
-	return arch_iommu->get_pte_attr(e);
+	u32 attr;
+
+	attr = e->mixed << 5;
+	attr |= e->endian;
+	attr |= e->elsz >> 3;
+	attr <<= (((e->pgsz == MMU_CAM_PGSZ_4K) ||
+			(e->pgsz == MMU_CAM_PGSZ_64K)) ? 0 : 6);
+	return attr;
 }
 
 static u32 iommu_report_fault(struct omap_iommu *obj, u32 *da)
 {
-	return arch_iommu->fault_isr(obj, da);
+	u32 status, fault_addr;
+
+	status = iommu_read_reg(obj, MMU_IRQSTATUS);
+	status &= MMU_IRQ_MASK;
+	if (!status) {
+		*da = 0;
+		return 0;
+	}
+
+	fault_addr = iommu_read_reg(obj, MMU_FAULT_AD);
+	*da = fault_addr;
+
+	iommu_write_reg(obj, status, MMU_IRQSTATUS);
+
+	return status;
 }
 
 static void iotlb_lock_get(struct omap_iommu *obj, struct iotlb_lock *l)
@@ -250,31 +273,19 @@ static void iotlb_lock_set(struct omap_iommu *obj, struct iotlb_lock *l)
 
 static void iotlb_read_cr(struct omap_iommu *obj, struct cr_regs *cr)
 {
-	arch_iommu->tlb_read_cr(obj, cr);
+	cr->cam = iommu_read_reg(obj, MMU_READ_CAM);
+	cr->ram = iommu_read_reg(obj, MMU_READ_RAM);
 }
 
 static void iotlb_load_cr(struct omap_iommu *obj, struct cr_regs *cr)
 {
-	arch_iommu->tlb_load_cr(obj, cr);
+	iommu_write_reg(obj, cr->cam | MMU_CAM_V, MMU_CAM);
+	iommu_write_reg(obj, cr->ram, MMU_RAM);
 
 	iommu_write_reg(obj, 1, MMU_FLUSH_ENTRY);
 	iommu_write_reg(obj, 1, MMU_LD_TLB);
 }
 
-/**
- * iotlb_dump_cr - Dump an iommu tlb entry into buf
- * @obj:	target iommu
- * @cr:		contents of cam and ram register
- * @buf:	output buffer
- **/
-static inline ssize_t iotlb_dump_cr(struct omap_iommu *obj, struct cr_regs *cr,
-				    char *buf)
-{
-	BUG_ON(!cr || !buf);
-
-	return arch_iommu->dump_cr(obj, cr, buf);
-}
-
 /* only used in iotlb iteration for-loop */
 static struct cr_regs __iotlb_read_cr(struct omap_iommu *obj, int n)
 {
@@ -289,12 +300,36 @@ static struct cr_regs __iotlb_read_cr(struct omap_iommu *obj, int n)
 	return cr;
 }
 
+#ifdef PREFETCH_IOTLB
+static struct cr_regs *iotlb_alloc_cr(struct omap_iommu *obj,
+				      struct iotlb_entry *e)
+{
+	struct cr_regs *cr;
+
+	if (!e)
+		return NULL;
+
+	if (e->da & ~(get_cam_va_mask(e->pgsz))) {
+		dev_err(obj->dev, "%s:\twrong alignment: %08x\n", __func__,
+			e->da);
+		return ERR_PTR(-EINVAL);
+	}
+
+	cr = kmalloc(sizeof(*cr), GFP_KERNEL);
+	if (!cr)
+		return ERR_PTR(-ENOMEM);
+
+	cr->cam = (e->da & MMU_CAM_VATAG_MASK) | e->prsvd | e->pgsz | e->valid;
+	cr->ram = e->pa | e->endian | e->elsz | e->mixed;
+
+	return cr;
+}
+
 /**
  * load_iotlb_entry - Set an iommu tlb entry
  * @obj:	target iommu
  * @e:		an iommu tlb entry info
  **/
-#ifdef PREFETCH_IOTLB
 static int load_iotlb_entry(struct omap_iommu *obj, struct iotlb_entry *e)
 {
 	int err = 0;
@@ -423,7 +458,45 @@ static void flush_iotlb_all(struct omap_iommu *obj)
 	pm_runtime_put_sync(obj->dev);
 }
 
-#if defined(CONFIG_OMAP_IOMMU_DEBUG) || defined(CONFIG_OMAP_IOMMU_DEBUG_MODULE)
+#ifdef CONFIG_OMAP_IOMMU_DEBUG
+
+#define pr_reg(name)							\
+	do {								\
+		ssize_t bytes;						\
+		const char *str = "%20s: %08x\n";			\
+		const int maxcol = 32;					\
+		bytes = snprintf(p, maxcol, str, __stringify(name),	\
+				 iommu_read_reg(obj, MMU_##name));	\
+		p += bytes;						\
+		len -= bytes;						\
+		if (len < maxcol)					\
+			goto out;					\
+	} while (0)
+
+static ssize_t
+omap2_iommu_dump_ctx(struct omap_iommu *obj, char *buf, ssize_t len)
+{
+	char *p = buf;
+
+	pr_reg(REVISION);
+	pr_reg(IRQSTATUS);
+	pr_reg(IRQENABLE);
+	pr_reg(WALKING_ST);
+	pr_reg(CNTL);
+	pr_reg(FAULT_AD);
+	pr_reg(TTB);
+	pr_reg(LOCK);
+	pr_reg(LD_TLB);
+	pr_reg(CAM);
+	pr_reg(RAM);
+	pr_reg(GFLUSH);
+	pr_reg(FLUSH_ENTRY);
+	pr_reg(READ_CAM);
+	pr_reg(READ_RAM);
+	pr_reg(EMU_FAULT_AD);
+out:
+	return p - buf;
+}
 
 ssize_t omap_iommu_dump_ctx(struct omap_iommu *obj, char *buf, ssize_t bytes)
 {
@@ -432,13 +505,12 @@ ssize_t omap_iommu_dump_ctx(struct omap_iommu *obj, char *buf, ssize_t bytes)
 
 	pm_runtime_get_sync(obj->dev);
 
-	bytes = arch_iommu->dump_ctx(obj, buf, bytes);
+	bytes = omap2_iommu_dump_ctx(obj, buf, bytes);
 
 	pm_runtime_put_sync(obj->dev);
 
 	return bytes;
 }
-EXPORT_SYMBOL_GPL(omap_iommu_dump_ctx);
 
 static int
 __dump_tlb_entries(struct omap_iommu *obj, struct cr_regs *crs, int num)
@@ -464,6 +536,24 @@ __dump_tlb_entries(struct omap_iommu *obj, struct cr_regs *crs, int num)
 }
 
 /**
+ * iotlb_dump_cr - Dump an iommu tlb entry into buf
+ * @obj:	target iommu
+ * @cr:		contents of cam and ram register
+ * @buf:	output buffer
+ **/
+static ssize_t iotlb_dump_cr(struct omap_iommu *obj, struct cr_regs *cr,
+			     char *buf)
+{
+	char *p = buf;
+
+	/* FIXME: Need more detail analysis of cam/ram */
+	p += sprintf(p, "%08x %08x %01x\n", cr->cam, cr->ram,
+					(cr->cam & MMU_CAM_P) ? 1 : 0);
+
+	return p - buf;
+}
+
+/**
  * omap_dump_tlb_entries - dump cr arrays to given buffer
  * @obj:	target iommu
  * @buf:	output buffer
@@ -488,16 +578,8 @@ size_t omap_dump_tlb_entries(struct omap_iommu *obj, char *buf, ssize_t bytes)
 
 	return p - buf;
 }
-EXPORT_SYMBOL_GPL(omap_dump_tlb_entries);
-
-int omap_foreach_iommu_device(void *data, int (*fn)(struct device *, void *))
-{
-	return driver_for_each_device(&omap_iommu_driver.driver,
-				      NULL, data, fn);
-}
-EXPORT_SYMBOL_GPL(omap_foreach_iommu_device);
 
-#endif /* CONFIG_OMAP_IOMMU_DEBUG_MODULE */
+#endif /* CONFIG_OMAP_IOMMU_DEBUG */
 
 /*
  *	H/W pagetable operations
@@ -680,7 +762,8 @@ iopgtable_store_entry_core(struct omap_iommu *obj, struct iotlb_entry *e)
  * @obj:	target iommu
  * @e:		an iommu tlb entry info
  **/
-int omap_iopgtable_store_entry(struct omap_iommu *obj, struct iotlb_entry *e)
+static int
+omap_iopgtable_store_entry(struct omap_iommu *obj, struct iotlb_entry *e)
 {
 	int err;
 
@@ -690,7 +773,6 @@ int omap_iopgtable_store_entry(struct omap_iommu *obj, struct iotlb_entry *e)
 		prefetch_iotlb_entry(obj, e);
 	return err;
 }
-EXPORT_SYMBOL_GPL(omap_iopgtable_store_entry);
 
 /**
  * iopgtable_lookup_entry - Lookup an iommu pte entry
@@ -819,8 +901,9 @@ static irqreturn_t iommu_fault_handler(int irq, void *data)
 	u32 *iopgd, *iopte;
 	struct omap_iommu *obj = data;
 	struct iommu_domain *domain = obj->domain;
+	struct omap_iommu_domain *omap_domain = domain->priv;
 
-	if (!obj->refcount)
+	if (!omap_domain->iommu_dev)
 		return IRQ_NONE;
 
 	errs = iommu_report_fault(obj, &da);
@@ -880,13 +963,6 @@ static struct omap_iommu *omap_iommu_attach(const char *name, u32 *iopgd)
 
 	spin_lock(&obj->iommu_lock);
 
-	/* an iommu device can only be attached once */
-	if (++obj->refcount > 1) {
-		dev_err(dev, "%s: already attached!\n", obj->name);
-		err = -EBUSY;
-		goto err_enable;
-	}
-
 	obj->iopgd = iopgd;
 	err = iommu_enable(obj);
 	if (err)
@@ -899,7 +975,6 @@ static struct omap_iommu *omap_iommu_attach(const char *name, u32 *iopgd)
 	return obj;
 
 err_enable:
-	obj->refcount--;
 	spin_unlock(&obj->iommu_lock);
 	return ERR_PTR(err);
 }
@@ -915,9 +990,7 @@ static void omap_iommu_detach(struct omap_iommu *obj)
 
 	spin_lock(&obj->iommu_lock);
 
-	if (--obj->refcount == 0)
-		iommu_disable(obj);
-
+	iommu_disable(obj);
 	obj->iopgd = NULL;
 
 	spin_unlock(&obj->iommu_lock);
@@ -934,7 +1007,7 @@ static int omap_iommu_probe(struct platform_device *pdev)
 	int irq;
 	struct omap_iommu *obj;
 	struct resource *res;
-	struct iommu_platform_data *pdata = pdev->dev.platform_data;
+	struct iommu_platform_data *pdata = dev_get_platdata(&pdev->dev);
 	struct device_node *of = pdev->dev.of_node;
 
 	obj = devm_kzalloc(&pdev->dev, sizeof(*obj) + MMU_REG_SIZE, GFP_KERNEL);
@@ -981,6 +1054,8 @@ static int omap_iommu_probe(struct platform_device *pdev)
 	pm_runtime_irq_safe(obj->dev);
 	pm_runtime_enable(obj->dev);
 
+	omap_iommu_debugfs_add(obj);
+
 	dev_info(&pdev->dev, "%s registered\n", obj->name);
 	return 0;
 }
@@ -990,6 +1065,7 @@ static int omap_iommu_remove(struct platform_device *pdev)
 	struct omap_iommu *obj = platform_get_drvdata(pdev);
 
 	iopgtable_clear_entry_all(obj);
+	omap_iommu_debugfs_remove(obj);
 
 	pm_runtime_disable(obj->dev);
 
@@ -1026,7 +1102,6 @@ static u32 iotlb_init_entry(struct iotlb_entry *e, u32 da, u32 pa, int pgsz)
 	e->da		= da;
 	e->pa		= pa;
 	e->valid	= MMU_CAM_V;
-	/* FIXME: add OMAP1 support */
 	e->pgsz		= pgsz;
 	e->endian	= MMU_RAM_ENDIAN_LITTLE;
 	e->elsz		= MMU_RAM_ELSZ_8;
@@ -1131,6 +1206,7 @@ static void _omap_iommu_detach_dev(struct omap_iommu_domain *omap_domain,
 
 	omap_domain->iommu_dev = arch_data->iommu_dev = NULL;
 	omap_domain->dev = NULL;
+	oiommu->domain = NULL;
 }
 
 static void omap_iommu_detach_dev(struct iommu_domain *domain,
@@ -1309,6 +1385,8 @@ static int __init omap_iommu_init(void)
 
 	bus_set_iommu(&platform_bus_type, &omap_iommu_ops);
 
+	omap_iommu_debugfs_init();
+
 	return platform_driver_register(&omap_iommu_driver);
 }
 /* must be ready before omap3isp is probed */
@@ -1319,6 +1397,8 @@ static void __exit omap_iommu_exit(void)
 	kmem_cache_destroy(iopte_cachep);
 
 	platform_driver_unregister(&omap_iommu_driver);
+
+	omap_iommu_debugfs_exit();
 }
 module_exit(omap_iommu_exit);
 
diff --git a/drivers/iommu/omap-iommu.h b/drivers/iommu/omap-iommu.h
index 4f1b68c08c15..d736630df3c8 100644
--- a/drivers/iommu/omap-iommu.h
+++ b/drivers/iommu/omap-iommu.h
@@ -10,9 +10,8 @@
  * published by the Free Software Foundation.
  */
 
-#if defined(CONFIG_ARCH_OMAP1)
-#error "iommu for this processor not implemented yet"
-#endif
+#ifndef _OMAP_IOMMU_H
+#define _OMAP_IOMMU_H
 
 struct iotlb_entry {
 	u32 da;
@@ -30,10 +29,9 @@ struct omap_iommu {
 	const char	*name;
 	void __iomem	*regbase;
 	struct device	*dev;
-	void		*isr_priv;
 	struct iommu_domain *domain;
+	struct dentry	*debug_dir;
 
-	unsigned int	refcount;
 	spinlock_t	iommu_lock;	/* global for this whole object */
 
 	/*
@@ -67,34 +65,6 @@ struct cr_regs {
 	};
 };
 
-/* architecture specific functions */
-struct iommu_functions {
-	unsigned long	version;
-
-	int (*enable)(struct omap_iommu *obj);
-	void (*disable)(struct omap_iommu *obj);
-	void (*set_twl)(struct omap_iommu *obj, bool on);
-	u32 (*fault_isr)(struct omap_iommu *obj, u32 *ra);
-
-	void (*tlb_read_cr)(struct omap_iommu *obj, struct cr_regs *cr);
-	void (*tlb_load_cr)(struct omap_iommu *obj, struct cr_regs *cr);
-
-	struct cr_regs *(*alloc_cr)(struct omap_iommu *obj,
-							struct iotlb_entry *e);
-	int (*cr_valid)(struct cr_regs *cr);
-	u32 (*cr_to_virt)(struct cr_regs *cr);
-	void (*cr_to_e)(struct cr_regs *cr, struct iotlb_entry *e);
-	ssize_t (*dump_cr)(struct omap_iommu *obj, struct cr_regs *cr,
-							char *buf);
-
-	u32 (*get_pte_attr)(struct iotlb_entry *e);
-
-	void (*save_ctx)(struct omap_iommu *obj);
-	void (*restore_ctx)(struct omap_iommu *obj);
-	ssize_t (*dump_ctx)(struct omap_iommu *obj, char *buf, ssize_t len);
-};
-
-#ifdef CONFIG_IOMMU_API
 /**
  * dev_to_omap_iommu() - retrieves an omap iommu object from a user device
  * @dev: iommu client device
@@ -105,7 +75,6 @@ static inline struct omap_iommu *dev_to_omap_iommu(struct device *dev)
 
 	return arch_data->iommu_dev;
 }
-#endif
 
 /*
  * MMU Register offsets
@@ -133,6 +102,28 @@ static inline struct omap_iommu *dev_to_omap_iommu(struct device *dev)
 /*
  * MMU Register bit definitions
  */
+/* IRQSTATUS & IRQENABLE */
+#define MMU_IRQ_MULTIHITFAULT	(1 << 4)
+#define MMU_IRQ_TABLEWALKFAULT	(1 << 3)
+#define MMU_IRQ_EMUMISS		(1 << 2)
+#define MMU_IRQ_TRANSLATIONFAULT	(1 << 1)
+#define MMU_IRQ_TLBMISS		(1 << 0)
+
+#define __MMU_IRQ_FAULT		\
+	(MMU_IRQ_MULTIHITFAULT | MMU_IRQ_EMUMISS | MMU_IRQ_TRANSLATIONFAULT)
+#define MMU_IRQ_MASK		\
+	(__MMU_IRQ_FAULT | MMU_IRQ_TABLEWALKFAULT | MMU_IRQ_TLBMISS)
+#define MMU_IRQ_TWL_MASK	(__MMU_IRQ_FAULT | MMU_IRQ_TABLEWALKFAULT)
+#define MMU_IRQ_TLB_MISS_MASK	(__MMU_IRQ_FAULT | MMU_IRQ_TLBMISS)
+
+/* MMU_CNTL */
+#define MMU_CNTL_SHIFT		1
+#define MMU_CNTL_MASK		(7 << MMU_CNTL_SHIFT)
+#define MMU_CNTL_EML_TLB	(1 << 3)
+#define MMU_CNTL_TWL_EN		(1 << 2)
+#define MMU_CNTL_MMU_EN		(1 << 1)
+
+/* CAM */
 #define MMU_CAM_VATAG_SHIFT	12
 #define MMU_CAM_VATAG_MASK \
 	((~0UL >> MMU_CAM_VATAG_SHIFT) << MMU_CAM_VATAG_SHIFT)
@@ -144,6 +135,7 @@ static inline struct omap_iommu *dev_to_omap_iommu(struct device *dev)
 #define MMU_CAM_PGSZ_4K		(2 << 0)
 #define MMU_CAM_PGSZ_16M	(3 << 0)
 
+/* RAM */
 #define MMU_RAM_PADDR_SHIFT	12
 #define MMU_RAM_PADDR_MASK \
 	((~0UL >> MMU_RAM_PADDR_SHIFT) << MMU_RAM_PADDR_SHIFT)
@@ -165,6 +157,12 @@ static inline struct omap_iommu *dev_to_omap_iommu(struct device *dev)
 
 #define MMU_GP_REG_BUS_ERR_BACK_EN	0x1
 
+#define get_cam_va_mask(pgsz)				\
+	(((pgsz) == MMU_CAM_PGSZ_16M) ? 0xff000000 :	\
+	 ((pgsz) == MMU_CAM_PGSZ_1M)  ? 0xfff00000 :	\
+	 ((pgsz) == MMU_CAM_PGSZ_64K) ? 0xffff0000 :	\
+	 ((pgsz) == MMU_CAM_PGSZ_4K)  ? 0xfffff000 : 0)
+
 /*
  * utilities for super page(16MB, 1MB, 64KB and 4KB)
  */
@@ -192,27 +190,25 @@ static inline struct omap_iommu *dev_to_omap_iommu(struct device *dev)
 /*
  * global functions
  */
-extern u32 omap_iommu_arch_version(void);
-
-extern void omap_iotlb_cr_to_e(struct cr_regs *cr, struct iotlb_entry *e);
-
-extern int
-omap_iopgtable_store_entry(struct omap_iommu *obj, struct iotlb_entry *e);
-
-extern void omap_iommu_save_ctx(struct device *dev);
-extern void omap_iommu_restore_ctx(struct device *dev);
-
-extern int omap_foreach_iommu_device(void *data,
-				int (*fn)(struct device *, void *));
-
-extern int omap_install_iommu_arch(const struct iommu_functions *ops);
-extern void omap_uninstall_iommu_arch(const struct iommu_functions *ops);
-
+#ifdef CONFIG_OMAP_IOMMU_DEBUG
 extern ssize_t
 omap_iommu_dump_ctx(struct omap_iommu *obj, char *buf, ssize_t len);
 extern size_t
 omap_dump_tlb_entries(struct omap_iommu *obj, char *buf, ssize_t len);
 
+void omap_iommu_debugfs_init(void);
+void omap_iommu_debugfs_exit(void);
+
+void omap_iommu_debugfs_add(struct omap_iommu *obj);
+void omap_iommu_debugfs_remove(struct omap_iommu *obj);
+#else
+static inline void omap_iommu_debugfs_init(void) { }
+static inline void omap_iommu_debugfs_exit(void) { }
+
+static inline void omap_iommu_debugfs_add(struct omap_iommu *obj) { }
+static inline void omap_iommu_debugfs_remove(struct omap_iommu *obj) { }
+#endif
+
 /*
  * register accessors
  */
@@ -225,3 +221,5 @@ static inline void iommu_write_reg(struct omap_iommu *obj, u32 val, size_t offs)
 {
 	__raw_writel(val, obj->regbase + offs);
 }
+
+#endif /* _OMAP_IOMMU_H */
diff --git a/drivers/iommu/omap-iommu2.c b/drivers/iommu/omap-iommu2.c
deleted file mode 100644
index 5e1ea3b0bf16..000000000000
--- a/drivers/iommu/omap-iommu2.c
+++ /dev/null
@@ -1,337 +0,0 @@
-/*
- * omap iommu: omap2/3 architecture specific functions
- *
- * Copyright (C) 2008-2009 Nokia Corporation
- *
- * Written by Hiroshi DOYU <Hiroshi.DOYU@nokia.com>,
- *		Paul Mundt and Toshihiro Kobayashi
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#include <linux/err.h>
-#include <linux/device.h>
-#include <linux/io.h>
-#include <linux/jiffies.h>
-#include <linux/module.h>
-#include <linux/omap-iommu.h>
-#include <linux/slab.h>
-#include <linux/stringify.h>
-#include <linux/platform_data/iommu-omap.h>
-
-#include "omap-iommu.h"
-
-/*
- * omap2 architecture specific register bit definitions
- */
-#define IOMMU_ARCH_VERSION	0x00000011
-
-/* IRQSTATUS & IRQENABLE */
-#define MMU_IRQ_MULTIHITFAULT	(1 << 4)
-#define MMU_IRQ_TABLEWALKFAULT	(1 << 3)
-#define MMU_IRQ_EMUMISS		(1 << 2)
-#define MMU_IRQ_TRANSLATIONFAULT	(1 << 1)
-#define MMU_IRQ_TLBMISS		(1 << 0)
-
-#define __MMU_IRQ_FAULT		\
-	(MMU_IRQ_MULTIHITFAULT | MMU_IRQ_EMUMISS | MMU_IRQ_TRANSLATIONFAULT)
-#define MMU_IRQ_MASK		\
-	(__MMU_IRQ_FAULT | MMU_IRQ_TABLEWALKFAULT | MMU_IRQ_TLBMISS)
-#define MMU_IRQ_TWL_MASK	(__MMU_IRQ_FAULT | MMU_IRQ_TABLEWALKFAULT)
-#define MMU_IRQ_TLB_MISS_MASK	(__MMU_IRQ_FAULT | MMU_IRQ_TLBMISS)
-
-/* MMU_CNTL */
-#define MMU_CNTL_SHIFT		1
-#define MMU_CNTL_MASK		(7 << MMU_CNTL_SHIFT)
-#define MMU_CNTL_EML_TLB	(1 << 3)
-#define MMU_CNTL_TWL_EN		(1 << 2)
-#define MMU_CNTL_MMU_EN		(1 << 1)
-
-#define get_cam_va_mask(pgsz)				\
-	(((pgsz) == MMU_CAM_PGSZ_16M) ? 0xff000000 :	\
-	 ((pgsz) == MMU_CAM_PGSZ_1M)  ? 0xfff00000 :	\
-	 ((pgsz) == MMU_CAM_PGSZ_64K) ? 0xffff0000 :	\
-	 ((pgsz) == MMU_CAM_PGSZ_4K)  ? 0xfffff000 : 0)
-
-/* IOMMU errors */
-#define OMAP_IOMMU_ERR_TLB_MISS		(1 << 0)
-#define OMAP_IOMMU_ERR_TRANS_FAULT	(1 << 1)
-#define OMAP_IOMMU_ERR_EMU_MISS		(1 << 2)
-#define OMAP_IOMMU_ERR_TBLWALK_FAULT	(1 << 3)
-#define OMAP_IOMMU_ERR_MULTIHIT_FAULT	(1 << 4)
-
-static void __iommu_set_twl(struct omap_iommu *obj, bool on)
-{
-	u32 l = iommu_read_reg(obj, MMU_CNTL);
-
-	if (on)
-		iommu_write_reg(obj, MMU_IRQ_TWL_MASK, MMU_IRQENABLE);
-	else
-		iommu_write_reg(obj, MMU_IRQ_TLB_MISS_MASK, MMU_IRQENABLE);
-
-	l &= ~MMU_CNTL_MASK;
-	if (on)
-		l |= (MMU_CNTL_MMU_EN | MMU_CNTL_TWL_EN);
-	else
-		l |= (MMU_CNTL_MMU_EN);
-
-	iommu_write_reg(obj, l, MMU_CNTL);
-}
-
-
-static int omap2_iommu_enable(struct omap_iommu *obj)
-{
-	u32 l, pa;
-
-	if (!obj->iopgd || !IS_ALIGNED((u32)obj->iopgd,  SZ_16K))
-		return -EINVAL;
-
-	pa = virt_to_phys(obj->iopgd);
-	if (!IS_ALIGNED(pa, SZ_16K))
-		return -EINVAL;
-
-	l = iommu_read_reg(obj, MMU_REVISION);
-	dev_info(obj->dev, "%s: version %d.%d\n", obj->name,
-		 (l >> 4) & 0xf, l & 0xf);
-
-	iommu_write_reg(obj, pa, MMU_TTB);
-
-	if (obj->has_bus_err_back)
-		iommu_write_reg(obj, MMU_GP_REG_BUS_ERR_BACK_EN, MMU_GP_REG);
-
-	__iommu_set_twl(obj, true);
-
-	return 0;
-}
-
-static void omap2_iommu_disable(struct omap_iommu *obj)
-{
-	u32 l = iommu_read_reg(obj, MMU_CNTL);
-
-	l &= ~MMU_CNTL_MASK;
-	iommu_write_reg(obj, l, MMU_CNTL);
-
-	dev_dbg(obj->dev, "%s is shutting down\n", obj->name);
-}
-
-static void omap2_iommu_set_twl(struct omap_iommu *obj, bool on)
-{
-	__iommu_set_twl(obj, false);
-}
-
-static u32 omap2_iommu_fault_isr(struct omap_iommu *obj, u32 *ra)
-{
-	u32 stat, da;
-	u32 errs = 0;
-
-	stat = iommu_read_reg(obj, MMU_IRQSTATUS);
-	stat &= MMU_IRQ_MASK;
-	if (!stat) {
-		*ra = 0;
-		return 0;
-	}
-
-	da = iommu_read_reg(obj, MMU_FAULT_AD);
-	*ra = da;
-
-	if (stat & MMU_IRQ_TLBMISS)
-		errs |= OMAP_IOMMU_ERR_TLB_MISS;
-	if (stat & MMU_IRQ_TRANSLATIONFAULT)
-		errs |= OMAP_IOMMU_ERR_TRANS_FAULT;
-	if (stat & MMU_IRQ_EMUMISS)
-		errs |= OMAP_IOMMU_ERR_EMU_MISS;
-	if (stat & MMU_IRQ_TABLEWALKFAULT)
-		errs |= OMAP_IOMMU_ERR_TBLWALK_FAULT;
-	if (stat & MMU_IRQ_MULTIHITFAULT)
-		errs |= OMAP_IOMMU_ERR_MULTIHIT_FAULT;
-	iommu_write_reg(obj, stat, MMU_IRQSTATUS);
-
-	return errs;
-}
-
-static void omap2_tlb_read_cr(struct omap_iommu *obj, struct cr_regs *cr)
-{
-	cr->cam = iommu_read_reg(obj, MMU_READ_CAM);
-	cr->ram = iommu_read_reg(obj, MMU_READ_RAM);
-}
-
-static void omap2_tlb_load_cr(struct omap_iommu *obj, struct cr_regs *cr)
-{
-	iommu_write_reg(obj, cr->cam | MMU_CAM_V, MMU_CAM);
-	iommu_write_reg(obj, cr->ram, MMU_RAM);
-}
-
-static u32 omap2_cr_to_virt(struct cr_regs *cr)
-{
-	u32 page_size = cr->cam & MMU_CAM_PGSZ_MASK;
-	u32 mask = get_cam_va_mask(cr->cam & page_size);
-
-	return cr->cam & mask;
-}
-
-static struct cr_regs *omap2_alloc_cr(struct omap_iommu *obj,
-						struct iotlb_entry *e)
-{
-	struct cr_regs *cr;
-
-	if (e->da & ~(get_cam_va_mask(e->pgsz))) {
-		dev_err(obj->dev, "%s:\twrong alignment: %08x\n", __func__,
-			e->da);
-		return ERR_PTR(-EINVAL);
-	}
-
-	cr = kmalloc(sizeof(*cr), GFP_KERNEL);
-	if (!cr)
-		return ERR_PTR(-ENOMEM);
-
-	cr->cam = (e->da & MMU_CAM_VATAG_MASK) | e->prsvd | e->pgsz | e->valid;
-	cr->ram = e->pa | e->endian | e->elsz | e->mixed;
-
-	return cr;
-}
-
-static inline int omap2_cr_valid(struct cr_regs *cr)
-{
-	return cr->cam & MMU_CAM_V;
-}
-
-static u32 omap2_get_pte_attr(struct iotlb_entry *e)
-{
-	u32 attr;
-
-	attr = e->mixed << 5;
-	attr |= e->endian;
-	attr |= e->elsz >> 3;
-	attr <<= (((e->pgsz == MMU_CAM_PGSZ_4K) ||
-			(e->pgsz == MMU_CAM_PGSZ_64K)) ? 0 : 6);
-	return attr;
-}
-
-static ssize_t
-omap2_dump_cr(struct omap_iommu *obj, struct cr_regs *cr, char *buf)
-{
-	char *p = buf;
-
-	/* FIXME: Need more detail analysis of cam/ram */
-	p += sprintf(p, "%08x %08x %01x\n", cr->cam, cr->ram,
-					(cr->cam & MMU_CAM_P) ? 1 : 0);
-
-	return p - buf;
-}
-
-#define pr_reg(name)							\
-	do {								\
-		ssize_t bytes;						\
-		const char *str = "%20s: %08x\n";			\
-		const int maxcol = 32;					\
-		bytes = snprintf(p, maxcol, str, __stringify(name),	\
-				 iommu_read_reg(obj, MMU_##name));	\
-		p += bytes;						\
-		len -= bytes;						\
-		if (len < maxcol)					\
-			goto out;					\
-	} while (0)
-
-static ssize_t
-omap2_iommu_dump_ctx(struct omap_iommu *obj, char *buf, ssize_t len)
-{
-	char *p = buf;
-
-	pr_reg(REVISION);
-	pr_reg(IRQSTATUS);
-	pr_reg(IRQENABLE);
-	pr_reg(WALKING_ST);
-	pr_reg(CNTL);
-	pr_reg(FAULT_AD);
-	pr_reg(TTB);
-	pr_reg(LOCK);
-	pr_reg(LD_TLB);
-	pr_reg(CAM);
-	pr_reg(RAM);
-	pr_reg(GFLUSH);
-	pr_reg(FLUSH_ENTRY);
-	pr_reg(READ_CAM);
-	pr_reg(READ_RAM);
-	pr_reg(EMU_FAULT_AD);
-out:
-	return p - buf;
-}
-
-static void omap2_iommu_save_ctx(struct omap_iommu *obj)
-{
-	int i;
-	u32 *p = obj->ctx;
-
-	for (i = 0; i < (MMU_REG_SIZE / sizeof(u32)); i++) {
-		p[i] = iommu_read_reg(obj, i * sizeof(u32));
-		dev_dbg(obj->dev, "%s\t[%02d] %08x\n", __func__, i, p[i]);
-	}
-
-	BUG_ON(p[0] != IOMMU_ARCH_VERSION);
-}
-
-static void omap2_iommu_restore_ctx(struct omap_iommu *obj)
-{
-	int i;
-	u32 *p = obj->ctx;
-
-	for (i = 0; i < (MMU_REG_SIZE / sizeof(u32)); i++) {
-		iommu_write_reg(obj, p[i], i * sizeof(u32));
-		dev_dbg(obj->dev, "%s\t[%02d] %08x\n", __func__, i, p[i]);
-	}
-
-	BUG_ON(p[0] != IOMMU_ARCH_VERSION);
-}
-
-static void omap2_cr_to_e(struct cr_regs *cr, struct iotlb_entry *e)
-{
-	e->da		= cr->cam & MMU_CAM_VATAG_MASK;
-	e->pa		= cr->ram & MMU_RAM_PADDR_MASK;
-	e->valid	= cr->cam & MMU_CAM_V;
-	e->pgsz		= cr->cam & MMU_CAM_PGSZ_MASK;
-	e->endian	= cr->ram & MMU_RAM_ENDIAN_MASK;
-	e->elsz		= cr->ram & MMU_RAM_ELSZ_MASK;
-	e->mixed	= cr->ram & MMU_RAM_MIXED;
-}
-
-static const struct iommu_functions omap2_iommu_ops = {
-	.version	= IOMMU_ARCH_VERSION,
-
-	.enable		= omap2_iommu_enable,
-	.disable	= omap2_iommu_disable,
-	.set_twl	= omap2_iommu_set_twl,
-	.fault_isr	= omap2_iommu_fault_isr,
-
-	.tlb_read_cr	= omap2_tlb_read_cr,
-	.tlb_load_cr	= omap2_tlb_load_cr,
-
-	.cr_to_e	= omap2_cr_to_e,
-	.cr_to_virt	= omap2_cr_to_virt,
-	.alloc_cr	= omap2_alloc_cr,
-	.cr_valid	= omap2_cr_valid,
-	.dump_cr	= omap2_dump_cr,
-
-	.get_pte_attr	= omap2_get_pte_attr,
-
-	.save_ctx	= omap2_iommu_save_ctx,
-	.restore_ctx	= omap2_iommu_restore_ctx,
-	.dump_ctx	= omap2_iommu_dump_ctx,
-};
-
-static int __init omap2_iommu_init(void)
-{
-	return omap_install_iommu_arch(&omap2_iommu_ops);
-}
-module_init(omap2_iommu_init);
-
-static void __exit omap2_iommu_exit(void)
-{
-	omap_uninstall_iommu_arch(&omap2_iommu_ops);
-}
-module_exit(omap2_iommu_exit);
-
-MODULE_AUTHOR("Hiroshi DOYU, Paul Mundt and Toshihiro Kobayashi");
-MODULE_DESCRIPTION("omap iommu: omap2/3 architecture specific functions");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/iommu/rockchip-iommu.c b/drivers/iommu/rockchip-iommu.c
new file mode 100644
index 000000000000..b2023af384b9
--- /dev/null
+++ b/drivers/iommu/rockchip-iommu.c
@@ -0,0 +1,1038 @@
+/*
+ * 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 <asm/cacheflush.h>
+#include <asm/pgtable.h>
+#include <linux/compiler.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/iommu.h>
+#include <linux/jiffies.h>
+#include <linux/list.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+/** MMU register offsets */
+#define RK_MMU_DTE_ADDR		0x00	/* Directory table address */
+#define RK_MMU_STATUS		0x04
+#define RK_MMU_COMMAND		0x08
+#define RK_MMU_PAGE_FAULT_ADDR	0x0C	/* IOVA of last page fault */
+#define RK_MMU_ZAP_ONE_LINE	0x10	/* Shootdown one IOTLB entry */
+#define RK_MMU_INT_RAWSTAT	0x14	/* IRQ status ignoring mask */
+#define RK_MMU_INT_CLEAR	0x18	/* Acknowledge and re-arm irq */
+#define RK_MMU_INT_MASK		0x1C	/* IRQ enable */
+#define RK_MMU_INT_STATUS	0x20	/* IRQ status after masking */
+#define RK_MMU_AUTO_GATING	0x24
+
+#define DTE_ADDR_DUMMY		0xCAFEBABE
+#define FORCE_RESET_TIMEOUT	100	/* ms */
+
+/* RK_MMU_STATUS fields */
+#define RK_MMU_STATUS_PAGING_ENABLED       BIT(0)
+#define RK_MMU_STATUS_PAGE_FAULT_ACTIVE    BIT(1)
+#define RK_MMU_STATUS_STALL_ACTIVE         BIT(2)
+#define RK_MMU_STATUS_IDLE                 BIT(3)
+#define RK_MMU_STATUS_REPLAY_BUFFER_EMPTY  BIT(4)
+#define RK_MMU_STATUS_PAGE_FAULT_IS_WRITE  BIT(5)
+#define RK_MMU_STATUS_STALL_NOT_ACTIVE     BIT(31)
+
+/* RK_MMU_COMMAND command values */
+#define RK_MMU_CMD_ENABLE_PAGING    0  /* Enable memory translation */
+#define RK_MMU_CMD_DISABLE_PAGING   1  /* Disable memory translation */
+#define RK_MMU_CMD_ENABLE_STALL     2  /* Stall paging to allow other cmds */
+#define RK_MMU_CMD_DISABLE_STALL    3  /* Stop stall re-enables paging */
+#define RK_MMU_CMD_ZAP_CACHE        4  /* Shoot down entire IOTLB */
+#define RK_MMU_CMD_PAGE_FAULT_DONE  5  /* Clear page fault */
+#define RK_MMU_CMD_FORCE_RESET      6  /* Reset all registers */
+
+/* RK_MMU_INT_* register fields */
+#define RK_MMU_IRQ_PAGE_FAULT    0x01  /* page fault */
+#define RK_MMU_IRQ_BUS_ERROR     0x02  /* bus read error */
+#define RK_MMU_IRQ_MASK          (RK_MMU_IRQ_PAGE_FAULT | RK_MMU_IRQ_BUS_ERROR)
+
+#define NUM_DT_ENTRIES 1024
+#define NUM_PT_ENTRIES 1024
+
+#define SPAGE_ORDER 12
+#define SPAGE_SIZE (1 << SPAGE_ORDER)
+
+ /*
+  * Support mapping any size that fits in one page table:
+  *   4 KiB to 4 MiB
+  */
+#define RK_IOMMU_PGSIZE_BITMAP 0x007ff000
+
+#define IOMMU_REG_POLL_COUNT_FAST 1000
+
+struct rk_iommu_domain {
+	struct list_head iommus;
+	u32 *dt; /* page directory table */
+	spinlock_t iommus_lock; /* lock for iommus list */
+	spinlock_t dt_lock; /* lock for modifying page directory table */
+};
+
+struct rk_iommu {
+	struct device *dev;
+	void __iomem *base;
+	int irq;
+	struct list_head node; /* entry in rk_iommu_domain.iommus */
+	struct iommu_domain *domain; /* domain to which iommu is attached */
+};
+
+static inline void rk_table_flush(u32 *va, unsigned int count)
+{
+	phys_addr_t pa_start = virt_to_phys(va);
+	phys_addr_t pa_end = virt_to_phys(va + count);
+	size_t size = pa_end - pa_start;
+
+	__cpuc_flush_dcache_area(va, size);
+	outer_flush_range(pa_start, pa_end);
+}
+
+/**
+ * Inspired by _wait_for in intel_drv.h
+ * This is NOT safe for use in interrupt context.
+ *
+ * Note that it's important that we check the condition again after having
+ * timed out, since the timeout could be due to preemption or similar and
+ * we've never had a chance to check the condition before the timeout.
+ */
+#define rk_wait_for(COND, MS) ({ \
+	unsigned long timeout__ = jiffies + msecs_to_jiffies(MS) + 1;	\
+	int ret__ = 0;							\
+	while (!(COND)) {						\
+		if (time_after(jiffies, timeout__)) {			\
+			ret__ = (COND) ? 0 : -ETIMEDOUT;		\
+			break;						\
+		}							\
+		usleep_range(50, 100);					\
+	}								\
+	ret__;								\
+})
+
+/*
+ * The Rockchip rk3288 iommu uses a 2-level page table.
+ * The first level is the "Directory Table" (DT).
+ * The DT consists of 1024 4-byte Directory Table Entries (DTEs), each pointing
+ * to a "Page Table".
+ * The second level is the 1024 Page Tables (PT).
+ * Each PT consists of 1024 4-byte Page Table Entries (PTEs), each pointing to
+ * a 4 KB page of physical memory.
+ *
+ * The DT and each PT fits in a single 4 KB page (4-bytes * 1024 entries).
+ * Each iommu device has a MMU_DTE_ADDR register that contains the physical
+ * address of the start of the DT page.
+ *
+ * The structure of the page table is as follows:
+ *
+ *                   DT
+ * MMU_DTE_ADDR -> +-----+
+ *                 |     |
+ *                 +-----+     PT
+ *                 | DTE | -> +-----+
+ *                 +-----+    |     |     Memory
+ *                 |     |    +-----+     Page
+ *                 |     |    | PTE | -> +-----+
+ *                 +-----+    +-----+    |     |
+ *                            |     |    |     |
+ *                            |     |    |     |
+ *                            +-----+    |     |
+ *                                       |     |
+ *                                       |     |
+ *                                       +-----+
+ */
+
+/*
+ * Each DTE has a PT address and a valid bit:
+ * +---------------------+-----------+-+
+ * | PT address          | Reserved  |V|
+ * +---------------------+-----------+-+
+ *  31:12 - PT address (PTs always starts on a 4 KB boundary)
+ *  11: 1 - Reserved
+ *      0 - 1 if PT @ PT address is valid
+ */
+#define RK_DTE_PT_ADDRESS_MASK    0xfffff000
+#define RK_DTE_PT_VALID           BIT(0)
+
+static inline phys_addr_t rk_dte_pt_address(u32 dte)
+{
+	return (phys_addr_t)dte & RK_DTE_PT_ADDRESS_MASK;
+}
+
+static inline bool rk_dte_is_pt_valid(u32 dte)
+{
+	return dte & RK_DTE_PT_VALID;
+}
+
+static u32 rk_mk_dte(u32 *pt)
+{
+	phys_addr_t pt_phys = virt_to_phys(pt);
+	return (pt_phys & RK_DTE_PT_ADDRESS_MASK) | RK_DTE_PT_VALID;
+}
+
+/*
+ * Each PTE has a Page address, some flags and a valid bit:
+ * +---------------------+---+-------+-+
+ * | Page address        |Rsv| Flags |V|
+ * +---------------------+---+-------+-+
+ *  31:12 - Page address (Pages always start on a 4 KB boundary)
+ *  11: 9 - Reserved
+ *   8: 1 - Flags
+ *      8 - Read allocate - allocate cache space on read misses
+ *      7 - Read cache - enable cache & prefetch of data
+ *      6 - Write buffer - enable delaying writes on their way to memory
+ *      5 - Write allocate - allocate cache space on write misses
+ *      4 - Write cache - different writes can be merged together
+ *      3 - Override cache attributes
+ *          if 1, bits 4-8 control cache attributes
+ *          if 0, the system bus defaults are used
+ *      2 - Writable
+ *      1 - Readable
+ *      0 - 1 if Page @ Page address is valid
+ */
+#define RK_PTE_PAGE_ADDRESS_MASK  0xfffff000
+#define RK_PTE_PAGE_FLAGS_MASK    0x000001fe
+#define RK_PTE_PAGE_WRITABLE      BIT(2)
+#define RK_PTE_PAGE_READABLE      BIT(1)
+#define RK_PTE_PAGE_VALID         BIT(0)
+
+static inline phys_addr_t rk_pte_page_address(u32 pte)
+{
+	return (phys_addr_t)pte & RK_PTE_PAGE_ADDRESS_MASK;
+}
+
+static inline bool rk_pte_is_page_valid(u32 pte)
+{
+	return pte & RK_PTE_PAGE_VALID;
+}
+
+/* TODO: set cache flags per prot IOMMU_CACHE */
+static u32 rk_mk_pte(phys_addr_t page, int prot)
+{
+	u32 flags = 0;
+	flags |= (prot & IOMMU_READ) ? RK_PTE_PAGE_READABLE : 0;
+	flags |= (prot & IOMMU_WRITE) ? RK_PTE_PAGE_WRITABLE : 0;
+	page &= RK_PTE_PAGE_ADDRESS_MASK;
+	return page | flags | RK_PTE_PAGE_VALID;
+}
+
+static u32 rk_mk_pte_invalid(u32 pte)
+{
+	return pte & ~RK_PTE_PAGE_VALID;
+}
+
+/*
+ * rk3288 iova (IOMMU Virtual Address) format
+ *  31       22.21       12.11          0
+ * +-----------+-----------+-------------+
+ * | DTE index | PTE index | Page offset |
+ * +-----------+-----------+-------------+
+ *  31:22 - DTE index   - index of DTE in DT
+ *  21:12 - PTE index   - index of PTE in PT @ DTE.pt_address
+ *  11: 0 - Page offset - offset into page @ PTE.page_address
+ */
+#define RK_IOVA_DTE_MASK    0xffc00000
+#define RK_IOVA_DTE_SHIFT   22
+#define RK_IOVA_PTE_MASK    0x003ff000
+#define RK_IOVA_PTE_SHIFT   12
+#define RK_IOVA_PAGE_MASK   0x00000fff
+#define RK_IOVA_PAGE_SHIFT  0
+
+static u32 rk_iova_dte_index(dma_addr_t iova)
+{
+	return (u32)(iova & RK_IOVA_DTE_MASK) >> RK_IOVA_DTE_SHIFT;
+}
+
+static u32 rk_iova_pte_index(dma_addr_t iova)
+{
+	return (u32)(iova & RK_IOVA_PTE_MASK) >> RK_IOVA_PTE_SHIFT;
+}
+
+static u32 rk_iova_page_offset(dma_addr_t iova)
+{
+	return (u32)(iova & RK_IOVA_PAGE_MASK) >> RK_IOVA_PAGE_SHIFT;
+}
+
+static u32 rk_iommu_read(struct rk_iommu *iommu, u32 offset)
+{
+	return readl(iommu->base + offset);
+}
+
+static void rk_iommu_write(struct rk_iommu *iommu, u32 offset, u32 value)
+{
+	writel(value, iommu->base + offset);
+}
+
+static void rk_iommu_command(struct rk_iommu *iommu, u32 command)
+{
+	writel(command, iommu->base + RK_MMU_COMMAND);
+}
+
+static void rk_iommu_zap_lines(struct rk_iommu *iommu, dma_addr_t iova,
+			       size_t size)
+{
+	dma_addr_t iova_end = iova + size;
+	/*
+	 * TODO(djkurtz): Figure out when it is more efficient to shootdown the
+	 * entire iotlb rather than iterate over individual iovas.
+	 */
+	for (; iova < iova_end; iova += SPAGE_SIZE)
+		rk_iommu_write(iommu, RK_MMU_ZAP_ONE_LINE, iova);
+}
+
+static bool rk_iommu_is_stall_active(struct rk_iommu *iommu)
+{
+	return rk_iommu_read(iommu, RK_MMU_STATUS) & RK_MMU_STATUS_STALL_ACTIVE;
+}
+
+static bool rk_iommu_is_paging_enabled(struct rk_iommu *iommu)
+{
+	return rk_iommu_read(iommu, RK_MMU_STATUS) &
+			     RK_MMU_STATUS_PAGING_ENABLED;
+}
+
+static int rk_iommu_enable_stall(struct rk_iommu *iommu)
+{
+	int ret;
+
+	if (rk_iommu_is_stall_active(iommu))
+		return 0;
+
+	/* Stall can only be enabled if paging is enabled */
+	if (!rk_iommu_is_paging_enabled(iommu))
+		return 0;
+
+	rk_iommu_command(iommu, RK_MMU_CMD_ENABLE_STALL);
+
+	ret = rk_wait_for(rk_iommu_is_stall_active(iommu), 1);
+	if (ret)
+		dev_err(iommu->dev, "Enable stall request timed out, status: %#08x\n",
+			rk_iommu_read(iommu, RK_MMU_STATUS));
+
+	return ret;
+}
+
+static int rk_iommu_disable_stall(struct rk_iommu *iommu)
+{
+	int ret;
+
+	if (!rk_iommu_is_stall_active(iommu))
+		return 0;
+
+	rk_iommu_command(iommu, RK_MMU_CMD_DISABLE_STALL);
+
+	ret = rk_wait_for(!rk_iommu_is_stall_active(iommu), 1);
+	if (ret)
+		dev_err(iommu->dev, "Disable stall request timed out, status: %#08x\n",
+			rk_iommu_read(iommu, RK_MMU_STATUS));
+
+	return ret;
+}
+
+static int rk_iommu_enable_paging(struct rk_iommu *iommu)
+{
+	int ret;
+
+	if (rk_iommu_is_paging_enabled(iommu))
+		return 0;
+
+	rk_iommu_command(iommu, RK_MMU_CMD_ENABLE_PAGING);
+
+	ret = rk_wait_for(rk_iommu_is_paging_enabled(iommu), 1);
+	if (ret)
+		dev_err(iommu->dev, "Enable paging request timed out, status: %#08x\n",
+			rk_iommu_read(iommu, RK_MMU_STATUS));
+
+	return ret;
+}
+
+static int rk_iommu_disable_paging(struct rk_iommu *iommu)
+{
+	int ret;
+
+	if (!rk_iommu_is_paging_enabled(iommu))
+		return 0;
+
+	rk_iommu_command(iommu, RK_MMU_CMD_DISABLE_PAGING);
+
+	ret = rk_wait_for(!rk_iommu_is_paging_enabled(iommu), 1);
+	if (ret)
+		dev_err(iommu->dev, "Disable paging request timed out, status: %#08x\n",
+			rk_iommu_read(iommu, RK_MMU_STATUS));
+
+	return ret;
+}
+
+static int rk_iommu_force_reset(struct rk_iommu *iommu)
+{
+	int ret;
+	u32 dte_addr;
+
+	/*
+	 * Check if register DTE_ADDR is working by writing DTE_ADDR_DUMMY
+	 * and verifying that upper 5 nybbles are read back.
+	 */
+	rk_iommu_write(iommu, RK_MMU_DTE_ADDR, DTE_ADDR_DUMMY);
+
+	dte_addr = rk_iommu_read(iommu, RK_MMU_DTE_ADDR);
+	if (dte_addr != (DTE_ADDR_DUMMY & RK_DTE_PT_ADDRESS_MASK)) {
+		dev_err(iommu->dev, "Error during raw reset. MMU_DTE_ADDR is not functioning\n");
+		return -EFAULT;
+	}
+
+	rk_iommu_command(iommu, RK_MMU_CMD_FORCE_RESET);
+
+	ret = rk_wait_for(rk_iommu_read(iommu, RK_MMU_DTE_ADDR) == 0x00000000,
+			  FORCE_RESET_TIMEOUT);
+	if (ret)
+		dev_err(iommu->dev, "FORCE_RESET command timed out\n");
+
+	return ret;
+}
+
+static void log_iova(struct rk_iommu *iommu, dma_addr_t iova)
+{
+	u32 dte_index, pte_index, page_offset;
+	u32 mmu_dte_addr;
+	phys_addr_t mmu_dte_addr_phys, dte_addr_phys;
+	u32 *dte_addr;
+	u32 dte;
+	phys_addr_t pte_addr_phys = 0;
+	u32 *pte_addr = NULL;
+	u32 pte = 0;
+	phys_addr_t page_addr_phys = 0;
+	u32 page_flags = 0;
+
+	dte_index = rk_iova_dte_index(iova);
+	pte_index = rk_iova_pte_index(iova);
+	page_offset = rk_iova_page_offset(iova);
+
+	mmu_dte_addr = rk_iommu_read(iommu, RK_MMU_DTE_ADDR);
+	mmu_dte_addr_phys = (phys_addr_t)mmu_dte_addr;
+
+	dte_addr_phys = mmu_dte_addr_phys + (4 * dte_index);
+	dte_addr = phys_to_virt(dte_addr_phys);
+	dte = *dte_addr;
+
+	if (!rk_dte_is_pt_valid(dte))
+		goto print_it;
+
+	pte_addr_phys = rk_dte_pt_address(dte) + (pte_index * 4);
+	pte_addr = phys_to_virt(pte_addr_phys);
+	pte = *pte_addr;
+
+	if (!rk_pte_is_page_valid(pte))
+		goto print_it;
+
+	page_addr_phys = rk_pte_page_address(pte) + page_offset;
+	page_flags = pte & RK_PTE_PAGE_FLAGS_MASK;
+
+print_it:
+	dev_err(iommu->dev, "iova = %pad: dte_index: %#03x pte_index: %#03x page_offset: %#03x\n",
+		&iova, dte_index, pte_index, page_offset);
+	dev_err(iommu->dev, "mmu_dte_addr: %pa dte@%pa: %#08x valid: %u pte@%pa: %#08x valid: %u page@%pa flags: %#03x\n",
+		&mmu_dte_addr_phys, &dte_addr_phys, dte,
+		rk_dte_is_pt_valid(dte), &pte_addr_phys, pte,
+		rk_pte_is_page_valid(pte), &page_addr_phys, page_flags);
+}
+
+static irqreturn_t rk_iommu_irq(int irq, void *dev_id)
+{
+	struct rk_iommu *iommu = dev_id;
+	u32 status;
+	u32 int_status;
+	dma_addr_t iova;
+
+	int_status = rk_iommu_read(iommu, RK_MMU_INT_STATUS);
+	if (int_status == 0)
+		return IRQ_NONE;
+
+	iova = rk_iommu_read(iommu, RK_MMU_PAGE_FAULT_ADDR);
+
+	if (int_status & RK_MMU_IRQ_PAGE_FAULT) {
+		int flags;
+
+		status = rk_iommu_read(iommu, RK_MMU_STATUS);
+		flags = (status & RK_MMU_STATUS_PAGE_FAULT_IS_WRITE) ?
+				IOMMU_FAULT_WRITE : IOMMU_FAULT_READ;
+
+		dev_err(iommu->dev, "Page fault at %pad of type %s\n",
+			&iova,
+			(flags == IOMMU_FAULT_WRITE) ? "write" : "read");
+
+		log_iova(iommu, iova);
+
+		/*
+		 * Report page fault to any installed handlers.
+		 * Ignore the return code, though, since we always zap cache
+		 * and clear the page fault anyway.
+		 */
+		if (iommu->domain)
+			report_iommu_fault(iommu->domain, iommu->dev, iova,
+					   flags);
+		else
+			dev_err(iommu->dev, "Page fault while iommu not attached to domain?\n");
+
+		rk_iommu_command(iommu, RK_MMU_CMD_ZAP_CACHE);
+		rk_iommu_command(iommu, RK_MMU_CMD_PAGE_FAULT_DONE);
+	}
+
+	if (int_status & RK_MMU_IRQ_BUS_ERROR)
+		dev_err(iommu->dev, "BUS_ERROR occurred at %pad\n", &iova);
+
+	if (int_status & ~RK_MMU_IRQ_MASK)
+		dev_err(iommu->dev, "unexpected int_status: %#08x\n",
+			int_status);
+
+	rk_iommu_write(iommu, RK_MMU_INT_CLEAR, int_status);
+
+	return IRQ_HANDLED;
+}
+
+static phys_addr_t rk_iommu_iova_to_phys(struct iommu_domain *domain,
+					 dma_addr_t iova)
+{
+	struct rk_iommu_domain *rk_domain = domain->priv;
+	unsigned long flags;
+	phys_addr_t pt_phys, phys = 0;
+	u32 dte, pte;
+	u32 *page_table;
+
+	spin_lock_irqsave(&rk_domain->dt_lock, flags);
+
+	dte = rk_domain->dt[rk_iova_dte_index(iova)];
+	if (!rk_dte_is_pt_valid(dte))
+		goto out;
+
+	pt_phys = rk_dte_pt_address(dte);
+	page_table = (u32 *)phys_to_virt(pt_phys);
+	pte = page_table[rk_iova_pte_index(iova)];
+	if (!rk_pte_is_page_valid(pte))
+		goto out;
+
+	phys = rk_pte_page_address(pte) + rk_iova_page_offset(iova);
+out:
+	spin_unlock_irqrestore(&rk_domain->dt_lock, flags);
+
+	return phys;
+}
+
+static void rk_iommu_zap_iova(struct rk_iommu_domain *rk_domain,
+			      dma_addr_t iova, size_t size)
+{
+	struct list_head *pos;
+	unsigned long flags;
+
+	/* shootdown these iova from all iommus using this domain */
+	spin_lock_irqsave(&rk_domain->iommus_lock, flags);
+	list_for_each(pos, &rk_domain->iommus) {
+		struct rk_iommu *iommu;
+		iommu = list_entry(pos, struct rk_iommu, node);
+		rk_iommu_zap_lines(iommu, iova, size);
+	}
+	spin_unlock_irqrestore(&rk_domain->iommus_lock, flags);
+}
+
+static u32 *rk_dte_get_page_table(struct rk_iommu_domain *rk_domain,
+				  dma_addr_t iova)
+{
+	u32 *page_table, *dte_addr;
+	u32 dte;
+	phys_addr_t pt_phys;
+
+	assert_spin_locked(&rk_domain->dt_lock);
+
+	dte_addr = &rk_domain->dt[rk_iova_dte_index(iova)];
+	dte = *dte_addr;
+	if (rk_dte_is_pt_valid(dte))
+		goto done;
+
+	page_table = (u32 *)get_zeroed_page(GFP_ATOMIC | GFP_DMA32);
+	if (!page_table)
+		return ERR_PTR(-ENOMEM);
+
+	dte = rk_mk_dte(page_table);
+	*dte_addr = dte;
+
+	rk_table_flush(page_table, NUM_PT_ENTRIES);
+	rk_table_flush(dte_addr, 1);
+
+	/*
+	 * Zap the first iova of newly allocated page table so iommu evicts
+	 * old cached value of new dte from the iotlb.
+	 */
+	rk_iommu_zap_iova(rk_domain, iova, SPAGE_SIZE);
+
+done:
+	pt_phys = rk_dte_pt_address(dte);
+	return (u32 *)phys_to_virt(pt_phys);
+}
+
+static size_t rk_iommu_unmap_iova(struct rk_iommu_domain *rk_domain,
+				  u32 *pte_addr, dma_addr_t iova, size_t size)
+{
+	unsigned int pte_count;
+	unsigned int pte_total = size / SPAGE_SIZE;
+
+	assert_spin_locked(&rk_domain->dt_lock);
+
+	for (pte_count = 0; pte_count < pte_total; pte_count++) {
+		u32 pte = pte_addr[pte_count];
+		if (!rk_pte_is_page_valid(pte))
+			break;
+
+		pte_addr[pte_count] = rk_mk_pte_invalid(pte);
+	}
+
+	rk_table_flush(pte_addr, pte_count);
+
+	return pte_count * SPAGE_SIZE;
+}
+
+static int rk_iommu_map_iova(struct rk_iommu_domain *rk_domain, u32 *pte_addr,
+			     dma_addr_t iova, phys_addr_t paddr, size_t size,
+			     int prot)
+{
+	unsigned int pte_count;
+	unsigned int pte_total = size / SPAGE_SIZE;
+	phys_addr_t page_phys;
+
+	assert_spin_locked(&rk_domain->dt_lock);
+
+	for (pte_count = 0; pte_count < pte_total; pte_count++) {
+		u32 pte = pte_addr[pte_count];
+
+		if (rk_pte_is_page_valid(pte))
+			goto unwind;
+
+		pte_addr[pte_count] = rk_mk_pte(paddr, prot);
+
+		paddr += SPAGE_SIZE;
+	}
+
+	rk_table_flush(pte_addr, pte_count);
+
+	return 0;
+unwind:
+	/* Unmap the range of iovas that we just mapped */
+	rk_iommu_unmap_iova(rk_domain, pte_addr, iova, pte_count * SPAGE_SIZE);
+
+	iova += pte_count * SPAGE_SIZE;
+	page_phys = rk_pte_page_address(pte_addr[pte_count]);
+	pr_err("iova: %pad already mapped to %pa cannot remap to phys: %pa prot: %#x\n",
+	       &iova, &page_phys, &paddr, prot);
+
+	return -EADDRINUSE;
+}
+
+static int rk_iommu_map(struct iommu_domain *domain, unsigned long _iova,
+			phys_addr_t paddr, size_t size, int prot)
+{
+	struct rk_iommu_domain *rk_domain = domain->priv;
+	unsigned long flags;
+	dma_addr_t iova = (dma_addr_t)_iova;
+	u32 *page_table, *pte_addr;
+	int ret;
+
+	spin_lock_irqsave(&rk_domain->dt_lock, flags);
+
+	/*
+	 * pgsize_bitmap specifies iova sizes that fit in one page table
+	 * (1024 4-KiB pages = 4 MiB).
+	 * So, size will always be 4096 <= size <= 4194304.
+	 * Since iommu_map() guarantees that both iova and size will be
+	 * aligned, we will always only be mapping from a single dte here.
+	 */
+	page_table = rk_dte_get_page_table(rk_domain, iova);
+	if (IS_ERR(page_table)) {
+		spin_unlock_irqrestore(&rk_domain->dt_lock, flags);
+		return PTR_ERR(page_table);
+	}
+
+	pte_addr = &page_table[rk_iova_pte_index(iova)];
+	ret = rk_iommu_map_iova(rk_domain, pte_addr, iova, paddr, size, prot);
+	spin_unlock_irqrestore(&rk_domain->dt_lock, flags);
+
+	return ret;
+}
+
+static size_t rk_iommu_unmap(struct iommu_domain *domain, unsigned long _iova,
+			     size_t size)
+{
+	struct rk_iommu_domain *rk_domain = domain->priv;
+	unsigned long flags;
+	dma_addr_t iova = (dma_addr_t)_iova;
+	phys_addr_t pt_phys;
+	u32 dte;
+	u32 *pte_addr;
+	size_t unmap_size;
+
+	spin_lock_irqsave(&rk_domain->dt_lock, flags);
+
+	/*
+	 * pgsize_bitmap specifies iova sizes that fit in one page table
+	 * (1024 4-KiB pages = 4 MiB).
+	 * So, size will always be 4096 <= size <= 4194304.
+	 * Since iommu_unmap() guarantees that both iova and size will be
+	 * aligned, we will always only be unmapping from a single dte here.
+	 */
+	dte = rk_domain->dt[rk_iova_dte_index(iova)];
+	/* Just return 0 if iova is unmapped */
+	if (!rk_dte_is_pt_valid(dte)) {
+		spin_unlock_irqrestore(&rk_domain->dt_lock, flags);
+		return 0;
+	}
+
+	pt_phys = rk_dte_pt_address(dte);
+	pte_addr = (u32 *)phys_to_virt(pt_phys) + rk_iova_pte_index(iova);
+	unmap_size = rk_iommu_unmap_iova(rk_domain, pte_addr, iova, size);
+
+	spin_unlock_irqrestore(&rk_domain->dt_lock, flags);
+
+	/* Shootdown iotlb entries for iova range that was just unmapped */
+	rk_iommu_zap_iova(rk_domain, iova, unmap_size);
+
+	return unmap_size;
+}
+
+static struct rk_iommu *rk_iommu_from_dev(struct device *dev)
+{
+	struct iommu_group *group;
+	struct device *iommu_dev;
+	struct rk_iommu *rk_iommu;
+
+	group = iommu_group_get(dev);
+	if (!group)
+		return NULL;
+	iommu_dev = iommu_group_get_iommudata(group);
+	rk_iommu = dev_get_drvdata(iommu_dev);
+	iommu_group_put(group);
+
+	return rk_iommu;
+}
+
+static int rk_iommu_attach_device(struct iommu_domain *domain,
+				  struct device *dev)
+{
+	struct rk_iommu *iommu;
+	struct rk_iommu_domain *rk_domain = domain->priv;
+	unsigned long flags;
+	int ret;
+	phys_addr_t dte_addr;
+
+	/*
+	 * Allow 'virtual devices' (e.g., drm) to attach to domain.
+	 * Such a device does not belong to an iommu group.
+	 */
+	iommu = rk_iommu_from_dev(dev);
+	if (!iommu)
+		return 0;
+
+	ret = rk_iommu_enable_stall(iommu);
+	if (ret)
+		return ret;
+
+	ret = rk_iommu_force_reset(iommu);
+	if (ret)
+		return ret;
+
+	iommu->domain = domain;
+
+	ret = devm_request_irq(dev, iommu->irq, rk_iommu_irq,
+			       IRQF_SHARED, dev_name(dev), iommu);
+	if (ret)
+		return ret;
+
+	dte_addr = virt_to_phys(rk_domain->dt);
+	rk_iommu_write(iommu, RK_MMU_DTE_ADDR, dte_addr);
+	rk_iommu_command(iommu, RK_MMU_CMD_ZAP_CACHE);
+	rk_iommu_write(iommu, RK_MMU_INT_MASK, RK_MMU_IRQ_MASK);
+
+	ret = rk_iommu_enable_paging(iommu);
+	if (ret)
+		return ret;
+
+	spin_lock_irqsave(&rk_domain->iommus_lock, flags);
+	list_add_tail(&iommu->node, &rk_domain->iommus);
+	spin_unlock_irqrestore(&rk_domain->iommus_lock, flags);
+
+	dev_info(dev, "Attached to iommu domain\n");
+
+	rk_iommu_disable_stall(iommu);
+
+	return 0;
+}
+
+static void rk_iommu_detach_device(struct iommu_domain *domain,
+				   struct device *dev)
+{
+	struct rk_iommu *iommu;
+	struct rk_iommu_domain *rk_domain = domain->priv;
+	unsigned long flags;
+
+	/* Allow 'virtual devices' (eg drm) to detach from domain */
+	iommu = rk_iommu_from_dev(dev);
+	if (!iommu)
+		return;
+
+	spin_lock_irqsave(&rk_domain->iommus_lock, flags);
+	list_del_init(&iommu->node);
+	spin_unlock_irqrestore(&rk_domain->iommus_lock, flags);
+
+	/* Ignore error while disabling, just keep going */
+	rk_iommu_enable_stall(iommu);
+	rk_iommu_disable_paging(iommu);
+	rk_iommu_write(iommu, RK_MMU_INT_MASK, 0);
+	rk_iommu_write(iommu, RK_MMU_DTE_ADDR, 0);
+	rk_iommu_disable_stall(iommu);
+
+	devm_free_irq(dev, iommu->irq, iommu);
+
+	iommu->domain = NULL;
+
+	dev_info(dev, "Detached from iommu domain\n");
+}
+
+static int rk_iommu_domain_init(struct iommu_domain *domain)
+{
+	struct rk_iommu_domain *rk_domain;
+
+	rk_domain = kzalloc(sizeof(*rk_domain), GFP_KERNEL);
+	if (!rk_domain)
+		return -ENOMEM;
+
+	/*
+	 * rk32xx iommus use a 2 level pagetable.
+	 * Each level1 (dt) and level2 (pt) table has 1024 4-byte entries.
+	 * Allocate one 4 KiB page for each table.
+	 */
+	rk_domain->dt = (u32 *)get_zeroed_page(GFP_KERNEL | GFP_DMA32);
+	if (!rk_domain->dt)
+		goto err_dt;
+
+	rk_table_flush(rk_domain->dt, NUM_DT_ENTRIES);
+
+	spin_lock_init(&rk_domain->iommus_lock);
+	spin_lock_init(&rk_domain->dt_lock);
+	INIT_LIST_HEAD(&rk_domain->iommus);
+
+	domain->priv = rk_domain;
+
+	return 0;
+err_dt:
+	kfree(rk_domain);
+	return -ENOMEM;
+}
+
+static void rk_iommu_domain_destroy(struct iommu_domain *domain)
+{
+	struct rk_iommu_domain *rk_domain = domain->priv;
+	int i;
+
+	WARN_ON(!list_empty(&rk_domain->iommus));
+
+	for (i = 0; i < NUM_DT_ENTRIES; i++) {
+		u32 dte = rk_domain->dt[i];
+		if (rk_dte_is_pt_valid(dte)) {
+			phys_addr_t pt_phys = rk_dte_pt_address(dte);
+			u32 *page_table = phys_to_virt(pt_phys);
+			free_page((unsigned long)page_table);
+		}
+	}
+
+	free_page((unsigned long)rk_domain->dt);
+	kfree(domain->priv);
+	domain->priv = NULL;
+}
+
+static bool rk_iommu_is_dev_iommu_master(struct device *dev)
+{
+	struct device_node *np = dev->of_node;
+	int ret;
+
+	/*
+	 * An iommu master has an iommus property containing a list of phandles
+	 * to iommu nodes, each with an #iommu-cells property with value 0.
+	 */
+	ret = of_count_phandle_with_args(np, "iommus", "#iommu-cells");
+	return (ret > 0);
+}
+
+static int rk_iommu_group_set_iommudata(struct iommu_group *group,
+					struct device *dev)
+{
+	struct device_node *np = dev->of_node;
+	struct platform_device *pd;
+	int ret;
+	struct of_phandle_args args;
+
+	/*
+	 * An iommu master has an iommus property containing a list of phandles
+	 * to iommu nodes, each with an #iommu-cells property with value 0.
+	 */
+	ret = of_parse_phandle_with_args(np, "iommus", "#iommu-cells", 0,
+					 &args);
+	if (ret) {
+		dev_err(dev, "of_parse_phandle_with_args(%s) => %d\n",
+			np->full_name, ret);
+		return ret;
+	}
+	if (args.args_count != 0) {
+		dev_err(dev, "incorrect number of iommu params found for %s (found %d, expected 0)\n",
+			args.np->full_name, args.args_count);
+		return -EINVAL;
+	}
+
+	pd = of_find_device_by_node(args.np);
+	of_node_put(args.np);
+	if (!pd) {
+		dev_err(dev, "iommu %s not found\n", args.np->full_name);
+		return -EPROBE_DEFER;
+	}
+
+	/* TODO(djkurtz): handle multiple slave iommus for a single master */
+	iommu_group_set_iommudata(group, &pd->dev, NULL);
+
+	return 0;
+}
+
+static int rk_iommu_add_device(struct device *dev)
+{
+	struct iommu_group *group;
+	int ret;
+
+	if (!rk_iommu_is_dev_iommu_master(dev))
+		return -ENODEV;
+
+	group = iommu_group_get(dev);
+	if (!group) {
+		group = iommu_group_alloc();
+		if (IS_ERR(group)) {
+			dev_err(dev, "Failed to allocate IOMMU group\n");
+			return PTR_ERR(group);
+		}
+	}
+
+	ret = iommu_group_add_device(group, dev);
+	if (ret)
+		goto err_put_group;
+
+	ret = rk_iommu_group_set_iommudata(group, dev);
+	if (ret)
+		goto err_remove_device;
+
+	iommu_group_put(group);
+
+	return 0;
+
+err_remove_device:
+	iommu_group_remove_device(dev);
+err_put_group:
+	iommu_group_put(group);
+	return ret;
+}
+
+static void rk_iommu_remove_device(struct device *dev)
+{
+	if (!rk_iommu_is_dev_iommu_master(dev))
+		return;
+
+	iommu_group_remove_device(dev);
+}
+
+static const struct iommu_ops rk_iommu_ops = {
+	.domain_init = rk_iommu_domain_init,
+	.domain_destroy = rk_iommu_domain_destroy,
+	.attach_dev = rk_iommu_attach_device,
+	.detach_dev = rk_iommu_detach_device,
+	.map = rk_iommu_map,
+	.unmap = rk_iommu_unmap,
+	.add_device = rk_iommu_add_device,
+	.remove_device = rk_iommu_remove_device,
+	.iova_to_phys = rk_iommu_iova_to_phys,
+	.pgsize_bitmap = RK_IOMMU_PGSIZE_BITMAP,
+};
+
+static int rk_iommu_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct rk_iommu *iommu;
+	struct resource *res;
+
+	iommu = devm_kzalloc(dev, sizeof(*iommu), GFP_KERNEL);
+	if (!iommu)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, iommu);
+	iommu->dev = dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	iommu->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(iommu->base))
+		return PTR_ERR(iommu->base);
+
+	iommu->irq = platform_get_irq(pdev, 0);
+	if (iommu->irq < 0) {
+		dev_err(dev, "Failed to get IRQ, %d\n", iommu->irq);
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+static int rk_iommu_remove(struct platform_device *pdev)
+{
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id rk_iommu_dt_ids[] = {
+	{ .compatible = "rockchip,iommu" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, rk_iommu_dt_ids);
+#endif
+
+static struct platform_driver rk_iommu_driver = {
+	.probe = rk_iommu_probe,
+	.remove = rk_iommu_remove,
+	.driver = {
+		   .name = "rk_iommu",
+		   .owner = THIS_MODULE,
+		   .of_match_table = of_match_ptr(rk_iommu_dt_ids),
+	},
+};
+
+static int __init rk_iommu_init(void)
+{
+	int ret;
+
+	ret = bus_set_iommu(&platform_bus_type, &rk_iommu_ops);
+	if (ret)
+		return ret;
+
+	return platform_driver_register(&rk_iommu_driver);
+}
+static void __exit rk_iommu_exit(void)
+{
+	platform_driver_unregister(&rk_iommu_driver);
+}
+
+subsys_initcall(rk_iommu_init);
+module_exit(rk_iommu_exit);
+
+MODULE_DESCRIPTION("IOMMU API for Rockchip");
+MODULE_AUTHOR("Simon Xue <xxm@rock-chips.com> and Daniel Kurtz <djkurtz@chromium.org>");
+MODULE_ALIAS("platform:rockchip-iommu");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iommu/shmobile-ipmmu.c b/drivers/iommu/shmobile-ipmmu.c
index bd97adecb1fd..951651a9746b 100644
--- a/drivers/iommu/shmobile-ipmmu.c
+++ b/drivers/iommu/shmobile-ipmmu.c
@@ -118,7 +118,6 @@ static int ipmmu_probe(struct platform_device *pdev)
 static struct platform_driver ipmmu_driver = {
 	.probe = ipmmu_probe,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "ipmmu",
 	},
 };
diff --git a/drivers/iommu/tegra-gart.c b/drivers/iommu/tegra-gart.c
index a6d76abf2c06..f722a0c466cf 100644
--- a/drivers/iommu/tegra-gart.c
+++ b/drivers/iommu/tegra-gart.c
@@ -425,7 +425,6 @@ static struct platform_driver tegra_gart_driver = {
 	.probe		= tegra_gart_probe,
 	.remove		= tegra_gart_remove,
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= "tegra-gart",
 		.pm	= &tegra_gart_pm_ops,
 		.of_match_table = tegra_gart_of_match,
diff --git a/drivers/irqchip/Kconfig b/drivers/irqchip/Kconfig
index 9efe5f10f97b..cc79d2a5a8c2 100644
--- a/drivers/irqchip/Kconfig
+++ b/drivers/irqchip/Kconfig
@@ -5,8 +5,15 @@ config IRQCHIP
 config ARM_GIC
 	bool
 	select IRQ_DOMAIN
+	select IRQ_DOMAIN_HIERARCHY
 	select MULTI_IRQ_HANDLER
 
+config ARM_GIC_V2M
+	bool
+	depends on ARM_GIC
+	depends on PCI && PCI_MSI
+	select PCI_MSI_IRQ_DOMAIN
+
 config GIC_NON_BANKED
 	bool
 
@@ -14,6 +21,11 @@ config ARM_GIC_V3
 	bool
 	select IRQ_DOMAIN
 	select MULTI_IRQ_HANDLER
+	select IRQ_DOMAIN_HIERARCHY
+
+config ARM_GIC_V3_ITS
+	bool
+	select PCI_MSI_IRQ_DOMAIN
 
 config ARM_NVIC
 	bool
@@ -130,3 +142,7 @@ config KEYSTONE_IRQ
 	help
 		Support for Texas Instruments Keystone 2 IRQ controller IP which
 		is part of the Keystone 2 IPC mechanism
+
+config MIPS_GIC
+	bool
+	select MIPS_CM
diff --git a/drivers/irqchip/Makefile b/drivers/irqchip/Makefile
index f0909d05eae3..9516a324be6d 100644
--- a/drivers/irqchip/Makefile
+++ b/drivers/irqchip/Makefile
@@ -19,7 +19,9 @@ obj-$(CONFIG_ARCH_SUNXI)		+= irq-sun4i.o
 obj-$(CONFIG_ARCH_SUNXI)		+= irq-sunxi-nmi.o
 obj-$(CONFIG_ARCH_SPEAR3XX)		+= spear-shirq.o
 obj-$(CONFIG_ARM_GIC)			+= irq-gic.o irq-gic-common.o
+obj-$(CONFIG_ARM_GIC_V2M)		+= irq-gic-v2m.o
 obj-$(CONFIG_ARM_GIC_V3)		+= irq-gic-v3.o irq-gic-common.o
+obj-$(CONFIG_ARM_GIC_V3_ITS)		+= irq-gic-v3-its.o
 obj-$(CONFIG_ARM_NVIC)			+= irq-nvic.o
 obj-$(CONFIG_ARM_VIC)			+= irq-vic.o
 obj-$(CONFIG_ATMEL_AIC_IRQ)		+= irq-atmel-aic-common.o irq-atmel-aic.o
@@ -38,3 +40,5 @@ obj-$(CONFIG_IRQ_CROSSBAR)		+= irq-crossbar.o
 obj-$(CONFIG_BCM7120_L2_IRQ)		+= irq-bcm7120-l2.o
 obj-$(CONFIG_BRCMSTB_L2_IRQ)		+= irq-brcmstb-l2.o
 obj-$(CONFIG_KEYSTONE_IRQ)		+= irq-keystone.o
+obj-$(CONFIG_MIPS_GIC)			+= irq-mips-gic.o
+obj-$(CONFIG_ARCH_MEDIATEK)		+= irq-mtk-sysirq.o
diff --git a/drivers/irqchip/irq-gic-v2m.c b/drivers/irqchip/irq-gic-v2m.c
new file mode 100644
index 000000000000..fdf706555d72
--- /dev/null
+++ b/drivers/irqchip/irq-gic-v2m.c
@@ -0,0 +1,333 @@
+/*
+ * ARM GIC v2m MSI(-X) support
+ * Support for Message Signaled Interrupts for systems that
+ * implement ARM Generic Interrupt Controller: GICv2m.
+ *
+ * Copyright (C) 2014 Advanced Micro Devices, Inc.
+ * Authors: Suravee Suthikulpanit <suravee.suthikulpanit@amd.com>
+ *	    Harish Kasiviswanathan <harish.kasiviswanathan@amd.com>
+ *	    Brandon Anderson <brandon.anderson@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.
+ */
+
+#define pr_fmt(fmt) "GICv2m: " fmt
+
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/kernel.h>
+#include <linux/of_address.h>
+#include <linux/of_pci.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+/*
+* MSI_TYPER:
+*     [31:26] Reserved
+*     [25:16] lowest SPI assigned to MSI
+*     [15:10] Reserved
+*     [9:0]   Numer of SPIs assigned to MSI
+*/
+#define V2M_MSI_TYPER		       0x008
+#define V2M_MSI_TYPER_BASE_SHIFT       16
+#define V2M_MSI_TYPER_BASE_MASK	       0x3FF
+#define V2M_MSI_TYPER_NUM_MASK	       0x3FF
+#define V2M_MSI_SETSPI_NS	       0x040
+#define V2M_MIN_SPI		       32
+#define V2M_MAX_SPI		       1019
+
+#define V2M_MSI_TYPER_BASE_SPI(x)      \
+	       (((x) >> V2M_MSI_TYPER_BASE_SHIFT) & V2M_MSI_TYPER_BASE_MASK)
+
+#define V2M_MSI_TYPER_NUM_SPI(x)       ((x) & V2M_MSI_TYPER_NUM_MASK)
+
+struct v2m_data {
+	spinlock_t msi_cnt_lock;
+	struct msi_controller mchip;
+	struct resource res;	/* GICv2m resource */
+	void __iomem *base;	/* GICv2m virt address */
+	u32 spi_start;		/* The SPI number that MSIs start */
+	u32 nr_spis;		/* The number of SPIs for MSIs */
+	unsigned long *bm;	/* MSI vector bitmap */
+	struct irq_domain *domain;
+};
+
+static void gicv2m_mask_msi_irq(struct irq_data *d)
+{
+	pci_msi_mask_irq(d);
+	irq_chip_mask_parent(d);
+}
+
+static void gicv2m_unmask_msi_irq(struct irq_data *d)
+{
+	pci_msi_unmask_irq(d);
+	irq_chip_unmask_parent(d);
+}
+
+static struct irq_chip gicv2m_msi_irq_chip = {
+	.name			= "MSI",
+	.irq_mask		= gicv2m_mask_msi_irq,
+	.irq_unmask		= gicv2m_unmask_msi_irq,
+	.irq_eoi		= irq_chip_eoi_parent,
+	.irq_write_msi_msg	= pci_msi_domain_write_msg,
+};
+
+static struct msi_domain_info gicv2m_msi_domain_info = {
+	.flags	= (MSI_FLAG_USE_DEF_DOM_OPS | MSI_FLAG_USE_DEF_CHIP_OPS |
+		   MSI_FLAG_PCI_MSIX),
+	.chip	= &gicv2m_msi_irq_chip,
+};
+
+static int gicv2m_set_affinity(struct irq_data *irq_data,
+			       const struct cpumask *mask, bool force)
+{
+	int ret;
+
+	ret = irq_chip_set_affinity_parent(irq_data, mask, force);
+	if (ret == IRQ_SET_MASK_OK)
+		ret = IRQ_SET_MASK_OK_DONE;
+
+	return ret;
+}
+
+static void gicv2m_compose_msi_msg(struct irq_data *data, struct msi_msg *msg)
+{
+	struct v2m_data *v2m = irq_data_get_irq_chip_data(data);
+	phys_addr_t addr = v2m->res.start + V2M_MSI_SETSPI_NS;
+
+	msg->address_hi = (u32) (addr >> 32);
+	msg->address_lo = (u32) (addr);
+	msg->data = data->hwirq;
+}
+
+static struct irq_chip gicv2m_irq_chip = {
+	.name			= "GICv2m",
+	.irq_mask		= irq_chip_mask_parent,
+	.irq_unmask		= irq_chip_unmask_parent,
+	.irq_eoi		= irq_chip_eoi_parent,
+	.irq_set_affinity	= gicv2m_set_affinity,
+	.irq_compose_msi_msg	= gicv2m_compose_msi_msg,
+};
+
+static int gicv2m_irq_gic_domain_alloc(struct irq_domain *domain,
+				       unsigned int virq,
+				       irq_hw_number_t hwirq)
+{
+	struct of_phandle_args args;
+	struct irq_data *d;
+	int err;
+
+	args.np = domain->parent->of_node;
+	args.args_count = 3;
+	args.args[0] = 0;
+	args.args[1] = hwirq - 32;
+	args.args[2] = IRQ_TYPE_EDGE_RISING;
+
+	err = irq_domain_alloc_irqs_parent(domain, virq, 1, &args);
+	if (err)
+		return err;
+
+	/* Configure the interrupt line to be edge */
+	d = irq_domain_get_irq_data(domain->parent, virq);
+	d->chip->irq_set_type(d, IRQ_TYPE_EDGE_RISING);
+	return 0;
+}
+
+static void gicv2m_unalloc_msi(struct v2m_data *v2m, unsigned int hwirq)
+{
+	int pos;
+
+	pos = hwirq - v2m->spi_start;
+	if (pos < 0 || pos >= v2m->nr_spis) {
+		pr_err("Failed to teardown msi. Invalid hwirq %d\n", hwirq);
+		return;
+	}
+
+	spin_lock(&v2m->msi_cnt_lock);
+	__clear_bit(pos, v2m->bm);
+	spin_unlock(&v2m->msi_cnt_lock);
+}
+
+static int gicv2m_irq_domain_alloc(struct irq_domain *domain, unsigned int virq,
+				   unsigned int nr_irqs, void *args)
+{
+	struct v2m_data *v2m = domain->host_data;
+	int hwirq, offset, err = 0;
+
+	spin_lock(&v2m->msi_cnt_lock);
+	offset = find_first_zero_bit(v2m->bm, v2m->nr_spis);
+	if (offset < v2m->nr_spis)
+		__set_bit(offset, v2m->bm);
+	else
+		err = -ENOSPC;
+	spin_unlock(&v2m->msi_cnt_lock);
+
+	if (err)
+		return err;
+
+	hwirq = v2m->spi_start + offset;
+
+	err = gicv2m_irq_gic_domain_alloc(domain, virq, hwirq);
+	if (err) {
+		gicv2m_unalloc_msi(v2m, hwirq);
+		return err;
+	}
+
+	irq_domain_set_hwirq_and_chip(domain, virq, hwirq,
+				      &gicv2m_irq_chip, v2m);
+
+	return 0;
+}
+
+static void gicv2m_irq_domain_free(struct irq_domain *domain,
+				   unsigned int virq, unsigned int nr_irqs)
+{
+	struct irq_data *d = irq_domain_get_irq_data(domain, virq);
+	struct v2m_data *v2m = irq_data_get_irq_chip_data(d);
+
+	BUG_ON(nr_irqs != 1);
+	gicv2m_unalloc_msi(v2m, d->hwirq);
+	irq_domain_free_irqs_parent(domain, virq, nr_irqs);
+}
+
+static const struct irq_domain_ops gicv2m_domain_ops = {
+	.alloc			= gicv2m_irq_domain_alloc,
+	.free			= gicv2m_irq_domain_free,
+};
+
+static bool is_msi_spi_valid(u32 base, u32 num)
+{
+	if (base < V2M_MIN_SPI) {
+		pr_err("Invalid MSI base SPI (base:%u)\n", base);
+		return false;
+	}
+
+	if ((num == 0) || (base + num > V2M_MAX_SPI)) {
+		pr_err("Number of SPIs (%u) exceed maximum (%u)\n",
+		       num, V2M_MAX_SPI - V2M_MIN_SPI + 1);
+		return false;
+	}
+
+	return true;
+}
+
+static int __init gicv2m_init_one(struct device_node *node,
+				  struct irq_domain *parent)
+{
+	int ret;
+	struct v2m_data *v2m;
+
+	v2m = kzalloc(sizeof(struct v2m_data), GFP_KERNEL);
+	if (!v2m) {
+		pr_err("Failed to allocate struct v2m_data.\n");
+		return -ENOMEM;
+	}
+
+	ret = of_address_to_resource(node, 0, &v2m->res);
+	if (ret) {
+		pr_err("Failed to allocate v2m resource.\n");
+		goto err_free_v2m;
+	}
+
+	v2m->base = ioremap(v2m->res.start, resource_size(&v2m->res));
+	if (!v2m->base) {
+		pr_err("Failed to map GICv2m resource\n");
+		ret = -ENOMEM;
+		goto err_free_v2m;
+	}
+
+	if (!of_property_read_u32(node, "arm,msi-base-spi", &v2m->spi_start) &&
+	    !of_property_read_u32(node, "arm,msi-num-spis", &v2m->nr_spis)) {
+		pr_info("Overriding V2M MSI_TYPER (base:%u, num:%u)\n",
+			v2m->spi_start, v2m->nr_spis);
+	} else {
+		u32 typer = readl_relaxed(v2m->base + V2M_MSI_TYPER);
+
+		v2m->spi_start = V2M_MSI_TYPER_BASE_SPI(typer);
+		v2m->nr_spis = V2M_MSI_TYPER_NUM_SPI(typer);
+	}
+
+	if (!is_msi_spi_valid(v2m->spi_start, v2m->nr_spis)) {
+		ret = -EINVAL;
+		goto err_iounmap;
+	}
+
+	v2m->bm = kzalloc(sizeof(long) * BITS_TO_LONGS(v2m->nr_spis),
+			  GFP_KERNEL);
+	if (!v2m->bm) {
+		ret = -ENOMEM;
+		goto err_iounmap;
+	}
+
+	v2m->domain = irq_domain_add_tree(NULL, &gicv2m_domain_ops, v2m);
+	if (!v2m->domain) {
+		pr_err("Failed to create GICv2m domain\n");
+		ret = -ENOMEM;
+		goto err_free_bm;
+	}
+
+	v2m->domain->parent = parent;
+	v2m->mchip.of_node = node;
+	v2m->mchip.domain = pci_msi_create_irq_domain(node,
+						      &gicv2m_msi_domain_info,
+						      v2m->domain);
+	if (!v2m->mchip.domain) {
+		pr_err("Failed to create MSI domain\n");
+		ret = -ENOMEM;
+		goto err_free_domains;
+	}
+
+	spin_lock_init(&v2m->msi_cnt_lock);
+
+	ret = of_pci_msi_chip_add(&v2m->mchip);
+	if (ret) {
+		pr_err("Failed to add msi_chip.\n");
+		goto err_free_domains;
+	}
+
+	pr_info("Node %s: range[%#lx:%#lx], SPI[%d:%d]\n", node->name,
+		(unsigned long)v2m->res.start, (unsigned long)v2m->res.end,
+		v2m->spi_start, (v2m->spi_start + v2m->nr_spis));
+
+	return 0;
+
+err_free_domains:
+	if (v2m->mchip.domain)
+		irq_domain_remove(v2m->mchip.domain);
+	if (v2m->domain)
+		irq_domain_remove(v2m->domain);
+err_free_bm:
+	kfree(v2m->bm);
+err_iounmap:
+	iounmap(v2m->base);
+err_free_v2m:
+	kfree(v2m);
+	return ret;
+}
+
+static struct of_device_id gicv2m_device_id[] = {
+	{	.compatible	= "arm,gic-v2m-frame",	},
+	{},
+};
+
+int __init gicv2m_of_init(struct device_node *node, struct irq_domain *parent)
+{
+	int ret = 0;
+	struct device_node *child;
+
+	for (child = of_find_matching_node(node, gicv2m_device_id); child;
+	     child = of_find_matching_node(child, gicv2m_device_id)) {
+		if (!of_find_property(child, "msi-controller", NULL))
+			continue;
+
+		ret = gicv2m_init_one(child, parent);
+		if (ret) {
+			of_node_put(node);
+			break;
+		}
+	}
+
+	return ret;
+}
diff --git a/drivers/irqchip/irq-gic-v3-its.c b/drivers/irqchip/irq-gic-v3-its.c
new file mode 100644
index 000000000000..86e4684adeb1
--- /dev/null
+++ b/drivers/irqchip/irq-gic-v3-its.c
@@ -0,0 +1,1425 @@
+/*
+ * Copyright (C) 2013, 2014 ARM Limited, All Rights Reserved.
+ * Author: Marc Zyngier <marc.zyngier@arm.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/bitmap.h>
+#include <linux/cpu.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/log2.h>
+#include <linux/mm.h>
+#include <linux/msi.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/of_pci.h>
+#include <linux/of_platform.h>
+#include <linux/percpu.h>
+#include <linux/slab.h>
+
+#include <linux/irqchip/arm-gic-v3.h>
+
+#include <asm/cacheflush.h>
+#include <asm/cputype.h>
+#include <asm/exception.h>
+
+#include "irqchip.h"
+
+#define ITS_FLAGS_CMDQ_NEEDS_FLUSHING		(1 << 0)
+
+#define RDIST_FLAGS_PROPBASE_NEEDS_FLUSHING	(1 << 0)
+
+/*
+ * Collection structure - just an ID, and a redistributor address to
+ * ping. We use one per CPU as a bag of interrupts assigned to this
+ * CPU.
+ */
+struct its_collection {
+	u64			target_address;
+	u16			col_id;
+};
+
+/*
+ * The ITS structure - contains most of the infrastructure, with the
+ * msi_controller, the command queue, the collections, and the list of
+ * devices writing to it.
+ */
+struct its_node {
+	raw_spinlock_t		lock;
+	struct list_head	entry;
+	struct msi_controller	msi_chip;
+	struct irq_domain	*domain;
+	void __iomem		*base;
+	unsigned long		phys_base;
+	struct its_cmd_block	*cmd_base;
+	struct its_cmd_block	*cmd_write;
+	void			*tables[GITS_BASER_NR_REGS];
+	struct its_collection	*collections;
+	struct list_head	its_device_list;
+	u64			flags;
+	u32			ite_size;
+};
+
+#define ITS_ITT_ALIGN		SZ_256
+
+/*
+ * The ITS view of a device - belongs to an ITS, a collection, owns an
+ * interrupt translation table, and a list of interrupts.
+ */
+struct its_device {
+	struct list_head	entry;
+	struct its_node		*its;
+	struct its_collection	*collection;
+	void			*itt;
+	unsigned long		*lpi_map;
+	irq_hw_number_t		lpi_base;
+	int			nr_lpis;
+	u32			nr_ites;
+	u32			device_id;
+};
+
+static LIST_HEAD(its_nodes);
+static DEFINE_SPINLOCK(its_lock);
+static struct device_node *gic_root_node;
+static struct rdists *gic_rdists;
+
+#define gic_data_rdist()		(raw_cpu_ptr(gic_rdists->rdist))
+#define gic_data_rdist_rd_base()	(gic_data_rdist()->rd_base)
+
+/*
+ * ITS command descriptors - parameters to be encoded in a command
+ * block.
+ */
+struct its_cmd_desc {
+	union {
+		struct {
+			struct its_device *dev;
+			u32 event_id;
+		} its_inv_cmd;
+
+		struct {
+			struct its_device *dev;
+			u32 event_id;
+		} its_int_cmd;
+
+		struct {
+			struct its_device *dev;
+			int valid;
+		} its_mapd_cmd;
+
+		struct {
+			struct its_collection *col;
+			int valid;
+		} its_mapc_cmd;
+
+		struct {
+			struct its_device *dev;
+			u32 phys_id;
+			u32 event_id;
+		} its_mapvi_cmd;
+
+		struct {
+			struct its_device *dev;
+			struct its_collection *col;
+			u32 id;
+		} its_movi_cmd;
+
+		struct {
+			struct its_device *dev;
+			u32 event_id;
+		} its_discard_cmd;
+
+		struct {
+			struct its_collection *col;
+		} its_invall_cmd;
+	};
+};
+
+/*
+ * The ITS command block, which is what the ITS actually parses.
+ */
+struct its_cmd_block {
+	u64	raw_cmd[4];
+};
+
+#define ITS_CMD_QUEUE_SZ		SZ_64K
+#define ITS_CMD_QUEUE_NR_ENTRIES	(ITS_CMD_QUEUE_SZ / sizeof(struct its_cmd_block))
+
+typedef struct its_collection *(*its_cmd_builder_t)(struct its_cmd_block *,
+						    struct its_cmd_desc *);
+
+static void its_encode_cmd(struct its_cmd_block *cmd, u8 cmd_nr)
+{
+	cmd->raw_cmd[0] &= ~0xffUL;
+	cmd->raw_cmd[0] |= cmd_nr;
+}
+
+static void its_encode_devid(struct its_cmd_block *cmd, u32 devid)
+{
+	cmd->raw_cmd[0] &= ~(0xffffUL << 32);
+	cmd->raw_cmd[0] |= ((u64)devid) << 32;
+}
+
+static void its_encode_event_id(struct its_cmd_block *cmd, u32 id)
+{
+	cmd->raw_cmd[1] &= ~0xffffffffUL;
+	cmd->raw_cmd[1] |= id;
+}
+
+static void its_encode_phys_id(struct its_cmd_block *cmd, u32 phys_id)
+{
+	cmd->raw_cmd[1] &= 0xffffffffUL;
+	cmd->raw_cmd[1] |= ((u64)phys_id) << 32;
+}
+
+static void its_encode_size(struct its_cmd_block *cmd, u8 size)
+{
+	cmd->raw_cmd[1] &= ~0x1fUL;
+	cmd->raw_cmd[1] |= size & 0x1f;
+}
+
+static void its_encode_itt(struct its_cmd_block *cmd, u64 itt_addr)
+{
+	cmd->raw_cmd[2] &= ~0xffffffffffffUL;
+	cmd->raw_cmd[2] |= itt_addr & 0xffffffffff00UL;
+}
+
+static void its_encode_valid(struct its_cmd_block *cmd, int valid)
+{
+	cmd->raw_cmd[2] &= ~(1UL << 63);
+	cmd->raw_cmd[2] |= ((u64)!!valid) << 63;
+}
+
+static void its_encode_target(struct its_cmd_block *cmd, u64 target_addr)
+{
+	cmd->raw_cmd[2] &= ~(0xffffffffUL << 16);
+	cmd->raw_cmd[2] |= (target_addr & (0xffffffffUL << 16));
+}
+
+static void its_encode_collection(struct its_cmd_block *cmd, u16 col)
+{
+	cmd->raw_cmd[2] &= ~0xffffUL;
+	cmd->raw_cmd[2] |= col;
+}
+
+static inline void its_fixup_cmd(struct its_cmd_block *cmd)
+{
+	/* Let's fixup BE commands */
+	cmd->raw_cmd[0] = cpu_to_le64(cmd->raw_cmd[0]);
+	cmd->raw_cmd[1] = cpu_to_le64(cmd->raw_cmd[1]);
+	cmd->raw_cmd[2] = cpu_to_le64(cmd->raw_cmd[2]);
+	cmd->raw_cmd[3] = cpu_to_le64(cmd->raw_cmd[3]);
+}
+
+static struct its_collection *its_build_mapd_cmd(struct its_cmd_block *cmd,
+						 struct its_cmd_desc *desc)
+{
+	unsigned long itt_addr;
+	u8 size = ilog2(desc->its_mapd_cmd.dev->nr_ites);
+
+	itt_addr = virt_to_phys(desc->its_mapd_cmd.dev->itt);
+	itt_addr = ALIGN(itt_addr, ITS_ITT_ALIGN);
+
+	its_encode_cmd(cmd, GITS_CMD_MAPD);
+	its_encode_devid(cmd, desc->its_mapd_cmd.dev->device_id);
+	its_encode_size(cmd, size - 1);
+	its_encode_itt(cmd, itt_addr);
+	its_encode_valid(cmd, desc->its_mapd_cmd.valid);
+
+	its_fixup_cmd(cmd);
+
+	return desc->its_mapd_cmd.dev->collection;
+}
+
+static struct its_collection *its_build_mapc_cmd(struct its_cmd_block *cmd,
+						 struct its_cmd_desc *desc)
+{
+	its_encode_cmd(cmd, GITS_CMD_MAPC);
+	its_encode_collection(cmd, desc->its_mapc_cmd.col->col_id);
+	its_encode_target(cmd, desc->its_mapc_cmd.col->target_address);
+	its_encode_valid(cmd, desc->its_mapc_cmd.valid);
+
+	its_fixup_cmd(cmd);
+
+	return desc->its_mapc_cmd.col;
+}
+
+static struct its_collection *its_build_mapvi_cmd(struct its_cmd_block *cmd,
+						  struct its_cmd_desc *desc)
+{
+	its_encode_cmd(cmd, GITS_CMD_MAPVI);
+	its_encode_devid(cmd, desc->its_mapvi_cmd.dev->device_id);
+	its_encode_event_id(cmd, desc->its_mapvi_cmd.event_id);
+	its_encode_phys_id(cmd, desc->its_mapvi_cmd.phys_id);
+	its_encode_collection(cmd, desc->its_mapvi_cmd.dev->collection->col_id);
+
+	its_fixup_cmd(cmd);
+
+	return desc->its_mapvi_cmd.dev->collection;
+}
+
+static struct its_collection *its_build_movi_cmd(struct its_cmd_block *cmd,
+						 struct its_cmd_desc *desc)
+{
+	its_encode_cmd(cmd, GITS_CMD_MOVI);
+	its_encode_devid(cmd, desc->its_movi_cmd.dev->device_id);
+	its_encode_event_id(cmd, desc->its_movi_cmd.id);
+	its_encode_collection(cmd, desc->its_movi_cmd.col->col_id);
+
+	its_fixup_cmd(cmd);
+
+	return desc->its_movi_cmd.dev->collection;
+}
+
+static struct its_collection *its_build_discard_cmd(struct its_cmd_block *cmd,
+						    struct its_cmd_desc *desc)
+{
+	its_encode_cmd(cmd, GITS_CMD_DISCARD);
+	its_encode_devid(cmd, desc->its_discard_cmd.dev->device_id);
+	its_encode_event_id(cmd, desc->its_discard_cmd.event_id);
+
+	its_fixup_cmd(cmd);
+
+	return desc->its_discard_cmd.dev->collection;
+}
+
+static struct its_collection *its_build_inv_cmd(struct its_cmd_block *cmd,
+						struct its_cmd_desc *desc)
+{
+	its_encode_cmd(cmd, GITS_CMD_INV);
+	its_encode_devid(cmd, desc->its_inv_cmd.dev->device_id);
+	its_encode_event_id(cmd, desc->its_inv_cmd.event_id);
+
+	its_fixup_cmd(cmd);
+
+	return desc->its_inv_cmd.dev->collection;
+}
+
+static struct its_collection *its_build_invall_cmd(struct its_cmd_block *cmd,
+						   struct its_cmd_desc *desc)
+{
+	its_encode_cmd(cmd, GITS_CMD_INVALL);
+	its_encode_collection(cmd, desc->its_mapc_cmd.col->col_id);
+
+	its_fixup_cmd(cmd);
+
+	return NULL;
+}
+
+static u64 its_cmd_ptr_to_offset(struct its_node *its,
+				 struct its_cmd_block *ptr)
+{
+	return (ptr - its->cmd_base) * sizeof(*ptr);
+}
+
+static int its_queue_full(struct its_node *its)
+{
+	int widx;
+	int ridx;
+
+	widx = its->cmd_write - its->cmd_base;
+	ridx = readl_relaxed(its->base + GITS_CREADR) / sizeof(struct its_cmd_block);
+
+	/* This is incredibly unlikely to happen, unless the ITS locks up. */
+	if (((widx + 1) % ITS_CMD_QUEUE_NR_ENTRIES) == ridx)
+		return 1;
+
+	return 0;
+}
+
+static struct its_cmd_block *its_allocate_entry(struct its_node *its)
+{
+	struct its_cmd_block *cmd;
+	u32 count = 1000000;	/* 1s! */
+
+	while (its_queue_full(its)) {
+		count--;
+		if (!count) {
+			pr_err_ratelimited("ITS queue not draining\n");
+			return NULL;
+		}
+		cpu_relax();
+		udelay(1);
+	}
+
+	cmd = its->cmd_write++;
+
+	/* Handle queue wrapping */
+	if (its->cmd_write == (its->cmd_base + ITS_CMD_QUEUE_NR_ENTRIES))
+		its->cmd_write = its->cmd_base;
+
+	return cmd;
+}
+
+static struct its_cmd_block *its_post_commands(struct its_node *its)
+{
+	u64 wr = its_cmd_ptr_to_offset(its, its->cmd_write);
+
+	writel_relaxed(wr, its->base + GITS_CWRITER);
+
+	return its->cmd_write;
+}
+
+static void its_flush_cmd(struct its_node *its, struct its_cmd_block *cmd)
+{
+	/*
+	 * Make sure the commands written to memory are observable by
+	 * the ITS.
+	 */
+	if (its->flags & ITS_FLAGS_CMDQ_NEEDS_FLUSHING)
+		__flush_dcache_area(cmd, sizeof(*cmd));
+	else
+		dsb(ishst);
+}
+
+static void its_wait_for_range_completion(struct its_node *its,
+					  struct its_cmd_block *from,
+					  struct its_cmd_block *to)
+{
+	u64 rd_idx, from_idx, to_idx;
+	u32 count = 1000000;	/* 1s! */
+
+	from_idx = its_cmd_ptr_to_offset(its, from);
+	to_idx = its_cmd_ptr_to_offset(its, to);
+
+	while (1) {
+		rd_idx = readl_relaxed(its->base + GITS_CREADR);
+		if (rd_idx >= to_idx || rd_idx < from_idx)
+			break;
+
+		count--;
+		if (!count) {
+			pr_err_ratelimited("ITS queue timeout\n");
+			return;
+		}
+		cpu_relax();
+		udelay(1);
+	}
+}
+
+static void its_send_single_command(struct its_node *its,
+				    its_cmd_builder_t builder,
+				    struct its_cmd_desc *desc)
+{
+	struct its_cmd_block *cmd, *sync_cmd, *next_cmd;
+	struct its_collection *sync_col;
+
+	raw_spin_lock(&its->lock);
+
+	cmd = its_allocate_entry(its);
+	if (!cmd) {		/* We're soooooo screewed... */
+		pr_err_ratelimited("ITS can't allocate, dropping command\n");
+		raw_spin_unlock(&its->lock);
+		return;
+	}
+	sync_col = builder(cmd, desc);
+	its_flush_cmd(its, cmd);
+
+	if (sync_col) {
+		sync_cmd = its_allocate_entry(its);
+		if (!sync_cmd) {
+			pr_err_ratelimited("ITS can't SYNC, skipping\n");
+			goto post;
+		}
+		its_encode_cmd(sync_cmd, GITS_CMD_SYNC);
+		its_encode_target(sync_cmd, sync_col->target_address);
+		its_fixup_cmd(sync_cmd);
+		its_flush_cmd(its, sync_cmd);
+	}
+
+post:
+	next_cmd = its_post_commands(its);
+	raw_spin_unlock(&its->lock);
+
+	its_wait_for_range_completion(its, cmd, next_cmd);
+}
+
+static void its_send_inv(struct its_device *dev, u32 event_id)
+{
+	struct its_cmd_desc desc;
+
+	desc.its_inv_cmd.dev = dev;
+	desc.its_inv_cmd.event_id = event_id;
+
+	its_send_single_command(dev->its, its_build_inv_cmd, &desc);
+}
+
+static void its_send_mapd(struct its_device *dev, int valid)
+{
+	struct its_cmd_desc desc;
+
+	desc.its_mapd_cmd.dev = dev;
+	desc.its_mapd_cmd.valid = !!valid;
+
+	its_send_single_command(dev->its, its_build_mapd_cmd, &desc);
+}
+
+static void its_send_mapc(struct its_node *its, struct its_collection *col,
+			  int valid)
+{
+	struct its_cmd_desc desc;
+
+	desc.its_mapc_cmd.col = col;
+	desc.its_mapc_cmd.valid = !!valid;
+
+	its_send_single_command(its, its_build_mapc_cmd, &desc);
+}
+
+static void its_send_mapvi(struct its_device *dev, u32 irq_id, u32 id)
+{
+	struct its_cmd_desc desc;
+
+	desc.its_mapvi_cmd.dev = dev;
+	desc.its_mapvi_cmd.phys_id = irq_id;
+	desc.its_mapvi_cmd.event_id = id;
+
+	its_send_single_command(dev->its, its_build_mapvi_cmd, &desc);
+}
+
+static void its_send_movi(struct its_device *dev,
+			  struct its_collection *col, u32 id)
+{
+	struct its_cmd_desc desc;
+
+	desc.its_movi_cmd.dev = dev;
+	desc.its_movi_cmd.col = col;
+	desc.its_movi_cmd.id = id;
+
+	its_send_single_command(dev->its, its_build_movi_cmd, &desc);
+}
+
+static void its_send_discard(struct its_device *dev, u32 id)
+{
+	struct its_cmd_desc desc;
+
+	desc.its_discard_cmd.dev = dev;
+	desc.its_discard_cmd.event_id = id;
+
+	its_send_single_command(dev->its, its_build_discard_cmd, &desc);
+}
+
+static void its_send_invall(struct its_node *its, struct its_collection *col)
+{
+	struct its_cmd_desc desc;
+
+	desc.its_invall_cmd.col = col;
+
+	its_send_single_command(its, its_build_invall_cmd, &desc);
+}
+
+/*
+ * irqchip functions - assumes MSI, mostly.
+ */
+
+static inline u32 its_get_event_id(struct irq_data *d)
+{
+	struct its_device *its_dev = irq_data_get_irq_chip_data(d);
+	return d->hwirq - its_dev->lpi_base;
+}
+
+static void lpi_set_config(struct irq_data *d, bool enable)
+{
+	struct its_device *its_dev = irq_data_get_irq_chip_data(d);
+	irq_hw_number_t hwirq = d->hwirq;
+	u32 id = its_get_event_id(d);
+	u8 *cfg = page_address(gic_rdists->prop_page) + hwirq - 8192;
+
+	if (enable)
+		*cfg |= LPI_PROP_ENABLED;
+	else
+		*cfg &= ~LPI_PROP_ENABLED;
+
+	/*
+	 * Make the above write visible to the redistributors.
+	 * And yes, we're flushing exactly: One. Single. Byte.
+	 * Humpf...
+	 */
+	if (gic_rdists->flags & RDIST_FLAGS_PROPBASE_NEEDS_FLUSHING)
+		__flush_dcache_area(cfg, sizeof(*cfg));
+	else
+		dsb(ishst);
+	its_send_inv(its_dev, id);
+}
+
+static void its_mask_irq(struct irq_data *d)
+{
+	lpi_set_config(d, false);
+}
+
+static void its_unmask_irq(struct irq_data *d)
+{
+	lpi_set_config(d, true);
+}
+
+static void its_eoi_irq(struct irq_data *d)
+{
+	gic_write_eoir(d->hwirq);
+}
+
+static int its_set_affinity(struct irq_data *d, const struct cpumask *mask_val,
+			    bool force)
+{
+	unsigned int cpu = cpumask_any_and(mask_val, cpu_online_mask);
+	struct its_device *its_dev = irq_data_get_irq_chip_data(d);
+	struct its_collection *target_col;
+	u32 id = its_get_event_id(d);
+
+	if (cpu >= nr_cpu_ids)
+		return -EINVAL;
+
+	target_col = &its_dev->its->collections[cpu];
+	its_send_movi(its_dev, target_col, id);
+	its_dev->collection = target_col;
+
+	return IRQ_SET_MASK_OK_DONE;
+}
+
+static void its_irq_compose_msi_msg(struct irq_data *d, struct msi_msg *msg)
+{
+	struct its_device *its_dev = irq_data_get_irq_chip_data(d);
+	struct its_node *its;
+	u64 addr;
+
+	its = its_dev->its;
+	addr = its->phys_base + GITS_TRANSLATER;
+
+	msg->address_lo		= addr & ((1UL << 32) - 1);
+	msg->address_hi		= addr >> 32;
+	msg->data		= its_get_event_id(d);
+}
+
+static struct irq_chip its_irq_chip = {
+	.name			= "ITS",
+	.irq_mask		= its_mask_irq,
+	.irq_unmask		= its_unmask_irq,
+	.irq_eoi		= its_eoi_irq,
+	.irq_set_affinity	= its_set_affinity,
+	.irq_compose_msi_msg	= its_irq_compose_msi_msg,
+};
+
+static void its_mask_msi_irq(struct irq_data *d)
+{
+	pci_msi_mask_irq(d);
+	irq_chip_mask_parent(d);
+}
+
+static void its_unmask_msi_irq(struct irq_data *d)
+{
+	pci_msi_unmask_irq(d);
+	irq_chip_unmask_parent(d);
+}
+
+static struct irq_chip its_msi_irq_chip = {
+	.name			= "ITS-MSI",
+	.irq_unmask		= its_unmask_msi_irq,
+	.irq_mask		= its_mask_msi_irq,
+	.irq_eoi		= irq_chip_eoi_parent,
+	.irq_write_msi_msg	= pci_msi_domain_write_msg,
+};
+
+/*
+ * How we allocate LPIs:
+ *
+ * The GIC has id_bits bits for interrupt identifiers. From there, we
+ * must subtract 8192 which are reserved for SGIs/PPIs/SPIs. Then, as
+ * we allocate LPIs by chunks of 32, we can shift the whole thing by 5
+ * bits to the right.
+ *
+ * This gives us (((1UL << id_bits) - 8192) >> 5) possible allocations.
+ */
+#define IRQS_PER_CHUNK_SHIFT	5
+#define IRQS_PER_CHUNK		(1 << IRQS_PER_CHUNK_SHIFT)
+
+static unsigned long *lpi_bitmap;
+static u32 lpi_chunks;
+static DEFINE_SPINLOCK(lpi_lock);
+
+static int its_lpi_to_chunk(int lpi)
+{
+	return (lpi - 8192) >> IRQS_PER_CHUNK_SHIFT;
+}
+
+static int its_chunk_to_lpi(int chunk)
+{
+	return (chunk << IRQS_PER_CHUNK_SHIFT) + 8192;
+}
+
+static int its_lpi_init(u32 id_bits)
+{
+	lpi_chunks = its_lpi_to_chunk(1UL << id_bits);
+
+	lpi_bitmap = kzalloc(BITS_TO_LONGS(lpi_chunks) * sizeof(long),
+			     GFP_KERNEL);
+	if (!lpi_bitmap) {
+		lpi_chunks = 0;
+		return -ENOMEM;
+	}
+
+	pr_info("ITS: Allocated %d chunks for LPIs\n", (int)lpi_chunks);
+	return 0;
+}
+
+static unsigned long *its_lpi_alloc_chunks(int nr_irqs, int *base, int *nr_ids)
+{
+	unsigned long *bitmap = NULL;
+	int chunk_id;
+	int nr_chunks;
+	int i;
+
+	nr_chunks = DIV_ROUND_UP(nr_irqs, IRQS_PER_CHUNK);
+
+	spin_lock(&lpi_lock);
+
+	do {
+		chunk_id = bitmap_find_next_zero_area(lpi_bitmap, lpi_chunks,
+						      0, nr_chunks, 0);
+		if (chunk_id < lpi_chunks)
+			break;
+
+		nr_chunks--;
+	} while (nr_chunks > 0);
+
+	if (!nr_chunks)
+		goto out;
+
+	bitmap = kzalloc(BITS_TO_LONGS(nr_chunks * IRQS_PER_CHUNK) * sizeof (long),
+			 GFP_ATOMIC);
+	if (!bitmap)
+		goto out;
+
+	for (i = 0; i < nr_chunks; i++)
+		set_bit(chunk_id + i, lpi_bitmap);
+
+	*base = its_chunk_to_lpi(chunk_id);
+	*nr_ids = nr_chunks * IRQS_PER_CHUNK;
+
+out:
+	spin_unlock(&lpi_lock);
+
+	return bitmap;
+}
+
+static void its_lpi_free(unsigned long *bitmap, int base, int nr_ids)
+{
+	int lpi;
+
+	spin_lock(&lpi_lock);
+
+	for (lpi = base; lpi < (base + nr_ids); lpi += IRQS_PER_CHUNK) {
+		int chunk = its_lpi_to_chunk(lpi);
+		BUG_ON(chunk > lpi_chunks);
+		if (test_bit(chunk, lpi_bitmap)) {
+			clear_bit(chunk, lpi_bitmap);
+		} else {
+			pr_err("Bad LPI chunk %d\n", chunk);
+		}
+	}
+
+	spin_unlock(&lpi_lock);
+
+	kfree(bitmap);
+}
+
+/*
+ * We allocate 64kB for PROPBASE. That gives us at most 64K LPIs to
+ * deal with (one configuration byte per interrupt). PENDBASE has to
+ * be 64kB aligned (one bit per LPI, plus 8192 bits for SPI/PPI/SGI).
+ */
+#define LPI_PROPBASE_SZ		SZ_64K
+#define LPI_PENDBASE_SZ		(LPI_PROPBASE_SZ / 8 + SZ_1K)
+
+/*
+ * This is how many bits of ID we need, including the useless ones.
+ */
+#define LPI_NRBITS		ilog2(LPI_PROPBASE_SZ + SZ_8K)
+
+#define LPI_PROP_DEFAULT_PRIO	0xa0
+
+static int __init its_alloc_lpi_tables(void)
+{
+	phys_addr_t paddr;
+
+	gic_rdists->prop_page = alloc_pages(GFP_NOWAIT,
+					   get_order(LPI_PROPBASE_SZ));
+	if (!gic_rdists->prop_page) {
+		pr_err("Failed to allocate PROPBASE\n");
+		return -ENOMEM;
+	}
+
+	paddr = page_to_phys(gic_rdists->prop_page);
+	pr_info("GIC: using LPI property table @%pa\n", &paddr);
+
+	/* Priority 0xa0, Group-1, disabled */
+	memset(page_address(gic_rdists->prop_page),
+	       LPI_PROP_DEFAULT_PRIO | LPI_PROP_GROUP1,
+	       LPI_PROPBASE_SZ);
+
+	/* Make sure the GIC will observe the written configuration */
+	__flush_dcache_area(page_address(gic_rdists->prop_page), LPI_PROPBASE_SZ);
+
+	return 0;
+}
+
+static const char *its_base_type_string[] = {
+	[GITS_BASER_TYPE_DEVICE]	= "Devices",
+	[GITS_BASER_TYPE_VCPU]		= "Virtual CPUs",
+	[GITS_BASER_TYPE_CPU]		= "Physical CPUs",
+	[GITS_BASER_TYPE_COLLECTION]	= "Interrupt Collections",
+	[GITS_BASER_TYPE_RESERVED5] 	= "Reserved (5)",
+	[GITS_BASER_TYPE_RESERVED6] 	= "Reserved (6)",
+	[GITS_BASER_TYPE_RESERVED7] 	= "Reserved (7)",
+};
+
+static void its_free_tables(struct its_node *its)
+{
+	int i;
+
+	for (i = 0; i < GITS_BASER_NR_REGS; i++) {
+		if (its->tables[i]) {
+			free_page((unsigned long)its->tables[i]);
+			its->tables[i] = NULL;
+		}
+	}
+}
+
+static int its_alloc_tables(struct its_node *its)
+{
+	int err;
+	int i;
+	int psz = PAGE_SIZE;
+	u64 shr = GITS_BASER_InnerShareable;
+
+	for (i = 0; i < GITS_BASER_NR_REGS; i++) {
+		u64 val = readq_relaxed(its->base + GITS_BASER + i * 8);
+		u64 type = GITS_BASER_TYPE(val);
+		u64 entry_size = GITS_BASER_ENTRY_SIZE(val);
+		u64 tmp;
+		void *base;
+
+		if (type == GITS_BASER_TYPE_NONE)
+			continue;
+
+		/* We're lazy and only allocate a single page for now */
+		base = (void *)get_zeroed_page(GFP_KERNEL);
+		if (!base) {
+			err = -ENOMEM;
+			goto out_free;
+		}
+
+		its->tables[i] = base;
+
+retry_baser:
+		val = (virt_to_phys(base) 				 |
+		       (type << GITS_BASER_TYPE_SHIFT)			 |
+		       ((entry_size - 1) << GITS_BASER_ENTRY_SIZE_SHIFT) |
+		       GITS_BASER_WaWb					 |
+		       shr						 |
+		       GITS_BASER_VALID);
+
+		switch (psz) {
+		case SZ_4K:
+			val |= GITS_BASER_PAGE_SIZE_4K;
+			break;
+		case SZ_16K:
+			val |= GITS_BASER_PAGE_SIZE_16K;
+			break;
+		case SZ_64K:
+			val |= GITS_BASER_PAGE_SIZE_64K;
+			break;
+		}
+
+		val |= (PAGE_SIZE / psz) - 1;
+
+		writeq_relaxed(val, its->base + GITS_BASER + i * 8);
+		tmp = readq_relaxed(its->base + GITS_BASER + i * 8);
+
+		if ((val ^ tmp) & GITS_BASER_SHAREABILITY_MASK) {
+			/*
+			 * Shareability didn't stick. Just use
+			 * whatever the read reported, which is likely
+			 * to be the only thing this redistributor
+			 * supports.
+			 */
+			shr = tmp & GITS_BASER_SHAREABILITY_MASK;
+			goto retry_baser;
+		}
+
+		if ((val ^ tmp) & GITS_BASER_PAGE_SIZE_MASK) {
+			/*
+			 * Page size didn't stick. Let's try a smaller
+			 * size and retry. If we reach 4K, then
+			 * something is horribly wrong...
+			 */
+			switch (psz) {
+			case SZ_16K:
+				psz = SZ_4K;
+				goto retry_baser;
+			case SZ_64K:
+				psz = SZ_16K;
+				goto retry_baser;
+			}
+		}
+
+		if (val != tmp) {
+			pr_err("ITS: %s: GITS_BASER%d doesn't stick: %lx %lx\n",
+			       its->msi_chip.of_node->full_name, i,
+			       (unsigned long) val, (unsigned long) tmp);
+			err = -ENXIO;
+			goto out_free;
+		}
+
+		pr_info("ITS: allocated %d %s @%lx (psz %dK, shr %d)\n",
+			(int)(PAGE_SIZE / entry_size),
+			its_base_type_string[type],
+			(unsigned long)virt_to_phys(base),
+			psz / SZ_1K, (int)shr >> GITS_BASER_SHAREABILITY_SHIFT);
+	}
+
+	return 0;
+
+out_free:
+	its_free_tables(its);
+
+	return err;
+}
+
+static int its_alloc_collections(struct its_node *its)
+{
+	its->collections = kzalloc(nr_cpu_ids * sizeof(*its->collections),
+				   GFP_KERNEL);
+	if (!its->collections)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void its_cpu_init_lpis(void)
+{
+	void __iomem *rbase = gic_data_rdist_rd_base();
+	struct page *pend_page;
+	u64 val, tmp;
+
+	/* If we didn't allocate the pending table yet, do it now */
+	pend_page = gic_data_rdist()->pend_page;
+	if (!pend_page) {
+		phys_addr_t paddr;
+		/*
+		 * The pending pages have to be at least 64kB aligned,
+		 * hence the 'max(LPI_PENDBASE_SZ, SZ_64K)' below.
+		 */
+		pend_page = alloc_pages(GFP_NOWAIT | __GFP_ZERO,
+					get_order(max(LPI_PENDBASE_SZ, SZ_64K)));
+		if (!pend_page) {
+			pr_err("Failed to allocate PENDBASE for CPU%d\n",
+			       smp_processor_id());
+			return;
+		}
+
+		/* Make sure the GIC will observe the zero-ed page */
+		__flush_dcache_area(page_address(pend_page), LPI_PENDBASE_SZ);
+
+		paddr = page_to_phys(pend_page);
+		pr_info("CPU%d: using LPI pending table @%pa\n",
+			smp_processor_id(), &paddr);
+		gic_data_rdist()->pend_page = pend_page;
+	}
+
+	/* Disable LPIs */
+	val = readl_relaxed(rbase + GICR_CTLR);
+	val &= ~GICR_CTLR_ENABLE_LPIS;
+	writel_relaxed(val, rbase + GICR_CTLR);
+
+	/*
+	 * Make sure any change to the table is observable by the GIC.
+	 */
+	dsb(sy);
+
+	/* set PROPBASE */
+	val = (page_to_phys(gic_rdists->prop_page) |
+	       GICR_PROPBASER_InnerShareable |
+	       GICR_PROPBASER_WaWb |
+	       ((LPI_NRBITS - 1) & GICR_PROPBASER_IDBITS_MASK));
+
+	writeq_relaxed(val, rbase + GICR_PROPBASER);
+	tmp = readq_relaxed(rbase + GICR_PROPBASER);
+
+	if ((tmp ^ val) & GICR_PROPBASER_SHAREABILITY_MASK) {
+		pr_info_once("GIC: using cache flushing for LPI property table\n");
+		gic_rdists->flags |= RDIST_FLAGS_PROPBASE_NEEDS_FLUSHING;
+	}
+
+	/* set PENDBASE */
+	val = (page_to_phys(pend_page) |
+	       GICR_PROPBASER_InnerShareable |
+	       GICR_PROPBASER_WaWb);
+
+	writeq_relaxed(val, rbase + GICR_PENDBASER);
+
+	/* Enable LPIs */
+	val = readl_relaxed(rbase + GICR_CTLR);
+	val |= GICR_CTLR_ENABLE_LPIS;
+	writel_relaxed(val, rbase + GICR_CTLR);
+
+	/* Make sure the GIC has seen the above */
+	dsb(sy);
+}
+
+static void its_cpu_init_collection(void)
+{
+	struct its_node *its;
+	int cpu;
+
+	spin_lock(&its_lock);
+	cpu = smp_processor_id();
+
+	list_for_each_entry(its, &its_nodes, entry) {
+		u64 target;
+
+		/*
+		 * We now have to bind each collection to its target
+		 * redistributor.
+		 */
+		if (readq_relaxed(its->base + GITS_TYPER) & GITS_TYPER_PTA) {
+			/*
+			 * This ITS wants the physical address of the
+			 * redistributor.
+			 */
+			target = gic_data_rdist()->phys_base;
+		} else {
+			/*
+			 * This ITS wants a linear CPU number.
+			 */
+			target = readq_relaxed(gic_data_rdist_rd_base() + GICR_TYPER);
+			target = GICR_TYPER_CPU_NUMBER(target);
+		}
+
+		/* Perform collection mapping */
+		its->collections[cpu].target_address = target;
+		its->collections[cpu].col_id = cpu;
+
+		its_send_mapc(its, &its->collections[cpu], 1);
+		its_send_invall(its, &its->collections[cpu]);
+	}
+
+	spin_unlock(&its_lock);
+}
+
+static struct its_device *its_find_device(struct its_node *its, u32 dev_id)
+{
+	struct its_device *its_dev = NULL, *tmp;
+
+	raw_spin_lock(&its->lock);
+
+	list_for_each_entry(tmp, &its->its_device_list, entry) {
+		if (tmp->device_id == dev_id) {
+			its_dev = tmp;
+			break;
+		}
+	}
+
+	raw_spin_unlock(&its->lock);
+
+	return its_dev;
+}
+
+static struct its_device *its_create_device(struct its_node *its, u32 dev_id,
+					    int nvecs)
+{
+	struct its_device *dev;
+	unsigned long *lpi_map;
+	void *itt;
+	int lpi_base;
+	int nr_lpis;
+	int nr_ites;
+	int cpu;
+	int sz;
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	/*
+	 * At least one bit of EventID is being used, hence a minimum
+	 * of two entries. No, the architecture doesn't let you
+	 * express an ITT with a single entry.
+	 */
+	nr_ites = max(2, roundup_pow_of_two(nvecs));
+	sz = nr_ites * its->ite_size;
+	sz = max(sz, ITS_ITT_ALIGN) + ITS_ITT_ALIGN - 1;
+	itt = kmalloc(sz, GFP_KERNEL);
+	lpi_map = its_lpi_alloc_chunks(nvecs, &lpi_base, &nr_lpis);
+
+	if (!dev || !itt || !lpi_map) {
+		kfree(dev);
+		kfree(itt);
+		kfree(lpi_map);
+		return NULL;
+	}
+
+	dev->its = its;
+	dev->itt = itt;
+	dev->nr_ites = nr_ites;
+	dev->lpi_map = lpi_map;
+	dev->lpi_base = lpi_base;
+	dev->nr_lpis = nr_lpis;
+	dev->device_id = dev_id;
+	INIT_LIST_HEAD(&dev->entry);
+
+	raw_spin_lock(&its->lock);
+	list_add(&dev->entry, &its->its_device_list);
+	raw_spin_unlock(&its->lock);
+
+	/* Bind the device to the first possible CPU */
+	cpu = cpumask_first(cpu_online_mask);
+	dev->collection = &its->collections[cpu];
+
+	/* Map device to its ITT */
+	its_send_mapd(dev, 1);
+
+	return dev;
+}
+
+static void its_free_device(struct its_device *its_dev)
+{
+	raw_spin_lock(&its_dev->its->lock);
+	list_del(&its_dev->entry);
+	raw_spin_unlock(&its_dev->its->lock);
+	kfree(its_dev->itt);
+	kfree(its_dev);
+}
+
+static int its_alloc_device_irq(struct its_device *dev, irq_hw_number_t *hwirq)
+{
+	int idx;
+
+	idx = find_first_zero_bit(dev->lpi_map, dev->nr_lpis);
+	if (idx == dev->nr_lpis)
+		return -ENOSPC;
+
+	*hwirq = dev->lpi_base + idx;
+	set_bit(idx, dev->lpi_map);
+
+	return 0;
+}
+
+static int its_msi_prepare(struct irq_domain *domain, struct device *dev,
+			   int nvec, msi_alloc_info_t *info)
+{
+	struct pci_dev *pdev;
+	struct its_node *its;
+	u32 dev_id;
+	struct its_device *its_dev;
+
+	if (!dev_is_pci(dev))
+		return -EINVAL;
+
+	pdev = to_pci_dev(dev);
+	dev_id = PCI_DEVID(pdev->bus->number, pdev->devfn);
+	its = domain->parent->host_data;
+
+	its_dev = its_find_device(its, dev_id);
+	if (WARN_ON(its_dev))
+		return -EINVAL;
+
+	its_dev = its_create_device(its, dev_id, nvec);
+	if (!its_dev)
+		return -ENOMEM;
+
+	dev_dbg(&pdev->dev, "ITT %d entries, %d bits\n", nvec, ilog2(nvec));
+
+	info->scratchpad[0].ptr = its_dev;
+	info->scratchpad[1].ptr = dev;
+	return 0;
+}
+
+static struct msi_domain_ops its_pci_msi_ops = {
+	.msi_prepare	= its_msi_prepare,
+};
+
+static struct msi_domain_info its_pci_msi_domain_info = {
+	.flags	= (MSI_FLAG_USE_DEF_DOM_OPS | MSI_FLAG_USE_DEF_CHIP_OPS |
+		   MSI_FLAG_MULTI_PCI_MSI | MSI_FLAG_PCI_MSIX),
+	.ops	= &its_pci_msi_ops,
+	.chip	= &its_msi_irq_chip,
+};
+
+static int its_irq_gic_domain_alloc(struct irq_domain *domain,
+				    unsigned int virq,
+				    irq_hw_number_t hwirq)
+{
+	struct of_phandle_args args;
+
+	args.np = domain->parent->of_node;
+	args.args_count = 3;
+	args.args[0] = GIC_IRQ_TYPE_LPI;
+	args.args[1] = hwirq;
+	args.args[2] = IRQ_TYPE_EDGE_RISING;
+
+	return irq_domain_alloc_irqs_parent(domain, virq, 1, &args);
+}
+
+static int its_irq_domain_alloc(struct irq_domain *domain, unsigned int virq,
+				unsigned int nr_irqs, void *args)
+{
+	msi_alloc_info_t *info = args;
+	struct its_device *its_dev = info->scratchpad[0].ptr;
+	irq_hw_number_t hwirq;
+	int err;
+	int i;
+
+	for (i = 0; i < nr_irqs; i++) {
+		err = its_alloc_device_irq(its_dev, &hwirq);
+		if (err)
+			return err;
+
+		err = its_irq_gic_domain_alloc(domain, virq + i, hwirq);
+		if (err)
+			return err;
+
+		irq_domain_set_hwirq_and_chip(domain, virq + i,
+					      hwirq, &its_irq_chip, its_dev);
+		dev_dbg(info->scratchpad[1].ptr, "ID:%d pID:%d vID:%d\n",
+			(int)(hwirq - its_dev->lpi_base), (int)hwirq, virq + i);
+	}
+
+	return 0;
+}
+
+static void its_irq_domain_activate(struct irq_domain *domain,
+				    struct irq_data *d)
+{
+	struct its_device *its_dev = irq_data_get_irq_chip_data(d);
+	u32 event = its_get_event_id(d);
+
+	/* Map the GIC IRQ and event to the device */
+	its_send_mapvi(its_dev, d->hwirq, event);
+}
+
+static void its_irq_domain_deactivate(struct irq_domain *domain,
+				      struct irq_data *d)
+{
+	struct its_device *its_dev = irq_data_get_irq_chip_data(d);
+	u32 event = its_get_event_id(d);
+
+	/* Stop the delivery of interrupts */
+	its_send_discard(its_dev, event);
+}
+
+static void its_irq_domain_free(struct irq_domain *domain, unsigned int virq,
+				unsigned int nr_irqs)
+{
+	struct irq_data *d = irq_domain_get_irq_data(domain, virq);
+	struct its_device *its_dev = irq_data_get_irq_chip_data(d);
+	int i;
+
+	for (i = 0; i < nr_irqs; i++) {
+		struct irq_data *data = irq_domain_get_irq_data(domain,
+								virq + i);
+		u32 event = its_get_event_id(data);
+
+		/* Mark interrupt index as unused */
+		clear_bit(event, its_dev->lpi_map);
+
+		/* Nuke the entry in the domain */
+		irq_domain_reset_irq_data(data);
+	}
+
+	/* If all interrupts have been freed, start mopping the floor */
+	if (bitmap_empty(its_dev->lpi_map, its_dev->nr_lpis)) {
+		its_lpi_free(its_dev->lpi_map,
+			     its_dev->lpi_base,
+			     its_dev->nr_lpis);
+
+		/* Unmap device/itt */
+		its_send_mapd(its_dev, 0);
+		its_free_device(its_dev);
+	}
+
+	irq_domain_free_irqs_parent(domain, virq, nr_irqs);
+}
+
+static const struct irq_domain_ops its_domain_ops = {
+	.alloc			= its_irq_domain_alloc,
+	.free			= its_irq_domain_free,
+	.activate		= its_irq_domain_activate,
+	.deactivate		= its_irq_domain_deactivate,
+};
+
+static int its_probe(struct device_node *node, struct irq_domain *parent)
+{
+	struct resource res;
+	struct its_node *its;
+	void __iomem *its_base;
+	u32 val;
+	u64 baser, tmp;
+	int err;
+
+	err = of_address_to_resource(node, 0, &res);
+	if (err) {
+		pr_warn("%s: no regs?\n", node->full_name);
+		return -ENXIO;
+	}
+
+	its_base = ioremap(res.start, resource_size(&res));
+	if (!its_base) {
+		pr_warn("%s: unable to map registers\n", node->full_name);
+		return -ENOMEM;
+	}
+
+	val = readl_relaxed(its_base + GITS_PIDR2) & GIC_PIDR2_ARCH_MASK;
+	if (val != 0x30 && val != 0x40) {
+		pr_warn("%s: no ITS detected, giving up\n", node->full_name);
+		err = -ENODEV;
+		goto out_unmap;
+	}
+
+	pr_info("ITS: %s\n", node->full_name);
+
+	its = kzalloc(sizeof(*its), GFP_KERNEL);
+	if (!its) {
+		err = -ENOMEM;
+		goto out_unmap;
+	}
+
+	raw_spin_lock_init(&its->lock);
+	INIT_LIST_HEAD(&its->entry);
+	INIT_LIST_HEAD(&its->its_device_list);
+	its->base = its_base;
+	its->phys_base = res.start;
+	its->msi_chip.of_node = node;
+	its->ite_size = ((readl_relaxed(its_base + GITS_TYPER) >> 4) & 0xf) + 1;
+
+	its->cmd_base = kzalloc(ITS_CMD_QUEUE_SZ, GFP_KERNEL);
+	if (!its->cmd_base) {
+		err = -ENOMEM;
+		goto out_free_its;
+	}
+	its->cmd_write = its->cmd_base;
+
+	err = its_alloc_tables(its);
+	if (err)
+		goto out_free_cmd;
+
+	err = its_alloc_collections(its);
+	if (err)
+		goto out_free_tables;
+
+	baser = (virt_to_phys(its->cmd_base)	|
+		 GITS_CBASER_WaWb		|
+		 GITS_CBASER_InnerShareable	|
+		 (ITS_CMD_QUEUE_SZ / SZ_4K - 1)	|
+		 GITS_CBASER_VALID);
+
+	writeq_relaxed(baser, its->base + GITS_CBASER);
+	tmp = readq_relaxed(its->base + GITS_CBASER);
+	writeq_relaxed(0, its->base + GITS_CWRITER);
+	writel_relaxed(1, its->base + GITS_CTLR);
+
+	if ((tmp ^ baser) & GITS_BASER_SHAREABILITY_MASK) {
+		pr_info("ITS: using cache flushing for cmd queue\n");
+		its->flags |= ITS_FLAGS_CMDQ_NEEDS_FLUSHING;
+	}
+
+	if (of_property_read_bool(its->msi_chip.of_node, "msi-controller")) {
+		its->domain = irq_domain_add_tree(NULL, &its_domain_ops, its);
+		if (!its->domain) {
+			err = -ENOMEM;
+			goto out_free_tables;
+		}
+
+		its->domain->parent = parent;
+
+		its->msi_chip.domain = pci_msi_create_irq_domain(node,
+								 &its_pci_msi_domain_info,
+								 its->domain);
+		if (!its->msi_chip.domain) {
+			err = -ENOMEM;
+			goto out_free_domains;
+		}
+
+		err = of_pci_msi_chip_add(&its->msi_chip);
+		if (err)
+			goto out_free_domains;
+	}
+
+	spin_lock(&its_lock);
+	list_add(&its->entry, &its_nodes);
+	spin_unlock(&its_lock);
+
+	return 0;
+
+out_free_domains:
+	if (its->msi_chip.domain)
+		irq_domain_remove(its->msi_chip.domain);
+	if (its->domain)
+		irq_domain_remove(its->domain);
+out_free_tables:
+	its_free_tables(its);
+out_free_cmd:
+	kfree(its->cmd_base);
+out_free_its:
+	kfree(its);
+out_unmap:
+	iounmap(its_base);
+	pr_err("ITS: failed probing %s (%d)\n", node->full_name, err);
+	return err;
+}
+
+static bool gic_rdists_supports_plpis(void)
+{
+	return !!(readl_relaxed(gic_data_rdist_rd_base() + GICR_TYPER) & GICR_TYPER_PLPIS);
+}
+
+int its_cpu_init(void)
+{
+	if (!gic_rdists_supports_plpis()) {
+		pr_info("CPU%d: LPIs not supported\n", smp_processor_id());
+		return -ENXIO;
+	}
+
+	if (!list_empty(&its_nodes)) {
+		its_cpu_init_lpis();
+		its_cpu_init_collection();
+	}
+
+	return 0;
+}
+
+static struct of_device_id its_device_id[] = {
+	{	.compatible	= "arm,gic-v3-its",	},
+	{},
+};
+
+int its_init(struct device_node *node, struct rdists *rdists,
+	     struct irq_domain *parent_domain)
+{
+	struct device_node *np;
+
+	for (np = of_find_matching_node(node, its_device_id); np;
+	     np = of_find_matching_node(np, its_device_id)) {
+		its_probe(np, parent_domain);
+	}
+
+	if (list_empty(&its_nodes)) {
+		pr_warn("ITS: No ITS available, not enabling LPIs\n");
+		return -ENXIO;
+	}
+
+	gic_rdists = rdists;
+	gic_root_node = node;
+
+	its_alloc_lpi_tables();
+	its_lpi_init(rdists->id_bits);
+
+	return 0;
+}
diff --git a/drivers/irqchip/irq-gic-v3.c b/drivers/irqchip/irq-gic-v3.c
index aa17ae805a70..1a146ccee701 100644
--- a/drivers/irqchip/irq-gic-v3.c
+++ b/drivers/irqchip/irq-gic-v3.c
@@ -34,20 +34,25 @@
 #include "irq-gic-common.h"
 #include "irqchip.h"
 
+struct redist_region {
+	void __iomem		*redist_base;
+	phys_addr_t		phys_base;
+};
+
 struct gic_chip_data {
 	void __iomem		*dist_base;
-	void __iomem		**redist_base;
-	void __iomem * __percpu	*rdist;
+	struct redist_region	*redist_regions;
+	struct rdists		rdists;
 	struct irq_domain	*domain;
 	u64			redist_stride;
-	u32			redist_regions;
+	u32			nr_redist_regions;
 	unsigned int		irq_nr;
 };
 
 static struct gic_chip_data gic_data __read_mostly;
 
-#define gic_data_rdist()		(this_cpu_ptr(gic_data.rdist))
-#define gic_data_rdist_rd_base()	(*gic_data_rdist())
+#define gic_data_rdist()		(this_cpu_ptr(gic_data.rdists.rdist))
+#define gic_data_rdist_rd_base()	(gic_data_rdist()->rd_base)
 #define gic_data_rdist_sgi_base()	(gic_data_rdist_rd_base() + SZ_64K)
 
 /* Our default, arbitrary priority value. Linux only uses one anyway. */
@@ -71,9 +76,6 @@ static inline void __iomem *gic_dist_base(struct irq_data *d)
 	if (d->hwirq <= 1023)		/* SPI -> dist_base */
 		return gic_data.dist_base;
 
-	if (d->hwirq >= 8192)
-		BUG();		/* LPI Detected!!! */
-
 	return NULL;
 }
 
@@ -271,11 +273,11 @@ static asmlinkage void __exception_irq_entry gic_handle_irq(struct pt_regs *regs
 	do {
 		irqnr = gic_read_iar();
 
-		if (likely(irqnr > 15 && irqnr < 1020)) {
+		if (likely(irqnr > 15 && irqnr < 1020) || irqnr >= 8192) {
 			int err;
 			err = handle_domain_irq(gic_data.domain, irqnr, regs);
 			if (err) {
-				WARN_ONCE(true, "Unexpected SPI received!\n");
+				WARN_ONCE(true, "Unexpected interrupt received!\n");
 				gic_write_eoir(irqnr);
 			}
 			continue;
@@ -333,8 +335,8 @@ static int gic_populate_rdist(void)
 	       MPIDR_AFFINITY_LEVEL(mpidr, 1) << 8 |
 	       MPIDR_AFFINITY_LEVEL(mpidr, 0));
 
-	for (i = 0; i < gic_data.redist_regions; i++) {
-		void __iomem *ptr = gic_data.redist_base[i];
+	for (i = 0; i < gic_data.nr_redist_regions; i++) {
+		void __iomem *ptr = gic_data.redist_regions[i].redist_base;
 		u32 reg;
 
 		reg = readl_relaxed(ptr + GICR_PIDR2) & GIC_PIDR2_ARCH_MASK;
@@ -347,10 +349,13 @@ static int gic_populate_rdist(void)
 		do {
 			typer = readq_relaxed(ptr + GICR_TYPER);
 			if ((typer >> 32) == aff) {
+				u64 offset = ptr - gic_data.redist_regions[i].redist_base;
 				gic_data_rdist_rd_base() = ptr;
-				pr_info("CPU%d: found redistributor %llx @%p\n",
+				gic_data_rdist()->phys_base = gic_data.redist_regions[i].phys_base + offset;
+				pr_info("CPU%d: found redistributor %llx region %d:%pa\n",
 					smp_processor_id(),
-					(unsigned long long)mpidr, ptr);
+					(unsigned long long)mpidr,
+					i, &gic_data_rdist()->phys_base);
 				return 0;
 			}
 
@@ -385,6 +390,11 @@ static void gic_cpu_sys_reg_init(void)
 	gic_write_grpen1(1);
 }
 
+static int gic_dist_supports_lpis(void)
+{
+	return !!(readl_relaxed(gic_data.dist_base + GICD_TYPER) & GICD_TYPER_LPIS);
+}
+
 static void gic_cpu_init(void)
 {
 	void __iomem *rbase;
@@ -399,6 +409,10 @@ static void gic_cpu_init(void)
 
 	gic_cpu_config(rbase, gic_redist_wait_for_rwp);
 
+	/* Give LPIs a spin */
+	if (IS_ENABLED(CONFIG_ARM_GIC_V3_ITS) && gic_dist_supports_lpis())
+		its_cpu_init();
+
 	/* initialise system registers */
 	gic_cpu_sys_reg_init();
 }
@@ -585,26 +599,43 @@ static struct irq_chip gic_chip = {
 	.irq_set_affinity	= gic_set_affinity,
 };
 
+#define GIC_ID_NR		(1U << gic_data.rdists.id_bits)
+
 static int gic_irq_domain_map(struct irq_domain *d, unsigned int irq,
 			      irq_hw_number_t hw)
 {
 	/* SGIs are private to the core kernel */
 	if (hw < 16)
 		return -EPERM;
+	/* Nothing here */
+	if (hw >= gic_data.irq_nr && hw < 8192)
+		return -EPERM;
+	/* Off limits */
+	if (hw >= GIC_ID_NR)
+		return -EPERM;
+
 	/* PPIs */
 	if (hw < 32) {
 		irq_set_percpu_devid(irq);
-		irq_set_chip_and_handler(irq, &gic_chip,
-					 handle_percpu_devid_irq);
+		irq_domain_set_info(d, irq, hw, &gic_chip, d->host_data,
+				    handle_percpu_devid_irq, NULL, NULL);
 		set_irq_flags(irq, IRQF_VALID | IRQF_NOAUTOEN);
 	}
 	/* SPIs */
 	if (hw >= 32 && hw < gic_data.irq_nr) {
-		irq_set_chip_and_handler(irq, &gic_chip,
-					 handle_fasteoi_irq);
+		irq_domain_set_info(d, irq, hw, &gic_chip, d->host_data,
+				    handle_fasteoi_irq, NULL, NULL);
 		set_irq_flags(irq, IRQF_VALID | IRQF_PROBE);
 	}
-	irq_set_chip_data(irq, d->host_data);
+	/* LPIs */
+	if (hw >= 8192 && hw < GIC_ID_NR) {
+		if (!gic_dist_supports_lpis())
+			return -EPERM;
+		irq_domain_set_info(d, irq, hw, &gic_chip, d->host_data,
+				    handle_fasteoi_irq, NULL, NULL);
+		set_irq_flags(irq, IRQF_VALID);
+	}
+
 	return 0;
 }
 
@@ -625,6 +656,9 @@ static int gic_irq_domain_xlate(struct irq_domain *d,
 	case 1:			/* PPI */
 		*out_hwirq = intspec[1] + 16;
 		break;
+	case GIC_IRQ_TYPE_LPI:	/* LPI */
+		*out_hwirq = intspec[1];
+		break;
 	default:
 		return -EINVAL;
 	}
@@ -633,17 +667,50 @@ static int gic_irq_domain_xlate(struct irq_domain *d,
 	return 0;
 }
 
+static int gic_irq_domain_alloc(struct irq_domain *domain, unsigned int virq,
+				unsigned int nr_irqs, void *arg)
+{
+	int i, ret;
+	irq_hw_number_t hwirq;
+	unsigned int type = IRQ_TYPE_NONE;
+	struct of_phandle_args *irq_data = arg;
+
+	ret = gic_irq_domain_xlate(domain, irq_data->np, irq_data->args,
+				   irq_data->args_count, &hwirq, &type);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < nr_irqs; i++)
+		gic_irq_domain_map(domain, virq + i, hwirq + i);
+
+	return 0;
+}
+
+static void gic_irq_domain_free(struct irq_domain *domain, unsigned int virq,
+				unsigned int nr_irqs)
+{
+	int i;
+
+	for (i = 0; i < nr_irqs; i++) {
+		struct irq_data *d = irq_domain_get_irq_data(domain, virq + i);
+		irq_set_handler(virq + i, NULL);
+		irq_domain_reset_irq_data(d);
+	}
+}
+
 static const struct irq_domain_ops gic_irq_domain_ops = {
-	.map = gic_irq_domain_map,
 	.xlate = gic_irq_domain_xlate,
+	.alloc = gic_irq_domain_alloc,
+	.free = gic_irq_domain_free,
 };
 
 static int __init gic_of_init(struct device_node *node, struct device_node *parent)
 {
 	void __iomem *dist_base;
-	void __iomem **redist_base;
+	struct redist_region *rdist_regs;
 	u64 redist_stride;
-	u32 redist_regions;
+	u32 nr_redist_regions;
+	u32 typer;
 	u32 reg;
 	int gic_irqs;
 	int err;
@@ -664,54 +731,63 @@ static int __init gic_of_init(struct device_node *node, struct device_node *pare
 		goto out_unmap_dist;
 	}
 
-	if (of_property_read_u32(node, "#redistributor-regions", &redist_regions))
-		redist_regions = 1;
+	if (of_property_read_u32(node, "#redistributor-regions", &nr_redist_regions))
+		nr_redist_regions = 1;
 
-	redist_base = kzalloc(sizeof(*redist_base) * redist_regions, GFP_KERNEL);
-	if (!redist_base) {
+	rdist_regs = kzalloc(sizeof(*rdist_regs) * nr_redist_regions, GFP_KERNEL);
+	if (!rdist_regs) {
 		err = -ENOMEM;
 		goto out_unmap_dist;
 	}
 
-	for (i = 0; i < redist_regions; i++) {
-		redist_base[i] = of_iomap(node, 1 + i);
-		if (!redist_base[i]) {
+	for (i = 0; i < nr_redist_regions; i++) {
+		struct resource res;
+		int ret;
+
+		ret = of_address_to_resource(node, 1 + i, &res);
+		rdist_regs[i].redist_base = of_iomap(node, 1 + i);
+		if (ret || !rdist_regs[i].redist_base) {
 			pr_err("%s: couldn't map region %d\n",
 			       node->full_name, i);
 			err = -ENODEV;
 			goto out_unmap_rdist;
 		}
+		rdist_regs[i].phys_base = res.start;
 	}
 
 	if (of_property_read_u64(node, "redistributor-stride", &redist_stride))
 		redist_stride = 0;
 
 	gic_data.dist_base = dist_base;
-	gic_data.redist_base = redist_base;
-	gic_data.redist_regions = redist_regions;
+	gic_data.redist_regions = rdist_regs;
+	gic_data.nr_redist_regions = nr_redist_regions;
 	gic_data.redist_stride = redist_stride;
 
 	/*
 	 * Find out how many interrupts are supported.
 	 * The GIC only supports up to 1020 interrupt sources (SGI+PPI+SPI)
 	 */
-	gic_irqs = readl_relaxed(gic_data.dist_base + GICD_TYPER) & 0x1f;
-	gic_irqs = (gic_irqs + 1) * 32;
+	typer = readl_relaxed(gic_data.dist_base + GICD_TYPER);
+	gic_data.rdists.id_bits = GICD_TYPER_ID_BITS(typer);
+	gic_irqs = GICD_TYPER_IRQS(typer);
 	if (gic_irqs > 1020)
 		gic_irqs = 1020;
 	gic_data.irq_nr = gic_irqs;
 
 	gic_data.domain = irq_domain_add_tree(node, &gic_irq_domain_ops,
 					      &gic_data);
-	gic_data.rdist = alloc_percpu(typeof(*gic_data.rdist));
+	gic_data.rdists.rdist = alloc_percpu(typeof(*gic_data.rdists.rdist));
 
-	if (WARN_ON(!gic_data.domain) || WARN_ON(!gic_data.rdist)) {
+	if (WARN_ON(!gic_data.domain) || WARN_ON(!gic_data.rdists.rdist)) {
 		err = -ENOMEM;
 		goto out_free;
 	}
 
 	set_handle_irq(gic_handle_irq);
 
+	if (IS_ENABLED(CONFIG_ARM_GIC_V3_ITS) && gic_dist_supports_lpis())
+		its_init(node, &gic_data.rdists, gic_data.domain);
+
 	gic_smp_init();
 	gic_dist_init();
 	gic_cpu_init();
@@ -722,12 +798,12 @@ static int __init gic_of_init(struct device_node *node, struct device_node *pare
 out_free:
 	if (gic_data.domain)
 		irq_domain_remove(gic_data.domain);
-	free_percpu(gic_data.rdist);
+	free_percpu(gic_data.rdists.rdist);
 out_unmap_rdist:
-	for (i = 0; i < redist_regions; i++)
-		if (redist_base[i])
-			iounmap(redist_base[i]);
-	kfree(redist_base);
+	for (i = 0; i < nr_redist_regions; i++)
+		if (rdist_regs[i].redist_base)
+			iounmap(rdist_regs[i].redist_base);
+	kfree(rdist_regs);
 out_unmap_dist:
 	iounmap(dist_base);
 	return err;
diff --git a/drivers/irqchip/irq-gic.c b/drivers/irqchip/irq-gic.c
index 7f9be0785c6a..d617ee5a3d8a 100644
--- a/drivers/irqchip/irq-gic.c
+++ b/drivers/irqchip/irq-gic.c
@@ -788,17 +788,16 @@ static int gic_irq_domain_map(struct irq_domain *d, unsigned int irq,
 {
 	if (hw < 32) {
 		irq_set_percpu_devid(irq);
-		irq_set_chip_and_handler(irq, &gic_chip,
-					 handle_percpu_devid_irq);
+		irq_domain_set_info(d, irq, hw, &gic_chip, d->host_data,
+				    handle_percpu_devid_irq, NULL, NULL);
 		set_irq_flags(irq, IRQF_VALID | IRQF_NOAUTOEN);
 	} else {
-		irq_set_chip_and_handler(irq, &gic_chip,
-					 handle_fasteoi_irq);
+		irq_domain_set_info(d, irq, hw, &gic_chip, d->host_data,
+				    handle_fasteoi_irq, NULL, NULL);
 		set_irq_flags(irq, IRQF_VALID | IRQF_PROBE);
 
 		gic_routable_irq_domain_ops->map(d, irq, hw);
 	}
-	irq_set_chip_data(irq, d->host_data);
 	return 0;
 }
 
@@ -858,6 +857,31 @@ static struct notifier_block gic_cpu_notifier = {
 };
 #endif
 
+static int gic_irq_domain_alloc(struct irq_domain *domain, unsigned int virq,
+				unsigned int nr_irqs, void *arg)
+{
+	int i, ret;
+	irq_hw_number_t hwirq;
+	unsigned int type = IRQ_TYPE_NONE;
+	struct of_phandle_args *irq_data = arg;
+
+	ret = gic_irq_domain_xlate(domain, irq_data->np, irq_data->args,
+				   irq_data->args_count, &hwirq, &type);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < nr_irqs; i++)
+		gic_irq_domain_map(domain, virq + i, hwirq + i);
+
+	return 0;
+}
+
+static const struct irq_domain_ops gic_irq_domain_hierarchy_ops = {
+	.xlate = gic_irq_domain_xlate,
+	.alloc = gic_irq_domain_alloc,
+	.free = irq_domain_free_irqs_top,
+};
+
 static const struct irq_domain_ops gic_irq_domain_ops = {
 	.map = gic_irq_domain_map,
 	.unmap = gic_irq_domain_unmap,
@@ -948,18 +972,6 @@ void __init gic_init_bases(unsigned int gic_nr, int irq_start,
 		gic_cpu_map[i] = 0xff;
 
 	/*
-	 * For primary GICs, skip over SGIs.
-	 * For secondary GICs, skip over PPIs, too.
-	 */
-	if (gic_nr == 0 && (irq_start & 31) > 0) {
-		hwirq_base = 16;
-		if (irq_start != -1)
-			irq_start = (irq_start & ~31) + 16;
-	} else {
-		hwirq_base = 32;
-	}
-
-	/*
 	 * Find out how many interrupts are supported.
 	 * The GIC only supports up to 1020 interrupt sources.
 	 */
@@ -969,10 +981,31 @@ void __init gic_init_bases(unsigned int gic_nr, int irq_start,
 		gic_irqs = 1020;
 	gic->gic_irqs = gic_irqs;
 
-	gic_irqs -= hwirq_base; /* calculate # of irqs to allocate */
+	if (node) {		/* DT case */
+		const struct irq_domain_ops *ops = &gic_irq_domain_hierarchy_ops;
+
+		if (!of_property_read_u32(node, "arm,routable-irqs",
+					  &nr_routable_irqs)) {
+			ops = &gic_irq_domain_ops;
+			gic_irqs = nr_routable_irqs;
+		}
+
+		gic->domain = irq_domain_add_linear(node, gic_irqs, ops, gic);
+	} else {		/* Non-DT case */
+		/*
+		 * For primary GICs, skip over SGIs.
+		 * For secondary GICs, skip over PPIs, too.
+		 */
+		if (gic_nr == 0 && (irq_start & 31) > 0) {
+			hwirq_base = 16;
+			if (irq_start != -1)
+				irq_start = (irq_start & ~31) + 16;
+		} else {
+			hwirq_base = 32;
+		}
+
+		gic_irqs -= hwirq_base; /* calculate # of irqs to allocate */
 
-	if (of_property_read_u32(node, "arm,routable-irqs",
-				 &nr_routable_irqs)) {
 		irq_base = irq_alloc_descs(irq_start, 16, gic_irqs,
 					   numa_node_id());
 		if (IS_ERR_VALUE(irq_base)) {
@@ -983,10 +1016,6 @@ void __init gic_init_bases(unsigned int gic_nr, int irq_start,
 
 		gic->domain = irq_domain_add_legacy(node, gic_irqs, irq_base,
 					hwirq_base, &gic_irq_domain_ops, gic);
-	} else {
-		gic->domain = irq_domain_add_linear(node, nr_routable_irqs,
-						    &gic_irq_domain_ops,
-						    gic);
 	}
 
 	if (WARN_ON(!gic->domain))
@@ -1037,6 +1066,10 @@ gic_of_init(struct device_node *node, struct device_node *parent)
 		irq = irq_of_parse_and_map(node, 0);
 		gic_cascade_irq(gic_cnt, irq);
 	}
+
+	if (IS_ENABLED(CONFIG_ARM_GIC_V2M))
+		gicv2m_of_init(node, gic_data[gic_cnt].domain);
+
 	gic_cnt++;
 	return 0;
 }
diff --git a/drivers/irqchip/irq-keystone.c b/drivers/irqchip/irq-keystone.c
index 608abf9c9283..78e8b3ce5252 100644
--- a/drivers/irqchip/irq-keystone.c
+++ b/drivers/irqchip/irq-keystone.c
@@ -218,7 +218,6 @@ static struct platform_driver keystone_irq_device_driver = {
 	.remove		= keystone_irq_remove,
 	.driver		= {
 		.name	= "keystone_irq",
-		.owner	= THIS_MODULE,
 		.of_match_table	= of_match_ptr(keystone_irq_dt_ids),
 	}
 };
diff --git a/drivers/irqchip/irq-mips-gic.c b/drivers/irqchip/irq-mips-gic.c
new file mode 100644
index 000000000000..2b0468e3df6a
--- /dev/null
+++ b/drivers/irqchip/irq-mips-gic.c
@@ -0,0 +1,789 @@
+/*
+ * 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) 2008 Ralf Baechle (ralf@linux-mips.org)
+ * Copyright (C) 2012 MIPS Technologies, Inc.  All rights reserved.
+ */
+#include <linux/bitmap.h>
+#include <linux/clocksource.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/irqchip/mips-gic.h>
+#include <linux/of_address.h>
+#include <linux/sched.h>
+#include <linux/smp.h>
+
+#include <asm/mips-cm.h>
+#include <asm/setup.h>
+#include <asm/traps.h>
+
+#include <dt-bindings/interrupt-controller/mips-gic.h>
+
+#include "irqchip.h"
+
+unsigned int gic_present;
+
+struct gic_pcpu_mask {
+	DECLARE_BITMAP(pcpu_mask, GIC_MAX_INTRS);
+};
+
+static void __iomem *gic_base;
+static struct gic_pcpu_mask pcpu_masks[NR_CPUS];
+static DEFINE_SPINLOCK(gic_lock);
+static struct irq_domain *gic_irq_domain;
+static int gic_shared_intrs;
+static int gic_vpes;
+static unsigned int gic_cpu_pin;
+static struct irq_chip gic_level_irq_controller, gic_edge_irq_controller;
+
+static void __gic_irq_dispatch(void);
+
+static inline unsigned int gic_read(unsigned int reg)
+{
+	return __raw_readl(gic_base + reg);
+}
+
+static inline void gic_write(unsigned int reg, unsigned int val)
+{
+	__raw_writel(val, gic_base + reg);
+}
+
+static inline void gic_update_bits(unsigned int reg, unsigned int mask,
+				   unsigned int val)
+{
+	unsigned int regval;
+
+	regval = gic_read(reg);
+	regval &= ~mask;
+	regval |= val;
+	gic_write(reg, regval);
+}
+
+static inline void gic_reset_mask(unsigned int intr)
+{
+	gic_write(GIC_REG(SHARED, GIC_SH_RMASK) + GIC_INTR_OFS(intr),
+		  1 << GIC_INTR_BIT(intr));
+}
+
+static inline void gic_set_mask(unsigned int intr)
+{
+	gic_write(GIC_REG(SHARED, GIC_SH_SMASK) + GIC_INTR_OFS(intr),
+		  1 << GIC_INTR_BIT(intr));
+}
+
+static inline void gic_set_polarity(unsigned int intr, unsigned int pol)
+{
+	gic_update_bits(GIC_REG(SHARED, GIC_SH_SET_POLARITY) +
+			GIC_INTR_OFS(intr), 1 << GIC_INTR_BIT(intr),
+			pol << GIC_INTR_BIT(intr));
+}
+
+static inline void gic_set_trigger(unsigned int intr, unsigned int trig)
+{
+	gic_update_bits(GIC_REG(SHARED, GIC_SH_SET_TRIGGER) +
+			GIC_INTR_OFS(intr), 1 << GIC_INTR_BIT(intr),
+			trig << GIC_INTR_BIT(intr));
+}
+
+static inline void gic_set_dual_edge(unsigned int intr, unsigned int dual)
+{
+	gic_update_bits(GIC_REG(SHARED, GIC_SH_SET_DUAL) + GIC_INTR_OFS(intr),
+			1 << GIC_INTR_BIT(intr),
+			dual << GIC_INTR_BIT(intr));
+}
+
+static inline void gic_map_to_pin(unsigned int intr, unsigned int pin)
+{
+	gic_write(GIC_REG(SHARED, GIC_SH_INTR_MAP_TO_PIN_BASE) +
+		  GIC_SH_MAP_TO_PIN(intr), GIC_MAP_TO_PIN_MSK | pin);
+}
+
+static inline void gic_map_to_vpe(unsigned int intr, unsigned int vpe)
+{
+	gic_write(GIC_REG(SHARED, GIC_SH_INTR_MAP_TO_VPE_BASE) +
+		  GIC_SH_MAP_TO_VPE_REG_OFF(intr, vpe),
+		  GIC_SH_MAP_TO_VPE_REG_BIT(vpe));
+}
+
+#ifdef CONFIG_CLKSRC_MIPS_GIC
+cycle_t gic_read_count(void)
+{
+	unsigned int hi, hi2, lo;
+
+	do {
+		hi = gic_read(GIC_REG(SHARED, GIC_SH_COUNTER_63_32));
+		lo = gic_read(GIC_REG(SHARED, GIC_SH_COUNTER_31_00));
+		hi2 = gic_read(GIC_REG(SHARED, GIC_SH_COUNTER_63_32));
+	} while (hi2 != hi);
+
+	return (((cycle_t) hi) << 32) + lo;
+}
+
+unsigned int gic_get_count_width(void)
+{
+	unsigned int bits, config;
+
+	config = gic_read(GIC_REG(SHARED, GIC_SH_CONFIG));
+	bits = 32 + 4 * ((config & GIC_SH_CONFIG_COUNTBITS_MSK) >>
+			 GIC_SH_CONFIG_COUNTBITS_SHF);
+
+	return bits;
+}
+
+void gic_write_compare(cycle_t cnt)
+{
+	gic_write(GIC_REG(VPE_LOCAL, GIC_VPE_COMPARE_HI),
+				(int)(cnt >> 32));
+	gic_write(GIC_REG(VPE_LOCAL, GIC_VPE_COMPARE_LO),
+				(int)(cnt & 0xffffffff));
+}
+
+void gic_write_cpu_compare(cycle_t cnt, int cpu)
+{
+	unsigned long flags;
+
+	local_irq_save(flags);
+
+	gic_write(GIC_REG(VPE_LOCAL, GIC_VPE_OTHER_ADDR), cpu);
+	gic_write(GIC_REG(VPE_OTHER, GIC_VPE_COMPARE_HI),
+				(int)(cnt >> 32));
+	gic_write(GIC_REG(VPE_OTHER, GIC_VPE_COMPARE_LO),
+				(int)(cnt & 0xffffffff));
+
+	local_irq_restore(flags);
+}
+
+cycle_t gic_read_compare(void)
+{
+	unsigned int hi, lo;
+
+	hi = gic_read(GIC_REG(VPE_LOCAL, GIC_VPE_COMPARE_HI));
+	lo = gic_read(GIC_REG(VPE_LOCAL, GIC_VPE_COMPARE_LO));
+
+	return (((cycle_t) hi) << 32) + lo;
+}
+#endif
+
+static bool gic_local_irq_is_routable(int intr)
+{
+	u32 vpe_ctl;
+
+	/* All local interrupts are routable in EIC mode. */
+	if (cpu_has_veic)
+		return true;
+
+	vpe_ctl = gic_read(GIC_REG(VPE_LOCAL, GIC_VPE_CTL));
+	switch (intr) {
+	case GIC_LOCAL_INT_TIMER:
+		return vpe_ctl & GIC_VPE_CTL_TIMER_RTBL_MSK;
+	case GIC_LOCAL_INT_PERFCTR:
+		return vpe_ctl & GIC_VPE_CTL_PERFCNT_RTBL_MSK;
+	case GIC_LOCAL_INT_FDC:
+		return vpe_ctl & GIC_VPE_CTL_FDC_RTBL_MSK;
+	case GIC_LOCAL_INT_SWINT0:
+	case GIC_LOCAL_INT_SWINT1:
+		return vpe_ctl & GIC_VPE_CTL_SWINT_RTBL_MSK;
+	default:
+		return true;
+	}
+}
+
+unsigned int gic_get_timer_pending(void)
+{
+	unsigned int vpe_pending;
+
+	vpe_pending = gic_read(GIC_REG(VPE_LOCAL, GIC_VPE_PEND));
+	return vpe_pending & GIC_VPE_PEND_TIMER_MSK;
+}
+
+static void gic_bind_eic_interrupt(int irq, int set)
+{
+	/* Convert irq vector # to hw int # */
+	irq -= GIC_PIN_TO_VEC_OFFSET;
+
+	/* Set irq to use shadow set */
+	gic_write(GIC_REG(VPE_LOCAL, GIC_VPE_EIC_SHADOW_SET_BASE) +
+		  GIC_VPE_EIC_SS(irq), set);
+}
+
+void gic_send_ipi(unsigned int intr)
+{
+	gic_write(GIC_REG(SHARED, GIC_SH_WEDGE), GIC_SH_WEDGE_SET(intr));
+}
+
+int gic_get_c0_compare_int(void)
+{
+	if (!gic_local_irq_is_routable(GIC_LOCAL_INT_TIMER))
+		return MIPS_CPU_IRQ_BASE + cp0_compare_irq;
+	return irq_create_mapping(gic_irq_domain,
+				  GIC_LOCAL_TO_HWIRQ(GIC_LOCAL_INT_TIMER));
+}
+
+int gic_get_c0_perfcount_int(void)
+{
+	if (!gic_local_irq_is_routable(GIC_LOCAL_INT_PERFCTR)) {
+		/* Is the erformance counter shared with the timer? */
+		if (cp0_perfcount_irq < 0)
+			return -1;
+		return MIPS_CPU_IRQ_BASE + cp0_perfcount_irq;
+	}
+	return irq_create_mapping(gic_irq_domain,
+				  GIC_LOCAL_TO_HWIRQ(GIC_LOCAL_INT_PERFCTR));
+}
+
+static unsigned int gic_get_int(void)
+{
+	unsigned int i;
+	unsigned long *pcpu_mask;
+	unsigned long pending_reg, intrmask_reg;
+	DECLARE_BITMAP(pending, GIC_MAX_INTRS);
+	DECLARE_BITMAP(intrmask, GIC_MAX_INTRS);
+
+	/* Get per-cpu bitmaps */
+	pcpu_mask = pcpu_masks[smp_processor_id()].pcpu_mask;
+
+	pending_reg = GIC_REG(SHARED, GIC_SH_PEND);
+	intrmask_reg = GIC_REG(SHARED, GIC_SH_MASK);
+
+	for (i = 0; i < BITS_TO_LONGS(gic_shared_intrs); i++) {
+		pending[i] = gic_read(pending_reg);
+		intrmask[i] = gic_read(intrmask_reg);
+		pending_reg += 0x4;
+		intrmask_reg += 0x4;
+	}
+
+	bitmap_and(pending, pending, intrmask, gic_shared_intrs);
+	bitmap_and(pending, pending, pcpu_mask, gic_shared_intrs);
+
+	return find_first_bit(pending, gic_shared_intrs);
+}
+
+static void gic_mask_irq(struct irq_data *d)
+{
+	gic_reset_mask(GIC_HWIRQ_TO_SHARED(d->hwirq));
+}
+
+static void gic_unmask_irq(struct irq_data *d)
+{
+	gic_set_mask(GIC_HWIRQ_TO_SHARED(d->hwirq));
+}
+
+static void gic_ack_irq(struct irq_data *d)
+{
+	unsigned int irq = GIC_HWIRQ_TO_SHARED(d->hwirq);
+
+	gic_write(GIC_REG(SHARED, GIC_SH_WEDGE), GIC_SH_WEDGE_CLR(irq));
+}
+
+static int gic_set_type(struct irq_data *d, unsigned int type)
+{
+	unsigned int irq = GIC_HWIRQ_TO_SHARED(d->hwirq);
+	unsigned long flags;
+	bool is_edge;
+
+	spin_lock_irqsave(&gic_lock, flags);
+	switch (type & IRQ_TYPE_SENSE_MASK) {
+	case IRQ_TYPE_EDGE_FALLING:
+		gic_set_polarity(irq, GIC_POL_NEG);
+		gic_set_trigger(irq, GIC_TRIG_EDGE);
+		gic_set_dual_edge(irq, GIC_TRIG_DUAL_DISABLE);
+		is_edge = true;
+		break;
+	case IRQ_TYPE_EDGE_RISING:
+		gic_set_polarity(irq, GIC_POL_POS);
+		gic_set_trigger(irq, GIC_TRIG_EDGE);
+		gic_set_dual_edge(irq, GIC_TRIG_DUAL_DISABLE);
+		is_edge = true;
+		break;
+	case IRQ_TYPE_EDGE_BOTH:
+		/* polarity is irrelevant in this case */
+		gic_set_trigger(irq, GIC_TRIG_EDGE);
+		gic_set_dual_edge(irq, GIC_TRIG_DUAL_ENABLE);
+		is_edge = true;
+		break;
+	case IRQ_TYPE_LEVEL_LOW:
+		gic_set_polarity(irq, GIC_POL_NEG);
+		gic_set_trigger(irq, GIC_TRIG_LEVEL);
+		gic_set_dual_edge(irq, GIC_TRIG_DUAL_DISABLE);
+		is_edge = false;
+		break;
+	case IRQ_TYPE_LEVEL_HIGH:
+	default:
+		gic_set_polarity(irq, GIC_POL_POS);
+		gic_set_trigger(irq, GIC_TRIG_LEVEL);
+		gic_set_dual_edge(irq, GIC_TRIG_DUAL_DISABLE);
+		is_edge = false;
+		break;
+	}
+
+	if (is_edge) {
+		__irq_set_chip_handler_name_locked(d->irq,
+						   &gic_edge_irq_controller,
+						   handle_edge_irq, NULL);
+	} else {
+		__irq_set_chip_handler_name_locked(d->irq,
+						   &gic_level_irq_controller,
+						   handle_level_irq, NULL);
+	}
+	spin_unlock_irqrestore(&gic_lock, flags);
+
+	return 0;
+}
+
+#ifdef CONFIG_SMP
+static int gic_set_affinity(struct irq_data *d, const struct cpumask *cpumask,
+			    bool force)
+{
+	unsigned int irq = GIC_HWIRQ_TO_SHARED(d->hwirq);
+	cpumask_t	tmp = CPU_MASK_NONE;
+	unsigned long	flags;
+	int		i;
+
+	cpumask_and(&tmp, cpumask, cpu_online_mask);
+	if (cpus_empty(tmp))
+		return -EINVAL;
+
+	/* Assumption : cpumask refers to a single CPU */
+	spin_lock_irqsave(&gic_lock, flags);
+
+	/* Re-route this IRQ */
+	gic_map_to_vpe(irq, first_cpu(tmp));
+
+	/* Update the pcpu_masks */
+	for (i = 0; i < NR_CPUS; i++)
+		clear_bit(irq, pcpu_masks[i].pcpu_mask);
+	set_bit(irq, pcpu_masks[first_cpu(tmp)].pcpu_mask);
+
+	cpumask_copy(d->affinity, cpumask);
+	spin_unlock_irqrestore(&gic_lock, flags);
+
+	return IRQ_SET_MASK_OK_NOCOPY;
+}
+#endif
+
+static struct irq_chip gic_level_irq_controller = {
+	.name			=	"MIPS GIC",
+	.irq_mask		=	gic_mask_irq,
+	.irq_unmask		=	gic_unmask_irq,
+	.irq_set_type		=	gic_set_type,
+#ifdef CONFIG_SMP
+	.irq_set_affinity	=	gic_set_affinity,
+#endif
+};
+
+static struct irq_chip gic_edge_irq_controller = {
+	.name			=	"MIPS GIC",
+	.irq_ack		=	gic_ack_irq,
+	.irq_mask		=	gic_mask_irq,
+	.irq_unmask		=	gic_unmask_irq,
+	.irq_set_type		=	gic_set_type,
+#ifdef CONFIG_SMP
+	.irq_set_affinity	=	gic_set_affinity,
+#endif
+};
+
+static unsigned int gic_get_local_int(void)
+{
+	unsigned long pending, masked;
+
+	pending = gic_read(GIC_REG(VPE_LOCAL, GIC_VPE_PEND));
+	masked = gic_read(GIC_REG(VPE_LOCAL, GIC_VPE_MASK));
+
+	bitmap_and(&pending, &pending, &masked, GIC_NUM_LOCAL_INTRS);
+
+	return find_first_bit(&pending, GIC_NUM_LOCAL_INTRS);
+}
+
+static void gic_mask_local_irq(struct irq_data *d)
+{
+	int intr = GIC_HWIRQ_TO_LOCAL(d->hwirq);
+
+	gic_write(GIC_REG(VPE_LOCAL, GIC_VPE_RMASK), 1 << intr);
+}
+
+static void gic_unmask_local_irq(struct irq_data *d)
+{
+	int intr = GIC_HWIRQ_TO_LOCAL(d->hwirq);
+
+	gic_write(GIC_REG(VPE_LOCAL, GIC_VPE_SMASK), 1 << intr);
+}
+
+static struct irq_chip gic_local_irq_controller = {
+	.name			=	"MIPS GIC Local",
+	.irq_mask		=	gic_mask_local_irq,
+	.irq_unmask		=	gic_unmask_local_irq,
+};
+
+static void gic_mask_local_irq_all_vpes(struct irq_data *d)
+{
+	int intr = GIC_HWIRQ_TO_LOCAL(d->hwirq);
+	int i;
+	unsigned long flags;
+
+	spin_lock_irqsave(&gic_lock, flags);
+	for (i = 0; i < gic_vpes; i++) {
+		gic_write(GIC_REG(VPE_LOCAL, GIC_VPE_OTHER_ADDR), i);
+		gic_write(GIC_REG(VPE_OTHER, GIC_VPE_RMASK), 1 << intr);
+	}
+	spin_unlock_irqrestore(&gic_lock, flags);
+}
+
+static void gic_unmask_local_irq_all_vpes(struct irq_data *d)
+{
+	int intr = GIC_HWIRQ_TO_LOCAL(d->hwirq);
+	int i;
+	unsigned long flags;
+
+	spin_lock_irqsave(&gic_lock, flags);
+	for (i = 0; i < gic_vpes; i++) {
+		gic_write(GIC_REG(VPE_LOCAL, GIC_VPE_OTHER_ADDR), i);
+		gic_write(GIC_REG(VPE_OTHER, GIC_VPE_SMASK), 1 << intr);
+	}
+	spin_unlock_irqrestore(&gic_lock, flags);
+}
+
+static struct irq_chip gic_all_vpes_local_irq_controller = {
+	.name			=	"MIPS GIC Local",
+	.irq_mask		=	gic_mask_local_irq_all_vpes,
+	.irq_unmask		=	gic_unmask_local_irq_all_vpes,
+};
+
+static void __gic_irq_dispatch(void)
+{
+	unsigned int intr, virq;
+
+	while ((intr = gic_get_local_int()) != GIC_NUM_LOCAL_INTRS) {
+		virq = irq_linear_revmap(gic_irq_domain,
+					 GIC_LOCAL_TO_HWIRQ(intr));
+		do_IRQ(virq);
+	}
+
+	while ((intr = gic_get_int()) != gic_shared_intrs) {
+		virq = irq_linear_revmap(gic_irq_domain,
+					 GIC_SHARED_TO_HWIRQ(intr));
+		do_IRQ(virq);
+	}
+}
+
+static void gic_irq_dispatch(unsigned int irq, struct irq_desc *desc)
+{
+	__gic_irq_dispatch();
+}
+
+#ifdef CONFIG_MIPS_GIC_IPI
+static int gic_resched_int_base;
+static int gic_call_int_base;
+
+unsigned int plat_ipi_resched_int_xlate(unsigned int cpu)
+{
+	return gic_resched_int_base + cpu;
+}
+
+unsigned int plat_ipi_call_int_xlate(unsigned int cpu)
+{
+	return gic_call_int_base + cpu;
+}
+
+static irqreturn_t ipi_resched_interrupt(int irq, void *dev_id)
+{
+	scheduler_ipi();
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t ipi_call_interrupt(int irq, void *dev_id)
+{
+	smp_call_function_interrupt();
+
+	return IRQ_HANDLED;
+}
+
+static struct irqaction irq_resched = {
+	.handler	= ipi_resched_interrupt,
+	.flags		= IRQF_PERCPU,
+	.name		= "IPI resched"
+};
+
+static struct irqaction irq_call = {
+	.handler	= ipi_call_interrupt,
+	.flags		= IRQF_PERCPU,
+	.name		= "IPI call"
+};
+
+static __init void gic_ipi_init_one(unsigned int intr, int cpu,
+				    struct irqaction *action)
+{
+	int virq = irq_create_mapping(gic_irq_domain,
+				      GIC_SHARED_TO_HWIRQ(intr));
+	int i;
+
+	gic_map_to_vpe(intr, cpu);
+	for (i = 0; i < NR_CPUS; i++)
+		clear_bit(intr, pcpu_masks[i].pcpu_mask);
+	set_bit(intr, pcpu_masks[cpu].pcpu_mask);
+
+	irq_set_irq_type(virq, IRQ_TYPE_EDGE_RISING);
+
+	irq_set_handler(virq, handle_percpu_irq);
+	setup_irq(virq, action);
+}
+
+static __init void gic_ipi_init(void)
+{
+	int i;
+
+	/* Use last 2 * NR_CPUS interrupts as IPIs */
+	gic_resched_int_base = gic_shared_intrs - nr_cpu_ids;
+	gic_call_int_base = gic_resched_int_base - nr_cpu_ids;
+
+	for (i = 0; i < nr_cpu_ids; i++) {
+		gic_ipi_init_one(gic_call_int_base + i, i, &irq_call);
+		gic_ipi_init_one(gic_resched_int_base + i, i, &irq_resched);
+	}
+}
+#else
+static inline void gic_ipi_init(void)
+{
+}
+#endif
+
+static void __init gic_basic_init(void)
+{
+	unsigned int i;
+
+	board_bind_eic_interrupt = &gic_bind_eic_interrupt;
+
+	/* Setup defaults */
+	for (i = 0; i < gic_shared_intrs; i++) {
+		gic_set_polarity(i, GIC_POL_POS);
+		gic_set_trigger(i, GIC_TRIG_LEVEL);
+		gic_reset_mask(i);
+	}
+
+	for (i = 0; i < gic_vpes; i++) {
+		unsigned int j;
+
+		gic_write(GIC_REG(VPE_LOCAL, GIC_VPE_OTHER_ADDR), i);
+		for (j = 0; j < GIC_NUM_LOCAL_INTRS; j++) {
+			if (!gic_local_irq_is_routable(j))
+				continue;
+			gic_write(GIC_REG(VPE_OTHER, GIC_VPE_RMASK), 1 << j);
+		}
+	}
+}
+
+static int gic_local_irq_domain_map(struct irq_domain *d, unsigned int virq,
+				    irq_hw_number_t hw)
+{
+	int intr = GIC_HWIRQ_TO_LOCAL(hw);
+	int ret = 0;
+	int i;
+	unsigned long flags;
+
+	if (!gic_local_irq_is_routable(intr))
+		return -EPERM;
+
+	/*
+	 * HACK: These are all really percpu interrupts, but the rest
+	 * of the MIPS kernel code does not use the percpu IRQ API for
+	 * the CP0 timer and performance counter interrupts.
+	 */
+	if (intr != GIC_LOCAL_INT_TIMER && intr != GIC_LOCAL_INT_PERFCTR) {
+		irq_set_chip_and_handler(virq,
+					 &gic_local_irq_controller,
+					 handle_percpu_devid_irq);
+		irq_set_percpu_devid(virq);
+	} else {
+		irq_set_chip_and_handler(virq,
+					 &gic_all_vpes_local_irq_controller,
+					 handle_percpu_irq);
+	}
+
+	spin_lock_irqsave(&gic_lock, flags);
+	for (i = 0; i < gic_vpes; i++) {
+		u32 val = GIC_MAP_TO_PIN_MSK | gic_cpu_pin;
+
+		gic_write(GIC_REG(VPE_LOCAL, GIC_VPE_OTHER_ADDR), i);
+
+		switch (intr) {
+		case GIC_LOCAL_INT_WD:
+			gic_write(GIC_REG(VPE_OTHER, GIC_VPE_WD_MAP), val);
+			break;
+		case GIC_LOCAL_INT_COMPARE:
+			gic_write(GIC_REG(VPE_OTHER, GIC_VPE_COMPARE_MAP), val);
+			break;
+		case GIC_LOCAL_INT_TIMER:
+			gic_write(GIC_REG(VPE_OTHER, GIC_VPE_TIMER_MAP), val);
+			break;
+		case GIC_LOCAL_INT_PERFCTR:
+			gic_write(GIC_REG(VPE_OTHER, GIC_VPE_PERFCTR_MAP), val);
+			break;
+		case GIC_LOCAL_INT_SWINT0:
+			gic_write(GIC_REG(VPE_OTHER, GIC_VPE_SWINT0_MAP), val);
+			break;
+		case GIC_LOCAL_INT_SWINT1:
+			gic_write(GIC_REG(VPE_OTHER, GIC_VPE_SWINT1_MAP), val);
+			break;
+		case GIC_LOCAL_INT_FDC:
+			gic_write(GIC_REG(VPE_OTHER, GIC_VPE_FDC_MAP), val);
+			break;
+		default:
+			pr_err("Invalid local IRQ %d\n", intr);
+			ret = -EINVAL;
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&gic_lock, flags);
+
+	return ret;
+}
+
+static int gic_shared_irq_domain_map(struct irq_domain *d, unsigned int virq,
+				     irq_hw_number_t hw)
+{
+	int intr = GIC_HWIRQ_TO_SHARED(hw);
+	unsigned long flags;
+
+	irq_set_chip_and_handler(virq, &gic_level_irq_controller,
+				 handle_level_irq);
+
+	spin_lock_irqsave(&gic_lock, flags);
+	gic_map_to_pin(intr, gic_cpu_pin);
+	/* Map to VPE 0 by default */
+	gic_map_to_vpe(intr, 0);
+	set_bit(intr, pcpu_masks[0].pcpu_mask);
+	spin_unlock_irqrestore(&gic_lock, flags);
+
+	return 0;
+}
+
+static int gic_irq_domain_map(struct irq_domain *d, unsigned int virq,
+			      irq_hw_number_t hw)
+{
+	if (GIC_HWIRQ_TO_LOCAL(hw) < GIC_NUM_LOCAL_INTRS)
+		return gic_local_irq_domain_map(d, virq, hw);
+	return gic_shared_irq_domain_map(d, virq, hw);
+}
+
+static int gic_irq_domain_xlate(struct irq_domain *d, struct device_node *ctrlr,
+				const u32 *intspec, unsigned int intsize,
+				irq_hw_number_t *out_hwirq,
+				unsigned int *out_type)
+{
+	if (intsize != 3)
+		return -EINVAL;
+
+	if (intspec[0] == GIC_SHARED)
+		*out_hwirq = GIC_SHARED_TO_HWIRQ(intspec[1]);
+	else if (intspec[0] == GIC_LOCAL)
+		*out_hwirq = GIC_LOCAL_TO_HWIRQ(intspec[1]);
+	else
+		return -EINVAL;
+	*out_type = intspec[2] & IRQ_TYPE_SENSE_MASK;
+
+	return 0;
+}
+
+static struct irq_domain_ops gic_irq_domain_ops = {
+	.map = gic_irq_domain_map,
+	.xlate = gic_irq_domain_xlate,
+};
+
+static void __init __gic_init(unsigned long gic_base_addr,
+			      unsigned long gic_addrspace_size,
+			      unsigned int cpu_vec, unsigned int irqbase,
+			      struct device_node *node)
+{
+	unsigned int gicconfig;
+
+	gic_base = ioremap_nocache(gic_base_addr, gic_addrspace_size);
+
+	gicconfig = gic_read(GIC_REG(SHARED, GIC_SH_CONFIG));
+	gic_shared_intrs = (gicconfig & GIC_SH_CONFIG_NUMINTRS_MSK) >>
+		   GIC_SH_CONFIG_NUMINTRS_SHF;
+	gic_shared_intrs = ((gic_shared_intrs + 1) * 8);
+
+	gic_vpes = (gicconfig & GIC_SH_CONFIG_NUMVPES_MSK) >>
+		  GIC_SH_CONFIG_NUMVPES_SHF;
+	gic_vpes = gic_vpes + 1;
+
+	if (cpu_has_veic) {
+		/* Always use vector 1 in EIC mode */
+		gic_cpu_pin = 0;
+		set_vi_handler(gic_cpu_pin + GIC_PIN_TO_VEC_OFFSET,
+			       __gic_irq_dispatch);
+	} else {
+		gic_cpu_pin = cpu_vec - GIC_CPU_PIN_OFFSET;
+		irq_set_chained_handler(MIPS_CPU_IRQ_BASE + cpu_vec,
+					gic_irq_dispatch);
+	}
+
+	gic_irq_domain = irq_domain_add_simple(node, GIC_NUM_LOCAL_INTRS +
+					       gic_shared_intrs, irqbase,
+					       &gic_irq_domain_ops, NULL);
+	if (!gic_irq_domain)
+		panic("Failed to add GIC IRQ domain");
+
+	gic_basic_init();
+
+	gic_ipi_init();
+}
+
+void __init gic_init(unsigned long gic_base_addr,
+		     unsigned long gic_addrspace_size,
+		     unsigned int cpu_vec, unsigned int irqbase)
+{
+	__gic_init(gic_base_addr, gic_addrspace_size, cpu_vec, irqbase, NULL);
+}
+
+static int __init gic_of_init(struct device_node *node,
+			      struct device_node *parent)
+{
+	struct resource res;
+	unsigned int cpu_vec, i = 0, reserved = 0;
+	phys_addr_t gic_base;
+	size_t gic_len;
+
+	/* Find the first available CPU vector. */
+	while (!of_property_read_u32_index(node, "mti,reserved-cpu-vectors",
+					   i++, &cpu_vec))
+		reserved |= BIT(cpu_vec);
+	for (cpu_vec = 2; cpu_vec < 8; cpu_vec++) {
+		if (!(reserved & BIT(cpu_vec)))
+			break;
+	}
+	if (cpu_vec == 8) {
+		pr_err("No CPU vectors available for GIC\n");
+		return -ENODEV;
+	}
+
+	if (of_address_to_resource(node, 0, &res)) {
+		/*
+		 * Probe the CM for the GIC base address if not specified
+		 * in the device-tree.
+		 */
+		if (mips_cm_present()) {
+			gic_base = read_gcr_gic_base() &
+				~CM_GCR_GIC_BASE_GICEN_MSK;
+			gic_len = 0x20000;
+		} else {
+			pr_err("Failed to get GIC memory range\n");
+			return -ENODEV;
+		}
+	} else {
+		gic_base = res.start;
+		gic_len = resource_size(&res);
+	}
+
+	if (mips_cm_present())
+		write_gcr_gic_base(gic_base | CM_GCR_GIC_BASE_GICEN_MSK);
+	gic_present = true;
+
+	__gic_init(gic_base, gic_len, cpu_vec, 0, node);
+
+	return 0;
+}
+IRQCHIP_DECLARE(mips_gic, "mti,gic", gic_of_init);
diff --git a/drivers/irqchip/irq-mtk-sysirq.c b/drivers/irqchip/irq-mtk-sysirq.c
new file mode 100644
index 000000000000..7e342df6a62f
--- /dev/null
+++ b/drivers/irqchip/irq-mtk-sysirq.c
@@ -0,0 +1,163 @@
+/*
+ * Copyright (c) 2014 MediaTek Inc.
+ * Author: Joe.C <yingjoe.chen@mediatek.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.
+ */
+
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+#include "irqchip.h"
+
+#define MT6577_SYS_INTPOL_NUM	(224)
+
+struct mtk_sysirq_chip_data {
+	spinlock_t lock;
+	void __iomem *intpol_base;
+};
+
+static int mtk_sysirq_set_type(struct irq_data *data, unsigned int type)
+{
+	irq_hw_number_t hwirq = data->hwirq;
+	struct mtk_sysirq_chip_data *chip_data = data->chip_data;
+	u32 offset, reg_index, value;
+	unsigned long flags;
+	int ret;
+
+	offset = hwirq & 0x1f;
+	reg_index = hwirq >> 5;
+
+	spin_lock_irqsave(&chip_data->lock, flags);
+	value = readl_relaxed(chip_data->intpol_base + reg_index * 4);
+	if (type == IRQ_TYPE_LEVEL_LOW || type == IRQ_TYPE_EDGE_FALLING) {
+		if (type == IRQ_TYPE_LEVEL_LOW)
+			type = IRQ_TYPE_LEVEL_HIGH;
+		else
+			type = IRQ_TYPE_EDGE_RISING;
+		value |= (1 << offset);
+	} else {
+		value &= ~(1 << offset);
+	}
+	writel(value, chip_data->intpol_base + reg_index * 4);
+
+	data = data->parent_data;
+	ret = data->chip->irq_set_type(data, type);
+	spin_unlock_irqrestore(&chip_data->lock, flags);
+	return ret;
+}
+
+static struct irq_chip mtk_sysirq_chip = {
+	.name			= "MT_SYSIRQ",
+	.irq_mask		= irq_chip_mask_parent,
+	.irq_unmask		= irq_chip_unmask_parent,
+	.irq_eoi		= irq_chip_eoi_parent,
+	.irq_set_type		= mtk_sysirq_set_type,
+	.irq_retrigger		= irq_chip_retrigger_hierarchy,
+	.irq_set_affinity	= irq_chip_set_affinity_parent,
+};
+
+static int mtk_sysirq_domain_xlate(struct irq_domain *d,
+				   struct device_node *controller,
+				   const u32 *intspec, unsigned int intsize,
+				   unsigned long *out_hwirq,
+				   unsigned int *out_type)
+{
+	if (intsize != 3)
+		return -EINVAL;
+
+	/* sysirq doesn't support PPI */
+	if (intspec[0])
+		return -EINVAL;
+
+	*out_hwirq = intspec[1];
+	*out_type = intspec[2] & IRQ_TYPE_SENSE_MASK;
+	return 0;
+}
+
+static int mtk_sysirq_domain_alloc(struct irq_domain *domain, unsigned int virq,
+				   unsigned int nr_irqs, void *arg)
+{
+	int i;
+	irq_hw_number_t hwirq;
+	struct of_phandle_args *irq_data = arg;
+	struct of_phandle_args gic_data = *irq_data;
+
+	if (irq_data->args_count != 3)
+		return -EINVAL;
+
+	/* sysirq doesn't support PPI */
+	if (irq_data->args[0])
+		return -EINVAL;
+
+	hwirq = irq_data->args[1];
+	for (i = 0; i < nr_irqs; i++)
+		irq_domain_set_hwirq_and_chip(domain, virq + i, hwirq + i,
+					      &mtk_sysirq_chip,
+					      domain->host_data);
+
+	gic_data.np = domain->parent->of_node;
+	return irq_domain_alloc_irqs_parent(domain, virq, nr_irqs, &gic_data);
+}
+
+static struct irq_domain_ops sysirq_domain_ops = {
+	.xlate = mtk_sysirq_domain_xlate,
+	.alloc = mtk_sysirq_domain_alloc,
+	.free = irq_domain_free_irqs_common,
+};
+
+static int __init mtk_sysirq_of_init(struct device_node *node,
+				     struct device_node *parent)
+{
+	struct irq_domain *domain, *domain_parent;
+	struct mtk_sysirq_chip_data *chip_data;
+	int ret = 0;
+
+	domain_parent = irq_find_host(parent);
+	if (!domain_parent) {
+		pr_err("mtk_sysirq: interrupt-parent not found\n");
+		return -EINVAL;
+	}
+
+	chip_data = kzalloc(sizeof(*chip_data), GFP_KERNEL);
+	if (!chip_data)
+		return -ENOMEM;
+
+	chip_data->intpol_base = of_io_request_and_map(node, 0, "intpol");
+	if (!chip_data->intpol_base) {
+		pr_err("mtk_sysirq: unable to map sysirq register\n");
+		ret = -ENOMEM;
+		goto out_free;
+	}
+
+	domain = irq_domain_add_hierarchy(domain_parent, 0,
+					  MT6577_SYS_INTPOL_NUM, node,
+					  &sysirq_domain_ops, chip_data);
+	if (!domain) {
+		ret = -ENOMEM;
+		goto out_unmap;
+	}
+	spin_lock_init(&chip_data->lock);
+
+	return 0;
+
+out_unmap:
+	iounmap(chip_data->intpol_base);
+out_free:
+	kfree(chip_data);
+	return ret;
+}
+IRQCHIP_DECLARE(mtk_sysirq, "mediatek,mt6577-sysirq", mtk_sysirq_of_init);
diff --git a/drivers/irqchip/irq-renesas-intc-irqpin.c b/drivers/irqchip/irq-renesas-intc-irqpin.c
index 542e850f4946..078cac5e2d08 100644
--- a/drivers/irqchip/irq-renesas-intc-irqpin.c
+++ b/drivers/irqchip/irq-renesas-intc-irqpin.c
@@ -562,7 +562,6 @@ static struct platform_driver intc_irqpin_device_driver = {
 	.driver		= {
 		.name	= "renesas_intc_irqpin",
 		.of_match_table = intc_irqpin_dt_ids,
-		.owner  = THIS_MODULE,
 	}
 };
 
diff --git a/drivers/irqchip/irq-renesas-irqc.c b/drivers/irqchip/irq-renesas-irqc.c
index 8777065012a5..384e6ed61d7c 100644
--- a/drivers/irqchip/irq-renesas-irqc.c
+++ b/drivers/irqchip/irq-renesas-irqc.c
@@ -281,7 +281,6 @@ static struct platform_driver irqc_device_driver = {
 	.driver		= {
 		.name	= "renesas_irqc",
 		.of_match_table	= irqc_dt_ids,
-		.owner	= THIS_MODULE,
 	}
 };
 
diff --git a/drivers/isdn/hisax/hfc4s8s_l1.c b/drivers/isdn/hisax/hfc4s8s_l1.c
index fc9f9d03fa13..0e5d673871c0 100644
--- a/drivers/isdn/hisax/hfc4s8s_l1.c
+++ b/drivers/isdn/hisax/hfc4s8s_l1.c
@@ -225,20 +225,6 @@ fWrite_hfc8(hfc4s8s_hw *a, u_char c)
 }
 
 static inline void
-Write_hfc16(hfc4s8s_hw *a, u_char b, u_short c)
-{
-	SetRegAddr(a, b);
-	outw(c, a->iobase);
-}
-
-static inline void
-Write_hfc32(hfc4s8s_hw *a, u_char b, u_long c)
-{
-	SetRegAddr(a, b);
-	outl(c, a->iobase);
-}
-
-static inline void
 fWrite_hfc32(hfc4s8s_hw *a, u_long c)
 {
 	outl(c, a->iobase);
@@ -266,13 +252,6 @@ Read_hfc16(hfc4s8s_hw *a, u_char b)
 }
 
 static inline u_long
-Read_hfc32(hfc4s8s_hw *a, u_char b)
-{
-	SetRegAddr(a, b);
-	return (inl((volatile u_int) a->iobase));
-}
-
-static inline u_long
 fRead_hfc32(hfc4s8s_hw *a)
 {
 	return (inl((volatile u_int) a->iobase));
diff --git a/drivers/isdn/mISDN/l1oip_core.c b/drivers/isdn/mISDN/l1oip_core.c
index 9f454d76cc06..67c21876c35f 100644
--- a/drivers/isdn/mISDN/l1oip_core.c
+++ b/drivers/isdn/mISDN/l1oip_core.c
@@ -1334,7 +1334,7 @@ init_card(struct l1oip *hc, int pri, int bundle)
 	if (id[l1oip_cnt] == 0) {
 		printk(KERN_WARNING "Warning: No 'id' value given or "
 		       "0, this is highly unsecure. Please use 32 "
-		       "bit randmom number 0x...\n");
+		       "bit random number 0x...\n");
 	}
 	hc->id = id[l1oip_cnt];
 	if (debug & DEBUG_L1OIP_INIT)
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index a210338cfeb1..a6c3d2f153f3 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -250,6 +250,17 @@ config LEDS_LP8788
 	help
 	  This option enables support for the Keyboard LEDs on the LP8788 PMIC.
 
+config LEDS_LP8860
+	tristate "LED support for the TI LP8860 4 channel LED driver"
+	depends on LEDS_CLASS && I2C
+	select REGMAP_I2C
+	help
+	  If you say yes here you get support for the TI LP8860 4 channel
+	  LED driver.
+	  This option enables support for the display cluster LEDs
+	  on the LP8860 4 channel LED driver using the I2C communication
+	  bus.
+
 config LEDS_CLEVO_MAIL
 	tristate "Mail LED on Clevo notebook"
 	depends on LEDS_CLASS
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index a2b164741465..1c65a191d907 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -28,6 +28,7 @@ obj-$(CONFIG_LEDS_LP5523)		+= leds-lp5523.o
 obj-$(CONFIG_LEDS_LP5562)		+= leds-lp5562.o
 obj-$(CONFIG_LEDS_LP8501)		+= leds-lp8501.o
 obj-$(CONFIG_LEDS_LP8788)		+= leds-lp8788.o
+obj-$(CONFIG_LEDS_LP8860)		+= leds-lp8860.o
 obj-$(CONFIG_LEDS_TCA6507)		+= leds-tca6507.o
 obj-$(CONFIG_LEDS_CLEVO_MAIL)		+= leds-clevo-mail.o
 obj-$(CONFIG_LEDS_IPAQ_MICRO)		+= leds-ipaq-micro.o
diff --git a/drivers/leds/led-class.c b/drivers/leds/led-class.c
index 7440c58b8e6f..dbeebac38d31 100644
--- a/drivers/leds/led-class.c
+++ b/drivers/leds/led-class.c
@@ -40,17 +40,27 @@ static ssize_t brightness_store(struct device *dev,
 {
 	struct led_classdev *led_cdev = dev_get_drvdata(dev);
 	unsigned long state;
-	ssize_t ret = -EINVAL;
+	ssize_t ret;
+
+	mutex_lock(&led_cdev->led_access);
+
+	if (led_sysfs_is_disabled(led_cdev)) {
+		ret = -EBUSY;
+		goto unlock;
+	}
 
 	ret = kstrtoul(buf, 10, &state);
 	if (ret)
-		return ret;
+		goto unlock;
 
 	if (state == LED_OFF)
 		led_trigger_remove(led_cdev);
-	__led_set_brightness(led_cdev, state);
+	led_set_brightness(led_cdev, state);
 
-	return size;
+	ret = size;
+unlock:
+	mutex_unlock(&led_cdev->led_access);
+	return ret;
 }
 static DEVICE_ATTR_RW(brightness);
 
@@ -99,7 +109,7 @@ static void led_timer_function(unsigned long data)
 	unsigned long delay;
 
 	if (!led_cdev->blink_delay_on || !led_cdev->blink_delay_off) {
-		__led_set_brightness(led_cdev, LED_OFF);
+		led_set_brightness_async(led_cdev, LED_OFF);
 		return;
 	}
 
@@ -122,7 +132,7 @@ static void led_timer_function(unsigned long data)
 		delay = led_cdev->blink_delay_off;
 	}
 
-	__led_set_brightness(led_cdev, brightness);
+	led_set_brightness_async(led_cdev, brightness);
 
 	/* Return in next iteration if led is in one-shot mode and we are in
 	 * the final blink state so that the led is toggled each delay_on +
@@ -148,7 +158,7 @@ static void set_brightness_delayed(struct work_struct *ws)
 
 	led_stop_software_blink(led_cdev);
 
-	__led_set_brightness(led_cdev, led_cdev->delayed_set_value);
+	led_set_brightness_async(led_cdev, led_cdev->delayed_set_value);
 }
 
 /**
@@ -214,6 +224,7 @@ int led_classdev_register(struct device *parent, struct led_classdev *led_cdev)
 #ifdef CONFIG_LEDS_TRIGGERS
 	init_rwsem(&led_cdev->trigger_lock);
 #endif
+	mutex_init(&led_cdev->led_access);
 	/* add to the list of leds */
 	down_write(&leds_list_lock);
 	list_add_tail(&led_cdev->node, &leds_list);
@@ -222,6 +233,8 @@ int led_classdev_register(struct device *parent, struct led_classdev *led_cdev)
 	if (!led_cdev->max_brightness)
 		led_cdev->max_brightness = LED_FULL;
 
+	led_cdev->flags |= SET_BRIGHTNESS_ASYNC;
+
 	led_update_brightness(led_cdev);
 
 	INIT_WORK(&led_cdev->set_brightness_work, set_brightness_delayed);
@@ -267,6 +280,8 @@ void led_classdev_unregister(struct led_classdev *led_cdev)
 	down_write(&leds_list_lock);
 	list_del(&led_cdev->node);
 	up_write(&leds_list_lock);
+
+	mutex_destroy(&led_cdev->led_access);
 }
 EXPORT_SYMBOL_GPL(led_classdev_unregister);
 
diff --git a/drivers/leds/led-core.c b/drivers/leds/led-core.c
index aaa8eba9099f..9886dace5ad2 100644
--- a/drivers/leds/led-core.c
+++ b/drivers/leds/led-core.c
@@ -42,13 +42,13 @@ static void led_set_software_blink(struct led_classdev *led_cdev,
 
 	/* never on - just set to off */
 	if (!delay_on) {
-		__led_set_brightness(led_cdev, LED_OFF);
+		led_set_brightness_async(led_cdev, LED_OFF);
 		return;
 	}
 
 	/* never off - just set to brightness */
 	if (!delay_off) {
-		__led_set_brightness(led_cdev, led_cdev->blink_brightness);
+		led_set_brightness_async(led_cdev, led_cdev->blink_brightness);
 		return;
 	}
 
@@ -117,6 +117,8 @@ EXPORT_SYMBOL_GPL(led_stop_software_blink);
 void led_set_brightness(struct led_classdev *led_cdev,
 			enum led_brightness brightness)
 {
+	int ret = 0;
+
 	/* delay brightness setting if need to stop soft-blink timer */
 	if (led_cdev->blink_delay_on || led_cdev->blink_delay_off) {
 		led_cdev->delayed_set_value = brightness;
@@ -124,7 +126,17 @@ void led_set_brightness(struct led_classdev *led_cdev,
 		return;
 	}
 
-	__led_set_brightness(led_cdev, brightness);
+	if (led_cdev->flags & SET_BRIGHTNESS_ASYNC) {
+		led_set_brightness_async(led_cdev, brightness);
+		return;
+	} else if (led_cdev->flags & SET_BRIGHTNESS_SYNC)
+		ret = led_set_brightness_sync(led_cdev, brightness);
+	else
+		ret = -EINVAL;
+
+	if (ret < 0)
+		dev_dbg(led_cdev->dev, "Setting LED brightness failed (%d)\n",
+			ret);
 }
 EXPORT_SYMBOL(led_set_brightness);
 
@@ -143,3 +155,21 @@ int led_update_brightness(struct led_classdev *led_cdev)
 	return ret;
 }
 EXPORT_SYMBOL(led_update_brightness);
+
+/* Caller must ensure led_cdev->led_access held */
+void led_sysfs_disable(struct led_classdev *led_cdev)
+{
+	lockdep_assert_held(&led_cdev->led_access);
+
+	led_cdev->flags |= LED_SYSFS_DISABLE;
+}
+EXPORT_SYMBOL_GPL(led_sysfs_disable);
+
+/* Caller must ensure led_cdev->led_access held */
+void led_sysfs_enable(struct led_classdev *led_cdev)
+{
+	lockdep_assert_held(&led_cdev->led_access);
+
+	led_cdev->flags &= ~LED_SYSFS_DISABLE;
+}
+EXPORT_SYMBOL_GPL(led_sysfs_enable);
diff --git a/drivers/leds/led-triggers.c b/drivers/leds/led-triggers.c
index c3734f10fdd5..e8b1120f486d 100644
--- a/drivers/leds/led-triggers.c
+++ b/drivers/leds/led-triggers.c
@@ -37,6 +37,14 @@ ssize_t led_trigger_store(struct device *dev, struct device_attribute *attr,
 	char trigger_name[TRIG_NAME_MAX];
 	struct led_trigger *trig;
 	size_t len;
+	int ret = count;
+
+	mutex_lock(&led_cdev->led_access);
+
+	if (led_sysfs_is_disabled(led_cdev)) {
+		ret = -EBUSY;
+		goto unlock;
+	}
 
 	trigger_name[sizeof(trigger_name) - 1] = '\0';
 	strncpy(trigger_name, buf, sizeof(trigger_name) - 1);
@@ -47,7 +55,7 @@ ssize_t led_trigger_store(struct device *dev, struct device_attribute *attr,
 
 	if (!strcmp(trigger_name, "none")) {
 		led_trigger_remove(led_cdev);
-		return count;
+		goto unlock;
 	}
 
 	down_read(&triggers_list_lock);
@@ -58,12 +66,14 @@ ssize_t led_trigger_store(struct device *dev, struct device_attribute *attr,
 			up_write(&led_cdev->trigger_lock);
 
 			up_read(&triggers_list_lock);
-			return count;
+			goto unlock;
 		}
 	}
 	up_read(&triggers_list_lock);
 
-	return -EINVAL;
+unlock:
+	mutex_unlock(&led_cdev->led_access);
+	return ret;
 }
 EXPORT_SYMBOL_GPL(led_trigger_store);
 
diff --git a/drivers/leds/leds-88pm860x.c b/drivers/leds/leds-88pm860x.c
index c2def5551ce1..1497a09166d6 100644
--- a/drivers/leds/leds-88pm860x.c
+++ b/drivers/leds/leds-88pm860x.c
@@ -237,7 +237,6 @@ static int pm860x_led_remove(struct platform_device *pdev)
 static struct platform_driver pm860x_led_driver = {
 	.driver	= {
 		.name	= "88pm860x-led",
-		.owner	= THIS_MODULE,
 	},
 	.probe	= pm860x_led_probe,
 	.remove	= pm860x_led_remove,
diff --git a/drivers/leds/leds-adp5520.c b/drivers/leds/leds-adp5520.c
index 5036d7b4f82e..07e66cae32d3 100644
--- a/drivers/leds/leds-adp5520.c
+++ b/drivers/leds/leds-adp5520.c
@@ -201,7 +201,6 @@ static int adp5520_led_remove(struct platform_device *pdev)
 static struct platform_driver adp5520_led_driver = {
 	.driver	= {
 		.name	= "adp5520-led",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= adp5520_led_probe,
 	.remove		= adp5520_led_remove,
diff --git a/drivers/leds/leds-asic3.c b/drivers/leds/leds-asic3.c
index 70c74a7f0dfe..1b71eac639f0 100644
--- a/drivers/leds/leds-asic3.c
+++ b/drivers/leds/leds-asic3.c
@@ -168,7 +168,6 @@ static struct platform_driver asic3_led_driver = {
 	.remove		= asic3_led_remove,
 	.driver		= {
 		.name	= "leds-asic3",
-		.owner	= THIS_MODULE,
 		.pm	= &asic3_led_pm_ops,
 	},
 };
diff --git a/drivers/leds/leds-clevo-mail.c b/drivers/leds/leds-clevo-mail.c
index f58a354428e3..0f9ed1ea0e89 100644
--- a/drivers/leds/leds-clevo-mail.c
+++ b/drivers/leds/leds-clevo-mail.c
@@ -168,7 +168,6 @@ static struct platform_driver clevo_mail_led_driver = {
 	.remove		= clevo_mail_led_remove,
 	.driver		= {
 		.name		= KBUILD_MODNAME,
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/leds/leds-cobalt-qube.c b/drivers/leds/leds-cobalt-qube.c
index 910339d86edf..d97522080491 100644
--- a/drivers/leds/leds-cobalt-qube.c
+++ b/drivers/leds/leds-cobalt-qube.c
@@ -76,7 +76,6 @@ static struct platform_driver cobalt_qube_led_driver = {
 	.remove	= cobalt_qube_led_remove,
 	.driver	= {
 		.name	= "cobalt-qube-leds",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/leds/leds-cobalt-raq.c b/drivers/leds/leds-cobalt-raq.c
index 001088b31373..06dbe18a2065 100644
--- a/drivers/leds/leds-cobalt-raq.c
+++ b/drivers/leds/leds-cobalt-raq.c
@@ -124,7 +124,6 @@ static struct platform_driver cobalt_raq_led_driver = {
 	.remove	= cobalt_raq_led_remove,
 	.driver = {
 		.name	= "cobalt-raq-leds",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/leds/leds-da903x.c b/drivers/leds/leds-da903x.c
index 54b8b5216b8b..952ba96e5b38 100644
--- a/drivers/leds/leds-da903x.c
+++ b/drivers/leds/leds-da903x.c
@@ -144,7 +144,6 @@ static int da903x_led_remove(struct platform_device *pdev)
 static struct platform_driver da903x_led_driver = {
 	.driver	= {
 		.name	= "da903x-led",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= da903x_led_probe,
 	.remove		= da903x_led_remove,
diff --git a/drivers/leds/leds-da9052.c b/drivers/leds/leds-da9052.c
index e4da1f460ac5..28291b6acc8e 100644
--- a/drivers/leds/leds-da9052.c
+++ b/drivers/leds/leds-da9052.c
@@ -199,7 +199,6 @@ static int da9052_led_remove(struct platform_device *pdev)
 static struct platform_driver da9052_led_driver = {
 	.driver		= {
 		.name	= "da9052-leds",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= da9052_led_probe,
 	.remove		= da9052_led_remove,
diff --git a/drivers/leds/leds-gpio.c b/drivers/leds/leds-gpio.c
index 868e6fc17cba..7ea1ea42c2d2 100644
--- a/drivers/leds/leds-gpio.c
+++ b/drivers/leds/leds-gpio.c
@@ -203,7 +203,7 @@ static struct gpio_leds_priv *gpio_leds_create(struct platform_device *pdev)
 		fwnode_property_read_string(child, "linux,default-trigger",
 					    &led.default_trigger);
 
-		if (!fwnode_property_read_string(child, "linux,default_state",
+		if (!fwnode_property_read_string(child, "default-state",
 						 &state)) {
 			if (!strcmp(state, "keep"))
 				led.default_state = LEDS_GPIO_DEFSTATE_KEEP;
@@ -291,7 +291,6 @@ static struct platform_driver gpio_led_driver = {
 	.remove		= gpio_led_remove,
 	.driver		= {
 		.name	= "leds-gpio",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_gpio_leds_match,
 	},
 };
diff --git a/drivers/leds/leds-hp6xx.c b/drivers/leds/leds-hp6xx.c
index d61a98896c71..0b84c0113126 100644
--- a/drivers/leds/leds-hp6xx.c
+++ b/drivers/leds/leds-hp6xx.c
@@ -83,7 +83,6 @@ static struct platform_driver hp6xxled_driver = {
 	.remove		= hp6xxled_remove,
 	.driver		= {
 		.name		= "hp6xx-led",
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/leds/leds-lm3533.c b/drivers/leds/leds-lm3533.c
index cbf61a40137d..6e2e02035dd7 100644
--- a/drivers/leds/leds-lm3533.c
+++ b/drivers/leds/leds-lm3533.c
@@ -766,7 +766,6 @@ static void lm3533_led_shutdown(struct platform_device *pdev)
 static struct platform_driver lm3533_led_driver = {
 	.driver = {
 		.name = "lm3533-leds",
-		.owner = THIS_MODULE,
 	},
 	.probe		= lm3533_led_probe,
 	.remove		= lm3533_led_remove,
diff --git a/drivers/leds/leds-lp8788.c b/drivers/leds/leds-lp8788.c
index 7c2cb384e7ae..3409f03c1fa8 100644
--- a/drivers/leds/leds-lp8788.c
+++ b/drivers/leds/leds-lp8788.c
@@ -183,7 +183,6 @@ static struct platform_driver lp8788_led_driver = {
 	.remove = lp8788_led_remove,
 	.driver = {
 		.name = LP8788_DEV_KEYLED,
-		.owner = THIS_MODULE,
 	},
 };
 module_platform_driver(lp8788_led_driver);
diff --git a/drivers/leds/leds-lp8860.c b/drivers/leds/leds-lp8860.c
new file mode 100644
index 000000000000..840e93f3ab3e
--- /dev/null
+++ b/drivers/leds/leds-lp8860.c
@@ -0,0 +1,491 @@
+/*
+ * TI LP8860 4-Channel LED Driver
+ *
+ * Copyright (C) 2014 Texas Instruments
+ *
+ * Author: Dan Murphy <dmurphy@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/i2c.h>
+#include <linux/init.h>
+#include <linux/leds.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+#include <linux/gpio/consumer.h>
+#include <linux/slab.h>
+
+#define LP8860_DISP_CL1_BRT_MSB		0x00
+#define LP8860_DISP_CL1_BRT_LSB		0x01
+#define LP8860_DISP_CL1_CURR_MSB	0x02
+#define LP8860_DISP_CL1_CURR_LSB	0x03
+#define LP8860_CL2_BRT_MSB		0x04
+#define LP8860_CL2_BRT_LSB		0x05
+#define LP8860_CL2_CURRENT		0x06
+#define LP8860_CL3_BRT_MSB		0x07
+#define LP8860_CL3_BRT_LSB		0x08
+#define LP8860_CL3_CURRENT		0x09
+#define LP8860_CL4_BRT_MSB		0x0a
+#define LP8860_CL4_BRT_LSB		0x0b
+#define LP8860_CL4_CURRENT		0x0c
+#define LP8860_CONFIG			0x0d
+#define LP8860_STATUS			0x0e
+#define LP8860_FAULT			0x0f
+#define LP8860_LED_FAULT		0x10
+#define LP8860_FAULT_CLEAR		0x11
+#define LP8860_ID			0x12
+#define LP8860_TEMP_MSB			0x13
+#define LP8860_TEMP_LSB			0x14
+#define LP8860_DISP_LED_CURR_MSB	0x15
+#define LP8860_DISP_LED_CURR_LSB	0x16
+#define LP8860_DISP_LED_PWM_MSB		0x17
+#define LP8860_DISP_LED_PWM_LSB		0x18
+#define LP8860_EEPROM_CNTRL		0x19
+#define LP8860_EEPROM_UNLOCK		0x1a
+
+#define LP8860_EEPROM_REG_0		0x60
+#define LP8860_EEPROM_REG_1		0x61
+#define LP8860_EEPROM_REG_2		0x62
+#define LP8860_EEPROM_REG_3		0x63
+#define LP8860_EEPROM_REG_4		0x64
+#define LP8860_EEPROM_REG_5		0x65
+#define LP8860_EEPROM_REG_6		0x66
+#define LP8860_EEPROM_REG_7		0x67
+#define LP8860_EEPROM_REG_8		0x68
+#define LP8860_EEPROM_REG_9		0x69
+#define LP8860_EEPROM_REG_10		0x6a
+#define LP8860_EEPROM_REG_11		0x6b
+#define LP8860_EEPROM_REG_12		0x6c
+#define LP8860_EEPROM_REG_13		0x6d
+#define LP8860_EEPROM_REG_14		0x6e
+#define LP8860_EEPROM_REG_15		0x6f
+#define LP8860_EEPROM_REG_16		0x70
+#define LP8860_EEPROM_REG_17		0x71
+#define LP8860_EEPROM_REG_18		0x72
+#define LP8860_EEPROM_REG_19		0x73
+#define LP8860_EEPROM_REG_20		0x74
+#define LP8860_EEPROM_REG_21		0x75
+#define LP8860_EEPROM_REG_22		0x76
+#define LP8860_EEPROM_REG_23		0x77
+#define LP8860_EEPROM_REG_24		0x78
+
+#define LP8860_LOCK_EEPROM		0x00
+#define LP8860_UNLOCK_EEPROM		0x01
+#define LP8860_PROGRAM_EEPROM		0x02
+#define LP8860_EEPROM_CODE_1		0x08
+#define LP8860_EEPROM_CODE_2		0xba
+#define LP8860_EEPROM_CODE_3		0xef
+
+#define LP8860_CLEAR_FAULTS		0x01
+
+#define LP8860_DISP_LED_NAME		"display_cluster"
+
+/**
+ * struct lp8860_led -
+ * @lock - Lock for reading/writing the device
+ * @work - Work item used to off load the brightness register writes
+ * @client - Pointer to the I2C client
+ * @led_dev - led class device pointer
+ * @regmap - Devices register map
+ * @eeprom_regmap - EEPROM register map
+ * @enable_gpio - VDDIO/EN gpio to enable communication interface
+ * @regulator - LED supply regulator pointer
+ * @brightness - Current brightness value requested
+ * @label - LED label
+**/
+struct lp8860_led {
+	struct mutex lock;
+	struct work_struct work;
+	struct i2c_client *client;
+	struct led_classdev led_dev;
+	struct regmap *regmap;
+	struct regmap *eeprom_regmap;
+	struct gpio_desc *enable_gpio;
+	struct regulator *regulator;
+	enum led_brightness brightness;
+	const char *label;
+};
+
+struct lp8860_eeprom_reg {
+	uint8_t reg;
+	uint8_t value;
+};
+
+static struct lp8860_eeprom_reg lp8860_eeprom_disp_regs[] = {
+	{ LP8860_EEPROM_REG_0, 0xed },
+	{ LP8860_EEPROM_REG_1, 0xdf },
+	{ LP8860_EEPROM_REG_2, 0xdc },
+	{ LP8860_EEPROM_REG_3, 0xf0 },
+	{ LP8860_EEPROM_REG_4, 0xdf },
+	{ LP8860_EEPROM_REG_5, 0xe5 },
+	{ LP8860_EEPROM_REG_6, 0xf2 },
+	{ LP8860_EEPROM_REG_7, 0x77 },
+	{ LP8860_EEPROM_REG_8, 0x77 },
+	{ LP8860_EEPROM_REG_9, 0x71 },
+	{ LP8860_EEPROM_REG_10, 0x3f },
+	{ LP8860_EEPROM_REG_11, 0xb7 },
+	{ LP8860_EEPROM_REG_12, 0x17 },
+	{ LP8860_EEPROM_REG_13, 0xef },
+	{ LP8860_EEPROM_REG_14, 0xb0 },
+	{ LP8860_EEPROM_REG_15, 0x87 },
+	{ LP8860_EEPROM_REG_16, 0xce },
+	{ LP8860_EEPROM_REG_17, 0x72 },
+	{ LP8860_EEPROM_REG_18, 0xe5 },
+	{ LP8860_EEPROM_REG_19, 0xdf },
+	{ LP8860_EEPROM_REG_20, 0x35 },
+	{ LP8860_EEPROM_REG_21, 0x06 },
+	{ LP8860_EEPROM_REG_22, 0xdc },
+	{ LP8860_EEPROM_REG_23, 0x88 },
+	{ LP8860_EEPROM_REG_24, 0x3E },
+};
+
+static int lp8860_unlock_eeprom(struct lp8860_led *led, int lock)
+{
+	int ret;
+
+	mutex_lock(&led->lock);
+
+	if (lock == LP8860_UNLOCK_EEPROM) {
+		ret = regmap_write(led->regmap,
+			LP8860_EEPROM_UNLOCK,
+			LP8860_EEPROM_CODE_1);
+		if (ret) {
+			dev_err(&led->client->dev, "EEPROM Unlock failed\n");
+			goto out;
+		}
+
+		ret = regmap_write(led->regmap,
+			LP8860_EEPROM_UNLOCK,
+			LP8860_EEPROM_CODE_2);
+		if (ret) {
+			dev_err(&led->client->dev, "EEPROM Unlock failed\n");
+			goto out;
+		}
+		ret = regmap_write(led->regmap,
+			LP8860_EEPROM_UNLOCK,
+			LP8860_EEPROM_CODE_3);
+		if (ret) {
+			dev_err(&led->client->dev, "EEPROM Unlock failed\n");
+			goto out;
+		}
+	} else {
+		ret = regmap_write(led->regmap,
+			LP8860_EEPROM_UNLOCK,
+			LP8860_LOCK_EEPROM);
+	}
+
+out:
+	mutex_unlock(&led->lock);
+	return ret;
+}
+
+static int lp8860_fault_check(struct lp8860_led *led)
+{
+	int ret, fault;
+	unsigned int read_buf;
+
+	ret = regmap_read(led->regmap, LP8860_LED_FAULT, &read_buf);
+	if (ret)
+		goto out;
+
+	fault = read_buf;
+
+	ret = regmap_read(led->regmap, LP8860_FAULT, &read_buf);
+	if (ret)
+		goto out;
+
+	fault |= read_buf;
+
+	/* Attempt to clear any faults */
+	if (fault)
+		ret = regmap_write(led->regmap, LP8860_FAULT_CLEAR,
+			LP8860_CLEAR_FAULTS);
+out:
+	return ret;
+}
+
+static void lp8860_led_brightness_work(struct work_struct *work)
+{
+	struct lp8860_led *led = container_of(work, struct lp8860_led, work);
+	int ret;
+	int disp_brightness = led->brightness * 255;
+
+	mutex_lock(&led->lock);
+
+	ret = lp8860_fault_check(led);
+	if (ret) {
+		dev_err(&led->client->dev, "Cannot read/clear faults\n");
+		goto out;
+	}
+
+	ret = regmap_write(led->regmap, LP8860_DISP_CL1_BRT_MSB,
+			(disp_brightness & 0xff00) >> 8);
+	if (ret) {
+		dev_err(&led->client->dev, "Cannot write CL1 MSB\n");
+		goto out;
+	}
+
+	ret = regmap_write(led->regmap, LP8860_DISP_CL1_BRT_LSB,
+			disp_brightness & 0xff);
+	if (ret) {
+		dev_err(&led->client->dev, "Cannot write CL1 LSB\n");
+		goto out;
+	}
+out:
+	mutex_unlock(&led->lock);
+}
+
+static void lp8860_brightness_set(struct led_classdev *led_cdev,
+				enum led_brightness brt_val)
+{
+	struct lp8860_led *led =
+			container_of(led_cdev, struct lp8860_led, led_dev);
+
+	led->brightness = brt_val;
+	schedule_work(&led->work);
+}
+
+static int lp8860_init(struct lp8860_led *led)
+{
+	unsigned int read_buf;
+	int ret, i, reg_count;
+
+	if (led->enable_gpio)
+		gpiod_direction_output(led->enable_gpio, 1);
+
+	ret = lp8860_fault_check(led);
+	if (ret)
+		goto out;
+
+	ret = regmap_read(led->regmap, LP8860_STATUS, &read_buf);
+	if (ret)
+		goto out;
+
+	ret = lp8860_unlock_eeprom(led, LP8860_UNLOCK_EEPROM);
+	if (ret) {
+		dev_err(&led->client->dev, "Failed unlocking EEPROM\n");
+		goto out;
+	}
+
+	reg_count = ARRAY_SIZE(lp8860_eeprom_disp_regs) / sizeof(lp8860_eeprom_disp_regs[0]);
+	for (i = 0; i < reg_count; i++) {
+		ret = regmap_write(led->eeprom_regmap,
+				lp8860_eeprom_disp_regs[i].reg,
+				lp8860_eeprom_disp_regs[i].value);
+		if (ret) {
+			dev_err(&led->client->dev, "Failed writing EEPROM\n");
+			goto out;
+		}
+	}
+
+	ret = lp8860_unlock_eeprom(led, LP8860_LOCK_EEPROM);
+	if (ret)
+		goto out;
+
+	ret = regmap_write(led->regmap,
+			LP8860_EEPROM_CNTRL,
+			LP8860_PROGRAM_EEPROM);
+	if (ret)
+		dev_err(&led->client->dev, "Failed programming EEPROM\n");
+out:
+	if (ret)
+		if (led->enable_gpio)
+			gpiod_direction_output(led->enable_gpio, 0);
+	return ret;
+}
+
+static struct reg_default lp8860_reg_defs[] = {
+	{ LP8860_DISP_CL1_BRT_MSB, 0x00},
+	{ LP8860_DISP_CL1_BRT_LSB, 0x00},
+	{ LP8860_DISP_CL1_CURR_MSB, 0x00},
+	{ LP8860_DISP_CL1_CURR_LSB, 0x00},
+	{ LP8860_CL2_BRT_MSB, 0x00},
+	{ LP8860_CL2_BRT_LSB, 0x00},
+	{ LP8860_CL2_CURRENT, 0x00},
+	{ LP8860_CL3_BRT_MSB, 0x00},
+	{ LP8860_CL3_BRT_LSB, 0x00},
+	{ LP8860_CL3_CURRENT, 0x00},
+	{ LP8860_CL4_BRT_MSB, 0x00},
+	{ LP8860_CL4_BRT_LSB, 0x00},
+	{ LP8860_CL4_CURRENT, 0x00},
+	{ LP8860_CONFIG, 0x00},
+	{ LP8860_FAULT_CLEAR, 0x00},
+	{ LP8860_EEPROM_CNTRL, 0x80},
+	{ LP8860_EEPROM_UNLOCK, 0x00},
+};
+
+static const struct regmap_config lp8860_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.max_register = LP8860_EEPROM_UNLOCK,
+	.reg_defaults = lp8860_reg_defs,
+	.num_reg_defaults = ARRAY_SIZE(lp8860_reg_defs),
+	.cache_type = REGCACHE_NONE,
+};
+
+static struct reg_default lp8860_eeprom_defs[] = {
+	{ LP8860_EEPROM_REG_0, 0x00 },
+	{ LP8860_EEPROM_REG_1, 0x00 },
+	{ LP8860_EEPROM_REG_2, 0x00 },
+	{ LP8860_EEPROM_REG_3, 0x00 },
+	{ LP8860_EEPROM_REG_4, 0x00 },
+	{ LP8860_EEPROM_REG_5, 0x00 },
+	{ LP8860_EEPROM_REG_6, 0x00 },
+	{ LP8860_EEPROM_REG_7, 0x00 },
+	{ LP8860_EEPROM_REG_8, 0x00 },
+	{ LP8860_EEPROM_REG_9, 0x00 },
+	{ LP8860_EEPROM_REG_10, 0x00 },
+	{ LP8860_EEPROM_REG_11, 0x00 },
+	{ LP8860_EEPROM_REG_12, 0x00 },
+	{ LP8860_EEPROM_REG_13, 0x00 },
+	{ LP8860_EEPROM_REG_14, 0x00 },
+	{ LP8860_EEPROM_REG_15, 0x00 },
+	{ LP8860_EEPROM_REG_16, 0x00 },
+	{ LP8860_EEPROM_REG_17, 0x00 },
+	{ LP8860_EEPROM_REG_18, 0x00 },
+	{ LP8860_EEPROM_REG_19, 0x00 },
+	{ LP8860_EEPROM_REG_20, 0x00 },
+	{ LP8860_EEPROM_REG_21, 0x00 },
+	{ LP8860_EEPROM_REG_22, 0x00 },
+	{ LP8860_EEPROM_REG_23, 0x00 },
+	{ LP8860_EEPROM_REG_24, 0x00 },
+};
+
+static const struct regmap_config lp8860_eeprom_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.max_register = LP8860_EEPROM_REG_24,
+	.reg_defaults = lp8860_eeprom_defs,
+	.num_reg_defaults = ARRAY_SIZE(lp8860_eeprom_defs),
+	.cache_type = REGCACHE_NONE,
+};
+
+static int lp8860_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int ret;
+	struct lp8860_led *led;
+	struct device_node *np = client->dev.of_node;
+
+	led = devm_kzalloc(&client->dev, sizeof(*led), GFP_KERNEL);
+	if (!led)
+		return -ENOMEM;
+
+	led->label = LP8860_DISP_LED_NAME;
+
+	if (client->dev.of_node) {
+		ret = of_property_read_string(np, "label", &led->label);
+		if (ret) {
+			dev_err(&client->dev, "Missing label in dt\n");
+			return -EINVAL;
+		}
+	}
+
+	led->enable_gpio = devm_gpiod_get(&client->dev, "enable");
+	if (IS_ERR(led->enable_gpio))
+		led->enable_gpio = NULL;
+	else
+		gpiod_direction_output(led->enable_gpio, 0);
+
+	led->regulator = devm_regulator_get(&client->dev, "vled");
+	if (IS_ERR(led->regulator))
+		led->regulator = NULL;
+
+	led->client = client;
+	led->led_dev.name = led->label;
+	led->led_dev.max_brightness = LED_FULL;
+	led->led_dev.brightness_set = lp8860_brightness_set;
+
+	mutex_init(&led->lock);
+	INIT_WORK(&led->work, lp8860_led_brightness_work);
+
+	i2c_set_clientdata(client, led);
+
+	led->regmap = devm_regmap_init_i2c(client, &lp8860_regmap_config);
+	if (IS_ERR(led->regmap)) {
+		ret = PTR_ERR(led->regmap);
+		dev_err(&client->dev, "Failed to allocate register map: %d\n",
+			ret);
+		return ret;
+	}
+
+	led->eeprom_regmap = devm_regmap_init_i2c(client, &lp8860_eeprom_regmap_config);
+	if (IS_ERR(led->eeprom_regmap)) {
+		ret = PTR_ERR(led->eeprom_regmap);
+		dev_err(&client->dev, "Failed to allocate register map: %d\n",
+			ret);
+		return ret;
+	}
+
+	ret = lp8860_init(led);
+	if (ret)
+		return ret;
+
+	ret = led_classdev_register(&client->dev, &led->led_dev);
+	if (ret) {
+		dev_err(&client->dev, "led register err: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int lp8860_remove(struct i2c_client *client)
+{
+	struct lp8860_led *led = i2c_get_clientdata(client);
+	int ret;
+
+	led_classdev_unregister(&led->led_dev);
+	cancel_work_sync(&led->work);
+
+	if (led->enable_gpio)
+		gpiod_direction_output(led->enable_gpio, 0);
+
+	if (led->regulator) {
+		ret = regulator_disable(led->regulator);
+		if (ret)
+			dev_err(&led->client->dev,
+				"Failed to disable regulator\n");
+	}
+
+	return 0;
+}
+
+static const struct i2c_device_id lp8860_id[] = {
+	{ "lp8860", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, lp8860_id);
+
+#ifdef CONFIG_OF
+static const struct of_device_id of_lp8860_leds_match[] = {
+	{ .compatible = "ti,lp8860", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, of_lp8860_leds_match);
+#endif
+
+static struct i2c_driver lp8860_driver = {
+	.driver = {
+		.name	= "lp8860",
+		.of_match_table = of_match_ptr(of_lp8860_leds_match),
+	},
+	.probe		= lp8860_probe,
+	.remove		= lp8860_remove,
+	.id_table	= lp8860_id,
+};
+module_i2c_driver(lp8860_driver);
+
+MODULE_DESCRIPTION("Texas Instruments LP8860 LED drvier");
+MODULE_AUTHOR("Dan Murphy <dmurphy@ti.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/leds/leds-lt3593.c b/drivers/leds/leds-lt3593.c
index 059f5b1f3553..9f41124765cc 100644
--- a/drivers/leds/leds-lt3593.c
+++ b/drivers/leds/leds-lt3593.c
@@ -184,7 +184,6 @@ static struct platform_driver lt3593_led_driver = {
 	.remove		= lt3593_led_remove,
 	.driver		= {
 		.name	= "leds-lt3593",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/leds/leds-max8997.c b/drivers/leds/leds-max8997.c
index 607bc2755aba..c592aa5662bb 100644
--- a/drivers/leds/leds-max8997.c
+++ b/drivers/leds/leds-max8997.c
@@ -303,7 +303,6 @@ static int max8997_led_remove(struct platform_device *pdev)
 static struct platform_driver max8997_led_driver = {
 	.driver = {
 		.name  = "max8997-led",
-		.owner = THIS_MODULE,
 	},
 	.probe  = max8997_led_probe,
 	.remove = max8997_led_remove,
diff --git a/drivers/leds/leds-mc13783.c b/drivers/leds/leds-mc13783.c
index f1db88e25138..85c3714e1b5a 100644
--- a/drivers/leds/leds-mc13783.c
+++ b/drivers/leds/leds-mc13783.c
@@ -325,7 +325,6 @@ MODULE_DEVICE_TABLE(platform, mc13xxx_led_id_table);
 static struct platform_driver mc13xxx_led_driver = {
 	.driver	= {
 		.name	= "mc13xxx-led",
-		.owner	= THIS_MODULE,
 	},
 	.remove		= mc13xxx_led_remove,
 	.id_table	= mc13xxx_led_id_table,
diff --git a/drivers/leds/leds-menf21bmc.c b/drivers/leds/leds-menf21bmc.c
index 89dd57769e3b..4b9eea815b1a 100644
--- a/drivers/leds/leds-menf21bmc.c
+++ b/drivers/leds/leds-menf21bmc.c
@@ -119,7 +119,6 @@ static struct platform_driver menf21bmc_led = {
 	.remove		= menf21bmc_led_remove,
 	.driver		= {
 		.name		= "menf21bmc_led",
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/leds/leds-net48xx.c b/drivers/leds/leds-net48xx.c
index 27d06c528246..ec3a2e8adcae 100644
--- a/drivers/leds/leds-net48xx.c
+++ b/drivers/leds/leds-net48xx.c
@@ -53,7 +53,6 @@ static struct platform_driver net48xx_led_driver = {
 	.remove		= net48xx_led_remove,
 	.driver		= {
 		.name		= DRVNAME,
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/leds/leds-netxbig.c b/drivers/leds/leds-netxbig.c
index 64fde485dcaa..26515c27ea8c 100644
--- a/drivers/leds/leds-netxbig.c
+++ b/drivers/leds/leds-netxbig.c
@@ -404,7 +404,6 @@ static struct platform_driver netxbig_led_driver = {
 	.remove		= netxbig_led_remove,
 	.driver		= {
 		.name	= "leds-netxbig",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/leds/leds-ns2.c b/drivers/leds/leds-ns2.c
index 231993d1fe21..1fd6adbb43b7 100644
--- a/drivers/leds/leds-ns2.c
+++ b/drivers/leds/leds-ns2.c
@@ -380,7 +380,6 @@ static struct platform_driver ns2_led_driver = {
 	.remove		= ns2_led_remove,
 	.driver		= {
 		.name		= "leds-ns2",
-		.owner		= THIS_MODULE,
 		.of_match_table	= of_match_ptr(of_ns2_leds_match),
 	},
 };
diff --git a/drivers/leds/leds-ot200.c b/drivers/leds/leds-ot200.c
index c9d906098466..39870de20a26 100644
--- a/drivers/leds/leds-ot200.c
+++ b/drivers/leds/leds-ot200.c
@@ -158,7 +158,6 @@ static struct platform_driver ot200_led_driver = {
 	.remove		= ot200_led_remove,
 	.driver		= {
 		.name	= "leds-ot200",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/leds/leds-pwm.c b/drivers/leds/leds-pwm.c
index d672bb4480f6..f668500a2157 100644
--- a/drivers/leds/leds-pwm.c
+++ b/drivers/leds/leds-pwm.c
@@ -232,7 +232,6 @@ static struct platform_driver led_pwm_driver = {
 	.remove		= led_pwm_remove,
 	.driver		= {
 		.name	= "leds_pwm",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_pwm_leds_match,
 	},
 };
diff --git a/drivers/leds/leds-rb532.c b/drivers/leds/leds-rb532.c
index 2e746d257b02..fcd1215b64a2 100644
--- a/drivers/leds/leds-rb532.c
+++ b/drivers/leds/leds-rb532.c
@@ -53,7 +53,6 @@ static struct platform_driver rb532_led_driver = {
 	.remove = rb532_led_remove,
 	.driver = {
 		.name = "rb532-led",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/leds/leds-regulator.c b/drivers/leds/leds-regulator.c
index 358430db6e66..ffc21397a675 100644
--- a/drivers/leds/leds-regulator.c
+++ b/drivers/leds/leds-regulator.c
@@ -153,24 +153,21 @@ static int regulator_led_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	vcc = regulator_get_exclusive(&pdev->dev, "vled");
+	vcc = devm_regulator_get_exclusive(&pdev->dev, "vled");
 	if (IS_ERR(vcc)) {
 		dev_err(&pdev->dev, "Cannot get vcc for %s\n", pdata->name);
 		return PTR_ERR(vcc);
 	}
 
 	led = devm_kzalloc(&pdev->dev, sizeof(*led), GFP_KERNEL);
-	if (led == NULL) {
-		ret = -ENOMEM;
-		goto err_vcc;
-	}
+	if (led == NULL)
+		return -ENOMEM;
 
 	led->cdev.max_brightness = led_regulator_get_max_brightness(vcc);
 	if (pdata->brightness > led->cdev.max_brightness) {
 		dev_err(&pdev->dev, "Invalid default brightness %d\n",
 				pdata->brightness);
-		ret = -EINVAL;
-		goto err_vcc;
+		return -EINVAL;
 	}
 	led->value = pdata->brightness;
 
@@ -191,7 +188,7 @@ static int regulator_led_probe(struct platform_device *pdev)
 	ret = led_classdev_register(&pdev->dev, &led->cdev);
 	if (ret < 0) {
 		cancel_work_sync(&led->work);
-		goto err_vcc;
+		return ret;
 	}
 
 	/* to expose the default value to userspace */
@@ -201,10 +198,6 @@ static int regulator_led_probe(struct platform_device *pdev)
 	regulator_led_set_value(led);
 
 	return 0;
-
-err_vcc:
-	regulator_put(vcc);
-	return ret;
 }
 
 static int regulator_led_remove(struct platform_device *pdev)
@@ -214,14 +207,12 @@ static int regulator_led_remove(struct platform_device *pdev)
 	led_classdev_unregister(&led->cdev);
 	cancel_work_sync(&led->work);
 	regulator_led_disable(led);
-	regulator_put(led->vcc);
 	return 0;
 }
 
 static struct platform_driver regulator_led_driver = {
 	.driver = {
 		   .name  = "leds-regulator",
-		   .owner = THIS_MODULE,
 		   },
 	.probe  = regulator_led_probe,
 	.remove = regulator_led_remove,
diff --git a/drivers/leds/leds-s3c24xx.c b/drivers/leds/leds-s3c24xx.c
index 785eb53a87fc..83641a7b299a 100644
--- a/drivers/leds/leds-s3c24xx.c
+++ b/drivers/leds/leds-s3c24xx.c
@@ -116,7 +116,6 @@ static struct platform_driver s3c24xx_led_driver = {
 	.remove		= s3c24xx_led_remove,
 	.driver		= {
 		.name		= "s3c24xx_led",
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/leds/leds-sunfire.c b/drivers/leds/leds-sunfire.c
index 0b8cc4a021a6..c2553c54f2cf 100644
--- a/drivers/leds/leds-sunfire.c
+++ b/drivers/leds/leds-sunfire.c
@@ -223,7 +223,6 @@ static struct platform_driver sunfire_clockboard_led_driver = {
 	.remove		= sunfire_led_generic_remove,
 	.driver		= {
 		.name	= "sunfire-clockboard-leds",
-		.owner	= THIS_MODULE,
 	},
 };
 
@@ -232,7 +231,6 @@ static struct platform_driver sunfire_fhc_led_driver = {
 	.remove		= sunfire_led_generic_remove,
 	.driver		= {
 		.name	= "sunfire-fhc-leds",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/leds/leds-syscon.c b/drivers/leds/leds-syscon.c
index 3afec79c43f4..6896e2d9ba58 100644
--- a/drivers/leds/leds-syscon.c
+++ b/drivers/leds/leds-syscon.c
@@ -18,10 +18,6 @@
  * 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 provides system reboot functionality for APM X-Gene SoC.
- * For system shutdown, this is board specify. If a board designer
- * implements GPIO shutdown, use the gpio-poweroff.c driver.
  */
 #include <linux/io.h>
 #include <linux/of_device.h>
@@ -70,39 +66,13 @@ static void syscon_led_set(struct led_classdev *led_cdev,
 		dev_err(sled->cdev.dev, "error updating LED status\n");
 }
 
-static const struct of_device_id syscon_match[] = {
-	{ .compatible = "syscon", },
-	{},
-};
-
-static int __init syscon_leds_init(void)
+static int __init syscon_leds_spawn(struct device_node *np,
+				    struct device *dev,
+				    struct regmap *map)
 {
-	const struct of_device_id *devid;
-	struct device_node *np;
 	struct device_node *child;
-	struct regmap *map;
-	struct platform_device *pdev;
-	struct device *dev;
 	int ret;
 
-	np = of_find_matching_node_and_match(NULL, syscon_match,
-					     &devid);
-	if (!np)
-		return -ENODEV;
-
-	map = syscon_node_to_regmap(np);
-	if (IS_ERR(map))
-		return PTR_ERR(map);
-
-	/*
-	 * If the map is there, the device should be there, we allocate
-	 * memory on the syscon device's behalf here.
-	 */
-	pdev = of_find_device_by_node(np);
-	if (!pdev)
-		return -ENODEV;
-	dev = &pdev->dev;
-
 	for_each_available_child_of_node(np, child) {
 		struct syscon_led *sled;
 		const char *state;
@@ -150,7 +120,6 @@ static int __init syscon_leds_init(void)
 				if (ret < 0)
 					return ret;
 			}
-
 		}
 		sled->cdev.brightness_set = syscon_led_set;
 
@@ -160,7 +129,39 @@ static int __init syscon_leds_init(void)
 
 		dev_info(dev, "registered LED %s\n", sled->cdev.name);
 	}
+	return 0;
+}
+
+static int __init syscon_leds_init(void)
+{
+	struct device_node *np;
+
+	for_each_of_allnodes(np) {
+		struct platform_device *pdev;
+		struct regmap *map;
+		int ret;
+
+		if (!of_device_is_compatible(np, "syscon"))
+			continue;
+
+		map = syscon_node_to_regmap(np);
+		if (IS_ERR(map)) {
+			pr_err("error getting regmap for syscon LEDs\n");
+			continue;
+		}
+
+		/*
+		 * If the map is there, the device should be there, we allocate
+		 * memory on the syscon device's behalf here.
+		 */
+		pdev = of_find_device_by_node(np);
+		if (!pdev)
+			return -ENODEV;
+		ret = syscon_leds_spawn(np, &pdev->dev, map);
+		if (ret)
+			dev_err(&pdev->dev, "could not spawn syscon LEDs\n");
+	}
 
-       return 0;
+	return 0;
 }
 device_initcall(syscon_leds_init);
diff --git a/drivers/leds/leds-wm831x-status.c b/drivers/leds/leds-wm831x-status.c
index 1b71e0701002..56027ef7c7e8 100644
--- a/drivers/leds/leds-wm831x-status.c
+++ b/drivers/leds/leds-wm831x-status.c
@@ -312,7 +312,6 @@ static int wm831x_status_remove(struct platform_device *pdev)
 static struct platform_driver wm831x_status_driver = {
 	.driver = {
 		   .name = "wm831x-status",
-		   .owner = THIS_MODULE,
 		   },
 	.probe = wm831x_status_probe,
 	.remove = wm831x_status_remove,
diff --git a/drivers/leds/leds-wm8350.c b/drivers/leds/leds-wm8350.c
index 4133ffe29015..0d121835673f 100644
--- a/drivers/leds/leds-wm8350.c
+++ b/drivers/leds/leds-wm8350.c
@@ -272,7 +272,6 @@ static int wm8350_led_remove(struct platform_device *pdev)
 static struct platform_driver wm8350_led_driver = {
 	.driver = {
 		   .name = "wm8350-led",
-		   .owner = THIS_MODULE,
 		   },
 	.probe = wm8350_led_probe,
 	.remove = wm8350_led_remove,
diff --git a/drivers/leds/leds-wrap.c b/drivers/leds/leds-wrap.c
index b358cc05eff5..1ba3defdd460 100644
--- a/drivers/leds/leds-wrap.c
+++ b/drivers/leds/leds-wrap.c
@@ -111,7 +111,6 @@ static struct platform_driver wrap_led_driver = {
 	.remove		= wrap_led_remove,
 	.driver		= {
 		.name		= DRVNAME,
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/leds/leds.h b/drivers/leds/leds.h
index 4c50365344a9..2348dbda5269 100644
--- a/drivers/leds/leds.h
+++ b/drivers/leds/leds.h
@@ -17,16 +17,28 @@
 #include <linux/rwsem.h>
 #include <linux/leds.h>
 
-static inline void __led_set_brightness(struct led_classdev *led_cdev,
+static inline void led_set_brightness_async(struct led_classdev *led_cdev,
 					enum led_brightness value)
 {
-	if (value > led_cdev->max_brightness)
-		value = led_cdev->max_brightness;
-	led_cdev->brightness = value;
+	led_cdev->brightness = min(value, led_cdev->max_brightness);
+
 	if (!(led_cdev->flags & LED_SUSPENDED))
 		led_cdev->brightness_set(led_cdev, value);
 }
 
+static inline int led_set_brightness_sync(struct led_classdev *led_cdev,
+					enum led_brightness value)
+{
+	int ret = 0;
+
+	led_cdev->brightness = min(value, led_cdev->max_brightness);
+
+	if (!(led_cdev->flags & LED_SUSPENDED))
+		ret = led_cdev->brightness_set_sync(led_cdev,
+						led_cdev->brightness);
+	return ret;
+}
+
 static inline int led_get_brightness(struct led_classdev *led_cdev)
 {
 	return led_cdev->brightness;
diff --git a/drivers/leds/trigger/ledtrig-backlight.c b/drivers/leds/trigger/ledtrig-backlight.c
index 47e55aa9eefa..59eca17d9661 100644
--- a/drivers/leds/trigger/ledtrig-backlight.c
+++ b/drivers/leds/trigger/ledtrig-backlight.c
@@ -51,9 +51,9 @@ static int fb_notifier_callback(struct notifier_block *p,
 
 	if ((n->old_status == UNBLANK) ^ n->invert) {
 		n->brightness = led->brightness;
-		__led_set_brightness(led, LED_OFF);
+		led_set_brightness_async(led, LED_OFF);
 	} else {
-		__led_set_brightness(led, n->brightness);
+		led_set_brightness_async(led, n->brightness);
 	}
 
 	n->old_status = new_status;
@@ -89,9 +89,9 @@ static ssize_t bl_trig_invert_store(struct device *dev,
 
 	/* After inverting, we need to update the LED. */
 	if ((n->old_status == BLANK) ^ n->invert)
-		__led_set_brightness(led, LED_OFF);
+		led_set_brightness_async(led, LED_OFF);
 	else
-		__led_set_brightness(led, n->brightness);
+		led_set_brightness_async(led, n->brightness);
 
 	return num;
 }
diff --git a/drivers/leds/trigger/ledtrig-default-on.c b/drivers/leds/trigger/ledtrig-default-on.c
index 81a91be8e18d..6f38f883aaf1 100644
--- a/drivers/leds/trigger/ledtrig-default-on.c
+++ b/drivers/leds/trigger/ledtrig-default-on.c
@@ -19,7 +19,7 @@
 
 static void defon_trig_activate(struct led_classdev *led_cdev)
 {
-	__led_set_brightness(led_cdev, led_cdev->max_brightness);
+	led_set_brightness_async(led_cdev, led_cdev->max_brightness);
 }
 
 static struct led_trigger defon_led_trigger = {
diff --git a/drivers/leds/trigger/ledtrig-gpio.c b/drivers/leds/trigger/ledtrig-gpio.c
index c86c41826476..4cc7040746c6 100644
--- a/drivers/leds/trigger/ledtrig-gpio.c
+++ b/drivers/leds/trigger/ledtrig-gpio.c
@@ -54,12 +54,12 @@ static void gpio_trig_work(struct work_struct *work)
 
 	if (tmp) {
 		if (gpio_data->desired_brightness)
-			__led_set_brightness(gpio_data->led,
+			led_set_brightness_async(gpio_data->led,
 					   gpio_data->desired_brightness);
 		else
-			__led_set_brightness(gpio_data->led, LED_FULL);
+			led_set_brightness_async(gpio_data->led, LED_FULL);
 	} else {
-		__led_set_brightness(gpio_data->led, LED_OFF);
+		led_set_brightness_async(gpio_data->led, LED_OFF);
 	}
 }
 
diff --git a/drivers/leds/trigger/ledtrig-heartbeat.c b/drivers/leds/trigger/ledtrig-heartbeat.c
index 5c8464a33172..fea6871d2609 100644
--- a/drivers/leds/trigger/ledtrig-heartbeat.c
+++ b/drivers/leds/trigger/ledtrig-heartbeat.c
@@ -74,7 +74,7 @@ static void led_heartbeat_function(unsigned long data)
 		break;
 	}
 
-	__led_set_brightness(led_cdev, brightness);
+	led_set_brightness_async(led_cdev, brightness);
 	mod_timer(&heartbeat_data->timer, jiffies + delay);
 }
 
diff --git a/drivers/leds/trigger/ledtrig-oneshot.c b/drivers/leds/trigger/ledtrig-oneshot.c
index cb4c7466692a..fbd02cdc3ad7 100644
--- a/drivers/leds/trigger/ledtrig-oneshot.c
+++ b/drivers/leds/trigger/ledtrig-oneshot.c
@@ -63,9 +63,9 @@ static ssize_t led_invert_store(struct device *dev,
 	oneshot_data->invert = !!state;
 
 	if (oneshot_data->invert)
-		__led_set_brightness(led_cdev, LED_FULL);
+		led_set_brightness_async(led_cdev, LED_FULL);
 	else
-		__led_set_brightness(led_cdev, LED_OFF);
+		led_set_brightness_async(led_cdev, LED_OFF);
 
 	return size;
 }
diff --git a/drivers/leds/trigger/ledtrig-transient.c b/drivers/leds/trigger/ledtrig-transient.c
index e5abc00bb00c..3c34de404d18 100644
--- a/drivers/leds/trigger/ledtrig-transient.c
+++ b/drivers/leds/trigger/ledtrig-transient.c
@@ -41,7 +41,7 @@ static void transient_timer_function(unsigned long data)
 	struct transient_trig_data *transient_data = led_cdev->trigger_data;
 
 	transient_data->activate = 0;
-	__led_set_brightness(led_cdev, transient_data->restore_state);
+	led_set_brightness_async(led_cdev, transient_data->restore_state);
 }
 
 static ssize_t transient_activate_show(struct device *dev,
@@ -72,7 +72,8 @@ static ssize_t transient_activate_store(struct device *dev,
 	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);
+		led_set_brightness_async(led_cdev,
+					transient_data->restore_state);
 		return size;
 	}
 
@@ -80,7 +81,7 @@ static ssize_t transient_activate_store(struct device *dev,
 	if (state == 1 && transient_data->activate == 0 &&
 	    transient_data->duration != 0) {
 		transient_data->activate = state;
-		__led_set_brightness(led_cdev, transient_data->state);
+		led_set_brightness_async(led_cdev, transient_data->state);
 		transient_data->restore_state =
 		    (transient_data->state == LED_FULL) ? LED_OFF : LED_FULL;
 		mod_timer(&transient_data->timer,
@@ -203,7 +204,8 @@ static void transient_trig_deactivate(struct led_classdev *led_cdev)
 
 	if (led_cdev->activated) {
 		del_timer_sync(&transient_data->timer);
-		__led_set_brightness(led_cdev, transient_data->restore_state);
+		led_set_brightness_async(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);
diff --git a/drivers/macintosh/Kconfig b/drivers/macintosh/Kconfig
index 3067d56b11a6..5844b80bd90e 100644
--- a/drivers/macintosh/Kconfig
+++ b/drivers/macintosh/Kconfig
@@ -204,16 +204,6 @@ config THERM_ADT746X
           iBook G4, and the ATI based aluminium PowerBooks, allowing slightly
 	  better fan behaviour by default, and some manual control.
 
-config THERM_PM72
-	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.
-
-	  This is deprecated, use windfarm instead.
-
 config WINDFARM
 	tristate "New PowerMac thermal control infrastructure"
 	depends on PPC
diff --git a/drivers/macintosh/Makefile b/drivers/macintosh/Makefile
index d2f0120bc878..383ba920085b 100644
--- a/drivers/macintosh/Makefile
+++ b/drivers/macintosh/Makefile
@@ -25,7 +25,6 @@ obj-$(CONFIG_ADB_IOP)		+= adb-iop.o
 obj-$(CONFIG_ADB_PMU68K)	+= via-pmu68k.o
 obj-$(CONFIG_ADB_MACIO)		+= macio-adb.o
 
-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
diff --git a/drivers/macintosh/smu.c b/drivers/macintosh/smu.c
index 4eab93aa570b..10ae69bcbbd2 100644
--- a/drivers/macintosh/smu.c
+++ b/drivers/macintosh/smu.c
@@ -667,7 +667,6 @@ static struct platform_driver smu_of_platform_driver =
 {
 	.driver = {
 		.name = "smu",
-		.owner = THIS_MODULE,
 		.of_match_table = smu_platform_match,
 	},
 	.probe		= smu_platform_probe,
diff --git a/drivers/macintosh/therm_pm72.c b/drivers/macintosh/therm_pm72.c
deleted file mode 100644
index 97cfc5ac9fd0..000000000000
--- a/drivers/macintosh/therm_pm72.c
+++ /dev/null
@@ -1,2279 +0,0 @@
-/*
- * Device driver for the thermostats & fan controller of  the
- * Apple G5 "PowerMac7,2" desktop machines.
- *
- * (c) Copyright IBM Corp. 2003-2004
- *
- * Maintained by: Benjamin Herrenschmidt
- *                <benh@kernel.crashing.org>
- * 
- *
- * The algorithm used is the PID control algorithm, used the same
- * way the published Darwin code does, using the same values that
- * are present in the Darwin 7.0 snapshot property lists.
- *
- * As far as the CPUs control loops are concerned, I use the
- * calibration & PID constants provided by the EEPROM,
- * I do _not_ embed any value from the property lists, as the ones
- * provided by Darwin 7.0 seem to always have an older version that
- * what I've seen on the actual computers.
- * It would be interesting to verify that though. Darwin has a
- * version code of 1.0.0d11 for all control loops it seems, while
- * so far, the machines EEPROMs contain a dataset versioned 1.0.0f
- *
- * Darwin doesn't provide source to all parts, some missing
- * bits like the AppleFCU driver or the actual scale of some
- * of the values returned by sensors had to be "guessed" some
- * way... or based on what Open Firmware does.
- *
- * I didn't yet figure out how to get the slots power consumption
- * out of the FCU, so that part has not been implemented yet and
- * the slots fan is set to a fixed 50% PWM, hoping this value is
- * safe enough ...
- *
- * Note: I have observed strange oscillations of the CPU control
- * loop on a dual G5 here. When idle, the CPU exhaust fan tend to
- * oscillates slowly (over several minutes) between the minimum
- * of 300RPMs and approx. 1000 RPMs. I don't know what is causing
- * this, it could be some incorrect constant or an error in the
- * way I ported the algorithm, or it could be just normal. I
- * don't have full understanding on the way Apple tweaked the PID
- * algorithm for the CPU control, it is definitely not a standard
- * implementation...
- *
- * TODO:  - Check MPU structure version/signature
- *        - Add things like /sbin/overtemp for non-critical
- *          overtemp conditions so userland can take some policy
- *          decisions, like slowing down CPUs
- *	  - Deal with fan and i2c failures in a better way
- *	  - Maybe do a generic PID based on params used for
- *	    U3 and Drives ? Definitely need to factor code a bit
- *          better... also make sensor detection more robust using
- *          the device-tree to probe for them
- *        - Figure out how to get the slots consumption and set the
- *          slots fan accordingly
- *
- * History:
- *
- *  Nov. 13, 2003 : 0.5
- *	- First release
- *
- *  Nov. 14, 2003 : 0.6
- *	- Read fan speed from FCU, low level fan routines now deal
- *	  with errors & check fan status, though higher level don't
- *	  do much.
- *	- Move a bunch of definitions to .h file
- *
- *  Nov. 18, 2003 : 0.7
- *	- Fix build on ppc64 kernel
- *	- Move back statics definitions to .c file
- *	- Avoid calling schedule_timeout with a negative number
- *
- *  Dec. 18, 2003 : 0.8
- *	- Fix typo when reading back fan speed on 2 CPU machines
- *
- *  Mar. 11, 2004 : 0.9
- *	- Rework code accessing the ADC chips, make it more robust and
- *	  closer to the chip spec. Also make sure it is configured properly,
- *        I've seen yet unexplained cases where on startup, I would have stale
- *        values in the configuration register
- *	- Switch back to use of target fan speed for PID, thus lowering
- *        pressure on i2c
- *
- *  Oct. 20, 2004 : 1.1
- *	- Add device-tree lookup for fan IDs, should detect liquid cooling
- *        pumps when present
- *	- Enable driver for PowerMac7,3 machines
- *	- Split the U3/Backside cooling on U3 & U3H versions as Darwin does
- *	- Add new CPU cooling algorithm for machines with liquid cooling
- *	- Workaround for some PowerMac7,3 with empty "fan" node in the devtree
- *	- Fix a signed/unsigned compare issue in some PID loops
- *
- *  Mar. 10, 2005 : 1.2
- *	- Add basic support for Xserve G5
- *	- Retrieve pumps min/max from EEPROM image in device-tree (broken)
- *	- Use min/max macros here or there
- *	- Latest darwin updated U3H min fan speed to 20% PWM
- *
- *  July. 06, 2006 : 1.3
- *	- Fix setting of RPM fans on Xserve G5 (they were going too fast)
- *      - Add missing slots fan control loop for Xserve G5
- *	- Lower fixed slots fan speed from 50% to 40% on desktop G5s. We
- *        still can't properly implement the control loop for these, so let's
- *        reduce the noise a little bit, it appears that 40% still gives us
- *        a pretty good air flow
- *	- Add code to "tickle" the FCU regulary so it doesn't think that
- *        we are gone while in fact, the machine just didn't need any fan
- *        speed change lately
- *
- */
-
-#include <linux/types.h>
-#include <linux/module.h>
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/delay.h>
-#include <linux/sched.h>
-#include <linux/init.h>
-#include <linux/spinlock.h>
-#include <linux/wait.h>
-#include <linux/reboot.h>
-#include <linux/kmod.h>
-#include <linux/i2c.h>
-#include <linux/kthread.h>
-#include <linux/mutex.h>
-#include <linux/of_device.h>
-#include <linux/of_platform.h>
-#include <asm/prom.h>
-#include <asm/machdep.h>
-#include <asm/io.h>
-#include <asm/sections.h>
-#include <asm/macio.h>
-
-#include "therm_pm72.h"
-
-#define VERSION "1.3"
-
-#undef DEBUG
-
-#ifdef DEBUG
-#define DBG(args...)	printk(args)
-#else
-#define DBG(args...)	do { } while(0)
-#endif
-
-
-/*
- * Driver statics
- */
-
-static struct platform_device *		of_dev;
-static struct i2c_adapter *		u3_0;
-static struct i2c_adapter *		u3_1;
-static struct i2c_adapter *		k2;
-static struct i2c_client *		fcu;
-static struct cpu_pid_state		processor_state[2];
-static struct basckside_pid_params	backside_params;
-static struct backside_pid_state	backside_state;
-static struct drives_pid_state		drives_state;
-static struct dimm_pid_state		dimms_state;
-static struct slots_pid_state		slots_state;
-static int				state;
-static int				cpu_count;
-static int				cpu_pid_type;
-static struct task_struct		*ctrl_task;
-static struct completion		ctrl_complete;
-static int				critical_state;
-static int				rackmac;
-static s32				dimm_output_clamp;
-static int 				fcu_rpm_shift;
-static int				fcu_tickle_ticks;
-static DEFINE_MUTEX(driver_lock);
-
-/*
- * We have 3 types of CPU PID control. One is "split" old style control
- * for intake & exhaust fans, the other is "combined" control for both
- * CPUs that also deals with the pumps when present. To be "compatible"
- * with OS X at this point, we only use "COMBINED" on the machines that
- * are identified as having the pumps (though that identification is at
- * least dodgy). Ultimately, we could probably switch completely to this
- * algorithm provided we hack it to deal with the UP case
- */
-#define CPU_PID_TYPE_SPLIT	0
-#define CPU_PID_TYPE_COMBINED	1
-#define CPU_PID_TYPE_RACKMAC	2
-
-/*
- * This table describes all fans in the FCU. The "id" and "type" values
- * are defaults valid for all earlier machines. Newer machines will
- * eventually override the table content based on the device-tree
- */
-struct fcu_fan_table
-{
-	char*	loc;	/* location code */
-	int	type;	/* 0 = rpm, 1 = pwm, 2 = pump */
-	int	id;	/* id or -1 */
-};
-
-#define FCU_FAN_RPM		0
-#define FCU_FAN_PWM		1
-
-#define FCU_FAN_ABSENT_ID	-1
-
-#define FCU_FAN_COUNT		ARRAY_SIZE(fcu_fans)
-
-struct fcu_fan_table	fcu_fans[] = {
-	[BACKSIDE_FAN_PWM_INDEX] = {
-		.loc	= "BACKSIDE,SYS CTRLR FAN",
-		.type	= FCU_FAN_PWM,
-		.id	= BACKSIDE_FAN_PWM_DEFAULT_ID,
-	},
-	[DRIVES_FAN_RPM_INDEX] = {
-		.loc	= "DRIVE BAY",
-		.type	= FCU_FAN_RPM,
-		.id	= DRIVES_FAN_RPM_DEFAULT_ID,
-	},
-	[SLOTS_FAN_PWM_INDEX] = {
-		.loc	= "SLOT,PCI FAN",
-		.type	= FCU_FAN_PWM,
-		.id	= SLOTS_FAN_PWM_DEFAULT_ID,
-	},
-	[CPUA_INTAKE_FAN_RPM_INDEX] = {
-		.loc	= "CPU A INTAKE",
-		.type	= FCU_FAN_RPM,
-		.id	= CPUA_INTAKE_FAN_RPM_DEFAULT_ID,
-	},
-	[CPUA_EXHAUST_FAN_RPM_INDEX] = {
-		.loc	= "CPU A EXHAUST",
-		.type	= FCU_FAN_RPM,
-		.id	= CPUA_EXHAUST_FAN_RPM_DEFAULT_ID,
-	},
-	[CPUB_INTAKE_FAN_RPM_INDEX] = {
-		.loc	= "CPU B INTAKE",
-		.type	= FCU_FAN_RPM,
-		.id	= CPUB_INTAKE_FAN_RPM_DEFAULT_ID,
-	},
-	[CPUB_EXHAUST_FAN_RPM_INDEX] = {
-		.loc	= "CPU B EXHAUST",
-		.type	= FCU_FAN_RPM,
-		.id	= CPUB_EXHAUST_FAN_RPM_DEFAULT_ID,
-	},
-	/* pumps aren't present by default, have to be looked up in the
-	 * device-tree
-	 */
-	[CPUA_PUMP_RPM_INDEX] = {
-		.loc	= "CPU A PUMP",
-		.type	= FCU_FAN_RPM,		
-		.id	= FCU_FAN_ABSENT_ID,
-	},
-	[CPUB_PUMP_RPM_INDEX] = {
-		.loc	= "CPU B PUMP",
-		.type	= FCU_FAN_RPM,
-		.id	= FCU_FAN_ABSENT_ID,
-	},
-	/* Xserve fans */
-	[CPU_A1_FAN_RPM_INDEX] = {
-		.loc	= "CPU A 1",
-		.type	= FCU_FAN_RPM,
-		.id	= FCU_FAN_ABSENT_ID,
-	},
-	[CPU_A2_FAN_RPM_INDEX] = {
-		.loc	= "CPU A 2",
-		.type	= FCU_FAN_RPM,
-		.id	= FCU_FAN_ABSENT_ID,
-	},
-	[CPU_A3_FAN_RPM_INDEX] = {
-		.loc	= "CPU A 3",
-		.type	= FCU_FAN_RPM,
-		.id	= FCU_FAN_ABSENT_ID,
-	},
-	[CPU_B1_FAN_RPM_INDEX] = {
-		.loc	= "CPU B 1",
-		.type	= FCU_FAN_RPM,
-		.id	= FCU_FAN_ABSENT_ID,
-	},
-	[CPU_B2_FAN_RPM_INDEX] = {
-		.loc	= "CPU B 2",
-		.type	= FCU_FAN_RPM,
-		.id	= FCU_FAN_ABSENT_ID,
-	},
-	[CPU_B3_FAN_RPM_INDEX] = {
-		.loc	= "CPU B 3",
-		.type	= FCU_FAN_RPM,
-		.id	= FCU_FAN_ABSENT_ID,
-	},
-};
-
-static struct i2c_driver therm_pm72_driver;
-
-/*
- * Utility function to create an i2c_client structure and
- * attach it to one of u3 adapters
- */
-static struct i2c_client *attach_i2c_chip(int id, const char *name)
-{
-	struct i2c_client *clt;
-	struct i2c_adapter *adap;
-	struct i2c_board_info info;
-
-	if (id & 0x200)
-		adap = k2;
-	else if (id & 0x100)
-		adap = u3_1;
-	else
-		adap = u3_0;
-	if (adap == NULL)
-		return NULL;
-
-	memset(&info, 0, sizeof(struct i2c_board_info));
-	info.addr = (id >> 1) & 0x7f;
-	strlcpy(info.type, "therm_pm72", I2C_NAME_SIZE);
-	clt = i2c_new_device(adap, &info);
-	if (!clt) {
-		printk(KERN_ERR "therm_pm72: Failed to attach to i2c ID 0x%x\n", id);
-		return NULL;
-	}
-
-	/*
-	 * 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(&clt->detected, &therm_pm72_driver.clients);
-	return clt;
-}
-
-/*
- * Here are the i2c chip access wrappers
- */
-
-static void initialize_adc(struct cpu_pid_state *state)
-{
-	int rc;
-	u8 buf[2];
-
-	/* 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(state->monitor, buf, 2);
-
-	/* Read & cache Config1 */
-	buf[0] = 1;
-	rc = i2c_master_send(state->monitor, buf, 1);
-	if (rc > 0) {
-		rc = i2c_master_recv(state->monitor, buf, 1);
-		if (rc > 0) {
-			state->adc_config = buf[0];
-			DBG("ADC config reg: %02x\n", state->adc_config);
-			/* Disable shutdown mode */
-		       	state->adc_config &= 0xfe;
-			buf[0] = 1;
-			buf[1] = state->adc_config;
-			rc = i2c_master_send(state->monitor, buf, 2);
-		}
-	}
-	if (rc <= 0)
-		printk(KERN_ERR "therm_pm72: Error reading ADC config"
-		       " register !\n");
-}
-
-static int read_smon_adc(struct cpu_pid_state *state, int chan)
-{
-	int rc, data, tries = 0;
-	u8 buf[2];
-
-	for (;;) {
-		/* Set channel */
-		buf[0] = 1;
-		buf[1] = (state->adc_config & 0x1f) | (chan << 5);
-		rc = i2c_master_send(state->monitor, buf, 2);
-		if (rc <= 0)
-			goto error;
-		/* Wait for conversion */
-		msleep(1);
-		/* Switch to data register */
-		buf[0] = 4;
-		rc = i2c_master_send(state->monitor, buf, 1);
-		if (rc <= 0)
-			goto error;
-		/* Read result */
-		rc = i2c_master_recv(state->monitor, buf, 2);
-		if (rc < 0)
-			goto error;
-		data = ((u16)buf[0]) << 8 | (u16)buf[1];
-		return data >> 6;
-	error:
-		DBG("Error reading ADC, retrying...\n");
-		if (++tries > 10) {
-			printk(KERN_ERR "therm_pm72: Error reading ADC !\n");
-			return -1;
-		}
-		msleep(10);
-	}
-}
-
-static int read_lm87_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("Error reading LM87, retrying...\n");
-		if (++tries > 10) {
-			printk(KERN_ERR "therm_pm72: Error reading LM87 !\n");
-			return -1;
-		}
-		msleep(10);
-	}
-}
-
-static int fan_read_reg(int reg, unsigned char *buf, int nb)
-{
-	int tries, nr, nw;
-
-	buf[0] = reg;
-	tries = 0;
-	for (;;) {
-		nw = i2c_master_send(fcu, buf, 1);
-		if (nw > 0 || (nw < 0 && nw != -EIO) || tries >= 100)
-			break;
-		msleep(10);
-		++tries;
-	}
-	if (nw <= 0) {
-		printk(KERN_ERR "Failure writing address to FCU: %d", nw);
-		return -EIO;
-	}
-	tries = 0;
-	for (;;) {
-		nr = i2c_master_recv(fcu, buf, nb);
-		if (nr > 0 || (nr < 0 && nr != -ENODEV) || tries >= 100)
-			break;
-		msleep(10);
-		++tries;
-	}
-	if (nr <= 0)
-		printk(KERN_ERR "Failure reading data from FCU: %d", nw);
-	return nr;
-}
-
-static int fan_write_reg(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(fcu, buf, nb);
-		if (nw > 0 || (nw < 0 && nw != -EIO) || tries >= 100)
-			break;
-		msleep(10);
-		++tries;
-	}
-	if (nw < 0)
-		printk(KERN_ERR "Failure writing to FCU: %d", nw);
-	return nw;
-}
-
-static int start_fcu(void)
-{
-	unsigned char buf = 0xff;
-	int rc;
-
-	rc = fan_write_reg(0xe, &buf, 1);
-	if (rc < 0)
-		return -EIO;
-	rc = fan_write_reg(0x2e, &buf, 1);
-	if (rc < 0)
-		return -EIO;
-	rc = fan_read_reg(0, &buf, 1);
-	if (rc < 0)
-		return -EIO;
-	fcu_rpm_shift = (buf == 1) ? 2 : 3;
-	printk(KERN_DEBUG "FCU Initialized, RPM fan shift is %d\n",
-	       fcu_rpm_shift);
-
-	return 0;
-}
-
-static int set_rpm_fan(int fan_index, int rpm)
-{
-	unsigned char buf[2];
-	int rc, id, min, max;
-
-	if (fcu_fans[fan_index].type != FCU_FAN_RPM)
-		return -EINVAL;
-	id = fcu_fans[fan_index].id; 
-	if (id == FCU_FAN_ABSENT_ID)
-		return -EINVAL;
-
-	min = 2400 >> fcu_rpm_shift;
-	max = 56000 >> fcu_rpm_shift;
-
-	if (rpm < min)
-		rpm = min;
-	else if (rpm > max)
-		rpm = max;
-	buf[0] = rpm >> (8 - fcu_rpm_shift);
-	buf[1] = rpm << fcu_rpm_shift;
-	rc = fan_write_reg(0x10 + (id * 2), buf, 2);
-	if (rc < 0)
-		return -EIO;
-	return 0;
-}
-
-static int get_rpm_fan(int fan_index, int programmed)
-{
-	unsigned char failure;
-	unsigned char active;
-	unsigned char buf[2];
-	int rc, id, reg_base;
-
-	if (fcu_fans[fan_index].type != FCU_FAN_RPM)
-		return -EINVAL;
-	id = fcu_fans[fan_index].id; 
-	if (id == FCU_FAN_ABSENT_ID)
-		return -EINVAL;
-
-	rc = fan_read_reg(0xb, &failure, 1);
-	if (rc != 1)
-		return -EIO;
-	if ((failure & (1 << id)) != 0)
-		return -EFAULT;
-	rc = fan_read_reg(0xd, &active, 1);
-	if (rc != 1)
-		return -EIO;
-	if ((active & (1 << id)) == 0)
-		return -ENXIO;
-
-	/* Programmed value or real current speed */
-	reg_base = programmed ? 0x10 : 0x11;
-	rc = fan_read_reg(reg_base + (id * 2), buf, 2);
-	if (rc != 2)
-		return -EIO;
-
-	return (buf[0] << (8 - fcu_rpm_shift)) | buf[1] >> fcu_rpm_shift;
-}
-
-static int set_pwm_fan(int fan_index, int pwm)
-{
-	unsigned char buf[2];
-	int rc, id;
-
-	if (fcu_fans[fan_index].type != FCU_FAN_PWM)
-		return -EINVAL;
-	id = fcu_fans[fan_index].id; 
-	if (id == FCU_FAN_ABSENT_ID)
-		return -EINVAL;
-
-	if (pwm < 10)
-		pwm = 10;
-	else if (pwm > 100)
-		pwm = 100;
-	pwm = (pwm * 2559) / 1000;
-	buf[0] = pwm;
-	rc = fan_write_reg(0x30 + (id * 2), buf, 1);
-	if (rc < 0)
-		return rc;
-	return 0;
-}
-
-static int get_pwm_fan(int fan_index)
-{
-	unsigned char failure;
-	unsigned char active;
-	unsigned char buf[2];
-	int rc, id;
-
-	if (fcu_fans[fan_index].type != FCU_FAN_PWM)
-		return -EINVAL;
-	id = fcu_fans[fan_index].id; 
-	if (id == FCU_FAN_ABSENT_ID)
-		return -EINVAL;
-
-	rc = fan_read_reg(0x2b, &failure, 1);
-	if (rc != 1)
-		return -EIO;
-	if ((failure & (1 << id)) != 0)
-		return -EFAULT;
-	rc = fan_read_reg(0x2d, &active, 1);
-	if (rc != 1)
-		return -EIO;
-	if ((active & (1 << id)) == 0)
-		return -ENXIO;
-
-	/* Programmed value or real current speed */
-	rc = fan_read_reg(0x30 + (id * 2), buf, 1);
-	if (rc != 1)
-		return -EIO;
-
-	return (buf[0] * 1000) / 2559;
-}
-
-static void tickle_fcu(void)
-{
-	int pwm;
-
-	pwm = get_pwm_fan(SLOTS_FAN_PWM_INDEX);
-
-	DBG("FCU Tickle, slots fan is: %d\n", pwm);
-	if (pwm < 0)
-		pwm = 100;
-
-	if (!rackmac) {
-		pwm = SLOTS_FAN_DEFAULT_PWM;
-	} else if (pwm < SLOTS_PID_OUTPUT_MIN)
-		pwm = SLOTS_PID_OUTPUT_MIN;
-
-	/* That is hopefully enough to make the FCU happy */
-	set_pwm_fan(SLOTS_FAN_PWM_INDEX, pwm);
-}
-
-
-/*
- * Utility routine to read the CPU calibration EEPROM data
- * from the device-tree
- */
-static int read_eeprom(int cpu, struct mpu_data *out)
-{
-	struct device_node *np;
-	char nodename[64];
-	const u8 *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 == NULL) {
-		printk(KERN_ERR "therm_pm72: Failed to retrieve cpuid node from device-tree\n");
-		return -ENODEV;
-	}
-	data = of_get_property(np, "cpuid", &len);
-	if (data == NULL) {
-		printk(KERN_ERR "therm_pm72: Failed to retrieve cpuid property from device-tree\n");
-		of_node_put(np);
-		return -ENODEV;
-	}
-	memcpy(out, data, sizeof(struct mpu_data));
-	of_node_put(np);
-	
-	return 0;
-}
-
-static void fetch_cpu_pumps_minmax(void)
-{
-	struct cpu_pid_state *state0 = &processor_state[0];
-	struct cpu_pid_state *state1 = &processor_state[1];
-	u16 pump_min = 0, pump_max = 0xffff;
-	u16 tmp[4];
-
-	/* Try to fetch pumps min/max infos from eeprom */
-
-	memcpy(&tmp, &state0->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;
-	}
-
-	state0->pump_min = state1->pump_min = pump_min;
-	state0->pump_max = state1->pump_max = pump_max;
-}
-
-/* 
- * 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...
- *
- * That sucks a bit, we take the lock because FIX32TOPRINT evaluates
- * the input twice... I accept patches :)
- */
-#define BUILD_SHOW_FUNC_FIX(name, data)				\
-static ssize_t show_##name(struct device *dev, struct device_attribute *attr, char *buf)	\
-{								\
-	ssize_t r;						\
-	mutex_lock(&driver_lock);					\
-	r = sprintf(buf, "%d.%03d", FIX32TOPRINT(data));	\
-	mutex_unlock(&driver_lock);					\
-	return r;						\
-}
-#define BUILD_SHOW_FUNC_INT(name, data)				\
-static ssize_t show_##name(struct device *dev, struct device_attribute *attr, char *buf)	\
-{								\
-	return sprintf(buf, "%d", data);			\
-}
-
-BUILD_SHOW_FUNC_FIX(cpu0_temperature, processor_state[0].last_temp)
-BUILD_SHOW_FUNC_FIX(cpu0_voltage, processor_state[0].voltage)
-BUILD_SHOW_FUNC_FIX(cpu0_current, processor_state[0].current_a)
-BUILD_SHOW_FUNC_INT(cpu0_exhaust_fan_rpm, processor_state[0].rpm)
-BUILD_SHOW_FUNC_INT(cpu0_intake_fan_rpm, processor_state[0].intake_rpm)
-
-BUILD_SHOW_FUNC_FIX(cpu1_temperature, processor_state[1].last_temp)
-BUILD_SHOW_FUNC_FIX(cpu1_voltage, processor_state[1].voltage)
-BUILD_SHOW_FUNC_FIX(cpu1_current, processor_state[1].current_a)
-BUILD_SHOW_FUNC_INT(cpu1_exhaust_fan_rpm, processor_state[1].rpm)
-BUILD_SHOW_FUNC_INT(cpu1_intake_fan_rpm, processor_state[1].intake_rpm)
-
-BUILD_SHOW_FUNC_FIX(backside_temperature, backside_state.last_temp)
-BUILD_SHOW_FUNC_INT(backside_fan_pwm, backside_state.pwm)
-
-BUILD_SHOW_FUNC_FIX(drives_temperature, drives_state.last_temp)
-BUILD_SHOW_FUNC_INT(drives_fan_rpm, drives_state.rpm)
-
-BUILD_SHOW_FUNC_FIX(slots_temperature, slots_state.last_temp)
-BUILD_SHOW_FUNC_INT(slots_fan_pwm, slots_state.pwm)
-
-BUILD_SHOW_FUNC_FIX(dimms_temperature, dimms_state.last_temp)
-
-static DEVICE_ATTR(cpu0_temperature,S_IRUGO,show_cpu0_temperature,NULL);
-static DEVICE_ATTR(cpu0_voltage,S_IRUGO,show_cpu0_voltage,NULL);
-static DEVICE_ATTR(cpu0_current,S_IRUGO,show_cpu0_current,NULL);
-static DEVICE_ATTR(cpu0_exhaust_fan_rpm,S_IRUGO,show_cpu0_exhaust_fan_rpm,NULL);
-static DEVICE_ATTR(cpu0_intake_fan_rpm,S_IRUGO,show_cpu0_intake_fan_rpm,NULL);
-
-static DEVICE_ATTR(cpu1_temperature,S_IRUGO,show_cpu1_temperature,NULL);
-static DEVICE_ATTR(cpu1_voltage,S_IRUGO,show_cpu1_voltage,NULL);
-static DEVICE_ATTR(cpu1_current,S_IRUGO,show_cpu1_current,NULL);
-static DEVICE_ATTR(cpu1_exhaust_fan_rpm,S_IRUGO,show_cpu1_exhaust_fan_rpm,NULL);
-static DEVICE_ATTR(cpu1_intake_fan_rpm,S_IRUGO,show_cpu1_intake_fan_rpm,NULL);
-
-static DEVICE_ATTR(backside_temperature,S_IRUGO,show_backside_temperature,NULL);
-static DEVICE_ATTR(backside_fan_pwm,S_IRUGO,show_backside_fan_pwm,NULL);
-
-static DEVICE_ATTR(drives_temperature,S_IRUGO,show_drives_temperature,NULL);
-static DEVICE_ATTR(drives_fan_rpm,S_IRUGO,show_drives_fan_rpm,NULL);
-
-static DEVICE_ATTR(slots_temperature,S_IRUGO,show_slots_temperature,NULL);
-static DEVICE_ATTR(slots_fan_pwm,S_IRUGO,show_slots_fan_pwm,NULL);
-
-static DEVICE_ATTR(dimms_temperature,S_IRUGO,show_dimms_temperature,NULL);
-
-/*
- * CPUs fans control loop
- */
-
-static int do_read_one_cpu_values(struct cpu_pid_state *state, s32 *temp, s32 *power)
-{
-	s32 ltemp, volts, amps;
-	int index, rc = 0;
-
-	/* Default (in case of error) */
-	*temp = state->cur_temp;
-	*power = state->cur_power;
-
-	if (cpu_pid_type == CPU_PID_TYPE_RACKMAC)
-		index = (state->index == 0) ?
-			CPU_A1_FAN_RPM_INDEX : CPU_B1_FAN_RPM_INDEX;
-	else
-		index = (state->index == 0) ?
-			CPUA_EXHAUST_FAN_RPM_INDEX : CPUB_EXHAUST_FAN_RPM_INDEX;
-
-	/* Read current fan status */
-	rc = get_rpm_fan(index, !RPM_PID_USE_ACTUAL_SPEED);
-	if (rc < 0) {
-		/* XXX What do we do now ? Nothing for now, keep old value, but
-		 * return error upstream
-		 */
-		DBG("  cpu %d, fan reading error !\n", state->index);
-	} else {
-		state->rpm = rc;
-		DBG("  cpu %d, exhaust RPM: %d\n", state->index, state->rpm);
-	}
-
-	/* Get some sensor readings and scale it */
-	ltemp = read_smon_adc(state, 1);
-	if (ltemp == -1) {
-		/* XXX What do we do now ? */
-		state->overtemp++;
-		if (rc == 0)
-			rc = -EIO;
-		DBG("  cpu %d, temp reading error !\n", state->index);
-	} else {
-		/* Fixup temperature according to diode calibration
-		 */
-		DBG("  cpu %d, temp raw: %04x, m_diode: %04x, b_diode: %04x\n",
-		    state->index,
-		    ltemp, state->mpu.mdiode, state->mpu.bdiode);
-		*temp = ((s32)ltemp * (s32)state->mpu.mdiode + ((s32)state->mpu.bdiode << 12)) >> 2;
-		state->last_temp = *temp;
-		DBG("  temp: %d.%03d\n", FIX32TOPRINT((*temp)));
-	}
-
-	/*
-	 * Read voltage & current and calculate power
-	 */
-	volts = read_smon_adc(state, 3);
-	amps = read_smon_adc(state, 4);
-
-	/* Scale voltage and current raw sensor values according to fixed scales
-	 * obtained in Darwin and calculate power from I and V
-	 */
-	volts *= ADC_CPU_VOLTAGE_SCALE;
-	amps *= ADC_CPU_CURRENT_SCALE;
-	*power = (((u64)volts) * ((u64)amps)) >> 16;
-	state->voltage = volts;
-	state->current_a = amps;
-	state->last_power = *power;
-
-	DBG("  cpu %d, current: %d.%03d, voltage: %d.%03d, power: %d.%03d W\n",
-	    state->index, FIX32TOPRINT(state->current_a),
-	    FIX32TOPRINT(state->voltage), FIX32TOPRINT(*power));
-
-	return 0;
-}
-
-static void do_cpu_pid(struct cpu_pid_state *state, s32 temp, s32 power)
-{
-	s32 power_target, integral, derivative, proportional, adj_in_target, sval;
-	s64 integ_p, deriv_p, prop_p, sum; 
-	int i;
-
-	/* Calculate power target value (could be done once for all)
-	 * and convert to a 16.16 fp number
-	 */
-	power_target = ((u32)(state->mpu.pmaxh - state->mpu.padjmax)) << 16;
-	DBG("  power target: %d.%03d, error: %d.%03d\n",
-	    FIX32TOPRINT(power_target), FIX32TOPRINT(power_target - power));
-
-	/* Store temperature and power in history array */
-	state->cur_temp = (state->cur_temp + 1) % CPU_TEMP_HISTORY_SIZE;
-	state->temp_history[state->cur_temp] = temp;
-	state->cur_power = (state->cur_power + 1) % state->count_power;
-	state->power_history[state->cur_power] = power;
-	state->error_history[state->cur_power] = power_target - power;
-	
-	/* If first loop, fill the history table */
-	if (state->first) {
-		for (i = 0; i < (state->count_power - 1); i++) {
-			state->cur_power = (state->cur_power + 1) % state->count_power;
-			state->power_history[state->cur_power] = power;
-			state->error_history[state->cur_power] = power_target - power;
-		}
-		for (i = 0; i < (CPU_TEMP_HISTORY_SIZE - 1); i++) {
-			state->cur_temp = (state->cur_temp + 1) % CPU_TEMP_HISTORY_SIZE;
-			state->temp_history[state->cur_temp] = temp;			
-		}
-		state->first = 0;
-	}
-
-	/* Calculate the integral term normally based on the "power" values */
-	sum = 0;
-	integral = 0;
-	for (i = 0; i < state->count_power; i++)
-		integral += state->error_history[i];
-	integral *= CPU_PID_INTERVAL;
-	DBG("  integral: %08x\n", integral);
-
-	/* Calculate the adjusted input (sense value).
-	 *   G_r is 12.20
-	 *   integ is 16.16
-	 *   so the result is 28.36
-	 *
-	 * input target is mpu.ttarget, input max is mpu.tmax
-	 */
-	integ_p = ((s64)state->mpu.pid_gr) * (s64)integral;
-	DBG("   integ_p: %d\n", (int)(integ_p >> 36));
-	sval = (state->mpu.tmax << 16) - ((integ_p >> 20) & 0xffffffff);
-	adj_in_target = (state->mpu.ttarget << 16);
-	if (adj_in_target > sval)
-		adj_in_target = sval;
-	DBG("   adj_in_target: %d.%03d, ttarget: %d\n", FIX32TOPRINT(adj_in_target),
-	    state->mpu.ttarget);
-
-	/* Calculate the derivative term */
-	derivative = state->temp_history[state->cur_temp] -
-		state->temp_history[(state->cur_temp + CPU_TEMP_HISTORY_SIZE - 1)
-				    % CPU_TEMP_HISTORY_SIZE];
-	derivative /= CPU_PID_INTERVAL;
-	deriv_p = ((s64)state->mpu.pid_gd) * (s64)derivative;
-	DBG("   deriv_p: %d\n", (int)(deriv_p >> 36));
-	sum += deriv_p;
-
-	/* Calculate the proportional term */
-	proportional = temp - adj_in_target;
-	prop_p = ((s64)state->mpu.pid_gp) * (s64)proportional;
-	DBG("   prop_p: %d\n", (int)(prop_p >> 36));
-	sum += prop_p;
-
-	/* Scale sum */
-	sum >>= 36;
-
-	DBG("   sum: %d\n", (int)sum);
-	state->rpm += (s32)sum;
-}
-
-static void do_monitor_cpu_combined(void)
-{
-	struct cpu_pid_state *state0 = &processor_state[0];
-	struct cpu_pid_state *state1 = &processor_state[1];
-	s32 temp0, power0, temp1, power1;
-	s32 temp_combi, power_combi;
-	int rc, intake, pump;
-
-	rc = do_read_one_cpu_values(state0, &temp0, &power0);
-	if (rc < 0) {
-		/* XXX What do we do now ? */
-	}
-	state1->overtemp = 0;
-	rc = do_read_one_cpu_values(state1, &temp1, &power1);
-	if (rc < 0) {
-		/* XXX What do we do now ? */
-	}
-	if (state1->overtemp)
-		state0->overtemp++;
-
-	temp_combi = max(temp0, temp1);
-	power_combi = max(power0, power1);
-
-	/* Check tmax, increment overtemp if we are there. At tmax+8, we go
-	 * full blown immediately and try to trigger a shutdown
-	 */
-	if (temp_combi >= ((state0->mpu.tmax + 8) << 16)) {
-		printk(KERN_WARNING "Warning ! Temperature way above maximum (%d) !\n",
-		       temp_combi >> 16);
-		state0->overtemp += CPU_MAX_OVERTEMP / 4;
-	} else if (temp_combi > (state0->mpu.tmax << 16)) {
-		state0->overtemp++;
-		printk(KERN_WARNING "Temperature %d above max %d. overtemp %d\n",
-		       temp_combi >> 16, state0->mpu.tmax, state0->overtemp);
-	} else {
-		if (state0->overtemp)
-			printk(KERN_WARNING "Temperature back down to %d\n",
-			       temp_combi >> 16);
-		state0->overtemp = 0;
-	}
-	if (state0->overtemp >= CPU_MAX_OVERTEMP)
-		critical_state = 1;
-	if (state0->overtemp > 0) {
-		state0->rpm = state0->mpu.rmaxn_exhaust_fan;
-		state0->intake_rpm = intake = state0->mpu.rmaxn_intake_fan;
-		pump = state0->pump_max;
-		goto do_set_fans;
-	}
-
-	/* Do the PID */
-	do_cpu_pid(state0, temp_combi, power_combi);
-
-	/* Range check */
-	state0->rpm = max(state0->rpm, (int)state0->mpu.rminn_exhaust_fan);
-	state0->rpm = min(state0->rpm, (int)state0->mpu.rmaxn_exhaust_fan);
-
-	/* Calculate intake fan speed */
-	intake = (state0->rpm * CPU_INTAKE_SCALE) >> 16;
-	intake = max(intake, (int)state0->mpu.rminn_intake_fan);
-	intake = min(intake, (int)state0->mpu.rmaxn_intake_fan);
-	state0->intake_rpm = intake;
-
-	/* Calculate pump speed */
-	pump = (state0->rpm * state0->pump_max) /
-		state0->mpu.rmaxn_exhaust_fan;
-	pump = min(pump, state0->pump_max);
-	pump = max(pump, state0->pump_min);
-	
- do_set_fans:
-	/* We copy values from state 0 to state 1 for /sysfs */
-	state1->rpm = state0->rpm;
-	state1->intake_rpm = state0->intake_rpm;
-
-	DBG("** CPU %d RPM: %d Ex, %d, Pump: %d, In, overtemp: %d\n",
-	    state1->index, (int)state1->rpm, intake, pump, state1->overtemp);
-
-	/* We should check for errors, shouldn't we ? But then, what
-	 * do we do once the error occurs ? For FCU notified fan
-	 * failures (-EFAULT) we probably want to notify userland
-	 * some way...
-	 */
-	set_rpm_fan(CPUA_INTAKE_FAN_RPM_INDEX, intake);
-	set_rpm_fan(CPUA_EXHAUST_FAN_RPM_INDEX, state0->rpm);
-	set_rpm_fan(CPUB_INTAKE_FAN_RPM_INDEX, intake);
-	set_rpm_fan(CPUB_EXHAUST_FAN_RPM_INDEX, state0->rpm);
-
-	if (fcu_fans[CPUA_PUMP_RPM_INDEX].id != FCU_FAN_ABSENT_ID)
-		set_rpm_fan(CPUA_PUMP_RPM_INDEX, pump);
-	if (fcu_fans[CPUB_PUMP_RPM_INDEX].id != FCU_FAN_ABSENT_ID)
-		set_rpm_fan(CPUB_PUMP_RPM_INDEX, pump);
-}
-
-static void do_monitor_cpu_split(struct cpu_pid_state *state)
-{
-	s32 temp, power;
-	int rc, intake;
-
-	/* Read current fan status */
-	rc = do_read_one_cpu_values(state, &temp, &power);
-	if (rc < 0) {
-		/* XXX What do we do now ? */
-	}
-
-	/* Check tmax, increment overtemp if we are there. At tmax+8, we go
-	 * full blown immediately and try to trigger a shutdown
-	 */
-	if (temp >= ((state->mpu.tmax + 8) << 16)) {
-		printk(KERN_WARNING "Warning ! CPU %d temperature way above maximum"
-		       " (%d) !\n",
-		       state->index, temp >> 16);
-		state->overtemp += CPU_MAX_OVERTEMP / 4;
-	} else if (temp > (state->mpu.tmax << 16)) {
-		state->overtemp++;
-		printk(KERN_WARNING "CPU %d temperature %d above max %d. overtemp %d\n",
-		       state->index, temp >> 16, state->mpu.tmax, state->overtemp);
-	} else {
-		if (state->overtemp)
-			printk(KERN_WARNING "CPU %d temperature back down to %d\n",
-			       state->index, temp >> 16);
-		state->overtemp = 0;
-	}
-	if (state->overtemp >= CPU_MAX_OVERTEMP)
-		critical_state = 1;
-	if (state->overtemp > 0) {
-		state->rpm = state->mpu.rmaxn_exhaust_fan;
-		state->intake_rpm = intake = state->mpu.rmaxn_intake_fan;
-		goto do_set_fans;
-	}
-
-	/* Do the PID */
-	do_cpu_pid(state, temp, power);
-
-	/* Range check */
-	state->rpm = max(state->rpm, (int)state->mpu.rminn_exhaust_fan);
-	state->rpm = min(state->rpm, (int)state->mpu.rmaxn_exhaust_fan);
-
-	/* Calculate intake fan */
-	intake = (state->rpm * CPU_INTAKE_SCALE) >> 16;
-	intake = max(intake, (int)state->mpu.rminn_intake_fan);
-	intake = min(intake, (int)state->mpu.rmaxn_intake_fan);
-	state->intake_rpm = intake;
-
- do_set_fans:
-	DBG("** CPU %d RPM: %d Ex, %d In, overtemp: %d\n",
-	    state->index, (int)state->rpm, intake, state->overtemp);
-
-	/* We should check for errors, shouldn't we ? But then, what
-	 * do we do once the error occurs ? For FCU notified fan
-	 * failures (-EFAULT) we probably want to notify userland
-	 * some way...
-	 */
-	if (state->index == 0) {
-		set_rpm_fan(CPUA_INTAKE_FAN_RPM_INDEX, intake);
-		set_rpm_fan(CPUA_EXHAUST_FAN_RPM_INDEX, state->rpm);
-	} else {
-		set_rpm_fan(CPUB_INTAKE_FAN_RPM_INDEX, intake);
-		set_rpm_fan(CPUB_EXHAUST_FAN_RPM_INDEX, state->rpm);
-	}
-}
-
-static void do_monitor_cpu_rack(struct cpu_pid_state *state)
-{
-	s32 temp, power, fan_min;
-	int rc;
-
-	/* Read current fan status */
-	rc = do_read_one_cpu_values(state, &temp, &power);
-	if (rc < 0) {
-		/* XXX What do we do now ? */
-	}
-
-	/* Check tmax, increment overtemp if we are there. At tmax+8, we go
-	 * full blown immediately and try to trigger a shutdown
-	 */
-	if (temp >= ((state->mpu.tmax + 8) << 16)) {
-		printk(KERN_WARNING "Warning ! CPU %d temperature way above maximum"
-		       " (%d) !\n",
-		       state->index, temp >> 16);
-		state->overtemp = CPU_MAX_OVERTEMP / 4;
-	} else if (temp > (state->mpu.tmax << 16)) {
-		state->overtemp++;
-		printk(KERN_WARNING "CPU %d temperature %d above max %d. overtemp %d\n",
-		       state->index, temp >> 16, state->mpu.tmax, state->overtemp);
-	} else {
-		if (state->overtemp)
-			printk(KERN_WARNING "CPU %d temperature back down to %d\n",
-			       state->index, temp >> 16);
-		state->overtemp = 0;
-	}
-	if (state->overtemp >= CPU_MAX_OVERTEMP)
-		critical_state = 1;
-	if (state->overtemp > 0) {
-		state->rpm = state->intake_rpm = state->mpu.rmaxn_intake_fan;
-		goto do_set_fans;
-	}
-
-	/* Do the PID */
-	do_cpu_pid(state, temp, power);
-
-	/* Check clamp from dimms */
-	fan_min = dimm_output_clamp;
-	fan_min = max(fan_min, (int)state->mpu.rminn_intake_fan);
-
-	DBG(" CPU min mpu = %d, min dimm = %d\n",
-	    state->mpu.rminn_intake_fan, dimm_output_clamp);
-
-	state->rpm = max(state->rpm, (int)fan_min);
-	state->rpm = min(state->rpm, (int)state->mpu.rmaxn_intake_fan);
-	state->intake_rpm = state->rpm;
-
- do_set_fans:
-	DBG("** CPU %d RPM: %d overtemp: %d\n",
-	    state->index, (int)state->rpm, state->overtemp);
-
-	/* We should check for errors, shouldn't we ? But then, what
-	 * do we do once the error occurs ? For FCU notified fan
-	 * failures (-EFAULT) we probably want to notify userland
-	 * some way...
-	 */
-	if (state->index == 0) {
-		set_rpm_fan(CPU_A1_FAN_RPM_INDEX, state->rpm);
-		set_rpm_fan(CPU_A2_FAN_RPM_INDEX, state->rpm);
-		set_rpm_fan(CPU_A3_FAN_RPM_INDEX, state->rpm);
-	} else {
-		set_rpm_fan(CPU_B1_FAN_RPM_INDEX, state->rpm);
-		set_rpm_fan(CPU_B2_FAN_RPM_INDEX, state->rpm);
-		set_rpm_fan(CPU_B3_FAN_RPM_INDEX, state->rpm);
-	}
-}
-
-/*
- * Initialize the state structure for one CPU control loop
- */
-static int init_processor_state(struct cpu_pid_state *state, int index)
-{
-	int err;
-
-	state->index = index;
-	state->first = 1;
-	state->rpm = (cpu_pid_type == CPU_PID_TYPE_RACKMAC) ? 4000 : 1000;
-	state->overtemp = 0;
-	state->adc_config = 0x00;
-
-
-	if (index == 0)
-		state->monitor = attach_i2c_chip(SUPPLY_MONITOR_ID, "CPU0_monitor");
-	else if (index == 1)
-		state->monitor = attach_i2c_chip(SUPPLY_MONITORB_ID, "CPU1_monitor");
-	if (state->monitor == NULL)
-		goto fail;
-
-	if (read_eeprom(index, &state->mpu))
-		goto fail;
-
-	state->count_power = state->mpu.tguardband;
-	if (state->count_power > CPU_POWER_HISTORY_SIZE) {
-		printk(KERN_WARNING "Warning ! too many power history slots\n");
-		state->count_power = CPU_POWER_HISTORY_SIZE;
-	}
-	DBG("CPU %d Using %d power history entries\n", index, state->count_power);
-
-	if (index == 0) {
-		err = device_create_file(&of_dev->dev, &dev_attr_cpu0_temperature);
-		err |= device_create_file(&of_dev->dev, &dev_attr_cpu0_voltage);
-		err |= device_create_file(&of_dev->dev, &dev_attr_cpu0_current);
-		err |= device_create_file(&of_dev->dev, &dev_attr_cpu0_exhaust_fan_rpm);
-		err |= device_create_file(&of_dev->dev, &dev_attr_cpu0_intake_fan_rpm);
-	} else {
-		err = device_create_file(&of_dev->dev, &dev_attr_cpu1_temperature);
-		err |= device_create_file(&of_dev->dev, &dev_attr_cpu1_voltage);
-		err |= device_create_file(&of_dev->dev, &dev_attr_cpu1_current);
-		err |= device_create_file(&of_dev->dev, &dev_attr_cpu1_exhaust_fan_rpm);
-		err |= device_create_file(&of_dev->dev, &dev_attr_cpu1_intake_fan_rpm);
-	}
-	if (err)
-		printk(KERN_WARNING "Failed to create some of the attribute"
-			"files for CPU %d\n", index);
-
-	return 0;
- fail:
-	state->monitor = NULL;
-	
-	return -ENODEV;
-}
-
-/*
- * Dispose of the state data for one CPU control loop
- */
-static void dispose_processor_state(struct cpu_pid_state *state)
-{
-	if (state->monitor == NULL)
-		return;
-
-	if (state->index == 0) {
-		device_remove_file(&of_dev->dev, &dev_attr_cpu0_temperature);
-		device_remove_file(&of_dev->dev, &dev_attr_cpu0_voltage);
-		device_remove_file(&of_dev->dev, &dev_attr_cpu0_current);
-		device_remove_file(&of_dev->dev, &dev_attr_cpu0_exhaust_fan_rpm);
-		device_remove_file(&of_dev->dev, &dev_attr_cpu0_intake_fan_rpm);
-	} else {
-		device_remove_file(&of_dev->dev, &dev_attr_cpu1_temperature);
-		device_remove_file(&of_dev->dev, &dev_attr_cpu1_voltage);
-		device_remove_file(&of_dev->dev, &dev_attr_cpu1_current);
-		device_remove_file(&of_dev->dev, &dev_attr_cpu1_exhaust_fan_rpm);
-		device_remove_file(&of_dev->dev, &dev_attr_cpu1_intake_fan_rpm);
-	}
-
-	state->monitor = NULL;
-}
-
-/*
- * Motherboard backside & U3 heatsink fan control loop
- */
-static void do_monitor_backside(struct backside_pid_state *state)
-{
-	s32 temp, integral, derivative, fan_min;
-	s64 integ_p, deriv_p, prop_p, sum; 
-	int i, rc;
-
-	if (--state->ticks != 0)
-		return;
-	state->ticks = backside_params.interval;
-
-	DBG("backside:\n");
-
-	/* Check fan status */
-	rc = get_pwm_fan(BACKSIDE_FAN_PWM_INDEX);
-	if (rc < 0) {
-		printk(KERN_WARNING "Error %d reading backside fan !\n", rc);
-		/* XXX What do we do now ? */
-	} else
-		state->pwm = rc;
-	DBG("  current pwm: %d\n", state->pwm);
-
-	/* Get some sensor readings */
-	temp = i2c_smbus_read_byte_data(state->monitor, MAX6690_EXT_TEMP) << 16;
-	state->last_temp = temp;
-	DBG("  temp: %d.%03d, target: %d.%03d\n", FIX32TOPRINT(temp),
-	    FIX32TOPRINT(backside_params.input_target));
-
-	/* Store temperature and error in history array */
-	state->cur_sample = (state->cur_sample + 1) % BACKSIDE_PID_HISTORY_SIZE;
-	state->sample_history[state->cur_sample] = temp;
-	state->error_history[state->cur_sample] = temp - backside_params.input_target;
-	
-	/* If first loop, fill the history table */
-	if (state->first) {
-		for (i = 0; i < (BACKSIDE_PID_HISTORY_SIZE - 1); i++) {
-			state->cur_sample = (state->cur_sample + 1) %
-				BACKSIDE_PID_HISTORY_SIZE;
-			state->sample_history[state->cur_sample] = temp;
-			state->error_history[state->cur_sample] =
-				temp - backside_params.input_target;
-		}
-		state->first = 0;
-	}
-
-	/* Calculate the integral term */
-	sum = 0;
-	integral = 0;
-	for (i = 0; i < BACKSIDE_PID_HISTORY_SIZE; i++)
-		integral += state->error_history[i];
-	integral *= backside_params.interval;
-	DBG("  integral: %08x\n", integral);
-	integ_p = ((s64)backside_params.G_r) * (s64)integral;
-	DBG("   integ_p: %d\n", (int)(integ_p >> 36));
-	sum += integ_p;
-
-	/* Calculate the derivative term */
-	derivative = state->error_history[state->cur_sample] -
-		state->error_history[(state->cur_sample + BACKSIDE_PID_HISTORY_SIZE - 1)
-				    % BACKSIDE_PID_HISTORY_SIZE];
-	derivative /= backside_params.interval;
-	deriv_p = ((s64)backside_params.G_d) * (s64)derivative;
-	DBG("   deriv_p: %d\n", (int)(deriv_p >> 36));
-	sum += deriv_p;
-
-	/* Calculate the proportional term */
-	prop_p = ((s64)backside_params.G_p) * (s64)(state->error_history[state->cur_sample]);
-	DBG("   prop_p: %d\n", (int)(prop_p >> 36));
-	sum += prop_p;
-
-	/* Scale sum */
-	sum >>= 36;
-
-	DBG("   sum: %d\n", (int)sum);
-	if (backside_params.additive)
-		state->pwm += (s32)sum;
-	else
-		state->pwm = sum;
-
-	/* Check for clamp */
-	fan_min = (dimm_output_clamp * 100) / 14000;
-	fan_min = max(fan_min, backside_params.output_min);
-
-	state->pwm = max(state->pwm, fan_min);
-	state->pwm = min(state->pwm, backside_params.output_max);
-
-	DBG("** BACKSIDE PWM: %d\n", (int)state->pwm);
-	set_pwm_fan(BACKSIDE_FAN_PWM_INDEX, state->pwm);
-}
-
-/*
- * Initialize the state structure for the backside fan control loop
- */
-static int init_backside_state(struct backside_pid_state *state)
-{
-	struct device_node *u3;
-	int u3h = 1; /* conservative by default */
-	int err;
-
-	/*
-	 * There are different PID params for machines with U3 and machines
-	 * with U3H, pick the right ones now
-	 */
-	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);
-	}
-
-	if (rackmac) {
-		backside_params.G_d = BACKSIDE_PID_RACK_G_d;
-		backside_params.input_target = BACKSIDE_PID_RACK_INPUT_TARGET;
-		backside_params.output_min = BACKSIDE_PID_U3H_OUTPUT_MIN;
-		backside_params.interval = BACKSIDE_PID_RACK_INTERVAL;
-		backside_params.G_p = BACKSIDE_PID_RACK_G_p;
-		backside_params.G_r = BACKSIDE_PID_G_r;
-		backside_params.output_max = BACKSIDE_PID_OUTPUT_MAX;
-		backside_params.additive = 0;
-	} else if (u3h) {
-		backside_params.G_d = BACKSIDE_PID_U3H_G_d;
-		backside_params.input_target = BACKSIDE_PID_U3H_INPUT_TARGET;
-		backside_params.output_min = BACKSIDE_PID_U3H_OUTPUT_MIN;
-		backside_params.interval = BACKSIDE_PID_INTERVAL;
-		backside_params.G_p = BACKSIDE_PID_G_p;
-		backside_params.G_r = BACKSIDE_PID_G_r;
-		backside_params.output_max = BACKSIDE_PID_OUTPUT_MAX;
-		backside_params.additive = 1;
-	} else {
-		backside_params.G_d = BACKSIDE_PID_U3_G_d;
-		backside_params.input_target = BACKSIDE_PID_U3_INPUT_TARGET;
-		backside_params.output_min = BACKSIDE_PID_U3_OUTPUT_MIN;
-		backside_params.interval = BACKSIDE_PID_INTERVAL;
-		backside_params.G_p = BACKSIDE_PID_G_p;
-		backside_params.G_r = BACKSIDE_PID_G_r;
-		backside_params.output_max = BACKSIDE_PID_OUTPUT_MAX;
-		backside_params.additive = 1;
-	}
-
-	state->ticks = 1;
-	state->first = 1;
-	state->pwm = 50;
-
-	state->monitor = attach_i2c_chip(BACKSIDE_MAX_ID, "backside_temp");
-	if (state->monitor == NULL)
-		return -ENODEV;
-
-	err = device_create_file(&of_dev->dev, &dev_attr_backside_temperature);
-	err |= device_create_file(&of_dev->dev, &dev_attr_backside_fan_pwm);
-	if (err)
-		printk(KERN_WARNING "Failed to create attribute file(s)"
-			" for backside fan\n");
-
-	return 0;
-}
-
-/*
- * Dispose of the state data for the backside control loop
- */
-static void dispose_backside_state(struct backside_pid_state *state)
-{
-	if (state->monitor == NULL)
-		return;
-
-	device_remove_file(&of_dev->dev, &dev_attr_backside_temperature);
-	device_remove_file(&of_dev->dev, &dev_attr_backside_fan_pwm);
-
-	state->monitor = NULL;
-}
- 
-/*
- * Drives bay fan control loop
- */
-static void do_monitor_drives(struct drives_pid_state *state)
-{
-	s32 temp, integral, derivative;
-	s64 integ_p, deriv_p, prop_p, sum; 
-	int i, rc;
-
-	if (--state->ticks != 0)
-		return;
-	state->ticks = DRIVES_PID_INTERVAL;
-
-	DBG("drives:\n");
-
-	/* Check fan status */
-	rc = get_rpm_fan(DRIVES_FAN_RPM_INDEX, !RPM_PID_USE_ACTUAL_SPEED);
-	if (rc < 0) {
-		printk(KERN_WARNING "Error %d reading drives fan !\n", rc);
-		/* XXX What do we do now ? */
-	} else
-		state->rpm = rc;
-	DBG("  current rpm: %d\n", state->rpm);
-
-	/* Get some sensor readings */
-	temp = le16_to_cpu(i2c_smbus_read_word_data(state->monitor,
-						    DS1775_TEMP)) << 8;
-	state->last_temp = temp;
-	DBG("  temp: %d.%03d, target: %d.%03d\n", FIX32TOPRINT(temp),
-	    FIX32TOPRINT(DRIVES_PID_INPUT_TARGET));
-
-	/* Store temperature and error in history array */
-	state->cur_sample = (state->cur_sample + 1) % DRIVES_PID_HISTORY_SIZE;
-	state->sample_history[state->cur_sample] = temp;
-	state->error_history[state->cur_sample] = temp - DRIVES_PID_INPUT_TARGET;
-	
-	/* If first loop, fill the history table */
-	if (state->first) {
-		for (i = 0; i < (DRIVES_PID_HISTORY_SIZE - 1); i++) {
-			state->cur_sample = (state->cur_sample + 1) %
-				DRIVES_PID_HISTORY_SIZE;
-			state->sample_history[state->cur_sample] = temp;
-			state->error_history[state->cur_sample] =
-				temp - DRIVES_PID_INPUT_TARGET;
-		}
-		state->first = 0;
-	}
-
-	/* Calculate the integral term */
-	sum = 0;
-	integral = 0;
-	for (i = 0; i < DRIVES_PID_HISTORY_SIZE; i++)
-		integral += state->error_history[i];
-	integral *= DRIVES_PID_INTERVAL;
-	DBG("  integral: %08x\n", integral);
-	integ_p = ((s64)DRIVES_PID_G_r) * (s64)integral;
-	DBG("   integ_p: %d\n", (int)(integ_p >> 36));
-	sum += integ_p;
-
-	/* Calculate the derivative term */
-	derivative = state->error_history[state->cur_sample] -
-		state->error_history[(state->cur_sample + DRIVES_PID_HISTORY_SIZE - 1)
-				    % DRIVES_PID_HISTORY_SIZE];
-	derivative /= DRIVES_PID_INTERVAL;
-	deriv_p = ((s64)DRIVES_PID_G_d) * (s64)derivative;
-	DBG("   deriv_p: %d\n", (int)(deriv_p >> 36));
-	sum += deriv_p;
-
-	/* Calculate the proportional term */
-	prop_p = ((s64)DRIVES_PID_G_p) * (s64)(state->error_history[state->cur_sample]);
-	DBG("   prop_p: %d\n", (int)(prop_p >> 36));
-	sum += prop_p;
-
-	/* Scale sum */
-	sum >>= 36;
-
-	DBG("   sum: %d\n", (int)sum);
-	state->rpm += (s32)sum;
-
-	state->rpm = max(state->rpm, DRIVES_PID_OUTPUT_MIN);
-	state->rpm = min(state->rpm, DRIVES_PID_OUTPUT_MAX);
-
-	DBG("** DRIVES RPM: %d\n", (int)state->rpm);
-	set_rpm_fan(DRIVES_FAN_RPM_INDEX, state->rpm);
-}
-
-/*
- * Initialize the state structure for the drives bay fan control loop
- */
-static int init_drives_state(struct drives_pid_state *state)
-{
-	int err;
-
-	state->ticks = 1;
-	state->first = 1;
-	state->rpm = 1000;
-
-	state->monitor = attach_i2c_chip(DRIVES_DALLAS_ID, "drives_temp");
-	if (state->monitor == NULL)
-		return -ENODEV;
-
-	err = device_create_file(&of_dev->dev, &dev_attr_drives_temperature);
-	err |= device_create_file(&of_dev->dev, &dev_attr_drives_fan_rpm);
-	if (err)
-		printk(KERN_WARNING "Failed to create attribute file(s)"
-			" for drives bay fan\n");
-
-	return 0;
-}
-
-/*
- * Dispose of the state data for the drives control loop
- */
-static void dispose_drives_state(struct drives_pid_state *state)
-{
-	if (state->monitor == NULL)
-		return;
-
-	device_remove_file(&of_dev->dev, &dev_attr_drives_temperature);
-	device_remove_file(&of_dev->dev, &dev_attr_drives_fan_rpm);
-
-	state->monitor = NULL;
-}
-
-/*
- * DIMMs temp control loop
- */
-static void do_monitor_dimms(struct dimm_pid_state *state)
-{
-	s32 temp, integral, derivative, fan_min;
-	s64 integ_p, deriv_p, prop_p, sum;
-	int i;
-
-	if (--state->ticks != 0)
-		return;
-	state->ticks = DIMM_PID_INTERVAL;
-
-	DBG("DIMM:\n");
-
-	DBG("  current value: %d\n", state->output);
-
-	temp = read_lm87_reg(state->monitor, LM87_INT_TEMP);
-	if (temp < 0)
-		return;
-	temp <<= 16;
-	state->last_temp = temp;
-	DBG("  temp: %d.%03d, target: %d.%03d\n", FIX32TOPRINT(temp),
-	    FIX32TOPRINT(DIMM_PID_INPUT_TARGET));
-
-	/* Store temperature and error in history array */
-	state->cur_sample = (state->cur_sample + 1) % DIMM_PID_HISTORY_SIZE;
-	state->sample_history[state->cur_sample] = temp;
-	state->error_history[state->cur_sample] = temp - DIMM_PID_INPUT_TARGET;
-
-	/* If first loop, fill the history table */
-	if (state->first) {
-		for (i = 0; i < (DIMM_PID_HISTORY_SIZE - 1); i++) {
-			state->cur_sample = (state->cur_sample + 1) %
-				DIMM_PID_HISTORY_SIZE;
-			state->sample_history[state->cur_sample] = temp;
-			state->error_history[state->cur_sample] =
-				temp - DIMM_PID_INPUT_TARGET;
-		}
-		state->first = 0;
-	}
-
-	/* Calculate the integral term */
-	sum = 0;
-	integral = 0;
-	for (i = 0; i < DIMM_PID_HISTORY_SIZE; i++)
-		integral += state->error_history[i];
-	integral *= DIMM_PID_INTERVAL;
-	DBG("  integral: %08x\n", integral);
-	integ_p = ((s64)DIMM_PID_G_r) * (s64)integral;
-	DBG("   integ_p: %d\n", (int)(integ_p >> 36));
-	sum += integ_p;
-
-	/* Calculate the derivative term */
-	derivative = state->error_history[state->cur_sample] -
-		state->error_history[(state->cur_sample + DIMM_PID_HISTORY_SIZE - 1)
-				    % DIMM_PID_HISTORY_SIZE];
-	derivative /= DIMM_PID_INTERVAL;
-	deriv_p = ((s64)DIMM_PID_G_d) * (s64)derivative;
-	DBG("   deriv_p: %d\n", (int)(deriv_p >> 36));
-	sum += deriv_p;
-
-	/* Calculate the proportional term */
-	prop_p = ((s64)DIMM_PID_G_p) * (s64)(state->error_history[state->cur_sample]);
-	DBG("   prop_p: %d\n", (int)(prop_p >> 36));
-	sum += prop_p;
-
-	/* Scale sum */
-	sum >>= 36;
-
-	DBG("   sum: %d\n", (int)sum);
-	state->output = (s32)sum;
-	state->output = max(state->output, DIMM_PID_OUTPUT_MIN);
-	state->output = min(state->output, DIMM_PID_OUTPUT_MAX);
-	dimm_output_clamp = state->output;
-
-	DBG("** DIMM clamp value: %d\n", (int)state->output);
-
-	/* Backside PID is only every 5 seconds, force backside fan clamping now */
-	fan_min = (dimm_output_clamp * 100) / 14000;
-	fan_min = max(fan_min, backside_params.output_min);
-	if (backside_state.pwm < fan_min) {
-		backside_state.pwm = fan_min;
-		DBG(" -> applying clamp to backside fan now: %d  !\n", fan_min);
-		set_pwm_fan(BACKSIDE_FAN_PWM_INDEX, fan_min);
-	}
-}
-
-/*
- * Initialize the state structure for the DIMM temp control loop
- */
-static int init_dimms_state(struct dimm_pid_state *state)
-{
-	state->ticks = 1;
-	state->first = 1;
-	state->output = 4000;
-
-	state->monitor = attach_i2c_chip(XSERVE_DIMMS_LM87, "dimms_temp");
-	if (state->monitor == NULL)
-		return -ENODEV;
-
-	if (device_create_file(&of_dev->dev, &dev_attr_dimms_temperature))
-		printk(KERN_WARNING "Failed to create attribute file"
-			" for DIMM temperature\n");
-
-	return 0;
-}
-
-/*
- * Dispose of the state data for the DIMM control loop
- */
-static void dispose_dimms_state(struct dimm_pid_state *state)
-{
-	if (state->monitor == NULL)
-		return;
-
-	device_remove_file(&of_dev->dev, &dev_attr_dimms_temperature);
-
-	state->monitor = NULL;
-}
-
-/*
- * Slots fan control loop
- */
-static void do_monitor_slots(struct slots_pid_state *state)
-{
-	s32 temp, integral, derivative;
-	s64 integ_p, deriv_p, prop_p, sum;
-	int i, rc;
-
-	if (--state->ticks != 0)
-		return;
-	state->ticks = SLOTS_PID_INTERVAL;
-
-	DBG("slots:\n");
-
-	/* Check fan status */
-	rc = get_pwm_fan(SLOTS_FAN_PWM_INDEX);
-	if (rc < 0) {
-		printk(KERN_WARNING "Error %d reading slots fan !\n", rc);
-		/* XXX What do we do now ? */
-	} else
-		state->pwm = rc;
-	DBG("  current pwm: %d\n", state->pwm);
-
-	/* Get some sensor readings */
-	temp = le16_to_cpu(i2c_smbus_read_word_data(state->monitor,
-						    DS1775_TEMP)) << 8;
-	state->last_temp = temp;
-	DBG("  temp: %d.%03d, target: %d.%03d\n", FIX32TOPRINT(temp),
-	    FIX32TOPRINT(SLOTS_PID_INPUT_TARGET));
-
-	/* Store temperature and error in history array */
-	state->cur_sample = (state->cur_sample + 1) % SLOTS_PID_HISTORY_SIZE;
-	state->sample_history[state->cur_sample] = temp;
-	state->error_history[state->cur_sample] = temp - SLOTS_PID_INPUT_TARGET;
-
-	/* If first loop, fill the history table */
-	if (state->first) {
-		for (i = 0; i < (SLOTS_PID_HISTORY_SIZE - 1); i++) {
-			state->cur_sample = (state->cur_sample + 1) %
-				SLOTS_PID_HISTORY_SIZE;
-			state->sample_history[state->cur_sample] = temp;
-			state->error_history[state->cur_sample] =
-				temp - SLOTS_PID_INPUT_TARGET;
-		}
-		state->first = 0;
-	}
-
-	/* Calculate the integral term */
-	sum = 0;
-	integral = 0;
-	for (i = 0; i < SLOTS_PID_HISTORY_SIZE; i++)
-		integral += state->error_history[i];
-	integral *= SLOTS_PID_INTERVAL;
-	DBG("  integral: %08x\n", integral);
-	integ_p = ((s64)SLOTS_PID_G_r) * (s64)integral;
-	DBG("   integ_p: %d\n", (int)(integ_p >> 36));
-	sum += integ_p;
-
-	/* Calculate the derivative term */
-	derivative = state->error_history[state->cur_sample] -
-		state->error_history[(state->cur_sample + SLOTS_PID_HISTORY_SIZE - 1)
-				    % SLOTS_PID_HISTORY_SIZE];
-	derivative /= SLOTS_PID_INTERVAL;
-	deriv_p = ((s64)SLOTS_PID_G_d) * (s64)derivative;
-	DBG("   deriv_p: %d\n", (int)(deriv_p >> 36));
-	sum += deriv_p;
-
-	/* Calculate the proportional term */
-	prop_p = ((s64)SLOTS_PID_G_p) * (s64)(state->error_history[state->cur_sample]);
-	DBG("   prop_p: %d\n", (int)(prop_p >> 36));
-	sum += prop_p;
-
-	/* Scale sum */
-	sum >>= 36;
-
-	DBG("   sum: %d\n", (int)sum);
-	state->pwm = (s32)sum;
-
-	state->pwm = max(state->pwm, SLOTS_PID_OUTPUT_MIN);
-	state->pwm = min(state->pwm, SLOTS_PID_OUTPUT_MAX);
-
-	DBG("** DRIVES PWM: %d\n", (int)state->pwm);
-	set_pwm_fan(SLOTS_FAN_PWM_INDEX, state->pwm);
-}
-
-/*
- * Initialize the state structure for the slots bay fan control loop
- */
-static int init_slots_state(struct slots_pid_state *state)
-{
-	int err;
-
-	state->ticks = 1;
-	state->first = 1;
-	state->pwm = 50;
-
-	state->monitor = attach_i2c_chip(XSERVE_SLOTS_LM75, "slots_temp");
-	if (state->monitor == NULL)
-		return -ENODEV;
-
-	err = device_create_file(&of_dev->dev, &dev_attr_slots_temperature);
-	err |= device_create_file(&of_dev->dev, &dev_attr_slots_fan_pwm);
-	if (err)
-		printk(KERN_WARNING "Failed to create attribute file(s)"
-			" for slots bay fan\n");
-
-	return 0;
-}
-
-/*
- * Dispose of the state data for the slots control loop
- */
-static void dispose_slots_state(struct slots_pid_state *state)
-{
-	if (state->monitor == NULL)
-		return;
-
-	device_remove_file(&of_dev->dev, &dev_attr_slots_temperature);
-	device_remove_file(&of_dev->dev, &dev_attr_slots_fan_pwm);
-
-	state->monitor = NULL;
-}
-
-
-static int call_critical_overtemp(void)
-{
-	char *argv[] = { critical_overtemp_path, NULL };
-	static char *envp[] = { "HOME=/",
-				"TERM=linux",
-				"PATH=/sbin:/usr/sbin:/bin:/usr/bin",
-				NULL };
-
-	return call_usermodehelper(critical_overtemp_path,
-				   argv, envp, UMH_WAIT_EXEC);
-}
-
-
-/*
- * Here's the kernel thread that calls the various control loops
- */
-static int main_control_loop(void *x)
-{
-	DBG("main_control_loop started\n");
-
-	mutex_lock(&driver_lock);
-
-	if (start_fcu() < 0) {
-		printk(KERN_ERR "kfand: failed to start FCU\n");
-		mutex_unlock(&driver_lock);
-		goto out;
-	}
-
-	/* Set the PCI fan once for now on non-RackMac */
-	if (!rackmac)
-		set_pwm_fan(SLOTS_FAN_PWM_INDEX, SLOTS_FAN_DEFAULT_PWM);
-
-	/* Initialize ADCs */
-	initialize_adc(&processor_state[0]);
-	if (processor_state[1].monitor != NULL)
-		initialize_adc(&processor_state[1]);
-
-	fcu_tickle_ticks = FCU_TICKLE_TICKS;
-
-	mutex_unlock(&driver_lock);
-
-	while (state == state_attached) {
-		unsigned long elapsed, start;
-
-		start = jiffies;
-
-		mutex_lock(&driver_lock);
-
-		/* Tickle the FCU just in case */
-		if (--fcu_tickle_ticks < 0) {
-			fcu_tickle_ticks = FCU_TICKLE_TICKS;
-			tickle_fcu();
-		}
-
-		/* First, we always calculate the new DIMMs state on an Xserve */
-		if (rackmac)
-			do_monitor_dimms(&dimms_state);
-
-		/* Then, the CPUs */
-		if (cpu_pid_type == CPU_PID_TYPE_COMBINED)
-			do_monitor_cpu_combined();
-		else if (cpu_pid_type == CPU_PID_TYPE_RACKMAC) {
-			do_monitor_cpu_rack(&processor_state[0]);
-			if (processor_state[1].monitor != NULL)
-				do_monitor_cpu_rack(&processor_state[1]);
-			// better deal with UP
-		} else {
-			do_monitor_cpu_split(&processor_state[0]);
-			if (processor_state[1].monitor != NULL)
-				do_monitor_cpu_split(&processor_state[1]);
-			// better deal with UP
-		}
-		/* Then, the rest */
-		do_monitor_backside(&backside_state);
-		if (rackmac)
-			do_monitor_slots(&slots_state);
-		else
-			do_monitor_drives(&drives_state);
-		mutex_unlock(&driver_lock);
-
-		if (critical_state == 1) {
-			printk(KERN_WARNING "Temperature control detected a critical condition\n");
-			printk(KERN_WARNING "Attempting to shut down...\n");
-			if (call_critical_overtemp()) {
-				printk(KERN_WARNING "Can't call %s, power off now!\n",
-				       critical_overtemp_path);
-				machine_power_off();
-			}
-		}
-		if (critical_state > 0)
-			critical_state++;
-		if (critical_state > MAX_CRITICAL_STATE) {
-			printk(KERN_WARNING "Shutdown timed out, power off now !\n");
-			machine_power_off();
-		}
-
-		// FIXME: Deal with signals
-		elapsed = jiffies - start;
-		if (elapsed < HZ)
-			schedule_timeout_interruptible(HZ - elapsed);
-	}
-
- out:
-	DBG("main_control_loop ended\n");
-
-	ctrl_task = 0;
-	complete_and_exit(&ctrl_complete, 0);
-}
-
-/*
- * Dispose the control loops when tearing down
- */
-static void dispose_control_loops(void)
-{
-	dispose_processor_state(&processor_state[0]);
-	dispose_processor_state(&processor_state[1]);
-	dispose_backside_state(&backside_state);
-	dispose_drives_state(&drives_state);
-	dispose_slots_state(&slots_state);
-	dispose_dimms_state(&dimms_state);
-}
-
-/*
- * Create the control loops. U3-0 i2c bus is up, so we can now
- * get to the various sensors
- */
-static int create_control_loops(void)
-{
-	struct device_node *np;
-
-	/* Count CPUs from the device-tree, we don't care how many are
-	 * actually used by Linux
-	 */
-	cpu_count = 0;
-	for (np = NULL; NULL != (np = of_find_node_by_type(np, "cpu"));)
-		cpu_count++;
-
-	DBG("counted %d CPUs in the device-tree\n", cpu_count);
-
-	/* Decide the type of PID algorithm to use based on the presence of
-	 * the pumps, though that may not be the best way, that is good enough
-	 * for now
-	 */
-	if (rackmac)
-		cpu_pid_type = CPU_PID_TYPE_RACKMAC;
-	else if (of_machine_is_compatible("PowerMac7,3")
-	    && (cpu_count > 1)
-	    && fcu_fans[CPUA_PUMP_RPM_INDEX].id != FCU_FAN_ABSENT_ID
-	    && fcu_fans[CPUB_PUMP_RPM_INDEX].id != FCU_FAN_ABSENT_ID) {
-		printk(KERN_INFO "Liquid cooling pumps detected, using new algorithm !\n");
-		cpu_pid_type = CPU_PID_TYPE_COMBINED;
-	} else
-		cpu_pid_type = CPU_PID_TYPE_SPLIT;
-
-	/* Create control loops for everything. If any fail, everything
-	 * fails
-	 */
-	if (init_processor_state(&processor_state[0], 0))
-		goto fail;
-	if (cpu_pid_type == CPU_PID_TYPE_COMBINED)
-		fetch_cpu_pumps_minmax();
-
-	if (cpu_count > 1 && init_processor_state(&processor_state[1], 1))
-		goto fail;
-	if (init_backside_state(&backside_state))
-		goto fail;
-	if (rackmac && init_dimms_state(&dimms_state))
-		goto fail;
-	if (rackmac && init_slots_state(&slots_state))
-		goto fail;
-	if (!rackmac && init_drives_state(&drives_state))
-		goto fail;
-
-	DBG("all control loops up !\n");
-
-	return 0;
-	
- fail:
-	DBG("failure creating control loops, disposing\n");
-
-	dispose_control_loops();
-
-	return -ENODEV;
-}
-
-/*
- * Start the control loops after everything is up, that is create
- * the thread that will make them run
- */
-static void start_control_loops(void)
-{
-	init_completion(&ctrl_complete);
-
-	ctrl_task = kthread_run(main_control_loop, NULL, "kfand");
-}
-
-/*
- * Stop the control loops when tearing down
- */
-static void stop_control_loops(void)
-{
-	if (ctrl_task)
-		wait_for_completion(&ctrl_complete);
-}
-
-/*
- * Attach to the i2c FCU after detecting U3-1 bus
- */
-static int attach_fcu(void)
-{
-	fcu = attach_i2c_chip(FAN_CTRLER_ID, "fcu");
-	if (fcu == NULL)
-		return -ENODEV;
-
-	DBG("FCU attached\n");
-
-	return 0;
-}
-
-/*
- * Detach from the i2c FCU when tearing down
- */
-static void detach_fcu(void)
-{
-	fcu = NULL;
-}
-
-/*
- * Attach to the i2c controller. We probe the various chips based
- * on the device-tree nodes and build everything for the driver to
- * run, we then kick the driver monitoring thread
- */
-static int therm_pm72_attach(struct i2c_adapter *adapter)
-{
-	mutex_lock(&driver_lock);
-
-	/* Check state */
-	if (state == state_detached)
-		state = state_attaching;
-	if (state != state_attaching) {
-		mutex_unlock(&driver_lock);
-		return 0;
-	}
-
-	/* Check if we are looking for one of these */
-	if (u3_0 == NULL && !strcmp(adapter->name, "u3 0")) {
-		u3_0 = adapter;
-		DBG("found U3-0\n");
-		if (k2 || !rackmac)
-			if (create_control_loops())
-				u3_0 = NULL;
-	} else if (u3_1 == NULL && !strcmp(adapter->name, "u3 1")) {
-		u3_1 = adapter;
-		DBG("found U3-1, attaching FCU\n");
-		if (attach_fcu())
-			u3_1 = NULL;
-	} else if (k2 == NULL && !strcmp(adapter->name, "mac-io 0")) {
-		k2 = adapter;
-		DBG("Found K2\n");
-		if (u3_0 && rackmac)
-			if (create_control_loops())
-				k2 = NULL;
-	}
-	/* We got all we need, start control loops */
-	if (u3_0 != NULL && u3_1 != NULL && (k2 || !rackmac)) {
-		DBG("everything up, starting control loops\n");
-		state = state_attached;
-		start_control_loops();
-	}
-	mutex_unlock(&driver_lock);
-
-	return 0;
-}
-
-static int therm_pm72_probe(struct i2c_client *client,
-			    const struct i2c_device_id *id)
-{
-	/* Always succeed, the real work was done in therm_pm72_attach() */
-	return 0;
-}
-
-/*
- * Called when any of the devices which participates into thermal management
- * is going away.
- */
-static int therm_pm72_remove(struct i2c_client *client)
-{
-	struct i2c_adapter *adapter = client->adapter;
-
-	mutex_lock(&driver_lock);
-
-	if (state != state_detached)
-		state = state_detaching;
-
-	/* Stop control loops if any */
-	DBG("stopping control loops\n");
-	mutex_unlock(&driver_lock);
-	stop_control_loops();
-	mutex_lock(&driver_lock);
-
-	if (u3_0 != NULL && !strcmp(adapter->name, "u3 0")) {
-		DBG("lost U3-0, disposing control loops\n");
-		dispose_control_loops();
-		u3_0 = NULL;
-	}
-	
-	if (u3_1 != NULL && !strcmp(adapter->name, "u3 1")) {
-		DBG("lost U3-1, detaching FCU\n");
-		detach_fcu();
-		u3_1 = NULL;
-	}
-	if (u3_0 == NULL && u3_1 == NULL)
-		state = state_detached;
-
-	mutex_unlock(&driver_lock);
-
-	return 0;
-}
-
-/*
- * i2c_driver structure to attach to the host i2c controller
- */
-
-static const struct i2c_device_id therm_pm72_id[] = {
-	/*
-	 * Fake device name, thermal management is done by several
-	 * chips but we don't need to differentiate between them at
-	 * this point.
-	 */
-	{ "therm_pm72", 0 },
-	{ }
-};
-
-static struct i2c_driver therm_pm72_driver = {
-	.driver = {
-		.name	= "therm_pm72",
-	},
-	.attach_adapter	= therm_pm72_attach,
-	.probe		= therm_pm72_probe,
-	.remove		= therm_pm72_remove,
-	.id_table	= therm_pm72_id,
-};
-
-static int fan_check_loc_match(const char *loc, int fan)
-{
-	char	tmp[64];
-	char	*c, *e;
-
-	strlcpy(tmp, fcu_fans[fan].loc, 64);
-
-	c = tmp;
-	for (;;) {
-		e = strchr(c, ',');
-		if (e)
-			*e = 0;
-		if (strcmp(loc, c) == 0)
-			return 1;
-		if (e == NULL)
-			break;
-		c = e + 1;
-	}
-	return 0;
-}
-
-static void fcu_lookup_fans(struct device_node *fcu_node)
-{
-	struct device_node *np = NULL;
-	int i;
-
-	/* The table is filled by default with values that are suitable
-	 * for the old machines without device-tree informations. We scan
-	 * the device-tree and override those values with whatever is
-	 * there
-	 */
-
-	DBG("Looking up FCU controls in device-tree...\n");
-
-	while ((np = of_get_next_child(fcu_node, np)) != NULL) {
-		int type = -1;
-		const char *loc;
-		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 < FCU_FAN_COUNT; i++) {
-			int fan_id;
-
-			if (!fan_check_loc_match(loc, i))
-				continue;
-			DBG(" location match, index: %d\n", i);
-			fcu_fans[i].id = FCU_FAN_ABSENT_ID;
-			if (type != fcu_fans[i].type) {
-				printk(KERN_WARNING "therm_pm72: Fan type mismatch "
-				       "in device-tree for %s\n", np->full_name);
-				break;
-			}
-			if (type == FCU_FAN_RPM)
-				fan_id = ((*reg) - 0x10) / 2;
-			else
-				fan_id = ((*reg) - 0x30) / 2;
-			if (fan_id > 7) {
-				printk(KERN_WARNING "therm_pm72: Can't parse "
-				       "fan ID in device-tree for %s\n", np->full_name);
-				break;
-			}
-			DBG(" fan id -> %d, type -> %d\n", fan_id, type);
-			fcu_fans[i].id = fan_id;
-		}
-	}
-
-	/* Now dump the array */
-	printk(KERN_INFO "Detected fan controls:\n");
-	for (i = 0; i < FCU_FAN_COUNT; i++) {
-		if (fcu_fans[i].id == FCU_FAN_ABSENT_ID)
-			continue;
-		printk(KERN_INFO "  %d: %s fan, id %d, location: %s\n", i,
-		       fcu_fans[i].type == FCU_FAN_RPM ? "RPM" : "PWM",
-		       fcu_fans[i].id, fcu_fans[i].loc);
-	}
-}
-
-static int fcu_of_probe(struct platform_device* dev)
-{
-	state = state_detached;
-	of_dev = dev;
-
-	dev_info(&dev->dev, "PowerMac G5 Thermal control driver %s\n", VERSION);
-
-	/* Lookup the fans in the device tree */
-	fcu_lookup_fans(dev->dev.of_node);
-
-	/* Add the driver */
-	return i2c_add_driver(&therm_pm72_driver);
-}
-
-static int fcu_of_remove(struct platform_device* dev)
-{
-	i2c_del_driver(&therm_pm72_driver);
-
-	return 0;
-}
-
-static const struct of_device_id fcu_match[] = 
-{
-	{
-	.type		= "fcu",
-	},
-	{},
-};
-MODULE_DEVICE_TABLE(of, fcu_match);
-
-static struct platform_driver fcu_of_platform_driver = 
-{
-	.driver = {
-		.name = "temperature",
-		.owner = THIS_MODULE,
-		.of_match_table = fcu_match,
-	},
-	.probe		= fcu_of_probe,
-	.remove		= fcu_of_remove
-};
-
-/*
- * Check machine type, attach to i2c controller
- */
-static int __init therm_pm72_init(void)
-{
-	rackmac = of_machine_is_compatible("RackMac3,1");
-
-	if (!of_machine_is_compatible("PowerMac7,2") &&
-	    !of_machine_is_compatible("PowerMac7,3") &&
-	    !rackmac)
-	    	return -ENODEV;
-
-	return platform_driver_register(&fcu_of_platform_driver);
-}
-
-static void __exit therm_pm72_exit(void)
-{
-	platform_driver_unregister(&fcu_of_platform_driver);
-}
-
-module_init(therm_pm72_init);
-module_exit(therm_pm72_exit);
-
-MODULE_AUTHOR("Benjamin Herrenschmidt <benh@kernel.crashing.org>");
-MODULE_DESCRIPTION("Driver for Apple's PowerMac G5 thermal control");
-MODULE_LICENSE("GPL");
-
diff --git a/drivers/macintosh/therm_pm72.h b/drivers/macintosh/therm_pm72.h
deleted file mode 100644
index df3680e2a22f..000000000000
--- a/drivers/macintosh/therm_pm72.h
+++ /dev/null
@@ -1,326 +0,0 @@
-#ifndef __THERM_PMAC_7_2_H__
-#define __THERM_PMAC_7_2_H__
-
-typedef unsigned short fu16;
-typedef int fs32;
-typedef short fs16;
-
-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 */
-
-/* Display a 16.16 fixed point value */
-#define FIX32TOPRINT(f)	((f) >> 16),((((f) & 0xffff) * 1000) >> 16)
-
-/*
- * Maximum number of seconds to be in critical state (after a
- * normal shutdown attempt). If the machine isn't down after
- * this counter elapses, we force an immediate machine power
- * off.
- */
-#define MAX_CRITICAL_STATE			30
-static char * critical_overtemp_path = "/sbin/critical_overtemp";
-
-/*
- * 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 not sure which of these Apple's algorithm is supposed
- * to use
- */
-#define RPM_PID_USE_ACTUAL_SPEED		0
-
-/*
- * i2c IDs. Currently, we hard code those and assume that
- * the FCU is on U3 bus 1 while all sensors are on U3 bus
- * 0. This appear to be safe enough for this first version
- * of the driver, though I would accept any clean patch
- * doing a better use of the device-tree without turning the
- * while i2c registration mechanism into a racy mess
- *
- * Note: Xserve changed this. We have some bits on the K2 bus,
- * which I arbitrarily set to 0x200. Ultimately, we really want
- * too lookup these in the device-tree though
- */
-#define FAN_CTRLER_ID		0x15e
-#define SUPPLY_MONITOR_ID      	0x58
-#define SUPPLY_MONITORB_ID     	0x5a
-#define DRIVES_DALLAS_ID	0x94
-#define BACKSIDE_MAX_ID		0x98
-#define XSERVE_DIMMS_LM87	0x25a
-#define XSERVE_SLOTS_LM75	0x290
-
-/*
- * Some MAX6690, DS1775, LM87 register definitions
- */
-#define MAX6690_INT_TEMP	0
-#define MAX6690_EXT_TEMP	1
-#define DS1775_TEMP		0
-#define LM87_INT_TEMP		0x27
-
-/*
- * 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 */
-
-/*
- * PID factors for the U3/Backside fan control loop. We have 2 sets
- * of values here, one set for U3 and one set for U3H
- */
-#define BACKSIDE_FAN_PWM_DEFAULT_ID	1
-#define BACKSIDE_FAN_PWM_INDEX		0
-#define BACKSIDE_PID_U3_G_d		0x02800000
-#define BACKSIDE_PID_U3H_G_d		0x01400000
-#define BACKSIDE_PID_RACK_G_d		0x00500000
-#define BACKSIDE_PID_G_p		0x00500000
-#define BACKSIDE_PID_RACK_G_p		0x0004cccc
-#define BACKSIDE_PID_G_r		0x00000000
-#define BACKSIDE_PID_U3_INPUT_TARGET	0x00410000
-#define BACKSIDE_PID_U3H_INPUT_TARGET	0x004b0000
-#define BACKSIDE_PID_RACK_INPUT_TARGET	0x00460000
-#define BACKSIDE_PID_INTERVAL		5
-#define BACKSIDE_PID_RACK_INTERVAL	1
-#define BACKSIDE_PID_OUTPUT_MAX		100
-#define BACKSIDE_PID_U3_OUTPUT_MIN	20
-#define BACKSIDE_PID_U3H_OUTPUT_MIN	20
-#define BACKSIDE_PID_HISTORY_SIZE	2
-
-struct basckside_pid_params
-{
-	s32			G_d;
-	s32			G_p;
-	s32			G_r;
-	s32			input_target;
-	s32			output_min;
-	s32			output_max;
-	s32			interval;
-	int			additive;
-};
-
-struct backside_pid_state
-{
-	int			ticks;
-	struct i2c_client *	monitor;
-	s32		       	sample_history[BACKSIDE_PID_HISTORY_SIZE];
-	s32			error_history[BACKSIDE_PID_HISTORY_SIZE];
-	int			cur_sample;
-	s32			last_temp;
-	int			pwm;
-	int			first;
-};
-
-/*
- * PID factors for the Drive Bay fan control loop
- */
-#define DRIVES_FAN_RPM_DEFAULT_ID	2
-#define DRIVES_FAN_RPM_INDEX		1
-#define DRIVES_PID_G_d			0x01e00000
-#define DRIVES_PID_G_p			0x00500000
-#define DRIVES_PID_G_r			0x00000000
-#define DRIVES_PID_INPUT_TARGET		0x00280000
-#define DRIVES_PID_INTERVAL    		5
-#define DRIVES_PID_OUTPUT_MAX		4000
-#define DRIVES_PID_OUTPUT_MIN		300
-#define DRIVES_PID_HISTORY_SIZE		2
-
-struct drives_pid_state
-{
-	int			ticks;
-	struct i2c_client *	monitor;
-	s32	       		sample_history[BACKSIDE_PID_HISTORY_SIZE];
-	s32			error_history[BACKSIDE_PID_HISTORY_SIZE];
-	int			cur_sample;
-	s32			last_temp;
-	int			rpm;
-	int			first;
-};
-
-#define SLOTS_FAN_PWM_DEFAULT_ID	2
-#define SLOTS_FAN_PWM_INDEX		2
-#define	SLOTS_FAN_DEFAULT_PWM		40 /* Do better here ! */
-
-
-/*
- * PID factors for the Xserve DIMM control loop
- */
-#define DIMM_PID_G_d			0
-#define DIMM_PID_G_p			0
-#define DIMM_PID_G_r			0x06553600
-#define DIMM_PID_INPUT_TARGET		3276800
-#define DIMM_PID_INTERVAL    		1
-#define DIMM_PID_OUTPUT_MAX		14000
-#define DIMM_PID_OUTPUT_MIN		4000
-#define DIMM_PID_HISTORY_SIZE		20
-
-struct dimm_pid_state
-{
-	int			ticks;
-	struct i2c_client *	monitor;
-	s32	       		sample_history[DIMM_PID_HISTORY_SIZE];
-	s32			error_history[DIMM_PID_HISTORY_SIZE];
-	int			cur_sample;
-	s32			last_temp;
-	int			first;
-	int			output;
-};
-
-
-/*
- * PID factors for the Xserve Slots control loop
- */
-#define SLOTS_PID_G_d			0
-#define SLOTS_PID_G_p			0
-#define SLOTS_PID_G_r			0x00100000
-#define SLOTS_PID_INPUT_TARGET		3200000
-#define SLOTS_PID_INTERVAL    		1
-#define SLOTS_PID_OUTPUT_MAX		100
-#define SLOTS_PID_OUTPUT_MIN		20
-#define SLOTS_PID_HISTORY_SIZE		20
-
-struct slots_pid_state
-{
-	int			ticks;
-	struct i2c_client *	monitor;
-	s32	       		sample_history[SLOTS_PID_HISTORY_SIZE];
-	s32			error_history[SLOTS_PID_HISTORY_SIZE];
-	int			cur_sample;
-	s32			last_temp;
-	int			first;
-	int			pwm;
-};
-
-
-
-/* Desktops */
-
-#define CPUA_INTAKE_FAN_RPM_DEFAULT_ID	3
-#define CPUA_EXHAUST_FAN_RPM_DEFAULT_ID	4
-#define CPUB_INTAKE_FAN_RPM_DEFAULT_ID	5
-#define CPUB_EXHAUST_FAN_RPM_DEFAULT_ID	6
-
-#define CPUA_INTAKE_FAN_RPM_INDEX	3
-#define CPUA_EXHAUST_FAN_RPM_INDEX	4
-#define CPUB_INTAKE_FAN_RPM_INDEX	5
-#define CPUB_EXHAUST_FAN_RPM_INDEX	6
-
-#define CPU_INTAKE_SCALE		0x0000f852
-#define CPU_TEMP_HISTORY_SIZE		2
-#define CPU_POWER_HISTORY_SIZE		10
-#define CPU_PID_INTERVAL		1
-#define CPU_MAX_OVERTEMP		90
-
-#define CPUA_PUMP_RPM_INDEX		7
-#define CPUB_PUMP_RPM_INDEX		8
-#define CPU_PUMP_OUTPUT_MAX		3200
-#define CPU_PUMP_OUTPUT_MIN		1250
-
-/* Xserve */
-#define CPU_A1_FAN_RPM_INDEX		9
-#define CPU_A2_FAN_RPM_INDEX		10
-#define CPU_A3_FAN_RPM_INDEX		11
-#define CPU_B1_FAN_RPM_INDEX		12
-#define CPU_B2_FAN_RPM_INDEX		13
-#define CPU_B3_FAN_RPM_INDEX		14
-
-
-struct cpu_pid_state
-{
-	int			index;
-	struct i2c_client *	monitor;
-	struct mpu_data		mpu;
-	int			overtemp;
-	s32	       		temp_history[CPU_TEMP_HISTORY_SIZE];
-	int			cur_temp;
-	s32			power_history[CPU_POWER_HISTORY_SIZE];
-	s32			error_history[CPU_POWER_HISTORY_SIZE];
-	int			cur_power;
-	int			count_power;
-	int			rpm;
-	int			intake_rpm;
-	s32			voltage;
-	s32			current_a;
-	s32			last_temp;
-	s32			last_power;
-	int			first;
-	u8			adc_config;
-	s32			pump_min;
-	s32			pump_max;
-};
-
-/* Tickle FCU every 10 seconds */
-#define FCU_TICKLE_TICKS	10
-
-/*
- * Driver state
- */
-enum {
-	state_detached,
-	state_attaching,
-	state_attached,
-	state_detaching,
-};
-
-
-#endif /* __THERM_PMAC_7_2_H__ */
diff --git a/drivers/macintosh/therm_windtunnel.c b/drivers/macintosh/therm_windtunnel.c
index 3b4a157714b1..109dcaa15934 100644
--- a/drivers/macintosh/therm_windtunnel.c
+++ b/drivers/macintosh/therm_windtunnel.c
@@ -463,7 +463,6 @@ static const struct of_device_id therm_of_match[] = {{
 static struct platform_driver therm_of_driver = {
 	.driver = {
 		.name = "temperature",
-		.owner = THIS_MODULE,
 		.of_match_table = therm_of_match,
 	},
 	.probe		= therm_of_probe,
diff --git a/drivers/macintosh/windfarm_pm81.c b/drivers/macintosh/windfarm_pm81.c
index 2a5e1b15b1d2..93faf298a3c5 100644
--- a/drivers/macintosh/windfarm_pm81.c
+++ b/drivers/macintosh/windfarm_pm81.c
@@ -770,7 +770,6 @@ static struct platform_driver wf_smu_driver = {
         .remove = wf_smu_remove,
 	.driver = {
 		.name = "windfarm",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/macintosh/windfarm_pm91.c b/drivers/macintosh/windfarm_pm91.c
index a8ac66cd3b13..81fdf40c5b82 100644
--- a/drivers/macintosh/windfarm_pm91.c
+++ b/drivers/macintosh/windfarm_pm91.c
@@ -699,7 +699,6 @@ static struct platform_driver wf_smu_driver = {
         .remove = wf_smu_remove,
 	.driver = {
 		.name = "windfarm",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mailbox/omap-mailbox.c b/drivers/mailbox/omap-mailbox.c
index 66b83ca94dcf..0f332c178b07 100644
--- a/drivers/mailbox/omap-mailbox.c
+++ b/drivers/mailbox/omap-mailbox.c
@@ -864,7 +864,6 @@ static struct platform_driver omap_mbox_driver = {
 	.remove	= omap_mbox_remove,
 	.driver	= {
 		.name = "omap-mailbox",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(omap_mailbox_of_match),
 	},
 };
diff --git a/drivers/md/bcache/request.c b/drivers/md/bcache/request.c
index 62e6e98186b5..ab43faddb447 100644
--- a/drivers/md/bcache/request.c
+++ b/drivers/md/bcache/request.c
@@ -601,13 +601,8 @@ static void request_endio(struct bio *bio, int error)
 static void bio_complete(struct search *s)
 {
 	if (s->orig_bio) {
-		int cpu, rw = bio_data_dir(s->orig_bio);
-		unsigned long duration = jiffies - s->start_time;
-
-		cpu = part_stat_lock();
-		part_round_stats(cpu, &s->d->disk->part0);
-		part_stat_add(cpu, &s->d->disk->part0, ticks[rw], duration);
-		part_stat_unlock();
+		generic_end_io_acct(bio_data_dir(s->orig_bio),
+				    &s->d->disk->part0, s->start_time);
 
 		trace_bcache_request_end(s->d, s->orig_bio);
 		bio_endio(s->orig_bio, s->iop.error);
@@ -959,12 +954,9 @@ static void cached_dev_make_request(struct request_queue *q, struct bio *bio)
 	struct search *s;
 	struct bcache_device *d = bio->bi_bdev->bd_disk->private_data;
 	struct cached_dev *dc = container_of(d, struct cached_dev, disk);
-	int cpu, rw = bio_data_dir(bio);
+	int rw = bio_data_dir(bio);
 
-	cpu = part_stat_lock();
-	part_stat_inc(cpu, &d->disk->part0, ios[rw]);
-	part_stat_add(cpu, &d->disk->part0, sectors[rw], bio_sectors(bio));
-	part_stat_unlock();
+	generic_start_io_acct(rw, bio_sectors(bio), &d->disk->part0);
 
 	bio->bi_bdev = dc->bdev;
 	bio->bi_iter.bi_sector += dc->sb.data_offset;
@@ -1074,12 +1066,9 @@ static void flash_dev_make_request(struct request_queue *q, struct bio *bio)
 	struct search *s;
 	struct closure *cl;
 	struct bcache_device *d = bio->bi_bdev->bd_disk->private_data;
-	int cpu, rw = bio_data_dir(bio);
+	int rw = bio_data_dir(bio);
 
-	cpu = part_stat_lock();
-	part_stat_inc(cpu, &d->disk->part0, ios[rw]);
-	part_stat_add(cpu, &d->disk->part0, sectors[rw], bio_sectors(bio));
-	part_stat_unlock();
+	generic_start_io_acct(rw, bio_sectors(bio), &d->disk->part0);
 
 	s = search_alloc(bio, d);
 	cl = &s->cl;
diff --git a/drivers/md/dm-thin.c b/drivers/md/dm-thin.c
index 8735543eacdb..493478989dbd 100644
--- a/drivers/md/dm-thin.c
+++ b/drivers/md/dm-thin.c
@@ -1127,6 +1127,24 @@ static void schedule_external_copy(struct thin_c *tc, dm_block_t virt_block,
 		schedule_zero(tc, virt_block, data_dest, cell, bio);
 }
 
+static void set_pool_mode(struct pool *pool, enum pool_mode new_mode);
+
+static void check_for_space(struct pool *pool)
+{
+	int r;
+	dm_block_t nr_free;
+
+	if (get_pool_mode(pool) != PM_OUT_OF_DATA_SPACE)
+		return;
+
+	r = dm_pool_get_free_block_count(pool->pmd, &nr_free);
+	if (r)
+		return;
+
+	if (nr_free)
+		set_pool_mode(pool, PM_WRITE);
+}
+
 /*
  * A non-zero return indicates read_only or fail_io mode.
  * Many callers don't care about the return value.
@@ -1141,6 +1159,8 @@ static int commit(struct pool *pool)
 	r = dm_pool_commit_metadata(pool->pmd);
 	if (r)
 		metadata_operation_failed(pool, "dm_pool_commit_metadata", r);
+	else
+		check_for_space(pool);
 
 	return r;
 }
@@ -1159,8 +1179,6 @@ static void check_low_water_mark(struct pool *pool, dm_block_t free_blocks)
 	}
 }
 
-static void set_pool_mode(struct pool *pool, enum pool_mode new_mode);
-
 static int alloc_data_block(struct thin_c *tc, dm_block_t *result)
 {
 	int r;
@@ -2155,7 +2173,7 @@ static void set_pool_mode(struct pool *pool, enum pool_mode new_mode)
 		pool->process_cell = process_cell_read_only;
 		pool->process_discard_cell = process_discard_cell;
 		pool->process_prepared_mapping = process_prepared_mapping;
-		pool->process_prepared_discard = process_prepared_discard_passdown;
+		pool->process_prepared_discard = process_prepared_discard;
 
 		if (!pool->pf.error_if_no_space && no_space_timeout)
 			queue_delayed_work(pool->wq, &pool->no_space_timeout, no_space_timeout);
@@ -3814,6 +3832,8 @@ static int thin_ctr(struct dm_target *ti, unsigned argc, char **argv)
 		r = -EINVAL;
 		goto bad;
 	}
+	atomic_set(&tc->refcount, 1);
+	init_completion(&tc->can_destroy);
 	list_add_tail_rcu(&tc->list, &tc->pool->active_thins);
 	spin_unlock_irqrestore(&tc->pool->lock, flags);
 	/*
@@ -3826,9 +3846,6 @@ static int thin_ctr(struct dm_target *ti, unsigned argc, char **argv)
 
 	dm_put(pool_md);
 
-	atomic_set(&tc->refcount, 1);
-	init_completion(&tc->can_destroy);
-
 	return 0;
 
 bad:
diff --git a/drivers/md/dm.c b/drivers/md/dm.c
index 8f37ed215b19..b98cd9d84435 100644
--- a/drivers/md/dm.c
+++ b/drivers/md/dm.c
@@ -605,13 +605,10 @@ static void end_io_acct(struct dm_io *io)
 	struct mapped_device *md = io->md;
 	struct bio *bio = io->bio;
 	unsigned long duration = jiffies - io->start_time;
-	int pending, cpu;
+	int pending;
 	int rw = bio_data_dir(bio);
 
-	cpu = part_stat_lock();
-	part_round_stats(cpu, &dm_disk(md)->part0);
-	part_stat_add(cpu, &dm_disk(md)->part0, ticks[rw], duration);
-	part_stat_unlock();
+	generic_end_io_acct(rw, &dm_disk(md)->part0, io->start_time);
 
 	if (unlikely(dm_stats_used(&md->stats)))
 		dm_stats_account_io(&md->stats, bio->bi_rw, bio->bi_iter.bi_sector,
@@ -902,7 +899,7 @@ static void disable_write_same(struct mapped_device *md)
 
 static void clone_endio(struct bio *bio, int error)
 {
-	int r = 0;
+	int r = error;
 	struct dm_target_io *tio = container_of(bio, struct dm_target_io, clone);
 	struct dm_io *io = tio->io;
 	struct mapped_device *md = tio->io->md;
@@ -1651,16 +1648,12 @@ static void _dm_request(struct request_queue *q, struct bio *bio)
 {
 	int rw = bio_data_dir(bio);
 	struct mapped_device *md = q->queuedata;
-	int cpu;
 	int srcu_idx;
 	struct dm_table *map;
 
 	map = dm_get_live_table(md, &srcu_idx);
 
-	cpu = part_stat_lock();
-	part_stat_inc(cpu, &dm_disk(md)->part0, ios[rw]);
-	part_stat_add(cpu, &dm_disk(md)->part0, sectors[rw], bio_sectors(bio));
-	part_stat_unlock();
+	generic_start_io_acct(rw, bio_sectors(bio), &dm_disk(md)->part0);
 
 	/* if we're suspended, we have to queue this io for later */
 	if (unlikely(test_bit(DMF_BLOCK_IO_FOR_SUSPEND, &md->flags))) {
diff --git a/drivers/md/md.c b/drivers/md/md.c
index 9233c71138f1..709755fb6d7b 100644
--- a/drivers/md/md.c
+++ b/drivers/md/md.c
@@ -247,7 +247,6 @@ static void md_make_request(struct request_queue *q, struct bio *bio)
 {
 	const int rw = bio_data_dir(bio);
 	struct mddev *mddev = q->queuedata;
-	int cpu;
 	unsigned int sectors;
 
 	if (mddev == NULL || mddev->pers == NULL
@@ -284,10 +283,7 @@ static void md_make_request(struct request_queue *q, struct bio *bio)
 	sectors = bio_sectors(bio);
 	mddev->pers->make_request(mddev, bio);
 
-	cpu = part_stat_lock();
-	part_stat_inc(cpu, &mddev->gendisk->part0, ios[rw]);
-	part_stat_add(cpu, &mddev->gendisk->part0, sectors[rw], sectors);
-	part_stat_unlock();
+	generic_start_io_acct(rw, sectors, &mddev->gendisk->part0);
 
 	if (atomic_dec_and_test(&mddev->active_io) && mddev->suspended)
 		wake_up(&mddev->sb_wait);
@@ -2695,7 +2691,8 @@ static ssize_t new_offset_store(struct md_rdev *rdev,
 	if (kstrtoull(buf, 10, &new_offset) < 0)
 		return -EINVAL;
 
-	if (mddev->sync_thread)
+	if (mddev->sync_thread ||
+	    test_bit(MD_RECOVERY_RUNNING,&mddev->recovery))
 		return -EBUSY;
 	if (new_offset == rdev->data_offset)
 		/* reset is always permitted */
@@ -3272,6 +3269,7 @@ level_store(struct mddev *mddev, const char *buf, size_t len)
 	 */
 
 	if (mddev->sync_thread ||
+	    test_bit(MD_RECOVERY_RUNNING, &mddev->recovery) ||
 	    mddev->reshape_position != MaxSector ||
 	    mddev->sysfs_active)
 		return -EBUSY;
@@ -4026,6 +4024,7 @@ action_store(struct mddev *mddev, const char *page, size_t len)
 		clear_bit(MD_RECOVERY_FROZEN, &mddev->recovery);
 
 	if (cmd_match(page, "idle") || cmd_match(page, "frozen")) {
+		flush_workqueue(md_misc_wq);
 		if (mddev->sync_thread) {
 			set_bit(MD_RECOVERY_INTR, &mddev->recovery);
 			md_reap_sync_thread(mddev);
@@ -5044,6 +5043,7 @@ static void md_clean(struct mddev *mddev)
 static void __md_stop_writes(struct mddev *mddev)
 {
 	set_bit(MD_RECOVERY_FROZEN, &mddev->recovery);
+	flush_workqueue(md_misc_wq);
 	if (mddev->sync_thread) {
 		set_bit(MD_RECOVERY_INTR, &mddev->recovery);
 		md_reap_sync_thread(mddev);
@@ -5104,19 +5104,22 @@ static int md_set_readonly(struct mddev *mddev, struct block_device *bdev)
 		set_bit(MD_RECOVERY_FROZEN, &mddev->recovery);
 		md_wakeup_thread(mddev->thread);
 	}
-	if (mddev->sync_thread) {
+	if (test_bit(MD_RECOVERY_RUNNING, &mddev->recovery))
 		set_bit(MD_RECOVERY_INTR, &mddev->recovery);
+	if (mddev->sync_thread)
 		/* Thread might be blocked waiting for metadata update
 		 * which will now never happen */
 		wake_up_process(mddev->sync_thread->tsk);
-	}
+
 	mddev_unlock(mddev);
-	wait_event(resync_wait, mddev->sync_thread == NULL);
+	wait_event(resync_wait, !test_bit(MD_RECOVERY_RUNNING,
+					  &mddev->recovery));
 	mddev_lock_nointr(mddev);
 
 	mutex_lock(&mddev->open_mutex);
 	if ((mddev->pers && atomic_read(&mddev->openers) > !!bdev) ||
 	    mddev->sync_thread ||
+	    test_bit(MD_RECOVERY_RUNNING, &mddev->recovery) ||
 	    (bdev && !test_bit(MD_STILL_CLOSED, &mddev->flags))) {
 		printk("md: %s still in use.\n",mdname(mddev));
 		if (did_freeze) {
@@ -5162,20 +5165,24 @@ static int do_md_stop(struct mddev *mddev, int mode,
 		set_bit(MD_RECOVERY_FROZEN, &mddev->recovery);
 		md_wakeup_thread(mddev->thread);
 	}
-	if (mddev->sync_thread) {
+	if (test_bit(MD_RECOVERY_RUNNING, &mddev->recovery))
 		set_bit(MD_RECOVERY_INTR, &mddev->recovery);
+	if (mddev->sync_thread)
 		/* Thread might be blocked waiting for metadata update
 		 * which will now never happen */
 		wake_up_process(mddev->sync_thread->tsk);
-	}
+
 	mddev_unlock(mddev);
-	wait_event(resync_wait, mddev->sync_thread == NULL);
+	wait_event(resync_wait, (mddev->sync_thread == NULL &&
+				 !test_bit(MD_RECOVERY_RUNNING,
+					   &mddev->recovery)));
 	mddev_lock_nointr(mddev);
 
 	mutex_lock(&mddev->open_mutex);
 	if ((mddev->pers && atomic_read(&mddev->openers) > !!bdev) ||
 	    mddev->sysfs_active ||
 	    mddev->sync_thread ||
+	    test_bit(MD_RECOVERY_RUNNING, &mddev->recovery) ||
 	    (bdev && !test_bit(MD_STILL_CLOSED, &mddev->flags))) {
 		printk("md: %s still in use.\n",mdname(mddev));
 		mutex_unlock(&mddev->open_mutex);
@@ -5950,7 +5957,8 @@ static int update_size(struct mddev *mddev, sector_t num_sectors)
 	 * of each device.  If num_sectors is zero, we find the largest size
 	 * that fits.
 	 */
-	if (mddev->sync_thread)
+	if (test_bit(MD_RECOVERY_RUNNING, &mddev->recovery) ||
+	    mddev->sync_thread)
 		return -EBUSY;
 	if (mddev->ro)
 		return -EROFS;
@@ -5981,7 +5989,9 @@ static int update_raid_disks(struct mddev *mddev, int raid_disks)
 	if (raid_disks <= 0 ||
 	    (mddev->max_disks && raid_disks >= mddev->max_disks))
 		return -EINVAL;
-	if (mddev->sync_thread || mddev->reshape_position != MaxSector)
+	if (mddev->sync_thread ||
+	    test_bit(MD_RECOVERY_RUNNING, &mddev->recovery) ||
+	    mddev->reshape_position != MaxSector)
 		return -EBUSY;
 
 	rdev_for_each(rdev, mddev) {
@@ -6969,7 +6979,7 @@ static unsigned int mdstat_poll(struct file *filp, poll_table *wait)
 	int mask;
 
 	if (md_unloading)
-		return POLLIN|POLLRDNORM|POLLERR|POLLPRI;;
+		return POLLIN|POLLRDNORM|POLLERR|POLLPRI;
 	poll_wait(filp, &md_event_waiters, wait);
 
 	/* always allow read */
@@ -7593,6 +7603,7 @@ static void md_start_sync(struct work_struct *ws)
 		clear_bit(MD_RECOVERY_REQUESTED, &mddev->recovery);
 		clear_bit(MD_RECOVERY_CHECK, &mddev->recovery);
 		clear_bit(MD_RECOVERY_RUNNING, &mddev->recovery);
+		wake_up(&resync_wait);
 		if (test_and_clear_bit(MD_RECOVERY_RECOVER,
 				       &mddev->recovery))
 			if (mddev->sysfs_action)
@@ -7761,6 +7772,7 @@ void md_check_recovery(struct mddev *mddev)
 	not_running:
 		if (!mddev->sync_thread) {
 			clear_bit(MD_RECOVERY_RUNNING, &mddev->recovery);
+			wake_up(&resync_wait);
 			if (test_and_clear_bit(MD_RECOVERY_RECOVER,
 					       &mddev->recovery))
 				if (mddev->sysfs_action)
@@ -7779,7 +7791,6 @@ void md_reap_sync_thread(struct mddev *mddev)
 
 	/* resync has finished, collect result */
 	md_unregister_thread(&mddev->sync_thread);
-	wake_up(&resync_wait);
 	if (!test_bit(MD_RECOVERY_INTR, &mddev->recovery) &&
 	    !test_bit(MD_RECOVERY_REQUESTED, &mddev->recovery)) {
 		/* success...*/
@@ -7807,6 +7818,7 @@ void md_reap_sync_thread(struct mddev *mddev)
 	clear_bit(MD_RECOVERY_RESHAPE, &mddev->recovery);
 	clear_bit(MD_RECOVERY_REQUESTED, &mddev->recovery);
 	clear_bit(MD_RECOVERY_CHECK, &mddev->recovery);
+	wake_up(&resync_wait);
 	/* flag recovery needed just to double check */
 	set_bit(MD_RECOVERY_NEEDED, &mddev->recovery);
 	sysfs_notify_dirent_safe(mddev->sysfs_action);
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index 9c66e5997fc8..c1b0d52bfcb0 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -2917,8 +2917,11 @@ static int fetch_block(struct stripe_head *sh, struct stripe_head_state *s,
 	     (sh->raid_conf->level <= 5 && s->failed && fdev[0]->towrite &&
 	      (!test_bit(R5_Insync, &dev->flags) || test_bit(STRIPE_PREREAD_ACTIVE, &sh->state)) &&
 	      !test_bit(R5_OVERWRITE, &fdev[0]->flags)) ||
-	     (sh->raid_conf->level == 6 && s->failed && s->to_write &&
-	      s->to_write - s->non_overwrite < sh->raid_conf->raid_disks - 2 &&
+	     ((sh->raid_conf->level == 6 ||
+	       sh->sector >= sh->raid_conf->mddev->recovery_cp)
+	      && s->failed && s->to_write &&
+	      (s->to_write - s->non_overwrite <
+	       sh->raid_conf->raid_disks - sh->raid_conf->max_degraded) &&
 	      (!test_bit(R5_Insync, &dev->flags) || test_bit(STRIPE_PREREAD_ACTIVE, &sh->state))))) {
 		/* we would like to get this block, possibly by computing it,
 		 * otherwise read it if the backing disk is insync
diff --git a/drivers/media/Kconfig b/drivers/media/Kconfig
index 3c89fcbc621e..49cd30870e0d 100644
--- a/drivers/media/Kconfig
+++ b/drivers/media/Kconfig
@@ -160,7 +160,6 @@ source "drivers/media/usb/Kconfig"
 source "drivers/media/pci/Kconfig"
 source "drivers/media/platform/Kconfig"
 source "drivers/media/mmc/Kconfig"
-source "drivers/media/parport/Kconfig"
 source "drivers/media/radio/Kconfig"
 
 comment "Supported FireWire (IEEE 1394) Adapters"
diff --git a/drivers/media/Makefile b/drivers/media/Makefile
index 620f275a45c9..e608bbce0c35 100644
--- a/drivers/media/Makefile
+++ b/drivers/media/Makefile
@@ -28,6 +28,6 @@ obj-y += rc/
 # Finally, merge the drivers that require the core
 #
 
-obj-y += common/ platform/ pci/ usb/ mmc/ firewire/ parport/
+obj-y += common/ platform/ pci/ usb/ mmc/ firewire/
 obj-$(CONFIG_VIDEO_DEV) += radio/
 
diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
index f40b4cf6107a..205d71364343 100644
--- a/drivers/media/i2c/Kconfig
+++ b/drivers/media/i2c/Kconfig
@@ -284,15 +284,6 @@ config VIDEO_SAA711X
 	  To compile this driver as a module, choose M here: the
 	  module will be called saa7115.
 
-config VIDEO_SAA7191
-	tristate "Philips SAA7191 video decoder"
-	depends on VIDEO_V4L2 && I2C
-	---help---
-	  Support for the Philips SAA7191 video decoder.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called saa7191.
-
 config VIDEO_TVP514X
 	tristate "Texas Instruments TVP514x video decoder"
 	depends on VIDEO_V4L2 && I2C
diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
index 01ae9328e582..9858900168bf 100644
--- a/drivers/media/i2c/Makefile
+++ b/drivers/media/i2c/Makefile
@@ -18,7 +18,6 @@ obj-$(CONFIG_VIDEO_SAA711X) += saa7115.o
 obj-$(CONFIG_VIDEO_SAA717X) += saa717x.o
 obj-$(CONFIG_VIDEO_SAA7127) += saa7127.o
 obj-$(CONFIG_VIDEO_SAA7185) += saa7185.o
-obj-$(CONFIG_VIDEO_SAA7191) += saa7191.o
 obj-$(CONFIG_VIDEO_SAA6752HS) += saa6752hs.o
 obj-$(CONFIG_VIDEO_ADV7170) += adv7170.o
 obj-$(CONFIG_VIDEO_ADV7175) += adv7175.o
diff --git a/drivers/media/pci/cx18/cx18-driver.h b/drivers/media/pci/cx18/cx18-driver.h
index dcfd7a1d317e..207d6e82403b 100644
--- a/drivers/media/pci/cx18/cx18-driver.h
+++ b/drivers/media/pci/cx18/cx18-driver.h
@@ -290,7 +290,7 @@ struct cx18_options {
  * list_entry_is_past_end - check if a previous loop cursor is off list end
  * @pos:	the type * previously used as a loop cursor.
  * @head:	the head for your list.
- * @member:	the name of the list_struct within the struct.
+ * @member:	the name of the list_head within the struct.
  *
  * Check if the entry's list_head is the head of the list, thus it's not a
  * real entry but was the loop cursor that walked past the end
diff --git a/drivers/media/pci/cx88/cx88-blackbird.c b/drivers/media/pci/cx88/cx88-blackbird.c
index 4160ca4e5413..d3c79d964f2c 100644
--- a/drivers/media/pci/cx88/cx88-blackbird.c
+++ b/drivers/media/pci/cx88/cx88-blackbird.c
@@ -647,6 +647,7 @@ static int queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt,
 	dev->ts_packet_size  = 188 * 4;
 	dev->ts_packet_count  = 32;
 	sizes[0] = dev->ts_packet_size * dev->ts_packet_count;
+	alloc_ctxs[0] = dev->alloc_ctx;
 	return 0;
 }
 
@@ -662,14 +663,11 @@ static void buffer_finish(struct vb2_buffer *vb)
 {
 	struct cx8802_dev *dev = vb->vb2_queue->drv_priv;
 	struct cx88_buffer *buf = container_of(vb, struct cx88_buffer, vb);
-	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, 0);
 	struct cx88_riscmem *risc = &buf->risc;
 
 	if (risc->cpu)
 		pci_free_consistent(dev->pci, risc->size, risc->cpu, risc->dma);
 	memset(risc, 0, sizeof(*risc));
-
-	dma_unmap_sg(&dev->pci->dev, sgt->sgl, sgt->nents, DMA_FROM_DEVICE);
 }
 
 static void buffer_queue(struct vb2_buffer *vb)
diff --git a/drivers/media/pci/cx88/cx88-dvb.c b/drivers/media/pci/cx88/cx88-dvb.c
index c344bfd0b896..5780e2f013b4 100644
--- a/drivers/media/pci/cx88/cx88-dvb.c
+++ b/drivers/media/pci/cx88/cx88-dvb.c
@@ -92,6 +92,7 @@ static int queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt,
 	dev->ts_packet_size  = 188 * 4;
 	dev->ts_packet_count = dvb_buf_tscnt;
 	sizes[0] = dev->ts_packet_size * dev->ts_packet_count;
+	alloc_ctxs[0] = dev->alloc_ctx;
 	*num_buffers = dvb_buf_tscnt;
 	return 0;
 }
@@ -108,14 +109,11 @@ static void buffer_finish(struct vb2_buffer *vb)
 {
 	struct cx8802_dev *dev = vb->vb2_queue->drv_priv;
 	struct cx88_buffer *buf = container_of(vb, struct cx88_buffer, vb);
-	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, 0);
 	struct cx88_riscmem *risc = &buf->risc;
 
 	if (risc->cpu)
 		pci_free_consistent(dev->pci, risc->size, risc->cpu, risc->dma);
 	memset(risc, 0, sizeof(*risc));
-
-	dma_unmap_sg(&dev->pci->dev, sgt->sgl, sgt->nents, DMA_FROM_DEVICE);
 }
 
 static void buffer_queue(struct vb2_buffer *vb)
diff --git a/drivers/media/pci/cx88/cx88-mpeg.c b/drivers/media/pci/cx88/cx88-mpeg.c
index f181a3a10389..1c1f69e6b0b9 100644
--- a/drivers/media/pci/cx88/cx88-mpeg.c
+++ b/drivers/media/pci/cx88/cx88-mpeg.c
@@ -235,10 +235,6 @@ int cx8802_buf_prepare(struct vb2_queue *q, struct cx8802_dev *dev,
 		return -EINVAL;
 	vb2_set_plane_payload(&buf->vb, 0, size);
 
-	rc = dma_map_sg(&dev->pci->dev, sgt->sgl, sgt->nents, DMA_FROM_DEVICE);
-	if (!rc)
-		return -EIO;
-
 	rc = cx88_risc_databuffer(dev->pci, risc, sgt->sgl,
 			     dev->ts_packet_size, dev->ts_packet_count, 0);
 	if (rc) {
@@ -733,6 +729,11 @@ static int cx8802_probe(struct pci_dev *pci_dev,
 	if (NULL == dev)
 		goto fail_core;
 	dev->pci = pci_dev;
+	dev->alloc_ctx = vb2_dma_sg_init_ctx(&pci_dev->dev);
+	if (IS_ERR(dev->alloc_ctx)) {
+		err = PTR_ERR(dev->alloc_ctx);
+		goto fail_core;
+	}
 	dev->core = core;
 
 	/* Maintain a reference so cx88-video can query the 8802 device. */
@@ -752,6 +753,7 @@ static int cx8802_probe(struct pci_dev *pci_dev,
 	return 0;
 
  fail_free:
+	vb2_dma_sg_cleanup_ctx(dev->alloc_ctx);
 	kfree(dev);
  fail_core:
 	core->dvbdev = NULL;
@@ -798,6 +800,7 @@ static void cx8802_remove(struct pci_dev *pci_dev)
 	/* common */
 	cx8802_fini_common(dev);
 	cx88_core_put(dev->core,dev->pci);
+	vb2_dma_sg_cleanup_ctx(dev->alloc_ctx);
 	kfree(dev);
 }
 
diff --git a/drivers/media/pci/cx88/cx88-vbi.c b/drivers/media/pci/cx88/cx88-vbi.c
index 6ab6e27648f6..32eb7fdb875e 100644
--- a/drivers/media/pci/cx88/cx88-vbi.c
+++ b/drivers/media/pci/cx88/cx88-vbi.c
@@ -120,6 +120,7 @@ static int queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt,
 		sizes[0] = VBI_LINE_NTSC_COUNT * VBI_LINE_LENGTH * 2;
 	else
 		sizes[0] = VBI_LINE_PAL_COUNT * VBI_LINE_LENGTH * 2;
+	alloc_ctxs[0] = dev->alloc_ctx;
 	return 0;
 }
 
@@ -131,7 +132,6 @@ static int buffer_prepare(struct vb2_buffer *vb)
 	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, 0);
 	unsigned int lines;
 	unsigned int size;
-	int rc;
 
 	if (dev->core->tvnorm & V4L2_STD_525_60)
 		lines = VBI_LINE_NTSC_COUNT;
@@ -142,10 +142,6 @@ static int buffer_prepare(struct vb2_buffer *vb)
 		return -EINVAL;
 	vb2_set_plane_payload(vb, 0, size);
 
-	rc = dma_map_sg(&dev->pci->dev, sgt->sgl, sgt->nents, DMA_FROM_DEVICE);
-	if (!rc)
-		return -EIO;
-
 	cx88_risc_buffer(dev->pci, &buf->risc, sgt->sgl,
 			 0, VBI_LINE_LENGTH * lines,
 			 VBI_LINE_LENGTH, 0,
@@ -157,14 +153,11 @@ static void buffer_finish(struct vb2_buffer *vb)
 {
 	struct cx8800_dev *dev = vb->vb2_queue->drv_priv;
 	struct cx88_buffer *buf = container_of(vb, struct cx88_buffer, vb);
-	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, 0);
 	struct cx88_riscmem *risc = &buf->risc;
 
 	if (risc->cpu)
 		pci_free_consistent(dev->pci, risc->size, risc->cpu, risc->dma);
 	memset(risc, 0, sizeof(*risc));
-
-	dma_unmap_sg(&dev->pci->dev, sgt->sgl, sgt->nents, DMA_FROM_DEVICE);
 }
 
 static void buffer_queue(struct vb2_buffer *vb)
diff --git a/drivers/media/pci/cx88/cx88-video.c b/drivers/media/pci/cx88/cx88-video.c
index a64ae31ae142..860c98fc72c7 100644
--- a/drivers/media/pci/cx88/cx88-video.c
+++ b/drivers/media/pci/cx88/cx88-video.c
@@ -440,6 +440,7 @@ static int queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt,
 
 	*num_planes = 1;
 	sizes[0] = (dev->fmt->depth * core->width * core->height) >> 3;
+	alloc_ctxs[0] = dev->alloc_ctx;
 	return 0;
 }
 
@@ -449,7 +450,6 @@ static int buffer_prepare(struct vb2_buffer *vb)
 	struct cx88_core *core = dev->core;
 	struct cx88_buffer *buf = container_of(vb, struct cx88_buffer, vb);
 	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, 0);
-	int rc;
 
 	buf->bpl = core->width * dev->fmt->depth >> 3;
 
@@ -457,10 +457,6 @@ static int buffer_prepare(struct vb2_buffer *vb)
 		return -EINVAL;
 	vb2_set_plane_payload(vb, 0, core->height * buf->bpl);
 
-	rc = dma_map_sg(&dev->pci->dev, sgt->sgl, sgt->nents, DMA_FROM_DEVICE);
-	if (!rc)
-		return -EIO;
-
 	switch (core->field) {
 	case V4L2_FIELD_TOP:
 		cx88_risc_buffer(dev->pci, &buf->risc,
@@ -505,14 +501,11 @@ static void buffer_finish(struct vb2_buffer *vb)
 {
 	struct cx8800_dev *dev = vb->vb2_queue->drv_priv;
 	struct cx88_buffer *buf = container_of(vb, struct cx88_buffer, vb);
-	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, 0);
 	struct cx88_riscmem *risc = &buf->risc;
 
 	if (risc->cpu)
 		pci_free_consistent(dev->pci, risc->size, risc->cpu, risc->dma);
 	memset(risc, 0, sizeof(*risc));
-
-	dma_unmap_sg(&dev->pci->dev, sgt->sgl, sgt->nents, DMA_FROM_DEVICE);
 }
 
 static void buffer_queue(struct vb2_buffer *vb)
@@ -530,7 +523,6 @@ static void buffer_queue(struct vb2_buffer *vb)
 
 	if (list_empty(&q->active)) {
 		list_add_tail(&buf->list, &q->active);
-		start_video_dma(dev, q, buf);
 		buf->count    = q->count++;
 		dprintk(2,"[%p/%d] buffer_queue - first active\n",
 			buf, buf->vb.v4l2_buf.index);
@@ -1345,6 +1337,12 @@ static int cx8800_initdev(struct pci_dev *pci_dev,
 		err = -EIO;
 		goto fail_core;
 	}
+	dev->alloc_ctx = vb2_dma_sg_init_ctx(&pci_dev->dev);
+	if (IS_ERR(dev->alloc_ctx)) {
+		err = PTR_ERR(dev->alloc_ctx);
+		goto fail_core;
+	}
+
 
 	/* initialize driver struct */
 	spin_lock_init(&dev->slock);
@@ -1549,6 +1547,7 @@ fail_unreg:
 	free_irq(pci_dev->irq, dev);
 	mutex_unlock(&core->lock);
 fail_core:
+	vb2_dma_sg_cleanup_ctx(dev->alloc_ctx);
 	core->v4ldev = NULL;
 	cx88_core_put(core,dev->pci);
 fail_free:
@@ -1582,6 +1581,7 @@ static void cx8800_finidev(struct pci_dev *pci_dev)
 
 	/* free memory */
 	cx88_core_put(core,dev->pci);
+	vb2_dma_sg_cleanup_ctx(dev->alloc_ctx);
 	kfree(dev);
 }
 
diff --git a/drivers/media/pci/cx88/cx88.h b/drivers/media/pci/cx88/cx88.h
index 3b0ae754f165..7748ca9abb09 100644
--- a/drivers/media/pci/cx88/cx88.h
+++ b/drivers/media/pci/cx88/cx88.h
@@ -485,6 +485,7 @@ struct cx8800_dev {
 	/* pci i/o */
 	struct pci_dev             *pci;
 	unsigned char              pci_rev,pci_lat;
+	void			   *alloc_ctx;
 
 	const struct cx8800_fmt    *fmt;
 
@@ -548,6 +549,7 @@ struct cx8802_dev {
 	/* pci i/o */
 	struct pci_dev             *pci;
 	unsigned char              pci_rev,pci_lat;
+	void			   *alloc_ctx;
 
 	/* dma queues */
 	struct cx88_dmaqueue       mpegq;
diff --git a/drivers/media/pci/ttpci/budget-patch.c b/drivers/media/pci/ttpci/budget-patch.c
index 2cb35c23d2ac..a4d8867e1d7b 100644
--- a/drivers/media/pci/ttpci/budget-patch.c
+++ b/drivers/media/pci/ttpci/budget-patch.c
@@ -490,7 +490,7 @@ static int budget_patch_attach (struct saa7146_dev* dev, struct saa7146_pci_exte
 
 	if(detected == 0)
 		printk("budget-patch not detected or saa7146 in non-default state.\n"
-		       "try enabling ressetting of 7146 with MASK_31 in MC1 register\n");
+		       "try enabling resetting of 7146 with MASK_31 in MC1 register\n");
 
 	else
 		printk("BUDGET-PATCH DETECTED.\n");
diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index 0c61155699f7..765bffb49a72 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -65,14 +65,6 @@ config VIDEO_TIMBERDALE
 	---help---
 	  Add support for the Video In peripherial of the timberdale FPGA.
 
-config VIDEO_VINO
-	tristate "SGI Vino Video For Linux"
-	depends on I2C && SGI_IP22 && VIDEO_V4L2
-	select VIDEO_SAA7191 if MEDIA_SUBDRV_AUTOSELECT
-	help
-	  Say Y here to build in support for the Vino video input system found
-	  on SGI Indy machines.
-
 config VIDEO_M32R_AR
 	tristate "AR devices"
 	depends on VIDEO_V4L2
@@ -112,7 +104,7 @@ config VIDEO_OMAP3_DEBUG
 config VIDEO_S3C_CAMIF
 	tristate "Samsung S3C24XX/S3C64XX SoC Camera Interface driver"
 	depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API
-	depends on PM_RUNTIME
+	depends on PM
 	depends on ARCH_S3C64XX || PLAT_S3C24XX || COMPILE_TEST
 	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
diff --git a/drivers/media/platform/Makefile b/drivers/media/platform/Makefile
index b818afb4d336..a49936b8ce8a 100644
--- a/drivers/media/platform/Makefile
+++ b/drivers/media/platform/Makefile
@@ -2,9 +2,6 @@
 # Makefile for the video capture/playback device drivers.
 #
 
-obj-$(CONFIG_VIDEO_VINO) += indycam.o
-obj-$(CONFIG_VIDEO_VINO) += vino.o
-
 obj-$(CONFIG_VIDEO_TIMBERDALE)	+= timblogiw.o
 obj-$(CONFIG_VIDEO_M32R_AR_M64278) += arv.o
 
diff --git a/drivers/media/platform/blackfin/bfin_capture.c b/drivers/media/platform/blackfin/bfin_capture.c
index 3112844e4fc1..8f6698668ecf 100644
--- a/drivers/media/platform/blackfin/bfin_capture.c
+++ b/drivers/media/platform/blackfin/bfin_capture.c
@@ -1120,7 +1120,6 @@ static int bcap_remove(struct platform_device *pdev)
 static struct platform_driver bcap_driver = {
 	.driver = {
 		.name  = CAPTURE_DRV_NAME,
-		.owner = THIS_MODULE,
 	},
 	.probe = bcap_probe,
 	.remove = bcap_remove,
diff --git a/drivers/media/platform/coda/coda-common.c b/drivers/media/platform/coda/coda-common.c
index 90b7791d36e1..39330a70f752 100644
--- a/drivers/media/platform/coda/coda-common.c
+++ b/drivers/media/platform/coda/coda-common.c
@@ -2225,7 +2225,6 @@ static struct platform_driver coda_driver = {
 	.remove	= coda_remove,
 	.driver	= {
 		.name	= CODA_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(coda_dt_ids),
 		.pm	= &coda_pm_ops,
 	},
diff --git a/drivers/media/platform/davinci/dm355_ccdc.c b/drivers/media/platform/davinci/dm355_ccdc.c
index 3f44deb5b7a7..c90b9a4f0c24 100644
--- a/drivers/media/platform/davinci/dm355_ccdc.c
+++ b/drivers/media/platform/davinci/dm355_ccdc.c
@@ -1030,7 +1030,6 @@ static int dm355_ccdc_remove(struct platform_device *pdev)
 static struct platform_driver dm355_ccdc_driver = {
 	.driver = {
 		.name	= "dm355_ccdc",
-		.owner = THIS_MODULE,
 	},
 	.remove = dm355_ccdc_remove,
 	.probe = dm355_ccdc_probe,
diff --git a/drivers/media/platform/davinci/dm644x_ccdc.c b/drivers/media/platform/davinci/dm644x_ccdc.c
index 62a0ebb01056..ffbefdff6b5e 100644
--- a/drivers/media/platform/davinci/dm644x_ccdc.c
+++ b/drivers/media/platform/davinci/dm644x_ccdc.c
@@ -1029,7 +1029,6 @@ static const struct dev_pm_ops dm644x_ccdc_pm_ops = {
 static struct platform_driver dm644x_ccdc_driver = {
 	.driver = {
 		.name	= "dm644x_ccdc",
-		.owner = THIS_MODULE,
 		.pm = &dm644x_ccdc_pm_ops,
 	},
 	.remove = dm644x_ccdc_remove,
diff --git a/drivers/media/platform/davinci/isif.c b/drivers/media/platform/davinci/isif.c
index 3332cca632e5..99faea2e84c6 100644
--- a/drivers/media/platform/davinci/isif.c
+++ b/drivers/media/platform/davinci/isif.c
@@ -1134,7 +1134,6 @@ static int isif_remove(struct platform_device *pdev)
 static struct platform_driver isif_driver = {
 	.driver = {
 		.name	= "isif",
-		.owner = THIS_MODULE,
 	},
 	.remove = isif_remove,
 	.probe = isif_probe,
diff --git a/drivers/media/platform/davinci/vpbe.c b/drivers/media/platform/davinci/vpbe.c
index 244d3d6c244c..9a6c2cc38acb 100644
--- a/drivers/media/platform/davinci/vpbe.c
+++ b/drivers/media/platform/davinci/vpbe.c
@@ -876,7 +876,6 @@ static int vpbe_remove(struct platform_device *device)
 static struct platform_driver vpbe_driver = {
 	.driver	= {
 		.name	= "vpbe_controller",
-		.owner	= THIS_MODULE,
 	},
 	.probe = vpbe_probe,
 	.remove = vpbe_remove,
diff --git a/drivers/media/platform/davinci/vpbe_display.c b/drivers/media/platform/davinci/vpbe_display.c
index 21a5a56eb5ea..c4ab46f5bd92 100644
--- a/drivers/media/platform/davinci/vpbe_display.c
+++ b/drivers/media/platform/davinci/vpbe_display.c
@@ -1526,7 +1526,6 @@ static int vpbe_display_remove(struct platform_device *pdev)
 static struct platform_driver vpbe_display_driver = {
 	.driver = {
 		.name = VPBE_DISPLAY_DRIVER,
-		.owner = THIS_MODULE,
 		.bus = &platform_bus_type,
 	},
 	.probe = vpbe_display_probe,
diff --git a/drivers/media/platform/davinci/vpbe_osd.c b/drivers/media/platform/davinci/vpbe_osd.c
index d053c2669c1f..7d96a4b13b32 100644
--- a/drivers/media/platform/davinci/vpbe_osd.c
+++ b/drivers/media/platform/davinci/vpbe_osd.c
@@ -1585,7 +1585,6 @@ static struct platform_driver osd_driver = {
 	.remove		= osd_remove,
 	.driver		= {
 		.name	= MODULE_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.id_table	= vpbe_osd_devtype
 };
diff --git a/drivers/media/platform/davinci/vpbe_venc.c b/drivers/media/platform/davinci/vpbe_venc.c
index 14a023a75d2d..36ed1466b290 100644
--- a/drivers/media/platform/davinci/vpbe_venc.c
+++ b/drivers/media/platform/davinci/vpbe_venc.c
@@ -687,7 +687,6 @@ static struct platform_driver venc_driver = {
 	.remove		= venc_remove,
 	.driver		= {
 		.name	= MODULE_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.id_table	= vpbe_venc_devtype
 };
diff --git a/drivers/media/platform/davinci/vpfe_capture.c b/drivers/media/platform/davinci/vpfe_capture.c
index 271c4600432d..b41bf7e822c8 100644
--- a/drivers/media/platform/davinci/vpfe_capture.c
+++ b/drivers/media/platform/davinci/vpfe_capture.c
@@ -2031,7 +2031,6 @@ static const struct dev_pm_ops vpfe_dev_pm_ops = {
 static struct platform_driver vpfe_driver = {
 	.driver = {
 		.name = CAPTURE_DRV_NAME,
-		.owner = THIS_MODULE,
 		.pm = &vpfe_dev_pm_ops,
 	},
 	.probe = vpfe_probe,
diff --git a/drivers/media/platform/davinci/vpif.c b/drivers/media/platform/davinci/vpif.c
index 3dad5bd7fe0a..0380cf2e5775 100644
--- a/drivers/media/platform/davinci/vpif.c
+++ b/drivers/media/platform/davinci/vpif.c
@@ -467,7 +467,6 @@ static const struct dev_pm_ops vpif_pm = {
 static struct platform_driver vpif_driver = {
 	.driver = {
 		.name	= "vpif",
-		.owner = THIS_MODULE,
 		.pm	= vpif_pm_ops,
 	},
 	.remove = vpif_remove,
diff --git a/drivers/media/platform/davinci/vpif_capture.c b/drivers/media/platform/davinci/vpif_capture.c
index d8e1b98b7784..fa0a51521772 100644
--- a/drivers/media/platform/davinci/vpif_capture.c
+++ b/drivers/media/platform/davinci/vpif_capture.c
@@ -1658,7 +1658,6 @@ static SIMPLE_DEV_PM_OPS(vpif_pm_ops, vpif_suspend, vpif_resume);
 static __refdata struct platform_driver vpif_driver = {
 	.driver	= {
 		.name	= VPIF_DRIVER_NAME,
-		.owner	= THIS_MODULE,
 		.pm	= &vpif_pm_ops,
 	},
 	.probe = vpif_probe,
diff --git a/drivers/media/platform/davinci/vpif_display.c b/drivers/media/platform/davinci/vpif_display.c
index 8d6ced56253c..839c24de1fd8 100644
--- a/drivers/media/platform/davinci/vpif_display.c
+++ b/drivers/media/platform/davinci/vpif_display.c
@@ -1459,7 +1459,6 @@ static SIMPLE_DEV_PM_OPS(vpif_pm_ops, vpif_suspend, vpif_resume);
 static __refdata struct platform_driver vpif_driver = {
 	.driver	= {
 			.name	= VPIF_DRIVER_NAME,
-			.owner	= THIS_MODULE,
 			.pm	= &vpif_pm_ops,
 	},
 	.probe	= vpif_probe,
diff --git a/drivers/media/platform/davinci/vpss.c b/drivers/media/platform/davinci/vpss.c
index 31120b4a4a33..fce86f17dffc 100644
--- a/drivers/media/platform/davinci/vpss.c
+++ b/drivers/media/platform/davinci/vpss.c
@@ -504,7 +504,6 @@ static const struct dev_pm_ops vpss_pm_ops = {
 static struct platform_driver vpss_driver = {
 	.driver = {
 		.name	= "vpss",
-		.owner = THIS_MODULE,
 		.pm = &vpss_pm_ops,
 	},
 	.remove = vpss_remove,
diff --git a/drivers/media/platform/exynos-gsc/gsc-core.c b/drivers/media/platform/exynos-gsc/gsc-core.c
index 3062e9fac6da..fd2891c886a3 100644
--- a/drivers/media/platform/exynos-gsc/gsc-core.c
+++ b/drivers/media/platform/exynos-gsc/gsc-core.c
@@ -1256,7 +1256,6 @@ static struct platform_driver gsc_driver = {
 	.id_table	= gsc_driver_ids,
 	.driver = {
 		.name	= GSC_MODULE_NAME,
-		.owner	= THIS_MODULE,
 		.pm	= &gsc_pm_ops,
 		.of_match_table = exynos_gsc_match,
 	}
diff --git a/drivers/media/platform/exynos4-is/fimc-core.c b/drivers/media/platform/exynos4-is/fimc-core.c
index 2acc19573b6b..1101c41ac117 100644
--- a/drivers/media/platform/exynos4-is/fimc-core.c
+++ b/drivers/media/platform/exynos4-is/fimc-core.c
@@ -1294,7 +1294,6 @@ static struct platform_driver fimc_driver = {
 	.driver = {
 		.of_match_table = fimc_of_match,
 		.name		= FIMC_DRIVER_NAME,
-		.owner		= THIS_MODULE,
 		.pm     	= &fimc_pm_ops,
 	}
 };
diff --git a/drivers/media/platform/exynos4-is/fimc-is-i2c.c b/drivers/media/platform/exynos4-is/fimc-is-i2c.c
index d83033170789..7521aa59b064 100644
--- a/drivers/media/platform/exynos4-is/fimc-is-i2c.c
+++ b/drivers/media/platform/exynos4-is/fimc-is-i2c.c
@@ -133,7 +133,6 @@ static struct platform_driver fimc_is_i2c_driver = {
 	.driver = {
 		.of_match_table = fimc_is_i2c_of_match,
 		.name		= "fimc-isp-i2c",
-		.owner		= THIS_MODULE,
 		.pm		= &fimc_is_i2c_pm_ops,
 	}
 };
diff --git a/drivers/media/platform/exynos4-is/fimc-is.c b/drivers/media/platform/exynos4-is/fimc-is.c
index 2a0cbeff0324..49658ca39e51 100644
--- a/drivers/media/platform/exynos4-is/fimc-is.c
+++ b/drivers/media/platform/exynos4-is/fimc-is.c
@@ -960,7 +960,6 @@ static struct platform_driver fimc_is_driver = {
 	.driver = {
 		.of_match_table	= fimc_is_of_match,
 		.name		= FIMC_IS_DRV_NAME,
-		.owner		= THIS_MODULE,
 		.pm		= &fimc_is_pm_ops,
 	}
 };
diff --git a/drivers/media/platform/exynos4-is/fimc-lite.c b/drivers/media/platform/exynos4-is/fimc-lite.c
index 7e8687338155..2510f189e242 100644
--- a/drivers/media/platform/exynos4-is/fimc-lite.c
+++ b/drivers/media/platform/exynos4-is/fimc-lite.c
@@ -1720,7 +1720,6 @@ static struct platform_driver fimc_lite_driver = {
 	.driver = {
 		.of_match_table = flite_of_match,
 		.name		= FIMC_LITE_DRV_NAME,
-		.owner		= THIS_MODULE,
 		.pm		= &fimc_lite_pm_ops,
 	}
 };
diff --git a/drivers/media/platform/exynos4-is/media-dev.c b/drivers/media/platform/exynos4-is/media-dev.c
index 54c49d5e7690..f315ef946cd4 100644
--- a/drivers/media/platform/exynos4-is/media-dev.c
+++ b/drivers/media/platform/exynos4-is/media-dev.c
@@ -1469,7 +1469,6 @@ static struct platform_driver fimc_md_driver = {
 	.driver = {
 		.of_match_table = of_match_ptr(fimc_md_of_match),
 		.name		= "s5p-fimc-md",
-		.owner		= THIS_MODULE,
 	}
 };
 
diff --git a/drivers/media/platform/exynos4-is/mipi-csis.c b/drivers/media/platform/exynos4-is/mipi-csis.c
index 9e1056d60c7e..2504aa89a6f4 100644
--- a/drivers/media/platform/exynos4-is/mipi-csis.c
+++ b/drivers/media/platform/exynos4-is/mipi-csis.c
@@ -1041,7 +1041,6 @@ static struct platform_driver s5pcsis_driver = {
 	.driver		= {
 		.of_match_table = s5pcsis_of_match,
 		.name		= CSIS_DRIVER_NAME,
-		.owner		= THIS_MODULE,
 		.pm		= &s5pcsis_pm_ops,
 	},
 };
diff --git a/drivers/media/platform/fsl-viu.c b/drivers/media/platform/fsl-viu.c
index 8afee3c17c1c..bbf428104871 100644
--- a/drivers/media/platform/fsl-viu.c
+++ b/drivers/media/platform/fsl-viu.c
@@ -1681,7 +1681,6 @@ static struct platform_driver viu_of_platform_driver = {
 #endif
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = mpc512x_viu_of_match,
 	},
 };
diff --git a/drivers/media/platform/m2m-deinterlace.c b/drivers/media/platform/m2m-deinterlace.c
index d36c507a0ba2..b70c1aecca37 100644
--- a/drivers/media/platform/m2m-deinterlace.c
+++ b/drivers/media/platform/m2m-deinterlace.c
@@ -1101,7 +1101,6 @@ static struct platform_driver deinterlace_pdrv = {
 	.remove		= deinterlace_remove,
 	.driver		= {
 		.name	= MEM2MEM_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(deinterlace_pdrv);
diff --git a/drivers/media/platform/marvell-ccic/mmp-driver.c b/drivers/media/platform/marvell-ccic/mmp-driver.c
index 054507f16734..0ed9b3adfcdf 100644
--- a/drivers/media/platform/marvell-ccic/mmp-driver.c
+++ b/drivers/media/platform/marvell-ccic/mmp-driver.c
@@ -512,7 +512,6 @@ static struct platform_driver mmpcam_driver = {
 #endif
 	.driver = {
 		.name	= "mmp-camera",
-		.owner	= THIS_MODULE
 	}
 };
 
diff --git a/drivers/media/platform/mx2_emmaprp.c b/drivers/media/platform/mx2_emmaprp.c
index f923d1bc43a0..87314b743f55 100644
--- a/drivers/media/platform/mx2_emmaprp.c
+++ b/drivers/media/platform/mx2_emmaprp.c
@@ -1005,7 +1005,6 @@ static struct platform_driver emmaprp_pdrv = {
 	.remove		= emmaprp_remove,
 	.driver		= {
 		.name	= MEM2MEM_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(emmaprp_pdrv);
diff --git a/drivers/media/platform/omap3isp/isp.c b/drivers/media/platform/omap3isp/isp.c
index 72265e58ca60..51c2129bdcc6 100644
--- a/drivers/media/platform/omap3isp/isp.c
+++ b/drivers/media/platform/omap3isp/isp.c
@@ -2380,7 +2380,6 @@ static struct platform_driver omap3isp_driver = {
 	.remove = isp_remove,
 	.id_table = omap3isp_id_table,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "omap3isp",
 		.pm	= &omap3isp_pm_ops,
 	},
diff --git a/drivers/media/platform/s3c-camif/camif-core.c b/drivers/media/platform/s3c-camif/camif-core.c
index 3b09b5b6ad51..2d5bd3ac7f81 100644
--- a/drivers/media/platform/s3c-camif/camif-core.c
+++ b/drivers/media/platform/s3c-camif/camif-core.c
@@ -651,7 +651,6 @@ static struct platform_driver s3c_camif_driver = {
 	.id_table	= s3c_camif_driver_ids,
 	.driver = {
 		.name	= S3C_CAMIF_DRIVER_NAME,
-		.owner	= THIS_MODULE,
 		.pm	= &s3c_camif_pm_ops,
 	}
 };
diff --git a/drivers/media/platform/s5p-g2d/g2d.c b/drivers/media/platform/s5p-g2d/g2d.c
index 51e4edc92d28..47ba8fbb0426 100644
--- a/drivers/media/platform/s5p-g2d/g2d.c
+++ b/drivers/media/platform/s5p-g2d/g2d.c
@@ -806,7 +806,6 @@ static struct platform_driver g2d_pdrv = {
 	.id_table	= g2d_driver_ids,
 	.driver		= {
 		.name = G2D_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = exynos_g2d_match,
 	},
 };
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-core.c b/drivers/media/platform/s5p-jpeg/jpeg-core.c
index 3ecf0e06a4dd..12f7452edce3 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-core.c
+++ b/drivers/media/platform/s5p-jpeg/jpeg-core.c
@@ -2763,7 +2763,6 @@ static struct platform_driver s5p_jpeg_driver = {
 	.remove = s5p_jpeg_remove,
 	.driver = {
 		.of_match_table	= of_match_ptr(samsung_jpeg_match),
-		.owner		= THIS_MODULE,
 		.name		= S5P_JPEG_M2M_NAME,
 		.pm		= &s5p_jpeg_pm_ops,
 	},
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc.c b/drivers/media/platform/s5p-mfc/s5p_mfc.c
index b80a576164f1..fbfdf03b9054 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc.c
@@ -1516,7 +1516,6 @@ static struct platform_driver s5p_mfc_driver = {
 	.id_table	= mfc_driver_ids,
 	.driver	= {
 		.name	= S5P_MFC_NAME,
-		.owner	= THIS_MODULE,
 		.pm	= &s5p_mfc_pm_ops,
 		.of_match_table = exynos_mfc_match,
 	},
diff --git a/drivers/media/platform/s5p-tv/Kconfig b/drivers/media/platform/s5p-tv/Kconfig
index beb180e71ba0..5a1835dd65e8 100644
--- a/drivers/media/platform/s5p-tv/Kconfig
+++ b/drivers/media/platform/s5p-tv/Kconfig
@@ -8,7 +8,7 @@
 
 config VIDEO_SAMSUNG_S5P_TV
 	bool "Samsung TV driver for S5P platform"
-	depends on PM_RUNTIME
+	depends on PM
 	depends on ARCH_S5PV210 || ARCH_EXYNOS || COMPILE_TEST
 	default n
 	---help---
diff --git a/drivers/media/platform/s5p-tv/hdmi_drv.c b/drivers/media/platform/s5p-tv/hdmi_drv.c
index 1d1ef211e113..0e74aabf5f9a 100644
--- a/drivers/media/platform/s5p-tv/hdmi_drv.c
+++ b/drivers/media/platform/s5p-tv/hdmi_drv.c
@@ -1046,7 +1046,6 @@ static struct platform_driver hdmi_driver __refdata = {
 	.id_table = hdmi_driver_types,
 	.driver = {
 		.name = "s5p-hdmi",
-		.owner = THIS_MODULE,
 		.pm = &hdmi_pm_ops,
 	}
 };
diff --git a/drivers/media/platform/s5p-tv/mixer_drv.c b/drivers/media/platform/s5p-tv/mixer_drv.c
index bc08b5f28e44..2a9501d7e7c8 100644
--- a/drivers/media/platform/s5p-tv/mixer_drv.c
+++ b/drivers/media/platform/s5p-tv/mixer_drv.c
@@ -487,7 +487,6 @@ static struct platform_driver mxr_driver __refdata = {
 	.remove = mxr_remove,
 	.driver = {
 		.name = MXR_DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.pm = &mxr_pm_ops,
 	}
 };
diff --git a/drivers/media/platform/s5p-tv/sdo_drv.c b/drivers/media/platform/s5p-tv/sdo_drv.c
index 46f4d56aaa1b..3621af91d460 100644
--- a/drivers/media/platform/s5p-tv/sdo_drv.c
+++ b/drivers/media/platform/s5p-tv/sdo_drv.c
@@ -482,7 +482,6 @@ static struct platform_driver sdo_driver __refdata = {
 	.remove = sdo_remove,
 	.driver = {
 		.name = "s5p-sdo",
-		.owner = THIS_MODULE,
 		.pm = &sdo_pm_ops,
 	}
 };
diff --git a/drivers/media/platform/sh_veu.c b/drivers/media/platform/sh_veu.c
index be3b3bc71a0f..aaa1f6f25a29 100644
--- a/drivers/media/platform/sh_veu.c
+++ b/drivers/media/platform/sh_veu.c
@@ -1235,7 +1235,6 @@ static struct platform_driver __refdata sh_veu_pdrv = {
 	.remove		= sh_veu_remove,
 	.driver		= {
 		.name	= "sh_veu",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/media/platform/sh_vou.c b/drivers/media/platform/sh_vou.c
index 154ef0b6b8ab..261f1195b49f 100644
--- a/drivers/media/platform/sh_vou.c
+++ b/drivers/media/platform/sh_vou.c
@@ -1450,7 +1450,6 @@ static struct platform_driver __refdata sh_vou = {
 	.remove  = sh_vou_remove,
 	.driver  = {
 		.name	= "sh-vou",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/media/platform/soc_camera/atmel-isi.c b/drivers/media/platform/soc_camera/atmel-isi.c
index ee5650f4ea2d..8efe40337608 100644
--- a/drivers/media/platform/soc_camera/atmel-isi.c
+++ b/drivers/media/platform/soc_camera/atmel-isi.c
@@ -1068,7 +1068,6 @@ static struct platform_driver atmel_isi_driver = {
 	.remove		= atmel_isi_remove,
 	.driver		= {
 		.name = "atmel_isi",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(atmel_isi_of_match),
 	},
 };
diff --git a/drivers/media/platform/soc_camera/mx3_camera.c b/drivers/media/platform/soc_camera/mx3_camera.c
index 8e52ccce66de..a60c3bb0e4cc 100644
--- a/drivers/media/platform/soc_camera/mx3_camera.c
+++ b/drivers/media/platform/soc_camera/mx3_camera.c
@@ -1253,7 +1253,6 @@ static int mx3_camera_remove(struct platform_device *pdev)
 static struct platform_driver mx3_camera_driver = {
 	.driver		= {
 		.name	= MX3_CAM_DRV_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= mx3_camera_probe,
 	.remove		= mx3_camera_remove,
diff --git a/drivers/media/platform/soc_camera/rcar_vin.c b/drivers/media/platform/soc_camera/rcar_vin.c
index 8d8438b10b87..0c1f55648106 100644
--- a/drivers/media/platform/soc_camera/rcar_vin.c
+++ b/drivers/media/platform/soc_camera/rcar_vin.c
@@ -64,6 +64,30 @@
 #define VNDMR_REG	0x58	/* Video n Data Mode Register */
 #define VNDMR2_REG	0x5C	/* Video n Data Mode Register 2 */
 #define VNUVAOF_REG	0x60	/* Video n UV Address Offset Register */
+#define VNC1A_REG	0x80	/* Video n Coefficient Set C1A Register */
+#define VNC1B_REG	0x84	/* Video n Coefficient Set C1B Register */
+#define VNC1C_REG	0x88	/* Video n Coefficient Set C1C Register */
+#define VNC2A_REG	0x90	/* Video n Coefficient Set C2A Register */
+#define VNC2B_REG	0x94	/* Video n Coefficient Set C2B Register */
+#define VNC2C_REG	0x98	/* Video n Coefficient Set C2C Register */
+#define VNC3A_REG	0xA0	/* Video n Coefficient Set C3A Register */
+#define VNC3B_REG	0xA4	/* Video n Coefficient Set C3B Register */
+#define VNC3C_REG	0xA8	/* Video n Coefficient Set C3C Register */
+#define VNC4A_REG	0xB0	/* Video n Coefficient Set C4A Register */
+#define VNC4B_REG	0xB4	/* Video n Coefficient Set C4B Register */
+#define VNC4C_REG	0xB8	/* Video n Coefficient Set C4C Register */
+#define VNC5A_REG	0xC0	/* Video n Coefficient Set C5A Register */
+#define VNC5B_REG	0xC4	/* Video n Coefficient Set C5B Register */
+#define VNC5C_REG	0xC8	/* Video n Coefficient Set C5C Register */
+#define VNC6A_REG	0xD0	/* Video n Coefficient Set C6A Register */
+#define VNC6B_REG	0xD4	/* Video n Coefficient Set C6B Register */
+#define VNC6C_REG	0xD8	/* Video n Coefficient Set C6C Register */
+#define VNC7A_REG	0xE0	/* Video n Coefficient Set C7A Register */
+#define VNC7B_REG	0xE4	/* Video n Coefficient Set C7B Register */
+#define VNC7C_REG	0xE8	/* Video n Coefficient Set C7C Register */
+#define VNC8A_REG	0xF0	/* Video n Coefficient Set C8A Register */
+#define VNC8B_REG	0xF4	/* Video n Coefficient Set C8B Register */
+#define VNC8C_REG	0xF8	/* Video n Coefficient Set C8C Register */
 
 /* Register bit fields for R-Car VIN */
 /* Video n Main Control Register bits */
@@ -106,6 +130,7 @@
 #define VNDMR2_VPS		(1 << 30)
 #define VNDMR2_HPS		(1 << 29)
 #define VNDMR2_FTEV		(1 << 17)
+#define VNDMR2_VLV(n)		((n & 0xf) << 12)
 
 #define VIN_MAX_WIDTH		2048
 #define VIN_MAX_HEIGHT		2048
@@ -117,6 +142,324 @@ enum chip_id {
 	RCAR_E1,
 };
 
+struct vin_coeff {
+	unsigned short xs_value;
+	u32 coeff_set[24];
+};
+
+static const struct vin_coeff vin_coeff_set[] = {
+	{ 0x0000, {
+		0x00000000,		0x00000000,		0x00000000,
+		0x00000000,		0x00000000,		0x00000000,
+		0x00000000,		0x00000000,		0x00000000,
+		0x00000000,		0x00000000,		0x00000000,
+		0x00000000,		0x00000000,		0x00000000,
+		0x00000000,		0x00000000,		0x00000000,
+		0x00000000,		0x00000000,		0x00000000,
+		0x00000000,		0x00000000,		0x00000000 },
+	},
+	{ 0x1000, {
+		0x000fa400,		0x000fa400,		0x09625902,
+		0x000003f8,		0x00000403,		0x3de0d9f0,
+		0x001fffed,		0x00000804,		0x3cc1f9c3,
+		0x001003de,		0x00000c01,		0x3cb34d7f,
+		0x002003d2,		0x00000c00,		0x3d24a92d,
+		0x00200bca,		0x00000bff,		0x3df600d2,
+		0x002013cc,		0x000007ff,		0x3ed70c7e,
+		0x00100fde,		0x00000000,		0x3f87c036 },
+	},
+	{ 0x1200, {
+		0x002ffff1,		0x002ffff1,		0x02a0a9c8,
+		0x002003e7,		0x001ffffa,		0x000185bc,
+		0x002007dc,		0x000003ff,		0x3e52859c,
+		0x00200bd4,		0x00000002,		0x3d53996b,
+		0x00100fd0,		0x00000403,		0x3d04ad2d,
+		0x00000bd5,		0x00000403,		0x3d35ace7,
+		0x3ff003e4,		0x00000801,		0x3dc674a1,
+		0x3fffe800,		0x00000800,		0x3e76f461 },
+	},
+	{ 0x1400, {
+		0x00100be3,		0x00100be3,		0x04d1359a,
+		0x00000fdb,		0x002003ed,		0x0211fd93,
+		0x00000fd6,		0x002003f4,		0x0002d97b,
+		0x000007d6,		0x002ffffb,		0x3e93b956,
+		0x3ff003da,		0x001003ff,		0x3db49926,
+		0x3fffefe9,		0x00100001,		0x3d655cee,
+		0x3fffd400,		0x00000003,		0x3d65f4b6,
+		0x000fb421,		0x00000402,		0x3dc6547e },
+	},
+	{ 0x1600, {
+		0x00000bdd,		0x00000bdd,		0x06519578,
+		0x3ff007da,		0x00000be3,		0x03c24973,
+		0x3ff003d9,		0x00000be9,		0x01b30d5f,
+		0x3ffff7df,		0x001003f1,		0x0003c542,
+		0x000fdfec,		0x001003f7,		0x3ec4711d,
+		0x000fc400,		0x002ffffd,		0x3df504f1,
+		0x001fa81a,		0x002ffc00,		0x3d957cc2,
+		0x002f8c3c,		0x00100000,		0x3db5c891 },
+	},
+	{ 0x1800, {
+		0x3ff003dc,		0x3ff003dc,		0x0791e558,
+		0x000ff7dd,		0x3ff007de,		0x05328554,
+		0x000fe7e3,		0x3ff00be2,		0x03232546,
+		0x000fd7ee,		0x000007e9,		0x0143bd30,
+		0x001fb800,		0x000007ee,		0x00044511,
+		0x002fa015,		0x000007f4,		0x3ef4bcee,
+		0x002f8832,		0x001003f9,		0x3e4514c7,
+		0x001f7853,		0x001003fd,		0x3de54c9f },
+	},
+	{ 0x1a00, {
+		0x000fefe0,		0x000fefe0,		0x08721d3c,
+		0x001fdbe7,		0x000ffbde,		0x0652a139,
+		0x001fcbf0,		0x000003df,		0x0463292e,
+		0x002fb3ff,		0x3ff007e3,		0x0293a91d,
+		0x002f9c12,		0x3ff00be7,		0x01241905,
+		0x001f8c29,		0x000007ed,		0x3fe470eb,
+		0x000f7c46,		0x000007f2,		0x3f04b8ca,
+		0x3fef7865,		0x000007f6,		0x3e74e4a8 },
+	},
+	{ 0x1c00, {
+		0x001fd3e9,		0x001fd3e9,		0x08f23d26,
+		0x002fbff3,		0x001fe3e4,		0x0712ad23,
+		0x002fa800,		0x000ff3e0,		0x05631d1b,
+		0x001f9810,		0x000ffbe1,		0x03b3890d,
+		0x000f8c23,		0x000003e3,		0x0233e8fa,
+		0x3fef843b,		0x000003e7,		0x00f430e4,
+		0x3fbf8456,		0x3ff00bea,		0x00046cc8,
+		0x3f8f8c72,		0x3ff00bef,		0x3f3490ac },
+	},
+	{ 0x1e00, {
+		0x001fbbf4,		0x001fbbf4,		0x09425112,
+		0x001fa800,		0x002fc7ed,		0x0792b110,
+		0x000f980e,		0x001fdbe6,		0x0613110a,
+		0x3fff8c20,		0x001fe7e3,		0x04a368fd,
+		0x3fcf8c33,		0x000ff7e2,		0x0343b8ed,
+		0x3f9f8c4a,		0x000fffe3,		0x0203f8da,
+		0x3f5f9c61,		0x000003e6,		0x00e428c5,
+		0x3f1fb07b,		0x000003eb,		0x3fe440af },
+	},
+	{ 0x2000, {
+		0x000fa400,		0x000fa400,		0x09625902,
+		0x3fff980c,		0x001fb7f5,		0x0812b0ff,
+		0x3fdf901c,		0x001fc7ed,		0x06b2fcfa,
+		0x3faf902d,		0x001fd3e8,		0x055348f1,
+		0x3f7f983f,		0x001fe3e5,		0x04038ce3,
+		0x3f3fa454,		0x001fefe3,		0x02e3c8d1,
+		0x3f0fb86a,		0x001ff7e4,		0x01c3e8c0,
+		0x3ecfd880,		0x000fffe6,		0x00c404ac },
+	},
+	{ 0x2200, {
+		0x3fdf9c0b,		0x3fdf9c0b,		0x09725cf4,
+		0x3fbf9818,		0x3fffa400,		0x0842a8f1,
+		0x3f8f9827,		0x000fb3f7,		0x0702f0ec,
+		0x3f5fa037,		0x000fc3ef,		0x05d330e4,
+		0x3f2fac49,		0x001fcfea,		0x04a364d9,
+		0x3effc05c,		0x001fdbe7,		0x038394ca,
+		0x3ecfdc6f,		0x001fe7e6,		0x0273b0bb,
+		0x3ea00083,		0x001fefe6,		0x0183c0a9 },
+	},
+	{ 0x2400, {
+		0x3f9fa014,		0x3f9fa014,		0x098260e6,
+		0x3f7f9c23,		0x3fcf9c0a,		0x08629ce5,
+		0x3f4fa431,		0x3fefa400,		0x0742d8e1,
+		0x3f1fb440,		0x3fffb3f8,		0x062310d9,
+		0x3eefc850,		0x000fbbf2,		0x050340d0,
+		0x3ecfe062,		0x000fcbec,		0x041364c2,
+		0x3ea00073,		0x001fd3ea,		0x03037cb5,
+		0x3e902086,		0x001fdfe8,		0x022388a5 },
+	},
+	{ 0x2600, {
+		0x3f5fa81e,		0x3f5fa81e,		0x096258da,
+		0x3f3fac2b,		0x3f8fa412,		0x088290d8,
+		0x3f0fbc38,		0x3fafa408,		0x0772c8d5,
+		0x3eefcc47,		0x3fcfa800,		0x0672f4ce,
+		0x3ecfe456,		0x3fefaffa,		0x05531cc6,
+		0x3eb00066,		0x3fffbbf3,		0x047334bb,
+		0x3ea01c77,		0x000fc7ee,		0x039348ae,
+		0x3ea04486,		0x000fd3eb,		0x02b350a1 },
+	},
+	{ 0x2800, {
+		0x3f2fb426,		0x3f2fb426,		0x094250ce,
+		0x3f0fc032,		0x3f4fac1b,		0x086284cd,
+		0x3eefd040,		0x3f7fa811,		0x0782acc9,
+		0x3ecfe84c,		0x3f9fa807,		0x06a2d8c4,
+		0x3eb0005b,		0x3fbfac00,		0x05b2f4bc,
+		0x3eb0186a,		0x3fdfb3fa,		0x04c308b4,
+		0x3eb04077,		0x3fefbbf4,		0x03f31ca8,
+		0x3ec06884,		0x000fbff2,		0x03031c9e },
+	},
+	{ 0x2a00, {
+		0x3f0fc42d,		0x3f0fc42d,		0x090240c4,
+		0x3eefd439,		0x3f2fb822,		0x08526cc2,
+		0x3edfe845,		0x3f4fb018,		0x078294bf,
+		0x3ec00051,		0x3f6fac0f,		0x06b2b4bb,
+		0x3ec0185f,		0x3f8fac07,		0x05e2ccb4,
+		0x3ec0386b,		0x3fafac00,		0x0502e8ac,
+		0x3ed05c77,		0x3fcfb3fb,		0x0432f0a3,
+		0x3ef08482,		0x3fdfbbf6,		0x0372f898 },
+	},
+	{ 0x2c00, {
+		0x3eefdc31,		0x3eefdc31,		0x08e238b8,
+		0x3edfec3d,		0x3f0fc828,		0x082258b9,
+		0x3ed00049,		0x3f1fc01e,		0x077278b6,
+		0x3ed01455,		0x3f3fb815,		0x06c294b2,
+		0x3ed03460,		0x3f5fb40d,		0x0602acac,
+		0x3ef0506c,		0x3f7fb006,		0x0542c0a4,
+		0x3f107476,		0x3f9fb400,		0x0472c89d,
+		0x3f309c80,		0x3fbfb7fc,		0x03b2cc94 },
+	},
+	{ 0x2e00, {
+		0x3eefec37,		0x3eefec37,		0x088220b0,
+		0x3ee00041,		0x3effdc2d,		0x07f244ae,
+		0x3ee0144c,		0x3f0fd023,		0x07625cad,
+		0x3ef02c57,		0x3f1fc81a,		0x06c274a9,
+		0x3f004861,		0x3f3fbc13,		0x060288a6,
+		0x3f20686b,		0x3f5fb80c,		0x05529c9e,
+		0x3f408c74,		0x3f6fb805,		0x04b2ac96,
+		0x3f80ac7e,		0x3f8fb800,		0x0402ac8e },
+	},
+	{ 0x3000, {
+		0x3ef0003a,		0x3ef0003a,		0x084210a6,
+		0x3ef01045,		0x3effec32,		0x07b228a7,
+		0x3f00284e,		0x3f0fdc29,		0x073244a4,
+		0x3f104058,		0x3f0fd420,		0x06a258a2,
+		0x3f305c62,		0x3f2fc818,		0x0612689d,
+		0x3f508069,		0x3f3fc011,		0x05728496,
+		0x3f80a072,		0x3f4fc00a,		0x04d28c90,
+		0x3fc0c07b,		0x3f6fbc04,		0x04429088 },
+	},
+	{ 0x3200, {
+		0x3f00103e,		0x3f00103e,		0x07f1fc9e,
+		0x3f102447,		0x3f000035,		0x0782149d,
+		0x3f203c4f,		0x3f0ff02c,		0x07122c9c,
+		0x3f405458,		0x3f0fe424,		0x06924099,
+		0x3f607061,		0x3f1fd41d,		0x06024c97,
+		0x3f909068,		0x3f2fcc16,		0x05726490,
+		0x3fc0b070,		0x3f3fc80f,		0x04f26c8a,
+		0x0000d077,		0x3f4fc409,		0x04627484 },
+	},
+	{ 0x3400, {
+		0x3f202040,		0x3f202040,		0x07a1e898,
+		0x3f303449,		0x3f100c38,		0x0741fc98,
+		0x3f504c50,		0x3f10002f,		0x06e21495,
+		0x3f706459,		0x3f1ff028,		0x06722492,
+		0x3fa08060,		0x3f1fe421,		0x05f2348f,
+		0x3fd09c67,		0x3f1fdc19,		0x05824c89,
+		0x0000bc6e,		0x3f2fd014,		0x04f25086,
+		0x0040dc74,		0x3f3fcc0d,		0x04825c7f },
+	},
+	{ 0x3600, {
+		0x3f403042,		0x3f403042,		0x0761d890,
+		0x3f504848,		0x3f301c3b,		0x0701f090,
+		0x3f805c50,		0x3f200c33,		0x06a2008f,
+		0x3fa07458,		0x3f10002b,		0x06520c8d,
+		0x3fd0905e,		0x3f1ff424,		0x05e22089,
+		0x0000ac65,		0x3f1fe81d,		0x05823483,
+		0x0030cc6a,		0x3f2fdc18,		0x04f23c81,
+		0x0080e871,		0x3f2fd412,		0x0482407c },
+	},
+	{ 0x3800, {
+		0x3f604043,		0x3f604043,		0x0721c88a,
+		0x3f80544a,		0x3f502c3c,		0x06d1d88a,
+		0x3fb06851,		0x3f301c35,		0x0681e889,
+		0x3fd08456,		0x3f30082f,		0x0611fc88,
+		0x00009c5d,		0x3f200027,		0x05d20884,
+		0x0030b863,		0x3f2ff421,		0x05621880,
+		0x0070d468,		0x3f2fe81b,		0x0502247c,
+		0x00c0ec6f,		0x3f2fe015,		0x04a22877 },
+	},
+	{ 0x3a00, {
+		0x3f904c44,		0x3f904c44,		0x06e1b884,
+		0x3fb0604a,		0x3f70383e,		0x0691c885,
+		0x3fe07451,		0x3f502c36,		0x0661d483,
+		0x00009055,		0x3f401831,		0x0601ec81,
+		0x0030a85b,		0x3f300c2a,		0x05b1f480,
+		0x0070c061,		0x3f300024,		0x0562047a,
+		0x00b0d867,		0x3f3ff41e,		0x05020c77,
+		0x00f0f46b,		0x3f2fec19,		0x04a21474 },
+	},
+	{ 0x3c00, {
+		0x3fb05c43,		0x3fb05c43,		0x06c1b07e,
+		0x3fe06c4b,		0x3f902c3f,		0x0681c081,
+		0x0000844f,		0x3f703838,		0x0631cc7d,
+		0x00309855,		0x3f602433,		0x05d1d47e,
+		0x0060b459,		0x3f50142e,		0x0581e47b,
+		0x00a0c85f,		0x3f400828,		0x0531f078,
+		0x00e0e064,		0x3f300021,		0x0501fc73,
+		0x00b0fc6a,		0x3f3ff41d,		0x04a20873 },
+	},
+	{ 0x3e00, {
+		0x3fe06444,		0x3fe06444,		0x0681a07a,
+		0x00007849,		0x3fc0503f,		0x0641b07a,
+		0x0020904d,		0x3fa0403a,		0x05f1c07a,
+		0x0060a453,		0x3f803034,		0x05c1c878,
+		0x0090b858,		0x3f70202f,		0x0571d477,
+		0x00d0d05d,		0x3f501829,		0x0531e073,
+		0x0110e462,		0x3f500825,		0x04e1e471,
+		0x01510065,		0x3f40001f,		0x04a1f06d },
+	},
+	{ 0x4000, {
+		0x00007044,		0x00007044,		0x06519476,
+		0x00208448,		0x3fe05c3f,		0x0621a476,
+		0x0050984d,		0x3fc04c3a,		0x05e1b075,
+		0x0080ac52,		0x3fa03c35,		0x05a1b875,
+		0x00c0c056,		0x3f803030,		0x0561c473,
+		0x0100d45b,		0x3f70202b,		0x0521d46f,
+		0x0140e860,		0x3f601427,		0x04d1d46e,
+		0x01810064,		0x3f500822,		0x0491dc6b },
+	},
+	{ 0x5000, {
+		0x0110a442,		0x0110a442,		0x0551545e,
+		0x0140b045,		0x00e0983f,		0x0531585f,
+		0x0160c047,		0x00c08c3c,		0x0511645e,
+		0x0190cc4a,		0x00908039,		0x04f1685f,
+		0x01c0dc4c,		0x00707436,		0x04d1705e,
+		0x0200e850,		0x00506833,		0x04b1785b,
+		0x0230f453,		0x00305c30,		0x0491805a,
+		0x02710056,		0x0010542d,		0x04718059 },
+	},
+	{ 0x6000, {
+		0x01c0bc40,		0x01c0bc40,		0x04c13052,
+		0x01e0c841,		0x01a0b43d,		0x04c13851,
+		0x0210cc44,		0x0180a83c,		0x04a13453,
+		0x0230d845,		0x0160a03a,		0x04913c52,
+		0x0260e047,		0x01409838,		0x04714052,
+		0x0280ec49,		0x01208c37,		0x04514c50,
+		0x02b0f44b,		0x01008435,		0x04414c50,
+		0x02d1004c,		0x00e07c33,		0x0431544f },
+	},
+	{ 0x7000, {
+		0x0230c83e,		0x0230c83e,		0x04711c4c,
+		0x0250d03f,		0x0210c43c,		0x0471204b,
+		0x0270d840,		0x0200b83c,		0x0451244b,
+		0x0290dc42,		0x01e0b43a,		0x0441244c,
+		0x02b0e443,		0x01c0b038,		0x0441284b,
+		0x02d0ec44,		0x01b0a438,		0x0421304a,
+		0x02f0f445,		0x0190a036,		0x04213449,
+		0x0310f847,		0x01709c34,		0x04213848 },
+	},
+	{ 0x8000, {
+		0x0280d03d,		0x0280d03d,		0x04310c48,
+		0x02a0d43e,		0x0270c83c,		0x04311047,
+		0x02b0dc3e,		0x0250c83a,		0x04311447,
+		0x02d0e040,		0x0240c03a,		0x04211446,
+		0x02e0e840,		0x0220bc39,		0x04111847,
+		0x0300e842,		0x0210b438,		0x04012445,
+		0x0310f043,		0x0200b037,		0x04012045,
+		0x0330f444,		0x01e0ac36,		0x03f12445 },
+	},
+	{ 0xefff, {
+		0x0340dc3a,		0x0340dc3a,		0x03b0ec40,
+		0x0340e03a,		0x0330e039,		0x03c0f03e,
+		0x0350e03b,		0x0330dc39,		0x03c0ec3e,
+		0x0350e43a,		0x0320dc38,		0x03c0f43e,
+		0x0360e43b,		0x0320d839,		0x03b0f03e,
+		0x0360e83b,		0x0310d838,		0x03c0fc3b,
+		0x0370e83b,		0x0310d439,		0x03a0f83d,
+		0x0370e83c,		0x0300d438,		0x03b0fc3c },
+	}
+};
+
 enum rcar_vin_state {
 	STOPPED = 0,
 	RUNNING,
@@ -161,6 +504,9 @@ struct rcar_vin_cam {
 	/* Client output, as seen by the VIN */
 	unsigned int			width;
 	unsigned int			height;
+	/* User window from S_FMT */
+	unsigned int out_width;
+	unsigned int out_height;
 	/*
 	 * User window from S_CROP / G_CROP, produced by client cropping and
 	 * scaling, VIN scaling and VIN cropping, mapped back onto the client
@@ -332,7 +678,7 @@ static int rcar_vin_setup(struct rcar_vin_priv *priv)
 		vnmc |= VNMC_BPS;
 
 	/* progressive or interlaced mode */
-	interrupts = progressive ? VNIE_FIE | VNIE_EFE : VNIE_EFE;
+	interrupts = progressive ? VNIE_FIE : VNIE_EFE;
 
 	/* ack interrupts */
 	iowrite32(interrupts, priv->base + VNINTS_REG);
@@ -667,6 +1013,60 @@ static void rcar_vin_clock_stop(struct soc_camera_host *ici)
 	/* VIN does not have "mclk" */
 }
 
+static void set_coeff(struct rcar_vin_priv *priv, unsigned short xs)
+{
+	int i;
+	const struct vin_coeff *p_prev_set = NULL;
+	const struct vin_coeff *p_set = NULL;
+
+	/* Look for suitable coefficient values */
+	for (i = 0; i < ARRAY_SIZE(vin_coeff_set); i++) {
+		p_prev_set = p_set;
+		p_set = &vin_coeff_set[i];
+
+		if (xs < p_set->xs_value)
+			break;
+	}
+
+	/* Use previous value if its XS value is closer */
+	if (p_prev_set && p_set &&
+	    xs - p_prev_set->xs_value < p_set->xs_value - xs)
+		p_set = p_prev_set;
+
+	/* Set coefficient registers */
+	iowrite32(p_set->coeff_set[0], priv->base + VNC1A_REG);
+	iowrite32(p_set->coeff_set[1], priv->base + VNC1B_REG);
+	iowrite32(p_set->coeff_set[2], priv->base + VNC1C_REG);
+
+	iowrite32(p_set->coeff_set[3], priv->base + VNC2A_REG);
+	iowrite32(p_set->coeff_set[4], priv->base + VNC2B_REG);
+	iowrite32(p_set->coeff_set[5], priv->base + VNC2C_REG);
+
+	iowrite32(p_set->coeff_set[6], priv->base + VNC3A_REG);
+	iowrite32(p_set->coeff_set[7], priv->base + VNC3B_REG);
+	iowrite32(p_set->coeff_set[8], priv->base + VNC3C_REG);
+
+	iowrite32(p_set->coeff_set[9], priv->base + VNC4A_REG);
+	iowrite32(p_set->coeff_set[10], priv->base + VNC4B_REG);
+	iowrite32(p_set->coeff_set[11], priv->base + VNC4C_REG);
+
+	iowrite32(p_set->coeff_set[12], priv->base + VNC5A_REG);
+	iowrite32(p_set->coeff_set[13], priv->base + VNC5B_REG);
+	iowrite32(p_set->coeff_set[14], priv->base + VNC5C_REG);
+
+	iowrite32(p_set->coeff_set[15], priv->base + VNC6A_REG);
+	iowrite32(p_set->coeff_set[16], priv->base + VNC6B_REG);
+	iowrite32(p_set->coeff_set[17], priv->base + VNC6C_REG);
+
+	iowrite32(p_set->coeff_set[18], priv->base + VNC7A_REG);
+	iowrite32(p_set->coeff_set[19], priv->base + VNC7B_REG);
+	iowrite32(p_set->coeff_set[20], priv->base + VNC7C_REG);
+
+	iowrite32(p_set->coeff_set[21], priv->base + VNC8A_REG);
+	iowrite32(p_set->coeff_set[22], priv->base + VNC8B_REG);
+	iowrite32(p_set->coeff_set[23], priv->base + VNC8C_REG);
+}
+
 /* rect is guaranteed to not exceed the scaled camera rectangle */
 static int rcar_vin_set_rect(struct soc_camera_device *icd)
 {
@@ -676,6 +1076,7 @@ static int rcar_vin_set_rect(struct soc_camera_device *icd)
 	unsigned int left_offset, top_offset;
 	unsigned char dsize = 0;
 	struct v4l2_rect *cam_subrect = &cam->subrect;
+	u32 value;
 
 	dev_dbg(icd->parent, "Crop %ux%u@%u:%u\n",
 		icd->user_width, icd->user_height, cam->vin_left, cam->vin_top);
@@ -695,40 +1096,64 @@ static int rcar_vin_set_rect(struct soc_camera_device *icd)
 
 	/* Set Start/End Pixel/Line Pre-Clip */
 	iowrite32(left_offset << dsize, priv->base + VNSPPRC_REG);
-	iowrite32((left_offset + cam->width - 1) << dsize,
+	iowrite32((left_offset + cam_subrect->width - 1) << dsize,
 		  priv->base + VNEPPRC_REG);
 	switch (priv->field) {
 	case V4L2_FIELD_INTERLACED:
 	case V4L2_FIELD_INTERLACED_TB:
 	case V4L2_FIELD_INTERLACED_BT:
 		iowrite32(top_offset / 2, priv->base + VNSLPRC_REG);
-		iowrite32((top_offset + cam->height) / 2 - 1,
+		iowrite32((top_offset + cam_subrect->height) / 2 - 1,
 			  priv->base + VNELPRC_REG);
 		break;
 	default:
 		iowrite32(top_offset, priv->base + VNSLPRC_REG);
-		iowrite32(top_offset + cam->height - 1,
+		iowrite32(top_offset + cam_subrect->height - 1,
 			  priv->base + VNELPRC_REG);
 		break;
 	}
 
+	/* Set scaling coefficient */
+	value = 0;
+	if (cam_subrect->height != cam->out_height)
+		value = (4096 * cam_subrect->height) / cam->out_height;
+	dev_dbg(icd->parent, "YS Value: %x\n", value);
+	iowrite32(value, priv->base + VNYS_REG);
+
+	value = 0;
+	if (cam_subrect->width != cam->out_width)
+		value = (4096 * cam_subrect->width) / cam->out_width;
+
+	/* Horizontal upscaling is up to double size */
+	if (0 < value && value < 2048)
+		value = 2048;
+
+	dev_dbg(icd->parent, "XS Value: %x\n", value);
+	iowrite32(value, priv->base + VNXS_REG);
+
+	/* Horizontal upscaling is carried out by scaling down from double size */
+	if (value < 4096)
+		value *= 2;
+
+	set_coeff(priv, value);
+
 	/* Set Start/End Pixel/Line Post-Clip */
 	iowrite32(0, priv->base + VNSPPOC_REG);
 	iowrite32(0, priv->base + VNSLPOC_REG);
-	iowrite32((cam_subrect->width - 1) << dsize, priv->base + VNEPPOC_REG);
+	iowrite32((cam->out_width - 1) << dsize, priv->base + VNEPPOC_REG);
 	switch (priv->field) {
 	case V4L2_FIELD_INTERLACED:
 	case V4L2_FIELD_INTERLACED_TB:
 	case V4L2_FIELD_INTERLACED_BT:
-		iowrite32(cam_subrect->height / 2 - 1,
+		iowrite32(cam->out_height / 2 - 1,
 			  priv->base + VNELPOC_REG);
 		break;
 	default:
-		iowrite32(cam_subrect->height - 1, priv->base + VNELPOC_REG);
+		iowrite32(cam->out_height - 1, priv->base + VNELPOC_REG);
 		break;
 	}
 
-	iowrite32(ALIGN(cam->width, 0x10), priv->base + VNIS_REG);
+	iowrite32(ALIGN(cam->out_width, 0x10), priv->base + VNIS_REG);
 
 	return 0;
 }
@@ -819,7 +1244,7 @@ static int rcar_vin_set_bus_param(struct soc_camera_device *icd)
 	if (ret < 0 && ret != -ENOIOCTLCMD)
 		return ret;
 
-	val = priv->field == V4L2_FIELD_NONE ? VNDMR2_FTEV : 0;
+	val = VNDMR2_FTEV | VNDMR2_VLV(1);
 	if (!(common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW))
 		val |= VNDMR2_VPS;
 	if (!(common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW))
@@ -880,6 +1305,14 @@ static const struct soc_mbus_pixelfmt rcar_vin_formats[] = {
 		.layout			= SOC_MBUS_LAYOUT_PLANAR_Y_C,
 	},
 	{
+		.fourcc			= V4L2_PIX_FMT_YUYV,
+		.name			= "YUYV",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+	{
 		.fourcc			= V4L2_PIX_FMT_UYVY,
 		.name			= "UYVY",
 		.bits_per_sample	= 16,
@@ -999,6 +1432,8 @@ static int rcar_vin_get_formats(struct soc_camera_device *icd, unsigned int idx,
 		cam->subrect = rect;
 		cam->width = mf.width;
 		cam->height = mf.height;
+		cam->out_width	= mf.width;
+		cam->out_height	= mf.height;
 
 		icd->host_priv = cam;
 	} else {
@@ -1259,6 +1694,9 @@ static int rcar_vin_set_fmt(struct soc_camera_device *icd,
 	dev_dbg(dev, "W: %u : %u, H: %u : %u\n",
 		vin_sub_width, pix->width, vin_sub_height, pix->height);
 
+	cam->out_width = pix->width;
+	cam->out_height = pix->height;
+
 	icd->current_fmt = xlate;
 
 	priv->field = field;
@@ -1310,8 +1748,12 @@ static int rcar_vin_try_fmt(struct soc_camera_device *icd,
 	if (ret < 0)
 		return ret;
 
-	pix->width = mf.width;
-	pix->height = mf.height;
+	/* Adjust only if VIN cannot scale */
+	if (pix->width > mf.width * 2)
+		pix->width = mf.width * 2;
+	if (pix->height > mf.height * 3)
+		pix->height = mf.height * 3;
+
 	pix->field = mf.field;
 	pix->colorspace = mf.colorspace;
 
@@ -1395,6 +1837,8 @@ static struct soc_camera_host_ops rcar_vin_host_ops = {
 
 #ifdef CONFIG_OF
 static struct of_device_id rcar_vin_of_table[] = {
+	{ .compatible = "renesas,vin-r8a7794", .data = (void *)RCAR_GEN2 },
+	{ .compatible = "renesas,vin-r8a7793", .data = (void *)RCAR_GEN2 },
 	{ .compatible = "renesas,vin-r8a7791", .data = (void *)RCAR_GEN2 },
 	{ .compatible = "renesas,vin-r8a7790", .data = (void *)RCAR_GEN2 },
 	{ .compatible = "renesas,vin-r8a7779", .data = (void *)RCAR_H1 },
@@ -1543,7 +1987,6 @@ static struct platform_driver rcar_vin_driver = {
 	.remove		= rcar_vin_remove,
 	.driver		= {
 		.name		= DRV_NAME,
-		.owner		= THIS_MODULE,
 		.of_match_table	= of_match_ptr(rcar_vin_of_table),
 	},
 	.id_table	= rcar_vin_id_table,
diff --git a/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c b/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
index 5f58ed995320..8b27b3eb2b25 100644
--- a/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
+++ b/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
@@ -2009,7 +2009,6 @@ MODULE_DEVICE_TABLE(of, sh_mobile_ceu_of_match);
 static struct platform_driver sh_mobile_ceu_driver = {
 	.driver		= {
 		.name	= "sh_mobile_ceu",
-		.owner	= THIS_MODULE,
 		.pm	= &sh_mobile_ceu_dev_pm_ops,
 		.of_match_table = sh_mobile_ceu_of_match,
 	},
diff --git a/drivers/media/platform/soc_camera/sh_mobile_csi2.c b/drivers/media/platform/soc_camera/sh_mobile_csi2.c
index c738e27a75d7..c4e7aa0ee7e1 100644
--- a/drivers/media/platform/soc_camera/sh_mobile_csi2.c
+++ b/drivers/media/platform/soc_camera/sh_mobile_csi2.c
@@ -391,7 +391,6 @@ static struct platform_driver __refdata sh_csi2_pdrv = {
 	.probe	= sh_csi2_probe,
 	.driver	= {
 		.name	= "sh-mobile-csi2",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/media/platform/soc_camera/soc_camera.c b/drivers/media/platform/soc_camera/soc_camera.c
index f4be2a1c659a..b3db51c82bde 100644
--- a/drivers/media/platform/soc_camera/soc_camera.c
+++ b/drivers/media/platform/soc_camera/soc_camera.c
@@ -2236,7 +2236,6 @@ static struct platform_driver __refdata soc_camera_pdrv = {
 	.remove  = soc_camera_pdrv_remove,
 	.driver  = {
 		.name	= "soc-camera-pdrv",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/media/platform/soc_camera/soc_camera_platform.c b/drivers/media/platform/soc_camera/soc_camera_platform.c
index f2ce1ab06d53..f535910b4187 100644
--- a/drivers/media/platform/soc_camera/soc_camera_platform.c
+++ b/drivers/media/platform/soc_camera/soc_camera_platform.c
@@ -180,7 +180,6 @@ static int soc_camera_platform_remove(struct platform_device *pdev)
 static struct platform_driver soc_camera_platform_driver = {
 	.driver		= {
 		.name	= "soc_camera_platform",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= soc_camera_platform_probe,
 	.remove		= soc_camera_platform_remove,
diff --git a/drivers/media/platform/ti-vpe/vpe.c b/drivers/media/platform/ti-vpe/vpe.c
index 9a081c291159..d628d1a7cf9e 100644
--- a/drivers/media/platform/ti-vpe/vpe.c
+++ b/drivers/media/platform/ti-vpe/vpe.c
@@ -2376,7 +2376,6 @@ static struct platform_driver vpe_pdrv = {
 	.remove		= vpe_remove,
 	.driver		= {
 		.name	= VPE_MODULE_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = vpe_of_match,
 	},
 };
diff --git a/drivers/media/platform/timblogiw.c b/drivers/media/platform/timblogiw.c
index 3cb2f3564873..5820e45b3a9f 100644
--- a/drivers/media/platform/timblogiw.c
+++ b/drivers/media/platform/timblogiw.c
@@ -857,7 +857,6 @@ static int timblogiw_remove(struct platform_device *pdev)
 static struct platform_driver timblogiw_platform_driver = {
 	.driver = {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= timblogiw_probe,
 	.remove		= timblogiw_remove,
diff --git a/drivers/media/platform/vim2m.c b/drivers/media/platform/vim2m.c
index 1105c111cc10..d9d844aab39b 100644
--- a/drivers/media/platform/vim2m.c
+++ b/drivers/media/platform/vim2m.c
@@ -1060,7 +1060,6 @@ static struct platform_driver vim2m_pdrv = {
 	.remove		= vim2m_remove,
 	.driver		= {
 		.name	= MEM2MEM_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/media/platform/vivid/vivid-vid-out.c b/drivers/media/platform/vivid/vivid-vid-out.c
index ee5c3992b276..39ff79f6aa67 100644
--- a/drivers/media/platform/vivid/vivid-vid-out.c
+++ b/drivers/media/platform/vivid/vivid-vid-out.c
@@ -625,7 +625,7 @@ int vivid_vid_out_g_selection(struct file *file, void *priv,
 		sel->r = dev->fmt_out_rect;
 		break;
 	case V4L2_SEL_TGT_CROP_BOUNDS:
-		if (!dev->has_compose_out)
+		if (!dev->has_crop_out)
 			return -EINVAL;
 		sel->r = vivid_max_rect;
 		break;
diff --git a/drivers/media/platform/vsp1/vsp1_drv.c b/drivers/media/platform/vsp1/vsp1_drv.c
index 3e6601b5b4de..5eb16e87d53f 100644
--- a/drivers/media/platform/vsp1/vsp1_drv.c
+++ b/drivers/media/platform/vsp1/vsp1_drv.c
@@ -562,7 +562,6 @@ static struct platform_driver vsp1_platform_driver = {
 	.probe		= vsp1_probe,
 	.remove		= vsp1_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "vsp1",
 		.pm	= &vsp1_pm_ops,
 		.of_match_table = vsp1_of_match,
diff --git a/drivers/media/radio/radio-si476x.c b/drivers/media/radio/radio-si476x.c
index 633022b45f33..dccf58691650 100644
--- a/drivers/media/radio/radio-si476x.c
+++ b/drivers/media/radio/radio-si476x.c
@@ -1575,7 +1575,6 @@ MODULE_ALIAS("platform:si476x-radio");
 static struct platform_driver si476x_radio_driver = {
 	.driver		= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= si476x_radio_probe,
 	.remove		= si476x_radio_remove,
diff --git a/drivers/media/radio/radio-timb.c b/drivers/media/radio/radio-timb.c
index b9285e6584af..e6b55edc8f8d 100644
--- a/drivers/media/radio/radio-timb.c
+++ b/drivers/media/radio/radio-timb.c
@@ -174,7 +174,6 @@ static int timbradio_remove(struct platform_device *pdev)
 static struct platform_driver timbradio_platform_driver = {
 	.driver = {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= timbradio_probe,
 	.remove		= timbradio_remove,
diff --git a/drivers/media/radio/radio-wl1273.c b/drivers/media/radio/radio-wl1273.c
index 284f789265e0..b8f36445516b 100644
--- a/drivers/media/radio/radio-wl1273.c
+++ b/drivers/media/radio/radio-wl1273.c
@@ -2150,7 +2150,6 @@ static struct platform_driver wl1273_fm_radio_driver = {
 	.remove		= wl1273_fm_radio_remove,
 	.driver		= {
 		.name	= "wl1273_fm_radio",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/media/radio/si4713/radio-platform-si4713.c b/drivers/media/radio/si4713/radio-platform-si4713.c
index 2de5439b9c79..6c7597383ca2 100644
--- a/drivers/media/radio/si4713/radio-platform-si4713.c
+++ b/drivers/media/radio/si4713/radio-platform-si4713.c
@@ -220,7 +220,6 @@ static int radio_si4713_pdriver_remove(struct platform_device *pdev)
 static struct platform_driver radio_si4713_pdriver = {
 	.driver		= {
 		.name	= "radio-si4713",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= radio_si4713_pdriver_probe,
 	.remove         = radio_si4713_pdriver_remove,
diff --git a/drivers/media/rc/gpio-ir-recv.c b/drivers/media/rc/gpio-ir-recv.c
index 59853085bc88..229853d68451 100644
--- a/drivers/media/rc/gpio-ir-recv.c
+++ b/drivers/media/rc/gpio-ir-recv.c
@@ -240,7 +240,6 @@ static struct platform_driver gpio_ir_recv_driver = {
 	.remove = gpio_ir_recv_remove,
 	.driver = {
 		.name   = GPIO_IR_DRIVER_NAME,
-		.owner  = THIS_MODULE,
 		.of_match_table = of_match_ptr(gpio_ir_recv_of_match),
 #ifdef CONFIG_PM
 		.pm	= &gpio_ir_recv_pm_ops,
diff --git a/drivers/media/rc/sunxi-cir.c b/drivers/media/rc/sunxi-cir.c
index bcee8e1a4e9e..340f7f51eed4 100644
--- a/drivers/media/rc/sunxi-cir.c
+++ b/drivers/media/rc/sunxi-cir.c
@@ -306,7 +306,6 @@ static struct platform_driver sunxi_ir_driver = {
 	.remove         = sunxi_ir_remove,
 	.driver = {
 		.name = SUNXI_IR_DEV,
-		.owner = THIS_MODULE,
 		.of_match_table = sunxi_ir_match,
 	},
 };
diff --git a/drivers/media/usb/Kconfig b/drivers/media/usb/Kconfig
index 056181f2f569..7496f332f3f5 100644
--- a/drivers/media/usb/Kconfig
+++ b/drivers/media/usb/Kconfig
@@ -24,7 +24,6 @@ if MEDIA_ANALOG_TV_SUPPORT
 	comment "Analog TV USB devices"
 source "drivers/media/usb/pvrusb2/Kconfig"
 source "drivers/media/usb/hdpvr/Kconfig"
-source "drivers/media/usb/tlg2300/Kconfig"
 source "drivers/media/usb/usbvision/Kconfig"
 source "drivers/media/usb/stk1160/Kconfig"
 source "drivers/media/usb/go7007/Kconfig"
diff --git a/drivers/media/usb/Makefile b/drivers/media/usb/Makefile
index 6f2eb7c8416c..8874ba774a34 100644
--- a/drivers/media/usb/Makefile
+++ b/drivers/media/usb/Makefile
@@ -16,7 +16,6 @@ obj-$(CONFIG_VIDEO_CPIA2) += cpia2/
 obj-$(CONFIG_VIDEO_AU0828) += au0828/
 obj-$(CONFIG_VIDEO_HDPVR)	+= hdpvr/
 obj-$(CONFIG_VIDEO_PVRUSB2) += pvrusb2/
-obj-$(CONFIG_VIDEO_TLG2300) += tlg2300/
 obj-$(CONFIG_VIDEO_USBVISION) += usbvision/
 obj-$(CONFIG_VIDEO_STK1160) += stk1160/
 obj-$(CONFIG_VIDEO_CX231XX) += cx231xx/
diff --git a/drivers/media/v4l2-core/v4l2-ioctl.c b/drivers/media/v4l2-core/v4l2-ioctl.c
index 75658717961f..faac2f4e0f3a 100644
--- a/drivers/media/v4l2-core/v4l2-ioctl.c
+++ b/drivers/media/v4l2-core/v4l2-ioctl.c
@@ -1017,6 +1017,12 @@ static int v4l_querycap(const struct v4l2_ioctl_ops *ops,
 	ret = ops->vidioc_querycap(file, fh, cap);
 
 	cap->capabilities |= V4L2_CAP_EXT_PIX_FORMAT;
+	/*
+	 * Drivers MUST fill in device_caps, so check for this and
+	 * warn if it was forgotten.
+	 */
+	WARN_ON(!(cap->capabilities & V4L2_CAP_DEVICE_CAPS) ||
+		!cap->device_caps);
 	cap->device_caps |= V4L2_CAP_EXT_PIX_FORMAT;
 
 	return ret;
diff --git a/drivers/memory/atmel-sdramc.c b/drivers/memory/atmel-sdramc.c
index fed04e8efe75..a3ebc8a87479 100644
--- a/drivers/memory/atmel-sdramc.c
+++ b/drivers/memory/atmel-sdramc.c
@@ -82,7 +82,6 @@ static struct platform_driver atmel_ramc_driver = {
 	.probe		= atmel_ramc_probe,
 	.driver		= {
 		.name	= "atmel-ramc",
-		.owner	= THIS_MODULE,
 		.of_match_table = atmel_ramc_of_match,
 	},
 };
diff --git a/drivers/memory/fsl-corenet-cf.c b/drivers/memory/fsl-corenet-cf.c
index c9443fc136db..fc7ab5a3561e 100644
--- a/drivers/memory/fsl-corenet-cf.c
+++ b/drivers/memory/fsl-corenet-cf.c
@@ -237,7 +237,6 @@ static int ccf_remove(struct platform_device *pdev)
 static struct platform_driver ccf_driver = {
 	.driver = {
 		.name = KBUILD_MODNAME,
-		.owner = THIS_MODULE,
 		.of_match_table = ccf_matches,
 	},
 	.probe = ccf_probe,
diff --git a/drivers/memory/fsl_ifc.c b/drivers/memory/fsl_ifc.c
index 3d5d792d5cb2..410c39749872 100644
--- a/drivers/memory/fsl_ifc.c
+++ b/drivers/memory/fsl_ifc.c
@@ -61,7 +61,7 @@ int fsl_ifc_find(phys_addr_t addr_base)
 	if (!fsl_ifc_ctrl_dev || !fsl_ifc_ctrl_dev->regs)
 		return -ENODEV;
 
-	for (i = 0; i < ARRAY_SIZE(fsl_ifc_ctrl_dev->regs->cspr_cs); i++) {
+	for (i = 0; i < fsl_ifc_ctrl_dev->banks; i++) {
 		u32 cspr = in_be32(&fsl_ifc_ctrl_dev->regs->cspr_cs[i].cspr);
 		if (cspr & CSPR_V && (cspr & CSPR_BA) ==
 				convert_ifc_address(addr_base))
@@ -213,7 +213,7 @@ static irqreturn_t fsl_ifc_ctrl_irq(int irqno, void *data)
 static int fsl_ifc_ctrl_probe(struct platform_device *dev)
 {
 	int ret = 0;
-
+	int version, banks;
 
 	dev_info(&dev->dev, "Freescale Integrated Flash Controller\n");
 
@@ -231,6 +231,15 @@ static int fsl_ifc_ctrl_probe(struct platform_device *dev)
 		goto err;
 	}
 
+	version = ioread32be(&fsl_ifc_ctrl_dev->regs->ifc_rev) &
+			FSL_IFC_VERSION_MASK;
+	banks = (version == FSL_IFC_VERSION_1_0_0) ? 4 : 8;
+	dev_info(&dev->dev, "IFC version %d.%d, %d banks\n",
+		version >> 24, (version >> 16) & 0xf, banks);
+
+	fsl_ifc_ctrl_dev->version = version;
+	fsl_ifc_ctrl_dev->banks = banks;
+
 	/* get the Controller level irq */
 	fsl_ifc_ctrl_dev->irq = irq_of_parse_and_map(dev->dev.of_node, 0);
 	if (fsl_ifc_ctrl_dev->irq == NO_IRQ) {
diff --git a/drivers/memory/mvebu-devbus.c b/drivers/memory/mvebu-devbus.c
index ff7138fd66d1..24852812fd44 100644
--- a/drivers/memory/mvebu-devbus.c
+++ b/drivers/memory/mvebu-devbus.c
@@ -346,7 +346,6 @@ static struct platform_driver mvebu_devbus_driver = {
 	.probe		= mvebu_devbus_probe,
 	.driver		= {
 		.name	= "mvebu-devbus",
-		.owner	= THIS_MODULE,
 		.of_match_table = mvebu_devbus_of_match,
 	},
 };
diff --git a/drivers/memory/omap-gpmc.c b/drivers/memory/omap-gpmc.c
index ffc5e60c0664..24696f59215b 100644
--- a/drivers/memory/omap-gpmc.c
+++ b/drivers/memory/omap-gpmc.c
@@ -1987,7 +1987,6 @@ static struct platform_driver gpmc_driver = {
 	.remove		= gpmc_remove,
 	.driver		= {
 		.name	= DEVICE_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(gpmc_dt_ids),
 		.pm	= &gpmc_pm_ops,
 	},
diff --git a/drivers/memory/tegra20-mc.c b/drivers/memory/tegra20-mc.c
index 7cd82b874abd..cc309a05289a 100644
--- a/drivers/memory/tegra20-mc.c
+++ b/drivers/memory/tegra20-mc.c
@@ -243,7 +243,6 @@ 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,
 	},
 };
diff --git a/drivers/memory/ti-aemif.c b/drivers/memory/ti-aemif.c
index d3df7602f406..ca7d97a9a9ba 100644
--- a/drivers/memory/ti-aemif.c
+++ b/drivers/memory/ti-aemif.c
@@ -413,7 +413,6 @@ static struct platform_driver aemif_driver = {
 	.remove = aemif_remove,
 	.driver = {
 		.name = KBUILD_MODNAME,
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(aemif_of_match),
 	},
 };
diff --git a/drivers/memstick/host/rtsx_pci_ms.c b/drivers/memstick/host/rtsx_pci_ms.c
index c880ba685754..818fa94354ae 100644
--- a/drivers/memstick/host/rtsx_pci_ms.c
+++ b/drivers/memstick/host/rtsx_pci_ms.c
@@ -645,7 +645,6 @@ static struct platform_driver rtsx_pci_ms_driver = {
 	.suspend	= rtsx_pci_ms_suspend,
 	.resume		= rtsx_pci_ms_resume,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= DRV_NAME_RTSX_PCI_MS,
 	},
 };
diff --git a/drivers/memstick/host/rtsx_usb_ms.c b/drivers/memstick/host/rtsx_usb_ms.c
index a7282b7d4de8..1105db2355d2 100644
--- a/drivers/memstick/host/rtsx_usb_ms.c
+++ b/drivers/memstick/host/rtsx_usb_ms.c
@@ -827,7 +827,6 @@ static struct platform_driver rtsx_usb_ms_driver = {
 	.remove		= rtsx_usb_ms_drv_remove,
 	.id_table       = rtsx_usb_ms_ids,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "rtsx_usb_ms",
 		.pm	= &rtsx_usb_ms_pm_ops,
 	},
diff --git a/drivers/mfd/ab3100-otp.c b/drivers/mfd/ab3100-otp.c
index c9af16cc7310..f391c5fee1b0 100644
--- a/drivers/mfd/ab3100-otp.c
+++ b/drivers/mfd/ab3100-otp.c
@@ -239,7 +239,6 @@ static int __exit ab3100_otp_remove(struct platform_device *pdev)
 static struct platform_driver ab3100_otp_driver = {
 	.driver = {
 		.name = "ab3100-otp",
-		.owner = THIS_MODULE,
 	},
 	.remove	 = __exit_p(ab3100_otp_remove),
 };
diff --git a/drivers/mfd/ab8500-core.c b/drivers/mfd/ab8500-core.c
index bde2fc072410..c80a2925f8e5 100644
--- a/drivers/mfd/ab8500-core.c
+++ b/drivers/mfd/ab8500-core.c
@@ -1848,7 +1848,6 @@ static const struct platform_device_id ab8500_id[] = {
 static struct platform_driver ab8500_core_driver = {
 	.driver = {
 		.name = "ab8500-core",
-		.owner = THIS_MODULE,
 	},
 	.probe	= ab8500_probe,
 	.remove	= ab8500_remove,
diff --git a/drivers/mfd/ab8500-debugfs.c b/drivers/mfd/ab8500-debugfs.c
index b2c7e3b1edfa..9a8e185f11df 100644
--- a/drivers/mfd/ab8500-debugfs.c
+++ b/drivers/mfd/ab8500-debugfs.c
@@ -3263,7 +3263,6 @@ static int ab8500_debug_remove(struct platform_device *plf)
 static struct platform_driver ab8500_debug_driver = {
 	.driver = {
 		.name = "ab8500-debug",
-		.owner = THIS_MODULE,
 	},
 	.probe  = ab8500_debug_probe,
 	.remove = ab8500_debug_remove
diff --git a/drivers/mfd/ab8500-gpadc.c b/drivers/mfd/ab8500-gpadc.c
index 8e3168d160b2..dabbc93abdd7 100644
--- a/drivers/mfd/ab8500-gpadc.c
+++ b/drivers/mfd/ab8500-gpadc.c
@@ -1044,7 +1044,6 @@ static struct platform_driver ab8500_gpadc_driver = {
 	.remove = ab8500_gpadc_remove,
 	.driver = {
 		.name = "ab8500-gpadc",
-		.owner = THIS_MODULE,
 		.pm = &ab8500_gpadc_pm_ops,
 	},
 };
diff --git a/drivers/mfd/ab8500-sysctrl.c b/drivers/mfd/ab8500-sysctrl.c
index 94dbcdd2a1ff..cfff0b643f1b 100644
--- a/drivers/mfd/ab8500-sysctrl.c
+++ b/drivers/mfd/ab8500-sysctrl.c
@@ -180,7 +180,6 @@ static int ab8500_sysctrl_remove(struct platform_device *pdev)
 static struct platform_driver ab8500_sysctrl_driver = {
 	.driver = {
 		.name = "ab8500-sysctrl",
-		.owner = THIS_MODULE,
 	},
 	.probe = ab8500_sysctrl_probe,
 	.remove = ab8500_sysctrl_remove,
diff --git a/drivers/mfd/davinci_voicecodec.c b/drivers/mfd/davinci_voicecodec.c
index 013ba8159dcd..c835e85539b2 100644
--- a/drivers/mfd/davinci_voicecodec.c
+++ b/drivers/mfd/davinci_voicecodec.c
@@ -143,7 +143,6 @@ static int davinci_vc_remove(struct platform_device *pdev)
 static struct platform_driver davinci_vc_driver = {
 	.driver	= {
 		.name = "davinci_voicecodec",
-		.owner = THIS_MODULE,
 	},
 	.remove	= davinci_vc_remove,
 };
diff --git a/drivers/mfd/db8500-prcmu.c b/drivers/mfd/db8500-prcmu.c
index a8204730f01c..16162bf43656 100644
--- a/drivers/mfd/db8500-prcmu.c
+++ b/drivers/mfd/db8500-prcmu.c
@@ -3232,7 +3232,6 @@ static const struct of_device_id db8500_prcmu_match[] = {
 static struct platform_driver db8500_prcmu_driver = {
 	.driver = {
 		.name = "db8500-prcmu",
-		.owner = THIS_MODULE,
 		.of_match_table = db8500_prcmu_match,
 	},
 	.probe = db8500_prcmu_probe,
diff --git a/drivers/mfd/intel_msic.c b/drivers/mfd/intel_msic.c
index 443e7cddff28..25d486c543cb 100644
--- a/drivers/mfd/intel_msic.c
+++ b/drivers/mfd/intel_msic.c
@@ -447,7 +447,6 @@ static struct platform_driver intel_msic_driver = {
 	.remove		= intel_msic_remove,
 	.driver		= {
 		.name	= "intel_msic",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mfd/jz4740-adc.c b/drivers/mfd/jz4740-adc.c
index 7a51c0d0d4f1..b31c54e4ecb2 100644
--- a/drivers/mfd/jz4740-adc.c
+++ b/drivers/mfd/jz4740-adc.c
@@ -324,7 +324,6 @@ static struct platform_driver jz4740_adc_driver = {
 	.remove = jz4740_adc_remove,
 	.driver = {
 		.name = "jz4740-adc",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mfd/kempld-core.c b/drivers/mfd/kempld-core.c
index bd2696136eee..f38ec424872e 100644
--- a/drivers/mfd/kempld-core.c
+++ b/drivers/mfd/kempld-core.c
@@ -494,7 +494,6 @@ static int kempld_remove(struct platform_device *pdev)
 static struct platform_driver kempld_driver = {
 	.driver		= {
 		.name	= "kempld",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= kempld_probe,
 	.remove		= kempld_remove,
diff --git a/drivers/mfd/mcp-sa11x0.c b/drivers/mfd/mcp-sa11x0.c
index 29d76986b40b..1fc458128405 100644
--- a/drivers/mfd/mcp-sa11x0.c
+++ b/drivers/mfd/mcp-sa11x0.c
@@ -299,7 +299,6 @@ static struct platform_driver mcp_sa11x0_driver = {
 	.remove		= mcp_sa11x0_remove,
 	.driver		= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 		.pm	= &mcp_sa11x0_pm_ops,
 	},
 };
diff --git a/drivers/mfd/omap-usb-host.c b/drivers/mfd/omap-usb-host.c
index 83dab2f0a50e..04cd54dd507c 100644
--- a/drivers/mfd/omap-usb-host.c
+++ b/drivers/mfd/omap-usb-host.c
@@ -882,7 +882,6 @@ MODULE_DEVICE_TABLE(of, usbhs_omap_dt_ids);
 static struct platform_driver usbhs_omap_driver = {
 	.driver = {
 		.name		= (char *)usbhs_driver_name,
-		.owner		= THIS_MODULE,
 		.pm		= &usbhsomap_dev_pm_ops,
 		.of_match_table = usbhs_omap_dt_ids,
 	},
diff --git a/drivers/mfd/omap-usb-tll.c b/drivers/mfd/omap-usb-tll.c
index 532eacab6b46..b7b3e8ee64f2 100644
--- a/drivers/mfd/omap-usb-tll.c
+++ b/drivers/mfd/omap-usb-tll.c
@@ -319,7 +319,6 @@ MODULE_DEVICE_TABLE(of, usbtll_omap_dt_ids);
 static struct platform_driver usbtll_omap_driver = {
 	.driver = {
 		.name		= (char *)usbtll_driver_name,
-		.owner		= THIS_MODULE,
 		.of_match_table = usbtll_omap_dt_ids,
 	},
 	.probe		= usbtll_omap_probe,
diff --git a/drivers/mfd/pm8921-core.c b/drivers/mfd/pm8921-core.c
index 39904f77c049..5a92646a2ccb 100644
--- a/drivers/mfd/pm8921-core.c
+++ b/drivers/mfd/pm8921-core.c
@@ -374,7 +374,6 @@ static struct platform_driver pm8921_driver = {
 	.remove		= pm8921_remove,
 	.driver		= {
 		.name	= "pm8921-core",
-		.owner	= THIS_MODULE,
 		.of_match_table = pm8921_id_table,
 	},
 };
diff --git a/drivers/mfd/sm501.c b/drivers/mfd/sm501.c
index 6ce6e6200359..91077efc8050 100644
--- a/drivers/mfd/sm501.c
+++ b/drivers/mfd/sm501.c
@@ -1723,7 +1723,6 @@ static const struct of_device_id of_sm501_match_tbl[] = {
 static struct platform_driver sm501_plat_driver = {
 	.driver		= {
 		.name	= "sm501",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_sm501_match_tbl,
 	},
 	.probe		= sm501_plat_probe,
diff --git a/drivers/mfd/ssbi.c b/drivers/mfd/ssbi.c
index b78942ed4c67..27986f641f7d 100644
--- a/drivers/mfd/ssbi.c
+++ b/drivers/mfd/ssbi.c
@@ -327,7 +327,6 @@ static struct platform_driver ssbi_driver = {
 	.probe		= ssbi_probe,
 	.driver		= {
 		.name	= "ssbi",
-		.owner	= THIS_MODULE,
 		.of_match_table = ssbi_match_table,
 	},
 };
diff --git a/drivers/mfd/sta2x11-mfd.c b/drivers/mfd/sta2x11-mfd.c
index 5b72db07d9de..b3e5c6f45105 100644
--- a/drivers/mfd/sta2x11-mfd.c
+++ b/drivers/mfd/sta2x11-mfd.c
@@ -368,7 +368,6 @@ static int sta2x11_scr_probe(struct platform_device *dev)
 static struct platform_driver sta2x11_sctl_platform_driver = {
 	.driver = {
 		.name	= STA2X11_MFD_SCTL_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= sta2x11_sctl_probe,
 };
@@ -382,7 +381,6 @@ static int __init sta2x11_sctl_init(void)
 static struct platform_driver sta2x11_platform_driver = {
 	.driver = {
 		.name	= STA2X11_MFD_APBREG_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= sta2x11_apbreg_probe,
 };
@@ -396,7 +394,6 @@ static int __init sta2x11_apbreg_init(void)
 static struct platform_driver sta2x11_apb_soc_regs_platform_driver = {
 	.driver = {
 		.name	= STA2X11_MFD_APB_SOC_REGS_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= sta2x11_apb_soc_regs_probe,
 };
@@ -410,7 +407,6 @@ static int __init sta2x11_apb_soc_regs_init(void)
 static struct platform_driver sta2x11_scr_platform_driver = {
 	.driver = {
 		.name = STA2X11_MFD_SCR_NAME,
-		.owner = THIS_MODULE,
 	},
 	.probe = sta2x11_scr_probe,
 };
diff --git a/drivers/mfd/sun6i-prcm.c b/drivers/mfd/sun6i-prcm.c
index 283ab8d197e4..2f2e9f062571 100644
--- a/drivers/mfd/sun6i-prcm.c
+++ b/drivers/mfd/sun6i-prcm.c
@@ -152,7 +152,6 @@ static int sun6i_prcm_probe(struct platform_device *pdev)
 static struct platform_driver sun6i_prcm_driver = {
 	.driver = {
 		.name = "sun6i-prcm",
-		.owner = THIS_MODULE,
 		.of_match_table = sun6i_prcm_dt_ids,
 	},
 	.probe = sun6i_prcm_probe,
diff --git a/drivers/mfd/syscon.c b/drivers/mfd/syscon.c
index 72373b113885..176bf0fa2685 100644
--- a/drivers/mfd/syscon.c
+++ b/drivers/mfd/syscon.c
@@ -219,7 +219,6 @@ static const struct platform_device_id syscon_ids[] = {
 static struct platform_driver syscon_driver = {
 	.driver = {
 		.name = "syscon",
-		.owner = THIS_MODULE,
 	},
 	.probe		= syscon_probe,
 	.id_table	= syscon_ids,
diff --git a/drivers/mfd/t7l66xb.c b/drivers/mfd/t7l66xb.c
index 439d905bb219..c09fb5dccd50 100644
--- a/drivers/mfd/t7l66xb.c
+++ b/drivers/mfd/t7l66xb.c
@@ -431,7 +431,6 @@ static int t7l66xb_remove(struct platform_device *dev)
 static struct platform_driver t7l66xb_platform_driver = {
 	.driver = {
 		.name	= "t7l66xb",
-		.owner	= THIS_MODULE,
 	},
 	.suspend	= t7l66xb_suspend,
 	.resume		= t7l66xb_resume,
diff --git a/drivers/mfd/tc6393xb.c b/drivers/mfd/tc6393xb.c
index d35f11fbeab7..63458b39a97d 100644
--- a/drivers/mfd/tc6393xb.c
+++ b/drivers/mfd/tc6393xb.c
@@ -842,7 +842,6 @@ static struct platform_driver tc6393xb_driver = {
 
 	.driver = {
 		.name = "tc6393xb",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mfd/ti_am335x_tscadc.c b/drivers/mfd/ti_am335x_tscadc.c
index d877e777cce6..467c80e1c4ae 100644
--- a/drivers/mfd/ti_am335x_tscadc.c
+++ b/drivers/mfd/ti_am335x_tscadc.c
@@ -368,7 +368,6 @@ MODULE_DEVICE_TABLE(of, ti_tscadc_dt_ids);
 static struct platform_driver ti_tscadc_driver = {
 	.driver = {
 		.name   = "ti_am3359-tscadc",
-		.owner	= THIS_MODULE,
 		.pm	= TSCADC_PM_OPS,
 		.of_match_table = ti_tscadc_dt_ids,
 	},
diff --git a/drivers/mfd/tps65911-comparator.c b/drivers/mfd/tps65911-comparator.c
index c0816ebd9d7e..c0789f81a1c5 100644
--- a/drivers/mfd/tps65911-comparator.c
+++ b/drivers/mfd/tps65911-comparator.c
@@ -166,7 +166,6 @@ static int tps65911_comparator_remove(struct platform_device *pdev)
 static struct platform_driver tps65911_comparator_driver = {
 	.driver = {
 		.name = "tps65911-comparator",
-		.owner = THIS_MODULE,
 	},
 	.probe = tps65911_comparator_probe,
 	.remove = tps65911_comparator_remove,
diff --git a/drivers/mfd/twl4030-audio.c b/drivers/mfd/twl4030-audio.c
index 07fe542e6fc0..0a1606480023 100644
--- a/drivers/mfd/twl4030-audio.c
+++ b/drivers/mfd/twl4030-audio.c
@@ -283,7 +283,6 @@ MODULE_DEVICE_TABLE(of, twl4030_audio_of_match);
 
 static struct platform_driver twl4030_audio_driver = {
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "twl4030-audio",
 		.of_match_table = twl4030_audio_of_match,
 	},
diff --git a/drivers/mfd/twl4030-power.c b/drivers/mfd/twl4030-power.c
index 7d63e324e6a8..393509246037 100644
--- a/drivers/mfd/twl4030-power.c
+++ b/drivers/mfd/twl4030-power.c
@@ -933,7 +933,6 @@ static int twl4030_power_remove(struct platform_device *pdev)
 static struct platform_driver twl4030_power_driver = {
 	.driver = {
 		.name	= "twl4030_power",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(twl4030_power_of_match),
 	},
 	.probe		= twl4030_power_probe,
diff --git a/drivers/misc/arm-charlcd.c b/drivers/misc/arm-charlcd.c
index c72e96b523ed..c65b5ea5d5ef 100644
--- a/drivers/misc/arm-charlcd.c
+++ b/drivers/misc/arm-charlcd.c
@@ -375,7 +375,6 @@ static const struct of_device_id charlcd_match[] = {
 static struct platform_driver charlcd_driver = {
 	.driver = {
 		.name = DRIVERNAME,
-		.owner = THIS_MODULE,
 		.pm = &charlcd_pm_ops,
 		.of_match_table = of_match_ptr(charlcd_match),
 	},
diff --git a/drivers/misc/atmel-ssc.c b/drivers/misc/atmel-ssc.c
index 60843a275abd..e11a0bd6c66e 100644
--- a/drivers/misc/atmel-ssc.c
+++ b/drivers/misc/atmel-ssc.c
@@ -57,7 +57,7 @@ struct ssc_device *ssc_request(unsigned int ssc_num)
 	ssc->user++;
 	spin_unlock(&user_lock);
 
-	clk_prepare_enable(ssc->clk);
+	clk_prepare(ssc->clk);
 
 	return ssc;
 }
@@ -77,7 +77,7 @@ void ssc_free(struct ssc_device *ssc)
 	spin_unlock(&user_lock);
 
 	if (disable_clk)
-		clk_disable_unprepare(ssc->clk);
+		clk_unprepare(ssc->clk);
 }
 EXPORT_SYMBOL(ssc_free);
 
@@ -220,7 +220,6 @@ static int ssc_remove(struct platform_device *pdev)
 static struct platform_driver ssc_driver = {
 	.driver		= {
 		.name		= "ssc",
-		.owner		= THIS_MODULE,
 		.of_match_table	= of_match_ptr(atmel_ssc_dt_ids),
 	},
 	.id_table	= atmel_ssc_devtypes,
diff --git a/drivers/misc/carma/Kconfig b/drivers/misc/carma/Kconfig
index c90370ed712b..295882bfb14e 100644
--- a/drivers/misc/carma/Kconfig
+++ b/drivers/misc/carma/Kconfig
@@ -1,7 +1,6 @@
 config CARMA_FPGA
 	tristate "CARMA DATA-FPGA Access Driver"
-	depends on FSL_SOC && PPC_83xx && MEDIA_SUPPORT && HAS_DMA && FSL_DMA
-	select VIDEOBUF_DMA_SG
+	depends on FSL_SOC && PPC_83xx && HAS_DMA && FSL_DMA
 	default n
 	help
 	  Say Y here to include support for communicating with the data
@@ -9,8 +8,7 @@ config CARMA_FPGA
 
 config CARMA_FPGA_PROGRAM
 	tristate "CARMA DATA-FPGA Programmer"
-	depends on FSL_SOC && PPC_83xx && MEDIA_SUPPORT && HAS_DMA && FSL_DMA
-	select VIDEOBUF_DMA_SG
+	depends on FSL_SOC && PPC_83xx && HAS_DMA && FSL_DMA
 	default n
 	help
 	  Say Y here to include support for programming the data processing
diff --git a/drivers/misc/carma/carma-fpga-program.c b/drivers/misc/carma/carma-fpga-program.c
index 339b252fcedd..06166ac000e0 100644
--- a/drivers/misc/carma/carma-fpga-program.c
+++ b/drivers/misc/carma/carma-fpga-program.c
@@ -19,6 +19,7 @@
 #include <linux/fsldma.h>
 #include <linux/interrupt.h>
 #include <linux/highmem.h>
+#include <linux/vmalloc.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/mutex.h>
@@ -30,8 +31,6 @@
 #include <linux/fs.h>
 #include <linux/io.h>
 
-#include <media/videobuf-dma-sg.h>
-
 /* MPC8349EMDS specific get_immrbase() */
 #include <sysdev/fsl_soc.h>
 
@@ -67,14 +66,79 @@ struct fpga_dev {
 	/* FPGA Bitfile */
 	struct mutex lock;
 
-	struct videobuf_dmabuf vb;
-	bool vb_allocated;
+	void *vaddr;
+	struct scatterlist *sglist;
+	int sglen;
+	int nr_pages;
+	bool buf_allocated;
 
 	/* max size and written bytes */
 	size_t fw_size;
 	size_t bytes;
 };
 
+static int fpga_dma_init(struct fpga_dev *priv, int nr_pages)
+{
+	struct page *pg;
+	int i;
+
+	priv->vaddr = vmalloc_32(nr_pages << PAGE_SHIFT);
+	if (NULL == priv->vaddr) {
+		pr_debug("vmalloc_32(%d pages) failed\n", nr_pages);
+		return -ENOMEM;
+	}
+
+	pr_debug("vmalloc is at addr 0x%08lx, size=%d\n",
+				(unsigned long)priv->vaddr,
+				nr_pages << PAGE_SHIFT);
+
+	memset(priv->vaddr, 0, nr_pages << PAGE_SHIFT);
+	priv->nr_pages = nr_pages;
+
+	priv->sglist = vzalloc(priv->nr_pages * sizeof(*priv->sglist));
+	if (NULL == priv->sglist)
+		goto vzalloc_err;
+
+	sg_init_table(priv->sglist, priv->nr_pages);
+	for (i = 0; i < priv->nr_pages; i++) {
+		pg = vmalloc_to_page(priv->vaddr + i * PAGE_SIZE);
+		if (NULL == pg)
+			goto vmalloc_to_page_err;
+		sg_set_page(&priv->sglist[i], pg, PAGE_SIZE, 0);
+	}
+	return 0;
+
+vmalloc_to_page_err:
+	vfree(priv->sglist);
+	priv->sglist = NULL;
+vzalloc_err:
+	vfree(priv->vaddr);
+	priv->vaddr = NULL;
+	return -ENOMEM;
+}
+
+static int fpga_dma_map(struct fpga_dev *priv)
+{
+	priv->sglen = dma_map_sg(priv->dev, priv->sglist,
+			priv->nr_pages, DMA_TO_DEVICE);
+
+	if (0 == priv->sglen) {
+		pr_warn("%s: dma_map_sg failed\n", __func__);
+		return -ENOMEM;
+	}
+	return 0;
+}
+
+static int fpga_dma_unmap(struct fpga_dev *priv)
+{
+	if (!priv->sglen)
+		return 0;
+
+	dma_unmap_sg(priv->dev, priv->sglist, priv->sglen, DMA_TO_DEVICE);
+	priv->sglen = 0;
+	return 0;
+}
+
 /*
  * FPGA Bitfile Helpers
  */
@@ -87,8 +151,9 @@ struct fpga_dev {
  */
 static void fpga_drop_firmware_data(struct fpga_dev *priv)
 {
-	videobuf_dma_free(&priv->vb);
-	priv->vb_allocated = false;
+	vfree(priv->sglist);
+	vfree(priv->vaddr);
+	priv->buf_allocated = false;
 	priv->bytes = 0;
 }
 
@@ -427,7 +492,7 @@ static noinline int fpga_program_cpu(struct fpga_dev *priv)
 	dev_dbg(priv->dev, "enabled the controller\n");
 
 	/* Write each chunk of the FPGA bitfile to FPGA programmer */
-	ret = fpga_program_block(priv, priv->vb.vaddr, priv->bytes);
+	ret = fpga_program_block(priv, priv->vaddr, priv->bytes);
 	if (ret)
 		goto out_disable_controller;
 
@@ -463,7 +528,6 @@ out_disable_controller:
  */
 static noinline int fpga_program_dma(struct fpga_dev *priv)
 {
-	struct videobuf_dmabuf *vb = &priv->vb;
 	struct dma_chan *chan = priv->chan;
 	struct dma_async_tx_descriptor *tx;
 	size_t num_pages, len, avail = 0;
@@ -505,7 +569,7 @@ static noinline int fpga_program_dma(struct fpga_dev *priv)
 	}
 
 	/* Map the buffer for DMA */
-	ret = videobuf_dma_map(priv->dev, &priv->vb);
+	ret = fpga_dma_map(priv);
 	if (ret) {
 		dev_err(priv->dev, "Unable to map buffer for DMA\n");
 		goto out_free_table;
@@ -525,7 +589,7 @@ static noinline int fpga_program_dma(struct fpga_dev *priv)
 		goto out_dma_unmap;
 	}
 
-	ret = fsl_dma_external_start(chan, 1)
+	ret = fsl_dma_external_start(chan, 1);
 	if (ret) {
 		dev_err(priv->dev, "DMA external control setup failed\n");
 		goto out_dma_unmap;
@@ -534,7 +598,7 @@ static noinline int fpga_program_dma(struct fpga_dev *priv)
 	/* setup and submit the DMA transaction */
 
 	tx = dmaengine_prep_dma_sg(chan, table.sgl, num_pages,
-			vb->sglist, vb->sglen, 0);
+			priv->sglist, priv->sglen, 0);
 	if (!tx) {
 		dev_err(priv->dev, "Unable to prep DMA transaction\n");
 		ret = -ENOMEM;
@@ -572,7 +636,7 @@ static noinline int fpga_program_dma(struct fpga_dev *priv)
 out_disable_controller:
 	fpga_programmer_disable(priv);
 out_dma_unmap:
-	videobuf_dma_unmap(priv->dev, vb);
+	fpga_dma_unmap(priv);
 out_free_table:
 	sg_free_table(&table);
 out_return:
@@ -702,12 +766,12 @@ static int fpga_open(struct inode *inode, struct file *filp)
 		priv->bytes = 0;
 
 	/* Check if we have already allocated a buffer */
-	if (priv->vb_allocated)
+	if (priv->buf_allocated)
 		return 0;
 
 	/* Allocate a buffer to hold enough data for the bitfile */
 	nr_pages = DIV_ROUND_UP(priv->fw_size, PAGE_SIZE);
-	ret = videobuf_dma_init_kernel(&priv->vb, DMA_TO_DEVICE, nr_pages);
+	ret = fpga_dma_init(priv, nr_pages);
 	if (ret) {
 		dev_err(priv->dev, "unable to allocate data buffer\n");
 		mutex_unlock(&priv->lock);
@@ -715,7 +779,7 @@ static int fpga_open(struct inode *inode, struct file *filp)
 		return ret;
 	}
 
-	priv->vb_allocated = true;
+	priv->buf_allocated = true;
 	return 0;
 }
 
@@ -738,7 +802,7 @@ static ssize_t fpga_write(struct file *filp, const char __user *buf,
 		return -ENOSPC;
 
 	count = min_t(size_t, priv->fw_size - priv->bytes, count);
-	if (copy_from_user(priv->vb.vaddr + priv->bytes, buf, count))
+	if (copy_from_user(priv->vaddr + priv->bytes, buf, count))
 		return -EFAULT;
 
 	priv->bytes += count;
@@ -749,20 +813,19 @@ static ssize_t fpga_read(struct file *filp, char __user *buf, size_t count,
 			 loff_t *f_pos)
 {
 	struct fpga_dev *priv = filp->private_data;
-	return simple_read_from_buffer(buf, count, ppos,
-				       priv->vb.vaddr, priv->bytes);
+	return simple_read_from_buffer(buf, count, f_pos,
+				       priv->vaddr, priv->bytes);
 }
 
 static loff_t fpga_llseek(struct file *filp, loff_t offset, int origin)
 {
 	struct fpga_dev *priv = filp->private_data;
-	loff_t newpos;
 
 	/* only read-only opens are allowed to seek */
 	if ((filp->f_flags & O_ACCMODE) != O_RDONLY)
 		return -EINVAL;
 
-	return fixed_size_llseek(file, offset, origin, priv->fw_size);
+	return fixed_size_llseek(filp, offset, origin, priv->fw_size);
 }
 
 static const struct file_operations fpga_fops = {
@@ -953,7 +1016,6 @@ static int fpga_of_probe(struct platform_device *op)
 	priv->dev = &op->dev;
 	mutex_init(&priv->lock);
 	init_completion(&priv->completion);
-	videobuf_dma_init(&priv->vb);
 
 	dev_set_drvdata(priv->dev, priv);
 	dma_cap_zero(mask);
@@ -1091,7 +1153,6 @@ static struct platform_driver fpga_of_driver = {
 	.driver		= {
 		.name		= drv_name,
 		.of_match_table	= fpga_of_match,
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/misc/carma/carma-fpga.c b/drivers/misc/carma/carma-fpga.c
index 55e913b7eb11..68cdfe151bdb 100644
--- a/drivers/misc/carma/carma-fpga.c
+++ b/drivers/misc/carma/carma-fpga.c
@@ -98,6 +98,7 @@
 #include <linux/seq_file.h>
 #include <linux/highmem.h>
 #include <linux/debugfs.h>
+#include <linux/vmalloc.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/poll.h>
@@ -105,8 +106,6 @@
 #include <linux/kref.h>
 #include <linux/io.h>
 
-#include <media/videobuf-dma-sg.h>
-
 /* system controller registers */
 #define SYS_IRQ_SOURCE_CTL	0x24
 #define SYS_IRQ_OUTPUT_EN	0x28
@@ -142,7 +141,10 @@ struct fpga_info {
 
 struct data_buf {
 	struct list_head entry;
-	struct videobuf_dmabuf vb;
+	void *vaddr;
+	struct scatterlist *sglist;
+	int sglen;
+	int nr_pages;
 	size_t size;
 };
 
@@ -207,6 +209,68 @@ static void fpga_device_release(struct kref *ref)
  * Data Buffer Allocation Helpers
  */
 
+static int carma_dma_init(struct data_buf *buf, int nr_pages)
+{
+	struct page *pg;
+	int i;
+
+	buf->vaddr = vmalloc_32(nr_pages << PAGE_SHIFT);
+	if (NULL == buf->vaddr) {
+		pr_debug("vmalloc_32(%d pages) failed\n", nr_pages);
+		return -ENOMEM;
+	}
+
+	pr_debug("vmalloc is at addr 0x%08lx, size=%d\n",
+				(unsigned long)buf->vaddr,
+				nr_pages << PAGE_SHIFT);
+
+	memset(buf->vaddr, 0, nr_pages << PAGE_SHIFT);
+	buf->nr_pages = nr_pages;
+
+	buf->sglist = vzalloc(buf->nr_pages * sizeof(*buf->sglist));
+	if (NULL == buf->sglist)
+		goto vzalloc_err;
+
+	sg_init_table(buf->sglist, buf->nr_pages);
+	for (i = 0; i < buf->nr_pages; i++) {
+		pg = vmalloc_to_page(buf->vaddr + i * PAGE_SIZE);
+		if (NULL == pg)
+			goto vmalloc_to_page_err;
+		sg_set_page(&buf->sglist[i], pg, PAGE_SIZE, 0);
+	}
+	return 0;
+
+vmalloc_to_page_err:
+	vfree(buf->sglist);
+	buf->sglist = NULL;
+vzalloc_err:
+	vfree(buf->vaddr);
+	buf->vaddr = NULL;
+	return -ENOMEM;
+}
+
+static int carma_dma_map(struct device *dev, struct data_buf *buf)
+{
+	buf->sglen = dma_map_sg(dev, buf->sglist,
+			buf->nr_pages, DMA_FROM_DEVICE);
+
+	if (0 == buf->sglen) {
+		pr_warn("%s: dma_map_sg failed\n", __func__);
+		return -ENOMEM;
+	}
+	return 0;
+}
+
+static int carma_dma_unmap(struct device *dev, struct data_buf *buf)
+{
+	if (!buf->sglen)
+		return 0;
+
+	dma_unmap_sg(dev, buf->sglist, buf->sglen, DMA_FROM_DEVICE);
+	buf->sglen = 0;
+	return 0;
+}
+
 /**
  * data_free_buffer() - free a single data buffer and all allocated memory
  * @buf: the buffer to free
@@ -221,7 +285,8 @@ static void data_free_buffer(struct data_buf *buf)
 		return;
 
 	/* free all memory */
-	videobuf_dma_free(&buf->vb);
+	vfree(buf->sglist);
+	vfree(buf->vaddr);
 	kfree(buf);
 }
 
@@ -230,7 +295,7 @@ static void data_free_buffer(struct data_buf *buf)
  * @bytes: the number of bytes required
  *
  * This allocates all space needed for a data buffer. It must be mapped before
- * use in a DMA transaction using videobuf_dma_map().
+ * use in a DMA transaction using carma_dma_map().
  *
  * Returns NULL on failure
  */
@@ -252,9 +317,8 @@ static struct data_buf *data_alloc_buffer(const size_t bytes)
 	INIT_LIST_HEAD(&buf->entry);
 	buf->size = bytes;
 
-	/* allocate the videobuf */
-	videobuf_dma_init(&buf->vb);
-	ret = videobuf_dma_init_kernel(&buf->vb, DMA_FROM_DEVICE, nr_pages);
+	/* allocate the buffer */
+	ret = carma_dma_init(buf, nr_pages);
 	if (ret)
 		goto out_free_buf;
 
@@ -285,13 +349,13 @@ static void data_free_buffers(struct fpga_device *priv)
 
 	list_for_each_entry_safe(buf, tmp, &priv->free, entry) {
 		list_del_init(&buf->entry);
-		videobuf_dma_unmap(priv->dev, &buf->vb);
+		carma_dma_unmap(priv->dev, buf);
 		data_free_buffer(buf);
 	}
 
 	list_for_each_entry_safe(buf, tmp, &priv->used, entry) {
 		list_del_init(&buf->entry);
-		videobuf_dma_unmap(priv->dev, &buf->vb);
+		carma_dma_unmap(priv->dev, buf);
 		data_free_buffer(buf);
 	}
 
@@ -330,7 +394,7 @@ static int data_alloc_buffers(struct fpga_device *priv)
 			break;
 
 		/* map it for DMA */
-		ret = videobuf_dma_map(priv->dev, &buf->vb);
+		ret = carma_dma_map(priv->dev, buf);
 		if (ret) {
 			data_free_buffer(buf);
 			break;
@@ -634,8 +698,8 @@ static int data_submit_dma(struct fpga_device *priv, struct data_buf *buf)
 	dma_addr_t dst, src;
 	unsigned long dma_flags = 0;
 
-	dst_sg = buf->vb.sglist;
-	dst_nents = buf->vb.sglen;
+	dst_sg = buf->sglist;
+	dst_nents = buf->sglen;
 
 	src_sg = priv->corl_table.sgl;
 	src_nents = priv->corl_nents;
@@ -1134,7 +1198,7 @@ static ssize_t data_read(struct file *filp, char __user *ubuf, size_t count,
 	spin_unlock_irq(&priv->lock);
 
 	/* Buffers are always mapped: unmap it */
-	videobuf_dma_unmap(priv->dev, &dbuf->vb);
+	carma_dma_unmap(priv->dev, dbuf);
 
 	/* save the buffer for later */
 	reader->buf = dbuf;
@@ -1143,7 +1207,7 @@ static ssize_t data_read(struct file *filp, char __user *ubuf, size_t count,
 have_buffer:
 	/* Get the number of bytes available */
 	avail = dbuf->size - reader->buf_start;
-	data = dbuf->vb.vaddr + reader->buf_start;
+	data = dbuf->vaddr + reader->buf_start;
 
 	/* Get the number of bytes we can transfer */
 	count = min(count, avail);
@@ -1171,7 +1235,7 @@ have_buffer:
 	 * If it fails, we pretend that the read never happed and return
 	 * -EFAULT to userspace. The read will be retried.
 	 */
-	ret = videobuf_dma_map(priv->dev, &dbuf->vb);
+	ret = carma_dma_map(priv->dev, dbuf);
 	if (ret) {
 		dev_err(priv->dev, "unable to remap buffer for DMA\n");
 		return -EFAULT;
@@ -1203,7 +1267,7 @@ out_unlock:
 	spin_unlock_irq(&priv->lock);
 
 	if (drop_buffer) {
-		videobuf_dma_unmap(priv->dev, &dbuf->vb);
+		carma_dma_unmap(priv->dev, dbuf);
 		data_free_buffer(dbuf);
 	}
 
@@ -1433,7 +1497,6 @@ static struct platform_driver data_of_driver = {
 	.driver		= {
 		.name		= drv_name,
 		.of_match_table	= data_of_match,
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/misc/cs5535-mfgpt.c b/drivers/misc/cs5535-mfgpt.c
index effd8c6b2b94..347f08f2fd48 100644
--- a/drivers/misc/cs5535-mfgpt.c
+++ b/drivers/misc/cs5535-mfgpt.c
@@ -365,7 +365,6 @@ done:
 static struct platform_driver cs5535_mfgpt_driver = {
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 	},
 	.probe = cs5535_mfgpt_probe,
 };
diff --git a/drivers/misc/cxl/context.c b/drivers/misc/cxl/context.c
index cca472109135..51fd6b524371 100644
--- a/drivers/misc/cxl/context.c
+++ b/drivers/misc/cxl/context.c
@@ -34,7 +34,8 @@ struct cxl_context *cxl_context_alloc(void)
 /*
  * Initialises a CXL context.
  */
-int cxl_context_init(struct cxl_context *ctx, struct cxl_afu *afu, bool master)
+int cxl_context_init(struct cxl_context *ctx, struct cxl_afu *afu, bool master,
+		     struct address_space *mapping)
 {
 	int i;
 
@@ -42,6 +43,8 @@ int cxl_context_init(struct cxl_context *ctx, struct cxl_afu *afu, bool master)
 	ctx->afu = afu;
 	ctx->master = master;
 	ctx->pid = NULL; /* Set in start work ioctl */
+	mutex_init(&ctx->mapping_lock);
+	ctx->mapping = mapping;
 
 	/*
 	 * Allocate the segment table before we put it in the IDR so that we
@@ -82,12 +85,12 @@ int cxl_context_init(struct cxl_context *ctx, struct cxl_afu *afu, bool master)
 	 * Allocating IDR! We better make sure everything's setup that
 	 * dereferences from it.
 	 */
+	mutex_lock(&afu->contexts_lock);
 	idr_preload(GFP_KERNEL);
-	spin_lock(&afu->contexts_lock);
 	i = idr_alloc(&ctx->afu->contexts_idr, ctx, 0,
 		      ctx->afu->num_procs, GFP_NOWAIT);
-	spin_unlock(&afu->contexts_lock);
 	idr_preload_end();
+	mutex_unlock(&afu->contexts_lock);
 	if (i < 0)
 		return i;
 
@@ -147,6 +150,12 @@ static void __detach_context(struct cxl_context *ctx)
 	afu_release_irqs(ctx);
 	flush_work(&ctx->fault_work); /* Only needed for dedicated process */
 	wake_up_all(&ctx->wq);
+
+	/* Release Problem State Area mapping */
+	mutex_lock(&ctx->mapping_lock);
+	if (ctx->mapping)
+		unmap_mapping_range(ctx->mapping, 0, 0, 1);
+	mutex_unlock(&ctx->mapping_lock);
 }
 
 /*
@@ -168,21 +177,22 @@ void cxl_context_detach_all(struct cxl_afu *afu)
 	struct cxl_context *ctx;
 	int tmp;
 
-	rcu_read_lock();
-	idr_for_each_entry(&afu->contexts_idr, ctx, tmp)
+	mutex_lock(&afu->contexts_lock);
+	idr_for_each_entry(&afu->contexts_idr, ctx, tmp) {
 		/*
 		 * Anything done in here needs to be setup before the IDR is
 		 * created and torn down after the IDR removed
 		 */
 		__detach_context(ctx);
-	rcu_read_unlock();
+	}
+	mutex_unlock(&afu->contexts_lock);
 }
 
 void cxl_context_free(struct cxl_context *ctx)
 {
-	spin_lock(&ctx->afu->contexts_lock);
+	mutex_lock(&ctx->afu->contexts_lock);
 	idr_remove(&ctx->afu->contexts_idr, ctx->pe);
-	spin_unlock(&ctx->afu->contexts_lock);
+	mutex_unlock(&ctx->afu->contexts_lock);
 	synchronize_rcu();
 
 	free_page((u64)ctx->sstp);
diff --git a/drivers/misc/cxl/cxl.h b/drivers/misc/cxl/cxl.h
index 3d2b8677ec8a..28078f8894a5 100644
--- a/drivers/misc/cxl/cxl.h
+++ b/drivers/misc/cxl/cxl.h
@@ -336,6 +336,8 @@ struct cxl_sste {
 struct cxl_afu {
 	irq_hw_number_t psl_hwirq;
 	irq_hw_number_t serr_hwirq;
+	char *err_irq_name;
+	char *psl_irq_name;
 	unsigned int serr_virq;
 	void __iomem *p1n_mmio;
 	void __iomem *p2n_mmio;
@@ -349,7 +351,7 @@ struct cxl_afu {
 	struct device *chardev_s, *chardev_m, *chardev_d;
 	struct idr contexts_idr;
 	struct dentry *debugfs;
-	spinlock_t contexts_lock;
+	struct mutex contexts_lock;
 	struct mutex spa_mutex;
 	spinlock_t afu_cntl_lock;
 
@@ -379,6 +381,12 @@ struct cxl_afu {
 	bool enabled;
 };
 
+
+struct cxl_irq_name {
+	struct list_head list;
+	char *name;
+};
+
 /*
  * This is a cxl context.  If the PSL is in dedicated mode, there will be one
  * of these per AFU.  If in AFU directed there can be lots of these.
@@ -390,6 +398,10 @@ struct cxl_context {
 	phys_addr_t psn_phys;
 	u64 psn_size;
 
+	/* Used to unmap any mmaps when force detaching */
+	struct address_space *mapping;
+	struct mutex mapping_lock;
+
 	spinlock_t sste_lock; /* Protects segment table entries */
 	struct cxl_sste *sstp;
 	u64 sstp0, sstp1;
@@ -403,6 +415,7 @@ struct cxl_context {
 
 	unsigned long *irq_bitmap; /* Accessed from IRQ context */
 	struct cxl_irq_ranges irqs;
+	struct list_head irq_names;
 	u64 fault_addr;
 	u64 fault_dsisr;
 	u64 afu_err;
@@ -444,6 +457,7 @@ struct cxl {
 	struct dentry *trace;
 	struct dentry *psl_err_chk;
 	struct dentry *debugfs;
+	char *irq_name;
 	struct bin_attribute cxl_attr;
 	int adapter_num;
 	int user_irqs;
@@ -563,9 +577,6 @@ int _cxl_afu_deactivate_mode(struct cxl_afu *afu, int mode);
 int cxl_afu_deactivate_mode(struct cxl_afu *afu);
 int cxl_afu_select_best_mode(struct cxl_afu *afu);
 
-unsigned int cxl_map_irq(struct cxl *adapter, irq_hw_number_t hwirq,
-		         irq_handler_t handler, void *cookie);
-void cxl_unmap_irq(unsigned int virq, void *cookie);
 int cxl_register_psl_irq(struct cxl_afu *afu);
 void cxl_release_psl_irq(struct cxl_afu *afu);
 int cxl_register_psl_err_irq(struct cxl *adapter);
@@ -592,7 +603,8 @@ int cxl_alloc_sst(struct cxl_context *ctx);
 void init_cxl_native(void);
 
 struct cxl_context *cxl_context_alloc(void);
-int cxl_context_init(struct cxl_context *ctx, struct cxl_afu *afu, bool master);
+int cxl_context_init(struct cxl_context *ctx, struct cxl_afu *afu, bool master,
+		     struct address_space *mapping);
 void cxl_context_free(struct cxl_context *ctx);
 int cxl_context_iomap(struct cxl_context *ctx, struct vm_area_struct *vma);
 
@@ -612,7 +624,7 @@ int cxl_attach_process(struct cxl_context *ctx, bool kernel, u64 wed,
 			    u64 amr);
 int cxl_detach_process(struct cxl_context *ctx);
 
-int cxl_get_irq(struct cxl_context *ctx, struct cxl_irq_info *info);
+int cxl_get_irq(struct cxl_afu *afu, struct cxl_irq_info *info);
 int cxl_ack_irq(struct cxl_context *ctx, u64 tfc, u64 psl_reset_mask);
 
 int cxl_check_error(struct cxl_afu *afu);
diff --git a/drivers/misc/cxl/fault.c b/drivers/misc/cxl/fault.c
index c99e896604ee..f8684bca2d79 100644
--- a/drivers/misc/cxl/fault.c
+++ b/drivers/misc/cxl/fault.c
@@ -133,7 +133,7 @@ static void cxl_handle_page_fault(struct cxl_context *ctx,
 {
 	unsigned flt = 0;
 	int result;
-	unsigned long access, flags;
+	unsigned long access, flags, inv_flags = 0;
 
 	if ((result = copro_handle_mm_fault(mm, dar, dsisr, &flt))) {
 		pr_devel("copro_handle_mm_fault failed: %#x\n", result);
@@ -149,8 +149,12 @@ static void cxl_handle_page_fault(struct cxl_context *ctx,
 		access |= _PAGE_RW;
 	if ((!ctx->kernel) || ~(dar & (1ULL << 63)))
 		access |= _PAGE_USER;
+
+	if (dsisr & DSISR_NOHPTE)
+		inv_flags |= HPTE_NOHPTE_UPDATE;
+
 	local_irq_save(flags);
-	hash_page_mm(mm, dar, access, 0x300);
+	hash_page_mm(mm, dar, access, 0x300, inv_flags);
 	local_irq_restore(flags);
 
 	pr_devel("Page fault successfully handled for pe: %i!\n", ctx->pe);
diff --git a/drivers/misc/cxl/file.c b/drivers/misc/cxl/file.c
index 378b099e7c0b..e9f2f10dbb37 100644
--- a/drivers/misc/cxl/file.c
+++ b/drivers/misc/cxl/file.c
@@ -77,7 +77,7 @@ static int __afu_open(struct inode *inode, struct file *file, bool master)
 		goto err_put_afu;
 	}
 
-	if ((rc = cxl_context_init(ctx, afu, master)))
+	if ((rc = cxl_context_init(ctx, afu, master, inode->i_mapping)))
 		goto err_put_afu;
 
 	pr_devel("afu_open pe: %i\n", ctx->pe);
@@ -113,6 +113,10 @@ static int afu_release(struct inode *inode, struct file *file)
 		 __func__, ctx->pe);
 	cxl_context_detach(ctx);
 
+	mutex_lock(&ctx->mapping_lock);
+	ctx->mapping = NULL;
+	mutex_unlock(&ctx->mapping_lock);
+
 	put_device(&ctx->afu->dev);
 
 	/*
diff --git a/drivers/misc/cxl/irq.c b/drivers/misc/cxl/irq.c
index 336020c8e1af..c294925f73ee 100644
--- a/drivers/misc/cxl/irq.c
+++ b/drivers/misc/cxl/irq.c
@@ -92,20 +92,13 @@ static irqreturn_t schedule_cxl_fault(struct cxl_context *ctx, u64 dsisr, u64 da
 	return IRQ_HANDLED;
 }
 
-static irqreturn_t cxl_irq(int irq, void *data)
+static irqreturn_t cxl_irq(int irq, void *data, struct cxl_irq_info *irq_info)
 {
 	struct cxl_context *ctx = data;
-	struct cxl_irq_info irq_info;
 	u64 dsisr, dar;
-	int result;
-
-	if ((result = cxl_get_irq(ctx, &irq_info))) {
-		WARN(1, "Unable to get CXL IRQ Info: %i\n", result);
-		return IRQ_HANDLED;
-	}
 
-	dsisr = irq_info.dsisr;
-	dar = irq_info.dar;
+	dsisr = irq_info->dsisr;
+	dar = irq_info->dar;
 
 	pr_devel("CXL interrupt %i for afu pe: %i DSISR: %#llx DAR: %#llx\n", irq, ctx->pe, dsisr, dar);
 
@@ -149,9 +142,9 @@ static irqreturn_t cxl_irq(int irq, void *data)
 	if (dsisr & CXL_PSL_DSISR_An_UR)
 		pr_devel("CXL interrupt: AURP PTE not found\n");
 	if (dsisr & CXL_PSL_DSISR_An_PE)
-		return handle_psl_slice_error(ctx, dsisr, irq_info.errstat);
+		return handle_psl_slice_error(ctx, dsisr, irq_info->errstat);
 	if (dsisr & CXL_PSL_DSISR_An_AE) {
-		pr_devel("CXL interrupt: AFU Error %.llx\n", irq_info.afu_err);
+		pr_devel("CXL interrupt: AFU Error %.llx\n", irq_info->afu_err);
 
 		if (ctx->pending_afu_err) {
 			/*
@@ -163,10 +156,10 @@ static irqreturn_t cxl_irq(int irq, void *data)
 			 */
 			dev_err_ratelimited(&ctx->afu->dev, "CXL AFU Error "
 					    "undelivered to pe %i: %.llx\n",
-					    ctx->pe, irq_info.afu_err);
+					    ctx->pe, irq_info->afu_err);
 		} else {
 			spin_lock(&ctx->lock);
-			ctx->afu_err = irq_info.afu_err;
+			ctx->afu_err = irq_info->afu_err;
 			ctx->pending_afu_err = 1;
 			spin_unlock(&ctx->lock);
 
@@ -182,24 +175,43 @@ static irqreturn_t cxl_irq(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
+static irqreturn_t fail_psl_irq(struct cxl_afu *afu, struct cxl_irq_info *irq_info)
+{
+	if (irq_info->dsisr & CXL_PSL_DSISR_TRANS)
+		cxl_p2n_write(afu, CXL_PSL_TFC_An, CXL_PSL_TFC_An_AE);
+	else
+		cxl_p2n_write(afu, CXL_PSL_TFC_An, CXL_PSL_TFC_An_A);
+
+	return IRQ_HANDLED;
+}
+
 static irqreturn_t cxl_irq_multiplexed(int irq, void *data)
 {
 	struct cxl_afu *afu = data;
 	struct cxl_context *ctx;
+	struct cxl_irq_info irq_info;
 	int ph = cxl_p2n_read(afu, CXL_PSL_PEHandle_An) & 0xffff;
 	int ret;
 
+	if ((ret = cxl_get_irq(afu, &irq_info))) {
+		WARN(1, "Unable to get CXL IRQ Info: %i\n", ret);
+		return fail_psl_irq(afu, &irq_info);
+	}
+
 	rcu_read_lock();
 	ctx = idr_find(&afu->contexts_idr, ph);
 	if (ctx) {
-		ret = cxl_irq(irq, ctx);
+		ret = cxl_irq(irq, ctx, &irq_info);
 		rcu_read_unlock();
 		return ret;
 	}
 	rcu_read_unlock();
 
-	WARN(1, "Unable to demultiplex CXL PSL IRQ\n");
-	return IRQ_HANDLED;
+	WARN(1, "Unable to demultiplex CXL PSL IRQ for PE %i DSISR %.16llx DAR"
+		" %.16llx\n(Possible AFU HW issue - was a term/remove acked"
+		" with outstanding transactions?)\n", ph, irq_info.dsisr,
+		irq_info.dar);
+	return fail_psl_irq(afu, &irq_info);
 }
 
 static irqreturn_t cxl_irq_afu(int irq, void *data)
@@ -243,7 +255,7 @@ static irqreturn_t cxl_irq_afu(int irq, void *data)
 }
 
 unsigned int cxl_map_irq(struct cxl *adapter, irq_hw_number_t hwirq,
-			 irq_handler_t handler, void *cookie)
+			 irq_handler_t handler, void *cookie, const char *name)
 {
 	unsigned int virq;
 	int result;
@@ -259,7 +271,7 @@ unsigned int cxl_map_irq(struct cxl *adapter, irq_hw_number_t hwirq,
 
 	pr_devel("hwirq %#lx mapped to virq %u\n", hwirq, virq);
 
-	result = request_irq(virq, handler, 0, "cxl", cookie);
+	result = request_irq(virq, handler, 0, name, cookie);
 	if (result) {
 		dev_warn(&adapter->dev, "cxl_map_irq: request_irq failed: %i\n", result);
 		return 0;
@@ -278,14 +290,15 @@ static int cxl_register_one_irq(struct cxl *adapter,
 				irq_handler_t handler,
 				void *cookie,
 				irq_hw_number_t *dest_hwirq,
-				unsigned int *dest_virq)
+				unsigned int *dest_virq,
+				const char *name)
 {
 	int hwirq, virq;
 
 	if ((hwirq = cxl_alloc_one_irq(adapter)) < 0)
 		return hwirq;
 
-	if (!(virq = cxl_map_irq(adapter, hwirq, handler, cookie)))
+	if (!(virq = cxl_map_irq(adapter, hwirq, handler, cookie, name)))
 		goto err;
 
 	*dest_hwirq = hwirq;
@@ -302,10 +315,19 @@ int cxl_register_psl_err_irq(struct cxl *adapter)
 {
 	int rc;
 
+	adapter->irq_name = kasprintf(GFP_KERNEL, "cxl-%s-err",
+				      dev_name(&adapter->dev));
+	if (!adapter->irq_name)
+		return -ENOMEM;
+
 	if ((rc = cxl_register_one_irq(adapter, cxl_irq_err, adapter,
 				       &adapter->err_hwirq,
-				       &adapter->err_virq)))
+				       &adapter->err_virq,
+				       adapter->irq_name))) {
+		kfree(adapter->irq_name);
+		adapter->irq_name = NULL;
 		return rc;
+	}
 
 	cxl_p1_write(adapter, CXL_PSL_ErrIVTE, adapter->err_hwirq & 0xffff);
 
@@ -317,6 +339,7 @@ void cxl_release_psl_err_irq(struct cxl *adapter)
 	cxl_p1_write(adapter, CXL_PSL_ErrIVTE, 0x0000000000000000);
 	cxl_unmap_irq(adapter->err_virq, adapter);
 	cxl_release_one_irq(adapter, adapter->err_hwirq);
+	kfree(adapter->irq_name);
 }
 
 int cxl_register_serr_irq(struct cxl_afu *afu)
@@ -324,10 +347,18 @@ int cxl_register_serr_irq(struct cxl_afu *afu)
 	u64 serr;
 	int rc;
 
+	afu->err_irq_name = kasprintf(GFP_KERNEL, "cxl-%s-err",
+				      dev_name(&afu->dev));
+	if (!afu->err_irq_name)
+		return -ENOMEM;
+
 	if ((rc = cxl_register_one_irq(afu->adapter, cxl_slice_irq_err, afu,
 				       &afu->serr_hwirq,
-				       &afu->serr_virq)))
+				       &afu->serr_virq, afu->err_irq_name))) {
+		kfree(afu->err_irq_name);
+		afu->err_irq_name = NULL;
 		return rc;
+	}
 
 	serr = cxl_p1n_read(afu, CXL_PSL_SERR_An);
 	serr = (serr & 0x00ffffffffff0000ULL) | (afu->serr_hwirq & 0xffff);
@@ -341,24 +372,50 @@ void cxl_release_serr_irq(struct cxl_afu *afu)
 	cxl_p1n_write(afu, CXL_PSL_SERR_An, 0x0000000000000000);
 	cxl_unmap_irq(afu->serr_virq, afu);
 	cxl_release_one_irq(afu->adapter, afu->serr_hwirq);
+	kfree(afu->err_irq_name);
 }
 
 int cxl_register_psl_irq(struct cxl_afu *afu)
 {
-	return cxl_register_one_irq(afu->adapter, cxl_irq_multiplexed, afu,
-			&afu->psl_hwirq, &afu->psl_virq);
+	int rc;
+
+	afu->psl_irq_name = kasprintf(GFP_KERNEL, "cxl-%s",
+				      dev_name(&afu->dev));
+	if (!afu->psl_irq_name)
+		return -ENOMEM;
+
+	if ((rc = cxl_register_one_irq(afu->adapter, cxl_irq_multiplexed, afu,
+				    &afu->psl_hwirq, &afu->psl_virq,
+				    afu->psl_irq_name))) {
+		kfree(afu->psl_irq_name);
+		afu->psl_irq_name = NULL;
+	}
+	return rc;
 }
 
 void cxl_release_psl_irq(struct cxl_afu *afu)
 {
 	cxl_unmap_irq(afu->psl_virq, afu);
 	cxl_release_one_irq(afu->adapter, afu->psl_hwirq);
+	kfree(afu->psl_irq_name);
+}
+
+void afu_irq_name_free(struct cxl_context *ctx)
+{
+	struct cxl_irq_name *irq_name, *tmp;
+
+	list_for_each_entry_safe(irq_name, tmp, &ctx->irq_names, list) {
+		kfree(irq_name->name);
+		list_del(&irq_name->list);
+		kfree(irq_name);
+	}
 }
 
 int afu_register_irqs(struct cxl_context *ctx, u32 count)
 {
 	irq_hw_number_t hwirq;
-	int rc, r, i;
+	int rc, r, i, j = 1;
+	struct cxl_irq_name *irq_name;
 
 	if ((rc = cxl_alloc_irq_ranges(&ctx->irqs, ctx->afu->adapter, count)))
 		return rc;
@@ -372,15 +429,47 @@ int afu_register_irqs(struct cxl_context *ctx, u32 count)
 				  sizeof(*ctx->irq_bitmap), GFP_KERNEL);
 	if (!ctx->irq_bitmap)
 		return -ENOMEM;
+
+	/*
+	 * Allocate names first.  If any fail, bail out before allocating
+	 * actual hardware IRQs.
+	 */
+	INIT_LIST_HEAD(&ctx->irq_names);
+	for (r = 1; r < CXL_IRQ_RANGES; r++) {
+		for (i = 0; i < ctx->irqs.range[r]; hwirq++, i++) {
+			irq_name = kmalloc(sizeof(struct cxl_irq_name),
+					   GFP_KERNEL);
+			if (!irq_name)
+				goto out;
+			irq_name->name = kasprintf(GFP_KERNEL, "cxl-%s-pe%i-%i",
+						   dev_name(&ctx->afu->dev),
+						   ctx->pe, j);
+			if (!irq_name->name) {
+				kfree(irq_name);
+				goto out;
+			}
+			/* Add to tail so next look get the correct order */
+			list_add_tail(&irq_name->list, &ctx->irq_names);
+			j++;
+		}
+	}
+
+	/* We've allocated all memory now, so let's do the irq allocations */
+	irq_name = list_first_entry(&ctx->irq_names, struct cxl_irq_name, list);
 	for (r = 1; r < CXL_IRQ_RANGES; r++) {
 		hwirq = ctx->irqs.offset[r];
 		for (i = 0; i < ctx->irqs.range[r]; hwirq++, i++) {
 			cxl_map_irq(ctx->afu->adapter, hwirq,
-				     cxl_irq_afu, ctx);
+				    cxl_irq_afu, ctx, irq_name->name);
+			irq_name = list_next_entry(irq_name, list);
 		}
 	}
 
 	return 0;
+
+out:
+	afu_irq_name_free(ctx);
+	return -ENOMEM;
 }
 
 void afu_release_irqs(struct cxl_context *ctx)
@@ -398,5 +487,6 @@ void afu_release_irqs(struct cxl_context *ctx)
 		}
 	}
 
+	afu_irq_name_free(ctx);
 	cxl_release_irq_ranges(&ctx->irqs, ctx->afu->adapter);
 }
diff --git a/drivers/misc/cxl/native.c b/drivers/misc/cxl/native.c
index d47532e8f4f1..f2b37b41a0da 100644
--- a/drivers/misc/cxl/native.c
+++ b/drivers/misc/cxl/native.c
@@ -277,6 +277,7 @@ static int do_process_element_cmd(struct cxl_context *ctx,
 				  u64 cmd, u64 pe_state)
 {
 	u64 state;
+	unsigned long timeout = jiffies + (HZ * CXL_TIMEOUT);
 
 	WARN_ON(!ctx->afu->enabled);
 
@@ -286,6 +287,10 @@ static int do_process_element_cmd(struct cxl_context *ctx,
 	smp_mb();
 	cxl_p1n_write(ctx->afu, CXL_PSL_LLCMD_An, cmd | ctx->pe);
 	while (1) {
+		if (time_after_eq(jiffies, timeout)) {
+			dev_warn(&ctx->afu->dev, "WARNING: Process Element Command timed out!\n");
+			return -EBUSY;
+		}
 		state = be64_to_cpup(ctx->afu->sw_command_status);
 		if (state == ~0ULL) {
 			pr_err("cxl: Error adding process element to AFU\n");
@@ -610,13 +615,6 @@ static inline int detach_process_native_dedicated(struct cxl_context *ctx)
 	return 0;
 }
 
-/*
- * TODO: handle case when this is called inside a rcu_read_lock() which may
- * happen when we unbind the driver (ie. cxl_context_detach_all()) .  Terminate
- * & remove use a mutex lock and schedule which will not good with lock held.
- * May need to write do_process_element_cmd() that handles outstanding page
- * faults synchronously.
- */
 static inline int detach_process_native_afu_directed(struct cxl_context *ctx)
 {
 	if (!ctx->pe_inserted)
@@ -637,18 +635,18 @@ int cxl_detach_process(struct cxl_context *ctx)
 	return detach_process_native_afu_directed(ctx);
 }
 
-int cxl_get_irq(struct cxl_context *ctx, struct cxl_irq_info *info)
+int cxl_get_irq(struct cxl_afu *afu, struct cxl_irq_info *info)
 {
 	u64 pidtid;
 
-	info->dsisr = cxl_p2n_read(ctx->afu, CXL_PSL_DSISR_An);
-	info->dar = cxl_p2n_read(ctx->afu, CXL_PSL_DAR_An);
-	info->dsr = cxl_p2n_read(ctx->afu, CXL_PSL_DSR_An);
-	pidtid = cxl_p2n_read(ctx->afu, CXL_PSL_PID_TID_An);
+	info->dsisr = cxl_p2n_read(afu, CXL_PSL_DSISR_An);
+	info->dar = cxl_p2n_read(afu, CXL_PSL_DAR_An);
+	info->dsr = cxl_p2n_read(afu, CXL_PSL_DSR_An);
+	pidtid = cxl_p2n_read(afu, CXL_PSL_PID_TID_An);
 	info->pid = pidtid >> 32;
 	info->tid = pidtid & 0xffffffff;
-	info->afu_err = cxl_p2n_read(ctx->afu, CXL_AFU_ERR_An);
-	info->errstat = cxl_p2n_read(ctx->afu, CXL_PSL_ErrStat_An);
+	info->afu_err = cxl_p2n_read(afu, CXL_AFU_ERR_An);
+	info->errstat = cxl_p2n_read(afu, CXL_PSL_ErrStat_An);
 
 	return 0;
 }
diff --git a/drivers/misc/cxl/pci.c b/drivers/misc/cxl/pci.c
index 10c98ab7f46e..0f2cc9f8b4db 100644
--- a/drivers/misc/cxl/pci.c
+++ b/drivers/misc/cxl/pci.c
@@ -502,7 +502,7 @@ static struct cxl_afu *cxl_alloc_afu(struct cxl *adapter, int slice)
 	afu->dev.release = cxl_release_afu;
 	afu->slice = slice;
 	idr_init(&afu->contexts_idr);
-	spin_lock_init(&afu->contexts_lock);
+	mutex_init(&afu->contexts_lock);
 	spin_lock_init(&afu->afu_cntl_lock);
 	mutex_init(&afu->spa_mutex);
 
diff --git a/drivers/misc/cxl/sysfs.c b/drivers/misc/cxl/sysfs.c
index ce7ec06d87d1..461bdbd5d483 100644
--- a/drivers/misc/cxl/sysfs.c
+++ b/drivers/misc/cxl/sysfs.c
@@ -121,7 +121,7 @@ static ssize_t reset_store_afu(struct device *device,
 	int rc;
 
 	/* Not safe to reset if it is currently in use */
-	spin_lock(&afu->contexts_lock);
+	mutex_lock(&afu->contexts_lock);
 	if (!idr_is_empty(&afu->contexts_idr)) {
 		rc = -EBUSY;
 		goto err;
@@ -132,7 +132,7 @@ static ssize_t reset_store_afu(struct device *device,
 
 	rc = count;
 err:
-	spin_unlock(&afu->contexts_lock);
+	mutex_unlock(&afu->contexts_lock);
 	return rc;
 }
 
@@ -247,7 +247,7 @@ static ssize_t mode_store(struct device *device, struct device_attribute *attr,
 	int rc = -EBUSY;
 
 	/* can't change this if we have a user */
-	spin_lock(&afu->contexts_lock);
+	mutex_lock(&afu->contexts_lock);
 	if (!idr_is_empty(&afu->contexts_idr))
 		goto err;
 
@@ -271,7 +271,7 @@ static ssize_t mode_store(struct device *device, struct device_attribute *attr,
 	afu->current_mode = 0;
 	afu->num_procs = 0;
 
-	spin_unlock(&afu->contexts_lock);
+	mutex_unlock(&afu->contexts_lock);
 
 	if ((rc = _cxl_afu_deactivate_mode(afu, old_mode)))
 		return rc;
@@ -280,7 +280,7 @@ static ssize_t mode_store(struct device *device, struct device_attribute *attr,
 
 	return count;
 err:
-	spin_unlock(&afu->contexts_lock);
+	mutex_unlock(&afu->contexts_lock);
 	return rc;
 }
 
diff --git a/drivers/misc/eeprom/at24.c b/drivers/misc/eeprom/at24.c
index d87f77f790d6..2d3db81be099 100644
--- a/drivers/misc/eeprom/at24.c
+++ b/drivers/misc/eeprom/at24.c
@@ -56,6 +56,7 @@ struct at24_data {
 	struct at24_platform_data chip;
 	struct memory_accessor macc;
 	int use_smbus;
+	int use_smbus_write;
 
 	/*
 	 * Lock protects against activities from other Linux tasks,
@@ -324,7 +325,7 @@ static ssize_t at24_eeprom_write(struct at24_data *at24, const char *buf,
 {
 	struct i2c_client *client;
 	struct i2c_msg msg;
-	ssize_t status;
+	ssize_t status = 0;
 	unsigned long timeout, write_time;
 	unsigned next_page;
 
@@ -365,9 +366,18 @@ static ssize_t at24_eeprom_write(struct at24_data *at24, const char *buf,
 	timeout = jiffies + msecs_to_jiffies(write_timeout);
 	do {
 		write_time = jiffies;
-		if (at24->use_smbus) {
-			status = i2c_smbus_write_i2c_block_data(client,
-					offset, count, buf);
+		if (at24->use_smbus_write) {
+			switch (at24->use_smbus_write) {
+			case I2C_SMBUS_I2C_BLOCK_DATA:
+				status = i2c_smbus_write_i2c_block_data(client,
+						offset, count, buf);
+				break;
+			case I2C_SMBUS_BYTE_DATA:
+				status = i2c_smbus_write_byte_data(client,
+						offset, buf[0]);
+				break;
+			}
+
 			if (status == 0)
 				status = count;
 		} else {
@@ -487,6 +497,7 @@ static int at24_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	struct at24_platform_data chip;
 	bool writable;
 	int use_smbus = 0;
+	int use_smbus_write = 0;
 	struct at24_data *at24;
 	int err;
 	unsigned i, num_addresses;
@@ -546,6 +557,18 @@ static int at24_probe(struct i2c_client *client, const struct i2c_device_id *id)
 		}
 	}
 
+	/* Use I2C operations unless we're stuck with SMBus extensions. */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		if (i2c_check_functionality(client->adapter,
+				I2C_FUNC_SMBUS_WRITE_I2C_BLOCK)) {
+			use_smbus_write = I2C_SMBUS_I2C_BLOCK_DATA;
+		} else if (i2c_check_functionality(client->adapter,
+				I2C_FUNC_SMBUS_WRITE_BYTE_DATA)) {
+			use_smbus_write = I2C_SMBUS_BYTE_DATA;
+			chip.page_size = 1;
+		}
+	}
+
 	if (chip.flags & AT24_FLAG_TAKE8ADDR)
 		num_addresses = 8;
 	else
@@ -559,6 +582,7 @@ static int at24_probe(struct i2c_client *client, const struct i2c_device_id *id)
 
 	mutex_init(&at24->lock);
 	at24->use_smbus = use_smbus;
+	at24->use_smbus_write = use_smbus_write;
 	at24->chip = chip;
 	at24->num_addresses = num_addresses;
 
@@ -576,8 +600,7 @@ static int at24_probe(struct i2c_client *client, const struct i2c_device_id *id)
 
 	writable = !(chip.flags & AT24_FLAG_READONLY);
 	if (writable) {
-		if (!use_smbus || i2c_check_functionality(client->adapter,
-				I2C_FUNC_SMBUS_WRITE_I2C_BLOCK)) {
+		if (!use_smbus || use_smbus_write) {
 
 			unsigned write_max = chip.page_size;
 
diff --git a/drivers/misc/eeprom/sunxi_sid.c b/drivers/misc/eeprom/sunxi_sid.c
index 3f2b625b2032..8385177ff32b 100644
--- a/drivers/misc/eeprom/sunxi_sid.c
+++ b/drivers/misc/eeprom/sunxi_sid.c
@@ -146,7 +146,6 @@ static struct platform_driver sunxi_sid_driver = {
 	.remove = sunxi_sid_remove,
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = sunxi_sid_of_match,
 	},
 };
diff --git a/drivers/misc/fuse/Makefile b/drivers/misc/fuse/Makefile
deleted file mode 100644
index 0679c4febc89..000000000000
--- a/drivers/misc/fuse/Makefile
+++ /dev/null
@@ -1 +0,0 @@
-obj-$(CONFIG_ARCH_TEGRA)	+= tegra/
diff --git a/drivers/misc/genwqe/card_utils.c b/drivers/misc/genwqe/card_utils.c
index 7cb3b7e41739..1ca94e6fa8fb 100644
--- a/drivers/misc/genwqe/card_utils.c
+++ b/drivers/misc/genwqe/card_utils.c
@@ -590,6 +590,8 @@ int genwqe_user_vmap(struct genwqe_dev *cd, struct dma_mapping *m, void *uaddr,
 				 m->nr_pages,
 				 1,		/* write by caller */
 				 m->page_list);	/* ptrs to pages */
+	if (rc < 0)
+		goto fail_get_user_pages;
 
 	/* assumption: get_user_pages can be killed by signals. */
 	if (rc < m->nr_pages) {
diff --git a/drivers/misc/mei/amthif.c b/drivers/misc/mei/amthif.c
index 6cdce8477f57..79f53941779d 100644
--- a/drivers/misc/mei/amthif.c
+++ b/drivers/misc/mei/amthif.c
@@ -262,6 +262,7 @@ out:
 static int mei_amthif_send_cmd(struct mei_device *dev, struct mei_cl_cb *cb)
 {
 	struct mei_msg_hdr mei_hdr;
+	struct mei_cl *cl;
 	int ret;
 
 	if (!dev || !cb)
@@ -277,8 +278,9 @@ static int mei_amthif_send_cmd(struct mei_device *dev, struct mei_cl_cb *cb)
 	dev->iamthif_msg_buf_size = cb->request_buffer.size;
 	memcpy(dev->iamthif_msg_buf, cb->request_buffer.data,
 	       cb->request_buffer.size);
+	cl = &dev->iamthif_cl;
 
-	ret = mei_cl_flow_ctrl_creds(&dev->iamthif_cl);
+	ret = mei_cl_flow_ctrl_creds(cl);
 	if (ret < 0)
 		return ret;
 
@@ -292,8 +294,8 @@ static int mei_amthif_send_cmd(struct mei_device *dev, struct mei_cl_cb *cb)
 			mei_hdr.msg_complete = 1;
 		}
 
-		mei_hdr.host_addr = dev->iamthif_cl.host_client_id;
-		mei_hdr.me_addr = dev->iamthif_cl.me_client_id;
+		mei_hdr.host_addr = cl->host_client_id;
+		mei_hdr.me_addr = cl->me_client_id;
 		mei_hdr.reserved = 0;
 		mei_hdr.internal = 0;
 		dev->iamthif_msg_buf_index += mei_hdr.length;
@@ -302,7 +304,7 @@ static int mei_amthif_send_cmd(struct mei_device *dev, struct mei_cl_cb *cb)
 			return ret;
 
 		if (mei_hdr.msg_complete) {
-			if (mei_cl_flow_ctrl_reduce(&dev->iamthif_cl))
+			if (mei_cl_flow_ctrl_reduce(cl))
 				return -EIO;
 			dev->iamthif_flow_control_pending = true;
 			dev->iamthif_state = MEI_IAMTHIF_FLOW_CONTROL;
@@ -360,8 +362,7 @@ int mei_amthif_write(struct mei_device *dev, struct mei_cl_cb *cb)
 void mei_amthif_run_next_cmd(struct mei_device *dev)
 {
 	struct mei_cl_cb *cb;
-	struct mei_cl_cb *next;
-	int status;
+	int ret;
 
 	if (!dev)
 		return;
@@ -376,16 +377,14 @@ void mei_amthif_run_next_cmd(struct mei_device *dev)
 
 	dev_dbg(dev->dev, "complete amthif cmd_list cb.\n");
 
-	list_for_each_entry_safe(cb, next, &dev->amthif_cmd_list.list, list) {
-		list_del(&cb->list);
-		if (!cb->cl)
-			continue;
-		status = mei_amthif_send_cmd(dev, cb);
-		if (status)
-			dev_warn(dev->dev, "amthif write failed status = %d\n",
-						status);
-		break;
-	}
+	cb = list_first_entry_or_null(&dev->amthif_cmd_list.list,
+					typeof(*cb), list);
+	if (!cb)
+		return;
+	list_del(&cb->list);
+	ret =  mei_amthif_send_cmd(dev, cb);
+	if (ret)
+		dev_warn(dev->dev, "amthif write failed status = %d\n", ret);
 }
 
 
@@ -536,9 +535,6 @@ int mei_amthif_irq_read_msg(struct mei_device *dev,
 	cb = dev->iamthif_current_cb;
 	dev->iamthif_current_cb = NULL;
 
-	if (!cb->cl)
-		return -ENODEV;
-
 	dev->iamthif_stall_timer = 0;
 	cb->buf_idx = dev->iamthif_msg_buf_index;
 	cb->read_time = jiffies;
diff --git a/drivers/misc/mei/bus.c b/drivers/misc/mei/bus.c
index 4d20d60ca38d..b3a72bca5242 100644
--- a/drivers/misc/mei/bus.c
+++ b/drivers/misc/mei/bus.c
@@ -140,7 +140,7 @@ static struct device_type mei_cl_device_type = {
 	.release	= mei_cl_dev_release,
 };
 
-static struct mei_cl *mei_bus_find_mei_cl_by_uuid(struct mei_device *dev,
+struct mei_cl *mei_cl_bus_find_cl_by_uuid(struct mei_device *dev,
 						uuid_le uuid)
 {
 	struct mei_cl *cl;
@@ -160,7 +160,7 @@ struct mei_cl_device *mei_cl_add_device(struct mei_device *dev,
 	struct mei_cl *cl;
 	int status;
 
-	cl = mei_bus_find_mei_cl_by_uuid(dev, uuid);
+	cl = mei_cl_bus_find_cl_by_uuid(dev, uuid);
 	if (cl == NULL)
 		return NULL;
 
diff --git a/drivers/misc/mei/client.c b/drivers/misc/mei/client.c
index bc9ba5359bc6..1382d551d7ed 100644
--- a/drivers/misc/mei/client.c
+++ b/drivers/misc/mei/client.c
@@ -146,7 +146,7 @@ static void __mei_io_list_flush(struct mei_cl_cb *list,
 
 	/* enable removing everything if no cl is specified */
 	list_for_each_entry_safe(cb, next, &list->list, list) {
-		if (!cl || (cb->cl && mei_cl_cmp_id(cl, cb->cl))) {
+		if (!cl || mei_cl_cmp_id(cl, cb->cl)) {
 			list_del(&cb->list);
 			if (free)
 				mei_io_cb_free(cb);
diff --git a/drivers/misc/mei/debugfs.c b/drivers/misc/mei/debugfs.c
index ce1566715f80..b60b4263cf0f 100644
--- a/drivers/misc/mei/debugfs.c
+++ b/drivers/misc/mei/debugfs.c
@@ -34,7 +34,7 @@ static ssize_t mei_dbgfs_read_meclients(struct file *fp, char __user *ubuf,
 	int pos = 0;
 	int ret;
 
-#define HDR "  |id|addr|         UUID                       |con|msg len|sb|\n"
+#define HDR "  |id|fix|         UUID                       |con|msg len|sb|\n"
 
 	mutex_lock(&dev->device_lock);
 
@@ -56,12 +56,8 @@ static ssize_t mei_dbgfs_read_meclients(struct file *fp, char __user *ubuf,
 
 	list_for_each_entry(me_cl, &dev->me_clients, list) {
 
-		/* skip me clients that cannot be connected */
-		if (me_cl->props.max_number_of_connections == 0)
-			continue;
-
 		pos += scnprintf(buf + pos, bufsz - pos,
-			"%2d|%2d|%4d|%pUl|%3d|%7d|%2d|\n",
+			"%2d|%2d|%3d|%pUl|%3d|%7d|%2d|\n",
 			i++, me_cl->client_id,
 			me_cl->props.fixed_address,
 			&me_cl->props.protocol_name,
diff --git a/drivers/misc/mei/hbm.c b/drivers/misc/mei/hbm.c
index 49a2653d91a5..239d7f5d6a92 100644
--- a/drivers/misc/mei/hbm.c
+++ b/drivers/misc/mei/hbm.c
@@ -562,17 +562,17 @@ int mei_hbm_cl_disconnect_rsp(struct mei_device *dev, struct mei_cl *cl)
  * mei_hbm_cl_disconnect_res - update the client state according
  *       disconnect response
  *
+ * @dev: the device structure
  * @cl: mei host client
  * @cmd: disconnect client response host bus message
  */
-static void mei_hbm_cl_disconnect_res(struct mei_cl *cl,
+static void mei_hbm_cl_disconnect_res(struct mei_device *dev, struct mei_cl *cl,
 				      struct mei_hbm_cl_cmd *cmd)
 {
 	struct hbm_client_connect_response *rs =
 		(struct hbm_client_connect_response *)cmd;
 
-	dev_dbg(cl->dev->dev, "hbm: disconnect response cl:host=%02d me=%02d status=%d\n",
-			rs->me_addr, rs->host_addr, rs->status);
+	cl_dbg(dev, cl, "hbm: disconnect response status=%d\n", rs->status);
 
 	if (rs->status == MEI_CL_DISCONN_SUCCESS)
 		cl->state = MEI_FILE_DISCONNECTED;
@@ -598,17 +598,17 @@ int mei_hbm_cl_connect_req(struct mei_device *dev, struct mei_cl *cl)
  * mei_hbm_cl_connect_res - update the client state according
  *        connection response
  *
+ * @dev: the device structure
  * @cl: mei host client
  * @cmd: connect client response host bus message
  */
-static void mei_hbm_cl_connect_res(struct mei_cl *cl,
+static void mei_hbm_cl_connect_res(struct mei_device *dev, struct mei_cl *cl,
 				   struct mei_hbm_cl_cmd *cmd)
 {
 	struct hbm_client_connect_response *rs =
 		(struct hbm_client_connect_response *)cmd;
 
-	dev_dbg(cl->dev->dev, "hbm: connect response cl:host=%02d me=%02d status=%s\n",
-			rs->me_addr, rs->host_addr,
+	cl_dbg(dev, cl, "hbm: connect response status=%s\n",
 			mei_cl_conn_status_str(rs->status));
 
 	if (rs->status == MEI_CL_CONN_SUCCESS)
@@ -637,11 +637,6 @@ static void mei_hbm_cl_res(struct mei_device *dev,
 	list_for_each_entry_safe(cb, next, &dev->ctrl_rd_list.list, list) {
 
 		cl = cb->cl;
-		/* this should not happen */
-		if (WARN_ON(!cl)) {
-			list_del_init(&cb->list);
-			continue;
-		}
 
 		if (cb->fop_type != fop_type)
 			continue;
@@ -657,10 +652,10 @@ static void mei_hbm_cl_res(struct mei_device *dev,
 
 	switch (fop_type) {
 	case MEI_FOP_CONNECT:
-		mei_hbm_cl_connect_res(cl, rs);
+		mei_hbm_cl_connect_res(dev, cl, rs);
 		break;
 	case MEI_FOP_DISCONNECT:
-		mei_hbm_cl_disconnect_res(cl, rs);
+		mei_hbm_cl_disconnect_res(dev, cl, rs);
 		break;
 	default:
 		return;
@@ -811,8 +806,6 @@ int mei_hbm_dispatch(struct mei_device *dev, struct mei_msg_hdr *hdr)
 			return -EPROTO;
 		}
 
-		dev->hbm_state = MEI_HBM_STARTED;
-
 		if (mei_hbm_enum_clients_req(dev)) {
 			dev_err(dev->dev, "hbm: start: failed to send enumeration request\n");
 			return -EIO;
diff --git a/drivers/misc/mei/hbm.h b/drivers/misc/mei/hbm.h
index b7cd3d857fd5..2544db7d1649 100644
--- a/drivers/misc/mei/hbm.h
+++ b/drivers/misc/mei/hbm.h
@@ -26,17 +26,17 @@ struct mei_cl;
  *
  * @MEI_HBM_IDLE : protocol not started
  * @MEI_HBM_STARTING : start request message was sent
- * @MEI_HBM_STARTED : start reply message was received
  * @MEI_HBM_ENUM_CLIENTS : enumeration request was sent
  * @MEI_HBM_CLIENT_PROPERTIES : acquiring clients properties
+ * @MEI_HBM_STARTED : enumeration was completed
  * @MEI_HBM_STOPPED : stopping exchange
  */
 enum mei_hbm_state {
 	MEI_HBM_IDLE = 0,
 	MEI_HBM_STARTING,
-	MEI_HBM_STARTED,
 	MEI_HBM_ENUM_CLIENTS,
 	MEI_HBM_CLIENT_PROPERTIES,
+	MEI_HBM_STARTED,
 	MEI_HBM_STOPPED,
 };
 
diff --git a/drivers/misc/mei/hw-me-regs.h b/drivers/misc/mei/hw-me-regs.h
index c5feafdd58a8..9eb7ed70ace2 100644
--- a/drivers/misc/mei/hw-me-regs.h
+++ b/drivers/misc/mei/hw-me-regs.h
@@ -117,14 +117,18 @@
 #define MEI_DEV_ID_WPT_LP     0x9CBA  /* Wildcat Point LP */
 #define MEI_DEV_ID_WPT_LP_2   0x9CBB  /* Wildcat Point LP 2 */
 
-/* Host Firmware Status Registers in PCI Config Space */
-#define PCI_CFG_HFS_1         0x40
-#define PCI_CFG_HFS_2         0x48
-
 /*
  * MEI HW Section
  */
 
+/* Host Firmware Status Registers in PCI Config Space */
+#define PCI_CFG_HFS_1         0x40
+#define PCI_CFG_HFS_2         0x48
+#define PCI_CFG_HFS_3         0x60
+#define PCI_CFG_HFS_4         0x64
+#define PCI_CFG_HFS_5         0x68
+#define PCI_CFG_HFS_6         0x6C
+
 /* MEI registers */
 /* H_CB_WW - Host Circular Buffer (CB) Write Window register */
 #define H_CB_WW    0
diff --git a/drivers/misc/mei/hw-me.c b/drivers/misc/mei/hw-me.c
index 4f2fd6fc1e23..ff2755062b44 100644
--- a/drivers/misc/mei/hw-me.c
+++ b/drivers/misc/mei/hw-me.c
@@ -270,10 +270,10 @@ static int mei_me_hw_reset(struct mei_device *dev, bool intr_enable)
 static void mei_me_host_set_ready(struct mei_device *dev)
 {
 	struct mei_me_hw *hw = to_me_hw(dev);
+	u32 hcsr = mei_hcsr_read(hw);
 
-	hw->host_hw_state = mei_hcsr_read(hw);
-	hw->host_hw_state |= H_IE | H_IG | H_RDY;
-	mei_hcsr_set(hw, hw->host_hw_state);
+	hcsr |= H_IE | H_IG | H_RDY;
+	mei_hcsr_set(hw, hcsr);
 }
 
 /**
@@ -285,9 +285,9 @@ static void mei_me_host_set_ready(struct mei_device *dev)
 static bool mei_me_host_is_ready(struct mei_device *dev)
 {
 	struct mei_me_hw *hw = to_me_hw(dev);
+	u32 hcsr = mei_hcsr_read(hw);
 
-	hw->host_hw_state = mei_hcsr_read(hw);
-	return (hw->host_hw_state & H_RDY) == H_RDY;
+	return (hcsr & H_RDY) == H_RDY;
 }
 
 /**
@@ -299,9 +299,9 @@ static bool mei_me_host_is_ready(struct mei_device *dev)
 static bool mei_me_hw_is_ready(struct mei_device *dev)
 {
 	struct mei_me_hw *hw = to_me_hw(dev);
+	u32 mecsr = mei_me_mecsr_read(hw);
 
-	hw->me_hw_state = mei_me_mecsr_read(hw);
-	return (hw->me_hw_state & ME_RDY_HRA) == ME_RDY_HRA;
+	return (mecsr & ME_RDY_HRA) == ME_RDY_HRA;
 }
 
 /**
@@ -356,12 +356,13 @@ static int mei_me_hw_start(struct mei_device *dev)
 static unsigned char mei_hbuf_filled_slots(struct mei_device *dev)
 {
 	struct mei_me_hw *hw = to_me_hw(dev);
+	u32 hcsr;
 	char read_ptr, write_ptr;
 
-	hw->host_hw_state = mei_hcsr_read(hw);
+	hcsr = mei_hcsr_read(hw);
 
-	read_ptr = (char) ((hw->host_hw_state & H_CBRP) >> 8);
-	write_ptr = (char) ((hw->host_hw_state & H_CBWP) >> 16);
+	read_ptr = (char) ((hcsr & H_CBRP) >> 8);
+	write_ptr = (char) ((hcsr & H_CBWP) >> 16);
 
 	return (unsigned char) (write_ptr - read_ptr);
 }
@@ -474,13 +475,14 @@ static int mei_me_write_message(struct mei_device *dev,
 static int mei_me_count_full_read_slots(struct mei_device *dev)
 {
 	struct mei_me_hw *hw = to_me_hw(dev);
+	u32 me_csr;
 	char read_ptr, write_ptr;
 	unsigned char buffer_depth, filled_slots;
 
-	hw->me_hw_state = mei_me_mecsr_read(hw);
-	buffer_depth = (unsigned char)((hw->me_hw_state & ME_CBD_HRA) >> 24);
-	read_ptr = (char) ((hw->me_hw_state & ME_CBRP_HRA) >> 8);
-	write_ptr = (char) ((hw->me_hw_state & ME_CBWP_HRA) >> 16);
+	me_csr = mei_me_mecsr_read(hw);
+	buffer_depth = (unsigned char)((me_csr & ME_CBD_HRA) >> 24);
+	read_ptr = (char) ((me_csr & ME_CBRP_HRA) >> 8);
+	write_ptr = (char) ((me_csr & ME_CBWP_HRA) >> 16);
 	filled_slots = (unsigned char) (write_ptr - read_ptr);
 
 	/* check for overflow */
@@ -833,6 +835,14 @@ static bool mei_me_fw_type_sps(struct pci_dev *pdev)
 	.fw_status.status[0] = PCI_CFG_HFS_1,   \
 	.fw_status.status[1] = PCI_CFG_HFS_2
 
+#define MEI_CFG_PCH8_HFS                        \
+	.fw_status.count = 6,                   \
+	.fw_status.status[0] = PCI_CFG_HFS_1,   \
+	.fw_status.status[1] = PCI_CFG_HFS_2,   \
+	.fw_status.status[2] = PCI_CFG_HFS_3,   \
+	.fw_status.status[3] = PCI_CFG_HFS_4,   \
+	.fw_status.status[4] = PCI_CFG_HFS_5,   \
+	.fw_status.status[5] = PCI_CFG_HFS_6
 
 /* ICH Legacy devices */
 const struct mei_cfg mei_me_legacy_cfg = {
@@ -856,9 +866,14 @@ const struct mei_cfg mei_me_pch_cpt_pbg_cfg = {
 	MEI_CFG_FW_NM,
 };
 
-/* PCH Lynx Point with quirk for SPS Firmware exclusion */
-const struct mei_cfg mei_me_lpt_cfg = {
-	MEI_CFG_PCH_HFS,
+/* PCH8 Lynx Point and newer devices */
+const struct mei_cfg mei_me_pch8_cfg = {
+	MEI_CFG_PCH8_HFS,
+};
+
+/* PCH8 Lynx Point with quirk for SPS Firmware exclusion */
+const struct mei_cfg mei_me_pch8_sps_cfg = {
+	MEI_CFG_PCH8_HFS,
 	MEI_CFG_FW_SPS,
 };
 
diff --git a/drivers/misc/mei/hw-me.h b/drivers/misc/mei/hw-me.h
index e6a59a62573a..d6567af44377 100644
--- a/drivers/misc/mei/hw-me.h
+++ b/drivers/misc/mei/hw-me.h
@@ -51,18 +51,11 @@ struct mei_cfg {
  *
  * @cfg: per device generation config and ops
  * @mem_addr:  io memory address
- * @host_hw_state: cached host state
- * @me_hw_state:   cached me (fw) state
  * @pg_state:      power gating state
  */
 struct mei_me_hw {
 	const struct mei_cfg *cfg;
 	void __iomem *mem_addr;
-	/*
-	 * hw states of host and fw(ME)
-	 */
-	u32 host_hw_state;
-	u32 me_hw_state;
 	enum mei_pg_state pg_state;
 };
 
@@ -72,7 +65,8 @@ extern const struct mei_cfg mei_me_legacy_cfg;
 extern const struct mei_cfg mei_me_ich_cfg;
 extern const struct mei_cfg mei_me_pch_cfg;
 extern const struct mei_cfg mei_me_pch_cpt_pbg_cfg;
-extern const struct mei_cfg mei_me_lpt_cfg;
+extern const struct mei_cfg mei_me_pch8_cfg;
+extern const struct mei_cfg mei_me_pch8_sps_cfg;
 
 struct mei_device *mei_me_dev_init(struct pci_dev *pdev,
 				   const struct mei_cfg *cfg);
diff --git a/drivers/misc/mei/hw-txe.c b/drivers/misc/mei/hw-txe.c
index c5e1902e493f..618ea721aca8 100644
--- a/drivers/misc/mei/hw-txe.c
+++ b/drivers/misc/mei/hw-txe.c
@@ -700,11 +700,10 @@ static int mei_txe_write(struct mei_device *dev,
 	mei_txe_input_ready_interrupt_enable(dev);
 
 	if (!mei_txe_is_input_ready(dev)) {
-		struct mei_fw_status fw_status;
+		char fw_sts_str[MEI_FW_STATUS_STR_SZ];
 
-		mei_fw_status(dev, &fw_status);
-		dev_err(dev->dev, "Input is not ready " FW_STS_FMT "\n",
-			FW_STS_PRM(fw_status));
+		mei_fw_status_str(dev, fw_sts_str, MEI_FW_STATUS_STR_SZ);
+		dev_err(dev->dev, "Input is not ready %s\n", fw_sts_str);
 		return -EAGAIN;
 	}
 
diff --git a/drivers/misc/mei/init.c b/drivers/misc/mei/init.c
index 7901d076c127..9306219d5675 100644
--- a/drivers/misc/mei/init.c
+++ b/drivers/misc/mei/init.c
@@ -54,6 +54,35 @@ const char *mei_pg_state_str(enum mei_pg_state state)
 #undef MEI_PG_STATE
 }
 
+/**
+ * mei_fw_status2str - convert fw status registers to printable string
+ *
+ * @fw_status:  firmware status
+ * @buf: string buffer at minimal size MEI_FW_STATUS_STR_SZ
+ * @len: buffer len must be >= MEI_FW_STATUS_STR_SZ
+ *
+ * Return: number of bytes written or -EINVAL if buffer is to small
+ */
+ssize_t mei_fw_status2str(struct mei_fw_status *fw_status,
+			  char *buf, size_t len)
+{
+	ssize_t cnt = 0;
+	int i;
+
+	buf[0] = '\0';
+
+	if (len < MEI_FW_STATUS_STR_SZ)
+		return -EINVAL;
+
+	for (i = 0; i < fw_status->count; i++)
+		cnt += scnprintf(buf + cnt, len - cnt, "%08X ",
+				fw_status->status[i]);
+
+	/* drop last space */
+	buf[cnt] = '\0';
+	return cnt;
+}
+EXPORT_SYMBOL_GPL(mei_fw_status2str);
 
 /**
  * mei_cancel_work - Cancel mei background jobs
@@ -86,12 +115,11 @@ int mei_reset(struct mei_device *dev)
 	    state != MEI_DEV_DISABLED &&
 	    state != MEI_DEV_POWER_DOWN &&
 	    state != MEI_DEV_POWER_UP) {
-		struct mei_fw_status fw_status;
+		char fw_sts_str[MEI_FW_STATUS_STR_SZ];
 
-		mei_fw_status(dev, &fw_status);
-		dev_warn(dev->dev,
-			"unexpected reset: dev_state = %s " FW_STS_FMT "\n",
-			mei_dev_state_str(state), FW_STS_PRM(fw_status));
+		mei_fw_status_str(dev, fw_sts_str, MEI_FW_STATUS_STR_SZ);
+		dev_warn(dev->dev, "unexpected reset: dev_state = %s fw status = %s\n",
+			 mei_dev_state_str(state), fw_sts_str);
 	}
 
 	/* we're already in reset, cancel the init timer
diff --git a/drivers/misc/mei/interrupt.c b/drivers/misc/mei/interrupt.c
index 20c6c511f438..711cddfa9c99 100644
--- a/drivers/misc/mei/interrupt.c
+++ b/drivers/misc/mei/interrupt.c
@@ -44,8 +44,6 @@ void mei_irq_compl_handler(struct mei_device *dev, struct mei_cl_cb *compl_list)
 	list_for_each_entry_safe(cb, next, &compl_list->list, list) {
 		cl = cb->cl;
 		list_del(&cb->list);
-		if (!cl)
-			continue;
 
 		dev_dbg(dev->dev, "completing call back.\n");
 		if (cl == &dev->iamthif_cl)
@@ -105,7 +103,7 @@ static int mei_cl_irq_read_msg(struct mei_device *dev,
 
 	list_for_each_entry_safe(cb, next, &dev->read_list.list, list) {
 		cl = cb->cl;
-		if (!cl || !mei_cl_is_reading(cl, mei_hdr))
+		if (!mei_cl_is_reading(cl, mei_hdr))
 			continue;
 
 		cl->reading_state = MEI_READING;
@@ -449,8 +447,6 @@ int mei_irq_write_handler(struct mei_device *dev, struct mei_cl_cb *cmpl_list)
 	list = &dev->write_waiting_list;
 	list_for_each_entry_safe(cb, next, &list->list, list) {
 		cl = cb->cl;
-		if (cl == NULL)
-			continue;
 
 		cl->status = 0;
 		list_del(&cb->list);
@@ -489,10 +485,6 @@ int mei_irq_write_handler(struct mei_device *dev, struct mei_cl_cb *cmpl_list)
 	dev_dbg(dev->dev, "complete control write list cb.\n");
 	list_for_each_entry_safe(cb, next, &dev->ctrl_wr_list.list, list) {
 		cl = cb->cl;
-		if (!cl) {
-			list_del(&cb->list);
-			return -ENODEV;
-		}
 		switch (cb->fop_type) {
 		case MEI_FOP_DISCONNECT:
 			/* send disconnect message */
@@ -530,8 +522,6 @@ int mei_irq_write_handler(struct mei_device *dev, struct mei_cl_cb *cmpl_list)
 	dev_dbg(dev->dev, "complete write list cb.\n");
 	list_for_each_entry_safe(cb, next, &dev->write_list.list, list) {
 		cl = cb->cl;
-		if (cl == NULL)
-			continue;
 		if (cl == &dev->iamthif_cl)
 			ret = mei_amthif_irq_write(cl, cb, cmpl_list);
 		else
diff --git a/drivers/misc/mei/main.c b/drivers/misc/mei/main.c
index beedc91f03a6..ae56ba6ca0e3 100644
--- a/drivers/misc/mei/main.c
+++ b/drivers/misc/mei/main.c
@@ -631,6 +631,44 @@ out:
 	return mask;
 }
 
+/**
+ * fw_status_show - mei device attribute show method
+ *
+ * @device: device pointer
+ * @attr: attribute pointer
+ * @buf:  char out buffer
+ *
+ * Return: number of the bytes printed into buf or error
+ */
+static ssize_t fw_status_show(struct device *device,
+		struct device_attribute *attr, char *buf)
+{
+	struct mei_device *dev = dev_get_drvdata(device);
+	struct mei_fw_status fw_status;
+	int err, i;
+	ssize_t cnt = 0;
+
+	mutex_lock(&dev->device_lock);
+	err = mei_fw_status(dev, &fw_status);
+	mutex_unlock(&dev->device_lock);
+	if (err) {
+		dev_err(device, "read fw_status error = %d\n", err);
+		return err;
+	}
+
+	for (i = 0; i < fw_status.count; i++)
+		cnt += scnprintf(buf + cnt, PAGE_SIZE - cnt, "%08X\n",
+				fw_status.status[i]);
+	return cnt;
+}
+static DEVICE_ATTR_RO(fw_status);
+
+static struct attribute *mei_attrs[] = {
+	&dev_attr_fw_status.attr,
+	NULL
+};
+ATTRIBUTE_GROUPS(mei);
+
 /*
  * file operations structure will be used for mei char device.
  */
@@ -710,8 +748,9 @@ int mei_register(struct mei_device *dev, struct device *parent)
 		goto err_dev_add;
 	}
 
-	clsdev = device_create(mei_class, parent, devno,
-			 NULL, "mei%d", dev->minor);
+	clsdev = device_create_with_groups(mei_class, parent, devno,
+					   dev, mei_groups,
+					   "mei%d", dev->minor);
 
 	if (IS_ERR(clsdev)) {
 		dev_err(parent, "unable to create device %d:%d\n",
diff --git a/drivers/misc/mei/mei_dev.h b/drivers/misc/mei/mei_dev.h
index 61b04d7646f1..3dad74a8d496 100644
--- a/drivers/misc/mei/mei_dev.h
+++ b/drivers/misc/mei/mei_dev.h
@@ -152,7 +152,10 @@ struct mei_msg_data {
 };
 
 /* Maximum number of processed FW status registers */
-#define MEI_FW_STATUS_MAX 2
+#define MEI_FW_STATUS_MAX 6
+/* Minimal  buffer for FW status string (8 bytes in dw + space or '\0') */
+#define MEI_FW_STATUS_STR_SZ (MEI_FW_STATUS_MAX * (8 + 1))
+
 
 /*
  * struct mei_fw_status - storage of FW status data
@@ -349,6 +352,7 @@ void mei_cl_bus_rx_event(struct mei_cl *cl);
 void mei_cl_bus_remove_devices(struct mei_device *dev);
 int mei_cl_bus_init(void);
 void mei_cl_bus_exit(void);
+struct mei_cl *mei_cl_bus_find_cl_by_uuid(struct mei_device *dev, uuid_le uuid);
 
 
 /**
@@ -804,11 +808,6 @@ static inline int mei_fw_status(struct mei_device *dev,
 	return dev->ops->fw_status(dev, fw_status);
 }
 
-#define FW_STS_FMT "%08X %08X"
-#define FW_STS_PRM(fw_status) \
-	(fw_status).count > 0 ? (fw_status).status[0] : 0xDEADBEEF, \
-	(fw_status).count > 1 ? (fw_status).status[1] : 0xDEADBEEF
-
 bool mei_hbuf_acquire(struct mei_device *dev);
 
 bool mei_write_is_idle(struct mei_device *dev);
@@ -832,4 +831,32 @@ void mei_deregister(struct mei_device *dev);
 	(hdr)->host_addr, (hdr)->me_addr, \
 	(hdr)->length, (hdr)->internal, (hdr)->msg_complete
 
+ssize_t mei_fw_status2str(struct mei_fw_status *fw_sts, char *buf, size_t len);
+/**
+ * mei_fw_status_str - fetch and convert fw status registers to printable string
+ *
+ * @dev: the device structure
+ * @buf: string buffer at minimal size MEI_FW_STATUS_STR_SZ
+ * @len: buffer len must be >= MEI_FW_STATUS_STR_SZ
+ *
+ * Return: number of bytes written or < 0 on failure
+ */
+static inline ssize_t mei_fw_status_str(struct mei_device *dev,
+					char *buf, size_t len)
+{
+	struct mei_fw_status fw_status;
+	int ret;
+
+	buf[0] = '\0';
+
+	ret = mei_fw_status(dev, &fw_status);
+	if (ret)
+		return ret;
+
+	ret = mei_fw_status2str(&fw_status, buf, MEI_FW_STATUS_STR_SZ);
+
+	return ret;
+}
+
+
 #endif
diff --git a/drivers/misc/mei/nfc.c b/drivers/misc/mei/nfc.c
index 622654323177..60ca9240368e 100644
--- a/drivers/misc/mei/nfc.c
+++ b/drivers/misc/mei/nfc.c
@@ -117,8 +117,6 @@ struct mei_nfc_dev {
 	u16 recv_req_id;
 };
 
-static struct mei_nfc_dev nfc_dev;
-
 /* UUIDs for NFC F/W clients */
 const uuid_le mei_nfc_guid = UUID_LE(0x0bb17a78, 0x2a8e, 0x4c50,
 				     0x94, 0xd4, 0x50, 0x26,
@@ -138,6 +136,9 @@ static const uuid_le mei_nfc_info_guid = UUID_LE(0xd2de1625, 0x382d, 0x417d,
 
 static void mei_nfc_free(struct mei_nfc_dev *ndev)
 {
+	if (!ndev)
+		return;
+
 	if (ndev->cl) {
 		list_del(&ndev->cl->device_link);
 		mei_cl_unlink(ndev->cl);
@@ -150,7 +151,7 @@ static void mei_nfc_free(struct mei_nfc_dev *ndev)
 		kfree(ndev->cl_info);
 	}
 
-	memset(ndev, 0, sizeof(struct mei_nfc_dev));
+	kfree(ndev);
 }
 
 static int mei_nfc_build_bus_name(struct mei_nfc_dev *ndev)
@@ -319,9 +320,10 @@ err:
 static int mei_nfc_enable(struct mei_cl_device *cldev)
 {
 	struct mei_device *dev;
-	struct mei_nfc_dev *ndev = &nfc_dev;
+	struct mei_nfc_dev *ndev;
 	int ret;
 
+	ndev = (struct mei_nfc_dev *)cldev->priv_data;
 	dev = ndev->cl->dev;
 
 	ret = mei_nfc_connect(ndev);
@@ -479,15 +481,25 @@ err:
 
 int mei_nfc_host_init(struct mei_device *dev)
 {
-	struct mei_nfc_dev *ndev = &nfc_dev;
+	struct mei_nfc_dev *ndev;
 	struct mei_cl *cl_info, *cl = NULL;
 	struct mei_me_client *me_cl;
 	int ret;
 
-	/* already initialized */
-	if (ndev->cl_info)
+
+	/* in case of internal reset bail out
+	 * as the device is already setup
+	 */
+	cl = mei_cl_bus_find_cl_by_uuid(dev, mei_nfc_guid);
+	if (cl)
 		return 0;
 
+	ndev = kzalloc(sizeof(struct mei_nfc_dev), GFP_KERNEL);
+	if (!ndev) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
 	ndev->cl_info = mei_cl_allocate(dev);
 	ndev->cl = mei_cl_allocate(dev);
 
@@ -550,9 +562,31 @@ err:
 
 void mei_nfc_host_exit(struct mei_device *dev)
 {
-	struct mei_nfc_dev *ndev = &nfc_dev;
+	struct mei_nfc_dev *ndev;
+	struct mei_cl *cl;
+	struct mei_cl_device *cldev;
+
+	cl = mei_cl_bus_find_cl_by_uuid(dev, mei_nfc_guid);
+	if (!cl)
+		return;
+
+	cldev = cl->device;
+	if (!cldev)
+		return;
 
-	cancel_work_sync(&ndev->init_work);
+	ndev = (struct mei_nfc_dev *)cldev->priv_data;
+	if (ndev)
+		cancel_work_sync(&ndev->init_work);
+
+	cldev->priv_data = NULL;
+
+	mutex_lock(&dev->device_lock);
+	/* Need to remove the device here
+	 * since mei_nfc_free will unlink the clients
+	 */
+	mei_cl_remove_device(cldev);
+	mei_nfc_free(ndev);
+	mutex_unlock(&dev->device_lock);
 }
 
 
diff --git a/drivers/misc/mei/pci-me.c b/drivers/misc/mei/pci-me.c
index cf20d397068a..bd3039ab8f98 100644
--- a/drivers/misc/mei/pci-me.c
+++ b/drivers/misc/mei/pci-me.c
@@ -76,12 +76,12 @@ static const struct pci_device_id mei_me_pci_tbl[] = {
 	{MEI_PCI_DEVICE(MEI_DEV_ID_PPT_1, mei_me_pch_cfg)},
 	{MEI_PCI_DEVICE(MEI_DEV_ID_PPT_2, mei_me_pch_cfg)},
 	{MEI_PCI_DEVICE(MEI_DEV_ID_PPT_3, mei_me_pch_cfg)},
-	{MEI_PCI_DEVICE(MEI_DEV_ID_LPT_H, mei_me_lpt_cfg)},
-	{MEI_PCI_DEVICE(MEI_DEV_ID_LPT_W, mei_me_lpt_cfg)},
-	{MEI_PCI_DEVICE(MEI_DEV_ID_LPT_LP, mei_me_pch_cfg)},
-	{MEI_PCI_DEVICE(MEI_DEV_ID_LPT_HR, mei_me_lpt_cfg)},
-	{MEI_PCI_DEVICE(MEI_DEV_ID_WPT_LP, mei_me_pch_cfg)},
-	{MEI_PCI_DEVICE(MEI_DEV_ID_WPT_LP_2, mei_me_pch_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_LPT_H, mei_me_pch8_sps_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_LPT_W, mei_me_pch8_sps_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_LPT_LP, mei_me_pch8_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_LPT_HR, mei_me_pch8_sps_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_WPT_LP, mei_me_pch8_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_WPT_LP_2, mei_me_pch8_cfg)},
 
 	/* required last entry */
 	{0, }
diff --git a/drivers/misc/mei/pci-txe.c b/drivers/misc/mei/pci-txe.c
index 1f572deacf54..c86e2ddbe30a 100644
--- a/drivers/misc/mei/pci-txe.c
+++ b/drivers/misc/mei/pci-txe.c
@@ -37,6 +37,7 @@
 
 static const struct pci_device_id mei_txe_pci_tbl[] = {
 	{PCI_VDEVICE(INTEL, 0x0F18)}, /* Baytrail */
+	{PCI_VDEVICE(INTEL, 0x2298)}, /* Cherrytrail */
 
 	{0, }
 };
diff --git a/drivers/misc/mei/wd.c b/drivers/misc/mei/wd.c
index b836dfffceb5..b1d892cea94d 100644
--- a/drivers/misc/mei/wd.c
+++ b/drivers/misc/mei/wd.c
@@ -270,15 +270,18 @@ static int mei_wd_ops_stop(struct watchdog_device *wd_dev)
 static int mei_wd_ops_ping(struct watchdog_device *wd_dev)
 {
 	struct mei_device *dev;
+	struct mei_cl *cl;
 	int ret;
 
 	dev = watchdog_get_drvdata(wd_dev);
 	if (!dev)
 		return -ENODEV;
 
+	cl = &dev->wd_cl;
+
 	mutex_lock(&dev->device_lock);
 
-	if (dev->wd_cl.state != MEI_FILE_CONNECTED) {
+	if (cl->state != MEI_FILE_CONNECTED) {
 		dev_err(dev->dev, "wd: not connected.\n");
 		ret = -ENODEV;
 		goto end;
@@ -286,12 +289,12 @@ static int mei_wd_ops_ping(struct watchdog_device *wd_dev)
 
 	dev->wd_state = MEI_WD_RUNNING;
 
-	ret = mei_cl_flow_ctrl_creds(&dev->wd_cl);
+	ret = mei_cl_flow_ctrl_creds(cl);
 	if (ret < 0)
 		goto end;
+
 	/* Check if we can send the ping to HW*/
 	if (ret && mei_hbuf_acquire(dev)) {
-
 		dev_dbg(dev->dev, "wd: sending ping\n");
 
 		ret = mei_wd_send(dev);
diff --git a/drivers/misc/mic/card/mic_x100.c b/drivers/misc/mic/card/mic_x100.c
index 9d57545d64f6..e98e537d68e3 100644
--- a/drivers/misc/mic/card/mic_x100.c
+++ b/drivers/misc/mic/card/mic_x100.c
@@ -255,7 +255,6 @@ static struct platform_driver __refdata mic_platform_driver = {
 	.shutdown = mic_platform_shutdown,
 	.driver         = {
 		.name   = mic_driver_name,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/misc/mic/host/mic_debugfs.c b/drivers/misc/mic/host/mic_debugfs.c
index 028ba5d6fd1c..687e9aacf3bb 100644
--- a/drivers/misc/mic/host/mic_debugfs.c
+++ b/drivers/misc/mic/host/mic_debugfs.c
@@ -326,21 +326,27 @@ static int mic_vdev_info_show(struct seq_file *s, void *unused)
 			}
 			avail = vrh->vring.avail;
 			seq_printf(s, "avail flags 0x%x idx %d\n",
-				   avail->flags, avail->idx & (num - 1));
+				   vringh16_to_cpu(vrh, avail->flags),
+				   vringh16_to_cpu(vrh, avail->idx) & (num - 1));
 			seq_printf(s, "avail flags 0x%x idx %d\n",
-				   avail->flags, avail->idx);
+				   vringh16_to_cpu(vrh, avail->flags),
+				   vringh16_to_cpu(vrh, avail->idx));
 			for (j = 0; j < num; j++)
 				seq_printf(s, "avail ring[%d] %d\n",
 					   j, avail->ring[j]);
 			used = vrh->vring.used;
 			seq_printf(s, "used flags 0x%x idx %d\n",
-				   used->flags, used->idx & (num - 1));
+				   vringh16_to_cpu(vrh, used->flags),
+				   vringh16_to_cpu(vrh, used->idx) & (num - 1));
 			seq_printf(s, "used flags 0x%x idx %d\n",
-				   used->flags, used->idx);
+				   vringh16_to_cpu(vrh, used->flags),
+				   vringh16_to_cpu(vrh, used->idx));
 			for (j = 0; j < num; j++)
 				seq_printf(s, "used ring[%d] id %d len %d\n",
-					   j, used->ring[j].id,
-					   used->ring[j].len);
+					   j, vringh32_to_cpu(vrh,
+							      used->ring[j].id),
+					   vringh32_to_cpu(vrh,
+							   used->ring[j].len));
 		}
 	}
 	mutex_unlock(&mdev->mic_mutex);
diff --git a/drivers/misc/pch_phub.c b/drivers/misc/pch_phub.c
index 956597321d2a..9a17a9bab8d6 100644
--- a/drivers/misc/pch_phub.c
+++ b/drivers/misc/pch_phub.c
@@ -158,6 +158,7 @@ static void pch_phub_read_modify_write_reg(struct pch_phub_reg *chip,
 	iowrite32(((ioread32(reg_addr) & ~mask)) | data, reg_addr);
 }
 
+#ifdef CONFIG_PM
 /* pch_phub_save_reg_conf - saves register configuration */
 static void pch_phub_save_reg_conf(struct pci_dev *pdev)
 {
@@ -280,6 +281,7 @@ static void pch_phub_restore_reg_conf(struct pci_dev *pdev)
 	if ((chip->ioh_type == 2) || (chip->ioh_type == 4))
 		iowrite32(chip->funcsel_reg, p + FUNCSEL_REG_OFFSET);
 }
+#endif
 
 /**
  * pch_phub_read_serial_rom() - Reading Serial ROM
diff --git a/drivers/misc/ti-st/st_kim.c b/drivers/misc/ti-st/st_kim.c
index 21c2337bad68..e4b7ee4f57b8 100644
--- a/drivers/misc/ti-st/st_kim.c
+++ b/drivers/misc/ti-st/st_kim.c
@@ -858,7 +858,6 @@ static struct platform_driver kim_platform_driver = {
 	.resume = kim_resume,
 	.driver = {
 		.name = "kim",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mmc/core/mmc.c b/drivers/mmc/core/mmc.c
index 02ad79229f65..7466ce098e60 100644
--- a/drivers/mmc/core/mmc.c
+++ b/drivers/mmc/core/mmc.c
@@ -886,7 +886,7 @@ static int mmc_select_bus_width(struct mmc_card *card)
 	unsigned idx, bus_width = 0;
 	int err = 0;
 
-	if (!mmc_can_ext_csd(card) &&
+	if (!mmc_can_ext_csd(card) ||
 	    !(host->caps & (MMC_CAP_4_BIT_DATA | MMC_CAP_8_BIT_DATA)))
 		return 0;
 
diff --git a/drivers/mmc/host/atmel-mci.c b/drivers/mmc/host/atmel-mci.c
index 62aba9af19f4..03d7c7521d97 100644
--- a/drivers/mmc/host/atmel-mci.c
+++ b/drivers/mmc/host/atmel-mci.c
@@ -2561,7 +2561,7 @@ static int atmci_runtime_resume(struct device *dev)
 static const struct dev_pm_ops atmci_dev_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
 				pm_runtime_force_resume)
-	SET_PM_RUNTIME_PM_OPS(atmci_runtime_suspend, atmci_runtime_resume, NULL)
+	SET_RUNTIME_PM_OPS(atmci_runtime_suspend, atmci_runtime_resume, NULL)
 };
 
 static struct platform_driver atmci_driver = {
diff --git a/drivers/mmc/host/au1xmmc.c b/drivers/mmc/host/au1xmmc.c
index 725f6a6fd89b..ed77fbfa4774 100644
--- a/drivers/mmc/host/au1xmmc.c
+++ b/drivers/mmc/host/au1xmmc.c
@@ -1206,7 +1206,6 @@ static struct platform_driver au1xmmc_driver = {
 	.resume        = au1xmmc_resume,
 	.driver        = {
 		.name  = DRIVER_NAME,
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mmc/host/davinci_mmc.c b/drivers/mmc/host/davinci_mmc.c
index 5d4c5e0fba2f..1625f908dc70 100644
--- a/drivers/mmc/host/davinci_mmc.c
+++ b/drivers/mmc/host/davinci_mmc.c
@@ -1466,7 +1466,6 @@ static const struct dev_pm_ops davinci_mmcsd_pm = {
 static struct platform_driver davinci_mmcsd_driver = {
 	.driver		= {
 		.name	= "davinci_mmc",
-		.owner	= THIS_MODULE,
 		.pm	= davinci_mmcsd_pm_ops,
 		.of_match_table = davinci_mmc_dt_ids,
 	},
diff --git a/drivers/mmc/host/sdhci-acpi.c b/drivers/mmc/host/sdhci-acpi.c
index c01eac7c8196..e3e56d35f0ee 100644
--- a/drivers/mmc/host/sdhci-acpi.c
+++ b/drivers/mmc/host/sdhci-acpi.c
@@ -463,7 +463,6 @@ static const struct dev_pm_ops sdhci_acpi_pm_ops = {
 static struct platform_driver sdhci_acpi_driver = {
 	.driver = {
 		.name			= "sdhci-acpi",
-		.owner			= THIS_MODULE,
 		.acpi_match_table	= sdhci_acpi_ids,
 		.pm			= &sdhci_acpi_pm_ops,
 	},
diff --git a/drivers/mmc/host/tmio_mmc.c b/drivers/mmc/host/tmio_mmc.c
index 2616fdfdbbeb..2ca0afaab792 100644
--- a/drivers/mmc/host/tmio_mmc.c
+++ b/drivers/mmc/host/tmio_mmc.c
@@ -143,7 +143,6 @@ static const struct dev_pm_ops tmio_mmc_dev_pm_ops = {
 static struct platform_driver tmio_mmc_driver = {
 	.driver = {
 		.name = "tmio-mmc",
-		.owner = THIS_MODULE,
 		.pm = &tmio_mmc_dev_pm_ops,
 	},
 	.probe = tmio_mmc_probe,
diff --git a/drivers/mmc/host/usdhi6rol0.c b/drivers/mmc/host/usdhi6rol0.c
index f0a39eb049af..54b082b1804a 100644
--- a/drivers/mmc/host/usdhi6rol0.c
+++ b/drivers/mmc/host/usdhi6rol0.c
@@ -1834,7 +1834,6 @@ static struct platform_driver usdhi6_driver = {
 	.remove		= usdhi6_remove,
 	.driver		= {
 		.name	= "usdhi6rol0",
-		.owner	= THIS_MODULE,
 		.of_match_table = usdhi6_of_match,
 	},
 };
diff --git a/drivers/mmc/host/wbsd.c b/drivers/mmc/host/wbsd.c
index 9a6dfb0c4ecc..ca183ea767b3 100644
--- a/drivers/mmc/host/wbsd.c
+++ b/drivers/mmc/host/wbsd.c
@@ -1914,7 +1914,6 @@ static struct platform_driver wbsd_driver = {
 	.resume		= wbsd_platform_resume,
 	.driver		= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mmc/host/wmt-sdmmc.c b/drivers/mmc/host/wmt-sdmmc.c
index 54181b4f6e9e..dd2e1aa95ba3 100644
--- a/drivers/mmc/host/wmt-sdmmc.c
+++ b/drivers/mmc/host/wmt-sdmmc.c
@@ -991,7 +991,6 @@ static struct platform_driver wmt_mci_driver = {
 	.remove = wmt_mci_remove,
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.pm = wmt_mci_pm_ops,
 		.of_match_table = wmt_mci_dt_ids,
 	},
diff --git a/drivers/mtd/Kconfig b/drivers/mtd/Kconfig
index 94b821042d9d..71fea895ce38 100644
--- a/drivers/mtd/Kconfig
+++ b/drivers/mtd/Kconfig
@@ -133,7 +133,7 @@ config MTD_OF_PARTS
 	help
 	  This provides a partition parsing function which derives
 	  the partition map from the children of the flash node,
-	  as described in Documentation/devicetree/booting-without-of.txt.
+	  as described in Documentation/devicetree/bindings/mtd/partition.txt.
 
 config MTD_AR7_PARTS
 	tristate "TI AR7 partitioning support"
diff --git a/drivers/mtd/bcm47xxpart.c b/drivers/mtd/bcm47xxpart.c
index 8057f52a45b7..cc13ea5ce4d5 100644
--- a/drivers/mtd/bcm47xxpart.c
+++ b/drivers/mtd/bcm47xxpart.c
@@ -15,8 +15,12 @@
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/partitions.h>
 
-/* 10 parts were found on sflash on Netgear WNDR4500 */
-#define BCM47XXPART_MAX_PARTS		12
+/*
+ * NAND flash on Netgear R6250 was verified to contain 15 partitions.
+ * This will result in allocating too big array for some old devices, but the
+ * memory will be freed soon anyway (see mtd_device_parse_register).
+ */
+#define BCM47XXPART_MAX_PARTS		20
 
 /*
  * Amount of bytes we read when analyzing each block of flash memory.
@@ -168,18 +172,26 @@ static int bcm47xxpart_parse(struct mtd_info *master,
 				i++;
 			}
 
-			bcm47xxpart_add_part(&parts[curr_part++], "linux",
-					     offset + trx->offset[i], 0);
-			i++;
+			if (trx->offset[i]) {
+				bcm47xxpart_add_part(&parts[curr_part++],
+						     "linux",
+						     offset + trx->offset[i],
+						     0);
+				i++;
+			}
 
 			/*
 			 * Pure rootfs size is known and can be calculated as:
 			 * trx->length - trx->offset[i]. We don't fill it as
 			 * we want to have jffs2 (overlay) in the same mtd.
 			 */
-			bcm47xxpart_add_part(&parts[curr_part++], "rootfs",
-					     offset + trx->offset[i], 0);
-			i++;
+			if (trx->offset[i]) {
+				bcm47xxpart_add_part(&parts[curr_part++],
+						     "rootfs",
+						     offset + trx->offset[i],
+						     0);
+				i++;
+			}
 
 			last_trx_part = curr_part - 1;
 
diff --git a/drivers/mtd/chips/cfi_cmdset_0001.c b/drivers/mtd/chips/cfi_cmdset_0001.c
index 3096f3ded3ad..286b97a304cf 100644
--- a/drivers/mtd/chips/cfi_cmdset_0001.c
+++ b/drivers/mtd/chips/cfi_cmdset_0001.c
@@ -2654,8 +2654,7 @@ static void cfi_intelext_destroy(struct mtd_info *mtd)
 	kfree(cfi);
 	for (i = 0; i < mtd->numeraseregions; i++) {
 		region = &mtd->eraseregions[i];
-		if (region->lockmap)
-			kfree(region->lockmap);
+		kfree(region->lockmap);
 	}
 	kfree(mtd->eraseregions);
 }
diff --git a/drivers/mtd/devices/bcm47xxsflash.c b/drivers/mtd/devices/bcm47xxsflash.c
index 77de29bc02ba..3d008a9410be 100644
--- a/drivers/mtd/devices/bcm47xxsflash.c
+++ b/drivers/mtd/devices/bcm47xxsflash.c
@@ -329,7 +329,6 @@ static struct platform_driver bcma_sflash_driver = {
 	.remove = bcm47xxsflash_bcma_remove,
 	.driver = {
 		.name = "bcma_sflash",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mtd/devices/docg3.c b/drivers/mtd/devices/docg3.c
index 21cc4b66feaa..448ce42f951e 100644
--- a/drivers/mtd/devices/docg3.c
+++ b/drivers/mtd/devices/docg3.c
@@ -22,6 +22,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/errno.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/string.h>
 #include <linux/slab.h>
@@ -1655,22 +1656,21 @@ static int dbg_flashctrl_show(struct seq_file *s, void *p)
 {
 	struct docg3 *docg3 = (struct docg3 *)s->private;
 
-	int pos = 0;
 	u8 fctrl;
 
 	mutex_lock(&docg3->cascade->lock);
 	fctrl = doc_register_readb(docg3, DOC_FLASHCONTROL);
 	mutex_unlock(&docg3->cascade->lock);
 
-	pos += seq_printf(s,
-		 "FlashControl : 0x%02x (%s,CE# %s,%s,%s,flash %s)\n",
-		 fctrl,
-		 fctrl & DOC_CTRL_VIOLATION ? "protocol violation" : "-",
-		 fctrl & DOC_CTRL_CE ? "active" : "inactive",
-		 fctrl & DOC_CTRL_PROTECTION_ERROR ? "protection error" : "-",
-		 fctrl & DOC_CTRL_SEQUENCE_ERROR ? "sequence error" : "-",
-		 fctrl & DOC_CTRL_FLASHREADY ? "ready" : "not ready");
-	return pos;
+	seq_printf(s, "FlashControl : 0x%02x (%s,CE# %s,%s,%s,flash %s)\n",
+		   fctrl,
+		   fctrl & DOC_CTRL_VIOLATION ? "protocol violation" : "-",
+		   fctrl & DOC_CTRL_CE ? "active" : "inactive",
+		   fctrl & DOC_CTRL_PROTECTION_ERROR ? "protection error" : "-",
+		   fctrl & DOC_CTRL_SEQUENCE_ERROR ? "sequence error" : "-",
+		   fctrl & DOC_CTRL_FLASHREADY ? "ready" : "not ready");
+
+	return 0;
 }
 DEBUGFS_RO_ATTR(flashcontrol, dbg_flashctrl_show);
 
@@ -1678,58 +1678,56 @@ static int dbg_asicmode_show(struct seq_file *s, void *p)
 {
 	struct docg3 *docg3 = (struct docg3 *)s->private;
 
-	int pos = 0, pctrl, mode;
+	int pctrl, mode;
 
 	mutex_lock(&docg3->cascade->lock);
 	pctrl = doc_register_readb(docg3, DOC_ASICMODE);
 	mode = pctrl & 0x03;
 	mutex_unlock(&docg3->cascade->lock);
 
-	pos += seq_printf(s,
-			 "%04x : RAM_WE=%d,RSTIN_RESET=%d,BDETCT_RESET=%d,WRITE_ENABLE=%d,POWERDOWN=%d,MODE=%d%d (",
-			 pctrl,
-			 pctrl & DOC_ASICMODE_RAM_WE ? 1 : 0,
-			 pctrl & DOC_ASICMODE_RSTIN_RESET ? 1 : 0,
-			 pctrl & DOC_ASICMODE_BDETCT_RESET ? 1 : 0,
-			 pctrl & DOC_ASICMODE_MDWREN ? 1 : 0,
-			 pctrl & DOC_ASICMODE_POWERDOWN ? 1 : 0,
-			 mode >> 1, mode & 0x1);
+	seq_printf(s,
+		   "%04x : RAM_WE=%d,RSTIN_RESET=%d,BDETCT_RESET=%d,WRITE_ENABLE=%d,POWERDOWN=%d,MODE=%d%d (",
+		   pctrl,
+		   pctrl & DOC_ASICMODE_RAM_WE ? 1 : 0,
+		   pctrl & DOC_ASICMODE_RSTIN_RESET ? 1 : 0,
+		   pctrl & DOC_ASICMODE_BDETCT_RESET ? 1 : 0,
+		   pctrl & DOC_ASICMODE_MDWREN ? 1 : 0,
+		   pctrl & DOC_ASICMODE_POWERDOWN ? 1 : 0,
+		   mode >> 1, mode & 0x1);
 
 	switch (mode) {
 	case DOC_ASICMODE_RESET:
-		pos += seq_puts(s, "reset");
+		seq_puts(s, "reset");
 		break;
 	case DOC_ASICMODE_NORMAL:
-		pos += seq_puts(s, "normal");
+		seq_puts(s, "normal");
 		break;
 	case DOC_ASICMODE_POWERDOWN:
-		pos += seq_puts(s, "powerdown");
+		seq_puts(s, "powerdown");
 		break;
 	}
-	pos += seq_puts(s, ")\n");
-	return pos;
+	seq_puts(s, ")\n");
+	return 0;
 }
 DEBUGFS_RO_ATTR(asic_mode, dbg_asicmode_show);
 
 static int dbg_device_id_show(struct seq_file *s, void *p)
 {
 	struct docg3 *docg3 = (struct docg3 *)s->private;
-	int pos = 0;
 	int id;
 
 	mutex_lock(&docg3->cascade->lock);
 	id = doc_register_readb(docg3, DOC_DEVICESELECT);
 	mutex_unlock(&docg3->cascade->lock);
 
-	pos += seq_printf(s, "DeviceId = %d\n", id);
-	return pos;
+	seq_printf(s, "DeviceId = %d\n", id);
+	return 0;
 }
 DEBUGFS_RO_ATTR(device_id, dbg_device_id_show);
 
 static int dbg_protection_show(struct seq_file *s, void *p)
 {
 	struct docg3 *docg3 = (struct docg3 *)s->private;
-	int pos = 0;
 	int protect, dps0, dps0_low, dps0_high, dps1, dps1_low, dps1_high;
 
 	mutex_lock(&docg3->cascade->lock);
@@ -1742,45 +1740,40 @@ static int dbg_protection_show(struct seq_file *s, void *p)
 	dps1_high = doc_register_readw(docg3, DOC_DPS1_ADDRHIGH);
 	mutex_unlock(&docg3->cascade->lock);
 
-	pos += seq_printf(s, "Protection = 0x%02x (",
-			 protect);
+	seq_printf(s, "Protection = 0x%02x (", protect);
 	if (protect & DOC_PROTECT_FOUNDRY_OTP_LOCK)
-		pos += seq_puts(s, "FOUNDRY_OTP_LOCK,");
+		seq_puts(s, "FOUNDRY_OTP_LOCK,");
 	if (protect & DOC_PROTECT_CUSTOMER_OTP_LOCK)
-		pos += seq_puts(s, "CUSTOMER_OTP_LOCK,");
+		seq_puts(s, "CUSTOMER_OTP_LOCK,");
 	if (protect & DOC_PROTECT_LOCK_INPUT)
-		pos += seq_puts(s, "LOCK_INPUT,");
+		seq_puts(s, "LOCK_INPUT,");
 	if (protect & DOC_PROTECT_STICKY_LOCK)
-		pos += seq_puts(s, "STICKY_LOCK,");
+		seq_puts(s, "STICKY_LOCK,");
 	if (protect & DOC_PROTECT_PROTECTION_ENABLED)
-		pos += seq_puts(s, "PROTECTION ON,");
+		seq_puts(s, "PROTECTION ON,");
 	if (protect & DOC_PROTECT_IPL_DOWNLOAD_LOCK)
-		pos += seq_puts(s, "IPL_DOWNLOAD_LOCK,");
+		seq_puts(s, "IPL_DOWNLOAD_LOCK,");
 	if (protect & DOC_PROTECT_PROTECTION_ERROR)
-		pos += seq_puts(s, "PROTECT_ERR,");
+		seq_puts(s, "PROTECT_ERR,");
 	else
-		pos += seq_puts(s, "NO_PROTECT_ERR");
-	pos += seq_puts(s, ")\n");
-
-	pos += seq_printf(s, "DPS0 = 0x%02x : "
-			 "Protected area [0x%x - 0x%x] : OTP=%d, READ=%d, "
-			 "WRITE=%d, HW_LOCK=%d, KEY_OK=%d\n",
-			 dps0, dps0_low, dps0_high,
-			 !!(dps0 & DOC_DPS_OTP_PROTECTED),
-			 !!(dps0 & DOC_DPS_READ_PROTECTED),
-			 !!(dps0 & DOC_DPS_WRITE_PROTECTED),
-			 !!(dps0 & DOC_DPS_HW_LOCK_ENABLED),
-			 !!(dps0 & DOC_DPS_KEY_OK));
-	pos += seq_printf(s, "DPS1 = 0x%02x : "
-			 "Protected area [0x%x - 0x%x] : OTP=%d, READ=%d, "
-			 "WRITE=%d, HW_LOCK=%d, KEY_OK=%d\n",
-			 dps1, dps1_low, dps1_high,
-			 !!(dps1 & DOC_DPS_OTP_PROTECTED),
-			 !!(dps1 & DOC_DPS_READ_PROTECTED),
-			 !!(dps1 & DOC_DPS_WRITE_PROTECTED),
-			 !!(dps1 & DOC_DPS_HW_LOCK_ENABLED),
-			 !!(dps1 & DOC_DPS_KEY_OK));
-	return pos;
+		seq_puts(s, "NO_PROTECT_ERR");
+	seq_puts(s, ")\n");
+
+	seq_printf(s, "DPS0 = 0x%02x : Protected area [0x%x - 0x%x] : OTP=%d, READ=%d, WRITE=%d, HW_LOCK=%d, KEY_OK=%d\n",
+		   dps0, dps0_low, dps0_high,
+		   !!(dps0 & DOC_DPS_OTP_PROTECTED),
+		   !!(dps0 & DOC_DPS_READ_PROTECTED),
+		   !!(dps0 & DOC_DPS_WRITE_PROTECTED),
+		   !!(dps0 & DOC_DPS_HW_LOCK_ENABLED),
+		   !!(dps0 & DOC_DPS_KEY_OK));
+	seq_printf(s, "DPS1 = 0x%02x : Protected area [0x%x - 0x%x] : OTP=%d, READ=%d, WRITE=%d, HW_LOCK=%d, KEY_OK=%d\n",
+		   dps1, dps1_low, dps1_high,
+		   !!(dps1 & DOC_DPS_OTP_PROTECTED),
+		   !!(dps1 & DOC_DPS_READ_PROTECTED),
+		   !!(dps1 & DOC_DPS_WRITE_PROTECTED),
+		   !!(dps1 & DOC_DPS_HW_LOCK_ENABLED),
+		   !!(dps1 & DOC_DPS_KEY_OK));
+	return 0;
 }
 DEBUGFS_RO_ATTR(protection, dbg_protection_show);
 
@@ -2126,10 +2119,18 @@ static int __exit docg3_release(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_OF
+static struct of_device_id docg3_dt_ids[] = {
+	{ .compatible = "m-systems,diskonchip-g3" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, docg3_dt_ids);
+#endif
+
 static struct platform_driver g3_driver = {
 	.driver		= {
 		.name	= "docg3",
-		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(docg3_dt_ids),
 	},
 	.suspend	= docg3_suspend,
 	.resume		= docg3_resume,
diff --git a/drivers/mtd/devices/m25p80.c b/drivers/mtd/devices/m25p80.c
index ed827cf894e4..85e35467fba6 100644
--- a/drivers/mtd/devices/m25p80.c
+++ b/drivers/mtd/devices/m25p80.c
@@ -128,13 +128,10 @@ static int m25p80_read(struct spi_nor *nor, loff_t from, size_t len,
 	struct spi_device *spi = flash->spi;
 	struct spi_transfer t[2];
 	struct spi_message m;
-	int dummy = nor->read_dummy;
-	int ret;
+	unsigned int dummy = nor->read_dummy;
 
-	/* Wait till previous write/erase is done. */
-	ret = nor->wait_till_ready(nor);
-	if (ret)
-		return ret;
+	/* convert the dummy cycles to the number of bytes */
+	dummy /= 8;
 
 	spi_message_init(&m);
 	memset(t, 0, (sizeof t));
@@ -160,21 +157,10 @@ static int m25p80_read(struct spi_nor *nor, loff_t from, size_t len,
 static int m25p80_erase(struct spi_nor *nor, loff_t offset)
 {
 	struct m25p *flash = nor->priv;
-	int ret;
 
 	dev_dbg(nor->dev, "%dKiB at 0x%08x\n",
 		flash->mtd.erasesize / 1024, (u32)offset);
 
-	/* Wait until finished previous write command. */
-	ret = nor->wait_till_ready(nor);
-	if (ret)
-		return ret;
-
-	/* Send write enable, then erase commands. */
-	ret = nor->write_reg(nor, SPINOR_OP_WREN, NULL, 0, 0);
-	if (ret)
-		return ret;
-
 	/* Set up command buffer. */
 	flash->command[0] = nor->erase_opcode;
 	m25p_addr2cmd(nor, offset, flash->command);
@@ -260,7 +246,6 @@ static int m25p_remove(struct spi_device *spi)
 	return mtd_device_unregister(&flash->mtd);
 }
 
-
 /*
  * XXX This needs to be kept in sync with spi_nor_ids.  We can't share
  * it with spi-nor, because if this is built as a module then modpost
@@ -287,7 +272,7 @@ static const struct spi_device_id m25p_ids[] = {
 	{"s25fl512s"},	{"s70fl01gs"},	{"s25sl12800"},	{"s25sl12801"},
 	{"s25fl129p0"},	{"s25fl129p1"},	{"s25sl004a"},	{"s25sl008a"},
 	{"s25sl016a"},	{"s25sl032a"},	{"s25sl064a"},	{"s25fl008k"},
-	{"s25fl016k"},	{"s25fl064k"},
+	{"s25fl016k"},	{"s25fl064k"},	{"s25fl132k"},
 	{"sst25vf040b"},{"sst25vf080b"},{"sst25vf016b"},{"sst25vf032b"},
 	{"sst25vf064c"},{"sst25wf512"},	{"sst25wf010"},	{"sst25wf020"},
 	{"sst25wf040"},
@@ -300,17 +285,16 @@ static const struct spi_device_id m25p_ids[] = {
 	{"m45pe10"},	{"m45pe80"},	{"m45pe16"},
 	{"m25pe20"},	{"m25pe80"},	{"m25pe16"},
 	{"m25px16"},	{"m25px32"},	{"m25px32-s0"},	{"m25px32-s1"},
-	{"m25px64"},
+	{"m25px64"},	{"m25px80"},
 	{"w25x10"},	{"w25x20"},	{"w25x40"},	{"w25x80"},
 	{"w25x16"},	{"w25x32"},	{"w25q32"},	{"w25q32dw"},
-	{"w25x64"},	{"w25q64"},	{"w25q128"},	{"w25q80"},
-	{"w25q80bl"},	{"w25q128"},	{"w25q256"},	{"cat25c11"},
+	{"w25x64"},	{"w25q64"},	{"w25q80"},	{"w25q80bl"},
+	{"w25q128"},	{"w25q256"},	{"cat25c11"},
 	{"cat25c03"},	{"cat25c09"},	{"cat25c17"},	{"cat25128"},
 	{ },
 };
 MODULE_DEVICE_TABLE(spi, m25p_ids);
 
-
 static struct spi_driver m25p80_driver = {
 	.driver = {
 		.name	= "m25p80",
diff --git a/drivers/mtd/devices/mtd_dataflash.c b/drivers/mtd/devices/mtd_dataflash.c
index dd22ce2cc9ad..0099aba72a8b 100644
--- a/drivers/mtd/devices/mtd_dataflash.c
+++ b/drivers/mtd/devices/mtd_dataflash.c
@@ -149,7 +149,7 @@ static int dataflash_erase(struct mtd_info *mtd, struct erase_info *instr)
 {
 	struct dataflash	*priv = mtd->priv;
 	struct spi_device	*spi = priv->spi;
-	struct spi_transfer	x = { .tx_dma = 0, };
+	struct spi_transfer	x = { };
 	struct spi_message	msg;
 	unsigned		blocksize = priv->page_size << 3;
 	uint8_t			*command;
@@ -235,7 +235,7 @@ static int dataflash_read(struct mtd_info *mtd, loff_t from, size_t len,
 			       size_t *retlen, u_char *buf)
 {
 	struct dataflash	*priv = mtd->priv;
-	struct spi_transfer	x[2] = { { .tx_dma = 0, }, };
+	struct spi_transfer	x[2] = { };
 	struct spi_message	msg;
 	unsigned int		addr;
 	uint8_t			*command;
@@ -301,7 +301,7 @@ static int dataflash_write(struct mtd_info *mtd, loff_t to, size_t len,
 {
 	struct dataflash	*priv = mtd->priv;
 	struct spi_device	*spi = priv->spi;
-	struct spi_transfer	x[2] = { { .tx_dma = 0, }, };
+	struct spi_transfer	x[2] = { };
 	struct spi_message	msg;
 	unsigned int		pageaddr, addr, offset, writelen;
 	size_t			remaining = len;
diff --git a/drivers/mtd/devices/phram.c b/drivers/mtd/devices/phram.c
index effd9a4ef7ee..8b66e52ca3cc 100644
--- a/drivers/mtd/devices/phram.c
+++ b/drivers/mtd/devices/phram.c
@@ -17,7 +17,7 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
-#include <asm/io.h>
+#include <linux/io.h>
 #include <linux/init.h>
 #include <linux/kernel.h>
 #include <linux/list.h>
diff --git a/drivers/mtd/devices/pmc551.c b/drivers/mtd/devices/pmc551.c
index f02603e1bfeb..708b7e8c8b18 100644
--- a/drivers/mtd/devices/pmc551.c
+++ b/drivers/mtd/devices/pmc551.c
@@ -812,8 +812,7 @@ static int __init init_pmc551(void)
 	}
 
 	/* Exited early, reference left over */
-	if (PCI_Device)
-		pci_dev_put(PCI_Device);
+	pci_dev_put(PCI_Device);
 
 	if (!pmc551list) {
 		printk(KERN_NOTICE "pmc551: not detected\n");
diff --git a/drivers/mtd/devices/spear_smi.c b/drivers/mtd/devices/spear_smi.c
index c4176b0f382d..508bab3bd0c4 100644
--- a/drivers/mtd/devices/spear_smi.c
+++ b/drivers/mtd/devices/spear_smi.c
@@ -1079,7 +1079,6 @@ static struct platform_driver spear_smi_driver = {
 	.driver = {
 		.name = "smi",
 		.bus = &platform_bus_type,
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(spear_smi_id_table),
 		.pm = &spear_smi_pm_ops,
 	},
diff --git a/drivers/mtd/devices/st_spi_fsm.c b/drivers/mtd/devices/st_spi_fsm.c
index d252514d3e98..54ffe5223e64 100644
--- a/drivers/mtd/devices/st_spi_fsm.c
+++ b/drivers/mtd/devices/st_spi_fsm.c
@@ -2069,7 +2069,6 @@ static struct platform_driver stfsm_driver = {
 	.remove		= stfsm_remove,
 	.driver		= {
 		.name	= "st-spi-fsm",
-		.owner	= THIS_MODULE,
 		.of_match_table = stfsm_match,
 	},
 };
diff --git a/drivers/mtd/inftlmount.c b/drivers/mtd/inftlmount.c
index 487e64f411a5..1388c8d7f309 100644
--- a/drivers/mtd/inftlmount.c
+++ b/drivers/mtd/inftlmount.c
@@ -518,7 +518,7 @@ void INFTL_dumpVUchains(struct INFTLrecord *s)
 	pr_debug("INFTL Virtual Unit Chains:\n");
 	for (logical = 0; logical < s->nb_blocks; logical++) {
 		block = s->VUtable[logical];
-		if (block > s->nb_blocks)
+		if (block >= s->nb_blocks)
 			continue;
 		pr_debug("  LOGICAL %d --> %d ", logical, block);
 		for (i = 0; i < s->nb_blocks; i++) {
diff --git a/drivers/mtd/maps/bfin-async-flash.c b/drivers/mtd/maps/bfin-async-flash.c
index 6ea51e549045..41730feeace8 100644
--- a/drivers/mtd/maps/bfin-async-flash.c
+++ b/drivers/mtd/maps/bfin-async-flash.c
@@ -126,7 +126,6 @@ static const char * const part_probe_types[] = {
 
 static int bfin_flash_probe(struct platform_device *pdev)
 {
-	int ret;
 	struct physmap_flash_data *pdata = dev_get_platdata(&pdev->dev);
 	struct resource *memory = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	struct resource *flash_ambctl = platform_get_resource(pdev, IORESOURCE_MEM, 1);
diff --git a/drivers/mtd/maps/ixp4xx.c b/drivers/mtd/maps/ixp4xx.c
index 6a589f1e2880..b4430741024e 100644
--- a/drivers/mtd/maps/ixp4xx.c
+++ b/drivers/mtd/maps/ixp4xx.c
@@ -250,7 +250,6 @@ static struct platform_driver ixp4xx_flash_driver = {
 	.remove		= ixp4xx_flash_remove,
 	.driver		= {
 		.name	= "IXP4XX-Flash",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mtd/maps/lantiq-flash.c b/drivers/mtd/maps/lantiq-flash.c
index 7aa682cd4d7e..33d26f5bee54 100644
--- a/drivers/mtd/maps/lantiq-flash.c
+++ b/drivers/mtd/maps/lantiq-flash.c
@@ -205,7 +205,6 @@ static struct platform_driver ltq_mtd_driver = {
 	.remove = ltq_mtd_remove,
 	.driver = {
 		.name = "ltq-nor",
-		.owner = THIS_MODULE,
 		.of_match_table = ltq_mtd_match,
 	},
 };
diff --git a/drivers/mtd/maps/physmap.c b/drivers/mtd/maps/physmap.c
index f73cd461257c..4305fd607015 100644
--- a/drivers/mtd/maps/physmap.c
+++ b/drivers/mtd/maps/physmap.c
@@ -217,7 +217,6 @@ static struct platform_driver physmap_flash_driver = {
 	.shutdown	= physmap_flash_shutdown,
 	.driver		= {
 		.name	= "physmap-flash",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mtd/maps/physmap_of.c b/drivers/mtd/maps/physmap_of.c
index c1d21cb501ca..f35cd2081314 100644
--- a/drivers/mtd/maps/physmap_of.c
+++ b/drivers/mtd/maps/physmap_of.c
@@ -47,14 +47,12 @@ static int of_flash_remove(struct platform_device *dev)
 		return 0;
 	dev_set_drvdata(&dev->dev, NULL);
 
-	if (info->cmtd != info->list[0].mtd) {
+	if (info->cmtd) {
 		mtd_device_unregister(info->cmtd);
-		mtd_concat_destroy(info->cmtd);
+		if (info->cmtd != info->list[0].mtd)
+			mtd_concat_destroy(info->cmtd);
 	}
 
-	if (info->cmtd)
-		mtd_device_unregister(info->cmtd);
-
 	for (i = 0; i < info->list_size; i++) {
 		if (info->list[i].mtd)
 			map_destroy(info->list[i].mtd);
@@ -354,7 +352,6 @@ MODULE_DEVICE_TABLE(of, of_flash_match);
 static struct platform_driver of_flash_driver = {
 	.driver = {
 		.name = "of-flash",
-		.owner = THIS_MODULE,
 		.of_match_table = of_flash_match,
 	},
 	.probe		= of_flash_probe,
diff --git a/drivers/mtd/maps/plat-ram.c b/drivers/mtd/maps/plat-ram.c
index d597e89f2692..4b65c08d15f6 100644
--- a/drivers/mtd/maps/plat-ram.c
+++ b/drivers/mtd/maps/plat-ram.c
@@ -251,7 +251,6 @@ static struct platform_driver platram_driver = {
 	.remove		= platram_remove,
 	.driver		= {
 		.name	= "mtd-ram",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mtd/maps/pxa2xx-flash.c b/drivers/mtd/maps/pxa2xx-flash.c
index cb4d92eea9fe..12fa75df5008 100644
--- a/drivers/mtd/maps/pxa2xx-flash.c
+++ b/drivers/mtd/maps/pxa2xx-flash.c
@@ -131,7 +131,6 @@ static void pxa2xx_flash_shutdown(struct platform_device *dev)
 static struct platform_driver pxa2xx_flash_driver = {
 	.driver = {
 		.name		= "pxa2xx-flash",
-		.owner		= THIS_MODULE,
 	},
 	.probe		= pxa2xx_flash_probe,
 	.remove		= pxa2xx_flash_remove,
diff --git a/drivers/mtd/maps/rbtx4939-flash.c b/drivers/mtd/maps/rbtx4939-flash.c
index a84fdfb10518..5a7551aa2d89 100644
--- a/drivers/mtd/maps/rbtx4939-flash.c
+++ b/drivers/mtd/maps/rbtx4939-flash.c
@@ -127,7 +127,6 @@ static struct platform_driver rbtx4939_flash_driver = {
 	.shutdown	= rbtx4939_flash_shutdown,
 	.driver		= {
 		.name	= "rbtx4939-flash",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mtd/maps/sa1100-flash.c b/drivers/mtd/maps/sa1100-flash.c
index 8fc06bf111c4..ea697202935a 100644
--- a/drivers/mtd/maps/sa1100-flash.c
+++ b/drivers/mtd/maps/sa1100-flash.c
@@ -289,7 +289,6 @@ static struct platform_driver sa1100_mtd_driver = {
 	.remove		= __exit_p(sa1100_mtd_remove),
 	.driver		= {
 		.name	= "sa1100-mtd",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mtd/maps/sun_uflash.c b/drivers/mtd/maps/sun_uflash.c
index b6f1aac3510c..d459aca07881 100644
--- a/drivers/mtd/maps/sun_uflash.c
+++ b/drivers/mtd/maps/sun_uflash.c
@@ -150,7 +150,6 @@ MODULE_DEVICE_TABLE(of, uflash_match);
 static struct platform_driver uflash_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = uflash_match,
 	},
 	.probe		= uflash_probe,
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index dd10646982ae..7d0150d20432 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -75,10 +75,12 @@ config MTD_NAND_DENALI_SCRATCH_REG_ADDR
           boards, the scratch register is at 0xFF108018.
 
 config MTD_NAND_GPIO
-	tristate "GPIO NAND Flash driver"
+	tristate "GPIO assisted NAND Flash driver"
 	depends on GPIOLIB
 	help
-	  This enables a GPIO based NAND flash driver.
+	  This enables a NAND flash driver where control signals are
+	  connected to GPIO pins, and commands and data are communicated
+	  via a memory mapped interface.
 
 config MTD_NAND_AMS_DELTA
 	tristate "NAND Flash device on Amstrad E3"
@@ -516,4 +518,10 @@ config MTD_NAND_XWAY
 	  Enables support for NAND Flash chips on Lantiq XWAY SoCs. NAND is attached
 	  to the External Bus Unit (EBU).
 
+config MTD_NAND_SUNXI
+	tristate "Support for NAND on Allwinner SoCs"
+	depends on ARCH_SUNXI
+	help
+	  Enables support for NAND Flash chips on Allwinner SoCs.
+
 endif # MTD_NAND
diff --git a/drivers/mtd/nand/Makefile b/drivers/mtd/nand/Makefile
index 9c847e469ca7..bd38f21d2e28 100644
--- a/drivers/mtd/nand/Makefile
+++ b/drivers/mtd/nand/Makefile
@@ -50,5 +50,6 @@ obj-$(CONFIG_MTD_NAND_JZ4740)		+= jz4740_nand.o
 obj-$(CONFIG_MTD_NAND_GPMI_NAND)	+= gpmi-nand/
 obj-$(CONFIG_MTD_NAND_XWAY)		+= xway_nand.o
 obj-$(CONFIG_MTD_NAND_BCM47XXNFLASH)	+= bcm47xxnflash/
+obj-$(CONFIG_MTD_NAND_SUNXI)		+= sunxi_nand.o
 
 nand-objs := nand_base.o nand_bbt.o nand_timings.o
diff --git a/drivers/mtd/nand/ams-delta.c b/drivers/mtd/nand/ams-delta.c
index 4936e9e0002f..f1d555cfb332 100644
--- a/drivers/mtd/nand/ams-delta.c
+++ b/drivers/mtd/nand/ams-delta.c
@@ -290,7 +290,6 @@ static struct platform_driver ams_delta_nand_driver = {
 	.remove		= ams_delta_cleanup,
 	.driver		= {
 		.name	= "ams-delta-nand",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mtd/nand/atmel_nand.c b/drivers/mtd/nand/atmel_nand.c
index 19d1e9d17bf9..a345e7b2463a 100644
--- a/drivers/mtd/nand/atmel_nand.c
+++ b/drivers/mtd/nand/atmel_nand.c
@@ -92,7 +92,7 @@ static struct nand_ecclayout atmel_oobinfo_small = {
 struct atmel_nfc {
 	void __iomem		*base_cmd_regs;
 	void __iomem		*hsmc_regs;
-	void __iomem		*sram_bank0;
+	void			*sram_bank0;
 	dma_addr_t		sram_bank0_phys;
 	bool			use_nfc_sram;
 	bool			write_by_sram;
@@ -105,7 +105,7 @@ struct atmel_nfc {
 	struct completion	comp_xfer_done;
 
 	/* Point to the sram bank which include readed data via NFC */
-	void __iomem		*data_in_sram;
+	void			*data_in_sram;
 	bool			will_write_sram;
 };
 static struct atmel_nfc	nand_nfc;
@@ -127,6 +127,7 @@ struct atmel_nand_host {
 	bool			has_pmecc;
 	u8			pmecc_corr_cap;
 	u16			pmecc_sector_size;
+	bool			has_no_lookup_table;
 	u32			pmecc_lookup_table_offset;
 	u32			pmecc_lookup_table_offset_512;
 	u32			pmecc_lookup_table_offset_1024;
@@ -256,26 +257,6 @@ static int atmel_nand_set_enable_ready_pins(struct mtd_info *mtd)
 	return res;
 }
 
-static void memcpy32_fromio(void *trg, const void __iomem  *src, size_t size)
-{
-	int i;
-	u32 *t = trg;
-	const __iomem u32 *s = src;
-
-	for (i = 0; i < (size >> 2); i++)
-		*t++ = readl_relaxed(s++);
-}
-
-static void memcpy32_toio(void __iomem *trg, const void *src, int size)
-{
-	int i;
-	u32 __iomem *t = trg;
-	const u32 *s = src;
-
-	for (i = 0; i < (size >> 2); i++)
-		writel_relaxed(*s++, t++);
-}
-
 /*
  * Minimal-overhead PIO for data access.
  */
@@ -285,7 +266,7 @@ static void atmel_read_buf8(struct mtd_info *mtd, u8 *buf, int len)
 	struct atmel_nand_host *host = nand_chip->priv;
 
 	if (host->nfc && host->nfc->use_nfc_sram && host->nfc->data_in_sram) {
-		memcpy32_fromio(buf, host->nfc->data_in_sram, len);
+		memcpy(buf, host->nfc->data_in_sram, len);
 		host->nfc->data_in_sram += len;
 	} else {
 		__raw_readsb(nand_chip->IO_ADDR_R, buf, len);
@@ -298,7 +279,7 @@ static void atmel_read_buf16(struct mtd_info *mtd, u8 *buf, int len)
 	struct atmel_nand_host *host = nand_chip->priv;
 
 	if (host->nfc && host->nfc->use_nfc_sram && host->nfc->data_in_sram) {
-		memcpy32_fromio(buf, host->nfc->data_in_sram, len);
+		memcpy(buf, host->nfc->data_in_sram, len);
 		host->nfc->data_in_sram += len;
 	} else {
 		__raw_readsw(nand_chip->IO_ADDR_R, buf, len / 2);
@@ -1112,12 +1093,66 @@ static int pmecc_choose_ecc(struct atmel_nand_host *host,
 	return 0;
 }
 
+static inline int deg(unsigned int poly)
+{
+	/* polynomial degree is the most-significant bit index */
+	return fls(poly) - 1;
+}
+
+static int build_gf_tables(int mm, unsigned int poly,
+		int16_t *index_of, int16_t *alpha_to)
+{
+	unsigned int i, x = 1;
+	const unsigned int k = 1 << deg(poly);
+	unsigned int nn = (1 << mm) - 1;
+
+	/* primitive polynomial must be of degree m */
+	if (k != (1u << mm))
+		return -EINVAL;
+
+	for (i = 0; i < nn; i++) {
+		alpha_to[i] = x;
+		index_of[x] = i;
+		if (i && (x == 1))
+			/* polynomial is not primitive (a^i=1 with 0<i<2^m-1) */
+			return -EINVAL;
+		x <<= 1;
+		if (x & k)
+			x ^= poly;
+	}
+	alpha_to[nn] = 1;
+	index_of[0] = 0;
+
+	return 0;
+}
+
+static uint16_t *create_lookup_table(struct device *dev, int sector_size)
+{
+	int degree = (sector_size == 512) ?
+			PMECC_GF_DIMENSION_13 :
+			PMECC_GF_DIMENSION_14;
+	unsigned int poly = (sector_size == 512) ?
+			PMECC_GF_13_PRIMITIVE_POLY :
+			PMECC_GF_14_PRIMITIVE_POLY;
+	int table_size = (sector_size == 512) ?
+			PMECC_LOOKUP_TABLE_SIZE_512 :
+			PMECC_LOOKUP_TABLE_SIZE_1024;
+
+	int16_t *addr = devm_kzalloc(dev, 2 * table_size * sizeof(uint16_t),
+			GFP_KERNEL);
+	if (addr && build_gf_tables(degree, poly, addr, addr + table_size))
+		return NULL;
+
+	return addr;
+}
+
 static int atmel_pmecc_nand_init_params(struct platform_device *pdev,
 					 struct atmel_nand_host *host)
 {
 	struct mtd_info *mtd = &host->mtd;
 	struct nand_chip *nand_chip = &host->nand_chip;
 	struct resource *regs, *regs_pmerr, *regs_rom;
+	uint16_t *galois_table;
 	int cap, sector_size, err_no;
 
 	err_no = pmecc_choose_ecc(host, &cap, &sector_size);
@@ -1163,8 +1198,24 @@ static int atmel_pmecc_nand_init_params(struct platform_device *pdev,
 	regs_rom = platform_get_resource(pdev, IORESOURCE_MEM, 3);
 	host->pmecc_rom_base = devm_ioremap_resource(&pdev->dev, regs_rom);
 	if (IS_ERR(host->pmecc_rom_base)) {
-		err_no = PTR_ERR(host->pmecc_rom_base);
-		goto err;
+		if (!host->has_no_lookup_table)
+			/* Don't display the information again */
+			dev_err(host->dev, "Can not get I/O resource for ROM, will build a lookup table in runtime!\n");
+
+		host->has_no_lookup_table = true;
+	}
+
+	if (host->has_no_lookup_table) {
+		/* Build the look-up table in runtime */
+		galois_table = create_lookup_table(host->dev, sector_size);
+		if (!galois_table) {
+			dev_err(host->dev, "Failed to build a lookup table in runtime!\n");
+			err_no = -EINVAL;
+			goto err;
+		}
+
+		host->pmecc_rom_base = (void __iomem *)galois_table;
+		host->pmecc_lookup_table_offset = 0;
 	}
 
 	nand_chip->ecc.size = sector_size;
@@ -1501,8 +1552,10 @@ static int atmel_of_init_port(struct atmel_nand_host *host,
 
 	if (of_property_read_u32_array(np, "atmel,pmecc-lookup-table-offset",
 			offset, 2) != 0) {
-		dev_err(host->dev, "Cannot get PMECC lookup table offset\n");
-		return -EINVAL;
+		dev_err(host->dev, "Cannot get PMECC lookup table offset, will build a lookup table in runtime.\n");
+		host->has_no_lookup_table = true;
+		/* Will build a lookup table and initialize the offset later */
+		return 0;
 	}
 	if (!offset[0] && !offset[1]) {
 		dev_err(host->dev, "Invalid PMECC lookup table offset\n");
@@ -1899,7 +1952,7 @@ static int nfc_sram_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 	int cfg, len;
 	int status = 0;
 	struct atmel_nand_host *host = chip->priv;
-	void __iomem *sram = host->nfc->sram_bank0 + nfc_get_sram_off(host);
+	void *sram = host->nfc->sram_bank0 + nfc_get_sram_off(host);
 
 	/* Subpage write is not supported */
 	if (offset || (data_len < mtd->writesize))
@@ -1910,14 +1963,14 @@ static int nfc_sram_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 	if (use_dma) {
 		if (atmel_nand_dma_op(mtd, (void *)buf, len, 0) != 0)
 			/* Fall back to use cpu copy */
-			memcpy32_toio(sram, buf, len);
+			memcpy(sram, buf, len);
 	} else {
-		memcpy32_toio(sram, buf, len);
+		memcpy(sram, buf, len);
 	}
 
 	cfg = nfc_readl(host->nfc->hsmc_regs, CFG);
 	if (unlikely(raw) && oob_required) {
-		memcpy32_toio(sram + len, chip->oob_poi, mtd->oobsize);
+		memcpy(sram + len, chip->oob_poi, mtd->oobsize);
 		len += mtd->oobsize;
 		nfc_writel(host->nfc->hsmc_regs, CFG, cfg | NFC_CFG_WSPARE);
 	} else {
@@ -2260,7 +2313,8 @@ static int atmel_nand_nfc_probe(struct platform_device *pdev)
 
 	nfc_sram = platform_get_resource(pdev, IORESOURCE_MEM, 2);
 	if (nfc_sram) {
-		nfc->sram_bank0 = devm_ioremap_resource(&pdev->dev, nfc_sram);
+		nfc->sram_bank0 = (void * __force)
+				devm_ioremap_resource(&pdev->dev, nfc_sram);
 		if (IS_ERR(nfc->sram_bank0)) {
 			dev_warn(&pdev->dev, "Fail to ioremap the NFC sram with error: %ld. So disable NFC sram.\n",
 					PTR_ERR(nfc->sram_bank0));
@@ -2312,7 +2366,6 @@ MODULE_DEVICE_TABLE(of, atmel_nand_nfc_match);
 static struct platform_driver atmel_nand_nfc_driver = {
 	.driver = {
 		.name = "atmel_nand_nfc",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(atmel_nand_nfc_match),
 	},
 	.probe = atmel_nand_nfc_probe,
@@ -2324,7 +2377,6 @@ static struct platform_driver atmel_nand_driver = {
 	.remove		= atmel_nand_remove,
 	.driver		= {
 		.name	= "atmel_nand",
-		.owner	= THIS_MODULE,
 		.of_match_table	= of_match_ptr(atmel_nand_dt_ids),
 	},
 };
diff --git a/drivers/mtd/nand/atmel_nand_ecc.h b/drivers/mtd/nand/atmel_nand_ecc.h
index 8a1e9a686759..d4035e335ad8 100644
--- a/drivers/mtd/nand/atmel_nand_ecc.h
+++ b/drivers/mtd/nand/atmel_nand_ecc.h
@@ -142,6 +142,10 @@
 #define PMECC_GF_DIMENSION_13			13
 #define PMECC_GF_DIMENSION_14			14
 
+/* Primitive Polynomial used by PMECC */
+#define PMECC_GF_13_PRIMITIVE_POLY		0x201b
+#define PMECC_GF_14_PRIMITIVE_POLY		0x4443
+
 #define PMECC_LOOKUP_TABLE_SIZE_512		0x2000
 #define PMECC_LOOKUP_TABLE_SIZE_1024		0x4000
 
diff --git a/drivers/mtd/nand/au1550nd.c b/drivers/mtd/nand/au1550nd.c
index 77d6c17b38c2..c0c3be180012 100644
--- a/drivers/mtd/nand/au1550nd.c
+++ b/drivers/mtd/nand/au1550nd.c
@@ -503,7 +503,6 @@ static int au1550nd_remove(struct platform_device *pdev)
 static struct platform_driver au1550nd_driver = {
 	.driver = {
 		.name	= "au1550-nand",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= au1550nd_probe,
 	.remove		= au1550nd_remove,
diff --git a/drivers/mtd/nand/bcm47xxnflash/main.c b/drivers/mtd/nand/bcm47xxnflash/main.c
index 107445911315..461577cfb5bc 100644
--- a/drivers/mtd/nand/bcm47xxnflash/main.c
+++ b/drivers/mtd/nand/bcm47xxnflash/main.c
@@ -73,7 +73,6 @@ static struct platform_driver bcm47xxnflash_driver = {
 	.remove = bcm47xxnflash_remove,
 	.driver = {
 		.name = "bcma_nflash",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mtd/nand/bf5xx_nand.c b/drivers/mtd/nand/bf5xx_nand.c
index 871c4f712654..4d8d4ba4b9c1 100644
--- a/drivers/mtd/nand/bf5xx_nand.c
+++ b/drivers/mtd/nand/bf5xx_nand.c
@@ -836,7 +836,6 @@ static struct platform_driver bf5xx_nand_driver = {
 	.remove		= bf5xx_nand_remove,
 	.driver		= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mtd/nand/cafe_nand.c b/drivers/mtd/nand/cafe_nand.c
index 4e66726da9aa..9a0f45f1d932 100644
--- a/drivers/mtd/nand/cafe_nand.c
+++ b/drivers/mtd/nand/cafe_nand.c
@@ -529,50 +529,6 @@ static int cafe_nand_write_page_lowlevel(struct mtd_info *mtd,
 	return 0;
 }
 
-static int cafe_nand_write_page(struct mtd_info *mtd, struct nand_chip *chip,
-			uint32_t offset, int data_len, const uint8_t *buf,
-			int oob_required, int page, int cached, int raw)
-{
-	int status;
-
-	chip->cmdfunc(mtd, NAND_CMD_SEQIN, 0x00, page);
-
-	if (unlikely(raw))
-		status = chip->ecc.write_page_raw(mtd, chip, buf, oob_required);
-	else
-		status = chip->ecc.write_page(mtd, chip, buf, oob_required);
-
-	if (status < 0)
-		return status;
-
-	/*
-	 * Cached progamming disabled for now, Not sure if its worth the
-	 * trouble. The speed gain is not very impressive. (2.3->2.6Mib/s)
-	 */
-	cached = 0;
-
-	if (!cached || !(chip->options & NAND_CACHEPRG)) {
-
-		chip->cmdfunc(mtd, NAND_CMD_PAGEPROG, -1, -1);
-		status = chip->waitfunc(mtd, chip);
-		/*
-		 * See if operation failed and additional status checks are
-		 * available
-		 */
-		if ((status & NAND_STATUS_FAIL) && (chip->errstat))
-			status = chip->errstat(mtd, chip, FL_WRITING, status,
-					       page);
-
-		if (status & NAND_STATUS_FAIL)
-			return -EIO;
-	} else {
-		chip->cmdfunc(mtd, NAND_CMD_CACHEDPROG, -1, -1);
-		status = chip->waitfunc(mtd, chip);
-	}
-
-	return 0;
-}
-
 static int cafe_nand_block_bad(struct mtd_info *mtd, loff_t ofs, int getchip)
 {
 	return 0;
@@ -800,7 +756,6 @@ static int cafe_nand_probe(struct pci_dev *pdev,
 	cafe->nand.ecc.hwctl  = (void *)cafe_nand_bug;
 	cafe->nand.ecc.calculate = (void *)cafe_nand_bug;
 	cafe->nand.ecc.correct  = (void *)cafe_nand_bug;
-	cafe->nand.write_page = cafe_nand_write_page;
 	cafe->nand.ecc.write_page = cafe_nand_write_page_lowlevel;
 	cafe->nand.ecc.write_oob = cafe_nand_write_oob;
 	cafe->nand.ecc.read_page = cafe_nand_read_page;
diff --git a/drivers/mtd/nand/davinci_nand.c b/drivers/mtd/nand/davinci_nand.c
index b922c8efcf40..feb6d18de78d 100644
--- a/drivers/mtd/nand/davinci_nand.c
+++ b/drivers/mtd/nand/davinci_nand.c
@@ -870,7 +870,6 @@ static struct platform_driver nand_davinci_driver = {
 	.remove		= nand_davinci_remove,
 	.driver		= {
 		.name	= "davinci_nand",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(davinci_nand_of_match),
 	},
 };
diff --git a/drivers/mtd/nand/denali_dt.c b/drivers/mtd/nand/denali_dt.c
index 35cb17f57800..0cb1e8d9fbfc 100644
--- a/drivers/mtd/nand/denali_dt.c
+++ b/drivers/mtd/nand/denali_dt.c
@@ -120,7 +120,6 @@ static struct platform_driver denali_dt_driver = {
 	.remove		= denali_dt_remove,
 	.driver		= {
 		.name	= "denali-nand-dt",
-		.owner	= THIS_MODULE,
 		.of_match_table	= denali_nand_dt_ids,
 	},
 };
diff --git a/drivers/mtd/nand/docg4.c b/drivers/mtd/nand/docg4.c
index ce24637e14f1..e5d7bcaafa7d 100644
--- a/drivers/mtd/nand/docg4.c
+++ b/drivers/mtd/nand/docg4.c
@@ -1380,7 +1380,6 @@ static int __exit cleanup_docg4(struct platform_device *pdev)
 static struct platform_driver docg4_driver = {
 	.driver		= {
 		.name	= "docg4",
-		.owner	= THIS_MODULE,
 	},
 	.suspend	= docg4_suspend,
 	.resume		= docg4_resume,
diff --git a/drivers/mtd/nand/fsl_elbc_nand.c b/drivers/mtd/nand/fsl_elbc_nand.c
index 545a5c002f09..04b22fd3732d 100644
--- a/drivers/mtd/nand/fsl_elbc_nand.c
+++ b/drivers/mtd/nand/fsl_elbc_nand.c
@@ -950,7 +950,6 @@ static const struct of_device_id fsl_elbc_nand_match[] = {
 static struct platform_driver fsl_elbc_nand_driver = {
 	.driver = {
 		.name = "fsl,elbc-fcm-nand",
-		.owner = THIS_MODULE,
 		.of_match_table = fsl_elbc_nand_match,
 	},
 	.probe = fsl_elbc_nand_probe,
diff --git a/drivers/mtd/nand/fsl_ifc_nand.c b/drivers/mtd/nand/fsl_ifc_nand.c
index 2338124dd05f..4c05f4f6a5c6 100644
--- a/drivers/mtd/nand/fsl_ifc_nand.c
+++ b/drivers/mtd/nand/fsl_ifc_nand.c
@@ -31,7 +31,6 @@
 #include <linux/mtd/nand_ecc.h>
 #include <linux/fsl_ifc.h>
 
-#define FSL_IFC_V1_1_0	0x01010000
 #define ERR_BYTE		0xFF /* Value returned for read
 					bytes when read failed	*/
 #define IFC_TIMEOUT_MSECS	500  /* Maximum number of mSecs to wait
@@ -877,7 +876,7 @@ static int fsl_ifc_chip_init(struct fsl_ifc_mtd *priv)
 	struct fsl_ifc_regs __iomem *ifc = ctrl->regs;
 	struct nand_chip *chip = &priv->chip;
 	struct nand_ecclayout *layout;
-	u32 csor, ver;
+	u32 csor;
 
 	/* Fill in fsl_ifc_mtd structure */
 	priv->mtd.priv = chip;
@@ -984,8 +983,7 @@ static int fsl_ifc_chip_init(struct fsl_ifc_mtd *priv)
 		chip->ecc.mode = NAND_ECC_SOFT;
 	}
 
-	ver = ioread32be(&ifc->ifc_rev);
-	if (ver == FSL_IFC_V1_1_0)
+	if (ctrl->version == FSL_IFC_VERSION_1_1_0)
 		fsl_ifc_sram_init(priv);
 
 	return 0;
@@ -1045,12 +1043,12 @@ static int fsl_ifc_nand_probe(struct platform_device *dev)
 	}
 
 	/* find which chip select it is connected to */
-	for (bank = 0; bank < FSL_IFC_BANK_COUNT; bank++) {
+	for (bank = 0; bank < fsl_ifc_ctrl_dev->banks; bank++) {
 		if (match_bank(ifc, bank, res.start))
 			break;
 	}
 
-	if (bank >= FSL_IFC_BANK_COUNT) {
+	if (bank >= fsl_ifc_ctrl_dev->banks) {
 		dev_err(&dev->dev, "%s: address did not match any chip selects\n",
 			__func__);
 		return -ENODEV;
@@ -1167,7 +1165,6 @@ static const struct of_device_id fsl_ifc_nand_match[] = {
 static struct platform_driver fsl_ifc_nand_driver = {
 	.driver = {
 		.name	= "fsl,ifc-nand",
-		.owner = THIS_MODULE,
 		.of_match_table = fsl_ifc_nand_match,
 	},
 	.probe       = fsl_ifc_nand_probe,
diff --git a/drivers/mtd/nand/fsl_upm.c b/drivers/mtd/nand/fsl_upm.c
index 4d203e84e8ca..72755d7ec25d 100644
--- a/drivers/mtd/nand/fsl_upm.c
+++ b/drivers/mtd/nand/fsl_upm.c
@@ -347,7 +347,6 @@ MODULE_DEVICE_TABLE(of, of_fun_match);
 static struct platform_driver of_fun_driver = {
 	.driver = {
 		.name = "fsl,upm-nand",
-		.owner = THIS_MODULE,
 		.of_match_table = of_fun_match,
 	},
 	.probe		= fun_probe,
diff --git a/drivers/mtd/nand/fsmc_nand.c b/drivers/mtd/nand/fsmc_nand.c
index 7a915870d9d6..edfaa21b1817 100644
--- a/drivers/mtd/nand/fsmc_nand.c
+++ b/drivers/mtd/nand/fsmc_nand.c
@@ -1224,7 +1224,6 @@ MODULE_DEVICE_TABLE(of, fsmc_nand_id_table);
 static struct platform_driver fsmc_nand_driver = {
 	.remove = fsmc_nand_remove,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "fsmc-nand",
 		.of_match_table = of_match_ptr(fsmc_nand_id_table),
 		.pm = &fsmc_nand_pm_ops,
diff --git a/drivers/mtd/nand/gpio.c b/drivers/mtd/nand/gpio.c
index 117ce333fdd4..73c4048c3a56 100644
--- a/drivers/mtd/nand/gpio.c
+++ b/drivers/mtd/nand/gpio.c
@@ -8,7 +8,9 @@
  *
  * © 2004 Simtec Electronics
  *
- * Device driver for NAND connected via GPIO
+ * Device driver for NAND flash that uses a memory mapped interface to
+ * read/write the NAND commands and data, and GPIO pins for control signals
+ * (the DT binding refers to this as "GPIO assisted NAND flash")
  *
  * 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
@@ -308,7 +310,6 @@ static struct platform_driver gpio_nand_driver = {
 	.remove		= gpio_nand_remove,
 	.driver		= {
 		.name	= "gpio-nand",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(gpio_nand_id_table),
 	},
 };
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-lib.c b/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
index 87e658ce23ef..27f272ed502a 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
@@ -1353,3 +1353,156 @@ int gpmi_read_page(struct gpmi_nand_data *this,
 	set_dma_type(this, DMA_FOR_READ_ECC_PAGE);
 	return start_dma_with_bch_irq(this, desc);
 }
+
+/**
+ * gpmi_copy_bits - copy bits from one memory region to another
+ * @dst: destination buffer
+ * @dst_bit_off: bit offset we're starting to write at
+ * @src: source buffer
+ * @src_bit_off: bit offset we're starting to read from
+ * @nbits: number of bits to copy
+ *
+ * This functions copies bits from one memory region to another, and is used by
+ * the GPMI driver to copy ECC sections which are not guaranteed to be byte
+ * aligned.
+ *
+ * src and dst should not overlap.
+ *
+ */
+void gpmi_copy_bits(u8 *dst, size_t dst_bit_off,
+		    const u8 *src, size_t src_bit_off,
+		    size_t nbits)
+{
+	size_t i;
+	size_t nbytes;
+	u32 src_buffer = 0;
+	size_t bits_in_src_buffer = 0;
+
+	if (!nbits)
+		return;
+
+	/*
+	 * Move src and dst pointers to the closest byte pointer and store bit
+	 * offsets within a byte.
+	 */
+	src += src_bit_off / 8;
+	src_bit_off %= 8;
+
+	dst += dst_bit_off / 8;
+	dst_bit_off %= 8;
+
+	/*
+	 * Initialize the src_buffer value with bits available in the first
+	 * byte of data so that we end up with a byte aligned src pointer.
+	 */
+	if (src_bit_off) {
+		src_buffer = src[0] >> src_bit_off;
+		if (nbits >= (8 - src_bit_off)) {
+			bits_in_src_buffer += 8 - src_bit_off;
+		} else {
+			src_buffer &= GENMASK(nbits - 1, 0);
+			bits_in_src_buffer += nbits;
+		}
+		nbits -= bits_in_src_buffer;
+		src++;
+	}
+
+	/* Calculate the number of bytes that can be copied from src to dst. */
+	nbytes = nbits / 8;
+
+	/* Try to align dst to a byte boundary. */
+	if (dst_bit_off) {
+		if (bits_in_src_buffer < (8 - dst_bit_off) && nbytes) {
+			src_buffer |= src[0] << bits_in_src_buffer;
+			bits_in_src_buffer += 8;
+			src++;
+			nbytes--;
+		}
+
+		if (bits_in_src_buffer >= (8 - dst_bit_off)) {
+			dst[0] &= GENMASK(dst_bit_off - 1, 0);
+			dst[0] |= src_buffer << dst_bit_off;
+			src_buffer >>= (8 - dst_bit_off);
+			bits_in_src_buffer -= (8 - dst_bit_off);
+			dst_bit_off = 0;
+			dst++;
+			if (bits_in_src_buffer > 7) {
+				bits_in_src_buffer -= 8;
+				dst[0] = src_buffer;
+				dst++;
+				src_buffer >>= 8;
+			}
+		}
+	}
+
+	if (!bits_in_src_buffer && !dst_bit_off) {
+		/*
+		 * Both src and dst pointers are byte aligned, thus we can
+		 * just use the optimized memcpy function.
+		 */
+		if (nbytes)
+			memcpy(dst, src, nbytes);
+	} else {
+		/*
+		 * src buffer is not byte aligned, hence we have to copy each
+		 * src byte to the src_buffer variable before extracting a byte
+		 * to store in dst.
+		 */
+		for (i = 0; i < nbytes; i++) {
+			src_buffer |= src[i] << bits_in_src_buffer;
+			dst[i] = src_buffer;
+			src_buffer >>= 8;
+		}
+	}
+	/* Update dst and src pointers */
+	dst += nbytes;
+	src += nbytes;
+
+	/*
+	 * nbits is the number of remaining bits. It should not exceed 8 as
+	 * we've already copied as much bytes as possible.
+	 */
+	nbits %= 8;
+
+	/*
+	 * If there's no more bits to copy to the destination and src buffer
+	 * was already byte aligned, then we're done.
+	 */
+	if (!nbits && !bits_in_src_buffer)
+		return;
+
+	/* Copy the remaining bits to src_buffer */
+	if (nbits)
+		src_buffer |= (*src & GENMASK(nbits - 1, 0)) <<
+			      bits_in_src_buffer;
+	bits_in_src_buffer += nbits;
+
+	/*
+	 * In case there were not enough bits to get a byte aligned dst buffer
+	 * prepare the src_buffer variable to match the dst organization (shift
+	 * src_buffer by dst_bit_off and retrieve the least significant bits
+	 * from dst).
+	 */
+	if (dst_bit_off)
+		src_buffer = (src_buffer << dst_bit_off) |
+			     (*dst & GENMASK(dst_bit_off - 1, 0));
+	bits_in_src_buffer += dst_bit_off;
+
+	/*
+	 * Keep most significant bits from dst if we end up with an unaligned
+	 * number of bits.
+	 */
+	nbytes = bits_in_src_buffer / 8;
+	if (bits_in_src_buffer % 8) {
+		src_buffer |= (dst[nbytes] &
+			       GENMASK(7, bits_in_src_buffer % 8)) <<
+			      (nbytes * 8);
+		nbytes++;
+	}
+
+	/* Copy the remaining bytes to dst */
+	for (i = 0; i < nbytes; i++) {
+		dst[i] = src_buffer;
+		src_buffer >>= 8;
+	}
+}
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-nand.c b/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
index 959cb9b70310..4f3851a24bb2 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
@@ -791,6 +791,7 @@ static void gpmi_free_dma_buffer(struct gpmi_nand_data *this)
 					this->page_buffer_phys);
 	kfree(this->cmd_buffer);
 	kfree(this->data_buffer_dma);
+	kfree(this->raw_buffer);
 
 	this->cmd_buffer	= NULL;
 	this->data_buffer_dma	= NULL;
@@ -837,6 +838,9 @@ static int gpmi_alloc_dma_buffer(struct gpmi_nand_data *this)
 	if (!this->page_buffer_virt)
 		goto error_alloc;
 
+	this->raw_buffer = kzalloc(mtd->writesize + mtd->oobsize, GFP_KERNEL);
+	if (!this->raw_buffer)
+		goto error_alloc;
 
 	/* Slice up the page buffer. */
 	this->payload_virt = this->page_buffer_virt;
@@ -1347,6 +1351,199 @@ gpmi_ecc_write_oob(struct mtd_info *mtd, struct nand_chip *chip, int page)
 	return status & NAND_STATUS_FAIL ? -EIO : 0;
 }
 
+/*
+ * This function reads a NAND page without involving the ECC engine (no HW
+ * ECC correction).
+ * The tricky part in the GPMI/BCH controller is that it stores ECC bits
+ * inline (interleaved with payload DATA), and do not align data chunk on
+ * byte boundaries.
+ * We thus need to take care moving the payload data and ECC bits stored in the
+ * page into the provided buffers, which is why we're using gpmi_copy_bits.
+ *
+ * See set_geometry_by_ecc_info inline comments to have a full description
+ * of the layout used by the GPMI controller.
+ */
+static int gpmi_ecc_read_page_raw(struct mtd_info *mtd,
+				  struct nand_chip *chip, uint8_t *buf,
+				  int oob_required, int page)
+{
+	struct gpmi_nand_data *this = chip->priv;
+	struct bch_geometry *nfc_geo = &this->bch_geometry;
+	int eccsize = nfc_geo->ecc_chunk_size;
+	int eccbits = nfc_geo->ecc_strength * nfc_geo->gf_len;
+	u8 *tmp_buf = this->raw_buffer;
+	size_t src_bit_off;
+	size_t oob_bit_off;
+	size_t oob_byte_off;
+	uint8_t *oob = chip->oob_poi;
+	int step;
+
+	chip->read_buf(mtd, tmp_buf,
+		       mtd->writesize + mtd->oobsize);
+
+	/*
+	 * If required, swap the bad block marker and the data stored in the
+	 * metadata section, so that we don't wrongly consider a block as bad.
+	 *
+	 * See the layout description for a detailed explanation on why this
+	 * is needed.
+	 */
+	if (this->swap_block_mark) {
+		u8 swap = tmp_buf[0];
+
+		tmp_buf[0] = tmp_buf[mtd->writesize];
+		tmp_buf[mtd->writesize] = swap;
+	}
+
+	/*
+	 * Copy the metadata section into the oob buffer (this section is
+	 * guaranteed to be aligned on a byte boundary).
+	 */
+	if (oob_required)
+		memcpy(oob, tmp_buf, nfc_geo->metadata_size);
+
+	oob_bit_off = nfc_geo->metadata_size * 8;
+	src_bit_off = oob_bit_off;
+
+	/* Extract interleaved payload data and ECC bits */
+	for (step = 0; step < nfc_geo->ecc_chunk_count; step++) {
+		if (buf)
+			gpmi_copy_bits(buf, step * eccsize * 8,
+				       tmp_buf, src_bit_off,
+				       eccsize * 8);
+		src_bit_off += eccsize * 8;
+
+		/* Align last ECC block to align a byte boundary */
+		if (step == nfc_geo->ecc_chunk_count - 1 &&
+		    (oob_bit_off + eccbits) % 8)
+			eccbits += 8 - ((oob_bit_off + eccbits) % 8);
+
+		if (oob_required)
+			gpmi_copy_bits(oob, oob_bit_off,
+				       tmp_buf, src_bit_off,
+				       eccbits);
+
+		src_bit_off += eccbits;
+		oob_bit_off += eccbits;
+	}
+
+	if (oob_required) {
+		oob_byte_off = oob_bit_off / 8;
+
+		if (oob_byte_off < mtd->oobsize)
+			memcpy(oob + oob_byte_off,
+			       tmp_buf + mtd->writesize + oob_byte_off,
+			       mtd->oobsize - oob_byte_off);
+	}
+
+	return 0;
+}
+
+/*
+ * This function writes a NAND page without involving the ECC engine (no HW
+ * ECC generation).
+ * The tricky part in the GPMI/BCH controller is that it stores ECC bits
+ * inline (interleaved with payload DATA), and do not align data chunk on
+ * byte boundaries.
+ * We thus need to take care moving the OOB area at the right place in the
+ * final page, which is why we're using gpmi_copy_bits.
+ *
+ * See set_geometry_by_ecc_info inline comments to have a full description
+ * of the layout used by the GPMI controller.
+ */
+static int gpmi_ecc_write_page_raw(struct mtd_info *mtd,
+				   struct nand_chip *chip,
+				   const uint8_t *buf,
+				   int oob_required)
+{
+	struct gpmi_nand_data *this = chip->priv;
+	struct bch_geometry *nfc_geo = &this->bch_geometry;
+	int eccsize = nfc_geo->ecc_chunk_size;
+	int eccbits = nfc_geo->ecc_strength * nfc_geo->gf_len;
+	u8 *tmp_buf = this->raw_buffer;
+	uint8_t *oob = chip->oob_poi;
+	size_t dst_bit_off;
+	size_t oob_bit_off;
+	size_t oob_byte_off;
+	int step;
+
+	/*
+	 * Initialize all bits to 1 in case we don't have a buffer for the
+	 * payload or oob data in order to leave unspecified bits of data
+	 * to their initial state.
+	 */
+	if (!buf || !oob_required)
+		memset(tmp_buf, 0xff, mtd->writesize + mtd->oobsize);
+
+	/*
+	 * First copy the metadata section (stored in oob buffer) at the
+	 * beginning of the page, as imposed by the GPMI layout.
+	 */
+	memcpy(tmp_buf, oob, nfc_geo->metadata_size);
+	oob_bit_off = nfc_geo->metadata_size * 8;
+	dst_bit_off = oob_bit_off;
+
+	/* Interleave payload data and ECC bits */
+	for (step = 0; step < nfc_geo->ecc_chunk_count; step++) {
+		if (buf)
+			gpmi_copy_bits(tmp_buf, dst_bit_off,
+				       buf, step * eccsize * 8, eccsize * 8);
+		dst_bit_off += eccsize * 8;
+
+		/* Align last ECC block to align a byte boundary */
+		if (step == nfc_geo->ecc_chunk_count - 1 &&
+		    (oob_bit_off + eccbits) % 8)
+			eccbits += 8 - ((oob_bit_off + eccbits) % 8);
+
+		if (oob_required)
+			gpmi_copy_bits(tmp_buf, dst_bit_off,
+				       oob, oob_bit_off, eccbits);
+
+		dst_bit_off += eccbits;
+		oob_bit_off += eccbits;
+	}
+
+	oob_byte_off = oob_bit_off / 8;
+
+	if (oob_required && oob_byte_off < mtd->oobsize)
+		memcpy(tmp_buf + mtd->writesize + oob_byte_off,
+		       oob + oob_byte_off, mtd->oobsize - oob_byte_off);
+
+	/*
+	 * If required, swap the bad block marker and the first byte of the
+	 * metadata section, so that we don't modify the bad block marker.
+	 *
+	 * See the layout description for a detailed explanation on why this
+	 * is needed.
+	 */
+	if (this->swap_block_mark) {
+		u8 swap = tmp_buf[0];
+
+		tmp_buf[0] = tmp_buf[mtd->writesize];
+		tmp_buf[mtd->writesize] = swap;
+	}
+
+	chip->write_buf(mtd, tmp_buf, mtd->writesize + mtd->oobsize);
+
+	return 0;
+}
+
+static int gpmi_ecc_read_oob_raw(struct mtd_info *mtd, struct nand_chip *chip,
+				 int page)
+{
+	chip->cmdfunc(mtd, NAND_CMD_READ0, 0, page);
+
+	return gpmi_ecc_read_page_raw(mtd, chip, NULL, 1, page);
+}
+
+static int gpmi_ecc_write_oob_raw(struct mtd_info *mtd, struct nand_chip *chip,
+				 int page)
+{
+	chip->cmdfunc(mtd, NAND_CMD_SEQIN, 0, page);
+
+	return gpmi_ecc_write_page_raw(mtd, chip, NULL, 1);
+}
+
 static int gpmi_block_markbad(struct mtd_info *mtd, loff_t ofs)
 {
 	struct nand_chip *chip = mtd->priv;
@@ -1664,6 +1861,10 @@ static int gpmi_init_last(struct gpmi_nand_data *this)
 	ecc->write_page	= gpmi_ecc_write_page;
 	ecc->read_oob	= gpmi_ecc_read_oob;
 	ecc->write_oob	= gpmi_ecc_write_oob;
+	ecc->read_page_raw = gpmi_ecc_read_page_raw;
+	ecc->write_page_raw = gpmi_ecc_write_page_raw;
+	ecc->read_oob_raw = gpmi_ecc_read_oob_raw;
+	ecc->write_oob_raw = gpmi_ecc_write_oob_raw;
 	ecc->mode	= NAND_ECC_HW;
 	ecc->size	= bch_geo->ecc_chunk_size;
 	ecc->strength	= bch_geo->ecc_strength;
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-nand.h b/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
index 32c6ba49f986..544062f65020 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
@@ -189,6 +189,8 @@ struct gpmi_nand_data {
 	void			*auxiliary_virt;
 	dma_addr_t		auxiliary_phys;
 
+	void			*raw_buffer;
+
 	/* DMA channels */
 #define DMA_CHANS		8
 	struct dma_chan		*dma_chans[DMA_CHANS];
@@ -290,6 +292,10 @@ extern int gpmi_send_page(struct gpmi_nand_data *,
 extern int gpmi_read_page(struct gpmi_nand_data *,
 			dma_addr_t payload, dma_addr_t auxiliary);
 
+void gpmi_copy_bits(u8 *dst, size_t dst_bit_off,
+		    const u8 *src, size_t src_bit_off,
+		    size_t nbits);
+
 /* BCH : Status Block Completion Codes */
 #define STATUS_GOOD		0x00
 #define STATUS_ERASED		0xff
diff --git a/drivers/mtd/nand/jz4740_nand.c b/drivers/mtd/nand/jz4740_nand.c
index a2c804de156b..1633ec9c5108 100644
--- a/drivers/mtd/nand/jz4740_nand.c
+++ b/drivers/mtd/nand/jz4740_nand.c
@@ -577,7 +577,6 @@ static struct platform_driver jz_nand_driver = {
 	.remove = jz_nand_remove,
 	.driver = {
 		.name = "jz4740-nand",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mtd/nand/lpc32xx_mlc.c b/drivers/mtd/nand/lpc32xx_mlc.c
index 7335346dc126..79c3b7801e1f 100644
--- a/drivers/mtd/nand/lpc32xx_mlc.c
+++ b/drivers/mtd/nand/lpc32xx_mlc.c
@@ -878,7 +878,6 @@ static struct platform_driver lpc32xx_nand_driver = {
 	.suspend	= lpc32xx_nand_suspend,
 	.driver		= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = lpc32xx_nand_match,
 	},
 };
diff --git a/drivers/mtd/nand/lpc32xx_slc.c b/drivers/mtd/nand/lpc32xx_slc.c
index 8caef28e0756..abfec13868e5 100644
--- a/drivers/mtd/nand/lpc32xx_slc.c
+++ b/drivers/mtd/nand/lpc32xx_slc.c
@@ -999,7 +999,6 @@ static struct platform_driver lpc32xx_nand_driver = {
 	.suspend	= lpc32xx_nand_suspend,
 	.driver		= {
 		.name	= LPC32XX_MODNAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = lpc32xx_nand_match,
 	},
 };
diff --git a/drivers/mtd/nand/mpc5121_nfc.c b/drivers/mtd/nand/mpc5121_nfc.c
index e78841a2dcc3..1f12e5bfbced 100644
--- a/drivers/mtd/nand/mpc5121_nfc.c
+++ b/drivers/mtd/nand/mpc5121_nfc.c
@@ -847,7 +847,6 @@ static struct platform_driver mpc5121_nfc_driver = {
 	.remove		= mpc5121_nfc_remove,
 	.driver		= {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = mpc5121_nfc_match,
 	},
 };
diff --git a/drivers/mtd/nand/mxc_nand.c b/drivers/mtd/nand/mxc_nand.c
index dba262bf766f..a8f550fec35e 100644
--- a/drivers/mtd/nand/mxc_nand.c
+++ b/drivers/mtd/nand/mxc_nand.c
@@ -280,14 +280,10 @@ static void memcpy32_fromio(void *trg, const void __iomem  *src, size_t size)
 		*t++ = __raw_readl(s++);
 }
 
-static void memcpy32_toio(void __iomem *trg, const void *src, int size)
+static inline void memcpy32_toio(void __iomem *trg, const void *src, int size)
 {
-	int i;
-	u32 __iomem *t = trg;
-	const u32 *s = src;
-
-	for (i = 0; i < (size >> 2); i++)
-		__raw_writel(*s++, t++);
+	/* __iowrite32_copy use 32bit size values so divide by 4 */
+	__iowrite32_copy(trg, src, size / 4);
 }
 
 static int check_int_v3(struct mxc_nand_host *host)
@@ -1600,7 +1596,6 @@ static int mxcnd_remove(struct platform_device *pdev)
 static struct platform_driver mxcnd_driver = {
 	.driver = {
 		   .name = DRIVER_NAME,
-		   .owner = THIS_MODULE,
 		   .of_match_table = of_match_ptr(mxcnd_dt_ids),
 	},
 	.id_table = mxcnd_devtype,
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index 5b5c62712814..41585dfb206f 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -485,11 +485,11 @@ static int nand_check_wp(struct mtd_info *mtd)
 }
 
 /**
- * nand_block_checkbad - [GENERIC] Check if a block is marked bad
+ * nand_block_isreserved - [GENERIC] Check if a block is marked reserved.
  * @mtd: MTD device structure
  * @ofs: offset from device start
  *
- * Check if the block is mark as reserved.
+ * Check if the block is marked as reserved.
  */
 static int nand_block_isreserved(struct mtd_info *mtd, loff_t ofs)
 {
@@ -720,7 +720,7 @@ static void nand_command_lp(struct mtd_info *mtd, unsigned int command,
 
 	/*
 	 * Program and erase have their own busy handlers status, sequential
-	 * in, and deplete1 need no delay.
+	 * in and status need no delay.
 	 */
 	switch (command) {
 
@@ -3765,9 +3765,9 @@ ident_done:
 		pr_info("%s %s\n", nand_manuf_ids[maf_idx].name,
 				type->name);
 
-	pr_info("%dMiB, %s, page size: %d, OOB size: %d\n",
+	pr_info("%d MiB, %s, erase size: %d KiB, page size: %d, OOB size: %d\n",
 		(int)(chip->chipsize >> 20), nand_is_slc(chip) ? "SLC" : "MLC",
-		mtd->writesize, mtd->oobsize);
+		mtd->erasesize >> 10, mtd->writesize, mtd->oobsize);
 	return type;
 }
 
@@ -4035,7 +4035,7 @@ int nand_scan_tail(struct mtd_info *mtd)
 		 */
 		if (!ecc->size && (mtd->oobsize >= 64)) {
 			ecc->size = 512;
-			ecc->bytes = 7;
+			ecc->bytes = DIV_ROUND_UP(13 * ecc->strength, 8);
 		}
 		ecc->priv = nand_bch_init(mtd, ecc->size, ecc->bytes,
 					       &ecc->layout);
diff --git a/drivers/mtd/nand/nand_ids.c b/drivers/mtd/nand/nand_ids.c
index fbde89105245..dd620c19c619 100644
--- a/drivers/mtd/nand/nand_ids.c
+++ b/drivers/mtd/nand/nand_ids.c
@@ -178,6 +178,7 @@ struct nand_manufacturers nand_manuf_ids[] = {
 	{NAND_MFR_EON, "Eon"},
 	{NAND_MFR_SANDISK, "SanDisk"},
 	{NAND_MFR_INTEL, "Intel"},
+	{NAND_MFR_ATO, "ATO"},
 	{0x0, "Unknown"}
 };
 
diff --git a/drivers/mtd/nand/nandsim.c b/drivers/mtd/nand/nandsim.c
index 7dc1dd28d896..ab5bbf567439 100644
--- a/drivers/mtd/nand/nandsim.c
+++ b/drivers/mtd/nand/nandsim.c
@@ -87,10 +87,6 @@
 #define CONFIG_NANDSIM_MAX_PARTS  32
 #endif
 
-static uint first_id_byte  = CONFIG_NANDSIM_FIRST_ID_BYTE;
-static uint second_id_byte = CONFIG_NANDSIM_SECOND_ID_BYTE;
-static uint third_id_byte  = CONFIG_NANDSIM_THIRD_ID_BYTE;
-static uint fourth_id_byte = CONFIG_NANDSIM_FOURTH_ID_BYTE;
 static uint access_delay   = CONFIG_NANDSIM_ACCESS_DELAY;
 static uint programm_delay = CONFIG_NANDSIM_PROGRAMM_DELAY;
 static uint erase_delay    = CONFIG_NANDSIM_ERASE_DELAY;
@@ -111,11 +107,19 @@ static unsigned int overridesize = 0;
 static char *cache_file = NULL;
 static unsigned int bbt;
 static unsigned int bch;
+static u_char id_bytes[8] = {
+	[0] = CONFIG_NANDSIM_FIRST_ID_BYTE,
+	[1] = CONFIG_NANDSIM_SECOND_ID_BYTE,
+	[2] = CONFIG_NANDSIM_THIRD_ID_BYTE,
+	[3] = CONFIG_NANDSIM_FOURTH_ID_BYTE,
+	[4 ... 7] = 0xFF,
+};
 
-module_param(first_id_byte,  uint, 0400);
-module_param(second_id_byte, uint, 0400);
-module_param(third_id_byte,  uint, 0400);
-module_param(fourth_id_byte, uint, 0400);
+module_param_array(id_bytes, byte, NULL, 0400);
+module_param_named(first_id_byte, id_bytes[0], byte, 0400);
+module_param_named(second_id_byte, id_bytes[1], byte, 0400);
+module_param_named(third_id_byte, id_bytes[2], byte, 0400);
+module_param_named(fourth_id_byte, id_bytes[3], byte, 0400);
 module_param(access_delay,   uint, 0400);
 module_param(programm_delay, uint, 0400);
 module_param(erase_delay,    uint, 0400);
@@ -136,10 +140,11 @@ module_param(cache_file,     charp, 0400);
 module_param(bbt,	     uint, 0400);
 module_param(bch,	     uint, 0400);
 
-MODULE_PARM_DESC(first_id_byte,  "The first byte returned by NAND Flash 'read ID' command (manufacturer ID)");
-MODULE_PARM_DESC(second_id_byte, "The second byte returned by NAND Flash 'read ID' command (chip ID)");
-MODULE_PARM_DESC(third_id_byte,  "The third byte returned by NAND Flash 'read ID' command");
-MODULE_PARM_DESC(fourth_id_byte, "The fourth byte returned by NAND Flash 'read ID' command");
+MODULE_PARM_DESC(id_bytes,       "The ID bytes returned by NAND Flash 'read ID' command");
+MODULE_PARM_DESC(first_id_byte,  "The first byte returned by NAND Flash 'read ID' command (manufacturer ID) (obsolete)");
+MODULE_PARM_DESC(second_id_byte, "The second byte returned by NAND Flash 'read ID' command (chip ID) (obsolete)");
+MODULE_PARM_DESC(third_id_byte,  "The third byte returned by NAND Flash 'read ID' command (obsolete)");
+MODULE_PARM_DESC(fourth_id_byte, "The fourth byte returned by NAND Flash 'read ID' command (obsolete)");
 MODULE_PARM_DESC(access_delay,   "Initial page access delay (microseconds)");
 MODULE_PARM_DESC(programm_delay, "Page programm delay (microseconds");
 MODULE_PARM_DESC(erase_delay,    "Sector erase delay (milliseconds)");
@@ -304,7 +309,7 @@ struct nandsim {
 	unsigned int nbparts;
 
 	uint busw;              /* flash chip bus width (8 or 16) */
-	u_char ids[4];          /* chip's ID bytes */
+	u_char ids[8];          /* chip's ID bytes */
 	uint32_t options;       /* chip's characteristic bits */
 	uint32_t state;         /* current chip state */
 	uint32_t nxstate;       /* next expected state */
@@ -2279,17 +2284,18 @@ static int __init ns_init_module(void)
 	 * Perform minimum nandsim structure initialization to handle
 	 * the initial ID read command correctly
 	 */
-	if (third_id_byte != 0xFF || fourth_id_byte != 0xFF)
+	if (id_bytes[6] != 0xFF || id_bytes[7] != 0xFF)
+		nand->geom.idbytes = 8;
+	else if (id_bytes[4] != 0xFF || id_bytes[5] != 0xFF)
+		nand->geom.idbytes = 6;
+	else if (id_bytes[2] != 0xFF || id_bytes[3] != 0xFF)
 		nand->geom.idbytes = 4;
 	else
 		nand->geom.idbytes = 2;
 	nand->regs.status = NS_STATUS_OK(nand);
 	nand->nxstate = STATE_UNKNOWN;
 	nand->options |= OPT_PAGE512; /* temporary value */
-	nand->ids[0] = first_id_byte;
-	nand->ids[1] = second_id_byte;
-	nand->ids[2] = third_id_byte;
-	nand->ids[3] = fourth_id_byte;
+	memcpy(nand->ids, id_bytes, sizeof(nand->ids));
 	if (bus_width == 16) {
 		nand->busw = 16;
 		chip->options |= NAND_BUSWIDTH_16;
diff --git a/drivers/mtd/nand/ndfc.c b/drivers/mtd/nand/ndfc.c
index 253a644da76a..3187c6b92d9a 100644
--- a/drivers/mtd/nand/ndfc.c
+++ b/drivers/mtd/nand/ndfc.c
@@ -277,7 +277,6 @@ MODULE_DEVICE_TABLE(of, ndfc_match);
 static struct platform_driver ndfc_driver = {
 	.driver = {
 		.name = "ndfc",
-		.owner = THIS_MODULE,
 		.of_match_table = ndfc_match,
 	},
 	.probe = ndfc_probe,
diff --git a/drivers/mtd/nand/nuc900_nand.c b/drivers/mtd/nand/nuc900_nand.c
index e8a5fffd6ab2..e58c644dd220 100644
--- a/drivers/mtd/nand/nuc900_nand.c
+++ b/drivers/mtd/nand/nuc900_nand.c
@@ -300,7 +300,6 @@ static struct platform_driver nuc900_nand_driver = {
 	.remove		= nuc900_nand_remove,
 	.driver		= {
 		.name	= "nuc900-fmi",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mtd/nand/omap2.c b/drivers/mtd/nand/omap2.c
index 3b357e920a0c..63f858e6bf39 100644
--- a/drivers/mtd/nand/omap2.c
+++ b/drivers/mtd/nand/omap2.c
@@ -144,11 +144,13 @@ static u_char bch8_vector[] = {0xf3, 0xdb, 0x14, 0x16, 0x8b, 0xd2, 0xbe, 0xcc,
 	0xac, 0x6b, 0xff, 0x99, 0x7b};
 static u_char bch4_vector[] = {0x00, 0x6b, 0x31, 0xdd, 0x41, 0xbc, 0x10};
 
-/* oob info generated runtime depending on ecc algorithm and layout selected */
-static struct nand_ecclayout omap_oobinfo;
+/* Shared among all NAND instances to synchronize access to the ECC Engine */
+static struct nand_hw_control omap_gpmc_controller = {
+	.lock = __SPIN_LOCK_UNLOCKED(omap_gpmc_controller.lock),
+	.wq = __WAIT_QUEUE_HEAD_INITIALIZER(omap_gpmc_controller.wq),
+};
 
 struct omap_nand_info {
-	struct nand_hw_control		controller;
 	struct omap_nand_platform_data	*pdata;
 	struct mtd_info			mtd;
 	struct nand_chip		nand;
@@ -168,6 +170,8 @@ struct omap_nand_info {
 	u_char				*buf;
 	int					buf_len;
 	struct gpmc_nand_regs		reg;
+	/* generated at runtime depending on ECC algorithm and layout selected */
+	struct nand_ecclayout		oobinfo;
 	/* fields specific for BCHx_HW ECC scheme */
 	struct device			*elm_dev;
 	struct device_node		*of_node;
@@ -1686,9 +1690,6 @@ static int omap_nand_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, info);
 
-	spin_lock_init(&info->controller.lock);
-	init_waitqueue_head(&info->controller.wq);
-
 	info->pdev		= pdev;
 	info->gpmc_cs		= pdata->cs;
 	info->reg		= pdata->reg;
@@ -1708,7 +1709,7 @@ static int omap_nand_probe(struct platform_device *pdev)
 
 	info->phys_base = res->start;
 
-	nand_chip->controller = &info->controller;
+	nand_chip->controller = &omap_gpmc_controller;
 
 	nand_chip->IO_ADDR_W = nand_chip->IO_ADDR_R;
 	nand_chip->cmd_ctrl  = omap_hwcontrol;
@@ -1741,13 +1742,6 @@ static int omap_nand_probe(struct platform_device *pdev)
 		goto return_error;
 	}
 
-	/* check for small page devices */
-	if ((mtd->oobsize < 64) && (pdata->ecc_opt != OMAP_ECC_HAM1_CODE_HW)) {
-		dev_err(&info->pdev->dev, "small page devices are not supported\n");
-		err = -EINVAL;
-		goto return_error;
-	}
-
 	/* re-populate low-level callbacks based on xfer modes */
 	switch (pdata->xfer_type) {
 	case NAND_OMAP_PREFETCH_POLLED:
@@ -1840,7 +1834,7 @@ static int omap_nand_probe(struct platform_device *pdev)
 	}
 
 	/* populate MTD interface based on ECC scheme */
-	ecclayout		= &omap_oobinfo;
+	ecclayout		= &info->oobinfo;
 	switch (info->ecc_opt) {
 	case OMAP_ECC_HAM1_CODE_SW:
 		nand_chip->ecc.mode = NAND_ECC_SOFT;
@@ -2095,7 +2089,6 @@ static struct platform_driver omap_nand_driver = {
 	.remove		= omap_nand_remove,
 	.driver		= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mtd/nand/omap_elm.c b/drivers/mtd/nand/omap_elm.c
index 058531044ceb..376bfe19104f 100644
--- a/drivers/mtd/nand/omap_elm.c
+++ b/drivers/mtd/nand/omap_elm.c
@@ -563,7 +563,6 @@ MODULE_DEVICE_TABLE(of, elm_of_match);
 static struct platform_driver elm_driver = {
 	.driver	= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(elm_of_match),
 		.pm	= &elm_pm_ops,
 	},
diff --git a/drivers/mtd/nand/orion_nand.c b/drivers/mtd/nand/orion_nand.c
index 471b4df3a5ac..c3c6d305caa7 100644
--- a/drivers/mtd/nand/orion_nand.c
+++ b/drivers/mtd/nand/orion_nand.c
@@ -19,7 +19,7 @@
 #include <linux/mtd/partitions.h>
 #include <linux/clk.h>
 #include <linux/err.h>
-#include <asm/io.h>
+#include <linux/io.h>
 #include <asm/sizes.h>
 #include <linux/platform_data/mtd-orion_nand.h>
 
@@ -85,33 +85,24 @@ static int __init orion_nand_probe(struct platform_device *pdev)
 	int ret = 0;
 	u32 val = 0;
 
-	nc = kzalloc(sizeof(struct nand_chip) + sizeof(struct mtd_info), GFP_KERNEL);
-	if (!nc) {
-		ret = -ENOMEM;
-		goto no_res;
-	}
+	nc = devm_kzalloc(&pdev->dev,
+			sizeof(struct nand_chip) + sizeof(struct mtd_info),
+			GFP_KERNEL);
+	if (!nc)
+		return -ENOMEM;
 	mtd = (struct mtd_info *)(nc + 1);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		ret = -ENODEV;
-		goto no_res;
-	}
+	io_base = devm_ioremap_resource(&pdev->dev, res);
 
-	io_base = ioremap(res->start, resource_size(res));
-	if (!io_base) {
-		dev_err(&pdev->dev, "ioremap failed\n");
-		ret = -EIO;
-		goto no_res;
-	}
+	if (IS_ERR(io_base))
+		return PTR_ERR(io_base);
 
 	if (pdev->dev.of_node) {
 		board = devm_kzalloc(&pdev->dev, sizeof(struct orion_nand_data),
 					GFP_KERNEL);
-		if (!board) {
-			ret = -ENOMEM;
-			goto no_res;
-		}
+		if (!board)
+			return -ENOMEM;
 		if (!of_property_read_u32(pdev->dev.of_node, "cle", &val))
 			board->cle = (u8)val;
 		else
@@ -185,9 +176,6 @@ no_dev:
 		clk_disable_unprepare(clk);
 		clk_put(clk);
 	}
-	iounmap(io_base);
-no_res:
-	kfree(nc);
 
 	return ret;
 }
@@ -195,15 +183,10 @@ no_res:
 static int 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);
 
-	iounmap(nc->IO_ADDR_W);
-
-	kfree(nc);
-
 	clk = clk_get(&pdev->dev, NULL);
 	if (!IS_ERR(clk)) {
 		clk_disable_unprepare(clk);
@@ -224,7 +207,6 @@ static struct platform_driver orion_nand_driver = {
 	.remove		= 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/nand/pasemi_nand.c b/drivers/mtd/nand/pasemi_nand.c
index 2c98f9da7471..66c345b42097 100644
--- a/drivers/mtd/nand/pasemi_nand.c
+++ b/drivers/mtd/nand/pasemi_nand.c
@@ -223,7 +223,6 @@ static struct platform_driver pasemi_nand_driver =
 {
 	.driver = {
 		.name = driver_name,
-		.owner = THIS_MODULE,
 		.of_match_table = pasemi_nand_match,
 	},
 	.probe		= pasemi_nand_probe,
diff --git a/drivers/mtd/nand/plat_nand.c b/drivers/mtd/nand/plat_nand.c
index 0b068a5c0bff..4535c263fae5 100644
--- a/drivers/mtd/nand/plat_nand.c
+++ b/drivers/mtd/nand/plat_nand.c
@@ -138,7 +138,6 @@ static struct platform_driver plat_nand_driver = {
 	.remove	= plat_nand_remove,
 	.driver	= {
 		.name		= "gen_nand",
-		.owner		= THIS_MODULE,
 		.of_match_table = plat_nand_match,
 	},
 };
diff --git a/drivers/mtd/nand/s3c2410.c b/drivers/mtd/nand/s3c2410.c
index 6b97bf17ce5d..35aef5edb588 100644
--- a/drivers/mtd/nand/s3c2410.c
+++ b/drivers/mtd/nand/s3c2410.c
@@ -1135,7 +1135,6 @@ static struct platform_driver s3c24xx_nand_driver = {
 	.id_table	= s3c24xx_driver_ids,
 	.driver		= {
 		.name	= "s3c24xx-nand",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mtd/nand/sh_flctl.c b/drivers/mtd/nand/sh_flctl.c
index 0ed7c603298f..a21c378f096a 100644
--- a/drivers/mtd/nand/sh_flctl.c
+++ b/drivers/mtd/nand/sh_flctl.c
@@ -1190,7 +1190,6 @@ static struct platform_driver flctl_driver = {
 	.remove		= flctl_remove,
 	.driver = {
 		.name	= "sh_flctl",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(of_flctl_match),
 	},
 };
diff --git a/drivers/mtd/nand/sharpsl.c b/drivers/mtd/nand/sharpsl.c
index e81059b58382..842c47a451a0 100644
--- a/drivers/mtd/nand/sharpsl.c
+++ b/drivers/mtd/nand/sharpsl.c
@@ -220,7 +220,6 @@ static int sharpsl_nand_remove(struct platform_device *pdev)
 static struct platform_driver sharpsl_nand_driver = {
 	.driver = {
 		.name	= "sharpsl-nand",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= sharpsl_nand_probe,
 	.remove		= sharpsl_nand_remove,
diff --git a/drivers/mtd/nand/socrates_nand.c b/drivers/mtd/nand/socrates_nand.c
index fe8058a45054..d71062273f55 100644
--- a/drivers/mtd/nand/socrates_nand.c
+++ b/drivers/mtd/nand/socrates_nand.c
@@ -240,7 +240,6 @@ MODULE_DEVICE_TABLE(of, socrates_nand_match);
 static struct platform_driver socrates_nand_driver = {
 	.driver = {
 		.name = "socrates_nand",
-		.owner = THIS_MODULE,
 		.of_match_table = socrates_nand_match,
 	},
 	.probe		= socrates_nand_probe,
diff --git a/drivers/mtd/nand/sunxi_nand.c b/drivers/mtd/nand/sunxi_nand.c
new file mode 100644
index 000000000000..ccaa8e283388
--- /dev/null
+++ b/drivers/mtd/nand/sunxi_nand.c
@@ -0,0 +1,1432 @@
+/*
+ * Copyright (C) 2013 Boris BREZILLON <b.brezillon.dev@gmail.com>
+ *
+ * Derived from:
+ *	https://github.com/yuq/sunxi-nfc-mtd
+ *	Copyright (C) 2013 Qiang Yu <yuq825@gmail.com>
+ *
+ *	https://github.com/hno/Allwinner-Info
+ *	Copyright (C) 2013 Henrik Nordström <Henrik Nordström>
+ *
+ *	Copyright (C) 2013 Dmitriy B. <rzk333@gmail.com>
+ *	Copyright (C) 2013 Sergey Lapin <slapin@ossfans.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
+#include <linux/of_mtd.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/mtd/partitions.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/dmaengine.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+
+#define NFC_REG_CTL		0x0000
+#define NFC_REG_ST		0x0004
+#define NFC_REG_INT		0x0008
+#define NFC_REG_TIMING_CTL	0x000C
+#define NFC_REG_TIMING_CFG	0x0010
+#define NFC_REG_ADDR_LOW	0x0014
+#define NFC_REG_ADDR_HIGH	0x0018
+#define NFC_REG_SECTOR_NUM	0x001C
+#define NFC_REG_CNT		0x0020
+#define NFC_REG_CMD		0x0024
+#define NFC_REG_RCMD_SET	0x0028
+#define NFC_REG_WCMD_SET	0x002C
+#define NFC_REG_IO_DATA		0x0030
+#define NFC_REG_ECC_CTL		0x0034
+#define NFC_REG_ECC_ST		0x0038
+#define NFC_REG_DEBUG		0x003C
+#define NFC_REG_ECC_CNT0	0x0040
+#define NFC_REG_ECC_CNT1	0x0044
+#define NFC_REG_ECC_CNT2	0x0048
+#define NFC_REG_ECC_CNT3	0x004c
+#define NFC_REG_USER_DATA_BASE	0x0050
+#define NFC_REG_SPARE_AREA	0x00A0
+#define NFC_RAM0_BASE		0x0400
+#define NFC_RAM1_BASE		0x0800
+
+/* define bit use in NFC_CTL */
+#define NFC_EN			BIT(0)
+#define NFC_RESET		BIT(1)
+#define NFC_BUS_WIDYH		BIT(2)
+#define NFC_RB_SEL		BIT(3)
+#define NFC_CE_SEL		GENMASK(26, 24)
+#define NFC_CE_CTL		BIT(6)
+#define NFC_CE_CTL1		BIT(7)
+#define NFC_PAGE_SIZE		GENMASK(11, 8)
+#define NFC_SAM			BIT(12)
+#define NFC_RAM_METHOD		BIT(14)
+#define NFC_DEBUG_CTL		BIT(31)
+
+/* define bit use in NFC_ST */
+#define NFC_RB_B2R		BIT(0)
+#define NFC_CMD_INT_FLAG	BIT(1)
+#define NFC_DMA_INT_FLAG	BIT(2)
+#define NFC_CMD_FIFO_STATUS	BIT(3)
+#define NFC_STA			BIT(4)
+#define NFC_NATCH_INT_FLAG	BIT(5)
+#define NFC_RB_STATE0		BIT(8)
+#define NFC_RB_STATE1		BIT(9)
+#define NFC_RB_STATE2		BIT(10)
+#define NFC_RB_STATE3		BIT(11)
+
+/* define bit use in NFC_INT */
+#define NFC_B2R_INT_ENABLE	BIT(0)
+#define NFC_CMD_INT_ENABLE	BIT(1)
+#define NFC_DMA_INT_ENABLE	BIT(2)
+#define NFC_INT_MASK		(NFC_B2R_INT_ENABLE | \
+				 NFC_CMD_INT_ENABLE | \
+				 NFC_DMA_INT_ENABLE)
+
+/* define bit use in NFC_CMD */
+#define NFC_CMD_LOW_BYTE	GENMASK(7, 0)
+#define NFC_CMD_HIGH_BYTE	GENMASK(15, 8)
+#define NFC_ADR_NUM		GENMASK(18, 16)
+#define NFC_SEND_ADR		BIT(19)
+#define NFC_ACCESS_DIR		BIT(20)
+#define NFC_DATA_TRANS		BIT(21)
+#define NFC_SEND_CMD1		BIT(22)
+#define NFC_WAIT_FLAG		BIT(23)
+#define NFC_SEND_CMD2		BIT(24)
+#define NFC_SEQ			BIT(25)
+#define NFC_DATA_SWAP_METHOD	BIT(26)
+#define NFC_ROW_AUTO_INC	BIT(27)
+#define NFC_SEND_CMD3		BIT(28)
+#define NFC_SEND_CMD4		BIT(29)
+#define NFC_CMD_TYPE		GENMASK(31, 30)
+
+/* define bit use in NFC_RCMD_SET */
+#define NFC_READ_CMD		GENMASK(7, 0)
+#define NFC_RANDOM_READ_CMD0	GENMASK(15, 8)
+#define NFC_RANDOM_READ_CMD1	GENMASK(23, 16)
+
+/* define bit use in NFC_WCMD_SET */
+#define NFC_PROGRAM_CMD		GENMASK(7, 0)
+#define NFC_RANDOM_WRITE_CMD	GENMASK(15, 8)
+#define NFC_READ_CMD0		GENMASK(23, 16)
+#define NFC_READ_CMD1		GENMASK(31, 24)
+
+/* define bit use in NFC_ECC_CTL */
+#define NFC_ECC_EN		BIT(0)
+#define NFC_ECC_PIPELINE	BIT(3)
+#define NFC_ECC_EXCEPTION	BIT(4)
+#define NFC_ECC_BLOCK_SIZE	BIT(5)
+#define NFC_RANDOM_EN		BIT(9)
+#define NFC_RANDOM_DIRECTION	BIT(10)
+#define NFC_ECC_MODE_SHIFT	12
+#define NFC_ECC_MODE		GENMASK(15, 12)
+#define NFC_RANDOM_SEED		GENMASK(30, 16)
+
+#define NFC_DEFAULT_TIMEOUT_MS	1000
+
+#define NFC_SRAM_SIZE		1024
+
+#define NFC_MAX_CS		7
+
+/*
+ * Ready/Busy detection type: describes the Ready/Busy detection modes
+ *
+ * @RB_NONE:	no external detection available, rely on STATUS command
+ *		and software timeouts
+ * @RB_NATIVE:	use sunxi NAND controller Ready/Busy support. The Ready/Busy
+ *		pin of the NAND flash chip must be connected to one of the
+ *		native NAND R/B pins (those which can be muxed to the NAND
+ *		Controller)
+ * @RB_GPIO:	use a simple GPIO to handle Ready/Busy status. The Ready/Busy
+ *		pin of the NAND flash chip must be connected to a GPIO capable
+ *		pin.
+ */
+enum sunxi_nand_rb_type {
+	RB_NONE,
+	RB_NATIVE,
+	RB_GPIO,
+};
+
+/*
+ * Ready/Busy structure: stores information related to Ready/Busy detection
+ *
+ * @type:	the Ready/Busy detection mode
+ * @info:	information related to the R/B detection mode. Either a gpio
+ *		id or a native R/B id (those supported by the NAND controller).
+ */
+struct sunxi_nand_rb {
+	enum sunxi_nand_rb_type type;
+	union {
+		int gpio;
+		int nativeid;
+	} info;
+};
+
+/*
+ * Chip Select structure: stores information related to NAND Chip Select
+ *
+ * @cs:		the NAND CS id used to communicate with a NAND Chip
+ * @rb:		the Ready/Busy description
+ */
+struct sunxi_nand_chip_sel {
+	u8 cs;
+	struct sunxi_nand_rb rb;
+};
+
+/*
+ * sunxi HW ECC infos: stores information related to HW ECC support
+ *
+ * @mode:	the sunxi ECC mode field deduced from ECC requirements
+ * @layout:	the OOB layout depending on the ECC requirements and the
+ *		selected ECC mode
+ */
+struct sunxi_nand_hw_ecc {
+	int mode;
+	struct nand_ecclayout layout;
+};
+
+/*
+ * NAND chip structure: stores NAND chip device related information
+ *
+ * @node:		used to store NAND chips into a list
+ * @nand:		base NAND chip structure
+ * @mtd:		base MTD structure
+ * @clk_rate:		clk_rate required for this NAND chip
+ * @selected:		current active CS
+ * @nsels:		number of CS lines required by the NAND chip
+ * @sels:		array of CS lines descriptions
+ */
+struct sunxi_nand_chip {
+	struct list_head node;
+	struct nand_chip nand;
+	struct mtd_info mtd;
+	unsigned long clk_rate;
+	int selected;
+	int nsels;
+	struct sunxi_nand_chip_sel sels[0];
+};
+
+static inline struct sunxi_nand_chip *to_sunxi_nand(struct nand_chip *nand)
+{
+	return container_of(nand, struct sunxi_nand_chip, nand);
+}
+
+/*
+ * NAND Controller structure: stores sunxi NAND controller information
+ *
+ * @controller:		base controller structure
+ * @dev:		parent device (used to print error messages)
+ * @regs:		NAND controller registers
+ * @ahb_clk:		NAND Controller AHB clock
+ * @mod_clk:		NAND Controller mod clock
+ * @assigned_cs:	bitmask describing already assigned CS lines
+ * @clk_rate:		NAND controller current clock rate
+ * @chips:		a list containing all the NAND chips attached to
+ *			this NAND controller
+ * @complete:		a completion object used to wait for NAND
+ *			controller events
+ */
+struct sunxi_nfc {
+	struct nand_hw_control controller;
+	struct device *dev;
+	void __iomem *regs;
+	struct clk *ahb_clk;
+	struct clk *mod_clk;
+	unsigned long assigned_cs;
+	unsigned long clk_rate;
+	struct list_head chips;
+	struct completion complete;
+};
+
+static inline struct sunxi_nfc *to_sunxi_nfc(struct nand_hw_control *ctrl)
+{
+	return container_of(ctrl, struct sunxi_nfc, controller);
+}
+
+static irqreturn_t sunxi_nfc_interrupt(int irq, void *dev_id)
+{
+	struct sunxi_nfc *nfc = dev_id;
+	u32 st = readl(nfc->regs + NFC_REG_ST);
+	u32 ien = readl(nfc->regs + NFC_REG_INT);
+
+	if (!(ien & st))
+		return IRQ_NONE;
+
+	if ((ien & st) == ien)
+		complete(&nfc->complete);
+
+	writel(st & NFC_INT_MASK, nfc->regs + NFC_REG_ST);
+	writel(~st & ien & NFC_INT_MASK, nfc->regs + NFC_REG_INT);
+
+	return IRQ_HANDLED;
+}
+
+static int sunxi_nfc_wait_int(struct sunxi_nfc *nfc, u32 flags,
+			      unsigned int timeout_ms)
+{
+	init_completion(&nfc->complete);
+
+	writel(flags, nfc->regs + NFC_REG_INT);
+
+	if (!timeout_ms)
+		timeout_ms = NFC_DEFAULT_TIMEOUT_MS;
+
+	if (!wait_for_completion_timeout(&nfc->complete,
+					 msecs_to_jiffies(timeout_ms))) {
+		dev_err(nfc->dev, "wait interrupt timedout\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int sunxi_nfc_wait_cmd_fifo_empty(struct sunxi_nfc *nfc)
+{
+	unsigned long timeout = jiffies +
+				msecs_to_jiffies(NFC_DEFAULT_TIMEOUT_MS);
+
+	do {
+		if (!(readl(nfc->regs + NFC_REG_ST) & NFC_CMD_FIFO_STATUS))
+			return 0;
+	} while (time_before(jiffies, timeout));
+
+	dev_err(nfc->dev, "wait for empty cmd FIFO timedout\n");
+	return -ETIMEDOUT;
+}
+
+static int sunxi_nfc_rst(struct sunxi_nfc *nfc)
+{
+	unsigned long timeout = jiffies +
+				msecs_to_jiffies(NFC_DEFAULT_TIMEOUT_MS);
+
+	writel(0, nfc->regs + NFC_REG_ECC_CTL);
+	writel(NFC_RESET, nfc->regs + NFC_REG_CTL);
+
+	do {
+		if (!(readl(nfc->regs + NFC_REG_CTL) & NFC_RESET))
+			return 0;
+	} while (time_before(jiffies, timeout));
+
+	dev_err(nfc->dev, "wait for NAND controller reset timedout\n");
+	return -ETIMEDOUT;
+}
+
+static int sunxi_nfc_dev_ready(struct mtd_info *mtd)
+{
+	struct nand_chip *nand = mtd->priv;
+	struct sunxi_nand_chip *sunxi_nand = to_sunxi_nand(nand);
+	struct sunxi_nfc *nfc = to_sunxi_nfc(sunxi_nand->nand.controller);
+	struct sunxi_nand_rb *rb;
+	unsigned long timeo = (sunxi_nand->nand.state == FL_ERASING ? 400 : 20);
+	int ret;
+
+	if (sunxi_nand->selected < 0)
+		return 0;
+
+	rb = &sunxi_nand->sels[sunxi_nand->selected].rb;
+
+	switch (rb->type) {
+	case RB_NATIVE:
+		ret = !!(readl(nfc->regs + NFC_REG_ST) &
+			 (NFC_RB_STATE0 << rb->info.nativeid));
+		if (ret)
+			break;
+
+		sunxi_nfc_wait_int(nfc, NFC_RB_B2R, timeo);
+		ret = !!(readl(nfc->regs + NFC_REG_ST) &
+			 (NFC_RB_STATE0 << rb->info.nativeid));
+		break;
+	case RB_GPIO:
+		ret = gpio_get_value(rb->info.gpio);
+		break;
+	case RB_NONE:
+	default:
+		ret = 0;
+		dev_err(nfc->dev, "cannot check R/B NAND status!\n");
+		break;
+	}
+
+	return ret;
+}
+
+static void sunxi_nfc_select_chip(struct mtd_info *mtd, int chip)
+{
+	struct nand_chip *nand = mtd->priv;
+	struct sunxi_nand_chip *sunxi_nand = to_sunxi_nand(nand);
+	struct sunxi_nfc *nfc = to_sunxi_nfc(sunxi_nand->nand.controller);
+	struct sunxi_nand_chip_sel *sel;
+	u32 ctl;
+
+	if (chip > 0 && chip >= sunxi_nand->nsels)
+		return;
+
+	if (chip == sunxi_nand->selected)
+		return;
+
+	ctl = readl(nfc->regs + NFC_REG_CTL) &
+	      ~(NFC_CE_SEL | NFC_RB_SEL | NFC_EN);
+
+	if (chip >= 0) {
+		sel = &sunxi_nand->sels[chip];
+
+		ctl |= (sel->cs << 24) | NFC_EN |
+		       (((nand->page_shift - 10) & 0xf) << 8);
+		if (sel->rb.type == RB_NONE) {
+			nand->dev_ready = NULL;
+		} else {
+			nand->dev_ready = sunxi_nfc_dev_ready;
+			if (sel->rb.type == RB_NATIVE)
+				ctl |= (sel->rb.info.nativeid << 3);
+		}
+
+		writel(mtd->writesize, nfc->regs + NFC_REG_SPARE_AREA);
+
+		if (nfc->clk_rate != sunxi_nand->clk_rate) {
+			clk_set_rate(nfc->mod_clk, sunxi_nand->clk_rate);
+			nfc->clk_rate = sunxi_nand->clk_rate;
+		}
+	}
+
+	writel(ctl, nfc->regs + NFC_REG_CTL);
+
+	sunxi_nand->selected = chip;
+}
+
+static void sunxi_nfc_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
+{
+	struct nand_chip *nand = mtd->priv;
+	struct sunxi_nand_chip *sunxi_nand = to_sunxi_nand(nand);
+	struct sunxi_nfc *nfc = to_sunxi_nfc(sunxi_nand->nand.controller);
+	int ret;
+	int cnt;
+	int offs = 0;
+	u32 tmp;
+
+	while (len > offs) {
+		cnt = min(len - offs, NFC_SRAM_SIZE);
+
+		ret = sunxi_nfc_wait_cmd_fifo_empty(nfc);
+		if (ret)
+			break;
+
+		writel(cnt, nfc->regs + NFC_REG_CNT);
+		tmp = NFC_DATA_TRANS | NFC_DATA_SWAP_METHOD;
+		writel(tmp, nfc->regs + NFC_REG_CMD);
+
+		ret = sunxi_nfc_wait_int(nfc, NFC_CMD_INT_FLAG, 0);
+		if (ret)
+			break;
+
+		if (buf)
+			memcpy_fromio(buf + offs, nfc->regs + NFC_RAM0_BASE,
+				      cnt);
+		offs += cnt;
+	}
+}
+
+static void sunxi_nfc_write_buf(struct mtd_info *mtd, const uint8_t *buf,
+				int len)
+{
+	struct nand_chip *nand = mtd->priv;
+	struct sunxi_nand_chip *sunxi_nand = to_sunxi_nand(nand);
+	struct sunxi_nfc *nfc = to_sunxi_nfc(sunxi_nand->nand.controller);
+	int ret;
+	int cnt;
+	int offs = 0;
+	u32 tmp;
+
+	while (len > offs) {
+		cnt = min(len - offs, NFC_SRAM_SIZE);
+
+		ret = sunxi_nfc_wait_cmd_fifo_empty(nfc);
+		if (ret)
+			break;
+
+		writel(cnt, nfc->regs + NFC_REG_CNT);
+		memcpy_toio(nfc->regs + NFC_RAM0_BASE, buf + offs, cnt);
+		tmp = NFC_DATA_TRANS | NFC_DATA_SWAP_METHOD |
+		      NFC_ACCESS_DIR;
+		writel(tmp, nfc->regs + NFC_REG_CMD);
+
+		ret = sunxi_nfc_wait_int(nfc, NFC_CMD_INT_FLAG, 0);
+		if (ret)
+			break;
+
+		offs += cnt;
+	}
+}
+
+static uint8_t sunxi_nfc_read_byte(struct mtd_info *mtd)
+{
+	uint8_t ret;
+
+	sunxi_nfc_read_buf(mtd, &ret, 1);
+
+	return ret;
+}
+
+static void sunxi_nfc_cmd_ctrl(struct mtd_info *mtd, int dat,
+			       unsigned int ctrl)
+{
+	struct nand_chip *nand = mtd->priv;
+	struct sunxi_nand_chip *sunxi_nand = to_sunxi_nand(nand);
+	struct sunxi_nfc *nfc = to_sunxi_nfc(sunxi_nand->nand.controller);
+	int ret;
+	u32 tmp;
+
+	ret = sunxi_nfc_wait_cmd_fifo_empty(nfc);
+	if (ret)
+		return;
+
+	if (ctrl & NAND_CTRL_CHANGE) {
+		tmp = readl(nfc->regs + NFC_REG_CTL);
+		if (ctrl & NAND_NCE)
+			tmp |= NFC_CE_CTL;
+		else
+			tmp &= ~NFC_CE_CTL;
+		writel(tmp, nfc->regs + NFC_REG_CTL);
+	}
+
+	if (dat == NAND_CMD_NONE)
+		return;
+
+	if (ctrl & NAND_CLE) {
+		writel(NFC_SEND_CMD1 | dat, nfc->regs + NFC_REG_CMD);
+	} else {
+		writel(dat, nfc->regs + NFC_REG_ADDR_LOW);
+		writel(NFC_SEND_ADR, nfc->regs + NFC_REG_CMD);
+	}
+
+	sunxi_nfc_wait_int(nfc, NFC_CMD_INT_FLAG, 0);
+}
+
+static int sunxi_nfc_hw_ecc_read_page(struct mtd_info *mtd,
+				      struct nand_chip *chip, uint8_t *buf,
+				      int oob_required, int page)
+{
+	struct sunxi_nfc *nfc = to_sunxi_nfc(chip->controller);
+	struct nand_ecc_ctrl *ecc = &chip->ecc;
+	struct nand_ecclayout *layout = ecc->layout;
+	struct sunxi_nand_hw_ecc *data = ecc->priv;
+	unsigned int max_bitflips = 0;
+	int offset;
+	int ret;
+	u32 tmp;
+	int i;
+	int cnt;
+
+	tmp = readl(nfc->regs + NFC_REG_ECC_CTL);
+	tmp &= ~(NFC_ECC_MODE | NFC_ECC_PIPELINE | NFC_ECC_BLOCK_SIZE);
+	tmp |= NFC_ECC_EN | (data->mode << NFC_ECC_MODE_SHIFT) |
+	       NFC_ECC_EXCEPTION;
+
+	writel(tmp, nfc->regs + NFC_REG_ECC_CTL);
+
+	for (i = 0; i < ecc->steps; i++) {
+		if (i)
+			chip->cmdfunc(mtd, NAND_CMD_RNDOUT, i * ecc->size, -1);
+
+		offset = mtd->writesize + layout->eccpos[i * ecc->bytes] - 4;
+
+		chip->read_buf(mtd, NULL, ecc->size);
+
+		chip->cmdfunc(mtd, NAND_CMD_RNDOUT, offset, -1);
+
+		ret = sunxi_nfc_wait_cmd_fifo_empty(nfc);
+		if (ret)
+			return ret;
+
+		tmp = NFC_DATA_TRANS | NFC_DATA_SWAP_METHOD | (1 << 30);
+		writel(tmp, nfc->regs + NFC_REG_CMD);
+
+		ret = sunxi_nfc_wait_int(nfc, NFC_CMD_INT_FLAG, 0);
+		if (ret)
+			return ret;
+
+		memcpy_fromio(buf + (i * ecc->size),
+			      nfc->regs + NFC_RAM0_BASE, ecc->size);
+
+		if (readl(nfc->regs + NFC_REG_ECC_ST) & 0x1) {
+			mtd->ecc_stats.failed++;
+		} else {
+			tmp = readl(nfc->regs + NFC_REG_ECC_CNT0) & 0xff;
+			mtd->ecc_stats.corrected += tmp;
+			max_bitflips = max_t(unsigned int, max_bitflips, tmp);
+		}
+
+		if (oob_required) {
+			chip->cmdfunc(mtd, NAND_CMD_RNDOUT, offset, -1);
+
+			ret = sunxi_nfc_wait_cmd_fifo_empty(nfc);
+			if (ret)
+				return ret;
+
+			offset -= mtd->writesize;
+			chip->read_buf(mtd, chip->oob_poi + offset,
+				      ecc->bytes + 4);
+		}
+	}
+
+	if (oob_required) {
+		cnt = ecc->layout->oobfree[ecc->steps].length;
+		if (cnt > 0) {
+			offset = mtd->writesize +
+				 ecc->layout->oobfree[ecc->steps].offset;
+			chip->cmdfunc(mtd, NAND_CMD_RNDOUT, offset, -1);
+			offset -= mtd->writesize;
+			chip->read_buf(mtd, chip->oob_poi + offset, cnt);
+		}
+	}
+
+	tmp = readl(nfc->regs + NFC_REG_ECC_CTL);
+	tmp &= ~NFC_ECC_EN;
+
+	writel(tmp, nfc->regs + NFC_REG_ECC_CTL);
+
+	return max_bitflips;
+}
+
+static int sunxi_nfc_hw_ecc_write_page(struct mtd_info *mtd,
+				       struct nand_chip *chip,
+				       const uint8_t *buf, int oob_required)
+{
+	struct sunxi_nfc *nfc = to_sunxi_nfc(chip->controller);
+	struct nand_ecc_ctrl *ecc = &chip->ecc;
+	struct nand_ecclayout *layout = ecc->layout;
+	struct sunxi_nand_hw_ecc *data = ecc->priv;
+	int offset;
+	int ret;
+	u32 tmp;
+	int i;
+	int cnt;
+
+	tmp = readl(nfc->regs + NFC_REG_ECC_CTL);
+	tmp &= ~(NFC_ECC_MODE | NFC_ECC_PIPELINE | NFC_ECC_BLOCK_SIZE);
+	tmp |= NFC_ECC_EN | (data->mode << NFC_ECC_MODE_SHIFT) |
+	       NFC_ECC_EXCEPTION;
+
+	writel(tmp, nfc->regs + NFC_REG_ECC_CTL);
+
+	for (i = 0; i < ecc->steps; i++) {
+		if (i)
+			chip->cmdfunc(mtd, NAND_CMD_RNDIN, i * ecc->size, -1);
+
+		chip->write_buf(mtd, buf + (i * ecc->size), ecc->size);
+
+		offset = layout->eccpos[i * ecc->bytes] - 4 + mtd->writesize;
+
+		/* Fill OOB data in */
+		if (oob_required) {
+			tmp = 0xffffffff;
+			memcpy_toio(nfc->regs + NFC_REG_USER_DATA_BASE, &tmp,
+				    4);
+		} else {
+			memcpy_toio(nfc->regs + NFC_REG_USER_DATA_BASE,
+				    chip->oob_poi + offset - mtd->writesize,
+				    4);
+		}
+
+		chip->cmdfunc(mtd, NAND_CMD_RNDIN, offset, -1);
+
+		ret = sunxi_nfc_wait_cmd_fifo_empty(nfc);
+		if (ret)
+			return ret;
+
+		tmp = NFC_DATA_TRANS | NFC_DATA_SWAP_METHOD | NFC_ACCESS_DIR |
+		      (1 << 30);
+		writel(tmp, nfc->regs + NFC_REG_CMD);
+		ret = sunxi_nfc_wait_int(nfc, NFC_CMD_INT_FLAG, 0);
+		if (ret)
+			return ret;
+	}
+
+	if (oob_required) {
+		cnt = ecc->layout->oobfree[i].length;
+		if (cnt > 0) {
+			offset = mtd->writesize +
+				 ecc->layout->oobfree[i].offset;
+			chip->cmdfunc(mtd, NAND_CMD_RNDIN, offset, -1);
+			offset -= mtd->writesize;
+			chip->write_buf(mtd, chip->oob_poi + offset, cnt);
+		}
+	}
+
+	tmp = readl(nfc->regs + NFC_REG_ECC_CTL);
+	tmp &= ~NFC_ECC_EN;
+
+	writel(tmp, nfc->regs + NFC_REG_ECC_CTL);
+
+	return 0;
+}
+
+static int sunxi_nfc_hw_syndrome_ecc_read_page(struct mtd_info *mtd,
+					       struct nand_chip *chip,
+					       uint8_t *buf, int oob_required,
+					       int page)
+{
+	struct sunxi_nfc *nfc = to_sunxi_nfc(chip->controller);
+	struct nand_ecc_ctrl *ecc = &chip->ecc;
+	struct sunxi_nand_hw_ecc *data = ecc->priv;
+	unsigned int max_bitflips = 0;
+	uint8_t *oob = chip->oob_poi;
+	int offset = 0;
+	int ret;
+	int cnt;
+	u32 tmp;
+	int i;
+
+	tmp = readl(nfc->regs + NFC_REG_ECC_CTL);
+	tmp &= ~(NFC_ECC_MODE | NFC_ECC_PIPELINE | NFC_ECC_BLOCK_SIZE);
+	tmp |= NFC_ECC_EN | (data->mode << NFC_ECC_MODE_SHIFT) |
+	       NFC_ECC_EXCEPTION;
+
+	writel(tmp, nfc->regs + NFC_REG_ECC_CTL);
+
+	for (i = 0; i < ecc->steps; i++) {
+		chip->read_buf(mtd, NULL, ecc->size);
+
+		tmp = NFC_DATA_TRANS | NFC_DATA_SWAP_METHOD | (1 << 30);
+		writel(tmp, nfc->regs + NFC_REG_CMD);
+
+		ret = sunxi_nfc_wait_int(nfc, NFC_CMD_INT_FLAG, 0);
+		if (ret)
+			return ret;
+
+		memcpy_fromio(buf, nfc->regs + NFC_RAM0_BASE, ecc->size);
+		buf += ecc->size;
+		offset += ecc->size;
+
+		if (readl(nfc->regs + NFC_REG_ECC_ST) & 0x1) {
+			mtd->ecc_stats.failed++;
+		} else {
+			tmp = readl(nfc->regs + NFC_REG_ECC_CNT0) & 0xff;
+			mtd->ecc_stats.corrected += tmp;
+			max_bitflips = max_t(unsigned int, max_bitflips, tmp);
+		}
+
+		if (oob_required) {
+			chip->cmdfunc(mtd, NAND_CMD_RNDOUT, offset, -1);
+			chip->read_buf(mtd, oob, ecc->bytes + ecc->prepad);
+			oob += ecc->bytes + ecc->prepad;
+		}
+
+		offset += ecc->bytes + ecc->prepad;
+	}
+
+	if (oob_required) {
+		cnt = mtd->oobsize - (oob - chip->oob_poi);
+		if (cnt > 0) {
+			chip->cmdfunc(mtd, NAND_CMD_RNDOUT, offset, -1);
+			chip->read_buf(mtd, oob, cnt);
+		}
+	}
+
+	writel(readl(nfc->regs + NFC_REG_ECC_CTL) & ~NFC_ECC_EN,
+	       nfc->regs + NFC_REG_ECC_CTL);
+
+	return max_bitflips;
+}
+
+static int sunxi_nfc_hw_syndrome_ecc_write_page(struct mtd_info *mtd,
+						struct nand_chip *chip,
+						const uint8_t *buf,
+						int oob_required)
+{
+	struct sunxi_nfc *nfc = to_sunxi_nfc(chip->controller);
+	struct nand_ecc_ctrl *ecc = &chip->ecc;
+	struct sunxi_nand_hw_ecc *data = ecc->priv;
+	uint8_t *oob = chip->oob_poi;
+	int offset = 0;
+	int ret;
+	int cnt;
+	u32 tmp;
+	int i;
+
+	tmp = readl(nfc->regs + NFC_REG_ECC_CTL);
+	tmp &= ~(NFC_ECC_MODE | NFC_ECC_PIPELINE | NFC_ECC_BLOCK_SIZE);
+	tmp |= NFC_ECC_EN | (data->mode << NFC_ECC_MODE_SHIFT) |
+	       NFC_ECC_EXCEPTION;
+
+	writel(tmp, nfc->regs + NFC_REG_ECC_CTL);
+
+	for (i = 0; i < ecc->steps; i++) {
+		chip->write_buf(mtd, buf + (i * ecc->size), ecc->size);
+		offset += ecc->size;
+
+		/* Fill OOB data in */
+		if (oob_required) {
+			tmp = 0xffffffff;
+			memcpy_toio(nfc->regs + NFC_REG_USER_DATA_BASE, &tmp,
+				    4);
+		} else {
+			memcpy_toio(nfc->regs + NFC_REG_USER_DATA_BASE, oob,
+				    4);
+		}
+
+		tmp = NFC_DATA_TRANS | NFC_DATA_SWAP_METHOD | NFC_ACCESS_DIR |
+		      (1 << 30);
+		writel(tmp, nfc->regs + NFC_REG_CMD);
+
+		ret = sunxi_nfc_wait_int(nfc, NFC_CMD_INT_FLAG, 0);
+		if (ret)
+			return ret;
+
+		offset += ecc->bytes + ecc->prepad;
+		oob += ecc->bytes + ecc->prepad;
+	}
+
+	if (oob_required) {
+		cnt = mtd->oobsize - (oob - chip->oob_poi);
+		if (cnt > 0) {
+			chip->cmdfunc(mtd, NAND_CMD_RNDIN, offset, -1);
+			chip->write_buf(mtd, oob, cnt);
+		}
+	}
+
+	tmp = readl(nfc->regs + NFC_REG_ECC_CTL);
+	tmp &= ~NFC_ECC_EN;
+
+	writel(tmp, nfc->regs + NFC_REG_ECC_CTL);
+
+	return 0;
+}
+
+static int sunxi_nand_chip_set_timings(struct sunxi_nand_chip *chip,
+				       const struct nand_sdr_timings *timings)
+{
+	u32 min_clk_period = 0;
+
+	/* T1 <=> tCLS */
+	if (timings->tCLS_min > min_clk_period)
+		min_clk_period = timings->tCLS_min;
+
+	/* T2 <=> tCLH */
+	if (timings->tCLH_min > min_clk_period)
+		min_clk_period = timings->tCLH_min;
+
+	/* T3 <=> tCS */
+	if (timings->tCS_min > min_clk_period)
+		min_clk_period = timings->tCS_min;
+
+	/* T4 <=> tCH */
+	if (timings->tCH_min > min_clk_period)
+		min_clk_period = timings->tCH_min;
+
+	/* T5 <=> tWP */
+	if (timings->tWP_min > min_clk_period)
+		min_clk_period = timings->tWP_min;
+
+	/* T6 <=> tWH */
+	if (timings->tWH_min > min_clk_period)
+		min_clk_period = timings->tWH_min;
+
+	/* T7 <=> tALS */
+	if (timings->tALS_min > min_clk_period)
+		min_clk_period = timings->tALS_min;
+
+	/* T8 <=> tDS */
+	if (timings->tDS_min > min_clk_period)
+		min_clk_period = timings->tDS_min;
+
+	/* T9 <=> tDH */
+	if (timings->tDH_min > min_clk_period)
+		min_clk_period = timings->tDH_min;
+
+	/* T10 <=> tRR */
+	if (timings->tRR_min > (min_clk_period * 3))
+		min_clk_period = DIV_ROUND_UP(timings->tRR_min, 3);
+
+	/* T11 <=> tALH */
+	if (timings->tALH_min > min_clk_period)
+		min_clk_period = timings->tALH_min;
+
+	/* T12 <=> tRP */
+	if (timings->tRP_min > min_clk_period)
+		min_clk_period = timings->tRP_min;
+
+	/* T13 <=> tREH */
+	if (timings->tREH_min > min_clk_period)
+		min_clk_period = timings->tREH_min;
+
+	/* T14 <=> tRC */
+	if (timings->tRC_min > (min_clk_period * 2))
+		min_clk_period = DIV_ROUND_UP(timings->tRC_min, 2);
+
+	/* T15 <=> tWC */
+	if (timings->tWC_min > (min_clk_period * 2))
+		min_clk_period = DIV_ROUND_UP(timings->tWC_min, 2);
+
+
+	/* Convert min_clk_period from picoseconds to nanoseconds */
+	min_clk_period = DIV_ROUND_UP(min_clk_period, 1000);
+
+	/*
+	 * Convert min_clk_period into a clk frequency, then get the
+	 * appropriate rate for the NAND controller IP given this formula
+	 * (specified in the datasheet):
+	 * nand clk_rate = 2 * min_clk_rate
+	 */
+	chip->clk_rate = (2 * NSEC_PER_SEC) / min_clk_period;
+
+	/* TODO: configure T16-T19 */
+
+	return 0;
+}
+
+static int sunxi_nand_chip_init_timings(struct sunxi_nand_chip *chip,
+					struct device_node *np)
+{
+	const struct nand_sdr_timings *timings;
+	int ret;
+	int mode;
+
+	mode = onfi_get_async_timing_mode(&chip->nand);
+	if (mode == ONFI_TIMING_MODE_UNKNOWN) {
+		mode = chip->nand.onfi_timing_mode_default;
+	} else {
+		uint8_t feature[ONFI_SUBFEATURE_PARAM_LEN] = {};
+
+		mode = fls(mode) - 1;
+		if (mode < 0)
+			mode = 0;
+
+		feature[0] = mode;
+		ret = chip->nand.onfi_set_features(&chip->mtd, &chip->nand,
+						ONFI_FEATURE_ADDR_TIMING_MODE,
+						feature);
+		if (ret)
+			return ret;
+	}
+
+	timings = onfi_async_timing_mode_to_sdr_timings(mode);
+	if (IS_ERR(timings))
+		return PTR_ERR(timings);
+
+	return sunxi_nand_chip_set_timings(chip, timings);
+}
+
+static int sunxi_nand_hw_common_ecc_ctrl_init(struct mtd_info *mtd,
+					      struct nand_ecc_ctrl *ecc,
+					      struct device_node *np)
+{
+	static const u8 strengths[] = { 16, 24, 28, 32, 40, 48, 56, 60, 64 };
+	struct nand_chip *nand = mtd->priv;
+	struct sunxi_nand_chip *sunxi_nand = to_sunxi_nand(nand);
+	struct sunxi_nfc *nfc = to_sunxi_nfc(sunxi_nand->nand.controller);
+	struct sunxi_nand_hw_ecc *data;
+	struct nand_ecclayout *layout;
+	int nsectors;
+	int ret;
+	int i;
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	/* Add ECC info retrieval from DT */
+	for (i = 0; i < ARRAY_SIZE(strengths); i++) {
+		if (ecc->strength <= strengths[i])
+			break;
+	}
+
+	if (i >= ARRAY_SIZE(strengths)) {
+		dev_err(nfc->dev, "unsupported strength\n");
+		ret = -ENOTSUPP;
+		goto err;
+	}
+
+	data->mode = i;
+
+	/* HW ECC always request ECC bytes for 1024 bytes blocks */
+	ecc->bytes = DIV_ROUND_UP(ecc->strength * fls(8 * 1024), 8);
+
+	/* HW ECC always work with even numbers of ECC bytes */
+	ecc->bytes = ALIGN(ecc->bytes, 2);
+
+	layout = &data->layout;
+	nsectors = mtd->writesize / ecc->size;
+
+	if (mtd->oobsize < ((ecc->bytes + 4) * nsectors)) {
+		ret = -EINVAL;
+		goto err;
+	}
+
+	layout->eccbytes = (ecc->bytes * nsectors);
+
+	ecc->layout = layout;
+	ecc->priv = data;
+
+	return 0;
+
+err:
+	kfree(data);
+
+	return ret;
+}
+
+static void sunxi_nand_hw_common_ecc_ctrl_cleanup(struct nand_ecc_ctrl *ecc)
+{
+	kfree(ecc->priv);
+}
+
+static int sunxi_nand_hw_ecc_ctrl_init(struct mtd_info *mtd,
+				       struct nand_ecc_ctrl *ecc,
+				       struct device_node *np)
+{
+	struct nand_ecclayout *layout;
+	int nsectors;
+	int i, j;
+	int ret;
+
+	ret = sunxi_nand_hw_common_ecc_ctrl_init(mtd, ecc, np);
+	if (ret)
+		return ret;
+
+	ecc->read_page = sunxi_nfc_hw_ecc_read_page;
+	ecc->write_page = sunxi_nfc_hw_ecc_write_page;
+	layout = ecc->layout;
+	nsectors = mtd->writesize / ecc->size;
+
+	for (i = 0; i < nsectors; i++) {
+		if (i) {
+			layout->oobfree[i].offset =
+				layout->oobfree[i - 1].offset +
+				layout->oobfree[i - 1].length +
+				ecc->bytes;
+			layout->oobfree[i].length = 4;
+		} else {
+			/*
+			 * The first 2 bytes are used for BB markers, hence we
+			 * only have 2 bytes available in the first user data
+			 * section.
+			 */
+			layout->oobfree[i].length = 2;
+			layout->oobfree[i].offset = 2;
+		}
+
+		for (j = 0; j < ecc->bytes; j++)
+			layout->eccpos[(ecc->bytes * i) + j] =
+					layout->oobfree[i].offset +
+					layout->oobfree[i].length + j;
+	}
+
+	if (mtd->oobsize > (ecc->bytes + 4) * nsectors) {
+		layout->oobfree[nsectors].offset =
+				layout->oobfree[nsectors - 1].offset +
+				layout->oobfree[nsectors - 1].length +
+				ecc->bytes;
+		layout->oobfree[nsectors].length = mtd->oobsize -
+				((ecc->bytes + 4) * nsectors);
+	}
+
+	return 0;
+}
+
+static int sunxi_nand_hw_syndrome_ecc_ctrl_init(struct mtd_info *mtd,
+						struct nand_ecc_ctrl *ecc,
+						struct device_node *np)
+{
+	struct nand_ecclayout *layout;
+	int nsectors;
+	int i;
+	int ret;
+
+	ret = sunxi_nand_hw_common_ecc_ctrl_init(mtd, ecc, np);
+	if (ret)
+		return ret;
+
+	ecc->prepad = 4;
+	ecc->read_page = sunxi_nfc_hw_syndrome_ecc_read_page;
+	ecc->write_page = sunxi_nfc_hw_syndrome_ecc_write_page;
+
+	layout = ecc->layout;
+	nsectors = mtd->writesize / ecc->size;
+
+	for (i = 0; i < (ecc->bytes * nsectors); i++)
+		layout->eccpos[i] = i;
+
+	layout->oobfree[0].length = mtd->oobsize - i;
+	layout->oobfree[0].offset = i;
+
+	return 0;
+}
+
+static void sunxi_nand_ecc_cleanup(struct nand_ecc_ctrl *ecc)
+{
+	switch (ecc->mode) {
+	case NAND_ECC_HW:
+	case NAND_ECC_HW_SYNDROME:
+		sunxi_nand_hw_common_ecc_ctrl_cleanup(ecc);
+		break;
+	case NAND_ECC_NONE:
+		kfree(ecc->layout);
+	default:
+		break;
+	}
+}
+
+static int sunxi_nand_ecc_init(struct mtd_info *mtd, struct nand_ecc_ctrl *ecc,
+			       struct device_node *np)
+{
+	struct nand_chip *nand = mtd->priv;
+	int strength;
+	int blk_size;
+	int ret;
+
+	blk_size = of_get_nand_ecc_step_size(np);
+	strength = of_get_nand_ecc_strength(np);
+	if (blk_size > 0 && strength > 0) {
+		ecc->size = blk_size;
+		ecc->strength = strength;
+	} else {
+		ecc->size = nand->ecc_step_ds;
+		ecc->strength = nand->ecc_strength_ds;
+	}
+
+	if (!ecc->size || !ecc->strength)
+		return -EINVAL;
+
+	ecc->mode = NAND_ECC_HW;
+
+	ret = of_get_nand_ecc_mode(np);
+	if (ret >= 0)
+		ecc->mode = ret;
+
+	switch (ecc->mode) {
+	case NAND_ECC_SOFT_BCH:
+		ecc->bytes = DIV_ROUND_UP(ecc->strength * fls(8 * ecc->size),
+					  8);
+		break;
+	case NAND_ECC_HW:
+		ret = sunxi_nand_hw_ecc_ctrl_init(mtd, ecc, np);
+		if (ret)
+			return ret;
+		break;
+	case NAND_ECC_HW_SYNDROME:
+		ret = sunxi_nand_hw_syndrome_ecc_ctrl_init(mtd, ecc, np);
+		if (ret)
+			return ret;
+		break;
+	case NAND_ECC_NONE:
+		ecc->layout = kzalloc(sizeof(*ecc->layout), GFP_KERNEL);
+		if (!ecc->layout)
+			return -ENOMEM;
+		ecc->layout->oobfree[0].length = mtd->oobsize;
+	case NAND_ECC_SOFT:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int sunxi_nand_chip_init(struct device *dev, struct sunxi_nfc *nfc,
+				struct device_node *np)
+{
+	const struct nand_sdr_timings *timings;
+	struct sunxi_nand_chip *chip;
+	struct mtd_part_parser_data ppdata;
+	struct mtd_info *mtd;
+	struct nand_chip *nand;
+	int nsels;
+	int ret;
+	int i;
+	u32 tmp;
+
+	if (!of_get_property(np, "reg", &nsels))
+		return -EINVAL;
+
+	nsels /= sizeof(u32);
+	if (!nsels) {
+		dev_err(dev, "invalid reg property size\n");
+		return -EINVAL;
+	}
+
+	chip = devm_kzalloc(dev,
+			    sizeof(*chip) +
+			    (nsels * sizeof(struct sunxi_nand_chip_sel)),
+			    GFP_KERNEL);
+	if (!chip) {
+		dev_err(dev, "could not allocate chip\n");
+		return -ENOMEM;
+	}
+
+	chip->nsels = nsels;
+	chip->selected = -1;
+
+	for (i = 0; i < nsels; i++) {
+		ret = of_property_read_u32_index(np, "reg", i, &tmp);
+		if (ret) {
+			dev_err(dev, "could not retrieve reg property: %d\n",
+				ret);
+			return ret;
+		}
+
+		if (tmp > NFC_MAX_CS) {
+			dev_err(dev,
+				"invalid reg value: %u (max CS = 7)\n",
+				tmp);
+			return -EINVAL;
+		}
+
+		if (test_and_set_bit(tmp, &nfc->assigned_cs)) {
+			dev_err(dev, "CS %d already assigned\n", tmp);
+			return -EINVAL;
+		}
+
+		chip->sels[i].cs = tmp;
+
+		if (!of_property_read_u32_index(np, "allwinner,rb", i, &tmp) &&
+		    tmp < 2) {
+			chip->sels[i].rb.type = RB_NATIVE;
+			chip->sels[i].rb.info.nativeid = tmp;
+		} else {
+			ret = of_get_named_gpio(np, "rb-gpios", i);
+			if (ret >= 0) {
+				tmp = ret;
+				chip->sels[i].rb.type = RB_GPIO;
+				chip->sels[i].rb.info.gpio = tmp;
+				ret = devm_gpio_request(dev, tmp, "nand-rb");
+				if (ret)
+					return ret;
+
+				ret = gpio_direction_input(tmp);
+				if (ret)
+					return ret;
+			} else {
+				chip->sels[i].rb.type = RB_NONE;
+			}
+		}
+	}
+
+	timings = onfi_async_timing_mode_to_sdr_timings(0);
+	if (IS_ERR(timings)) {
+		ret = PTR_ERR(timings);
+		dev_err(dev,
+			"could not retrieve timings for ONFI mode 0: %d\n",
+			ret);
+		return ret;
+	}
+
+	ret = sunxi_nand_chip_set_timings(chip, timings);
+	if (ret) {
+		dev_err(dev, "could not configure chip timings: %d\n", ret);
+		return ret;
+	}
+
+	nand = &chip->nand;
+	/* Default tR value specified in the ONFI spec (chapter 4.15.1) */
+	nand->chip_delay = 200;
+	nand->controller = &nfc->controller;
+	nand->select_chip = sunxi_nfc_select_chip;
+	nand->cmd_ctrl = sunxi_nfc_cmd_ctrl;
+	nand->read_buf = sunxi_nfc_read_buf;
+	nand->write_buf = sunxi_nfc_write_buf;
+	nand->read_byte = sunxi_nfc_read_byte;
+
+	if (of_get_nand_on_flash_bbt(np))
+		nand->bbt_options |= NAND_BBT_USE_FLASH | NAND_BBT_NO_OOB;
+
+	mtd = &chip->mtd;
+	mtd->dev.parent = dev;
+	mtd->priv = nand;
+	mtd->owner = THIS_MODULE;
+
+	ret = nand_scan_ident(mtd, nsels, NULL);
+	if (ret)
+		return ret;
+
+	ret = sunxi_nand_chip_init_timings(chip, np);
+	if (ret) {
+		dev_err(dev, "could not configure chip timings: %d\n", ret);
+		return ret;
+	}
+
+	ret = sunxi_nand_ecc_init(mtd, &nand->ecc, np);
+	if (ret) {
+		dev_err(dev, "ECC init failed: %d\n", ret);
+		return ret;
+	}
+
+	ret = nand_scan_tail(mtd);
+	if (ret) {
+		dev_err(dev, "nand_scan_tail failed: %d\n", ret);
+		return ret;
+	}
+
+	ppdata.of_node = np;
+	ret = mtd_device_parse_register(mtd, NULL, &ppdata, NULL, 0);
+	if (ret) {
+		dev_err(dev, "failed to register mtd device: %d\n", ret);
+		nand_release(mtd);
+		return ret;
+	}
+
+	list_add_tail(&chip->node, &nfc->chips);
+
+	return 0;
+}
+
+static int sunxi_nand_chips_init(struct device *dev, struct sunxi_nfc *nfc)
+{
+	struct device_node *np = dev->of_node;
+	struct device_node *nand_np;
+	int nchips = of_get_child_count(np);
+	int ret;
+
+	if (nchips > 8) {
+		dev_err(dev, "too many NAND chips: %d (max = 8)\n", nchips);
+		return -EINVAL;
+	}
+
+	for_each_child_of_node(np, nand_np) {
+		ret = sunxi_nand_chip_init(dev, nfc, nand_np);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static void sunxi_nand_chips_cleanup(struct sunxi_nfc *nfc)
+{
+	struct sunxi_nand_chip *chip;
+
+	while (!list_empty(&nfc->chips)) {
+		chip = list_first_entry(&nfc->chips, struct sunxi_nand_chip,
+					node);
+		nand_release(&chip->mtd);
+		sunxi_nand_ecc_cleanup(&chip->nand.ecc);
+	}
+}
+
+static int sunxi_nfc_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct resource *r;
+	struct sunxi_nfc *nfc;
+	int irq;
+	int ret;
+
+	nfc = devm_kzalloc(dev, sizeof(*nfc), GFP_KERNEL);
+	if (!nfc)
+		return -ENOMEM;
+
+	nfc->dev = dev;
+	spin_lock_init(&nfc->controller.lock);
+	init_waitqueue_head(&nfc->controller.wq);
+	INIT_LIST_HEAD(&nfc->chips);
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	nfc->regs = devm_ioremap_resource(dev, r);
+	if (IS_ERR(nfc->regs))
+		return PTR_ERR(nfc->regs);
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(dev, "failed to retrieve irq\n");
+		return irq;
+	}
+
+	nfc->ahb_clk = devm_clk_get(dev, "ahb");
+	if (IS_ERR(nfc->ahb_clk)) {
+		dev_err(dev, "failed to retrieve ahb clk\n");
+		return PTR_ERR(nfc->ahb_clk);
+	}
+
+	ret = clk_prepare_enable(nfc->ahb_clk);
+	if (ret)
+		return ret;
+
+	nfc->mod_clk = devm_clk_get(dev, "mod");
+	if (IS_ERR(nfc->mod_clk)) {
+		dev_err(dev, "failed to retrieve mod clk\n");
+		ret = PTR_ERR(nfc->mod_clk);
+		goto out_ahb_clk_unprepare;
+	}
+
+	ret = clk_prepare_enable(nfc->mod_clk);
+	if (ret)
+		goto out_ahb_clk_unprepare;
+
+	ret = sunxi_nfc_rst(nfc);
+	if (ret)
+		goto out_mod_clk_unprepare;
+
+	writel(0, nfc->regs + NFC_REG_INT);
+	ret = devm_request_irq(dev, irq, sunxi_nfc_interrupt,
+			       0, "sunxi-nand", nfc);
+	if (ret)
+		goto out_mod_clk_unprepare;
+
+	platform_set_drvdata(pdev, nfc);
+
+	/*
+	 * TODO: replace these magic values with proper flags as soon as we
+	 * know what they are encoding.
+	 */
+	writel(0x100, nfc->regs + NFC_REG_TIMING_CTL);
+	writel(0x7ff, nfc->regs + NFC_REG_TIMING_CFG);
+
+	ret = sunxi_nand_chips_init(dev, nfc);
+	if (ret) {
+		dev_err(dev, "failed to init nand chips\n");
+		goto out_mod_clk_unprepare;
+	}
+
+	return 0;
+
+out_mod_clk_unprepare:
+	clk_disable_unprepare(nfc->mod_clk);
+out_ahb_clk_unprepare:
+	clk_disable_unprepare(nfc->ahb_clk);
+
+	return ret;
+}
+
+static int sunxi_nfc_remove(struct platform_device *pdev)
+{
+	struct sunxi_nfc *nfc = platform_get_drvdata(pdev);
+
+	sunxi_nand_chips_cleanup(nfc);
+
+	return 0;
+}
+
+static const struct of_device_id sunxi_nfc_ids[] = {
+	{ .compatible = "allwinner,sun4i-a10-nand" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, sunxi_nfc_ids);
+
+static struct platform_driver sunxi_nfc_driver = {
+	.driver = {
+		.name = "sunxi_nand",
+		.of_match_table = sunxi_nfc_ids,
+	},
+	.probe = sunxi_nfc_probe,
+	.remove = sunxi_nfc_remove,
+};
+module_platform_driver(sunxi_nfc_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Boris BREZILLON");
+MODULE_DESCRIPTION("Allwinner NAND Flash Controller driver");
+MODULE_ALIAS("platform:sunxi_nand");
diff --git a/drivers/mtd/nand/txx9ndfmc.c b/drivers/mtd/nand/txx9ndfmc.c
index c1622a5ba814..9c0bc45e28a9 100644
--- a/drivers/mtd/nand/txx9ndfmc.c
+++ b/drivers/mtd/nand/txx9ndfmc.c
@@ -417,7 +417,6 @@ static struct platform_driver txx9ndfmc_driver = {
 	.resume		= txx9ndfmc_resume,
 	.driver		= {
 		.name	= "txx9ndfmc",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mtd/onenand/generic.c b/drivers/mtd/onenand/generic.c
index 093c29ac1a13..32a216d31141 100644
--- a/drivers/mtd/onenand/generic.c
+++ b/drivers/mtd/onenand/generic.c
@@ -106,7 +106,6 @@ static int generic_onenand_remove(struct platform_device *pdev)
 static struct platform_driver generic_onenand_driver = {
 	.driver = {
 		.name		= DRIVER_NAME,
-		.owner		= THIS_MODULE,
 	},
 	.probe		= generic_onenand_probe,
 	.remove		= generic_onenand_remove,
diff --git a/drivers/mtd/onenand/omap2.c b/drivers/mtd/onenand/omap2.c
index d945473c3882..646ddd6db1b4 100644
--- a/drivers/mtd/onenand/omap2.c
+++ b/drivers/mtd/onenand/omap2.c
@@ -804,7 +804,6 @@ static struct platform_driver omap2_onenand_driver = {
 	.shutdown	= omap2_onenand_shutdown,
 	.driver		= {
 		.name	= DRIVER_NAME,
-		.owner  = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/mtd/spi-nor/fsl-quadspi.c b/drivers/mtd/spi-nor/fsl-quadspi.c
index d5269a26c839..39763b94f67d 100644
--- a/drivers/mtd/spi-nor/fsl-quadspi.c
+++ b/drivers/mtd/spi-nor/fsl-quadspi.c
@@ -719,16 +719,10 @@ static int fsl_qspi_read(struct spi_nor *nor, loff_t from,
 {
 	struct fsl_qspi *q = nor->priv;
 	u8 cmd = nor->read_opcode;
-	int ret;
 
 	dev_dbg(q->dev, "cmd [%x],read from (0x%p, 0x%.8x, 0x%.8x),len:%d\n",
 		cmd, q->ahb_base, q->chip_base_addr, (unsigned int)from, len);
 
-	/* Wait until the previous command is finished. */
-	ret = nor->wait_till_ready(nor);
-	if (ret)
-		return ret;
-
 	/* Read out the data directly from the AHB buffer.*/
 	memcpy(buf, q->ahb_base + q->chip_base_addr + from, len);
 
@@ -744,16 +738,6 @@ static int fsl_qspi_erase(struct spi_nor *nor, loff_t offs)
 	dev_dbg(nor->dev, "%dKiB at 0x%08x:0x%08x\n",
 		nor->mtd->erasesize / 1024, q->chip_base_addr, (u32)offs);
 
-	/* Wait until finished previous write command. */
-	ret = nor->wait_till_ready(nor);
-	if (ret)
-		return ret;
-
-	/* Send write enable, then erase commands. */
-	ret = nor->write_reg(nor, SPINOR_OP_WREN, NULL, 0, 0);
-	if (ret)
-		return ret;
-
 	ret = fsl_qspi_runcmd(q, nor->erase_opcode, offs, 0);
 	if (ret)
 		return ret;
@@ -849,9 +833,8 @@ static int fsl_qspi_probe(struct platform_device *pdev)
 
 	ret = clk_prepare_enable(q->clk);
 	if (ret) {
-		clk_disable_unprepare(q->clk_en);
 		dev_err(dev, "can not enable the qspi clock\n");
-		goto map_failed;
+		goto clk_failed;
 	}
 
 	/* find the irq */
@@ -905,7 +888,8 @@ static int fsl_qspi_probe(struct platform_device *pdev)
 		nor->prepare = fsl_qspi_prep;
 		nor->unprepare = fsl_qspi_unprep;
 
-		if (of_modalias_node(np, modalias, sizeof(modalias)) < 0)
+		ret = of_modalias_node(np, modalias, sizeof(modalias));
+		if (ret < 0)
 			goto map_failed;
 
 		ret = of_property_read_u32(np, "spi-max-frequency",
@@ -964,6 +948,7 @@ last_init_failed:
 
 irq_failed:
 	clk_disable_unprepare(q->clk);
+clk_failed:
 	clk_disable_unprepare(q->clk_en);
 map_failed:
 	dev_err(dev, "Freescale QuadSPI probe failed\n");
@@ -991,7 +976,6 @@ static struct platform_driver fsl_qspi_driver = {
 	.driver = {
 		.name	= "fsl-quadspi",
 		.bus	= &platform_bus_type,
-		.owner	= THIS_MODULE,
 		.of_match_table = fsl_qspi_dt_ids,
 	},
 	.probe          = fsl_qspi_probe,
diff --git a/drivers/mtd/spi-nor/spi-nor.c b/drivers/mtd/spi-nor/spi-nor.c
index c51ee52386a7..0f8ec3c2d015 100644
--- a/drivers/mtd/spi-nor/spi-nor.c
+++ b/drivers/mtd/spi-nor/spi-nor.c
@@ -26,7 +26,38 @@
 /* Define max times to check status register before we give up. */
 #define	MAX_READY_WAIT_JIFFIES	(40 * HZ) /* M25P16 specs 40s max chip erase */
 
-#define JEDEC_MFR(_jedec_id)	((_jedec_id) >> 16)
+#define SPI_NOR_MAX_ID_LEN	6
+
+struct flash_info {
+	/*
+	 * This array stores the ID bytes.
+	 * The first three bytes are the JEDIC ID.
+	 * JEDEC ID zero means "no ID" (mostly older chips).
+	 */
+	u8		id[SPI_NOR_MAX_ID_LEN];
+	u8		id_len;
+
+	/* The size listed here is what works with SPINOR_OP_SE, which isn't
+	 * necessarily called a "sector" by the vendor.
+	 */
+	unsigned	sector_size;
+	u16		n_sectors;
+
+	u16		page_size;
+	u16		addr_width;
+
+	u16		flags;
+#define	SECT_4K			0x01	/* SPINOR_OP_BE_4K works uniformly */
+#define	SPI_NOR_NO_ERASE	0x02	/* No erase command needed */
+#define	SST_WRITE		0x04	/* use SST byte programming */
+#define	SPI_NOR_NO_FR		0x08	/* Can't do fastread */
+#define	SECT_4K_PMC		0x10	/* SPINOR_OP_BE_4K_PMC works uniformly */
+#define	SPI_NOR_DUAL_READ	0x20    /* Flash supports Dual Read */
+#define	SPI_NOR_QUAD_READ	0x40    /* Flash supports Quad Read */
+#define	USE_FSR			0x80	/* use flag status register */
+};
+
+#define JEDEC_MFR(info)	((info)->id[0])
 
 static const struct spi_device_id *spi_nor_match_id(const char *name);
 
@@ -98,7 +129,7 @@ static inline int spi_nor_read_dummy_cycles(struct spi_nor *nor)
 	case SPI_NOR_FAST:
 	case SPI_NOR_DUAL:
 	case SPI_NOR_QUAD:
-		return 1;
+		return 8;
 	case SPI_NOR_NORMAL:
 		return 0;
 	}
@@ -138,13 +169,14 @@ static inline struct spi_nor *mtd_to_spi_nor(struct mtd_info *mtd)
 }
 
 /* Enable/disable 4-byte addressing mode. */
-static inline int set_4byte(struct spi_nor *nor, u32 jedec_id, int enable)
+static inline int set_4byte(struct spi_nor *nor, struct flash_info *info,
+			    int enable)
 {
 	int status;
 	bool need_wren = false;
 	u8 cmd;
 
-	switch (JEDEC_MFR(jedec_id)) {
+	switch (JEDEC_MFR(info)) {
 	case CFI_MFR_ST: /* Micron, actually */
 		/* Some Micron need WREN command; all will accept it */
 		need_wren = true;
@@ -165,81 +197,74 @@ static inline int set_4byte(struct spi_nor *nor, u32 jedec_id, int enable)
 		return nor->write_reg(nor, SPINOR_OP_BRWR, nor->cmd_buf, 1, 0);
 	}
 }
-
-static int spi_nor_wait_till_ready(struct spi_nor *nor)
+static inline int spi_nor_sr_ready(struct spi_nor *nor)
 {
-	unsigned long deadline;
-	int sr;
-
-	deadline = jiffies + MAX_READY_WAIT_JIFFIES;
-
-	do {
-		cond_resched();
+	int sr = read_sr(nor);
+	if (sr < 0)
+		return sr;
+	else
+		return !(sr & SR_WIP);
+}
 
-		sr = read_sr(nor);
-		if (sr < 0)
-			break;
-		else if (!(sr & SR_WIP))
-			return 0;
-	} while (!time_after_eq(jiffies, deadline));
+static inline int spi_nor_fsr_ready(struct spi_nor *nor)
+{
+	int fsr = read_fsr(nor);
+	if (fsr < 0)
+		return fsr;
+	else
+		return fsr & FSR_READY;
+}
 
-	return -ETIMEDOUT;
+static int spi_nor_ready(struct spi_nor *nor)
+{
+	int sr, fsr;
+	sr = spi_nor_sr_ready(nor);
+	if (sr < 0)
+		return sr;
+	fsr = nor->flags & SNOR_F_USE_FSR ? spi_nor_fsr_ready(nor) : 1;
+	if (fsr < 0)
+		return fsr;
+	return sr && fsr;
 }
 
-static int spi_nor_wait_till_fsr_ready(struct spi_nor *nor)
+/*
+ * Service routine to read status register until ready, or timeout occurs.
+ * Returns non-zero if error.
+ */
+static int spi_nor_wait_till_ready(struct spi_nor *nor)
 {
 	unsigned long deadline;
-	int sr;
-	int fsr;
+	int timeout = 0, ret;
 
 	deadline = jiffies + MAX_READY_WAIT_JIFFIES;
 
-	do {
+	while (!timeout) {
+		if (time_after_eq(jiffies, deadline))
+			timeout = 1;
+
+		ret = spi_nor_ready(nor);
+		if (ret < 0)
+			return ret;
+		if (ret)
+			return 0;
+
 		cond_resched();
+	}
 
-		sr = read_sr(nor);
-		if (sr < 0) {
-			break;
-		} else if (!(sr & SR_WIP)) {
-			fsr = read_fsr(nor);
-			if (fsr < 0)
-				break;
-			if (fsr & FSR_READY)
-				return 0;
-		}
-	} while (!time_after_eq(jiffies, deadline));
+	dev_err(nor->dev, "flash operation timed out\n");
 
 	return -ETIMEDOUT;
 }
 
 /*
- * Service routine to read status register until ready, or timeout occurs.
- * Returns non-zero if error.
- */
-static int wait_till_ready(struct spi_nor *nor)
-{
-	return nor->wait_till_ready(nor);
-}
-
-/*
  * Erase the whole flash memory
  *
  * Returns 0 if successful, non-zero otherwise.
  */
 static int erase_chip(struct spi_nor *nor)
 {
-	int ret;
-
 	dev_dbg(nor->dev, " %lldKiB\n", (long long)(nor->mtd->size >> 10));
 
-	/* Wait until finished previous write command. */
-	ret = wait_till_ready(nor);
-	if (ret)
-		return ret;
-
-	/* Send write enable, then erase commands. */
-	write_enable(nor);
-
 	return nor->write_reg(nor, SPINOR_OP_CHIP_ERASE, NULL, 0, 0);
 }
 
@@ -294,11 +319,17 @@ static int spi_nor_erase(struct mtd_info *mtd, struct erase_info *instr)
 
 	/* whole-chip erase? */
 	if (len == mtd->size) {
+		write_enable(nor);
+
 		if (erase_chip(nor)) {
 			ret = -EIO;
 			goto erase_err;
 		}
 
+		ret = spi_nor_wait_till_ready(nor);
+		if (ret)
+			goto erase_err;
+
 	/* REVISIT in some cases we could speed up erasing large regions
 	 * by using SPINOR_OP_SE instead of SPINOR_OP_BE_4K.  We may have set up
 	 * to use "small sector erase", but that's not always optimal.
@@ -307,6 +338,8 @@ static int spi_nor_erase(struct mtd_info *mtd, struct erase_info *instr)
 	/* "sector"-at-a-time erase */
 	} else {
 		while (len) {
+			write_enable(nor);
+
 			if (nor->erase(nor, addr)) {
 				ret = -EIO;
 				goto erase_err;
@@ -314,9 +347,15 @@ static int spi_nor_erase(struct mtd_info *mtd, struct erase_info *instr)
 
 			addr += mtd->erasesize;
 			len -= mtd->erasesize;
+
+			ret = spi_nor_wait_till_ready(nor);
+			if (ret)
+				goto erase_err;
 		}
 	}
 
+	write_disable(nor);
+
 	spi_nor_unlock_and_unprep(nor, SPI_NOR_OPS_ERASE);
 
 	instr->state = MTD_ERASE_DONE;
@@ -341,11 +380,6 @@ static int spi_nor_lock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 	if (ret)
 		return ret;
 
-	/* Wait until finished previous command */
-	ret = wait_till_ready(nor);
-	if (ret)
-		goto err;
-
 	status_old = read_sr(nor);
 
 	if (offset < mtd->size - (mtd->size / 2))
@@ -388,11 +422,6 @@ static int spi_nor_unlock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 	if (ret)
 		return ret;
 
-	/* Wait until finished previous command */
-	ret = wait_till_ready(nor);
-	if (ret)
-		goto err;
-
 	status_old = read_sr(nor);
 
 	if (offset+len > mtd->size - (mtd->size / 64))
@@ -424,38 +453,34 @@ err:
 	return ret;
 }
 
-struct flash_info {
-	/* JEDEC id zero means "no ID" (most older chips); otherwise it has
-	 * a high byte of zero plus three data bytes: the manufacturer id,
-	 * then a two byte device id.
-	 */
-	u32		jedec_id;
-	u16             ext_id;
-
-	/* The size listed here is what works with SPINOR_OP_SE, which isn't
-	 * necessarily called a "sector" by the vendor.
-	 */
-	unsigned	sector_size;
-	u16		n_sectors;
-
-	u16		page_size;
-	u16		addr_width;
-
-	u16		flags;
-#define	SECT_4K			0x01	/* SPINOR_OP_BE_4K works uniformly */
-#define	SPI_NOR_NO_ERASE	0x02	/* No erase command needed */
-#define	SST_WRITE		0x04	/* use SST byte programming */
-#define	SPI_NOR_NO_FR		0x08	/* Can't do fastread */
-#define	SECT_4K_PMC		0x10	/* SPINOR_OP_BE_4K_PMC works uniformly */
-#define	SPI_NOR_DUAL_READ	0x20    /* Flash supports Dual Read */
-#define	SPI_NOR_QUAD_READ	0x40    /* Flash supports Quad Read */
-#define	USE_FSR			0x80	/* use flag status register */
-};
-
+/* Used when the "_ext_id" is two bytes at most */
 #define INFO(_jedec_id, _ext_id, _sector_size, _n_sectors, _flags)	\
 	((kernel_ulong_t)&(struct flash_info) {				\
-		.jedec_id = (_jedec_id),				\
-		.ext_id = (_ext_id),					\
+		.id = {							\
+			((_jedec_id) >> 16) & 0xff,			\
+			((_jedec_id) >> 8) & 0xff,			\
+			(_jedec_id) & 0xff,				\
+			((_ext_id) >> 8) & 0xff,			\
+			(_ext_id) & 0xff,				\
+			},						\
+		.id_len = (!(_jedec_id) ? 0 : (3 + ((_ext_id) ? 2 : 0))),	\
+		.sector_size = (_sector_size),				\
+		.n_sectors = (_n_sectors),				\
+		.page_size = 256,					\
+		.flags = (_flags),					\
+	})
+
+#define INFO6(_jedec_id, _ext_id, _sector_size, _n_sectors, _flags)	\
+	((kernel_ulong_t)&(struct flash_info) {				\
+		.id = {							\
+			((_jedec_id) >> 16) & 0xff,			\
+			((_jedec_id) >> 8) & 0xff,			\
+			(_jedec_id) & 0xff,				\
+			((_ext_id) >> 16) & 0xff,			\
+			((_ext_id) >> 8) & 0xff,			\
+			(_ext_id) & 0xff,				\
+			},						\
+		.id_len = 6,						\
 		.sector_size = (_sector_size),				\
 		.n_sectors = (_n_sectors),				\
 		.page_size = 256,					\
@@ -507,6 +532,9 @@ static const struct spi_device_id spi_nor_ids[] = {
 	{ "mr25h256", CAT25_INFO( 32 * 1024, 1, 256, 2, SPI_NOR_NO_ERASE | SPI_NOR_NO_FR) },
 	{ "mr25h10",  CAT25_INFO(128 * 1024, 1, 256, 3, SPI_NOR_NO_ERASE | SPI_NOR_NO_FR) },
 
+	/* Fujitsu */
+	{ "mb85rs1mt", INFO(0x047f27, 0, 128 * 1024, 1, SPI_NOR_NO_ERASE) },
+
 	/* GigaDevice */
 	{ "gd25q32", INFO(0xc84016, 0, 64 * 1024,  64, SECT_4K) },
 	{ "gd25q64", INFO(0xc84017, 0, 64 * 1024, 128, SECT_4K) },
@@ -532,6 +560,7 @@ static const struct spi_device_id spi_nor_ids[] = {
 	{ "mx66l1g55g",  INFO(0xc2261b, 0, 64 * 1024, 2048, SPI_NOR_QUAD_READ) },
 
 	/* Micron */
+	{ "n25q032",	 INFO(0x20ba16, 0, 64 * 1024,   64, 0) },
 	{ "n25q064",     INFO(0x20ba17, 0, 64 * 1024,  128, 0) },
 	{ "n25q128a11",  INFO(0x20bb18, 0, 64 * 1024,  256, 0) },
 	{ "n25q128a13",  INFO(0x20ba18, 0, 64 * 1024,  256, 0) },
@@ -556,6 +585,7 @@ static const struct spi_device_id spi_nor_ids[] = {
 	{ "s70fl01gs",  INFO(0x010221, 0x4d00, 256 * 1024, 256, 0) },
 	{ "s25sl12800", INFO(0x012018, 0x0300, 256 * 1024,  64, 0) },
 	{ "s25sl12801", INFO(0x012018, 0x0301,  64 * 1024, 256, 0) },
+	{ "s25fl128s",	INFO6(0x012018, 0x4d0180, 64 * 1024, 256, SPI_NOR_QUAD_READ) },
 	{ "s25fl129p0", INFO(0x012018, 0x4d00, 256 * 1024,  64, 0) },
 	{ "s25fl129p1", INFO(0x012018, 0x4d01,  64 * 1024, 256, 0) },
 	{ "s25sl004a",  INFO(0x010212,      0,  64 * 1024,   8, 0) },
@@ -566,6 +596,7 @@ static const struct spi_device_id spi_nor_ids[] = {
 	{ "s25fl008k",  INFO(0xef4014,      0,  64 * 1024,  16, SECT_4K) },
 	{ "s25fl016k",  INFO(0xef4015,      0,  64 * 1024,  32, SECT_4K) },
 	{ "s25fl064k",  INFO(0xef4017,      0,  64 * 1024, 128, SECT_4K) },
+	{ "s25fl132k",  INFO(0x014016,      0,  64 * 1024,  64, 0) },
 
 	/* SST -- large erase sizes are "overlays", "sectors" are 4K */
 	{ "sst25vf040b", INFO(0xbf258d, 0, 64 * 1024,  8, SECT_4K | SST_WRITE) },
@@ -577,6 +608,7 @@ static const struct spi_device_id spi_nor_ids[] = {
 	{ "sst25wf010",  INFO(0xbf2502, 0, 64 * 1024,  2, SECT_4K | SST_WRITE) },
 	{ "sst25wf020",  INFO(0xbf2503, 0, 64 * 1024,  4, SECT_4K | SST_WRITE) },
 	{ "sst25wf040",  INFO(0xbf2504, 0, 64 * 1024,  8, SECT_4K | SST_WRITE) },
+	{ "sst25wf080",  INFO(0xbf2505, 0, 64 * 1024, 16, SECT_4K | SST_WRITE) },
 
 	/* ST Microelectronics -- newer production may have feature updates */
 	{ "m25p05",  INFO(0x202010,  0,  32 * 1024,   2, 0) },
@@ -588,7 +620,6 @@ static const struct spi_device_id spi_nor_ids[] = {
 	{ "m25p32",  INFO(0x202016,  0,  64 * 1024,  64, 0) },
 	{ "m25p64",  INFO(0x202017,  0,  64 * 1024, 128, 0) },
 	{ "m25p128", INFO(0x202018,  0, 256 * 1024,  64, 0) },
-	{ "n25q032", INFO(0x20ba16,  0,  64 * 1024,  64, 0) },
 
 	{ "m25p05-nonjedec",  INFO(0, 0,  32 * 1024,   2, 0) },
 	{ "m25p10-nonjedec",  INFO(0, 0,  32 * 1024,   4, 0) },
@@ -643,32 +674,24 @@ static const struct spi_device_id spi_nor_ids[] = {
 static const struct spi_device_id *spi_nor_read_id(struct spi_nor *nor)
 {
 	int			tmp;
-	u8			id[5];
-	u32			jedec;
-	u16                     ext_jedec;
+	u8			id[SPI_NOR_MAX_ID_LEN];
 	struct flash_info	*info;
 
-	tmp = nor->read_reg(nor, SPINOR_OP_RDID, id, 5);
+	tmp = nor->read_reg(nor, SPINOR_OP_RDID, id, SPI_NOR_MAX_ID_LEN);
 	if (tmp < 0) {
 		dev_dbg(nor->dev, " error %d reading JEDEC ID\n", tmp);
 		return ERR_PTR(tmp);
 	}
-	jedec = id[0];
-	jedec = jedec << 8;
-	jedec |= id[1];
-	jedec = jedec << 8;
-	jedec |= id[2];
-
-	ext_jedec = id[3] << 8 | id[4];
 
 	for (tmp = 0; tmp < ARRAY_SIZE(spi_nor_ids) - 1; tmp++) {
 		info = (void *)spi_nor_ids[tmp].driver_data;
-		if (info->jedec_id == jedec) {
-			if (info->ext_id == 0 || info->ext_id == ext_jedec)
+		if (info->id_len) {
+			if (!memcmp(info->id, id, info->id_len))
 				return &spi_nor_ids[tmp];
 		}
 	}
-	dev_err(nor->dev, "unrecognized JEDEC id %06x\n", jedec);
+	dev_err(nor->dev, "unrecognized JEDEC id bytes: %02x, %2x, %2x\n",
+		id[0], id[1], id[2]);
 	return ERR_PTR(-ENODEV);
 }
 
@@ -703,11 +726,6 @@ static int sst_write(struct mtd_info *mtd, loff_t to, size_t len,
 	if (ret)
 		return ret;
 
-	/* Wait until finished previous write command. */
-	ret = wait_till_ready(nor);
-	if (ret)
-		goto time_out;
-
 	write_enable(nor);
 
 	nor->sst_write_second = false;
@@ -719,7 +737,7 @@ static int sst_write(struct mtd_info *mtd, loff_t to, size_t len,
 
 		/* write one byte. */
 		nor->write(nor, to, 1, retlen, buf);
-		ret = wait_till_ready(nor);
+		ret = spi_nor_wait_till_ready(nor);
 		if (ret)
 			goto time_out;
 	}
@@ -731,7 +749,7 @@ static int sst_write(struct mtd_info *mtd, loff_t to, size_t len,
 
 		/* write two bytes. */
 		nor->write(nor, to, 2, retlen, buf + actual);
-		ret = wait_till_ready(nor);
+		ret = spi_nor_wait_till_ready(nor);
 		if (ret)
 			goto time_out;
 		to += 2;
@@ -740,7 +758,7 @@ static int sst_write(struct mtd_info *mtd, loff_t to, size_t len,
 	nor->sst_write_second = false;
 
 	write_disable(nor);
-	ret = wait_till_ready(nor);
+	ret = spi_nor_wait_till_ready(nor);
 	if (ret)
 		goto time_out;
 
@@ -751,7 +769,7 @@ static int sst_write(struct mtd_info *mtd, loff_t to, size_t len,
 		nor->program_opcode = SPINOR_OP_BP;
 		nor->write(nor, to, 1, retlen, buf + actual);
 
-		ret = wait_till_ready(nor);
+		ret = spi_nor_wait_till_ready(nor);
 		if (ret)
 			goto time_out;
 		write_disable(nor);
@@ -779,11 +797,6 @@ static int spi_nor_write(struct mtd_info *mtd, loff_t to, size_t len,
 	if (ret)
 		return ret;
 
-	/* Wait until finished previous write command. */
-	ret = wait_till_ready(nor);
-	if (ret)
-		goto write_err;
-
 	write_enable(nor);
 
 	page_offset = to & (nor->page_size - 1);
@@ -802,16 +815,20 @@ static int spi_nor_write(struct mtd_info *mtd, loff_t to, size_t len,
 			if (page_size > nor->page_size)
 				page_size = nor->page_size;
 
-			wait_till_ready(nor);
+			ret = spi_nor_wait_till_ready(nor);
+			if (ret)
+				goto write_err;
+
 			write_enable(nor);
 
 			nor->write(nor, to + i, page_size, retlen, buf + i);
 		}
 	}
 
+	ret = spi_nor_wait_till_ready(nor);
 write_err:
 	spi_nor_unlock_and_unprep(nor, SPI_NOR_OPS_WRITE);
-	return 0;
+	return ret;
 }
 
 static int macronix_quad_enable(struct spi_nor *nor)
@@ -824,7 +841,7 @@ static int macronix_quad_enable(struct spi_nor *nor)
 	nor->cmd_buf[0] = val | SR_QUAD_EN_MX;
 	nor->write_reg(nor, SPINOR_OP_WRSR, nor->cmd_buf, 1, 0);
 
-	if (wait_till_ready(nor))
+	if (spi_nor_wait_till_ready(nor))
 		return 1;
 
 	ret = read_sr(nor);
@@ -874,11 +891,11 @@ static int spansion_quad_enable(struct spi_nor *nor)
 	return 0;
 }
 
-static int set_quad_mode(struct spi_nor *nor, u32 jedec_id)
+static int set_quad_mode(struct spi_nor *nor, struct flash_info *info)
 {
 	int status;
 
-	switch (JEDEC_MFR(jedec_id)) {
+	switch (JEDEC_MFR(info)) {
 	case CFI_MFR_MACRONIX:
 		status = macronix_quad_enable(nor);
 		if (status) {
@@ -904,11 +921,6 @@ static int spi_nor_check(struct spi_nor *nor)
 		return -EINVAL;
 	}
 
-	if (!nor->read_id)
-		nor->read_id = spi_nor_read_id;
-	if (!nor->wait_till_ready)
-		nor->wait_till_ready = spi_nor_wait_till_ready;
-
 	return 0;
 }
 
@@ -926,16 +938,24 @@ int spi_nor_scan(struct spi_nor *nor, const char *name, enum read_mode mode)
 	if (ret)
 		return ret;
 
-	id = spi_nor_match_id(name);
-	if (!id)
+	/* Try to auto-detect if chip name wasn't specified */
+	if (!name)
+		id = spi_nor_read_id(nor);
+	else
+		id = spi_nor_match_id(name);
+	if (IS_ERR_OR_NULL(id))
 		return -ENOENT;
 
 	info = (void *)id->driver_data;
 
-	if (info->jedec_id) {
+	/*
+	 * If caller has specified name of flash model that can normally be
+	 * detected using JEDEC, let's verify it.
+	 */
+	if (name && info->id_len) {
 		const struct spi_device_id *jid;
 
-		jid = nor->read_id(nor);
+		jid = spi_nor_read_id(nor);
 		if (IS_ERR(jid)) {
 			return PTR_ERR(jid);
 		} else if (jid != id) {
@@ -960,9 +980,9 @@ int spi_nor_scan(struct spi_nor *nor, const char *name, enum read_mode mode)
 	 * up with the software protection bits set
 	 */
 
-	if (JEDEC_MFR(info->jedec_id) == CFI_MFR_ATMEL ||
-	    JEDEC_MFR(info->jedec_id) == CFI_MFR_INTEL ||
-	    JEDEC_MFR(info->jedec_id) == CFI_MFR_SST) {
+	if (JEDEC_MFR(info) == CFI_MFR_ATMEL ||
+	    JEDEC_MFR(info) == CFI_MFR_INTEL ||
+	    JEDEC_MFR(info) == CFI_MFR_SST) {
 		write_enable(nor);
 		write_sr(nor, 0);
 	}
@@ -977,7 +997,7 @@ int spi_nor_scan(struct spi_nor *nor, const char *name, enum read_mode mode)
 	mtd->_read = spi_nor_read;
 
 	/* nor protection support for STmicro chips */
-	if (JEDEC_MFR(info->jedec_id) == CFI_MFR_ST) {
+	if (JEDEC_MFR(info) == CFI_MFR_ST) {
 		mtd->_lock = spi_nor_lock;
 		mtd->_unlock = spi_nor_unlock;
 	}
@@ -988,9 +1008,8 @@ int spi_nor_scan(struct spi_nor *nor, const char *name, enum read_mode mode)
 	else
 		mtd->_write = spi_nor_write;
 
-	if ((info->flags & USE_FSR) &&
-	    nor->wait_till_ready == spi_nor_wait_till_ready)
-		nor->wait_till_ready = spi_nor_wait_till_fsr_ready;
+	if (info->flags & USE_FSR)
+		nor->flags |= SNOR_F_USE_FSR;
 
 #ifdef CONFIG_MTD_SPI_NOR_USE_4K_SECTORS
 	/* prefer "small sector" erase if possible */
@@ -1031,7 +1050,7 @@ int spi_nor_scan(struct spi_nor *nor, const char *name, enum read_mode mode)
 
 	/* Quad/Dual-read mode takes precedence over fast/normal */
 	if (mode == SPI_NOR_QUAD && info->flags & SPI_NOR_QUAD_READ) {
-		ret = set_quad_mode(nor, info->jedec_id);
+		ret = set_quad_mode(nor, info);
 		if (ret) {
 			dev_err(dev, "quad mode not supported\n");
 			return ret;
@@ -1067,7 +1086,7 @@ int spi_nor_scan(struct spi_nor *nor, const char *name, enum read_mode mode)
 	else if (mtd->size > 0x1000000) {
 		/* enable 4-byte addressing if the device exceeds 16MiB */
 		nor->addr_width = 4;
-		if (JEDEC_MFR(info->jedec_id) == CFI_MFR_AMD) {
+		if (JEDEC_MFR(info) == CFI_MFR_AMD) {
 			/* Dedicated 4-byte command set */
 			switch (nor->flash_read) {
 			case SPI_NOR_QUAD:
@@ -1088,7 +1107,7 @@ int spi_nor_scan(struct spi_nor *nor, const char *name, enum read_mode mode)
 			nor->erase_opcode = SPINOR_OP_SE_4B;
 			mtd->erasesize = info->sector_size;
 		} else
-			set_4byte(nor, info->jedec_id, 1);
+			set_4byte(nor, info, 1);
 	} else {
 		nor->addr_width = 3;
 	}
diff --git a/drivers/mtd/tests/oobtest.c b/drivers/mtd/tests/oobtest.c
index dc4f9602b97e..5e061186eab1 100644
--- a/drivers/mtd/tests/oobtest.c
+++ b/drivers/mtd/tests/oobtest.c
@@ -34,8 +34,11 @@
 #include "mtd_test.h"
 
 static int dev = -EINVAL;
+static int bitflip_limit;
 module_param(dev, int, S_IRUGO);
 MODULE_PARM_DESC(dev, "MTD device number to use");
+module_param(bitflip_limit, int, S_IRUGO);
+MODULE_PARM_DESC(bitflip_limit, "Max. allowed bitflips per page");
 
 static struct mtd_info *mtd;
 static unsigned char *readbuf;
@@ -115,12 +118,36 @@ static int write_whole_device(void)
 	return 0;
 }
 
+/*
+ * Display the address, offset and data bytes at comparison failure.
+ * Return number of bitflips encountered.
+ */
+static size_t memcmpshow(loff_t addr, const void *cs, const void *ct, size_t count)
+{
+	const unsigned char *su1, *su2;
+	int res;
+	size_t i = 0;
+	size_t bitflips = 0;
+
+	for (su1 = cs, su2 = ct; 0 < count; ++su1, ++su2, count--, i++) {
+		res = *su1 ^ *su2;
+		if (res) {
+			pr_info("error @addr[0x%lx:0x%zx] 0x%x -> 0x%x diff 0x%x\n",
+				(unsigned long)addr, i, *su1, *su2, res);
+			bitflips += hweight8(res);
+		}
+	}
+
+	return bitflips;
+}
+
 static int verify_eraseblock(int ebnum)
 {
 	int i;
 	struct mtd_oob_ops ops;
 	int err = 0;
 	loff_t addr = (loff_t)ebnum * mtd->erasesize;
+	size_t bitflips;
 
 	prandom_bytes_state(&rnd_state, writebuf, use_len_max * pgcnt);
 	for (i = 0; i < pgcnt; ++i, addr += mtd->writesize) {
@@ -139,8 +166,11 @@ static int verify_eraseblock(int ebnum)
 			errcnt += 1;
 			return err ? err : -1;
 		}
-		if (memcmp(readbuf, writebuf + (use_len_max * i) + use_offset,
-			   use_len)) {
+
+		bitflips = memcmpshow(addr, readbuf,
+				      writebuf + (use_len_max * i) + use_offset,
+				      use_len);
+		if (bitflips > bitflip_limit) {
 			pr_err("error: verify failed at %#llx\n",
 			       (long long)addr);
 			errcnt += 1;
@@ -148,7 +178,10 @@ static int verify_eraseblock(int ebnum)
 				pr_err("error: too many errors\n");
 				return -1;
 			}
+		} else if (bitflips) {
+			pr_info("ignoring error as within bitflip_limit\n");
 		}
+
 		if (use_offset != 0 || use_len < mtd->ecclayout->oobavail) {
 			int k;
 
@@ -167,9 +200,10 @@ static int verify_eraseblock(int ebnum)
 				errcnt += 1;
 				return err ? err : -1;
 			}
-			if (memcmp(readbuf + use_offset,
-				   writebuf + (use_len_max * i) + use_offset,
-				   use_len)) {
+			bitflips = memcmpshow(addr, readbuf + use_offset,
+					      writebuf + (use_len_max * i) + use_offset,
+					      use_len);
+			if (bitflips > bitflip_limit) {
 				pr_err("error: verify failed at %#llx\n",
 						(long long)addr);
 				errcnt += 1;
@@ -177,7 +211,10 @@ static int verify_eraseblock(int ebnum)
 					pr_err("error: too many errors\n");
 					return -1;
 				}
+			} else if (bitflips) {
+				pr_info("ignoring error as within bitflip_limit\n");
 			}
+
 			for (k = 0; k < use_offset; ++k)
 				if (readbuf[k] != 0xff) {
 					pr_err("error: verify 0xff "
@@ -216,6 +253,9 @@ static int verify_eraseblock_in_one_go(int ebnum)
 	int err = 0;
 	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 	size_t len = mtd->ecclayout->oobavail * pgcnt;
+	size_t oobavail = mtd->ecclayout->oobavail;
+	size_t bitflips;
+	int i;
 
 	prandom_bytes_state(&rnd_state, writebuf, len);
 	ops.mode      = MTD_OPS_AUTO_OOB;
@@ -226,6 +266,8 @@ static int verify_eraseblock_in_one_go(int ebnum)
 	ops.ooboffs   = 0;
 	ops.datbuf    = NULL;
 	ops.oobbuf    = readbuf;
+
+	/* read entire block's OOB at one go */
 	err = mtd_read_oob(mtd, addr, &ops);
 	if (err || ops.oobretlen != len) {
 		pr_err("error: readoob failed at %#llx\n",
@@ -233,13 +275,21 @@ static int verify_eraseblock_in_one_go(int ebnum)
 		errcnt += 1;
 		return err ? err : -1;
 	}
-	if (memcmp(readbuf, writebuf, len)) {
-		pr_err("error: verify failed at %#llx\n",
-		       (long long)addr);
-		errcnt += 1;
-		if (errcnt > 1000) {
-			pr_err("error: too many errors\n");
-			return -1;
+
+	/* verify one page OOB at a time for bitflip per page limit check */
+	for (i = 0; i < pgcnt; ++i, addr += mtd->writesize) {
+		bitflips = memcmpshow(addr, readbuf + (i * oobavail),
+				      writebuf + (i * oobavail), oobavail);
+		if (bitflips > bitflip_limit) {
+			pr_err("error: verify failed at %#llx\n",
+			       (long long)addr);
+			errcnt += 1;
+			if (errcnt > 1000) {
+				pr_err("error: too many errors\n");
+				return -1;
+			}
+		} else if (bitflips) {
+			pr_info("ignoring error as within bitflip_limit\n");
 		}
 	}
 
@@ -610,7 +660,8 @@ static int __init mtd_oobtest_init(void)
 		err = mtd_read_oob(mtd, addr, &ops);
 		if (err)
 			goto out;
-		if (memcmp(readbuf, writebuf, mtd->ecclayout->oobavail * 2)) {
+		if (memcmpshow(addr, readbuf, writebuf,
+			       mtd->ecclayout->oobavail * 2)) {
 			pr_err("error: verify failed at %#llx\n",
 			       (long long)addr);
 			errcnt += 1;
diff --git a/drivers/mtd/tests/torturetest.c b/drivers/mtd/tests/torturetest.c
index eeab96973cf0..b55bc52a1340 100644
--- a/drivers/mtd/tests/torturetest.c
+++ b/drivers/mtd/tests/torturetest.c
@@ -264,7 +264,9 @@ static int __init tort_init(void)
 		int i;
 		void *patt;
 
-		mtdtest_erase_good_eraseblocks(mtd, bad_ebs, eb, ebcnt);
+		err = mtdtest_erase_good_eraseblocks(mtd, bad_ebs, eb, ebcnt);
+		if (err)
+			goto out;
 
 		/* Check if the eraseblocks contain only 0xFF bytes */
 		if (check) {
diff --git a/drivers/mtd/ubi/attach.c b/drivers/mtd/ubi/attach.c
index 6f27d9a1be3b..9d2e16f3150a 100644
--- a/drivers/mtd/ubi/attach.c
+++ b/drivers/mtd/ubi/attach.c
@@ -176,6 +176,7 @@ static int add_corrupted(struct ubi_attach_info *ai, int pnum, int ec)
 
 /**
  * validate_vid_hdr - check volume identifier header.
+ * @ubi: UBI device description object
  * @vid_hdr: the volume identifier header to check
  * @av: information about the volume this logical eraseblock belongs to
  * @pnum: physical eraseblock number the VID header came from
@@ -188,7 +189,8 @@ static int add_corrupted(struct ubi_attach_info *ai, int pnum, int ec)
  * information in the VID header is consistent to the information in other VID
  * headers of the same volume.
  */
-static int validate_vid_hdr(const struct ubi_vid_hdr *vid_hdr,
+static int validate_vid_hdr(const struct ubi_device *ubi,
+			    const struct ubi_vid_hdr *vid_hdr,
 			    const struct ubi_ainf_volume *av, int pnum)
 {
 	int vol_type = vid_hdr->vol_type;
@@ -206,7 +208,7 @@ static int validate_vid_hdr(const struct ubi_vid_hdr *vid_hdr,
 		 */
 
 		if (vol_id != av->vol_id) {
-			ubi_err("inconsistent vol_id");
+			ubi_err(ubi, "inconsistent vol_id");
 			goto bad;
 		}
 
@@ -216,17 +218,17 @@ static int validate_vid_hdr(const struct ubi_vid_hdr *vid_hdr,
 			av_vol_type = UBI_VID_DYNAMIC;
 
 		if (vol_type != av_vol_type) {
-			ubi_err("inconsistent vol_type");
+			ubi_err(ubi, "inconsistent vol_type");
 			goto bad;
 		}
 
 		if (used_ebs != av->used_ebs) {
-			ubi_err("inconsistent used_ebs");
+			ubi_err(ubi, "inconsistent used_ebs");
 			goto bad;
 		}
 
 		if (data_pad != av->data_pad) {
-			ubi_err("inconsistent data_pad");
+			ubi_err(ubi, "inconsistent data_pad");
 			goto bad;
 		}
 	}
@@ -234,7 +236,7 @@ static int validate_vid_hdr(const struct ubi_vid_hdr *vid_hdr,
 	return 0;
 
 bad:
-	ubi_err("inconsistent VID header at PEB %d", pnum);
+	ubi_err(ubi, "inconsistent VID header at PEB %d", pnum);
 	ubi_dump_vid_hdr(vid_hdr);
 	ubi_dump_av(av);
 	return -EINVAL;
@@ -336,7 +338,7 @@ int ubi_compare_lebs(struct ubi_device *ubi, const struct ubi_ainf_peb *aeb,
 		 * support these images anymore. Well, those images still work,
 		 * but only if no unclean reboots happened.
 		 */
-		ubi_err("unsupported on-flash UBI format");
+		ubi_err(ubi, "unsupported on-flash UBI format");
 		return -EINVAL;
 	}
 
@@ -377,7 +379,7 @@ int ubi_compare_lebs(struct ubi_device *ubi, const struct ubi_ainf_peb *aeb,
 			if (err == UBI_IO_BITFLIPS)
 				bitflips = 1;
 			else {
-				ubi_err("VID of PEB %d header is bad, but it was OK earlier, err %d",
+				ubi_err(ubi, "VID of PEB %d header is bad, but it was OK earlier, err %d",
 					pnum, err);
 				if (err > 0)
 					err = -EIO;
@@ -507,7 +509,7 @@ int ubi_add_to_av(struct ubi_device *ubi, struct ubi_attach_info *ai, int pnum,
 		 * logical eraseblocks because there was an unclean reboot.
 		 */
 		if (aeb->sqnum == sqnum && sqnum != 0) {
-			ubi_err("two LEBs with same sequence number %llu",
+			ubi_err(ubi, "two LEBs with same sequence number %llu",
 				sqnum);
 			ubi_dump_aeb(aeb, 0);
 			ubi_dump_vid_hdr(vid_hdr);
@@ -527,7 +529,7 @@ int ubi_add_to_av(struct ubi_device *ubi, struct ubi_attach_info *ai, int pnum,
 			 * This logical eraseblock is newer than the one
 			 * found earlier.
 			 */
-			err = validate_vid_hdr(vid_hdr, av, pnum);
+			err = validate_vid_hdr(ubi, vid_hdr, av, pnum);
 			if (err)
 				return err;
 
@@ -565,7 +567,7 @@ int ubi_add_to_av(struct ubi_device *ubi, struct ubi_attach_info *ai, int pnum,
 	 * attaching information.
 	 */
 
-	err = validate_vid_hdr(vid_hdr, av, pnum);
+	err = validate_vid_hdr(ubi, vid_hdr, av, pnum);
 	if (err)
 		return err;
 
@@ -668,7 +670,8 @@ static int early_erase_peb(struct ubi_device *ubi,
 		 * Erase counter overflow. Upgrade UBI and use 64-bit
 		 * erase counters internally.
 		 */
-		ubi_err("erase counter overflow at PEB %d, EC %d", pnum, ec);
+		ubi_err(ubi, "erase counter overflow at PEB %d, EC %d",
+			pnum, ec);
 		return -EINVAL;
 	}
 
@@ -736,7 +739,7 @@ struct ubi_ainf_peb *ubi_early_get_peb(struct ubi_device *ubi,
 		return aeb;
 	}
 
-	ubi_err("no free eraseblocks");
+	ubi_err(ubi, "no free eraseblocks");
 	return ERR_PTR(-ENOSPC);
 }
 
@@ -785,9 +788,9 @@ static int check_corruption(struct ubi_device *ubi, struct ubi_vid_hdr *vid_hdr,
 	if (ubi_check_pattern(ubi->peb_buf, 0xFF, ubi->leb_size))
 		goto out_unlock;
 
-	ubi_err("PEB %d contains corrupted VID header, and the data does not contain all 0xFF",
+	ubi_err(ubi, "PEB %d contains corrupted VID header, and the data does not contain all 0xFF",
 		pnum);
-	ubi_err("this may be a non-UBI PEB or a severe VID header corruption which requires manual inspection");
+	ubi_err(ubi, "this may be a non-UBI PEB or a severe VID header corruption which requires manual inspection");
 	ubi_dump_vid_hdr(vid_hdr);
 	pr_err("hexdump of PEB %d offset %d, length %d",
 	       pnum, ubi->leb_start, ubi->leb_size);
@@ -859,7 +862,8 @@ static int scan_peb(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		bitflips = 1;
 		break;
 	default:
-		ubi_err("'ubi_io_read_ec_hdr()' returned unknown code %d", err);
+		ubi_err(ubi, "'ubi_io_read_ec_hdr()' returned unknown code %d",
+			err);
 		return -EINVAL;
 	}
 
@@ -868,7 +872,7 @@ static int scan_peb(struct ubi_device *ubi, struct ubi_attach_info *ai,
 
 		/* Make sure UBI version is OK */
 		if (ech->version != UBI_VERSION) {
-			ubi_err("this UBI version is %d, image version is %d",
+			ubi_err(ubi, "this UBI version is %d, image version is %d",
 				UBI_VERSION, (int)ech->version);
 			return -EINVAL;
 		}
@@ -882,7 +886,7 @@ static int scan_peb(struct ubi_device *ubi, struct ubi_attach_info *ai,
 			 * flash. Upgrade UBI and use 64-bit erase counters
 			 * internally.
 			 */
-			ubi_err("erase counter overflow, max is %d",
+			ubi_err(ubi, "erase counter overflow, max is %d",
 				UBI_MAX_ERASECOUNTER);
 			ubi_dump_ec_hdr(ech);
 			return -EINVAL;
@@ -903,7 +907,7 @@ static int scan_peb(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		if (!ubi->image_seq)
 			ubi->image_seq = image_seq;
 		if (image_seq && ubi->image_seq != image_seq) {
-			ubi_err("bad image sequence number %d in PEB %d, expected %d",
+			ubi_err(ubi, "bad image sequence number %d in PEB %d, expected %d",
 				image_seq, pnum, ubi->image_seq);
 			ubi_dump_ec_hdr(ech);
 			return -EINVAL;
@@ -981,7 +985,7 @@ static int scan_peb(struct ubi_device *ubi, struct ubi_attach_info *ai,
 			return err;
 		goto adjust_mean_ec;
 	default:
-		ubi_err("'ubi_io_read_vid_hdr()' returned unknown code %d",
+		ubi_err(ubi, "'ubi_io_read_vid_hdr()' returned unknown code %d",
 			err);
 		return -EINVAL;
 	}
@@ -999,7 +1003,7 @@ static int scan_peb(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		case UBI_COMPAT_DELETE:
 			if (vol_id != UBI_FM_SB_VOLUME_ID
 			    && vol_id != UBI_FM_DATA_VOLUME_ID) {
-				ubi_msg("\"delete\" compatible internal volume %d:%d found, will remove it",
+				ubi_msg(ubi, "\"delete\" compatible internal volume %d:%d found, will remove it",
 					vol_id, lnum);
 			}
 			err = add_to_list(ai, pnum, vol_id, lnum,
@@ -1009,13 +1013,13 @@ static int scan_peb(struct ubi_device *ubi, struct ubi_attach_info *ai,
 			return 0;
 
 		case UBI_COMPAT_RO:
-			ubi_msg("read-only compatible internal volume %d:%d found, switch to read-only mode",
+			ubi_msg(ubi, "read-only compatible internal volume %d:%d found, switch to read-only mode",
 				vol_id, lnum);
 			ubi->ro_mode = 1;
 			break;
 
 		case UBI_COMPAT_PRESERVE:
-			ubi_msg("\"preserve\" compatible internal volume %d:%d found",
+			ubi_msg(ubi, "\"preserve\" compatible internal volume %d:%d found",
 				vol_id, lnum);
 			err = add_to_list(ai, pnum, vol_id, lnum,
 					  ec, 0, &ai->alien);
@@ -1024,14 +1028,14 @@ static int scan_peb(struct ubi_device *ubi, struct ubi_attach_info *ai,
 			return 0;
 
 		case UBI_COMPAT_REJECT:
-			ubi_err("incompatible internal volume %d:%d found",
+			ubi_err(ubi, "incompatible internal volume %d:%d found",
 				vol_id, lnum);
 			return -EINVAL;
 		}
 	}
 
 	if (ec_err)
-		ubi_warn("valid VID header but corrupted EC header at PEB %d",
+		ubi_warn(ubi, "valid VID header but corrupted EC header at PEB %d",
 			 pnum);
 	err = ubi_add_to_av(ubi, ai, pnum, ec, vidh, bitflips);
 	if (err)
@@ -1075,7 +1079,7 @@ static int late_analysis(struct ubi_device *ubi, struct ubi_attach_info *ai)
 	 * with the flash HW or driver.
 	 */
 	if (ai->corr_peb_count) {
-		ubi_err("%d PEBs are corrupted and preserved",
+		ubi_err(ubi, "%d PEBs are corrupted and preserved",
 			ai->corr_peb_count);
 		pr_err("Corrupted PEBs are:");
 		list_for_each_entry(aeb, &ai->corr, u.list)
@@ -1087,7 +1091,7 @@ static int late_analysis(struct ubi_device *ubi, struct ubi_attach_info *ai)
 		 * otherwise, only print a warning.
 		 */
 		if (ai->corr_peb_count >= max_corr) {
-			ubi_err("too many corrupted PEBs, refusing");
+			ubi_err(ubi, "too many corrupted PEBs, refusing");
 			return -EINVAL;
 		}
 	}
@@ -1110,11 +1114,11 @@ static int late_analysis(struct ubi_device *ubi, struct ubi_attach_info *ai)
 		 */
 		if (ai->maybe_bad_peb_count <= 2) {
 			ai->is_empty = 1;
-			ubi_msg("empty MTD device detected");
+			ubi_msg(ubi, "empty MTD device detected");
 			get_random_bytes(&ubi->image_seq,
 					 sizeof(ubi->image_seq));
 		} else {
-			ubi_err("MTD device is not UBI-formatted and possibly contains non-UBI data - refusing it");
+			ubi_err(ubi, "MTD device is not UBI-formatted and possibly contains non-UBI data - refusing it");
 			return -EINVAL;
 		}
 
@@ -1248,7 +1252,7 @@ static int scan_all(struct ubi_device *ubi, struct ubi_attach_info *ai,
 			goto out_vidh;
 	}
 
-	ubi_msg("scanning is finished");
+	ubi_msg(ubi, "scanning is finished");
 
 	/* Calculate mean erase counter */
 	if (ai->ec_count)
@@ -1515,37 +1519,37 @@ static int self_check_ai(struct ubi_device *ubi, struct ubi_attach_info *ai)
 		vols_found += 1;
 
 		if (ai->is_empty) {
-			ubi_err("bad is_empty flag");
+			ubi_err(ubi, "bad is_empty flag");
 			goto bad_av;
 		}
 
 		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");
+			ubi_err(ubi, "negative values");
 			goto bad_av;
 		}
 
 		if (av->vol_id >= UBI_MAX_VOLUMES &&
 		    av->vol_id < UBI_INTERNAL_VOL_START) {
-			ubi_err("bad vol_id");
+			ubi_err(ubi, "bad vol_id");
 			goto bad_av;
 		}
 
 		if (av->vol_id > ai->highest_vol_id) {
-			ubi_err("highest_vol_id is %d, but vol_id %d is there",
+			ubi_err(ubi, "highest_vol_id is %d, but vol_id %d is there",
 				ai->highest_vol_id, av->vol_id);
 			goto out;
 		}
 
 		if (av->vol_type != UBI_DYNAMIC_VOLUME &&
 		    av->vol_type != UBI_STATIC_VOLUME) {
-			ubi_err("bad vol_type");
+			ubi_err(ubi, "bad vol_type");
 			goto bad_av;
 		}
 
 		if (av->data_pad > ubi->leb_size / 2) {
-			ubi_err("bad data_pad");
+			ubi_err(ubi, "bad data_pad");
 			goto bad_av;
 		}
 
@@ -1557,48 +1561,48 @@ static int self_check_ai(struct ubi_device *ubi, struct ubi_attach_info *ai)
 			leb_count += 1;
 
 			if (aeb->pnum < 0 || aeb->ec < 0) {
-				ubi_err("negative values");
+				ubi_err(ubi, "negative values");
 				goto bad_aeb;
 			}
 
 			if (aeb->ec < ai->min_ec) {
-				ubi_err("bad ai->min_ec (%d), %d found",
+				ubi_err(ubi, "bad ai->min_ec (%d), %d found",
 					ai->min_ec, aeb->ec);
 				goto bad_aeb;
 			}
 
 			if (aeb->ec > ai->max_ec) {
-				ubi_err("bad ai->max_ec (%d), %d found",
+				ubi_err(ubi, "bad ai->max_ec (%d), %d found",
 					ai->max_ec, aeb->ec);
 				goto bad_aeb;
 			}
 
 			if (aeb->pnum >= ubi->peb_count) {
-				ubi_err("too high PEB number %d, total PEBs %d",
+				ubi_err(ubi, "too high PEB number %d, total PEBs %d",
 					aeb->pnum, ubi->peb_count);
 				goto bad_aeb;
 			}
 
 			if (av->vol_type == UBI_STATIC_VOLUME) {
 				if (aeb->lnum >= av->used_ebs) {
-					ubi_err("bad lnum or used_ebs");
+					ubi_err(ubi, "bad lnum or used_ebs");
 					goto bad_aeb;
 				}
 			} else {
 				if (av->used_ebs != 0) {
-					ubi_err("non-zero used_ebs");
+					ubi_err(ubi, "non-zero used_ebs");
 					goto bad_aeb;
 				}
 			}
 
 			if (aeb->lnum > av->highest_lnum) {
-				ubi_err("incorrect highest_lnum or lnum");
+				ubi_err(ubi, "incorrect highest_lnum or lnum");
 				goto bad_aeb;
 			}
 		}
 
 		if (av->leb_count != leb_count) {
-			ubi_err("bad leb_count, %d objects in the tree",
+			ubi_err(ubi, "bad leb_count, %d objects in the tree",
 				leb_count);
 			goto bad_av;
 		}
@@ -1609,13 +1613,13 @@ static int self_check_ai(struct ubi_device *ubi, struct ubi_attach_info *ai)
 		aeb = last_aeb;
 
 		if (aeb->lnum != av->highest_lnum) {
-			ubi_err("bad highest_lnum");
+			ubi_err(ubi, "bad highest_lnum");
 			goto bad_aeb;
 		}
 	}
 
 	if (vols_found != ai->vols_found) {
-		ubi_err("bad ai->vols_found %d, should be %d",
+		ubi_err(ubi, "bad ai->vols_found %d, should be %d",
 			ai->vols_found, vols_found);
 		goto out;
 	}
@@ -1632,7 +1636,8 @@ static int self_check_ai(struct ubi_device *ubi, struct ubi_attach_info *ai)
 
 			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);
+				ubi_err(ubi, "VID header is not OK (%d)",
+					err);
 				if (err > 0)
 					err = -EIO;
 				return err;
@@ -1641,37 +1646,37 @@ static int self_check_ai(struct ubi_device *ubi, struct ubi_attach_info *ai)
 			vol_type = vidh->vol_type == UBI_VID_DYNAMIC ?
 				   UBI_DYNAMIC_VOLUME : UBI_STATIC_VOLUME;
 			if (av->vol_type != vol_type) {
-				ubi_err("bad vol_type");
+				ubi_err(ubi, "bad vol_type");
 				goto bad_vid_hdr;
 			}
 
 			if (aeb->sqnum != be64_to_cpu(vidh->sqnum)) {
-				ubi_err("bad sqnum %llu", aeb->sqnum);
+				ubi_err(ubi, "bad sqnum %llu", aeb->sqnum);
 				goto bad_vid_hdr;
 			}
 
 			if (av->vol_id != be32_to_cpu(vidh->vol_id)) {
-				ubi_err("bad vol_id %d", av->vol_id);
+				ubi_err(ubi, "bad vol_id %d", av->vol_id);
 				goto bad_vid_hdr;
 			}
 
 			if (av->compat != vidh->compat) {
-				ubi_err("bad compat %d", vidh->compat);
+				ubi_err(ubi, "bad compat %d", vidh->compat);
 				goto bad_vid_hdr;
 			}
 
 			if (aeb->lnum != be32_to_cpu(vidh->lnum)) {
-				ubi_err("bad lnum %d", aeb->lnum);
+				ubi_err(ubi, "bad lnum %d", aeb->lnum);
 				goto bad_vid_hdr;
 			}
 
 			if (av->used_ebs != be32_to_cpu(vidh->used_ebs)) {
-				ubi_err("bad used_ebs %d", av->used_ebs);
+				ubi_err(ubi, "bad used_ebs %d", av->used_ebs);
 				goto bad_vid_hdr;
 			}
 
 			if (av->data_pad != be32_to_cpu(vidh->data_pad)) {
-				ubi_err("bad data_pad %d", av->data_pad);
+				ubi_err(ubi, "bad data_pad %d", av->data_pad);
 				goto bad_vid_hdr;
 			}
 		}
@@ -1680,12 +1685,13 @@ static int self_check_ai(struct ubi_device *ubi, struct ubi_attach_info *ai)
 			continue;
 
 		if (av->highest_lnum != be32_to_cpu(vidh->lnum)) {
-			ubi_err("bad highest_lnum %d", av->highest_lnum);
+			ubi_err(ubi, "bad highest_lnum %d", av->highest_lnum);
 			goto bad_vid_hdr;
 		}
 
 		if (av->last_data_size != be32_to_cpu(vidh->data_size)) {
-			ubi_err("bad last_data_size %d", av->last_data_size);
+			ubi_err(ubi, "bad last_data_size %d",
+				av->last_data_size);
 			goto bad_vid_hdr;
 		}
 	}
@@ -1726,7 +1732,7 @@ static int self_check_ai(struct ubi_device *ubi, struct ubi_attach_info *ai)
 	err = 0;
 	for (pnum = 0; pnum < ubi->peb_count; pnum++)
 		if (!buf[pnum]) {
-			ubi_err("PEB %d is not referred", pnum);
+			ubi_err(ubi, "PEB %d is not referred", pnum);
 			err = 1;
 		}
 
@@ -1736,18 +1742,18 @@ static int self_check_ai(struct ubi_device *ubi, struct ubi_attach_info *ai)
 	return 0;
 
 bad_aeb:
-	ubi_err("bad attaching information about LEB %d", aeb->lnum);
+	ubi_err(ubi, "bad attaching information about LEB %d", aeb->lnum);
 	ubi_dump_aeb(aeb, 0);
 	ubi_dump_av(av);
 	goto out;
 
 bad_av:
-	ubi_err("bad attaching information about volume %d", av->vol_id);
+	ubi_err(ubi, "bad attaching information about volume %d", av->vol_id);
 	ubi_dump_av(av);
 	goto out;
 
 bad_vid_hdr:
-	ubi_err("bad attaching information about volume %d", av->vol_id);
+	ubi_err(ubi, "bad attaching information about volume %d", av->vol_id);
 	ubi_dump_av(av);
 	ubi_dump_vid_hdr(vidh);
 
diff --git a/drivers/mtd/ubi/block.c b/drivers/mtd/ubi/block.c
index 8876c7d3d712..6b6bce28bd63 100644
--- a/drivers/mtd/ubi/block.c
+++ b/drivers/mtd/ubi/block.c
@@ -111,13 +111,13 @@ static int __init ubiblock_set_param(const char *val,
 
 	len = strnlen(val, UBIBLOCK_PARAM_LEN);
 	if (len == 0) {
-		ubi_warn("block: empty 'block=' parameter - ignored\n");
+		pr_warn("UBI: block: empty 'block=' parameter - ignored\n");
 		return 0;
 	}
 
 	if (len == UBIBLOCK_PARAM_LEN) {
-		ubi_err("block: parameter \"%s\" is too long, max. is %d\n",
-			val, UBIBLOCK_PARAM_LEN);
+		pr_err("UBI: block: parameter \"%s\" is too long, max. is %d\n",
+		       val, UBIBLOCK_PARAM_LEN);
 		return -EINVAL;
 	}
 
@@ -188,9 +188,8 @@ static int ubiblock_read_to_buf(struct ubiblock *dev, char *buffer,
 
 	ret = ubi_read(dev->desc, leb, buffer, offset, len);
 	if (ret) {
-		ubi_err("%s: error %d while reading from LEB %d (offset %d, "
-		        "length %d)", dev->gd->disk_name, ret, leb, offset,
-			len);
+		dev_err(disk_to_dev(dev->gd), "%d while reading from LEB %d (offset %d, length %d)",
+			ret, leb, offset, len);
 		return ret;
 	}
 	return 0;
@@ -328,8 +327,8 @@ static int ubiblock_open(struct block_device *bdev, fmode_t mode)
 
 	dev->desc = ubi_open_volume(dev->ubi_num, dev->vol_id, UBI_READONLY);
 	if (IS_ERR(dev->desc)) {
-		ubi_err("%s failed to open ubi volume %d_%d",
-			dev->gd->disk_name, dev->ubi_num, dev->vol_id);
+		dev_err(disk_to_dev(dev->gd), "failed to open ubi volume %d_%d",
+			dev->ubi_num, dev->vol_id);
 		ret = PTR_ERR(dev->desc);
 		dev->desc = NULL;
 		goto out_unlock;
@@ -405,7 +404,7 @@ int ubiblock_create(struct ubi_volume_info *vi)
 	/* Initialize the gendisk of this ubiblock device */
 	gd = alloc_disk(1);
 	if (!gd) {
-		ubi_err("block: alloc_disk failed");
+		pr_err("UBI: block: alloc_disk failed");
 		ret = -ENODEV;
 		goto out_free_dev;
 	}
@@ -421,7 +420,7 @@ int ubiblock_create(struct ubi_volume_info *vi)
 	spin_lock_init(&dev->queue_lock);
 	dev->rq = blk_init_queue(ubiblock_request, &dev->queue_lock);
 	if (!dev->rq) {
-		ubi_err("block: blk_init_queue failed");
+		dev_err(disk_to_dev(gd), "blk_init_queue failed");
 		ret = -ENODEV;
 		goto out_put_disk;
 	}
@@ -446,8 +445,8 @@ int ubiblock_create(struct ubi_volume_info *vi)
 
 	/* Must be the last step: anyone can call file ops from now on */
 	add_disk(dev->gd);
-	ubi_msg("%s created from ubi%d:%d(%s)",
-		dev->gd->disk_name, dev->ubi_num, dev->vol_id, vi->name);
+	dev_info(disk_to_dev(dev->gd), "created from ubi%d:%d(%s)",
+		 dev->ubi_num, dev->vol_id, vi->name);
 	return 0;
 
 out_free_queue:
@@ -464,7 +463,7 @@ static void ubiblock_cleanup(struct ubiblock *dev)
 {
 	del_gendisk(dev->gd);
 	blk_cleanup_queue(dev->rq);
-	ubi_msg("%s released", dev->gd->disk_name);
+	dev_info(disk_to_dev(dev->gd), "released");
 	put_disk(dev->gd);
 }
 
@@ -518,8 +517,8 @@ static int ubiblock_resize(struct ubi_volume_info *vi)
 	}
 	if ((sector_t)disk_capacity != disk_capacity) {
 		mutex_unlock(&devices_mutex);
-		ubi_warn("%s: the volume is too big (%d LEBs), cannot resize",
-			 dev->gd->disk_name, vi->size);
+		dev_warn(disk_to_dev(dev->gd), "the volume is too big (%d LEBs), cannot resize",
+			 vi->size);
 		return -EFBIG;
 	}
 
@@ -527,8 +526,8 @@ static int ubiblock_resize(struct ubi_volume_info *vi)
 
 	if (get_capacity(dev->gd) != disk_capacity) {
 		set_capacity(dev->gd, disk_capacity);
-		ubi_msg("%s resized to %lld bytes", dev->gd->disk_name,
-			vi->used_bytes);
+		dev_info(disk_to_dev(dev->gd), "resized to %lld bytes",
+			 vi->used_bytes);
 	}
 	mutex_unlock(&dev->dev_mutex);
 	mutex_unlock(&devices_mutex);
@@ -596,8 +595,8 @@ static int __init ubiblock_create_from_param(void)
 
 		desc = open_volume_desc(p->name, p->ubi_num, p->vol_id);
 		if (IS_ERR(desc)) {
-			ubi_err("block: can't open volume, err=%ld\n",
-				PTR_ERR(desc));
+			pr_err("UBI: block: can't open volume, err=%ld\n",
+			       PTR_ERR(desc));
 			ret = PTR_ERR(desc);
 			break;
 		}
@@ -607,8 +606,8 @@ static int __init ubiblock_create_from_param(void)
 
 		ret = ubiblock_create(&vi);
 		if (ret) {
-			ubi_err("block: can't add '%s' volume, err=%d\n",
-				vi.name, ret);
+			pr_err("UBI: block: can't add '%s' volume, err=%d\n",
+			       vi.name, ret);
 			break;
 		}
 	}
diff --git a/drivers/mtd/ubi/build.c b/drivers/mtd/ubi/build.c
index 6e30a3c280d0..3405be46ebe9 100644
--- a/drivers/mtd/ubi/build.c
+++ b/drivers/mtd/ubi/build.c
@@ -166,7 +166,7 @@ int ubi_volume_notify(struct ubi_device *ubi, struct ubi_volume *vol, int ntype)
 	case UBI_VOLUME_RESIZED:
 	case UBI_VOLUME_RENAMED:
 		if (ubi_update_fastmap(ubi)) {
-			ubi_err("Unable to update fastmap!");
+			ubi_err(ubi, "Unable to update fastmap!");
 			ubi_ro_mode(ubi);
 		}
 	}
@@ -517,7 +517,7 @@ static int uif_init(struct ubi_device *ubi, int *ref)
 	 */
 	err = alloc_chrdev_region(&dev, 0, ubi->vtbl_slots + 1, ubi->ubi_name);
 	if (err) {
-		ubi_err("cannot register UBI character devices");
+		ubi_err(ubi, "cannot register UBI character devices");
 		return err;
 	}
 
@@ -528,7 +528,7 @@ static int uif_init(struct ubi_device *ubi, int *ref)
 
 	err = cdev_add(&ubi->cdev, dev, 1);
 	if (err) {
-		ubi_err("cannot add character device");
+		ubi_err(ubi, "cannot add character device");
 		goto out_unreg;
 	}
 
@@ -540,7 +540,7 @@ static int uif_init(struct ubi_device *ubi, int *ref)
 		if (ubi->volumes[i]) {
 			err = ubi_add_volume(ubi, ubi->volumes[i]);
 			if (err) {
-				ubi_err("cannot add volume %d", i);
+				ubi_err(ubi, "cannot add volume %d", i);
 				goto out_volumes;
 			}
 		}
@@ -556,7 +556,8 @@ out_sysfs:
 	cdev_del(&ubi->cdev);
 out_unreg:
 	unregister_chrdev_region(ubi->cdev.dev, ubi->vtbl_slots + 1);
-	ubi_err("cannot initialize UBI %s, error %d", ubi->ubi_name, err);
+	ubi_err(ubi, "cannot initialize UBI %s, error %d",
+		ubi->ubi_name, err);
 	return err;
 }
 
@@ -650,7 +651,7 @@ static int io_init(struct ubi_device *ubi, int max_beb_per1024)
 		 * guess we should just pick the largest region. But this is
 		 * not implemented.
 		 */
-		ubi_err("multiple regions, not implemented");
+		ubi_err(ubi, "multiple regions, not implemented");
 		return -EINVAL;
 	}
 
@@ -685,7 +686,7 @@ static int io_init(struct ubi_device *ubi, int max_beb_per1024)
 	 * which allows us to avoid costly division operations.
 	 */
 	if (!is_power_of_2(ubi->min_io_size)) {
-		ubi_err("min. I/O unit (%d) is not power of 2",
+		ubi_err(ubi, "min. I/O unit (%d) is not power of 2",
 			ubi->min_io_size);
 		return -EINVAL;
 	}
@@ -702,7 +703,7 @@ static int io_init(struct ubi_device *ubi, int max_beb_per1024)
 	if (ubi->max_write_size < ubi->min_io_size ||
 	    ubi->max_write_size % ubi->min_io_size ||
 	    !is_power_of_2(ubi->max_write_size)) {
-		ubi_err("bad write buffer size %d for %d min. I/O unit",
+		ubi_err(ubi, "bad write buffer size %d for %d min. I/O unit",
 			ubi->max_write_size, ubi->min_io_size);
 		return -EINVAL;
 	}
@@ -739,7 +740,7 @@ static int io_init(struct ubi_device *ubi, int max_beb_per1024)
 
 	/* The shift must be aligned to 32-bit boundary */
 	if (ubi->vid_hdr_shift % 4) {
-		ubi_err("unaligned VID header shift %d",
+		ubi_err(ubi, "unaligned VID header shift %d",
 			ubi->vid_hdr_shift);
 		return -EINVAL;
 	}
@@ -749,7 +750,7 @@ static int io_init(struct ubi_device *ubi, int max_beb_per1024)
 	    ubi->leb_start < ubi->vid_hdr_offset + UBI_VID_HDR_SIZE ||
 	    ubi->leb_start > ubi->peb_size - UBI_VID_HDR_SIZE ||
 	    ubi->leb_start & (ubi->min_io_size - 1)) {
-		ubi_err("bad VID header (%d) or data offsets (%d)",
+		ubi_err(ubi, "bad VID header (%d) or data offsets (%d)",
 			ubi->vid_hdr_offset, ubi->leb_start);
 		return -EINVAL;
 	}
@@ -769,14 +770,14 @@ static int io_init(struct ubi_device *ubi, int max_beb_per1024)
 	 * read-only mode.
 	 */
 	if (ubi->vid_hdr_offset + UBI_VID_HDR_SIZE <= ubi->hdrs_min_io_size) {
-		ubi_warn("EC and VID headers are in the same minimal I/O unit, switch to read-only mode");
+		ubi_warn(ubi, "EC and VID headers are in the same minimal I/O unit, switch to read-only mode");
 		ubi->ro_mode = 1;
 	}
 
 	ubi->leb_size = ubi->peb_size - ubi->leb_start;
 
 	if (!(ubi->mtd->flags & MTD_WRITEABLE)) {
-		ubi_msg("MTD device %d is write-protected, attach in read-only mode",
+		ubi_msg(ubi, "MTD device %d is write-protected, attach in read-only mode",
 			ubi->mtd->index);
 		ubi->ro_mode = 1;
 	}
@@ -809,7 +810,7 @@ static int autoresize(struct ubi_device *ubi, int vol_id)
 	int err, old_reserved_pebs = vol->reserved_pebs;
 
 	if (ubi->ro_mode) {
-		ubi_warn("skip auto-resize because of R/O mode");
+		ubi_warn(ubi, "skip auto-resize because of R/O mode");
 		return 0;
 	}
 
@@ -830,21 +831,22 @@ static int autoresize(struct ubi_device *ubi, int vol_id)
 		vtbl_rec = ubi->vtbl[vol_id];
 		err = ubi_change_vtbl_record(ubi, vol_id, &vtbl_rec);
 		if (err)
-			ubi_err("cannot clean auto-resize flag for volume %d",
+			ubi_err(ubi, "cannot clean auto-resize flag for volume %d",
 				vol_id);
 	} else {
 		desc.vol = vol;
 		err = ubi_resize_volume(&desc,
 					old_reserved_pebs + ubi->avail_pebs);
 		if (err)
-			ubi_err("cannot auto-resize volume %d", vol_id);
+			ubi_err(ubi, "cannot auto-resize volume %d",
+				vol_id);
 	}
 
 	if (err)
 		return err;
 
-	ubi_msg("volume %d (\"%s\") re-sized from %d to %d LEBs", vol_id,
-		vol->name, old_reserved_pebs, vol->reserved_pebs);
+	ubi_msg(ubi, "volume %d (\"%s\") re-sized from %d to %d LEBs",
+		vol_id, vol->name, old_reserved_pebs, vol->reserved_pebs);
 	return 0;
 }
 
@@ -885,7 +887,7 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num,
 	for (i = 0; i < UBI_MAX_DEVICES; i++) {
 		ubi = ubi_devices[i];
 		if (ubi && mtd->index == ubi->mtd->index) {
-			ubi_err("mtd%d is already attached to ubi%d",
+			ubi_err(ubi, "mtd%d is already attached to ubi%d",
 				mtd->index, i);
 			return -EEXIST;
 		}
@@ -900,7 +902,7 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num,
 	 * no sense to attach emulated MTD devices, so we prohibit this.
 	 */
 	if (mtd->type == MTD_UBIVOLUME) {
-		ubi_err("refuse attaching mtd%d - it is already emulated on top of UBI",
+		ubi_err(ubi, "refuse attaching mtd%d - it is already emulated on top of UBI",
 			mtd->index);
 		return -EINVAL;
 	}
@@ -911,7 +913,7 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num,
 			if (!ubi_devices[ubi_num])
 				break;
 		if (ubi_num == UBI_MAX_DEVICES) {
-			ubi_err("only %d UBI devices may be created",
+			ubi_err(ubi, "only %d UBI devices may be created",
 				UBI_MAX_DEVICES);
 			return -ENFILE;
 		}
@@ -921,7 +923,7 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num,
 
 		/* Make sure ubi_num is not busy */
 		if (ubi_devices[ubi_num]) {
-			ubi_err("ubi%d already exists", ubi_num);
+			ubi_err(ubi, "ubi%d already exists", ubi_num);
 			return -EEXIST;
 		}
 	}
@@ -953,13 +955,14 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num,
 
 	if (!ubi->fm_disabled && (int)mtd_div_by_eb(ubi->mtd->size, ubi->mtd)
 	    <= UBI_FM_MAX_START) {
-		ubi_err("More than %i PEBs are needed for fastmap, sorry.",
+		ubi_err(ubi, "More than %i PEBs are needed for fastmap, sorry.",
 			UBI_FM_MAX_START);
 		ubi->fm_disabled = 1;
 	}
 
-	ubi_msg("default fastmap pool size: %d", ubi->fm_pool.max_size);
-	ubi_msg("default fastmap WL pool size: %d", ubi->fm_wl_pool.max_size);
+	ubi_msg(ubi, "default fastmap pool size: %d", ubi->fm_pool.max_size);
+	ubi_msg(ubi, "default fastmap WL pool size: %d",
+		ubi->fm_wl_pool.max_size);
 #else
 	ubi->fm_disabled = 1;
 #endif
@@ -970,7 +973,7 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num,
 	mutex_init(&ubi->fm_mutex);
 	init_rwsem(&ubi->fm_sem);
 
-	ubi_msg("attaching mtd%d to ubi%d", mtd->index, ubi_num);
+	ubi_msg(ubi, "attaching mtd%d to ubi%d", mtd->index, ubi_num);
 
 	err = io_init(ubi, max_beb_per1024);
 	if (err)
@@ -989,7 +992,8 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num,
 #endif
 	err = ubi_attach(ubi, 0);
 	if (err) {
-		ubi_err("failed to attach mtd%d, error %d", mtd->index, err);
+		ubi_err(ubi, "failed to attach mtd%d, error %d",
+			mtd->index, err);
 		goto out_free;
 	}
 
@@ -1010,28 +1014,28 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num,
 	ubi->bgt_thread = kthread_create(ubi_thread, ubi, "%s", ubi->bgt_name);
 	if (IS_ERR(ubi->bgt_thread)) {
 		err = PTR_ERR(ubi->bgt_thread);
-		ubi_err("cannot spawn \"%s\", error %d", ubi->bgt_name,
-			err);
+		ubi_err(ubi, "cannot spawn \"%s\", error %d",
+			ubi->bgt_name, err);
 		goto out_debugfs;
 	}
 
-	ubi_msg("attached mtd%d (name \"%s\", size %llu MiB) to ubi%d",
-		mtd->index, mtd->name, ubi->flash_size >> 20, ubi_num);
-	ubi_msg("PEB size: %d bytes (%d KiB), LEB size: %d bytes",
+	ubi_msg(ubi, "attached mtd%d (name \"%s\", size %llu MiB)",
+		mtd->index, mtd->name, ubi->flash_size >> 20);
+	ubi_msg(ubi, "PEB size: %d bytes (%d KiB), LEB size: %d bytes",
 		ubi->peb_size, ubi->peb_size >> 10, ubi->leb_size);
-	ubi_msg("min./max. I/O unit sizes: %d/%d, sub-page size %d",
+	ubi_msg(ubi, "min./max. I/O unit sizes: %d/%d, sub-page size %d",
 		ubi->min_io_size, ubi->max_write_size, ubi->hdrs_min_io_size);
-	ubi_msg("VID header offset: %d (aligned %d), data offset: %d",
+	ubi_msg(ubi, "VID header offset: %d (aligned %d), data offset: %d",
 		ubi->vid_hdr_offset, ubi->vid_hdr_aloffset, ubi->leb_start);
-	ubi_msg("good PEBs: %d, bad PEBs: %d, corrupted PEBs: %d",
+	ubi_msg(ubi, "good PEBs: %d, bad PEBs: %d, corrupted PEBs: %d",
 		ubi->good_peb_count, ubi->bad_peb_count, ubi->corr_peb_count);
-	ubi_msg("user volume: %d, internal volumes: %d, max. volumes count: %d",
+	ubi_msg(ubi, "user volume: %d, internal volumes: %d, max. volumes count: %d",
 		ubi->vol_count - UBI_INT_VOL_COUNT, UBI_INT_VOL_COUNT,
 		ubi->vtbl_slots);
-	ubi_msg("max/mean erase counter: %d/%d, WL threshold: %d, image sequence number: %u",
+	ubi_msg(ubi, "max/mean erase counter: %d/%d, WL threshold: %d, image sequence number: %u",
 		ubi->max_ec, ubi->mean_ec, CONFIG_MTD_UBI_WL_THRESHOLD,
 		ubi->image_seq);
-	ubi_msg("available PEBs: %d, total reserved PEBs: %d, PEBs reserved for bad PEB handling: %d",
+	ubi_msg(ubi, "available PEBs: %d, total reserved PEBs: %d, PEBs reserved for bad PEB handling: %d",
 		ubi->avail_pebs, ubi->rsvd_pebs, ubi->beb_rsvd_pebs);
 
 	/*
@@ -1100,7 +1104,7 @@ int ubi_detach_mtd_dev(int ubi_num, int anyway)
 			return -EBUSY;
 		}
 		/* This may only happen if there is a bug */
-		ubi_err("%s reference count %d, destroy anyway",
+		ubi_err(ubi, "%s reference count %d, destroy anyway",
 			ubi->ubi_name, ubi->ref_count);
 	}
 	ubi_devices[ubi_num] = NULL;
@@ -1108,7 +1112,7 @@ int ubi_detach_mtd_dev(int ubi_num, int anyway)
 
 	ubi_assert(ubi_num == ubi->ubi_num);
 	ubi_notify_all(ubi, UBI_VOLUME_REMOVED, NULL);
-	ubi_msg("detaching mtd%d from ubi%d", ubi->mtd->index, ubi_num);
+	ubi_msg(ubi, "detaching mtd%d", ubi->mtd->index);
 #ifdef CONFIG_MTD_UBI_FASTMAP
 	/* If we don't write a new fastmap at detach time we lose all
 	 * EC updates that have been made since the last written fastmap. */
@@ -1136,7 +1140,7 @@ int ubi_detach_mtd_dev(int ubi_num, int anyway)
 	put_mtd_device(ubi->mtd);
 	vfree(ubi->peb_buf);
 	vfree(ubi->fm_buf);
-	ubi_msg("mtd%d is detached from ubi%d", ubi->mtd->index, ubi->ubi_num);
+	ubi_msg(ubi, "mtd%d is detached", ubi->mtd->index);
 	put_device(&ubi->dev);
 	return 0;
 }
@@ -1218,7 +1222,8 @@ static int __init ubi_init(void)
 	BUILD_BUG_ON(sizeof(struct ubi_vid_hdr) != 64);
 
 	if (mtd_devs > UBI_MAX_DEVICES) {
-		ubi_err("too many MTD devices, maximum is %d", UBI_MAX_DEVICES);
+		pr_err("UBI error: too many MTD devices, maximum is %d",
+		       UBI_MAX_DEVICES);
 		return -EINVAL;
 	}
 
@@ -1226,19 +1231,19 @@ static int __init ubi_init(void)
 	ubi_class = class_create(THIS_MODULE, UBI_NAME_STR);
 	if (IS_ERR(ubi_class)) {
 		err = PTR_ERR(ubi_class);
-		ubi_err("cannot create UBI class");
+		pr_err("UBI error: cannot create UBI class");
 		goto out;
 	}
 
 	err = class_create_file(ubi_class, &ubi_version);
 	if (err) {
-		ubi_err("cannot create sysfs file");
+		pr_err("UBI error: cannot create sysfs file");
 		goto out_class;
 	}
 
 	err = misc_register(&ubi_ctrl_cdev);
 	if (err) {
-		ubi_err("cannot register device");
+		pr_err("UBI error: cannot register device");
 		goto out_version;
 	}
 
@@ -1265,7 +1270,8 @@ static int __init ubi_init(void)
 		mtd = open_mtd_device(p->name);
 		if (IS_ERR(mtd)) {
 			err = PTR_ERR(mtd);
-			ubi_err("cannot open mtd %s, error %d", p->name, err);
+			pr_err("UBI error: cannot open mtd %s, error %d",
+			       p->name, err);
 			/* See comment below re-ubi_is_module(). */
 			if (ubi_is_module())
 				goto out_detach;
@@ -1277,7 +1283,8 @@ static int __init ubi_init(void)
 					 p->vid_hdr_offs, p->max_beb_per1024);
 		mutex_unlock(&ubi_devices_mutex);
 		if (err < 0) {
-			ubi_err("cannot attach mtd%d", mtd->index);
+			pr_err("UBI error: cannot attach mtd%d",
+			       mtd->index);
 			put_mtd_device(mtd);
 
 			/*
@@ -1300,7 +1307,7 @@ static int __init ubi_init(void)
 
 	err = ubiblock_init();
 	if (err) {
-		ubi_err("block: cannot initialize, error %d", err);
+		pr_err("UBI error: block: cannot initialize, error %d", err);
 
 		/* See comment above re-ubi_is_module(). */
 		if (ubi_is_module())
@@ -1326,7 +1333,7 @@ out_version:
 out_class:
 	class_destroy(ubi_class);
 out:
-	ubi_err("cannot initialize UBI, error %d", err);
+	pr_err("UBI error: cannot initialize UBI, error %d", err);
 	return err;
 }
 late_initcall(ubi_init);
@@ -1365,7 +1372,7 @@ static int __init bytes_str_to_int(const char *str)
 
 	result = simple_strtoul(str, &endp, 0);
 	if (str == endp || result >= INT_MAX) {
-		ubi_err("incorrect bytes count: \"%s\"\n", str);
+		pr_err("UBI error: incorrect bytes count: \"%s\"\n", str);
 		return -EINVAL;
 	}
 
@@ -1381,7 +1388,7 @@ static int __init bytes_str_to_int(const char *str)
 	case '\0':
 		break;
 	default:
-		ubi_err("incorrect bytes count: \"%s\"\n", str);
+		pr_err("UBI error: incorrect bytes count: \"%s\"\n", str);
 		return -EINVAL;
 	}
 
@@ -1408,20 +1415,20 @@ static int __init ubi_mtd_param_parse(const char *val, struct kernel_param *kp)
 		return -EINVAL;
 
 	if (mtd_devs == UBI_MAX_DEVICES) {
-		ubi_err("too many parameters, max. is %d\n",
-			UBI_MAX_DEVICES);
+		pr_err("UBI error: too many parameters, max. is %d\n",
+		       UBI_MAX_DEVICES);
 		return -EINVAL;
 	}
 
 	len = strnlen(val, MTD_PARAM_LEN_MAX);
 	if (len == MTD_PARAM_LEN_MAX) {
-		ubi_err("parameter \"%s\" is too long, max. is %d\n",
-			val, MTD_PARAM_LEN_MAX);
+		pr_err("UBI error: parameter \"%s\" is too long, max. is %d\n",
+		       val, MTD_PARAM_LEN_MAX);
 		return -EINVAL;
 	}
 
 	if (len == 0) {
-		pr_warn("UBI warning: empty 'mtd=' parameter - ignored\n");
+		pr_err("UBI warning: empty 'mtd=' parameter - ignored\n");
 		return 0;
 	}
 
@@ -1435,7 +1442,7 @@ static int __init ubi_mtd_param_parse(const char *val, struct kernel_param *kp)
 		tokens[i] = strsep(&pbuf, ",");
 
 	if (pbuf) {
-		ubi_err("too many arguments at \"%s\"\n", val);
+		pr_err("UBI error: too many arguments at \"%s\"\n", val);
 		return -EINVAL;
 	}
 
@@ -1455,8 +1462,8 @@ static int __init ubi_mtd_param_parse(const char *val, struct kernel_param *kp)
 		int err = kstrtoint(token, 10, &p->max_beb_per1024);
 
 		if (err) {
-			ubi_err("bad value for max_beb_per1024 parameter: %s",
-				token);
+			pr_err("UBI error: bad value for max_beb_per1024 parameter: %s",
+			       token);
 			return -EINVAL;
 		}
 	}
@@ -1466,7 +1473,8 @@ static int __init ubi_mtd_param_parse(const char *val, struct kernel_param *kp)
 		int err = kstrtoint(token, 10, &p->ubi_num);
 
 		if (err) {
-			ubi_err("bad value for ubi_num parameter: %s", token);
+			pr_err("UBI error: bad value for ubi_num parameter: %s",
+			       token);
 			return -EINVAL;
 		}
 	} else
diff --git a/drivers/mtd/ubi/cdev.c b/drivers/mtd/ubi/cdev.c
index 59de69a24e40..3410ea8109f8 100644
--- a/drivers/mtd/ubi/cdev.c
+++ b/drivers/mtd/ubi/cdev.c
@@ -48,13 +48,14 @@
 
 /**
  * get_exclusive - get exclusive access to an UBI volume.
+ * @ubi: UBI device description object
  * @desc: volume descriptor
  *
  * This function changes UBI volume open mode to "exclusive". Returns previous
  * mode value (positive integer) in case of success and a negative error code
  * in case of failure.
  */
-static int get_exclusive(struct ubi_volume_desc *desc)
+static int get_exclusive(struct ubi_device *ubi, struct ubi_volume_desc *desc)
 {
 	int users, err;
 	struct ubi_volume *vol = desc->vol;
@@ -63,7 +64,7 @@ static int get_exclusive(struct ubi_volume_desc *desc)
 	users = vol->readers + vol->writers + vol->exclusive;
 	ubi_assert(users > 0);
 	if (users > 1) {
-		ubi_err("%d users for volume %d", users, vol->vol_id);
+		ubi_err(ubi, "%d users for volume %d", users, vol->vol_id);
 		err = -EBUSY;
 	} else {
 		vol->readers = vol->writers = 0;
@@ -134,7 +135,7 @@ static int vol_cdev_release(struct inode *inode, struct file *file)
 		vol->ubi->ubi_num, vol->vol_id, desc->mode);
 
 	if (vol->updating) {
-		ubi_warn("update of volume %d not finished, volume is damaged",
+		ubi_warn(vol->ubi, "update of volume %d not finished, volume is damaged",
 			 vol->vol_id);
 		ubi_assert(!vol->changing_leb);
 		vol->updating = 0;
@@ -158,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 */
-		ubi_err("updating");
+		ubi_err(vol->ubi, "updating");
 		return -EBUSY;
 	}
 
@@ -193,11 +194,11 @@ static ssize_t vol_cdev_read(struct file *file, __user char *buf, size_t count,
 		count, *offp, vol->vol_id);
 
 	if (vol->updating) {
-		ubi_err("updating");
+		ubi_err(vol->ubi, "updating");
 		return -EBUSY;
 	}
 	if (vol->upd_marker) {
-		ubi_err("damaged volume, update marker is set");
+		ubi_err(vol->ubi, "damaged volume, update marker is set");
 		return -EBADF;
 	}
 	if (*offp == vol->used_bytes || count == 0)
@@ -277,7 +278,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)) {
-		ubi_err("unaligned position");
+		ubi_err(ubi, "unaligned position");
 		return -EINVAL;
 	}
 
@@ -286,7 +287,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)) {
-		ubi_err("unaligned write length");
+		ubi_err(ubi, "unaligned write length");
 		return -EINVAL;
 	}
 
@@ -348,7 +349,7 @@ static ssize_t vol_cdev_write(struct file *file, const char __user *buf,
 		err = ubi_more_leb_change_data(ubi, vol, buf, count);
 
 	if (err < 0) {
-		ubi_err("cannot accept more %zd bytes of data, error %d",
+		ubi_err(ubi, "cannot accept more %zd bytes of data, error %d",
 			count, err);
 		return err;
 	}
@@ -370,7 +371,7 @@ static ssize_t vol_cdev_write(struct file *file, const char __user *buf,
 			return err;
 
 		if (err) {
-			ubi_warn("volume %d on UBI device %d is corrupted",
+			ubi_warn(ubi, "volume %d on UBI device %d is corrupted",
 				 vol->vol_id, ubi->ubi_num);
 			vol->corrupted = 1;
 		}
@@ -420,7 +421,7 @@ static long vol_cdev_ioctl(struct file *file, unsigned int cmd,
 			break;
 		}
 
-		err = get_exclusive(desc);
+		err = get_exclusive(ubi, desc);
 		if (err < 0)
 			break;
 
@@ -456,7 +457,7 @@ static long vol_cdev_ioctl(struct file *file, unsigned int cmd,
 		    req.bytes < 0 || req.lnum >= vol->usable_leb_size)
 			break;
 
-		err = get_exclusive(desc);
+		err = get_exclusive(ubi, desc);
 		if (err < 0)
 			break;
 
@@ -642,7 +643,7 @@ static int verify_mkvol_req(const struct ubi_device *ubi,
 	return 0;
 
 bad:
-	ubi_err("bad volume creation request");
+	ubi_err(ubi, "bad volume creation request");
 	ubi_dump_mkvol_req(req);
 	return err;
 }
@@ -708,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) {
-				ubi_err("duplicated volume id %d",
+				ubi_err(ubi, "duplicated volume id %d",
 					req->ents[i].vol_id);
 				return -EINVAL;
 			}
 			if (!strcmp(req->ents[i].name, req->ents[n].name)) {
-				ubi_err("duplicated volume name \"%s\"",
+				ubi_err(ubi, "duplicated volume name \"%s\"",
 					req->ents[i].name);
 				return -EINVAL;
 			}
@@ -736,7 +737,8 @@ static int rename_volumes(struct ubi_device *ubi,
 		re->desc = ubi_open_volume(ubi->ubi_num, vol_id, UBI_READWRITE);
 		if (IS_ERR(re->desc)) {
 			err = PTR_ERR(re->desc);
-			ubi_err("cannot open volume %d, error %d", vol_id, err);
+			ubi_err(ubi, "cannot open volume %d, error %d",
+				vol_id, err);
 			kfree(re);
 			goto out_free;
 		}
@@ -795,7 +797,7 @@ static int rename_volumes(struct ubi_device *ubi,
 				continue;
 
 			/* The volume exists but busy, or an error occurred */
-			ubi_err("cannot open volume \"%s\", error %d",
+			ubi_err(ubi, "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 63cb1d7236ce..7335c9ff9d99 100644
--- a/drivers/mtd/ubi/debug.c
+++ b/drivers/mtd/ubi/debug.c
@@ -43,12 +43,12 @@ void ubi_dump_flash(struct ubi_device *ubi, int pnum, int offset, int len)
 		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",
+		ubi_err(ubi, "err %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",
+	ubi_msg(ubi, "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:
@@ -238,8 +238,8 @@ int ubi_debugfs_init(void)
 	if (IS_ERR_OR_NULL(dfs_rootdir)) {
 		int err = dfs_rootdir ? -ENODEV : PTR_ERR(dfs_rootdir);
 
-		ubi_err("cannot create \"ubi\" debugfs directory, error %d\n",
-			err);
+		pr_err("UBI error: cannot create \"ubi\" debugfs directory, error %d\n",
+		       err);
 		return err;
 	}
 
@@ -433,7 +433,7 @@ out_remove:
 	debugfs_remove_recursive(d->dfs_dir);
 out:
 	err = dent ? PTR_ERR(dent) : -ENODEV;
-	ubi_err("cannot create \"%s\" debugfs file or directory, error %d\n",
+	ubi_err(ubi, "cannot create \"%s\" debugfs file or directory, error %d\n",
 		fname, err);
 	return err;
 }
diff --git a/drivers/mtd/ubi/eba.c b/drivers/mtd/ubi/eba.c
index 2402d3b50171..a40020cf0923 100644
--- a/drivers/mtd/ubi/eba.c
+++ b/drivers/mtd/ubi/eba.c
@@ -422,7 +422,7 @@ retry:
 				 */
 				if (err == UBI_IO_BAD_HDR_EBADMSG ||
 				    err == UBI_IO_BAD_HDR) {
-					ubi_warn("corrupted VID header at PEB %d, LEB %d:%d",
+					ubi_warn(ubi, "corrupted VID header at PEB %d, LEB %d:%d",
 						 pnum, vol_id, lnum);
 					err = -EBADMSG;
 				} else
@@ -448,7 +448,7 @@ retry:
 				goto out_unlock;
 			scrub = 1;
 			if (!check) {
-				ubi_msg("force data checking");
+				ubi_msg(ubi, "force data checking");
 				check = 1;
 				goto retry;
 			}
@@ -459,7 +459,7 @@ retry:
 	if (check) {
 		uint32_t crc1 = crc32(UBI_CRC32_INIT, buf, len);
 		if (crc1 != crc) {
-			ubi_warn("CRC error: calculated %#08x, must be %#08x",
+			ubi_warn(ubi, "CRC error: calculated %#08x, must be %#08x",
 				 crc1, crc);
 			err = -EBADMSG;
 			goto out_unlock;
@@ -513,7 +513,8 @@ retry:
 		return new_pnum;
 	}
 
-	ubi_msg("recover PEB %d, move data to PEB %d", pnum, new_pnum);
+	ubi_msg(ubi, "recover PEB %d, move data to PEB %d",
+		pnum, new_pnum);
 
 	err = ubi_io_read_vid_hdr(ubi, pnum, vid_hdr, 1);
 	if (err && err != UBI_IO_BITFLIPS) {
@@ -554,7 +555,7 @@ retry:
 	up_read(&ubi->fm_sem);
 	ubi_wl_put_peb(ubi, vol_id, lnum, pnum, 1);
 
-	ubi_msg("data was successfully recovered");
+	ubi_msg(ubi, "data was successfully recovered");
 	return 0;
 
 out_unlock:
@@ -569,13 +570,13 @@ write_error:
 	 * Bad luck? This physical eraseblock is bad too? Crud. Let's try to
 	 * get another one.
 	 */
-	ubi_warn("failed to write to PEB %d", new_pnum);
+	ubi_warn(ubi, "failed to write to PEB %d", new_pnum);
 	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;
 	}
-	ubi_msg("try again");
+	ubi_msg(ubi, "try again");
 	goto retry;
 }
 
@@ -613,7 +614,7 @@ int ubi_eba_write_leb(struct ubi_device *ubi, struct ubi_volume *vol, int lnum,
 
 		err = ubi_io_write_data(ubi, buf, pnum, offset, len);
 		if (err) {
-			ubi_warn("failed to write data to PEB %d", pnum);
+			ubi_warn(ubi, "failed to write data to PEB %d", pnum);
 			if (err == -EIO && ubi->bad_allowed)
 				err = recover_peb(ubi, pnum, vol_id, lnum, buf,
 						  offset, len);
@@ -654,7 +655,7 @@ retry:
 
 	err = ubi_io_write_vid_hdr(ubi, pnum, vid_hdr);
 	if (err) {
-		ubi_warn("failed to write VID header to LEB %d:%d, PEB %d",
+		ubi_warn(ubi, "failed to write VID header to LEB %d:%d, PEB %d",
 			 vol_id, lnum, pnum);
 		goto write_error;
 	}
@@ -662,7 +663,7 @@ retry:
 	if (len) {
 		err = ubi_io_write_data(ubi, buf, pnum, offset, len);
 		if (err) {
-			ubi_warn("failed to write %d bytes at offset %d of LEB %d:%d, PEB %d",
+			ubi_warn(ubi, "failed to write %d bytes at offset %d of LEB %d:%d, PEB %d",
 				 len, offset, vol_id, lnum, pnum);
 			goto write_error;
 		}
@@ -698,7 +699,7 @@ write_error:
 	}
 
 	vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
-	ubi_msg("try another PEB");
+	ubi_msg(ubi, "try another PEB");
 	goto retry;
 }
 
@@ -775,14 +776,14 @@ retry:
 
 	err = ubi_io_write_vid_hdr(ubi, pnum, vid_hdr);
 	if (err) {
-		ubi_warn("failed to write VID header to LEB %d:%d, PEB %d",
+		ubi_warn(ubi, "failed to write VID header to LEB %d:%d, PEB %d",
 			 vol_id, lnum, pnum);
 		goto write_error;
 	}
 
 	err = ubi_io_write_data(ubi, buf, pnum, 0, len);
 	if (err) {
-		ubi_warn("failed to write %d bytes of data to PEB %d",
+		ubi_warn(ubi, "failed to write %d bytes of data to PEB %d",
 			 len, pnum);
 		goto write_error;
 	}
@@ -818,7 +819,7 @@ write_error:
 	}
 
 	vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
-	ubi_msg("try another PEB");
+	ubi_msg(ubi, "try another PEB");
 	goto retry;
 }
 
@@ -893,14 +894,14 @@ retry:
 
 	err = ubi_io_write_vid_hdr(ubi, pnum, vid_hdr);
 	if (err) {
-		ubi_warn("failed to write VID header to LEB %d:%d, PEB %d",
+		ubi_warn(ubi, "failed to write VID header to LEB %d:%d, PEB %d",
 			 vol_id, lnum, pnum);
 		goto write_error;
 	}
 
 	err = ubi_io_write_data(ubi, buf, pnum, 0, len);
 	if (err) {
-		ubi_warn("failed to write %d bytes of data to PEB %d",
+		ubi_warn(ubi, "failed to write %d bytes of data to PEB %d",
 			 len, pnum);
 		goto write_error;
 	}
@@ -940,7 +941,7 @@ write_error:
 	}
 
 	vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
-	ubi_msg("try another PEB");
+	ubi_msg(ubi, "try another PEB");
 	goto retry;
 }
 
@@ -1063,7 +1064,7 @@ int ubi_eba_copy_leb(struct ubi_device *ubi, int from, int to,
 	dbg_wl("read %d bytes of data", aldata_size);
 	err = ubi_io_read_data(ubi, ubi->peb_buf, from, 0, aldata_size);
 	if (err && err != UBI_IO_BITFLIPS) {
-		ubi_warn("error %d while reading data from PEB %d",
+		ubi_warn(ubi, "error %d while reading data from PEB %d",
 			 err, from);
 		err = MOVE_SOURCE_RD_ERR;
 		goto out_unlock_buf;
@@ -1113,7 +1114,7 @@ int ubi_eba_copy_leb(struct ubi_device *ubi, int from, int to,
 	err = ubi_io_read_vid_hdr(ubi, to, vid_hdr, 1);
 	if (err) {
 		if (err != UBI_IO_BITFLIPS) {
-			ubi_warn("error %d while reading VID header back from PEB %d",
+			ubi_warn(ubi, "error %d while reading VID header back from PEB %d",
 				 err, to);
 			if (is_error_sane(err))
 				err = MOVE_TARGET_RD_ERR;
@@ -1140,7 +1141,7 @@ int ubi_eba_copy_leb(struct ubi_device *ubi, int from, int to,
 		err = ubi_io_read_data(ubi, ubi->peb_buf, to, 0, aldata_size);
 		if (err) {
 			if (err != UBI_IO_BITFLIPS) {
-				ubi_warn("error %d while reading data back from PEB %d",
+				ubi_warn(ubi, "error %d while reading data back from PEB %d",
 					 err, to);
 				if (is_error_sane(err))
 					err = MOVE_TARGET_RD_ERR;
@@ -1152,7 +1153,7 @@ int ubi_eba_copy_leb(struct ubi_device *ubi, int from, int to,
 		cond_resched();
 
 		if (crc != crc32(UBI_CRC32_INIT, ubi->peb_buf, data_size)) {
-			ubi_warn("read data back from PEB %d and it is different",
+			ubi_warn(ubi, "read data back from PEB %d and it is different",
 				 to);
 			err = -EINVAL;
 			goto out_unlock_buf;
@@ -1205,10 +1206,10 @@ static void print_rsvd_warning(struct ubi_device *ubi,
 			return;
 	}
 
-	ubi_warn("cannot reserve enough PEBs for bad PEB handling, reserved %d, need %d",
+	ubi_warn(ubi, "cannot reserve enough PEBs for bad PEB handling, reserved %d, need %d",
 		 ubi->beb_rsvd_pebs, ubi->beb_rsvd_level);
 	if (ubi->corr_peb_count)
-		ubi_warn("%d PEBs are corrupted and not used",
+		ubi_warn(ubi, "%d PEBs are corrupted and not used",
 			 ubi->corr_peb_count);
 }
 
@@ -1286,7 +1287,7 @@ int self_check_eba(struct ubi_device *ubi, struct ubi_attach_info *ai_fastmap,
 					fm_eba[i][j] == UBI_LEB_UNMAPPED)
 					continue;
 
-				ubi_err("LEB:%i:%i is PEB:%i instead of %i!",
+				ubi_err(ubi, "LEB:%i:%i is PEB:%i instead of %i!",
 					vol->vol_id, i, fm_eba[i][j],
 					scan_eba[i][j]);
 				ubi_assert(0);
@@ -1366,10 +1367,10 @@ int ubi_eba_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 	}
 
 	if (ubi->avail_pebs < EBA_RESERVED_PEBS) {
-		ubi_err("no enough physical eraseblocks (%d, need %d)",
+		ubi_err(ubi, "no enough physical eraseblocks (%d, need %d)",
 			ubi->avail_pebs, EBA_RESERVED_PEBS);
 		if (ubi->corr_peb_count)
-			ubi_err("%d PEBs are corrupted and not used",
+			ubi_err(ubi, "%d PEBs are corrupted and not used",
 				ubi->corr_peb_count);
 		err = -ENOSPC;
 		goto out_free;
diff --git a/drivers/mtd/ubi/fastmap.c b/drivers/mtd/ubi/fastmap.c
index cfd5b5e90156..b56672bf3294 100644
--- a/drivers/mtd/ubi/fastmap.c
+++ b/drivers/mtd/ubi/fastmap.c
@@ -330,7 +330,7 @@ static int process_pool_aeb(struct ubi_device *ubi, struct ubi_attach_info *ai,
 	if (found)
 		av = tmp_av;
 	else {
-		ubi_err("orphaned volume in fastmap pool!");
+		ubi_err(ubi, "orphaned volume in fastmap pool!");
 		kmem_cache_free(ai->aeb_slab_cache, new_aeb);
 		return UBI_BAD_FASTMAP;
 	}
@@ -414,14 +414,14 @@ static int scan_pool(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		pnum = be32_to_cpu(pebs[i]);
 
 		if (ubi_io_is_bad(ubi, pnum)) {
-			ubi_err("bad PEB in fastmap pool!");
+			ubi_err(ubi, "bad PEB in fastmap pool!");
 			ret = UBI_BAD_FASTMAP;
 			goto out;
 		}
 
 		err = ubi_io_read_ec_hdr(ubi, pnum, ech, 0);
 		if (err && err != UBI_IO_BITFLIPS) {
-			ubi_err("unable to read EC header! PEB:%i err:%i",
+			ubi_err(ubi, "unable to read EC header! PEB:%i err:%i",
 				pnum, err);
 			ret = err > 0 ? UBI_BAD_FASTMAP : err;
 			goto out;
@@ -435,7 +435,7 @@ static int scan_pool(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		image_seq = be32_to_cpu(ech->image_seq);
 
 		if (image_seq && (image_seq != ubi->image_seq)) {
-			ubi_err("bad image seq: 0x%x, expected: 0x%x",
+			ubi_err(ubi, "bad image seq: 0x%x, expected: 0x%x",
 				be32_to_cpu(ech->image_seq), ubi->image_seq);
 			ret = UBI_BAD_FASTMAP;
 			goto out;
@@ -493,7 +493,7 @@ static int scan_pool(struct ubi_device *ubi, struct ubi_attach_info *ai,
 			}
 		} else {
 			/* We are paranoid and fall back to scanning mode */
-			ubi_err("fastmap pool PEBs contains damaged PEBs!");
+			ubi_err(ubi, "fastmap pool PEBs contains damaged PEBs!");
 			ret = err > 0 ? UBI_BAD_FASTMAP : err;
 			goto out;
 		}
@@ -588,7 +588,7 @@ static int ubi_attach_fastmap(struct ubi_device *ubi,
 		goto fail_bad;
 
 	if (be32_to_cpu(fmhdr->magic) != UBI_FM_HDR_MAGIC) {
-		ubi_err("bad fastmap header magic: 0x%x, expected: 0x%x",
+		ubi_err(ubi, "bad fastmap header magic: 0x%x, expected: 0x%x",
 			be32_to_cpu(fmhdr->magic), UBI_FM_HDR_MAGIC);
 		goto fail_bad;
 	}
@@ -598,7 +598,7 @@ static int ubi_attach_fastmap(struct ubi_device *ubi,
 	if (fm_pos >= fm_size)
 		goto fail_bad;
 	if (be32_to_cpu(fmpl1->magic) != UBI_FM_POOL_MAGIC) {
-		ubi_err("bad fastmap pool magic: 0x%x, expected: 0x%x",
+		ubi_err(ubi, "bad fastmap pool magic: 0x%x, expected: 0x%x",
 			be32_to_cpu(fmpl1->magic), UBI_FM_POOL_MAGIC);
 		goto fail_bad;
 	}
@@ -608,7 +608,7 @@ static int ubi_attach_fastmap(struct ubi_device *ubi,
 	if (fm_pos >= fm_size)
 		goto fail_bad;
 	if (be32_to_cpu(fmpl2->magic) != UBI_FM_POOL_MAGIC) {
-		ubi_err("bad fastmap pool magic: 0x%x, expected: 0x%x",
+		ubi_err(ubi, "bad fastmap pool magic: 0x%x, expected: 0x%x",
 			be32_to_cpu(fmpl2->magic), UBI_FM_POOL_MAGIC);
 		goto fail_bad;
 	}
@@ -619,25 +619,26 @@ static int ubi_attach_fastmap(struct ubi_device *ubi,
 	fm->max_wl_pool_size = be16_to_cpu(fmpl2->max_size);
 
 	if (pool_size > UBI_FM_MAX_POOL_SIZE || pool_size < 0) {
-		ubi_err("bad pool size: %i", pool_size);
+		ubi_err(ubi, "bad pool size: %i", pool_size);
 		goto fail_bad;
 	}
 
 	if (wl_pool_size > UBI_FM_MAX_POOL_SIZE || wl_pool_size < 0) {
-		ubi_err("bad WL pool size: %i", wl_pool_size);
+		ubi_err(ubi, "bad WL pool size: %i", wl_pool_size);
 		goto fail_bad;
 	}
 
 
 	if (fm->max_pool_size > UBI_FM_MAX_POOL_SIZE ||
 	    fm->max_pool_size < 0) {
-		ubi_err("bad maximal pool size: %i", fm->max_pool_size);
+		ubi_err(ubi, "bad maximal pool size: %i", fm->max_pool_size);
 		goto fail_bad;
 	}
 
 	if (fm->max_wl_pool_size > UBI_FM_MAX_POOL_SIZE ||
 	    fm->max_wl_pool_size < 0) {
-		ubi_err("bad maximal WL pool size: %i", fm->max_wl_pool_size);
+		ubi_err(ubi, "bad maximal WL pool size: %i",
+			fm->max_wl_pool_size);
 		goto fail_bad;
 	}
 
@@ -696,8 +697,7 @@ static int ubi_attach_fastmap(struct ubi_device *ubi,
 			goto fail_bad;
 
 		if (be32_to_cpu(fmvhdr->magic) != UBI_FM_VHDR_MAGIC) {
-			ubi_err("bad fastmap vol header magic: 0x%x, " \
-				"expected: 0x%x",
+			ubi_err(ubi, "bad fastmap vol header magic: 0x%x, expected: 0x%x",
 				be32_to_cpu(fmvhdr->magic), UBI_FM_VHDR_MAGIC);
 			goto fail_bad;
 		}
@@ -722,8 +722,7 @@ static int ubi_attach_fastmap(struct ubi_device *ubi,
 			goto fail_bad;
 
 		if (be32_to_cpu(fm_eba->magic) != UBI_FM_EBA_MAGIC) {
-			ubi_err("bad fastmap EBA header magic: 0x%x, " \
-				"expected: 0x%x",
+			ubi_err(ubi, "bad fastmap EBA header magic: 0x%x, expected: 0x%x",
 				be32_to_cpu(fm_eba->magic), UBI_FM_EBA_MAGIC);
 			goto fail_bad;
 		}
@@ -788,7 +787,7 @@ static int ubi_attach_fastmap(struct ubi_device *ubi,
 			int err;
 
 			if (ubi_io_is_bad(ubi, tmp_aeb->pnum)) {
-				ubi_err("bad PEB in fastmap EBA orphan list");
+				ubi_err(ubi, "bad PEB in fastmap EBA orphan list");
 				ret = UBI_BAD_FASTMAP;
 				kfree(ech);
 				goto fail;
@@ -796,8 +795,8 @@ static int ubi_attach_fastmap(struct ubi_device *ubi,
 
 			err = ubi_io_read_ec_hdr(ubi, tmp_aeb->pnum, ech, 0);
 			if (err && err != UBI_IO_BITFLIPS) {
-				ubi_err("unable to read EC header! PEB:%i " \
-					"err:%i", tmp_aeb->pnum, err);
+				ubi_err(ubi, "unable to read EC header! PEB:%i err:%i",
+					tmp_aeb->pnum, err);
 				ret = err > 0 ? UBI_BAD_FASTMAP : err;
 				kfree(ech);
 
@@ -908,14 +907,14 @@ int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		fm->to_be_tortured[0] = 1;
 
 	if (be32_to_cpu(fmsb->magic) != UBI_FM_SB_MAGIC) {
-		ubi_err("bad super block magic: 0x%x, expected: 0x%x",
+		ubi_err(ubi, "bad super block magic: 0x%x, expected: 0x%x",
 			be32_to_cpu(fmsb->magic), UBI_FM_SB_MAGIC);
 		ret = UBI_BAD_FASTMAP;
 		goto free_fm_sb;
 	}
 
 	if (fmsb->version != UBI_FM_FMT_VERSION) {
-		ubi_err("bad fastmap version: %i, expected: %i",
+		ubi_err(ubi, "bad fastmap version: %i, expected: %i",
 			fmsb->version, UBI_FM_FMT_VERSION);
 		ret = UBI_BAD_FASTMAP;
 		goto free_fm_sb;
@@ -923,15 +922,16 @@ int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
 
 	used_blocks = be32_to_cpu(fmsb->used_blocks);
 	if (used_blocks > UBI_FM_MAX_BLOCKS || used_blocks < 1) {
-		ubi_err("number of fastmap blocks is invalid: %i", used_blocks);
+		ubi_err(ubi, "number of fastmap blocks is invalid: %i",
+			used_blocks);
 		ret = UBI_BAD_FASTMAP;
 		goto free_fm_sb;
 	}
 
 	fm_size = ubi->leb_size * used_blocks;
 	if (fm_size != ubi->fm_size) {
-		ubi_err("bad fastmap size: %zi, expected: %zi", fm_size,
-			ubi->fm_size);
+		ubi_err(ubi, "bad fastmap size: %zi, expected: %zi",
+			fm_size, ubi->fm_size);
 		ret = UBI_BAD_FASTMAP;
 		goto free_fm_sb;
 	}
@@ -960,7 +960,7 @@ int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
 
 		ret = ubi_io_read_ec_hdr(ubi, pnum, ech, 0);
 		if (ret && ret != UBI_IO_BITFLIPS) {
-			ubi_err("unable to read fastmap block# %i EC (PEB: %i)",
+			ubi_err(ubi, "unable to read fastmap block# %i EC (PEB: %i)",
 				i, pnum);
 			if (ret > 0)
 				ret = UBI_BAD_FASTMAP;
@@ -977,7 +977,7 @@ int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		 * we shouldn't fail if image_seq == 0.
 		 */
 		if (image_seq && (image_seq != ubi->image_seq)) {
-			ubi_err("wrong image seq:%d instead of %d",
+			ubi_err(ubi, "wrong image seq:%d instead of %d",
 				be32_to_cpu(ech->image_seq), ubi->image_seq);
 			ret = UBI_BAD_FASTMAP;
 			goto free_hdr;
@@ -985,15 +985,14 @@ int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
 
 		ret = ubi_io_read_vid_hdr(ubi, pnum, vh, 0);
 		if (ret && ret != UBI_IO_BITFLIPS) {
-			ubi_err("unable to read fastmap block# %i (PEB: %i)",
+			ubi_err(ubi, "unable to read fastmap block# %i (PEB: %i)",
 				i, pnum);
 			goto free_hdr;
 		}
 
 		if (i == 0) {
 			if (be32_to_cpu(vh->vol_id) != UBI_FM_SB_VOLUME_ID) {
-				ubi_err("bad fastmap anchor vol_id: 0x%x," \
-					" expected: 0x%x",
+				ubi_err(ubi, "bad fastmap anchor vol_id: 0x%x, expected: 0x%x",
 					be32_to_cpu(vh->vol_id),
 					UBI_FM_SB_VOLUME_ID);
 				ret = UBI_BAD_FASTMAP;
@@ -1001,8 +1000,7 @@ int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
 			}
 		} else {
 			if (be32_to_cpu(vh->vol_id) != UBI_FM_DATA_VOLUME_ID) {
-				ubi_err("bad fastmap data vol_id: 0x%x," \
-					" expected: 0x%x",
+				ubi_err(ubi, "bad fastmap data vol_id: 0x%x, expected: 0x%x",
 					be32_to_cpu(vh->vol_id),
 					UBI_FM_DATA_VOLUME_ID);
 				ret = UBI_BAD_FASTMAP;
@@ -1016,7 +1014,7 @@ int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		ret = ubi_io_read(ubi, ubi->fm_buf + (ubi->leb_size * i), pnum,
 				  ubi->leb_start, ubi->leb_size);
 		if (ret && ret != UBI_IO_BITFLIPS) {
-			ubi_err("unable to read fastmap block# %i (PEB: %i, " \
+			ubi_err(ubi, "unable to read fastmap block# %i (PEB: %i, "
 				"err: %i)", i, pnum, ret);
 			goto free_hdr;
 		}
@@ -1030,8 +1028,9 @@ int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
 	fmsb2->data_crc = 0;
 	crc = crc32(UBI_CRC32_INIT, ubi->fm_buf, fm_size);
 	if (crc != tmp_crc) {
-		ubi_err("fastmap data CRC is invalid");
-		ubi_err("CRC should be: 0x%x, calc: 0x%x", tmp_crc, crc);
+		ubi_err(ubi, "fastmap data CRC is invalid");
+		ubi_err(ubi, "CRC should be: 0x%x, calc: 0x%x",
+			tmp_crc, crc);
 		ret = UBI_BAD_FASTMAP;
 		goto free_hdr;
 	}
@@ -1067,9 +1066,10 @@ int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
 	ubi->fm = fm;
 	ubi->fm_pool.max_size = ubi->fm->max_pool_size;
 	ubi->fm_wl_pool.max_size = ubi->fm->max_wl_pool_size;
-	ubi_msg("attached by fastmap");
-	ubi_msg("fastmap pool size: %d", ubi->fm_pool.max_size);
-	ubi_msg("fastmap WL pool size: %d", ubi->fm_wl_pool.max_size);
+	ubi_msg(ubi, "attached by fastmap");
+	ubi_msg(ubi, "fastmap pool size: %d", ubi->fm_pool.max_size);
+	ubi_msg(ubi, "fastmap WL pool size: %d",
+		ubi->fm_wl_pool.max_size);
 	ubi->fm_disabled = 0;
 
 	ubi_free_vid_hdr(ubi, vh);
@@ -1077,7 +1077,7 @@ int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
 out:
 	mutex_unlock(&ubi->fm_mutex);
 	if (ret == UBI_BAD_FASTMAP)
-		ubi_err("Attach by fastmap failed, doing a full scan!");
+		ubi_err(ubi, "Attach by fastmap failed, doing a full scan!");
 	return ret;
 
 free_hdr:
@@ -1273,7 +1273,7 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 	dbg_bld("writing fastmap SB to PEB %i", new_fm->e[0]->pnum);
 	ret = ubi_io_write_vid_hdr(ubi, new_fm->e[0]->pnum, avhdr);
 	if (ret) {
-		ubi_err("unable to write vid_hdr to fastmap SB!");
+		ubi_err(ubi, "unable to write vid_hdr to fastmap SB!");
 		goto out_kfree;
 	}
 
@@ -1293,7 +1293,7 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 			new_fm->e[i]->pnum, be64_to_cpu(dvhdr->sqnum));
 		ret = ubi_io_write_vid_hdr(ubi, new_fm->e[i]->pnum, dvhdr);
 		if (ret) {
-			ubi_err("unable to write vid_hdr to PEB %i!",
+			ubi_err(ubi, "unable to write vid_hdr to PEB %i!",
 				new_fm->e[i]->pnum);
 			goto out_kfree;
 		}
@@ -1303,7 +1303,7 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 		ret = ubi_io_write(ubi, fm_raw + (i * ubi->leb_size),
 			new_fm->e[i]->pnum, ubi->leb_start, ubi->leb_size);
 		if (ret) {
-			ubi_err("unable to write fastmap to PEB %i!",
+			ubi_err(ubi, "unable to write fastmap to PEB %i!",
 				new_fm->e[i]->pnum);
 			goto out_kfree;
 		}
@@ -1450,7 +1450,7 @@ int ubi_update_fastmap(struct ubi_device *ubi)
 	ubi->fm = NULL;
 
 	if (new_fm->used_blocks > UBI_FM_MAX_BLOCKS) {
-		ubi_err("fastmap too large");
+		ubi_err(ubi, "fastmap too large");
 		ret = -ENOSPC;
 		goto err;
 	}
@@ -1462,7 +1462,7 @@ int ubi_update_fastmap(struct ubi_device *ubi)
 
 		if (!tmp_e && !old_fm) {
 			int j;
-			ubi_err("could not get any free erase block");
+			ubi_err(ubi, "could not get any free erase block");
 
 			for (j = 1; j < i; j++)
 				ubi_wl_put_fm_peb(ubi, new_fm->e[j], j, 0);
@@ -1478,7 +1478,7 @@ int ubi_update_fastmap(struct ubi_device *ubi)
 					ubi_wl_put_fm_peb(ubi, new_fm->e[j],
 							  j, 0);
 
-				ubi_err("could not erase old fastmap PEB");
+				ubi_err(ubi, "could not erase old fastmap PEB");
 				goto err;
 			}
 
@@ -1504,7 +1504,7 @@ int ubi_update_fastmap(struct ubi_device *ubi)
 			ret = erase_block(ubi, old_fm->e[0]->pnum);
 			if (ret < 0) {
 				int i;
-				ubi_err("could not erase old anchor PEB");
+				ubi_err(ubi, "could not erase old anchor PEB");
 
 				for (i = 1; i < new_fm->used_blocks; i++)
 					ubi_wl_put_fm_peb(ubi, new_fm->e[i],
@@ -1525,7 +1525,7 @@ int ubi_update_fastmap(struct ubi_device *ubi)
 	} else {
 		if (!tmp_e) {
 			int i;
-			ubi_err("could not find any anchor PEB");
+			ubi_err(ubi, "could not find any anchor PEB");
 
 			for (i = 1; i < new_fm->used_blocks; i++)
 				ubi_wl_put_fm_peb(ubi, new_fm->e[i], i, 0);
@@ -1555,13 +1555,13 @@ out_unlock:
 err:
 	kfree(new_fm);
 
-	ubi_warn("Unable to write new fastmap, err=%i", ret);
+	ubi_warn(ubi, "Unable to write new fastmap, err=%i", ret);
 
 	ret = 0;
 	if (old_fm) {
 		ret = invalidate_fastmap(ubi, old_fm);
 		if (ret < 0)
-			ubi_err("Unable to invalidiate current fastmap!");
+			ubi_err(ubi, "Unable to invalidiate current fastmap!");
 		else if (ret)
 			ret = 0;
 	}
diff --git a/drivers/mtd/ubi/io.c b/drivers/mtd/ubi/io.c
index d36134925d31..396aaa543362 100644
--- a/drivers/mtd/ubi/io.c
+++ b/drivers/mtd/ubi/io.c
@@ -177,19 +177,20 @@ retry:
 			 * enabled. A corresponding message will be printed
 			 * later, when it is has been scrubbed.
 			 */
-			ubi_msg("fixable bit-flip detected at PEB %d", pnum);
+			ubi_msg(ubi, "fixable bit-flip detected at PEB %d",
+				pnum);
 			ubi_assert(len == read);
 			return UBI_IO_BITFLIPS;
 		}
 
 		if (retries++ < UBI_IO_RETRIES) {
-			ubi_warn("error %d%s while reading %d bytes from PEB %d:%d, read only %zd bytes, retry",
+			ubi_warn(ubi, "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",
+		ubi_err(ubi, "error %d%s while reading %d bytes from PEB %d:%d, read %zd bytes",
 			err, errstr, len, pnum, offset, read);
 		dump_stack();
 
@@ -246,7 +247,7 @@ int ubi_io_write(struct ubi_device *ubi, const void *buf, int pnum, int offset,
 	ubi_assert(len > 0 && len % ubi->hdrs_min_io_size == 0);
 
 	if (ubi->ro_mode) {
-		ubi_err("read-only mode");
+		ubi_err(ubi, "read-only mode");
 		return -EROFS;
 	}
 
@@ -273,7 +274,7 @@ int ubi_io_write(struct ubi_device *ubi, const void *buf, int pnum, int offset,
 	}
 
 	if (ubi_dbg_is_write_failure(ubi)) {
-		ubi_err("cannot write %d bytes to PEB %d:%d (emulated)",
+		ubi_err(ubi, "cannot write %d bytes to PEB %d:%d (emulated)",
 			len, pnum, offset);
 		dump_stack();
 		return -EIO;
@@ -282,7 +283,7 @@ int ubi_io_write(struct ubi_device *ubi, const void *buf, int pnum, int offset,
 	addr = (loff_t)pnum * ubi->peb_size + offset;
 	err = mtd_write(ubi->mtd, addr, len, &written, buf);
 	if (err) {
-		ubi_err("error %d while writing %d bytes to PEB %d:%d, written %zd bytes",
+		ubi_err(ubi, "error %d while writing %d bytes to PEB %d:%d, written %zd bytes",
 			err, len, pnum, offset, written);
 		dump_stack();
 		ubi_dump_flash(ubi, pnum, offset, len);
@@ -338,7 +339,7 @@ static int do_sync_erase(struct ubi_device *ubi, int pnum)
 	ubi_assert(pnum >= 0 && pnum < ubi->peb_count);
 
 	if (ubi->ro_mode) {
-		ubi_err("read-only mode");
+		ubi_err(ubi, "read-only mode");
 		return -EROFS;
 	}
 
@@ -355,12 +356,12 @@ retry:
 	err = mtd_erase(ubi->mtd, &ei);
 	if (err) {
 		if (retries++ < UBI_IO_RETRIES) {
-			ubi_warn("error %d while erasing PEB %d, retry",
+			ubi_warn(ubi, "error %d while erasing PEB %d, retry",
 				 err, pnum);
 			yield();
 			goto retry;
 		}
-		ubi_err("cannot erase PEB %d, error %d", pnum, err);
+		ubi_err(ubi, "cannot erase PEB %d, error %d", pnum, err);
 		dump_stack();
 		return err;
 	}
@@ -368,17 +369,18 @@ retry:
 	err = wait_event_interruptible(wq, ei.state == MTD_ERASE_DONE ||
 					   ei.state == MTD_ERASE_FAILED);
 	if (err) {
-		ubi_err("interrupted PEB %d erasure", pnum);
+		ubi_err(ubi, "interrupted PEB %d erasure", pnum);
 		return -EINTR;
 	}
 
 	if (ei.state == MTD_ERASE_FAILED) {
 		if (retries++ < UBI_IO_RETRIES) {
-			ubi_warn("error while erasing PEB %d, retry", pnum);
+			ubi_warn(ubi, "error while erasing PEB %d, retry",
+				 pnum);
 			yield();
 			goto retry;
 		}
-		ubi_err("cannot erase PEB %d", pnum);
+		ubi_err(ubi, "cannot erase PEB %d", pnum);
 		dump_stack();
 		return -EIO;
 	}
@@ -388,7 +390,7 @@ retry:
 		return err;
 
 	if (ubi_dbg_is_erase_failure(ubi)) {
-		ubi_err("cannot erase PEB %d (emulated)", pnum);
+		ubi_err(ubi, "cannot erase PEB %d (emulated)", pnum);
 		return -EIO;
 	}
 
@@ -411,7 +413,7 @@ static int torture_peb(struct ubi_device *ubi, int pnum)
 {
 	int err, i, patt_count;
 
-	ubi_msg("run torture test for PEB %d", pnum);
+	ubi_msg(ubi, "run torture test for PEB %d", pnum);
 	patt_count = ARRAY_SIZE(patterns);
 	ubi_assert(patt_count > 0);
 
@@ -428,7 +430,7 @@ static int torture_peb(struct ubi_device *ubi, int pnum)
 
 		err = ubi_check_pattern(ubi->peb_buf, 0xFF, ubi->peb_size);
 		if (err == 0) {
-			ubi_err("erased PEB %d, but a non-0xFF byte found",
+			ubi_err(ubi, "erased PEB %d, but a non-0xFF byte found",
 				pnum);
 			err = -EIO;
 			goto out;
@@ -448,7 +450,7 @@ static int torture_peb(struct ubi_device *ubi, int pnum)
 		err = ubi_check_pattern(ubi->peb_buf, patterns[i],
 					ubi->peb_size);
 		if (err == 0) {
-			ubi_err("pattern %x checking failed for PEB %d",
+			ubi_err(ubi, "pattern %x checking failed for PEB %d",
 				patterns[i], pnum);
 			err = -EIO;
 			goto out;
@@ -456,7 +458,7 @@ static int torture_peb(struct ubi_device *ubi, int pnum)
 	}
 
 	err = patt_count;
-	ubi_msg("PEB %d passed torture test, do not mark it as bad", pnum);
+	ubi_msg(ubi, "PEB %d passed torture test, do not mark it as bad", pnum);
 
 out:
 	mutex_unlock(&ubi->buf_mutex);
@@ -466,7 +468,7 @@ out:
 		 * has not passed because it happened on a freshly erased
 		 * physical eraseblock which means something is wrong with it.
 		 */
-		ubi_err("read problems on freshly erased PEB %d, must be bad",
+		ubi_err(ubi, "read problems on freshly erased PEB %d, must be bad",
 			pnum);
 		err = -EIO;
 	}
@@ -542,7 +544,7 @@ error:
 	 * it. Supposedly the flash media or the driver is screwed up, so
 	 * return an error.
 	 */
-	ubi_err("cannot invalidate PEB %d, write returned %d", pnum, err);
+	ubi_err(ubi, "cannot invalidate PEB %d, write returned %d", pnum, err);
 	ubi_dump_flash(ubi, pnum, 0, ubi->peb_size);
 	return -EIO;
 }
@@ -574,7 +576,7 @@ int ubi_io_sync_erase(struct ubi_device *ubi, int pnum, int torture)
 		return err;
 
 	if (ubi->ro_mode) {
-		ubi_err("read-only mode");
+		ubi_err(ubi, "read-only mode");
 		return -EROFS;
 	}
 
@@ -616,7 +618,7 @@ int ubi_io_is_bad(const struct ubi_device *ubi, int pnum)
 
 		ret = mtd_block_isbad(mtd, (loff_t)pnum * ubi->peb_size);
 		if (ret < 0)
-			ubi_err("error %d while checking if PEB %d is bad",
+			ubi_err(ubi, "error %d while checking if PEB %d is bad",
 				ret, pnum);
 		else if (ret)
 			dbg_io("PEB %d is bad", pnum);
@@ -642,7 +644,7 @@ int ubi_io_mark_bad(const struct ubi_device *ubi, int pnum)
 	ubi_assert(pnum >= 0 && pnum < ubi->peb_count);
 
 	if (ubi->ro_mode) {
-		ubi_err("read-only mode");
+		ubi_err(ubi, "read-only mode");
 		return -EROFS;
 	}
 
@@ -651,7 +653,7 @@ int ubi_io_mark_bad(const struct ubi_device *ubi, int pnum)
 
 	err = mtd_block_markbad(mtd, (loff_t)pnum * ubi->peb_size);
 	if (err)
-		ubi_err("cannot mark PEB %d bad, error %d", pnum, err);
+		ubi_err(ubi, "cannot mark PEB %d bad, error %d", pnum, err);
 	return err;
 }
 
@@ -674,32 +676,32 @@ static int validate_ec_hdr(const struct ubi_device *ubi,
 	leb_start = be32_to_cpu(ec_hdr->data_offset);
 
 	if (ec_hdr->version != UBI_VERSION) {
-		ubi_err("node with incompatible UBI version found: this UBI version is %d, image version is %d",
+		ubi_err(ubi, "node with incompatible UBI version found: this UBI version is %d, image version is %d",
 			UBI_VERSION, (int)ec_hdr->version);
 		goto bad;
 	}
 
 	if (vid_hdr_offset != ubi->vid_hdr_offset) {
-		ubi_err("bad VID header offset %d, expected %d",
+		ubi_err(ubi, "bad VID header offset %d, expected %d",
 			vid_hdr_offset, ubi->vid_hdr_offset);
 		goto bad;
 	}
 
 	if (leb_start != ubi->leb_start) {
-		ubi_err("bad data offset %d, expected %d",
+		ubi_err(ubi, "bad data offset %d, expected %d",
 			leb_start, ubi->leb_start);
 		goto bad;
 	}
 
 	if (ec < 0 || ec > UBI_MAX_ERASECOUNTER) {
-		ubi_err("bad erase counter %lld", ec);
+		ubi_err(ubi, "bad erase counter %lld", ec);
 		goto bad;
 	}
 
 	return 0;
 
 bad:
-	ubi_err("bad EC header");
+	ubi_err(ubi, "bad EC header");
 	ubi_dump_ec_hdr(ec_hdr);
 	dump_stack();
 	return 1;
@@ -765,7 +767,7 @@ int ubi_io_read_ec_hdr(struct ubi_device *ubi, int pnum,
 		if (ubi_check_pattern(ec_hdr, 0xFF, UBI_EC_HDR_SIZE)) {
 			/* The physical eraseblock is supposedly empty */
 			if (verbose)
-				ubi_warn("no EC header found at PEB %d, only 0xFF bytes",
+				ubi_warn(ubi, "no EC header found at PEB %d, only 0xFF bytes",
 					 pnum);
 			dbg_bld("no EC header found at PEB %d, only 0xFF bytes",
 				pnum);
@@ -780,7 +782,7 @@ int ubi_io_read_ec_hdr(struct ubi_device *ubi, int pnum,
 		 * 0xFF bytes. Report that the header is corrupted.
 		 */
 		if (verbose) {
-			ubi_warn("bad magic number at PEB %d: %08x instead of %08x",
+			ubi_warn(ubi, "bad magic number at PEB %d: %08x instead of %08x",
 				 pnum, magic, UBI_EC_HDR_MAGIC);
 			ubi_dump_ec_hdr(ec_hdr);
 		}
@@ -794,7 +796,7 @@ int ubi_io_read_ec_hdr(struct ubi_device *ubi, int pnum,
 
 	if (hdr_crc != crc) {
 		if (verbose) {
-			ubi_warn("bad EC header CRC at PEB %d, calculated %#08x, read %#08x",
+			ubi_warn(ubi, "bad EC header CRC at PEB %d, calculated %#08x, read %#08x",
 				 pnum, crc, hdr_crc);
 			ubi_dump_ec_hdr(ec_hdr);
 		}
@@ -810,7 +812,7 @@ int ubi_io_read_ec_hdr(struct ubi_device *ubi, int pnum,
 	/* And of course validate what has just been read from the media */
 	err = validate_ec_hdr(ubi, ec_hdr);
 	if (err) {
-		ubi_err("validation failed for PEB %d", pnum);
+		ubi_err(ubi, "validation failed for PEB %d", pnum);
 		return -EINVAL;
 	}
 
@@ -884,40 +886,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) {
-		ubi_err("bad copy_flag");
+		ubi_err(ubi, "bad copy_flag");
 		goto bad;
 	}
 
 	if (vol_id < 0 || lnum < 0 || data_size < 0 || used_ebs < 0 ||
 	    data_pad < 0) {
-		ubi_err("negative values");
+		ubi_err(ubi, "negative values");
 		goto bad;
 	}
 
 	if (vol_id >= UBI_MAX_VOLUMES && vol_id < UBI_INTERNAL_VOL_START) {
-		ubi_err("bad vol_id");
+		ubi_err(ubi, "bad vol_id");
 		goto bad;
 	}
 
 	if (vol_id < UBI_INTERNAL_VOL_START && compat != 0) {
-		ubi_err("bad compat");
+		ubi_err(ubi, "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) {
-		ubi_err("bad compat");
+		ubi_err(ubi, "bad compat");
 		goto bad;
 	}
 
 	if (vol_type != UBI_VID_DYNAMIC && vol_type != UBI_VID_STATIC) {
-		ubi_err("bad vol_type");
+		ubi_err(ubi, "bad vol_type");
 		goto bad;
 	}
 
 	if (data_pad >= ubi->leb_size / 2) {
-		ubi_err("bad data_pad");
+		ubi_err(ubi, "bad data_pad");
 		goto bad;
 	}
 
@@ -929,45 +931,45 @@ static int validate_vid_hdr(const struct ubi_device *ubi,
 		 * mapped logical eraseblocks.
 		 */
 		if (used_ebs == 0) {
-			ubi_err("zero used_ebs");
+			ubi_err(ubi, "zero used_ebs");
 			goto bad;
 		}
 		if (data_size == 0) {
-			ubi_err("zero data_size");
+			ubi_err(ubi, "zero data_size");
 			goto bad;
 		}
 		if (lnum < used_ebs - 1) {
 			if (data_size != usable_leb_size) {
-				ubi_err("bad data_size");
+				ubi_err(ubi, "bad data_size");
 				goto bad;
 			}
 		} else if (lnum == used_ebs - 1) {
 			if (data_size == 0) {
-				ubi_err("bad data_size at last LEB");
+				ubi_err(ubi, "bad data_size at last LEB");
 				goto bad;
 			}
 		} else {
-			ubi_err("too high lnum");
+			ubi_err(ubi, "too high lnum");
 			goto bad;
 		}
 	} else {
 		if (copy_flag == 0) {
 			if (data_crc != 0) {
-				ubi_err("non-zero data CRC");
+				ubi_err(ubi, "non-zero data CRC");
 				goto bad;
 			}
 			if (data_size != 0) {
-				ubi_err("non-zero data_size");
+				ubi_err(ubi, "non-zero data_size");
 				goto bad;
 			}
 		} else {
 			if (data_size == 0) {
-				ubi_err("zero data_size of copy");
+				ubi_err(ubi, "zero data_size of copy");
 				goto bad;
 			}
 		}
 		if (used_ebs != 0) {
-			ubi_err("bad used_ebs");
+			ubi_err(ubi, "bad used_ebs");
 			goto bad;
 		}
 	}
@@ -975,7 +977,7 @@ static int validate_vid_hdr(const struct ubi_device *ubi,
 	return 0;
 
 bad:
-	ubi_err("bad VID header");
+	ubi_err(ubi, "bad VID header");
 	ubi_dump_vid_hdr(vid_hdr);
 	dump_stack();
 	return 1;
@@ -1020,7 +1022,7 @@ int ubi_io_read_vid_hdr(struct ubi_device *ubi, int pnum,
 
 		if (ubi_check_pattern(vid_hdr, 0xFF, UBI_VID_HDR_SIZE)) {
 			if (verbose)
-				ubi_warn("no VID header found at PEB %d, only 0xFF bytes",
+				ubi_warn(ubi, "no VID header found at PEB %d, only 0xFF bytes",
 					 pnum);
 			dbg_bld("no VID header found at PEB %d, only 0xFF bytes",
 				pnum);
@@ -1031,7 +1033,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",
+			ubi_warn(ubi, "bad magic number at PEB %d: %08x instead of %08x",
 				 pnum, magic, UBI_VID_HDR_MAGIC);
 			ubi_dump_vid_hdr(vid_hdr);
 		}
@@ -1045,7 +1047,7 @@ int ubi_io_read_vid_hdr(struct ubi_device *ubi, int pnum,
 
 	if (hdr_crc != crc) {
 		if (verbose) {
-			ubi_warn("bad CRC at PEB %d, calculated %#08x, read %#08x",
+			ubi_warn(ubi, "bad CRC at PEB %d, calculated %#08x, read %#08x",
 				 pnum, crc, hdr_crc);
 			ubi_dump_vid_hdr(vid_hdr);
 		}
@@ -1059,7 +1061,7 @@ int ubi_io_read_vid_hdr(struct ubi_device *ubi, int pnum,
 
 	err = validate_vid_hdr(ubi, vid_hdr);
 	if (err) {
-		ubi_err("validation failed for PEB %d", pnum);
+		ubi_err(ubi, "validation failed for PEB %d", pnum);
 		return -EINVAL;
 	}
 
@@ -1129,7 +1131,7 @@ static int self_check_not_bad(const struct ubi_device *ubi, int pnum)
 	if (!err)
 		return err;
 
-	ubi_err("self-check failed for PEB %d", pnum);
+	ubi_err(ubi, "self-check failed for PEB %d", pnum);
 	dump_stack();
 	return err > 0 ? -EINVAL : err;
 }
@@ -1154,14 +1156,14 @@ static int self_check_ec_hdr(const struct ubi_device *ubi, int pnum,
 
 	magic = be32_to_cpu(ec_hdr->magic);
 	if (magic != UBI_EC_HDR_MAGIC) {
-		ubi_err("bad magic %#08x, must be %#08x",
+		ubi_err(ubi, "bad magic %#08x, must be %#08x",
 			magic, UBI_EC_HDR_MAGIC);
 		goto fail;
 	}
 
 	err = validate_ec_hdr(ubi, ec_hdr);
 	if (err) {
-		ubi_err("self-check failed for PEB %d", pnum);
+		ubi_err(ubi, "self-check failed for PEB %d", pnum);
 		goto fail;
 	}
 
@@ -1201,8 +1203,9 @@ static int self_check_peb_ec_hdr(const struct ubi_device *ubi, int pnum)
 	crc = crc32(UBI_CRC32_INIT, ec_hdr, UBI_EC_HDR_SIZE_CRC);
 	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("self-check failed for PEB %d", pnum);
+		ubi_err(ubi, "bad CRC, calculated %#08x, read %#08x",
+			crc, hdr_crc);
+		ubi_err(ubi, "self-check failed for PEB %d", pnum);
 		ubi_dump_ec_hdr(ec_hdr);
 		dump_stack();
 		err = -EINVAL;
@@ -1236,21 +1239,21 @@ static int self_check_vid_hdr(const struct ubi_device *ubi, int pnum,
 
 	magic = be32_to_cpu(vid_hdr->magic);
 	if (magic != UBI_VID_HDR_MAGIC) {
-		ubi_err("bad VID header magic %#08x at PEB %d, must be %#08x",
+		ubi_err(ubi, "bad VID header magic %#08x at PEB %d, must be %#08x",
 			magic, pnum, UBI_VID_HDR_MAGIC);
 		goto fail;
 	}
 
 	err = validate_vid_hdr(ubi, vid_hdr);
 	if (err) {
-		ubi_err("self-check failed for PEB %d", pnum);
+		ubi_err(ubi, "self-check failed for PEB %d", pnum);
 		goto fail;
 	}
 
 	return err;
 
 fail:
-	ubi_err("self-check failed for PEB %d", pnum);
+	ubi_err(ubi, "self-check failed for PEB %d", pnum);
 	ubi_dump_vid_hdr(vid_hdr);
 	dump_stack();
 	return -EINVAL;
@@ -1288,9 +1291,9 @@ static int self_check_peb_vid_hdr(const struct ubi_device *ubi, int pnum)
 	crc = crc32(UBI_CRC32_INIT, vid_hdr, UBI_EC_HDR_SIZE_CRC);
 	hdr_crc = be32_to_cpu(vid_hdr->hdr_crc);
 	if (hdr_crc != crc) {
-		ubi_err("bad VID header CRC at PEB %d, calculated %#08x, read %#08x",
+		ubi_err(ubi, "bad VID header CRC at PEB %d, calculated %#08x, read %#08x",
 			pnum, crc, hdr_crc);
-		ubi_err("self-check failed for PEB %d", pnum);
+		ubi_err(ubi, "self-check failed for PEB %d", pnum);
 		ubi_dump_vid_hdr(vid_hdr);
 		dump_stack();
 		err = -EINVAL;
@@ -1329,7 +1332,7 @@ static int self_check_write(struct ubi_device *ubi, const void *buf, int pnum,
 
 	buf1 = __vmalloc(len, GFP_NOFS, PAGE_KERNEL);
 	if (!buf1) {
-		ubi_err("cannot allocate memory to check writes");
+		ubi_err(ubi, "cannot allocate memory to check writes");
 		return 0;
 	}
 
@@ -1345,15 +1348,15 @@ static int self_check_write(struct ubi_device *ubi, const void *buf, int pnum,
 		if (c == c1)
 			continue;
 
-		ubi_err("self-check failed for PEB %d:%d, len %d",
+		ubi_err(ubi, "self-check failed for PEB %d:%d, len %d",
 			pnum, offset, len);
-		ubi_msg("data differ at position %d", i);
+		ubi_msg(ubi, "data differ at position %d", i);
 		dump_len = max_t(int, 128, len - i);
-		ubi_msg("hex dump of the original buffer from %d to %d",
+		ubi_msg(ubi, "hex dump of the original buffer from %d to %d",
 			i, i + dump_len);
 		print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET, 32, 1,
 			       buf + i, dump_len, 1);
-		ubi_msg("hex dump of the read buffer from %d to %d",
+		ubi_msg(ubi, "hex dump of the read buffer from %d to %d",
 			i, i + dump_len);
 		print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET, 32, 1,
 			       buf1 + i, dump_len, 1);
@@ -1393,20 +1396,20 @@ int ubi_self_check_all_ff(struct ubi_device *ubi, int pnum, int offset, int len)
 
 	buf = __vmalloc(len, GFP_NOFS, PAGE_KERNEL);
 	if (!buf) {
-		ubi_err("cannot allocate memory to check for 0xFFs");
+		ubi_err(ubi, "cannot allocate memory to check for 0xFFs");
 		return 0;
 	}
 
 	err = mtd_read(ubi->mtd, addr, len, &read, buf);
 	if (err && !mtd_is_bitflip(err)) {
-		ubi_err("error %d while reading %d bytes from PEB %d:%d, read %zd bytes",
+		ubi_err(ubi, "err %d while reading %d bytes from PEB %d:%d, read %zd bytes",
 			err, len, pnum, offset, read);
 		goto error;
 	}
 
 	err = ubi_check_pattern(buf, 0xFF, len);
 	if (err == 0) {
-		ubi_err("flash region at PEB %d:%d, length %d does not contain all 0xFF bytes",
+		ubi_err(ubi, "flash region at PEB %d:%d, length %d does not contain all 0xFF bytes",
 			pnum, offset, len);
 		goto fail;
 	}
@@ -1415,8 +1418,9 @@ int ubi_self_check_all_ff(struct ubi_device *ubi, int pnum, int offset, int len)
 	return 0;
 
 fail:
-	ubi_err("self-check failed for PEB %d", pnum);
-	ubi_msg("hex dump of the %d-%d region", offset, offset + len);
+	ubi_err(ubi, "self-check failed for PEB %d", pnum);
+	ubi_msg(ubi, "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:
diff --git a/drivers/mtd/ubi/kapi.c b/drivers/mtd/ubi/kapi.c
index 3aac1acceeb4..f3bab669f6bb 100644
--- a/drivers/mtd/ubi/kapi.c
+++ b/drivers/mtd/ubi/kapi.c
@@ -204,7 +204,7 @@ struct ubi_volume_desc *ubi_open_volume(int ubi_num, int vol_id, int mode)
 			return ERR_PTR(err);
 		}
 		if (err == 1) {
-			ubi_warn("volume %d on UBI device %d is corrupted",
+			ubi_warn(ubi, "volume %d on UBI device %d is corrupted",
 				 vol_id, ubi->ubi_num);
 			vol->corrupted = 1;
 		}
@@ -221,7 +221,7 @@ out_free:
 	kfree(desc);
 out_put_ubi:
 	ubi_put_device(ubi);
-	ubi_err("cannot open device %d, volume %d, error %d",
+	ubi_err(ubi, "cannot open device %d, volume %d, error %d",
 		ubi_num, vol_id, err);
 	return ERR_PTR(err);
 }
@@ -411,7 +411,7 @@ int ubi_leb_read(struct ubi_volume_desc *desc, int lnum, char *buf, int offset,
 
 	err = ubi_eba_read_leb(ubi, vol, lnum, buf, offset, len, check);
 	if (err && mtd_is_eccerr(err) && vol->vol_type == UBI_STATIC_VOLUME) {
-		ubi_warn("mark volume %d as corrupted", vol_id);
+		ubi_warn(ubi, "mark volume %d as corrupted", vol_id);
 		vol->corrupted = 1;
 	}
 
diff --git a/drivers/mtd/ubi/misc.c b/drivers/mtd/ubi/misc.c
index f913d701a5b3..dbda77e556cb 100644
--- a/drivers/mtd/ubi/misc.c
+++ b/drivers/mtd/ubi/misc.c
@@ -111,7 +111,7 @@ void ubi_update_reserved(struct ubi_device *ubi)
 	ubi->avail_pebs -= need;
 	ubi->rsvd_pebs += need;
 	ubi->beb_rsvd_pebs += need;
-	ubi_msg("reserved more %d PEBs for bad PEB handling", need);
+	ubi_msg(ubi, "reserved more %d PEBs for bad PEB handling", need);
 }
 
 /**
@@ -128,7 +128,7 @@ void ubi_calculate_reserved(struct ubi_device *ubi)
 	ubi->beb_rsvd_level = ubi->bad_peb_limit - ubi->bad_peb_count;
 	if (ubi->beb_rsvd_level < 0) {
 		ubi->beb_rsvd_level = 0;
-		ubi_warn("number of bad PEBs (%d) is above the expected limit (%d), not reserving any PEBs for bad PEB handling, will use available PEBs (if any)",
+		ubi_warn(ubi, "number of bad PEBs (%d) is above the expected limit (%d), not reserving any PEBs for bad PEB handling, will use available PEBs (if any)",
 			 ubi->bad_peb_count, ubi->bad_peb_limit);
 	}
 }
diff --git a/drivers/mtd/ubi/ubi.h b/drivers/mtd/ubi/ubi.h
index 320fc38fa2a1..f80ffaba9058 100644
--- a/drivers/mtd/ubi/ubi.h
+++ b/drivers/mtd/ubi/ubi.h
@@ -50,13 +50,14 @@
 #define UBI_NAME_STR "ubi"
 
 /* Normal UBI messages */
-#define ubi_msg(fmt, ...) pr_notice("UBI: " fmt "\n", ##__VA_ARGS__)
+#define ubi_msg(ubi, fmt, ...) pr_notice("UBI-%d: %s:" fmt "\n", \
+					 ubi->ubi_num, __func__, ##__VA_ARGS__)
 /* UBI warning messages */
-#define ubi_warn(fmt, ...) pr_warn("UBI warning: %s: " fmt "\n",  \
-				   __func__, ##__VA_ARGS__)
+#define ubi_warn(ubi, fmt, ...) pr_warn("UBI-%d warning: %s: " fmt "\n", \
+					ubi->ubi_num, __func__, ##__VA_ARGS__)
 /* UBI error messages */
-#define ubi_err(fmt, ...) pr_err("UBI error: %s: " fmt "\n",      \
-				 __func__, ##__VA_ARGS__)
+#define ubi_err(ubi, fmt, ...) pr_err("UBI-%d error: %s: " fmt "\n", \
+				      ubi->ubi_num, __func__, ##__VA_ARGS__)
 
 /* Background thread name pattern */
 #define UBI_BGT_NAME_PATTERN "ubi_bgt%dd"
@@ -987,7 +988,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_warn(ubi, "switch to read-only mode");
 		dump_stack();
 	}
 }
diff --git a/drivers/mtd/ubi/upd.c b/drivers/mtd/ubi/upd.c
index ec2c2dc1c1ca..2a1b6e037e1a 100644
--- a/drivers/mtd/ubi/upd.c
+++ b/drivers/mtd/ubi/upd.c
@@ -133,6 +133,10 @@ int ubi_start_update(struct ubi_device *ubi, struct ubi_volume *vol,
 	ubi_assert(!vol->updating && !vol->changing_leb);
 	vol->updating = 1;
 
+	vol->upd_buf = vmalloc(ubi->leb_size);
+	if (!vol->upd_buf)
+		return -ENOMEM;
+
 	err = set_update_marker(ubi, vol);
 	if (err)
 		return err;
@@ -152,14 +156,12 @@ int ubi_start_update(struct ubi_device *ubi, struct ubi_volume *vol,
 		err = clear_update_marker(ubi, vol, 0);
 		if (err)
 			return err;
+
+		vfree(vol->upd_buf);
 		vol->updating = 0;
 		return 0;
 	}
 
-	vol->upd_buf = vmalloc(ubi->leb_size);
-	if (!vol->upd_buf)
-		return -ENOMEM;
-
 	vol->upd_ebs = div_u64(bytes + vol->usable_leb_size - 1,
 			       vol->usable_leb_size);
 	vol->upd_bytes = bytes;
diff --git a/drivers/mtd/ubi/vmt.c b/drivers/mtd/ubi/vmt.c
index 8330703c098f..ff4d97848d1c 100644
--- a/drivers/mtd/ubi/vmt.c
+++ b/drivers/mtd/ubi/vmt.c
@@ -223,7 +223,7 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 			}
 
 		if (vol_id == UBI_VOL_NUM_AUTO) {
-			ubi_err("out of volume IDs");
+			ubi_err(ubi, "out of volume IDs");
 			err = -ENFILE;
 			goto out_unlock;
 		}
@@ -237,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]) {
-		ubi_err("volume %d already exists", vol_id);
+		ubi_err(ubi, "volume %d already exists", vol_id);
 		goto out_unlock;
 	}
 
@@ -246,7 +246,8 @@ 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)) {
-			ubi_err("volume \"%s\" exists (ID %d)", req->name, i);
+			ubi_err(ubi, "volume \"%s\" exists (ID %d)",
+				req->name, i);
 			goto out_unlock;
 		}
 
@@ -257,9 +258,10 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 
 	/* Reserve physical eraseblocks */
 	if (vol->reserved_pebs > ubi->avail_pebs) {
-		ubi_err("not enough PEBs, only %d available", ubi->avail_pebs);
+		ubi_err(ubi, "not enough PEBs, only %d available",
+			ubi->avail_pebs);
 		if (ubi->corr_peb_count)
-			ubi_err("%d PEBs are corrupted and not used",
+			ubi_err(ubi, "%d PEBs are corrupted and not used",
 				ubi->corr_peb_count);
 		err = -ENOSPC;
 		goto out_unlock;
@@ -314,7 +316,7 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 	dev = MKDEV(MAJOR(ubi->cdev.dev), vol_id + 1);
 	err = cdev_add(&vol->cdev, dev, 1);
 	if (err) {
-		ubi_err("cannot add character device");
+		ubi_err(ubi, "cannot add character device");
 		goto out_mapping;
 	}
 
@@ -326,7 +328,7 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 	dev_set_name(&vol->dev, "%s_%d", ubi->ubi_name, vol->vol_id);
 	err = device_register(&vol->dev);
 	if (err) {
-		ubi_err("cannot register device");
+		ubi_err(ubi, "cannot register device");
 		goto out_cdev;
 	}
 
@@ -386,7 +388,7 @@ out_unlock:
 		kfree(vol);
 	else
 		put_device(&vol->dev);
-	ubi_err("cannot create volume %d, error %d", vol_id, err);
+	ubi_err(ubi, "cannot create volume %d, error %d", vol_id, err);
 	return err;
 }
 
@@ -454,7 +456,7 @@ int ubi_remove_volume(struct ubi_volume_desc *desc, int no_vtbl)
 	return err;
 
 out_err:
-	ubi_err("cannot remove volume %d, error %d", vol_id, err);
+	ubi_err(ubi, "cannot remove volume %d, error %d", vol_id, err);
 	spin_lock(&ubi->volumes_lock);
 	ubi->volumes[vol_id] = vol;
 out_unlock:
@@ -487,7 +489,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) {
-		ubi_err("too small size %d, %d LEBs contain data",
+		ubi_err(ubi, "too small size %d, %d LEBs contain data",
 			reserved_pebs, vol->used_ebs);
 		return -EINVAL;
 	}
@@ -516,10 +518,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) {
-			ubi_err("not enough PEBs: requested %d, available %d",
+			ubi_err(ubi, "not enough PEBs: requested %d, available %d",
 				pebs, ubi->avail_pebs);
 			if (ubi->corr_peb_count)
-				ubi_err("%d PEBs are corrupted and not used",
+				ubi_err(ubi, "%d PEBs are corrupted and not used",
 					ubi->corr_peb_count);
 			spin_unlock(&ubi->volumes_lock);
 			err = -ENOSPC;
@@ -643,7 +645,7 @@ int ubi_add_volume(struct ubi_device *ubi, struct ubi_volume *vol)
 	dev = MKDEV(MAJOR(ubi->cdev.dev), vol->vol_id + 1);
 	err = cdev_add(&vol->cdev, dev, 1);
 	if (err) {
-		ubi_err("cannot add character device for volume %d, error %d",
+		ubi_err(ubi, "cannot add character device for volume %d, error %d",
 			vol_id, err);
 		return err;
 	}
@@ -710,7 +712,7 @@ static int self_check_volume(struct ubi_device *ubi, int vol_id)
 
 	if (!vol) {
 		if (reserved_pebs) {
-			ubi_err("no volume info, but volume exists");
+			ubi_err(ubi, "no volume info, but volume exists");
 			goto fail;
 		}
 		spin_unlock(&ubi->volumes_lock);
@@ -719,90 +721,91 @@ static int self_check_volume(struct ubi_device *ubi, int vol_id)
 
 	if (vol->reserved_pebs < 0 || vol->alignment < 0 || vol->data_pad < 0 ||
 	    vol->name_len < 0) {
-		ubi_err("negative values");
+		ubi_err(ubi, "negative values");
 		goto fail;
 	}
 	if (vol->alignment > ubi->leb_size || vol->alignment == 0) {
-		ubi_err("bad alignment");
+		ubi_err(ubi, "bad alignment");
 		goto fail;
 	}
 
 	n = vol->alignment & (ubi->min_io_size - 1);
 	if (vol->alignment != 1 && n) {
-		ubi_err("alignment is not multiple of min I/O unit");
+		ubi_err(ubi, "alignment is not multiple of min I/O unit");
 		goto fail;
 	}
 
 	n = ubi->leb_size % vol->alignment;
 	if (vol->data_pad != n) {
-		ubi_err("bad data_pad, has to be %lld", n);
+		ubi_err(ubi, "bad data_pad, has to be %lld", n);
 		goto fail;
 	}
 
 	if (vol->vol_type != UBI_DYNAMIC_VOLUME &&
 	    vol->vol_type != UBI_STATIC_VOLUME) {
-		ubi_err("bad vol_type");
+		ubi_err(ubi, "bad vol_type");
 		goto fail;
 	}
 
 	if (vol->upd_marker && vol->corrupted) {
-		ubi_err("update marker and corrupted simultaneously");
+		ubi_err(ubi, "update marker and corrupted simultaneously");
 		goto fail;
 	}
 
 	if (vol->reserved_pebs > ubi->good_peb_count) {
-		ubi_err("too large reserved_pebs");
+		ubi_err(ubi, "too large reserved_pebs");
 		goto fail;
 	}
 
 	n = ubi->leb_size - vol->data_pad;
 	if (vol->usable_leb_size != ubi->leb_size - vol->data_pad) {
-		ubi_err("bad usable_leb_size, has to be %lld", n);
+		ubi_err(ubi, "bad usable_leb_size, has to be %lld", n);
 		goto fail;
 	}
 
 	if (vol->name_len > UBI_VOL_NAME_MAX) {
-		ubi_err("too long volume name, max is %d", UBI_VOL_NAME_MAX);
+		ubi_err(ubi, "too long volume name, max is %d",
+			UBI_VOL_NAME_MAX);
 		goto fail;
 	}
 
 	n = strnlen(vol->name, vol->name_len + 1);
 	if (n != vol->name_len) {
-		ubi_err("bad name_len %lld", n);
+		ubi_err(ubi, "bad name_len %lld", n);
 		goto fail;
 	}
 
 	n = (long long)vol->used_ebs * vol->usable_leb_size;
 	if (vol->vol_type == UBI_DYNAMIC_VOLUME) {
 		if (vol->corrupted) {
-			ubi_err("corrupted dynamic volume");
+			ubi_err(ubi, "corrupted dynamic volume");
 			goto fail;
 		}
 		if (vol->used_ebs != vol->reserved_pebs) {
-			ubi_err("bad used_ebs");
+			ubi_err(ubi, "bad used_ebs");
 			goto fail;
 		}
 		if (vol->last_eb_bytes != vol->usable_leb_size) {
-			ubi_err("bad last_eb_bytes");
+			ubi_err(ubi, "bad last_eb_bytes");
 			goto fail;
 		}
 		if (vol->used_bytes != n) {
-			ubi_err("bad used_bytes");
+			ubi_err(ubi, "bad used_bytes");
 			goto fail;
 		}
 	} else {
 		if (vol->used_ebs < 0 || vol->used_ebs > vol->reserved_pebs) {
-			ubi_err("bad used_ebs");
+			ubi_err(ubi, "bad used_ebs");
 			goto fail;
 		}
 		if (vol->last_eb_bytes < 0 ||
 		    vol->last_eb_bytes > vol->usable_leb_size) {
-			ubi_err("bad last_eb_bytes");
+			ubi_err(ubi, "bad last_eb_bytes");
 			goto fail;
 		}
 		if (vol->used_bytes < 0 || vol->used_bytes > n ||
 		    vol->used_bytes < n - vol->usable_leb_size) {
-			ubi_err("bad used_bytes");
+			ubi_err(ubi, "bad used_bytes");
 			goto fail;
 		}
 	}
@@ -820,7 +823,7 @@ static int self_check_volume(struct ubi_device *ubi, int vol_id)
 	if (alignment != vol->alignment || data_pad != vol->data_pad ||
 	    upd_marker != vol->upd_marker || vol_type != vol->vol_type ||
 	    name_len != vol->name_len || strncmp(name, vol->name, name_len)) {
-		ubi_err("volume info is different");
+		ubi_err(ubi, "volume info is different");
 		goto fail;
 	}
 
@@ -828,7 +831,7 @@ static int self_check_volume(struct ubi_device *ubi, int vol_id)
 	return 0;
 
 fail:
-	ubi_err("self-check failed for volume %d", vol_id);
+	ubi_err(ubi, "self-check failed for volume %d", vol_id);
 	if (vol)
 		ubi_dump_vol_info(vol);
 	ubi_dump_vtbl_record(&ubi->vtbl[vol_id], vol_id);
diff --git a/drivers/mtd/ubi/vtbl.c b/drivers/mtd/ubi/vtbl.c
index 07cac5f9ffb8..f8fc3081bbb4 100644
--- a/drivers/mtd/ubi/vtbl.c
+++ b/drivers/mtd/ubi/vtbl.c
@@ -30,9 +30,12 @@
  * eraseblock stores one volume table copy, i.e. LEB 0 and LEB 1 duplicate each
  * other. This redundancy guarantees robustness to unclean reboots. The volume
  * table is basically an array of volume table records. Each record contains
- * full information about the volume and protected by a CRC checksum.
+ * full information about the volume and protected by a CRC checksum. Note,
+ * nowadays we use the atomic LEB change operation when updating the volume
+ * table, so we do not really need 2 LEBs anymore, but we preserve the older
+ * design for the backward compatibility reasons.
  *
- * The volume table is changed, it is first changed in RAM. Then LEB 0 is
+ * When the volume table is changed, it is first changed in RAM. Then LEB 0 is
  * erased, and the updated volume table is written back to LEB 0. Then same for
  * LEB 1. This scheme guarantees recoverability from unclean reboots.
  *
@@ -96,12 +99,8 @@ int ubi_change_vtbl_record(struct ubi_device *ubi, int idx,
 
 	memcpy(&ubi->vtbl[idx], vtbl_rec, sizeof(struct ubi_vtbl_record));
 	for (i = 0; i < UBI_LAYOUT_VOLUME_EBS; i++) {
-		err = ubi_eba_unmap_leb(ubi, layout_vol, i);
-		if (err)
-			return err;
-
-		err = ubi_eba_write_leb(ubi, layout_vol, i, ubi->vtbl, 0,
-					ubi->vtbl_size);
+		err = ubi_eba_atomic_leb_change(ubi, layout_vol, i, ubi->vtbl,
+						ubi->vtbl_size);
 		if (err)
 			return err;
 	}
@@ -148,12 +147,8 @@ int ubi_vtbl_rename_volumes(struct ubi_device *ubi,
 
 	layout_vol = ubi->volumes[vol_id2idx(ubi, UBI_LAYOUT_VOLUME_ID)];
 	for (i = 0; i < UBI_LAYOUT_VOLUME_EBS; i++) {
-		err = ubi_eba_unmap_leb(ubi, layout_vol, i);
-		if (err)
-			return err;
-
-		err = ubi_eba_write_leb(ubi, layout_vol, i, ubi->vtbl, 0,
-					ubi->vtbl_size);
+		err = ubi_eba_atomic_leb_change(ubi, layout_vol, i, ubi->vtbl,
+						ubi->vtbl_size);
 		if (err)
 			return err;
 	}
@@ -190,7 +185,7 @@ static int vtbl_check(const struct ubi_device *ubi,
 
 		crc = crc32(UBI_CRC32_INIT, &vtbl[i], UBI_VTBL_RECORD_SIZE_CRC);
 		if (be32_to_cpu(vtbl[i].crc) != crc) {
-			ubi_err("bad CRC at record %u: %#08x, not %#08x",
+			ubi_err(ubi, "bad CRC at record %u: %#08x, not %#08x",
 				 i, crc, be32_to_cpu(vtbl[i].crc));
 			ubi_dump_vtbl_record(&vtbl[i], i);
 			return 1;
@@ -224,7 +219,7 @@ static int vtbl_check(const struct ubi_device *ubi,
 
 		n = ubi->leb_size % alignment;
 		if (data_pad != n) {
-			ubi_err("bad data_pad, has to be %d", n);
+			ubi_err(ubi, "bad data_pad, has to be %d", n);
 			err = 6;
 			goto bad;
 		}
@@ -240,7 +235,7 @@ static int vtbl_check(const struct ubi_device *ubi,
 		}
 
 		if (reserved_pebs > ubi->good_peb_count) {
-			ubi_err("too large reserved_pebs %d, good PEBs %d",
+			ubi_err(ubi, "too large reserved_pebs %d, good PEBs %d",
 				reserved_pebs, ubi->good_peb_count);
 			err = 9;
 			goto bad;
@@ -270,7 +265,7 @@ static int vtbl_check(const struct ubi_device *ubi,
 
 			if (len1 > 0 && len1 == len2 &&
 			    !strncmp(vtbl[i].name, vtbl[n].name, len1)) {
-				ubi_err("volumes %d and %d have the same name \"%s\"",
+				ubi_err(ubi, "volumes %d and %d have the same name \"%s\"",
 					i, n, vtbl[i].name);
 				ubi_dump_vtbl_record(&vtbl[i], i);
 				ubi_dump_vtbl_record(&vtbl[n], n);
@@ -282,7 +277,7 @@ static int vtbl_check(const struct ubi_device *ubi,
 	return 0;
 
 bad:
-	ubi_err("volume table check failed: record %d, error %d", i, err);
+	ubi_err(ubi, "volume table check failed: record %d, error %d", i, err);
 	ubi_dump_vtbl_record(&vtbl[i], i);
 	return -EINVAL;
 }
@@ -446,11 +441,11 @@ static struct ubi_vtbl_record *process_lvol(struct ubi_device *ubi,
 			leb_corrupted[1] = memcmp(leb[0], leb[1],
 						  ubi->vtbl_size);
 		if (leb_corrupted[1]) {
-			ubi_warn("volume table copy #2 is corrupted");
+			ubi_warn(ubi, "volume table copy #2 is corrupted");
 			err = create_vtbl(ubi, ai, 1, leb[0]);
 			if (err)
 				goto out_free;
-			ubi_msg("volume table was restored");
+			ubi_msg(ubi, "volume table was restored");
 		}
 
 		/* Both LEB 1 and LEB 2 are OK and consistent */
@@ -465,15 +460,15 @@ static struct ubi_vtbl_record *process_lvol(struct ubi_device *ubi,
 		}
 		if (leb_corrupted[1]) {
 			/* Both LEB 0 and LEB 1 are corrupted */
-			ubi_err("both volume tables are corrupted");
+			ubi_err(ubi, "both volume tables are corrupted");
 			goto out_free;
 		}
 
-		ubi_warn("volume table copy #1 is corrupted");
+		ubi_warn(ubi, "volume table copy #1 is corrupted");
 		err = create_vtbl(ubi, ai, 0, leb[1]);
 		if (err)
 			goto out_free;
-		ubi_msg("volume table was restored");
+		ubi_msg(ubi, "volume table was restored");
 
 		vfree(leb[0]);
 		return leb[1];
@@ -562,7 +557,7 @@ static int init_volumes(struct ubi_device *ubi,
 		if (vtbl[i].flags & UBI_VTBL_AUTORESIZE_FLG) {
 			/* Auto re-size flag may be set only for one volume */
 			if (ubi->autoresize_vol_id != -1) {
-				ubi_err("more than one auto-resize volume (%d and %d)",
+				ubi_err(ubi, "more than one auto-resize volume (%d and %d)",
 					ubi->autoresize_vol_id, i);
 				kfree(vol);
 				return -EINVAL;
@@ -608,7 +603,7 @@ static int init_volumes(struct ubi_device *ubi,
 			 * We found a static volume which misses several
 			 * eraseblocks. Treat it as corrupted.
 			 */
-			ubi_warn("static volume %d misses %d LEBs - corrupted",
+			ubi_warn(ubi, "static volume %d misses %d LEBs - corrupted",
 				 av->vol_id, av->used_ebs - av->leb_count);
 			vol->corrupted = 1;
 			continue;
@@ -646,10 +641,10 @@ static int init_volumes(struct ubi_device *ubi,
 	vol->ubi = ubi;
 
 	if (reserved_pebs > ubi->avail_pebs) {
-		ubi_err("not enough PEBs, required %d, available %d",
+		ubi_err(ubi, "not enough PEBs, required %d, available %d",
 			reserved_pebs, ubi->avail_pebs);
 		if (ubi->corr_peb_count)
-			ubi_err("%d PEBs are corrupted and not used",
+			ubi_err(ubi, "%d PEBs are corrupted and not used",
 				ubi->corr_peb_count);
 	}
 	ubi->rsvd_pebs += reserved_pebs;
@@ -660,13 +655,14 @@ static int init_volumes(struct ubi_device *ubi,
 
 /**
  * check_av - check volume attaching information.
+ * @ubi: UBI device description object
  * @vol: UBI volume description object
  * @av: volume attaching information
  *
  * 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_av(const struct ubi_volume *vol,
+static int check_av(const struct ubi_device *ubi, const struct ubi_volume *vol,
 		    const struct ubi_ainf_volume *av)
 {
 	int err;
@@ -694,7 +690,7 @@ static int check_av(const struct ubi_volume *vol,
 	return 0;
 
 bad:
-	ubi_err("bad attaching information, error %d", err);
+	ubi_err(ubi, "bad attaching information, error %d", err);
 	ubi_dump_av(av);
 	ubi_dump_vol_info(vol);
 	return -EINVAL;
@@ -718,14 +714,15 @@ static int check_attaching_info(const struct ubi_device *ubi,
 	struct ubi_volume *vol;
 
 	if (ai->vols_found > UBI_INT_VOL_COUNT + ubi->vtbl_slots) {
-		ubi_err("found %d volumes while attaching, maximum is %d + %d",
+		ubi_err(ubi, "found %d volumes while attaching, maximum is %d + %d",
 			ai->vols_found, UBI_INT_VOL_COUNT, ubi->vtbl_slots);
 		return -EINVAL;
 	}
 
 	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);
+		ubi_err(ubi, "too large volume ID %d found",
+			ai->highest_vol_id);
 		return -EINVAL;
 	}
 
@@ -753,10 +750,10 @@ static int check_attaching_info(const struct ubi_device *ubi,
 			 * reboot while the volume was being removed. Discard
 			 * these eraseblocks.
 			 */
-			ubi_msg("finish volume %d removal", av->vol_id);
+			ubi_msg(ubi, "finish volume %d removal", av->vol_id);
 			ubi_remove_av(ai, av);
 		} else if (av) {
-			err = check_av(vol, av);
+			err = check_av(ubi, vol, av);
 			if (err)
 				return err;
 		}
@@ -807,13 +804,13 @@ int ubi_read_volume_table(struct ubi_device *ubi, struct ubi_attach_info *ai)
 			if (IS_ERR(ubi->vtbl))
 				return PTR_ERR(ubi->vtbl);
 		} else {
-			ubi_err("the layout volume was not found");
+			ubi_err(ubi, "the layout volume was not found");
 			return -EINVAL;
 		}
 	} else {
 		if (av->leb_count > UBI_LAYOUT_VOLUME_EBS) {
 			/* This must not happen with proper UBI images */
-			ubi_err("too many LEBs (%d) in layout volume",
+			ubi_err(ubi, "too many LEBs (%d) in layout volume",
 				av->leb_count);
 			return -EINVAL;
 		}
@@ -862,7 +859,7 @@ static void self_vtbl_check(const struct ubi_device *ubi)
 		return;
 
 	if (vtbl_check(ubi, ubi->vtbl)) {
-		ubi_err("self-check failed");
+		ubi_err(ubi, "self-check failed");
 		BUG();
 	}
 }
diff --git a/drivers/mtd/ubi/wl.c b/drivers/mtd/ubi/wl.c
index 6654f191868e..834f6fe1f5fa 100644
--- a/drivers/mtd/ubi/wl.c
+++ b/drivers/mtd/ubi/wl.c
@@ -253,7 +253,7 @@ static int do_work(struct ubi_device *ubi)
 	 */
 	err = wrk->func(ubi, wrk, 0);
 	if (err)
-		ubi_err("work failed with error code %d", err);
+		ubi_err(ubi, "work failed with error code %d", err);
 	up_read(&ubi->work_sem);
 
 	return err;
@@ -470,8 +470,11 @@ struct ubi_wl_entry *ubi_wl_get_fm_peb(struct ubi_device *ubi, int anchor)
 {
 	struct ubi_wl_entry *e = NULL;
 
-	if (!ubi->free.rb_node || (ubi->free_count - ubi->beb_rsvd_pebs < 1))
+	if (!ubi->free.rb_node || (ubi->free_count - ubi->beb_rsvd_pebs < 1)) {
+		ubi_warn(ubi, "Can't get peb for fastmap:anchor=%d, free_cnt=%d, reserved=%d",
+			 anchor, ubi->free_count, ubi->beb_rsvd_pebs);
 		goto out;
+	}
 
 	if (anchor)
 		e = find_anchor_wl_entry(&ubi->free);
@@ -507,7 +510,7 @@ static int __wl_get_peb(struct ubi_device *ubi)
 retry:
 	if (!ubi->free.rb_node) {
 		if (ubi->works_count == 0) {
-			ubi_err("no free eraseblocks");
+			ubi_err(ubi, "no free eraseblocks");
 			ubi_assert(list_empty(&ubi->works));
 			return -ENOSPC;
 		}
@@ -520,7 +523,7 @@ retry:
 
 	e = find_mean_wl_entry(ubi, &ubi->free);
 	if (!e) {
-		ubi_err("no free eraseblocks");
+		ubi_err(ubi, "no free eraseblocks");
 		return -ENOSPC;
 	}
 
@@ -692,7 +695,8 @@ int ubi_wl_get_peb(struct ubi_device *ubi)
 	err = ubi_self_check_all_ff(ubi, peb, ubi->vid_hdr_aloffset,
 				    ubi->peb_size - ubi->vid_hdr_aloffset);
 	if (err) {
-		ubi_err("new PEB %d does not contain all 0xFF bytes", peb);
+		ubi_err(ubi, "new PEB %d does not contain all 0xFF bytes",
+			peb);
 		return err;
 	}
 
@@ -760,7 +764,7 @@ static int sync_erase(struct ubi_device *ubi, struct ubi_wl_entry *e,
 		 * Erase counter overflow. Upgrade UBI and use 64-bit
 		 * erase counters internally.
 		 */
-		ubi_err("erase counter overflow at PEB %d, EC %llu",
+		ubi_err(ubi, "erase counter overflow at PEB %d, EC %llu",
 			e->pnum, ec);
 		err = -EINVAL;
 		goto out_free;
@@ -1137,7 +1141,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 			goto out_not_moved;
 		}
 
-		ubi_err("error %d while reading VID header from PEB %d",
+		ubi_err(ubi, "error %d while reading VID header from PEB %d",
 			err, e1->pnum);
 		goto out_error;
 	}
@@ -1181,7 +1185,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 			 * UBI from trying to move it over and over again.
 			 */
 			if (ubi->erroneous_peb_count > ubi->max_erroneous) {
-				ubi_err("too many erroneous eraseblocks (%d)",
+				ubi_err(ubi, "too many erroneous eraseblocks (%d)",
 					ubi->erroneous_peb_count);
 				goto out_error;
 			}
@@ -1197,7 +1201,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 
 	/* The PEB has been successfully moved */
 	if (scrubbing)
-		ubi_msg("scrubbed PEB %d (LEB %d:%d), data moved to PEB %d",
+		ubi_msg(ubi, "scrubbed PEB %d (LEB %d:%d), data moved to PEB %d",
 			e1->pnum, vol_id, lnum, e2->pnum);
 	ubi_free_vid_hdr(ubi, vid_hdr);
 
@@ -1212,7 +1216,6 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 
 	err = do_sync_erase(ubi, e1, vol_id, lnum, 0);
 	if (err) {
-		kmem_cache_free(ubi_wl_entry_slab, e1);
 		if (e2)
 			kmem_cache_free(ubi_wl_entry_slab, e2);
 		goto out_ro;
@@ -1226,10 +1229,8 @@ 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 = do_sync_erase(ubi, e2, vol_id, lnum, 0);
-		if (err) {
-			kmem_cache_free(ubi_wl_entry_slab, e2);
+		if (err)
 			goto out_ro;
-		}
 	}
 
 	dbg_wl("done");
@@ -1265,19 +1266,18 @@ out_not_moved:
 
 	ubi_free_vid_hdr(ubi, vid_hdr);
 	err = do_sync_erase(ubi, e2, vol_id, lnum, torture);
-	if (err) {
-		kmem_cache_free(ubi_wl_entry_slab, e2);
+	if (err)
 		goto out_ro;
-	}
+
 	mutex_unlock(&ubi->move_mutex);
 	return 0;
 
 out_error:
 	if (vol_id != -1)
-		ubi_err("error %d while moving PEB %d to PEB %d",
+		ubi_err(ubi, "error %d while moving PEB %d to PEB %d",
 			err, e1->pnum, e2->pnum);
 	else
-		ubi_err("error %d while moving PEB %d (LEB %d:%d) to PEB %d",
+		ubi_err(ubi, "error %d while moving PEB %d (LEB %d:%d) to PEB %d",
 			err, e1->pnum, vol_id, lnum, e2->pnum);
 	spin_lock(&ubi->wl_lock);
 	ubi->move_from = ubi->move_to = NULL;
@@ -1458,7 +1458,7 @@ static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
 		return err;
 	}
 
-	ubi_err("failed to erase PEB %d, error %d", pnum, err);
+	ubi_err(ubi, "failed to erase PEB %d, error %d", pnum, err);
 	kfree(wl_wrk);
 
 	if (err == -EINTR || err == -ENOMEM || err == -EAGAIN ||
@@ -1486,7 +1486,7 @@ static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
 	/* It is %-EIO, the PEB went bad */
 
 	if (!ubi->bad_allowed) {
-		ubi_err("bad physical eraseblock %d detected", pnum);
+		ubi_err(ubi, "bad physical eraseblock %d detected", pnum);
 		goto out_ro;
 	}
 
@@ -1494,7 +1494,7 @@ static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
 	if (ubi->beb_rsvd_pebs == 0) {
 		if (ubi->avail_pebs == 0) {
 			spin_unlock(&ubi->volumes_lock);
-			ubi_err("no reserved/available physical eraseblocks");
+			ubi_err(ubi, "no reserved/available physical eraseblocks");
 			goto out_ro;
 		}
 		ubi->avail_pebs -= 1;
@@ -1502,7 +1502,7 @@ static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
 	}
 	spin_unlock(&ubi->volumes_lock);
 
-	ubi_msg("mark PEB %d as bad", pnum);
+	ubi_msg(ubi, "mark PEB %d as bad", pnum);
 	err = ubi_io_mark_bad(ubi, pnum);
 	if (err)
 		goto out_ro;
@@ -1523,11 +1523,12 @@ static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
 	ubi->good_peb_count -= 1;
 	ubi_calculate_reserved(ubi);
 	if (available_consumed)
-		ubi_warn("no PEBs in the reserved pool, used an available PEB");
+		ubi_warn(ubi, "no PEBs in the reserved pool, used an available PEB");
 	else if (ubi->beb_rsvd_pebs)
-		ubi_msg("%d PEBs left in the reserve", ubi->beb_rsvd_pebs);
+		ubi_msg(ubi, "%d PEBs left in the reserve",
+			ubi->beb_rsvd_pebs);
 	else
-		ubi_warn("last PEB from the reserve was used");
+		ubi_warn(ubi, "last PEB from the reserve was used");
 	spin_unlock(&ubi->volumes_lock);
 
 	return err;
@@ -1613,7 +1614,7 @@ retry:
 		} else {
 			err = prot_queue_del(ubi, e->pnum);
 			if (err) {
-				ubi_err("PEB %d not found", pnum);
+				ubi_err(ubi, "PEB %d not found", pnum);
 				ubi_ro_mode(ubi);
 				spin_unlock(&ubi->wl_lock);
 				return err;
@@ -1646,7 +1647,7 @@ int ubi_wl_scrub_peb(struct ubi_device *ubi, int pnum)
 {
 	struct ubi_wl_entry *e;
 
-	ubi_msg("schedule PEB %d for scrubbing", pnum);
+	ubi_msg(ubi, "schedule PEB %d for scrubbing", pnum);
 
 retry:
 	spin_lock(&ubi->wl_lock);
@@ -1678,7 +1679,7 @@ retry:
 
 		err = prot_queue_del(ubi, e->pnum);
 		if (err) {
-			ubi_err("PEB %d not found", pnum);
+			ubi_err(ubi, "PEB %d not found", pnum);
 			ubi_ro_mode(ubi);
 			spin_unlock(&ubi->wl_lock);
 			return err;
@@ -1798,15 +1799,18 @@ int ubi_thread(void *u)
 	int failures = 0;
 	struct ubi_device *ubi = u;
 
-	ubi_msg("background thread \"%s\" started, PID %d",
+	ubi_msg(ubi, "background thread \"%s\" started, PID %d",
 		ubi->bgt_name, task_pid_nr(current));
 
 	set_freezable();
 	for (;;) {
 		int err;
 
-		if (kthread_should_stop())
+		if (kthread_should_stop()) {
+			ubi_msg(ubi, "background thread \"%s\" should stop, PID %d",
+				ubi->bgt_name, task_pid_nr(current));
 			break;
+		}
 
 		if (try_to_freeze())
 			continue;
@@ -1823,14 +1827,14 @@ int ubi_thread(void *u)
 
 		err = do_work(ubi);
 		if (err) {
-			ubi_err("%s: work failed with error code %d",
+			ubi_err(ubi, "%s: work failed with error code %d",
 				ubi->bgt_name, err);
 			if (failures++ > WL_MAX_FAILURES) {
 				/*
 				 * Too many failures, disable the thread and
 				 * switch to read-only mode.
 				 */
-				ubi_msg("%s: %d consecutive failures",
+				ubi_msg(ubi, "%s: %d consecutive failures",
 					ubi->bgt_name, WL_MAX_FAILURES);
 				ubi_ro_mode(ubi);
 				ubi->thread_enabled = 0;
@@ -1981,10 +1985,10 @@ int ubi_wl_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 #endif
 
 	if (ubi->avail_pebs < reserved_pebs) {
-		ubi_err("no enough physical eraseblocks (%d, need %d)",
+		ubi_err(ubi, "no enough physical eraseblocks (%d, need %d)",
 			ubi->avail_pebs, reserved_pebs);
 		if (ubi->corr_peb_count)
-			ubi_err("%d PEBs are corrupted and not used",
+			ubi_err(ubi, "%d PEBs are corrupted and not used",
 				ubi->corr_peb_count);
 		goto out_free;
 	}
@@ -2072,8 +2076,8 @@ static int self_check_ec(struct ubi_device *ubi, int pnum, int ec)
 
 	read_ec = be64_to_cpu(ec_hdr->ec);
 	if (ec != read_ec && read_ec - ec > 1) {
-		ubi_err("self-check failed for PEB %d", pnum);
-		ubi_err("read EC is %lld, should be %d", read_ec, ec);
+		ubi_err(ubi, "self-check failed for PEB %d", pnum);
+		ubi_err(ubi, "read EC is %lld, should be %d", read_ec, ec);
 		dump_stack();
 		err = 1;
 	} else
@@ -2102,7 +2106,7 @@ static int self_check_in_wl_tree(const struct ubi_device *ubi,
 	if (in_wl_tree(e, root))
 		return 0;
 
-	ubi_err("self-check failed for PEB %d, EC %d, RB-tree %p ",
+	ubi_err(ubi, "self-check failed for PEB %d, EC %d, RB-tree %p ",
 		e->pnum, e->ec, root);
 	dump_stack();
 	return -EINVAL;
@@ -2130,7 +2134,7 @@ static int self_check_in_pq(const struct ubi_device *ubi,
 			if (p == e)
 				return 0;
 
-	ubi_err("self-check failed for PEB %d, EC %d, Protect queue",
+	ubi_err(ubi, "self-check failed for PEB %d, EC %d, Protect queue",
 		e->pnum, e->ec);
 	dump_stack();
 	return -EINVAL;
diff --git a/drivers/net/bonding/bond_main.c b/drivers/net/bonding/bond_main.c
index 184c434ae305..0dceba1a2ba1 100644
--- a/drivers/net/bonding/bond_main.c
+++ b/drivers/net/bonding/bond_main.c
@@ -1648,7 +1648,7 @@ static int __bond_release_one(struct net_device *bond_dev,
 	/* slave is not a slave or master is not master of this slave */
 	if (!(slave_dev->flags & IFF_SLAVE) ||
 	    !netdev_has_upper_dev(slave_dev, bond_dev)) {
-		netdev_err(bond_dev, "cannot release %s\n",
+		netdev_dbg(bond_dev, "cannot release %s\n",
 			   slave_dev->name);
 		return -EINVAL;
 	}
diff --git a/drivers/net/caif/caif_virtio.c b/drivers/net/caif/caif_virtio.c
index a5fefb9059c5..b306210b02b7 100644
--- a/drivers/net/caif/caif_virtio.c
+++ b/drivers/net/caif/caif_virtio.c
@@ -257,7 +257,6 @@ static int cfv_rx_poll(struct napi_struct *napi, int quota)
 	struct vringh_kiov *riov = &cfv->ctx.riov;
 	unsigned int skb_len;
 
-again:
 	do {
 		skb = NULL;
 
@@ -322,7 +321,6 @@ exit:
 		    napi_schedule_prep(napi)) {
 			vringh_notify_disable_kern(cfv->vr_rx);
 			__napi_schedule(napi);
-			goto again;
 		}
 		break;
 
diff --git a/drivers/net/can/at91_can.c b/drivers/net/can/at91_can.c
index 05e1aa090add..d0c2463b573f 100644
--- a/drivers/net/can/at91_can.c
+++ b/drivers/net/can/at91_can.c
@@ -1428,7 +1428,6 @@ static struct platform_driver at91_can_driver = {
 	.remove = at91_can_remove,
 	.driver = {
 		.name = KBUILD_MODNAME,
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(at91_can_dt_ids),
 	},
 	.id_table = at91_can_id_table,
diff --git a/drivers/net/can/bfin_can.c b/drivers/net/can/bfin_can.c
index 543ecceb33e9..417d50998e31 100644
--- a/drivers/net/can/bfin_can.c
+++ b/drivers/net/can/bfin_can.c
@@ -680,7 +680,6 @@ static struct platform_driver bfin_can_driver = {
 	.resume = bfin_can_resume,
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/can/c_can/c_can_platform.c b/drivers/net/can/c_can/c_can_platform.c
index a4535d2142a7..f363972cd77d 100644
--- a/drivers/net/can/c_can/c_can_platform.c
+++ b/drivers/net/can/c_can/c_can_platform.c
@@ -471,7 +471,6 @@ static int c_can_resume(struct platform_device *pdev)
 static struct platform_driver c_can_plat_driver = {
 	.driver = {
 		.name = KBUILD_MODNAME,
-		.owner = THIS_MODULE,
 		.of_match_table = c_can_of_table,
 	},
 	.probe = c_can_plat_probe,
diff --git a/drivers/net/can/cc770/cc770_isa.c b/drivers/net/can/cc770/cc770_isa.c
index 87a47c0cfd49..e0d15711e9ac 100644
--- a/drivers/net/can/cc770/cc770_isa.c
+++ b/drivers/net/can/cc770/cc770_isa.c
@@ -318,7 +318,6 @@ static struct platform_driver cc770_isa_driver = {
 	.remove = cc770_isa_remove,
 	.driver = {
 		.name = KBUILD_MODNAME,
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/can/cc770/cc770_platform.c b/drivers/net/can/cc770/cc770_platform.c
index ad76734b3ecc..b1e8851d3cc4 100644
--- a/drivers/net/can/cc770/cc770_platform.c
+++ b/drivers/net/can/cc770/cc770_platform.c
@@ -264,7 +264,6 @@ MODULE_DEVICE_TABLE(of, cc770_platform_table);
 static struct platform_driver cc770_platform_driver = {
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = cc770_platform_table,
 	},
 	.probe = cc770_platform_probe,
diff --git a/drivers/net/can/flexcan.c b/drivers/net/can/flexcan.c
index dde05486bc99..b1d583ba9674 100644
--- a/drivers/net/can/flexcan.c
+++ b/drivers/net/can/flexcan.c
@@ -1302,7 +1302,6 @@ static SIMPLE_DEV_PM_OPS(flexcan_pm_ops, flexcan_suspend, flexcan_resume);
 static struct platform_driver flexcan_driver = {
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 		.pm = &flexcan_pm_ops,
 		.of_match_table = flexcan_of_match,
 	},
diff --git a/drivers/net/can/grcan.c b/drivers/net/can/grcan.c
index 3fd9fd942c6e..fed1bbd0b0d2 100644
--- a/drivers/net/can/grcan.c
+++ b/drivers/net/can/grcan.c
@@ -1738,7 +1738,6 @@ MODULE_DEVICE_TABLE(of, grcan_match);
 static struct platform_driver grcan_driver = {
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = grcan_match,
 	},
 	.probe = grcan_probe,
diff --git a/drivers/net/can/janz-ican3.c b/drivers/net/can/janz-ican3.c
index 2382c04dc780..1b118394907f 100644
--- a/drivers/net/can/janz-ican3.c
+++ b/drivers/net/can/janz-ican3.c
@@ -1910,7 +1910,6 @@ static int ican3_remove(struct platform_device *pdev)
 static struct platform_driver ican3_driver = {
 	.driver		= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= ican3_probe,
 	.remove		= ican3_remove,
diff --git a/drivers/net/can/mscan/mpc5xxx_can.c b/drivers/net/can/mscan/mpc5xxx_can.c
index 44725296f72a..ad024e60ba8c 100644
--- a/drivers/net/can/mscan/mpc5xxx_can.c
+++ b/drivers/net/can/mscan/mpc5xxx_can.c
@@ -441,7 +441,6 @@ MODULE_DEVICE_TABLE(of, mpc5xxx_can_table);
 static struct platform_driver mpc5xxx_can_driver = {
 	.driver = {
 		.name = "mpc5xxx_can",
-		.owner = THIS_MODULE,
 		.of_match_table = mpc5xxx_can_table,
 	},
 	.probe = mpc5xxx_can_probe,
diff --git a/drivers/net/can/rcar_can.c b/drivers/net/can/rcar_can.c
index 9718248e55f1..91cd48ca0efc 100644
--- a/drivers/net/can/rcar_can.c
+++ b/drivers/net/can/rcar_can.c
@@ -907,7 +907,6 @@ MODULE_DEVICE_TABLE(of, rcar_can_of_table);
 static struct platform_driver rcar_can_driver = {
 	.driver = {
 		.name = RCAR_CAN_DRV_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(rcar_can_of_table),
 		.pm = &rcar_can_pm_ops,
 	},
diff --git a/drivers/net/can/sja1000/sja1000_isa.c b/drivers/net/can/sja1000/sja1000_isa.c
index 014695d7e6a3..e97e6d35b300 100644
--- a/drivers/net/can/sja1000/sja1000_isa.c
+++ b/drivers/net/can/sja1000/sja1000_isa.c
@@ -259,7 +259,6 @@ static struct platform_driver sja1000_isa_driver = {
 	.remove = sja1000_isa_remove,
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/can/sja1000/sja1000_platform.c b/drivers/net/can/sja1000/sja1000_platform.c
index 95a844a7ee7b..93115250eaf5 100644
--- a/drivers/net/can/sja1000/sja1000_platform.c
+++ b/drivers/net/can/sja1000/sja1000_platform.c
@@ -253,7 +253,6 @@ static struct platform_driver sp_driver = {
 	.remove = sp_remove,
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = sp_of_table,
 	},
 };
diff --git a/drivers/net/can/softing/softing_main.c b/drivers/net/can/softing/softing_main.c
index bacd236ce306..2bf98d862302 100644
--- a/drivers/net/can/softing/softing_main.c
+++ b/drivers/net/can/softing/softing_main.c
@@ -856,7 +856,6 @@ platform_resource_failed:
 static struct platform_driver softing_driver = {
 	.driver = {
 		.name = "softing",
-		.owner = THIS_MODULE,
 	},
 	.probe = softing_pdev_probe,
 	.remove = softing_pdev_remove,
diff --git a/drivers/net/can/ti_hecc.c b/drivers/net/can/ti_hecc.c
index 258b9c4856ec..9a07eafe554b 100644
--- a/drivers/net/can/ti_hecc.c
+++ b/drivers/net/can/ti_hecc.c
@@ -1039,7 +1039,6 @@ static int ti_hecc_resume(struct platform_device *pdev)
 static struct platform_driver ti_hecc_driver = {
 	.driver = {
 		.name    = DRV_NAME,
-		.owner   = THIS_MODULE,
 	},
 	.probe = ti_hecc_probe,
 	.remove = ti_hecc_remove,
diff --git a/drivers/net/can/xilinx_can.c b/drivers/net/can/xilinx_can.c
index 8a998e3884ce..6c6764312285 100644
--- a/drivers/net/can/xilinx_can.c
+++ b/drivers/net/can/xilinx_can.c
@@ -1196,7 +1196,6 @@ static struct platform_driver xcan_driver = {
 	.probe = xcan_probe,
 	.remove	= xcan_remove,
 	.driver	= {
-		.owner = THIS_MODULE,
 		.name = DRIVER_NAME,
 		.pm = &xcan_dev_pm_ops,
 		.of_match_table	= xcan_of_match,
diff --git a/drivers/net/dsa/Kconfig b/drivers/net/dsa/Kconfig
index 7cf8f4ac281f..48e62a34f7f2 100644
--- a/drivers/net/dsa/Kconfig
+++ b/drivers/net/dsa/Kconfig
@@ -59,7 +59,7 @@ config NET_DSA_BCM_SF2
 	depends on HAS_IOMEM
 	select NET_DSA
 	select NET_DSA_TAG_BRCM
-	select FIXED_PHY if NET_DSA_BCM_SF2=y
+	select FIXED_PHY
 	select BCM7XXX_PHY
 	select MDIO_BCM_UNIMAC
 	---help---
diff --git a/drivers/net/ethernet/8390/ax88796.c b/drivers/net/ethernet/8390/ax88796.c
index 1d162ccb4733..0443654f0339 100644
--- a/drivers/net/ethernet/8390/ax88796.c
+++ b/drivers/net/ethernet/8390/ax88796.c
@@ -1000,7 +1000,6 @@ static int ax_resume(struct platform_device *pdev)
 static struct platform_driver axdrv = {
 	.driver	= {
 		.name		= "ax88796",
-		.owner		= THIS_MODULE,
 	},
 	.probe		= ax_probe,
 	.remove		= ax_remove,
diff --git a/drivers/net/ethernet/8390/mcf8390.c b/drivers/net/ethernet/8390/mcf8390.c
index 38fcdcf7c4c7..e1c055574a11 100644
--- a/drivers/net/ethernet/8390/mcf8390.c
+++ b/drivers/net/ethernet/8390/mcf8390.c
@@ -467,7 +467,6 @@ static int mcf8390_remove(struct platform_device *pdev)
 static struct platform_driver mcf8390_drv = {
 	.driver = {
 		.name	= "mcf8390",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= mcf8390_probe,
 	.remove		= mcf8390_remove,
diff --git a/drivers/net/ethernet/8390/ne.c b/drivers/net/ethernet/8390/ne.c
index de566fb6e0f7..c063b410a163 100644
--- a/drivers/net/ethernet/8390/ne.c
+++ b/drivers/net/ethernet/8390/ne.c
@@ -920,7 +920,6 @@ static struct platform_driver ne_driver = {
 	.resume		= ne_drv_resume,
 	.driver		= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/ethernet/8390/ne2k-pci.c b/drivers/net/ethernet/8390/ne2k-pci.c
index 89c8d9fc97de..57e97910c728 100644
--- a/drivers/net/ethernet/8390/ne2k-pci.c
+++ b/drivers/net/ethernet/8390/ne2k-pci.c
@@ -246,13 +246,13 @@ static int ne2k_pci_init_one(struct pci_dev *pdev,
 
 	if (!ioaddr || ((pci_resource_flags (pdev, 0) & IORESOURCE_IO) == 0)) {
 		dev_err(&pdev->dev, "no I/O resource at PCI BAR #0\n");
-		return -ENODEV;
+		goto err_out;
 	}
 
 	if (request_region (ioaddr, NE_IO_EXTENT, DRV_NAME) == NULL) {
 		dev_err(&pdev->dev, "I/O resource 0x%x @ 0x%lx busy\n",
 			NE_IO_EXTENT, ioaddr);
-		return -EBUSY;
+		goto err_out;
 	}
 
 	reg0 = inb(ioaddr);
@@ -392,6 +392,8 @@ err_out_free_netdev:
 	free_netdev (dev);
 err_out_free_res:
 	release_region (ioaddr, NE_IO_EXTENT);
+err_out:
+	pci_disable_device(pdev);
 	return -ENODEV;
 }
 
diff --git a/drivers/net/ethernet/Kconfig b/drivers/net/ethernet/Kconfig
index df76050d0a9d..eadcb053807e 100644
--- a/drivers/net/ethernet/Kconfig
+++ b/drivers/net/ethernet/Kconfig
@@ -156,18 +156,6 @@ source "drivers/net/ethernet/realtek/Kconfig"
 source "drivers/net/ethernet/renesas/Kconfig"
 source "drivers/net/ethernet/rdc/Kconfig"
 source "drivers/net/ethernet/rocker/Kconfig"
-
-config S6GMAC
-	tristate "S6105 GMAC ethernet support"
-	depends on XTENSA_VARIANT_S6000
-	select PHYLIB
-	---help---
-	  This driver supports the on chip ethernet device on the
-	  S6105 xtensa processor.
-
-	  To compile this driver as a module, choose M here. The module
-	  will be called s6gmac.
-
 source "drivers/net/ethernet/samsung/Kconfig"
 source "drivers/net/ethernet/seeq/Kconfig"
 source "drivers/net/ethernet/silan/Kconfig"
diff --git a/drivers/net/ethernet/Makefile b/drivers/net/ethernet/Makefile
index bf56f8b36e90..1367afcd0a8b 100644
--- a/drivers/net/ethernet/Makefile
+++ b/drivers/net/ethernet/Makefile
@@ -66,7 +66,6 @@ obj-$(CONFIG_NET_VENDOR_REALTEK) += realtek/
 obj-$(CONFIG_SH_ETH) += renesas/
 obj-$(CONFIG_NET_VENDOR_RDC) += rdc/
 obj-$(CONFIG_NET_VENDOR_ROCKER) += rocker/
-obj-$(CONFIG_S6GMAC) += s6gmac.o
 obj-$(CONFIG_NET_VENDOR_SAMSUNG) += samsung/
 obj-$(CONFIG_NET_VENDOR_SEEQ) += seeq/
 obj-$(CONFIG_NET_VENDOR_SILAN) += silan/
diff --git a/drivers/net/ethernet/adi/bfin_mac.c b/drivers/net/ethernet/adi/bfin_mac.c
index 8ed4d3408ef6..ec20611e9de2 100644
--- a/drivers/net/ethernet/adi/bfin_mac.c
+++ b/drivers/net/ethernet/adi/bfin_mac.c
@@ -1901,7 +1901,6 @@ static struct platform_driver bfin_mii_bus_driver = {
 	.remove = bfin_mii_bus_remove,
 	.driver = {
 		.name = "bfin_mii_bus",
-		.owner	= THIS_MODULE,
 	},
 };
 
@@ -1912,7 +1911,6 @@ static struct platform_driver bfin_mac_driver = {
 	.suspend = bfin_mac_suspend,
 	.driver = {
 		.name = KBUILD_MODNAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/ethernet/aeroflex/greth.c b/drivers/net/ethernet/aeroflex/greth.c
index 3005155e412b..2b8bfeeee9cf 100644
--- a/drivers/net/ethernet/aeroflex/greth.c
+++ b/drivers/net/ethernet/aeroflex/greth.c
@@ -1603,7 +1603,6 @@ MODULE_DEVICE_TABLE(of, greth_of_match);
 static struct platform_driver greth_of_driver = {
 	.driver = {
 		.name = "grlib-greth",
-		.owner = THIS_MODULE,
 		.of_match_table = greth_of_match,
 	},
 	.probe = greth_of_probe,
diff --git a/drivers/net/ethernet/altera/altera_tse_main.c b/drivers/net/ethernet/altera/altera_tse_main.c
index 4efc4355d345..3498760dc22a 100644
--- a/drivers/net/ethernet/altera/altera_tse_main.c
+++ b/drivers/net/ethernet/altera/altera_tse_main.c
@@ -1587,7 +1587,6 @@ static struct platform_driver altera_tse_driver = {
 	.resume		= NULL,
 	.driver		= {
 		.name	= ALTERA_TSE_RESOURCE_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = altera_tse_ids,
 	},
 };
diff --git a/drivers/net/ethernet/amd/au1000_eth.c b/drivers/net/ethernet/amd/au1000_eth.c
index 6c323f4f457b..cb367cc59e0b 100644
--- a/drivers/net/ethernet/amd/au1000_eth.c
+++ b/drivers/net/ethernet/amd/au1000_eth.c
@@ -1461,7 +1461,6 @@ static struct platform_driver au1000_eth_driver = {
 	.remove = au1000_remove,
 	.driver = {
 		.name   = "au1000-eth",
-		.owner  = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/ethernet/amd/sunlance.c b/drivers/net/ethernet/amd/sunlance.c
index 5e4273b7aa27..7847638bdd22 100644
--- a/drivers/net/ethernet/amd/sunlance.c
+++ b/drivers/net/ethernet/amd/sunlance.c
@@ -1524,7 +1524,6 @@ MODULE_DEVICE_TABLE(of, sunlance_sbus_match);
 static struct platform_driver sunlance_sbus_driver = {
 	.driver = {
 		.name = "sunlance",
-		.owner = THIS_MODULE,
 		.of_match_table = sunlance_sbus_match,
 	},
 	.probe		= sunlance_sbus_probe,
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-ptp.c b/drivers/net/ethernet/amd/xgbe/xgbe-ptp.c
index a1bf9d1cdae1..f5acf4cc69bd 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-ptp.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-ptp.c
@@ -171,15 +171,9 @@ static int xgbe_adjtime(struct ptp_clock_info *info, s64 delta)
 						   struct xgbe_prv_data,
 						   ptp_clock_info);
 	unsigned long flags;
-	u64 nsec;
 
 	spin_lock_irqsave(&pdata->tstamp_lock, flags);
-
-	nsec = timecounter_read(&pdata->tstamp_tc);
-
-	nsec += delta;
-	timecounter_init(&pdata->tstamp_tc, &pdata->tstamp_cc, nsec);
-
+	timecounter_adjtime(&pdata->tstamp_tc, delta);
 	spin_unlock_irqrestore(&pdata->tstamp_lock, flags);
 
 	return 0;
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe.h b/drivers/net/ethernet/amd/xgbe/xgbe.h
index f9ec762ac3f0..2af6affc35a7 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe.h
+++ b/drivers/net/ethernet/amd/xgbe/xgbe.h
@@ -124,7 +124,7 @@
 #include <linux/if_vlan.h>
 #include <linux/bitops.h>
 #include <linux/ptp_clock_kernel.h>
-#include <linux/clocksource.h>
+#include <linux/timecounter.h>
 #include <linux/net_tstamp.h>
 #include <net/dcbnl.h>
 
diff --git a/drivers/net/ethernet/apple/macmace.c b/drivers/net/ethernet/apple/macmace.c
index 58a200df4c35..6e66127e6abf 100644
--- a/drivers/net/ethernet/apple/macmace.c
+++ b/drivers/net/ethernet/apple/macmace.c
@@ -768,7 +768,6 @@ static struct platform_driver mac_mace_driver = {
 	.remove = mac_mace_device_remove,
 	.driver	= {
 		.name	= mac_mace_string,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/ethernet/atheros/atl1c/atl1c_main.c b/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
index 72fb86b9aa24..c9946c6c119e 100644
--- a/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
+++ b/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
@@ -48,7 +48,7 @@ MODULE_DEVICE_TABLE(pci, atl1c_pci_tbl);
 
 MODULE_AUTHOR("Jie Yang");
 MODULE_AUTHOR("Qualcomm Atheros Inc., <nic-devel@qualcomm.com>");
-MODULE_DESCRIPTION("Qualcom Atheros 100/1000M Ethernet Network Driver");
+MODULE_DESCRIPTION("Qualcomm Atheros 100/1000M Ethernet Network Driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(ATL1C_DRV_VERSION);
 
diff --git a/drivers/net/ethernet/atheros/atl1e/atl1e_main.c b/drivers/net/ethernet/atheros/atl1e/atl1e_main.c
index 2326579f9454..c88abf5b6415 100644
--- a/drivers/net/ethernet/atheros/atl1e/atl1e_main.c
+++ b/drivers/net/ethernet/atheros/atl1e/atl1e_main.c
@@ -2373,9 +2373,8 @@ static int atl1e_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	netif_napi_add(netdev, &adapter->napi, atl1e_clean, 64);
 
-	init_timer(&adapter->phy_config_timer);
-	adapter->phy_config_timer.function = atl1e_phy_config;
-	adapter->phy_config_timer.data = (unsigned long) adapter;
+	setup_timer(&adapter->phy_config_timer, atl1e_phy_config,
+		    (unsigned long)adapter);
 
 	/* get user settings */
 	atl1e_check_options(adapter);
diff --git a/drivers/net/ethernet/atheros/atlx/atl2.c b/drivers/net/ethernet/atheros/atlx/atl2.c
index 84a09e8ddd9c..482a7cabb0a1 100644
--- a/drivers/net/ethernet/atheros/atlx/atl2.c
+++ b/drivers/net/ethernet/atheros/atlx/atl2.c
@@ -1436,13 +1436,11 @@ static int atl2_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	atl2_check_options(adapter);
 
-	init_timer(&adapter->watchdog_timer);
-	adapter->watchdog_timer.function = atl2_watchdog;
-	adapter->watchdog_timer.data = (unsigned long) adapter;
+	setup_timer(&adapter->watchdog_timer, atl2_watchdog,
+		    (unsigned long)adapter);
 
-	init_timer(&adapter->phy_config_timer);
-	adapter->phy_config_timer.function = atl2_phy_config;
-	adapter->phy_config_timer.data = (unsigned long) adapter;
+	setup_timer(&adapter->phy_config_timer, atl2_phy_config,
+		    (unsigned long)adapter);
 
 	INIT_WORK(&adapter->reset_task, atl2_reset_task);
 	INIT_WORK(&adapter->link_chg_task, atl2_link_chg_task);
diff --git a/drivers/net/ethernet/broadcom/Kconfig b/drivers/net/ethernet/broadcom/Kconfig
index 888247ad9068..41a3c9804427 100644
--- a/drivers/net/ethernet/broadcom/Kconfig
+++ b/drivers/net/ethernet/broadcom/Kconfig
@@ -64,7 +64,7 @@ config BCMGENET
 	tristate "Broadcom GENET internal MAC support"
 	select MII
 	select PHYLIB
-	select FIXED_PHY if BCMGENET=y
+	select FIXED_PHY
 	select BCM7XXX_PHY
 	help
 	  This driver supports the built-in Ethernet MACs found in the
@@ -155,7 +155,7 @@ config SYSTEMPORT
 	depends on OF
 	select MII
 	select PHYLIB
-	select FIXED_PHY if SYSTEMPORT=y
+	select FIXED_PHY
 	help
 	  This driver supports the built-in Ethernet MACs found in the
 	  Broadcom BCM7xxx Set Top Box family chipset using an internal
diff --git a/drivers/net/ethernet/broadcom/bcmsysport.c b/drivers/net/ethernet/broadcom/bcmsysport.c
index a91a8c263391..5b308a4a4d0e 100644
--- a/drivers/net/ethernet/broadcom/bcmsysport.c
+++ b/drivers/net/ethernet/broadcom/bcmsysport.c
@@ -1994,7 +1994,6 @@ static struct platform_driver bcm_sysport_driver = {
 	.remove	= bcm_sysport_remove,
 	.driver =  {
 		.name = "brcm-systemport",
-		.owner = THIS_MODULE,
 		.of_match_table = bcm_sysport_of_match,
 		.pm = &bcm_sysport_pm_ops,
 	},
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
index c3a6072134f5..792ba72fb5c8 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
@@ -22,7 +22,7 @@
 
 #include <linux/ptp_clock_kernel.h>
 #include <linux/net_tstamp.h>
-#include <linux/clocksource.h>
+#include <linux/timecounter.h>
 
 /* compilation time flags */
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
index 691f0bf09ee1..0758c8bef4ba 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
@@ -12553,9 +12553,11 @@ static int bnx2x_get_phys_port_id(struct net_device *netdev,
 	return 0;
 }
 
-static bool bnx2x_gso_check(struct sk_buff *skb, struct net_device *dev)
+static netdev_features_t bnx2x_features_check(struct sk_buff *skb,
+					      struct net_device *dev,
+					      netdev_features_t features)
 {
-	return vxlan_gso_check(skb);
+	return vxlan_features_check(skb, features);
 }
 
 static const struct net_device_ops bnx2x_netdev_ops = {
@@ -12589,7 +12591,7 @@ static const struct net_device_ops bnx2x_netdev_ops = {
 #endif
 	.ndo_get_phys_port_id	= bnx2x_get_phys_port_id,
 	.ndo_set_vf_link_state	= bnx2x_set_vf_link_state,
-	.ndo_gso_check		= bnx2x_gso_check,
+	.ndo_features_check	= bnx2x_features_check,
 };
 
 static int bnx2x_set_coherency_mask(struct bnx2x *bp)
@@ -13256,7 +13258,7 @@ static int bnx2x_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
 		return -EFAULT;
 	}
 
-	DP(BNX2X_MSG_PTP, "Configrued val = %d, period = %d\n", best_val,
+	DP(BNX2X_MSG_PTP, "Configured val = %d, period = %d\n", best_val,
 	   best_period);
 
 	return 0;
@@ -13265,14 +13267,10 @@ static int bnx2x_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
 static int bnx2x_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 {
 	struct bnx2x *bp = container_of(ptp, struct bnx2x, ptp_clock_info);
-	u64 now;
 
 	DP(BNX2X_MSG_PTP, "PTP adjtime called, delta = %llx\n", delta);
 
-	now = timecounter_read(&bp->timecounter);
-	now += delta;
-	/* Re-init the timecounter */
-	timecounter_init(&bp->timecounter, &bp->cyclecounter, now);
+	timecounter_adjtime(&bp->timecounter, delta);
 
 	return 0;
 }
@@ -14612,7 +14610,7 @@ static void bnx2x_init_cyclecounter(struct bnx2x *bp)
 {
 	memset(&bp->cyclecounter, 0, sizeof(bp->cyclecounter));
 	bp->cyclecounter.read = bnx2x_cyclecounter_read;
-	bp->cyclecounter.mask = CLOCKSOURCE_MASK(64);
+	bp->cyclecounter.mask = CYCLECOUNTER_MASK(64);
 	bp->cyclecounter.shift = 1;
 	bp->cyclecounter.mult = 1;
 }
@@ -14784,7 +14782,7 @@ static int bnx2x_hwtstamp_ioctl(struct bnx2x *bp, struct ifreq *ifr)
 		-EFAULT : 0;
 }
 
-/* Configrues HW for PTP */
+/* Configures HW for PTP */
 static int bnx2x_configure_ptp(struct bnx2x *bp)
 {
 	int rc, port = BP_PORT(bp);
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
index b0779d773343..6fe547c93e74 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
@@ -7549,7 +7549,7 @@ Theotherbitsarereservedandshouldbezero*/
 #define IGU_REG_SISR_MDPC_WOMASK_UPPER		0x05a6
 
 #define IGU_REG_RESERVED_UPPER				0x05ff
-/* Fields of IGU PF CONFIGRATION REGISTER */
+/* Fields of IGU PF CONFIGURATION REGISTER */
 #define IGU_PF_CONF_FUNC_EN	  (0x1<<0)  /* function enable	      */
 #define IGU_PF_CONF_MSI_MSIX_EN   (0x1<<1)  /* MSI/MSIX enable	      */
 #define IGU_PF_CONF_INT_LINE_EN   (0x1<<2)  /* INT enable	      */
@@ -7557,7 +7557,7 @@ Theotherbitsarereservedandshouldbezero*/
 #define IGU_PF_CONF_SINGLE_ISR_EN (0x1<<4)  /* single ISR mode enable */
 #define IGU_PF_CONF_SIMD_MODE	  (0x1<<5)  /* simd all ones mode     */
 
-/* Fields of IGU VF CONFIGRATION REGISTER */
+/* Fields of IGU VF CONFIGURATION REGISTER */
 #define IGU_VF_CONF_FUNC_EN	   (0x1<<0)  /* function enable        */
 #define IGU_VF_CONF_MSI_MSIX_EN    (0x1<<1)  /* MSI/MSIX enable        */
 #define IGU_VF_CONF_PARENT_MASK    (0x3<<2)  /* Parent PF	       */
diff --git a/drivers/net/ethernet/broadcom/genet/bcmgenet.c b/drivers/net/ethernet/broadcom/genet/bcmgenet.c
index 7078bd386fb7..ff83c46bc389 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmgenet.c
+++ b/drivers/net/ethernet/broadcom/genet/bcmgenet.c
@@ -2889,7 +2889,6 @@ static struct platform_driver bcmgenet_driver = {
 	.remove	= bcmgenet_remove,
 	.driver	= {
 		.name	= "bcmgenet",
-		.owner	= THIS_MODULE,
 		.of_match_table = bcmgenet_match,
 		.pm	= &bcmgenet_pm_ops,
 	},
diff --git a/drivers/net/ethernet/broadcom/sb1250-mac.c b/drivers/net/ethernet/broadcom/sb1250-mac.c
index b61c14ed9b8d..ac27e24264a5 100644
--- a/drivers/net/ethernet/broadcom/sb1250-mac.c
+++ b/drivers/net/ethernet/broadcom/sb1250-mac.c
@@ -2664,7 +2664,6 @@ static struct platform_driver sbmac_driver = {
 	.remove = __exit_p(sbmac_remove),
 	.driver = {
 		.name = sbmac_string,
-		.owner  = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/ethernet/broadcom/tg3.c b/drivers/net/ethernet/broadcom/tg3.c
index bb48a610b72a..553dcd8a9df2 100644
--- a/drivers/net/ethernet/broadcom/tg3.c
+++ b/drivers/net/ethernet/broadcom/tg3.c
@@ -17800,23 +17800,6 @@ static int tg3_init_one(struct pci_dev *pdev,
 		goto err_out_apeunmap;
 	}
 
-	/*
-	 * Reset chip in case UNDI or EFI driver did not shutdown
-	 * DMA self test will enable WDMAC and we'll see (spurious)
-	 * pending DMA on the PCI bus at that point.
-	 */
-	if ((tr32(HOSTCC_MODE) & HOSTCC_MODE_ENABLE) ||
-	    (tr32(WDMAC_MODE) & WDMAC_MODE_ENABLE)) {
-		tw32(MEMARB_MODE, MEMARB_MODE_ENABLE);
-		tg3_halt(tp, RESET_KIND_SHUTDOWN, 1);
-	}
-
-	err = tg3_test_dma(tp);
-	if (err) {
-		dev_err(&pdev->dev, "DMA engine test failed, aborting\n");
-		goto err_out_apeunmap;
-	}
-
 	intmbx = MAILBOX_INTERRUPT_0 + TG3_64BIT_REG_LOW;
 	rcvmbx = MAILBOX_RCVRET_CON_IDX_0 + TG3_64BIT_REG_LOW;
 	sndmbx = MAILBOX_SNDHOST_PROD_IDX_0 + TG3_64BIT_REG_LOW;
@@ -17861,6 +17844,23 @@ static int tg3_init_one(struct pci_dev *pdev,
 			sndmbx += 0xc;
 	}
 
+	/*
+	 * Reset chip in case UNDI or EFI driver did not shutdown
+	 * DMA self test will enable WDMAC and we'll see (spurious)
+	 * pending DMA on the PCI bus at that point.
+	 */
+	if ((tr32(HOSTCC_MODE) & HOSTCC_MODE_ENABLE) ||
+	    (tr32(WDMAC_MODE) & WDMAC_MODE_ENABLE)) {
+		tw32(MEMARB_MODE, MEMARB_MODE_ENABLE);
+		tg3_halt(tp, RESET_KIND_SHUTDOWN, 1);
+	}
+
+	err = tg3_test_dma(tp);
+	if (err) {
+		dev_err(&pdev->dev, "DMA engine test failed, aborting\n");
+		goto err_out_apeunmap;
+	}
+
 	tg3_init_coal(tp);
 
 	pci_set_drvdata(pdev, dev);
diff --git a/drivers/net/ethernet/brocade/bna/bnad_debugfs.c b/drivers/net/ethernet/brocade/bna/bnad_debugfs.c
index 7d6aa8c87df8..619083a860a4 100644
--- a/drivers/net/ethernet/brocade/bna/bnad_debugfs.c
+++ b/drivers/net/ethernet/brocade/bna/bnad_debugfs.c
@@ -172,7 +172,7 @@ bnad_get_debug_drvinfo(struct bnad *bnad, void *buffer, u32 len)
 
 	/* Retrieve flash partition info */
 	fcomp.comp_status = 0;
-	init_completion(&fcomp.comp);
+	reinit_completion(&fcomp.comp);
 	spin_lock_irqsave(&bnad->bna_lock, flags);
 	ret = bfa_nw_flash_get_attr(&bnad->bna.flash, &drvinfo->flash_attr,
 				bnad_cb_completion, &fcomp);
diff --git a/drivers/net/ethernet/cadence/at91_ether.c b/drivers/net/ethernet/cadence/at91_ether.c
index 4a24b9a6ad75..55eb7f2af2b4 100644
--- a/drivers/net/ethernet/cadence/at91_ether.c
+++ b/drivers/net/ethernet/cadence/at91_ether.c
@@ -469,7 +469,6 @@ static struct platform_driver at91ether_driver = {
 	.resume		= at91ether_resume,
 	.driver		= {
 		.name	= "at91_ether",
-		.owner	= THIS_MODULE,
 		.of_match_table	= of_match_ptr(at91ether_dt_ids),
 	},
 };
diff --git a/drivers/net/ethernet/cadence/macb.c b/drivers/net/ethernet/cadence/macb.c
index b6bc318b148e..3767271c7667 100644
--- a/drivers/net/ethernet/cadence/macb.c
+++ b/drivers/net/ethernet/cadence/macb.c
@@ -66,23 +66,25 @@ static unsigned int macb_tx_ring_wrap(unsigned int index)
 	return index & (TX_RING_SIZE - 1);
 }
 
-static struct macb_dma_desc *macb_tx_desc(struct macb *bp, unsigned int index)
+static struct macb_dma_desc *macb_tx_desc(struct macb_queue *queue,
+					  unsigned int index)
 {
-	return &bp->tx_ring[macb_tx_ring_wrap(index)];
+	return &queue->tx_ring[macb_tx_ring_wrap(index)];
 }
 
-static struct macb_tx_skb *macb_tx_skb(struct macb *bp, unsigned int index)
+static struct macb_tx_skb *macb_tx_skb(struct macb_queue *queue,
+				       unsigned int index)
 {
-	return &bp->tx_skb[macb_tx_ring_wrap(index)];
+	return &queue->tx_skb[macb_tx_ring_wrap(index)];
 }
 
-static dma_addr_t macb_tx_dma(struct macb *bp, unsigned int index)
+static dma_addr_t macb_tx_dma(struct macb_queue *queue, unsigned int index)
 {
 	dma_addr_t offset;
 
 	offset = macb_tx_ring_wrap(index) * sizeof(struct macb_dma_desc);
 
-	return bp->tx_ring_dma + offset;
+	return queue->tx_ring_dma + offset;
 }
 
 static unsigned int macb_rx_ring_wrap(unsigned int index)
@@ -490,38 +492,49 @@ static void macb_tx_unmap(struct macb *bp, struct macb_tx_skb *tx_skb)
 
 static void macb_tx_error_task(struct work_struct *work)
 {
-	struct macb	*bp = container_of(work, struct macb, tx_error_task);
+	struct macb_queue	*queue = container_of(work, struct macb_queue,
+						      tx_error_task);
+	struct macb		*bp = queue->bp;
 	struct macb_tx_skb	*tx_skb;
+	struct macb_dma_desc	*desc;
 	struct sk_buff		*skb;
 	unsigned int		tail;
+	unsigned long		flags;
+
+	netdev_vdbg(bp->dev, "macb_tx_error_task: q = %u, t = %u, h = %u\n",
+		    (unsigned int)(queue - bp->queues),
+		    queue->tx_tail, queue->tx_head);
 
-	netdev_vdbg(bp->dev, "macb_tx_error_task: t = %u, h = %u\n",
-		    bp->tx_tail, bp->tx_head);
+	/* Prevent the queue IRQ handlers from running: each of them may call
+	 * macb_tx_interrupt(), which in turn may call netif_wake_subqueue().
+	 * As explained below, we have to halt the transmission before updating
+	 * TBQP registers so we call netif_tx_stop_all_queues() to notify the
+	 * network engine about the macb/gem being halted.
+	 */
+	spin_lock_irqsave(&bp->lock, flags);
 
 	/* Make sure nobody is trying to queue up new packets */
-	netif_stop_queue(bp->dev);
+	netif_tx_stop_all_queues(bp->dev);
 
 	/*
 	 * Stop transmission now
 	 * (in case we have just queued new packets)
+	 * macb/gem must be halted to write TBQP register
 	 */
 	if (macb_halt_tx(bp))
 		/* Just complain for now, reinitializing TX path can be good */
 		netdev_err(bp->dev, "BUG: halt tx timed out\n");
 
-	/* No need for the lock here as nobody will interrupt us anymore */
-
 	/*
 	 * Treat frames in TX queue including the ones that caused the error.
 	 * Free transmit buffers in upper layer.
 	 */
-	for (tail = bp->tx_tail; tail != bp->tx_head; tail++) {
-		struct macb_dma_desc	*desc;
-		u32			ctrl;
+	for (tail = queue->tx_tail; tail != queue->tx_head; tail++) {
+		u32	ctrl;
 
-		desc = macb_tx_desc(bp, tail);
+		desc = macb_tx_desc(queue, tail);
 		ctrl = desc->ctrl;
-		tx_skb = macb_tx_skb(bp, tail);
+		tx_skb = macb_tx_skb(queue, tail);
 		skb = tx_skb->skb;
 
 		if (ctrl & MACB_BIT(TX_USED)) {
@@ -529,7 +542,7 @@ static void macb_tx_error_task(struct work_struct *work)
 			while (!skb) {
 				macb_tx_unmap(bp, tx_skb);
 				tail++;
-				tx_skb = macb_tx_skb(bp, tail);
+				tx_skb = macb_tx_skb(queue, tail);
 				skb = tx_skb->skb;
 			}
 
@@ -558,45 +571,56 @@ static void macb_tx_error_task(struct work_struct *work)
 		macb_tx_unmap(bp, tx_skb);
 	}
 
+	/* Set end of TX queue */
+	desc = macb_tx_desc(queue, 0);
+	desc->addr = 0;
+	desc->ctrl = MACB_BIT(TX_USED);
+
 	/* Make descriptor updates visible to hardware */
 	wmb();
 
 	/* Reinitialize the TX desc queue */
-	macb_writel(bp, TBQP, bp->tx_ring_dma);
+	queue_writel(queue, TBQP, queue->tx_ring_dma);
 	/* Make TX ring reflect state of hardware */
-	bp->tx_head = bp->tx_tail = 0;
-
-	/* Now we are ready to start transmission again */
-	netif_wake_queue(bp->dev);
+	queue->tx_head = 0;
+	queue->tx_tail = 0;
 
 	/* Housework before enabling TX IRQ */
 	macb_writel(bp, TSR, macb_readl(bp, TSR));
-	macb_writel(bp, IER, MACB_TX_INT_FLAGS);
+	queue_writel(queue, IER, MACB_TX_INT_FLAGS);
+
+	/* Now we are ready to start transmission again */
+	netif_tx_start_all_queues(bp->dev);
+	macb_writel(bp, NCR, macb_readl(bp, NCR) | MACB_BIT(TSTART));
+
+	spin_unlock_irqrestore(&bp->lock, flags);
 }
 
-static void macb_tx_interrupt(struct macb *bp)
+static void macb_tx_interrupt(struct macb_queue *queue)
 {
 	unsigned int tail;
 	unsigned int head;
 	u32 status;
+	struct macb *bp = queue->bp;
+	u16 queue_index = queue - bp->queues;
 
 	status = macb_readl(bp, TSR);
 	macb_writel(bp, TSR, status);
 
 	if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
-		macb_writel(bp, ISR, MACB_BIT(TCOMP));
+		queue_writel(queue, ISR, MACB_BIT(TCOMP));
 
 	netdev_vdbg(bp->dev, "macb_tx_interrupt status = 0x%03lx\n",
 		(unsigned long)status);
 
-	head = bp->tx_head;
-	for (tail = bp->tx_tail; tail != head; tail++) {
+	head = queue->tx_head;
+	for (tail = queue->tx_tail; tail != head; tail++) {
 		struct macb_tx_skb	*tx_skb;
 		struct sk_buff		*skb;
 		struct macb_dma_desc	*desc;
 		u32			ctrl;
 
-		desc = macb_tx_desc(bp, tail);
+		desc = macb_tx_desc(queue, tail);
 
 		/* Make hw descriptor updates visible to CPU */
 		rmb();
@@ -611,7 +635,7 @@ static void macb_tx_interrupt(struct macb *bp)
 
 		/* Process all buffers of the current transmitted frame */
 		for (;; tail++) {
-			tx_skb = macb_tx_skb(bp, tail);
+			tx_skb = macb_tx_skb(queue, tail);
 			skb = tx_skb->skb;
 
 			/* First, update TX stats if needed */
@@ -634,11 +658,11 @@ static void macb_tx_interrupt(struct macb *bp)
 		}
 	}
 
-	bp->tx_tail = tail;
-	if (netif_queue_stopped(bp->dev)
-			&& CIRC_CNT(bp->tx_head, bp->tx_tail,
-				    TX_RING_SIZE) <= MACB_TX_WAKEUP_THRESH)
-		netif_wake_queue(bp->dev);
+	queue->tx_tail = tail;
+	if (__netif_subqueue_stopped(bp->dev, queue_index) &&
+	    CIRC_CNT(queue->tx_head, queue->tx_tail,
+		     TX_RING_SIZE) <= MACB_TX_WAKEUP_THRESH)
+		netif_wake_subqueue(bp->dev, queue_index);
 }
 
 static void gem_rx_refill(struct macb *bp)
@@ -949,11 +973,12 @@ static int macb_poll(struct napi_struct *napi, int budget)
 
 static irqreturn_t macb_interrupt(int irq, void *dev_id)
 {
-	struct net_device *dev = dev_id;
-	struct macb *bp = netdev_priv(dev);
+	struct macb_queue *queue = dev_id;
+	struct macb *bp = queue->bp;
+	struct net_device *dev = bp->dev;
 	u32 status;
 
-	status = macb_readl(bp, ISR);
+	status = queue_readl(queue, ISR);
 
 	if (unlikely(!status))
 		return IRQ_NONE;
@@ -963,11 +988,13 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
 	while (status) {
 		/* close possible race with dev_close */
 		if (unlikely(!netif_running(dev))) {
-			macb_writel(bp, IDR, -1);
+			queue_writel(queue, IDR, -1);
 			break;
 		}
 
-		netdev_vdbg(bp->dev, "isr = 0x%08lx\n", (unsigned long)status);
+		netdev_vdbg(bp->dev, "queue = %u, isr = 0x%08lx\n",
+			    (unsigned int)(queue - bp->queues),
+			    (unsigned long)status);
 
 		if (status & MACB_RX_INT_FLAGS) {
 			/*
@@ -977,9 +1004,9 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
 			 * is already scheduled, so disable interrupts
 			 * now.
 			 */
-			macb_writel(bp, IDR, MACB_RX_INT_FLAGS);
+			queue_writel(queue, IDR, MACB_RX_INT_FLAGS);
 			if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
-				macb_writel(bp, ISR, MACB_BIT(RCOMP));
+				queue_writel(queue, ISR, MACB_BIT(RCOMP));
 
 			if (napi_schedule_prep(&bp->napi)) {
 				netdev_vdbg(bp->dev, "scheduling RX softirq\n");
@@ -988,17 +1015,17 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
 		}
 
 		if (unlikely(status & (MACB_TX_ERR_FLAGS))) {
-			macb_writel(bp, IDR, MACB_TX_INT_FLAGS);
-			schedule_work(&bp->tx_error_task);
+			queue_writel(queue, IDR, MACB_TX_INT_FLAGS);
+			schedule_work(&queue->tx_error_task);
 
 			if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
-				macb_writel(bp, ISR, MACB_TX_ERR_FLAGS);
+				queue_writel(queue, ISR, MACB_TX_ERR_FLAGS);
 
 			break;
 		}
 
 		if (status & MACB_BIT(TCOMP))
-			macb_tx_interrupt(bp);
+			macb_tx_interrupt(queue);
 
 		/*
 		 * Link change detection isn't possible with RMII, so we'll
@@ -1013,7 +1040,7 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
 				bp->hw_stats.macb.rx_overruns++;
 
 			if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
-				macb_writel(bp, ISR, MACB_BIT(ISR_ROVR));
+				queue_writel(queue, ISR, MACB_BIT(ISR_ROVR));
 		}
 
 		if (status & MACB_BIT(HRESP)) {
@@ -1025,10 +1052,10 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
 			netdev_err(dev, "DMA bus error: HRESP not OK\n");
 
 			if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
-				macb_writel(bp, ISR, MACB_BIT(HRESP));
+				queue_writel(queue, ISR, MACB_BIT(HRESP));
 		}
 
-		status = macb_readl(bp, ISR);
+		status = queue_readl(queue, ISR);
 	}
 
 	spin_unlock(&bp->lock);
@@ -1043,10 +1070,14 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
  */
 static void macb_poll_controller(struct net_device *dev)
 {
+	struct macb *bp = netdev_priv(dev);
+	struct macb_queue *queue;
 	unsigned long flags;
+	unsigned int q;
 
 	local_irq_save(flags);
-	macb_interrupt(dev->irq, dev);
+	for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue)
+		macb_interrupt(dev->irq, queue);
 	local_irq_restore(flags);
 }
 #endif
@@ -1058,10 +1089,11 @@ static inline unsigned int macb_count_tx_descriptors(struct macb *bp,
 }
 
 static unsigned int macb_tx_map(struct macb *bp,
+				struct macb_queue *queue,
 				struct sk_buff *skb)
 {
 	dma_addr_t mapping;
-	unsigned int len, entry, i, tx_head = bp->tx_head;
+	unsigned int len, entry, i, tx_head = queue->tx_head;
 	struct macb_tx_skb *tx_skb = NULL;
 	struct macb_dma_desc *desc;
 	unsigned int offset, size, count = 0;
@@ -1075,7 +1107,7 @@ static unsigned int macb_tx_map(struct macb *bp,
 	while (len) {
 		size = min(len, bp->max_tx_length);
 		entry = macb_tx_ring_wrap(tx_head);
-		tx_skb = &bp->tx_skb[entry];
+		tx_skb = &queue->tx_skb[entry];
 
 		mapping = dma_map_single(&bp->pdev->dev,
 					 skb->data + offset,
@@ -1104,7 +1136,7 @@ static unsigned int macb_tx_map(struct macb *bp,
 		while (len) {
 			size = min(len, bp->max_tx_length);
 			entry = macb_tx_ring_wrap(tx_head);
-			tx_skb = &bp->tx_skb[entry];
+			tx_skb = &queue->tx_skb[entry];
 
 			mapping = skb_frag_dma_map(&bp->pdev->dev, frag,
 						   offset, size, DMA_TO_DEVICE);
@@ -1143,14 +1175,14 @@ static unsigned int macb_tx_map(struct macb *bp,
 	i = tx_head;
 	entry = macb_tx_ring_wrap(i);
 	ctrl = MACB_BIT(TX_USED);
-	desc = &bp->tx_ring[entry];
+	desc = &queue->tx_ring[entry];
 	desc->ctrl = ctrl;
 
 	do {
 		i--;
 		entry = macb_tx_ring_wrap(i);
-		tx_skb = &bp->tx_skb[entry];
-		desc = &bp->tx_ring[entry];
+		tx_skb = &queue->tx_skb[entry];
+		desc = &queue->tx_ring[entry];
 
 		ctrl = (u32)tx_skb->size;
 		if (eof) {
@@ -1167,17 +1199,17 @@ static unsigned int macb_tx_map(struct macb *bp,
 		 */
 		wmb();
 		desc->ctrl = ctrl;
-	} while (i != bp->tx_head);
+	} while (i != queue->tx_head);
 
-	bp->tx_head = tx_head;
+	queue->tx_head = tx_head;
 
 	return count;
 
 dma_error:
 	netdev_err(bp->dev, "TX DMA map failed\n");
 
-	for (i = bp->tx_head; i != tx_head; i++) {
-		tx_skb = macb_tx_skb(bp, i);
+	for (i = queue->tx_head; i != tx_head; i++) {
+		tx_skb = macb_tx_skb(queue, i);
 
 		macb_tx_unmap(bp, tx_skb);
 	}
@@ -1187,14 +1219,16 @@ dma_error:
 
 static int macb_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
+	u16 queue_index = skb_get_queue_mapping(skb);
 	struct macb *bp = netdev_priv(dev);
+	struct macb_queue *queue = &bp->queues[queue_index];
 	unsigned long flags;
 	unsigned int count, nr_frags, frag_size, f;
 
 #if defined(DEBUG) && defined(VERBOSE_DEBUG)
 	netdev_vdbg(bp->dev,
-		   "start_xmit: len %u head %p data %p tail %p end %p\n",
-		   skb->len, skb->head, skb->data,
+		   "start_xmit: queue %hu len %u head %p data %p tail %p end %p\n",
+		   queue_index, skb->len, skb->head, skb->data,
 		   skb_tail_pointer(skb), skb_end_pointer(skb));
 	print_hex_dump(KERN_DEBUG, "data: ", DUMP_PREFIX_OFFSET, 16, 1,
 		       skb->data, 16, true);
@@ -1214,16 +1248,16 @@ static int macb_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	spin_lock_irqsave(&bp->lock, flags);
 
 	/* This is a hard error, log it. */
-	if (CIRC_SPACE(bp->tx_head, bp->tx_tail, TX_RING_SIZE) < count) {
-		netif_stop_queue(dev);
+	if (CIRC_SPACE(queue->tx_head, queue->tx_tail, TX_RING_SIZE) < count) {
+		netif_stop_subqueue(dev, queue_index);
 		spin_unlock_irqrestore(&bp->lock, flags);
 		netdev_dbg(bp->dev, "tx_head = %u, tx_tail = %u\n",
-			   bp->tx_head, bp->tx_tail);
+			   queue->tx_head, queue->tx_tail);
 		return NETDEV_TX_BUSY;
 	}
 
 	/* Map socket buffer for DMA transfer */
-	if (!macb_tx_map(bp, skb)) {
+	if (!macb_tx_map(bp, queue, skb)) {
 		dev_kfree_skb_any(skb);
 		goto unlock;
 	}
@@ -1235,8 +1269,8 @@ static int macb_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	macb_writel(bp, NCR, macb_readl(bp, NCR) | MACB_BIT(TSTART));
 
-	if (CIRC_SPACE(bp->tx_head, bp->tx_tail, TX_RING_SIZE) < 1)
-		netif_stop_queue(dev);
+	if (CIRC_SPACE(queue->tx_head, queue->tx_tail, TX_RING_SIZE) < 1)
+		netif_stop_subqueue(dev, queue_index);
 
 unlock:
 	spin_unlock_irqrestore(&bp->lock, flags);
@@ -1304,20 +1338,24 @@ static void macb_free_rx_buffers(struct macb *bp)
 
 static void macb_free_consistent(struct macb *bp)
 {
-	if (bp->tx_skb) {
-		kfree(bp->tx_skb);
-		bp->tx_skb = NULL;
-	}
+	struct macb_queue *queue;
+	unsigned int q;
+
 	bp->macbgem_ops.mog_free_rx_buffers(bp);
 	if (bp->rx_ring) {
 		dma_free_coherent(&bp->pdev->dev, RX_RING_BYTES,
 				  bp->rx_ring, bp->rx_ring_dma);
 		bp->rx_ring = NULL;
 	}
-	if (bp->tx_ring) {
-		dma_free_coherent(&bp->pdev->dev, TX_RING_BYTES,
-				  bp->tx_ring, bp->tx_ring_dma);
-		bp->tx_ring = NULL;
+
+	for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
+		kfree(queue->tx_skb);
+		queue->tx_skb = NULL;
+		if (queue->tx_ring) {
+			dma_free_coherent(&bp->pdev->dev, TX_RING_BYTES,
+					  queue->tx_ring, queue->tx_ring_dma);
+			queue->tx_ring = NULL;
+		}
 	}
 }
 
@@ -1354,12 +1392,27 @@ static int macb_alloc_rx_buffers(struct macb *bp)
 
 static int macb_alloc_consistent(struct macb *bp)
 {
+	struct macb_queue *queue;
+	unsigned int q;
 	int size;
 
-	size = TX_RING_SIZE * sizeof(struct macb_tx_skb);
-	bp->tx_skb = kmalloc(size, GFP_KERNEL);
-	if (!bp->tx_skb)
-		goto out_err;
+	for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
+		size = TX_RING_BYTES;
+		queue->tx_ring = dma_alloc_coherent(&bp->pdev->dev, size,
+						    &queue->tx_ring_dma,
+						    GFP_KERNEL);
+		if (!queue->tx_ring)
+			goto out_err;
+		netdev_dbg(bp->dev,
+			   "Allocated TX ring for queue %u of %d bytes at %08lx (mapped %p)\n",
+			   q, size, (unsigned long)queue->tx_ring_dma,
+			   queue->tx_ring);
+
+		size = TX_RING_SIZE * sizeof(struct macb_tx_skb);
+		queue->tx_skb = kmalloc(size, GFP_KERNEL);
+		if (!queue->tx_skb)
+			goto out_err;
+	}
 
 	size = RX_RING_BYTES;
 	bp->rx_ring = dma_alloc_coherent(&bp->pdev->dev, size,
@@ -1370,15 +1423,6 @@ static int macb_alloc_consistent(struct macb *bp)
 		   "Allocated RX ring of %d bytes at %08lx (mapped %p)\n",
 		   size, (unsigned long)bp->rx_ring_dma, bp->rx_ring);
 
-	size = TX_RING_BYTES;
-	bp->tx_ring = dma_alloc_coherent(&bp->pdev->dev, size,
-					 &bp->tx_ring_dma, GFP_KERNEL);
-	if (!bp->tx_ring)
-		goto out_err;
-	netdev_dbg(bp->dev,
-		   "Allocated TX ring of %d bytes at %08lx (mapped %p)\n",
-		   size, (unsigned long)bp->tx_ring_dma, bp->tx_ring);
-
 	if (bp->macbgem_ops.mog_alloc_rx_buffers(bp))
 		goto out_err;
 
@@ -1391,15 +1435,22 @@ out_err:
 
 static void gem_init_rings(struct macb *bp)
 {
+	struct macb_queue *queue;
+	unsigned int q;
 	int i;
 
-	for (i = 0; i < TX_RING_SIZE; i++) {
-		bp->tx_ring[i].addr = 0;
-		bp->tx_ring[i].ctrl = MACB_BIT(TX_USED);
+	for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
+		for (i = 0; i < TX_RING_SIZE; i++) {
+			queue->tx_ring[i].addr = 0;
+			queue->tx_ring[i].ctrl = MACB_BIT(TX_USED);
+		}
+		queue->tx_ring[TX_RING_SIZE - 1].ctrl |= MACB_BIT(TX_WRAP);
+		queue->tx_head = 0;
+		queue->tx_tail = 0;
 	}
-	bp->tx_ring[TX_RING_SIZE - 1].ctrl |= MACB_BIT(TX_WRAP);
 
-	bp->rx_tail = bp->rx_prepared_head = bp->tx_head = bp->tx_tail = 0;
+	bp->rx_tail = 0;
+	bp->rx_prepared_head = 0;
 
 	gem_rx_refill(bp);
 }
@@ -1418,16 +1469,21 @@ static void macb_init_rings(struct macb *bp)
 	bp->rx_ring[RX_RING_SIZE - 1].addr |= MACB_BIT(RX_WRAP);
 
 	for (i = 0; i < TX_RING_SIZE; i++) {
-		bp->tx_ring[i].addr = 0;
-		bp->tx_ring[i].ctrl = MACB_BIT(TX_USED);
+		bp->queues[0].tx_ring[i].addr = 0;
+		bp->queues[0].tx_ring[i].ctrl = MACB_BIT(TX_USED);
+		bp->queues[0].tx_head = 0;
+		bp->queues[0].tx_tail = 0;
 	}
-	bp->tx_ring[TX_RING_SIZE - 1].ctrl |= MACB_BIT(TX_WRAP);
+	bp->queues[0].tx_ring[TX_RING_SIZE - 1].ctrl |= MACB_BIT(TX_WRAP);
 
-	bp->rx_tail = bp->tx_head = bp->tx_tail = 0;
+	bp->rx_tail = 0;
 }
 
 static void macb_reset_hw(struct macb *bp)
 {
+	struct macb_queue *queue;
+	unsigned int q;
+
 	/*
 	 * Disable RX and TX (XXX: Should we halt the transmission
 	 * more gracefully?)
@@ -1442,8 +1498,10 @@ static void macb_reset_hw(struct macb *bp)
 	macb_writel(bp, RSR, -1);
 
 	/* Disable all interrupts */
-	macb_writel(bp, IDR, -1);
-	macb_readl(bp, ISR);
+	for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
+		queue_writel(queue, IDR, -1);
+		queue_readl(queue, ISR);
+	}
 }
 
 static u32 gem_mdc_clk_div(struct macb *bp)
@@ -1540,6 +1598,9 @@ static void macb_configure_dma(struct macb *bp)
 
 static void macb_init_hw(struct macb *bp)
 {
+	struct macb_queue *queue;
+	unsigned int q;
+
 	u32 config;
 
 	macb_reset_hw(bp);
@@ -1565,16 +1626,18 @@ static void macb_init_hw(struct macb *bp)
 
 	/* Initialize TX and RX buffers */
 	macb_writel(bp, RBQP, bp->rx_ring_dma);
-	macb_writel(bp, TBQP, bp->tx_ring_dma);
+	for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
+		queue_writel(queue, TBQP, queue->tx_ring_dma);
+
+		/* Enable interrupts */
+		queue_writel(queue, IER,
+			     MACB_RX_INT_FLAGS |
+			     MACB_TX_INT_FLAGS |
+			     MACB_BIT(HRESP));
+	}
 
 	/* Enable TX and RX */
 	macb_writel(bp, NCR, MACB_BIT(RE) | MACB_BIT(TE) | MACB_BIT(MPE));
-
-	/* Enable interrupts */
-	macb_writel(bp, IER, (MACB_RX_INT_FLAGS
-			      | MACB_TX_INT_FLAGS
-			      | MACB_BIT(HRESP)));
-
 }
 
 /*
@@ -1736,7 +1799,7 @@ static int macb_open(struct net_device *dev)
 	/* schedule a link state check */
 	phy_start(bp->phy_dev);
 
-	netif_start_queue(dev);
+	netif_tx_start_all_queues(dev);
 
 	return 0;
 }
@@ -1746,7 +1809,7 @@ static int macb_close(struct net_device *dev)
 	struct macb *bp = netdev_priv(dev);
 	unsigned long flags;
 
-	netif_stop_queue(dev);
+	netif_tx_stop_all_queues(dev);
 	napi_disable(&bp->napi);
 
 	if (bp->phy_dev)
@@ -1895,8 +1958,8 @@ static void macb_get_regs(struct net_device *dev, struct ethtool_regs *regs,
 	regs->version = (macb_readl(bp, MID) & ((1 << MACB_REV_SIZE) - 1))
 			| MACB_GREGS_VERSION;
 
-	tail = macb_tx_ring_wrap(bp->tx_tail);
-	head = macb_tx_ring_wrap(bp->tx_head);
+	tail = macb_tx_ring_wrap(bp->queues[0].tx_tail);
+	head = macb_tx_ring_wrap(bp->queues[0].tx_head);
 
 	regs_buff[0]  = macb_readl(bp, NCR);
 	regs_buff[1]  = macb_or_gem_readl(bp, NCFGR);
@@ -1909,8 +1972,8 @@ static void macb_get_regs(struct net_device *dev, struct ethtool_regs *regs,
 
 	regs_buff[8]  = tail;
 	regs_buff[9]  = head;
-	regs_buff[10] = macb_tx_dma(bp, tail);
-	regs_buff[11] = macb_tx_dma(bp, head);
+	regs_buff[10] = macb_tx_dma(&bp->queues[0], tail);
+	regs_buff[11] = macb_tx_dma(&bp->queues[0], head);
 
 	if (macb_is_gem(bp)) {
 		regs_buff[12] = gem_readl(bp, USRIO);
@@ -2061,16 +2124,44 @@ static void macb_configure_caps(struct macb *bp)
 	netdev_dbg(bp->dev, "Cadence caps 0x%08x\n", bp->caps);
 }
 
+static void macb_probe_queues(void __iomem *mem,
+			      unsigned int *queue_mask,
+			      unsigned int *num_queues)
+{
+	unsigned int hw_q;
+	u32 mid;
+
+	*queue_mask = 0x1;
+	*num_queues = 1;
+
+	/* is it macb or gem ? */
+	mid = __raw_readl(mem + MACB_MID);
+	if (MACB_BFEXT(IDNUM, mid) != 0x2)
+		return;
+
+	/* bit 0 is never set but queue 0 always exists */
+	*queue_mask = __raw_readl(mem + GEM_DCFG6) & 0xff;
+	*queue_mask |= 0x1;
+
+	for (hw_q = 1; hw_q < MACB_MAX_QUEUES; ++hw_q)
+		if (*queue_mask & (1 << hw_q))
+			(*num_queues)++;
+}
+
 static int __init macb_probe(struct platform_device *pdev)
 {
 	struct macb_platform_data *pdata;
 	struct resource *regs;
 	struct net_device *dev;
 	struct macb *bp;
+	struct macb_queue *queue;
 	struct phy_device *phydev;
 	u32 config;
 	int err = -ENXIO;
 	const char *mac;
+	void __iomem *mem;
+	unsigned int hw_q, queue_mask, q, num_queues;
+	struct clk *pclk, *hclk, *tx_clk;
 
 	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!regs) {
@@ -2078,72 +2169,112 @@ static int __init macb_probe(struct platform_device *pdev)
 		goto err_out;
 	}
 
-	err = -ENOMEM;
-	dev = alloc_etherdev(sizeof(*bp));
-	if (!dev)
-		goto err_out;
-
-	SET_NETDEV_DEV(dev, &pdev->dev);
-
-	bp = netdev_priv(dev);
-	bp->pdev = pdev;
-	bp->dev = dev;
-
-	spin_lock_init(&bp->lock);
-	INIT_WORK(&bp->tx_error_task, macb_tx_error_task);
-
-	bp->pclk = devm_clk_get(&pdev->dev, "pclk");
-	if (IS_ERR(bp->pclk)) {
-		err = PTR_ERR(bp->pclk);
+	pclk = devm_clk_get(&pdev->dev, "pclk");
+	if (IS_ERR(pclk)) {
+		err = PTR_ERR(pclk);
 		dev_err(&pdev->dev, "failed to get macb_clk (%u)\n", err);
-		goto err_out_free_dev;
+		goto err_out;
 	}
 
-	bp->hclk = devm_clk_get(&pdev->dev, "hclk");
-	if (IS_ERR(bp->hclk)) {
-		err = PTR_ERR(bp->hclk);
+	hclk = devm_clk_get(&pdev->dev, "hclk");
+	if (IS_ERR(hclk)) {
+		err = PTR_ERR(hclk);
 		dev_err(&pdev->dev, "failed to get hclk (%u)\n", err);
-		goto err_out_free_dev;
+		goto err_out;
 	}
 
-	bp->tx_clk = devm_clk_get(&pdev->dev, "tx_clk");
+	tx_clk = devm_clk_get(&pdev->dev, "tx_clk");
 
-	err = clk_prepare_enable(bp->pclk);
+	err = clk_prepare_enable(pclk);
 	if (err) {
 		dev_err(&pdev->dev, "failed to enable pclk (%u)\n", err);
-		goto err_out_free_dev;
+		goto err_out;
 	}
 
-	err = clk_prepare_enable(bp->hclk);
+	err = clk_prepare_enable(hclk);
 	if (err) {
 		dev_err(&pdev->dev, "failed to enable hclk (%u)\n", err);
 		goto err_out_disable_pclk;
 	}
 
-	if (!IS_ERR(bp->tx_clk)) {
-		err = clk_prepare_enable(bp->tx_clk);
+	if (!IS_ERR(tx_clk)) {
+		err = clk_prepare_enable(tx_clk);
 		if (err) {
 			dev_err(&pdev->dev, "failed to enable tx_clk (%u)\n",
-					err);
+				err);
 			goto err_out_disable_hclk;
 		}
 	}
 
-	bp->regs = devm_ioremap(&pdev->dev, regs->start, resource_size(regs));
-	if (!bp->regs) {
+	err = -ENOMEM;
+	mem = devm_ioremap(&pdev->dev, regs->start, resource_size(regs));
+	if (!mem) {
 		dev_err(&pdev->dev, "failed to map registers, aborting.\n");
-		err = -ENOMEM;
 		goto err_out_disable_clocks;
 	}
 
-	dev->irq = platform_get_irq(pdev, 0);
-	err = devm_request_irq(&pdev->dev, dev->irq, macb_interrupt, 0,
-			dev->name, dev);
-	if (err) {
-		dev_err(&pdev->dev, "Unable to request IRQ %d (error %d)\n",
-			dev->irq, err);
+	macb_probe_queues(mem, &queue_mask, &num_queues);
+	dev = alloc_etherdev_mq(sizeof(*bp), num_queues);
+	if (!dev)
 		goto err_out_disable_clocks;
+
+	SET_NETDEV_DEV(dev, &pdev->dev);
+
+	bp = netdev_priv(dev);
+	bp->pdev = pdev;
+	bp->dev = dev;
+	bp->regs = mem;
+	bp->num_queues = num_queues;
+	bp->pclk = pclk;
+	bp->hclk = hclk;
+	bp->tx_clk = tx_clk;
+
+	spin_lock_init(&bp->lock);
+
+	/* set the queue register mapping once for all: queue0 has a special
+	 * register mapping but we don't want to test the queue index then
+	 * compute the corresponding register offset at run time.
+	 */
+	for (hw_q = 0, q = 0; hw_q < MACB_MAX_QUEUES; ++hw_q) {
+		if (!(queue_mask & (1 << hw_q)))
+			continue;
+
+		queue = &bp->queues[q];
+		queue->bp = bp;
+		if (hw_q) {
+			queue->ISR  = GEM_ISR(hw_q - 1);
+			queue->IER  = GEM_IER(hw_q - 1);
+			queue->IDR  = GEM_IDR(hw_q - 1);
+			queue->IMR  = GEM_IMR(hw_q - 1);
+			queue->TBQP = GEM_TBQP(hw_q - 1);
+		} else {
+			/* queue0 uses legacy registers */
+			queue->ISR  = MACB_ISR;
+			queue->IER  = MACB_IER;
+			queue->IDR  = MACB_IDR;
+			queue->IMR  = MACB_IMR;
+			queue->TBQP = MACB_TBQP;
+		}
+
+		/* get irq: here we use the linux queue index, not the hardware
+		 * queue index. the queue irq definitions in the device tree
+		 * must remove the optional gaps that could exist in the
+		 * hardware queue mask.
+		 */
+		queue->irq = platform_get_irq(pdev, q);
+		err = devm_request_irq(&pdev->dev, queue->irq, macb_interrupt,
+				       0, dev->name, queue);
+		if (err) {
+			dev_err(&pdev->dev,
+				"Unable to request IRQ %d (error %d)\n",
+				queue->irq, err);
+			goto err_out_free_netdev;
+		}
+
+		INIT_WORK(&queue->tx_error_task, macb_tx_error_task);
+		q++;
 	}
+	dev->irq = bp->queues[0].irq;
 
 	dev->netdev_ops = &macb_netdev_ops;
 	netif_napi_add(dev, &bp->napi, macb_poll, 64);
@@ -2219,7 +2350,7 @@ static int __init macb_probe(struct platform_device *pdev)
 	err = register_netdev(dev);
 	if (err) {
 		dev_err(&pdev->dev, "Cannot register net device, aborting.\n");
-		goto err_out_disable_clocks;
+		goto err_out_free_netdev;
 	}
 
 	err = macb_mii_init(bp);
@@ -2242,15 +2373,15 @@ static int __init macb_probe(struct platform_device *pdev)
 
 err_out_unregister_netdev:
 	unregister_netdev(dev);
+err_out_free_netdev:
+	free_netdev(dev);
 err_out_disable_clocks:
-	if (!IS_ERR(bp->tx_clk))
-		clk_disable_unprepare(bp->tx_clk);
+	if (!IS_ERR(tx_clk))
+		clk_disable_unprepare(tx_clk);
 err_out_disable_hclk:
-	clk_disable_unprepare(bp->hclk);
+	clk_disable_unprepare(hclk);
 err_out_disable_pclk:
-	clk_disable_unprepare(bp->pclk);
-err_out_free_dev:
-	free_netdev(dev);
+	clk_disable_unprepare(pclk);
 err_out:
 	return err;
 }
@@ -2321,7 +2452,6 @@ static struct platform_driver macb_driver = {
 	.remove		= __exit_p(macb_remove),
 	.driver		= {
 		.name		= "macb",
-		.owner	= THIS_MODULE,
 		.of_match_table	= of_match_ptr(macb_dt_ids),
 		.pm	= &macb_pm_ops,
 	},
diff --git a/drivers/net/ethernet/cadence/macb.h b/drivers/net/ethernet/cadence/macb.h
index 517c09d72c4a..084191b6fad2 100644
--- a/drivers/net/ethernet/cadence/macb.h
+++ b/drivers/net/ethernet/cadence/macb.h
@@ -12,6 +12,7 @@
 
 #define MACB_GREGS_NBR 16
 #define MACB_GREGS_VERSION 1
+#define MACB_MAX_QUEUES 8
 
 /* MACB register offsets */
 #define MACB_NCR				0x0000
@@ -89,6 +90,13 @@
 #define GEM_DCFG6				0x0294
 #define GEM_DCFG7				0x0298
 
+#define GEM_ISR(hw_q)				(0x0400 + ((hw_q) << 2))
+#define GEM_TBQP(hw_q)				(0x0440 + ((hw_q) << 2))
+#define GEM_RBQP(hw_q)				(0x0480 + ((hw_q) << 2))
+#define GEM_IER(hw_q)				(0x0600 + ((hw_q) << 2))
+#define GEM_IDR(hw_q)				(0x0620 + ((hw_q) << 2))
+#define GEM_IMR(hw_q)				(0x0640 + ((hw_q) << 2))
+
 /* Bitfields in NCR */
 #define MACB_LB_OFFSET				0
 #define MACB_LB_SIZE				1
@@ -376,6 +384,10 @@
 	__raw_readl((port)->regs + GEM_##reg)
 #define gem_writel(port, reg, value)			\
 	__raw_writel((value), (port)->regs + GEM_##reg)
+#define queue_readl(queue, reg)				\
+	__raw_readl((queue)->bp->regs + (queue)->reg)
+#define queue_writel(queue, reg, value)			\
+	__raw_writel((value), (queue)->bp->regs + (queue)->reg)
 
 /*
  * Conditional GEM/MACB macros.  These perform the operation to the correct
@@ -597,6 +609,23 @@ struct macb_config {
 	unsigned int		dma_burst_length;
 };
 
+struct macb_queue {
+	struct macb		*bp;
+	int			irq;
+
+	unsigned int		ISR;
+	unsigned int		IER;
+	unsigned int		IDR;
+	unsigned int		IMR;
+	unsigned int		TBQP;
+
+	unsigned int		tx_head, tx_tail;
+	struct macb_dma_desc	*tx_ring;
+	struct macb_tx_skb	*tx_skb;
+	dma_addr_t		tx_ring_dma;
+	struct work_struct	tx_error_task;
+};
+
 struct macb {
 	void __iomem		*regs;
 
@@ -607,9 +636,8 @@ struct macb {
 	void			*rx_buffers;
 	size_t			rx_buffer_size;
 
-	unsigned int		tx_head, tx_tail;
-	struct macb_dma_desc	*tx_ring;
-	struct macb_tx_skb	*tx_skb;
+	unsigned int		num_queues;
+	struct macb_queue	queues[MACB_MAX_QUEUES];
 
 	spinlock_t		lock;
 	struct platform_device	*pdev;
@@ -618,7 +646,6 @@ struct macb {
 	struct clk		*tx_clk;
 	struct net_device	*dev;
 	struct napi_struct	napi;
-	struct work_struct	tx_error_task;
 	struct net_device_stats	stats;
 	union {
 		struct macb_stats	macb;
@@ -626,7 +653,6 @@ struct macb {
 	}			hw_stats;
 
 	dma_addr_t		rx_ring_dma;
-	dma_addr_t		tx_ring_dma;
 	dma_addr_t		rx_buffers_dma;
 
 	struct macb_or_gem_ops	macbgem_ops;
diff --git a/drivers/net/ethernet/chelsio/cxgb3/mc5.c b/drivers/net/ethernet/chelsio/cxgb3/mc5.c
index e13b7fe9d082..338301b11518 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/mc5.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/mc5.c
@@ -97,14 +97,6 @@ static int mc5_cmd_write(struct adapter *adapter, u32 cmd)
 			       F_DBGIRSPVALID, 1, MAX_WRITE_ATTEMPTS, 1);
 }
 
-static inline void dbgi_wr_addr3(struct adapter *adapter, u32 v1, u32 v2,
-				 u32 v3)
-{
-	t3_write_reg(adapter, A_MC5_DB_DBGI_REQ_ADDR0, v1);
-	t3_write_reg(adapter, A_MC5_DB_DBGI_REQ_ADDR1, v2);
-	t3_write_reg(adapter, A_MC5_DB_DBGI_REQ_ADDR2, v3);
-}
-
 static inline void dbgi_wr_data3(struct adapter *adapter, u32 v1, u32 v2,
 				 u32 v3)
 {
@@ -113,14 +105,6 @@ static inline void dbgi_wr_data3(struct adapter *adapter, u32 v1, u32 v2,
 	t3_write_reg(adapter, A_MC5_DB_DBGI_REQ_DATA2, v3);
 }
 
-static inline void dbgi_rd_rsp3(struct adapter *adapter, u32 *v1, u32 *v2,
-				u32 *v3)
-{
-	*v1 = t3_read_reg(adapter, A_MC5_DB_DBGI_RSP_DATA0);
-	*v2 = t3_read_reg(adapter, A_MC5_DB_DBGI_RSP_DATA1);
-	*v3 = t3_read_reg(adapter, A_MC5_DB_DBGI_RSP_DATA2);
-}
-
 /*
  * Write data to the TCAM register at address (0, 0, addr_lo) using the TCAM
  * command cmd.  The data to be written must have been set up by the caller.
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
index a18d33fdb271..5ab5c3133acd 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
@@ -392,7 +392,7 @@ struct port_info {
 	s16    xact_addr_filt;        /* index of exact MAC address filter */
 	u16    rss_size;              /* size of VI's RSS table slice */
 	s8     mdio_addr;
-	u8     port_type;
+	enum fw_port_type port_type;
 	u8     mod_type;
 	u8     port_id;
 	u8     tx_chan;
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
index 973dbb7938c3..ccf3436024bc 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
@@ -2325,7 +2325,7 @@ static int identify_port(struct net_device *dev,
 	return t4_identify_port(adap, adap->fn, netdev2pinfo(dev)->viid, val);
 }
 
-static unsigned int from_fw_linkcaps(unsigned int type, unsigned int caps)
+static unsigned int from_fw_linkcaps(enum fw_port_type type, unsigned int caps)
 {
 	unsigned int v = 0;
 
@@ -2354,14 +2354,20 @@ static unsigned int from_fw_linkcaps(unsigned int type, unsigned int caps)
 		     SUPPORTED_10000baseKR_Full | SUPPORTED_1000baseKX_Full |
 		     SUPPORTED_10000baseKX4_Full;
 	else if (type == FW_PORT_TYPE_FIBER_XFI ||
-		 type == FW_PORT_TYPE_FIBER_XAUI || type == FW_PORT_TYPE_SFP) {
+		 type == FW_PORT_TYPE_FIBER_XAUI ||
+		 type == FW_PORT_TYPE_SFP ||
+		 type == FW_PORT_TYPE_QSFP_10G ||
+		 type == FW_PORT_TYPE_QSA) {
 		v |= SUPPORTED_FIBRE;
 		if (caps & FW_PORT_CAP_SPEED_1G)
 			v |= SUPPORTED_1000baseT_Full;
 		if (caps & FW_PORT_CAP_SPEED_10G)
 			v |= SUPPORTED_10000baseT_Full;
-	} else if (type == FW_PORT_TYPE_BP40_BA)
+	} else if (type == FW_PORT_TYPE_BP40_BA ||
+		   type == FW_PORT_TYPE_QSFP) {
 		v |= SUPPORTED_40000baseSR4_Full;
+		v |= SUPPORTED_FIBRE;
+	}
 
 	if (caps & FW_PORT_CAP_ANEG)
 		v |= SUPPORTED_Autoneg;
@@ -2396,6 +2402,7 @@ static int get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 		cmd->port = PORT_FIBRE;
 	else if (p->port_type == FW_PORT_TYPE_SFP ||
 		 p->port_type == FW_PORT_TYPE_QSFP_10G ||
+		 p->port_type == FW_PORT_TYPE_QSA ||
 		 p->port_type == FW_PORT_TYPE_QSFP) {
 		if (p->mod_type == FW_PORT_MOD_TYPE_LR ||
 		    p->mod_type == FW_PORT_MOD_TYPE_SR ||
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
index 28d04153f999..c132d9030729 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
@@ -2376,7 +2376,7 @@ const char *t4_get_port_type_description(enum fw_port_type port_type)
 		"KR/KX",
 		"KR/KX/KX4",
 		"R QSFP_10G",
-		"",
+		"R QSA",
 		"R QSFP",
 		"R BP40_BA",
 	};
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h b/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
index beaf80a6214b..7c0aec85137a 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
@@ -560,6 +560,7 @@ enum fw_flowc_mnem {
 	FW_FLOWC_MNEM_RCVNXT,
 	FW_FLOWC_MNEM_SNDBUF,
 	FW_FLOWC_MNEM_MSS,
+	FW_FLOWC_MNEM_TXDATAPLEN_MAX,
 };
 
 struct fw_flowc_mnemval {
@@ -2469,6 +2470,7 @@ enum fw_port_type {
 	FW_PORT_TYPE_BP_AP,
 	FW_PORT_TYPE_BP4_AP,
 	FW_PORT_TYPE_QSFP_10G,
+	FW_PORT_TYPE_QSA,
 	FW_PORT_TYPE_QSFP,
 	FW_PORT_TYPE_BP40_BA,
 
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/adapter.h b/drivers/net/ethernet/chelsio/cxgb4vf/adapter.h
index d00a751f0588..6049f70e110c 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/adapter.h
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/adapter.h
@@ -96,6 +96,9 @@ struct port_info {
 	s16 xact_addr_filt;		/* index of our MAC address filter */
 	u16 rss_size;			/* size of VI's RSS table slice */
 	u8 pidx;			/* index into adapter port[] */
+	s8 mdio_addr;
+	u8 port_type;			/* firmware port type */
+	u8 mod_type;			/* firmware module type */
 	u8 port_id;			/* physical port ID */
 	u8 nqsets;			/* # of "Queue Sets" */
 	u8 first_qset;			/* index of first "Queue Set" */
@@ -522,6 +525,7 @@ static inline struct adapter *netdev2adap(const struct net_device *dev)
  * is "contracted" to provide for the common code.
  */
 void t4vf_os_link_changed(struct adapter *, int, int);
+void t4vf_os_portmod_changed(struct adapter *, int);
 
 /*
  * SGE function prototype declarations.
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c b/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
index aa74ec34a467..2215d432a059 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
@@ -44,6 +44,7 @@
 #include <linux/etherdevice.h>
 #include <linux/debugfs.h>
 #include <linux/ethtool.h>
+#include <linux/mdio.h>
 
 #include "t4vf_common.h"
 #include "t4vf_defs.h"
@@ -210,6 +211,38 @@ void t4vf_os_link_changed(struct adapter *adapter, int pidx, int link_ok)
 }
 
 /*
+ * THe port module type has changed on the indicated "port" (Virtual
+ * Interface).
+ */
+void t4vf_os_portmod_changed(struct adapter *adapter, int pidx)
+{
+	static const char * const mod_str[] = {
+		NULL, "LR", "SR", "ER", "passive DA", "active DA", "LRM"
+	};
+	const struct net_device *dev = adapter->port[pidx];
+	const struct port_info *pi = netdev_priv(dev);
+
+	if (pi->mod_type == FW_PORT_MOD_TYPE_NONE)
+		dev_info(adapter->pdev_dev, "%s: port module unplugged\n",
+			 dev->name);
+	else if (pi->mod_type < ARRAY_SIZE(mod_str))
+		dev_info(adapter->pdev_dev, "%s: %s port module inserted\n",
+			 dev->name, mod_str[pi->mod_type]);
+	else if (pi->mod_type == FW_PORT_MOD_TYPE_NOTSUPPORTED)
+		dev_info(adapter->pdev_dev, "%s: unsupported optical port "
+			 "module inserted\n", dev->name);
+	else if (pi->mod_type == FW_PORT_MOD_TYPE_UNKNOWN)
+		dev_info(adapter->pdev_dev, "%s: unknown port module inserted,"
+			 "forcing TWINAX\n", dev->name);
+	else if (pi->mod_type == FW_PORT_MOD_TYPE_ERROR)
+		dev_info(adapter->pdev_dev, "%s: transceiver module error\n",
+			 dev->name);
+	else
+		dev_info(adapter->pdev_dev, "%s: unknown module type %d "
+			 "inserted\n", dev->name, pi->mod_type);
+}
+
+/*
  * Net device operations.
  * ======================
  */
@@ -1193,24 +1226,103 @@ static void cxgb4vf_poll_controller(struct net_device *dev)
  * state of the port to which we're linked.
  */
 
-/*
- * Return current port link settings.
- */
-static int cxgb4vf_get_settings(struct net_device *dev,
-				struct ethtool_cmd *cmd)
-{
-	const struct port_info *pi = netdev_priv(dev);
+static unsigned int t4vf_from_fw_linkcaps(enum fw_port_type type,
+					  unsigned int caps)
+{
+	unsigned int v = 0;
+
+	if (type == FW_PORT_TYPE_BT_SGMII || type == FW_PORT_TYPE_BT_XFI ||
+	    type == FW_PORT_TYPE_BT_XAUI) {
+		v |= SUPPORTED_TP;
+		if (caps & FW_PORT_CAP_SPEED_100M)
+			v |= SUPPORTED_100baseT_Full;
+		if (caps & FW_PORT_CAP_SPEED_1G)
+			v |= SUPPORTED_1000baseT_Full;
+		if (caps & FW_PORT_CAP_SPEED_10G)
+			v |= SUPPORTED_10000baseT_Full;
+	} else if (type == FW_PORT_TYPE_KX4 || type == FW_PORT_TYPE_KX) {
+		v |= SUPPORTED_Backplane;
+		if (caps & FW_PORT_CAP_SPEED_1G)
+			v |= SUPPORTED_1000baseKX_Full;
+		if (caps & FW_PORT_CAP_SPEED_10G)
+			v |= SUPPORTED_10000baseKX4_Full;
+	} else if (type == FW_PORT_TYPE_KR)
+		v |= SUPPORTED_Backplane | SUPPORTED_10000baseKR_Full;
+	else if (type == FW_PORT_TYPE_BP_AP)
+		v |= SUPPORTED_Backplane | SUPPORTED_10000baseR_FEC |
+		     SUPPORTED_10000baseKR_Full | SUPPORTED_1000baseKX_Full;
+	else if (type == FW_PORT_TYPE_BP4_AP)
+		v |= SUPPORTED_Backplane | SUPPORTED_10000baseR_FEC |
+		     SUPPORTED_10000baseKR_Full | SUPPORTED_1000baseKX_Full |
+		     SUPPORTED_10000baseKX4_Full;
+	else if (type == FW_PORT_TYPE_FIBER_XFI ||
+		 type == FW_PORT_TYPE_FIBER_XAUI ||
+		 type == FW_PORT_TYPE_SFP ||
+		 type == FW_PORT_TYPE_QSFP_10G ||
+		 type == FW_PORT_TYPE_QSA) {
+		v |= SUPPORTED_FIBRE;
+		if (caps & FW_PORT_CAP_SPEED_1G)
+			v |= SUPPORTED_1000baseT_Full;
+		if (caps & FW_PORT_CAP_SPEED_10G)
+			v |= SUPPORTED_10000baseT_Full;
+	} else if (type == FW_PORT_TYPE_BP40_BA ||
+		   type == FW_PORT_TYPE_QSFP) {
+		v |= SUPPORTED_40000baseSR4_Full;
+		v |= SUPPORTED_FIBRE;
+	}
+
+	if (caps & FW_PORT_CAP_ANEG)
+		v |= SUPPORTED_Autoneg;
+	return v;
+}
+
+static int cxgb4vf_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
+{
+	const struct port_info *p = netdev_priv(dev);
+
+	if (p->port_type == FW_PORT_TYPE_BT_SGMII ||
+	    p->port_type == FW_PORT_TYPE_BT_XFI ||
+	    p->port_type == FW_PORT_TYPE_BT_XAUI)
+		cmd->port = PORT_TP;
+	else if (p->port_type == FW_PORT_TYPE_FIBER_XFI ||
+		 p->port_type == FW_PORT_TYPE_FIBER_XAUI)
+		cmd->port = PORT_FIBRE;
+	else if (p->port_type == FW_PORT_TYPE_SFP ||
+		 p->port_type == FW_PORT_TYPE_QSFP_10G ||
+		 p->port_type == FW_PORT_TYPE_QSA ||
+		 p->port_type == FW_PORT_TYPE_QSFP) {
+		if (p->mod_type == FW_PORT_MOD_TYPE_LR ||
+		    p->mod_type == FW_PORT_MOD_TYPE_SR ||
+		    p->mod_type == FW_PORT_MOD_TYPE_ER ||
+		    p->mod_type == FW_PORT_MOD_TYPE_LRM)
+			cmd->port = PORT_FIBRE;
+		else if (p->mod_type == FW_PORT_MOD_TYPE_TWINAX_PASSIVE ||
+			 p->mod_type == FW_PORT_MOD_TYPE_TWINAX_ACTIVE)
+			cmd->port = PORT_DA;
+		else
+			cmd->port = PORT_OTHER;
+	} else
+		cmd->port = PORT_OTHER;
 
-	cmd->supported = pi->link_cfg.supported;
-	cmd->advertising = pi->link_cfg.advertising;
+	if (p->mdio_addr >= 0) {
+		cmd->phy_address = p->mdio_addr;
+		cmd->transceiver = XCVR_EXTERNAL;
+		cmd->mdio_support = p->port_type == FW_PORT_TYPE_BT_SGMII ?
+			MDIO_SUPPORTS_C22 : MDIO_SUPPORTS_C45;
+	} else {
+		cmd->phy_address = 0;  /* not really, but no better option */
+		cmd->transceiver = XCVR_INTERNAL;
+		cmd->mdio_support = 0;
+	}
+
+	cmd->supported = t4vf_from_fw_linkcaps(p->port_type,
+					       p->link_cfg.supported);
+	cmd->advertising = t4vf_from_fw_linkcaps(p->port_type,
+					    p->link_cfg.advertising);
 	ethtool_cmd_speed_set(cmd,
-			      netif_carrier_ok(dev) ? pi->link_cfg.speed : -1);
+			      netif_carrier_ok(dev) ? p->link_cfg.speed : 0);
 	cmd->duplex = DUPLEX_FULL;
-
-	cmd->port = (cmd->supported & SUPPORTED_TP) ? PORT_TP : PORT_FIBRE;
-	cmd->phy_address = pi->port_id;
-	cmd->transceiver = XCVR_EXTERNAL;
-	cmd->autoneg = pi->link_cfg.autoneg;
+	cmd->autoneg = p->link_cfg.autoneg;
 	cmd->maxtxpkt = 0;
 	cmd->maxrxpkt = 0;
 	return 0;
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h
index 8d3237f5e364..b9debb4f29a3 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h
@@ -230,7 +230,7 @@ struct adapter_params {
 
 static inline bool is_10g_port(const struct link_config *lc)
 {
-	return (lc->supported & SUPPORTED_10000baseT_Full) != 0;
+	return (lc->supported & FW_PORT_CAP_SPEED_10G) != 0;
 }
 
 static inline bool is_x_10g_port(const struct link_config *lc)
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
index 02e8833b7797..21dc9a20308c 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
@@ -245,6 +245,10 @@ static int hash_mac_addr(const u8 *addr)
 	return a & 0x3f;
 }
 
+#define ADVERT_MASK (FW_PORT_CAP_SPEED_100M | FW_PORT_CAP_SPEED_1G |\
+		     FW_PORT_CAP_SPEED_10G | FW_PORT_CAP_SPEED_40G | \
+		     FW_PORT_CAP_SPEED_100G | FW_PORT_CAP_ANEG)
+
 /**
  *	init_link_config - initialize a link's SW state
  *	@lc: structure holding the link state
@@ -259,8 +263,8 @@ static void init_link_config(struct link_config *lc, unsigned int caps)
 	lc->requested_speed = 0;
 	lc->speed = 0;
 	lc->requested_fc = lc->fc = PAUSE_RX | PAUSE_TX;
-	if (lc->supported & SUPPORTED_Autoneg) {
-		lc->advertising = lc->supported;
+	if (lc->supported & FW_PORT_CAP_ANEG) {
+		lc->advertising = lc->supported & ADVERT_MASK;
 		lc->autoneg = AUTONEG_ENABLE;
 		lc->requested_fc |= PAUSE_AUTONEG;
 	} else {
@@ -280,7 +284,6 @@ int t4vf_port_init(struct adapter *adapter, int pidx)
 	struct fw_vi_cmd vi_cmd, vi_rpl;
 	struct fw_port_cmd port_cmd, port_rpl;
 	int v;
-	u32 word;
 
 	/*
 	 * Execute a VI Read command to get our Virtual Interface information
@@ -319,19 +322,11 @@ int t4vf_port_init(struct adapter *adapter, int pidx)
 	if (v)
 		return v;
 
-	v = 0;
-	word = be16_to_cpu(port_rpl.u.info.pcap);
-	if (word & FW_PORT_CAP_SPEED_100M)
-		v |= SUPPORTED_100baseT_Full;
-	if (word & FW_PORT_CAP_SPEED_1G)
-		v |= SUPPORTED_1000baseT_Full;
-	if (word & FW_PORT_CAP_SPEED_10G)
-		v |= SUPPORTED_10000baseT_Full;
-	if (word & FW_PORT_CAP_SPEED_40G)
-		v |= SUPPORTED_40000baseSR4_Full;
-	if (word & FW_PORT_CAP_ANEG)
-		v |= SUPPORTED_Autoneg;
-	init_link_config(&pi->link_cfg, v);
+	v = be32_to_cpu(port_rpl.u.info.lstatus_to_modtype);
+	pi->port_type = FW_PORT_CMD_PTYPE_G(v);
+	pi->mod_type = FW_PORT_MOD_TYPE_NA;
+
+	init_link_config(&pi->link_cfg, be16_to_cpu(port_rpl.u.info.pcap));
 
 	return 0;
 }
@@ -1491,7 +1486,7 @@ int t4vf_handle_fw_rpl(struct adapter *adapter, const __be64 *rpl)
 		 */
 		const struct fw_port_cmd *port_cmd =
 			(const struct fw_port_cmd *)rpl;
-		u32 word;
+		u32 stat, mod;
 		int action, port_id, link_ok, speed, fc, pidx;
 
 		/*
@@ -1509,21 +1504,21 @@ int t4vf_handle_fw_rpl(struct adapter *adapter, const __be64 *rpl)
 		port_id = FW_PORT_CMD_PORTID_G(
 			be32_to_cpu(port_cmd->op_to_portid));
 
-		word = be32_to_cpu(port_cmd->u.info.lstatus_to_modtype);
-		link_ok = (word & FW_PORT_CMD_LSTATUS_F) != 0;
+		stat = be32_to_cpu(port_cmd->u.info.lstatus_to_modtype);
+		link_ok = (stat & FW_PORT_CMD_LSTATUS_F) != 0;
 		speed = 0;
 		fc = 0;
-		if (word & FW_PORT_CMD_RXPAUSE_F)
+		if (stat & FW_PORT_CMD_RXPAUSE_F)
 			fc |= PAUSE_RX;
-		if (word & FW_PORT_CMD_TXPAUSE_F)
+		if (stat & FW_PORT_CMD_TXPAUSE_F)
 			fc |= PAUSE_TX;
-		if (word & FW_PORT_CMD_LSPEED_V(FW_PORT_CAP_SPEED_100M))
+		if (stat & FW_PORT_CMD_LSPEED_V(FW_PORT_CAP_SPEED_100M))
 			speed = 100;
-		else if (word & FW_PORT_CMD_LSPEED_V(FW_PORT_CAP_SPEED_1G))
+		else if (stat & FW_PORT_CMD_LSPEED_V(FW_PORT_CAP_SPEED_1G))
 			speed = 1000;
-		else if (word & FW_PORT_CMD_LSPEED_V(FW_PORT_CAP_SPEED_10G))
+		else if (stat & FW_PORT_CMD_LSPEED_V(FW_PORT_CAP_SPEED_10G))
 			speed = 10000;
-		else if (word & FW_PORT_CMD_LSPEED_V(FW_PORT_CAP_SPEED_40G))
+		else if (stat & FW_PORT_CMD_LSPEED_V(FW_PORT_CAP_SPEED_40G))
 			speed = 40000;
 
 		/*
@@ -1540,12 +1535,21 @@ int t4vf_handle_fw_rpl(struct adapter *adapter, const __be64 *rpl)
 				continue;
 
 			lc = &pi->link_cfg;
+
+			mod = FW_PORT_CMD_MODTYPE_G(stat);
+			if (mod != pi->mod_type) {
+				pi->mod_type = mod;
+				t4vf_os_portmod_changed(adapter, pidx);
+			}
+
 			if (link_ok != lc->link_ok || speed != lc->speed ||
 			    fc != lc->fc) {
 				/* something changed */
 				lc->link_ok = link_ok;
 				lc->speed = speed;
 				lc->fc = fc;
+				lc->supported =
+					be16_to_cpu(port_cmd->u.info.pcap);
 				t4vf_os_link_changed(adapter, pidx, link_ok);
 			}
 		}
diff --git a/drivers/net/ethernet/cirrus/cs89x0.c b/drivers/net/ethernet/cirrus/cs89x0.c
index 9823a0ea7937..d1c025fd9726 100644
--- a/drivers/net/ethernet/cirrus/cs89x0.c
+++ b/drivers/net/ethernet/cirrus/cs89x0.c
@@ -60,6 +60,7 @@
 #include <linux/interrupt.h>
 #include <linux/ioport.h>
 #include <linux/in.h>
+#include <linux/jiffies.h>
 #include <linux/skbuff.h>
 #include <linux/spinlock.h>
 #include <linux/string.h>
@@ -238,13 +239,13 @@ writereg(struct net_device *dev, u16 regno, u16 value)
 static int __init
 wait_eeprom_ready(struct net_device *dev)
 {
-	int timeout = jiffies;
+	unsigned long 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)
-		if (jiffies - timeout >= 40)
+		if (time_after_eq(jiffies, timeout + 40))
 			return -1;
 	return 0;
 }
@@ -485,7 +486,7 @@ control_dc_dc(struct net_device *dev, int on_not_off)
 {
 	struct net_local *lp = netdev_priv(dev);
 	unsigned int selfcontrol;
-	int timenow = jiffies;
+	unsigned long 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.
@@ -499,7 +500,7 @@ control_dc_dc(struct net_device *dev, int on_not_off)
 	writereg(dev, PP_SelfCTL, selfcontrol);
 
 	/* Wait for the DC/DC converter to power up - 500ms */
-	while (jiffies - timenow < HZ)
+	while (time_before(jiffies, timenow + HZ))
 		;
 }
 
@@ -514,7 +515,7 @@ send_test_pkt(struct net_device *dev)
 		0, 0,		/* DSAP=0 & SSAP=0 fields */
 		0xf3, 0		/* Control (Test Req + P bit set) */
 	};
-	long timenow = jiffies;
+	unsigned long timenow = jiffies;
 
 	writereg(dev, PP_LineCTL, readreg(dev, PP_LineCTL) | SERIAL_TX_ON);
 
@@ -525,10 +526,10 @@ send_test_pkt(struct net_device *dev)
 	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)
+	while (time_before(jiffies, timenow + 5))
 		if (readreg(dev, PP_BusST) & READY_FOR_TX_NOW)
 			break;
-	if (jiffies - timenow >= 5)
+	if (time_after_eq(jiffies, timenow + 5))
 		return 0;	/* this shouldn't happen */
 
 	/* Write the contents of the packet */
@@ -536,7 +537,7 @@ send_test_pkt(struct net_device *dev)
 
 	cs89_dbg(1, debug, "Sending test packet ");
 	/* wait a couple of jiffies for packet to be received */
-	for (timenow = jiffies; jiffies - timenow < 3;)
+	for (timenow = jiffies; time_before(jiffies, timenow + 3);)
 		;
 	if ((readreg(dev, PP_TxEvent) & TX_SEND_OK_BITS) == TX_OK) {
 		cs89_dbg(1, cont, "succeeded\n");
@@ -556,7 +557,7 @@ static int
 detect_tp(struct net_device *dev)
 {
 	struct net_local *lp = netdev_priv(dev);
-	int timenow = jiffies;
+	unsigned long timenow = jiffies;
 	int fdx;
 
 	cs89_dbg(1, debug, "%s: Attempting TP\n", dev->name);
@@ -574,7 +575,7 @@ detect_tp(struct net_device *dev)
 	/* Delay for the hardware to work out if the TP cable is present
 	 * - 150ms
 	 */
-	for (timenow = jiffies; jiffies - timenow < 15;)
+	for (timenow = jiffies; time_before(jiffies, timenow + 15);)
 		;
 	if ((readreg(dev, PP_LineST) & LINK_OK) == 0)
 		return DETECTED_NONE;
@@ -618,7 +619,7 @@ detect_tp(struct net_device *dev)
 		if ((lp->auto_neg_cnf & AUTO_NEG_BITS) == AUTO_NEG_ENABLE) {
 			pr_info("%s: negotiating duplex...\n", dev->name);
 			while (readreg(dev, PP_AutoNegST) & AUTO_NEG_BUSY) {
-				if (jiffies - timenow > 4000) {
+				if (time_after(jiffies, timenow + 4000)) {
 					pr_err("**** Full / half duplex auto-negotiation timed out ****\n");
 					break;
 				}
@@ -1271,7 +1272,7 @@ static void __init reset_chip(struct net_device *dev)
 {
 #if !defined(CONFIG_MACH_MX31ADS)
 	struct net_local *lp = netdev_priv(dev);
-	int reset_start_time;
+	unsigned long reset_start_time;
 
 	writereg(dev, PP_SelfCTL, readreg(dev, PP_SelfCTL) | POWER_ON_RESET);
 
@@ -1294,7 +1295,7 @@ static void __init reset_chip(struct net_device *dev)
 	/* Wait until the chip is reset */
 	reset_start_time = jiffies;
 	while ((readreg(dev, PP_SelfST) & INIT_DONE) == 0 &&
-	       jiffies - reset_start_time < 2)
+	       time_before(jiffies, reset_start_time + 2))
 		;
 #endif /* !CONFIG_MACH_MX31ADS */
 }
@@ -1897,7 +1898,6 @@ static int cs89x0_platform_remove(struct platform_device *pdev)
 static struct platform_driver cs89x0_driver = {
 	.driver	= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.remove	= cs89x0_platform_remove,
 };
diff --git a/drivers/net/ethernet/cirrus/ep93xx_eth.c b/drivers/net/ethernet/cirrus/ep93xx_eth.c
index 2be2a99c5ea3..3a12c096ea1c 100644
--- a/drivers/net/ethernet/cirrus/ep93xx_eth.c
+++ b/drivers/net/ethernet/cirrus/ep93xx_eth.c
@@ -881,7 +881,6 @@ static struct platform_driver ep93xx_eth_driver = {
 	.remove		= ep93xx_eth_remove,
 	.driver		= {
 		.name	= "ep93xx-eth",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/ethernet/cisco/enic/enic.h b/drivers/net/ethernet/cisco/enic/enic.h
index 25c4d88853d8..d2a103547c67 100644
--- a/drivers/net/ethernet/cisco/enic/enic.h
+++ b/drivers/net/ethernet/cisco/enic/enic.h
@@ -188,6 +188,7 @@ struct enic {
 	struct enic_rfs_flw_tbl rfs_h;
 	u32 rx_copybreak;
 	u8 rss_key[ENIC_RSS_LEN];
+	struct vnic_gen_stats gen_stats;
 };
 
 static inline struct device *enic_get_dev(struct enic *enic)
@@ -242,6 +243,19 @@ static inline unsigned int enic_msix_notify_intr(struct enic *enic)
 	return enic->rq_count + enic->wq_count + 1;
 }
 
+static inline int enic_dma_map_check(struct enic *enic, dma_addr_t dma_addr)
+{
+	if (unlikely(pci_dma_mapping_error(enic->pdev, dma_addr))) {
+		net_warn_ratelimited("%s: PCI dma mapping failed!\n",
+				     enic->netdev->name);
+		enic->gen_stats.dma_map_error++;
+
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
 void enic_reset_addr_lists(struct enic *enic);
 int enic_sriov_enabled(struct enic *enic);
 int enic_is_valid_vf(struct enic *enic, int vf);
diff --git a/drivers/net/ethernet/cisco/enic/enic_dev.c b/drivers/net/ethernet/cisco/enic/enic_dev.c
index 87ddc44b590e..f8d2a6a34282 100644
--- a/drivers/net/ethernet/cisco/enic/enic_dev.c
+++ b/drivers/net/ethernet/cisco/enic/enic_dev.c
@@ -177,40 +177,6 @@ int enic_dev_intr_coal_timer_info(struct enic *enic)
 	return err;
 }
 
-int enic_vnic_dev_deinit(struct enic *enic)
-{
-	int err;
-
-	spin_lock_bh(&enic->devcmd_lock);
-	err = vnic_dev_deinit(enic->vdev);
-	spin_unlock_bh(&enic->devcmd_lock);
-
-	return err;
-}
-
-int enic_dev_init_prov2(struct enic *enic, struct vic_provinfo *vp)
-{
-	int err;
-
-	spin_lock_bh(&enic->devcmd_lock);
-	err = vnic_dev_init_prov2(enic->vdev,
-		(u8 *)vp, vic_provinfo_size(vp));
-	spin_unlock_bh(&enic->devcmd_lock);
-
-	return err;
-}
-
-int enic_dev_deinit_done(struct enic *enic, int *status)
-{
-	int err;
-
-	spin_lock_bh(&enic->devcmd_lock);
-	err = vnic_dev_deinit_done(enic->vdev, status);
-	spin_unlock_bh(&enic->devcmd_lock);
-
-	return err;
-}
-
 /* rtnl lock is held */
 int enic_vlan_rx_add_vid(struct net_device *netdev, __be16 proto, u16 vid)
 {
@@ -237,28 +203,6 @@ int enic_vlan_rx_kill_vid(struct net_device *netdev, __be16 proto, u16 vid)
 	return err;
 }
 
-int enic_dev_enable2(struct enic *enic, int active)
-{
-	int err;
-
-	spin_lock_bh(&enic->devcmd_lock);
-	err = vnic_dev_enable2(enic->vdev, active);
-	spin_unlock_bh(&enic->devcmd_lock);
-
-	return err;
-}
-
-int enic_dev_enable2_done(struct enic *enic, int *status)
-{
-	int err;
-
-	spin_lock_bh(&enic->devcmd_lock);
-	err = vnic_dev_enable2_done(enic->vdev, status);
-	spin_unlock_bh(&enic->devcmd_lock);
-
-	return err;
-}
-
 int enic_dev_status_to_errno(int devcmd_status)
 {
 	switch (devcmd_status) {
diff --git a/drivers/net/ethernet/cisco/enic/enic_dev.h b/drivers/net/ethernet/cisco/enic/enic_dev.h
index 10bb970b2f35..f5bb058b3f96 100644
--- a/drivers/net/ethernet/cisco/enic/enic_dev.h
+++ b/drivers/net/ethernet/cisco/enic/enic_dev.h
@@ -55,11 +55,6 @@ int enic_dev_set_ig_vlan_rewrite_mode(struct enic *enic);
 int enic_dev_enable(struct enic *enic);
 int enic_dev_disable(struct enic *enic);
 int enic_dev_intr_coal_timer_info(struct enic *enic);
-int enic_vnic_dev_deinit(struct enic *enic);
-int enic_dev_init_prov2(struct enic *enic, struct vic_provinfo *vp);
-int enic_dev_deinit_done(struct enic *enic, int *status);
-int enic_dev_enable2(struct enic *enic, int arg);
-int enic_dev_enable2_done(struct enic *enic, int *status);
 int enic_dev_status_to_errno(int devcmd_status);
 
 #endif /* _ENIC_DEV_H_ */
diff --git a/drivers/net/ethernet/cisco/enic/enic_ethtool.c b/drivers/net/ethernet/cisco/enic/enic_ethtool.c
index eba1eb846d34..0c396c1f55dc 100644
--- a/drivers/net/ethernet/cisco/enic/enic_ethtool.c
+++ b/drivers/net/ethernet/cisco/enic/enic_ethtool.c
@@ -24,6 +24,7 @@
 #include "enic_dev.h"
 #include "enic_clsf.h"
 #include "vnic_rss.h"
+#include "vnic_stats.h"
 
 struct enic_stat {
 	char name[ETH_GSTRING_LEN];
@@ -40,6 +41,11 @@ struct enic_stat {
 	.index = offsetof(struct vnic_rx_stats, stat) / sizeof(u64) \
 }
 
+#define ENIC_GEN_STAT(stat) { \
+	.name = #stat, \
+	.index = offsetof(struct vnic_gen_stats, stat) / sizeof(u64)\
+}
+
 static const struct enic_stat enic_tx_stats[] = {
 	ENIC_TX_STAT(tx_frames_ok),
 	ENIC_TX_STAT(tx_unicast_frames_ok),
@@ -78,8 +84,13 @@ static const struct enic_stat enic_rx_stats[] = {
 	ENIC_RX_STAT(rx_frames_to_max),
 };
 
+static const struct enic_stat enic_gen_stats[] = {
+	ENIC_GEN_STAT(dma_map_error),
+};
+
 static const unsigned int enic_n_tx_stats = ARRAY_SIZE(enic_tx_stats);
 static const unsigned int enic_n_rx_stats = ARRAY_SIZE(enic_rx_stats);
+static const unsigned int enic_n_gen_stats = ARRAY_SIZE(enic_gen_stats);
 
 void enic_intr_coal_set_rx(struct enic *enic, u32 timer)
 {
@@ -146,6 +157,10 @@ static void enic_get_strings(struct net_device *netdev, u32 stringset,
 			memcpy(data, enic_rx_stats[i].name, ETH_GSTRING_LEN);
 			data += ETH_GSTRING_LEN;
 		}
+		for (i = 0; i < enic_n_gen_stats; i++) {
+			memcpy(data, enic_gen_stats[i].name, ETH_GSTRING_LEN);
+			data += ETH_GSTRING_LEN;
+		}
 		break;
 	}
 }
@@ -154,7 +169,7 @@ static int enic_get_sset_count(struct net_device *netdev, int sset)
 {
 	switch (sset) {
 	case ETH_SS_STATS:
-		return enic_n_tx_stats + enic_n_rx_stats;
+		return enic_n_tx_stats + enic_n_rx_stats + enic_n_gen_stats;
 	default:
 		return -EOPNOTSUPP;
 	}
@@ -173,6 +188,8 @@ static void enic_get_ethtool_stats(struct net_device *netdev,
 		*(data++) = ((u64 *)&vstats->tx)[enic_tx_stats[i].index];
 	for (i = 0; i < enic_n_rx_stats; i++)
 		*(data++) = ((u64 *)&vstats->rx)[enic_rx_stats[i].index];
+	for (i = 0; i < enic_n_gen_stats; i++)
+		*(data++) = ((u64 *)&enic->gen_stats)[enic_gen_stats[i].index];
 }
 
 static u32 enic_get_msglevel(struct net_device *netdev)
diff --git a/drivers/net/ethernet/cisco/enic/enic_main.c b/drivers/net/ethernet/cisco/enic/enic_main.c
index 868d0f605d60..142c9b5509ae 100644
--- a/drivers/net/ethernet/cisco/enic/enic_main.c
+++ b/drivers/net/ethernet/cisco/enic/enic_main.c
@@ -351,80 +351,94 @@ static irqreturn_t enic_isr_msix_notify(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
-static inline void enic_queue_wq_skb_cont(struct enic *enic,
-	struct vnic_wq *wq, struct sk_buff *skb,
-	unsigned int len_left, int loopback)
+static int enic_queue_wq_skb_cont(struct enic *enic, struct vnic_wq *wq,
+				  struct sk_buff *skb, unsigned int len_left,
+				  int loopback)
 {
 	const skb_frag_t *frag;
+	dma_addr_t dma_addr;
 
 	/* Queue additional data fragments */
 	for (frag = skb_shinfo(skb)->frags; len_left; frag++) {
 		len_left -= skb_frag_size(frag);
-		enic_queue_wq_desc_cont(wq, skb,
-			skb_frag_dma_map(&enic->pdev->dev,
-					 frag, 0, skb_frag_size(frag),
-					 DMA_TO_DEVICE),
-			skb_frag_size(frag),
-			(len_left == 0),	/* EOP? */
-			loopback);
+		dma_addr = skb_frag_dma_map(&enic->pdev->dev, frag, 0,
+					    skb_frag_size(frag),
+					    DMA_TO_DEVICE);
+		if (unlikely(enic_dma_map_check(enic, dma_addr)))
+			return -ENOMEM;
+		enic_queue_wq_desc_cont(wq, skb, dma_addr, skb_frag_size(frag),
+					(len_left == 0),	/* EOP? */
+					loopback);
 	}
+
+	return 0;
 }
 
-static inline void enic_queue_wq_skb_vlan(struct enic *enic,
-	struct vnic_wq *wq, struct sk_buff *skb,
-	int vlan_tag_insert, unsigned int vlan_tag, int loopback)
+static int enic_queue_wq_skb_vlan(struct enic *enic, struct vnic_wq *wq,
+				  struct sk_buff *skb, int vlan_tag_insert,
+				  unsigned int vlan_tag, int loopback)
 {
 	unsigned int head_len = skb_headlen(skb);
 	unsigned int len_left = skb->len - head_len;
 	int eop = (len_left == 0);
+	dma_addr_t dma_addr;
+	int err = 0;
+
+	dma_addr = pci_map_single(enic->pdev, skb->data, head_len,
+				  PCI_DMA_TODEVICE);
+	if (unlikely(enic_dma_map_check(enic, dma_addr)))
+		return -ENOMEM;
 
 	/* Queue the main skb fragment. The fragments are no larger
 	 * than max MTU(9000)+ETH_HDR_LEN(14) bytes, which is less
 	 * than WQ_ENET_MAX_DESC_LEN length. So only one descriptor
 	 * per fragment is queued.
 	 */
-	enic_queue_wq_desc(wq, skb,
-		pci_map_single(enic->pdev, skb->data,
-			head_len, PCI_DMA_TODEVICE),
-		head_len,
-		vlan_tag_insert, vlan_tag,
-		eop, loopback);
+	enic_queue_wq_desc(wq, skb, dma_addr, head_len,	vlan_tag_insert,
+			   vlan_tag, eop, loopback);
 
 	if (!eop)
-		enic_queue_wq_skb_cont(enic, wq, skb, len_left, loopback);
+		err = enic_queue_wq_skb_cont(enic, wq, skb, len_left, loopback);
+
+	return err;
 }
 
-static inline void enic_queue_wq_skb_csum_l4(struct enic *enic,
-	struct vnic_wq *wq, struct sk_buff *skb,
-	int vlan_tag_insert, unsigned int vlan_tag, int loopback)
+static int enic_queue_wq_skb_csum_l4(struct enic *enic, struct vnic_wq *wq,
+				     struct sk_buff *skb, int vlan_tag_insert,
+				     unsigned int vlan_tag, int loopback)
 {
 	unsigned int head_len = skb_headlen(skb);
 	unsigned int len_left = skb->len - head_len;
 	unsigned int hdr_len = skb_checksum_start_offset(skb);
 	unsigned int csum_offset = hdr_len + skb->csum_offset;
 	int eop = (len_left == 0);
+	dma_addr_t dma_addr;
+	int err = 0;
+
+	dma_addr = pci_map_single(enic->pdev, skb->data, head_len,
+				  PCI_DMA_TODEVICE);
+	if (unlikely(enic_dma_map_check(enic, dma_addr)))
+		return -ENOMEM;
 
 	/* Queue the main skb fragment. The fragments are no larger
 	 * than max MTU(9000)+ETH_HDR_LEN(14) bytes, which is less
 	 * than WQ_ENET_MAX_DESC_LEN length. So only one descriptor
 	 * per fragment is queued.
 	 */
-	enic_queue_wq_desc_csum_l4(wq, skb,
-		pci_map_single(enic->pdev, skb->data,
-			head_len, PCI_DMA_TODEVICE),
-		head_len,
-		csum_offset,
-		hdr_len,
-		vlan_tag_insert, vlan_tag,
-		eop, loopback);
+	enic_queue_wq_desc_csum_l4(wq, skb, dma_addr, head_len,	csum_offset,
+				   hdr_len, vlan_tag_insert, vlan_tag, eop,
+				   loopback);
 
 	if (!eop)
-		enic_queue_wq_skb_cont(enic, wq, skb, len_left, loopback);
+		err = enic_queue_wq_skb_cont(enic, wq, skb, len_left, loopback);
+
+	return err;
 }
 
-static inline void enic_queue_wq_skb_tso(struct enic *enic,
-	struct vnic_wq *wq, struct sk_buff *skb, unsigned int mss,
-	int vlan_tag_insert, unsigned int vlan_tag, int loopback)
+static int enic_queue_wq_skb_tso(struct enic *enic, struct vnic_wq *wq,
+				 struct sk_buff *skb, unsigned int mss,
+				 int vlan_tag_insert, unsigned int vlan_tag,
+				 int loopback)
 {
 	unsigned int frag_len_left = skb_headlen(skb);
 	unsigned int len_left = skb->len - frag_len_left;
@@ -454,20 +468,19 @@ static inline void enic_queue_wq_skb_tso(struct enic *enic,
 	 */
 	while (frag_len_left) {
 		len = min(frag_len_left, (unsigned int)WQ_ENET_MAX_DESC_LEN);
-		dma_addr = pci_map_single(enic->pdev, skb->data + offset,
-				len, PCI_DMA_TODEVICE);
-		enic_queue_wq_desc_tso(wq, skb,
-			dma_addr,
-			len,
-			mss, hdr_len,
-			vlan_tag_insert, vlan_tag,
-			eop && (len == frag_len_left), loopback);
+		dma_addr = pci_map_single(enic->pdev, skb->data + offset, len,
+					  PCI_DMA_TODEVICE);
+		if (unlikely(enic_dma_map_check(enic, dma_addr)))
+			return -ENOMEM;
+		enic_queue_wq_desc_tso(wq, skb, dma_addr, len, mss, hdr_len,
+				       vlan_tag_insert, vlan_tag,
+				       eop && (len == frag_len_left), loopback);
 		frag_len_left -= len;
 		offset += len;
 	}
 
 	if (eop)
-		return;
+		return 0;
 
 	/* Queue WQ_ENET_MAX_DESC_LEN length descriptors
 	 * for additional data fragments
@@ -483,16 +496,18 @@ static inline void enic_queue_wq_skb_tso(struct enic *enic,
 			dma_addr = skb_frag_dma_map(&enic->pdev->dev, frag,
 						    offset, len,
 						    DMA_TO_DEVICE);
-			enic_queue_wq_desc_cont(wq, skb,
-				dma_addr,
-				len,
-				(len_left == 0) &&
-				(len == frag_len_left),		/* EOP? */
-				loopback);
+			if (unlikely(enic_dma_map_check(enic, dma_addr)))
+				return -ENOMEM;
+			enic_queue_wq_desc_cont(wq, skb, dma_addr, len,
+						(len_left == 0) &&
+						 (len == frag_len_left),/*EOP*/
+						loopback);
 			frag_len_left -= len;
 			offset += len;
 		}
 	}
+
+	return 0;
 }
 
 static inline void enic_queue_wq_skb(struct enic *enic,
@@ -502,6 +517,7 @@ static inline void enic_queue_wq_skb(struct enic *enic,
 	unsigned int vlan_tag = 0;
 	int vlan_tag_insert = 0;
 	int loopback = 0;
+	int err;
 
 	if (vlan_tx_tag_present(skb)) {
 		/* VLAN tag from trunking driver */
@@ -513,14 +529,30 @@ static inline void enic_queue_wq_skb(struct enic *enic,
 	}
 
 	if (mss)
-		enic_queue_wq_skb_tso(enic, wq, skb, mss,
-			vlan_tag_insert, vlan_tag, loopback);
+		err = enic_queue_wq_skb_tso(enic, wq, skb, mss,
+					    vlan_tag_insert, vlan_tag,
+					    loopback);
 	else if	(skb->ip_summed == CHECKSUM_PARTIAL)
-		enic_queue_wq_skb_csum_l4(enic, wq, skb,
-			vlan_tag_insert, vlan_tag, loopback);
+		err = enic_queue_wq_skb_csum_l4(enic, wq, skb, vlan_tag_insert,
+						vlan_tag, loopback);
 	else
-		enic_queue_wq_skb_vlan(enic, wq, skb,
-			vlan_tag_insert, vlan_tag, loopback);
+		err = enic_queue_wq_skb_vlan(enic, wq, skb, vlan_tag_insert,
+					     vlan_tag, loopback);
+	if (unlikely(err)) {
+		struct vnic_wq_buf *buf;
+
+		buf = wq->to_use->prev;
+		/* while not EOP of previous pkt && queue not empty.
+		 * For all non EOP bufs, os_buf is NULL.
+		 */
+		while (!buf->os_buf && (buf->next != wq->to_clean)) {
+			enic_free_wq_buf(wq, buf);
+			wq->ring.desc_avail++;
+			buf = buf->prev;
+		}
+		wq->to_use = buf->next;
+		dev_kfree_skb(skb);
+	}
 }
 
 /* netif_tx_lock held, process context with BHs disabled, or BH */
@@ -950,8 +982,12 @@ static int enic_rq_alloc_buf(struct vnic_rq *rq)
 	if (!skb)
 		return -ENOMEM;
 
-	dma_addr = pci_map_single(enic->pdev, skb->data,
-		len, PCI_DMA_FROMDEVICE);
+	dma_addr = pci_map_single(enic->pdev, skb->data, len,
+				  PCI_DMA_FROMDEVICE);
+	if (unlikely(enic_dma_map_check(enic, dma_addr))) {
+		dev_kfree_skb(skb);
+		return -ENOMEM;
+	}
 
 	enic_queue_rq_desc(rq, skb, os_buf_index,
 		dma_addr, len);
@@ -1060,10 +1096,14 @@ static void enic_rq_indicate_buf(struct vnic_rq *rq,
 				     PKT_HASH_TYPE_L4 : PKT_HASH_TYPE_L3);
 		}
 
-		if ((netdev->features & NETIF_F_RXCSUM) && !csum_not_calc) {
-			skb->csum = htons(checksum);
-			skb->ip_summed = CHECKSUM_COMPLETE;
-		}
+		/* Hardware does not provide whole packet checksum. It only
+		 * provides pseudo checksum. Since hw validates the packet
+		 * checksum but not provide us the checksum value. use
+		 * CHECSUM_UNNECESSARY.
+		 */
+		if ((netdev->features & NETIF_F_RXCSUM) && tcp_udp_csum_ok &&
+		    ipv4_csum_ok)
+			skb->ip_summed = CHECKSUM_UNNECESSARY;
 
 		if (vlan_stripped)
 			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_tci);
diff --git a/drivers/net/ethernet/cisco/enic/vnic_stats.h b/drivers/net/ethernet/cisco/enic/vnic_stats.h
index 77750ec93954..74c81ed6fdab 100644
--- a/drivers/net/ethernet/cisco/enic/vnic_stats.h
+++ b/drivers/net/ethernet/cisco/enic/vnic_stats.h
@@ -62,6 +62,11 @@ struct vnic_rx_stats {
 	u64 rsvd[16];
 };
 
+/* Generic statistics */
+struct vnic_gen_stats {
+	u64 dma_map_error;
+};
+
 struct vnic_stats {
 	struct vnic_tx_stats tx;
 	struct vnic_rx_stats rx;
diff --git a/drivers/net/ethernet/cisco/enic/vnic_wq.c b/drivers/net/ethernet/cisco/enic/vnic_wq.c
index 3e6b8d54dafc..b5a1c937fad2 100644
--- a/drivers/net/ethernet/cisco/enic/vnic_wq.c
+++ b/drivers/net/ethernet/cisco/enic/vnic_wq.c
@@ -47,11 +47,14 @@ static int vnic_wq_alloc_bufs(struct vnic_wq *wq)
 				wq->ring.desc_size * buf->index;
 			if (buf->index + 1 == count) {
 				buf->next = wq->bufs[0];
+				buf->next->prev = buf;
 				break;
 			} else if (j + 1 == VNIC_WQ_BUF_BLK_ENTRIES(count)) {
 				buf->next = wq->bufs[i + 1];
+				buf->next->prev = buf;
 			} else {
 				buf->next = buf + 1;
+				buf->next->prev = buf;
 				buf++;
 			}
 		}
diff --git a/drivers/net/ethernet/cisco/enic/vnic_wq.h b/drivers/net/ethernet/cisco/enic/vnic_wq.h
index 816f1ad6072f..296154351823 100644
--- a/drivers/net/ethernet/cisco/enic/vnic_wq.h
+++ b/drivers/net/ethernet/cisco/enic/vnic_wq.h
@@ -62,6 +62,7 @@ struct vnic_wq_buf {
 	uint8_t cq_entry; /* Gets completion event from hw */
 	uint8_t desc_skip_cnt; /* Num descs to occupy */
 	uint8_t compressed_send; /* Both hdr and payload in one desc */
+	struct vnic_wq_buf *prev;
 };
 
 /* Break the vnic_wq_buf allocations into blocks of 32/64 entries */
diff --git a/drivers/net/ethernet/davicom/Kconfig b/drivers/net/ethernet/davicom/Kconfig
index 316c5e5a92ad..7ec2d74f94d3 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 || COLDFIRE
+	depends on ARM || BLACKFIN || MIPS || COLDFIRE || NIOS2
 	select CRC32
 	select MII
 	---help---
diff --git a/drivers/net/ethernet/davicom/dm9000.c b/drivers/net/ethernet/davicom/dm9000.c
index f3ba840cbf7b..ef0bb58750e6 100644
--- a/drivers/net/ethernet/davicom/dm9000.c
+++ b/drivers/net/ethernet/davicom/dm9000.c
@@ -1749,7 +1749,6 @@ MODULE_DEVICE_TABLE(of, dm9000_of_matches);
 static struct platform_driver dm9000_driver = {
 	.driver	= {
 		.name    = "dm9000",
-		.owner	 = THIS_MODULE,
 		.pm	 = &dm9000_drv_pm_ops,
 		.of_match_table = of_match_ptr(dm9000_of_matches),
 	},
diff --git a/drivers/net/ethernet/emulex/benet/be_main.c b/drivers/net/ethernet/emulex/benet/be_main.c
index 2aacd4731051..41a0a5498da7 100644
--- a/drivers/net/ethernet/emulex/benet/be_main.c
+++ b/drivers/net/ethernet/emulex/benet/be_main.c
@@ -3138,6 +3138,7 @@ static void be_disable_vxlan_offloads(struct be_adapter *adapter)
 
 	netdev->hw_enc_features = 0;
 	netdev->hw_features &= ~(NETIF_F_GSO_UDP_TUNNEL);
+	netdev->features &= ~(NETIF_F_GSO_UDP_TUNNEL);
 }
 #endif
 
@@ -4429,6 +4430,7 @@ static void be_add_vxlan_port(struct net_device *netdev, sa_family_t sa_family,
 				   NETIF_F_TSO | NETIF_F_TSO6 |
 				   NETIF_F_GSO_UDP_TUNNEL;
 	netdev->hw_features |= NETIF_F_GSO_UDP_TUNNEL;
+	netdev->features |= NETIF_F_GSO_UDP_TUNNEL;
 
 	dev_info(dev, "Enabled VxLAN offloads for UDP port %d\n",
 		 be16_to_cpu(port));
@@ -4457,9 +4459,11 @@ done:
 	adapter->vxlan_port_count--;
 }
 
-static bool be_gso_check(struct sk_buff *skb, struct net_device *dev)
+static netdev_features_t be_features_check(struct sk_buff *skb,
+					   struct net_device *dev,
+					   netdev_features_t features)
 {
-	return vxlan_gso_check(skb);
+	return vxlan_features_check(skb, features);
 }
 #endif
 
@@ -4490,7 +4494,7 @@ static const struct net_device_ops be_netdev_ops = {
 #ifdef CONFIG_BE2NET_VXLAN
 	.ndo_add_vxlan_port	= be_add_vxlan_port,
 	.ndo_del_vxlan_port	= be_del_vxlan_port,
-	.ndo_gso_check		= be_gso_check,
+	.ndo_features_check	= be_features_check,
 #endif
 };
 
diff --git a/drivers/net/ethernet/ethoc.c b/drivers/net/ethernet/ethoc.c
index 0bc6c102f3ac..f88cfaa359e7 100644
--- a/drivers/net/ethernet/ethoc.c
+++ b/drivers/net/ethernet/ethoc.c
@@ -1312,7 +1312,6 @@ static struct platform_driver ethoc_driver = {
 	.resume  = ethoc_resume,
 	.driver  = {
 		.name = "ethoc",
-		.owner = THIS_MODULE,
 		.of_match_table = ethoc_match,
 	},
 };
diff --git a/drivers/net/ethernet/faraday/ftgmac100.c b/drivers/net/ethernet/faraday/ftgmac100.c
index c77fa4a69844..6d0c5d5eea6d 100644
--- a/drivers/net/ethernet/faraday/ftgmac100.c
+++ b/drivers/net/ethernet/faraday/ftgmac100.c
@@ -1335,7 +1335,6 @@ static struct platform_driver ftgmac100_driver = {
 	.remove		= __exit_p(ftgmac100_remove),
 	.driver		= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/ethernet/faraday/ftmac100.c b/drivers/net/ethernet/faraday/ftmac100.c
index 4ff1adc6bfca..dce5f7b7f772 100644
--- a/drivers/net/ethernet/faraday/ftmac100.c
+++ b/drivers/net/ethernet/faraday/ftmac100.c
@@ -1177,7 +1177,6 @@ static struct platform_driver ftmac100_driver = {
 	.remove		= __exit_p(ftmac100_remove),
 	.driver		= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/ethernet/freescale/fec.h b/drivers/net/ethernet/freescale/fec.h
index 469691ad4a1e..d77a96fdf1dd 100644
--- a/drivers/net/ethernet/freescale/fec.h
+++ b/drivers/net/ethernet/freescale/fec.h
@@ -16,6 +16,7 @@
 #include <linux/clocksource.h>
 #include <linux/net_tstamp.h>
 #include <linux/ptp_clock_kernel.h>
+#include <linux/timecounter.h>
 
 #if defined(CONFIG_M523x) || defined(CONFIG_M527x) || defined(CONFIG_M528x) || \
     defined(CONFIG_M520x) || defined(CONFIG_M532x) || \
@@ -356,6 +357,7 @@ struct bufdesc_ex {
 #define FEC_ENET_RXB    ((uint)0x01000000)      /* A buffer was received */
 #define FEC_ENET_MII    ((uint)0x00800000)      /* MII interrupt */
 #define FEC_ENET_EBERR  ((uint)0x00400000)      /* SDMA bus error */
+#define FEC_ENET_WAKEUP	((uint)0x00020000)	/* Wakeup request */
 #define FEC_ENET_TXF	(FEC_ENET_TXF_0 | FEC_ENET_TXF_1 | FEC_ENET_TXF_2)
 #define FEC_ENET_RXF	(FEC_ENET_RXF_0 | FEC_ENET_RXF_1 | FEC_ENET_RXF_2)
 #define FEC_ENET_TS_AVAIL       ((uint)0x00010000)
@@ -511,6 +513,7 @@ struct fec_enet_private {
 	int	irq[FEC_IRQ_NUM];
 	bool	bufdesc_ex;
 	int	pause_flag;
+	int	wol_flag;
 	u32	quirks;
 
 	struct	napi_struct napi;
diff --git a/drivers/net/ethernet/freescale/fec_main.c b/drivers/net/ethernet/freescale/fec_main.c
index 8c5b15ee5ed7..49cd358c30fa 100644
--- a/drivers/net/ethernet/freescale/fec_main.c
+++ b/drivers/net/ethernet/freescale/fec_main.c
@@ -187,6 +187,9 @@ MODULE_PARM_DESC(macaddr, "FEC Ethernet MAC address");
 #define FEC_MMFR_RA(v)		((v & 0x1f) << 18)
 #define FEC_MMFR_TA		(2 << 16)
 #define FEC_MMFR_DATA(v)	(v & 0xffff)
+/* FEC ECR bits definition */
+#define FEC_ECR_MAGICEN		(1 << 2)
+#define FEC_ECR_SLEEP		(1 << 3)
 
 #define FEC_MII_TIMEOUT		30000 /* us */
 
@@ -195,6 +198,9 @@ MODULE_PARM_DESC(macaddr, "FEC Ethernet MAC address");
 
 #define FEC_PAUSE_FLAG_AUTONEG	0x1
 #define FEC_PAUSE_FLAG_ENABLE	0x2
+#define FEC_WOL_HAS_MAGIC_PACKET	(0x1 << 0)
+#define FEC_WOL_FLAG_ENABLE		(0x1 << 1)
+#define FEC_WOL_FLAG_SLEEP_ON		(0x1 << 2)
 
 #define COPYBREAK_DEFAULT	256
 
@@ -1089,7 +1095,9 @@ static void
 fec_stop(struct net_device *ndev)
 {
 	struct fec_enet_private *fep = netdev_priv(ndev);
+	struct fec_platform_data *pdata = fep->pdev->dev.platform_data;
 	u32 rmii_mode = readl(fep->hwp + FEC_R_CNTRL) & (1 << 8);
+	u32 val;
 
 	/* We cannot expect a graceful transmit stop without link !!! */
 	if (fep->link) {
@@ -1103,17 +1111,28 @@ fec_stop(struct net_device *ndev)
 	 * For i.MX6SX SOC, enet use AXI bus, we use disable MAC
 	 * instead of reset MAC itself.
 	 */
-	if (fep->quirks & FEC_QUIRK_HAS_AVB) {
-		writel(0, fep->hwp + FEC_ECNTRL);
+	if (!(fep->wol_flag & FEC_WOL_FLAG_SLEEP_ON)) {
+		if (fep->quirks & FEC_QUIRK_HAS_AVB) {
+			writel(0, fep->hwp + FEC_ECNTRL);
+		} else {
+			writel(1, fep->hwp + FEC_ECNTRL);
+			udelay(10);
+		}
+		writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK);
 	} else {
-		writel(1, fep->hwp + FEC_ECNTRL);
-		udelay(10);
+		writel(FEC_DEFAULT_IMASK | FEC_ENET_WAKEUP, fep->hwp + FEC_IMASK);
+		val = readl(fep->hwp + FEC_ECNTRL);
+		val |= (FEC_ECR_MAGICEN | FEC_ECR_SLEEP);
+		writel(val, fep->hwp + FEC_ECNTRL);
+
+		if (pdata && pdata->sleep_mode_enable)
+			pdata->sleep_mode_enable(true);
 	}
 	writel(fep->phy_speed, fep->hwp + FEC_MII_SPEED);
-	writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK);
 
 	/* We have to keep ENET enabled to have MII interrupt stay working */
-	if (fep->quirks & FEC_QUIRK_ENET_MAC) {
+	if (fep->quirks & FEC_QUIRK_ENET_MAC &&
+		!(fep->wol_flag & FEC_WOL_FLAG_SLEEP_ON)) {
 		writel(2, fep->hwp + FEC_ECNTRL);
 		writel(rmii_mode, fep->hwp + FEC_R_CNTRL);
 	}
@@ -1558,20 +1577,21 @@ fec_enet_interrupt(int irq, void *dev_id)
 {
 	struct net_device *ndev = dev_id;
 	struct fec_enet_private *fep = netdev_priv(ndev);
-	const unsigned napi_mask = FEC_ENET_RXF | FEC_ENET_TXF;
 	uint int_events;
 	irqreturn_t ret = IRQ_NONE;
 
 	int_events = readl(fep->hwp + FEC_IEVENT);
-	writel(int_events & ~napi_mask, fep->hwp + FEC_IEVENT);
+	writel(int_events, fep->hwp + FEC_IEVENT);
 	fec_enet_collect_events(fep, int_events);
 
-	if (int_events & napi_mask) {
+	if (fep->work_tx || fep->work_rx) {
 		ret = IRQ_HANDLED;
 
-		/* Disable the NAPI interrupts */
-		writel(FEC_ENET_MII, fep->hwp + FEC_IMASK);
-		napi_schedule(&fep->napi);
+		if (napi_schedule_prep(&fep->napi)) {
+			/* Disable the NAPI interrupts */
+			writel(FEC_ENET_MII, fep->hwp + FEC_IMASK);
+			__napi_schedule(&fep->napi);
+		}
 	}
 
 	if (int_events & FEC_ENET_MII) {
@@ -1591,12 +1611,6 @@ static int fec_enet_rx_napi(struct napi_struct *napi, int budget)
 	struct fec_enet_private *fep = netdev_priv(ndev);
 	int pkts;
 
-	/*
-	 * Clear any pending transmit or receive interrupts before
-	 * processing the rings to avoid racing with the hardware.
-	 */
-	writel(FEC_ENET_RXF | FEC_ENET_TXF, fep->hwp + FEC_IEVENT);
-
 	pkts = fec_enet_rx(ndev, budget);
 
 	fec_enet_tx(ndev);
@@ -2432,6 +2446,44 @@ static int fec_enet_set_tunable(struct net_device *netdev,
 	return ret;
 }
 
+static void
+fec_enet_get_wol(struct net_device *ndev, struct ethtool_wolinfo *wol)
+{
+	struct fec_enet_private *fep = netdev_priv(ndev);
+
+	if (fep->wol_flag & FEC_WOL_HAS_MAGIC_PACKET) {
+		wol->supported = WAKE_MAGIC;
+		wol->wolopts = fep->wol_flag & FEC_WOL_FLAG_ENABLE ? WAKE_MAGIC : 0;
+	} else {
+		wol->supported = wol->wolopts = 0;
+	}
+}
+
+static int
+fec_enet_set_wol(struct net_device *ndev, struct ethtool_wolinfo *wol)
+{
+	struct fec_enet_private *fep = netdev_priv(ndev);
+
+	if (!(fep->wol_flag & FEC_WOL_HAS_MAGIC_PACKET))
+		return -EINVAL;
+
+	if (wol->wolopts & ~WAKE_MAGIC)
+		return -EINVAL;
+
+	device_set_wakeup_enable(&ndev->dev, wol->wolopts & WAKE_MAGIC);
+	if (device_may_wakeup(&ndev->dev)) {
+		fep->wol_flag |= FEC_WOL_FLAG_ENABLE;
+		if (fep->irq[0] > 0)
+			enable_irq_wake(fep->irq[0]);
+	} else {
+		fep->wol_flag &= (~FEC_WOL_FLAG_ENABLE);
+		if (fep->irq[0] > 0)
+			disable_irq_wake(fep->irq[0]);
+	}
+
+	return 0;
+}
+
 static const struct ethtool_ops fec_enet_ethtool_ops = {
 	.get_settings		= fec_enet_get_settings,
 	.set_settings		= fec_enet_set_settings,
@@ -2450,6 +2502,8 @@ static const struct ethtool_ops fec_enet_ethtool_ops = {
 	.get_ts_info		= fec_enet_get_ts_info,
 	.get_tunable		= fec_enet_get_tunable,
 	.set_tunable		= fec_enet_set_tunable,
+	.get_wol		= fec_enet_get_wol,
+	.set_wol		= fec_enet_set_wol,
 };
 
 static int fec_enet_ioctl(struct net_device *ndev, struct ifreq *rq, int cmd)
@@ -2710,6 +2764,9 @@ fec_enet_open(struct net_device *ndev)
 	phy_start(fep->phy_dev);
 	netif_tx_start_all_queues(ndev);
 
+	device_set_wakeup_enable(&ndev->dev, fep->wol_flag &
+				 FEC_WOL_FLAG_ENABLE);
+
 	return 0;
 
 err_enet_mii_probe:
@@ -3158,6 +3215,9 @@ fec_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, ndev);
 
+	if (of_get_property(np, "fsl,magic-packet", NULL))
+		fep->wol_flag |= FEC_WOL_HAS_MAGIC_PACKET;
+
 	phy_node = of_parse_phandle(np, "phy-handle", 0);
 	if (!phy_node && of_phy_is_fixed_link(np)) {
 		ret = of_phy_register_fixed_link(np);
@@ -3252,6 +3312,8 @@ fec_probe(struct platform_device *pdev)
 				       0, pdev->name, ndev);
 		if (ret)
 			goto failed_irq;
+
+		fep->irq[i] = irq;
 	}
 
 	init_completion(&fep->mdio_done);
@@ -3268,6 +3330,9 @@ fec_probe(struct platform_device *pdev)
 	if (ret)
 		goto failed_register;
 
+	device_init_wakeup(&ndev->dev, fep->wol_flag &
+			   FEC_WOL_HAS_MAGIC_PACKET);
+
 	if (fep->bufdesc_ex && fep->ptp_clock)
 		netdev_info(ndev, "registered PHC device %d\n", fep->dev_id);
 
@@ -3321,6 +3386,8 @@ static int __maybe_unused fec_suspend(struct device *dev)
 
 	rtnl_lock();
 	if (netif_running(ndev)) {
+		if (fep->wol_flag & FEC_WOL_FLAG_ENABLE)
+			fep->wol_flag |= FEC_WOL_FLAG_SLEEP_ON;
 		phy_stop(fep->phy_dev);
 		napi_disable(&fep->napi);
 		netif_tx_lock_bh(ndev);
@@ -3328,11 +3395,12 @@ static int __maybe_unused fec_suspend(struct device *dev)
 		netif_tx_unlock_bh(ndev);
 		fec_stop(ndev);
 		fec_enet_clk_enable(ndev, false);
-		pinctrl_pm_select_sleep_state(&fep->pdev->dev);
+		if (!(fep->wol_flag & FEC_WOL_FLAG_ENABLE))
+			pinctrl_pm_select_sleep_state(&fep->pdev->dev);
 	}
 	rtnl_unlock();
 
-	if (fep->reg_phy)
+	if (fep->reg_phy && !(fep->wol_flag & FEC_WOL_FLAG_ENABLE))
 		regulator_disable(fep->reg_phy);
 
 	/* SOC supply clock to phy, when clock is disabled, phy link down
@@ -3348,9 +3416,11 @@ static int __maybe_unused fec_resume(struct device *dev)
 {
 	struct net_device *ndev = dev_get_drvdata(dev);
 	struct fec_enet_private *fep = netdev_priv(ndev);
+	struct fec_platform_data *pdata = fep->pdev->dev.platform_data;
 	int ret;
+	int val;
 
-	if (fep->reg_phy) {
+	if (fep->reg_phy && !(fep->wol_flag & FEC_WOL_FLAG_ENABLE)) {
 		ret = regulator_enable(fep->reg_phy);
 		if (ret)
 			return ret;
@@ -3358,12 +3428,21 @@ static int __maybe_unused fec_resume(struct device *dev)
 
 	rtnl_lock();
 	if (netif_running(ndev)) {
-		pinctrl_pm_select_default_state(&fep->pdev->dev);
 		ret = fec_enet_clk_enable(ndev, true);
 		if (ret) {
 			rtnl_unlock();
 			goto failed_clk;
 		}
+		if (fep->wol_flag & FEC_WOL_FLAG_ENABLE) {
+			if (pdata && pdata->sleep_mode_enable)
+				pdata->sleep_mode_enable(false);
+			val = readl(fep->hwp + FEC_ECNTRL);
+			val &= ~(FEC_ECR_MAGICEN | FEC_ECR_SLEEP);
+			writel(val, fep->hwp + FEC_ECNTRL);
+			fep->wol_flag &= ~FEC_WOL_FLAG_SLEEP_ON;
+		} else {
+			pinctrl_pm_select_default_state(&fep->pdev->dev);
+		}
 		fec_restart(ndev);
 		netif_tx_lock_bh(ndev);
 		netif_device_attach(ndev);
@@ -3386,7 +3465,6 @@ static SIMPLE_DEV_PM_OPS(fec_pm_ops, fec_suspend, fec_resume);
 static struct platform_driver fec_driver = {
 	.driver	= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 		.pm	= &fec_pm_ops,
 		.of_match_table = fec_dt_ids,
 	},
diff --git a/drivers/net/ethernet/freescale/fec_mpc52xx.c b/drivers/net/ethernet/freescale/fec_mpc52xx.c
index ff55fbb20a75..f495796248db 100644
--- a/drivers/net/ethernet/freescale/fec_mpc52xx.c
+++ b/drivers/net/ethernet/freescale/fec_mpc52xx.c
@@ -1069,7 +1069,6 @@ MODULE_DEVICE_TABLE(of, mpc52xx_fec_match);
 static struct platform_driver mpc52xx_fec_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = mpc52xx_fec_match,
 	},
 	.probe		= mpc52xx_fec_probe,
diff --git a/drivers/net/ethernet/freescale/fec_ptp.c b/drivers/net/ethernet/freescale/fec_ptp.c
index 992c8c3db553..1f9cf2345266 100644
--- a/drivers/net/ethernet/freescale/fec_ptp.c
+++ b/drivers/net/ethernet/freescale/fec_ptp.c
@@ -374,23 +374,9 @@ static int fec_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 	struct fec_enet_private *fep =
 	    container_of(ptp, struct fec_enet_private, ptp_caps);
 	unsigned long flags;
-	u64 now;
-	u32 counter;
 
 	spin_lock_irqsave(&fep->tmreg_lock, flags);
-
-	now = timecounter_read(&fep->tc);
-	now += delta;
-
-	/* Get the timer value based on adjusted timestamp.
-	 * Update the counter with the masked value.
-	 */
-	counter = now & fep->cc.mask;
-	writel(counter, fep->hwp + FEC_ATIME);
-
-	/* reset the timecounter */
-	timecounter_init(&fep->tc, &fep->cc, now);
-
+	timecounter_adjtime(&fep->tc, delta);
 	spin_unlock_irqrestore(&fep->tmreg_lock, flags);
 
 	return 0;
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 c92c3b7876ca..9e2bcb807923 100644
--- a/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c
+++ b/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c
@@ -1087,7 +1087,6 @@ MODULE_DEVICE_TABLE(of, fs_enet_match);
 
 static struct platform_driver fs_enet_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "fs_enet",
 		.of_match_table = fs_enet_match,
 	},
diff --git a/drivers/net/ethernet/freescale/fs_enet/mii-bitbang.c b/drivers/net/ethernet/freescale/fs_enet/mii-bitbang.c
index 3d3fde66c2cc..1d5617d2d8bd 100644
--- a/drivers/net/ethernet/freescale/fs_enet/mii-bitbang.c
+++ b/drivers/net/ethernet/freescale/fs_enet/mii-bitbang.c
@@ -224,7 +224,6 @@ MODULE_DEVICE_TABLE(of, fs_enet_mdio_bb_match);
 static struct platform_driver fs_enet_bb_mdio_driver = {
 	.driver = {
 		.name = "fsl-bb-mdio",
-		.owner = THIS_MODULE,
 		.of_match_table = fs_enet_mdio_bb_match,
 	},
 	.probe = fs_enet_mdio_probe,
diff --git a/drivers/net/ethernet/freescale/fs_enet/mii-fec.c b/drivers/net/ethernet/freescale/fs_enet/mii-fec.c
index ebf5d6429a8d..1648e3582500 100644
--- a/drivers/net/ethernet/freescale/fs_enet/mii-fec.c
+++ b/drivers/net/ethernet/freescale/fs_enet/mii-fec.c
@@ -225,7 +225,6 @@ MODULE_DEVICE_TABLE(of, fs_enet_mdio_fec_match);
 static struct platform_driver fs_enet_fec_mdio_driver = {
 	.driver = {
 		.name = "fsl-fec-mdio",
-		.owner = THIS_MODULE,
 		.of_match_table = fs_enet_mdio_fec_match,
 	},
 	.probe = fs_enet_mdio_probe,
diff --git a/drivers/net/ethernet/freescale/fsl_pq_mdio.c b/drivers/net/ethernet/freescale/fsl_pq_mdio.c
index 964c6bf37710..d1a91e344e6b 100644
--- a/drivers/net/ethernet/freescale/fsl_pq_mdio.c
+++ b/drivers/net/ethernet/freescale/fsl_pq_mdio.c
@@ -488,7 +488,6 @@ static int fsl_pq_mdio_remove(struct platform_device *pdev)
 static struct platform_driver fsl_pq_mdio_driver = {
 	.driver = {
 		.name = "fsl-pq_mdio",
-		.owner = THIS_MODULE,
 		.of_match_table = fsl_pq_mdio_match,
 	},
 	.probe = fsl_pq_mdio_probe,
diff --git a/drivers/net/ethernet/freescale/gianfar.c b/drivers/net/ethernet/freescale/gianfar.c
index 7402ab12e46b..e54b1e39f9b4 100644
--- a/drivers/net/ethernet/freescale/gianfar.c
+++ b/drivers/net/ethernet/freescale/gianfar.c
@@ -116,7 +116,8 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev);
 static void gfar_reset_task(struct work_struct *work);
 static void gfar_timeout(struct net_device *dev);
 static int gfar_close(struct net_device *dev);
-struct sk_buff *gfar_new_skb(struct net_device *dev, dma_addr_t *bufaddr);
+static struct sk_buff *gfar_new_skb(struct net_device *dev,
+				    dma_addr_t *bufaddr);
 static int gfar_set_mac_address(struct net_device *dev);
 static int gfar_change_mtu(struct net_device *dev, int new_mtu);
 static irqreturn_t gfar_error(int irq, void *dev_id);
@@ -176,7 +177,7 @@ static int gfar_init_bds(struct net_device *ndev)
 	struct gfar_priv_rx_q *rx_queue = NULL;
 	struct txbd8 *txbdp;
 	struct rxbd8 *rxbdp;
-	u32 *rfbptr;
+	u32 __iomem *rfbptr;
 	int i, j;
 	dma_addr_t bufaddr;
 
@@ -554,7 +555,7 @@ static void gfar_ints_enable(struct gfar_private *priv)
 	}
 }
 
-void lock_tx_qs(struct gfar_private *priv)
+static void lock_tx_qs(struct gfar_private *priv)
 {
 	int i;
 
@@ -562,7 +563,7 @@ void lock_tx_qs(struct gfar_private *priv)
 		spin_lock(&priv->tx_queue[i]->txlock);
 }
 
-void unlock_tx_qs(struct gfar_private *priv)
+static void unlock_tx_qs(struct gfar_private *priv)
 {
 	int i;
 
@@ -2671,7 +2672,7 @@ static struct sk_buff *gfar_alloc_skb(struct net_device *dev)
 	return skb;
 }
 
-struct sk_buff *gfar_new_skb(struct net_device *dev, dma_addr_t *bufaddr)
+static struct sk_buff *gfar_new_skb(struct net_device *dev, dma_addr_t *bufaddr)
 {
 	struct gfar_private *priv = netdev_priv(dev);
 	struct sk_buff *skb;
@@ -3561,7 +3562,6 @@ MODULE_DEVICE_TABLE(of, gfar_match);
 static struct platform_driver gfar_driver = {
 	.driver = {
 		.name = "fsl-gianfar",
-		.owner = THIS_MODULE,
 		.pm = GFAR_PM_OPS,
 		.of_match_table = gfar_match,
 	},
diff --git a/drivers/net/ethernet/freescale/gianfar.h b/drivers/net/ethernet/freescale/gianfar.h
index b581b8823a2a..9e1802400c23 100644
--- a/drivers/net/ethernet/freescale/gianfar.h
+++ b/drivers/net/ethernet/freescale/gianfar.h
@@ -1039,7 +1039,7 @@ struct gfar_priv_rx_q {
 	/* RX Coalescing values */
 	unsigned char rxcoalescing;
 	unsigned long rxic;
-	u32 *rfbptr;
+	u32 __iomem *rfbptr;
 };
 
 enum gfar_irqinfo_id {
diff --git a/drivers/net/ethernet/freescale/gianfar_ptp.c b/drivers/net/ethernet/freescale/gianfar_ptp.c
index bb568006f37d..16826341a4c9 100644
--- a/drivers/net/ethernet/freescale/gianfar_ptp.c
+++ b/drivers/net/ethernet/freescale/gianfar_ptp.c
@@ -563,7 +563,6 @@ static struct platform_driver gianfar_ptp_driver = {
 	.driver = {
 		.name		= "gianfar_ptp",
 		.of_match_table	= match_table,
-		.owner		= THIS_MODULE,
 	},
 	.probe       = gianfar_ptp_probe,
 	.remove      = gianfar_ptp_remove,
diff --git a/drivers/net/ethernet/freescale/ucc_geth.c b/drivers/net/ethernet/freescale/ucc_geth.c
index 3cf0478b3728..357e8b576905 100644
--- a/drivers/net/ethernet/freescale/ucc_geth.c
+++ b/drivers/net/ethernet/freescale/ucc_geth.c
@@ -3943,7 +3943,6 @@ MODULE_DEVICE_TABLE(of, ucc_geth_match);
 static struct platform_driver ucc_geth_driver = {
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = ucc_geth_match,
 	},
 	.probe		= ucc_geth_probe,
diff --git a/drivers/net/ethernet/freescale/xgmac_mdio.c b/drivers/net/ethernet/freescale/xgmac_mdio.c
index 6e7db66069aa..a35244586a63 100644
--- a/drivers/net/ethernet/freescale/xgmac_mdio.c
+++ b/drivers/net/ethernet/freescale/xgmac_mdio.c
@@ -94,13 +94,6 @@ static int xgmac_mdio_write(struct mii_bus *bus, int phy_id, int regnum, u16 val
 	uint16_t dev_addr = regnum >> 16;
 	int ret;
 
-	/* Setup the MII Mgmt clock speed */
-	out_be32(&regs->mdio_stat, MDIO_STAT_CLKDIV(100));
-
-	ret = xgmac_wait_until_free(&bus->dev, regs);
-	if (ret)
-		return ret;
-
 	/* Set the port and dev addr */
 	out_be32(&regs->mdio_ctl,
 		 MDIO_CTL_PORT_ADDR(phy_id) | MDIO_CTL_DEV_ADDR(dev_addr));
@@ -135,13 +128,6 @@ static int xgmac_mdio_read(struct mii_bus *bus, int phy_id, int regnum)
 	uint16_t value;
 	int ret;
 
-	/* Setup the MII Mgmt clock speed */
-	out_be32(&regs->mdio_stat, MDIO_STAT_CLKDIV(100));
-
-	ret = xgmac_wait_until_free(&bus->dev, regs);
-	if (ret)
-		return ret;
-
 	/* Set the Port and Device Addrs */
 	mdio_ctl = MDIO_CTL_PORT_ADDR(phy_id) | MDIO_CTL_DEV_ADDR(dev_addr);
 	out_be32(&regs->mdio_ctl, mdio_ctl);
@@ -174,24 +160,6 @@ static int xgmac_mdio_read(struct mii_bus *bus, int phy_id, int regnum)
 	return value;
 }
 
-/* Reset the MIIM registers, and wait for the bus to free */
-static int xgmac_mdio_reset(struct mii_bus *bus)
-{
-	struct tgec_mdio_controller __iomem *regs = bus->priv;
-	int ret;
-
-	mutex_lock(&bus->mdio_lock);
-
-	/* Setup the MII Mgmt clock speed */
-	out_be32(&regs->mdio_stat, MDIO_STAT_CLKDIV(100));
-
-	ret = xgmac_wait_until_free(&bus->dev, regs);
-
-	mutex_unlock(&bus->mdio_lock);
-
-	return ret;
-}
-
 static int xgmac_mdio_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
@@ -205,15 +173,13 @@ static int xgmac_mdio_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	bus = mdiobus_alloc_size(PHY_MAX_ADDR * sizeof(int));
+	bus = mdiobus_alloc();
 	if (!bus)
 		return -ENOMEM;
 
 	bus->name = "Freescale XGMAC MDIO Bus";
 	bus->read = xgmac_mdio_read;
 	bus->write = xgmac_mdio_write;
-	bus->reset = xgmac_mdio_reset;
-	bus->irq = bus->priv;
 	bus->parent = &pdev->dev;
 	snprintf(bus->id, MII_BUS_ID_SIZE, "%llx", (unsigned long long)res.start);
 
diff --git a/drivers/net/ethernet/i825xx/sni_82596.c b/drivers/net/ethernet/i825xx/sni_82596.c
index 372fa8d1fda1..2af7f77345fb 100644
--- a/drivers/net/ethernet/i825xx/sni_82596.c
+++ b/drivers/net/ethernet/i825xx/sni_82596.c
@@ -165,7 +165,6 @@ static struct platform_driver sni_82596_driver = {
 	.remove	= sni_82596_driver_remove,
 	.driver	= {
 		.name	= sni_82596_string,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/ethernet/ibm/emac/core.c b/drivers/net/ethernet/ibm/emac/core.c
index 3f3fba9e4650..9388a83818f2 100644
--- a/drivers/net/ethernet/ibm/emac/core.c
+++ b/drivers/net/ethernet/ibm/emac/core.c
@@ -3002,7 +3002,6 @@ MODULE_DEVICE_TABLE(of, emac_match);
 static struct platform_driver emac_driver = {
 	.driver = {
 		.name = "emac",
-		.owner = THIS_MODULE,
 		.of_match_table = emac_match,
 	},
 	.probe = emac_probe,
diff --git a/drivers/net/ethernet/ibm/emac/mal.c b/drivers/net/ethernet/ibm/emac/mal.c
index 63eb959a28aa..dddaab11a4c7 100644
--- a/drivers/net/ethernet/ibm/emac/mal.c
+++ b/drivers/net/ethernet/ibm/emac/mal.c
@@ -776,7 +776,6 @@ static struct of_device_id mal_platform_match[] =
 static struct platform_driver mal_of_driver = {
 	.driver = {
 		.name = "mcmal",
-		.owner = THIS_MODULE,
 		.of_match_table = mal_platform_match,
 	},
 	.probe = mal_probe,
diff --git a/drivers/net/ethernet/ibm/emac/rgmii.c b/drivers/net/ethernet/ibm/emac/rgmii.c
index a01182cce965..457088fc5b06 100644
--- a/drivers/net/ethernet/ibm/emac/rgmii.c
+++ b/drivers/net/ethernet/ibm/emac/rgmii.c
@@ -319,7 +319,6 @@ static struct of_device_id rgmii_match[] =
 static struct platform_driver rgmii_driver = {
 	.driver = {
 		.name = "emac-rgmii",
-		.owner = THIS_MODULE,
 		.of_match_table = rgmii_match,
 	},
 	.probe = rgmii_probe,
diff --git a/drivers/net/ethernet/ibm/emac/tah.c b/drivers/net/ethernet/ibm/emac/tah.c
index 9f24769ed826..cb18e7f917c6 100644
--- a/drivers/net/ethernet/ibm/emac/tah.c
+++ b/drivers/net/ethernet/ibm/emac/tah.c
@@ -163,7 +163,6 @@ static struct of_device_id tah_match[] =
 static struct platform_driver tah_driver = {
 	.driver = {
 		.name = "emac-tah",
-		.owner = THIS_MODULE,
 		.of_match_table = tah_match,
 	},
 	.probe = tah_probe,
diff --git a/drivers/net/ethernet/ibm/emac/zmii.c b/drivers/net/ethernet/ibm/emac/zmii.c
index 9ca67a38c062..36409ccb75ea 100644
--- a/drivers/net/ethernet/ibm/emac/zmii.c
+++ b/drivers/net/ethernet/ibm/emac/zmii.c
@@ -310,7 +310,6 @@ static struct of_device_id zmii_match[] =
 static struct platform_driver zmii_driver = {
 	.driver = {
 		.name = "emac-zmii",
-		.owner = THIS_MODULE,
 		.of_match_table = zmii_match,
 	},
 	.probe = zmii_probe,
diff --git a/drivers/net/ethernet/intel/e1000e/e1000.h b/drivers/net/ethernet/intel/e1000e/e1000.h
index 7785240a0da1..9416e5a7e0c8 100644
--- a/drivers/net/ethernet/intel/e1000e/e1000.h
+++ b/drivers/net/ethernet/intel/e1000e/e1000.h
@@ -34,7 +34,7 @@
 #include <linux/pci-aspm.h>
 #include <linux/crc32.h>
 #include <linux/if_vlan.h>
-#include <linux/clocksource.h>
+#include <linux/timecounter.h>
 #include <linux/net_tstamp.h>
 #include <linux/ptp_clock_kernel.h>
 #include <linux/ptp_classify.h>
diff --git a/drivers/net/ethernet/intel/e1000e/netdev.c b/drivers/net/ethernet/intel/e1000e/netdev.c
index e14fd85f64eb..332a298e95b5 100644
--- a/drivers/net/ethernet/intel/e1000e/netdev.c
+++ b/drivers/net/ethernet/intel/e1000e/netdev.c
@@ -4189,7 +4189,7 @@ static int e1000_sw_init(struct e1000_adapter *adapter)
 	/* Setup hardware time stamping cyclecounter */
 	if (adapter->flags & FLAG_HAS_HW_TIMESTAMP) {
 		adapter->cc.read = e1000e_cyclecounter_read;
-		adapter->cc.mask = CLOCKSOURCE_MASK(64);
+		adapter->cc.mask = CYCLECOUNTER_MASK(64);
 		adapter->cc.mult = 1;
 		/* cc.shift set in e1000e_get_base_tininca() */
 
diff --git a/drivers/net/ethernet/intel/e1000e/ptp.c b/drivers/net/ethernet/intel/e1000e/ptp.c
index fb1a914a3ad4..978ef9c4a043 100644
--- a/drivers/net/ethernet/intel/e1000e/ptp.c
+++ b/drivers/net/ethernet/intel/e1000e/ptp.c
@@ -90,12 +90,9 @@ static int e1000e_phc_adjtime(struct ptp_clock_info *ptp, s64 delta)
 	struct e1000_adapter *adapter = container_of(ptp, struct e1000_adapter,
 						     ptp_clock_info);
 	unsigned long flags;
-	s64 now;
 
 	spin_lock_irqsave(&adapter->systim_lock, flags);
-	now = timecounter_read(&adapter->tc);
-	now += delta;
-	timecounter_init(&adapter->tc, &adapter->cc, now);
+	timecounter_adjtime(&adapter->tc, delta);
 	spin_unlock_irqrestore(&adapter->systim_lock, flags);
 
 	return 0;
diff --git a/drivers/net/ethernet/intel/i40e/i40e_main.c b/drivers/net/ethernet/intel/i40e/i40e_main.c
index 0a7ea4c5f9d3..a5f2660d552d 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_main.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_main.c
@@ -7549,6 +7549,11 @@ static int i40e_ndo_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 	if (!(pf->flags & I40E_FLAG_SRIOV_ENABLED))
 		return -EOPNOTSUPP;
 
+	if (vid) {
+		pr_info("%s: vlans aren't supported yet for dev_uc|mc_add()\n", dev->name);
+		return -EINVAL;
+	}
+
 	/* Hardware does not support aging addresses so if a
 	 * ndm_state is given only allow permanent addresses
 	 */
diff --git a/drivers/net/ethernet/intel/igb/igb.h b/drivers/net/ethernet/intel/igb/igb.h
index 82d891e183b1..ee22da391474 100644
--- a/drivers/net/ethernet/intel/igb/igb.h
+++ b/drivers/net/ethernet/intel/igb/igb.h
@@ -29,7 +29,7 @@
 #include "e1000_mac.h"
 #include "e1000_82575.h"
 
-#include <linux/clocksource.h>
+#include <linux/timecounter.h>
 #include <linux/net_tstamp.h>
 #include <linux/ptp_clock_kernel.h>
 #include <linux/bitops.h>
diff --git a/drivers/net/ethernet/intel/igb/igb_ptp.c b/drivers/net/ethernet/intel/igb/igb_ptp.c
index 794c139f0cc0..5e7a4e30a7b6 100644
--- a/drivers/net/ethernet/intel/igb/igb_ptp.c
+++ b/drivers/net/ethernet/intel/igb/igb_ptp.c
@@ -256,14 +256,9 @@ static int igb_ptp_adjtime_82576(struct ptp_clock_info *ptp, s64 delta)
 	struct igb_adapter *igb = container_of(ptp, struct igb_adapter,
 					       ptp_caps);
 	unsigned long flags;
-	s64 now;
 
 	spin_lock_irqsave(&igb->tmreg_lock, flags);
-
-	now = timecounter_read(&igb->tc);
-	now += delta;
-	timecounter_init(&igb->tc, &igb->cc, now);
-
+	timecounter_adjtime(&igb->tc, delta);
 	spin_unlock_irqrestore(&igb->tmreg_lock, flags);
 
 	return 0;
@@ -770,7 +765,7 @@ void igb_ptp_init(struct igb_adapter *adapter)
 		adapter->ptp_caps.settime = igb_ptp_settime_82576;
 		adapter->ptp_caps.enable = igb_ptp_feature_enable;
 		adapter->cc.read = igb_ptp_read_82576;
-		adapter->cc.mask = CLOCKSOURCE_MASK(64);
+		adapter->cc.mask = CYCLECOUNTER_MASK(64);
 		adapter->cc.mult = 1;
 		adapter->cc.shift = IGB_82576_TSYNC_SHIFT;
 		/* Dial the nominal frequency. */
@@ -790,7 +785,7 @@ void igb_ptp_init(struct igb_adapter *adapter)
 		adapter->ptp_caps.settime = igb_ptp_settime_82576;
 		adapter->ptp_caps.enable = igb_ptp_feature_enable;
 		adapter->cc.read = igb_ptp_read_82580;
-		adapter->cc.mask = CLOCKSOURCE_MASK(IGB_NBITS_82580);
+		adapter->cc.mask = CYCLECOUNTER_MASK(IGB_NBITS_82580);
 		adapter->cc.mult = 1;
 		adapter->cc.shift = 0;
 		/* Enable the timer functions by clearing bit 31. */
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe.h b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
index b6137be43920..38fc64cf5dca 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
@@ -38,7 +38,7 @@
 #include <linux/if_vlan.h>
 #include <linux/jiffies.h>
 
-#include <linux/clocksource.h>
+#include <linux/timecounter.h>
 #include <linux/net_tstamp.h>
 #include <linux/ptp_clock_kernel.h>
 
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
index 5fd4b5271f9a..79c00f57d3e7 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
@@ -261,18 +261,9 @@ 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);
-
+	timecounter_adjtime(&adapter->tc, delta);
 	spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
 
 	ixgbe_ptp_setup_sdp(adapter);
@@ -802,7 +793,7 @@ void ixgbe_ptp_start_cyclecounter(struct ixgbe_adapter *adapter)
 
 	memset(&adapter->cc, 0, sizeof(adapter->cc));
 	adapter->cc.read = ixgbe_ptp_read;
-	adapter->cc.mask = CLOCKSOURCE_MASK(64);
+	adapter->cc.mask = CYCLECOUNTER_MASK(64);
 	adapter->cc.shift = shift;
 	adapter->cc.mult = 1;
 
diff --git a/drivers/net/ethernet/jme.c b/drivers/net/ethernet/jme.c
index 4a1be34d7214..44ce7d88f554 100644
--- a/drivers/net/ethernet/jme.c
+++ b/drivers/net/ethernet/jme.c
@@ -1364,8 +1364,8 @@ err_out_free_rx_resources:
 	jme_free_rx_resources(jme);
 out_enable_tasklet:
 	tasklet_enable(&jme->txclean_task);
-	tasklet_hi_enable(&jme->rxclean_task);
-	tasklet_hi_enable(&jme->rxempty_task);
+	tasklet_enable(&jme->rxclean_task);
+	tasklet_enable(&jme->rxempty_task);
 out:
 	atomic_inc(&jme->link_changing);
 }
@@ -2408,8 +2408,8 @@ static inline void jme_resume_rx(struct jme_adapter *jme)
 	if (test_bit(JME_FLAG_POLL, &jme->flags)) {
 		JME_NAPI_ENABLE(jme);
 	} else {
-		tasklet_hi_enable(&jme->rxclean_task);
-		tasklet_hi_enable(&jme->rxempty_task);
+		tasklet_enable(&jme->rxclean_task);
+		tasklet_enable(&jme->rxempty_task);
 	}
 	dpi->cur		= PCC_P1;
 	dpi->attempt		= PCC_P1;
@@ -3290,8 +3290,8 @@ jme_suspend(struct device *dev)
 	}
 
 	tasklet_enable(&jme->txclean_task);
-	tasklet_hi_enable(&jme->rxclean_task);
-	tasklet_hi_enable(&jme->rxempty_task);
+	tasklet_enable(&jme->rxclean_task);
+	tasklet_enable(&jme->rxempty_task);
 
 	jme_powersave_phy(jme);
 
diff --git a/drivers/net/ethernet/lantiq_etop.c b/drivers/net/ethernet/lantiq_etop.c
index 2dad4d5047ba..581928c068f2 100644
--- a/drivers/net/ethernet/lantiq_etop.c
+++ b/drivers/net/ethernet/lantiq_etop.c
@@ -789,7 +789,6 @@ static struct platform_driver ltq_mii_driver = {
 	.remove = ltq_etop_remove,
 	.driver = {
 		.name = "ltq_etop",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/ethernet/marvell/mv643xx_eth.c b/drivers/net/ethernet/marvell/mv643xx_eth.c
index d44560d1d268..a62fc38f045e 100644
--- a/drivers/net/ethernet/marvell/mv643xx_eth.c
+++ b/drivers/net/ethernet/marvell/mv643xx_eth.c
@@ -2839,7 +2839,6 @@ static struct platform_driver mv643xx_eth_shared_driver = {
 	.remove		= mv643xx_eth_shared_remove,
 	.driver = {
 		.name	= MV643XX_ETH_SHARED_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(mv643xx_eth_shared_ids),
 	},
 };
@@ -3188,7 +3187,6 @@ static struct platform_driver mv643xx_eth_driver = {
 	.shutdown	= mv643xx_eth_shutdown,
 	.driver = {
 		.name	= MV643XX_ETH_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_clock.c b/drivers/net/ethernet/mellanox/mlx4/en_clock.c
index 999014413b1a..90b5309cdb5c 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_clock.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_clock.c
@@ -32,6 +32,7 @@
  */
 
 #include <linux/mlx4/device.h>
+#include <linux/clocksource.h>
 
 #include "mlx4_en.h"
 
@@ -147,12 +148,9 @@ static int mlx4_en_phc_adjtime(struct ptp_clock_info *ptp, s64 delta)
 	struct mlx4_en_dev *mdev = container_of(ptp, struct mlx4_en_dev,
 						ptp_clock_info);
 	unsigned long flags;
-	s64 now;
 
 	write_lock_irqsave(&mdev->clock_lock, flags);
-	now = timecounter_read(&mdev->clock);
-	now += delta;
-	timecounter_init(&mdev->clock, &mdev->cycles, now);
+	timecounter_adjtime(&mdev->clock, delta);
 	write_unlock_irqrestore(&mdev->clock_lock, flags);
 
 	return 0;
@@ -243,7 +241,7 @@ void mlx4_en_init_timestamp(struct mlx4_en_dev *mdev)
 {
 	struct mlx4_dev *dev = mdev->dev;
 	unsigned long flags;
-	u64 ns;
+	u64 ns, zero = 0;
 
 	rwlock_init(&mdev->clock_lock);
 
@@ -268,7 +266,7 @@ void mlx4_en_init_timestamp(struct mlx4_en_dev *mdev)
 	/* Calculate period in seconds to call the overflow watchdog - to make
 	 * sure counter is checked at least once every wrap around.
 	 */
-	ns = cyclecounter_cyc2ns(&mdev->cycles, mdev->cycles.mask);
+	ns = cyclecounter_cyc2ns(&mdev->cycles, mdev->cycles.mask, zero, &zero);
 	do_div(ns, NSEC_PER_SEC / 2 / HZ);
 	mdev->overflow_period = ns;
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
index 6ff214de1111..d0d6dc1b8e46 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
@@ -1569,8 +1569,15 @@ int mlx4_en_start_port(struct net_device *dev)
 			mlx4_en_free_affinity_hint(priv, i);
 			goto cq_err;
 		}
-		for (j = 0; j < cq->size; j++)
-			cq->buf[j].owner_sr_opcode = MLX4_CQE_OWNER_MASK;
+
+		for (j = 0; j < cq->size; j++) {
+			struct mlx4_cqe *cqe = NULL;
+
+			cqe = mlx4_en_get_cqe(cq->buf, j, priv->cqe_size) +
+			      priv->cqe_factor;
+			cqe->owner_sr_opcode = MLX4_CQE_OWNER_MASK;
+		}
+
 		err = mlx4_en_set_cq_moder(priv, cq);
 		if (err) {
 			en_err(priv, "Failed setting cq moderation parameters\n");
@@ -2358,9 +2365,11 @@ static void mlx4_en_del_vxlan_port(struct  net_device *dev,
 	queue_work(priv->mdev->workqueue, &priv->vxlan_del_task);
 }
 
-static bool mlx4_en_gso_check(struct sk_buff *skb, struct net_device *dev)
+static netdev_features_t mlx4_en_features_check(struct sk_buff *skb,
+						struct net_device *dev,
+						netdev_features_t features)
 {
-	return vxlan_gso_check(skb);
+	return vxlan_features_check(skb, features);
 }
 #endif
 
@@ -2393,7 +2402,7 @@ static const struct net_device_ops mlx4_netdev_ops = {
 #ifdef CONFIG_MLX4_EN_VXLAN
 	.ndo_add_vxlan_port	= mlx4_en_add_vxlan_port,
 	.ndo_del_vxlan_port	= mlx4_en_del_vxlan_port,
-	.ndo_gso_check		= mlx4_en_gso_check,
+	.ndo_features_check	= mlx4_en_features_check,
 #endif
 };
 
@@ -2427,7 +2436,7 @@ static const struct net_device_ops mlx4_netdev_ops_master = {
 #ifdef CONFIG_MLX4_EN_VXLAN
 	.ndo_add_vxlan_port	= mlx4_en_add_vxlan_port,
 	.ndo_del_vxlan_port	= mlx4_en_del_vxlan_port,
-	.ndo_gso_check		= mlx4_en_gso_check,
+	.ndo_features_check	= mlx4_en_features_check,
 #endif
 };
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_tx.c b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
index a308d41e4de0..e3357bf523df 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_tx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
@@ -962,7 +962,17 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 		tx_desc->ctrl.owner_opcode = op_own;
 		if (send_doorbell) {
 			wmb();
-			iowrite32(ring->doorbell_qpn,
+			/* Since there is no iowrite*_native() that writes the
+			 * value as is, without byteswapping - using the one
+			 * the doesn't do byteswapping in the relevant arch
+			 * endianness.
+			 */
+#if defined(__LITTLE_ENDIAN)
+			iowrite32(
+#else
+			iowrite32be(
+#endif
+				  ring->doorbell_qpn,
 				  ring->bf.uar->map + MLX4_SEND_DOORBELL);
 		} else {
 			ring->xmit_more++;
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw.c b/drivers/net/ethernet/mellanox/mlx4/fw.c
index ef3b95bac2ad..982861d1df44 100644
--- a/drivers/net/ethernet/mellanox/mlx4/fw.c
+++ b/drivers/net/ethernet/mellanox/mlx4/fw.c
@@ -787,11 +787,8 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 		if ((1 << (field & 0x3f)) > (PAGE_SIZE / dev_cap->bf_reg_size))
 			field = 3;
 		dev_cap->bf_regs_per_page = 1 << (field & 0x3f);
-		mlx4_dbg(dev, "BlueFlame available (reg size %d, regs/page %d)\n",
-			 dev_cap->bf_reg_size, dev_cap->bf_regs_per_page);
 	} else {
 		dev_cap->bf_reg_size = 0;
-		mlx4_dbg(dev, "BlueFlame not available\n");
 	}
 
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_MAX_SG_SQ_OFFSET);
@@ -902,9 +899,6 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 			goto out;
 	}
 
-	mlx4_dbg(dev, "Base MM extensions: flags %08x, rsvd L_Key %08x\n",
-		 dev_cap->bmme_flags, dev_cap->reserved_lkey);
-
 	/*
 	 * Each UAR has 4 EQ doorbells; so if a UAR is reserved, then
 	 * we can't use any EQs whose doorbell falls on that page,
@@ -916,6 +910,21 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	else
 		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_SYS_EQS;
 
+out:
+	mlx4_free_cmd_mailbox(dev, mailbox);
+	return err;
+}
+
+void mlx4_dev_cap_dump(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
+{
+	if (dev_cap->bf_reg_size > 0)
+		mlx4_dbg(dev, "BlueFlame available (reg size %d, regs/page %d)\n",
+			 dev_cap->bf_reg_size, dev_cap->bf_regs_per_page);
+	else
+		mlx4_dbg(dev, "BlueFlame not available\n");
+
+	mlx4_dbg(dev, "Base MM extensions: flags %08x, rsvd L_Key %08x\n",
+		 dev_cap->bmme_flags, dev_cap->reserved_lkey);
 	mlx4_dbg(dev, "Max ICM size %lld MB\n",
 		 (unsigned long long) dev_cap->max_icm_sz >> 20);
 	mlx4_dbg(dev, "Max QPs: %d, reserved QPs: %d, entry size: %d\n",
@@ -949,13 +958,8 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 		 dev_cap->dmfs_high_rate_qpn_base);
 	mlx4_dbg(dev, "DMFS high rate steer QPn range: %d\n",
 		 dev_cap->dmfs_high_rate_qpn_range);
-
 	dump_dev_cap_flags(dev, dev_cap->flags);
 	dump_dev_cap_flags2(dev, dev_cap->flags2);
-
-out:
-	mlx4_free_cmd_mailbox(dev, mailbox);
-	return err;
 }
 
 int mlx4_QUERY_PORT(struct mlx4_dev *dev, int port, struct mlx4_port_cap *port_cap)
@@ -1848,8 +1852,8 @@ int mlx4_QUERY_HCA(struct mlx4_dev *dev,
 	/* CX3 is capable of extending CQEs\EQEs to strides larger than 64B */
 	MLX4_GET(byte_field, outbox, INIT_HCA_EQE_CQE_STRIDE_OFFSET);
 	if (byte_field) {
-		param->dev_cap_enabled |= MLX4_DEV_CAP_64B_EQE_ENABLED;
-		param->dev_cap_enabled |= MLX4_DEV_CAP_64B_CQE_ENABLED;
+		param->dev_cap_enabled |= MLX4_DEV_CAP_EQE_STRIDE_ENABLED;
+		param->dev_cap_enabled |= MLX4_DEV_CAP_CQE_STRIDE_ENABLED;
 		param->cqe_size = 1 << ((byte_field &
 					 MLX4_CQE_SIZE_MASK_STRIDE) + 5);
 		param->eqe_size = 1 << (((byte_field &
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw.h b/drivers/net/ethernet/mellanox/mlx4/fw.h
index 794e2826609a..62562b60fa87 100644
--- a/drivers/net/ethernet/mellanox/mlx4/fw.h
+++ b/drivers/net/ethernet/mellanox/mlx4/fw.h
@@ -224,6 +224,7 @@ struct mlx4_set_ib_param {
 	u32 cap_mask;
 };
 
+void mlx4_dev_cap_dump(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap);
 int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap);
 int mlx4_QUERY_PORT(struct mlx4_dev *dev, int port, struct mlx4_port_cap *port_cap);
 int mlx4_QUERY_FUNC_CAP(struct mlx4_dev *dev, u8 gen_or_port,
diff --git a/drivers/net/ethernet/mellanox/mlx4/main.c b/drivers/net/ethernet/mellanox/mlx4/main.c
index e25436b24ce7..943cbd47d832 100644
--- a/drivers/net/ethernet/mellanox/mlx4/main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/main.c
@@ -171,9 +171,9 @@ int mlx4_check_port_params(struct mlx4_dev *dev,
 {
 	int i;
 
-	for (i = 0; i < dev->caps.num_ports - 1; i++) {
-		if (port_type[i] != port_type[i + 1]) {
-			if (!(dev->caps.flags & MLX4_DEV_CAP_FLAG_DPDP)) {
+	if (!(dev->caps.flags & MLX4_DEV_CAP_FLAG_DPDP)) {
+		for (i = 0; i < dev->caps.num_ports - 1; i++) {
+			if (port_type[i] != port_type[i + 1]) {
 				mlx4_err(dev, "Only same port types supported on this HCA, aborting\n");
 				return -EINVAL;
 			}
@@ -305,6 +305,7 @@ static int mlx4_dev_cap(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 		mlx4_err(dev, "QUERY_DEV_CAP command failed, aborting\n");
 		return err;
 	}
+	mlx4_dev_cap_dump(dev, dev_cap);
 
 	if (dev_cap->min_page_sz > PAGE_SIZE) {
 		mlx4_err(dev, "HCA minimum page size of %d bigger than kernel PAGE_SIZE of %ld, aborting\n",
@@ -2488,41 +2489,42 @@ static u64 mlx4_enable_sriov(struct mlx4_dev *dev, struct pci_dev *pdev,
 			     u8 total_vfs, int existing_vfs)
 {
 	u64 dev_flags = dev->flags;
+	int err = 0;
 
-	dev->dev_vfs = kzalloc(
-			total_vfs * sizeof(*dev->dev_vfs),
-			GFP_KERNEL);
+	atomic_inc(&pf_loading);
+	if (dev->flags &  MLX4_FLAG_SRIOV) {
+		if (existing_vfs != total_vfs) {
+			mlx4_err(dev, "SR-IOV was already enabled, but with num_vfs (%d) different than requested (%d)\n",
+				 existing_vfs, total_vfs);
+			total_vfs = existing_vfs;
+		}
+	}
+
+	dev->dev_vfs = kzalloc(total_vfs * sizeof(*dev->dev_vfs), GFP_KERNEL);
 	if (NULL == dev->dev_vfs) {
 		mlx4_err(dev, "Failed to allocate memory for VFs\n");
 		goto disable_sriov;
-	} else if (!(dev->flags &  MLX4_FLAG_SRIOV)) {
-		int err = 0;
-
-		atomic_inc(&pf_loading);
-		if (existing_vfs) {
-			if (existing_vfs != total_vfs)
-				mlx4_err(dev, "SR-IOV was already enabled, but with num_vfs (%d) different than requested (%d)\n",
-					 existing_vfs, total_vfs);
-		} else {
-			mlx4_warn(dev, "Enabling SR-IOV with %d VFs\n", total_vfs);
-			err = pci_enable_sriov(pdev, total_vfs);
-		}
-		if (err) {
-			mlx4_err(dev, "Failed to enable SR-IOV, continuing without SR-IOV (err = %d)\n",
-				 err);
-			atomic_dec(&pf_loading);
-			goto disable_sriov;
-		} else {
-			mlx4_warn(dev, "Running in master mode\n");
-			dev_flags |= MLX4_FLAG_SRIOV |
-				MLX4_FLAG_MASTER;
-			dev_flags &= ~MLX4_FLAG_SLAVE;
-			dev->num_vfs = total_vfs;
-		}
+	}
+
+	if (!(dev->flags &  MLX4_FLAG_SRIOV)) {
+		mlx4_warn(dev, "Enabling SR-IOV with %d VFs\n", total_vfs);
+		err = pci_enable_sriov(pdev, total_vfs);
+	}
+	if (err) {
+		mlx4_err(dev, "Failed to enable SR-IOV, continuing without SR-IOV (err = %d)\n",
+			 err);
+		goto disable_sriov;
+	} else {
+		mlx4_warn(dev, "Running in master mode\n");
+		dev_flags |= MLX4_FLAG_SRIOV |
+			MLX4_FLAG_MASTER;
+		dev_flags &= ~MLX4_FLAG_SLAVE;
+		dev->num_vfs = total_vfs;
 	}
 	return dev_flags;
 
 disable_sriov:
+	atomic_dec(&pf_loading);
 	dev->num_vfs = 0;
 	kfree(dev->dev_vfs);
 	return dev_flags & ~MLX4_FLAG_MASTER;
@@ -2606,8 +2608,10 @@ static int mlx4_load_one(struct pci_dev *pdev, int pci_dev_data,
 		}
 
 		if (total_vfs) {
-			existing_vfs = pci_num_vf(pdev);
 			dev->flags = MLX4_FLAG_MASTER;
+			existing_vfs = pci_num_vf(pdev);
+			if (existing_vfs)
+				dev->flags |= MLX4_FLAG_SRIOV;
 			dev->num_vfs = total_vfs;
 		}
 	}
@@ -2643,6 +2647,7 @@ slave_start:
 	}
 
 	if (mlx4_is_master(dev)) {
+		/* when we hit the goto slave_start below, dev_cap already initialized */
 		if (!dev_cap) {
 			dev_cap = kzalloc(sizeof(*dev_cap), GFP_KERNEL);
 
@@ -2849,6 +2854,7 @@ slave_start:
 	if (mlx4_is_master(dev) && dev->num_vfs)
 		atomic_dec(&pf_loading);
 
+	kfree(dev_cap);
 	return 0;
 
 err_port:
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/eq.c b/drivers/net/ethernet/mellanox/mlx5/core/eq.c
index ab684463780b..da82991239a8 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/eq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/eq.c
@@ -157,6 +157,8 @@ static const char *eqe_type_str(u8 type)
 		return "MLX5_EVENT_TYPE_CMD";
 	case MLX5_EVENT_TYPE_PAGE_REQUEST:
 		return "MLX5_EVENT_TYPE_PAGE_REQUEST";
+	case MLX5_EVENT_TYPE_PAGE_FAULT:
+		return "MLX5_EVENT_TYPE_PAGE_FAULT";
 	default:
 		return "Unrecognized event";
 	}
@@ -279,6 +281,11 @@ static int mlx5_eq_int(struct mlx5_core_dev *dev, struct mlx5_eq *eq)
 			}
 			break;
 
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+		case MLX5_EVENT_TYPE_PAGE_FAULT:
+			mlx5_eq_pagefault(dev, eqe);
+			break;
+#endif
 
 		default:
 			mlx5_core_warn(dev, "Unhandled event 0x%x on EQ 0x%x\n",
@@ -446,8 +453,12 @@ void mlx5_eq_cleanup(struct mlx5_core_dev *dev)
 int mlx5_start_eqs(struct mlx5_core_dev *dev)
 {
 	struct mlx5_eq_table *table = &dev->priv.eq_table;
+	u32 async_event_mask = MLX5_ASYNC_EVENT_MASK;
 	int err;
 
+	if (dev->caps.gen.flags & MLX5_DEV_CAP_FLAG_ON_DMND_PG)
+		async_event_mask |= (1ull << MLX5_EVENT_TYPE_PAGE_FAULT);
+
 	err = mlx5_create_map_eq(dev, &table->cmd_eq, MLX5_EQ_VEC_CMD,
 				 MLX5_NUM_CMD_EQE, 1ull << MLX5_EVENT_TYPE_CMD,
 				 "mlx5_cmd_eq", &dev->priv.uuari.uars[0]);
@@ -459,7 +470,7 @@ int mlx5_start_eqs(struct mlx5_core_dev *dev)
 	mlx5_cmd_use_events(dev);
 
 	err = mlx5_create_map_eq(dev, &table->async_eq, MLX5_EQ_VEC_ASYNC,
-				 MLX5_NUM_ASYNC_EQE, MLX5_ASYNC_EVENT_MASK,
+				 MLX5_NUM_ASYNC_EQE, async_event_mask,
 				 "mlx5_async_eq", &dev->priv.uuari.uars[0]);
 	if (err) {
 		mlx5_core_warn(dev, "failed to create async EQ %d\n", err);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fw.c b/drivers/net/ethernet/mellanox/mlx5/core/fw.c
index 087c4c797deb..06f9036acd83 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/fw.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fw.c
@@ -69,6 +69,46 @@ int mlx5_cmd_query_hca_cap(struct mlx5_core_dev *dev, struct mlx5_caps *caps)
 	return mlx5_core_get_caps(dev, caps, HCA_CAP_OPMOD_GET_CUR);
 }
 
+int mlx5_query_odp_caps(struct mlx5_core_dev *dev, struct mlx5_odp_caps *caps)
+{
+	u8 in[MLX5_ST_SZ_BYTES(query_hca_cap_in)];
+	int out_sz = MLX5_ST_SZ_BYTES(query_hca_cap_out);
+	void *out;
+	int err;
+
+	if (!(dev->caps.gen.flags & MLX5_DEV_CAP_FLAG_ON_DMND_PG))
+		return -ENOTSUPP;
+
+	memset(in, 0, sizeof(in));
+	out = kzalloc(out_sz, GFP_KERNEL);
+	if (!out)
+		return -ENOMEM;
+	MLX5_SET(query_hca_cap_in, in, opcode, MLX5_CMD_OP_QUERY_HCA_CAP);
+	MLX5_SET(query_hca_cap_in, in, op_mod, HCA_CAP_OPMOD_GET_ODP_CUR);
+	err = mlx5_cmd_exec(dev, in, sizeof(in), out, out_sz);
+	if (err)
+		goto out;
+
+	err = mlx5_cmd_status_to_err_v2(out);
+	if (err) {
+		mlx5_core_warn(dev, "query cur hca ODP caps failed, %d\n", err);
+		goto out;
+	}
+
+	memcpy(caps, MLX5_ADDR_OF(query_hca_cap_out, out, capability_struct),
+	       sizeof(*caps));
+
+	mlx5_core_dbg(dev, "on-demand paging capabilities:\nrc: %08x\nuc: %08x\nud: %08x\n",
+		be32_to_cpu(caps->per_transport_caps.rc_odp_caps),
+		be32_to_cpu(caps->per_transport_caps.uc_odp_caps),
+		be32_to_cpu(caps->per_transport_caps.ud_odp_caps));
+
+out:
+	kfree(out);
+	return err;
+}
+EXPORT_SYMBOL(mlx5_query_odp_caps);
+
 int mlx5_cmd_init_hca(struct mlx5_core_dev *dev)
 {
 	struct mlx5_cmd_init_hca_mbox_in in;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/qp.c b/drivers/net/ethernet/mellanox/mlx5/core/qp.c
index 5261a2b0da43..575d853dbe05 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/qp.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/qp.c
@@ -88,6 +88,95 @@ void mlx5_rsc_event(struct mlx5_core_dev *dev, u32 rsn, int event_type)
 	mlx5_core_put_rsc(common);
 }
 
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+void mlx5_eq_pagefault(struct mlx5_core_dev *dev, struct mlx5_eqe *eqe)
+{
+	struct mlx5_eqe_page_fault *pf_eqe = &eqe->data.page_fault;
+	int qpn = be32_to_cpu(pf_eqe->flags_qpn) & MLX5_QPN_MASK;
+	struct mlx5_core_rsc_common *common = mlx5_get_rsc(dev, qpn);
+	struct mlx5_core_qp *qp =
+		container_of(common, struct mlx5_core_qp, common);
+	struct mlx5_pagefault pfault;
+
+	if (!qp) {
+		mlx5_core_warn(dev, "ODP event for non-existent QP %06x\n",
+			       qpn);
+		return;
+	}
+
+	pfault.event_subtype = eqe->sub_type;
+	pfault.flags = (be32_to_cpu(pf_eqe->flags_qpn) >> MLX5_QPN_BITS) &
+		(MLX5_PFAULT_REQUESTOR | MLX5_PFAULT_WRITE | MLX5_PFAULT_RDMA);
+	pfault.bytes_committed = be32_to_cpu(
+		pf_eqe->bytes_committed);
+
+	mlx5_core_dbg(dev,
+		      "PAGE_FAULT: subtype: 0x%02x, flags: 0x%02x,\n",
+		      eqe->sub_type, pfault.flags);
+
+	switch (eqe->sub_type) {
+	case MLX5_PFAULT_SUBTYPE_RDMA:
+		/* RDMA based event */
+		pfault.rdma.r_key =
+			be32_to_cpu(pf_eqe->rdma.r_key);
+		pfault.rdma.packet_size =
+			be16_to_cpu(pf_eqe->rdma.packet_length);
+		pfault.rdma.rdma_op_len =
+			be32_to_cpu(pf_eqe->rdma.rdma_op_len);
+		pfault.rdma.rdma_va =
+			be64_to_cpu(pf_eqe->rdma.rdma_va);
+		mlx5_core_dbg(dev,
+			      "PAGE_FAULT: qpn: 0x%06x, r_key: 0x%08x,\n",
+			      qpn, pfault.rdma.r_key);
+		mlx5_core_dbg(dev,
+			      "PAGE_FAULT: rdma_op_len: 0x%08x,\n",
+			      pfault.rdma.rdma_op_len);
+		mlx5_core_dbg(dev,
+			      "PAGE_FAULT: rdma_va: 0x%016llx,\n",
+			      pfault.rdma.rdma_va);
+		mlx5_core_dbg(dev,
+			      "PAGE_FAULT: bytes_committed: 0x%06x\n",
+			      pfault.bytes_committed);
+		break;
+
+	case MLX5_PFAULT_SUBTYPE_WQE:
+		/* WQE based event */
+		pfault.wqe.wqe_index =
+			be16_to_cpu(pf_eqe->wqe.wqe_index);
+		pfault.wqe.packet_size =
+			be16_to_cpu(pf_eqe->wqe.packet_length);
+		mlx5_core_dbg(dev,
+			      "PAGE_FAULT: qpn: 0x%06x, wqe_index: 0x%04x,\n",
+			      qpn, pfault.wqe.wqe_index);
+		mlx5_core_dbg(dev,
+			      "PAGE_FAULT: bytes_committed: 0x%06x\n",
+			      pfault.bytes_committed);
+		break;
+
+	default:
+		mlx5_core_warn(dev,
+			       "Unsupported page fault event sub-type: 0x%02hhx, QP %06x\n",
+			       eqe->sub_type, qpn);
+		/* Unsupported page faults should still be resolved by the
+		 * page fault handler
+		 */
+	}
+
+	if (qp->pfault_handler) {
+		qp->pfault_handler(qp, &pfault);
+	} else {
+		mlx5_core_err(dev,
+			      "ODP event for QP %08x, without a fault handler in QP\n",
+			      qpn);
+		/* Page fault will remain unresolved. QP will hang until it is
+		 * destroyed
+		 */
+	}
+
+	mlx5_core_put_rsc(common);
+}
+#endif
+
 int mlx5_core_create_qp(struct mlx5_core_dev *dev,
 			struct mlx5_core_qp *qp,
 			struct mlx5_create_qp_mbox_in *in,
@@ -322,3 +411,33 @@ int mlx5_core_xrcd_dealloc(struct mlx5_core_dev *dev, u32 xrcdn)
 	return err;
 }
 EXPORT_SYMBOL_GPL(mlx5_core_xrcd_dealloc);
+
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+int mlx5_core_page_fault_resume(struct mlx5_core_dev *dev, u32 qpn,
+				u8 flags, int error)
+{
+	struct mlx5_page_fault_resume_mbox_in in;
+	struct mlx5_page_fault_resume_mbox_out out;
+	int err;
+
+	memset(&in, 0, sizeof(in));
+	memset(&out, 0, sizeof(out));
+	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_PAGE_FAULT_RESUME);
+	in.hdr.opmod = 0;
+	flags &= (MLX5_PAGE_FAULT_RESUME_REQUESTOR |
+		  MLX5_PAGE_FAULT_RESUME_WRITE	   |
+		  MLX5_PAGE_FAULT_RESUME_RDMA);
+	flags |= (error ? MLX5_PAGE_FAULT_RESUME_ERROR : 0);
+	in.flags_qpn = cpu_to_be32((qpn & MLX5_QPN_MASK) |
+				   (flags << MLX5_QPN_BITS));
+	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
+	if (err)
+		return err;
+
+	if (out.hdr.status)
+		err = mlx5_cmd_status_to_err(&out.hdr);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx5_core_page_fault_resume);
+#endif
diff --git a/drivers/net/ethernet/micrel/ks8695net.c b/drivers/net/ethernet/micrel/ks8695net.c
index 6c7c78baedca..a8522d8af95d 100644
--- a/drivers/net/ethernet/micrel/ks8695net.c
+++ b/drivers/net/ethernet/micrel/ks8695net.c
@@ -1612,7 +1612,6 @@ ks8695_drv_remove(struct platform_device *pdev)
 static struct platform_driver ks8695_driver = {
 	.driver = {
 		.name	= MODULENAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= ks8695_probe,
 	.remove		= ks8695_drv_remove,
diff --git a/drivers/net/ethernet/micrel/ks8842.c b/drivers/net/ethernet/micrel/ks8842.c
index 0c33b92a5a81..f78909a00f15 100644
--- a/drivers/net/ethernet/micrel/ks8842.c
+++ b/drivers/net/ethernet/micrel/ks8842.c
@@ -1255,7 +1255,6 @@ static int ks8842_remove(struct platform_device *pdev)
 static struct platform_driver ks8842_platform_driver = {
 	.driver = {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= ks8842_probe,
 	.remove		= ks8842_remove,
diff --git a/drivers/net/ethernet/micrel/ks8851_mll.c b/drivers/net/ethernet/micrel/ks8851_mll.c
index 0eb47649191b..2fc5cd56c0a8 100644
--- a/drivers/net/ethernet/micrel/ks8851_mll.c
+++ b/drivers/net/ethernet/micrel/ks8851_mll.c
@@ -1679,7 +1679,6 @@ static int ks8851_remove(struct platform_device *pdev)
 static struct platform_driver ks8851_platform_driver = {
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table	= of_match_ptr(ks8851_ml_dt_ids),
 	},
 	.probe = ks8851_probe,
diff --git a/drivers/net/ethernet/micrel/ksz884x.c b/drivers/net/ethernet/micrel/ksz884x.c
index f1ebed6c63b1..10988fbf47eb 100644
--- a/drivers/net/ethernet/micrel/ksz884x.c
+++ b/drivers/net/ethernet/micrel/ksz884x.c
@@ -2303,12 +2303,6 @@ static inline int port_chk_force_flow_ctrl(struct ksz_hw *hw, int p)
 
 /* Spanning Tree */
 
-static inline void port_cfg_dis_learn(struct ksz_hw *hw, int p, int set)
-{
-	port_cfg(hw, p,
-		KS8842_PORT_CTRL_2_OFFSET, PORT_LEARN_DISABLE, set);
-}
-
 static inline void port_cfg_rx(struct ksz_hw *hw, int p, int set)
 {
 	port_cfg(hw, p,
@@ -4348,9 +4342,7 @@ static void ksz_init_timer(struct ksz_timer_info *info, int period,
 {
 	info->max = 0;
 	info->period = period;
-	init_timer(&info->timer);
-	info->timer.function = function;
-	info->timer.data = (unsigned long) data;
+	setup_timer(&info->timer, function, (unsigned long)data);
 }
 
 static void ksz_update_timer(struct ksz_timer_info *info)
diff --git a/drivers/net/ethernet/moxa/moxart_ether.c b/drivers/net/ethernet/moxa/moxart_ether.c
index bde1b70f473b..6c72e74fef3e 100644
--- a/drivers/net/ethernet/moxa/moxart_ether.c
+++ b/drivers/net/ethernet/moxa/moxart_ether.c
@@ -559,7 +559,6 @@ static struct platform_driver moxart_mac_driver = {
 	.remove	= moxart_remove,
 	.driver	= {
 		.name		= "moxart-ethernet",
-		.owner		= THIS_MODULE,
 		.of_match_table	= moxart_mac_match,
 	},
 };
diff --git a/drivers/net/ethernet/natsemi/jazzsonic.c b/drivers/net/ethernet/natsemi/jazzsonic.c
index a5512a97cc4d..acf3f11e38cc 100644
--- a/drivers/net/ethernet/natsemi/jazzsonic.c
+++ b/drivers/net/ethernet/natsemi/jazzsonic.c
@@ -287,7 +287,6 @@ static struct platform_driver jazz_sonic_driver = {
 	.remove	= jazz_sonic_device_remove,
 	.driver	= {
 		.name	= jazz_sonic_string,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/ethernet/natsemi/macsonic.c b/drivers/net/ethernet/natsemi/macsonic.c
index 66c2d50d5b8d..d98f5b8a1c66 100644
--- a/drivers/net/ethernet/natsemi/macsonic.c
+++ b/drivers/net/ethernet/natsemi/macsonic.c
@@ -630,7 +630,6 @@ static struct platform_driver mac_sonic_driver = {
 	.remove = mac_sonic_device_remove,
 	.driver	= {
 		.name	= mac_sonic_string,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/ethernet/netx-eth.c b/drivers/net/ethernet/netx-eth.c
index 8176c8a1cc6a..9fbc30264237 100644
--- a/drivers/net/ethernet/netx-eth.c
+++ b/drivers/net/ethernet/netx-eth.c
@@ -457,7 +457,6 @@ static struct platform_driver netx_eth_driver = {
 	.resume		= netx_eth_drv_resume,
 	.driver		= {
 		.name	= CARDNAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/ethernet/nuvoton/w90p910_ether.c b/drivers/net/ethernet/nuvoton/w90p910_ether.c
index 379b7fbded78..afa445842f3e 100644
--- a/drivers/net/ethernet/nuvoton/w90p910_ether.c
+++ b/drivers/net/ethernet/nuvoton/w90p910_ether.c
@@ -1081,7 +1081,6 @@ static struct platform_driver w90p910_ether_driver = {
 	.remove		= w90p910_ether_remove,
 	.driver		= {
 		.name	= "nuc900-emc",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/ethernet/octeon/octeon_mgmt.c b/drivers/net/ethernet/octeon/octeon_mgmt.c
index a42293092ea4..d36599f47af5 100644
--- a/drivers/net/ethernet/octeon/octeon_mgmt.c
+++ b/drivers/net/ethernet/octeon/octeon_mgmt.c
@@ -1568,7 +1568,6 @@ MODULE_DEVICE_TABLE(of, octeon_mgmt_match);
 static struct platform_driver octeon_mgmt_driver = {
 	.driver = {
 		.name		= "octeon_mgmt",
-		.owner		= THIS_MODULE,
 		.of_match_table = octeon_mgmt_match,
 	},
 	.probe		= octeon_mgmt_probe,
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
index 1aa25b13ace1..9929b97cfb36 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
@@ -505,9 +505,11 @@ static void qlcnic_del_vxlan_port(struct net_device *netdev,
 	adapter->flags |= QLCNIC_DEL_VXLAN_PORT;
 }
 
-static bool qlcnic_gso_check(struct sk_buff *skb, struct net_device *dev)
+static netdev_features_t qlcnic_features_check(struct sk_buff *skb,
+					       struct net_device *dev,
+					       netdev_features_t features)
 {
-	return vxlan_gso_check(skb);
+	return vxlan_features_check(skb, features);
 }
 #endif
 
@@ -532,7 +534,7 @@ static const struct net_device_ops qlcnic_netdev_ops = {
 #ifdef CONFIG_QLCNIC_VXLAN
 	.ndo_add_vxlan_port	= qlcnic_add_vxlan_port,
 	.ndo_del_vxlan_port	= qlcnic_del_vxlan_port,
-	.ndo_gso_check		= qlcnic_gso_check,
+	.ndo_features_check	= qlcnic_features_check,
 #endif
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller = qlcnic_poll_controller,
diff --git a/drivers/net/ethernet/realtek/8139too.c b/drivers/net/ethernet/realtek/8139too.c
index 6d0b9dfac313..78bb4ceb1cdd 100644
--- a/drivers/net/ethernet/realtek/8139too.c
+++ b/drivers/net/ethernet/realtek/8139too.c
@@ -787,10 +787,10 @@ static struct net_device *rtl8139_init_board(struct pci_dev *pdev)
 	if (rc)
 		goto err_out;
 
+	disable_dev_on_err = 1;
 	rc = pci_request_regions (pdev, DRV_NAME);
 	if (rc)
 		goto err_out;
-	disable_dev_on_err = 1;
 
 	pci_set_master (pdev);
 
@@ -1110,6 +1110,7 @@ static int rtl8139_init_one(struct pci_dev *pdev,
 	return 0;
 
 err_out:
+	netif_napi_del(&tp->napi);
 	__rtl8139_cleanup_dev (dev);
 	pci_disable_device (pdev);
 	return i;
@@ -1124,6 +1125,7 @@ static void rtl8139_remove_one(struct pci_dev *pdev)
 	assert (dev != NULL);
 
 	cancel_delayed_work_sync(&tp->thread);
+	netif_napi_del(&tp->napi);
 
 	unregister_netdev (dev);
 
diff --git a/drivers/net/ethernet/s6gmac.c b/drivers/net/ethernet/s6gmac.c
deleted file mode 100644
index 1f4449ad8900..000000000000
--- a/drivers/net/ethernet/s6gmac.c
+++ /dev/null
@@ -1,1059 +0,0 @@
-/*
- * Ethernet driver for S6105 on chip network device
- * (c)2008 emlix GmbH http://www.emlix.com
- * Authors:	Oskar Schirmer <oskar@scara.com>
- *		Daniel Gloeckner <dg@emlix.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/module.h>
-#include <linux/interrupt.h>
-#include <linux/types.h>
-#include <linux/delay.h>
-#include <linux/spinlock.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/if.h>
-#include <linux/stddef.h>
-#include <linux/mii.h>
-#include <linux/phy.h>
-#include <linux/platform_device.h>
-#include <variant/hardware.h>
-#include <variant/dmac.h>
-
-#define DRV_NAME "s6gmac"
-#define DRV_PRMT DRV_NAME ": "
-
-
-/* register declarations */
-
-#define S6_GMAC_MACCONF1	0x000
-#define S6_GMAC_MACCONF1_TXENA		0
-#define S6_GMAC_MACCONF1_SYNCTX		1
-#define S6_GMAC_MACCONF1_RXENA		2
-#define S6_GMAC_MACCONF1_SYNCRX		3
-#define S6_GMAC_MACCONF1_TXFLOWCTRL	4
-#define S6_GMAC_MACCONF1_RXFLOWCTRL	5
-#define S6_GMAC_MACCONF1_LOOPBACK	8
-#define S6_GMAC_MACCONF1_RESTXFUNC	16
-#define S6_GMAC_MACCONF1_RESRXFUNC	17
-#define S6_GMAC_MACCONF1_RESTXMACCTRL	18
-#define S6_GMAC_MACCONF1_RESRXMACCTRL	19
-#define S6_GMAC_MACCONF1_SIMULRES	30
-#define S6_GMAC_MACCONF1_SOFTRES	31
-#define S6_GMAC_MACCONF2	0x004
-#define S6_GMAC_MACCONF2_FULL		0
-#define S6_GMAC_MACCONF2_CRCENA		1
-#define S6_GMAC_MACCONF2_PADCRCENA	2
-#define S6_GMAC_MACCONF2_LENGTHFCHK	4
-#define S6_GMAC_MACCONF2_HUGEFRAMENA	5
-#define S6_GMAC_MACCONF2_IFMODE		8
-#define S6_GMAC_MACCONF2_IFMODE_NIBBLE		1
-#define S6_GMAC_MACCONF2_IFMODE_BYTE		2
-#define S6_GMAC_MACCONF2_IFMODE_MASK		3
-#define S6_GMAC_MACCONF2_PREAMBLELEN	12
-#define S6_GMAC_MACCONF2_PREAMBLELEN_MASK	0x0F
-#define S6_GMAC_MACIPGIFG	0x008
-#define S6_GMAC_MACIPGIFG_B2BINTERPGAP	0
-#define S6_GMAC_MACIPGIFG_B2BINTERPGAP_MASK	0x7F
-#define S6_GMAC_MACIPGIFG_MINIFGENFORCE	8
-#define S6_GMAC_MACIPGIFG_B2BINTERPGAP2	16
-#define S6_GMAC_MACIPGIFG_B2BINTERPGAP1	24
-#define S6_GMAC_MACHALFDUPLEX	0x00C
-#define S6_GMAC_MACHALFDUPLEX_COLLISWIN	0
-#define S6_GMAC_MACHALFDUPLEX_COLLISWIN_MASK	0x3F
-#define S6_GMAC_MACHALFDUPLEX_RETXMAX	12
-#define S6_GMAC_MACHALFDUPLEX_RETXMAX_MASK	0x0F
-#define S6_GMAC_MACHALFDUPLEX_EXCESSDEF	16
-#define S6_GMAC_MACHALFDUPLEX_NOBACKOFF	17
-#define S6_GMAC_MACHALFDUPLEX_BPNOBCKOF	18
-#define S6_GMAC_MACHALFDUPLEX_ALTBEBENA	19
-#define S6_GMAC_MACHALFDUPLEX_ALTBEBTRN	20
-#define S6_GMAC_MACHALFDUPLEX_ALTBEBTR_MASK	0x0F
-#define S6_GMAC_MACMAXFRAMELEN	0x010
-#define S6_GMAC_MACMIICONF	0x020
-#define S6_GMAC_MACMIICONF_CSEL		0
-#define S6_GMAC_MACMIICONF_CSEL_DIV10		0
-#define S6_GMAC_MACMIICONF_CSEL_DIV12		1
-#define S6_GMAC_MACMIICONF_CSEL_DIV14		2
-#define S6_GMAC_MACMIICONF_CSEL_DIV18		3
-#define S6_GMAC_MACMIICONF_CSEL_DIV24		4
-#define S6_GMAC_MACMIICONF_CSEL_DIV34		5
-#define S6_GMAC_MACMIICONF_CSEL_DIV68		6
-#define S6_GMAC_MACMIICONF_CSEL_DIV168		7
-#define S6_GMAC_MACMIICONF_CSEL_MASK		7
-#define S6_GMAC_MACMIICONF_PREAMBLESUPR	4
-#define S6_GMAC_MACMIICONF_SCANAUTOINCR	5
-#define S6_GMAC_MACMIICMD	0x024
-#define S6_GMAC_MACMIICMD_READ		0
-#define S6_GMAC_MACMIICMD_SCAN		1
-#define S6_GMAC_MACMIIADDR	0x028
-#define S6_GMAC_MACMIIADDR_REG		0
-#define S6_GMAC_MACMIIADDR_REG_MASK		0x1F
-#define S6_GMAC_MACMIIADDR_PHY		8
-#define S6_GMAC_MACMIIADDR_PHY_MASK		0x1F
-#define S6_GMAC_MACMIICTRL	0x02C
-#define S6_GMAC_MACMIISTAT	0x030
-#define S6_GMAC_MACMIIINDI	0x034
-#define S6_GMAC_MACMIIINDI_BUSY		0
-#define S6_GMAC_MACMIIINDI_SCAN		1
-#define S6_GMAC_MACMIIINDI_INVAL	2
-#define S6_GMAC_MACINTERFSTAT	0x03C
-#define S6_GMAC_MACINTERFSTAT_LINKFAIL	3
-#define S6_GMAC_MACINTERFSTAT_EXCESSDEF	9
-#define S6_GMAC_MACSTATADDR1	0x040
-#define S6_GMAC_MACSTATADDR2	0x044
-
-#define S6_GMAC_FIFOCONF0	0x048
-#define S6_GMAC_FIFOCONF0_HSTRSTWT	0
-#define S6_GMAC_FIFOCONF0_HSTRSTSR	1
-#define S6_GMAC_FIFOCONF0_HSTRSTFR	2
-#define S6_GMAC_FIFOCONF0_HSTRSTST	3
-#define S6_GMAC_FIFOCONF0_HSTRSTFT	4
-#define S6_GMAC_FIFOCONF0_WTMENREQ	8
-#define S6_GMAC_FIFOCONF0_SRFENREQ	9
-#define S6_GMAC_FIFOCONF0_FRFENREQ	10
-#define S6_GMAC_FIFOCONF0_STFENREQ	11
-#define S6_GMAC_FIFOCONF0_FTFENREQ	12
-#define S6_GMAC_FIFOCONF0_WTMENRPLY	16
-#define S6_GMAC_FIFOCONF0_SRFENRPLY	17
-#define S6_GMAC_FIFOCONF0_FRFENRPLY	18
-#define S6_GMAC_FIFOCONF0_STFENRPLY	19
-#define S6_GMAC_FIFOCONF0_FTFENRPLY	20
-#define S6_GMAC_FIFOCONF1	0x04C
-#define S6_GMAC_FIFOCONF2	0x050
-#define S6_GMAC_FIFOCONF2_CFGLWM	0
-#define S6_GMAC_FIFOCONF2_CFGHWM	16
-#define S6_GMAC_FIFOCONF3	0x054
-#define S6_GMAC_FIFOCONF3_CFGFTTH	0
-#define S6_GMAC_FIFOCONF3_CFGHWMFT	16
-#define S6_GMAC_FIFOCONF4	0x058
-#define S6_GMAC_FIFOCONF_RSV_PREVDROP	0
-#define S6_GMAC_FIFOCONF_RSV_RUNT	1
-#define S6_GMAC_FIFOCONF_RSV_FALSECAR	2
-#define S6_GMAC_FIFOCONF_RSV_CODEERR	3
-#define S6_GMAC_FIFOCONF_RSV_CRCERR	4
-#define S6_GMAC_FIFOCONF_RSV_LENGTHERR	5
-#define S6_GMAC_FIFOCONF_RSV_LENRANGE	6
-#define S6_GMAC_FIFOCONF_RSV_OK		7
-#define S6_GMAC_FIFOCONF_RSV_MULTICAST	8
-#define S6_GMAC_FIFOCONF_RSV_BROADCAST	9
-#define S6_GMAC_FIFOCONF_RSV_DRIBBLE	10
-#define S6_GMAC_FIFOCONF_RSV_CTRLFRAME	11
-#define S6_GMAC_FIFOCONF_RSV_PAUSECTRL	12
-#define S6_GMAC_FIFOCONF_RSV_UNOPCODE	13
-#define S6_GMAC_FIFOCONF_RSV_VLANTAG	14
-#define S6_GMAC_FIFOCONF_RSV_LONGEVENT	15
-#define S6_GMAC_FIFOCONF_RSV_TRUNCATED	16
-#define S6_GMAC_FIFOCONF_RSV_MASK		0x3FFFF
-#define S6_GMAC_FIFOCONF5	0x05C
-#define S6_GMAC_FIFOCONF5_DROPLT64	18
-#define S6_GMAC_FIFOCONF5_CFGBYTM	19
-#define S6_GMAC_FIFOCONF5_RXDROPSIZE	20
-#define S6_GMAC_FIFOCONF5_RXDROPSIZE_MASK	0xF
-
-#define S6_GMAC_STAT_REGS	0x080
-#define S6_GMAC_STAT_SIZE_MIN		12
-#define S6_GMAC_STATTR64	0x080
-#define S6_GMAC_STATTR64_SIZE		18
-#define S6_GMAC_STATTR127	0x084
-#define S6_GMAC_STATTR127_SIZE		18
-#define S6_GMAC_STATTR255	0x088
-#define S6_GMAC_STATTR255_SIZE		18
-#define S6_GMAC_STATTR511	0x08C
-#define S6_GMAC_STATTR511_SIZE		18
-#define S6_GMAC_STATTR1K	0x090
-#define S6_GMAC_STATTR1K_SIZE		18
-#define S6_GMAC_STATTRMAX	0x094
-#define S6_GMAC_STATTRMAX_SIZE		18
-#define S6_GMAC_STATTRMGV	0x098
-#define S6_GMAC_STATTRMGV_SIZE		18
-#define S6_GMAC_STATRBYT	0x09C
-#define S6_GMAC_STATRBYT_SIZE		24
-#define S6_GMAC_STATRPKT	0x0A0
-#define S6_GMAC_STATRPKT_SIZE		18
-#define S6_GMAC_STATRFCS	0x0A4
-#define S6_GMAC_STATRFCS_SIZE		12
-#define S6_GMAC_STATRMCA	0x0A8
-#define S6_GMAC_STATRMCA_SIZE		18
-#define S6_GMAC_STATRBCA	0x0AC
-#define S6_GMAC_STATRBCA_SIZE		22
-#define S6_GMAC_STATRXCF	0x0B0
-#define S6_GMAC_STATRXCF_SIZE		18
-#define S6_GMAC_STATRXPF	0x0B4
-#define S6_GMAC_STATRXPF_SIZE		12
-#define S6_GMAC_STATRXUO	0x0B8
-#define S6_GMAC_STATRXUO_SIZE		12
-#define S6_GMAC_STATRALN	0x0BC
-#define S6_GMAC_STATRALN_SIZE		12
-#define S6_GMAC_STATRFLR	0x0C0
-#define S6_GMAC_STATRFLR_SIZE		16
-#define S6_GMAC_STATRCDE	0x0C4
-#define S6_GMAC_STATRCDE_SIZE		12
-#define S6_GMAC_STATRCSE	0x0C8
-#define S6_GMAC_STATRCSE_SIZE		12
-#define S6_GMAC_STATRUND	0x0CC
-#define S6_GMAC_STATRUND_SIZE		12
-#define S6_GMAC_STATROVR	0x0D0
-#define S6_GMAC_STATROVR_SIZE		12
-#define S6_GMAC_STATRFRG	0x0D4
-#define S6_GMAC_STATRFRG_SIZE		12
-#define S6_GMAC_STATRJBR	0x0D8
-#define S6_GMAC_STATRJBR_SIZE		12
-#define S6_GMAC_STATRDRP	0x0DC
-#define S6_GMAC_STATRDRP_SIZE		12
-#define S6_GMAC_STATTBYT	0x0E0
-#define S6_GMAC_STATTBYT_SIZE		24
-#define S6_GMAC_STATTPKT	0x0E4
-#define S6_GMAC_STATTPKT_SIZE		18
-#define S6_GMAC_STATTMCA	0x0E8
-#define S6_GMAC_STATTMCA_SIZE		18
-#define S6_GMAC_STATTBCA	0x0EC
-#define S6_GMAC_STATTBCA_SIZE		18
-#define S6_GMAC_STATTXPF	0x0F0
-#define S6_GMAC_STATTXPF_SIZE		12
-#define S6_GMAC_STATTDFR	0x0F4
-#define S6_GMAC_STATTDFR_SIZE		12
-#define S6_GMAC_STATTEDF	0x0F8
-#define S6_GMAC_STATTEDF_SIZE		12
-#define S6_GMAC_STATTSCL	0x0FC
-#define S6_GMAC_STATTSCL_SIZE		12
-#define S6_GMAC_STATTMCL	0x100
-#define S6_GMAC_STATTMCL_SIZE		12
-#define S6_GMAC_STATTLCL	0x104
-#define S6_GMAC_STATTLCL_SIZE		12
-#define S6_GMAC_STATTXCL	0x108
-#define S6_GMAC_STATTXCL_SIZE		12
-#define S6_GMAC_STATTNCL	0x10C
-#define S6_GMAC_STATTNCL_SIZE		13
-#define S6_GMAC_STATTPFH	0x110
-#define S6_GMAC_STATTPFH_SIZE		12
-#define S6_GMAC_STATTDRP	0x114
-#define S6_GMAC_STATTDRP_SIZE		12
-#define S6_GMAC_STATTJBR	0x118
-#define S6_GMAC_STATTJBR_SIZE		12
-#define S6_GMAC_STATTFCS	0x11C
-#define S6_GMAC_STATTFCS_SIZE		12
-#define S6_GMAC_STATTXCF	0x120
-#define S6_GMAC_STATTXCF_SIZE		12
-#define S6_GMAC_STATTOVR	0x124
-#define S6_GMAC_STATTOVR_SIZE		12
-#define S6_GMAC_STATTUND	0x128
-#define S6_GMAC_STATTUND_SIZE		12
-#define S6_GMAC_STATTFRG	0x12C
-#define S6_GMAC_STATTFRG_SIZE		12
-#define S6_GMAC_STATCARRY(n)	(0x130 + 4*(n))
-#define S6_GMAC_STATCARRYMSK(n)	(0x138 + 4*(n))
-#define S6_GMAC_STATCARRY1_RDRP		0
-#define S6_GMAC_STATCARRY1_RJBR		1
-#define S6_GMAC_STATCARRY1_RFRG		2
-#define S6_GMAC_STATCARRY1_ROVR		3
-#define S6_GMAC_STATCARRY1_RUND		4
-#define S6_GMAC_STATCARRY1_RCSE		5
-#define S6_GMAC_STATCARRY1_RCDE		6
-#define S6_GMAC_STATCARRY1_RFLR		7
-#define S6_GMAC_STATCARRY1_RALN		8
-#define S6_GMAC_STATCARRY1_RXUO		9
-#define S6_GMAC_STATCARRY1_RXPF		10
-#define S6_GMAC_STATCARRY1_RXCF		11
-#define S6_GMAC_STATCARRY1_RBCA		12
-#define S6_GMAC_STATCARRY1_RMCA		13
-#define S6_GMAC_STATCARRY1_RFCS		14
-#define S6_GMAC_STATCARRY1_RPKT		15
-#define S6_GMAC_STATCARRY1_RBYT		16
-#define S6_GMAC_STATCARRY1_TRMGV	25
-#define S6_GMAC_STATCARRY1_TRMAX	26
-#define S6_GMAC_STATCARRY1_TR1K		27
-#define S6_GMAC_STATCARRY1_TR511	28
-#define S6_GMAC_STATCARRY1_TR255	29
-#define S6_GMAC_STATCARRY1_TR127	30
-#define S6_GMAC_STATCARRY1_TR64		31
-#define S6_GMAC_STATCARRY2_TDRP		0
-#define S6_GMAC_STATCARRY2_TPFH		1
-#define S6_GMAC_STATCARRY2_TNCL		2
-#define S6_GMAC_STATCARRY2_TXCL		3
-#define S6_GMAC_STATCARRY2_TLCL		4
-#define S6_GMAC_STATCARRY2_TMCL		5
-#define S6_GMAC_STATCARRY2_TSCL		6
-#define S6_GMAC_STATCARRY2_TEDF		7
-#define S6_GMAC_STATCARRY2_TDFR		8
-#define S6_GMAC_STATCARRY2_TXPF		9
-#define S6_GMAC_STATCARRY2_TBCA		10
-#define S6_GMAC_STATCARRY2_TMCA		11
-#define S6_GMAC_STATCARRY2_TPKT		12
-#define S6_GMAC_STATCARRY2_TBYT		13
-#define S6_GMAC_STATCARRY2_TFRG		14
-#define S6_GMAC_STATCARRY2_TUND		15
-#define S6_GMAC_STATCARRY2_TOVR		16
-#define S6_GMAC_STATCARRY2_TXCF		17
-#define S6_GMAC_STATCARRY2_TFCS		18
-#define S6_GMAC_STATCARRY2_TJBR		19
-
-#define S6_GMAC_HOST_PBLKCTRL	0x140
-#define S6_GMAC_HOST_PBLKCTRL_TXENA	0
-#define S6_GMAC_HOST_PBLKCTRL_RXENA	1
-#define S6_GMAC_HOST_PBLKCTRL_TXSRES	2
-#define S6_GMAC_HOST_PBLKCTRL_RXSRES	3
-#define S6_GMAC_HOST_PBLKCTRL_TXBSIZ	8
-#define S6_GMAC_HOST_PBLKCTRL_RXBSIZ	12
-#define S6_GMAC_HOST_PBLKCTRL_SIZ_16		4
-#define S6_GMAC_HOST_PBLKCTRL_SIZ_32		5
-#define S6_GMAC_HOST_PBLKCTRL_SIZ_64		6
-#define S6_GMAC_HOST_PBLKCTRL_SIZ_128		7
-#define S6_GMAC_HOST_PBLKCTRL_SIZ_MASK		0xF
-#define S6_GMAC_HOST_PBLKCTRL_STATENA	16
-#define S6_GMAC_HOST_PBLKCTRL_STATAUTOZ	17
-#define S6_GMAC_HOST_PBLKCTRL_STATCLEAR	18
-#define S6_GMAC_HOST_PBLKCTRL_RGMII	19
-#define S6_GMAC_HOST_INTMASK	0x144
-#define S6_GMAC_HOST_INTSTAT	0x148
-#define S6_GMAC_HOST_INT_TXBURSTOVER	3
-#define S6_GMAC_HOST_INT_TXPREWOVER	4
-#define S6_GMAC_HOST_INT_RXBURSTUNDER	5
-#define S6_GMAC_HOST_INT_RXPOSTRFULL	6
-#define S6_GMAC_HOST_INT_RXPOSTRUNDER	7
-#define S6_GMAC_HOST_RXFIFOHWM	0x14C
-#define S6_GMAC_HOST_CTRLFRAMXP	0x150
-#define S6_GMAC_HOST_DSTADDRLO(n) (0x160 + 8*(n))
-#define S6_GMAC_HOST_DSTADDRHI(n) (0x164 + 8*(n))
-#define S6_GMAC_HOST_DSTMASKLO(n) (0x180 + 8*(n))
-#define S6_GMAC_HOST_DSTMASKHI(n) (0x184 + 8*(n))
-
-#define S6_GMAC_BURST_PREWR	0x1B0
-#define S6_GMAC_BURST_PREWR_LEN		0
-#define S6_GMAC_BURST_PREWR_LEN_MASK		((1 << 20) - 1)
-#define S6_GMAC_BURST_PREWR_CFE		20
-#define S6_GMAC_BURST_PREWR_PPE		21
-#define S6_GMAC_BURST_PREWR_FCS		22
-#define S6_GMAC_BURST_PREWR_PAD		23
-#define S6_GMAC_BURST_POSTRD	0x1D0
-#define S6_GMAC_BURST_POSTRD_LEN	0
-#define S6_GMAC_BURST_POSTRD_LEN_MASK		((1 << 20) - 1)
-#define S6_GMAC_BURST_POSTRD_DROP	20
-
-
-/* data handling */
-
-#define S6_NUM_TX_SKB	8	/* must be larger than TX fifo size */
-#define S6_NUM_RX_SKB	16
-#define S6_MAX_FRLEN	1536
-
-struct s6gmac {
-	u32 reg;
-	u32 tx_dma;
-	u32 rx_dma;
-	u32 io;
-	u8 tx_chan;
-	u8 rx_chan;
-	spinlock_t lock;
-	u8 tx_skb_i, tx_skb_o;
-	u8 rx_skb_i, rx_skb_o;
-	struct sk_buff *tx_skb[S6_NUM_TX_SKB];
-	struct sk_buff *rx_skb[S6_NUM_RX_SKB];
-	unsigned long carry[sizeof(struct net_device_stats) / sizeof(long)];
-	unsigned long stats[sizeof(struct net_device_stats) / sizeof(long)];
-	struct phy_device *phydev;
-	struct {
-		struct mii_bus *bus;
-		int irq[PHY_MAX_ADDR];
-	} mii;
-	struct {
-		unsigned int mbit;
-		u8 giga;
-		u8 isup;
-		u8 full;
-	} link;
-};
-
-static void s6gmac_rx_fillfifo(struct net_device *dev)
-{
-	struct s6gmac *pd = netdev_priv(dev);
-	struct sk_buff *skb;
-	while ((((u8)(pd->rx_skb_i - pd->rx_skb_o)) < S6_NUM_RX_SKB) &&
-	       (!s6dmac_fifo_full(pd->rx_dma, pd->rx_chan)) &&
-	       (skb = netdev_alloc_skb(dev, S6_MAX_FRLEN + 2))) {
-		pd->rx_skb[(pd->rx_skb_i++) % S6_NUM_RX_SKB] = skb;
-		s6dmac_put_fifo_cache(pd->rx_dma, pd->rx_chan,
-			pd->io, (u32)skb->data, S6_MAX_FRLEN);
-	}
-}
-
-static void s6gmac_rx_interrupt(struct net_device *dev)
-{
-	struct s6gmac *pd = netdev_priv(dev);
-	u32 pfx;
-	struct sk_buff *skb;
-	while (((u8)(pd->rx_skb_i - pd->rx_skb_o)) >
-			s6dmac_pending_count(pd->rx_dma, pd->rx_chan)) {
-		skb = pd->rx_skb[(pd->rx_skb_o++) % S6_NUM_RX_SKB];
-		pfx = readl(pd->reg + S6_GMAC_BURST_POSTRD);
-		if (pfx & (1 << S6_GMAC_BURST_POSTRD_DROP)) {
-			dev_kfree_skb_irq(skb);
-		} else {
-			skb_put(skb, (pfx >> S6_GMAC_BURST_POSTRD_LEN)
-				& S6_GMAC_BURST_POSTRD_LEN_MASK);
-			skb->protocol = eth_type_trans(skb, dev);
-			skb->ip_summed = CHECKSUM_UNNECESSARY;
-			netif_rx(skb);
-		}
-	}
-}
-
-static void s6gmac_tx_interrupt(struct net_device *dev)
-{
-	struct s6gmac *pd = netdev_priv(dev);
-	while (((u8)(pd->tx_skb_i - pd->tx_skb_o)) >
-			s6dmac_pending_count(pd->tx_dma, pd->tx_chan)) {
-		dev_kfree_skb_irq(pd->tx_skb[(pd->tx_skb_o++) % S6_NUM_TX_SKB]);
-	}
-	if (!s6dmac_fifo_full(pd->tx_dma, pd->tx_chan))
-		netif_wake_queue(dev);
-}
-
-struct s6gmac_statinf {
-	unsigned reg_size : 4; /* 0: unused */
-	unsigned reg_off : 6;
-	unsigned net_index : 6;
-};
-
-#define S6_STATS_B (8 * sizeof(u32))
-#define S6_STATS_C(b, r, f) [b] = { \
-	BUILD_BUG_ON_ZERO(r##_SIZE < S6_GMAC_STAT_SIZE_MIN) + \
-	BUILD_BUG_ON_ZERO((r##_SIZE - (S6_GMAC_STAT_SIZE_MIN - 1)) \
-			>= (1<<4)) + \
-	r##_SIZE - (S6_GMAC_STAT_SIZE_MIN - 1), \
-	BUILD_BUG_ON_ZERO(((unsigned)((r - S6_GMAC_STAT_REGS) / sizeof(u32))) \
-			>= ((1<<6)-1)) + \
-	(r - S6_GMAC_STAT_REGS) / sizeof(u32), \
-	BUILD_BUG_ON_ZERO((offsetof(struct net_device_stats, f)) \
-			% sizeof(unsigned long)) + \
-	BUILD_BUG_ON_ZERO((((unsigned)(offsetof(struct net_device_stats, f)) \
-			/ sizeof(unsigned long)) >= (1<<6))) + \
-	BUILD_BUG_ON_ZERO((sizeof(((struct net_device_stats *)0)->f) \
-			!= sizeof(unsigned long))) + \
-	(offsetof(struct net_device_stats, f)) / sizeof(unsigned long)},
-
-static const struct s6gmac_statinf statinf[2][S6_STATS_B] = { {
-	S6_STATS_C(S6_GMAC_STATCARRY1_RBYT, S6_GMAC_STATRBYT, rx_bytes)
-	S6_STATS_C(S6_GMAC_STATCARRY1_RPKT, S6_GMAC_STATRPKT, rx_packets)
-	S6_STATS_C(S6_GMAC_STATCARRY1_RFCS, S6_GMAC_STATRFCS, rx_crc_errors)
-	S6_STATS_C(S6_GMAC_STATCARRY1_RMCA, S6_GMAC_STATRMCA, multicast)
-	S6_STATS_C(S6_GMAC_STATCARRY1_RALN, S6_GMAC_STATRALN, rx_frame_errors)
-	S6_STATS_C(S6_GMAC_STATCARRY1_RFLR, S6_GMAC_STATRFLR, rx_length_errors)
-	S6_STATS_C(S6_GMAC_STATCARRY1_RCDE, S6_GMAC_STATRCDE, rx_missed_errors)
-	S6_STATS_C(S6_GMAC_STATCARRY1_RUND, S6_GMAC_STATRUND, rx_length_errors)
-	S6_STATS_C(S6_GMAC_STATCARRY1_ROVR, S6_GMAC_STATROVR, rx_length_errors)
-	S6_STATS_C(S6_GMAC_STATCARRY1_RFRG, S6_GMAC_STATRFRG, rx_crc_errors)
-	S6_STATS_C(S6_GMAC_STATCARRY1_RJBR, S6_GMAC_STATRJBR, rx_crc_errors)
-	S6_STATS_C(S6_GMAC_STATCARRY1_RDRP, S6_GMAC_STATRDRP, rx_dropped)
-}, {
-	S6_STATS_C(S6_GMAC_STATCARRY2_TBYT, S6_GMAC_STATTBYT, tx_bytes)
-	S6_STATS_C(S6_GMAC_STATCARRY2_TPKT, S6_GMAC_STATTPKT, tx_packets)
-	S6_STATS_C(S6_GMAC_STATCARRY2_TEDF, S6_GMAC_STATTEDF, tx_aborted_errors)
-	S6_STATS_C(S6_GMAC_STATCARRY2_TXCL, S6_GMAC_STATTXCL, tx_aborted_errors)
-	S6_STATS_C(S6_GMAC_STATCARRY2_TNCL, S6_GMAC_STATTNCL, collisions)
-	S6_STATS_C(S6_GMAC_STATCARRY2_TDRP, S6_GMAC_STATTDRP, tx_dropped)
-	S6_STATS_C(S6_GMAC_STATCARRY2_TJBR, S6_GMAC_STATTJBR, tx_errors)
-	S6_STATS_C(S6_GMAC_STATCARRY2_TFCS, S6_GMAC_STATTFCS, tx_errors)
-	S6_STATS_C(S6_GMAC_STATCARRY2_TOVR, S6_GMAC_STATTOVR, tx_errors)
-	S6_STATS_C(S6_GMAC_STATCARRY2_TUND, S6_GMAC_STATTUND, tx_errors)
-	S6_STATS_C(S6_GMAC_STATCARRY2_TFRG, S6_GMAC_STATTFRG, tx_errors)
-} };
-
-static void s6gmac_stats_collect(struct s6gmac *pd,
-		const struct s6gmac_statinf *inf)
-{
-	int b;
-	for (b = 0; b < S6_STATS_B; b++) {
-		if (inf[b].reg_size) {
-			pd->stats[inf[b].net_index] +=
-				readl(pd->reg + S6_GMAC_STAT_REGS
-					+ sizeof(u32) * inf[b].reg_off);
-		}
-	}
-}
-
-static void s6gmac_stats_carry(struct s6gmac *pd,
-		const struct s6gmac_statinf *inf, u32 mask)
-{
-	int b;
-	while (mask) {
-		b = fls(mask) - 1;
-		mask &= ~(1 << b);
-		pd->carry[inf[b].net_index] += (1 << inf[b].reg_size);
-	}
-}
-
-static inline u32 s6gmac_stats_pending(struct s6gmac *pd, int carry)
-{
-	int r = readl(pd->reg + S6_GMAC_STATCARRY(carry)) &
-		~readl(pd->reg + S6_GMAC_STATCARRYMSK(carry));
-	return r;
-}
-
-static inline void s6gmac_stats_interrupt(struct s6gmac *pd, int carry)
-{
-	u32 mask;
-	mask = s6gmac_stats_pending(pd, carry);
-	if (mask) {
-		writel(mask, pd->reg + S6_GMAC_STATCARRY(carry));
-		s6gmac_stats_carry(pd, &statinf[carry][0], mask);
-	}
-}
-
-static irqreturn_t s6gmac_interrupt(int irq, void *dev_id)
-{
-	struct net_device *dev = (struct net_device *)dev_id;
-	struct s6gmac *pd = netdev_priv(dev);
-	if (!dev)
-		return IRQ_NONE;
-	spin_lock(&pd->lock);
-	if (s6dmac_termcnt_irq(pd->rx_dma, pd->rx_chan))
-		s6gmac_rx_interrupt(dev);
-	s6gmac_rx_fillfifo(dev);
-	if (s6dmac_termcnt_irq(pd->tx_dma, pd->tx_chan))
-		s6gmac_tx_interrupt(dev);
-	s6gmac_stats_interrupt(pd, 0);
-	s6gmac_stats_interrupt(pd, 1);
-	spin_unlock(&pd->lock);
-	return IRQ_HANDLED;
-}
-
-static inline void s6gmac_set_dstaddr(struct s6gmac *pd, int n,
-	u32 addrlo, u32 addrhi, u32 masklo, u32 maskhi)
-{
-	writel(addrlo, pd->reg + S6_GMAC_HOST_DSTADDRLO(n));
-	writel(addrhi, pd->reg + S6_GMAC_HOST_DSTADDRHI(n));
-	writel(masklo, pd->reg + S6_GMAC_HOST_DSTMASKLO(n));
-	writel(maskhi, pd->reg + S6_GMAC_HOST_DSTMASKHI(n));
-}
-
-static inline void s6gmac_stop_device(struct net_device *dev)
-{
-	struct s6gmac *pd = netdev_priv(dev);
-	writel(0, pd->reg + S6_GMAC_MACCONF1);
-}
-
-static inline void s6gmac_init_device(struct net_device *dev)
-{
-	struct s6gmac *pd = netdev_priv(dev);
-	int is_rgmii = !!(pd->phydev->supported
-		& (SUPPORTED_1000baseT_Full | SUPPORTED_1000baseT_Half));
-#if 0
-	writel(1 << S6_GMAC_MACCONF1_SYNCTX |
-		1 << S6_GMAC_MACCONF1_SYNCRX |
-		1 << S6_GMAC_MACCONF1_TXFLOWCTRL |
-		1 << S6_GMAC_MACCONF1_RXFLOWCTRL |
-		1 << S6_GMAC_MACCONF1_RESTXFUNC |
-		1 << S6_GMAC_MACCONF1_RESRXFUNC |
-		1 << S6_GMAC_MACCONF1_RESTXMACCTRL |
-		1 << S6_GMAC_MACCONF1_RESRXMACCTRL,
-		pd->reg + S6_GMAC_MACCONF1);
-#endif
-	writel(1 << S6_GMAC_MACCONF1_SOFTRES, pd->reg + S6_GMAC_MACCONF1);
-	udelay(1000);
-	writel(1 << S6_GMAC_MACCONF1_TXENA | 1 << S6_GMAC_MACCONF1_RXENA,
-		pd->reg + S6_GMAC_MACCONF1);
-	writel(1 << S6_GMAC_HOST_PBLKCTRL_TXSRES |
-		1 << S6_GMAC_HOST_PBLKCTRL_RXSRES,
-		pd->reg + S6_GMAC_HOST_PBLKCTRL);
-	writel(S6_GMAC_HOST_PBLKCTRL_SIZ_128 << S6_GMAC_HOST_PBLKCTRL_TXBSIZ |
-		S6_GMAC_HOST_PBLKCTRL_SIZ_128 << S6_GMAC_HOST_PBLKCTRL_RXBSIZ |
-		1 << S6_GMAC_HOST_PBLKCTRL_STATENA |
-		1 << S6_GMAC_HOST_PBLKCTRL_STATCLEAR |
-		is_rgmii << S6_GMAC_HOST_PBLKCTRL_RGMII,
-		pd->reg + S6_GMAC_HOST_PBLKCTRL);
-	writel(1 << S6_GMAC_MACCONF1_TXENA |
-		1 << S6_GMAC_MACCONF1_RXENA |
-		(dev->flags & IFF_LOOPBACK ? 1 : 0)
-			<< S6_GMAC_MACCONF1_LOOPBACK,
-		pd->reg + S6_GMAC_MACCONF1);
-	writel(dev->mtu && (dev->mtu < (S6_MAX_FRLEN - ETH_HLEN-ETH_FCS_LEN)) ?
-			dev->mtu+ETH_HLEN+ETH_FCS_LEN : S6_MAX_FRLEN,
-		pd->reg + S6_GMAC_MACMAXFRAMELEN);
-	writel((pd->link.full ? 1 : 0) << S6_GMAC_MACCONF2_FULL |
-		1 << S6_GMAC_MACCONF2_PADCRCENA |
-		1 << S6_GMAC_MACCONF2_LENGTHFCHK |
-		(pd->link.giga ?
-			S6_GMAC_MACCONF2_IFMODE_BYTE :
-			S6_GMAC_MACCONF2_IFMODE_NIBBLE)
-			<< S6_GMAC_MACCONF2_IFMODE |
-		7 << S6_GMAC_MACCONF2_PREAMBLELEN,
-		pd->reg + S6_GMAC_MACCONF2);
-	writel(0, pd->reg + S6_GMAC_MACSTATADDR1);
-	writel(0, pd->reg + S6_GMAC_MACSTATADDR2);
-	writel(1 << S6_GMAC_FIFOCONF0_WTMENREQ |
-		1 << S6_GMAC_FIFOCONF0_SRFENREQ |
-		1 << S6_GMAC_FIFOCONF0_FRFENREQ |
-		1 << S6_GMAC_FIFOCONF0_STFENREQ |
-		1 << S6_GMAC_FIFOCONF0_FTFENREQ,
-		pd->reg + S6_GMAC_FIFOCONF0);
-	writel(128 << S6_GMAC_FIFOCONF3_CFGFTTH |
-		128 << S6_GMAC_FIFOCONF3_CFGHWMFT,
-		pd->reg + S6_GMAC_FIFOCONF3);
-	writel((S6_GMAC_FIFOCONF_RSV_MASK & ~(
-			1 << S6_GMAC_FIFOCONF_RSV_RUNT |
-			1 << S6_GMAC_FIFOCONF_RSV_CRCERR |
-			1 << S6_GMAC_FIFOCONF_RSV_OK |
-			1 << S6_GMAC_FIFOCONF_RSV_DRIBBLE |
-			1 << S6_GMAC_FIFOCONF_RSV_CTRLFRAME |
-			1 << S6_GMAC_FIFOCONF_RSV_PAUSECTRL |
-			1 << S6_GMAC_FIFOCONF_RSV_UNOPCODE |
-			1 << S6_GMAC_FIFOCONF_RSV_TRUNCATED)) |
-		1 << S6_GMAC_FIFOCONF5_DROPLT64 |
-		pd->link.giga << S6_GMAC_FIFOCONF5_CFGBYTM |
-		1 << S6_GMAC_FIFOCONF5_RXDROPSIZE,
-		pd->reg + S6_GMAC_FIFOCONF5);
-	writel(1 << S6_GMAC_FIFOCONF_RSV_RUNT |
-		1 << S6_GMAC_FIFOCONF_RSV_CRCERR |
-		1 << S6_GMAC_FIFOCONF_RSV_DRIBBLE |
-		1 << S6_GMAC_FIFOCONF_RSV_CTRLFRAME |
-		1 << S6_GMAC_FIFOCONF_RSV_PAUSECTRL |
-		1 << S6_GMAC_FIFOCONF_RSV_UNOPCODE |
-		1 << S6_GMAC_FIFOCONF_RSV_TRUNCATED,
-		pd->reg + S6_GMAC_FIFOCONF4);
-	s6gmac_set_dstaddr(pd, 0,
-		0xFFFFFFFF, 0x0000FFFF, 0xFFFFFFFF, 0x0000FFFF);
-	s6gmac_set_dstaddr(pd, 1,
-		dev->dev_addr[5] |
-		dev->dev_addr[4] << 8 |
-		dev->dev_addr[3] << 16 |
-		dev->dev_addr[2] << 24,
-		dev->dev_addr[1] |
-		dev->dev_addr[0] << 8,
-		0xFFFFFFFF, 0x0000FFFF);
-	s6gmac_set_dstaddr(pd, 2,
-		0x00000000, 0x00000100, 0x00000000, 0x00000100);
-	s6gmac_set_dstaddr(pd, 3,
-		0x00000000, 0x00000000, 0x00000000, 0x00000000);
-	writel(1 << S6_GMAC_HOST_PBLKCTRL_TXENA |
-		1 << S6_GMAC_HOST_PBLKCTRL_RXENA |
-		S6_GMAC_HOST_PBLKCTRL_SIZ_128 << S6_GMAC_HOST_PBLKCTRL_TXBSIZ |
-		S6_GMAC_HOST_PBLKCTRL_SIZ_128 << S6_GMAC_HOST_PBLKCTRL_RXBSIZ |
-		1 << S6_GMAC_HOST_PBLKCTRL_STATENA |
-		1 << S6_GMAC_HOST_PBLKCTRL_STATCLEAR |
-		is_rgmii << S6_GMAC_HOST_PBLKCTRL_RGMII,
-		pd->reg + S6_GMAC_HOST_PBLKCTRL);
-}
-
-static void s6mii_enable(struct s6gmac *pd)
-{
-	writel(readl(pd->reg + S6_GMAC_MACCONF1) &
-		~(1 << S6_GMAC_MACCONF1_SOFTRES),
-		pd->reg + S6_GMAC_MACCONF1);
-	writel((readl(pd->reg + S6_GMAC_MACMIICONF)
-		& ~(S6_GMAC_MACMIICONF_CSEL_MASK << S6_GMAC_MACMIICONF_CSEL))
-		| (S6_GMAC_MACMIICONF_CSEL_DIV168 << S6_GMAC_MACMIICONF_CSEL),
-		pd->reg + S6_GMAC_MACMIICONF);
-}
-
-static int s6mii_busy(struct s6gmac *pd, int tmo)
-{
-	while (readl(pd->reg + S6_GMAC_MACMIIINDI)) {
-		if (--tmo == 0)
-			return -ETIME;
-		udelay(64);
-	}
-	return 0;
-}
-
-static int s6mii_read(struct mii_bus *bus, int phy_addr, int regnum)
-{
-	struct s6gmac *pd = bus->priv;
-	s6mii_enable(pd);
-	if (s6mii_busy(pd, 256))
-		return -ETIME;
-	writel(phy_addr << S6_GMAC_MACMIIADDR_PHY |
-		regnum << S6_GMAC_MACMIIADDR_REG,
-		pd->reg + S6_GMAC_MACMIIADDR);
-	writel(1 << S6_GMAC_MACMIICMD_READ, pd->reg + S6_GMAC_MACMIICMD);
-	writel(0, pd->reg + S6_GMAC_MACMIICMD);
-	if (s6mii_busy(pd, 256))
-		return -ETIME;
-	return (u16)readl(pd->reg + S6_GMAC_MACMIISTAT);
-}
-
-static int s6mii_write(struct mii_bus *bus, int phy_addr, int regnum, u16 value)
-{
-	struct s6gmac *pd = bus->priv;
-	s6mii_enable(pd);
-	if (s6mii_busy(pd, 256))
-		return -ETIME;
-	writel(phy_addr << S6_GMAC_MACMIIADDR_PHY |
-		regnum << S6_GMAC_MACMIIADDR_REG,
-		pd->reg + S6_GMAC_MACMIIADDR);
-	writel(value, pd->reg + S6_GMAC_MACMIICTRL);
-	if (s6mii_busy(pd, 256))
-		return -ETIME;
-	return 0;
-}
-
-static int s6mii_reset(struct mii_bus *bus)
-{
-	struct s6gmac *pd = bus->priv;
-	s6mii_enable(pd);
-	if (s6mii_busy(pd, PHY_INIT_TIMEOUT))
-		return -ETIME;
-	return 0;
-}
-
-static void s6gmac_set_rgmii_txclock(struct s6gmac *pd)
-{
-	u32 pllsel = readl(S6_REG_GREG1 + S6_GREG1_PLLSEL);
-	pllsel &= ~(S6_GREG1_PLLSEL_GMAC_MASK << S6_GREG1_PLLSEL_GMAC);
-	switch (pd->link.mbit) {
-	case 10:
-		pllsel |= S6_GREG1_PLLSEL_GMAC_2500KHZ << S6_GREG1_PLLSEL_GMAC;
-		break;
-	case 100:
-		pllsel |= S6_GREG1_PLLSEL_GMAC_25MHZ << S6_GREG1_PLLSEL_GMAC;
-		break;
-	case 1000:
-		pllsel |= S6_GREG1_PLLSEL_GMAC_125MHZ << S6_GREG1_PLLSEL_GMAC;
-		break;
-	default:
-		return;
-	}
-	writel(pllsel, S6_REG_GREG1 + S6_GREG1_PLLSEL);
-}
-
-static inline void s6gmac_linkisup(struct net_device *dev, int isup)
-{
-	struct s6gmac *pd = netdev_priv(dev);
-	struct phy_device *phydev = pd->phydev;
-
-	pd->link.full = phydev->duplex;
-	pd->link.giga = (phydev->speed == 1000);
-	if (pd->link.mbit != phydev->speed) {
-		pd->link.mbit = phydev->speed;
-		s6gmac_set_rgmii_txclock(pd);
-	}
-	pd->link.isup = isup;
-	if (isup)
-		netif_carrier_on(dev);
-	phy_print_status(phydev);
-}
-
-static void s6gmac_adjust_link(struct net_device *dev)
-{
-	struct s6gmac *pd = netdev_priv(dev);
-	struct phy_device *phydev = pd->phydev;
-	if (pd->link.isup &&
-			(!phydev->link ||
-			(pd->link.mbit != phydev->speed) ||
-			(pd->link.full != phydev->duplex))) {
-		pd->link.isup = 0;
-		netif_tx_disable(dev);
-		if (!phydev->link) {
-			netif_carrier_off(dev);
-			phy_print_status(phydev);
-		}
-	}
-	if (!pd->link.isup && phydev->link) {
-		if (pd->link.full != phydev->duplex) {
-			u32 maccfg = readl(pd->reg + S6_GMAC_MACCONF2);
-			if (phydev->duplex)
-				maccfg |= 1 << S6_GMAC_MACCONF2_FULL;
-			else
-				maccfg &= ~(1 << S6_GMAC_MACCONF2_FULL);
-			writel(maccfg, pd->reg + S6_GMAC_MACCONF2);
-		}
-
-		if (pd->link.giga != (phydev->speed == 1000)) {
-			u32 fifocfg = readl(pd->reg + S6_GMAC_FIFOCONF5);
-			u32 maccfg = readl(pd->reg + S6_GMAC_MACCONF2);
-			maccfg &= ~(S6_GMAC_MACCONF2_IFMODE_MASK
-				     << S6_GMAC_MACCONF2_IFMODE);
-			if (phydev->speed == 1000) {
-				fifocfg |= 1 << S6_GMAC_FIFOCONF5_CFGBYTM;
-				maccfg |= S6_GMAC_MACCONF2_IFMODE_BYTE
-					   << S6_GMAC_MACCONF2_IFMODE;
-			} else {
-				fifocfg &= ~(1 << S6_GMAC_FIFOCONF5_CFGBYTM);
-				maccfg |= S6_GMAC_MACCONF2_IFMODE_NIBBLE
-					   << S6_GMAC_MACCONF2_IFMODE;
-			}
-			writel(fifocfg, pd->reg + S6_GMAC_FIFOCONF5);
-			writel(maccfg, pd->reg + S6_GMAC_MACCONF2);
-		}
-
-		if (!s6dmac_fifo_full(pd->tx_dma, pd->tx_chan))
-			netif_wake_queue(dev);
-		s6gmac_linkisup(dev, 1);
-	}
-}
-
-static inline int s6gmac_phy_start(struct net_device *dev)
-{
-	struct s6gmac *pd = netdev_priv(dev);
-	int i = 0;
-	struct phy_device *p = NULL;
-	while ((i < PHY_MAX_ADDR) && (!(p = pd->mii.bus->phy_map[i])))
-		i++;
-	p = phy_connect(dev, dev_name(&p->dev), &s6gmac_adjust_link,
-			PHY_INTERFACE_MODE_RGMII);
-	if (IS_ERR(p)) {
-		printk(KERN_ERR "%s: Could not attach to PHY\n", dev->name);
-		return PTR_ERR(p);
-	}
-	p->supported &= PHY_GBIT_FEATURES;
-	p->advertising = p->supported;
-	pd->phydev = p;
-	return 0;
-}
-
-static inline void s6gmac_init_stats(struct net_device *dev)
-{
-	struct s6gmac *pd = netdev_priv(dev);
-	u32 mask;
-	mask =	1 << S6_GMAC_STATCARRY1_RDRP |
-		1 << S6_GMAC_STATCARRY1_RJBR |
-		1 << S6_GMAC_STATCARRY1_RFRG |
-		1 << S6_GMAC_STATCARRY1_ROVR |
-		1 << S6_GMAC_STATCARRY1_RUND |
-		1 << S6_GMAC_STATCARRY1_RCDE |
-		1 << S6_GMAC_STATCARRY1_RFLR |
-		1 << S6_GMAC_STATCARRY1_RALN |
-		1 << S6_GMAC_STATCARRY1_RMCA |
-		1 << S6_GMAC_STATCARRY1_RFCS |
-		1 << S6_GMAC_STATCARRY1_RPKT |
-		1 << S6_GMAC_STATCARRY1_RBYT;
-	writel(mask, pd->reg + S6_GMAC_STATCARRY(0));
-	writel(~mask, pd->reg + S6_GMAC_STATCARRYMSK(0));
-	mask =	1 << S6_GMAC_STATCARRY2_TDRP |
-		1 << S6_GMAC_STATCARRY2_TNCL |
-		1 << S6_GMAC_STATCARRY2_TXCL |
-		1 << S6_GMAC_STATCARRY2_TEDF |
-		1 << S6_GMAC_STATCARRY2_TPKT |
-		1 << S6_GMAC_STATCARRY2_TBYT |
-		1 << S6_GMAC_STATCARRY2_TFRG |
-		1 << S6_GMAC_STATCARRY2_TUND |
-		1 << S6_GMAC_STATCARRY2_TOVR |
-		1 << S6_GMAC_STATCARRY2_TFCS |
-		1 << S6_GMAC_STATCARRY2_TJBR;
-	writel(mask, pd->reg + S6_GMAC_STATCARRY(1));
-	writel(~mask, pd->reg + S6_GMAC_STATCARRYMSK(1));
-}
-
-static inline void s6gmac_init_dmac(struct net_device *dev)
-{
-	struct s6gmac *pd = netdev_priv(dev);
-	s6dmac_disable_chan(pd->tx_dma, pd->tx_chan);
-	s6dmac_disable_chan(pd->rx_dma, pd->rx_chan);
-	s6dmac_disable_error_irqs(pd->tx_dma, 1 << S6_HIFDMA_GMACTX);
-	s6dmac_disable_error_irqs(pd->rx_dma, 1 << S6_HIFDMA_GMACRX);
-}
-
-static int s6gmac_tx(struct sk_buff *skb, struct net_device *dev)
-{
-	struct s6gmac *pd = netdev_priv(dev);
-	unsigned long flags;
-
-	spin_lock_irqsave(&pd->lock, flags);
-	writel(skb->len << S6_GMAC_BURST_PREWR_LEN |
-		0 << S6_GMAC_BURST_PREWR_CFE |
-		1 << S6_GMAC_BURST_PREWR_PPE |
-		1 << S6_GMAC_BURST_PREWR_FCS |
-		((skb->len < ETH_ZLEN) ? 1 : 0) << S6_GMAC_BURST_PREWR_PAD,
-		pd->reg + S6_GMAC_BURST_PREWR);
-	s6dmac_put_fifo_cache(pd->tx_dma, pd->tx_chan,
-		(u32)skb->data, pd->io, skb->len);
-	if (s6dmac_fifo_full(pd->tx_dma, pd->tx_chan))
-		netif_stop_queue(dev);
-	if (((u8)(pd->tx_skb_i - pd->tx_skb_o)) >= S6_NUM_TX_SKB) {
-		printk(KERN_ERR "GMAC BUG: skb tx ring overflow [%x, %x]\n",
-			pd->tx_skb_o, pd->tx_skb_i);
-		BUG();
-	}
-	pd->tx_skb[(pd->tx_skb_i++) % S6_NUM_TX_SKB] = skb;
-	spin_unlock_irqrestore(&pd->lock, flags);
-	return 0;
-}
-
-static void s6gmac_tx_timeout(struct net_device *dev)
-{
-	struct s6gmac *pd = netdev_priv(dev);
-	unsigned long flags;
-	spin_lock_irqsave(&pd->lock, flags);
-	s6gmac_tx_interrupt(dev);
-	spin_unlock_irqrestore(&pd->lock, flags);
-}
-
-static int s6gmac_open(struct net_device *dev)
-{
-	struct s6gmac *pd = netdev_priv(dev);
-	unsigned long flags;
-	phy_read_status(pd->phydev);
-	spin_lock_irqsave(&pd->lock, flags);
-	pd->link.mbit = 0;
-	s6gmac_linkisup(dev, pd->phydev->link);
-	s6gmac_init_device(dev);
-	s6gmac_init_stats(dev);
-	s6gmac_init_dmac(dev);
-	s6gmac_rx_fillfifo(dev);
-	s6dmac_enable_chan(pd->rx_dma, pd->rx_chan,
-		2, 1, 0, 1, 0, 0, 0, 7, -1, 2, 0, 1);
-	s6dmac_enable_chan(pd->tx_dma, pd->tx_chan,
-		2, 0, 1, 0, 0, 0, 0, 7, -1, 2, 0, 1);
-	writel(0 << S6_GMAC_HOST_INT_TXBURSTOVER |
-		0 << S6_GMAC_HOST_INT_TXPREWOVER |
-		0 << S6_GMAC_HOST_INT_RXBURSTUNDER |
-		0 << S6_GMAC_HOST_INT_RXPOSTRFULL |
-		0 << S6_GMAC_HOST_INT_RXPOSTRUNDER,
-		pd->reg + S6_GMAC_HOST_INTMASK);
-	spin_unlock_irqrestore(&pd->lock, flags);
-	phy_start(pd->phydev);
-	netif_start_queue(dev);
-	return 0;
-}
-
-static int s6gmac_stop(struct net_device *dev)
-{
-	struct s6gmac *pd = netdev_priv(dev);
-	unsigned long flags;
-	netif_stop_queue(dev);
-	phy_stop(pd->phydev);
-	spin_lock_irqsave(&pd->lock, flags);
-	s6gmac_init_dmac(dev);
-	s6gmac_stop_device(dev);
-	while (pd->tx_skb_i != pd->tx_skb_o)
-		dev_kfree_skb(pd->tx_skb[(pd->tx_skb_o++) % S6_NUM_TX_SKB]);
-	while (pd->rx_skb_i != pd->rx_skb_o)
-		dev_kfree_skb(pd->rx_skb[(pd->rx_skb_o++) % S6_NUM_RX_SKB]);
-	spin_unlock_irqrestore(&pd->lock, flags);
-	return 0;
-}
-
-static struct net_device_stats *s6gmac_stats(struct net_device *dev)
-{
-	struct s6gmac *pd = netdev_priv(dev);
-	struct net_device_stats *st = (struct net_device_stats *)&pd->stats;
-	int i;
-	do {
-		unsigned long flags;
-		spin_lock_irqsave(&pd->lock, flags);
-		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]);
-		s6gmac_stats_collect(pd, &statinf[1][0]);
-		i = s6gmac_stats_pending(pd, 0) |
-			s6gmac_stats_pending(pd, 1);
-		spin_unlock_irqrestore(&pd->lock, flags);
-	} while (i);
-	st->rx_errors = st->rx_crc_errors +
-			st->rx_frame_errors +
-			st->rx_length_errors +
-			st->rx_missed_errors;
-	st->tx_errors += st->tx_aborted_errors;
-	return st;
-}
-
-static int s6gmac_probe(struct platform_device *pdev)
-{
-	struct net_device *dev;
-	struct s6gmac *pd;
-	int res;
-	unsigned long i;
-	struct mii_bus *mb;
-
-	dev = alloc_etherdev(sizeof(*pd));
-	if (!dev)
-		return -ENOMEM;
-
-	dev->open = s6gmac_open;
-	dev->stop = s6gmac_stop;
-	dev->hard_start_xmit = s6gmac_tx;
-	dev->tx_timeout = s6gmac_tx_timeout;
-	dev->watchdog_timeo = HZ;
-	dev->get_stats = s6gmac_stats;
-	dev->irq = platform_get_irq(pdev, 0);
-	pd = netdev_priv(dev);
-	memset(pd, 0, sizeof(*pd));
-	spin_lock_init(&pd->lock);
-	pd->reg = platform_get_resource(pdev, IORESOURCE_MEM, 0)->start;
-	i = platform_get_resource(pdev, IORESOURCE_DMA, 0)->start;
-	pd->tx_dma = DMA_MASK_DMAC(i);
-	pd->tx_chan = DMA_INDEX_CHNL(i);
-	i = platform_get_resource(pdev, IORESOURCE_DMA, 1)->start;
-	pd->rx_dma = DMA_MASK_DMAC(i);
-	pd->rx_chan = DMA_INDEX_CHNL(i);
-	pd->io = platform_get_resource(pdev, IORESOURCE_IO, 0)->start;
-	res = request_irq(dev->irq, s6gmac_interrupt, 0, dev->name, dev);
-	if (res) {
-		printk(KERN_ERR DRV_PRMT "irq request failed: %d\n", dev->irq);
-		goto errirq;
-	}
-	res = register_netdev(dev);
-	if (res) {
-		printk(KERN_ERR DRV_PRMT "error registering device %s\n",
-			dev->name);
-		goto errdev;
-	}
-	mb = mdiobus_alloc();
-	if (!mb) {
-		printk(KERN_ERR DRV_PRMT "error allocating mii bus\n");
-		res = -ENOMEM;
-		goto errmii;
-	}
-	mb->name = "s6gmac_mii";
-	mb->read = s6mii_read;
-	mb->write = s6mii_write;
-	mb->reset = s6mii_reset;
-	mb->priv = pd;
-	snprintf(mb->id, MII_BUS_ID_SIZE, "%s-%x", pdev->name, pdev->id);
-	mb->phy_mask = ~(1 << 0);
-	mb->irq = &pd->mii.irq[0];
-	for (i = 0; i < PHY_MAX_ADDR; i++) {
-		int n = platform_get_irq(pdev, i + 1);
-		if (n < 0)
-			n = PHY_POLL;
-		pd->mii.irq[i] = n;
-	}
-	mdiobus_register(mb);
-	pd->mii.bus = mb;
-	res = s6gmac_phy_start(dev);
-	if (res)
-		return res;
-	platform_set_drvdata(pdev, dev);
-	return 0;
-errmii:
-	unregister_netdev(dev);
-errdev:
-	free_irq(dev->irq, dev);
-errirq:
-	free_netdev(dev);
-	return res;
-}
-
-static int s6gmac_remove(struct platform_device *pdev)
-{
-	struct net_device *dev = platform_get_drvdata(pdev);
-	if (dev) {
-		struct s6gmac *pd = netdev_priv(dev);
-		mdiobus_unregister(pd->mii.bus);
-		unregister_netdev(dev);
-		free_irq(dev->irq, dev);
-		free_netdev(dev);
-	}
-	return 0;
-}
-
-static struct platform_driver s6gmac_driver = {
-	.probe = s6gmac_probe,
-	.remove = s6gmac_remove,
-	.driver = {
-		.name = "s6gmac",
-		.owner = THIS_MODULE,
-	},
-};
-
-module_platform_driver(s6gmac_driver);
-
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("S6105 on chip Ethernet driver");
-MODULE_AUTHOR("Oskar Schirmer <oskar@scara.com>");
diff --git a/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c b/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c
index 698494481d18..b6612d6090ac 100644
--- a/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c
+++ b/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c
@@ -133,9 +133,8 @@ bool sxgbe_eee_init(struct sxgbe_priv_data * const priv)
 			return false;
 
 		priv->eee_active = 1;
-		init_timer(&priv->eee_ctrl_timer);
-		priv->eee_ctrl_timer.function = sxgbe_eee_ctrl_timer;
-		priv->eee_ctrl_timer.data = (unsigned long)priv;
+		setup_timer(&priv->eee_ctrl_timer, sxgbe_eee_ctrl_timer,
+			    (unsigned long)priv);
 		priv->eee_ctrl_timer.expires = SXGBE_LPI_TIMER(eee_timer);
 		add_timer(&priv->eee_ctrl_timer);
 
@@ -1009,10 +1008,9 @@ static void sxgbe_tx_init_coalesce(struct sxgbe_priv_data *priv)
 		struct sxgbe_tx_queue *p = priv->txq[queue_num];
 		p->tx_coal_frames =  SXGBE_TX_FRAMES;
 		p->tx_coal_timer = SXGBE_COAL_TX_TIMER;
-		init_timer(&p->txtimer);
+		setup_timer(&p->txtimer, sxgbe_tx_timer,
+			    (unsigned long)&priv->txq[queue_num]);
 		p->txtimer.expires = SXGBE_COAL_TIMER(p->tx_coal_timer);
-		p->txtimer.data = (unsigned long)&priv->txq[queue_num];
-		p->txtimer.function = sxgbe_tx_timer;
 		add_timer(&p->txtimer);
 	}
 }
diff --git a/drivers/net/ethernet/samsung/sxgbe/sxgbe_platform.c b/drivers/net/ethernet/samsung/sxgbe/sxgbe_platform.c
index 7fd6e275d1c2..866560ea9e18 100644
--- a/drivers/net/ethernet/samsung/sxgbe/sxgbe_platform.c
+++ b/drivers/net/ethernet/samsung/sxgbe/sxgbe_platform.c
@@ -233,7 +233,6 @@ static struct platform_driver sxgbe_platform_driver = {
 	.remove	= sxgbe_platform_remove,
 	.driver	= {
 		.name		= SXGBE_RESOURCE_NAME,
-		.owner		= THIS_MODULE,
 		.pm		= &sxgbe_platform_pm_ops,
 		.of_match_table	= of_match_ptr(sxgbe_dt_ids),
 	},
diff --git a/drivers/net/ethernet/seeq/sgiseeq.c b/drivers/net/ethernet/seeq/sgiseeq.c
index 69e4fd21adb4..ca7336605748 100644
--- a/drivers/net/ethernet/seeq/sgiseeq.c
+++ b/drivers/net/ethernet/seeq/sgiseeq.c
@@ -826,7 +826,6 @@ static struct platform_driver sgiseeq_driver = {
 	.remove	= __exit_p(sgiseeq_remove),
 	.driver = {
 		.name	= "sgiseeq",
-		.owner	= THIS_MODULE,
 	}
 };
 
diff --git a/drivers/net/ethernet/sgi/meth.c b/drivers/net/ethernet/sgi/meth.c
index 5564a5fa3385..5eac523b4b0c 100644
--- a/drivers/net/ethernet/sgi/meth.c
+++ b/drivers/net/ethernet/sgi/meth.c
@@ -870,7 +870,6 @@ static struct platform_driver meth_driver = {
 	.remove	= __exit_p(meth_remove),
 	.driver = {
 		.name	= "meth",
-		.owner	= THIS_MODULE,
 	}
 };
 
diff --git a/drivers/net/ethernet/smsc/Kconfig b/drivers/net/ethernet/smsc/Kconfig
index 753630f5d3d3..9468e64e6007 100644
--- a/drivers/net/ethernet/smsc/Kconfig
+++ b/drivers/net/ethernet/smsc/Kconfig
@@ -6,7 +6,7 @@ config NET_VENDOR_SMSC
 	bool "SMC (SMSC)/Western Digital devices"
 	default y
 	depends on ARM || ISA || MAC || ARM64 || MIPS || M32R || SUPERH || \
-		BLACKFIN || MN10300 || COLDFIRE || XTENSA || PCI || PCMCIA
+		BLACKFIN || MN10300 || COLDFIRE || XTENSA || NIOS2 || PCI || PCMCIA
 	---help---
 	  If you have a network (Ethernet) card belonging to this class, say Y
 	  and read the Ethernet-HOWTO, available from
@@ -39,7 +39,7 @@ config SMC91X
 	select CRC32
 	select MII
 	depends on (ARM || M32R || SUPERH || MIPS || BLACKFIN || \
-		    MN10300 || COLDFIRE || ARM64 || XTENSA)
+		    MN10300 || COLDFIRE || ARM64 || XTENSA || NIOS2) && (!OF || GPIOLIB)
 	---help---
 	  This is a driver for SMC's 91x series of Ethernet chipsets,
 	  including the SMC91C94 and the SMC91C111. Say Y if you want it
diff --git a/drivers/net/ethernet/smsc/smc911x.c b/drivers/net/ethernet/smsc/smc911x.c
index e88df9c7f1c0..bd64eb982e52 100644
--- a/drivers/net/ethernet/smsc/smc911x.c
+++ b/drivers/net/ethernet/smsc/smc911x.c
@@ -2173,7 +2173,6 @@ static struct platform_driver smc911x_driver = {
 	.resume	 = smc911x_drv_resume,
 	.driver	 = {
 		.name	 = CARDNAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/ethernet/smsc/smc91x.c b/drivers/net/ethernet/smsc/smc91x.c
index 6cc3cf6f17c8..88a55f95fe09 100644
--- a/drivers/net/ethernet/smsc/smc91x.c
+++ b/drivers/net/ethernet/smsc/smc91x.c
@@ -2472,7 +2472,6 @@ static struct platform_driver smc_driver = {
 	.remove		= smc_drv_remove,
 	.driver		= {
 		.name	= CARDNAME,
-		.owner	= THIS_MODULE,
 		.pm	= &smc_drv_pm_ops,
 		.of_match_table = of_match_ptr(smc91x_match),
 	},
diff --git a/drivers/net/ethernet/smsc/smsc911x.c b/drivers/net/ethernet/smsc/smsc911x.c
index f9c87624a0af..2965c6ae7d6e 100644
--- a/drivers/net/ethernet/smsc/smsc911x.c
+++ b/drivers/net/ethernet/smsc/smsc911x.c
@@ -2659,7 +2659,6 @@ static struct platform_driver smsc911x_driver = {
 	.remove = smsc911x_drv_remove,
 	.driver = {
 		.name	= SMSC_CHIPNAME,
-		.owner	= THIS_MODULE,
 		.pm	= SMSC911X_PM_OPS,
 		.of_match_table = of_match_ptr(smsc911x_dt_ids),
 	},
diff --git a/drivers/net/ethernet/stmicro/stmmac/Makefile b/drivers/net/ethernet/stmicro/stmmac/Makefile
index ac4d5629d905..73c2715a27f3 100644
--- a/drivers/net/ethernet/stmicro/stmmac/Makefile
+++ b/drivers/net/ethernet/stmicro/stmmac/Makefile
@@ -6,7 +6,7 @@ stmmac-objs:= stmmac_main.o stmmac_ethtool.o stmmac_mdio.o ring_mode.o	\
 
 obj-$(CONFIG_STMMAC_PLATFORM) += stmmac-platform.o
 stmmac-platform-objs:= stmmac_platform.o dwmac-meson.o dwmac-sunxi.o	\
-		       dwmac-sti.o dwmac-socfpga.o
+		       dwmac-sti.o dwmac-socfpga.o dwmac-rk.o
 
 obj-$(CONFIG_STMMAC_PCI) += stmmac-pci.o
 stmmac-pci-objs:= stmmac_pci.o
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-rk.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-rk.c
new file mode 100644
index 000000000000..35f9b86bc9e5
--- /dev/null
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-rk.c
@@ -0,0 +1,459 @@
+/**
+ * dwmac-rk.c - Rockchip RK3288 DWMAC specific glue layer
+ *
+ * Copyright (C) 2014 Chen-Zhi (Roger Chen)
+ *
+ * Chen-Zhi (Roger Chen)  <roger.chen@rock-chips.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.
+ */
+
+#include <linux/stmmac.h>
+#include <linux/bitops.h>
+#include <linux/clk.h>
+#include <linux/phy.h>
+#include <linux/of_net.h>
+#include <linux/gpio.h>
+#include <linux/of_gpio.h>
+#include <linux/of_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/delay.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
+
+struct rk_priv_data {
+	struct platform_device *pdev;
+	int phy_iface;
+	char regulator[32];
+
+	bool clk_enabled;
+	bool clock_input;
+
+	struct clk *clk_mac;
+	struct clk *clk_mac_pll;
+	struct clk *gmac_clkin;
+	struct clk *mac_clk_rx;
+	struct clk *mac_clk_tx;
+	struct clk *clk_mac_ref;
+	struct clk *clk_mac_refout;
+	struct clk *aclk_mac;
+	struct clk *pclk_mac;
+
+	int tx_delay;
+	int rx_delay;
+
+	struct regmap *grf;
+};
+
+#define HIWORD_UPDATE(val, mask, shift) \
+		((val) << (shift) | (mask) << ((shift) + 16))
+
+#define GRF_BIT(nr)	(BIT(nr) | BIT(nr+16))
+#define GRF_CLR_BIT(nr)	(BIT(nr+16))
+
+#define RK3288_GRF_SOC_CON1	0x0248
+#define RK3288_GRF_SOC_CON3	0x0250
+#define RK3288_GRF_GPIO3D_E	0x01ec
+#define RK3288_GRF_GPIO4A_E	0x01f0
+#define RK3288_GRF_GPIO4B_E	0x01f4
+
+/*RK3288_GRF_SOC_CON1*/
+#define GMAC_PHY_INTF_SEL_RGMII	(GRF_BIT(6) | GRF_CLR_BIT(7) | GRF_CLR_BIT(8))
+#define GMAC_PHY_INTF_SEL_RMII	(GRF_CLR_BIT(6) | GRF_CLR_BIT(7) | GRF_BIT(8))
+#define GMAC_FLOW_CTRL		GRF_BIT(9)
+#define GMAC_FLOW_CTRL_CLR	GRF_CLR_BIT(9)
+#define GMAC_SPEED_10M		GRF_CLR_BIT(10)
+#define GMAC_SPEED_100M		GRF_BIT(10)
+#define GMAC_RMII_CLK_25M	GRF_BIT(11)
+#define GMAC_RMII_CLK_2_5M	GRF_CLR_BIT(11)
+#define GMAC_CLK_125M		(GRF_CLR_BIT(12) | GRF_CLR_BIT(13))
+#define GMAC_CLK_25M		(GRF_BIT(12) | GRF_BIT(13))
+#define GMAC_CLK_2_5M		(GRF_CLR_BIT(12) | GRF_BIT(13))
+#define GMAC_RMII_MODE		GRF_BIT(14)
+#define GMAC_RMII_MODE_CLR	GRF_CLR_BIT(14)
+
+/*RK3288_GRF_SOC_CON3*/
+#define GMAC_TXCLK_DLY_ENABLE	GRF_BIT(14)
+#define GMAC_TXCLK_DLY_DISABLE	GRF_CLR_BIT(14)
+#define GMAC_RXCLK_DLY_ENABLE	GRF_BIT(15)
+#define GMAC_RXCLK_DLY_DISABLE	GRF_CLR_BIT(15)
+#define GMAC_CLK_RX_DL_CFG(val)	HIWORD_UPDATE(val, 0x7F, 7)
+#define GMAC_CLK_TX_DL_CFG(val)	HIWORD_UPDATE(val, 0x7F, 0)
+
+static void set_to_rgmii(struct rk_priv_data *bsp_priv,
+			 int tx_delay, int rx_delay)
+{
+	struct device *dev = &bsp_priv->pdev->dev;
+
+	if (IS_ERR(bsp_priv->grf)) {
+		dev_err(dev, "%s: Missing rockchip,grf property\n", __func__);
+		return;
+	}
+
+	regmap_write(bsp_priv->grf, RK3288_GRF_SOC_CON1,
+		     GMAC_PHY_INTF_SEL_RGMII | GMAC_RMII_MODE_CLR);
+	regmap_write(bsp_priv->grf, RK3288_GRF_SOC_CON3,
+		     GMAC_RXCLK_DLY_ENABLE | GMAC_TXCLK_DLY_ENABLE |
+		     GMAC_CLK_RX_DL_CFG(rx_delay) |
+		     GMAC_CLK_TX_DL_CFG(tx_delay));
+}
+
+static void set_to_rmii(struct rk_priv_data *bsp_priv)
+{
+	struct device *dev = &bsp_priv->pdev->dev;
+
+	if (IS_ERR(bsp_priv->grf)) {
+		dev_err(dev, "%s: Missing rockchip,grf property\n", __func__);
+		return;
+	}
+
+	regmap_write(bsp_priv->grf, RK3288_GRF_SOC_CON1,
+		     GMAC_PHY_INTF_SEL_RMII | GMAC_RMII_MODE);
+}
+
+static void set_rgmii_speed(struct rk_priv_data *bsp_priv, int speed)
+{
+	struct device *dev = &bsp_priv->pdev->dev;
+
+	if (IS_ERR(bsp_priv->grf)) {
+		dev_err(dev, "%s: Missing rockchip,grf property\n", __func__);
+		return;
+	}
+
+	if (speed == 10)
+		regmap_write(bsp_priv->grf, RK3288_GRF_SOC_CON1, GMAC_CLK_2_5M);
+	else if (speed == 100)
+		regmap_write(bsp_priv->grf, RK3288_GRF_SOC_CON1, GMAC_CLK_25M);
+	else if (speed == 1000)
+		regmap_write(bsp_priv->grf, RK3288_GRF_SOC_CON1, GMAC_CLK_125M);
+	else
+		dev_err(dev, "unknown speed value for RGMII! speed=%d", speed);
+}
+
+static void set_rmii_speed(struct rk_priv_data *bsp_priv, int speed)
+{
+	struct device *dev = &bsp_priv->pdev->dev;
+
+	if (IS_ERR(bsp_priv->grf)) {
+		dev_err(dev, "%s: Missing rockchip,grf property\n", __func__);
+		return;
+	}
+
+	if (speed == 10) {
+		regmap_write(bsp_priv->grf, RK3288_GRF_SOC_CON1,
+			     GMAC_RMII_CLK_2_5M | GMAC_SPEED_10M);
+	} else if (speed == 100) {
+		regmap_write(bsp_priv->grf, RK3288_GRF_SOC_CON1,
+			     GMAC_RMII_CLK_25M | GMAC_SPEED_100M);
+	} else {
+		dev_err(dev, "unknown speed value for RMII! speed=%d", speed);
+	}
+}
+
+static int gmac_clk_init(struct rk_priv_data *bsp_priv)
+{
+	struct device *dev = &bsp_priv->pdev->dev;
+
+	bsp_priv->clk_enabled = false;
+
+	bsp_priv->mac_clk_rx = devm_clk_get(dev, "mac_clk_rx");
+	if (IS_ERR(bsp_priv->mac_clk_rx))
+		dev_err(dev, "%s: cannot get clock %s\n",
+			__func__, "mac_clk_rx");
+
+	bsp_priv->mac_clk_tx = devm_clk_get(dev, "mac_clk_tx");
+	if (IS_ERR(bsp_priv->mac_clk_tx))
+		dev_err(dev, "%s: cannot get clock %s\n",
+			__func__, "mac_clk_tx");
+
+	bsp_priv->aclk_mac = devm_clk_get(dev, "aclk_mac");
+	if (IS_ERR(bsp_priv->aclk_mac))
+		dev_err(dev, "%s: cannot get clock %s\n",
+			__func__, "aclk_mac");
+
+	bsp_priv->pclk_mac = devm_clk_get(dev, "pclk_mac");
+	if (IS_ERR(bsp_priv->pclk_mac))
+		dev_err(dev, "%s: cannot get clock %s\n",
+			__func__, "pclk_mac");
+
+	bsp_priv->clk_mac = devm_clk_get(dev, "stmmaceth");
+	if (IS_ERR(bsp_priv->clk_mac))
+		dev_err(dev, "%s: cannot get clock %s\n",
+			__func__, "stmmaceth");
+
+	if (bsp_priv->phy_iface == PHY_INTERFACE_MODE_RMII) {
+		bsp_priv->clk_mac_ref = devm_clk_get(dev, "clk_mac_ref");
+		if (IS_ERR(bsp_priv->clk_mac_ref))
+			dev_err(dev, "%s: cannot get clock %s\n",
+				__func__, "clk_mac_ref");
+
+		if (!bsp_priv->clock_input) {
+			bsp_priv->clk_mac_refout =
+				devm_clk_get(dev, "clk_mac_refout");
+			if (IS_ERR(bsp_priv->clk_mac_refout))
+				dev_err(dev, "%s: cannot get clock %s\n",
+					__func__, "clk_mac_refout");
+		}
+	}
+
+	if (bsp_priv->clock_input) {
+		dev_info(dev, "%s: clock input from PHY\n", __func__);
+	} else {
+		if (bsp_priv->phy_iface == PHY_INTERFACE_MODE_RMII)
+			clk_set_rate(bsp_priv->clk_mac_pll, 50000000);
+	}
+
+	return 0;
+}
+
+static int gmac_clk_enable(struct rk_priv_data *bsp_priv, bool enable)
+{
+	int phy_iface = phy_iface = bsp_priv->phy_iface;
+
+	if (enable) {
+		if (!bsp_priv->clk_enabled) {
+			if (phy_iface == PHY_INTERFACE_MODE_RMII) {
+				if (!IS_ERR(bsp_priv->mac_clk_rx))
+					clk_prepare_enable(
+						bsp_priv->mac_clk_rx);
+
+				if (!IS_ERR(bsp_priv->clk_mac_ref))
+					clk_prepare_enable(
+						bsp_priv->clk_mac_ref);
+
+				if (!IS_ERR(bsp_priv->clk_mac_refout))
+					clk_prepare_enable(
+						bsp_priv->clk_mac_refout);
+			}
+
+			if (!IS_ERR(bsp_priv->aclk_mac))
+				clk_prepare_enable(bsp_priv->aclk_mac);
+
+			if (!IS_ERR(bsp_priv->pclk_mac))
+				clk_prepare_enable(bsp_priv->pclk_mac);
+
+			if (!IS_ERR(bsp_priv->mac_clk_tx))
+				clk_prepare_enable(bsp_priv->mac_clk_tx);
+
+			/**
+			 * if (!IS_ERR(bsp_priv->clk_mac))
+			 *	clk_prepare_enable(bsp_priv->clk_mac);
+			 */
+			mdelay(5);
+			bsp_priv->clk_enabled = true;
+		}
+	} else {
+		if (bsp_priv->clk_enabled) {
+			if (phy_iface == PHY_INTERFACE_MODE_RMII) {
+				if (!IS_ERR(bsp_priv->mac_clk_rx))
+					clk_disable_unprepare(
+						bsp_priv->mac_clk_rx);
+
+				if (!IS_ERR(bsp_priv->clk_mac_ref))
+					clk_disable_unprepare(
+						bsp_priv->clk_mac_ref);
+
+				if (!IS_ERR(bsp_priv->clk_mac_refout))
+					clk_disable_unprepare(
+						bsp_priv->clk_mac_refout);
+			}
+
+			if (!IS_ERR(bsp_priv->aclk_mac))
+				clk_disable_unprepare(bsp_priv->aclk_mac);
+
+			if (!IS_ERR(bsp_priv->pclk_mac))
+				clk_disable_unprepare(bsp_priv->pclk_mac);
+
+			if (!IS_ERR(bsp_priv->mac_clk_tx))
+				clk_disable_unprepare(bsp_priv->mac_clk_tx);
+			/**
+			 * if (!IS_ERR(bsp_priv->clk_mac))
+			 *	clk_disable_unprepare(bsp_priv->clk_mac);
+			 */
+			bsp_priv->clk_enabled = false;
+		}
+	}
+
+	return 0;
+}
+
+static int phy_power_on(struct rk_priv_data *bsp_priv, bool enable)
+{
+	struct regulator *ldo;
+	char *ldostr = bsp_priv->regulator;
+	int ret;
+	struct device *dev = &bsp_priv->pdev->dev;
+
+	if (!ldostr) {
+		dev_err(dev, "%s: no ldo found\n", __func__);
+		return -1;
+	}
+
+	ldo = regulator_get(NULL, ldostr);
+	if (!ldo) {
+		dev_err(dev, "\n%s get ldo %s failed\n", __func__, ldostr);
+	} else {
+		if (enable) {
+			if (!regulator_is_enabled(ldo)) {
+				regulator_set_voltage(ldo, 3300000, 3300000);
+				ret = regulator_enable(ldo);
+				if (ret != 0)
+					dev_err(dev, "%s: fail to enable %s\n",
+						__func__, ldostr);
+				else
+					dev_info(dev, "turn on ldo done.\n");
+			} else {
+				dev_warn(dev, "%s is enabled before enable",
+					 ldostr);
+			}
+		} else {
+			if (regulator_is_enabled(ldo)) {
+				ret = regulator_disable(ldo);
+				if (ret != 0)
+					dev_err(dev, "%s: fail to disable %s\n",
+						__func__, ldostr);
+				else
+					dev_info(dev, "turn off ldo done.\n");
+			} else {
+				dev_warn(dev, "%s is disabled before disable",
+					 ldostr);
+			}
+		}
+		regulator_put(ldo);
+	}
+
+	return 0;
+}
+
+static void *rk_gmac_setup(struct platform_device *pdev)
+{
+	struct rk_priv_data *bsp_priv;
+	struct device *dev = &pdev->dev;
+	int ret;
+	const char *strings = NULL;
+	int value;
+
+	bsp_priv = devm_kzalloc(dev, sizeof(*bsp_priv), GFP_KERNEL);
+	if (!bsp_priv)
+		return ERR_PTR(-ENOMEM);
+
+	bsp_priv->phy_iface = of_get_phy_mode(dev->of_node);
+
+	ret = of_property_read_string(dev->of_node, "phy_regulator", &strings);
+	if (ret) {
+		dev_warn(dev, "%s: Can not read property: phy_regulator.\n",
+			 __func__);
+	} else {
+		dev_info(dev, "%s: PHY power controlled by regulator(%s).\n",
+			 __func__, strings);
+		strcpy(bsp_priv->regulator, strings);
+	}
+
+	ret = of_property_read_string(dev->of_node, "clock_in_out", &strings);
+	if (ret) {
+		dev_err(dev, "%s: Can not read property: clock_in_out.\n",
+			__func__);
+		bsp_priv->clock_input = true;
+	} else {
+		dev_info(dev, "%s: clock input or output? (%s).\n",
+			 __func__, strings);
+		if (!strcmp(strings, "input"))
+			bsp_priv->clock_input = true;
+		else
+			bsp_priv->clock_input = false;
+	}
+
+	ret = of_property_read_u32(dev->of_node, "tx_delay", &value);
+	if (ret) {
+		bsp_priv->tx_delay = 0x30;
+		dev_err(dev, "%s: Can not read property: tx_delay.", __func__);
+		dev_err(dev, "%s: set tx_delay to 0x%x\n",
+			__func__, bsp_priv->tx_delay);
+	} else {
+		dev_info(dev, "%s: TX delay(0x%x).\n", __func__, value);
+		bsp_priv->tx_delay = value;
+	}
+
+	ret = of_property_read_u32(dev->of_node, "rx_delay", &value);
+	if (ret) {
+		bsp_priv->rx_delay = 0x10;
+		dev_err(dev, "%s: Can not read property: rx_delay.", __func__);
+		dev_err(dev, "%s: set rx_delay to 0x%x\n",
+			__func__, bsp_priv->rx_delay);
+	} else {
+		dev_info(dev, "%s: RX delay(0x%x).\n", __func__, value);
+		bsp_priv->rx_delay = value;
+	}
+
+	bsp_priv->grf = syscon_regmap_lookup_by_phandle(dev->of_node,
+							"rockchip,grf");
+	bsp_priv->pdev = pdev;
+
+	/*rmii or rgmii*/
+	if (bsp_priv->phy_iface == PHY_INTERFACE_MODE_RGMII) {
+		dev_info(dev, "%s: init for RGMII\n", __func__);
+		set_to_rgmii(bsp_priv, bsp_priv->tx_delay, bsp_priv->rx_delay);
+	} else if (bsp_priv->phy_iface == PHY_INTERFACE_MODE_RMII) {
+		dev_info(dev, "%s: init for RMII\n", __func__);
+		set_to_rmii(bsp_priv);
+	} else {
+		dev_err(dev, "%s: NO interface defined!\n", __func__);
+	}
+
+	gmac_clk_init(bsp_priv);
+
+	return bsp_priv;
+}
+
+static int rk_gmac_init(struct platform_device *pdev, void *priv)
+{
+	struct rk_priv_data *bsp_priv = priv;
+	int ret;
+
+	ret = phy_power_on(bsp_priv, true);
+	if (ret)
+		return ret;
+
+	ret = gmac_clk_enable(bsp_priv, true);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static void rk_gmac_exit(struct platform_device *pdev, void *priv)
+{
+	struct rk_priv_data *gmac = priv;
+
+	phy_power_on(gmac, false);
+	gmac_clk_enable(gmac, false);
+}
+
+static void rk_fix_speed(void *priv, unsigned int speed)
+{
+	struct rk_priv_data *bsp_priv = priv;
+	struct device *dev = &bsp_priv->pdev->dev;
+
+	if (bsp_priv->phy_iface == PHY_INTERFACE_MODE_RGMII)
+		set_rgmii_speed(bsp_priv, speed);
+	else if (bsp_priv->phy_iface == PHY_INTERFACE_MODE_RMII)
+		set_rmii_speed(bsp_priv, speed);
+	else
+		dev_err(dev, "unsupported interface %d", bsp_priv->phy_iface);
+}
+
+const struct stmmac_of_data rk3288_gmac_data = {
+	.has_gmac = 1,
+	.fix_mac_speed = rk_fix_speed,
+	.setup = rk_gmac_setup,
+	.init = rk_gmac_init,
+	.exit = rk_gmac_exit,
+};
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-sti.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-sti.c
index 0e137751e76e..056b358b4a72 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-sti.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-sti.c
@@ -309,16 +309,16 @@ static int sti_dwmac_parse_data(struct sti_dwmac *dwmac,
 
 	if (IS_PHY_IF_MODE_GBIT(dwmac->interface)) {
 		const char *rs;
-		dwmac->tx_retime_src = TX_RETIME_SRC_CLKGEN;
 
 		err = of_property_read_string(np, "st,tx-retime-src", &rs);
-		if (err < 0)
+		if (err < 0) {
 			dev_warn(dev, "Use internal clock source\n");
-
-		if (!strcasecmp(rs, "clk_125"))
+			dwmac->tx_retime_src = TX_RETIME_SRC_CLKGEN;
+		} else if (!strcasecmp(rs, "clk_125")) {
 			dwmac->tx_retime_src = TX_RETIME_SRC_CLK_125;
-		else if (!strcasecmp(rs, "txclk"))
+		} else if (!strcasecmp(rs, "txclk")) {
 			dwmac->tx_retime_src = TX_RETIME_SRC_TXCLK;
+		}
 
 		dwmac->speed = SPEED_1000;
 	}
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 118a427d1942..8c6b7c1651e5 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -1671,7 +1671,7 @@ static void stmmac_init_tx_coalesce(struct stmmac_priv *priv)
  *  0 on success and an appropriate (-)ve integer as defined in errno.h
  *  file on failure.
  */
-static int stmmac_hw_setup(struct net_device *dev)
+static int stmmac_hw_setup(struct net_device *dev, bool init_ptp)
 {
 	struct stmmac_priv *priv = netdev_priv(dev);
 	int ret;
@@ -1708,9 +1708,11 @@ static int stmmac_hw_setup(struct net_device *dev)
 
 	stmmac_mmc_setup(priv);
 
-	ret = stmmac_init_ptp(priv);
-	if (ret && ret != -EOPNOTSUPP)
-		pr_warn("%s: failed PTP initialisation\n", __func__);
+	if (init_ptp) {
+		ret = stmmac_init_ptp(priv);
+		if (ret && ret != -EOPNOTSUPP)
+			pr_warn("%s: failed PTP initialisation\n", __func__);
+	}
 
 #ifdef CONFIG_DEBUG_FS
 	ret = stmmac_init_fs(dev);
@@ -1787,7 +1789,7 @@ static int stmmac_open(struct net_device *dev)
 		goto init_error;
 	}
 
-	ret = stmmac_hw_setup(dev);
+	ret = stmmac_hw_setup(dev, true);
 	if (ret < 0) {
 		pr_err("%s: Hw setup failed\n", __func__);
 		goto init_error;
@@ -3036,7 +3038,7 @@ int stmmac_resume(struct net_device *ndev)
 	netif_device_attach(ndev);
 
 	init_dma_desc_rings(ndev, GFP_ATOMIC);
-	stmmac_hw_setup(ndev);
+	stmmac_hw_setup(ndev, false);
 	stmmac_init_tx_coalesce(priv);
 
 	napi_enable(&priv->napi);
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
index 4032b170fe24..879e29f48a89 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
@@ -33,6 +33,7 @@
 
 static const struct of_device_id stmmac_dt_ids[] = {
 	/* SoC specific glue layers should come before generic bindings */
+	{ .compatible = "rockchip,rk3288-gmac", .data = &rk3288_gmac_data},
 	{ .compatible = "amlogic,meson6-dwmac", .data = &meson6_dwmac_data},
 	{ .compatible = "allwinner,sun7i-a20-gmac", .data = &sun7i_gmac_data},
 	{ .compatible = "st,stih415-dwmac", .data = &stih4xx_dwmac_data},
@@ -430,7 +431,6 @@ static struct platform_driver stmmac_pltfr_driver = {
 	.remove = stmmac_pltfr_remove,
 	.driver = {
 		   .name = STMMAC_RESOURCE_NAME,
-		   .owner = THIS_MODULE,
 		   .pm = &stmmac_pltfr_pm_ops,
 		   .of_match_table = of_match_ptr(stmmac_dt_ids),
 	},
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.h b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.h
index 25dd1f7ace02..093eb99e5ffd 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.h
@@ -24,5 +24,6 @@ extern const struct stmmac_of_data sun7i_gmac_data;
 extern const struct stmmac_of_data stih4xx_dwmac_data;
 extern const struct stmmac_of_data stid127_dwmac_data;
 extern const struct stmmac_of_data socfpga_gmac_data;
+extern const struct stmmac_of_data rk3288_gmac_data;
 
 #endif /* __STMMAC_PLATFORM_H__ */
diff --git a/drivers/net/ethernet/sun/niu.c b/drivers/net/ethernet/sun/niu.c
index 4aaa3240453a..0c6416213837 100644
--- a/drivers/net/ethernet/sun/niu.c
+++ b/drivers/net/ethernet/sun/niu.c
@@ -10180,7 +10180,6 @@ MODULE_DEVICE_TABLE(of, niu_match);
 static struct platform_driver niu_of_driver = {
 	.driver = {
 		.name = "niu",
-		.owner = THIS_MODULE,
 		.of_match_table = niu_match,
 	},
 	.probe		= niu_of_probe,
diff --git a/drivers/net/ethernet/sun/sunbmac.c b/drivers/net/ethernet/sun/sunbmac.c
index 206c1063815a..aa4f9d2d8fa9 100644
--- a/drivers/net/ethernet/sun/sunbmac.c
+++ b/drivers/net/ethernet/sun/sunbmac.c
@@ -1273,7 +1273,6 @@ MODULE_DEVICE_TABLE(of, bigmac_sbus_match);
 static struct platform_driver bigmac_sbus_driver = {
 	.driver = {
 		.name = "sunbmac",
-		.owner = THIS_MODULE,
 		.of_match_table = bigmac_sbus_match,
 	},
 	.probe		= bigmac_sbus_probe,
diff --git a/drivers/net/ethernet/sun/sunhme.c b/drivers/net/ethernet/sun/sunhme.c
index 9c014803b03b..7a8ca2c7b7df 100644
--- a/drivers/net/ethernet/sun/sunhme.c
+++ b/drivers/net/ethernet/sun/sunhme.c
@@ -3323,7 +3323,6 @@ MODULE_DEVICE_TABLE(of, hme_sbus_match);
 static struct platform_driver hme_sbus_driver = {
 	.driver = {
 		.name = "hme",
-		.owner = THIS_MODULE,
 		.of_match_table = hme_sbus_match,
 	},
 	.probe		= hme_sbus_probe,
diff --git a/drivers/net/ethernet/sun/sunqe.c b/drivers/net/ethernet/sun/sunqe.c
index 5695ae2411de..9b825780b3be 100644
--- a/drivers/net/ethernet/sun/sunqe.c
+++ b/drivers/net/ethernet/sun/sunqe.c
@@ -963,7 +963,6 @@ MODULE_DEVICE_TABLE(of, qec_sbus_match);
 static struct platform_driver qec_sbus_driver = {
 	.driver = {
 		.name = "qec",
-		.owner = THIS_MODULE,
 		.of_match_table = qec_sbus_match,
 	},
 	.probe		= qec_sbus_probe,
diff --git a/drivers/net/ethernet/sun/sunvnet.c b/drivers/net/ethernet/sun/sunvnet.c
index 90c86cd3be14..d2835bf7b4fb 100644
--- a/drivers/net/ethernet/sun/sunvnet.c
+++ b/drivers/net/ethernet/sun/sunvnet.c
@@ -466,23 +466,6 @@ static int vnet_send_ack(struct vnet_port *port, struct vio_dring_state *dr,
 	return err;
 }
 
-static u32 next_idx(u32 idx, struct vio_dring_state *dr)
-{
-	if (++idx == dr->num_entries)
-		idx = 0;
-	return idx;
-}
-
-static u32 prev_idx(u32 idx, struct vio_dring_state *dr)
-{
-	if (idx == 0)
-		idx = dr->num_entries - 1;
-	else
-		idx--;
-
-	return idx;
-}
-
 static struct vio_net_desc *get_rx_desc(struct vnet_port *port,
 					struct vio_dring_state *dr,
 					u32 index)
@@ -556,7 +539,8 @@ static int vnet_walk_rx(struct vnet_port *port, struct vio_dring_state *dr,
 	int ack_start = -1, ack_end = -1;
 	bool send_ack = true;
 
-	end = (end == (u32) -1) ? prev_idx(start, dr) : next_idx(end, dr);
+	end = (end == (u32) -1) ? vio_dring_prev(dr, start)
+				: vio_dring_next(dr, end);
 
 	viodbg(DATA, "vnet_walk_rx start[%08x] end[%08x]\n", start, end);
 
@@ -570,7 +554,7 @@ static int vnet_walk_rx(struct vnet_port *port, struct vio_dring_state *dr,
 		if (ack_start == -1)
 			ack_start = start;
 		ack_end = start;
-		start = next_idx(start, dr);
+		start = vio_dring_next(dr, start);
 		if (ack && start != end) {
 			err = vnet_send_ack(port, dr, ack_start, ack_end,
 					    VIO_DRING_ACTIVE);
@@ -584,7 +568,7 @@ static int vnet_walk_rx(struct vnet_port *port, struct vio_dring_state *dr,
 		}
 	}
 	if (unlikely(ack_start == -1))
-		ack_start = ack_end = prev_idx(start, dr);
+		ack_start = ack_end = vio_dring_prev(dr, start);
 	if (send_ack) {
 		port->napi_resume = false;
 		return vnet_send_ack(port, dr, ack_start, ack_end,
@@ -633,7 +617,7 @@ static int idx_is_pending(struct vio_dring_state *dr, u32 end)
 			found = 1;
 			break;
 		}
-		idx = next_idx(idx, dr);
+		idx = vio_dring_next(dr, idx);
 	}
 	return found;
 }
@@ -663,7 +647,7 @@ static int vnet_ack(struct vnet_port *port, void *msgbuf)
 	/* sync for race conditions with vnet_start_xmit() and tell xmit it
 	 * is time to send a trigger.
 	 */
-	dr->cons = next_idx(end, dr);
+	dr->cons = vio_dring_next(dr, end);
 	desc = vio_dring_entry(dr, dr->cons);
 	if (desc->hdr.state == VIO_DESC_READY && !port->start_cons) {
 		/* vnet_start_xmit() just populated this dring but missed
@@ -784,7 +768,7 @@ ldc_ctrl:
 			pkt->tag.stype = VIO_SUBTYPE_INFO;
 			pkt->tag.stype_env = VIO_DRING_DATA;
 			pkt->seq = dr->rcv_nxt;
-			pkt->start_idx = next_idx(port->napi_stop_idx, dr);
+			pkt->start_idx = vio_dring_next(dr, port->napi_stop_idx);
 			pkt->end_idx = -1;
 			goto napi_resume;
 		}
@@ -1217,6 +1201,7 @@ static int vnet_handle_offloads(struct vnet_port *port, struct sk_buff *skb)
 		segs = skb_gso_segment(skb, dev->features & ~NETIF_F_TSO);
 	if (IS_ERR(segs)) {
 		dev->stats.tx_dropped++;
+		dev_kfree_skb_any(skb);
 		return NETDEV_TX_OK;
 	}
 
diff --git a/drivers/net/ethernet/ti/cpts.c b/drivers/net/ethernet/ti/cpts.c
index 4a4388b813ac..fbe42cb107ec 100644
--- a/drivers/net/ethernet/ti/cpts.c
+++ b/drivers/net/ethernet/ti/cpts.c
@@ -157,14 +157,11 @@ static int cpts_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
 
 static int cpts_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 {
-	s64 now;
 	unsigned long flags;
 	struct cpts *cpts = container_of(ptp, struct cpts, info);
 
 	spin_lock_irqsave(&cpts->lock, flags);
-	now = timecounter_read(&cpts->tc);
-	now += delta;
-	timecounter_init(&cpts->tc, &cpts->cc, now);
+	timecounter_adjtime(&cpts->tc, delta);
 	spin_unlock_irqrestore(&cpts->lock, flags);
 
 	return 0;
diff --git a/drivers/net/ethernet/ti/cpts.h b/drivers/net/ethernet/ti/cpts.h
index 1a581ef7eee8..69a46b92c7d6 100644
--- a/drivers/net/ethernet/ti/cpts.h
+++ b/drivers/net/ethernet/ti/cpts.h
@@ -27,6 +27,7 @@
 #include <linux/list.h>
 #include <linux/ptp_clock_kernel.h>
 #include <linux/skbuff.h>
+#include <linux/timecounter.h>
 
 struct cpsw_cpts {
 	u32 idver;                /* Identification and version */
diff --git a/drivers/net/ethernet/tundra/tsi108_eth.c b/drivers/net/ethernet/tundra/tsi108_eth.c
index 47eeb3abf7f7..520cf50a3d5a 100644
--- a/drivers/net/ethernet/tundra/tsi108_eth.c
+++ b/drivers/net/ethernet/tundra/tsi108_eth.c
@@ -161,7 +161,6 @@ static struct platform_driver tsi_eth_driver = {
 	.remove = tsi108_ether_remove,
 	.driver	= {
 		.name = "tsi-ethernet",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/ethernet/via/via-rhine.c b/drivers/net/ethernet/via/via-rhine.c
index 68c5260cc322..a191afc23b56 100644
--- a/drivers/net/ethernet/via/via-rhine.c
+++ b/drivers/net/ethernet/via/via-rhine.c
@@ -2508,7 +2508,6 @@ static struct platform_driver rhine_driver_platform = {
 	.remove		= rhine_remove_one_platform,
 	.driver = {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table	= rhine_of_tbl,
 		.pm		= RHINE_PM_OPS,
 	}
diff --git a/drivers/net/ethernet/via/via-velocity.c b/drivers/net/ethernet/via/via-velocity.c
index a43e8492b1ce..282f83a63b67 100644
--- a/drivers/net/ethernet/via/via-velocity.c
+++ b/drivers/net/ethernet/via/via-velocity.c
@@ -3281,7 +3281,6 @@ static struct platform_driver velocity_platform_driver = {
 	.remove		= velocity_platform_remove,
 	.driver = {
 		.name = "via-velocity",
-		.owner = THIS_MODULE,
 		.of_match_table = velocity_of_ids,
 		.pm = &velocity_pm_ops,
 	},
diff --git a/drivers/net/ethernet/wiznet/w5100.c b/drivers/net/ethernet/wiznet/w5100.c
index 70a930ac4fa9..a495931a66a1 100644
--- a/drivers/net/ethernet/wiznet/w5100.c
+++ b/drivers/net/ethernet/wiznet/w5100.c
@@ -788,7 +788,6 @@ 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,
diff --git a/drivers/net/ethernet/wiznet/w5300.c b/drivers/net/ethernet/wiznet/w5300.c
index 7974b7d90fcc..09322d9db578 100644
--- a/drivers/net/ethernet/wiznet/w5300.c
+++ b/drivers/net/ethernet/wiznet/w5300.c
@@ -700,7 +700,6 @@ 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,
diff --git a/drivers/net/ethernet/xilinx/xilinx_axienet.h b/drivers/net/ethernet/xilinx/xilinx_axienet.h
index 44b8d2bad8c3..4c9b4fa1d3c1 100644
--- a/drivers/net/ethernet/xilinx/xilinx_axienet.h
+++ b/drivers/net/ethernet/xilinx/xilinx_axienet.h
@@ -388,7 +388,6 @@ struct axidma_bd {
  * @dma_err_tasklet: Tasklet structure to process Axi DMA errors
  * @tx_irq:	Axidma TX IRQ number
  * @rx_irq:	Axidma RX IRQ number
- * @temac_type:	axienet type to identify between soft and hard temac
  * @phy_type:	Phy type to identify between MII/GMII/RGMII/SGMII/1000 Base-X
  * @options:	AxiEthernet option word
  * @last_link:	Phy link state in which the PHY was negotiated earlier
@@ -431,7 +430,6 @@ struct axienet_local {
 
 	int tx_irq;
 	int rx_irq;
-	u32 temac_type;
 	u32 phy_type;
 
 	u32 options;			/* Current options word */
diff --git a/drivers/net/ethernet/xilinx/xilinx_axienet_main.c b/drivers/net/ethernet/xilinx/xilinx_axienet_main.c
index 4ea2d4e6f1d1..c18a0c637c44 100644
--- a/drivers/net/ethernet/xilinx/xilinx_axienet_main.c
+++ b/drivers/net/ethernet/xilinx/xilinx_axienet_main.c
@@ -1555,10 +1555,6 @@ static int axienet_of_probe(struct platform_device *op)
 		if ((be32_to_cpup(p)) >= 0x4000)
 			lp->jumbo_support = 1;
 	}
-	p = (__be32 *) of_get_property(op->dev.of_node, "xlnx,temac-type",
-				       NULL);
-	if (p)
-		lp->temac_type = be32_to_cpup(p);
 	p = (__be32 *) of_get_property(op->dev.of_node, "xlnx,phy-type", NULL);
 	if (p)
 		lp->phy_type = be32_to_cpup(p);
diff --git a/drivers/net/fddi/skfp/smt.c b/drivers/net/fddi/skfp/smt.c
index 9edada85ed02..cd78b7cacc75 100644
--- a/drivers/net/fddi/skfp/smt.c
+++ b/drivers/net/fddi/skfp/smt.c
@@ -1736,18 +1736,6 @@ char *addr_to_string(struct fddi_addr *addr)
 }
 #endif
 
-#ifdef	AM29K
-int smt_ifconfig(int argc, char *argv[])
-{
-	if (argc >= 2 && !strcmp(argv[0],"opt_bypass") &&
-	    !strcmp(argv[1],"yes")) {
-		smc->mib.fddiSMTBypassPresent = 1 ;
-		return 0;
-	}
-	return amdfddi_config(0, argc, argv);
-}
-#endif
-
 /*
  * return static mac index
  */
diff --git a/drivers/net/hyperv/hyperv_net.h b/drivers/net/hyperv/hyperv_net.h
index 2f48f790c9b4..384ca4f4de4a 100644
--- a/drivers/net/hyperv/hyperv_net.h
+++ b/drivers/net/hyperv/hyperv_net.h
@@ -590,6 +590,7 @@ struct nvsp_message {
 
 
 #define NETVSC_RECEIVE_BUFFER_ID		0xcafe
+#define NETVSC_SEND_BUFFER_ID			0
 
 #define NETVSC_PACKET_SIZE                      4096
 
diff --git a/drivers/net/hyperv/netvsc.c b/drivers/net/hyperv/netvsc.c
index dd867e6cabd6..9f49c0129a78 100644
--- a/drivers/net/hyperv/netvsc.c
+++ b/drivers/net/hyperv/netvsc.c
@@ -161,8 +161,8 @@ static int netvsc_destroy_buf(struct netvsc_device *net_device)
 
 	/* Deal with the send buffer we may have setup.
 	 * If we got a  send section size, it means we received a
-	 * SendsendBufferComplete msg (ie sent
-	 * NvspMessage1TypeSendReceiveBuffer msg) therefore, we need
+	 * NVSP_MSG1_TYPE_SEND_SEND_BUF_COMPLETE msg (ie sent
+	 * NVSP_MSG1_TYPE_SEND_SEND_BUF msg) therefore, we need
 	 * to send a revoke msg here
 	 */
 	if (net_device->send_section_size) {
@@ -172,7 +172,8 @@ static int netvsc_destroy_buf(struct netvsc_device *net_device)
 
 		revoke_packet->hdr.msg_type =
 			NVSP_MSG1_TYPE_REVOKE_SEND_BUF;
-		revoke_packet->msg.v1_msg.revoke_recv_buf.id = 0;
+		revoke_packet->msg.v1_msg.revoke_send_buf.id =
+			NETVSC_SEND_BUFFER_ID;
 
 		ret = vmbus_sendpacket(net_device->dev->channel,
 				       revoke_packet,
@@ -204,7 +205,7 @@ static int netvsc_destroy_buf(struct netvsc_device *net_device)
 		net_device->send_buf_gpadl_handle = 0;
 	}
 	if (net_device->send_buf) {
-		/* Free up the receive buffer */
+		/* Free up the send buffer */
 		vfree(net_device->send_buf);
 		net_device->send_buf = NULL;
 	}
@@ -339,9 +340,9 @@ static int netvsc_init_buf(struct hv_device *device)
 	init_packet = &net_device->channel_init_pkt;
 	memset(init_packet, 0, sizeof(struct nvsp_message));
 	init_packet->hdr.msg_type = NVSP_MSG1_TYPE_SEND_SEND_BUF;
-	init_packet->msg.v1_msg.send_recv_buf.gpadl_handle =
+	init_packet->msg.v1_msg.send_send_buf.gpadl_handle =
 		net_device->send_buf_gpadl_handle;
-	init_packet->msg.v1_msg.send_recv_buf.id = 0;
+	init_packet->msg.v1_msg.send_send_buf.id = NETVSC_SEND_BUFFER_ID;
 
 	/* Send the gpadl notification request */
 	ret = vmbus_sendpacket(device->channel, init_packet,
@@ -364,7 +365,7 @@ static int netvsc_init_buf(struct hv_device *device)
 		netdev_err(ndev, "Unable to complete send buffer "
 			   "initialization with NetVsp - status %d\n",
 			   init_packet->msg.v1_msg.
-			   send_recv_buf_complete.status);
+			   send_send_buf_complete.status);
 		ret = -EINVAL;
 		goto cleanup;
 	}
diff --git a/drivers/net/ieee802154/at86rf230.c b/drivers/net/ieee802154/at86rf230.c
index 1c0135620c62..80632fc59756 100644
--- a/drivers/net/ieee802154/at86rf230.c
+++ b/drivers/net/ieee802154/at86rf230.c
@@ -450,7 +450,7 @@ at86rf230_async_error_recover(void *context)
 	ieee802154_wake_queue(lp->hw);
 }
 
-static void
+static inline void
 at86rf230_async_error(struct at86rf230_local *lp,
 		      struct at86rf230_state_change *ctx, int rc)
 {
@@ -524,7 +524,6 @@ at86rf230_async_state_assert(void *context)
 			}
 		}
 
-
 		dev_warn(&lp->spi->dev, "unexcept state change from 0x%02x to 0x%02x. Actual state: 0x%02x\n",
 			 ctx->from_state, ctx->to_state, trx_state);
 	}
@@ -655,7 +654,7 @@ at86rf230_async_state_change_start(void *context)
 		if (ctx->irq_enable)
 			enable_irq(lp->spi->irq);
 
-		at86rf230_async_error(lp, &lp->state, rc);
+		at86rf230_async_error(lp, ctx, rc);
 	}
 }
 
@@ -715,10 +714,7 @@ at86rf230_tx_complete(void *context)
 
 	enable_irq(lp->spi->irq);
 
-	if (lp->max_frame_retries <= 0)
-		ieee802154_xmit_complete(lp->hw, skb, true);
-	else
-		ieee802154_xmit_complete(lp->hw, skb, false);
+	ieee802154_xmit_complete(lp->hw, skb, !lp->tx_aret);
 }
 
 static void
@@ -753,16 +749,13 @@ at86rf230_tx_trac_check(void *context)
 	 * to STATE_FORCE_TRX_OFF then STATE_TX_ON to recover the transceiver
 	 * state to TX_ON.
 	 */
-	if (trac) {
+	if (trac)
 		at86rf230_async_state_change(lp, ctx, STATE_FORCE_TRX_OFF,
 					     at86rf230_tx_trac_error, true);
-		return;
-	}
-
-	at86rf230_tx_on(context);
+	else
+		at86rf230_tx_on(context);
 }
 
-
 static void
 at86rf230_tx_trac_status(void *context)
 {
@@ -1082,7 +1075,7 @@ at86rf230_set_hw_addr_filt(struct ieee802154_hw *hw,
 		u16 addr = le16_to_cpu(filt->short_addr);
 
 		dev_vdbg(&lp->spi->dev,
-			"at86rf230_set_hw_addr_filt called for saddr\n");
+			 "at86rf230_set_hw_addr_filt called for saddr\n");
 		__at86rf230_write(lp, RG_SHORT_ADDR_0, addr);
 		__at86rf230_write(lp, RG_SHORT_ADDR_1, addr >> 8);
 	}
@@ -1091,7 +1084,7 @@ at86rf230_set_hw_addr_filt(struct ieee802154_hw *hw,
 		u16 pan = le16_to_cpu(filt->pan_id);
 
 		dev_vdbg(&lp->spi->dev,
-			"at86rf230_set_hw_addr_filt called for pan id\n");
+			 "at86rf230_set_hw_addr_filt called for pan id\n");
 		__at86rf230_write(lp, RG_PAN_ID_0, pan);
 		__at86rf230_write(lp, RG_PAN_ID_1, pan >> 8);
 	}
@@ -1101,14 +1094,14 @@ at86rf230_set_hw_addr_filt(struct ieee802154_hw *hw,
 
 		memcpy(addr, &filt->ieee_addr, 8);
 		dev_vdbg(&lp->spi->dev,
-			"at86rf230_set_hw_addr_filt called for IEEE addr\n");
+			 "at86rf230_set_hw_addr_filt called for IEEE addr\n");
 		for (i = 0; i < 8; i++)
 			__at86rf230_write(lp, RG_IEEE_ADDR_0 + i, addr[i]);
 	}
 
 	if (changed & IEEE802154_AFILT_PANC_CHANGED) {
 		dev_vdbg(&lp->spi->dev,
-			"at86rf230_set_hw_addr_filt called for panc change\n");
+			 "at86rf230_set_hw_addr_filt called for panc change\n");
 		if (filt->pan_coord)
 			at86rf230_write_subreg(lp, SR_AACK_I_AM_COORD, 1);
 		else
@@ -1146,11 +1139,37 @@ at86rf230_set_lbt(struct ieee802154_hw *hw, bool on)
 }
 
 static int
-at86rf230_set_cca_mode(struct ieee802154_hw *hw, u8 mode)
+at86rf230_set_cca_mode(struct ieee802154_hw *hw,
+		       const struct wpan_phy_cca *cca)
 {
 	struct at86rf230_local *lp = hw->priv;
+	u8 val;
+
+	/* mapping 802.15.4 to driver spec */
+	switch (cca->mode) {
+	case NL802154_CCA_ENERGY:
+		val = 1;
+		break;
+	case NL802154_CCA_CARRIER:
+		val = 2;
+		break;
+	case NL802154_CCA_ENERGY_CARRIER:
+		switch (cca->opt) {
+		case NL802154_CCA_OPT_ENERGY_CARRIER_AND:
+			val = 3;
+			break;
+		case NL802154_CCA_OPT_ENERGY_CARRIER_OR:
+			val = 0;
+			break;
+		default:
+			return -EINVAL;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
 
-	return at86rf230_write_subreg(lp, SR_CCA_MODE, mode);
+	return at86rf230_write_subreg(lp, SR_CCA_MODE, val);
 }
 
 static int
@@ -1400,7 +1419,7 @@ at86rf230_detect_device(struct at86rf230_local *lp)
 	if (rc)
 		return rc;
 
-	rc = __at86rf230_read(lp, RG_PART_NUM, &version);
+	rc = __at86rf230_read(lp, RG_VERSION_NUM, &version);
 	if (rc)
 		return rc;
 
@@ -1410,11 +1429,12 @@ at86rf230_detect_device(struct at86rf230_local *lp)
 		return -EINVAL;
 	}
 
-	lp->hw->extra_tx_headroom = 0;
 	lp->hw->flags = IEEE802154_HW_TX_OMIT_CKSUM | IEEE802154_HW_AACK |
 			IEEE802154_HW_TXPOWER | IEEE802154_HW_ARET |
 			IEEE802154_HW_AFILT | IEEE802154_HW_PROMISCUOUS;
 
+	lp->hw->phy->cca.mode = NL802154_CCA_ENERGY;
+
 	switch (part) {
 	case 2:
 		chip = "at86rf230";
@@ -1429,16 +1449,12 @@ at86rf230_detect_device(struct at86rf230_local *lp)
 		break;
 	case 7:
 		chip = "at86rf212";
-		if (version == 1) {
-			lp->data = &at86rf212_data;
-			lp->hw->flags |= IEEE802154_HW_LBT;
-			lp->hw->phy->channels_supported[0] = 0x00007FF;
-			lp->hw->phy->channels_supported[2] = 0x00007FF;
-			lp->hw->phy->current_channel = 5;
-			lp->hw->phy->symbol_duration = 25;
-		} else {
-			rc = -ENOTSUPP;
-		}
+		lp->data = &at86rf212_data;
+		lp->hw->flags |= IEEE802154_HW_LBT;
+		lp->hw->phy->channels_supported[0] = 0x00007FF;
+		lp->hw->phy->channels_supported[2] = 0x00007FF;
+		lp->hw->phy->current_channel = 5;
+		lp->hw->phy->symbol_duration = 25;
 		break;
 	case 11:
 		chip = "at86rf233";
@@ -1448,7 +1464,7 @@ at86rf230_detect_device(struct at86rf230_local *lp)
 		lp->hw->phy->symbol_duration = 16;
 		break;
 	default:
-		chip = "unkown";
+		chip = "unknown";
 		rc = -ENOTSUPP;
 		break;
 	}
diff --git a/drivers/net/ieee802154/cc2520.c b/drivers/net/ieee802154/cc2520.c
index f9df9fa86d5f..a43c8acb7268 100644
--- a/drivers/net/ieee802154/cc2520.c
+++ b/drivers/net/ieee802154/cc2520.c
@@ -19,7 +19,6 @@
 #include <linux/workqueue.h>
 #include <linux/interrupt.h>
 #include <linux/skbuff.h>
-#include <linux/pinctrl/consumer.h>
 #include <linux/of_gpio.h>
 #include <linux/ieee802154.h>
 
@@ -513,7 +512,6 @@ err_tx:
 	return rc;
 }
 
-
 static int cc2520_rx(struct cc2520_private *priv)
 {
 	u8 len = 0, lqi = 0, bytes = 1;
@@ -652,6 +650,7 @@ static int cc2520_register(struct cc2520_private *priv)
 	priv->hw->parent = &priv->spi->dev;
 	priv->hw->extra_tx_headroom = 0;
 	priv->hw->vif_data_size = sizeof(*priv);
+	ieee802154_random_extended_addr(&priv->hw->phy->perm_extended_addr);
 
 	/* We do support only 2.4 Ghz */
 	priv->hw->phy->channels_supported[0] = 0x7FFF800;
@@ -842,24 +841,15 @@ done:
 static int cc2520_probe(struct spi_device *spi)
 {
 	struct cc2520_private *priv;
-	struct pinctrl *pinctrl;
 	struct cc2520_platform_data *pdata;
 	int ret;
 
-	priv = devm_kzalloc(&spi->dev,
-			    sizeof(struct cc2520_private), GFP_KERNEL);
-	if (!priv) {
-		ret = -ENOMEM;
-		goto err_ret;
-	}
+	priv = devm_kzalloc(&spi->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
 
 	spi_set_drvdata(spi, priv);
 
-	pinctrl = devm_pinctrl_get_select_default(&spi->dev);
-	if (IS_ERR(pinctrl))
-		dev_warn(&spi->dev,
-			 "pinctrl pins are not configured\n");
-
 	pdata = cc2520_get_platform_data(spi);
 	if (!pdata) {
 		dev_err(&spi->dev, "no platform data\n");
@@ -870,10 +860,8 @@ static int cc2520_probe(struct spi_device *spi)
 
 	priv->buf = devm_kzalloc(&spi->dev,
 				 SPI_COMMAND_BUFFER, GFP_KERNEL);
-	if (!priv->buf) {
-		ret = -ENOMEM;
-		goto err_ret;
-	}
+	if (!priv->buf)
+		return -ENOMEM;
 
 	mutex_init(&priv->buffer_mutex);
 	INIT_WORK(&priv->fifop_irqwork, cc2520_fifop_irqwork);
@@ -947,7 +935,6 @@ static int cc2520_probe(struct spi_device *spi)
 	if (ret)
 		goto err_hw_init;
 
-
 	gpio_set_value(pdata->vreg, HIGH);
 	usleep_range(100, 150);
 
@@ -991,8 +978,6 @@ static int cc2520_probe(struct spi_device *spi)
 err_hw_init:
 	mutex_destroy(&priv->buffer_mutex);
 	flush_work(&priv->fifop_irqwork);
-
-err_ret:
 	return ret;
 }
 
diff --git a/drivers/net/ieee802154/fakelb.c b/drivers/net/ieee802154/fakelb.c
index 96947d724189..dc2bfb600b4b 100644
--- a/drivers/net/ieee802154/fakelb.c
+++ b/drivers/net/ieee802154/fakelb.c
@@ -261,7 +261,6 @@ static struct platform_driver ieee802154fake_driver = {
 	.remove = fakelb_remove,
 	.driver = {
 			.name = "ieee802154fakelb",
-			.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/ieee802154/mrf24j40.c b/drivers/net/ieee802154/mrf24j40.c
index a200fa16beae..fba2dfd910f7 100644
--- a/drivers/net/ieee802154/mrf24j40.c
+++ b/drivers/net/ieee802154/mrf24j40.c
@@ -289,7 +289,7 @@ static int mrf24j40_read_rx_buf(struct mrf24j40 *devrec,
 		goto out;
 
 	/* Range check the RX FIFO length, accounting for the one-byte
-	 * length field at the begining. */
+	 * length field at the beginning. */
 	if (rx_len > RX_FIFO_SIZE-1) {
 		dev_err(printdev(devrec), "Invalid length read from device. Performing short read.\n");
 		rx_len = RX_FIFO_SIZE-1;
@@ -323,7 +323,7 @@ static int mrf24j40_read_rx_buf(struct mrf24j40 *devrec,
 
 #ifdef DEBUG
 	print_hex_dump(KERN_DEBUG, "mrf24j40 rx: ",
-		DUMP_PREFIX_OFFSET, 16, 1, data, *len, 0);
+		       DUMP_PREFIX_OFFSET, 16, 1, data, *len, 0);
 	pr_debug("mrf24j40 rx: lqi: %02hhx rssi: %02hhx\n",
 		 lqi_rssi[0], lqi_rssi[1]);
 #endif
@@ -521,7 +521,7 @@ static int mrf24j40_filter(struct ieee802154_hw *hw,
 		 */
 
 		dev_dbg(printdev(devrec), "Set Pan Coord to %s\n",
-					filt->pan_coord ? "on" : "off");
+			filt->pan_coord ? "on" : "off");
 	}
 
 	return 0;
diff --git a/drivers/net/irda/ali-ircc.c b/drivers/net/irda/ali-ircc.c
index 588680a72fa1..58f98f4de773 100644
--- a/drivers/net/irda/ali-ircc.c
+++ b/drivers/net/irda/ali-ircc.c
@@ -61,7 +61,6 @@ static struct platform_driver ali_ircc_driver = {
 	.resume		= ali_ircc_resume,
 	.driver		= {
 		.name	= ALI_IRCC_DRIVER_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/irda/au1k_ir.c b/drivers/net/irda/au1k_ir.c
index aab2cf72d025..e151205281e2 100644
--- a/drivers/net/irda/au1k_ir.c
+++ b/drivers/net/irda/au1k_ir.c
@@ -989,7 +989,6 @@ static int au1k_irda_remove(struct platform_device *pdev)
 static struct platform_driver au1k_irda_driver = {
 	.driver	= {
 		.name	= "au1000-irda",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= au1k_irda_probe,
 	.remove		= au1k_irda_remove,
diff --git a/drivers/net/irda/pxaficp_ir.c b/drivers/net/irda/pxaficp_ir.c
index 3eeaaf800494..100454662e4b 100644
--- a/drivers/net/irda/pxaficp_ir.c
+++ b/drivers/net/irda/pxaficp_ir.c
@@ -966,7 +966,6 @@ static int pxa_irda_remove(struct platform_device *_dev)
 static struct platform_driver pxa_ir_driver = {
 	.driver         = {
 		.name   = "pxa2xx-ir",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= pxa_irda_probe,
 	.remove		= pxa_irda_remove,
diff --git a/drivers/net/irda/sa1100_ir.c b/drivers/net/irda/sa1100_ir.c
index 42fde9ed23e1..b6e44ff4e373 100644
--- a/drivers/net/irda/sa1100_ir.c
+++ b/drivers/net/irda/sa1100_ir.c
@@ -38,7 +38,7 @@
 #include <net/irda/irda_device.h>
 
 #include <mach/hardware.h>
-#include <asm/mach/irda.h>
+#include <linux/platform_data/irda-sa11x0.h>
 
 static int power_level = 3;
 static int tx_lpm;
@@ -1114,7 +1114,6 @@ static struct platform_driver sa1100ir_driver = {
 	.resume		= sa1100_irda_resume,
 	.driver		= {
 		.name	= "sa11x0-ir",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/macvtap.c b/drivers/net/macvtap.c
index 60f7ee5fafbe..7df221788cd4 100644
--- a/drivers/net/macvtap.c
+++ b/drivers/net/macvtap.c
@@ -46,16 +46,18 @@ struct macvtap_queue {
 	struct list_head next;
 };
 
-#define MACVTAP_FEATURES (IFF_VNET_HDR | IFF_VNET_LE | IFF_MULTI_QUEUE)
+#define MACVTAP_FEATURES (IFF_VNET_HDR | IFF_MULTI_QUEUE)
+
+#define MACVTAP_VNET_LE 0x80000000
 
 static inline u16 macvtap16_to_cpu(struct macvtap_queue *q, __virtio16 val)
 {
-	return __virtio16_to_cpu(q->flags & IFF_VNET_LE, val);
+	return __virtio16_to_cpu(q->flags & MACVTAP_VNET_LE, val);
 }
 
 static inline __virtio16 cpu_to_macvtap16(struct macvtap_queue *q, u16 val)
 {
-	return __cpu_to_virtio16(q->flags & IFF_VNET_LE, val);
+	return __cpu_to_virtio16(q->flags & MACVTAP_VNET_LE, val);
 }
 
 static struct proto macvtap_proto = {
@@ -999,7 +1001,7 @@ static long macvtap_ioctl(struct file *file, unsigned int cmd,
 	void __user *argp = (void __user *)arg;
 	struct ifreq __user *ifr = argp;
 	unsigned int __user *up = argp;
-	unsigned int u;
+	unsigned short u;
 	int __user *sp = argp;
 	int s;
 	int ret;
@@ -1014,7 +1016,7 @@ static long macvtap_ioctl(struct file *file, unsigned int cmd,
 		if ((u & ~MACVTAP_FEATURES) != (IFF_NO_PI | IFF_TAP))
 			ret = -EINVAL;
 		else
-			q->flags = u;
+			q->flags = (q->flags & ~MACVTAP_FEATURES) | u;
 
 		return ret;
 
@@ -1027,8 +1029,9 @@ static long macvtap_ioctl(struct file *file, unsigned int cmd,
 		}
 
 		ret = 0;
+		u = q->flags;
 		if (copy_to_user(&ifr->ifr_name, vlan->dev->name, IFNAMSIZ) ||
-		    put_user(q->flags, &ifr->ifr_flags))
+		    put_user(u, &ifr->ifr_flags))
 			ret = -EFAULT;
 		macvtap_put_vlan(vlan);
 		rtnl_unlock();
@@ -1069,6 +1072,21 @@ static long macvtap_ioctl(struct file *file, unsigned int cmd,
 		q->vnet_hdr_sz = s;
 		return 0;
 
+	case TUNGETVNETLE:
+		s = !!(q->flags & MACVTAP_VNET_LE);
+		if (put_user(s, sp))
+			return -EFAULT;
+		return 0;
+
+	case TUNSETVNETLE:
+		if (get_user(s, sp))
+			return -EFAULT;
+		if (s)
+			q->flags |= MACVTAP_VNET_LE;
+		else
+			q->flags &= ~MACVTAP_VNET_LE;
+		return 0;
+
 	case TUNSETOFFLOAD:
 		/* let the user check for future flags */
 		if (arg & ~(TUN_F_CSUM | TUN_F_TSO4 | TUN_F_TSO6 |
diff --git a/drivers/net/phy/Kconfig b/drivers/net/phy/Kconfig
index b4b0f804e84c..a3c251b79f38 100644
--- a/drivers/net/phy/Kconfig
+++ b/drivers/net/phy/Kconfig
@@ -119,8 +119,8 @@ config MICREL_PHY
 	  Supports the KSZ9021, VSC8201, KS8001 PHYs.
 
 config FIXED_PHY
-	bool "Driver for MDIO Bus/PHY emulation with fixed speed/link PHYs"
-	depends on PHYLIB=y
+	tristate "Driver for MDIO Bus/PHY emulation with fixed speed/link PHYs"
+	depends on PHYLIB
 	---help---
 	  Adds the platform "fixed" MDIO Bus to cover the boards that use
 	  PHYs that are not connected to the real MDIO bus.
diff --git a/drivers/net/phy/Makefile b/drivers/net/phy/Makefile
index eb3b18b5978b..501ea7699a2d 100644
--- a/drivers/net/phy/Makefile
+++ b/drivers/net/phy/Makefile
@@ -17,7 +17,7 @@ obj-$(CONFIG_BCM87XX_PHY)	+= bcm87xx.o
 obj-$(CONFIG_ICPLUS_PHY)	+= icplus.o
 obj-$(CONFIG_REALTEK_PHY)	+= realtek.o
 obj-$(CONFIG_LSI_ET1011C_PHY)	+= et1011c.o
-obj-$(CONFIG_FIXED_PHY)		+= fixed.o
+obj-$(CONFIG_FIXED_PHY)		+= fixed_phy.o
 obj-$(CONFIG_MDIO_BITBANG)	+= mdio-bitbang.o
 obj-$(CONFIG_MDIO_GPIO)		+= mdio-gpio.o
 obj-$(CONFIG_NATIONAL_PHY)	+= national.o
diff --git a/drivers/net/phy/fixed.c b/drivers/net/phy/fixed_phy.c
index 47872caa0081..3ad0e6e16c39 100644
--- a/drivers/net/phy/fixed.c
+++ b/drivers/net/phy/fixed_phy.c
@@ -274,6 +274,7 @@ struct phy_device *fixed_phy_register(unsigned int irq,
 
 	return phy;
 }
+EXPORT_SYMBOL_GPL(fixed_phy_register);
 
 static int __init fixed_mdio_bus_init(void)
 {
diff --git a/drivers/net/phy/mdio-bcm-unimac.c b/drivers/net/phy/mdio-bcm-unimac.c
index 5b643e588e8f..6deac6d32f57 100644
--- a/drivers/net/phy/mdio-bcm-unimac.c
+++ b/drivers/net/phy/mdio-bcm-unimac.c
@@ -199,7 +199,6 @@ static struct of_device_id unimac_mdio_ids[] = {
 static struct platform_driver unimac_mdio_driver = {
 	.driver = {
 		.name = "unimac-mdio",
-		.owner = THIS_MODULE,
 		.of_match_table = unimac_mdio_ids,
 	},
 	.probe	= unimac_mdio_probe,
diff --git a/drivers/net/phy/mdio-gpio.c b/drivers/net/phy/mdio-gpio.c
index 5f1a2250018f..0a0578a592b8 100644
--- a/drivers/net/phy/mdio-gpio.c
+++ b/drivers/net/phy/mdio-gpio.c
@@ -259,7 +259,6 @@ static struct platform_driver mdio_gpio_driver = {
 	.remove = mdio_gpio_remove,
 	.driver		= {
 		.name	= "mdio-gpio",
-		.owner	= THIS_MODULE,
 		.of_match_table = mdio_gpio_of_match,
 	},
 };
diff --git a/drivers/net/phy/mdio-mux-gpio.c b/drivers/net/phy/mdio-mux-gpio.c
index 096695163491..320eb15315c8 100644
--- a/drivers/net/phy/mdio-mux-gpio.c
+++ b/drivers/net/phy/mdio-mux-gpio.c
@@ -14,13 +14,13 @@
 #include <linux/mdio-mux.h>
 #include <linux/of_gpio.h>
 
-#define DRV_VERSION "1.0"
+#define DRV_VERSION "1.1"
 #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];
+	struct gpio_desc *gpio[MDIO_MUX_GPIO_MAX_BITS];
 	unsigned int num_gpios;
 	void *mux_handle;
 };
@@ -28,29 +28,23 @@ struct mdio_mux_gpio_state {
 static int mdio_mux_gpio_switch_fn(int current_child, int desired_child,
 				   void *data)
 {
-	int change;
+	int values[MDIO_MUX_GPIO_MAX_BITS];
 	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;
+		values[n] = (desired_child >> n) & 1;
 	}
+	gpiod_set_array_cansleep(s->num_gpios, s->gpio, values);
 
 	return 0;
 }
 
 static int mdio_mux_gpio_probe(struct platform_device *pdev)
 {
-	enum of_gpio_flags f;
 	struct mdio_mux_gpio_state *s;
 	int num_gpios;
 	unsigned int n;
@@ -70,22 +64,14 @@ static int mdio_mux_gpio_probe(struct platform_device *pdev)
 	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;
+		struct gpio_desc *gpio = gpiod_get_index(&pdev->dev, NULL, n,
+							 GPIOD_OUT_LOW);
+		if (IS_ERR(gpio)) {
+			r = PTR_ERR(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,
@@ -98,15 +84,18 @@ static int mdio_mux_gpio_probe(struct platform_device *pdev)
 err:
 	while (n) {
 		n--;
-		gpio_free(s->gpio[n]);
+		gpiod_put(s->gpio[n]);
 	}
 	return r;
 }
 
 static int mdio_mux_gpio_remove(struct platform_device *pdev)
 {
+	unsigned int n;
 	struct mdio_mux_gpio_state *s = dev_get_platdata(&pdev->dev);
 	mdio_mux_uninit(s->mux_handle);
+	for (n = 0; n < s->num_gpios; n++)
+		gpiod_put(s->gpio[n]);
 	return 0;
 }
 
@@ -125,7 +114,6 @@ 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,
diff --git a/drivers/net/phy/mdio-mux-mmioreg.c b/drivers/net/phy/mdio-mux-mmioreg.c
index 1656785ff339..0aa985c74014 100644
--- a/drivers/net/phy/mdio-mux-mmioreg.c
+++ b/drivers/net/phy/mdio-mux-mmioreg.c
@@ -156,7 +156,6 @@ MODULE_DEVICE_TABLE(of, mdio_mux_mmioreg_match);
 static struct platform_driver mdio_mux_mmioreg_driver = {
 	.driver = {
 		.name		= "mdio-mux-mmioreg",
-		.owner		= THIS_MODULE,
 		.of_match_table = mdio_mux_mmioreg_match,
 	},
 	.probe		= mdio_mux_mmioreg_probe,
diff --git a/drivers/net/phy/mdio-octeon.c b/drivers/net/phy/mdio-octeon.c
index a51ed92fbada..c81052486edc 100644
--- a/drivers/net/phy/mdio-octeon.c
+++ b/drivers/net/phy/mdio-octeon.c
@@ -263,7 +263,6 @@ MODULE_DEVICE_TABLE(of, octeon_mdiobus_match);
 static struct platform_driver octeon_mdiobus_driver = {
 	.driver = {
 		.name		= "mdio-octeon",
-		.owner		= THIS_MODULE,
 		.of_match_table = octeon_mdiobus_match,
 	},
 	.probe		= octeon_mdiobus_probe,
diff --git a/drivers/net/phy/micrel.c b/drivers/net/phy/micrel.c
index c530de1e63f5..3ad8ca76196d 100644
--- a/drivers/net/phy/micrel.c
+++ b/drivers/net/phy/micrel.c
@@ -88,6 +88,7 @@ struct kszphy_priv {
 
 static const struct kszphy_type ksz8021_type = {
 	.led_mode_reg		= MII_KSZPHY_CTRL_2,
+	.has_broadcast_disable	= true,
 	.has_rmii_ref_clk_sel	= true,
 };
 
@@ -258,19 +259,6 @@ static int kszphy_config_init(struct phy_device *phydev)
 	return 0;
 }
 
-static int ksz8021_config_init(struct phy_device *phydev)
-{
-	int rc;
-
-	rc = kszphy_config_init(phydev);
-	if (rc)
-		return rc;
-
-	rc = kszphy_broadcast_disable(phydev);
-
-	return rc < 0 ? rc : 0;
-}
-
 static int ksz9021_load_values_from_of(struct phy_device *phydev,
 				       struct device_node *of_node, u16 reg,
 				       char *field1, char *field2,
@@ -584,7 +572,7 @@ static struct phy_driver ksphy_driver[] = {
 	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
 	.driver_data	= &ksz8021_type,
 	.probe		= kszphy_probe,
-	.config_init	= ksz8021_config_init,
+	.config_init	= kszphy_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= kszphy_ack_interrupt,
@@ -601,7 +589,7 @@ static struct phy_driver ksphy_driver[] = {
 	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
 	.driver_data	= &ksz8021_type,
 	.probe		= kszphy_probe,
-	.config_init	= ksz8021_config_init,
+	.config_init	= kszphy_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= kszphy_ack_interrupt,
diff --git a/drivers/net/tun.c b/drivers/net/tun.c
index a5cbf67517f0..c0df872f5b8c 100644
--- a/drivers/net/tun.c
+++ b/drivers/net/tun.c
@@ -110,9 +110,11 @@ do {								\
  * overload it to mean fasync when stored there.
  */
 #define TUN_FASYNC	IFF_ATTACH_QUEUE
+/* High bits in flags field are unused. */
+#define TUN_VNET_LE     0x80000000
 
 #define TUN_FEATURES (IFF_NO_PI | IFF_ONE_QUEUE | IFF_VNET_HDR | \
-		      IFF_VNET_LE | IFF_MULTI_QUEUE)
+		      IFF_MULTI_QUEUE)
 #define GOODCOPY_LEN 128
 
 #define FLT_EXACT_COUNT 8
@@ -208,12 +210,12 @@ struct tun_struct {
 
 static inline u16 tun16_to_cpu(struct tun_struct *tun, __virtio16 val)
 {
-	return __virtio16_to_cpu(tun->flags & IFF_VNET_LE, val);
+	return __virtio16_to_cpu(tun->flags & TUN_VNET_LE, val);
 }
 
 static inline __virtio16 cpu_to_tun16(struct tun_struct *tun, u16 val)
 {
-	return __cpu_to_virtio16(tun->flags & IFF_VNET_LE, val);
+	return __cpu_to_virtio16(tun->flags & TUN_VNET_LE, val);
 }
 
 static inline u32 tun_hashfn(u32 rxhash)
@@ -1378,7 +1380,7 @@ static ssize_t tun_do_read(struct tun_struct *tun, struct tun_file *tfile,
 	skb = __skb_recv_datagram(tfile->socket.sk, noblock ? MSG_DONTWAIT : 0,
 				  &peeked, &off, &err);
 	if (!skb)
-		return 0;
+		return err;
 
 	ret = tun_put_user(tun, tfile, skb, to);
 	if (unlikely(ret < 0))
@@ -1499,7 +1501,7 @@ static int tun_recvmsg(struct kiocb *iocb, struct socket *sock,
 		goto out;
 	}
 	ret = tun_do_read(tun, tfile, &m->msg_iter, flags & MSG_DONTWAIT);
-	if (ret > total_len) {
+	if (ret > (ssize_t)total_len) {
 		m->msg_flags |= MSG_TRUNC;
 		ret = flags & MSG_TRUNC ? ret : total_len;
 	}
@@ -1843,6 +1845,7 @@ static long __tun_chr_ioctl(struct file *file, unsigned int cmd,
 	int sndbuf;
 	int vnet_hdr_sz;
 	unsigned int ifindex;
+	int le;
 	int ret;
 
 	if (cmd == TUNSETIFF || cmd == TUNSETQUEUE || _IOC_TYPE(cmd) == 0x89) {
@@ -2042,6 +2045,23 @@ static long __tun_chr_ioctl(struct file *file, unsigned int cmd,
 		tun->vnet_hdr_sz = vnet_hdr_sz;
 		break;
 
+	case TUNGETVNETLE:
+		le = !!(tun->flags & TUN_VNET_LE);
+		if (put_user(le, (int __user *)argp))
+			ret = -EFAULT;
+		break;
+
+	case TUNSETVNETLE:
+		if (get_user(le, (int __user *)argp)) {
+			ret = -EFAULT;
+			break;
+		}
+		if (le)
+			tun->flags |= TUN_VNET_LE;
+		else
+			tun->flags &= ~TUN_VNET_LE;
+		break;
+
 	case TUNATTACHFILTER:
 		/* Can be set only for TAPs */
 		ret = -EINVAL;
diff --git a/drivers/net/virtio_net.c b/drivers/net/virtio_net.c
index b8bd7191572d..11e2e8131359 100644
--- a/drivers/net/virtio_net.c
+++ b/drivers/net/virtio_net.c
@@ -760,7 +760,6 @@ static int virtnet_poll(struct napi_struct *napi, int budget)
 		container_of(napi, struct receive_queue, napi);
 	unsigned int r, received = 0;
 
-again:
 	received += virtnet_receive(rq, budget - received);
 
 	/* Out of packets? */
@@ -771,7 +770,6 @@ again:
 		    napi_schedule_prep(napi)) {
 			virtqueue_disable_cb(rq->vq);
 			__napi_schedule(napi);
-			goto again;
 		}
 	}
 
@@ -1761,6 +1759,8 @@ static int virtnet_probe(struct virtio_device *vdev)
 		if (virtio_has_feature(vdev, VIRTIO_NET_F_HOST_ECN))
 			dev->hw_features |= NETIF_F_TSO_ECN;
 
+		dev->features |= NETIF_F_GSO_ROBUST;
+
 		if (gso)
 			dev->features |= dev->hw_features & NETIF_F_ALL_TSO;
 		/* (!csum && gso) case will be fixed by register_netdev() */
diff --git a/drivers/net/vxlan.c b/drivers/net/vxlan.c
index 49d9f2291998..2ab0922af0b4 100644
--- a/drivers/net/vxlan.c
+++ b/drivers/net/vxlan.c
@@ -549,10 +549,7 @@ static struct sk_buff **vxlan_gro_receive(struct sk_buff **head, struct sk_buff
 {
 	struct sk_buff *p, **pp = NULL;
 	struct vxlanhdr *vh, *vh2;
-	struct ethhdr *eh, *eh2;
-	unsigned int hlen, off_vx, off_eth;
-	const struct packet_offload *ptype;
-	__be16 type;
+	unsigned int hlen, off_vx;
 	int flush = 1;
 
 	off_vx = skb_gro_offset(skb);
@@ -563,17 +560,6 @@ static struct sk_buff **vxlan_gro_receive(struct sk_buff **head, struct sk_buff
 		if (unlikely(!vh))
 			goto out;
 	}
-	skb_gro_pull(skb, sizeof(struct vxlanhdr)); /* pull vxlan header */
-	skb_gro_postpull_rcsum(skb, vh, sizeof(struct vxlanhdr));
-
-	off_eth = skb_gro_offset(skb);
-	hlen = off_eth + sizeof(*eh);
-	eh   = skb_gro_header_fast(skb, off_eth);
-	if (skb_gro_header_hard(skb, hlen)) {
-		eh = skb_gro_header_slow(skb, hlen, off_eth);
-		if (unlikely(!eh))
-			goto out;
-	}
 
 	flush = 0;
 
@@ -582,28 +568,16 @@ static struct sk_buff **vxlan_gro_receive(struct sk_buff **head, struct sk_buff
 			continue;
 
 		vh2 = (struct vxlanhdr *)(p->data + off_vx);
-		eh2 = (struct ethhdr   *)(p->data + off_eth);
-		if (vh->vx_vni != vh2->vx_vni || compare_ether_header(eh, eh2)) {
+		if (vh->vx_vni != vh2->vx_vni) {
 			NAPI_GRO_CB(p)->same_flow = 0;
 			continue;
 		}
 	}
 
-	type = eh->h_proto;
-
-	rcu_read_lock();
-	ptype = gro_find_receive_by_type(type);
-	if (ptype == NULL) {
-		flush = 1;
-		goto out_unlock;
-	}
-
-	skb_gro_pull(skb, sizeof(*eh)); /* pull inner eth header */
-	skb_gro_postpull_rcsum(skb, eh, sizeof(*eh));
-	pp = ptype->callbacks.gro_receive(head, skb);
+	skb_gro_pull(skb, sizeof(struct vxlanhdr));
+	skb_gro_postpull_rcsum(skb, vh, sizeof(struct vxlanhdr));
+	pp = eth_gro_receive(head, skb);
 
-out_unlock:
-	rcu_read_unlock();
 out:
 	NAPI_GRO_CB(skb)->flush |= flush;
 
@@ -612,24 +586,9 @@ out:
 
 static int vxlan_gro_complete(struct sk_buff *skb, int nhoff)
 {
-	struct ethhdr *eh;
-	struct packet_offload *ptype;
-	__be16 type;
-	int vxlan_len  = sizeof(struct vxlanhdr) + sizeof(struct ethhdr);
-	int err = -ENOSYS;
-
 	udp_tunnel_gro_complete(skb, nhoff);
 
-	eh = (struct ethhdr *)(skb->data + nhoff + sizeof(struct vxlanhdr));
-	type = eh->h_proto;
-
-	rcu_read_lock();
-	ptype = gro_find_complete_by_type(type);
-	if (ptype != NULL)
-		err = ptype->callbacks.gro_complete(skb, nhoff + vxlan_len);
-
-	rcu_read_unlock();
-	return err;
+	return eth_gro_complete(skb, nhoff + sizeof(struct vxlanhdr));
 }
 
 /* Notify netdevs that UDP port started listening */
@@ -1579,8 +1538,10 @@ static int vxlan6_xmit_skb(struct vxlan_sock *vs,
 	bool udp_sum = !udp_get_no_check6_tx(vs->sock->sk);
 
 	skb = udp_tunnel_handle_offloads(skb, udp_sum);
-	if (IS_ERR(skb))
-		return -EINVAL;
+	if (IS_ERR(skb)) {
+		err = -EINVAL;
+		goto err;
+	}
 
 	skb_scrub_packet(skb, xnet);
 
@@ -1590,12 +1551,16 @@ static int vxlan6_xmit_skb(struct vxlan_sock *vs,
 
 	/* Need space for new headers (invalidates iph ptr) */
 	err = skb_cow_head(skb, min_headroom);
-	if (unlikely(err))
-		return err;
+	if (unlikely(err)) {
+		kfree_skb(skb);
+		goto err;
+	}
 
 	skb = vlan_hwaccel_push_inside(skb);
-	if (WARN_ON(!skb))
-		return -ENOMEM;
+	if (WARN_ON(!skb)) {
+		err = -ENOMEM;
+		goto err;
+	}
 
 	vxh = (struct vxlanhdr *) __skb_push(skb, sizeof(*vxh));
 	vxh->vx_flags = htonl(VXLAN_FLAGS);
@@ -1606,6 +1571,9 @@ static int vxlan6_xmit_skb(struct vxlan_sock *vs,
 	udp_tunnel6_xmit_skb(vs->sock, dst, skb, dev, saddr, daddr, prio,
 			     ttl, src_port, dst_port);
 	return 0;
+err:
+	dst_release(dst);
+	return err;
 }
 #endif
 
@@ -1621,7 +1589,7 @@ int vxlan_xmit_skb(struct vxlan_sock *vs,
 
 	skb = udp_tunnel_handle_offloads(skb, udp_sum);
 	if (IS_ERR(skb))
-		return -EINVAL;
+		return PTR_ERR(skb);
 
 	min_headroom = LL_RESERVED_SPACE(rt->dst.dev) + rt->dst.header_len
 			+ VXLAN_HLEN + sizeof(struct iphdr)
@@ -1629,8 +1597,10 @@ int vxlan_xmit_skb(struct vxlan_sock *vs,
 
 	/* Need space for new headers (invalidates iph ptr) */
 	err = skb_cow_head(skb, min_headroom);
-	if (unlikely(err))
+	if (unlikely(err)) {
+		kfree_skb(skb);
 		return err;
+	}
 
 	skb = vlan_hwaccel_push_inside(skb);
 	if (WARN_ON(!skb))
@@ -1776,9 +1746,12 @@ static void vxlan_xmit_one(struct sk_buff *skb, struct net_device *dev,
 				     tos, ttl, df, src_port, dst_port,
 				     htonl(vni << 8),
 				     !net_eq(vxlan->net, dev_net(vxlan->dev)));
-
-		if (err < 0)
+		if (err < 0) {
+			/* skb is already freed. */
+			skb = NULL;
 			goto rt_tx_error;
+		}
+
 		iptunnel_xmit_stats(err, &dev->stats, dev->tstats);
 #if IS_ENABLED(CONFIG_IPV6)
 	} else {
diff --git a/drivers/net/wireless/adm8211.c b/drivers/net/wireless/adm8211.c
index 17fcaabb2687..f07a61899545 100644
--- a/drivers/net/wireless/adm8211.c
+++ b/drivers/net/wireless/adm8211.c
@@ -1837,6 +1837,7 @@ static int adm8211_probe(struct pci_dev *pdev,
 	if (!priv->map) {
 		printk(KERN_ERR "%s (adm8211): Cannot map device memory\n",
 		       pci_name(pdev));
+		err = -ENOMEM;
 		goto err_free_dev;
 	}
 
diff --git a/drivers/net/wireless/ath/ath10k/Makefile b/drivers/net/wireless/ath/ath10k/Makefile
index 8b1b1adb477a..ffa3b1a8745f 100644
--- a/drivers/net/wireless/ath/ath10k/Makefile
+++ b/drivers/net/wireless/ath/ath10k/Makefile
@@ -8,11 +8,13 @@ ath10k_core-y += mac.o \
 		 htt_tx.o \
 		 txrx.o \
 		 wmi.o \
+		 wmi-tlv.o \
 		 bmi.o
 
 ath10k_core-$(CONFIG_ATH10K_DEBUGFS) += spectral.o
 ath10k_core-$(CONFIG_NL80211_TESTMODE) += testmode.o
 ath10k_core-$(CONFIG_ATH10K_TRACING) += trace.o
+ath10k_core-$(CONFIG_THERMAL) += thermal.o
 
 obj-$(CONFIG_ATH10K_PCI) += ath10k_pci.o
 ath10k_pci-y += pci.o \
diff --git a/drivers/net/wireless/ath/ath10k/ce.c b/drivers/net/wireless/ath/ath10k/ce.c
index a156e6e48708..42ec79327943 100644
--- a/drivers/net/wireless/ath/ath10k/ce.c
+++ b/drivers/net/wireless/ath/ath10k/ce.c
@@ -1093,6 +1093,8 @@ int ath10k_ce_alloc_pipe(struct ath10k *ar, int ce_id,
 		     (CE_HTT_H2T_MSG_SRC_NENTRIES - 1));
 	BUILD_BUG_ON(2*TARGET_10X_NUM_MSDU_DESC >
 		     (CE_HTT_H2T_MSG_SRC_NENTRIES - 1));
+	BUILD_BUG_ON(2*TARGET_TLV_NUM_MSDU_DESC >
+		     (CE_HTT_H2T_MSG_SRC_NENTRIES - 1));
 
 	ce_state->ar = ar;
 	ce_state->id = ce_id;
diff --git a/drivers/net/wireless/ath/ath10k/core.c b/drivers/net/wireless/ath/ath10k/core.c
index 7762061a1944..2d0671ebcf2b 100644
--- a/drivers/net/wireless/ath/ath10k/core.c
+++ b/drivers/net/wireless/ath/ath10k/core.c
@@ -17,6 +17,7 @@
 
 #include <linux/module.h>
 #include <linux/firmware.h>
+#include <linux/of.h>
 
 #include "core.h"
 #include "mac.h"
@@ -27,20 +28,18 @@
 #include "debug.h"
 #include "htt.h"
 #include "testmode.h"
+#include "wmi-ops.h"
 
 unsigned int ath10k_debug_mask;
 static bool uart_print;
-static unsigned int ath10k_p2p;
 static bool skip_otp;
 
 module_param_named(debug_mask, ath10k_debug_mask, uint, 0644);
 module_param(uart_print, bool, 0644);
-module_param_named(p2p, ath10k_p2p, uint, 0644);
 module_param(skip_otp, bool, 0644);
 
 MODULE_PARM_DESC(debug_mask, "Debugging mask");
 MODULE_PARM_DESC(uart_print, "Uart target debugging");
-MODULE_PARM_DESC(p2p, "Enable ath10k P2P support");
 MODULE_PARM_DESC(skip_otp, "Skip otp failure for calibration in testmode");
 
 static const struct ath10k_hw_params ath10k_hw_params_list[] = {
@@ -48,11 +47,14 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 		.id = QCA988X_HW_2_0_VERSION,
 		.name = "qca988x hw2.0",
 		.patch_load_addr = QCA988X_HW_2_0_PATCH_LOAD_ADDR,
+		.uart_pin = 7,
 		.fw = {
 			.dir = QCA988X_HW_2_0_FW_DIR,
 			.fw = QCA988X_HW_2_0_FW_FILE,
 			.otp = QCA988X_HW_2_0_OTP_FILE,
 			.board = QCA988X_HW_2_0_BOARD_DATA_FILE,
+			.board_size = QCA988X_BOARD_DATA_SZ,
+			.board_ext_size = QCA988X_BOARD_EXT_DATA_SZ,
 		},
 	},
 };
@@ -146,8 +148,8 @@ static const struct firmware *ath10k_fetch_fw_file(struct ath10k *ar,
 static int ath10k_push_board_ext_data(struct ath10k *ar, const void *data,
 				      size_t data_len)
 {
-	u32 board_data_size = QCA988X_BOARD_DATA_SZ;
-	u32 board_ext_data_size = QCA988X_BOARD_EXT_DATA_SZ;
+	u32 board_data_size = ar->hw_params.fw.board_size;
+	u32 board_ext_data_size = ar->hw_params.fw.board_ext_size;
 	u32 board_ext_data_addr;
 	int ret;
 
@@ -193,7 +195,7 @@ static int ath10k_push_board_ext_data(struct ath10k *ar, const void *data,
 static int ath10k_download_board_data(struct ath10k *ar, const void *data,
 				      size_t data_len)
 {
-	u32 board_data_size = QCA988X_BOARD_DATA_SZ;
+	u32 board_data_size = ar->hw_params.fw.board_size;
 	u32 address;
 	int ret;
 
@@ -249,6 +251,63 @@ static int ath10k_download_cal_file(struct ath10k *ar)
 	return 0;
 }
 
+static int ath10k_download_cal_dt(struct ath10k *ar)
+{
+	struct device_node *node;
+	int data_len;
+	void *data;
+	int ret;
+
+	node = ar->dev->of_node;
+	if (!node)
+		/* Device Tree is optional, don't print any warnings if
+		 * there's no node for ath10k.
+		 */
+		return -ENOENT;
+
+	if (!of_get_property(node, "qcom,ath10k-calibration-data",
+			     &data_len)) {
+		/* The calibration data node is optional */
+		return -ENOENT;
+	}
+
+	if (data_len != QCA988X_CAL_DATA_LEN) {
+		ath10k_warn(ar, "invalid calibration data length in DT: %d\n",
+			    data_len);
+		ret = -EMSGSIZE;
+		goto out;
+	}
+
+	data = kmalloc(data_len, GFP_KERNEL);
+	if (!data) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	ret = of_property_read_u8_array(node, "qcom,ath10k-calibration-data",
+					data, data_len);
+	if (ret) {
+		ath10k_warn(ar, "failed to read calibration data from DT: %d\n",
+			    ret);
+		goto out_free;
+	}
+
+	ret = ath10k_download_board_data(ar, data, data_len);
+	if (ret) {
+		ath10k_warn(ar, "failed to download calibration data from Device Tree: %d\n",
+			    ret);
+		goto out_free;
+	}
+
+	ret = 0;
+
+out_free:
+	kfree(data);
+
+out:
+	return ret;
+}
+
 static int ath10k_download_and_run_otp(struct ath10k *ar)
 {
 	u32 result, address = ar->hw_params.patch_load_addr;
@@ -447,7 +506,7 @@ static int ath10k_core_fetch_firmware_api_n(struct ath10k *ar, const char *name)
 	int ie_id, i, index, bit, ret;
 	struct ath10k_fw_ie *hdr;
 	const u8 *data;
-	__le32 *timestamp;
+	__le32 *timestamp, *version;
 
 	/* first fetch the firmware file (firmware-*.bin) */
 	ar->firmware = ath10k_fetch_fw_file(ar, ar->hw_params.fw.dir, name);
@@ -562,6 +621,17 @@ static int ath10k_core_fetch_firmware_api_n(struct ath10k *ar, const char *name)
 			ar->otp_len = ie_len;
 
 			break;
+		case ATH10K_FW_IE_WMI_OP_VERSION:
+			if (ie_len != sizeof(u32))
+				break;
+
+			version = (__le32 *)data;
+
+			ar->wmi.op_version = le32_to_cpup(version);
+
+			ath10k_dbg(ar, ATH10K_DBG_BOOT, "found fw ie wmi op version %d\n",
+				   ar->wmi.op_version);
+			break;
 		default:
 			ath10k_warn(ar, "Unknown FW IE: %u\n",
 				    le32_to_cpu(hdr->id));
@@ -582,13 +652,6 @@ static int ath10k_core_fetch_firmware_api_n(struct ath10k *ar, const char *name)
 		goto err;
 	}
 
-	if (test_bit(ATH10K_FW_FEATURE_WMI_10_2, ar->fw_features) &&
-	    !test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features)) {
-		ath10k_err(ar, "feature bits corrupted: 10.2 feature requires 10.x feature to be set as well");
-		ret = -EINVAL;
-		goto err;
-	}
-
 	/* now fetch the board file */
 	if (ar->hw_params.fw.board == NULL) {
 		ath10k_err(ar, "board data file not defined");
@@ -624,6 +687,13 @@ static int ath10k_core_fetch_firmware_files(struct ath10k *ar)
 	/* calibration file is optional, don't check for any errors */
 	ath10k_fetch_cal_file(ar);
 
+	ar->fw_api = 4;
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "trying fw api %d\n", ar->fw_api);
+
+	ret = ath10k_core_fetch_firmware_api_n(ar, ATH10K_FW_API4_FILE);
+	if (ret == 0)
+		goto success;
+
 	ar->fw_api = 3;
 	ath10k_dbg(ar, ATH10K_DBG_BOOT, "trying fw api %d\n", ar->fw_api);
 
@@ -662,7 +732,17 @@ static int ath10k_download_cal_data(struct ath10k *ar)
 	}
 
 	ath10k_dbg(ar, ATH10K_DBG_BOOT,
-		   "boot did not find a calibration file, try OTP next: %d\n",
+		   "boot did not find a calibration file, try DT next: %d\n",
+		   ret);
+
+	ret = ath10k_download_cal_dt(ar);
+	if (ret == 0) {
+		ar->cal_mode = ATH10K_CAL_MODE_DT;
+		goto done;
+	}
+
+	ath10k_dbg(ar, ATH10K_DBG_BOOT,
+		   "boot did not find DT entry, try OTP next: %d\n",
 		   ret);
 
 	ret = ath10k_download_and_run_otp(ar);
@@ -696,7 +776,7 @@ static int ath10k_init_uart(struct ath10k *ar)
 	if (!uart_print)
 		return 0;
 
-	ret = ath10k_bmi_write32(ar, hi_dbg_uart_txpin, 7);
+	ret = ath10k_bmi_write32(ar, hi_dbg_uart_txpin, ar->hw_params.uart_pin);
 	if (ret) {
 		ath10k_warn(ar, "could not enable UART prints (%d)\n", ret);
 		return ret;
@@ -764,6 +844,7 @@ static void ath10k_core_restart(struct work_struct *work)
 	complete_all(&ar->offchan_tx_completed);
 	complete_all(&ar->install_key_done);
 	complete_all(&ar->vdev_setup_done);
+	complete_all(&ar->thermal.wmi_sync);
 	wake_up(&ar->htt.empty_tx_wq);
 	wake_up(&ar->wmi.tx_credits_wq);
 	wake_up(&ar->peer_mapping_wq);
@@ -799,15 +880,62 @@ static void ath10k_core_restart(struct work_struct *work)
 	mutex_unlock(&ar->conf_mutex);
 }
 
-static void ath10k_core_init_max_sta_count(struct ath10k *ar)
+static int ath10k_core_init_firmware_features(struct ath10k *ar)
 {
-	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features)) {
-		ar->max_num_peers = TARGET_10X_NUM_PEERS;
-		ar->max_num_stations = TARGET_10X_NUM_STATIONS;
-	} else {
+	if (test_bit(ATH10K_FW_FEATURE_WMI_10_2, ar->fw_features) &&
+	    !test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features)) {
+		ath10k_err(ar, "feature bits corrupted: 10.2 feature requires 10.x feature to be set as well");
+		return -EINVAL;
+	}
+
+	if (ar->wmi.op_version >= ATH10K_FW_WMI_OP_VERSION_MAX) {
+		ath10k_err(ar, "unsupported WMI OP version (max %d): %d\n",
+			   ATH10K_FW_WMI_OP_VERSION_MAX, ar->wmi.op_version);
+		return -EINVAL;
+	}
+
+	/* Backwards compatibility for firmwares without
+	 * ATH10K_FW_IE_WMI_OP_VERSION.
+	 */
+	if (ar->wmi.op_version == ATH10K_FW_WMI_OP_VERSION_UNSET) {
+		if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features)) {
+			if (test_bit(ATH10K_FW_FEATURE_WMI_10_2,
+				     ar->fw_features))
+				ar->wmi.op_version = ATH10K_FW_WMI_OP_VERSION_10_2;
+			else
+				ar->wmi.op_version = ATH10K_FW_WMI_OP_VERSION_10_1;
+		} else {
+			ar->wmi.op_version = ATH10K_FW_WMI_OP_VERSION_MAIN;
+		}
+	}
+
+	switch (ar->wmi.op_version) {
+	case ATH10K_FW_WMI_OP_VERSION_MAIN:
 		ar->max_num_peers = TARGET_NUM_PEERS;
 		ar->max_num_stations = TARGET_NUM_STATIONS;
+		ar->max_num_vdevs = TARGET_NUM_VDEVS;
+		ar->htt.max_num_pending_tx = TARGET_NUM_MSDU_DESC;
+		break;
+	case ATH10K_FW_WMI_OP_VERSION_10_1:
+	case ATH10K_FW_WMI_OP_VERSION_10_2:
+	case ATH10K_FW_WMI_OP_VERSION_10_2_4:
+		ar->max_num_peers = TARGET_10X_NUM_PEERS;
+		ar->max_num_stations = TARGET_10X_NUM_STATIONS;
+		ar->max_num_vdevs = TARGET_10X_NUM_VDEVS;
+		ar->htt.max_num_pending_tx = TARGET_10X_NUM_MSDU_DESC;
+		break;
+	case ATH10K_FW_WMI_OP_VERSION_TLV:
+		ar->max_num_peers = TARGET_TLV_NUM_PEERS;
+		ar->max_num_stations = TARGET_TLV_NUM_STATIONS;
+		ar->htt.max_num_pending_tx = TARGET_TLV_NUM_MSDU_DESC;
+		break;
+	case ATH10K_FW_WMI_OP_VERSION_UNSET:
+	case ATH10K_FW_WMI_OP_VERSION_MAX:
+		WARN_ON(1);
+		return -EINVAL;
 	}
+
+	return 0;
 }
 
 int ath10k_core_start(struct ath10k *ar, enum ath10k_firmware_mode mode)
@@ -945,10 +1073,7 @@ int ath10k_core_start(struct ath10k *ar, enum ath10k_firmware_mode mode)
 	if (status)
 		goto err_hif_stop;
 
-	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features))
-		ar->free_vdev_map = (1LL << TARGET_10X_NUM_VDEVS) - 1;
-	else
-		ar->free_vdev_map = (1LL << TARGET_NUM_VDEVS) - 1;
+	ar->free_vdev_map = (1LL << ar->max_num_vdevs) - 1;
 
 	INIT_LIST_HEAD(&ar->arvifs);
 
@@ -1025,8 +1150,7 @@ static int ath10k_core_probe_fw(struct ath10k *ar)
 	ret = ath10k_bmi_get_target_info(ar, &target_info);
 	if (ret) {
 		ath10k_err(ar, "could not get target info (%d)\n", ret);
-		ath10k_hif_power_down(ar);
-		return ret;
+		goto err_power_down;
 	}
 
 	ar->target_version = target_info.version;
@@ -1035,28 +1159,28 @@ static int ath10k_core_probe_fw(struct ath10k *ar)
 	ret = ath10k_init_hw_params(ar);
 	if (ret) {
 		ath10k_err(ar, "could not get hw params (%d)\n", ret);
-		ath10k_hif_power_down(ar);
-		return ret;
+		goto err_power_down;
 	}
 
 	ret = ath10k_core_fetch_firmware_files(ar);
 	if (ret) {
 		ath10k_err(ar, "could not fetch firmware files (%d)\n", ret);
-		ath10k_hif_power_down(ar);
-		return ret;
+		goto err_power_down;
 	}
 
-	ath10k_core_init_max_sta_count(ar);
+	ret = ath10k_core_init_firmware_features(ar);
+	if (ret) {
+		ath10k_err(ar, "fatal problem with firmware features: %d\n",
+			   ret);
+		goto err_free_firmware_files;
+	}
 
 	mutex_lock(&ar->conf_mutex);
 
 	ret = ath10k_core_start(ar, ATH10K_FIRMWARE_MODE_NORMAL);
 	if (ret) {
 		ath10k_err(ar, "could not init core (%d)\n", ret);
-		ath10k_core_free_firmware_files(ar);
-		ath10k_hif_power_down(ar);
-		mutex_unlock(&ar->conf_mutex);
-		return ret;
+		goto err_unlock;
 	}
 
 	ath10k_print_driver_info(ar);
@@ -1066,34 +1190,17 @@ static int ath10k_core_probe_fw(struct ath10k *ar)
 
 	ath10k_hif_power_down(ar);
 	return 0;
-}
-
-static int ath10k_core_check_chip_id(struct ath10k *ar)
-{
-	u32 hw_revision = MS(ar->chip_id, SOC_CHIP_ID_REV);
-
-	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot chip_id 0x%08x hw_revision 0x%x\n",
-		   ar->chip_id, hw_revision);
 
-	/* Check that we are not using hw1.0 (some of them have same pci id
-	 * as hw2.0) before doing anything else as ath10k crashes horribly
-	 * due to missing hw1.0 workarounds. */
-	switch (hw_revision) {
-	case QCA988X_HW_1_0_CHIP_ID_REV:
-		ath10k_err(ar, "ERROR: qca988x hw1.0 is not supported\n");
-		return -EOPNOTSUPP;
+err_unlock:
+	mutex_unlock(&ar->conf_mutex);
 
-	case QCA988X_HW_2_0_CHIP_ID_REV:
-		/* known hardware revision, continue normally */
-		return 0;
+err_free_firmware_files:
+	ath10k_core_free_firmware_files(ar);
 
-	default:
-		ath10k_warn(ar, "Warning: hardware revision unknown (0x%x), expect problems\n",
-			    ar->chip_id);
-		return 0;
-	}
+err_power_down:
+	ath10k_hif_power_down(ar);
 
-	return 0;
+	return ret;
 }
 
 static void ath10k_core_register_work(struct work_struct *work)
@@ -1125,9 +1232,18 @@ static void ath10k_core_register_work(struct work_struct *work)
 		goto err_debug_destroy;
 	}
 
+	status = ath10k_thermal_register(ar);
+	if (status) {
+		ath10k_err(ar, "could not register thermal device: %d\n",
+			   status);
+		goto err_spectral_destroy;
+	}
+
 	set_bit(ATH10K_FLAG_CORE_REGISTERED, &ar->dev_flags);
 	return;
 
+err_spectral_destroy:
+	ath10k_spectral_destroy(ar);
 err_debug_destroy:
 	ath10k_debug_destroy(ar);
 err_unregister_mac:
@@ -1143,16 +1259,7 @@ err:
 
 int ath10k_core_register(struct ath10k *ar, u32 chip_id)
 {
-	int status;
-
 	ar->chip_id = chip_id;
-
-	status = ath10k_core_check_chip_id(ar);
-	if (status) {
-		ath10k_err(ar, "Unsupported chip id 0x%08x\n", ar->chip_id);
-		return status;
-	}
-
 	queue_work(ar->workqueue, &ar->register_work);
 
 	return 0;
@@ -1166,6 +1273,7 @@ void ath10k_core_unregister(struct ath10k *ar)
 	if (!test_bit(ATH10K_FLAG_CORE_REGISTERED, &ar->dev_flags))
 		return;
 
+	ath10k_thermal_unregister(ar);
 	/* Stop spectral before unregistering from mac80211 to remove the
 	 * relayfs debugfs file cleanly. Otherwise the parent debugfs tree
 	 * would be already be free'd recursively, leading to a double free.
@@ -1198,10 +1306,7 @@ struct ath10k *ath10k_core_create(size_t priv_size, struct device *dev,
 
 	ar->ath_common.priv = ar;
 	ar->ath_common.hw = ar->hw;
-
-	ar->p2p = !!ath10k_p2p;
 	ar->dev = dev;
-
 	ar->hif.ops = hif_ops;
 	ar->hif.bus = bus;
 
@@ -1212,6 +1317,7 @@ struct ath10k *ath10k_core_create(size_t priv_size, struct device *dev,
 
 	init_completion(&ar->install_key_done);
 	init_completion(&ar->vdev_setup_done);
+	init_completion(&ar->thermal.wmi_sync);
 
 	INIT_DELAYED_WORK(&ar->scan.timeout, ath10k_scan_timeout_work);
 
diff --git a/drivers/net/wireless/ath/ath10k/core.h b/drivers/net/wireless/ath/ath10k/core.h
index 514c219263a5..7b6d9e4567a3 100644
--- a/drivers/net/wireless/ath/ath10k/core.h
+++ b/drivers/net/wireless/ath/ath10k/core.h
@@ -34,6 +34,7 @@
 #include "../regd.h"
 #include "../dfs_pattern_detector.h"
 #include "spectral.h"
+#include "thermal.h"
 
 #define MS(_v, _f) (((_v) & _f##_MASK) >> _f##_LSB)
 #define SM(_v, _f) (((_v) << _f##_LSB) & _f##_MASK)
@@ -120,6 +121,7 @@ struct ath10k_mem_chunk {
 };
 
 struct ath10k_wmi {
+	enum ath10k_fw_wmi_op_version op_version;
 	enum ath10k_htc_ep_id eid;
 	struct completion service_ready;
 	struct completion unified_ready;
@@ -128,6 +130,7 @@ struct ath10k_wmi {
 	struct wmi_cmd_map *cmd;
 	struct wmi_vdev_param_map *vdev_param;
 	struct wmi_pdev_param_map *pdev_param;
+	const struct wmi_ops *ops;
 
 	u32 num_mem_chunks;
 	struct ath10k_mem_chunk mem_chunks[WMI_MAX_MEM_REQS];
@@ -325,6 +328,7 @@ struct ath10k_debug {
 	u32 fw_dbglog_mask;
 	u32 pktlog_filter;
 	u32 reg_addr;
+	u32 nf_cal_period;
 
 	u8 htt_max_amsdu;
 	u8 htt_max_ampdu;
@@ -369,7 +373,7 @@ enum ath10k_fw_features {
 	/* wmi_mgmt_rx_hdr contains extra RSSI information */
 	ATH10K_FW_FEATURE_EXT_WMI_MGMT_RX = 0,
 
-	/* firmware from 10X branch */
+	/* Firmware from 10X branch. Deprecated, don't use in new code. */
 	ATH10K_FW_FEATURE_WMI_10X = 1,
 
 	/* firmware support tx frame management over WMI, otherwise it's HTT */
@@ -378,8 +382,9 @@ enum ath10k_fw_features {
 	/* Firmware does not support P2P */
 	ATH10K_FW_FEATURE_NO_P2P = 3,
 
-	/* Firmware 10.2 feature bit. The ATH10K_FW_FEATURE_WMI_10X feature bit
-	 * is required to be set as well.
+	/* Firmware 10.2 feature bit. The ATH10K_FW_FEATURE_WMI_10X feature
+	 * bit is required to be set as well. Deprecated, don't use in new
+	 * code.
 	 */
 	ATH10K_FW_FEATURE_WMI_10_2 = 4,
 
@@ -401,6 +406,7 @@ enum ath10k_dev_flags {
 enum ath10k_cal_mode {
 	ATH10K_CAL_MODE_FILE,
 	ATH10K_CAL_MODE_OTP,
+	ATH10K_CAL_MODE_DT,
 };
 
 static inline const char *ath10k_cal_mode_str(enum ath10k_cal_mode mode)
@@ -410,6 +416,8 @@ static inline const char *ath10k_cal_mode_str(enum ath10k_cal_mode mode)
 		return "file";
 	case ATH10K_CAL_MODE_OTP:
 		return "otp";
+	case ATH10K_CAL_MODE_DT:
+		return "dt";
 	}
 
 	return "unknown";
@@ -480,12 +488,15 @@ struct ath10k {
 		u32 id;
 		const char *name;
 		u32 patch_load_addr;
+		int uart_pin;
 
 		struct ath10k_hw_params_fw {
 			const char *dir;
 			const char *fw;
 			const char *otp;
 			const char *board;
+			size_t board_size;
+			size_t board_ext_size;
 		} fw;
 	} hw_params;
 
@@ -571,6 +582,7 @@ struct ath10k {
 
 	int max_num_peers;
 	int max_num_stations;
+	int max_num_vdevs;
 
 	struct work_struct offchan_tx_work;
 	struct sk_buff_head offchan_tx_queue;
@@ -610,6 +622,7 @@ struct ath10k {
 		/* protected by conf_mutex */
 		const struct firmware *utf;
 		DECLARE_BITMAP(orig_fw_features, ATH10K_FW_FEATURE_COUNT);
+		enum ath10k_fw_wmi_op_version orig_wmi_op_version;
 
 		/* protected by data_lock */
 		bool utf_monitor;
@@ -622,6 +635,8 @@ struct ath10k {
 		u32 fw_cold_reset_counter;
 	} stats;
 
+	struct ath10k_thermal thermal;
+
 	/* must be last */
 	u8 drv_priv[0] __aligned(sizeof(void *));
 };
diff --git a/drivers/net/wireless/ath/ath10k/debug.c b/drivers/net/wireless/ath/ath10k/debug.c
index a716758f14b0..6ca24427e184 100644
--- a/drivers/net/wireless/ath/ath10k/debug.c
+++ b/drivers/net/wireless/ath/ath10k/debug.c
@@ -23,6 +23,7 @@
 #include "core.h"
 #include "debug.h"
 #include "hif.h"
+#include "wmi-ops.h"
 
 /* ms */
 #define ATH10K_DEBUG_HTT_STATS_INTERVAL 1000
@@ -123,7 +124,7 @@ EXPORT_SYMBOL(ath10k_info);
 
 void ath10k_print_driver_info(struct ath10k *ar)
 {
-	ath10k_info(ar, "%s (0x%08x, 0x%08x) fw %s api %d htt %d.%d wmi %d.%d.%d.%d cal %s max_sta %d\n",
+	ath10k_info(ar, "%s (0x%08x, 0x%08x) fw %s api %d htt %d.%d wmi %d cal %s max_sta %d\n",
 		    ar->hw_params.name,
 		    ar->target_version,
 		    ar->chip_id,
@@ -131,10 +132,7 @@ void ath10k_print_driver_info(struct ath10k *ar)
 		    ar->fw_api,
 		    ar->htt.target_version_major,
 		    ar->htt.target_version_minor,
-		    ar->fw_version_major,
-		    ar->fw_version_minor,
-		    ar->fw_version_release,
-		    ar->fw_version_build,
+		    ar->wmi.op_version,
 		    ath10k_cal_mode_str(ar->cal_mode),
 		    ar->max_num_stations);
 	ath10k_info(ar, "debug %d debugfs %d tracing %d dfs %d testmode %d\n",
@@ -1607,6 +1605,73 @@ static const struct file_operations fops_cal_data = {
 	.llseek = default_llseek,
 };
 
+static ssize_t ath10k_read_nf_cal_period(struct file *file,
+					 char __user *user_buf,
+					 size_t count, loff_t *ppos)
+{
+	struct ath10k *ar = file->private_data;
+	unsigned int len;
+	char buf[32];
+
+	len = scnprintf(buf, sizeof(buf), "%d\n",
+			ar->debug.nf_cal_period);
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, len);
+}
+
+static ssize_t ath10k_write_nf_cal_period(struct file *file,
+					  const char __user *user_buf,
+					  size_t count, loff_t *ppos)
+{
+	struct ath10k *ar = file->private_data;
+	unsigned long period;
+	int ret;
+
+	ret = kstrtoul_from_user(user_buf, count, 0, &period);
+	if (ret)
+		return ret;
+
+	if (period > WMI_PDEV_PARAM_CAL_PERIOD_MAX)
+		return -EINVAL;
+
+	/* there's no way to switch back to the firmware default */
+	if (period == 0)
+		return -EINVAL;
+
+	mutex_lock(&ar->conf_mutex);
+
+	ar->debug.nf_cal_period = period;
+
+	if (ar->state != ATH10K_STATE_ON) {
+		/* firmware is not running, nothing else to do */
+		ret = count;
+		goto exit;
+	}
+
+	ret = ath10k_wmi_pdev_set_param(ar, ar->wmi.pdev_param->cal_period,
+					ar->debug.nf_cal_period);
+	if (ret) {
+		ath10k_warn(ar, "cal period cfg failed from debugfs: %d\n",
+			    ret);
+		goto exit;
+	}
+
+	ret = count;
+
+exit:
+	mutex_unlock(&ar->conf_mutex);
+
+	return ret;
+}
+
+static const struct file_operations fops_nf_cal_period = {
+	.read = ath10k_read_nf_cal_period,
+	.write = ath10k_write_nf_cal_period,
+	.open = simple_open,
+	.owner = THIS_MODULE,
+	.llseek = default_llseek,
+};
+
 int ath10k_debug_start(struct ath10k *ar)
 {
 	int ret;
@@ -1642,6 +1707,16 @@ int ath10k_debug_start(struct ath10k *ar)
 			ath10k_warn(ar, "failed to disable pktlog: %d\n", ret);
 	}
 
+	if (ar->debug.nf_cal_period) {
+		ret = ath10k_wmi_pdev_set_param(ar,
+						ar->wmi.pdev_param->cal_period,
+						ar->debug.nf_cal_period);
+		if (ret)
+			/* not serious */
+			ath10k_warn(ar, "cal period cfg failed from debug start: %d\n",
+				    ret);
+	}
+
 	return ret;
 }
 
@@ -1880,6 +1955,9 @@ int ath10k_debug_register(struct ath10k *ar)
 	debugfs_create_file("cal_data", S_IRUSR, ar->debug.debugfs_phy,
 			    ar, &fops_cal_data);
 
+	debugfs_create_file("nf_cal_period", S_IRUSR | S_IWUSR,
+			    ar->debug.debugfs_phy, ar, &fops_nf_cal_period);
+
 	if (config_enabled(CONFIG_ATH10K_DFS_CERTIFIED)) {
 		debugfs_create_file("dfs_simulate_radar", S_IWUSR,
 				    ar->debug.debugfs_phy, ar,
diff --git a/drivers/net/wireless/ath/ath10k/htt_tx.c b/drivers/net/wireless/ath/ath10k/htt_tx.c
index 4bc51d8a14a3..a1bda41fb543 100644
--- a/drivers/net/wireless/ath/ath10k/htt_tx.c
+++ b/drivers/net/wireless/ath/ath10k/htt_tx.c
@@ -93,11 +93,6 @@ int ath10k_htt_tx_alloc(struct ath10k_htt *htt)
 
 	spin_lock_init(&htt->tx_lock);
 
-	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, htt->ar->fw_features))
-		htt->max_num_pending_tx = TARGET_10X_NUM_MSDU_DESC;
-	else
-		htt->max_num_pending_tx = TARGET_NUM_MSDU_DESC;
-
 	ath10k_dbg(ar, ATH10K_DBG_BOOT, "htt tx max num pending tx %d\n",
 		   htt->max_num_pending_tx);
 
diff --git a/drivers/net/wireless/ath/ath10k/hw.h b/drivers/net/wireless/ath/ath10k/hw.h
index dfedfd0e0f34..5729901923ac 100644
--- a/drivers/net/wireless/ath/ath10k/hw.h
+++ b/drivers/net/wireless/ath/ath10k/hw.h
@@ -37,6 +37,9 @@
 #define ATH10K_FW_API2_FILE		"firmware-2.bin"
 #define ATH10K_FW_API3_FILE		"firmware-3.bin"
 
+/* added support for ATH10K_FW_IE_WMI_OP_VERSION */
+#define ATH10K_FW_API4_FILE		"firmware-4.bin"
+
 #define ATH10K_FW_UTF_FILE		"utf.bin"
 
 /* includes also the null byte */
@@ -58,6 +61,24 @@ enum ath10k_fw_ie_type {
 	ATH10K_FW_IE_FEATURES = 2,
 	ATH10K_FW_IE_FW_IMAGE = 3,
 	ATH10K_FW_IE_OTP_IMAGE = 4,
+
+	/* WMI "operations" interface version, 32 bit value. Supported from
+	 * FW API 4 and above.
+	 */
+	ATH10K_FW_IE_WMI_OP_VERSION = 5,
+};
+
+enum ath10k_fw_wmi_op_version {
+	ATH10K_FW_WMI_OP_VERSION_UNSET = 0,
+
+	ATH10K_FW_WMI_OP_VERSION_MAIN = 1,
+	ATH10K_FW_WMI_OP_VERSION_10_1 = 2,
+	ATH10K_FW_WMI_OP_VERSION_10_2 = 3,
+	ATH10K_FW_WMI_OP_VERSION_TLV = 4,
+	ATH10K_FW_WMI_OP_VERSION_10_2_4 = 5,
+
+	/* keep last */
+	ATH10K_FW_WMI_OP_VERSION_MAX,
 };
 
 /* Known pecularities:
@@ -162,6 +183,15 @@ struct ath10k_pktlog_hdr {
 #define TARGET_10X_NUM_MSDU_DESC		(1024 + 400)
 #define TARGET_10X_MAX_FRAG_ENTRIES		0
 
+/* Target specific defines for WMI-TLV firmware */
+#define TARGET_TLV_NUM_VDEVS			3
+#define TARGET_TLV_NUM_STATIONS			32
+#define TARGET_TLV_NUM_PEERS			((TARGET_TLV_NUM_STATIONS) + \
+						 (TARGET_TLV_NUM_VDEVS) + \
+						 2)
+#define TARGET_TLV_NUM_TIDS			((TARGET_TLV_NUM_PEERS) * 2)
+#define TARGET_TLV_NUM_MSDU_DESC		(1024 + 32)
+
 /* Number of Copy Engines supported */
 #define CE_COUNT 8
 
diff --git a/drivers/net/wireless/ath/ath10k/mac.c b/drivers/net/wireless/ath/ath10k/mac.c
index 2619db1e3e74..08f293411bf0 100644
--- a/drivers/net/wireless/ath/ath10k/mac.c
+++ b/drivers/net/wireless/ath/ath10k/mac.c
@@ -27,6 +27,8 @@
 #include "htt.h"
 #include "txrx.h"
 #include "testmode.h"
+#include "wmi.h"
+#include "wmi-ops.h"
 
 /**********/
 /* Crypto */
@@ -267,7 +269,10 @@ chan_to_phymode(const struct cfg80211_chan_def *chandef)
 	case IEEE80211_BAND_2GHZ:
 		switch (chandef->width) {
 		case NL80211_CHAN_WIDTH_20_NOHT:
-			phymode = MODE_11G;
+			if (chandef->chan->flags & IEEE80211_CHAN_NO_OFDM)
+				phymode = MODE_11B;
+			else
+				phymode = MODE_11G;
 			break;
 		case NL80211_CHAN_WIDTH_20:
 			phymode = MODE_11NG_HT20;
@@ -1046,28 +1051,85 @@ static void ath10k_control_ibss(struct ath10k_vif *arvif,
 			    arvif->vdev_id, ret);
 }
 
-/*
- * Review this when mac80211 gains per-interface powersave support.
- */
+static int ath10k_mac_vif_recalc_ps_wake_threshold(struct ath10k_vif *arvif)
+{
+	struct ath10k *ar = arvif->ar;
+	u32 param;
+	u32 value;
+	int ret;
+
+	lockdep_assert_held(&arvif->ar->conf_mutex);
+
+	if (arvif->u.sta.uapsd)
+		value = WMI_STA_PS_TX_WAKE_THRESHOLD_NEVER;
+	else
+		value = WMI_STA_PS_TX_WAKE_THRESHOLD_ALWAYS;
+
+	param = WMI_STA_PS_PARAM_TX_WAKE_THRESHOLD;
+	ret = ath10k_wmi_set_sta_ps_param(ar, arvif->vdev_id, param, value);
+	if (ret) {
+		ath10k_warn(ar, "failed to submit ps wake threshold %u on vdev %i: %d\n",
+			    value, arvif->vdev_id, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ath10k_mac_vif_recalc_ps_poll_count(struct ath10k_vif *arvif)
+{
+	struct ath10k *ar = arvif->ar;
+	u32 param;
+	u32 value;
+	int ret;
+
+	lockdep_assert_held(&arvif->ar->conf_mutex);
+
+	if (arvif->u.sta.uapsd)
+		value = WMI_STA_PS_PSPOLL_COUNT_UAPSD;
+	else
+		value = WMI_STA_PS_PSPOLL_COUNT_NO_MAX;
+
+	param = WMI_STA_PS_PARAM_PSPOLL_COUNT;
+	ret = ath10k_wmi_set_sta_ps_param(ar, arvif->vdev_id,
+					  param, value);
+	if (ret) {
+		ath10k_warn(ar, "failed to submit ps poll count %u on vdev %i: %d\n",
+			    value, arvif->vdev_id, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
 static int ath10k_mac_vif_setup_ps(struct ath10k_vif *arvif)
 {
 	struct ath10k *ar = arvif->ar;
+	struct ieee80211_vif *vif = arvif->vif;
 	struct ieee80211_conf *conf = &ar->hw->conf;
 	enum wmi_sta_powersave_param param;
 	enum wmi_sta_ps_mode psmode;
 	int ret;
+	int ps_timeout;
 
 	lockdep_assert_held(&arvif->ar->conf_mutex);
 
 	if (arvif->vif->type != NL80211_IFTYPE_STATION)
 		return 0;
 
-	if (conf->flags & IEEE80211_CONF_PS) {
+	if (vif->bss_conf.ps) {
 		psmode = WMI_STA_PS_MODE_ENABLED;
 		param = WMI_STA_PS_PARAM_INACTIVITY_TIME;
 
+		ps_timeout = conf->dynamic_ps_timeout;
+		if (ps_timeout == 0) {
+			/* Firmware doesn't like 0 */
+			ps_timeout = ieee80211_tu_to_usec(
+				vif->bss_conf.beacon_int) / 1000;
+		}
+
 		ret = ath10k_wmi_set_sta_ps_param(ar, arvif->vdev_id, param,
-						  conf->dynamic_ps_timeout);
+						  ps_timeout);
 		if (ret) {
 			ath10k_warn(ar, "failed to set inactivity time for vdev %d: %i\n",
 				    arvif->vdev_id, ret);
@@ -1409,9 +1471,22 @@ static void ath10k_peer_assoc_h_qos(struct ath10k *ar,
 		if (vif->bss_conf.qos)
 			arg->peer_flags |= WMI_PEER_QOS;
 		break;
+	case WMI_VDEV_TYPE_IBSS:
+		if (sta->wme)
+			arg->peer_flags |= WMI_PEER_QOS;
+		break;
 	default:
 		break;
 	}
+
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac peer %pM qos %d\n",
+		   sta->addr, !!(arg->peer_flags & WMI_PEER_QOS));
+}
+
+static bool ath10k_mac_sta_has_11g_rates(struct ieee80211_sta *sta)
+{
+	/* First 4 rates in ath10k_rates are CCK (11b) rates. */
+	return sta->supp_rates[IEEE80211_BAND_2GHZ] >> 4;
 }
 
 static void ath10k_peer_assoc_h_phymode(struct ath10k *ar,
@@ -1428,8 +1503,10 @@ static void ath10k_peer_assoc_h_phymode(struct ath10k *ar,
 				phymode = MODE_11NG_HT40;
 			else
 				phymode = MODE_11NG_HT20;
-		} else {
+		} else if (ath10k_mac_sta_has_11g_rates(sta)) {
 			phymode = MODE_11G;
+		} else {
+			phymode = MODE_11B;
 		}
 
 		break;
@@ -2896,10 +2973,11 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 	arvif->vdev_id = bit;
 	arvif->vdev_subtype = WMI_VDEV_SUBTYPE_NONE;
 
-	if (ar->p2p)
-		arvif->vdev_subtype = WMI_VDEV_SUBTYPE_P2P_DEVICE;
-
 	switch (vif->type) {
+	case NL80211_IFTYPE_P2P_DEVICE:
+		arvif->vdev_type = WMI_VDEV_TYPE_STA;
+		arvif->vdev_subtype = WMI_VDEV_SUBTYPE_P2P_DEVICE;
+		break;
 	case NL80211_IFTYPE_UNSPECIFIED:
 	case NL80211_IFTYPE_STATION:
 		arvif->vdev_type = WMI_VDEV_TYPE_STA;
@@ -3028,22 +3106,16 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 			goto err_peer_delete;
 		}
 
-		param = WMI_STA_PS_PARAM_TX_WAKE_THRESHOLD;
-		value = WMI_STA_PS_TX_WAKE_THRESHOLD_ALWAYS;
-		ret = ath10k_wmi_set_sta_ps_param(ar, arvif->vdev_id,
-						  param, value);
+		ret = ath10k_mac_vif_recalc_ps_wake_threshold(arvif);
 		if (ret) {
-			ath10k_warn(ar, "failed to set vdev %i TX wake thresh: %d\n",
+			ath10k_warn(ar, "failed to recalc ps wake threshold on vdev %i: %d\n",
 				    arvif->vdev_id, ret);
 			goto err_peer_delete;
 		}
 
-		param = WMI_STA_PS_PARAM_PSPOLL_COUNT;
-		value = WMI_STA_PS_PSPOLL_COUNT_NO_MAX;
-		ret = ath10k_wmi_set_sta_ps_param(ar, arvif->vdev_id,
-						  param, value);
+		ret = ath10k_mac_vif_recalc_ps_poll_count(arvif);
 		if (ret) {
-			ath10k_warn(ar, "failed to set vdev %i PSPOLL count: %d\n",
+			ath10k_warn(ar, "failed to recalc ps poll count on vdev %i: %d\n",
 				    arvif->vdev_id, ret);
 			goto err_peer_delete;
 		}
@@ -3316,6 +3388,13 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 			ath10k_warn(ar, "failed to recalc tx power: %d\n", ret);
 	}
 
+	if (changed & BSS_CHANGED_PS) {
+		ret = ath10k_mac_vif_setup_ps(arvif);
+		if (ret)
+			ath10k_warn(ar, "failed to setup ps on vdev %i: %d\n",
+				    arvif->vdev_id, ret);
+	}
+
 	mutex_unlock(&ar->conf_mutex);
 }
 
@@ -3585,8 +3664,9 @@ static void ath10k_sta_rc_update_wk(struct work_struct *wk)
 				    sta->addr, smps, err);
 	}
 
-	if (changed & IEEE80211_RC_SUPP_RATES_CHANGED) {
-		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac update sta %pM supp rates\n",
+	if (changed & IEEE80211_RC_SUPP_RATES_CHANGED ||
+	    changed & IEEE80211_RC_NSS_CHANGED) {
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac update sta %pM supp rates/nss\n",
 			   sta->addr);
 
 		err = ath10k_station_assoc(ar, arvif->vif, sta, true);
@@ -3810,6 +3890,20 @@ static int ath10k_conf_tx_uapsd(struct ath10k *ar, struct ieee80211_vif *vif,
 	if (ret)
 		ath10k_warn(ar, "failed to set rx wake param: %d\n", ret);
 
+	ret = ath10k_mac_vif_recalc_ps_wake_threshold(arvif);
+	if (ret) {
+		ath10k_warn(ar, "failed to recalc ps wake threshold on vdev %i: %d\n",
+			    arvif->vdev_id, ret);
+		return ret;
+	}
+
+	ret = ath10k_mac_vif_recalc_ps_poll_count(arvif);
+	if (ret) {
+		ath10k_warn(ar, "failed to recalc ps poll count on vdev %i: %d\n",
+			    arvif->vdev_id, ret);
+		return ret;
+	}
+
 exit:
 	return ret;
 }
@@ -3991,29 +4085,6 @@ static int ath10k_set_rts_threshold(struct ieee80211_hw *hw, u32 value)
 	return ret;
 }
 
-static int ath10k_set_frag_threshold(struct ieee80211_hw *hw, u32 value)
-{
-	struct ath10k *ar = hw->priv;
-	struct ath10k_vif *arvif;
-	int ret = 0;
-
-	mutex_lock(&ar->conf_mutex);
-	list_for_each_entry(arvif, &ar->arvifs, list) {
-		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac vdev %d fragmentation threshold %d\n",
-			   arvif->vdev_id, value);
-
-		ret = ath10k_mac_set_frag(arvif, value);
-		if (ret) {
-			ath10k_warn(ar, "failed to set fragmentation threshold for vdev %d: %d\n",
-				    arvif->vdev_id, ret);
-			break;
-		}
-	}
-	mutex_unlock(&ar->conf_mutex);
-
-	return ret;
-}
-
 static void ath10k_flush(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 			 u32 queues, bool drop)
 {
@@ -4657,7 +4728,6 @@ static const struct ieee80211_ops ath10k_ops = {
 	.remain_on_channel		= ath10k_remain_on_channel,
 	.cancel_remain_on_channel	= ath10k_cancel_remain_on_channel,
 	.set_rts_threshold		= ath10k_set_rts_threshold,
-	.set_frag_threshold		= ath10k_set_frag_threshold,
 	.flush				= ath10k_flush,
 	.tx_last_beacon			= ath10k_tx_last_beacon,
 	.set_antenna			= ath10k_set_antenna,
@@ -4748,6 +4818,9 @@ static const struct ieee80211_channel ath10k_5ghz_channels[] = {
 	CHAN5G(165, 5825, 0),
 };
 
+/* Note: Be careful if you re-order these. There is code which depends on this
+ * ordering.
+ */
 static struct ieee80211_rate ath10k_rates[] = {
 	/* CCK */
 	RATETAB_ENT(10,  0x82, 0),
@@ -4801,6 +4874,10 @@ static const struct ieee80211_iface_limit ath10k_if_limits[] = {
 	.types	= BIT(NL80211_IFTYPE_P2P_GO)
 	},
 	{
+	.max	= 1,
+	.types	= BIT(NL80211_IFTYPE_P2P_DEVICE)
+	},
+	{
 	.max	= 7,
 	.types	= BIT(NL80211_IFTYPE_AP)
 	},
@@ -5020,6 +5097,7 @@ int ath10k_mac_register(struct ath10k *ar)
 
 	if (!test_bit(ATH10K_FW_FEATURE_NO_P2P, ar->fw_features))
 		ar->hw->wiphy->interface_modes |=
+			BIT(NL80211_IFTYPE_P2P_DEVICE) |
 			BIT(NL80211_IFTYPE_P2P_CLIENT) |
 			BIT(NL80211_IFTYPE_P2P_GO);
 
@@ -5063,16 +5141,26 @@ int ath10k_mac_register(struct ath10k *ar)
 	 */
 	ar->hw->queues = 4;
 
-	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features)) {
-		ar->hw->wiphy->iface_combinations = ath10k_10x_if_comb;
-		ar->hw->wiphy->n_iface_combinations =
-			ARRAY_SIZE(ath10k_10x_if_comb);
-	} else {
+	switch (ar->wmi.op_version) {
+	case ATH10K_FW_WMI_OP_VERSION_MAIN:
+	case ATH10K_FW_WMI_OP_VERSION_TLV:
 		ar->hw->wiphy->iface_combinations = ath10k_if_comb;
 		ar->hw->wiphy->n_iface_combinations =
 			ARRAY_SIZE(ath10k_if_comb);
-
 		ar->hw->wiphy->interface_modes |= BIT(NL80211_IFTYPE_ADHOC);
+		break;
+	case ATH10K_FW_WMI_OP_VERSION_10_1:
+	case ATH10K_FW_WMI_OP_VERSION_10_2:
+	case ATH10K_FW_WMI_OP_VERSION_10_2_4:
+		ar->hw->wiphy->iface_combinations = ath10k_10x_if_comb;
+		ar->hw->wiphy->n_iface_combinations =
+			ARRAY_SIZE(ath10k_10x_if_comb);
+		break;
+	case ATH10K_FW_WMI_OP_VERSION_UNSET:
+	case ATH10K_FW_WMI_OP_VERSION_MAX:
+		WARN_ON(1);
+		ret = -EINVAL;
+		goto err_free;
 	}
 
 	ar->hw->netdev_features = NETIF_F_HW_CSUM;
diff --git a/drivers/net/wireless/ath/ath10k/pci.c b/drivers/net/wireless/ath/ath10k/pci.c
index 7abb8367119a..5e50214246f8 100644
--- a/drivers/net/wireless/ath/ath10k/pci.c
+++ b/drivers/net/wireless/ath/ath10k/pci.c
@@ -64,6 +64,14 @@ static const struct pci_device_id ath10k_pci_id_table[] = {
 	{0}
 };
 
+static const struct ath10k_pci_supp_chip ath10k_pci_supp_chips[] = {
+	/* QCA988X pre 2.0 chips are not supported because they need some nasty
+	 * hacks. ath10k doesn't have them and these devices crash horribly
+	 * because of that.
+	 */
+	{ QCA988X_2_0_DEVICE_ID, QCA988X_HW_2_0_CHIP_ID_REV },
+};
+
 static void ath10k_pci_buffer_cleanup(struct ath10k *ar);
 static int ath10k_pci_cold_reset(struct ath10k *ar);
 static int ath10k_pci_warm_reset(struct ath10k *ar);
@@ -2476,6 +2484,23 @@ static void ath10k_pci_release(struct ath10k *ar)
 	pci_disable_device(pdev);
 }
 
+static bool ath10k_pci_chip_is_supported(u32 dev_id, u32 chip_id)
+{
+	const struct ath10k_pci_supp_chip *supp_chip;
+	int i;
+	u32 rev_id = MS(chip_id, SOC_CHIP_ID_REV);
+
+	for (i = 0; i < ARRAY_SIZE(ath10k_pci_supp_chips); i++) {
+		supp_chip = &ath10k_pci_supp_chips[i];
+
+		if (supp_chip->dev_id == dev_id &&
+		    supp_chip->rev_id == rev_id)
+			return true;
+	}
+
+	return false;
+}
+
 static int ath10k_pci_probe(struct pci_dev *pdev,
 			    const struct pci_device_id *pci_dev)
 {
@@ -2521,6 +2546,12 @@ static int ath10k_pci_probe(struct pci_dev *pdev,
 		goto err_sleep;
 	}
 
+	if (!ath10k_pci_chip_is_supported(pdev->device, chip_id)) {
+		ath10k_err(ar, "device %04x with chip_id %08x isn't supported\n",
+			   pdev->device, chip_id);
+		goto err_sleep;
+	}
+
 	ret = ath10k_pci_alloc_pipes(ar);
 	if (ret) {
 		ath10k_err(ar, "failed to allocate copy engine pipes: %d\n",
diff --git a/drivers/net/wireless/ath/ath10k/pci.h b/drivers/net/wireless/ath/ath10k/pci.h
index cf36511c7f4d..ce4a1ef89961 100644
--- a/drivers/net/wireless/ath/ath10k/pci.h
+++ b/drivers/net/wireless/ath/ath10k/pci.h
@@ -152,6 +152,11 @@ struct ath10k_pci_pipe {
 	struct tasklet_struct intr;
 };
 
+struct ath10k_pci_supp_chip {
+	u32 dev_id;
+	u32 rev_id;
+};
+
 struct ath10k_pci {
 	struct pci_dev *pdev;
 	struct device *dev;
diff --git a/drivers/net/wireless/ath/ath10k/spectral.c b/drivers/net/wireless/ath/ath10k/spectral.c
index 63ce61fcdac8..d22addf6118b 100644
--- a/drivers/net/wireless/ath/ath10k/spectral.c
+++ b/drivers/net/wireless/ath/ath10k/spectral.c
@@ -17,6 +17,7 @@
 #include <linux/relay.h>
 #include "core.h"
 #include "debug.h"
+#include "wmi-ops.h"
 
 static void send_fft_sample(struct ath10k *ar,
 			    const struct fft_sample_tlv *fft_sample_tlv)
diff --git a/drivers/net/wireless/ath/ath10k/testmode.c b/drivers/net/wireless/ath/ath10k/testmode.c
index 483db9cb8c96..b084f88da102 100644
--- a/drivers/net/wireless/ath/ath10k/testmode.c
+++ b/drivers/net/wireless/ath/ath10k/testmode.c
@@ -187,13 +187,14 @@ static int ath10k_tm_cmd_utf_start(struct ath10k *ar, struct nlattr *tb[])
 
 	memcpy(ar->testmode.orig_fw_features, ar->fw_features,
 	       sizeof(ar->fw_features));
+	ar->testmode.orig_wmi_op_version = ar->wmi.op_version;
 
 	/* utf.bin firmware image does not advertise firmware features. Do
 	 * an ugly hack where we force the firmware features so that wmi.c
 	 * will use the correct WMI interface.
 	 */
 	memset(ar->fw_features, 0, sizeof(ar->fw_features));
-	__set_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features);
+	ar->wmi.op_version = ATH10K_FW_WMI_OP_VERSION_10_1;
 
 	ret = ath10k_hif_power_up(ar);
 	if (ret) {
@@ -224,6 +225,7 @@ err_fw_features:
 	/* return the original firmware features */
 	memcpy(ar->fw_features, ar->testmode.orig_fw_features,
 	       sizeof(ar->fw_features));
+	ar->wmi.op_version = ar->testmode.orig_wmi_op_version;
 
 	release_firmware(ar->testmode.utf);
 	ar->testmode.utf = NULL;
@@ -250,6 +252,7 @@ static void __ath10k_tm_cmd_utf_stop(struct ath10k *ar)
 	/* return the original firmware features */
 	memcpy(ar->fw_features, ar->testmode.orig_fw_features,
 	       sizeof(ar->fw_features));
+	ar->wmi.op_version = ar->testmode.orig_wmi_op_version;
 
 	release_firmware(ar->testmode.utf);
 	ar->testmode.utf = NULL;
diff --git a/drivers/net/wireless/ath/ath10k/thermal.c b/drivers/net/wireless/ath/ath10k/thermal.c
new file mode 100644
index 000000000000..b14ae8d135f6
--- /dev/null
+++ b/drivers/net/wireless/ath/ath10k/thermal.c
@@ -0,0 +1,243 @@
+/*
+ * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include <linux/device.h>
+#include <linux/sysfs.h>
+#include <linux/thermal.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include "core.h"
+#include "debug.h"
+#include "wmi-ops.h"
+
+static int ath10k_thermal_get_active_vifs(struct ath10k *ar,
+					  enum wmi_vdev_type type)
+{
+	struct ath10k_vif *arvif;
+	int count = 0;
+
+	lockdep_assert_held(&ar->conf_mutex);
+
+	list_for_each_entry(arvif, &ar->arvifs, list) {
+		if (!arvif->is_started)
+			continue;
+
+		if (!arvif->is_up)
+			continue;
+
+		if (arvif->vdev_type != type)
+			continue;
+
+		count++;
+	}
+	return count;
+}
+
+static int ath10k_thermal_get_max_dutycycle(struct thermal_cooling_device *cdev,
+					    unsigned long *state)
+{
+	*state = ATH10K_QUIET_DUTY_CYCLE_MAX;
+
+	return 0;
+}
+
+static int ath10k_thermal_get_cur_dutycycle(struct thermal_cooling_device *cdev,
+					    unsigned long *state)
+{
+	struct ath10k *ar = cdev->devdata;
+
+	mutex_lock(&ar->conf_mutex);
+	*state = ar->thermal.duty_cycle;
+	mutex_unlock(&ar->conf_mutex);
+
+	return 0;
+}
+
+static int ath10k_thermal_set_cur_dutycycle(struct thermal_cooling_device *cdev,
+					    unsigned long duty_cycle)
+{
+	struct ath10k *ar = cdev->devdata;
+	u32 period, duration, enabled;
+	int num_bss, ret = 0;
+
+	mutex_lock(&ar->conf_mutex);
+	if (ar->state != ATH10K_STATE_ON) {
+		ret = -ENETDOWN;
+		goto out;
+	}
+
+	if (duty_cycle > ATH10K_QUIET_DUTY_CYCLE_MAX) {
+		ath10k_warn(ar, "duty cycle %ld is exceeding the limit %d\n",
+			    duty_cycle, ATH10K_QUIET_DUTY_CYCLE_MAX);
+		ret = -EINVAL;
+		goto out;
+	}
+	/* TODO: Right now, thermal mitigation is handled only for single/multi
+	 * vif AP mode. Since quiet param is not validated in STA mode, it needs
+	 * to be investigated further to handle multi STA and multi-vif (AP+STA)
+	 * mode properly.
+	 */
+	num_bss = ath10k_thermal_get_active_vifs(ar, WMI_VDEV_TYPE_AP);
+	if (!num_bss) {
+		ath10k_warn(ar, "no active AP interfaces\n");
+		ret = -ENETDOWN;
+		goto out;
+	}
+	period = max(ATH10K_QUIET_PERIOD_MIN,
+		     (ATH10K_QUIET_PERIOD_DEFAULT / num_bss));
+	duration = period * (duty_cycle / 100);
+	enabled = duration ? 1 : 0;
+
+	ret = ath10k_wmi_pdev_set_quiet_mode(ar, period, duration,
+					     ATH10K_QUIET_START_OFFSET,
+					     enabled);
+	if (ret) {
+		ath10k_warn(ar, "failed to set quiet mode period %u duarion %u enabled %u ret %d\n",
+			    period, duration, enabled, ret);
+		goto out;
+	}
+	ar->thermal.duty_cycle = duty_cycle;
+out:
+	mutex_unlock(&ar->conf_mutex);
+	return ret;
+}
+
+static struct thermal_cooling_device_ops ath10k_thermal_ops = {
+	.get_max_state = ath10k_thermal_get_max_dutycycle,
+	.get_cur_state = ath10k_thermal_get_cur_dutycycle,
+	.set_cur_state = ath10k_thermal_set_cur_dutycycle,
+};
+
+static ssize_t ath10k_thermal_show_temp(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct ath10k *ar = dev_get_drvdata(dev);
+	int ret, temperature;
+
+	mutex_lock(&ar->conf_mutex);
+
+	/* Can't get temperature when the card is off */
+	if (ar->state != ATH10K_STATE_ON) {
+		ret = -ENETDOWN;
+		goto out;
+	}
+
+	reinit_completion(&ar->thermal.wmi_sync);
+	ret = ath10k_wmi_pdev_get_temperature(ar);
+	if (ret) {
+		ath10k_warn(ar, "failed to read temperature %d\n", ret);
+		goto out;
+	}
+
+	if (test_bit(ATH10K_FLAG_CRASH_FLUSH, &ar->dev_flags)) {
+		ret = -ESHUTDOWN;
+		goto out;
+	}
+
+	ret = wait_for_completion_timeout(&ar->thermal.wmi_sync,
+					  ATH10K_THERMAL_SYNC_TIMEOUT_HZ);
+	if (ret == 0) {
+		ath10k_warn(ar, "failed to synchronize thermal read\n");
+		ret = -ETIMEDOUT;
+		goto out;
+	}
+
+	spin_lock_bh(&ar->data_lock);
+	temperature = ar->thermal.temperature;
+	spin_unlock_bh(&ar->data_lock);
+
+	ret = snprintf(buf, PAGE_SIZE, "%d", temperature);
+out:
+	mutex_unlock(&ar->conf_mutex);
+	return ret;
+}
+
+void ath10k_thermal_event_temperature(struct ath10k *ar, int temperature)
+{
+	spin_lock_bh(&ar->data_lock);
+	ar->thermal.temperature = temperature;
+	spin_unlock_bh(&ar->data_lock);
+	complete(&ar->thermal.wmi_sync);
+}
+
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, ath10k_thermal_show_temp,
+			  NULL, 0);
+
+static struct attribute *ath10k_hwmon_attrs[] = {
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	NULL,
+};
+ATTRIBUTE_GROUPS(ath10k_hwmon);
+
+int ath10k_thermal_register(struct ath10k *ar)
+{
+	struct thermal_cooling_device *cdev;
+	struct device *hwmon_dev;
+	int ret;
+
+	cdev = thermal_cooling_device_register("ath10k_thermal", ar,
+					       &ath10k_thermal_ops);
+
+	if (IS_ERR(cdev)) {
+		ath10k_err(ar, "failed to setup thermal device result: %ld\n",
+			   PTR_ERR(cdev));
+		return -EINVAL;
+	}
+
+	ret = sysfs_create_link(&ar->dev->kobj, &cdev->device.kobj,
+				"cooling_device");
+	if (ret) {
+		ath10k_err(ar, "failed to create thermal symlink\n");
+		goto err_cooling_destroy;
+	}
+
+	ar->thermal.cdev = cdev;
+
+	/* Do not register hwmon device when temperature reading is not
+	 * supported by firmware
+	 */
+	if (ar->wmi.op_version != ATH10K_FW_WMI_OP_VERSION_10_2_4)
+		return 0;
+
+	/* Avoid linking error on devm_hwmon_device_register_with_groups, I
+	 * guess linux/hwmon.h is missing proper stubs. */
+	if (!config_enabled(HWMON))
+		return 0;
+
+	hwmon_dev = devm_hwmon_device_register_with_groups(ar->dev,
+							   "ath10k_hwmon", ar,
+							   ath10k_hwmon_groups);
+	if (IS_ERR(hwmon_dev)) {
+		ath10k_err(ar, "failed to register hwmon device: %ld\n",
+			   PTR_ERR(hwmon_dev));
+		ret = -EINVAL;
+		goto err_remove_link;
+	}
+	return 0;
+
+err_remove_link:
+	sysfs_remove_link(&ar->dev->kobj, "thermal_sensor");
+err_cooling_destroy:
+	thermal_cooling_device_unregister(cdev);
+	return ret;
+}
+
+void ath10k_thermal_unregister(struct ath10k *ar)
+{
+	thermal_cooling_device_unregister(ar->thermal.cdev);
+	sysfs_remove_link(&ar->dev->kobj, "cooling_device");
+}
diff --git a/drivers/net/wireless/ath/ath10k/thermal.h b/drivers/net/wireless/ath/ath10k/thermal.h
new file mode 100644
index 000000000000..bccc17ae0fde
--- /dev/null
+++ b/drivers/net/wireless/ath/ath10k/thermal.h
@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#ifndef _THERMAL_
+#define _THERMAL_
+
+#define ATH10K_QUIET_PERIOD_DEFAULT     100
+#define ATH10K_QUIET_PERIOD_MIN         25
+#define ATH10K_QUIET_START_OFFSET       10
+#define ATH10K_QUIET_DUTY_CYCLE_MAX     70
+#define ATH10K_HWMON_NAME_LEN           15
+#define ATH10K_THERMAL_SYNC_TIMEOUT_HZ (5*HZ)
+
+struct ath10k_thermal {
+	struct thermal_cooling_device *cdev;
+	struct completion wmi_sync;
+
+	/* protected by conf_mutex */
+	u32 duty_cycle;
+	/* temperature value in Celcius degree
+	 * protected by data_lock
+	 */
+	int temperature;
+};
+
+#ifdef CONFIG_THERMAL
+int ath10k_thermal_register(struct ath10k *ar);
+void ath10k_thermal_unregister(struct ath10k *ar);
+void ath10k_thermal_event_temperature(struct ath10k *ar, int temperature);
+#else
+static inline int ath10k_thermal_register(struct ath10k *ar)
+{
+	return 0;
+}
+
+static inline void ath10k_thermal_unregister(struct ath10k *ar)
+{
+}
+
+static inline void ath10k_thermal_event_temperature(struct ath10k *ar,
+						    int temperature)
+{
+}
+
+#endif
+#endif /* _THERMAL_ */
diff --git a/drivers/net/wireless/ath/ath10k/wmi-ops.h b/drivers/net/wireless/ath/ath10k/wmi-ops.h
new file mode 100644
index 000000000000..20e2c3002bb5
--- /dev/null
+++ b/drivers/net/wireless/ath/ath10k/wmi-ops.h
@@ -0,0 +1,860 @@
+/*
+ * Copyright (c) 2005-2011 Atheros Communications Inc.
+ * Copyright (c) 2011-2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _WMI_OPS_H_
+#define _WMI_OPS_H_
+
+struct ath10k;
+struct sk_buff;
+
+struct wmi_ops {
+	void (*rx)(struct ath10k *ar, struct sk_buff *skb);
+	void (*map_svc)(const __le32 *in, unsigned long *out, size_t len);
+
+	int (*pull_scan)(struct ath10k *ar, struct sk_buff *skb,
+			 struct wmi_scan_ev_arg *arg);
+	int (*pull_mgmt_rx)(struct ath10k *ar, struct sk_buff *skb,
+			    struct wmi_mgmt_rx_ev_arg *arg);
+	int (*pull_ch_info)(struct ath10k *ar, struct sk_buff *skb,
+			    struct wmi_ch_info_ev_arg *arg);
+	int (*pull_vdev_start)(struct ath10k *ar, struct sk_buff *skb,
+			       struct wmi_vdev_start_ev_arg *arg);
+	int (*pull_peer_kick)(struct ath10k *ar, struct sk_buff *skb,
+			      struct wmi_peer_kick_ev_arg *arg);
+	int (*pull_swba)(struct ath10k *ar, struct sk_buff *skb,
+			 struct wmi_swba_ev_arg *arg);
+	int (*pull_phyerr)(struct ath10k *ar, struct sk_buff *skb,
+			   struct wmi_phyerr_ev_arg *arg);
+	int (*pull_svc_rdy)(struct ath10k *ar, struct sk_buff *skb,
+			    struct wmi_svc_rdy_ev_arg *arg);
+	int (*pull_rdy)(struct ath10k *ar, struct sk_buff *skb,
+			struct wmi_rdy_ev_arg *arg);
+	int (*pull_fw_stats)(struct ath10k *ar, struct sk_buff *skb,
+			     struct ath10k_fw_stats *stats);
+
+	struct sk_buff *(*gen_pdev_suspend)(struct ath10k *ar, u32 suspend_opt);
+	struct sk_buff *(*gen_pdev_resume)(struct ath10k *ar);
+	struct sk_buff *(*gen_pdev_set_rd)(struct ath10k *ar, u16 rd, u16 rd2g,
+					   u16 rd5g, u16 ctl2g, u16 ctl5g,
+					   enum wmi_dfs_region dfs_reg);
+	struct sk_buff *(*gen_pdev_set_param)(struct ath10k *ar, u32 id,
+					      u32 value);
+	struct sk_buff *(*gen_init)(struct ath10k *ar);
+	struct sk_buff *(*gen_start_scan)(struct ath10k *ar,
+					  const struct wmi_start_scan_arg *arg);
+	struct sk_buff *(*gen_stop_scan)(struct ath10k *ar,
+					 const struct wmi_stop_scan_arg *arg);
+	struct sk_buff *(*gen_vdev_create)(struct ath10k *ar, u32 vdev_id,
+					   enum wmi_vdev_type type,
+					   enum wmi_vdev_subtype subtype,
+					   const u8 macaddr[ETH_ALEN]);
+	struct sk_buff *(*gen_vdev_delete)(struct ath10k *ar, u32 vdev_id);
+	struct sk_buff *(*gen_vdev_start)(struct ath10k *ar,
+					  const struct wmi_vdev_start_request_arg *arg,
+					  bool restart);
+	struct sk_buff *(*gen_vdev_stop)(struct ath10k *ar, u32 vdev_id);
+	struct sk_buff *(*gen_vdev_up)(struct ath10k *ar, u32 vdev_id, u32 aid,
+				       const u8 *bssid);
+	struct sk_buff *(*gen_vdev_down)(struct ath10k *ar, u32 vdev_id);
+	struct sk_buff *(*gen_vdev_set_param)(struct ath10k *ar, u32 vdev_id,
+					      u32 param_id, u32 param_value);
+	struct sk_buff *(*gen_vdev_install_key)(struct ath10k *ar,
+						const struct wmi_vdev_install_key_arg *arg);
+	struct sk_buff *(*gen_vdev_spectral_conf)(struct ath10k *ar,
+						  const struct wmi_vdev_spectral_conf_arg *arg);
+	struct sk_buff *(*gen_vdev_spectral_enable)(struct ath10k *ar, u32 vdev_id,
+						    u32 trigger, u32 enable);
+	struct sk_buff *(*gen_peer_create)(struct ath10k *ar, u32 vdev_id,
+					   const u8 peer_addr[ETH_ALEN]);
+	struct sk_buff *(*gen_peer_delete)(struct ath10k *ar, u32 vdev_id,
+					   const u8 peer_addr[ETH_ALEN]);
+	struct sk_buff *(*gen_peer_flush)(struct ath10k *ar, u32 vdev_id,
+					  const u8 peer_addr[ETH_ALEN],
+					  u32 tid_bitmap);
+	struct sk_buff *(*gen_peer_set_param)(struct ath10k *ar, u32 vdev_id,
+					      const u8 *peer_addr,
+					      enum wmi_peer_param param_id,
+					      u32 param_value);
+	struct sk_buff *(*gen_peer_assoc)(struct ath10k *ar,
+					  const struct wmi_peer_assoc_complete_arg *arg);
+	struct sk_buff *(*gen_set_psmode)(struct ath10k *ar, u32 vdev_id,
+					  enum wmi_sta_ps_mode psmode);
+	struct sk_buff *(*gen_set_sta_ps)(struct ath10k *ar, u32 vdev_id,
+					  enum wmi_sta_powersave_param param_id,
+					  u32 value);
+	struct sk_buff *(*gen_set_ap_ps)(struct ath10k *ar, u32 vdev_id,
+					 const u8 *mac,
+					 enum wmi_ap_ps_peer_param param_id,
+					 u32 value);
+	struct sk_buff *(*gen_scan_chan_list)(struct ath10k *ar,
+					      const struct wmi_scan_chan_list_arg *arg);
+	struct sk_buff *(*gen_beacon_dma)(struct ath10k_vif *arvif);
+	struct sk_buff *(*gen_pdev_set_wmm)(struct ath10k *ar,
+					    const struct wmi_pdev_set_wmm_params_arg *arg);
+	struct sk_buff *(*gen_request_stats)(struct ath10k *ar,
+					     enum wmi_stats_id stats_id);
+	struct sk_buff *(*gen_force_fw_hang)(struct ath10k *ar,
+					     enum wmi_force_fw_hang_type type,
+					     u32 delay_ms);
+	struct sk_buff *(*gen_mgmt_tx)(struct ath10k *ar, struct sk_buff *skb);
+	struct sk_buff *(*gen_dbglog_cfg)(struct ath10k *ar, u32 module_enable);
+	struct sk_buff *(*gen_pktlog_enable)(struct ath10k *ar, u32 filter);
+	struct sk_buff *(*gen_pktlog_disable)(struct ath10k *ar);
+	struct sk_buff *(*gen_pdev_set_quiet_mode)(struct ath10k *ar,
+						   u32 period, u32 duration,
+						   u32 next_offset,
+						   u32 enabled);
+	struct sk_buff *(*gen_pdev_get_temperature)(struct ath10k *ar);
+};
+
+int ath10k_wmi_cmd_send(struct ath10k *ar, struct sk_buff *skb, u32 cmd_id);
+
+static inline int
+ath10k_wmi_rx(struct ath10k *ar, struct sk_buff *skb)
+{
+	if (WARN_ON_ONCE(!ar->wmi.ops->rx))
+		return -EOPNOTSUPP;
+
+	ar->wmi.ops->rx(ar, skb);
+	return 0;
+}
+
+static inline int
+ath10k_wmi_map_svc(struct ath10k *ar, const __le32 *in, unsigned long *out,
+		   size_t len)
+{
+	if (!ar->wmi.ops->map_svc)
+		return -EOPNOTSUPP;
+
+	ar->wmi.ops->map_svc(in, out, len);
+	return 0;
+}
+
+static inline int
+ath10k_wmi_pull_scan(struct ath10k *ar, struct sk_buff *skb,
+		     struct wmi_scan_ev_arg *arg)
+{
+	if (!ar->wmi.ops->pull_scan)
+		return -EOPNOTSUPP;
+
+	return ar->wmi.ops->pull_scan(ar, skb, arg);
+}
+
+static inline int
+ath10k_wmi_pull_mgmt_rx(struct ath10k *ar, struct sk_buff *skb,
+			struct wmi_mgmt_rx_ev_arg *arg)
+{
+	if (!ar->wmi.ops->pull_mgmt_rx)
+		return -EOPNOTSUPP;
+
+	return ar->wmi.ops->pull_mgmt_rx(ar, skb, arg);
+}
+
+static inline int
+ath10k_wmi_pull_ch_info(struct ath10k *ar, struct sk_buff *skb,
+			struct wmi_ch_info_ev_arg *arg)
+{
+	if (!ar->wmi.ops->pull_ch_info)
+		return -EOPNOTSUPP;
+
+	return ar->wmi.ops->pull_ch_info(ar, skb, arg);
+}
+
+static inline int
+ath10k_wmi_pull_vdev_start(struct ath10k *ar, struct sk_buff *skb,
+			   struct wmi_vdev_start_ev_arg *arg)
+{
+	if (!ar->wmi.ops->pull_vdev_start)
+		return -EOPNOTSUPP;
+
+	return ar->wmi.ops->pull_vdev_start(ar, skb, arg);
+}
+
+static inline int
+ath10k_wmi_pull_peer_kick(struct ath10k *ar, struct sk_buff *skb,
+			  struct wmi_peer_kick_ev_arg *arg)
+{
+	if (!ar->wmi.ops->pull_peer_kick)
+		return -EOPNOTSUPP;
+
+	return ar->wmi.ops->pull_peer_kick(ar, skb, arg);
+}
+
+static inline int
+ath10k_wmi_pull_swba(struct ath10k *ar, struct sk_buff *skb,
+		     struct wmi_swba_ev_arg *arg)
+{
+	if (!ar->wmi.ops->pull_swba)
+		return -EOPNOTSUPP;
+
+	return ar->wmi.ops->pull_swba(ar, skb, arg);
+}
+
+static inline int
+ath10k_wmi_pull_phyerr(struct ath10k *ar, struct sk_buff *skb,
+		       struct wmi_phyerr_ev_arg *arg)
+{
+	if (!ar->wmi.ops->pull_phyerr)
+		return -EOPNOTSUPP;
+
+	return ar->wmi.ops->pull_phyerr(ar, skb, arg);
+}
+
+static inline int
+ath10k_wmi_pull_svc_rdy(struct ath10k *ar, struct sk_buff *skb,
+			struct wmi_svc_rdy_ev_arg *arg)
+{
+	if (!ar->wmi.ops->pull_svc_rdy)
+		return -EOPNOTSUPP;
+
+	return ar->wmi.ops->pull_svc_rdy(ar, skb, arg);
+}
+
+static inline int
+ath10k_wmi_pull_rdy(struct ath10k *ar, struct sk_buff *skb,
+		    struct wmi_rdy_ev_arg *arg)
+{
+	if (!ar->wmi.ops->pull_rdy)
+		return -EOPNOTSUPP;
+
+	return ar->wmi.ops->pull_rdy(ar, skb, arg);
+}
+
+static inline int
+ath10k_wmi_pull_fw_stats(struct ath10k *ar, struct sk_buff *skb,
+			 struct ath10k_fw_stats *stats)
+{
+	if (!ar->wmi.ops->pull_fw_stats)
+		return -EOPNOTSUPP;
+
+	return ar->wmi.ops->pull_fw_stats(ar, skb, stats);
+}
+
+static inline int
+ath10k_wmi_mgmt_tx(struct ath10k *ar, struct sk_buff *msdu)
+{
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(msdu);
+	struct sk_buff *skb;
+	int ret;
+
+	if (!ar->wmi.ops->gen_mgmt_tx)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_mgmt_tx(ar, msdu);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	ret = ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->mgmt_tx_cmdid);
+	if (ret)
+		return ret;
+
+	/* FIXME There's no ACK event for Management Tx. This probably
+	 * shouldn't be called here either. */
+	info->flags |= IEEE80211_TX_STAT_ACK;
+	ieee80211_tx_status_irqsafe(ar->hw, msdu);
+
+	return 0;
+}
+
+static inline int
+ath10k_wmi_pdev_set_regdomain(struct ath10k *ar, u16 rd, u16 rd2g, u16 rd5g,
+			      u16 ctl2g, u16 ctl5g,
+			      enum wmi_dfs_region dfs_reg)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_pdev_set_rd)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_pdev_set_rd(ar, rd, rd2g, rd5g, ctl2g, ctl5g,
+					   dfs_reg);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb,
+				   ar->wmi.cmd->pdev_set_regdomain_cmdid);
+}
+
+static inline int
+ath10k_wmi_pdev_suspend_target(struct ath10k *ar, u32 suspend_opt)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_pdev_suspend)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_pdev_suspend(ar, suspend_opt);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->pdev_suspend_cmdid);
+}
+
+static inline int
+ath10k_wmi_pdev_resume_target(struct ath10k *ar)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_pdev_resume)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_pdev_resume(ar);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->pdev_resume_cmdid);
+}
+
+static inline int
+ath10k_wmi_pdev_set_param(struct ath10k *ar, u32 id, u32 value)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_pdev_set_param)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_pdev_set_param(ar, id, value);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->pdev_set_param_cmdid);
+}
+
+static inline int
+ath10k_wmi_cmd_init(struct ath10k *ar)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_init)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_init(ar);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->init_cmdid);
+}
+
+static inline int
+ath10k_wmi_start_scan(struct ath10k *ar,
+		      const struct wmi_start_scan_arg *arg)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_start_scan)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_start_scan(ar, arg);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->start_scan_cmdid);
+}
+
+static inline int
+ath10k_wmi_stop_scan(struct ath10k *ar, const struct wmi_stop_scan_arg *arg)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_stop_scan)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_stop_scan(ar, arg);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->stop_scan_cmdid);
+}
+
+static inline int
+ath10k_wmi_vdev_create(struct ath10k *ar, u32 vdev_id,
+		       enum wmi_vdev_type type,
+		       enum wmi_vdev_subtype subtype,
+		       const u8 macaddr[ETH_ALEN])
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_vdev_create)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_vdev_create(ar, vdev_id, type, subtype, macaddr);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_create_cmdid);
+}
+
+static inline int
+ath10k_wmi_vdev_delete(struct ath10k *ar, u32 vdev_id)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_vdev_delete)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_vdev_delete(ar, vdev_id);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_delete_cmdid);
+}
+
+static inline int
+ath10k_wmi_vdev_start(struct ath10k *ar,
+		      const struct wmi_vdev_start_request_arg *arg)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_vdev_start)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_vdev_start(ar, arg, false);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb,
+				   ar->wmi.cmd->vdev_start_request_cmdid);
+}
+
+static inline int
+ath10k_wmi_vdev_restart(struct ath10k *ar,
+			const struct wmi_vdev_start_request_arg *arg)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_vdev_start)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_vdev_start(ar, arg, true);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb,
+				   ar->wmi.cmd->vdev_restart_request_cmdid);
+}
+
+static inline int
+ath10k_wmi_vdev_stop(struct ath10k *ar, u32 vdev_id)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_vdev_stop)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_vdev_stop(ar, vdev_id);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_stop_cmdid);
+}
+
+static inline int
+ath10k_wmi_vdev_up(struct ath10k *ar, u32 vdev_id, u32 aid, const u8 *bssid)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_vdev_up)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_vdev_up(ar, vdev_id, aid, bssid);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_up_cmdid);
+}
+
+static inline int
+ath10k_wmi_vdev_down(struct ath10k *ar, u32 vdev_id)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_vdev_down)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_vdev_down(ar, vdev_id);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_down_cmdid);
+}
+
+static inline int
+ath10k_wmi_vdev_set_param(struct ath10k *ar, u32 vdev_id, u32 param_id,
+			  u32 param_value)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_vdev_set_param)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_vdev_set_param(ar, vdev_id, param_id,
+					      param_value);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_set_param_cmdid);
+}
+
+static inline int
+ath10k_wmi_vdev_install_key(struct ath10k *ar,
+			    const struct wmi_vdev_install_key_arg *arg)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_vdev_install_key)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_vdev_install_key(ar, arg);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb,
+				   ar->wmi.cmd->vdev_install_key_cmdid);
+}
+
+static inline int
+ath10k_wmi_vdev_spectral_conf(struct ath10k *ar,
+			      const struct wmi_vdev_spectral_conf_arg *arg)
+{
+	struct sk_buff *skb;
+	u32 cmd_id;
+
+	skb = ar->wmi.ops->gen_vdev_spectral_conf(ar, arg);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	cmd_id = ar->wmi.cmd->vdev_spectral_scan_configure_cmdid;
+	return ath10k_wmi_cmd_send(ar, skb, cmd_id);
+}
+
+static inline int
+ath10k_wmi_vdev_spectral_enable(struct ath10k *ar, u32 vdev_id, u32 trigger,
+				u32 enable)
+{
+	struct sk_buff *skb;
+	u32 cmd_id;
+
+	skb = ar->wmi.ops->gen_vdev_spectral_enable(ar, vdev_id, trigger,
+						    enable);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	cmd_id = ar->wmi.cmd->vdev_spectral_scan_enable_cmdid;
+	return ath10k_wmi_cmd_send(ar, skb, cmd_id);
+}
+
+static inline int
+ath10k_wmi_peer_create(struct ath10k *ar, u32 vdev_id,
+		       const u8 peer_addr[ETH_ALEN])
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_peer_create)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_peer_create(ar, vdev_id, peer_addr);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->peer_create_cmdid);
+}
+
+static inline int
+ath10k_wmi_peer_delete(struct ath10k *ar, u32 vdev_id,
+		       const u8 peer_addr[ETH_ALEN])
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_peer_delete)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_peer_delete(ar, vdev_id, peer_addr);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->peer_delete_cmdid);
+}
+
+static inline int
+ath10k_wmi_peer_flush(struct ath10k *ar, u32 vdev_id,
+		      const u8 peer_addr[ETH_ALEN], u32 tid_bitmap)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_peer_flush)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_peer_flush(ar, vdev_id, peer_addr, tid_bitmap);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->peer_flush_tids_cmdid);
+}
+
+static inline int
+ath10k_wmi_peer_set_param(struct ath10k *ar, u32 vdev_id, const u8 *peer_addr,
+			  enum wmi_peer_param param_id, u32 param_value)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_peer_set_param)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_peer_set_param(ar, vdev_id, peer_addr, param_id,
+					      param_value);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->peer_set_param_cmdid);
+}
+
+static inline int
+ath10k_wmi_set_psmode(struct ath10k *ar, u32 vdev_id,
+		      enum wmi_sta_ps_mode psmode)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_set_psmode)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_set_psmode(ar, vdev_id, psmode);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb,
+				   ar->wmi.cmd->sta_powersave_mode_cmdid);
+}
+
+static inline int
+ath10k_wmi_set_sta_ps_param(struct ath10k *ar, u32 vdev_id,
+			    enum wmi_sta_powersave_param param_id, u32 value)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_set_sta_ps)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_set_sta_ps(ar, vdev_id, param_id, value);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb,
+				   ar->wmi.cmd->sta_powersave_param_cmdid);
+}
+
+static inline int
+ath10k_wmi_set_ap_ps_param(struct ath10k *ar, u32 vdev_id, const u8 *mac,
+			   enum wmi_ap_ps_peer_param param_id, u32 value)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_set_ap_ps)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_set_ap_ps(ar, vdev_id, mac, param_id, value);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb,
+				   ar->wmi.cmd->ap_ps_peer_param_cmdid);
+}
+
+static inline int
+ath10k_wmi_scan_chan_list(struct ath10k *ar,
+			  const struct wmi_scan_chan_list_arg *arg)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_scan_chan_list)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_scan_chan_list(ar, arg);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->scan_chan_list_cmdid);
+}
+
+static inline int
+ath10k_wmi_peer_assoc(struct ath10k *ar,
+		      const struct wmi_peer_assoc_complete_arg *arg)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_peer_assoc)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_peer_assoc(ar, arg);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->peer_assoc_cmdid);
+}
+
+static inline int
+ath10k_wmi_beacon_send_ref_nowait(struct ath10k_vif *arvif)
+{
+	struct ath10k *ar = arvif->ar;
+	struct sk_buff *skb;
+	int ret;
+
+	if (!ar->wmi.ops->gen_beacon_dma)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_beacon_dma(arvif);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	ret = ath10k_wmi_cmd_send_nowait(ar, skb,
+					 ar->wmi.cmd->pdev_send_bcn_cmdid);
+	if (ret) {
+		dev_kfree_skb(skb);
+		return ret;
+	}
+
+	return 0;
+}
+
+static inline int
+ath10k_wmi_pdev_set_wmm_params(struct ath10k *ar,
+			       const struct wmi_pdev_set_wmm_params_arg *arg)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_pdev_set_wmm)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_pdev_set_wmm(ar, arg);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb,
+				   ar->wmi.cmd->pdev_set_wmm_params_cmdid);
+}
+
+static inline int
+ath10k_wmi_request_stats(struct ath10k *ar, enum wmi_stats_id stats_id)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_request_stats)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_request_stats(ar, stats_id);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->request_stats_cmdid);
+}
+
+static inline int
+ath10k_wmi_force_fw_hang(struct ath10k *ar,
+			 enum wmi_force_fw_hang_type type, u32 delay_ms)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_force_fw_hang)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_force_fw_hang(ar, type, delay_ms);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->force_fw_hang_cmdid);
+}
+
+static inline int
+ath10k_wmi_dbglog_cfg(struct ath10k *ar, u32 module_enable)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_dbglog_cfg)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_dbglog_cfg(ar, module_enable);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->dbglog_cfg_cmdid);
+}
+
+static inline int
+ath10k_wmi_pdev_pktlog_enable(struct ath10k *ar, u32 filter)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_pktlog_enable)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_pktlog_enable(ar, filter);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->pdev_pktlog_enable_cmdid);
+}
+
+static inline int
+ath10k_wmi_pdev_pktlog_disable(struct ath10k *ar)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_pktlog_disable)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_pktlog_disable(ar);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb,
+				   ar->wmi.cmd->pdev_pktlog_disable_cmdid);
+}
+
+static inline int
+ath10k_wmi_pdev_set_quiet_mode(struct ath10k *ar, u32 period, u32 duration,
+			       u32 next_offset, u32 enabled)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_pdev_set_quiet_mode)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_pdev_set_quiet_mode(ar, period, duration,
+						   next_offset, enabled);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb,
+				   ar->wmi.cmd->pdev_set_quiet_mode_cmdid);
+}
+
+static inline int
+ath10k_wmi_pdev_get_temperature(struct ath10k *ar)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_pdev_get_temperature)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_pdev_get_temperature(ar);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb,
+				   ar->wmi.cmd->pdev_get_temperature_cmdid);
+}
+
+#endif
diff --git a/drivers/net/wireless/ath/ath10k/wmi-tlv.c b/drivers/net/wireless/ath/ath10k/wmi-tlv.c
new file mode 100644
index 000000000000..4c050cec3966
--- /dev/null
+++ b/drivers/net/wireless/ath/ath10k/wmi-tlv.c
@@ -0,0 +1,2222 @@
+/*
+ * Copyright (c) 2005-2011 Atheros Communications Inc.
+ * Copyright (c) 2011-2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "core.h"
+#include "debug.h"
+#include "hw.h"
+#include "wmi.h"
+#include "wmi-ops.h"
+#include "wmi-tlv.h"
+
+/***************/
+/* TLV helpers */
+/**************/
+
+struct wmi_tlv_policy {
+	size_t min_len;
+};
+
+static const struct wmi_tlv_policy wmi_tlv_policies[] = {
+	[WMI_TLV_TAG_ARRAY_BYTE]
+		= { .min_len = sizeof(u8) },
+	[WMI_TLV_TAG_ARRAY_UINT32]
+		= { .min_len = sizeof(u32) },
+	[WMI_TLV_TAG_STRUCT_SCAN_EVENT]
+		= { .min_len = sizeof(struct wmi_scan_event) },
+	[WMI_TLV_TAG_STRUCT_MGMT_RX_HDR]
+		= { .min_len = sizeof(struct wmi_tlv_mgmt_rx_ev) },
+	[WMI_TLV_TAG_STRUCT_CHAN_INFO_EVENT]
+		= { .min_len = sizeof(struct wmi_chan_info_event) },
+	[WMI_TLV_TAG_STRUCT_VDEV_START_RESPONSE_EVENT]
+		= { .min_len = sizeof(struct wmi_vdev_start_response_event) },
+	[WMI_TLV_TAG_STRUCT_PEER_STA_KICKOUT_EVENT]
+		= { .min_len = sizeof(struct wmi_peer_sta_kickout_event) },
+	[WMI_TLV_TAG_STRUCT_HOST_SWBA_EVENT]
+		= { .min_len = sizeof(struct wmi_host_swba_event) },
+	[WMI_TLV_TAG_STRUCT_TIM_INFO]
+		= { .min_len = sizeof(struct wmi_tim_info) },
+	[WMI_TLV_TAG_STRUCT_P2P_NOA_INFO]
+		= { .min_len = sizeof(struct wmi_p2p_noa_info) },
+	[WMI_TLV_TAG_STRUCT_SERVICE_READY_EVENT]
+		= { .min_len = sizeof(struct wmi_tlv_svc_rdy_ev) },
+	[WMI_TLV_TAG_STRUCT_HAL_REG_CAPABILITIES]
+		= { .min_len = sizeof(struct hal_reg_capabilities) },
+	[WMI_TLV_TAG_STRUCT_WLAN_HOST_MEM_REQ]
+		= { .min_len = sizeof(struct wlan_host_mem_req) },
+	[WMI_TLV_TAG_STRUCT_READY_EVENT]
+		= { .min_len = sizeof(struct wmi_tlv_rdy_ev) },
+};
+
+static int
+ath10k_wmi_tlv_iter(struct ath10k *ar, const void *ptr, size_t len,
+		    int (*iter)(struct ath10k *ar, u16 tag, u16 len,
+				const void *ptr, void *data),
+		    void *data)
+{
+	const void *begin = ptr;
+	const struct wmi_tlv *tlv;
+	u16 tlv_tag, tlv_len;
+	int ret;
+
+	while (len > 0) {
+		if (len < sizeof(*tlv)) {
+			ath10k_dbg(ar, ATH10K_DBG_WMI,
+				   "wmi tlv parse failure at byte %zd (%zu bytes left, %zu expected)\n",
+				   ptr - begin, len, sizeof(*tlv));
+			return -EINVAL;
+		}
+
+		tlv = ptr;
+		tlv_tag = __le16_to_cpu(tlv->tag);
+		tlv_len = __le16_to_cpu(tlv->len);
+		ptr += sizeof(*tlv);
+		len -= sizeof(*tlv);
+
+		if (tlv_len > len) {
+			ath10k_dbg(ar, ATH10K_DBG_WMI,
+				   "wmi tlv parse failure of tag %hhu at byte %zd (%zu bytes left, %hhu expected)\n",
+				   tlv_tag, ptr - begin, len, tlv_len);
+			return -EINVAL;
+		}
+
+		if (tlv_tag < ARRAY_SIZE(wmi_tlv_policies) &&
+		    wmi_tlv_policies[tlv_tag].min_len &&
+		    wmi_tlv_policies[tlv_tag].min_len > tlv_len) {
+			ath10k_dbg(ar, ATH10K_DBG_WMI,
+				   "wmi tlv parse failure of tag %hhu at byte %zd (%hhu bytes is less than min length %zu)\n",
+				   tlv_tag, ptr - begin, tlv_len,
+				   wmi_tlv_policies[tlv_tag].min_len);
+			return -EINVAL;
+		}
+
+		ret = iter(ar, tlv_tag, tlv_len, ptr, data);
+		if (ret)
+			return ret;
+
+		ptr += tlv_len;
+		len -= tlv_len;
+	}
+
+	return 0;
+}
+
+static int ath10k_wmi_tlv_iter_parse(struct ath10k *ar, u16 tag, u16 len,
+				     const void *ptr, void *data)
+{
+	const void **tb = data;
+
+	if (tag < WMI_TLV_TAG_MAX)
+		tb[tag] = ptr;
+
+	return 0;
+}
+
+static int ath10k_wmi_tlv_parse(struct ath10k *ar, const void **tb,
+				const void *ptr, size_t len)
+{
+	return ath10k_wmi_tlv_iter(ar, ptr, len, ath10k_wmi_tlv_iter_parse,
+				   (void *)tb);
+}
+
+static const void **
+ath10k_wmi_tlv_parse_alloc(struct ath10k *ar, const void *ptr,
+			   size_t len, gfp_t gfp)
+{
+	const void **tb;
+	int ret;
+
+	tb = kzalloc(sizeof(*tb) * WMI_TLV_TAG_MAX, gfp);
+	if (!tb)
+		return ERR_PTR(-ENOMEM);
+
+	ret = ath10k_wmi_tlv_parse(ar, tb, ptr, len);
+	if (ret) {
+		kfree(tb);
+		return ERR_PTR(ret);
+	}
+
+	return tb;
+}
+
+static u16 ath10k_wmi_tlv_len(const void *ptr)
+{
+	return __le16_to_cpu((((const struct wmi_tlv *)ptr) - 1)->len);
+}
+
+/***********/
+/* TLV ops */
+/***********/
+
+static void ath10k_wmi_tlv_op_rx(struct ath10k *ar, struct sk_buff *skb)
+{
+	struct wmi_cmd_hdr *cmd_hdr;
+	enum wmi_tlv_event_id id;
+
+	cmd_hdr = (struct wmi_cmd_hdr *)skb->data;
+	id = MS(__le32_to_cpu(cmd_hdr->cmd_id), WMI_CMD_HDR_CMD_ID);
+
+	if (skb_pull(skb, sizeof(struct wmi_cmd_hdr)) == NULL)
+		return;
+
+	trace_ath10k_wmi_event(ar, id, skb->data, skb->len);
+
+	switch (id) {
+	case WMI_TLV_MGMT_RX_EVENTID:
+		ath10k_wmi_event_mgmt_rx(ar, skb);
+		/* mgmt_rx() owns the skb now! */
+		return;
+	case WMI_TLV_SCAN_EVENTID:
+		ath10k_wmi_event_scan(ar, skb);
+		break;
+	case WMI_TLV_CHAN_INFO_EVENTID:
+		ath10k_wmi_event_chan_info(ar, skb);
+		break;
+	case WMI_TLV_ECHO_EVENTID:
+		ath10k_wmi_event_echo(ar, skb);
+		break;
+	case WMI_TLV_DEBUG_MESG_EVENTID:
+		ath10k_wmi_event_debug_mesg(ar, skb);
+		break;
+	case WMI_TLV_UPDATE_STATS_EVENTID:
+		ath10k_wmi_event_update_stats(ar, skb);
+		break;
+	case WMI_TLV_VDEV_START_RESP_EVENTID:
+		ath10k_wmi_event_vdev_start_resp(ar, skb);
+		break;
+	case WMI_TLV_VDEV_STOPPED_EVENTID:
+		ath10k_wmi_event_vdev_stopped(ar, skb);
+		break;
+	case WMI_TLV_PEER_STA_KICKOUT_EVENTID:
+		ath10k_wmi_event_peer_sta_kickout(ar, skb);
+		break;
+	case WMI_TLV_HOST_SWBA_EVENTID:
+		ath10k_wmi_event_host_swba(ar, skb);
+		break;
+	case WMI_TLV_TBTTOFFSET_UPDATE_EVENTID:
+		ath10k_wmi_event_tbttoffset_update(ar, skb);
+		break;
+	case WMI_TLV_PHYERR_EVENTID:
+		ath10k_wmi_event_phyerr(ar, skb);
+		break;
+	case WMI_TLV_ROAM_EVENTID:
+		ath10k_wmi_event_roam(ar, skb);
+		break;
+	case WMI_TLV_PROFILE_MATCH:
+		ath10k_wmi_event_profile_match(ar, skb);
+		break;
+	case WMI_TLV_DEBUG_PRINT_EVENTID:
+		ath10k_wmi_event_debug_print(ar, skb);
+		break;
+	case WMI_TLV_PDEV_QVIT_EVENTID:
+		ath10k_wmi_event_pdev_qvit(ar, skb);
+		break;
+	case WMI_TLV_WLAN_PROFILE_DATA_EVENTID:
+		ath10k_wmi_event_wlan_profile_data(ar, skb);
+		break;
+	case WMI_TLV_RTT_MEASUREMENT_REPORT_EVENTID:
+		ath10k_wmi_event_rtt_measurement_report(ar, skb);
+		break;
+	case WMI_TLV_TSF_MEASUREMENT_REPORT_EVENTID:
+		ath10k_wmi_event_tsf_measurement_report(ar, skb);
+		break;
+	case WMI_TLV_RTT_ERROR_REPORT_EVENTID:
+		ath10k_wmi_event_rtt_error_report(ar, skb);
+		break;
+	case WMI_TLV_WOW_WAKEUP_HOST_EVENTID:
+		ath10k_wmi_event_wow_wakeup_host(ar, skb);
+		break;
+	case WMI_TLV_DCS_INTERFERENCE_EVENTID:
+		ath10k_wmi_event_dcs_interference(ar, skb);
+		break;
+	case WMI_TLV_PDEV_TPC_CONFIG_EVENTID:
+		ath10k_wmi_event_pdev_tpc_config(ar, skb);
+		break;
+	case WMI_TLV_PDEV_FTM_INTG_EVENTID:
+		ath10k_wmi_event_pdev_ftm_intg(ar, skb);
+		break;
+	case WMI_TLV_GTK_OFFLOAD_STATUS_EVENTID:
+		ath10k_wmi_event_gtk_offload_status(ar, skb);
+		break;
+	case WMI_TLV_GTK_REKEY_FAIL_EVENTID:
+		ath10k_wmi_event_gtk_rekey_fail(ar, skb);
+		break;
+	case WMI_TLV_TX_DELBA_COMPLETE_EVENTID:
+		ath10k_wmi_event_delba_complete(ar, skb);
+		break;
+	case WMI_TLV_TX_ADDBA_COMPLETE_EVENTID:
+		ath10k_wmi_event_addba_complete(ar, skb);
+		break;
+	case WMI_TLV_VDEV_INSTALL_KEY_COMPLETE_EVENTID:
+		ath10k_wmi_event_vdev_install_key_complete(ar, skb);
+		break;
+	case WMI_TLV_SERVICE_READY_EVENTID:
+		ath10k_wmi_event_service_ready(ar, skb);
+		break;
+	case WMI_TLV_READY_EVENTID:
+		ath10k_wmi_event_ready(ar, skb);
+		break;
+	default:
+		ath10k_warn(ar, "Unknown eventid: %d\n", id);
+		break;
+	}
+
+	dev_kfree_skb(skb);
+}
+
+static int ath10k_wmi_tlv_op_pull_scan_ev(struct ath10k *ar,
+					  struct sk_buff *skb,
+					  struct wmi_scan_ev_arg *arg)
+{
+	const void **tb;
+	const struct wmi_scan_event *ev;
+	int ret;
+
+	tb = ath10k_wmi_tlv_parse_alloc(ar, skb->data, skb->len, GFP_ATOMIC);
+	if (IS_ERR(tb)) {
+		ret = PTR_ERR(tb);
+		ath10k_warn(ar, "failed to parse tlv: %d\n", ret);
+		return ret;
+	}
+
+	ev = tb[WMI_TLV_TAG_STRUCT_SCAN_EVENT];
+	if (!ev) {
+		kfree(tb);
+		return -EPROTO;
+	}
+
+	arg->event_type = ev->event_type;
+	arg->reason = ev->reason;
+	arg->channel_freq = ev->channel_freq;
+	arg->scan_req_id = ev->scan_req_id;
+	arg->scan_id = ev->scan_id;
+	arg->vdev_id = ev->vdev_id;
+
+	kfree(tb);
+	return 0;
+}
+
+static int ath10k_wmi_tlv_op_pull_mgmt_rx_ev(struct ath10k *ar,
+					     struct sk_buff *skb,
+					     struct wmi_mgmt_rx_ev_arg *arg)
+{
+	const void **tb;
+	const struct wmi_tlv_mgmt_rx_ev *ev;
+	const u8 *frame;
+	u32 msdu_len;
+	int ret;
+
+	tb = ath10k_wmi_tlv_parse_alloc(ar, skb->data, skb->len, GFP_ATOMIC);
+	if (IS_ERR(tb)) {
+		ret = PTR_ERR(tb);
+		ath10k_warn(ar, "failed to parse tlv: %d\n", ret);
+		return ret;
+	}
+
+	ev = tb[WMI_TLV_TAG_STRUCT_MGMT_RX_HDR];
+	frame = tb[WMI_TLV_TAG_ARRAY_BYTE];
+
+	if (!ev || !frame) {
+		kfree(tb);
+		return -EPROTO;
+	}
+
+	arg->channel = ev->channel;
+	arg->buf_len = ev->buf_len;
+	arg->status = ev->status;
+	arg->snr = ev->snr;
+	arg->phy_mode = ev->phy_mode;
+	arg->rate = ev->rate;
+
+	msdu_len = __le32_to_cpu(arg->buf_len);
+
+	if (skb->len < (frame - skb->data) + msdu_len) {
+		kfree(tb);
+		return -EPROTO;
+	}
+
+	/* shift the sk_buff to point to `frame` */
+	skb_trim(skb, 0);
+	skb_put(skb, frame - skb->data);
+	skb_pull(skb, frame - skb->data);
+	skb_put(skb, msdu_len);
+
+	kfree(tb);
+	return 0;
+}
+
+static int ath10k_wmi_tlv_op_pull_ch_info_ev(struct ath10k *ar,
+					     struct sk_buff *skb,
+					     struct wmi_ch_info_ev_arg *arg)
+{
+	const void **tb;
+	const struct wmi_chan_info_event *ev;
+	int ret;
+
+	tb = ath10k_wmi_tlv_parse_alloc(ar, skb->data, skb->len, GFP_ATOMIC);
+	if (IS_ERR(tb)) {
+		ret = PTR_ERR(tb);
+		ath10k_warn(ar, "failed to parse tlv: %d\n", ret);
+		return ret;
+	}
+
+	ev = tb[WMI_TLV_TAG_STRUCT_CHAN_INFO_EVENT];
+	if (!ev) {
+		kfree(tb);
+		return -EPROTO;
+	}
+
+	arg->err_code = ev->err_code;
+	arg->freq = ev->freq;
+	arg->cmd_flags = ev->cmd_flags;
+	arg->noise_floor = ev->noise_floor;
+	arg->rx_clear_count = ev->rx_clear_count;
+	arg->cycle_count = ev->cycle_count;
+
+	kfree(tb);
+	return 0;
+}
+
+static int
+ath10k_wmi_tlv_op_pull_vdev_start_ev(struct ath10k *ar, struct sk_buff *skb,
+				     struct wmi_vdev_start_ev_arg *arg)
+{
+	const void **tb;
+	const struct wmi_vdev_start_response_event *ev;
+	int ret;
+
+	tb = ath10k_wmi_tlv_parse_alloc(ar, skb->data, skb->len, GFP_ATOMIC);
+	if (IS_ERR(tb)) {
+		ret = PTR_ERR(tb);
+		ath10k_warn(ar, "failed to parse tlv: %d\n", ret);
+		return ret;
+	}
+
+	ev = tb[WMI_TLV_TAG_STRUCT_VDEV_START_RESPONSE_EVENT];
+	if (!ev) {
+		kfree(tb);
+		return -EPROTO;
+	}
+
+	skb_pull(skb, sizeof(*ev));
+	arg->vdev_id = ev->vdev_id;
+	arg->req_id = ev->req_id;
+	arg->resp_type = ev->resp_type;
+	arg->status = ev->status;
+
+	kfree(tb);
+	return 0;
+}
+
+static int ath10k_wmi_tlv_op_pull_peer_kick_ev(struct ath10k *ar,
+					       struct sk_buff *skb,
+					       struct wmi_peer_kick_ev_arg *arg)
+{
+	const void **tb;
+	const struct wmi_peer_sta_kickout_event *ev;
+	int ret;
+
+	tb = ath10k_wmi_tlv_parse_alloc(ar, skb->data, skb->len, GFP_ATOMIC);
+	if (IS_ERR(tb)) {
+		ret = PTR_ERR(tb);
+		ath10k_warn(ar, "failed to parse tlv: %d\n", ret);
+		return ret;
+	}
+
+	ev = tb[WMI_TLV_TAG_STRUCT_PEER_STA_KICKOUT_EVENT];
+	if (!ev) {
+		kfree(tb);
+		return -EPROTO;
+	}
+
+	arg->mac_addr = ev->peer_macaddr.addr;
+
+	kfree(tb);
+	return 0;
+}
+
+struct wmi_tlv_swba_parse {
+	const struct wmi_host_swba_event *ev;
+	bool tim_done;
+	bool noa_done;
+	size_t n_tim;
+	size_t n_noa;
+	struct wmi_swba_ev_arg *arg;
+};
+
+static int ath10k_wmi_tlv_swba_tim_parse(struct ath10k *ar, u16 tag, u16 len,
+					 const void *ptr, void *data)
+{
+	struct wmi_tlv_swba_parse *swba = data;
+
+	if (tag != WMI_TLV_TAG_STRUCT_TIM_INFO)
+		return -EPROTO;
+
+	if (swba->n_tim >= ARRAY_SIZE(swba->arg->tim_info))
+		return -ENOBUFS;
+
+	swba->arg->tim_info[swba->n_tim++] = ptr;
+	return 0;
+}
+
+static int ath10k_wmi_tlv_swba_noa_parse(struct ath10k *ar, u16 tag, u16 len,
+					 const void *ptr, void *data)
+{
+	struct wmi_tlv_swba_parse *swba = data;
+
+	if (tag != WMI_TLV_TAG_STRUCT_P2P_NOA_INFO)
+		return -EPROTO;
+
+	if (swba->n_noa >= ARRAY_SIZE(swba->arg->noa_info))
+		return -ENOBUFS;
+
+	swba->arg->noa_info[swba->n_noa++] = ptr;
+	return 0;
+}
+
+static int ath10k_wmi_tlv_swba_parse(struct ath10k *ar, u16 tag, u16 len,
+				     const void *ptr, void *data)
+{
+	struct wmi_tlv_swba_parse *swba = data;
+	int ret;
+
+	switch (tag) {
+	case WMI_TLV_TAG_STRUCT_HOST_SWBA_EVENT:
+		swba->ev = ptr;
+		break;
+	case WMI_TLV_TAG_ARRAY_STRUCT:
+		if (!swba->tim_done) {
+			swba->tim_done = true;
+			ret = ath10k_wmi_tlv_iter(ar, ptr, len,
+						  ath10k_wmi_tlv_swba_tim_parse,
+						  swba);
+			if (ret)
+				return ret;
+		} else if (!swba->noa_done) {
+			swba->noa_done = true;
+			ret = ath10k_wmi_tlv_iter(ar, ptr, len,
+						  ath10k_wmi_tlv_swba_noa_parse,
+						  swba);
+			if (ret)
+				return ret;
+		}
+		break;
+	default:
+		break;
+	}
+	return 0;
+}
+
+static int ath10k_wmi_tlv_op_pull_swba_ev(struct ath10k *ar,
+					  struct sk_buff *skb,
+					  struct wmi_swba_ev_arg *arg)
+{
+	struct wmi_tlv_swba_parse swba = { .arg = arg };
+	u32 map;
+	size_t n_vdevs;
+	int ret;
+
+	ret = ath10k_wmi_tlv_iter(ar, skb->data, skb->len,
+				  ath10k_wmi_tlv_swba_parse, &swba);
+	if (ret) {
+		ath10k_warn(ar, "failed to parse tlv: %d\n", ret);
+		return ret;
+	}
+
+	if (!swba.ev)
+		return -EPROTO;
+
+	arg->vdev_map = swba.ev->vdev_map;
+
+	for (map = __le32_to_cpu(arg->vdev_map), n_vdevs = 0; map; map >>= 1)
+		if (map & BIT(0))
+			n_vdevs++;
+
+	if (n_vdevs != swba.n_tim ||
+	    n_vdevs != swba.n_noa)
+		return -EPROTO;
+
+	return 0;
+}
+
+static int ath10k_wmi_tlv_op_pull_phyerr_ev(struct ath10k *ar,
+					    struct sk_buff *skb,
+					    struct wmi_phyerr_ev_arg *arg)
+{
+	const void **tb;
+	const struct wmi_tlv_phyerr_ev *ev;
+	const void *phyerrs;
+	int ret;
+
+	tb = ath10k_wmi_tlv_parse_alloc(ar, skb->data, skb->len, GFP_ATOMIC);
+	if (IS_ERR(tb)) {
+		ret = PTR_ERR(tb);
+		ath10k_warn(ar, "failed to parse tlv: %d\n", ret);
+		return ret;
+	}
+
+	ev = tb[WMI_TLV_TAG_STRUCT_COMB_PHYERR_RX_HDR];
+	phyerrs = tb[WMI_TLV_TAG_ARRAY_BYTE];
+
+	if (!ev || !phyerrs) {
+		kfree(tb);
+		return -EPROTO;
+	}
+
+	arg->num_phyerrs  = ev->num_phyerrs;
+	arg->tsf_l32 = ev->tsf_l32;
+	arg->tsf_u32 = ev->tsf_u32;
+	arg->buf_len = ev->buf_len;
+	arg->phyerrs = phyerrs;
+
+	kfree(tb);
+	return 0;
+}
+
+#define WMI_TLV_ABI_VER_NS0 0x5F414351
+#define WMI_TLV_ABI_VER_NS1 0x00004C4D
+#define WMI_TLV_ABI_VER_NS2 0x00000000
+#define WMI_TLV_ABI_VER_NS3 0x00000000
+
+#define WMI_TLV_ABI_VER0_MAJOR 1
+#define WMI_TLV_ABI_VER0_MINOR 0
+#define WMI_TLV_ABI_VER0 ((((WMI_TLV_ABI_VER0_MAJOR) << 24) & 0xFF000000) | \
+			  (((WMI_TLV_ABI_VER0_MINOR) <<  0) & 0x00FFFFFF))
+#define WMI_TLV_ABI_VER1 53
+
+static int
+ath10k_wmi_tlv_parse_mem_reqs(struct ath10k *ar, u16 tag, u16 len,
+			      const void *ptr, void *data)
+{
+	struct wmi_svc_rdy_ev_arg *arg = data;
+	int i;
+
+	if (tag != WMI_TLV_TAG_STRUCT_WLAN_HOST_MEM_REQ)
+		return -EPROTO;
+
+	for (i = 0; i < ARRAY_SIZE(arg->mem_reqs); i++) {
+		if (!arg->mem_reqs[i]) {
+			arg->mem_reqs[i] = ptr;
+			return 0;
+		}
+	}
+
+	return -ENOMEM;
+}
+
+static int ath10k_wmi_tlv_op_pull_svc_rdy_ev(struct ath10k *ar,
+					     struct sk_buff *skb,
+					     struct wmi_svc_rdy_ev_arg *arg)
+{
+	const void **tb;
+	const struct hal_reg_capabilities *reg;
+	const struct wmi_tlv_svc_rdy_ev *ev;
+	const __le32 *svc_bmap;
+	const struct wlan_host_mem_req *mem_reqs;
+	int ret;
+
+	tb = ath10k_wmi_tlv_parse_alloc(ar, skb->data, skb->len, GFP_ATOMIC);
+	if (IS_ERR(tb)) {
+		ret = PTR_ERR(tb);
+		ath10k_warn(ar, "failed to parse tlv: %d\n", ret);
+		return ret;
+	}
+
+	ev = tb[WMI_TLV_TAG_STRUCT_SERVICE_READY_EVENT];
+	reg = tb[WMI_TLV_TAG_STRUCT_HAL_REG_CAPABILITIES];
+	svc_bmap = tb[WMI_TLV_TAG_ARRAY_UINT32];
+	mem_reqs = tb[WMI_TLV_TAG_ARRAY_STRUCT];
+
+	if (!ev || !reg || !svc_bmap || !mem_reqs) {
+		kfree(tb);
+		return -EPROTO;
+	}
+
+	/* This is an internal ABI compatibility check for WMI TLV so check it
+	 * here instead of the generic WMI code.
+	 */
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
+		   "wmi tlv abi 0x%08x ?= 0x%08x, 0x%08x ?= 0x%08x, 0x%08x ?= 0x%08x, 0x%08x ?= 0x%08x, 0x%08x ?= 0x%08x\n",
+		   __le32_to_cpu(ev->abi.abi_ver0), WMI_TLV_ABI_VER0,
+		   __le32_to_cpu(ev->abi.abi_ver_ns0), WMI_TLV_ABI_VER_NS0,
+		   __le32_to_cpu(ev->abi.abi_ver_ns1), WMI_TLV_ABI_VER_NS1,
+		   __le32_to_cpu(ev->abi.abi_ver_ns2), WMI_TLV_ABI_VER_NS2,
+		   __le32_to_cpu(ev->abi.abi_ver_ns3), WMI_TLV_ABI_VER_NS3);
+
+	if (__le32_to_cpu(ev->abi.abi_ver0) != WMI_TLV_ABI_VER0 ||
+	    __le32_to_cpu(ev->abi.abi_ver_ns0) != WMI_TLV_ABI_VER_NS0 ||
+	    __le32_to_cpu(ev->abi.abi_ver_ns1) != WMI_TLV_ABI_VER_NS1 ||
+	    __le32_to_cpu(ev->abi.abi_ver_ns2) != WMI_TLV_ABI_VER_NS2 ||
+	    __le32_to_cpu(ev->abi.abi_ver_ns3) != WMI_TLV_ABI_VER_NS3) {
+		kfree(tb);
+		return -ENOTSUPP;
+	}
+
+	arg->min_tx_power = ev->hw_min_tx_power;
+	arg->max_tx_power = ev->hw_max_tx_power;
+	arg->ht_cap = ev->ht_cap_info;
+	arg->vht_cap = ev->vht_cap_info;
+	arg->sw_ver0 = ev->abi.abi_ver0;
+	arg->sw_ver1 = ev->abi.abi_ver1;
+	arg->fw_build = ev->fw_build_vers;
+	arg->phy_capab = ev->phy_capability;
+	arg->num_rf_chains = ev->num_rf_chains;
+	arg->eeprom_rd = reg->eeprom_rd;
+	arg->num_mem_reqs = ev->num_mem_reqs;
+	arg->service_map = svc_bmap;
+	arg->service_map_len = ath10k_wmi_tlv_len(svc_bmap);
+
+	ret = ath10k_wmi_tlv_iter(ar, mem_reqs, ath10k_wmi_tlv_len(mem_reqs),
+				  ath10k_wmi_tlv_parse_mem_reqs, arg);
+	if (ret) {
+		kfree(tb);
+		ath10k_warn(ar, "failed to parse mem_reqs tlv: %d\n", ret);
+		return ret;
+	}
+
+	kfree(tb);
+	return 0;
+}
+
+static int ath10k_wmi_tlv_op_pull_rdy_ev(struct ath10k *ar,
+					 struct sk_buff *skb,
+					 struct wmi_rdy_ev_arg *arg)
+{
+	const void **tb;
+	const struct wmi_tlv_rdy_ev *ev;
+	int ret;
+
+	tb = ath10k_wmi_tlv_parse_alloc(ar, skb->data, skb->len, GFP_ATOMIC);
+	if (IS_ERR(tb)) {
+		ret = PTR_ERR(tb);
+		ath10k_warn(ar, "failed to parse tlv: %d\n", ret);
+		return ret;
+	}
+
+	ev = tb[WMI_TLV_TAG_STRUCT_READY_EVENT];
+	if (!ev) {
+		kfree(tb);
+		return -EPROTO;
+	}
+
+	arg->sw_version = ev->abi.abi_ver0;
+	arg->abi_version = ev->abi.abi_ver1;
+	arg->status = ev->status;
+	arg->mac_addr = ev->mac_addr.addr;
+
+	kfree(tb);
+	return 0;
+}
+
+static int ath10k_wmi_tlv_op_pull_fw_stats(struct ath10k *ar,
+					   struct sk_buff *skb,
+					   struct ath10k_fw_stats *stats)
+{
+	const void **tb;
+	const struct wmi_stats_event *ev;
+	const void *data;
+	u32 num_pdev_stats, num_vdev_stats, num_peer_stats;
+	size_t data_len;
+	int ret;
+
+	tb = ath10k_wmi_tlv_parse_alloc(ar, skb->data, skb->len, GFP_ATOMIC);
+	if (IS_ERR(tb)) {
+		ret = PTR_ERR(tb);
+		ath10k_warn(ar, "failed to parse tlv: %d\n", ret);
+		return ret;
+	}
+
+	ev = tb[WMI_TLV_TAG_STRUCT_STATS_EVENT];
+	data = tb[WMI_TLV_TAG_ARRAY_BYTE];
+
+	if (!ev || !data) {
+		kfree(tb);
+		return -EPROTO;
+	}
+
+	data_len = ath10k_wmi_tlv_len(data);
+	num_pdev_stats = __le32_to_cpu(ev->num_pdev_stats);
+	num_vdev_stats = __le32_to_cpu(ev->num_vdev_stats);
+	num_peer_stats = __le32_to_cpu(ev->num_peer_stats);
+
+	WARN_ON(1); /* FIXME: not implemented yet */
+
+	kfree(tb);
+	return 0;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_pdev_suspend(struct ath10k *ar, u32 opt)
+{
+	struct wmi_tlv_pdev_suspend *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_PDEV_SUSPEND_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->opt = __cpu_to_le32(opt);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv pdev suspend\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_pdev_resume(struct ath10k *ar)
+{
+	struct wmi_tlv_resume_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_PDEV_RESUME_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->reserved = __cpu_to_le32(0);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv pdev resume\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_pdev_set_rd(struct ath10k *ar,
+				  u16 rd, u16 rd2g, u16 rd5g,
+				  u16 ctl2g, u16 ctl5g,
+				  enum wmi_dfs_region dfs_reg)
+{
+	struct wmi_tlv_pdev_set_rd_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_PDEV_SET_REGDOMAIN_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->regd = __cpu_to_le32(rd);
+	cmd->regd_2ghz = __cpu_to_le32(rd2g);
+	cmd->regd_5ghz = __cpu_to_le32(rd5g);
+	cmd->conform_limit_2ghz = __cpu_to_le32(rd2g);
+	cmd->conform_limit_5ghz = __cpu_to_le32(rd5g);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv pdev set rd\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_pdev_set_param(struct ath10k *ar, u32 param_id,
+				     u32 param_value)
+{
+	struct wmi_tlv_pdev_set_param_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_PDEV_SET_PARAM_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->param_id = __cpu_to_le32(param_id);
+	cmd->param_value = __cpu_to_le32(param_value);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv pdev set param\n");
+	return skb;
+}
+
+static struct sk_buff *ath10k_wmi_tlv_op_gen_init(struct ath10k *ar)
+{
+	struct sk_buff *skb;
+	struct wmi_tlv *tlv;
+	struct wmi_tlv_init_cmd *cmd;
+	struct wmi_tlv_resource_config *cfg;
+	struct wmi_host_mem_chunks *chunks;
+	size_t len, chunks_len;
+	void *ptr;
+
+	chunks_len = ar->wmi.num_mem_chunks * sizeof(struct host_memory_chunk);
+	len = (sizeof(*tlv) + sizeof(*cmd)) +
+	      (sizeof(*tlv) + sizeof(*cfg)) +
+	      (sizeof(*tlv) + chunks_len);
+
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	ptr = skb->data;
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_INIT_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	ptr += sizeof(*tlv);
+	ptr += sizeof(*cmd);
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_RESOURCE_CONFIG);
+	tlv->len = __cpu_to_le16(sizeof(*cfg));
+	cfg = (void *)tlv->value;
+	ptr += sizeof(*tlv);
+	ptr += sizeof(*cfg);
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_STRUCT);
+	tlv->len = __cpu_to_le16(chunks_len);
+	chunks = (void *)tlv->value;
+
+	ptr += sizeof(*tlv);
+	ptr += chunks_len;
+
+	cmd->abi.abi_ver0 = __cpu_to_le32(WMI_TLV_ABI_VER0);
+	cmd->abi.abi_ver1 = __cpu_to_le32(WMI_TLV_ABI_VER1);
+	cmd->abi.abi_ver_ns0 = __cpu_to_le32(WMI_TLV_ABI_VER_NS0);
+	cmd->abi.abi_ver_ns1 = __cpu_to_le32(WMI_TLV_ABI_VER_NS1);
+	cmd->abi.abi_ver_ns2 = __cpu_to_le32(WMI_TLV_ABI_VER_NS2);
+	cmd->abi.abi_ver_ns3 = __cpu_to_le32(WMI_TLV_ABI_VER_NS3);
+	cmd->num_host_mem_chunks = __cpu_to_le32(ar->wmi.num_mem_chunks);
+
+	cfg->num_vdevs = __cpu_to_le32(TARGET_TLV_NUM_VDEVS);
+	cfg->num_peers = __cpu_to_le32(TARGET_TLV_NUM_PEERS);
+	cfg->num_offload_peers = __cpu_to_le32(0);
+	cfg->num_offload_reorder_bufs = __cpu_to_le32(0);
+	cfg->num_peer_keys = __cpu_to_le32(2);
+	cfg->num_tids = __cpu_to_le32(TARGET_TLV_NUM_TIDS);
+	cfg->ast_skid_limit = __cpu_to_le32(0x10);
+	cfg->tx_chain_mask = __cpu_to_le32(0x7);
+	cfg->rx_chain_mask = __cpu_to_le32(0x7);
+	cfg->rx_timeout_pri[0] = __cpu_to_le32(0x64);
+	cfg->rx_timeout_pri[1] = __cpu_to_le32(0x64);
+	cfg->rx_timeout_pri[2] = __cpu_to_le32(0x64);
+	cfg->rx_timeout_pri[3] = __cpu_to_le32(0x28);
+	cfg->rx_decap_mode = __cpu_to_le32(1);
+	cfg->scan_max_pending_reqs = __cpu_to_le32(4);
+	cfg->bmiss_offload_max_vdev = __cpu_to_le32(3);
+	cfg->roam_offload_max_vdev = __cpu_to_le32(3);
+	cfg->roam_offload_max_ap_profiles = __cpu_to_le32(8);
+	cfg->num_mcast_groups = __cpu_to_le32(0);
+	cfg->num_mcast_table_elems = __cpu_to_le32(0);
+	cfg->mcast2ucast_mode = __cpu_to_le32(0);
+	cfg->tx_dbg_log_size = __cpu_to_le32(0x400);
+	cfg->num_wds_entries = __cpu_to_le32(0x20);
+	cfg->dma_burst_size = __cpu_to_le32(0);
+	cfg->mac_aggr_delim = __cpu_to_le32(0);
+	cfg->rx_skip_defrag_timeout_dup_detection_check = __cpu_to_le32(0);
+	cfg->vow_config = __cpu_to_le32(0);
+	cfg->gtk_offload_max_vdev = __cpu_to_le32(2);
+	cfg->num_msdu_desc = __cpu_to_le32(TARGET_TLV_NUM_MSDU_DESC);
+	cfg->max_frag_entries = __cpu_to_le32(2);
+	cfg->num_tdls_vdevs = __cpu_to_le32(1);
+	cfg->num_tdls_conn_table_entries = __cpu_to_le32(0x20);
+	cfg->beacon_tx_offload_max_vdev = __cpu_to_le32(2);
+	cfg->num_multicast_filter_entries = __cpu_to_le32(5);
+	cfg->num_wow_filters = __cpu_to_le32(0x16);
+	cfg->num_keep_alive_pattern = __cpu_to_le32(6);
+	cfg->keep_alive_pattern_size = __cpu_to_le32(0);
+	cfg->max_tdls_concurrent_sleep_sta = __cpu_to_le32(1);
+	cfg->max_tdls_concurrent_buffer_sta = __cpu_to_le32(1);
+
+	ath10k_wmi_put_host_mem_chunks(ar, chunks);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv init\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_start_scan(struct ath10k *ar,
+				 const struct wmi_start_scan_arg *arg)
+{
+	struct wmi_tlv_start_scan_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+	size_t len, chan_len, ssid_len, bssid_len, ie_len;
+	__le32 *chans;
+	struct wmi_ssid *ssids;
+	struct wmi_mac_addr *addrs;
+	void *ptr;
+	int i, ret;
+
+	ret = ath10k_wmi_start_scan_verify(arg);
+	if (ret)
+		return ERR_PTR(ret);
+
+	chan_len = arg->n_channels * sizeof(__le32);
+	ssid_len = arg->n_ssids * sizeof(struct wmi_ssid);
+	bssid_len = arg->n_bssids * sizeof(struct wmi_mac_addr);
+	ie_len = roundup(arg->ie_len, 4);
+	len = (sizeof(*tlv) + sizeof(*cmd)) +
+	      (arg->n_channels ? sizeof(*tlv) + chan_len : 0) +
+	      (arg->n_ssids ? sizeof(*tlv) + ssid_len : 0) +
+	      (arg->n_bssids ? sizeof(*tlv) + bssid_len : 0) +
+	      (arg->ie_len ? sizeof(*tlv) + ie_len : 0);
+
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	ptr = (void *)skb->data;
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_START_SCAN_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+
+	ath10k_wmi_put_start_scan_common(&cmd->common, arg);
+	cmd->burst_duration_ms = __cpu_to_le32(0);
+	cmd->num_channels = __cpu_to_le32(arg->n_channels);
+	cmd->num_ssids = __cpu_to_le32(arg->n_ssids);
+	cmd->num_bssids = __cpu_to_le32(arg->n_bssids);
+	cmd->ie_len = __cpu_to_le32(arg->ie_len);
+	cmd->num_probes = __cpu_to_le32(3);
+
+	/* FIXME: There are some scan flag inconsistencies across firmwares,
+	 * e.g. WMI-TLV inverts the logic behind the following flag.
+	 */
+	cmd->common.scan_ctrl_flags ^= __cpu_to_le32(WMI_SCAN_FILTER_PROBE_REQ);
+
+	ptr += sizeof(*tlv);
+	ptr += sizeof(*cmd);
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_UINT32);
+	tlv->len = __cpu_to_le16(chan_len);
+	chans = (void *)tlv->value;
+	for (i = 0; i < arg->n_channels; i++)
+		chans[i] = __cpu_to_le32(arg->channels[i]);
+
+	ptr += sizeof(*tlv);
+	ptr += chan_len;
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_FIXED_STRUCT);
+	tlv->len = __cpu_to_le16(ssid_len);
+	ssids = (void *)tlv->value;
+	for (i = 0; i < arg->n_ssids; i++) {
+		ssids[i].ssid_len = __cpu_to_le32(arg->ssids[i].len);
+		memcpy(ssids[i].ssid, arg->ssids[i].ssid, arg->ssids[i].len);
+	}
+
+	ptr += sizeof(*tlv);
+	ptr += ssid_len;
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_FIXED_STRUCT);
+	tlv->len = __cpu_to_le16(bssid_len);
+	addrs = (void *)tlv->value;
+	for (i = 0; i < arg->n_bssids; i++)
+		ether_addr_copy(addrs[i].addr, arg->bssids[i].bssid);
+
+	ptr += sizeof(*tlv);
+	ptr += bssid_len;
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_BYTE);
+	tlv->len = __cpu_to_le16(ie_len);
+	memcpy(tlv->value, arg->ie, arg->ie_len);
+
+	ptr += sizeof(*tlv);
+	ptr += ie_len;
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv start scan\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_stop_scan(struct ath10k *ar,
+				const struct wmi_stop_scan_arg *arg)
+{
+	struct wmi_stop_scan_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+	u32 scan_id;
+	u32 req_id;
+
+	if (arg->req_id > 0xFFF)
+		return ERR_PTR(-EINVAL);
+	if (arg->req_type == WMI_SCAN_STOP_ONE && arg->u.scan_id > 0xFFF)
+		return ERR_PTR(-EINVAL);
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	scan_id = arg->u.scan_id;
+	scan_id |= WMI_HOST_SCAN_REQ_ID_PREFIX;
+
+	req_id = arg->req_id;
+	req_id |= WMI_HOST_SCAN_REQUESTOR_ID_PREFIX;
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_STOP_SCAN_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->req_type = __cpu_to_le32(arg->req_type);
+	cmd->vdev_id = __cpu_to_le32(arg->u.vdev_id);
+	cmd->scan_id = __cpu_to_le32(scan_id);
+	cmd->scan_req_id = __cpu_to_le32(req_id);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv stop scan\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_vdev_create(struct ath10k *ar,
+				  u32 vdev_id,
+				  enum wmi_vdev_type vdev_type,
+				  enum wmi_vdev_subtype vdev_subtype,
+				  const u8 mac_addr[ETH_ALEN])
+{
+	struct wmi_vdev_create_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_VDEV_CREATE_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->vdev_id = __cpu_to_le32(vdev_id);
+	cmd->vdev_type = __cpu_to_le32(vdev_type);
+	cmd->vdev_subtype = __cpu_to_le32(vdev_subtype);
+	ether_addr_copy(cmd->vdev_macaddr.addr, mac_addr);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv vdev create\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_vdev_delete(struct ath10k *ar, u32 vdev_id)
+{
+	struct wmi_vdev_delete_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_VDEV_DELETE_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->vdev_id = __cpu_to_le32(vdev_id);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv vdev delete\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_vdev_start(struct ath10k *ar,
+				 const struct wmi_vdev_start_request_arg *arg,
+				 bool restart)
+{
+	struct wmi_tlv_vdev_start_cmd *cmd;
+	struct wmi_channel *ch;
+	struct wmi_p2p_noa_descriptor *noa;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+	size_t len;
+	void *ptr;
+	u32 flags = 0;
+
+	if (WARN_ON(arg->ssid && arg->ssid_len == 0))
+		return ERR_PTR(-EINVAL);
+	if (WARN_ON(arg->hidden_ssid && !arg->ssid))
+		return ERR_PTR(-EINVAL);
+	if (WARN_ON(arg->ssid_len > sizeof(cmd->ssid.ssid)))
+		return ERR_PTR(-EINVAL);
+
+	len = (sizeof(*tlv) + sizeof(*cmd)) +
+	      (sizeof(*tlv) + sizeof(*ch)) +
+	      (sizeof(*tlv) + 0);
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	if (arg->hidden_ssid)
+		flags |= WMI_VDEV_START_HIDDEN_SSID;
+	if (arg->pmf_enabled)
+		flags |= WMI_VDEV_START_PMF_ENABLED;
+
+	ptr = (void *)skb->data;
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_VDEV_START_REQUEST_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->vdev_id = __cpu_to_le32(arg->vdev_id);
+	cmd->bcn_intval = __cpu_to_le32(arg->bcn_intval);
+	cmd->dtim_period = __cpu_to_le32(arg->dtim_period);
+	cmd->flags = __cpu_to_le32(flags);
+	cmd->bcn_tx_rate = __cpu_to_le32(arg->bcn_tx_rate);
+	cmd->bcn_tx_power = __cpu_to_le32(arg->bcn_tx_power);
+	cmd->disable_hw_ack = __cpu_to_le32(arg->disable_hw_ack);
+
+	if (arg->ssid) {
+		cmd->ssid.ssid_len = __cpu_to_le32(arg->ssid_len);
+		memcpy(cmd->ssid.ssid, arg->ssid, arg->ssid_len);
+	}
+
+	ptr += sizeof(*tlv);
+	ptr += sizeof(*cmd);
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_CHANNEL);
+	tlv->len = __cpu_to_le16(sizeof(*ch));
+	ch = (void *)tlv->value;
+	ath10k_wmi_put_wmi_channel(ch, &arg->channel);
+
+	ptr += sizeof(*tlv);
+	ptr += sizeof(*ch);
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_STRUCT);
+	tlv->len = 0;
+	noa = (void *)tlv->value;
+
+	/* Note: This is a nested TLV containing:
+	 * [wmi_tlv][wmi_p2p_noa_descriptor][wmi_tlv]..
+	 */
+
+	ptr += sizeof(*tlv);
+	ptr += 0;
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv vdev start\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_vdev_stop(struct ath10k *ar, u32 vdev_id)
+{
+	struct wmi_vdev_stop_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_VDEV_STOP_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->vdev_id = __cpu_to_le32(vdev_id);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv vdev stop\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_vdev_up(struct ath10k *ar, u32 vdev_id, u32 aid,
+			      const u8 *bssid)
+
+{
+	struct wmi_vdev_up_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_VDEV_UP_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->vdev_id = __cpu_to_le32(vdev_id);
+	cmd->vdev_assoc_id = __cpu_to_le32(aid);
+	ether_addr_copy(cmd->vdev_bssid.addr, bssid);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv vdev up\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_vdev_down(struct ath10k *ar, u32 vdev_id)
+{
+	struct wmi_vdev_down_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_VDEV_DOWN_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->vdev_id = __cpu_to_le32(vdev_id);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv vdev down\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_vdev_set_param(struct ath10k *ar, u32 vdev_id,
+				     u32 param_id, u32 param_value)
+{
+	struct wmi_vdev_set_param_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_VDEV_SET_PARAM_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->vdev_id = __cpu_to_le32(vdev_id);
+	cmd->param_id = __cpu_to_le32(param_id);
+	cmd->param_value = __cpu_to_le32(param_value);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv vdev set param\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_vdev_install_key(struct ath10k *ar,
+				       const struct wmi_vdev_install_key_arg *arg)
+{
+	struct wmi_vdev_install_key_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+	size_t len;
+	void *ptr;
+
+	if (arg->key_cipher == WMI_CIPHER_NONE && arg->key_data != NULL)
+		return ERR_PTR(-EINVAL);
+	if (arg->key_cipher != WMI_CIPHER_NONE && arg->key_data == NULL)
+		return ERR_PTR(-EINVAL);
+
+	len = sizeof(*tlv) + sizeof(*cmd) +
+	      sizeof(*tlv) + roundup(arg->key_len, sizeof(__le32));
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	ptr = (void *)skb->data;
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_VDEV_INSTALL_KEY_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->vdev_id = __cpu_to_le32(arg->vdev_id);
+	cmd->key_idx = __cpu_to_le32(arg->key_idx);
+	cmd->key_flags = __cpu_to_le32(arg->key_flags);
+	cmd->key_cipher = __cpu_to_le32(arg->key_cipher);
+	cmd->key_len = __cpu_to_le32(arg->key_len);
+	cmd->key_txmic_len = __cpu_to_le32(arg->key_txmic_len);
+	cmd->key_rxmic_len = __cpu_to_le32(arg->key_rxmic_len);
+
+	if (arg->macaddr)
+		ether_addr_copy(cmd->peer_macaddr.addr, arg->macaddr);
+
+	ptr += sizeof(*tlv);
+	ptr += sizeof(*cmd);
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_BYTE);
+	tlv->len = __cpu_to_le16(roundup(arg->key_len, sizeof(__le32)));
+	if (arg->key_data)
+		memcpy(tlv->value, arg->key_data, arg->key_len);
+
+	ptr += sizeof(*tlv);
+	ptr += roundup(arg->key_len, sizeof(__le32));
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv vdev install key\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_peer_create(struct ath10k *ar, u32 vdev_id,
+				  const u8 peer_addr[ETH_ALEN])
+{
+	struct wmi_tlv_peer_create_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_PEER_CREATE_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->vdev_id = __cpu_to_le32(vdev_id);
+	cmd->peer_type = __cpu_to_le32(WMI_TLV_PEER_TYPE_DEFAULT); /* FIXME */
+	ether_addr_copy(cmd->peer_addr.addr, peer_addr);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv peer create\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_peer_delete(struct ath10k *ar, u32 vdev_id,
+				  const u8 peer_addr[ETH_ALEN])
+{
+	struct wmi_peer_delete_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_PEER_DELETE_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->vdev_id = __cpu_to_le32(vdev_id);
+	ether_addr_copy(cmd->peer_macaddr.addr, peer_addr);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv peer delete\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_peer_flush(struct ath10k *ar, u32 vdev_id,
+				 const u8 peer_addr[ETH_ALEN], u32 tid_bitmap)
+{
+	struct wmi_peer_flush_tids_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_PEER_FLUSH_TIDS_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->vdev_id = __cpu_to_le32(vdev_id);
+	cmd->peer_tid_bitmap = __cpu_to_le32(tid_bitmap);
+	ether_addr_copy(cmd->peer_macaddr.addr, peer_addr);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv peer flush\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_peer_set_param(struct ath10k *ar, u32 vdev_id,
+				     const u8 *peer_addr,
+				     enum wmi_peer_param param_id,
+				     u32 param_value)
+{
+	struct wmi_peer_set_param_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_PEER_SET_PARAM_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->vdev_id = __cpu_to_le32(vdev_id);
+	cmd->param_id = __cpu_to_le32(param_id);
+	cmd->param_value = __cpu_to_le32(param_value);
+	ether_addr_copy(cmd->peer_macaddr.addr, peer_addr);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv peer set param\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_peer_assoc(struct ath10k *ar,
+				 const struct wmi_peer_assoc_complete_arg *arg)
+{
+	struct wmi_tlv_peer_assoc_cmd *cmd;
+	struct wmi_vht_rate_set *vht_rate;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+	size_t len, legacy_rate_len, ht_rate_len;
+	void *ptr;
+
+	if (arg->peer_mpdu_density > 16)
+		return ERR_PTR(-EINVAL);
+	if (arg->peer_legacy_rates.num_rates > MAX_SUPPORTED_RATES)
+		return ERR_PTR(-EINVAL);
+	if (arg->peer_ht_rates.num_rates > MAX_SUPPORTED_RATES)
+		return ERR_PTR(-EINVAL);
+
+	legacy_rate_len = roundup(arg->peer_legacy_rates.num_rates,
+				  sizeof(__le32));
+	ht_rate_len = roundup(arg->peer_ht_rates.num_rates, sizeof(__le32));
+	len = (sizeof(*tlv) + sizeof(*cmd)) +
+	      (sizeof(*tlv) + legacy_rate_len) +
+	      (sizeof(*tlv) + ht_rate_len) +
+	      (sizeof(*tlv) + sizeof(*vht_rate));
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	ptr = (void *)skb->data;
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_PEER_ASSOC_COMPLETE_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+
+	cmd->vdev_id = __cpu_to_le32(arg->vdev_id);
+	cmd->new_assoc = __cpu_to_le32(arg->peer_reassoc ? 0 : 1);
+	cmd->assoc_id = __cpu_to_le32(arg->peer_aid);
+	cmd->flags = __cpu_to_le32(arg->peer_flags);
+	cmd->caps = __cpu_to_le32(arg->peer_caps);
+	cmd->listen_intval = __cpu_to_le32(arg->peer_listen_intval);
+	cmd->ht_caps = __cpu_to_le32(arg->peer_ht_caps);
+	cmd->max_mpdu = __cpu_to_le32(arg->peer_max_mpdu);
+	cmd->mpdu_density = __cpu_to_le32(arg->peer_mpdu_density);
+	cmd->rate_caps = __cpu_to_le32(arg->peer_rate_caps);
+	cmd->nss = __cpu_to_le32(arg->peer_num_spatial_streams);
+	cmd->vht_caps = __cpu_to_le32(arg->peer_vht_caps);
+	cmd->phy_mode = __cpu_to_le32(arg->peer_phymode);
+	cmd->num_legacy_rates = __cpu_to_le32(arg->peer_legacy_rates.num_rates);
+	cmd->num_ht_rates = __cpu_to_le32(arg->peer_ht_rates.num_rates);
+	ether_addr_copy(cmd->mac_addr.addr, arg->addr);
+
+	ptr += sizeof(*tlv);
+	ptr += sizeof(*cmd);
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_BYTE);
+	tlv->len = __cpu_to_le16(legacy_rate_len);
+	memcpy(tlv->value, arg->peer_legacy_rates.rates,
+	       arg->peer_legacy_rates.num_rates);
+
+	ptr += sizeof(*tlv);
+	ptr += legacy_rate_len;
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_BYTE);
+	tlv->len = __cpu_to_le16(ht_rate_len);
+	memcpy(tlv->value, arg->peer_ht_rates.rates,
+	       arg->peer_ht_rates.num_rates);
+
+	ptr += sizeof(*tlv);
+	ptr += ht_rate_len;
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_VHT_RATE_SET);
+	tlv->len = __cpu_to_le16(sizeof(*vht_rate));
+	vht_rate = (void *)tlv->value;
+
+	vht_rate->rx_max_rate = __cpu_to_le32(arg->peer_vht_rates.rx_max_rate);
+	vht_rate->rx_mcs_set = __cpu_to_le32(arg->peer_vht_rates.rx_mcs_set);
+	vht_rate->tx_max_rate = __cpu_to_le32(arg->peer_vht_rates.tx_max_rate);
+	vht_rate->tx_mcs_set = __cpu_to_le32(arg->peer_vht_rates.tx_mcs_set);
+
+	ptr += sizeof(*tlv);
+	ptr += sizeof(*vht_rate);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv peer assoc\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_set_psmode(struct ath10k *ar, u32 vdev_id,
+				 enum wmi_sta_ps_mode psmode)
+{
+	struct wmi_sta_powersave_mode_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_STA_POWERSAVE_MODE_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->vdev_id = __cpu_to_le32(vdev_id);
+	cmd->sta_ps_mode = __cpu_to_le32(psmode);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv set psmode\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_set_sta_ps(struct ath10k *ar, u32 vdev_id,
+				 enum wmi_sta_powersave_param param_id,
+				 u32 param_value)
+{
+	struct wmi_sta_powersave_param_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_STA_POWERSAVE_PARAM_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->vdev_id = __cpu_to_le32(vdev_id);
+	cmd->param_id = __cpu_to_le32(param_id);
+	cmd->param_value = __cpu_to_le32(param_value);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv set sta ps\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_set_ap_ps(struct ath10k *ar, u32 vdev_id, const u8 *mac,
+				enum wmi_ap_ps_peer_param param_id, u32 value)
+{
+	struct wmi_ap_ps_peer_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+
+	if (!mac)
+		return ERR_PTR(-EINVAL);
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_AP_PS_PEER_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->vdev_id = __cpu_to_le32(vdev_id);
+	cmd->param_id = __cpu_to_le32(param_id);
+	cmd->param_value = __cpu_to_le32(value);
+	ether_addr_copy(cmd->peer_macaddr.addr, mac);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv ap ps param\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_scan_chan_list(struct ath10k *ar,
+				     const struct wmi_scan_chan_list_arg *arg)
+{
+	struct wmi_tlv_scan_chan_list_cmd *cmd;
+	struct wmi_channel *ci;
+	struct wmi_channel_arg *ch;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+	size_t chans_len, len;
+	int i;
+	void *ptr, *chans;
+
+	chans_len = arg->n_channels * (sizeof(*tlv) + sizeof(*ci));
+	len = (sizeof(*tlv) + sizeof(*cmd)) +
+	      (sizeof(*tlv) + chans_len);
+
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	ptr = (void *)skb->data;
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_SCAN_CHAN_LIST_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->num_scan_chans = __cpu_to_le32(arg->n_channels);
+
+	ptr += sizeof(*tlv);
+	ptr += sizeof(*cmd);
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_STRUCT);
+	tlv->len = __cpu_to_le16(chans_len);
+	chans = (void *)tlv->value;
+
+	for (i = 0; i < arg->n_channels; i++) {
+		ch = &arg->channels[i];
+
+		tlv = chans;
+		tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_CHANNEL);
+		tlv->len = __cpu_to_le16(sizeof(*ci));
+		ci = (void *)tlv->value;
+
+		ath10k_wmi_put_wmi_channel(ci, ch);
+
+		chans += sizeof(*tlv);
+		chans += sizeof(*ci);
+	}
+
+	ptr += sizeof(*tlv);
+	ptr += chans_len;
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv scan chan list\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_beacon_dma(struct ath10k_vif *arvif)
+{
+	struct ath10k *ar = arvif->ar;
+	struct wmi_bcn_tx_ref_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+	struct sk_buff *beacon = arvif->beacon;
+	struct ieee80211_hdr *hdr;
+	u16 fc;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	hdr = (struct ieee80211_hdr *)beacon->data;
+	fc = le16_to_cpu(hdr->frame_control);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_BCN_SEND_FROM_HOST_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->vdev_id = __cpu_to_le32(arvif->vdev_id);
+	cmd->data_len = __cpu_to_le32(beacon->len);
+	cmd->data_ptr = __cpu_to_le32(ATH10K_SKB_CB(beacon)->paddr);
+	cmd->msdu_id = 0;
+	cmd->frame_control = __cpu_to_le32(fc);
+	cmd->flags = 0;
+
+	if (ATH10K_SKB_CB(beacon)->bcn.dtim_zero)
+		cmd->flags |= __cpu_to_le32(WMI_BCN_TX_REF_FLAG_DTIM_ZERO);
+
+	if (ATH10K_SKB_CB(beacon)->bcn.deliver_cab)
+		cmd->flags |= __cpu_to_le32(WMI_BCN_TX_REF_FLAG_DELIVER_CAB);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv beacon dma\n");
+	return skb;
+}
+
+static void *ath10k_wmi_tlv_put_wmm(void *ptr,
+				    const struct wmi_wmm_params_arg *arg)
+{
+	struct wmi_wmm_params *wmm;
+	struct wmi_tlv *tlv;
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_WMM_PARAMS);
+	tlv->len = __cpu_to_le16(sizeof(*wmm));
+	wmm = (void *)tlv->value;
+	ath10k_wmi_pdev_set_wmm_param(wmm, arg);
+
+	return ptr + sizeof(*tlv) + sizeof(*wmm);
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_pdev_set_wmm(struct ath10k *ar,
+				   const struct wmi_pdev_set_wmm_params_arg *arg)
+{
+	struct wmi_tlv_pdev_set_wmm_cmd *cmd;
+	struct wmi_wmm_params *wmm;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+	size_t len;
+	void *ptr;
+
+	len = (sizeof(*tlv) + sizeof(*cmd)) +
+	      (4 * (sizeof(*tlv) + sizeof(*wmm)));
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	ptr = (void *)skb->data;
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_PDEV_SET_WMM_PARAMS_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+
+	/* nothing to set here */
+
+	ptr += sizeof(*tlv);
+	ptr += sizeof(*cmd);
+
+	ptr = ath10k_wmi_tlv_put_wmm(ptr, &arg->ac_be);
+	ptr = ath10k_wmi_tlv_put_wmm(ptr, &arg->ac_bk);
+	ptr = ath10k_wmi_tlv_put_wmm(ptr, &arg->ac_vi);
+	ptr = ath10k_wmi_tlv_put_wmm(ptr, &arg->ac_vo);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv pdev set wmm\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_request_stats(struct ath10k *ar,
+				    enum wmi_stats_id stats_id)
+{
+	struct wmi_request_stats_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_REQUEST_STATS_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->stats_id = __cpu_to_le32(stats_id);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv request stats\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_force_fw_hang(struct ath10k *ar,
+				    enum wmi_force_fw_hang_type type,
+				    u32 delay_ms)
+{
+	struct wmi_force_fw_hang_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (void *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_FORCE_FW_HANG_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->type = __cpu_to_le32(type);
+	cmd->delay_ms = __cpu_to_le32(delay_ms);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv force fw hang\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_dbglog_cfg(struct ath10k *ar, u32 module_enable)
+{
+	struct wmi_tlv_dbglog_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+	size_t len, bmap_len;
+	u32 value;
+	void *ptr;
+
+	if (module_enable) {
+		value = WMI_TLV_DBGLOG_LOG_LEVEL_VALUE(
+				module_enable,
+				WMI_TLV_DBGLOG_LOG_LEVEL_VERBOSE);
+	} else {
+		value = WMI_TLV_DBGLOG_LOG_LEVEL_VALUE(
+				WMI_TLV_DBGLOG_ALL_MODULES,
+				WMI_TLV_DBGLOG_LOG_LEVEL_WARN);
+	}
+
+	bmap_len = 0;
+	len = sizeof(*tlv) + sizeof(*cmd) + sizeof(*tlv) + bmap_len;
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	ptr = (void *)skb->data;
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_DEBUG_LOG_CONFIG_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->param = __cpu_to_le32(WMI_TLV_DBGLOG_PARAM_LOG_LEVEL);
+	cmd->value = __cpu_to_le32(value);
+
+	ptr += sizeof(*tlv);
+	ptr += sizeof(*cmd);
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_UINT32);
+	tlv->len = __cpu_to_le16(bmap_len);
+
+	/* nothing to do here */
+
+	ptr += sizeof(*tlv);
+	ptr += sizeof(bmap_len);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv dbglog value 0x%08x\n", value);
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_pktlog_enable(struct ath10k *ar, u32 filter)
+{
+	struct wmi_tlv_pktlog_enable *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+	void *ptr;
+	size_t len;
+
+	len = sizeof(*tlv) + sizeof(*cmd);
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	ptr = (void *)skb->data;
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_PDEV_PKTLOG_ENABLE_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->filter = __cpu_to_le32(filter);
+
+	ptr += sizeof(*tlv);
+	ptr += sizeof(*cmd);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv pktlog enable filter 0x%08x\n",
+		   filter);
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_pktlog_disable(struct ath10k *ar)
+{
+	struct wmi_tlv_pktlog_disable *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+	void *ptr;
+	size_t len;
+
+	len = sizeof(*tlv) + sizeof(*cmd);
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	ptr = (void *)skb->data;
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_PDEV_PKTLOG_DISABLE_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+
+	ptr += sizeof(*tlv);
+	ptr += sizeof(*cmd);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv pktlog disable\n");
+	return skb;
+}
+
+/****************/
+/* TLV mappings */
+/****************/
+
+static struct wmi_cmd_map wmi_tlv_cmd_map = {
+	.init_cmdid = WMI_TLV_INIT_CMDID,
+	.start_scan_cmdid = WMI_TLV_START_SCAN_CMDID,
+	.stop_scan_cmdid = WMI_TLV_STOP_SCAN_CMDID,
+	.scan_chan_list_cmdid = WMI_TLV_SCAN_CHAN_LIST_CMDID,
+	.scan_sch_prio_tbl_cmdid = WMI_TLV_SCAN_SCH_PRIO_TBL_CMDID,
+	.pdev_set_regdomain_cmdid = WMI_TLV_PDEV_SET_REGDOMAIN_CMDID,
+	.pdev_set_channel_cmdid = WMI_TLV_PDEV_SET_CHANNEL_CMDID,
+	.pdev_set_param_cmdid = WMI_TLV_PDEV_SET_PARAM_CMDID,
+	.pdev_pktlog_enable_cmdid = WMI_TLV_PDEV_PKTLOG_ENABLE_CMDID,
+	.pdev_pktlog_disable_cmdid = WMI_TLV_PDEV_PKTLOG_DISABLE_CMDID,
+	.pdev_set_wmm_params_cmdid = WMI_TLV_PDEV_SET_WMM_PARAMS_CMDID,
+	.pdev_set_ht_cap_ie_cmdid = WMI_TLV_PDEV_SET_HT_CAP_IE_CMDID,
+	.pdev_set_vht_cap_ie_cmdid = WMI_TLV_PDEV_SET_VHT_CAP_IE_CMDID,
+	.pdev_set_dscp_tid_map_cmdid = WMI_TLV_PDEV_SET_DSCP_TID_MAP_CMDID,
+	.pdev_set_quiet_mode_cmdid = WMI_TLV_PDEV_SET_QUIET_MODE_CMDID,
+	.pdev_green_ap_ps_enable_cmdid = WMI_TLV_PDEV_GREEN_AP_PS_ENABLE_CMDID,
+	.pdev_get_tpc_config_cmdid = WMI_TLV_PDEV_GET_TPC_CONFIG_CMDID,
+	.pdev_set_base_macaddr_cmdid = WMI_TLV_PDEV_SET_BASE_MACADDR_CMDID,
+	.vdev_create_cmdid = WMI_TLV_VDEV_CREATE_CMDID,
+	.vdev_delete_cmdid = WMI_TLV_VDEV_DELETE_CMDID,
+	.vdev_start_request_cmdid = WMI_TLV_VDEV_START_REQUEST_CMDID,
+	.vdev_restart_request_cmdid = WMI_TLV_VDEV_RESTART_REQUEST_CMDID,
+	.vdev_up_cmdid = WMI_TLV_VDEV_UP_CMDID,
+	.vdev_stop_cmdid = WMI_TLV_VDEV_STOP_CMDID,
+	.vdev_down_cmdid = WMI_TLV_VDEV_DOWN_CMDID,
+	.vdev_set_param_cmdid = WMI_TLV_VDEV_SET_PARAM_CMDID,
+	.vdev_install_key_cmdid = WMI_TLV_VDEV_INSTALL_KEY_CMDID,
+	.peer_create_cmdid = WMI_TLV_PEER_CREATE_CMDID,
+	.peer_delete_cmdid = WMI_TLV_PEER_DELETE_CMDID,
+	.peer_flush_tids_cmdid = WMI_TLV_PEER_FLUSH_TIDS_CMDID,
+	.peer_set_param_cmdid = WMI_TLV_PEER_SET_PARAM_CMDID,
+	.peer_assoc_cmdid = WMI_TLV_PEER_ASSOC_CMDID,
+	.peer_add_wds_entry_cmdid = WMI_TLV_PEER_ADD_WDS_ENTRY_CMDID,
+	.peer_remove_wds_entry_cmdid = WMI_TLV_PEER_REMOVE_WDS_ENTRY_CMDID,
+	.peer_mcast_group_cmdid = WMI_TLV_PEER_MCAST_GROUP_CMDID,
+	.bcn_tx_cmdid = WMI_TLV_BCN_TX_CMDID,
+	.pdev_send_bcn_cmdid = WMI_TLV_PDEV_SEND_BCN_CMDID,
+	.bcn_tmpl_cmdid = WMI_TLV_BCN_TMPL_CMDID,
+	.bcn_filter_rx_cmdid = WMI_TLV_BCN_FILTER_RX_CMDID,
+	.prb_req_filter_rx_cmdid = WMI_TLV_PRB_REQ_FILTER_RX_CMDID,
+	.mgmt_tx_cmdid = WMI_TLV_MGMT_TX_CMDID,
+	.prb_tmpl_cmdid = WMI_TLV_PRB_TMPL_CMDID,
+	.addba_clear_resp_cmdid = WMI_TLV_ADDBA_CLEAR_RESP_CMDID,
+	.addba_send_cmdid = WMI_TLV_ADDBA_SEND_CMDID,
+	.addba_status_cmdid = WMI_TLV_ADDBA_STATUS_CMDID,
+	.delba_send_cmdid = WMI_TLV_DELBA_SEND_CMDID,
+	.addba_set_resp_cmdid = WMI_TLV_ADDBA_SET_RESP_CMDID,
+	.send_singleamsdu_cmdid = WMI_TLV_SEND_SINGLEAMSDU_CMDID,
+	.sta_powersave_mode_cmdid = WMI_TLV_STA_POWERSAVE_MODE_CMDID,
+	.sta_powersave_param_cmdid = WMI_TLV_STA_POWERSAVE_PARAM_CMDID,
+	.sta_mimo_ps_mode_cmdid = WMI_TLV_STA_MIMO_PS_MODE_CMDID,
+	.pdev_dfs_enable_cmdid = WMI_TLV_PDEV_DFS_ENABLE_CMDID,
+	.pdev_dfs_disable_cmdid = WMI_TLV_PDEV_DFS_DISABLE_CMDID,
+	.roam_scan_mode = WMI_TLV_ROAM_SCAN_MODE,
+	.roam_scan_rssi_threshold = WMI_TLV_ROAM_SCAN_RSSI_THRESHOLD,
+	.roam_scan_period = WMI_TLV_ROAM_SCAN_PERIOD,
+	.roam_scan_rssi_change_threshold =
+				WMI_TLV_ROAM_SCAN_RSSI_CHANGE_THRESHOLD,
+	.roam_ap_profile = WMI_TLV_ROAM_AP_PROFILE,
+	.ofl_scan_add_ap_profile = WMI_TLV_ROAM_AP_PROFILE,
+	.ofl_scan_remove_ap_profile = WMI_TLV_OFL_SCAN_REMOVE_AP_PROFILE,
+	.ofl_scan_period = WMI_TLV_OFL_SCAN_PERIOD,
+	.p2p_dev_set_device_info = WMI_TLV_P2P_DEV_SET_DEVICE_INFO,
+	.p2p_dev_set_discoverability = WMI_TLV_P2P_DEV_SET_DISCOVERABILITY,
+	.p2p_go_set_beacon_ie = WMI_TLV_P2P_GO_SET_BEACON_IE,
+	.p2p_go_set_probe_resp_ie = WMI_TLV_P2P_GO_SET_PROBE_RESP_IE,
+	.p2p_set_vendor_ie_data_cmdid = WMI_TLV_P2P_SET_VENDOR_IE_DATA_CMDID,
+	.ap_ps_peer_param_cmdid = WMI_TLV_AP_PS_PEER_PARAM_CMDID,
+	.ap_ps_peer_uapsd_coex_cmdid = WMI_TLV_AP_PS_PEER_UAPSD_COEX_CMDID,
+	.peer_rate_retry_sched_cmdid = WMI_TLV_PEER_RATE_RETRY_SCHED_CMDID,
+	.wlan_profile_trigger_cmdid = WMI_TLV_WLAN_PROFILE_TRIGGER_CMDID,
+	.wlan_profile_set_hist_intvl_cmdid =
+				WMI_TLV_WLAN_PROFILE_SET_HIST_INTVL_CMDID,
+	.wlan_profile_get_profile_data_cmdid =
+				WMI_TLV_WLAN_PROFILE_GET_PROFILE_DATA_CMDID,
+	.wlan_profile_enable_profile_id_cmdid =
+				WMI_TLV_WLAN_PROFILE_ENABLE_PROFILE_ID_CMDID,
+	.wlan_profile_list_profile_id_cmdid =
+				WMI_TLV_WLAN_PROFILE_LIST_PROFILE_ID_CMDID,
+	.pdev_suspend_cmdid = WMI_TLV_PDEV_SUSPEND_CMDID,
+	.pdev_resume_cmdid = WMI_TLV_PDEV_RESUME_CMDID,
+	.add_bcn_filter_cmdid = WMI_TLV_ADD_BCN_FILTER_CMDID,
+	.rmv_bcn_filter_cmdid = WMI_TLV_RMV_BCN_FILTER_CMDID,
+	.wow_add_wake_pattern_cmdid = WMI_TLV_WOW_ADD_WAKE_PATTERN_CMDID,
+	.wow_del_wake_pattern_cmdid = WMI_TLV_WOW_DEL_WAKE_PATTERN_CMDID,
+	.wow_enable_disable_wake_event_cmdid =
+				WMI_TLV_WOW_ENABLE_DISABLE_WAKE_EVENT_CMDID,
+	.wow_enable_cmdid = WMI_TLV_WOW_ENABLE_CMDID,
+	.wow_hostwakeup_from_sleep_cmdid =
+				WMI_TLV_WOW_HOSTWAKEUP_FROM_SLEEP_CMDID,
+	.rtt_measreq_cmdid = WMI_TLV_RTT_MEASREQ_CMDID,
+	.rtt_tsf_cmdid = WMI_TLV_RTT_TSF_CMDID,
+	.vdev_spectral_scan_configure_cmdid = WMI_TLV_SPECTRAL_SCAN_CONF_CMDID,
+	.vdev_spectral_scan_enable_cmdid = WMI_TLV_SPECTRAL_SCAN_ENABLE_CMDID,
+	.request_stats_cmdid = WMI_TLV_REQUEST_STATS_CMDID,
+	.set_arp_ns_offload_cmdid = WMI_TLV_SET_ARP_NS_OFFLOAD_CMDID,
+	.network_list_offload_config_cmdid =
+				WMI_TLV_NETWORK_LIST_OFFLOAD_CONFIG_CMDID,
+	.gtk_offload_cmdid = WMI_TLV_GTK_OFFLOAD_CMDID,
+	.csa_offload_enable_cmdid = WMI_TLV_CSA_OFFLOAD_ENABLE_CMDID,
+	.csa_offload_chanswitch_cmdid = WMI_TLV_CSA_OFFLOAD_CHANSWITCH_CMDID,
+	.chatter_set_mode_cmdid = WMI_TLV_CHATTER_SET_MODE_CMDID,
+	.peer_tid_addba_cmdid = WMI_TLV_PEER_TID_ADDBA_CMDID,
+	.peer_tid_delba_cmdid = WMI_TLV_PEER_TID_DELBA_CMDID,
+	.sta_dtim_ps_method_cmdid = WMI_TLV_STA_DTIM_PS_METHOD_CMDID,
+	.sta_uapsd_auto_trig_cmdid = WMI_TLV_STA_UAPSD_AUTO_TRIG_CMDID,
+	.sta_keepalive_cmd = WMI_TLV_STA_KEEPALIVE_CMDID,
+	.echo_cmdid = WMI_TLV_ECHO_CMDID,
+	.pdev_utf_cmdid = WMI_TLV_PDEV_UTF_CMDID,
+	.dbglog_cfg_cmdid = WMI_TLV_DBGLOG_CFG_CMDID,
+	.pdev_qvit_cmdid = WMI_TLV_PDEV_QVIT_CMDID,
+	.pdev_ftm_intg_cmdid = WMI_TLV_PDEV_FTM_INTG_CMDID,
+	.vdev_set_keepalive_cmdid = WMI_TLV_VDEV_SET_KEEPALIVE_CMDID,
+	.vdev_get_keepalive_cmdid = WMI_TLV_VDEV_GET_KEEPALIVE_CMDID,
+	.force_fw_hang_cmdid = WMI_TLV_FORCE_FW_HANG_CMDID,
+	.gpio_config_cmdid = WMI_TLV_GPIO_CONFIG_CMDID,
+	.gpio_output_cmdid = WMI_TLV_GPIO_OUTPUT_CMDID,
+	.pdev_get_temperature_cmdid = WMI_TLV_CMD_UNSUPPORTED,
+};
+
+static struct wmi_pdev_param_map wmi_tlv_pdev_param_map = {
+	.tx_chain_mask = WMI_TLV_PDEV_PARAM_TX_CHAIN_MASK,
+	.rx_chain_mask = WMI_TLV_PDEV_PARAM_RX_CHAIN_MASK,
+	.txpower_limit2g = WMI_TLV_PDEV_PARAM_TXPOWER_LIMIT2G,
+	.txpower_limit5g = WMI_TLV_PDEV_PARAM_TXPOWER_LIMIT5G,
+	.txpower_scale = WMI_TLV_PDEV_PARAM_TXPOWER_SCALE,
+	.beacon_gen_mode = WMI_TLV_PDEV_PARAM_BEACON_GEN_MODE,
+	.beacon_tx_mode = WMI_TLV_PDEV_PARAM_BEACON_TX_MODE,
+	.resmgr_offchan_mode = WMI_TLV_PDEV_PARAM_RESMGR_OFFCHAN_MODE,
+	.protection_mode = WMI_TLV_PDEV_PARAM_PROTECTION_MODE,
+	.dynamic_bw = WMI_TLV_PDEV_PARAM_DYNAMIC_BW,
+	.non_agg_sw_retry_th = WMI_TLV_PDEV_PARAM_NON_AGG_SW_RETRY_TH,
+	.agg_sw_retry_th = WMI_TLV_PDEV_PARAM_AGG_SW_RETRY_TH,
+	.sta_kickout_th = WMI_TLV_PDEV_PARAM_STA_KICKOUT_TH,
+	.ac_aggrsize_scaling = WMI_TLV_PDEV_PARAM_AC_AGGRSIZE_SCALING,
+	.ltr_enable = WMI_TLV_PDEV_PARAM_LTR_ENABLE,
+	.ltr_ac_latency_be = WMI_TLV_PDEV_PARAM_LTR_AC_LATENCY_BE,
+	.ltr_ac_latency_bk = WMI_TLV_PDEV_PARAM_LTR_AC_LATENCY_BK,
+	.ltr_ac_latency_vi = WMI_TLV_PDEV_PARAM_LTR_AC_LATENCY_VI,
+	.ltr_ac_latency_vo = WMI_TLV_PDEV_PARAM_LTR_AC_LATENCY_VO,
+	.ltr_ac_latency_timeout = WMI_TLV_PDEV_PARAM_LTR_AC_LATENCY_TIMEOUT,
+	.ltr_sleep_override = WMI_TLV_PDEV_PARAM_LTR_SLEEP_OVERRIDE,
+	.ltr_rx_override = WMI_TLV_PDEV_PARAM_LTR_RX_OVERRIDE,
+	.ltr_tx_activity_timeout = WMI_TLV_PDEV_PARAM_LTR_TX_ACTIVITY_TIMEOUT,
+	.l1ss_enable = WMI_TLV_PDEV_PARAM_L1SS_ENABLE,
+	.dsleep_enable = WMI_TLV_PDEV_PARAM_DSLEEP_ENABLE,
+	.pcielp_txbuf_flush = WMI_TLV_PDEV_PARAM_PCIELP_TXBUF_FLUSH,
+	.pcielp_txbuf_watermark = WMI_TLV_PDEV_PARAM_PCIELP_TXBUF_TMO_EN,
+	.pcielp_txbuf_tmo_en = WMI_TLV_PDEV_PARAM_PCIELP_TXBUF_TMO_EN,
+	.pcielp_txbuf_tmo_value = WMI_TLV_PDEV_PARAM_PCIELP_TXBUF_TMO_VALUE,
+	.pdev_stats_update_period = WMI_TLV_PDEV_PARAM_PDEV_STATS_UPDATE_PERIOD,
+	.vdev_stats_update_period = WMI_TLV_PDEV_PARAM_VDEV_STATS_UPDATE_PERIOD,
+	.peer_stats_update_period = WMI_TLV_PDEV_PARAM_PEER_STATS_UPDATE_PERIOD,
+	.bcnflt_stats_update_period =
+				WMI_TLV_PDEV_PARAM_BCNFLT_STATS_UPDATE_PERIOD,
+	.pmf_qos = WMI_TLV_PDEV_PARAM_PMF_QOS,
+	.arp_ac_override = WMI_TLV_PDEV_PARAM_ARP_AC_OVERRIDE,
+	.dcs = WMI_TLV_PDEV_PARAM_DCS,
+	.ani_enable = WMI_TLV_PDEV_PARAM_ANI_ENABLE,
+	.ani_poll_period = WMI_TLV_PDEV_PARAM_ANI_POLL_PERIOD,
+	.ani_listen_period = WMI_TLV_PDEV_PARAM_ANI_LISTEN_PERIOD,
+	.ani_ofdm_level = WMI_TLV_PDEV_PARAM_ANI_OFDM_LEVEL,
+	.ani_cck_level = WMI_TLV_PDEV_PARAM_ANI_CCK_LEVEL,
+	.dyntxchain = WMI_TLV_PDEV_PARAM_DYNTXCHAIN,
+	.proxy_sta = WMI_TLV_PDEV_PARAM_PROXY_STA,
+	.idle_ps_config = WMI_TLV_PDEV_PARAM_IDLE_PS_CONFIG,
+	.power_gating_sleep = WMI_TLV_PDEV_PARAM_POWER_GATING_SLEEP,
+	.fast_channel_reset = WMI_TLV_PDEV_PARAM_UNSUPPORTED,
+	.burst_dur = WMI_TLV_PDEV_PARAM_BURST_DUR,
+	.burst_enable = WMI_TLV_PDEV_PARAM_BURST_ENABLE,
+	.cal_period = WMI_PDEV_PARAM_UNSUPPORTED,
+};
+
+static struct wmi_vdev_param_map wmi_tlv_vdev_param_map = {
+	.rts_threshold = WMI_TLV_VDEV_PARAM_RTS_THRESHOLD,
+	.fragmentation_threshold = WMI_TLV_VDEV_PARAM_FRAGMENTATION_THRESHOLD,
+	.beacon_interval = WMI_TLV_VDEV_PARAM_BEACON_INTERVAL,
+	.listen_interval = WMI_TLV_VDEV_PARAM_LISTEN_INTERVAL,
+	.multicast_rate = WMI_TLV_VDEV_PARAM_MULTICAST_RATE,
+	.mgmt_tx_rate = WMI_TLV_VDEV_PARAM_MGMT_TX_RATE,
+	.slot_time = WMI_TLV_VDEV_PARAM_SLOT_TIME,
+	.preamble = WMI_TLV_VDEV_PARAM_PREAMBLE,
+	.swba_time = WMI_TLV_VDEV_PARAM_SWBA_TIME,
+	.wmi_vdev_stats_update_period = WMI_TLV_VDEV_STATS_UPDATE_PERIOD,
+	.wmi_vdev_pwrsave_ageout_time = WMI_TLV_VDEV_PWRSAVE_AGEOUT_TIME,
+	.wmi_vdev_host_swba_interval = WMI_TLV_VDEV_HOST_SWBA_INTERVAL,
+	.dtim_period = WMI_TLV_VDEV_PARAM_DTIM_PERIOD,
+	.wmi_vdev_oc_scheduler_air_time_limit =
+				WMI_TLV_VDEV_OC_SCHEDULER_AIR_TIME_LIMIT,
+	.wds = WMI_TLV_VDEV_PARAM_WDS,
+	.atim_window = WMI_TLV_VDEV_PARAM_ATIM_WINDOW,
+	.bmiss_count_max = WMI_TLV_VDEV_PARAM_BMISS_COUNT_MAX,
+	.bmiss_first_bcnt = WMI_TLV_VDEV_PARAM_BMISS_FIRST_BCNT,
+	.bmiss_final_bcnt = WMI_TLV_VDEV_PARAM_BMISS_FINAL_BCNT,
+	.feature_wmm = WMI_TLV_VDEV_PARAM_FEATURE_WMM,
+	.chwidth = WMI_TLV_VDEV_PARAM_CHWIDTH,
+	.chextoffset = WMI_TLV_VDEV_PARAM_CHEXTOFFSET,
+	.disable_htprotection =	WMI_TLV_VDEV_PARAM_DISABLE_HTPROTECTION,
+	.sta_quickkickout = WMI_TLV_VDEV_PARAM_STA_QUICKKICKOUT,
+	.mgmt_rate = WMI_TLV_VDEV_PARAM_MGMT_RATE,
+	.protection_mode = WMI_TLV_VDEV_PARAM_PROTECTION_MODE,
+	.fixed_rate = WMI_TLV_VDEV_PARAM_FIXED_RATE,
+	.sgi = WMI_TLV_VDEV_PARAM_SGI,
+	.ldpc = WMI_TLV_VDEV_PARAM_LDPC,
+	.tx_stbc = WMI_TLV_VDEV_PARAM_TX_STBC,
+	.rx_stbc = WMI_TLV_VDEV_PARAM_RX_STBC,
+	.intra_bss_fwd = WMI_TLV_VDEV_PARAM_INTRA_BSS_FWD,
+	.def_keyid = WMI_TLV_VDEV_PARAM_DEF_KEYID,
+	.nss = WMI_TLV_VDEV_PARAM_NSS,
+	.bcast_data_rate = WMI_TLV_VDEV_PARAM_BCAST_DATA_RATE,
+	.mcast_data_rate = WMI_TLV_VDEV_PARAM_MCAST_DATA_RATE,
+	.mcast_indicate = WMI_TLV_VDEV_PARAM_MCAST_INDICATE,
+	.dhcp_indicate = WMI_TLV_VDEV_PARAM_DHCP_INDICATE,
+	.unknown_dest_indicate = WMI_TLV_VDEV_PARAM_UNKNOWN_DEST_INDICATE,
+	.ap_keepalive_min_idle_inactive_time_secs =
+		WMI_TLV_VDEV_PARAM_AP_KEEPALIVE_MIN_IDLE_INACTIVE_TIME_SECS,
+	.ap_keepalive_max_idle_inactive_time_secs =
+		WMI_TLV_VDEV_PARAM_AP_KEEPALIVE_MAX_IDLE_INACTIVE_TIME_SECS,
+	.ap_keepalive_max_unresponsive_time_secs =
+		WMI_TLV_VDEV_PARAM_AP_KEEPALIVE_MAX_UNRESPONSIVE_TIME_SECS,
+	.ap_enable_nawds = WMI_TLV_VDEV_PARAM_AP_ENABLE_NAWDS,
+	.mcast2ucast_set = WMI_TLV_VDEV_PARAM_UNSUPPORTED,
+	.enable_rtscts = WMI_TLV_VDEV_PARAM_ENABLE_RTSCTS,
+	.txbf = WMI_TLV_VDEV_PARAM_TXBF,
+	.packet_powersave = WMI_TLV_VDEV_PARAM_PACKET_POWERSAVE,
+	.drop_unencry = WMI_TLV_VDEV_PARAM_DROP_UNENCRY,
+	.tx_encap_type = WMI_TLV_VDEV_PARAM_TX_ENCAP_TYPE,
+	.ap_detect_out_of_sync_sleeping_sta_time_secs =
+					WMI_TLV_VDEV_PARAM_UNSUPPORTED,
+};
+
+static const struct wmi_ops wmi_tlv_ops = {
+	.rx = ath10k_wmi_tlv_op_rx,
+	.map_svc = wmi_tlv_svc_map,
+
+	.pull_scan = ath10k_wmi_tlv_op_pull_scan_ev,
+	.pull_mgmt_rx = ath10k_wmi_tlv_op_pull_mgmt_rx_ev,
+	.pull_ch_info = ath10k_wmi_tlv_op_pull_ch_info_ev,
+	.pull_vdev_start = ath10k_wmi_tlv_op_pull_vdev_start_ev,
+	.pull_peer_kick = ath10k_wmi_tlv_op_pull_peer_kick_ev,
+	.pull_swba = ath10k_wmi_tlv_op_pull_swba_ev,
+	.pull_phyerr = ath10k_wmi_tlv_op_pull_phyerr_ev,
+	.pull_svc_rdy = ath10k_wmi_tlv_op_pull_svc_rdy_ev,
+	.pull_rdy = ath10k_wmi_tlv_op_pull_rdy_ev,
+	.pull_fw_stats = ath10k_wmi_tlv_op_pull_fw_stats,
+
+	.gen_pdev_suspend = ath10k_wmi_tlv_op_gen_pdev_suspend,
+	.gen_pdev_resume = ath10k_wmi_tlv_op_gen_pdev_resume,
+	.gen_pdev_set_rd = ath10k_wmi_tlv_op_gen_pdev_set_rd,
+	.gen_pdev_set_param = ath10k_wmi_tlv_op_gen_pdev_set_param,
+	.gen_init = ath10k_wmi_tlv_op_gen_init,
+	.gen_start_scan = ath10k_wmi_tlv_op_gen_start_scan,
+	.gen_stop_scan = ath10k_wmi_tlv_op_gen_stop_scan,
+	.gen_vdev_create = ath10k_wmi_tlv_op_gen_vdev_create,
+	.gen_vdev_delete = ath10k_wmi_tlv_op_gen_vdev_delete,
+	.gen_vdev_start = ath10k_wmi_tlv_op_gen_vdev_start,
+	.gen_vdev_stop = ath10k_wmi_tlv_op_gen_vdev_stop,
+	.gen_vdev_up = ath10k_wmi_tlv_op_gen_vdev_up,
+	.gen_vdev_down = ath10k_wmi_tlv_op_gen_vdev_down,
+	.gen_vdev_set_param = ath10k_wmi_tlv_op_gen_vdev_set_param,
+	.gen_vdev_install_key = ath10k_wmi_tlv_op_gen_vdev_install_key,
+	.gen_peer_create = ath10k_wmi_tlv_op_gen_peer_create,
+	.gen_peer_delete = ath10k_wmi_tlv_op_gen_peer_delete,
+	.gen_peer_flush = ath10k_wmi_tlv_op_gen_peer_flush,
+	.gen_peer_set_param = ath10k_wmi_tlv_op_gen_peer_set_param,
+	.gen_peer_assoc = ath10k_wmi_tlv_op_gen_peer_assoc,
+	.gen_set_psmode = ath10k_wmi_tlv_op_gen_set_psmode,
+	.gen_set_sta_ps = ath10k_wmi_tlv_op_gen_set_sta_ps,
+	.gen_set_ap_ps = ath10k_wmi_tlv_op_gen_set_ap_ps,
+	.gen_scan_chan_list = ath10k_wmi_tlv_op_gen_scan_chan_list,
+	.gen_beacon_dma = ath10k_wmi_tlv_op_gen_beacon_dma,
+	.gen_pdev_set_wmm = ath10k_wmi_tlv_op_gen_pdev_set_wmm,
+	.gen_request_stats = ath10k_wmi_tlv_op_gen_request_stats,
+	.gen_force_fw_hang = ath10k_wmi_tlv_op_gen_force_fw_hang,
+	/* .gen_mgmt_tx = not implemented; HTT is used */
+	.gen_dbglog_cfg = ath10k_wmi_tlv_op_gen_dbglog_cfg,
+	.gen_pktlog_enable = ath10k_wmi_tlv_op_gen_pktlog_enable,
+	.gen_pktlog_disable = ath10k_wmi_tlv_op_gen_pktlog_disable,
+	/* .gen_pdev_set_quiet_mode not implemented */
+	/* .gen_pdev_get_temperature not implemented */
+};
+
+/************/
+/* TLV init */
+/************/
+
+void ath10k_wmi_tlv_attach(struct ath10k *ar)
+{
+	ar->wmi.cmd = &wmi_tlv_cmd_map;
+	ar->wmi.vdev_param = &wmi_tlv_vdev_param_map;
+	ar->wmi.pdev_param = &wmi_tlv_pdev_param_map;
+	ar->wmi.ops = &wmi_tlv_ops;
+}
diff --git a/drivers/net/wireless/ath/ath10k/wmi-tlv.h b/drivers/net/wireless/ath/ath10k/wmi-tlv.h
new file mode 100644
index 000000000000..54ffa120cd60
--- /dev/null
+++ b/drivers/net/wireless/ath/ath10k/wmi-tlv.h
@@ -0,0 +1,1380 @@
+/*
+ * Copyright (c) 2005-2011 Atheros Communications Inc.
+ * Copyright (c) 2011-2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#ifndef _WMI_TLV_H
+#define _WMI_TLV_H
+
+#define WMI_TLV_CMD(grp_id) (((grp_id) << 12) | 0x1)
+#define WMI_TLV_EV(grp_id) (((grp_id) << 12) | 0x1)
+#define WMI_TLV_CMD_UNSUPPORTED 0
+#define WMI_TLV_PDEV_PARAM_UNSUPPORTED 0
+#define WMI_TLV_VDEV_PARAM_UNSUPPORTED 0
+
+enum wmi_tlv_grp_id {
+	WMI_TLV_GRP_START = 0x3,
+	WMI_TLV_GRP_SCAN = WMI_TLV_GRP_START,
+	WMI_TLV_GRP_PDEV,
+	WMI_TLV_GRP_VDEV,
+	WMI_TLV_GRP_PEER,
+	WMI_TLV_GRP_MGMT,
+	WMI_TLV_GRP_BA_NEG,
+	WMI_TLV_GRP_STA_PS,
+	WMI_TLV_GRP_DFS,
+	WMI_TLV_GRP_ROAM,
+	WMI_TLV_GRP_OFL_SCAN,
+	WMI_TLV_GRP_P2P,
+	WMI_TLV_GRP_AP_PS,
+	WMI_TLV_GRP_RATECTL,
+	WMI_TLV_GRP_PROFILE,
+	WMI_TLV_GRP_SUSPEND,
+	WMI_TLV_GRP_BCN_FILTER,
+	WMI_TLV_GRP_WOW,
+	WMI_TLV_GRP_RTT,
+	WMI_TLV_GRP_SPECTRAL,
+	WMI_TLV_GRP_STATS,
+	WMI_TLV_GRP_ARP_NS_OFL,
+	WMI_TLV_GRP_NLO_OFL,
+	WMI_TLV_GRP_GTK_OFL,
+	WMI_TLV_GRP_CSA_OFL,
+	WMI_TLV_GRP_CHATTER,
+	WMI_TLV_GRP_TID_ADDBA,
+	WMI_TLV_GRP_MISC,
+	WMI_TLV_GRP_GPIO,
+	WMI_TLV_GRP_FWTEST,
+	WMI_TLV_GRP_TDLS,
+	WMI_TLV_GRP_RESMGR,
+	WMI_TLV_GRP_STA_SMPS,
+	WMI_TLV_GRP_WLAN_HB,
+	WMI_TLV_GRP_RMC,
+	WMI_TLV_GRP_MHF_OFL,
+	WMI_TLV_GRP_LOCATION_SCAN,
+	WMI_TLV_GRP_OEM,
+	WMI_TLV_GRP_NAN,
+	WMI_TLV_GRP_COEX,
+	WMI_TLV_GRP_OBSS_OFL,
+	WMI_TLV_GRP_LPI,
+	WMI_TLV_GRP_EXTSCAN,
+	WMI_TLV_GRP_DHCP_OFL,
+	WMI_TLV_GRP_IPA,
+	WMI_TLV_GRP_MDNS_OFL,
+	WMI_TLV_GRP_SAP_OFL,
+};
+
+enum wmi_tlv_cmd_id {
+	WMI_TLV_INIT_CMDID = 0x1,
+	WMI_TLV_START_SCAN_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_SCAN),
+	WMI_TLV_STOP_SCAN_CMDID,
+	WMI_TLV_SCAN_CHAN_LIST_CMDID,
+	WMI_TLV_SCAN_SCH_PRIO_TBL_CMDID,
+	WMI_TLV_SCAN_UPDATE_REQUEST_CMDID,
+	WMI_TLV_SCAN_PROB_REQ_OUI_CMDID,
+	WMI_TLV_PDEV_SET_REGDOMAIN_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_PDEV),
+	WMI_TLV_PDEV_SET_CHANNEL_CMDID,
+	WMI_TLV_PDEV_SET_PARAM_CMDID,
+	WMI_TLV_PDEV_PKTLOG_ENABLE_CMDID,
+	WMI_TLV_PDEV_PKTLOG_DISABLE_CMDID,
+	WMI_TLV_PDEV_SET_WMM_PARAMS_CMDID,
+	WMI_TLV_PDEV_SET_HT_CAP_IE_CMDID,
+	WMI_TLV_PDEV_SET_VHT_CAP_IE_CMDID,
+	WMI_TLV_PDEV_SET_DSCP_TID_MAP_CMDID,
+	WMI_TLV_PDEV_SET_QUIET_MODE_CMDID,
+	WMI_TLV_PDEV_GREEN_AP_PS_ENABLE_CMDID,
+	WMI_TLV_PDEV_GET_TPC_CONFIG_CMDID,
+	WMI_TLV_PDEV_SET_BASE_MACADDR_CMDID,
+	WMI_TLV_PDEV_DUMP_CMDID,
+	WMI_TLV_PDEV_SET_LED_CONFIG_CMDID,
+	WMI_TLV_PDEV_GET_TEMPERATURE_CMDID,
+	WMI_TLV_PDEV_SET_LED_FLASHING_CMDID,
+	WMI_TLV_VDEV_CREATE_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_VDEV),
+	WMI_TLV_VDEV_DELETE_CMDID,
+	WMI_TLV_VDEV_START_REQUEST_CMDID,
+	WMI_TLV_VDEV_RESTART_REQUEST_CMDID,
+	WMI_TLV_VDEV_UP_CMDID,
+	WMI_TLV_VDEV_STOP_CMDID,
+	WMI_TLV_VDEV_DOWN_CMDID,
+	WMI_TLV_VDEV_SET_PARAM_CMDID,
+	WMI_TLV_VDEV_INSTALL_KEY_CMDID,
+	WMI_TLV_VDEV_WNM_SLEEPMODE_CMDID,
+	WMI_TLV_VDEV_WMM_ADDTS_CMDID,
+	WMI_TLV_VDEV_WMM_DELTS_CMDID,
+	WMI_TLV_VDEV_SET_WMM_PARAMS_CMDID,
+	WMI_TLV_VDEV_SET_GTX_PARAMS_CMDID,
+	WMI_TLV_VDEV_IPSEC_NATKEEPALIVE_FILTER_CMDID,
+	WMI_TLV_VDEV_PLMREQ_START_CMDID,
+	WMI_TLV_VDEV_PLMREQ_STOP_CMDID,
+	WMI_TLV_PEER_CREATE_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_PEER),
+	WMI_TLV_PEER_DELETE_CMDID,
+	WMI_TLV_PEER_FLUSH_TIDS_CMDID,
+	WMI_TLV_PEER_SET_PARAM_CMDID,
+	WMI_TLV_PEER_ASSOC_CMDID,
+	WMI_TLV_PEER_ADD_WDS_ENTRY_CMDID,
+	WMI_TLV_PEER_REMOVE_WDS_ENTRY_CMDID,
+	WMI_TLV_PEER_MCAST_GROUP_CMDID,
+	WMI_TLV_PEER_INFO_REQ_CMDID,
+	WMI_TLV_PEER_GET_ESTIMATED_LINKSPEED_CMDID,
+	WMI_TLV_BCN_TX_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_MGMT),
+	WMI_TLV_PDEV_SEND_BCN_CMDID,
+	WMI_TLV_BCN_TMPL_CMDID,
+	WMI_TLV_BCN_FILTER_RX_CMDID,
+	WMI_TLV_PRB_REQ_FILTER_RX_CMDID,
+	WMI_TLV_MGMT_TX_CMDID,
+	WMI_TLV_PRB_TMPL_CMDID,
+	WMI_TLV_ADDBA_CLEAR_RESP_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_BA_NEG),
+	WMI_TLV_ADDBA_SEND_CMDID,
+	WMI_TLV_ADDBA_STATUS_CMDID,
+	WMI_TLV_DELBA_SEND_CMDID,
+	WMI_TLV_ADDBA_SET_RESP_CMDID,
+	WMI_TLV_SEND_SINGLEAMSDU_CMDID,
+	WMI_TLV_STA_POWERSAVE_MODE_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_STA_PS),
+	WMI_TLV_STA_POWERSAVE_PARAM_CMDID,
+	WMI_TLV_STA_MIMO_PS_MODE_CMDID,
+	WMI_TLV_PDEV_DFS_ENABLE_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_DFS),
+	WMI_TLV_PDEV_DFS_DISABLE_CMDID,
+	WMI_TLV_DFS_PHYERR_FILTER_ENA_CMDID,
+	WMI_TLV_DFS_PHYERR_FILTER_DIS_CMDID,
+	WMI_TLV_ROAM_SCAN_MODE = WMI_TLV_CMD(WMI_TLV_GRP_ROAM),
+	WMI_TLV_ROAM_SCAN_RSSI_THRESHOLD,
+	WMI_TLV_ROAM_SCAN_PERIOD,
+	WMI_TLV_ROAM_SCAN_RSSI_CHANGE_THRESHOLD,
+	WMI_TLV_ROAM_AP_PROFILE,
+	WMI_TLV_ROAM_CHAN_LIST,
+	WMI_TLV_ROAM_SCAN_CMD,
+	WMI_TLV_ROAM_SYNCH_COMPLETE,
+	WMI_TLV_ROAM_SET_RIC_REQUEST_CMDID,
+	WMI_TLV_ROAM_INVOKE_CMDID,
+	WMI_TLV_OFL_SCAN_ADD_AP_PROFILE = WMI_TLV_CMD(WMI_TLV_GRP_OFL_SCAN),
+	WMI_TLV_OFL_SCAN_REMOVE_AP_PROFILE,
+	WMI_TLV_OFL_SCAN_PERIOD,
+	WMI_TLV_P2P_DEV_SET_DEVICE_INFO = WMI_TLV_CMD(WMI_TLV_GRP_P2P),
+	WMI_TLV_P2P_DEV_SET_DISCOVERABILITY,
+	WMI_TLV_P2P_GO_SET_BEACON_IE,
+	WMI_TLV_P2P_GO_SET_PROBE_RESP_IE,
+	WMI_TLV_P2P_SET_VENDOR_IE_DATA_CMDID,
+	WMI_TLV_P2P_DISC_OFFLOAD_CONFIG_CMDID,
+	WMI_TLV_P2P_DISC_OFFLOAD_APPIE_CMDID,
+	WMI_TLV_P2P_DISC_OFFLOAD_PATTERN_CMDID,
+	WMI_TLV_P2P_SET_OPPPS_PARAM_CMDID,
+	WMI_TLV_AP_PS_PEER_PARAM_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_AP_PS),
+	WMI_TLV_AP_PS_PEER_UAPSD_COEX_CMDID,
+	WMI_TLV_PEER_RATE_RETRY_SCHED_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_RATECTL),
+	WMI_TLV_WLAN_PROFILE_TRIGGER_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_PROFILE),
+	WMI_TLV_WLAN_PROFILE_SET_HIST_INTVL_CMDID,
+	WMI_TLV_WLAN_PROFILE_GET_PROFILE_DATA_CMDID,
+	WMI_TLV_WLAN_PROFILE_ENABLE_PROFILE_ID_CMDID,
+	WMI_TLV_WLAN_PROFILE_LIST_PROFILE_ID_CMDID,
+	WMI_TLV_PDEV_SUSPEND_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_SUSPEND),
+	WMI_TLV_PDEV_RESUME_CMDID,
+	WMI_TLV_ADD_BCN_FILTER_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_BCN_FILTER),
+	WMI_TLV_RMV_BCN_FILTER_CMDID,
+	WMI_TLV_WOW_ADD_WAKE_PATTERN_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_WOW),
+	WMI_TLV_WOW_DEL_WAKE_PATTERN_CMDID,
+	WMI_TLV_WOW_ENABLE_DISABLE_WAKE_EVENT_CMDID,
+	WMI_TLV_WOW_ENABLE_CMDID,
+	WMI_TLV_WOW_HOSTWAKEUP_FROM_SLEEP_CMDID,
+	WMI_TLV_WOW_ACER_IOAC_ADD_KEEPALIVE_CMDID,
+	WMI_TLV_WOW_ACER_IOAC_DEL_KEEPALIVE_CMDID,
+	WMI_TLV_WOW_ACER_IOAC_ADD_WAKE_PATTERN_CMDID,
+	WMI_TLV_WOW_ACER_IOAC_DEL_WAKE_PATTERN_CMDID,
+	WMI_TLV_D0_WOW_ENABLE_DISABLE_CMDID,
+	WMI_TLV_EXTWOW_ENABLE_CMDID,
+	WMI_TLV_EXTWOW_SET_APP_TYPE1_PARAMS_CMDID,
+	WMI_TLV_EXTWOW_SET_APP_TYPE2_PARAMS_CMDID,
+	WMI_TLV_RTT_MEASREQ_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_RTT),
+	WMI_TLV_RTT_TSF_CMDID,
+	WMI_TLV_SPECTRAL_SCAN_CONF_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_SPECTRAL),
+	WMI_TLV_SPECTRAL_SCAN_ENABLE_CMDID,
+	WMI_TLV_REQUEST_STATS_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_STATS),
+	WMI_TLV_MCC_SCHED_TRAFFIC_STATS_CMDID,
+	WMI_TLV_REQUEST_STATS_EXT_CMDID,
+	WMI_TLV_REQUEST_LINK_STATS_CMDID,
+	WMI_TLV_START_LINK_STATS_CMDID,
+	WMI_TLV_CLEAR_LINK_STATS_CMDID,
+	WMI_TLV_SET_ARP_NS_OFFLOAD_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_ARP_NS_OFL),
+	WMI_TLV_ADD_PROACTIVE_ARP_RSP_PATTERN_CMDID,
+	WMI_TLV_DEL_PROACTIVE_ARP_RSP_PATTERN_CMDID,
+	WMI_TLV_NETWORK_LIST_OFFLOAD_CONFIG_CMDID =
+			WMI_TLV_CMD(WMI_TLV_GRP_NLO_OFL),
+	WMI_TLV_APFIND_CMDID,
+	WMI_TLV_GTK_OFFLOAD_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_GTK_OFL),
+	WMI_TLV_CSA_OFFLOAD_ENABLE_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_CSA_OFL),
+	WMI_TLV_CSA_OFFLOAD_CHANSWITCH_CMDID,
+	WMI_TLV_CHATTER_SET_MODE_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_CHATTER),
+	WMI_TLV_CHATTER_ADD_COALESCING_FILTER_CMDID,
+	WMI_TLV_CHATTER_DELETE_COALESCING_FILTER_CMDID,
+	WMI_TLV_CHATTER_COALESCING_QUERY_CMDID,
+	WMI_TLV_PEER_TID_ADDBA_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_TID_ADDBA),
+	WMI_TLV_PEER_TID_DELBA_CMDID,
+	WMI_TLV_STA_DTIM_PS_METHOD_CMDID,
+	WMI_TLV_STA_UAPSD_AUTO_TRIG_CMDID,
+	WMI_TLV_STA_KEEPALIVE_CMDID,
+	WMI_TLV_BA_REQ_SSN_CMDID,
+	WMI_TLV_ECHO_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_MISC),
+	WMI_TLV_PDEV_UTF_CMDID,
+	WMI_TLV_DBGLOG_CFG_CMDID,
+	WMI_TLV_PDEV_QVIT_CMDID,
+	WMI_TLV_PDEV_FTM_INTG_CMDID,
+	WMI_TLV_VDEV_SET_KEEPALIVE_CMDID,
+	WMI_TLV_VDEV_GET_KEEPALIVE_CMDID,
+	WMI_TLV_FORCE_FW_HANG_CMDID,
+	WMI_TLV_SET_MCASTBCAST_FILTER_CMDID,
+	WMI_TLV_THERMAL_MGMT_CMDID,
+	WMI_TLV_HOST_AUTO_SHUTDOWN_CFG_CMDID,
+	WMI_TLV_TPC_CHAINMASK_CONFIG_CMDID,
+	WMI_TLV_SET_ANTENNA_DIVERSITY_CMDID,
+	WMI_TLV_GPIO_CONFIG_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_GPIO),
+	WMI_TLV_GPIO_OUTPUT_CMDID,
+	WMI_TLV_TXBF_CMDID,
+	WMI_TLV_FWTEST_VDEV_MCC_SET_TBTT_MODE_CMDID =
+			WMI_TLV_CMD(WMI_TLV_GRP_FWTEST),
+	WMI_TLV_FWTEST_P2P_SET_NOA_PARAM_CMDID,
+	WMI_TLV_UNIT_TEST_CMDID,
+	WMI_TLV_TDLS_SET_STATE_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_TDLS),
+	WMI_TLV_TDLS_PEER_UPDATE_CMDID,
+	WMI_TLV_TDLS_SET_OFFCHAN_MODE_CMDID,
+	WMI_TLV_RESMGR_ADAPTIVE_OCS_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_RESMGR),
+	WMI_TLV_RESMGR_SET_CHAN_TIME_QUOTA_CMDID,
+	WMI_TLV_RESMGR_SET_CHAN_LATENCY_CMDID,
+	WMI_TLV_STA_SMPS_FORCE_MODE_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_STA_SMPS),
+	WMI_TLV_STA_SMPS_PARAM_CMDID,
+	WMI_TLV_HB_SET_ENABLE_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_WLAN_HB),
+	WMI_TLV_HB_SET_TCP_PARAMS_CMDID,
+	WMI_TLV_HB_SET_TCP_PKT_FILTER_CMDID,
+	WMI_TLV_HB_SET_UDP_PARAMS_CMDID,
+	WMI_TLV_HB_SET_UDP_PKT_FILTER_CMDID,
+	WMI_TLV_RMC_SET_MODE_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_RMC),
+	WMI_TLV_RMC_SET_ACTION_PERIOD_CMDID,
+	WMI_TLV_RMC_CONFIG_CMDID,
+	WMI_TLV_MHF_OFFLOAD_SET_MODE_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_MHF_OFL),
+	WMI_TLV_MHF_OFFLOAD_PLUMB_ROUTING_TBL_CMDID,
+	WMI_TLV_BATCH_SCAN_ENABLE_CMDID =
+			WMI_TLV_CMD(WMI_TLV_GRP_LOCATION_SCAN),
+	WMI_TLV_BATCH_SCAN_DISABLE_CMDID,
+	WMI_TLV_BATCH_SCAN_TRIGGER_RESULT_CMDID,
+	WMI_TLV_OEM_REQ_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_OEM),
+	WMI_TLV_NAN_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_NAN),
+	WMI_TLV_MODEM_POWER_STATE_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_COEX),
+	WMI_TLV_CHAN_AVOID_UPDATE_CMDID,
+	WMI_TLV_OBSS_SCAN_ENABLE_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_OBSS_OFL),
+	WMI_TLV_OBSS_SCAN_DISABLE_CMDID,
+	WMI_TLV_LPI_MGMT_SNOOPING_CONFIG_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_LPI),
+	WMI_TLV_LPI_START_SCAN_CMDID,
+	WMI_TLV_LPI_STOP_SCAN_CMDID,
+	WMI_TLV_EXTSCAN_START_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_EXTSCAN),
+	WMI_TLV_EXTSCAN_STOP_CMDID,
+	WMI_TLV_EXTSCAN_CONFIGURE_WLAN_CHANGE_MONITOR_CMDID,
+	WMI_TLV_EXTSCAN_CONFIGURE_HOTLIST_MONITOR_CMDID,
+	WMI_TLV_EXTSCAN_GET_CACHED_RESULTS_CMDID,
+	WMI_TLV_EXTSCAN_GET_WLAN_CHANGE_RESULTS_CMDID,
+	WMI_TLV_EXTSCAN_SET_CAPABILITIES_CMDID,
+	WMI_TLV_EXTSCAN_GET_CAPABILITIES_CMDID,
+	WMI_TLV_SET_DHCP_SERVER_OFFLOAD_CMDID =
+			WMI_TLV_CMD(WMI_TLV_GRP_DHCP_OFL),
+	WMI_TLV_IPA_OFFLOAD_ENABLE_DISABLE_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_IPA),
+	WMI_TLV_MDNS_OFFLOAD_ENABLE_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_MDNS_OFL),
+	WMI_TLV_MDNS_SET_FQDN_CMDID,
+	WMI_TLV_MDNS_SET_RESPONSE_CMDID,
+	WMI_TLV_MDNS_GET_STATS_CMDID,
+	WMI_TLV_SAP_OFL_ENABLE_CMDID = WMI_TLV_CMD(WMI_TLV_GRP_SAP_OFL),
+};
+
+enum wmi_tlv_event_id {
+	WMI_TLV_SERVICE_READY_EVENTID = 0x1,
+	WMI_TLV_READY_EVENTID,
+	WMI_TLV_SCAN_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_SCAN),
+	WMI_TLV_PDEV_TPC_CONFIG_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_PDEV),
+	WMI_TLV_CHAN_INFO_EVENTID,
+	WMI_TLV_PHYERR_EVENTID,
+	WMI_TLV_PDEV_DUMP_EVENTID,
+	WMI_TLV_TX_PAUSE_EVENTID,
+	WMI_TLV_DFS_RADAR_EVENTID,
+	WMI_TLV_PDEV_L1SS_TRACK_EVENTID,
+	WMI_TLV_PDEV_TEMPERATURE_EVENTID,
+	WMI_TLV_VDEV_START_RESP_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_VDEV),
+	WMI_TLV_VDEV_STOPPED_EVENTID,
+	WMI_TLV_VDEV_INSTALL_KEY_COMPLETE_EVENTID,
+	WMI_TLV_VDEV_MCC_BCN_INTERVAL_CHANGE_REQ_EVENTID,
+	WMI_TLV_PEER_STA_KICKOUT_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_PEER),
+	WMI_TLV_PEER_INFO_EVENTID,
+	WMI_TLV_PEER_TX_FAIL_CNT_THR_EVENTID,
+	WMI_TLV_PEER_ESTIMATED_LINKSPEED_EVENTID,
+	WMI_TLV_PEER_STATE_EVENTID,
+	WMI_TLV_MGMT_RX_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_MGMT),
+	WMI_TLV_HOST_SWBA_EVENTID,
+	WMI_TLV_TBTTOFFSET_UPDATE_EVENTID,
+	WMI_TLV_OFFLOAD_BCN_TX_STATUS_EVENTID,
+	WMI_TLV_OFFLOAD_PROB_RESP_TX_STATUS_EVENTID,
+	WMI_TLV_TX_DELBA_COMPLETE_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_BA_NEG),
+	WMI_TLV_TX_ADDBA_COMPLETE_EVENTID,
+	WMI_TLV_BA_RSP_SSN_EVENTID,
+	WMI_TLV_AGGR_STATE_TRIG_EVENTID,
+	WMI_TLV_ROAM_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_ROAM),
+	WMI_TLV_PROFILE_MATCH,
+	WMI_TLV_ROAM_SYNCH_EVENTID,
+	WMI_TLV_P2P_DISC_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_P2P),
+	WMI_TLV_P2P_NOA_EVENTID,
+	WMI_TLV_PDEV_RESUME_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_SUSPEND),
+	WMI_TLV_WOW_WAKEUP_HOST_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_WOW),
+	WMI_TLV_D0_WOW_DISABLE_ACK_EVENTID,
+	WMI_TLV_RTT_MEASUREMENT_REPORT_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_RTT),
+	WMI_TLV_TSF_MEASUREMENT_REPORT_EVENTID,
+	WMI_TLV_RTT_ERROR_REPORT_EVENTID,
+	WMI_TLV_STATS_EXT_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_STATS),
+	WMI_TLV_IFACE_LINK_STATS_EVENTID,
+	WMI_TLV_PEER_LINK_STATS_EVENTID,
+	WMI_TLV_RADIO_LINK_STATS_EVENTID,
+	WMI_TLV_NLO_MATCH_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_NLO_OFL),
+	WMI_TLV_NLO_SCAN_COMPLETE_EVENTID,
+	WMI_TLV_APFIND_EVENTID,
+	WMI_TLV_GTK_OFFLOAD_STATUS_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_GTK_OFL),
+	WMI_TLV_GTK_REKEY_FAIL_EVENTID,
+	WMI_TLV_CSA_HANDLING_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_CSA_OFL),
+	WMI_TLV_CHATTER_PC_QUERY_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_CHATTER),
+	WMI_TLV_ECHO_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_MISC),
+	WMI_TLV_PDEV_UTF_EVENTID,
+	WMI_TLV_DEBUG_MESG_EVENTID,
+	WMI_TLV_UPDATE_STATS_EVENTID,
+	WMI_TLV_DEBUG_PRINT_EVENTID,
+	WMI_TLV_DCS_INTERFERENCE_EVENTID,
+	WMI_TLV_PDEV_QVIT_EVENTID,
+	WMI_TLV_WLAN_PROFILE_DATA_EVENTID,
+	WMI_TLV_PDEV_FTM_INTG_EVENTID,
+	WMI_TLV_WLAN_FREQ_AVOID_EVENTID,
+	WMI_TLV_VDEV_GET_KEEPALIVE_EVENTID,
+	WMI_TLV_THERMAL_MGMT_EVENTID,
+	WMI_TLV_DIAG_DATA_CONTAINER_EVENTID,
+	WMI_TLV_HOST_AUTO_SHUTDOWN_EVENTID,
+	WMI_TLV_UPDATE_WHAL_MIB_STATS_EVENTID,
+	WMI_TLV_UPDATE_VDEV_RATE_STATS_EVENTID,
+	WMI_TLV_DIAG_EVENTID,
+	WMI_TLV_GPIO_INPUT_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_GPIO),
+	WMI_TLV_UPLOADH_EVENTID,
+	WMI_TLV_CAPTUREH_EVENTID,
+	WMI_TLV_RFKILL_STATE_CHANGE_EVENTID,
+	WMI_TLV_TDLS_PEER_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_TDLS),
+	WMI_TLV_BATCH_SCAN_ENABLED_EVENTID =
+			WMI_TLV_EV(WMI_TLV_GRP_LOCATION_SCAN),
+	WMI_TLV_BATCH_SCAN_RESULT_EVENTID,
+	WMI_TLV_OEM_CAPABILITY_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_OEM),
+	WMI_TLV_OEM_MEASUREMENT_REPORT_EVENTID,
+	WMI_TLV_OEM_ERROR_REPORT_EVENTID,
+	WMI_TLV_NAN_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_NAN),
+	WMI_TLV_LPI_RESULT_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_LPI),
+	WMI_TLV_LPI_STATUS_EVENTID,
+	WMI_TLV_LPI_HANDOFF_EVENTID,
+	WMI_TLV_EXTSCAN_START_STOP_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_EXTSCAN),
+	WMI_TLV_EXTSCAN_OPERATION_EVENTID,
+	WMI_TLV_EXTSCAN_TABLE_USAGE_EVENTID,
+	WMI_TLV_EXTSCAN_CACHED_RESULTS_EVENTID,
+	WMI_TLV_EXTSCAN_WLAN_CHANGE_RESULTS_EVENTID,
+	WMI_TLV_EXTSCAN_HOTLIST_MATCH_EVENTID,
+	WMI_TLV_EXTSCAN_CAPABILITIES_EVENTID,
+	WMI_TLV_MDNS_STATS_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_MDNS_OFL),
+	WMI_TLV_SAP_OFL_ADD_STA_EVENTID = WMI_TLV_EV(WMI_TLV_GRP_SAP_OFL),
+	WMI_TLV_SAP_OFL_DEL_STA_EVENTID,
+};
+
+enum wmi_tlv_pdev_param {
+	WMI_TLV_PDEV_PARAM_TX_CHAIN_MASK = 0x1,
+	WMI_TLV_PDEV_PARAM_RX_CHAIN_MASK,
+	WMI_TLV_PDEV_PARAM_TXPOWER_LIMIT2G,
+	WMI_TLV_PDEV_PARAM_TXPOWER_LIMIT5G,
+	WMI_TLV_PDEV_PARAM_TXPOWER_SCALE,
+	WMI_TLV_PDEV_PARAM_BEACON_GEN_MODE,
+	WMI_TLV_PDEV_PARAM_BEACON_TX_MODE,
+	WMI_TLV_PDEV_PARAM_RESMGR_OFFCHAN_MODE,
+	WMI_TLV_PDEV_PARAM_PROTECTION_MODE,
+	WMI_TLV_PDEV_PARAM_DYNAMIC_BW,
+	WMI_TLV_PDEV_PARAM_NON_AGG_SW_RETRY_TH,
+	WMI_TLV_PDEV_PARAM_AGG_SW_RETRY_TH,
+	WMI_TLV_PDEV_PARAM_STA_KICKOUT_TH,
+	WMI_TLV_PDEV_PARAM_AC_AGGRSIZE_SCALING,
+	WMI_TLV_PDEV_PARAM_LTR_ENABLE,
+	WMI_TLV_PDEV_PARAM_LTR_AC_LATENCY_BE,
+	WMI_TLV_PDEV_PARAM_LTR_AC_LATENCY_BK,
+	WMI_TLV_PDEV_PARAM_LTR_AC_LATENCY_VI,
+	WMI_TLV_PDEV_PARAM_LTR_AC_LATENCY_VO,
+	WMI_TLV_PDEV_PARAM_LTR_AC_LATENCY_TIMEOUT,
+	WMI_TLV_PDEV_PARAM_LTR_SLEEP_OVERRIDE,
+	WMI_TLV_PDEV_PARAM_LTR_RX_OVERRIDE,
+	WMI_TLV_PDEV_PARAM_LTR_TX_ACTIVITY_TIMEOUT,
+	WMI_TLV_PDEV_PARAM_L1SS_ENABLE,
+	WMI_TLV_PDEV_PARAM_DSLEEP_ENABLE,
+	WMI_TLV_PDEV_PARAM_PCIELP_TXBUF_FLUSH,
+	WMI_TLV_PDEV_PARAM_PCIELP_TXBUF_WATERMARK,
+	WMI_TLV_PDEV_PARAM_PCIELP_TXBUF_TMO_EN,
+	WMI_TLV_PDEV_PARAM_PCIELP_TXBUF_TMO_VALUE,
+	WMI_TLV_PDEV_PARAM_PDEV_STATS_UPDATE_PERIOD,
+	WMI_TLV_PDEV_PARAM_VDEV_STATS_UPDATE_PERIOD,
+	WMI_TLV_PDEV_PARAM_PEER_STATS_UPDATE_PERIOD,
+	WMI_TLV_PDEV_PARAM_BCNFLT_STATS_UPDATE_PERIOD,
+	WMI_TLV_PDEV_PARAM_PMF_QOS,
+	WMI_TLV_PDEV_PARAM_ARP_AC_OVERRIDE,
+	WMI_TLV_PDEV_PARAM_DCS,
+	WMI_TLV_PDEV_PARAM_ANI_ENABLE,
+	WMI_TLV_PDEV_PARAM_ANI_POLL_PERIOD,
+	WMI_TLV_PDEV_PARAM_ANI_LISTEN_PERIOD,
+	WMI_TLV_PDEV_PARAM_ANI_OFDM_LEVEL,
+	WMI_TLV_PDEV_PARAM_ANI_CCK_LEVEL,
+	WMI_TLV_PDEV_PARAM_DYNTXCHAIN,
+	WMI_TLV_PDEV_PARAM_PROXY_STA,
+	WMI_TLV_PDEV_PARAM_IDLE_PS_CONFIG,
+	WMI_TLV_PDEV_PARAM_POWER_GATING_SLEEP,
+	WMI_TLV_PDEV_PARAM_RFKILL_ENABLE,
+	WMI_TLV_PDEV_PARAM_BURST_DUR,
+	WMI_TLV_PDEV_PARAM_BURST_ENABLE,
+	WMI_TLV_PDEV_PARAM_HW_RFKILL_CONFIG,
+	WMI_TLV_PDEV_PARAM_LOW_POWER_RF_ENABLE,
+	WMI_TLV_PDEV_PARAM_L1SS_TRACK,
+	WMI_TLV_PDEV_PARAM_HYST_EN,
+	WMI_TLV_PDEV_PARAM_POWER_COLLAPSE_ENABLE,
+	WMI_TLV_PDEV_PARAM_LED_SYS_STATE,
+	WMI_TLV_PDEV_PARAM_LED_ENABLE,
+	WMI_TLV_PDEV_PARAM_AUDIO_OVER_WLAN_LATENCY,
+	WMI_TLV_PDEV_PARAM_AUDIO_OVER_WLAN_ENABLE,
+	WMI_TLV_PDEV_PARAM_WHAL_MIB_STATS_UPDATE_ENABLE,
+	WMI_TLV_PDEV_PARAM_VDEV_RATE_STATS_UPDATE_PERIOD,
+	WMI_TLV_PDEV_PARAM_TXPOWER_REASON_NONE,
+	WMI_TLV_PDEV_PARAM_TXPOWER_REASON_SAR,
+	WMI_TLV_PDEV_PARAM_TXPOWER_REASON_MAX,
+};
+
+enum wmi_tlv_vdev_param {
+	WMI_TLV_VDEV_PARAM_RTS_THRESHOLD = 0x1,
+	WMI_TLV_VDEV_PARAM_FRAGMENTATION_THRESHOLD,
+	WMI_TLV_VDEV_PARAM_BEACON_INTERVAL,
+	WMI_TLV_VDEV_PARAM_LISTEN_INTERVAL,
+	WMI_TLV_VDEV_PARAM_MULTICAST_RATE,
+	WMI_TLV_VDEV_PARAM_MGMT_TX_RATE,
+	WMI_TLV_VDEV_PARAM_SLOT_TIME,
+	WMI_TLV_VDEV_PARAM_PREAMBLE,
+	WMI_TLV_VDEV_PARAM_SWBA_TIME,
+	WMI_TLV_VDEV_STATS_UPDATE_PERIOD,
+	WMI_TLV_VDEV_PWRSAVE_AGEOUT_TIME,
+	WMI_TLV_VDEV_HOST_SWBA_INTERVAL,
+	WMI_TLV_VDEV_PARAM_DTIM_PERIOD,
+	WMI_TLV_VDEV_OC_SCHEDULER_AIR_TIME_LIMIT,
+	WMI_TLV_VDEV_PARAM_WDS,
+	WMI_TLV_VDEV_PARAM_ATIM_WINDOW,
+	WMI_TLV_VDEV_PARAM_BMISS_COUNT_MAX,
+	WMI_TLV_VDEV_PARAM_BMISS_FIRST_BCNT,
+	WMI_TLV_VDEV_PARAM_BMISS_FINAL_BCNT,
+	WMI_TLV_VDEV_PARAM_FEATURE_WMM,
+	WMI_TLV_VDEV_PARAM_CHWIDTH,
+	WMI_TLV_VDEV_PARAM_CHEXTOFFSET,
+	WMI_TLV_VDEV_PARAM_DISABLE_HTPROTECTION,
+	WMI_TLV_VDEV_PARAM_STA_QUICKKICKOUT,
+	WMI_TLV_VDEV_PARAM_MGMT_RATE,
+	WMI_TLV_VDEV_PARAM_PROTECTION_MODE,
+	WMI_TLV_VDEV_PARAM_FIXED_RATE,
+	WMI_TLV_VDEV_PARAM_SGI,
+	WMI_TLV_VDEV_PARAM_LDPC,
+	WMI_TLV_VDEV_PARAM_TX_STBC,
+	WMI_TLV_VDEV_PARAM_RX_STBC,
+	WMI_TLV_VDEV_PARAM_INTRA_BSS_FWD,
+	WMI_TLV_VDEV_PARAM_DEF_KEYID,
+	WMI_TLV_VDEV_PARAM_NSS,
+	WMI_TLV_VDEV_PARAM_BCAST_DATA_RATE,
+	WMI_TLV_VDEV_PARAM_MCAST_DATA_RATE,
+	WMI_TLV_VDEV_PARAM_MCAST_INDICATE,
+	WMI_TLV_VDEV_PARAM_DHCP_INDICATE,
+	WMI_TLV_VDEV_PARAM_UNKNOWN_DEST_INDICATE,
+	WMI_TLV_VDEV_PARAM_AP_KEEPALIVE_MIN_IDLE_INACTIVE_TIME_SECS,
+	WMI_TLV_VDEV_PARAM_AP_KEEPALIVE_MAX_IDLE_INACTIVE_TIME_SECS,
+	WMI_TLV_VDEV_PARAM_AP_KEEPALIVE_MAX_UNRESPONSIVE_TIME_SECS,
+	WMI_TLV_VDEV_PARAM_AP_ENABLE_NAWDS,
+	WMI_TLV_VDEV_PARAM_ENABLE_RTSCTS,
+	WMI_TLV_VDEV_PARAM_TXBF,
+	WMI_TLV_VDEV_PARAM_PACKET_POWERSAVE,
+	WMI_TLV_VDEV_PARAM_DROP_UNENCRY,
+	WMI_TLV_VDEV_PARAM_TX_ENCAP_TYPE,
+	WMI_TLV_VDEV_PARAM_AP_DETECT_OUT_OF_SYNC_SLEEPING_STA_TIME_SECS,
+	WMI_TLV_VDEV_PARAM_EARLY_RX_ADJUST_ENABLE,
+	WMI_TLV_VDEV_PARAM_EARLY_RX_TGT_BMISS_NUM,
+	WMI_TLV_VDEV_PARAM_EARLY_RX_BMISS_SAMPLE_CYCLE,
+	WMI_TLV_VDEV_PARAM_EARLY_RX_SLOP_STEP,
+	WMI_TLV_VDEV_PARAM_EARLY_RX_INIT_SLOP,
+	WMI_TLV_VDEV_PARAM_EARLY_RX_ADJUST_PAUSE,
+	WMI_TLV_VDEV_PARAM_TX_PWRLIMIT,
+	WMI_TLV_VDEV_PARAM_SNR_NUM_FOR_CAL,
+	WMI_TLV_VDEV_PARAM_ROAM_FW_OFFLOAD,
+	WMI_TLV_VDEV_PARAM_ENABLE_RMC,
+	WMI_TLV_VDEV_PARAM_IBSS_MAX_BCN_LOST_MS,
+	WMI_TLV_VDEV_PARAM_MAX_RATE,
+	WMI_TLV_VDEV_PARAM_EARLY_RX_DRIFT_SAMPLE,
+	WMI_TLV_VDEV_PARAM_SET_IBSS_TX_FAIL_CNT_THR,
+	WMI_TLV_VDEV_PARAM_EBT_RESYNC_TIMEOUT,
+	WMI_TLV_VDEV_PARAM_AGGR_TRIG_EVENT_ENABLE,
+	WMI_TLV_VDEV_PARAM_IS_IBSS_POWER_SAVE_ALLOWED,
+	WMI_TLV_VDEV_PARAM_IS_POWER_COLLAPSE_ALLOWED,
+	WMI_TLV_VDEV_PARAM_IS_AWAKE_ON_TXRX_ENABLED,
+	WMI_TLV_VDEV_PARAM_INACTIVITY_CNT,
+	WMI_TLV_VDEV_PARAM_TXSP_END_INACTIVITY_TIME_MS,
+	WMI_TLV_VDEV_PARAM_DTIM_POLICY,
+	WMI_TLV_VDEV_PARAM_IBSS_PS_WARMUP_TIME_SECS,
+	WMI_TLV_VDEV_PARAM_IBSS_PS_1RX_CHAIN_IN_ATIM_WINDOW_ENABLE,
+};
+
+enum wmi_tlv_tag {
+	WMI_TLV_TAG_LAST_RESERVED = 15,
+
+	WMI_TLV_TAG_FIRST_ARRAY_ENUM,
+	WMI_TLV_TAG_ARRAY_UINT32 = WMI_TLV_TAG_FIRST_ARRAY_ENUM,
+	WMI_TLV_TAG_ARRAY_BYTE,
+	WMI_TLV_TAG_ARRAY_STRUCT,
+	WMI_TLV_TAG_ARRAY_FIXED_STRUCT,
+	WMI_TLV_TAG_LAST_ARRAY_ENUM = 31,
+
+	WMI_TLV_TAG_STRUCT_SERVICE_READY_EVENT,
+	WMI_TLV_TAG_STRUCT_HAL_REG_CAPABILITIES,
+	WMI_TLV_TAG_STRUCT_WLAN_HOST_MEM_REQ,
+	WMI_TLV_TAG_STRUCT_READY_EVENT,
+	WMI_TLV_TAG_STRUCT_SCAN_EVENT,
+	WMI_TLV_TAG_STRUCT_PDEV_TPC_CONFIG_EVENT,
+	WMI_TLV_TAG_STRUCT_CHAN_INFO_EVENT,
+	WMI_TLV_TAG_STRUCT_COMB_PHYERR_RX_HDR,
+	WMI_TLV_TAG_STRUCT_VDEV_START_RESPONSE_EVENT,
+	WMI_TLV_TAG_STRUCT_VDEV_STOPPED_EVENT,
+	WMI_TLV_TAG_STRUCT_VDEV_INSTALL_KEY_COMPLETE_EVENT,
+	WMI_TLV_TAG_STRUCT_PEER_STA_KICKOUT_EVENT,
+	WMI_TLV_TAG_STRUCT_MGMT_RX_HDR,
+	WMI_TLV_TAG_STRUCT_TBTT_OFFSET_EVENT,
+	WMI_TLV_TAG_STRUCT_TX_DELBA_COMPLETE_EVENT,
+	WMI_TLV_TAG_STRUCT_TX_ADDBA_COMPLETE_EVENT,
+	WMI_TLV_TAG_STRUCT_ROAM_EVENT,
+	WMI_TLV_TAG_STRUCT_WOW_EVENT_INFO,
+	WMI_TLV_TAG_STRUCT_WOW_EVENT_INFO_SECTION_BITMAP,
+	WMI_TLV_TAG_STRUCT_RTT_EVENT_HEADER,
+	WMI_TLV_TAG_STRUCT_RTT_ERROR_REPORT_EVENT,
+	WMI_TLV_TAG_STRUCT_RTT_MEAS_EVENT,
+	WMI_TLV_TAG_STRUCT_ECHO_EVENT,
+	WMI_TLV_TAG_STRUCT_FTM_INTG_EVENT,
+	WMI_TLV_TAG_STRUCT_VDEV_GET_KEEPALIVE_EVENT,
+	WMI_TLV_TAG_STRUCT_GPIO_INPUT_EVENT,
+	WMI_TLV_TAG_STRUCT_CSA_EVENT,
+	WMI_TLV_TAG_STRUCT_GTK_OFFLOAD_STATUS_EVENT,
+	WMI_TLV_TAG_STRUCT_IGTK_INFO,
+	WMI_TLV_TAG_STRUCT_DCS_INTERFERENCE_EVENT,
+	WMI_TLV_TAG_STRUCT_ATH_DCS_CW_INT,
+	WMI_TLV_TAG_STRUCT_ATH_DCS_WLAN_INT_STAT,
+	WMI_TLV_TAG_STRUCT_WLAN_PROFILE_CTX_T,
+	WMI_TLV_TAG_STRUCT_WLAN_PROFILE_T,
+	WMI_TLV_TAG_STRUCT_PDEV_QVIT_EVENT,
+	WMI_TLV_TAG_STRUCT_HOST_SWBA_EVENT,
+	WMI_TLV_TAG_STRUCT_TIM_INFO,
+	WMI_TLV_TAG_STRUCT_P2P_NOA_INFO,
+	WMI_TLV_TAG_STRUCT_STATS_EVENT,
+	WMI_TLV_TAG_STRUCT_AVOID_FREQ_RANGES_EVENT,
+	WMI_TLV_TAG_STRUCT_AVOID_FREQ_RANGE_DESC,
+	WMI_TLV_TAG_STRUCT_GTK_REKEY_FAIL_EVENT,
+	WMI_TLV_TAG_STRUCT_INIT_CMD,
+	WMI_TLV_TAG_STRUCT_RESOURCE_CONFIG,
+	WMI_TLV_TAG_STRUCT_WLAN_HOST_MEMORY_CHUNK,
+	WMI_TLV_TAG_STRUCT_START_SCAN_CMD,
+	WMI_TLV_TAG_STRUCT_STOP_SCAN_CMD,
+	WMI_TLV_TAG_STRUCT_SCAN_CHAN_LIST_CMD,
+	WMI_TLV_TAG_STRUCT_CHANNEL,
+	WMI_TLV_TAG_STRUCT_PDEV_SET_REGDOMAIN_CMD,
+	WMI_TLV_TAG_STRUCT_PDEV_SET_PARAM_CMD,
+	WMI_TLV_TAG_STRUCT_PDEV_SET_WMM_PARAMS_CMD,
+	WMI_TLV_TAG_STRUCT_WMM_PARAMS,
+	WMI_TLV_TAG_STRUCT_PDEV_SET_QUIET_CMD,
+	WMI_TLV_TAG_STRUCT_VDEV_CREATE_CMD,
+	WMI_TLV_TAG_STRUCT_VDEV_DELETE_CMD,
+	WMI_TLV_TAG_STRUCT_VDEV_START_REQUEST_CMD,
+	WMI_TLV_TAG_STRUCT_P2P_NOA_DESCRIPTOR,
+	WMI_TLV_TAG_STRUCT_P2P_GO_SET_BEACON_IE,
+	WMI_TLV_TAG_STRUCT_GTK_OFFLOAD_CMD,
+	WMI_TLV_TAG_STRUCT_VDEV_UP_CMD,
+	WMI_TLV_TAG_STRUCT_VDEV_STOP_CMD,
+	WMI_TLV_TAG_STRUCT_VDEV_DOWN_CMD,
+	WMI_TLV_TAG_STRUCT_VDEV_SET_PARAM_CMD,
+	WMI_TLV_TAG_STRUCT_VDEV_INSTALL_KEY_CMD,
+	WMI_TLV_TAG_STRUCT_PEER_CREATE_CMD,
+	WMI_TLV_TAG_STRUCT_PEER_DELETE_CMD,
+	WMI_TLV_TAG_STRUCT_PEER_FLUSH_TIDS_CMD,
+	WMI_TLV_TAG_STRUCT_PEER_SET_PARAM_CMD,
+	WMI_TLV_TAG_STRUCT_PEER_ASSOC_COMPLETE_CMD,
+	WMI_TLV_TAG_STRUCT_VHT_RATE_SET,
+	WMI_TLV_TAG_STRUCT_BCN_TMPL_CMD,
+	WMI_TLV_TAG_STRUCT_PRB_TMPL_CMD,
+	WMI_TLV_TAG_STRUCT_BCN_PRB_INFO,
+	WMI_TLV_TAG_STRUCT_PEER_TID_ADDBA_CMD,
+	WMI_TLV_TAG_STRUCT_PEER_TID_DELBA_CMD,
+	WMI_TLV_TAG_STRUCT_STA_POWERSAVE_MODE_CMD,
+	WMI_TLV_TAG_STRUCT_STA_POWERSAVE_PARAM_CMD,
+	WMI_TLV_TAG_STRUCT_STA_DTIM_PS_METHOD_CMD,
+	WMI_TLV_TAG_STRUCT_ROAM_SCAN_MODE,
+	WMI_TLV_TAG_STRUCT_ROAM_SCAN_RSSI_THRESHOLD,
+	WMI_TLV_TAG_STRUCT_ROAM_SCAN_PERIOD,
+	WMI_TLV_TAG_STRUCT_ROAM_SCAN_RSSI_CHANGE_THRESHOLD,
+	WMI_TLV_TAG_STRUCT_PDEV_SUSPEND_CMD,
+	WMI_TLV_TAG_STRUCT_PDEV_RESUME_CMD,
+	WMI_TLV_TAG_STRUCT_ADD_BCN_FILTER_CMD,
+	WMI_TLV_TAG_STRUCT_RMV_BCN_FILTER_CMD,
+	WMI_TLV_TAG_STRUCT_WOW_ENABLE_CMD,
+	WMI_TLV_TAG_STRUCT_WOW_HOSTWAKEUP_FROM_SLEEP_CMD,
+	WMI_TLV_TAG_STRUCT_STA_UAPSD_AUTO_TRIG_CMD,
+	WMI_TLV_TAG_STRUCT_STA_UAPSD_AUTO_TRIG_PARAM,
+	WMI_TLV_TAG_STRUCT_SET_ARP_NS_OFFLOAD_CMD,
+	WMI_TLV_TAG_STRUCT_ARP_OFFLOAD_TUPLE,
+	WMI_TLV_TAG_STRUCT_NS_OFFLOAD_TUPLE,
+	WMI_TLV_TAG_STRUCT_FTM_INTG_CMD,
+	WMI_TLV_TAG_STRUCT_STA_KEEPALIVE_CMD,
+	WMI_TLV_TAG_STRUCT_STA_KEEPALVE_ARP_RESPONSE,
+	WMI_TLV_TAG_STRUCT_P2P_SET_VENDOR_IE_DATA_CMD,
+	WMI_TLV_TAG_STRUCT_AP_PS_PEER_CMD,
+	WMI_TLV_TAG_STRUCT_PEER_RATE_RETRY_SCHED_CMD,
+	WMI_TLV_TAG_STRUCT_WLAN_PROFILE_TRIGGER_CMD,
+	WMI_TLV_TAG_STRUCT_WLAN_PROFILE_SET_HIST_INTVL_CMD,
+	WMI_TLV_TAG_STRUCT_WLAN_PROFILE_GET_PROF_DATA_CMD,
+	WMI_TLV_TAG_STRUCT_WLAN_PROFILE_ENABLE_PROFILE_ID_CMD,
+	WMI_TLV_TAG_STRUCT_WOW_DEL_PATTERN_CMD,
+	WMI_TLV_TAG_STRUCT_WOW_ADD_DEL_EVT_CMD,
+	WMI_TLV_TAG_STRUCT_RTT_MEASREQ_HEAD,
+	WMI_TLV_TAG_STRUCT_RTT_MEASREQ_BODY,
+	WMI_TLV_TAG_STRUCT_RTT_TSF_CMD,
+	WMI_TLV_TAG_STRUCT_VDEV_SPECTRAL_CONFIGURE_CMD,
+	WMI_TLV_TAG_STRUCT_VDEV_SPECTRAL_ENABLE_CMD,
+	WMI_TLV_TAG_STRUCT_REQUEST_STATS_CMD,
+	WMI_TLV_TAG_STRUCT_NLO_CONFIG_CMD,
+	WMI_TLV_TAG_STRUCT_NLO_CONFIGURED_PARAMETERS,
+	WMI_TLV_TAG_STRUCT_CSA_OFFLOAD_ENABLE_CMD,
+	WMI_TLV_TAG_STRUCT_CSA_OFFLOAD_CHANSWITCH_CMD,
+	WMI_TLV_TAG_STRUCT_CHATTER_SET_MODE_CMD,
+	WMI_TLV_TAG_STRUCT_ECHO_CMD,
+	WMI_TLV_TAG_STRUCT_VDEV_SET_KEEPALIVE_CMD,
+	WMI_TLV_TAG_STRUCT_VDEV_GET_KEEPALIVE_CMD,
+	WMI_TLV_TAG_STRUCT_FORCE_FW_HANG_CMD,
+	WMI_TLV_TAG_STRUCT_GPIO_CONFIG_CMD,
+	WMI_TLV_TAG_STRUCT_GPIO_OUTPUT_CMD,
+	WMI_TLV_TAG_STRUCT_PEER_ADD_WDS_ENTRY_CMD,
+	WMI_TLV_TAG_STRUCT_PEER_REMOVE_WDS_ENTRY_CMD,
+	WMI_TLV_TAG_STRUCT_BCN_TX_HDR,
+	WMI_TLV_TAG_STRUCT_BCN_SEND_FROM_HOST_CMD,
+	WMI_TLV_TAG_STRUCT_MGMT_TX_HDR,
+	WMI_TLV_TAG_STRUCT_ADDBA_CLEAR_RESP_CMD,
+	WMI_TLV_TAG_STRUCT_ADDBA_SEND_CMD,
+	WMI_TLV_TAG_STRUCT_DELBA_SEND_CMD,
+	WMI_TLV_TAG_STRUCT_ADDBA_SETRESPONSE_CMD,
+	WMI_TLV_TAG_STRUCT_SEND_SINGLEAMSDU_CMD,
+	WMI_TLV_TAG_STRUCT_PDEV_PKTLOG_ENABLE_CMD,
+	WMI_TLV_TAG_STRUCT_PDEV_PKTLOG_DISABLE_CMD,
+	WMI_TLV_TAG_STRUCT_PDEV_SET_HT_IE_CMD,
+	WMI_TLV_TAG_STRUCT_PDEV_SET_VHT_IE_CMD,
+	WMI_TLV_TAG_STRUCT_PDEV_SET_DSCP_TID_MAP_CMD,
+	WMI_TLV_TAG_STRUCT_PDEV_GREEN_AP_PS_ENABLE_CMD,
+	WMI_TLV_TAG_STRUCT_PDEV_GET_TPC_CONFIG_CMD,
+	WMI_TLV_TAG_STRUCT_PDEV_SET_BASE_MACADDR_CMD,
+	WMI_TLV_TAG_STRUCT_PEER_MCAST_GROUP_CMD,
+	WMI_TLV_TAG_STRUCT_ROAM_AP_PROFILE,
+	WMI_TLV_TAG_STRUCT_AP_PROFILE,
+	WMI_TLV_TAG_STRUCT_SCAN_SCH_PRIORITY_TABLE_CMD,
+	WMI_TLV_TAG_STRUCT_PDEV_DFS_ENABLE_CMD,
+	WMI_TLV_TAG_STRUCT_PDEV_DFS_DISABLE_CMD,
+	WMI_TLV_TAG_STRUCT_WOW_ADD_PATTERN_CMD,
+	WMI_TLV_TAG_STRUCT_WOW_BITMAP_PATTERN_T,
+	WMI_TLV_TAG_STRUCT_WOW_IPV4_SYNC_PATTERN_T,
+	WMI_TLV_TAG_STRUCT_WOW_IPV6_SYNC_PATTERN_T,
+	WMI_TLV_TAG_STRUCT_WOW_MAGIC_PATTERN_CMD,
+	WMI_TLV_TAG_STRUCT_SCAN_UPDATE_REQUEST_CMD,
+	WMI_TLV_TAG_STRUCT_CHATTER_PKT_COALESCING_FILTER,
+	WMI_TLV_TAG_STRUCT_CHATTER_COALESCING_ADD_FILTER_CMD,
+	WMI_TLV_TAG_STRUCT_CHATTER_COALESCING_DELETE_FILTER_CMD,
+	WMI_TLV_TAG_STRUCT_CHATTER_COALESCING_QUERY_CMD,
+	WMI_TLV_TAG_STRUCT_TXBF_CMD,
+	WMI_TLV_TAG_STRUCT_DEBUG_LOG_CONFIG_CMD,
+	WMI_TLV_TAG_STRUCT_NLO_EVENT,
+	WMI_TLV_TAG_STRUCT_CHATTER_QUERY_REPLY_EVENT,
+	WMI_TLV_TAG_STRUCT_UPLOAD_H_HDR,
+	WMI_TLV_TAG_STRUCT_CAPTURE_H_EVENT_HDR,
+	WMI_TLV_TAG_STRUCT_VDEV_WNM_SLEEPMODE_CMD,
+	WMI_TLV_TAG_STRUCT_VDEV_IPSEC_NATKEEPALIVE_FILTER_CMD,
+	WMI_TLV_TAG_STRUCT_VDEV_WMM_ADDTS_CMD,
+	WMI_TLV_TAG_STRUCT_VDEV_WMM_DELTS_CMD,
+	WMI_TLV_TAG_STRUCT_VDEV_SET_WMM_PARAMS_CMD,
+	WMI_TLV_TAG_STRUCT_TDLS_SET_STATE_CMD,
+	WMI_TLV_TAG_STRUCT_TDLS_PEER_UPDATE_CMD,
+	WMI_TLV_TAG_STRUCT_TDLS_PEER_EVENT,
+	WMI_TLV_TAG_STRUCT_TDLS_PEER_CAPABILITIES,
+	WMI_TLV_TAG_STRUCT_VDEV_MCC_SET_TBTT_MODE_CMD,
+	WMI_TLV_TAG_STRUCT_ROAM_CHAN_LIST,
+	WMI_TLV_TAG_STRUCT_VDEV_MCC_BCN_INTVL_CHANGE_EVENT,
+	WMI_TLV_TAG_STRUCT_RESMGR_ADAPTIVE_OCS_CMD,
+	WMI_TLV_TAG_STRUCT_RESMGR_SET_CHAN_TIME_QUOTA_CMD,
+	WMI_TLV_TAG_STRUCT_RESMGR_SET_CHAN_LATENCY_CMD,
+	WMI_TLV_TAG_STRUCT_BA_REQ_SSN_CMD,
+	WMI_TLV_TAG_STRUCT_BA_RSP_SSN_EVENT,
+	WMI_TLV_TAG_STRUCT_STA_SMPS_FORCE_MODE_CMD,
+	WMI_TLV_TAG_STRUCT_SET_MCASTBCAST_FILTER_CMD,
+	WMI_TLV_TAG_STRUCT_P2P_SET_OPPPS_CMD,
+	WMI_TLV_TAG_STRUCT_P2P_SET_NOA_CMD,
+	WMI_TLV_TAG_STRUCT_BA_REQ_SSN_CMD_SUB_STRUCT_PARAM,
+	WMI_TLV_TAG_STRUCT_BA_REQ_SSN_EVENT_SUB_STRUCT_PARAM,
+	WMI_TLV_TAG_STRUCT_STA_SMPS_PARAM_CMD,
+	WMI_TLV_TAG_STRUCT_VDEV_SET_GTX_PARAMS_CMD,
+	WMI_TLV_TAG_STRUCT_MCC_SCHED_TRAFFIC_STATS_CMD,
+	WMI_TLV_TAG_STRUCT_MCC_SCHED_STA_TRAFFIC_STATS,
+	WMI_TLV_TAG_STRUCT_OFFLOAD_BCN_TX_STATUS_EVENT,
+	WMI_TLV_TAG_STRUCT_P2P_NOA_EVENT,
+	WMI_TLV_TAG_STRUCT_HB_SET_ENABLE_CMD,
+	WMI_TLV_TAG_STRUCT_HB_SET_TCP_PARAMS_CMD,
+	WMI_TLV_TAG_STRUCT_HB_SET_TCP_PKT_FILTER_CMD,
+	WMI_TLV_TAG_STRUCT_HB_SET_UDP_PARAMS_CMD,
+	WMI_TLV_TAG_STRUCT_HB_SET_UDP_PKT_FILTER_CMD,
+	WMI_TLV_TAG_STRUCT_HB_IND_EVENT,
+	WMI_TLV_TAG_STRUCT_TX_PAUSE_EVENT,
+	WMI_TLV_TAG_STRUCT_RFKILL_EVENT,
+	WMI_TLV_TAG_STRUCT_DFS_RADAR_EVENT,
+	WMI_TLV_TAG_STRUCT_DFS_PHYERR_FILTER_ENA_CMD,
+	WMI_TLV_TAG_STRUCT_DFS_PHYERR_FILTER_DIS_CMD,
+	WMI_TLV_TAG_STRUCT_BATCH_SCAN_RESULT_SCAN_LIST,
+	WMI_TLV_TAG_STRUCT_BATCH_SCAN_RESULT_NETWORK_INFO,
+	WMI_TLV_TAG_STRUCT_BATCH_SCAN_ENABLE_CMD,
+	WMI_TLV_TAG_STRUCT_BATCH_SCAN_DISABLE_CMD,
+	WMI_TLV_TAG_STRUCT_BATCH_SCAN_TRIGGER_RESULT_CMD,
+	WMI_TLV_TAG_STRUCT_BATCH_SCAN_ENABLED_EVENT,
+	WMI_TLV_TAG_STRUCT_BATCH_SCAN_RESULT_EVENT,
+	WMI_TLV_TAG_STRUCT_VDEV_PLMREQ_START_CMD,
+	WMI_TLV_TAG_STRUCT_VDEV_PLMREQ_STOP_CMD,
+	WMI_TLV_TAG_STRUCT_THERMAL_MGMT_CMD,
+	WMI_TLV_TAG_STRUCT_THERMAL_MGMT_EVENT,
+	WMI_TLV_TAG_STRUCT_PEER_INFO_REQ_CMD,
+	WMI_TLV_TAG_STRUCT_PEER_INFO_EVENT,
+	WMI_TLV_TAG_STRUCT_PEER_INFO,
+	WMI_TLV_TAG_STRUCT_PEER_TX_FAIL_CNT_THR_EVENT,
+	WMI_TLV_TAG_STRUCT_RMC_SET_MODE_CMD,
+	WMI_TLV_TAG_STRUCT_RMC_SET_ACTION_PERIOD_CMD,
+	WMI_TLV_TAG_STRUCT_RMC_CONFIG_CMD,
+	WMI_TLV_TAG_STRUCT_MHF_OFFLOAD_SET_MODE_CMD,
+	WMI_TLV_TAG_STRUCT_MHF_OFFLOAD_PLUMB_ROUTING_TABLE_CMD,
+	WMI_TLV_TAG_STRUCT_ADD_PROACTIVE_ARP_RSP_PATTERN_CMD,
+	WMI_TLV_TAG_STRUCT_DEL_PROACTIVE_ARP_RSP_PATTERN_CMD,
+	WMI_TLV_TAG_STRUCT_NAN_CMD_PARAM,
+	WMI_TLV_TAG_STRUCT_NAN_EVENT_HDR,
+	WMI_TLV_TAG_STRUCT_PDEV_L1SS_TRACK_EVENT,
+	WMI_TLV_TAG_STRUCT_DIAG_DATA_CONTAINER_EVENT,
+	WMI_TLV_TAG_STRUCT_MODEM_POWER_STATE_CMD_PARAM,
+	WMI_TLV_TAG_STRUCT_PEER_GET_ESTIMATED_LINKSPEED_CMD,
+	WMI_TLV_TAG_STRUCT_PEER_ESTIMATED_LINKSPEED_EVENT,
+	WMI_TLV_TAG_STRUCT_AGGR_STATE_TRIG_EVENT,
+	WMI_TLV_TAG_STRUCT_MHF_OFFLOAD_ROUTING_TABLE_ENTRY,
+	WMI_TLV_TAG_STRUCT_ROAM_SCAN_CMD,
+	WMI_TLV_TAG_STRUCT_REQ_STATS_EXT_CMD,
+	WMI_TLV_TAG_STRUCT_STATS_EXT_EVENT,
+	WMI_TLV_TAG_STRUCT_OBSS_SCAN_ENABLE_CMD,
+	WMI_TLV_TAG_STRUCT_OBSS_SCAN_DISABLE_CMD,
+	WMI_TLV_TAG_STRUCT_OFFLOAD_PRB_RSP_TX_STATUS_EVENT,
+	WMI_TLV_TAG_STRUCT_PDEV_SET_LED_CONFIG_CMD,
+	WMI_TLV_TAG_STRUCT_HOST_AUTO_SHUTDOWN_CFG_CMD,
+	WMI_TLV_TAG_STRUCT_HOST_AUTO_SHUTDOWN_EVENT,
+	WMI_TLV_TAG_STRUCT_UPDATE_WHAL_MIB_STATS_EVENT,
+	WMI_TLV_TAG_STRUCT_CHAN_AVOID_UPDATE_CMD_PARAM,
+	WMI_TLV_TAG_STRUCT_WOW_ACER_IOAC_PKT_PATTERN_T,
+	WMI_TLV_TAG_STRUCT_WOW_ACER_IOAC_TMR_PATTERN_T,
+	WMI_TLV_TAG_STRUCT_WOW_IOAC_ADD_KEEPALIVE_CMD,
+	WMI_TLV_TAG_STRUCT_WOW_IOAC_DEL_KEEPALIVE_CMD,
+	WMI_TLV_TAG_STRUCT_WOW_IOAC_KEEPALIVE_T,
+	WMI_TLV_TAG_STRUCT_WOW_ACER_IOAC_ADD_PATTERN_CMD,
+	WMI_TLV_TAG_STRUCT_WOW_ACER_IOAC_DEL_PATTERN_CMD,
+	WMI_TLV_TAG_STRUCT_START_LINK_STATS_CMD,
+	WMI_TLV_TAG_STRUCT_CLEAR_LINK_STATS_CMD,
+	WMI_TLV_TAG_STRUCT_REQUEST_LINK_STATS_CMD,
+	WMI_TLV_TAG_STRUCT_IFACE_LINK_STATS_EVENT,
+	WMI_TLV_TAG_STRUCT_RADIO_LINK_STATS_EVENT,
+	WMI_TLV_TAG_STRUCT_PEER_STATS_EVENT,
+	WMI_TLV_TAG_STRUCT_CHANNEL_STATS,
+	WMI_TLV_TAG_STRUCT_RADIO_LINK_STATS,
+	WMI_TLV_TAG_STRUCT_RATE_STATS,
+	WMI_TLV_TAG_STRUCT_PEER_LINK_STATS,
+	WMI_TLV_TAG_STRUCT_WMM_AC_STATS,
+	WMI_TLV_TAG_STRUCT_IFACE_LINK_STATS,
+	WMI_TLV_TAG_STRUCT_LPI_MGMT_SNOOPING_CONFIG_CMD,
+	WMI_TLV_TAG_STRUCT_LPI_START_SCAN_CMD,
+	WMI_TLV_TAG_STRUCT_LPI_STOP_SCAN_CMD,
+	WMI_TLV_TAG_STRUCT_LPI_RESULT_EVENT,
+	WMI_TLV_TAG_STRUCT_PEER_STATE_EVENT,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_BUCKET_CMD,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_BUCKET_CHANNEL_EVENT,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_START_CMD,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_STOP_CMD,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_CONFIGURE_WLAN_CHANGE_MONITOR_CMD,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_WLAN_CHANGE_BSSID_PARAM_CMD,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_CONFIGURE_HOTLIST_MONITOR_CMD,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_GET_CACHED_RESULTS_CMD,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_GET_WLAN_CHANGE_RESULTS_CMD,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_SET_CAPABILITIES_CMD,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_GET_CAPABILITIES_CMD,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_OPERATION_EVENT,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_START_STOP_EVENT,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_TABLE_USAGE_EVENT,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_WLAN_DESCRIPTOR_EVENT,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_RSSI_INFO_EVENT,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_CACHED_RESULTS_EVENT,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_WLAN_CHANGE_RESULTS_EVENT,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_WLAN_CHANGE_RESULT_BSSID_EVENT,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_HOTLIST_MATCH_EVENT,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_CAPABILITIES_EVENT,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_CACHE_CAPABILITIES_EVENT,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_WLAN_CHANGE_MONITOR_CAPABILITIES_EVENT,
+	WMI_TLV_TAG_STRUCT_EXTSCAN_HOTLIST_MONITOR_CAPABILITIES_EVENT,
+	WMI_TLV_TAG_STRUCT_D0_WOW_ENABLE_DISABLE_CMD,
+	WMI_TLV_TAG_STRUCT_D0_WOW_DISABLE_ACK_EVENT,
+	WMI_TLV_TAG_STRUCT_UNIT_TEST_CMD,
+	WMI_TLV_TAG_STRUCT_ROAM_OFFLOAD_TLV_PARAM,
+	WMI_TLV_TAG_STRUCT_ROAM_11I_OFFLOAD_TLV_PARAM,
+	WMI_TLV_TAG_STRUCT_ROAM_11R_OFFLOAD_TLV_PARAM,
+	WMI_TLV_TAG_STRUCT_ROAM_ESE_OFFLOAD_TLV_PARAM,
+	WMI_TLV_TAG_STRUCT_ROAM_SYNCH_EVENT,
+	WMI_TLV_TAG_STRUCT_ROAM_SYNCH_COMPLETE,
+	WMI_TLV_TAG_STRUCT_EXTWOW_ENABLE_CMD,
+	WMI_TLV_TAG_STRUCT_EXTWOW_SET_APP_TYPE1_PARAMS_CMD,
+	WMI_TLV_TAG_STRUCT_EXTWOW_SET_APP_TYPE2_PARAMS_CMD,
+	WMI_TLV_TAG_STRUCT_LPI_STATUS_EVENT,
+	WMI_TLV_TAG_STRUCT_LPI_HANDOFF_EVENT,
+	WMI_TLV_TAG_STRUCT_VDEV_RATE_STATS_EVENT,
+	WMI_TLV_TAG_STRUCT_VDEV_RATE_HT_INFO,
+	WMI_TLV_TAG_STRUCT_RIC_REQUEST,
+	WMI_TLV_TAG_STRUCT_PDEV_GET_TEMPERATURE_CMD,
+	WMI_TLV_TAG_STRUCT_PDEV_TEMPERATURE_EVENT,
+	WMI_TLV_TAG_STRUCT_SET_DHCP_SERVER_OFFLOAD_CMD,
+	WMI_TLV_TAG_STRUCT_TPC_CHAINMASK_CONFIG_CMD,
+	WMI_TLV_TAG_STRUCT_RIC_TSPEC,
+	WMI_TLV_TAG_STRUCT_TPC_CHAINMASK_CONFIG,
+	WMI_TLV_TAG_STRUCT_IPA_OFFLOAD_CMD,
+	WMI_TLV_TAG_STRUCT_SCAN_PROB_REQ_OUI_CMD,
+	WMI_TLV_TAG_STRUCT_KEY_MATERIAL,
+	WMI_TLV_TAG_STRUCT_TDLS_SET_OFFCHAN_MODE_CMD,
+	WMI_TLV_TAG_STRUCT_SET_LED_FLASHING_CMD,
+	WMI_TLV_TAG_STRUCT_MDNS_OFFLOAD_CMD,
+	WMI_TLV_TAG_STRUCT_MDNS_SET_FQDN_CMD,
+	WMI_TLV_TAG_STRUCT_MDNS_SET_RESP_CMD,
+	WMI_TLV_TAG_STRUCT_MDNS_GET_STATS_CMD,
+	WMI_TLV_TAG_STRUCT_MDNS_STATS_EVENT,
+	WMI_TLV_TAG_STRUCT_ROAM_INVOKE_CMD,
+	WMI_TLV_TAG_STRUCT_PDEV_RESUME_EVENT,
+	WMI_TLV_TAG_STRUCT_PDEV_SET_ANTENNA_DIVERSITY_CMD,
+	WMI_TLV_TAG_STRUCT_SAP_OFL_ENABLE_CMD,
+	WMI_TLV_TAG_STRUCT_SAP_OFL_ADD_STA_EVENT,
+	WMI_TLV_TAG_STRUCT_SAP_OFL_DEL_STA_EVENT,
+	WMI_TLV_TAG_STRUCT_APFIND_CMD_PARAM,
+	WMI_TLV_TAG_STRUCT_APFIND_EVENT_HDR,
+
+	WMI_TLV_TAG_MAX
+};
+
+enum wmi_tlv_service {
+	WMI_TLV_SERVICE_BEACON_OFFLOAD = 0,
+	WMI_TLV_SERVICE_SCAN_OFFLOAD,
+	WMI_TLV_SERVICE_ROAM_SCAN_OFFLOAD,
+	WMI_TLV_SERVICE_BCN_MISS_OFFLOAD,
+	WMI_TLV_SERVICE_STA_PWRSAVE,
+	WMI_TLV_SERVICE_STA_ADVANCED_PWRSAVE,
+	WMI_TLV_SERVICE_AP_UAPSD,
+	WMI_TLV_SERVICE_AP_DFS,
+	WMI_TLV_SERVICE_11AC,
+	WMI_TLV_SERVICE_BLOCKACK,
+	WMI_TLV_SERVICE_PHYERR,
+	WMI_TLV_SERVICE_BCN_FILTER,
+	WMI_TLV_SERVICE_RTT,
+	WMI_TLV_SERVICE_WOW,
+	WMI_TLV_SERVICE_RATECTRL_CACHE,
+	WMI_TLV_SERVICE_IRAM_TIDS,
+	WMI_TLV_SERVICE_ARPNS_OFFLOAD,
+	WMI_TLV_SERVICE_NLO,
+	WMI_TLV_SERVICE_GTK_OFFLOAD,
+	WMI_TLV_SERVICE_SCAN_SCH,
+	WMI_TLV_SERVICE_CSA_OFFLOAD,
+	WMI_TLV_SERVICE_CHATTER,
+	WMI_TLV_SERVICE_COEX_FREQAVOID,
+	WMI_TLV_SERVICE_PACKET_POWER_SAVE,
+	WMI_TLV_SERVICE_FORCE_FW_HANG,
+	WMI_TLV_SERVICE_GPIO,
+	WMI_TLV_SERVICE_STA_DTIM_PS_MODULATED_DTIM,
+	WMI_TLV_SERVICE_STA_UAPSD_BASIC_AUTO_TRIG,
+	WMI_TLV_SERVICE_STA_UAPSD_VAR_AUTO_TRIG,
+	WMI_TLV_SERVICE_STA_KEEP_ALIVE,
+	WMI_TLV_SERVICE_TX_ENCAP,
+	WMI_TLV_SERVICE_AP_PS_DETECT_OUT_OF_SYNC,
+	WMI_TLV_SERVICE_EARLY_RX,
+	WMI_TLV_SERVICE_STA_SMPS,
+	WMI_TLV_SERVICE_FWTEST,
+	WMI_TLV_SERVICE_STA_WMMAC,
+	WMI_TLV_SERVICE_TDLS,
+	WMI_TLV_SERVICE_BURST,
+	WMI_TLV_SERVICE_MCC_BCN_INTERVAL_CHANGE,
+	WMI_TLV_SERVICE_ADAPTIVE_OCS,
+	WMI_TLV_SERVICE_BA_SSN_SUPPORT,
+	WMI_TLV_SERVICE_FILTER_IPSEC_NATKEEPALIVE,
+	WMI_TLV_SERVICE_WLAN_HB,
+	WMI_TLV_SERVICE_LTE_ANT_SHARE_SUPPORT,
+	WMI_TLV_SERVICE_BATCH_SCAN,
+	WMI_TLV_SERVICE_QPOWER,
+	WMI_TLV_SERVICE_PLMREQ,
+	WMI_TLV_SERVICE_THERMAL_MGMT,
+	WMI_TLV_SERVICE_RMC,
+	WMI_TLV_SERVICE_MHF_OFFLOAD,
+	WMI_TLV_SERVICE_COEX_SAR,
+	WMI_TLV_SERVICE_BCN_TXRATE_OVERRIDE,
+	WMI_TLV_SERVICE_NAN,
+	WMI_TLV_SERVICE_L1SS_STAT,
+	WMI_TLV_SERVICE_ESTIMATE_LINKSPEED,
+	WMI_TLV_SERVICE_OBSS_SCAN,
+	WMI_TLV_SERVICE_TDLS_OFFCHAN,
+	WMI_TLV_SERVICE_TDLS_UAPSD_BUFFER_STA,
+	WMI_TLV_SERVICE_TDLS_UAPSD_SLEEP_STA,
+	WMI_TLV_SERVICE_IBSS_PWRSAVE,
+	WMI_TLV_SERVICE_LPASS,
+	WMI_TLV_SERVICE_EXTSCAN,
+	WMI_TLV_SERVICE_D0WOW,
+	WMI_TLV_SERVICE_HSOFFLOAD,
+	WMI_TLV_SERVICE_ROAM_HO_OFFLOAD,
+	WMI_TLV_SERVICE_RX_FULL_REORDER,
+	WMI_TLV_SERVICE_DHCP_OFFLOAD,
+	WMI_TLV_SERVICE_STA_RX_IPA_OFFLOAD_SUPPORT,
+	WMI_TLV_SERVICE_MDNS_OFFLOAD,
+	WMI_TLV_SERVICE_SAP_AUTH_OFFLOAD,
+};
+
+#define WMI_SERVICE_IS_ENABLED(wmi_svc_bmap, svc_id, len) \
+	((svc_id) < (len) && \
+	 __le32_to_cpu((wmi_svc_bmap)[(svc_id)/(sizeof(u32))]) & \
+	 BIT((svc_id)%(sizeof(u32))))
+
+#define SVCMAP(x, y, len) \
+	do { \
+		if (WMI_SERVICE_IS_ENABLED((in), (x), (len))) \
+			__set_bit(y, out); \
+	} while (0)
+
+static inline void
+wmi_tlv_svc_map(const __le32 *in, unsigned long *out, size_t len)
+{
+	SVCMAP(WMI_TLV_SERVICE_BEACON_OFFLOAD,
+	       WMI_SERVICE_BEACON_OFFLOAD, len);
+	SVCMAP(WMI_TLV_SERVICE_SCAN_OFFLOAD,
+	       WMI_SERVICE_SCAN_OFFLOAD, len);
+	SVCMAP(WMI_TLV_SERVICE_ROAM_SCAN_OFFLOAD,
+	       WMI_SERVICE_ROAM_SCAN_OFFLOAD, len);
+	SVCMAP(WMI_TLV_SERVICE_BCN_MISS_OFFLOAD,
+	       WMI_SERVICE_BCN_MISS_OFFLOAD, len);
+	SVCMAP(WMI_TLV_SERVICE_STA_PWRSAVE,
+	       WMI_SERVICE_STA_PWRSAVE, len);
+	SVCMAP(WMI_TLV_SERVICE_STA_ADVANCED_PWRSAVE,
+	       WMI_SERVICE_STA_ADVANCED_PWRSAVE, len);
+	SVCMAP(WMI_TLV_SERVICE_AP_UAPSD,
+	       WMI_SERVICE_AP_UAPSD, len);
+	SVCMAP(WMI_TLV_SERVICE_AP_DFS,
+	       WMI_SERVICE_AP_DFS, len);
+	SVCMAP(WMI_TLV_SERVICE_11AC,
+	       WMI_SERVICE_11AC, len);
+	SVCMAP(WMI_TLV_SERVICE_BLOCKACK,
+	       WMI_SERVICE_BLOCKACK, len);
+	SVCMAP(WMI_TLV_SERVICE_PHYERR,
+	       WMI_SERVICE_PHYERR, len);
+	SVCMAP(WMI_TLV_SERVICE_BCN_FILTER,
+	       WMI_SERVICE_BCN_FILTER, len);
+	SVCMAP(WMI_TLV_SERVICE_RTT,
+	       WMI_SERVICE_RTT, len);
+	SVCMAP(WMI_TLV_SERVICE_WOW,
+	       WMI_SERVICE_WOW, len);
+	SVCMAP(WMI_TLV_SERVICE_RATECTRL_CACHE,
+	       WMI_SERVICE_RATECTRL_CACHE, len);
+	SVCMAP(WMI_TLV_SERVICE_IRAM_TIDS,
+	       WMI_SERVICE_IRAM_TIDS, len);
+	SVCMAP(WMI_TLV_SERVICE_ARPNS_OFFLOAD,
+	       WMI_SERVICE_ARPNS_OFFLOAD, len);
+	SVCMAP(WMI_TLV_SERVICE_NLO,
+	       WMI_SERVICE_NLO, len);
+	SVCMAP(WMI_TLV_SERVICE_GTK_OFFLOAD,
+	       WMI_SERVICE_GTK_OFFLOAD, len);
+	SVCMAP(WMI_TLV_SERVICE_SCAN_SCH,
+	       WMI_SERVICE_SCAN_SCH, len);
+	SVCMAP(WMI_TLV_SERVICE_CSA_OFFLOAD,
+	       WMI_SERVICE_CSA_OFFLOAD, len);
+	SVCMAP(WMI_TLV_SERVICE_CHATTER,
+	       WMI_SERVICE_CHATTER, len);
+	SVCMAP(WMI_TLV_SERVICE_COEX_FREQAVOID,
+	       WMI_SERVICE_COEX_FREQAVOID, len);
+	SVCMAP(WMI_TLV_SERVICE_PACKET_POWER_SAVE,
+	       WMI_SERVICE_PACKET_POWER_SAVE, len);
+	SVCMAP(WMI_TLV_SERVICE_FORCE_FW_HANG,
+	       WMI_SERVICE_FORCE_FW_HANG, len);
+	SVCMAP(WMI_TLV_SERVICE_GPIO,
+	       WMI_SERVICE_GPIO, len);
+	SVCMAP(WMI_TLV_SERVICE_STA_DTIM_PS_MODULATED_DTIM,
+	       WMI_SERVICE_STA_DTIM_PS_MODULATED_DTIM, len);
+	SVCMAP(WMI_TLV_SERVICE_STA_UAPSD_BASIC_AUTO_TRIG,
+	       WMI_SERVICE_STA_UAPSD_BASIC_AUTO_TRIG, len);
+	SVCMAP(WMI_TLV_SERVICE_STA_UAPSD_VAR_AUTO_TRIG,
+	       WMI_SERVICE_STA_UAPSD_VAR_AUTO_TRIG, len);
+	SVCMAP(WMI_TLV_SERVICE_STA_KEEP_ALIVE,
+	       WMI_SERVICE_STA_KEEP_ALIVE, len);
+	SVCMAP(WMI_TLV_SERVICE_TX_ENCAP,
+	       WMI_SERVICE_TX_ENCAP, len);
+	SVCMAP(WMI_TLV_SERVICE_AP_PS_DETECT_OUT_OF_SYNC,
+	       WMI_SERVICE_AP_PS_DETECT_OUT_OF_SYNC, len);
+	SVCMAP(WMI_TLV_SERVICE_EARLY_RX,
+	       WMI_SERVICE_EARLY_RX, len);
+	SVCMAP(WMI_TLV_SERVICE_STA_SMPS,
+	       WMI_SERVICE_STA_SMPS, len);
+	SVCMAP(WMI_TLV_SERVICE_FWTEST,
+	       WMI_SERVICE_FWTEST, len);
+	SVCMAP(WMI_TLV_SERVICE_STA_WMMAC,
+	       WMI_SERVICE_STA_WMMAC, len);
+	SVCMAP(WMI_TLV_SERVICE_TDLS,
+	       WMI_SERVICE_TDLS, len);
+	SVCMAP(WMI_TLV_SERVICE_BURST,
+	       WMI_SERVICE_BURST, len);
+	SVCMAP(WMI_TLV_SERVICE_MCC_BCN_INTERVAL_CHANGE,
+	       WMI_SERVICE_MCC_BCN_INTERVAL_CHANGE, len);
+	SVCMAP(WMI_TLV_SERVICE_ADAPTIVE_OCS,
+	       WMI_SERVICE_ADAPTIVE_OCS, len);
+	SVCMAP(WMI_TLV_SERVICE_BA_SSN_SUPPORT,
+	       WMI_SERVICE_BA_SSN_SUPPORT, len);
+	SVCMAP(WMI_TLV_SERVICE_FILTER_IPSEC_NATKEEPALIVE,
+	       WMI_SERVICE_FILTER_IPSEC_NATKEEPALIVE, len);
+	SVCMAP(WMI_TLV_SERVICE_WLAN_HB,
+	       WMI_SERVICE_WLAN_HB, len);
+	SVCMAP(WMI_TLV_SERVICE_LTE_ANT_SHARE_SUPPORT,
+	       WMI_SERVICE_LTE_ANT_SHARE_SUPPORT, len);
+	SVCMAP(WMI_TLV_SERVICE_BATCH_SCAN,
+	       WMI_SERVICE_BATCH_SCAN, len);
+	SVCMAP(WMI_TLV_SERVICE_QPOWER,
+	       WMI_SERVICE_QPOWER, len);
+	SVCMAP(WMI_TLV_SERVICE_PLMREQ,
+	       WMI_SERVICE_PLMREQ, len);
+	SVCMAP(WMI_TLV_SERVICE_THERMAL_MGMT,
+	       WMI_SERVICE_THERMAL_MGMT, len);
+	SVCMAP(WMI_TLV_SERVICE_RMC,
+	       WMI_SERVICE_RMC, len);
+	SVCMAP(WMI_TLV_SERVICE_MHF_OFFLOAD,
+	       WMI_SERVICE_MHF_OFFLOAD, len);
+	SVCMAP(WMI_TLV_SERVICE_COEX_SAR,
+	       WMI_SERVICE_COEX_SAR, len);
+	SVCMAP(WMI_TLV_SERVICE_BCN_TXRATE_OVERRIDE,
+	       WMI_SERVICE_BCN_TXRATE_OVERRIDE, len);
+	SVCMAP(WMI_TLV_SERVICE_NAN,
+	       WMI_SERVICE_NAN, len);
+	SVCMAP(WMI_TLV_SERVICE_L1SS_STAT,
+	       WMI_SERVICE_L1SS_STAT, len);
+	SVCMAP(WMI_TLV_SERVICE_ESTIMATE_LINKSPEED,
+	       WMI_SERVICE_ESTIMATE_LINKSPEED, len);
+	SVCMAP(WMI_TLV_SERVICE_OBSS_SCAN,
+	       WMI_SERVICE_OBSS_SCAN, len);
+	SVCMAP(WMI_TLV_SERVICE_TDLS_OFFCHAN,
+	       WMI_SERVICE_TDLS_OFFCHAN, len);
+	SVCMAP(WMI_TLV_SERVICE_TDLS_UAPSD_BUFFER_STA,
+	       WMI_SERVICE_TDLS_UAPSD_BUFFER_STA, len);
+	SVCMAP(WMI_TLV_SERVICE_TDLS_UAPSD_SLEEP_STA,
+	       WMI_SERVICE_TDLS_UAPSD_SLEEP_STA, len);
+	SVCMAP(WMI_TLV_SERVICE_IBSS_PWRSAVE,
+	       WMI_SERVICE_IBSS_PWRSAVE, len);
+	SVCMAP(WMI_TLV_SERVICE_LPASS,
+	       WMI_SERVICE_LPASS, len);
+	SVCMAP(WMI_TLV_SERVICE_EXTSCAN,
+	       WMI_SERVICE_EXTSCAN, len);
+	SVCMAP(WMI_TLV_SERVICE_D0WOW,
+	       WMI_SERVICE_D0WOW, len);
+	SVCMAP(WMI_TLV_SERVICE_HSOFFLOAD,
+	       WMI_SERVICE_HSOFFLOAD, len);
+	SVCMAP(WMI_TLV_SERVICE_ROAM_HO_OFFLOAD,
+	       WMI_SERVICE_ROAM_HO_OFFLOAD, len);
+	SVCMAP(WMI_TLV_SERVICE_RX_FULL_REORDER,
+	       WMI_SERVICE_RX_FULL_REORDER, len);
+	SVCMAP(WMI_TLV_SERVICE_DHCP_OFFLOAD,
+	       WMI_SERVICE_DHCP_OFFLOAD, len);
+	SVCMAP(WMI_TLV_SERVICE_STA_RX_IPA_OFFLOAD_SUPPORT,
+	       WMI_SERVICE_STA_RX_IPA_OFFLOAD_SUPPORT, len);
+	SVCMAP(WMI_TLV_SERVICE_MDNS_OFFLOAD,
+	       WMI_SERVICE_MDNS_OFFLOAD, len);
+	SVCMAP(WMI_TLV_SERVICE_SAP_AUTH_OFFLOAD,
+	       WMI_SERVICE_SAP_AUTH_OFFLOAD, len);
+}
+
+#undef SVCMAP
+
+struct wmi_tlv {
+	__le16 len;
+	__le16 tag;
+	u8 value[0];
+} __packed;
+
+#define WMI_TLV_MGMT_RX_NUM_RSSI 4
+
+struct wmi_tlv_mgmt_rx_ev {
+	__le32 channel;
+	__le32 snr;
+	__le32 rate;
+	__le32 phy_mode;
+	__le32 buf_len;
+	__le32 status;
+	__le32 rssi[WMI_TLV_MGMT_RX_NUM_RSSI];
+} __packed;
+
+struct wmi_tlv_abi_version {
+	__le32 abi_ver0;
+	__le32 abi_ver1;
+	__le32 abi_ver_ns0;
+	__le32 abi_ver_ns1;
+	__le32 abi_ver_ns2;
+	__le32 abi_ver_ns3;
+} __packed;
+
+enum wmi_tlv_hw_bd_id {
+	WMI_TLV_HW_BD_LEGACY = 0,
+	WMI_TLV_HW_BD_QCA6174 = 1,
+	WMI_TLV_HW_BD_QCA2582 = 2,
+};
+
+struct wmi_tlv_hw_bd_info {
+	u8 rev;
+	u8 project_id;
+	u8 custom_id;
+	u8 reference_design_id;
+} __packed;
+
+struct wmi_tlv_svc_rdy_ev {
+	__le32 fw_build_vers;
+	struct wmi_tlv_abi_version abi;
+	__le32 phy_capability;
+	__le32 max_frag_entry;
+	__le32 num_rf_chains;
+	__le32 ht_cap_info;
+	__le32 vht_cap_info;
+	__le32 vht_supp_mcs;
+	__le32 hw_min_tx_power;
+	__le32 hw_max_tx_power;
+	__le32 sys_cap_info;
+	__le32 min_pkt_size_enable;
+	__le32 max_bcn_ie_size;
+	__le32 num_mem_reqs;
+	__le32 max_num_scan_chans;
+	__le32 hw_bd_id; /* 0 means hw_bd_info is invalid */
+	struct wmi_tlv_hw_bd_info hw_bd_info[5];
+} __packed;
+
+struct wmi_tlv_rdy_ev {
+	struct wmi_tlv_abi_version abi;
+	struct wmi_mac_addr mac_addr;
+	__le32 status;
+} __packed;
+
+struct wmi_tlv_resource_config {
+	__le32 num_vdevs;
+	__le32 num_peers;
+	__le32 num_offload_peers;
+	__le32 num_offload_reorder_bufs;
+	__le32 num_peer_keys;
+	__le32 num_tids;
+	__le32 ast_skid_limit;
+	__le32 tx_chain_mask;
+	__le32 rx_chain_mask;
+	__le32 rx_timeout_pri[4];
+	__le32 rx_decap_mode;
+	__le32 scan_max_pending_reqs;
+	__le32 bmiss_offload_max_vdev;
+	__le32 roam_offload_max_vdev;
+	__le32 roam_offload_max_ap_profiles;
+	__le32 num_mcast_groups;
+	__le32 num_mcast_table_elems;
+	__le32 mcast2ucast_mode;
+	__le32 tx_dbg_log_size;
+	__le32 num_wds_entries;
+	__le32 dma_burst_size;
+	__le32 mac_aggr_delim;
+	__le32 rx_skip_defrag_timeout_dup_detection_check;
+	__le32 vow_config;
+	__le32 gtk_offload_max_vdev;
+	__le32 num_msdu_desc;
+	__le32 max_frag_entries;
+	__le32 num_tdls_vdevs;
+	__le32 num_tdls_conn_table_entries;
+	__le32 beacon_tx_offload_max_vdev;
+	__le32 num_multicast_filter_entries;
+	__le32 num_wow_filters;
+	__le32 num_keep_alive_pattern;
+	__le32 keep_alive_pattern_size;
+	__le32 max_tdls_concurrent_sleep_sta;
+	__le32 max_tdls_concurrent_buffer_sta;
+} __packed;
+
+struct wmi_tlv_init_cmd {
+	struct wmi_tlv_abi_version abi;
+	__le32 num_host_mem_chunks;
+} __packed;
+
+struct wmi_tlv_pdev_set_param_cmd {
+	__le32 pdev_id; /* not used yet */
+	__le32 param_id;
+	__le32 param_value;
+} __packed;
+
+struct wmi_tlv_pdev_set_rd_cmd {
+	__le32 pdev_id; /* not used yet */
+	__le32 regd;
+	__le32 regd_2ghz;
+	__le32 regd_5ghz;
+	__le32 conform_limit_2ghz;
+	__le32 conform_limit_5ghz;
+} __packed;
+
+struct wmi_tlv_scan_chan_list_cmd {
+	__le32 num_scan_chans;
+} __packed;
+
+struct wmi_tlv_start_scan_cmd {
+	struct wmi_start_scan_common common;
+	__le32 burst_duration_ms;
+	__le32 num_channels;
+	__le32 num_bssids;
+	__le32 num_ssids;
+	__le32 ie_len;
+	__le32 num_probes;
+} __packed;
+
+struct wmi_tlv_vdev_start_cmd {
+	__le32 vdev_id;
+	__le32 requestor_id;
+	__le32 bcn_intval;
+	__le32 dtim_period;
+	__le32 flags;
+	struct wmi_ssid ssid;
+	__le32 bcn_tx_rate;
+	__le32 bcn_tx_power;
+	__le32 num_noa_descr;
+	__le32 disable_hw_ack;
+} __packed;
+
+enum {
+	WMI_TLV_PEER_TYPE_DEFAULT = 0, /* generic / non-BSS / self-peer */
+	WMI_TLV_PEER_TYPE_BSS = 1,
+	WMI_TLV_PEER_TYPE_TDLS = 2,
+	WMI_TLV_PEER_TYPE_HOST_MAX = 127,
+	WMI_TLV_PEER_TYPE_ROAMOFFLOAD_TMP = 128,
+};
+
+struct wmi_tlv_peer_create_cmd {
+	__le32 vdev_id;
+	struct wmi_mac_addr peer_addr;
+	__le32 peer_type;
+} __packed;
+
+struct wmi_tlv_peer_assoc_cmd {
+	struct wmi_mac_addr mac_addr;
+	__le32 vdev_id;
+	__le32 new_assoc;
+	__le32 assoc_id;
+	__le32 flags;
+	__le32 caps;
+	__le32 listen_intval;
+	__le32 ht_caps;
+	__le32 max_mpdu;
+	__le32 mpdu_density;
+	__le32 rate_caps;
+	__le32 nss;
+	__le32 vht_caps;
+	__le32 phy_mode;
+	__le32 ht_info[2];
+	__le32 num_legacy_rates;
+	__le32 num_ht_rates;
+} __packed;
+
+struct wmi_tlv_pdev_suspend {
+	__le32 pdev_id; /* not used yet */
+	__le32 opt;
+} __packed;
+
+struct wmi_tlv_pdev_set_wmm_cmd {
+	__le32 pdev_id; /* not used yet */
+	__le32 dg_type; /* no idea.. */
+} __packed;
+
+struct wmi_tlv_phyerr_ev {
+	__le32 num_phyerrs;
+	__le32 tsf_l32;
+	__le32 tsf_u32;
+	__le32 buf_len;
+} __packed;
+
+enum wmi_tlv_dbglog_param {
+	WMI_TLV_DBGLOG_PARAM_LOG_LEVEL = 1,
+	WMI_TLV_DBGLOG_PARAM_VDEV_ENABLE,
+	WMI_TLV_DBGLOG_PARAM_VDEV_DISABLE,
+	WMI_TLV_DBGLOG_PARAM_VDEV_ENABLE_BITMAP,
+	WMI_TLV_DBGLOG_PARAM_VDEV_DISABLE_BITMAP,
+};
+
+enum wmi_tlv_dbglog_log_level {
+	WMI_TLV_DBGLOG_LOG_LEVEL_VERBOSE = 0,
+	WMI_TLV_DBGLOG_LOG_LEVEL_INFO,
+	WMI_TLV_DBGLOG_LOG_LEVEL_INFO_LVL_1,
+	WMI_TLV_DBGLOG_LOG_LEVEL_INFO_LVL_2,
+	WMI_TLV_DBGLOG_LOG_LEVEL_WARN,
+	WMI_TLV_DBGLOG_LOG_LEVEL_ERR,
+};
+
+#define WMI_TLV_DBGLOG_BITMAP_MAX_IDS 512
+#define WMI_TLV_DBGLOG_BITMAP_MAX_WORDS (WMI_TLV_DBGLOG_BITMAP_MAX_IDS / \
+					 sizeof(__le32))
+#define WMI_TLV_DBGLOG_ALL_MODULES 0xffff
+#define WMI_TLV_DBGLOG_LOG_LEVEL_VALUE(module_id, log_level) \
+		(((module_id << 16) & 0xffff0000) | \
+		 ((log_level <<  0) & 0x000000ff))
+
+struct wmi_tlv_dbglog_cmd {
+	__le32 param;
+	__le32 value;
+} __packed;
+
+struct wmi_tlv_resume_cmd {
+	__le32 reserved;
+} __packed;
+
+struct wmi_tlv_req_stats_cmd {
+	__le32 stats_id; /* wmi_stats_id */
+	__le32 vdev_id;
+	struct wmi_mac_addr peer_macaddr;
+} __packed;
+
+struct wmi_tlv_vdev_stats {
+	__le32 vdev_id;
+	__le32 beacon_snr;
+	__le32 data_snr;
+	__le32 num_tx_frames[4]; /* per-AC */
+	__le32 num_rx_frames;
+	__le32 num_tx_frames_retries[4];
+	__le32 num_tx_frames_failures[4];
+	__le32 num_rts_fail;
+	__le32 num_rts_success;
+	__le32 num_rx_err;
+	__le32 num_rx_discard;
+	__le32 num_tx_not_acked;
+	__le32 tx_rate_history[10];
+	__le32 beacon_rssi_history[10];
+} __packed;
+
+struct wmi_tlv_pktlog_enable {
+	__le32 reserved;
+	__le32 filter;
+} __packed;
+
+struct wmi_tlv_pktlog_disable {
+	__le32 reserved;
+} __packed;
+
+void ath10k_wmi_tlv_attach(struct ath10k *ar);
+
+#endif
diff --git a/drivers/net/wireless/ath/ath10k/wmi.c b/drivers/net/wireless/ath/ath10k/wmi.c
index 721631c3dd3e..23eca8bc85d1 100644
--- a/drivers/net/wireless/ath/ath10k/wmi.c
+++ b/drivers/net/wireless/ath/ath10k/wmi.c
@@ -22,8 +22,10 @@
 #include "htc.h"
 #include "debug.h"
 #include "wmi.h"
+#include "wmi-tlv.h"
 #include "mac.h"
 #include "testmode.h"
+#include "wmi-ops.h"
 
 /* MAIN WMI cmd track */
 static struct wmi_cmd_map wmi_cmd_map = {
@@ -143,6 +145,7 @@ static struct wmi_cmd_map wmi_cmd_map = {
 	.force_fw_hang_cmdid = WMI_FORCE_FW_HANG_CMDID,
 	.gpio_config_cmdid = WMI_GPIO_CONFIG_CMDID,
 	.gpio_output_cmdid = WMI_GPIO_OUTPUT_CMDID,
+	.pdev_get_temperature_cmdid = WMI_CMD_UNSUPPORTED,
 };
 
 /* 10.X WMI cmd track */
@@ -265,6 +268,129 @@ static struct wmi_cmd_map wmi_10x_cmd_map = {
 	.force_fw_hang_cmdid = WMI_CMD_UNSUPPORTED,
 	.gpio_config_cmdid = WMI_10X_GPIO_CONFIG_CMDID,
 	.gpio_output_cmdid = WMI_10X_GPIO_OUTPUT_CMDID,
+	.pdev_get_temperature_cmdid = WMI_CMD_UNSUPPORTED,
+};
+
+/* 10.2.4 WMI cmd track */
+static struct wmi_cmd_map wmi_10_2_4_cmd_map = {
+	.init_cmdid = WMI_10_2_INIT_CMDID,
+	.start_scan_cmdid = WMI_10_2_START_SCAN_CMDID,
+	.stop_scan_cmdid = WMI_10_2_STOP_SCAN_CMDID,
+	.scan_chan_list_cmdid = WMI_10_2_SCAN_CHAN_LIST_CMDID,
+	.scan_sch_prio_tbl_cmdid = WMI_CMD_UNSUPPORTED,
+	.pdev_set_regdomain_cmdid = WMI_10_2_PDEV_SET_REGDOMAIN_CMDID,
+	.pdev_set_channel_cmdid = WMI_10_2_PDEV_SET_CHANNEL_CMDID,
+	.pdev_set_param_cmdid = WMI_10_2_PDEV_SET_PARAM_CMDID,
+	.pdev_pktlog_enable_cmdid = WMI_10_2_PDEV_PKTLOG_ENABLE_CMDID,
+	.pdev_pktlog_disable_cmdid = WMI_10_2_PDEV_PKTLOG_DISABLE_CMDID,
+	.pdev_set_wmm_params_cmdid = WMI_10_2_PDEV_SET_WMM_PARAMS_CMDID,
+	.pdev_set_ht_cap_ie_cmdid = WMI_10_2_PDEV_SET_HT_CAP_IE_CMDID,
+	.pdev_set_vht_cap_ie_cmdid = WMI_10_2_PDEV_SET_VHT_CAP_IE_CMDID,
+	.pdev_set_quiet_mode_cmdid = WMI_10_2_PDEV_SET_QUIET_MODE_CMDID,
+	.pdev_green_ap_ps_enable_cmdid = WMI_10_2_PDEV_GREEN_AP_PS_ENABLE_CMDID,
+	.pdev_get_tpc_config_cmdid = WMI_10_2_PDEV_GET_TPC_CONFIG_CMDID,
+	.pdev_set_base_macaddr_cmdid = WMI_10_2_PDEV_SET_BASE_MACADDR_CMDID,
+	.vdev_create_cmdid = WMI_10_2_VDEV_CREATE_CMDID,
+	.vdev_delete_cmdid = WMI_10_2_VDEV_DELETE_CMDID,
+	.vdev_start_request_cmdid = WMI_10_2_VDEV_START_REQUEST_CMDID,
+	.vdev_restart_request_cmdid = WMI_10_2_VDEV_RESTART_REQUEST_CMDID,
+	.vdev_up_cmdid = WMI_10_2_VDEV_UP_CMDID,
+	.vdev_stop_cmdid = WMI_10_2_VDEV_STOP_CMDID,
+	.vdev_down_cmdid = WMI_10_2_VDEV_DOWN_CMDID,
+	.vdev_set_param_cmdid = WMI_10_2_VDEV_SET_PARAM_CMDID,
+	.vdev_install_key_cmdid = WMI_10_2_VDEV_INSTALL_KEY_CMDID,
+	.peer_create_cmdid = WMI_10_2_PEER_CREATE_CMDID,
+	.peer_delete_cmdid = WMI_10_2_PEER_DELETE_CMDID,
+	.peer_flush_tids_cmdid = WMI_10_2_PEER_FLUSH_TIDS_CMDID,
+	.peer_set_param_cmdid = WMI_10_2_PEER_SET_PARAM_CMDID,
+	.peer_assoc_cmdid = WMI_10_2_PEER_ASSOC_CMDID,
+	.peer_add_wds_entry_cmdid = WMI_10_2_PEER_ADD_WDS_ENTRY_CMDID,
+	.peer_remove_wds_entry_cmdid = WMI_10_2_PEER_REMOVE_WDS_ENTRY_CMDID,
+	.peer_mcast_group_cmdid = WMI_10_2_PEER_MCAST_GROUP_CMDID,
+	.bcn_tx_cmdid = WMI_10_2_BCN_TX_CMDID,
+	.pdev_send_bcn_cmdid = WMI_10_2_PDEV_SEND_BCN_CMDID,
+	.bcn_tmpl_cmdid = WMI_CMD_UNSUPPORTED,
+	.bcn_filter_rx_cmdid = WMI_10_2_BCN_FILTER_RX_CMDID,
+	.prb_req_filter_rx_cmdid = WMI_10_2_PRB_REQ_FILTER_RX_CMDID,
+	.mgmt_tx_cmdid = WMI_10_2_MGMT_TX_CMDID,
+	.prb_tmpl_cmdid = WMI_CMD_UNSUPPORTED,
+	.addba_clear_resp_cmdid = WMI_10_2_ADDBA_CLEAR_RESP_CMDID,
+	.addba_send_cmdid = WMI_10_2_ADDBA_SEND_CMDID,
+	.addba_status_cmdid = WMI_10_2_ADDBA_STATUS_CMDID,
+	.delba_send_cmdid = WMI_10_2_DELBA_SEND_CMDID,
+	.addba_set_resp_cmdid = WMI_10_2_ADDBA_SET_RESP_CMDID,
+	.send_singleamsdu_cmdid = WMI_10_2_SEND_SINGLEAMSDU_CMDID,
+	.sta_powersave_mode_cmdid = WMI_10_2_STA_POWERSAVE_MODE_CMDID,
+	.sta_powersave_param_cmdid = WMI_10_2_STA_POWERSAVE_PARAM_CMDID,
+	.sta_mimo_ps_mode_cmdid = WMI_10_2_STA_MIMO_PS_MODE_CMDID,
+	.pdev_dfs_enable_cmdid = WMI_10_2_PDEV_DFS_ENABLE_CMDID,
+	.pdev_dfs_disable_cmdid = WMI_10_2_PDEV_DFS_DISABLE_CMDID,
+	.roam_scan_mode = WMI_10_2_ROAM_SCAN_MODE,
+	.roam_scan_rssi_threshold = WMI_10_2_ROAM_SCAN_RSSI_THRESHOLD,
+	.roam_scan_period = WMI_10_2_ROAM_SCAN_PERIOD,
+	.roam_scan_rssi_change_threshold =
+				WMI_10_2_ROAM_SCAN_RSSI_CHANGE_THRESHOLD,
+	.roam_ap_profile = WMI_10_2_ROAM_AP_PROFILE,
+	.ofl_scan_add_ap_profile = WMI_10_2_OFL_SCAN_ADD_AP_PROFILE,
+	.ofl_scan_remove_ap_profile = WMI_10_2_OFL_SCAN_REMOVE_AP_PROFILE,
+	.ofl_scan_period = WMI_10_2_OFL_SCAN_PERIOD,
+	.p2p_dev_set_device_info = WMI_10_2_P2P_DEV_SET_DEVICE_INFO,
+	.p2p_dev_set_discoverability = WMI_10_2_P2P_DEV_SET_DISCOVERABILITY,
+	.p2p_go_set_beacon_ie = WMI_10_2_P2P_GO_SET_BEACON_IE,
+	.p2p_go_set_probe_resp_ie = WMI_10_2_P2P_GO_SET_PROBE_RESP_IE,
+	.p2p_set_vendor_ie_data_cmdid = WMI_CMD_UNSUPPORTED,
+	.ap_ps_peer_param_cmdid = WMI_10_2_AP_PS_PEER_PARAM_CMDID,
+	.ap_ps_peer_uapsd_coex_cmdid = WMI_CMD_UNSUPPORTED,
+	.peer_rate_retry_sched_cmdid = WMI_10_2_PEER_RATE_RETRY_SCHED_CMDID,
+	.wlan_profile_trigger_cmdid = WMI_10_2_WLAN_PROFILE_TRIGGER_CMDID,
+	.wlan_profile_set_hist_intvl_cmdid =
+				WMI_10_2_WLAN_PROFILE_SET_HIST_INTVL_CMDID,
+	.wlan_profile_get_profile_data_cmdid =
+				WMI_10_2_WLAN_PROFILE_GET_PROFILE_DATA_CMDID,
+	.wlan_profile_enable_profile_id_cmdid =
+				WMI_10_2_WLAN_PROFILE_ENABLE_PROFILE_ID_CMDID,
+	.wlan_profile_list_profile_id_cmdid =
+				WMI_10_2_WLAN_PROFILE_LIST_PROFILE_ID_CMDID,
+	.pdev_suspend_cmdid = WMI_10_2_PDEV_SUSPEND_CMDID,
+	.pdev_resume_cmdid = WMI_10_2_PDEV_RESUME_CMDID,
+	.add_bcn_filter_cmdid = WMI_10_2_ADD_BCN_FILTER_CMDID,
+	.rmv_bcn_filter_cmdid = WMI_10_2_RMV_BCN_FILTER_CMDID,
+	.wow_add_wake_pattern_cmdid = WMI_10_2_WOW_ADD_WAKE_PATTERN_CMDID,
+	.wow_del_wake_pattern_cmdid = WMI_10_2_WOW_DEL_WAKE_PATTERN_CMDID,
+	.wow_enable_disable_wake_event_cmdid =
+				WMI_10_2_WOW_ENABLE_DISABLE_WAKE_EVENT_CMDID,
+	.wow_enable_cmdid = WMI_10_2_WOW_ENABLE_CMDID,
+	.wow_hostwakeup_from_sleep_cmdid =
+				WMI_10_2_WOW_HOSTWAKEUP_FROM_SLEEP_CMDID,
+	.rtt_measreq_cmdid = WMI_10_2_RTT_MEASREQ_CMDID,
+	.rtt_tsf_cmdid = WMI_10_2_RTT_TSF_CMDID,
+	.vdev_spectral_scan_configure_cmdid =
+				WMI_10_2_VDEV_SPECTRAL_SCAN_CONFIGURE_CMDID,
+	.vdev_spectral_scan_enable_cmdid =
+				WMI_10_2_VDEV_SPECTRAL_SCAN_ENABLE_CMDID,
+	.request_stats_cmdid = WMI_10_2_REQUEST_STATS_CMDID,
+	.set_arp_ns_offload_cmdid = WMI_CMD_UNSUPPORTED,
+	.network_list_offload_config_cmdid = WMI_CMD_UNSUPPORTED,
+	.gtk_offload_cmdid = WMI_CMD_UNSUPPORTED,
+	.csa_offload_enable_cmdid = WMI_CMD_UNSUPPORTED,
+	.csa_offload_chanswitch_cmdid = WMI_CMD_UNSUPPORTED,
+	.chatter_set_mode_cmdid = WMI_CMD_UNSUPPORTED,
+	.peer_tid_addba_cmdid = WMI_CMD_UNSUPPORTED,
+	.peer_tid_delba_cmdid = WMI_CMD_UNSUPPORTED,
+	.sta_dtim_ps_method_cmdid = WMI_CMD_UNSUPPORTED,
+	.sta_uapsd_auto_trig_cmdid = WMI_CMD_UNSUPPORTED,
+	.sta_keepalive_cmd = WMI_CMD_UNSUPPORTED,
+	.echo_cmdid = WMI_10_2_ECHO_CMDID,
+	.pdev_utf_cmdid = WMI_10_2_PDEV_UTF_CMDID,
+	.dbglog_cfg_cmdid = WMI_10_2_DBGLOG_CFG_CMDID,
+	.pdev_qvit_cmdid = WMI_10_2_PDEV_QVIT_CMDID,
+	.pdev_ftm_intg_cmdid = WMI_CMD_UNSUPPORTED,
+	.vdev_set_keepalive_cmdid = WMI_CMD_UNSUPPORTED,
+	.vdev_get_keepalive_cmdid = WMI_CMD_UNSUPPORTED,
+	.force_fw_hang_cmdid = WMI_CMD_UNSUPPORTED,
+	.gpio_config_cmdid = WMI_10_2_GPIO_CONFIG_CMDID,
+	.gpio_output_cmdid = WMI_10_2_GPIO_OUTPUT_CMDID,
+	.pdev_get_temperature_cmdid = WMI_10_2_PDEV_GET_TEMPERATURE_CMDID,
 };
 
 /* MAIN WMI VDEV param map */
@@ -385,6 +511,64 @@ static struct wmi_vdev_param_map wmi_10x_vdev_param_map = {
 		WMI_10X_VDEV_PARAM_AP_DETECT_OUT_OF_SYNC_SLEEPING_STA_TIME_SECS,
 };
 
+static struct wmi_vdev_param_map wmi_10_2_4_vdev_param_map = {
+	.rts_threshold = WMI_10X_VDEV_PARAM_RTS_THRESHOLD,
+	.fragmentation_threshold = WMI_10X_VDEV_PARAM_FRAGMENTATION_THRESHOLD,
+	.beacon_interval = WMI_10X_VDEV_PARAM_BEACON_INTERVAL,
+	.listen_interval = WMI_10X_VDEV_PARAM_LISTEN_INTERVAL,
+	.multicast_rate = WMI_10X_VDEV_PARAM_MULTICAST_RATE,
+	.mgmt_tx_rate = WMI_10X_VDEV_PARAM_MGMT_TX_RATE,
+	.slot_time = WMI_10X_VDEV_PARAM_SLOT_TIME,
+	.preamble = WMI_10X_VDEV_PARAM_PREAMBLE,
+	.swba_time = WMI_10X_VDEV_PARAM_SWBA_TIME,
+	.wmi_vdev_stats_update_period = WMI_10X_VDEV_STATS_UPDATE_PERIOD,
+	.wmi_vdev_pwrsave_ageout_time = WMI_10X_VDEV_PWRSAVE_AGEOUT_TIME,
+	.wmi_vdev_host_swba_interval = WMI_10X_VDEV_HOST_SWBA_INTERVAL,
+	.dtim_period = WMI_10X_VDEV_PARAM_DTIM_PERIOD,
+	.wmi_vdev_oc_scheduler_air_time_limit =
+				WMI_10X_VDEV_OC_SCHEDULER_AIR_TIME_LIMIT,
+	.wds = WMI_10X_VDEV_PARAM_WDS,
+	.atim_window = WMI_10X_VDEV_PARAM_ATIM_WINDOW,
+	.bmiss_count_max = WMI_10X_VDEV_PARAM_BMISS_COUNT_MAX,
+	.bmiss_first_bcnt = WMI_VDEV_PARAM_UNSUPPORTED,
+	.bmiss_final_bcnt = WMI_VDEV_PARAM_UNSUPPORTED,
+	.feature_wmm = WMI_10X_VDEV_PARAM_FEATURE_WMM,
+	.chwidth = WMI_10X_VDEV_PARAM_CHWIDTH,
+	.chextoffset = WMI_10X_VDEV_PARAM_CHEXTOFFSET,
+	.disable_htprotection = WMI_10X_VDEV_PARAM_DISABLE_HTPROTECTION,
+	.sta_quickkickout = WMI_10X_VDEV_PARAM_STA_QUICKKICKOUT,
+	.mgmt_rate = WMI_10X_VDEV_PARAM_MGMT_RATE,
+	.protection_mode = WMI_10X_VDEV_PARAM_PROTECTION_MODE,
+	.fixed_rate = WMI_10X_VDEV_PARAM_FIXED_RATE,
+	.sgi = WMI_10X_VDEV_PARAM_SGI,
+	.ldpc = WMI_10X_VDEV_PARAM_LDPC,
+	.tx_stbc = WMI_10X_VDEV_PARAM_TX_STBC,
+	.rx_stbc = WMI_10X_VDEV_PARAM_RX_STBC,
+	.intra_bss_fwd = WMI_10X_VDEV_PARAM_INTRA_BSS_FWD,
+	.def_keyid = WMI_10X_VDEV_PARAM_DEF_KEYID,
+	.nss = WMI_10X_VDEV_PARAM_NSS,
+	.bcast_data_rate = WMI_10X_VDEV_PARAM_BCAST_DATA_RATE,
+	.mcast_data_rate = WMI_10X_VDEV_PARAM_MCAST_DATA_RATE,
+	.mcast_indicate = WMI_10X_VDEV_PARAM_MCAST_INDICATE,
+	.dhcp_indicate = WMI_10X_VDEV_PARAM_DHCP_INDICATE,
+	.unknown_dest_indicate = WMI_10X_VDEV_PARAM_UNKNOWN_DEST_INDICATE,
+	.ap_keepalive_min_idle_inactive_time_secs =
+		WMI_10X_VDEV_PARAM_AP_KEEPALIVE_MIN_IDLE_INACTIVE_TIME_SECS,
+	.ap_keepalive_max_idle_inactive_time_secs =
+		WMI_10X_VDEV_PARAM_AP_KEEPALIVE_MAX_IDLE_INACTIVE_TIME_SECS,
+	.ap_keepalive_max_unresponsive_time_secs =
+		WMI_10X_VDEV_PARAM_AP_KEEPALIVE_MAX_UNRESPONSIVE_TIME_SECS,
+	.ap_enable_nawds = WMI_10X_VDEV_PARAM_AP_ENABLE_NAWDS,
+	.mcast2ucast_set = WMI_10X_VDEV_PARAM_MCAST2UCAST_SET,
+	.enable_rtscts = WMI_10X_VDEV_PARAM_ENABLE_RTSCTS,
+	.txbf = WMI_VDEV_PARAM_UNSUPPORTED,
+	.packet_powersave = WMI_VDEV_PARAM_UNSUPPORTED,
+	.drop_unencry = WMI_VDEV_PARAM_UNSUPPORTED,
+	.tx_encap_type = WMI_VDEV_PARAM_UNSUPPORTED,
+	.ap_detect_out_of_sync_sleeping_sta_time_secs =
+		WMI_10X_VDEV_PARAM_AP_DETECT_OUT_OF_SYNC_SLEEPING_STA_TIME_SECS,
+};
+
 static struct wmi_pdev_param_map wmi_pdev_param_map = {
 	.tx_chain_mask = WMI_PDEV_PARAM_TX_CHAIN_MASK,
 	.rx_chain_mask = WMI_PDEV_PARAM_RX_CHAIN_MASK,
@@ -434,6 +618,7 @@ static struct wmi_pdev_param_map wmi_pdev_param_map = {
 	.fast_channel_reset = WMI_PDEV_PARAM_UNSUPPORTED,
 	.burst_dur = WMI_PDEV_PARAM_UNSUPPORTED,
 	.burst_enable = WMI_PDEV_PARAM_UNSUPPORTED,
+	.cal_period = WMI_PDEV_PARAM_UNSUPPORTED,
 };
 
 static struct wmi_pdev_param_map wmi_10x_pdev_param_map = {
@@ -486,6 +671,60 @@ static struct wmi_pdev_param_map wmi_10x_pdev_param_map = {
 	.fast_channel_reset = WMI_10X_PDEV_PARAM_FAST_CHANNEL_RESET,
 	.burst_dur = WMI_10X_PDEV_PARAM_BURST_DUR,
 	.burst_enable = WMI_10X_PDEV_PARAM_BURST_ENABLE,
+	.cal_period = WMI_10X_PDEV_PARAM_CAL_PERIOD,
+};
+
+static struct wmi_pdev_param_map wmi_10_2_4_pdev_param_map = {
+	.tx_chain_mask = WMI_10X_PDEV_PARAM_TX_CHAIN_MASK,
+	.rx_chain_mask = WMI_10X_PDEV_PARAM_RX_CHAIN_MASK,
+	.txpower_limit2g = WMI_10X_PDEV_PARAM_TXPOWER_LIMIT2G,
+	.txpower_limit5g = WMI_10X_PDEV_PARAM_TXPOWER_LIMIT5G,
+	.txpower_scale = WMI_10X_PDEV_PARAM_TXPOWER_SCALE,
+	.beacon_gen_mode = WMI_10X_PDEV_PARAM_BEACON_GEN_MODE,
+	.beacon_tx_mode = WMI_10X_PDEV_PARAM_BEACON_TX_MODE,
+	.resmgr_offchan_mode = WMI_10X_PDEV_PARAM_RESMGR_OFFCHAN_MODE,
+	.protection_mode = WMI_10X_PDEV_PARAM_PROTECTION_MODE,
+	.dynamic_bw = WMI_10X_PDEV_PARAM_DYNAMIC_BW,
+	.non_agg_sw_retry_th = WMI_10X_PDEV_PARAM_NON_AGG_SW_RETRY_TH,
+	.agg_sw_retry_th = WMI_10X_PDEV_PARAM_AGG_SW_RETRY_TH,
+	.sta_kickout_th = WMI_10X_PDEV_PARAM_STA_KICKOUT_TH,
+	.ac_aggrsize_scaling = WMI_10X_PDEV_PARAM_AC_AGGRSIZE_SCALING,
+	.ltr_enable = WMI_10X_PDEV_PARAM_LTR_ENABLE,
+	.ltr_ac_latency_be = WMI_10X_PDEV_PARAM_LTR_AC_LATENCY_BE,
+	.ltr_ac_latency_bk = WMI_10X_PDEV_PARAM_LTR_AC_LATENCY_BK,
+	.ltr_ac_latency_vi = WMI_10X_PDEV_PARAM_LTR_AC_LATENCY_VI,
+	.ltr_ac_latency_vo = WMI_10X_PDEV_PARAM_LTR_AC_LATENCY_VO,
+	.ltr_ac_latency_timeout = WMI_10X_PDEV_PARAM_LTR_AC_LATENCY_TIMEOUT,
+	.ltr_sleep_override = WMI_10X_PDEV_PARAM_LTR_SLEEP_OVERRIDE,
+	.ltr_rx_override = WMI_10X_PDEV_PARAM_LTR_RX_OVERRIDE,
+	.ltr_tx_activity_timeout = WMI_10X_PDEV_PARAM_LTR_TX_ACTIVITY_TIMEOUT,
+	.l1ss_enable = WMI_10X_PDEV_PARAM_L1SS_ENABLE,
+	.dsleep_enable = WMI_10X_PDEV_PARAM_DSLEEP_ENABLE,
+	.pcielp_txbuf_flush = WMI_PDEV_PARAM_UNSUPPORTED,
+	.pcielp_txbuf_watermark = WMI_PDEV_PARAM_UNSUPPORTED,
+	.pcielp_txbuf_tmo_en = WMI_PDEV_PARAM_UNSUPPORTED,
+	.pcielp_txbuf_tmo_value = WMI_PDEV_PARAM_UNSUPPORTED,
+	.pdev_stats_update_period = WMI_10X_PDEV_PARAM_PDEV_STATS_UPDATE_PERIOD,
+	.vdev_stats_update_period = WMI_10X_PDEV_PARAM_VDEV_STATS_UPDATE_PERIOD,
+	.peer_stats_update_period = WMI_10X_PDEV_PARAM_PEER_STATS_UPDATE_PERIOD,
+	.bcnflt_stats_update_period =
+				WMI_10X_PDEV_PARAM_BCNFLT_STATS_UPDATE_PERIOD,
+	.pmf_qos = WMI_10X_PDEV_PARAM_PMF_QOS,
+	.arp_ac_override = WMI_10X_PDEV_PARAM_ARPDHCP_AC_OVERRIDE,
+	.dcs = WMI_10X_PDEV_PARAM_DCS,
+	.ani_enable = WMI_10X_PDEV_PARAM_ANI_ENABLE,
+	.ani_poll_period = WMI_10X_PDEV_PARAM_ANI_POLL_PERIOD,
+	.ani_listen_period = WMI_10X_PDEV_PARAM_ANI_LISTEN_PERIOD,
+	.ani_ofdm_level = WMI_10X_PDEV_PARAM_ANI_OFDM_LEVEL,
+	.ani_cck_level = WMI_10X_PDEV_PARAM_ANI_CCK_LEVEL,
+	.dyntxchain = WMI_10X_PDEV_PARAM_DYNTXCHAIN,
+	.proxy_sta = WMI_PDEV_PARAM_UNSUPPORTED,
+	.idle_ps_config = WMI_PDEV_PARAM_UNSUPPORTED,
+	.power_gating_sleep = WMI_PDEV_PARAM_UNSUPPORTED,
+	.fast_channel_reset = WMI_10X_PDEV_PARAM_FAST_CHANNEL_RESET,
+	.burst_dur = WMI_10X_PDEV_PARAM_BURST_DUR,
+	.burst_enable = WMI_10X_PDEV_PARAM_BURST_ENABLE,
+	.cal_period = WMI_10X_PDEV_PARAM_CAL_PERIOD,
 };
 
 /* firmware 10.2 specific mappings */
@@ -607,11 +846,11 @@ static struct wmi_cmd_map wmi_10_2_cmd_map = {
 	.force_fw_hang_cmdid = WMI_CMD_UNSUPPORTED,
 	.gpio_config_cmdid = WMI_10_2_GPIO_CONFIG_CMDID,
 	.gpio_output_cmdid = WMI_10_2_GPIO_OUTPUT_CMDID,
+	.pdev_get_temperature_cmdid = WMI_CMD_UNSUPPORTED,
 };
 
-static void
-ath10k_wmi_put_wmi_channel(struct wmi_channel *ch,
-			   const struct wmi_channel_arg *arg)
+void ath10k_wmi_put_wmi_channel(struct wmi_channel *ch,
+				const struct wmi_channel_arg *arg)
 {
 	u32 flags = 0;
 
@@ -685,8 +924,8 @@ static void ath10k_wmi_htc_tx_complete(struct ath10k *ar, struct sk_buff *skb)
 	dev_kfree_skb(skb);
 }
 
-static int ath10k_wmi_cmd_send_nowait(struct ath10k *ar, struct sk_buff *skb,
-				      u32 cmd_id)
+int ath10k_wmi_cmd_send_nowait(struct ath10k *ar, struct sk_buff *skb,
+			       u32 cmd_id)
 {
 	struct ath10k_skb_cb *skb_cb = ATH10K_SKB_CB(skb);
 	struct wmi_cmd_hdr *cmd_hdr;
@@ -792,24 +1031,23 @@ int ath10k_wmi_cmd_send(struct ath10k *ar, struct sk_buff *skb, u32 cmd_id)
 	return ret;
 }
 
-int ath10k_wmi_mgmt_tx(struct ath10k *ar, struct sk_buff *skb)
+static struct sk_buff *
+ath10k_wmi_op_gen_mgmt_tx(struct ath10k *ar, struct sk_buff *msdu)
 {
-	int ret = 0;
 	struct wmi_mgmt_tx_cmd *cmd;
 	struct ieee80211_hdr *hdr;
-	struct sk_buff *wmi_skb;
-	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct sk_buff *skb;
 	int len;
-	u32 buf_len = skb->len;
+	u32 buf_len = msdu->len;
 	u16 fc;
 
-	hdr = (struct ieee80211_hdr *)skb->data;
+	hdr = (struct ieee80211_hdr *)msdu->data;
 	fc = le16_to_cpu(hdr->frame_control);
 
 	if (WARN_ON_ONCE(!ieee80211_is_mgmt(hdr->frame_control)))
-		return -EINVAL;
+		return ERR_PTR(-EINVAL);
 
-	len = sizeof(cmd->hdr) + skb->len;
+	len = sizeof(cmd->hdr) + msdu->len;
 
 	if ((ieee80211_is_action(hdr->frame_control) ||
 	     ieee80211_is_deauth(hdr->frame_control) ||
@@ -821,36 +1059,27 @@ int ath10k_wmi_mgmt_tx(struct ath10k *ar, struct sk_buff *skb)
 
 	len = round_up(len, 4);
 
-	wmi_skb = ath10k_wmi_alloc_skb(ar, len);
-	if (!wmi_skb)
-		return -ENOMEM;
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
 
-	cmd = (struct wmi_mgmt_tx_cmd *)wmi_skb->data;
+	cmd = (struct wmi_mgmt_tx_cmd *)skb->data;
 
-	cmd->hdr.vdev_id = __cpu_to_le32(ATH10K_SKB_CB(skb)->vdev_id);
+	cmd->hdr.vdev_id = __cpu_to_le32(ATH10K_SKB_CB(msdu)->vdev_id);
 	cmd->hdr.tx_rate = 0;
 	cmd->hdr.tx_power = 0;
 	cmd->hdr.buf_len = __cpu_to_le32(buf_len);
 
 	ether_addr_copy(cmd->hdr.peer_macaddr.addr, ieee80211_get_DA(hdr));
-	memcpy(cmd->buf, skb->data, skb->len);
+	memcpy(cmd->buf, msdu->data, msdu->len);
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi mgmt tx skb %p len %d ftype %02x stype %02x\n",
-		   wmi_skb, wmi_skb->len, fc & IEEE80211_FCTL_FTYPE,
+		   msdu, skb->len, fc & IEEE80211_FCTL_FTYPE,
 		   fc & IEEE80211_FCTL_STYPE);
 	trace_ath10k_tx_hdr(ar, skb->data, skb->len);
 	trace_ath10k_tx_payload(ar, skb->data, skb->len);
 
-	/* Send the management frame buffer to the target */
-	ret = ath10k_wmi_cmd_send(ar, wmi_skb, ar->wmi.cmd->mgmt_tx_cmdid);
-	if (ret)
-		return ret;
-
-	/* TODO: report tx status to mac80211 - temporary just ACK */
-	info->flags |= IEEE80211_TX_STAT_ACK;
-	ieee80211_tx_status_irqsafe(ar->hw, skb);
-
-	return ret;
+	return skb;
 }
 
 static void ath10k_wmi_event_scan_started(struct ath10k *ar)
@@ -977,22 +1206,48 @@ ath10k_wmi_event_scan_type_str(enum wmi_scan_event_type type,
 	}
 }
 
-static int ath10k_wmi_event_scan(struct ath10k *ar, struct sk_buff *skb)
+static int ath10k_wmi_op_pull_scan_ev(struct ath10k *ar, struct sk_buff *skb,
+				      struct wmi_scan_ev_arg *arg)
 {
-	struct wmi_scan_event *event = (struct wmi_scan_event *)skb->data;
+	struct wmi_scan_event *ev = (void *)skb->data;
+
+	if (skb->len < sizeof(*ev))
+		return -EPROTO;
+
+	skb_pull(skb, sizeof(*ev));
+	arg->event_type = ev->event_type;
+	arg->reason = ev->reason;
+	arg->channel_freq = ev->channel_freq;
+	arg->scan_req_id = ev->scan_req_id;
+	arg->scan_id = ev->scan_id;
+	arg->vdev_id = ev->vdev_id;
+
+	return 0;
+}
+
+int ath10k_wmi_event_scan(struct ath10k *ar, struct sk_buff *skb)
+{
+	struct wmi_scan_ev_arg arg = {};
 	enum wmi_scan_event_type event_type;
 	enum wmi_scan_completion_reason reason;
 	u32 freq;
 	u32 req_id;
 	u32 scan_id;
 	u32 vdev_id;
+	int ret;
 
-	event_type = __le32_to_cpu(event->event_type);
-	reason     = __le32_to_cpu(event->reason);
-	freq       = __le32_to_cpu(event->channel_freq);
-	req_id     = __le32_to_cpu(event->scan_req_id);
-	scan_id    = __le32_to_cpu(event->scan_id);
-	vdev_id    = __le32_to_cpu(event->vdev_id);
+	ret = ath10k_wmi_pull_scan(ar, skb, &arg);
+	if (ret) {
+		ath10k_warn(ar, "failed to parse scan event: %d\n", ret);
+		return ret;
+	}
+
+	event_type = __le32_to_cpu(arg.event_type);
+	reason = __le32_to_cpu(arg.reason);
+	freq = __le32_to_cpu(arg.channel_freq);
+	req_id = __le32_to_cpu(arg.scan_req_id);
+	scan_id = __le32_to_cpu(arg.scan_id);
+	vdev_id = __le32_to_cpu(arg.vdev_id);
 
 	spin_lock_bh(&ar->data_lock);
 
@@ -1147,11 +1402,51 @@ static void ath10k_wmi_handle_wep_reauth(struct ath10k *ar,
 	}
 }
 
-static int ath10k_wmi_event_mgmt_rx(struct ath10k *ar, struct sk_buff *skb)
+static int ath10k_wmi_op_pull_mgmt_rx_ev(struct ath10k *ar, struct sk_buff *skb,
+					 struct wmi_mgmt_rx_ev_arg *arg)
 {
 	struct wmi_mgmt_rx_event_v1 *ev_v1;
 	struct wmi_mgmt_rx_event_v2 *ev_v2;
 	struct wmi_mgmt_rx_hdr_v1 *ev_hdr;
+	size_t pull_len;
+	u32 msdu_len;
+
+	if (test_bit(ATH10K_FW_FEATURE_EXT_WMI_MGMT_RX, ar->fw_features)) {
+		ev_v2 = (struct wmi_mgmt_rx_event_v2 *)skb->data;
+		ev_hdr = &ev_v2->hdr.v1;
+		pull_len = sizeof(*ev_v2);
+	} else {
+		ev_v1 = (struct wmi_mgmt_rx_event_v1 *)skb->data;
+		ev_hdr = &ev_v1->hdr;
+		pull_len = sizeof(*ev_v1);
+	}
+
+	if (skb->len < pull_len)
+		return -EPROTO;
+
+	skb_pull(skb, pull_len);
+	arg->channel = ev_hdr->channel;
+	arg->buf_len = ev_hdr->buf_len;
+	arg->status = ev_hdr->status;
+	arg->snr = ev_hdr->snr;
+	arg->phy_mode = ev_hdr->phy_mode;
+	arg->rate = ev_hdr->rate;
+
+	msdu_len = __le32_to_cpu(arg->buf_len);
+	if (skb->len < msdu_len)
+		return -EPROTO;
+
+	/* the WMI buffer might've ended up being padded to 4 bytes due to HTC
+	 * trailer with credit update. Trim the excess garbage.
+	 */
+	skb_trim(skb, msdu_len);
+
+	return 0;
+}
+
+int ath10k_wmi_event_mgmt_rx(struct ath10k *ar, struct sk_buff *skb)
+{
+	struct wmi_mgmt_rx_ev_arg arg = {};
 	struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(skb);
 	struct ieee80211_hdr *hdr;
 	u32 rx_status;
@@ -1161,24 +1456,20 @@ static int ath10k_wmi_event_mgmt_rx(struct ath10k *ar, struct sk_buff *skb)
 	u32 rate;
 	u32 buf_len;
 	u16 fc;
-	int pull_len;
+	int ret;
 
-	if (test_bit(ATH10K_FW_FEATURE_EXT_WMI_MGMT_RX, ar->fw_features)) {
-		ev_v2 = (struct wmi_mgmt_rx_event_v2 *)skb->data;
-		ev_hdr = &ev_v2->hdr.v1;
-		pull_len = sizeof(*ev_v2);
-	} else {
-		ev_v1 = (struct wmi_mgmt_rx_event_v1 *)skb->data;
-		ev_hdr = &ev_v1->hdr;
-		pull_len = sizeof(*ev_v1);
+	ret = ath10k_wmi_pull_mgmt_rx(ar, skb, &arg);
+	if (ret) {
+		ath10k_warn(ar, "failed to parse mgmt rx event: %d\n", ret);
+		return ret;
 	}
 
-	channel   = __le32_to_cpu(ev_hdr->channel);
-	buf_len   = __le32_to_cpu(ev_hdr->buf_len);
-	rx_status = __le32_to_cpu(ev_hdr->status);
-	snr       = __le32_to_cpu(ev_hdr->snr);
-	phy_mode  = __le32_to_cpu(ev_hdr->phy_mode);
-	rate	  = __le32_to_cpu(ev_hdr->rate);
+	channel = __le32_to_cpu(arg.channel);
+	buf_len = __le32_to_cpu(arg.buf_len);
+	rx_status = __le32_to_cpu(arg.status);
+	snr = __le32_to_cpu(arg.snr);
+	phy_mode = __le32_to_cpu(arg.phy_mode);
+	rate = __le32_to_cpu(arg.rate);
 
 	memset(status, 0, sizeof(*status));
 
@@ -1232,8 +1523,6 @@ static int ath10k_wmi_event_mgmt_rx(struct ath10k *ar, struct sk_buff *skb)
 	status->signal = snr + ATH10K_DEFAULT_NOISE_FLOOR;
 	status->rate_idx = get_rate_idx(rate, status->band);
 
-	skb_pull(skb, pull_len);
-
 	hdr = (struct ieee80211_hdr *)skb->data;
 	fc = le16_to_cpu(hdr->frame_control);
 
@@ -1266,12 +1555,6 @@ static int ath10k_wmi_event_mgmt_rx(struct ath10k *ar, struct sk_buff *skb)
 		   status->freq, status->band, status->signal,
 		   status->rate_idx);
 
-	/*
-	 * packets from HTC come aligned to 4byte boundaries
-	 * because they can originally come in along with a trailer
-	 */
-	skb_trim(skb, buf_len);
-
 	ieee80211_rx(ar->hw, skb);
 	return 0;
 }
@@ -1295,21 +1578,44 @@ exit:
 	return idx;
 }
 
-static void ath10k_wmi_event_chan_info(struct ath10k *ar, struct sk_buff *skb)
+static int ath10k_wmi_op_pull_ch_info_ev(struct ath10k *ar, struct sk_buff *skb,
+					 struct wmi_ch_info_ev_arg *arg)
 {
-	struct wmi_chan_info_event *ev;
+	struct wmi_chan_info_event *ev = (void *)skb->data;
+
+	if (skb->len < sizeof(*ev))
+		return -EPROTO;
+
+	skb_pull(skb, sizeof(*ev));
+	arg->err_code = ev->err_code;
+	arg->freq = ev->freq;
+	arg->cmd_flags = ev->cmd_flags;
+	arg->noise_floor = ev->noise_floor;
+	arg->rx_clear_count = ev->rx_clear_count;
+	arg->cycle_count = ev->cycle_count;
+
+	return 0;
+}
+
+void ath10k_wmi_event_chan_info(struct ath10k *ar, struct sk_buff *skb)
+{
+	struct wmi_ch_info_ev_arg arg = {};
 	struct survey_info *survey;
 	u32 err_code, freq, cmd_flags, noise_floor, rx_clear_count, cycle_count;
-	int idx;
+	int idx, ret;
 
-	ev = (struct wmi_chan_info_event *)skb->data;
+	ret = ath10k_wmi_pull_ch_info(ar, skb, &arg);
+	if (ret) {
+		ath10k_warn(ar, "failed to parse chan info event: %d\n", ret);
+		return;
+	}
 
-	err_code = __le32_to_cpu(ev->err_code);
-	freq = __le32_to_cpu(ev->freq);
-	cmd_flags = __le32_to_cpu(ev->cmd_flags);
-	noise_floor = __le32_to_cpu(ev->noise_floor);
-	rx_clear_count = __le32_to_cpu(ev->rx_clear_count);
-	cycle_count = __le32_to_cpu(ev->cycle_count);
+	err_code = __le32_to_cpu(arg.err_code);
+	freq = __le32_to_cpu(arg.freq);
+	cmd_flags = __le32_to_cpu(arg.cmd_flags);
+	noise_floor = __le32_to_cpu(arg.noise_floor);
+	rx_clear_count = __le32_to_cpu(arg.rx_clear_count);
+	cycle_count = __le32_to_cpu(arg.cycle_count);
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "chan info err_code %d freq %d cmd_flags %d noise_floor %d rx_clear_count %d cycle_count %d\n",
@@ -1359,12 +1665,12 @@ exit:
 	spin_unlock_bh(&ar->data_lock);
 }
 
-static void ath10k_wmi_event_echo(struct ath10k *ar, struct sk_buff *skb)
+void ath10k_wmi_event_echo(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_ECHO_EVENTID\n");
 }
 
-static int ath10k_wmi_event_debug_mesg(struct ath10k *ar, struct sk_buff *skb)
+int ath10k_wmi_event_debug_mesg(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi event debug mesg len %d\n",
 		   skb->len);
@@ -1374,8 +1680,8 @@ static int ath10k_wmi_event_debug_mesg(struct ath10k *ar, struct sk_buff *skb)
 	return 0;
 }
 
-static void ath10k_wmi_pull_pdev_stats(const struct wmi_pdev_stats *src,
-				       struct ath10k_fw_stats_pdev *dst)
+void ath10k_wmi_pull_pdev_stats(const struct wmi_pdev_stats *src,
+				struct ath10k_fw_stats_pdev *dst)
 {
 	const struct wal_dbg_tx_stats *tx = &src->wal.tx;
 	const struct wal_dbg_rx_stats *rx = &src->wal.rx;
@@ -1427,17 +1733,17 @@ static void ath10k_wmi_pull_pdev_stats(const struct wmi_pdev_stats *src,
 	dst->mpdu_errs = __le32_to_cpu(rx->mpdu_errs);
 }
 
-static void ath10k_wmi_pull_peer_stats(const struct wmi_peer_stats *src,
-				       struct ath10k_fw_stats_peer *dst)
+void ath10k_wmi_pull_peer_stats(const struct wmi_peer_stats *src,
+				struct ath10k_fw_stats_peer *dst)
 {
 	ether_addr_copy(dst->peer_macaddr, src->peer_macaddr.addr);
 	dst->peer_rssi = __le32_to_cpu(src->peer_rssi);
 	dst->peer_tx_rate = __le32_to_cpu(src->peer_tx_rate);
 }
 
-static int ath10k_wmi_main_pull_fw_stats(struct ath10k *ar,
-					 struct sk_buff *skb,
-					 struct ath10k_fw_stats *stats)
+static int ath10k_wmi_main_op_pull_fw_stats(struct ath10k *ar,
+					    struct sk_buff *skb,
+					    struct ath10k_fw_stats *stats)
 {
 	const struct wmi_stats_event *ev = (void *)skb->data;
 	u32 num_pdev_stats, num_vdev_stats, num_peer_stats;
@@ -1487,9 +1793,9 @@ static int ath10k_wmi_main_pull_fw_stats(struct ath10k *ar,
 	return 0;
 }
 
-static int ath10k_wmi_10x_pull_fw_stats(struct ath10k *ar,
-					struct sk_buff *skb,
-					struct ath10k_fw_stats *stats)
+static int ath10k_wmi_10x_op_pull_fw_stats(struct ath10k *ar,
+					   struct sk_buff *skb,
+					   struct ath10k_fw_stats *stats)
 {
 	const struct wmi_stats_event *ev = (void *)skb->data;
 	u32 num_pdev_stats, num_vdev_stats, num_peer_stats;
@@ -1550,61 +1856,92 @@ static int ath10k_wmi_10x_pull_fw_stats(struct ath10k *ar,
 	return 0;
 }
 
-int ath10k_wmi_pull_fw_stats(struct ath10k *ar, struct sk_buff *skb,
-			     struct ath10k_fw_stats *stats)
+void ath10k_wmi_event_update_stats(struct ath10k *ar, struct sk_buff *skb)
 {
-	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features))
-		return ath10k_wmi_10x_pull_fw_stats(ar, skb, stats);
-	else
-		return ath10k_wmi_main_pull_fw_stats(ar, skb, stats);
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_UPDATE_STATS_EVENTID\n");
+	ath10k_debug_fw_stats_process(ar, skb);
 }
 
-static void ath10k_wmi_event_update_stats(struct ath10k *ar,
-					  struct sk_buff *skb)
+static int
+ath10k_wmi_op_pull_vdev_start_ev(struct ath10k *ar, struct sk_buff *skb,
+				 struct wmi_vdev_start_ev_arg *arg)
 {
-	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_UPDATE_STATS_EVENTID\n");
-	ath10k_debug_fw_stats_process(ar, skb);
+	struct wmi_vdev_start_response_event *ev = (void *)skb->data;
+
+	if (skb->len < sizeof(*ev))
+		return -EPROTO;
+
+	skb_pull(skb, sizeof(*ev));
+	arg->vdev_id = ev->vdev_id;
+	arg->req_id = ev->req_id;
+	arg->resp_type = ev->resp_type;
+	arg->status = ev->status;
+
+	return 0;
 }
 
-static void ath10k_wmi_event_vdev_start_resp(struct ath10k *ar,
-					     struct sk_buff *skb)
+void ath10k_wmi_event_vdev_start_resp(struct ath10k *ar, struct sk_buff *skb)
 {
-	struct wmi_vdev_start_response_event *ev;
+	struct wmi_vdev_start_ev_arg arg = {};
+	int ret;
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_VDEV_START_RESP_EVENTID\n");
 
-	ev = (struct wmi_vdev_start_response_event *)skb->data;
+	ret = ath10k_wmi_pull_vdev_start(ar, skb, &arg);
+	if (ret) {
+		ath10k_warn(ar, "failed to parse vdev start event: %d\n", ret);
+		return;
+	}
 
-	if (WARN_ON(__le32_to_cpu(ev->status)))
+	if (WARN_ON(__le32_to_cpu(arg.status)))
 		return;
 
 	complete(&ar->vdev_setup_done);
 }
 
-static void ath10k_wmi_event_vdev_stopped(struct ath10k *ar,
-					  struct sk_buff *skb)
+void ath10k_wmi_event_vdev_stopped(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_VDEV_STOPPED_EVENTID\n");
 	complete(&ar->vdev_setup_done);
 }
 
-static void ath10k_wmi_event_peer_sta_kickout(struct ath10k *ar,
-					      struct sk_buff *skb)
+static int
+ath10k_wmi_op_pull_peer_kick_ev(struct ath10k *ar, struct sk_buff *skb,
+				struct wmi_peer_kick_ev_arg *arg)
 {
-	struct wmi_peer_sta_kickout_event *ev;
+	struct wmi_peer_sta_kickout_event *ev = (void *)skb->data;
+
+	if (skb->len < sizeof(*ev))
+		return -EPROTO;
+
+	skb_pull(skb, sizeof(*ev));
+	arg->mac_addr = ev->peer_macaddr.addr;
+
+	return 0;
+}
+
+void ath10k_wmi_event_peer_sta_kickout(struct ath10k *ar, struct sk_buff *skb)
+{
+	struct wmi_peer_kick_ev_arg arg = {};
 	struct ieee80211_sta *sta;
+	int ret;
 
-	ev = (struct wmi_peer_sta_kickout_event *)skb->data;
+	ret = ath10k_wmi_pull_peer_kick(ar, skb, &arg);
+	if (ret) {
+		ath10k_warn(ar, "failed to parse peer kickout event: %d\n",
+			    ret);
+		return;
+	}
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi event peer sta kickout %pM\n",
-		   ev->peer_macaddr.addr);
+		   arg.mac_addr);
 
 	rcu_read_lock();
 
-	sta = ieee80211_find_sta_by_ifaddr(ar->hw, ev->peer_macaddr.addr, NULL);
+	sta = ieee80211_find_sta_by_ifaddr(ar->hw, arg.mac_addr, NULL);
 	if (!sta) {
 		ath10k_warn(ar, "Spurious quick kickout for STA %pM\n",
-			    ev->peer_macaddr.addr);
+			    arg.mac_addr);
 		goto exit;
 	}
 
@@ -1641,7 +1978,7 @@ exit:
 static void ath10k_wmi_update_tim(struct ath10k *ar,
 				  struct ath10k_vif *arvif,
 				  struct sk_buff *bcn,
-				  struct wmi_bcn_info *bcn_info)
+				  const struct wmi_tim_info *tim_info)
 {
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)bcn->data;
 	struct ieee80211_tim_ie *tim;
@@ -1652,14 +1989,14 @@ static void ath10k_wmi_update_tim(struct ath10k *ar,
 
 	/* if next SWBA has no tim_changed the tim_bitmap is garbage.
 	 * we must copy the bitmap upon change and reuse it later */
-	if (__le32_to_cpu(bcn_info->tim_info.tim_changed)) {
+	if (__le32_to_cpu(tim_info->tim_changed)) {
 		int i;
 
 		BUILD_BUG_ON(sizeof(arvif->u.ap.tim_bitmap) !=
-			     sizeof(bcn_info->tim_info.tim_bitmap));
+			     sizeof(tim_info->tim_bitmap));
 
 		for (i = 0; i < sizeof(arvif->u.ap.tim_bitmap); i++) {
-			t = bcn_info->tim_info.tim_bitmap[i / 4];
+			t = tim_info->tim_bitmap[i / 4];
 			v = __le32_to_cpu(t);
 			arvif->u.ap.tim_bitmap[i] = (v >> ((i % 4) * 8)) & 0xFF;
 		}
@@ -1711,13 +2048,13 @@ static void ath10k_wmi_update_tim(struct ath10k *ar,
 		return;
 	}
 
-	tim->bitmap_ctrl = !!__le32_to_cpu(bcn_info->tim_info.tim_mcast);
+	tim->bitmap_ctrl = !!__le32_to_cpu(tim_info->tim_mcast);
 	memcpy(tim->virtual_map, arvif->u.ap.tim_bitmap, pvm_len);
 
 	if (tim->dtim_count == 0) {
 		ATH10K_SKB_CB(bcn)->bcn.dtim_zero = true;
 
-		if (__le32_to_cpu(bcn_info->tim_info.tim_mcast) == 1)
+		if (__le32_to_cpu(tim_info->tim_mcast) == 1)
 			ATH10K_SKB_CB(bcn)->bcn.deliver_cab = true;
 	}
 
@@ -1727,7 +2064,7 @@ static void ath10k_wmi_update_tim(struct ath10k *ar,
 }
 
 static void ath10k_p2p_fill_noa_ie(u8 *data, u32 len,
-				   struct wmi_p2p_noa_info *noa)
+				   const struct wmi_p2p_noa_info *noa)
 {
 	struct ieee80211_p2p_noa_attr *noa_attr;
 	u8  ctwindow_oppps = noa->ctwindow_oppps;
@@ -1769,7 +2106,7 @@ static void ath10k_p2p_fill_noa_ie(u8 *data, u32 len,
 	*noa_attr_len = __cpu_to_le16(attr_len);
 }
 
-static u32 ath10k_p2p_calc_noa_ie_len(struct wmi_p2p_noa_info *noa)
+static u32 ath10k_p2p_calc_noa_ie_len(const struct wmi_p2p_noa_info *noa)
 {
 	u32 len = 0;
 	u8 noa_descriptors = noa->num_descriptors;
@@ -1789,9 +2126,8 @@ static u32 ath10k_p2p_calc_noa_ie_len(struct wmi_p2p_noa_info *noa)
 
 static void ath10k_wmi_update_noa(struct ath10k *ar, struct ath10k_vif *arvif,
 				  struct sk_buff *bcn,
-				  struct wmi_bcn_info *bcn_info)
+				  const struct wmi_p2p_noa_info *noa)
 {
-	struct wmi_p2p_noa_info *noa = &bcn_info->p2p_noa_info;
 	u8 *new_data, *old_data = arvif->u.ap.noa_data;
 	u32 new_len;
 
@@ -1832,22 +2168,59 @@ cleanup:
 	kfree(old_data);
 }
 
-static void ath10k_wmi_event_host_swba(struct ath10k *ar, struct sk_buff *skb)
+static int ath10k_wmi_op_pull_swba_ev(struct ath10k *ar, struct sk_buff *skb,
+				      struct wmi_swba_ev_arg *arg)
+{
+	struct wmi_host_swba_event *ev = (void *)skb->data;
+	u32 map;
+	size_t i;
+
+	if (skb->len < sizeof(*ev))
+		return -EPROTO;
+
+	skb_pull(skb, sizeof(*ev));
+	arg->vdev_map = ev->vdev_map;
+
+	for (i = 0, map = __le32_to_cpu(ev->vdev_map); map; map >>= 1) {
+		if (!(map & BIT(0)))
+			continue;
+
+		/* If this happens there were some changes in firmware and
+		 * ath10k should update the max size of tim_info array.
+		 */
+		if (WARN_ON_ONCE(i == ARRAY_SIZE(arg->tim_info)))
+			break;
+
+		arg->tim_info[i] = &ev->bcn_info[i].tim_info;
+		arg->noa_info[i] = &ev->bcn_info[i].p2p_noa_info;
+		i++;
+	}
+
+	return 0;
+}
+
+void ath10k_wmi_event_host_swba(struct ath10k *ar, struct sk_buff *skb)
 {
-	struct wmi_host_swba_event *ev;
+	struct wmi_swba_ev_arg arg = {};
 	u32 map;
 	int i = -1;
-	struct wmi_bcn_info *bcn_info;
+	const struct wmi_tim_info *tim_info;
+	const struct wmi_p2p_noa_info *noa_info;
 	struct ath10k_vif *arvif;
 	struct sk_buff *bcn;
 	dma_addr_t paddr;
 	int ret, vdev_id = 0;
 
-	ev = (struct wmi_host_swba_event *)skb->data;
-	map = __le32_to_cpu(ev->vdev_map);
+	ret = ath10k_wmi_pull_swba(ar, skb, &arg);
+	if (ret) {
+		ath10k_warn(ar, "failed to parse swba event: %d\n", ret);
+		return;
+	}
+
+	map = __le32_to_cpu(arg.vdev_map);
 
 	ath10k_dbg(ar, ATH10K_DBG_MGMT, "mgmt swba vdev_map 0x%x\n",
-		   ev->vdev_map);
+		   map);
 
 	for (; map; map >>= 1, vdev_id++) {
 		if (!(map & 0x1))
@@ -1860,19 +2233,20 @@ static void ath10k_wmi_event_host_swba(struct ath10k *ar, struct sk_buff *skb)
 			break;
 		}
 
-		bcn_info = &ev->bcn_info[i];
+		tim_info = arg.tim_info[i];
+		noa_info = arg.noa_info[i];
 
 		ath10k_dbg(ar, ATH10K_DBG_MGMT,
 			   "mgmt event bcn_info %d tim_len %d mcast %d changed %d num_ps_pending %d bitmap 0x%08x%08x%08x%08x\n",
 			   i,
-			   __le32_to_cpu(bcn_info->tim_info.tim_len),
-			   __le32_to_cpu(bcn_info->tim_info.tim_mcast),
-			   __le32_to_cpu(bcn_info->tim_info.tim_changed),
-			   __le32_to_cpu(bcn_info->tim_info.tim_num_ps_pending),
-			   __le32_to_cpu(bcn_info->tim_info.tim_bitmap[3]),
-			   __le32_to_cpu(bcn_info->tim_info.tim_bitmap[2]),
-			   __le32_to_cpu(bcn_info->tim_info.tim_bitmap[1]),
-			   __le32_to_cpu(bcn_info->tim_info.tim_bitmap[0]));
+			   __le32_to_cpu(tim_info->tim_len),
+			   __le32_to_cpu(tim_info->tim_mcast),
+			   __le32_to_cpu(tim_info->tim_changed),
+			   __le32_to_cpu(tim_info->tim_num_ps_pending),
+			   __le32_to_cpu(tim_info->tim_bitmap[3]),
+			   __le32_to_cpu(tim_info->tim_bitmap[2]),
+			   __le32_to_cpu(tim_info->tim_bitmap[1]),
+			   __le32_to_cpu(tim_info->tim_bitmap[0]));
 
 		arvif = ath10k_get_arvif(ar, vdev_id);
 		if (arvif == NULL) {
@@ -1899,8 +2273,8 @@ static void ath10k_wmi_event_host_swba(struct ath10k *ar, struct sk_buff *skb)
 		}
 
 		ath10k_tx_h_seq_no(arvif->vif, bcn);
-		ath10k_wmi_update_tim(ar, arvif, bcn, bcn_info);
-		ath10k_wmi_update_noa(ar, arvif, bcn, bcn_info);
+		ath10k_wmi_update_tim(ar, arvif, bcn, tim_info);
+		ath10k_wmi_update_noa(ar, arvif, bcn, noa_info);
 
 		spin_lock_bh(&ar->data_lock);
 
@@ -1946,8 +2320,7 @@ skip:
 	}
 }
 
-static void ath10k_wmi_event_tbttoffset_update(struct ath10k *ar,
-					       struct sk_buff *skb)
+void ath10k_wmi_event_tbttoffset_update(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_TBTTOFFSET_UPDATE_EVENTID\n");
 }
@@ -2068,9 +2441,9 @@ static int ath10k_dfs_fft_report(struct ath10k *ar,
 	return 0;
 }
 
-static void ath10k_wmi_event_dfs(struct ath10k *ar,
-				 const struct wmi_phyerr *phyerr,
-				 u64 tsf)
+void ath10k_wmi_event_dfs(struct ath10k *ar,
+			  const struct wmi_phyerr *phyerr,
+			  u64 tsf)
 {
 	int buf_len, tlv_len, res, i = 0;
 	const struct phyerr_tlv *tlv;
@@ -2133,10 +2506,9 @@ static void ath10k_wmi_event_dfs(struct ath10k *ar,
 	}
 }
 
-static void
-ath10k_wmi_event_spectral_scan(struct ath10k *ar,
-			       const struct wmi_phyerr *phyerr,
-			       u64 tsf)
+void ath10k_wmi_event_spectral_scan(struct ath10k *ar,
+				    const struct wmi_phyerr *phyerr,
+				    u64 tsf)
 {
 	int buf_len, tlv_len, res, i = 0;
 	struct phyerr_tlv *tlv;
@@ -2188,37 +2560,53 @@ ath10k_wmi_event_spectral_scan(struct ath10k *ar,
 	}
 }
 
-static void ath10k_wmi_event_phyerr(struct ath10k *ar, struct sk_buff *skb)
+static int ath10k_wmi_op_pull_phyerr_ev(struct ath10k *ar, struct sk_buff *skb,
+					struct wmi_phyerr_ev_arg *arg)
+{
+	struct wmi_phyerr_event *ev = (void *)skb->data;
+
+	if (skb->len < sizeof(*ev))
+		return -EPROTO;
+
+	arg->num_phyerrs = ev->num_phyerrs;
+	arg->tsf_l32 = ev->tsf_l32;
+	arg->tsf_u32 = ev->tsf_u32;
+	arg->buf_len = __cpu_to_le32(skb->len - sizeof(*ev));
+	arg->phyerrs = ev->phyerrs;
+
+	return 0;
+}
+
+void ath10k_wmi_event_phyerr(struct ath10k *ar, struct sk_buff *skb)
 {
-	const struct wmi_phyerr_event *ev;
+	struct wmi_phyerr_ev_arg arg = {};
 	const struct wmi_phyerr *phyerr;
 	u32 count, i, buf_len, phy_err_code;
 	u64 tsf;
-	int left_len = skb->len;
+	int left_len, ret;
 
 	ATH10K_DFS_STAT_INC(ar, phy_errors);
 
-	/* Check if combined event available */
-	if (left_len < sizeof(*ev)) {
-		ath10k_warn(ar, "wmi phyerr combined event wrong len\n");
+	ret = ath10k_wmi_pull_phyerr(ar, skb, &arg);
+	if (ret) {
+		ath10k_warn(ar, "failed to parse phyerr event: %d\n", ret);
 		return;
 	}
 
-	left_len -= sizeof(*ev);
+	left_len = __le32_to_cpu(arg.buf_len);
 
 	/* Check number of included events */
-	ev = (const struct wmi_phyerr_event *)skb->data;
-	count = __le32_to_cpu(ev->num_phyerrs);
+	count = __le32_to_cpu(arg.num_phyerrs);
 
-	tsf = __le32_to_cpu(ev->tsf_u32);
+	tsf = __le32_to_cpu(arg.tsf_u32);
 	tsf <<= 32;
-	tsf |= __le32_to_cpu(ev->tsf_l32);
+	tsf |= __le32_to_cpu(arg.tsf_l32);
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi event phyerr count %d tsf64 0x%llX\n",
 		   count, tsf);
 
-	phyerr = ev->phyerrs;
+	phyerr = arg.phyerrs;
 	for (i = 0; i < count; i++) {
 		/* Check if we can read event header */
 		if (left_len < sizeof(*phyerr)) {
@@ -2258,19 +2646,17 @@ static void ath10k_wmi_event_phyerr(struct ath10k *ar, struct sk_buff *skb)
 	}
 }
 
-static void ath10k_wmi_event_roam(struct ath10k *ar, struct sk_buff *skb)
+void ath10k_wmi_event_roam(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_ROAM_EVENTID\n");
 }
 
-static void ath10k_wmi_event_profile_match(struct ath10k *ar,
-					   struct sk_buff *skb)
+void ath10k_wmi_event_profile_match(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_PROFILE_MATCH\n");
 }
 
-static void ath10k_wmi_event_debug_print(struct ath10k *ar,
-					 struct sk_buff *skb)
+void ath10k_wmi_event_debug_print(struct ath10k *ar, struct sk_buff *skb)
 {
 	char buf[101], c;
 	int i;
@@ -2303,103 +2689,90 @@ static void ath10k_wmi_event_debug_print(struct ath10k *ar,
 	ath10k_dbg(ar, ATH10K_DBG_WMI_PRINT, "wmi print '%s'\n", buf);
 }
 
-static void ath10k_wmi_event_pdev_qvit(struct ath10k *ar, struct sk_buff *skb)
+void ath10k_wmi_event_pdev_qvit(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_PDEV_QVIT_EVENTID\n");
 }
 
-static void ath10k_wmi_event_wlan_profile_data(struct ath10k *ar,
-					       struct sk_buff *skb)
+void ath10k_wmi_event_wlan_profile_data(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_WLAN_PROFILE_DATA_EVENTID\n");
 }
 
-static void ath10k_wmi_event_rtt_measurement_report(struct ath10k *ar,
-						    struct sk_buff *skb)
+void ath10k_wmi_event_rtt_measurement_report(struct ath10k *ar,
+					     struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_RTT_MEASUREMENT_REPORT_EVENTID\n");
 }
 
-static void ath10k_wmi_event_tsf_measurement_report(struct ath10k *ar,
-						    struct sk_buff *skb)
+void ath10k_wmi_event_tsf_measurement_report(struct ath10k *ar,
+					     struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_TSF_MEASUREMENT_REPORT_EVENTID\n");
 }
 
-static void ath10k_wmi_event_rtt_error_report(struct ath10k *ar,
-					      struct sk_buff *skb)
+void ath10k_wmi_event_rtt_error_report(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_RTT_ERROR_REPORT_EVENTID\n");
 }
 
-static void ath10k_wmi_event_wow_wakeup_host(struct ath10k *ar,
-					     struct sk_buff *skb)
+void ath10k_wmi_event_wow_wakeup_host(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_WOW_WAKEUP_HOST_EVENTID\n");
 }
 
-static void ath10k_wmi_event_dcs_interference(struct ath10k *ar,
-					      struct sk_buff *skb)
+void ath10k_wmi_event_dcs_interference(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_DCS_INTERFERENCE_EVENTID\n");
 }
 
-static void ath10k_wmi_event_pdev_tpc_config(struct ath10k *ar,
-					     struct sk_buff *skb)
+void ath10k_wmi_event_pdev_tpc_config(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_PDEV_TPC_CONFIG_EVENTID\n");
 }
 
-static void ath10k_wmi_event_pdev_ftm_intg(struct ath10k *ar,
-					   struct sk_buff *skb)
+void ath10k_wmi_event_pdev_ftm_intg(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_PDEV_FTM_INTG_EVENTID\n");
 }
 
-static void ath10k_wmi_event_gtk_offload_status(struct ath10k *ar,
-						struct sk_buff *skb)
+void ath10k_wmi_event_gtk_offload_status(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_GTK_OFFLOAD_STATUS_EVENTID\n");
 }
 
-static void ath10k_wmi_event_gtk_rekey_fail(struct ath10k *ar,
-					    struct sk_buff *skb)
+void ath10k_wmi_event_gtk_rekey_fail(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_GTK_REKEY_FAIL_EVENTID\n");
 }
 
-static void ath10k_wmi_event_delba_complete(struct ath10k *ar,
-					    struct sk_buff *skb)
+void ath10k_wmi_event_delba_complete(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_TX_DELBA_COMPLETE_EVENTID\n");
 }
 
-static void ath10k_wmi_event_addba_complete(struct ath10k *ar,
-					    struct sk_buff *skb)
+void ath10k_wmi_event_addba_complete(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_TX_ADDBA_COMPLETE_EVENTID\n");
 }
 
-static void ath10k_wmi_event_vdev_install_key_complete(struct ath10k *ar,
-						       struct sk_buff *skb)
+void ath10k_wmi_event_vdev_install_key_complete(struct ath10k *ar,
+						struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_VDEV_INSTALL_KEY_COMPLETE_EVENTID\n");
 }
 
-static void ath10k_wmi_event_inst_rssi_stats(struct ath10k *ar,
-					     struct sk_buff *skb)
+void ath10k_wmi_event_inst_rssi_stats(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_INST_RSSI_STATS_EVENTID\n");
 }
 
-static void ath10k_wmi_event_vdev_standby_req(struct ath10k *ar,
-					      struct sk_buff *skb)
+void ath10k_wmi_event_vdev_standby_req(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_VDEV_STANDBY_REQ_EVENTID\n");
 }
 
-static void ath10k_wmi_event_vdev_resume_req(struct ath10k *ar,
-					     struct sk_buff *skb)
+void ath10k_wmi_event_vdev_resume_req(struct ath10k *ar, struct sk_buff *skb)
 {
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_VDEV_RESUME_REQ_EVENTID\n");
 }
@@ -2435,8 +2808,9 @@ static int ath10k_wmi_alloc_host_mem(struct ath10k *ar, u32 req_id,
 	return 0;
 }
 
-static int ath10k_wmi_main_pull_svc_rdy_ev(struct sk_buff *skb,
-					   struct wmi_svc_rdy_ev_arg *arg)
+static int
+ath10k_wmi_main_op_pull_svc_rdy_ev(struct ath10k *ar, struct sk_buff *skb,
+				   struct wmi_svc_rdy_ev_arg *arg)
 {
 	struct wmi_service_ready_event *ev;
 	size_t i, n;
@@ -2471,8 +2845,9 @@ static int ath10k_wmi_main_pull_svc_rdy_ev(struct sk_buff *skb,
 	return 0;
 }
 
-static int ath10k_wmi_10x_pull_svc_rdy_ev(struct sk_buff *skb,
-					  struct wmi_svc_rdy_ev_arg *arg)
+static int
+ath10k_wmi_10x_op_pull_svc_rdy_ev(struct ath10k *ar, struct sk_buff *skb,
+				  struct wmi_svc_rdy_ev_arg *arg)
 {
 	struct wmi_10x_service_ready_event *ev;
 	int i, n;
@@ -2506,30 +2881,22 @@ static int ath10k_wmi_10x_pull_svc_rdy_ev(struct sk_buff *skb,
 	return 0;
 }
 
-static void ath10k_wmi_event_service_ready(struct ath10k *ar,
-					   struct sk_buff *skb)
+void ath10k_wmi_event_service_ready(struct ath10k *ar, struct sk_buff *skb)
 {
 	struct wmi_svc_rdy_ev_arg arg = {};
 	u32 num_units, req_id, unit_size, num_mem_reqs, num_unit_info, i;
 	int ret;
 
-	memset(&ar->wmi.svc_map, 0, sizeof(ar->wmi.svc_map));
-
-	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features)) {
-		ret = ath10k_wmi_10x_pull_svc_rdy_ev(skb, &arg);
-		wmi_10x_svc_map(arg.service_map, ar->wmi.svc_map,
-				arg.service_map_len);
-	} else {
-		ret = ath10k_wmi_main_pull_svc_rdy_ev(skb, &arg);
-		wmi_main_svc_map(arg.service_map, ar->wmi.svc_map,
-				 arg.service_map_len);
-	}
-
+	ret = ath10k_wmi_pull_svc_rdy(ar, skb, &arg);
 	if (ret) {
 		ath10k_warn(ar, "failed to parse service ready: %d\n", ret);
 		return;
 	}
 
+	memset(&ar->wmi.svc_map, 0, sizeof(ar->wmi.svc_map));
+	ath10k_wmi_map_svc(ar, arg.service_map, ar->wmi.svc_map,
+			   arg.service_map_len);
+
 	ar->hw_min_tx_power = __le32_to_cpu(arg.min_tx_power);
 	ar->hw_max_tx_power = __le32_to_cpu(arg.max_tx_power);
 	ar->ht_cap_info = __le32_to_cpu(arg.ht_cap);
@@ -2607,13 +2974,14 @@ static void ath10k_wmi_event_service_ready(struct ath10k *ar,
 	}
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
-		   "wmi event service ready min_tx_power 0x%08x max_tx_power 0x%08x ht_cap 0x%08x vht_cap 0x%08x sw_ver0 0x%08x sw_ver1 0x%08x phy_capab 0x%08x num_rf_chains 0x%08x eeprom_rd 0x%08x num_mem_reqs 0x%08x\n",
+		   "wmi event service ready min_tx_power 0x%08x max_tx_power 0x%08x ht_cap 0x%08x vht_cap 0x%08x sw_ver0 0x%08x sw_ver1 0x%08x fw_build 0x%08x phy_capab 0x%08x num_rf_chains 0x%08x eeprom_rd 0x%08x num_mem_reqs 0x%08x\n",
 		   __le32_to_cpu(arg.min_tx_power),
 		   __le32_to_cpu(arg.max_tx_power),
 		   __le32_to_cpu(arg.ht_cap),
 		   __le32_to_cpu(arg.vht_cap),
 		   __le32_to_cpu(arg.sw_ver0),
 		   __le32_to_cpu(arg.sw_ver1),
+		   __le32_to_cpu(arg.fw_build),
 		   __le32_to_cpu(arg.phy_capab),
 		   __le32_to_cpu(arg.num_rf_chains),
 		   __le32_to_cpu(arg.eeprom_rd),
@@ -2622,27 +2990,59 @@ static void ath10k_wmi_event_service_ready(struct ath10k *ar,
 	complete(&ar->wmi.service_ready);
 }
 
-static int ath10k_wmi_event_ready(struct ath10k *ar, struct sk_buff *skb)
+static int ath10k_wmi_op_pull_rdy_ev(struct ath10k *ar, struct sk_buff *skb,
+				     struct wmi_rdy_ev_arg *arg)
 {
-	struct wmi_ready_event *ev = (struct wmi_ready_event *)skb->data;
+	struct wmi_ready_event *ev = (void *)skb->data;
 
-	if (WARN_ON(skb->len < sizeof(*ev)))
-		return -EINVAL;
+	if (skb->len < sizeof(*ev))
+		return -EPROTO;
+
+	skb_pull(skb, sizeof(*ev));
+	arg->sw_version = ev->sw_version;
+	arg->abi_version = ev->abi_version;
+	arg->status = ev->status;
+	arg->mac_addr = ev->mac_addr.addr;
 
-	ether_addr_copy(ar->mac_addr, ev->mac_addr.addr);
+	return 0;
+}
+
+int ath10k_wmi_event_ready(struct ath10k *ar, struct sk_buff *skb)
+{
+	struct wmi_rdy_ev_arg arg = {};
+	int ret;
+
+	ret = ath10k_wmi_pull_rdy(ar, skb, &arg);
+	if (ret) {
+		ath10k_warn(ar, "failed to parse ready event: %d\n", ret);
+		return ret;
+	}
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
-		   "wmi event ready sw_version %u abi_version %u mac_addr %pM status %d skb->len %i ev-sz %zu\n",
-		   __le32_to_cpu(ev->sw_version),
-		   __le32_to_cpu(ev->abi_version),
-		   ev->mac_addr.addr,
-		   __le32_to_cpu(ev->status), skb->len, sizeof(*ev));
+		   "wmi event ready sw_version %u abi_version %u mac_addr %pM status %d\n",
+		   __le32_to_cpu(arg.sw_version),
+		   __le32_to_cpu(arg.abi_version),
+		   arg.mac_addr,
+		   __le32_to_cpu(arg.status));
 
+	ether_addr_copy(ar->mac_addr, arg.mac_addr);
 	complete(&ar->wmi.unified_ready);
 	return 0;
 }
 
-static void ath10k_wmi_main_process_rx(struct ath10k *ar, struct sk_buff *skb)
+static int ath10k_wmi_event_temperature(struct ath10k *ar, struct sk_buff *skb)
+{
+	const struct wmi_pdev_temperature_event *ev;
+
+	ev = (struct wmi_pdev_temperature_event *)skb->data;
+	if (WARN_ON(skb->len < sizeof(*ev)))
+		return -EPROTO;
+
+	ath10k_thermal_event_temperature(ar, __le32_to_cpu(ev->temperature));
+	return 0;
+}
+
+static void ath10k_wmi_op_rx(struct ath10k *ar, struct sk_buff *skb)
 {
 	struct wmi_cmd_hdr *cmd_hdr;
 	enum wmi_event_id id;
@@ -2758,7 +3158,7 @@ static void ath10k_wmi_main_process_rx(struct ath10k *ar, struct sk_buff *skb)
 	dev_kfree_skb(skb);
 }
 
-static void ath10k_wmi_10x_process_rx(struct ath10k *ar, struct sk_buff *skb)
+static void ath10k_wmi_10_1_op_rx(struct ath10k *ar, struct sk_buff *skb)
 {
 	struct wmi_cmd_hdr *cmd_hdr;
 	enum wmi_10x_event_id id;
@@ -2882,7 +3282,7 @@ out:
 	dev_kfree_skb(skb);
 }
 
-static void ath10k_wmi_10_2_process_rx(struct ath10k *ar, struct sk_buff *skb)
+static void ath10k_wmi_10_2_op_rx(struct ath10k *ar, struct sk_buff *skb)
 {
 	struct wmi_cmd_hdr *cmd_hdr;
 	enum wmi_10_2_event_id id;
@@ -2981,6 +3381,9 @@ static void ath10k_wmi_10_2_process_rx(struct ath10k *ar, struct sk_buff *skb)
 	case WMI_10_2_READY_EVENTID:
 		ath10k_wmi_event_ready(ar, skb);
 		break;
+	case WMI_10_2_PDEV_TEMPERATURE_EVENTID:
+		ath10k_wmi_event_temperature(ar, skb);
+		break;
 	case WMI_10_2_RTT_KEEPALIVE_EVENTID:
 	case WMI_10_2_GPIO_INPUT_EVENTID:
 	case WMI_10_2_PEER_RATECODE_LIST_EVENTID:
@@ -3001,14 +3404,11 @@ static void ath10k_wmi_10_2_process_rx(struct ath10k *ar, struct sk_buff *skb)
 
 static void ath10k_wmi_process_rx(struct ath10k *ar, struct sk_buff *skb)
 {
-	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features)) {
-		if (test_bit(ATH10K_FW_FEATURE_WMI_10_2, ar->fw_features))
-			ath10k_wmi_10_2_process_rx(ar, skb);
-		else
-			ath10k_wmi_10x_process_rx(ar, skb);
-	} else {
-		ath10k_wmi_main_process_rx(ar, skb);
-	}
+	int ret;
+
+	ret = ath10k_wmi_rx(ar, skb);
+	if (ret)
+		ath10k_warn(ar, "failed to process wmi rx: %d\n", ret);
 }
 
 int ath10k_wmi_connect(struct ath10k *ar)
@@ -3039,16 +3439,17 @@ int ath10k_wmi_connect(struct ath10k *ar)
 	return 0;
 }
 
-static int ath10k_wmi_main_pdev_set_regdomain(struct ath10k *ar, u16 rd,
-					      u16 rd2g, u16 rd5g, u16 ctl2g,
-					      u16 ctl5g)
+static struct sk_buff *
+ath10k_wmi_op_gen_pdev_set_rd(struct ath10k *ar, u16 rd, u16 rd2g, u16 rd5g,
+			      u16 ctl2g, u16 ctl5g,
+			      enum wmi_dfs_region dfs_reg)
 {
 	struct wmi_pdev_set_regdomain_cmd *cmd;
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_pdev_set_regdomain_cmd *)skb->data;
 	cmd->reg_domain = __cpu_to_le32(rd);
@@ -3060,22 +3461,20 @@ static int ath10k_wmi_main_pdev_set_regdomain(struct ath10k *ar, u16 rd,
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi pdev regdomain rd %x rd2g %x rd5g %x ctl2g %x ctl5g %x\n",
 		   rd, rd2g, rd5g, ctl2g, ctl5g);
-
-	return ath10k_wmi_cmd_send(ar, skb,
-				   ar->wmi.cmd->pdev_set_regdomain_cmdid);
+	return skb;
 }
 
-static int ath10k_wmi_10x_pdev_set_regdomain(struct ath10k *ar, u16 rd,
-					     u16 rd2g, u16 rd5g,
-					     u16 ctl2g, u16 ctl5g,
-					     enum wmi_dfs_region dfs_reg)
+static struct sk_buff *
+ath10k_wmi_10x_op_gen_pdev_set_rd(struct ath10k *ar, u16 rd, u16 rd2g, u16
+				  rd5g, u16 ctl2g, u16 ctl5g,
+				  enum wmi_dfs_region dfs_reg)
 {
 	struct wmi_pdev_set_regdomain_cmd_10x *cmd;
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_pdev_set_regdomain_cmd_10x *)skb->data;
 	cmd->reg_domain = __cpu_to_le32(rd);
@@ -3088,50 +3487,39 @@ static int ath10k_wmi_10x_pdev_set_regdomain(struct ath10k *ar, u16 rd,
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi pdev regdomain rd %x rd2g %x rd5g %x ctl2g %x ctl5g %x dfs_region %x\n",
 		   rd, rd2g, rd5g, ctl2g, ctl5g, dfs_reg);
-
-	return ath10k_wmi_cmd_send(ar, skb,
-				   ar->wmi.cmd->pdev_set_regdomain_cmdid);
-}
-
-int ath10k_wmi_pdev_set_regdomain(struct ath10k *ar, u16 rd, u16 rd2g,
-				  u16 rd5g, u16 ctl2g, u16 ctl5g,
-				  enum wmi_dfs_region dfs_reg)
-{
-	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features))
-		return ath10k_wmi_10x_pdev_set_regdomain(ar, rd, rd2g, rd5g,
-							ctl2g, ctl5g, dfs_reg);
-	else
-		return ath10k_wmi_main_pdev_set_regdomain(ar, rd, rd2g, rd5g,
-							 ctl2g, ctl5g);
+	return skb;
 }
 
-int ath10k_wmi_pdev_suspend_target(struct ath10k *ar, u32 suspend_opt)
+static struct sk_buff *
+ath10k_wmi_op_gen_pdev_suspend(struct ath10k *ar, u32 suspend_opt)
 {
 	struct wmi_pdev_suspend_cmd *cmd;
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_pdev_suspend_cmd *)skb->data;
 	cmd->suspend_opt = __cpu_to_le32(suspend_opt);
 
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->pdev_suspend_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_pdev_resume_target(struct ath10k *ar)
+static struct sk_buff *
+ath10k_wmi_op_gen_pdev_resume(struct ath10k *ar)
 {
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, 0);
-	if (skb == NULL)
-		return -ENOMEM;
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
 
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->pdev_resume_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_pdev_set_param(struct ath10k *ar, u32 id, u32 value)
+static struct sk_buff *
+ath10k_wmi_op_gen_pdev_set_param(struct ath10k *ar, u32 id, u32 value)
 {
 	struct wmi_pdev_set_param_cmd *cmd;
 	struct sk_buff *skb;
@@ -3139,12 +3527,12 @@ int ath10k_wmi_pdev_set_param(struct ath10k *ar, u32 id, u32 value)
 	if (id == WMI_PDEV_PARAM_UNSUPPORTED) {
 		ath10k_warn(ar, "pdev param %d not supported by firmware\n",
 			    id);
-		return -EOPNOTSUPP;
+		return ERR_PTR(-EOPNOTSUPP);
 	}
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_pdev_set_param_cmd *)skb->data;
 	cmd->param_id    = __cpu_to_le32(id);
@@ -3152,11 +3540,11 @@ int ath10k_wmi_pdev_set_param(struct ath10k *ar, u32 id, u32 value)
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi pdev set param %d value %d\n",
 		   id, value);
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->pdev_set_param_cmdid);
+	return skb;
 }
 
-static void ath10k_wmi_put_host_mem_chunks(struct ath10k *ar,
-					   struct wmi_host_mem_chunks *chunks)
+void ath10k_wmi_put_host_mem_chunks(struct ath10k *ar,
+				    struct wmi_host_mem_chunks *chunks)
 {
 	struct host_memory_chunk *chunk;
 	int i;
@@ -3177,7 +3565,7 @@ static void ath10k_wmi_put_host_mem_chunks(struct ath10k *ar,
 	}
 }
 
-static int ath10k_wmi_main_cmd_init(struct ath10k *ar)
+static struct sk_buff *ath10k_wmi_op_gen_init(struct ath10k *ar)
 {
 	struct wmi_init_cmd *cmd;
 	struct sk_buff *buf;
@@ -3240,7 +3628,7 @@ static int ath10k_wmi_main_cmd_init(struct ath10k *ar)
 
 	buf = ath10k_wmi_alloc_skb(ar, len);
 	if (!buf)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_init_cmd *)buf->data;
 
@@ -3248,10 +3636,10 @@ static int ath10k_wmi_main_cmd_init(struct ath10k *ar)
 	ath10k_wmi_put_host_mem_chunks(ar, &cmd->mem_chunks);
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi init\n");
-	return ath10k_wmi_cmd_send(ar, buf, ar->wmi.cmd->init_cmdid);
+	return buf;
 }
 
-static int ath10k_wmi_10x_cmd_init(struct ath10k *ar)
+static struct sk_buff *ath10k_wmi_10_1_op_gen_init(struct ath10k *ar)
 {
 	struct wmi_init_cmd_10x *cmd;
 	struct sk_buff *buf;
@@ -3306,7 +3694,7 @@ static int ath10k_wmi_10x_cmd_init(struct ath10k *ar)
 
 	buf = ath10k_wmi_alloc_skb(ar, len);
 	if (!buf)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_init_cmd_10x *)buf->data;
 
@@ -3314,10 +3702,10 @@ static int ath10k_wmi_10x_cmd_init(struct ath10k *ar)
 	ath10k_wmi_put_host_mem_chunks(ar, &cmd->mem_chunks);
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi init 10x\n");
-	return ath10k_wmi_cmd_send(ar, buf, ar->wmi.cmd->init_cmdid);
+	return buf;
 }
 
-static int ath10k_wmi_10_2_cmd_init(struct ath10k *ar)
+static struct sk_buff *ath10k_wmi_10_2_op_gen_init(struct ath10k *ar)
 {
 	struct wmi_init_cmd_10_2 *cmd;
 	struct sk_buff *buf;
@@ -3372,7 +3760,7 @@ static int ath10k_wmi_10_2_cmd_init(struct ath10k *ar)
 
 	buf = ath10k_wmi_alloc_skb(ar, len);
 	if (!buf)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_init_cmd_10_2 *)buf->data;
 
@@ -3380,26 +3768,10 @@ static int ath10k_wmi_10_2_cmd_init(struct ath10k *ar)
 	ath10k_wmi_put_host_mem_chunks(ar, &cmd->mem_chunks);
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi init 10.2\n");
-	return ath10k_wmi_cmd_send(ar, buf, ar->wmi.cmd->init_cmdid);
+	return buf;
 }
 
-int ath10k_wmi_cmd_init(struct ath10k *ar)
-{
-	int ret;
-
-	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features)) {
-		if (test_bit(ATH10K_FW_FEATURE_WMI_10_2, ar->fw_features))
-			ret = ath10k_wmi_10_2_cmd_init(ar);
-		else
-			ret = ath10k_wmi_10x_cmd_init(ar);
-	} else {
-		ret = ath10k_wmi_main_cmd_init(ar);
-	}
-
-	return ret;
-}
-
-static int ath10k_wmi_start_scan_verify(const struct wmi_start_scan_arg *arg)
+int ath10k_wmi_start_scan_verify(const struct wmi_start_scan_arg *arg)
 {
 	if (arg->ie_len && !arg->ie)
 		return -EINVAL;
@@ -3450,9 +3822,8 @@ ath10k_wmi_start_scan_tlvs_len(const struct wmi_start_scan_arg *arg)
 	return len;
 }
 
-static void
-ath10k_wmi_put_start_scan_common(struct wmi_start_scan_common *cmn,
-				 const struct wmi_start_scan_arg *arg)
+void ath10k_wmi_put_start_scan_common(struct wmi_start_scan_common *cmn,
+				      const struct wmi_start_scan_arg *arg)
 {
 	u32 scan_id;
 	u32 scan_req_id;
@@ -3546,46 +3917,60 @@ ath10k_wmi_put_start_scan_tlvs(struct wmi_start_scan_tlvs *tlvs,
 	}
 }
 
-int ath10k_wmi_start_scan(struct ath10k *ar,
-			  const struct wmi_start_scan_arg *arg)
+static struct sk_buff *
+ath10k_wmi_op_gen_start_scan(struct ath10k *ar,
+			     const struct wmi_start_scan_arg *arg)
 {
+	struct wmi_start_scan_cmd *cmd;
 	struct sk_buff *skb;
 	size_t len;
 	int ret;
 
 	ret = ath10k_wmi_start_scan_verify(arg);
 	if (ret)
-		return ret;
-
-	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features))
-		len = sizeof(struct wmi_10x_start_scan_cmd) +
-		      ath10k_wmi_start_scan_tlvs_len(arg);
-	else
-		len = sizeof(struct wmi_start_scan_cmd) +
-		      ath10k_wmi_start_scan_tlvs_len(arg);
+		return ERR_PTR(ret);
 
+	len = sizeof(*cmd) + ath10k_wmi_start_scan_tlvs_len(arg);
 	skb = ath10k_wmi_alloc_skb(ar, len);
 	if (!skb)
-		return -ENOMEM;
-
-	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features)) {
-		struct wmi_10x_start_scan_cmd *cmd;
+		return ERR_PTR(-ENOMEM);
 
-		cmd = (struct wmi_10x_start_scan_cmd *)skb->data;
-		ath10k_wmi_put_start_scan_common(&cmd->common, arg);
-		ath10k_wmi_put_start_scan_tlvs(&cmd->tlvs, arg);
-	} else {
-		struct wmi_start_scan_cmd *cmd;
+	cmd = (struct wmi_start_scan_cmd *)skb->data;
 
-		cmd = (struct wmi_start_scan_cmd *)skb->data;
-		cmd->burst_duration_ms = __cpu_to_le32(0);
+	ath10k_wmi_put_start_scan_common(&cmd->common, arg);
+	ath10k_wmi_put_start_scan_tlvs(&cmd->tlvs, arg);
 
-		ath10k_wmi_put_start_scan_common(&cmd->common, arg);
-		ath10k_wmi_put_start_scan_tlvs(&cmd->tlvs, arg);
-	}
+	cmd->burst_duration_ms = __cpu_to_le32(0);
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi start scan\n");
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->start_scan_cmdid);
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_10x_op_gen_start_scan(struct ath10k *ar,
+				 const struct wmi_start_scan_arg *arg)
+{
+	struct wmi_10x_start_scan_cmd *cmd;
+	struct sk_buff *skb;
+	size_t len;
+	int ret;
+
+	ret = ath10k_wmi_start_scan_verify(arg);
+	if (ret)
+		return ERR_PTR(ret);
+
+	len = sizeof(*cmd) + ath10k_wmi_start_scan_tlvs_len(arg);
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	cmd = (struct wmi_10x_start_scan_cmd *)skb->data;
+
+	ath10k_wmi_put_start_scan_common(&cmd->common, arg);
+	ath10k_wmi_put_start_scan_tlvs(&cmd->tlvs, arg);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi 10x start scan\n");
+	return skb;
 }
 
 void ath10k_wmi_start_scan_init(struct ath10k *ar,
@@ -3614,7 +3999,9 @@ void ath10k_wmi_start_scan_init(struct ath10k *ar,
 	arg->bssids[0].bssid = "\xFF\xFF\xFF\xFF\xFF\xFF";
 }
 
-int ath10k_wmi_stop_scan(struct ath10k *ar, const struct wmi_stop_scan_arg *arg)
+static struct sk_buff *
+ath10k_wmi_op_gen_stop_scan(struct ath10k *ar,
+			    const struct wmi_stop_scan_arg *arg)
 {
 	struct wmi_stop_scan_cmd *cmd;
 	struct sk_buff *skb;
@@ -3622,13 +4009,13 @@ int ath10k_wmi_stop_scan(struct ath10k *ar, const struct wmi_stop_scan_arg *arg)
 	u32 req_id;
 
 	if (arg->req_id > 0xFFF)
-		return -EINVAL;
+		return ERR_PTR(-EINVAL);
 	if (arg->req_type == WMI_SCAN_STOP_ONE && arg->u.scan_id > 0xFFF)
-		return -EINVAL;
+		return ERR_PTR(-EINVAL);
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	scan_id = arg->u.scan_id;
 	scan_id |= WMI_HOST_SCAN_REQ_ID_PREFIX;
@@ -3645,20 +4032,21 @@ int ath10k_wmi_stop_scan(struct ath10k *ar, const struct wmi_stop_scan_arg *arg)
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi stop scan reqid %d req_type %d vdev/scan_id %d\n",
 		   arg->req_id, arg->req_type, arg->u.scan_id);
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->stop_scan_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_vdev_create(struct ath10k *ar, u32 vdev_id,
-			   enum wmi_vdev_type type,
-			   enum wmi_vdev_subtype subtype,
-			   const u8 macaddr[ETH_ALEN])
+static struct sk_buff *
+ath10k_wmi_op_gen_vdev_create(struct ath10k *ar, u32 vdev_id,
+			      enum wmi_vdev_type type,
+			      enum wmi_vdev_subtype subtype,
+			      const u8 macaddr[ETH_ALEN])
 {
 	struct wmi_vdev_create_cmd *cmd;
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_vdev_create_cmd *)skb->data;
 	cmd->vdev_id      = __cpu_to_le32(vdev_id);
@@ -3669,58 +4057,52 @@ int ath10k_wmi_vdev_create(struct ath10k *ar, u32 vdev_id,
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "WMI vdev create: id %d type %d subtype %d macaddr %pM\n",
 		   vdev_id, type, subtype, macaddr);
-
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_create_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_vdev_delete(struct ath10k *ar, u32 vdev_id)
+static struct sk_buff *
+ath10k_wmi_op_gen_vdev_delete(struct ath10k *ar, u32 vdev_id)
 {
 	struct wmi_vdev_delete_cmd *cmd;
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_vdev_delete_cmd *)skb->data;
 	cmd->vdev_id = __cpu_to_le32(vdev_id);
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "WMI vdev delete id %d\n", vdev_id);
-
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_delete_cmdid);
+	return skb;
 }
 
-static int
-ath10k_wmi_vdev_start_restart(struct ath10k *ar,
-			      const struct wmi_vdev_start_request_arg *arg,
-			      u32 cmd_id)
+static struct sk_buff *
+ath10k_wmi_op_gen_vdev_start(struct ath10k *ar,
+			     const struct wmi_vdev_start_request_arg *arg,
+			     bool restart)
 {
 	struct wmi_vdev_start_request_cmd *cmd;
 	struct sk_buff *skb;
 	const char *cmdname;
 	u32 flags = 0;
 
-	if (cmd_id != ar->wmi.cmd->vdev_start_request_cmdid &&
-	    cmd_id != ar->wmi.cmd->vdev_restart_request_cmdid)
-		return -EINVAL;
 	if (WARN_ON(arg->ssid && arg->ssid_len == 0))
-		return -EINVAL;
+		return ERR_PTR(-EINVAL);
 	if (WARN_ON(arg->hidden_ssid && !arg->ssid))
-		return -EINVAL;
+		return ERR_PTR(-EINVAL);
 	if (WARN_ON(arg->ssid_len > sizeof(cmd->ssid.ssid)))
-		return -EINVAL;
+		return ERR_PTR(-EINVAL);
 
-	if (cmd_id == ar->wmi.cmd->vdev_start_request_cmdid)
-		cmdname = "start";
-	else if (cmd_id == ar->wmi.cmd->vdev_restart_request_cmdid)
+	if (restart)
 		cmdname = "restart";
 	else
-		return -EINVAL; /* should not happen, we already check cmd_id */
+		cmdname = "start";
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	if (arg->hidden_ssid)
 		flags |= WMI_VDEV_START_HIDDEN_SSID;
@@ -3749,50 +4131,36 @@ ath10k_wmi_vdev_start_restart(struct ath10k *ar,
 		   flags, arg->channel.freq, arg->channel.mode,
 		   cmd->chan.flags, arg->channel.max_power);
 
-	return ath10k_wmi_cmd_send(ar, skb, cmd_id);
-}
-
-int ath10k_wmi_vdev_start(struct ath10k *ar,
-			  const struct wmi_vdev_start_request_arg *arg)
-{
-	u32 cmd_id = ar->wmi.cmd->vdev_start_request_cmdid;
-
-	return ath10k_wmi_vdev_start_restart(ar, arg, cmd_id);
-}
-
-int ath10k_wmi_vdev_restart(struct ath10k *ar,
-			    const struct wmi_vdev_start_request_arg *arg)
-{
-	u32 cmd_id = ar->wmi.cmd->vdev_restart_request_cmdid;
-
-	return ath10k_wmi_vdev_start_restart(ar, arg, cmd_id);
+	return skb;
 }
 
-int ath10k_wmi_vdev_stop(struct ath10k *ar, u32 vdev_id)
+static struct sk_buff *
+ath10k_wmi_op_gen_vdev_stop(struct ath10k *ar, u32 vdev_id)
 {
 	struct wmi_vdev_stop_cmd *cmd;
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_vdev_stop_cmd *)skb->data;
 	cmd->vdev_id = __cpu_to_le32(vdev_id);
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi vdev stop id 0x%x\n", vdev_id);
-
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_stop_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_vdev_up(struct ath10k *ar, u32 vdev_id, u32 aid, const u8 *bssid)
+static struct sk_buff *
+ath10k_wmi_op_gen_vdev_up(struct ath10k *ar, u32 vdev_id, u32 aid,
+			  const u8 *bssid)
 {
 	struct wmi_vdev_up_cmd *cmd;
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_vdev_up_cmd *)skb->data;
 	cmd->vdev_id       = __cpu_to_le32(vdev_id);
@@ -3802,30 +4170,30 @@ int ath10k_wmi_vdev_up(struct ath10k *ar, u32 vdev_id, u32 aid, const u8 *bssid)
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi mgmt vdev up id 0x%x assoc id %d bssid %pM\n",
 		   vdev_id, aid, bssid);
-
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_up_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_vdev_down(struct ath10k *ar, u32 vdev_id)
+static struct sk_buff *
+ath10k_wmi_op_gen_vdev_down(struct ath10k *ar, u32 vdev_id)
 {
 	struct wmi_vdev_down_cmd *cmd;
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_vdev_down_cmd *)skb->data;
 	cmd->vdev_id = __cpu_to_le32(vdev_id);
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi mgmt vdev down id 0x%x\n", vdev_id);
-
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_down_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_vdev_set_param(struct ath10k *ar, u32 vdev_id,
-			      u32 param_id, u32 param_value)
+static struct sk_buff *
+ath10k_wmi_op_gen_vdev_set_param(struct ath10k *ar, u32 vdev_id,
+				 u32 param_id, u32 param_value)
 {
 	struct wmi_vdev_set_param_cmd *cmd;
 	struct sk_buff *skb;
@@ -3834,12 +4202,12 @@ int ath10k_wmi_vdev_set_param(struct ath10k *ar, u32 vdev_id,
 		ath10k_dbg(ar, ATH10K_DBG_WMI,
 			   "vdev param %d not supported by firmware\n",
 			    param_id);
-		return -EOPNOTSUPP;
+		return ERR_PTR(-EOPNOTSUPP);
 	}
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_vdev_set_param_cmd *)skb->data;
 	cmd->vdev_id     = __cpu_to_le32(vdev_id);
@@ -3849,24 +4217,24 @@ int ath10k_wmi_vdev_set_param(struct ath10k *ar, u32 vdev_id,
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi vdev id 0x%x set param %d value %d\n",
 		   vdev_id, param_id, param_value);
-
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_set_param_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_vdev_install_key(struct ath10k *ar,
-				const struct wmi_vdev_install_key_arg *arg)
+static struct sk_buff *
+ath10k_wmi_op_gen_vdev_install_key(struct ath10k *ar,
+				   const struct wmi_vdev_install_key_arg *arg)
 {
 	struct wmi_vdev_install_key_cmd *cmd;
 	struct sk_buff *skb;
 
 	if (arg->key_cipher == WMI_CIPHER_NONE && arg->key_data != NULL)
-		return -EINVAL;
+		return ERR_PTR(-EINVAL);
 	if (arg->key_cipher != WMI_CIPHER_NONE && arg->key_data == NULL)
-		return -EINVAL;
+		return ERR_PTR(-EINVAL);
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd) + arg->key_len);
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_vdev_install_key_cmd *)skb->data;
 	cmd->vdev_id       = __cpu_to_le32(arg->vdev_id);
@@ -3885,20 +4253,19 @@ int ath10k_wmi_vdev_install_key(struct ath10k *ar,
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi vdev install key idx %d cipher %d len %d\n",
 		   arg->key_idx, arg->key_cipher, arg->key_len);
-	return ath10k_wmi_cmd_send(ar, skb,
-				   ar->wmi.cmd->vdev_install_key_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_vdev_spectral_conf(struct ath10k *ar,
-				  const struct wmi_vdev_spectral_conf_arg *arg)
+static struct sk_buff *
+ath10k_wmi_op_gen_vdev_spectral_conf(struct ath10k *ar,
+				     const struct wmi_vdev_spectral_conf_arg *arg)
 {
 	struct wmi_vdev_spectral_conf_cmd *cmd;
 	struct sk_buff *skb;
-	u32 cmdid;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_vdev_spectral_conf_cmd *)skb->data;
 	cmd->vdev_id = __cpu_to_le32(arg->vdev_id);
@@ -3921,39 +4288,38 @@ int ath10k_wmi_vdev_spectral_conf(struct ath10k *ar,
 	cmd->scan_dbm_adj = __cpu_to_le32(arg->scan_dbm_adj);
 	cmd->scan_chn_mask = __cpu_to_le32(arg->scan_chn_mask);
 
-	cmdid = ar->wmi.cmd->vdev_spectral_scan_configure_cmdid;
-	return ath10k_wmi_cmd_send(ar, skb, cmdid);
+	return skb;
 }
 
-int ath10k_wmi_vdev_spectral_enable(struct ath10k *ar, u32 vdev_id, u32 trigger,
-				    u32 enable)
+static struct sk_buff *
+ath10k_wmi_op_gen_vdev_spectral_enable(struct ath10k *ar, u32 vdev_id,
+				       u32 trigger, u32 enable)
 {
 	struct wmi_vdev_spectral_enable_cmd *cmd;
 	struct sk_buff *skb;
-	u32 cmdid;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_vdev_spectral_enable_cmd *)skb->data;
 	cmd->vdev_id = __cpu_to_le32(vdev_id);
 	cmd->trigger_cmd = __cpu_to_le32(trigger);
 	cmd->enable_cmd = __cpu_to_le32(enable);
 
-	cmdid = ar->wmi.cmd->vdev_spectral_scan_enable_cmdid;
-	return ath10k_wmi_cmd_send(ar, skb, cmdid);
+	return skb;
 }
 
-int ath10k_wmi_peer_create(struct ath10k *ar, u32 vdev_id,
-			   const u8 peer_addr[ETH_ALEN])
+static struct sk_buff *
+ath10k_wmi_op_gen_peer_create(struct ath10k *ar, u32 vdev_id,
+			      const u8 peer_addr[ETH_ALEN])
 {
 	struct wmi_peer_create_cmd *cmd;
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_peer_create_cmd *)skb->data;
 	cmd->vdev_id = __cpu_to_le32(vdev_id);
@@ -3962,18 +4328,19 @@ int ath10k_wmi_peer_create(struct ath10k *ar, u32 vdev_id,
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi peer create vdev_id %d peer_addr %pM\n",
 		   vdev_id, peer_addr);
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->peer_create_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_peer_delete(struct ath10k *ar, u32 vdev_id,
-			   const u8 peer_addr[ETH_ALEN])
+static struct sk_buff *
+ath10k_wmi_op_gen_peer_delete(struct ath10k *ar, u32 vdev_id,
+			      const u8 peer_addr[ETH_ALEN])
 {
 	struct wmi_peer_delete_cmd *cmd;
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_peer_delete_cmd *)skb->data;
 	cmd->vdev_id = __cpu_to_le32(vdev_id);
@@ -3982,18 +4349,19 @@ int ath10k_wmi_peer_delete(struct ath10k *ar, u32 vdev_id,
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi peer delete vdev_id %d peer_addr %pM\n",
 		   vdev_id, peer_addr);
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->peer_delete_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_peer_flush(struct ath10k *ar, u32 vdev_id,
-			  const u8 peer_addr[ETH_ALEN], u32 tid_bitmap)
+static struct sk_buff *
+ath10k_wmi_op_gen_peer_flush(struct ath10k *ar, u32 vdev_id,
+			     const u8 peer_addr[ETH_ALEN], u32 tid_bitmap)
 {
 	struct wmi_peer_flush_tids_cmd *cmd;
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_peer_flush_tids_cmd *)skb->data;
 	cmd->vdev_id         = __cpu_to_le32(vdev_id);
@@ -4003,19 +4371,21 @@ int ath10k_wmi_peer_flush(struct ath10k *ar, u32 vdev_id,
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi peer flush vdev_id %d peer_addr %pM tids %08x\n",
 		   vdev_id, peer_addr, tid_bitmap);
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->peer_flush_tids_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_peer_set_param(struct ath10k *ar, u32 vdev_id,
-			      const u8 *peer_addr, enum wmi_peer_param param_id,
-			      u32 param_value)
+static struct sk_buff *
+ath10k_wmi_op_gen_peer_set_param(struct ath10k *ar, u32 vdev_id,
+				 const u8 *peer_addr,
+				 enum wmi_peer_param param_id,
+				 u32 param_value)
 {
 	struct wmi_peer_set_param_cmd *cmd;
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_peer_set_param_cmd *)skb->data;
 	cmd->vdev_id     = __cpu_to_le32(vdev_id);
@@ -4026,19 +4396,19 @@ int ath10k_wmi_peer_set_param(struct ath10k *ar, u32 vdev_id,
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi vdev %d peer 0x%pM set param %d value %d\n",
 		   vdev_id, peer_addr, param_id, param_value);
-
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->peer_set_param_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_set_psmode(struct ath10k *ar, u32 vdev_id,
-			  enum wmi_sta_ps_mode psmode)
+static struct sk_buff *
+ath10k_wmi_op_gen_set_psmode(struct ath10k *ar, u32 vdev_id,
+			     enum wmi_sta_ps_mode psmode)
 {
 	struct wmi_sta_powersave_mode_cmd *cmd;
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_sta_powersave_mode_cmd *)skb->data;
 	cmd->vdev_id     = __cpu_to_le32(vdev_id);
@@ -4047,21 +4417,20 @@ int ath10k_wmi_set_psmode(struct ath10k *ar, u32 vdev_id,
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi set powersave id 0x%x mode %d\n",
 		   vdev_id, psmode);
-
-	return ath10k_wmi_cmd_send(ar, skb,
-				   ar->wmi.cmd->sta_powersave_mode_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_set_sta_ps_param(struct ath10k *ar, u32 vdev_id,
-				enum wmi_sta_powersave_param param_id,
-				u32 value)
+static struct sk_buff *
+ath10k_wmi_op_gen_set_sta_ps(struct ath10k *ar, u32 vdev_id,
+			     enum wmi_sta_powersave_param param_id,
+			     u32 value)
 {
 	struct wmi_sta_powersave_param_cmd *cmd;
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_sta_powersave_param_cmd *)skb->data;
 	cmd->vdev_id     = __cpu_to_le32(vdev_id);
@@ -4071,22 +4440,22 @@ int ath10k_wmi_set_sta_ps_param(struct ath10k *ar, u32 vdev_id,
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi sta ps param vdev_id 0x%x param %d value %d\n",
 		   vdev_id, param_id, value);
-	return ath10k_wmi_cmd_send(ar, skb,
-				   ar->wmi.cmd->sta_powersave_param_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_set_ap_ps_param(struct ath10k *ar, u32 vdev_id, const u8 *mac,
-			       enum wmi_ap_ps_peer_param param_id, u32 value)
+static struct sk_buff *
+ath10k_wmi_op_gen_set_ap_ps(struct ath10k *ar, u32 vdev_id, const u8 *mac,
+			    enum wmi_ap_ps_peer_param param_id, u32 value)
 {
 	struct wmi_ap_ps_peer_cmd *cmd;
 	struct sk_buff *skb;
 
 	if (!mac)
-		return -EINVAL;
+		return ERR_PTR(-EINVAL);
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_ap_ps_peer_cmd *)skb->data;
 	cmd->vdev_id = __cpu_to_le32(vdev_id);
@@ -4097,13 +4466,12 @@ int ath10k_wmi_set_ap_ps_param(struct ath10k *ar, u32 vdev_id, const u8 *mac,
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi ap ps param vdev_id 0x%X param %d value %d mac_addr %pM\n",
 		   vdev_id, param_id, value, mac);
-
-	return ath10k_wmi_cmd_send(ar, skb,
-				   ar->wmi.cmd->ap_ps_peer_param_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_scan_chan_list(struct ath10k *ar,
-			      const struct wmi_scan_chan_list_arg *arg)
+static struct sk_buff *
+ath10k_wmi_op_gen_scan_chan_list(struct ath10k *ar,
+				 const struct wmi_scan_chan_list_arg *arg)
 {
 	struct wmi_scan_chan_list_cmd *cmd;
 	struct sk_buff *skb;
@@ -4116,7 +4484,7 @@ int ath10k_wmi_scan_chan_list(struct ath10k *ar,
 
 	skb = ath10k_wmi_alloc_skb(ar, len);
 	if (!skb)
-		return -EINVAL;
+		return ERR_PTR(-EINVAL);
 
 	cmd = (struct wmi_scan_chan_list_cmd *)skb->data;
 	cmd->num_scan_chans = __cpu_to_le32(arg->n_channels);
@@ -4128,7 +4496,7 @@ int ath10k_wmi_scan_chan_list(struct ath10k *ar,
 		ath10k_wmi_put_wmi_channel(ci, ch);
 	}
 
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->scan_chan_list_cmdid);
+	return skb;
 }
 
 static void
@@ -4209,12 +4577,9 @@ ath10k_wmi_peer_assoc_fill_10_2(struct ath10k *ar, void *buf,
 	cmd->info0 = __cpu_to_le32(info0);
 }
 
-int ath10k_wmi_peer_assoc(struct ath10k *ar,
-			  const struct wmi_peer_assoc_complete_arg *arg)
+static int
+ath10k_wmi_peer_assoc_check_arg(const struct wmi_peer_assoc_complete_arg *arg)
 {
-	struct sk_buff *skb;
-	int len;
-
 	if (arg->peer_mpdu_density > 16)
 		return -EINVAL;
 	if (arg->peer_legacy_rates.num_rates > MAX_SUPPORTED_RATES)
@@ -4222,49 +4587,111 @@ int ath10k_wmi_peer_assoc(struct ath10k *ar,
 	if (arg->peer_ht_rates.num_rates > MAX_SUPPORTED_RATES)
 		return -EINVAL;
 
-	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features)) {
-		if (test_bit(ATH10K_FW_FEATURE_WMI_10_2, ar->fw_features))
-			len = sizeof(struct wmi_10_2_peer_assoc_complete_cmd);
-		else
-			len = sizeof(struct wmi_10_1_peer_assoc_complete_cmd);
-	} else {
-		len = sizeof(struct wmi_main_peer_assoc_complete_cmd);
-	}
+	return 0;
+}
+
+static struct sk_buff *
+ath10k_wmi_op_gen_peer_assoc(struct ath10k *ar,
+			     const struct wmi_peer_assoc_complete_arg *arg)
+{
+	size_t len = sizeof(struct wmi_main_peer_assoc_complete_cmd);
+	struct sk_buff *skb;
+	int ret;
+
+	ret = ath10k_wmi_peer_assoc_check_arg(arg);
+	if (ret)
+		return ERR_PTR(ret);
 
 	skb = ath10k_wmi_alloc_skb(ar, len);
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
-	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features)) {
-		if (test_bit(ATH10K_FW_FEATURE_WMI_10_2, ar->fw_features))
-			ath10k_wmi_peer_assoc_fill_10_2(ar, skb->data, arg);
-		else
-			ath10k_wmi_peer_assoc_fill_10_1(ar, skb->data, arg);
-	} else {
-		ath10k_wmi_peer_assoc_fill_main(ar, skb->data, arg);
-	}
+	ath10k_wmi_peer_assoc_fill_main(ar, skb->data, arg);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
+		   "wmi peer assoc vdev %d addr %pM (%s)\n",
+		   arg->vdev_id, arg->addr,
+		   arg->peer_reassoc ? "reassociate" : "new");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_10_1_op_gen_peer_assoc(struct ath10k *ar,
+				  const struct wmi_peer_assoc_complete_arg *arg)
+{
+	size_t len = sizeof(struct wmi_10_1_peer_assoc_complete_cmd);
+	struct sk_buff *skb;
+	int ret;
+
+	ret = ath10k_wmi_peer_assoc_check_arg(arg);
+	if (ret)
+		return ERR_PTR(ret);
+
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	ath10k_wmi_peer_assoc_fill_10_1(ar, skb->data, arg);
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi peer assoc vdev %d addr %pM (%s)\n",
 		   arg->vdev_id, arg->addr,
 		   arg->peer_reassoc ? "reassociate" : "new");
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->peer_assoc_cmdid);
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_10_2_op_gen_peer_assoc(struct ath10k *ar,
+				  const struct wmi_peer_assoc_complete_arg *arg)
+{
+	size_t len = sizeof(struct wmi_10_2_peer_assoc_complete_cmd);
+	struct sk_buff *skb;
+	int ret;
+
+	ret = ath10k_wmi_peer_assoc_check_arg(arg);
+	if (ret)
+		return ERR_PTR(ret);
+
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	ath10k_wmi_peer_assoc_fill_10_2(ar, skb->data, arg);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
+		   "wmi peer assoc vdev %d addr %pM (%s)\n",
+		   arg->vdev_id, arg->addr,
+		   arg->peer_reassoc ? "reassociate" : "new");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_10_2_op_gen_pdev_get_temperature(struct ath10k *ar)
+{
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, 0);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi pdev get temperature\n");
+	return skb;
 }
 
 /* This function assumes the beacon is already DMA mapped */
-int ath10k_wmi_beacon_send_ref_nowait(struct ath10k_vif *arvif)
+static struct sk_buff *
+ath10k_wmi_op_gen_beacon_dma(struct ath10k_vif *arvif)
 {
+	struct ath10k *ar = arvif->ar;
 	struct wmi_bcn_tx_ref_cmd *cmd;
 	struct sk_buff *skb;
 	struct sk_buff *beacon = arvif->beacon;
-	struct ath10k *ar = arvif->ar;
 	struct ieee80211_hdr *hdr;
-	int ret;
 	u16 fc;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	hdr = (struct ieee80211_hdr *)beacon->data;
 	fc = le16_to_cpu(hdr->frame_control);
@@ -4284,17 +4711,11 @@ int ath10k_wmi_beacon_send_ref_nowait(struct ath10k_vif *arvif)
 	if (ATH10K_SKB_CB(beacon)->bcn.deliver_cab)
 		cmd->flags |= __cpu_to_le32(WMI_BCN_TX_REF_FLAG_DELIVER_CAB);
 
-	ret = ath10k_wmi_cmd_send_nowait(ar, skb,
-					 ar->wmi.cmd->pdev_send_bcn_cmdid);
-
-	if (ret)
-		dev_kfree_skb(skb);
-
-	return ret;
+	return skb;
 }
 
-static void ath10k_wmi_pdev_set_wmm_param(struct wmi_wmm_params *params,
-					  const struct wmi_wmm_params_arg *arg)
+void ath10k_wmi_pdev_set_wmm_param(struct wmi_wmm_params *params,
+				   const struct wmi_wmm_params_arg *arg)
 {
 	params->cwmin  = __cpu_to_le32(arg->cwmin);
 	params->cwmax  = __cpu_to_le32(arg->cwmax);
@@ -4304,15 +4725,16 @@ static void ath10k_wmi_pdev_set_wmm_param(struct wmi_wmm_params *params,
 	params->no_ack = __cpu_to_le32(arg->no_ack);
 }
 
-int ath10k_wmi_pdev_set_wmm_params(struct ath10k *ar,
-				   const struct wmi_pdev_set_wmm_params_arg *arg)
+static struct sk_buff *
+ath10k_wmi_op_gen_pdev_set_wmm(struct ath10k *ar,
+			       const struct wmi_pdev_set_wmm_params_arg *arg)
 {
 	struct wmi_pdev_set_wmm_params *cmd;
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_pdev_set_wmm_params *)skb->data;
 	ath10k_wmi_pdev_set_wmm_param(&cmd->ac_be, &arg->ac_be);
@@ -4321,35 +4743,36 @@ int ath10k_wmi_pdev_set_wmm_params(struct ath10k *ar,
 	ath10k_wmi_pdev_set_wmm_param(&cmd->ac_vo, &arg->ac_vo);
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi pdev set wmm params\n");
-	return ath10k_wmi_cmd_send(ar, skb,
-				   ar->wmi.cmd->pdev_set_wmm_params_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_request_stats(struct ath10k *ar, enum wmi_stats_id stats_id)
+static struct sk_buff *
+ath10k_wmi_op_gen_request_stats(struct ath10k *ar, enum wmi_stats_id stats_id)
 {
 	struct wmi_request_stats_cmd *cmd;
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_request_stats_cmd *)skb->data;
 	cmd->stats_id = __cpu_to_le32(stats_id);
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi request stats %d\n", (int)stats_id);
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->request_stats_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_force_fw_hang(struct ath10k *ar,
-			     enum wmi_force_fw_hang_type type, u32 delay_ms)
+static struct sk_buff *
+ath10k_wmi_op_gen_force_fw_hang(struct ath10k *ar,
+				enum wmi_force_fw_hang_type type, u32 delay_ms)
 {
 	struct wmi_force_fw_hang_cmd *cmd;
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_force_fw_hang_cmd *)skb->data;
 	cmd->type = __cpu_to_le32(type);
@@ -4357,10 +4780,11 @@ int ath10k_wmi_force_fw_hang(struct ath10k *ar,
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi force fw hang %d delay %d\n",
 		   type, delay_ms);
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->force_fw_hang_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_dbglog_cfg(struct ath10k *ar, u32 module_enable)
+static struct sk_buff *
+ath10k_wmi_op_gen_dbglog_cfg(struct ath10k *ar, u32 module_enable)
 {
 	struct wmi_dbglog_cfg_cmd *cmd;
 	struct sk_buff *skb;
@@ -4368,7 +4792,7 @@ int ath10k_wmi_dbglog_cfg(struct ath10k *ar, u32 module_enable)
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_dbglog_cfg_cmd *)skb->data;
 
@@ -4393,57 +4817,318 @@ int ath10k_wmi_dbglog_cfg(struct ath10k *ar, u32 module_enable)
 		   __le32_to_cpu(cmd->module_valid),
 		   __le32_to_cpu(cmd->config_enable),
 		   __le32_to_cpu(cmd->config_valid));
-
-	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->dbglog_cfg_cmdid);
+	return skb;
 }
 
-int ath10k_wmi_pdev_pktlog_enable(struct ath10k *ar, u32 ev_bitmap)
+static struct sk_buff *
+ath10k_wmi_op_gen_pktlog_enable(struct ath10k *ar, u32 ev_bitmap)
 {
 	struct wmi_pdev_pktlog_enable_cmd *cmd;
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	ev_bitmap &= ATH10K_PKTLOG_ANY;
-	ath10k_dbg(ar, ATH10K_DBG_WMI,
-		   "wmi enable pktlog filter:%x\n", ev_bitmap);
 
 	cmd = (struct wmi_pdev_pktlog_enable_cmd *)skb->data;
 	cmd->ev_bitmap = __cpu_to_le32(ev_bitmap);
-	return ath10k_wmi_cmd_send(ar, skb,
-				   ar->wmi.cmd->pdev_pktlog_enable_cmdid);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi enable pktlog filter 0x%08x\n",
+		   ev_bitmap);
+	return skb;
 }
 
-int ath10k_wmi_pdev_pktlog_disable(struct ath10k *ar)
+static struct sk_buff *
+ath10k_wmi_op_gen_pktlog_disable(struct ath10k *ar)
 {
 	struct sk_buff *skb;
 
 	skb = ath10k_wmi_alloc_skb(ar, 0);
 	if (!skb)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi disable pktlog\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_op_gen_pdev_set_quiet_mode(struct ath10k *ar, u32 period,
+				      u32 duration, u32 next_offset,
+				      u32 enabled)
+{
+	struct wmi_pdev_set_quiet_cmd *cmd;
+	struct sk_buff *skb;
 
-	return ath10k_wmi_cmd_send(ar, skb,
-				   ar->wmi.cmd->pdev_pktlog_disable_cmdid);
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	cmd = (struct wmi_pdev_set_quiet_cmd *)skb->data;
+	cmd->period = __cpu_to_le32(period);
+	cmd->duration = __cpu_to_le32(duration);
+	cmd->next_start = __cpu_to_le32(next_offset);
+	cmd->enabled = __cpu_to_le32(enabled);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
+		   "wmi quiet param: period %u duration %u enabled %d\n",
+		   period, duration, enabled);
+	return skb;
 }
 
+static const struct wmi_ops wmi_ops = {
+	.rx = ath10k_wmi_op_rx,
+	.map_svc = wmi_main_svc_map,
+
+	.pull_scan = ath10k_wmi_op_pull_scan_ev,
+	.pull_mgmt_rx = ath10k_wmi_op_pull_mgmt_rx_ev,
+	.pull_ch_info = ath10k_wmi_op_pull_ch_info_ev,
+	.pull_vdev_start = ath10k_wmi_op_pull_vdev_start_ev,
+	.pull_peer_kick = ath10k_wmi_op_pull_peer_kick_ev,
+	.pull_swba = ath10k_wmi_op_pull_swba_ev,
+	.pull_phyerr = ath10k_wmi_op_pull_phyerr_ev,
+	.pull_svc_rdy = ath10k_wmi_main_op_pull_svc_rdy_ev,
+	.pull_rdy = ath10k_wmi_op_pull_rdy_ev,
+	.pull_fw_stats = ath10k_wmi_main_op_pull_fw_stats,
+
+	.gen_pdev_suspend = ath10k_wmi_op_gen_pdev_suspend,
+	.gen_pdev_resume = ath10k_wmi_op_gen_pdev_resume,
+	.gen_pdev_set_rd = ath10k_wmi_op_gen_pdev_set_rd,
+	.gen_pdev_set_param = ath10k_wmi_op_gen_pdev_set_param,
+	.gen_init = ath10k_wmi_op_gen_init,
+	.gen_start_scan = ath10k_wmi_op_gen_start_scan,
+	.gen_stop_scan = ath10k_wmi_op_gen_stop_scan,
+	.gen_vdev_create = ath10k_wmi_op_gen_vdev_create,
+	.gen_vdev_delete = ath10k_wmi_op_gen_vdev_delete,
+	.gen_vdev_start = ath10k_wmi_op_gen_vdev_start,
+	.gen_vdev_stop = ath10k_wmi_op_gen_vdev_stop,
+	.gen_vdev_up = ath10k_wmi_op_gen_vdev_up,
+	.gen_vdev_down = ath10k_wmi_op_gen_vdev_down,
+	.gen_vdev_set_param = ath10k_wmi_op_gen_vdev_set_param,
+	.gen_vdev_install_key = ath10k_wmi_op_gen_vdev_install_key,
+	.gen_vdev_spectral_conf = ath10k_wmi_op_gen_vdev_spectral_conf,
+	.gen_vdev_spectral_enable = ath10k_wmi_op_gen_vdev_spectral_enable,
+	.gen_peer_create = ath10k_wmi_op_gen_peer_create,
+	.gen_peer_delete = ath10k_wmi_op_gen_peer_delete,
+	.gen_peer_flush = ath10k_wmi_op_gen_peer_flush,
+	.gen_peer_set_param = ath10k_wmi_op_gen_peer_set_param,
+	.gen_peer_assoc = ath10k_wmi_op_gen_peer_assoc,
+	.gen_set_psmode = ath10k_wmi_op_gen_set_psmode,
+	.gen_set_sta_ps = ath10k_wmi_op_gen_set_sta_ps,
+	.gen_set_ap_ps = ath10k_wmi_op_gen_set_ap_ps,
+	.gen_scan_chan_list = ath10k_wmi_op_gen_scan_chan_list,
+	.gen_beacon_dma = ath10k_wmi_op_gen_beacon_dma,
+	.gen_pdev_set_wmm = ath10k_wmi_op_gen_pdev_set_wmm,
+	.gen_request_stats = ath10k_wmi_op_gen_request_stats,
+	.gen_force_fw_hang = ath10k_wmi_op_gen_force_fw_hang,
+	.gen_mgmt_tx = ath10k_wmi_op_gen_mgmt_tx,
+	.gen_dbglog_cfg = ath10k_wmi_op_gen_dbglog_cfg,
+	.gen_pktlog_enable = ath10k_wmi_op_gen_pktlog_enable,
+	.gen_pktlog_disable = ath10k_wmi_op_gen_pktlog_disable,
+	.gen_pdev_set_quiet_mode = ath10k_wmi_op_gen_pdev_set_quiet_mode,
+	/* .gen_pdev_get_temperature not implemented */
+};
+
+static const struct wmi_ops wmi_10_1_ops = {
+	.rx = ath10k_wmi_10_1_op_rx,
+	.map_svc = wmi_10x_svc_map,
+	.pull_svc_rdy = ath10k_wmi_10x_op_pull_svc_rdy_ev,
+	.pull_fw_stats = ath10k_wmi_10x_op_pull_fw_stats,
+	.gen_init = ath10k_wmi_10_1_op_gen_init,
+	.gen_pdev_set_rd = ath10k_wmi_10x_op_gen_pdev_set_rd,
+	.gen_start_scan = ath10k_wmi_10x_op_gen_start_scan,
+	.gen_peer_assoc = ath10k_wmi_10_1_op_gen_peer_assoc,
+	/* .gen_pdev_get_temperature not implemented */
+
+	/* shared with main branch */
+	.pull_scan = ath10k_wmi_op_pull_scan_ev,
+	.pull_mgmt_rx = ath10k_wmi_op_pull_mgmt_rx_ev,
+	.pull_ch_info = ath10k_wmi_op_pull_ch_info_ev,
+	.pull_vdev_start = ath10k_wmi_op_pull_vdev_start_ev,
+	.pull_peer_kick = ath10k_wmi_op_pull_peer_kick_ev,
+	.pull_swba = ath10k_wmi_op_pull_swba_ev,
+	.pull_phyerr = ath10k_wmi_op_pull_phyerr_ev,
+	.pull_rdy = ath10k_wmi_op_pull_rdy_ev,
+
+	.gen_pdev_suspend = ath10k_wmi_op_gen_pdev_suspend,
+	.gen_pdev_resume = ath10k_wmi_op_gen_pdev_resume,
+	.gen_pdev_set_param = ath10k_wmi_op_gen_pdev_set_param,
+	.gen_stop_scan = ath10k_wmi_op_gen_stop_scan,
+	.gen_vdev_create = ath10k_wmi_op_gen_vdev_create,
+	.gen_vdev_delete = ath10k_wmi_op_gen_vdev_delete,
+	.gen_vdev_start = ath10k_wmi_op_gen_vdev_start,
+	.gen_vdev_stop = ath10k_wmi_op_gen_vdev_stop,
+	.gen_vdev_up = ath10k_wmi_op_gen_vdev_up,
+	.gen_vdev_down = ath10k_wmi_op_gen_vdev_down,
+	.gen_vdev_set_param = ath10k_wmi_op_gen_vdev_set_param,
+	.gen_vdev_install_key = ath10k_wmi_op_gen_vdev_install_key,
+	.gen_vdev_spectral_conf = ath10k_wmi_op_gen_vdev_spectral_conf,
+	.gen_vdev_spectral_enable = ath10k_wmi_op_gen_vdev_spectral_enable,
+	.gen_peer_create = ath10k_wmi_op_gen_peer_create,
+	.gen_peer_delete = ath10k_wmi_op_gen_peer_delete,
+	.gen_peer_flush = ath10k_wmi_op_gen_peer_flush,
+	.gen_peer_set_param = ath10k_wmi_op_gen_peer_set_param,
+	.gen_set_psmode = ath10k_wmi_op_gen_set_psmode,
+	.gen_set_sta_ps = ath10k_wmi_op_gen_set_sta_ps,
+	.gen_set_ap_ps = ath10k_wmi_op_gen_set_ap_ps,
+	.gen_scan_chan_list = ath10k_wmi_op_gen_scan_chan_list,
+	.gen_beacon_dma = ath10k_wmi_op_gen_beacon_dma,
+	.gen_pdev_set_wmm = ath10k_wmi_op_gen_pdev_set_wmm,
+	.gen_request_stats = ath10k_wmi_op_gen_request_stats,
+	.gen_force_fw_hang = ath10k_wmi_op_gen_force_fw_hang,
+	.gen_mgmt_tx = ath10k_wmi_op_gen_mgmt_tx,
+	.gen_dbglog_cfg = ath10k_wmi_op_gen_dbglog_cfg,
+	.gen_pktlog_enable = ath10k_wmi_op_gen_pktlog_enable,
+	.gen_pktlog_disable = ath10k_wmi_op_gen_pktlog_disable,
+	.gen_pdev_set_quiet_mode = ath10k_wmi_op_gen_pdev_set_quiet_mode,
+};
+
+static const struct wmi_ops wmi_10_2_ops = {
+	.rx = ath10k_wmi_10_2_op_rx,
+	.gen_init = ath10k_wmi_10_2_op_gen_init,
+	.gen_peer_assoc = ath10k_wmi_10_2_op_gen_peer_assoc,
+	/* .gen_pdev_get_temperature not implemented */
+
+	/* shared with 10.1 */
+	.map_svc = wmi_10x_svc_map,
+	.pull_svc_rdy = ath10k_wmi_10x_op_pull_svc_rdy_ev,
+	.pull_fw_stats = ath10k_wmi_10x_op_pull_fw_stats,
+	.gen_pdev_set_rd = ath10k_wmi_10x_op_gen_pdev_set_rd,
+	.gen_start_scan = ath10k_wmi_10x_op_gen_start_scan,
+
+	.pull_scan = ath10k_wmi_op_pull_scan_ev,
+	.pull_mgmt_rx = ath10k_wmi_op_pull_mgmt_rx_ev,
+	.pull_ch_info = ath10k_wmi_op_pull_ch_info_ev,
+	.pull_vdev_start = ath10k_wmi_op_pull_vdev_start_ev,
+	.pull_peer_kick = ath10k_wmi_op_pull_peer_kick_ev,
+	.pull_swba = ath10k_wmi_op_pull_swba_ev,
+	.pull_phyerr = ath10k_wmi_op_pull_phyerr_ev,
+	.pull_rdy = ath10k_wmi_op_pull_rdy_ev,
+
+	.gen_pdev_suspend = ath10k_wmi_op_gen_pdev_suspend,
+	.gen_pdev_resume = ath10k_wmi_op_gen_pdev_resume,
+	.gen_pdev_set_param = ath10k_wmi_op_gen_pdev_set_param,
+	.gen_stop_scan = ath10k_wmi_op_gen_stop_scan,
+	.gen_vdev_create = ath10k_wmi_op_gen_vdev_create,
+	.gen_vdev_delete = ath10k_wmi_op_gen_vdev_delete,
+	.gen_vdev_start = ath10k_wmi_op_gen_vdev_start,
+	.gen_vdev_stop = ath10k_wmi_op_gen_vdev_stop,
+	.gen_vdev_up = ath10k_wmi_op_gen_vdev_up,
+	.gen_vdev_down = ath10k_wmi_op_gen_vdev_down,
+	.gen_vdev_set_param = ath10k_wmi_op_gen_vdev_set_param,
+	.gen_vdev_install_key = ath10k_wmi_op_gen_vdev_install_key,
+	.gen_vdev_spectral_conf = ath10k_wmi_op_gen_vdev_spectral_conf,
+	.gen_vdev_spectral_enable = ath10k_wmi_op_gen_vdev_spectral_enable,
+	.gen_peer_create = ath10k_wmi_op_gen_peer_create,
+	.gen_peer_delete = ath10k_wmi_op_gen_peer_delete,
+	.gen_peer_flush = ath10k_wmi_op_gen_peer_flush,
+	.gen_peer_set_param = ath10k_wmi_op_gen_peer_set_param,
+	.gen_set_psmode = ath10k_wmi_op_gen_set_psmode,
+	.gen_set_sta_ps = ath10k_wmi_op_gen_set_sta_ps,
+	.gen_set_ap_ps = ath10k_wmi_op_gen_set_ap_ps,
+	.gen_scan_chan_list = ath10k_wmi_op_gen_scan_chan_list,
+	.gen_beacon_dma = ath10k_wmi_op_gen_beacon_dma,
+	.gen_pdev_set_wmm = ath10k_wmi_op_gen_pdev_set_wmm,
+	.gen_request_stats = ath10k_wmi_op_gen_request_stats,
+	.gen_force_fw_hang = ath10k_wmi_op_gen_force_fw_hang,
+	.gen_mgmt_tx = ath10k_wmi_op_gen_mgmt_tx,
+	.gen_dbglog_cfg = ath10k_wmi_op_gen_dbglog_cfg,
+	.gen_pktlog_enable = ath10k_wmi_op_gen_pktlog_enable,
+	.gen_pktlog_disable = ath10k_wmi_op_gen_pktlog_disable,
+	.gen_pdev_set_quiet_mode = ath10k_wmi_op_gen_pdev_set_quiet_mode,
+};
+
+static const struct wmi_ops wmi_10_2_4_ops = {
+	.rx = ath10k_wmi_10_2_op_rx,
+	.gen_init = ath10k_wmi_10_2_op_gen_init,
+	.gen_peer_assoc = ath10k_wmi_10_2_op_gen_peer_assoc,
+	.gen_pdev_get_temperature = ath10k_wmi_10_2_op_gen_pdev_get_temperature,
+
+	/* shared with 10.1 */
+	.map_svc = wmi_10x_svc_map,
+	.pull_svc_rdy = ath10k_wmi_10x_op_pull_svc_rdy_ev,
+	.pull_fw_stats = ath10k_wmi_10x_op_pull_fw_stats,
+	.gen_pdev_set_rd = ath10k_wmi_10x_op_gen_pdev_set_rd,
+	.gen_start_scan = ath10k_wmi_10x_op_gen_start_scan,
+
+	.pull_scan = ath10k_wmi_op_pull_scan_ev,
+	.pull_mgmt_rx = ath10k_wmi_op_pull_mgmt_rx_ev,
+	.pull_ch_info = ath10k_wmi_op_pull_ch_info_ev,
+	.pull_vdev_start = ath10k_wmi_op_pull_vdev_start_ev,
+	.pull_peer_kick = ath10k_wmi_op_pull_peer_kick_ev,
+	.pull_swba = ath10k_wmi_op_pull_swba_ev,
+	.pull_phyerr = ath10k_wmi_op_pull_phyerr_ev,
+	.pull_rdy = ath10k_wmi_op_pull_rdy_ev,
+
+	.gen_pdev_suspend = ath10k_wmi_op_gen_pdev_suspend,
+	.gen_pdev_resume = ath10k_wmi_op_gen_pdev_resume,
+	.gen_pdev_set_param = ath10k_wmi_op_gen_pdev_set_param,
+	.gen_stop_scan = ath10k_wmi_op_gen_stop_scan,
+	.gen_vdev_create = ath10k_wmi_op_gen_vdev_create,
+	.gen_vdev_delete = ath10k_wmi_op_gen_vdev_delete,
+	.gen_vdev_start = ath10k_wmi_op_gen_vdev_start,
+	.gen_vdev_stop = ath10k_wmi_op_gen_vdev_stop,
+	.gen_vdev_up = ath10k_wmi_op_gen_vdev_up,
+	.gen_vdev_down = ath10k_wmi_op_gen_vdev_down,
+	.gen_vdev_set_param = ath10k_wmi_op_gen_vdev_set_param,
+	.gen_vdev_install_key = ath10k_wmi_op_gen_vdev_install_key,
+	.gen_vdev_spectral_conf = ath10k_wmi_op_gen_vdev_spectral_conf,
+	.gen_vdev_spectral_enable = ath10k_wmi_op_gen_vdev_spectral_enable,
+	.gen_peer_create = ath10k_wmi_op_gen_peer_create,
+	.gen_peer_delete = ath10k_wmi_op_gen_peer_delete,
+	.gen_peer_flush = ath10k_wmi_op_gen_peer_flush,
+	.gen_peer_set_param = ath10k_wmi_op_gen_peer_set_param,
+	.gen_set_psmode = ath10k_wmi_op_gen_set_psmode,
+	.gen_set_sta_ps = ath10k_wmi_op_gen_set_sta_ps,
+	.gen_set_ap_ps = ath10k_wmi_op_gen_set_ap_ps,
+	.gen_scan_chan_list = ath10k_wmi_op_gen_scan_chan_list,
+	.gen_beacon_dma = ath10k_wmi_op_gen_beacon_dma,
+	.gen_pdev_set_wmm = ath10k_wmi_op_gen_pdev_set_wmm,
+	.gen_request_stats = ath10k_wmi_op_gen_request_stats,
+	.gen_force_fw_hang = ath10k_wmi_op_gen_force_fw_hang,
+	.gen_mgmt_tx = ath10k_wmi_op_gen_mgmt_tx,
+	.gen_dbglog_cfg = ath10k_wmi_op_gen_dbglog_cfg,
+	.gen_pktlog_enable = ath10k_wmi_op_gen_pktlog_enable,
+	.gen_pktlog_disable = ath10k_wmi_op_gen_pktlog_disable,
+	.gen_pdev_set_quiet_mode = ath10k_wmi_op_gen_pdev_set_quiet_mode,
+};
+
 int ath10k_wmi_attach(struct ath10k *ar)
 {
-	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features)) {
-		if (test_bit(ATH10K_FW_FEATURE_WMI_10_2, ar->fw_features))
-			ar->wmi.cmd = &wmi_10_2_cmd_map;
-		else
-			ar->wmi.cmd = &wmi_10x_cmd_map;
-
+	switch (ar->wmi.op_version) {
+	case ATH10K_FW_WMI_OP_VERSION_10_2_4:
+		ar->wmi.cmd = &wmi_10_2_4_cmd_map;
+		ar->wmi.ops = &wmi_10_2_4_ops;
+		ar->wmi.vdev_param = &wmi_10_2_4_vdev_param_map;
+		ar->wmi.pdev_param = &wmi_10_2_4_pdev_param_map;
+		break;
+	case ATH10K_FW_WMI_OP_VERSION_10_2:
+		ar->wmi.cmd = &wmi_10_2_cmd_map;
+		ar->wmi.ops = &wmi_10_2_ops;
 		ar->wmi.vdev_param = &wmi_10x_vdev_param_map;
 		ar->wmi.pdev_param = &wmi_10x_pdev_param_map;
-	} else {
+		break;
+	case ATH10K_FW_WMI_OP_VERSION_10_1:
+		ar->wmi.cmd = &wmi_10x_cmd_map;
+		ar->wmi.ops = &wmi_10_1_ops;
+		ar->wmi.vdev_param = &wmi_10x_vdev_param_map;
+		ar->wmi.pdev_param = &wmi_10x_pdev_param_map;
+		break;
+	case ATH10K_FW_WMI_OP_VERSION_MAIN:
 		ar->wmi.cmd = &wmi_cmd_map;
+		ar->wmi.ops = &wmi_ops;
 		ar->wmi.vdev_param = &wmi_vdev_param_map;
 		ar->wmi.pdev_param = &wmi_pdev_param_map;
+		break;
+	case ATH10K_FW_WMI_OP_VERSION_TLV:
+		ath10k_wmi_tlv_attach(ar);
+		break;
+	case ATH10K_FW_WMI_OP_VERSION_UNSET:
+	case ATH10K_FW_WMI_OP_VERSION_MAX:
+		ath10k_err(ar, "unsupported WMI op version: %d\n",
+			   ar->wmi.op_version);
+		return -EINVAL;
 	}
 
 	init_completion(&ar->wmi.service_ready);
diff --git a/drivers/net/wireless/ath/ath10k/wmi.h b/drivers/net/wireless/ath/ath10k/wmi.h
index 21391929d318..bd7f29a3a122 100644
--- a/drivers/net/wireless/ath/ath10k/wmi.h
+++ b/drivers/net/wireless/ath/ath10k/wmi.h
@@ -109,6 +109,45 @@ enum wmi_service {
 	WMI_SERVICE_BURST,
 	WMI_SERVICE_SMART_ANTENNA_SW_SUPPORT,
 	WMI_SERVICE_SMART_ANTENNA_HW_SUPPORT,
+	WMI_SERVICE_ROAM_SCAN_OFFLOAD,
+	WMI_SERVICE_AP_PS_DETECT_OUT_OF_SYNC,
+	WMI_SERVICE_EARLY_RX,
+	WMI_SERVICE_STA_SMPS,
+	WMI_SERVICE_FWTEST,
+	WMI_SERVICE_STA_WMMAC,
+	WMI_SERVICE_TDLS,
+	WMI_SERVICE_MCC_BCN_INTERVAL_CHANGE,
+	WMI_SERVICE_ADAPTIVE_OCS,
+	WMI_SERVICE_BA_SSN_SUPPORT,
+	WMI_SERVICE_FILTER_IPSEC_NATKEEPALIVE,
+	WMI_SERVICE_WLAN_HB,
+	WMI_SERVICE_LTE_ANT_SHARE_SUPPORT,
+	WMI_SERVICE_BATCH_SCAN,
+	WMI_SERVICE_QPOWER,
+	WMI_SERVICE_PLMREQ,
+	WMI_SERVICE_THERMAL_MGMT,
+	WMI_SERVICE_RMC,
+	WMI_SERVICE_MHF_OFFLOAD,
+	WMI_SERVICE_COEX_SAR,
+	WMI_SERVICE_BCN_TXRATE_OVERRIDE,
+	WMI_SERVICE_NAN,
+	WMI_SERVICE_L1SS_STAT,
+	WMI_SERVICE_ESTIMATE_LINKSPEED,
+	WMI_SERVICE_OBSS_SCAN,
+	WMI_SERVICE_TDLS_OFFCHAN,
+	WMI_SERVICE_TDLS_UAPSD_BUFFER_STA,
+	WMI_SERVICE_TDLS_UAPSD_SLEEP_STA,
+	WMI_SERVICE_IBSS_PWRSAVE,
+	WMI_SERVICE_LPASS,
+	WMI_SERVICE_EXTSCAN,
+	WMI_SERVICE_D0WOW,
+	WMI_SERVICE_HSOFFLOAD,
+	WMI_SERVICE_ROAM_HO_OFFLOAD,
+	WMI_SERVICE_RX_FULL_REORDER,
+	WMI_SERVICE_DHCP_OFFLOAD,
+	WMI_SERVICE_STA_RX_IPA_OFFLOAD_SUPPORT,
+	WMI_SERVICE_MDNS_OFFLOAD,
+	WMI_SERVICE_SAP_AUTH_OFFLOAD,
 
 	/* keep last */
 	WMI_SERVICE_MAX,
@@ -215,6 +254,45 @@ static inline char *wmi_service_name(int service_id)
 	SVCSTR(WMI_SERVICE_BURST);
 	SVCSTR(WMI_SERVICE_SMART_ANTENNA_SW_SUPPORT);
 	SVCSTR(WMI_SERVICE_SMART_ANTENNA_HW_SUPPORT);
+	SVCSTR(WMI_SERVICE_ROAM_SCAN_OFFLOAD);
+	SVCSTR(WMI_SERVICE_AP_PS_DETECT_OUT_OF_SYNC);
+	SVCSTR(WMI_SERVICE_EARLY_RX);
+	SVCSTR(WMI_SERVICE_STA_SMPS);
+	SVCSTR(WMI_SERVICE_FWTEST);
+	SVCSTR(WMI_SERVICE_STA_WMMAC);
+	SVCSTR(WMI_SERVICE_TDLS);
+	SVCSTR(WMI_SERVICE_MCC_BCN_INTERVAL_CHANGE);
+	SVCSTR(WMI_SERVICE_ADAPTIVE_OCS);
+	SVCSTR(WMI_SERVICE_BA_SSN_SUPPORT);
+	SVCSTR(WMI_SERVICE_FILTER_IPSEC_NATKEEPALIVE);
+	SVCSTR(WMI_SERVICE_WLAN_HB);
+	SVCSTR(WMI_SERVICE_LTE_ANT_SHARE_SUPPORT);
+	SVCSTR(WMI_SERVICE_BATCH_SCAN);
+	SVCSTR(WMI_SERVICE_QPOWER);
+	SVCSTR(WMI_SERVICE_PLMREQ);
+	SVCSTR(WMI_SERVICE_THERMAL_MGMT);
+	SVCSTR(WMI_SERVICE_RMC);
+	SVCSTR(WMI_SERVICE_MHF_OFFLOAD);
+	SVCSTR(WMI_SERVICE_COEX_SAR);
+	SVCSTR(WMI_SERVICE_BCN_TXRATE_OVERRIDE);
+	SVCSTR(WMI_SERVICE_NAN);
+	SVCSTR(WMI_SERVICE_L1SS_STAT);
+	SVCSTR(WMI_SERVICE_ESTIMATE_LINKSPEED);
+	SVCSTR(WMI_SERVICE_OBSS_SCAN);
+	SVCSTR(WMI_SERVICE_TDLS_OFFCHAN);
+	SVCSTR(WMI_SERVICE_TDLS_UAPSD_BUFFER_STA);
+	SVCSTR(WMI_SERVICE_TDLS_UAPSD_SLEEP_STA);
+	SVCSTR(WMI_SERVICE_IBSS_PWRSAVE);
+	SVCSTR(WMI_SERVICE_LPASS);
+	SVCSTR(WMI_SERVICE_EXTSCAN);
+	SVCSTR(WMI_SERVICE_D0WOW);
+	SVCSTR(WMI_SERVICE_HSOFFLOAD);
+	SVCSTR(WMI_SERVICE_ROAM_HO_OFFLOAD);
+	SVCSTR(WMI_SERVICE_RX_FULL_REORDER);
+	SVCSTR(WMI_SERVICE_DHCP_OFFLOAD);
+	SVCSTR(WMI_SERVICE_STA_RX_IPA_OFFLOAD_SUPPORT);
+	SVCSTR(WMI_SERVICE_MDNS_OFFLOAD);
+	SVCSTR(WMI_SERVICE_SAP_AUTH_OFFLOAD);
 	default:
 		return NULL;
 	}
@@ -472,6 +550,7 @@ struct wmi_cmd_map {
 	u32 force_fw_hang_cmdid;
 	u32 gpio_config_cmdid;
 	u32 gpio_output_cmdid;
+	u32 pdev_get_temperature_cmdid;
 };
 
 /*
@@ -1076,6 +1155,11 @@ enum wmi_10_2_cmd_id {
 	WMI_10_2_PDEV_SET_MIMOGAIN_TABLE_CMDID,
 	WMI_10_2_PDEV_RATEPWR_TABLE_CMDID,
 	WMI_10_2_PDEV_RATEPWR_CHAINMSK_TABLE_CMDID,
+	WMI_10_2_PDEV_GET_INFO,
+	WMI_10_2_VDEV_GET_INFO,
+	WMI_10_2_VDEV_ATF_REQUEST_CMDID,
+	WMI_10_2_PEER_ATF_REQUEST_CMDID,
+	WMI_10_2_PDEV_GET_TEMPERATURE_CMDID,
 	WMI_10_2_PDEV_UTF_CMDID = WMI_10_2_END_CMDID - 1,
 };
 
@@ -1117,6 +1201,8 @@ enum wmi_10_2_event_id {
 	WMI_10_2_MCAST_BUF_RELEASE_EVENTID,
 	WMI_10_2_MCAST_LIST_AGEOUT_EVENTID,
 	WMI_10_2_WDS_PEER_EVENTID,
+	WMI_10_2_PEER_STA_PS_STATECHG_EVENTID,
+	WMI_10_2_PDEV_TEMPERATURE_EVENTID,
 	WMI_10_2_PDEV_UTF_EVENTID = WMI_10_2_END_EVENTID - 1,
 };
 
@@ -1862,6 +1948,11 @@ struct wmi_resource_config_10x {
 	__le32 max_frag_entries;
 } __packed;
 
+enum wmi_10_2_feature_mask {
+	WMI_10_2_RX_BATCH_MODE = BIT(0),
+	WMI_10_2_ATF_CONFIG    = BIT(1),
+};
+
 struct wmi_resource_config_10_2 {
 	struct wmi_resource_config_10x common;
 	__le32 max_peer_ext_stats;
@@ -1870,7 +1961,7 @@ struct wmi_resource_config_10_2 {
 	__le32 be_min_free;
 	__le32 vi_min_free;
 	__le32 vo_min_free;
-	__le32 rx_batchmode; /* 0-disable, 1-enable */
+	__le32 feature_mask;
 } __packed;
 
 #define NUM_UNITS_IS_NUM_VDEVS   0x1
@@ -2505,6 +2596,7 @@ struct wmi_pdev_param_map {
 	u32 fast_channel_reset;
 	u32 burst_dur;
 	u32 burst_enable;
+	u32 cal_period;
 };
 
 #define WMI_PDEV_PARAM_UNSUPPORTED 0
@@ -2715,6 +2807,9 @@ enum wmi_10x_pdev_param {
 	WMI_10X_PDEV_PARAM_SET_MCAST2UCAST_MODE,
 	WMI_10X_PDEV_PARAM_SET_MCAST2UCAST_BUFFER,
 	WMI_10X_PDEV_PARAM_REMOVE_MCAST2UCAST_BUFFER,
+	WMI_10X_PDEV_PARAM_PEER_STA_PS_STATECHG_ENABLE,
+	WMI_10X_PDEV_PARAM_RTS_FIXED_RATE,
+	WMI_10X_PDEV_PARAM_CAL_PERIOD
 };
 
 struct wmi_pdev_set_param_cmd {
@@ -2722,6 +2817,9 @@ struct wmi_pdev_set_param_cmd {
 	__le32 param_value;
 } __packed;
 
+/* valid period is 1 ~ 60000ms, unit in millisecond */
+#define WMI_PDEV_PARAM_CAL_PERIOD_MAX 60000
+
 struct wmi_pdev_get_tpc_config_cmd {
 	/* parameter   */
 	__le32 param;
@@ -3930,6 +4028,13 @@ enum wmi_sta_ps_param_pspoll_count {
 	 * Values greater than 0 indicate the maximum numer of PS-Poll frames
 	 * FW will send before waking up.
 	 */
+
+	/* When u-APSD is enabled the firmware will be very reluctant to exit
+	 * STA PS. This could result in very poor Rx performance with STA doing
+	 * PS-Poll for each and every buffered frame. This value is a bit
+	 * arbitrary.
+	 */
+	WMI_STA_PS_PSPOLL_COUNT_UAPSD = 3,
 };
 
 /*
@@ -4120,7 +4225,7 @@ struct wmi_bcn_info {
 
 struct wmi_host_swba_event {
 	__le32 vdev_map;
-	struct wmi_bcn_info bcn_info[1];
+	struct wmi_bcn_info bcn_info[0];
 } __packed;
 
 #define WMI_MAX_AP_VDEV 16
@@ -4567,6 +4672,58 @@ struct wmi_dbglog_cfg_cmd {
 
 #define WMI_MAX_MEM_REQS 16
 
+struct wmi_scan_ev_arg {
+	__le32 event_type; /* %WMI_SCAN_EVENT_ */
+	__le32 reason; /* %WMI_SCAN_REASON_ */
+	__le32 channel_freq; /* only valid for WMI_SCAN_EVENT_FOREIGN_CHANNEL */
+	__le32 scan_req_id;
+	__le32 scan_id;
+	__le32 vdev_id;
+};
+
+struct wmi_mgmt_rx_ev_arg {
+	__le32 channel;
+	__le32 snr;
+	__le32 rate;
+	__le32 phy_mode;
+	__le32 buf_len;
+	__le32 status; /* %WMI_RX_STATUS_ */
+};
+
+struct wmi_ch_info_ev_arg {
+	__le32 err_code;
+	__le32 freq;
+	__le32 cmd_flags;
+	__le32 noise_floor;
+	__le32 rx_clear_count;
+	__le32 cycle_count;
+};
+
+struct wmi_vdev_start_ev_arg {
+	__le32 vdev_id;
+	__le32 req_id;
+	__le32 resp_type; /* %WMI_VDEV_RESP_ */
+	__le32 status;
+};
+
+struct wmi_peer_kick_ev_arg {
+	const u8 *mac_addr;
+};
+
+struct wmi_swba_ev_arg {
+	__le32 vdev_map;
+	const struct wmi_tim_info *tim_info[WMI_MAX_AP_VDEV];
+	const struct wmi_p2p_noa_info *noa_info[WMI_MAX_AP_VDEV];
+};
+
+struct wmi_phyerr_ev_arg {
+	__le32 num_phyerrs;
+	__le32 tsf_l32;
+	__le32 tsf_u32;
+	__le32 buf_len;
+	const struct wmi_phyerr *phyerrs;
+};
+
 struct wmi_svc_rdy_ev_arg {
 	__le32 min_tx_power;
 	__le32 max_tx_power;
@@ -4574,6 +4731,7 @@ struct wmi_svc_rdy_ev_arg {
 	__le32 vht_cap;
 	__le32 sw_ver0;
 	__le32 sw_ver1;
+	__le32 fw_build;
 	__le32 phy_capab;
 	__le32 num_rf_chains;
 	__le32 eeprom_rd;
@@ -4583,83 +4741,93 @@ struct wmi_svc_rdy_ev_arg {
 	const struct wlan_host_mem_req *mem_reqs[WMI_MAX_MEM_REQS];
 };
 
+struct wmi_rdy_ev_arg {
+	__le32 sw_version;
+	__le32 abi_version;
+	__le32 status;
+	const u8 *mac_addr;
+};
+
+struct wmi_pdev_temperature_event {
+	/* temperature value in Celcius degree */
+	__le32 temperature;
+} __packed;
+
 struct ath10k;
 struct ath10k_vif;
-struct ath10k_fw_stats;
+struct ath10k_fw_stats_pdev;
+struct ath10k_fw_stats_peer;
 
 int ath10k_wmi_attach(struct ath10k *ar);
 void ath10k_wmi_detach(struct ath10k *ar);
 int ath10k_wmi_wait_for_service_ready(struct ath10k *ar);
 int ath10k_wmi_wait_for_unified_ready(struct ath10k *ar);
 
+struct sk_buff *ath10k_wmi_alloc_skb(struct ath10k *ar, u32 len);
 int ath10k_wmi_connect(struct ath10k *ar);
 
 struct sk_buff *ath10k_wmi_alloc_skb(struct ath10k *ar, u32 len);
 int ath10k_wmi_cmd_send(struct ath10k *ar, struct sk_buff *skb, u32 cmd_id);
-
-int ath10k_wmi_pdev_suspend_target(struct ath10k *ar, u32 suspend_opt);
-int ath10k_wmi_pdev_resume_target(struct ath10k *ar);
-int ath10k_wmi_pdev_set_regdomain(struct ath10k *ar, u16 rd, u16 rd2g,
-				  u16 rd5g, u16 ctl2g, u16 ctl5g,
-				  enum wmi_dfs_region dfs_reg);
-int ath10k_wmi_pdev_set_param(struct ath10k *ar, u32 id, u32 value);
-int ath10k_wmi_cmd_init(struct ath10k *ar);
-int ath10k_wmi_start_scan(struct ath10k *ar, const struct wmi_start_scan_arg *);
+int ath10k_wmi_cmd_send_nowait(struct ath10k *ar, struct sk_buff *skb,
+			       u32 cmd_id);
 void ath10k_wmi_start_scan_init(struct ath10k *ar, struct wmi_start_scan_arg *);
-int ath10k_wmi_stop_scan(struct ath10k *ar,
-			 const struct wmi_stop_scan_arg *arg);
-int ath10k_wmi_vdev_create(struct ath10k *ar, u32 vdev_id,
-			   enum wmi_vdev_type type,
-			   enum wmi_vdev_subtype subtype,
-			   const u8 macaddr[ETH_ALEN]);
-int ath10k_wmi_vdev_delete(struct ath10k *ar, u32 vdev_id);
-int ath10k_wmi_vdev_start(struct ath10k *ar,
-			  const struct wmi_vdev_start_request_arg *);
-int ath10k_wmi_vdev_restart(struct ath10k *ar,
-			    const struct wmi_vdev_start_request_arg *);
-int ath10k_wmi_vdev_stop(struct ath10k *ar, u32 vdev_id);
-int ath10k_wmi_vdev_up(struct ath10k *ar, u32 vdev_id, u32 aid,
-		       const u8 *bssid);
-int ath10k_wmi_vdev_down(struct ath10k *ar, u32 vdev_id);
-int ath10k_wmi_vdev_set_param(struct ath10k *ar, u32 vdev_id,
-			      u32 param_id, u32 param_value);
-int ath10k_wmi_vdev_install_key(struct ath10k *ar,
-				const struct wmi_vdev_install_key_arg *arg);
-int ath10k_wmi_vdev_spectral_conf(struct ath10k *ar,
-				  const struct wmi_vdev_spectral_conf_arg *arg);
-int ath10k_wmi_vdev_spectral_enable(struct ath10k *ar, u32 vdev_id, u32 trigger,
-				    u32 enable);
-int ath10k_wmi_peer_create(struct ath10k *ar, u32 vdev_id,
-			   const u8 peer_addr[ETH_ALEN]);
-int ath10k_wmi_peer_delete(struct ath10k *ar, u32 vdev_id,
-			   const u8 peer_addr[ETH_ALEN]);
-int ath10k_wmi_peer_flush(struct ath10k *ar, u32 vdev_id,
-			  const u8 peer_addr[ETH_ALEN], u32 tid_bitmap);
-int ath10k_wmi_peer_set_param(struct ath10k *ar, u32 vdev_id,
-			      const u8 *peer_addr,
-			      enum wmi_peer_param param_id, u32 param_value);
-int ath10k_wmi_peer_assoc(struct ath10k *ar,
-			  const struct wmi_peer_assoc_complete_arg *arg);
-int ath10k_wmi_set_psmode(struct ath10k *ar, u32 vdev_id,
-			  enum wmi_sta_ps_mode psmode);
-int ath10k_wmi_set_sta_ps_param(struct ath10k *ar, u32 vdev_id,
-				enum wmi_sta_powersave_param param_id,
-				u32 value);
-int ath10k_wmi_set_ap_ps_param(struct ath10k *ar, u32 vdev_id, const u8 *mac,
-			       enum wmi_ap_ps_peer_param param_id, u32 value);
-int ath10k_wmi_scan_chan_list(struct ath10k *ar,
-			      const struct wmi_scan_chan_list_arg *arg);
-int ath10k_wmi_beacon_send_ref_nowait(struct ath10k_vif *arvif);
-int ath10k_wmi_pdev_set_wmm_params(struct ath10k *ar,
-				   const struct wmi_pdev_set_wmm_params_arg *arg);
-int ath10k_wmi_request_stats(struct ath10k *ar, enum wmi_stats_id stats_id);
-int ath10k_wmi_force_fw_hang(struct ath10k *ar,
-			     enum wmi_force_fw_hang_type type, u32 delay_ms);
-int ath10k_wmi_mgmt_tx(struct ath10k *ar, struct sk_buff *skb);
-int ath10k_wmi_dbglog_cfg(struct ath10k *ar, u32 module_enable);
-int ath10k_wmi_pull_fw_stats(struct ath10k *ar, struct sk_buff *skb,
-			     struct ath10k_fw_stats *stats);
-int ath10k_wmi_pdev_pktlog_enable(struct ath10k *ar, u32 ev_list);
-int ath10k_wmi_pdev_pktlog_disable(struct ath10k *ar);
+
+void ath10k_wmi_pull_pdev_stats(const struct wmi_pdev_stats *src,
+				struct ath10k_fw_stats_pdev *dst);
+void ath10k_wmi_pull_peer_stats(const struct wmi_peer_stats *src,
+				struct ath10k_fw_stats_peer *dst);
+void ath10k_wmi_put_host_mem_chunks(struct ath10k *ar,
+				    struct wmi_host_mem_chunks *chunks);
+void ath10k_wmi_put_start_scan_common(struct wmi_start_scan_common *cmn,
+				      const struct wmi_start_scan_arg *arg);
+void ath10k_wmi_pdev_set_wmm_param(struct wmi_wmm_params *params,
+				   const struct wmi_wmm_params_arg *arg);
+void ath10k_wmi_put_wmi_channel(struct wmi_channel *ch,
+				const struct wmi_channel_arg *arg);
+int ath10k_wmi_start_scan_verify(const struct wmi_start_scan_arg *arg);
+
+int ath10k_wmi_event_scan(struct ath10k *ar, struct sk_buff *skb);
+int ath10k_wmi_event_mgmt_rx(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_chan_info(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_echo(struct ath10k *ar, struct sk_buff *skb);
+int ath10k_wmi_event_debug_mesg(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_update_stats(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_vdev_start_resp(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_vdev_stopped(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_peer_sta_kickout(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_host_swba(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_tbttoffset_update(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_dfs(struct ath10k *ar,
+			  const struct wmi_phyerr *phyerr, u64 tsf);
+void ath10k_wmi_event_spectral_scan(struct ath10k *ar,
+				    const struct wmi_phyerr *phyerr,
+				    u64 tsf);
+void ath10k_wmi_event_phyerr(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_roam(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_profile_match(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_debug_print(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_pdev_qvit(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_wlan_profile_data(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_rtt_measurement_report(struct ath10k *ar,
+					     struct sk_buff *skb);
+void ath10k_wmi_event_tsf_measurement_report(struct ath10k *ar,
+					     struct sk_buff *skb);
+void ath10k_wmi_event_rtt_error_report(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_wow_wakeup_host(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_dcs_interference(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_pdev_tpc_config(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_pdev_ftm_intg(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_gtk_offload_status(struct ath10k *ar,
+					 struct sk_buff *skb);
+void ath10k_wmi_event_gtk_rekey_fail(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_delba_complete(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_addba_complete(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_vdev_install_key_complete(struct ath10k *ar,
+						struct sk_buff *skb);
+void ath10k_wmi_event_inst_rssi_stats(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_vdev_standby_req(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_vdev_resume_req(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_wmi_event_service_ready(struct ath10k *ar, struct sk_buff *skb);
+int ath10k_wmi_event_ready(struct ath10k *ar, struct sk_buff *skb);
 
 #endif /* _WMI_H_ */
diff --git a/drivers/net/wireless/ath/ath5k/Kconfig b/drivers/net/wireless/ath/ath5k/Kconfig
index 93caf8e68901..2399a3921762 100644
--- a/drivers/net/wireless/ath/ath5k/Kconfig
+++ b/drivers/net/wireless/ath/ath5k/Kconfig
@@ -1,12 +1,13 @@
 config ATH5K
 	tristate "Atheros 5xxx wireless cards support"
-	depends on PCI && MAC80211
+	depends on (PCI || ATH25) && MAC80211
 	select ATH_COMMON
 	select MAC80211_LEDS
 	select LEDS_CLASS
 	select NEW_LEDS
 	select AVERAGE
-	select ATH5K_PCI
+	select ATH5K_AHB if ATH25
+	select ATH5K_PCI if !ATH25
 	---help---
 	  This module adds support for wireless adapters based on
 	  Atheros 5xxx chipset.
@@ -51,9 +52,16 @@ config ATH5K_TRACER
 
 	  If unsure, say N.
 
+config ATH5K_AHB
+	bool "Atheros 5xxx AHB bus support"
+	depends on ATH25
+	---help---
+	  This adds support for WiSoC type chipsets of the 5xxx Atheros
+	  family.
+
 config ATH5K_PCI
 	bool "Atheros 5xxx PCI bus support"
-	depends on PCI
+	depends on (!ATH25 && PCI)
 	---help---
 	  This adds support for PCI type chipsets of the 5xxx Atheros
 	  family.
diff --git a/drivers/net/wireless/ath/ath5k/Makefile b/drivers/net/wireless/ath/ath5k/Makefile
index 51e2d8668041..1b3a34f7f224 100644
--- a/drivers/net/wireless/ath/ath5k/Makefile
+++ b/drivers/net/wireless/ath/ath5k/Makefile
@@ -17,5 +17,6 @@ ath5k-y				+= ani.o
 ath5k-y				+= sysfs.o
 ath5k-y				+= mac80211-ops.o
 ath5k-$(CONFIG_ATH5K_DEBUG)	+= debug.o
+ath5k-$(CONFIG_ATH5K_AHB)	+= ahb.o
 ath5k-$(CONFIG_ATH5K_PCI)	+= pci.o
 obj-$(CONFIG_ATH5K)		+= ath5k.o
diff --git a/drivers/net/wireless/ath/ath5k/ahb.c b/drivers/net/wireless/ath/ath5k/ahb.c
new file mode 100644
index 000000000000..2ca88b593e4c
--- /dev/null
+++ b/drivers/net/wireless/ath/ath5k/ahb.c
@@ -0,0 +1,233 @@
+/*
+ * Copyright (c) 2008-2009 Atheros Communications Inc.
+ * Copyright (c) 2009 Gabor Juhos <juhosg@openwrt.org>
+ * Copyright (c) 2009 Imre Kaloz <kaloz@openwrt.org>
+ *
+ * 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/nl80211.h>
+#include <linux/platform_device.h>
+#include <linux/etherdevice.h>
+#include <linux/export.h>
+#include <ath25_platform.h>
+#include "ath5k.h"
+#include "debug.h"
+#include "base.h"
+#include "reg.h"
+
+/* return bus cachesize in 4B word units */
+static void ath5k_ahb_read_cachesize(struct ath_common *common, int *csz)
+{
+	*csz = L1_CACHE_BYTES >> 2;
+}
+
+static bool
+ath5k_ahb_eeprom_read(struct ath_common *common, u32 off, u16 *data)
+{
+	struct ath5k_hw *ah = common->priv;
+	struct platform_device *pdev = to_platform_device(ah->dev);
+	struct ar231x_board_config *bcfg = dev_get_platdata(&pdev->dev);
+	u16 *eeprom, *eeprom_end;
+
+	eeprom = (u16 *) bcfg->radio;
+	eeprom_end = ((void *) bcfg->config) + BOARD_CONFIG_BUFSZ;
+
+	eeprom += off;
+	if (eeprom > eeprom_end)
+		return false;
+
+	*data = *eeprom;
+	return true;
+}
+
+int ath5k_hw_read_srev(struct ath5k_hw *ah)
+{
+	struct platform_device *pdev = to_platform_device(ah->dev);
+	struct ar231x_board_config *bcfg = dev_get_platdata(&pdev->dev);
+	ah->ah_mac_srev = bcfg->devid;
+	return 0;
+}
+
+static int ath5k_ahb_eeprom_read_mac(struct ath5k_hw *ah, u8 *mac)
+{
+	struct platform_device *pdev = to_platform_device(ah->dev);
+	struct ar231x_board_config *bcfg = dev_get_platdata(&pdev->dev);
+	u8 *cfg_mac;
+
+	if (to_platform_device(ah->dev)->id == 0)
+		cfg_mac = bcfg->config->wlan0_mac;
+	else
+		cfg_mac = bcfg->config->wlan1_mac;
+
+	memcpy(mac, cfg_mac, ETH_ALEN);
+	return 0;
+}
+
+static const struct ath_bus_ops ath_ahb_bus_ops = {
+	.ath_bus_type = ATH_AHB,
+	.read_cachesize = ath5k_ahb_read_cachesize,
+	.eeprom_read = ath5k_ahb_eeprom_read,
+	.eeprom_read_mac = ath5k_ahb_eeprom_read_mac,
+};
+
+/*Initialization*/
+static int ath_ahb_probe(struct platform_device *pdev)
+{
+	struct ar231x_board_config *bcfg = dev_get_platdata(&pdev->dev);
+	struct ath5k_hw *ah;
+	struct ieee80211_hw *hw;
+	struct resource *res;
+	void __iomem *mem;
+	int irq;
+	int ret = 0;
+	u32 reg;
+
+	if (!dev_get_platdata(&pdev->dev)) {
+		dev_err(&pdev->dev, "no platform data specified\n");
+		ret = -EINVAL;
+		goto err_out;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "no memory resource found\n");
+		ret = -ENXIO;
+		goto err_out;
+	}
+
+	mem = ioremap_nocache(res->start, resource_size(res));
+	if (mem == NULL) {
+		dev_err(&pdev->dev, "ioremap failed\n");
+		ret = -ENOMEM;
+		goto err_out;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "no IRQ resource found\n");
+		ret = -ENXIO;
+		goto err_iounmap;
+	}
+
+	irq = res->start;
+
+	hw = ieee80211_alloc_hw(sizeof(struct ath5k_hw), &ath5k_hw_ops);
+	if (hw == NULL) {
+		dev_err(&pdev->dev, "no memory for ieee80211_hw\n");
+		ret = -ENOMEM;
+		goto err_iounmap;
+	}
+
+	ah = hw->priv;
+	ah->hw = hw;
+	ah->dev = &pdev->dev;
+	ah->iobase = mem;
+	ah->irq = irq;
+	ah->devid = bcfg->devid;
+
+	if (bcfg->devid >= AR5K_SREV_AR2315_R6) {
+		/* Enable WMAC AHB arbitration */
+		reg = ioread32((void __iomem *) AR5K_AR2315_AHB_ARB_CTL);
+		reg |= AR5K_AR2315_AHB_ARB_CTL_WLAN;
+		iowrite32(reg, (void __iomem *) AR5K_AR2315_AHB_ARB_CTL);
+
+		/* Enable global WMAC swapping */
+		reg = ioread32((void __iomem *) AR5K_AR2315_BYTESWAP);
+		reg |= AR5K_AR2315_BYTESWAP_WMAC;
+		iowrite32(reg, (void __iomem *) AR5K_AR2315_BYTESWAP);
+	} else {
+		/* Enable WMAC DMA access (assuming 5312 or 231x*/
+		/* TODO: check other platforms */
+		reg = ioread32((void __iomem *) AR5K_AR5312_ENABLE);
+		if (to_platform_device(ah->dev)->id == 0)
+			reg |= AR5K_AR5312_ENABLE_WLAN0;
+		else
+			reg |= AR5K_AR5312_ENABLE_WLAN1;
+		iowrite32(reg, (void __iomem *) AR5K_AR5312_ENABLE);
+
+		/*
+		 * On a dual-band AR5312, the multiband radio is only
+		 * used as pass-through. Disable 2 GHz support in the
+		 * driver for it
+		 */
+		if (to_platform_device(ah->dev)->id == 0 &&
+		    (bcfg->config->flags & (BD_WLAN0 | BD_WLAN1)) ==
+		     (BD_WLAN1 | BD_WLAN0))
+			ah->ah_capabilities.cap_needs_2GHz_ovr = true;
+		else
+			ah->ah_capabilities.cap_needs_2GHz_ovr = false;
+	}
+
+	ret = ath5k_init_ah(ah, &ath_ahb_bus_ops);
+	if (ret != 0) {
+		dev_err(&pdev->dev, "failed to attach device, err=%d\n", ret);
+		ret = -ENODEV;
+		goto err_free_hw;
+	}
+
+	platform_set_drvdata(pdev, hw);
+
+	return 0;
+
+ err_free_hw:
+	ieee80211_free_hw(hw);
+ err_iounmap:
+        iounmap(mem);
+ err_out:
+	return ret;
+}
+
+static int ath_ahb_remove(struct platform_device *pdev)
+{
+	struct ar231x_board_config *bcfg = dev_get_platdata(&pdev->dev);
+	struct ieee80211_hw *hw = platform_get_drvdata(pdev);
+	struct ath5k_hw *ah;
+	u32 reg;
+
+	if (!hw)
+		return 0;
+
+	ah = hw->priv;
+
+	if (bcfg->devid >= AR5K_SREV_AR2315_R6) {
+		/* Disable WMAC AHB arbitration */
+		reg = ioread32((void __iomem *) AR5K_AR2315_AHB_ARB_CTL);
+		reg &= ~AR5K_AR2315_AHB_ARB_CTL_WLAN;
+		iowrite32(reg, (void __iomem *) AR5K_AR2315_AHB_ARB_CTL);
+	} else {
+		/*Stop DMA access */
+		reg = ioread32((void __iomem *) AR5K_AR5312_ENABLE);
+		if (to_platform_device(ah->dev)->id == 0)
+			reg &= ~AR5K_AR5312_ENABLE_WLAN0;
+		else
+			reg &= ~AR5K_AR5312_ENABLE_WLAN1;
+		iowrite32(reg, (void __iomem *) AR5K_AR5312_ENABLE);
+	}
+
+	ath5k_deinit_ah(ah);
+	iounmap(ah->iobase);
+	ieee80211_free_hw(hw);
+
+	return 0;
+}
+
+static struct platform_driver ath_ahb_driver = {
+	.probe      = ath_ahb_probe,
+	.remove     = ath_ahb_remove,
+	.driver		= {
+		.name	= "ar231x-wmac",
+	},
+};
+
+module_platform_driver(ath_ahb_driver);
diff --git a/drivers/net/wireless/ath/ath5k/ath5k.h b/drivers/net/wireless/ath/ath5k/ath5k.h
index ed2468220216..1ed7a88aeea9 100644
--- a/drivers/net/wireless/ath/ath5k/ath5k.h
+++ b/drivers/net/wireless/ath/ath5k/ath5k.h
@@ -1647,6 +1647,32 @@ static inline struct ath_regulatory *ath5k_hw_regulatory(struct ath5k_hw *ah)
 	return &(ath5k_hw_common(ah)->regulatory);
 }
 
+#ifdef CONFIG_ATH5K_AHB
+#define AR5K_AR2315_PCI_BASE	((void __iomem *)0xb0100000)
+
+static inline void __iomem *ath5k_ahb_reg(struct ath5k_hw *ah, u16 reg)
+{
+	/* On AR2315 and AR2317 the PCI clock domain registers
+	 * are outside of the WMAC register space */
+	if (unlikely((reg >= 0x4000) && (reg < 0x5000) &&
+	    (ah->ah_mac_srev >= AR5K_SREV_AR2315_R6)))
+		return AR5K_AR2315_PCI_BASE + reg;
+
+	return ah->iobase + reg;
+}
+
+static inline u32 ath5k_hw_reg_read(struct ath5k_hw *ah, u16 reg)
+{
+	return ioread32(ath5k_ahb_reg(ah, reg));
+}
+
+static inline void ath5k_hw_reg_write(struct ath5k_hw *ah, u32 val, u16 reg)
+{
+	iowrite32(val, ath5k_ahb_reg(ah, reg));
+}
+
+#else
+
 static inline u32 ath5k_hw_reg_read(struct ath5k_hw *ah, u16 reg)
 {
 	return ioread32(ah->iobase + reg);
@@ -1657,6 +1683,8 @@ static inline void ath5k_hw_reg_write(struct ath5k_hw *ah, u32 val, u16 reg)
 	iowrite32(val, ah->iobase + reg);
 }
 
+#endif
+
 static inline enum ath_bus_type ath5k_get_bus_type(struct ath5k_hw *ah)
 {
 	return ath5k_hw_common(ah)->bus_ops->ath_bus_type;
diff --git a/drivers/net/wireless/ath/ath5k/base.c b/drivers/net/wireless/ath/ath5k/base.c
index a4a09bb8f2f3..bc9cb356fa69 100644
--- a/drivers/net/wireless/ath/ath5k/base.c
+++ b/drivers/net/wireless/ath/ath5k/base.c
@@ -99,6 +99,15 @@ static int ath5k_reset(struct ath5k_hw *ah, struct ieee80211_channel *chan,
 
 /* Known SREVs */
 static const struct ath5k_srev_name srev_names[] = {
+#ifdef CONFIG_ATH5K_AHB
+	{ "5312",	AR5K_VERSION_MAC,	AR5K_SREV_AR5312_R2 },
+	{ "5312",	AR5K_VERSION_MAC,	AR5K_SREV_AR5312_R7 },
+	{ "2313",	AR5K_VERSION_MAC,	AR5K_SREV_AR2313_R8 },
+	{ "2315",	AR5K_VERSION_MAC,	AR5K_SREV_AR2315_R6 },
+	{ "2315",	AR5K_VERSION_MAC,	AR5K_SREV_AR2315_R7 },
+	{ "2317",	AR5K_VERSION_MAC,	AR5K_SREV_AR2317_R1 },
+	{ "2317",	AR5K_VERSION_MAC,	AR5K_SREV_AR2317_R2 },
+#else
 	{ "5210",	AR5K_VERSION_MAC,	AR5K_SREV_AR5210 },
 	{ "5311",	AR5K_VERSION_MAC,	AR5K_SREV_AR5311 },
 	{ "5311A",	AR5K_VERSION_MAC,	AR5K_SREV_AR5311A },
@@ -117,6 +126,7 @@ static const struct ath5k_srev_name srev_names[] = {
 	{ "5418",	AR5K_VERSION_MAC,	AR5K_SREV_AR5418 },
 	{ "2425",	AR5K_VERSION_MAC,	AR5K_SREV_AR2425 },
 	{ "2417",	AR5K_VERSION_MAC,	AR5K_SREV_AR2417 },
+#endif
 	{ "xxxxx",	AR5K_VERSION_MAC,	AR5K_SREV_UNKNOWN },
 	{ "5110",	AR5K_VERSION_RAD,	AR5K_SREV_RAD_5110 },
 	{ "5111",	AR5K_VERSION_RAD,	AR5K_SREV_RAD_5111 },
@@ -132,6 +142,10 @@ static const struct ath5k_srev_name srev_names[] = {
 	{ "5413",	AR5K_VERSION_RAD,	AR5K_SREV_RAD_5413 },
 	{ "5424",	AR5K_VERSION_RAD,	AR5K_SREV_RAD_5424 },
 	{ "5133",	AR5K_VERSION_RAD,	AR5K_SREV_RAD_5133 },
+#ifdef CONFIG_ATH5K_AHB
+	{ "2316",	AR5K_VERSION_RAD,	AR5K_SREV_RAD_2316 },
+	{ "2317",	AR5K_VERSION_RAD,	AR5K_SREV_RAD_2317 },
+#endif
 	{ "xxxxx",	AR5K_VERSION_RAD,	AR5K_SREV_UNKNOWN },
 };
 
diff --git a/drivers/net/wireless/ath/ath5k/led.c b/drivers/net/wireless/ath/ath5k/led.c
index 0beb7e7d6075..ca4b7ccd697f 100644
--- a/drivers/net/wireless/ath/ath5k/led.c
+++ b/drivers/net/wireless/ath/ath5k/led.c
@@ -163,14 +163,20 @@ int ath5k_init_leds(struct ath5k_hw *ah)
 {
 	int ret = 0;
 	struct ieee80211_hw *hw = ah->hw;
+#ifndef CONFIG_ATH5K_AHB
 	struct pci_dev *pdev = ah->pdev;
+#endif
 	char name[ATH5K_LED_MAX_NAME_LEN + 1];
 	const struct pci_device_id *match;
 
 	if (!ah->pdev)
 		return 0;
 
+#ifdef CONFIG_ATH5K_AHB
+	match = NULL;
+#else
 	match = pci_match_id(&ath5k_led_devices[0], pdev);
+#endif
 	if (match) {
 		__set_bit(ATH_STAT_LEDSOFT, ah->status);
 		ah->led_pin = ATH_PIN(match->driver_data);
diff --git a/drivers/net/wireless/ath/ath9k/ahb.c b/drivers/net/wireless/ath/ath9k/ahb.c
index 4173838f4684..bd4a1a655f42 100644
--- a/drivers/net/wireless/ath/ath9k/ahb.c
+++ b/drivers/net/wireless/ath/ath9k/ahb.c
@@ -43,6 +43,10 @@ static const struct platform_device_id ath9k_platform_id_table[] = {
 		.name = "qca953x_wmac",
 		.driver_data = AR9300_DEVID_AR953X,
 	},
+	{
+		.name = "qca956x_wmac",
+		.driver_data = AR9300_DEVID_QCA956X,
+	},
 	{},
 };
 
@@ -175,7 +179,6 @@ static struct platform_driver ath_ahb_driver = {
 	.remove     = ath_ahb_remove,
 	.driver		= {
 		.name	= "ath9k",
-		.owner	= THIS_MODULE,
 	},
 	.id_table    = ath9k_platform_id_table,
 };
diff --git a/drivers/net/wireless/ath/ath9k/ani.c b/drivers/net/wireless/ath/ath9k/ani.c
index ba502a2d199b..ca01d17d130f 100644
--- a/drivers/net/wireless/ath/ath9k/ani.c
+++ b/drivers/net/wireless/ath/ath9k/ani.c
@@ -259,7 +259,8 @@ static void ath9k_hw_set_cck_nil(struct ath_hw *ah, u_int8_t immunityLevel,
 				     entry_cck->fir_step_level);
 
 	/* Skip MRC CCK for pre AR9003 families */
-	if (!AR_SREV_9300_20_OR_LATER(ah) || AR_SREV_9485(ah) || AR_SREV_9565(ah))
+	if (!AR_SREV_9300_20_OR_LATER(ah) || AR_SREV_9485(ah) ||
+	    AR_SREV_9565(ah) || AR_SREV_9561(ah))
 		return;
 
 	if (aniState->mrcCCK != entry_cck->mrc_cck_on)
diff --git a/drivers/net/wireless/ath/ath9k/ar5008_phy.c b/drivers/net/wireless/ath/ath9k/ar5008_phy.c
index 5829074208fa..f273427fdd29 100644
--- a/drivers/net/wireless/ath/ath9k/ar5008_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar5008_phy.c
@@ -22,6 +22,21 @@
 
 /* All code below is for AR5008, AR9001, AR9002 */
 
+#define AR5008_OFDM_RATES		8
+#define AR5008_HT_SS_RATES		8
+#define AR5008_HT_DS_RATES		8
+
+#define AR5008_HT20_SHIFT		16
+#define AR5008_HT40_SHIFT		24
+
+#define AR5008_11NA_OFDM_SHIFT		0
+#define AR5008_11NA_HT_SS_SHIFT		8
+#define AR5008_11NA_HT_DS_SHIFT		16
+
+#define AR5008_11NG_OFDM_SHIFT		4
+#define AR5008_11NG_HT_SS_SHIFT		12
+#define AR5008_11NG_HT_DS_SHIFT		20
+
 static const int firstep_table[] =
 /* level:  0   1   2   3   4   5   6   7   8  */
 	{ -4, -2,  0,  2,  4,  6,  8, 10, 12 }; /* lvl 0-8, default 2 */
@@ -1235,6 +1250,71 @@ static void ar5008_hw_set_radar_conf(struct ath_hw *ah)
 	conf->radar_inband = 8;
 }
 
+static void ar5008_hw_init_txpower_cck(struct ath_hw *ah, int16_t *rate_array)
+{
+#define CCK_DELTA(x) ((OLC_FOR_AR9280_20_LATER) ? max((x) - 2, 0) : (x))
+	ah->tx_power[0] = CCK_DELTA(rate_array[rate1l]);
+	ah->tx_power[1] = CCK_DELTA(min(rate_array[rate2l],
+					rate_array[rate2s]));
+	ah->tx_power[2] = CCK_DELTA(min(rate_array[rate5_5l],
+					rate_array[rate5_5s]));
+	ah->tx_power[3] = CCK_DELTA(min(rate_array[rate11l],
+					rate_array[rate11s]));
+#undef CCK_DELTA
+}
+
+static void ar5008_hw_init_txpower_ofdm(struct ath_hw *ah, int16_t *rate_array,
+					int offset)
+{
+	int i, idx = 0;
+
+	for (i = offset; i < offset + AR5008_OFDM_RATES; i++) {
+		ah->tx_power[i] = rate_array[idx];
+		idx++;
+	}
+}
+
+static void ar5008_hw_init_txpower_ht(struct ath_hw *ah, int16_t *rate_array,
+				      int ss_offset, int ds_offset,
+				      bool is_40, int ht40_delta)
+{
+	int i, mcs_idx = (is_40) ? AR5008_HT40_SHIFT : AR5008_HT20_SHIFT;
+
+	for (i = ss_offset; i < ss_offset + AR5008_HT_SS_RATES; i++) {
+		ah->tx_power[i] = rate_array[mcs_idx] + ht40_delta;
+		mcs_idx++;
+	}
+	memcpy(&ah->tx_power[ds_offset], &ah->tx_power[ss_offset],
+	       AR5008_HT_SS_RATES);
+}
+
+void ar5008_hw_init_rate_txpower(struct ath_hw *ah, int16_t *rate_array,
+				 struct ath9k_channel *chan, int ht40_delta)
+{
+	if (IS_CHAN_5GHZ(chan)) {
+		ar5008_hw_init_txpower_ofdm(ah, rate_array,
+					    AR5008_11NA_OFDM_SHIFT);
+		if (IS_CHAN_HT20(chan) || IS_CHAN_HT40(chan)) {
+			ar5008_hw_init_txpower_ht(ah, rate_array,
+						  AR5008_11NA_HT_SS_SHIFT,
+						  AR5008_11NA_HT_DS_SHIFT,
+						  IS_CHAN_HT40(chan),
+						  ht40_delta);
+		}
+	} else {
+		ar5008_hw_init_txpower_cck(ah, rate_array);
+		ar5008_hw_init_txpower_ofdm(ah, rate_array,
+					    AR5008_11NG_OFDM_SHIFT);
+		if (IS_CHAN_HT20(chan) || IS_CHAN_HT40(chan)) {
+			ar5008_hw_init_txpower_ht(ah, rate_array,
+						  AR5008_11NG_HT_SS_SHIFT,
+						  AR5008_11NG_HT_DS_SHIFT,
+						  IS_CHAN_HT40(chan),
+						  ht40_delta);
+		}
+	}
+}
+
 int ar5008_hw_attach_phy_ops(struct ath_hw *ah)
 {
 	struct ath_hw_private_ops *priv_ops = ath9k_hw_private_ops(ah);
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
index 08225a0067c2..8b4561e8ce1a 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
@@ -3536,7 +3536,7 @@ static void ar9003_hw_xpa_bias_level_apply(struct ath_hw *ah, bool is2ghz)
 	int bias = ar9003_modal_header(ah, is2ghz)->xpaBiasLvl;
 
 	if (AR_SREV_9485(ah) || AR_SREV_9330(ah) || AR_SREV_9340(ah) ||
-	    AR_SREV_9531(ah))
+	    AR_SREV_9531(ah) || AR_SREV_9561(ah))
 		REG_RMW_FIELD(ah, AR_CH0_TOP2, AR_CH0_TOP2_XPABIASLVL, bias);
 	else if (AR_SREV_9462(ah) || AR_SREV_9550(ah) || AR_SREV_9565(ah))
 		REG_RMW_FIELD(ah, AR_CH0_TOP, AR_CH0_TOP_XPABIASLVL, bias);
@@ -3599,7 +3599,7 @@ static void ar9003_hw_ant_ctrl_apply(struct ath_hw *ah, bool is2ghz)
 	if (AR_SREV_9462(ah) || AR_SREV_9565(ah)) {
 		REG_RMW_FIELD(ah, AR_PHY_SWITCH_COM,
 				AR_SWITCH_TABLE_COM_AR9462_ALL, value);
-	} else if (AR_SREV_9550(ah) || AR_SREV_9531(ah)) {
+	} else if (AR_SREV_9550(ah) || AR_SREV_9531(ah) || AR_SREV_9561(ah)) {
 		REG_RMW_FIELD(ah, AR_PHY_SWITCH_COM,
 				AR_SWITCH_TABLE_COM_AR9550_ALL, value);
 	} else
@@ -3929,9 +3929,13 @@ void ar9003_hw_internal_regulator_apply(struct ath_hw *ah)
 			REG_WRITE(ah, AR_PHY_PMU2, reg_pmu_set);
 			if (!is_pmu_set(ah, AR_PHY_PMU2, reg_pmu_set))
 				return;
-		} else if (AR_SREV_9462(ah) || AR_SREV_9565(ah)) {
+		} else if (AR_SREV_9462(ah) || AR_SREV_9565(ah) ||
+			   AR_SREV_9561(ah)) {
 			reg_val = le32_to_cpu(pBase->swreg);
 			REG_WRITE(ah, AR_PHY_PMU1, reg_val);
+
+			if (AR_SREV_9561(ah))
+				REG_WRITE(ah, AR_PHY_PMU2, 0x10200000);
 		} else {
 			/* Internal regulator is ON. Write swreg register. */
 			reg_val = le32_to_cpu(pBase->swreg);
@@ -4034,7 +4038,8 @@ static void ar9003_hw_xpa_timing_control_apply(struct ath_hw *ah, bool is2ghz)
 	if (!AR_SREV_9300(ah) &&
 	    !AR_SREV_9340(ah) &&
 	    !AR_SREV_9580(ah) &&
-	    !AR_SREV_9531(ah))
+	    !AR_SREV_9531(ah) &&
+	    !AR_SREV_9561(ah))
 		return;
 
 	xpa_ctl = ar9003_modal_header(ah, is2ghz)->txFrameToXpaOn;
@@ -4812,7 +4817,7 @@ static void ar9003_hw_power_control_override(struct ath_hw *ah,
 	}
 
 tempslope:
-	if (AR_SREV_9550(ah) || AR_SREV_9531(ah)) {
+	if (AR_SREV_9550(ah) || AR_SREV_9531(ah) || AR_SREV_9561(ah)) {
 		u8 txmask = (eep->baseEepHeader.txrxMask & 0xf0) >> 4;
 
 		/*
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_hw.c b/drivers/net/wireless/ath/ath9k/ar9003_hw.c
index 06ad2172030e..4335ccbe7d7e 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_hw.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_hw.c
@@ -29,6 +29,7 @@
 #include "ar9565_1p0_initvals.h"
 #include "ar9565_1p1_initvals.h"
 #include "ar953x_initvals.h"
+#include "ar956x_initvals.h"
 
 /* General hardware code for the AR9003 hadware family */
 
@@ -358,6 +359,40 @@ static void ar9003_hw_init_mode_regs(struct ath_hw *ah)
 
 		INIT_INI_ARRAY(&ah->iniModesFastClock,
 			       qca953x_1p0_modes_fast_clock);
+	} else if (AR_SREV_9561(ah)) {
+		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_CORE],
+			       qca956x_1p0_mac_core);
+		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_POST],
+			       qca956x_1p0_mac_postamble);
+
+		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_CORE],
+			       qca956x_1p0_baseband_core);
+		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_POST],
+			       qca956x_1p0_baseband_postamble);
+
+		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_CORE],
+			       qca956x_1p0_radio_core);
+		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_POST],
+			       qca956x_1p0_radio_postamble);
+
+		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_PRE],
+			       qca956x_1p0_soc_preamble);
+		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_POST],
+			       qca956x_1p0_soc_postamble);
+
+		INIT_INI_ARRAY(&ah->iniModesRxGain,
+			       qca956x_1p0_common_wo_xlna_rx_gain_table);
+		INIT_INI_ARRAY(&ah->ini_modes_rx_gain_bounds,
+			       qca956x_1p0_common_wo_xlna_rx_gain_bounds);
+		INIT_INI_ARRAY(&ah->iniModesTxGain,
+			       qca956x_1p0_modes_no_xpa_tx_gain_table);
+
+		INIT_INI_ARRAY(&ah->ini_dfs,
+			       qca956x_1p0_baseband_postamble_dfs_channel);
+		INIT_INI_ARRAY(&ah->iniCckfirJapan2484,
+			       qca956x_1p0_baseband_core_txfir_coeff_japan_2484);
+		INIT_INI_ARRAY(&ah->iniModesFastClock,
+			       qca956x_1p0_modes_fast_clock);
 	} else if (AR_SREV_9580(ah)) {
 		/* mac */
 		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_CORE],
@@ -544,6 +579,9 @@ static void ar9003_tx_gain_table_mode0(struct ath_hw *ah)
 	else if (AR_SREV_9531_20(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
 			       qca953x_2p0_modes_xpa_tx_gain_table);
+	else if (AR_SREV_9561(ah))
+		INIT_INI_ARRAY(&ah->iniModesTxGain,
+			       qca956x_1p0_modes_xpa_tx_gain_table);
 	else if (AR_SREV_9580(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
 			ar9580_1p0_lowest_ob_db_tx_gain_table);
@@ -594,7 +632,10 @@ static void ar9003_tx_gain_table_mode1(struct ath_hw *ah)
 		else
 			INIT_INI_ARRAY(&ah->iniModesTxGain,
 				       qca953x_1p0_modes_no_xpa_tx_gain_table);
-	} else if (AR_SREV_9462_21(ah))
+	} else if (AR_SREV_9561(ah))
+		INIT_INI_ARRAY(&ah->iniModesTxGain,
+			       qca956x_1p0_modes_no_xpa_tx_gain_table);
+	else if (AR_SREV_9462_21(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
 			ar9462_2p1_modes_high_ob_db_tx_gain);
 	else if (AR_SREV_9462_20(ah))
@@ -628,6 +669,9 @@ static void ar9003_tx_gain_table_mode2(struct ath_hw *ah)
 	else if (AR_SREV_9580(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
 			ar9580_1p0_low_ob_db_tx_gain_table);
+	else if (AR_SREV_9561(ah))
+		INIT_INI_ARRAY(&ah->iniModesTxGain,
+			       qca956x_1p0_modes_no_xpa_low_ob_db_tx_gain_table);
 	else if (AR_SREV_9565_11(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
 			       ar9565_1p1_modes_low_ob_db_tx_gain_table);
@@ -699,6 +743,9 @@ static void ar9003_tx_gain_table_mode5(struct ath_hw *ah)
 	else if (AR_SREV_9580(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
 			ar9580_1p0_type5_tx_gain_table);
+	else if (AR_SREV_9561(ah))
+		INIT_INI_ARRAY(&ah->iniModesTxGain,
+			       qca956x_1p0_modes_no_xpa_green_tx_gain_table);
 	else if (AR_SREV_9300_22(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
 			ar9300Modes_type5_tx_gain_table_2p2);
@@ -770,6 +817,13 @@ static void ar9003_rx_gain_table_mode0(struct ath_hw *ah)
 			       qca953x_1p0_common_rx_gain_table);
 		INIT_INI_ARRAY(&ah->ini_modes_rx_gain_bounds,
 			       qca953x_1p0_common_rx_gain_bounds);
+	} else if (AR_SREV_9561(ah)) {
+		INIT_INI_ARRAY(&ah->iniModesRxGain,
+			       qca956x_1p0_common_rx_gain_table);
+		INIT_INI_ARRAY(&ah->ini_modes_rx_gain_bounds,
+			       qca956x_1p0_common_rx_gain_bounds);
+		INIT_INI_ARRAY(&ah->ini_modes_rxgain_5g_xlna,
+			       qca956x_1p0_xlna_only);
 	} else if (AR_SREV_9580(ah))
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
 				ar9580_1p0_rx_gain_table);
@@ -825,6 +879,11 @@ static void ar9003_rx_gain_table_mode1(struct ath_hw *ah)
 			       qca953x_2p0_common_wo_xlna_rx_gain_table);
 		INIT_INI_ARRAY(&ah->ini_modes_rx_gain_bounds,
 			       qca953x_2p0_common_wo_xlna_rx_gain_bounds);
+	} else if (AR_SREV_9561(ah)) {
+		INIT_INI_ARRAY(&ah->iniModesRxGain,
+			       qca956x_1p0_common_wo_xlna_rx_gain_table);
+		INIT_INI_ARRAY(&ah->ini_modes_rx_gain_bounds,
+			       qca956x_1p0_common_wo_xlna_rx_gain_bounds);
 	} else if (AR_SREV_9580(ah))
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
 			ar9580_1p0_wo_xlna_rx_gain_table);
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_phy.c b/drivers/net/wireless/ath/ath9k/ar9003_phy.c
index ae6cde273414..1ad66b76749b 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_phy.c
@@ -183,7 +183,8 @@ static int ar9003_hw_set_channel(struct ath_hw *ah, struct ath9k_channel *chan)
 			} else {
 				channelSel = CHANSEL_2G(freq) >> 1;
 			}
-		} else if (AR_SREV_9550(ah) || AR_SREV_9531(ah)) {
+		} else if (AR_SREV_9550(ah) || AR_SREV_9531(ah) ||
+			   AR_SREV_9561(ah)) {
 			if (ah->is_clk_25mhz)
 				div = 75;
 			else
@@ -198,7 +199,8 @@ static int ar9003_hw_set_channel(struct ath_hw *ah, struct ath9k_channel *chan)
 		/* Set to 2G mode */
 		bMode = 1;
 	} else {
-		if ((AR_SREV_9340(ah) || AR_SREV_9550(ah) || AR_SREV_9531(ah)) &&
+		if ((AR_SREV_9340(ah) || AR_SREV_9550(ah) ||
+		     AR_SREV_9531(ah) || AR_SREV_9561(ah)) &&
 		    ah->is_clk_25mhz) {
 			channelSel = freq / 75;
 			chan_frac = ((freq % 75) * 0x20000) / 75;
@@ -265,7 +267,7 @@ static void ar9003_hw_spur_mitigate_mrc_cck(struct ath_hw *ah,
 	 */
 
 	if (AR_SREV_9485(ah) || AR_SREV_9340(ah) || AR_SREV_9330(ah) ||
-	    AR_SREV_9550(ah)) {
+	    AR_SREV_9550(ah) || AR_SREV_9561(ah)) {
 		if (spur_fbin_ptr[0] == 0) /* No spur */
 			return;
 		max_spur_cnts = 5;
@@ -292,7 +294,7 @@ static void ar9003_hw_spur_mitigate_mrc_cck(struct ath_hw *ah,
 
 		negative = 0;
 		if (AR_SREV_9485(ah) || AR_SREV_9340(ah) || AR_SREV_9330(ah) ||
-		    AR_SREV_9550(ah))
+		    AR_SREV_9550(ah) || AR_SREV_9561(ah))
 			cur_bb_spur = ath9k_hw_fbin2freq(spur_fbin_ptr[i],
 							 IS_CHAN_2GHZ(chan));
 		else
@@ -641,8 +643,10 @@ static void ar9003_hw_set_channel_regs(struct ath_hw *ah,
 		(REG_READ(ah, AR_PHY_GEN_CTRL) & AR_PHY_GC_ENABLE_DAC_FIFO);
 
 	/* Enable 11n HT, 20 MHz */
-	phymode = AR_PHY_GC_HT_EN | AR_PHY_GC_SINGLE_HT_LTF1 |
-		  AR_PHY_GC_SHORT_GI_40 | enableDacFifo;
+	phymode = AR_PHY_GC_HT_EN | AR_PHY_GC_SHORT_GI_40 | enableDacFifo;
+
+	if (!AR_SREV_9561(ah))
+		phymode |= AR_PHY_GC_SINGLE_HT_LTF1;
 
 	/* Configure baseband for dynamic 20/40 operation */
 	if (IS_CHAN_HT40(chan)) {
@@ -745,7 +749,8 @@ static void ar9003_hw_override_ini(struct ath_hw *ah)
 	else
 		ah->enabled_cals &= ~TX_CL_CAL;
 
-	if (AR_SREV_9340(ah) || AR_SREV_9531(ah) || AR_SREV_9550(ah)) {
+	if (AR_SREV_9340(ah) || AR_SREV_9531(ah) || AR_SREV_9550(ah) ||
+	    AR_SREV_9561(ah)) {
 		if (ah->is_clk_25mhz) {
 			REG_WRITE(ah, AR_RTC_DERIVED_CLK, 0x17c << 1);
 			REG_WRITE(ah, AR_SLP32_MODE, 0x0010f3d7);
@@ -812,6 +817,19 @@ static int ar9550_hw_get_modes_txgain_index(struct ath_hw *ah,
 	return ret;
 }
 
+static int ar9561_hw_get_modes_txgain_index(struct ath_hw *ah,
+					    struct ath9k_channel *chan)
+{
+	if (IS_CHAN_2GHZ(chan)) {
+		if (IS_CHAN_HT40(chan))
+			return 1;
+		else
+			return 2;
+	}
+
+	return 0;
+}
+
 static void ar9003_doubler_fix(struct ath_hw *ah)
 {
 	if (AR_SREV_9300(ah) || AR_SREV_9580(ah) || AR_SREV_9550(ah)) {
@@ -911,21 +929,29 @@ static int ar9003_hw_process_ini(struct ath_hw *ah,
 			REG_WRITE_ARRAY(&ah->ini_modes_rxgain_5g_xlna,
 					modesIndex, regWrites);
 		}
+
+		if (AR_SREV_9561(ah) && (ar9003_hw_get_rx_gain_idx(ah) == 0))
+			REG_WRITE_ARRAY(&ah->ini_modes_rxgain_5g_xlna,
+					modesIndex, regWrites);
 	}
 
-	if (AR_SREV_9550(ah))
+	if (AR_SREV_9550(ah) || AR_SREV_9561(ah))
 		REG_WRITE_ARRAY(&ah->ini_modes_rx_gain_bounds, modesIndex,
 				regWrites);
 
 	/*
 	 * TXGAIN initvals.
 	 */
-	if (AR_SREV_9550(ah) || AR_SREV_9531(ah)) {
+	if (AR_SREV_9550(ah) || AR_SREV_9531(ah) || AR_SREV_9561(ah)) {
 		int modes_txgain_index = 1;
 
 		if (AR_SREV_9550(ah))
 			modes_txgain_index = ar9550_hw_get_modes_txgain_index(ah, chan);
 
+		if (AR_SREV_9561(ah))
+			modes_txgain_index =
+				ar9561_hw_get_modes_txgain_index(ah, chan);
+
 		if (modes_txgain_index < 0)
 			return -EINVAL;
 
@@ -1989,7 +2015,8 @@ void ar9003_hw_attach_phy_ops(struct ath_hw *ah)
 	priv_ops->rf_set_freq = ar9003_hw_set_channel;
 	priv_ops->spur_mitigate_freq = ar9003_hw_spur_mitigate;
 
-	if (AR_SREV_9340(ah) || AR_SREV_9550(ah) || AR_SREV_9531(ah))
+	if (AR_SREV_9340(ah) || AR_SREV_9550(ah) || AR_SREV_9531(ah) ||
+	    AR_SREV_9561(ah))
 		priv_ops->compute_pll_control = ar9003_hw_compute_pll_control_soc;
 	else
 		priv_ops->compute_pll_control = ar9003_hw_compute_pll_control;
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_phy.h b/drivers/net/wireless/ath/ath9k/ar9003_phy.h
index fd090b1f2d0f..c311b2bfdb00 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_phy.h
+++ b/drivers/net/wireless/ath/ath9k/ar9003_phy.h
@@ -454,7 +454,7 @@
 #define AR_PHY_GEN_CTRL          (AR_SM_BASE + 0x4)
 #define AR_PHY_MODE              (AR_SM_BASE + 0x8)
 #define AR_PHY_ACTIVE            (AR_SM_BASE + 0xc)
-#define AR_PHY_SPUR_MASK_A       (AR_SM_BASE + 0x20)
+#define AR_PHY_SPUR_MASK_A       (AR_SM_BASE + (AR_SREV_9561(ah) ? 0x18 : 0x20))
 #define AR_PHY_SPUR_MASK_B       (AR_SM_BASE + 0x24)
 #define AR_PHY_SPECTRAL_SCAN     (AR_SM_BASE + 0x28)
 #define AR_PHY_RADAR_BW_FILTER   (AR_SM_BASE + 0x2c)
@@ -506,7 +506,7 @@
 #define AR_PHY_TEST_CHAIN_SEL      0xC0000000
 #define AR_PHY_TEST_CHAIN_SEL_S    30
 
-#define AR_PHY_TEST_CTL_STATUS   (AR_SM_BASE + 0x164)
+#define AR_PHY_TEST_CTL_STATUS   (AR_SM_BASE + (AR_SREV_9561(ah) ? 0x160 : 0x164))
 #define AR_PHY_TEST_CTL_TSTDAC_EN         0x1
 #define AR_PHY_TEST_CTL_TSTDAC_EN_S       0
 #define AR_PHY_TEST_CTL_TX_OBS_SEL        0x1C
@@ -525,7 +525,7 @@
 
 #define AR_PHY_CHAN_STATUS       (AR_SM_BASE + 0x16c)
 
-#define AR_PHY_CHAN_INFO_MEMORY				(AR_SM_BASE + 0x170)
+#define AR_PHY_CHAN_INFO_MEMORY (AR_SM_BASE + (AR_SREV_9561(ah) ? 0x16c : 0x170))
 #define AR_PHY_CHAN_INFO_MEMORY_CHANINFOMEM_S2_READ	0x00000008
 #define AR_PHY_CHAN_INFO_MEMORY_CHANINFOMEM_S2_READ_S	3
 
@@ -536,7 +536,7 @@
 #define AR_PHY_SCRAMBLER_SEED    (AR_SM_BASE + 0x190)
 #define AR_PHY_CCK_TX_CTRL       (AR_SM_BASE + 0x194)
 
-#define AR_PHY_HEAVYCLIP_CTL     (AR_SM_BASE + 0x1a4)
+#define AR_PHY_HEAVYCLIP_CTL     (AR_SM_BASE + (AR_SREV_9561(ah) ? 0x198 : 0x1a4))
 #define AR_PHY_HEAVYCLIP_20      (AR_SM_BASE + 0x1a8)
 #define AR_PHY_HEAVYCLIP_40      (AR_SM_BASE + 0x1ac)
 #define AR_PHY_ILLEGAL_TXRATE    (AR_SM_BASE + 0x1b0)
@@ -726,21 +726,24 @@
 
 #define AR_CH0_TOP2		(AR_SREV_9300(ah) ? 0x1628c : \
 					(AR_SREV_9462(ah) ? 0x16290 : 0x16284))
-#define AR_CH0_TOP2_XPABIASLVL		0xf000
+#define AR_CH0_TOP2_XPABIASLVL		(AR_SREV_9561(ah) ? 0x1e00 : 0xf000)
 #define AR_CH0_TOP2_XPABIASLVL_S	12
 
 #define AR_CH0_XTAL		(AR_SREV_9300(ah) ? 0x16294 : \
-				 ((AR_SREV_9462(ah) || AR_SREV_9565(ah)) ? 0x16298 : 0x16290))
+				 ((AR_SREV_9462(ah) || AR_SREV_9565(ah)) ? 0x16298 : \
+				  (AR_SREV_9561(ah) ? 0x162c0 : 0x16290)))
 #define AR_CH0_XTAL_CAPINDAC	0x7f000000
 #define AR_CH0_XTAL_CAPINDAC_S	24
 #define AR_CH0_XTAL_CAPOUTDAC	0x00fe0000
 #define AR_CH0_XTAL_CAPOUTDAC_S	17
 
-#define AR_PHY_PMU1		((AR_SREV_9462(ah) || AR_SREV_9565(ah)) ? 0x16340 : 0x16c40)
+#define AR_PHY_PMU1		((AR_SREV_9462(ah) || AR_SREV_9565(ah)) ? 0x16340 : \
+				 (AR_SREV_9561(ah) ? 0x16cc0 : 0x16c40))
 #define AR_PHY_PMU1_PWD		0x1
 #define AR_PHY_PMU1_PWD_S	0
 
-#define AR_PHY_PMU2		((AR_SREV_9462(ah) || AR_SREV_9565(ah)) ? 0x16344 : 0x16c44)
+#define AR_PHY_PMU2		((AR_SREV_9462(ah) || AR_SREV_9565(ah)) ? 0x16344 : \
+				 (AR_SREV_9561(ah) ? 0x16cc4 : 0x16c44))
 #define AR_PHY_PMU2_PGM		0x00200000
 #define AR_PHY_PMU2_PGM_S	21
 
diff --git a/drivers/net/wireless/ath/ath9k/ar956x_initvals.h b/drivers/net/wireless/ath/ath9k/ar956x_initvals.h
new file mode 100644
index 000000000000..c3a47eaaf0c0
--- /dev/null
+++ b/drivers/net/wireless/ath/ath9k/ar956x_initvals.h
@@ -0,0 +1,1046 @@
+/*
+ * Copyright (c) 2010-2011 Atheros Communications Inc.
+ * Copyright (c) 2011-2012 Qualcomm Atheros Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef INITVALS_956X_H
+#define INITVALS_956X_H
+
+#define qca956x_1p0_mac_core ar955x_1p0_mac_core
+
+#define qca956x_1p0_mac_postamble ar9331_1p1_mac_postamble
+
+#define qca956x_1p0_soc_preamble ar955x_1p0_soc_preamble
+
+#define qca956x_1p0_soc_postamble ar9300_2p2_soc_postamble
+
+#define qca956x_1p0_common_wo_xlna_rx_gain_table ar9300Common_wo_xlna_rx_gain_table_2p2
+
+#define qca956x_1p0_baseband_postamble_dfs_channel ar9300_2p2_baseband_postamble_dfs_channel
+
+#define qca956x_1p0_common_wo_xlna_rx_gain_bounds ar955x_1p0_common_wo_xlna_rx_gain_bounds
+
+#define qca956x_1p0_common_rx_gain_bounds ar955x_1p0_common_rx_gain_bounds
+
+#define qca956x_1p0_modes_fast_clock ar9462_2p0_modes_fast_clock
+
+static const u32 qca956x_1p0_baseband_core[][2] = {
+	/* Addr      allmodes  */
+	{0x00009800, 0xafe68e30},
+	{0x00009804, 0xfd14e000},
+	{0x00009808, 0x9c0a9f6b},
+	{0x0000980c, 0x04900000},
+	{0x00009814, 0x0280c00a},
+	{0x00009818, 0x00000000},
+	{0x0000981c, 0x00020028},
+	{0x00009834, 0x6400a190},
+	{0x00009838, 0x0108ecff},
+	{0x0000983c, 0x14000600},
+	{0x00009880, 0x201fff00},
+	{0x00009884, 0x00001042},
+	{0x000098a4, 0x00200400},
+	{0x000098b0, 0x32840cbf},
+	{0x000098bc, 0x00000002},
+	{0x000098d0, 0x004b6a8e},
+	{0x000098d4, 0x00000820},
+	{0x000098dc, 0x00000000},
+	{0x000098f0, 0x00000000},
+	{0x000098f4, 0x00000000},
+	{0x00009c04, 0xff55ff55},
+	{0x00009c08, 0x0320ff55},
+	{0x00009c0c, 0x00000000},
+	{0x00009c10, 0x00000000},
+	{0x00009c14, 0x00046384},
+	{0x00009c18, 0x05b6b440},
+	{0x00009c1c, 0x00b6b440},
+	{0x00009d00, 0xc080a333},
+	{0x00009d04, 0x40206c10},
+	{0x00009d08, 0x009c4060},
+	{0x00009d0c, 0x9883800a},
+	{0x00009d10, 0x01834061},
+	{0x00009d14, 0x00c0040b},
+	{0x00009d18, 0x00000000},
+	{0x00009e08, 0x0038230c},
+	{0x00009e24, 0x990bb514},
+	{0x00009e28, 0x0c6f0000},
+	{0x00009e30, 0x06336f77},
+	{0x00009e34, 0x6af6532f},
+	{0x00009e38, 0x0cc80c00},
+	{0x00009e40, 0x0d261820},
+	{0x00009e4c, 0x00001004},
+	{0x00009e50, 0x00ff03f1},
+	{0x00009fc0, 0x813e4789},
+	{0x00009fc4, 0x0001efb5},
+	{0x00009fcc, 0x40000014},
+	{0x00009fd0, 0x02993b93},
+	{0x0000a20c, 0x00000000},
+	{0x0000a218, 0x00000000},
+	{0x0000a21c, 0x00000000},
+	{0x0000a228, 0x10002310},
+	{0x0000a23c, 0x00000000},
+	{0x0000a244, 0x0c000000},
+	{0x0000a248, 0x00000140},
+	{0x0000a2a0, 0x00000007},
+	{0x0000a2c0, 0x00000007},
+	{0x0000a2c8, 0x00000000},
+	{0x0000a2d4, 0x00000000},
+	{0x0000a2ec, 0x00000000},
+	{0x0000a2f0, 0x00000000},
+	{0x0000a2f4, 0x00000000},
+	{0x0000a2f8, 0x00000000},
+	{0x0000a344, 0x00000000},
+	{0x0000a34c, 0x00000000},
+	{0x0000a350, 0x0000a000},
+	{0x0000a360, 0x00000000},
+	{0x0000a36c, 0x00000000},
+	{0x0000a384, 0x00000001},
+	{0x0000a388, 0x00000444},
+	{0x0000a38c, 0x00000000},
+	{0x0000a390, 0x210d0401},
+	{0x0000a394, 0xab9a7144},
+	{0x0000a398, 0x00000201},
+	{0x0000a39c, 0x42424848},
+	{0x0000a3a0, 0x3c466478},
+	{0x0000a3a4, 0x3a363600},
+	{0x0000a3a8, 0x0000003a},
+	{0x0000a3ac, 0x00000000},
+	{0x0000a3b0, 0x009011fe},
+	{0x0000a3b4, 0x00000034},
+	{0x0000a3b8, 0x00b3ec0a},
+	{0x0000a3bc, 0x00000036},
+	{0x0000a3c0, 0x20202020},
+	{0x0000a3c4, 0x22222220},
+	{0x0000a3c8, 0x20200020},
+	{0x0000a3cc, 0x20202020},
+	{0x0000a3d0, 0x20202020},
+	{0x0000a3d4, 0x20202020},
+	{0x0000a3d8, 0x20202020},
+	{0x0000a3dc, 0x20202020},
+	{0x0000a3e0, 0x20202020},
+	{0x0000a3e4, 0x20202020},
+	{0x0000a3e8, 0x20202020},
+	{0x0000a3ec, 0x20202020},
+	{0x0000a3f0, 0x00000000},
+	{0x0000a3f4, 0x00000000},
+	{0x0000a3f8, 0x0c9bd380},
+	{0x0000a3fc, 0x000f0f01},
+	{0x0000a400, 0x8fa91f01},
+	{0x0000a404, 0x00000000},
+	{0x0000a408, 0x0e79e5c6},
+	{0x0000a40c, 0x00820820},
+	{0x0000a414, 0x1ce739ce},
+	{0x0000a418, 0x2d0019ce},
+	{0x0000a41c, 0x1ce739ce},
+	{0x0000a420, 0x000001ce},
+	{0x0000a424, 0x1ce739ce},
+	{0x0000a428, 0x000001ce},
+	{0x0000a42c, 0x1ce739ce},
+	{0x0000a430, 0x1ce739ce},
+	{0x0000a434, 0x00000000},
+	{0x0000a438, 0x00001801},
+	{0x0000a43c, 0x00100000},
+	{0x0000a444, 0x00000000},
+	{0x0000a448, 0x05000080},
+	{0x0000a44c, 0x00000001},
+	{0x0000a450, 0x00010000},
+	{0x0000a454, 0x05000000},
+	{0x0000a458, 0x00000000},
+	{0x0000a644, 0xbfad9fee},
+	{0x0000a648, 0x0048660d},
+	{0x0000a64c, 0x00003c37},
+	{0x0000a670, 0x03020100},
+	{0x0000a674, 0x21200504},
+	{0x0000a678, 0x61602322},
+	{0x0000a67c, 0x65646362},
+	{0x0000a680, 0x6b6a6968},
+	{0x0000a684, 0xe2706d6c},
+	{0x0000a688, 0x000000e3},
+	{0x0000a690, 0x00000838},
+	{0x0000a7cc, 0x00000000},
+	{0x0000a7d0, 0x00000000},
+	{0x0000a7d4, 0x00000004},
+	{0x0000a7dc, 0x00000000},
+	{0x0000a8d0, 0x004b6a8e},
+	{0x0000a8d4, 0x00000820},
+	{0x0000a8dc, 0x00000000},
+	{0x0000a8f0, 0x00000000},
+	{0x0000a8f4, 0x00000000},
+	{0x0000b2d0, 0x00000080},
+	{0x0000b2d4, 0x00000000},
+	{0x0000b2ec, 0x00000000},
+	{0x0000b2f0, 0x00000000},
+	{0x0000b2f4, 0x00000000},
+	{0x0000b2f8, 0x00000000},
+	{0x0000b408, 0x0e79e5c0},
+	{0x0000b40c, 0x00820820},
+	{0x0000b420, 0x00000000},
+	{0x0000b8d0, 0x004b6a8e},
+	{0x0000b8d4, 0x00000820},
+	{0x0000b8dc, 0x00000000},
+	{0x0000b8f0, 0x00000000},
+	{0x0000b8f4, 0x00000000},
+	{0x0000c2d0, 0x00000080},
+	{0x0000c2d4, 0x00000000},
+	{0x0000c2ec, 0x00000000},
+	{0x0000c2f0, 0x00000000},
+	{0x0000c2f4, 0x00000000},
+	{0x0000c2f8, 0x00000000},
+	{0x0000c408, 0x0e79e5c0},
+	{0x0000c40c, 0x00820820},
+	{0x0000c420, 0x00000000},
+};
+
+static const u32 qca956x_1p0_baseband_postamble[][5] = {
+	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
+	{0x00009810, 0xd00a8005, 0xd00a8005, 0xd00a8011, 0xd00a8011},
+	{0x00009820, 0x206a022e, 0x206a022e, 0x206a01ae, 0x206a01ae},
+	{0x00009824, 0x5ac640d0, 0x5ac640d0, 0x5ac621f1, 0x5ac621f1},
+	{0x00009828, 0x06903081, 0x06903081, 0x07d43881, 0x07d43881},
+	{0x0000982c, 0x05eea6d4, 0x05eea6d4, 0x05eea6d4, 0x05eea6d4},
+	{0x00009830, 0x0000059c, 0x0000059c, 0x0000119c, 0x0000119c},
+	{0x00009c00, 0x000000c4, 0x000000c4, 0x000000c4, 0x000000c4},
+	{0x00009e00, 0x0372111a, 0x0372111a, 0x037216a0, 0x037216a0},
+	{0x00009e04, 0x001c2020, 0x001c2020, 0x001c2020, 0x001c2020},
+	{0x00009e0c, 0x6c4000e2, 0x6d4000e2, 0x6d4000de, 0x6c4000de},
+	{0x00009e10, 0x7ec88d2e, 0x7ec88d2e, 0x7ec84d2e, 0x7ec86d2e},
+	{0x00009e14, 0x37b95d5e, 0x37b9605e, 0x337d605e, 0x337d5d5e},
+	{0x00009e18, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x00009e1c, 0x0001cf9c, 0x0001cf9c, 0x00021f9c, 0x00021f9c},
+	{0x00009e20, 0x000003b5, 0x000003b5, 0x000003a6, 0x000003a6},
+	{0x00009e2c, 0x0000001c, 0x0000001c, 0x00000021, 0x00000021},
+	{0x00009e3c, 0xcfa10820, 0xcfa10820, 0xcf946222, 0xcf946222},
+	{0x00009e44, 0xfe321e27, 0xfe321e27, 0xfe291e27, 0xfe291e27},
+	{0x00009e48, 0x5030201a, 0x5030201a, 0x50302012, 0x50302012},
+	{0x00009fc8, 0x0003f000, 0x0003f000, 0x0001a000, 0x0001a000},
+	{0x0000a204, 0x005c0ec0, 0x005c0ec4, 0x045c0cc4, 0x045c0cc0},
+	{0x0000a208, 0x00000104, 0x00000104, 0x00000004, 0x00000004},
+	{0x0000a22c, 0x07e26a2f, 0x07e26a2f, 0x01026a2f, 0x01026a2f},
+	{0x0000a230, 0x0000400a, 0x00004014, 0x00004016, 0x0000400b},
+	{0x0000a234, 0x00000fff, 0x10000fff, 0x10000fff, 0x00000fff},
+	{0x0000a238, 0xffb01018, 0xffb01018, 0xffb01018, 0xffb01018},
+	{0x0000a250, 0x00000000, 0x00000000, 0x00000210, 0x00000108},
+	{0x0000a254, 0x000007d0, 0x00000fa0, 0x00001130, 0x00000898},
+	{0x0000a258, 0x02020002, 0x02020002, 0x02020002, 0x02020002},
+	{0x0000a25c, 0x01000e0e, 0x01000e0e, 0x01010e0e, 0x01010e0e},
+	{0x0000a260, 0x0a021501, 0x0a021501, 0x3a021501, 0x3a021501},
+	{0x0000a264, 0x00000e0e, 0x00000e0e, 0x01000e0e, 0x01000e0e},
+	{0x0000a280, 0x00000007, 0x00000007, 0x0000000b, 0x0000000b},
+	{0x0000a284, 0x00000000, 0x00000000, 0x00000010, 0x00000010},
+	{0x0000a288, 0x00000110, 0x00000110, 0x00000110, 0x00000110},
+	{0x0000a28c, 0x00022222, 0x00022222, 0x00022222, 0x00022222},
+	{0x0000a2c4, 0x00058d18, 0x00058d18, 0x00058d18, 0x00058d18},
+	{0x0000a2cc, 0x18c50033, 0x18c43433, 0x18c41033, 0x18c44c33},
+	{0x0000a2d0, 0x00041982, 0x00041982, 0x00041982, 0x00041982},
+	{0x0000a2d8, 0x7999a83b, 0x7999a83b, 0x7999a83b, 0x7999a83b},
+	{0x0000a358, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a830, 0x0000019c, 0x0000019c, 0x0000019c, 0x0000019c},
+	{0x0000ae04, 0x001c0000, 0x001c0000, 0x001c0000, 0x001c0000},
+	{0x0000ae18, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000ae1c, 0x0000019c, 0x0000019c, 0x0000019c, 0x0000019c},
+	{0x0000ae20, 0x000001b5, 0x000001b5, 0x000001a6, 0x000001a6},
+	{0x0000b284, 0x00000000, 0x00000000, 0x00000010, 0x00000010},
+	{0x0000b830, 0x0000019c, 0x0000019c, 0x0000019c, 0x0000019c},
+	{0x0000be04, 0x001c0000, 0x001c0000, 0x001c0000, 0x001c0000},
+	{0x0000be18, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000be1c, 0x0000019c, 0x0000019c, 0x0000019c, 0x0000019c},
+	{0x0000be20, 0x000001b5, 0x000001b5, 0x000001a6, 0x000001a6},
+	{0x0000c284, 0x00000000, 0x00000000, 0x00000010, 0x00000010},
+};
+
+static const u32 qca956x_1p0_radio_core[][2] = {
+	/* Addr      allmodes  */
+	{0x00016000, 0x36db6db6},
+	{0x00016004, 0x6db6db40},
+	{0x00016008, 0x73f00000},
+	{0x0001600c, 0x00000000},
+	{0x00016040, 0x3f80fff8},
+	{0x0001604c, 0x000f0278},
+	{0x00016050, 0x8036db6c},
+	{0x00016054, 0x6db60000},
+	{0x00016080, 0x00080000},
+	{0x00016084, 0x0e48048c},
+	{0x00016088, 0x14214514},
+	{0x0001608c, 0x119f080a},
+	{0x00016090, 0x24926490},
+	{0x00016094, 0x00000000},
+	{0x000160a0, 0xc2108ffe},
+	{0x000160a4, 0x812fc370},
+	{0x000160a8, 0x423c8000},
+	{0x000160b4, 0x92480000},
+	{0x000160c0, 0x006db6d8},
+	{0x000160c4, 0x24b6db6c},
+	{0x000160c8, 0x6db6db6c},
+	{0x000160cc, 0x6db6fb7c},
+	{0x000160d0, 0x6db6da44},
+	{0x00016100, 0x07ff8001},
+	{0x00016108, 0x00080010},
+	{0x00016144, 0x01884080},
+	{0x00016148, 0x00008058},
+	{0x00016288, 0x001c6000},
+	{0x0001628c, 0x50000000},
+	{0x000162c0, 0x4b962100},
+	{0x000162c4, 0x00000480},
+	{0x000162c8, 0x04000144},
+	{0x00016380, 0x00000000},
+	{0x00016384, 0x00000000},
+	{0x00016388, 0x00800700},
+	{0x0001638c, 0x00800700},
+	{0x00016390, 0x00800700},
+	{0x00016394, 0x00000000},
+	{0x00016398, 0x00000000},
+	{0x0001639c, 0x00000000},
+	{0x000163a0, 0x00000001},
+	{0x000163a4, 0x00000001},
+	{0x000163a8, 0x00000000},
+	{0x000163ac, 0x00000000},
+	{0x000163b0, 0x00000000},
+	{0x000163b4, 0x00000000},
+	{0x000163b8, 0x00000000},
+	{0x000163bc, 0x00000000},
+	{0x000163c0, 0x000000a0},
+	{0x000163c4, 0x000c0000},
+	{0x000163c8, 0x14021402},
+	{0x000163cc, 0x00001402},
+	{0x000163d0, 0x00000000},
+	{0x000163d4, 0x00000000},
+	{0x00016400, 0x36db6db6},
+	{0x00016404, 0x6db6db40},
+	{0x00016408, 0x73f00000},
+	{0x0001640c, 0x00000000},
+	{0x00016440, 0x3f80fff8},
+	{0x0001644c, 0x000f0278},
+	{0x00016450, 0x8036db6c},
+	{0x00016454, 0x6db60000},
+	{0x00016500, 0x07ff8001},
+	{0x00016508, 0x00080010},
+	{0x00016544, 0x01884080},
+	{0x00016548, 0x00008058},
+	{0x00016780, 0x00000000},
+	{0x00016784, 0x00000000},
+	{0x00016788, 0x00800700},
+	{0x0001678c, 0x00800700},
+	{0x00016790, 0x00800700},
+	{0x00016794, 0x00000000},
+	{0x00016798, 0x00000000},
+	{0x0001679c, 0x00000000},
+	{0x000167a0, 0x00000001},
+	{0x000167a4, 0x00000001},
+	{0x000167a8, 0x00000000},
+	{0x000167ac, 0x00000000},
+	{0x000167b0, 0x00000000},
+	{0x000167b4, 0x00000000},
+	{0x000167b8, 0x00000000},
+	{0x000167bc, 0x00000000},
+	{0x000167c0, 0x000000a0},
+	{0x000167c4, 0x000c0000},
+	{0x000167c8, 0x14021402},
+	{0x000167cc, 0x00001402},
+	{0x000167d0, 0x00000000},
+	{0x000167d4, 0x00000000},
+	{0x00016800, 0x36db6db6},
+	{0x00016804, 0x6db6db40},
+	{0x00016808, 0x73f00000},
+	{0x0001680c, 0x00000000},
+	{0x00016840, 0x3f80fff8},
+	{0x0001684c, 0x000f0278},
+	{0x00016850, 0x8036db6c},
+	{0x00016854, 0x6db60000},
+	{0x00016900, 0x07ff8001},
+	{0x00016908, 0x00080010},
+	{0x00016944, 0x01884080},
+	{0x00016948, 0x00008058},
+	{0x00016b80, 0x00000000},
+	{0x00016b84, 0x00000000},
+	{0x00016b88, 0x00800700},
+	{0x00016b8c, 0x00800700},
+	{0x00016b90, 0x00800700},
+	{0x00016b94, 0x00000000},
+	{0x00016b98, 0x00000000},
+	{0x00016b9c, 0x00000000},
+	{0x00016ba0, 0x00000001},
+	{0x00016ba4, 0x00000001},
+	{0x00016ba8, 0x00000000},
+	{0x00016bac, 0x00000000},
+	{0x00016bb0, 0x00000000},
+	{0x00016bb4, 0x00000000},
+	{0x00016bb8, 0x00000000},
+	{0x00016bbc, 0x00000000},
+	{0x00016bc0, 0x000000a0},
+	{0x00016bc4, 0x000c0000},
+	{0x00016bc8, 0x14021402},
+	{0x00016bcc, 0x00001402},
+	{0x00016bd0, 0x00000000},
+	{0x00016bd4, 0x00000000},
+};
+
+static const u32 qca956x_1p0_radio_postamble[][5] = {
+	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
+	{0x00016098, 0xd2dd5554, 0xd2dd5554, 0xc4128f5c, 0xc4128f5c},
+	{0x0001609c, 0x0a566f3a, 0x0a566f3a, 0x0fd08f25, 0x0fd08f25},
+	{0x000160ac, 0xa4647c00, 0xa4647c00, 0x24646800, 0x24646800},
+	{0x000160b0, 0x01885f52, 0x01885f52, 0x00fe7f46, 0x00fe7f46},
+	{0x00016104, 0xb7a00000, 0xb7a00000, 0xfff80001, 0xfff80001},
+	{0x0001610c, 0xc0000000, 0xc0000000, 0x00000000, 0x00000000},
+	{0x00016140, 0x10804008, 0x10804008, 0x50804000, 0x50804000},
+	{0x00016504, 0xb7a00000, 0xb7a00000, 0xfff80001, 0xfff80001},
+	{0x0001650c, 0xc0000000, 0xc0000000, 0x00000000, 0x00000000},
+	{0x00016540, 0x10804008, 0x10804008, 0x50804000, 0x50804000},
+	{0x00016904, 0xb7a00000, 0xb7a00000, 0xfff80001, 0xfff80001},
+	{0x0001690c, 0xc0000000, 0xc0000000, 0x00000000, 0x00000000},
+	{0x00016940, 0x10804008, 0x10804008, 0x50804000, 0x50804000},
+};
+
+static const u32 qca956x_1p0_baseband_core_txfir_coeff_japan_2484[][2] = {
+	/* Addr      allmodes  */
+	{0x0000a38c, 0x00000000},
+	{0x0000a390, 0x6f7f0301},
+	{0x0000a394, 0xca9228ee},
+};
+
+static const u32 qca956x_1p0_modes_no_xpa_tx_gain_table[][3] = {
+	/* Addr      5G          2G        */
+	{0x0000a2dc, 0xffa9ac94, 0xffa9ac94},
+	{0x0000a2e0, 0xff323118, 0xff323118},
+	{0x0000a2e4, 0xff3ffe00, 0xff3ffe00},
+	{0x0000a2e8, 0xffc00000, 0xffc00000},
+	{0x0000a39c, 0x42424242, 0x42424242},
+	{0x0000a3a4, 0x3a3e3e00, 0x3a3e3e00},
+	{0x0000a3b0, 0x00a01404, 0x00a01404},
+	{0x0000a3b4, 0x00000034, 0x00000034},
+	{0x0000a3b8, 0x00800408, 0x00800408},
+	{0x0000a3bc, 0x00000036, 0x00000036},
+	{0x0000a410, 0x000050dc, 0x000050dc},
+	{0x0000a500, 0x09000040, 0x09000040},
+	{0x0000a504, 0x0b000041, 0x0b000041},
+	{0x0000a508, 0x0d000042, 0x0d000042},
+	{0x0000a50c, 0x11000044, 0x11000044},
+	{0x0000a510, 0x15000046, 0x15000046},
+	{0x0000a514, 0x1d000440, 0x1d000440},
+	{0x0000a518, 0x1f000441, 0x1f000441},
+	{0x0000a51c, 0x23000443, 0x23000443},
+	{0x0000a520, 0x25000444, 0x25000444},
+	{0x0000a524, 0x280004e0, 0x280004e0},
+	{0x0000a528, 0x2c0004e2, 0x2c0004e2},
+	{0x0000a52c, 0x2e0004e3, 0x2e0004e3},
+	{0x0000a530, 0x300004e4, 0x300004e4},
+	{0x0000a534, 0x340004e6, 0x340004e6},
+	{0x0000a538, 0x37000ce0, 0x37000ce0},
+	{0x0000a53c, 0x3b000ce2, 0x3b000ce2},
+	{0x0000a540, 0x3d000ce3, 0x3d000ce3},
+	{0x0000a544, 0x3f000ce4, 0x3f000ce4},
+	{0x0000a548, 0x45001ee0, 0x45001ee0},
+	{0x0000a54c, 0x49001ee2, 0x49001ee2},
+	{0x0000a550, 0x4d001ee4, 0x4d001ee4},
+	{0x0000a554, 0x51001ee6, 0x51001ee6},
+	{0x0000a558, 0x55001eea, 0x55001eea},
+	{0x0000a55c, 0x59001eec, 0x59001eec},
+	{0x0000a560, 0x5d001ef0, 0x5d001ef0},
+	{0x0000a564, 0x5f001ef1, 0x5f001ef1},
+	{0x0000a568, 0x60001ef2, 0x60001ef2},
+	{0x0000a56c, 0x61001ef3, 0x61001ef3},
+	{0x0000a570, 0x62001ef4, 0x62001ef4},
+	{0x0000a574, 0x63001ef5, 0x63001ef5},
+	{0x0000a578, 0x64001ffc, 0x64001ffc},
+	{0x0000a57c, 0x64001ffc, 0x64001ffc},
+	{0x0000a600, 0x00000000, 0x00000000},
+	{0x0000a604, 0x00000000, 0x00000000},
+	{0x0000a608, 0x00000000, 0x00000000},
+	{0x0000a60c, 0x00000000, 0x00000000},
+	{0x0000a610, 0x00804000, 0x00804000},
+	{0x0000a614, 0x00804201, 0x00804201},
+	{0x0000a618, 0x00804201, 0x00804201},
+	{0x0000a61c, 0x00804201, 0x00804201},
+	{0x0000a620, 0x00804201, 0x00804201},
+	{0x0000a624, 0x00804201, 0x00804201},
+	{0x0000a628, 0x00804201, 0x00804201},
+	{0x0000a62c, 0x02808a02, 0x02808a02},
+	{0x0000a630, 0x0340cd03, 0x0340cd03},
+	{0x0000a634, 0x0340cd03, 0x0340cd03},
+	{0x0000a638, 0x0340cd03, 0x0340cd03},
+	{0x0000a63c, 0x05011404, 0x05011404},
+	{0x0000b2dc, 0xffa9ac94, 0xffa9ac94},
+	{0x0000b2e0, 0xff323118, 0xff323118},
+	{0x0000b2e4, 0xff3ffe00, 0xff3ffe00},
+	{0x0000b2e8, 0xffc00000, 0xffc00000},
+	{0x0000c2dc, 0xffa9ac94, 0xffa9ac94},
+	{0x0000c2e0, 0xff323118, 0xff323118},
+	{0x0000c2e4, 0xff3ffe00, 0xff3ffe00},
+	{0x0000c2e8, 0xffc00000, 0xffc00000},
+	{0x00016044, 0x049242db, 0x049242db},
+	{0x00016048, 0x64925a70, 0x64925a70},
+	{0x00016148, 0x00008050, 0x00008050},
+	{0x00016280, 0x41110005, 0x41110005},
+	{0x00016284, 0x453a6000, 0x453a6000},
+	{0x00016444, 0x049242db, 0x049242db},
+	{0x00016448, 0x6c925a70, 0x6c925a70},
+	{0x00016548, 0x00008050, 0x00008050},
+	{0x00016844, 0x049242db, 0x049242db},
+	{0x00016848, 0x6c925a70, 0x6c925a70},
+	{0x00016948, 0x00008050, 0x00008050},
+};
+
+static const u32 qca956x_1p0_modes_xpa_tx_gain_table[][3] = {
+	/* Addr      5G          2G        */
+	{0x0000a2dc, 0xcc69ac94, 0xcc69ac94},
+	{0x0000a2e0, 0xf0b23118, 0xf0b23118},
+	{0x0000a2e4, 0xffffc000, 0xffffc000},
+	{0x0000a2e8, 0xc0000000, 0xc0000000},
+	{0x0000a410, 0x000050d2, 0x000050d2},
+	{0x0000a500, 0x0a000040, 0x0a000040},
+	{0x0000a504, 0x0c000041, 0x0c000041},
+	{0x0000a508, 0x0e000042, 0x0e000042},
+	{0x0000a50c, 0x12000044, 0x12000044},
+	{0x0000a510, 0x16000046, 0x16000046},
+	{0x0000a514, 0x1d000440, 0x1d000440},
+	{0x0000a518, 0x1f000441, 0x1f000441},
+	{0x0000a51c, 0x23000443, 0x23000443},
+	{0x0000a520, 0x25000444, 0x25000444},
+	{0x0000a524, 0x29000a40, 0x29000a40},
+	{0x0000a528, 0x2d000a42, 0x2d000a42},
+	{0x0000a52c, 0x2f000a43, 0x2f000a43},
+	{0x0000a530, 0x31000a44, 0x31000a44},
+	{0x0000a534, 0x35000a46, 0x35000a46},
+	{0x0000a538, 0x38000ce0, 0x38000ce0},
+	{0x0000a53c, 0x3c000ce2, 0x3c000ce2},
+	{0x0000a540, 0x3e000ce3, 0x3e000ce3},
+	{0x0000a544, 0x40000ce4, 0x40000ce4},
+	{0x0000a548, 0x46001ee0, 0x46001ee0},
+	{0x0000a54c, 0x4a001ee2, 0x4a001ee2},
+	{0x0000a550, 0x4e001ee4, 0x4e001ee4},
+	{0x0000a554, 0x52001ee6, 0x52001ee6},
+	{0x0000a558, 0x56001eea, 0x56001eea},
+	{0x0000a55c, 0x5a001eec, 0x5a001eec},
+	{0x0000a560, 0x5e001ef0, 0x5e001ef0},
+	{0x0000a564, 0x60001ef1, 0x60001ef1},
+	{0x0000a568, 0x61001ef2, 0x61001ef2},
+	{0x0000a56c, 0x62001ef3, 0x62001ef3},
+	{0x0000a570, 0x63001ef4, 0x63001ef4},
+	{0x0000a574, 0x64001ef5, 0x64001ef5},
+	{0x0000a578, 0x65001ffc, 0x65001ffc},
+	{0x0000a57c, 0x65001ffc, 0x65001ffc},
+	{0x0000a600, 0x00000000, 0x00000000},
+	{0x0000a604, 0x00000000, 0x00000000},
+	{0x0000a608, 0x00000000, 0x00000000},
+	{0x0000a60c, 0x00000000, 0x00000000},
+	{0x0000a610, 0x00000000, 0x00000000},
+	{0x0000a614, 0x00000000, 0x00000000},
+	{0x0000a618, 0x00000000, 0x00000000},
+	{0x0000a61c, 0x00804201, 0x00804201},
+	{0x0000a620, 0x00804201, 0x00804201},
+	{0x0000a624, 0x00804201, 0x00804201},
+	{0x0000a628, 0x00804201, 0x00804201},
+	{0x0000a62c, 0x02808a02, 0x02808a02},
+	{0x0000a630, 0x0340cd03, 0x0340cd03},
+	{0x0000a634, 0x0340cd03, 0x0340cd03},
+	{0x0000a638, 0x0340cd03, 0x0340cd03},
+	{0x0000a63c, 0x05011404, 0x05011404},
+	{0x0000b2dc, 0xcc69ac94, 0xcc69ac94},
+	{0x0000b2e0, 0xf0b23118, 0xf0b23118},
+	{0x0000b2e4, 0xffffc000, 0xffffc000},
+	{0x0000b2e8, 0xc0000000, 0xc0000000},
+	{0x0000c2dc, 0xcc69ac94, 0xcc69ac94},
+	{0x0000c2e0, 0xf0b23118, 0xf0b23118},
+	{0x0000c2e4, 0xffffc000, 0xffffc000},
+	{0x0000c2e8, 0xc0000000, 0xc0000000},
+	{0x00016044, 0x012492db, 0x012492db},
+	{0x00016048, 0x6c927a70, 0x6c927a70},
+	{0x00016050, 0x8036d36c, 0x8036d36c},
+	{0x00016280, 0x41110005, 0x41110005},
+	{0x00016284, 0x453a7e00, 0x453a7e00},
+	{0x00016444, 0x012492db, 0x012492db},
+	{0x00016448, 0x6c927a70, 0x6c927a70},
+	{0x00016450, 0x8036d36c, 0x8036d36c},
+	{0x00016844, 0x012492db, 0x012492db},
+	{0x00016848, 0x6c927a70, 0x6c927a70},
+	{0x00016850, 0x8036d36c, 0x8036d36c},
+};
+
+static const u32 qca956x_1p0_modes_no_xpa_low_ob_db_tx_gain_table[][3] = {
+	/* Addr      5G          2G        */
+	{0x0000a2dc, 0xffa9ac94, 0xffa9ac94},
+	{0x0000a2e0, 0xff323118, 0xff323118},
+	{0x0000a2e4, 0xff3ffe00, 0xff3ffe00},
+	{0x0000a2e8, 0xffc00000, 0xffc00000},
+	{0x0000a39c, 0x42424242, 0x42424242},
+	{0x0000a3a4, 0x3a3e3e00, 0x3a3e3e00},
+	{0x0000a3b0, 0x00a01404, 0x00a01404},
+	{0x0000a3b4, 0x00000034, 0x00000034},
+	{0x0000a3b8, 0x00800408, 0x00800408},
+	{0x0000a3bc, 0x00000036, 0x00000036},
+	{0x0000a410, 0x000050dc, 0x000050dc},
+	{0x0000a414, 0x16b739ce, 0x16b739ce},
+	{0x0000a418, 0x2d00198b, 0x2d00198b},
+	{0x0000a41c, 0x16b5adce, 0x16b5adce},
+	{0x0000a420, 0x0000014a, 0x0000014a},
+	{0x0000a424, 0x14a525cc, 0x14a525cc},
+	{0x0000a428, 0x0000012a, 0x0000012a},
+	{0x0000a42c, 0x14a5294a, 0x14a5294a},
+	{0x0000a430, 0x1294a929, 0x1294a929},
+	{0x0000a500, 0x09000040, 0x09000040},
+	{0x0000a504, 0x0b000041, 0x0b000041},
+	{0x0000a508, 0x0d000042, 0x0d000042},
+	{0x0000a50c, 0x11000044, 0x11000044},
+	{0x0000a510, 0x15000046, 0x15000046},
+	{0x0000a514, 0x1d000440, 0x1d000440},
+	{0x0000a518, 0x1f000441, 0x1f000441},
+	{0x0000a51c, 0x23000443, 0x23000443},
+	{0x0000a520, 0x25000444, 0x25000444},
+	{0x0000a524, 0x280004e0, 0x280004e0},
+	{0x0000a528, 0x2c0004e2, 0x2c0004e2},
+	{0x0000a52c, 0x2e0004e3, 0x2e0004e3},
+	{0x0000a530, 0x300004e4, 0x300004e4},
+	{0x0000a534, 0x340004e6, 0x340004e6},
+	{0x0000a538, 0x37000ce0, 0x37000ce0},
+	{0x0000a53c, 0x3b000ce2, 0x3b000ce2},
+	{0x0000a540, 0x3d000ce3, 0x3d000ce3},
+	{0x0000a544, 0x3f000ce4, 0x3f000ce4},
+	{0x0000a548, 0x45001ee0, 0x45001ee0},
+	{0x0000a54c, 0x49001ee2, 0x49001ee2},
+	{0x0000a550, 0x4d001ee4, 0x4d001ee4},
+	{0x0000a554, 0x51001ee6, 0x51001ee6},
+	{0x0000a558, 0x55001eea, 0x55001eea},
+	{0x0000a55c, 0x59001eec, 0x59001eec},
+	{0x0000a560, 0x5d001ef0, 0x5d001ef0},
+	{0x0000a564, 0x5f001ef1, 0x5f001ef1},
+	{0x0000a568, 0x60001ef2, 0x60001ef2},
+	{0x0000a56c, 0x61001ef3, 0x61001ef3},
+	{0x0000a570, 0x62001ef4, 0x62001ef4},
+	{0x0000a574, 0x63001ef5, 0x63001ef5},
+	{0x0000a578, 0x64001ffc, 0x64001ffc},
+	{0x0000a57c, 0x64001ffc, 0x64001ffc},
+	{0x0000a600, 0x00000000, 0x00000000},
+	{0x0000a604, 0x00000000, 0x00000000},
+	{0x0000a608, 0x00000000, 0x00000000},
+	{0x0000a60c, 0x00000000, 0x00000000},
+	{0x0000a610, 0x00804000, 0x00804000},
+	{0x0000a614, 0x00804201, 0x00804201},
+	{0x0000a618, 0x00804201, 0x00804201},
+	{0x0000a61c, 0x00804201, 0x00804201},
+	{0x0000a620, 0x00804201, 0x00804201},
+	{0x0000a624, 0x00804201, 0x00804201},
+	{0x0000a628, 0x00804201, 0x00804201},
+	{0x0000a62c, 0x02808a02, 0x02808a02},
+	{0x0000a630, 0x0340cd03, 0x0340cd03},
+	{0x0000a634, 0x0340cd03, 0x0340cd03},
+	{0x0000a638, 0x0340cd03, 0x0340cd03},
+	{0x0000a63c, 0x05011404, 0x05011404},
+	{0x0000b2dc, 0xffa9ac94, 0xffa9ac94},
+	{0x0000b2e0, 0xff323118, 0xff323118},
+	{0x0000b2e4, 0xff3ffe00, 0xff3ffe00},
+	{0x0000b2e8, 0xffc00000, 0xffc00000},
+	{0x0000c2dc, 0xffa9ac94, 0xffa9ac94},
+	{0x0000c2e0, 0xff323118, 0xff323118},
+	{0x0000c2e4, 0xff3ffe00, 0xff3ffe00},
+	{0x0000c2e8, 0xffc00000, 0xffc00000},
+	{0x00016044, 0x046e42db, 0x046e42db},
+	{0x00016048, 0x64925a70, 0x64925a70},
+	{0x00016148, 0x00008050, 0x00008050},
+	{0x00016280, 0x41110005, 0x41110005},
+	{0x00016284, 0x453a6000, 0x453a6000},
+	{0x00016444, 0x046e42db, 0x046e42db},
+	{0x00016448, 0x6c925a70, 0x6c925a70},
+	{0x00016548, 0x00008050, 0x00008050},
+	{0x00016844, 0x046e42db, 0x046e42db},
+	{0x00016848, 0x6c925a70, 0x6c925a70},
+	{0x00016948, 0x00008050, 0x00008050},
+};
+
+static const u32 qca956x_1p0_modes_no_xpa_green_tx_gain_table[][3] = {
+	/* Addr      5G          2G        */
+	{0x000098bc, 0x00000001, 0x00000001},
+	{0x0000a2dc, 0xd3555284, 0xd3555284},
+	{0x0000a2e0, 0x1c666318, 0x1c666318},
+	{0x0000a2e4, 0xe07bbc00, 0xe07bbc00},
+	{0x0000a2e8, 0xff800000, 0xff800000},
+	{0x0000a3a4, 0x3a3e3e00, 0x3a3e3e00},
+	{0x0000a410, 0x000050dc, 0x000050dc},
+	{0x0000a500, 0x02000040, 0x02000040},
+	{0x0000a504, 0x04000041, 0x04000041},
+	{0x0000a508, 0x06000042, 0x06000042},
+	{0x0000a50c, 0x0a000044, 0x0a000044},
+	{0x0000a510, 0x0c000045, 0x0c000045},
+	{0x0000a514, 0x13000440, 0x13000440},
+	{0x0000a518, 0x15000441, 0x15000441},
+	{0x0000a51c, 0x19000443, 0x19000443},
+	{0x0000a520, 0x1b000444, 0x1b000444},
+	{0x0000a524, 0x1e0004e0, 0x1e0004e0},
+	{0x0000a528, 0x220004e2, 0x220004e2},
+	{0x0000a52c, 0x240004e3, 0x240004e3},
+	{0x0000a530, 0x260004e4, 0x260004e4},
+	{0x0000a534, 0x2a0004e6, 0x2a0004e6},
+	{0x0000a538, 0x32000ce0, 0x32000ce0},
+	{0x0000a53c, 0x36000ce2, 0x36000ce2},
+	{0x0000a540, 0x3a000ce4, 0x3a000ce4},
+	{0x0000a544, 0x3e000ce6, 0x3e000ce6},
+	{0x0000a548, 0x45001ee0, 0x45001ee0},
+	{0x0000a54c, 0x49001ee2, 0x49001ee2},
+	{0x0000a550, 0x4d001ee4, 0x4d001ee4},
+	{0x0000a554, 0x51001ee6, 0x51001ee6},
+	{0x0000a558, 0x55001eea, 0x55001eea},
+	{0x0000a55c, 0x59001eec, 0x59001eec},
+	{0x0000a560, 0x5d001ef0, 0x5d001ef0},
+	{0x0000a564, 0x5f001ef1, 0x5f001ef1},
+	{0x0000a568, 0x60001ef2, 0x60001ef2},
+	{0x0000a56c, 0x61001ef3, 0x61001ef3},
+	{0x0000a570, 0x62001ef4, 0x62001ef4},
+	{0x0000a574, 0x63001ff5, 0x63001ff5},
+	{0x0000a578, 0x64001ffc, 0x64001ffc},
+	{0x0000a57c, 0x64001ffc, 0x64001ffc},
+	{0x0000a600, 0x00000000, 0x00000000},
+	{0x0000a604, 0x00000000, 0x00000000},
+	{0x0000a608, 0x00000000, 0x00000000},
+	{0x0000a60c, 0x00000000, 0x00000000},
+	{0x0000a610, 0x00804000, 0x00804000},
+	{0x0000a614, 0x00804201, 0x00804201},
+	{0x0000a618, 0x00804201, 0x00804201},
+	{0x0000a61c, 0x00804201, 0x00804201},
+	{0x0000a620, 0x00804201, 0x00804201},
+	{0x0000a624, 0x00804201, 0x00804201},
+	{0x0000a628, 0x00804201, 0x00804201},
+	{0x0000a62c, 0x02808a02, 0x02808a02},
+	{0x0000a630, 0x0340cd03, 0x0340cd03},
+	{0x0000a634, 0x0340cd03, 0x0340cd03},
+	{0x0000a638, 0x0340cd03, 0x0340cd03},
+	{0x0000a63c, 0x05011404, 0x05011404},
+	{0x0000b2dc, 0xd3555284, 0xd3555284},
+	{0x0000b2e0, 0x1c666318, 0x1c666318},
+	{0x0000b2e4, 0xe07bbc00, 0xe07bbc00},
+	{0x0000b2e8, 0xff800000, 0xff800000},
+	{0x0000c2dc, 0xd3555284, 0xd3555284},
+	{0x0000c2e0, 0x1c666318, 0x1c666318},
+	{0x0000c2e4, 0xe07bbc00, 0xe07bbc00},
+	{0x0000c2e8, 0xff800000, 0xff800000},
+	{0x00016044, 0x849242db, 0x849242db},
+	{0x00016048, 0x64925a70, 0x64925a70},
+	{0x00016280, 0x41110005, 0x41110005},
+	{0x00016284, 0x453a6000, 0x453a6000},
+	{0x00016444, 0x849242db, 0x849242db},
+	{0x00016448, 0x6c925a70, 0x6c925a70},
+	{0x00016844, 0x849242db, 0x849242db},
+	{0x00016848, 0x6c925a70, 0x6c925a70},
+	{0x0000a7f0, 0x800002cc, 0x800002cc},
+	{0x0000a7f4, 0x00000018, 0x00000018},
+	{0x0000a7f4, 0x00000018, 0x00000018},
+	{0x0000a7f4, 0x00000018, 0x00000018},
+	{0x0000a7f4, 0x00000018, 0x00000018},
+	{0x0000a7f4, 0x00000018, 0x00000018},
+	{0x0000a7f4, 0x00000018, 0x00000018},
+	{0x0000a7f4, 0x00000018, 0x00000018},
+	{0x0000a7f4, 0x00000018, 0x00000018},
+	{0x0000a7f4, 0x00000018, 0x00000018},
+	{0x0000a7f4, 0x00000018, 0x00000018},
+	{0x0000a7f4, 0x00000018, 0x00000018},
+	{0x0000a7f4, 0x00000018, 0x00000018},
+	{0x0000a7f4, 0x00000018, 0x00000018},
+	{0x0000a7f4, 0x00000018, 0x00000018},
+	{0x0000a7f4, 0x00000028, 0x00000028},
+	{0x0000a7f4, 0x00000028, 0x00000028},
+	{0x0000a7f4, 0x00000028, 0x00000028},
+	{0x0000a7f4, 0x00000028, 0x00000028},
+	{0x0000a7f4, 0x00000048, 0x00000048},
+	{0x0000a7f4, 0x00000048, 0x00000048},
+	{0x0000a7f4, 0x00000048, 0x00000048},
+	{0x0000a7f4, 0x00000048, 0x00000048},
+	{0x0000a7f4, 0x00000048, 0x00000048},
+	{0x0000a7f4, 0x00000048, 0x00000048},
+	{0x0000a7f4, 0x00000048, 0x00000048},
+	{0x0000a7f4, 0x00000048, 0x00000048},
+	{0x0000a7f4, 0x00000048, 0x00000048},
+	{0x0000a7f4, 0x00000048, 0x00000048},
+	{0x0000a7f4, 0x00000048, 0x00000048},
+	{0x0000a7f4, 0x00000048, 0x00000048},
+	{0x0000a7f4, 0x00000048, 0x00000048},
+	{0x0000a7f4, 0x00000048, 0x00000048},
+};
+
+static const u32 qca956x_1p0_common_rx_gain_table[][2] = {
+	/* Addr      allmodes  */
+	{0x0000a000, 0x00010000},
+	{0x0000a004, 0x00030002},
+	{0x0000a008, 0x00050004},
+	{0x0000a00c, 0x00810080},
+	{0x0000a010, 0x00830082},
+	{0x0000a014, 0x01810180},
+	{0x0000a018, 0x01830182},
+	{0x0000a01c, 0x01850184},
+	{0x0000a020, 0x01890188},
+	{0x0000a024, 0x018b018a},
+	{0x0000a028, 0x018d018c},
+	{0x0000a02c, 0x01910190},
+	{0x0000a030, 0x01930192},
+	{0x0000a034, 0x01950194},
+	{0x0000a038, 0x038a0196},
+	{0x0000a03c, 0x038c038b},
+	{0x0000a040, 0x0390038d},
+	{0x0000a044, 0x03920391},
+	{0x0000a048, 0x03940393},
+	{0x0000a04c, 0x03960395},
+	{0x0000a050, 0x00000000},
+	{0x0000a054, 0x00000000},
+	{0x0000a058, 0x00000000},
+	{0x0000a05c, 0x00000000},
+	{0x0000a060, 0x00000000},
+	{0x0000a064, 0x00000000},
+	{0x0000a068, 0x00000000},
+	{0x0000a06c, 0x00000000},
+	{0x0000a070, 0x00000000},
+	{0x0000a074, 0x00000000},
+	{0x0000a078, 0x00000000},
+	{0x0000a07c, 0x00000000},
+	{0x0000a080, 0x22222222},
+	{0x0000a084, 0x1d1d1d1d},
+	{0x0000a088, 0x1d1d1d1d},
+	{0x0000a08c, 0x1d1d1d1d},
+	{0x0000a090, 0x17171717},
+	{0x0000a094, 0x11111717},
+	{0x0000a098, 0x00030311},
+	{0x0000a09c, 0x00000000},
+	{0x0000a0a0, 0x00000000},
+	{0x0000a0a4, 0x00000000},
+	{0x0000a0a8, 0x00000000},
+	{0x0000a0ac, 0x00000000},
+	{0x0000a0b0, 0x00000000},
+	{0x0000a0b4, 0x00000000},
+	{0x0000a0b8, 0x00000000},
+	{0x0000a0bc, 0x00000000},
+	{0x0000a0c0, 0x001f0000},
+	{0x0000a0c4, 0x01000101},
+	{0x0000a0c8, 0x011e011f},
+	{0x0000a0cc, 0x011c011d},
+	{0x0000a0d0, 0x02030204},
+	{0x0000a0d4, 0x02010202},
+	{0x0000a0d8, 0x021f0200},
+	{0x0000a0dc, 0x0302021e},
+	{0x0000a0e0, 0x03000301},
+	{0x0000a0e4, 0x031e031f},
+	{0x0000a0e8, 0x0402031d},
+	{0x0000a0ec, 0x04000401},
+	{0x0000a0f0, 0x041e041f},
+	{0x0000a0f4, 0x0502041d},
+	{0x0000a0f8, 0x05000501},
+	{0x0000a0fc, 0x051e051f},
+	{0x0000a100, 0x06010602},
+	{0x0000a104, 0x061f0600},
+	{0x0000a108, 0x061d061e},
+	{0x0000a10c, 0x07020703},
+	{0x0000a110, 0x07000701},
+	{0x0000a114, 0x00000000},
+	{0x0000a118, 0x00000000},
+	{0x0000a11c, 0x00000000},
+	{0x0000a120, 0x00000000},
+	{0x0000a124, 0x00000000},
+	{0x0000a128, 0x00000000},
+	{0x0000a12c, 0x00000000},
+	{0x0000a130, 0x00000000},
+	{0x0000a134, 0x00000000},
+	{0x0000a138, 0x00000000},
+	{0x0000a13c, 0x00000000},
+	{0x0000a140, 0x001f0000},
+	{0x0000a144, 0x01000101},
+	{0x0000a148, 0x011e011f},
+	{0x0000a14c, 0x011c011d},
+	{0x0000a150, 0x02030204},
+	{0x0000a154, 0x02010202},
+	{0x0000a158, 0x021f0200},
+	{0x0000a15c, 0x0302021e},
+	{0x0000a160, 0x03000301},
+	{0x0000a164, 0x031e031f},
+	{0x0000a168, 0x0402031d},
+	{0x0000a16c, 0x04000401},
+	{0x0000a170, 0x041e041f},
+	{0x0000a174, 0x0502041d},
+	{0x0000a178, 0x05000501},
+	{0x0000a17c, 0x051e051f},
+	{0x0000a180, 0x06010602},
+	{0x0000a184, 0x061f0600},
+	{0x0000a188, 0x061d061e},
+	{0x0000a18c, 0x07020703},
+	{0x0000a190, 0x07000701},
+	{0x0000a194, 0x00000000},
+	{0x0000a198, 0x00000000},
+	{0x0000a19c, 0x00000000},
+	{0x0000a1a0, 0x00000000},
+	{0x0000a1a4, 0x00000000},
+	{0x0000a1a8, 0x00000000},
+	{0x0000a1ac, 0x00000000},
+	{0x0000a1b0, 0x00000000},
+	{0x0000a1b4, 0x00000000},
+	{0x0000a1b8, 0x00000000},
+	{0x0000a1bc, 0x00000000},
+	{0x0000a1c0, 0x00000000},
+	{0x0000a1c4, 0x00000000},
+	{0x0000a1c8, 0x00000000},
+	{0x0000a1cc, 0x00000000},
+	{0x0000a1d0, 0x00000000},
+	{0x0000a1d4, 0x00000000},
+	{0x0000a1d8, 0x00000000},
+	{0x0000a1dc, 0x00000000},
+	{0x0000a1e0, 0x00000000},
+	{0x0000a1e4, 0x00000000},
+	{0x0000a1e8, 0x00000000},
+	{0x0000a1ec, 0x00000000},
+	{0x0000a1f0, 0x00000396},
+	{0x0000a1f4, 0x00000396},
+	{0x0000a1f8, 0x00000396},
+	{0x0000a1fc, 0x00000196},
+	{0x0000b000, 0x00010000},
+	{0x0000b004, 0x00030002},
+	{0x0000b008, 0x00050004},
+	{0x0000b00c, 0x00810080},
+	{0x0000b010, 0x00830082},
+	{0x0000b014, 0x01810180},
+	{0x0000b018, 0x01830182},
+	{0x0000b01c, 0x01850184},
+	{0x0000b020, 0x02810280},
+	{0x0000b024, 0x02830282},
+	{0x0000b028, 0x02850284},
+	{0x0000b02c, 0x02890288},
+	{0x0000b030, 0x028b028a},
+	{0x0000b034, 0x0388028c},
+	{0x0000b038, 0x038a0389},
+	{0x0000b03c, 0x038c038b},
+	{0x0000b040, 0x0390038d},
+	{0x0000b044, 0x03920391},
+	{0x0000b048, 0x03940393},
+	{0x0000b04c, 0x03960395},
+	{0x0000b050, 0x00000000},
+	{0x0000b054, 0x00000000},
+	{0x0000b058, 0x00000000},
+	{0x0000b05c, 0x00000000},
+	{0x0000b060, 0x00000000},
+	{0x0000b064, 0x00000000},
+	{0x0000b068, 0x00000000},
+	{0x0000b06c, 0x00000000},
+	{0x0000b070, 0x00000000},
+	{0x0000b074, 0x00000000},
+	{0x0000b078, 0x00000000},
+	{0x0000b07c, 0x00000000},
+	{0x0000b080, 0x23232323},
+	{0x0000b084, 0x21232323},
+	{0x0000b088, 0x19191c1e},
+	{0x0000b08c, 0x12141417},
+	{0x0000b090, 0x07070e0e},
+	{0x0000b094, 0x03030305},
+	{0x0000b098, 0x00000003},
+	{0x0000b09c, 0x00000000},
+	{0x0000b0a0, 0x00000000},
+	{0x0000b0a4, 0x00000000},
+	{0x0000b0a8, 0x00000000},
+	{0x0000b0ac, 0x00000000},
+	{0x0000b0b0, 0x00000000},
+	{0x0000b0b4, 0x00000000},
+	{0x0000b0b8, 0x00000000},
+	{0x0000b0bc, 0x00000000},
+	{0x0000b0c0, 0x003f0020},
+	{0x0000b0c4, 0x00400041},
+	{0x0000b0c8, 0x0140005f},
+	{0x0000b0cc, 0x0160015f},
+	{0x0000b0d0, 0x017e017f},
+	{0x0000b0d4, 0x02410242},
+	{0x0000b0d8, 0x025f0240},
+	{0x0000b0dc, 0x027f0260},
+	{0x0000b0e0, 0x0341027e},
+	{0x0000b0e4, 0x035f0340},
+	{0x0000b0e8, 0x037f0360},
+	{0x0000b0ec, 0x04400441},
+	{0x0000b0f0, 0x0460045f},
+	{0x0000b0f4, 0x0541047f},
+	{0x0000b0f8, 0x055f0540},
+	{0x0000b0fc, 0x057f0560},
+	{0x0000b100, 0x06400641},
+	{0x0000b104, 0x0660065f},
+	{0x0000b108, 0x067e067f},
+	{0x0000b10c, 0x07410742},
+	{0x0000b110, 0x075f0740},
+	{0x0000b114, 0x077f0760},
+	{0x0000b118, 0x07800781},
+	{0x0000b11c, 0x07a0079f},
+	{0x0000b120, 0x07c107bf},
+	{0x0000b124, 0x000007c0},
+	{0x0000b128, 0x00000000},
+	{0x0000b12c, 0x00000000},
+	{0x0000b130, 0x00000000},
+	{0x0000b134, 0x00000000},
+	{0x0000b138, 0x00000000},
+	{0x0000b13c, 0x00000000},
+	{0x0000b140, 0x003f0020},
+	{0x0000b144, 0x00400041},
+	{0x0000b148, 0x0140005f},
+	{0x0000b14c, 0x0160015f},
+	{0x0000b150, 0x017e017f},
+	{0x0000b154, 0x02410242},
+	{0x0000b158, 0x025f0240},
+	{0x0000b15c, 0x027f0260},
+	{0x0000b160, 0x0341027e},
+	{0x0000b164, 0x035f0340},
+	{0x0000b168, 0x037f0360},
+	{0x0000b16c, 0x04400441},
+	{0x0000b170, 0x0460045f},
+	{0x0000b174, 0x0541047f},
+	{0x0000b178, 0x055f0540},
+	{0x0000b17c, 0x057f0560},
+	{0x0000b180, 0x06400641},
+	{0x0000b184, 0x0660065f},
+	{0x0000b188, 0x067e067f},
+	{0x0000b18c, 0x07410742},
+	{0x0000b190, 0x075f0740},
+	{0x0000b194, 0x077f0760},
+	{0x0000b198, 0x07800781},
+	{0x0000b19c, 0x07a0079f},
+	{0x0000b1a0, 0x07c107bf},
+	{0x0000b1a4, 0x000007c0},
+	{0x0000b1a8, 0x00000000},
+	{0x0000b1ac, 0x00000000},
+	{0x0000b1b0, 0x00000000},
+	{0x0000b1b4, 0x00000000},
+	{0x0000b1b8, 0x00000000},
+	{0x0000b1bc, 0x00000000},
+	{0x0000b1c0, 0x00000000},
+	{0x0000b1c4, 0x00000000},
+	{0x0000b1c8, 0x00000000},
+	{0x0000b1cc, 0x00000000},
+	{0x0000b1d0, 0x00000000},
+	{0x0000b1d4, 0x00000000},
+	{0x0000b1d8, 0x00000000},
+	{0x0000b1dc, 0x00000000},
+	{0x0000b1e0, 0x00000000},
+	{0x0000b1e4, 0x00000000},
+	{0x0000b1e8, 0x00000000},
+	{0x0000b1ec, 0x00000000},
+	{0x0000b1f0, 0x00000396},
+	{0x0000b1f4, 0x00000396},
+	{0x0000b1f8, 0x00000396},
+	{0x0000b1fc, 0x00000196},
+};
+
+static const u32 qca956x_1p0_xlna_only[][5] = {
+	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
+	{0x00009820, 0x206a022e, 0x206a022e, 0x206a01ae, 0x206a01ae},
+	{0x00009824, 0x5ac640d0, 0x5ac640d0, 0x5ac621f1, 0x5ac621f1},
+	{0x00009828, 0x06903081, 0x06903081, 0x07d43881, 0x07d43881},
+	{0x00009e00, 0x0372111a, 0x0372111a, 0x037216a0, 0x03721720},
+	{0x00009e0c, 0x6c4000e2, 0x6d4000e2, 0x6d4000de, 0x6c4000da},
+	{0x00009e10, 0x7ec88d2e, 0x7ec88d2e, 0x7ec86d2e, 0x7ec8ad2e},
+	{0x00009e14, 0x37b95d5e, 0x37b9605e, 0x317a6062, 0x317a5ae2},
+	{0x00009e18, 0x00000000, 0x00000000, 0x03c00000, 0x03c00000},
+	{0x00009e20, 0x000003b5, 0x000003b5, 0x000003b2, 0x000003b2},
+	{0x00009fc0, 0x813e4788, 0x813e4788, 0x813e4789, 0x813e4789},
+	{0x0000ae18, 0x00000000, 0x00000000, 0x03c00000, 0x03c00000},
+	{0x0000ae20, 0x000001b5, 0x000001b5, 0x000001b2, 0x000001b2},
+	{0x0000be18, 0x00000000, 0x00000000, 0x03c00000, 0x03c00000},
+	{0x0000be20, 0x000001b5, 0x000001b5, 0x000001b2, 0x000001b2},
+};
+
+#endif /* INITVALS_956X_H */
diff --git a/drivers/net/wireless/ath/ath9k/debug.c b/drivers/net/wireless/ath/ath9k/debug.c
index 696e3d5309c6..dd5d3914799b 100644
--- a/drivers/net/wireless/ath/ath9k/debug.c
+++ b/drivers/net/wireless/ath/ath9k/debug.c
@@ -401,22 +401,15 @@ static const struct file_operations fops_antenna_diversity = {
 	.llseek = default_llseek,
 };
 
-static ssize_t read_file_dma(struct file *file, char __user *user_buf,
-			     size_t count, loff_t *ppos)
+static int read_file_dma(struct seq_file *file, void *data)
 {
-	struct ath_softc *sc = file->private_data;
+	struct ieee80211_hw *hw = dev_get_drvdata(file->private);
+	struct ath_softc *sc = hw->priv;
 	struct ath_hw *ah = sc->sc_ah;
-	char *buf;
-	int retval;
-	unsigned int len = 0;
 	u32 val[ATH9K_NUM_DMA_DEBUG_REGS];
 	int i, qcuOffset = 0, dcuOffset = 0;
 	u32 *qcuBase = &val[0], *dcuBase = &val[4];
 
-	buf = kmalloc(DMA_BUF_LEN, GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
-
 	ath9k_ps_wakeup(sc);
 
 	REG_WRITE_D(ah, AR_MACMISC,
@@ -424,21 +417,18 @@ static ssize_t read_file_dma(struct file *file, char __user *user_buf,
 		   (AR_MACMISC_MISC_OBS_BUS_1 <<
 		    AR_MACMISC_MISC_OBS_BUS_MSB_S)));
 
-	len += scnprintf(buf + len, DMA_BUF_LEN - len,
-			 "Raw DMA Debug values:\n");
+	seq_puts(file, "Raw DMA Debug values:\n");
 
 	for (i = 0; i < ATH9K_NUM_DMA_DEBUG_REGS; i++) {
 		if (i % 4 == 0)
-			len += scnprintf(buf + len, DMA_BUF_LEN - len, "\n");
+			seq_puts(file, "\n");
 
 		val[i] = REG_READ_D(ah, AR_DMADBG_0 + (i * sizeof(u32)));
-		len += scnprintf(buf + len, DMA_BUF_LEN - len, "%d: %08x ",
-				 i, val[i]);
+		seq_printf(file, "%d: %08x ", i, val[i]);
 	}
 
-	len += scnprintf(buf + len, DMA_BUF_LEN - len, "\n\n");
-	len += scnprintf(buf + len, DMA_BUF_LEN - len,
-			 "Num QCU: chain_st fsp_ok fsp_st DCU: chain_st\n");
+	seq_puts(file, "\n\n");
+	seq_puts(file, "Num QCU: chain_st fsp_ok fsp_st DCU: chain_st\n");
 
 	for (i = 0; i < ATH9K_NUM_QUEUES; i++, qcuOffset += 4, dcuOffset += 5) {
 		if (i == 8) {
@@ -451,57 +441,35 @@ static ssize_t read_file_dma(struct file *file, char __user *user_buf,
 			dcuBase++;
 		}
 
-		len += scnprintf(buf + len, DMA_BUF_LEN - len,
-			 "%2d          %2x      %1x     %2x           %2x\n",
-			 i, (*qcuBase & (0x7 << qcuOffset)) >> qcuOffset,
-			 (*qcuBase & (0x8 << qcuOffset)) >> (qcuOffset + 3),
-			 (val[2] & (0x7 << (i * 3))) >> (i * 3),
-			 (*dcuBase & (0x1f << dcuOffset)) >> dcuOffset);
+		seq_printf(file, "%2d          %2x      %1x     %2x           %2x\n",
+			   i, (*qcuBase & (0x7 << qcuOffset)) >> qcuOffset,
+			   (*qcuBase & (0x8 << qcuOffset)) >> (qcuOffset + 3),
+			   (val[2] & (0x7 << (i * 3))) >> (i * 3),
+			   (*dcuBase & (0x1f << dcuOffset)) >> dcuOffset);
 	}
 
-	len += scnprintf(buf + len, DMA_BUF_LEN - len, "\n");
-
-	len += scnprintf(buf + len, DMA_BUF_LEN - len,
-		"qcu_stitch state:   %2x    qcu_fetch state:        %2x\n",
-		(val[3] & 0x003c0000) >> 18, (val[3] & 0x03c00000) >> 22);
-	len += scnprintf(buf + len, DMA_BUF_LEN - len,
-		"qcu_complete state: %2x    dcu_complete state:     %2x\n",
-		(val[3] & 0x1c000000) >> 26, (val[6] & 0x3));
-	len += scnprintf(buf + len, DMA_BUF_LEN - len,
-		"dcu_arb state:      %2x    dcu_fp state:           %2x\n",
-		(val[5] & 0x06000000) >> 25, (val[5] & 0x38000000) >> 27);
-	len += scnprintf(buf + len, DMA_BUF_LEN - len,
-		"chan_idle_dur:     %3d    chan_idle_dur_valid:     %1d\n",
-		(val[6] & 0x000003fc) >> 2, (val[6] & 0x00000400) >> 10);
-	len += scnprintf(buf + len, DMA_BUF_LEN - len,
-		"txfifo_valid_0:      %1d    txfifo_valid_1:          %1d\n",
-		(val[6] & 0x00000800) >> 11, (val[6] & 0x00001000) >> 12);
-	len += scnprintf(buf + len, DMA_BUF_LEN - len,
-		"txfifo_dcu_num_0:   %2d    txfifo_dcu_num_1:       %2d\n",
-		(val[6] & 0x0001e000) >> 13, (val[6] & 0x001e0000) >> 17);
-
-	len += scnprintf(buf + len, DMA_BUF_LEN - len, "pcu observe: 0x%x\n",
-			 REG_READ_D(ah, AR_OBS_BUS_1));
-	len += scnprintf(buf + len, DMA_BUF_LEN - len,
-			 "AR_CR: 0x%x\n", REG_READ_D(ah, AR_CR));
+	seq_puts(file, "\n");
 
-	ath9k_ps_restore(sc);
+	seq_printf(file, "qcu_stitch state:   %2x    qcu_fetch state:        %2x\n",
+		   (val[3] & 0x003c0000) >> 18, (val[3] & 0x03c00000) >> 22);
+	seq_printf(file, "qcu_complete state: %2x    dcu_complete state:     %2x\n",
+		   (val[3] & 0x1c000000) >> 26, (val[6] & 0x3));
+	seq_printf(file, "dcu_arb state:      %2x    dcu_fp state:           %2x\n",
+		   (val[5] & 0x06000000) >> 25, (val[5] & 0x38000000) >> 27);
+	seq_printf(file, "chan_idle_dur:     %3d    chan_idle_dur_valid:     %1d\n",
+		   (val[6] & 0x000003fc) >> 2, (val[6] & 0x00000400) >> 10);
+	seq_printf(file, "txfifo_valid_0:      %1d    txfifo_valid_1:          %1d\n",
+		   (val[6] & 0x00000800) >> 11, (val[6] & 0x00001000) >> 12);
+	seq_printf(file, "txfifo_dcu_num_0:   %2d    txfifo_dcu_num_1:       %2d\n",
+		   (val[6] & 0x0001e000) >> 13, (val[6] & 0x001e0000) >> 17);
 
-	if (len > DMA_BUF_LEN)
-		len = DMA_BUF_LEN;
+	seq_printf(file, "pcu observe: 0x%x\n", REG_READ_D(ah, AR_OBS_BUS_1));
+	seq_printf(file, "AR_CR: 0x%x\n", REG_READ_D(ah, AR_CR));
 
-	retval = simple_read_from_buffer(user_buf, count, ppos, buf, len);
-	kfree(buf);
-	return retval;
-}
-
-static const struct file_operations fops_dma = {
-	.read = read_file_dma,
-	.open = simple_open,
-	.owner = THIS_MODULE,
-	.llseek = default_llseek,
-};
+	ath9k_ps_restore(sc);
 
+	return 0;
+}
 
 void ath_debug_stat_interrupt(struct ath_softc *sc, enum ath9k_int status)
 {
@@ -556,22 +524,15 @@ void ath_debug_stat_interrupt(struct ath_softc *sc, enum ath9k_int status)
 		sc->debug.stats.istats.gen_timer++;
 }
 
-static ssize_t read_file_interrupt(struct file *file, char __user *user_buf,
-				   size_t count, loff_t *ppos)
+static int read_file_interrupt(struct seq_file *file, void *data)
 {
-	struct ath_softc *sc = file->private_data;
-	unsigned int len = 0;
-	int rv;
-	int mxlen = 4000;
-	char *buf = kmalloc(mxlen, GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
+	struct ieee80211_hw *hw = dev_get_drvdata(file->private);
+	struct ath_softc *sc = hw->priv;
 
 #define PR_IS(a, s)						\
 	do {							\
-		len += scnprintf(buf + len, mxlen - len,	\
-				 "%21s: %10u\n", a,		\
-				 sc->debug.stats.istats.s);	\
+		seq_printf(file, "%21s: %10u\n", a,		\
+			   sc->debug.stats.istats.s);		\
 	} while (0)
 
 	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_EDMA) {
@@ -602,8 +563,7 @@ static ssize_t read_file_interrupt(struct file *file, char __user *user_buf,
 	PR_IS("GENTIMER", gen_timer);
 	PR_IS("TOTAL", total);
 
-	len += scnprintf(buf + len, mxlen - len,
-			 "SYNC_CAUSE stats:\n");
+	seq_puts(file, "SYNC_CAUSE stats:\n");
 
 	PR_IS("Sync-All", sync_cause_all);
 	PR_IS("RTC-IRQ", sync_rtc_irq);
@@ -625,35 +585,15 @@ static ssize_t read_file_interrupt(struct file *file, char __user *user_buf,
 	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;
+	return 0;
 }
 
-static const struct file_operations fops_interrupt = {
-	.read = read_file_interrupt,
-	.open = simple_open,
-	.owner = THIS_MODULE,
-	.llseek = default_llseek,
-};
-
-static ssize_t read_file_xmit(struct file *file, char __user *user_buf,
-			      size_t count, loff_t *ppos)
+static int read_file_xmit(struct seq_file *file, void *data)
 {
-	struct ath_softc *sc = file->private_data;
-	char *buf;
-	unsigned int len = 0, size = 2048;
-	ssize_t retval = 0;
-
-	buf = kzalloc(size, GFP_KERNEL);
-	if (buf == NULL)
-		return -ENOMEM;
+	struct ieee80211_hw *hw = dev_get_drvdata(file->private);
+	struct ath_softc *sc = hw->priv;
 
-	len += sprintf(buf, "%30s %10s%10s%10s\n\n",
-		       "BE", "BK", "VI", "VO");
+	seq_printf(file, "%30s %10s%10s%10s\n\n", "BE", "BK", "VI", "VO");
 
 	PR("MPDUs Queued:    ", queued);
 	PR("MPDUs Completed: ", completed);
@@ -678,153 +618,117 @@ static ssize_t read_file_xmit(struct file *file, char __user *user_buf,
 	PR("HW-tx-proc-desc: ", txprocdesc);
 	PR("TX-Failed:       ", txfailed);
 
-	if (len > size)
-		len = size;
-
-	retval = simple_read_from_buffer(user_buf, count, ppos, buf, len);
-	kfree(buf);
-
-	return retval;
+	return 0;
 }
 
-static ssize_t print_queue(struct ath_softc *sc, struct ath_txq *txq,
-			   char *buf, ssize_t size)
+static void print_queue(struct ath_softc *sc, struct ath_txq *txq,
+			struct seq_file *file)
 {
-	ssize_t len = 0;
-
 	ath_txq_lock(sc, txq);
 
-	len += scnprintf(buf + len, size - len, "%s: %d ",
-			 "qnum", txq->axq_qnum);
-	len += scnprintf(buf + len, size - len, "%s: %2d ",
-			 "qdepth", txq->axq_depth);
-	len += scnprintf(buf + len, size - len, "%s: %2d ",
-			 "ampdu-depth", txq->axq_ampdu_depth);
-	len += scnprintf(buf + len, size - len, "%s: %3d ",
-			 "pending", txq->pending_frames);
-	len += scnprintf(buf + len, size - len, "%s: %d\n",
-			 "stopped", txq->stopped);
+	seq_printf(file, "%s: %d ", "qnum", txq->axq_qnum);
+	seq_printf(file, "%s: %2d ", "qdepth", txq->axq_depth);
+	seq_printf(file, "%s: %2d ", "ampdu-depth", txq->axq_ampdu_depth);
+	seq_printf(file, "%s: %3d ", "pending", txq->pending_frames);
+	seq_printf(file, "%s: %d\n", "stopped", txq->stopped);
 
 	ath_txq_unlock(sc, txq);
-	return len;
 }
 
-static ssize_t read_file_queues(struct file *file, char __user *user_buf,
-				size_t count, loff_t *ppos)
+static int read_file_queues(struct seq_file *file, void *data)
 {
-	struct ath_softc *sc = file->private_data;
+	struct ieee80211_hw *hw = dev_get_drvdata(file->private);
+	struct ath_softc *sc = hw->priv;
 	struct ath_txq *txq;
-	char *buf;
-	unsigned int len = 0;
-	const unsigned int size = 1024;
-	ssize_t retval = 0;
 	int i;
 	static const char *qname[4] = {
 		"VO", "VI", "BE", "BK"
 	};
 
-	buf = kzalloc(size, GFP_KERNEL);
-	if (buf == NULL)
-		return -ENOMEM;
-
 	for (i = 0; i < IEEE80211_NUM_ACS; i++) {
 		txq = sc->tx.txq_map[i];
-		len += scnprintf(buf + len, size - len, "(%s):  ", qname[i]);
-		len += print_queue(sc, txq, buf + len, size - len);
+		seq_printf(file, "(%s):  ", qname[i]);
+		print_queue(sc, txq, file);
 	}
 
-	len += scnprintf(buf + len, size - len, "(CAB): ");
-	len += print_queue(sc, sc->beacon.cabq, buf + len, size - len);
+	seq_puts(file, "(CAB): ");
+	print_queue(sc, sc->beacon.cabq, file);
 
-	if (len > size)
-		len = size;
-
-	retval = simple_read_from_buffer(user_buf, count, ppos, buf, len);
-	kfree(buf);
-
-	return retval;
+	return 0;
 }
 
-static ssize_t read_file_misc(struct file *file, char __user *user_buf,
-			      size_t count, loff_t *ppos)
+static int read_file_misc(struct seq_file *file, void *data)
 {
-	struct ath_softc *sc = file->private_data;
+	struct ieee80211_hw *hw = dev_get_drvdata(file->private);
+	struct ath_softc *sc = hw->priv;
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath9k_vif_iter_data iter_data;
 	struct ath_chanctx *ctx;
-	char buf[512];
-	unsigned int len = 0;
-	ssize_t retval = 0;
 	unsigned int reg;
 	u32 rxfilter, i;
 
-	len += scnprintf(buf + len, sizeof(buf) - len,
-			 "BSSID: %pM\n", common->curbssid);
-	len += scnprintf(buf + len, sizeof(buf) - len,
-			 "BSSID-MASK: %pM\n", common->bssidmask);
-	len += scnprintf(buf + len, sizeof(buf) - len,
-			 "OPMODE: %s\n",
-			 ath_opmode_to_string(sc->sc_ah->opmode));
+	seq_printf(file, "BSSID: %pM\n", common->curbssid);
+	seq_printf(file, "BSSID-MASK: %pM\n", common->bssidmask);
+	seq_printf(file, "OPMODE: %s\n",
+		   ath_opmode_to_string(sc->sc_ah->opmode));
 
 	ath9k_ps_wakeup(sc);
 	rxfilter = ath9k_hw_getrxfilter(sc->sc_ah);
 	ath9k_ps_restore(sc);
 
-	len += scnprintf(buf + len, sizeof(buf) - len,
-			 "RXFILTER: 0x%x", rxfilter);
+	seq_printf(file, "RXFILTER: 0x%x", rxfilter);
 
 	if (rxfilter & ATH9K_RX_FILTER_UCAST)
-		len += scnprintf(buf + len, sizeof(buf) - len, " UCAST");
+		seq_puts(file, " UCAST");
 	if (rxfilter & ATH9K_RX_FILTER_MCAST)
-		len += scnprintf(buf + len, sizeof(buf) - len, " MCAST");
+		seq_puts(file, " MCAST");
 	if (rxfilter & ATH9K_RX_FILTER_BCAST)
-		len += scnprintf(buf + len, sizeof(buf) - len, " BCAST");
+		seq_puts(file, " BCAST");
 	if (rxfilter & ATH9K_RX_FILTER_CONTROL)
-		len += scnprintf(buf + len, sizeof(buf) - len, " CONTROL");
+		seq_puts(file, " CONTROL");
 	if (rxfilter & ATH9K_RX_FILTER_BEACON)
-		len += scnprintf(buf + len, sizeof(buf) - len, " BEACON");
+		seq_puts(file, " BEACON");
 	if (rxfilter & ATH9K_RX_FILTER_PROM)
-		len += scnprintf(buf + len, sizeof(buf) - len, " PROM");
+		seq_puts(file, " PROM");
 	if (rxfilter & ATH9K_RX_FILTER_PROBEREQ)
-		len += scnprintf(buf + len, sizeof(buf) - len, " PROBEREQ");
+		seq_puts(file, " PROBEREQ");
 	if (rxfilter & ATH9K_RX_FILTER_PHYERR)
-		len += scnprintf(buf + len, sizeof(buf) - len, " PHYERR");
+		seq_puts(file, " PHYERR");
 	if (rxfilter & ATH9K_RX_FILTER_MYBEACON)
-		len += scnprintf(buf + len, sizeof(buf) - len, " MYBEACON");
+		seq_puts(file, " MYBEACON");
 	if (rxfilter & ATH9K_RX_FILTER_COMP_BAR)
-		len += scnprintf(buf + len, sizeof(buf) - len, " COMP_BAR");
+		seq_puts(file, " COMP_BAR");
 	if (rxfilter & ATH9K_RX_FILTER_PSPOLL)
-		len += scnprintf(buf + len, sizeof(buf) - len, " PSPOLL");
+		seq_puts(file, " PSPOLL");
 	if (rxfilter & ATH9K_RX_FILTER_PHYRADAR)
-		len += scnprintf(buf + len, sizeof(buf) - len, " PHYRADAR");
+		seq_puts(file, " PHYRADAR");
 	if (rxfilter & ATH9K_RX_FILTER_MCAST_BCAST_ALL)
-		len += scnprintf(buf + len, sizeof(buf) - len, " MCAST_BCAST_ALL");
+		seq_puts(file, " MCAST_BCAST_ALL");
 	if (rxfilter & ATH9K_RX_FILTER_CONTROL_WRAPPER)
-		len += scnprintf(buf + len, sizeof(buf) - len, " CONTROL_WRAPPER");
+		seq_puts(file, " CONTROL_WRAPPER");
 
-	len += scnprintf(buf + len, sizeof(buf) - len, "\n");
+	seq_puts(file, "\n");
 
 	reg = sc->sc_ah->imask;
 
-	len += scnprintf(buf + len, sizeof(buf) - len,
-			 "INTERRUPT-MASK: 0x%x", reg);
+	seq_printf(file, "INTERRUPT-MASK: 0x%x", reg);
 
 	if (reg & ATH9K_INT_SWBA)
-		len += scnprintf(buf + len, sizeof(buf) - len, " SWBA");
+		seq_puts(file, " SWBA");
 	if (reg & ATH9K_INT_BMISS)
-		len += scnprintf(buf + len, sizeof(buf) - len, " BMISS");
+		seq_puts(file, " BMISS");
 	if (reg & ATH9K_INT_CST)
-		len += scnprintf(buf + len, sizeof(buf) - len, " CST");
+		seq_puts(file, " CST");
 	if (reg & ATH9K_INT_RX)
-		len += scnprintf(buf + len, sizeof(buf) - len, " RX");
+		seq_puts(file, " RX");
 	if (reg & ATH9K_INT_RXHP)
-		len += scnprintf(buf + len, sizeof(buf) - len, " RXHP");
+		seq_puts(file, " RXHP");
 	if (reg & ATH9K_INT_RXLP)
-		len += scnprintf(buf + len, sizeof(buf) - len, " RXLP");
+		seq_puts(file, " RXLP");
 	if (reg & ATH9K_INT_BB_WATCHDOG)
-		len += scnprintf(buf + len, sizeof(buf) - len, " BB_WATCHDOG");
+		seq_puts(file, " BB_WATCHDOG");
 
-	len += scnprintf(buf + len, sizeof(buf) - len, "\n");
+	seq_puts(file, "\n");
 
 	i = 0;
 	ath_for_each_chanctx(sc, ctx) {
@@ -832,27 +736,23 @@ static ssize_t read_file_misc(struct file *file, char __user *user_buf,
 			continue;
 		ath9k_calculate_iter_data(sc, ctx, &iter_data);
 
-		len += scnprintf(buf + len, sizeof(buf) - len,
-			"VIFS: CTX %i(%i) AP: %i STA: %i MESH: %i WDS: %i",
-			i++, (int)(ctx->assigned), iter_data.naps,
-			iter_data.nstations,
-			iter_data.nmeshes, iter_data.nwds);
-		len += scnprintf(buf + len, sizeof(buf) - len,
-			" ADHOC: %i TOTAL: %hi BEACON-VIF: %hi\n",
-			iter_data.nadhocs, sc->cur_chan->nvifs, sc->nbcnvifs);
+		seq_printf(file,
+			   "VIFS: CTX %i(%i) AP: %i STA: %i MESH: %i WDS: %i",
+			   i++, (int)(ctx->assigned), iter_data.naps,
+			   iter_data.nstations,
+			   iter_data.nmeshes, iter_data.nwds);
+		seq_printf(file, " ADHOC: %i TOTAL: %hi BEACON-VIF: %hi\n",
+			   iter_data.nadhocs, sc->cur_chan->nvifs,
+			   sc->nbcnvifs);
 	}
 
-	if (len > sizeof(buf))
-		len = sizeof(buf);
-
-	retval = simple_read_from_buffer(user_buf, count, ppos, buf, len);
-	return retval;
+	return 0;
 }
 
-static ssize_t read_file_reset(struct file *file, char __user *user_buf,
-			       size_t count, loff_t *ppos)
+static int read_file_reset(struct seq_file *file, void *data)
 {
-	struct ath_softc *sc = file->private_data;
+	struct ieee80211_hw *hw = dev_get_drvdata(file->private);
+	struct ath_softc *sc = hw->priv;
 	static const char * const reset_cause[__RESET_TYPE_MAX] = {
 		[RESET_TYPE_BB_HANG] = "Baseband Hang",
 		[RESET_TYPE_BB_WATCHDOG] = "Baseband Watchdog",
@@ -866,23 +766,17 @@ static ssize_t read_file_reset(struct file *file, char __user *user_buf,
 		[RESET_TYPE_MCI] = "MCI Reset",
 		[RESET_TYPE_CALIBRATION] = "Calibration error",
 	};
-	char buf[512];
-	unsigned int len = 0;
 	int i;
 
 	for (i = 0; i < ARRAY_SIZE(reset_cause); i++) {
 		if (!reset_cause[i])
 		    continue;
 
-		len += scnprintf(buf + len, sizeof(buf) - len,
-				 "%17s: %2d\n", reset_cause[i],
-				 sc->debug.stats.reset[i]);
+		seq_printf(file, "%17s: %2d\n", reset_cause[i],
+			   sc->debug.stats.reset[i]);
 	}
 
-	if (len > sizeof(buf))
-		len = sizeof(buf);
-
-	return simple_read_from_buffer(user_buf, count, ppos, buf, len);
+	return 0;
 }
 
 void ath_debug_stat_tx(struct ath_softc *sc, struct ath_buf *bf,
@@ -922,41 +816,13 @@ void ath_debug_stat_tx(struct ath_softc *sc, struct ath_buf *bf,
 		TX_STAT_INC(qnum, delim_underrun);
 }
 
-static const struct file_operations fops_xmit = {
-	.read = read_file_xmit,
-	.open = simple_open,
-	.owner = THIS_MODULE,
-	.llseek = default_llseek,
-};
-
-static const struct file_operations fops_queues = {
-	.read = read_file_queues,
-	.open = simple_open,
-	.owner = THIS_MODULE,
-	.llseek = default_llseek,
-};
-
-static const struct file_operations fops_misc = {
-	.read = read_file_misc,
-	.open = simple_open,
-	.owner = THIS_MODULE,
-	.llseek = default_llseek,
-};
-
-static const struct file_operations fops_reset = {
-	.read = read_file_reset,
-	.open = simple_open,
-	.owner = THIS_MODULE,
-	.llseek = default_llseek,
-};
-
 void ath_debug_stat_rx(struct ath_softc *sc, struct ath_rx_status *rs)
 {
 	ath9k_cmn_debug_stat_rx(&sc->debug.stats.rxstats, rs);
 }
 
 static ssize_t read_file_regidx(struct file *file, char __user *user_buf,
-                                size_t count, loff_t *ppos)
+				size_t count, loff_t *ppos)
 {
 	struct ath_softc *sc = file->private_data;
 	char buf[32];
@@ -967,7 +833,7 @@ static ssize_t read_file_regidx(struct file *file, char __user *user_buf,
 }
 
 static ssize_t write_file_regidx(struct file *file, const char __user *user_buf,
-			     size_t count, loff_t *ppos)
+				 size_t count, loff_t *ppos)
 {
 	struct ath_softc *sc = file->private_data;
 	unsigned long regidx;
@@ -995,7 +861,7 @@ static const struct file_operations fops_regidx = {
 };
 
 static ssize_t read_file_regval(struct file *file, char __user *user_buf,
-			     size_t count, loff_t *ppos)
+				size_t count, loff_t *ppos)
 {
 	struct ath_softc *sc = file->private_data;
 	struct ath_hw *ah = sc->sc_ah;
@@ -1011,7 +877,7 @@ static ssize_t read_file_regval(struct file *file, char __user *user_buf,
 }
 
 static ssize_t write_file_regval(struct file *file, const char __user *user_buf,
-			     size_t count, loff_t *ppos)
+				 size_t count, loff_t *ppos)
 {
 	struct ath_softc *sc = file->private_data;
 	struct ath_hw *ah = sc->sc_ah;
@@ -1077,57 +943,46 @@ static const struct file_operations fops_regdump = {
 	.llseek = default_llseek,/* read accesses f_pos */
 };
 
-static ssize_t read_file_dump_nfcal(struct file *file, char __user *user_buf,
-				    size_t count, loff_t *ppos)
+static int read_file_dump_nfcal(struct seq_file *file, void *data)
 {
-	struct ath_softc *sc = file->private_data;
+	struct ieee80211_hw *hw = dev_get_drvdata(file->private);
+	struct ath_softc *sc = hw->priv;
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath9k_nfcal_hist *h = sc->cur_chan->caldata.nfCalHist;
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ieee80211_conf *conf = &common->hw->conf;
-	u32 len = 0, size = 1500;
 	u32 i, j;
-	ssize_t retval = 0;
-	char *buf;
 	u8 chainmask = (ah->rxchainmask << 3) | ah->rxchainmask;
 	u8 nread;
 
-	buf = kzalloc(size, GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
-
-	len += scnprintf(buf + len, size - len,
-			 "Channel Noise Floor : %d\n", ah->noise);
-	len += scnprintf(buf + len, size - len,
-			 "Chain | privNF | # Readings | NF Readings\n");
+	seq_printf(file, "Channel Noise Floor : %d\n", ah->noise);
+	seq_puts(file, "Chain | privNF | # Readings | NF Readings\n");
 	for (i = 0; i < NUM_NF_READINGS; i++) {
 		if (!(chainmask & (1 << i)) ||
 		    ((i >= AR5416_MAX_CHAINS) && !conf_is_ht40(conf)))
 			continue;
 
 		nread = AR_PHY_CCA_FILTERWINDOW_LENGTH - h[i].invalidNFcount;
-		len += scnprintf(buf + len, size - len, " %d\t %d\t %d\t\t",
-				 i, h[i].privNF, nread);
+		seq_printf(file, " %d\t %d\t %d\t\t", i, h[i].privNF, nread);
 		for (j = 0; j < nread; j++)
-			len += scnprintf(buf + len, size - len,
-					 " %d", h[i].nfCalBuffer[j]);
-		len += scnprintf(buf + len, size - len, "\n");
+			seq_printf(file, " %d", h[i].nfCalBuffer[j]);
+		seq_puts(file, "\n");
 	}
 
-	if (len > size)
-		len = size;
-
-	retval = simple_read_from_buffer(user_buf, count, ppos, buf, len);
-	kfree(buf);
+	return 0;
+}
 
-	return retval;
+static int open_file_dump_nfcal(struct inode *inode, struct file *f)
+{
+	return single_open(f, read_file_dump_nfcal, inode->i_private);
 }
 
 static const struct file_operations fops_dump_nfcal = {
-	.read = read_file_dump_nfcal,
-	.open = simple_open,
+	.read = seq_read,
+	.open = open_file_dump_nfcal,
 	.owner = THIS_MODULE,
-	.llseek = default_llseek,
+	.llseek = seq_lseek,
+	.release = single_release,
 };
 
 #ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
@@ -1188,6 +1043,70 @@ static const struct file_operations fops_ackto = {
 };
 #endif
 
+static ssize_t read_file_tpc(struct file *file, char __user *user_buf,
+			     size_t count, loff_t *ppos)
+{
+	struct ath_softc *sc = file->private_data;
+	struct ath_hw *ah = sc->sc_ah;
+	unsigned int len = 0, size = 32;
+	ssize_t retval;
+	char *buf;
+
+	buf = kzalloc(size, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	len += scnprintf(buf + len, size - len, "%s\n",
+			 ah->tpc_enabled ? "ENABLED" : "DISABLED");
+
+	if (len > size)
+		len = size;
+
+	retval = simple_read_from_buffer(user_buf, count, ppos, buf, len);
+	kfree(buf);
+
+	return retval;
+}
+
+static ssize_t write_file_tpc(struct file *file, const char __user *user_buf,
+			      size_t count, loff_t *ppos)
+{
+	struct ath_softc *sc = file->private_data;
+	struct ath_hw *ah = sc->sc_ah;
+	unsigned long val;
+	char buf[32];
+	ssize_t len;
+	bool tpc_enabled;
+
+	len = min(count, sizeof(buf) - 1);
+	if (copy_from_user(buf, user_buf, len))
+		return -EFAULT;
+
+	buf[len] = '\0';
+	if (kstrtoul(buf, 0, &val))
+		return -EINVAL;
+
+	if (val < 0 || val > 1)
+		return -EINVAL;
+
+	tpc_enabled = !!val;
+
+	if (tpc_enabled != ah->tpc_enabled) {
+		ah->tpc_enabled = tpc_enabled;
+		ath9k_hw_set_txpowerlimit(ah, sc->cur_chan->txpower, false);
+	}
+
+	return count;
+}
+
+static const struct file_operations fops_tpc = {
+	.read = read_file_tpc,
+	.write = write_file_tpc,
+	.open = simple_open,
+	.owner = THIS_MODULE,
+	.llseek = default_llseek,
+};
+
 /* Ethtool support for get-stats */
 
 #define AMKSTR(nm) #nm "_BE", #nm "_BK", #nm "_VI", #nm "_VO"
@@ -1333,14 +1252,14 @@ int ath9k_init_debug(struct ath_hw *ah)
 	ath9k_tx99_init_debug(sc);
 	ath9k_cmn_spectral_init_debug(&sc->spec_priv, sc->debug.debugfs_phy);
 
-	debugfs_create_file("dma", S_IRUSR, sc->debug.debugfs_phy, sc,
-			    &fops_dma);
-	debugfs_create_file("interrupt", S_IRUSR, sc->debug.debugfs_phy, sc,
-			    &fops_interrupt);
-	debugfs_create_file("xmit", S_IRUSR, sc->debug.debugfs_phy, sc,
-			    &fops_xmit);
-	debugfs_create_file("queues", S_IRUSR, sc->debug.debugfs_phy, sc,
-			    &fops_queues);
+	debugfs_create_devm_seqfile(sc->dev, "dma", sc->debug.debugfs_phy,
+				    read_file_dma);
+	debugfs_create_devm_seqfile(sc->dev, "interrupt", sc->debug.debugfs_phy,
+				    read_file_interrupt);
+	debugfs_create_devm_seqfile(sc->dev, "xmit", sc->debug.debugfs_phy,
+				    read_file_xmit);
+	debugfs_create_devm_seqfile(sc->dev, "queues", sc->debug.debugfs_phy,
+				    read_file_queues);
 	debugfs_create_u32("qlen_bk", S_IRUSR | S_IWUSR, sc->debug.debugfs_phy,
 			   &sc->tx.txq_max_pending[IEEE80211_AC_BK]);
 	debugfs_create_u32("qlen_be", S_IRUSR | S_IWUSR, sc->debug.debugfs_phy,
@@ -1349,10 +1268,10 @@ int ath9k_init_debug(struct ath_hw *ah)
 			   &sc->tx.txq_max_pending[IEEE80211_AC_VI]);
 	debugfs_create_u32("qlen_vo", S_IRUSR | S_IWUSR, sc->debug.debugfs_phy,
 			   &sc->tx.txq_max_pending[IEEE80211_AC_VO]);
-	debugfs_create_file("misc", S_IRUSR, sc->debug.debugfs_phy, sc,
-			    &fops_misc);
-	debugfs_create_file("reset", S_IRUSR, sc->debug.debugfs_phy, sc,
-			    &fops_reset);
+	debugfs_create_devm_seqfile(sc->dev, "misc", sc->debug.debugfs_phy,
+				    read_file_misc);
+	debugfs_create_devm_seqfile(sc->dev, "reset", sc->debug.debugfs_phy,
+				    read_file_reset);
 
 	ath9k_cmn_debug_recv(sc->debug.debugfs_phy, &sc->debug.stats.rxstats);
 	ath9k_cmn_debug_phy_err(sc->debug.debugfs_phy, &sc->debug.stats.rxstats);
@@ -1374,8 +1293,9 @@ int ath9k_init_debug(struct ath_hw *ah)
 			    &ah->config.cwm_ignore_extcca);
 	debugfs_create_file("regdump", S_IRUSR, sc->debug.debugfs_phy, sc,
 			    &fops_regdump);
-	debugfs_create_file("dump_nfcal", S_IRUSR, sc->debug.debugfs_phy, sc,
-			    &fops_dump_nfcal);
+	debugfs_create_devm_seqfile(sc->dev, "dump_nfcal",
+				    sc->debug.debugfs_phy,
+				    read_file_dump_nfcal);
 
 	ath9k_cmn_debug_base_eeprom(sc->debug.debugfs_phy, sc->sc_ah);
 	ath9k_cmn_debug_modal_eeprom(sc->debug.debugfs_phy, sc->sc_ah);
@@ -1397,6 +1317,8 @@ int ath9k_init_debug(struct ath_hw *ah)
 	debugfs_create_file("ack_to", S_IRUSR | S_IWUSR, sc->debug.debugfs_phy,
 			    sc, &fops_ackto);
 #endif
+	debugfs_create_file("tpc", S_IRUSR | S_IWUSR,
+			    sc->debug.debugfs_phy, sc, &fops_tpc);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/ath/ath9k/debug.h b/drivers/net/wireless/ath/ath9k/debug.h
index bd75b1f716db..a8e9319958e6 100644
--- a/drivers/net/wireless/ath/ath9k/debug.h
+++ b/drivers/net/wireless/ath/ath9k/debug.h
@@ -196,12 +196,11 @@ struct ath_tx_stats {
 #define TXSTATS sc->debug.stats.txstats
 #define PR(str, elem)							\
 	do {								\
-		len += scnprintf(buf + len, size - len,			\
-				 "%s%13u%11u%10u%10u\n", str,		\
-				 TXSTATS[PR_QNUM(IEEE80211_AC_BE)].elem,\
-				 TXSTATS[PR_QNUM(IEEE80211_AC_BK)].elem,\
-				 TXSTATS[PR_QNUM(IEEE80211_AC_VI)].elem,\
-				 TXSTATS[PR_QNUM(IEEE80211_AC_VO)].elem); \
+		seq_printf(file, "%s%13u%11u%10u%10u\n", str,		\
+			   TXSTATS[PR_QNUM(IEEE80211_AC_BE)].elem,\
+			   TXSTATS[PR_QNUM(IEEE80211_AC_BK)].elem,\
+			   TXSTATS[PR_QNUM(IEEE80211_AC_VI)].elem,\
+			   TXSTATS[PR_QNUM(IEEE80211_AC_VO)].elem); \
 	} while(0)
 
 struct ath_rx_rate_stats {
diff --git a/drivers/net/wireless/ath/ath9k/eeprom_4k.c b/drivers/net/wireless/ath/ath9k/eeprom_4k.c
index 07b806c56c56..e5a78d4fd66e 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom_4k.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom_4k.c
@@ -748,6 +748,20 @@ static void ath9k_hw_4k_set_txpower(struct ath_hw *ah,
 			  | ATH9K_POW_SM(ratesArray[rateDupCck], 0));
 	}
 
+	/* TPC initializations */
+	if (ah->tpc_enabled) {
+		int ht40_delta;
+
+		ht40_delta = (IS_CHAN_HT40(chan)) ? ht40PowerIncForPdadc : 0;
+		ar5008_hw_init_rate_txpower(ah, ratesArray, chan, ht40_delta);
+		/* Enable TPC */
+		REG_WRITE(ah, AR_PHY_POWER_TX_RATE_MAX,
+			MAX_RATE_POWER | AR_PHY_POWER_TX_RATE_MAX_TPC_ENABLE);
+	} else {
+		/* Disable TPC */
+		REG_WRITE(ah, AR_PHY_POWER_TX_RATE_MAX, MAX_RATE_POWER);
+	}
+
 	REGWRITE_BUFFER_FLUSH(ah);
 }
 
diff --git a/drivers/net/wireless/ath/ath9k/eeprom_9287.c b/drivers/net/wireless/ath/ath9k/eeprom_9287.c
index 5ba1385c9838..6ca33dfde1fd 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom_9287.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom_9287.c
@@ -886,6 +886,21 @@ static void ath9k_hw_ar9287_set_txpower(struct ath_hw *ah,
 			  | ATH9K_POW_SM(ratesArray[rateDupOfdm], 8)
 			  | ATH9K_POW_SM(ratesArray[rateDupCck], 0));
 	}
+
+	/* TPC initializations */
+	if (ah->tpc_enabled) {
+		int ht40_delta;
+
+		ht40_delta = (IS_CHAN_HT40(chan)) ? ht40PowerIncForPdadc : 0;
+		ar5008_hw_init_rate_txpower(ah, ratesArray, chan, ht40_delta);
+		/* Enable TPC */
+		REG_WRITE(ah, AR_PHY_POWER_TX_RATE_MAX,
+			MAX_RATE_POWER | AR_PHY_POWER_TX_RATE_MAX_TPC_ENABLE);
+	} else {
+		/* Disable TPC */
+		REG_WRITE(ah, AR_PHY_POWER_TX_RATE_MAX, MAX_RATE_POWER);
+	}
+
 	REGWRITE_BUFFER_FLUSH(ah);
 }
 
diff --git a/drivers/net/wireless/ath/ath9k/eeprom_def.c b/drivers/net/wireless/ath/ath9k/eeprom_def.c
index 122b846b8ec0..098059039351 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom_def.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom_def.c
@@ -1332,6 +1332,20 @@ static void ath9k_hw_def_set_txpower(struct ath_hw *ah,
 		  ATH9K_POW_SM(pModal->pwrDecreaseFor3Chain, 6)
 		  | ATH9K_POW_SM(pModal->pwrDecreaseFor2Chain, 0));
 
+	/* TPC initializations */
+	if (ah->tpc_enabled) {
+		int ht40_delta;
+
+		ht40_delta = (IS_CHAN_HT40(chan)) ? ht40PowerIncForPdadc : 0;
+		ar5008_hw_init_rate_txpower(ah, ratesArray, chan, ht40_delta);
+		/* Enable TPC */
+		REG_WRITE(ah, AR_PHY_POWER_TX_RATE_MAX,
+			MAX_RATE_POWER | AR_PHY_POWER_TX_RATE_MAX_TPC_ENABLE);
+	} else {
+		/* Disable TPC */
+		REG_WRITE(ah, AR_PHY_POWER_TX_RATE_MAX, MAX_RATE_POWER);
+	}
+
 	REGWRITE_BUFFER_FLUSH(ah);
 }
 
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index 6d4b273469b1..82d8f32a3461 100644
--- a/drivers/net/wireless/ath/ath9k/hw.c
+++ b/drivers/net/wireless/ath/ath9k/hw.c
@@ -246,6 +246,8 @@ static void ath9k_hw_read_revisions(struct ath_hw *ah)
 	case AR9300_DEVID_AR953X:
 		ah->hw_version.macVersion = AR_SREV_VERSION_9531;
 		return;
+	case AR9300_DEVID_QCA956X:
+		ah->hw_version.macVersion = AR_SREV_VERSION_9561;
 	}
 
 	val = REG_READ(ah, AR_SREV) & AR_SREV_ID;
@@ -422,6 +424,8 @@ static void ath9k_hw_init_defaults(struct ath_hw *ah)
 	ah->power_mode = ATH9K_PM_UNDEFINED;
 	ah->htc_reset_init = true;
 
+	ah->tpc_enabled = true;
+
 	ah->ani_function = ATH9K_ANI_ALL;
 	if (!AR_SREV_9300_20_OR_LATER(ah))
 		ah->ani_function &= ~ATH9K_ANI_MRC_CCK;
@@ -536,6 +540,7 @@ static int __ath9k_hw_init(struct ath_hw *ah)
 	case AR_SREV_VERSION_9550:
 	case AR_SREV_VERSION_9565:
 	case AR_SREV_VERSION_9531:
+	case AR_SREV_VERSION_9561:
 		break;
 	default:
 		ath_err(common,
@@ -636,6 +641,7 @@ int ath9k_hw_init(struct ath_hw *ah)
 	case AR9485_DEVID_AR1111:
 	case AR9300_DEVID_AR9565:
 	case AR9300_DEVID_AR953X:
+	case AR9300_DEVID_QCA956X:
 		break;
 	default:
 		if (common->bus_ops->ath_bus_type == ATH_USB)
@@ -776,7 +782,8 @@ static void ath9k_hw_init_pll(struct ath_hw *ah,
 		/* program BB PLL phase_shift */
 		REG_RMW_FIELD(ah, AR_CH0_BB_DPLL3,
 			      AR_CH0_BB_DPLL3_PHASE_SHIFT, 0x1);
-	} else if (AR_SREV_9340(ah) || AR_SREV_9550(ah) || AR_SREV_9531(ah)) {
+	} else if (AR_SREV_9340(ah) || AR_SREV_9550(ah) || AR_SREV_9531(ah) ||
+		   AR_SREV_9561(ah)) {
 		u32 regval, pll2_divint, pll2_divfrac, refdiv;
 
 		REG_WRITE(ah, AR_RTC_PLL_CONTROL,
@@ -787,7 +794,7 @@ static void ath9k_hw_init_pll(struct ath_hw *ah,
 		udelay(100);
 
 		if (ah->is_clk_25mhz) {
-			if (AR_SREV_9531(ah)) {
+			if (AR_SREV_9531(ah) || AR_SREV_9561(ah)) {
 				pll2_divint = 0x1c;
 				pll2_divfrac = 0xa3d2;
 				refdiv = 1;
@@ -803,14 +810,15 @@ static void ath9k_hw_init_pll(struct ath_hw *ah,
 				refdiv = 5;
 			} else {
 				pll2_divint = 0x11;
-				pll2_divfrac =
-					AR_SREV_9531(ah) ? 0x26665 : 0x26666;
+				pll2_divfrac = (AR_SREV_9531(ah) ||
+						AR_SREV_9561(ah)) ?
+						0x26665 : 0x26666;
 				refdiv = 1;
 			}
 		}
 
 		regval = REG_READ(ah, AR_PHY_PLL_MODE);
-		if (AR_SREV_9531(ah))
+		if (AR_SREV_9531(ah) || AR_SREV_9561(ah))
 			regval |= (0x1 << 22);
 		else
 			regval |= (0x1 << 16);
@@ -828,14 +836,16 @@ static void ath9k_hw_init_pll(struct ath_hw *ah,
 				(0x1 << 13) |
 				(0x4 << 26) |
 				(0x18 << 19);
-		else if (AR_SREV_9531(ah))
+		else if (AR_SREV_9531(ah) || AR_SREV_9561(ah)) {
 			regval = (regval & 0x01c00fff) |
 				(0x1 << 31) |
 				(0x2 << 29) |
 				(0xa << 25) |
-				(0x1 << 19) |
-				(0x6 << 12);
-		else
+				(0x1 << 19);
+
+			if (AR_SREV_9531(ah))
+				regval |= (0x6 << 12);
+		} else
 			regval = (regval & 0x80071fff) |
 				(0x3 << 30) |
 				(0x1 << 13) |
@@ -843,7 +853,7 @@ static void ath9k_hw_init_pll(struct ath_hw *ah,
 				(0x60 << 19);
 		REG_WRITE(ah, AR_PHY_PLL_MODE, regval);
 
-		if (AR_SREV_9531(ah))
+		if (AR_SREV_9531(ah) || AR_SREV_9561(ah))
 			REG_WRITE(ah, AR_PHY_PLL_MODE,
 				  REG_READ(ah, AR_PHY_PLL_MODE) & 0xffbfffff);
 		else
@@ -882,7 +892,8 @@ static void ath9k_hw_init_interrupt_masks(struct ath_hw *ah,
 		AR_IMR_RXORN |
 		AR_IMR_BCNMISC;
 
-	if (AR_SREV_9340(ah) || AR_SREV_9550(ah) || AR_SREV_9531(ah))
+	if (AR_SREV_9340(ah) || AR_SREV_9550(ah) || AR_SREV_9531(ah) ||
+	    AR_SREV_9561(ah))
 		sync_default &= ~AR_INTR_SYNC_HOST1_FATAL;
 
 	if (AR_SREV_9300_20_OR_LATER(ah)) {
@@ -1671,7 +1682,8 @@ static void ath9k_hw_init_desc(struct ath_hw *ah)
 		}
 #ifdef __BIG_ENDIAN
 		else if (AR_SREV_9330(ah) || AR_SREV_9340(ah) ||
-			 AR_SREV_9550(ah) || AR_SREV_9531(ah))
+			 AR_SREV_9550(ah) || AR_SREV_9531(ah) ||
+			 AR_SREV_9561(ah))
 			REG_RMW(ah, AR_CFG, AR_CFG_SWRB | AR_CFG_SWTB, 0);
 		else
 			REG_WRITE(ah, AR_CFG, AR_CFG_SWTD | AR_CFG_SWRD);
@@ -2459,7 +2471,8 @@ int ath9k_hw_fill_cap_info(struct ath_hw *ah)
 
 	if (AR_SREV_9300_20_OR_LATER(ah)) {
 		pCap->hw_caps |= ATH9K_HW_CAP_EDMA | ATH9K_HW_CAP_FASTCLOCK;
-		if (!AR_SREV_9330(ah) && !AR_SREV_9485(ah) && !AR_SREV_9565(ah))
+		if (!AR_SREV_9330(ah) && !AR_SREV_9485(ah) &&
+		    !AR_SREV_9561(ah) && !AR_SREV_9565(ah))
 			pCap->hw_caps |= ATH9K_HW_CAP_LDPC;
 
 		pCap->rx_hp_qdepth = ATH9K_HW_RX_HP_QDEPTH;
@@ -2476,7 +2489,9 @@ int ath9k_hw_fill_cap_info(struct ath_hw *ah)
 	if (AR_SREV_9300_20_OR_LATER(ah))
 		pCap->hw_caps |= ATH9K_HW_CAP_RAC_SUPPORTED;
 
-	if (AR_SREV_9300_20_OR_LATER(ah))
+	if (AR_SREV_9561(ah))
+		ah->ent_mode = 0x3BDA000;
+	else if (AR_SREV_9300_20_OR_LATER(ah))
 		ah->ent_mode = REG_READ(ah, AR_ENT_OTP);
 
 	if (AR_SREV_9287_11_OR_LATER(ah) || AR_SREV_9271(ah))
diff --git a/drivers/net/wireless/ath/ath9k/hw.h b/drivers/net/wireless/ath/ath9k/hw.h
index 1cbd33551513..450704e49f03 100644
--- a/drivers/net/wireless/ath/ath9k/hw.h
+++ b/drivers/net/wireless/ath/ath9k/hw.h
@@ -54,6 +54,7 @@
 #define AR9485_DEVID_AR1111	0x0037
 #define AR9300_DEVID_AR9565     0x0036
 #define AR9300_DEVID_AR953X     0x003d
+#define AR9300_DEVID_QCA956X    0x003f
 
 #define AR5416_AR9100_DEVID	0x000b
 
@@ -1086,6 +1087,8 @@ bool ar9003_is_paprd_enabled(struct ath_hw *ah);
 void ar9003_hw_set_chain_masks(struct ath_hw *ah, u8 rx, u8 tx);
 void ar9003_hw_init_rate_txpower(struct ath_hw *ah, u8 *rate_array,
 				 struct ath9k_channel *chan);
+void ar5008_hw_init_rate_txpower(struct ath_hw *ah, int16_t *rate_array,
+				 struct ath9k_channel *chan, int ht40_delta);
 
 /* Hardware family op attach helpers */
 int ar5008_hw_attach_phy_ops(struct ath_hw *ah);
diff --git a/drivers/net/wireless/ath/ath9k/mac.c b/drivers/net/wireless/ath/ath9k/mac.c
index 3e58bfa0c1fd..bba85d1a6cd1 100644
--- a/drivers/net/wireless/ath/ath9k/mac.c
+++ b/drivers/net/wireless/ath/ath9k/mac.c
@@ -820,7 +820,8 @@ void ath9k_hw_enable_interrupts(struct ath_hw *ah)
 		return;
 	}
 
-	if (AR_SREV_9340(ah) || AR_SREV_9550(ah) || AR_SREV_9531(ah))
+	if (AR_SREV_9340(ah) || AR_SREV_9550(ah) || AR_SREV_9531(ah) ||
+	    AR_SREV_9561(ah))
 		sync_default &= ~AR_INTR_SYNC_HOST1_FATAL;
 
 	async_mask = AR_INTR_MAC_IRQ;
diff --git a/drivers/net/wireless/ath/ath9k/pci.c b/drivers/net/wireless/ath/ath9k/pci.c
index f009b5b57e5e..cc5c6810f32e 100644
--- a/drivers/net/wireless/ath/ath9k/pci.c
+++ b/drivers/net/wireless/ath/ath9k/pci.c
@@ -427,6 +427,11 @@ static const struct pci_device_id ath_pci_id_table[] = {
 	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
 			 0x0036,
 			 0x11AD, /* LITEON */
+			 0x1842),
+	  .driver_data = ATH9K_PCI_AR9565_1ANT },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
+			 0x0036,
+			 0x11AD, /* LITEON */
 			 0x6671),
 	  .driver_data = ATH9K_PCI_AR9565_1ANT },
 	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
@@ -446,9 +451,19 @@ static const struct pci_device_id ath_pci_id_table[] = {
 	  .driver_data = ATH9K_PCI_AR9565_1ANT },
 	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
 			 0x0036,
+			 0x1B9A, /* XAVI */
+			 0x28A3),
+	  .driver_data = ATH9K_PCI_AR9565_1ANT },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
+			 0x0036,
 			 PCI_VENDOR_ID_AZWAVE,
 			 0x218A),
 	  .driver_data = ATH9K_PCI_AR9565_1ANT },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
+			 0x0036,
+			 PCI_VENDOR_ID_AZWAVE,
+			 0x2F8A),
+	  .driver_data = ATH9K_PCI_AR9565_1ANT },
 
 	/* WB335 1-ANT / Antenna Diversity */
 	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
@@ -508,6 +523,11 @@ static const struct pci_device_id ath_pci_id_table[] = {
 	  .driver_data = ATH9K_PCI_AR9565_1ANT | ATH9K_PCI_BT_ANT_DIV },
 	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
 			 0x0036,
+			 PCI_VENDOR_ID_AZWAVE,
+			 0x213C),
+	  .driver_data = ATH9K_PCI_AR9565_1ANT | ATH9K_PCI_BT_ANT_DIV },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
+			 0x0036,
 			 PCI_VENDOR_ID_HP,
 			 0x18E3),
 	  .driver_data = ATH9K_PCI_AR9565_1ANT | ATH9K_PCI_BT_ANT_DIV },
@@ -555,6 +575,16 @@ static const struct pci_device_id ath_pci_id_table[] = {
 	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
 	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
 			 0x0036,
+			 PCI_VENDOR_ID_SAMSUNG,
+			 0x4129),
+	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
+			 0x0036,
+			 PCI_VENDOR_ID_SAMSUNG,
+			 0x412A),
+	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
+			 0x0036,
 			 PCI_VENDOR_ID_ATHEROS,
 			 0x3027),
 	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
@@ -586,10 +616,25 @@ static const struct pci_device_id ath_pci_id_table[] = {
 	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
 			 0x0036,
 			 0x11AD, /* LITEON */
+			 0x1832),
+	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
+			 0x0036,
+			 0x11AD, /* LITEON */
 			 0x0692),
 	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
 	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
 			 0x0036,
+			 0x11AD, /* LITEON */
+			 0x0803),
+	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
+			 0x0036,
+			 0x11AD, /* LITEON */
+			 0x0813),
+	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
+			 0x0036,
 			 PCI_VENDOR_ID_AZWAVE,
 			 0x2130),
 	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
@@ -605,6 +650,21 @@ static const struct pci_device_id ath_pci_id_table[] = {
 	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
 	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
 			 0x0036,
+			 PCI_VENDOR_ID_AZWAVE,
+			 0x218B),
+	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
+			 0x0036,
+			 PCI_VENDOR_ID_AZWAVE,
+			 0x218C),
+	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
+			 0x0036,
+			 PCI_VENDOR_ID_AZWAVE,
+			 0x2F82),
+	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
+			 0x0036,
 			 0x144F, /* ASKEY */
 			 0x7202),
 	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
@@ -616,10 +676,20 @@ static const struct pci_device_id ath_pci_id_table[] = {
 	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
 			 0x0036,
 			 0x1B9A, /* XAVI */
+			 0x2813),
+	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
+			 0x0036,
+			 0x1B9A, /* XAVI */
 			 0x28A2),
 	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
 	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
 			 0x0036,
+			 0x1B9A, /* XAVI */
+			 0x28A4),
+	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
+			 0x0036,
 			 0x185F, /* WNC */
 			 0x3027),
 	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
@@ -636,10 +706,25 @@ static const struct pci_device_id ath_pci_id_table[] = {
 	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
 			 0x0036,
 			 PCI_VENDOR_ID_FOXCONN,
+			 0xE08F),
+	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
+			 0x0036,
+			 PCI_VENDOR_ID_FOXCONN,
 			 0xE081),
 	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
 	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
 			 0x0036,
+			 PCI_VENDOR_ID_FOXCONN,
+			 0xE091),
+	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
+			 0x0036,
+			 PCI_VENDOR_ID_FOXCONN,
+			 0xE099),
+	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_ATHEROS,
+			 0x0036,
 			 PCI_VENDOR_ID_LENOVO,
 			 0x3026),
 	  .driver_data = ATH9K_PCI_AR9565_2ANT | ATH9K_PCI_BT_ANT_DIV },
diff --git a/drivers/net/wireless/ath/ath9k/recv.c b/drivers/net/wireless/ath/ath9k/recv.c
index 7395afbc5124..6fb40ef86fd6 100644
--- a/drivers/net/wireless/ath/ath9k/recv.c
+++ b/drivers/net/wireless/ath/ath9k/recv.c
@@ -425,7 +425,8 @@ u32 ath_calcrxfilter(struct ath_softc *sc)
 		rfilt |= ATH9K_RX_FILTER_MCAST_BCAST_ALL;
 	}
 
-	if (AR_SREV_9550(sc->sc_ah) || AR_SREV_9531(sc->sc_ah))
+	if (AR_SREV_9550(sc->sc_ah) || AR_SREV_9531(sc->sc_ah) ||
+	    AR_SREV_9561(sc->sc_ah))
 		rfilt |= ATH9K_RX_FILTER_4ADDRESS;
 
 	if (ath9k_is_chanctx_enabled() &&
diff --git a/drivers/net/wireless/ath/ath9k/reg.h b/drivers/net/wireless/ath/ath9k/reg.h
index fb11a9172f38..eb2bb0db297f 100644
--- a/drivers/net/wireless/ath/ath9k/reg.h
+++ b/drivers/net/wireless/ath/ath9k/reg.h
@@ -814,6 +814,7 @@
 #define AR_SREV_REVISION_9531_10        0
 #define AR_SREV_REVISION_9531_11        1
 #define AR_SREV_REVISION_9531_20        2
+#define AR_SREV_VERSION_9561            0x600
 
 #define AR_SREV_5416(_ah) \
 	(((_ah)->hw_version.macVersion == AR_SREV_VERSION_5416_PCI) || \
@@ -974,6 +975,9 @@
 	(((_ah)->hw_version.macVersion == AR_SREV_VERSION_9531) && \
 	 ((_ah)->hw_version.macRev == AR_SREV_REVISION_9531_20))
 
+#define AR_SREV_9561(_ah) \
+	(((_ah)->hw_version.macVersion == AR_SREV_VERSION_9561))
+
 /* NOTE: When adding chips newer than Peacock, add chip check here */
 #define AR_SREV_9580_10_OR_LATER(_ah) \
 	(AR_SREV_9580(_ah))
diff --git a/drivers/net/wireless/ath/ath9k/xmit.c b/drivers/net/wireless/ath/ath9k/xmit.c
index 4caee66e5f79..1176d919569e 100644
--- a/drivers/net/wireless/ath/ath9k/xmit.c
+++ b/drivers/net/wireless/ath/ath9k/xmit.c
@@ -1097,24 +1097,65 @@ void ath_update_max_aggr_framelen(struct ath_softc *sc, int queue, int txop)
 }
 
 static u8 ath_get_rate_txpower(struct ath_softc *sc, struct ath_buf *bf,
-			       u8 rateidx)
+			       u8 rateidx, bool is_40, bool is_cck)
 {
 	u8 max_power;
+	struct sk_buff *skb;
+	struct ath_frame_info *fi;
+	struct ieee80211_tx_info *info;
 	struct ath_hw *ah = sc->sc_ah;
 
-	if (sc->tx99_state)
+	if (sc->tx99_state || !ah->tpc_enabled)
 		return MAX_RATE_POWER;
 
+	skb = bf->bf_mpdu;
+	fi = get_frame_info(skb);
+	info = IEEE80211_SKB_CB(skb);
+
 	if (!AR_SREV_9300_20_OR_LATER(ah)) {
-		/* ar9002 is not sipported for the moment */
-		return MAX_RATE_POWER;
-	}
+		int txpower = fi->tx_power;
 
-	if (!bf->bf_state.bfs_paprd) {
-		struct sk_buff *skb = bf->bf_mpdu;
-		struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
-		struct ath_frame_info *fi = get_frame_info(skb);
+		if (is_40) {
+			u8 power_ht40delta;
+			struct ar5416_eeprom_def *eep = &ah->eeprom.def;
 
+			if (AR5416_VER_MASK >= AR5416_EEP_MINOR_VER_2) {
+				bool is_2ghz;
+				struct modal_eep_header *pmodal;
+
+				is_2ghz = info->band == IEEE80211_BAND_2GHZ;
+				pmodal = &eep->modalHeader[is_2ghz];
+				power_ht40delta = pmodal->ht40PowerIncForPdadc;
+			} else {
+				power_ht40delta = 2;
+			}
+			txpower += power_ht40delta;
+		}
+
+		if (AR_SREV_9287(ah) || AR_SREV_9285(ah) ||
+		    AR_SREV_9271(ah)) {
+			txpower -= 2 * AR9287_PWR_TABLE_OFFSET_DB;
+		} else if (AR_SREV_9280_20_OR_LATER(ah)) {
+			s8 power_offset;
+
+			power_offset = ah->eep_ops->get_eeprom(ah,
+							EEP_PWR_TABLE_OFFSET);
+			txpower -= 2 * power_offset;
+		}
+
+		if (OLC_FOR_AR9280_20_LATER && is_cck)
+			txpower -= 2;
+
+		txpower = max(txpower, 0);
+		max_power = min_t(u8, ah->tx_power[rateidx], txpower);
+
+		/* XXX: clamp minimum TX power at 1 for AR9160 since if
+		 * max_power is set to 0, frames are transmitted at max
+		 * TX power
+		 */
+		if (!max_power && !AR_SREV_9280_20_OR_LATER(ah))
+			max_power = 1;
+	} else if (!bf->bf_state.bfs_paprd) {
 		if (rateidx < 8 && (info->flags & IEEE80211_TX_CTL_STBC))
 			max_power = min(ah->tx_power_stbc[rateidx],
 					fi->tx_power);
@@ -1152,7 +1193,7 @@ static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf,
 	info->rtscts_rate = fi->rtscts_rate;
 
 	for (i = 0; i < ARRAY_SIZE(bf->rates); i++) {
-		bool is_40, is_sgi, is_sp;
+		bool is_40, is_sgi, is_sp, is_cck;
 		int phy;
 
 		if (!rates[i].count || (rates[i].idx < 0))
@@ -1198,7 +1239,8 @@ static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf,
 			if (rix < 8 && (tx_info->flags & IEEE80211_TX_CTL_STBC))
 				info->rates[i].RateFlags |= ATH9K_RATESERIES_STBC;
 
-			info->txpower[i] = ath_get_rate_txpower(sc, bf, rix);
+			info->txpower[i] = ath_get_rate_txpower(sc, bf, rix,
+								is_40, false);
 			continue;
 		}
 
@@ -1227,7 +1269,9 @@ static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf,
 		info->rates[i].PktDuration = ath9k_hw_computetxtime(sc->sc_ah,
 			phy, rate->bitrate * 100, len, rix, is_sp);
 
-		info->txpower[i] = ath_get_rate_txpower(sc, bf, rix);
+		is_cck = IS_CCK_RATE(info->rates[i].Rate);
+		info->txpower[i] = ath_get_rate_txpower(sc, bf, rix, false,
+							is_cck);
 	}
 
 	/* For AR5416 - RTS cannot be followed by a frame larger than 8K */
diff --git a/drivers/net/wireless/ath/dfs_pattern_detector.c b/drivers/net/wireless/ath/dfs_pattern_detector.c
index cfd0554cf140..3d57f8772389 100644
--- a/drivers/net/wireless/ath/dfs_pattern_detector.c
+++ b/drivers/net/wireless/ath/dfs_pattern_detector.c
@@ -86,7 +86,7 @@ static const struct radar_detector_specs fcc_radar_ref_types[] = {
 	FCC_PATTERN(1, 0, 5, 150, 230, 1, 23),
 	FCC_PATTERN(2, 6, 10, 200, 500, 1, 16),
 	FCC_PATTERN(3, 11, 20, 200, 500, 1, 12),
-	FCC_PATTERN(4, 50, 100, 1000, 2000, 1, 20),
+	FCC_PATTERN(4, 50, 100, 1000, 2000, 1, 1),
 	FCC_PATTERN(5, 0, 1, 333, 333, 1, 9),
 };
 
diff --git a/drivers/net/wireless/ath/wcn36xx/main.c b/drivers/net/wireless/ath/wcn36xx/main.c
index 267c35d1f699..7dd8873f757e 100644
--- a/drivers/net/wireless/ath/wcn36xx/main.c
+++ b/drivers/net/wireless/ath/wcn36xx/main.c
@@ -1078,7 +1078,6 @@ static struct platform_driver wcn36xx_driver = {
 	.remove     = wcn36xx_remove,
 	.driver         = {
 		.name   = "wcn36xx",
-		.owner  = THIS_MODULE,
 	},
 	.id_table    = wcn36xx_platform_id_table,
 };
diff --git a/drivers/net/wireless/ath/wil6210/cfg80211.c b/drivers/net/wireless/ath/wil6210/cfg80211.c
index e72a95d1ced6..bd013fdb86dc 100644
--- a/drivers/net/wireless/ath/wil6210/cfg80211.c
+++ b/drivers/net/wireless/ath/wil6210/cfg80211.c
@@ -162,7 +162,7 @@ int wil_cid_fill_sinfo(struct wil6210_priv *wil, int cid,
 	sinfo->tx_packets = stats->tx_packets;
 	sinfo->tx_failed = stats->tx_errors;
 
-	if (test_bit(wil_status_fwconnected, &wil->status)) {
+	if (test_bit(wil_status_fwconnected, wil->status)) {
 		sinfo->filled |= BIT(NL80211_STA_INFO_SIGNAL);
 		sinfo->signal = reply.evt.sqi;
 	}
@@ -282,7 +282,7 @@ static int wil_cfg80211_scan(struct wiphy *wiphy,
 	}
 
 	/* FW don't support scan after connection attempt */
-	if (test_bit(wil_status_dontscan, &wil->status)) {
+	if (test_bit(wil_status_dontscan, wil->status)) {
 		wil_err(wil, "Can't scan now\n");
 		return -EBUSY;
 	}
@@ -362,8 +362,8 @@ static int wil_cfg80211_connect(struct wiphy *wiphy,
 	int ch;
 	int rc = 0;
 
-	if (test_bit(wil_status_fwconnecting, &wil->status) ||
-	    test_bit(wil_status_fwconnected, &wil->status))
+	if (test_bit(wil_status_fwconnecting, wil->status) ||
+	    test_bit(wil_status_fwconnected, wil->status))
 		return -EALREADY;
 
 	wil_print_connect_params(wil, sme);
@@ -450,7 +450,7 @@ static int wil_cfg80211_connect(struct wiphy *wiphy,
 	memcpy(conn.bssid, bss->bssid, ETH_ALEN);
 	memcpy(conn.dst_mac, bss->bssid, ETH_ALEN);
 
-	set_bit(wil_status_fwconnecting, &wil->status);
+	set_bit(wil_status_fwconnecting, wil->status);
 
 	rc = wmi_send(wil, WMI_CONNECT_CMDID, &conn, sizeof(conn));
 	if (rc == 0) {
@@ -458,7 +458,7 @@ static int wil_cfg80211_connect(struct wiphy *wiphy,
 		mod_timer(&wil->connect_timer,
 			  jiffies + msecs_to_jiffies(2000));
 	} else {
-		clear_bit(wil_status_fwconnecting, &wil->status);
+		clear_bit(wil_status_fwconnecting, wil->status);
 	}
 
  out:
diff --git a/drivers/net/wireless/ath/wil6210/debugfs.c b/drivers/net/wireless/ath/wil6210/debugfs.c
index 4e6e14501c2f..45c3558ec804 100644
--- a/drivers/net/wireless/ath/wil6210/debugfs.c
+++ b/drivers/net/wireless/ath/wil6210/debugfs.c
@@ -50,6 +50,7 @@ static void wil_print_vring(struct seq_file *s, struct wil6210_priv *wil,
 			    char _s, char _h)
 {
 	void __iomem *x = wmi_addr(wil, vring->hwtail);
+	u32 v;
 
 	seq_printf(s, "VRING %s = {\n", name);
 	seq_printf(s, "  pa     = %pad\n", &vring->pa);
@@ -58,10 +59,12 @@ static void wil_print_vring(struct seq_file *s, struct wil6210_priv *wil,
 	seq_printf(s, "  swtail = %d\n", vring->swtail);
 	seq_printf(s, "  swhead = %d\n", vring->swhead);
 	seq_printf(s, "  hwtail = [0x%08x] -> ", vring->hwtail);
-	if (x)
-		seq_printf(s, "0x%08x\n", ioread32(x));
-	else
+	if (x) {
+		v = ioread32(x);
+		seq_printf(s, "0x%08x = %d\n", v, v);
+	} else {
 		seq_puts(s, "???\n");
+	}
 
 	if (vring->va && (vring->size < 1025)) {
 		uint i;
@@ -101,8 +104,8 @@ static int wil_vring_debugfs_show(struct seq_file *s, void *data)
 			char name[10];
 			/* performance monitoring */
 			cycles_t now = get_cycles();
-			cycles_t idle = txdata->idle * 100;
-			cycles_t total = now - txdata->begin;
+			uint64_t idle = txdata->idle * 100;
+			uint64_t total = now - txdata->begin;
 
 			do_div(idle, total);
 			txdata->begin = now;
@@ -110,9 +113,12 @@ static int wil_vring_debugfs_show(struct seq_file *s, void *data)
 
 			snprintf(name, sizeof(name), "tx_%2d", i);
 
-			seq_printf(s, "\n%pM CID %d TID %d [%3d|%3d] idle %3d%%\n",
-				   wil->sta[cid].addr, cid, tid, used, avail,
-				   (int)idle);
+			seq_printf(s,
+				   "\n%pM CID %d TID %d BACK([%d] %d TU A%s) [%3d|%3d] idle %3d%%\n",
+				   wil->sta[cid].addr, cid, tid,
+				   txdata->agg_wsize, txdata->agg_timeout,
+				   txdata->agg_amsdu ? "+" : "-",
+				   used, avail, (int)idle);
 
 			wil_print_vring(s, wil, name, vring, '_', 'H');
 		}
@@ -384,24 +390,67 @@ static int wil6210_debugfs_create_pseudo_ISR(struct wil6210_priv *wil,
 	return 0;
 }
 
-static const struct dbg_off itr_cnt_off[] = {
+static const struct dbg_off lgc_itr_cnt_off[] = {
 	{"TRSH", S_IRUGO | S_IWUSR, HOSTADDR(RGF_DMA_ITR_CNT_TRSH), doff_io32},
 	{"DATA", S_IRUGO | S_IWUSR, HOSTADDR(RGF_DMA_ITR_CNT_DATA), doff_io32},
 	{"CTL",  S_IRUGO | S_IWUSR, HOSTADDR(RGF_DMA_ITR_CNT_CRL), doff_io32},
 	{},
 };
 
+static const struct dbg_off tx_itr_cnt_off[] = {
+	{"TRSH", S_IRUGO | S_IWUSR, HOSTADDR(RGF_DMA_ITR_TX_CNT_TRSH),
+	 doff_io32},
+	{"DATA", S_IRUGO | S_IWUSR, HOSTADDR(RGF_DMA_ITR_TX_CNT_DATA),
+	 doff_io32},
+	{"CTL",  S_IRUGO | S_IWUSR, HOSTADDR(RGF_DMA_ITR_TX_CNT_CTL),
+	 doff_io32},
+	{"IDL_TRSH", S_IRUGO | S_IWUSR, HOSTADDR(RGF_DMA_ITR_TX_IDL_CNT_TRSH),
+	 doff_io32},
+	{"IDL_DATA", S_IRUGO | S_IWUSR, HOSTADDR(RGF_DMA_ITR_TX_IDL_CNT_DATA),
+	 doff_io32},
+	{"IDL_CTL",  S_IRUGO | S_IWUSR, HOSTADDR(RGF_DMA_ITR_TX_IDL_CNT_CTL),
+	 doff_io32},
+	{},
+};
+
+static const struct dbg_off rx_itr_cnt_off[] = {
+	{"TRSH", S_IRUGO | S_IWUSR, HOSTADDR(RGF_DMA_ITR_RX_CNT_TRSH),
+	 doff_io32},
+	{"DATA", S_IRUGO | S_IWUSR, HOSTADDR(RGF_DMA_ITR_RX_CNT_DATA),
+	 doff_io32},
+	{"CTL",  S_IRUGO | S_IWUSR, HOSTADDR(RGF_DMA_ITR_RX_CNT_CTL),
+	 doff_io32},
+	{"IDL_TRSH", S_IRUGO | S_IWUSR, HOSTADDR(RGF_DMA_ITR_RX_IDL_CNT_TRSH),
+	 doff_io32},
+	{"IDL_DATA", S_IRUGO | S_IWUSR, HOSTADDR(RGF_DMA_ITR_RX_IDL_CNT_DATA),
+	 doff_io32},
+	{"IDL_CTL",  S_IRUGO | S_IWUSR, HOSTADDR(RGF_DMA_ITR_RX_IDL_CNT_CTL),
+	 doff_io32},
+	{},
+};
+
 static int wil6210_debugfs_create_ITR_CNT(struct wil6210_priv *wil,
 					  struct dentry *parent)
 {
-	struct dentry *d = debugfs_create_dir("ITR_CNT", parent);
+	struct dentry *d, *dtx, *drx;
 
+	d = debugfs_create_dir("ITR_CNT", parent);
 	if (IS_ERR_OR_NULL(d))
 		return -ENODEV;
 
+	dtx = debugfs_create_dir("TX", d);
+	drx = debugfs_create_dir("RX", d);
+	if (IS_ERR_OR_NULL(dtx) || IS_ERR_OR_NULL(drx))
+		return -ENODEV;
+
 	wil6210_debugfs_init_offset(wil, d, (void * __force)wil->csr,
-				    itr_cnt_off);
+				    lgc_itr_cnt_off);
+
+	wil6210_debugfs_init_offset(wil, dtx, (void * __force)wil->csr,
+				    tx_itr_cnt_off);
 
+	wil6210_debugfs_init_offset(wil, drx, (void * __force)wil->csr,
+				    rx_itr_cnt_off);
 	return 0;
 }
 
@@ -558,6 +607,87 @@ static const struct file_operations fops_rxon = {
 	.open  = simple_open,
 };
 
+/* block ack control, write:
+ * - "add <ringid> <agg_size> <timeout>" to trigger ADDBA
+ * - "del_tx <ringid> <reason>" to trigger DELBA for Tx side
+ * - "del_rx <CID> <TID> <reason>" to trigger DELBA for Rx side
+ */
+static ssize_t wil_write_back(struct file *file, const char __user *buf,
+			      size_t len, loff_t *ppos)
+{
+	struct wil6210_priv *wil = file->private_data;
+	int rc;
+	char *kbuf = kmalloc(len + 1, GFP_KERNEL);
+	char cmd[8];
+	int p1, p2, p3;
+
+	if (!kbuf)
+		return -ENOMEM;
+
+	rc = simple_write_to_buffer(kbuf, len, ppos, buf, len);
+	if (rc != len) {
+		kfree(kbuf);
+		return rc >= 0 ? -EIO : rc;
+	}
+
+	kbuf[len] = '\0';
+	rc = sscanf(kbuf, "%8s %d %d %d", cmd, &p1, &p2, &p3);
+	kfree(kbuf);
+
+	if (rc < 0)
+		return rc;
+	if (rc < 2)
+		return -EINVAL;
+
+	if (0 == strcmp(cmd, "add")) {
+		if (rc < 3) {
+			wil_err(wil, "BACK: add require at least 2 params\n");
+			return -EINVAL;
+		}
+		if (rc < 4)
+			p3 = 0;
+		wmi_addba(wil, p1, p2, p3);
+	} else if (0 == strcmp(cmd, "del_tx")) {
+		if (rc < 3)
+			p2 = WLAN_REASON_QSTA_LEAVE_QBSS;
+		wmi_delba_tx(wil, p1, p2);
+	} else if (0 == strcmp(cmd, "del_rx")) {
+		if (rc < 3) {
+			wil_err(wil,
+				"BACK: del_rx require at least 2 params\n");
+			return -EINVAL;
+		}
+		if (rc < 4)
+			p3 = WLAN_REASON_QSTA_LEAVE_QBSS;
+		wmi_delba_rx(wil, mk_cidxtid(p1, p2), p3);
+	} else {
+		wil_err(wil, "BACK: Unrecognized command \"%s\"\n", cmd);
+		return -EINVAL;
+	}
+
+	return len;
+}
+
+static ssize_t wil_read_back(struct file *file, char __user *user_buf,
+			     size_t count, loff_t *ppos)
+{
+	static const char text[] = "block ack control, write:\n"
+	" - \"add <ringid> <agg_size> <timeout>\" to trigger ADDBA\n"
+	"If missing, <timeout> defaults to 0\n"
+	" - \"del_tx <ringid> <reason>\" to trigger DELBA for Tx side\n"
+	" - \"del_rx <CID> <TID> <reason>\" to trigger DELBA for Rx side\n"
+	"If missing, <reason> set to \"STA_LEAVING\" (36)\n";
+
+	return simple_read_from_buffer(user_buf, count, ppos, text,
+				       sizeof(text));
+}
+
+static const struct file_operations fops_back = {
+	.read = wil_read_back,
+	.write = wil_write_back,
+	.open  = simple_open,
+};
+
 /*---tx_mgmt---*/
 /* Write mgmt frame to this file to send it */
 static ssize_t wil_write_file_txmgmt(struct file *file, const char __user *buf,
@@ -1116,7 +1246,8 @@ static void wil_print_rxtid(struct seq_file *s, struct wil_tid_ampdu_rx *r)
 	int i;
 	u16 index = ((r->head_seq_num - r->ssn) & 0xfff) % r->buf_size;
 
-	seq_printf(s, "0x%03x [", r->head_seq_num);
+	seq_printf(s, "([%2d] %3d TU) 0x%03x [", r->buf_size, r->timeout,
+		   r->head_seq_num);
 	for (i = 0; i < r->buf_size; i++) {
 		if (i == index)
 			seq_printf(s, "%c", r->reorder_buf[i] ? 'O' : '|');
@@ -1127,10 +1258,10 @@ static void wil_print_rxtid(struct seq_file *s, struct wil_tid_ampdu_rx *r)
 }
 
 static int wil_sta_debugfs_show(struct seq_file *s, void *data)
+__acquires(&p->tid_rx_lock) __releases(&p->tid_rx_lock)
 {
 	struct wil6210_priv *wil = s->private;
 	int i, tid;
-	unsigned long flags;
 
 	for (i = 0; i < ARRAY_SIZE(wil->sta); i++) {
 		struct wil_sta_info *p = &wil->sta[i];
@@ -1151,7 +1282,7 @@ static int wil_sta_debugfs_show(struct seq_file *s, void *data)
 			   (p->data_port_open ? " data_port_open" : ""));
 
 		if (p->status == wil_sta_connected) {
-			spin_lock_irqsave(&p->tid_rx_lock, flags);
+			spin_lock_bh(&p->tid_rx_lock);
 			for (tid = 0; tid < WIL_STA_TID_NUM; tid++) {
 				struct wil_tid_ampdu_rx *r = p->tid_rx[tid];
 
@@ -1160,7 +1291,7 @@ static int wil_sta_debugfs_show(struct seq_file *s, void *data)
 					wil_print_rxtid(s, r);
 				}
 			}
-			spin_unlock_irqrestore(&p->tid_rx_lock, flags);
+			spin_unlock_bh(&p->tid_rx_lock);
 		}
 	}
 
@@ -1217,6 +1348,7 @@ static const struct {
 	{"rxon",		  S_IWUSR,	&fops_rxon},
 	{"tx_mgmt",		  S_IWUSR,	&fops_txmgmt},
 	{"wmi_send",		  S_IWUSR,	&fops_wmi},
+	{"back",	S_IRUGO | S_IWUSR,	&fops_back},
 	{"temp",	S_IRUGO,		&fops_temp},
 	{"freq",	S_IRUGO,		&fops_freq},
 	{"link",	S_IRUGO,		&fops_link},
@@ -1261,7 +1393,7 @@ static void wil6210_debugfs_init_isr(struct wil6210_priv *wil,
 /* fields in struct wil6210_priv */
 static const struct dbg_off dbg_wil_off[] = {
 	WIL_FIELD(secure_pcp,	S_IRUGO | S_IWUSR,	doff_u32),
-	WIL_FIELD(status,	S_IRUGO | S_IWUSR,	doff_ulong),
+	WIL_FIELD(status[0],	S_IRUGO | S_IWUSR,	doff_ulong),
 	WIL_FIELD(fw_version,	S_IRUGO,		doff_u32),
 	WIL_FIELD(hw_version,	S_IRUGO,		doff_x32),
 	WIL_FIELD(recovery_count, S_IRUGO,		doff_u32),
diff --git a/drivers/net/wireless/ath/wil6210/ethtool.c b/drivers/net/wireless/ath/wil6210/ethtool.c
index d686638972be..4c44a82c34d7 100644
--- a/drivers/net/wireless/ath/wil6210/ethtool.c
+++ b/drivers/net/wireless/ath/wil6210/ethtool.c
@@ -45,16 +45,35 @@ static int wil_ethtoolops_get_coalesce(struct net_device *ndev,
 				       struct ethtool_coalesce *cp)
 {
 	struct wil6210_priv *wil = ndev_to_wil(ndev);
-	u32 itr_en, itr_val = 0;
+	u32 tx_itr_en, tx_itr_val = 0;
+	u32 rx_itr_en, rx_itr_val = 0;
 
 	wil_dbg_misc(wil, "%s()\n", __func__);
 
-	itr_en = ioread32(wil->csr + HOSTADDR(RGF_DMA_ITR_CNT_CRL));
-	if (itr_en & BIT_DMA_ITR_CNT_CRL_EN)
-		itr_val = ioread32(wil->csr + HOSTADDR(RGF_DMA_ITR_CNT_TRSH));
-
-	cp->rx_coalesce_usecs = itr_val;
+	if (test_bit(hw_capability_advanced_itr_moderation,
+		     wil->hw_capabilities)) {
+		tx_itr_en = ioread32(wil->csr +
+				     HOSTADDR(RGF_DMA_ITR_TX_CNT_CTL));
+		if (tx_itr_en & BIT_DMA_ITR_TX_CNT_CTL_EN)
+			tx_itr_val =
+				ioread32(wil->csr +
+					 HOSTADDR(RGF_DMA_ITR_TX_CNT_TRSH));
+
+		rx_itr_en = ioread32(wil->csr +
+				     HOSTADDR(RGF_DMA_ITR_RX_CNT_CTL));
+		if (rx_itr_en & BIT_DMA_ITR_RX_CNT_CTL_EN)
+			rx_itr_val =
+				ioread32(wil->csr +
+					 HOSTADDR(RGF_DMA_ITR_RX_CNT_TRSH));
+	} else {
+		rx_itr_en = ioread32(wil->csr + HOSTADDR(RGF_DMA_ITR_CNT_CRL));
+		if (rx_itr_en & BIT_DMA_ITR_CNT_CRL_EN)
+			rx_itr_val = ioread32(wil->csr +
+					      HOSTADDR(RGF_DMA_ITR_CNT_TRSH));
+	}
 
+	cp->tx_coalesce_usecs = tx_itr_val;
+	cp->rx_coalesce_usecs = rx_itr_val;
 	return 0;
 }
 
@@ -63,22 +82,25 @@ static int wil_ethtoolops_set_coalesce(struct net_device *ndev,
 {
 	struct wil6210_priv *wil = ndev_to_wil(ndev);
 
-	wil_dbg_misc(wil, "%s(%d usec)\n", __func__, cp->rx_coalesce_usecs);
+	wil_dbg_misc(wil, "%s(rx %d usec, tx %d usec)\n", __func__,
+		     cp->rx_coalesce_usecs, cp->tx_coalesce_usecs);
 
 	if (wil->wdev->iftype == NL80211_IFTYPE_MONITOR) {
 		wil_dbg_misc(wil, "No IRQ coalescing in monitor mode\n");
 		return -EINVAL;
 	}
 
-	/* only @rx_coalesce_usecs supported, ignore
-	 * other parameters
+	/* only @rx_coalesce_usecs and @tx_coalesce_usecs supported,
+	 * ignore other parameters
 	 */
 
-	if (cp->rx_coalesce_usecs > WIL6210_ITR_TRSH_MAX)
+	if (cp->rx_coalesce_usecs > WIL6210_ITR_TRSH_MAX ||
+	    cp->tx_coalesce_usecs > WIL6210_ITR_TRSH_MAX)
 		goto out_bad;
 
-	wil->itr_trsh = cp->rx_coalesce_usecs;
-	wil_set_itr_trsh(wil);
+	wil->tx_max_burst_duration = cp->tx_coalesce_usecs;
+	wil->rx_max_burst_duration = cp->rx_coalesce_usecs;
+	wil_configure_interrupt_moderation(wil);
 
 	return 0;
 
diff --git a/drivers/net/wireless/ath/wil6210/interrupt.c b/drivers/net/wireless/ath/wil6210/interrupt.c
index 4bcbd6297b3e..a6f923086f31 100644
--- a/drivers/net/wireless/ath/wil6210/interrupt.c
+++ b/drivers/net/wireless/ath/wil6210/interrupt.c
@@ -102,7 +102,7 @@ static void wil6210_mask_irq_pseudo(struct wil6210_priv *wil)
 	iowrite32(WIL6210_IRQ_DISABLE, wil->csr +
 		  HOSTADDR(RGF_DMA_PSEUDO_CAUSE_MASK_SW));
 
-	clear_bit(wil_status_irqen, &wil->status);
+	clear_bit(wil_status_irqen, wil->status);
 }
 
 void wil6210_unmask_irq_tx(struct wil6210_priv *wil)
@@ -130,7 +130,7 @@ static void wil6210_unmask_irq_pseudo(struct wil6210_priv *wil)
 {
 	wil_dbg_irq(wil, "%s()\n", __func__);
 
-	set_bit(wil_status_irqen, &wil->status);
+	set_bit(wil_status_irqen, wil->status);
 
 	iowrite32(WIL6210_IRQ_PSEUDO_MASK, wil->csr +
 		  HOSTADDR(RGF_DMA_PSEUDO_CAUSE_MASK_SW));
@@ -157,15 +157,91 @@ void wil_unmask_irq(struct wil6210_priv *wil)
 	iowrite32(WIL_ICR_ICC_VALUE, wil->csr + HOSTADDR(RGF_DMA_EP_MISC_ICR) +
 		  offsetof(struct RGF_ICR, ICC));
 
-	/* interrupt moderation parameters */
-	wil_set_itr_trsh(wil);
-
 	wil6210_unmask_irq_pseudo(wil);
 	wil6210_unmask_irq_tx(wil);
 	wil6210_unmask_irq_rx(wil);
 	wil6210_unmask_irq_misc(wil);
 }
 
+/* target write operation */
+#define W(a, v) do { iowrite32(v, wil->csr + HOSTADDR(a)); wmb(); } while (0)
+
+static
+void wil_configure_interrupt_moderation_new(struct wil6210_priv *wil)
+{
+	/* Disable and clear tx counter before (re)configuration */
+	W(RGF_DMA_ITR_TX_CNT_CTL, BIT_DMA_ITR_TX_CNT_CTL_CLR);
+	W(RGF_DMA_ITR_TX_CNT_TRSH, wil->tx_max_burst_duration);
+	wil_info(wil, "set ITR_TX_CNT_TRSH = %d usec\n",
+		 wil->tx_max_burst_duration);
+	/* Configure TX max burst duration timer to use usec units */
+	W(RGF_DMA_ITR_TX_CNT_CTL,
+	  BIT_DMA_ITR_TX_CNT_CTL_EN | BIT_DMA_ITR_TX_CNT_CTL_EXT_TIC_SEL);
+
+	/* Disable and clear tx idle counter before (re)configuration */
+	W(RGF_DMA_ITR_TX_IDL_CNT_CTL, BIT_DMA_ITR_TX_IDL_CNT_CTL_CLR);
+	W(RGF_DMA_ITR_TX_IDL_CNT_TRSH, wil->tx_interframe_timeout);
+	wil_info(wil, "set ITR_TX_IDL_CNT_TRSH = %d usec\n",
+		 wil->tx_interframe_timeout);
+	/* Configure TX max burst duration timer to use usec units */
+	W(RGF_DMA_ITR_TX_IDL_CNT_CTL, BIT_DMA_ITR_TX_IDL_CNT_CTL_EN |
+				      BIT_DMA_ITR_TX_IDL_CNT_CTL_EXT_TIC_SEL);
+
+	/* Disable and clear rx counter before (re)configuration */
+	W(RGF_DMA_ITR_RX_CNT_CTL, BIT_DMA_ITR_RX_CNT_CTL_CLR);
+	W(RGF_DMA_ITR_RX_CNT_TRSH, wil->rx_max_burst_duration);
+	wil_info(wil, "set ITR_RX_CNT_TRSH = %d usec\n",
+		 wil->rx_max_burst_duration);
+	/* Configure TX max burst duration timer to use usec units */
+	W(RGF_DMA_ITR_RX_CNT_CTL,
+	  BIT_DMA_ITR_RX_CNT_CTL_EN | BIT_DMA_ITR_RX_CNT_CTL_EXT_TIC_SEL);
+
+	/* Disable and clear rx idle counter before (re)configuration */
+	W(RGF_DMA_ITR_RX_IDL_CNT_CTL, BIT_DMA_ITR_RX_IDL_CNT_CTL_CLR);
+	W(RGF_DMA_ITR_RX_IDL_CNT_TRSH, wil->rx_interframe_timeout);
+	wil_info(wil, "set ITR_RX_IDL_CNT_TRSH = %d usec\n",
+		 wil->rx_interframe_timeout);
+	/* Configure TX max burst duration timer to use usec units */
+	W(RGF_DMA_ITR_RX_IDL_CNT_CTL, BIT_DMA_ITR_RX_IDL_CNT_CTL_EN |
+				      BIT_DMA_ITR_RX_IDL_CNT_CTL_EXT_TIC_SEL);
+}
+
+static
+void wil_configure_interrupt_moderation_lgc(struct wil6210_priv *wil)
+{
+	/* disable, use usec resolution */
+	W(RGF_DMA_ITR_CNT_CRL, BIT_DMA_ITR_CNT_CRL_CLR);
+
+	wil_info(wil, "set ITR_TRSH = %d usec\n", wil->rx_max_burst_duration);
+	W(RGF_DMA_ITR_CNT_TRSH, wil->rx_max_burst_duration);
+	/* start it */
+	W(RGF_DMA_ITR_CNT_CRL,
+	  BIT_DMA_ITR_CNT_CRL_EN | BIT_DMA_ITR_CNT_CRL_EXT_TICK);
+}
+
+#undef W
+
+void wil_configure_interrupt_moderation(struct wil6210_priv *wil)
+{
+	wil_dbg_irq(wil, "%s()\n", __func__);
+
+	/* disable interrupt moderation for monitor
+	 * to get better timestamp precision
+	 */
+	if (wil->wdev->iftype == NL80211_IFTYPE_MONITOR)
+		return;
+
+	if (test_bit(hw_capability_advanced_itr_moderation,
+		     wil->hw_capabilities))
+		wil_configure_interrupt_moderation_new(wil);
+	else {
+		/* Advanced interrupt moderation is not available before
+		 * Sparrow v2. Will use legacy interrupt moderation
+		 */
+		wil_configure_interrupt_moderation_lgc(wil);
+	}
+}
+
 static irqreturn_t wil6210_irq_rx(int irq, void *cookie)
 {
 	struct wil6210_priv *wil = cookie;
@@ -194,18 +270,19 @@ static irqreturn_t wil6210_irq_rx(int irq, void *cookie)
 		wil_dbg_irq(wil, "RX done\n");
 
 		if (isr & BIT_DMA_EP_RX_ICR_RX_HTRSH)
-			wil_err_ratelimited(wil, "Received \"Rx buffer is in risk "
-				"of overflow\" interrupt\n");
+			wil_err_ratelimited(wil,
+					    "Received \"Rx buffer is in risk of overflow\" interrupt\n");
 
-		isr &= ~(BIT_DMA_EP_RX_ICR_RX_DONE | BIT_DMA_EP_RX_ICR_RX_HTRSH);
-		if (test_bit(wil_status_reset_done, &wil->status)) {
-			if (test_bit(wil_status_napi_en, &wil->status)) {
+		isr &= ~(BIT_DMA_EP_RX_ICR_RX_DONE |
+			 BIT_DMA_EP_RX_ICR_RX_HTRSH);
+		if (test_bit(wil_status_reset_done, wil->status)) {
+			if (test_bit(wil_status_napi_en, wil->status)) {
 				wil_dbg_txrx(wil, "NAPI(Rx) schedule\n");
 				need_unmask = false;
 				napi_schedule(&wil->napi_rx);
 			} else {
-				wil_err(wil, "Got Rx interrupt while "
-					"stopping interface\n");
+				wil_err(wil,
+					"Got Rx interrupt while stopping interface\n");
 			}
 		} else {
 			wil_err(wil, "Got Rx interrupt while in reset\n");
@@ -248,7 +325,7 @@ static irqreturn_t wil6210_irq_tx(int irq, void *cookie)
 		isr &= ~BIT_DMA_EP_TX_ICR_TX_DONE;
 		/* clear also all VRING interrupts */
 		isr &= ~(BIT(25) - 1UL);
-		if (test_bit(wil_status_reset_done, &wil->status)) {
+		if (test_bit(wil_status_reset_done, wil->status)) {
 			wil_dbg_txrx(wil, "NAPI(Tx) schedule\n");
 			need_unmask = false;
 			napi_schedule(&wil->napi_tx);
@@ -310,7 +387,7 @@ static irqreturn_t wil6210_irq_misc(int irq, void *cookie)
 
 	if (isr & ISR_MISC_FW_ERROR) {
 		wil_err(wil, "Firmware error detected\n");
-		clear_bit(wil_status_fwready, &wil->status);
+		clear_bit(wil_status_fwready, wil->status);
 		/*
 		 * do not clear @isr here - we do 2-nd part in thread
 		 * there, user space get notified, and it should be done
@@ -321,7 +398,7 @@ static irqreturn_t wil6210_irq_misc(int irq, void *cookie)
 	if (isr & ISR_MISC_FW_READY) {
 		wil_dbg_irq(wil, "IRQ: FW ready\n");
 		wil_cache_mbox_regs(wil);
-		set_bit(wil_status_reset_done, &wil->status);
+		set_bit(wil_status_reset_done, wil->status);
 		/**
 		 * Actual FW ready indicated by the
 		 * WMI_FW_READY_EVENTID
@@ -394,7 +471,7 @@ static irqreturn_t wil6210_thread_irq(int irq, void *cookie)
  */
 static int wil6210_debug_irq_mask(struct wil6210_priv *wil, u32 pseudo_cause)
 {
-	if (!test_bit(wil_status_irqen, &wil->status)) {
+	if (!test_bit(wil_status_irqen, wil->status)) {
 		u32 icm_rx = wil_ioread32_and_clear(wil->csr +
 				HOSTADDR(RGF_DMA_EP_RX_ICR) +
 				offsetof(struct RGF_ICR, ICM));
diff --git a/drivers/net/wireless/ath/wil6210/main.c b/drivers/net/wireless/ath/wil6210/main.c
index 8ff3fe34fe05..62dc24189bd3 100644
--- a/drivers/net/wireless/ath/wil6210/main.c
+++ b/drivers/net/wireless/ath/wil6210/main.c
@@ -33,15 +33,46 @@ static bool no_fw_load = true;
 module_param(no_fw_load, bool, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(no_fw_load, " do not download FW, use one in on-card flash.");
 
-static unsigned int itr_trsh = WIL6210_ITR_TRSH_DEFAULT;
+static unsigned int tx_interframe_timeout =
+		WIL6210_ITR_TX_INTERFRAME_TIMEOUT_DEFAULT;
 
-module_param(itr_trsh, uint, S_IRUGO);
-MODULE_PARM_DESC(itr_trsh, " Interrupt moderation threshold, usecs.");
+module_param(tx_interframe_timeout, uint, S_IRUGO);
+MODULE_PARM_DESC(tx_interframe_timeout,
+		 " Interrupt moderation TX interframe timeout, usecs.");
+
+static unsigned int rx_interframe_timeout =
+		WIL6210_ITR_RX_INTERFRAME_TIMEOUT_DEFAULT;
+
+module_param(rx_interframe_timeout, uint, S_IRUGO);
+MODULE_PARM_DESC(rx_interframe_timeout,
+		 " Interrupt moderation RX interframe timeout, usecs.");
+
+static unsigned int tx_max_burst_duration =
+		WIL6210_ITR_TX_MAX_BURST_DURATION_DEFAULT;
+
+module_param(tx_max_burst_duration, uint, S_IRUGO);
+MODULE_PARM_DESC(tx_max_burst_duration,
+		 " Interrupt moderation TX max burst duration, usecs.");
+
+static unsigned int rx_max_burst_duration =
+		WIL6210_ITR_RX_MAX_BURST_DURATION_DEFAULT;
+
+module_param(rx_max_burst_duration, uint, S_IRUGO);
+MODULE_PARM_DESC(rx_max_burst_duration,
+		 " Interrupt moderation RX max burst duration, usecs.");
+
+/* if not set via modparam, will be set to default value of 1/8 of
+ * rx ring size during init flow
+ */
+unsigned short rx_ring_overflow_thrsh = WIL6210_RX_HIGH_TRSH_INIT;
+module_param(rx_ring_overflow_thrsh, ushort, S_IRUGO);
+MODULE_PARM_DESC(rx_ring_overflow_thrsh,
+		 " RX ring overflow threshold in descriptors.");
 
 /* We allow allocation of more than 1 page buffers to support large packets.
  * It is suboptimal behavior performance wise in case MTU above page size.
  */
-unsigned int mtu_max = TXRX_BUF_LEN_DEFAULT - ETH_HLEN;
+unsigned int mtu_max = TXRX_BUF_LEN_DEFAULT - WIL_MAX_MPDU_OVERHEAD;
 static int mtu_max_set(const char *val, const struct kernel_param *kp)
 {
 	int ret;
@@ -53,7 +84,7 @@ static int mtu_max_set(const char *val, const struct kernel_param *kp)
 	if (ret)
 		return ret;
 
-	if (mtu_max < 68 || mtu_max > IEEE80211_MAX_DATA_LEN_DMG)
+	if (mtu_max < 68 || mtu_max > WIL_MAX_ETH_MTU)
 		ret = -EINVAL;
 
 	return ret;
@@ -135,12 +166,14 @@ void wil_memcpy_toio_32(volatile void __iomem *dst, const void *src,
 
 static void wil_disconnect_cid(struct wil6210_priv *wil, int cid,
 			       u16 reason_code, bool from_event)
+__acquires(&sta->tid_rx_lock) __releases(&sta->tid_rx_lock)
 {
 	uint i;
 	struct net_device *ndev = wil_to_ndev(wil);
 	struct wireless_dev *wdev = wil->wdev;
 	struct wil_sta_info *sta = &wil->sta[cid];
 
+	might_sleep();
 	wil_dbg_misc(wil, "%s(CID %d, status %d)\n", __func__, cid,
 		     sta->status);
 
@@ -163,15 +196,14 @@ static void wil_disconnect_cid(struct wil6210_priv *wil, int cid,
 
 	for (i = 0; i < WIL_STA_TID_NUM; i++) {
 		struct wil_tid_ampdu_rx *r;
-		unsigned long flags;
 
-		spin_lock_irqsave(&sta->tid_rx_lock, flags);
+		spin_lock_bh(&sta->tid_rx_lock);
 
 		r = sta->tid_rx[i];
 		sta->tid_rx[i] = NULL;
 		wil_tid_ampdu_rx_free(wil, r);
 
-		spin_unlock_irqrestore(&sta->tid_rx_lock, flags);
+		spin_unlock_bh(&sta->tid_rx_lock);
 	}
 	for (i = 0; i < ARRAY_SIZE(wil->vring_tx); i++) {
 		if (wil->vring2cid_tid[i][0] == cid)
@@ -188,34 +220,45 @@ static void _wil6210_disconnect(struct wil6210_priv *wil, const u8 *bssid,
 	struct wireless_dev *wdev = wil->wdev;
 
 	might_sleep();
-	if (bssid) {
+	wil_dbg_misc(wil, "%s(bssid=%pM, reason=%d, ev%s)\n", __func__, bssid,
+		     reason_code, from_event ? "+" : "-");
+
+	/* Cases are:
+	 * - disconnect single STA, still connected
+	 * - disconnect single STA, already disconnected
+	 * - disconnect all
+	 *
+	 * For "disconnect all", there are 2 options:
+	 * - bssid == NULL
+	 * - bssid is our MAC address
+	 */
+	if (bssid && memcmp(ndev->dev_addr, bssid, ETH_ALEN)) {
 		cid = wil_find_cid(wil, bssid);
-		wil_dbg_misc(wil, "%s(%pM, CID %d)\n", __func__, bssid, cid);
-	} else {
-		wil_dbg_misc(wil, "%s(all)\n", __func__);
-	}
-
-	if (cid >= 0) /* disconnect 1 peer */
-		wil_disconnect_cid(wil, cid, reason_code, from_event);
-	else /* disconnect all */
+		wil_dbg_misc(wil, "Disconnect %pM, CID=%d, reason=%d\n",
+			     bssid, cid, reason_code);
+		if (cid >= 0) /* disconnect 1 peer */
+			wil_disconnect_cid(wil, cid, reason_code, from_event);
+	} else { /* all */
+		wil_dbg_misc(wil, "Disconnect all\n");
 		for (cid = 0; cid < WIL6210_MAX_CID; cid++)
 			wil_disconnect_cid(wil, cid, reason_code, from_event);
+	}
 
 	/* link state */
 	switch (wdev->iftype) {
 	case NL80211_IFTYPE_STATION:
 	case NL80211_IFTYPE_P2P_CLIENT:
 		wil_link_off(wil);
-		if (test_bit(wil_status_fwconnected, &wil->status)) {
-			clear_bit(wil_status_fwconnected, &wil->status);
+		if (test_bit(wil_status_fwconnected, wil->status)) {
+			clear_bit(wil_status_fwconnected, wil->status);
 			cfg80211_disconnected(ndev, reason_code,
 					      NULL, 0, GFP_KERNEL);
-		} else if (test_bit(wil_status_fwconnecting, &wil->status)) {
+		} else if (test_bit(wil_status_fwconnecting, wil->status)) {
 			cfg80211_connect_result(ndev, bssid, NULL, 0, NULL, 0,
 						WLAN_STATUS_UNSPECIFIED_FAILURE,
 						GFP_KERNEL);
 		}
-		clear_bit(wil_status_fwconnecting, &wil->status);
+		clear_bit(wil_status_fwconnecting, wil->status);
 		break;
 	default:
 		break;
@@ -248,7 +291,7 @@ static void wil_scan_timer_fn(ulong x)
 {
 	struct wil6210_priv *wil = (void *)x;
 
-	clear_bit(wil_status_fwready, &wil->status);
+	clear_bit(wil_status_fwready, wil->status);
 	wil_err(wil, "Scan timeout detected, start fw error recovery\n");
 	wil->recovery_state = fw_recovery_pending;
 	schedule_work(&wil->fw_error_worker);
@@ -384,6 +427,8 @@ int wil_priv_init(struct wil6210_priv *wil)
 
 	mutex_init(&wil->mutex);
 	mutex_init(&wil->wmi_mutex);
+	mutex_init(&wil->back_rx_mutex);
+	mutex_init(&wil->back_tx_mutex);
 
 	init_completion(&wil->wmi_ready);
 	init_completion(&wil->wmi_call);
@@ -396,25 +441,37 @@ int wil_priv_init(struct wil6210_priv *wil)
 	INIT_WORK(&wil->disconnect_worker, wil_disconnect_worker);
 	INIT_WORK(&wil->wmi_event_worker, wmi_event_worker);
 	INIT_WORK(&wil->fw_error_worker, wil_fw_error_worker);
+	INIT_WORK(&wil->back_rx_worker, wil_back_rx_worker);
+	INIT_WORK(&wil->back_tx_worker, wil_back_tx_worker);
 
 	INIT_LIST_HEAD(&wil->pending_wmi_ev);
+	INIT_LIST_HEAD(&wil->back_rx_pending);
+	INIT_LIST_HEAD(&wil->back_tx_pending);
 	spin_lock_init(&wil->wmi_ev_lock);
 	init_waitqueue_head(&wil->wq);
 
-	wil->wmi_wq = create_singlethread_workqueue(WIL_NAME"_wmi");
+	wil->wmi_wq = create_singlethread_workqueue(WIL_NAME "_wmi");
 	if (!wil->wmi_wq)
 		return -EAGAIN;
 
-	wil->wmi_wq_conn = create_singlethread_workqueue(WIL_NAME"_connect");
-	if (!wil->wmi_wq_conn) {
-		destroy_workqueue(wil->wmi_wq);
-		return -EAGAIN;
-	}
+	wil->wq_service = create_singlethread_workqueue(WIL_NAME "_service");
+	if (!wil->wq_service)
+		goto out_wmi_wq;
 
 	wil->last_fw_recovery = jiffies;
-	wil->itr_trsh = itr_trsh;
+	wil->tx_interframe_timeout = tx_interframe_timeout;
+	wil->rx_interframe_timeout = rx_interframe_timeout;
+	wil->tx_max_burst_duration = tx_max_burst_duration;
+	wil->rx_max_burst_duration = rx_max_burst_duration;
 
+	if (rx_ring_overflow_thrsh == WIL6210_RX_HIGH_TRSH_INIT)
+		rx_ring_overflow_thrsh = WIL6210_RX_HIGH_TRSH_DEFAULT;
 	return 0;
+
+out_wmi_wq:
+	destroy_workqueue(wil->wmi_wq);
+
+	return -EAGAIN;
 }
 
 /**
@@ -448,7 +505,11 @@ void wil_priv_deinit(struct wil6210_priv *wil)
 	wil6210_disconnect(wil, NULL, WLAN_REASON_DEAUTH_LEAVING, false);
 	mutex_unlock(&wil->mutex);
 	wmi_event_flush(wil);
-	destroy_workqueue(wil->wmi_wq_conn);
+	wil_back_rx_flush(wil);
+	cancel_work_sync(&wil->back_rx_worker);
+	wil_back_tx_flush(wil);
+	cancel_work_sync(&wil->back_tx_worker);
+	destroy_workqueue(wil->wq_service);
 	destroy_workqueue(wil->wmi_wq);
 }
 
@@ -478,13 +539,10 @@ static int wil_target_reset(struct wil6210_priv *wil)
 {
 	int delay = 0;
 	u32 x;
-	u32 rev_id;
-	bool is_sparrow = (wil->board->board == WIL_BOARD_SPARROW);
-
-	wil_dbg_misc(wil, "Resetting \"%s\"...\n", wil->board->name);
+	bool is_reset_v2 = test_bit(hw_capability_reset_v2,
+				    wil->hw_capabilities);
 
-	wil->hw_version = R(RGF_USER_FW_REV_ID);
-	rev_id = wil->hw_version & 0xff;
+	wil_dbg_misc(wil, "Resetting \"%s\"...\n", wil->hw_name);
 
 	/* Clear MAC link up */
 	S(RGF_HP_CTRL, BIT(15));
@@ -496,7 +554,7 @@ static int wil_target_reset(struct wil6210_priv *wil)
 	/* Clear Fw Download notification */
 	C(RGF_USER_USAGE_6, BIT(0));
 
-	if (is_sparrow) {
+	if (is_reset_v2) {
 		S(RGF_CAF_OSC_CONTROL, BIT_CAF_OSC_XTAL_EN);
 		/* XTAL stabilization should take about 3ms */
 		usleep_range(5000, 7000);
@@ -517,10 +575,11 @@ static int wil_target_reset(struct wil6210_priv *wil)
 
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_2, 0xFE000000);
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_1, 0x0000003F);
-	W(RGF_USER_CLKS_CTL_SW_RST_VEC_3, is_sparrow ? 0x000000f0 : 0x00000170);
+	W(RGF_USER_CLKS_CTL_SW_RST_VEC_3,
+	  is_reset_v2 ? 0x000000f0 : 0x00000170);
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_0, 0xFFE7FE00);
 
-	if (is_sparrow) {
+	if (is_reset_v2) {
 		W(RGF_USER_CLKS_CTL_EXT_SW_RST_VEC_0, 0x0);
 		W(RGF_USER_CLKS_CTL_EXT_SW_RST_VEC_1, 0x0);
 	}
@@ -530,19 +589,14 @@ static int wil_target_reset(struct wil6210_priv *wil)
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_1, 0);
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_0, 0);
 
-	if (is_sparrow) {
+	if (is_reset_v2) {
 		W(RGF_USER_CLKS_CTL_SW_RST_VEC_3, 0x00000003);
 		/* reset A2 PCIE AHB */
 		W(RGF_USER_CLKS_CTL_SW_RST_VEC_2, 0x00008000);
 	} else {
 		W(RGF_USER_CLKS_CTL_SW_RST_VEC_3, 0x00000001);
-		if (rev_id == 1) {
-			/* reset A1 BOTH PCIE AHB & PCIE RGF */
-			W(RGF_USER_CLKS_CTL_SW_RST_VEC_2, 0x00000080);
-		} else {
-			W(RGF_PCIE_LOS_COUNTER_CTL, BIT(6) | BIT(8));
-			W(RGF_USER_CLKS_CTL_SW_RST_VEC_2, 0x00008000);
-		}
+		W(RGF_PCIE_LOS_COUNTER_CTL, BIT(6) | BIT(8));
+		W(RGF_USER_CLKS_CTL_SW_RST_VEC_2, 0x00008000);
 	}
 
 	/* TODO: check order here!!! Erez code is different */
@@ -559,8 +613,7 @@ static int wil_target_reset(struct wil6210_priv *wil)
 		}
 	} while (x != HW_MACHINE_BOOT_DONE);
 
-	/* TODO: Erez check rev_id != 1 */
-	if (!is_sparrow && (rev_id != 1))
+	if (!is_reset_v2)
 		W(RGF_PCIE_LOS_COUNTER_CTL, BIT(8));
 
 	C(RGF_USER_CLKS_CTL_0, BIT_USER_CLKS_RST_PWGD);
@@ -569,26 +622,6 @@ static int wil_target_reset(struct wil6210_priv *wil)
 	return 0;
 }
 
-/**
- * wil_set_itr_trsh: - apply interrupt coalescing params
- */
-void wil_set_itr_trsh(struct wil6210_priv *wil)
-{
-	/* disable, use usec resolution */
-	W(RGF_DMA_ITR_CNT_CRL, BIT_DMA_ITR_CNT_CRL_EXT_TICK);
-
-	/* disable interrupt moderation for monitor
-	 * to get better timestamp precision
-	 */
-	if (wil->wdev->iftype == NL80211_IFTYPE_MONITOR)
-		return;
-
-	wil_info(wil, "set ITR_TRSH = %d usec\n", wil->itr_trsh);
-	W(RGF_DMA_ITR_CNT_TRSH, wil->itr_trsh);
-	W(RGF_DMA_ITR_CNT_CRL, BIT_DMA_ITR_CNT_CRL_EN |
-	  BIT_DMA_ITR_CNT_CRL_EXT_TICK); /* start it */
-}
-
 #undef R
 #undef W
 #undef S
@@ -629,13 +662,17 @@ int wil_reset(struct wil6210_priv *wil)
 
 	wil_dbg_misc(wil, "%s()\n", __func__);
 
+	if (wil->hw_version == HW_VER_UNKNOWN)
+		return -ENODEV;
+
 	WARN_ON(!mutex_is_locked(&wil->mutex));
-	WARN_ON(test_bit(wil_status_napi_en, &wil->status));
+	WARN_ON(test_bit(wil_status_napi_en, wil->status));
 
 	cancel_work_sync(&wil->disconnect_worker);
 	wil6210_disconnect(wil, NULL, WLAN_REASON_DEAUTH_LEAVING, false);
 
-	wil->status = 0; /* prevent NAPI from being scheduled */
+	/* prevent NAPI from being scheduled */
+	bitmap_zero(wil->status, wil_status_last);
 
 	if (wil->scan_request) {
 		wil_dbg_misc(wil, "Abort scan_request 0x%p\n",
@@ -649,7 +686,7 @@ int wil_reset(struct wil6210_priv *wil)
 
 	wmi_event_flush(wil);
 
-	flush_workqueue(wil->wmi_wq_conn);
+	flush_workqueue(wil->wq_service);
 	flush_workqueue(wil->wmi_wq);
 
 	rc = wil_target_reset(wil);
@@ -688,6 +725,7 @@ int wil_reset(struct wil6210_priv *wil)
 	reinit_completion(&wil->wmi_ready);
 	reinit_completion(&wil->wmi_call);
 
+	wil_configure_interrupt_moderation(wil);
 	wil_unmask_irq(wil);
 
 	/* we just started MAC, wait for FW ready */
@@ -774,7 +812,7 @@ int __wil_up(struct wil6210_priv *wil)
 	wil_dbg_misc(wil, "NAPI enable\n");
 	napi_enable(&wil->napi_rx);
 	napi_enable(&wil->napi_tx);
-	set_bit(wil_status_napi_en, &wil->status);
+	set_bit(wil_status_napi_en, wil->status);
 
 	if (wil->platform_ops.bus_request)
 		wil->platform_ops.bus_request(wil->platform_handle,
@@ -807,7 +845,7 @@ int __wil_down(struct wil6210_priv *wil)
 		wil->platform_ops.bus_request(wil->platform_handle, 0);
 
 	wil_disable_irq(wil);
-	if (test_and_clear_bit(wil_status_napi_en, &wil->status)) {
+	if (test_and_clear_bit(wil_status_napi_en, wil->status)) {
 		napi_disable(&wil->napi_rx);
 		napi_disable(&wil->napi_tx);
 		wil_dbg_misc(wil, "NAPI disable\n");
@@ -822,15 +860,15 @@ int __wil_down(struct wil6210_priv *wil)
 		wil->scan_request = NULL;
 	}
 
-	if (test_bit(wil_status_fwconnected, &wil->status) ||
-	    test_bit(wil_status_fwconnecting, &wil->status))
+	if (test_bit(wil_status_fwconnected, wil->status) ||
+	    test_bit(wil_status_fwconnecting, wil->status))
 		wmi_send(wil, WMI_DISCONNECT_CMDID, NULL, 0);
 
 	/* make sure wil is idle (not connected) */
 	mutex_unlock(&wil->mutex);
 	while (iter--) {
-		int idle = !test_bit(wil_status_fwconnected, &wil->status) &&
-			   !test_bit(wil_status_fwconnecting, &wil->status);
+		int idle = !test_bit(wil_status_fwconnected, wil->status) &&
+			   !test_bit(wil_status_fwconnecting, wil->status);
 		if (idle)
 			break;
 		msleep(WAIT_FOR_DISCONNECT_INTERVAL_MS);
diff --git a/drivers/net/wireless/ath/wil6210/pcie_bus.c b/drivers/net/wireless/ath/wil6210/pcie_bus.c
index 66626a8ee728..3dd26709ccb2 100644
--- a/drivers/net/wireless/ath/wil6210/pcie_bus.c
+++ b/drivers/net/wireless/ath/wil6210/pcie_bus.c
@@ -31,6 +31,46 @@ static bool debug_fw; /* = false; */
 module_param(debug_fw, bool, S_IRUGO);
 MODULE_PARM_DESC(debug_fw, " load driver if FW not ready. For FW debug");
 
+static
+void wil_set_capabilities(struct wil6210_priv *wil)
+{
+	u32 rev_id = ioread32(wil->csr + HOSTADDR(RGF_USER_JTAG_DEV_ID));
+
+	bitmap_zero(wil->hw_capabilities, hw_capability_last);
+
+	switch (rev_id) {
+	case JTAG_DEV_ID_MARLON_B0:
+		wil->hw_name = "Marlon B0";
+		wil->hw_version = HW_VER_MARLON_B0;
+		break;
+	case JTAG_DEV_ID_SPARROW_A0:
+		wil->hw_name = "Sparrow A0";
+		wil->hw_version = HW_VER_SPARROW_A0;
+		break;
+	case JTAG_DEV_ID_SPARROW_A1:
+		wil->hw_name = "Sparrow A1";
+		wil->hw_version = HW_VER_SPARROW_A1;
+		break;
+	case JTAG_DEV_ID_SPARROW_B0:
+		wil->hw_name = "Sparrow B0";
+		wil->hw_version = HW_VER_SPARROW_B0;
+		break;
+	default:
+		wil_err(wil, "Unknown board hardware 0x%08x\n", rev_id);
+		wil->hw_name = "Unknown";
+		wil->hw_version = HW_VER_UNKNOWN;
+	}
+
+	wil_info(wil, "Board hardware is %s\n", wil->hw_name);
+
+	if (wil->hw_version >= HW_VER_SPARROW_A0)
+		set_bit(hw_capability_reset_v2, wil->hw_capabilities);
+
+	if (wil->hw_version >= HW_VER_SPARROW_B0)
+		set_bit(hw_capability_advanced_itr_moderation,
+			wil->hw_capabilities);
+}
+
 void wil_disable_irq(struct wil6210_priv *wil)
 {
 	int irq = wil->pdev->irq;
@@ -149,12 +189,11 @@ static int wil_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	struct wil6210_priv *wil;
 	struct device *dev = &pdev->dev;
 	void __iomem *csr;
-	struct wil_board *board = (struct wil_board *)id->driver_data;
 	int rc;
 
 	/* check HW */
 	dev_info(&pdev->dev, WIL_NAME
-		 " \"%s\" device found [%04x:%04x] (rev %x)\n", board->name,
+		 " device found [%04x:%04x] (rev %x)\n",
 		 (int)pdev->vendor, (int)pdev->device, (int)pdev->revision);
 
 	if (pci_resource_len(pdev, 0) != WIL6210_MEM_SIZE) {
@@ -204,8 +243,7 @@ static int wil_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	pci_set_drvdata(pdev, wil);
 	wil->pdev = pdev;
-	wil->board = board;
-
+	wil_set_capabilities(wil);
 	wil6210_clear_irq(wil);
 
 	wil->platform_handle =
@@ -266,23 +304,10 @@ static void wil_pcie_remove(struct pci_dev *pdev)
 	pci_disable_device(pdev);
 }
 
-static const struct wil_board wil_board_marlon = {
-	.board = WIL_BOARD_MARLON,
-	.name = "marlon",
-};
-
-static const struct wil_board wil_board_sparrow = {
-	.board = WIL_BOARD_SPARROW,
-	.name = "sparrow",
-};
-
 static const struct pci_device_id wil6210_pcie_ids[] = {
-	{ PCI_DEVICE(0x1ae9, 0x0301),
-	  .driver_data = (kernel_ulong_t)&wil_board_marlon },
-	{ PCI_DEVICE(0x1ae9, 0x0310),
-	  .driver_data = (kernel_ulong_t)&wil_board_sparrow },
-	{ PCI_DEVICE(0x1ae9, 0x0302), /* same as above, firmware broken */
-	  .driver_data = (kernel_ulong_t)&wil_board_sparrow },
+	{ PCI_DEVICE(0x1ae9, 0x0301) },
+	{ PCI_DEVICE(0x1ae9, 0x0310) },
+	{ PCI_DEVICE(0x1ae9, 0x0302) }, /* same as above, firmware broken */
 	{ /* end: all zeroes */	},
 };
 MODULE_DEVICE_TABLE(pci, wil6210_pcie_ids);
diff --git a/drivers/net/wireless/ath/wil6210/rx_reorder.c b/drivers/net/wireless/ath/wil6210/rx_reorder.c
index 489cb73d139b..552209227de9 100644
--- a/drivers/net/wireless/ath/wil6210/rx_reorder.c
+++ b/drivers/net/wireless/ath/wil6210/rx_reorder.c
@@ -89,7 +89,9 @@ static void wil_reorder_release(struct wil6210_priv *wil,
 	}
 }
 
+/* called in NAPI context */
 void wil_rx_reorder(struct wil6210_priv *wil, struct sk_buff *skb)
+__acquires(&sta->tid_rx_lock) __releases(&sta->tid_rx_lock)
 {
 	struct net_device *ndev = wil_to_ndev(wil);
 	struct vring_rx_desc *d = wil_skb_rxdesc(skb);
@@ -97,22 +99,26 @@ void wil_rx_reorder(struct wil6210_priv *wil, struct sk_buff *skb)
 	int cid = wil_rxdesc_cid(d);
 	int mid = wil_rxdesc_mid(d);
 	u16 seq = wil_rxdesc_seq(d);
+	int mcast = wil_rxdesc_mcast(d);
 	struct wil_sta_info *sta = &wil->sta[cid];
 	struct wil_tid_ampdu_rx *r;
 	u16 hseq;
 	int index;
-	unsigned long flags;
 
-	wil_dbg_txrx(wil, "MID %d CID %d TID %d Seq 0x%03x\n",
-		     mid, cid, tid, seq);
+	wil_dbg_txrx(wil, "MID %d CID %d TID %d Seq 0x%03x mcast %01x\n",
+		     mid, cid, tid, seq, mcast);
 
-	spin_lock_irqsave(&sta->tid_rx_lock, flags);
+	if (unlikely(mcast)) {
+		wil_netif_rx_any(skb, ndev);
+		return;
+	}
+
+	spin_lock(&sta->tid_rx_lock);
 
 	r = sta->tid_rx[tid];
 	if (!r) {
-		spin_unlock_irqrestore(&sta->tid_rx_lock, flags);
 		wil_netif_rx_any(skb, ndev);
-		return;
+		goto out;
 	}
 
 	hseq = r->head_seq_num;
@@ -121,13 +127,24 @@ void wil_rx_reorder(struct wil6210_priv *wil, struct sk_buff *skb)
 	 * reported, and data Rx, few packets may be pass up before reorder
 	 * buffer get allocated. Catch up by pretending SSN is what we
 	 * see in the 1-st Rx packet
+	 *
+	 * Another scenario, Rx get delayed and we got packet from before
+	 * BACK. Pass it to the stack and wait.
 	 */
 	if (r->first_time) {
 		r->first_time = false;
 		if (seq != r->head_seq_num) {
-			wil_err(wil, "Error: 1-st frame with wrong sequence"
-				" %d, should be %d. Fixing...\n", seq,
-				r->head_seq_num);
+			if (seq_less(seq, r->head_seq_num)) {
+				wil_err(wil,
+					"Error: frame with early sequence 0x%03x, should be 0x%03x. Waiting...\n",
+					seq, r->head_seq_num);
+				r->first_time = true;
+				wil_netif_rx_any(skb, ndev);
+				goto out;
+			}
+			wil_err(wil,
+				"Error: 1-st frame with wrong sequence 0x%03x, should be 0x%03x. Fixing...\n",
+				seq, r->head_seq_num);
 			r->head_seq_num = seq;
 			r->ssn = seq;
 		}
@@ -179,7 +196,7 @@ void wil_rx_reorder(struct wil6210_priv *wil, struct sk_buff *skb)
 	wil_reorder_release(wil, r);
 
 out:
-	spin_unlock_irqrestore(&sta->tid_rx_lock, flags);
+	spin_unlock(&sta->tid_rx_lock);
 }
 
 struct wil_tid_ampdu_rx *wil_tid_ampdu_rx_alloc(struct wil6210_priv *wil,
@@ -219,3 +236,241 @@ void wil_tid_ampdu_rx_free(struct wil6210_priv *wil,
 	kfree(r->reorder_time);
 	kfree(r);
 }
+
+/* ADDBA processing */
+static u16 wil_agg_size(struct wil6210_priv *wil, u16 req_agg_wsize)
+{
+	u16 max_agg_size = min_t(u16, WIL_MAX_AGG_WSIZE, WIL_MAX_AMPDU_SIZE /
+				 (mtu_max + WIL_MAX_MPDU_OVERHEAD));
+
+	if (!req_agg_wsize)
+		return max_agg_size;
+
+	return min(max_agg_size, req_agg_wsize);
+}
+
+/* Block Ack - Rx side (recipient */
+int wil_addba_rx_request(struct wil6210_priv *wil, u8 cidxtid,
+			 u8 dialog_token, __le16 ba_param_set,
+			 __le16 ba_timeout, __le16 ba_seq_ctrl)
+{
+	struct wil_back_rx *req = kzalloc(sizeof(*req), GFP_KERNEL);
+
+	if (!req)
+		return -ENOMEM;
+
+	req->cidxtid = cidxtid;
+	req->dialog_token = dialog_token;
+	req->ba_param_set = le16_to_cpu(ba_param_set);
+	req->ba_timeout = le16_to_cpu(ba_timeout);
+	req->ba_seq_ctrl = le16_to_cpu(ba_seq_ctrl);
+
+	mutex_lock(&wil->back_rx_mutex);
+	list_add_tail(&req->list, &wil->back_rx_pending);
+	mutex_unlock(&wil->back_rx_mutex);
+
+	queue_work(wil->wq_service, &wil->back_rx_worker);
+
+	return 0;
+}
+
+static void wil_back_rx_handle(struct wil6210_priv *wil,
+			       struct wil_back_rx *req)
+__acquires(&sta->tid_rx_lock) __releases(&sta->tid_rx_lock)
+{
+	struct wil_sta_info *sta;
+	u8 cid, tid;
+	u16 agg_wsize = 0;
+	/* bit 0: A-MSDU supported
+	 * bit 1: policy (should be 0 for us)
+	 * bits 2..5: TID
+	 * bits 6..15: buffer size
+	 */
+	u16 req_agg_wsize = WIL_GET_BITS(req->ba_param_set, 6, 15);
+	bool agg_amsdu = !!(req->ba_param_set & BIT(0));
+	int ba_policy = req->ba_param_set & BIT(1);
+	u16 agg_timeout = req->ba_timeout;
+	u16 status = WLAN_STATUS_SUCCESS;
+	u16 ssn = req->ba_seq_ctrl >> 4;
+	int rc;
+
+	might_sleep();
+	parse_cidxtid(req->cidxtid, &cid, &tid);
+
+	/* sanity checks */
+	if (cid >= WIL6210_MAX_CID) {
+		wil_err(wil, "BACK: invalid CID %d\n", cid);
+		return;
+	}
+
+	sta = &wil->sta[cid];
+	if (sta->status != wil_sta_connected) {
+		wil_err(wil, "BACK: CID %d not connected\n", cid);
+		return;
+	}
+
+	wil_dbg_wmi(wil,
+		    "ADDBA request for CID %d %pM TID %d size %d timeout %d AMSDU%s policy %d token %d SSN 0x%03x\n",
+		    cid, sta->addr, tid, req_agg_wsize, req->ba_timeout,
+		    agg_amsdu ? "+" : "-", !!ba_policy, req->dialog_token, ssn);
+
+	/* apply policies */
+	if (ba_policy) {
+		wil_err(wil, "BACK requested unsupported ba_policy == 1\n");
+		status = WLAN_STATUS_INVALID_QOS_PARAM;
+	}
+	if (status == WLAN_STATUS_SUCCESS)
+		agg_wsize = wil_agg_size(wil, req_agg_wsize);
+
+	rc = wmi_addba_rx_resp(wil, cid, tid, req->dialog_token, status,
+			       agg_amsdu, agg_wsize, agg_timeout);
+	if (rc || (status != WLAN_STATUS_SUCCESS))
+		return;
+
+	/* apply */
+	spin_lock_bh(&sta->tid_rx_lock);
+
+	wil_tid_ampdu_rx_free(wil, sta->tid_rx[tid]);
+	sta->tid_rx[tid] = wil_tid_ampdu_rx_alloc(wil, agg_wsize, ssn);
+
+	spin_unlock_bh(&sta->tid_rx_lock);
+}
+
+void wil_back_rx_flush(struct wil6210_priv *wil)
+{
+	struct wil_back_rx *evt, *t;
+
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
+	mutex_lock(&wil->back_rx_mutex);
+
+	list_for_each_entry_safe(evt, t, &wil->back_rx_pending, list) {
+		list_del(&evt->list);
+		kfree(evt);
+	}
+
+	mutex_unlock(&wil->back_rx_mutex);
+}
+
+/* Retrieve next ADDBA request from the pending list */
+static struct list_head *next_back_rx(struct wil6210_priv *wil)
+{
+	struct list_head *ret = NULL;
+
+	mutex_lock(&wil->back_rx_mutex);
+
+	if (!list_empty(&wil->back_rx_pending)) {
+		ret = wil->back_rx_pending.next;
+		list_del(ret);
+	}
+
+	mutex_unlock(&wil->back_rx_mutex);
+
+	return ret;
+}
+
+void wil_back_rx_worker(struct work_struct *work)
+{
+	struct wil6210_priv *wil = container_of(work, struct wil6210_priv,
+						back_rx_worker);
+	struct wil_back_rx *evt;
+	struct list_head *lh;
+
+	while ((lh = next_back_rx(wil)) != NULL) {
+		evt = list_entry(lh, struct wil_back_rx, list);
+
+		wil_back_rx_handle(wil, evt);
+		kfree(evt);
+	}
+}
+
+/* BACK - Tx (originator) side */
+static void wil_back_tx_handle(struct wil6210_priv *wil,
+			       struct wil_back_tx *req)
+{
+	struct vring_tx_data *txdata = &wil->vring_tx_data[req->ringid];
+	int rc;
+
+	if (txdata->addba_in_progress) {
+		wil_dbg_misc(wil, "ADDBA for vring[%d] already in progress\n",
+			     req->ringid);
+		return;
+	}
+	if (txdata->agg_wsize) {
+		wil_dbg_misc(wil,
+			     "ADDBA for vring[%d] already established wsize %d\n",
+			     req->ringid, txdata->agg_wsize);
+		return;
+	}
+	txdata->addba_in_progress = true;
+	rc = wmi_addba(wil, req->ringid, req->agg_wsize, req->agg_timeout);
+	if (rc)
+		txdata->addba_in_progress = false;
+}
+
+static struct list_head *next_back_tx(struct wil6210_priv *wil)
+{
+	struct list_head *ret = NULL;
+
+	mutex_lock(&wil->back_tx_mutex);
+
+	if (!list_empty(&wil->back_tx_pending)) {
+		ret = wil->back_tx_pending.next;
+		list_del(ret);
+	}
+
+	mutex_unlock(&wil->back_tx_mutex);
+
+	return ret;
+}
+
+void wil_back_tx_worker(struct work_struct *work)
+{
+	struct wil6210_priv *wil = container_of(work, struct wil6210_priv,
+						 back_tx_worker);
+	struct wil_back_tx *evt;
+	struct list_head *lh;
+
+	while ((lh = next_back_tx(wil)) != NULL) {
+		evt = list_entry(lh, struct wil_back_tx, list);
+
+		wil_back_tx_handle(wil, evt);
+		kfree(evt);
+	}
+}
+
+void wil_back_tx_flush(struct wil6210_priv *wil)
+{
+	struct wil_back_tx *evt, *t;
+
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
+	mutex_lock(&wil->back_tx_mutex);
+
+	list_for_each_entry_safe(evt, t, &wil->back_tx_pending, list) {
+		list_del(&evt->list);
+		kfree(evt);
+	}
+
+	mutex_unlock(&wil->back_tx_mutex);
+}
+
+int wil_addba_tx_request(struct wil6210_priv *wil, u8 ringid, u16 wsize)
+{
+	struct wil_back_tx *req = kzalloc(sizeof(*req), GFP_KERNEL);
+
+	if (!req)
+		return -ENOMEM;
+
+	req->ringid = ringid;
+	req->agg_wsize = wil_agg_size(wil, wsize);
+	req->agg_timeout = 0;
+
+	mutex_lock(&wil->back_tx_mutex);
+	list_add_tail(&req->list, &wil->back_tx_pending);
+	mutex_unlock(&wil->back_tx_mutex);
+
+	queue_work(wil->wq_service, &wil->back_tx_worker);
+
+	return 0;
+}
diff --git a/drivers/net/wireless/ath/wil6210/txrx.c b/drivers/net/wireless/ath/wil6210/txrx.c
index e3f8bdce5abc..b58ee52e1860 100644
--- a/drivers/net/wireless/ath/wil6210/txrx.c
+++ b/drivers/net/wireless/ath/wil6210/txrx.c
@@ -463,7 +463,7 @@ static struct sk_buff *wil_vring_reap_rx(struct wil6210_priv *wil,
 	 * and in case of error drop the packet
 	 * higher stack layers will handle retransmission (if required)
 	 */
-	if (d->dma.status & RX_DMA_STATUS_L4_IDENT) {
+	if (d->dma.status & RX_DMA_STATUS_L4I) {
 		/* L4 protocol identified, csum calculated */
 		if ((d->dma.error & RX_DMA_ERROR_L4_ERR) == 0)
 			skb->ip_summed = CHECKSUM_UNNECESSARY;
@@ -581,14 +581,8 @@ void wil_rx_handle(struct wil6210_priv *wil, int *quota)
 			skb->protocol = htons(ETH_P_802_2);
 			wil_netif_rx_any(skb, ndev);
 		} else {
-			struct ethhdr *eth = (void *)skb->data;
-
 			skb->protocol = eth_type_trans(skb, ndev);
-
-			if (is_unicast_ether_addr(eth->h_dest))
-				wil_rx_reorder(wil, skb);
-			else
-				wil_netif_rx_any(skb, ndev);
+			wil_rx_reorder(wil, skb);
 		}
 	}
 	wil_rx_refill(wil, v->size);
@@ -645,7 +639,7 @@ int wil_vring_init_tx(struct wil6210_priv *wil, int id, int size,
 		.vring_cfg = {
 			.tx_sw_ring = {
 				.max_mpdu_size =
-					cpu_to_le16(mtu_max + ETH_HLEN),
+					cpu_to_le16(wil_mtu2macbuf(mtu_max)),
 				.ring_size = cpu_to_le16(size),
 			},
 			.ringid = id,
@@ -653,7 +647,7 @@ int wil_vring_init_tx(struct wil6210_priv *wil, int id, int size,
 			.encap_trans_type = WMI_VRING_ENC_TYPE_802_3,
 			.mac_ctrl = 0,
 			.to_resolution = 0,
-			.agg_max_wsize = 16,
+			.agg_max_wsize = 0,
 			.schd_params = {
 				.priority = cpu_to_le16(0),
 				.timeslot_us = cpu_to_le16(0xfff),
@@ -701,6 +695,8 @@ int wil_vring_init_tx(struct wil6210_priv *wil, int id, int size,
 	vring->hwtail = le32_to_cpu(reply.cmd.tx_vring_tail_ptr);
 
 	txdata->enabled = 1;
+	if (wil->sta[cid].data_port_open && (agg_wsize >= 0))
+		wil_addba_tx_request(wil, id, agg_wsize);
 
 	return 0;
  out_free:
@@ -713,6 +709,7 @@ int wil_vring_init_tx(struct wil6210_priv *wil, int id, int size,
 void wil_vring_fini_tx(struct wil6210_priv *wil, int id)
 {
 	struct vring *vring = &wil->vring_tx[id];
+	struct vring_tx_data *txdata = &wil->vring_tx_data[id];
 
 	WARN_ON(!mutex_is_locked(&wil->mutex));
 
@@ -723,10 +720,11 @@ void wil_vring_fini_tx(struct wil6210_priv *wil, int id)
 
 	/* make sure NAPI won't touch this vring */
 	wil->vring_tx_data[id].enabled = 0;
-	if (test_bit(wil_status_napi_en, &wil->status))
+	if (test_bit(wil_status_napi_en, wil->status))
 		napi_synchronize(&wil->napi_tx);
 
 	wil_vring_free(wil, vring, 1);
+	memset(txdata, 0, sizeof(*txdata));
 }
 
 static struct vring *wil_find_tx_vring(struct wil6210_priv *wil,
@@ -773,6 +771,38 @@ static void wil_set_da_for_vring(struct wil6210_priv *wil,
 
 static int wil_tx_vring(struct wil6210_priv *wil, struct vring *vring,
 			struct sk_buff *skb);
+
+static struct vring *wil_find_tx_vring_sta(struct wil6210_priv *wil,
+					   struct sk_buff *skb)
+{
+	struct vring *v;
+	int i;
+	u8 cid;
+
+	/* In the STA mode, it is expected to have only 1 VRING
+	 * for the AP we connected to.
+	 * find 1-st vring and see whether it is eligible for data
+	 */
+	for (i = 0; i < WIL6210_MAX_TX_RINGS; i++) {
+		v = &wil->vring_tx[i];
+		if (!v->va)
+			continue;
+
+		cid = wil->vring2cid_tid[i][0];
+		if (!wil->sta[cid].data_port_open &&
+		    (skb->protocol != cpu_to_be16(ETH_P_PAE)))
+			break;
+
+		wil_dbg_txrx(wil, "Tx -> ring %d\n", i);
+
+		return v;
+	}
+
+	wil_dbg_txrx(wil, "Tx while no vrings active?\n");
+
+	return NULL;
+}
+
 /*
  * Find 1-st vring and return it; set dest address for this vring in skb
  * duplicate skb and send it to other active vrings
@@ -1034,14 +1064,14 @@ netdev_tx_t wil_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	int rc;
 
 	wil_dbg_txrx(wil, "%s()\n", __func__);
-	if (!test_bit(wil_status_fwready, &wil->status)) {
+	if (!test_bit(wil_status_fwready, wil->status)) {
 		if (!pr_once_fw) {
 			wil_err(wil, "FW not ready\n");
 			pr_once_fw = true;
 		}
 		goto drop;
 	}
-	if (!test_bit(wil_status_fwconnected, &wil->status)) {
+	if (!test_bit(wil_status_fwconnected, wil->status)) {
 		wil_err(wil, "FW not connected\n");
 		goto drop;
 	}
@@ -1052,15 +1082,19 @@ netdev_tx_t wil_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	pr_once_fw = false;
 
 	/* find vring */
-	if (is_unicast_ether_addr(eth->h_dest))
-		vring = wil_find_tx_vring(wil, skb);
-	else
-		vring = wil_tx_bcast(wil, skb);
+	if (wil->wdev->iftype == NL80211_IFTYPE_STATION) {
+		/* in STA mode (ESS), all to same VRING */
+		vring = wil_find_tx_vring_sta(wil, skb);
+	} else { /* direct communication, find matching VRING */
+		if (is_unicast_ether_addr(eth->h_dest))
+			vring = wil_find_tx_vring(wil, skb);
+		else
+			vring = wil_tx_bcast(wil, skb);
+	}
 	if (!vring) {
 		wil_dbg_txrx(wil, "No Tx VRING found for %pM\n", eth->h_dest);
 		goto drop;
 	}
-
 	/* set up vring entry */
 	rc = wil_tx_vring(wil, vring, skb);
 
diff --git a/drivers/net/wireless/ath/wil6210/txrx.h b/drivers/net/wireless/ath/wil6210/txrx.h
index 630aeb5fa7f4..d90c8aa20c15 100644
--- a/drivers/net/wireless/ath/wil6210/txrx.h
+++ b/drivers/net/wireless/ath/wil6210/txrx.h
@@ -20,17 +20,15 @@
 #define BUF_SW_OWNED    (1)
 #define BUF_HW_OWNED    (0)
 
-/* size of max. Tx/Rx buffers, as supported by FW */
-#define TXRX_BUF_LEN_DEFAULT (2242)
+/* default size of MAC Tx/Rx buffers */
+#define TXRX_BUF_LEN_DEFAULT (2048)
 
 /* how many bytes to reserve for rtap header? */
 #define WIL6210_RTAP_SIZE (128)
 
 /* Tx/Rx path */
 
-/*
- * Common representation of physical address in Vring
- */
+/* Common representation of physical address in Vring */
 struct vring_dma_addr {
 	__le32 addr_low;
 	__le16 addr_high;
@@ -49,11 +47,10 @@ static inline void wil_desc_addr_set(struct vring_dma_addr *addr,
 	addr->addr_high = cpu_to_le16((u16)upper_32_bits(pa));
 }
 
-/*
- * Tx descriptor - MAC part
+/* Tx descriptor - MAC part
  * [dword 0]
  * bit  0.. 9 : lifetime_expiry_value:10
- * bit     10 : interrup_en:1
+ * bit     10 : interrupt_en:1
  * bit     11 : status_en:1
  * bit 12..13 : txss_override:2
  * bit     14 : timestamp_insertion:1
@@ -61,15 +58,12 @@ static inline void wil_desc_addr_set(struct vring_dma_addr *addr,
  * bit 16..21 : reserved0:6
  * bit 22..26 : mcs_index:5
  * bit     27 : mcs_en:1
- * bit 28..29 : reserved1:2
- * bit     30 : reserved2:1
+ * bit 28..30 : reserved1:3
  * bit     31 : sn_preserved:1
  * [dword 1]
  * bit  0.. 3 : pkt_mode:4
  * bit      4 : pkt_mode_en:1
- * bit  5.. 7 : reserved0:3
- * bit  8..13 : reserved1:6
- * bit     14 : reserved2:1
+ * bit  5..14 : reserved0:10
  * bit     15 : ack_policy_en:1
  * bit 16..19 : dst_index:4
  * bit     20 : dst_index_en:1
@@ -80,7 +74,7 @@ static inline void wil_desc_addr_set(struct vring_dma_addr *addr,
  * [dword 2]
  * bit  0.. 7 : num_of_descriptors:8
  * bit  8..17 : reserved:10
- * bit 18..19 : l2_translation_type:2
+ * bit 18..19 : l2_translation_type:2 00 - bypass, 01 - 802.3, 10 - 802.11
  * bit     20 : snap_hdr_insertion_en:1
  * bit     21 : vlan_removal_en:1
  * bit 22..31 : reserved0:10
@@ -247,6 +241,46 @@ struct vring_tx_mac {
 
 #define TX_DMA_STATUS_DU         BIT(0)
 
+/* Tx descriptor - DMA part
+ * [dword 0]
+ * bit  0.. 7 : l4_length:8 layer 4 length
+ * bit      8 : cmd_eop:1 This descriptor is the last one in the packet
+ * bit      9 : reserved
+ * bit     10 : cmd_dma_it:1 immediate interrupt
+ * bit 11..12 : SBD - Segment Buffer Details
+ *		00 - Header Segment
+ *		01 - First Data Segment
+ *		10 - Medium Data Segment
+ *		11 - Last Data Segment
+ * bit     13 : TSE - TCP Segmentation Enable
+ * bit     14 : IIC - Directs the HW to Insert IPv4 Checksum
+ * bit     15 : ITC - Directs the HW to Insert TCP/UDP Checksum
+ * bit 16..20 : QID - The target QID that the packet should be stored
+ *		in the MAC.
+ * bit     21 : PO - Pseudo header Offload:
+ *		0 - Use the pseudo header value from the TCP checksum field
+ *		1- Calculate Pseudo header Checksum
+ * bit     22 : NC - No UDP Checksum
+ * bit 23..29 : reserved
+ * bit 30..31 : L4T - Layer 4 Type: 00 - UDP , 10 - TCP , 10, 11 - Reserved
+ *		If L4Len equal 0, no L4 at all
+ * [dword 1]
+ * bit  0..31 : addr_low:32 The payload buffer low address
+ * [dword 2]
+ * bit  0..15 : addr_high:16 The payload buffer high address
+ * bit 16..23 : ip_length:8 The IP header length for the TX IP checksum
+ *		offload feature
+ * bit 24..30 : mac_length:7
+ * bit     31 : ip_version:1 1 - IPv4, 0 - IPv6
+ * [dword 3]
+ *  [byte 12] error
+ * bit  0   2 : mac_status:3
+ * bit  3   7 : reserved:5
+ *  [byte 13] status
+ * bit      0 : DU:1 Descriptor Used
+ * bit  1   7 : reserved:7
+ *  [word 7] length
+ */
 struct vring_tx_dma {
 	u32 d0;
 	struct vring_dma_addr addr;
@@ -257,45 +291,45 @@ struct vring_tx_dma {
 	__le16 length;
 } __packed;
 
-/*
- * Rx descriptor - MAC part
+/* Rx descriptor - MAC part
  * [dword 0]
  * bit  0.. 3 : tid:4 The QoS (b3-0) TID Field
- * bit  4.. 6 : connection_id:3 :The Source index that  was found during
- *  Parsing the TA.  This field is used to  define the source of the packet
+ * bit  4.. 6 : cid:3 The Source index that  was found during parsing the TA.
+ *		This field is used to define the source of the packet
  * bit      7 : reserved:1
- * bit  8.. 9 : mac_id:2 : The MAC virtual  Ring number (always zero)
- * bit 10..11 : frame_type:2 : The FC Control  (b3-2) -  MPDU Type
- *              (management, data, control  and extension)
- * bit 12..15 : frame_subtype:4 : The FC Control  (b7-4) -  Frame Subtype
+ * bit  8.. 9 : mid:2 The MAC virtual number
+ * bit 10..11 : frame_type:2 : The FC (b3-2) - MPDU Type
+ *		(management, data, control and extension)
+ * bit 12..15 : frame_subtype:4 : The FC (b7-4) - Frame Subtype
  * bit 16..27 : seq_number:12 The received Sequence number field
  * bit 28..31 : extended:4 extended subtype
  * [dword 1]
  * bit  0.. 3 : reserved
  * bit  4.. 5 : key_id:2
  * bit      6 : decrypt_bypass:1
- * bit      7 : security:1
- * bit  8.. 9 : ds_bits:2
- * bit     10 : a_msdu_present:1  from qos header
- * bit     11 : a_msdu_type:1  from qos header
+ * bit      7 : security:1 FC (b14)
+ * bit  8.. 9 : ds_bits:2 FC (b9-8)
+ * bit     10 : a_msdu_present:1  QoS (b7)
+ * bit     11 : a_msdu_type:1  QoS (b8)
  * bit     12 : a_mpdu:1  part of AMPDU aggregation
  * bit     13 : broadcast:1
  * bit     14 : mutlicast:1
  * bit     15 : reserved:1
- * bit 16..20 : rx_mac_qid:5   The Queue Identifier that the packet
- *                             is received from
+ * bit 16..20 : rx_mac_qid:5 The Queue Identifier that the packet
+ *		is received from
  * bit 21..24 : mcs:4
- * bit 25..28 : mic_icr:4
+ * bit 25..28 : mic_icr:4 this signal tells the DMA to assert an interrupt
+ *		after it writes the packet
  * bit 29..31 : reserved:3
  * [dword 2]
  * bit  0.. 2 : time_slot:3 The timeslot that the MPDU is received
- * bit      3 : fc_protocol_ver:1 The FC Control  (b0) - Protocol  Version
- * bit      4 : fc_order:1 The FC Control (b15) -Order
- * bit  5.. 7 : qos_ack_policy:3  The QoS (b6-5) ack policy Field
+ * bit  3.. 4 : fc_protocol_ver:1 The FC (b1-0) - Protocol Version
+ * bit      5 : fc_order:1 The FC Control (b15) -Order
+ * bit  6.. 7 : qos_ack_policy:2 The QoS (b6-5) ack policy Field
  * bit      8 : esop:1 The QoS (b4) ESOP field
- * bit      9 : qos_rdg_more_ppdu:1 The QoS (b9) RDG  field
- * bit 10..14 : qos_reserved:5 The QoS (b14-10) Reserved  field
- * bit     15 : qos_ac_constraint:1
+ * bit      9 : qos_rdg_more_ppdu:1 The QoS (b9) RDG field
+ * bit 10..14 : qos_reserved:5 The QoS (b14-10) Reserved field
+ * bit     15 : qos_ac_constraint:1 QoS (b15)
  * bit 16..31 : pn_15_0:16 low 2 bytes of PN
  * [dword 3]
  * bit  0..31 : pn_47_16:32 high 4 bytes of PN
@@ -308,35 +342,46 @@ struct vring_rx_mac {
 	u32 pn_47_16;
 } __packed;
 
-/*
- * Rx descriptor - DMA part
+/* Rx descriptor - DMA part
  * [dword 0]
- * bit  0.. 7 : l4_length:8 layer 4 length
- * bit  8.. 9 : reserved:2
- * bit     10 : cmd_dma_it:1
+ * bit  0.. 7 : l4_length:8 layer 4 length. The field is only valid if
+ *		L4I bit is set
+ * bit      8 : cmd_eop:1 set to 1
+ * bit      9 : cmd_rt:1 set to 1
+ * bit     10 : cmd_dma_it:1 immediate interrupt
  * bit 11..15 : reserved:5
- * bit 16..29 : phy_info_length:14
+ * bit 16..29 : phy_info_length:14 It is valid when the PII is set.
+ *		When the FFM bit is set bits 29-27 are used for for
+ *		Flex Filter Match. Matching Index to one of the L2
+ *		EtherType Flex Filter
  * bit 30..31 : l4_type:2 valid if the L4I bit is set in the status field
+ *		00 - UDP, 01 - TCP, 10, 11 - reserved
  * [dword 1]
  * bit  0..31 : addr_low:32 The payload buffer low address
  * [dword 2]
  * bit  0..15 : addr_high:16 The payload buffer high address
- * bit 16..23 : ip_length:8
+ * bit 16..23 : ip_length:8 The filed is valid only if the L3I bit is set
  * bit 24..30 : mac_length:7
- * bit     31 : ip_version:1
+ * bit     31 : ip_version:1 1 - IPv4, 0 - IPv6
  * [dword 3]
  *  [byte 12] error
+ * bit      0 : FCS:1
+ * bit      1 : MIC:1
+ * bit      2 : Key miss:1
+ * bit      3 : Replay:1
+ * bit      4 : L3:1 IPv4 checksum
+ * bit      5 : L4:1 TCP/UDP checksum
+ * bit  6   7 : reserved:2
  *  [byte 13] status
- * bit      0 : du:1
- * bit      1 : eop:1
+ * bit      0 : DU:1 Descriptor Used
+ * bit      1 : EOP:1 The descriptor indicates the End of Packet
  * bit      2 : error:1
- * bit      3 : mi:1
- * bit      4 : l3_identified:1
- * bit      5 : l4_identified:1
- * bit      6 : phy_info_included:1
- * bit      7 : reserved:1
+ * bit      3 : MI:1 MAC Interrupt is asserted (according to parser decision)
+ * bit      4 : L3I:1 L3 identified and checksum calculated
+ * bit      5 : L4I:1 L4 identified and checksum calculated
+ * bit      6 : PII:1 PHY Info Included in the packet
+ * bit      7 : FFM:1 EtherType Flex Filter Match
  *  [word 7] length
- *
  */
 
 #define RX_DMA_D0_CMD_DMA_IT     BIT(10)
@@ -349,9 +394,9 @@ struct vring_rx_mac {
 #define RX_DMA_STATUS_DU         BIT(0)
 #define RX_DMA_STATUS_ERROR      BIT(2)
 
-#define RX_DMA_STATUS_L3_IDENT   BIT(4)
-#define RX_DMA_STATUS_L4_IDENT   BIT(5)
-#define RX_DMA_STATUS_PHY_INFO   BIT(6)
+#define RX_DMA_STATUS_L3I	BIT(4)
+#define RX_DMA_STATUS_L4I	BIT(5)
+#define RX_DMA_STATUS_PHY_INFO	BIT(6)
 
 struct vring_rx_dma {
 	u32 d0;
@@ -423,6 +468,11 @@ static inline int wil_rxdesc_mcs(struct vring_rx_desc *d)
 	return WIL_GET_BITS(d->mac.d1, 21, 24);
 }
 
+static inline int wil_rxdesc_mcast(struct vring_rx_desc *d)
+{
+	return WIL_GET_BITS(d->mac.d1, 13, 14);
+}
+
 static inline int wil_rxdesc_phy_length(struct vring_rx_desc *d)
 {
 	return WIL_GET_BITS(d->dma.d0, 16, 29);
diff --git a/drivers/net/wireless/ath/wil6210/wil6210.h b/drivers/net/wireless/ath/wil6210/wil6210.h
index c6ec5b99ac7d..da3fe7853d63 100644
--- a/drivers/net/wireless/ath/wil6210/wil6210.h
+++ b/drivers/net/wireless/ath/wil6210/wil6210.h
@@ -25,19 +25,14 @@
 
 extern bool no_fw_recovery;
 extern unsigned int mtu_max;
+extern unsigned short rx_ring_overflow_thrsh;
+extern int agg_wsize;
 
 #define WIL_NAME "wil6210"
 #define WIL_FW_NAME "wil6210.fw"
 
 #define WIL_MAX_BUS_REQUEST_KBPS 800000 /* ~6.1Gbps */
 
-struct wil_board {
-	int board;
-#define WIL_BOARD_MARLON	(1)
-#define WIL_BOARD_SPARROW	(2)
-	const char * const name;
-};
-
 /**
  * extract bits [@b0:@b1] (inclusive) from the value @x
  * it should be @b0 <= @b1, or result is incorrect
@@ -57,13 +52,41 @@ static inline u32 WIL_GET_BITS(u32 x, int b0, int b1)
 #define WIL6210_MAX_TX_RINGS	(24) /* HW limit */
 #define WIL6210_MAX_CID		(8) /* HW limit */
 #define WIL6210_NAPI_BUDGET	(16) /* arbitrary */
+#define WIL_MAX_AMPDU_SIZE	(64 * 1024) /* FW/HW limit */
+#define WIL_MAX_AGG_WSIZE	(32) /* FW/HW limit */
+/* Hardware offload block adds the following:
+ * 26 bytes - 3-address QoS data header
+ *  8 bytes - IV + EIV (for GCMP)
+ *  8 bytes - SNAP
+ * 16 bytes - MIC (for GCMP)
+ *  4 bytes - CRC
+ */
+#define WIL_MAX_MPDU_OVERHEAD	(62)
+
+/* Calculate MAC buffer size for the firmware. It includes all overhead,
+ * as it will go over the air, and need to be 8 byte aligned
+ */
+static inline u32 wil_mtu2macbuf(u32 mtu)
+{
+	return ALIGN(mtu + WIL_MAX_MPDU_OVERHEAD, 8);
+}
+
+/* MTU for Ethernet need to take into account 8-byte SNAP header
+ * to be added when encapsulating Ethernet frame into 802.11
+ */
+#define WIL_MAX_ETH_MTU		(IEEE80211_MAX_DATA_LEN_DMG - 8)
 /* Max supported by wil6210 value for interrupt threshold is 5sec. */
 #define WIL6210_ITR_TRSH_MAX (5000000)
-#define WIL6210_ITR_TRSH_DEFAULT	(300) /* usec */
+#define WIL6210_ITR_TX_INTERFRAME_TIMEOUT_DEFAULT (15) /* usec */
+#define WIL6210_ITR_RX_INTERFRAME_TIMEOUT_DEFAULT (15) /* usec */
+#define WIL6210_ITR_TX_MAX_BURST_DURATION_DEFAULT (500) /* usec */
+#define WIL6210_ITR_RX_MAX_BURST_DURATION_DEFAULT (500) /* usec */
 #define WIL6210_FW_RECOVERY_RETRIES	(5) /* try to recover this many times */
 #define WIL6210_FW_RECOVERY_TO	msecs_to_jiffies(5000)
 #define WIL6210_SCAN_TO		msecs_to_jiffies(10000)
-
+#define WIL6210_RX_HIGH_TRSH_INIT		(0)
+#define WIL6210_RX_HIGH_TRSH_DEFAULT \
+				(1 << (WIL_RX_RING_SIZE_ORDER_DEFAULT - 3))
 /* Hardware definitions begin */
 
 /*
@@ -135,7 +158,7 @@ struct RGF_ICR {
 	#define BIT_DMA_EP_MISC_ICR_TX_NO_ACT	BIT(1)
 	#define BIT_DMA_EP_MISC_ICR_FW_INT(n)	BIT(28+n) /* n = [0..3] */
 
-/* Interrupt moderation control */
+/* Legacy interrupt moderation control (before Sparrow v2)*/
 #define RGF_DMA_ITR_CNT_TRSH		(0x881c5c)
 #define RGF_DMA_ITR_CNT_DATA		(0x881c60)
 #define RGF_DMA_ITR_CNT_CRL		(0x881c64)
@@ -145,6 +168,46 @@ struct RGF_ICR {
 	#define BIT_DMA_ITR_CNT_CRL_CLR		BIT(3)
 	#define BIT_DMA_ITR_CNT_CRL_REACH_TRSH	BIT(4)
 
+/* New (sparrow v2+) interrupt moderation control */
+#define RGF_DMA_ITR_TX_DESQ_NO_MOD		(0x881d40)
+#define RGF_DMA_ITR_TX_CNT_TRSH			(0x881d34)
+#define RGF_DMA_ITR_TX_CNT_DATA			(0x881d38)
+#define RGF_DMA_ITR_TX_CNT_CTL			(0x881d3c)
+	#define BIT_DMA_ITR_TX_CNT_CTL_EN		BIT(0)
+	#define BIT_DMA_ITR_TX_CNT_CTL_EXT_TIC_SEL	BIT(1)
+	#define BIT_DMA_ITR_TX_CNT_CTL_FOREVER		BIT(2)
+	#define BIT_DMA_ITR_TX_CNT_CTL_CLR		BIT(3)
+	#define BIT_DMA_ITR_TX_CNT_CTL_REACHED_TRESH	BIT(4)
+	#define BIT_DMA_ITR_TX_CNT_CTL_CROSS_EN		BIT(5)
+	#define BIT_DMA_ITR_TX_CNT_CTL_FREE_RUNNIG	BIT(6)
+#define RGF_DMA_ITR_TX_IDL_CNT_TRSH			(0x881d60)
+#define RGF_DMA_ITR_TX_IDL_CNT_DATA			(0x881d64)
+#define RGF_DMA_ITR_TX_IDL_CNT_CTL			(0x881d68)
+	#define BIT_DMA_ITR_TX_IDL_CNT_CTL_EN			BIT(0)
+	#define BIT_DMA_ITR_TX_IDL_CNT_CTL_EXT_TIC_SEL		BIT(1)
+	#define BIT_DMA_ITR_TX_IDL_CNT_CTL_FOREVER		BIT(2)
+	#define BIT_DMA_ITR_TX_IDL_CNT_CTL_CLR			BIT(3)
+	#define BIT_DMA_ITR_TX_IDL_CNT_CTL_REACHED_TRESH	BIT(4)
+#define RGF_DMA_ITR_RX_DESQ_NO_MOD		(0x881d50)
+#define RGF_DMA_ITR_RX_CNT_TRSH			(0x881d44)
+#define RGF_DMA_ITR_RX_CNT_DATA			(0x881d48)
+#define RGF_DMA_ITR_RX_CNT_CTL			(0x881d4c)
+	#define BIT_DMA_ITR_RX_CNT_CTL_EN		BIT(0)
+	#define BIT_DMA_ITR_RX_CNT_CTL_EXT_TIC_SEL	BIT(1)
+	#define BIT_DMA_ITR_RX_CNT_CTL_FOREVER		BIT(2)
+	#define BIT_DMA_ITR_RX_CNT_CTL_CLR		BIT(3)
+	#define BIT_DMA_ITR_RX_CNT_CTL_REACHED_TRESH	BIT(4)
+	#define BIT_DMA_ITR_RX_CNT_CTL_CROSS_EN		BIT(5)
+	#define BIT_DMA_ITR_RX_CNT_CTL_FREE_RUNNIG	BIT(6)
+#define RGF_DMA_ITR_RX_IDL_CNT_TRSH			(0x881d54)
+#define RGF_DMA_ITR_RX_IDL_CNT_DATA			(0x881d58)
+#define RGF_DMA_ITR_RX_IDL_CNT_CTL			(0x881d5c)
+	#define BIT_DMA_ITR_RX_IDL_CNT_CTL_EN			BIT(0)
+	#define BIT_DMA_ITR_RX_IDL_CNT_CTL_EXT_TIC_SEL		BIT(1)
+	#define BIT_DMA_ITR_RX_IDL_CNT_CTL_FOREVER		BIT(2)
+	#define BIT_DMA_ITR_RX_IDL_CNT_CTL_CLR			BIT(3)
+	#define BIT_DMA_ITR_RX_IDL_CNT_CTL_REACHED_TRESH	BIT(4)
+
 #define RGF_DMA_PSEUDO_CAUSE		(0x881c68)
 #define RGF_DMA_PSEUDO_CAUSE_MASK_SW	(0x881c6c)
 #define RGF_DMA_PSEUDO_CAUSE_MASK_FW	(0x881c70)
@@ -164,6 +227,20 @@ struct RGF_ICR {
 #define RGF_CAF_PLL_LOCK_STATUS		(0x88afec)
 	#define BIT_CAF_OSC_DIG_XTAL_STABLE	BIT(0)
 
+#define RGF_USER_JTAG_DEV_ID	(0x880b34) /* device ID */
+	#define JTAG_DEV_ID_MARLON_B0	(0x0612072f)
+	#define JTAG_DEV_ID_SPARROW_A0	(0x0632072f)
+	#define JTAG_DEV_ID_SPARROW_A1	(0x1632072f)
+	#define JTAG_DEV_ID_SPARROW_B0	(0x2632072f)
+
+enum {
+	HW_VER_UNKNOWN,
+	HW_VER_MARLON_B0,  /* JTAG_DEV_ID_MARLON_B0  */
+	HW_VER_SPARROW_A0, /* JTAG_DEV_ID_SPARROW_A0 */
+	HW_VER_SPARROW_A1, /* JTAG_DEV_ID_SPARROW_A1 */
+	HW_VER_SPARROW_B0, /* JTAG_DEV_ID_SPARROW_B0 */
+};
+
 /* popular locations */
 #define HOST_MBOX   HOSTADDR(RGF_USER_USER_SCRATCH_PAD)
 #define HOST_SW_INT (HOSTADDR(RGF_USER_USER_ICR) + \
@@ -303,6 +380,10 @@ struct vring {
 struct vring_tx_data {
 	int enabled;
 	cycles_t idle, last_idle, begin;
+	u8 agg_wsize; /* agreed aggregation window, 0 - no agg */
+	u16 agg_timeout;
+	u8 agg_amsdu;
+	bool addba_in_progress; /* if set, agg_xxx is for request in progress */
 };
 
 enum { /* for wil6210_priv.status */
@@ -313,6 +394,7 @@ enum { /* for wil6210_priv.status */
 	wil_status_reset_done,
 	wil_status_irqen, /* FIXME: interrupts enabled - for debug */
 	wil_status_napi_en, /* NAPI enabled protected by wil->mutex */
+	wil_status_last /* keep last */
 };
 
 struct pci_dev;
@@ -397,15 +479,40 @@ enum {
 	fw_recovery_running = 2,
 };
 
+enum {
+	hw_capability_reset_v2 = 0,
+	hw_capability_advanced_itr_moderation = 1,
+	hw_capability_last
+};
+
+struct wil_back_rx {
+	struct list_head list;
+	/* request params, converted to CPU byte order - what we asked for */
+	u8 cidxtid;
+	u8 dialog_token;
+	u16 ba_param_set;
+	u16 ba_timeout;
+	u16 ba_seq_ctrl;
+};
+
+struct wil_back_tx {
+	struct list_head list;
+	/* request params, converted to CPU byte order - what we asked for */
+	u8 ringid;
+	u8 agg_wsize;
+	u16 agg_timeout;
+};
+
 struct wil6210_priv {
 	struct pci_dev *pdev;
 	int n_msi;
 	struct wireless_dev *wdev;
 	void __iomem *csr;
-	ulong status;
+	DECLARE_BITMAP(status, wil_status_last);
 	u32 fw_version;
 	u32 hw_version;
-	struct wil_board *board;
+	const char *hw_name;
+	DECLARE_BITMAP(hw_capabilities, hw_capability_last);
 	u8 n_mids; /* number of additional MIDs as reported by FW */
 	u32 recovery_count; /* num of FW recovery attempts in a short time */
 	u32 recovery_state; /* FW recovery state machine */
@@ -415,7 +522,11 @@ struct wil6210_priv {
 	u32 monitor_flags;
 	u32 secure_pcp; /* create secure PCP? */
 	int sinfo_gen;
-	u32 itr_trsh;
+	/* interrupt moderation */
+	u32 tx_max_burst_duration;
+	u32 tx_interframe_timeout;
+	u32 rx_max_burst_duration;
+	u32 rx_interframe_timeout;
 	/* cached ISR registers */
 	u32 isr_misc;
 	/* mailbox related */
@@ -429,7 +540,7 @@ struct wil6210_priv {
 	u16 reply_size;
 	struct workqueue_struct *wmi_wq; /* for deferred calls */
 	struct work_struct wmi_event_worker;
-	struct workqueue_struct *wmi_wq_conn; /* for connect worker */
+	struct workqueue_struct *wq_service;
 	struct work_struct connect_worker;
 	struct work_struct disconnect_worker;
 	struct work_struct fw_error_worker;	/* for FW error recovery */
@@ -445,6 +556,13 @@ struct wil6210_priv {
 	spinlock_t wmi_ev_lock;
 	struct napi_struct napi_rx;
 	struct napi_struct napi_tx;
+	/* BACK */
+	struct list_head back_rx_pending;
+	struct mutex back_rx_mutex; /* protect @back_rx_pending */
+	struct work_struct back_rx_worker;
+	struct list_head back_tx_pending;
+	struct mutex back_tx_mutex; /* protect @back_tx_pending */
+	struct work_struct back_tx_worker;
 	/* DMA related */
 	struct vring vring_rx;
 	struct vring vring_tx[WIL6210_MAX_TX_RINGS];
@@ -529,7 +647,6 @@ void wil_if_remove(struct wil6210_priv *wil);
 int wil_priv_init(struct wil6210_priv *wil);
 void wil_priv_deinit(struct wil6210_priv *wil);
 int wil_reset(struct wil6210_priv *wil);
-void wil_set_itr_trsh(struct wil6210_priv *wil);
 void wil_fw_error_recovery(struct wil6210_priv *wil);
 void wil_set_recovery_state(struct wil6210_priv *wil, int state);
 void wil_link_on(struct wil6210_priv *wil);
@@ -567,12 +684,26 @@ int wmi_p2p_cfg(struct wil6210_priv *wil, int channel);
 int wmi_rxon(struct wil6210_priv *wil, bool on);
 int wmi_get_temperature(struct wil6210_priv *wil, u32 *t_m, u32 *t_r);
 int wmi_disconnect_sta(struct wil6210_priv *wil, const u8 *mac, u16 reason);
+int wmi_addba(struct wil6210_priv *wil, u8 ringid, u8 size, u16 timeout);
+int wmi_delba_tx(struct wil6210_priv *wil, u8 ringid, u16 reason);
+int wmi_delba_rx(struct wil6210_priv *wil, u8 cidxtid, u16 reason);
+int wmi_addba_rx_resp(struct wil6210_priv *wil, u8 cid, u8 tid, u8 token,
+		      u16 status, bool amsdu, u16 agg_wsize, u16 timeout);
+int wil_addba_rx_request(struct wil6210_priv *wil, u8 cidxtid,
+			 u8 dialog_token, __le16 ba_param_set,
+			 __le16 ba_timeout, __le16 ba_seq_ctrl);
+void wil_back_rx_worker(struct work_struct *work);
+void wil_back_rx_flush(struct wil6210_priv *wil);
+int wil_addba_tx_request(struct wil6210_priv *wil, u8 ringid, u16 wsize);
+void wil_back_tx_worker(struct work_struct *work);
+void wil_back_tx_flush(struct wil6210_priv *wil);
 
 void wil6210_clear_irq(struct wil6210_priv *wil);
 int wil6210_init_irq(struct wil6210_priv *wil, int irq);
 void wil6210_fini_irq(struct wil6210_priv *wil, int irq);
 void wil_mask_irq(struct wil6210_priv *wil);
 void wil_unmask_irq(struct wil6210_priv *wil);
+void wil_configure_interrupt_moderation(struct wil6210_priv *wil);
 void wil_disable_irq(struct wil6210_priv *wil);
 void wil_enable_irq(struct wil6210_priv *wil);
 int wil_cfg80211_mgmt_tx(struct wiphy *wiphy, struct wireless_dev *wdev,
diff --git a/drivers/net/wireless/ath/wil6210/wmi.c b/drivers/net/wireless/ath/wil6210/wmi.c
index 899754920955..b2b0fe1faa96 100644
--- a/drivers/net/wireless/ath/wil6210/wmi.c
+++ b/drivers/net/wireless/ath/wil6210/wmi.c
@@ -23,10 +23,15 @@
 #include "wmi.h"
 #include "trace.h"
 
-static uint max_assoc_sta = 1;
+static uint max_assoc_sta = WIL6210_MAX_CID;
 module_param(max_assoc_sta, uint, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(max_assoc_sta, " Max number of stations associated to the AP");
 
+int agg_wsize; /* = 0; */
+module_param(agg_wsize, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(agg_wsize, " Window size for Tx Block Ack after connect;"
+		 " 0 - use default; < 0 - don't auto-establish");
+
 /**
  * WMI event receiving - theory of operations
  *
@@ -197,7 +202,7 @@ static int __wmi_send(struct wil6210_priv *wil, u16 cmdid, void *buf, u16 len)
 
 	might_sleep();
 
-	if (!test_bit(wil_status_fwready, &wil->status)) {
+	if (!test_bit(wil_status_fwready, wil->status)) {
 		wil_err(wil, "WMI: cannot send command while FW not ready\n");
 		return -EAGAIN;
 	}
@@ -300,7 +305,7 @@ static void wmi_evt_fw_ready(struct wil6210_priv *wil, int id, void *d,
 	wil_dbg_wmi(wil, "WMI: got FW ready event\n");
 
 	wil_set_recovery_state(wil, fw_recovery_idle);
-	set_bit(wil_status_fwready, &wil->status);
+	set_bit(wil_status_fwready, wil->status);
 	/* let the reset sequence continue */
 	complete(&wil->wmi_ready);
 }
@@ -438,7 +443,7 @@ static void wmi_evt_connect(struct wil6210_priv *wil, int id, void *d, int len)
 
 	if ((wdev->iftype == NL80211_IFTYPE_STATION) ||
 	    (wdev->iftype == NL80211_IFTYPE_P2P_CLIENT)) {
-		if (!test_bit(wil_status_fwconnecting, &wil->status)) {
+		if (!test_bit(wil_status_fwconnecting, wil->status)) {
 			wil_err(wil, "Not in connecting state\n");
 			return;
 		}
@@ -461,8 +466,8 @@ static void wmi_evt_connect(struct wil6210_priv *wil, int id, void *d, int len)
 
 		cfg80211_new_sta(ndev, evt->bssid, &sinfo, GFP_KERNEL);
 	}
-	clear_bit(wil_status_fwconnecting, &wil->status);
-	set_bit(wil_status_fwconnected, &wil->status);
+	clear_bit(wil_status_fwconnecting, wil->status);
+	set_bit(wil_status_fwconnected, wil->status);
 
 	/* FIXME FW can transmit only ucast frames to peer */
 	/* FIXME real ring_id instead of hard coded 0 */
@@ -470,7 +475,7 @@ static void wmi_evt_connect(struct wil6210_priv *wil, int id, void *d, int len)
 	wil->sta[evt->cid].status = wil_sta_conn_pending;
 
 	wil->pending_connect_cid = evt->cid;
-	queue_work(wil->wmi_wq_conn, &wil->connect_worker);
+	queue_work(wil->wq_service, &wil->connect_worker);
 }
 
 static void wmi_evt_disconnect(struct wil6210_priv *wil, int id,
@@ -543,6 +548,22 @@ static void wmi_evt_eapol_rx(struct wil6210_priv *wil, int id,
 	}
 }
 
+static void wil_addba_tx_cid(struct wil6210_priv *wil, u8 cid, u16 wsize)
+{
+	struct vring_tx_data *t;
+	int i;
+
+	for (i = 0; i < WIL6210_MAX_TX_RINGS; i++) {
+		if (cid != wil->vring2cid_tid[i][0])
+			continue;
+		t = &wil->vring_tx_data[i];
+		if (!t->enabled)
+			continue;
+
+		wil_addba_tx_request(wil, i, wsize);
+	}
+}
+
 static void wmi_evt_linkup(struct wil6210_priv *wil, int id, void *d, int len)
 {
 	struct net_device *ndev = wil_to_ndev(wil);
@@ -557,6 +578,8 @@ static void wmi_evt_linkup(struct wil6210_priv *wil, int id, void *d, int len)
 	}
 
 	wil->sta[cid].data_port_open = true;
+	if (agg_wsize >= 0)
+		wil_addba_tx_cid(wil, cid, agg_wsize);
 	netif_carrier_on(ndev);
 }
 
@@ -582,55 +605,89 @@ static void wmi_evt_ba_status(struct wil6210_priv *wil, int id, void *d,
 			      int len)
 {
 	struct wmi_vring_ba_status_event *evt = d;
-	struct wil_sta_info *sta;
-	uint i, cid;
-
-	/* TODO: use Rx BA status, not Tx one */
+	struct vring_tx_data *txdata;
 
-	wil_dbg_wmi(wil, "BACK[%d] %s {%d} timeout %d\n",
+	wil_dbg_wmi(wil, "BACK[%d] %s {%d} timeout %d AMSDU%s\n",
 		    evt->ringid,
 		    evt->status == WMI_BA_AGREED ? "OK" : "N/A",
-		    evt->agg_wsize, __le16_to_cpu(evt->ba_timeout));
+		    evt->agg_wsize, __le16_to_cpu(evt->ba_timeout),
+		    evt->amsdu ? "+" : "-");
 
 	if (evt->ringid >= WIL6210_MAX_TX_RINGS) {
 		wil_err(wil, "invalid ring id %d\n", evt->ringid);
 		return;
 	}
 
-	mutex_lock(&wil->mutex);
-
-	cid = wil->vring2cid_tid[evt->ringid][0];
-	if (cid >= WIL6210_MAX_CID) {
-		wil_err(wil, "invalid CID %d for vring %d\n", cid, evt->ringid);
-		goto out;
+	if (evt->status != WMI_BA_AGREED) {
+		evt->ba_timeout = 0;
+		evt->agg_wsize = 0;
+		evt->amsdu = 0;
 	}
 
-	sta = &wil->sta[cid];
-	if (sta->status == wil_sta_unused) {
-		wil_err(wil, "CID %d unused\n", cid);
-		goto out;
-	}
+	txdata = &wil->vring_tx_data[evt->ringid];
 
-	wil_dbg_wmi(wil, "BACK for CID %d %pM\n", cid, sta->addr);
-	for (i = 0; i < WIL_STA_TID_NUM; i++) {
-		struct wil_tid_ampdu_rx *r;
-		unsigned long flags;
+	txdata->agg_timeout = le16_to_cpu(evt->ba_timeout);
+	txdata->agg_wsize = evt->agg_wsize;
+	txdata->agg_amsdu = evt->amsdu;
+	txdata->addba_in_progress = false;
+}
 
-		spin_lock_irqsave(&sta->tid_rx_lock, flags);
+static void wmi_evt_addba_rx_req(struct wil6210_priv *wil, int id, void *d,
+				 int len)
+{
+	struct wmi_rcp_addba_req_event *evt = d;
 
-		r = sta->tid_rx[i];
-		sta->tid_rx[i] = NULL;
-		wil_tid_ampdu_rx_free(wil, r);
+	wil_addba_rx_request(wil, evt->cidxtid, evt->dialog_token,
+			     evt->ba_param_set, evt->ba_timeout,
+			     evt->ba_seq_ctrl);
+}
 
-		spin_unlock_irqrestore(&sta->tid_rx_lock, flags);
+static void wmi_evt_delba(struct wil6210_priv *wil, int id, void *d, int len)
+__acquires(&sta->tid_rx_lock) __releases(&sta->tid_rx_lock)
+{
+	struct wmi_delba_event *evt = d;
+	u8 cid, tid;
+	u16 reason = __le16_to_cpu(evt->reason);
+	struct wil_sta_info *sta;
+	struct wil_tid_ampdu_rx *r;
 
-		if ((evt->status == WMI_BA_AGREED) && evt->agg_wsize)
-			sta->tid_rx[i] = wil_tid_ampdu_rx_alloc(wil,
-						evt->agg_wsize, 0);
+	might_sleep();
+	parse_cidxtid(evt->cidxtid, &cid, &tid);
+	wil_dbg_wmi(wil, "DELBA CID %d TID %d from %s reason %d\n",
+		    cid, tid,
+		    evt->from_initiator ? "originator" : "recipient",
+		    reason);
+	if (!evt->from_initiator) {
+		int i;
+		/* find Tx vring it belongs to */
+		for (i = 0; i < ARRAY_SIZE(wil->vring2cid_tid); i++) {
+			if ((wil->vring2cid_tid[i][0] == cid) &&
+			    (wil->vring2cid_tid[i][1] == tid)) {
+				struct vring_tx_data *txdata =
+					&wil->vring_tx_data[i];
+
+				wil_dbg_wmi(wil, "DELBA Tx vring %d\n", i);
+				txdata->agg_timeout = 0;
+				txdata->agg_wsize = 0;
+				txdata->addba_in_progress = false;
+
+				break; /* max. 1 matching ring */
+			}
+		}
+		if (i >= ARRAY_SIZE(wil->vring2cid_tid))
+			wil_err(wil, "DELBA: unable to find Tx vring\n");
+		return;
 	}
 
-out:
-	mutex_unlock(&wil->mutex);
+	sta = &wil->sta[cid];
+
+	spin_lock_bh(&sta->tid_rx_lock);
+
+	r = sta->tid_rx[tid];
+	sta->tid_rx[tid] = NULL;
+	wil_tid_ampdu_rx_free(wil, r);
+
+	spin_unlock_bh(&sta->tid_rx_lock);
 }
 
 static const struct {
@@ -648,6 +705,8 @@ static const struct {
 	{WMI_DATA_PORT_OPEN_EVENTID,	wmi_evt_linkup},
 	{WMI_WBE_LINKDOWN_EVENTID,	wmi_evt_linkdown},
 	{WMI_BA_STATUS_EVENTID,		wmi_evt_ba_status},
+	{WMI_RCP_ADDBA_REQ_EVENTID,	wmi_evt_addba_rx_req},
+	{WMI_DELBA_EVENTID,		wmi_evt_delba},
 };
 
 /*
@@ -667,7 +726,7 @@ void wmi_recv_cmd(struct wil6210_priv *wil)
 	ulong flags;
 	unsigned n;
 
-	if (!test_bit(wil_status_reset_done, &wil->status)) {
+	if (!test_bit(wil_status_reset_done, wil->status)) {
 		wil_err(wil, "Reset in progress. Cannot handle WMI event\n");
 		return;
 	}
@@ -1024,13 +1083,14 @@ int wmi_rx_chain_add(struct wil6210_priv *wil, struct vring *vring)
 	struct wmi_cfg_rx_chain_cmd cmd = {
 		.action = WMI_RX_CHAIN_ADD,
 		.rx_sw_ring = {
-			.max_mpdu_size = cpu_to_le16(mtu_max + ETH_HLEN),
+			.max_mpdu_size = cpu_to_le16(wil_mtu2macbuf(mtu_max)),
 			.ring_mem_base = cpu_to_le64(vring->pa),
 			.ring_size = cpu_to_le16(vring->size),
 		},
 		.mid = 0, /* TODO - what is it? */
 		.decap_trans_type = WMI_DECAP_TYPE_802_3,
 		.reorder_type = WMI_RX_SW_REORDER,
+		.host_thrsh = cpu_to_le16(rx_ring_overflow_thrsh),
 	};
 	struct {
 		struct wil6210_mbox_hdr_wmi wmi;
@@ -1110,6 +1170,87 @@ int wmi_disconnect_sta(struct wil6210_priv *wil, const u8 *mac, u16 reason)
 	return wmi_send(wil, WMI_DISCONNECT_STA_CMDID, &cmd, sizeof(cmd));
 }
 
+int wmi_addba(struct wil6210_priv *wil, u8 ringid, u8 size, u16 timeout)
+{
+	struct wmi_vring_ba_en_cmd cmd = {
+		.ringid = ringid,
+		.agg_max_wsize = size,
+		.ba_timeout = cpu_to_le16(timeout),
+		.amsdu = 0,
+	};
+
+	wil_dbg_wmi(wil, "%s(ring %d size %d timeout %d)\n", __func__,
+		    ringid, size, timeout);
+
+	return wmi_send(wil, WMI_VRING_BA_EN_CMDID, &cmd, sizeof(cmd));
+}
+
+int wmi_delba_tx(struct wil6210_priv *wil, u8 ringid, u16 reason)
+{
+	struct wmi_vring_ba_dis_cmd cmd = {
+		.ringid = ringid,
+		.reason = cpu_to_le16(reason),
+	};
+
+	wil_dbg_wmi(wil, "%s(ring %d reason %d)\n", __func__,
+		    ringid, reason);
+
+	return wmi_send(wil, WMI_VRING_BA_DIS_CMDID, &cmd, sizeof(cmd));
+}
+
+int wmi_delba_rx(struct wil6210_priv *wil, u8 cidxtid, u16 reason)
+{
+	struct wmi_rcp_delba_cmd cmd = {
+		.cidxtid = cidxtid,
+		.reason = cpu_to_le16(reason),
+	};
+
+	wil_dbg_wmi(wil, "%s(CID %d TID %d reason %d)\n", __func__,
+		    cidxtid & 0xf, (cidxtid >> 4) & 0xf, reason);
+
+	return wmi_send(wil, WMI_RCP_DELBA_CMDID, &cmd, sizeof(cmd));
+}
+
+int wmi_addba_rx_resp(struct wil6210_priv *wil, u8 cid, u8 tid, u8 token,
+		      u16 status, bool amsdu, u16 agg_wsize, u16 timeout)
+{
+	int rc;
+	struct wmi_rcp_addba_resp_cmd cmd = {
+		.cidxtid = mk_cidxtid(cid, tid),
+		.dialog_token = token,
+		.status_code = cpu_to_le16(status),
+		/* bit 0: A-MSDU supported
+		 * bit 1: policy (should be 0 for us)
+		 * bits 2..5: TID
+		 * bits 6..15: buffer size
+		 */
+		.ba_param_set = cpu_to_le16((amsdu ? 1 : 0) | (tid << 2) |
+					    (agg_wsize << 6)),
+		.ba_timeout = cpu_to_le16(timeout),
+	};
+	struct {
+		struct wil6210_mbox_hdr_wmi wmi;
+		struct wmi_rcp_addba_resp_sent_event evt;
+	} __packed reply;
+
+	wil_dbg_wmi(wil,
+		    "ADDBA response for CID %d TID %d size %d timeout %d status %d AMSDU%s\n",
+		    cid, tid, agg_wsize, timeout, status, amsdu ? "+" : "-");
+
+	rc = wmi_call(wil, WMI_RCP_ADDBA_RESP_CMDID, &cmd, sizeof(cmd),
+		      WMI_ADDBA_RESP_SENT_EVENTID, &reply, sizeof(reply), 100);
+	if (rc)
+		return rc;
+
+	if (reply.evt.status) {
+		wil_err(wil, "ADDBA response failed with status %d\n",
+			le16_to_cpu(reply.evt.status));
+		rc = -EINVAL;
+	}
+
+	return rc;
+}
+
 void wmi_event_flush(struct wil6210_priv *wil)
 {
 	struct pending_wmi_event *evt, *t;
diff --git a/drivers/net/wireless/ath/wil6210/wmi.h b/drivers/net/wireless/ath/wil6210/wmi.h
index 27b97432d1c2..b5102f0b97f4 100644
--- a/drivers/net/wireless/ath/wil6210/wmi.h
+++ b/drivers/net/wireless/ath/wil6210/wmi.h
@@ -586,6 +586,7 @@ struct wmi_vring_ba_en_cmd {
 	u8 ringid;
 	u8 agg_max_wsize;
 	__le16 ba_timeout;
+	u8 amsdu;
 } __packed;
 
 /*
@@ -1052,14 +1053,23 @@ struct wmi_scan_complete_event {
 enum wmi_vring_ba_status {
 	WMI_BA_AGREED			= 0,
 	WMI_BA_NON_AGREED		= 1,
+	/* BA_EN in middle of teardown flow */
+	WMI_BA_TD_WIP			= 2,
+	/* BA_DIS or BA_EN in middle of BA SETUP flow */
+	WMI_BA_SETUP_WIP		= 3,
+	/* BA_EN when the BA session is already active */
+	WMI_BA_SESSION_ACTIVE		= 4,
+	/* BA_DIS when the BA session is not active */
+	WMI_BA_SESSION_NOT_ACTIVE	= 5,
 };
 
 struct wmi_vring_ba_status_event {
-	__le16 status;
+	__le16 status; /* enum wmi_vring_ba_status */
 	u8 reserved[2];
 	u8 ringid;
 	u8 agg_wsize;
 	__le16 ba_timeout;
+	u8 amsdu;
 } __packed;
 
 /*
diff --git a/drivers/net/wireless/b43legacy/radio.c b/drivers/net/wireless/b43legacy/radio.c
index 896177690394..9501420340a9 100644
--- a/drivers/net/wireless/b43legacy/radio.c
+++ b/drivers/net/wireless/b43legacy/radio.c
@@ -1743,25 +1743,6 @@ u16 freq_r3A_value(u16 frequency)
 	return value;
 }
 
-void b43legacy_radio_set_tx_iq(struct b43legacy_wldev *dev)
-{
-	static const u8 data_high[5] = { 0x00, 0x40, 0x80, 0x90, 0xD0 };
-	static const u8 data_low[5]  = { 0x00, 0x01, 0x05, 0x06, 0x0A };
-	u16 tmp = b43legacy_radio_read16(dev, 0x001E);
-	int i;
-	int j;
-
-	for (i = 0; i < 5; i++) {
-		for (j = 0; j < 5; j++) {
-			if (tmp == (data_high[i] | data_low[j])) {
-				b43legacy_phy_write(dev, 0x0069, (i - j) << 8 |
-						    0x00C0);
-				return;
-			}
-		}
-	}
-}
-
 int b43legacy_radio_selectchannel(struct b43legacy_wldev *dev,
 				  u8 channel,
 				  int synthetic_pu_workaround)
diff --git a/drivers/net/wireless/b43legacy/radio.h b/drivers/net/wireless/b43legacy/radio.h
index bccb3d7da682..dd2976d1d561 100644
--- a/drivers/net/wireless/b43legacy/radio.h
+++ b/drivers/net/wireless/b43legacy/radio.h
@@ -92,7 +92,6 @@ void b43legacy_nrssi_hw_write(struct b43legacy_wldev *dev, u16 offset, s16 val);
 void b43legacy_nrssi_hw_update(struct b43legacy_wldev *dev, u16 val);
 void b43legacy_nrssi_mem_update(struct b43legacy_wldev *dev);
 
-void b43legacy_radio_set_tx_iq(struct b43legacy_wldev *dev);
 u16 b43legacy_radio_calibrationvalue(struct b43legacy_wldev *dev);
 
 #endif /* B43legacy_RADIO_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
index f754ffcd0308..00ba90b89455 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
@@ -996,18 +996,20 @@ out:
 }
 
 #define BRCMF_SDIO_DEVICE(dev_id)	\
-	{SDIO_DEVICE(BRCM_SDIO_VENDOR_ID_BROADCOM, dev_id)}
+	{SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, dev_id)}
 
 /* devices we support, null terminated */
 static const struct sdio_device_id brcmf_sdmmc_ids[] = {
-	BRCMF_SDIO_DEVICE(BRCM_SDIO_43143_DEVICE_ID),
-	BRCMF_SDIO_DEVICE(BRCM_SDIO_43241_DEVICE_ID),
-	BRCMF_SDIO_DEVICE(BRCM_SDIO_4329_DEVICE_ID),
-	BRCMF_SDIO_DEVICE(BRCM_SDIO_4330_DEVICE_ID),
-	BRCMF_SDIO_DEVICE(BRCM_SDIO_4334_DEVICE_ID),
-	BRCMF_SDIO_DEVICE(BRCM_SDIO_43362_DEVICE_ID),
-	BRCMF_SDIO_DEVICE(BRCM_SDIO_4335_4339_DEVICE_ID),
-	BRCMF_SDIO_DEVICE(BRCM_SDIO_4354_DEVICE_ID),
+	BRCMF_SDIO_DEVICE(SDIO_DEVICE_ID_BROADCOM_43143),
+	BRCMF_SDIO_DEVICE(SDIO_DEVICE_ID_BROADCOM_43241),
+	BRCMF_SDIO_DEVICE(SDIO_DEVICE_ID_BROADCOM_4329),
+	BRCMF_SDIO_DEVICE(SDIO_DEVICE_ID_BROADCOM_4330),
+	BRCMF_SDIO_DEVICE(SDIO_DEVICE_ID_BROADCOM_4334),
+	BRCMF_SDIO_DEVICE(SDIO_DEVICE_ID_BROADCOM_43340),
+	BRCMF_SDIO_DEVICE(SDIO_DEVICE_ID_BROADCOM_43341),
+	BRCMF_SDIO_DEVICE(SDIO_DEVICE_ID_BROADCOM_43362),
+	BRCMF_SDIO_DEVICE(SDIO_DEVICE_ID_BROADCOM_4335_4339),
+	BRCMF_SDIO_DEVICE(SDIO_DEVICE_ID_BROADCOM_4354),
 	{ /* end: all zeroes */ }
 };
 MODULE_DEVICE_TABLE(sdio, brcmf_sdmmc_ids);
@@ -1070,7 +1072,7 @@ static int brcmf_ops_sdio_probe(struct sdio_func *func,
 	 */
 	if ((sdio_get_host_pm_caps(sdiodev->func[1]) & MMC_PM_KEEP_POWER) &&
 	    ((sdio_get_host_pm_caps(sdiodev->func[1]) & MMC_PM_WAKE_SDIO_IRQ) ||
-	     (sdiodev->pdata->oob_irq_supported)))
+	     (sdiodev->pdata && sdiodev->pdata->oob_irq_supported)))
 		bus_if->wowl_supported = true;
 #endif
 
@@ -1167,7 +1169,7 @@ static int brcmf_ops_sdio_resume(struct device *dev)
 	struct brcmf_sdio_dev *sdiodev = bus_if->bus_priv.sdio;
 
 	brcmf_dbg(SDIO, "Enter\n");
-	if (sdiodev->pdata->oob_irq_supported)
+	if (sdiodev->pdata && sdiodev->pdata->oob_irq_supported)
 		disable_irq_wake(sdiodev->pdata->oob_irq_nr);
 	brcmf_sdio_wd_timer(sdiodev->bus, BRCMF_WD_POLL_MS);
 	atomic_set(&sdiodev->suspend, false);
@@ -1221,7 +1223,6 @@ static struct platform_driver brcmf_sdio_pd = {
 	.remove		= brcmf_sdio_pd_remove,
 	.driver		= {
 		.name	= BRCMFMAC_SDIO_PDATA_NAME,
-		.owner	= THIS_MODULE,
 	}
 };
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.c b/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.c
index 4a88b2381a68..ebe654312b74 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.c
@@ -38,6 +38,7 @@
 #include "proto.h"
 #include "vendor.h"
 #include "bus.h"
+#include "common.h"
 
 #define BRCMF_SCAN_IE_LEN_MAX		2048
 #define BRCMF_PNO_VERSION		2
@@ -452,16 +453,16 @@ static void convert_key_from_CPU(struct brcmf_wsec_key *key,
 }
 
 static int
-send_key_to_dongle(struct net_device *ndev, struct brcmf_wsec_key *key)
+send_key_to_dongle(struct brcmf_if *ifp, struct brcmf_wsec_key *key)
 {
 	int err;
 	struct brcmf_wsec_key_le key_le;
 
 	convert_key_from_CPU(key, &key_le);
 
-	brcmf_netdev_wait_pend8021x(ndev);
+	brcmf_netdev_wait_pend8021x(ifp);
 
-	err = brcmf_fil_bsscfg_data_set(netdev_priv(ndev), "wsec_key", &key_le,
+	err = brcmf_fil_bsscfg_data_set(ifp, "wsec_key", &key_le,
 					sizeof(key_le));
 
 	if (err)
@@ -1670,7 +1671,7 @@ brcmf_set_sharedkey(struct net_device *ndev,
 	brcmf_dbg(CONN, "key length (%d) key index (%d) algo (%d)\n",
 		  key.len, key.index, key.algo);
 	brcmf_dbg(CONN, "key \"%s\"\n", key.data);
-	err = send_key_to_dongle(ndev, &key);
+	err = send_key_to_dongle(netdev_priv(ndev), &key);
 	if (err)
 		return err;
 
@@ -2052,7 +2053,7 @@ brcmf_add_keyext(struct wiphy *wiphy, struct net_device *ndev,
 	/* check for key index change */
 	if (key.len == 0) {
 		/* key delete */
-		err = send_key_to_dongle(ndev, &key);
+		err = send_key_to_dongle(ifp, &key);
 		if (err)
 			brcmf_err("key delete error (%d)\n", err);
 	} else {
@@ -2108,7 +2109,7 @@ brcmf_add_keyext(struct wiphy *wiphy, struct net_device *ndev,
 			brcmf_err("Invalid cipher (0x%x)\n", params->cipher);
 			return -EINVAL;
 		}
-		err = send_key_to_dongle(ndev, &key);
+		err = send_key_to_dongle(ifp, &key);
 		if (err)
 			brcmf_err("wsec_key error (%d)\n", err);
 	}
@@ -2121,7 +2122,7 @@ brcmf_cfg80211_add_key(struct wiphy *wiphy, struct net_device *ndev,
 		    struct key_params *params)
 {
 	struct brcmf_if *ifp = netdev_priv(ndev);
-	struct brcmf_wsec_key key;
+	struct brcmf_wsec_key *key;
 	s32 val;
 	s32 wsec;
 	s32 err = 0;
@@ -2132,54 +2133,62 @@ brcmf_cfg80211_add_key(struct wiphy *wiphy, struct net_device *ndev,
 	if (!check_vif_up(ifp->vif))
 		return -EIO;
 
+	if (key_idx >= BRCMF_MAX_DEFAULT_KEYS) {
+		/* we ignore this key index in this case */
+		brcmf_err("invalid key index (%d)\n", key_idx);
+		return -EINVAL;
+	}
+
 	if (mac_addr &&
 		(params->cipher != WLAN_CIPHER_SUITE_WEP40) &&
 		(params->cipher != WLAN_CIPHER_SUITE_WEP104)) {
 		brcmf_dbg(TRACE, "Exit");
 		return brcmf_add_keyext(wiphy, ndev, key_idx, mac_addr, params);
 	}
-	memset(&key, 0, sizeof(key));
 
-	key.len = (u32) params->key_len;
-	key.index = (u32) key_idx;
+	key = &ifp->vif->profile.key[key_idx];
+	memset(key, 0, sizeof(*key));
 
-	if (key.len > sizeof(key.data)) {
-		brcmf_err("Too long key length (%u)\n", key.len);
+	if (params->key_len > sizeof(key->data)) {
+		brcmf_err("Too long key length (%u)\n", params->key_len);
 		err = -EINVAL;
 		goto done;
 	}
-	memcpy(key.data, params->key, key.len);
+	key->len = params->key_len;
+	key->index = key_idx;
 
-	key.flags = BRCMF_PRIMARY_KEY;
+	memcpy(key->data, params->key, key->len);
+
+	key->flags = BRCMF_PRIMARY_KEY;
 	switch (params->cipher) {
 	case WLAN_CIPHER_SUITE_WEP40:
-		key.algo = CRYPTO_ALGO_WEP1;
+		key->algo = CRYPTO_ALGO_WEP1;
 		val = WEP_ENABLED;
 		brcmf_dbg(CONN, "WLAN_CIPHER_SUITE_WEP40\n");
 		break;
 	case WLAN_CIPHER_SUITE_WEP104:
-		key.algo = CRYPTO_ALGO_WEP128;
+		key->algo = CRYPTO_ALGO_WEP128;
 		val = WEP_ENABLED;
 		brcmf_dbg(CONN, "WLAN_CIPHER_SUITE_WEP104\n");
 		break;
 	case WLAN_CIPHER_SUITE_TKIP:
 		if (!brcmf_is_apmode(ifp->vif)) {
 			brcmf_dbg(CONN, "Swapping RX/TX MIC key\n");
-			memcpy(keybuf, &key.data[24], sizeof(keybuf));
-			memcpy(&key.data[24], &key.data[16], sizeof(keybuf));
-			memcpy(&key.data[16], keybuf, sizeof(keybuf));
+			memcpy(keybuf, &key->data[24], sizeof(keybuf));
+			memcpy(&key->data[24], &key->data[16], sizeof(keybuf));
+			memcpy(&key->data[16], keybuf, sizeof(keybuf));
 		}
-		key.algo = CRYPTO_ALGO_TKIP;
+		key->algo = CRYPTO_ALGO_TKIP;
 		val = TKIP_ENABLED;
 		brcmf_dbg(CONN, "WLAN_CIPHER_SUITE_TKIP\n");
 		break;
 	case WLAN_CIPHER_SUITE_AES_CMAC:
-		key.algo = CRYPTO_ALGO_AES_CCM;
+		key->algo = CRYPTO_ALGO_AES_CCM;
 		val = AES_ENABLED;
 		brcmf_dbg(CONN, "WLAN_CIPHER_SUITE_AES_CMAC\n");
 		break;
 	case WLAN_CIPHER_SUITE_CCMP:
-		key.algo = CRYPTO_ALGO_AES_CCM;
+		key->algo = CRYPTO_ALGO_AES_CCM;
 		val = AES_ENABLED;
 		brcmf_dbg(CONN, "WLAN_CIPHER_SUITE_CCMP\n");
 		break;
@@ -2189,7 +2198,7 @@ brcmf_cfg80211_add_key(struct wiphy *wiphy, struct net_device *ndev,
 		goto done;
 	}
 
-	err = send_key_to_dongle(ndev, &key);
+	err = send_key_to_dongle(ifp, key);
 	if (err)
 		goto done;
 
@@ -2222,7 +2231,7 @@ brcmf_cfg80211_del_key(struct wiphy *wiphy, struct net_device *ndev,
 	if (!check_vif_up(ifp->vif))
 		return -EIO;
 
-	if (key_idx >= DOT11_MAX_DEFAULT_KEYS) {
+	if (key_idx >= BRCMF_MAX_DEFAULT_KEYS) {
 		/* we ignore this key index in this case */
 		brcmf_err("invalid key index (%d)\n", key_idx);
 		return -EINVAL;
@@ -2237,7 +2246,7 @@ brcmf_cfg80211_del_key(struct wiphy *wiphy, struct net_device *ndev,
 	brcmf_dbg(CONN, "key index (%d)\n", key_idx);
 
 	/* Set the new key/index */
-	err = send_key_to_dongle(ndev, &key);
+	err = send_key_to_dongle(ifp, &key);
 
 	brcmf_dbg(TRACE, "Exit\n");
 	return err;
@@ -2305,6 +2314,39 @@ brcmf_cfg80211_config_default_mgmt_key(struct wiphy *wiphy,
 	return -EOPNOTSUPP;
 }
 
+static void
+brcmf_cfg80211_reconfigure_wep(struct brcmf_if *ifp)
+{
+	s32 err;
+	u8 key_idx;
+	struct brcmf_wsec_key *key;
+	s32 wsec;
+
+	for (key_idx = 0; key_idx < BRCMF_MAX_DEFAULT_KEYS; key_idx++) {
+		key = &ifp->vif->profile.key[key_idx];
+		if ((key->algo == CRYPTO_ALGO_WEP1) ||
+		    (key->algo == CRYPTO_ALGO_WEP128))
+			break;
+	}
+	if (key_idx == BRCMF_MAX_DEFAULT_KEYS)
+		return;
+
+	err = send_key_to_dongle(ifp, key);
+	if (err) {
+		brcmf_err("Setting WEP key failed (%d)\n", err);
+		return;
+	}
+	err = brcmf_fil_bsscfg_int_get(ifp, "wsec", &wsec);
+	if (err) {
+		brcmf_err("get wsec error (%d)\n", err);
+		return;
+	}
+	wsec |= WEP_ENABLED;
+	err = brcmf_fil_bsscfg_int_set(ifp, "wsec", wsec);
+	if (err)
+		brcmf_err("set wsec error (%d)\n", err);
+}
+
 static s32
 brcmf_cfg80211_get_station(struct wiphy *wiphy, struct net_device *ndev,
 			   const u8 *mac, struct station_info *sinfo)
@@ -3924,6 +3966,7 @@ brcmf_cfg80211_start_ap(struct wiphy *wiphy, struct net_device *ndev,
 	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
 	struct brcmf_if *ifp = netdev_priv(ndev);
 	const struct brcmf_tlv *ssid_ie;
+	const struct brcmf_tlv *country_ie;
 	struct brcmf_ssid_le ssid_le;
 	s32 err = -EPERM;
 	const struct brcmf_tlv *rsn_ie;
@@ -3933,6 +3976,7 @@ brcmf_cfg80211_start_ap(struct wiphy *wiphy, struct net_device *ndev,
 	struct brcmf_fil_bss_enable_le bss_enable;
 	u16 chanspec;
 	bool mbss;
+	int is_11d;
 
 	brcmf_dbg(TRACE, "ctrlchn=%d, center=%d, bw=%d, beacon_interval=%d, dtim_period=%d,\n",
 		  settings->chandef.chan->hw_value,
@@ -3941,10 +3985,16 @@ brcmf_cfg80211_start_ap(struct wiphy *wiphy, struct net_device *ndev,
 	brcmf_dbg(TRACE, "ssid=%s(%zu), auth_type=%d, inactivity_timeout=%d\n",
 		  settings->ssid, settings->ssid_len, settings->auth_type,
 		  settings->inactivity_timeout);
-
 	dev_role = ifp->vif->wdev.iftype;
 	mbss = ifp->vif->mbss;
 
+	/* store current 11d setting */
+	brcmf_fil_cmd_int_get(ifp, BRCMF_C_GET_REGULATORY, &ifp->vif->is_11d);
+	country_ie = brcmf_parse_tlvs((u8 *)settings->beacon.tail,
+				      settings->beacon.tail_len,
+				      WLAN_EID_COUNTRY);
+	is_11d = country_ie ? 1 : 0;
+
 	memset(&ssid_le, 0, sizeof(ssid_le));
 	if (settings->ssid == NULL || settings->ssid_len == 0) {
 		ie_offset = DOT11_MGMT_HDR_LEN + DOT11_BCN_PRB_FIXED_LEN;
@@ -4010,6 +4060,14 @@ brcmf_cfg80211_start_ap(struct wiphy *wiphy, struct net_device *ndev,
 			goto exit;
 		}
 
+		if (is_11d != ifp->vif->is_11d) {
+			err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_REGULATORY,
+						    is_11d);
+			if (err < 0) {
+				brcmf_err("Regulatory Set Error, %d\n", err);
+				goto exit;
+			}
+		}
 		if (settings->beacon_interval) {
 			err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_BCNPRD,
 						    settings->beacon_interval);
@@ -4042,6 +4100,10 @@ brcmf_cfg80211_start_ap(struct wiphy *wiphy, struct net_device *ndev,
 			brcmf_err("SET INFRA error %d\n", err);
 			goto exit;
 		}
+	} else if (WARN_ON(is_11d != ifp->vif->is_11d)) {
+		/* Multiple-BSS should use same 11d configuration */
+		err = -EINVAL;
+		goto exit;
 	}
 	if (dev_role == NL80211_IFTYPE_AP) {
 		if ((brcmf_feat_is_enabled(ifp, BRCMF_FEAT_MBSS)) && (!mbss))
@@ -4057,6 +4119,10 @@ brcmf_cfg80211_start_ap(struct wiphy *wiphy, struct net_device *ndev,
 			brcmf_err("BRCMF_C_UP error (%d)\n", err);
 			goto exit;
 		}
+		/* On DOWN the firmware removes the WEP keys, reconfigure
+		 * them if they were set.
+		 */
+		brcmf_cfg80211_reconfigure_wep(ifp);
 
 		memset(&join_params, 0, sizeof(join_params));
 		/* join parameters starts with ssid */
@@ -4133,6 +4199,11 @@ static int brcmf_cfg80211_stop_ap(struct wiphy *wiphy, struct net_device *ndev)
 			brcmf_err("setting INFRA mode failed %d\n", err);
 		if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_MBSS))
 			brcmf_fil_iovar_int_set(ifp, "mbss", 0);
+		err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_REGULATORY,
+					    ifp->vif->is_11d);
+		if (err < 0)
+			brcmf_err("restoring REGULATORY setting failed %d\n",
+				  err);
 		/* Bring device back up so it can be used again */
 		err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_UP, 1);
 		if (err < 0)
@@ -4197,6 +4268,34 @@ brcmf_cfg80211_del_station(struct wiphy *wiphy, struct net_device *ndev,
 	return err;
 }
 
+static int
+brcmf_cfg80211_change_station(struct wiphy *wiphy, struct net_device *ndev,
+			      const u8 *mac, struct station_parameters *params)
+{
+	struct brcmf_if *ifp = netdev_priv(ndev);
+	s32 err;
+
+	brcmf_dbg(TRACE, "Enter, MAC %pM, mask 0x%04x set 0x%04x\n", mac,
+		  params->sta_flags_mask, params->sta_flags_set);
+
+	/* Ignore all 00 MAC */
+	if (is_zero_ether_addr(mac))
+		return 0;
+
+	if (!(params->sta_flags_mask & BIT(NL80211_STA_FLAG_AUTHORIZED)))
+		return 0;
+
+	if (params->sta_flags_set & BIT(NL80211_STA_FLAG_AUTHORIZED))
+		err = brcmf_fil_cmd_data_set(ifp, BRCMF_C_SET_SCB_AUTHORIZE,
+					     (void *)mac, ETH_ALEN);
+	else
+		err = brcmf_fil_cmd_data_set(ifp, BRCMF_C_SET_SCB_DEAUTHORIZE,
+					     (void *)mac, ETH_ALEN);
+	if (err < 0)
+		brcmf_err("Setting SCB (de-)authorize failed, %d\n", err);
+
+	return err;
+}
 
 static void
 brcmf_cfg80211_mgmt_frame_register(struct wiphy *wiphy,
@@ -4471,6 +4570,7 @@ static struct cfg80211_ops wl_cfg80211_ops = {
 	.stop_ap = brcmf_cfg80211_stop_ap,
 	.change_beacon = brcmf_cfg80211_change_beacon,
 	.del_station = brcmf_cfg80211_del_station,
+	.change_station = brcmf_cfg80211_change_station,
 	.sched_scan_start = brcmf_cfg80211_sched_scan_start,
 	.sched_scan_stop = brcmf_cfg80211_sched_scan_stop,
 	.mgmt_frame_register = brcmf_cfg80211_mgmt_frame_register,
@@ -5875,6 +5975,29 @@ int brcmf_cfg80211_wait_vif_event_timeout(struct brcmf_cfg80211_info *cfg,
 				  vif_event_equals(event, action), timeout);
 }
 
+static void brcmf_cfg80211_reg_notifier(struct wiphy *wiphy,
+					struct regulatory_request *req)
+{
+	struct brcmf_cfg80211_info *cfg = wiphy_priv(wiphy);
+	struct brcmf_if *ifp = netdev_priv(cfg_to_ndev(cfg));
+	struct brcmf_fil_country_le ccreq;
+	int i;
+
+	brcmf_dbg(TRACE, "enter: initiator=%d, alpha=%c%c\n", req->initiator,
+		  req->alpha2[0], req->alpha2[1]);
+
+	/* ignore non-ISO3166 country codes */
+	for (i = 0; i < sizeof(req->alpha2); i++)
+		if (req->alpha2[i] < 'A' || req->alpha2[i] > 'Z') {
+			brcmf_err("not a ISO3166 code\n");
+			return;
+		}
+	memset(&ccreq, 0, sizeof(ccreq));
+	ccreq.rev = cpu_to_le32(-1);
+	memcpy(ccreq.ccode, req->alpha2, sizeof(req->alpha2));
+	brcmf_fil_iovar_data_set(ifp, "country", &ccreq, sizeof(ccreq));
+}
+
 static void brcmf_free_wiphy(struct wiphy *wiphy)
 {
 	kfree(wiphy->iface_combinations);
@@ -5951,6 +6074,7 @@ struct brcmf_cfg80211_info *brcmf_cfg80211_attach(struct brcmf_pub *drvr,
 		goto priv_out;
 
 	brcmf_dbg(INFO, "Registering custom regulatory\n");
+	wiphy->reg_notifier = brcmf_cfg80211_reg_notifier;
 	wiphy->regulatory_flags |= REGULATORY_CUSTOM_REG;
 	wiphy_apply_custom_regulatory(wiphy, &brcmf_regdom);
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.h b/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.h
index 9e98b8d52757..d9e6d01b2b69 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.h
@@ -75,6 +75,8 @@
 
 #define BRCMF_VNDR_IE_P2PAF_SHIFT	12
 
+#define BRCMF_MAX_DEFAULT_KEYS		4
+
 
 /**
  * enum brcmf_scan_status - scan engine status
@@ -125,11 +127,13 @@ struct brcmf_cfg80211_security {
  * @ssid: ssid of associated/associating ap.
  * @bssid: bssid of joined/joining ibss.
  * @sec: security information.
+ * @key: key information
  */
 struct brcmf_cfg80211_profile {
 	struct brcmf_ssid ssid;
 	u8 bssid[ETH_ALEN];
 	struct brcmf_cfg80211_security sec;
+	struct brcmf_wsec_key key[BRCMF_MAX_DEFAULT_KEYS];
 };
 
 /**
@@ -196,6 +200,7 @@ struct brcmf_cfg80211_vif {
 	struct list_head list;
 	u16 mgmt_rx_reg;
 	bool mbss;
+	int is_11d;
 };
 
 /* association inform */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/chip.c b/drivers/net/wireless/brcm80211/brcmfmac/chip.c
index ddae0b5e56ec..04d2ca0d87d6 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/chip.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/chip.c
@@ -101,14 +101,7 @@
 /* ARM Cortex M3 core, ID 0x82a */
 #define BCM4329_CORE_ARM_BASE		0x18002000
 #define BCM4329_RAMSIZE			0x48000
-
 /* bcm43143 */
-/* SDIO device core */
-#define BCM43143_CORE_BUS_BASE		0x18002000
-/* internal memory core */
-#define BCM43143_CORE_SOCRAM_BASE	0x18004000
-/* ARM Cortex M3 core, ID 0x82a */
-#define BCM43143_CORE_ARM_BASE		0x18003000
 #define BCM43143_RAMSIZE		0x70000
 
 #define CORE_SB(base, field) \
@@ -164,13 +157,6 @@ struct brcmf_core_priv {
 	struct brcmf_chip_priv *chip;
 };
 
-/* ARM CR4 core specific control flag bits */
-#define ARMCR4_BCMA_IOCTL_CPUHALT	0x0020
-
-/* D11 core specific control flag bits */
-#define D11_BCMA_IOCTL_PHYCLOCKEN	0x0004
-#define D11_BCMA_IOCTL_PHYRESET		0x0008
-
 struct brcmf_chip_priv {
 	struct brcmf_chip pub;
 	const struct brcmf_buscore_ops *ops;
@@ -495,6 +481,7 @@ static void brcmf_chip_get_raminfo(struct brcmf_chip_priv *ci)
 		ci->pub.ramsize = 0x48000;
 		break;
 	case BRCM_CC_4334_CHIP_ID:
+	case BRCM_CC_43340_CHIP_ID:
 		ci->pub.ramsize = 0x80000;
 		break;
 	case BRCM_CC_4335_CHIP_ID:
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/common.c b/drivers/net/wireless/brcm80211/brcmfmac/common.c
index 1861a13e8d03..ddf05af13d44 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/common.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/common.c
@@ -25,6 +25,9 @@
 #include "fwil.h"
 #include "fwil_types.h"
 #include "tracepoint.h"
+#include "common.h"
+
+const u8 ALLFFMAC[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
 
 #define BRCMF_DEFAULT_BCN_TIMEOUT	3
 #define BRCMF_DEFAULT_SCAN_CHANNEL_TIME	40
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/common.h b/drivers/net/wireless/brcm80211/brcmfmac/common.h
new file mode 100644
index 000000000000..0d39d80cee28
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/brcmfmac/common.h
@@ -0,0 +1,20 @@
+/* Copyright (c) 2014 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 BRCMFMAC_COMMON_H
+#define BRCMFMAC_COMMON_H
+
+extern const u8 ALLFFMAC[ETH_ALEN];
+
+#endif /* BRCMFMAC_COMMON_H */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/core.c b/drivers/net/wireless/brcm80211/brcmfmac/core.c
index effe6d7831d9..e2a9e33f71ab 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/core.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/core.c
@@ -1093,9 +1093,8 @@ static int brcmf_get_pend_8021x_cnt(struct brcmf_if *ifp)
 	return atomic_read(&ifp->pend_8021x_cnt);
 }
 
-int brcmf_netdev_wait_pend8021x(struct net_device *ndev)
+int brcmf_netdev_wait_pend8021x(struct brcmf_if *ifp)
 {
-	struct brcmf_if *ifp = netdev_priv(ndev);
 	int err;
 
 	err = wait_event_timeout(ifp->pend_8021x_wait,
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/core.h b/drivers/net/wireless/brcm80211/brcmfmac/core.h
index 23f74b139cc8..f2f7d3d1a8ef 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/core.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/core.h
@@ -29,8 +29,6 @@
 /* For supporting multiple interfaces */
 #define BRCMF_MAX_IFS	16
 
-#define DOT11_MAX_DEFAULT_KEYS	4
-
 /* Small, medium and maximum buffer size for dcmd
  */
 #define BRCMF_DCMD_SMLEN	256
@@ -167,7 +165,7 @@ struct brcmf_skb_reorder_data {
 	u8 *reorder;
 };
 
-int brcmf_netdev_wait_pend8021x(struct net_device *ndev);
+int brcmf_netdev_wait_pend8021x(struct brcmf_if *ifp);
 
 /* Return pointer to interface name */
 char *brcmf_ifname(struct brcmf_pub *drvr, int idx);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/flowring.c b/drivers/net/wireless/brcm80211/brcmfmac/flowring.c
index 44f3a84d1999..910fbb561469 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/flowring.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/flowring.c
@@ -25,6 +25,7 @@
 #include "proto.h"
 #include "flowring.h"
 #include "msgbuf.h"
+#include "common.h"
 
 
 #define BRCMF_FLOWRING_HIGH		1024
@@ -34,9 +35,6 @@
 #define BRCMF_FLOWRING_HASH_AP(da, fifo, ifidx) (da[5] + fifo + ifidx * 16)
 #define BRCMF_FLOWRING_HASH_STA(fifo, ifidx) (fifo + ifidx * 16)
 
-static const u8 ALLZEROMAC[ETH_ALEN] = { 0, 0, 0, 0, 0, 0 };
-static const u8 ALLFFMAC[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
-
 static const u8 brcmf_flowring_prio2fifo[] = {
 	1,
 	0,
@@ -137,7 +135,7 @@ u32 brcmf_flowring_create(struct brcmf_flowring *flow, u8 da[ETH_ALEN],
 	hash = flow->hash;
 	for (i = 0; i < BRCMF_FLOWRING_HASHSIZE; i++) {
 		if ((hash[hash_idx].ifidx == BRCMF_FLOWRING_INVALID_IFIDX) &&
-		    (memcmp(hash[hash_idx].mac, ALLZEROMAC, ETH_ALEN) == 0)) {
+		    (is_zero_ether_addr(hash[hash_idx].mac))) {
 			found = true;
 			break;
 		}
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fwil.h b/drivers/net/wireless/brcm80211/brcmfmac/fwil.h
index a30be683f4a1..37345e7b873d 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fwil.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/fwil.h
@@ -43,6 +43,8 @@
 #define BRCMF_C_SET_RADIO			38
 #define BRCMF_C_GET_PHYTYPE			39
 #define BRCMF_C_SET_KEY				45
+#define BRCMF_C_GET_REGULATORY			46
+#define BRCMF_C_SET_REGULATORY			47
 #define BRCMF_C_SET_PASSIVE_SCAN		49
 #define BRCMF_C_SCAN				50
 #define BRCMF_C_SCAN_RESULTS			51
@@ -60,6 +62,8 @@
 #define BRCMF_C_GET_CURR_RATESET		114
 #define BRCMF_C_GET_AP				117
 #define BRCMF_C_SET_AP				118
+#define BRCMF_C_SET_SCB_AUTHORIZE		121
+#define BRCMF_C_SET_SCB_DEAUTHORIZE		122
 #define BRCMF_C_GET_RSSI			127
 #define BRCMF_C_GET_WSEC			133
 #define BRCMF_C_SET_WSEC			134
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fwil_types.h b/drivers/net/wireless/brcm80211/brcmfmac/fwil_types.h
index 50891c02c4c1..619669bbdb83 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fwil_types.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/fwil_types.h
@@ -112,6 +112,7 @@
 #define BRCMF_WOWL_MAXPATTERNS		8
 #define BRCMF_WOWL_MAXPATTERNSIZE	128
 
+#define BRCMF_COUNTRY_BUF_SZ		4
 
 /* join preference types for join_pref iovar */
 enum brcmf_join_pref_types {
@@ -525,4 +526,17 @@ struct brcmf_mbss_ssid_le {
 	unsigned char SSID[32];
 };
 
+/**
+ * struct brcmf_fil_country_le - country configuration structure.
+ *
+ * @country_abbrev: null-terminated country code used in the country IE.
+ * @rev: revision specifier for ccode. on set, -1 indicates unspecified.
+ * @ccode: null-terminated built-in country code.
+ */
+struct brcmf_fil_country_le {
+	char country_abbrev[BRCMF_COUNTRY_BUF_SZ];
+	__le32 rev;
+	char ccode[BRCMF_COUNTRY_BUF_SZ];
+};
+
 #endif /* FWIL_TYPES_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.c b/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.c
index 456944a6a2db..ee147f5c706a 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.c
@@ -583,7 +583,7 @@ brcmf_msgbuf_flowring_create_worker(struct brcmf_msgbuf *msgbuf,
 	u32 flowid;
 	void *dma_buf;
 	u32 dma_sz;
-	long long address;
+	u64 address;
 	int err;
 
 	flowid = work->flowid;
@@ -620,7 +620,7 @@ brcmf_msgbuf_flowring_create_worker(struct brcmf_msgbuf *msgbuf,
 					   BRCMF_NROF_H2D_COMMON_MSGRINGS);
 	memcpy(create->sa, work->sa, ETH_ALEN);
 	memcpy(create->da, work->da, ETH_ALEN);
-	address = (long long)(long)msgbuf->flowring_dma_handle[flowid];
+	address = (u64)msgbuf->flowring_dma_handle[flowid];
 	create->flow_ring_addr.high_addr = cpu_to_le32(address >> 32);
 	create->flow_ring_addr.low_addr = cpu_to_le32(address & 0xffffffff);
 	create->max_items = cpu_to_le16(BRCMF_H2D_TXFLOWRING_MAX_ITEM);
@@ -698,7 +698,7 @@ static void brcmf_msgbuf_txflow(struct brcmf_msgbuf *msgbuf, u8 flowid)
 	dma_addr_t physaddr;
 	u32 pktid;
 	struct msgbuf_tx_msghdr *tx_msghdr;
-	long long address;
+	u64 address;
 
 	commonring = msgbuf->flowrings[flowid];
 	if (!brcmf_commonring_write_available(commonring))
@@ -742,7 +742,7 @@ static void brcmf_msgbuf_txflow(struct brcmf_msgbuf *msgbuf, u8 flowid)
 		tx_msghdr->seg_cnt = 1;
 		memcpy(tx_msghdr->txhdr, skb->data, ETH_HLEN);
 		tx_msghdr->data_len = cpu_to_le16(skb->len - ETH_HLEN);
-		address = (long long)(long)physaddr;
+		address = (u64)physaddr;
 		tx_msghdr->data_buf_addr.high_addr = cpu_to_le32(address >> 32);
 		tx_msghdr->data_buf_addr.low_addr =
 			cpu_to_le32(address & 0xffffffff);
@@ -885,7 +885,7 @@ static u32 brcmf_msgbuf_rxbuf_data_post(struct brcmf_msgbuf *msgbuf, u32 count)
 	u32 pktlen;
 	dma_addr_t physaddr;
 	struct msgbuf_rx_bufpost *rx_bufpost;
-	long long address;
+	u64 address;
 	u32 pktid;
 	u32 i;
 
@@ -894,7 +894,7 @@ static u32 brcmf_msgbuf_rxbuf_data_post(struct brcmf_msgbuf *msgbuf, u32 count)
 							      count,
 							      &alloced);
 	if (!ret_ptr) {
-		brcmf_err("Failed to reserve space in commonring\n");
+		brcmf_dbg(MSGBUF, "Failed to reserve space in commonring\n");
 		return 0;
 	}
 
@@ -921,7 +921,7 @@ static u32 brcmf_msgbuf_rxbuf_data_post(struct brcmf_msgbuf *msgbuf, u32 count)
 		}
 
 		if (msgbuf->rx_metadata_offset) {
-			address = (long long)(long)physaddr;
+			address = (u64)physaddr;
 			rx_bufpost->metadata_buf_len =
 				cpu_to_le16(msgbuf->rx_metadata_offset);
 			rx_bufpost->metadata_buf_addr.high_addr =
@@ -936,7 +936,7 @@ static u32 brcmf_msgbuf_rxbuf_data_post(struct brcmf_msgbuf *msgbuf, u32 count)
 		rx_bufpost->msg.msgtype = MSGBUF_TYPE_RXBUF_POST;
 		rx_bufpost->msg.request_id = cpu_to_le32(pktid);
 
-		address = (long long)(long)physaddr;
+		address = (u64)physaddr;
 		rx_bufpost->data_buf_len = cpu_to_le16((u16)pktlen);
 		rx_bufpost->data_buf_addr.high_addr =
 			cpu_to_le32(address >> 32);
@@ -992,7 +992,7 @@ brcmf_msgbuf_rxbuf_ctrl_post(struct brcmf_msgbuf *msgbuf, bool event_buf,
 	u32 pktlen;
 	dma_addr_t physaddr;
 	struct msgbuf_rx_ioctl_resp_or_event *rx_bufpost;
-	long long address;
+	u64 address;
 	u32 pktid;
 	u32 i;
 
@@ -1035,7 +1035,7 @@ brcmf_msgbuf_rxbuf_ctrl_post(struct brcmf_msgbuf *msgbuf, bool event_buf,
 				MSGBUF_TYPE_IOCTLRESP_BUF_POST;
 		rx_bufpost->msg.request_id = cpu_to_le32(pktid);
 
-		address = (long long)(long)physaddr;
+		address = (u64)physaddr;
 		rx_bufpost->host_buf_len = cpu_to_le16((u16)pktlen);
 		rx_bufpost->host_buf_addr.high_addr =
 			cpu_to_le32(address >> 32);
@@ -1348,7 +1348,7 @@ int brcmf_proto_msgbuf_attach(struct brcmf_pub *drvr)
 {
 	struct brcmf_bus_msgbuf *if_msgbuf;
 	struct brcmf_msgbuf *msgbuf;
-	long long address;
+	u64 address;
 	u32 count;
 
 	if_msgbuf = drvr->bus_if->msgbuf;
@@ -1379,7 +1379,7 @@ int brcmf_proto_msgbuf_attach(struct brcmf_pub *drvr)
 					     GFP_KERNEL);
 	if (!msgbuf->ioctbuf)
 		goto fail;
-	address = (long long)(long)msgbuf->ioctbuf_handle;
+	address = (u64)msgbuf->ioctbuf_handle;
 	msgbuf->ioctbuf_phys_hi = address >> 32;
 	msgbuf->ioctbuf_phys_lo = address & 0xffffffff;
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/pcie.c b/drivers/net/wireless/brcm80211/brcmfmac/pcie.c
index 905991fdb7b1..e91fa9a2c885 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/pcie.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/pcie.c
@@ -959,14 +959,14 @@ brcmf_pcie_init_dmabuffer_for_device(struct brcmf_pciedev_info *devinfo,
 				     dma_addr_t *dma_handle)
 {
 	void *ring;
-	long long address;
+	u64 address;
 
 	ring = dma_alloc_coherent(&devinfo->pdev->dev, size, dma_handle,
 				  GFP_KERNEL);
 	if (!ring)
 		return NULL;
 
-	address = (long long)(long)*dma_handle;
+	address = (u64)*dma_handle;
 	brcmf_pcie_write_tcm32(devinfo, tcm_dma_phys_addr,
 			       address & 0xffffffff);
 	brcmf_pcie_write_tcm32(devinfo, tcm_dma_phys_addr + 4, address >> 32);
@@ -1166,7 +1166,7 @@ brcmf_pcie_release_scratchbuffers(struct brcmf_pciedev_info *devinfo)
 
 static int brcmf_pcie_init_scratchbuffers(struct brcmf_pciedev_info *devinfo)
 {
-	long long address;
+	u64 address;
 	u32 addr;
 
 	devinfo->shared.scratch = dma_alloc_coherent(&devinfo->pdev->dev,
@@ -1180,7 +1180,7 @@ static int brcmf_pcie_init_scratchbuffers(struct brcmf_pciedev_info *devinfo)
 
 	addr = devinfo->shared.tcm_base_address +
 	       BRCMF_SHARED_DMA_SCRATCH_ADDR_OFFSET;
-	address = (long long)(long)devinfo->shared.scratch_dmahandle;
+	address = (u64)devinfo->shared.scratch_dmahandle;
 	brcmf_pcie_write_tcm32(devinfo, addr, address & 0xffffffff);
 	brcmf_pcie_write_tcm32(devinfo, addr + 4, address >> 32);
 	addr = devinfo->shared.tcm_base_address +
@@ -1198,7 +1198,7 @@ static int brcmf_pcie_init_scratchbuffers(struct brcmf_pciedev_info *devinfo)
 
 	addr = devinfo->shared.tcm_base_address +
 	       BRCMF_SHARED_DMA_RINGUPD_ADDR_OFFSET;
-	address = (long long)(long)devinfo->shared.ringupd_dmahandle;
+	address = (u64)devinfo->shared.ringupd_dmahandle;
 	brcmf_pcie_write_tcm32(devinfo, addr, address & 0xffffffff);
 	brcmf_pcie_write_tcm32(devinfo, addr + 4, address >> 32);
 	addr = devinfo->shared.tcm_base_address +
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/sdio.c b/drivers/net/wireless/brcm80211/brcmfmac/sdio.c
index 0b0d51a61060..99a37765888d 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/sdio.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/sdio.c
@@ -608,6 +608,8 @@ static const struct sdiod_drive_str sdiod_drvstr_tab2_3v3[] = {
 #define BCM4330_NVRAM_NAME		"brcm/brcmfmac4330-sdio.txt"
 #define BCM4334_FIRMWARE_NAME		"brcm/brcmfmac4334-sdio.bin"
 #define BCM4334_NVRAM_NAME		"brcm/brcmfmac4334-sdio.txt"
+#define BCM43340_FIRMWARE_NAME		"brcm/brcmfmac43340-sdio.bin"
+#define BCM43340_NVRAM_NAME		"brcm/brcmfmac43340-sdio.txt"
 #define BCM4335_FIRMWARE_NAME		"brcm/brcmfmac4335-sdio.bin"
 #define BCM4335_NVRAM_NAME		"brcm/brcmfmac4335-sdio.txt"
 #define BCM43362_FIRMWARE_NAME		"brcm/brcmfmac43362-sdio.bin"
@@ -629,6 +631,8 @@ MODULE_FIRMWARE(BCM4330_FIRMWARE_NAME);
 MODULE_FIRMWARE(BCM4330_NVRAM_NAME);
 MODULE_FIRMWARE(BCM4334_FIRMWARE_NAME);
 MODULE_FIRMWARE(BCM4334_NVRAM_NAME);
+MODULE_FIRMWARE(BCM43340_FIRMWARE_NAME);
+MODULE_FIRMWARE(BCM43340_NVRAM_NAME);
 MODULE_FIRMWARE(BCM4335_FIRMWARE_NAME);
 MODULE_FIRMWARE(BCM4335_NVRAM_NAME);
 MODULE_FIRMWARE(BCM43362_FIRMWARE_NAME);
@@ -660,6 +664,7 @@ static const struct brcmf_firmware_names brcmf_fwname_data[] = {
 	{ BRCM_CC_4329_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM4329) },
 	{ BRCM_CC_4330_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM4330) },
 	{ BRCM_CC_4334_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM4334) },
+	{ BRCM_CC_43340_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM43340) },
 	{ BRCM_CC_4335_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM4335) },
 	{ BRCM_CC_43362_CHIP_ID, 0xFFFFFFFE, BRCMF_FIRMWARE_NVRAM(BCM43362) },
 	{ BRCM_CC_4339_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM4339) },
@@ -3811,7 +3816,7 @@ static u32 brcmf_sdio_buscore_read32(void *ctx, u32 addr)
 	u32 val, rev;
 
 	val = brcmf_sdiod_regrl(sdiodev, addr, NULL);
-	if (sdiodev->func[0]->device == BRCM_SDIO_4335_4339_DEVICE_ID &&
+	if (sdiodev->func[0]->device == SDIO_DEVICE_ID_BROADCOM_4335_4339 &&
 	    addr == CORE_CC_REG(SI_ENUM_BASE, chipid)) {
 		rev = (val & CID_REV_MASK) >> CID_REV_SHIFT;
 		if (rev >= 2) {
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/main.c b/drivers/net/wireless/brcm80211/brcmsmac/main.c
index a104d7ac3796..eb8584a9c49a 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/main.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/main.c
@@ -316,7 +316,7 @@ static const u16 xmtfifo_sz[][NFIFO] = {
 static const char * const fifo_names[] = {
 	"AC_BK", "AC_BE", "AC_VI", "AC_VO", "BCMC", "ATIM" };
 #else
-static const char fifo_names[6][0];
+static const char fifo_names[6][1];
 #endif
 
 #ifdef DEBUG
diff --git a/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h b/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
index 6996fcc144cf..2124a17d0bfd 100644
--- a/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
+++ b/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
@@ -22,7 +22,6 @@
 
 #define BRCM_USB_VENDOR_ID_BROADCOM	0x0a5c
 #define BRCM_PCIE_VENDOR_ID_BROADCOM	PCI_VENDOR_ID_BROADCOM
-#define BRCM_SDIO_VENDOR_ID_BROADCOM	SDIO_VENDOR_ID_BROADCOM
 
 /* Chipcommon Core Chip IDs */
 #define BRCM_CC_43143_CHIP_ID		43143
@@ -34,6 +33,7 @@
 #define BRCM_CC_4329_CHIP_ID		0x4329
 #define BRCM_CC_4330_CHIP_ID		0x4330
 #define BRCM_CC_4334_CHIP_ID		0x4334
+#define BRCM_CC_43340_CHIP_ID		43340
 #define BRCM_CC_43362_CHIP_ID		43362
 #define BRCM_CC_4335_CHIP_ID		0x4335
 #define BRCM_CC_4339_CHIP_ID		0x4339
@@ -45,16 +45,6 @@
 #define BRCM_CC_43570_CHIP_ID		43570
 #define BRCM_CC_43602_CHIP_ID		43602
 
-/* SDIO Device IDs */
-#define BRCM_SDIO_43143_DEVICE_ID	BRCM_CC_43143_CHIP_ID
-#define BRCM_SDIO_43241_DEVICE_ID	BRCM_CC_43241_CHIP_ID
-#define BRCM_SDIO_4329_DEVICE_ID	BRCM_CC_4329_CHIP_ID
-#define BRCM_SDIO_4330_DEVICE_ID	BRCM_CC_4330_CHIP_ID
-#define BRCM_SDIO_4334_DEVICE_ID	BRCM_CC_4334_CHIP_ID
-#define BRCM_SDIO_43362_DEVICE_ID	BRCM_CC_43362_CHIP_ID
-#define BRCM_SDIO_4335_4339_DEVICE_ID	BRCM_CC_4335_CHIP_ID
-#define BRCM_SDIO_4354_DEVICE_ID	BRCM_CC_4354_CHIP_ID
-
 /* USB Device IDs */
 #define BRCM_USB_43143_DEVICE_ID	0xbd1e
 #define BRCM_USB_43236_DEVICE_ID	0xbd17
diff --git a/drivers/net/wireless/cw1200/main.c b/drivers/net/wireless/cw1200/main.c
index 0da6e423da63..3689dbbd10bd 100644
--- a/drivers/net/wireless/cw1200/main.c
+++ b/drivers/net/wireless/cw1200/main.c
@@ -373,9 +373,8 @@ static struct ieee80211_hw *cw1200_init_common(const u8 *macaddr,
 	INIT_WORK(&priv->update_filtering_work, cw1200_update_filtering_work);
 	INIT_WORK(&priv->set_beacon_wakeup_period_work,
 		  cw1200_set_beacon_wakeup_period_work);
-	init_timer(&priv->mcast_timeout);
-	priv->mcast_timeout.data = (unsigned long)priv;
-	priv->mcast_timeout.function = cw1200_mcast_timeout;
+	setup_timer(&priv->mcast_timeout, cw1200_mcast_timeout,
+		    (unsigned long)priv);
 
 	if (cw1200_queue_stats_init(&priv->tx_queue_stats,
 				    CW1200_LINK_ID_MAX,
diff --git a/drivers/net/wireless/cw1200/pm.c b/drivers/net/wireless/cw1200/pm.c
index 6907c8fd4578..d2202ae92bdd 100644
--- a/drivers/net/wireless/cw1200/pm.c
+++ b/drivers/net/wireless/cw1200/pm.c
@@ -101,9 +101,8 @@ int cw1200_pm_init(struct cw1200_pm_state *pm,
 {
 	spin_lock_init(&pm->lock);
 
-	init_timer(&pm->stay_awake);
-	pm->stay_awake.data = (unsigned long)pm;
-	pm->stay_awake.function = cw1200_pm_stay_awake_tmo;
+	setup_timer(&pm->stay_awake, cw1200_pm_stay_awake_tmo,
+		    (unsigned long)pm);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/cw1200/queue.c b/drivers/net/wireless/cw1200/queue.c
index 9c3925f58d79..0ba5ef9b3e7b 100644
--- a/drivers/net/wireless/cw1200/queue.c
+++ b/drivers/net/wireless/cw1200/queue.c
@@ -179,9 +179,7 @@ int cw1200_queue_init(struct cw1200_queue *queue,
 	INIT_LIST_HEAD(&queue->pending);
 	INIT_LIST_HEAD(&queue->free_pool);
 	spin_lock_init(&queue->lock);
-	init_timer(&queue->gc);
-	queue->gc.data = (unsigned long)queue;
-	queue->gc.function = cw1200_queue_gc;
+	setup_timer(&queue->gc, cw1200_queue_gc, (unsigned long)queue);
 
 	queue->pool = kzalloc(sizeof(struct cw1200_queue_item) * capacity,
 			GFP_KERNEL);
diff --git a/drivers/net/wireless/hostap/hostap_cs.c b/drivers/net/wireless/hostap/hostap_cs.c
index b6ec51923b20..50033aa7c7d5 100644
--- a/drivers/net/wireless/hostap/hostap_cs.c
+++ b/drivers/net/wireless/hostap/hostap_cs.c
@@ -381,18 +381,15 @@ static void prism2_pccard_genesis_reset(local_info_t *local, int hcr)
 
 	res = pcmcia_read_config_byte(hw_priv->link, CISREG_COR, &old_cor);
 	if (res != 0) {
-		printk(KERN_DEBUG "prism2_pccard_genesis_sreset failed 1 "
-		       "(%d)\n", res);
+		printk(KERN_DEBUG "%s failed 1 (%d)\n", __func__, res);
 		return;
 	}
-	printk(KERN_DEBUG "prism2_pccard_genesis_sreset: original COR %02x\n",
-		old_cor);
+	printk(KERN_DEBUG "%s: original COR %02x\n", __func__, old_cor);
 
 	res = pcmcia_write_config_byte(hw_priv->link, CISREG_COR,
 				old_cor | COR_SOFT_RESET);
 	if (res != 0) {
-		printk(KERN_DEBUG "prism2_pccard_genesis_sreset failed 2 "
-		       "(%d)\n", res);
+		printk(KERN_DEBUG "%s failed 2 (%d)\n", __func__, res);
 		return;
 	}
 
@@ -401,8 +398,7 @@ static void prism2_pccard_genesis_reset(local_info_t *local, int hcr)
 	/* Setup Genesis mode */
 	res = pcmcia_write_config_byte(hw_priv->link, CISREG_CCSR, hcr);
 	if (res != 0) {
-		printk(KERN_DEBUG "prism2_pccard_genesis_sreset failed 3 "
-		       "(%d)\n", res);
+		printk(KERN_DEBUG "%s failed 3 (%d)\n", __func__, res);
 		return;
 	}
 	mdelay(10);
@@ -410,8 +406,7 @@ static void prism2_pccard_genesis_reset(local_info_t *local, int hcr)
 	res = pcmcia_write_config_byte(hw_priv->link, CISREG_COR,
 				old_cor & ~COR_SOFT_RESET);
 	if (res != 0) {
-		printk(KERN_DEBUG "prism2_pccard_genesis_sreset failed 4 "
-		       "(%d)\n", res);
+		printk(KERN_DEBUG "%s failed 4 (%d)\n", __func__, res);
 		return;
 	}
 
diff --git a/drivers/net/wireless/ipw2x00/Kconfig b/drivers/net/wireless/ipw2x00/Kconfig
index 91c0cb3c368e..21de4fe6cf2d 100644
--- a/drivers/net/wireless/ipw2x00/Kconfig
+++ b/drivers/net/wireless/ipw2x00/Kconfig
@@ -65,7 +65,8 @@ config IPW2100_DEBUG
 
 config IPW2200
 	tristate "Intel PRO/Wireless 2200BG and 2915ABG Network Connection"
-	depends on PCI && CFG80211 && CFG80211_WEXT
+	depends on PCI && CFG80211
+	select CFG80211_WEXT
 	select WIRELESS_EXT
 	select WEXT_SPY
 	select WEXT_PRIV
diff --git a/drivers/net/wireless/iwlegacy/3945-mac.c b/drivers/net/wireless/iwlegacy/3945-mac.c
index dc1d20cf64ee..e5665804d986 100644
--- a/drivers/net/wireless/iwlegacy/3945-mac.c
+++ b/drivers/net/wireless/iwlegacy/3945-mac.c
@@ -3429,9 +3429,7 @@ il3945_setup_deferred_work(struct il_priv *il)
 
 	il3945_hw_setup_deferred_work(il);
 
-	init_timer(&il->watchdog);
-	il->watchdog.data = (unsigned long)il;
-	il->watchdog.function = il_bg_watchdog;
+	setup_timer(&il->watchdog, il_bg_watchdog, (unsigned long)il);
 
 	tasklet_init(&il->irq_tasklet,
 		     (void (*)(unsigned long))il3945_irq_tasklet,
diff --git a/drivers/net/wireless/iwlegacy/4965-mac.c b/drivers/net/wireless/iwlegacy/4965-mac.c
index 2748fde4b90c..976f65fe9c38 100644
--- a/drivers/net/wireless/iwlegacy/4965-mac.c
+++ b/drivers/net/wireless/iwlegacy/4965-mac.c
@@ -6247,13 +6247,10 @@ il4965_setup_deferred_work(struct il_priv *il)
 
 	INIT_WORK(&il->txpower_work, il4965_bg_txpower_work);
 
-	init_timer(&il->stats_periodic);
-	il->stats_periodic.data = (unsigned long)il;
-	il->stats_periodic.function = il4965_bg_stats_periodic;
+	setup_timer(&il->stats_periodic, il4965_bg_stats_periodic,
+		    (unsigned long)il);
 
-	init_timer(&il->watchdog);
-	il->watchdog.data = (unsigned long)il;
-	il->watchdog.function = il_bg_watchdog;
+	setup_timer(&il->watchdog, il_bg_watchdog, (unsigned long)il);
 
 	tasklet_init(&il->irq_tasklet,
 		     (void (*)(unsigned long))il4965_irq_tasklet,
diff --git a/drivers/net/wireless/iwlwifi/dvm/main.c b/drivers/net/wireless/iwlwifi/dvm/main.c
index 0b7f46f0b079..de43dd7e170a 100644
--- a/drivers/net/wireless/iwlwifi/dvm/main.c
+++ b/drivers/net/wireless/iwlwifi/dvm/main.c
@@ -64,22 +64,8 @@
  *
  ******************************************************************************/
 
-/*
- * module name, copyright, version, etc.
- */
 #define DRV_DESCRIPTION	"Intel(R) Wireless WiFi Link AGN driver for Linux"
-
-#ifdef CONFIG_IWLWIFI_DEBUG
-#define VD "d"
-#else
-#define VD
-#endif
-
-#define DRV_VERSION     IWLWIFI_VERSION VD
-
-
 MODULE_DESCRIPTION(DRV_DESCRIPTION);
-MODULE_VERSION(DRV_VERSION);
 MODULE_AUTHOR(DRV_COPYRIGHT " " DRV_AUTHOR);
 MODULE_LICENSE("GPL");
 
@@ -1011,13 +997,11 @@ static void iwl_setup_deferred_work(struct iwl_priv *priv)
 	if (priv->lib->bt_params)
 		iwlagn_bt_setup_deferred_work(priv);
 
-	init_timer(&priv->statistics_periodic);
-	priv->statistics_periodic.data = (unsigned long)priv;
-	priv->statistics_periodic.function = iwl_bg_statistics_periodic;
+	setup_timer(&priv->statistics_periodic, iwl_bg_statistics_periodic,
+		    (unsigned long)priv);
 
-	init_timer(&priv->ucode_trace);
-	priv->ucode_trace.data = (unsigned long)priv;
-	priv->ucode_trace.function = iwl_bg_ucode_trace;
+	setup_timer(&priv->ucode_trace, iwl_bg_ucode_trace,
+		    (unsigned long)priv);
 }
 
 void iwl_cancel_deferred_work(struct iwl_priv *priv)
diff --git a/drivers/net/wireless/iwlwifi/dvm/tt.c b/drivers/net/wireless/iwlwifi/dvm/tt.c
index acb981a0a0aa..c4736c8834c5 100644
--- a/drivers/net/wireless/iwlwifi/dvm/tt.c
+++ b/drivers/net/wireless/iwlwifi/dvm/tt.c
@@ -612,15 +612,10 @@ void iwl_tt_initialize(struct iwl_priv *priv)
 	memset(tt, 0, sizeof(struct iwl_tt_mgmt));
 
 	tt->state = IWL_TI_0;
-	init_timer(&priv->thermal_throttle.ct_kill_exit_tm);
-	priv->thermal_throttle.ct_kill_exit_tm.data = (unsigned long)priv;
-	priv->thermal_throttle.ct_kill_exit_tm.function =
-		iwl_tt_check_exit_ct_kill;
-	init_timer(&priv->thermal_throttle.ct_kill_waiting_tm);
-	priv->thermal_throttle.ct_kill_waiting_tm.data =
-		(unsigned long)priv;
-	priv->thermal_throttle.ct_kill_waiting_tm.function =
-		iwl_tt_ready_for_ct_kill;
+	setup_timer(&priv->thermal_throttle.ct_kill_exit_tm,
+		    iwl_tt_check_exit_ct_kill, (unsigned long)priv);
+	setup_timer(&priv->thermal_throttle.ct_kill_waiting_tm,
+		    iwl_tt_ready_for_ct_kill, (unsigned long)priv);
 	/* setup deferred ct kill work */
 	INIT_WORK(&priv->tt_work, iwl_bg_tt_work);
 	INIT_WORK(&priv->ct_enter, iwl_bg_ct_enter);
diff --git a/drivers/net/wireless/iwlwifi/iwl-7000.c b/drivers/net/wireless/iwlwifi/iwl-7000.c
index e5be2d21868f..9e76799c4750 100644
--- a/drivers/net/wireless/iwlwifi/iwl-7000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-7000.c
@@ -92,6 +92,12 @@
 #define IWL7265D_NVM_VERSION		0x0c11
 #define IWL7265_TX_POWER_VERSION	0xffff /* meaningless */
 
+/* DCCM offsets and lengths */
+#define IWL7000_DCCM_OFFSET		0x800000
+#define IWL7260_DCCM_LEN		0x14000
+#define IWL3160_DCCM_LEN		0x10000
+#define IWL7265_DCCM_LEN		0x17A00
+
 #define IWL7260_FW_PRE "iwlwifi-7260-"
 #define IWL7260_MODULE_FIRMWARE(api) IWL7260_FW_PRE __stringify(api) ".ucode"
 
@@ -138,7 +144,8 @@ static const struct iwl_ht_params iwl7000_ht_params = {
 	.led_mode = IWL_LED_RF_STATE,				\
 	.nvm_hw_section_num = NVM_HW_SECTION_NUM_FAMILY_7000,	\
 	.non_shared_ant = ANT_A,				\
-	.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K
+	.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K,	\
+	.dccm_offset = IWL7000_DCCM_OFFSET
 
 const struct iwl_cfg iwl7260_2ac_cfg = {
 	.name = "Intel(R) Dual Band Wireless AC 7260",
@@ -149,6 +156,7 @@ const struct iwl_cfg iwl7260_2ac_cfg = {
 	.nvm_calib_ver = IWL7260_TX_POWER_VERSION,
 	.host_interrupt_operation_mode = true,
 	.lp_xtal_workaround = true,
+	.dccm_len = IWL7260_DCCM_LEN,
 };
 
 const struct iwl_cfg iwl7260_2ac_cfg_high_temp = {
@@ -161,6 +169,7 @@ const struct iwl_cfg iwl7260_2ac_cfg_high_temp = {
 	.high_temp = true,
 	.host_interrupt_operation_mode = true,
 	.lp_xtal_workaround = true,
+	.dccm_len = IWL7260_DCCM_LEN,
 };
 
 const struct iwl_cfg iwl7260_2n_cfg = {
@@ -172,6 +181,7 @@ const struct iwl_cfg iwl7260_2n_cfg = {
 	.nvm_calib_ver = IWL7260_TX_POWER_VERSION,
 	.host_interrupt_operation_mode = true,
 	.lp_xtal_workaround = true,
+	.dccm_len = IWL7260_DCCM_LEN,
 };
 
 const struct iwl_cfg iwl7260_n_cfg = {
@@ -183,6 +193,7 @@ const struct iwl_cfg iwl7260_n_cfg = {
 	.nvm_calib_ver = IWL7260_TX_POWER_VERSION,
 	.host_interrupt_operation_mode = true,
 	.lp_xtal_workaround = true,
+	.dccm_len = IWL7260_DCCM_LEN,
 };
 
 const struct iwl_cfg iwl3160_2ac_cfg = {
@@ -193,6 +204,7 @@ const struct iwl_cfg iwl3160_2ac_cfg = {
 	.nvm_ver = IWL3160_NVM_VERSION,
 	.nvm_calib_ver = IWL3160_TX_POWER_VERSION,
 	.host_interrupt_operation_mode = true,
+	.dccm_len = IWL3160_DCCM_LEN,
 };
 
 const struct iwl_cfg iwl3160_2n_cfg = {
@@ -203,6 +215,7 @@ const struct iwl_cfg iwl3160_2n_cfg = {
 	.nvm_ver = IWL3160_NVM_VERSION,
 	.nvm_calib_ver = IWL3160_TX_POWER_VERSION,
 	.host_interrupt_operation_mode = true,
+	.dccm_len = IWL3160_DCCM_LEN,
 };
 
 const struct iwl_cfg iwl3160_n_cfg = {
@@ -213,6 +226,7 @@ const struct iwl_cfg iwl3160_n_cfg = {
 	.nvm_ver = IWL3160_NVM_VERSION,
 	.nvm_calib_ver = IWL3160_TX_POWER_VERSION,
 	.host_interrupt_operation_mode = true,
+	.dccm_len = IWL3160_DCCM_LEN,
 };
 
 static const struct iwl_pwr_tx_backoff iwl7265_pwr_tx_backoffs[] = {
@@ -240,6 +254,7 @@ const struct iwl_cfg iwl3165_2ac_cfg = {
 	.nvm_ver = IWL3165_NVM_VERSION,
 	.nvm_calib_ver = IWL3165_TX_POWER_VERSION,
 	.pwr_tx_backoffs = iwl7265_pwr_tx_backoffs,
+	.dccm_len = IWL7265_DCCM_LEN,
 };
 
 const struct iwl_cfg iwl7265_2ac_cfg = {
@@ -250,6 +265,7 @@ const struct iwl_cfg iwl7265_2ac_cfg = {
 	.nvm_ver = IWL7265_NVM_VERSION,
 	.nvm_calib_ver = IWL7265_TX_POWER_VERSION,
 	.pwr_tx_backoffs = iwl7265_pwr_tx_backoffs,
+	.dccm_len = IWL7265_DCCM_LEN,
 };
 
 const struct iwl_cfg iwl7265_2n_cfg = {
@@ -260,6 +276,7 @@ const struct iwl_cfg iwl7265_2n_cfg = {
 	.nvm_ver = IWL7265_NVM_VERSION,
 	.nvm_calib_ver = IWL7265_TX_POWER_VERSION,
 	.pwr_tx_backoffs = iwl7265_pwr_tx_backoffs,
+	.dccm_len = IWL7265_DCCM_LEN,
 };
 
 const struct iwl_cfg iwl7265_n_cfg = {
@@ -270,6 +287,7 @@ const struct iwl_cfg iwl7265_n_cfg = {
 	.nvm_ver = IWL7265_NVM_VERSION,
 	.nvm_calib_ver = IWL7265_TX_POWER_VERSION,
 	.pwr_tx_backoffs = iwl7265_pwr_tx_backoffs,
+	.dccm_len = IWL7265_DCCM_LEN,
 };
 
 const struct iwl_cfg iwl7265d_2ac_cfg = {
@@ -280,6 +298,7 @@ const struct iwl_cfg iwl7265d_2ac_cfg = {
 	.nvm_ver = IWL7265D_NVM_VERSION,
 	.nvm_calib_ver = IWL7265_TX_POWER_VERSION,
 	.pwr_tx_backoffs = iwl7265_pwr_tx_backoffs,
+	.dccm_len = IWL7265_DCCM_LEN,
 };
 
 const struct iwl_cfg iwl7265d_2n_cfg = {
@@ -290,6 +309,7 @@ const struct iwl_cfg iwl7265d_2n_cfg = {
 	.nvm_ver = IWL7265D_NVM_VERSION,
 	.nvm_calib_ver = IWL7265_TX_POWER_VERSION,
 	.pwr_tx_backoffs = iwl7265_pwr_tx_backoffs,
+	.dccm_len = IWL7265_DCCM_LEN,
 };
 
 const struct iwl_cfg iwl7265d_n_cfg = {
@@ -300,6 +320,7 @@ const struct iwl_cfg iwl7265d_n_cfg = {
 	.nvm_ver = IWL7265D_NVM_VERSION,
 	.nvm_calib_ver = IWL7265_TX_POWER_VERSION,
 	.pwr_tx_backoffs = iwl7265_pwr_tx_backoffs,
+	.dccm_len = IWL7265_DCCM_LEN,
 };
 
 MODULE_FIRMWARE(IWL7260_MODULE_FIRMWARE(IWL7260_UCODE_API_OK));
diff --git a/drivers/net/wireless/iwlwifi/iwl-8000.c b/drivers/net/wireless/iwlwifi/iwl-8000.c
index bf0a95cb7153..46e9c9ac6ba2 100644
--- a/drivers/net/wireless/iwlwifi/iwl-8000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-8000.c
@@ -81,12 +81,19 @@
 #define IWL8000_NVM_VERSION		0x0a1d
 #define IWL8000_TX_POWER_VERSION	0xffff /* meaningless */
 
+/* Memory offsets and lengths */
+#define IWL8260_DCCM_OFFSET		0x800000
+#define IWL8260_DCCM_LEN		0x18000
+#define IWL8260_SMEM_OFFSET		0x400000
+#define IWL8260_SMEM_LEN		0x68000
+
 #define IWL8000_FW_PRE "iwlwifi-8000"
 #define IWL8000_MODULE_FIRMWARE(api) \
 	IWL8000_FW_PRE "-" __stringify(api) ".ucode"
 
 #define NVM_HW_SECTION_NUM_FAMILY_8000		10
-#define DEFAULT_NVM_FILE_FAMILY_8000		"iwl_nvm_8000.bin"
+#define DEFAULT_NVM_FILE_FAMILY_8000A		"iwl_nvm_8000.bin"
+#define DEFAULT_NVM_FILE_FAMILY_8000		"iwl_nvm_8000B.bin"
 
 /* Max SDIO RX aggregation size of the ADDBA request/response */
 #define MAX_RX_AGG_SIZE_8260_SDIO	28
@@ -124,7 +131,11 @@ static const struct iwl_ht_params iwl8000_ht_params = {
 	.led_mode = IWL_LED_RF_STATE,				\
 	.nvm_hw_section_num = NVM_HW_SECTION_NUM_FAMILY_8000,	\
 	.d0i3 = true,						\
-	.non_shared_ant = ANT_A
+	.non_shared_ant = ANT_A,				\
+	.dccm_offset = IWL8260_DCCM_OFFSET,			\
+	.dccm_len = IWL8260_DCCM_LEN,				\
+	.smem_offset = IWL8260_SMEM_OFFSET,			\
+	.smem_len = IWL8260_SMEM_LEN
 
 const struct iwl_cfg iwl8260_2n_cfg = {
 	.name = "Intel(R) Dual Band Wireless N 8260",
@@ -153,6 +164,7 @@ const struct iwl_cfg iwl8260_2ac_sdio_cfg = {
 	.nvm_ver = IWL8000_NVM_VERSION,
 	.nvm_calib_ver = IWL8000_TX_POWER_VERSION,
 	.default_nvm_file = DEFAULT_NVM_FILE_FAMILY_8000,
+	.default_nvm_file_8000A = DEFAULT_NVM_FILE_FAMILY_8000A,
 	.max_rx_agg_size = MAX_RX_AGG_SIZE_8260_SDIO,
 	.disable_dummy_notification = true,
 	.max_ht_ampdu_exponent  = MAX_HT_AMPDU_EXPONENT_8260_SDIO,
@@ -167,6 +179,7 @@ const struct iwl_cfg iwl4165_2ac_sdio_cfg = {
 	.nvm_ver = IWL8000_NVM_VERSION,
 	.nvm_calib_ver = IWL8000_TX_POWER_VERSION,
 	.default_nvm_file = DEFAULT_NVM_FILE_FAMILY_8000,
+	.default_nvm_file_8000A = DEFAULT_NVM_FILE_FAMILY_8000A,
 	.max_rx_agg_size = MAX_RX_AGG_SIZE_8260_SDIO,
 	.bt_shared_single_ant = true,
 	.disable_dummy_notification = true,
diff --git a/drivers/net/wireless/iwlwifi/iwl-config.h b/drivers/net/wireless/iwlwifi/iwl-config.h
index 3a4b9c7fc083..fa0bc4845e1a 100644
--- a/drivers/net/wireless/iwlwifi/iwl-config.h
+++ b/drivers/net/wireless/iwlwifi/iwl-config.h
@@ -261,6 +261,10 @@ struct iwl_pwr_tx_backoff {
  *	station can receive in HT
  * @max_vht_ampdu_exponent: the exponent of the max length of A-MPDU that the
  *	station can receive in VHT
+ * @dccm_offset: offset from which DCCM begins
+ * @dccm_len: length of DCCM (including runtime stack CCM)
+ * @smem_offset: offset from which the SMEM begins
+ * @smem_len: the length of SMEM
  *
  * We enable the driver to be backward compatible wrt. hardware features.
  * API differences in uCode shouldn't be handled here but through TLVs
@@ -298,11 +302,16 @@ struct iwl_cfg {
 	const struct iwl_pwr_tx_backoff *pwr_tx_backoffs;
 	bool no_power_up_nic_in_init;
 	const char *default_nvm_file;
+	const char *default_nvm_file_8000A;
 	unsigned int max_rx_agg_size;
 	bool disable_dummy_notification;
 	unsigned int max_tx_agg_size;
 	unsigned int max_ht_ampdu_exponent;
 	unsigned int max_vht_ampdu_exponent;
+	const u32 dccm_offset;
+	const u32 dccm_len;
+	const u32 smem_offset;
+	const u32 smem_len;
 };
 
 /*
@@ -370,7 +379,6 @@ extern const struct iwl_cfg iwl7265d_n_cfg;
 extern const struct iwl_cfg iwl8260_2n_cfg;
 extern const struct iwl_cfg iwl8260_2ac_cfg;
 extern const struct iwl_cfg iwl8260_2ac_sdio_cfg;
-extern const struct iwl_cfg iwl4265_2ac_sdio_cfg;
 extern const struct iwl_cfg iwl4165_2ac_sdio_cfg;
 #endif /* CONFIG_IWLMVM */
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-csr.h b/drivers/net/wireless/iwlwifi/iwl-csr.h
index aff63c3f5bf8..7f40cf36ec0e 100644
--- a/drivers/net/wireless/iwlwifi/iwl-csr.h
+++ b/drivers/net/wireless/iwlwifi/iwl-csr.h
@@ -184,6 +184,7 @@
 #define CSR_HW_IF_CONFIG_REG_BIT_NIC_READY	(0x00400000) /* PCI_OWN_SEM */
 #define CSR_HW_IF_CONFIG_REG_BIT_NIC_PREPARE_DONE (0x02000000) /* ME_OWN */
 #define CSR_HW_IF_CONFIG_REG_PREPARE		  (0x08000000) /* WAKE_ME */
+#define CSR_HW_IF_CONFIG_REG_ENABLE_PME		  (0x10000000)
 #define CSR_HW_IF_CONFIG_REG_PERSIST_MODE	  (0x40000000) /* PERSISTENCE */
 
 #define CSR_MBOX_SET_REG_OS_ALIVE		BIT(5)
diff --git a/drivers/net/wireless/iwlwifi/iwl-drv.c b/drivers/net/wireless/iwlwifi/iwl-drv.c
index 38de1513e4de..e766dcdf72ab 100644
--- a/drivers/net/wireless/iwlwifi/iwl-drv.c
+++ b/drivers/net/wireless/iwlwifi/iwl-drv.c
@@ -84,21 +84,8 @@
  *
  ******************************************************************************/
 
-/*
- * module name, copyright, version, etc.
- */
 #define DRV_DESCRIPTION	"Intel(R) Wireless WiFi driver for Linux"
-
-#ifdef CONFIG_IWLWIFI_DEBUG
-#define VD "d"
-#else
-#define VD
-#endif
-
-#define DRV_VERSION     IWLWIFI_VERSION VD
-
 MODULE_DESCRIPTION(DRV_DESCRIPTION);
-MODULE_VERSION(DRV_VERSION);
 MODULE_AUTHOR(DRV_COPYRIGHT " " DRV_AUTHOR);
 MODULE_LICENSE("GPL");
 
@@ -250,9 +237,6 @@ static int iwl_request_firmware(struct iwl_drv *drv, bool first)
 	/*
 	 * Starting 8000B - FW name format has changed. This overwrites the
 	 * previous name and uses the new format.
-	 *
-	 * TODO:
-	 * Once there is only one supported step for 8000 family - delete this!
 	 */
 	if (drv->trans->cfg->device_family == IWL_DEVICE_FAMILY_8000) {
 		char rev_step[2] = {
@@ -263,13 +247,6 @@ static int iwl_request_firmware(struct iwl_drv *drv, bool first)
 		if (CSR_HW_REV_STEP(drv->trans->hw_rev) == SILICON_A_STEP)
 			rev_step[0] = 0;
 
-		/*
-		 * If hw_rev wasn't set yet - default as B-step. If it IS A-step
-		 * we'll reload that FW later instead.
-		 */
-		if (drv->trans->hw_rev == 0)
-			rev_step[0] = 'B';
-
 		snprintf(drv->firmware_name, sizeof(drv->firmware_name),
 			 "%s%s-%s.ucode", name_pre, rev_step, tag);
 	}
@@ -926,6 +903,12 @@ static int iwl_parse_tlv_firmware(struct iwl_drv *drv,
 					    IWL_UCODE_REGULAR_USNIFFER,
 					    tlv_len);
 			break;
+		case IWL_UCODE_TLV_SDIO_ADMA_ADDR:
+			if (tlv_len != sizeof(u32))
+				goto invalid_tlv_len;
+			drv->fw.sdio_adma_addr =
+				le32_to_cpup((__le32 *)tlv_data);
+			break;
 		default:
 			IWL_DEBUG_INFO(drv, "unknown TLV: %d\n", tlv_type);
 			break;
@@ -1082,7 +1065,6 @@ static void iwl_req_fw_callback(const struct firmware *ucode_raw, void *context)
 	u32 api_ver;
 	int i;
 	bool load_module = false;
-	u32 hw_rev = drv->trans->hw_rev;
 
 	fw->ucode_capa.max_probe_length = IWL_DEFAULT_MAX_PROBE_LENGTH;
 	fw->ucode_capa.standard_phy_calibration_size =
@@ -1275,58 +1257,14 @@ static void iwl_req_fw_callback(const struct firmware *ucode_raw, void *context)
 				op->name, err);
 #endif
 	}
-
-	/*
-	 * We may have loaded the wrong FW file in 8000 HW family if it is an
-	 * A-step card, and if drv->trans->hw_rev wasn't properly read when
-	 * the FW file had been loaded. (This might happen in SDIO.) In such a
-	 * case - unload and reload the correct file.
-	 *
-	 * TODO:
-	 * Once there is only one supported step for 8000 family - delete this!
-	 */
-	if (drv->trans->cfg->device_family == IWL_DEVICE_FAMILY_8000 &&
-	    CSR_HW_REV_STEP(drv->trans->hw_rev) == SILICON_A_STEP &&
-	    drv->trans->hw_rev != hw_rev) {
-		char firmware_name[32];
-
-		/* Free previous FW resources */
-		if (drv->op_mode)
-			_iwl_op_mode_stop(drv);
-		iwl_dealloc_ucode(drv);
-
-		/* Build name of correct-step FW */
-		snprintf(firmware_name, sizeof(firmware_name),
-			 strrchr(drv->firmware_name, '-'));
-		snprintf(drv->firmware_name, sizeof(drv->firmware_name),
-			 "%s%s", drv->cfg->fw_name_pre, firmware_name);
-
-		/* Clear data before loading correct FW */
-		list_del(&drv->list);
-
-		/* Request correct FW file this time */
-		IWL_DEBUG_INFO(drv, "attempting to load A-step FW %s\n",
-			       drv->firmware_name);
-		err = request_firmware(&ucode_raw, drv->firmware_name,
-				       drv->trans->dev);
-		if (err) {
-			IWL_ERR(drv, "Failed swapping FW!\n");
-			goto out_unbind;
-		}
-
-		/* Redo callback function - this time with right FW */
-		iwl_req_fw_callback(ucode_raw, context);
-	}
-
-	kfree(pieces);
 	return;
 
  try_again:
 	/* try next, if any */
-	kfree(pieces);
 	release_firmware(ucode_raw);
 	if (iwl_request_firmware(drv, false))
 		goto out_unbind;
+	kfree(pieces);
 	return;
 
  out_free_fw:
@@ -1430,6 +1368,7 @@ struct iwl_mod_params iwlwifi_mod_params = {
 	.bt_coex_active = true,
 	.power_level = IWL_POWER_INDEX_1,
 	.wd_disable = true,
+	.d0i3_disable = true,
 #ifndef CONFIG_IWLWIFI_UAPSD
 	.uapsd_disable = true,
 #endif /* CONFIG_IWLWIFI_UAPSD */
@@ -1492,7 +1431,7 @@ static int __init iwl_drv_init(void)
 	for (i = 0; i < ARRAY_SIZE(iwlwifi_opmode_table); i++)
 		INIT_LIST_HEAD(&iwlwifi_opmode_table[i].drv);
 
-	pr_info(DRV_DESCRIPTION ", " DRV_VERSION "\n");
+	pr_info(DRV_DESCRIPTION "\n");
 	pr_info(DRV_COPYRIGHT "\n");
 
 #ifdef CONFIG_IWLWIFI_DEBUGFS
@@ -1546,6 +1485,10 @@ MODULE_PARM_DESC(wd_disable,
 module_param_named(nvm_file, iwlwifi_mod_params.nvm_file, charp, S_IRUGO);
 MODULE_PARM_DESC(nvm_file, "NVM file name");
 
+module_param_named(d0i3_disable, iwlwifi_mod_params.d0i3_disable,
+		   bool, S_IRUGO);
+MODULE_PARM_DESC(d0i3_disable, "disable d0i3 functionality (default: Y)");
+
 module_param_named(uapsd_disable, iwlwifi_mod_params.uapsd_disable,
 		   bool, S_IRUGO);
 #ifdef CONFIG_IWLWIFI_UAPSD
diff --git a/drivers/net/wireless/iwlwifi/iwl-drv.h b/drivers/net/wireless/iwlwifi/iwl-drv.h
index be4f8972241a..adf522c756e6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-drv.h
+++ b/drivers/net/wireless/iwlwifi/iwl-drv.h
@@ -68,7 +68,6 @@
 
 /* for all modules */
 #define DRV_NAME        "iwlwifi"
-#define IWLWIFI_VERSION "in-tree:"
 #define DRV_COPYRIGHT	"Copyright(c) 2003- 2014 Intel Corporation"
 #define DRV_AUTHOR     "<ilw@linux.intel.com>"
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-fh.h b/drivers/net/wireless/iwlwifi/iwl-fh.h
index 9564ae173d06..1f7f15eb86da 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fh.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fh.h
@@ -310,6 +310,7 @@ static inline unsigned int FH_MEM_CBBC_QUEUE(unsigned int chnl)
 #define FH_RSSR_CHNL0_RX_STATUS_CHNL_IDLE	(0x01000000)
 
 #define FH_MEM_TFDIB_REG1_ADDR_BITSHIFT	28
+#define FH_MEM_TB_MAX_LENGTH			(0x00020000)
 
 /* TFDB  Area - TFDs buffer table */
 #define FH_MEM_TFDIB_DRAM_ADDR_LSB_MSK      (0xFFFFFFFF)
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw-error-dump.h b/drivers/net/wireless/iwlwifi/iwl-fw-error-dump.h
index 20a8a64c9fe3..ec115bded88a 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw-error-dump.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw-error-dump.h
@@ -71,7 +71,6 @@
 
 /**
  * enum iwl_fw_error_dump_type - types of data in the dump file
- * @IWL_FW_ERROR_DUMP_SRAM:
  * @IWL_FW_ERROR_DUMP_CSR: Control Status Registers - from offset 0
  * @IWL_FW_ERROR_DUMP_RXF:
  * @IWL_FW_ERROR_DUMP_TXCMD: last TX command data, structured as
@@ -82,9 +81,10 @@
  * @IWL_FW_ERROR_DUMP_PRPH: range of periphery registers - there can be several
  *	sections like this in a single file.
  * @IWL_FW_ERROR_DUMP_FH_REGS: range of FH registers
+ * @IWL_FW_ERROR_DUMP_MEM: chunk of memory
  */
 enum iwl_fw_error_dump_type {
-	IWL_FW_ERROR_DUMP_SRAM = 0,
+	/* 0 is deprecated */
 	IWL_FW_ERROR_DUMP_CSR = 1,
 	IWL_FW_ERROR_DUMP_RXF = 2,
 	IWL_FW_ERROR_DUMP_TXCMD = 3,
@@ -93,6 +93,7 @@ enum iwl_fw_error_dump_type {
 	IWL_FW_ERROR_DUMP_PRPH = 6,
 	IWL_FW_ERROR_DUMP_TXF = 7,
 	IWL_FW_ERROR_DUMP_FH_REGS = 8,
+	IWL_FW_ERROR_DUMP_MEM = 9,
 
 	IWL_FW_ERROR_DUMP_MAX,
 };
@@ -180,6 +181,23 @@ struct iwl_fw_error_dump_prph {
 	__le32 data[];
 };
 
+enum iwl_fw_error_dump_mem_type {
+	IWL_FW_ERROR_DUMP_MEM_SRAM,
+	IWL_FW_ERROR_DUMP_MEM_SMEM,
+};
+
+/**
+ * struct iwl_fw_error_dump_mem - chunk of memory
+ * @type: %enum iwl_fw_error_dump_mem_type
+ * @offset: the offset from which the memory was read
+ * @data: the content of the memory
+ */
+struct iwl_fw_error_dump_mem {
+	__le32 type;
+	__le32 offset;
+	u8 data[];
+};
+
 /**
  * iwl_fw_error_next_data - advance fw error dump data pointer
  * @data: previous data block
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw-file.h b/drivers/net/wireless/iwlwifi/iwl-fw-file.h
index f2a047f6bb3e..752c72b77fba 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw-file.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw-file.h
@@ -132,6 +132,7 @@ enum iwl_ucode_tlv_type {
 	IWL_UCODE_TLV_ENABLED_CAPABILITIES	= 30,
 	IWL_UCODE_TLV_N_SCAN_CHANNELS		= 31,
 	IWL_UCODE_TLV_SEC_RT_USNIFFER	= 34,
+	IWL_UCODE_TLV_SDIO_ADMA_ADDR	= 35,
 	IWL_UCODE_TLV_FW_DBG_DEST	= 38,
 	IWL_UCODE_TLV_FW_DBG_CONF	= 39,
 };
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw.h b/drivers/net/wireless/iwlwifi/iwl-fw.h
index e6dc3b870949..ffd785cc67d6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw.h
@@ -152,6 +152,8 @@ struct iwl_fw_cscheme_list {
  * @mvm_fw: indicates this is MVM firmware
  * @cipher_scheme: optional external cipher scheme.
  * @human_readable: human readable version
+ * @sdio_adma_addr: the default address to set for the ADMA in SDIO mode until
+ *	we get the ALIVE from the uCode
  * @dbg_dest_tlv: points to the destination TLV for debug
  * @dbg_conf_tlv: array of pointers to configuration TLVs for debug
  * @dbg_conf_tlv_len: lengths of the @dbg_conf_tlv entries
@@ -181,6 +183,8 @@ struct iwl_fw {
 	struct ieee80211_cipher_scheme cs[IWL_UCODE_MAX_CS];
 	u8 human_readable[FW_VER_HUMAN_READABLE_SZ];
 
+	u32 sdio_adma_addr;
+
 	struct iwl_fw_dbg_dest_tlv *dbg_dest_tlv;
 	struct iwl_fw_dbg_conf_tlv *dbg_conf_tlv[FW_DBG_MAX];
 	size_t dbg_conf_tlv_len[FW_DBG_MAX];
diff --git a/drivers/net/wireless/iwlwifi/iwl-modparams.h b/drivers/net/wireless/iwlwifi/iwl-modparams.h
index 71507cf490e6..2a8cf4b2445c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-modparams.h
+++ b/drivers/net/wireless/iwlwifi/iwl-modparams.h
@@ -103,6 +103,7 @@ enum iwl_disable_11n {
  * @power_level: power level, default = 1
  * @debug_level: levels are IWL_DL_*
  * @ant_coupling: antenna coupling in dB, default = 0
+ * @d0i3_disable: disable d0i3, default = 1,
  * @fw_monitor: allow to use firmware monitor
  */
 struct iwl_mod_params {
@@ -121,6 +122,7 @@ struct iwl_mod_params {
 	int ant_coupling;
 	char *nvm_file;
 	bool uapsd_disable;
+	bool d0i3_disable;
 	bool fw_monitor;
 };
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c b/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c
index 06e02fcd6f7b..c74f1a4edf23 100644
--- a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c
+++ b/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c
@@ -468,6 +468,8 @@ static void iwl_set_radio_cfg(const struct iwl_cfg *cfg,
 	data->radio_cfg_step = NVM_RF_CFG_STEP_MSK_FAMILY_8000(radio_cfg);
 	data->radio_cfg_dash = NVM_RF_CFG_DASH_MSK_FAMILY_8000(radio_cfg);
 	data->radio_cfg_pnum = NVM_RF_CFG_FLAVOR_MSK_FAMILY_8000(radio_cfg);
+	data->valid_tx_ant = NVM_RF_CFG_TX_ANT_MSK_FAMILY_8000(radio_cfg);
+	data->valid_rx_ant = NVM_RF_CFG_RX_ANT_MSK_FAMILY_8000(radio_cfg);
 }
 
 static void iwl_set_hw_address(const struct iwl_cfg *cfg,
@@ -592,6 +594,10 @@ iwl_parse_nvm_data(struct device *dev, const struct iwl_cfg *cfg,
 
 	radio_cfg = iwl_get_radio_cfg(cfg, nvm_sw);
 	iwl_set_radio_cfg(cfg, data, radio_cfg);
+	if (data->valid_tx_ant)
+		tx_chains &= data->valid_tx_ant;
+	if (data->valid_rx_ant)
+		rx_chains &= data->valid_rx_ant;
 
 	sku = iwl_get_sku(cfg, nvm_sw);
 	data->sku_cap_band_24GHz_enable = sku & NVM_SKU_CAP_BAND_24GHZ;
diff --git a/drivers/net/wireless/iwlwifi/iwl-prph.h b/drivers/net/wireless/iwlwifi/iwl-prph.h
index 2df51eab1348..83ab4239082c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-prph.h
+++ b/drivers/net/wireless/iwlwifi/iwl-prph.h
@@ -99,6 +99,7 @@
 
 #define APMG_PCIDEV_STT_VAL_PERSIST_DIS	(0x00000200)
 #define APMG_PCIDEV_STT_VAL_L1_ACT_DIS	(0x00000800)
+#define APMG_PCIDEV_STT_VAL_WAKE_ME	(0x00004000)
 
 #define APMG_RTC_INT_STT_RFKILL		(0x10000000)
 
@@ -365,11 +366,15 @@ enum secure_load_status_reg {
 #define RXF_FIFO_RD_FENCE_ADDR		(0xa00c0c)
 
 /* FW monitor */
+#define MON_BUFF_SAMPLE_CTL		(0xa03c00)
 #define MON_BUFF_BASE_ADDR		(0xa03c3c)
 #define MON_BUFF_END_ADDR		(0xa03c40)
 #define MON_BUFF_WRPTR			(0xa03c44)
 #define MON_BUFF_CYCLE_CNT		(0xa03c48)
 
+#define DBGC_IN_SAMPLE			(0xa03c00)
+#define DBGC_OUT_CTRL			(0xa03c0c)
+
 /* FW chicken bits */
 #define LMPM_CHICK			0xA01FF8
 enum {
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans.h b/drivers/net/wireless/iwlwifi/iwl-trans.h
index 028408a6ecba..84d8477432a2 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans.h
+++ b/drivers/net/wireless/iwlwifi/iwl-trans.h
@@ -382,6 +382,8 @@ enum iwl_trans_status {
  *	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
+ * @sdio_adma_addr: the default address to set for the ADMA in SDIO mode until
+ *	we get the ALIVE from the uCode
  */
 struct iwl_trans_config {
 	struct iwl_op_mode *op_mode;
@@ -396,6 +398,8 @@ struct iwl_trans_config {
 	bool scd_set_active;
 	unsigned int queue_watchdog_timeout;
 	const char *const *command_names;
+
+	u32 sdio_adma_addr;
 };
 
 struct iwl_trans_dump_data {
@@ -552,6 +556,21 @@ enum iwl_trans_state {
 };
 
 /**
+ * enum iwl_d0i3_mode - d0i3 mode
+ *
+ * @IWL_D0I3_MODE_OFF - d0i3 is disabled
+ * @IWL_D0I3_MODE_ON_IDLE - enter d0i3 when device is idle
+ *	(e.g. no active references)
+ * @IWL_D0I3_MODE_ON_SUSPEND - enter d0i3 only on suspend
+ *	(in case of 'any' trigger)
+ */
+enum iwl_d0i3_mode {
+	IWL_D0I3_MODE_OFF = 0,
+	IWL_D0I3_MODE_ON_IDLE,
+	IWL_D0I3_MODE_ON_SUSPEND,
+};
+
+/**
  * struct iwl_trans - transport common data
  *
  * @ops - pointer to iwl_trans_ops
@@ -612,6 +631,8 @@ struct iwl_trans {
 	const struct iwl_fw_dbg_conf_tlv *dbg_conf_tlv[FW_DBG_MAX];
 	u8 dbg_dest_reg_num;
 
+	enum iwl_d0i3_mode d0i3_mode;
+
 	/* pointer to trans specific struct */
 	/*Ensure that this pointer will always be aligned to sizeof pointer */
 	char trans_specific[0] __aligned(sizeof(void *));
diff --git a/drivers/net/wireless/iwlwifi/mvm/coex.c b/drivers/net/wireless/iwlwifi/mvm/coex.c
index a3bfda45d9e6..d2fd48c31d3d 100644
--- a/drivers/net/wireless/iwlwifi/mvm/coex.c
+++ b/drivers/net/wireless/iwlwifi/mvm/coex.c
@@ -989,7 +989,7 @@ int iwl_mvm_rx_bt_coex_notif(struct iwl_mvm *mvm,
 static void iwl_mvm_bt_rssi_iterator(void *_data, u8 *mac,
 				   struct ieee80211_vif *vif)
 {
-	struct iwl_mvm_vif *mvmvif = (void *)vif->drv_priv;
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	struct iwl_bt_iterator_data *data = _data;
 	struct iwl_mvm *mvm = data->mvm;
 
@@ -1025,7 +1025,7 @@ static void iwl_mvm_bt_rssi_iterator(void *_data, u8 *mac,
 void iwl_mvm_bt_rssi_event(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 			   enum ieee80211_rssi_event rssi_event)
 {
-	struct iwl_mvm_vif *mvmvif = (void *)vif->drv_priv;
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	struct iwl_bt_iterator_data data = {
 		.mvm = mvm,
 	};
diff --git a/drivers/net/wireless/iwlwifi/mvm/coex_legacy.c b/drivers/net/wireless/iwlwifi/mvm/coex_legacy.c
index b3210cfbecc8..973f2881dd1d 100644
--- a/drivers/net/wireless/iwlwifi/mvm/coex_legacy.c
+++ b/drivers/net/wireless/iwlwifi/mvm/coex_legacy.c
@@ -1034,7 +1034,7 @@ int iwl_mvm_rx_bt_coex_notif_old(struct iwl_mvm *mvm,
 static void iwl_mvm_bt_rssi_iterator(void *_data, u8 *mac,
 				     struct ieee80211_vif *vif)
 {
-	struct iwl_mvm_vif *mvmvif = (void *)vif->drv_priv;
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	struct iwl_bt_iterator_data *data = _data;
 	struct iwl_mvm *mvm = data->mvm;
 
@@ -1070,7 +1070,7 @@ static void iwl_mvm_bt_rssi_iterator(void *_data, u8 *mac,
 void iwl_mvm_bt_rssi_event_old(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 			       enum ieee80211_rssi_event rssi_event)
 {
-	struct iwl_mvm_vif *mvmvif = (void *)vif->drv_priv;
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	struct iwl_bt_iterator_data data = {
 		.mvm = mvm,
 	};
diff --git a/drivers/net/wireless/iwlwifi/mvm/constants.h b/drivers/net/wireless/iwlwifi/mvm/constants.h
index 3bd93476ec1c..7cd1de820ca7 100644
--- a/drivers/net/wireless/iwlwifi/mvm/constants.h
+++ b/drivers/net/wireless/iwlwifi/mvm/constants.h
@@ -102,5 +102,33 @@
 #define IWL_MVM_QUOTA_THRESHOLD			8
 #define IWL_MVM_RS_RSSI_BASED_INIT_RATE         0
 #define IWL_MVM_RS_DISABLE_MIMO			0
+#define IWL_MVM_RS_NUM_TRY_BEFORE_ANT_TOGGLE    1
+#define IWL_MVM_RS_LEGACY_RETRIES_PER_RATE      1
+#define IWL_MVM_RS_HT_VHT_RETRIES_PER_RATE      2
+#define IWL_MVM_RS_HT_VHT_RETRIES_PER_RATE_TW   1
+#define IWL_MVM_RS_INITIAL_MIMO_NUM_RATES       3
+#define IWL_MVM_RS_INITIAL_SISO_NUM_RATES       3
+#define IWL_MVM_RS_INITIAL_LEGACY_NUM_RATES     16
+#define IWL_MVM_RS_SECONDARY_LEGACY_NUM_RATES   16
+#define IWL_MVM_RS_SECONDARY_SISO_NUM_RATES     3
+#define IWL_MVM_RS_SECONDARY_SISO_RETRIES       1
+#define IWL_MVM_RS_RATE_MIN_FAILURE_TH		3
+#define IWL_MVM_RS_RATE_MIN_SUCCESS_TH		8
+#define IWL_MVM_RS_STAY_IN_COLUMN_TIMEOUT	5	/* Seconds */
+#define IWL_MVM_RS_IDLE_TIMEOUT			5	/* Seconds */
+#define IWL_MVM_RS_MISSED_RATE_MAX		15
+#define IWL_MVM_RS_LEGACY_FAILURE_LIMIT		160
+#define IWL_MVM_RS_LEGACY_SUCCESS_LIMIT		480
+#define IWL_MVM_RS_LEGACY_TABLE_COUNT		160
+#define IWL_MVM_RS_NON_LEGACY_FAILURE_LIMIT	400
+#define IWL_MVM_RS_NON_LEGACY_SUCCESS_LIMIT	4500
+#define IWL_MVM_RS_NON_LEGACY_TABLE_COUNT	1500
+#define IWL_MVM_RS_SR_FORCE_DECREASE		15	/* percent */
+#define IWL_MVM_RS_SR_NO_DECREASE		85	/* percent */
+#define IWL_MVM_RS_AGG_TIME_LIMIT	        4000    /* 4 msecs. valid 100-8000 */
+#define IWL_MVM_RS_AGG_DISABLE_START	        3
+#define IWL_MVM_RS_TPC_SR_FORCE_INCREASE	75	/* percent */
+#define IWL_MVM_RS_TPC_SR_NO_INCREASE		85	/* percent */
+#define IWL_MVM_RS_TPC_TX_POWER_STEP		3
 
 #endif /* __MVM_CONSTANTS_H */
diff --git a/drivers/net/wireless/iwlwifi/mvm/d3.c b/drivers/net/wireless/iwlwifi/mvm/d3.c
index 744de262373e..14e8fd661889 100644
--- a/drivers/net/wireless/iwlwifi/mvm/d3.c
+++ b/drivers/net/wireless/iwlwifi/mvm/d3.c
@@ -793,7 +793,7 @@ iwl_mvm_get_wowlan_config(struct iwl_mvm *mvm,
 			  struct ieee80211_sta *ap_sta)
 {
 	int ret;
-	struct iwl_mvm_sta *mvm_ap_sta = (struct iwl_mvm_sta *)ap_sta->drv_priv;
+	struct iwl_mvm_sta *mvm_ap_sta = iwl_mvm_sta_from_mac80211(ap_sta);
 
 	/* TODO: wowlan_config_cmd->wowlan_ba_teardown_tids */
 
@@ -1137,12 +1137,43 @@ static int __iwl_mvm_suspend(struct ieee80211_hw *hw,
 	return ret;
 }
 
+static int iwl_mvm_enter_d0i3_sync(struct iwl_mvm *mvm)
+{
+	struct iwl_notification_wait wait_d3;
+	static const u8 d3_notif[] = { D3_CONFIG_CMD };
+	int ret;
+
+	iwl_init_notification_wait(&mvm->notif_wait, &wait_d3,
+				   d3_notif, ARRAY_SIZE(d3_notif),
+				   NULL, NULL);
+
+	ret = iwl_mvm_enter_d0i3(mvm->hw->priv);
+	if (ret)
+		goto remove_notif;
+
+	ret = iwl_wait_notification(&mvm->notif_wait, &wait_d3, HZ);
+	WARN_ON_ONCE(ret);
+	return ret;
+
+remove_notif:
+	iwl_remove_notification(&mvm->notif_wait, &wait_d3);
+	return ret;
+}
+
 int iwl_mvm_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan)
 {
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
 
 	iwl_trans_suspend(mvm->trans);
-	if (iwl_mvm_is_d0i3_supported(mvm)) {
+	if (wowlan->any) {
+		/* 'any' trigger means d0i3 usage */
+		if (mvm->trans->d0i3_mode == IWL_D0I3_MODE_ON_SUSPEND) {
+			int ret = iwl_mvm_enter_d0i3_sync(mvm);
+
+			if (ret)
+				return ret;
+		}
+
 		mutex_lock(&mvm->d0i3_suspend_mutex);
 		__set_bit(D0I3_DEFER_WAKEUP, &mvm->d0i3_suspend_flags);
 		mutex_unlock(&mvm->d0i3_suspend_mutex);
@@ -1626,7 +1657,7 @@ static bool iwl_mvm_query_wakeup_reasons(struct iwl_mvm *mvm,
 	if (IS_ERR_OR_NULL(ap_sta))
 		goto out_free;
 
-	mvm_ap_sta = (struct iwl_mvm_sta *)ap_sta->drv_priv;
+	mvm_ap_sta = iwl_mvm_sta_from_mac80211(ap_sta);
 	for (i = 0; i < IWL_MAX_TID_COUNT; i++) {
 		u16 seq = status.qos_seq_ctr[i];
 		/* firmware stores last-used value, we store next value */
@@ -1876,8 +1907,20 @@ int iwl_mvm_resume(struct ieee80211_hw *hw)
 
 	iwl_trans_resume(mvm->trans);
 
-	if (iwl_mvm_is_d0i3_supported(mvm))
+	if (mvm->hw->wiphy->wowlan_config->any) {
+		/* 'any' trigger means d0i3 usage */
+		if (mvm->trans->d0i3_mode == IWL_D0I3_MODE_ON_SUSPEND) {
+			int ret = iwl_mvm_exit_d0i3(hw->priv);
+
+			if (ret)
+				return ret;
+			/*
+			 * d0i3 exit will be deferred until reconfig_complete.
+			 * make sure there we are out of d0i3.
+			 */
+		}
 		return 0;
+	}
 
 	return __iwl_mvm_resume(mvm, false);
 }
diff --git a/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c b/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c
index 9aa2311a776c..8dc3ca9f4904 100644
--- a/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c
+++ b/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c
@@ -268,7 +268,7 @@ static ssize_t iwl_dbgfs_mac_params_read(struct file *file,
 		sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[ap_sta_id],
 						lockdep_is_held(&mvm->mutex));
 		if (!IS_ERR_OR_NULL(sta)) {
-			struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
+			struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 
 			pos += scnprintf(buf+pos, bufsz-pos,
 					 "ap_sta_id %d - reduced Tx power %d\n",
diff --git a/drivers/net/wireless/iwlwifi/mvm/debugfs.c b/drivers/net/wireless/iwlwifi/mvm/debugfs.c
index 33bf915cd7ea..a1b276c4dee0 100644
--- a/drivers/net/wireless/iwlwifi/mvm/debugfs.c
+++ b/drivers/net/wireless/iwlwifi/mvm/debugfs.c
@@ -933,7 +933,7 @@ iwl_dbgfs_scan_ant_rxchain_write(struct iwl_mvm *mvm, char *buf,
 		return -EINVAL;
 	if (scan_rx_ant > ANT_ABC)
 		return -EINVAL;
-	if (scan_rx_ant & ~mvm->fw->valid_rx_ant)
+	if (scan_rx_ant & ~(iwl_mvm_get_valid_rx_ant(mvm)))
 		return -EINVAL;
 
 	if (mvm->scan_rx_ant != scan_rx_ant) {
@@ -945,6 +945,56 @@ iwl_dbgfs_scan_ant_rxchain_write(struct iwl_mvm *mvm, char *buf,
 	return count;
 }
 
+static ssize_t iwl_dbgfs_fw_dbg_conf_read(struct file *file,
+					  char __user *user_buf,
+					  size_t count, loff_t *ppos)
+{
+	struct iwl_mvm *mvm = file->private_data;
+	enum iwl_fw_dbg_conf conf;
+	char buf[8];
+	const size_t bufsz = sizeof(buf);
+	int pos = 0;
+
+	mutex_lock(&mvm->mutex);
+	conf = mvm->fw_dbg_conf;
+	mutex_unlock(&mvm->mutex);
+
+	pos += scnprintf(buf + pos, bufsz - pos, "%d\n", conf);
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
+static ssize_t iwl_dbgfs_fw_dbg_conf_write(struct iwl_mvm *mvm,
+					   char *buf, size_t count,
+					   loff_t *ppos)
+{
+	int ret, conf_id;
+
+	ret = kstrtoint(buf, 0, &conf_id);
+	if (ret)
+		return ret;
+
+	if (WARN_ON(conf_id >= FW_DBG_MAX))
+		return -EINVAL;
+
+	mutex_lock(&mvm->mutex);
+	ret = iwl_mvm_start_fw_dbg_conf(mvm, conf_id);
+	mutex_unlock(&mvm->mutex);
+
+	return ret ?: count;
+}
+
+static ssize_t iwl_dbgfs_fw_dbg_collect_write(struct iwl_mvm *mvm,
+					      char *buf, size_t count,
+					      loff_t *ppos)
+{
+	mutex_lock(&mvm->mutex);
+	iwl_mvm_fw_dbg_collect(mvm);
+	mutex_unlock(&mvm->mutex);
+
+	return count;
+}
+
 #define ADD_TEXT(...) pos += scnprintf(buf + pos, bufsz - pos, __VA_ARGS__)
 #ifdef CONFIG_IWLWIFI_BCAST_FILTERING
 static ssize_t iwl_dbgfs_bcast_filters_read(struct file *file,
@@ -1459,6 +1509,8 @@ MVM_DEBUGFS_WRITE_FILE_OPS(bt_tx_prio, 10);
 MVM_DEBUGFS_WRITE_FILE_OPS(bt_force_ant, 10);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(scan_ant_rxchain, 8);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(d0i3_refs, 8);
+MVM_DEBUGFS_READ_WRITE_FILE_OPS(fw_dbg_conf, 8);
+MVM_DEBUGFS_WRITE_FILE_OPS(fw_dbg_collect, 8);
 
 #ifdef CONFIG_IWLWIFI_BCAST_FILTERING
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(bcast_filters, 256);
@@ -1500,6 +1552,8 @@ int iwl_mvm_dbgfs_register(struct iwl_mvm *mvm, struct dentry *dbgfs_dir)
 			     S_IWUSR | S_IRUSR);
 	MVM_DEBUGFS_ADD_FILE(prph_reg, mvm->debugfs_dir, S_IWUSR | S_IRUSR);
 	MVM_DEBUGFS_ADD_FILE(d0i3_refs, mvm->debugfs_dir, S_IRUSR | S_IWUSR);
+	MVM_DEBUGFS_ADD_FILE(fw_dbg_conf, mvm->debugfs_dir, S_IRUSR | S_IWUSR);
+	MVM_DEBUGFS_ADD_FILE(fw_dbg_collect, mvm->debugfs_dir, S_IWUSR);
 
 #ifdef CONFIG_IWLWIFI_BCAST_FILTERING
 	if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_BCAST_FILTERING) {
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
index 1f2acf47bfb2..2d1a81c493f1 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
@@ -653,8 +653,11 @@ enum iwl_scan_channel_flags {
 };
 
 /* iwl_scan_channel_opt - CHANNEL_OPTIMIZATION_API_S
- * @flags: enum iwl_scan_channel_flgs
- * @non_ebs_ratio: how many regular scan iteration before EBS
+ * @flags: enum iwl_scan_channel_flags
+ * @non_ebs_ratio: defines the ratio of number of scan iterations where EBS is
+ *	involved.
+ *	1 - EBS is disabled.
+ *	2 - every second scan will be full scan(and so on).
  */
 struct iwl_scan_channel_opt {
 	__le16 flags;
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw.c b/drivers/net/wireless/iwlwifi/mvm/fw.c
index d0fa6e9ed590..534ee3123a63 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw.c
+++ b/drivers/net/wireless/iwlwifi/mvm/fw.c
@@ -70,6 +70,7 @@
 #include "iwl-debug.h"
 #include "iwl-csr.h" /* for iwl_mvm_rx_card_state_notif */
 #include "iwl-io.h" /* for iwl_mvm_rx_card_state_notif */
+#include "iwl-prph.h"
 #include "iwl-eeprom-parse.h"
 
 #include "mvm.h"
@@ -269,7 +270,7 @@ static int iwl_send_phy_cfg_cmd(struct iwl_mvm *mvm)
 	enum iwl_ucode_type ucode_type = mvm->cur_ucode;
 
 	/* Set parameters */
-	phy_cfg_cmd.phy_cfg = cpu_to_le32(mvm->fw->phy_config);
+	phy_cfg_cmd.phy_cfg = cpu_to_le32(iwl_mvm_get_phy_config(mvm));
 	phy_cfg_cmd.calib_control.event_trigger =
 		mvm->fw->default_calib[ucode_type].event_trigger;
 	phy_cfg_cmd.calib_control.flow_trigger =
@@ -346,7 +347,7 @@ int iwl_run_init_mvm_ucode(struct iwl_mvm *mvm, bool read_nvm)
 	mvm->calibrating = true;
 
 	/* Send TX valid antennas before triggering calibrations */
-	ret = iwl_send_tx_ant_cfg(mvm, mvm->fw->valid_tx_ant);
+	ret = iwl_send_tx_ant_cfg(mvm, iwl_mvm_get_valid_tx_ant(mvm));
 	if (ret)
 		goto error;
 
@@ -399,8 +400,26 @@ out:
 	return ret;
 }
 
-static int iwl_mvm_start_fw_dbg_conf(struct iwl_mvm *mvm,
-				     enum iwl_fw_dbg_conf conf_id)
+void iwl_mvm_fw_dbg_collect(struct iwl_mvm *mvm)
+{
+	lockdep_assert_held(&mvm->mutex);
+
+	/* stop recording */
+	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_7000) {
+		iwl_set_bits_prph(mvm->trans, MON_BUFF_SAMPLE_CTL, 0x100);
+	} else {
+		iwl_write_prph(mvm->trans, DBGC_IN_SAMPLE, 0);
+		iwl_write_prph(mvm->trans, DBGC_OUT_CTRL, 0);
+	}
+
+	iwl_mvm_fw_error_dump(mvm);
+
+	/* start recording again */
+	WARN_ON_ONCE(mvm->fw->dbg_dest_tlv &&
+		     iwl_mvm_start_fw_dbg_conf(mvm, mvm->fw_dbg_conf));
+}
+
+int iwl_mvm_start_fw_dbg_conf(struct iwl_mvm *mvm, enum iwl_fw_dbg_conf conf_id)
 {
 	u8 *ptr;
 	int ret;
@@ -489,7 +508,7 @@ int iwl_mvm_up(struct iwl_mvm *mvm)
 	mvm->fw_dbg_conf = FW_DBG_INVALID;
 	iwl_mvm_start_fw_dbg_conf(mvm, FW_DBG_CUSTOM);
 
-	ret = iwl_send_tx_ant_cfg(mvm, mvm->fw->valid_tx_ant);
+	ret = iwl_send_tx_ant_cfg(mvm, iwl_mvm_get_valid_tx_ant(mvm));
 	if (ret)
 		goto error;
 
@@ -584,7 +603,7 @@ int iwl_mvm_load_d3_fw(struct iwl_mvm *mvm)
 		goto error;
 	}
 
-	ret = iwl_send_tx_ant_cfg(mvm, mvm->fw->valid_tx_ant);
+	ret = iwl_send_tx_ant_cfg(mvm, iwl_mvm_get_valid_tx_ant(mvm));
 	if (ret)
 		goto error;
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c b/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
index f6d86ccce6a8..7196b4d6b7cc 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
+++ b/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
@@ -975,7 +975,7 @@ static int iwl_mvm_mac_ctxt_send_beacon(struct iwl_mvm *mvm,
 	beacon_cmd.tx.tx_flags = cpu_to_le32(tx_flags);
 
 	mvm->mgmt_last_antenna_idx =
-		iwl_mvm_next_antenna(mvm, mvm->fw->valid_tx_ant,
+		iwl_mvm_next_antenna(mvm, iwl_mvm_get_valid_tx_ant(mvm),
 				     mvm->mgmt_last_antenna_idx);
 
 	beacon_cmd.tx.rate_n_flags =
diff --git a/drivers/net/wireless/iwlwifi/mvm/mac80211.c b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
index 346331d3c696..aa1ee04b1514 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac80211.c
+++ b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
@@ -85,6 +85,7 @@
 #include "testmode.h"
 #include "iwl-fw-error-dump.h"
 #include "iwl-prph.h"
+#include "iwl-csr.h"
 
 static const struct ieee80211_iface_limit iwl_mvm_limits[] = {
 	{
@@ -105,7 +106,7 @@ static const struct ieee80211_iface_limit iwl_mvm_limits[] = {
 
 static const struct ieee80211_iface_combination iwl_mvm_iface_combinations[] = {
 	{
-		.num_different_channels = 1,
+		.num_different_channels = 2,
 		.max_interfaces = 3,
 		.limits = iwl_mvm_limits,
 		.n_limits = ARRAY_SIZE(iwl_mvm_limits),
@@ -372,6 +373,8 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 
 	hw->wiphy->max_remain_on_channel_duration = 10000;
 	hw->max_listen_interval = IWL_CONN_MAX_LISTEN_INTERVAL;
+	/* we can compensate an offset of up to 3 channels = 15 MHz */
+	hw->wiphy->max_adj_channel_rssi_comp = 3 * 5;
 
 	/* Extract MAC address */
 	memcpy(mvm->addresses[0].addr, mvm->nvm_data->hw_addr, ETH_ALEN);
@@ -454,15 +457,17 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 	    device_can_wakeup(mvm->trans->dev)) {
 		mvm->wowlan.flags = WIPHY_WOWLAN_ANY;
 		hw->wiphy->wowlan = &mvm->wowlan;
-	} else if (mvm->fw->img[IWL_UCODE_WOWLAN].sec[0].len &&
+	}
+
+	if (mvm->fw->img[IWL_UCODE_WOWLAN].sec[0].len &&
 	    mvm->trans->ops->d3_suspend &&
 	    mvm->trans->ops->d3_resume &&
 	    device_can_wakeup(mvm->trans->dev)) {
-		mvm->wowlan.flags = WIPHY_WOWLAN_MAGIC_PKT |
-				    WIPHY_WOWLAN_DISCONNECT |
-				    WIPHY_WOWLAN_EAP_IDENTITY_REQ |
-				    WIPHY_WOWLAN_RFKILL_RELEASE |
-				    WIPHY_WOWLAN_NET_DETECT;
+		mvm->wowlan.flags |= WIPHY_WOWLAN_MAGIC_PKT |
+				     WIPHY_WOWLAN_DISCONNECT |
+				     WIPHY_WOWLAN_EAP_IDENTITY_REQ |
+				     WIPHY_WOWLAN_RFKILL_RELEASE |
+				     WIPHY_WOWLAN_NET_DETECT;
 		if (!iwlwifi_mod_params.sw_crypto)
 			mvm->wowlan.flags |= WIPHY_WOWLAN_SUPPORTS_GTK_REKEY |
 					     WIPHY_WOWLAN_GTK_REKEY_FAILURE |
@@ -761,22 +766,37 @@ void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
 	struct iwl_fw_error_dump_file *dump_file;
 	struct iwl_fw_error_dump_data *dump_data;
 	struct iwl_fw_error_dump_info *dump_info;
+	struct iwl_fw_error_dump_mem *dump_mem;
 	struct iwl_mvm_dump_ptrs *fw_error_dump;
-	const struct fw_img *img;
 	u32 sram_len, sram_ofs;
 	u32 file_len, rxf_len;
 	unsigned long flags;
 	int reg_val;
+	u32 smem_len = mvm->cfg->smem_len;
 
 	lockdep_assert_held(&mvm->mutex);
 
+	/* W/A for 8000 HW family A-step */
+	if (mvm->cfg->smem_len &&
+	    mvm->cfg->device_family == IWL_DEVICE_FAMILY_8000 &&
+	    CSR_HW_REV_STEP(mvm->trans->hw_rev) == SILICON_A_STEP)
+		smem_len = 0x38000;
+
 	fw_error_dump = kzalloc(sizeof(*fw_error_dump), GFP_KERNEL);
 	if (!fw_error_dump)
 		return;
 
-	img = &mvm->fw->img[mvm->cur_ucode];
-	sram_ofs = img->sec[IWL_UCODE_SECTION_DATA].offset;
-	sram_len = img->sec[IWL_UCODE_SECTION_DATA].len;
+	/* SRAM - include stack CCM if driver knows the values for it */
+	if (!mvm->cfg->dccm_offset || !mvm->cfg->dccm_len) {
+		const struct fw_img *img;
+
+		img = &mvm->fw->img[mvm->cur_ucode];
+		sram_ofs = img->sec[IWL_UCODE_SECTION_DATA].offset;
+		sram_len = img->sec[IWL_UCODE_SECTION_DATA].len;
+	} else {
+		sram_ofs = mvm->cfg->dccm_offset;
+		sram_len = mvm->cfg->dccm_len;
+	}
 
 	/* reading buffer size */
 	reg_val = iwl_trans_read_prph(mvm->trans, RXF_SIZE_ADDR);
@@ -787,10 +807,14 @@ void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
 
 	file_len = sizeof(*dump_file) +
 		   sizeof(*dump_data) * 3 +
-		   sram_len +
+		   sram_len + sizeof(*dump_mem) +
 		   rxf_len +
 		   sizeof(*dump_info);
 
+	/* Make room for the SMEM, if it exists */
+	if (smem_len)
+		file_len += sizeof(*dump_data) + sizeof(*dump_mem) + smem_len;
+
 	dump_file = vzalloc(file_len);
 	if (!dump_file) {
 		kfree(fw_error_dump);
@@ -809,6 +833,7 @@ void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
 		mvm->cfg->device_family == IWL_DEVICE_FAMILY_7000 ?
 			cpu_to_le32(IWL_FW_ERROR_DUMP_FAMILY_7) :
 			cpu_to_le32(IWL_FW_ERROR_DUMP_FAMILY_8);
+	dump_info->hw_step = cpu_to_le32(CSR_HW_REV_STEP(mvm->trans->hw_rev));
 	memcpy(dump_info->fw_human_readable, mvm->fw->human_readable,
 	       sizeof(dump_info->fw_human_readable));
 	strncpy(dump_info->dev_human_readable, mvm->cfg->name,
@@ -835,11 +860,25 @@ void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
 	}
 
 	dump_data = iwl_fw_error_next_data(dump_data);
-	dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_SRAM);
-	dump_data->len = cpu_to_le32(sram_len);
-	iwl_trans_read_mem_bytes(mvm->trans, sram_ofs, dump_data->data,
+	dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM);
+	dump_data->len = cpu_to_le32(sram_len + sizeof(*dump_mem));
+	dump_mem = (void *)dump_data->data;
+	dump_mem->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM_SRAM);
+	dump_mem->offset = cpu_to_le32(sram_ofs);
+	iwl_trans_read_mem_bytes(mvm->trans, sram_ofs, dump_mem->data,
 				 sram_len);
 
+	if (smem_len) {
+		dump_data = iwl_fw_error_next_data(dump_data);
+		dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM);
+		dump_data->len = cpu_to_le32(smem_len + sizeof(*dump_mem));
+		dump_mem = (void *)dump_data->data;
+		dump_mem->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM_SMEM);
+		dump_mem->offset = cpu_to_le32(mvm->cfg->smem_offset);
+		iwl_trans_read_mem_bytes(mvm->trans, mvm->cfg->smem_offset,
+					 dump_mem->data, smem_len);
+	}
+
 	fw_error_dump->trans_ptr = iwl_trans_dump_data(mvm->trans);
 	fw_error_dump->op_mode_len = file_len;
 	if (fw_error_dump->trans_ptr)
@@ -859,6 +898,11 @@ static void iwl_mvm_restart_cleanup(struct iwl_mvm *mvm)
 	if (!test_and_clear_bit(IWL_MVM_STATUS_D3_RECONFIG, &mvm->status))
 		iwl_mvm_fw_error_dump(mvm);
 
+	/* cleanup all stale references (scan, roc), but keep the
+	 * ucode_down ref until reconfig is complete
+	 */
+	iwl_mvm_unref_all_except(mvm, IWL_MVM_REF_UCODE_DOWN);
+
 	iwl_trans_stop_device(mvm->trans);
 
 	mvm->scan_status = IWL_MVM_SCAN_NONE;
@@ -888,10 +932,6 @@ static void iwl_mvm_restart_cleanup(struct iwl_mvm *mvm)
 
 	ieee80211_wake_queues(mvm->hw);
 
-	/* cleanup all stale references (scan, roc), but keep the
-	 * ucode_down ref until reconfig is complete */
-	iwl_mvm_unref_all_except(mvm, IWL_MVM_REF_UCODE_DOWN);
-
 	/* clear any stale d0i3 state */
 	clear_bit(IWL_MVM_STATUS_IN_D0I3, &mvm->status);
 
@@ -928,6 +968,19 @@ static int iwl_mvm_mac_start(struct ieee80211_hw *hw)
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
 	int ret;
 
+	/* Some hw restart cleanups must not hold the mutex */
+	if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
+		/*
+		 * Make sure we are out of d0i3. This is needed
+		 * to make sure the reference accounting is correct
+		 * (and there is no stale d0i3_exit_work).
+		 */
+		wait_event_timeout(mvm->d0i3_exit_waitq,
+				   !test_bit(IWL_MVM_STATUS_IN_D0I3,
+					     &mvm->status),
+				   HZ);
+	}
+
 	mutex_lock(&mvm->mutex);
 	ret = __iwl_mvm_mac_start(mvm);
 	mutex_unlock(&mvm->mutex);
@@ -977,6 +1030,13 @@ static void iwl_mvm_resume_complete(struct iwl_mvm *mvm)
 		IWL_DEBUG_RPM(mvm, "Run deferred d0i3 exit\n");
 		_iwl_mvm_exit_d0i3(mvm);
 	}
+
+	if (mvm->trans->d0i3_mode == IWL_D0I3_MODE_ON_SUSPEND)
+		if (!wait_event_timeout(mvm->d0i3_exit_waitq,
+					!test_bit(IWL_MVM_STATUS_IN_D0I3,
+						  &mvm->status),
+					HZ))
+			WARN_ONCE(1, "D0i3 exit on resume timed out\n");
 }
 
 static void
@@ -999,8 +1059,13 @@ void __iwl_mvm_mac_stop(struct iwl_mvm *mvm)
 {
 	lockdep_assert_held(&mvm->mutex);
 
-	/* disallow low power states when the FW is down */
-	iwl_mvm_ref(mvm, IWL_MVM_REF_UCODE_DOWN);
+	/*
+	 * Disallow low power states when the FW is down by taking
+	 * the UCODE_DOWN ref. in case of ongoing hw restart the
+	 * ref is already taken, so don't take it again.
+	 */
+	if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
+		iwl_mvm_ref(mvm, IWL_MVM_REF_UCODE_DOWN);
 
 	/* async_handlers_wk is now blocked */
 
@@ -1018,6 +1083,12 @@ void __iwl_mvm_mac_stop(struct iwl_mvm *mvm)
 	/* the fw is stopped, the aux sta is dead: clean up driver state */
 	iwl_mvm_del_aux_sta(mvm);
 
+	/*
+	 * Clear IN_HW_RESTART flag when stopping the hw (as restart_complete()
+	 * won't be called in this case).
+	 */
+	clear_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status);
+
 	mvm->ucode_loaded = false;
 }
 
@@ -2076,7 +2147,7 @@ static void iwl_mvm_sta_pre_rcu_remove(struct ieee80211_hw *hw,
 				       struct ieee80211_sta *sta)
 {
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
-	struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
+	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 
 	/*
 	 * This is called before mac80211 does RCU synchronisation,
@@ -3091,7 +3162,7 @@ static int iwl_mvm_set_tim(struct ieee80211_hw *hw,
 			   bool set)
 {
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
-	struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
+	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 
 	if (!mvm_sta || !mvm_sta->vif) {
 		IWL_ERR(mvm, "Station is not associated to a vif\n");
diff --git a/drivers/net/wireless/iwlwifi/mvm/mvm.h b/drivers/net/wireless/iwlwifi/mvm/mvm.h
index d24660fb4ef2..b2100b414055 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mvm.h
+++ b/drivers/net/wireless/iwlwifi/mvm/mvm.h
@@ -850,6 +850,8 @@ iwl_mvm_sta_from_staid_protected(struct iwl_mvm *mvm, u8 sta_id)
 static inline bool iwl_mvm_is_d0i3_supported(struct iwl_mvm *mvm)
 {
 	return mvm->trans->cfg->d0i3 &&
+	       mvm->trans->d0i3_mode != IWL_D0I3_MODE_OFF &&
+	       !iwlwifi_mod_params.d0i3_disable &&
 	       (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_D0I3_SUPPORT);
 }
 
@@ -937,6 +939,33 @@ int iwl_mvm_rx_statistics(struct iwl_mvm *mvm,
 int iwl_nvm_init(struct iwl_mvm *mvm, bool read_nvm_from_nic);
 int iwl_mvm_load_nvm_to_nic(struct iwl_mvm *mvm);
 
+static inline u8 iwl_mvm_get_valid_tx_ant(struct iwl_mvm *mvm)
+{
+	return mvm->nvm_data && mvm->nvm_data->valid_tx_ant ?
+	       mvm->fw->valid_tx_ant & mvm->nvm_data->valid_tx_ant :
+	       mvm->fw->valid_tx_ant;
+}
+
+static inline u8 iwl_mvm_get_valid_rx_ant(struct iwl_mvm *mvm)
+{
+	return mvm->nvm_data && mvm->nvm_data->valid_rx_ant ?
+	       mvm->fw->valid_rx_ant & mvm->nvm_data->valid_rx_ant :
+	       mvm->fw->valid_rx_ant;
+}
+
+static inline u32 iwl_mvm_get_phy_config(struct iwl_mvm *mvm)
+{
+	u32 phy_config = ~(FW_PHY_CFG_TX_CHAIN |
+			   FW_PHY_CFG_RX_CHAIN);
+	u32 valid_rx_ant = iwl_mvm_get_valid_rx_ant(mvm);
+	u32 valid_tx_ant = iwl_mvm_get_valid_tx_ant(mvm);
+
+	phy_config |= valid_tx_ant << FW_PHY_CFG_TX_CHAIN_POS |
+		      valid_rx_ant << FW_PHY_CFG_RX_CHAIN_POS;
+
+	return mvm->fw->phy_config & phy_config;
+}
+
 int iwl_mvm_up(struct iwl_mvm *mvm);
 int iwl_mvm_load_d3_fw(struct iwl_mvm *mvm);
 
@@ -1159,6 +1188,8 @@ void iwl_mvm_unref(struct iwl_mvm *mvm, enum iwl_mvm_ref_type ref_type);
 int iwl_mvm_ref_sync(struct iwl_mvm *mvm, enum iwl_mvm_ref_type ref_type);
 bool iwl_mvm_ref_taken(struct iwl_mvm *mvm);
 void iwl_mvm_d0i3_enable_tx(struct iwl_mvm *mvm, __le16 *qos_seq);
+int iwl_mvm_enter_d0i3(struct iwl_op_mode *op_mode);
+int iwl_mvm_exit_d0i3(struct iwl_op_mode *op_mode);
 int _iwl_mvm_exit_d0i3(struct iwl_mvm *mvm);
 
 /* BT Coex */
@@ -1344,4 +1375,7 @@ struct ieee80211_vif *iwl_mvm_get_bss_vif(struct iwl_mvm *mvm);
 void iwl_mvm_nic_restart(struct iwl_mvm *mvm, bool fw_error);
 void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm);
 
+int iwl_mvm_start_fw_dbg_conf(struct iwl_mvm *mvm, enum iwl_fw_dbg_conf id);
+void iwl_mvm_fw_dbg_collect(struct iwl_mvm *mvm);
+
 #endif /* __IWL_MVM_H__ */
diff --git a/drivers/net/wireless/iwlwifi/mvm/ops.c b/drivers/net/wireless/iwlwifi/mvm/ops.c
index 97dfba50c682..239f033e3b93 100644
--- a/drivers/net/wireless/iwlwifi/mvm/ops.c
+++ b/drivers/net/wireless/iwlwifi/mvm/ops.c
@@ -84,15 +84,8 @@
 #include "time-event.h"
 #include "iwl-fw-error-dump.h"
 
-/*
- * module name, copyright, version, etc.
- */
 #define DRV_DESCRIPTION	"The new Intel(R) wireless AGN driver for Linux"
-
-#define DRV_VERSION     IWLWIFI_VERSION
-
 MODULE_DESCRIPTION(DRV_DESCRIPTION);
-MODULE_VERSION(DRV_VERSION);
 MODULE_AUTHOR(DRV_COPYRIGHT " " DRV_AUTHOR);
 MODULE_LICENSE("GPL");
 
@@ -146,13 +139,14 @@ static void iwl_mvm_nic_config(struct iwl_op_mode *op_mode)
 	struct iwl_mvm *mvm = IWL_OP_MODE_GET_MVM(op_mode);
 	u8 radio_cfg_type, radio_cfg_step, radio_cfg_dash;
 	u32 reg_val = 0;
+	u32 phy_config = iwl_mvm_get_phy_config(mvm);
 
-	radio_cfg_type = (mvm->fw->phy_config & FW_PHY_CFG_RADIO_TYPE) >>
-			  FW_PHY_CFG_RADIO_TYPE_POS;
-	radio_cfg_step = (mvm->fw->phy_config & FW_PHY_CFG_RADIO_STEP) >>
-			  FW_PHY_CFG_RADIO_STEP_POS;
-	radio_cfg_dash = (mvm->fw->phy_config & FW_PHY_CFG_RADIO_DASH) >>
-			  FW_PHY_CFG_RADIO_DASH_POS;
+	radio_cfg_type = (phy_config & FW_PHY_CFG_RADIO_TYPE) >>
+			 FW_PHY_CFG_RADIO_TYPE_POS;
+	radio_cfg_step = (phy_config & FW_PHY_CFG_RADIO_STEP) >>
+			 FW_PHY_CFG_RADIO_STEP_POS;
+	radio_cfg_dash = (phy_config & FW_PHY_CFG_RADIO_DASH) >>
+			 FW_PHY_CFG_RADIO_DASH_POS;
 
 	/* SKU control */
 	reg_val |= CSR_HW_REV_STEP(mvm->trans->hw_rev) <<
@@ -487,6 +481,8 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 	trans_cfg.cmd_fifo = IWL_MVM_TX_FIFO_CMD;
 	trans_cfg.scd_set_active = true;
 
+	trans_cfg.sdio_adma_addr = fw->sdio_adma_addr;
+
 	snprintf(mvm->hw->wiphy->fw_version,
 		 sizeof(mvm->hw->wiphy->fw_version),
 		 "%s", fw->fw_version);
@@ -517,10 +513,15 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 	min_backoff = calc_min_backoff(trans, cfg);
 	iwl_mvm_tt_initialize(mvm, min_backoff);
 	/* set the nvm_file_name according to priority */
-	if (iwlwifi_mod_params.nvm_file)
+	if (iwlwifi_mod_params.nvm_file) {
 		mvm->nvm_file_name = iwlwifi_mod_params.nvm_file;
-	else
-		mvm->nvm_file_name = mvm->cfg->default_nvm_file;
+	} else {
+		if ((trans->cfg->device_family == IWL_DEVICE_FAMILY_8000) &&
+		    (CSR_HW_REV_STEP(trans->hw_rev) == SILICON_A_STEP))
+			mvm->nvm_file_name = mvm->cfg->default_nvm_file_8000A;
+		else
+			mvm->nvm_file_name = mvm->cfg->default_nvm_file;
+	}
 
 	if (WARN(cfg->no_power_up_nic_in_init && !mvm->nvm_file_name,
 		 "not allowing power-up and not having nvm_file\n"))
@@ -1031,7 +1032,8 @@ static void iwl_mvm_set_wowlan_data(struct iwl_mvm *mvm,
 out:
 	rcu_read_unlock();
 }
-static int iwl_mvm_enter_d0i3(struct iwl_op_mode *op_mode)
+
+int iwl_mvm_enter_d0i3(struct iwl_op_mode *op_mode)
 {
 	struct iwl_mvm *mvm = IWL_OP_MODE_GET_MVM(op_mode);
 	u32 flags = CMD_ASYNC | CMD_HIGH_PRIO | CMD_SEND_IN_IDLE;
@@ -1047,6 +1049,7 @@ static int iwl_mvm_enter_d0i3(struct iwl_op_mode *op_mode)
 	};
 	struct iwl_d3_manager_config d3_cfg_cmd = {
 		.min_sleep_time = cpu_to_le32(1000),
+		.wakeup_flags = cpu_to_le32(IWL_WAKEUP_D3_CONFIG_FW_ERROR),
 	};
 
 	IWL_DEBUG_RPM(mvm, "MVM entering D0i3\n");
@@ -1146,7 +1149,7 @@ void iwl_mvm_d0i3_enable_tx(struct iwl_mvm *mvm, __le16 *qos_seq)
 
 	if (mvm->d0i3_offloading && qos_seq) {
 		/* update qos seq numbers if offloading was enabled */
-		mvm_ap_sta = (struct iwl_mvm_sta *)sta->drv_priv;
+		mvm_ap_sta = iwl_mvm_sta_from_mac80211(sta);
 		for (i = 0; i < IWL_MAX_TID_COUNT; i++) {
 			u16 seq = le16_to_cpu(qos_seq[i]);
 			/* firmware stores last-used one, we store next one */
@@ -1245,7 +1248,7 @@ out:
 	return ret;
 }
 
-static int iwl_mvm_exit_d0i3(struct iwl_op_mode *op_mode)
+int iwl_mvm_exit_d0i3(struct iwl_op_mode *op_mode)
 {
 	struct iwl_mvm *mvm = IWL_OP_MODE_GET_MVM(op_mode);
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c b/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c
index 1c0d4a45c1a8..540c36bae268 100644
--- a/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c
+++ b/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c
@@ -176,7 +176,7 @@ static void iwl_mvm_phy_ctxt_cmd_data(struct iwl_mvm *mvm,
 	cmd->rxchain_info |= cpu_to_le32(active_cnt <<
 					 PHY_RX_CHAIN_MIMO_CNT_POS);
 
-	cmd->txchain_info = cpu_to_le32(mvm->fw->valid_tx_ant);
+	cmd->txchain_info = cpu_to_le32(iwl_mvm_get_valid_tx_ant(mvm));
 }
 
 /*
diff --git a/drivers/net/wireless/iwlwifi/mvm/rs.c b/drivers/net/wireless/iwlwifi/mvm/rs.c
index 30ceb67ed7a7..7ba6e5dbbbd0 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rs.c
+++ b/drivers/net/wireless/iwlwifi/mvm/rs.c
@@ -42,25 +42,12 @@
 
 #define RS_NAME "iwl-mvm-rs"
 
-#define NUM_TRY_BEFORE_ANT_TOGGLE       1
-#define RS_LEGACY_RETRIES_PER_RATE      1
-#define RS_HT_VHT_RETRIES_PER_RATE      2
-#define RS_HT_VHT_RETRIES_PER_RATE_TW   1
-#define RS_INITIAL_MIMO_NUM_RATES       3
-#define RS_INITIAL_SISO_NUM_RATES       3
-#define RS_INITIAL_LEGACY_NUM_RATES     LINK_QUAL_MAX_RETRY_NUM
-#define RS_SECONDARY_LEGACY_NUM_RATES   LINK_QUAL_MAX_RETRY_NUM
-#define RS_SECONDARY_SISO_NUM_RATES     3
-#define RS_SECONDARY_SISO_RETRIES       1
-
 #define IWL_RATE_MAX_WINDOW		62	/* # tx in history window */
-#define IWL_RATE_MIN_FAILURE_TH		3	/* min failures to calc tpt */
-#define IWL_RATE_MIN_SUCCESS_TH		8	/* min successes to calc tpt */
 
-/* max allowed rate miss before sync LQ cmd */
-#define IWL_MISSED_RATE_MAX		15
-#define RS_STAY_IN_COLUMN_TIMEOUT       (5*HZ)
-#define RS_IDLE_TIMEOUT                 (5*HZ)
+/* Calculations of success ratio are done in fixed point where 12800 is 100%.
+ * Use this macro when dealing with thresholds consts set as a percentage
+ */
+#define RS_PERCENT(x) (128 * x)
 
 static u8 rs_ht_to_legacy[] = {
 	[IWL_RATE_MCS_0_INDEX] = IWL_RATE_6M_INDEX,
@@ -173,7 +160,7 @@ static bool rs_mimo_allow(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 	if (sta->smps_mode == IEEE80211_SMPS_STATIC)
 		return false;
 
-	if (num_of_ant(mvm->fw->valid_tx_ant) < 2)
+	if (num_of_ant(iwl_mvm_get_valid_tx_ant(mvm)) < 2)
 		return false;
 
 	if (!iwl_mvm_bt_coex_is_mimo_allowed(mvm, sta))
@@ -613,7 +600,8 @@ static s32 get_expected_tpt(struct iwl_scale_tbl_info *tbl, int rs_index)
  * at this rate.  window->data contains the bitmask of successful
  * packets.
  */
-static int _rs_collect_tx_data(struct iwl_scale_tbl_info *tbl,
+static int _rs_collect_tx_data(struct iwl_mvm *mvm,
+			       struct iwl_scale_tbl_info *tbl,
 			       int scale_index, int attempts, int successes,
 			       struct iwl_rate_scale_data *window)
 {
@@ -668,8 +656,8 @@ static int _rs_collect_tx_data(struct iwl_scale_tbl_info *tbl,
 	fail_count = window->counter - window->success_counter;
 
 	/* Calculate average throughput, if we have enough history. */
-	if ((fail_count >= IWL_RATE_MIN_FAILURE_TH) ||
-	    (window->success_counter >= IWL_RATE_MIN_SUCCESS_TH))
+	if ((fail_count >= IWL_MVM_RS_RATE_MIN_FAILURE_TH) ||
+	    (window->success_counter >= IWL_MVM_RS_RATE_MIN_SUCCESS_TH))
 		window->average_tpt = (window->success_ratio * tpt + 64) / 128;
 	else
 		window->average_tpt = IWL_INVALID_VALUE;
@@ -677,7 +665,8 @@ static int _rs_collect_tx_data(struct iwl_scale_tbl_info *tbl,
 	return 0;
 }
 
-static int rs_collect_tx_data(struct iwl_lq_sta *lq_sta,
+static int rs_collect_tx_data(struct iwl_mvm *mvm,
+			      struct iwl_lq_sta *lq_sta,
 			      struct iwl_scale_tbl_info *tbl,
 			      int scale_index, int attempts, int successes,
 			      u8 reduced_txp)
@@ -698,7 +687,7 @@ static int rs_collect_tx_data(struct iwl_lq_sta *lq_sta,
 	/* Select window for current tx bit rate */
 	window = &(tbl->win[scale_index]);
 
-	ret = _rs_collect_tx_data(tbl, scale_index, attempts, successes,
+	ret = _rs_collect_tx_data(mvm, tbl, scale_index, attempts, successes,
 				  window);
 	if (ret)
 		return ret;
@@ -707,7 +696,7 @@ static int rs_collect_tx_data(struct iwl_lq_sta *lq_sta,
 		return -EINVAL;
 
 	window = &tbl->tpc_win[reduced_txp];
-	return _rs_collect_tx_data(tbl, scale_index, attempts, successes,
+	return _rs_collect_tx_data(mvm, tbl, scale_index, attempts, successes,
 				   window);
 }
 
@@ -1004,7 +993,7 @@ static void rs_get_lower_rate_down_column(struct iwl_lq_sta *lq_sta,
 	}
 
 	if (num_of_ant(rate->ant) > 1)
-		rate->ant = first_antenna(mvm->fw->valid_tx_ant);
+		rate->ant = first_antenna(iwl_mvm_get_valid_tx_ant(mvm));
 
 	/* Relevant in both switching to SISO or Legacy */
 	rate->sgi = false;
@@ -1125,7 +1114,8 @@ void iwl_mvm_rs_tx_status(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 	}
 
 	if (time_after(jiffies,
-		       (unsigned long)(lq_sta->last_tx + RS_IDLE_TIMEOUT))) {
+		       (unsigned long)(lq_sta->last_tx +
+				       (IWL_MVM_RS_IDLE_TIMEOUT * HZ)))) {
 		int t;
 
 		IWL_DEBUG_RATE(mvm, "Tx idle for too long. reinit rs\n");
@@ -1158,7 +1148,7 @@ void iwl_mvm_rs_tx_status(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 		 * ... driver.
 		 */
 		lq_sta->missed_rate_counter++;
-		if (lq_sta->missed_rate_counter > IWL_MISSED_RATE_MAX) {
+		if (lq_sta->missed_rate_counter > IWL_MVM_RS_MISSED_RATE_MAX) {
 			lq_sta->missed_rate_counter = 0;
 			IWL_DEBUG_RATE(mvm,
 				       "Too many rates mismatch. Send sync LQ. rs_state %d\n",
@@ -1213,7 +1203,7 @@ void iwl_mvm_rs_tx_status(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 
 		ucode_rate = le32_to_cpu(table->rs_table[0]);
 		rs_rate_from_ucode_rate(ucode_rate, info->band, &rate);
-		rs_collect_tx_data(lq_sta, curr_tbl, rate.index,
+		rs_collect_tx_data(mvm, lq_sta, curr_tbl, rate.index,
 				   info->status.ampdu_len,
 				   info->status.ampdu_ack_len,
 				   reduced_txp);
@@ -1249,7 +1239,7 @@ void iwl_mvm_rs_tx_status(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 			else
 				continue;
 
-			rs_collect_tx_data(lq_sta, tmp_tbl, rate.index, 1,
+			rs_collect_tx_data(mvm, lq_sta, tmp_tbl, rate.index, 1,
 					   i < retries ? 0 : legacy_success,
 					   reduced_txp);
 		}
@@ -1303,13 +1293,13 @@ static void rs_set_stay_in_table(struct iwl_mvm *mvm, u8 is_legacy,
 	IWL_DEBUG_RATE(mvm, "Moving to RS_STATE_STAY_IN_COLUMN\n");
 	lq_sta->rs_state = RS_STATE_STAY_IN_COLUMN;
 	if (is_legacy) {
-		lq_sta->table_count_limit = IWL_LEGACY_TABLE_COUNT;
-		lq_sta->max_failure_limit = IWL_LEGACY_FAILURE_LIMIT;
-		lq_sta->max_success_limit = IWL_LEGACY_SUCCESS_LIMIT;
+		lq_sta->table_count_limit = IWL_MVM_RS_LEGACY_TABLE_COUNT;
+		lq_sta->max_failure_limit = IWL_MVM_RS_LEGACY_FAILURE_LIMIT;
+		lq_sta->max_success_limit = IWL_MVM_RS_LEGACY_SUCCESS_LIMIT;
 	} else {
-		lq_sta->table_count_limit = IWL_NONE_LEGACY_TABLE_COUNT;
-		lq_sta->max_failure_limit = IWL_NONE_LEGACY_FAILURE_LIMIT;
-		lq_sta->max_success_limit = IWL_NONE_LEGACY_SUCCESS_LIMIT;
+		lq_sta->table_count_limit = IWL_MVM_RS_NON_LEGACY_TABLE_COUNT;
+		lq_sta->max_failure_limit = IWL_MVM_RS_NON_LEGACY_FAILURE_LIMIT;
+		lq_sta->max_success_limit = IWL_MVM_RS_NON_LEGACY_SUCCESS_LIMIT;
 	}
 	lq_sta->table_count = 0;
 	lq_sta->total_failed = 0;
@@ -1318,6 +1308,13 @@ static void rs_set_stay_in_table(struct iwl_mvm *mvm, u8 is_legacy,
 	lq_sta->visited_columns = 0;
 }
 
+static inline int rs_get_max_rate_from_mask(unsigned long rate_mask)
+{
+	if (rate_mask)
+		return find_last_bit(&rate_mask, BITS_PER_LONG);
+	return IWL_RATE_INVALID;
+}
+
 static int rs_get_max_allowed_rate(struct iwl_lq_sta *lq_sta,
 				   const struct rs_tx_column *column)
 {
@@ -1420,7 +1417,7 @@ static s32 rs_get_best_rate(struct iwl_mvm *mvm,
 	u32 target_tpt;
 	int rate_idx;
 
-	if (success_ratio > RS_SR_NO_DECREASE) {
+	if (success_ratio > IWL_MVM_RS_SR_NO_DECREASE) {
 		target_tpt = 100 * expected_current_tpt;
 		IWL_DEBUG_RATE(mvm,
 			       "SR %d high. Find rate exceeding EXPECTED_CURRENT %d\n",
@@ -1488,7 +1485,7 @@ static void rs_stay_in_table(struct iwl_lq_sta *lq_sta, bool force_search)
 			flush_interval_passed =
 				time_after(jiffies,
 					   (unsigned long)(lq_sta->flush_timer +
-						RS_STAY_IN_COLUMN_TIMEOUT));
+							   (IWL_MVM_RS_STAY_IN_COLUMN_TIMEOUT * HZ)));
 
 		/*
 		 * Check if we should allow search for new modulation mode.
@@ -1567,7 +1564,7 @@ static enum rs_column rs_get_next_column(struct iwl_mvm *mvm,
 	const struct rs_tx_column *curr_col = &rs_tx_columns[tbl->column];
 	const struct rs_tx_column *next_col;
 	allow_column_func_t allow_func;
-	u8 valid_ants = mvm->fw->valid_tx_ant;
+	u8 valid_ants = iwl_mvm_get_valid_tx_ant(mvm);
 	const u16 *expected_tpt_tbl;
 	u16 tpt, max_expected_tpt;
 
@@ -1613,8 +1610,12 @@ static enum rs_column rs_get_next_column(struct iwl_mvm *mvm,
 			continue;
 
 		max_rate = rs_get_max_allowed_rate(lq_sta, next_col);
-		if (WARN_ON_ONCE(max_rate == IWL_RATE_INVALID))
+		if (max_rate == IWL_RATE_INVALID) {
+			IWL_DEBUG_RATE(mvm,
+				       "Skip column %d: no rate is allowed in this column\n",
+				       next_col_id);
 			continue;
+		}
 
 		max_expected_tpt = expected_tpt_tbl[max_rate];
 		if (tpt >= max_expected_tpt) {
@@ -1724,7 +1725,8 @@ static enum rs_action rs_get_rate_action(struct iwl_mvm *mvm,
 {
 	enum rs_action action = RS_ACTION_STAY;
 
-	if ((sr <= RS_SR_FORCE_DECREASE) || (current_tpt == 0)) {
+	if ((sr <= RS_PERCENT(IWL_MVM_RS_SR_FORCE_DECREASE)) ||
+	    (current_tpt == 0)) {
 		IWL_DEBUG_RATE(mvm,
 			       "Decrease rate because of low SR\n");
 		return RS_ACTION_DOWNSCALE;
@@ -1783,7 +1785,7 @@ static enum rs_action rs_get_rate_action(struct iwl_mvm *mvm,
 
 out:
 	if ((action == RS_ACTION_DOWNSCALE) && (low != IWL_RATE_INVALID)) {
-		if (sr >= RS_SR_NO_DECREASE) {
+		if (sr >= RS_PERCENT(IWL_MVM_RS_SR_NO_DECREASE)) {
 			IWL_DEBUG_RATE(mvm,
 				       "SR is above NO DECREASE. Avoid downscale\n");
 			action = RS_ACTION_STAY;
@@ -1825,11 +1827,11 @@ static bool rs_stbc_allow(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 static void rs_get_adjacent_txp(struct iwl_mvm *mvm, int index,
 				int *weaker, int *stronger)
 {
-	*weaker = index + TPC_TX_POWER_STEP;
+	*weaker = index + IWL_MVM_RS_TPC_TX_POWER_STEP;
 	if (*weaker > TPC_MAX_REDUCTION)
 		*weaker = TPC_INVALID;
 
-	*stronger = index - TPC_TX_POWER_STEP;
+	*stronger = index - IWL_MVM_RS_TPC_TX_POWER_STEP;
 	if (*stronger < 0)
 		*stronger = TPC_INVALID;
 }
@@ -1885,7 +1887,8 @@ static enum tpc_action rs_get_tpc_action(struct iwl_mvm *mvm,
 	}
 
 	/* Too many failures, increase txp */
-	if (sr <= TPC_SR_FORCE_INCREASE || current_tpt == 0) {
+	if (sr <= RS_PERCENT(IWL_MVM_RS_TPC_SR_FORCE_INCREASE) ||
+	    current_tpt == 0) {
 		IWL_DEBUG_RATE(mvm, "increase txp because of weak SR\n");
 		return TPC_ACTION_NO_RESTIRCTION;
 	}
@@ -1908,7 +1911,8 @@ static enum tpc_action rs_get_tpc_action(struct iwl_mvm *mvm,
 	}
 
 	/* next, increase if needed */
-	if (sr < TPC_SR_NO_INCREASE && strong != TPC_INVALID) {
+	if (sr < RS_PERCENT(IWL_MVM_RS_TPC_SR_NO_INCREASE) &&
+	    strong != TPC_INVALID) {
 		if (weak_tpt == IWL_INVALID_VALUE &&
 		    strong_tpt != IWL_INVALID_VALUE &&
 		    current_tpt < strong_tpt) {
@@ -1935,7 +1939,7 @@ static bool rs_tpc_perform(struct iwl_mvm *mvm,
 			   struct iwl_lq_sta *lq_sta,
 			   struct iwl_scale_tbl_info *tbl)
 {
-	struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
+	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 	struct ieee80211_vif *vif = mvm_sta->vif;
 	struct ieee80211_chanctx_conf *chanctx_conf;
 	enum ieee80211_band band;
@@ -2044,7 +2048,7 @@ static void rs_rate_scale_perform(struct iwl_mvm *mvm,
 	u16 high_low;
 	s32 sr;
 	u8 prev_agg = lq_sta->is_agg;
-	struct iwl_mvm_sta *sta_priv = (void *)sta->drv_priv;
+	struct iwl_mvm_sta *sta_priv = iwl_mvm_sta_from_mac80211(sta);
 	struct iwl_mvm_tid_data *tid_data;
 	struct rs_rate *rate;
 
@@ -2106,8 +2110,8 @@ static void rs_rate_scale_perform(struct iwl_mvm *mvm,
 	 * in current association (use new rate found above).
 	 */
 	fail_count = window->counter - window->success_counter;
-	if ((fail_count < IWL_RATE_MIN_FAILURE_TH) &&
-	    (window->success_counter < IWL_RATE_MIN_SUCCESS_TH)) {
+	if ((fail_count < IWL_MVM_RS_RATE_MIN_FAILURE_TH) &&
+	    (window->success_counter < IWL_MVM_RS_RATE_MIN_SUCCESS_TH)) {
 		IWL_DEBUG_RATE(mvm,
 			       "(%s: %d): Test Window: succ %d total %d\n",
 			       rs_pretty_lq_type(rate->type),
@@ -2385,7 +2389,7 @@ static void rs_get_initial_rate(struct iwl_mvm *mvm,
 	int i, nentries;
 	s8 best_rssi = S8_MIN;
 	u8 best_ant = ANT_NONE;
-	u8 valid_tx_ant = mvm->fw->valid_tx_ant;
+	u8 valid_tx_ant = iwl_mvm_get_valid_tx_ant(mvm);
 	const struct rs_init_rate_info *initial_rates;
 
 	for (i = 0; i < ARRAY_SIZE(lq_sta->pers.chain_signal); i++) {
@@ -2530,7 +2534,7 @@ static void rs_get_rate(void *mvm_r, struct ieee80211_sta *sta, void *mvm_sta,
 static void *rs_alloc_sta(void *mvm_rate, struct ieee80211_sta *sta,
 			  gfp_t gfp)
 {
-	struct iwl_mvm_sta *sta_priv = (struct iwl_mvm_sta *)sta->drv_priv;
+	struct iwl_mvm_sta *sta_priv = iwl_mvm_sta_from_mac80211(sta);
 	struct iwl_op_mode *op_mode = (struct iwl_op_mode *)mvm_rate;
 	struct iwl_mvm *mvm  = IWL_OP_MODE_GET_MVM(op_mode);
 	struct iwl_lq_sta *lq_sta = &sta_priv->lq_sta;
@@ -2683,14 +2687,11 @@ void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 	struct ieee80211_hw *hw = mvm->hw;
 	struct ieee80211_sta_ht_cap *ht_cap = &sta->ht_cap;
 	struct ieee80211_sta_vht_cap *vht_cap = &sta->vht_cap;
-	struct iwl_mvm_sta *sta_priv;
-	struct iwl_lq_sta *lq_sta;
+	struct iwl_mvm_sta *sta_priv = iwl_mvm_sta_from_mac80211(sta);
+	struct iwl_lq_sta *lq_sta = &sta_priv->lq_sta;
 	struct ieee80211_supported_band *sband;
 	unsigned long supp; /* must be unsigned long for for_each_set_bit */
 
-	sta_priv = (struct iwl_mvm_sta *)sta->drv_priv;
-	lq_sta = &sta_priv->lq_sta;
-
 	/* clear all non-persistent lq data */
 	memset(lq_sta, 0, offsetof(typeof(*lq_sta), pers));
 
@@ -2712,7 +2713,7 @@ void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 	 * previous packets? Need to have IEEE 802.1X auth succeed immediately
 	 * after assoc.. */
 
-	lq_sta->missed_rate_counter = IWL_MISSED_RATE_MAX;
+	lq_sta->missed_rate_counter = IWL_MVM_RS_MISSED_RATE_MAX;
 	lq_sta->band = sband->band;
 	/*
 	 * active legacy rates as per supported rates bitmap
@@ -2745,7 +2746,7 @@ void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 			lq_sta->ldpc = true;
 
 		if (mvm->cfg->ht_params->stbc &&
-		    (num_of_ant(mvm->fw->valid_tx_ant) > 1) &&
+		    (num_of_ant(iwl_mvm_get_valid_tx_ant(mvm)) > 1) &&
 		    (ht_cap->cap & IEEE80211_HT_CAP_RX_STBC))
 			lq_sta->stbc = true;
 	} else {
@@ -2757,7 +2758,7 @@ void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 			lq_sta->ldpc = true;
 
 		if (mvm->cfg->ht_params->stbc &&
-		    (num_of_ant(mvm->fw->valid_tx_ant) > 1) &&
+		    (num_of_ant(iwl_mvm_get_valid_tx_ant(mvm)) > 1) &&
 		    (vht_cap->cap & IEEE80211_VHT_CAP_RXSTBC_MASK))
 			lq_sta->stbc = true;
 	}
@@ -2765,12 +2766,12 @@ void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 	if (IWL_MVM_RS_DISABLE_MIMO)
 		lq_sta->active_mimo2_rate = 0;
 
-	lq_sta->max_legacy_rate_idx = find_last_bit(&lq_sta->active_legacy_rate,
-						    BITS_PER_LONG);
-	lq_sta->max_siso_rate_idx = find_last_bit(&lq_sta->active_siso_rate,
-						  BITS_PER_LONG);
-	lq_sta->max_mimo2_rate_idx = find_last_bit(&lq_sta->active_mimo2_rate,
-						   BITS_PER_LONG);
+	lq_sta->max_legacy_rate_idx =
+		rs_get_max_rate_from_mask(lq_sta->active_legacy_rate);
+	lq_sta->max_siso_rate_idx =
+		rs_get_max_rate_from_mask(lq_sta->active_siso_rate);
+	lq_sta->max_mimo2_rate_idx =
+		rs_get_max_rate_from_mask(lq_sta->active_mimo2_rate);
 
 	IWL_DEBUG_RATE(mvm,
 		       "RATE MASK: LEGACY=%lX SISO=%lX MIMO2=%lX VHT=%d LDPC=%d STBC%d\n",
@@ -2785,7 +2786,7 @@ void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 
 	/* These values will be overridden later */
 	lq_sta->lq.single_stream_ant_msk =
-		first_antenna(mvm->fw->valid_tx_ant);
+		first_antenna(iwl_mvm_get_valid_tx_ant(mvm));
 	lq_sta->lq.dual_stream_ant_msk = ANT_AB;
 
 	/* as default allow aggregation for all tids */
@@ -2913,18 +2914,18 @@ static void rs_build_rates_table(struct iwl_mvm *mvm,
 
 	memcpy(&rate, initial_rate, sizeof(rate));
 
-	valid_tx_ant = mvm->fw->valid_tx_ant;
+	valid_tx_ant = iwl_mvm_get_valid_tx_ant(mvm);
 	rate.stbc = rs_stbc_allow(mvm, sta, lq_sta);
 
 	if (is_siso(&rate)) {
-		num_rates = RS_INITIAL_SISO_NUM_RATES;
-		num_retries = RS_HT_VHT_RETRIES_PER_RATE;
+		num_rates = IWL_MVM_RS_INITIAL_SISO_NUM_RATES;
+		num_retries = IWL_MVM_RS_HT_VHT_RETRIES_PER_RATE;
 	} else if (is_mimo(&rate)) {
-		num_rates = RS_INITIAL_MIMO_NUM_RATES;
-		num_retries = RS_HT_VHT_RETRIES_PER_RATE;
+		num_rates = IWL_MVM_RS_INITIAL_MIMO_NUM_RATES;
+		num_retries = IWL_MVM_RS_HT_VHT_RETRIES_PER_RATE;
 	} else {
-		num_rates = RS_INITIAL_LEGACY_NUM_RATES;
-		num_retries = RS_LEGACY_RETRIES_PER_RATE;
+		num_rates = IWL_MVM_RS_INITIAL_LEGACY_NUM_RATES;
+		num_retries = IWL_MVM_RS_LEGACY_RETRIES_PER_RATE;
 		toggle_ant = true;
 	}
 
@@ -2935,12 +2936,12 @@ static void rs_build_rates_table(struct iwl_mvm *mvm,
 	rs_get_lower_rate_down_column(lq_sta, &rate);
 
 	if (is_siso(&rate)) {
-		num_rates = RS_SECONDARY_SISO_NUM_RATES;
-		num_retries = RS_SECONDARY_SISO_RETRIES;
+		num_rates = IWL_MVM_RS_SECONDARY_SISO_NUM_RATES;
+		num_retries = IWL_MVM_RS_SECONDARY_SISO_RETRIES;
 		lq_cmd->mimo_delim = index;
 	} else if (is_legacy(&rate)) {
-		num_rates = RS_SECONDARY_LEGACY_NUM_RATES;
-		num_retries = RS_LEGACY_RETRIES_PER_RATE;
+		num_rates = IWL_MVM_RS_SECONDARY_LEGACY_NUM_RATES;
+		num_retries = IWL_MVM_RS_LEGACY_RETRIES_PER_RATE;
 	} else {
 		WARN_ON_ONCE(1);
 	}
@@ -2953,8 +2954,8 @@ static void rs_build_rates_table(struct iwl_mvm *mvm,
 
 	rs_get_lower_rate_down_column(lq_sta, &rate);
 
-	num_rates = RS_SECONDARY_LEGACY_NUM_RATES;
-	num_retries = RS_LEGACY_RETRIES_PER_RATE;
+	num_rates = IWL_MVM_RS_SECONDARY_LEGACY_NUM_RATES;
+	num_retries = IWL_MVM_RS_LEGACY_RETRIES_PER_RATE;
 
 	rs_fill_rates_for_column(mvm, lq_sta, &rate, lq_cmd->rs_table, &index,
 				 num_rates, num_retries, valid_tx_ant,
@@ -2971,9 +2972,9 @@ static void rs_fill_lq_cmd(struct iwl_mvm *mvm,
 	struct iwl_mvm_sta *mvmsta;
 	struct iwl_mvm_vif *mvmvif;
 
-	lq_cmd->agg_disable_start_th = LINK_QUAL_AGG_DISABLE_START_DEF;
+	lq_cmd->agg_disable_start_th = IWL_MVM_RS_AGG_DISABLE_START;
 	lq_cmd->agg_time_limit =
-		cpu_to_le16(LINK_QUAL_AGG_TIME_LIMIT_DEF);
+		cpu_to_le16(IWL_MVM_RS_AGG_TIME_LIMIT);
 
 #ifdef CONFIG_MAC80211_DEBUGFS
 	if (lq_sta->pers.dbg_fixed_rate) {
@@ -3167,9 +3168,9 @@ static ssize_t rs_sta_dbgfs_scale_table_read(struct file *file,
 	desc += sprintf(buff+desc, "fixed rate 0x%X\n",
 			lq_sta->pers.dbg_fixed_rate);
 	desc += sprintf(buff+desc, "valid_tx_ant %s%s%s\n",
-	    (mvm->fw->valid_tx_ant & ANT_A) ? "ANT_A," : "",
-	    (mvm->fw->valid_tx_ant & ANT_B) ? "ANT_B," : "",
-	    (mvm->fw->valid_tx_ant & ANT_C) ? "ANT_C" : "");
+	    (iwl_mvm_get_valid_tx_ant(mvm) & ANT_A) ? "ANT_A," : "",
+	    (iwl_mvm_get_valid_tx_ant(mvm) & ANT_B) ? "ANT_B," : "",
+	    (iwl_mvm_get_valid_tx_ant(mvm) & ANT_C) ? "ANT_C" : "");
 	desc += sprintf(buff+desc, "lq type %s\n",
 			(is_legacy(rate)) ? "legacy" :
 			is_vht(rate) ? "VHT" : "HT");
diff --git a/drivers/net/wireless/iwlwifi/mvm/rs.h b/drivers/net/wireless/iwlwifi/mvm/rs.h
index defd70a6d9e6..f8f5bf21cc38 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rs.h
+++ b/drivers/net/wireless/iwlwifi/mvm/rs.h
@@ -137,42 +137,10 @@ enum {
 
 #define IWL_INVALID_VALUE    -1
 
-#define IWL_MIN_RSSI_VAL                 -100
-#define IWL_MAX_RSSI_VAL                    0
-
-/* These values specify how many Tx frame attempts before
- * searching for a new modulation mode */
-#define IWL_LEGACY_FAILURE_LIMIT	160
-#define IWL_LEGACY_SUCCESS_LIMIT	480
-#define IWL_LEGACY_TABLE_COUNT		160
-
-#define IWL_NONE_LEGACY_FAILURE_LIMIT	400
-#define IWL_NONE_LEGACY_SUCCESS_LIMIT	4500
-#define IWL_NONE_LEGACY_TABLE_COUNT	1500
-
-/* Success ratio (ACKed / attempted tx frames) values (perfect is 128 * 100) */
-#define IWL_RS_GOOD_RATIO		12800	/* 100% */
-#define IWL_RATE_SCALE_SWITCH		10880	/*  85% */
-#define IWL_RATE_HIGH_TH		10880	/*  85% */
-#define IWL_RATE_INCREASE_TH		6400	/*  50% */
-#define RS_SR_FORCE_DECREASE		1920	/*  15% */
-#define RS_SR_NO_DECREASE		10880	/*  85% */
-
-#define TPC_SR_FORCE_INCREASE		9600	/* 75% */
-#define TPC_SR_NO_INCREASE		10880	/* 85% */
-#define TPC_TX_POWER_STEP		3
 #define TPC_MAX_REDUCTION		15
 #define TPC_NO_REDUCTION		0
 #define TPC_INVALID			0xff
 
-#define LINK_QUAL_AGG_TIME_LIMIT_DEF	(4000) /* 4 milliseconds */
-#define LINK_QUAL_AGG_TIME_LIMIT_MAX	(8000)
-#define LINK_QUAL_AGG_TIME_LIMIT_MIN	(100)
-
-#define LINK_QUAL_AGG_DISABLE_START_DEF	(3)
-#define LINK_QUAL_AGG_DISABLE_START_MAX	(255)
-#define LINK_QUAL_AGG_DISABLE_START_MIN	(0)
-
 #define LINK_QUAL_AGG_FRAME_LIMIT_DEF	(63)
 #define LINK_QUAL_AGG_FRAME_LIMIT_MAX	(63)
 #define LINK_QUAL_AGG_FRAME_LIMIT_MIN	(0)
@@ -181,14 +149,7 @@ enum {
 
 /* load per tid defines for A-MPDU activation */
 #define IWL_AGG_TPT_THREHOLD	0
-#define IWL_AGG_LOAD_THRESHOLD	10
 #define IWL_AGG_ALL_TID		0xff
-#define TID_QUEUE_CELL_SPACING	50	/*mS */
-#define TID_QUEUE_MAX_SIZE	20
-#define TID_ROUND_VALUE		5	/* mS */
-
-#define TID_MAX_TIME_DIFF ((TID_QUEUE_MAX_SIZE - 1) * TID_QUEUE_CELL_SPACING)
-#define TIME_WRAP_AROUND(x, y) (((y) > (x)) ? (y) - (x) : (0-(x)) + (y))
 
 enum iwl_table_type {
 	LQ_NONE,
diff --git a/drivers/net/wireless/iwlwifi/mvm/scan.c b/drivers/net/wireless/iwlwifi/mvm/scan.c
index e5294d01181e..348b9c4b694a 100644
--- a/drivers/net/wireless/iwlwifi/mvm/scan.c
+++ b/drivers/net/wireless/iwlwifi/mvm/scan.c
@@ -72,6 +72,8 @@
 
 #define IWL_PLCP_QUIET_THRESH 1
 #define IWL_ACTIVE_QUIET_TIME 10
+#define IWL_DENSE_EBS_SCAN_RATIO 5
+#define IWL_SPARSE_EBS_SCAN_RATIO 1
 
 struct iwl_mvm_scan_params {
 	u32 max_out_time;
@@ -97,7 +99,7 @@ static u8 iwl_mvm_scan_rx_ant(struct iwl_mvm *mvm)
 {
 	if (mvm->scan_rx_ant != ANT_NONE)
 		return mvm->scan_rx_ant;
-	return mvm->fw->valid_rx_ant;
+	return iwl_mvm_get_valid_rx_ant(mvm);
 }
 
 static inline __le16 iwl_mvm_scan_rx_chain(struct iwl_mvm *mvm)
@@ -128,7 +130,7 @@ iwl_mvm_scan_rate_n_flags(struct iwl_mvm *mvm, enum ieee80211_band band,
 	u32 tx_ant;
 
 	mvm->scan_last_antenna_idx =
-		iwl_mvm_next_antenna(mvm, mvm->fw->valid_tx_ant,
+		iwl_mvm_next_antenna(mvm, iwl_mvm_get_valid_tx_ant(mvm),
 				     mvm->scan_last_antenna_idx);
 	tx_ant = BIT(mvm->scan_last_antenna_idx) << RATE_MCS_ANT_POS;
 
@@ -282,11 +284,11 @@ static void iwl_mvm_scan_condition_iterator(void *data, u8 *mac,
 					    struct ieee80211_vif *vif)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
-	bool *global_bound = data;
+	int *global_cnt = data;
 
 	if (vif->type != NL80211_IFTYPE_P2P_DEVICE && mvmvif->phy_ctxt &&
 	    mvmvif->phy_ctxt->id < MAX_PHYS)
-		*global_bound = true;
+		*global_cnt += 1;
 }
 
 static void iwl_mvm_scan_calc_params(struct iwl_mvm *mvm,
@@ -294,16 +296,16 @@ static void iwl_mvm_scan_calc_params(struct iwl_mvm *mvm,
 				     int n_ssids, u32 flags,
 				     struct iwl_mvm_scan_params *params)
 {
-	bool global_bound = false;
+	int global_cnt = 0;
 	enum ieee80211_band band;
 	u8 frag_passive_dwell = 0;
 
 	ieee80211_iterate_active_interfaces_atomic(mvm->hw,
 					    IEEE80211_IFACE_ITER_NORMAL,
 					    iwl_mvm_scan_condition_iterator,
-					    &global_bound);
+					    &global_cnt);
 
-	if (!global_bound)
+	if (!global_cnt)
 		goto not_bound;
 
 	params->suspend_time = 30;
@@ -314,7 +316,11 @@ static void iwl_mvm_scan_calc_params(struct iwl_mvm *mvm,
 		    IWL_UCODE_TLV_API_FRAGMENTED_SCAN) {
 			params->suspend_time = 105;
 			params->max_out_time = 70;
-			frag_passive_dwell = 20;
+			/*
+			 * If there is more than one active interface make
+			 * passive scan more fragmented.
+			 */
+			frag_passive_dwell = (global_cnt < 2) ? 40 : 20;
 		} else {
 			params->suspend_time = 120;
 			params->max_out_time = 120;
@@ -1296,10 +1302,14 @@ iwl_mvm_build_generic_unified_scan_cmd(struct iwl_mvm *mvm,
 			cpu_to_le16(IWL_SCAN_CHANNEL_FLAG_EBS |
 				    IWL_SCAN_CHANNEL_FLAG_EBS_ACCURATE |
 				    IWL_SCAN_CHANNEL_FLAG_CACHE_ADD);
+		cmd->channel_opt[0].non_ebs_ratio =
+			cpu_to_le16(IWL_DENSE_EBS_SCAN_RATIO);
 		cmd->channel_opt[1].flags =
 			cpu_to_le16(IWL_SCAN_CHANNEL_FLAG_EBS |
 				    IWL_SCAN_CHANNEL_FLAG_EBS_ACCURATE |
 				    IWL_SCAN_CHANNEL_FLAG_CACHE_ADD);
+		cmd->channel_opt[1].non_ebs_ratio =
+			cpu_to_le16(IWL_SPARSE_EBS_SCAN_RATIO);
 	}
 
 	if (iwl_mvm_rrm_scan_needed(mvm))
@@ -1603,7 +1613,7 @@ int iwl_mvm_config_scan(struct iwl_mvm *mvm)
 					 SCAN_CONFIG_FLAG_SET_MAC_ADDR |
 					 SCAN_CONFIG_FLAG_SET_CHANNEL_FLAGS|
 					 SCAN_CONFIG_N_CHANNELS(num_channels));
-	scan_config->tx_chains = cpu_to_le32(mvm->fw->valid_tx_ant);
+	scan_config->tx_chains = cpu_to_le32(iwl_mvm_get_valid_tx_ant(mvm));
 	scan_config->rx_chains = cpu_to_le32(iwl_mvm_scan_rx_ant(mvm));
 	scan_config->legacy_rates = iwl_mvm_scan_config_rates(mvm);
 	scan_config->out_of_channel_time = cpu_to_le32(170);
diff --git a/drivers/net/wireless/iwlwifi/mvm/sta.c b/drivers/net/wireless/iwlwifi/mvm/sta.c
index d86fe432e51f..ad327984b099 100644
--- a/drivers/net/wireless/iwlwifi/mvm/sta.c
+++ b/drivers/net/wireless/iwlwifi/mvm/sta.c
@@ -99,7 +99,7 @@ static int iwl_mvm_find_free_sta_id(struct iwl_mvm *mvm,
 int iwl_mvm_sta_send_to_fw(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 			   bool update)
 {
-	struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
+	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 	struct iwl_mvm_add_sta_cmd add_sta_cmd = {
 		.sta_id = mvm_sta->sta_id,
 		.mac_id_n_color = cpu_to_le32(mvm_sta->mac_id_n_color),
@@ -259,7 +259,7 @@ int iwl_mvm_add_sta(struct iwl_mvm *mvm,
 		    struct ieee80211_sta *sta)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
-	struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
+	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 	int i, ret, sta_id;
 
 	lockdep_assert_held(&mvm->mutex);
@@ -481,7 +481,7 @@ int iwl_mvm_rm_sta(struct iwl_mvm *mvm,
 		   struct ieee80211_sta *sta)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
-	struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
+	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 	int ret;
 
 	lockdep_assert_held(&mvm->mutex);
@@ -774,7 +774,7 @@ int iwl_mvm_rm_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 int iwl_mvm_sta_rx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 		       int tid, u16 ssn, bool start)
 {
-	struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
+	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 	struct iwl_mvm_add_sta_cmd cmd = {};
 	int ret;
 	u32 status;
@@ -834,7 +834,7 @@ int iwl_mvm_sta_rx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 static int iwl_mvm_sta_tx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 			      int tid, u8 queue, bool start)
 {
-	struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
+	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 	struct iwl_mvm_add_sta_cmd cmd = {};
 	int ret;
 	u32 status;
@@ -1144,10 +1144,10 @@ static int iwl_mvm_set_fw_key_idx(struct iwl_mvm *mvm)
 static u8 iwl_mvm_get_key_sta_id(struct ieee80211_vif *vif,
 				 struct ieee80211_sta *sta)
 {
-	struct iwl_mvm_vif *mvmvif = (void *)vif->drv_priv;
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 
 	if (sta) {
-		struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
+		struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 
 		return mvm_sta->sta_id;
 	}
@@ -1280,7 +1280,7 @@ static inline u8 *iwl_mvm_get_mac_addr(struct iwl_mvm *mvm,
 				       struct ieee80211_vif *vif,
 				       struct ieee80211_sta *sta)
 {
-	struct iwl_mvm_vif *mvmvif = (void *)vif->drv_priv;
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 
 	if (sta)
 		return sta->addr;
diff --git a/drivers/net/wireless/iwlwifi/mvm/tt.c b/drivers/net/wireless/iwlwifi/mvm/tt.c
index 2b1e61fac34a..ba615ad2176c 100644
--- a/drivers/net/wireless/iwlwifi/mvm/tt.c
+++ b/drivers/net/wireless/iwlwifi/mvm/tt.c
@@ -69,6 +69,7 @@
 
 static void iwl_mvm_enter_ctkill(struct iwl_mvm *mvm)
 {
+	struct iwl_mvm_tt_mgmt *tt = &mvm->thermal_throttle;
 	u32 duration = mvm->thermal_throttle.params->ct_kill_duration;
 
 	if (test_bit(IWL_MVM_STATUS_HW_CTKILL, &mvm->status))
@@ -77,12 +78,15 @@ static void iwl_mvm_enter_ctkill(struct iwl_mvm *mvm)
 	IWL_ERR(mvm, "Enter CT Kill\n");
 	iwl_mvm_set_hw_ctkill_state(mvm, true);
 
+	tt->throttle = false;
+	tt->dynamic_smps = false;
+
 	/* Don't schedule an exit work if we're in test mode, since
 	 * the temperature will not change unless we manually set it
 	 * again (or disable testing).
 	 */
 	if (!mvm->temperature_test)
-		schedule_delayed_work(&mvm->thermal_throttle.ct_kill_exit,
+		schedule_delayed_work(&tt->ct_kill_exit,
 				      round_jiffies_relative(duration * HZ));
 }
 
@@ -452,6 +456,7 @@ void iwl_mvm_tt_initialize(struct iwl_mvm *mvm, u32 min_backoff)
 		tt->params = &iwl7000_tt_params;
 
 	tt->throttle = false;
+	tt->dynamic_smps = false;
 	tt->min_backoff = min_backoff;
 	INIT_DELAYED_WORK(&tt->ct_kill_exit, check_exit_ctkill);
 }
diff --git a/drivers/net/wireless/iwlwifi/mvm/tx.c b/drivers/net/wireless/iwlwifi/mvm/tx.c
index 4f15d9decc81..d6cdb770881a 100644
--- a/drivers/net/wireless/iwlwifi/mvm/tx.c
+++ b/drivers/net/wireless/iwlwifi/mvm/tx.c
@@ -209,7 +209,7 @@ void iwl_mvm_set_tx_cmd_rate(struct iwl_mvm *mvm, struct iwl_tx_cmd *tx_cmd,
 	rate_plcp = iwl_mvm_mac80211_idx_to_hwrate(rate_idx);
 
 	mvm->mgmt_last_antenna_idx =
-		iwl_mvm_next_antenna(mvm, mvm->fw->valid_tx_ant,
+		iwl_mvm_next_antenna(mvm, iwl_mvm_get_valid_tx_ant(mvm),
 				     mvm->mgmt_last_antenna_idx);
 
 	if (info->band == IEEE80211_BAND_2GHZ &&
diff --git a/drivers/net/wireless/iwlwifi/mvm/utils.c b/drivers/net/wireless/iwlwifi/mvm/utils.c
index e56e77ef5d2e..f0a114102c3f 100644
--- a/drivers/net/wireless/iwlwifi/mvm/utils.c
+++ b/drivers/net/wireless/iwlwifi/mvm/utils.c
@@ -620,7 +620,7 @@ void iwl_mvm_update_smps(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	lockdep_assert_held(&mvm->mutex);
 
 	/* SMPS is irrelevant for NICs that don't have at least 2 RX antenna */
-	if (num_of_ant(mvm->fw->valid_rx_ant) == 1)
+	if (num_of_ant(iwl_mvm_get_valid_rx_ant(mvm)) == 1)
 		return;
 
 	if (vif->type == NL80211_IFTYPE_AP)
@@ -662,7 +662,7 @@ bool iwl_mvm_rx_diversity_allowed(struct iwl_mvm *mvm)
 
 	lockdep_assert_held(&mvm->mutex);
 
-	if (num_of_ant(mvm->fw->valid_rx_ant) == 1)
+	if (num_of_ant(iwl_mvm_get_valid_rx_ant(mvm)) == 1)
 		return false;
 
 	if (!mvm->cfg->rx_with_siso_diversity)
diff --git a/drivers/net/wireless/iwlwifi/pcie/drv.c b/drivers/net/wireless/iwlwifi/pcie/drv.c
index 3ee8e3848876..2f0c4b170344 100644
--- a/drivers/net/wireless/iwlwifi/pcie/drv.c
+++ b/drivers/net/wireless/iwlwifi/pcie/drv.c
@@ -367,7 +367,11 @@ static const struct pci_device_id iwl_hw_card_ids[] = {
 
 /* 3165 Series */
 	{IWL_PCI_DEVICE(0x3165, 0x4010, iwl3165_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x3165, 0x4012, iwl3165_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x3165, 0x4110, iwl3165_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x3165, 0x4210, iwl3165_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x3165, 0x4410, iwl3165_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x3165, 0x4510, iwl3165_2ac_cfg)},
 
 /* 7265 Series */
 	{IWL_PCI_DEVICE(0x095A, 0x5010, iwl7265_2ac_cfg)},
diff --git a/drivers/net/wireless/iwlwifi/pcie/internal.h b/drivers/net/wireless/iwlwifi/pcie/internal.h
index 1aea6b66c594..e5652d82d79e 100644
--- a/drivers/net/wireless/iwlwifi/pcie/internal.h
+++ b/drivers/net/wireless/iwlwifi/pcie/internal.h
@@ -318,6 +318,11 @@ struct iwl_trans_pcie {
 	/*protect hw register */
 	spinlock_t reg_lock;
 	bool cmd_in_flight;
+	bool ref_cmd_in_flight;
+
+	/* protect ref counter */
+	spinlock_t ref_lock;
+	u32 ref_count;
 
 	dma_addr_t fw_mon_phys;
 	struct page *fw_mon_page;
@@ -381,6 +386,9 @@ void iwl_trans_pcie_reclaim(struct iwl_trans *trans, int txq_id, int ssn,
 			    struct sk_buff_head *skbs);
 void iwl_trans_pcie_tx_reset(struct iwl_trans *trans);
 
+void iwl_trans_pcie_ref(struct iwl_trans *trans);
+void iwl_trans_pcie_unref(struct iwl_trans *trans);
+
 static inline u16 iwl_pcie_tfd_tb_get_len(struct iwl_tfd *tfd, u8 idx)
 {
 	struct iwl_tfd_tb *tb = &tfd->tbs[idx];
diff --git a/drivers/net/wireless/iwlwifi/pcie/trans.c b/drivers/net/wireless/iwlwifi/pcie/trans.c
index 5d79a1f44b8e..9ee4ca0ba8d3 100644
--- a/drivers/net/wireless/iwlwifi/pcie/trans.c
+++ b/drivers/net/wireless/iwlwifi/pcie/trans.c
@@ -443,10 +443,25 @@ static int iwl_pcie_apm_stop_master(struct iwl_trans *trans)
 	return ret;
 }
 
-static void iwl_pcie_apm_stop(struct iwl_trans *trans)
+static void iwl_pcie_apm_stop(struct iwl_trans *trans, bool op_mode_leave)
 {
 	IWL_DEBUG_INFO(trans, "Stop card, put in low power state\n");
 
+	if (op_mode_leave) {
+		if (!test_bit(STATUS_DEVICE_ENABLED, &trans->status))
+			iwl_pcie_apm_init(trans);
+
+		/* inform ME that we are leaving */
+		if (trans->cfg->device_family == IWL_DEVICE_FAMILY_7000)
+			iwl_set_bits_prph(trans, APMG_PCIDEV_STT_REG,
+					  APMG_PCIDEV_STT_VAL_WAKE_ME);
+		else if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000)
+			iwl_set_bit(trans, CSR_HW_IF_CONFIG_REG,
+				    CSR_HW_IF_CONFIG_REG_PREPARE |
+				    CSR_HW_IF_CONFIG_REG_ENABLE_PME);
+		mdelay(5);
+	}
+
 	clear_bit(STATUS_DEVICE_ENABLED, &trans->status);
 
 	/* Stop device's DMA activity */
@@ -614,7 +629,7 @@ static int iwl_pcie_load_section(struct iwl_trans *trans, u8 section_num,
 {
 	u8 *v_addr;
 	dma_addr_t p_addr;
-	u32 offset, chunk_sz = section->len;
+	u32 offset, chunk_sz = min_t(u32, FH_MEM_TB_MAX_LENGTH, section->len);
 	int ret = 0;
 
 	IWL_DEBUG_FW(trans, "[%d] uCode section being loaded...\n",
@@ -893,6 +908,9 @@ static int iwl_pcie_load_given_ucode_8000b(struct iwl_trans *trans,
 	if (ret)
 		return ret;
 
+	if (trans->dbg_dest_tlv)
+		iwl_pcie_apply_destination(trans);
+
 	/* Notify FW loading is done */
 	iwl_write_direct32(trans, FH_UCODE_LOAD_STATUS, 0xFFFFFFFF);
 
@@ -916,6 +934,7 @@ static int iwl_pcie_load_given_ucode_8000b(struct iwl_trans *trans,
 static int iwl_trans_pcie_start_fw(struct iwl_trans *trans,
 				   const struct fw_img *fw, bool run_in_rfkill)
 {
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	int ret;
 	bool hw_rfkill;
 
@@ -945,6 +964,9 @@ static int iwl_trans_pcie_start_fw(struct iwl_trans *trans,
 		return ret;
 	}
 
+	/* init ref_count to 1 (should be cleared when ucode is loaded) */
+	trans_pcie->ref_count = 1;
+
 	/* make sure rfkill handshake bits are cleared */
 	iwl_write32(trans, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
 	iwl_write32(trans, CSR_UCODE_DRV_GP1_CLR,
@@ -1010,18 +1032,23 @@ static void iwl_trans_pcie_stop_device(struct iwl_trans *trans)
 		      CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
 
 	/* Stop the device, and put it in low power state */
-	iwl_pcie_apm_stop(trans);
+	iwl_pcie_apm_stop(trans, false);
 
-	/* Upon stop, the APM issues an interrupt if HW RF kill is set.
-	 * Clean again the interrupt here
+	/* stop and reset the on-board processor */
+	iwl_write32(trans, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
+	udelay(20);
+
+	/*
+	 * Upon stop, the APM issues an interrupt if HW RF kill is set.
+	 * This is a bug in certain verions of the hardware.
+	 * Certain devices also keep sending HW RF kill interrupt all
+	 * the time, unless the interrupt is ACKed even if the interrupt
+	 * should be masked. Re-ACK all the interrupts here.
 	 */
 	spin_lock(&trans_pcie->irq_lock);
 	iwl_disable_interrupts(trans);
 	spin_unlock(&trans_pcie->irq_lock);
 
-	/* stop and reset the on-board processor */
-	iwl_write32(trans, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
-	udelay(20);
 
 	/* clear all status bits */
 	clear_bit(STATUS_SYNC_HCMD_ACTIVE, &trans->status);
@@ -1187,7 +1214,7 @@ static void iwl_trans_pcie_op_mode_leave(struct iwl_trans *trans)
 	iwl_disable_interrupts(trans);
 	spin_unlock(&trans_pcie->irq_lock);
 
-	iwl_pcie_apm_stop(trans);
+	iwl_pcie_apm_stop(trans, true);
 
 	spin_lock(&trans_pcie->irq_lock);
 	iwl_disable_interrupts(trans);
@@ -1535,6 +1562,38 @@ static void iwl_trans_pcie_set_bits_mask(struct iwl_trans *trans, u32 reg,
 	spin_unlock_irqrestore(&trans_pcie->reg_lock, flags);
 }
 
+void iwl_trans_pcie_ref(struct iwl_trans *trans)
+{
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	unsigned long flags;
+
+	if (iwlwifi_mod_params.d0i3_disable)
+		return;
+
+	spin_lock_irqsave(&trans_pcie->ref_lock, flags);
+	IWL_DEBUG_RPM(trans, "ref_counter: %d\n", trans_pcie->ref_count);
+	trans_pcie->ref_count++;
+	spin_unlock_irqrestore(&trans_pcie->ref_lock, flags);
+}
+
+void iwl_trans_pcie_unref(struct iwl_trans *trans)
+{
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	unsigned long flags;
+
+	if (iwlwifi_mod_params.d0i3_disable)
+		return;
+
+	spin_lock_irqsave(&trans_pcie->ref_lock, flags);
+	IWL_DEBUG_RPM(trans, "ref_counter: %d\n", trans_pcie->ref_count);
+	if (WARN_ON_ONCE(trans_pcie->ref_count == 0)) {
+		spin_unlock_irqrestore(&trans_pcie->ref_lock, flags);
+		return;
+	}
+	trans_pcie->ref_count--;
+	spin_unlock_irqrestore(&trans_pcie->ref_lock, flags);
+}
+
 static const char *get_csr_string(int cmd)
 {
 #define IWL_CMD(x) case x: return #x
@@ -2259,6 +2318,9 @@ static const struct iwl_trans_ops trans_ops_pcie = {
 	.release_nic_access = iwl_trans_pcie_release_nic_access,
 	.set_bits_mask = iwl_trans_pcie_set_bits_mask,
 
+	.ref = iwl_trans_pcie_ref,
+	.unref = iwl_trans_pcie_unref,
+
 	.dump_data = iwl_trans_pcie_dump_data,
 };
 
@@ -2399,6 +2461,7 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 	}
 
 	trans_pcie->inta_mask = CSR_INI_SET_MASK;
+	trans->d0i3_mode = IWL_D0I3_MODE_ON_SUSPEND;
 
 	return trans;
 
diff --git a/drivers/net/wireless/iwlwifi/pcie/tx.c b/drivers/net/wireless/iwlwifi/pcie/tx.c
index 8a6c7a084aa1..c1c4c75026b2 100644
--- a/drivers/net/wireless/iwlwifi/pcie/tx.c
+++ b/drivers/net/wireless/iwlwifi/pcie/tx.c
@@ -985,17 +985,31 @@ void iwl_trans_pcie_reclaim(struct iwl_trans *trans, int txq_id, int ssn,
 
 	if (iwl_queue_space(&txq->q) > txq->q.low_mark)
 		iwl_wake_queue(trans, txq);
+
+	if (q->read_ptr == q->write_ptr) {
+		IWL_DEBUG_RPM(trans, "Q %d - last tx reclaimed\n", q->id);
+		iwl_trans_pcie_unref(trans);
+	}
+
 out:
 	spin_unlock_bh(&txq->lock);
 }
 
-static int iwl_pcie_set_cmd_in_flight(struct iwl_trans *trans)
+static int iwl_pcie_set_cmd_in_flight(struct iwl_trans *trans,
+				      const struct iwl_host_cmd *cmd)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	int ret;
 
 	lockdep_assert_held(&trans_pcie->reg_lock);
 
+	if (!(cmd->flags & CMD_SEND_IN_IDLE) &&
+	    !trans_pcie->ref_cmd_in_flight) {
+		trans_pcie->ref_cmd_in_flight = true;
+		IWL_DEBUG_RPM(trans, "set ref_cmd_in_flight - ref\n");
+		iwl_trans_pcie_ref(trans);
+	}
+
 	if (trans_pcie->cmd_in_flight)
 		return 0;
 
@@ -1036,6 +1050,12 @@ static int iwl_pcie_clear_cmd_in_flight(struct iwl_trans *trans)
 
 	lockdep_assert_held(&trans_pcie->reg_lock);
 
+	if (trans_pcie->ref_cmd_in_flight) {
+		trans_pcie->ref_cmd_in_flight = false;
+		IWL_DEBUG_RPM(trans, "clear ref_cmd_in_flight - unref\n");
+		iwl_trans_pcie_unref(trans);
+	}
+
 	if (WARN_ON(!trans_pcie->cmd_in_flight))
 		return 0;
 
@@ -1473,7 +1493,7 @@ static int iwl_pcie_enqueue_hcmd(struct iwl_trans *trans,
 		mod_timer(&txq->stuck_timer, jiffies + trans_pcie->wd_timeout);
 
 	spin_lock_irqsave(&trans_pcie->reg_lock, flags);
-	ret = iwl_pcie_set_cmd_in_flight(trans);
+	ret = iwl_pcie_set_cmd_in_flight(trans, cmd);
 	if (ret < 0) {
 		idx = ret;
 		spin_unlock_irqrestore(&trans_pcie->reg_lock, flags);
@@ -1819,9 +1839,13 @@ int iwl_trans_pcie_tx(struct iwl_trans *trans, struct sk_buff *skb,
 	wait_write_ptr = ieee80211_has_morefrags(fc);
 
 	/* start timer if queue currently empty */
-	if (txq->need_update && q->read_ptr == q->write_ptr &&
-	    trans_pcie->wd_timeout)
-		mod_timer(&txq->stuck_timer, jiffies + trans_pcie->wd_timeout);
+	if (q->read_ptr == q->write_ptr) {
+		if (txq->need_update && trans_pcie->wd_timeout)
+			mod_timer(&txq->stuck_timer,
+				  jiffies + trans_pcie->wd_timeout);
+		IWL_DEBUG_RPM(trans, "Q: %d first tx - take ref\n", q->id);
+		iwl_trans_pcie_ref(trans);
+	}
 
 	/* Tell device the write index *just past* this latest filled TFD */
 	q->write_ptr = iwl_queue_inc_wrap(q->write_ptr);
diff --git a/drivers/net/wireless/mac80211_hwsim.c b/drivers/net/wireless/mac80211_hwsim.c
index 74f4e1c18322..494e7335aa64 100644
--- a/drivers/net/wireless/mac80211_hwsim.c
+++ b/drivers/net/wireless/mac80211_hwsim.c
@@ -395,7 +395,6 @@ static int hwsim_radio_idx;
 static struct platform_driver mac80211_hwsim_driver = {
 	.driver = {
 		.name = "mac80211_hwsim",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/net/wireless/mwifiex/11n.h b/drivers/net/wireless/mwifiex/11n.h
index f275675cdbd3..8e2e39422ad8 100644
--- a/drivers/net/wireless/mwifiex/11n.h
+++ b/drivers/net/wireless/mwifiex/11n.h
@@ -130,7 +130,9 @@ static inline u8 mwifiex_space_avail_for_new_ba_stream(
 {
 	struct mwifiex_private *priv;
 	u8 i;
-	u32 ba_stream_num = 0;
+	u32 ba_stream_num = 0, ba_stream_max;
+
+	ba_stream_max = MWIFIEX_MAX_TX_BASTREAM_SUPPORTED;
 
 	for (i = 0; i < adapter->priv_num; i++) {
 		priv = adapter->priv[i];
@@ -139,8 +141,14 @@ static inline u8 mwifiex_space_avail_for_new_ba_stream(
 				&priv->tx_ba_stream_tbl_ptr);
 	}
 
-	return ((ba_stream_num <
-		 MWIFIEX_MAX_TX_BASTREAM_SUPPORTED) ? true : false);
+	if (adapter->fw_api_ver == MWIFIEX_FW_V15) {
+		ba_stream_max =
+			       GETSUPP_TXBASTREAMS(adapter->hw_dot_11n_dev_cap);
+		if (!ba_stream_max)
+			ba_stream_max = MWIFIEX_MAX_TX_BASTREAM_SUPPORTED;
+	}
+
+	return ((ba_stream_num < ba_stream_max) ? true : false);
 }
 
 /*
diff --git a/drivers/net/wireless/mwifiex/11n_aggr.c b/drivers/net/wireless/mwifiex/11n_aggr.c
index 8720a3d3c755..9b983b5cebbd 100644
--- a/drivers/net/wireless/mwifiex/11n_aggr.c
+++ b/drivers/net/wireless/mwifiex/11n_aggr.c
@@ -101,6 +101,13 @@ mwifiex_11n_form_amsdu_txpd(struct mwifiex_private *priv,
 {
 	struct txpd *local_tx_pd;
 	struct mwifiex_txinfo *tx_info = MWIFIEX_SKB_TXCB(skb);
+	unsigned int pad;
+	int headroom = (priv->adapter->iface_type ==
+			MWIFIEX_USB) ? 0 : INTF_HEADER_LEN;
+
+	pad = ((void *)skb->data - sizeof(*local_tx_pd) -
+		headroom - NULL) & (MWIFIEX_DMA_ALIGN_SZ - 1);
+	skb_push(skb, pad);
 
 	skb_push(skb, sizeof(*local_tx_pd));
 
@@ -114,10 +121,12 @@ mwifiex_11n_form_amsdu_txpd(struct mwifiex_private *priv,
 	local_tx_pd->bss_num = priv->bss_num;
 	local_tx_pd->bss_type = priv->bss_type;
 	/* Always zero as the data is followed by struct txpd */
-	local_tx_pd->tx_pkt_offset = cpu_to_le16(sizeof(struct txpd));
+	local_tx_pd->tx_pkt_offset = cpu_to_le16(sizeof(struct txpd) +
+						 pad);
 	local_tx_pd->tx_pkt_type = cpu_to_le16(PKT_TYPE_AMSDU);
 	local_tx_pd->tx_pkt_length = cpu_to_le16(skb->len -
-						 sizeof(*local_tx_pd));
+						 sizeof(*local_tx_pd) -
+						 pad);
 
 	if (tx_info->flags & MWIFIEX_BUF_FLAG_TDLS_PKT)
 		local_tx_pd->flags |= MWIFIEX_TXPD_FLAGS_TDLS_PACKET;
@@ -182,7 +191,7 @@ mwifiex_11n_aggregate_pkt(struct mwifiex_private *priv,
 				       ra_list_flags);
 		return -1;
 	}
-	skb_reserve(skb_aggr, headroom + sizeof(struct txpd));
+	skb_reserve(skb_aggr, MWIFIEX_MIN_DATA_HEADER_LEN);
 	tx_info_aggr =  MWIFIEX_SKB_TXCB(skb_aggr);
 
 	memset(tx_info_aggr, 0, sizeof(*tx_info_aggr));
diff --git a/drivers/net/wireless/mwifiex/11n_rxreorder.c b/drivers/net/wireless/mwifiex/11n_rxreorder.c
index d73fda312c87..f33dc81c5228 100644
--- a/drivers/net/wireless/mwifiex/11n_rxreorder.c
+++ b/drivers/net/wireless/mwifiex/11n_rxreorder.c
@@ -391,10 +391,8 @@ mwifiex_11n_create_rx_reorder_tbl(struct mwifiex_private *priv, u8 *ta,
 	new_node->timer_context.priv = priv;
 	new_node->timer_context.timer_is_set = false;
 
-	init_timer(&new_node->timer_context.timer);
-	new_node->timer_context.timer.function = mwifiex_flush_data;
-	new_node->timer_context.timer.data =
-			(unsigned long) &new_node->timer_context;
+	setup_timer(&new_node->timer_context.timer, mwifiex_flush_data,
+		    (unsigned long)&new_node->timer_context);
 
 	for (i = 0; i < win_size; ++i)
 		new_node->rx_reorder_ptr[i] = NULL;
diff --git a/drivers/net/wireless/mwifiex/Makefile b/drivers/net/wireless/mwifiex/Makefile
index 9487d728ac20..fdfd9bf15ed4 100644
--- a/drivers/net/wireless/mwifiex/Makefile
+++ b/drivers/net/wireless/mwifiex/Makefile
@@ -53,3 +53,5 @@ obj-$(CONFIG_MWIFIEX_PCIE) += mwifiex_pcie.o
 
 mwifiex_usb-y += usb.o
 obj-$(CONFIG_MWIFIEX_USB) += mwifiex_usb.o
+
+ccflags-y += -D__CHECK_ENDIAN
diff --git a/drivers/net/wireless/mwifiex/cfg80211.c b/drivers/net/wireless/mwifiex/cfg80211.c
index 1996a8b612d2..b3bf2cd150b7 100644
--- a/drivers/net/wireless/mwifiex/cfg80211.c
+++ b/drivers/net/wireless/mwifiex/cfg80211.c
@@ -2388,6 +2388,10 @@ int mwifiex_del_virtual_intf(struct wiphy *wiphy, struct wireless_dev *wdev)
 
 	priv->bss_mode = NL80211_IFTYPE_UNSPECIFIED;
 
+	if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA ||
+	    GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP)
+		kfree(priv->hist_data);
+
 	return 0;
 }
 EXPORT_SYMBOL_GPL(mwifiex_del_virtual_intf);
@@ -2423,30 +2427,16 @@ mwifiex_is_pattern_supported(struct cfg80211_pkt_pattern *pat, s8 *byte_seq,
 }
 
 #ifdef CONFIG_PM
-static int mwifiex_cfg80211_suspend(struct wiphy *wiphy,
-				    struct cfg80211_wowlan *wowlan)
+static int mwifiex_set_mef_filter(struct mwifiex_private *priv,
+				  struct cfg80211_wowlan *wowlan)
 {
-	struct mwifiex_adapter *adapter = mwifiex_cfg80211_get_adapter(wiphy);
-	struct mwifiex_ds_mef_cfg mef_cfg;
-	struct mwifiex_mef_entry *mef_entry;
-	int i, filt_num = 0, ret;
+	int i, filt_num = 0, ret = 0;
 	bool first_pat = true;
 	u8 byte_seq[MWIFIEX_MEF_MAX_BYTESEQ + 1];
 	const u8 ipv4_mc_mac[] = {0x33, 0x33};
 	const u8 ipv6_mc_mac[] = {0x01, 0x00, 0x5e};
-	struct mwifiex_private *priv =
-			mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_STA);
-
-	if (!wowlan) {
-		dev_warn(adapter->dev, "None of the WOWLAN triggers enabled\n");
-		return 0;
-	}
-
-	if (!priv->media_connected) {
-		dev_warn(adapter->dev,
-			 "Can not configure WOWLAN in disconnected state\n");
-		return 0;
-	}
+	struct mwifiex_ds_mef_cfg mef_cfg;
+	struct mwifiex_mef_entry *mef_entry;
 
 	mef_entry = kzalloc(sizeof(*mef_entry), GFP_KERNEL);
 	if (!mef_entry)
@@ -2461,9 +2451,9 @@ static int mwifiex_cfg80211_suspend(struct wiphy *wiphy,
 	for (i = 0; i < wowlan->n_patterns; i++) {
 		memset(byte_seq, 0, sizeof(byte_seq));
 		if (!mwifiex_is_pattern_supported(&wowlan->patterns[i],
-						  byte_seq,
-						  MWIFIEX_MEF_MAX_BYTESEQ)) {
-			wiphy_err(wiphy, "Pattern not supported\n");
+					byte_seq,
+					MWIFIEX_MEF_MAX_BYTESEQ)) {
+			dev_err(priv->adapter->dev, "Pattern not supported\n");
 			kfree(mef_entry);
 			return -EOPNOTSUPP;
 		}
@@ -2487,9 +2477,9 @@ static int mwifiex_cfg80211_suspend(struct wiphy *wiphy,
 
 		mef_entry->filter[filt_num].repeat = 1;
 		mef_entry->filter[filt_num].offset =
-						wowlan->patterns[i].pkt_offset;
+			wowlan->patterns[i].pkt_offset;
 		memcpy(mef_entry->filter[filt_num].byte_seq, byte_seq,
-		       sizeof(byte_seq));
+				sizeof(byte_seq));
 		mef_entry->filter[filt_num].filt_type = TYPE_EQ;
 
 		if (first_pat)
@@ -2504,9 +2494,9 @@ static int mwifiex_cfg80211_suspend(struct wiphy *wiphy,
 		mef_cfg.criteria |= MWIFIEX_CRITERIA_UNICAST;
 		mef_entry->filter[filt_num].repeat = 16;
 		memcpy(mef_entry->filter[filt_num].byte_seq, priv->curr_addr,
-		       ETH_ALEN);
+				ETH_ALEN);
 		mef_entry->filter[filt_num].byte_seq[MWIFIEX_MEF_MAX_BYTESEQ] =
-								ETH_ALEN;
+			ETH_ALEN;
 		mef_entry->filter[filt_num].offset = 28;
 		mef_entry->filter[filt_num].filt_type = TYPE_EQ;
 		if (filt_num)
@@ -2515,9 +2505,9 @@ static int mwifiex_cfg80211_suspend(struct wiphy *wiphy,
 		filt_num++;
 		mef_entry->filter[filt_num].repeat = 16;
 		memcpy(mef_entry->filter[filt_num].byte_seq, priv->curr_addr,
-		       ETH_ALEN);
+				ETH_ALEN);
 		mef_entry->filter[filt_num].byte_seq[MWIFIEX_MEF_MAX_BYTESEQ] =
-								ETH_ALEN;
+			ETH_ALEN;
 		mef_entry->filter[filt_num].offset = 56;
 		mef_entry->filter[filt_num].filt_type = TYPE_EQ;
 		mef_entry->filter[filt_num].filt_action = TYPE_OR;
@@ -2525,16 +2515,61 @@ static int mwifiex_cfg80211_suspend(struct wiphy *wiphy,
 
 	if (!mef_cfg.criteria)
 		mef_cfg.criteria = MWIFIEX_CRITERIA_BROADCAST |
-				   MWIFIEX_CRITERIA_UNICAST |
-				   MWIFIEX_CRITERIA_MULTICAST;
+			MWIFIEX_CRITERIA_UNICAST |
+			MWIFIEX_CRITERIA_MULTICAST;
 
 	ret = mwifiex_send_cmd(priv, HostCmd_CMD_MEF_CFG,
-			       HostCmd_ACT_GEN_SET, 0, &mef_cfg, true);
+			HostCmd_ACT_GEN_SET, 0, &mef_cfg, true);
 
 	kfree(mef_entry);
 	return ret;
 }
 
+static int mwifiex_cfg80211_suspend(struct wiphy *wiphy,
+				    struct cfg80211_wowlan *wowlan)
+{
+	struct mwifiex_adapter *adapter = mwifiex_cfg80211_get_adapter(wiphy);
+	struct mwifiex_ds_hs_cfg hs_cfg;
+	int ret = 0;
+	struct mwifiex_private *priv =
+			mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_STA);
+
+	if (!wowlan) {
+		dev_warn(adapter->dev, "None of the WOWLAN triggers enabled\n");
+		return 0;
+	}
+
+	if (!priv->media_connected) {
+		dev_warn(adapter->dev,
+			 "Can not configure WOWLAN in disconnected state\n");
+		return 0;
+	}
+
+	if (wowlan->n_patterns || wowlan->magic_pkt) {
+		ret = mwifiex_set_mef_filter(priv, wowlan);
+		if (ret) {
+			dev_err(adapter->dev, "Failed to set MEF filter\n");
+			return ret;
+		}
+	}
+
+	if (wowlan->disconnect) {
+		memset(&hs_cfg, 0, sizeof(hs_cfg));
+		hs_cfg.is_invoke_hostcmd = false;
+		hs_cfg.conditions = HS_CFG_COND_MAC_EVENT;
+		hs_cfg.gpio = HS_CFG_GPIO_DEF;
+		hs_cfg.gap = HS_CFG_GAP_DEF;
+		ret = mwifiex_set_hs_params(priv, HostCmd_ACT_GEN_SET,
+					    MWIFIEX_SYNC_CMD, &hs_cfg);
+		if (ret) {
+			dev_err(adapter->dev, "Failed to set HS params\n");
+			return ret;
+		}
+	}
+
+	return ret;
+}
+
 static int mwifiex_cfg80211_resume(struct wiphy *wiphy)
 {
 	return 0;
@@ -2872,7 +2907,7 @@ static struct cfg80211_ops mwifiex_cfg80211_ops = {
 
 #ifdef CONFIG_PM
 static const struct wiphy_wowlan_support mwifiex_wowlan_support = {
-	.flags = WIPHY_WOWLAN_MAGIC_PKT,
+	.flags = WIPHY_WOWLAN_MAGIC_PKT | WIPHY_WOWLAN_DISCONNECT,
 	.n_patterns = MWIFIEX_MEF_MAX_FILTERS,
 	.pattern_min_len = 1,
 	.pattern_max_len = MWIFIEX_MAX_PATTERN_LEN,
diff --git a/drivers/net/wireless/mwifiex/cfp.c b/drivers/net/wireless/mwifiex/cfp.c
index b8242eb2be6f..f494fc7eeb62 100644
--- a/drivers/net/wireless/mwifiex/cfp.c
+++ b/drivers/net/wireless/mwifiex/cfp.c
@@ -509,3 +509,21 @@ u32 mwifiex_get_supported_rates(struct mwifiex_private *priv, u8 *rates)
 
 	return k;
 }
+
+u8 mwifiex_adjust_data_rate(struct mwifiex_private *priv,
+			    u8 rx_rate, u8 rate_info)
+{
+	u8 rate_index = 0;
+
+	/* HT40 */
+	if ((rate_info & BIT(0)) && (rate_info & BIT(1)))
+		rate_index = MWIFIEX_RATE_INDEX_MCS0 +
+			     MWIFIEX_BW20_MCS_NUM + rx_rate;
+	else if (rate_info & BIT(0)) /* HT20 */
+		rate_index = MWIFIEX_RATE_INDEX_MCS0 + rx_rate;
+	else
+		rate_index = (rx_rate > MWIFIEX_RATE_INDEX_OFDM0) ?
+			      rx_rate - 1 : rx_rate;
+
+	return rate_index;
+}
diff --git a/drivers/net/wireless/mwifiex/cmdevt.c b/drivers/net/wireless/mwifiex/cmdevt.c
index 85597200badc..946a2f7a172f 100644
--- a/drivers/net/wireless/mwifiex/cmdevt.c
+++ b/drivers/net/wireless/mwifiex/cmdevt.c
@@ -1008,11 +1008,9 @@ mwifiex_cancel_all_pending_cmd(struct mwifiex_adapter *adapter)
 	list_for_each_entry_safe(cmd_node, tmp_node,
 				 &adapter->scan_pending_q, list) {
 		list_del(&cmd_node->list);
-		spin_unlock_irqrestore(&adapter->scan_pending_q_lock, flags);
 
 		cmd_node->wait_q_enabled = false;
 		mwifiex_insert_cmd_to_free_q(adapter, cmd_node);
-		spin_lock_irqsave(&adapter->scan_pending_q_lock, flags);
 	}
 	spin_unlock_irqrestore(&adapter->scan_pending_q_lock, flags);
 
@@ -1070,12 +1068,8 @@ mwifiex_cancel_pending_ioctl(struct mwifiex_adapter *adapter)
 	list_for_each_entry_safe(cmd_node, tmp_node,
 				 &adapter->scan_pending_q, list) {
 		list_del(&cmd_node->list);
-		spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
-				       scan_pending_q_flags);
 		cmd_node->wait_q_enabled = false;
 		mwifiex_insert_cmd_to_free_q(adapter, cmd_node);
-		spin_lock_irqsave(&adapter->scan_pending_q_lock,
-				  scan_pending_q_flags);
 	}
 	spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
 			       scan_pending_q_flags);
diff --git a/drivers/net/wireless/mwifiex/debugfs.c b/drivers/net/wireless/mwifiex/debugfs.c
index 2713f7acd35e..1fb329dc6744 100644
--- a/drivers/net/wireless/mwifiex/debugfs.c
+++ b/drivers/net/wireless/mwifiex/debugfs.c
@@ -39,111 +39,6 @@ static char *bss_modes[] = {
 	"P2P_DEVICE",
 };
 
-/* size/addr for mwifiex_debug_info */
-#define item_size(n)		(FIELD_SIZEOF(struct mwifiex_debug_info, n))
-#define item_addr(n)		(offsetof(struct mwifiex_debug_info, n))
-
-/* size/addr for struct mwifiex_adapter */
-#define adapter_item_size(n)	(FIELD_SIZEOF(struct mwifiex_adapter, n))
-#define adapter_item_addr(n)	(offsetof(struct mwifiex_adapter, n))
-
-struct mwifiex_debug_data {
-	char name[32];		/* variable/array name */
-	u32 size;		/* size of the variable/array */
-	size_t addr;		/* address of the variable/array */
-	int num;		/* number of variables in an array */
-};
-
-static struct mwifiex_debug_data items[] = {
-	{"int_counter", item_size(int_counter),
-	 item_addr(int_counter), 1},
-	{"wmm_ac_vo", item_size(packets_out[WMM_AC_VO]),
-	 item_addr(packets_out[WMM_AC_VO]), 1},
-	{"wmm_ac_vi", item_size(packets_out[WMM_AC_VI]),
-	 item_addr(packets_out[WMM_AC_VI]), 1},
-	{"wmm_ac_be", item_size(packets_out[WMM_AC_BE]),
-	 item_addr(packets_out[WMM_AC_BE]), 1},
-	{"wmm_ac_bk", item_size(packets_out[WMM_AC_BK]),
-	 item_addr(packets_out[WMM_AC_BK]), 1},
-	{"tx_buf_size", item_size(tx_buf_size),
-	 item_addr(tx_buf_size), 1},
-	{"curr_tx_buf_size", item_size(curr_tx_buf_size),
-	 item_addr(curr_tx_buf_size), 1},
-	{"ps_mode", item_size(ps_mode),
-	 item_addr(ps_mode), 1},
-	{"ps_state", item_size(ps_state),
-	 item_addr(ps_state), 1},
-	{"is_deep_sleep", item_size(is_deep_sleep),
-	 item_addr(is_deep_sleep), 1},
-	{"wakeup_dev_req", item_size(pm_wakeup_card_req),
-	 item_addr(pm_wakeup_card_req), 1},
-	{"wakeup_tries", item_size(pm_wakeup_fw_try),
-	 item_addr(pm_wakeup_fw_try), 1},
-	{"hs_configured", item_size(is_hs_configured),
-	 item_addr(is_hs_configured), 1},
-	{"hs_activated", item_size(hs_activated),
-	 item_addr(hs_activated), 1},
-	{"num_tx_timeout", item_size(num_tx_timeout),
-	 item_addr(num_tx_timeout), 1},
-	{"is_cmd_timedout", item_size(is_cmd_timedout),
-	 item_addr(is_cmd_timedout), 1},
-	{"timeout_cmd_id", item_size(timeout_cmd_id),
-	 item_addr(timeout_cmd_id), 1},
-	{"timeout_cmd_act", item_size(timeout_cmd_act),
-	 item_addr(timeout_cmd_act), 1},
-	{"last_cmd_id", item_size(last_cmd_id),
-	 item_addr(last_cmd_id), DBG_CMD_NUM},
-	{"last_cmd_act", item_size(last_cmd_act),
-	 item_addr(last_cmd_act), DBG_CMD_NUM},
-	{"last_cmd_index", item_size(last_cmd_index),
-	 item_addr(last_cmd_index), 1},
-	{"last_cmd_resp_id", item_size(last_cmd_resp_id),
-	 item_addr(last_cmd_resp_id), DBG_CMD_NUM},
-	{"last_cmd_resp_index", item_size(last_cmd_resp_index),
-	 item_addr(last_cmd_resp_index), 1},
-	{"last_event", item_size(last_event),
-	 item_addr(last_event), DBG_CMD_NUM},
-	{"last_event_index", item_size(last_event_index),
-	 item_addr(last_event_index), 1},
-	{"num_cmd_h2c_fail", item_size(num_cmd_host_to_card_failure),
-	 item_addr(num_cmd_host_to_card_failure), 1},
-	{"num_cmd_sleep_cfm_fail",
-	 item_size(num_cmd_sleep_cfm_host_to_card_failure),
-	 item_addr(num_cmd_sleep_cfm_host_to_card_failure), 1},
-	{"num_tx_h2c_fail", item_size(num_tx_host_to_card_failure),
-	 item_addr(num_tx_host_to_card_failure), 1},
-	{"num_evt_deauth", item_size(num_event_deauth),
-	 item_addr(num_event_deauth), 1},
-	{"num_evt_disassoc", item_size(num_event_disassoc),
-	 item_addr(num_event_disassoc), 1},
-	{"num_evt_link_lost", item_size(num_event_link_lost),
-	 item_addr(num_event_link_lost), 1},
-	{"num_cmd_deauth", item_size(num_cmd_deauth),
-	 item_addr(num_cmd_deauth), 1},
-	{"num_cmd_assoc_ok", item_size(num_cmd_assoc_success),
-	 item_addr(num_cmd_assoc_success), 1},
-	{"num_cmd_assoc_fail", item_size(num_cmd_assoc_failure),
-	 item_addr(num_cmd_assoc_failure), 1},
-	{"cmd_sent", item_size(cmd_sent),
-	 item_addr(cmd_sent), 1},
-	{"data_sent", item_size(data_sent),
-	 item_addr(data_sent), 1},
-	{"cmd_resp_received", item_size(cmd_resp_received),
-	 item_addr(cmd_resp_received), 1},
-	{"event_received", item_size(event_received),
-	 item_addr(event_received), 1},
-
-	/* variables defined in struct mwifiex_adapter */
-	{"cmd_pending", adapter_item_size(cmd_pending),
-	 adapter_item_addr(cmd_pending), 1},
-	{"tx_pending", adapter_item_size(tx_pending),
-	 adapter_item_addr(tx_pending), 1},
-	{"rx_pending", adapter_item_size(rx_pending),
-	 adapter_item_addr(rx_pending), 1},
-};
-
-static int num_of_items = ARRAY_SIZE(items);
-
 /*
  * Proc info file read handler.
  *
@@ -297,6 +192,8 @@ mwifiex_fw_dump_read(struct file *file, char __user *ubuf,
  *      - Number of FCS errors
  *      - Number of Tx frames
  *      - WEP ICV error counts
+ *      - Number of received beacons
+ *      - Number of missed beacons
  */
 static ssize_t
 mwifiex_getlog_read(struct file *file, char __user *ubuf,
@@ -333,7 +230,9 @@ mwifiex_getlog_read(struct file *file, char __user *ubuf,
 		     "wepicverrcnt-1   %u\n"
 		     "wepicverrcnt-2   %u\n"
 		     "wepicverrcnt-3   %u\n"
-		     "wepicverrcnt-4   %u\n",
+		     "wepicverrcnt-4   %u\n"
+		     "bcn_rcv_cnt   %u\n"
+		     "bcn_miss_cnt   %u\n",
 		     stats.mcast_tx_frame,
 		     stats.failed,
 		     stats.retry,
@@ -349,7 +248,9 @@ mwifiex_getlog_read(struct file *file, char __user *ubuf,
 		     stats.wep_icv_error[0],
 		     stats.wep_icv_error[1],
 		     stats.wep_icv_error[2],
-		     stats.wep_icv_error[3]);
+		     stats.wep_icv_error[3],
+		     stats.bcn_rcv_cnt,
+		     stats.bcn_miss_cnt);
 
 
 	ret = simple_read_from_buffer(ubuf, count, ppos, (char *) page,
@@ -360,6 +261,103 @@ free_and_exit:
 	return ret;
 }
 
+/* Sysfs histogram file read handler.
+ *
+ * This function is called when the 'histogram' file is opened for reading
+ * It prints the following histogram information -
+ *      - Number of histogram samples
+ *      - Receive packet number of each rx_rate
+ *      - Receive packet number of each snr
+ *      - Receive packet number of each nosie_flr
+ *      - Receive packet number of each signal streath
+ */
+static ssize_t
+mwifiex_histogram_read(struct file *file, char __user *ubuf,
+		       size_t count, loff_t *ppos)
+{
+	struct mwifiex_private *priv =
+		(struct mwifiex_private *)file->private_data;
+	ssize_t ret;
+	struct mwifiex_histogram_data *phist_data;
+	int i, value;
+	unsigned long page = get_zeroed_page(GFP_KERNEL);
+	char *p = (char *)page;
+
+	if (!p)
+		return -ENOMEM;
+
+	if (!priv || !priv->hist_data)
+		return -EFAULT;
+	phist_data = priv->hist_data;
+
+	p += sprintf(p, "\n"
+		     "total samples = %d\n",
+		     atomic_read(&phist_data->num_samples));
+
+	p += sprintf(p, "rx rates (in Mbps): 0=1M   1=2M");
+	p += sprintf(p, "2=5.5M  3=11M   4=6M   5=9M  6=12M\n");
+	p += sprintf(p, "7=18M  8=24M  9=36M  10=48M  11=54M");
+	p += sprintf(p, "12-27=MCS0-15(BW20) 28-43=MCS0-15(BW40)\n");
+
+	if (ISSUPP_11ACENABLED(priv->adapter->fw_cap_info)) {
+		p += sprintf(p, "44-53=MCS0-9(VHT:BW20)");
+		p += sprintf(p, "54-63=MCS0-9(VHT:BW40)");
+		p += sprintf(p, "64-73=MCS0-9(VHT:BW80)\n\n");
+	} else {
+		p += sprintf(p, "\n");
+	}
+
+	for (i = 0; i < MWIFIEX_MAX_RX_RATES; i++) {
+		value = atomic_read(&phist_data->rx_rate[i]);
+		if (value)
+			p += sprintf(p, "rx_rate[%02d] = %d\n", i, value);
+	}
+
+	if (ISSUPP_11ACENABLED(priv->adapter->fw_cap_info)) {
+		for (i = MWIFIEX_MAX_RX_RATES; i < MWIFIEX_MAX_AC_RX_RATES;
+		     i++) {
+			value = atomic_read(&phist_data->rx_rate[i]);
+			if (value)
+				p += sprintf(p, "rx_rate[%02d] = %d\n",
+					   i, value);
+		}
+	}
+
+	for (i = 0; i < MWIFIEX_MAX_SNR; i++) {
+		value =  atomic_read(&phist_data->snr[i]);
+		if (value)
+			p += sprintf(p, "snr[%02ddB] = %d\n", i, value);
+	}
+	for (i = 0; i < MWIFIEX_MAX_NOISE_FLR; i++) {
+		value = atomic_read(&phist_data->noise_flr[i]);
+		if (value)
+			p += sprintf(p, "noise_flr[-%02ddBm] = %d\n",
+				(int)(i-128), value);
+	}
+	for (i = 0; i < MWIFIEX_MAX_SIG_STRENGTH; i++) {
+		value = atomic_read(&phist_data->sig_str[i]);
+		if (value)
+			p += sprintf(p, "sig_strength[-%02ddBm] = %d\n",
+				i, value);
+	}
+
+	ret = simple_read_from_buffer(ubuf, count, ppos, (char *)page,
+				      (unsigned long)p - page);
+
+	return ret;
+}
+
+static ssize_t
+mwifiex_histogram_write(struct file *file, const char __user *ubuf,
+			size_t count, loff_t *ppos)
+{
+	struct mwifiex_private *priv = (void *)file->private_data;
+
+	if (priv && priv->hist_data)
+		mwifiex_hist_data_reset(priv);
+	return 0;
+}
+
 static struct mwifiex_debug_info info;
 
 /*
@@ -415,13 +413,9 @@ mwifiex_debug_read(struct file *file, char __user *ubuf,
 {
 	struct mwifiex_private *priv =
 		(struct mwifiex_private *) file->private_data;
-	struct mwifiex_debug_data *d = &items[0];
 	unsigned long page = get_zeroed_page(GFP_KERNEL);
 	char *p = (char *) page;
 	ssize_t ret;
-	size_t size, addr;
-	long val;
-	int i, j;
 
 	if (!p)
 		return -ENOMEM;
@@ -430,68 +424,7 @@ mwifiex_debug_read(struct file *file, char __user *ubuf,
 	if (ret)
 		goto free_and_exit;
 
-	for (i = 0; i < num_of_items; i++) {
-		p += sprintf(p, "%s=", d[i].name);
-
-		size = d[i].size / d[i].num;
-
-		if (i < (num_of_items - 3))
-			addr = d[i].addr + (size_t) &info;
-		else /* The last 3 items are struct mwifiex_adapter variables */
-			addr = d[i].addr + (size_t) priv->adapter;
-
-		for (j = 0; j < d[i].num; j++) {
-			switch (size) {
-			case 1:
-				val = *((u8 *) addr);
-				break;
-			case 2:
-				val = *((u16 *) addr);
-				break;
-			case 4:
-				val = *((u32 *) addr);
-				break;
-			case 8:
-				val = *((long long *) addr);
-				break;
-			default:
-				val = -1;
-				break;
-			}
-
-			p += sprintf(p, "%#lx ", val);
-			addr += size;
-		}
-
-		p += sprintf(p, "\n");
-	}
-
-	if (info.tx_tbl_num) {
-		p += sprintf(p, "Tx BA stream table:\n");
-		for (i = 0; i < info.tx_tbl_num; i++)
-			p += sprintf(p, "tid = %d, ra = %pM\n",
-				     info.tx_tbl[i].tid, info.tx_tbl[i].ra);
-	}
-
-	if (info.rx_tbl_num) {
-		p += sprintf(p, "Rx reorder table:\n");
-		for (i = 0; i < info.rx_tbl_num; i++) {
-			p += sprintf(p, "tid = %d, ta = %pM, "
-				     "start_win = %d, "
-				     "win_size = %d, buffer: ",
-				     info.rx_tbl[i].tid,
-				     info.rx_tbl[i].ta,
-				     info.rx_tbl[i].start_win,
-				     info.rx_tbl[i].win_size);
-
-			for (j = 0; j < info.rx_tbl[i].win_size; j++)
-				p += sprintf(p, "%c ",
-					     info.rx_tbl[i].buffer[j] ?
-					     '1' : '0');
-
-			p += sprintf(p, "\n");
-		}
-	}
+	p += mwifiex_debug_info_to_buffer(priv, p, &info);
 
 	ret = simple_read_from_buffer(ubuf, count, ppos, (char *) page,
 				      (unsigned long) p - page);
@@ -817,6 +750,7 @@ MWIFIEX_DFS_FILE_READ_OPS(fw_dump);
 MWIFIEX_DFS_FILE_OPS(regrdwr);
 MWIFIEX_DFS_FILE_OPS(rdeeprom);
 MWIFIEX_DFS_FILE_OPS(hscfg);
+MWIFIEX_DFS_FILE_OPS(histogram);
 
 /*
  * This function creates the debug FS directory structure and the files.
@@ -840,6 +774,7 @@ mwifiex_dev_debugfs_init(struct mwifiex_private *priv)
 	MWIFIEX_DFS_ADD_FILE(rdeeprom);
 	MWIFIEX_DFS_ADD_FILE(fw_dump);
 	MWIFIEX_DFS_ADD_FILE(hscfg);
+	MWIFIEX_DFS_ADD_FILE(histogram);
 }
 
 /*
diff --git a/drivers/net/wireless/mwifiex/decl.h b/drivers/net/wireless/mwifiex/decl.h
index 2269acf41ad8..7aa988e1dc7a 100644
--- a/drivers/net/wireless/mwifiex/decl.h
+++ b/drivers/net/wireless/mwifiex/decl.h
@@ -32,15 +32,19 @@
 
 #define MWIFIEX_MAX_BSS_NUM         (3)
 
-#define MWIFIEX_MIN_DATA_HEADER_LEN 36	/* sizeof(mwifiex_txpd)
-					 *   + 4 byte alignment
-					 */
+#define MWIFIEX_DMA_ALIGN_SZ	    64
+#define MAX_TXPD_SZ		    32
+#define INTF_HDR_ALIGN		     4
+
+#define MWIFIEX_MIN_DATA_HEADER_LEN (MWIFIEX_DMA_ALIGN_SZ + INTF_HDR_ALIGN + \
+				     MAX_TXPD_SZ)
 #define MWIFIEX_MGMT_FRAME_HEADER_SIZE	8	/* sizeof(pkt_type)
 						 *   + sizeof(tx_control)
 						 */
 
 #define MWIFIEX_MAX_TX_BASTREAM_SUPPORTED	2
 #define MWIFIEX_MAX_RX_BASTREAM_SUPPORTED	16
+#define MWIFIEX_MAX_TDLS_PEER_SUPPORTED 8
 
 #define MWIFIEX_STA_AMPDU_DEF_TXWINSIZE        64
 #define MWIFIEX_STA_AMPDU_DEF_RXWINSIZE        64
@@ -92,6 +96,14 @@
 #define MWIFIEX_TDLS_MAX_FAIL_COUNT      4
 #define MWIFIEX_AUTO_TDLS_IDLE_TIME     10
 
+/* 54M rates, index from 0 to 11 */
+#define MWIFIEX_RATE_INDEX_MCS0 12
+/* 12-27=MCS0-15(BW20) */
+#define MWIFIEX_BW20_MCS_NUM 15
+
+/* Rate index for OFDM 0 */
+#define MWIFIEX_RATE_INDEX_OFDM0   4
+
 enum mwifiex_bss_type {
 	MWIFIEX_BSS_TYPE_STA = 0,
 	MWIFIEX_BSS_TYPE_UAP = 1,
@@ -204,4 +216,20 @@ struct mwifiex_chan_stats {
 	u16 cca_scan_dur;
 	u16 cca_busy_dur;
 } __packed;
+
+#define MWIFIEX_HIST_MAX_SAMPLES	1048576
+#define MWIFIEX_MAX_RX_RATES		     44
+#define MWIFIEX_MAX_AC_RX_RATES		     74
+#define MWIFIEX_MAX_SNR			    256
+#define MWIFIEX_MAX_NOISE_FLR		    256
+#define MWIFIEX_MAX_SIG_STRENGTH	    256
+
+struct mwifiex_histogram_data {
+	atomic_t rx_rate[MWIFIEX_MAX_AC_RX_RATES];
+	atomic_t snr[MWIFIEX_MAX_SNR];
+	atomic_t noise_flr[MWIFIEX_MAX_NOISE_FLR];
+	atomic_t sig_str[MWIFIEX_MAX_SIG_STRENGTH];
+	atomic_t num_samples;
+};
+
 #endif /* !_MWIFIEX_DECL_H_ */
diff --git a/drivers/net/wireless/mwifiex/ethtool.c b/drivers/net/wireless/mwifiex/ethtool.c
index 04e56b5fc535..65d8d6d4b6ba 100644
--- a/drivers/net/wireless/mwifiex/ethtool.c
+++ b/drivers/net/wireless/mwifiex/ethtool.c
@@ -76,7 +76,9 @@ mwifiex_get_dump_flag(struct net_device *dev, struct ethtool_dump *dump)
 
 	dump->flag = adapter->curr_mem_idx;
 	dump->version = 1;
-	if (adapter->curr_mem_idx != MWIFIEX_FW_DUMP_IDX) {
+	if (adapter->curr_mem_idx == MWIFIEX_DRV_INFO_IDX) {
+		dump->len = adapter->drv_info_size;
+	} else if (adapter->curr_mem_idx != MWIFIEX_FW_DUMP_IDX) {
 		entry = &adapter->mem_type_mapping_tbl[adapter->curr_mem_idx];
 		dump->len = entry->mem_size;
 	} else {
@@ -98,6 +100,13 @@ mwifiex_get_dump_data(struct net_device *dev, struct ethtool_dump *dump,
 	if (!adapter->if_ops.fw_dump)
 		return -ENOTSUPP;
 
+	if (adapter->curr_mem_idx == MWIFIEX_DRV_INFO_IDX) {
+		if (!adapter->drv_info_dump)
+			return -EFAULT;
+		memcpy(p, adapter->drv_info_dump, adapter->drv_info_size);
+		return 0;
+	}
+
 	if (adapter->curr_mem_idx == MWIFIEX_FW_DUMP_IDX) {
 		dev_err(adapter->dev, "firmware dump in progress!!\n");
 		return -EBUSY;
@@ -125,6 +134,11 @@ static int mwifiex_set_dump(struct net_device *dev, struct ethtool_dump *val)
 	if (!adapter->if_ops.fw_dump)
 		return -ENOTSUPP;
 
+	if (val->flag == MWIFIEX_DRV_INFO_IDX) {
+		adapter->curr_mem_idx = MWIFIEX_DRV_INFO_IDX;
+		return 0;
+	}
+
 	if (adapter->curr_mem_idx == MWIFIEX_FW_DUMP_IDX) {
 		dev_err(adapter->dev, "firmware dump in progress!!\n");
 		return -EBUSY;
diff --git a/drivers/net/wireless/mwifiex/fw.h b/drivers/net/wireless/mwifiex/fw.h
index fb5936eb82e3..15ad776ae08e 100644
--- a/drivers/net/wireless/mwifiex/fw.h
+++ b/drivers/net/wireless/mwifiex/fw.h
@@ -233,6 +233,7 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define ISSUPP_RXLDPC(Dot11nDevCap) (Dot11nDevCap & BIT(22))
 #define ISSUPP_BEAMFORMING(Dot11nDevCap) (Dot11nDevCap & BIT(30))
 #define ISALLOWED_CHANWIDTH40(ht_param) (ht_param & BIT(2))
+#define GETSUPP_TXBASTREAMS(Dot11nDevCap) ((Dot11nDevCap >> 18) & 0xF)
 
 /* httxcfg bitmap
  * 0		reserved
@@ -1076,6 +1077,8 @@ struct host_cmd_ds_802_11_get_log {
 	__le32 tx_frame;
 	__le32 reserved;
 	__le32 wep_icv_err_cnt[4];
+	__le32 bcn_rcv_cnt;
+	__le32 bcn_miss_cnt;
 };
 
 /* Enumeration for rate format */
diff --git a/drivers/net/wireless/mwifiex/init.c b/drivers/net/wireless/mwifiex/init.c
index 520ad4a3018b..b115e0f94dd7 100644
--- a/drivers/net/wireless/mwifiex/init.c
+++ b/drivers/net/wireless/mwifiex/init.c
@@ -52,6 +52,18 @@ static int mwifiex_add_bss_prio_tbl(struct mwifiex_private *priv)
 	return 0;
 }
 
+static void wakeup_timer_fn(unsigned long data)
+{
+	struct mwifiex_adapter *adapter = (struct mwifiex_adapter *)data;
+
+	dev_err(adapter->dev, "Firmware wakeup failed\n");
+	adapter->hw_status = MWIFIEX_HW_STATUS_RESET;
+	mwifiex_cancel_all_pending_cmd(adapter);
+
+	if (adapter->if_ops.card_reset)
+		adapter->if_ops.card_reset(adapter);
+}
+
 /*
  * This function initializes the private structure and sets default
  * values to the members.
@@ -285,6 +297,9 @@ static void mwifiex_init_adapter(struct mwifiex_adapter *adapter)
 	adapter->ext_scan = true;
 	adapter->key_api_major_ver = 0;
 	adapter->key_api_minor_ver = 0;
+
+	setup_timer(&adapter->wakeup_timer, wakeup_timer_fn,
+		    (unsigned long)adapter);
 }
 
 /*
@@ -391,7 +406,10 @@ mwifiex_adapter_cleanup(struct mwifiex_adapter *adapter)
 		return;
 	}
 
+	del_timer(&adapter->wakeup_timer);
 	mwifiex_cancel_all_pending_cmd(adapter);
+	wake_up_interruptible(&adapter->cmd_wait_q.wait);
+	wake_up_interruptible(&adapter->hs_activate_wait_q);
 
 	/* Free lock variables */
 	mwifiex_free_lock_list(adapter);
@@ -411,6 +429,11 @@ mwifiex_adapter_cleanup(struct mwifiex_adapter *adapter)
 		entry->mem_size = 0;
 	}
 
+	if (adapter->drv_info_dump) {
+		vfree(adapter->drv_info_dump);
+		adapter->drv_info_size = 0;
+	}
+
 	if (adapter->sleep_cfm)
 		dev_kfree_skb_any(adapter->sleep_cfm);
 }
diff --git a/drivers/net/wireless/mwifiex/ioctl.h b/drivers/net/wireless/mwifiex/ioctl.h
index 0847f3e07ab7..d2b05c3a96da 100644
--- a/drivers/net/wireless/mwifiex/ioctl.h
+++ b/drivers/net/wireless/mwifiex/ioctl.h
@@ -137,6 +137,8 @@ struct mwifiex_ds_get_stats {
 	u32 fcs_error;
 	u32 tx_frame;
 	u32 wep_icv_error[4];
+	u32 bcn_rcv_cnt;
+	u32 bcn_miss_cnt;
 };
 
 #define MWIFIEX_MAX_VER_STR_LEN    128
@@ -180,7 +182,11 @@ struct mwifiex_ds_tx_ba_stream_tbl {
 	u8 amsdu;
 };
 
-#define DBG_CMD_NUM	5
+#define DBG_CMD_NUM    5
+
+struct tdls_peer_info {
+	u8 peer_addr[ETH_ALEN];
+};
 
 struct mwifiex_debug_info {
 	u32 int_counter;
@@ -193,6 +199,9 @@ struct mwifiex_debug_info {
 	u32 rx_tbl_num;
 	struct mwifiex_ds_rx_reorder_tbl rx_tbl
 		[MWIFIEX_MAX_RX_BASTREAM_SUPPORTED];
+	u32 tdls_peer_num;
+	struct tdls_peer_info tdls_list
+		[MWIFIEX_MAX_TDLS_PEER_SUPPORTED];
 	u16 ps_mode;
 	u32 ps_state;
 	u8 is_deep_sleep;
diff --git a/drivers/net/wireless/mwifiex/main.c b/drivers/net/wireless/mwifiex/main.c
index d4d2223d1f31..d235adb82c94 100644
--- a/drivers/net/wireless/mwifiex/main.c
+++ b/drivers/net/wireless/mwifiex/main.c
@@ -83,9 +83,8 @@ static int mwifiex_register(void *card, struct mwifiex_if_ops *if_ops,
 	}
 	mwifiex_init_lock_list(adapter);
 
-	init_timer(&adapter->cmd_timer);
-	adapter->cmd_timer.function = mwifiex_cmd_timeout_func;
-	adapter->cmd_timer.data = (unsigned long) adapter;
+	setup_timer(&adapter->cmd_timer, mwifiex_cmd_timeout_func,
+		    (unsigned long)adapter);
 
 	return 0;
 
@@ -237,6 +236,7 @@ process_start:
 		    (is_command_pending(adapter) ||
 		     !mwifiex_wmm_lists_empty(adapter))) {
 			adapter->pm_wakeup_fw_try = true;
+			mod_timer(&adapter->wakeup_timer, jiffies + (HZ*3));
 			adapter->if_ops.wakeup(adapter);
 			continue;
 		}
@@ -244,6 +244,7 @@ process_start:
 		if (IS_CARD_RX_RCVD(adapter)) {
 			adapter->data_received = false;
 			adapter->pm_wakeup_fw_try = false;
+			del_timer_sync(&adapter->wakeup_timer);
 			if (adapter->ps_state == PS_STATE_SLEEP)
 				adapter->ps_state = PS_STATE_AWAKE;
 		} else {
@@ -511,8 +512,7 @@ err_dnld_fw:
 	if (adapter->if_ops.unregister_dev)
 		adapter->if_ops.unregister_dev(adapter);
 
-	if ((adapter->hw_status == MWIFIEX_HW_STATUS_FW_READY) ||
-	    (adapter->hw_status == MWIFIEX_HW_STATUS_READY)) {
+	if (adapter->hw_status == MWIFIEX_HW_STATUS_READY) {
 		pr_debug("info: %s: shutdown mwifiex\n", __func__);
 		adapter->init_wait_q_woken = false;
 
@@ -801,6 +801,114 @@ mwifiex_tx_timeout(struct net_device *dev)
 	}
 }
 
+void mwifiex_dump_drv_info(struct mwifiex_adapter *adapter)
+{
+	void *p;
+	char drv_version[64];
+	struct usb_card_rec *cardp;
+	struct sdio_mmc_card *sdio_card;
+	struct mwifiex_private *priv;
+	int i, idx;
+	struct netdev_queue *txq;
+	struct mwifiex_debug_info *debug_info;
+
+	if (adapter->drv_info_dump) {
+		vfree(adapter->drv_info_dump);
+		adapter->drv_info_size = 0;
+	}
+
+	dev_info(adapter->dev, "=== DRIVER INFO DUMP START===\n");
+
+	adapter->drv_info_dump = vzalloc(MWIFIEX_DRV_INFO_SIZE_MAX);
+
+	if (!adapter->drv_info_dump)
+		return;
+
+	p = (char *)(adapter->drv_info_dump);
+	p += sprintf(p, "driver_name = " "\"mwifiex\"\n");
+
+	mwifiex_drv_get_driver_version(adapter, drv_version,
+				       sizeof(drv_version) - 1);
+	p += sprintf(p, "driver_version = %s\n", drv_version);
+
+	if (adapter->iface_type == MWIFIEX_USB) {
+		cardp = (struct usb_card_rec *)adapter->card;
+		p += sprintf(p, "tx_cmd_urb_pending = %d\n",
+			     atomic_read(&cardp->tx_cmd_urb_pending));
+		p += sprintf(p, "tx_data_urb_pending = %d\n",
+			     atomic_read(&cardp->tx_data_urb_pending));
+		p += sprintf(p, "rx_cmd_urb_pending = %d\n",
+			     atomic_read(&cardp->rx_cmd_urb_pending));
+		p += sprintf(p, "rx_data_urb_pending = %d\n",
+			     atomic_read(&cardp->rx_data_urb_pending));
+	}
+
+	p += sprintf(p, "tx_pending = %d\n",
+		     atomic_read(&adapter->tx_pending));
+	p += sprintf(p, "rx_pending = %d\n",
+		     atomic_read(&adapter->rx_pending));
+
+	if (adapter->iface_type == MWIFIEX_SDIO) {
+		sdio_card = (struct sdio_mmc_card *)adapter->card;
+		p += sprintf(p, "\nmp_rd_bitmap=0x%x curr_rd_port=0x%x\n",
+			     sdio_card->mp_rd_bitmap, sdio_card->curr_rd_port);
+		p += sprintf(p, "mp_wr_bitmap=0x%x curr_wr_port=0x%x\n",
+			     sdio_card->mp_wr_bitmap, sdio_card->curr_wr_port);
+	}
+
+	for (i = 0; i < adapter->priv_num; i++) {
+		if (!adapter->priv[i] || !adapter->priv[i]->netdev)
+			continue;
+		priv = adapter->priv[i];
+		p += sprintf(p, "\n[interface  : \"%s\"]\n",
+			     priv->netdev->name);
+		p += sprintf(p, "wmm_tx_pending[0] = %d\n",
+			     atomic_read(&priv->wmm_tx_pending[0]));
+		p += sprintf(p, "wmm_tx_pending[1] = %d\n",
+			     atomic_read(&priv->wmm_tx_pending[1]));
+		p += sprintf(p, "wmm_tx_pending[2] = %d\n",
+			     atomic_read(&priv->wmm_tx_pending[2]));
+		p += sprintf(p, "wmm_tx_pending[3] = %d\n",
+			     atomic_read(&priv->wmm_tx_pending[3]));
+		p += sprintf(p, "media_state=\"%s\"\n", !priv->media_connected ?
+			     "Disconnected" : "Connected");
+		p += sprintf(p, "carrier %s\n", (netif_carrier_ok(priv->netdev)
+			     ? "on" : "off"));
+		for (idx = 0; idx < priv->netdev->num_tx_queues; idx++) {
+			txq = netdev_get_tx_queue(priv->netdev, idx);
+			p += sprintf(p, "tx queue %d:%s  ", idx,
+				     netif_tx_queue_stopped(txq) ?
+				     "stopped" : "started");
+		}
+		p += sprintf(p, "\n%s: num_tx_timeout = %d\n",
+			     priv->netdev->name, priv->num_tx_timeout);
+	}
+
+	if (adapter->iface_type == MWIFIEX_SDIO) {
+		p += sprintf(p, "\n=== SDIO register DUMP===\n");
+		if (adapter->if_ops.reg_dump)
+			p += adapter->if_ops.reg_dump(adapter, p);
+	}
+
+	p += sprintf(p, "\n=== MORE DEBUG INFORMATION\n");
+	debug_info = kzalloc(sizeof(*debug_info), GFP_KERNEL);
+	if (debug_info) {
+		for (i = 0; i < adapter->priv_num; i++) {
+			if (!adapter->priv[i] || !adapter->priv[i]->netdev)
+				continue;
+			priv = adapter->priv[i];
+			mwifiex_get_debug_info(priv, debug_info);
+			p += mwifiex_debug_info_to_buffer(priv, p, debug_info);
+			break;
+		}
+		kfree(debug_info);
+	}
+
+	adapter->drv_info_size = p - adapter->drv_info_dump;
+	dev_info(adapter->dev, "=== DRIVER INFO DUMP END===\n");
+}
+EXPORT_SYMBOL_GPL(mwifiex_dump_drv_info);
+
 /*
  * CFG802.11 network device handler for statistics retrieval.
  */
@@ -847,6 +955,7 @@ static const struct net_device_ops mwifiex_netdev_ops = {
  *      - Nick name             : Set to null
  *      - Number of Tx timeout  : Set to 0
  *      - Device address        : Set to current address
+ *      - Rx histogram statistc : Set to 0
  *
  * In addition, the CFG80211 work queue is also created.
  */
@@ -867,6 +976,13 @@ void mwifiex_init_priv_params(struct mwifiex_private *priv,
 	priv->rsn_idx = MWIFIEX_AUTO_IDX_MASK;
 	priv->num_tx_timeout = 0;
 	memcpy(dev->dev_addr, priv->curr_addr, ETH_ALEN);
+
+	if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA ||
+	    GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP) {
+		priv->hist_data = kmalloc(sizeof(*priv->hist_data), GFP_KERNEL);
+		if (priv->hist_data)
+			mwifiex_hist_data_reset(priv);
+	}
 }
 
 /*
@@ -1000,8 +1116,7 @@ err_init_fw:
 	pr_debug("info: %s: unregister device\n", __func__);
 	if (adapter->if_ops.unregister_dev)
 		adapter->if_ops.unregister_dev(adapter);
-	if ((adapter->hw_status == MWIFIEX_HW_STATUS_FW_READY) ||
-	    (adapter->hw_status == MWIFIEX_HW_STATUS_READY)) {
+	if (adapter->hw_status == MWIFIEX_HW_STATUS_READY) {
 		pr_debug("info: %s: shutdown mwifiex\n", __func__);
 		adapter->init_wait_q_woken = false;
 
@@ -1052,6 +1167,8 @@ int mwifiex_remove_card(struct mwifiex_adapter *adapter, struct semaphore *sem)
 
 	adapter->surprise_removed = true;
 
+	mwifiex_terminate_workqueue(adapter);
+
 	/* Stop data */
 	for (i = 0; i < adapter->priv_num; i++) {
 		priv = adapter->priv[i];
@@ -1094,8 +1211,6 @@ int mwifiex_remove_card(struct mwifiex_adapter *adapter, struct semaphore *sem)
 	wiphy_unregister(adapter->wiphy);
 	wiphy_free(adapter->wiphy);
 
-	mwifiex_terminate_workqueue(adapter);
-
 	/* Unregister device */
 	dev_dbg(adapter->dev, "info: unregister device\n");
 	if (adapter->if_ops.unregister_dev)
diff --git a/drivers/net/wireless/mwifiex/main.h b/drivers/net/wireless/mwifiex/main.h
index e66993cb5daf..55273eefb785 100644
--- a/drivers/net/wireless/mwifiex/main.h
+++ b/drivers/net/wireless/mwifiex/main.h
@@ -41,6 +41,8 @@
 #include "util.h"
 #include "fw.h"
 #include "pcie.h"
+#include "usb.h"
+#include "sdio.h"
 
 extern const char driver_version[];
 
@@ -136,6 +138,8 @@ enum {
 /* Threshold for tx_timeout_cnt before we trigger a card reset */
 #define TX_TIMEOUT_THRESHOLD	6
 
+#define MWIFIEX_DRV_INFO_SIZE_MAX 0x40000
+
 struct mwifiex_dbg {
 	u32 num_cmd_host_to_card_failure;
 	u32 num_cmd_sleep_cfm_host_to_card_failure;
@@ -161,7 +165,6 @@ struct mwifiex_dbg {
 enum MWIFIEX_HARDWARE_STATUS {
 	MWIFIEX_HW_STATUS_READY,
 	MWIFIEX_HW_STATUS_INITIALIZING,
-	MWIFIEX_HW_STATUS_FW_READY,
 	MWIFIEX_HW_STATUS_INIT_DONE,
 	MWIFIEX_HW_STATUS_RESET,
 	MWIFIEX_HW_STATUS_CLOSING,
@@ -413,6 +416,7 @@ struct mwifiex_roc_cfg {
 };
 
 #define MWIFIEX_FW_DUMP_IDX		0xff
+#define MWIFIEX_DRV_INFO_IDX		20
 #define FW_DUMP_MAX_NAME_LEN		8
 #define FW_DUMP_HOST_READY		0xEE
 #define FW_DUMP_DONE			0xFF
@@ -582,6 +586,8 @@ struct mwifiex_private {
 	struct idr ack_status_frames;
 	/* spin lock for ack status */
 	spinlock_t ack_status_lock;
+	/** rx histogram data */
+	struct mwifiex_histogram_data *hist_data;
 };
 
 enum mwifiex_ba_status {
@@ -717,6 +723,7 @@ struct mwifiex_if_ops {
 	int (*dnld_fw) (struct mwifiex_adapter *, struct mwifiex_fw_image *);
 	void (*card_reset) (struct mwifiex_adapter *);
 	void (*fw_dump)(struct mwifiex_adapter *);
+	int (*reg_dump)(struct mwifiex_adapter *, char *);
 	int (*clean_pcie_ring) (struct mwifiex_adapter *adapter);
 	void (*iface_work)(struct work_struct *work);
 	void (*submit_rem_rx_urbs)(struct mwifiex_adapter *adapter);
@@ -823,6 +830,7 @@ struct mwifiex_adapter {
 	u16 gen_null_pkt;
 	u16 pps_uapsd_mode;
 	u32 pm_wakeup_fw_try;
+	struct timer_list wakeup_timer;
 	u8 is_hs_configured;
 	struct mwifiex_hs_config_param hs_cfg;
 	u8 hs_activated;
@@ -865,6 +873,8 @@ struct mwifiex_adapter {
 	struct memory_type_mapping *mem_type_mapping_tbl;
 	u8 num_mem_types;
 	u8 curr_mem_idx;
+	void *drv_info_dump;
+	u32 drv_info_size;
 	bool scan_chan_gap_enabled;
 	struct sk_buff_head rx_data_q;
 	struct mwifiex_chan_stats *chan_stats;
@@ -1324,6 +1334,8 @@ void mwifiex_process_tdls_action_frame(struct mwifiex_private *priv,
 				       u8 *buf, int len);
 int mwifiex_tdls_oper(struct mwifiex_private *priv, const u8 *peer, u8 action);
 int mwifiex_get_tdls_link_status(struct mwifiex_private *priv, const u8 *mac);
+int mwifiex_get_tdls_list(struct mwifiex_private *priv,
+			  struct tdls_peer_info *buf);
 void mwifiex_disable_all_tdls_links(struct mwifiex_private *priv);
 bool mwifiex_is_bss_in_11ac_mode(struct mwifiex_private *priv);
 u8 mwifiex_get_center_freq_index(struct mwifiex_private *priv, u8 band,
@@ -1348,6 +1360,16 @@ struct sk_buff *
 mwifiex_clone_skb_for_tx_status(struct mwifiex_private *priv,
 				struct sk_buff *skb, u8 flag, u64 *cookie);
 
+void mwifiex_hist_data_set(struct mwifiex_private *priv, u8 rx_rate, s8 snr,
+			   s8 nflr);
+void mwifiex_hist_data_reset(struct mwifiex_private *priv);
+void mwifiex_hist_data_add(struct mwifiex_private *priv,
+			   u8 rx_rate, s8 snr, s8 nflr);
+u8 mwifiex_adjust_data_rate(struct mwifiex_private *priv,
+			    u8 rx_rate, u8 ht_info);
+
+void mwifiex_dump_drv_info(struct mwifiex_adapter *adapter);
+
 #ifdef CONFIG_DEBUG_FS
 void mwifiex_debugfs_init(void);
 void mwifiex_debugfs_remove(void);
diff --git a/drivers/net/wireless/mwifiex/pcie.c b/drivers/net/wireless/mwifiex/pcie.c
index c3a20f94f3c9..a460b0e6a151 100644
--- a/drivers/net/wireless/mwifiex/pcie.c
+++ b/drivers/net/wireless/mwifiex/pcie.c
@@ -1952,8 +1952,8 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 		offset += txlen;
 	} while (true);
 
-	dev_dbg(adapter->dev, "info:\nFW download over, size %d bytes\n",
-		offset);
+	dev_notice(adapter->dev,
+		   "info: FW download over, size %d bytes\n", offset);
 
 	ret = 0;
 
@@ -2064,6 +2064,7 @@ static void mwifiex_interrupt_status(struct mwifiex_adapter *adapter)
 				 * state until cookie is set */
 				adapter->ps_state = PS_STATE_AWAKE;
 				adapter->pm_wakeup_fw_try = false;
+				del_timer(&adapter->wakeup_timer);
 		}
 	}
 }
diff --git a/drivers/net/wireless/mwifiex/sdio.c b/drivers/net/wireless/mwifiex/sdio.c
index 933dae137850..a70f103359ea 100644
--- a/drivers/net/wireless/mwifiex/sdio.c
+++ b/drivers/net/wireless/mwifiex/sdio.c
@@ -986,8 +986,8 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 		offset += txlen;
 	} while (true);
 
-	dev_dbg(adapter->dev, "info: FW download over, size %d bytes\n",
-		offset);
+	dev_notice(adapter->dev,
+		   "info: FW download over, size %d bytes\n", offset);
 
 	ret = 0;
 done:
@@ -1958,8 +1958,8 @@ static void mwifiex_sdio_card_reset_work(struct mwifiex_adapter *adapter)
 
 	pr_err("Resetting card...\n");
 	mmc_remove_host(target);
-	/* 20ms delay is based on experiment with sdhci controller */
-	mdelay(20);
+	/* 200ms delay is based on experiment with sdhci controller */
+	mdelay(200);
 	target->rescan_entered = 0; /* rescan non-removable cards */
 	mmc_add_host(target);
 }
@@ -2023,6 +2023,8 @@ static void mwifiex_sdio_fw_dump_work(struct work_struct *work)
 	u32 memory_size;
 	static char *env[] = { "DRIVER=mwifiex_sdio", "EVENT=fw_dump", NULL };
 
+	mwifiex_dump_drv_info(adapter);
+
 	if (!card->supports_fw_dump)
 		return;
 
@@ -2166,6 +2168,99 @@ static void mwifiex_sdio_fw_dump(struct mwifiex_adapter *adapter)
 	schedule_work(&adapter->iface_work);
 }
 
+/* Function to dump SDIO function registers and SDIO scratch registers in case
+ * of FW crash
+ */
+static int
+mwifiex_sdio_reg_dump(struct mwifiex_adapter *adapter, char *drv_buf)
+{
+	char *p = drv_buf;
+	struct sdio_mmc_card *cardp = adapter->card;
+	int ret = 0;
+	u8 count, func, data, index = 0, size = 0;
+	u8 reg, reg_start, reg_end;
+	char buf[256], *ptr;
+
+	if (!p)
+		return 0;
+
+	dev_info(adapter->dev, "SDIO register DUMP START\n");
+
+	mwifiex_pm_wakeup_card(adapter);
+
+	sdio_claim_host(cardp->func);
+
+	for (count = 0; count < 5; count++) {
+		memset(buf, 0, sizeof(buf));
+		ptr = buf;
+
+		switch (count) {
+		case 0:
+			/* Read the registers of SDIO function0 */
+			func = count;
+			reg_start = 0;
+			reg_end = 9;
+			break;
+		case 1:
+			/* Read the registers of SDIO function1 */
+			func = count;
+			reg_start = cardp->reg->func1_dump_reg_start;
+			reg_end = cardp->reg->func1_dump_reg_end;
+			break;
+		case 2:
+			index = 0;
+			func = 1;
+			reg_start = cardp->reg->func1_spec_reg_table[index++];
+			size = cardp->reg->func1_spec_reg_num;
+			reg_end = cardp->reg->func1_spec_reg_table[size-1];
+			break;
+		default:
+			/* Read the scratch registers of SDIO function1 */
+			if (count == 4)
+				mdelay(100);
+			func = 1;
+			reg_start = cardp->reg->func1_scratch_reg;
+			reg_end = reg_start + MWIFIEX_SDIO_SCRATCH_SIZE;
+		}
+
+		if (count != 2)
+			ptr += sprintf(ptr, "SDIO Func%d (%#x-%#x): ",
+				       func, reg_start, reg_end);
+		else
+			ptr += sprintf(ptr, "SDIO Func%d: ", func);
+
+		for (reg = reg_start; reg <= reg_end;) {
+			if (func == 0)
+				data = sdio_f0_readb(cardp->func, reg, &ret);
+			else
+				data = sdio_readb(cardp->func, reg, &ret);
+
+			if (count == 2)
+				ptr += sprintf(ptr, "(%#x) ", reg);
+			if (!ret) {
+				ptr += sprintf(ptr, "%02x ", data);
+			} else {
+				ptr += sprintf(ptr, "ERR");
+				break;
+			}
+
+			if (count == 2 && reg < reg_end)
+				reg = cardp->reg->func1_spec_reg_table[index++];
+			else
+				reg++;
+		}
+
+		dev_info(adapter->dev, "%s\n", buf);
+		p += sprintf(p, "%s\n", buf);
+	}
+
+	sdio_release_host(cardp->func);
+
+	dev_info(adapter->dev, "SDIO register DUMP END\n");
+
+	return p - drv_buf;
+}
+
 static struct mwifiex_if_ops sdio_ops = {
 	.init_if = mwifiex_init_sdio,
 	.cleanup_if = mwifiex_cleanup_sdio,
@@ -2188,6 +2283,7 @@ static struct mwifiex_if_ops sdio_ops = {
 	.card_reset = mwifiex_sdio_card_reset,
 	.iface_work = mwifiex_sdio_work,
 	.fw_dump = mwifiex_sdio_fw_dump,
+	.reg_dump = mwifiex_sdio_reg_dump,
 };
 
 /*
diff --git a/drivers/net/wireless/mwifiex/sdio.h b/drivers/net/wireless/mwifiex/sdio.h
index 54c07156dd78..895eea054c9e 100644
--- a/drivers/net/wireless/mwifiex/sdio.h
+++ b/drivers/net/wireless/mwifiex/sdio.h
@@ -44,6 +44,9 @@
 
 #define MWIFIEX_SDIO_BYTE_MODE_MASK	0x80000000
 
+#define MWIFIEX_MAX_FUNC2_REG_NUM	13
+#define MWIFIEX_SDIO_SCRATCH_SIZE	10
+
 #define SDIO_MPA_ADDR_BASE		0x1000
 #define CTRL_PORT			0
 #define CTRL_PORT_MASK			0x0001
@@ -219,6 +222,11 @@ struct mwifiex_sdio_card_reg {
 	u8 fw_dump_ctrl;
 	u8 fw_dump_start;
 	u8 fw_dump_end;
+	u8 func1_dump_reg_start;
+	u8 func1_dump_reg_end;
+	u8 func1_scratch_reg;
+	u8 func1_spec_reg_num;
+	u8 func1_spec_reg_table[MWIFIEX_MAX_FUNC2_REG_NUM];
 };
 
 struct sdio_mmc_card {
@@ -291,6 +299,11 @@ static const struct mwifiex_sdio_card_reg mwifiex_reg_sd87xx = {
 	.rd_len_p0_l = 0x08,
 	.rd_len_p0_u = 0x09,
 	.card_misc_cfg_reg = 0x6c,
+	.func1_dump_reg_start = 0x0,
+	.func1_dump_reg_end = 0x9,
+	.func1_scratch_reg = 0x60,
+	.func1_spec_reg_num = 5,
+	.func1_spec_reg_table = {0x28, 0x30, 0x34, 0x38, 0x3c},
 };
 
 static const struct mwifiex_sdio_card_reg mwifiex_reg_sd8897 = {
@@ -335,6 +348,12 @@ static const struct mwifiex_sdio_card_reg mwifiex_reg_sd8897 = {
 	.fw_dump_ctrl = 0xe2,
 	.fw_dump_start = 0xe3,
 	.fw_dump_end = 0xea,
+	.func1_dump_reg_start = 0x0,
+	.func1_dump_reg_end = 0xb,
+	.func1_scratch_reg = 0xc0,
+	.func1_spec_reg_num = 8,
+	.func1_spec_reg_table = {0x4C, 0x50, 0x54, 0x55, 0x58,
+				 0x59, 0x5c, 0x5d},
 };
 
 static const struct mwifiex_sdio_card_reg mwifiex_reg_sd8887 = {
@@ -376,6 +395,13 @@ static const struct mwifiex_sdio_card_reg mwifiex_reg_sd8887 = {
 	.cmd_cfg_1 = 0xc5,
 	.cmd_cfg_2 = 0xc6,
 	.cmd_cfg_3 = 0xc7,
+	.func1_dump_reg_start = 0x10,
+	.func1_dump_reg_end = 0x17,
+	.func1_scratch_reg = 0x90,
+	.func1_spec_reg_num = 13,
+	.func1_spec_reg_table = {0x08, 0x58, 0x5C, 0x5D, 0x60,
+				 0x61, 0x62, 0x64, 0x65, 0x66,
+				 0x68, 0x69, 0x6a},
 };
 
 static const struct mwifiex_sdio_device mwifiex_sdio_sd8786 = {
diff --git a/drivers/net/wireless/mwifiex/sta_cmd.c b/drivers/net/wireless/mwifiex/sta_cmd.c
index 1c2ca291d1f5..f7b920d7a95a 100644
--- a/drivers/net/wireless/mwifiex/sta_cmd.c
+++ b/drivers/net/wireless/mwifiex/sta_cmd.c
@@ -26,6 +26,10 @@
 #include "11n.h"
 #include "11ac.h"
 
+static bool disable_auto_ds;
+module_param(disable_auto_ds, bool, 0);
+MODULE_PARM_DESC(disable_auto_ds,
+		 "deepsleep enabled=0(default), deepsleep disabled=1");
 /*
  * This function prepares command to set/get RSSI information.
  *
@@ -2031,7 +2035,8 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta)
 	if (ret)
 		return -1;
 
-	if (first_sta && priv->adapter->iface_type != MWIFIEX_USB &&
+	if (!disable_auto_ds &&
+	    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;
diff --git a/drivers/net/wireless/mwifiex/sta_cmdresp.c b/drivers/net/wireless/mwifiex/sta_cmdresp.c
index b65e1014b0fc..65d10a33eab5 100644
--- a/drivers/net/wireless/mwifiex/sta_cmdresp.c
+++ b/drivers/net/wireless/mwifiex/sta_cmdresp.c
@@ -248,6 +248,8 @@ static int mwifiex_ret_get_log(struct mwifiex_private *priv,
 			le32_to_cpu(get_log->wep_icv_err_cnt[2]);
 		stats->wep_icv_error[3] =
 			le32_to_cpu(get_log->wep_icv_err_cnt[3]);
+		stats->bcn_rcv_cnt = le32_to_cpu(get_log->bcn_rcv_cnt);
+		stats->bcn_miss_cnt = le32_to_cpu(get_log->bcn_miss_cnt);
 	}
 
 	return 0;
diff --git a/drivers/net/wireless/mwifiex/sta_event.c b/drivers/net/wireless/mwifiex/sta_event.c
index b8c171df6223..419e35f1dbf3 100644
--- a/drivers/net/wireless/mwifiex/sta_event.c
+++ b/drivers/net/wireless/mwifiex/sta_event.c
@@ -90,6 +90,10 @@ mwifiex_reset_connect_state(struct mwifiex_private *priv, u16 reason_code)
 	priv->is_data_rate_auto = true;
 	priv->data_rate = 0;
 
+	if ((GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA ||
+	     GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP) && priv->hist_data)
+		mwifiex_hist_data_reset(priv);
+
 	if (priv->bss_mode == NL80211_IFTYPE_ADHOC) {
 		priv->adhoc_state = ADHOC_IDLE;
 		priv->adhoc_is_link_sensed = false;
@@ -308,6 +312,8 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 					adapter->ps_state = PS_STATE_AWAKE;
 					adapter->pm_wakeup_card_req = false;
 					adapter->pm_wakeup_fw_try = false;
+					mod_timer(&adapter->wakeup_timer,
+						  jiffies + (HZ*3));
 					break;
 				}
 				if (!mwifiex_send_null_packet
@@ -322,6 +328,7 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 		adapter->ps_state = PS_STATE_AWAKE;
 		adapter->pm_wakeup_card_req = false;
 		adapter->pm_wakeup_fw_try = false;
+		del_timer_sync(&adapter->wakeup_timer);
 
 		break;
 
diff --git a/drivers/net/wireless/mwifiex/sta_ioctl.c b/drivers/net/wireless/mwifiex/sta_ioctl.c
index 1626868a4b5c..fb9c5fc83e5d 100644
--- a/drivers/net/wireless/mwifiex/sta_ioctl.c
+++ b/drivers/net/wireless/mwifiex/sta_ioctl.c
@@ -902,9 +902,12 @@ static int mwifiex_sec_ioctl_set_wep_key(struct mwifiex_private *priv,
 	if (wep_key->key_length) {
 		void *enc_key;
 
-		if (encrypt_key->key_disable)
+		if (encrypt_key->key_disable) {
 			memset(&priv->wep_key[index], 0,
 			       sizeof(struct mwifiex_wep_key));
+			if (wep_key->key_length)
+				goto done;
+			}
 
 		if (adapter->key_api_major_ver == KEY_API_VER_MAJOR_V2)
 			enc_key = encrypt_key;
@@ -918,6 +921,7 @@ static int mwifiex_sec_ioctl_set_wep_key(struct mwifiex_private *priv,
 			return ret;
 	}
 
+done:
 	if (priv->sec_info.wep_enabled)
 		priv->curr_pkt_filter |= HostCmd_ACT_MAC_WEP_ENABLE;
 	else
diff --git a/drivers/net/wireless/mwifiex/sta_rx.c b/drivers/net/wireless/mwifiex/sta_rx.c
index c2ad3b63ae70..b8729c9394e9 100644
--- a/drivers/net/wireless/mwifiex/sta_rx.c
+++ b/drivers/net/wireless/mwifiex/sta_rx.c
@@ -90,6 +90,7 @@ int mwifiex_process_rx_packet(struct mwifiex_private *priv,
 	struct ethhdr *eth;
 	u16 rx_pkt_off, rx_pkt_len;
 	u8 *offset;
+	u8 adj_rx_rate = 0;
 
 	local_rx_pd = (struct rxpd *) (skb->data);
 
@@ -155,6 +156,14 @@ int mwifiex_process_rx_packet(struct mwifiex_private *priv,
 
 	priv->rxpd_htinfo = local_rx_pd->ht_info;
 
+	if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA ||
+	    GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP) {
+		adj_rx_rate = mwifiex_adjust_data_rate(priv, priv->rxpd_rate,
+						       priv->rxpd_htinfo);
+		mwifiex_hist_data_add(priv, adj_rx_rate, local_rx_pd->snr,
+				      local_rx_pd->nf);
+	}
+
 	ret = mwifiex_recv_packet(priv, skb);
 	if (ret == -1)
 		dev_err(priv->adapter->dev, "recv packet failed\n");
diff --git a/drivers/net/wireless/mwifiex/sta_tx.c b/drivers/net/wireless/mwifiex/sta_tx.c
index b896d7375b52..1debe76017b1 100644
--- a/drivers/net/wireless/mwifiex/sta_tx.c
+++ b/drivers/net/wireless/mwifiex/sta_tx.c
@@ -47,8 +47,10 @@ void *mwifiex_process_sta_txpd(struct mwifiex_private *priv,
 	struct mwifiex_adapter *adapter = priv->adapter;
 	struct txpd *local_tx_pd;
 	struct mwifiex_txinfo *tx_info = MWIFIEX_SKB_TXCB(skb);
-	u8 pad;
+	unsigned int pad;
 	u16 pkt_type, pkt_offset;
+	int hroom = (priv->adapter->iface_type == MWIFIEX_USB) ? 0 :
+		       INTF_HEADER_LEN;
 
 	if (!skb->len) {
 		dev_err(adapter->dev, "Tx: bad packet length: %d\n", skb->len);
@@ -56,13 +58,12 @@ void *mwifiex_process_sta_txpd(struct mwifiex_private *priv,
 		return skb->data;
 	}
 
-	pkt_type = mwifiex_is_skb_mgmt_frame(skb) ? PKT_TYPE_MGMT : 0;
+	BUG_ON(skb_headroom(skb) < MWIFIEX_MIN_DATA_HEADER_LEN);
 
-	/* If skb->data is not aligned; add padding */
-	pad = (4 - (((void *)skb->data - NULL) & 0x3)) % 4;
+	pkt_type = mwifiex_is_skb_mgmt_frame(skb) ? PKT_TYPE_MGMT : 0;
 
-	BUG_ON(skb_headroom(skb) < (sizeof(*local_tx_pd) + INTF_HEADER_LEN
-				    + pad));
+	pad = ((void *)skb->data - (sizeof(*local_tx_pd) + hroom)-
+			 NULL) & (MWIFIEX_DMA_ALIGN_SZ - 1);
 	skb_push(skb, sizeof(*local_tx_pd) + pad);
 
 	local_tx_pd = (struct txpd *) skb->data;
@@ -70,8 +71,8 @@ void *mwifiex_process_sta_txpd(struct mwifiex_private *priv,
 	local_tx_pd->bss_num = priv->bss_num;
 	local_tx_pd->bss_type = priv->bss_type;
 	local_tx_pd->tx_pkt_length = cpu_to_le16((u16)(skb->len -
-						       (sizeof(struct txpd)
-							+ pad)));
+						       (sizeof(struct txpd) +
+							pad)));
 
 	local_tx_pd->priority = (u8) skb->priority;
 	local_tx_pd->pkt_delay_2ms =
@@ -115,7 +116,7 @@ void *mwifiex_process_sta_txpd(struct mwifiex_private *priv,
 	local_tx_pd->tx_pkt_offset = cpu_to_le16(pkt_offset);
 
 	/* make space for INTF_HEADER_LEN */
-	skb_push(skb, INTF_HEADER_LEN);
+	skb_push(skb, hroom);
 
 	if (!local_tx_pd->tx_control)
 		/* TxCtrl set by user or default */
diff --git a/drivers/net/wireless/mwifiex/tdls.c b/drivers/net/wireless/mwifiex/tdls.c
index 22884b429be7..087d84762cd3 100644
--- a/drivers/net/wireless/mwifiex/tdls.c
+++ b/drivers/net/wireless/mwifiex/tdls.c
@@ -1123,6 +1123,36 @@ int mwifiex_get_tdls_link_status(struct mwifiex_private *priv, const u8 *mac)
 	return TDLS_NOT_SETUP;
 }
 
+int mwifiex_get_tdls_list(struct mwifiex_private *priv,
+			  struct tdls_peer_info *buf)
+{
+	struct mwifiex_sta_node *sta_ptr;
+	struct tdls_peer_info *peer = buf;
+	int count = 0;
+	unsigned long flags;
+
+	if (!ISSUPP_TDLS_ENABLED(priv->adapter->fw_cap_info))
+		return 0;
+
+	/* make sure we are in station mode and connected */
+	if (!(priv->bss_type == MWIFIEX_BSS_TYPE_STA && priv->media_connected))
+		return 0;
+
+	spin_lock_irqsave(&priv->sta_list_spinlock, flags);
+	list_for_each_entry(sta_ptr, &priv->sta_list, list) {
+		if (sta_ptr->tdls_status == TDLS_SETUP_COMPLETE) {
+			ether_addr_copy(peer->peer_addr, sta_ptr->mac_addr);
+			peer++;
+			count++;
+			if (count >= MWIFIEX_MAX_TDLS_PEER_SUPPORTED)
+				break;
+		}
+	}
+	spin_unlock_irqrestore(&priv->sta_list_spinlock, flags);
+
+	return count;
+}
+
 void mwifiex_disable_all_tdls_links(struct mwifiex_private *priv)
 {
 	struct mwifiex_sta_node *sta_ptr;
@@ -1367,9 +1397,8 @@ void mwifiex_check_auto_tdls(unsigned long context)
 
 void mwifiex_setup_auto_tdls_timer(struct mwifiex_private *priv)
 {
-	init_timer(&priv->auto_tdls_timer);
-	priv->auto_tdls_timer.function = mwifiex_check_auto_tdls;
-	priv->auto_tdls_timer.data = (unsigned long)priv;
+	setup_timer(&priv->auto_tdls_timer, mwifiex_check_auto_tdls,
+		    (unsigned long)priv);
 	priv->auto_tdls_timer_active = true;
 	mod_timer(&priv->auto_tdls_timer,
 		  jiffies + msecs_to_jiffies(MWIFIEX_TIMER_10S));
diff --git a/drivers/net/wireless/mwifiex/uap_event.c b/drivers/net/wireless/mwifiex/uap_event.c
index 3b3a970e2086..96ff39722f8f 100644
--- a/drivers/net/wireless/mwifiex/uap_event.c
+++ b/drivers/net/wireless/mwifiex/uap_event.c
@@ -131,6 +131,8 @@ int mwifiex_process_uap_event(struct mwifiex_private *priv)
 		dev_dbg(adapter->dev, "AP EVENT: event id: %#x\n", eventcause);
 		memcpy(priv->netdev->dev_addr, adapter->event_body + 2,
 		       ETH_ALEN);
+		if (priv->hist_data)
+			mwifiex_hist_data_reset(priv);
 		break;
 	case EVENT_UAP_MIC_COUNTERMEASURES:
 		/* For future development */
diff --git a/drivers/net/wireless/mwifiex/uap_txrx.c b/drivers/net/wireless/mwifiex/uap_txrx.c
index be3a203a529b..38ac4d74c486 100644
--- a/drivers/net/wireless/mwifiex/uap_txrx.c
+++ b/drivers/net/wireless/mwifiex/uap_txrx.c
@@ -348,8 +348,10 @@ void *mwifiex_process_uap_txpd(struct mwifiex_private *priv,
 	struct mwifiex_adapter *adapter = priv->adapter;
 	struct uap_txpd *txpd;
 	struct mwifiex_txinfo *tx_info = MWIFIEX_SKB_TXCB(skb);
-	int pad, len;
-	u16 pkt_type;
+	int pad;
+	u16 pkt_type, pkt_offset;
+	int hroom = (priv->adapter->iface_type == MWIFIEX_USB) ? 0 :
+		       INTF_HEADER_LEN;
 
 	if (!skb->len) {
 		dev_err(adapter->dev, "Tx: bad packet length: %d\n", skb->len);
@@ -357,22 +359,21 @@ void *mwifiex_process_uap_txpd(struct mwifiex_private *priv,
 		return skb->data;
 	}
 
-	pkt_type = mwifiex_is_skb_mgmt_frame(skb) ? PKT_TYPE_MGMT : 0;
-
-	/* If skb->data is not aligned, add padding */
-	pad = (4 - (((void *)skb->data - NULL) & 0x3)) % 4;
+	BUG_ON(skb_headroom(skb) < MWIFIEX_MIN_DATA_HEADER_LEN);
 
-	len = sizeof(*txpd) + pad;
+	pkt_type = mwifiex_is_skb_mgmt_frame(skb) ? PKT_TYPE_MGMT : 0;
 
-	BUG_ON(skb_headroom(skb) < len + INTF_HEADER_LEN);
+	pad = ((void *)skb->data - (sizeof(*txpd) + hroom) - NULL) &
+			(MWIFIEX_DMA_ALIGN_SZ - 1);
 
-	skb_push(skb, len);
+	skb_push(skb, sizeof(*txpd) + pad);
 
 	txpd = (struct uap_txpd *)skb->data;
 	memset(txpd, 0, sizeof(*txpd));
 	txpd->bss_num = priv->bss_num;
 	txpd->bss_type = priv->bss_type;
-	txpd->tx_pkt_length = cpu_to_le16((u16)(skb->len - len));
+	txpd->tx_pkt_length = cpu_to_le16((u16)(skb->len - (sizeof(*txpd) +
+						pad)));
 	txpd->priority = (u8)skb->priority;
 
 	txpd->pkt_delay_2ms = mwifiex_wmm_compute_drv_pkt_delay(priv, skb);
@@ -392,16 +393,17 @@ void *mwifiex_process_uap_txpd(struct mwifiex_private *priv,
 		    cpu_to_le32(priv->wmm.user_pri_pkt_tx_ctrl[txpd->priority]);
 
 	/* Offset of actual data */
+	pkt_offset = sizeof(*txpd) + pad;
 	if (pkt_type == PKT_TYPE_MGMT) {
 		/* Set the packet type and add header for management frame */
 		txpd->tx_pkt_type = cpu_to_le16(pkt_type);
-		len += MWIFIEX_MGMT_FRAME_HEADER_SIZE;
+		pkt_offset += MWIFIEX_MGMT_FRAME_HEADER_SIZE;
 	}
 
-	txpd->tx_pkt_offset = cpu_to_le16(len);
+	txpd->tx_pkt_offset = cpu_to_le16(pkt_offset);
 
 	/* make space for INTF_HEADER_LEN */
-	skb_push(skb, INTF_HEADER_LEN);
+	skb_push(skb, hroom);
 
 	if (!txpd->tx_control)
 		/* TxCtrl set by user or default */
diff --git a/drivers/net/wireless/mwifiex/usb.c b/drivers/net/wireless/mwifiex/usb.c
index 1b56495ec872..6c62995028e6 100644
--- a/drivers/net/wireless/mwifiex/usb.c
+++ b/drivers/net/wireless/mwifiex/usb.c
@@ -930,7 +930,8 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 	} while ((dnld_cmd != FW_HAS_LAST_BLOCK) && retries);
 
 cleanup:
-	dev_dbg(adapter->dev, "%s: %d bytes downloaded\n", __func__, tlen);
+	dev_notice(adapter->dev,
+		   "info: FW download over, size %d bytes\n", tlen);
 
 	kfree(recv_buff);
 	kfree(fwdata);
@@ -990,6 +991,7 @@ static int mwifiex_pm_wakeup_card(struct mwifiex_adapter *adapter)
 {
 	/* Simulation of HS_AWAKE event */
 	adapter->pm_wakeup_fw_try = false;
+	del_timer_sync(&adapter->wakeup_timer);
 	adapter->pm_wakeup_card_req = false;
 	adapter->ps_state = PS_STATE_AWAKE;
 
@@ -1010,6 +1012,13 @@ static void mwifiex_usb_submit_rem_rx_urbs(struct mwifiex_adapter *adapter)
 	}
 }
 
+/* This function is called after the card has woken up. */
+static inline int
+mwifiex_pm_wakeup_card_complete(struct mwifiex_adapter *adapter)
+{
+	return 0;
+}
+
 static struct mwifiex_if_ops usb_ops = {
 	.register_dev =		mwifiex_register_dev,
 	.unregister_dev =	mwifiex_unregister_dev,
diff --git a/drivers/net/wireless/mwifiex/usb.h b/drivers/net/wireless/mwifiex/usb.h
index a7cbba1355af..0ad1bebc3f93 100644
--- a/drivers/net/wireless/mwifiex/usb.h
+++ b/drivers/net/wireless/mwifiex/usb.h
@@ -96,11 +96,4 @@ struct fw_data {
 	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 b1768fbf98f2..707319799942 100644
--- a/drivers/net/wireless/mwifiex/util.c
+++ b/drivers/net/wireless/mwifiex/util.c
@@ -25,6 +25,96 @@
 #include "wmm.h"
 #include "11n.h"
 
+static struct mwifiex_debug_data items[] = {
+	{"int_counter", item_size(int_counter),
+	 item_addr(int_counter), 1},
+	{"wmm_ac_vo", item_size(packets_out[WMM_AC_VO]),
+	 item_addr(packets_out[WMM_AC_VO]), 1},
+	{"wmm_ac_vi", item_size(packets_out[WMM_AC_VI]),
+	 item_addr(packets_out[WMM_AC_VI]), 1},
+	{"wmm_ac_be", item_size(packets_out[WMM_AC_BE]),
+	 item_addr(packets_out[WMM_AC_BE]), 1},
+	{"wmm_ac_bk", item_size(packets_out[WMM_AC_BK]),
+	 item_addr(packets_out[WMM_AC_BK]), 1},
+	{"tx_buf_size", item_size(tx_buf_size),
+	 item_addr(tx_buf_size), 1},
+	{"curr_tx_buf_size", item_size(curr_tx_buf_size),
+	 item_addr(curr_tx_buf_size), 1},
+	{"ps_mode", item_size(ps_mode),
+	 item_addr(ps_mode), 1},
+	{"ps_state", item_size(ps_state),
+	 item_addr(ps_state), 1},
+	{"is_deep_sleep", item_size(is_deep_sleep),
+	 item_addr(is_deep_sleep), 1},
+	{"wakeup_dev_req", item_size(pm_wakeup_card_req),
+	 item_addr(pm_wakeup_card_req), 1},
+	{"wakeup_tries", item_size(pm_wakeup_fw_try),
+	 item_addr(pm_wakeup_fw_try), 1},
+	{"hs_configured", item_size(is_hs_configured),
+	 item_addr(is_hs_configured), 1},
+	{"hs_activated", item_size(hs_activated),
+	 item_addr(hs_activated), 1},
+	{"num_tx_timeout", item_size(num_tx_timeout),
+	 item_addr(num_tx_timeout), 1},
+	{"is_cmd_timedout", item_size(is_cmd_timedout),
+	 item_addr(is_cmd_timedout), 1},
+	{"timeout_cmd_id", item_size(timeout_cmd_id),
+	 item_addr(timeout_cmd_id), 1},
+	{"timeout_cmd_act", item_size(timeout_cmd_act),
+	 item_addr(timeout_cmd_act), 1},
+	{"last_cmd_id", item_size(last_cmd_id),
+	 item_addr(last_cmd_id), DBG_CMD_NUM},
+	{"last_cmd_act", item_size(last_cmd_act),
+	 item_addr(last_cmd_act), DBG_CMD_NUM},
+	{"last_cmd_index", item_size(last_cmd_index),
+	 item_addr(last_cmd_index), 1},
+	{"last_cmd_resp_id", item_size(last_cmd_resp_id),
+	 item_addr(last_cmd_resp_id), DBG_CMD_NUM},
+	{"last_cmd_resp_index", item_size(last_cmd_resp_index),
+	 item_addr(last_cmd_resp_index), 1},
+	{"last_event", item_size(last_event),
+	 item_addr(last_event), DBG_CMD_NUM},
+	{"last_event_index", item_size(last_event_index),
+	 item_addr(last_event_index), 1},
+	{"num_cmd_h2c_fail", item_size(num_cmd_host_to_card_failure),
+	 item_addr(num_cmd_host_to_card_failure), 1},
+	{"num_cmd_sleep_cfm_fail",
+	 item_size(num_cmd_sleep_cfm_host_to_card_failure),
+	 item_addr(num_cmd_sleep_cfm_host_to_card_failure), 1},
+	{"num_tx_h2c_fail", item_size(num_tx_host_to_card_failure),
+	 item_addr(num_tx_host_to_card_failure), 1},
+	{"num_evt_deauth", item_size(num_event_deauth),
+	 item_addr(num_event_deauth), 1},
+	{"num_evt_disassoc", item_size(num_event_disassoc),
+	 item_addr(num_event_disassoc), 1},
+	{"num_evt_link_lost", item_size(num_event_link_lost),
+	 item_addr(num_event_link_lost), 1},
+	{"num_cmd_deauth", item_size(num_cmd_deauth),
+	 item_addr(num_cmd_deauth), 1},
+	{"num_cmd_assoc_ok", item_size(num_cmd_assoc_success),
+	 item_addr(num_cmd_assoc_success), 1},
+	{"num_cmd_assoc_fail", item_size(num_cmd_assoc_failure),
+	 item_addr(num_cmd_assoc_failure), 1},
+	{"cmd_sent", item_size(cmd_sent),
+	 item_addr(cmd_sent), 1},
+	{"data_sent", item_size(data_sent),
+	 item_addr(data_sent), 1},
+	{"cmd_resp_received", item_size(cmd_resp_received),
+	 item_addr(cmd_resp_received), 1},
+	{"event_received", item_size(event_received),
+	 item_addr(event_received), 1},
+
+	/* variables defined in struct mwifiex_adapter */
+	{"cmd_pending", adapter_item_size(cmd_pending),
+	 adapter_item_addr(cmd_pending), 1},
+	{"tx_pending", adapter_item_size(tx_pending),
+	 adapter_item_addr(tx_pending), 1},
+	{"rx_pending", adapter_item_size(rx_pending),
+	 adapter_item_addr(rx_pending), 1},
+};
+
+static int num_of_items = ARRAY_SIZE(items);
+
 /*
  * Firmware initialization complete callback handler.
  *
@@ -97,6 +187,8 @@ int mwifiex_get_debug_info(struct mwifiex_private *priv,
 							      info->rx_tbl);
 		info->tx_tbl_num = mwifiex_get_tx_ba_stream_tbl(priv,
 								info->tx_tbl);
+		info->tdls_peer_num = mwifiex_get_tdls_list(priv,
+							    info->tdls_list);
 		info->ps_mode = adapter->ps_mode;
 		info->ps_state = adapter->ps_state;
 		info->is_deep_sleep = adapter->is_deep_sleep;
@@ -141,6 +233,93 @@ int mwifiex_get_debug_info(struct mwifiex_private *priv,
 	return 0;
 }
 
+int mwifiex_debug_info_to_buffer(struct mwifiex_private *priv, char *buf,
+				 struct mwifiex_debug_info *info)
+{
+	char *p = buf;
+	struct mwifiex_debug_data *d = &items[0];
+	size_t size, addr;
+	long val;
+	int i, j;
+
+	if (!info)
+		return 0;
+
+	for (i = 0; i < num_of_items; i++) {
+		p += sprintf(p, "%s=", d[i].name);
+
+		size = d[i].size / d[i].num;
+
+		if (i < (num_of_items - 3))
+			addr = d[i].addr + (size_t)info;
+		else /* The last 3 items are struct mwifiex_adapter variables */
+			addr = d[i].addr + (size_t)priv->adapter;
+
+		for (j = 0; j < d[i].num; j++) {
+			switch (size) {
+			case 1:
+				val = *((u8 *)addr);
+				break;
+			case 2:
+				val = *((u16 *)addr);
+				break;
+			case 4:
+				val = *((u32 *)addr);
+				break;
+			case 8:
+				val = *((long long *)addr);
+				break;
+			default:
+				val = -1;
+				break;
+			}
+
+			p += sprintf(p, "%#lx ", val);
+			addr += size;
+		}
+
+		p += sprintf(p, "\n");
+	}
+
+	if (info->tx_tbl_num) {
+		p += sprintf(p, "Tx BA stream table:\n");
+		for (i = 0; i < info->tx_tbl_num; i++)
+			p += sprintf(p, "tid = %d, ra = %pM\n",
+				     info->tx_tbl[i].tid, info->tx_tbl[i].ra);
+	}
+
+	if (info->rx_tbl_num) {
+		p += sprintf(p, "Rx reorder table:\n");
+		for (i = 0; i < info->rx_tbl_num; i++) {
+			p += sprintf(p, "tid = %d, ta = %pM, ",
+				     info->rx_tbl[i].tid,
+				     info->rx_tbl[i].ta);
+			p += sprintf(p, "start_win = %d, ",
+				     info->rx_tbl[i].start_win);
+			p += sprintf(p, "win_size = %d, buffer: ",
+				     info->rx_tbl[i].win_size);
+
+			for (j = 0; j < info->rx_tbl[i].win_size; j++)
+				p += sprintf(p, "%c ",
+					     info->rx_tbl[i].buffer[j] ?
+					     '1' : '0');
+
+			p += sprintf(p, "\n");
+		}
+	}
+
+	if (info->tdls_peer_num) {
+		p += sprintf(p, "TDLS peer table:\n");
+		for (i = 0; i < info->tdls_peer_num; i++) {
+			p += sprintf(p, "peer = %pM",
+				     info->tdls_list[i].peer_addr);
+			p += sprintf(p, "\n");
+		}
+	}
+
+	return p - buf;
+}
+
 static int
 mwifiex_parse_mgmt_packet(struct mwifiex_private *priv, u8 *payload, u16 len,
 			  struct rxpd *rx_pd)
@@ -404,3 +583,44 @@ void mwifiex_del_all_sta_list(struct mwifiex_private *priv)
 	spin_unlock_irqrestore(&priv->sta_list_spinlock, flags);
 	return;
 }
+
+/* This function adds histogram data to histogram array*/
+void mwifiex_hist_data_add(struct mwifiex_private *priv,
+			   u8 rx_rate, s8 snr, s8 nflr)
+{
+	struct mwifiex_histogram_data *phist_data = priv->hist_data;
+
+	if (atomic_read(&phist_data->num_samples) > MWIFIEX_HIST_MAX_SAMPLES)
+		mwifiex_hist_data_reset(priv);
+	mwifiex_hist_data_set(priv, rx_rate, snr, nflr);
+}
+
+/* function to add histogram record */
+void mwifiex_hist_data_set(struct mwifiex_private *priv, u8 rx_rate, s8 snr,
+			   s8 nflr)
+{
+	struct mwifiex_histogram_data *phist_data = priv->hist_data;
+
+	atomic_inc(&phist_data->num_samples);
+	atomic_inc(&phist_data->rx_rate[rx_rate]);
+	atomic_inc(&phist_data->snr[snr]);
+	atomic_inc(&phist_data->noise_flr[128 + nflr]);
+	atomic_inc(&phist_data->sig_str[nflr - snr]);
+}
+
+/* function to reset histogram data during init/reset */
+void mwifiex_hist_data_reset(struct mwifiex_private *priv)
+{
+	int ix;
+	struct mwifiex_histogram_data *phist_data = priv->hist_data;
+
+	atomic_set(&phist_data->num_samples, 0);
+	for (ix = 0; ix < MWIFIEX_MAX_AC_RX_RATES; ix++)
+		atomic_set(&phist_data->rx_rate[ix], 0);
+	for (ix = 0; ix < MWIFIEX_MAX_SNR; ix++)
+		atomic_set(&phist_data->snr[ix], 0);
+	for (ix = 0; ix < MWIFIEX_MAX_NOISE_FLR; ix++)
+		atomic_set(&phist_data->noise_flr[ix], 0);
+	for (ix = 0; ix < MWIFIEX_MAX_SIG_STRENGTH; ix++)
+		atomic_set(&phist_data->sig_str[ix], 0);
+}
diff --git a/drivers/net/wireless/mwifiex/util.h b/drivers/net/wireless/mwifiex/util.h
index 40296cb4a3f1..b541d66c01eb 100644
--- a/drivers/net/wireless/mwifiex/util.h
+++ b/drivers/net/wireless/mwifiex/util.h
@@ -20,6 +20,8 @@
 #ifndef _MWIFIEX_UTIL_H_
 #define _MWIFIEX_UTIL_H_
 
+struct mwifiex_private;
+
 struct mwifiex_dma_mapping {
 	dma_addr_t addr;
 	size_t len;
@@ -33,6 +35,21 @@ struct mwifiex_cb {
 	};
 };
 
+/* size/addr for mwifiex_debug_info */
+#define item_size(n)		(FIELD_SIZEOF(struct mwifiex_debug_info, n))
+#define item_addr(n)		(offsetof(struct mwifiex_debug_info, n))
+
+/* size/addr for struct mwifiex_adapter */
+#define adapter_item_size(n)	(FIELD_SIZEOF(struct mwifiex_adapter, n))
+#define adapter_item_addr(n)	(offsetof(struct mwifiex_adapter, n))
+
+struct mwifiex_debug_data {
+	char name[32];		/* variable/array name */
+	u32 size;		/* size of the variable/array */
+	size_t addr;		/* address of the variable/array */
+	int num;		/* number of variables in an array */
+};
+
 static inline struct mwifiex_rxinfo *MWIFIEX_SKB_RXCB(struct sk_buff *skb)
 {
 	struct mwifiex_cb *cb = (struct mwifiex_cb *)skb->cb;
@@ -73,4 +90,7 @@ static inline dma_addr_t MWIFIEX_SKB_DMA_ADDR(struct sk_buff *skb)
 	return mapping.addr;
 }
 
+int mwifiex_debug_info_to_buffer(struct mwifiex_private *priv, char *buf,
+				 struct mwifiex_debug_info *info);
+
 #endif /* !_MWIFIEX_UTIL_H_ */
diff --git a/drivers/net/wireless/orinoco/Kconfig b/drivers/net/wireless/orinoco/Kconfig
index 60698b020851..6d831d4d1b5f 100644
--- a/drivers/net/wireless/orinoco/Kconfig
+++ b/drivers/net/wireless/orinoco/Kconfig
@@ -1,7 +1,8 @@
 config HERMES
 	tristate "Hermes chipset 802.11b support (Orinoco/Prism2/Symbol)"
 	depends on (PPC_PMAC || PCI || PCMCIA)
-	depends on CFG80211 && CFG80211_WEXT
+	depends on CFG80211
+	select CFG80211_WEXT
 	select WIRELESS_EXT
 	select WEXT_SPY
 	select WEXT_PRIV
diff --git a/drivers/net/wireless/orinoco/orinoco_usb.c b/drivers/net/wireless/orinoco/orinoco_usb.c
index 995846422dc0..91f05442de28 100644
--- a/drivers/net/wireless/orinoco/orinoco_usb.c
+++ b/drivers/net/wireless/orinoco/orinoco_usb.c
@@ -364,9 +364,7 @@ static struct request_context *ezusb_alloc_ctx(struct ezusb_priv *upriv,
 	atomic_set(&ctx->refcount, 1);
 	init_completion(&ctx->done);
 
-	init_timer(&ctx->timer);
-	ctx->timer.function = ezusb_request_timerfn;
-	ctx->timer.data = (u_long) ctx;
+	setup_timer(&ctx->timer, ezusb_request_timerfn, (u_long)ctx);
 	return ctx;
 }
 
diff --git a/drivers/net/wireless/rsi/rsi_91x_sdio_ops.c b/drivers/net/wireless/rsi/rsi_91x_sdio_ops.c
index 4834a9abc171..b6cc9ff47fc2 100644
--- a/drivers/net/wireless/rsi/rsi_91x_sdio_ops.c
+++ b/drivers/net/wireless/rsi/rsi_91x_sdio_ops.c
@@ -172,7 +172,6 @@ static int rsi_load_ta_instructions(struct rsi_common *common)
 		(struct rsi_91x_sdiodev *)adapter->rsi_dev;
 	u32 len;
 	u32 num_blocks;
-	const u8 *fw;
 	const struct firmware *fw_entry = NULL;
 	u32 block_size = dev->tx_blk_size;
 	int status = 0;
@@ -201,7 +200,6 @@ static int rsi_load_ta_instructions(struct rsi_common *common)
 		return status;
 	}
 
-	fw = kmemdup(fw_entry->data, fw_entry->size, GFP_KERNEL);
 	len = fw_entry->size;
 
 	if (len % 4)
@@ -212,7 +210,7 @@ static int rsi_load_ta_instructions(struct rsi_common *common)
 	rsi_dbg(INIT_ZONE, "%s: Instruction size:%d\n", __func__, len);
 	rsi_dbg(INIT_ZONE, "%s: num blocks: %d\n", __func__, num_blocks);
 
-	status = rsi_copy_to_card(common, fw, len, num_blocks);
+	status = rsi_copy_to_card(common, fw_entry->data, len, num_blocks);
 	release_firmware(fw_entry);
 	return status;
 }
diff --git a/drivers/net/wireless/rt2x00/rt2800soc.c b/drivers/net/wireless/rt2x00/rt2800soc.c
index f6d1bf5be006..aaa7aa4cad9d 100644
--- a/drivers/net/wireless/rt2x00/rt2800soc.c
+++ b/drivers/net/wireless/rt2x00/rt2800soc.c
@@ -244,7 +244,6 @@ static int rt2800soc_probe(struct platform_device *pdev)
 static struct platform_driver rt2800soc_driver = {
 	.driver		= {
 		.name		= "rt2800_wmac",
-		.owner		= THIS_MODULE,
 		.mod_name	= KBUILD_MODNAME,
 	},
 	.probe		= rt2800soc_probe,
diff --git a/drivers/net/wireless/rt2x00/rt2x00config.c b/drivers/net/wireless/rt2x00/rt2x00config.c
index 1122dc44c9fd..48a2cad29477 100644
--- a/drivers/net/wireless/rt2x00/rt2x00config.c
+++ b/drivers/net/wireless/rt2x00/rt2x00config.c
@@ -240,7 +240,7 @@ void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
 		rt2x00dev->rf_channel = libconf.rf.channel;
 	}
 
-	if (test_bit(REQUIRE_PS_AUTOWAKE, &rt2x00dev->cap_flags) &&
+	if (rt2x00_has_cap_flag(rt2x00dev, REQUIRE_PS_AUTOWAKE) &&
 	    (ieee80211_flags & IEEE80211_CONF_CHANGE_PS))
 		cancel_delayed_work_sync(&rt2x00dev->autowakeup_work);
 
@@ -257,7 +257,7 @@ void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
 		rt2x00link_reset_tuner(rt2x00dev, false);
 
 	if (test_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags) &&
-	    test_bit(REQUIRE_PS_AUTOWAKE, &rt2x00dev->cap_flags) &&
+	    rt2x00_has_cap_flag(rt2x00dev, REQUIRE_PS_AUTOWAKE) &&
 	    (ieee80211_flags & IEEE80211_CONF_CHANGE_PS) &&
 	    (conf->flags & IEEE80211_CONF_PS)) {
 		beacon_diff = (long)jiffies - (long)rt2x00dev->last_beacon;
diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/rt2x00/rt2x00dev.c
index 9967a1d9f0ec..5639ed816813 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dev.c
+++ b/drivers/net/wireless/rt2x00/rt2x00dev.c
@@ -351,7 +351,7 @@ void rt2x00lib_txdone(struct queue_entry *entry,
 	/*
 	 * Remove L2 padding which was added during
 	 */
-	if (test_bit(REQUIRE_L2PAD, &rt2x00dev->cap_flags))
+	if (rt2x00_has_cap_flag(rt2x00dev, REQUIRE_L2PAD))
 		rt2x00queue_remove_l2pad(entry->skb, header_length);
 
 	/*
@@ -460,7 +460,7 @@ void rt2x00lib_txdone(struct queue_entry *entry,
 	 * send the status report back.
 	 */
 	if (!(skbdesc_flags & SKBDESC_NOT_MAC80211)) {
-		if (test_bit(REQUIRE_TASKLET_CONTEXT, &rt2x00dev->cap_flags))
+		if (rt2x00_has_cap_flag(rt2x00dev, REQUIRE_TASKLET_CONTEXT))
 			ieee80211_tx_status(rt2x00dev->hw, entry->skb);
 		else
 			ieee80211_tx_status_ni(rt2x00dev->hw, entry->skb);
@@ -1056,9 +1056,9 @@ static int rt2x00lib_probe_hw(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Take TX headroom required for alignment into account.
 	 */
-	if (test_bit(REQUIRE_L2PAD, &rt2x00dev->cap_flags))
+	if (rt2x00_has_cap_flag(rt2x00dev, REQUIRE_L2PAD))
 		rt2x00dev->hw->extra_tx_headroom += RT2X00_L2PAD_SIZE;
-	else if (test_bit(REQUIRE_DMA, &rt2x00dev->cap_flags))
+	else if (rt2x00_has_cap_flag(rt2x00dev, REQUIRE_DMA))
 		rt2x00dev->hw->extra_tx_headroom += RT2X00_ALIGN_SIZE;
 
 	/*
@@ -1069,7 +1069,7 @@ static int rt2x00lib_probe_hw(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Allocate tx status FIFO for driver use.
 	 */
-	if (test_bit(REQUIRE_TXSTATUS_FIFO, &rt2x00dev->cap_flags)) {
+	if (rt2x00_has_cap_flag(rt2x00dev, REQUIRE_TXSTATUS_FIFO)) {
 		/*
 		 * Allocate the txstatus fifo. In the worst case the tx
 		 * status fifo has to hold the tx status of all entries
@@ -1131,7 +1131,7 @@ static void rt2x00lib_uninitialize(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Stop rfkill polling.
 	 */
-	if (test_bit(REQUIRE_DELAYED_RFKILL, &rt2x00dev->cap_flags))
+	if (rt2x00_has_cap_flag(rt2x00dev, REQUIRE_DELAYED_RFKILL))
 		rt2x00rfkill_unregister(rt2x00dev);
 
 	/*
@@ -1173,7 +1173,7 @@ static int rt2x00lib_initialize(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Start rfkill polling.
 	 */
-	if (test_bit(REQUIRE_DELAYED_RFKILL, &rt2x00dev->cap_flags))
+	if (rt2x00_has_cap_flag(rt2x00dev, REQUIRE_DELAYED_RFKILL))
 		rt2x00rfkill_register(rt2x00dev);
 
 	return 0;
@@ -1389,7 +1389,7 @@ int rt2x00lib_probe_dev(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Start rfkill polling.
 	 */
-	if (!test_bit(REQUIRE_DELAYED_RFKILL, &rt2x00dev->cap_flags))
+	if (!rt2x00_has_cap_flag(rt2x00dev, REQUIRE_DELAYED_RFKILL))
 		rt2x00rfkill_register(rt2x00dev);
 
 	return 0;
@@ -1408,7 +1408,7 @@ void rt2x00lib_remove_dev(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Stop rfkill polling.
 	 */
-	if (!test_bit(REQUIRE_DELAYED_RFKILL, &rt2x00dev->cap_flags))
+	if (!rt2x00_has_cap_flag(rt2x00dev, REQUIRE_DELAYED_RFKILL))
 		rt2x00rfkill_unregister(rt2x00dev);
 
 	/*
diff --git a/drivers/net/wireless/rt2x00/rt2x00firmware.c b/drivers/net/wireless/rt2x00/rt2x00firmware.c
index fbae2799e3ee..5813300f68a2 100644
--- a/drivers/net/wireless/rt2x00/rt2x00firmware.c
+++ b/drivers/net/wireless/rt2x00/rt2x00firmware.c
@@ -96,7 +96,7 @@ int rt2x00lib_load_firmware(struct rt2x00_dev *rt2x00dev)
 {
 	int retval;
 
-	if (!test_bit(REQUIRE_FIRMWARE, &rt2x00dev->cap_flags))
+	if (!rt2x00_has_cap_flag(rt2x00dev, REQUIRE_FIRMWARE))
 		return 0;
 
 	if (!rt2x00dev->fw) {
diff --git a/drivers/net/wireless/rt2x00/rt2x00mac.c b/drivers/net/wireless/rt2x00/rt2x00mac.c
index cb40245a0695..300876df056f 100644
--- a/drivers/net/wireless/rt2x00/rt2x00mac.c
+++ b/drivers/net/wireless/rt2x00/rt2x00mac.c
@@ -119,7 +119,7 @@ void rt2x00mac_tx(struct ieee80211_hw *hw,
 	 * Use the ATIM queue if appropriate and present.
 	 */
 	if (tx_info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM &&
-	    test_bit(REQUIRE_ATIM_QUEUE, &rt2x00dev->cap_flags))
+	    rt2x00_has_cap_flag(rt2x00dev, REQUIRE_ATIM_QUEUE))
 		qid = QID_ATIM;
 
 	queue = rt2x00queue_get_tx_queue(rt2x00dev, qid);
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/rt2x00/rt2x00queue.c
index 66ff36447b94..68b620b2462f 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.c
@@ -85,7 +85,7 @@ struct sk_buff *rt2x00queue_alloc_rxskb(struct queue_entry *entry, gfp_t gfp)
 	memset(skbdesc, 0, sizeof(*skbdesc));
 	skbdesc->entry = entry;
 
-	if (test_bit(REQUIRE_DMA, &rt2x00dev->cap_flags)) {
+	if (rt2x00_has_cap_flag(rt2x00dev, REQUIRE_DMA)) {
 		dma_addr_t skb_dma;
 
 		skb_dma = dma_map_single(rt2x00dev->dev, skb->data, skb->len,
@@ -198,7 +198,7 @@ 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)) {
+	if (!rt2x00_has_cap_flag(rt2x00dev, REQUIRE_SW_SEQNO)) {
 		/*
 		 * rt2800 has a H/W (or F/W) bug, device incorrectly increase
 		 * seqno on retransmited data (non-QOS) frames. To workaround
@@ -484,7 +484,7 @@ static void rt2x00queue_create_tx_descriptor(struct rt2x00_dev *rt2x00dev,
 	rt2x00crypto_create_tx_descriptor(rt2x00dev, skb, txdesc);
 	rt2x00queue_create_tx_descriptor_seq(rt2x00dev, skb, txdesc);
 
-	if (test_bit(REQUIRE_HT_TX_DESC, &rt2x00dev->cap_flags))
+	if (rt2x00_has_cap_flag(rt2x00dev, REQUIRE_HT_TX_DESC))
 		rt2x00queue_create_tx_descriptor_ht(rt2x00dev, skb, txdesc,
 						   sta, hwrate);
 	else
@@ -526,7 +526,7 @@ static int rt2x00queue_write_tx_data(struct queue_entry *entry,
 	/*
 	 * Map the skb to DMA.
 	 */
-	if (test_bit(REQUIRE_DMA, &rt2x00dev->cap_flags) &&
+	if (rt2x00_has_cap_flag(rt2x00dev, REQUIRE_DMA) &&
 	    rt2x00queue_map_txskb(entry))
 		return -ENOMEM;
 
@@ -646,7 +646,7 @@ int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb,
 	 */
 	if (test_bit(ENTRY_TXD_ENCRYPT, &txdesc.flags) &&
 	    !test_bit(ENTRY_TXD_ENCRYPT_IV, &txdesc.flags)) {
-		if (test_bit(REQUIRE_COPY_IV, &queue->rt2x00dev->cap_flags))
+		if (rt2x00_has_cap_flag(queue->rt2x00dev, REQUIRE_COPY_IV))
 			rt2x00crypto_tx_copy_iv(skb, &txdesc);
 		else
 			rt2x00crypto_tx_remove_iv(skb, &txdesc);
@@ -660,9 +660,9 @@ int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb,
 	 * PCI and USB devices, while header alignment only is valid
 	 * for PCI devices.
 	 */
-	if (test_bit(REQUIRE_L2PAD, &queue->rt2x00dev->cap_flags))
+	if (rt2x00_has_cap_flag(queue->rt2x00dev, REQUIRE_L2PAD))
 		rt2x00queue_insert_l2pad(skb, txdesc.header_length);
-	else if (test_bit(REQUIRE_DMA, &queue->rt2x00dev->cap_flags))
+	else if (rt2x00_has_cap_flag(queue->rt2x00dev, REQUIRE_DMA))
 		rt2x00queue_align_frame(skb);
 
 	/*
@@ -1178,7 +1178,7 @@ int rt2x00queue_initialize(struct rt2x00_dev *rt2x00dev)
 	if (status)
 		goto exit;
 
-	if (test_bit(REQUIRE_ATIM_QUEUE, &rt2x00dev->cap_flags)) {
+	if (rt2x00_has_cap_flag(rt2x00dev, REQUIRE_ATIM_QUEUE)) {
 		status = rt2x00queue_alloc_entries(rt2x00dev->atim);
 		if (status)
 			goto exit;
@@ -1234,7 +1234,7 @@ int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
 	struct data_queue *queue;
 	enum data_queue_qid qid;
 	unsigned int req_atim =
-	    !!test_bit(REQUIRE_ATIM_QUEUE, &rt2x00dev->cap_flags);
+	    rt2x00_has_cap_flag(rt2x00dev, REQUIRE_ATIM_QUEUE);
 
 	/*
 	 * We need the following queues:
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.c b/drivers/net/wireless/rt2x00/rt2x00usb.c
index 892270dd3e7b..7627af6098eb 100644
--- a/drivers/net/wireless/rt2x00/rt2x00usb.c
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.c
@@ -274,7 +274,7 @@ static void rt2x00usb_interrupt_txdone(struct urb *urb)
 	 * Schedule the delayed work for reading the TX status
 	 * from the device.
 	 */
-	if (!test_bit(REQUIRE_TXSTATUS_FIFO, &rt2x00dev->cap_flags) ||
+	if (!rt2x00_has_cap_flag(rt2x00dev, REQUIRE_TXSTATUS_FIFO) ||
 	    !kfifo_is_empty(&rt2x00dev->txstatus_fifo))
 		queue_work(rt2x00dev->workqueue, &rt2x00dev->txdone_work);
 }
@@ -456,7 +456,7 @@ static bool rt2x00usb_flush_entry(struct queue_entry *entry, void *data)
 	 * Kill guardian urb (if required by driver).
 	 */
 	if ((entry->queue->qid == QID_BEACON) &&
-	    (test_bit(REQUIRE_BEACON_GUARD, &rt2x00dev->cap_flags)))
+	    (rt2x00_has_cap_flag(rt2x00dev, REQUIRE_BEACON_GUARD)))
 		usb_kill_urb(bcn_priv->guardian_urb);
 
 	return false;
@@ -655,7 +655,7 @@ static int rt2x00usb_alloc_entries(struct data_queue *queue)
 	 * then we are done.
 	 */
 	if (queue->qid != QID_BEACON ||
-	    !test_bit(REQUIRE_BEACON_GUARD, &rt2x00dev->cap_flags))
+	    !rt2x00_has_cap_flag(rt2x00dev, REQUIRE_BEACON_GUARD))
 		return 0;
 
 	for (i = 0; i < queue->limit; i++) {
@@ -690,7 +690,7 @@ static void rt2x00usb_free_entries(struct data_queue *queue)
 	 * then we are done.
 	 */
 	if (queue->qid != QID_BEACON ||
-	    !test_bit(REQUIRE_BEACON_GUARD, &rt2x00dev->cap_flags))
+	    !rt2x00_has_cap_flag(rt2x00dev, REQUIRE_BEACON_GUARD))
 		return;
 
 	for (i = 0; i < queue->limit; i++) {
diff --git a/drivers/net/wireless/rtlwifi/base.c b/drivers/net/wireless/rtlwifi/base.c
index 40b6d1d006d7..1d4677460711 100644
--- a/drivers/net/wireless/rtlwifi/base.c
+++ b/drivers/net/wireless/rtlwifi/base.c
@@ -867,63 +867,135 @@ static u8 _rtl_get_highest_n_rate(struct ieee80211_hw *hw,
  *
  * B/G rate:
  * (rx_status->flag & RX_FLAG_HT) = 0,
- * DESC92_RATE1M-->DESC92_RATE54M ==> idx is 0-->11,
+ * DESC_RATE1M-->DESC_RATE54M ==> idx is 0-->11,
  *
  * N rate:
  * (rx_status->flag & RX_FLAG_HT) = 1,
- * DESC92_RATEMCS0-->DESC92_RATEMCS15 ==> idx is 0-->15
+ * DESC_RATEMCS0-->DESC_RATEMCS15 ==> idx is 0-->15
  *
  * 5G band:rx_status->band == IEEE80211_BAND_5GHZ
  * A rate:
  * (rx_status->flag & RX_FLAG_HT) = 0,
- * DESC92_RATE6M-->DESC92_RATE54M ==> idx is 0-->7,
+ * DESC_RATE6M-->DESC_RATE54M ==> idx is 0-->7,
  *
  * N rate:
  * (rx_status->flag & RX_FLAG_HT) = 1,
- * DESC92_RATEMCS0-->DESC92_RATEMCS15 ==> idx is 0-->15
+ * DESC_RATEMCS0-->DESC_RATEMCS15 ==> idx is 0-->15
+ *
+ * VHT rates:
+ * DESC_RATEVHT1SS_MCS0-->DESC_RATEVHT1SS_MCS9 ==> idx is 0-->9
+ * DESC_RATEVHT2SS_MCS0-->DESC_RATEVHT2SS_MCS9 ==> idx is 0-->9
  */
-int rtlwifi_rate_mapping(struct ieee80211_hw *hw,
-			 bool isht, u8 desc_rate, bool first_ampdu)
+int rtlwifi_rate_mapping(struct ieee80211_hw *hw, bool isht, bool isvht,
+			 u8 desc_rate)
 {
 	int rate_idx;
 
+	if (isvht) {
+		switch (desc_rate) {
+		case DESC_RATEVHT1SS_MCS0:
+			rate_idx = 0;
+			break;
+		case DESC_RATEVHT1SS_MCS1:
+			rate_idx = 1;
+			break;
+		case DESC_RATEVHT1SS_MCS2:
+			rate_idx = 2;
+			break;
+		case DESC_RATEVHT1SS_MCS3:
+			rate_idx = 3;
+			break;
+		case DESC_RATEVHT1SS_MCS4:
+			rate_idx = 4;
+			break;
+		case DESC_RATEVHT1SS_MCS5:
+			rate_idx = 5;
+			break;
+		case DESC_RATEVHT1SS_MCS6:
+			rate_idx = 6;
+			break;
+		case DESC_RATEVHT1SS_MCS7:
+			rate_idx = 7;
+			break;
+		case DESC_RATEVHT1SS_MCS8:
+			rate_idx = 8;
+			break;
+		case DESC_RATEVHT1SS_MCS9:
+			rate_idx = 9;
+			break;
+		case DESC_RATEVHT2SS_MCS0:
+			rate_idx = 0;
+			break;
+		case DESC_RATEVHT2SS_MCS1:
+			rate_idx = 1;
+			break;
+		case DESC_RATEVHT2SS_MCS2:
+			rate_idx = 2;
+			break;
+		case DESC_RATEVHT2SS_MCS3:
+			rate_idx = 3;
+			break;
+		case DESC_RATEVHT2SS_MCS4:
+			rate_idx = 4;
+			break;
+		case DESC_RATEVHT2SS_MCS5:
+			rate_idx = 5;
+			break;
+		case DESC_RATEVHT2SS_MCS6:
+			rate_idx = 6;
+			break;
+		case DESC_RATEVHT2SS_MCS7:
+			rate_idx = 7;
+			break;
+		case DESC_RATEVHT2SS_MCS8:
+			rate_idx = 8;
+			break;
+		case DESC_RATEVHT2SS_MCS9:
+			rate_idx = 9;
+			break;
+		default:
+			rate_idx = 0;
+			break;
+		}
+		return rate_idx;
+	}
 	if (false == isht) {
 		if (IEEE80211_BAND_2GHZ == hw->conf.chandef.chan->band) {
 			switch (desc_rate) {
-			case DESC92_RATE1M:
+			case DESC_RATE1M:
 				rate_idx = 0;
 				break;
-			case DESC92_RATE2M:
+			case DESC_RATE2M:
 				rate_idx = 1;
 				break;
-			case DESC92_RATE5_5M:
+			case DESC_RATE5_5M:
 				rate_idx = 2;
 				break;
-			case DESC92_RATE11M:
+			case DESC_RATE11M:
 				rate_idx = 3;
 				break;
-			case DESC92_RATE6M:
+			case DESC_RATE6M:
 				rate_idx = 4;
 				break;
-			case DESC92_RATE9M:
+			case DESC_RATE9M:
 				rate_idx = 5;
 				break;
-			case DESC92_RATE12M:
+			case DESC_RATE12M:
 				rate_idx = 6;
 				break;
-			case DESC92_RATE18M:
+			case DESC_RATE18M:
 				rate_idx = 7;
 				break;
-			case DESC92_RATE24M:
+			case DESC_RATE24M:
 				rate_idx = 8;
 				break;
-			case DESC92_RATE36M:
+			case DESC_RATE36M:
 				rate_idx = 9;
 				break;
-			case DESC92_RATE48M:
+			case DESC_RATE48M:
 				rate_idx = 10;
 				break;
-			case DESC92_RATE54M:
+			case DESC_RATE54M:
 				rate_idx = 11;
 				break;
 			default:
@@ -932,28 +1004,28 @@ int rtlwifi_rate_mapping(struct ieee80211_hw *hw,
 			}
 		} else {
 			switch (desc_rate) {
-			case DESC92_RATE6M:
+			case DESC_RATE6M:
 				rate_idx = 0;
 				break;
-			case DESC92_RATE9M:
+			case DESC_RATE9M:
 				rate_idx = 1;
 				break;
-			case DESC92_RATE12M:
+			case DESC_RATE12M:
 				rate_idx = 2;
 				break;
-			case DESC92_RATE18M:
+			case DESC_RATE18M:
 				rate_idx = 3;
 				break;
-			case DESC92_RATE24M:
+			case DESC_RATE24M:
 				rate_idx = 4;
 				break;
-			case DESC92_RATE36M:
+			case DESC_RATE36M:
 				rate_idx = 5;
 				break;
-			case DESC92_RATE48M:
+			case DESC_RATE48M:
 				rate_idx = 6;
 				break;
-			case DESC92_RATE54M:
+			case DESC_RATE54M:
 				rate_idx = 7;
 				break;
 			default:
@@ -963,52 +1035,52 @@ int rtlwifi_rate_mapping(struct ieee80211_hw *hw,
 		}
 	} else {
 		switch (desc_rate) {
-		case DESC92_RATEMCS0:
+		case DESC_RATEMCS0:
 			rate_idx = 0;
 			break;
-		case DESC92_RATEMCS1:
+		case DESC_RATEMCS1:
 			rate_idx = 1;
 			break;
-		case DESC92_RATEMCS2:
+		case DESC_RATEMCS2:
 			rate_idx = 2;
 			break;
-		case DESC92_RATEMCS3:
+		case DESC_RATEMCS3:
 			rate_idx = 3;
 			break;
-		case DESC92_RATEMCS4:
+		case DESC_RATEMCS4:
 			rate_idx = 4;
 			break;
-		case DESC92_RATEMCS5:
+		case DESC_RATEMCS5:
 			rate_idx = 5;
 			break;
-		case DESC92_RATEMCS6:
+		case DESC_RATEMCS6:
 			rate_idx = 6;
 			break;
-		case DESC92_RATEMCS7:
+		case DESC_RATEMCS7:
 			rate_idx = 7;
 			break;
-		case DESC92_RATEMCS8:
+		case DESC_RATEMCS8:
 			rate_idx = 8;
 			break;
-		case DESC92_RATEMCS9:
+		case DESC_RATEMCS9:
 			rate_idx = 9;
 			break;
-		case DESC92_RATEMCS10:
+		case DESC_RATEMCS10:
 			rate_idx = 10;
 			break;
-		case DESC92_RATEMCS11:
+		case DESC_RATEMCS11:
 			rate_idx = 11;
 			break;
-		case DESC92_RATEMCS12:
+		case DESC_RATEMCS12:
 			rate_idx = 12;
 			break;
-		case DESC92_RATEMCS13:
+		case DESC_RATEMCS13:
 			rate_idx = 13;
 			break;
-		case DESC92_RATEMCS14:
+		case DESC_RATEMCS14:
 			rate_idx = 14;
 			break;
-		case DESC92_RATEMCS15:
+		case DESC_RATEMCS15:
 			rate_idx = 15;
 			break;
 		default:
diff --git a/drivers/net/wireless/rtlwifi/base.h b/drivers/net/wireless/rtlwifi/base.h
index 982f2450feea..c6cb49c3ee32 100644
--- a/drivers/net/wireless/rtlwifi/base.h
+++ b/drivers/net/wireless/rtlwifi/base.h
@@ -123,8 +123,8 @@ void rtl_watch_dog_timer_callback(unsigned long data);
 void rtl_deinit_deferred_work(struct ieee80211_hw *hw);
 
 bool rtl_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx);
-int rtlwifi_rate_mapping(struct ieee80211_hw *hw,
-			 bool isht, u8 desc_rate, bool first_ampdu);
+int rtlwifi_rate_mapping(struct ieee80211_hw *hw, bool isht,
+			 bool isvht, u8 desc_rate);
 bool rtl_tx_mgmt_proc(struct ieee80211_hw *hw, struct sk_buff *skb);
 u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx);
 
diff --git a/drivers/net/wireless/rtlwifi/core.c b/drivers/net/wireless/rtlwifi/core.c
index 5fc6f52641bd..eb203163ed05 100644
--- a/drivers/net/wireless/rtlwifi/core.c
+++ b/drivers/net/wireless/rtlwifi/core.c
@@ -95,7 +95,8 @@ void rtl_bb_delay(struct ieee80211_hw *hw, u32 addr, u32 data)
 }
 EXPORT_SYMBOL(rtl_bb_delay);
 
-void rtl_fw_cb(const struct firmware *firmware, void *context)
+static void rtl_fw_do_work(const struct firmware *firmware, void *context,
+			   bool is_wow)
 {
 	struct ieee80211_hw *hw = context;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -125,12 +126,31 @@ found_alt:
 		release_firmware(firmware);
 		return;
 	}
-	memcpy(rtlpriv->rtlhal.pfirmware, firmware->data, firmware->size);
+	if (!is_wow) {
+		memcpy(rtlpriv->rtlhal.pfirmware, firmware->data,
+		       firmware->size);
+		rtlpriv->rtlhal.fwsize = firmware->size;
+	} else {
+		memcpy(rtlpriv->rtlhal.wowlan_firmware, firmware->data,
+		       firmware->size);
+		rtlpriv->rtlhal.wowlan_fwsize = firmware->size;
+	}
 	rtlpriv->rtlhal.fwsize = firmware->size;
 	release_firmware(firmware);
 }
+
+void rtl_fw_cb(const struct firmware *firmware, void *context)
+{
+	rtl_fw_do_work(firmware, context, false);
+}
 EXPORT_SYMBOL(rtl_fw_cb);
 
+void rtl_wowlan_fw_cb(const struct firmware *firmware, void *context)
+{
+	rtl_fw_do_work(firmware, context, true);
+}
+EXPORT_SYMBOL(rtl_wowlan_fw_cb);
+
 /*mutex for start & stop is must here. */
 static int rtl_op_start(struct ieee80211_hw *hw)
 {
@@ -1851,3 +1871,40 @@ bool rtl_btc_status_false(void)
 	return false;
 }
 EXPORT_SYMBOL_GPL(rtl_btc_status_false);
+
+void rtl_dm_diginit(struct ieee80211_hw *hw, u32 cur_igvalue)
+{
+	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 = cur_igvalue;
+	dm_digtable->pre_igvalue = 0;
+	dm_digtable->cur_sta_cstate = DIG_STA_DISCONNECT;
+	dm_digtable->presta_cstate = DIG_STA_DISCONNECT;
+	dm_digtable->curmultista_cstate = 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_max = DM_DIG_MAX;
+	dm_digtable->rx_gain_min = DM_DIG_MIN;
+	dm_digtable->back_val = DM_DIG_BACKOFF_DEFAULT;
+	dm_digtable->back_range_max = DM_DIG_BACKOFF_MAX;
+	dm_digtable->back_range_min = DM_DIG_BACKOFF_MIN;
+	dm_digtable->pre_cck_cca_thres = 0xff;
+	dm_digtable->cur_cck_cca_thres = 0x83;
+	dm_digtable->forbidden_igi = DM_DIG_MIN;
+	dm_digtable->large_fa_hit = 0;
+	dm_digtable->recover_cnt = 0;
+	dm_digtable->dig_min_0 = 0x25;
+	dm_digtable->dig_min_1 = 0x25;
+	dm_digtable->media_connect_0 = false;
+	dm_digtable->media_connect_1 = false;
+	rtlpriv->dm.dm_initialgain_enable = true;
+	dm_digtable->bt30_cur_igi = 0x32;
+	dm_digtable->pre_cck_pd_state = CCK_PD_STAGE_MAX;
+	dm_digtable->cur_cck_pd_state = CCK_PD_STAGE_LOWRSSI;
+}
+EXPORT_SYMBOL(rtl_dm_diginit);
diff --git a/drivers/net/wireless/rtlwifi/core.h b/drivers/net/wireless/rtlwifi/core.h
index 624e1dc16d31..7b64e34f421e 100644
--- a/drivers/net/wireless/rtlwifi/core.h
+++ b/drivers/net/wireless/rtlwifi/core.h
@@ -35,13 +35,55 @@
 
 #define RTL_SUPPORTED_CTRL_FILTER	0xFF
 
+#define DM_DIG_THRESH_HIGH		40
+#define DM_DIG_THRESH_LOW		35
+#define DM_FALSEALARM_THRESH_LOW	400
+#define DM_FALSEALARM_THRESH_HIGH	1000
+
+#define DM_DIG_MAX			0x3e
+#define DM_DIG_MIN			0x1e
+#define DM_DIG_MAX_AP			0x32
+#define DM_DIG_BACKOFF_MAX		12
+#define DM_DIG_BACKOFF_MIN		-4
+#define DM_DIG_BACKOFF_DEFAULT		10
+
+enum cck_packet_detection_threshold {
+	CCK_PD_STAGE_LOWRSSI = 0,
+	CCK_PD_STAGE_HIGHRSSI = 1,
+	CCK_FA_STAGE_LOW = 2,
+	CCK_FA_STAGE_HIGH = 3,
+	CCK_PD_STAGE_MAX = 4,
+};
+
+enum dm_dig_ext_port_alg_e {
+	DIG_EXT_PORT_STAGE_0 = 0,
+	DIG_EXT_PORT_STAGE_1 = 1,
+	DIG_EXT_PORT_STAGE_2 = 2,
+	DIG_EXT_PORT_STAGE_3 = 3,
+	DIG_EXT_PORT_STAGE_MAX = 4,
+};
+
+enum dm_dig_connect_e {
+	DIG_STA_DISCONNECT,
+	DIG_STA_CONNECT,
+	DIG_STA_BEFORE_CONNECT,
+	DIG_MULTISTA_DISCONNECT,
+	DIG_MULTISTA_CONNECT,
+	DIG_AP_DISCONNECT,
+	DIG_AP_CONNECT,
+	DIG_AP_ADD_STATION,
+	DIG_CONNECT_MAX
+};
+
 extern const struct ieee80211_ops rtl_ops;
 void rtl_fw_cb(const struct firmware *firmware, void *context);
+void rtl_wowlan_fw_cb(const struct firmware *firmware, void *context);
 void rtl_addr_delay(u32 addr);
 void rtl_rfreg_delay(struct ieee80211_hw *hw, enum radio_path rfpath, u32 addr,
 		     u32 mask, u32 data);
 void rtl_bb_delay(struct ieee80211_hw *hw, u32 addr, u32 data);
 bool rtl_cmd_send_packet(struct ieee80211_hw *hw, struct sk_buff *skb);
 bool rtl_btc_status_false(void);
+void rtl_dm_diginit(struct ieee80211_hw *hw, u32 cur_igval);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/dm.c b/drivers/net/wireless/rtlwifi/rtl8188ee/dm.c
index 2aa34d9055f0..d930c1f78721 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/dm.c
@@ -26,6 +26,7 @@
 #include "../wifi.h"
 #include "../base.h"
 #include "../pci.h"
+#include "../core.h"
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
@@ -341,38 +342,6 @@ static void dm_tx_pwr_track_set_pwr(struct ieee80211_hw *hw,
 	}
 }
 
-static void rtl88e_dm_diginit(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
-
-	dm_dig->dig_enable_flag = true;
-	dm_dig->cur_igvalue = rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f);
-	dm_dig->pre_igvalue = 0;
-	dm_dig->cur_sta_cstate = DIG_STA_DISCONNECT;
-	dm_dig->presta_cstate = DIG_STA_DISCONNECT;
-	dm_dig->curmultista_cstate = DIG_MULTISTA_DISCONNECT;
-	dm_dig->rssi_lowthresh = DM_DIG_THRESH_LOW;
-	dm_dig->rssi_highthresh = DM_DIG_THRESH_HIGH;
-	dm_dig->fa_lowthresh = DM_FALSEALARM_THRESH_LOW;
-	dm_dig->fa_highthresh = DM_FALSEALARM_THRESH_HIGH;
-	dm_dig->rx_gain_max = DM_DIG_MAX;
-	dm_dig->rx_gain_min = DM_DIG_MIN;
-	dm_dig->back_val = DM_DIG_BACKOFF_DEFAULT;
-	dm_dig->back_range_max = DM_DIG_BACKOFF_MAX;
-	dm_dig->back_range_min = DM_DIG_BACKOFF_MIN;
-	dm_dig->pre_cck_cca_thres = 0xff;
-	dm_dig->cur_cck_cca_thres = 0x83;
-	dm_dig->forbidden_igi = DM_DIG_MIN;
-	dm_dig->large_fa_hit = 0;
-	dm_dig->recover_cnt = 0;
-	dm_dig->dig_min_0 = 0x25;
-	dm_dig->dig_min_1 = 0x25;
-	dm_dig->media_connect_0 = false;
-	dm_dig->media_connect_1 = false;
-	rtlpriv->dm.dm_initialgain_enable = true;
-}
-
 static u8 rtl88e_dm_initial_gain_min_pwdb(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -1796,9 +1765,10 @@ static void rtl88e_dm_antenna_diversity(struct ieee80211_hw *hw)
 void rtl88e_dm_init(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 cur_igvalue = rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f);
 
 	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
-	rtl88e_dm_diginit(hw);
+	rtl_dm_diginit(hw, cur_igvalue);
 	rtl88e_dm_init_dynamic_txpower(hw);
 	rtl88e_dm_init_edca_turbo(hw);
 	rtl88e_dm_init_rate_adaptive_mask(hw);
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/dm.h b/drivers/net/wireless/rtlwifi/rtl8188ee/dm.h
index 64f1f3ea9807..071ccee69eae 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/dm.h
@@ -186,28 +186,12 @@
 #define BW_AUTO_SWITCH_HIGH_LOW				25
 #define BW_AUTO_SWITCH_LOW_HIGH				30
 
-#define DM_DIG_THRESH_HIGH				40
-#define DM_DIG_THRESH_LOW				35
-
-#define DM_FALSEALARM_THRESH_LOW			400
-#define DM_FALSEALARM_THRESH_HIGH			1000
-
-#define DM_DIG_MAX					0x3e
-#define DM_DIG_MIN					0x1e
-
-#define DM_DIG_MAX_AP					0x32
-#define DM_DIG_MIN_AP					0x20
-
 #define DM_DIG_FA_UPPER					0x3e
 #define DM_DIG_FA_LOWER					0x1e
 #define DM_DIG_FA_TH0					0x200
 #define DM_DIG_FA_TH1					0x300
 #define DM_DIG_FA_TH2					0x400
 
-#define DM_DIG_BACKOFF_MAX				12
-#define DM_DIG_BACKOFF_MIN				-4
-#define DM_DIG_BACKOFF_DEFAULT				10
-
 #define RXPATHSELECTION_SS_TH_W				30
 #define RXPATHSELECTION_DIFF_TH				18
 
@@ -262,14 +246,6 @@ enum tag_dynamic_init_gain_operation_type_definition {
 	DIG_OP_TYPE_MAX
 };
 
-enum tag_cck_packet_detection_threshold_type_definition {
-	CCK_PD_STAGE_LOWRSSI = 0,
-	CCK_PD_STAGE_HIGHRSSI = 1,
-	CCK_FA_STAGE_LOW = 2,
-	CCK_FA_STAGE_HIGH = 3,
-	CCK_PD_STAGE_MAX = 4,
-};
-
 enum dm_1r_cca_e {
 	CCA_1R = 0,
 	CCA_2R = 1,
@@ -288,23 +264,6 @@ enum dm_sw_ant_switch_e {
 	ANS_ANTENNA_MAX = 3,
 };
 
-enum dm_dig_ext_port_alg_e {
-	DIG_EXT_PORT_STAGE_0 = 0,
-	DIG_EXT_PORT_STAGE_1 = 1,
-	DIG_EXT_PORT_STAGE_2 = 2,
-	DIG_EXT_PORT_STAGE_3 = 3,
-	DIG_EXT_PORT_STAGE_MAX = 4,
-};
-
-enum dm_dig_connect_e {
-	DIG_STA_DISCONNECT = 0,
-	DIG_STA_CONNECT = 1,
-	DIG_STA_BEFORE_CONNECT = 2,
-	DIG_MULTISTA_DISCONNECT = 3,
-	DIG_MULTISTA_CONNECT = 4,
-	DIG_CONNECT_MAX
-};
-
 enum pwr_track_control_method {
 	BBSWING,
 	TXAGC
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/trx.c b/drivers/net/wireless/rtlwifi/rtl8188ee/trx.c
index df549c96adef..791efbe6b18c 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/trx.c
@@ -47,164 +47,6 @@ static u8 _rtl88ee_map_hwqueue_to_fwqueue(struct sk_buff *skb, u8 hw_queue)
 	return skb->priority;
 }
 
-/* mac80211's rate_idx is like this:
- *
- * 2.4G band:rx_status->band == IEEE80211_BAND_2GHZ
- *
- * B/G rate:
- * (rx_status->flag & RX_FLAG_HT) = 0,
- * DESC92C_RATE1M-->DESC92C_RATE54M ==> idx is 0-->11,
- *
- * N rate:
- * (rx_status->flag & RX_FLAG_HT) = 1,
- * DESC92C_RATEMCS0-->DESC92C_RATEMCS15 ==> idx is 0-->15
- *
- * 5G band:rx_status->band == IEEE80211_BAND_5GHZ
- * A rate:
- * (rx_status->flag & RX_FLAG_HT) = 0,
- * DESC92C_RATE6M-->DESC92C_RATE54M ==> idx is 0-->7,
- *
- * N rate:
- * (rx_status->flag & RX_FLAG_HT) = 1,
- * DESC92C_RATEMCS0-->DESC92C_RATEMCS15 ==> idx is 0-->15
- */
-static int _rtl88ee_rate_mapping(struct ieee80211_hw *hw,
-				 bool isht, u8 desc_rate)
-{
-	int rate_idx;
-
-	if (!isht) {
-		if (IEEE80211_BAND_2GHZ == hw->conf.chandef.chan->band) {
-			switch (desc_rate) {
-			case DESC92C_RATE1M:
-				rate_idx = 0;
-				break;
-			case DESC92C_RATE2M:
-				rate_idx = 1;
-				break;
-			case DESC92C_RATE5_5M:
-				rate_idx = 2;
-				break;
-			case DESC92C_RATE11M:
-				rate_idx = 3;
-				break;
-			case DESC92C_RATE6M:
-				rate_idx = 4;
-				break;
-			case DESC92C_RATE9M:
-				rate_idx = 5;
-				break;
-			case DESC92C_RATE12M:
-				rate_idx = 6;
-				break;
-			case DESC92C_RATE18M:
-				rate_idx = 7;
-				break;
-			case DESC92C_RATE24M:
-				rate_idx = 8;
-				break;
-			case DESC92C_RATE36M:
-				rate_idx = 9;
-				break;
-			case DESC92C_RATE48M:
-				rate_idx = 10;
-				break;
-			case DESC92C_RATE54M:
-				rate_idx = 11;
-				break;
-			default:
-				rate_idx = 0;
-				break;
-			}
-		} else {
-			switch (desc_rate) {
-			case DESC92C_RATE6M:
-				rate_idx = 0;
-				break;
-			case DESC92C_RATE9M:
-				rate_idx = 1;
-				break;
-			case DESC92C_RATE12M:
-				rate_idx = 2;
-				break;
-			case DESC92C_RATE18M:
-				rate_idx = 3;
-				break;
-			case DESC92C_RATE24M:
-				rate_idx = 4;
-				break;
-			case DESC92C_RATE36M:
-				rate_idx = 5;
-				break;
-			case DESC92C_RATE48M:
-				rate_idx = 6;
-				break;
-			case DESC92C_RATE54M:
-				rate_idx = 7;
-				break;
-			default:
-				rate_idx = 0;
-				break;
-			}
-		}
-	} else {
-		switch (desc_rate) {
-		case DESC92C_RATEMCS0:
-			rate_idx = 0;
-			break;
-		case DESC92C_RATEMCS1:
-			rate_idx = 1;
-			break;
-		case DESC92C_RATEMCS2:
-			rate_idx = 2;
-			break;
-		case DESC92C_RATEMCS3:
-			rate_idx = 3;
-			break;
-		case DESC92C_RATEMCS4:
-			rate_idx = 4;
-			break;
-		case DESC92C_RATEMCS5:
-			rate_idx = 5;
-			break;
-		case DESC92C_RATEMCS6:
-			rate_idx = 6;
-			break;
-		case DESC92C_RATEMCS7:
-			rate_idx = 7;
-			break;
-		case DESC92C_RATEMCS8:
-			rate_idx = 8;
-			break;
-		case DESC92C_RATEMCS9:
-			rate_idx = 9;
-			break;
-		case DESC92C_RATEMCS10:
-			rate_idx = 10;
-			break;
-		case DESC92C_RATEMCS11:
-			rate_idx = 11;
-			break;
-		case DESC92C_RATEMCS12:
-			rate_idx = 12;
-			break;
-		case DESC92C_RATEMCS13:
-			rate_idx = 13;
-			break;
-		case DESC92C_RATEMCS14:
-			rate_idx = 14;
-			break;
-		case DESC92C_RATEMCS15:
-			rate_idx = 15;
-			break;
-		default:
-			rate_idx = 0;
-			break;
-		}
-	}
-	return rate_idx;
-}
-
 static void _rtl88ee_query_rxphystatus(struct ieee80211_hw *hw,
 			struct rtl_stats *pstatus, u8 *pdesc,
 			struct rx_fwinfo_88e *p_drvinfo,
@@ -630,8 +472,8 @@ bool rtl88ee_rx_query_desc(struct ieee80211_hw *hw,
 	 * are use (RX_FLAG_HT)
 	 * Notice: this is diff with windows define
 	 */
-	rx_status->rate_idx = _rtl88ee_rate_mapping(hw,
-				status->is_ht, status->rate);
+	rx_status->rate_idx = rtlwifi_rate_mapping(hw, status->is_ht,
+						   false, status->rate);
 
 	rx_status->mactime = status->timestamp_low;
 	if (phystatus == true) {
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
index f6cb5aedfdd1..f5ee67cda73a 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
@@ -32,6 +32,7 @@
 #include "phy_common.h"
 #include "../pci.h"
 #include "../base.h"
+#include "../core.h"
 
 #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)
@@ -194,36 +195,6 @@ void dm_savepowerindex(struct ieee80211_hw *hw)
 }
 EXPORT_SYMBOL_GPL(dm_savepowerindex);
 
-static void rtl92c_dm_diginit(struct ieee80211_hw *hw)
-{
-	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_cstate = DIG_STA_DISCONNECT;
-	dm_digtable->presta_cstate = DIG_STA_DISCONNECT;
-	dm_digtable->curmultista_cstate = 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_max = DM_DIG_MAX;
-	dm_digtable->rx_gain_min = DM_DIG_MIN;
-	dm_digtable->back_val = DM_DIG_BACKOFF_DEFAULT;
-	dm_digtable->back_range_max = DM_DIG_BACKOFF_MAX;
-	dm_digtable->back_range_min = DM_DIG_BACKOFF_MIN;
-	dm_digtable->pre_cck_pd_state = CCK_PD_STAGE_MAX;
-	dm_digtable->cur_cck_pd_state = CCK_PD_STAGE_LowRssi;
-
-	dm_digtable->forbidden_igi = DM_DIG_MIN;
-	dm_digtable->large_fa_hit = 0;
-	dm_digtable->recover_cnt = 0;
-	dm_digtable->dig_dynamic_min  = 0x25;
-}
-
 static u8 rtl92c_dm_initial_gain_min_pwdb(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -507,27 +478,27 @@ static void rtl92c_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
 		if (dm_digtable->rssi_val_min > 100)
 			dm_digtable->rssi_val_min = 100;
 
-		if (dm_digtable->pre_cck_pd_state == CCK_PD_STAGE_LowRssi) {
+		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;
+				    CCK_PD_STAGE_LOWRSSI;
 			else
 				dm_digtable->cur_cck_pd_state =
-				    CCK_PD_STAGE_HighRssi;
+				    CCK_PD_STAGE_HIGHRSSI;
 		} else {
 			if (dm_digtable->rssi_val_min <= 20)
 				dm_digtable->cur_cck_pd_state =
-				    CCK_PD_STAGE_LowRssi;
+				    CCK_PD_STAGE_LOWRSSI;
 			else
 				dm_digtable->cur_cck_pd_state =
-				    CCK_PD_STAGE_HighRssi;
+				    CCK_PD_STAGE_HIGHRSSI;
 		}
 	} else {
 		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->cur_cck_pd_state == CCK_PD_STAGE_LOWRSSI) ||
 		    (dm_digtable->cur_cck_pd_state == CCK_PD_STAGE_MAX))
 			rtl_set_bbreg(hw, RCCK0_CCA, MASKBYTE2, 0x83);
 		else
@@ -1374,7 +1345,7 @@ void rtl92c_dm_init(struct ieee80211_hw *hw)
 	rtlpriv->dm.undec_sm_pwdb = -1;
 	rtlpriv->dm.undec_sm_cck = -1;
 	rtlpriv->dm.dm_initialgain_enable = true;
-	rtl92c_dm_diginit(hw);
+	rtl_dm_diginit(hw, 0x20);
 
 	rtlpriv->dm.dm_flag |= HAL_DM_HIPWR_DISABLE;
 	rtl92c_dm_init_dynamic_txpower(hw);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.h b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.h
index 4f232a063636..4422e31fedd9 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.h
@@ -47,25 +47,12 @@
 #define BW_AUTO_SWITCH_HIGH_LOW			25
 #define BW_AUTO_SWITCH_LOW_HIGH			30
 
-#define DM_DIG_THRESH_HIGH			40
-#define DM_DIG_THRESH_LOW			35
-
-#define DM_FALSEALARM_THRESH_LOW		400
-#define DM_FALSEALARM_THRESH_HIGH		1000
-
-#define DM_DIG_MAX				0x3e
-#define DM_DIG_MIN				0x1e
-
 #define DM_DIG_FA_UPPER				0x32
 #define DM_DIG_FA_LOWER				0x20
 #define DM_DIG_FA_TH0				0x20
 #define DM_DIG_FA_TH1				0x100
 #define DM_DIG_FA_TH2				0x200
 
-#define DM_DIG_BACKOFF_MAX			12
-#define DM_DIG_BACKOFF_MIN			-4
-#define DM_DIG_BACKOFF_DEFAULT			10
-
 #define RXPATHSELECTION_SS_TH_lOW		30
 #define RXPATHSELECTION_DIFF_TH			18
 
@@ -123,14 +110,6 @@ enum tag_dynamic_init_gain_operation_type_definition {
 	DIG_OP_TYPE_MAX
 };
 
-enum tag_cck_packet_detection_threshold_type_definition {
-	CCK_PD_STAGE_LowRssi = 0,
-	CCK_PD_STAGE_HighRssi = 1,
-	CCK_FA_STAGE_Low = 2,
-	CCK_FA_STAGE_High = 3,
-	CCK_PD_STAGE_MAX = 4,
-};
-
 enum dm_1r_cca_e {
 	CCA_1R = 0,
 	CCA_2R = 1,
@@ -149,23 +128,6 @@ enum dm_sw_ant_switch_e {
 	ANS_ANTENNA_MAX = 3,
 };
 
-enum dm_dig_ext_port_alg_e {
-	DIG_EXT_PORT_STAGE_0 = 0,
-	DIG_EXT_PORT_STAGE_1 = 1,
-	DIG_EXT_PORT_STAGE_2 = 2,
-	DIG_EXT_PORT_STAGE_3 = 3,
-	DIG_EXT_PORT_STAGE_MAX = 4,
-};
-
-enum dm_dig_connect_e {
-	DIG_STA_DISCONNECT = 0,
-	DIG_STA_CONNECT = 1,
-	DIG_STA_BEFORE_CONNECT = 2,
-	DIG_MULTISTA_DISCONNECT = 3,
-	DIG_MULTISTA_CONNECT = 4,
-	DIG_CONNECT_MAX
-};
-
 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.h b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.h
index b64ae45dc674..e9f4281f5067 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.h
@@ -37,6 +37,7 @@
 #define FW_8192C_POLLING_DELAY			5
 #define FW_8192C_POLLING_TIMEOUT_COUNT		100
 #define NORMAL_CHIP				BIT(4)
+#define H2C_92C_KEEP_ALIVE_CTRL			48
 
 #define IS_FW_HEADER_EXIST(_pfwhdr)	\
 	((le16_to_cpu(_pfwhdr->signature)&0xFFF0) == 0x92C0 ||\
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/dm.c b/drivers/net/wireless/rtlwifi/rtl8192ce/dm.c
index 74f9c083b80d..09898cf2e07a 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/dm.c
@@ -30,6 +30,7 @@
 #include "../wifi.h"
 #include "../base.h"
 #include "../pci.h"
+#include "../core.h"
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/dm.h b/drivers/net/wireless/rtlwifi/rtl8192ce/dm.h
index 9c5311c299fd..38ba707015f5 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/dm.h
@@ -42,25 +42,12 @@
 #define BW_AUTO_SWITCH_HIGH_LOW			25
 #define BW_AUTO_SWITCH_LOW_HIGH			30
 
-#define DM_DIG_THRESH_HIGH			40
-#define DM_DIG_THRESH_LOW			35
-
-#define DM_FALSEALARM_THRESH_LOW		400
-#define DM_FALSEALARM_THRESH_HIGH		1000
-
-#define DM_DIG_MAX				0x3e
-#define DM_DIG_MIN				0x1e
-
 #define DM_DIG_FA_UPPER				0x32
 #define DM_DIG_FA_LOWER				0x20
 #define DM_DIG_FA_TH0				0x20
 #define DM_DIG_FA_TH1				0x100
 #define DM_DIG_FA_TH2				0x200
 
-#define DM_DIG_BACKOFF_MAX			12
-#define DM_DIG_BACKOFF_MIN			-4
-#define DM_DIG_BACKOFF_DEFAULT			10
-
 #define RXPATHSELECTION_SS_TH_lOW		30
 #define RXPATHSELECTION_DIFF_TH			18
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
index d2ec5160bbf0..303b299376c9 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
@@ -544,8 +544,13 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 						(u8 *)(&fw_current_inps));
 			}
 		break; }
-	case HW_VAR_KEEP_ALIVE:
-		break;
+	case HW_VAR_KEEP_ALIVE: {
+		u8 array[2];
+
+		array[0] = 0xff;
+		array[1] = *((u8 *)val);
+		rtl92c_fill_h2c_cmd(hw, H2C_92C_KEEP_ALIVE_CTRL, 2, array);
+		break; }
 	default:
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "switch case %d not processed\n", variable);
@@ -955,6 +960,7 @@ int rtl92ce_hw_init(struct ieee80211_hw *hw)
 	local_save_flags(flags);
 	local_irq_enable();
 
+	rtlhal->fw_ready = false;
 	rtlpriv->intf_ops->disable_aspm(hw);
 	rtstatus = _rtl92ce_init_mac(hw);
 	if (!rtstatus) {
@@ -971,6 +977,7 @@ int rtl92ce_hw_init(struct ieee80211_hw *hw)
 		goto exit;
 	}
 
+	rtlhal->fw_ready = true;
 	rtlhal->last_hmeboxnum = 0;
 	rtl92c_phy_mac_config(hw);
 	/* because last function modify RCR, so we update
@@ -1154,47 +1161,35 @@ static int _rtl92ce_set_media_status(struct ieee80211_hw *hw,
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 bt_msr = rtl_read_byte(rtlpriv, MSR);
 	enum led_ctl_mode ledaction = LED_CTL_NO_LINK;
-	bt_msr &= 0xfc;
+	u8 mode = MSR_NOLINK;
 
-	if (type == NL80211_IFTYPE_UNSPECIFIED ||
-	    type == NL80211_IFTYPE_STATION) {
-		_rtl92ce_stop_tx_beacon(hw);
-		_rtl92ce_enable_bcn_sub_func(hw);
-	} else if (type == NL80211_IFTYPE_ADHOC || type == NL80211_IFTYPE_AP ||
-		   type == NL80211_IFTYPE_MESH_POINT) {
-		_rtl92ce_resume_tx_beacon(hw);
-		_rtl92ce_disable_bcn_sub_func(hw);
-	} else {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-			 "Set HW_VAR_MEDIA_STATUS: No such media status(%x)\n",
-			 type);
-	}
+	bt_msr &= 0xfc;
 
 	switch (type) {
 	case NL80211_IFTYPE_UNSPECIFIED:
-		bt_msr |= MSR_NOLINK;
-		ledaction = LED_CTL_LINK;
+		mode = MSR_NOLINK;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "Set Network type to NO LINK!\n");
 		break;
 	case NL80211_IFTYPE_ADHOC:
-		bt_msr |= MSR_ADHOC;
+		mode = MSR_ADHOC;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "Set Network type to Ad Hoc!\n");
 		break;
 	case NL80211_IFTYPE_STATION:
-		bt_msr |= MSR_INFRA;
+		mode = MSR_INFRA;
 		ledaction = LED_CTL_LINK;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "Set Network type to STA!\n");
 		break;
 	case NL80211_IFTYPE_AP:
-		bt_msr |= MSR_AP;
+		mode = MSR_AP;
+		ledaction = LED_CTL_LINK;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "Set Network type to AP!\n");
 		break;
 	case NL80211_IFTYPE_MESH_POINT:
-		bt_msr |= MSR_ADHOC;
+		mode = MSR_ADHOC;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "Set Network type to Mesh Point!\n");
 		break;
@@ -1205,9 +1200,32 @@ static int _rtl92ce_set_media_status(struct ieee80211_hw *hw,
 
 	}
 
-	rtl_write_byte(rtlpriv, (MSR), bt_msr);
+	/* MSR_INFRA == Link in infrastructure network;
+	 * MSR_ADHOC == Link in ad hoc network;
+	 * Therefore, check link state is necessary.
+	 *
+	 * MSR_AP == AP mode; link state does not matter here.
+	 */
+	if (mode != MSR_AP &&
+	    rtlpriv->mac80211.link_state < MAC80211_LINKED) {
+		mode = MSR_NOLINK;
+		ledaction = LED_CTL_NO_LINK;
+	}
+	if (mode == MSR_NOLINK || mode == MSR_INFRA) {
+		_rtl92ce_stop_tx_beacon(hw);
+		_rtl92ce_enable_bcn_sub_func(hw);
+	} else if (mode == MSR_ADHOC || mode == MSR_AP) {
+		_rtl92ce_resume_tx_beacon(hw);
+		_rtl92ce_disable_bcn_sub_func(hw);
+	} else {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "Set HW_VAR_MEDIA_STATUS: No such media status(%x).\n",
+			 mode);
+	}
+	rtl_write_byte(rtlpriv, MSR, bt_msr | mode);
+
 	rtlpriv->cfg->ops->led_control(hw, ledaction);
-	if ((bt_msr & MSR_MASK) == MSR_AP)
+	if (mode == MSR_AP)
 		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x00);
 	else
 		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x66);
@@ -1831,7 +1849,6 @@ static void rtl92ce_update_hal_rate_table(struct ieee80211_hw *hw,
 	u32 ratr_value;
 	u8 ratr_index = 0;
 	u8 nmode = mac->ht_enable;
-	u8 mimo_ps = IEEE80211_SMPS_OFF;
 	u16 shortgi_rate;
 	u32 tmp_ratr_value;
 	u8 curtxbw_40mhz = mac->bw_40;
@@ -1840,6 +1857,7 @@ static void rtl92ce_update_hal_rate_table(struct ieee80211_hw *hw,
 	u8 curshortgi_20mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20) ?
 			       1 : 0;
 	enum wireless_mode wirelessmode = mac->mode;
+	u32 ratr_mask;
 
 	if (rtlhal->current_bandtype == BAND_ON_5G)
 		ratr_value = sta->supp_rates[1] << 4;
@@ -1863,19 +1881,13 @@ static void rtl92ce_update_hal_rate_table(struct ieee80211_hw *hw,
 	case WIRELESS_MODE_N_24G:
 	case WIRELESS_MODE_N_5G:
 		nmode = 1;
-		if (mimo_ps == IEEE80211_SMPS_STATIC) {
-			ratr_value &= 0x0007F005;
-		} else {
-			u32 ratr_mask;
-
-			if (get_rf_type(rtlphy) == RF_1T2R ||
-			    get_rf_type(rtlphy) == RF_1T1R)
-				ratr_mask = 0x000ff005;
-			else
-				ratr_mask = 0x0f0ff005;
+		if (get_rf_type(rtlphy) == RF_1T2R ||
+		    get_rf_type(rtlphy) == RF_1T1R)
+			ratr_mask = 0x000ff005;
+		else
+			ratr_mask = 0x0f0ff005;
 
-			ratr_value &= ratr_mask;
-		}
+		ratr_value &= ratr_mask;
 		break;
 	default:
 		if (rtlphy->rf_type == RF_1T2R)
@@ -1928,17 +1940,16 @@ static void rtl92ce_update_hal_rate_mask(struct ieee80211_hw *hw,
 	struct rtl_sta_info *sta_entry = NULL;
 	u32 ratr_bitmap;
 	u8 ratr_index;
-	u8 curtxbw_40mhz = (sta->bandwidth >= IEEE80211_STA_RX_BW_40) ? 1 : 0;
-	u8 curshortgi_40mhz = curtxbw_40mhz &&
-			      (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40) ?
-				1 : 0;
+	u8 curtxbw_40mhz = (sta->ht_cap.cap &
+			    IEEE80211_HT_CAP_SUP_WIDTH_20_40) ? 1 : 0;
+	u8 curshortgi_40mhz = (sta->ht_cap.cap &
+			       IEEE80211_HT_CAP_SGI_40) ?  1 : 0;
 	u8 curshortgi_20mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20) ?
 				1 : 0;
 	enum wireless_mode wirelessmode = 0;
 	bool shortgi = false;
 	u8 rate_mask[5];
 	u8 macid = 0;
-	u8 mimo_ps = IEEE80211_SMPS_OFF;
 
 	sta_entry = (struct rtl_sta_info *) sta->drv_priv;
 	wirelessmode = sta_entry->wireless_mode;
@@ -1983,47 +1994,38 @@ static void rtl92ce_update_hal_rate_mask(struct ieee80211_hw *hw,
 	case WIRELESS_MODE_N_5G:
 		ratr_index = RATR_INX_WIRELESS_NGB;
 
-		if (mimo_ps == IEEE80211_SMPS_STATIC) {
-			if (rssi_level == 1)
-				ratr_bitmap &= 0x00070000;
-			else if (rssi_level == 2)
-				ratr_bitmap &= 0x0007f000;
-			else
-				ratr_bitmap &= 0x0007f005;
+		if (rtlphy->rf_type == RF_1T2R ||
+		    rtlphy->rf_type == RF_1T1R) {
+			if (curtxbw_40mhz) {
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x000f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x000ff000;
+				else
+					ratr_bitmap &= 0x000ff015;
+			} else {
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x000f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x000ff000;
+				else
+					ratr_bitmap &= 0x000ff005;
+			}
 		} else {
-			if (rtlphy->rf_type == RF_1T2R ||
-			    rtlphy->rf_type == RF_1T1R) {
-				if (curtxbw_40mhz) {
-					if (rssi_level == 1)
-						ratr_bitmap &= 0x000f0000;
-					else if (rssi_level == 2)
-						ratr_bitmap &= 0x000ff000;
-					else
-						ratr_bitmap &= 0x000ff015;
-				} else {
-					if (rssi_level == 1)
-						ratr_bitmap &= 0x000f0000;
-					else if (rssi_level == 2)
-						ratr_bitmap &= 0x000ff000;
-					else
-						ratr_bitmap &= 0x000ff005;
-				}
+			if (curtxbw_40mhz) {
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x0f0f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x0f0ff000;
+				else
+					ratr_bitmap &= 0x0f0ff015;
 			} else {
-				if (curtxbw_40mhz) {
-					if (rssi_level == 1)
-						ratr_bitmap &= 0x0f0f0000;
-					else if (rssi_level == 2)
-						ratr_bitmap &= 0x0f0ff000;
-					else
-						ratr_bitmap &= 0x0f0ff015;
-				} else {
-					if (rssi_level == 1)
-						ratr_bitmap &= 0x0f0f0000;
-					else if (rssi_level == 2)
-						ratr_bitmap &= 0x0f0ff000;
-					else
-						ratr_bitmap &= 0x0f0ff005;
-				}
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x0f0f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x0f0ff000;
+				else
+					ratr_bitmap &= 0x0f0ff005;
 			}
 		}
 
@@ -2056,9 +2058,6 @@ static void rtl92ce_update_hal_rate_mask(struct ieee80211_hw *hw,
 		 "Rate_index:%x, ratr_val:%x, %5phC\n",
 		 ratr_index, ratr_bitmap, rate_mask);
 	rtl92c_fill_h2c_cmd(hw, H2C_RA_MASK, 5, rate_mask);
-
-	if (macid != 0)
-		sta_entry->ratr_index = ratr_index;
 }
 
 void rtl92ce_update_hal_rate_tbl(struct ieee80211_hw *hw,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c b/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c
index bc5ca989b915..1ee5a6ae9960 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c
@@ -518,11 +518,12 @@ static bool _rtl92ce_phy_set_rf_power_state(struct ieee80211_hw *hw,
 		}
 	case ERFSLEEP:{
 			if (ppsc->rfpwr_state == ERFOFF)
-				return false;
+				break;
 			for (queue_id = 0, i = 0;
 			     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
 				ring = &pcipriv->dev.tx_ring[queue_id];
-				if (skb_queue_len(&ring->queue) == 0) {
+				if (queue_id == BEACON_QUEUE ||
+				    skb_queue_len(&ring->queue) == 0) {
 					queue_id++;
 					continue;
 				} else {
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c b/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c
index dd5aa089126a..de6cb6c3a48c 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c
@@ -334,21 +334,21 @@ static struct rtl_hal_cfg rtl92ce_hal_cfg = {
 	.maps[RTL_IMR_ROK] = IMR_ROK,
 	.maps[RTL_IBSS_INT_MASKS] = (IMR_BCNINT | IMR_TBDOK | IMR_TBDER),
 
-	.maps[RTL_RC_CCK_RATE1M] = DESC92_RATE1M,
-	.maps[RTL_RC_CCK_RATE2M] = DESC92_RATE2M,
-	.maps[RTL_RC_CCK_RATE5_5M] = DESC92_RATE5_5M,
-	.maps[RTL_RC_CCK_RATE11M] = DESC92_RATE11M,
-	.maps[RTL_RC_OFDM_RATE6M] = DESC92_RATE6M,
-	.maps[RTL_RC_OFDM_RATE9M] = DESC92_RATE9M,
-	.maps[RTL_RC_OFDM_RATE12M] = DESC92_RATE12M,
-	.maps[RTL_RC_OFDM_RATE18M] = DESC92_RATE18M,
-	.maps[RTL_RC_OFDM_RATE24M] = DESC92_RATE24M,
-	.maps[RTL_RC_OFDM_RATE36M] = DESC92_RATE36M,
-	.maps[RTL_RC_OFDM_RATE48M] = DESC92_RATE48M,
-	.maps[RTL_RC_OFDM_RATE54M] = DESC92_RATE54M,
-
-	.maps[RTL_RC_HT_RATEMCS7] = DESC92_RATEMCS7,
-	.maps[RTL_RC_HT_RATEMCS15] = DESC92_RATEMCS15,
+	.maps[RTL_RC_CCK_RATE1M] = DESC_RATE1M,
+	.maps[RTL_RC_CCK_RATE2M] = DESC_RATE2M,
+	.maps[RTL_RC_CCK_RATE5_5M] = DESC_RATE5_5M,
+	.maps[RTL_RC_CCK_RATE11M] = DESC_RATE11M,
+	.maps[RTL_RC_OFDM_RATE6M] = DESC_RATE6M,
+	.maps[RTL_RC_OFDM_RATE9M] = DESC_RATE9M,
+	.maps[RTL_RC_OFDM_RATE12M] = DESC_RATE12M,
+	.maps[RTL_RC_OFDM_RATE18M] = DESC_RATE18M,
+	.maps[RTL_RC_OFDM_RATE24M] = DESC_RATE24M,
+	.maps[RTL_RC_OFDM_RATE36M] = DESC_RATE36M,
+	.maps[RTL_RC_OFDM_RATE48M] = DESC_RATE48M,
+	.maps[RTL_RC_OFDM_RATE54M] = DESC_RATE54M,
+
+	.maps[RTL_RC_HT_RATEMCS7] = DESC_RATEMCS7,
+	.maps[RTL_RC_HT_RATEMCS15] = DESC_RATEMCS15,
 };
 
 static const struct pci_device_id rtl92ce_pci_ids[] = {
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
index e88dcd0e0af1..84ddd4d07a1d 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
@@ -257,8 +257,8 @@ static void _rtl92ce_query_rxphystatus(struct ieee80211_hw *hw,
 		pstats->recvsignalpower = rx_pwr_all;
 
 		/* (3)EVM of HT rate */
-		if (pstats->is_ht && pstats->rate >= DESC92_RATEMCS8 &&
-		    pstats->rate <= DESC92_RATEMCS15)
+		if (pstats->is_ht && pstats->rate >= DESC_RATEMCS8 &&
+		    pstats->rate <= DESC_RATEMCS15)
 			max_spatial_stream = 2;
 		else
 			max_spatial_stream = 1;
@@ -400,9 +400,8 @@ bool rtl92ce_rx_query_desc(struct ieee80211_hw *hw,
 	 * are use (RX_FLAG_HT)
 	 * Notice: this is diff with windows define
 	 */
-	rx_status->rate_idx = rtlwifi_rate_mapping(hw,
-				stats->is_ht, stats->rate,
-				stats->isfirst_ampdu);
+	rx_status->rate_idx = rtlwifi_rate_mapping(hw, stats->is_ht,
+						   false, stats->rate);
 
 	rx_status->mactime = stats->timestamp_low;
 	if (phystatus) {
@@ -501,7 +500,7 @@ void rtl92ce_tx_fill_desc(struct ieee80211_hw *hw,
 		SET_TX_DESC_RTS_BW(pdesc, 0);
 		SET_TX_DESC_RTS_SC(pdesc, tcb_desc->rts_sc);
 		SET_TX_DESC_RTS_SHORT(pdesc,
-				      ((tcb_desc->rts_rate <= DESC92_RATE54M) ?
+				      ((tcb_desc->rts_rate <= DESC_RATE54M) ?
 				       (tcb_desc->rts_use_shortpreamble ? 1 : 0)
 				       : (tcb_desc->rts_use_shortgi ? 1 : 0)));
 
@@ -624,7 +623,7 @@ void rtl92ce_tx_fill_cmddesc(struct ieee80211_hw *hw,
 	if (firstseg)
 		SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
 
-	SET_TX_DESC_TX_RATE(pdesc, DESC92_RATE1M);
+	SET_TX_DESC_TX_RATE(pdesc, DESC_RATE1M);
 
 	SET_TX_DESC_SEQ(pdesc, 0);
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
index 873363acbacf..551321728ae0 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
@@ -1592,7 +1592,7 @@ void rtl92cu_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 	}
 }
 
-bool usb_cmd_send_packet(struct ieee80211_hw *hw, struct sk_buff *skb)
+static bool usb_cmd_send_packet(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
   /* Currently nothing happens here.
    * Traffic stops after some seconds in WPA2 802.11n mode.
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c b/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c
index c2d8ec6afcda..133e395b7401 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c
@@ -880,8 +880,8 @@ static void _rtl92c_query_rxphystatus(struct ieee80211_hw *hw,
 		pstats->rxpower = rx_pwr_all;
 		pstats->recvsignalpower = rx_pwr_all;
 		if (GET_RX_DESC_RX_MCS(pdesc) &&
-		    GET_RX_DESC_RX_MCS(pdesc) >= DESC92_RATEMCS8 &&
-		    GET_RX_DESC_RX_MCS(pdesc) <= DESC92_RATEMCS15)
+		    GET_RX_DESC_RX_MCS(pdesc) >= DESC_RATEMCS8 &&
+		    GET_RX_DESC_RX_MCS(pdesc) <= DESC_RATEMCS15)
 			max_spatial_stream = 2;
 		else
 			max_spatial_stream = 1;
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c b/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c
index e06bafee37f9..90a714c189a8 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c
@@ -257,20 +257,20 @@ static struct rtl_hal_cfg rtl92cu_hal_cfg = {
 	.maps[RTL_IMR_ROK] = IMR_ROK,
 	.maps[RTL_IBSS_INT_MASKS] = (IMR_BCNINT | IMR_TBDOK | IMR_TBDER),
 
-	.maps[RTL_RC_CCK_RATE1M] = DESC92_RATE1M,
-	.maps[RTL_RC_CCK_RATE2M] = DESC92_RATE2M,
-	.maps[RTL_RC_CCK_RATE5_5M] = DESC92_RATE5_5M,
-	.maps[RTL_RC_CCK_RATE11M] = DESC92_RATE11M,
-	.maps[RTL_RC_OFDM_RATE6M] = DESC92_RATE6M,
-	.maps[RTL_RC_OFDM_RATE9M] = DESC92_RATE9M,
-	.maps[RTL_RC_OFDM_RATE12M] = DESC92_RATE12M,
-	.maps[RTL_RC_OFDM_RATE18M] = DESC92_RATE18M,
-	.maps[RTL_RC_OFDM_RATE24M] = DESC92_RATE24M,
-	.maps[RTL_RC_OFDM_RATE36M] = DESC92_RATE36M,
-	.maps[RTL_RC_OFDM_RATE48M] = DESC92_RATE48M,
-	.maps[RTL_RC_OFDM_RATE54M] = DESC92_RATE54M,
-	.maps[RTL_RC_HT_RATEMCS7] = DESC92_RATEMCS7,
-	.maps[RTL_RC_HT_RATEMCS15] = DESC92_RATEMCS15,
+	.maps[RTL_RC_CCK_RATE1M] = DESC_RATE1M,
+	.maps[RTL_RC_CCK_RATE2M] = DESC_RATE2M,
+	.maps[RTL_RC_CCK_RATE5_5M] = DESC_RATE5_5M,
+	.maps[RTL_RC_CCK_RATE11M] = DESC_RATE11M,
+	.maps[RTL_RC_OFDM_RATE6M] = DESC_RATE6M,
+	.maps[RTL_RC_OFDM_RATE9M] = DESC_RATE9M,
+	.maps[RTL_RC_OFDM_RATE12M] = DESC_RATE12M,
+	.maps[RTL_RC_OFDM_RATE18M] = DESC_RATE18M,
+	.maps[RTL_RC_OFDM_RATE24M] = DESC_RATE24M,
+	.maps[RTL_RC_OFDM_RATE36M] = DESC_RATE36M,
+	.maps[RTL_RC_OFDM_RATE48M] = DESC_RATE48M,
+	.maps[RTL_RC_OFDM_RATE54M] = DESC_RATE54M,
+	.maps[RTL_RC_HT_RATEMCS7] = DESC_RATEMCS7,
+	.maps[RTL_RC_HT_RATEMCS15] = DESC_RATEMCS15,
 };
 
 #define USB_VENDER_ID_REALTEK		0x0bda
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c b/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
index f383d5f1fed5..cbead007171f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
@@ -325,6 +325,7 @@ bool rtl92cu_rx_query_desc(struct ieee80211_hw *hw,
 				   && (GET_RX_DESC_FAGGR(pdesc) == 1));
 	stats->timestamp_low = GET_RX_DESC_TSFL(pdesc);
 	stats->rx_is40Mhzpacket = (bool) GET_RX_DESC_BW(pdesc);
+	stats->is_ht = (bool)GET_RX_DESC_RX_HT(pdesc);
 	rx_status->freq = hw->conf.chandef.chan->center_freq;
 	rx_status->band = hw->conf.chandef.chan->band;
 	if (GET_RX_DESC_CRC32(pdesc))
@@ -338,10 +339,8 @@ bool rtl92cu_rx_query_desc(struct ieee80211_hw *hw,
 	rx_status->flag |= RX_FLAG_MACTIME_START;
 	if (stats->decrypted)
 		rx_status->flag |= RX_FLAG_DECRYPTED;
-	rx_status->rate_idx = rtlwifi_rate_mapping(hw,
-					(bool)GET_RX_DESC_RX_HT(pdesc),
-					(u8)GET_RX_DESC_RX_MCS(pdesc),
-					(bool)GET_RX_DESC_PAGGR(pdesc));
+	rx_status->rate_idx = rtlwifi_rate_mapping(hw, stats->is_ht,
+						   false, stats->rate);
 	rx_status->mactime = GET_RX_DESC_TSFL(pdesc);
 	if (phystatus) {
 		p_drvinfo = (struct rx_fwinfo_92c *)(skb->data +
@@ -393,6 +392,7 @@ static void _rtl_rx_process(struct ieee80211_hw *hw, struct sk_buff *skb)
 				   && (GET_RX_DESC_FAGGR(rxdesc) == 1));
 	stats.timestamp_low = GET_RX_DESC_TSFL(rxdesc);
 	stats.rx_is40Mhzpacket = (bool) GET_RX_DESC_BW(rxdesc);
+	stats.is_ht = (bool)GET_RX_DESC_RX_HT(rxdesc);
 	/* TODO: is center_freq changed when doing scan? */
 	/* TODO: Shall we add protection or just skip those two step? */
 	rx_status->freq = hw->conf.chandef.chan->center_freq;
@@ -406,10 +406,8 @@ static void _rtl_rx_process(struct ieee80211_hw *hw, struct sk_buff *skb)
 	if (GET_RX_DESC_RX_HT(rxdesc))
 		rx_status->flag |= RX_FLAG_HT;
 	/* Data rate */
-	rx_status->rate_idx = rtlwifi_rate_mapping(hw,
-					(bool)GET_RX_DESC_RX_HT(rxdesc),
-					(u8)GET_RX_DESC_RX_MCS(rxdesc),
-					(bool)GET_RX_DESC_PAGGR(rxdesc));
+	rx_status->rate_idx = rtlwifi_rate_mapping(hw, stats.is_ht,
+						   false, stats.rate);
 	/*  There is a phy status after this rx descriptor. */
 	if (GET_RX_DESC_PHY_STATUS(rxdesc)) {
 		p_drvinfo = (struct rx_fwinfo_92c *)(rxdesc + RTL_RX_DESC_SIZE);
@@ -545,7 +543,7 @@ void rtl92cu_tx_fill_desc(struct ieee80211_hw *hw,
 	SET_TX_DESC_RTS_BW(txdesc, 0);
 	SET_TX_DESC_RTS_SC(txdesc, tcb_desc->rts_sc);
 	SET_TX_DESC_RTS_SHORT(txdesc,
-			      ((tcb_desc->rts_rate <= DESC92_RATE54M) ?
+			      ((tcb_desc->rts_rate <= DESC_RATE54M) ?
 			       (tcb_desc->rts_use_shortpreamble ? 1 : 0)
 			       : (tcb_desc->rts_use_shortgi ? 1 : 0)));
 	if (mac->bw_40) {
@@ -644,7 +642,7 @@ void rtl92cu_fill_fake_txdesc(struct ieee80211_hw *hw, u8 * pDesc,
 	}
 	SET_TX_DESC_USE_RATE(pDesc, 1); /* use data rate which is set by Sw */
 	SET_TX_DESC_OWN(pDesc, 1);
-	SET_TX_DESC_TX_RATE(pDesc, DESC92_RATE1M);
+	SET_TX_DESC_TX_RATE(pDesc, DESC_RATE1M);
 	_rtl_tx_desc_checksum(pDesc);
 }
 
@@ -660,7 +658,7 @@ void rtl92cu_tx_fill_cmddesc(struct ieee80211_hw *hw,
 	memset((void *)pdesc, 0, RTL_TX_HEADER_SIZE);
 	if (firstseg)
 		SET_TX_DESC_OFFSET(pdesc, RTL_TX_HEADER_SIZE);
-	SET_TX_DESC_TX_RATE(pdesc, DESC92_RATE1M);
+	SET_TX_DESC_TX_RATE(pdesc, DESC_RATE1M);
 	SET_TX_DESC_SEQ(pdesc, 0);
 	SET_TX_DESC_LINIP(pdesc, 0);
 	SET_TX_DESC_QUEUE_SEL(pdesc, fw_queue);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/dm.c b/drivers/net/wireless/rtlwifi/rtl8192de/dm.c
index 304c443b89b2..a1be5a68edfb 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/dm.c
@@ -29,6 +29,7 @@
 
 #include "../wifi.h"
 #include "../base.h"
+#include "../core.h"
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
@@ -155,34 +156,6 @@ static const u8 cckswing_table_ch14[CCK_TABLE_SIZE][8] = {
 	{0x09, 0x08, 0x07, 0x04, 0x00, 0x00, 0x00, 0x00}     /* 32, -16.0dB */
 };
 
-static void rtl92d_dm_diginit(struct ieee80211_hw *hw)
-{
-	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_cstate = DIG_STA_DISCONNECT;
-	de_digtable->presta_cstate = DIG_STA_DISCONNECT;
-	de_digtable->curmultista_cstate = 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_max = DM_DIG_FA_UPPER;
-	de_digtable->rx_gain_min = DM_DIG_FA_LOWER;
-	de_digtable->back_val = DM_DIG_BACKOFF_DEFAULT;
-	de_digtable->back_range_max = DM_DIG_BACKOFF_MAX;
-	de_digtable->back_range_min = DM_DIG_BACKOFF_MIN;
-	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)
 {
 	u32 ret_value;
@@ -1305,7 +1278,9 @@ void rtl92d_dm_init(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
-	rtl92d_dm_diginit(hw);
+	rtl_dm_diginit(hw, 0x20);
+	rtlpriv->dm_digtable.rx_gain_max = DM_DIG_FA_UPPER;
+	rtlpriv->dm_digtable.rx_gain_min = DM_DIG_FA_LOWER;
 	rtl92d_dm_init_dynamic_txpower(hw);
 	rtl92d_dm_init_edca_turbo(hw);
 	rtl92d_dm_init_rate_adaptive_mask(hw);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/dm.h b/drivers/net/wireless/rtlwifi/rtl8192de/dm.h
index 3fea0c11c24a..f2d318ceeb28 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/dm.h
@@ -42,25 +42,12 @@
 #define BW_AUTO_SWITCH_HIGH_LOW			25
 #define BW_AUTO_SWITCH_LOW_HIGH			30
 
-#define DM_DIG_THRESH_HIGH			40
-#define DM_DIG_THRESH_LOW			35
-
-#define DM_FALSEALARM_THRESH_LOW		400
-#define DM_FALSEALARM_THRESH_HIGH		1000
-
-#define DM_DIG_MAX				0x3e
-#define DM_DIG_MIN				0x1c
-
 #define DM_DIG_FA_UPPER				0x32
 #define DM_DIG_FA_LOWER				0x20
 #define DM_DIG_FA_TH0				0x100
 #define DM_DIG_FA_TH1				0x400
 #define DM_DIG_FA_TH2				0x600
 
-#define DM_DIG_BACKOFF_MAX			12
-#define DM_DIG_BACKOFF_MIN			-4
-#define DM_DIG_BACKOFF_DEFAULT			10
-
 #define RXPATHSELECTION_SS_TH_lOW		30
 #define RXPATHSELECTION_DIFF_TH			18
 
@@ -108,14 +95,6 @@ enum tag_dynamic_init_gain_operation_type_definition {
 	DIG_OP_TYPE_MAX
 };
 
-enum tag_cck_packet_detection_threshold_type_definition {
-	CCK_PD_STAGE_LOWRSSI = 0,
-	CCK_PD_STAGE_HIGHRSSI = 1,
-	CCK_FA_STAGE_LOW = 2,
-	CCK_FA_STAGE_HIGH = 3,
-	CCK_PD_STAGE_MAX = 4,
-};
-
 enum dm_1r_cca {
 	CCA_1R = 0,
 	CCA_2R = 1,
@@ -134,23 +113,6 @@ enum dm_sw_ant_switch {
 	ANS_ANTENNA_MAX = 3,
 };
 
-enum dm_dig_ext_port_alg {
-	DIG_EXT_PORT_STAGE_0 = 0,
-	DIG_EXT_PORT_STAGE_1 = 1,
-	DIG_EXT_PORT_STAGE_2 = 2,
-	DIG_EXT_PORT_STAGE_3 = 3,
-	DIG_EXT_PORT_STAGE_MAX = 4,
-};
-
-enum dm_dig_connect {
-	DIG_STA_DISCONNECT = 0,
-	DIG_STA_CONNECT = 1,
-	DIG_STA_BEFORE_CONNECT = 2,
-	DIG_MULTISTA_DISCONNECT = 3,
-	DIG_MULTISTA_CONNECT = 4,
-	DIG_CONNECT_MAX
-};
-
 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/fw.c b/drivers/net/wireless/rtlwifi/rtl8192de/fw.c
index 23177076b97f..62ef8209718f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/fw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/fw.c
@@ -540,23 +540,6 @@ void rtl92d_fill_h2c_cmd(struct ieee80211_hw *hw,
 	return;
 }
 
-void rtl92d_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 u1_h2c_set_pwrmode[3] = { 0 };
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-
-	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
-	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, mode);
-	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode, 1);
-	SET_H2CCMD_PWRMODE_PARM_BCN_PASS_TIME(u1_h2c_set_pwrmode,
-					      ppsc->reg_max_lps_awakeintvl);
-	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
-		      "rtl92d_set_fw_rsvdpagepkt(): u1_h2c_set_pwrmode",
-		      u1_h2c_set_pwrmode, 3);
-	rtl92d_fill_h2c_cmd(hw, H2C_SETPWRMODE, 3, u1_h2c_set_pwrmode);
-}
-
 static bool _rtl92d_cmd_send_packet(struct ieee80211_hw *hw,
 				    struct sk_buff *skb)
 {
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/fw.h b/drivers/net/wireless/rtlwifi/rtl8192de/fw.h
index a55a803a0b4d..1646e7c3d0f8 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/fw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/fw.h
@@ -136,7 +136,6 @@ int rtl92d_download_fw(struct ieee80211_hw *hw);
 void rtl92d_fill_h2c_cmd(struct ieee80211_hw *hw, u8 element_id,
 			 u32 cmd_len, u8 *p_cmdbuffer);
 void rtl92d_firmware_selfreset(struct ieee80211_hw *hw);
-void rtl92d_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode);
 void rtl92d_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished);
 void rtl92d_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus);
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/sw.c b/drivers/net/wireless/rtlwifi/rtl8192de/sw.c
index a0aba088259a..b19d0398215f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/sw.c
@@ -337,21 +337,21 @@ static struct rtl_hal_cfg rtl92de_hal_cfg = {
 	.maps[RTL_IMR_ROK] = IMR_ROK,
 	.maps[RTL_IBSS_INT_MASKS] = (IMR_BCNINT | IMR_TBDOK | IMR_TBDER),
 
-	.maps[RTL_RC_CCK_RATE1M] = DESC92_RATE1M,
-	.maps[RTL_RC_CCK_RATE2M] = DESC92_RATE2M,
-	.maps[RTL_RC_CCK_RATE5_5M] = DESC92_RATE5_5M,
-	.maps[RTL_RC_CCK_RATE11M] = DESC92_RATE11M,
-	.maps[RTL_RC_OFDM_RATE6M] = DESC92_RATE6M,
-	.maps[RTL_RC_OFDM_RATE9M] = DESC92_RATE9M,
-	.maps[RTL_RC_OFDM_RATE12M] = DESC92_RATE12M,
-	.maps[RTL_RC_OFDM_RATE18M] = DESC92_RATE18M,
-	.maps[RTL_RC_OFDM_RATE24M] = DESC92_RATE24M,
-	.maps[RTL_RC_OFDM_RATE36M] = DESC92_RATE36M,
-	.maps[RTL_RC_OFDM_RATE48M] = DESC92_RATE48M,
-	.maps[RTL_RC_OFDM_RATE54M] = DESC92_RATE54M,
-
-	.maps[RTL_RC_HT_RATEMCS7] = DESC92_RATEMCS7,
-	.maps[RTL_RC_HT_RATEMCS15] = DESC92_RATEMCS15,
+	.maps[RTL_RC_CCK_RATE1M] = DESC_RATE1M,
+	.maps[RTL_RC_CCK_RATE2M] = DESC_RATE2M,
+	.maps[RTL_RC_CCK_RATE5_5M] = DESC_RATE5_5M,
+	.maps[RTL_RC_CCK_RATE11M] = DESC_RATE11M,
+	.maps[RTL_RC_OFDM_RATE6M] = DESC_RATE6M,
+	.maps[RTL_RC_OFDM_RATE9M] = DESC_RATE9M,
+	.maps[RTL_RC_OFDM_RATE12M] = DESC_RATE12M,
+	.maps[RTL_RC_OFDM_RATE18M] = DESC_RATE18M,
+	.maps[RTL_RC_OFDM_RATE24M] = DESC_RATE24M,
+	.maps[RTL_RC_OFDM_RATE36M] = DESC_RATE36M,
+	.maps[RTL_RC_OFDM_RATE48M] = DESC_RATE48M,
+	.maps[RTL_RC_OFDM_RATE54M] = DESC_RATE54M,
+
+	.maps[RTL_RC_HT_RATEMCS7] = DESC_RATEMCS7,
+	.maps[RTL_RC_HT_RATEMCS15] = DESC_RATEMCS15,
 };
 
 static struct pci_device_id rtl92de_pci_ids[] = {
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/trx.c b/drivers/net/wireless/rtlwifi/rtl8192de/trx.c
index 8efbcc7af250..1feaa629dd4f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/trx.c
@@ -235,8 +235,8 @@ static void _rtl92de_query_rxphystatus(struct ieee80211_hw *hw,
 		pstats->rx_pwdb_all = pwdb_all;
 		pstats->rxpower = rx_pwr_all;
 		pstats->recvsignalpower = rx_pwr_all;
-		if (pdesc->rxht && pdesc->rxmcs >= DESC92_RATEMCS8 &&
-		    pdesc->rxmcs <= DESC92_RATEMCS15)
+		if (pdesc->rxht && pdesc->rxmcs >= DESC_RATEMCS8 &&
+		    pdesc->rxmcs <= DESC_RATEMCS15)
 			max_spatial_stream = 2;
 		else
 			max_spatial_stream = 1;
@@ -499,6 +499,7 @@ bool rtl92de_rx_query_desc(struct ieee80211_hw *hw,	struct rtl_stats *stats,
 					 && (GET_RX_DESC_FAGGR(pdesc) == 1));
 	stats->timestamp_low = GET_RX_DESC_TSFL(pdesc);
 	stats->rx_is40Mhzpacket = (bool) GET_RX_DESC_BW(pdesc);
+	stats->is_ht = (bool)GET_RX_DESC_RXHT(pdesc);
 	rx_status->freq = hw->conf.chandef.chan->center_freq;
 	rx_status->band = hw->conf.chandef.chan->band;
 	if (GET_RX_DESC_CRC32(pdesc))
@@ -512,10 +513,8 @@ bool rtl92de_rx_query_desc(struct ieee80211_hw *hw,	struct rtl_stats *stats,
 	rx_status->flag |= RX_FLAG_MACTIME_START;
 	if (stats->decrypted)
 		rx_status->flag |= RX_FLAG_DECRYPTED;
-	rx_status->rate_idx = rtlwifi_rate_mapping(hw,
-					(bool)GET_RX_DESC_RXHT(pdesc),
-					(u8)GET_RX_DESC_RXMCS(pdesc),
-					(bool)GET_RX_DESC_PAGGR(pdesc));
+	rx_status->rate_idx = rtlwifi_rate_mapping(hw, stats->is_ht,
+						   false, stats->rate);
 	rx_status->mactime = GET_RX_DESC_TSFL(pdesc);
 	if (phystatus) {
 		p_drvinfo = (struct rx_fwinfo_92d *)(skb->data +
@@ -612,14 +611,14 @@ void rtl92de_tx_fill_desc(struct ieee80211_hw *hw,
 		}
 		/* 5G have no CCK rate */
 		if (rtlhal->current_bandtype == BAND_ON_5G)
-			if (ptcb_desc->hw_rate < DESC92_RATE6M)
-				ptcb_desc->hw_rate = DESC92_RATE6M;
+			if (ptcb_desc->hw_rate < DESC_RATE6M)
+				ptcb_desc->hw_rate = DESC_RATE6M;
 		SET_TX_DESC_TX_RATE(pdesc, ptcb_desc->hw_rate);
 		if (ptcb_desc->use_shortgi || ptcb_desc->use_shortpreamble)
 			SET_TX_DESC_DATA_SHORTGI(pdesc, 1);
 
 		if (rtlhal->macphymode == DUALMAC_DUALPHY &&
-			ptcb_desc->hw_rate == DESC92_RATEMCS7)
+			ptcb_desc->hw_rate == DESC_RATEMCS7)
 			SET_TX_DESC_DATA_SHORTGI(pdesc, 1);
 
 		if (info->flags & IEEE80211_TX_CTL_AMPDU) {
@@ -635,13 +634,13 @@ void rtl92de_tx_fill_desc(struct ieee80211_hw *hw,
 		SET_TX_DESC_RTS_STBC(pdesc, ((ptcb_desc->rts_stbc) ? 1 : 0));
 		/* 5G have no CCK rate */
 		if (rtlhal->current_bandtype == BAND_ON_5G)
-			if (ptcb_desc->rts_rate < DESC92_RATE6M)
-				ptcb_desc->rts_rate = DESC92_RATE6M;
+			if (ptcb_desc->rts_rate < DESC_RATE6M)
+				ptcb_desc->rts_rate = DESC_RATE6M;
 		SET_TX_DESC_RTS_RATE(pdesc, ptcb_desc->rts_rate);
 		SET_TX_DESC_RTS_BW(pdesc, 0);
 		SET_TX_DESC_RTS_SC(pdesc, ptcb_desc->rts_sc);
 		SET_TX_DESC_RTS_SHORT(pdesc, ((ptcb_desc->rts_rate <=
-			DESC92_RATE54M) ?
+			DESC_RATE54M) ?
 			(ptcb_desc->rts_use_shortpreamble ? 1 : 0) :
 			(ptcb_desc->rts_use_shortgi ? 1 : 0)));
 		if (bw_40) {
@@ -756,9 +755,9 @@ void rtl92de_tx_fill_cmddesc(struct ieee80211_hw *hw,
 	 * The braces are needed no matter what checkpatch says
 	 */
 	if (rtlhal->current_bandtype == BAND_ON_5G) {
-		SET_TX_DESC_TX_RATE(pdesc, DESC92_RATE6M);
+		SET_TX_DESC_TX_RATE(pdesc, DESC_RATE6M);
 	} else {
-		SET_TX_DESC_TX_RATE(pdesc, DESC92_RATE1M);
+		SET_TX_DESC_TX_RATE(pdesc, DESC_RATE1M);
 	}
 	SET_TX_DESC_SEQ(pdesc, 0);
 	SET_TX_DESC_LINIP(pdesc, 0);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ee/dm.c b/drivers/net/wireless/rtlwifi/rtl8192ee/dm.c
index 77deedf79d1d..459f3d0efa2f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ee/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/dm.c
@@ -26,6 +26,7 @@
 #include "../wifi.h"
 #include "../base.h"
 #include "../pci.h"
+#include "../core.h"
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
@@ -151,35 +152,6 @@ static const u8 cckswing_table_ch14[CCK_TABLE_SIZE][8] = {
 	{0x09, 0x08, 0x07, 0x04, 0x00, 0x00, 0x00, 0x00}  /* 32, -16.0dB */
 };
 
-static void rtl92ee_dm_diginit(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
-
-	dm_dig->cur_igvalue = rtl_get_bbreg(hw, DM_REG_IGI_A_11N,
-					    DM_BIT_IGI_11N);
-	dm_dig->rssi_lowthresh = DM_DIG_THRESH_LOW;
-	dm_dig->rssi_highthresh = DM_DIG_THRESH_HIGH;
-	dm_dig->fa_lowthresh = DM_FALSEALARM_THRESH_LOW;
-	dm_dig->fa_highthresh = DM_FALSEALARM_THRESH_HIGH;
-	dm_dig->rx_gain_max = DM_DIG_MAX;
-	dm_dig->rx_gain_min = DM_DIG_MIN;
-	dm_dig->back_val = DM_DIG_BACKOFF_DEFAULT;
-	dm_dig->back_range_max = DM_DIG_BACKOFF_MAX;
-	dm_dig->back_range_min = DM_DIG_BACKOFF_MIN;
-	dm_dig->pre_cck_cca_thres = 0xff;
-	dm_dig->cur_cck_cca_thres = 0x83;
-	dm_dig->forbidden_igi = DM_DIG_MIN;
-	dm_dig->large_fa_hit = 0;
-	dm_dig->recover_cnt = 0;
-	dm_dig->dig_dynamic_min = DM_DIG_MIN;
-	dm_dig->dig_dynamic_min_1 = DM_DIG_MIN;
-	dm_dig->media_connect_0 = false;
-	dm_dig->media_connect_1 = false;
-	rtlpriv->dm.dm_initialgain_enable = true;
-	dm_dig->bt30_cur_igi = 0x32;
-}
-
 static void rtl92ee_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
 {
 	u32 ret_value;
@@ -298,7 +270,7 @@ static void rtl92ee_dm_dig(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
-	u8 dig_dynamic_min , dig_maxofmin;
+	u8 dig_min_0, dig_maxofmin;
 	bool bfirstconnect , bfirstdisconnect;
 	u8 dm_dig_max, dm_dig_min;
 	u8 current_igi = dm_dig->cur_igvalue;
@@ -308,7 +280,7 @@ static void rtl92ee_dm_dig(struct ieee80211_hw *hw)
 	if (mac->act_scanning)
 		return;
 
-	dig_dynamic_min = dm_dig->dig_dynamic_min;
+	dig_min_0 = dm_dig->dig_min_0;
 	bfirstconnect = (mac->link_state >= MAC80211_LINKED) &&
 			!dm_dig->media_connect_0;
 	bfirstdisconnect = (mac->link_state < MAC80211_LINKED) &&
@@ -329,19 +301,19 @@ static void rtl92ee_dm_dig(struct ieee80211_hw *hw)
 		if (rtlpriv->dm.one_entry_only) {
 			offset = 0;
 			if (dm_dig->rssi_val_min - offset < dm_dig_min)
-				dig_dynamic_min = dm_dig_min;
+				dig_min_0 = dm_dig_min;
 			else if (dm_dig->rssi_val_min - offset >
 				 dig_maxofmin)
-				dig_dynamic_min = dig_maxofmin;
+				dig_min_0 = dig_maxofmin;
 			else
-				dig_dynamic_min = dm_dig->rssi_val_min - offset;
+				dig_min_0 = dm_dig->rssi_val_min - offset;
 		} else {
-			dig_dynamic_min = dm_dig_min;
+			dig_min_0 = dm_dig_min;
 		}
 
 	} else {
 		dm_dig->rx_gain_max = dm_dig_max;
-		dig_dynamic_min = dm_dig_min;
+		dig_min_0 = dm_dig_min;
 		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "no link\n");
 	}
 
@@ -368,10 +340,10 @@ static void rtl92ee_dm_dig(struct ieee80211_hw *hw)
 		} else {
 			if (dm_dig->large_fa_hit < 3) {
 				if ((dm_dig->forbidden_igi - 1) <
-				    dig_dynamic_min) {
-					dm_dig->forbidden_igi = dig_dynamic_min;
+				    dig_min_0) {
+					dm_dig->forbidden_igi = dig_min_0;
 					dm_dig->rx_gain_min =
-								dig_dynamic_min;
+								dig_min_0;
 				} else {
 					dm_dig->forbidden_igi--;
 					dm_dig->rx_gain_min =
@@ -430,7 +402,7 @@ static void rtl92ee_dm_dig(struct ieee80211_hw *hw)
 	rtl92ee_dm_write_dig(hw , current_igi);
 	dm_dig->media_connect_0 = ((mac->link_state >= MAC80211_LINKED) ?
 				   true : false);
-	dm_dig->dig_dynamic_min = dig_dynamic_min;
+	dm_dig->dig_min_0 = dig_min_0;
 }
 
 void rtl92ee_dm_write_cck_cca_thres(struct ieee80211_hw *hw, u8 cur_thres)
@@ -1088,10 +1060,11 @@ static void rtl92ee_dm_init_dynamic_atc_switch(struct ieee80211_hw *hw)
 void rtl92ee_dm_init(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 cur_igvalue = rtl_get_bbreg(hw, DM_REG_IGI_A_11N, DM_BIT_IGI_11N);
 
 	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
 
-	rtl92ee_dm_diginit(hw);
+	rtl_dm_diginit(hw, cur_igvalue);
 	rtl92ee_dm_init_rate_adaptive_mask(hw);
 	rtl92ee_dm_init_primary_cca_check(hw);
 	rtl92ee_dm_init_edca_turbo(hw);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ee/dm.h b/drivers/net/wireless/rtlwifi/rtl8192ee/dm.h
index 881db7d6fef7..107d5a488fa8 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ee/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/dm.h
@@ -189,28 +189,12 @@
 #define BW_AUTO_SWITCH_HIGH_LOW			25
 #define BW_AUTO_SWITCH_LOW_HIGH			30
 
-#define DM_DIG_THRESH_HIGH			40
-#define DM_DIG_THRESH_LOW			35
-
-#define DM_FALSEALARM_THRESH_LOW		400
-#define DM_FALSEALARM_THRESH_HIGH		1000
-
-#define DM_DIG_MAX				0x3e
-#define DM_DIG_MIN				0x1e
-
-#define DM_DIG_MAX_AP				0x32
-#define DM_DIG_MIN_AP				0x20
-
 #define DM_DIG_FA_UPPER				0x3e
 #define DM_DIG_FA_LOWER				0x1e
 #define DM_DIG_FA_TH0				0x200
 #define DM_DIG_FA_TH1				0x300
 #define DM_DIG_FA_TH2				0x400
 
-#define DM_DIG_BACKOFF_MAX			12
-#define DM_DIG_BACKOFF_MIN			-4
-#define DM_DIG_BACKOFF_DEFAULT			10
-
 #define RXPATHSELECTION_SS_TH_LOW		30
 #define RXPATHSELECTION_DIFF_TH			18
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ee/trx.c b/drivers/net/wireless/rtlwifi/rtl8192ee/trx.c
index 2fcbef1d029f..55d1da5e162b 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ee/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/trx.c
@@ -47,164 +47,6 @@ static u8 _rtl92ee_map_hwqueue_to_fwqueue(struct sk_buff *skb, u8 hw_queue)
 	return skb->priority;
 }
 
-/* mac80211's rate_idx is like this:
- *
- * 2.4G band:rx_status->band == IEEE80211_BAND_2GHZ
- *
- * B/G rate:
- * (rx_status->flag & RX_FLAG_HT) = 0,
- * DESC92C_RATE1M-->DESC92C_RATE54M ==> idx is 0-->11,
- *
- * N rate:
- * (rx_status->flag & RX_FLAG_HT) = 1,
- * DESC92C_RATEMCS0-->DESC92C_RATEMCS15 ==> idx is 0-->15
- *
- * 5G band:rx_status->band == IEEE80211_BAND_5GHZ
- * A rate:
- * (rx_status->flag & RX_FLAG_HT) = 0,
- * DESC92C_RATE6M-->DESC92C_RATE54M ==> idx is 0-->7,
- *
- * N rate:
- * (rx_status->flag & RX_FLAG_HT) = 1,
- * DESC92C_RATEMCS0-->DESC92C_RATEMCS15 ==> idx is 0-->15
- */
-static int _rtl92ee_rate_mapping(struct ieee80211_hw *hw,
-				 bool isht, u8 desc_rate)
-{
-	int rate_idx;
-
-	if (!isht) {
-		if (IEEE80211_BAND_2GHZ == hw->conf.chandef.chan->band) {
-			switch (desc_rate) {
-			case DESC92C_RATE1M:
-				rate_idx = 0;
-				break;
-			case DESC92C_RATE2M:
-				rate_idx = 1;
-				break;
-			case DESC92C_RATE5_5M:
-				rate_idx = 2;
-				break;
-			case DESC92C_RATE11M:
-				rate_idx = 3;
-				break;
-			case DESC92C_RATE6M:
-				rate_idx = 4;
-				break;
-			case DESC92C_RATE9M:
-				rate_idx = 5;
-				break;
-			case DESC92C_RATE12M:
-				rate_idx = 6;
-				break;
-			case DESC92C_RATE18M:
-				rate_idx = 7;
-				break;
-			case DESC92C_RATE24M:
-				rate_idx = 8;
-				break;
-			case DESC92C_RATE36M:
-				rate_idx = 9;
-				break;
-			case DESC92C_RATE48M:
-				rate_idx = 10;
-				break;
-			case DESC92C_RATE54M:
-				rate_idx = 11;
-				break;
-			default:
-				rate_idx = 0;
-				break;
-			}
-		} else {
-			switch (desc_rate) {
-			case DESC92C_RATE6M:
-				rate_idx = 0;
-				break;
-			case DESC92C_RATE9M:
-				rate_idx = 1;
-				break;
-			case DESC92C_RATE12M:
-				rate_idx = 2;
-				break;
-			case DESC92C_RATE18M:
-				rate_idx = 3;
-				break;
-			case DESC92C_RATE24M:
-				rate_idx = 4;
-				break;
-			case DESC92C_RATE36M:
-				rate_idx = 5;
-				break;
-			case DESC92C_RATE48M:
-				rate_idx = 6;
-				break;
-			case DESC92C_RATE54M:
-				rate_idx = 7;
-				break;
-			default:
-				rate_idx = 0;
-				break;
-			}
-		}
-	} else {
-		switch (desc_rate) {
-		case DESC92C_RATEMCS0:
-			rate_idx = 0;
-			break;
-		case DESC92C_RATEMCS1:
-			rate_idx = 1;
-			break;
-		case DESC92C_RATEMCS2:
-			rate_idx = 2;
-			break;
-		case DESC92C_RATEMCS3:
-			rate_idx = 3;
-			break;
-		case DESC92C_RATEMCS4:
-			rate_idx = 4;
-			break;
-		case DESC92C_RATEMCS5:
-			rate_idx = 5;
-			break;
-		case DESC92C_RATEMCS6:
-			rate_idx = 6;
-			break;
-		case DESC92C_RATEMCS7:
-			rate_idx = 7;
-			break;
-		case DESC92C_RATEMCS8:
-			rate_idx = 8;
-			break;
-		case DESC92C_RATEMCS9:
-			rate_idx = 9;
-			break;
-		case DESC92C_RATEMCS10:
-			rate_idx = 10;
-			break;
-		case DESC92C_RATEMCS11:
-			rate_idx = 11;
-			break;
-		case DESC92C_RATEMCS12:
-			rate_idx = 12;
-			break;
-		case DESC92C_RATEMCS13:
-			rate_idx = 13;
-			break;
-		case DESC92C_RATEMCS14:
-			rate_idx = 14;
-			break;
-		case DESC92C_RATEMCS15:
-			rate_idx = 15;
-			break;
-		default:
-			rate_idx = 0;
-			break;
-		}
-	}
-	return rate_idx;
-}
-
 static void _rtl92ee_query_rxphystatus(struct ieee80211_hw *hw,
 				       struct rtl_stats *pstatus, u8 *pdesc,
 				       struct rx_fwinfo *p_drvinfo,
@@ -345,8 +187,8 @@ static void _rtl92ee_query_rxphystatus(struct ieee80211_hw *hw,
 		pstatus->recvsignalpower = rx_pwr_all;
 
 		/* (3)EVM of HT rate */
-		if (pstatus->rate >= DESC92C_RATEMCS8 &&
-		    pstatus->rate <= DESC92C_RATEMCS15)
+		if (pstatus->rate >= DESC_RATEMCS8 &&
+		    pstatus->rate <= DESC_RATEMCS15)
 			max_spatial_stream = 2;
 		else
 			max_spatial_stream = 1;
@@ -576,9 +418,8 @@ bool rtl92ee_rx_query_desc(struct ieee80211_hw *hw,
 	 * are use (RX_FLAG_HT)
 	 * Notice: this is diff with windows define
 	 */
-	rx_status->rate_idx = _rtl92ee_rate_mapping(hw,
-						    status->is_ht,
-						    status->rate);
+	rx_status->rate_idx = rtlwifi_rate_mapping(hw, status->is_ht,
+						   false, status->rate);
 
 	rx_status->mactime = status->timestamp_low;
 	if (phystatus) {
@@ -710,27 +551,6 @@ static u16 get_desc_addr_fr_q_idx(u16 queue_index)
 	return desc_address;
 }
 
-void rtl92ee_get_available_desc(struct ieee80211_hw *hw, u8 q_idx)
-{
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u16 point_diff = 0;
-	u16 current_tx_read_point = 0, current_tx_write_point = 0;
-	u32 tmp_4byte;
-
-	tmp_4byte = rtl_read_dword(rtlpriv,
-				   get_desc_addr_fr_q_idx(q_idx));
-	current_tx_read_point = (u16)((tmp_4byte >> 16) & 0x0fff);
-	current_tx_write_point = (u16)((tmp_4byte) & 0x0fff);
-
-	point_diff = ((current_tx_read_point > current_tx_write_point) ?
-		      (current_tx_read_point - current_tx_write_point) :
-		      (TX_DESC_NUM_92E - current_tx_write_point +
-		       current_tx_read_point));
-
-	rtlpci->tx_ring[q_idx].avl_desc = point_diff;
-}
-
 void rtl92ee_pre_fill_tx_bd_desc(struct ieee80211_hw *hw,
 				 u8 *tx_bd_desc, u8 *desc, u8 queue_index,
 				 struct sk_buff *skb, dma_addr_t addr)
@@ -901,13 +721,13 @@ void rtl92ee_tx_fill_desc(struct ieee80211_hw *hw,
 		} else {
 			if (rtlpriv->ra.is_special_data) {
 				ptcb_desc->use_driver_rate = true;
-				SET_TX_DESC_TX_RATE(pdesc, DESC92C_RATE11M);
+				SET_TX_DESC_TX_RATE(pdesc, DESC_RATE11M);
 			} else {
 				ptcb_desc->use_driver_rate = false;
 			}
 		}
 
-		if (ptcb_desc->hw_rate > DESC92C_RATEMCS0)
+		if (ptcb_desc->hw_rate > DESC_RATEMCS0)
 			short_gi = (ptcb_desc->use_shortgi) ? 1 : 0;
 		else
 			short_gi = (ptcb_desc->use_shortpreamble) ? 1 : 0;
@@ -927,7 +747,7 @@ void rtl92ee_tx_fill_desc(struct ieee80211_hw *hw,
 		SET_TX_DESC_RTS_RATE(pdesc, ptcb_desc->rts_rate);
 		SET_TX_DESC_RTS_SC(pdesc, ptcb_desc->rts_sc);
 		SET_TX_DESC_RTS_SHORT(pdesc,
-				((ptcb_desc->rts_rate <= DESC92C_RATE54M) ?
+				((ptcb_desc->rts_rate <= DESC_RATE54M) ?
 				 (ptcb_desc->rts_use_shortpreamble ? 1 : 0) :
 				 (ptcb_desc->rts_use_shortgi ? 1 : 0)));
 
@@ -1038,7 +858,7 @@ void rtl92ee_tx_fill_cmddesc(struct ieee80211_hw *hw,
 	if (firstseg)
 		SET_TX_DESC_OFFSET(pdesc, txdesc_len);
 
-	SET_TX_DESC_TX_RATE(pdesc, DESC92C_RATE1M);
+	SET_TX_DESC_TX_RATE(pdesc, DESC_RATE1M);
 
 	SET_TX_DESC_SEQ(pdesc, 0);
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ee/trx.h b/drivers/net/wireless/rtlwifi/rtl8192ee/trx.h
index 6f9be1c7515c..48504c25fffb 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ee/trx.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/trx.h
@@ -591,10 +591,10 @@ do {								\
 } while (0)
 
 #define RTL92EE_RX_HAL_IS_CCK_RATE(rxmcs)\
-	(rxmcs == DESC92C_RATE1M ||\
-	 rxmcs == DESC92C_RATE2M ||\
-	 rxmcs == DESC92C_RATE5_5M ||\
-	 rxmcs == DESC92C_RATE11M)
+	(rxmcs == DESC_RATE1M ||\
+	 rxmcs == DESC_RATE2M ||\
+	 rxmcs == DESC_RATE5_5M ||\
+	 rxmcs == DESC_RATE11M)
 
 #define IS_LITTLE_ENDIAN	1
 
@@ -829,7 +829,6 @@ void rtl92ee_rx_check_dma_ok(struct ieee80211_hw *hw, u8 *header_desc,
 			     u8 queue_index);
 u16	rtl92ee_rx_desc_buff_remained_cnt(struct ieee80211_hw *hw,
 					  u8 queue_index);
-void rtl92ee_get_available_desc(struct ieee80211_hw *hw, u8 queue_index);
 void rtl92ee_pre_fill_tx_bd_desc(struct ieee80211_hw *hw,
 				 u8 *tx_bd_desc, u8 *desc, u8 queue_index,
 				 struct sk_buff *skb, dma_addr_t addr);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/def.h b/drivers/net/wireless/rtlwifi/rtl8192se/def.h
index 6e7a70b43949..ef87c09b77d0 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/def.h
@@ -450,10 +450,10 @@
 	SHIFT_AND_MASK_LE(__pdesc + 24, 0, 32)
 
 #define SE_RX_HAL_IS_CCK_RATE(_pdesc)\
-	(GET_RX_STATUS_DESC_RX_MCS(_pdesc) == DESC92_RATE1M ||	\
-	 GET_RX_STATUS_DESC_RX_MCS(_pdesc) == DESC92_RATE2M ||	\
-	 GET_RX_STATUS_DESC_RX_MCS(_pdesc) == DESC92_RATE5_5M ||\
-	 GET_RX_STATUS_DESC_RX_MCS(_pdesc) == DESC92_RATE11M)
+	(GET_RX_STATUS_DESC_RX_MCS(_pdesc) == DESC_RATE1M ||	\
+	 GET_RX_STATUS_DESC_RX_MCS(_pdesc) == DESC_RATE2M ||	\
+	 GET_RX_STATUS_DESC_RX_MCS(_pdesc) == DESC_RATE5_5M ||\
+	 GET_RX_STATUS_DESC_RX_MCS(_pdesc) == DESC_RATE11M)
 
 enum rf_optype {
 	RF_OP_BY_SW_3WIRE = 0,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/dm.c b/drivers/net/wireless/rtlwifi/rtl8192se/dm.c
index b3a2d5ec59e6..575980b88658 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/dm.c
@@ -29,6 +29,7 @@
 
 #include "../wifi.h"
 #include "../base.h"
+#include "../core.h"
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
@@ -469,7 +470,7 @@ static void _rtl92s_dm_initial_gain_sta_beforeconnect(struct ieee80211_hw *hw)
 			if (digtable->backoff_enable_flag)
 				rtl92s_backoff_enable_flag(hw);
 			else
-				digtable->back_val = DM_DIG_BACKOFF;
+				digtable->back_val = DM_DIG_BACKOFF_MAX;
 
 			if ((digtable->rssi_val + 10 - digtable->back_val) >
 				digtable->rx_gain_max)
@@ -503,7 +504,7 @@ static void _rtl92s_dm_initial_gain_sta_beforeconnect(struct ieee80211_hw *hw)
 		digtable->dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
 		rtl92s_phy_set_fw_cmd(hw, FW_CMD_DIG_ENABLE);
 
-		digtable->back_val = DM_DIG_BACKOFF;
+		digtable->back_val = DM_DIG_BACKOFF_MAX;
 		digtable->cur_igvalue = rtlpriv->phy.default_initialgain[0];
 		digtable->pre_igvalue = 0;
 		return;
@@ -691,7 +692,7 @@ static void _rtl92s_dm_init_dig(struct ieee80211_hw *hw)
 
 	/* for dig debug rssi value */
 	digtable->rssi_val = 50;
-	digtable->back_val = DM_DIG_BACKOFF;
+	digtable->back_val = DM_DIG_BACKOFF_MAX;
 	digtable->rx_gain_max = DM_DIG_MAX;
 
 	digtable->rx_gain_min = DM_DIG_MIN;
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/dm.h b/drivers/net/wireless/rtlwifi/rtl8192se/dm.h
index 2e9052c8fe4b..de6ac796c74d 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/dm.h
@@ -54,24 +54,6 @@ enum dm_dig_sta {
 	DM_STA_DIG_MAX
 };
 
-enum dm_dig_connect {
-	DIG_STA_DISCONNECT = 0,
-	DIG_STA_CONNECT = 1,
-	DIG_STA_BEFORE_CONNECT = 2,
-	DIG_AP_DISCONNECT = 3,
-	DIG_AP_CONNECT = 4,
-	DIG_AP_ADD_STATION = 5,
-	DIG_CONNECT_MAX
-};
-
-enum dm_dig_ext_port_alg {
-	DIG_EXT_PORT_STAGE_0 = 0,
-	DIG_EXT_PORT_STAGE_1 = 1,
-	DIG_EXT_PORT_STAGE_2 = 2,
-	DIG_EXT_PORT_STAGE_3 = 3,
-	DIG_EXT_PORT_STAGE_MAX = 4,
-};
-
 enum dm_ratr_sta {
 	DM_RATR_STA_HIGH = 0,
 	DM_RATR_STA_MIDDLEHIGH = 1,
@@ -99,22 +81,12 @@ enum dm_ratr_sta {
 #define	TX_POWER_NEAR_FIELD_THRESH_LVL2	74
 #define	TX_POWER_NEAR_FIELD_THRESH_LVL1	67
 
-#define DM_DIG_THRESH_HIGH		40
-#define DM_DIG_THRESH_LOW		35
-#define	DM_FALSEALARM_THRESH_LOW	40
-#define	DM_FALSEALARM_THRESH_HIGH	1000
 #define	DM_DIG_HIGH_PWR_THRESH_HIGH	75
 #define	DM_DIG_HIGH_PWR_THRESH_LOW	70
-#define	DM_DIG_BACKOFF			12
-#define	DM_DIG_MAX			0x3e
-#define	DM_DIG_MIN			0x1c
 #define	DM_DIG_MIN_Netcore		0x12
-#define	DM_DIG_BACKOFF_MAX		12
-#define	DM_DIG_BACKOFF_MIN		-4
 
 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);
 
 #endif
-
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/sw.c b/drivers/net/wireless/rtlwifi/rtl8192se/sw.c
index fb003868bdef..e1fd27c888bf 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/sw.c
@@ -383,21 +383,21 @@ static struct rtl_hal_cfg rtl92se_hal_cfg = {
 	.maps[RTL_IMR_ROK] = IMR_ROK,
 	.maps[RTL_IBSS_INT_MASKS] = (IMR_BCNINT | IMR_TBDOK | IMR_TBDER),
 
-	.maps[RTL_RC_CCK_RATE1M] = DESC92_RATE1M,
-	.maps[RTL_RC_CCK_RATE2M] = DESC92_RATE2M,
-	.maps[RTL_RC_CCK_RATE5_5M] = DESC92_RATE5_5M,
-	.maps[RTL_RC_CCK_RATE11M] = DESC92_RATE11M,
-	.maps[RTL_RC_OFDM_RATE6M] = DESC92_RATE6M,
-	.maps[RTL_RC_OFDM_RATE9M] = DESC92_RATE9M,
-	.maps[RTL_RC_OFDM_RATE12M] = DESC92_RATE12M,
-	.maps[RTL_RC_OFDM_RATE18M] = DESC92_RATE18M,
-	.maps[RTL_RC_OFDM_RATE24M] = DESC92_RATE24M,
-	.maps[RTL_RC_OFDM_RATE36M] = DESC92_RATE36M,
-	.maps[RTL_RC_OFDM_RATE48M] = DESC92_RATE48M,
-	.maps[RTL_RC_OFDM_RATE54M] = DESC92_RATE54M,
-
-	.maps[RTL_RC_HT_RATEMCS7] = DESC92_RATEMCS7,
-	.maps[RTL_RC_HT_RATEMCS15] = DESC92_RATEMCS15,
+	.maps[RTL_RC_CCK_RATE1M] = DESC_RATE1M,
+	.maps[RTL_RC_CCK_RATE2M] = DESC_RATE2M,
+	.maps[RTL_RC_CCK_RATE5_5M] = DESC_RATE5_5M,
+	.maps[RTL_RC_CCK_RATE11M] = DESC_RATE11M,
+	.maps[RTL_RC_OFDM_RATE6M] = DESC_RATE6M,
+	.maps[RTL_RC_OFDM_RATE9M] = DESC_RATE9M,
+	.maps[RTL_RC_OFDM_RATE12M] = DESC_RATE12M,
+	.maps[RTL_RC_OFDM_RATE18M] = DESC_RATE18M,
+	.maps[RTL_RC_OFDM_RATE24M] = DESC_RATE24M,
+	.maps[RTL_RC_OFDM_RATE36M] = DESC_RATE36M,
+	.maps[RTL_RC_OFDM_RATE48M] = DESC_RATE48M,
+	.maps[RTL_RC_OFDM_RATE54M] = DESC_RATE54M,
+
+	.maps[RTL_RC_HT_RATEMCS7] = DESC_RATEMCS7,
+	.maps[RTL_RC_HT_RATEMCS15] = DESC_RATEMCS15,
 };
 
 static struct pci_device_id rtl92se_pci_ids[] = {
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/trx.c b/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
index 672fd3b02835..125b29bd2f93 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
@@ -191,8 +191,8 @@ static void _rtl92se_query_rxphystatus(struct ieee80211_hw *hw,
 		pstats->rxpower = rx_pwr_all;
 		pstats->recvsignalpower = rx_pwr_all;
 
-		if (pstats->is_ht && pstats->rate >= DESC92_RATEMCS8 &&
-		    pstats->rate <= DESC92_RATEMCS15)
+		if (pstats->is_ht && pstats->rate >= DESC_RATEMCS8 &&
+		    pstats->rate <= DESC_RATEMCS15)
 			max_spatial_stream = 2;
 		else
 			max_spatial_stream = 1;
@@ -264,7 +264,6 @@ bool rtl92se_rx_query_desc(struct ieee80211_hw *hw, struct rtl_stats *stats,
 	struct rx_fwinfo *p_drvinfo;
 	u32 phystatus = (u32)GET_RX_STATUS_DESC_PHY_STATUS(pdesc);
 	struct ieee80211_hdr *hdr;
-	bool first_ampdu = false;
 
 	stats->length = (u16)GET_RX_STATUS_DESC_PKT_LEN(pdesc);
 	stats->rx_drvinfo_size = (u8)GET_RX_STATUS_DESC_DRVINFO_SIZE(pdesc) * 8;
@@ -319,8 +318,8 @@ bool rtl92se_rx_query_desc(struct ieee80211_hw *hw, struct rtl_stats *stats,
 			rx_status->flag |= RX_FLAG_DECRYPTED;
 	}
 
-	rx_status->rate_idx = rtlwifi_rate_mapping(hw,
-			      stats->is_ht, stats->rate, first_ampdu);
+	rx_status->rate_idx = rtlwifi_rate_mapping(hw, stats->is_ht,
+						   false, stats->rate);
 
 	rx_status->mactime = stats->timestamp_low;
 	if (phystatus) {
@@ -394,14 +393,14 @@ void rtl92se_tx_fill_desc(struct ieee80211_hw *hw,
 		SET_TX_DESC_RSVD_MACID(pdesc, reserved_macid);
 
 		SET_TX_DESC_TXHT(pdesc, ((ptcb_desc->hw_rate >=
-				 DESC92_RATEMCS0) ? 1 : 0));
+				 DESC_RATEMCS0) ? 1 : 0));
 
 		if (rtlhal->version == VERSION_8192S_ACUT) {
-			if (ptcb_desc->hw_rate == DESC92_RATE1M ||
-				ptcb_desc->hw_rate  == DESC92_RATE2M ||
-				ptcb_desc->hw_rate == DESC92_RATE5_5M ||
-				ptcb_desc->hw_rate == DESC92_RATE11M) {
-				ptcb_desc->hw_rate = DESC92_RATE12M;
+			if (ptcb_desc->hw_rate == DESC_RATE1M ||
+			    ptcb_desc->hw_rate  == DESC_RATE2M ||
+			    ptcb_desc->hw_rate == DESC_RATE5_5M ||
+			    ptcb_desc->hw_rate == DESC_RATE11M) {
+				ptcb_desc->hw_rate = DESC_RATE12M;
 			}
 		}
 
@@ -430,7 +429,7 @@ void rtl92se_tx_fill_desc(struct ieee80211_hw *hw,
 		SET_TX_DESC_RTS_BANDWIDTH(pdesc, 0);
 		SET_TX_DESC_RTS_SUB_CARRIER(pdesc, ptcb_desc->rts_sc);
 		SET_TX_DESC_RTS_SHORT(pdesc, ((ptcb_desc->rts_rate <=
-		       DESC92_RATE54M) ?
+		       DESC_RATE54M) ?
 		       (ptcb_desc->rts_use_shortpreamble ? 1 : 0)
 		       : (ptcb_desc->rts_use_shortgi ? 1 : 0)));
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/dm.c b/drivers/net/wireless/rtlwifi/rtl8723ae/dm.c
index a0e86922780a..4c1c96c96a5a 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/dm.c
@@ -26,6 +26,7 @@
 #include "../wifi.h"
 #include "../base.h"
 #include "../pci.h"
+#include "../core.h"
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
@@ -146,31 +147,6 @@ static const u8 cckswing_table_ch14[CCK_TABLE_SIZE][8] = {
 	{0x09, 0x08, 0x07, 0x04, 0x00, 0x00, 0x00, 0x00}
 };
 
-static void rtl8723e_dm_diginit(struct ieee80211_hw *hw)
-{
-	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_cstate = DIG_STA_DISCONNECT;
-	dm_digtable->presta_cstate = DIG_STA_DISCONNECT;
-	dm_digtable->curmultista_cstate = 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_max = DM_DIG_MAX;
-	dm_digtable->rx_gain_min = DM_DIG_MIN;
-	dm_digtable->back_val = DM_DIG_BACKOFF_DEFAULT;
-	dm_digtable->back_range_max = DM_DIG_BACKOFF_MAX;
-	dm_digtable->back_range_min = DM_DIG_BACKOFF_MIN;
-	dm_digtable->pre_cck_pd_state = CCK_PD_STAGE_MAX;
-	dm_digtable->cur_cck_pd_state = CCK_PD_STAGE_MAX;
-}
-
 static u8 rtl8723e_dm_initial_gain_min_pwdb(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -395,30 +371,30 @@ static void rtl8723e_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
 	if (dm_digtable->cursta_cstate == DIG_STA_CONNECT) {
 		dm_digtable->rssi_val_min = rtl8723e_dm_initial_gain_min_pwdb(hw);
 
-		if (dm_digtable->pre_cck_pd_state == CCK_PD_STAGE_LowRssi) {
+		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;
+				    CCK_PD_STAGE_LOWRSSI;
 			else
 				dm_digtable->cur_cck_pd_state =
-				    CCK_PD_STAGE_HighRssi;
+				    CCK_PD_STAGE_HIGHRSSI;
 		} else {
 			if (dm_digtable->rssi_val_min <= 20)
 				dm_digtable->cur_cck_pd_state =
-				    CCK_PD_STAGE_LowRssi;
+				    CCK_PD_STAGE_LOWRSSI;
 			else
 				dm_digtable->cur_cck_pd_state =
-				    CCK_PD_STAGE_HighRssi;
+				    CCK_PD_STAGE_HIGHRSSI;
 		}
 	} else {
 		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->cur_cck_pd_state == CCK_PD_STAGE_LOWRSSI) {
 			if (rtlpriv->falsealm_cnt.cnt_cck_fail > 800)
 				dm_digtable->cur_cck_fa_state =
-				    CCK_FA_STAGE_High;
+				    CCK_FA_STAGE_HIGH;
 			else
 				dm_digtable->cur_cck_fa_state =
 				    CCK_FA_STAGE_LOW;
@@ -818,7 +794,7 @@ void rtl8723e_dm_init(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
-	rtl8723e_dm_diginit(hw);
+	rtl_dm_diginit(hw, 0x20);
 	rtl8723_dm_init_dynamic_txpower(hw);
 	rtl8723_dm_init_edca_turbo(hw);
 	rtl8723e_dm_init_rate_adaptive_mask(hw);
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/dm.h b/drivers/net/wireless/rtlwifi/rtl8723ae/dm.h
index 6fa0feb05f6d..57111052e86b 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/dm.h
@@ -42,25 +42,12 @@
 #define BW_AUTO_SWITCH_HIGH_LOW			25
 #define BW_AUTO_SWITCH_LOW_HIGH			30
 
-#define DM_DIG_THRESH_HIGH			40
-#define DM_DIG_THRESH_LOW			35
-
-#define DM_FALSEALARM_THRESH_LOW		400
-#define DM_FALSEALARM_THRESH_HIGH		1000
-
-#define DM_DIG_MAX				0x3e
-#define DM_DIG_MIN				0x1e
-
 #define DM_DIG_FA_UPPER				0x32
 #define DM_DIG_FA_LOWER				0x20
 #define DM_DIG_FA_TH0				0x20
 #define DM_DIG_FA_TH1				0x100
 #define DM_DIG_FA_TH2				0x200
 
-#define DM_DIG_BACKOFF_MAX			12
-#define DM_DIG_BACKOFF_MIN			-4
-#define DM_DIG_BACKOFF_DEFAULT			10
-
 #define RXPATHSELECTION_SS_TH_LOW		30
 #define RXPATHSELECTION_DIFF_TH			18
 
@@ -108,14 +95,6 @@ enum tag_dynamic_init_gain_operation_type_definition {
 	DIG_OP_TYPE_MAX
 };
 
-enum tag_cck_packet_detection_threshold_type_definition {
-	CCK_PD_STAGE_LowRssi = 0,
-	CCK_PD_STAGE_HighRssi = 1,
-	CCK_FA_STAGE_LOW = 2,
-	CCK_FA_STAGE_High = 3,
-	CCK_PD_STAGE_MAX = 4,
-};
-
 enum dm_1r_cca_e {
 	CCA_1R = 0,
 	CCA_2R = 1,
@@ -134,23 +113,6 @@ enum dm_sw_ant_switch_e {
 	ANS_ANTENNA_MAX = 3,
 };
 
-enum dm_dig_ext_port_alg_e {
-	DIG_EXT_PORT_STAGE_0 = 0,
-	DIG_EXT_PORT_STAGE_1 = 1,
-	DIG_EXT_PORT_STAGE_2 = 2,
-	DIG_EXT_PORT_STAGE_3 = 3,
-	DIG_EXT_PORT_STAGE_MAX = 4,
-};
-
-enum dm_dig_connect_e {
-	DIG_STA_DISCONNECT = 0,
-	DIG_STA_CONNECT = 1,
-	DIG_STA_BEFORE_CONNECT = 2,
-	DIG_MULTISTA_DISCONNECT = 3,
-	DIG_MULTISTA_CONNECT = 4,
-	DIG_CONNECT_MAX
-};
-
 #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)
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/trx.c b/drivers/net/wireless/rtlwifi/rtl8723ae/trx.c
index d372ccaf3465..2f7c144d7980 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/trx.c
@@ -45,164 +45,6 @@ static u8 _rtl8723e_map_hwqueue_to_fwqueue(struct sk_buff *skb, u8 hw_queue)
 	return skb->priority;
 }
 
-/* mac80211's rate_idx is like this:
- *
- * 2.4G band:rx_status->band == IEEE80211_BAND_2GHZ
- *
- * B/G rate:
- * (rx_status->flag & RX_FLAG_HT) = 0,
- * DESC92C_RATE1M-->DESC92C_RATE54M ==> idx is 0-->11,
- *
- * N rate:
- * (rx_status->flag & RX_FLAG_HT) = 1,
- * DESC92C_RATEMCS0-->DESC92C_RATEMCS15 ==> idx is 0-->15
- *
- * 5G band:rx_status->band == IEEE80211_BAND_5GHZ
- * A rate:
- * (rx_status->flag & RX_FLAG_HT) = 0,
- * DESC92C_RATE6M-->DESC92C_RATE54M ==> idx is 0-->7,
- *
- * N rate:
- * (rx_status->flag & RX_FLAG_HT) = 1,
- * DESC92C_RATEMCS0-->DESC92C_RATEMCS15 ==> idx is 0-->15
- */
-static int _rtl8723e_rate_mapping(struct ieee80211_hw *hw,
-				  bool isht, u8 desc_rate)
-{
-	int rate_idx;
-
-	if (!isht) {
-		if (IEEE80211_BAND_2GHZ == hw->conf.chandef.chan->band) {
-			switch (desc_rate) {
-			case DESC92C_RATE1M:
-				rate_idx = 0;
-				break;
-			case DESC92C_RATE2M:
-				rate_idx = 1;
-				break;
-			case DESC92C_RATE5_5M:
-				rate_idx = 2;
-				break;
-			case DESC92C_RATE11M:
-				rate_idx = 3;
-				break;
-			case DESC92C_RATE6M:
-				rate_idx = 4;
-				break;
-			case DESC92C_RATE9M:
-				rate_idx = 5;
-				break;
-			case DESC92C_RATE12M:
-				rate_idx = 6;
-				break;
-			case DESC92C_RATE18M:
-				rate_idx = 7;
-				break;
-			case DESC92C_RATE24M:
-				rate_idx = 8;
-				break;
-			case DESC92C_RATE36M:
-				rate_idx = 9;
-				break;
-			case DESC92C_RATE48M:
-				rate_idx = 10;
-				break;
-			case DESC92C_RATE54M:
-				rate_idx = 11;
-				break;
-			default:
-				rate_idx = 0;
-				break;
-			}
-		} else {
-			switch (desc_rate) {
-			case DESC92C_RATE6M:
-				rate_idx = 0;
-				break;
-			case DESC92C_RATE9M:
-				rate_idx = 1;
-				break;
-			case DESC92C_RATE12M:
-				rate_idx = 2;
-				break;
-			case DESC92C_RATE18M:
-				rate_idx = 3;
-				break;
-			case DESC92C_RATE24M:
-				rate_idx = 4;
-				break;
-			case DESC92C_RATE36M:
-				rate_idx = 5;
-				break;
-			case DESC92C_RATE48M:
-				rate_idx = 6;
-				break;
-			case DESC92C_RATE54M:
-				rate_idx = 7;
-				break;
-			default:
-				rate_idx = 0;
-				break;
-			}
-		}
-	} else {
-		switch (desc_rate) {
-		case DESC92C_RATEMCS0:
-			rate_idx = 0;
-			break;
-		case DESC92C_RATEMCS1:
-			rate_idx = 1;
-			break;
-		case DESC92C_RATEMCS2:
-			rate_idx = 2;
-			break;
-		case DESC92C_RATEMCS3:
-			rate_idx = 3;
-			break;
-		case DESC92C_RATEMCS4:
-			rate_idx = 4;
-			break;
-		case DESC92C_RATEMCS5:
-			rate_idx = 5;
-			break;
-		case DESC92C_RATEMCS6:
-			rate_idx = 6;
-			break;
-		case DESC92C_RATEMCS7:
-			rate_idx = 7;
-			break;
-		case DESC92C_RATEMCS8:
-			rate_idx = 8;
-			break;
-		case DESC92C_RATEMCS9:
-			rate_idx = 9;
-			break;
-		case DESC92C_RATEMCS10:
-			rate_idx = 10;
-			break;
-		case DESC92C_RATEMCS11:
-			rate_idx = 11;
-			break;
-		case DESC92C_RATEMCS12:
-			rate_idx = 12;
-			break;
-		case DESC92C_RATEMCS13:
-			rate_idx = 13;
-			break;
-		case DESC92C_RATEMCS14:
-			rate_idx = 14;
-			break;
-		case DESC92C_RATEMCS15:
-			rate_idx = 15;
-			break;
-		default:
-			rate_idx = 0;
-			break;
-		}
-	}
-	return rate_idx;
-}
-
 static void _rtl8723e_query_rxphystatus(struct ieee80211_hw *hw,
 					struct rtl_stats *pstatus, u8 *pdesc,
 					struct rx_fwinfo_8723e *p_drvinfo,
@@ -503,8 +345,8 @@ bool rtl8723e_rx_query_desc(struct ieee80211_hw *hw,
 	 * are use (RX_FLAG_HT)
 	 * Notice: this is diff with windows define
 	 */
-	rx_status->rate_idx = _rtl8723e_rate_mapping(hw,
-				status->is_ht, status->rate);
+	rx_status->rate_idx = rtlwifi_rate_mapping(hw, status->is_ht,
+						   false, status->rate);
 
 	rx_status->mactime = status->timestamp_low;
 	if (phystatus == true) {
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/dm.c b/drivers/net/wireless/rtlwifi/rtl8723be/dm.c
index dd7eb4371f49..2367e8f47a5b 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/dm.c
@@ -26,6 +26,7 @@
 #include "../wifi.h"
 #include "../base.h"
 #include "../pci.h"
+#include "../core.h"
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
@@ -211,35 +212,6 @@ void rtl8723be_dm_txpower_track_adjust(struct ieee80211_hw *hw, u8 type,
 		(pwr_val << 16) | (pwr_val << 24);
 }
 
-static void rtl8723be_dm_diginit(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
-
-	dm_digtable->dig_enable_flag = true;
-	dm_digtable->cur_igvalue = rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f);
-	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_max = DM_DIG_MAX;
-	dm_digtable->rx_gain_min = DM_DIG_MIN;
-	dm_digtable->back_val = DM_DIG_BACKOFF_DEFAULT;
-	dm_digtable->back_range_max = DM_DIG_BACKOFF_MAX;
-	dm_digtable->back_range_min = DM_DIG_BACKOFF_MIN;
-	dm_digtable->pre_cck_cca_thres = 0xff;
-	dm_digtable->cur_cck_cca_thres = 0x83;
-	dm_digtable->forbidden_igi = DM_DIG_MIN;
-	dm_digtable->large_fa_hit = 0;
-	dm_digtable->recover_cnt = 0;
-	dm_digtable->dig_dynamic_min = DM_DIG_MIN;
-	dm_digtable->dig_dynamic_min_1 = DM_DIG_MIN;
-	dm_digtable->media_connect_0 = false;
-	dm_digtable->media_connect_1 = false;
-	rtlpriv->dm.dm_initialgain_enable = true;
-	dm_digtable->bt30_cur_igi = 0x32;
-}
-
 void rtl8723be_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -293,9 +265,10 @@ static void rtl8723be_dm_init_dynamic_atc_switch(struct ieee80211_hw *hw)
 void rtl8723be_dm_init(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 cur_igvalue = rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f);
 
 	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
-	rtl8723be_dm_diginit(hw);
+	rtl_dm_diginit(hw, cur_igvalue);
 	rtl8723be_dm_init_rate_adaptive_mask(hw);
 	rtl8723_dm_init_edca_turbo(hw);
 	rtl8723_dm_init_dynamic_bb_powersaving(hw);
@@ -424,7 +397,7 @@ static void rtl8723be_dm_dig(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));
-	u8 dig_dynamic_min, dig_maxofmin;
+	u8 dig_min_0, dig_maxofmin;
 	bool bfirstconnect, bfirstdisconnect;
 	u8 dm_dig_max, dm_dig_min;
 	u8 current_igi = dm_digtable->cur_igvalue;
@@ -434,7 +407,7 @@ static void rtl8723be_dm_dig(struct ieee80211_hw *hw)
 	if (mac->act_scanning)
 		return;
 
-	dig_dynamic_min = dm_digtable->dig_dynamic_min;
+	dig_min_0 = dm_digtable->dig_min_0;
 	bfirstconnect = (mac->link_state >= MAC80211_LINKED) &&
 			!dm_digtable->media_connect_0;
 	bfirstdisconnect = (mac->link_state < MAC80211_LINKED) &&
@@ -456,20 +429,20 @@ static void rtl8723be_dm_dig(struct ieee80211_hw *hw)
 		if (rtlpriv->dm.one_entry_only) {
 			offset = 12;
 			if (dm_digtable->rssi_val_min - offset < dm_dig_min)
-				dig_dynamic_min = dm_dig_min;
+				dig_min_0 = dm_dig_min;
 			else if (dm_digtable->rssi_val_min - offset >
 							dig_maxofmin)
-				dig_dynamic_min = dig_maxofmin;
+				dig_min_0 = dig_maxofmin;
 			else
-				dig_dynamic_min =
+				dig_min_0 =
 					dm_digtable->rssi_val_min - offset;
 		} else {
-			dig_dynamic_min = dm_dig_min;
+			dig_min_0 = dm_dig_min;
 		}
 
 	} else {
 		dm_digtable->rx_gain_max = dm_dig_max;
-		dig_dynamic_min = dm_dig_min;
+		dig_min_0 = dm_dig_min;
 		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "no link\n");
 	}
 
@@ -497,11 +470,11 @@ static void rtl8723be_dm_dig(struct ieee80211_hw *hw)
 		} else {
 			if (dm_digtable->large_fa_hit < 3) {
 				if ((dm_digtable->forbidden_igi - 1) <
-				     dig_dynamic_min) {
+				     dig_min_0) {
 					dm_digtable->forbidden_igi =
-							dig_dynamic_min;
+							dig_min_0;
 					dm_digtable->rx_gain_min =
-							dig_dynamic_min;
+							dig_min_0;
 				} else {
 					dm_digtable->forbidden_igi--;
 					dm_digtable->rx_gain_min =
@@ -552,7 +525,7 @@ static void rtl8723be_dm_dig(struct ieee80211_hw *hw)
 	rtl8723be_dm_write_dig(hw, current_igi);
 	dm_digtable->media_connect_0 =
 		((mac->link_state >= MAC80211_LINKED) ? true : false);
-	dm_digtable->dig_dynamic_min = dig_dynamic_min;
+	dm_digtable->dig_min_0 = dig_min_0;
 }
 
 static void rtl8723be_dm_false_alarm_counter_statistics(
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/dm.h b/drivers/net/wireless/rtlwifi/rtl8723be/dm.h
index e4c0e8ae6f47..f752a2cad63d 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/dm.h
@@ -180,28 +180,12 @@
 #define BW_AUTO_SWITCH_HIGH_LOW			25
 #define BW_AUTO_SWITCH_LOW_HIGH			30
 
-#define DM_DIG_THRESH_HIGH			40
-#define DM_DIG_THRESH_LOW			35
-
-#define DM_FALSEALARM_THRESH_LOW		400
-#define DM_FALSEALARM_THRESH_HIGH		1000
-
-#define DM_DIG_MAX				0x3e
-#define DM_DIG_MIN				0x1e
-
-#define DM_DIG_MAX_AP				0x32
-#define DM_DIG_MIN_AP				0x20
-
 #define DM_DIG_FA_UPPER				0x3e
 #define DM_DIG_FA_LOWER				0x1e
 #define DM_DIG_FA_TH0				0x200
 #define DM_DIG_FA_TH1				0x300
 #define DM_DIG_FA_TH2				0x400
 
-#define DM_DIG_BACKOFF_MAX			12
-#define DM_DIG_BACKOFF_MIN			-4
-#define DM_DIG_BACKOFF_DEFAULT			10
-
 #define RXPATHSELECTION_SS_TH_LOW		30
 #define RXPATHSELECTION_DIFF_TH			18
 
@@ -252,23 +236,6 @@ enum dm_sw_ant_switch_e {
 	ANS_ANTENNA_MAX	= 3,
 };
 
-enum dm_dig_ext_port_alg_e {
-	DIG_EXT_PORT_STAGE_0	= 0,
-	DIG_EXT_PORT_STAGE_1	= 1,
-	DIG_EXT_PORT_STAGE_2	= 2,
-	DIG_EXT_PORT_STAGE_3	= 3,
-	DIG_EXT_PORT_STAGE_MAX	= 4,
-};
-
-enum dm_dig_connect_e {
-	DIG_STA_DISCONNECT	= 0,
-	DIG_STA_CONNECT		= 1,
-	DIG_STA_BEFORE_CONNECT	= 2,
-	DIG_MULTISTA_DISCONNECT	= 3,
-	DIG_MULTISTA_CONNECT	= 4,
-	DIG_CONNECT_MAX
-};
-
 enum pwr_track_control_method {
 	BBSWING,
 	TXAGC
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/phy.c b/drivers/net/wireless/rtlwifi/rtl8723be/phy.c
index 20dcc25c506c..b7b73cbe346d 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/phy.c
@@ -874,31 +874,6 @@ void rtl8723be_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
 		  ROFDM0_RXDETECTOR3, rtlphy->framesync);
 }
 
-void rtl8723be_phy_get_txpower_level(struct ieee80211_hw *hw, long *powerlevel)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &rtlpriv->phy;
-	u8 txpwr_level;
-	long txpwr_dbm;
-
-	txpwr_level = rtlphy->cur_cck_txpwridx;
-	txpwr_dbm = rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_B,
-						 txpwr_level);
-	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
-	if (rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G, txpwr_level) >
-	    txpwr_dbm)
-		txpwr_dbm =
-		    rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G,
-						 txpwr_level);
-	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
-	if (rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G,
-					 txpwr_level) > txpwr_dbm)
-		txpwr_dbm =
-		    rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G,
-						 txpwr_level);
-	*powerlevel = txpwr_dbm;
-}
-
 static u8 _rtl8723be_phy_get_ratesection_intxpower_byrate(enum radio_path path,
 							  u8 rate)
 {
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/phy.h b/drivers/net/wireless/rtlwifi/rtl8723be/phy.h
index 6339738a0e33..9021d4745ab7 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/phy.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/phy.h
@@ -114,8 +114,6 @@ bool rtl8723be_phy_mac_config(struct ieee80211_hw *hw);
 bool rtl8723be_phy_bb_config(struct ieee80211_hw *hw);
 bool rtl8723be_phy_rf_config(struct ieee80211_hw *hw);
 void rtl8723be_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw);
-void rtl8723be_phy_get_txpower_level(struct ieee80211_hw *hw,
-				     long *powerlevel);
 void rtl8723be_phy_set_txpower_level(struct ieee80211_hw *hw,
 				     u8 channel);
 void rtl8723be_phy_scan_operation_backup(struct ieee80211_hw *hw,
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/sw.c b/drivers/net/wireless/rtlwifi/rtl8723be/sw.c
index 223eb42992bd..1017f02d7bf7 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/sw.c
@@ -387,12 +387,14 @@ module_param_named(swlps, rtl8723be_mod_params.swctrl_lps, bool, 0444);
 module_param_named(fwlps, rtl8723be_mod_params.fwctrl_lps, bool, 0444);
 module_param_named(disable_watchdog, rtl8723be_mod_params.disable_watchdog,
 		   bool, 0444);
-MODULE_PARM_DESC(swenc, "using hardware crypto (default 0 [hardware])\n");
-MODULE_PARM_DESC(ips, "using no link power save (default 1 is open)\n");
-MODULE_PARM_DESC(fwlps, "using linked fw control power save (default 1 is open)\n");
+MODULE_PARM_DESC(swenc, "Set to 1 for software crypto (default 0)\n");
+MODULE_PARM_DESC(ips, "Set to 0 to not use link power save (default 1)\n");
+MODULE_PARM_DESC(swlps, "Set to 1 to use SW control power save (default 0)\n");
+MODULE_PARM_DESC(fwlps, "Set to 1 to use FW control power save (default 1)\n");
 MODULE_PARM_DESC(msi, "Set to 1 to use MSI interrupts mode (default 0)\n");
 MODULE_PARM_DESC(debug, "Set debug level (0-5) (default 0)");
-MODULE_PARM_DESC(disable_watchdog, "Set to 1 to disable the watchdog (default 0)\n");
+MODULE_PARM_DESC(disable_watchdog,
+		 "Set to 1 to disable the watchdog (default 0)\n");
 
 static SIMPLE_DEV_PM_OPS(rtlwifi_pm_ops, rtl_pci_suspend, rtl_pci_resume);
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/trx.c b/drivers/net/wireless/rtlwifi/rtl8723be/trx.c
index d6a1c70cb657..338ec9a9d09b 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/trx.c
@@ -47,164 +47,6 @@ static u8 _rtl8723be_map_hwqueue_to_fwqueue(struct sk_buff *skb, u8 hw_queue)
 	return skb->priority;
 }
 
-/* mac80211's rate_idx is like this:
- *
- * 2.4G band:rx_status->band == IEEE80211_BAND_2GHZ
- *
- * B/G rate:
- * (rx_status->flag & RX_FLAG_HT) = 0,
- * DESC92C_RATE1M-->DESC92C_RATE54M ==> idx is 0-->11,
- *
- * N rate:
- * (rx_status->flag & RX_FLAG_HT) = 1,
- * DESC92C_RATEMCS0-->DESC92C_RATEMCS15 ==> idx is 0-->15
- *
- * 5G band:rx_status->band == IEEE80211_BAND_5GHZ
- * A rate:
- * (rx_status->flag & RX_FLAG_HT) = 0,
- * DESC92C_RATE6M-->DESC92C_RATE54M ==> idx is 0-->7,
- *
- * N rate:
- * (rx_status->flag & RX_FLAG_HT) = 1,
- * DESC92C_RATEMCS0-->DESC92C_RATEMCS15 ==> idx is 0-->15
- */
-static int _rtl8723be_rate_mapping(struct ieee80211_hw *hw,
-				   bool isht, u8 desc_rate)
-{
-	int rate_idx;
-
-	if (!isht) {
-		if (IEEE80211_BAND_2GHZ == hw->conf.chandef.chan->band) {
-			switch (desc_rate) {
-			case DESC92C_RATE1M:
-				rate_idx = 0;
-				break;
-			case DESC92C_RATE2M:
-				rate_idx = 1;
-				break;
-			case DESC92C_RATE5_5M:
-				rate_idx = 2;
-				break;
-			case DESC92C_RATE11M:
-				rate_idx = 3;
-				break;
-			case DESC92C_RATE6M:
-				rate_idx = 4;
-				break;
-			case DESC92C_RATE9M:
-				rate_idx = 5;
-				break;
-			case DESC92C_RATE12M:
-				rate_idx = 6;
-				break;
-			case DESC92C_RATE18M:
-				rate_idx = 7;
-				break;
-			case DESC92C_RATE24M:
-				rate_idx = 8;
-				break;
-			case DESC92C_RATE36M:
-				rate_idx = 9;
-				break;
-			case DESC92C_RATE48M:
-				rate_idx = 10;
-				break;
-			case DESC92C_RATE54M:
-				rate_idx = 11;
-				break;
-			default:
-				rate_idx = 0;
-				break;
-			}
-		} else {
-			switch (desc_rate) {
-			case DESC92C_RATE6M:
-				rate_idx = 0;
-				break;
-			case DESC92C_RATE9M:
-				rate_idx = 1;
-				break;
-			case DESC92C_RATE12M:
-				rate_idx = 2;
-				break;
-			case DESC92C_RATE18M:
-				rate_idx = 3;
-				break;
-			case DESC92C_RATE24M:
-				rate_idx = 4;
-				break;
-			case DESC92C_RATE36M:
-				rate_idx = 5;
-				break;
-			case DESC92C_RATE48M:
-				rate_idx = 6;
-				break;
-			case DESC92C_RATE54M:
-				rate_idx = 7;
-				break;
-			default:
-				rate_idx = 0;
-				break;
-			}
-		}
-	} else {
-		switch (desc_rate) {
-		case DESC92C_RATEMCS0:
-			rate_idx = 0;
-			break;
-		case DESC92C_RATEMCS1:
-			rate_idx = 1;
-			break;
-		case DESC92C_RATEMCS2:
-			rate_idx = 2;
-			break;
-		case DESC92C_RATEMCS3:
-			rate_idx = 3;
-			break;
-		case DESC92C_RATEMCS4:
-			rate_idx = 4;
-			break;
-		case DESC92C_RATEMCS5:
-			rate_idx = 5;
-			break;
-		case DESC92C_RATEMCS6:
-			rate_idx = 6;
-			break;
-		case DESC92C_RATEMCS7:
-			rate_idx = 7;
-			break;
-		case DESC92C_RATEMCS8:
-			rate_idx = 8;
-			break;
-		case DESC92C_RATEMCS9:
-			rate_idx = 9;
-			break;
-		case DESC92C_RATEMCS10:
-			rate_idx = 10;
-			break;
-		case DESC92C_RATEMCS11:
-			rate_idx = 11;
-			break;
-		case DESC92C_RATEMCS12:
-			rate_idx = 12;
-			break;
-		case DESC92C_RATEMCS13:
-			rate_idx = 13;
-			break;
-		case DESC92C_RATEMCS14:
-			rate_idx = 14;
-			break;
-		case DESC92C_RATEMCS15:
-			rate_idx = 15;
-			break;
-		default:
-			rate_idx = 0;
-			break;
-		}
-	}
-	return rate_idx;
-}
-
 static void _rtl8723be_query_rxphystatus(struct ieee80211_hw *hw,
 					 struct rtl_stats *pstatus, u8 *pdesc,
 					 struct rx_fwinfo_8723be *p_drvinfo,
@@ -558,8 +400,8 @@ bool rtl8723be_rx_query_desc(struct ieee80211_hw *hw,
 	 * supported rates or MCS index if HT rates
 	 * are use (RX_FLAG_HT)
 	 */
-	rx_status->rate_idx = _rtl8723be_rate_mapping(hw, status->is_ht,
-						      status->rate);
+	rx_status->rate_idx = rtlwifi_rate_mapping(hw, status->is_ht,
+						   false, status->rate);
 
 	rx_status->mactime = status->timestamp_low;
 	if (phystatus) {
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/def.h b/drivers/net/wireless/rtlwifi/rtl8821ae/def.h
index a730985ae81d..ee7c208bd070 100644
--- a/drivers/net/wireless/rtlwifi/rtl8821ae/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/def.h
@@ -373,60 +373,6 @@ enum rtl_desc_qsel {
 	QSLT_CMD = 0x13,
 };
 
-enum rtl_desc8821ae_rate {
-	DESC_RATE1M = 0x00,
-	DESC_RATE2M = 0x01,
-	DESC_RATE5_5M = 0x02,
-	DESC_RATE11M = 0x03,
-
-	DESC_RATE6M = 0x04,
-	DESC_RATE9M = 0x05,
-	DESC_RATE12M = 0x06,
-	DESC_RATE18M = 0x07,
-	DESC_RATE24M = 0x08,
-	DESC_RATE36M = 0x09,
-	DESC_RATE48M = 0x0a,
-	DESC_RATE54M = 0x0b,
-
-	DESC_RATEMCS0 = 0x0c,
-	DESC_RATEMCS1 = 0x0d,
-	DESC_RATEMCS2 = 0x0e,
-	DESC_RATEMCS3 = 0x0f,
-	DESC_RATEMCS4 = 0x10,
-	DESC_RATEMCS5 = 0x11,
-	DESC_RATEMCS6 = 0x12,
-	DESC_RATEMCS7 = 0x13,
-	DESC_RATEMCS8 = 0x14,
-	DESC_RATEMCS9 = 0x15,
-	DESC_RATEMCS10 = 0x16,
-	DESC_RATEMCS11 = 0x17,
-	DESC_RATEMCS12 = 0x18,
-	DESC_RATEMCS13 = 0x19,
-	DESC_RATEMCS14 = 0x1a,
-	DESC_RATEMCS15 = 0x1b,
-
-	DESC_RATEVHT1SS_MCS0 = 0x2c,
-	DESC_RATEVHT1SS_MCS1 = 0x2d,
-	DESC_RATEVHT1SS_MCS2 = 0x2e,
-	DESC_RATEVHT1SS_MCS3 = 0x2f,
-	DESC_RATEVHT1SS_MCS4 = 0x30,
-	DESC_RATEVHT1SS_MCS5 = 0x31,
-	DESC_RATEVHT1SS_MCS6 = 0x32,
-	DESC_RATEVHT1SS_MCS7 = 0x33,
-	DESC_RATEVHT1SS_MCS8 = 0x34,
-	DESC_RATEVHT1SS_MCS9 = 0x35,
-	DESC_RATEVHT2SS_MCS0 = 0x36,
-	DESC_RATEVHT2SS_MCS1 = 0x37,
-	DESC_RATEVHT2SS_MCS2 = 0x38,
-	DESC_RATEVHT2SS_MCS3 = 0x39,
-	DESC_RATEVHT2SS_MCS4 = 0x3a,
-	DESC_RATEVHT2SS_MCS5 = 0x3b,
-	DESC_RATEVHT2SS_MCS6 = 0x3c,
-	DESC_RATEVHT2SS_MCS7 = 0x3d,
-	DESC_RATEVHT2SS_MCS8 = 0x3e,
-	DESC_RATEVHT2SS_MCS9 = 0x3f,
-};
-
 enum rx_packet_type {
 	NORMAL_RX,
 	TX_REPORT1,
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/dm.c b/drivers/net/wireless/rtlwifi/rtl8821ae/dm.c
index 9be106109921..0b2082dc48f1 100644
--- a/drivers/net/wireless/rtlwifi/rtl8821ae/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/dm.c
@@ -26,6 +26,7 @@
 #include "../wifi.h"
 #include "../base.h"
 #include "../pci.h"
+#include "../core.h"
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
@@ -519,34 +520,6 @@ void rtl8821ae_dm_initialize_txpower_tracking_thermalmeter(
 	}
 }
 
-static void rtl8821ae_dm_diginit(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
-
-	dm_digtable->cur_igvalue = rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f);
-	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_max = DM_DIG_MAX;
-	dm_digtable->rx_gain_min = DM_DIG_MIN;
-	dm_digtable->back_val = DM_DIG_BACKOFF_DEFAULT;
-	dm_digtable->back_range_max = DM_DIG_BACKOFF_MAX;
-	dm_digtable->back_range_min = DM_DIG_BACKOFF_MIN;
-	dm_digtable->pre_cck_cca_thres = 0xff;
-	dm_digtable->cur_cck_cca_thres = 0x83;
-	dm_digtable->forbidden_igi = DM_DIG_MIN;
-	dm_digtable->large_fa_hit = 0;
-	dm_digtable->recover_cnt = 0;
-	dm_digtable->dig_dynamic_min = DM_DIG_MIN;
-	dm_digtable->dig_dynamic_min_1 = DM_DIG_MIN;
-	dm_digtable->media_connect_0 = false;
-	dm_digtable->media_connect_1 = false;
-	rtlpriv->dm.dm_initialgain_enable = true;
-	dm_digtable->bt30_cur_igi = 0x32;
-}
-
 void rtl8821ae_dm_init_edca_turbo(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -606,6 +579,7 @@ void rtl8821ae_dm_init(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u32 cur_igvalue = rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f);
 
 	spin_lock(&rtlpriv->locks.iqk_lock);
 	rtlphy->lck_inprogress = false;
@@ -613,7 +587,7 @@ void rtl8821ae_dm_init(struct ieee80211_hw *hw)
 
 	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
 	rtl8821ae_dm_common_info_self_init(hw);
-	rtl8821ae_dm_diginit(hw);
+	rtl_dm_diginit(hw, cur_igvalue);
 	rtl8821ae_dm_init_rate_adaptive_mask(hw);
 	rtl8821ae_dm_init_edca_turbo(hw);
 	rtl8821ae_dm_initialize_txpower_tracking_thermalmeter(hw);
@@ -822,7 +796,7 @@ static void rtl8821ae_dm_dig(struct ieee80211_hw *hw)
 	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 dig_dynamic_min;
+	u8 dig_min_0;
 	u8 dig_max_of_min;
 	bool first_connect, first_disconnect;
 	u8 dm_dig_max, dm_dig_min, offset;
@@ -837,7 +811,7 @@ static void rtl8821ae_dm_dig(struct ieee80211_hw *hw)
 	}
 
 	/*add by Neil Chen to avoid PSD is processing*/
-	dig_dynamic_min = dm_digtable->dig_dynamic_min;
+	dig_min_0 = dm_digtable->dig_min_0;
 	first_connect = (mac->link_state >= MAC80211_LINKED) &&
 			(!dm_digtable->media_connect_0);
 	first_disconnect = (mac->link_state < MAC80211_LINKED) &&
@@ -876,23 +850,23 @@ static void rtl8821ae_dm_dig(struct ieee80211_hw *hw)
 			offset = 0;
 
 			if (dm_digtable->rssi_val_min - offset < dm_dig_min)
-				dig_dynamic_min = dm_dig_min;
+				dig_min_0 = dm_dig_min;
 			else if (dm_digtable->rssi_val_min -
 				offset > dig_max_of_min)
-				dig_dynamic_min = dig_max_of_min;
+				dig_min_0 = dig_max_of_min;
 			else
-				dig_dynamic_min =
+				dig_min_0 =
 					dm_digtable->rssi_val_min - offset;
 
 			RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
-				 "bOneEntryOnly=TRUE, dig_dynamic_min=0x%x\n",
-				 dig_dynamic_min);
+				 "bOneEntryOnly=TRUE, dig_min_0=0x%x\n",
+				 dig_min_0);
 		} else {
-			dig_dynamic_min = dm_dig_min;
+			dig_min_0 = dm_dig_min;
 		}
 	} else {
 		dm_digtable->rx_gain_max = dm_dig_max;
-		dig_dynamic_min = dm_dig_min;
+		dig_min_0 = dm_dig_min;
 		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
 			 "No Link\n");
 	}
@@ -925,11 +899,11 @@ static void rtl8821ae_dm_dig(struct ieee80211_hw *hw)
 		} else {
 			if (dm_digtable->large_fa_hit < 3) {
 				if ((dm_digtable->forbidden_igi - 1) <
-				    dig_dynamic_min) {
+				    dig_min_0) {
 					dm_digtable->forbidden_igi =
-						dig_dynamic_min;
+						dig_min_0;
 					dm_digtable->rx_gain_min =
-						dig_dynamic_min;
+						dig_min_0;
 					RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
 						 "Normal Case: At Lower Bound\n");
 				} else {
@@ -1024,7 +998,7 @@ static void rtl8821ae_dm_dig(struct ieee80211_hw *hw)
 	rtl8821ae_dm_write_dig(hw, current_igi);
 	dm_digtable->media_connect_0 =
 		((mac->link_state >= MAC80211_LINKED) ? true : false);
-	dm_digtable->dig_dynamic_min = dig_dynamic_min;
+	dm_digtable->dig_min_0 = dig_min_0;
 }
 
 static void rtl8821ae_dm_common_info_self_update(struct ieee80211_hw *hw)
@@ -2078,8 +2052,7 @@ void rtl8821ae_dm_txpwr_track_set_pwr(struct ieee80211_hw *hw,
 	if (rtldm->tx_rate != 0xFF)
 		tx_rate = rtl8821ae_hw_rate_to_mrate(hw, rtldm->tx_rate);
 
-	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-		 "===>rtl8812ae_dm_txpwr_track_set_pwr\n");
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD, "===>%s\n", __func__);
 
 	if (tx_rate != 0xFF) { /* Mimic Modify High Rate BBSwing Limit.*/
 		/*CCK*/
@@ -2128,7 +2101,7 @@ void rtl8821ae_dm_txpwr_track_set_pwr(struct ieee80211_hw *hw,
 
 	if (method == BBSWING) {
 		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-			 "===>rtl8812ae_dm_txpwr_track_set_pwr\n");
+			 "===>%s\n", __func__);
 		if (rf_path == RF90_PATH_A) {
 			final_swing_idx[RF90_PATH_A] =
 				(rtldm->ofdm_index[RF90_PATH_A] >
@@ -2260,7 +2233,8 @@ void rtl8821ae_dm_txpower_tracking_callback_thermalmeter(
 	rtldm->txpower_trackinginit = true;
 
 	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-		 "===>rtl8812ae_dm_txpower_tracking_callback_thermalmeter,\n pDM_Odm->BbSwingIdxCckBase: %d,pDM_Odm->BbSwingIdxOfdmBase[A]:%d, pDM_Odm->DefaultOfdmIndex: %d\n",
+		 "===>%s,\n pDM_Odm->BbSwingIdxCckBase: %d,pDM_Odm->BbSwingIdxOfdmBase[A]:%d, pDM_Odm->DefaultOfdmIndex: %d\n",
+		 __func__,
 		 rtldm->swing_idx_cck_base,
 		 rtldm->swing_idx_ofdm_base[RF90_PATH_A],
 		 rtldm->default_ofdm_index);
@@ -2539,8 +2513,7 @@ void rtl8821ae_dm_txpower_tracking_callback_thermalmeter(
 		}
 	}
 
-	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-		 "<===rtl8812ae_dm_txpower_tracking_callback_thermalmeter\n");
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD, "<===%s\n", __func__);
 }
 
 void rtl8821ae_dm_check_txpower_tracking_thermalmeter(struct ieee80211_hw *hw)
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/dm.h b/drivers/net/wireless/rtlwifi/rtl8821ae/dm.h
index 9dd40dd316c1..625a6bbb21fc 100644
--- a/drivers/net/wireless/rtlwifi/rtl8821ae/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/dm.h
@@ -187,28 +187,12 @@
 #define BW_AUTO_SWITCH_HIGH_LOW		25
 #define BW_AUTO_SWITCH_LOW_HIGH		30
 
-#define DM_DIG_THRESH_HIGH			40
-#define DM_DIG_THRESH_LOW			35
-
-#define DM_FALSEALARM_THRESH_LOW	400
-#define DM_FALSEALARM_THRESH_HIGH	1000
-
-#define DM_DIG_MAX					0x3e
-#define DM_DIG_MIN					0x1e
-
-#define DM_DIG_MAX_AP				0x32
-#define DM_DIG_MIN_AP				0x20
-
 #define DM_DIG_FA_UPPER				0x3e
 #define DM_DIG_FA_LOWER				0x1e
 #define DM_DIG_FA_TH0				200
 #define DM_DIG_FA_TH1				0x300
 #define DM_DIG_FA_TH2				0x400
 
-#define DM_DIG_BACKOFF_MAX			12
-#define DM_DIG_BACKOFF_MIN			-4
-#define DM_DIG_BACKOFF_DEFAULT		10
-
 #define RXPATHSELECTION_SS_TH_LOW	30
 #define RXPATHSELECTION_DIFF_TH		18
 
@@ -262,14 +246,6 @@ enum tag_dynamic_init_gain_operation_type_definition {
 	DIG_OP_TYPE_MAX
 };
 
-enum tag_cck_packet_detection_threshold_type_definition {
-	CCK_PD_STAGE_LOWRSSI = 0,
-	CCK_PD_STAGE_HIGHRSSI = 1,
-	CCK_FA_STAGE_LOW = 2,
-	CCK_FA_STAGE_HIGH = 3,
-	CCK_PD_STAGE_MAX = 4,
-};
-
 enum dm_1r_cca_e {
 	CCA_1R = 0,
 	CCA_2R = 1,
@@ -288,23 +264,6 @@ enum dm_sw_ant_switch_e {
 	ANS_ANTENNA_MAX = 3,
 };
 
-enum dm_dig_ext_port_alg_e {
-	DIG_EXT_PORT_STAGE_0 = 0,
-	DIG_EXT_PORT_STAGE_1 = 1,
-	DIG_EXT_PORT_STAGE_2 = 2,
-	DIG_EXT_PORT_STAGE_3 = 3,
-	DIG_EXT_PORT_STAGE_MAX = 4,
-};
-
-enum dm_dig_connect_e {
-	DIG_STA_DISCONNECT = 0,
-	DIG_STA_CONNECT = 1,
-	DIG_STA_BEFORE_CONNECT = 2,
-	DIG_MULTISTA_DISCONNECT = 3,
-	DIG_MULTISTA_CONNECT = 4,
-	DIG_CONNECT_MAX
-};
-
 enum pwr_track_control_method {
 	BBSWING,
 	TXAGC,
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/pwrseq.h b/drivers/net/wireless/rtlwifi/rtl8821ae/pwrseq.h
index bf0b0ce9519c..36b3e91d996e 100644
--- a/drivers/net/wireless/rtlwifi/rtl8821ae/pwrseq.h
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/pwrseq.h
@@ -93,9 +93,9 @@
 
 #define RTL8812_TRANS_CARDEMU_TO_SUS					\
 	{0x0042, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xF0, 0xcc}, \
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xF0, 0xc0}, \
 	{0x0042, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xF0, 0xEC}, \
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xF0, 0xE0}, \
 	{0x0043, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
 	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x07 \
 	/* gpio11 input mode, gpio10~8 output mode */},	\
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/sw.c b/drivers/net/wireless/rtlwifi/rtl8821ae/sw.c
index fc92dd6a0d07..a4988121e1ab 100644
--- a/drivers/net/wireless/rtlwifi/rtl8821ae/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/sw.c
@@ -85,52 +85,6 @@ static void rtl8821ae_init_aspm_vars(struct ieee80211_hw *hw)
 	rtlpci->const_support_pciaspm = 1;
 }
 
-static void load_wowlan_fw(struct rtl_priv *rtlpriv)
-{
-	/* callback routine to load wowlan firmware after main fw has
-	 * been loaded
-	 */
-	const struct firmware *wowlan_firmware;
-	char *fw_name = NULL;
-	int err;
-
-	/* for wowlan firmware buf */
-	rtlpriv->rtlhal.wowlan_firmware = vzalloc(0x8000);
-	if (!rtlpriv->rtlhal.wowlan_firmware) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "Can't alloc buffer for wowlan fw.\n");
-		return;
-	}
-
-	if (rtlpriv->rtlhal.hw_type == HARDWARE_TYPE_RTL8821AE)
-		fw_name = "rtlwifi/rtl8821aefw_wowlan.bin";
-	else
-		fw_name = "rtlwifi/rtl8812aefw_wowlan.bin";
-	err = request_firmware(&wowlan_firmware, fw_name, rtlpriv->io.dev);
-	if (err) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "Failed to request wowlan firmware!\n");
-		goto error;
-	}
-
-	if (wowlan_firmware->size > 0x8000) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "Wowlan Firmware is too big!\n");
-		goto error;
-	}
-
-	memcpy(rtlpriv->rtlhal.wowlan_firmware, wowlan_firmware->data,
-	       wowlan_firmware->size);
-	rtlpriv->rtlhal.wowlan_fwsize = wowlan_firmware->size;
-	release_firmware(wowlan_firmware);
-
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "WOWLAN FirmwareDownload OK\n");
-	return;
-error:
-	release_firmware(wowlan_firmware);
-	vfree(rtlpriv->rtlhal.wowlan_firmware);
-}
-
 /*InitializeVariables8812E*/
 int rtl8821ae_init_sw_vars(struct ieee80211_hw *hw)
 {
@@ -231,7 +185,6 @@ int rtl8821ae_init_sw_vars(struct ieee80211_hw *hw)
 	else if (rtlpriv->psc.reg_fwctrl_lps == 3)
 		rtlpriv->psc.fwctrl_psmode = FW_PS_DTIM_MODE;
 
-	rtlpriv->rtl_fw_second_cb = load_wowlan_fw;
 	/* for firmware buf */
 	rtlpriv->rtlhal.pfirmware = vzalloc(0x8000);
 	if (!rtlpriv->rtlhal.pfirmware) {
@@ -239,20 +192,41 @@ int rtl8821ae_init_sw_vars(struct ieee80211_hw *hw)
 			 "Can't alloc buffer for fw.\n");
 		return 1;
 	}
+	rtlpriv->rtlhal.wowlan_firmware = vzalloc(0x8000);
+	if (!rtlpriv->rtlhal.wowlan_firmware) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Can't alloc buffer for wowlan fw.\n");
+		return 1;
+	}
 
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
 		rtlpriv->cfg->fw_name = "rtlwifi/rtl8812aefw.bin";
-	else
+		rtlpriv->cfg->wowlan_fw_name = "rtlwifi/rtl8812aefw_wowlan.bin";
+	} else {
 		rtlpriv->cfg->fw_name = "rtlwifi/rtl8821aefw.bin";
+		rtlpriv->cfg->wowlan_fw_name = "rtlwifi/rtl8821aefw_wowlan.bin";
+	}
 
 	rtlpriv->max_fw_size = 0x8000;
+	/*load normal firmware*/
 	pr_info("Using firmware %s\n", rtlpriv->cfg->fw_name);
 	err = request_firmware_nowait(THIS_MODULE, 1, rtlpriv->cfg->fw_name,
 				      rtlpriv->io.dev, GFP_KERNEL, hw,
 				      rtl_fw_cb);
 	if (err) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "Failed to request firmware!\n");
+			 "Failed to request normal firmware!\n");
+		return 1;
+	}
+	/*load wowlan firmware*/
+	pr_info("Using firmware %s\n", rtlpriv->cfg->wowlan_fw_name);
+	err = request_firmware_nowait(THIS_MODULE, 1,
+				      rtlpriv->cfg->wowlan_fw_name,
+				      rtlpriv->io.dev, GFP_KERNEL, hw,
+				      rtl_wowlan_fw_cb);
+	if (err) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Failed to request wowlan firmware!\n");
 		return 1;
 	}
 	return 0;
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/trx.c b/drivers/net/wireless/rtlwifi/rtl8821ae/trx.c
index 383b86b05cba..72af4b9ee32b 100644
--- a/drivers/net/wireless/rtlwifi/rtl8821ae/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/trx.c
@@ -48,232 +48,6 @@ static u8 _rtl8821ae_map_hwqueue_to_fwqueue(struct sk_buff *skb, u8 hw_queue)
 	return skb->priority;
 }
 
-/* mac80211's rate_idx is like this:
- *
- * 2.4G band:rx_status->band == IEEE80211_BAND_2GHZ
- *
- * B/G rate:
- * (rx_status->flag & RX_FLAG_HT) = 0,
- * DESC_RATE1M-->DESC_RATE54M ==> idx is 0-->11,
- *
- * N rate:
- * (rx_status->flag & RX_FLAG_HT) = 1,
- * DESC_RATEMCS0-->DESC_RATEMCS15 ==> idx is 0-->15
- *
- * 5G band:rx_status->band == IEEE80211_BAND_5GHZ
- * A rate:
- * (rx_status->flag & RX_FLAG_HT) = 0,
- * DESC_RATE6M-->DESC_RATE54M ==> idx is 0-->7,
- *
- * N rate:
- * (rx_status->flag & RX_FLAG_HT) = 1,
- * DESC_RATEMCS0-->DESC_RATEMCS15 ==> idx is 0-->15
- */
-static int _rtl8821ae_rate_mapping(struct ieee80211_hw *hw,
-				   bool isht, bool isvht, u8 desc_rate)
-{
-	int rate_idx;
-
-	if (!isht) {
-		if (IEEE80211_BAND_2GHZ == hw->conf.chandef.chan->band) {
-			switch (desc_rate) {
-			case DESC_RATE1M:
-				rate_idx = 0;
-				break;
-			case DESC_RATE2M:
-				rate_idx = 1;
-				break;
-			case DESC_RATE5_5M:
-				rate_idx = 2;
-				break;
-			case DESC_RATE11M:
-				rate_idx = 3;
-				break;
-			case DESC_RATE6M:
-				rate_idx = 4;
-				break;
-			case DESC_RATE9M:
-				rate_idx = 5;
-				break;
-			case DESC_RATE12M:
-				rate_idx = 6;
-				break;
-			case DESC_RATE18M:
-				rate_idx = 7;
-				break;
-			case DESC_RATE24M:
-				rate_idx = 8;
-				break;
-			case DESC_RATE36M:
-				rate_idx = 9;
-				break;
-			case DESC_RATE48M:
-				rate_idx = 10;
-				break;
-			case DESC_RATE54M:
-				rate_idx = 11;
-				break;
-			default:
-				rate_idx = 0;
-				break;
-			}
-		} else {
-			switch (desc_rate) {
-			case DESC_RATE6M:
-				rate_idx = 0;
-				break;
-			case DESC_RATE9M:
-				rate_idx = 1;
-				break;
-			case DESC_RATE12M:
-				rate_idx = 2;
-				break;
-			case DESC_RATE18M:
-				rate_idx = 3;
-				break;
-			case DESC_RATE24M:
-				rate_idx = 4;
-				break;
-			case DESC_RATE36M:
-				rate_idx = 5;
-				break;
-			case DESC_RATE48M:
-				rate_idx = 6;
-				break;
-			case DESC_RATE54M:
-				rate_idx = 7;
-				break;
-			default:
-				rate_idx = 0;
-				break;
-			}
-		}
-	} else {
-		switch (desc_rate) {
-		case DESC_RATEMCS0:
-			rate_idx = 0;
-			break;
-		case DESC_RATEMCS1:
-			rate_idx = 1;
-			break;
-		case DESC_RATEMCS2:
-			rate_idx = 2;
-			break;
-		case DESC_RATEMCS3:
-			rate_idx = 3;
-			break;
-		case DESC_RATEMCS4:
-			rate_idx = 4;
-			break;
-		case DESC_RATEMCS5:
-			rate_idx = 5;
-			break;
-		case DESC_RATEMCS6:
-			rate_idx = 6;
-			break;
-		case DESC_RATEMCS7:
-			rate_idx = 7;
-			break;
-		case DESC_RATEMCS8:
-			rate_idx = 8;
-			break;
-		case DESC_RATEMCS9:
-			rate_idx = 9;
-			break;
-		case DESC_RATEMCS10:
-			rate_idx = 10;
-			break;
-		case DESC_RATEMCS11:
-			rate_idx = 11;
-			break;
-		case DESC_RATEMCS12:
-			rate_idx = 12;
-			break;
-		case DESC_RATEMCS13:
-			rate_idx = 13;
-			break;
-		case DESC_RATEMCS14:
-			rate_idx = 14;
-			break;
-		case DESC_RATEMCS15:
-			rate_idx = 15;
-			break;
-		default:
-			rate_idx = 0;
-			break;
-		}
-	}
-
-	if (isvht) {
-		switch (desc_rate) {
-		case DESC_RATEVHT1SS_MCS0:
-			rate_idx = 0;
-			break;
-		case DESC_RATEVHT1SS_MCS1:
-			rate_idx = 1;
-			break;
-		case DESC_RATEVHT1SS_MCS2:
-			rate_idx = 2;
-			break;
-		case DESC_RATEVHT1SS_MCS3:
-			rate_idx = 3;
-			break;
-		case DESC_RATEVHT1SS_MCS4:
-			rate_idx = 4;
-			break;
-		case DESC_RATEVHT1SS_MCS5:
-			rate_idx = 5;
-			break;
-		case DESC_RATEVHT1SS_MCS6:
-			rate_idx = 6;
-			break;
-		case DESC_RATEVHT1SS_MCS7:
-			rate_idx = 7;
-			break;
-		case DESC_RATEVHT1SS_MCS8:
-			rate_idx = 8;
-			break;
-		case DESC_RATEVHT1SS_MCS9:
-			rate_idx = 9;
-			break;
-		case DESC_RATEVHT2SS_MCS0:
-			rate_idx = 0;
-			break;
-		case DESC_RATEVHT2SS_MCS1:
-			rate_idx = 1;
-			break;
-		case DESC_RATEVHT2SS_MCS2:
-			rate_idx = 2;
-			break;
-		case DESC_RATEVHT2SS_MCS3:
-			rate_idx = 3;
-			break;
-		case DESC_RATEVHT2SS_MCS4:
-			rate_idx = 4;
-			break;
-		case DESC_RATEVHT2SS_MCS5:
-			rate_idx = 5;
-			break;
-		case DESC_RATEVHT2SS_MCS6:
-			rate_idx = 6;
-			break;
-		case DESC_RATEVHT2SS_MCS7:
-			rate_idx = 7;
-			break;
-		case DESC_RATEVHT2SS_MCS8:
-			rate_idx = 8;
-			break;
-		case DESC_RATEVHT2SS_MCS9:
-			rate_idx = 9;
-			break;
-		default:
-			rate_idx = 0;
-			break;
-		}
-	}
-	return rate_idx;
-}
-
 static u16 odm_cfo(char value)
 {
 	int ret_val;
@@ -766,9 +540,9 @@ bool rtl8821ae_rx_query_desc(struct ieee80211_hw *hw,
 	 * supported rates or MCS index if HT rates
 	 * are use (RX_FLAG_HT)
 	 */
-	rx_status->rate_idx =
-	  _rtl8821ae_rate_mapping(hw, status->is_ht,
-				  status->is_vht, status->rate);
+	rx_status->rate_idx = rtlwifi_rate_mapping(hw, status->is_ht,
+						   status->is_vht,
+						   status->rate);
 
 	rx_status->mactime = status->timestamp_low;
 	if (phystatus) {
diff --git a/drivers/net/wireless/rtlwifi/wifi.h b/drivers/net/wireless/rtlwifi/wifi.h
index 6866dcf24340..b53d9dd7a595 100644
--- a/drivers/net/wireless/rtlwifi/wifi.h
+++ b/drivers/net/wireless/rtlwifi/wifi.h
@@ -331,10 +331,10 @@ enum hardware_type {
 (IS_HARDWARE_TYPE_8723E(rtlhal) || IS_HARDWARE_TYPE_8723U(rtlhal))
 
 #define RX_HAL_IS_CCK_RATE(rxmcs)			\
-	((rxmcs) == DESC92_RATE1M ||			\
-	 (rxmcs) == DESC92_RATE2M ||			\
-	 (rxmcs) == DESC92_RATE5_5M ||			\
-	 (rxmcs) == DESC92_RATE11M)
+	((rxmcs) == DESC_RATE1M ||			\
+	 (rxmcs) == DESC_RATE2M ||			\
+	 (rxmcs) == DESC_RATE5_5M ||			\
+	 (rxmcs) == DESC_RATE11M)
 
 enum scan_operation_backup_opt {
 	SCAN_OPT_BACKUP = 0,
@@ -579,38 +579,59 @@ enum rtl_hal_state {
 };
 
 enum rtl_desc92_rate {
-	DESC92_RATE1M = 0x00,
-	DESC92_RATE2M = 0x01,
-	DESC92_RATE5_5M = 0x02,
-	DESC92_RATE11M = 0x03,
-
-	DESC92_RATE6M = 0x04,
-	DESC92_RATE9M = 0x05,
-	DESC92_RATE12M = 0x06,
-	DESC92_RATE18M = 0x07,
-	DESC92_RATE24M = 0x08,
-	DESC92_RATE36M = 0x09,
-	DESC92_RATE48M = 0x0a,
-	DESC92_RATE54M = 0x0b,
-
-	DESC92_RATEMCS0 = 0x0c,
-	DESC92_RATEMCS1 = 0x0d,
-	DESC92_RATEMCS2 = 0x0e,
-	DESC92_RATEMCS3 = 0x0f,
-	DESC92_RATEMCS4 = 0x10,
-	DESC92_RATEMCS5 = 0x11,
-	DESC92_RATEMCS6 = 0x12,
-	DESC92_RATEMCS7 = 0x13,
-	DESC92_RATEMCS8 = 0x14,
-	DESC92_RATEMCS9 = 0x15,
-	DESC92_RATEMCS10 = 0x16,
-	DESC92_RATEMCS11 = 0x17,
-	DESC92_RATEMCS12 = 0x18,
-	DESC92_RATEMCS13 = 0x19,
-	DESC92_RATEMCS14 = 0x1a,
-	DESC92_RATEMCS15 = 0x1b,
-	DESC92_RATEMCS15_SG = 0x1c,
-	DESC92_RATEMCS32 = 0x20,
+	DESC_RATE1M = 0x00,
+	DESC_RATE2M = 0x01,
+	DESC_RATE5_5M = 0x02,
+	DESC_RATE11M = 0x03,
+
+	DESC_RATE6M = 0x04,
+	DESC_RATE9M = 0x05,
+	DESC_RATE12M = 0x06,
+	DESC_RATE18M = 0x07,
+	DESC_RATE24M = 0x08,
+	DESC_RATE36M = 0x09,
+	DESC_RATE48M = 0x0a,
+	DESC_RATE54M = 0x0b,
+
+	DESC_RATEMCS0 = 0x0c,
+	DESC_RATEMCS1 = 0x0d,
+	DESC_RATEMCS2 = 0x0e,
+	DESC_RATEMCS3 = 0x0f,
+	DESC_RATEMCS4 = 0x10,
+	DESC_RATEMCS5 = 0x11,
+	DESC_RATEMCS6 = 0x12,
+	DESC_RATEMCS7 = 0x13,
+	DESC_RATEMCS8 = 0x14,
+	DESC_RATEMCS9 = 0x15,
+	DESC_RATEMCS10 = 0x16,
+	DESC_RATEMCS11 = 0x17,
+	DESC_RATEMCS12 = 0x18,
+	DESC_RATEMCS13 = 0x19,
+	DESC_RATEMCS14 = 0x1a,
+	DESC_RATEMCS15 = 0x1b,
+	DESC_RATEMCS15_SG = 0x1c,
+	DESC_RATEMCS32 = 0x20,
+
+	DESC_RATEVHT1SS_MCS0 = 0x2c,
+	DESC_RATEVHT1SS_MCS1 = 0x2d,
+	DESC_RATEVHT1SS_MCS2 = 0x2e,
+	DESC_RATEVHT1SS_MCS3 = 0x2f,
+	DESC_RATEVHT1SS_MCS4 = 0x30,
+	DESC_RATEVHT1SS_MCS5 = 0x31,
+	DESC_RATEVHT1SS_MCS6 = 0x32,
+	DESC_RATEVHT1SS_MCS7 = 0x33,
+	DESC_RATEVHT1SS_MCS8 = 0x34,
+	DESC_RATEVHT1SS_MCS9 = 0x35,
+	DESC_RATEVHT2SS_MCS0 = 0x36,
+	DESC_RATEVHT2SS_MCS1 = 0x37,
+	DESC_RATEVHT2SS_MCS2 = 0x38,
+	DESC_RATEVHT2SS_MCS3 = 0x39,
+	DESC_RATEVHT2SS_MCS4 = 0x3a,
+	DESC_RATEVHT2SS_MCS5 = 0x3b,
+	DESC_RATEVHT2SS_MCS6 = 0x3c,
+	DESC_RATEVHT2SS_MCS7 = 0x3d,
+	DESC_RATEVHT2SS_MCS8 = 0x3e,
+	DESC_RATEVHT2SS_MCS9 = 0x3f,
 };
 
 enum rtl_var_map {
@@ -2242,6 +2263,7 @@ struct rtl_hal_cfg {
 	char *name;
 	char *fw_name;
 	char *alt_fw_name;
+	char *wowlan_fw_name;
 	struct rtl_hal_ops *ops;
 	struct rtl_mod_params *mod_params;
 	struct rtl_hal_usbint_cfg *usb_interface_cfg;
@@ -2390,8 +2412,6 @@ struct dig_t {
 	u8 pre_ccastate;
 	u8 cur_ccasate;
 	u8 large_fa_hit;
-	u8 dig_dynamic_min;
-	u8 dig_dynamic_min_1;
 	u8 forbidden_igi;
 	u8 dig_state;
 	u8 dig_highpwrstate;
@@ -2518,8 +2538,6 @@ struct proxim {
 
 struct rtl_priv {
 	struct ieee80211_hw *hw;
-	/* Used to load a second firmware */
-	void (*rtl_fw_second_cb)(struct rtl_priv *rtlpriv);
 	struct completion firmware_loading_complete;
 	struct list_head list;
 	struct rtl_priv *buddy_priv;
diff --git a/drivers/net/wireless/ti/wl12xx/main.c b/drivers/net/wireless/ti/wl12xx/main.c
index 0bccf123831e..144d1f8ba473 100644
--- a/drivers/net/wireless/ti/wl12xx/main.c
+++ b/drivers/net/wireless/ti/wl12xx/main.c
@@ -250,6 +250,7 @@ static struct wlcore_conf wl12xx_conf = {
 		.keep_alive_interval         = 55000,
 		.max_listen_interval         = 20,
 		.sta_sleep_auth              = WL1271_PSM_ILLEGAL,
+		.suspend_rx_ba_activity      = 0,
 	},
 	.itrim = {
 		.enable = false,
@@ -1728,6 +1729,9 @@ static struct wlcore_ops wl12xx_ops = {
 	.convert_hwaddr		= wl12xx_convert_hwaddr,
 	.lnk_high_prio		= wl12xx_lnk_high_prio,
 	.lnk_low_prio		= wl12xx_lnk_low_prio,
+	.interrupt_notify	= NULL,
+	.rx_ba_filter		= NULL,
+	.ap_sleep		= NULL,
 };
 
 static struct ieee80211_sta_ht_cap wl12xx_ht_cap = {
@@ -1900,7 +1904,6 @@ static struct platform_driver wl12xx_driver = {
 	.id_table	= wl12xx_id_table,
 	.driver = {
 		.name	= "wl12xx_driver",
-		.owner	= THIS_MODULE,
 	}
 };
 
diff --git a/drivers/net/wireless/ti/wl18xx/acx.c b/drivers/net/wireless/ti/wl18xx/acx.c
index a169bb5a5dbf..67f2a0eec854 100644
--- a/drivers/net/wireless/ti/wl18xx/acx.c
+++ b/drivers/net/wireless/ti/wl18xx/acx.c
@@ -24,6 +24,7 @@
 #include "../wlcore/acx.h"
 
 #include "acx.h"
+#include "wl18xx.h"
 
 int wl18xx_acx_host_if_cfg_bitmap(struct wl1271 *wl, u32 host_cfg_bitmap,
 				  u32 sdio_blk_size, u32 extra_mem_blks,
@@ -194,3 +195,90 @@ out:
 	kfree(acx);
 	return ret;
 }
+
+/*
+ * When the host is suspended, we don't want to get any fast-link/PSM
+ * notifications
+ */
+int wl18xx_acx_interrupt_notify_config(struct wl1271 *wl,
+				       bool action)
+{
+	struct wl18xx_acx_interrupt_notify *acx;
+	int ret = 0;
+
+	acx = kzalloc(sizeof(*acx), GFP_KERNEL);
+	if (!acx) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	acx->enable = action;
+	ret = wl1271_cmd_configure(wl, ACX_INTERRUPT_NOTIFY, acx, sizeof(*acx));
+	if (ret < 0) {
+		wl1271_warning("acx interrupt notify setting failed: %d", ret);
+		goto out;
+	}
+
+out:
+	kfree(acx);
+	return ret;
+}
+
+/*
+ * When the host is suspended, we can configure the FW to disable RX BA
+ * notifications.
+ */
+int wl18xx_acx_rx_ba_filter(struct wl1271 *wl, bool action)
+{
+	struct wl18xx_acx_rx_ba_filter *acx;
+	int ret = 0;
+
+	acx = kzalloc(sizeof(*acx), GFP_KERNEL);
+	if (!acx) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	acx->enable = (u32)action;
+	ret = wl1271_cmd_configure(wl, ACX_RX_BA_FILTER, acx, sizeof(*acx));
+	if (ret < 0) {
+		wl1271_warning("acx rx ba activity filter setting failed: %d",
+			       ret);
+		goto out;
+	}
+
+out:
+	kfree(acx);
+	return ret;
+}
+
+int wl18xx_acx_ap_sleep(struct wl1271 *wl)
+{
+	struct wl18xx_priv *priv = wl->priv;
+	struct acx_ap_sleep_cfg *acx;
+	struct conf_ap_sleep_settings *conf = &priv->conf.ap_sleep;
+	int ret;
+
+	wl1271_debug(DEBUG_ACX, "acx config ap sleep");
+
+	acx = kzalloc(sizeof(*acx), GFP_KERNEL);
+	if (!acx) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	acx->idle_duty_cycle = conf->idle_duty_cycle;
+	acx->connected_duty_cycle = conf->connected_duty_cycle;
+	acx->max_stations_thresh = conf->max_stations_thresh;
+	acx->idle_conn_thresh = conf->idle_conn_thresh;
+
+	ret = wl1271_cmd_configure(wl, ACX_AP_SLEEP_CFG, acx, sizeof(*acx));
+	if (ret < 0) {
+		wl1271_warning("acx config ap-sleep failed: %d", ret);
+		goto out;
+	}
+
+out:
+	kfree(acx);
+	return ret;
+}
diff --git a/drivers/net/wireless/ti/wl18xx/acx.h b/drivers/net/wireless/ti/wl18xx/acx.h
index 0e636def1217..4afccd4b9467 100644
--- a/drivers/net/wireless/ti/wl18xx/acx.h
+++ b/drivers/net/wireless/ti/wl18xx/acx.h
@@ -32,7 +32,10 @@ enum {
 	ACX_SIM_CONFIG			 = 0x0053,
 	ACX_CLEAR_STATISTICS		 = 0x0054,
 	ACX_AUTO_RX_STREAMING		 = 0x0055,
-	ACX_PEER_CAP			 = 0x0056
+	ACX_PEER_CAP			 = 0x0056,
+	ACX_INTERRUPT_NOTIFY		 = 0x0057,
+	ACX_RX_BA_FILTER		 = 0x0058,
+	ACX_AP_SLEEP_CFG                 = 0x0059
 };
 
 /* numbers of bits the length field takes (add 1 for the actual number) */
@@ -326,6 +329,44 @@ struct wlcore_acx_peer_cap {
 	u8 padding;
 } __packed;
 
+/*
+ * ACX_INTERRUPT_NOTIFY
+ * enable/disable fast-link/PSM notification from FW
+ */
+struct wl18xx_acx_interrupt_notify {
+	struct acx_header header;
+	u32 enable;
+};
+
+/*
+ * ACX_RX_BA_FILTER
+ * enable/disable RX BA filtering in FW
+ */
+struct wl18xx_acx_rx_ba_filter {
+	struct acx_header header;
+	u32 enable;
+};
+
+struct acx_ap_sleep_cfg {
+	struct acx_header header;
+	/* Duty Cycle (20-80% of staying Awake) for IDLE AP
+	 * (0: disable)
+	 */
+	u8 idle_duty_cycle;
+	/* Duty Cycle (20-80% of staying Awake) for Connected AP
+	 * (0: disable)
+	 */
+	u8 connected_duty_cycle;
+	/* Maximum stations that are allowed to be connected to AP
+	 *  (255: no limit)
+	 */
+	u8 max_stations_thresh;
+	/* Timeout till enabling the Sleep Mechanism after data stops
+	 * [unit: 100 msec]
+	 */
+	u8 idle_conn_thresh;
+} __packed;
+
 int wl18xx_acx_host_if_cfg_bitmap(struct wl1271 *wl, u32 host_cfg_bitmap,
 				  u32 sdio_blk_size, u32 extra_mem_blks,
 				  u32 len_field_size);
@@ -336,5 +377,8 @@ int wl18xx_acx_set_peer_cap(struct wl1271 *wl,
 			    struct ieee80211_sta_ht_cap *ht_cap,
 			    bool allow_ht_operation,
 			    u32 rate_set, u8 hlid);
+int wl18xx_acx_interrupt_notify_config(struct wl1271 *wl, bool action);
+int wl18xx_acx_rx_ba_filter(struct wl1271 *wl, bool action);
+int wl18xx_acx_ap_sleep(struct wl1271 *wl);
 
 #endif /* __WL18XX_ACX_H__ */
diff --git a/drivers/net/wireless/ti/wl18xx/cmd.c b/drivers/net/wireless/ti/wl18xx/cmd.c
index 44f0b205b065..a8d176ddc73c 100644
--- a/drivers/net/wireless/ti/wl18xx/cmd.c
+++ b/drivers/net/wireless/ti/wl18xx/cmd.c
@@ -33,7 +33,8 @@ int wl18xx_cmd_channel_switch(struct wl1271 *wl,
 	u32 supported_rates;
 	int ret;
 
-	wl1271_debug(DEBUG_ACX, "cmd channel switch");
+	wl1271_debug(DEBUG_ACX, "cmd channel switch (count=%d)",
+		     ch_switch->count);
 
 	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
 	if (!cmd) {
@@ -60,8 +61,12 @@ int wl18xx_cmd_channel_switch(struct wl1271 *wl,
 		goto out_free;
 	}
 
-	supported_rates = CONF_TX_ENABLED_RATES | CONF_TX_MCS_RATES |
-			  wlcore_hw_sta_get_ap_rate_mask(wl, wlvif);
+	supported_rates = CONF_TX_ENABLED_RATES | CONF_TX_MCS_RATES;
+	if (wlvif->bss_type == BSS_TYPE_STA_BSS)
+		supported_rates |= wlcore_hw_sta_get_ap_rate_mask(wl, wlvif);
+	else
+		supported_rates |=
+			wlcore_hw_ap_get_mimo_wide_rate_mask(wl, wlvif);
 	if (wlvif->p2p)
 		supported_rates &= ~CONF_TX_CCK_RATES;
 	cmd->local_supported_rates = cpu_to_le32(supported_rates);
@@ -167,3 +172,85 @@ out_free:
 out:
 	return ret;
 }
+
+int wl18xx_cmd_set_cac(struct wl1271 *wl, struct wl12xx_vif *wlvif, bool start)
+{
+	struct wlcore_cmd_cac_start *cmd;
+	int ret = 0;
+
+	wl1271_debug(DEBUG_CMD, "cmd cac (channel %d) %s",
+		     wlvif->channel, start ? "start" : "stop");
+
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (!cmd)
+		return -ENOMEM;
+
+	cmd->role_id = wlvif->role_id;
+	cmd->channel = wlvif->channel;
+	if (wlvif->band == IEEE80211_BAND_5GHZ)
+		cmd->band = WLCORE_BAND_5GHZ;
+	cmd->bandwidth = wlcore_get_native_channel_type(wlvif->channel_type);
+
+	ret = wl1271_cmd_send(wl,
+			      start ? CMD_CAC_START : CMD_CAC_STOP,
+			      cmd, sizeof(*cmd), 0);
+	if (ret < 0) {
+		wl1271_error("failed to send cac command");
+		goto out_free;
+	}
+
+out_free:
+	kfree(cmd);
+	return ret;
+}
+
+int wl18xx_cmd_radar_detection_debug(struct wl1271 *wl, u8 channel)
+{
+	struct wl18xx_cmd_dfs_radar_debug *cmd;
+	int ret = 0;
+
+	wl1271_debug(DEBUG_CMD, "cmd radar detection debug (chan %d)",
+		     channel);
+
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (!cmd)
+		return -ENOMEM;
+
+	cmd->channel = channel;
+
+	ret = wl1271_cmd_send(wl, CMD_DFS_RADAR_DETECTION_DEBUG,
+			      cmd, sizeof(*cmd), 0);
+	if (ret < 0) {
+		wl1271_error("failed to send radar detection debug command");
+		goto out_free;
+	}
+
+out_free:
+	kfree(cmd);
+	return ret;
+}
+
+int wl18xx_cmd_dfs_master_restart(struct wl1271 *wl, struct wl12xx_vif *wlvif)
+{
+	struct wl18xx_cmd_dfs_master_restart *cmd;
+	int ret = 0;
+
+	wl1271_debug(DEBUG_CMD, "cmd dfs master restart (role %d)",
+		     wlvif->role_id);
+
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (!cmd)
+		return -ENOMEM;
+
+	cmd->role_id = wlvif->role_id;
+
+	ret = wl1271_cmd_send(wl, CMD_DFS_MASTER_RESTART,
+			      cmd, sizeof(*cmd), 0);
+	if (ret < 0) {
+		wl1271_error("failed to send dfs master restart command");
+		goto out_free;
+	}
+out_free:
+	kfree(cmd);
+	return ret;
+}
diff --git a/drivers/net/wireless/ti/wl18xx/cmd.h b/drivers/net/wireless/ti/wl18xx/cmd.h
index 92499e2dfa83..7f9440a2bff8 100644
--- a/drivers/net/wireless/ti/wl18xx/cmd.h
+++ b/drivers/net/wireless/ti/wl18xx/cmd.h
@@ -59,6 +59,30 @@ struct wl18xx_cmd_smart_config_set_group_key {
 	u8 key[16];
 } __packed;
 
+struct wl18xx_cmd_dfs_radar_debug {
+	struct wl1271_cmd_header header;
+
+	u8 channel;
+	u8 padding[3];
+} __packed;
+
+struct wl18xx_cmd_dfs_master_restart {
+	struct wl1271_cmd_header header;
+
+	u8 role_id;
+	u8 padding[3];
+} __packed;
+
+/* cac_start and cac_stop share the same params */
+struct wlcore_cmd_cac_start {
+	struct wl1271_cmd_header header;
+
+	u8 role_id;
+	u8 channel;
+	u8 band;
+	u8 bandwidth;
+} __packed;
+
 int wl18xx_cmd_channel_switch(struct wl1271 *wl,
 			      struct wl12xx_vif *wlvif,
 			      struct ieee80211_channel_switch *ch_switch);
@@ -66,4 +90,7 @@ int wl18xx_cmd_smart_config_start(struct wl1271 *wl, u32 group_bitmap);
 int wl18xx_cmd_smart_config_stop(struct wl1271 *wl);
 int wl18xx_cmd_smart_config_set_group_key(struct wl1271 *wl, u16 group_id,
 					  u8 key_len, u8 *key);
+int wl18xx_cmd_set_cac(struct wl1271 *wl, struct wl12xx_vif *wlvif, bool start);
+int wl18xx_cmd_radar_detection_debug(struct wl1271 *wl, u8 channel);
+int wl18xx_cmd_dfs_master_restart(struct wl1271 *wl, struct wl12xx_vif *wlvif);
 #endif
diff --git a/drivers/net/wireless/ti/wl18xx/conf.h b/drivers/net/wireless/ti/wl18xx/conf.h
index e34302e3b51d..71f1ec448ba5 100644
--- a/drivers/net/wireless/ti/wl18xx/conf.h
+++ b/drivers/net/wireless/ti/wl18xx/conf.h
@@ -23,7 +23,7 @@
 #define __WL18XX_CONF_H__
 
 #define WL18XX_CONF_MAGIC	0x10e100ca
-#define WL18XX_CONF_VERSION	(WLCORE_CONF_VERSION | 0x0006)
+#define WL18XX_CONF_VERSION	(WLCORE_CONF_VERSION | 0x0007)
 #define WL18XX_CONF_MASK	0x0000ffff
 #define WL18XX_CONF_SIZE	(WLCORE_CONF_SIZE + \
 				 sizeof(struct wl18xx_priv_conf))
@@ -110,12 +110,33 @@ struct wl18xx_ht_settings {
 	u8 mode;
 } __packed;
 
+struct conf_ap_sleep_settings {
+	/* Duty Cycle (20-80% of staying Awake) for IDLE AP
+	 * (0: disable)
+	 */
+	u8 idle_duty_cycle;
+	/* Duty Cycle (20-80% of staying Awake) for Connected AP
+	 * (0: disable)
+	 */
+	u8 connected_duty_cycle;
+	/* Maximum stations that are allowed to be connected to AP
+	 *  (255: no limit)
+	 */
+	u8 max_stations_thresh;
+	/* Timeout till enabling the Sleep Mechanism after data stops
+	 * [unit: 100 msec]
+	 */
+	u8 idle_conn_thresh;
+} __packed;
+
 struct wl18xx_priv_conf {
 	/* Module params structures */
 	struct wl18xx_ht_settings ht;
 
 	/* this structure is copied wholesale to FW */
 	struct wl18xx_mac_and_phy_params phy;
+
+	struct conf_ap_sleep_settings ap_sleep;
 } __packed;
 
 #endif /* __WL18XX_CONF_H__ */
diff --git a/drivers/net/wireless/ti/wl18xx/debugfs.c b/drivers/net/wireless/ti/wl18xx/debugfs.c
index 7f1669cdea09..c93fae95baac 100644
--- a/drivers/net/wireless/ti/wl18xx/debugfs.c
+++ b/drivers/net/wireless/ti/wl18xx/debugfs.c
@@ -22,9 +22,12 @@
 
 #include "../wlcore/debugfs.h"
 #include "../wlcore/wlcore.h"
+#include "../wlcore/debug.h"
+#include "../wlcore/ps.h"
 
 #include "wl18xx.h"
 #include "acx.h"
+#include "cmd.h"
 #include "debugfs.h"
 
 #define WL18XX_DEBUGFS_FWSTATS_FILE(a, b, c) \
@@ -239,6 +242,45 @@ static const struct file_operations clear_fw_stats_ops = {
 	.llseek = default_llseek,
 };
 
+static ssize_t radar_detection_write(struct file *file,
+				     const char __user *user_buf,
+				     size_t count, loff_t *ppos)
+{
+	struct wl1271 *wl = file->private_data;
+	int ret;
+	u8 channel;
+
+	ret = kstrtou8_from_user(user_buf, count, 10, &channel);
+	if (ret < 0) {
+		wl1271_warning("illegal channel");
+		return -EINVAL;
+	}
+
+	mutex_lock(&wl->mutex);
+
+	if (unlikely(wl->state != WLCORE_STATE_ON))
+		goto out;
+
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		goto out;
+
+	ret = wl18xx_cmd_radar_detection_debug(wl, channel);
+	if (ret < 0)
+		count = ret;
+
+	wl1271_ps_elp_sleep(wl);
+out:
+	mutex_unlock(&wl->mutex);
+	return count;
+}
+
+static const struct file_operations radar_detection_ops = {
+	.write = radar_detection_write,
+	.open = simple_open,
+	.llseek = default_llseek,
+};
+
 int wl18xx_debugfs_add_files(struct wl1271 *wl,
 			     struct dentry *rootdir)
 {
@@ -390,6 +432,7 @@ int wl18xx_debugfs_add_files(struct wl1271 *wl,
 	DEBUGFS_FWSTATS_ADD(mem, fw_gen_free_mem_blks);
 
 	DEBUGFS_ADD(conf, moddir);
+	DEBUGFS_ADD(radar_detection, moddir);
 
 	return 0;
 
diff --git a/drivers/net/wireless/ti/wl18xx/event.c b/drivers/net/wireless/ti/wl18xx/event.c
index eb1848e08424..c28f06854195 100644
--- a/drivers/net/wireless/ti/wl18xx/event.c
+++ b/drivers/net/wireless/ti/wl18xx/event.c
@@ -47,6 +47,19 @@ int wl18xx_wait_for_event(struct wl1271 *wl, enum wlcore_wait_event event,
 	return wlcore_cmd_wait_for_event_or_timeout(wl, local_event, timeout);
 }
 
+static const char *wl18xx_radar_type_decode(u8 radar_type)
+{
+	switch (radar_type) {
+	case RADAR_TYPE_REGULAR:
+		return "REGULAR";
+	case RADAR_TYPE_CHIRP:
+		return "CHIRP";
+	case RADAR_TYPE_NONE:
+	default:
+		return "N/A";
+	}
+}
+
 static int wlcore_smart_config_sync_event(struct wl1271 *wl, u8 sync_channel,
 					  u8 sync_band)
 {
@@ -115,6 +128,14 @@ int wl18xx_process_mailbox_events(struct wl1271 *wl)
 			wl18xx_scan_completed(wl, wl->scan_wlvif);
 	}
 
+	if (vector & RADAR_DETECTED_EVENT_ID) {
+		wl1271_info("radar event: channel %d type %s",
+			    mbox->radar_channel,
+			    wl18xx_radar_type_decode(mbox->radar_type));
+
+		ieee80211_radar_detected(wl->hw);
+	}
+
 	if (vector & PERIODIC_SCAN_REPORT_EVENT_ID) {
 		wl1271_debug(DEBUG_EVENT,
 			     "PERIODIC_SCAN_REPORT_EVENT (results %d)",
diff --git a/drivers/net/wireless/ti/wl18xx/event.h b/drivers/net/wireless/ti/wl18xx/event.h
index 0680312d4943..266ee87834e4 100644
--- a/drivers/net/wireless/ti/wl18xx/event.h
+++ b/drivers/net/wireless/ti/wl18xx/event.h
@@ -42,6 +42,12 @@ enum {
 	SMART_CONFIG_DECODE_EVENT_ID		 = BIT(23),
 };
 
+enum wl18xx_radar_types {
+	RADAR_TYPE_NONE,
+	RADAR_TYPE_REGULAR,
+	RADAR_TYPE_CHIRP
+};
+
 struct wl18xx_event_mailbox {
 	__le32 events_vector;
 
@@ -83,13 +89,19 @@ struct wl18xx_event_mailbox {
 	u8 sc_token_len;
 	u8 padding1;
 	u8 sc_ssid[32];
-	u8 sc_pwd[32];
+	u8 sc_pwd[64];
 	u8 sc_token[32];
 
 	/* smart config sync channel */
 	u8 sc_sync_channel;
 	u8 sc_sync_band;
 	u8 padding2[2];
+
+	/* radar detect */
+	u8 radar_channel;
+	u8 radar_type;
+
+	u8 padding3[2];
 } __packed;
 
 int wl18xx_wait_for_event(struct wl1271 *wl, enum wlcore_wait_event event,
diff --git a/drivers/net/wireless/ti/wl18xx/main.c b/drivers/net/wireless/ti/wl18xx/main.c
index 7af1936719eb..717c4f5a02c2 100644
--- a/drivers/net/wireless/ti/wl18xx/main.c
+++ b/drivers/net/wireless/ti/wl18xx/main.c
@@ -378,6 +378,7 @@ static struct wlcore_conf wl18xx_conf = {
 		.keep_alive_interval         = 55000,
 		.max_listen_interval         = 20,
 		.sta_sleep_auth              = WL1271_PSM_ILLEGAL,
+		.suspend_rx_ba_activity      = 0,
 	},
 	.itrim = {
 		.enable = false,
@@ -567,6 +568,12 @@ static struct wl18xx_priv_conf wl18xx_default_priv_conf = {
 		.high_power_val_2nd		= 0xff,
 		.tx_rf_margin			= 1,
 	},
+	.ap_sleep = {               /* disabled by default */
+		.idle_duty_cycle        = 0,
+		.connected_duty_cycle   = 0,
+		.max_stations_thresh    = 0,
+		.idle_conn_thresh       = 0,
+	},
 };
 
 static const struct wlcore_partition_set wl18xx_ptable[PART_TABLE_LEN] = {
@@ -648,7 +655,7 @@ static const struct wl18xx_clk_cfg wl18xx_clk_table[NUM_CLOCK_CONFIGS] = {
 };
 
 /* TODO: maybe move to a new header file? */
-#define WL18XX_FW_NAME "ti-connectivity/wl18xx-fw-3.bin"
+#define WL18XX_FW_NAME "ti-connectivity/wl18xx-fw-4.bin"
 
 static int wl18xx_identify_chip(struct wl1271 *wl)
 {
@@ -983,6 +990,7 @@ static int wl18xx_boot(struct wl1271 *wl)
 
 	wl->event_mask = BSS_LOSS_EVENT_ID |
 		SCAN_COMPLETE_EVENT_ID |
+		RADAR_DETECTED_EVENT_ID |
 		RSSI_SNR_TRIGGER_0_EVENT_ID |
 		PERIODIC_SCAN_COMPLETE_EVENT_ID |
 		PERIODIC_SCAN_REPORT_EVENT_ID |
@@ -1559,26 +1567,19 @@ static u32 wl18xx_pre_pkt_send(struct wl1271 *wl,
 }
 
 static void wl18xx_sta_rc_update(struct wl1271 *wl,
-				 struct wl12xx_vif *wlvif,
-				 struct ieee80211_sta *sta,
-				 u32 changed)
+				 struct wl12xx_vif *wlvif)
 {
-	bool wide = sta->bandwidth >= IEEE80211_STA_RX_BW_40;
+	bool wide = wlvif->rc_update_bw >= IEEE80211_STA_RX_BW_40;
 
 	wl1271_debug(DEBUG_MAC80211, "mac80211 sta_rc_update wide %d", wide);
 
-	if (!(changed & IEEE80211_RC_BW_CHANGED))
-		return;
-
-	mutex_lock(&wl->mutex);
-
 	/* sanity */
 	if (WARN_ON(wlvif->bss_type != BSS_TYPE_STA_BSS))
-		goto out;
+		return;
 
 	/* ignore the change before association */
 	if (!test_bit(WLVIF_FLAG_STA_ASSOCIATED, &wlvif->flags))
-		goto out;
+		return;
 
 	/*
 	 * If we started out as wide, we can change the operation mode. If we
@@ -1589,9 +1590,6 @@ static void wl18xx_sta_rc_update(struct wl1271 *wl,
 		wl18xx_acx_peer_ht_operation_mode(wl, wlvif->sta.hlid, wide);
 	else
 		ieee80211_connection_loss(wl12xx_wlvif_to_vif(wlvif));
-
-out:
-	mutex_unlock(&wl->mutex);
 }
 
 static int wl18xx_set_peer_cap(struct wl1271 *wl,
@@ -1703,6 +1701,11 @@ static struct wlcore_ops wl18xx_ops = {
 	.smart_config_start = wl18xx_cmd_smart_config_start,
 	.smart_config_stop  = wl18xx_cmd_smart_config_stop,
 	.smart_config_set_group_key = wl18xx_cmd_smart_config_set_group_key,
+	.interrupt_notify = wl18xx_acx_interrupt_notify_config,
+	.rx_ba_filter	= wl18xx_acx_rx_ba_filter,
+	.ap_sleep	= wl18xx_acx_ap_sleep,
+	.set_cac	= wl18xx_cmd_set_cac,
+	.dfs_master_restart	= wl18xx_cmd_dfs_master_restart,
 };
 
 /* HT cap appropriate for wide channels in 2Ghz */
@@ -1796,6 +1799,10 @@ wl18xx_iface_combinations[] = {
 		.limits = wl18xx_iface_ap_limits,
 		.n_limits = ARRAY_SIZE(wl18xx_iface_ap_limits),
 		.num_different_channels = 1,
+		.radar_detect_widths =	BIT(NL80211_CHAN_NO_HT) |
+					BIT(NL80211_CHAN_HT20) |
+					BIT(NL80211_CHAN_HT40MINUS) |
+					BIT(NL80211_CHAN_HT40PLUS),
 	}
 };
 
@@ -1968,7 +1975,6 @@ static struct platform_driver wl18xx_driver = {
 	.id_table	= wl18xx_id_table,
 	.driver = {
 		.name	= "wl18xx_driver",
-		.owner	= THIS_MODULE,
 	}
 };
 
diff --git a/drivers/net/wireless/ti/wl18xx/wl18xx.h b/drivers/net/wireless/ti/wl18xx/wl18xx.h
index 6a2b88030c1d..71e9e382ce80 100644
--- a/drivers/net/wireless/ti/wl18xx/wl18xx.h
+++ b/drivers/net/wireless/ti/wl18xx/wl18xx.h
@@ -26,10 +26,10 @@
 
 /* minimum FW required for driver */
 #define WL18XX_CHIP_VER		8
-#define WL18XX_IFTYPE_VER	8
+#define WL18XX_IFTYPE_VER	9
 #define WL18XX_MAJOR_VER	WLCORE_FW_VER_IGNORE
 #define WL18XX_SUBTYPE_VER	WLCORE_FW_VER_IGNORE
-#define WL18XX_MINOR_VER	13
+#define WL18XX_MINOR_VER	11
 
 #define WL18XX_CMD_MAX_SIZE          740
 
diff --git a/drivers/net/wireless/ti/wlcore/cmd.c b/drivers/net/wireless/ti/wlcore/cmd.c
index b82661962d33..c26fc2106e5b 100644
--- a/drivers/net/wireless/ti/wlcore/cmd.c
+++ b/drivers/net/wireless/ti/wlcore/cmd.c
@@ -403,7 +403,7 @@ void wl12xx_free_link(struct wl1271 *wl, struct wl12xx_vif *wlvif, u8 *hlid)
 	WARN_ON_ONCE(wl->active_link_count < 0);
 }
 
-static u8 wlcore_get_native_channel_type(u8 nl_channel_type)
+u8 wlcore_get_native_channel_type(u8 nl_channel_type)
 {
 	switch (nl_channel_type) {
 	case NL80211_CHAN_NO_HT:
@@ -419,6 +419,7 @@ static u8 wlcore_get_native_channel_type(u8 nl_channel_type)
 		return WLCORE_CHAN_NO_HT;
 	}
 }
+EXPORT_SYMBOL_GPL(wlcore_get_native_channel_type);
 
 static int wl12xx_cmd_role_start_dev(struct wl1271 *wl,
 				     struct wl12xx_vif *wlvif,
@@ -1686,9 +1687,7 @@ int wlcore_cmd_regdomain_config_locked(struct wl1271 *wl)
 {
 	struct wl12xx_cmd_regdomain_dfs_config *cmd = NULL;
 	int ret = 0, i, b, ch_bit_idx;
-	struct ieee80211_channel *channel;
 	u32 tmp_ch_bitmap[2];
-	u16 ch;
 	struct wiphy *wiphy = wl->hw->wiphy;
 	struct ieee80211_supported_band *band;
 	bool timeout = false;
@@ -1703,12 +1702,16 @@ int wlcore_cmd_regdomain_config_locked(struct wl1271 *wl)
 	for (b = IEEE80211_BAND_2GHZ; b <= IEEE80211_BAND_5GHZ; b++) {
 		band = wiphy->bands[b];
 		for (i = 0; i < band->n_channels; i++) {
-			channel = &band->channels[i];
-			ch = channel->hw_value;
+			struct ieee80211_channel *channel = &band->channels[i];
+			u16 ch = channel->hw_value;
+			u32 flags = channel->flags;
 
-			if (channel->flags & (IEEE80211_CHAN_DISABLED |
-					      IEEE80211_CHAN_RADAR |
-					      IEEE80211_CHAN_NO_IR))
+			if (flags & (IEEE80211_CHAN_DISABLED |
+				     IEEE80211_CHAN_NO_IR))
+				continue;
+
+			if ((flags & IEEE80211_CHAN_RADAR) &&
+			    channel->dfs_state != NL80211_DFS_AVAILABLE)
 				continue;
 
 			ch_bit_idx = wlcore_get_reg_conf_ch_idx(b, ch);
@@ -1733,6 +1736,7 @@ int wlcore_cmd_regdomain_config_locked(struct wl1271 *wl)
 
 	cmd->ch_bit_map1 = cpu_to_le32(tmp_ch_bitmap[0]);
 	cmd->ch_bit_map2 = cpu_to_le32(tmp_ch_bitmap[1]);
+	cmd->dfs_region = wl->dfs_region;
 
 	wl1271_debug(DEBUG_CMD,
 		     "cmd reg domain bitmap1: 0x%08x, bitmap2: 0x%08x",
diff --git a/drivers/net/wireless/ti/wlcore/cmd.h b/drivers/net/wireless/ti/wlcore/cmd.h
index 453684a71d30..e14cd407a6ae 100644
--- a/drivers/net/wireless/ti/wlcore/cmd.h
+++ b/drivers/net/wireless/ti/wlcore/cmd.h
@@ -105,6 +105,7 @@ int wl12xx_allocate_link(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 void wl12xx_free_link(struct wl1271 *wl, struct wl12xx_vif *wlvif, u8 *hlid);
 int wlcore_cmd_wait_for_event_or_timeout(struct wl1271 *wl,
 					 u32 mask, bool *timeout);
+u8 wlcore_get_native_channel_type(u8 nl_channel_type);
 
 enum wl1271_commands {
 	CMD_INTERROGATE	= 1, /* use this to read information elements */
@@ -172,6 +173,11 @@ enum wl1271_commands {
 	CMD_SMART_CONFIG_STOP		= 62,
 	CMD_SMART_CONFIG_SET_GROUP_KEY	= 63,
 
+	CMD_CAC_START			= 64,
+	CMD_CAC_STOP			= 65,
+	CMD_DFS_MASTER_RESTART		= 66,
+	CMD_DFS_RADAR_DETECTION_DEBUG	= 67,
+
 	MAX_COMMAND_ID = 0xFFFF,
 };
 
@@ -642,6 +648,8 @@ struct wl12xx_cmd_regdomain_dfs_config {
 
 	__le32 ch_bit_map1;
 	__le32 ch_bit_map2;
+	u8 dfs_region;
+	u8 padding[3];
 } __packed;
 
 struct wl12xx_cmd_config_fwlog {
diff --git a/drivers/net/wireless/ti/wlcore/conf.h b/drivers/net/wireless/ti/wlcore/conf.h
index 40995c42bef8..166add00b50f 100644
--- a/drivers/net/wireless/ti/wlcore/conf.h
+++ b/drivers/net/wireless/ti/wlcore/conf.h
@@ -997,6 +997,11 @@ struct conf_conn_settings {
 	 * whether we can go to ELP.
 	 */
 	u8 sta_sleep_auth;
+
+	/*
+	 * Default RX BA Activity filter configuration
+	 */
+	u8 suspend_rx_ba_activity;
 } __packed;
 
 enum {
@@ -1347,7 +1352,7 @@ struct conf_recovery_settings {
  * version, the two LSB are the lower driver's private conf
  * version.
  */
-#define WLCORE_CONF_VERSION	(0x0005 << 16)
+#define WLCORE_CONF_VERSION	(0x0006 << 16)
 #define WLCORE_CONF_MASK	0xffff0000
 #define WLCORE_CONF_SIZE	(sizeof(struct wlcore_conf_header) +	\
 				 sizeof(struct wlcore_conf))
diff --git a/drivers/net/wireless/ti/wlcore/debugfs.c b/drivers/net/wireless/ti/wlcore/debugfs.c
index 0be21f62fcb0..68f3bf229b5a 100644
--- a/drivers/net/wireless/ti/wlcore/debugfs.c
+++ b/drivers/net/wireless/ti/wlcore/debugfs.c
@@ -929,17 +929,10 @@ static ssize_t beacon_filtering_write(struct file *file,
 {
 	struct wl1271 *wl = file->private_data;
 	struct wl12xx_vif *wlvif;
-	char buf[10];
-	size_t len;
 	unsigned long value;
 	int ret;
 
-	len = min(count, sizeof(buf) - 1);
-	if (copy_from_user(buf, user_buf, len))
-		return -EFAULT;
-	buf[len] = '\0';
-
-	ret = kstrtoul(buf, 0, &value);
+	ret = kstrtoul_from_user(user_buf, count, 0, &value);
 	if (ret < 0) {
 		wl1271_warning("illegal value for beacon_filtering!");
 		return -EINVAL;
diff --git a/drivers/net/wireless/ti/wlcore/event.c b/drivers/net/wireless/ti/wlcore/event.c
index 5153640f4532..c42e78955e7b 100644
--- a/drivers/net/wireless/ti/wlcore/event.c
+++ b/drivers/net/wireless/ti/wlcore/event.c
@@ -139,7 +139,7 @@ void wlcore_event_channel_switch(struct wl1271 *wl,
 	wl1271_debug(DEBUG_EVENT, "%s: roles=0x%lx success=%d",
 		     __func__, roles_bitmap, success);
 
-	wl12xx_for_each_wlvif_sta(wl, wlvif) {
+	wl12xx_for_each_wlvif(wl, wlvif) {
 		if (wlvif->role_id == WL12XX_INVALID_ROLE_ID ||
 		    !test_bit(wlvif->role_id , &roles_bitmap))
 			continue;
@@ -150,8 +150,13 @@ void wlcore_event_channel_switch(struct wl1271 *wl,
 
 		vif = wl12xx_wlvif_to_vif(wlvif);
 
-		ieee80211_chswitch_done(vif, success);
-		cancel_delayed_work(&wlvif->channel_switch_work);
+		if (wlvif->bss_type == BSS_TYPE_STA_BSS) {
+			ieee80211_chswitch_done(vif, success);
+			cancel_delayed_work(&wlvif->channel_switch_work);
+		} else {
+			set_bit(WLVIF_FLAG_BEACON_DISABLED, &wlvif->flags);
+			ieee80211_csa_finish(vif);
+		}
 	}
 }
 EXPORT_SYMBOL_GPL(wlcore_event_channel_switch);
diff --git a/drivers/net/wireless/ti/wlcore/hw_ops.h b/drivers/net/wireless/ti/wlcore/hw_ops.h
index aa9f82c72296..eec56935b1b6 100644
--- a/drivers/net/wireless/ti/wlcore/hw_ops.h
+++ b/drivers/net/wireless/ti/wlcore/hw_ops.h
@@ -211,11 +211,35 @@ wlcore_hw_pre_pkt_send(struct wl1271 *wl, u32 buf_offset, u32 last_len)
 }
 
 static inline void
-wlcore_hw_sta_rc_update(struct wl1271 *wl, struct wl12xx_vif *wlvif,
-			struct ieee80211_sta *sta, u32 changed)
+wlcore_hw_sta_rc_update(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 {
 	if (wl->ops->sta_rc_update)
-		wl->ops->sta_rc_update(wl, wlvif, sta, changed);
+		wl->ops->sta_rc_update(wl, wlvif);
+}
+
+static inline int
+wlcore_hw_interrupt_notify(struct wl1271 *wl, bool action)
+{
+	if (wl->ops->interrupt_notify)
+		return wl->ops->interrupt_notify(wl, action);
+	return 0;
+}
+
+static inline int
+wlcore_hw_rx_ba_filter(struct wl1271 *wl, bool action)
+{
+	if (wl->ops->rx_ba_filter)
+		return wl->ops->rx_ba_filter(wl, action);
+	return 0;
+}
+
+static inline int
+wlcore_hw_ap_sleep(struct wl1271 *wl)
+{
+	if (wl->ops->ap_sleep)
+		return wl->ops->ap_sleep(wl);
+
+	return 0;
 }
 
 static inline int
@@ -287,4 +311,22 @@ wlcore_smart_config_set_group_key(struct wl1271 *wl, u16 group_id,
 
 	return wl->ops->smart_config_set_group_key(wl, group_id, key_len, key);
 }
+
+static inline int
+wlcore_hw_set_cac(struct wl1271 *wl, struct wl12xx_vif *wlvif, bool start)
+{
+	if (!wl->ops->set_cac)
+		return -EINVAL;
+
+	return wl->ops->set_cac(wl, wlvif, start);
+}
+
+static inline int
+wlcore_hw_dfs_master_restart(struct wl1271 *wl, struct wl12xx_vif *wlvif)
+{
+	if (!wl->ops->dfs_master_restart)
+		return -EINVAL;
+
+	return wl->ops->dfs_master_restart(wl, wlvif);
+}
 #endif
diff --git a/drivers/net/wireless/ti/wlcore/init.c b/drivers/net/wireless/ti/wlcore/init.c
index 199e94120864..5ca1fb161a50 100644
--- a/drivers/net/wireless/ti/wlcore/init.c
+++ b/drivers/net/wireless/ti/wlcore/init.c
@@ -392,6 +392,11 @@ static int wl1271_ap_hw_init(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 	if (ret < 0)
 		return ret;
 
+	/* configure AP sleep, if enabled */
+	ret = wlcore_hw_ap_sleep(wl);
+	if (ret < 0)
+		return ret;
+
 	return 0;
 }
 
@@ -567,8 +572,7 @@ int wl1271_init_vif_specific(struct wl1271 *wl, struct ieee80211_vif *vif)
 	/* consider all existing roles before configuring psm. */
 
 	if (wl->ap_count == 0 && is_ap) { /* first AP */
-		/* Configure for power always on */
-		ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_CAM);
+		ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_ELP);
 		if (ret < 0)
 			return ret;
 
diff --git a/drivers/net/wireless/ti/wlcore/main.c b/drivers/net/wireless/ti/wlcore/main.c
index a2133b1fd631..0a9d9a1b2d85 100644
--- a/drivers/net/wireless/ti/wlcore/main.c
+++ b/drivers/net/wireless/ti/wlcore/main.c
@@ -79,22 +79,12 @@ static int wl12xx_set_authorized(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 static void wl1271_reg_notify(struct wiphy *wiphy,
 			      struct regulatory_request *request)
 {
-	struct ieee80211_supported_band *band;
-	struct ieee80211_channel *ch;
-	int i;
 	struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
 	struct wl1271 *wl = hw->priv;
 
-	band = wiphy->bands[IEEE80211_BAND_5GHZ];
-	for (i = 0; i < band->n_channels; i++) {
-		ch = &band->channels[i];
-		if (ch->flags & IEEE80211_CHAN_DISABLED)
-			continue;
-
-		if (ch->flags & IEEE80211_CHAN_RADAR)
-			ch->flags |= IEEE80211_CHAN_NO_IR;
-
-	}
+	/* copy the current dfs region */
+	if (request)
+		wl->dfs_region = request->dfs_region;
 
 	wlcore_regdomain_config(wl);
 }
@@ -226,6 +216,29 @@ void wl12xx_rearm_tx_watchdog_locked(struct wl1271 *wl)
 		msecs_to_jiffies(wl->conf.tx.tx_watchdog_timeout));
 }
 
+static void wlcore_rc_update_work(struct work_struct *work)
+{
+	int ret;
+	struct wl12xx_vif *wlvif = container_of(work, struct wl12xx_vif,
+						rc_update_work);
+	struct wl1271 *wl = wlvif->wl;
+
+	mutex_lock(&wl->mutex);
+
+	if (unlikely(wl->state != WLCORE_STATE_ON))
+		goto out;
+
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		goto out;
+
+	wlcore_hw_sta_rc_update(wl, wlvif);
+
+	wl1271_ps_elp_sleep(wl);
+out:
+	mutex_unlock(&wl->mutex);
+}
+
 static void wl12xx_tx_watchdog_work(struct work_struct *work)
 {
 	struct delayed_work *dwork;
@@ -1662,19 +1675,15 @@ static int wl1271_configure_suspend_sta(struct wl1271 *wl,
 	if (!test_bit(WLVIF_FLAG_STA_ASSOCIATED, &wlvif->flags))
 		goto out;
 
-	ret = wl1271_ps_elp_wakeup(wl);
-	if (ret < 0)
-		goto out;
-
 	ret = wl1271_configure_wowlan(wl, wow);
 	if (ret < 0)
-		goto out_sleep;
+		goto out;
 
 	if ((wl->conf.conn.suspend_wake_up_event ==
 	     wl->conf.conn.wake_up_event) &&
 	    (wl->conf.conn.suspend_listen_interval ==
 	     wl->conf.conn.listen_interval))
-		goto out_sleep;
+		goto out;
 
 	ret = wl1271_acx_wake_up_conditions(wl, wlvif,
 				    wl->conf.conn.suspend_wake_up_event,
@@ -1682,29 +1691,28 @@ static int wl1271_configure_suspend_sta(struct wl1271 *wl,
 
 	if (ret < 0)
 		wl1271_error("suspend: set wake up conditions failed: %d", ret);
-
-out_sleep:
-	wl1271_ps_elp_sleep(wl);
 out:
 	return ret;
 
 }
 
 static int wl1271_configure_suspend_ap(struct wl1271 *wl,
-				       struct wl12xx_vif *wlvif)
+					struct wl12xx_vif *wlvif,
+					struct cfg80211_wowlan *wow)
 {
 	int ret = 0;
 
 	if (!test_bit(WLVIF_FLAG_AP_STARTED, &wlvif->flags))
 		goto out;
 
-	ret = wl1271_ps_elp_wakeup(wl);
+	ret = wl1271_acx_beacon_filter_opt(wl, wlvif, true);
 	if (ret < 0)
 		goto out;
 
-	ret = wl1271_acx_beacon_filter_opt(wl, wlvif, true);
+	ret = wl1271_configure_wowlan(wl, wow);
+	if (ret < 0)
+		goto out;
 
-	wl1271_ps_elp_sleep(wl);
 out:
 	return ret;
 
@@ -1717,7 +1725,7 @@ static int wl1271_configure_suspend(struct wl1271 *wl,
 	if (wlvif->bss_type == BSS_TYPE_STA_BSS)
 		return wl1271_configure_suspend_sta(wl, wlvif, wow);
 	if (wlvif->bss_type == BSS_TYPE_AP_BSS)
-		return wl1271_configure_suspend_ap(wl, wlvif);
+		return wl1271_configure_suspend_ap(wl, wlvif, wow);
 	return 0;
 }
 
@@ -1730,21 +1738,18 @@ static void wl1271_configure_resume(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 	if ((!is_ap) && (!is_sta))
 		return;
 
-	if (is_sta && !test_bit(WLVIF_FLAG_STA_ASSOCIATED, &wlvif->flags))
+	if ((is_sta && !test_bit(WLVIF_FLAG_STA_ASSOCIATED, &wlvif->flags)) ||
+	    (is_ap && !test_bit(WLVIF_FLAG_AP_STARTED, &wlvif->flags)))
 		return;
 
-	ret = wl1271_ps_elp_wakeup(wl);
-	if (ret < 0)
-		return;
+	wl1271_configure_wowlan(wl, NULL);
 
 	if (is_sta) {
-		wl1271_configure_wowlan(wl, NULL);
-
 		if ((wl->conf.conn.suspend_wake_up_event ==
 		     wl->conf.conn.wake_up_event) &&
 		    (wl->conf.conn.suspend_listen_interval ==
 		     wl->conf.conn.listen_interval))
-			goto out_sleep;
+			return;
 
 		ret = wl1271_acx_wake_up_conditions(wl, wlvif,
 				    wl->conf.conn.wake_up_event,
@@ -1757,9 +1762,6 @@ static void wl1271_configure_resume(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 	} else if (is_ap) {
 		ret = wl1271_acx_beacon_filter_opt(wl, wlvif, false);
 	}
-
-out_sleep:
-	wl1271_ps_elp_sleep(wl);
 }
 
 static int wl1271_op_suspend(struct ieee80211_hw *hw,
@@ -1781,6 +1783,11 @@ static int wl1271_op_suspend(struct ieee80211_hw *hw,
 	wl1271_tx_flush(wl);
 
 	mutex_lock(&wl->mutex);
+
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		return ret;
+
 	wl->wow_enabled = true;
 	wl12xx_for_each_wlvif(wl, wlvif) {
 		ret = wl1271_configure_suspend(wl, wlvif, wow);
@@ -1790,7 +1797,27 @@ static int wl1271_op_suspend(struct ieee80211_hw *hw,
 			return ret;
 		}
 	}
+
+	/* disable fast link flow control notifications from FW */
+	ret = wlcore_hw_interrupt_notify(wl, false);
+	if (ret < 0)
+		goto out_sleep;
+
+	/* if filtering is enabled, configure the FW to drop all RX BA frames */
+	ret = wlcore_hw_rx_ba_filter(wl,
+				     !!wl->conf.conn.suspend_rx_ba_activity);
+	if (ret < 0)
+		goto out_sleep;
+
+out_sleep:
+	wl1271_ps_elp_sleep(wl);
 	mutex_unlock(&wl->mutex);
+
+	if (ret < 0) {
+		wl1271_warning("couldn't prepare device to suspend");
+		return ret;
+	}
+
 	/* flush any remaining work */
 	wl1271_debug(DEBUG_MAC80211, "flushing remaining works");
 
@@ -1864,13 +1891,29 @@ static int wl1271_op_resume(struct ieee80211_hw *hw)
 	if (pending_recovery) {
 		wl1271_warning("queuing forgotten recovery on resume");
 		ieee80211_queue_work(wl->hw, &wl->recovery_work);
-		goto out;
+		goto out_sleep;
 	}
 
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		goto out;
+
 	wl12xx_for_each_wlvif(wl, wlvif) {
 		wl1271_configure_resume(wl, wlvif);
 	}
 
+	ret = wlcore_hw_interrupt_notify(wl, true);
+	if (ret < 0)
+		goto out_sleep;
+
+	/* if filtering is enabled, configure the FW to drop all RX BA frames */
+	ret = wlcore_hw_rx_ba_filter(wl, false);
+	if (ret < 0)
+		goto out_sleep;
+
+out_sleep:
+	wl1271_ps_elp_sleep(wl);
+
 out:
 	wl->wow_enabled = false;
 
@@ -2279,6 +2322,7 @@ static int wl12xx_init_vif_data(struct wl1271 *wl, struct ieee80211_vif *vif)
 		  wl1271_rx_streaming_enable_work);
 	INIT_WORK(&wlvif->rx_streaming_disable_work,
 		  wl1271_rx_streaming_disable_work);
+	INIT_WORK(&wlvif->rc_update_work, wlcore_rc_update_work);
 	INIT_DELAYED_WORK(&wlvif->channel_switch_work,
 			  wlcore_channel_switch_work);
 	INIT_DELAYED_WORK(&wlvif->connection_loss_work,
@@ -2724,6 +2768,7 @@ unlock:
 	del_timer_sync(&wlvif->rx_streaming_timer);
 	cancel_work_sync(&wlvif->rx_streaming_enable_work);
 	cancel_work_sync(&wlvif->rx_streaming_disable_work);
+	cancel_work_sync(&wlvif->rc_update_work);
 	cancel_delayed_work_sync(&wlvif->connection_loss_work);
 	cancel_delayed_work_sync(&wlvif->channel_switch_work);
 	cancel_delayed_work_sync(&wlvif->pending_auth_complete_work);
@@ -4073,8 +4118,14 @@ static int wl1271_bss_beacon_info_changed(struct wl1271 *wl,
 		ret = wlcore_set_beacon_template(wl, vif, is_ap);
 		if (ret < 0)
 			goto out;
-	}
 
+		if (test_and_clear_bit(WLVIF_FLAG_BEACON_DISABLED,
+				       &wlvif->flags)) {
+			ret = wlcore_hw_dfs_master_restart(wl, wlvif);
+			if (ret < 0)
+				goto out;
+		}
+	}
 out:
 	if (ret != 0)
 		wl1271_error("beacon info change failed: %d", ret);
@@ -4575,10 +4626,46 @@ static void wlcore_op_change_chanctx(struct ieee80211_hw *hw,
 				     struct ieee80211_chanctx_conf *ctx,
 				     u32 changed)
 {
+	struct wl1271 *wl = hw->priv;
+	struct wl12xx_vif *wlvif;
+	int ret;
+	int channel = ieee80211_frequency_to_channel(
+		ctx->def.chan->center_freq);
+
 	wl1271_debug(DEBUG_MAC80211,
 		     "mac80211 change chanctx %d (type %d) changed 0x%x",
-		     ieee80211_frequency_to_channel(ctx->def.chan->center_freq),
-		     cfg80211_get_chandef_type(&ctx->def), changed);
+		     channel, cfg80211_get_chandef_type(&ctx->def), changed);
+
+	mutex_lock(&wl->mutex);
+
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		goto out;
+
+	wl12xx_for_each_wlvif(wl, wlvif) {
+		struct ieee80211_vif *vif = wl12xx_wlvif_to_vif(wlvif);
+
+		rcu_read_lock();
+		if (rcu_access_pointer(vif->chanctx_conf) != ctx) {
+			rcu_read_unlock();
+			continue;
+		}
+		rcu_read_unlock();
+
+		/* start radar if needed */
+		if (changed & IEEE80211_CHANCTX_CHANGE_RADAR &&
+		    wlvif->bss_type == BSS_TYPE_AP_BSS &&
+		    ctx->radar_enabled && !wlvif->radar_enabled &&
+		    ctx->def.chan->dfs_state == NL80211_DFS_USABLE) {
+			wl1271_debug(DEBUG_MAC80211, "Start radar detection");
+			wlcore_hw_set_cac(wl, wlvif, true);
+			wlvif->radar_enabled = true;
+		}
+	}
+
+	wl1271_ps_elp_sleep(wl);
+out:
+	mutex_unlock(&wl->mutex);
 }
 
 static int wlcore_op_assign_vif_chanctx(struct ieee80211_hw *hw,
@@ -4589,13 +4676,26 @@ static int wlcore_op_assign_vif_chanctx(struct ieee80211_hw *hw,
 	struct wl12xx_vif *wlvif = wl12xx_vif_to_data(vif);
 	int channel = ieee80211_frequency_to_channel(
 		ctx->def.chan->center_freq);
+	int ret = -EINVAL;
 
 	wl1271_debug(DEBUG_MAC80211,
-		     "mac80211 assign chanctx (role %d) %d (type %d)",
-		     wlvif->role_id, channel, cfg80211_get_chandef_type(&ctx->def));
+		     "mac80211 assign chanctx (role %d) %d (type %d) (radar %d dfs_state %d)",
+		     wlvif->role_id, channel,
+		     cfg80211_get_chandef_type(&ctx->def),
+		     ctx->radar_enabled, ctx->def.chan->dfs_state);
 
 	mutex_lock(&wl->mutex);
 
+	if (unlikely(wl->state != WLCORE_STATE_ON))
+		goto out;
+
+	if (unlikely(!test_bit(WLVIF_FLAG_INITIALIZED, &wlvif->flags)))
+		goto out;
+
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		goto out;
+
 	wlvif->band = ctx->def.chan->band;
 	wlvif->channel = channel;
 	wlvif->channel_type = cfg80211_get_chandef_type(&ctx->def);
@@ -4603,6 +4703,15 @@ static int wlcore_op_assign_vif_chanctx(struct ieee80211_hw *hw,
 	/* update default rates according to the band */
 	wl1271_set_band_rate(wl, wlvif);
 
+	if (ctx->radar_enabled &&
+	    ctx->def.chan->dfs_state == NL80211_DFS_USABLE) {
+		wl1271_debug(DEBUG_MAC80211, "Start radar detection");
+		wlcore_hw_set_cac(wl, wlvif, true);
+		wlvif->radar_enabled = true;
+	}
+
+	wl1271_ps_elp_sleep(wl);
+out:
 	mutex_unlock(&wl->mutex);
 
 	return 0;
@@ -4614,6 +4723,7 @@ static void wlcore_op_unassign_vif_chanctx(struct ieee80211_hw *hw,
 {
 	struct wl1271 *wl = hw->priv;
 	struct wl12xx_vif *wlvif = wl12xx_vif_to_data(vif);
+	int ret;
 
 	wl1271_debug(DEBUG_MAC80211,
 		     "mac80211 unassign chanctx (role %d) %d (type %d)",
@@ -4622,6 +4732,99 @@ static void wlcore_op_unassign_vif_chanctx(struct ieee80211_hw *hw,
 		     cfg80211_get_chandef_type(&ctx->def));
 
 	wl1271_tx_flush(wl);
+
+	mutex_lock(&wl->mutex);
+
+	if (unlikely(wl->state != WLCORE_STATE_ON))
+		goto out;
+
+	if (unlikely(!test_bit(WLVIF_FLAG_INITIALIZED, &wlvif->flags)))
+		goto out;
+
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		goto out;
+
+	if (wlvif->radar_enabled) {
+		wl1271_debug(DEBUG_MAC80211, "Stop radar detection");
+		wlcore_hw_set_cac(wl, wlvif, false);
+		wlvif->radar_enabled = false;
+	}
+
+	wl1271_ps_elp_sleep(wl);
+out:
+	mutex_unlock(&wl->mutex);
+}
+
+static int __wlcore_switch_vif_chan(struct wl1271 *wl,
+				    struct wl12xx_vif *wlvif,
+				    struct ieee80211_chanctx_conf *new_ctx)
+{
+	int channel = ieee80211_frequency_to_channel(
+		new_ctx->def.chan->center_freq);
+
+	wl1271_debug(DEBUG_MAC80211,
+		     "switch vif (role %d) %d -> %d chan_type: %d",
+		     wlvif->role_id, wlvif->channel, channel,
+		     cfg80211_get_chandef_type(&new_ctx->def));
+
+	if (WARN_ON_ONCE(wlvif->bss_type != BSS_TYPE_AP_BSS))
+		return 0;
+
+	WARN_ON(!test_bit(WLVIF_FLAG_BEACON_DISABLED, &wlvif->flags));
+
+	if (wlvif->radar_enabled) {
+		wl1271_debug(DEBUG_MAC80211, "Stop radar detection");
+		wlcore_hw_set_cac(wl, wlvif, false);
+		wlvif->radar_enabled = false;
+	}
+
+	wlvif->band = new_ctx->def.chan->band;
+	wlvif->channel = channel;
+	wlvif->channel_type = cfg80211_get_chandef_type(&new_ctx->def);
+
+	/* start radar if needed */
+	if (new_ctx->radar_enabled) {
+		wl1271_debug(DEBUG_MAC80211, "Start radar detection");
+		wlcore_hw_set_cac(wl, wlvif, true);
+		wlvif->radar_enabled = true;
+	}
+
+	return 0;
+}
+
+static int
+wlcore_op_switch_vif_chanctx(struct ieee80211_hw *hw,
+			     struct ieee80211_vif_chanctx_switch *vifs,
+			     int n_vifs,
+			     enum ieee80211_chanctx_switch_mode mode)
+{
+	struct wl1271 *wl = hw->priv;
+	int i, ret;
+
+	wl1271_debug(DEBUG_MAC80211,
+		     "mac80211 switch chanctx n_vifs %d mode %d",
+		     n_vifs, mode);
+
+	mutex_lock(&wl->mutex);
+
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		goto out;
+
+	for (i = 0; i < n_vifs; i++) {
+		struct wl12xx_vif *wlvif = wl12xx_vif_to_data(vifs[i].vif);
+
+		ret = __wlcore_switch_vif_chan(wl, wlvif, vifs[i].new_ctx);
+		if (ret)
+			goto out_sleep;
+	}
+out_sleep:
+	wl1271_ps_elp_sleep(wl);
+out:
+	mutex_unlock(&wl->mutex);
+
+	return 0;
 }
 
 static int wl1271_op_conf_tx(struct ieee80211_hw *hw,
@@ -5229,6 +5432,83 @@ out:
 	mutex_unlock(&wl->mutex);
 }
 
+static const void *wlcore_get_beacon_ie(struct wl1271 *wl,
+					struct wl12xx_vif *wlvif,
+					u8 eid)
+{
+	int ieoffset = offsetof(struct ieee80211_mgmt, u.beacon.variable);
+	struct sk_buff *beacon =
+		ieee80211_beacon_get(wl->hw, wl12xx_wlvif_to_vif(wlvif));
+
+	if (!beacon)
+		return NULL;
+
+	return cfg80211_find_ie(eid,
+				beacon->data + ieoffset,
+				beacon->len - ieoffset);
+}
+
+static int wlcore_get_csa_count(struct wl1271 *wl, struct wl12xx_vif *wlvif,
+				u8 *csa_count)
+{
+	const u8 *ie;
+	const struct ieee80211_channel_sw_ie *ie_csa;
+
+	ie = wlcore_get_beacon_ie(wl, wlvif, WLAN_EID_CHANNEL_SWITCH);
+	if (!ie)
+		return -EINVAL;
+
+	ie_csa = (struct ieee80211_channel_sw_ie *)&ie[2];
+	*csa_count = ie_csa->count;
+
+	return 0;
+}
+
+static void wlcore_op_channel_switch_beacon(struct ieee80211_hw *hw,
+					    struct ieee80211_vif *vif,
+					    struct cfg80211_chan_def *chandef)
+{
+	struct wl1271 *wl = hw->priv;
+	struct wl12xx_vif *wlvif = wl12xx_vif_to_data(vif);
+	struct ieee80211_channel_switch ch_switch = {
+		.block_tx = true,
+		.chandef = *chandef,
+	};
+	int ret;
+
+	wl1271_debug(DEBUG_MAC80211,
+		     "mac80211 channel switch beacon (role %d)",
+		     wlvif->role_id);
+
+	ret = wlcore_get_csa_count(wl, wlvif, &ch_switch.count);
+	if (ret < 0) {
+		wl1271_error("error getting beacon (for CSA counter)");
+		return;
+	}
+
+	mutex_lock(&wl->mutex);
+
+	if (unlikely(wl->state != WLCORE_STATE_ON)) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		goto out;
+
+	ret = wl->ops->channel_switch(wl, wlvif, &ch_switch);
+	if (ret)
+		goto out_sleep;
+
+	set_bit(WLVIF_FLAG_CS_PROGRESS, &wlvif->flags);
+
+out_sleep:
+	wl1271_ps_elp_sleep(wl);
+out:
+	mutex_unlock(&wl->mutex);
+}
+
 static void wlcore_op_flush(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 			    u32 queues, bool drop)
 {
@@ -5371,9 +5651,15 @@ static void wlcore_op_sta_rc_update(struct ieee80211_hw *hw,
 				    u32 changed)
 {
 	struct wl12xx_vif *wlvif = wl12xx_vif_to_data(vif);
-	struct wl1271 *wl = hw->priv;
 
-	wlcore_hw_sta_rc_update(wl, wlvif, sta, changed);
+	wl1271_debug(DEBUG_MAC80211, "mac80211 sta_rc_update");
+
+	if (!(changed & IEEE80211_RC_BW_CHANGED))
+		return;
+
+	/* this callback is atomic, so schedule a new work */
+	wlvif->rc_update_bw = sta->bandwidth;
+	ieee80211_queue_work(hw, &wlvif->rc_update_work);
 }
 
 static void wlcore_op_sta_statistics(struct ieee80211_hw *hw,
@@ -5599,6 +5885,7 @@ static const struct ieee80211_ops wl1271_ops = {
 	.set_bitrate_mask = wl12xx_set_bitrate_mask,
 	.set_default_unicast_key = wl1271_op_set_default_key_idx,
 	.channel_switch = wl12xx_op_channel_switch,
+	.channel_switch_beacon = wlcore_op_channel_switch_beacon,
 	.flush = wlcore_op_flush,
 	.remain_on_channel = wlcore_op_remain_on_channel,
 	.cancel_remain_on_channel = wlcore_op_cancel_remain_on_channel,
@@ -5607,6 +5894,7 @@ static const struct ieee80211_ops wl1271_ops = {
 	.change_chanctx = wlcore_op_change_chanctx,
 	.assign_vif_chanctx = wlcore_op_assign_vif_chanctx,
 	.unassign_vif_chanctx = wlcore_op_unassign_vif_chanctx,
+	.switch_vif_chanctx = wlcore_op_switch_vif_chanctx,
 	.sta_rc_update = wlcore_op_sta_rc_update,
 	.sta_statistics = wlcore_op_sta_statistics,
 	CFG80211_TESTMODE_CMD(wl1271_tm_cmd)
@@ -5813,7 +6101,8 @@ static int wl1271_init_ieee80211(struct wl1271 *wl)
 
 	wl->hw->wiphy->flags |= WIPHY_FLAG_AP_UAPSD |
 				WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL |
-				WIPHY_FLAG_SUPPORTS_SCHED_SCAN;
+				WIPHY_FLAG_SUPPORTS_SCHED_SCAN |
+				WIPHY_FLAG_HAS_CHANNEL_SWITCH;
 
 	/* make sure all our channels fit in the scanned_ch bitmask */
 	BUILD_BUG_ON(ARRAY_SIZE(wl1271_channels) +
diff --git a/drivers/net/wireless/ti/wlcore/ps.c b/drivers/net/wireless/ti/wlcore/ps.c
index b52516eed7b2..4cd316e61466 100644
--- a/drivers/net/wireless/ti/wlcore/ps.c
+++ b/drivers/net/wireless/ti/wlcore/ps.c
@@ -56,9 +56,6 @@ void wl1271_elp_work(struct work_struct *work)
 		goto out;
 
 	wl12xx_for_each_wlvif(wl, wlvif) {
-		if (wlvif->bss_type == BSS_TYPE_AP_BSS)
-			goto out;
-
 		if (!test_bit(WLVIF_FLAG_IN_PS, &wlvif->flags) &&
 		    test_bit(WLVIF_FLAG_IN_USE, &wlvif->flags))
 			goto out;
@@ -95,9 +92,6 @@ void wl1271_ps_elp_sleep(struct wl1271 *wl)
 		return;
 
 	wl12xx_for_each_wlvif(wl, wlvif) {
-		if (wlvif->bss_type == BSS_TYPE_AP_BSS)
-			return;
-
 		if (!test_bit(WLVIF_FLAG_IN_PS, &wlvif->flags) &&
 		    test_bit(WLVIF_FLAG_IN_USE, &wlvif->flags))
 			return;
@@ -108,6 +102,7 @@ void wl1271_ps_elp_sleep(struct wl1271 *wl)
 	ieee80211_queue_delayed_work(wl->hw, &wl->elp_work,
 				     msecs_to_jiffies(timeout));
 }
+EXPORT_SYMBOL_GPL(wl1271_ps_elp_sleep);
 
 int wl1271_ps_elp_wakeup(struct wl1271 *wl)
 {
@@ -175,6 +170,7 @@ err:
 out:
 	return 0;
 }
+EXPORT_SYMBOL_GPL(wl1271_ps_elp_wakeup);
 
 int wl1271_ps_set_mode(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 		       enum wl1271_cmd_ps_mode mode)
diff --git a/drivers/net/wireless/ti/wlcore/vendor_cmd.c b/drivers/net/wireless/ti/wlcore/vendor_cmd.c
index ad86a48dcfcb..fd4e9ba176c9 100644
--- a/drivers/net/wireless/ti/wlcore/vendor_cmd.c
+++ b/drivers/net/wireless/ti/wlcore/vendor_cmd.c
@@ -21,7 +21,7 @@ static const
 struct nla_policy wlcore_vendor_attr_policy[NUM_WLCORE_VENDOR_ATTR] = {
 	[WLCORE_VENDOR_ATTR_FREQ]		= { .type = NLA_U32 },
 	[WLCORE_VENDOR_ATTR_GROUP_ID]		= { .type = NLA_U32 },
-	[WLCORE_VENDOR_ATTR_GROUP_KEY]		= { .type = NLA_U32,
+	[WLCORE_VENDOR_ATTR_GROUP_KEY]		= { .type = NLA_BINARY,
 						    .len = WLAN_MAX_KEY_LEN },
 };
 
diff --git a/drivers/net/wireless/ti/wlcore/wlcore.h b/drivers/net/wireless/ti/wlcore/wlcore.h
index df78cf12ef15..d599c869e6e8 100644
--- a/drivers/net/wireless/ti/wlcore/wlcore.h
+++ b/drivers/net/wireless/ti/wlcore/wlcore.h
@@ -106,8 +106,7 @@ struct wlcore_ops {
 			      struct wl12xx_vif *wlvif,
 			      struct ieee80211_channel_switch *ch_switch);
 	u32 (*pre_pkt_send)(struct wl1271 *wl, u32 buf_offset, u32 last_len);
-	void (*sta_rc_update)(struct wl1271 *wl, struct wl12xx_vif *wlvif,
-			      struct ieee80211_sta *sta, u32 changed);
+	void (*sta_rc_update)(struct wl1271 *wl, struct wl12xx_vif *wlvif);
 	int (*set_peer_cap)(struct wl1271 *wl,
 			    struct ieee80211_sta_ht_cap *ht_cap,
 			    bool allow_ht_operation,
@@ -117,10 +116,16 @@ struct wlcore_ops {
 			      struct wl1271_link *lnk);
 	bool (*lnk_low_prio)(struct wl1271 *wl, u8 hlid,
 			     struct wl1271_link *lnk);
+	int (*interrupt_notify)(struct wl1271 *wl, bool action);
+	int (*rx_ba_filter)(struct wl1271 *wl, bool action);
+	int (*ap_sleep)(struct wl1271 *wl);
 	int (*smart_config_start)(struct wl1271 *wl, u32 group_bitmap);
 	int (*smart_config_stop)(struct wl1271 *wl);
 	int (*smart_config_set_group_key)(struct wl1271 *wl, u16 group_id,
 					  u8 key_len, u8 *key);
+	int (*set_cac)(struct wl1271 *wl, struct wl12xx_vif *wlvif,
+		       bool start);
+	int (*dfs_master_restart)(struct wl1271 *wl, struct wl12xx_vif *wlvif);
 };
 
 enum wlcore_partitions {
@@ -460,6 +465,9 @@ struct wl1271 {
 	/* HW HT (11n) capabilities */
 	struct ieee80211_sta_ht_cap ht_cap[WLCORE_NUM_BANDS];
 
+	/* the current dfs region */
+	enum nl80211_dfs_regions dfs_region;
+
 	/* size of the private FW status data */
 	size_t fw_status_len;
 	size_t fw_status_priv_len;
diff --git a/drivers/net/wireless/ti/wlcore/wlcore_i.h b/drivers/net/wireless/ti/wlcore/wlcore_i.h
index 0e52556044d9..3396ce5a934d 100644
--- a/drivers/net/wireless/ti/wlcore/wlcore_i.h
+++ b/drivers/net/wireless/ti/wlcore/wlcore_i.h
@@ -251,6 +251,7 @@ enum wl12xx_vif_flags {
 	WLVIF_FLAG_AP_PROBE_RESP_SET,
 	WLVIF_FLAG_IN_USE,
 	WLVIF_FLAG_ACTIVE,
+	WLVIF_FLAG_BEACON_DISABLED,
 };
 
 struct wl12xx_vif;
@@ -434,6 +435,8 @@ struct wl12xx_vif {
 
 	bool wmm_enabled;
 
+	bool radar_enabled;
+
 	/* Rx Streaming */
 	struct work_struct rx_streaming_enable_work;
 	struct work_struct rx_streaming_disable_work;
@@ -463,6 +466,10 @@ struct wl12xx_vif {
 	/* work for canceling ROC after pending auth reply */
 	struct delayed_work pending_auth_complete_work;
 
+	/* update rate conrol */
+	enum ieee80211_sta_rx_bandwidth rc_update_bw;
+	struct work_struct rc_update_work;
+
 	/*
 	 * total freed FW packets on the link.
 	 * For STA this holds the PN of the link to the AP.
diff --git a/drivers/net/wireless/zd1211rw/zd_chip.c b/drivers/net/wireless/zd1211rw/zd_chip.c
index 73a49b868035..07b94eda9604 100644
--- a/drivers/net/wireless/zd1211rw/zd_chip.c
+++ b/drivers/net/wireless/zd1211rw/zd_chip.c
@@ -129,7 +129,7 @@ int zd_ioread32v_locked(struct zd_chip *chip, u32 *values, const zd_addr_t *addr
 	r = zd_ioread16v_locked(chip, v16, a16, count16);
 	if (r) {
 		dev_dbg_f(zd_chip_dev(chip),
-			  "error: zd_ioread16v_locked. Error number %d\n", r);
+			  "error: %s. Error number %d\n", __func__, r);
 		return r;
 	}
 
@@ -256,8 +256,8 @@ int zd_iowrite32a_locked(struct zd_chip *chip,
 		if (r) {
 			zd_usb_iowrite16v_async_end(&chip->usb, 0);
 			dev_dbg_f(zd_chip_dev(chip),
-				"error _zd_iowrite32v_locked."
-				" Error number %d\n", r);
+				"error _%s. Error number %d\n", __func__,
+				r);
 			return r;
 		}
 	}
diff --git a/drivers/net/xen-netback/common.h b/drivers/net/xen-netback/common.h
index 083ecc93fe5e..5f1fda44882b 100644
--- a/drivers/net/xen-netback/common.h
+++ b/drivers/net/xen-netback/common.h
@@ -230,6 +230,8 @@ struct xenvif {
 	 */
 	bool disabled;
 	unsigned long status;
+	unsigned long drain_timeout;
+	unsigned long stall_timeout;
 
 	/* Queues */
 	struct xenvif_queue *queues;
@@ -328,7 +330,7 @@ irqreturn_t xenvif_interrupt(int irq, void *dev_id);
 extern bool separate_tx_rx_irq;
 
 extern unsigned int rx_drain_timeout_msecs;
-extern unsigned int rx_drain_timeout_jiffies;
+extern unsigned int rx_stall_timeout_msecs;
 extern unsigned int xenvif_max_queues;
 
 #ifdef CONFIG_DEBUG_FS
diff --git a/drivers/net/xen-netback/interface.c b/drivers/net/xen-netback/interface.c
index a6a32d337bbb..9259a732e8a4 100644
--- a/drivers/net/xen-netback/interface.c
+++ b/drivers/net/xen-netback/interface.c
@@ -166,7 +166,7 @@ static int xenvif_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		goto drop;
 
 	cb = XENVIF_RX_CB(skb);
-	cb->expires = jiffies + rx_drain_timeout_jiffies;
+	cb->expires = jiffies + vif->drain_timeout;
 
 	xenvif_rx_queue_tail(queue, skb);
 	xenvif_kick_thread(queue);
@@ -414,6 +414,8 @@ struct xenvif *xenvif_alloc(struct device *parent, domid_t domid,
 	vif->ip_csum = 1;
 	vif->dev = dev;
 	vif->disabled = false;
+	vif->drain_timeout = msecs_to_jiffies(rx_drain_timeout_msecs);
+	vif->stall_timeout = msecs_to_jiffies(rx_stall_timeout_msecs);
 
 	/* Start out with no queues. */
 	vif->queues = NULL;
diff --git a/drivers/net/xen-netback/netback.c b/drivers/net/xen-netback/netback.c
index 4a509f715fe8..908e65e9b821 100644
--- a/drivers/net/xen-netback/netback.c
+++ b/drivers/net/xen-netback/netback.c
@@ -60,14 +60,12 @@ module_param(separate_tx_rx_irq, bool, 0644);
  */
 unsigned int rx_drain_timeout_msecs = 10000;
 module_param(rx_drain_timeout_msecs, uint, 0444);
-unsigned int rx_drain_timeout_jiffies;
 
 /* The length of time before the frontend is considered unresponsive
  * because it isn't providing Rx slots.
  */
-static unsigned int rx_stall_timeout_msecs = 60000;
+unsigned int rx_stall_timeout_msecs = 60000;
 module_param(rx_stall_timeout_msecs, uint, 0444);
-static unsigned int rx_stall_timeout_jiffies;
 
 unsigned int xenvif_max_queues;
 module_param_named(max_queues, xenvif_max_queues, uint, 0644);
@@ -2020,7 +2018,7 @@ static bool xenvif_rx_queue_stalled(struct xenvif_queue *queue)
 	return !queue->stalled
 		&& prod - cons < XEN_NETBK_RX_SLOTS_MAX
 		&& time_after(jiffies,
-			      queue->last_rx_time + rx_stall_timeout_jiffies);
+			      queue->last_rx_time + queue->vif->stall_timeout);
 }
 
 static bool xenvif_rx_queue_ready(struct xenvif_queue *queue)
@@ -2038,8 +2036,9 @@ static bool xenvif_have_rx_work(struct xenvif_queue *queue)
 {
 	return (!skb_queue_empty(&queue->rx_queue)
 		&& xenvif_rx_ring_slots_available(queue, XEN_NETBK_RX_SLOTS_MAX))
-		|| xenvif_rx_queue_stalled(queue)
-		|| xenvif_rx_queue_ready(queue)
+		|| (queue->vif->stall_timeout &&
+		    (xenvif_rx_queue_stalled(queue)
+		     || xenvif_rx_queue_ready(queue)))
 		|| kthread_should_stop()
 		|| queue->vif->disabled;
 }
@@ -2092,6 +2091,9 @@ int xenvif_kthread_guest_rx(void *data)
 	struct xenvif_queue *queue = data;
 	struct xenvif *vif = queue->vif;
 
+	if (!vif->stall_timeout)
+		xenvif_queue_carrier_on(queue);
+
 	for (;;) {
 		xenvif_wait_for_rx_work(queue);
 
@@ -2118,10 +2120,12 @@ int xenvif_kthread_guest_rx(void *data)
 		 * while it's probably not responsive, drop the
 		 * carrier so packets are dropped earlier.
 		 */
-		if (xenvif_rx_queue_stalled(queue))
-			xenvif_queue_carrier_off(queue);
-		else if (xenvif_rx_queue_ready(queue))
-			xenvif_queue_carrier_on(queue);
+		if (vif->stall_timeout) {
+			if (xenvif_rx_queue_stalled(queue))
+				xenvif_queue_carrier_off(queue);
+			else if (xenvif_rx_queue_ready(queue))
+				xenvif_queue_carrier_on(queue);
+		}
 
 		/* Queued packets may have foreign pages from other
 		 * domains.  These cannot be queued indefinitely as
@@ -2192,9 +2196,6 @@ static int __init netback_init(void)
 	if (rc)
 		goto failed_init;
 
-	rx_drain_timeout_jiffies = msecs_to_jiffies(rx_drain_timeout_msecs);
-	rx_stall_timeout_jiffies = msecs_to_jiffies(rx_stall_timeout_msecs);
-
 #ifdef CONFIG_DEBUG_FS
 	xen_netback_dbg_root = debugfs_create_dir("xen-netback", NULL);
 	if (IS_ERR_OR_NULL(xen_netback_dbg_root))
diff --git a/drivers/net/xen-netback/xenbus.c b/drivers/net/xen-netback/xenbus.c
index d44cd19169bd..efbaf2ae1999 100644
--- a/drivers/net/xen-netback/xenbus.c
+++ b/drivers/net/xen-netback/xenbus.c
@@ -887,9 +887,15 @@ static int read_xenbus_vif_flags(struct backend_info *be)
 		return -EOPNOTSUPP;
 
 	if (xenbus_scanf(XBT_NIL, dev->otherend,
-			 "feature-rx-notify", "%d", &val) < 0 || val == 0) {
-		xenbus_dev_fatal(dev, -EINVAL, "feature-rx-notify is mandatory");
-		return -EINVAL;
+			 "feature-rx-notify", "%d", &val) < 0)
+		val = 0;
+	if (!val) {
+		/* - Reduce drain timeout to poll more frequently for
+		 *   Rx requests.
+		 * - Disable Rx stall detection.
+		 */
+		be->vif->drain_timeout = msecs_to_jiffies(30);
+		be->vif->stall_timeout = 0;
 	}
 
 	if (xenbus_scanf(XBT_NIL, dev->otherend, "feature-sg",
diff --git a/drivers/net/xen-netfront.c b/drivers/net/xen-netfront.c
index 2f0a9ce9ff73..22bcb4e12e2a 100644
--- a/drivers/net/xen-netfront.c
+++ b/drivers/net/xen-netfront.c
@@ -977,7 +977,6 @@ static int xennet_poll(struct napi_struct *napi, int budget)
 	struct sk_buff_head rxq;
 	struct sk_buff_head errq;
 	struct sk_buff_head tmpq;
-	unsigned long flags;
 	int err;
 
 	spin_lock(&queue->rx_lock);
@@ -1050,15 +1049,11 @@ err:
 	if (work_done < budget) {
 		int more_to_do = 0;
 
-		napi_gro_flush(napi, false);
-
-		local_irq_save(flags);
+		napi_complete(napi);
 
 		RING_FINAL_CHECK_FOR_RESPONSES(&queue->rx, more_to_do);
-		if (!more_to_do)
-			__napi_complete(napi);
-
-		local_irq_restore(flags);
+		if (more_to_do)
+			napi_schedule(napi);
 	}
 
 	spin_unlock(&queue->rx_lock);
diff --git a/drivers/nfc/nfcwilink.c b/drivers/nfc/nfcwilink.c
index 683671a71c7e..ce2e2cf54fbc 100644
--- a/drivers/nfc/nfcwilink.c
+++ b/drivers/nfc/nfcwilink.c
@@ -566,7 +566,6 @@ static struct platform_driver nfcwilink_driver = {
 	.remove = nfcwilink_remove,
 	.driver = {
 		.name = "nfcwilink",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/nfc/trf7970a.c b/drivers/nfc/trf7970a.c
index d2ccd2890647..aa6a333b2ead 100644
--- a/drivers/nfc/trf7970a.c
+++ b/drivers/nfc/trf7970a.c
@@ -2154,7 +2154,7 @@ static int trf7970a_resume(struct device *dev)
 }
 #endif
 
-#ifdef CONFIG_PM_RUNTIME
+#ifdef CONFIG_PM
 static int trf7970a_pm_runtime_suspend(struct device *dev)
 {
 	struct spi_device *spi = container_of(dev, struct spi_device, dev);
diff --git a/drivers/of/irq.c b/drivers/of/irq.c
index 1471e0a223a5..0d7765807f49 100644
--- a/drivers/of/irq.c
+++ b/drivers/of/irq.c
@@ -405,6 +405,7 @@ int of_irq_get(struct device_node *dev, int index)
 
 	return irq_create_of_mapping(&oirq);
 }
+EXPORT_SYMBOL_GPL(of_irq_get);
 
 /**
  * of_irq_get_byname - Decode a node's IRQ and return it as a Linux irq number
diff --git a/drivers/of/platform.c b/drivers/of/platform.c
index cd87a36495be..5b33c6a21807 100644
--- a/drivers/of/platform.c
+++ b/drivers/of/platform.c
@@ -19,6 +19,7 @@
 #include <linux/slab.h>
 #include <linux/of_address.h>
 #include <linux/of_device.h>
+#include <linux/of_iommu.h>
 #include <linux/of_irq.h>
 #include <linux/of_platform.h>
 #include <linux/platform_device.h>
@@ -164,6 +165,9 @@ static void of_dma_configure(struct device *dev)
 {
 	u64 dma_addr, paddr, size;
 	int ret;
+	bool coherent;
+	unsigned long offset;
+	struct iommu_ops *iommu;
 
 	/*
 	 * Set default dma-mask to 32 bit. Drivers are expected to setup
@@ -178,28 +182,30 @@ static void of_dma_configure(struct device *dev)
 	if (!dev->dma_mask)
 		dev->dma_mask = &dev->coherent_dma_mask;
 
-	/*
-	 * if dma-coherent property exist, call arch hook to setup
-	 * dma coherent operations.
-	 */
-	if (of_dma_is_coherent(dev->of_node)) {
-		set_arch_dma_coherent_ops(dev);
-		dev_dbg(dev, "device is dma coherent\n");
-	}
-
-	/*
-	 * if dma-ranges property doesn't exist - just return else
-	 * setup the dma offset
-	 */
 	ret = of_dma_get_range(dev->of_node, &dma_addr, &paddr, &size);
 	if (ret < 0) {
-		dev_dbg(dev, "no dma range information to setup\n");
-		return;
+		dma_addr = offset = 0;
+		size = dev->coherent_dma_mask;
+	} else {
+		offset = PFN_DOWN(paddr - dma_addr);
+		dev_dbg(dev, "dma_pfn_offset(%#08lx)\n", dev->dma_pfn_offset);
 	}
+	dev->dma_pfn_offset = offset;
+
+	coherent = of_dma_is_coherent(dev->of_node);
+	dev_dbg(dev, "device is%sdma coherent\n",
+		coherent ? " " : " not ");
+
+	iommu = of_iommu_configure(dev);
+	dev_dbg(dev, "device is%sbehind an iommu\n",
+		iommu ? " " : " not ");
 
-	/* DMA ranges found. Calculate and set dma_pfn_offset */
-	dev->dma_pfn_offset = PFN_DOWN(paddr - dma_addr);
-	dev_dbg(dev, "dma_pfn_offset(%#08lx)\n", dev->dma_pfn_offset);
+	arch_setup_dma_ops(dev, dma_addr, size, iommu, coherent);
+}
+
+static void of_dma_deconfigure(struct device *dev)
+{
+	arch_teardown_dma_ops(dev);
 }
 
 /**
@@ -228,16 +234,12 @@ static struct platform_device *of_platform_device_create_pdata(
 	if (!dev)
 		goto err_clear_flag;
 
-	of_dma_configure(&dev->dev);
 	dev->dev.bus = &platform_bus_type;
 	dev->dev.platform_data = platform_data;
-
-	/* We do not fill the DMA ops for platform devices by default.
-	 * This is currently the responsibility of the platform code
-	 * to do such, possibly using a device notifier
-	 */
+	of_dma_configure(&dev->dev);
 
 	if (of_device_add(dev) != 0) {
+		of_dma_deconfigure(&dev->dev);
 		platform_device_put(dev);
 		goto err_clear_flag;
 	}
diff --git a/drivers/parport/parport_amiga.c b/drivers/parport/parport_amiga.c
index 26ecdea84fb5..9c68f2aec4ff 100644
--- a/drivers/parport/parport_amiga.c
+++ b/drivers/parport/parport_amiga.c
@@ -239,7 +239,6 @@ static struct platform_driver amiga_parallel_driver = {
 	.remove = __exit_p(amiga_parallel_remove),
 	.driver   = {
 		.name	= "amiga-parallel",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/parport/parport_ax88796.c b/drivers/parport/parport_ax88796.c
index 7c5d86696eed..8f8c9f3aa691 100644
--- a/drivers/parport/parport_ax88796.c
+++ b/drivers/parport/parport_ax88796.c
@@ -412,7 +412,6 @@ MODULE_ALIAS("platform:ax88796-pp");
 static struct platform_driver axdrv = {
 	.driver		= {
 		.name	= "ax88796-pp",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= parport_ax88796_probe,
 	.remove		= parport_ax88796_remove,
diff --git a/drivers/parport/parport_pc.c b/drivers/parport/parport_pc.c
index f721299eb1ba..53d15b30636a 100644
--- a/drivers/parport/parport_pc.c
+++ b/drivers/parport/parport_pc.c
@@ -3011,7 +3011,6 @@ static int parport_pc_platform_probe(struct platform_device *pdev)
 
 static struct platform_driver parport_pc_platform_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= "parport_pc",
 	},
 	.probe		= parport_pc_platform_probe,
@@ -3340,13 +3339,14 @@ static void __exit parport_pc_exit(void)
 	while (!list_empty(&ports_list)) {
 		struct parport_pc_private *priv;
 		struct parport *port;
+		struct device *dev;
 		priv = list_entry(ports_list.next,
 				  struct parport_pc_private, list);
 		port = priv->port;
-		if (port->dev && port->dev->bus == &platform_bus_type)
-			platform_device_unregister(
-				to_platform_device(port->dev));
+		dev = port->dev;
 		parport_pc_unregister_port(port);
+		if (dev && dev->bus == &platform_bus_type)
+			platform_device_unregister(to_platform_device(dev));
 	}
 }
 
diff --git a/drivers/parport/parport_serial.c b/drivers/parport/parport_serial.c
index ee932004724f..e15b4845f7c6 100644
--- a/drivers/parport/parport_serial.c
+++ b/drivers/parport/parport_serial.c
@@ -64,6 +64,7 @@ enum parport_pc_pci_cards {
 	timedia_9079c,
 	wch_ch353_1s1p,
 	wch_ch353_2s1p,
+	wch_ch382_2s1p,
 	sunix_2s1p,
 };
 
@@ -151,6 +152,7 @@ static struct parport_pc_pci cards[] = {
 	/* timedia_9079c */             { 1, { { 2, 3 }, } },
 	/* wch_ch353_1s1p*/             { 1, { { 1, -1}, } },
 	/* wch_ch353_2s1p*/             { 1, { { 2, -1}, } },
+	/* wch_ch382_2s1p*/             { 1, { { 2, -1}, } },
 	/* sunix_2s1p */                { 1, { { 3, -1 }, } },
 };
 
@@ -257,6 +259,7 @@ static struct pci_device_id parport_serial_pci_tbl[] = {
 	/* WCH CARDS */
 	{ 0x4348, 0x5053, PCI_ANY_ID, PCI_ANY_ID, 0, 0, wch_ch353_1s1p},
 	{ 0x4348, 0x7053, 0x4348, 0x3253, 0, 0, wch_ch353_2s1p},
+	{ 0x1c00, 0x3250, 0x1c00, 0x3250, 0, 0, wch_ch382_2s1p},
 
 	/*
 	 * More SUNIX variations. At least one of these has part number
@@ -494,6 +497,13 @@ static struct pciserial_board pci_parport_serial_boards[] = {
 		.base_baud      = 115200,
 		.uart_offset    = 8,
 	},
+	[wch_ch382_2s1p] = {
+		.flags          = FL_BASE0,
+		.num_ports      = 2,
+		.base_baud      = 115200,
+		.uart_offset    = 8,
+		.first_offset   = 0xC0,
+	},
 	[sunix_2s1p] = {
 		.flags		= FL_BASE0|FL_BASE_BARS,
 		.num_ports	= 2,
diff --git a/drivers/parport/parport_sunbpp.c b/drivers/parport/parport_sunbpp.c
index dffd6d0bd15b..01cf1c1a841a 100644
--- a/drivers/parport/parport_sunbpp.c
+++ b/drivers/parport/parport_sunbpp.c
@@ -362,7 +362,6 @@ MODULE_DEVICE_TABLE(of, bpp_match);
 static struct platform_driver bpp_sbus_driver = {
 	.driver = {
 		.name = "bpp",
-		.owner = THIS_MODULE,
 		.of_match_table = bpp_match,
 	},
 	.probe		= bpp_probe,
diff --git a/drivers/pci/Kconfig b/drivers/pci/Kconfig
index cced84233ac0..7a8f1c5e65af 100644
--- a/drivers/pci/Kconfig
+++ b/drivers/pci/Kconfig
@@ -67,7 +67,7 @@ config XEN_PCIDEV_FRONTEND
 config HT_IRQ
 	bool "Interrupts on hypertransport devices"
 	default y
-	depends on PCI && X86_LOCAL_APIC && X86_IO_APIC
+	depends on PCI && X86_LOCAL_APIC
 	help
 	   This allows native hypertransport devices to use interrupts.
 
@@ -110,13 +110,6 @@ config PCI_PASID
 
 	  If unsure, say N.
 
-config PCI_IOAPIC
-	bool "PCI IO-APIC hotplug support" if X86
-	depends on PCI
-	depends on ACPI
-	depends on X86_IO_APIC
-	default !X86
-
 config PCI_LABEL
 	def_bool y if (DMI || ACPI)
 	select NLS
diff --git a/drivers/pci/Makefile b/drivers/pci/Makefile
index e04fe2d9df3b..73e4af400a5a 100644
--- a/drivers/pci/Makefile
+++ b/drivers/pci/Makefile
@@ -13,8 +13,6 @@ obj-$(CONFIG_PCI_QUIRKS) += quirks.o
 # Build PCI Express stuff if needed
 obj-$(CONFIG_PCIEPORTBUS) += pcie/
 
-obj-$(CONFIG_PCI_IOAPIC) += ioapic.o
-
 # Build the PCI Hotplug drivers if we were asked to
 obj-$(CONFIG_HOTPLUG_PCI) += hotplug/
 ifdef CONFIG_HOTPLUG_PCI
diff --git a/drivers/pci/host/pci-dra7xx.c b/drivers/pci/host/pci-dra7xx.c
index 8c6969747acd..2d57e19a2cd4 100644
--- a/drivers/pci/host/pci-dra7xx.c
+++ b/drivers/pci/host/pci-dra7xx.c
@@ -446,7 +446,6 @@ static struct platform_driver dra7xx_pcie_driver = {
 	.remove		= __exit_p(dra7xx_pcie_remove),
 	.driver = {
 		.name	= "dra7-pcie",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_dra7xx_pcie_match,
 	},
 };
diff --git a/drivers/pci/host/pci-exynos.c b/drivers/pci/host/pci-exynos.c
index 850c9f951a3f..d202b37c3698 100644
--- a/drivers/pci/host/pci-exynos.c
+++ b/drivers/pci/host/pci-exynos.c
@@ -641,7 +641,6 @@ static struct platform_driver exynos_pcie_driver = {
 	.remove		= __exit_p(exynos_pcie_remove),
 	.driver = {
 		.name	= "exynos-pcie",
-		.owner	= THIS_MODULE,
 		.of_match_table = exynos_pcie_of_match,
 	},
 };
diff --git a/drivers/pci/host/pci-host-generic.c b/drivers/pci/host/pci-host-generic.c
index 18959075d164..6eb1aa75bd37 100644
--- a/drivers/pci/host/pci-host-generic.c
+++ b/drivers/pci/host/pci-host-generic.c
@@ -310,7 +310,6 @@ static int gen_pci_probe(struct platform_device *pdev)
 static struct platform_driver gen_pci_driver = {
 	.driver = {
 		.name = "pci-host-generic",
-		.owner = THIS_MODULE,
 		.of_match_table = gen_pci_of_match,
 	},
 	.probe = gen_pci_probe,
diff --git a/drivers/pci/host/pci-imx6.c b/drivers/pci/host/pci-imx6.c
index d1a26d17b586..fdb95367721e 100644
--- a/drivers/pci/host/pci-imx6.c
+++ b/drivers/pci/host/pci-imx6.c
@@ -643,7 +643,6 @@ MODULE_DEVICE_TABLE(of, imx6_pcie_of_match);
 static struct platform_driver imx6_pcie_driver = {
 	.driver = {
 		.name	= "imx6q-pcie",
-		.owner	= THIS_MODULE,
 		.of_match_table = imx6_pcie_of_match,
 	},
 	.shutdown = imx6_pcie_shutdown,
diff --git a/drivers/pci/host/pci-keystone.c b/drivers/pci/host/pci-keystone.c
index 62b9454c86fb..78f79e31ac5c 100644
--- a/drivers/pci/host/pci-keystone.c
+++ b/drivers/pci/host/pci-keystone.c
@@ -402,7 +402,6 @@ static struct platform_driver ks_pcie_driver __refdata = {
 	.remove = __exit_p(ks_pcie_remove),
 	.driver = {
 		.name	= "keystone-pcie",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(ks_pcie_of_match),
 	},
 };
diff --git a/drivers/pci/host/pci-mvebu.c b/drivers/pci/host/pci-mvebu.c
index fed3fab132f2..1dd759596b0a 100644
--- a/drivers/pci/host/pci-mvebu.c
+++ b/drivers/pci/host/pci-mvebu.c
@@ -1081,7 +1081,6 @@ MODULE_DEVICE_TABLE(of, mvebu_pcie_of_match_table);
 
 static struct platform_driver mvebu_pcie_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "mvebu-pcie",
 		.of_match_table = mvebu_pcie_of_match_table,
 		/* driver unloading/unbinding currently not supported */
diff --git a/drivers/pci/host/pci-rcar-gen2.c b/drivers/pci/host/pci-rcar-gen2.c
index 3ef854f5a5b5..d9c042febb1a 100644
--- a/drivers/pci/host/pci-rcar-gen2.c
+++ b/drivers/pci/host/pci-rcar-gen2.c
@@ -412,7 +412,6 @@ MODULE_DEVICE_TABLE(of, rcar_pci_of_match);
 static struct platform_driver rcar_pci_driver = {
 	.driver = {
 		.name = "pci-rcar-gen2",
-		.owner = THIS_MODULE,
 		.suppress_bind_attrs = true,
 		.of_match_table = rcar_pci_of_match,
 	},
diff --git a/drivers/pci/host/pci-tegra.c b/drivers/pci/host/pci-tegra.c
index feccfa6b6c11..a800ae916394 100644
--- a/drivers/pci/host/pci-tegra.c
+++ b/drivers/pci/host/pci-tegra.c
@@ -2129,7 +2129,6 @@ put_resources:
 static struct platform_driver tegra_pcie_driver = {
 	.driver = {
 		.name = "tegra-pcie",
-		.owner = THIS_MODULE,
 		.of_match_table = tegra_pcie_of_match,
 		.suppress_bind_attrs = true,
 	},
diff --git a/drivers/pci/host/pci-xgene.c b/drivers/pci/host/pci-xgene.c
index 2988fe136c1e..b1d0596457c5 100644
--- a/drivers/pci/host/pci-xgene.c
+++ b/drivers/pci/host/pci-xgene.c
@@ -652,7 +652,6 @@ static const struct of_device_id xgene_pcie_match_table[] = {
 static struct platform_driver xgene_pcie_driver = {
 	.driver = {
 		   .name = "xgene-pcie",
-		   .owner = THIS_MODULE,
 		   .of_match_table = of_match_ptr(xgene_pcie_match_table),
 	},
 	.probe = xgene_pcie_probe_bridge,
diff --git a/drivers/pci/host/pcie-rcar.c b/drivers/pci/host/pcie-rcar.c
index 5519e939e412..748786c402fc 100644
--- a/drivers/pci/host/pcie-rcar.c
+++ b/drivers/pci/host/pcie-rcar.c
@@ -981,7 +981,6 @@ static int rcar_pcie_probe(struct platform_device *pdev)
 static struct platform_driver rcar_pcie_driver = {
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = rcar_pcie_of_match,
 		.suppress_bind_attrs = true,
 	},
diff --git a/drivers/pci/host/pcie-spear13xx.c b/drivers/pci/host/pcie-spear13xx.c
index 2ca10cc887ee..866465fd3dbf 100644
--- a/drivers/pci/host/pcie-spear13xx.c
+++ b/drivers/pci/host/pcie-spear13xx.c
@@ -374,7 +374,6 @@ static struct platform_driver spear13xx_pcie_driver __initdata = {
 	.probe		= spear13xx_pcie_probe,
 	.driver = {
 		.name	= "spear-pcie",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(spear13xx_pcie_of_match),
 	},
 };
diff --git a/drivers/pci/host/pcie-xilinx.c b/drivers/pci/host/pcie-xilinx.c
index 2f50fa5953fd..ef3ebaf9a738 100644
--- a/drivers/pci/host/pcie-xilinx.c
+++ b/drivers/pci/host/pcie-xilinx.c
@@ -947,7 +947,6 @@ static struct of_device_id xilinx_pcie_of_match[] = {
 static struct platform_driver xilinx_pcie_driver = {
 	.driver = {
 		.name = "xilinx-pcie",
-		.owner = THIS_MODULE,
 		.of_match_table = xilinx_pcie_of_match,
 		.suppress_bind_attrs = true,
 	},
diff --git a/drivers/pci/hotplug/ibmphp_core.c b/drivers/pci/hotplug/ibmphp_core.c
index 3efaf4c38528..96c5c729cdbc 100644
--- a/drivers/pci/hotplug/ibmphp_core.c
+++ b/drivers/pci/hotplug/ibmphp_core.c
@@ -36,6 +36,7 @@
 #include <linux/wait.h>
 #include "../pci.h"
 #include <asm/pci_x86.h>		/* for struct irq_routing_table */
+#include <asm/io_apic.h>
 #include "ibmphp.h"
 
 #define attn_on(sl)  ibmphp_hpc_writeslot (sl, HPC_SLOT_ATTNON)
@@ -155,13 +156,10 @@ int ibmphp_init_devno(struct slot **cur_slot)
 	for (loop = 0; loop < len; loop++) {
 		if ((*cur_slot)->number == rtable->slots[loop].slot &&
 		    (*cur_slot)->bus == rtable->slots[loop].bus) {
-			struct io_apic_irq_attr irq_attr;
-
 			(*cur_slot)->device = PCI_SLOT(rtable->slots[loop].devfn);
 			for (i = 0; i < 4; i++)
 				(*cur_slot)->irq[i] = IO_APIC_get_PCI_irq_vector((int) (*cur_slot)->bus,
-						(int) (*cur_slot)->device, i,
-						&irq_attr);
+						(int) (*cur_slot)->device, i);
 
 			debug("(*cur_slot)->irq[0] = %x\n",
 					(*cur_slot)->irq[0]);
diff --git a/drivers/pci/ioapic.c b/drivers/pci/ioapic.c
deleted file mode 100644
index f6219d36227f..000000000000
--- a/drivers/pci/ioapic.c
+++ /dev/null
@@ -1,121 +0,0 @@
-/*
- * IOAPIC/IOxAPIC/IOSAPIC driver
- *
- * Copyright (C) 2009 Fujitsu Limited.
- * (c) Copyright 2009 Hewlett-Packard Development Company, L.P.
- *
- * 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 driver manages PCI I/O APICs added by hotplug after boot.  We try to
- * claim all I/O APIC PCI devices, but those present at boot were registered
- * when we parsed the ACPI MADT, so we'll fail when we try to re-register
- * them.
- */
-
-#include <linux/pci.h>
-#include <linux/module.h>
-#include <linux/acpi.h>
-#include <linux/slab.h>
-
-struct ioapic {
-	acpi_handle	handle;
-	u32		gsi_base;
-};
-
-static int ioapic_probe(struct pci_dev *dev, const struct pci_device_id *ent)
-{
-	acpi_handle handle;
-	acpi_status status;
-	unsigned long long gsb;
-	struct ioapic *ioapic;
-	int ret;
-	char *type;
-	struct resource *res;
-
-	handle = ACPI_HANDLE(&dev->dev);
-	if (!handle)
-		return -EINVAL;
-
-	status = acpi_evaluate_integer(handle, "_GSB", NULL, &gsb);
-	if (ACPI_FAILURE(status))
-		return -EINVAL;
-
-	/*
-	 * The previous code in acpiphp evaluated _MAT if _GSB failed, but
-	 * ACPI spec 4.0 sec 6.2.2 requires _GSB for hot-pluggable I/O APICs.
-	 */
-
-	ioapic = kzalloc(sizeof(*ioapic), GFP_KERNEL);
-	if (!ioapic)
-		return -ENOMEM;
-
-	ioapic->handle = handle;
-	ioapic->gsi_base = (u32) gsb;
-
-	if (dev->class == PCI_CLASS_SYSTEM_PIC_IOAPIC)
-		type = "IOAPIC";
-	else
-		type = "IOxAPIC";
-
-	ret = pci_enable_device(dev);
-	if (ret < 0)
-		goto exit_free;
-
-	pci_set_master(dev);
-
-	if (pci_request_region(dev, 0, type))
-		goto exit_disable;
-
-	res = &dev->resource[0];
-	if (acpi_register_ioapic(ioapic->handle, res->start, ioapic->gsi_base))
-		goto exit_release;
-
-	pci_set_drvdata(dev, ioapic);
-	dev_info(&dev->dev, "%s at %pR, GSI %u\n", type, res, ioapic->gsi_base);
-	return 0;
-
-exit_release:
-	pci_release_region(dev, 0);
-exit_disable:
-	pci_disable_device(dev);
-exit_free:
-	kfree(ioapic);
-	return -ENODEV;
-}
-
-static void ioapic_remove(struct pci_dev *dev)
-{
-	struct ioapic *ioapic = pci_get_drvdata(dev);
-
-	acpi_unregister_ioapic(ioapic->handle, ioapic->gsi_base);
-	pci_release_region(dev, 0);
-	pci_disable_device(dev);
-	kfree(ioapic);
-}
-
-
-static const struct pci_device_id ioapic_devices[] = {
-	{ PCI_DEVICE_CLASS(PCI_CLASS_SYSTEM_PIC_IOAPIC, ~0) },
-	{ PCI_DEVICE_CLASS(PCI_CLASS_SYSTEM_PIC_IOXAPIC, ~0) },
-	{ }
-};
-MODULE_DEVICE_TABLE(pci, ioapic_devices);
-
-static struct pci_driver ioapic_driver = {
-	.name		= "ioapic",
-	.id_table	= ioapic_devices,
-	.probe		= ioapic_probe,
-	.remove		= ioapic_remove,
-};
-
-static int __init ioapic_init(void)
-{
-	return pci_register_driver(&ioapic_driver);
-}
-module_init(ioapic_init);
-
-MODULE_LICENSE("GPL");
diff --git a/drivers/pci/pci-sysfs.c b/drivers/pci/pci-sysfs.c
index a62acc443d5b..aa012fb3834b 100644
--- a/drivers/pci/pci-sysfs.c
+++ b/drivers/pci/pci-sysfs.c
@@ -77,11 +77,10 @@ static ssize_t broken_parity_status_store(struct device *dev,
 }
 static DEVICE_ATTR_RW(broken_parity_status);
 
-static ssize_t pci_dev_show_local_cpu(struct device *dev, int type,
+static ssize_t pci_dev_show_local_cpu(struct device *dev, bool list,
 				      struct device_attribute *attr, char *buf)
 {
 	const struct cpumask *mask;
-	int len;
 
 #ifdef CONFIG_NUMA
 	mask = (dev_to_node(dev) == -1) ? cpu_online_mask :
@@ -89,59 +88,41 @@ static ssize_t pci_dev_show_local_cpu(struct device *dev, int type,
 #else
 	mask = cpumask_of_pcibus(to_pci_dev(dev)->bus);
 #endif
-	len = type ?
-		cpumask_scnprintf(buf, PAGE_SIZE-2, mask) :
-		cpulist_scnprintf(buf, PAGE_SIZE-2, mask);
-
-	buf[len++] = '\n';
-	buf[len] = '\0';
-	return len;
+	return cpumap_print_to_pagebuf(list, buf, mask);
 }
 
 static ssize_t local_cpus_show(struct device *dev,
 			       struct device_attribute *attr, char *buf)
 {
-	return pci_dev_show_local_cpu(dev, 1, attr, buf);
+	return pci_dev_show_local_cpu(dev, false, attr, buf);
 }
 static DEVICE_ATTR_RO(local_cpus);
 
 static ssize_t local_cpulist_show(struct device *dev,
 				  struct device_attribute *attr, char *buf)
 {
-	return pci_dev_show_local_cpu(dev, 0, attr, buf);
+	return pci_dev_show_local_cpu(dev, true, attr, buf);
 }
 static DEVICE_ATTR_RO(local_cpulist);
 
 /*
  * PCI Bus Class Devices
  */
-static ssize_t pci_bus_show_cpuaffinity(struct device *dev, int type,
-					struct device_attribute *attr,
-					char *buf)
-{
-	int ret;
-	const struct cpumask *cpumask;
-
-	cpumask = cpumask_of_pcibus(to_pci_bus(dev));
-	ret = type ?
-		cpulist_scnprintf(buf, PAGE_SIZE-2, cpumask) :
-		cpumask_scnprintf(buf, PAGE_SIZE-2, cpumask);
-	buf[ret++] = '\n';
-	buf[ret] = '\0';
-	return ret;
-}
-
 static ssize_t cpuaffinity_show(struct device *dev,
 				struct device_attribute *attr, char *buf)
 {
-	return pci_bus_show_cpuaffinity(dev, 0, attr, buf);
+	const struct cpumask *cpumask = cpumask_of_pcibus(to_pci_bus(dev));
+
+	return cpumap_print_to_pagebuf(false, buf, cpumask);
 }
 static DEVICE_ATTR_RO(cpuaffinity);
 
 static ssize_t cpulistaffinity_show(struct device *dev,
 				    struct device_attribute *attr, char *buf)
 {
-	return pci_bus_show_cpuaffinity(dev, 1, attr, buf);
+	const struct cpumask *cpumask = cpumask_of_pcibus(to_pci_bus(dev));
+
+	return cpumap_print_to_pagebuf(true, buf, cpumask);
 }
 static DEVICE_ATTR_RO(cpulistaffinity);
 
diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index a7ac72639c52..cab05f31223f 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -1138,8 +1138,8 @@ EXPORT_SYMBOL_GPL(pci_store_saved_state);
  * @dev: PCI device that we're dealing with
  * @state: Saved state returned from pci_store_saved_state()
  */
-static int pci_load_saved_state(struct pci_dev *dev,
-				struct pci_saved_state *state)
+int pci_load_saved_state(struct pci_dev *dev,
+			 struct pci_saved_state *state)
 {
 	struct pci_cap_saved_data *cap;
 
@@ -1167,6 +1167,7 @@ static int pci_load_saved_state(struct pci_dev *dev,
 	dev->state_saved = true;
 	return 0;
 }
+EXPORT_SYMBOL_GPL(pci_load_saved_state);
 
 /**
  * pci_load_and_free_saved_state - Reload the save state pointed to by state,
diff --git a/drivers/pci/quirks.c b/drivers/pci/quirks.c
index 90acb32c85b1..ed6f89b6efe5 100644
--- a/drivers/pci/quirks.c
+++ b/drivers/pci/quirks.c
@@ -379,6 +379,26 @@ static void quirk_ati_exploding_mce(struct pci_dev *dev)
 DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_ATI,	PCI_DEVICE_ID_ATI_RS100,   quirk_ati_exploding_mce);
 
 /*
+ * In the AMD NL platform, this device ([1022:7912]) has a class code of
+ * PCI_CLASS_SERIAL_USB_XHCI (0x0c0330), which means the xhci driver will
+ * claim it.
+ * But the dwc3 driver is a more specific driver for this device, and we'd
+ * prefer to use it instead of xhci. To prevent xhci from claiming the
+ * device, change the class code to 0x0c03fe, which the PCI r3.0 spec
+ * defines as "USB device (not host controller)". The dwc3 driver can then
+ * claim it based on its Vendor and Device ID.
+ */
+static void quirk_amd_nl_class(struct pci_dev *pdev)
+{
+	/*
+	 * Use 'USB Device' (0x0c03fe) instead of PCI header provided
+	 */
+	pdev->class = 0x0c03fe;
+}
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_AMD, PCI_DEVICE_ID_AMD_NL_USB,
+		quirk_amd_nl_class);
+
+/*
  * Let's make the southbridge information explicit instead
  * of having to worry about people probing the ACPI areas,
  * for example.. (Yes, it happens, and if you read the wrong
diff --git a/drivers/pcmcia/Kconfig b/drivers/pcmcia/Kconfig
index b0ce7cdee0c2..910e90bf16c6 100644
--- a/drivers/pcmcia/Kconfig
+++ b/drivers/pcmcia/Kconfig
@@ -147,7 +147,6 @@ config TCIC
 config PCMCIA_ALCHEMY_DEVBOARD
 	tristate "Alchemy Db/Pb1xxx PCMCIA socket services"
 	depends on MIPS_ALCHEMY && PCMCIA
-	select 64BIT_PHYS_ADDR
 	help
 	  Enable this driver of you want PCMCIA support on your Alchemy
 	  Db1000, Db/Pb1100, Db/Pb1500, Db/Pb1550, Db/Pb1200, DB1300
@@ -158,7 +157,6 @@ config PCMCIA_ALCHEMY_DEVBOARD
 config PCMCIA_XXS1500
 	tristate "MyCable XXS1500 PCMCIA socket support"
 	depends on PCMCIA && MIPS_XXS1500
-	select 64BIT_PHYS_ADDR
 	help
 	  Support for the PCMCIA/CF socket interface on MyCable XXS1500
 	  systems.
diff --git a/drivers/pcmcia/at91_cf.c b/drivers/pcmcia/at91_cf.c
index de24232c5191..bfb799c7b343 100644
--- a/drivers/pcmcia/at91_cf.c
+++ b/drivers/pcmcia/at91_cf.c
@@ -401,7 +401,6 @@ static int at91_cf_resume(struct platform_device *pdev)
 static struct platform_driver at91_cf_driver = {
 	.driver = {
 		.name		= "at91_cf",
-		.owner		= THIS_MODULE,
 		.of_match_table = of_match_ptr(at91_cf_dt_ids),
 	},
 	.probe		= at91_cf_probe,
diff --git a/drivers/pcmcia/bfin_cf_pcmcia.c b/drivers/pcmcia/bfin_cf_pcmcia.c
index 971991bab975..bba1dcbb8075 100644
--- a/drivers/pcmcia/bfin_cf_pcmcia.c
+++ b/drivers/pcmcia/bfin_cf_pcmcia.c
@@ -304,7 +304,6 @@ static int bfin_cf_remove(struct platform_device *pdev)
 static struct platform_driver bfin_cf_driver = {
 	.driver = {
 		   .name = driver_name,
-		   .owner = THIS_MODULE,
 		   },
 	.probe = bfin_cf_probe,
 	.remove = bfin_cf_remove,
diff --git a/drivers/pcmcia/db1xxx_ss.c b/drivers/pcmcia/db1xxx_ss.c
index a31e69ea99f3..4c2fa05b4589 100644
--- a/drivers/pcmcia/db1xxx_ss.c
+++ b/drivers/pcmcia/db1xxx_ss.c
@@ -574,7 +574,6 @@ static int db1x_pcmcia_socket_remove(struct platform_device *pdev)
 static struct platform_driver db1x_pcmcia_socket_driver = {
 	.driver	= {
 		.name	= "db1xxx_pcmcia",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= db1x_pcmcia_socket_probe,
 	.remove		= db1x_pcmcia_socket_remove,
diff --git a/drivers/pcmcia/electra_cf.c b/drivers/pcmcia/electra_cf.c
index 5ea64d0f61ab..7f9950d324df 100644
--- a/drivers/pcmcia/electra_cf.c
+++ b/drivers/pcmcia/electra_cf.c
@@ -360,7 +360,6 @@ MODULE_DEVICE_TABLE(of, electra_cf_match);
 static struct platform_driver electra_cf_driver = {
 	.driver = {
 		.name = driver_name,
-		.owner = THIS_MODULE,
 		.of_match_table = electra_cf_match,
 	},
 	.probe	  = electra_cf_probe,
diff --git a/drivers/pcmcia/i82365.c b/drivers/pcmcia/i82365.c
index e6f3d17dd2b4..a2c138719bac 100644
--- a/drivers/pcmcia/i82365.c
+++ b/drivers/pcmcia/i82365.c
@@ -1233,7 +1233,6 @@ static struct pccard_operations pcic_operations = {
 static struct platform_driver i82365_driver = {
 	.driver = {
 		.name = "i82365",
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/pcmcia/m32r_cfc.c b/drivers/pcmcia/m32r_cfc.c
index a26f38c6402a..0075bd7162ed 100644
--- a/drivers/pcmcia/m32r_cfc.c
+++ b/drivers/pcmcia/m32r_cfc.c
@@ -687,7 +687,6 @@ static struct pccard_operations pcc_operations = {
 static struct platform_driver pcc_driver = {
 	.driver = {
 		.name		= "cfc",
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/pcmcia/m32r_pcc.c b/drivers/pcmcia/m32r_pcc.c
index 296514155cd5..a77e571b08b8 100644
--- a/drivers/pcmcia/m32r_pcc.c
+++ b/drivers/pcmcia/m32r_pcc.c
@@ -664,7 +664,6 @@ static struct pccard_operations pcc_operations = {
 static struct platform_driver pcc_driver = {
 	.driver = {
 		.name		= "pcc",
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/pcmcia/omap_cf.c b/drivers/pcmcia/omap_cf.c
index 25c4b1993b3d..8170102d1e93 100644
--- a/drivers/pcmcia/omap_cf.c
+++ b/drivers/pcmcia/omap_cf.c
@@ -334,7 +334,6 @@ static int __exit omap_cf_remove(struct platform_device *pdev)
 static struct platform_driver omap_cf_driver = {
 	.driver = {
 		.name	= (char *) driver_name,
-		.owner	= THIS_MODULE,
 	},
 	.remove		= __exit_p(omap_cf_remove),
 };
diff --git a/drivers/pcmcia/pxa2xx_base.c b/drivers/pcmcia/pxa2xx_base.c
index cfec9dd18ff5..984a8ff559d8 100644
--- a/drivers/pcmcia/pxa2xx_base.c
+++ b/drivers/pcmcia/pxa2xx_base.c
@@ -368,7 +368,6 @@ static struct platform_driver pxa2xx_pcmcia_driver = {
 	.remove		= pxa2xx_drv_pcmcia_remove,
 	.driver		= {
 		.name	= "pxa2xx-pcmcia",
-		.owner	= THIS_MODULE,
 		.pm	= &pxa2xx_drv_pcmcia_pm_ops,
 	},
 };
diff --git a/drivers/pcmcia/pxa2xx_viper.c b/drivers/pcmcia/pxa2xx_viper.c
index a76f495953ab..7ac6647d286e 100644
--- a/drivers/pcmcia/pxa2xx_viper.c
+++ b/drivers/pcmcia/pxa2xx_viper.c
@@ -172,7 +172,6 @@ static struct platform_driver viper_pcmcia_driver = {
 	.remove		= viper_pcmcia_remove,
 	.driver		= {
 		.name	= "arcom-pcmcia",
-		.owner	= THIS_MODULE,
 	},
 	.id_table	= viper_pcmcia_id_table,
 };
diff --git a/drivers/pcmcia/sa1100_generic.c b/drivers/pcmcia/sa1100_generic.c
index ff8a027a4afb..803945259da8 100644
--- a/drivers/pcmcia/sa1100_generic.c
+++ b/drivers/pcmcia/sa1100_generic.c
@@ -93,6 +93,7 @@ static int sa11x0_drv_pcmcia_remove(struct platform_device *dev)
 	for (i = 0; i < sinfo->nskt; i++)
 		soc_pcmcia_remove_one(&sinfo->skt[i]);
 
+	clk_put(sinfo->clk);
 	kfree(sinfo);
 	return 0;
 }
@@ -100,7 +101,6 @@ static int sa11x0_drv_pcmcia_remove(struct platform_device *dev)
 static struct platform_driver sa11x0_pcmcia_driver = {
 	.driver = {
 		.name		= "sa11x0-pcmcia",
-		.owner		= THIS_MODULE,
 	},
 	.probe		= sa11x0_drv_pcmcia_probe,
 	.remove		= sa11x0_drv_pcmcia_remove,
diff --git a/drivers/pcmcia/sa1111_badge4.c b/drivers/pcmcia/sa1111_badge4.c
index 4d206f4dd67b..12f0dd091477 100644
--- a/drivers/pcmcia/sa1111_badge4.c
+++ b/drivers/pcmcia/sa1111_badge4.c
@@ -132,7 +132,7 @@ static struct pcmcia_low_level badge4_pcmcia_ops = {
 	.nr			= 2,
 };
 
-int pcmcia_badge4_init(struct device *dev)
+int pcmcia_badge4_init(struct sa1111_dev *dev)
 {
 	int ret = -ENODEV;
 
diff --git a/drivers/pcmcia/sa1111_generic.c b/drivers/pcmcia/sa1111_generic.c
index 65b02c3e14ce..80b8e9d05275 100644
--- a/drivers/pcmcia/sa1111_generic.c
+++ b/drivers/pcmcia/sa1111_generic.c
@@ -145,6 +145,12 @@ int sa1111_pcmcia_add(struct sa1111_dev *dev, struct pcmcia_low_level *ops,
 			return -ENOMEM;
 
 		s->soc.nr = ops->first + i;
+		s->soc.clk = clk_get(&dev->dev, NULL);
+		if (IS_ERR(s->soc.clk)) {
+			ret = PTR_ERR(s->soc.clk);
+			kfree(s);
+			return ret;
+		}
 		soc_pcmcia_init_one(&s->soc, ops, &dev->dev);
 		s->dev = dev;
 		if (s->soc.nr) {
@@ -197,10 +203,10 @@ static int pcmcia_probe(struct sa1111_dev *dev)
 	sa1111_writel(PCCR_S0_FLT | PCCR_S1_FLT, base + PCCR);
 
 #ifdef CONFIG_SA1100_BADGE4
-	pcmcia_badge4_init(&dev->dev);
+	pcmcia_badge4_init(dev);
 #endif
 #ifdef CONFIG_SA1100_JORNADA720
-	pcmcia_jornada720_init(&dev->dev);
+	pcmcia_jornada720_init(dev);
 #endif
 #ifdef CONFIG_ARCH_LUBBOCK
 	pcmcia_lubbock_init(dev);
@@ -220,6 +226,7 @@ static int pcmcia_remove(struct sa1111_dev *dev)
 	for (; s; s = next) {
 		next = s->next;
 		soc_pcmcia_remove_one(&s->soc);
+		clk_put(s->soc.clk);
 		kfree(s);
 	}
 
diff --git a/drivers/pcmcia/sa1111_generic.h b/drivers/pcmcia/sa1111_generic.h
index f6376e34a7e4..e74ecfdc1b26 100644
--- a/drivers/pcmcia/sa1111_generic.h
+++ b/drivers/pcmcia/sa1111_generic.h
@@ -18,8 +18,8 @@ int sa1111_pcmcia_add(struct sa1111_dev *dev, struct pcmcia_low_level *ops,
 extern void sa1111_pcmcia_socket_state(struct soc_pcmcia_socket *, struct pcmcia_state *);
 extern int sa1111_pcmcia_configure_socket(struct soc_pcmcia_socket *, const socket_state_t *);
 
-extern int pcmcia_badge4_init(struct device *);
-extern int pcmcia_jornada720_init(struct device *);
+extern int pcmcia_badge4_init(struct sa1111_dev *);
+extern int pcmcia_jornada720_init(struct sa1111_dev *);
 extern int pcmcia_lubbock_init(struct sa1111_dev *);
 extern int pcmcia_neponset_init(struct sa1111_dev *);
 
diff --git a/drivers/pcmcia/sa1111_jornada720.c b/drivers/pcmcia/sa1111_jornada720.c
index 40e040314503..c2c30580c83f 100644
--- a/drivers/pcmcia/sa1111_jornada720.c
+++ b/drivers/pcmcia/sa1111_jornada720.c
@@ -92,10 +92,9 @@ static struct pcmcia_low_level jornada720_pcmcia_ops = {
 	.nr			= 2,
 };
 
-int pcmcia_jornada720_init(struct device *dev)
+int pcmcia_jornada720_init(struct sa1111_dev *sadev)
 {
 	int ret = -ENODEV;
-	struct sa1111_dev *sadev = SA1111_DEV(dev);
 
 	if (machine_is_jornada720()) {
 		unsigned int pin = GPIO_A0 | GPIO_A1 | GPIO_A2 | GPIO_A3;
diff --git a/drivers/pcmcia/sa11xx_base.c b/drivers/pcmcia/sa11xx_base.c
index 54d3089d157b..cf6de2c2b329 100644
--- a/drivers/pcmcia/sa11xx_base.c
+++ b/drivers/pcmcia/sa11xx_base.c
@@ -135,14 +135,16 @@ sa1100_pcmcia_frequency_change(struct soc_pcmcia_socket *skt,
 static int
 sa1100_pcmcia_set_timing(struct soc_pcmcia_socket *skt)
 {
-	return sa1100_pcmcia_set_mecr(skt, cpufreq_get(0));
+	unsigned long clk = clk_get_rate(skt->clk);
+
+	return sa1100_pcmcia_set_mecr(skt, clk / 1000);
 }
 
 static int
 sa1100_pcmcia_show_timing(struct soc_pcmcia_socket *skt, char *buf)
 {
 	struct soc_pcmcia_timing timing;
-	unsigned int clock = cpufreq_get(0);
+	unsigned int clock = clk_get_rate(skt->clk);
 	unsigned long mecr = MECR;
 	char *p = buf;
 
@@ -218,6 +220,11 @@ int sa11xx_drv_pcmcia_probe(struct device *dev, struct pcmcia_low_level *ops,
 	struct skt_dev_info *sinfo;
 	struct soc_pcmcia_socket *skt;
 	int i, ret = 0;
+	struct clk *clk;
+
+	clk = clk_get(dev, NULL);
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
 
 	sa11xx_drv_pcmcia_ops(ops);
 
@@ -226,12 +233,14 @@ int sa11xx_drv_pcmcia_probe(struct device *dev, struct pcmcia_low_level *ops,
 		return -ENOMEM;
 
 	sinfo->nskt = nr;
+	sinfo->clk = clk;
 
 	/* Initialize processor specific parameters */
 	for (i = 0; i < nr; i++) {
 		skt = &sinfo->skt[i];
 
 		skt->nr = first + i;
+		skt->clk = clk;
 		soc_pcmcia_init_one(skt, ops, dev);
 
 		ret = sa11xx_drv_pcmcia_add_one(skt);
@@ -242,6 +251,7 @@ int sa11xx_drv_pcmcia_probe(struct device *dev, struct pcmcia_low_level *ops,
 	if (ret) {
 		while (--i >= 0)
 			soc_pcmcia_remove_one(&sinfo->skt[i]);
+		clk_put(clk);
 		kfree(sinfo);
 	} else {
 		dev_set_drvdata(dev, sinfo);
diff --git a/drivers/pcmcia/soc_common.c b/drivers/pcmcia/soc_common.c
index a2bc6ee1702e..933f4657515b 100644
--- a/drivers/pcmcia/soc_common.c
+++ b/drivers/pcmcia/soc_common.c
@@ -120,6 +120,8 @@ static void __soc_pcmcia_hw_shutdown(struct soc_pcmcia_socket *skt,
 
 	if (skt->ops->hw_shutdown)
 		skt->ops->hw_shutdown(skt);
+
+	clk_disable_unprepare(skt->clk);
 }
 
 static void soc_pcmcia_hw_shutdown(struct soc_pcmcia_socket *skt)
@@ -131,6 +133,8 @@ static int soc_pcmcia_hw_init(struct soc_pcmcia_socket *skt)
 {
 	int ret = 0, i;
 
+	clk_prepare_enable(skt->clk);
+
 	if (skt->ops->hw_init) {
 		ret = skt->ops->hw_init(skt);
 		if (ret)
diff --git a/drivers/pcmcia/tcic.c b/drivers/pcmcia/tcic.c
index cbe15fc37411..1ee63e5f0550 100644
--- a/drivers/pcmcia/tcic.c
+++ b/drivers/pcmcia/tcic.c
@@ -349,7 +349,6 @@ static int __init get_tcic_id(void)
 static struct platform_driver tcic_driver = {
 	.driver = {
 		.name = "tcic-pcmcia",
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/pcmcia/vrc4171_card.c b/drivers/pcmcia/vrc4171_card.c
index d98a08612492..21973d55a055 100644
--- a/drivers/pcmcia/vrc4171_card.c
+++ b/drivers/pcmcia/vrc4171_card.c
@@ -709,7 +709,6 @@ __setup("vrc4171_card=", vrc4171_card_setup);
 static struct platform_driver vrc4171_card_driver = {
 	.driver = {
 		.name		= vrc4171_card_name,
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/pcmcia/xxs1500_ss.c b/drivers/pcmcia/xxs1500_ss.c
index 95f5b270ad44..4c04360f378b 100644
--- a/drivers/pcmcia/xxs1500_ss.c
+++ b/drivers/pcmcia/xxs1500_ss.c
@@ -314,7 +314,6 @@ static int xxs1500_pcmcia_remove(struct platform_device *pdev)
 static struct platform_driver xxs1500_pcmcia_socket_driver = {
 	.driver	= {
 		.name	= "xxs1500_pcmcia",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= xxs1500_pcmcia_probe,
 	.remove		= xxs1500_pcmcia_remove,
diff --git a/drivers/phy/Kconfig b/drivers/phy/Kconfig
index 2a436e607f99..ccad8809ecb1 100644
--- a/drivers/phy/Kconfig
+++ b/drivers/phy/Kconfig
@@ -15,6 +15,13 @@ config GENERIC_PHY
 	  phy users can obtain reference to the PHY. All the users of this
 	  framework should select this config.
 
+config PHY_BERLIN_USB
+	tristate "Marvell Berlin USB PHY Driver"
+	depends on ARCH_BERLIN && RESET_CONTROLLER && HAS_IOMEM && OF
+	select GENERIC_PHY
+	help
+	  Enable this to support the USB PHY on Marvell Berlin SoCs.
+
 config PHY_BERLIN_SATA
 	tristate "Marvell Berlin SATA PHY driver"
 	depends on ARCH_BERLIN && HAS_IOMEM && OF
@@ -22,6 +29,12 @@ config PHY_BERLIN_SATA
 	help
 	  Enable this to support the SATA PHY on Marvell Berlin SoCs.
 
+config ARMADA375_USBCLUSTER_PHY
+	def_bool y
+	depends on MACH_ARMADA_375 || COMPILE_TEST
+	depends on OF
+	select GENERIC_PHY
+
 config PHY_EXYNOS_MIPI_VIDEO
 	tristate "S5P/EXYNOS SoC series MIPI CSI-2/DSI PHY driver"
 	depends on HAS_IOMEM
@@ -38,6 +51,14 @@ config PHY_MVEBU_SATA
 	depends on OF
 	select GENERIC_PHY
 
+config PHY_MIPHY28LP
+	tristate "STMicroelectronics MIPHY28LP PHY driver for STiH407"
+	depends on ARCH_STI
+	select GENERIC_PHY
+	help
+	  Enable this to support the miphy transceiver (for SATA/PCIE/USB3)
+	  that is part of STMicroelectronics STiH407 SoC.
+
 config PHY_MIPHY365X
 	tristate "STMicroelectronics MIPHY365X PHY driver for STiH41x series"
 	depends on ARCH_STI
@@ -193,7 +214,7 @@ config PHY_EXYNOS5250_USB2
 
 config PHY_EXYNOS5_USBDRD
 	tristate "Exynos5 SoC series USB DRD PHY driver"
-	depends on ARCH_EXYNOS5 && OF
+	depends on ARCH_EXYNOS && OF
 	depends on HAS_IOMEM
 	depends on USB_DWC3_EXYNOS
 	select GENERIC_PHY
diff --git a/drivers/phy/Makefile b/drivers/phy/Makefile
index c4590fce082f..aa74f961e44e 100644
--- a/drivers/phy/Makefile
+++ b/drivers/phy/Makefile
@@ -3,11 +3,14 @@
 #
 
 obj-$(CONFIG_GENERIC_PHY)		+= phy-core.o
+obj-$(CONFIG_PHY_BERLIN_USB)		+= phy-berlin-usb.o
 obj-$(CONFIG_PHY_BERLIN_SATA)		+= phy-berlin-sata.o
+obj-$(CONFIG_ARMADA375_USBCLUSTER_PHY)	+= phy-armada375-usb2.o
 obj-$(CONFIG_BCM_KONA_USB2_PHY)		+= phy-bcm-kona-usb2.o
 obj-$(CONFIG_PHY_EXYNOS_DP_VIDEO)	+= phy-exynos-dp-video.o
 obj-$(CONFIG_PHY_EXYNOS_MIPI_VIDEO)	+= phy-exynos-mipi-video.o
 obj-$(CONFIG_PHY_MVEBU_SATA)		+= phy-mvebu-sata.o
+obj-$(CONFIG_PHY_MIPHY28LP) 		+= phy-miphy28lp.o
 obj-$(CONFIG_PHY_MIPHY365X)		+= phy-miphy365x.o
 obj-$(CONFIG_PHY_RCAR_GEN2)		+= phy-rcar-gen2.o
 obj-$(CONFIG_OMAP_CONTROL_PHY)		+= phy-omap-control.o
diff --git a/drivers/phy/phy-armada375-usb2.c b/drivers/phy/phy-armada375-usb2.c
new file mode 100644
index 000000000000..ac7d99d01cb3
--- /dev/null
+++ b/drivers/phy/phy-armada375-usb2.c
@@ -0,0 +1,158 @@
+/*
+ * USB cluster support for Armada 375 platform.
+ *
+ * Copyright (C) 2014 Marvell
+ *
+ * Gregory CLEMENT <gregory.clement@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2 or later. This program is licensed "as is"
+ * without any warranty of any kind, whether express or implied.
+ *
+ * Armada 375 comes with an USB2 host and device controller and an
+ * USB3 controller. The USB cluster control register allows to manage
+ * common features of both USB controllers.
+ */
+
+#include <dt-bindings/phy/phy.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/phy/phy.h>
+#include <linux/platform_device.h>
+
+#define USB2_PHY_CONFIG_DISABLE BIT(0)
+
+struct armada375_cluster_phy {
+	struct phy *phy;
+	void __iomem *reg;
+	bool use_usb3;
+	int phy_provided;
+};
+
+static int armada375_usb_phy_init(struct phy *phy)
+{
+	struct armada375_cluster_phy *cluster_phy;
+	u32 reg;
+
+	cluster_phy = dev_get_drvdata(phy->dev.parent);
+	if (!cluster_phy)
+		return -ENODEV;
+
+	reg = readl(cluster_phy->reg);
+	if (cluster_phy->use_usb3)
+		reg |= USB2_PHY_CONFIG_DISABLE;
+	else
+		reg &= ~USB2_PHY_CONFIG_DISABLE;
+	writel(reg, cluster_phy->reg);
+
+	return 0;
+}
+
+static struct phy_ops armada375_usb_phy_ops = {
+	.init = armada375_usb_phy_init,
+	.owner = THIS_MODULE,
+};
+
+/*
+ * Only one controller can use this PHY. We shouldn't have the case
+ * when two controllers want to use this PHY. But if this case occurs
+ * then we provide a phy to the first one and return an error for the
+ * next one. This error has also to be an error returned by
+ * devm_phy_optional_get() so different from ENODEV for USB2. In the
+ * USB3 case it still optional and we use ENODEV.
+ */
+static struct phy *armada375_usb_phy_xlate(struct device *dev,
+					struct of_phandle_args *args)
+{
+	struct armada375_cluster_phy *cluster_phy = dev_get_drvdata(dev);
+
+	if (!cluster_phy)
+		return  ERR_PTR(-ENODEV);
+
+	/*
+	 * Either the phy had never been requested and then the first
+	 * usb claiming it can get it, or it had already been
+	 * requested in this case, we only allow to use it with the
+	 * same configuration.
+	 */
+	if (WARN_ON((cluster_phy->phy_provided != PHY_NONE) &&
+			(cluster_phy->phy_provided != args->args[0]))) {
+		dev_err(dev, "This PHY has already been provided!\n");
+		dev_err(dev, "Check your device tree, only one controller can use it\n.");
+		if (args->args[0] == PHY_TYPE_USB2)
+			return ERR_PTR(-EBUSY);
+		else
+			return ERR_PTR(-ENODEV);
+	}
+
+	if (args->args[0] == PHY_TYPE_USB2)
+		cluster_phy->use_usb3 = false;
+	else if (args->args[0] == PHY_TYPE_USB3)
+		cluster_phy->use_usb3 = true;
+	else {
+		dev_err(dev, "Invalid PHY mode\n");
+		return ERR_PTR(-ENODEV);
+	}
+
+	/* Store which phy mode is used for next test */
+	cluster_phy->phy_provided = args->args[0];
+
+	return cluster_phy->phy;
+}
+
+static int armada375_usb_phy_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct phy *phy;
+	struct phy_provider *phy_provider;
+	void __iomem *usb_cluster_base;
+	struct resource *res;
+	struct armada375_cluster_phy *cluster_phy;
+
+	cluster_phy = devm_kzalloc(dev, sizeof(*cluster_phy), GFP_KERNEL);
+	if (!cluster_phy)
+		return  -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	usb_cluster_base = devm_ioremap_resource(&pdev->dev, res);
+	if (!usb_cluster_base)
+		return -ENOMEM;
+
+	phy = devm_phy_create(dev, NULL, &armada375_usb_phy_ops);
+	if (IS_ERR(phy)) {
+		dev_err(dev, "failed to create PHY\n");
+		return PTR_ERR(phy);
+	}
+
+	cluster_phy->phy = phy;
+	cluster_phy->reg = usb_cluster_base;
+
+	dev_set_drvdata(dev, cluster_phy);
+
+	phy_provider = devm_of_phy_provider_register(&pdev->dev,
+						     armada375_usb_phy_xlate);
+	return PTR_ERR_OR_ZERO(phy_provider);
+}
+
+static const struct of_device_id of_usb_cluster_table[] = {
+	{ .compatible = "marvell,armada-375-usb-cluster", },
+	{ /* end of list */ },
+};
+MODULE_DEVICE_TABLE(of, of_usb_cluster_table);
+
+static struct platform_driver armada375_usb_phy_driver = {
+	.probe	= armada375_usb_phy_probe,
+	.driver = {
+		.of_match_table	= of_usb_cluster_table,
+		.name  = "armada-375-usb-cluster",
+		.owner = THIS_MODULE,
+	}
+};
+module_platform_driver(armada375_usb_phy_driver);
+
+MODULE_DESCRIPTION("Armada 375 USB cluster driver");
+MODULE_AUTHOR("Gregory CLEMENT <gregory.clement@free-electrons.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/phy/phy-bcm-kona-usb2.c b/drivers/phy/phy-bcm-kona-usb2.c
index c1e0ca335c0e..ef2dc1aab2b9 100644
--- a/drivers/phy/phy-bcm-kona-usb2.c
+++ b/drivers/phy/phy-bcm-kona-usb2.c
@@ -117,7 +117,7 @@ static int bcm_kona_usb2_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, phy);
 
-	gphy = devm_phy_create(dev, NULL, &ops, NULL);
+	gphy = devm_phy_create(dev, NULL, &ops);
 	if (IS_ERR(gphy))
 		return PTR_ERR(gphy);
 
diff --git a/drivers/phy/phy-berlin-sata.c b/drivers/phy/phy-berlin-sata.c
index 69ced52d72aa..099eee8851e5 100644
--- a/drivers/phy/phy-berlin-sata.c
+++ b/drivers/phy/phy-berlin-sata.c
@@ -30,7 +30,8 @@
 #define MBUS_WRITE_REQUEST_SIZE_128	(BIT(2) << 16)
 #define MBUS_READ_REQUEST_SIZE_128	(BIT(2) << 19)
 
-#define PHY_BASE		0x200
+#define BG2_PHY_BASE		0x080
+#define BG2Q_PHY_BASE		0x200
 
 /* register 0x01 */
 #define REF_FREF_SEL_25		BIT(0)
@@ -61,15 +62,16 @@ struct phy_berlin_priv {
 	struct clk		*clk;
 	struct phy_berlin_desc	**phys;
 	unsigned		nphys;
+	u32			phy_base;
 };
 
-static inline void phy_berlin_sata_reg_setbits(void __iomem *ctrl_reg, u32 reg,
-					       u32 mask, u32 val)
+static inline void phy_berlin_sata_reg_setbits(void __iomem *ctrl_reg,
+			       u32 phy_base, u32 reg, u32 mask, u32 val)
 {
 	u32 regval;
 
 	/* select register */
-	writel(PHY_BASE + reg, ctrl_reg + PORT_VSR_ADDR);
+	writel(phy_base + reg, ctrl_reg + PORT_VSR_ADDR);
 
 	/* set bits */
 	regval = readl(ctrl_reg + PORT_VSR_DATA);
@@ -103,17 +105,20 @@ static int phy_berlin_sata_power_on(struct phy *phy)
 	writel(regval, priv->base + HOST_VSA_DATA);
 
 	/* set PHY mode and ref freq to 25 MHz */
-	phy_berlin_sata_reg_setbits(ctrl_reg, 0x1, 0xff,
-				    REF_FREF_SEL_25 | PHY_MODE_SATA);
+	phy_berlin_sata_reg_setbits(ctrl_reg, priv->phy_base, 0x01,
+				    0x00ff, REF_FREF_SEL_25 | PHY_MODE_SATA);
 
 	/* set PHY up to 6 Gbps */
-	phy_berlin_sata_reg_setbits(ctrl_reg, 0x25, 0xc00, PHY_GEN_MAX_6_0);
+	phy_berlin_sata_reg_setbits(ctrl_reg, priv->phy_base, 0x25,
+				    0x0c00, PHY_GEN_MAX_6_0);
 
 	/* set 40 bits width */
-	phy_berlin_sata_reg_setbits(ctrl_reg, 0x23,  0xc00, DATA_BIT_WIDTH_40);
+	phy_berlin_sata_reg_setbits(ctrl_reg, priv->phy_base, 0x23,
+				    0x0c00, DATA_BIT_WIDTH_40);
 
 	/* use max pll rate */
-	phy_berlin_sata_reg_setbits(ctrl_reg, 0x2, 0x0, USE_MAX_PLL_RATE);
+	phy_berlin_sata_reg_setbits(ctrl_reg, priv->phy_base, 0x02,
+				    0x0000, USE_MAX_PLL_RATE);
 
 	/* set Gen3 controller speed */
 	regval = readl(ctrl_reg + PORT_SCR_CTL);
@@ -218,6 +223,11 @@ static int phy_berlin_sata_probe(struct platform_device *pdev)
 	if (!priv->phys)
 		return -ENOMEM;
 
+	if (of_device_is_compatible(dev->of_node, "marvell,berlin2-sata-phy"))
+		priv->phy_base = BG2_PHY_BASE;
+	else
+		priv->phy_base = BG2Q_PHY_BASE;
+
 	dev_set_drvdata(dev, priv);
 	spin_lock_init(&priv->lock);
 
@@ -239,7 +249,7 @@ static int phy_berlin_sata_probe(struct platform_device *pdev)
 		if (!phy_desc)
 			return -ENOMEM;
 
-		phy = devm_phy_create(dev, NULL, &phy_berlin_sata_ops, NULL);
+		phy = devm_phy_create(dev, NULL, &phy_berlin_sata_ops);
 		if (IS_ERR(phy)) {
 			dev_err(dev, "failed to create PHY %d\n", phy_id);
 			return PTR_ERR(phy);
@@ -258,13 +268,11 @@ static int phy_berlin_sata_probe(struct platform_device *pdev)
 
 	phy_provider =
 		devm_of_phy_provider_register(dev, phy_berlin_sata_phy_xlate);
-	if (IS_ERR(phy_provider))
-		return PTR_ERR(phy_provider);
-
-	return 0;
+	return PTR_ERR_OR_ZERO(phy_provider);
 }
 
 static const struct of_device_id phy_berlin_sata_of_match[] = {
+	{ .compatible = "marvell,berlin2-sata-phy" },
 	{ .compatible = "marvell,berlin2q-sata-phy" },
 	{ },
 };
diff --git a/drivers/phy/phy-berlin-usb.c b/drivers/phy/phy-berlin-usb.c
new file mode 100644
index 000000000000..c8a8d53a6ece
--- /dev/null
+++ b/drivers/phy/phy-berlin-usb.c
@@ -0,0 +1,223 @@
+/*
+ * Copyright (C) 2014 Marvell Technology Group Ltd.
+ *
+ * Antoine Tenart <antoine.tenart@free-electrons.com>
+ * Jisheng Zhang <jszhang@marvell.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/gpio.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
+#include <linux/phy/phy.h>
+#include <linux/platform_device.h>
+#include <linux/reset.h>
+
+#define USB_PHY_PLL		0x04
+#define USB_PHY_PLL_CONTROL	0x08
+#define USB_PHY_TX_CTRL0	0x10
+#define USB_PHY_TX_CTRL1	0x14
+#define USB_PHY_TX_CTRL2	0x18
+#define USB_PHY_RX_CTRL		0x20
+#define USB_PHY_ANALOG		0x34
+
+/* USB_PHY_PLL */
+#define CLK_REF_DIV(x)		((x) << 4)
+#define FEEDBACK_CLK_DIV(x)	((x) << 8)
+
+/* USB_PHY_PLL_CONTROL */
+#define CLK_STABLE		BIT(0)
+#define PLL_CTRL_PIN		BIT(1)
+#define PLL_CTRL_REG		BIT(2)
+#define PLL_ON			BIT(3)
+#define PHASE_OFF_TOL_125	(0x0 << 5)
+#define PHASE_OFF_TOL_250	BIT(5)
+#define KVC0_CALIB		(0x0 << 9)
+#define KVC0_REG_CTRL		BIT(9)
+#define KVC0_HIGH		(0x0 << 10)
+#define KVC0_LOW		(0x3 << 10)
+#define CLK_BLK_EN		BIT(13)
+
+/* USB_PHY_TX_CTRL0 */
+#define EXT_HS_RCAL_EN		BIT(3)
+#define EXT_FS_RCAL_EN		BIT(4)
+#define IMPCAL_VTH_DIV(x)	((x) << 5)
+#define EXT_RS_RCAL_DIV(x)	((x) << 8)
+#define EXT_FS_RCAL_DIV(x)	((x) << 12)
+
+/* USB_PHY_TX_CTRL1 */
+#define TX_VDD15_14		(0x0 << 4)
+#define TX_VDD15_15		BIT(4)
+#define TX_VDD15_16		(0x2 << 4)
+#define TX_VDD15_17		(0x3 << 4)
+#define TX_VDD12_VDD		(0x0 << 6)
+#define TX_VDD12_11		BIT(6)
+#define TX_VDD12_12		(0x2 << 6)
+#define TX_VDD12_13		(0x3 << 6)
+#define LOW_VDD_EN		BIT(8)
+#define TX_OUT_AMP(x)		((x) << 9)
+
+/* USB_PHY_TX_CTRL2 */
+#define TX_CHAN_CTRL_REG(x)	((x) << 0)
+#define DRV_SLEWRATE(x)		((x) << 4)
+#define IMP_CAL_FS_HS_DLY_0	(0x0 << 6)
+#define IMP_CAL_FS_HS_DLY_1	BIT(6)
+#define IMP_CAL_FS_HS_DLY_2	(0x2 << 6)
+#define IMP_CAL_FS_HS_DLY_3	(0x3 << 6)
+#define FS_DRV_EN_MASK(x)	((x) << 8)
+#define HS_DRV_EN_MASK(x)	((x) << 12)
+
+/* USB_PHY_RX_CTRL */
+#define PHASE_FREEZE_DLY_2_CL	(0x0 << 0)
+#define PHASE_FREEZE_DLY_4_CL	BIT(0)
+#define ACK_LENGTH_8_CL		(0x0 << 2)
+#define ACK_LENGTH_12_CL	BIT(2)
+#define ACK_LENGTH_16_CL	(0x2 << 2)
+#define ACK_LENGTH_20_CL	(0x3 << 2)
+#define SQ_LENGTH_3		(0x0 << 4)
+#define SQ_LENGTH_6		BIT(4)
+#define SQ_LENGTH_9		(0x2 << 4)
+#define SQ_LENGTH_12		(0x3 << 4)
+#define DISCON_THRESHOLD_260	(0x0 << 6)
+#define DISCON_THRESHOLD_270	BIT(6)
+#define DISCON_THRESHOLD_280	(0x2 << 6)
+#define DISCON_THRESHOLD_290	(0x3 << 6)
+#define SQ_THRESHOLD(x)		((x) << 8)
+#define LPF_COEF(x)		((x) << 12)
+#define INTPL_CUR_10		(0x0 << 14)
+#define INTPL_CUR_20		BIT(14)
+#define INTPL_CUR_30		(0x2 << 14)
+#define INTPL_CUR_40		(0x3 << 14)
+
+/* USB_PHY_ANALOG */
+#define ANA_PWR_UP		BIT(1)
+#define ANA_PWR_DOWN		BIT(2)
+#define V2I_VCO_RATIO(x)	((x) << 7)
+#define R_ROTATE_90		(0x0 << 10)
+#define R_ROTATE_0		BIT(10)
+#define MODE_TEST_EN		BIT(11)
+#define ANA_TEST_DC_CTRL(x)	((x) << 12)
+
+#define to_phy_berlin_usb_priv(p)	\
+	container_of((p), struct phy_berlin_usb_priv, phy)
+
+static const u32 phy_berlin_pll_dividers[] = {
+	/* Berlin 2 */
+	CLK_REF_DIV(0xc) | FEEDBACK_CLK_DIV(0x54),
+	/* Berlin 2CD */
+	CLK_REF_DIV(0x6) | FEEDBACK_CLK_DIV(0x55),
+};
+
+struct phy_berlin_usb_priv {
+	void __iomem		*base;
+	struct phy		*phy;
+	struct reset_control	*rst_ctrl;
+	u32			pll_divider;
+};
+
+static int phy_berlin_usb_power_on(struct phy *phy)
+{
+	struct phy_berlin_usb_priv *priv = dev_get_drvdata(phy->dev.parent);
+
+	reset_control_reset(priv->rst_ctrl);
+
+	writel(priv->pll_divider,
+	       priv->base + USB_PHY_PLL);
+	writel(CLK_STABLE | PLL_CTRL_REG | PHASE_OFF_TOL_250 | KVC0_REG_CTRL |
+	       CLK_BLK_EN, priv->base + USB_PHY_PLL_CONTROL);
+	writel(V2I_VCO_RATIO(0x5) | R_ROTATE_0 | ANA_TEST_DC_CTRL(0x5),
+	       priv->base + USB_PHY_ANALOG);
+	writel(PHASE_FREEZE_DLY_4_CL | ACK_LENGTH_16_CL | SQ_LENGTH_12 |
+	       DISCON_THRESHOLD_260 | SQ_THRESHOLD(0xa) | LPF_COEF(0x2) |
+	       INTPL_CUR_30, priv->base + USB_PHY_RX_CTRL);
+
+	writel(TX_VDD12_13 | TX_OUT_AMP(0x3), priv->base + USB_PHY_TX_CTRL1);
+	writel(EXT_HS_RCAL_EN | IMPCAL_VTH_DIV(0x3) | EXT_RS_RCAL_DIV(0x4),
+	       priv->base + USB_PHY_TX_CTRL0);
+
+	writel(EXT_HS_RCAL_EN | IMPCAL_VTH_DIV(0x3) | EXT_RS_RCAL_DIV(0x4) |
+	       EXT_FS_RCAL_DIV(0x2), priv->base + USB_PHY_TX_CTRL0);
+
+	writel(EXT_HS_RCAL_EN | IMPCAL_VTH_DIV(0x3) | EXT_RS_RCAL_DIV(0x4),
+	       priv->base + USB_PHY_TX_CTRL0);
+	writel(TX_CHAN_CTRL_REG(0xf) | DRV_SLEWRATE(0x3) | IMP_CAL_FS_HS_DLY_3 |
+	       FS_DRV_EN_MASK(0xd), priv->base + USB_PHY_TX_CTRL2);
+
+	return 0;
+}
+
+static struct phy_ops phy_berlin_usb_ops = {
+	.power_on	= phy_berlin_usb_power_on,
+	.owner		= THIS_MODULE,
+};
+
+static const struct of_device_id phy_berlin_sata_of_match[] = {
+	{
+		.compatible = "marvell,berlin2-usb-phy",
+		.data = &phy_berlin_pll_dividers[0],
+	},
+	{
+		.compatible = "marvell,berlin2cd-usb-phy",
+		.data = &phy_berlin_pll_dividers[1],
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, phy_berlin_sata_of_match);
+
+static int phy_berlin_usb_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match =
+		of_match_device(phy_berlin_sata_of_match, &pdev->dev);
+	struct phy_berlin_usb_priv *priv;
+	struct resource *res;
+	struct phy_provider *phy_provider;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	priv->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(priv->base))
+		return PTR_ERR(priv->base);
+
+	priv->rst_ctrl = devm_reset_control_get(&pdev->dev, NULL);
+	if (IS_ERR(priv->rst_ctrl))
+		return PTR_ERR(priv->rst_ctrl);
+
+	priv->pll_divider = *((u32 *)match->data);
+
+	priv->phy = devm_phy_create(&pdev->dev, NULL, &phy_berlin_usb_ops);
+	if (IS_ERR(priv->phy)) {
+		dev_err(&pdev->dev, "failed to create PHY\n");
+		return PTR_ERR(priv->phy);
+	}
+
+	platform_set_drvdata(pdev, priv);
+
+	phy_provider =
+		devm_of_phy_provider_register(&pdev->dev, of_phy_simple_xlate);
+	if (IS_ERR(phy_provider))
+		return PTR_ERR(phy_provider);
+
+	return 0;
+}
+
+static struct platform_driver phy_berlin_usb_driver = {
+	.probe	= phy_berlin_usb_probe,
+	.driver	= {
+		.name		= "phy-berlin-usb",
+		.owner		= THIS_MODULE,
+		.of_match_table	= phy_berlin_sata_of_match,
+	 },
+};
+module_platform_driver(phy_berlin_usb_driver);
+
+MODULE_AUTHOR("Antoine Tenart <antoine.tenart@free-electrons.com>");
+MODULE_DESCRIPTION("Marvell Berlin PHY driver for USB");
+MODULE_LICENSE("GPL");
diff --git a/drivers/phy/phy-core.c b/drivers/phy/phy-core.c
index ff5eec5af817..a12d35338313 100644
--- a/drivers/phy/phy-core.c
+++ b/drivers/phy/phy-core.c
@@ -26,6 +26,7 @@
 static struct class *phy_class;
 static DEFINE_MUTEX(phy_provider_mutex);
 static LIST_HEAD(phy_provider_list);
+static LIST_HEAD(phys);
 static DEFINE_IDA(phy_ida);
 
 static void devm_phy_release(struct device *dev, void *res)
@@ -54,34 +55,79 @@ static int devm_phy_match(struct device *dev, void *res, void *match_data)
 	return res == match_data;
 }
 
-static struct phy *phy_lookup(struct device *device, const char *port)
+/**
+ * phy_create_lookup() - allocate and register PHY/device association
+ * @phy: the phy of the association
+ * @con_id: connection ID string on device
+ * @dev_id: the device of the association
+ *
+ * Creates and registers phy_lookup entry.
+ */
+int phy_create_lookup(struct phy *phy, const char *con_id, const char *dev_id)
 {
-	unsigned int count;
-	struct phy *phy;
-	struct device *dev;
-	struct phy_consumer *consumers;
-	struct class_dev_iter iter;
+	struct phy_lookup *pl;
 
-	class_dev_iter_init(&iter, phy_class, NULL, NULL);
-	while ((dev = class_dev_iter_next(&iter))) {
-		phy = to_phy(dev);
+	if (!phy || !dev_id || !con_id)
+		return -EINVAL;
 
-		if (!phy->init_data)
-			continue;
-		count = phy->init_data->num_consumers;
-		consumers = phy->init_data->consumers;
-		while (count--) {
-			if (!strcmp(consumers->dev_name, dev_name(device)) &&
-					!strcmp(consumers->port, port)) {
-				class_dev_iter_exit(&iter);
-				return phy;
-			}
-			consumers++;
+	pl = kzalloc(sizeof(*pl), GFP_KERNEL);
+	if (!pl)
+		return -ENOMEM;
+
+	pl->dev_id = dev_id;
+	pl->con_id = con_id;
+	pl->phy = phy;
+
+	mutex_lock(&phy_provider_mutex);
+	list_add_tail(&pl->node, &phys);
+	mutex_unlock(&phy_provider_mutex);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(phy_create_lookup);
+
+/**
+ * phy_remove_lookup() - find and remove PHY/device association
+ * @phy: the phy of the association
+ * @con_id: connection ID string on device
+ * @dev_id: the device of the association
+ *
+ * Finds and unregisters phy_lookup entry that was created with
+ * phy_create_lookup().
+ */
+void phy_remove_lookup(struct phy *phy, const char *con_id, const char *dev_id)
+{
+	struct phy_lookup *pl;
+
+	if (!phy || !dev_id || !con_id)
+		return;
+
+	mutex_lock(&phy_provider_mutex);
+	list_for_each_entry(pl, &phys, node)
+		if (pl->phy == phy && !strcmp(pl->dev_id, dev_id) &&
+		    !strcmp(pl->con_id, con_id)) {
+			list_del(&pl->node);
+			kfree(pl);
+			break;
 		}
-	}
+	mutex_unlock(&phy_provider_mutex);
+}
+EXPORT_SYMBOL_GPL(phy_remove_lookup);
 
-	class_dev_iter_exit(&iter);
-	return ERR_PTR(-ENODEV);
+static struct phy *phy_find(struct device *dev, const char *con_id)
+{
+	const char *dev_id = dev_name(dev);
+	struct phy_lookup *p, *pl = NULL;
+
+	mutex_lock(&phy_provider_mutex);
+	list_for_each_entry(p, &phys, node)
+		if (!strcmp(p->dev_id, dev_id) && !strcmp(p->con_id, con_id)) {
+			pl = p;
+			break;
+		}
+	mutex_unlock(&phy_provider_mutex);
+
+	return pl ? pl->phy : ERR_PTR(-ENODEV);
 }
 
 static struct phy_provider *of_phy_provider_lookup(struct device_node *node)
@@ -414,21 +460,13 @@ struct phy *of_phy_simple_xlate(struct device *dev, struct of_phandle_args
 {
 	struct phy *phy;
 	struct class_dev_iter iter;
-	struct device_node *node = dev->of_node;
-	struct device_node *child;
 
 	class_dev_iter_init(&iter, phy_class, NULL, NULL);
 	while ((dev = class_dev_iter_next(&iter))) {
 		phy = to_phy(dev);
-		if (node != phy->dev.of_node) {
-			for_each_child_of_node(node, child) {
-				if (child == phy->dev.of_node)
-					goto phy_found;
-			}
+		if (args->np != phy->dev.of_node)
 			continue;
-		}
 
-phy_found:
 		class_dev_iter_exit(&iter);
 		return phy;
 	}
@@ -463,7 +501,7 @@ struct phy *phy_get(struct device *dev, const char *string)
 			string);
 		phy = _of_phy_get(dev->of_node, index);
 	} else {
-		phy = phy_lookup(dev, string);
+		phy = phy_find(dev, string);
 	}
 	if (IS_ERR(phy))
 		return phy;
@@ -588,13 +626,11 @@ EXPORT_SYMBOL_GPL(devm_of_phy_get);
  * @dev: device that is creating the new phy
  * @node: device node of the phy
  * @ops: function pointers for performing phy operations
- * @init_data: contains the list of PHY consumers or NULL
  *
  * Called to create a phy using phy framework.
  */
 struct phy *phy_create(struct device *dev, struct device_node *node,
-		       const struct phy_ops *ops,
-		       struct phy_init_data *init_data)
+		       const struct phy_ops *ops)
 {
 	int ret;
 	int id;
@@ -632,7 +668,6 @@ struct phy *phy_create(struct device *dev, struct device_node *node,
 	phy->dev.of_node = node ?: dev->of_node;
 	phy->id = id;
 	phy->ops = ops;
-	phy->init_data = init_data;
 
 	ret = dev_set_name(&phy->dev, "phy-%s.%d", dev_name(dev), id);
 	if (ret)
@@ -667,7 +702,6 @@ EXPORT_SYMBOL_GPL(phy_create);
  * @dev: device that is creating the new phy
  * @node: device node of the phy
  * @ops: function pointers for performing phy operations
- * @init_data: contains the list of PHY consumers or NULL
  *
  * Creates a new PHY device adding it to the PHY class.
  * While at that, it also associates the device with the phy using devres.
@@ -675,8 +709,7 @@ EXPORT_SYMBOL_GPL(phy_create);
  * then, devres data is freed.
  */
 struct phy *devm_phy_create(struct device *dev, struct device_node *node,
-			    const struct phy_ops *ops,
-			    struct phy_init_data *init_data)
+			    const struct phy_ops *ops)
 {
 	struct phy **ptr, *phy;
 
@@ -684,7 +717,7 @@ struct phy *devm_phy_create(struct device *dev, struct device_node *node,
 	if (!ptr)
 		return ERR_PTR(-ENOMEM);
 
-	phy = phy_create(dev, node, ops, init_data);
+	phy = phy_create(dev, node, ops);
 	if (!IS_ERR(phy)) {
 		*ptr = phy;
 		devres_add(dev, ptr);
diff --git a/drivers/phy/phy-exynos-dp-video.c b/drivers/phy/phy-exynos-dp-video.c
index 84f49e5a3f24..f86cbe68ddaf 100644
--- a/drivers/phy/phy-exynos-dp-video.c
+++ b/drivers/phy/phy-exynos-dp-video.c
@@ -112,7 +112,7 @@ static int exynos_dp_video_phy_probe(struct platform_device *pdev)
 	match = of_match_node(exynos_dp_video_phy_of_match, dev->of_node);
 	state->drvdata = match->data;
 
-	phy = devm_phy_create(dev, NULL, &exynos_dp_video_phy_ops, NULL);
+	phy = devm_phy_create(dev, NULL, &exynos_dp_video_phy_ops);
 	if (IS_ERR(phy)) {
 		dev_err(dev, "failed to create Display Port PHY\n");
 		return PTR_ERR(phy);
diff --git a/drivers/phy/phy-exynos-mipi-video.c b/drivers/phy/phy-exynos-mipi-video.c
index 6a9bef138617..943e0f88a120 100644
--- a/drivers/phy/phy-exynos-mipi-video.c
+++ b/drivers/phy/phy-exynos-mipi-video.c
@@ -137,7 +137,7 @@ static int exynos_mipi_video_phy_probe(struct platform_device *pdev)
 
 	for (i = 0; i < EXYNOS_MIPI_PHYS_NUM; i++) {
 		struct phy *phy = devm_phy_create(dev, NULL,
-					&exynos_mipi_video_phy_ops, NULL);
+						  &exynos_mipi_video_phy_ops);
 		if (IS_ERR(phy)) {
 			dev_err(dev, "failed to create PHY %d\n", i);
 			return PTR_ERR(phy);
diff --git a/drivers/phy/phy-exynos5-usbdrd.c b/drivers/phy/phy-exynos5-usbdrd.c
index f756aca871db..04374018425f 100644
--- a/drivers/phy/phy-exynos5-usbdrd.c
+++ b/drivers/phy/phy-exynos5-usbdrd.c
@@ -141,6 +141,7 @@ struct exynos5_usbdrd_phy_drvdata {
 	const struct exynos5_usbdrd_phy_config *phy_cfg;
 	u32 pmu_offset_usbdrd0_phy;
 	u32 pmu_offset_usbdrd1_phy;
+	bool has_common_clk_gate;
 };
 
 /**
@@ -148,6 +149,9 @@ struct exynos5_usbdrd_phy_drvdata {
  * @dev: pointer to device instance of this platform device
  * @reg_phy: usb phy controller register memory base
  * @clk: phy clock for register access
+ * @pipeclk: clock for pipe3 phy
+ * @utmiclk: clock for utmi+ phy
+ * @itpclk: clock for ITP generation
  * @drv_data: pointer to SoC level driver data structure
  * @phys[]: array for 'EXYNOS5_DRDPHYS_NUM' number of PHY
  *	    instances each with its 'phy' and 'phy_cfg'.
@@ -155,12 +159,16 @@ struct exynos5_usbdrd_phy_drvdata {
  *	       reference clocks' for SS and HS operations
  * @ref_clk: reference clock to PHY block from which PHY's
  *	     operational clocks are derived
- * @ref_rate: rate of above reference clock
+ * vbus: VBUS regulator for phy
+ * vbus_boost: Boost regulator for VBUS present on few Exynos boards
  */
 struct exynos5_usbdrd_phy {
 	struct device *dev;
 	void __iomem *reg_phy;
 	struct clk *clk;
+	struct clk *pipeclk;
+	struct clk *utmiclk;
+	struct clk *itpclk;
 	const struct exynos5_usbdrd_phy_drvdata *drv_data;
 	struct phy_usb_instance {
 		struct phy *phy;
@@ -172,6 +180,7 @@ struct exynos5_usbdrd_phy {
 	u32 extrefclk;
 	struct clk *ref_clk;
 	struct regulator *vbus;
+	struct regulator *vbus_boost;
 };
 
 static inline
@@ -447,13 +456,27 @@ static int exynos5_usbdrd_phy_power_on(struct phy *phy)
 	dev_dbg(phy_drd->dev, "Request to power_on usbdrd_phy phy\n");
 
 	clk_prepare_enable(phy_drd->ref_clk);
+	if (!phy_drd->drv_data->has_common_clk_gate) {
+		clk_prepare_enable(phy_drd->pipeclk);
+		clk_prepare_enable(phy_drd->utmiclk);
+		clk_prepare_enable(phy_drd->itpclk);
+	}
 
 	/* Enable VBUS supply */
+	if (phy_drd->vbus_boost) {
+		ret = regulator_enable(phy_drd->vbus_boost);
+		if (ret) {
+			dev_err(phy_drd->dev,
+				"Failed to enable VBUS boost supply\n");
+			goto fail_vbus;
+		}
+	}
+
 	if (phy_drd->vbus) {
 		ret = regulator_enable(phy_drd->vbus);
 		if (ret) {
 			dev_err(phy_drd->dev, "Failed to enable VBUS supply\n");
-			goto fail_vbus;
+			goto fail_vbus_boost;
 		}
 	}
 
@@ -462,8 +485,17 @@ static int exynos5_usbdrd_phy_power_on(struct phy *phy)
 
 	return 0;
 
+fail_vbus_boost:
+	if (phy_drd->vbus_boost)
+		regulator_disable(phy_drd->vbus_boost);
+
 fail_vbus:
 	clk_disable_unprepare(phy_drd->ref_clk);
+	if (!phy_drd->drv_data->has_common_clk_gate) {
+		clk_disable_unprepare(phy_drd->itpclk);
+		clk_disable_unprepare(phy_drd->utmiclk);
+		clk_disable_unprepare(phy_drd->pipeclk);
+	}
 
 	return ret;
 }
@@ -481,8 +513,15 @@ static int exynos5_usbdrd_phy_power_off(struct phy *phy)
 	/* Disable VBUS supply */
 	if (phy_drd->vbus)
 		regulator_disable(phy_drd->vbus);
+	if (phy_drd->vbus_boost)
+		regulator_disable(phy_drd->vbus_boost);
 
 	clk_disable_unprepare(phy_drd->ref_clk);
+	if (!phy_drd->drv_data->has_common_clk_gate) {
+		clk_disable_unprepare(phy_drd->itpclk);
+		clk_disable_unprepare(phy_drd->pipeclk);
+		clk_disable_unprepare(phy_drd->utmiclk);
+	}
 
 	return 0;
 }
@@ -506,6 +545,57 @@ static struct phy_ops exynos5_usbdrd_phy_ops = {
 	.owner		= THIS_MODULE,
 };
 
+static int exynos5_usbdrd_phy_clk_handle(struct exynos5_usbdrd_phy *phy_drd)
+{
+	unsigned long ref_rate;
+	int ret;
+
+	phy_drd->clk = devm_clk_get(phy_drd->dev, "phy");
+	if (IS_ERR(phy_drd->clk)) {
+		dev_err(phy_drd->dev, "Failed to get phy clock\n");
+		return PTR_ERR(phy_drd->clk);
+	}
+
+	phy_drd->ref_clk = devm_clk_get(phy_drd->dev, "ref");
+	if (IS_ERR(phy_drd->ref_clk)) {
+		dev_err(phy_drd->dev, "Failed to get phy reference clock\n");
+		return PTR_ERR(phy_drd->ref_clk);
+	}
+	ref_rate = clk_get_rate(phy_drd->ref_clk);
+
+	ret = exynos5_rate_to_clk(ref_rate, &phy_drd->extrefclk);
+	if (ret) {
+		dev_err(phy_drd->dev, "Clock rate (%ld) not supported\n",
+			ref_rate);
+		return ret;
+	}
+
+	if (!phy_drd->drv_data->has_common_clk_gate) {
+		phy_drd->pipeclk = devm_clk_get(phy_drd->dev, "phy_pipe");
+		if (IS_ERR(phy_drd->pipeclk)) {
+			dev_info(phy_drd->dev,
+				 "PIPE3 phy operational clock not specified\n");
+			phy_drd->pipeclk = NULL;
+		}
+
+		phy_drd->utmiclk = devm_clk_get(phy_drd->dev, "phy_utmi");
+		if (IS_ERR(phy_drd->utmiclk)) {
+			dev_info(phy_drd->dev,
+				 "UTMI phy operational clock not specified\n");
+			phy_drd->utmiclk = NULL;
+		}
+
+		phy_drd->itpclk = devm_clk_get(phy_drd->dev, "itp");
+		if (IS_ERR(phy_drd->itpclk)) {
+			dev_info(phy_drd->dev,
+				 "ITP clock from main OSC not specified\n");
+			phy_drd->itpclk = NULL;
+		}
+	}
+
+	return 0;
+}
+
 static const struct exynos5_usbdrd_phy_config phy_cfg_exynos5[] = {
 	{
 		.id		= EXYNOS5_DRDPHY_UTMI,
@@ -525,11 +615,19 @@ static const struct exynos5_usbdrd_phy_drvdata exynos5420_usbdrd_phy = {
 	.phy_cfg		= phy_cfg_exynos5,
 	.pmu_offset_usbdrd0_phy	= EXYNOS5_USBDRD_PHY_CONTROL,
 	.pmu_offset_usbdrd1_phy	= EXYNOS5420_USBDRD1_PHY_CONTROL,
+	.has_common_clk_gate	= true,
 };
 
 static const struct exynos5_usbdrd_phy_drvdata exynos5250_usbdrd_phy = {
 	.phy_cfg		= phy_cfg_exynos5,
 	.pmu_offset_usbdrd0_phy	= EXYNOS5_USBDRD_PHY_CONTROL,
+	.has_common_clk_gate	= true,
+};
+
+static const struct exynos5_usbdrd_phy_drvdata exynos7_usbdrd_phy = {
+	.phy_cfg		= phy_cfg_exynos5,
+	.pmu_offset_usbdrd0_phy	= EXYNOS5_USBDRD_PHY_CONTROL,
+	.has_common_clk_gate	= false,
 };
 
 static const struct of_device_id exynos5_usbdrd_phy_of_match[] = {
@@ -539,6 +637,9 @@ static const struct of_device_id exynos5_usbdrd_phy_of_match[] = {
 	}, {
 		.compatible = "samsung,exynos5420-usbdrd-phy",
 		.data = &exynos5420_usbdrd_phy
+	}, {
+		.compatible = "samsung,exynos7-usbdrd-phy",
+		.data = &exynos7_usbdrd_phy
 	},
 	{ },
 };
@@ -555,7 +656,6 @@ static int exynos5_usbdrd_phy_probe(struct platform_device *pdev)
 	const struct exynos5_usbdrd_phy_drvdata *drv_data;
 	struct regmap *reg_pmu;
 	u32 pmu_offset;
-	unsigned long ref_rate;
 	int i, ret;
 	int channel;
 
@@ -576,23 +676,9 @@ static int exynos5_usbdrd_phy_probe(struct platform_device *pdev)
 	drv_data = match->data;
 	phy_drd->drv_data = drv_data;
 
-	phy_drd->clk = devm_clk_get(dev, "phy");
-	if (IS_ERR(phy_drd->clk)) {
-		dev_err(dev, "Failed to get clock of phy controller\n");
-		return PTR_ERR(phy_drd->clk);
-	}
-
-	phy_drd->ref_clk = devm_clk_get(dev, "ref");
-	if (IS_ERR(phy_drd->ref_clk)) {
-		dev_err(dev, "Failed to get reference clock of usbdrd phy\n");
-		return PTR_ERR(phy_drd->ref_clk);
-	}
-	ref_rate = clk_get_rate(phy_drd->ref_clk);
-
-	ret = exynos5_rate_to_clk(ref_rate, &phy_drd->extrefclk);
+	ret = exynos5_usbdrd_phy_clk_handle(phy_drd);
 	if (ret) {
-		dev_err(phy_drd->dev, "Clock rate (%ld) not supported\n",
-			ref_rate);
+		dev_err(dev, "Failed to initialize clocks\n");
 		return ret;
 	}
 
@@ -622,7 +708,7 @@ static int exynos5_usbdrd_phy_probe(struct platform_device *pdev)
 		break;
 	}
 
-	/* Get Vbus regulator */
+	/* Get Vbus regulators */
 	phy_drd->vbus = devm_regulator_get(dev, "vbus");
 	if (IS_ERR(phy_drd->vbus)) {
 		ret = PTR_ERR(phy_drd->vbus);
@@ -633,12 +719,21 @@ static int exynos5_usbdrd_phy_probe(struct platform_device *pdev)
 		phy_drd->vbus = NULL;
 	}
 
+	phy_drd->vbus_boost = devm_regulator_get(dev, "vbus-boost");
+	if (IS_ERR(phy_drd->vbus_boost)) {
+		ret = PTR_ERR(phy_drd->vbus_boost);
+		if (ret == -EPROBE_DEFER)
+			return ret;
+
+		dev_warn(dev, "Failed to get VBUS boost supply regulator\n");
+		phy_drd->vbus_boost = NULL;
+	}
+
 	dev_vdbg(dev, "Creating usbdrd_phy phy\n");
 
 	for (i = 0; i < EXYNOS5_DRDPHYS_NUM; i++) {
 		struct phy *phy = devm_phy_create(dev, NULL,
-						  &exynos5_usbdrd_phy_ops,
-						  NULL);
+						  &exynos5_usbdrd_phy_ops);
 		if (IS_ERR(phy)) {
 			dev_err(dev, "Failed to create usbdrd_phy phy\n");
 			return PTR_ERR(phy);
diff --git a/drivers/phy/phy-exynos5250-sata.c b/drivers/phy/phy-exynos5250-sata.c
index 54cf4ae60d29..bc858cc800a1 100644
--- a/drivers/phy/phy-exynos5250-sata.c
+++ b/drivers/phy/phy-exynos5250-sata.c
@@ -210,7 +210,7 @@ static int exynos_sata_phy_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	sata_phy->phy = devm_phy_create(dev, NULL, &exynos_sata_phy_ops, NULL);
+	sata_phy->phy = devm_phy_create(dev, NULL, &exynos_sata_phy_ops);
 	if (IS_ERR(sata_phy->phy)) {
 		clk_disable_unprepare(sata_phy->phyclk);
 		dev_err(dev, "failed to create PHY\n");
diff --git a/drivers/phy/phy-hix5hd2-sata.c b/drivers/phy/phy-hix5hd2-sata.c
index d5d978085c6d..34915b4202f1 100644
--- a/drivers/phy/phy-hix5hd2-sata.c
+++ b/drivers/phy/phy-hix5hd2-sata.c
@@ -156,7 +156,7 @@ static int hix5hd2_sata_phy_probe(struct platform_device *pdev)
 	if (IS_ERR(priv->peri_ctrl))
 		priv->peri_ctrl = NULL;
 
-	phy = devm_phy_create(dev, NULL, &hix5hd2_sata_phy_ops, NULL);
+	phy = devm_phy_create(dev, NULL, &hix5hd2_sata_phy_ops);
 	if (IS_ERR(phy)) {
 		dev_err(dev, "failed to create PHY\n");
 		return PTR_ERR(phy);
@@ -164,10 +164,7 @@ static int hix5hd2_sata_phy_probe(struct platform_device *pdev)
 
 	phy_set_drvdata(phy, priv);
 	phy_provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
-	if (IS_ERR(phy_provider))
-		return PTR_ERR(phy_provider);
-
-	return 0;
+	return PTR_ERR_OR_ZERO(phy_provider);
 }
 
 static const struct of_device_id hix5hd2_sata_phy_of_match[] = {
diff --git a/drivers/phy/phy-miphy28lp.c b/drivers/phy/phy-miphy28lp.c
new file mode 100644
index 000000000000..e34da13885e8
--- /dev/null
+++ b/drivers/phy/phy-miphy28lp.c
@@ -0,0 +1,1283 @@
+/*
+ * Copyright (C) 2014 STMicroelectronics
+ *
+ * STMicroelectronics PHY driver MiPHY28lp (for SoC STiH407).
+ *
+ * Author: Alexandre Torgue <alexandre.torgue@st.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/platform_device.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/of_address.h>
+#include <linux/clk.h>
+#include <linux/phy/phy.h>
+#include <linux/delay.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
+#include <linux/reset.h>
+
+#include <dt-bindings/phy/phy.h>
+
+/* MiPHY registers */
+#define MIPHY_CONF_RESET		0x00
+#define RST_APPLI_SW		BIT(0)
+#define RST_CONF_SW		BIT(1)
+#define RST_MACRO_SW		BIT(2)
+
+#define MIPHY_RESET			0x01
+#define RST_PLL_SW		BIT(0)
+#define RST_COMP_SW		BIT(2)
+
+#define MIPHY_STATUS_1			0x02
+#define PHY_RDY			BIT(0)
+#define HFC_RDY			BIT(1)
+#define HFC_PLL			BIT(2)
+
+#define MIPHY_CONTROL			0x04
+#define TERM_EN_SW		BIT(2)
+#define DIS_LINK_RST		BIT(3)
+#define AUTO_RST_RX		BIT(4)
+#define PX_RX_POL		BIT(5)
+
+#define MIPHY_BOUNDARY_SEL		0x0a
+#define TX_SEL			BIT(6)
+#define SSC_SEL			BIT(4)
+#define GENSEL_SEL		BIT(0)
+
+#define MIPHY_BOUNDARY_1		0x0b
+#define MIPHY_BOUNDARY_2		0x0c
+#define SSC_EN_SW		BIT(2)
+
+#define MIPHY_PLL_CLKREF_FREQ		0x0d
+#define MIPHY_SPEED			0x0e
+#define TX_SPDSEL_80DEC		0
+#define TX_SPDSEL_40DEC		1
+#define TX_SPDSEL_20DEC		2
+#define RX_SPDSEL_80DEC		0
+#define RX_SPDSEL_40DEC		(1 << 2)
+#define RX_SPDSEL_20DEC		(2 << 2)
+
+#define MIPHY_CONF			0x0f
+#define MIPHY_CTRL_TEST_SEL		0x20
+#define MIPHY_CTRL_TEST_1		0x21
+#define MIPHY_CTRL_TEST_2		0x22
+#define MIPHY_CTRL_TEST_3		0x23
+#define MIPHY_CTRL_TEST_4		0x24
+#define MIPHY_FEEDBACK_TEST		0x25
+#define MIPHY_DEBUG_BUS			0x26
+#define MIPHY_DEBUG_STATUS_MSB		0x27
+#define MIPHY_DEBUG_STATUS_LSB		0x28
+#define MIPHY_PWR_RAIL_1		0x29
+#define MIPHY_PWR_RAIL_2		0x2a
+#define MIPHY_SYNCHAR_CONTROL		0x30
+
+#define MIPHY_COMP_FSM_1		0x3a
+#define COMP_START		BIT(6)
+
+#define MIPHY_COMP_FSM_6		0x3f
+#define COMP_DONE		BIT(7)
+
+#define MIPHY_COMP_POSTP		0x42
+#define MIPHY_TX_CTRL_1			0x49
+#define TX_REG_STEP_0V		0
+#define TX_REG_STEP_P_25MV	1
+#define TX_REG_STEP_P_50MV	2
+#define TX_REG_STEP_N_25MV	7
+#define TX_REG_STEP_N_50MV	6
+#define TX_REG_STEP_N_75MV	5
+
+#define MIPHY_TX_CTRL_2			0x4a
+#define TX_SLEW_SW_40_PS	0
+#define TX_SLEW_SW_80_PS	1
+#define TX_SLEW_SW_120_PS	2
+
+#define MIPHY_TX_CTRL_3			0x4b
+#define MIPHY_TX_CAL_MAN		0x4e
+#define TX_SLEW_CAL_MAN_EN	BIT(0)
+
+#define MIPHY_TST_BIAS_BOOST_2		0x62
+#define MIPHY_BIAS_BOOST_1		0x63
+#define MIPHY_BIAS_BOOST_2		0x64
+#define MIPHY_RX_DESBUFF_FDB_2		0x67
+#define MIPHY_RX_DESBUFF_FDB_3		0x68
+#define MIPHY_SIGDET_COMPENS1		0x69
+#define MIPHY_SIGDET_COMPENS2		0x6a
+#define MIPHY_JITTER_PERIOD		0x6b
+#define MIPHY_JITTER_AMPLITUDE_1	0x6c
+#define MIPHY_JITTER_AMPLITUDE_2	0x6d
+#define MIPHY_JITTER_AMPLITUDE_3	0x6e
+#define MIPHY_RX_K_GAIN			0x78
+#define MIPHY_RX_BUFFER_CTRL		0x7a
+#define VGA_GAIN		BIT(0)
+#define EQ_DC_GAIN		BIT(2)
+#define EQ_BOOST_GAIN		BIT(3)
+
+#define MIPHY_RX_VGA_GAIN		0x7b
+#define MIPHY_RX_EQU_GAIN_1		0x7f
+#define MIPHY_RX_EQU_GAIN_2		0x80
+#define MIPHY_RX_EQU_GAIN_3		0x81
+#define MIPHY_RX_CAL_CTRL_1		0x97
+#define MIPHY_RX_CAL_CTRL_2		0x98
+
+#define MIPHY_RX_CAL_OFFSET_CTRL	0x99
+#define CAL_OFFSET_VGA_64	(0x03 << 0)
+#define CAL_OFFSET_THRESHOLD_64	(0x03 << 2)
+#define VGA_OFFSET_POLARITY	BIT(4)
+#define OFFSET_COMPENSATION_EN	BIT(6)
+
+#define MIPHY_RX_CAL_VGA_STEP		0x9a
+#define MIPHY_RX_CAL_EYE_MIN		0x9d
+#define MIPHY_RX_CAL_OPT_LENGTH		0x9f
+#define MIPHY_RX_LOCK_CTRL_1		0xc1
+#define MIPHY_RX_LOCK_SETTINGS_OPT	0xc2
+#define MIPHY_RX_LOCK_STEP		0xc4
+
+#define MIPHY_RX_SIGDET_SLEEP_OA	0xc9
+#define MIPHY_RX_SIGDET_SLEEP_SEL	0xca
+#define MIPHY_RX_SIGDET_WAIT_SEL	0xcb
+#define MIPHY_RX_SIGDET_DATA_SEL	0xcc
+#define EN_ULTRA_LOW_POWER	BIT(0)
+#define EN_FIRST_HALF		BIT(1)
+#define EN_SECOND_HALF		BIT(2)
+#define EN_DIGIT_SIGNAL_CHECK	BIT(3)
+
+#define MIPHY_RX_POWER_CTRL_1		0xcd
+#define MIPHY_RX_POWER_CTRL_2		0xce
+#define MIPHY_PLL_CALSET_CTRL		0xd3
+#define MIPHY_PLL_CALSET_1		0xd4
+#define MIPHY_PLL_CALSET_2		0xd5
+#define MIPHY_PLL_CALSET_3		0xd6
+#define MIPHY_PLL_CALSET_4		0xd7
+#define MIPHY_PLL_SBR_1			0xe3
+#define SET_NEW_CHANGE		BIT(1)
+
+#define MIPHY_PLL_SBR_2			0xe4
+#define MIPHY_PLL_SBR_3			0xe5
+#define MIPHY_PLL_SBR_4			0xe6
+#define MIPHY_PLL_COMMON_MISC_2		0xe9
+#define START_ACT_FILT		BIT(6)
+
+#define MIPHY_PLL_SPAREIN		0xeb
+
+/*
+ * On STiH407 the glue logic can be different among MiPHY devices; for example:
+ * MiPHY0: OSC_FORCE_EXT means:
+ *  0: 30MHz crystal clk - 1: 100MHz ext clk routed through MiPHY1
+ * MiPHY1: OSC_FORCE_EXT means:
+ *  1: 30MHz crystal clk - 0: 100MHz ext clk routed through MiPHY1
+ * Some devices have not the possibility to check if the osc is ready.
+ */
+#define MIPHY_OSC_FORCE_EXT	BIT(3)
+#define MIPHY_OSC_RDY		BIT(5)
+
+#define MIPHY_CTRL_MASK		0x0f
+#define MIPHY_CTRL_DEFAULT	0
+#define MIPHY_CTRL_SYNC_D_EN	BIT(2)
+
+/* SATA / PCIe defines */
+#define SATA_CTRL_MASK		0x07
+#define PCIE_CTRL_MASK		0xff
+#define SATA_CTRL_SELECT_SATA	1
+#define SATA_CTRL_SELECT_PCIE	0
+#define SYSCFG_PCIE_PCIE_VAL	0x80
+#define SATA_SPDMODE		1
+
+#define MIPHY_SATA_BANK_NB	3
+#define MIPHY_PCIE_BANK_NB	2
+
+struct miphy28lp_phy {
+	struct phy *phy;
+	struct miphy28lp_dev *phydev;
+	void __iomem *base;
+	void __iomem *pipebase;
+
+	bool osc_force_ext;
+	bool osc_rdy;
+	bool px_rx_pol_inv;
+	bool ssc;
+	bool tx_impedance;
+
+	struct reset_control *miphy_rst;
+
+	u32 sata_gen;
+
+	/* Sysconfig registers offsets needed to configure the device */
+	u32 syscfg_miphy_ctrl;
+	u32 syscfg_miphy_status;
+	u32 syscfg_pci;
+	u32 syscfg_sata;
+	u8 type;
+};
+
+struct miphy28lp_dev {
+	struct device *dev;
+	struct regmap *regmap;
+	struct mutex miphy_mutex;
+	struct miphy28lp_phy **phys;
+};
+
+struct miphy_initval {
+	u16 reg;
+	u16 val;
+};
+
+enum miphy_sata_gen { SATA_GEN1, SATA_GEN2, SATA_GEN3 };
+
+static char *PHY_TYPE_name[] = { "sata-up", "pcie-up", "", "usb3-up" };
+
+struct pll_ratio {
+	int clk_ref;
+	int calset_1;
+	int calset_2;
+	int calset_3;
+	int calset_4;
+	int cal_ctrl;
+};
+
+static struct pll_ratio sata_pll_ratio = {
+	.clk_ref = 0x1e,
+	.calset_1 = 0xc8,
+	.calset_2 = 0x00,
+	.calset_3 = 0x00,
+	.calset_4 = 0x00,
+	.cal_ctrl = 0x00,
+};
+
+static struct pll_ratio pcie_pll_ratio = {
+	.clk_ref = 0x1e,
+	.calset_1 = 0xa6,
+	.calset_2 = 0xaa,
+	.calset_3 = 0xaa,
+	.calset_4 = 0x00,
+	.cal_ctrl = 0x00,
+};
+
+static struct pll_ratio usb3_pll_ratio = {
+	.clk_ref = 0x1e,
+	.calset_1 = 0xa6,
+	.calset_2 = 0xaa,
+	.calset_3 = 0xaa,
+	.calset_4 = 0x04,
+	.cal_ctrl = 0x00,
+};
+
+struct miphy28lp_pll_gen {
+	int bank;
+	int speed;
+	int bias_boost_1;
+	int bias_boost_2;
+	int tx_ctrl_1;
+	int tx_ctrl_2;
+	int tx_ctrl_3;
+	int rx_k_gain;
+	int rx_vga_gain;
+	int rx_equ_gain_1;
+	int rx_equ_gain_2;
+	int rx_equ_gain_3;
+	int rx_buff_ctrl;
+};
+
+static struct miphy28lp_pll_gen sata_pll_gen[] = {
+	{
+		.bank		= 0x00,
+		.speed		= TX_SPDSEL_80DEC | RX_SPDSEL_80DEC,
+		.bias_boost_1	= 0x00,
+		.bias_boost_2	= 0xae,
+		.tx_ctrl_2	= 0x53,
+		.tx_ctrl_3	= 0x00,
+		.rx_buff_ctrl	= EQ_BOOST_GAIN | EQ_DC_GAIN | VGA_GAIN,
+		.rx_vga_gain	= 0x00,
+		.rx_equ_gain_1	= 0x7d,
+		.rx_equ_gain_2	= 0x56,
+		.rx_equ_gain_3	= 0x00,
+	},
+	{
+		.bank		= 0x01,
+		.speed		= TX_SPDSEL_40DEC | RX_SPDSEL_40DEC,
+		.bias_boost_1	= 0x00,
+		.bias_boost_2	= 0xae,
+		.tx_ctrl_2	= 0x72,
+		.tx_ctrl_3	= 0x20,
+		.rx_buff_ctrl	= EQ_BOOST_GAIN | EQ_DC_GAIN | VGA_GAIN,
+		.rx_vga_gain	= 0x00,
+		.rx_equ_gain_1	= 0x7d,
+		.rx_equ_gain_2	= 0x56,
+		.rx_equ_gain_3	= 0x00,
+	},
+	{
+		.bank		= 0x02,
+		.speed		= TX_SPDSEL_20DEC | RX_SPDSEL_20DEC,
+		.bias_boost_1	= 0x00,
+		.bias_boost_2	= 0xae,
+		.tx_ctrl_2	= 0xc0,
+		.tx_ctrl_3	= 0x20,
+		.rx_buff_ctrl	= EQ_BOOST_GAIN | EQ_DC_GAIN | VGA_GAIN,
+		.rx_vga_gain	= 0x00,
+		.rx_equ_gain_1	= 0x7d,
+		.rx_equ_gain_2	= 0x56,
+		.rx_equ_gain_3	= 0x00,
+	},
+};
+
+static struct miphy28lp_pll_gen pcie_pll_gen[] = {
+	{
+		.bank		= 0x00,
+		.speed		= TX_SPDSEL_40DEC | RX_SPDSEL_40DEC,
+		.bias_boost_1	= 0x00,
+		.bias_boost_2	= 0xa5,
+		.tx_ctrl_1	= TX_REG_STEP_N_25MV,
+		.tx_ctrl_2	= 0x71,
+		.tx_ctrl_3	= 0x60,
+		.rx_k_gain	= 0x98,
+		.rx_buff_ctrl	= EQ_BOOST_GAIN | EQ_DC_GAIN | VGA_GAIN,
+		.rx_vga_gain	= 0x00,
+		.rx_equ_gain_1	= 0x79,
+		.rx_equ_gain_2	= 0x56,
+	},
+	{
+		.bank		= 0x01,
+		.speed		= TX_SPDSEL_20DEC | RX_SPDSEL_20DEC,
+		.bias_boost_1	= 0x00,
+		.bias_boost_2	= 0xa5,
+		.tx_ctrl_1	= TX_REG_STEP_N_25MV,
+		.tx_ctrl_2	= 0x70,
+		.tx_ctrl_3	= 0x60,
+		.rx_k_gain	= 0xcc,
+		.rx_buff_ctrl	= EQ_BOOST_GAIN | EQ_DC_GAIN | VGA_GAIN,
+		.rx_vga_gain	= 0x00,
+		.rx_equ_gain_1	= 0x78,
+		.rx_equ_gain_2	= 0x07,
+	},
+};
+
+static inline void miphy28lp_set_reset(struct miphy28lp_phy *miphy_phy)
+{
+	void *base = miphy_phy->base;
+	u8 val;
+
+	/* Putting Macro in reset */
+	writeb_relaxed(RST_APPLI_SW, base + MIPHY_CONF_RESET);
+
+	val = RST_APPLI_SW | RST_CONF_SW;
+	writeb_relaxed(val, base + MIPHY_CONF_RESET);
+
+	writeb_relaxed(RST_APPLI_SW, base + MIPHY_CONF_RESET);
+
+	/* Bringing the MIPHY-CPU registers out of reset */
+	if (miphy_phy->type == PHY_TYPE_PCIE) {
+		val = AUTO_RST_RX | TERM_EN_SW;
+		writeb_relaxed(val, base + MIPHY_CONTROL);
+	} else {
+		val = AUTO_RST_RX | TERM_EN_SW | DIS_LINK_RST;
+		writeb_relaxed(val, base + MIPHY_CONTROL);
+	}
+}
+
+static inline void miphy28lp_pll_calibration(struct miphy28lp_phy *miphy_phy,
+		struct pll_ratio *pll_ratio)
+{
+	void *base = miphy_phy->base;
+	u8 val;
+
+	/* Applying PLL Settings */
+	writeb_relaxed(0x1d, base + MIPHY_PLL_SPAREIN);
+	writeb_relaxed(pll_ratio->clk_ref, base + MIPHY_PLL_CLKREF_FREQ);
+
+	/* PLL Ratio */
+	writeb_relaxed(pll_ratio->calset_1, base + MIPHY_PLL_CALSET_1);
+	writeb_relaxed(pll_ratio->calset_2, base + MIPHY_PLL_CALSET_2);
+	writeb_relaxed(pll_ratio->calset_3, base + MIPHY_PLL_CALSET_3);
+	writeb_relaxed(pll_ratio->calset_4, base + MIPHY_PLL_CALSET_4);
+	writeb_relaxed(pll_ratio->cal_ctrl, base + MIPHY_PLL_CALSET_CTRL);
+
+	writeb_relaxed(TX_SEL, base + MIPHY_BOUNDARY_SEL);
+
+	val = (0x68 << 1) | TX_SLEW_CAL_MAN_EN;
+	writeb_relaxed(val, base + MIPHY_TX_CAL_MAN);
+
+	val = VGA_OFFSET_POLARITY | CAL_OFFSET_THRESHOLD_64 | CAL_OFFSET_VGA_64;
+
+	if (miphy_phy->type != PHY_TYPE_SATA)
+		val |= OFFSET_COMPENSATION_EN;
+
+	writeb_relaxed(val, base + MIPHY_RX_CAL_OFFSET_CTRL);
+
+	if (miphy_phy->type == PHY_TYPE_USB3) {
+		writeb_relaxed(0x00, base + MIPHY_CONF);
+		writeb_relaxed(0x70, base + MIPHY_RX_LOCK_STEP);
+		writeb_relaxed(EN_FIRST_HALF, base + MIPHY_RX_SIGDET_SLEEP_OA);
+		writeb_relaxed(EN_FIRST_HALF, base + MIPHY_RX_SIGDET_SLEEP_SEL);
+		writeb_relaxed(EN_FIRST_HALF, base + MIPHY_RX_SIGDET_WAIT_SEL);
+
+		val = EN_DIGIT_SIGNAL_CHECK | EN_FIRST_HALF;
+		writeb_relaxed(val, base + MIPHY_RX_SIGDET_DATA_SEL);
+	}
+
+}
+
+static inline void miphy28lp_sata_config_gen(struct miphy28lp_phy *miphy_phy)
+{
+	void __iomem *base = miphy_phy->base;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(sata_pll_gen); i++) {
+		struct miphy28lp_pll_gen *gen = &sata_pll_gen[i];
+
+		/* Banked settings */
+		writeb_relaxed(gen->bank, base + MIPHY_CONF);
+		writeb_relaxed(gen->speed, base + MIPHY_SPEED);
+		writeb_relaxed(gen->bias_boost_1, base + MIPHY_BIAS_BOOST_1);
+		writeb_relaxed(gen->bias_boost_2, base + MIPHY_BIAS_BOOST_2);
+
+		/* TX buffer Settings */
+		writeb_relaxed(gen->tx_ctrl_2, base + MIPHY_TX_CTRL_2);
+		writeb_relaxed(gen->tx_ctrl_3, base + MIPHY_TX_CTRL_3);
+
+		/* RX Buffer Settings */
+		writeb_relaxed(gen->rx_buff_ctrl, base + MIPHY_RX_BUFFER_CTRL);
+		writeb_relaxed(gen->rx_vga_gain, base + MIPHY_RX_VGA_GAIN);
+		writeb_relaxed(gen->rx_equ_gain_1, base + MIPHY_RX_EQU_GAIN_1);
+		writeb_relaxed(gen->rx_equ_gain_2, base + MIPHY_RX_EQU_GAIN_2);
+		writeb_relaxed(gen->rx_equ_gain_3, base + MIPHY_RX_EQU_GAIN_3);
+	}
+}
+
+static inline void miphy28lp_pcie_config_gen(struct miphy28lp_phy *miphy_phy)
+{
+	void __iomem *base = miphy_phy->base;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(pcie_pll_gen); i++) {
+		struct miphy28lp_pll_gen *gen = &pcie_pll_gen[i];
+
+		/* Banked settings */
+		writeb_relaxed(gen->bank, base + MIPHY_CONF);
+		writeb_relaxed(gen->speed, base + MIPHY_SPEED);
+		writeb_relaxed(gen->bias_boost_1, base + MIPHY_BIAS_BOOST_1);
+		writeb_relaxed(gen->bias_boost_2, base + MIPHY_BIAS_BOOST_2);
+
+		/* TX buffer Settings */
+		writeb_relaxed(gen->tx_ctrl_1, base + MIPHY_TX_CTRL_1);
+		writeb_relaxed(gen->tx_ctrl_2, base + MIPHY_TX_CTRL_2);
+		writeb_relaxed(gen->tx_ctrl_3, base + MIPHY_TX_CTRL_3);
+
+		writeb_relaxed(gen->rx_k_gain, base + MIPHY_RX_K_GAIN);
+
+		/* RX Buffer Settings */
+		writeb_relaxed(gen->rx_buff_ctrl, base + MIPHY_RX_BUFFER_CTRL);
+		writeb_relaxed(gen->rx_vga_gain, base + MIPHY_RX_VGA_GAIN);
+		writeb_relaxed(gen->rx_equ_gain_1, base + MIPHY_RX_EQU_GAIN_1);
+		writeb_relaxed(gen->rx_equ_gain_2, base + MIPHY_RX_EQU_GAIN_2);
+	}
+}
+
+static inline int miphy28lp_wait_compensation(struct miphy28lp_phy *miphy_phy)
+{
+	unsigned long finish = jiffies + 5 * HZ;
+	u8 val;
+
+	/* Waiting for Compensation to complete */
+	do {
+		val = readb_relaxed(miphy_phy->base + MIPHY_COMP_FSM_6);
+
+		if (time_after_eq(jiffies, finish))
+			return -EBUSY;
+		cpu_relax();
+	} while (!(val & COMP_DONE));
+
+	return 0;
+}
+
+
+static inline int miphy28lp_compensation(struct miphy28lp_phy *miphy_phy,
+		struct pll_ratio *pll_ratio)
+{
+	void __iomem *base = miphy_phy->base;
+
+	/* Poll for HFC ready after reset release */
+	/* Compensation measurement */
+	writeb_relaxed(RST_PLL_SW | RST_COMP_SW, base + MIPHY_RESET);
+
+	writeb_relaxed(0x00, base + MIPHY_PLL_COMMON_MISC_2);
+	writeb_relaxed(pll_ratio->clk_ref, base + MIPHY_PLL_CLKREF_FREQ);
+	writeb_relaxed(COMP_START, base + MIPHY_COMP_FSM_1);
+
+	if (miphy_phy->type == PHY_TYPE_PCIE)
+		writeb_relaxed(RST_PLL_SW, base + MIPHY_RESET);
+
+	writeb_relaxed(0x00, base + MIPHY_RESET);
+	writeb_relaxed(START_ACT_FILT, base + MIPHY_PLL_COMMON_MISC_2);
+	writeb_relaxed(SET_NEW_CHANGE, base + MIPHY_PLL_SBR_1);
+
+	/* TX compensation offset to re-center TX impedance */
+	writeb_relaxed(0x00, base + MIPHY_COMP_POSTP);
+
+	if (miphy_phy->type == PHY_TYPE_PCIE)
+		return miphy28lp_wait_compensation(miphy_phy);
+
+	return 0;
+}
+
+static inline void miphy28_usb3_miphy_reset(struct miphy28lp_phy *miphy_phy)
+{
+	void __iomem *base = miphy_phy->base;
+	u8 val;
+
+	/* MIPHY Reset */
+	writeb_relaxed(RST_APPLI_SW, base + MIPHY_CONF_RESET);
+	writeb_relaxed(0x00, base + MIPHY_CONF_RESET);
+	writeb_relaxed(RST_COMP_SW, base + MIPHY_RESET);
+
+	val = RST_COMP_SW | RST_PLL_SW;
+	writeb_relaxed(val, base + MIPHY_RESET);
+
+	writeb_relaxed(0x00, base + MIPHY_PLL_COMMON_MISC_2);
+	writeb_relaxed(0x1e, base + MIPHY_PLL_CLKREF_FREQ);
+	writeb_relaxed(COMP_START, base + MIPHY_COMP_FSM_1);
+	writeb_relaxed(RST_PLL_SW, base + MIPHY_RESET);
+	writeb_relaxed(0x00, base + MIPHY_RESET);
+	writeb_relaxed(START_ACT_FILT, base + MIPHY_PLL_COMMON_MISC_2);
+	writeb_relaxed(0x00, base + MIPHY_CONF);
+	writeb_relaxed(0x00, base + MIPHY_BOUNDARY_1);
+	writeb_relaxed(0x00, base + MIPHY_TST_BIAS_BOOST_2);
+	writeb_relaxed(0x00, base + MIPHY_CONF);
+	writeb_relaxed(SET_NEW_CHANGE, base + MIPHY_PLL_SBR_1);
+	writeb_relaxed(0xa5, base + MIPHY_DEBUG_BUS);
+	writeb_relaxed(0x00, base + MIPHY_CONF);
+}
+
+static void miphy_sata_tune_ssc(struct miphy28lp_phy *miphy_phy)
+{
+	void __iomem *base = miphy_phy->base;
+	u8 val;
+
+	/* Compensate Tx impedance to avoid out of range values */
+	/*
+	 * Enable the SSC on PLL for all banks
+	 * SSC Modulation @ 31 KHz and 4000 ppm modulation amp
+	 */
+	val = readb_relaxed(base + MIPHY_BOUNDARY_2);
+	val |= SSC_EN_SW;
+	writeb_relaxed(val, base + MIPHY_BOUNDARY_2);
+
+	val = readb_relaxed(base + MIPHY_BOUNDARY_SEL);
+	val |= SSC_SEL;
+	writeb_relaxed(val, base + MIPHY_BOUNDARY_SEL);
+
+	for (val = 0; val < MIPHY_SATA_BANK_NB; val++) {
+		writeb_relaxed(val, base + MIPHY_CONF);
+
+		/* Add value to each reference clock cycle  */
+		/* and define the period length of the SSC */
+		writeb_relaxed(0x3c, base + MIPHY_PLL_SBR_2);
+		writeb_relaxed(0x6c, base + MIPHY_PLL_SBR_3);
+		writeb_relaxed(0x81, base + MIPHY_PLL_SBR_4);
+
+		/* Clear any previous request */
+		writeb_relaxed(0x00, base + MIPHY_PLL_SBR_1);
+
+		/* requests the PLL to take in account new parameters */
+		writeb_relaxed(SET_NEW_CHANGE, base + MIPHY_PLL_SBR_1);
+
+		/* To be sure there is no other pending requests */
+		writeb_relaxed(0x00, base + MIPHY_PLL_SBR_1);
+	}
+}
+
+static void miphy_pcie_tune_ssc(struct miphy28lp_phy *miphy_phy)
+{
+	void __iomem *base = miphy_phy->base;
+	u8 val;
+
+	/* Compensate Tx impedance to avoid out of range values */
+	/*
+	 * Enable the SSC on PLL for all banks
+	 * SSC Modulation @ 31 KHz and 4000 ppm modulation amp
+	 */
+	val = readb_relaxed(base + MIPHY_BOUNDARY_2);
+	val |= SSC_EN_SW;
+	writeb_relaxed(val, base + MIPHY_BOUNDARY_2);
+
+	val = readb_relaxed(base + MIPHY_BOUNDARY_SEL);
+	val |= SSC_SEL;
+	writeb_relaxed(val, base + MIPHY_BOUNDARY_SEL);
+
+	for (val = 0; val < MIPHY_PCIE_BANK_NB; val++) {
+		writeb_relaxed(val, base + MIPHY_CONF);
+
+		/* Validate Step component */
+		writeb_relaxed(0x69, base + MIPHY_PLL_SBR_3);
+		writeb_relaxed(0x21, base + MIPHY_PLL_SBR_4);
+
+		/* Validate Period component */
+		writeb_relaxed(0x3c, base + MIPHY_PLL_SBR_2);
+		writeb_relaxed(0x21, base + MIPHY_PLL_SBR_4);
+
+		/* Clear any previous request */
+		writeb_relaxed(0x00, base + MIPHY_PLL_SBR_1);
+
+		/* requests the PLL to take in account new parameters */
+		writeb_relaxed(SET_NEW_CHANGE, base + MIPHY_PLL_SBR_1);
+
+		/* To be sure there is no other pending requests */
+		writeb_relaxed(0x00, base + MIPHY_PLL_SBR_1);
+	}
+}
+
+static inline void miphy_tune_tx_impedance(struct miphy28lp_phy *miphy_phy)
+{
+	/* Compensate Tx impedance to avoid out of range values */
+	writeb_relaxed(0x02, miphy_phy->base + MIPHY_COMP_POSTP);
+}
+
+static inline int miphy28lp_configure_sata(struct miphy28lp_phy *miphy_phy)
+{
+	void __iomem *base = miphy_phy->base;
+	int err;
+	u8 val;
+
+	/* Putting Macro in reset */
+	miphy28lp_set_reset(miphy_phy);
+
+	/* PLL calibration */
+	miphy28lp_pll_calibration(miphy_phy, &sata_pll_ratio);
+
+	/* Banked settings Gen1/Gen2/Gen3 */
+	miphy28lp_sata_config_gen(miphy_phy);
+
+	/* Power control */
+	/* Input bridge enable, manual input bridge control */
+	writeb_relaxed(0x21, base + MIPHY_RX_POWER_CTRL_1);
+
+	/* Macro out of reset */
+	writeb_relaxed(0x00, base + MIPHY_CONF_RESET);
+
+	/* Poll for HFC ready after reset release */
+	/* Compensation measurement */
+	err = miphy28lp_compensation(miphy_phy, &sata_pll_ratio);
+	if (err)
+		return err;
+
+	if (miphy_phy->px_rx_pol_inv) {
+		/* Invert Rx polarity */
+		val = readb_relaxed(miphy_phy->base + MIPHY_CONTROL);
+		val |= PX_RX_POL;
+		writeb_relaxed(val, miphy_phy->base + MIPHY_CONTROL);
+	}
+
+	if (miphy_phy->ssc)
+		miphy_sata_tune_ssc(miphy_phy);
+
+	if (miphy_phy->tx_impedance)
+		miphy_tune_tx_impedance(miphy_phy);
+
+	return 0;
+}
+
+static inline int miphy28lp_configure_pcie(struct miphy28lp_phy *miphy_phy)
+{
+	void __iomem *base = miphy_phy->base;
+	int err;
+
+	/* Putting Macro in reset */
+	miphy28lp_set_reset(miphy_phy);
+
+	/* PLL calibration */
+	miphy28lp_pll_calibration(miphy_phy, &pcie_pll_ratio);
+
+	/* Banked settings Gen1/Gen2 */
+	miphy28lp_pcie_config_gen(miphy_phy);
+
+	/* Power control */
+	/* Input bridge enable, manual input bridge control */
+	writeb_relaxed(0x21, base + MIPHY_RX_POWER_CTRL_1);
+
+	/* Macro out of reset */
+	writeb_relaxed(0x00, base + MIPHY_CONF_RESET);
+
+	/* Poll for HFC ready after reset release */
+	/* Compensation measurement */
+	err = miphy28lp_compensation(miphy_phy, &pcie_pll_ratio);
+	if (err)
+		return err;
+
+	if (miphy_phy->ssc)
+		miphy_pcie_tune_ssc(miphy_phy);
+
+	if (miphy_phy->tx_impedance)
+		miphy_tune_tx_impedance(miphy_phy);
+
+	return 0;
+}
+
+
+static inline void miphy28lp_configure_usb3(struct miphy28lp_phy *miphy_phy)
+{
+	void __iomem *base = miphy_phy->base;
+	u8 val;
+
+	/* Putting Macro in reset */
+	miphy28lp_set_reset(miphy_phy);
+
+	/* PLL calibration */
+	miphy28lp_pll_calibration(miphy_phy, &usb3_pll_ratio);
+
+	/* Writing The Speed Rate */
+	writeb_relaxed(0x00, base + MIPHY_CONF);
+
+	val = RX_SPDSEL_20DEC | TX_SPDSEL_20DEC;
+	writeb_relaxed(val, base + MIPHY_SPEED);
+
+	/* RX Channel compensation and calibration */
+	writeb_relaxed(0x1c, base + MIPHY_RX_LOCK_SETTINGS_OPT);
+	writeb_relaxed(0x51, base + MIPHY_RX_CAL_CTRL_1);
+	writeb_relaxed(0x70, base + MIPHY_RX_CAL_CTRL_2);
+
+	val = OFFSET_COMPENSATION_EN | VGA_OFFSET_POLARITY |
+	      CAL_OFFSET_THRESHOLD_64 | CAL_OFFSET_VGA_64;
+	writeb_relaxed(val, base + MIPHY_RX_CAL_OFFSET_CTRL);
+	writeb_relaxed(0x22, base + MIPHY_RX_CAL_VGA_STEP);
+	writeb_relaxed(0x0e, base + MIPHY_RX_CAL_OPT_LENGTH);
+
+	val = EQ_DC_GAIN | VGA_GAIN;
+	writeb_relaxed(val, base + MIPHY_RX_BUFFER_CTRL);
+	writeb_relaxed(0x78, base + MIPHY_RX_EQU_GAIN_1);
+	writeb_relaxed(0x1b, base + MIPHY_SYNCHAR_CONTROL);
+
+	/* TX compensation offset to re-center TX impedance */
+	writeb_relaxed(0x02, base + MIPHY_COMP_POSTP);
+
+	/* Enable GENSEL_SEL and SSC */
+	/* TX_SEL=0 swing preemp forced by pipe registres */
+	val = SSC_SEL | GENSEL_SEL;
+	writeb_relaxed(val, base + MIPHY_BOUNDARY_SEL);
+
+	/* MIPHY Bias boost */
+	writeb_relaxed(0x00, base + MIPHY_BIAS_BOOST_1);
+	writeb_relaxed(0xa7, base + MIPHY_BIAS_BOOST_2);
+
+	/* SSC modulation */
+	writeb_relaxed(SSC_EN_SW, base + MIPHY_BOUNDARY_2);
+
+	/* MIPHY TX control */
+	writeb_relaxed(0x00, base + MIPHY_CONF);
+
+	/* Validate Step component */
+	writeb_relaxed(0x5a, base + MIPHY_PLL_SBR_3);
+	writeb_relaxed(0xa0, base + MIPHY_PLL_SBR_4);
+
+	/* Validate Period component */
+	writeb_relaxed(0x3c, base + MIPHY_PLL_SBR_2);
+	writeb_relaxed(0xa1, base + MIPHY_PLL_SBR_4);
+
+	/* Clear any previous request */
+	writeb_relaxed(0x00, base + MIPHY_PLL_SBR_1);
+
+	/* requests the PLL to take in account new parameters */
+	writeb_relaxed(0x02, base + MIPHY_PLL_SBR_1);
+
+	/* To be sure there is no other pending requests */
+	writeb_relaxed(0x00, base + MIPHY_PLL_SBR_1);
+
+	/* Rx PI controller settings */
+	writeb_relaxed(0xca, base + MIPHY_RX_K_GAIN);
+
+	/* MIPHY RX input bridge control */
+	/* INPUT_BRIDGE_EN_SW=1, manual input bridge control[0]=1 */
+	writeb_relaxed(0x21, base + MIPHY_RX_POWER_CTRL_1);
+	writeb_relaxed(0x29, base + MIPHY_RX_POWER_CTRL_1);
+	writeb_relaxed(0x1a, base + MIPHY_RX_POWER_CTRL_2);
+
+	/* MIPHY Reset for usb3 */
+	miphy28_usb3_miphy_reset(miphy_phy);
+}
+
+static inline int miphy_is_ready(struct miphy28lp_phy *miphy_phy)
+{
+	unsigned long finish = jiffies + 5 * HZ;
+	u8 mask = HFC_PLL | HFC_RDY;
+	u8 val;
+
+	/*
+	 * For PCIe and USB3 check only that PLL and HFC are ready
+	 * For SATA check also that phy is ready!
+	 */
+	if (miphy_phy->type == PHY_TYPE_SATA)
+		mask |= PHY_RDY;
+
+	do {
+		val = readb_relaxed(miphy_phy->base + MIPHY_STATUS_1);
+		if ((val & mask) != mask)
+			cpu_relax();
+		else
+			return 0;
+	} while (!time_after_eq(jiffies, finish));
+
+	return -EBUSY;
+}
+
+static int miphy_osc_is_ready(struct miphy28lp_phy *miphy_phy)
+{
+	struct miphy28lp_dev *miphy_dev = miphy_phy->phydev;
+	unsigned long finish = jiffies + 5 * HZ;
+	u32 val;
+
+	if (!miphy_phy->osc_rdy)
+		return 0;
+
+	if (!miphy_phy->syscfg_miphy_status)
+		return -EINVAL;
+
+	do {
+		regmap_read(miphy_dev->regmap, miphy_phy->syscfg_miphy_status,
+			    &val);
+
+		if ((val & MIPHY_OSC_RDY) != MIPHY_OSC_RDY)
+			cpu_relax();
+		else
+			return 0;
+	} while (!time_after_eq(jiffies, finish));
+
+	return -EBUSY;
+}
+
+static int miphy28lp_get_resource_byname(struct device_node *child,
+					  char *rname, struct resource *res)
+{
+	int index;
+
+	index = of_property_match_string(child, "reg-names", rname);
+	if (index < 0)
+		return -ENODEV;
+
+	return of_address_to_resource(child, index, res);
+}
+
+static int miphy28lp_get_one_addr(struct device *dev,
+				  struct device_node *child, char *rname,
+				  void __iomem **base)
+{
+	struct resource res;
+	int ret;
+
+	ret = miphy28lp_get_resource_byname(child, rname, &res);
+	if (!ret) {
+		*base = devm_ioremap(dev, res.start, resource_size(&res));
+		if (!*base) {
+			dev_err(dev, "failed to ioremap %s address region\n"
+					, rname);
+			return -ENOENT;
+		}
+	}
+
+	return 0;
+}
+
+/* MiPHY reset and sysconf setup */
+static int miphy28lp_setup(struct miphy28lp_phy *miphy_phy, u32 miphy_val)
+{
+	int err;
+	struct miphy28lp_dev *miphy_dev = miphy_phy->phydev;
+
+	if (!miphy_phy->syscfg_miphy_ctrl)
+		return -EINVAL;
+
+	err = reset_control_assert(miphy_phy->miphy_rst);
+	if (err) {
+		dev_err(miphy_dev->dev, "unable to bring out of miphy reset\n");
+		return err;
+	}
+
+	if (miphy_phy->osc_force_ext)
+		miphy_val |= MIPHY_OSC_FORCE_EXT;
+
+	regmap_update_bits(miphy_dev->regmap, miphy_phy->syscfg_miphy_ctrl,
+			   MIPHY_CTRL_MASK, miphy_val);
+
+	err = reset_control_deassert(miphy_phy->miphy_rst);
+	if (err) {
+		dev_err(miphy_dev->dev, "unable to bring out of miphy reset\n");
+		return err;
+	}
+
+	return miphy_osc_is_ready(miphy_phy);
+}
+
+static int miphy28lp_init_sata(struct miphy28lp_phy *miphy_phy)
+{
+	struct miphy28lp_dev *miphy_dev = miphy_phy->phydev;
+	int err, sata_conf = SATA_CTRL_SELECT_SATA;
+
+	if ((!miphy_phy->syscfg_sata) || (!miphy_phy->syscfg_pci)
+		|| (!miphy_phy->base))
+		return -EINVAL;
+
+	dev_info(miphy_dev->dev, "sata-up mode, addr 0x%p\n", miphy_phy->base);
+
+	/* Configure the glue-logic */
+	sata_conf |= ((miphy_phy->sata_gen - SATA_GEN1) << SATA_SPDMODE);
+
+	regmap_update_bits(miphy_dev->regmap, miphy_phy->syscfg_sata,
+			   SATA_CTRL_MASK, sata_conf);
+
+	regmap_update_bits(miphy_dev->regmap, miphy_phy->syscfg_pci,
+			   PCIE_CTRL_MASK, SATA_CTRL_SELECT_PCIE);
+
+	/* MiPHY path and clocking init */
+	err = miphy28lp_setup(miphy_phy, MIPHY_CTRL_DEFAULT);
+
+	if (err) {
+		dev_err(miphy_dev->dev, "SATA phy setup failed\n");
+		return err;
+	}
+
+	/* initialize miphy */
+	miphy28lp_configure_sata(miphy_phy);
+
+	return miphy_is_ready(miphy_phy);
+}
+
+static int miphy28lp_init_pcie(struct miphy28lp_phy *miphy_phy)
+{
+	struct miphy28lp_dev *miphy_dev = miphy_phy->phydev;
+	int err;
+
+	if ((!miphy_phy->syscfg_sata) || (!miphy_phy->syscfg_pci)
+		|| (!miphy_phy->base) || (!miphy_phy->pipebase))
+		return -EINVAL;
+
+	dev_info(miphy_dev->dev, "pcie-up mode, addr 0x%p\n", miphy_phy->base);
+
+	/* Configure the glue-logic */
+	regmap_update_bits(miphy_dev->regmap, miphy_phy->syscfg_sata,
+			   SATA_CTRL_MASK, SATA_CTRL_SELECT_PCIE);
+
+	regmap_update_bits(miphy_dev->regmap, miphy_phy->syscfg_pci,
+			   PCIE_CTRL_MASK, SYSCFG_PCIE_PCIE_VAL);
+
+	/* MiPHY path and clocking init */
+	err = miphy28lp_setup(miphy_phy, MIPHY_CTRL_DEFAULT);
+
+	if (err) {
+		dev_err(miphy_dev->dev, "PCIe phy setup failed\n");
+		return err;
+	}
+
+	/* initialize miphy */
+	err = miphy28lp_configure_pcie(miphy_phy);
+	if (err)
+		return err;
+
+	/* PIPE Wrapper Configuration */
+	writeb_relaxed(0x68, miphy_phy->pipebase + 0x104); /* Rise_0 */
+	writeb_relaxed(0x61, miphy_phy->pipebase + 0x105); /* Rise_1 */
+	writeb_relaxed(0x68, miphy_phy->pipebase + 0x108); /* Fall_0 */
+	writeb_relaxed(0x61, miphy_phy->pipebase + 0x109); /* Fall-1 */
+	writeb_relaxed(0x68, miphy_phy->pipebase + 0x10c); /* Threshold_0 */
+	writeb_relaxed(0x60, miphy_phy->pipebase + 0x10d); /* Threshold_1 */
+
+	/* Wait for phy_ready */
+	return miphy_is_ready(miphy_phy);
+}
+
+static int miphy28lp_init_usb3(struct miphy28lp_phy *miphy_phy)
+{
+	struct miphy28lp_dev *miphy_dev = miphy_phy->phydev;
+	int err;
+
+	if ((!miphy_phy->base) || (!miphy_phy->pipebase))
+		return -EINVAL;
+
+	dev_info(miphy_dev->dev, "usb3-up mode, addr 0x%p\n", miphy_phy->base);
+
+	/* MiPHY path and clocking init */
+	err = miphy28lp_setup(miphy_phy, MIPHY_CTRL_SYNC_D_EN);
+	if (err) {
+		dev_err(miphy_dev->dev, "USB3 phy setup failed\n");
+		return err;
+	}
+
+	/* initialize miphy */
+	miphy28lp_configure_usb3(miphy_phy);
+
+	/* PIPE Wrapper Configuration */
+	writeb_relaxed(0x68, miphy_phy->pipebase + 0x23);
+	writeb_relaxed(0x61, miphy_phy->pipebase + 0x24);
+	writeb_relaxed(0x68, miphy_phy->pipebase + 0x26);
+	writeb_relaxed(0x61, miphy_phy->pipebase + 0x27);
+	writeb_relaxed(0x18, miphy_phy->pipebase + 0x29);
+	writeb_relaxed(0x61, miphy_phy->pipebase + 0x2a);
+
+	/* pipe Wrapper usb3 TX swing de-emph margin PREEMPH[7:4], SWING[3:0] */
+	writeb_relaxed(0X67, miphy_phy->pipebase + 0x68);
+	writeb_relaxed(0x0d, miphy_phy->pipebase + 0x69);
+	writeb_relaxed(0X67, miphy_phy->pipebase + 0x6a);
+	writeb_relaxed(0X0d, miphy_phy->pipebase + 0x6b);
+	writeb_relaxed(0X67, miphy_phy->pipebase + 0x6c);
+	writeb_relaxed(0X0d, miphy_phy->pipebase + 0x6d);
+	writeb_relaxed(0X67, miphy_phy->pipebase + 0x6e);
+	writeb_relaxed(0X0d, miphy_phy->pipebase + 0x6f);
+
+	return miphy_is_ready(miphy_phy);
+}
+
+static int miphy28lp_init(struct phy *phy)
+{
+	struct miphy28lp_phy *miphy_phy = phy_get_drvdata(phy);
+	struct miphy28lp_dev *miphy_dev = miphy_phy->phydev;
+	int ret;
+
+	mutex_lock(&miphy_dev->miphy_mutex);
+
+	switch (miphy_phy->type) {
+
+	case PHY_TYPE_SATA:
+		ret = miphy28lp_init_sata(miphy_phy);
+		break;
+	case PHY_TYPE_PCIE:
+		ret = miphy28lp_init_pcie(miphy_phy);
+		break;
+	case PHY_TYPE_USB3:
+		ret = miphy28lp_init_usb3(miphy_phy);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	mutex_unlock(&miphy_dev->miphy_mutex);
+
+	return ret;
+}
+
+static int miphy28lp_get_addr(struct miphy28lp_phy *miphy_phy)
+{
+	struct miphy28lp_dev *miphy_dev = miphy_phy->phydev;
+	struct device_node *phynode = miphy_phy->phy->dev.of_node;
+	int err;
+
+	if ((miphy_phy->type != PHY_TYPE_SATA) &&
+	    (miphy_phy->type != PHY_TYPE_PCIE) &&
+	    (miphy_phy->type != PHY_TYPE_USB3)) {
+		return -EINVAL;
+	}
+
+	err = miphy28lp_get_one_addr(miphy_dev->dev, phynode,
+			PHY_TYPE_name[miphy_phy->type - PHY_TYPE_SATA],
+			&miphy_phy->base);
+	if (err)
+		return err;
+
+	if ((miphy_phy->type == PHY_TYPE_PCIE) ||
+	    (miphy_phy->type == PHY_TYPE_USB3)) {
+		err = miphy28lp_get_one_addr(miphy_dev->dev, phynode, "pipew",
+					     &miphy_phy->pipebase);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+static struct phy *miphy28lp_xlate(struct device *dev,
+				   struct of_phandle_args *args)
+{
+	struct miphy28lp_dev *miphy_dev = dev_get_drvdata(dev);
+	struct miphy28lp_phy *miphy_phy = NULL;
+	struct device_node *phynode = args->np;
+	int ret, index = 0;
+
+	if (!of_device_is_available(phynode)) {
+		dev_warn(dev, "Requested PHY is disabled\n");
+		return ERR_PTR(-ENODEV);
+	}
+
+	if (args->args_count != 1) {
+		dev_err(dev, "Invalid number of cells in 'phy' property\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	for (index = 0; index < of_get_child_count(dev->of_node); index++)
+		if (phynode == miphy_dev->phys[index]->phy->dev.of_node) {
+			miphy_phy = miphy_dev->phys[index];
+			break;
+		}
+
+	if (!miphy_phy) {
+		dev_err(dev, "Failed to find appropriate phy\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	miphy_phy->type = args->args[0];
+
+	ret = miphy28lp_get_addr(miphy_phy);
+	if (ret < 0)
+		return ERR_PTR(ret);
+
+	return miphy_phy->phy;
+}
+
+static struct phy_ops miphy28lp_ops = {
+	.init = miphy28lp_init,
+};
+
+static int miphy28lp_probe_resets(struct device_node *node,
+				  struct miphy28lp_phy *miphy_phy)
+{
+	struct miphy28lp_dev *miphy_dev = miphy_phy->phydev;
+	int err;
+
+	miphy_phy->miphy_rst = of_reset_control_get(node, "miphy-sw-rst");
+
+	if (IS_ERR(miphy_phy->miphy_rst)) {
+		dev_err(miphy_dev->dev,
+				"miphy soft reset control not defined\n");
+		return PTR_ERR(miphy_phy->miphy_rst);
+	}
+
+	err = reset_control_deassert(miphy_phy->miphy_rst);
+	if (err) {
+		dev_err(miphy_dev->dev, "unable to bring out of miphy reset\n");
+		return err;
+	}
+
+	return 0;
+}
+
+static int miphy28lp_of_probe(struct device_node *np,
+			      struct miphy28lp_phy *miphy_phy)
+{
+	struct resource res;
+
+	miphy_phy->osc_force_ext =
+		of_property_read_bool(np, "st,osc-force-ext");
+
+	miphy_phy->osc_rdy = of_property_read_bool(np, "st,osc-rdy");
+
+	miphy_phy->px_rx_pol_inv =
+		of_property_read_bool(np, "st,px_rx_pol_inv");
+
+	miphy_phy->ssc = of_property_read_bool(np, "st,ssc-on");
+
+	miphy_phy->tx_impedance =
+		of_property_read_bool(np, "st,tx-impedance-comp");
+
+	of_property_read_u32(np, "st,sata-gen", &miphy_phy->sata_gen);
+	if (!miphy_phy->sata_gen)
+		miphy_phy->sata_gen = SATA_GEN1;
+
+	if (!miphy28lp_get_resource_byname(np, "miphy-ctrl-glue", &res))
+		miphy_phy->syscfg_miphy_ctrl = res.start;
+
+	if (!miphy28lp_get_resource_byname(np, "miphy-status-glue", &res))
+		miphy_phy->syscfg_miphy_status = res.start;
+
+	if (!miphy28lp_get_resource_byname(np, "pcie-glue", &res))
+		miphy_phy->syscfg_pci = res.start;
+
+	if (!miphy28lp_get_resource_byname(np, "sata-glue", &res))
+		miphy_phy->syscfg_sata = res.start;
+
+
+	return 0;
+}
+
+static int miphy28lp_probe(struct platform_device *pdev)
+{
+	struct device_node *child, *np = pdev->dev.of_node;
+	struct miphy28lp_dev *miphy_dev;
+	struct phy_provider *provider;
+	struct phy *phy;
+	int chancount, port = 0;
+	int ret;
+
+	miphy_dev = devm_kzalloc(&pdev->dev, sizeof(*miphy_dev), GFP_KERNEL);
+	if (!miphy_dev)
+		return -ENOMEM;
+
+	chancount = of_get_child_count(np);
+	miphy_dev->phys = devm_kzalloc(&pdev->dev, sizeof(phy) * chancount,
+				       GFP_KERNEL);
+	if (!miphy_dev->phys)
+		return -ENOMEM;
+
+	miphy_dev->regmap = syscon_regmap_lookup_by_phandle(np, "st,syscfg");
+	if (IS_ERR(miphy_dev->regmap)) {
+		dev_err(miphy_dev->dev, "No syscfg phandle specified\n");
+		return PTR_ERR(miphy_dev->regmap);
+	}
+
+	miphy_dev->dev = &pdev->dev;
+
+	dev_set_drvdata(&pdev->dev, miphy_dev);
+
+	mutex_init(&miphy_dev->miphy_mutex);
+
+	for_each_child_of_node(np, child) {
+		struct miphy28lp_phy *miphy_phy;
+
+		miphy_phy = devm_kzalloc(&pdev->dev, sizeof(*miphy_phy),
+					 GFP_KERNEL);
+		if (!miphy_phy)
+			return -ENOMEM;
+
+		miphy_dev->phys[port] = miphy_phy;
+
+		phy = devm_phy_create(&pdev->dev, child, &miphy28lp_ops);
+		if (IS_ERR(phy)) {
+			dev_err(&pdev->dev, "failed to create PHY\n");
+			return PTR_ERR(phy);
+		}
+
+		miphy_dev->phys[port]->phy = phy;
+		miphy_dev->phys[port]->phydev = miphy_dev;
+
+		ret = miphy28lp_of_probe(child, miphy_phy);
+		if (ret)
+			return ret;
+
+		ret = miphy28lp_probe_resets(child, miphy_dev->phys[port]);
+		if (ret)
+			return ret;
+
+		phy_set_drvdata(phy, miphy_dev->phys[port]);
+		port++;
+
+	}
+
+	provider = devm_of_phy_provider_register(&pdev->dev, miphy28lp_xlate);
+	if (IS_ERR(provider))
+		return PTR_ERR(provider);
+
+	return 0;
+}
+
+static const struct of_device_id miphy28lp_of_match[] = {
+	{.compatible = "st,miphy28lp-phy", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, miphy28lp_of_match);
+
+static struct platform_driver miphy28lp_driver = {
+	.probe = miphy28lp_probe,
+	.driver = {
+		.name = "miphy28lp-phy",
+		.owner = THIS_MODULE,
+		.of_match_table = miphy28lp_of_match,
+	}
+};
+
+module_platform_driver(miphy28lp_driver);
+
+MODULE_AUTHOR("Alexandre Torgue <alexandre.torgue@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics miphy28lp driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/phy/phy-miphy365x.c b/drivers/phy/phy-miphy365x.c
index 801afaf2d449..6ab43a814ad2 100644
--- a/drivers/phy/phy-miphy365x.c
+++ b/drivers/phy/phy-miphy365x.c
@@ -593,7 +593,7 @@ static int miphy365x_probe(struct platform_device *pdev)
 
 		miphy_dev->phys[port] = miphy_phy;
 
-		phy = devm_phy_create(&pdev->dev, child, &miphy365x_ops, NULL);
+		phy = devm_phy_create(&pdev->dev, child, &miphy365x_ops);
 		if (IS_ERR(phy)) {
 			dev_err(&pdev->dev, "failed to create PHY\n");
 			return PTR_ERR(phy);
@@ -610,10 +610,7 @@ static int miphy365x_probe(struct platform_device *pdev)
 	}
 
 	provider = devm_of_phy_provider_register(&pdev->dev, miphy365x_xlate);
-	if (IS_ERR(provider))
-		return PTR_ERR(provider);
-
-	return 0;
+	return PTR_ERR_OR_ZERO(provider);
 }
 
 static const struct of_device_id miphy365x_of_match[] = {
diff --git a/drivers/phy/phy-mvebu-sata.c b/drivers/phy/phy-mvebu-sata.c
index d395558cb12e..03b94f92e6f1 100644
--- a/drivers/phy/phy-mvebu-sata.c
+++ b/drivers/phy/phy-mvebu-sata.c
@@ -101,7 +101,7 @@ static int phy_mvebu_sata_probe(struct platform_device *pdev)
 	if (IS_ERR(priv->clk))
 		return PTR_ERR(priv->clk);
 
-	phy = devm_phy_create(&pdev->dev, NULL, &phy_mvebu_sata_ops, NULL);
+	phy = devm_phy_create(&pdev->dev, NULL, &phy_mvebu_sata_ops);
 	if (IS_ERR(phy))
 		return PTR_ERR(phy);
 
diff --git a/drivers/phy/phy-omap-usb2.c b/drivers/phy/phy-omap-usb2.c
index f091576b6449..6f4aef3db248 100644
--- a/drivers/phy/phy-omap-usb2.c
+++ b/drivers/phy/phy-omap-usb2.c
@@ -60,7 +60,7 @@ EXPORT_SYMBOL_GPL(omap_usb2_set_comparator);
 
 static int omap_usb_set_vbus(struct usb_otg *otg, bool enabled)
 {
-	struct omap_usb *phy = phy_to_omapusb(otg->phy);
+	struct omap_usb *phy = phy_to_omapusb(otg->usb_phy);
 
 	if (!phy->comparator)
 		return -ENODEV;
@@ -70,7 +70,7 @@ static int omap_usb_set_vbus(struct usb_otg *otg, bool enabled)
 
 static int omap_usb_start_srp(struct usb_otg *otg)
 {
-	struct omap_usb *phy = phy_to_omapusb(otg->phy);
+	struct omap_usb *phy = phy_to_omapusb(otg->usb_phy);
 
 	if (!phy->comparator)
 		return -ENODEV;
@@ -80,11 +80,9 @@ static int omap_usb_start_srp(struct usb_otg *otg)
 
 static int omap_usb_set_host(struct usb_otg *otg, struct usb_bus *host)
 {
-	struct usb_phy	*phy = otg->phy;
-
 	otg->host = host;
 	if (!host)
-		phy->state = OTG_STATE_UNDEFINED;
+		otg->state = OTG_STATE_UNDEFINED;
 
 	return 0;
 }
@@ -92,11 +90,9 @@ static int omap_usb_set_host(struct usb_otg *otg, struct usb_bus *host)
 static int omap_usb_set_peripheral(struct usb_otg *otg,
 		struct usb_gadget *gadget)
 {
-	struct usb_phy	*phy = otg->phy;
-
 	otg->gadget = gadget;
 	if (!gadget)
-		phy->state = OTG_STATE_UNDEFINED;
+		otg->state = OTG_STATE_UNDEFINED;
 
 	return 0;
 }
@@ -255,12 +251,12 @@ static int omap_usb2_probe(struct platform_device *pdev)
 		otg->set_vbus		= omap_usb_set_vbus;
 	if (phy_data->flags & OMAP_USB2_HAS_START_SRP)
 		otg->start_srp		= omap_usb_start_srp;
-	otg->phy		= &phy->phy;
+	otg->usb_phy		= &phy->phy;
 
 	platform_set_drvdata(pdev, phy);
 	pm_runtime_enable(phy->dev);
 
-	generic_phy = devm_phy_create(phy->dev, NULL, &ops, NULL);
+	generic_phy = devm_phy_create(phy->dev, NULL, &ops);
 	if (IS_ERR(generic_phy)) {
 		pm_runtime_disable(phy->dev);
 		return PTR_ERR(generic_phy);
@@ -322,7 +318,7 @@ static int omap_usb2_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_RUNTIME
+#ifdef CONFIG_PM
 
 static int omap_usb2_runtime_suspend(struct device *dev)
 {
diff --git a/drivers/phy/phy-qcom-apq8064-sata.c b/drivers/phy/phy-qcom-apq8064-sata.c
index 7b3ddfb65898..4b243f7a10e4 100644
--- a/drivers/phy/phy-qcom-apq8064-sata.c
+++ b/drivers/phy/phy-qcom-apq8064-sata.c
@@ -228,8 +228,7 @@ static int qcom_apq8064_sata_phy_probe(struct platform_device *pdev)
 	if (IS_ERR(phy->mmio))
 		return PTR_ERR(phy->mmio);
 
-	generic_phy = devm_phy_create(dev, NULL, &qcom_apq8064_sata_phy_ops,
-				      NULL);
+	generic_phy = devm_phy_create(dev, NULL, &qcom_apq8064_sata_phy_ops);
 	if (IS_ERR(generic_phy)) {
 		dev_err(dev, "%s: failed to create phy\n", __func__);
 		return PTR_ERR(generic_phy);
diff --git a/drivers/phy/phy-qcom-ipq806x-sata.c b/drivers/phy/phy-qcom-ipq806x-sata.c
index 759b0bf5b6b3..6f2fe2627916 100644
--- a/drivers/phy/phy-qcom-ipq806x-sata.c
+++ b/drivers/phy/phy-qcom-ipq806x-sata.c
@@ -150,8 +150,7 @@ static int qcom_ipq806x_sata_phy_probe(struct platform_device *pdev)
 	if (IS_ERR(phy->mmio))
 		return PTR_ERR(phy->mmio);
 
-	generic_phy = devm_phy_create(dev, NULL, &qcom_ipq806x_sata_phy_ops,
-				      NULL);
+	generic_phy = devm_phy_create(dev, NULL, &qcom_ipq806x_sata_phy_ops);
 	if (IS_ERR(generic_phy)) {
 		dev_err(dev, "%s: failed to create phy\n", __func__);
 		return PTR_ERR(generic_phy);
diff --git a/drivers/phy/phy-rcar-gen2.c b/drivers/phy/phy-rcar-gen2.c
index 2793af17799f..778276aba3aa 100644
--- a/drivers/phy/phy-rcar-gen2.c
+++ b/drivers/phy/phy-rcar-gen2.c
@@ -304,7 +304,7 @@ static int rcar_gen2_phy_probe(struct platform_device *pdev)
 			phy->select_value = select_value[channel_num][n];
 
 			phy->phy = devm_phy_create(dev, NULL,
-						   &rcar_gen2_phy_ops, NULL);
+						   &rcar_gen2_phy_ops);
 			if (IS_ERR(phy->phy)) {
 				dev_err(dev, "Failed to create PHY\n");
 				return PTR_ERR(phy->phy);
diff --git a/drivers/phy/phy-samsung-usb2.c b/drivers/phy/phy-samsung-usb2.c
index 908949dfb4db..4a12f66b7fb5 100644
--- a/drivers/phy/phy-samsung-usb2.c
+++ b/drivers/phy/phy-samsung-usb2.c
@@ -202,8 +202,7 @@ static int samsung_usb2_phy_probe(struct platform_device *pdev)
 		struct samsung_usb2_phy_instance *p = &drv->instances[i];
 
 		dev_dbg(dev, "Creating phy \"%s\"\n", label);
-		p->phy = devm_phy_create(dev, NULL, &samsung_usb2_phy_ops,
-					 NULL);
+		p->phy = devm_phy_create(dev, NULL, &samsung_usb2_phy_ops);
 		if (IS_ERR(p->phy)) {
 			dev_err(drv->dev, "Failed to create usb2_phy \"%s\"\n",
 				label);
diff --git a/drivers/phy/phy-spear1310-miphy.c b/drivers/phy/phy-spear1310-miphy.c
index 5f4c586ee951..9f47fae7eecb 100644
--- a/drivers/phy/phy-spear1310-miphy.c
+++ b/drivers/phy/phy-spear1310-miphy.c
@@ -227,7 +227,7 @@ static int spear1310_miphy_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	priv->phy = devm_phy_create(dev, NULL, &spear1310_miphy_ops, NULL);
+	priv->phy = devm_phy_create(dev, NULL, &spear1310_miphy_ops);
 	if (IS_ERR(priv->phy)) {
 		dev_err(dev, "failed to create SATA PCIe PHY\n");
 		return PTR_ERR(priv->phy);
diff --git a/drivers/phy/phy-spear1340-miphy.c b/drivers/phy/phy-spear1340-miphy.c
index 1ecd0945bad3..e42bc200275f 100644
--- a/drivers/phy/phy-spear1340-miphy.c
+++ b/drivers/phy/phy-spear1340-miphy.c
@@ -259,7 +259,7 @@ static int spear1340_miphy_probe(struct platform_device *pdev)
 		return PTR_ERR(priv->misc);
 	}
 
-	priv->phy = devm_phy_create(dev, NULL, &spear1340_miphy_ops, NULL);
+	priv->phy = devm_phy_create(dev, NULL, &spear1340_miphy_ops);
 	if (IS_ERR(priv->phy)) {
 		dev_err(dev, "failed to create SATA PCIe PHY\n");
 		return PTR_ERR(priv->phy);
diff --git a/drivers/phy/phy-stih407-usb.c b/drivers/phy/phy-stih407-usb.c
index 42428d4181ea..74f0fab3cd8a 100644
--- a/drivers/phy/phy-stih407-usb.c
+++ b/drivers/phy/phy-stih407-usb.c
@@ -137,7 +137,7 @@ static int stih407_usb2_picophy_probe(struct platform_device *pdev)
 	}
 	phy_dev->param = res->start;
 
-	phy = devm_phy_create(dev, NULL, &stih407_usb2_picophy_data, NULL);
+	phy = devm_phy_create(dev, NULL, &stih407_usb2_picophy_data);
 	if (IS_ERR(phy)) {
 		dev_err(dev, "failed to create Display Port PHY\n");
 		return PTR_ERR(phy);
diff --git a/drivers/phy/phy-stih41x-usb.c b/drivers/phy/phy-stih41x-usb.c
index 9f16cb8e01f4..a603801293ff 100644
--- a/drivers/phy/phy-stih41x-usb.c
+++ b/drivers/phy/phy-stih41x-usb.c
@@ -148,7 +148,7 @@ static int stih41x_usb_phy_probe(struct platform_device *pdev)
 		return PTR_ERR(phy_dev->clk);
 	}
 
-	phy = devm_phy_create(dev, NULL, &stih41x_usb_phy_ops, NULL);
+	phy = devm_phy_create(dev, NULL, &stih41x_usb_phy_ops);
 
 	if (IS_ERR(phy)) {
 		dev_err(dev, "failed to create phy\n");
@@ -160,10 +160,7 @@ static int stih41x_usb_phy_probe(struct platform_device *pdev)
 	phy_set_drvdata(phy, phy_dev);
 
 	phy_provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
-	if (IS_ERR(phy_provider))
-		return PTR_ERR(phy_provider);
-
-	return 0;
+	return PTR_ERR_OR_ZERO(phy_provider);
 }
 
 static const struct of_device_id stih41x_usb_phy_of_match[] = {
diff --git a/drivers/phy/phy-sun4i-usb.c b/drivers/phy/phy-sun4i-usb.c
index 0baf5efc8a40..fb02a67c9181 100644
--- a/drivers/phy/phy-sun4i-usb.c
+++ b/drivers/phy/phy-sun4i-usb.c
@@ -157,6 +157,10 @@ static int sun4i_usb_phy_init(struct phy *_phy)
 		return ret;
 	}
 
+	/* Enable USB 45 Ohm resistor calibration */
+	if (phy->index == 0)
+		sun4i_usb_phy_write(phy, PHY_RES45_CAL_EN, 0x01, 1);
+
 	/* Adjust PHY's magnitude and rate */
 	sun4i_usb_phy_write(phy, PHY_TX_AMPLITUDE_TUNE, 0x14, 5);
 
@@ -213,7 +217,7 @@ static struct phy *sun4i_usb_phy_xlate(struct device *dev,
 {
 	struct sun4i_usb_phy_data *data = dev_get_drvdata(dev);
 
-	if (WARN_ON(args->args[0] == 0 || args->args[0] >= data->num_phys))
+	if (args->args[0] >= data->num_phys)
 		return ERR_PTR(-ENODEV);
 
 	return data->phys[args->args[0]].phy;
@@ -255,8 +259,7 @@ static int sun4i_usb_phy_probe(struct platform_device *pdev)
 	if (IS_ERR(data->base))
 		return PTR_ERR(data->base);
 
-	/* Skip 0, 0 is the phy for otg which is not yet supported. */
-	for (i = 1; i < data->num_phys; i++) {
+	for (i = 0; i < data->num_phys; i++) {
 		struct sun4i_usb_phy *phy = data->phys + i;
 		char name[16];
 
@@ -295,7 +298,7 @@ static int sun4i_usb_phy_probe(struct platform_device *pdev)
 				return PTR_ERR(phy->pmu);
 		}
 
-		phy->phy = devm_phy_create(dev, NULL, &sun4i_usb_phy_ops, NULL);
+		phy->phy = devm_phy_create(dev, NULL, &sun4i_usb_phy_ops);
 		if (IS_ERR(phy->phy)) {
 			dev_err(dev, "failed to create PHY %d\n", i);
 			return PTR_ERR(phy->phy);
diff --git a/drivers/phy/phy-ti-pipe3.c b/drivers/phy/phy-ti-pipe3.c
index ab1e22d9a1e8..1387b4d4afe3 100644
--- a/drivers/phy/phy-ti-pipe3.c
+++ b/drivers/phy/phy-ti-pipe3.c
@@ -399,7 +399,7 @@ static int ti_pipe3_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, phy);
 	pm_runtime_enable(phy->dev);
 
-	generic_phy = devm_phy_create(phy->dev, NULL, &ops, NULL);
+	generic_phy = devm_phy_create(phy->dev, NULL, &ops);
 	if (IS_ERR(generic_phy))
 		return PTR_ERR(generic_phy);
 
@@ -423,7 +423,7 @@ static int ti_pipe3_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_RUNTIME
+#ifdef CONFIG_PM
 
 static int ti_pipe3_runtime_suspend(struct device *dev)
 {
diff --git a/drivers/phy/phy-twl4030-usb.c b/drivers/phy/phy-twl4030-usb.c
index 7b04befd5271..8e87f54671f3 100644
--- a/drivers/phy/phy-twl4030-usb.c
+++ b/drivers/phy/phy-twl4030-usb.c
@@ -606,7 +606,7 @@ static int twl4030_set_peripheral(struct usb_otg *otg,
 
 	otg->gadget = gadget;
 	if (!gadget)
-		otg->phy->state = OTG_STATE_UNDEFINED;
+		otg->state = OTG_STATE_UNDEFINED;
 
 	return 0;
 }
@@ -618,7 +618,7 @@ static int twl4030_set_host(struct usb_otg *otg, struct usb_bus *host)
 
 	otg->host = host;
 	if (!host)
-		otg->phy->state = OTG_STATE_UNDEFINED;
+		otg->state = OTG_STATE_UNDEFINED;
 
 	return 0;
 }
@@ -644,7 +644,6 @@ static int twl4030_usb_probe(struct platform_device *pdev)
 	struct usb_otg		*otg;
 	struct device_node	*np = pdev->dev.of_node;
 	struct phy_provider	*phy_provider;
-	struct phy_init_data	*init_data = NULL;
 
 	twl = devm_kzalloc(&pdev->dev, sizeof(*twl), GFP_KERNEL);
 	if (!twl)
@@ -655,7 +654,6 @@ static int twl4030_usb_probe(struct platform_device *pdev)
 				(enum twl4030_usb_mode *)&twl->usb_mode);
 	else if (pdata) {
 		twl->usb_mode = pdata->usb_mode;
-		init_data = pdata->init_data;
 	} else {
 		dev_err(&pdev->dev, "twl4030 initialized without pdata\n");
 		return -EINVAL;
@@ -676,11 +674,11 @@ static int twl4030_usb_probe(struct platform_device *pdev)
 	twl->phy.otg		= otg;
 	twl->phy.type		= USB_PHY_TYPE_USB2;
 
-	otg->phy		= &twl->phy;
+	otg->usb_phy		= &twl->phy;
 	otg->set_host		= twl4030_set_host;
 	otg->set_peripheral	= twl4030_set_peripheral;
 
-	phy = devm_phy_create(twl->dev, NULL, &ops, init_data);
+	phy = devm_phy_create(twl->dev, NULL, &ops);
 	if (IS_ERR(phy)) {
 		dev_dbg(&pdev->dev, "Failed to create PHY\n");
 		return PTR_ERR(phy);
@@ -733,6 +731,11 @@ static int twl4030_usb_probe(struct platform_device *pdev)
 		return status;
 	}
 
+	if (pdata)
+		err = phy_create_lookup(phy, "usb", "musb-hdrc.0");
+	if (err)
+		return err;
+
 	pm_runtime_mark_last_busy(&pdev->dev);
 	pm_runtime_put_autosuspend(twl->dev);
 
diff --git a/drivers/phy/phy-xgene.c b/drivers/phy/phy-xgene.c
index f8a51b16ade8..29214a36ea28 100644
--- a/drivers/phy/phy-xgene.c
+++ b/drivers/phy/phy-xgene.c
@@ -1707,7 +1707,7 @@ static int xgene_phy_probe(struct platform_device *pdev)
 	ctx->dev = &pdev->dev;
 	platform_set_drvdata(pdev, ctx);
 
-	ctx->phy = devm_phy_create(ctx->dev, NULL, &xgene_phy_ops, NULL);
+	ctx->phy = devm_phy_create(ctx->dev, NULL, &xgene_phy_ops);
 	if (IS_ERR(ctx->phy)) {
 		dev_dbg(&pdev->dev, "Failed to create PHY\n");
 		rc = PTR_ERR(ctx->phy);
diff --git a/drivers/pinctrl/berlin/berlin-bg2.c b/drivers/pinctrl/berlin/berlin-bg2.c
index dcd4f6a4fc50..b71a6fffef1b 100644
--- a/drivers/pinctrl/berlin/berlin-bg2.c
+++ b/drivers/pinctrl/berlin/berlin-bg2.c
@@ -263,7 +263,6 @@ static struct platform_driver berlin2_pinctrl_driver = {
 	.probe	= berlin2_pinctrl_probe,
 	.driver	= {
 		.name = "berlin-bg2-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = berlin2_pinctrl_match,
 	},
 };
diff --git a/drivers/pinctrl/berlin/berlin-bg2cd.c b/drivers/pinctrl/berlin/berlin-bg2cd.c
index 89d585ef7da7..19ac5a22c947 100644
--- a/drivers/pinctrl/berlin/berlin-bg2cd.c
+++ b/drivers/pinctrl/berlin/berlin-bg2cd.c
@@ -206,7 +206,6 @@ static struct platform_driver berlin2cd_pinctrl_driver = {
 	.probe	= berlin2cd_pinctrl_probe,
 	.driver	= {
 		.name = "berlin-bg2cd-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = berlin2cd_pinctrl_match,
 	},
 };
diff --git a/drivers/pinctrl/berlin/berlin-bg2q.c b/drivers/pinctrl/berlin/berlin-bg2q.c
index 9fcf9836045c..bd9662e57ad3 100644
--- a/drivers/pinctrl/berlin/berlin-bg2q.c
+++ b/drivers/pinctrl/berlin/berlin-bg2q.c
@@ -425,7 +425,6 @@ static struct platform_driver berlin2q_pinctrl_driver = {
 	.probe	= berlin2q_pinctrl_probe,
 	.driver	= {
 		.name = "berlin-bg2q-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = berlin2q_pinctrl_match,
 	},
 };
diff --git a/drivers/pinctrl/freescale/pinctrl-imx1.c b/drivers/pinctrl/freescale/pinctrl-imx1.c
index 533a6e519648..d3bacb7d6d37 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx1.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx1.c
@@ -267,7 +267,6 @@ MODULE_DEVICE_TABLE(of, imx1_pinctrl_of_match);
 static struct platform_driver imx1_pinctrl_driver = {
 	.driver	= {
 		.name		= "imx1-pinctrl",
-		.owner		= THIS_MODULE,
 		.of_match_table	= imx1_pinctrl_of_match,
 	},
 	.remove	= imx1_pinctrl_core_remove,
diff --git a/drivers/pinctrl/freescale/pinctrl-imx21.c b/drivers/pinctrl/freescale/pinctrl-imx21.c
index 1b3b2311b033..9d9aca3dbd7e 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx21.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx21.c
@@ -330,7 +330,6 @@ MODULE_DEVICE_TABLE(of, imx21_pinctrl_of_match);
 static struct platform_driver imx21_pinctrl_driver = {
 	.driver	= {
 		.name		= "imx21-pinctrl",
-		.owner		= THIS_MODULE,
 		.of_match_table	= imx21_pinctrl_of_match,
 	},
 	.remove	= imx1_pinctrl_core_remove,
diff --git a/drivers/pinctrl/freescale/pinctrl-imx23.c b/drivers/pinctrl/freescale/pinctrl-imx23.c
index df79096becb0..955cbf4f094f 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx23.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx23.c
@@ -281,7 +281,6 @@ 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,
diff --git a/drivers/pinctrl/freescale/pinctrl-imx25.c b/drivers/pinctrl/freescale/pinctrl-imx25.c
index 550e6d77ac2b..8d1013a040c9 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx25.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx25.c
@@ -328,7 +328,6 @@ static int imx25_pinctrl_probe(struct platform_device *pdev)
 static struct platform_driver imx25_pinctrl_driver = {
 	.driver = {
 		.name = "imx25-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(imx25_pinctrl_of_match),
 	},
 	.probe = imx25_pinctrl_probe,
diff --git a/drivers/pinctrl/freescale/pinctrl-imx27.c b/drivers/pinctrl/freescale/pinctrl-imx27.c
index 945eccadea74..a461d5881f37 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx27.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx27.c
@@ -402,7 +402,6 @@ static int imx27_pinctrl_probe(struct platform_device *pdev)
 static struct platform_driver imx27_pinctrl_driver = {
 	.driver = {
 		.name = "imx27-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(imx27_pinctrl_of_match),
 	},
 	.probe = imx27_pinctrl_probe,
diff --git a/drivers/pinctrl/freescale/pinctrl-imx28.c b/drivers/pinctrl/freescale/pinctrl-imx28.c
index 3bd45da21229..5082efec4f72 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx28.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx28.c
@@ -397,7 +397,6 @@ 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,
diff --git a/drivers/pinctrl/freescale/pinctrl-imx35.c b/drivers/pinctrl/freescale/pinctrl-imx35.c
index 6bfbcd0112c1..9109c10c5dab 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx35.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx35.c
@@ -1018,7 +1018,6 @@ static int imx35_pinctrl_probe(struct platform_device *pdev)
 static struct platform_driver imx35_pinctrl_driver = {
 	.driver = {
 		.name = "imx35-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = imx35_pinctrl_of_match,
 	},
 	.probe = imx35_pinctrl_probe,
diff --git a/drivers/pinctrl/freescale/pinctrl-imx50.c b/drivers/pinctrl/freescale/pinctrl-imx50.c
index e8bd604ab147..51b31df96273 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx50.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx50.c
@@ -404,7 +404,6 @@ static int imx50_pinctrl_probe(struct platform_device *pdev)
 static struct platform_driver imx50_pinctrl_driver = {
 	.driver = {
 		.name = "imx50-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(imx50_pinctrl_of_match),
 	},
 	.probe = imx50_pinctrl_probe,
diff --git a/drivers/pinctrl/freescale/pinctrl-imx51.c b/drivers/pinctrl/freescale/pinctrl-imx51.c
index b818051db7c9..8dec494aa2c6 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx51.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx51.c
@@ -781,7 +781,6 @@ static int imx51_pinctrl_probe(struct platform_device *pdev)
 static struct platform_driver imx51_pinctrl_driver = {
 	.driver = {
 		.name = "imx51-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = imx51_pinctrl_of_match,
 	},
 	.probe = imx51_pinctrl_probe,
diff --git a/drivers/pinctrl/freescale/pinctrl-imx53.c b/drivers/pinctrl/freescale/pinctrl-imx53.c
index 1884d53cf750..7344d340013c 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx53.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx53.c
@@ -467,7 +467,6 @@ static int imx53_pinctrl_probe(struct platform_device *pdev)
 static struct platform_driver imx53_pinctrl_driver = {
 	.driver = {
 		.name = "imx53-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = imx53_pinctrl_of_match,
 	},
 	.probe = imx53_pinctrl_probe,
diff --git a/drivers/pinctrl/freescale/pinctrl-imx6dl.c b/drivers/pinctrl/freescale/pinctrl-imx6dl.c
index 656c4b08cc2e..6805c678c3b2 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx6dl.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx6dl.c
@@ -473,7 +473,6 @@ static int imx6dl_pinctrl_probe(struct platform_device *pdev)
 static struct platform_driver imx6dl_pinctrl_driver = {
 	.driver = {
 		.name = "imx6dl-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = imx6dl_pinctrl_of_match,
 	},
 	.probe = imx6dl_pinctrl_probe,
diff --git a/drivers/pinctrl/freescale/pinctrl-imx6q.c b/drivers/pinctrl/freescale/pinctrl-imx6q.c
index 59bb5b4ec0f6..4d1fcb861ac1 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx6q.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx6q.c
@@ -479,7 +479,6 @@ static int imx6q_pinctrl_probe(struct platform_device *pdev)
 static struct platform_driver imx6q_pinctrl_driver = {
 	.driver = {
 		.name = "imx6q-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = imx6q_pinctrl_of_match,
 	},
 	.probe = imx6q_pinctrl_probe,
diff --git a/drivers/pinctrl/freescale/pinctrl-imx6sl.c b/drivers/pinctrl/freescale/pinctrl-imx6sl.c
index e0924bd7b98c..83fa5f19ae89 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx6sl.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx6sl.c
@@ -380,7 +380,6 @@ static int imx6sl_pinctrl_probe(struct platform_device *pdev)
 static struct platform_driver imx6sl_pinctrl_driver = {
 	.driver = {
 		.name = "imx6sl-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = imx6sl_pinctrl_of_match,
 	},
 	.probe = imx6sl_pinctrl_probe,
diff --git a/drivers/pinctrl/freescale/pinctrl-imx6sx.c b/drivers/pinctrl/freescale/pinctrl-imx6sx.c
index 840344c8580d..0d78fe690818 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx6sx.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx6sx.c
@@ -383,7 +383,6 @@ static int imx6sx_pinctrl_probe(struct platform_device *pdev)
 static struct platform_driver imx6sx_pinctrl_driver = {
 	.driver = {
 		.name = "imx6sx-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(imx6sx_pinctrl_of_match),
 	},
 	.probe = imx6sx_pinctrl_probe,
diff --git a/drivers/pinctrl/freescale/pinctrl-vf610.c b/drivers/pinctrl/freescale/pinctrl-vf610.c
index b788e1578954..fc86276892fd 100644
--- a/drivers/pinctrl/freescale/pinctrl-vf610.c
+++ b/drivers/pinctrl/freescale/pinctrl-vf610.c
@@ -315,7 +315,6 @@ static int vf610_pinctrl_probe(struct platform_device *pdev)
 static struct platform_driver vf610_pinctrl_driver = {
 	.driver = {
 		.name = "vf610-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = vf610_pinctrl_of_match,
 	},
 	.probe = vf610_pinctrl_probe,
diff --git a/drivers/pinctrl/intel/pinctrl-baytrail.c b/drivers/pinctrl/intel/pinctrl-baytrail.c
index 7db000431da7..5afe03e28b91 100644
--- a/drivers/pinctrl/intel/pinctrl-baytrail.c
+++ b/drivers/pinctrl/intel/pinctrl-baytrail.c
@@ -606,7 +606,6 @@ static struct platform_driver byt_gpio_driver = {
 	.remove         = byt_gpio_remove,
 	.driver         = {
 		.name   = "byt_gpio",
-		.owner  = THIS_MODULE,
 		.pm	= &byt_gpio_pm_ops,
 		.acpi_match_table = ACPI_PTR(byt_gpio_acpi_match),
 	},
diff --git a/drivers/pinctrl/mvebu/pinctrl-armada-370.c b/drivers/pinctrl/mvebu/pinctrl-armada-370.c
index 670e5b01c678..c4f51d0cd2cc 100644
--- a/drivers/pinctrl/mvebu/pinctrl-armada-370.c
+++ b/drivers/pinctrl/mvebu/pinctrl-armada-370.c
@@ -425,7 +425,6 @@ static int armada_370_pinctrl_remove(struct platform_device *pdev)
 static struct platform_driver armada_370_pinctrl_driver = {
 	.driver = {
 		.name = "armada-370-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = armada_370_pinctrl_of_match,
 	},
 	.probe = armada_370_pinctrl_probe,
diff --git a/drivers/pinctrl/mvebu/pinctrl-armada-375.c b/drivers/pinctrl/mvebu/pinctrl-armada-375.c
index db078fe7ace6..cd7c8f51f7d9 100644
--- a/drivers/pinctrl/mvebu/pinctrl-armada-375.c
+++ b/drivers/pinctrl/mvebu/pinctrl-armada-375.c
@@ -445,7 +445,6 @@ static int armada_375_pinctrl_remove(struct platform_device *pdev)
 static struct platform_driver armada_375_pinctrl_driver = {
 	.driver = {
 		.name = "armada-375-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(armada_375_pinctrl_of_match),
 	},
 	.probe = armada_375_pinctrl_probe,
diff --git a/drivers/pinctrl/mvebu/pinctrl-armada-38x.c b/drivers/pinctrl/mvebu/pinctrl-armada-38x.c
index 1049f82fb62f..224c6cff6aa2 100644
--- a/drivers/pinctrl/mvebu/pinctrl-armada-38x.c
+++ b/drivers/pinctrl/mvebu/pinctrl-armada-38x.c
@@ -448,7 +448,6 @@ static int armada_38x_pinctrl_remove(struct platform_device *pdev)
 static struct platform_driver armada_38x_pinctrl_driver = {
 	.driver = {
 		.name = "armada-38x-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(armada_38x_pinctrl_of_match),
 	},
 	.probe = armada_38x_pinctrl_probe,
diff --git a/drivers/pinctrl/mvebu/pinctrl-armada-xp.c b/drivers/pinctrl/mvebu/pinctrl-armada-xp.c
index de311129f7a0..fc3376147c18 100644
--- a/drivers/pinctrl/mvebu/pinctrl-armada-xp.c
+++ b/drivers/pinctrl/mvebu/pinctrl-armada-xp.c
@@ -472,7 +472,6 @@ static int armada_xp_pinctrl_remove(struct platform_device *pdev)
 static struct platform_driver armada_xp_pinctrl_driver = {
 	.driver = {
 		.name = "armada-xp-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = armada_xp_pinctrl_of_match,
 	},
 	.probe = armada_xp_pinctrl_probe,
diff --git a/drivers/pinctrl/mvebu/pinctrl-dove.c b/drivers/pinctrl/mvebu/pinctrl-dove.c
index 3b022178a566..89a52e15f0ae 100644
--- a/drivers/pinctrl/mvebu/pinctrl-dove.c
+++ b/drivers/pinctrl/mvebu/pinctrl-dove.c
@@ -851,7 +851,6 @@ static int dove_pinctrl_remove(struct platform_device *pdev)
 static struct platform_driver dove_pinctrl_driver = {
 	.driver = {
 		.name = "dove-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = dove_pinctrl_of_match,
 	},
 	.probe = dove_pinctrl_probe,
diff --git a/drivers/pinctrl/mvebu/pinctrl-kirkwood.c b/drivers/pinctrl/mvebu/pinctrl-kirkwood.c
index 0d0211a1a0b0..dbc673cf7131 100644
--- a/drivers/pinctrl/mvebu/pinctrl-kirkwood.c
+++ b/drivers/pinctrl/mvebu/pinctrl-kirkwood.c
@@ -489,7 +489,6 @@ static int kirkwood_pinctrl_remove(struct platform_device *pdev)
 static struct platform_driver kirkwood_pinctrl_driver = {
 	.driver = {
 		.name = "kirkwood-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = kirkwood_pinctrl_of_match,
 	},
 	.probe = kirkwood_pinctrl_probe,
diff --git a/drivers/pinctrl/mvebu/pinctrl-orion.c b/drivers/pinctrl/mvebu/pinctrl-orion.c
index dda1e7254e15..3a632efb56bb 100644
--- a/drivers/pinctrl/mvebu/pinctrl-orion.c
+++ b/drivers/pinctrl/mvebu/pinctrl-orion.c
@@ -247,7 +247,6 @@ static int orion_pinctrl_remove(struct platform_device *pdev)
 static struct platform_driver orion_pinctrl_driver = {
 	.driver = {
 		.name = "orion-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(orion_pinctrl_of_match),
 	},
 	.probe = orion_pinctrl_probe,
diff --git a/drivers/pinctrl/nomadik/pinctrl-abx500.c b/drivers/pinctrl/nomadik/pinctrl-abx500.c
index e1087c75e4f4..3d6d97228523 100644
--- a/drivers/pinctrl/nomadik/pinctrl-abx500.c
+++ b/drivers/pinctrl/nomadik/pinctrl-abx500.c
@@ -1278,7 +1278,6 @@ static int abx500_gpio_remove(struct platform_device *pdev)
 static struct platform_driver abx500_gpio_driver = {
 	.driver = {
 		.name = "abx500-gpio",
-		.owner = THIS_MODULE,
 		.of_match_table = abx500_gpio_match,
 	},
 	.probe = abx500_gpio_probe,
diff --git a/drivers/pinctrl/nomadik/pinctrl-nomadik.c b/drivers/pinctrl/nomadik/pinctrl-nomadik.c
index ad99ba886e50..a6a22054c0ba 100644
--- a/drivers/pinctrl/nomadik/pinctrl-nomadik.c
+++ b/drivers/pinctrl/nomadik/pinctrl-nomadik.c
@@ -2051,7 +2051,6 @@ static const struct of_device_id nmk_gpio_match[] = {
 
 static struct platform_driver nmk_gpio_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "gpio",
 		.of_match_table = nmk_gpio_match,
 	},
@@ -2064,7 +2063,6 @@ static SIMPLE_DEV_PM_OPS(nmk_pinctrl_pm_ops,
 
 static struct platform_driver nmk_pinctrl_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "pinctrl-nomadik",
 		.of_match_table = nmk_pinctrl_match,
 		.pm = &nmk_pinctrl_pm_ops,
diff --git a/drivers/pinctrl/pinctrl-as3722.c b/drivers/pinctrl/pinctrl-as3722.c
index 1f790a4b83fe..169b1bfa00c8 100644
--- a/drivers/pinctrl/pinctrl-as3722.c
+++ b/drivers/pinctrl/pinctrl-as3722.c
@@ -634,7 +634,6 @@ MODULE_DEVICE_TABLE(of, as3722_pinctrl_of_match);
 static struct platform_driver as3722_pinctrl_driver = {
 	.driver = {
 		.name = "as3722-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = as3722_pinctrl_of_match,
 	},
 	.probe = as3722_pinctrl_probe,
diff --git a/drivers/pinctrl/pinctrl-at91.c b/drivers/pinctrl/pinctrl-at91.c
index 66db9849aca8..dfd021e8268f 100644
--- a/drivers/pinctrl/pinctrl-at91.c
+++ b/drivers/pinctrl/pinctrl-at91.c
@@ -1471,7 +1471,7 @@ static unsigned int gpio_irq_startup(struct irq_data *d)
 	unsigned	pin = d->hwirq;
 	int ret;
 
-	ret = gpio_lock_as_irq(&at91_gpio->chip, pin);
+	ret = gpiochip_lock_as_irq(&at91_gpio->chip, pin);
 	if (ret) {
 		dev_err(at91_gpio->chip.dev, "unable to lock pind %lu IRQ\n",
 			d->hwirq);
@@ -1487,7 +1487,7 @@ static void gpio_irq_shutdown(struct irq_data *d)
 	unsigned	pin = d->hwirq;
 
 	gpio_irq_mask(d);
-	gpio_unlock_as_irq(&at91_gpio->chip, pin);
+	gpiochip_unlock_as_irq(&at91_gpio->chip, pin);
 }
 
 #ifdef CONFIG_PM
@@ -1831,7 +1831,6 @@ err:
 static struct platform_driver at91_gpio_driver = {
 	.driver = {
 		.name = "gpio-at91",
-		.owner = THIS_MODULE,
 		.of_match_table = at91_gpio_of_match,
 	},
 	.probe = at91_gpio_probe,
@@ -1840,7 +1839,6 @@ static struct platform_driver at91_gpio_driver = {
 static struct platform_driver at91_pinctrl_driver = {
 	.driver = {
 		.name = "pinctrl-at91",
-		.owner = THIS_MODULE,
 		.of_match_table = at91_pinctrl_of_match,
 	},
 	.probe = at91_pinctrl_probe,
diff --git a/drivers/pinctrl/pinctrl-bcm281xx.c b/drivers/pinctrl/pinctrl-bcm281xx.c
index 2b25047fef8d..fa2a00f22ff1 100644
--- a/drivers/pinctrl/pinctrl-bcm281xx.c
+++ b/drivers/pinctrl/pinctrl-bcm281xx.c
@@ -1443,7 +1443,6 @@ static struct of_device_id bcm281xx_pinctrl_of_match[] = {
 static struct platform_driver bcm281xx_pinctrl_driver = {
 	.driver = {
 		.name = "bcm281xx-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = bcm281xx_pinctrl_of_match,
 	},
 };
diff --git a/drivers/pinctrl/pinctrl-bcm2835.c b/drivers/pinctrl/pinctrl-bcm2835.c
index eabba02f71f9..9aa8a3f10b10 100644
--- a/drivers/pinctrl/pinctrl-bcm2835.c
+++ b/drivers/pinctrl/pinctrl-bcm2835.c
@@ -1062,7 +1062,6 @@ static struct platform_driver bcm2835_pinctrl_driver = {
 	.remove = bcm2835_pinctrl_remove,
 	.driver = {
 		.name = MODULE_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = bcm2835_pinctrl_match,
 	},
 };
diff --git a/drivers/pinctrl/pinctrl-falcon.c b/drivers/pinctrl/pinctrl-falcon.c
index 2e62689b5e9f..1d21dc226920 100644
--- a/drivers/pinctrl/pinctrl-falcon.c
+++ b/drivers/pinctrl/pinctrl-falcon.c
@@ -500,7 +500,6 @@ static struct platform_driver pinctrl_falcon_driver = {
 	.probe = pinctrl_falcon_probe,
 	.driver = {
 		.name = "pinctrl-falcon",
-		.owner = THIS_MODULE,
 		.of_match_table = falcon_match,
 	},
 };
diff --git a/drivers/pinctrl/pinctrl-palmas.c b/drivers/pinctrl/pinctrl-palmas.c
index e3079d3d19fe..26461e30f0ae 100644
--- a/drivers/pinctrl/pinctrl-palmas.c
+++ b/drivers/pinctrl/pinctrl-palmas.c
@@ -1062,7 +1062,6 @@ static int palmas_pinctrl_remove(struct platform_device *pdev)
 static struct platform_driver palmas_pinctrl_driver = {
 	.driver = {
 		.name = "palmas-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = palmas_pinctrl_of_match,
 	},
 	.probe = palmas_pinctrl_probe,
diff --git a/drivers/pinctrl/pinctrl-rockchip.c b/drivers/pinctrl/pinctrl-rockchip.c
index 40970c305dd0..ba74f0aa60c7 100644
--- a/drivers/pinctrl/pinctrl-rockchip.c
+++ b/drivers/pinctrl/pinctrl-rockchip.c
@@ -2053,7 +2053,6 @@ static struct platform_driver rockchip_pinctrl_driver = {
 	.probe		= rockchip_pinctrl_probe,
 	.driver = {
 		.name	= "rockchip-pinctrl",
-		.owner	= THIS_MODULE,
 		.pm = &rockchip_pinctrl_dev_pm_ops,
 		.of_match_table = rockchip_pinctrl_dt_match,
 	},
diff --git a/drivers/pinctrl/pinctrl-single.c b/drivers/pinctrl/pinctrl-single.c
index fb94b772ad62..69e84427f913 100644
--- a/drivers/pinctrl/pinctrl-single.c
+++ b/drivers/pinctrl/pinctrl-single.c
@@ -2016,7 +2016,6 @@ static struct platform_driver pcs_driver = {
 	.probe		= pcs_probe,
 	.remove		= pcs_remove,
 	.driver = {
-		.owner		= THIS_MODULE,
 		.name		= DRIVER_NAME,
 		.of_match_table	= pcs_of_match,
 	},
diff --git a/drivers/pinctrl/pinctrl-st.c b/drivers/pinctrl/pinctrl-st.c
index caeeb1c65b0f..7c9d51382248 100644
--- a/drivers/pinctrl/pinctrl-st.c
+++ b/drivers/pinctrl/pinctrl-st.c
@@ -1689,7 +1689,6 @@ static int st_pctl_probe(struct platform_device *pdev)
 static struct platform_driver st_pctl_driver = {
 	.driver = {
 		.name = "st-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = st_pctl_of_match,
 	},
 	.probe = st_pctl_probe,
diff --git a/drivers/pinctrl/pinctrl-tb10x.c b/drivers/pinctrl/pinctrl-tb10x.c
index 9363563f9777..160a1f5e9896 100644
--- a/drivers/pinctrl/pinctrl-tb10x.c
+++ b/drivers/pinctrl/pinctrl-tb10x.c
@@ -843,7 +843,6 @@ static struct platform_driver tb10x_pinctrl_pdrv = {
 	.driver  = {
 		.name  = "tb10x_pinctrl",
 		.of_match_table = of_match_ptr(tb10x_pinctrl_dt_ids),
-		.owner = THIS_MODULE
 	}
 };
 
diff --git a/drivers/pinctrl/pinctrl-tegra-xusb.c b/drivers/pinctrl/pinctrl-tegra-xusb.c
index 080ec7723ef2..753d747d4261 100644
--- a/drivers/pinctrl/pinctrl-tegra-xusb.c
+++ b/drivers/pinctrl/pinctrl-tegra-xusb.c
@@ -915,7 +915,7 @@ static int tegra_xusb_padctl_probe(struct platform_device *pdev)
 		goto reset;
 	}
 
-	phy = devm_phy_create(&pdev->dev, NULL, &pcie_phy_ops, NULL);
+	phy = devm_phy_create(&pdev->dev, NULL, &pcie_phy_ops);
 	if (IS_ERR(phy)) {
 		err = PTR_ERR(phy);
 		goto unregister;
@@ -924,7 +924,7 @@ static int tegra_xusb_padctl_probe(struct platform_device *pdev)
 	padctl->phys[TEGRA_XUSB_PADCTL_PCIE] = phy;
 	phy_set_drvdata(phy, padctl);
 
-	phy = devm_phy_create(&pdev->dev, NULL, &sata_phy_ops, NULL);
+	phy = devm_phy_create(&pdev->dev, NULL, &sata_phy_ops);
 	if (IS_ERR(phy)) {
 		err = PTR_ERR(phy);
 		goto unregister;
diff --git a/drivers/pinctrl/pinctrl-tegra114.c b/drivers/pinctrl/pinctrl-tegra114.c
index a3db85b0b75f..52e4ec6386b4 100644
--- a/drivers/pinctrl/pinctrl-tegra114.c
+++ b/drivers/pinctrl/pinctrl-tegra114.c
@@ -1859,7 +1859,6 @@ MODULE_DEVICE_TABLE(of, tegra114_pinctrl_of_match);
 static struct platform_driver tegra114_pinctrl_driver = {
 	.driver = {
 		.name = "tegra114-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = tegra114_pinctrl_of_match,
 	},
 	.probe = tegra114_pinctrl_probe,
diff --git a/drivers/pinctrl/pinctrl-tegra124.c b/drivers/pinctrl/pinctrl-tegra124.c
index 2f9b75c14967..2b20906c5356 100644
--- a/drivers/pinctrl/pinctrl-tegra124.c
+++ b/drivers/pinctrl/pinctrl-tegra124.c
@@ -2072,7 +2072,6 @@ MODULE_DEVICE_TABLE(of, tegra124_pinctrl_of_match);
 static struct platform_driver tegra124_pinctrl_driver = {
 	.driver = {
 		.name = "tegra124-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = tegra124_pinctrl_of_match,
 	},
 	.probe = tegra124_pinctrl_probe,
diff --git a/drivers/pinctrl/pinctrl-tegra20.c b/drivers/pinctrl/pinctrl-tegra20.c
index c9805d2e71b0..d3a5722e4acb 100644
--- a/drivers/pinctrl/pinctrl-tegra20.c
+++ b/drivers/pinctrl/pinctrl-tegra20.c
@@ -2236,7 +2236,6 @@ static const struct of_device_id tegra20_pinctrl_of_match[] = {
 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,
diff --git a/drivers/pinctrl/pinctrl-tegra30.c b/drivers/pinctrl/pinctrl-tegra30.c
index e7b72e916558..f6edc2ff5494 100644
--- a/drivers/pinctrl/pinctrl-tegra30.c
+++ b/drivers/pinctrl/pinctrl-tegra30.c
@@ -2493,7 +2493,6 @@ MODULE_DEVICE_TABLE(of, tegra30_pinctrl_of_match);
 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,
diff --git a/drivers/pinctrl/pinctrl-tz1090-pdc.c b/drivers/pinctrl/pinctrl-tz1090-pdc.c
index 3bb6a3b78864..146e48a9b839 100644
--- a/drivers/pinctrl/pinctrl-tz1090-pdc.c
+++ b/drivers/pinctrl/pinctrl-tz1090-pdc.c
@@ -977,7 +977,6 @@ static struct of_device_id tz1090_pdc_pinctrl_of_match[] = {
 static struct platform_driver tz1090_pdc_pinctrl_driver = {
 	.driver = {
 		.name		= "tz1090-pdc-pinctrl",
-		.owner		= THIS_MODULE,
 		.of_match_table	= tz1090_pdc_pinctrl_of_match,
 	},
 	.probe	= tz1090_pdc_pinctrl_probe,
diff --git a/drivers/pinctrl/pinctrl-tz1090.c b/drivers/pinctrl/pinctrl-tz1090.c
index 48d36413b99f..df8cb1e5b7b4 100644
--- a/drivers/pinctrl/pinctrl-tz1090.c
+++ b/drivers/pinctrl/pinctrl-tz1090.c
@@ -1992,7 +1992,6 @@ static struct of_device_id tz1090_pinctrl_of_match[] = {
 static struct platform_driver tz1090_pinctrl_driver = {
 	.driver = {
 		.name		= "tz1090-pinctrl",
-		.owner		= THIS_MODULE,
 		.of_match_table	= tz1090_pinctrl_of_match,
 	},
 	.probe	= tz1090_pinctrl_probe,
diff --git a/drivers/pinctrl/pinctrl-u300.c b/drivers/pinctrl/pinctrl-u300.c
index e9c7113d81f2..f931e65aba3a 100644
--- a/drivers/pinctrl/pinctrl-u300.c
+++ b/drivers/pinctrl/pinctrl-u300.c
@@ -1098,7 +1098,6 @@ static const struct of_device_id u300_pinctrl_match[] = {
 static struct platform_driver u300_pmx_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = u300_pinctrl_match,
 	},
 	.probe = u300_pmx_probe,
diff --git a/drivers/pinctrl/pinctrl-xway.c b/drivers/pinctrl/pinctrl-xway.c
index 37040ab42890..c5cef59f5965 100644
--- a/drivers/pinctrl/pinctrl-xway.c
+++ b/drivers/pinctrl/pinctrl-xway.c
@@ -840,7 +840,6 @@ static struct platform_driver pinmux_xway_driver = {
 	.probe	= pinmux_xway_probe,
 	.driver = {
 		.name	= "pinctrl-xway",
-		.owner	= THIS_MODULE,
 		.of_match_table = xway_match,
 	},
 };
diff --git a/drivers/pinctrl/qcom/pinctrl-apq8064.c b/drivers/pinctrl/qcom/pinctrl-apq8064.c
index c832d7d6b912..cd96699b1929 100644
--- a/drivers/pinctrl/qcom/pinctrl-apq8064.c
+++ b/drivers/pinctrl/qcom/pinctrl-apq8064.c
@@ -612,7 +612,6 @@ static const struct of_device_id apq8064_pinctrl_of_match[] = {
 static struct platform_driver apq8064_pinctrl_driver = {
 	.driver = {
 		.name = "apq8064-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = apq8064_pinctrl_of_match,
 	},
 	.probe = apq8064_pinctrl_probe,
diff --git a/drivers/pinctrl/qcom/pinctrl-apq8084.c b/drivers/pinctrl/qcom/pinctrl-apq8084.c
index 138cbf6134a5..d07e8df43b90 100644
--- a/drivers/pinctrl/qcom/pinctrl-apq8084.c
+++ b/drivers/pinctrl/qcom/pinctrl-apq8084.c
@@ -1221,7 +1221,6 @@ static const struct of_device_id apq8084_pinctrl_of_match[] = {
 static struct platform_driver apq8084_pinctrl_driver = {
 	.driver = {
 		.name = "apq8084-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = apq8084_pinctrl_of_match,
 	},
 	.probe = apq8084_pinctrl_probe,
diff --git a/drivers/pinctrl/qcom/pinctrl-ipq8064.c b/drivers/pinctrl/qcom/pinctrl-ipq8064.c
index 81f49a9b4dbe..bcb29c02f4b0 100644
--- a/drivers/pinctrl/qcom/pinctrl-ipq8064.c
+++ b/drivers/pinctrl/qcom/pinctrl-ipq8064.c
@@ -645,7 +645,6 @@ static const struct of_device_id ipq8064_pinctrl_of_match[] = {
 static struct platform_driver ipq8064_pinctrl_driver = {
 	.driver = {
 		.name = "ipq8064-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = ipq8064_pinctrl_of_match,
 	},
 	.probe = ipq8064_pinctrl_probe,
diff --git a/drivers/pinctrl/qcom/pinctrl-msm8960.c b/drivers/pinctrl/qcom/pinctrl-msm8960.c
index 2ab21ce5575a..ed23e367fe89 100644
--- a/drivers/pinctrl/qcom/pinctrl-msm8960.c
+++ b/drivers/pinctrl/qcom/pinctrl-msm8960.c
@@ -1259,7 +1259,6 @@ static const struct of_device_id msm8960_pinctrl_of_match[] = {
 static struct platform_driver msm8960_pinctrl_driver = {
 	.driver = {
 		.name = "msm8960-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = msm8960_pinctrl_of_match,
 	},
 	.probe = msm8960_pinctrl_probe,
diff --git a/drivers/pinctrl/qcom/pinctrl-msm8x74.c b/drivers/pinctrl/qcom/pinctrl-msm8x74.c
index 3c858384d041..46fe6ad5f97e 100644
--- a/drivers/pinctrl/qcom/pinctrl-msm8x74.c
+++ b/drivers/pinctrl/qcom/pinctrl-msm8x74.c
@@ -1043,7 +1043,6 @@ static const struct of_device_id msm8x74_pinctrl_of_match[] = {
 static struct platform_driver msm8x74_pinctrl_driver = {
 	.driver = {
 		.name = "msm8x74-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = msm8x74_pinctrl_of_match,
 	},
 	.probe = msm8x74_pinctrl_probe,
diff --git a/drivers/pinctrl/samsung/pinctrl-exynos.c b/drivers/pinctrl/samsung/pinctrl-exynos.c
index d5d4cfc55873..becb3792977b 100644
--- a/drivers/pinctrl/samsung/pinctrl-exynos.c
+++ b/drivers/pinctrl/samsung/pinctrl-exynos.c
@@ -174,7 +174,7 @@ static int exynos_irq_request_resources(struct irq_data *irqd)
 	unsigned int con;
 	int ret;
 
-	ret = gpio_lock_as_irq(&bank->gpio_chip, irqd->hwirq);
+	ret = gpiochip_lock_as_irq(&bank->gpio_chip, irqd->hwirq);
 	if (ret) {
 		dev_err(bank->gpio_chip.dev, "unable to lock pin %s-%lu IRQ\n",
 			bank->name, irqd->hwirq);
@@ -227,7 +227,7 @@ static void exynos_irq_release_resources(struct irq_data *irqd)
 
 	spin_unlock_irqrestore(&bank->slock, flags);
 
-	gpio_unlock_as_irq(&bank->gpio_chip, irqd->hwirq);
+	gpiochip_unlock_as_irq(&bank->gpio_chip, irqd->hwirq);
 }
 
 /*
diff --git a/drivers/pinctrl/samsung/pinctrl-exynos5440.c b/drivers/pinctrl/samsung/pinctrl-exynos5440.c
index 88acfc0efd54..86192be3b679 100644
--- a/drivers/pinctrl/samsung/pinctrl-exynos5440.c
+++ b/drivers/pinctrl/samsung/pinctrl-exynos5440.c
@@ -1036,7 +1036,6 @@ static struct platform_driver exynos5440_pinctrl_driver = {
 	.probe		= exynos5440_pinctrl_probe,
 	.driver = {
 		.name	= "exynos5440-pinctrl",
-		.owner	= THIS_MODULE,
 		.of_match_table = exynos5440_pinctrl_dt_match,
 	},
 };
diff --git a/drivers/pinctrl/samsung/pinctrl-samsung.c b/drivers/pinctrl/samsung/pinctrl-samsung.c
index 32940a01a84f..ec580af35856 100644
--- a/drivers/pinctrl/samsung/pinctrl-samsung.c
+++ b/drivers/pinctrl/samsung/pinctrl-samsung.c
@@ -1266,7 +1266,6 @@ static struct platform_driver samsung_pinctrl_driver = {
 	.probe		= samsung_pinctrl_probe,
 	.driver = {
 		.name	= "samsung-pinctrl",
-		.owner	= THIS_MODULE,
 		.of_match_table = samsung_pinctrl_dt_match,
 	},
 };
diff --git a/drivers/pinctrl/sh-pfc/core.c b/drivers/pinctrl/sh-pfc/core.c
index 6572c233f73d..66dc62d2156c 100644
--- a/drivers/pinctrl/sh-pfc/core.c
+++ b/drivers/pinctrl/sh-pfc/core.c
@@ -650,7 +650,6 @@ static struct platform_driver sh_pfc_driver = {
 	.id_table	= sh_pfc_id_table,
 	.driver		= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(sh_pfc_of_table),
 	},
 };
diff --git a/drivers/pinctrl/sirf/pinctrl-sirf.c b/drivers/pinctrl/sirf/pinctrl-sirf.c
index 4c831fdfcc2f..4871647c7f85 100644
--- a/drivers/pinctrl/sirf/pinctrl-sirf.c
+++ b/drivers/pinctrl/sirf/pinctrl-sirf.c
@@ -406,7 +406,6 @@ static const struct dev_pm_ops sirfsoc_pinmux_pm_ops = {
 static struct platform_driver sirfsoc_pinmux_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = pinmux_ids,
 #ifdef CONFIG_PM_SLEEP
 		.pm = &sirfsoc_pinmux_pm_ops,
diff --git a/drivers/pinctrl/spear/pinctrl-plgpio.c b/drivers/pinctrl/spear/pinctrl-plgpio.c
index bddb79105d67..ae8f29fb5536 100644
--- a/drivers/pinctrl/spear/pinctrl-plgpio.c
+++ b/drivers/pinctrl/spear/pinctrl-plgpio.c
@@ -710,7 +710,6 @@ MODULE_DEVICE_TABLE(of, plgpio_of_match);
 static struct platform_driver plgpio_driver = {
 	.probe = plgpio_probe,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "spear-plgpio",
 		.pm = &plgpio_dev_pm_ops,
 		.of_match_table = plgpio_of_match,
@@ -724,5 +723,5 @@ static int __init plgpio_init(void)
 subsys_initcall(plgpio_init);
 
 MODULE_AUTHOR("Viresh Kumar <viresh.kumar@linaro.org>");
-MODULE_DESCRIPTION("ST Microlectronics SPEAr PLGPIO driver");
+MODULE_DESCRIPTION("STMicroelectronics SPEAr PLGPIO driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/spear/pinctrl-spear1310.c b/drivers/pinctrl/spear/pinctrl-spear1310.c
index 6d57d43ab640..a7bdc537efa7 100644
--- a/drivers/pinctrl/spear/pinctrl-spear1310.c
+++ b/drivers/pinctrl/spear/pinctrl-spear1310.c
@@ -2712,7 +2712,6 @@ static int spear1310_pinctrl_remove(struct platform_device *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,
diff --git a/drivers/pinctrl/spear/pinctrl-spear1340.c b/drivers/pinctrl/spear/pinctrl-spear1340.c
index d243e43e7f6d..f43ec85a0328 100644
--- a/drivers/pinctrl/spear/pinctrl-spear1340.c
+++ b/drivers/pinctrl/spear/pinctrl-spear1340.c
@@ -2028,7 +2028,6 @@ static int spear1340_pinctrl_remove(struct platform_device *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,
diff --git a/drivers/pinctrl/spear/pinctrl-spear300.c b/drivers/pinctrl/spear/pinctrl-spear300.c
index 9db83e9ee18c..da8990a8eeef 100644
--- a/drivers/pinctrl/spear/pinctrl-spear300.c
+++ b/drivers/pinctrl/spear/pinctrl-spear300.c
@@ -685,7 +685,6 @@ static int spear300_pinctrl_remove(struct platform_device *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,
diff --git a/drivers/pinctrl/spear/pinctrl-spear310.c b/drivers/pinctrl/spear/pinctrl-spear310.c
index db775a414b7a..31ede51e819b 100644
--- a/drivers/pinctrl/spear/pinctrl-spear310.c
+++ b/drivers/pinctrl/spear/pinctrl-spear310.c
@@ -408,7 +408,6 @@ static int spear310_pinctrl_remove(struct platform_device *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,
diff --git a/drivers/pinctrl/spear/pinctrl-spear320.c b/drivers/pinctrl/spear/pinctrl-spear320.c
index 80fbd68e17bc..506e40b641e0 100644
--- a/drivers/pinctrl/spear/pinctrl-spear320.c
+++ b/drivers/pinctrl/spear/pinctrl-spear320.c
@@ -3449,7 +3449,6 @@ static int spear320_pinctrl_remove(struct platform_device *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,
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c b/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c
index 86b608bedca6..24c5d88f943f 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c
@@ -1029,7 +1029,6 @@ static struct platform_driver sun4i_a10_pinctrl_driver = {
 	.probe	= sun4i_a10_pinctrl_probe,
 	.driver	= {
 		.name		= "sun4i-pinctrl",
-		.owner		= THIS_MODULE,
 		.of_match_table	= sun4i_a10_pinctrl_match,
 	},
 };
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun5i-a10s.c b/drivers/pinctrl/sunxi/pinctrl-sun5i-a10s.c
index 2fa7430cabaf..45a351affa59 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun5i-a10s.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun5i-a10s.c
@@ -680,7 +680,6 @@ static struct platform_driver sun5i_a10s_pinctrl_driver = {
 	.probe	= sun5i_a10s_pinctrl_probe,
 	.driver	= {
 		.name		= "sun5i-a10s-pinctrl",
-		.owner		= THIS_MODULE,
 		.of_match_table	= sun5i_a10s_pinctrl_match,
 	},
 };
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun5i-a13.c b/drivers/pinctrl/sunxi/pinctrl-sun5i-a13.c
index e47c33dbae3a..4bd23471412c 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun5i-a13.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun5i-a13.c
@@ -398,7 +398,6 @@ static struct platform_driver sun5i_a13_pinctrl_driver = {
 	.probe	= sun5i_a13_pinctrl_probe,
 	.driver	= {
 		.name		= "sun5i-a13-pinctrl",
-		.owner		= THIS_MODULE,
 		.of_match_table	= sun5i_a13_pinctrl_match,
 	},
 };
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun6i-a31-r.c b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31-r.c
index 9a2517b65113..02174fa57997 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun6i-a31-r.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31-r.c
@@ -130,7 +130,6 @@ static struct platform_driver sun6i_a31_r_pinctrl_driver = {
 	.probe	= sun6i_a31_r_pinctrl_probe,
 	.driver	= {
 		.name		= "sun6i-a31-r-pinctrl",
-		.owner		= THIS_MODULE,
 		.of_match_table	= sun6i_a31_r_pinctrl_match,
 	},
 };
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun6i-a31.c b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31.c
index a2b4b85c5ad5..f42858eaca28 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun6i-a31.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31.c
@@ -927,7 +927,6 @@ static struct platform_driver sun6i_a31_pinctrl_driver = {
 	.probe	= sun6i_a31_pinctrl_probe,
 	.driver	= {
 		.name		= "sun6i-a31-pinctrl",
-		.owner		= THIS_MODULE,
 		.of_match_table	= sun6i_a31_pinctrl_match,
 	},
 };
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun7i-a20.c b/drivers/pinctrl/sunxi/pinctrl-sun7i-a20.c
index dac99e02bfdb..6af6cc8547b0 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun7i-a20.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun7i-a20.c
@@ -1055,7 +1055,6 @@ static struct platform_driver sun7i_a20_pinctrl_driver = {
 	.probe	= sun7i_a20_pinctrl_probe,
 	.driver	= {
 		.name		= "sun7i-a20-pinctrl",
-		.owner		= THIS_MODULE,
 		.of_match_table	= sun7i_a20_pinctrl_match,
 	},
 };
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun8i-a23-r.c b/drivers/pinctrl/sunxi/pinctrl-sun8i-a23-r.c
index 90f3b3a7c51e..327e03ff7c4d 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun8i-a23-r.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun8i-a23-r.c
@@ -129,7 +129,6 @@ static struct platform_driver sun8i_a23_r_pinctrl_driver = {
 	.probe	= sun8i_a23_r_pinctrl_probe,
 	.driver	= {
 		.name		= "sun8i-a23-r-pinctrl",
-		.owner		= THIS_MODULE,
 		.of_match_table	= sun8i_a23_r_pinctrl_match,
 	},
 };
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun8i-a23.c b/drivers/pinctrl/sunxi/pinctrl-sun8i-a23.c
index ac71e8c5901b..62695c9a92c2 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun8i-a23.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun8i-a23.c
@@ -581,7 +581,6 @@ static struct platform_driver sun8i_a23_pinctrl_driver = {
 	.probe	= sun8i_a23_pinctrl_probe,
 	.driver	= {
 		.name		= "sun8i-a23-pinctrl",
-		.owner		= THIS_MODULE,
 		.of_match_table	= sun8i_a23_pinctrl_match,
 	},
 };
diff --git a/drivers/pinctrl/sunxi/pinctrl-sunxi.c b/drivers/pinctrl/sunxi/pinctrl-sunxi.c
index ef9d804e55de..3d0744337736 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sunxi.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sunxi.c
@@ -553,7 +553,7 @@ static int sunxi_pinctrl_irq_request_resources(struct irq_data *d)
 	if (!func)
 		return -EINVAL;
 
-	ret = gpio_lock_as_irq(pctl->chip,
+	ret = gpiochip_lock_as_irq(pctl->chip,
 			pctl->irq_array[d->hwirq] - pctl->desc->pin_base);
 	if (ret) {
 		dev_err(pctl->dev, "unable to lock HW IRQ %lu for IRQ\n",
@@ -571,8 +571,8 @@ static void sunxi_pinctrl_irq_release_resources(struct irq_data *d)
 {
 	struct sunxi_pinctrl *pctl = irq_data_get_irq_chip_data(d);
 
-	gpio_unlock_as_irq(pctl->chip,
-			   pctl->irq_array[d->hwirq] - pctl->desc->pin_base);
+	gpiochip_unlock_as_irq(pctl->chip,
+			      pctl->irq_array[d->hwirq] - pctl->desc->pin_base);
 }
 
 static int sunxi_pinctrl_irq_set_type(struct irq_data *d, unsigned int type)
diff --git a/drivers/pinctrl/vt8500/pinctrl-vt8500.c b/drivers/pinctrl/vt8500/pinctrl-vt8500.c
index f2fe9f85cfa6..cf8bbc946ff7 100644
--- a/drivers/pinctrl/vt8500/pinctrl-vt8500.c
+++ b/drivers/pinctrl/vt8500/pinctrl-vt8500.c
@@ -488,7 +488,6 @@ static struct platform_driver wmt_pinctrl_driver = {
 	.remove	= vt8500_pinctrl_remove,
 	.driver = {
 		.name	= "pinctrl-vt8500",
-		.owner	= THIS_MODULE,
 		.of_match_table	= wmt_pinctrl_of_match,
 	},
 };
diff --git a/drivers/pinctrl/vt8500/pinctrl-wm8505.c b/drivers/pinctrl/vt8500/pinctrl-wm8505.c
index 483ba732694e..3f9c32dcb3d0 100644
--- a/drivers/pinctrl/vt8500/pinctrl-wm8505.c
+++ b/drivers/pinctrl/vt8500/pinctrl-wm8505.c
@@ -519,7 +519,6 @@ static struct platform_driver wmt_pinctrl_driver = {
 	.remove	= wm8505_pinctrl_remove,
 	.driver = {
 		.name	= "pinctrl-wm8505",
-		.owner	= THIS_MODULE,
 		.of_match_table	= wmt_pinctrl_of_match,
 	},
 };
diff --git a/drivers/pinctrl/vt8500/pinctrl-wm8650.c b/drivers/pinctrl/vt8500/pinctrl-wm8650.c
index 7de57f063153..4e80f98c2ba7 100644
--- a/drivers/pinctrl/vt8500/pinctrl-wm8650.c
+++ b/drivers/pinctrl/vt8500/pinctrl-wm8650.c
@@ -357,7 +357,6 @@ static struct platform_driver wmt_pinctrl_driver = {
 	.remove	= wm8650_pinctrl_remove,
 	.driver = {
 		.name	= "pinctrl-wm8650",
-		.owner	= THIS_MODULE,
 		.of_match_table	= wmt_pinctrl_of_match,
 	},
 };
diff --git a/drivers/pinctrl/vt8500/pinctrl-wm8750.c b/drivers/pinctrl/vt8500/pinctrl-wm8750.c
index de43262398db..47b52a7cacac 100644
--- a/drivers/pinctrl/vt8500/pinctrl-wm8750.c
+++ b/drivers/pinctrl/vt8500/pinctrl-wm8750.c
@@ -396,7 +396,6 @@ static struct platform_driver wmt_pinctrl_driver = {
 	.remove	= wm8750_pinctrl_remove,
 	.driver = {
 		.name	= "pinctrl-wm8750",
-		.owner	= THIS_MODULE,
 		.of_match_table	= wmt_pinctrl_of_match,
 	},
 };
diff --git a/drivers/pinctrl/vt8500/pinctrl-wm8850.c b/drivers/pinctrl/vt8500/pinctrl-wm8850.c
index ecadce9c91d5..8bbb38c931f6 100644
--- a/drivers/pinctrl/vt8500/pinctrl-wm8850.c
+++ b/drivers/pinctrl/vt8500/pinctrl-wm8850.c
@@ -375,7 +375,6 @@ static struct platform_driver wmt_pinctrl_driver = {
 	.remove	= wm8850_pinctrl_remove,
 	.driver = {
 		.name	= "pinctrl-wm8850",
-		.owner	= THIS_MODULE,
 		.of_match_table	= wmt_pinctrl_of_match,
 	},
 };
diff --git a/drivers/platform/chrome/chromeos_laptop.c b/drivers/platform/chrome/chromeos_laptop.c
index d866db80b4fd..b84fdd6b629b 100644
--- a/drivers/platform/chrome/chromeos_laptop.c
+++ b/drivers/platform/chrome/chromeos_laptop.c
@@ -571,7 +571,6 @@ static struct platform_device *cros_platform_device;
 static struct platform_driver cros_platform_driver = {
 	.driver = {
 		.name = "chromeos_laptop",
-		.owner = THIS_MODULE,
 	},
 	.probe = chromeos_laptop_probe,
 };
diff --git a/drivers/platform/x86/Kconfig b/drivers/platform/x86/Kconfig
index a2eabe6ff9ad..638e797037da 100644
--- a/drivers/platform/x86/Kconfig
+++ b/drivers/platform/x86/Kconfig
@@ -38,7 +38,8 @@ config ACER_WMI
 
 config ACERHDF
 	tristate "Acer Aspire One temperature and fan driver"
-	depends on THERMAL && ACPI
+	depends on ACPI && THERMAL
+	select THERMAL_GOV_BANG_BANG
 	---help---
 	  This is a driver for Acer Aspire One netbooks. It allows to access
 	  the temperature sensor and to control the fan.
@@ -128,10 +129,10 @@ config DELL_WMI_AIO
 	  be called dell-wmi-aio.
 
 config DELL_SMO8800
-	tristate "Dell Latitude freefall driver (ACPI SMO8800/SMO8810)"
+	tristate "Dell Latitude freefall driver (ACPI SMO88XX)"
 	depends on ACPI
 	---help---
-	  Say Y here if you want to support SMO8800/SMO8810 freefall device
+	  Say Y here if you want to support SMO88XX freefall devices
 	  on Dell Latitude laptops.
 
 	  To compile this driver as a module, choose M here: the module will
diff --git a/drivers/platform/x86/acer-wmi.c b/drivers/platform/x86/acer-wmi.c
index 26c4fd1394da..3ac29a1e8f92 100644
--- a/drivers/platform/x86/acer-wmi.c
+++ b/drivers/platform/x86/acer-wmi.c
@@ -2117,7 +2117,6 @@ static void acer_platform_shutdown(struct platform_device *device)
 static struct platform_driver acer_platform_driver = {
 	.driver = {
 		.name = "acer-wmi",
-		.owner = THIS_MODULE,
 		.pm = &acer_pm,
 	},
 	.probe = acer_platform_probe,
diff --git a/drivers/platform/x86/acerhdf.c b/drivers/platform/x86/acerhdf.c
index f94467c05225..594c918b553d 100644
--- a/drivers/platform/x86/acerhdf.c
+++ b/drivers/platform/x86/acerhdf.c
@@ -50,7 +50,7 @@
  */
 #undef START_IN_KERNEL_MODE
 
-#define DRV_VER "0.5.26"
+#define DRV_VER "0.7.0"
 
 /*
  * According to the Atom N270 datasheet,
@@ -119,116 +119,152 @@ struct fancmd {
 	u8 cmd_auto;
 };
 
+struct manualcmd {
+	u8 mreg;
+	u8 moff;
+};
+
+/* default register and command to disable fan in manual mode */
+static const struct manualcmd mcmd = {
+	.mreg = 0x94,
+	.moff = 0xff,
+};
+
 /* BIOS settings */
-struct bios_settings_t {
+struct bios_settings {
 	const char *vendor;
 	const char *product;
 	const char *version;
-	unsigned char fanreg;
-	unsigned char tempreg;
+	u8 fanreg;
+	u8 tempreg;
 	struct fancmd cmd;
+	int mcmd_enable;
 };
 
 /* Register addresses and values for different BIOS versions */
-static const struct bios_settings_t bios_tbl[] = {
+static const struct bios_settings bios_tbl[] = {
 	/* AOA110 */
-	{"Acer", "AOA110", "v0.3109", 0x55, 0x58, {0x1f, 0x00} },
-	{"Acer", "AOA110", "v0.3114", 0x55, 0x58, {0x1f, 0x00} },
-	{"Acer", "AOA110", "v0.3301", 0x55, 0x58, {0xaf, 0x00} },
-	{"Acer", "AOA110", "v0.3304", 0x55, 0x58, {0xaf, 0x00} },
-	{"Acer", "AOA110", "v0.3305", 0x55, 0x58, {0xaf, 0x00} },
-	{"Acer", "AOA110", "v0.3307", 0x55, 0x58, {0xaf, 0x00} },
-	{"Acer", "AOA110", "v0.3308", 0x55, 0x58, {0x21, 0x00} },
-	{"Acer", "AOA110", "v0.3309", 0x55, 0x58, {0x21, 0x00} },
-	{"Acer", "AOA110", "v0.3310", 0x55, 0x58, {0x21, 0x00} },
+	{"Acer", "AOA110", "v0.3109", 0x55, 0x58, {0x1f, 0x00}, 0},
+	{"Acer", "AOA110", "v0.3114", 0x55, 0x58, {0x1f, 0x00}, 0},
+	{"Acer", "AOA110", "v0.3301", 0x55, 0x58, {0xaf, 0x00}, 0},
+	{"Acer", "AOA110", "v0.3304", 0x55, 0x58, {0xaf, 0x00}, 0},
+	{"Acer", "AOA110", "v0.3305", 0x55, 0x58, {0xaf, 0x00}, 0},
+	{"Acer", "AOA110", "v0.3307", 0x55, 0x58, {0xaf, 0x00}, 0},
+	{"Acer", "AOA110", "v0.3308", 0x55, 0x58, {0x21, 0x00}, 0},
+	{"Acer", "AOA110", "v0.3309", 0x55, 0x58, {0x21, 0x00}, 0},
+	{"Acer", "AOA110", "v0.3310", 0x55, 0x58, {0x21, 0x00}, 0},
 	/* AOA150 */
-	{"Acer", "AOA150", "v0.3114", 0x55, 0x58, {0x1f, 0x00} },
-	{"Acer", "AOA150", "v0.3301", 0x55, 0x58, {0x20, 0x00} },
-	{"Acer", "AOA150", "v0.3304", 0x55, 0x58, {0x20, 0x00} },
-	{"Acer", "AOA150", "v0.3305", 0x55, 0x58, {0x20, 0x00} },
-	{"Acer", "AOA150", "v0.3307", 0x55, 0x58, {0x20, 0x00} },
-	{"Acer", "AOA150", "v0.3308", 0x55, 0x58, {0x20, 0x00} },
-	{"Acer", "AOA150", "v0.3309", 0x55, 0x58, {0x20, 0x00} },
-	{"Acer", "AOA150", "v0.3310", 0x55, 0x58, {0x20, 0x00} },
+	{"Acer", "AOA150", "v0.3114", 0x55, 0x58, {0x1f, 0x00}, 0},
+	{"Acer", "AOA150", "v0.3301", 0x55, 0x58, {0x20, 0x00}, 0},
+	{"Acer", "AOA150", "v0.3304", 0x55, 0x58, {0x20, 0x00}, 0},
+	{"Acer", "AOA150", "v0.3305", 0x55, 0x58, {0x20, 0x00}, 0},
+	{"Acer", "AOA150", "v0.3307", 0x55, 0x58, {0x20, 0x00}, 0},
+	{"Acer", "AOA150", "v0.3308", 0x55, 0x58, {0x20, 0x00}, 0},
+	{"Acer", "AOA150", "v0.3309", 0x55, 0x58, {0x20, 0x00}, 0},
+	{"Acer", "AOA150", "v0.3310", 0x55, 0x58, {0x20, 0x00}, 0},
 	/* LT1005u */
-	{"Acer", "LT-10Q", "v0.3310", 0x55, 0x58, {0x20, 0x00} },
+	{"Acer", "LT-10Q", "v0.3310", 0x55, 0x58, {0x20, 0x00}, 0},
 	/* Acer 1410 */
-	{"Acer", "Aspire 1410", "v0.3108", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1410", "v0.3113", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1410", "v0.3115", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1410", "v0.3117", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1410", "v0.3119", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1410", "v0.3120", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1410", "v1.3204", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1410", "v1.3303", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1410", "v1.3308", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1410", "v1.3310", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1410", "v1.3314", 0x55, 0x58, {0x9e, 0x00} },
+	{"Acer", "Aspire 1410", "v0.3108", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1410", "v0.3113", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1410", "v0.3115", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1410", "v0.3117", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1410", "v0.3119", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1410", "v0.3120", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1410", "v1.3204", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1410", "v1.3303", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1410", "v1.3308", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1410", "v1.3310", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1410", "v1.3314", 0x55, 0x58, {0x9e, 0x00}, 0},
 	/* Acer 1810xx */
-	{"Acer", "Aspire 1810TZ", "v0.3108", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810T",  "v0.3108", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810TZ", "v0.3113", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810T",  "v0.3113", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810TZ", "v0.3115", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810T",  "v0.3115", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810TZ", "v0.3117", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810T",  "v0.3117", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810TZ", "v0.3119", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810T",  "v0.3119", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810TZ", "v0.3120", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810T",  "v0.3120", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810TZ", "v1.3204", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810T",  "v1.3204", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810TZ", "v1.3303", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810T",  "v1.3303", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810TZ", "v1.3308", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810T",  "v1.3308", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810TZ", "v1.3310", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810T",  "v1.3310", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810TZ", "v1.3314", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1810T",  "v1.3314", 0x55, 0x58, {0x9e, 0x00} },
+	{"Acer", "Aspire 1810TZ", "v0.3108", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810T",  "v0.3108", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810TZ", "v0.3113", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810T",  "v0.3113", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810TZ", "v0.3115", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810T",  "v0.3115", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810TZ", "v0.3117", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810T",  "v0.3117", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810TZ", "v0.3119", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810T",  "v0.3119", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810TZ", "v0.3120", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810T",  "v0.3120", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810TZ", "v1.3204", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810T",  "v1.3204", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810TZ", "v1.3303", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810T",  "v1.3303", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810TZ", "v1.3308", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810T",  "v1.3308", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810TZ", "v1.3310", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810T",  "v1.3310", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810TZ", "v1.3314", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1810T",  "v1.3314", 0x55, 0x58, {0x9e, 0x00}, 0},
+	/* Acer 5755G */
+	{"Acer", "Aspire 5755G",  "V1.20",   0xab, 0xb4, {0x00, 0x08}, 0},
+	{"Acer", "Aspire 5755G",  "V1.21",   0xab, 0xb3, {0x00, 0x08}, 0},
+	/* Acer 521 */
+	{"Acer", "AO521", "V1.11", 0x55, 0x58, {0x1f, 0x00}, 0},
 	/* Acer 531 */
-	{"Acer", "AO531h", "v0.3104", 0x55, 0x58, {0x20, 0x00} },
-	{"Acer", "AO531h", "v0.3201", 0x55, 0x58, {0x20, 0x00} },
-	{"Acer", "AO531h", "v0.3304", 0x55, 0x58, {0x20, 0x00} },
+	{"Acer", "AO531h", "v0.3104", 0x55, 0x58, {0x20, 0x00}, 0},
+	{"Acer", "AO531h", "v0.3201", 0x55, 0x58, {0x20, 0x00}, 0},
+	{"Acer", "AO531h", "v0.3304", 0x55, 0x58, {0x20, 0x00}, 0},
 	/* Acer 751 */
-	{"Acer", "AO751h", "V0.3212", 0x55, 0x58, {0x21, 0x00} },
+	{"Acer", "AO751h", "V0.3206", 0x55, 0x58, {0x21, 0x00}, 0},
+	{"Acer", "AO751h", "V0.3212", 0x55, 0x58, {0x21, 0x00}, 0},
+	/* Acer 753 */
+	{"Acer", "Aspire One 753", "V1.24", 0x93, 0xac, {0x14, 0x04}, 1},
 	/* Acer 1825 */
-	{"Acer", "Aspire 1825PTZ", "V1.3118", 0x55, 0x58, {0x9e, 0x00} },
-	{"Acer", "Aspire 1825PTZ", "V1.3127", 0x55, 0x58, {0x9e, 0x00} },
+	{"Acer", "Aspire 1825PTZ", "V1.3118", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Acer", "Aspire 1825PTZ", "V1.3127", 0x55, 0x58, {0x9e, 0x00}, 0},
+	/* Acer Extensa 5420 */
+	{"Acer", "Extensa 5420", "V1.17", 0x93, 0xac, {0x14, 0x04}, 1},
+	/* Acer Aspire 5315 */
+	{"Acer", "Aspire 5315", "V1.19", 0x93, 0xac, {0x14, 0x04}, 1},
+	/* Acer Aspire 5739 */
+	{"Acer", "Aspire 5739G", "V1.3311", 0x55, 0x58, {0x20, 0x00}, 0},
 	/* Acer TravelMate 7730 */
-	{"Acer", "TravelMate 7730G", "v0.3509", 0x55, 0x58, {0xaf, 0x00} },
+	{"Acer", "TravelMate 7730G", "v0.3509", 0x55, 0x58, {0xaf, 0x00}, 0},
+	/* Acer TravelMate TM8573T */
+	{"Acer", "TM8573T", "V1.13", 0x93, 0xa8, {0x14, 0x04}, 1},
 	/* Gateway */
-	{"Gateway", "AOA110", "v0.3103",  0x55, 0x58, {0x21, 0x00} },
-	{"Gateway", "AOA150", "v0.3103",  0x55, 0x58, {0x20, 0x00} },
-	{"Gateway", "LT31",   "v1.3103",  0x55, 0x58, {0x9e, 0x00} },
-	{"Gateway", "LT31",   "v1.3201",  0x55, 0x58, {0x9e, 0x00} },
-	{"Gateway", "LT31",   "v1.3302",  0x55, 0x58, {0x9e, 0x00} },
-	{"Gateway", "LT31",   "v1.3303t", 0x55, 0x58, {0x9e, 0x00} },
+	{"Gateway", "AOA110", "v0.3103",  0x55, 0x58, {0x21, 0x00}, 0},
+	{"Gateway", "AOA150", "v0.3103",  0x55, 0x58, {0x20, 0x00}, 0},
+	{"Gateway", "LT31",   "v1.3103",  0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Gateway", "LT31",   "v1.3201",  0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Gateway", "LT31",   "v1.3302",  0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Gateway", "LT31",   "v1.3303t", 0x55, 0x58, {0x9e, 0x00}, 0},
 	/* Packard Bell */
-	{"Packard Bell", "DOA150",  "v0.3104",  0x55, 0x58, {0x21, 0x00} },
-	{"Packard Bell", "DOA150",  "v0.3105",  0x55, 0x58, {0x20, 0x00} },
-	{"Packard Bell", "AOA110",  "v0.3105",  0x55, 0x58, {0x21, 0x00} },
-	{"Packard Bell", "AOA150",  "v0.3105",  0x55, 0x58, {0x20, 0x00} },
-	{"Packard Bell", "ENBFT",   "V1.3118",  0x55, 0x58, {0x9e, 0x00} },
-	{"Packard Bell", "ENBFT",   "V1.3127",  0x55, 0x58, {0x9e, 0x00} },
-	{"Packard Bell", "DOTMU",   "v1.3303",  0x55, 0x58, {0x9e, 0x00} },
-	{"Packard Bell", "DOTMU",   "v0.3120",  0x55, 0x58, {0x9e, 0x00} },
-	{"Packard Bell", "DOTMU",   "v0.3108",  0x55, 0x58, {0x9e, 0x00} },
-	{"Packard Bell", "DOTMU",   "v0.3113",  0x55, 0x58, {0x9e, 0x00} },
-	{"Packard Bell", "DOTMU",   "v0.3115",  0x55, 0x58, {0x9e, 0x00} },
-	{"Packard Bell", "DOTMU",   "v0.3117",  0x55, 0x58, {0x9e, 0x00} },
-	{"Packard Bell", "DOTMU",   "v0.3119",  0x55, 0x58, {0x9e, 0x00} },
-	{"Packard Bell", "DOTMU",   "v1.3204",  0x55, 0x58, {0x9e, 0x00} },
-	{"Packard Bell", "DOTMA",   "v1.3201",  0x55, 0x58, {0x9e, 0x00} },
-	{"Packard Bell", "DOTMA",   "v1.3302",  0x55, 0x58, {0x9e, 0x00} },
-	{"Packard Bell", "DOTMA",   "v1.3303t", 0x55, 0x58, {0x9e, 0x00} },
-	{"Packard Bell", "DOTVR46", "v1.3308",  0x55, 0x58, {0x9e, 0x00} },
+	{"Packard Bell", "DOA150",  "v0.3104",  0x55, 0x58, {0x21, 0x00}, 0},
+	{"Packard Bell", "DOA150",  "v0.3105",  0x55, 0x58, {0x20, 0x00}, 0},
+	{"Packard Bell", "AOA110",  "v0.3105",  0x55, 0x58, {0x21, 0x00}, 0},
+	{"Packard Bell", "AOA150",  "v0.3105",  0x55, 0x58, {0x20, 0x00}, 0},
+	{"Packard Bell", "ENBFT",   "V1.3118",  0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Packard Bell", "ENBFT",   "V1.3127",  0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Packard Bell", "DOTMU",   "v1.3303",  0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Packard Bell", "DOTMU",   "v0.3120",  0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Packard Bell", "DOTMU",   "v0.3108",  0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Packard Bell", "DOTMU",   "v0.3113",  0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Packard Bell", "DOTMU",   "v0.3115",  0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Packard Bell", "DOTMU",   "v0.3117",  0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Packard Bell", "DOTMU",   "v0.3119",  0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Packard Bell", "DOTMU",   "v1.3204",  0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Packard Bell", "DOTMA",   "v1.3201",  0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Packard Bell", "DOTMA",   "v1.3302",  0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Packard Bell", "DOTMA",   "v1.3303t", 0x55, 0x58, {0x9e, 0x00}, 0},
+	{"Packard Bell", "DOTVR46", "v1.3308",  0x55, 0x58, {0x9e, 0x00}, 0},
 	/* pewpew-terminator */
-	{"", "", "", 0, 0, {0, 0} }
+	{"", "", "", 0, 0, {0, 0}, 0}
 };
 
-static const struct bios_settings_t *bios_cfg __read_mostly;
+static const struct bios_settings *bios_cfg __read_mostly;
+
+/*
+ * this struct is used to instruct thermal layer to use bang_bang instead of
+ * default governor for acerhdf
+ */
+static struct thermal_zone_params acerhdf_zone_params = {
+	.governor_name = "bang_bang",
+};
 
 static int acerhdf_get_temp(int *temp)
 {
@@ -275,6 +311,12 @@ static void acerhdf_change_fanstate(int state)
 	fanstate = state;
 
 	ec_write(bios_cfg->fanreg, cmd);
+
+	if (bios_cfg->mcmd_enable && state == ACERHDF_FAN_OFF) {
+		if (verbose)
+			pr_notice("turning off fan manually\n");
+		ec_write(mcmd.mreg, mcmd.moff);
+	}
 }
 
 static void acerhdf_check_param(struct thermal_zone_device *thermal)
@@ -401,6 +443,21 @@ static int acerhdf_get_trip_type(struct thermal_zone_device *thermal, int trip,
 {
 	if (trip == 0)
 		*type = THERMAL_TRIP_ACTIVE;
+	else if (trip == 1)
+		*type = THERMAL_TRIP_CRITICAL;
+	else
+		return -EINVAL;
+
+	return 0;
+}
+
+static int acerhdf_get_trip_hyst(struct thermal_zone_device *thermal, int trip,
+				 unsigned long *temp)
+{
+	if (trip != 0)
+		return -EINVAL;
+
+	*temp = fanon - fanoff;
 
 	return 0;
 }
@@ -410,6 +467,10 @@ static int acerhdf_get_trip_temp(struct thermal_zone_device *thermal, int trip,
 {
 	if (trip == 0)
 		*temp = fanon;
+	else if (trip == 1)
+		*temp = ACERHDF_TEMP_CRIT;
+	else
+		return -EINVAL;
 
 	return 0;
 }
@@ -429,6 +490,7 @@ static struct thermal_zone_device_ops acerhdf_dev_ops = {
 	.get_mode = acerhdf_get_mode,
 	.set_mode = acerhdf_set_mode,
 	.get_trip_type = acerhdf_get_trip_type,
+	.get_trip_hyst = acerhdf_get_trip_hyst,
 	.get_trip_temp = acerhdf_get_trip_temp,
 	.get_crit_temp = acerhdf_get_crit_temp,
 };
@@ -481,9 +543,7 @@ static int acerhdf_set_cur_state(struct thermal_cooling_device *cdev,
 	}
 
 	if (state == 0) {
-		/* turn fan off only if below fanoff temperature */
-		if ((cur_state == ACERHDF_FAN_AUTO) &&
-		    (cur_temp < fanoff))
+		if (cur_state == ACERHDF_FAN_AUTO)
 			acerhdf_change_fanstate(ACERHDF_FAN_OFF);
 	} else {
 		if (cur_state == ACERHDF_FAN_OFF)
@@ -533,7 +593,6 @@ static const struct dev_pm_ops acerhdf_pm_ops = {
 static struct platform_driver acerhdf_driver = {
 	.driver = {
 		.name  = "acerhdf",
-		.owner = THIS_MODULE,
 		.pm    = &acerhdf_pm_ops,
 	},
 	.probe = acerhdf_probe,
@@ -559,7 +618,7 @@ static int str_starts_with(const char *str, const char *start)
 static int acerhdf_check_hardware(void)
 {
 	char const *vendor, *version, *product;
-	const struct bios_settings_t *bt = NULL;
+	const struct bios_settings *bt = NULL;
 
 	/* get BIOS data */
 	vendor  = dmi_get_system_info(DMI_SYS_VENDOR);
@@ -661,12 +720,20 @@ static int acerhdf_register_thermal(void)
 	if (IS_ERR(cl_dev))
 		return -EINVAL;
 
-	thz_dev = thermal_zone_device_register("acerhdf", 1, 0, NULL,
-					      &acerhdf_dev_ops, NULL, 0,
+	thz_dev = thermal_zone_device_register("acerhdf", 2, 0, NULL,
+					      &acerhdf_dev_ops,
+					      &acerhdf_zone_params, 0,
 					      (kernelmode) ? interval*1000 : 0);
 	if (IS_ERR(thz_dev))
 		return -EINVAL;
 
+	if (strcmp(thz_dev->governor->name,
+				acerhdf_zone_params.governor_name)) {
+		pr_err("Didn't get thermal governor %s, perhaps not compiled into thermal subsystem.\n",
+				acerhdf_zone_params.governor_name);
+		return -EINVAL;
+	}
+
 	return 0;
 }
 
@@ -723,9 +790,15 @@ MODULE_ALIAS("dmi:*:*Acer*:pnAOA*:");
 MODULE_ALIAS("dmi:*:*Acer*:pnAO751h*:");
 MODULE_ALIAS("dmi:*:*Acer*:pnAspire*1410*:");
 MODULE_ALIAS("dmi:*:*Acer*:pnAspire*1810*:");
+MODULE_ALIAS("dmi:*:*Acer*:pnAspire*5755G:");
 MODULE_ALIAS("dmi:*:*Acer*:pnAspire*1825PTZ:");
+MODULE_ALIAS("dmi:*:*Acer*:pnAO521*:");
 MODULE_ALIAS("dmi:*:*Acer*:pnAO531*:");
+MODULE_ALIAS("dmi:*:*Acer*:pnAspire*5739G:");
+MODULE_ALIAS("dmi:*:*Acer*:pnAspire*One*753:");
+MODULE_ALIAS("dmi:*:*Acer*:pnAspire*5315:");
 MODULE_ALIAS("dmi:*:*Acer*:TravelMate*7730G:");
+MODULE_ALIAS("dmi:*:*Acer*:TM8573T:");
 MODULE_ALIAS("dmi:*:*Gateway*:pnAOA*:");
 MODULE_ALIAS("dmi:*:*Gateway*:pnLT31*:");
 MODULE_ALIAS("dmi:*:*Packard*Bell*:pnAOA*:");
@@ -734,6 +807,7 @@ MODULE_ALIAS("dmi:*:*Packard*Bell*:pnDOTMU*:");
 MODULE_ALIAS("dmi:*:*Packard*Bell*:pnENBFT*:");
 MODULE_ALIAS("dmi:*:*Packard*Bell*:pnDOTMA*:");
 MODULE_ALIAS("dmi:*:*Packard*Bell*:pnDOTVR46*:");
+MODULE_ALIAS("dmi:*:*Acer*:pnExtensa 5420*:");
 
 module_init(acerhdf_init);
 module_exit(acerhdf_exit);
diff --git a/drivers/platform/x86/alienware-wmi.c b/drivers/platform/x86/alienware-wmi.c
index c5af23b64438..1e1e59423889 100644
--- a/drivers/platform/x86/alienware-wmi.c
+++ b/drivers/platform/x86/alienware-wmi.c
@@ -157,7 +157,6 @@ static struct platform_zone *zone_data;
 static struct platform_driver platform_driver = {
 	.driver = {
 		   .name = "alienware-wmi",
-		   .owner = THIS_MODULE,
 		   }
 };
 
diff --git a/drivers/platform/x86/amilo-rfkill.c b/drivers/platform/x86/amilo-rfkill.c
index da36b5e824d4..0157625cb918 100644
--- a/drivers/platform/x86/amilo-rfkill.c
+++ b/drivers/platform/x86/amilo-rfkill.c
@@ -138,7 +138,6 @@ static int amilo_rfkill_remove(struct platform_device *device)
 static struct platform_driver amilo_rfkill_driver = {
 	.driver = {
 		.name	= KBUILD_MODNAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe	= amilo_rfkill_probe,
 	.remove	= amilo_rfkill_remove,
diff --git a/drivers/platform/x86/asus-laptop.c b/drivers/platform/x86/asus-laptop.c
index 7f4dc6f51f8a..f71700e0d132 100644
--- a/drivers/platform/x86/asus-laptop.c
+++ b/drivers/platform/x86/asus-laptop.c
@@ -843,8 +843,7 @@ static int asus_backlight_init(struct asus_laptop *asus)
 
 static void asus_backlight_exit(struct asus_laptop *asus)
 {
-	if (asus->backlight_device)
-		backlight_device_unregister(asus->backlight_device);
+	backlight_device_unregister(asus->backlight_device);
 	asus->backlight_device = NULL;
 }
 
@@ -1699,7 +1698,6 @@ static void asus_platform_exit(struct asus_laptop *asus)
 static struct platform_driver platform_driver = {
 	.driver = {
 		.name = ASUS_LAPTOP_FILE,
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/platform/x86/asus-nb-wmi.c b/drivers/platform/x86/asus-nb-wmi.c
index c1a6cd66af42..abdaed34c728 100644
--- a/drivers/platform/x86/asus-nb-wmi.c
+++ b/drivers/platform/x86/asus-nb-wmi.c
@@ -191,6 +191,15 @@ static const struct dmi_system_id asus_quirks[] = {
 	},
 	{
 		.callback = dmi_matched,
+		.ident = "ASUSTeK COMPUTER INC. X551CA",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "X551CA"),
+		},
+		.driver_data = &quirk_asus_wapf4,
+	},
+	{
+		.callback = dmi_matched,
 		.ident = "ASUSTeK COMPUTER INC. X55A",
 		.matches = {
 			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
diff --git a/drivers/platform/x86/asus-wmi.c b/drivers/platform/x86/asus-wmi.c
index 21fc932da3a1..7543a56e0f45 100644
--- a/drivers/platform/x86/asus-wmi.c
+++ b/drivers/platform/x86/asus-wmi.c
@@ -1308,8 +1308,7 @@ static int asus_wmi_backlight_init(struct asus_wmi *asus)
 
 static void asus_wmi_backlight_exit(struct asus_wmi *asus)
 {
-	if (asus->backlight_device)
-		backlight_device_unregister(asus->backlight_device);
+	backlight_device_unregister(asus->backlight_device);
 
 	asus->backlight_device = NULL;
 }
diff --git a/drivers/platform/x86/compal-laptop.c b/drivers/platform/x86/compal-laptop.c
index 26bfd7bb5c13..15c0fab2bfa1 100644
--- a/drivers/platform/x86/compal-laptop.c
+++ b/drivers/platform/x86/compal-laptop.c
@@ -710,7 +710,6 @@ static int compal_remove(struct platform_device *);
 static struct platform_driver compal_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 	},
 	.probe	= compal_probe,
 	.remove	= compal_remove,
diff --git a/drivers/platform/x86/dell-laptop.c b/drivers/platform/x86/dell-laptop.c
index 233d2ee598a6..9411eae39a4e 100644
--- a/drivers/platform/x86/dell-laptop.c
+++ b/drivers/platform/x86/dell-laptop.c
@@ -2,9 +2,11 @@
  *  Driver for Dell laptop extras
  *
  *  Copyright (c) Red Hat <mjg@redhat.com>
+ *  Copyright (c) 2014 Gabriele Mazzotta <gabriele.mzt@gmail.com>
+ *  Copyright (c) 2014 Pali Rohár <pali.rohar@gmail.com>
  *
- *  Based on documentation in the libsmbios package, Copyright (C) 2005 Dell
- *  Inc.
+ *  Based on documentation in the libsmbios package:
+ *  Copyright (C) 2005-2014 Dell 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
@@ -32,6 +34,13 @@
 #include "../../firmware/dcdbas.h"
 
 #define BRIGHTNESS_TOKEN 0x7d
+#define KBD_LED_OFF_TOKEN 0x01E1
+#define KBD_LED_ON_TOKEN 0x01E2
+#define KBD_LED_AUTO_TOKEN 0x01E3
+#define KBD_LED_AUTO_25_TOKEN 0x02EA
+#define KBD_LED_AUTO_50_TOKEN 0x02EB
+#define KBD_LED_AUTO_75_TOKEN 0x02EC
+#define KBD_LED_AUTO_100_TOKEN 0x02F6
 
 /* This structure will be modified by the firmware when we enter
  * system management mode, hence the volatiles */
@@ -62,6 +71,13 @@ struct calling_interface_structure {
 
 struct quirk_entry {
 	u8 touchpad_led;
+
+	int needs_kbd_timeouts;
+	/*
+	 * Ordered list of timeouts expressed in seconds.
+	 * The list must end with -1
+	 */
+	int kbd_timeouts[];
 };
 
 static struct quirk_entry *quirks;
@@ -76,6 +92,15 @@ static int __init dmi_matched(const struct dmi_system_id *dmi)
 	return 1;
 }
 
+/*
+ * These values come from Windows utility provided by Dell. If any other value
+ * is used then BIOS silently set timeout to 0 without any error message.
+ */
+static struct quirk_entry quirk_dell_xps13_9333 = {
+	.needs_kbd_timeouts = 1,
+	.kbd_timeouts = { 0, 5, 15, 60, 5 * 60, 15 * 60, -1 },
+};
+
 static int da_command_address;
 static int da_command_code;
 static int da_num_tokens;
@@ -84,7 +109,6 @@ static struct calling_interface_token *da_tokens;
 static struct platform_driver platform_driver = {
 	.driver = {
 		.name = "dell-laptop",
-		.owner = THIS_MODULE,
 	}
 };
 
@@ -268,6 +292,15 @@ static const struct dmi_system_id dell_quirks[] __initconst = {
 		},
 		.driver_data = &quirk_dell_vostro_v130,
 	},
+	{
+		.callback = dmi_matched,
+		.ident = "Dell XPS13 9333",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "XPS13 9333"),
+		},
+		.driver_data = &quirk_dell_xps13_9333,
+	},
 	{ }
 };
 
@@ -332,17 +365,29 @@ static void __init find_tokens(const struct dmi_header *dm, void *dummy)
 	}
 }
 
-static int find_token_location(int tokenid)
+static int find_token_id(int tokenid)
 {
 	int i;
+
 	for (i = 0; i < da_num_tokens; i++) {
 		if (da_tokens[i].tokenID == tokenid)
-			return da_tokens[i].location;
+			return i;
 	}
 
 	return -1;
 }
 
+static int find_token_location(int tokenid)
+{
+	int id;
+
+	id = find_token_id(tokenid);
+	if (id == -1)
+		return -1;
+
+	return da_tokens[id].location;
+}
+
 static struct calling_interface_buffer *
 dell_send_request(struct calling_interface_buffer *buffer, int class,
 		  int select)
@@ -363,6 +408,20 @@ dell_send_request(struct calling_interface_buffer *buffer, int class,
 	return buffer;
 }
 
+static inline int dell_smi_error(int value)
+{
+	switch (value) {
+	case 0: /* Completed successfully */
+		return 0;
+	case -1: /* Completed with error */
+		return -EIO;
+	case -2: /* Function not supported */
+		return -ENXIO;
+	default: /* Unknown error */
+		return -EINVAL;
+	}
+}
+
 /* Derived from information in DellWirelessCtl.cpp:
    Class 17, select 11 is radio control. It returns an array of 32-bit values.
 
@@ -564,7 +623,7 @@ static bool dell_laptop_i8042_filter(unsigned char data, unsigned char str,
 {
 	static bool extended;
 
-	if (str & 0x20)
+	if (str & I8042_STR_AUXDATA)
 		return false;
 
 	if (unlikely(data == 0xe0)) {
@@ -717,7 +776,7 @@ static int dell_send_intensity(struct backlight_device *bd)
 	else
 		dell_send_request(buffer, 1, 1);
 
-out:
+ out:
 	release_buffer();
 	return ret;
 }
@@ -741,7 +800,7 @@ static int dell_get_intensity(struct backlight_device *bd)
 
 	ret = buffer->output[1];
 
-out:
+ out:
 	release_buffer();
 	return ret;
 }
@@ -790,6 +849,984 @@ static void touchpad_led_exit(void)
 	led_classdev_unregister(&touchpad_led);
 }
 
+/*
+ * Derived from information in smbios-keyboard-ctl:
+ *
+ * cbClass 4
+ * cbSelect 11
+ * Keyboard illumination
+ * cbArg1 determines the function to be performed
+ *
+ * cbArg1 0x0 = Get Feature Information
+ *  cbRES1         Standard return codes (0, -1, -2)
+ *  cbRES2, word0  Bitmap of user-selectable modes
+ *     bit 0     Always off (All systems)
+ *     bit 1     Always on (Travis ATG, Siberia)
+ *     bit 2     Auto: ALS-based On; ALS-based Off (Travis ATG)
+ *     bit 3     Auto: ALS- and input-activity-based On; input-activity based Off
+ *     bit 4     Auto: Input-activity-based On; input-activity based Off
+ *     bit 5     Auto: Input-activity-based On (illumination level 25%); input-activity based Off
+ *     bit 6     Auto: Input-activity-based On (illumination level 50%); input-activity based Off
+ *     bit 7     Auto: Input-activity-based On (illumination level 75%); input-activity based Off
+ *     bit 8     Auto: Input-activity-based On (illumination level 100%); input-activity based Off
+ *     bits 9-15 Reserved for future use
+ *  cbRES2, byte2  Reserved for future use
+ *  cbRES2, byte3  Keyboard illumination type
+ *     0         Reserved
+ *     1         Tasklight
+ *     2         Backlight
+ *     3-255     Reserved for future use
+ *  cbRES3, byte0  Supported auto keyboard illumination trigger bitmap.
+ *     bit 0     Any keystroke
+ *     bit 1     Touchpad activity
+ *     bit 2     Pointing stick
+ *     bit 3     Any mouse
+ *     bits 4-7  Reserved for future use
+ *  cbRES3, byte1  Supported timeout unit bitmap
+ *     bit 0     Seconds
+ *     bit 1     Minutes
+ *     bit 2     Hours
+ *     bit 3     Days
+ *     bits 4-7  Reserved for future use
+ *  cbRES3, byte2  Number of keyboard light brightness levels
+ *  cbRES4, byte0  Maximum acceptable seconds value (0 if seconds not supported).
+ *  cbRES4, byte1  Maximum acceptable minutes value (0 if minutes not supported).
+ *  cbRES4, byte2  Maximum acceptable hours value (0 if hours not supported).
+ *  cbRES4, byte3  Maximum acceptable days value (0 if days not supported)
+ *
+ * cbArg1 0x1 = Get Current State
+ *  cbRES1         Standard return codes (0, -1, -2)
+ *  cbRES2, word0  Bitmap of current mode state
+ *     bit 0     Always off (All systems)
+ *     bit 1     Always on (Travis ATG, Siberia)
+ *     bit 2     Auto: ALS-based On; ALS-based Off (Travis ATG)
+ *     bit 3     Auto: ALS- and input-activity-based On; input-activity based Off
+ *     bit 4     Auto: Input-activity-based On; input-activity based Off
+ *     bit 5     Auto: Input-activity-based On (illumination level 25%); input-activity based Off
+ *     bit 6     Auto: Input-activity-based On (illumination level 50%); input-activity based Off
+ *     bit 7     Auto: Input-activity-based On (illumination level 75%); input-activity based Off
+ *     bit 8     Auto: Input-activity-based On (illumination level 100%); input-activity based Off
+ *     bits 9-15 Reserved for future use
+ *     Note: Only One bit can be set
+ *  cbRES2, byte2  Currently active auto keyboard illumination triggers.
+ *     bit 0     Any keystroke
+ *     bit 1     Touchpad activity
+ *     bit 2     Pointing stick
+ *     bit 3     Any mouse
+ *     bits 4-7  Reserved for future use
+ *  cbRES2, byte3  Current Timeout
+ *     bits 7:6  Timeout units indicator:
+ *     00b       Seconds
+ *     01b       Minutes
+ *     10b       Hours
+ *     11b       Days
+ *     bits 5:0  Timeout value (0-63) in sec/min/hr/day
+ *     NOTE: A value of 0 means always on (no timeout) if any bits of RES3 byte
+ *     are set upon return from the [Get feature information] call.
+ *  cbRES3, byte0  Current setting of ALS value that turns the light on or off.
+ *  cbRES3, byte1  Current ALS reading
+ *  cbRES3, byte2  Current keyboard light level.
+ *
+ * cbArg1 0x2 = Set New State
+ *  cbRES1         Standard return codes (0, -1, -2)
+ *  cbArg2, word0  Bitmap of current mode state
+ *     bit 0     Always off (All systems)
+ *     bit 1     Always on (Travis ATG, Siberia)
+ *     bit 2     Auto: ALS-based On; ALS-based Off (Travis ATG)
+ *     bit 3     Auto: ALS- and input-activity-based On; input-activity based Off
+ *     bit 4     Auto: Input-activity-based On; input-activity based Off
+ *     bit 5     Auto: Input-activity-based On (illumination level 25%); input-activity based Off
+ *     bit 6     Auto: Input-activity-based On (illumination level 50%); input-activity based Off
+ *     bit 7     Auto: Input-activity-based On (illumination level 75%); input-activity based Off
+ *     bit 8     Auto: Input-activity-based On (illumination level 100%); input-activity based Off
+ *     bits 9-15 Reserved for future use
+ *     Note: Only One bit can be set
+ *  cbArg2, byte2  Desired auto keyboard illumination triggers. Must remain inactive to allow
+ *                 keyboard to turn off automatically.
+ *     bit 0     Any keystroke
+ *     bit 1     Touchpad activity
+ *     bit 2     Pointing stick
+ *     bit 3     Any mouse
+ *     bits 4-7  Reserved for future use
+ *  cbArg2, byte3  Desired Timeout
+ *     bits 7:6  Timeout units indicator:
+ *     00b       Seconds
+ *     01b       Minutes
+ *     10b       Hours
+ *     11b       Days
+ *     bits 5:0  Timeout value (0-63) in sec/min/hr/day
+ *  cbArg3, byte0  Desired setting of ALS value that turns the light on or off.
+ *  cbArg3, byte2  Desired keyboard light level.
+ */
+
+
+enum kbd_timeout_unit {
+	KBD_TIMEOUT_SECONDS = 0,
+	KBD_TIMEOUT_MINUTES,
+	KBD_TIMEOUT_HOURS,
+	KBD_TIMEOUT_DAYS,
+};
+
+enum kbd_mode_bit {
+	KBD_MODE_BIT_OFF = 0,
+	KBD_MODE_BIT_ON,
+	KBD_MODE_BIT_ALS,
+	KBD_MODE_BIT_TRIGGER_ALS,
+	KBD_MODE_BIT_TRIGGER,
+	KBD_MODE_BIT_TRIGGER_25,
+	KBD_MODE_BIT_TRIGGER_50,
+	KBD_MODE_BIT_TRIGGER_75,
+	KBD_MODE_BIT_TRIGGER_100,
+};
+
+#define kbd_is_als_mode_bit(bit) \
+	((bit) == KBD_MODE_BIT_ALS || (bit) == KBD_MODE_BIT_TRIGGER_ALS)
+#define kbd_is_trigger_mode_bit(bit) \
+	((bit) >= KBD_MODE_BIT_TRIGGER_ALS && (bit) <= KBD_MODE_BIT_TRIGGER_100)
+#define kbd_is_level_mode_bit(bit) \
+	((bit) >= KBD_MODE_BIT_TRIGGER_25 && (bit) <= KBD_MODE_BIT_TRIGGER_100)
+
+struct kbd_info {
+	u16 modes;
+	u8 type;
+	u8 triggers;
+	u8 levels;
+	u8 seconds;
+	u8 minutes;
+	u8 hours;
+	u8 days;
+};
+
+struct kbd_state {
+	u8 mode_bit;
+	u8 triggers;
+	u8 timeout_value;
+	u8 timeout_unit;
+	u8 als_setting;
+	u8 als_value;
+	u8 level;
+};
+
+static const int kbd_tokens[] = {
+	KBD_LED_OFF_TOKEN,
+	KBD_LED_AUTO_25_TOKEN,
+	KBD_LED_AUTO_50_TOKEN,
+	KBD_LED_AUTO_75_TOKEN,
+	KBD_LED_AUTO_100_TOKEN,
+	KBD_LED_ON_TOKEN,
+};
+
+static u16 kbd_token_bits;
+
+static struct kbd_info kbd_info;
+static bool kbd_als_supported;
+static bool kbd_triggers_supported;
+
+static u8 kbd_mode_levels[16];
+static int kbd_mode_levels_count;
+
+static u8 kbd_previous_level;
+static u8 kbd_previous_mode_bit;
+
+static bool kbd_led_present;
+
+/*
+ * NOTE: there are three ways to set the keyboard backlight level.
+ * First, via kbd_state.mode_bit (assigning KBD_MODE_BIT_TRIGGER_* value).
+ * Second, via kbd_state.level (assigning numerical value <= kbd_info.levels).
+ * Third, via SMBIOS tokens (KBD_LED_* in kbd_tokens)
+ *
+ * There are laptops which support only one of these methods. If we want to
+ * support as many machines as possible we need to implement all three methods.
+ * The first two methods use the kbd_state structure. The third uses SMBIOS
+ * tokens. If kbd_info.levels == 0, the machine does not support setting the
+ * keyboard backlight level via kbd_state.level.
+ */
+
+static int kbd_get_info(struct kbd_info *info)
+{
+	u8 units;
+	int ret;
+
+	get_buffer();
+
+	buffer->input[0] = 0x0;
+	dell_send_request(buffer, 4, 11);
+	ret = buffer->output[0];
+
+	if (ret) {
+		ret = dell_smi_error(ret);
+		goto out;
+	}
+
+	info->modes = buffer->output[1] & 0xFFFF;
+	info->type = (buffer->output[1] >> 24) & 0xFF;
+	info->triggers = buffer->output[2] & 0xFF;
+	units = (buffer->output[2] >> 8) & 0xFF;
+	info->levels = (buffer->output[2] >> 16) & 0xFF;
+
+	if (units & BIT(0))
+		info->seconds = (buffer->output[3] >> 0) & 0xFF;
+	if (units & BIT(1))
+		info->minutes = (buffer->output[3] >> 8) & 0xFF;
+	if (units & BIT(2))
+		info->hours = (buffer->output[3] >> 16) & 0xFF;
+	if (units & BIT(3))
+		info->days = (buffer->output[3] >> 24) & 0xFF;
+
+ out:
+	release_buffer();
+	return ret;
+}
+
+static unsigned int kbd_get_max_level(void)
+{
+	if (kbd_info.levels != 0)
+		return kbd_info.levels;
+	if (kbd_mode_levels_count > 0)
+		return kbd_mode_levels_count - 1;
+	return 0;
+}
+
+static int kbd_get_level(struct kbd_state *state)
+{
+	int i;
+
+	if (kbd_info.levels != 0)
+		return state->level;
+
+	if (kbd_mode_levels_count > 0) {
+		for (i = 0; i < kbd_mode_levels_count; ++i)
+			if (kbd_mode_levels[i] == state->mode_bit)
+				return i;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int kbd_set_level(struct kbd_state *state, u8 level)
+{
+	if (kbd_info.levels != 0) {
+		if (level != 0)
+			kbd_previous_level = level;
+		if (state->level == level)
+			return 0;
+		state->level = level;
+		if (level != 0 && state->mode_bit == KBD_MODE_BIT_OFF)
+			state->mode_bit = kbd_previous_mode_bit;
+		else if (level == 0 && state->mode_bit != KBD_MODE_BIT_OFF) {
+			kbd_previous_mode_bit = state->mode_bit;
+			state->mode_bit = KBD_MODE_BIT_OFF;
+		}
+		return 0;
+	}
+
+	if (kbd_mode_levels_count > 0 && level < kbd_mode_levels_count) {
+		if (level != 0)
+			kbd_previous_level = level;
+		state->mode_bit = kbd_mode_levels[level];
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int kbd_get_state(struct kbd_state *state)
+{
+	int ret;
+
+	get_buffer();
+
+	buffer->input[0] = 0x1;
+	dell_send_request(buffer, 4, 11);
+	ret = buffer->output[0];
+
+	if (ret) {
+		ret = dell_smi_error(ret);
+		goto out;
+	}
+
+	state->mode_bit = ffs(buffer->output[1] & 0xFFFF);
+	if (state->mode_bit != 0)
+		state->mode_bit--;
+
+	state->triggers = (buffer->output[1] >> 16) & 0xFF;
+	state->timeout_value = (buffer->output[1] >> 24) & 0x3F;
+	state->timeout_unit = (buffer->output[1] >> 30) & 0x3;
+	state->als_setting = buffer->output[2] & 0xFF;
+	state->als_value = (buffer->output[2] >> 8) & 0xFF;
+	state->level = (buffer->output[2] >> 16) & 0xFF;
+
+ out:
+	release_buffer();
+	return ret;
+}
+
+static int kbd_set_state(struct kbd_state *state)
+{
+	int ret;
+
+	get_buffer();
+	buffer->input[0] = 0x2;
+	buffer->input[1] = BIT(state->mode_bit) & 0xFFFF;
+	buffer->input[1] |= (state->triggers & 0xFF) << 16;
+	buffer->input[1] |= (state->timeout_value & 0x3F) << 24;
+	buffer->input[1] |= (state->timeout_unit & 0x3) << 30;
+	buffer->input[2] = state->als_setting & 0xFF;
+	buffer->input[2] |= (state->level & 0xFF) << 16;
+	dell_send_request(buffer, 4, 11);
+	ret = buffer->output[0];
+	release_buffer();
+
+	return dell_smi_error(ret);
+}
+
+static int kbd_set_state_safe(struct kbd_state *state, struct kbd_state *old)
+{
+	int ret;
+
+	ret = kbd_set_state(state);
+	if (ret == 0)
+		return 0;
+
+	/*
+	 * When setting the new state fails,try to restore the previous one.
+	 * This is needed on some machines where BIOS sets a default state when
+	 * setting a new state fails. This default state could be all off.
+	 */
+
+	if (kbd_set_state(old))
+		pr_err("Setting old previous keyboard state failed\n");
+
+	return ret;
+}
+
+static int kbd_set_token_bit(u8 bit)
+{
+	int id;
+	int ret;
+
+	if (bit >= ARRAY_SIZE(kbd_tokens))
+		return -EINVAL;
+
+	id = find_token_id(kbd_tokens[bit]);
+	if (id == -1)
+		return -EINVAL;
+
+	get_buffer();
+	buffer->input[0] = da_tokens[id].location;
+	buffer->input[1] = da_tokens[id].value;
+	dell_send_request(buffer, 1, 0);
+	ret = buffer->output[0];
+	release_buffer();
+
+	return dell_smi_error(ret);
+}
+
+static int kbd_get_token_bit(u8 bit)
+{
+	int id;
+	int ret;
+	int val;
+
+	if (bit >= ARRAY_SIZE(kbd_tokens))
+		return -EINVAL;
+
+	id = find_token_id(kbd_tokens[bit]);
+	if (id == -1)
+		return -EINVAL;
+
+	get_buffer();
+	buffer->input[0] = da_tokens[id].location;
+	dell_send_request(buffer, 0, 0);
+	ret = buffer->output[0];
+	val = buffer->output[1];
+	release_buffer();
+
+	if (ret)
+		return dell_smi_error(ret);
+
+	return (val == da_tokens[id].value);
+}
+
+static int kbd_get_first_active_token_bit(void)
+{
+	int i;
+	int ret;
+
+	for (i = 0; i < ARRAY_SIZE(kbd_tokens); ++i) {
+		ret = kbd_get_token_bit(i);
+		if (ret == 1)
+			return i;
+	}
+
+	return ret;
+}
+
+static int kbd_get_valid_token_counts(void)
+{
+	return hweight16(kbd_token_bits);
+}
+
+static inline int kbd_init_info(void)
+{
+	struct kbd_state state;
+	int ret;
+	int i;
+
+	ret = kbd_get_info(&kbd_info);
+	if (ret)
+		return ret;
+
+	kbd_get_state(&state);
+
+	/* NOTE: timeout value is stored in 6 bits so max value is 63 */
+	if (kbd_info.seconds > 63)
+		kbd_info.seconds = 63;
+	if (kbd_info.minutes > 63)
+		kbd_info.minutes = 63;
+	if (kbd_info.hours > 63)
+		kbd_info.hours = 63;
+	if (kbd_info.days > 63)
+		kbd_info.days = 63;
+
+	/* NOTE: On tested machines ON mode did not work and caused
+	 *       problems (turned backlight off) so do not use it
+	 */
+	kbd_info.modes &= ~BIT(KBD_MODE_BIT_ON);
+
+	kbd_previous_level = kbd_get_level(&state);
+	kbd_previous_mode_bit = state.mode_bit;
+
+	if (kbd_previous_level == 0 && kbd_get_max_level() != 0)
+		kbd_previous_level = 1;
+
+	if (kbd_previous_mode_bit == KBD_MODE_BIT_OFF) {
+		kbd_previous_mode_bit =
+			ffs(kbd_info.modes & ~BIT(KBD_MODE_BIT_OFF));
+		if (kbd_previous_mode_bit != 0)
+			kbd_previous_mode_bit--;
+	}
+
+	if (kbd_info.modes & (BIT(KBD_MODE_BIT_ALS) |
+			      BIT(KBD_MODE_BIT_TRIGGER_ALS)))
+		kbd_als_supported = true;
+
+	if (kbd_info.modes & (
+	    BIT(KBD_MODE_BIT_TRIGGER_ALS) | BIT(KBD_MODE_BIT_TRIGGER) |
+	    BIT(KBD_MODE_BIT_TRIGGER_25) | BIT(KBD_MODE_BIT_TRIGGER_50) |
+	    BIT(KBD_MODE_BIT_TRIGGER_75) | BIT(KBD_MODE_BIT_TRIGGER_100)
+	   ))
+		kbd_triggers_supported = true;
+
+	/* kbd_mode_levels[0] is reserved, see below */
+	for (i = 0; i < 16; ++i)
+		if (kbd_is_level_mode_bit(i) && (BIT(i) & kbd_info.modes))
+			kbd_mode_levels[1 + kbd_mode_levels_count++] = i;
+
+	/*
+	 * Find the first supported mode and assign to kbd_mode_levels[0].
+	 * This should be 0 (off), but we cannot depend on the BIOS to
+	 * support 0.
+	 */
+	if (kbd_mode_levels_count > 0) {
+		for (i = 0; i < 16; ++i) {
+			if (BIT(i) & kbd_info.modes) {
+				kbd_mode_levels[0] = i;
+				break;
+			}
+		}
+		kbd_mode_levels_count++;
+	}
+
+	return 0;
+
+}
+
+static inline void kbd_init_tokens(void)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(kbd_tokens); ++i)
+		if (find_token_id(kbd_tokens[i]) != -1)
+			kbd_token_bits |= BIT(i);
+}
+
+static void kbd_init(void)
+{
+	int ret;
+
+	ret = kbd_init_info();
+	kbd_init_tokens();
+
+	if (kbd_token_bits != 0 || ret == 0)
+		kbd_led_present = true;
+}
+
+static ssize_t kbd_led_timeout_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
+{
+	struct kbd_state new_state;
+	struct kbd_state state;
+	bool convert;
+	int value;
+	int ret;
+	char ch;
+	u8 unit;
+	int i;
+
+	ret = sscanf(buf, "%d %c", &value, &ch);
+	if (ret < 1)
+		return -EINVAL;
+	else if (ret == 1)
+		ch = 's';
+
+	if (value < 0)
+		return -EINVAL;
+
+	convert = false;
+
+	switch (ch) {
+	case 's':
+		if (value > kbd_info.seconds)
+			convert = true;
+		unit = KBD_TIMEOUT_SECONDS;
+		break;
+	case 'm':
+		if (value > kbd_info.minutes)
+			convert = true;
+		unit = KBD_TIMEOUT_MINUTES;
+		break;
+	case 'h':
+		if (value > kbd_info.hours)
+			convert = true;
+		unit = KBD_TIMEOUT_HOURS;
+		break;
+	case 'd':
+		if (value > kbd_info.days)
+			convert = true;
+		unit = KBD_TIMEOUT_DAYS;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (quirks && quirks->needs_kbd_timeouts)
+		convert = true;
+
+	if (convert) {
+		/* Convert value from current units to seconds */
+		switch (unit) {
+		case KBD_TIMEOUT_DAYS:
+			value *= 24;
+		case KBD_TIMEOUT_HOURS:
+			value *= 60;
+		case KBD_TIMEOUT_MINUTES:
+			value *= 60;
+			unit = KBD_TIMEOUT_SECONDS;
+		}
+
+		if (quirks && quirks->needs_kbd_timeouts) {
+			for (i = 0; quirks->kbd_timeouts[i] != -1; i++) {
+				if (value <= quirks->kbd_timeouts[i]) {
+					value = quirks->kbd_timeouts[i];
+					break;
+				}
+			}
+		}
+
+		if (value <= kbd_info.seconds && kbd_info.seconds) {
+			unit = KBD_TIMEOUT_SECONDS;
+		} else if (value / 60 <= kbd_info.minutes && kbd_info.minutes) {
+			value /= 60;
+			unit = KBD_TIMEOUT_MINUTES;
+		} else if (value / (60 * 60) <= kbd_info.hours && kbd_info.hours) {
+			value /= (60 * 60);
+			unit = KBD_TIMEOUT_HOURS;
+		} else if (value / (60 * 60 * 24) <= kbd_info.days && kbd_info.days) {
+			value /= (60 * 60 * 24);
+			unit = KBD_TIMEOUT_DAYS;
+		} else {
+			return -EINVAL;
+		}
+	}
+
+	ret = kbd_get_state(&state);
+	if (ret)
+		return ret;
+
+	new_state = state;
+	new_state.timeout_value = value;
+	new_state.timeout_unit = unit;
+
+	ret = kbd_set_state_safe(&new_state, &state);
+	if (ret)
+		return ret;
+
+	return count;
+}
+
+static ssize_t kbd_led_timeout_show(struct device *dev,
+				    struct device_attribute *attr, char *buf)
+{
+	struct kbd_state state;
+	int ret;
+	int len;
+
+	ret = kbd_get_state(&state);
+	if (ret)
+		return ret;
+
+	len = sprintf(buf, "%d", state.timeout_value);
+
+	switch (state.timeout_unit) {
+	case KBD_TIMEOUT_SECONDS:
+		return len + sprintf(buf+len, "s\n");
+	case KBD_TIMEOUT_MINUTES:
+		return len + sprintf(buf+len, "m\n");
+	case KBD_TIMEOUT_HOURS:
+		return len + sprintf(buf+len, "h\n");
+	case KBD_TIMEOUT_DAYS:
+		return len + sprintf(buf+len, "d\n");
+	default:
+		return -EINVAL;
+	}
+
+	return len;
+}
+
+static DEVICE_ATTR(stop_timeout, S_IRUGO | S_IWUSR,
+		   kbd_led_timeout_show, kbd_led_timeout_store);
+
+static const char * const kbd_led_triggers[] = {
+	"keyboard",
+	"touchpad",
+	/*"trackstick"*/ NULL, /* NOTE: trackstick is just alias for touchpad */
+	"mouse",
+};
+
+static ssize_t kbd_led_triggers_store(struct device *dev,
+				      struct device_attribute *attr,
+				      const char *buf, size_t count)
+{
+	struct kbd_state new_state;
+	struct kbd_state state;
+	bool triggers_enabled = false;
+	bool als_enabled = false;
+	bool disable_als = false;
+	bool enable_als = false;
+	int trigger_bit = -1;
+	char trigger[21];
+	int i, ret;
+
+	ret = sscanf(buf, "%20s", trigger);
+	if (ret != 1)
+		return -EINVAL;
+
+	if (trigger[0] != '+' && trigger[0] != '-')
+		return -EINVAL;
+
+	ret = kbd_get_state(&state);
+	if (ret)
+		return ret;
+
+	if (kbd_als_supported)
+		als_enabled = kbd_is_als_mode_bit(state.mode_bit);
+
+	if (kbd_triggers_supported)
+		triggers_enabled = kbd_is_trigger_mode_bit(state.mode_bit);
+
+	if (kbd_als_supported) {
+		if (strcmp(trigger, "+als") == 0) {
+			if (als_enabled)
+				return count;
+			enable_als = true;
+		} else if (strcmp(trigger, "-als") == 0) {
+			if (!als_enabled)
+				return count;
+			disable_als = true;
+		}
+	}
+
+	if (enable_als || disable_als) {
+		new_state = state;
+		if (enable_als) {
+			if (triggers_enabled)
+				new_state.mode_bit = KBD_MODE_BIT_TRIGGER_ALS;
+			else
+				new_state.mode_bit = KBD_MODE_BIT_ALS;
+		} else {
+			if (triggers_enabled) {
+				new_state.mode_bit = KBD_MODE_BIT_TRIGGER;
+				kbd_set_level(&new_state, kbd_previous_level);
+			} else {
+				new_state.mode_bit = KBD_MODE_BIT_ON;
+			}
+		}
+		if (!(kbd_info.modes & BIT(new_state.mode_bit)))
+			return -EINVAL;
+		ret = kbd_set_state_safe(&new_state, &state);
+		if (ret)
+			return ret;
+		kbd_previous_mode_bit = new_state.mode_bit;
+		return count;
+	}
+
+	if (kbd_triggers_supported) {
+		for (i = 0; i < ARRAY_SIZE(kbd_led_triggers); ++i) {
+			if (!(kbd_info.triggers & BIT(i)))
+				continue;
+			if (!kbd_led_triggers[i])
+				continue;
+			if (strcmp(trigger+1, kbd_led_triggers[i]) != 0)
+				continue;
+			if (trigger[0] == '+' &&
+			    triggers_enabled && (state.triggers & BIT(i)))
+				return count;
+			if (trigger[0] == '-' &&
+			    (!triggers_enabled || !(state.triggers & BIT(i))))
+				return count;
+			trigger_bit = i;
+			break;
+		}
+	}
+
+	if (trigger_bit != -1) {
+		new_state = state;
+		if (trigger[0] == '+')
+			new_state.triggers |= BIT(trigger_bit);
+		else {
+			new_state.triggers &= ~BIT(trigger_bit);
+			/* NOTE: trackstick bit (2) must be disabled when
+			 *       disabling touchpad bit (1), otherwise touchpad
+			 *       bit (1) will not be disabled */
+			if (trigger_bit == 1)
+				new_state.triggers &= ~BIT(2);
+		}
+		if ((kbd_info.triggers & new_state.triggers) !=
+		    new_state.triggers)
+			return -EINVAL;
+		if (new_state.triggers && !triggers_enabled) {
+			if (als_enabled)
+				new_state.mode_bit = KBD_MODE_BIT_TRIGGER_ALS;
+			else {
+				new_state.mode_bit = KBD_MODE_BIT_TRIGGER;
+				kbd_set_level(&new_state, kbd_previous_level);
+			}
+		} else if (new_state.triggers == 0) {
+			if (als_enabled)
+				new_state.mode_bit = KBD_MODE_BIT_ALS;
+			else
+				kbd_set_level(&new_state, 0);
+		}
+		if (!(kbd_info.modes & BIT(new_state.mode_bit)))
+			return -EINVAL;
+		ret = kbd_set_state_safe(&new_state, &state);
+		if (ret)
+			return ret;
+		if (new_state.mode_bit != KBD_MODE_BIT_OFF)
+			kbd_previous_mode_bit = new_state.mode_bit;
+		return count;
+	}
+
+	return -EINVAL;
+}
+
+static ssize_t kbd_led_triggers_show(struct device *dev,
+				     struct device_attribute *attr, char *buf)
+{
+	struct kbd_state state;
+	bool triggers_enabled;
+	int level, i, ret;
+	int len = 0;
+
+	ret = kbd_get_state(&state);
+	if (ret)
+		return ret;
+
+	len = 0;
+
+	if (kbd_triggers_supported) {
+		triggers_enabled = kbd_is_trigger_mode_bit(state.mode_bit);
+		level = kbd_get_level(&state);
+		for (i = 0; i < ARRAY_SIZE(kbd_led_triggers); ++i) {
+			if (!(kbd_info.triggers & BIT(i)))
+				continue;
+			if (!kbd_led_triggers[i])
+				continue;
+			if ((triggers_enabled || level <= 0) &&
+			    (state.triggers & BIT(i)))
+				buf[len++] = '+';
+			else
+				buf[len++] = '-';
+			len += sprintf(buf+len, "%s ", kbd_led_triggers[i]);
+		}
+	}
+
+	if (kbd_als_supported) {
+		if (kbd_is_als_mode_bit(state.mode_bit))
+			len += sprintf(buf+len, "+als ");
+		else
+			len += sprintf(buf+len, "-als ");
+	}
+
+	if (len)
+		buf[len - 1] = '\n';
+
+	return len;
+}
+
+static DEVICE_ATTR(start_triggers, S_IRUGO | S_IWUSR,
+		   kbd_led_triggers_show, kbd_led_triggers_store);
+
+static ssize_t kbd_led_als_store(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf, size_t count)
+{
+	struct kbd_state state;
+	struct kbd_state new_state;
+	u8 setting;
+	int ret;
+
+	ret = kstrtou8(buf, 10, &setting);
+	if (ret)
+		return ret;
+
+	ret = kbd_get_state(&state);
+	if (ret)
+		return ret;
+
+	new_state = state;
+	new_state.als_setting = setting;
+
+	ret = kbd_set_state_safe(&new_state, &state);
+	if (ret)
+		return ret;
+
+	return count;
+}
+
+static ssize_t kbd_led_als_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct kbd_state state;
+	int ret;
+
+	ret = kbd_get_state(&state);
+	if (ret)
+		return ret;
+
+	return sprintf(buf, "%d\n", state.als_setting);
+}
+
+static DEVICE_ATTR(als_setting, S_IRUGO | S_IWUSR,
+		   kbd_led_als_show, kbd_led_als_store);
+
+static struct attribute *kbd_led_attrs[] = {
+	&dev_attr_stop_timeout.attr,
+	&dev_attr_start_triggers.attr,
+	&dev_attr_als_setting.attr,
+	NULL,
+};
+ATTRIBUTE_GROUPS(kbd_led);
+
+static enum led_brightness kbd_led_level_get(struct led_classdev *led_cdev)
+{
+	int ret;
+	u16 num;
+	struct kbd_state state;
+
+	if (kbd_get_max_level()) {
+		ret = kbd_get_state(&state);
+		if (ret)
+			return 0;
+		ret = kbd_get_level(&state);
+		if (ret < 0)
+			return 0;
+		return ret;
+	}
+
+	if (kbd_get_valid_token_counts()) {
+		ret = kbd_get_first_active_token_bit();
+		if (ret < 0)
+			return 0;
+		for (num = kbd_token_bits; num != 0 && ret > 0; --ret)
+			num &= num - 1; /* clear the first bit set */
+		if (num == 0)
+			return 0;
+		return ffs(num) - 1;
+	}
+
+	pr_warn("Keyboard brightness level control not supported\n");
+	return 0;
+}
+
+static void kbd_led_level_set(struct led_classdev *led_cdev,
+			      enum led_brightness value)
+{
+	struct kbd_state state;
+	struct kbd_state new_state;
+	u16 num;
+
+	if (kbd_get_max_level()) {
+		if (kbd_get_state(&state))
+			return;
+		new_state = state;
+		if (kbd_set_level(&new_state, value))
+			return;
+		kbd_set_state_safe(&new_state, &state);
+		return;
+	}
+
+	if (kbd_get_valid_token_counts()) {
+		for (num = kbd_token_bits; num != 0 && value > 0; --value)
+			num &= num - 1; /* clear the first bit set */
+		if (num == 0)
+			return;
+		kbd_set_token_bit(ffs(num) - 1);
+		return;
+	}
+
+	pr_warn("Keyboard brightness level control not supported\n");
+}
+
+static struct led_classdev kbd_led = {
+	.name           = "dell::kbd_backlight",
+	.brightness_set = kbd_led_level_set,
+	.brightness_get = kbd_led_level_get,
+	.groups         = kbd_led_groups,
+};
+
+static int __init kbd_led_init(struct device *dev)
+{
+	kbd_init();
+	if (!kbd_led_present)
+		return -ENODEV;
+	kbd_led.max_brightness = kbd_get_max_level();
+	if (!kbd_led.max_brightness) {
+		kbd_led.max_brightness = kbd_get_valid_token_counts();
+		if (kbd_led.max_brightness)
+			kbd_led.max_brightness--;
+	}
+	return led_classdev_register(dev, &kbd_led);
+}
+
+static void brightness_set_exit(struct led_classdev *led_cdev,
+				enum led_brightness value)
+{
+	/* Don't change backlight level on exit */
+};
+
+static void kbd_led_exit(void)
+{
+	if (!kbd_led_present)
+		return;
+	kbd_led.brightness_set = brightness_set_exit;
+	led_classdev_unregister(&kbd_led);
+}
+
 static int __init dell_init(void)
 {
 	int max_intensity = 0;
@@ -842,6 +1879,8 @@ static int __init dell_init(void)
 	if (quirks && quirks->touchpad_led)
 		touchpad_led_init(&platform_device->dev);
 
+	kbd_led_init(&platform_device->dev);
+
 	dell_laptop_dir = debugfs_create_dir("dell_laptop", NULL);
 	if (dell_laptop_dir != NULL)
 		debugfs_create_file("rfkill", 0444, dell_laptop_dir, NULL,
@@ -909,6 +1948,7 @@ static void __exit dell_exit(void)
 	debugfs_remove_recursive(dell_laptop_dir);
 	if (quirks && quirks->touchpad_led)
 		touchpad_led_exit();
+	kbd_led_exit();
 	i8042_remove_filter(dell_laptop_i8042_filter);
 	cancel_delayed_work_sync(&dell_rfkill_work);
 	backlight_device_unregister(dell_backlight_device);
@@ -925,5 +1965,7 @@ module_init(dell_init);
 module_exit(dell_exit);
 
 MODULE_AUTHOR("Matthew Garrett <mjg@redhat.com>");
+MODULE_AUTHOR("Gabriele Mazzotta <gabriele.mzt@gmail.com>");
+MODULE_AUTHOR("Pali Rohár <pali.rohar@gmail.com>");
 MODULE_DESCRIPTION("Dell laptop driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/platform/x86/dell-smo8800.c b/drivers/platform/x86/dell-smo8800.c
index a653716055d1..0aec4fd4c48e 100644
--- a/drivers/platform/x86/dell-smo8800.c
+++ b/drivers/platform/x86/dell-smo8800.c
@@ -1,5 +1,5 @@
 /*
- *  dell-smo8800.c - Dell Latitude ACPI SMO8800/SMO8810 freefall sensor driver
+ *  dell-smo8800.c - Dell Latitude ACPI SMO88XX freefall sensor driver
  *
  *  Copyright (C) 2012 Sonal Santan <sonal.santan@gmail.com>
  *  Copyright (C) 2014 Pali Rohár <pali.rohar@gmail.com>
@@ -209,7 +209,13 @@ static int smo8800_remove(struct acpi_device *device)
 
 static const struct acpi_device_id smo8800_ids[] = {
 	{ "SMO8800", 0 },
+	{ "SMO8801", 0 },
 	{ "SMO8810", 0 },
+	{ "SMO8811", 0 },
+	{ "SMO8820", 0 },
+	{ "SMO8821", 0 },
+	{ "SMO8830", 0 },
+	{ "SMO8831", 0 },
 	{ "", 0 },
 };
 
@@ -228,6 +234,6 @@ static struct acpi_driver smo8800_driver = {
 
 module_acpi_driver(smo8800_driver);
 
-MODULE_DESCRIPTION("Dell Latitude freefall driver (ACPI SMO8800/SMO8810)");
+MODULE_DESCRIPTION("Dell Latitude freefall driver (ACPI SMO88XX)");
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Sonal Santan, Pali Rohár");
diff --git a/drivers/platform/x86/dell-wmi.c b/drivers/platform/x86/dell-wmi.c
index 25721bf20092..6512a06bc053 100644
--- a/drivers/platform/x86/dell-wmi.c
+++ b/drivers/platform/x86/dell-wmi.c
@@ -65,10 +65,8 @@ static const struct key_entry dell_wmi_legacy_keymap[] __initconst = {
 	/* Battery health status button */
 	{ KE_KEY, 0xe007, { KEY_BATTERY } },
 
-	/* This is actually for all radios. Although physically a
-	 * switch, the notification does not provide an indication of
-	 * state and so it should be reported as a key */
-	{ KE_KEY, 0xe008, { KEY_WLAN } },
+	/* Radio devices state change */
+	{ KE_IGNORE, 0xe008, { KEY_RFKILL } },
 
 	/* The next device is at offset 6, the active devices are at
 	   offset 8 and the attached devices at offset 10 */
@@ -145,57 +143,154 @@ static const u16 bios_to_linux_keycode[256] __initconst = {
 
 static struct input_dev *dell_wmi_input_dev;
 
+static void dell_wmi_process_key(int reported_key)
+{
+	const struct key_entry *key;
+
+	key = sparse_keymap_entry_from_scancode(dell_wmi_input_dev,
+						reported_key);
+	if (!key) {
+		pr_info("Unknown key %x pressed\n", reported_key);
+		return;
+	}
+
+	pr_debug("Key %x pressed\n", reported_key);
+
+	/* Don't report brightness notifications that will also come via ACPI */
+	if ((key->keycode == KEY_BRIGHTNESSUP ||
+	     key->keycode == KEY_BRIGHTNESSDOWN) && acpi_video)
+		return;
+
+	sparse_keymap_report_entry(dell_wmi_input_dev, key, 1, true);
+}
+
 static void dell_wmi_notify(u32 value, void *context)
 {
 	struct acpi_buffer response = { ACPI_ALLOCATE_BUFFER, NULL };
 	union acpi_object *obj;
 	acpi_status status;
+	acpi_size buffer_size;
+	u16 *buffer_entry, *buffer_end;
+	int len, i;
 
 	status = wmi_get_event_data(value, &response);
 	if (status != AE_OK) {
-		pr_info("bad event status 0x%x\n", status);
+		pr_warn("bad event status 0x%x\n", status);
 		return;
 	}
 
 	obj = (union acpi_object *)response.pointer;
+	if (!obj) {
+		pr_warn("no response\n");
+		return;
+	}
 
-	if (obj && obj->type == ACPI_TYPE_BUFFER) {
-		const struct key_entry *key;
-		int reported_key;
-		u16 *buffer_entry = (u16 *)obj->buffer.pointer;
-		int buffer_size = obj->buffer.length/2;
-
-		if (buffer_size >= 2 && dell_new_hk_type && buffer_entry[1] != 0x10) {
-			pr_info("Received unknown WMI event (0x%x)\n",
-				buffer_entry[1]);
-			kfree(obj);
-			return;
-		}
+	if (obj->type != ACPI_TYPE_BUFFER) {
+		pr_warn("bad response type %x\n", obj->type);
+		kfree(obj);
+		return;
+	}
 
-		if (buffer_size >= 3 && (dell_new_hk_type || buffer_entry[1] == 0x0))
-			reported_key = (int)buffer_entry[2];
+	pr_debug("Received WMI event (%*ph)\n",
+		obj->buffer.length, obj->buffer.pointer);
+
+	buffer_entry = (u16 *)obj->buffer.pointer;
+	buffer_size = obj->buffer.length/2;
+
+	if (!dell_new_hk_type) {
+		if (buffer_size >= 3 && buffer_entry[1] == 0x0)
+			dell_wmi_process_key(buffer_entry[2]);
 		else if (buffer_size >= 2)
-			reported_key = (int)buffer_entry[1] & 0xffff;
-		else {
+			dell_wmi_process_key(buffer_entry[1]);
+		else
 			pr_info("Received unknown WMI event\n");
-			kfree(obj);
-			return;
+		kfree(obj);
+		return;
+	}
+
+	buffer_end = buffer_entry + buffer_size;
+
+	while (buffer_entry < buffer_end) {
+
+		len = buffer_entry[0];
+		if (len == 0)
+			break;
+
+		len++;
+
+		if (buffer_entry + len > buffer_end) {
+			pr_warn("Invalid length of WMI event\n");
+			break;
 		}
 
-		key = sparse_keymap_entry_from_scancode(dell_wmi_input_dev,
-							reported_key);
-		if (!key) {
-			pr_info("Unknown key %x pressed\n", reported_key);
-		} else if ((key->keycode == KEY_BRIGHTNESSUP ||
-			    key->keycode == KEY_BRIGHTNESSDOWN) && acpi_video) {
-			/* Don't report brightness notifications that will also
-			 * come via ACPI */
-			;
-		} else {
-			sparse_keymap_report_entry(dell_wmi_input_dev, key,
-						   1, true);
+		pr_debug("Process buffer (%*ph)\n", len*2, buffer_entry);
+
+		switch (buffer_entry[1]) {
+		case 0x00:
+			for (i = 2; i < len; ++i) {
+				switch (buffer_entry[i]) {
+				case 0xe043:
+					/* NIC Link is Up */
+					pr_debug("NIC Link is Up\n");
+					break;
+				case 0xe044:
+					/* NIC Link is Down */
+					pr_debug("NIC Link is Down\n");
+					break;
+				case 0xe045:
+					/* Unknown event but defined in DSDT */
+				default:
+					/* Unknown event */
+					pr_info("Unknown WMI event type 0x00: "
+						"0x%x\n", (int)buffer_entry[i]);
+					break;
+				}
+			}
+			break;
+		case 0x10:
+			/* Keys pressed */
+			for (i = 2; i < len; ++i)
+				dell_wmi_process_key(buffer_entry[i]);
+			break;
+		case 0x11:
+			for (i = 2; i < len; ++i) {
+				switch (buffer_entry[i]) {
+				case 0xfff0:
+					/* Battery unplugged */
+					pr_debug("Battery unplugged\n");
+					break;
+				case 0xfff1:
+					/* Battery inserted */
+					pr_debug("Battery inserted\n");
+					break;
+				case 0x01e1:
+				case 0x02ea:
+				case 0x02eb:
+				case 0x02ec:
+				case 0x02f6:
+					/* Keyboard backlight level changed */
+					pr_debug("Keyboard backlight level "
+						 "changed\n");
+					break;
+				default:
+					/* Unknown event */
+					pr_info("Unknown WMI event type 0x11: "
+						"0x%x\n", (int)buffer_entry[i]);
+					break;
+				}
+			}
+			break;
+		default:
+			/* Unknown event */
+			pr_info("Unknown WMI event type 0x%x\n",
+				(int)buffer_entry[1]);
+			break;
 		}
+
+		buffer_entry += len;
+
 	}
+
 	kfree(obj);
 }
 
@@ -213,11 +308,16 @@ static const struct key_entry * __init dell_wmi_prepare_new_keymap(void)
 	for (i = 0; i < hotkey_num; i++) {
 		const struct dell_bios_keymap_entry *bios_entry =
 					&dell_bios_hotkey_table->keymap[i];
-		keymap[i].type = KE_KEY;
-		keymap[i].code = bios_entry->scancode;
-		keymap[i].keycode = bios_entry->keycode < 256 ?
+		u16 keycode = bios_entry->keycode < 256 ?
 				    bios_to_linux_keycode[bios_entry->keycode] :
 				    KEY_RESERVED;
+
+		if (keycode == KEY_KBDILLUMTOGGLE)
+			keymap[i].type = KE_IGNORE;
+		else
+			keymap[i].type = KE_KEY;
+		keymap[i].code = bios_entry->scancode;
+		keymap[i].keycode = keycode;
 	}
 
 	keymap[hotkey_num].type = KE_END;
diff --git a/drivers/platform/x86/eeepc-laptop.c b/drivers/platform/x86/eeepc-laptop.c
index db79902c4a8e..844c2096bde9 100644
--- a/drivers/platform/x86/eeepc-laptop.c
+++ b/drivers/platform/x86/eeepc-laptop.c
@@ -417,8 +417,7 @@ static ssize_t cpufv_disabled_store(struct device *dev,
 	switch (value) {
 	case 0:
 		if (eeepc->cpufv_disabled)
-			pr_warn("cpufv enabled (not officially supported "
-				"on this model)\n");
+			pr_warn("cpufv enabled (not officially supported on this model)\n");
 		eeepc->cpufv_disabled = false;
 		return count;
 	case 1:
@@ -580,59 +579,58 @@ static void eeepc_rfkill_hotplug(struct eeepc_laptop *eeepc, acpi_handle handle)
 	mutex_lock(&eeepc->hotplug_lock);
 	pci_lock_rescan_remove();
 
-	if (eeepc->hotplug_slot) {
-		port = acpi_get_pci_dev(handle);
-		if (!port) {
-			pr_warning("Unable to find port\n");
-			goto out_unlock;
-		}
+	if (!eeepc->hotplug_slot)
+		goto out_unlock;
 
-		bus = port->subordinate;
+	port = acpi_get_pci_dev(handle);
+	if (!port) {
+		pr_warning("Unable to find port\n");
+		goto out_unlock;
+	}
 
-		if (!bus) {
-			pr_warn("Unable to find PCI bus 1?\n");
-			goto out_put_dev;
-		}
+	bus = port->subordinate;
 
-		if (pci_bus_read_config_dword(bus, 0, PCI_VENDOR_ID, &l)) {
-			pr_err("Unable to read PCI config space?\n");
-			goto out_put_dev;
-		}
+	if (!bus) {
+		pr_warn("Unable to find PCI bus 1?\n");
+		goto out_put_dev;
+	}
+
+	if (pci_bus_read_config_dword(bus, 0, PCI_VENDOR_ID, &l)) {
+		pr_err("Unable to read PCI config space?\n");
+		goto out_put_dev;
+	}
 
-		absent = (l == 0xffffffff);
+	absent = (l == 0xffffffff);
 
-		if (blocked != absent) {
-			pr_warn("BIOS says wireless lan is %s, "
-				"but the pci device is %s\n",
-				blocked ? "blocked" : "unblocked",
-				absent ? "absent" : "present");
-			pr_warn("skipped wireless hotplug as probably "
-				"inappropriate for this model\n");
+	if (blocked != absent) {
+		pr_warn("BIOS says wireless lan is %s, but the pci device is %s\n",
+			blocked ? "blocked" : "unblocked",
+			absent ? "absent" : "present");
+		pr_warn("skipped wireless hotplug as probably inappropriate for this model\n");
+		goto out_put_dev;
+	}
+
+	if (!blocked) {
+		dev = pci_get_slot(bus, 0);
+		if (dev) {
+			/* Device already present */
+			pci_dev_put(dev);
 			goto out_put_dev;
 		}
-
-		if (!blocked) {
-			dev = pci_get_slot(bus, 0);
-			if (dev) {
-				/* Device already present */
-				pci_dev_put(dev);
-				goto out_put_dev;
-			}
-			dev = pci_scan_single_device(bus, 0);
-			if (dev) {
-				pci_bus_assign_resources(bus);
-				pci_bus_add_device(dev);
-			}
-		} else {
-			dev = pci_get_slot(bus, 0);
-			if (dev) {
-				pci_stop_and_remove_bus_device(dev);
-				pci_dev_put(dev);
-			}
+		dev = pci_scan_single_device(bus, 0);
+		if (dev) {
+			pci_bus_assign_resources(bus);
+			pci_bus_add_device(dev);
+		}
+	} else {
+		dev = pci_get_slot(bus, 0);
+		if (dev) {
+			pci_stop_and_remove_bus_device(dev);
+			pci_dev_put(dev);
 		}
-out_put_dev:
-		pci_dev_put(port);
 	}
+out_put_dev:
+	pci_dev_put(port);
 
 out_unlock:
 	pci_unlock_rescan_remove();
@@ -821,11 +819,15 @@ static int eeepc_new_rfkill(struct eeepc_laptop *eeepc,
 	return 0;
 }
 
+static char EEEPC_RFKILL_NODE_1[] = "\\_SB.PCI0.P0P5";
+static char EEEPC_RFKILL_NODE_2[] = "\\_SB.PCI0.P0P6";
+static char EEEPC_RFKILL_NODE_3[] = "\\_SB.PCI0.P0P7";
+
 static void eeepc_rfkill_exit(struct eeepc_laptop *eeepc)
 {
-	eeepc_unregister_rfkill_notifier(eeepc, "\\_SB.PCI0.P0P5");
-	eeepc_unregister_rfkill_notifier(eeepc, "\\_SB.PCI0.P0P6");
-	eeepc_unregister_rfkill_notifier(eeepc, "\\_SB.PCI0.P0P7");
+	eeepc_unregister_rfkill_notifier(eeepc, EEEPC_RFKILL_NODE_1);
+	eeepc_unregister_rfkill_notifier(eeepc, EEEPC_RFKILL_NODE_2);
+	eeepc_unregister_rfkill_notifier(eeepc, EEEPC_RFKILL_NODE_3);
 	if (eeepc->wlan_rfkill) {
 		rfkill_unregister(eeepc->wlan_rfkill);
 		rfkill_destroy(eeepc->wlan_rfkill);
@@ -897,9 +899,9 @@ static int eeepc_rfkill_init(struct eeepc_laptop *eeepc)
 	if (result == -EBUSY)
 		result = 0;
 
-	eeepc_register_rfkill_notifier(eeepc, "\\_SB.PCI0.P0P5");
-	eeepc_register_rfkill_notifier(eeepc, "\\_SB.PCI0.P0P6");
-	eeepc_register_rfkill_notifier(eeepc, "\\_SB.PCI0.P0P7");
+	eeepc_register_rfkill_notifier(eeepc, EEEPC_RFKILL_NODE_1);
+	eeepc_register_rfkill_notifier(eeepc, EEEPC_RFKILL_NODE_2);
+	eeepc_register_rfkill_notifier(eeepc, EEEPC_RFKILL_NODE_3);
 
 exit:
 	if (result && result != -ENODEV)
@@ -915,7 +917,7 @@ static int eeepc_hotk_thaw(struct device *device)
 	struct eeepc_laptop *eeepc = dev_get_drvdata(device);
 
 	if (eeepc->wlan_rfkill) {
-		bool wlan;
+		int wlan;
 
 		/*
 		 * Work around bios bug - acpi _PTS turns off the wireless led
@@ -923,7 +925,8 @@ static int eeepc_hotk_thaw(struct device *device)
 		 * we should kick it ourselves in case hibernation is aborted.
 		 */
 		wlan = get_acpi(eeepc, CM_ASL_WLAN);
-		set_acpi(eeepc, CM_ASL_WLAN, wlan);
+		if (wlan >= 0)
+			set_acpi(eeepc, CM_ASL_WLAN, wlan);
 	}
 
 	return 0;
@@ -935,9 +938,9 @@ static int eeepc_hotk_restore(struct device *device)
 
 	/* Refresh both wlan rfkill state and pci hotplug */
 	if (eeepc->wlan_rfkill) {
-		eeepc_rfkill_hotplug_update(eeepc, "\\_SB.PCI0.P0P5");
-		eeepc_rfkill_hotplug_update(eeepc, "\\_SB.PCI0.P0P6");
-		eeepc_rfkill_hotplug_update(eeepc, "\\_SB.PCI0.P0P7");
+		eeepc_rfkill_hotplug_update(eeepc, EEEPC_RFKILL_NODE_1);
+		eeepc_rfkill_hotplug_update(eeepc, EEEPC_RFKILL_NODE_2);
+		eeepc_rfkill_hotplug_update(eeepc, EEEPC_RFKILL_NODE_3);
 	}
 
 	if (eeepc->bluetooth_rfkill)
@@ -961,7 +964,6 @@ static const struct dev_pm_ops eeepc_pm_ops = {
 static struct platform_driver platform_driver = {
 	.driver = {
 		.name = EEEPC_LAPTOP_FILE,
-		.owner = THIS_MODULE,
 		.pm = &eeepc_pm_ops,
 	}
 };
@@ -978,18 +980,28 @@ static struct platform_driver platform_driver = {
 #define EEEPC_EC_SFB0      0xD0
 #define EEEPC_EC_FAN_CTRL  (EEEPC_EC_SFB0 + 3) /* Byte containing SF25  */
 
+static inline int eeepc_pwm_to_lmsensors(int value)
+{
+	return value * 255 / 100;
+}
+
+static inline int eeepc_lmsensors_to_pwm(int value)
+{
+	value = clamp_val(value, 0, 255);
+	return value * 100 / 255;
+}
+
 static int eeepc_get_fan_pwm(void)
 {
 	u8 value = 0;
 
 	ec_read(EEEPC_EC_FAN_PWM, &value);
-	return value * 255 / 100;
+	return eeepc_pwm_to_lmsensors(value);
 }
 
 static void eeepc_set_fan_pwm(int value)
 {
-	value = clamp_val(value, 0, 255);
-	value = value * 100 / 255;
+	value = eeepc_lmsensors_to_pwm(value);
 	ec_write(EEEPC_EC_FAN_PWM, value);
 }
 
@@ -1003,15 +1015,19 @@ static int eeepc_get_fan_rpm(void)
 	return high << 8 | low;
 }
 
+#define EEEPC_EC_FAN_CTRL_BIT	0x02
+#define EEEPC_FAN_CTRL_MANUAL	1
+#define EEEPC_FAN_CTRL_AUTO	2
+
 static int eeepc_get_fan_ctrl(void)
 {
 	u8 value = 0;
 
 	ec_read(EEEPC_EC_FAN_CTRL, &value);
-	if (value & 0x02)
-		return 1; /* manual */
+	if (value & EEEPC_EC_FAN_CTRL_BIT)
+		return EEEPC_FAN_CTRL_MANUAL;
 	else
-		return 2; /* automatic */
+		return EEEPC_FAN_CTRL_AUTO;
 }
 
 static void eeepc_set_fan_ctrl(int manual)
@@ -1019,10 +1035,10 @@ static void eeepc_set_fan_ctrl(int manual)
 	u8 value = 0;
 
 	ec_read(EEEPC_EC_FAN_CTRL, &value);
-	if (manual == 1)
-		value |= 0x02;
+	if (manual == EEEPC_FAN_CTRL_MANUAL)
+		value |= EEEPC_EC_FAN_CTRL_BIT;
 	else
-		value &= ~0x02;
+		value &= ~EEEPC_EC_FAN_CTRL_BIT;
 	ec_write(EEEPC_EC_FAN_CTRL, value);
 }
 
@@ -1157,8 +1173,7 @@ static int eeepc_backlight_init(struct eeepc_laptop *eeepc)
 
 static void eeepc_backlight_exit(struct eeepc_laptop *eeepc)
 {
-	if (eeepc->backlight_device)
-		backlight_device_unregister(eeepc->backlight_device);
+	backlight_device_unregister(eeepc->backlight_device);
 	eeepc->backlight_device = NULL;
 }
 
@@ -1217,7 +1232,7 @@ static void eeepc_input_exit(struct eeepc_laptop *eeepc)
 static void eeepc_input_notify(struct eeepc_laptop *eeepc, int event)
 {
 	if (!eeepc->inputdev)
-		return ;
+		return;
 	if (!sparse_keymap_report_event(eeepc->inputdev, event, 1, true))
 		pr_info("Unknown key %x pressed\n", event);
 }
@@ -1225,6 +1240,7 @@ static void eeepc_input_notify(struct eeepc_laptop *eeepc, int event)
 static void eeepc_acpi_notify(struct acpi_device *device, u32 event)
 {
 	struct eeepc_laptop *eeepc = acpi_driver_data(device);
+	int old_brightness, new_brightness;
 	u16 count;
 
 	if (event > ACPI_MAX_SYS_NOTIFY)
@@ -1235,34 +1251,32 @@ static void eeepc_acpi_notify(struct acpi_device *device, u32 event)
 					count);
 
 	/* Brightness events are special */
-	if (event >= NOTIFY_BRN_MIN && event <= NOTIFY_BRN_MAX) {
-
-		/* Ignore them completely if the acpi video driver is used */
-		if (eeepc->backlight_device != NULL) {
-			int old_brightness, new_brightness;
-
-			/* Update the backlight device. */
-			old_brightness = eeepc_backlight_notify(eeepc);
-
-			/* Convert event to keypress (obsolescent hack) */
-			new_brightness = event - NOTIFY_BRN_MIN;
-
-			if (new_brightness < old_brightness) {
-				event = NOTIFY_BRN_MIN; /* brightness down */
-			} else if (new_brightness > old_brightness) {
-				event = NOTIFY_BRN_MAX; /* brightness up */
-			} else {
-				/*
-				* no change in brightness - already at min/max,
-				* event will be desired value (or else ignored)
-				*/
-			}
-			eeepc_input_notify(eeepc, event);
-		}
-	} else {
-		/* Everything else is a bona-fide keypress event */
+	if (event < NOTIFY_BRN_MIN || event > NOTIFY_BRN_MAX) {
 		eeepc_input_notify(eeepc, event);
+		return;
+	}
+
+	/* Ignore them completely if the acpi video driver is used */
+	if (!eeepc->backlight_device)
+		return;
+
+	/* Update the backlight device. */
+	old_brightness = eeepc_backlight_notify(eeepc);
+
+	/* Convert event to keypress (obsolescent hack) */
+	new_brightness = event - NOTIFY_BRN_MIN;
+
+	if (new_brightness < old_brightness) {
+		event = NOTIFY_BRN_MIN; /* brightness down */
+	} else if (new_brightness > old_brightness) {
+		event = NOTIFY_BRN_MAX; /* brightness up */
+	} else {
+		/*
+		 * no change in brightness - already at min/max,
+		 * event will be desired value (or else ignored)
+		 */
 	}
+	eeepc_input_notify(eeepc, event);
 }
 
 static void eeepc_dmi_check(struct eeepc_laptop *eeepc)
@@ -1294,8 +1308,8 @@ static void eeepc_dmi_check(struct eeepc_laptop *eeepc)
 	 */
 	if (strcmp(model, "701") == 0 || strcmp(model, "702") == 0) {
 		eeepc->cpufv_disabled = true;
-		pr_info("model %s does not officially support setting cpu "
-			"speed\n", model);
+		pr_info("model %s does not officially support setting cpu speed\n",
+			model);
 		pr_info("cpufv disabled to avoid instability\n");
 	}
 
@@ -1321,8 +1335,8 @@ static void cmsg_quirk(struct eeepc_laptop *eeepc, int cm, const char *name)
 	   Check if cm_getv[cm] works and, if yes, assume cm should be set. */
 	if (!(eeepc->cm_supported & (1 << cm))
 	    && !read_acpi_int(eeepc->handle, cm_getv[cm], &dummy)) {
-		pr_info("%s (%x) not reported by BIOS,"
-			" enabling anyway\n", name, 1 << cm);
+		pr_info("%s (%x) not reported by BIOS, enabling anyway\n",
+			name, 1 << cm);
 		eeepc->cm_supported |= 1 << cm;
 	}
 }
diff --git a/drivers/platform/x86/fujitsu-laptop.c b/drivers/platform/x86/fujitsu-laptop.c
index 2655d4a988f3..7c21c1c44dfa 100644
--- a/drivers/platform/x86/fujitsu-laptop.c
+++ b/drivers/platform/x86/fujitsu-laptop.c
@@ -559,7 +559,6 @@ static struct attribute_group fujitsupf_attribute_group = {
 static struct platform_driver fujitsupf_driver = {
 	.driver = {
 		   .name = "fujitsu-laptop",
-		   .owner = THIS_MODULE,
 		   }
 };
 
@@ -1154,8 +1153,7 @@ fail_hotkey1:
 fail_hotkey:
 	platform_driver_unregister(&fujitsupf_driver);
 fail_backlight:
-	if (fujitsu->bl_device)
-		backlight_device_unregister(fujitsu->bl_device);
+	backlight_device_unregister(fujitsu->bl_device);
 fail_sysfs_group:
 	sysfs_remove_group(&fujitsu->pf_device->dev.kobj,
 			   &fujitsupf_attribute_group);
@@ -1179,8 +1177,7 @@ static void __exit fujitsu_cleanup(void)
 
 	platform_driver_unregister(&fujitsupf_driver);
 
-	if (fujitsu->bl_device)
-		backlight_device_unregister(fujitsu->bl_device);
+	backlight_device_unregister(fujitsu->bl_device);
 
 	sysfs_remove_group(&fujitsu->pf_device->dev.kobj,
 			   &fujitsupf_attribute_group);
diff --git a/drivers/platform/x86/hdaps.c b/drivers/platform/x86/hdaps.c
index 777c7e3dda51..458e6c948c11 100644
--- a/drivers/platform/x86/hdaps.c
+++ b/drivers/platform/x86/hdaps.c
@@ -318,7 +318,6 @@ static struct platform_driver hdaps_driver = {
 	.probe = hdaps_probe,
 	.driver	= {
 		.name = "hdaps",
-		.owner = THIS_MODULE,
 		.pm = &hdaps_pm,
 	},
 };
diff --git a/drivers/platform/x86/hp-wireless.c b/drivers/platform/x86/hp-wireless.c
index 415348fc1210..4e4cc8bd7557 100644
--- a/drivers/platform/x86/hp-wireless.c
+++ b/drivers/platform/x86/hp-wireless.c
@@ -85,6 +85,9 @@ static int hpwl_add(struct acpi_device *device)
 	int err;
 
 	err = hp_wireless_input_setup();
+	if (err)
+		pr_err("Failed to setup hp wireless hotkeys\n");
+
 	return err;
 }
 
diff --git a/drivers/platform/x86/hp-wmi.c b/drivers/platform/x86/hp-wmi.c
index 4c559640dcba..0ab2b377a778 100644
--- a/drivers/platform/x86/hp-wmi.c
+++ b/drivers/platform/x86/hp-wmi.c
@@ -1006,7 +1006,6 @@ static const struct dev_pm_ops hp_wmi_pm_ops = {
 static struct platform_driver hp_wmi_driver = {
 	.driver = {
 		.name = "hp-wmi",
-		.owner = THIS_MODULE,
 		.pm = &hp_wmi_pm_ops,
 	},
 	.remove = __exit_p(hp_wmi_bios_remove),
diff --git a/drivers/platform/x86/hp_accel.c b/drivers/platform/x86/hp_accel.c
index 6bec745b6b92..10ce6cba4455 100644
--- a/drivers/platform/x86/hp_accel.c
+++ b/drivers/platform/x86/hp_accel.c
@@ -246,6 +246,7 @@ static const struct dmi_system_id lis3lv02d_dmi_ids[] = {
 	AXIS_DMI_MATCH("HPB64xx", "HP ProBook 64", xy_swap),
 	AXIS_DMI_MATCH("HPB64xx", "HP EliteBook 84", xy_swap),
 	AXIS_DMI_MATCH("HPB65xx", "HP ProBook 65", x_inverted),
+	AXIS_DMI_MATCH("HPZBook15", "HP ZBook 15", x_inverted),
 	{ NULL, }
 /* Laptop models without axis info (yet):
  * "NC6910" "HP Compaq 6910"
diff --git a/drivers/platform/x86/ideapad-laptop.c b/drivers/platform/x86/ideapad-laptop.c
index ed494f37c40f..b3d419a84723 100644
--- a/drivers/platform/x86/ideapad-laptop.c
+++ b/drivers/platform/x86/ideapad-laptop.c
@@ -729,8 +729,7 @@ static int ideapad_backlight_init(struct ideapad_private *priv)
 
 static void ideapad_backlight_exit(struct ideapad_private *priv)
 {
-	if (priv->blightdev)
-		backlight_device_unregister(priv->blightdev);
+	backlight_device_unregister(priv->blightdev);
 	priv->blightdev = NULL;
 }
 
@@ -966,7 +965,6 @@ static struct platform_driver ideapad_acpi_driver = {
 	.remove = ideapad_acpi_remove,
 	.driver = {
 		.name   = "ideapad_acpi",
-		.owner  = THIS_MODULE,
 		.pm     = &ideapad_pm,
 		.acpi_match_table = ACPI_PTR(ideapad_device_ids),
 	},
diff --git a/drivers/platform/x86/intel_ips.c b/drivers/platform/x86/intel_ips.c
index c0242ed13d9e..e2065e06a3f3 100644
--- a/drivers/platform/x86/intel_ips.c
+++ b/drivers/platform/x86/intel_ips.c
@@ -33,7 +33,7 @@
  * performance by allocating more power or thermal budget to the CPU or GPU
  * based on available headroom and activity.
  *
- * The basic algorithm is driven by a 5s moving average of tempurature.  If
+ * The basic algorithm is driven by a 5s moving average of temperature.  If
  * thermal headroom is available, the CPU and/or GPU power clamps may be
  * adjusted upwards.  If we hit the thermal ceiling or a thermal trigger,
  * we scale back the clamp.  Aside from trigger events (when we're critically
@@ -593,7 +593,7 @@ static void ips_disable_gpu_turbo(struct ips_driver *ips)
 		return;
 
 	if (!ips->gpu_turbo_disable())
-		dev_err(&ips->dev->dev, "failed to disable graphis turbo\n");
+		dev_err(&ips->dev->dev, "failed to disable graphics turbo\n");
 	else
 		ips->__gpu_turbo_on = false;
 }
diff --git a/drivers/platform/x86/intel_mid_powerbtn.c b/drivers/platform/x86/intel_mid_powerbtn.c
index 8d6775266d66..22606d6b2af3 100644
--- a/drivers/platform/x86/intel_mid_powerbtn.c
+++ b/drivers/platform/x86/intel_mid_powerbtn.c
@@ -133,7 +133,6 @@ static int mfld_pb_remove(struct platform_device *pdev)
 static struct platform_driver mfld_pb_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 	},
 	.probe	= mfld_pb_probe,
 	.remove	= mfld_pb_remove,
diff --git a/drivers/platform/x86/intel_mid_thermal.c b/drivers/platform/x86/intel_mid_thermal.c
index ab7860a21a22..0944e834af8d 100644
--- a/drivers/platform/x86/intel_mid_thermal.c
+++ b/drivers/platform/x86/intel_mid_thermal.c
@@ -555,7 +555,6 @@ static const struct platform_device_id therm_id_table[] = {
 static struct platform_driver mid_thermal_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.pm = &mid_thermal_pm,
 	},
 	.probe = mid_thermal_probe,
diff --git a/drivers/platform/x86/intel_oaktrail.c b/drivers/platform/x86/intel_oaktrail.c
index 4bc960416785..a4a4258f6134 100644
--- a/drivers/platform/x86/intel_oaktrail.c
+++ b/drivers/platform/x86/intel_oaktrail.c
@@ -271,8 +271,7 @@ static int oaktrail_backlight_init(void)
 
 static void oaktrail_backlight_exit(void)
 {
-	if (oaktrail_bl_device)
-		backlight_device_unregister(oaktrail_bl_device);
+	backlight_device_unregister(oaktrail_bl_device);
 }
 
 static int oaktrail_probe(struct platform_device *pdev)
@@ -288,7 +287,6 @@ static int oaktrail_remove(struct platform_device *pdev)
 static struct platform_driver oaktrail_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 	},
 	.probe	= oaktrail_probe,
 	.remove	= oaktrail_remove,
diff --git a/drivers/platform/x86/intel_pmic_gpio.c b/drivers/platform/x86/intel_pmic_gpio.c
index 04fed00b88e9..709f0afdafa8 100644
--- a/drivers/platform/x86/intel_pmic_gpio.c
+++ b/drivers/platform/x86/intel_pmic_gpio.c
@@ -314,7 +314,6 @@ err2:
 static struct platform_driver platform_pmic_gpio_driver = {
 	.driver = {
 		.name		= DRIVER_NAME,
-		.owner		= THIS_MODULE,
 	},
 	.probe		= platform_pmic_gpio_probe,
 };
diff --git a/drivers/platform/x86/msi-laptop.c b/drivers/platform/x86/msi-laptop.c
index 62f8030b9e77..085987730aab 100644
--- a/drivers/platform/x86/msi-laptop.c
+++ b/drivers/platform/x86/msi-laptop.c
@@ -573,7 +573,6 @@ static struct attribute_group msipf_old_attribute_group = {
 static struct platform_driver msipf_driver = {
 	.driver = {
 		.name = "msi-laptop-pf",
-		.owner = THIS_MODULE,
 		.pm = &msi_laptop_pm,
 	},
 };
@@ -821,7 +820,7 @@ static bool msi_laptop_i8042_filter(unsigned char data, unsigned char str,
 {
 	static bool extended;
 
-	if (str & 0x20)
+	if (str & I8042_STR_AUXDATA)
 		return false;
 
 	/* 0x54 wwan, 0x62 bluetooth, 0x76 wlan, 0xE4 touchpad toggle*/
diff --git a/drivers/platform/x86/msi-wmi.c b/drivers/platform/x86/msi-wmi.c
index 70222f265f68..6d2bac0c463c 100644
--- a/drivers/platform/x86/msi-wmi.c
+++ b/drivers/platform/x86/msi-wmi.c
@@ -354,8 +354,7 @@ static void __exit msi_wmi_exit(void)
 		sparse_keymap_free(msi_wmi_input_dev);
 		input_unregister_device(msi_wmi_input_dev);
 	}
-	if (backlight)
-		backlight_device_unregister(backlight);
+	backlight_device_unregister(backlight);
 }
 
 module_init(msi_wmi_init);
diff --git a/drivers/platform/x86/samsung-q10.c b/drivers/platform/x86/samsung-q10.c
index 28d12bda3ac1..e6aac725a0af 100644
--- a/drivers/platform/x86/samsung-q10.c
+++ b/drivers/platform/x86/samsung-q10.c
@@ -82,7 +82,6 @@ static int samsungq10_remove(struct platform_device *pdev)
 static struct platform_driver samsungq10_driver = {
 	.driver		= {
 		.name	= KBUILD_MODNAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= samsungq10_probe,
 	.remove		= samsungq10_remove,
diff --git a/drivers/platform/x86/sony-laptop.c b/drivers/platform/x86/sony-laptop.c
index 26ad9ff12ac5..6dd1c0e7dcd9 100644
--- a/drivers/platform/x86/sony-laptop.c
+++ b/drivers/platform/x86/sony-laptop.c
@@ -581,7 +581,6 @@ static atomic_t sony_pf_users = ATOMIC_INIT(0);
 static struct platform_driver sony_pf_driver = {
 	.driver = {
 		   .name = "sony-laptop",
-		   .owner = THIS_MODULE,
 		   }
 };
 static struct platform_device *sony_pf_device;
@@ -3141,8 +3140,7 @@ static void sony_nc_backlight_setup(void)
 
 static void sony_nc_backlight_cleanup(void)
 {
-	if (sony_bl_props.dev)
-		backlight_device_unregister(sony_bl_props.dev);
+	backlight_device_unregister(sony_bl_props.dev);
 }
 
 static int sony_nc_add(struct acpi_device *device)
@@ -3717,8 +3715,7 @@ static void sony_pic_detect_device_type(struct sony_pic_dev *dev)
 	dev->event_types = type2_events;
 
 out:
-	if (pcidev)
-		pci_dev_put(pcidev);
+	pci_dev_put(pcidev);
 
 	pr_info("detected Type%d model\n",
 		dev->model == SONYPI_DEVICE_TYPE1 ? 1 :
diff --git a/drivers/platform/x86/tc1100-wmi.c b/drivers/platform/x86/tc1100-wmi.c
index 6a6ea28a7e51..e36542564131 100644
--- a/drivers/platform/x86/tc1100-wmi.c
+++ b/drivers/platform/x86/tc1100-wmi.c
@@ -234,7 +234,6 @@ static const struct dev_pm_ops tc1100_pm_ops = {
 static struct platform_driver tc1100_driver = {
 	.driver = {
 		.name = "tc1100-wmi",
-		.owner = THIS_MODULE,
 #ifdef CONFIG_PM
 		.pm = &tc1100_pm_ops,
 #endif
diff --git a/drivers/platform/x86/thinkpad_acpi.c b/drivers/platform/x86/thinkpad_acpi.c
index cf0f89364d44..c3d11fabc46f 100644
--- a/drivers/platform/x86/thinkpad_acpi.c
+++ b/drivers/platform/x86/thinkpad_acpi.c
@@ -972,7 +972,6 @@ static void tpacpi_shutdown_handler(struct platform_device *pdev)
 static struct platform_driver tpacpi_pdriver = {
 	.driver = {
 		.name = TPACPI_DRVR_NAME,
-		.owner = THIS_MODULE,
 		.pm = &tpacpi_pm,
 	},
 	.shutdown = tpacpi_shutdown_handler,
@@ -981,7 +980,6 @@ static struct platform_driver tpacpi_pdriver = {
 static struct platform_driver tpacpi_hwmon_pdriver = {
 	.driver = {
 		.name = TPACPI_HWMON_DRVR_NAME,
-		.owner = THIS_MODULE,
 	},
 };
 
@@ -6559,6 +6557,17 @@ static struct ibm_struct brightness_driver_data = {
  * bits 3-0 (volume).  Other bits in NVRAM may have other functions,
  * such as bit 7 which is used to detect repeated presses of MUTE,
  * and we leave them unchanged.
+ *
+ * On newer Lenovo ThinkPads, the EC can automatically change the volume
+ * in response to user input.  Unfortunately, this rarely works well.
+ * The laptop changes the state of its internal MUTE gate and, on some
+ * models, sends KEY_MUTE, causing any user code that responds to the
+ * mute button to get confused.  The hardware MUTE gate is also
+ * unnecessary, since user code can handle the mute button without
+ * kernel or EC help.
+ *
+ * To avoid confusing userspace, we simply disable all EC-based mute
+ * and volume controls when possible.
  */
 
 #ifdef CONFIG_THINKPAD_ACPI_ALSA_SUPPORT
@@ -6613,11 +6622,21 @@ enum tpacpi_volume_capabilities {
 	TPACPI_VOL_CAP_MAX
 };
 
+enum tpacpi_mute_btn_mode {
+	TP_EC_MUTE_BTN_LATCH  = 0,	/* Mute mutes; up/down unmutes */
+	/* We don't know what mode 1 is. */
+	TP_EC_MUTE_BTN_NONE   = 2,	/* Mute and up/down are just keys */
+	TP_EC_MUTE_BTN_TOGGLE = 3,	/* Mute toggles; up/down unmutes */
+};
+
 static enum tpacpi_volume_access_mode volume_mode =
 	TPACPI_VOL_MODE_MAX;
 
 static enum tpacpi_volume_capabilities volume_capabilities;
 static bool volume_control_allowed;
+static bool software_mute_requested = true;
+static bool software_mute_active;
+static int software_mute_orig_mode;
 
 /*
  * Used to syncronize writers to TP_EC_AUDIO and
@@ -6635,6 +6654,8 @@ static void tpacpi_volume_checkpoint_nvram(void)
 		return;
 	if (!volume_control_allowed)
 		return;
+	if (software_mute_active)
+		return;
 
 	vdbg_printk(TPACPI_DBG_MIXER,
 		"trying to checkpoint mixer state to NVRAM...\n");
@@ -6696,6 +6717,12 @@ static int volume_set_status_ec(const u8 status)
 
 	dbg_printk(TPACPI_DBG_MIXER, "set EC mixer to 0x%02x\n", status);
 
+	/*
+	 * On X200s, and possibly on others, it can take a while for
+	 * reads to become correct.
+	 */
+	msleep(1);
+
 	return 0;
 }
 
@@ -6778,6 +6805,57 @@ unlock:
 	return rc;
 }
 
+static int volume_set_software_mute(bool startup)
+{
+	int result;
+
+	if (!tpacpi_is_lenovo())
+		return -ENODEV;
+
+	if (startup) {
+		if (!acpi_evalf(ec_handle, &software_mute_orig_mode,
+				"HAUM", "qd"))
+			return -EIO;
+
+		dbg_printk(TPACPI_DBG_INIT | TPACPI_DBG_MIXER,
+			    "Initial HAUM setting was %d\n",
+			    software_mute_orig_mode);
+	}
+
+	if (!acpi_evalf(ec_handle, &result, "SAUM", "qdd",
+			(int)TP_EC_MUTE_BTN_NONE))
+		return -EIO;
+
+	if (result != TP_EC_MUTE_BTN_NONE)
+		pr_warn("Unexpected SAUM result %d\n",
+			result);
+
+	/*
+	 * In software mute mode, the standard codec controls take
+	 * precendence, so we unmute the ThinkPad HW switch at
+	 * startup.  Just on case there are SAUM-capable ThinkPads
+	 * with level controls, set max HW volume as well.
+	 */
+	if (tp_features.mixer_no_level_control)
+		result = volume_set_mute(false);
+	else
+		result = volume_set_status(TP_EC_VOLUME_MAX);
+
+	if (result != 0)
+		pr_warn("Failed to unmute the HW mute switch\n");
+
+	return 0;
+}
+
+static void volume_exit_software_mute(void)
+{
+	int r;
+
+	if (!acpi_evalf(ec_handle, &r, "SAUM", "qdd", software_mute_orig_mode)
+	    || r != software_mute_orig_mode)
+		pr_warn("Failed to restore mute mode\n");
+}
+
 static int volume_alsa_set_volume(const u8 vol)
 {
 	dbg_printk(TPACPI_DBG_MIXER,
@@ -6885,7 +6963,12 @@ static void volume_suspend(void)
 
 static void volume_resume(void)
 {
-	volume_alsa_notify_change();
+	if (software_mute_active) {
+		if (volume_set_software_mute(false) < 0)
+			pr_warn("Failed to restore software mute\n");
+	} else {
+		volume_alsa_notify_change();
+	}
 }
 
 static void volume_shutdown(void)
@@ -6901,6 +6984,9 @@ static void volume_exit(void)
 	}
 
 	tpacpi_volume_checkpoint_nvram();
+
+	if (software_mute_active)
+		volume_exit_software_mute();
 }
 
 static int __init volume_create_alsa_mixer(void)
@@ -7085,16 +7171,20 @@ static int __init volume_init(struct ibm_init_struct *iibm)
 			"mute is supported, volume control is %s\n",
 			str_supported(!tp_features.mixer_no_level_control));
 
-	rc = volume_create_alsa_mixer();
-	if (rc) {
-		pr_err("Could not create the ALSA mixer interface\n");
-		return rc;
-	}
+	if (software_mute_requested && volume_set_software_mute(true) == 0) {
+		software_mute_active = true;
+	} else {
+		rc = volume_create_alsa_mixer();
+		if (rc) {
+			pr_err("Could not create the ALSA mixer interface\n");
+			return rc;
+		}
 
-	pr_info("Console audio control enabled, mode: %s\n",
-		(volume_control_allowed) ?
-			"override (read/write)" :
-			"monitor (read only)");
+		pr_info("Console audio control enabled, mode: %s\n",
+			(volume_control_allowed) ?
+				"override (read/write)" :
+				"monitor (read only)");
+	}
 
 	vdbg_printk(TPACPI_DBG_INIT | TPACPI_DBG_MIXER,
 		"registering volume hotkeys as change notification\n");
@@ -9091,6 +9181,10 @@ MODULE_PARM_DESC(volume_control,
 		 "Enables software override for the console audio "
 		 "control when true");
 
+module_param_named(software_mute, software_mute_requested, bool, 0444);
+MODULE_PARM_DESC(software_mute,
+		 "Request full software mute control");
+
 /* ALSA module API parameters */
 module_param_named(index, alsa_index, int, 0444);
 MODULE_PARM_DESC(index, "ALSA index for the ACPI EC Mixer");
diff --git a/drivers/platform/x86/toshiba_acpi.c b/drivers/platform/x86/toshiba_acpi.c
index ab6151f05420..fc34a71866ed 100644
--- a/drivers/platform/x86/toshiba_acpi.c
+++ b/drivers/platform/x86/toshiba_acpi.c
@@ -186,6 +186,7 @@ static struct toshiba_acpi_dev *toshiba_acpi;
 
 static const struct acpi_device_id toshiba_device_ids[] = {
 	{"TOS6200", 0},
+	{"TOS6207", 0},
 	{"TOS6208", 0},
 	{"TOS1900", 0},
 	{"", 0},
@@ -928,9 +929,7 @@ static int lcd_proc_open(struct inode *inode, struct file *file)
 
 static int set_lcd_brightness(struct toshiba_acpi_dev *dev, int value)
 {
-	u32 in[TCI_WORDS] = { HCI_SET, HCI_LCD_BRIGHTNESS, 0, 0, 0, 0 };
-	u32 out[TCI_WORDS];
-	acpi_status status;
+	u32 hci_result;
 
 	if (dev->tr_backlight_supported) {
 		bool enable = !value;
@@ -941,20 +940,9 @@ static int set_lcd_brightness(struct toshiba_acpi_dev *dev, int value)
 			value--;
 	}
 
-	in[2] = value << HCI_LCD_BRIGHTNESS_SHIFT;
-	status = tci_raw(dev, in, out);
-	if (ACPI_FAILURE(status) || out[0] == TOS_FAILURE) {
-		pr_err("ACPI call to set brightness failed");
-		return -EIO;
-	}
-	/* Extra check for "incomplete" backlight method, where the AML code
-	 * doesn't check for HCI_SET or HCI_GET and returns TOS_SUCCESS,
-	 * the actual brightness, and in some cases the max brightness.
-	 */
-	if (out[2] > 0  || out[3] == 0xE000)
-		return -ENODEV;
-
-	return out[0] == TOS_SUCCESS ? 0 : -EIO;
+	value = value << HCI_LCD_BRIGHTNESS_SHIFT;
+	hci_result = hci_write1(dev, HCI_LCD_BRIGHTNESS, value);
+	return hci_result == TOS_SUCCESS ? 0 : -EIO;
 }
 
 static int set_lcd_status(struct backlight_device *bd)
@@ -1406,12 +1394,6 @@ static ssize_t toshiba_kbd_bl_mode_store(struct device *dev,
 		if (ret)
 			return ret;
 
-		/* Update sysfs entries on successful mode change*/
-		ret = sysfs_update_group(&toshiba->acpi_dev->dev.kobj,
-					 &toshiba_attr_group);
-		if (ret)
-			return ret;
-
 		toshiba->kbd_mode = mode;
 	}
 
@@ -1586,10 +1568,32 @@ static umode_t toshiba_sysfs_is_visible(struct kobject *kobj,
 	return exists ? attr->mode : 0;
 }
 
+/*
+ * Hotkeys
+ */
+static int toshiba_acpi_enable_hotkeys(struct toshiba_acpi_dev *dev)
+{
+	acpi_status status;
+	u32 result;
+
+	status = acpi_evaluate_object(dev->acpi_dev->handle,
+				      "ENAB", NULL, NULL);
+	if (ACPI_FAILURE(status))
+		return -ENODEV;
+
+	result = hci_write1(dev, HCI_HOTKEY_EVENT, HCI_HOTKEY_ENABLE);
+	if (result == TOS_FAILURE)
+		return -EIO;
+	else if (result == TOS_NOT_SUPPORTED)
+		return -ENODEV;
+
+	return 0;
+}
+
 static bool toshiba_acpi_i8042_filter(unsigned char data, unsigned char str,
 				      struct serio *port)
 {
-	if (str & 0x20)
+	if (str & I8042_STR_AUXDATA)
 		return false;
 
 	if (unlikely(data == 0xe0))
@@ -1648,9 +1652,45 @@ static void toshiba_acpi_report_hotkey(struct toshiba_acpi_dev *dev,
 		pr_info("Unknown key %x\n", scancode);
 }
 
+static void toshiba_acpi_process_hotkeys(struct toshiba_acpi_dev *dev)
+{
+	u32 hci_result, value;
+	int retries = 3;
+	int scancode;
+
+	if (dev->info_supported) {
+		scancode = toshiba_acpi_query_hotkey(dev);
+		if (scancode < 0)
+			pr_err("Failed to query hotkey event\n");
+		else if (scancode != 0)
+			toshiba_acpi_report_hotkey(dev, scancode);
+	} else if (dev->system_event_supported) {
+		do {
+			hci_result = hci_read1(dev, HCI_SYSTEM_EVENT, &value);
+			switch (hci_result) {
+			case TOS_SUCCESS:
+				toshiba_acpi_report_hotkey(dev, (int)value);
+				break;
+			case TOS_NOT_SUPPORTED:
+				/*
+				 * This is a workaround for an unresolved
+				 * issue on some machines where system events
+				 * sporadically become disabled.
+				 */
+				hci_result =
+					hci_write1(dev, HCI_SYSTEM_EVENT, 1);
+				pr_notice("Re-enabled hotkeys\n");
+				/* fall through */
+			default:
+				retries--;
+				break;
+			}
+		} while (retries && hci_result != TOS_FIFO_EMPTY);
+	}
+}
+
 static int toshiba_acpi_setup_keyboard(struct toshiba_acpi_dev *dev)
 {
-	acpi_status status;
 	acpi_handle ec_handle;
 	int error;
 	u32 hci_result;
@@ -1677,7 +1717,6 @@ static int toshiba_acpi_setup_keyboard(struct toshiba_acpi_dev *dev)
 	 * supported, so if it's present set up an i8042 key filter
 	 * for this purpose.
 	 */
-	status = AE_ERROR;
 	ec_handle = ec_get_handle();
 	if (ec_handle && acpi_has_method(ec_handle, "NTFY")) {
 		INIT_WORK(&dev->hotkey_work, toshiba_acpi_hotkey_work);
@@ -1708,10 +1747,9 @@ static int toshiba_acpi_setup_keyboard(struct toshiba_acpi_dev *dev)
 		goto err_remove_filter;
 	}
 
-	status = acpi_evaluate_object(dev->acpi_dev->handle, "ENAB", NULL, NULL);
-	if (ACPI_FAILURE(status)) {
+	error = toshiba_acpi_enable_hotkeys(dev);
+	if (error) {
 		pr_info("Unable to enable hotkeys\n");
-		error = -ENODEV;
 		goto err_remove_filter;
 	}
 
@@ -1721,7 +1759,6 @@ static int toshiba_acpi_setup_keyboard(struct toshiba_acpi_dev *dev)
 		goto err_remove_filter;
 	}
 
-	hci_result = hci_write1(dev, HCI_HOTKEY_EVENT, HCI_HOTKEY_ENABLE);
 	return 0;
 
  err_remove_filter:
@@ -1810,8 +1847,7 @@ static int toshiba_acpi_remove(struct acpi_device *acpi_dev)
 		rfkill_destroy(dev->bt_rfk);
 	}
 
-	if (dev->backlight_dev)
-		backlight_device_unregister(dev->backlight_dev);
+	backlight_device_unregister(dev->backlight_dev);
 
 	if (dev->illumination_supported)
 		led_classdev_unregister(&dev->led_dev);
@@ -1967,41 +2003,29 @@ error:
 static void toshiba_acpi_notify(struct acpi_device *acpi_dev, u32 event)
 {
 	struct toshiba_acpi_dev *dev = acpi_driver_data(acpi_dev);
-	u32 hci_result, value;
-	int retries = 3;
-	int scancode;
-
-	if (event != 0x80)
-		return;
+	int ret;
 
-	if (dev->info_supported) {
-		scancode = toshiba_acpi_query_hotkey(dev);
-		if (scancode < 0)
-			pr_err("Failed to query hotkey event\n");
-		else if (scancode != 0)
-			toshiba_acpi_report_hotkey(dev, scancode);
-	} else if (dev->system_event_supported) {
-		do {
-			hci_result = hci_read1(dev, HCI_SYSTEM_EVENT, &value);
-			switch (hci_result) {
-			case TOS_SUCCESS:
-				toshiba_acpi_report_hotkey(dev, (int)value);
-				break;
-			case TOS_NOT_SUPPORTED:
-				/*
-				 * This is a workaround for an unresolved
-				 * issue on some machines where system events
-				 * sporadically become disabled.
-				 */
-				hci_result =
-					hci_write1(dev, HCI_SYSTEM_EVENT, 1);
-				pr_notice("Re-enabled hotkeys\n");
-				/* fall through */
-			default:
-				retries--;
-				break;
-			}
-		} while (retries && hci_result != TOS_FIFO_EMPTY);
+	switch (event) {
+	case 0x80: /* Hotkeys and some system events */
+		toshiba_acpi_process_hotkeys(dev);
+		break;
+	case 0x92: /* Keyboard backlight mode changed */
+		/* Update sysfs entries */
+		ret = sysfs_update_group(&acpi_dev->dev.kobj,
+					 &toshiba_attr_group);
+		if (ret)
+			pr_err("Unable to update sysfs entries\n");
+		break;
+	case 0x81: /* Unknown */
+	case 0x82: /* Unknown */
+	case 0x83: /* Unknown */
+	case 0x8c: /* Unknown */
+	case 0x8e: /* Unknown */
+	case 0x8f: /* Unknown */
+	case 0x90: /* Unknown */
+	default:
+		pr_info("Unknown event received %x\n", event);
+		break;
 	}
 }
 
@@ -2020,16 +2044,12 @@ static int toshiba_acpi_suspend(struct device *device)
 static int toshiba_acpi_resume(struct device *device)
 {
 	struct toshiba_acpi_dev *dev = acpi_driver_data(to_acpi_device(device));
-	u32 result;
-	acpi_status status;
+	int error;
 
 	if (dev->hotkey_dev) {
-		status = acpi_evaluate_object(dev->acpi_dev->handle, "ENAB",
-				NULL, NULL);
-		if (ACPI_FAILURE(status))
+		error = toshiba_acpi_enable_hotkeys(dev);
+		if (error)
 			pr_info("Unable to re-enable hotkeys\n");
-
-		result = hci_write1(dev, HCI_HOTKEY_EVENT, HCI_HOTKEY_ENABLE);
 	}
 
 	return 0;
diff --git a/drivers/platform/x86/xo1-rfkill.c b/drivers/platform/x86/xo1-rfkill.c
index 4bd17248dfc6..e46fa9cebc7d 100644
--- a/drivers/platform/x86/xo1-rfkill.c
+++ b/drivers/platform/x86/xo1-rfkill.c
@@ -71,7 +71,6 @@ static int xo1_rfkill_remove(struct platform_device *pdev)
 static struct platform_driver xo1_rfkill_driver = {
 	.driver = {
 		.name = "xo1-rfkill",
-		.owner = THIS_MODULE,
 	},
 	.probe		= xo1_rfkill_probe,
 	.remove		= xo1_rfkill_remove,
diff --git a/drivers/power/88pm860x_battery.c b/drivers/power/88pm860x_battery.c
index dfcda3a49403..bd3c997f4fee 100644
--- a/drivers/power/88pm860x_battery.c
+++ b/drivers/power/88pm860x_battery.c
@@ -1023,7 +1023,6 @@ static SIMPLE_DEV_PM_OPS(pm860x_battery_pm_ops,
 static struct platform_driver pm860x_battery_driver = {
 	.driver = {
 		   .name = "88pm860x-battery",
-		   .owner = THIS_MODULE,
 		   .pm = &pm860x_battery_pm_ops,
 	},
 	.probe = pm860x_battery_probe,
diff --git a/drivers/power/88pm860x_charger.c b/drivers/power/88pm860x_charger.c
index de029bbc1cc1..650930e4fa79 100644
--- a/drivers/power/88pm860x_charger.c
+++ b/drivers/power/88pm860x_charger.c
@@ -732,7 +732,6 @@ static int pm860x_charger_remove(struct platform_device *pdev)
 static struct platform_driver pm860x_charger_driver = {
 	.driver = {
 		   .name = "88pm860x-charger",
-		   .owner = THIS_MODULE,
 	},
 	.probe = pm860x_charger_probe,
 	.remove = pm860x_charger_remove,
diff --git a/drivers/power/ab8500_btemp.c b/drivers/power/ab8500_btemp.c
index 7f9a4547dccd..4ebf7b0819f7 100644
--- a/drivers/power/ab8500_btemp.c
+++ b/drivers/power/ab8500_btemp.c
@@ -1195,7 +1195,6 @@ static struct platform_driver ab8500_btemp_driver = {
 	.resume = ab8500_btemp_resume,
 	.driver = {
 		.name = "ab8500-btemp",
-		.owner = THIS_MODULE,
 		.of_match_table = ab8500_btemp_match,
 	},
 };
diff --git a/drivers/power/ab8500_charger.c b/drivers/power/ab8500_charger.c
index 19110aa613a1..8c8d170ff0f8 100644
--- a/drivers/power/ab8500_charger.c
+++ b/drivers/power/ab8500_charger.c
@@ -3733,7 +3733,6 @@ static struct platform_driver ab8500_charger_driver = {
 	.resume = ab8500_charger_resume,
 	.driver = {
 		.name = "ab8500-charger",
-		.owner = THIS_MODULE,
 		.of_match_table = ab8500_charger_match,
 	},
 };
diff --git a/drivers/power/ab8500_fg.c b/drivers/power/ab8500_fg.c
index 99a78d365ceb..69b80bcaa9e7 100644
--- a/drivers/power/ab8500_fg.c
+++ b/drivers/power/ab8500_fg.c
@@ -3281,7 +3281,6 @@ static struct platform_driver ab8500_fg_driver = {
 	.resume = ab8500_fg_resume,
 	.driver = {
 		.name = "ab8500-fg",
-		.owner = THIS_MODULE,
 		.of_match_table = ab8500_fg_match,
 	},
 };
diff --git a/drivers/power/abx500_chargalg.c b/drivers/power/abx500_chargalg.c
index 6d2723664a01..ab54b8dea670 100644
--- a/drivers/power/abx500_chargalg.c
+++ b/drivers/power/abx500_chargalg.c
@@ -2156,7 +2156,6 @@ static struct platform_driver abx500_chargalg_driver = {
 	.resume = abx500_chargalg_resume,
 	.driver = {
 		.name = "ab8500-chargalg",
-		.owner = THIS_MODULE,
 		.of_match_table = ab8500_chargalg_match,
 	},
 };
diff --git a/drivers/power/bq27x00_battery.c b/drivers/power/bq27x00_battery.c
index e3bacfe3bcd0..a78ac201828e 100644
--- a/drivers/power/bq27x00_battery.c
+++ b/drivers/power/bq27x00_battery.c
@@ -1014,7 +1014,6 @@ static struct platform_driver bq27000_battery_driver = {
 	.remove = bq27000_battery_remove,
 	.driver = {
 		.name = "bq27000-battery",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/power/charger-manager.c b/drivers/power/charger-manager.c
index ef8094a61f1e..649052e1f2d9 100644
--- a/drivers/power/charger-manager.c
+++ b/drivers/power/charger-manager.c
@@ -2086,7 +2086,6 @@ static const struct dev_pm_ops charger_manager_pm = {
 static struct platform_driver charger_manager_driver = {
 	.driver = {
 		.name = "charger-manager",
-		.owner = THIS_MODULE,
 		.pm = &charger_manager_pm,
 		.of_match_table = charger_manager_match,
 	},
diff --git a/drivers/power/da9030_battery.c b/drivers/power/da9030_battery.c
index ae6c41835ee6..78cd5d66144b 100644
--- a/drivers/power/da9030_battery.c
+++ b/drivers/power/da9030_battery.c
@@ -579,7 +579,6 @@ static int da9030_battery_remove(struct platform_device *dev)
 static struct platform_driver da903x_battery_driver = {
 	.driver	= {
 		.name	= "da903x-battery",
-		.owner	= THIS_MODULE,
 	},
 	.probe = da9030_battery_probe,
 	.remove = da9030_battery_remove,
diff --git a/drivers/power/da9052-battery.c b/drivers/power/da9052-battery.c
index f8f4c0f7c17d..d17250f745c2 100644
--- a/drivers/power/da9052-battery.c
+++ b/drivers/power/da9052-battery.c
@@ -656,7 +656,6 @@ static struct platform_driver da9052_bat_driver = {
 	.remove = da9052_bat_remove,
 	.driver = {
 		.name = "da9052-bat",
-		.owner = THIS_MODULE,
 	},
 };
 module_platform_driver(da9052_bat_driver);
diff --git a/drivers/power/ds2782_battery.c b/drivers/power/ds2782_battery.c
index 041f9b638d28..39694883d3bf 100644
--- a/drivers/power/ds2782_battery.c
+++ b/drivers/power/ds2782_battery.c
@@ -351,13 +351,9 @@ static int ds278x_resume(struct device *dev)
 	schedule_delayed_work(&info->bat_work, DS278x_DELAY);
 	return 0;
 }
+#endif /* CONFIG_PM_SLEEP */
 
 static SIMPLE_DEV_PM_OPS(ds278x_battery_pm_ops, ds278x_suspend, ds278x_resume);
-#define DS278X_BATTERY_PM_OPS (&ds278x_battery_pm_ops)
-
-#else
-#define DS278X_BATTERY_PM_OPS NULL
-#endif /* CONFIG_PM_SLEEP */
 
 enum ds278x_num_id {
 	DS2782 = 0,
@@ -460,7 +456,7 @@ MODULE_DEVICE_TABLE(i2c, ds278x_id);
 static struct i2c_driver ds278x_battery_driver = {
 	.driver 	= {
 		.name	= "ds2782-battery",
-		.pm	= DS278X_BATTERY_PM_OPS,
+		.pm	= &ds278x_battery_pm_ops,
 	},
 	.probe		= ds278x_battery_probe,
 	.remove		= ds278x_battery_remove,
diff --git a/drivers/power/generic-adc-battery.c b/drivers/power/generic-adc-battery.c
index 59a1421f9288..d72733e4f93a 100644
--- a/drivers/power/generic-adc-battery.c
+++ b/drivers/power/generic-adc-battery.c
@@ -414,7 +414,6 @@ static const struct dev_pm_ops gab_pm_ops = {
 static struct platform_driver gab_driver = {
 	.driver		= {
 		.name	= "generic-adc-battery",
-		.owner	= THIS_MODULE,
 		.pm	= GAB_PM_OPS
 	},
 	.probe		= gab_probe,
diff --git a/drivers/power/gpio-charger.c b/drivers/power/gpio-charger.c
index 7536933d0ab9..aef74bdf7ab3 100644
--- a/drivers/power/gpio-charger.c
+++ b/drivers/power/gpio-charger.c
@@ -22,6 +22,8 @@
 #include <linux/platform_device.h>
 #include <linux/power_supply.h>
 #include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
 
 #include <linux/power/gpio-charger.h>
 
@@ -69,6 +71,59 @@ static enum power_supply_property gpio_charger_properties[] = {
 	POWER_SUPPLY_PROP_ONLINE,
 };
 
+static
+struct gpio_charger_platform_data *gpio_charger_parse_dt(struct device *dev)
+{
+	struct device_node *np = dev->of_node;
+	struct gpio_charger_platform_data *pdata;
+	const char *chargetype;
+	enum of_gpio_flags flags;
+	int ret;
+
+	if (!np)
+		return ERR_PTR(-ENOENT);
+
+	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return ERR_PTR(-ENOMEM);
+
+	pdata->name = np->name;
+
+	pdata->gpio = of_get_gpio_flags(np, 0, &flags);
+	if (pdata->gpio < 0) {
+		if (pdata->gpio != -EPROBE_DEFER)
+			dev_err(dev, "could not get charger gpio\n");
+		return ERR_PTR(pdata->gpio);
+	}
+
+	pdata->gpio_active_low = !!(flags & OF_GPIO_ACTIVE_LOW);
+
+	pdata->type = POWER_SUPPLY_TYPE_UNKNOWN;
+	ret = of_property_read_string(np, "charger-type", &chargetype);
+	if (ret >= 0) {
+		if (!strncmp("unknown", chargetype, 7))
+			pdata->type = POWER_SUPPLY_TYPE_UNKNOWN;
+		else if (!strncmp("battery", chargetype, 7))
+			pdata->type = POWER_SUPPLY_TYPE_BATTERY;
+		else if (!strncmp("ups", chargetype, 3))
+			pdata->type = POWER_SUPPLY_TYPE_UPS;
+		else if (!strncmp("mains", chargetype, 5))
+			pdata->type = POWER_SUPPLY_TYPE_MAINS;
+		else if (!strncmp("usb-sdp", chargetype, 7))
+			pdata->type = POWER_SUPPLY_TYPE_USB;
+		else if (!strncmp("usb-dcp", chargetype, 7))
+			pdata->type = POWER_SUPPLY_TYPE_USB_DCP;
+		else if (!strncmp("usb-cdp", chargetype, 7))
+			pdata->type = POWER_SUPPLY_TYPE_USB_CDP;
+		else if (!strncmp("usb-aca", chargetype, 7))
+			pdata->type = POWER_SUPPLY_TYPE_USB_ACA;
+		else
+			dev_warn(dev, "unknown charger type %s\n", chargetype);
+	}
+
+	return pdata;
+}
+
 static int gpio_charger_probe(struct platform_device *pdev)
 {
 	const struct gpio_charger_platform_data *pdata = pdev->dev.platform_data;
@@ -78,8 +133,13 @@ static int gpio_charger_probe(struct platform_device *pdev)
 	int irq;
 
 	if (!pdata) {
-		dev_err(&pdev->dev, "No platform data\n");
-		return -EINVAL;
+		pdata = gpio_charger_parse_dt(&pdev->dev);
+		if (IS_ERR(pdata)) {
+			ret = PTR_ERR(pdata);
+			if (ret != -EPROBE_DEFER)
+				dev_err(&pdev->dev, "No platform data\n");
+			return ret;
+		}
 	}
 
 	if (!gpio_is_valid(pdata->gpio)) {
@@ -103,6 +163,7 @@ static int gpio_charger_probe(struct platform_device *pdev)
 	charger->get_property = gpio_charger_get_property;
 	charger->supplied_to = pdata->supplied_to;
 	charger->num_supplicants = pdata->num_supplicants;
+	charger->of_node = pdev->dev.of_node;
 
 	ret = gpio_request(pdata->gpio, dev_name(&pdev->dev));
 	if (ret) {
@@ -189,13 +250,19 @@ static int gpio_charger_resume(struct device *dev)
 static SIMPLE_DEV_PM_OPS(gpio_charger_pm_ops,
 		gpio_charger_suspend, gpio_charger_resume);
 
+static const struct of_device_id gpio_charger_match[] = {
+	{ .compatible = "gpio-charger" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, gpio_charger_match);
+
 static struct platform_driver gpio_charger_driver = {
 	.probe = gpio_charger_probe,
 	.remove = gpio_charger_remove,
 	.driver = {
 		.name = "gpio-charger",
-		.owner = THIS_MODULE,
 		.pm = &gpio_charger_pm_ops,
+		.of_match_table = gpio_charger_match,
 	},
 };
 
diff --git a/drivers/power/intel_mid_battery.c b/drivers/power/intel_mid_battery.c
index 4520811168ad..de3f39e6fa8e 100644
--- a/drivers/power/intel_mid_battery.c
+++ b/drivers/power/intel_mid_battery.c
@@ -773,7 +773,6 @@ static int platform_pmic_battery_remove(struct platform_device *pdev)
 static struct platform_driver platform_pmic_battery_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 	},
 	.probe = platform_pmic_battery_probe,
 	.remove = platform_pmic_battery_remove,
diff --git a/drivers/power/jz4740-battery.c b/drivers/power/jz4740-battery.c
index 6c8931d4ad62..9cd391d61819 100644
--- a/drivers/power/jz4740-battery.c
+++ b/drivers/power/jz4740-battery.c
@@ -406,7 +406,6 @@ static struct platform_driver jz_battery_driver = {
 	.remove		= jz_battery_remove,
 	.driver = {
 		.name = "jz4740-battery",
-		.owner = THIS_MODULE,
 		.pm = JZ_BATTERY_PM_OPS,
 	},
 };
diff --git a/drivers/power/lp8788-charger.c b/drivers/power/lp8788-charger.c
index ed49b50b220b..21fc233c7d61 100644
--- a/drivers/power/lp8788-charger.c
+++ b/drivers/power/lp8788-charger.c
@@ -740,7 +740,6 @@ static struct platform_driver lp8788_charger_driver = {
 	.remove = lp8788_charger_remove,
 	.driver = {
 		.name = LP8788_DEV_CHARGER,
-		.owner = THIS_MODULE,
 	},
 };
 module_platform_driver(lp8788_charger_driver);
diff --git a/drivers/power/max14577_charger.c b/drivers/power/max14577_charger.c
index 0a2bc7277026..ef4103ee6021 100644
--- a/drivers/power/max14577_charger.c
+++ b/drivers/power/max14577_charger.c
@@ -599,7 +599,6 @@ MODULE_DEVICE_TABLE(platform, max14577_charger_id);
 
 static struct platform_driver max14577_charger_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= "max14577-charger",
 	},
 	.probe		= max14577_charger_probe,
diff --git a/drivers/power/max8903_charger.c b/drivers/power/max8903_charger.c
index 08f0d7909b6b..99e3cdcd3e11 100644
--- a/drivers/power/max8903_charger.c
+++ b/drivers/power/max8903_charger.c
@@ -368,7 +368,6 @@ static struct platform_driver max8903_driver = {
 	.remove	= max8903_remove,
 	.driver = {
 		.name	= "max8903-charger",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/power/max8997_charger.c b/drivers/power/max8997_charger.c
index 4bdedfed936d..aefa0c9a3007 100644
--- a/drivers/power/max8997_charger.c
+++ b/drivers/power/max8997_charger.c
@@ -181,7 +181,6 @@ static const struct platform_device_id max8997_battery_id[] = {
 static struct platform_driver max8997_battery_driver = {
 	.driver = {
 		.name = "max8997-battery",
-		.owner = THIS_MODULE,
 	},
 	.probe = max8997_battery_probe,
 	.remove = max8997_battery_remove,
diff --git a/drivers/power/max8998_charger.c b/drivers/power/max8998_charger.c
index 5017470c2fc9..08694c7a9f38 100644
--- a/drivers/power/max8998_charger.c
+++ b/drivers/power/max8998_charger.c
@@ -195,7 +195,6 @@ static const struct platform_device_id max8998_battery_id[] = {
 static struct platform_driver max8998_battery_driver = {
 	.driver = {
 		.name = "max8998-battery",
-		.owner = THIS_MODULE,
 	},
 	.probe = max8998_battery_probe,
 	.remove = max8998_battery_remove,
diff --git a/drivers/power/olpc_battery.c b/drivers/power/olpc_battery.c
index 1ec810ada5ed..ad9cde705de1 100644
--- a/drivers/power/olpc_battery.c
+++ b/drivers/power/olpc_battery.c
@@ -677,7 +677,6 @@ MODULE_DEVICE_TABLE(of, olpc_battery_ids);
 static struct platform_driver olpc_battery_driver = {
 	.driver = {
 		.name = "olpc-battery",
-		.owner = THIS_MODULE,
 		.of_match_table = olpc_battery_ids,
 	},
 	.probe = olpc_battery_probe,
diff --git a/drivers/power/pm2301_charger.c b/drivers/power/pm2301_charger.c
index 62c15af58c9a..777324992c59 100644
--- a/drivers/power/pm2301_charger.c
+++ b/drivers/power/pm2301_charger.c
@@ -951,8 +951,6 @@ static int pm2xxx_wall_charger_suspend(struct device *dev)
 
 #endif
 
-#ifdef CONFIG_PM_RUNTIME
-
 static int  pm2xxx_runtime_suspend(struct device *dev)
 {
 	struct i2c_client *pm2xxx_i2c_client = to_i2c_client(dev);
@@ -977,8 +975,6 @@ static int  pm2xxx_runtime_resume(struct device *dev)
 	return 0;
 }
 
-#endif
-
 static const struct dev_pm_ops pm2xxx_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(pm2xxx_wall_charger_suspend,
 		pm2xxx_wall_charger_resume)
diff --git a/drivers/power/reset/as3722-poweroff.c b/drivers/power/reset/as3722-poweroff.c
index 684971199bd3..60d0295fffb1 100644
--- a/drivers/power/reset/as3722-poweroff.c
+++ b/drivers/power/reset/as3722-poweroff.c
@@ -82,7 +82,6 @@ static int as3722_poweroff_remove(struct platform_device *pdev)
 static struct platform_driver as3722_poweroff_driver = {
 	.driver = {
 		.name = "as3722-power-off",
-		.owner = THIS_MODULE,
 	},
 	.probe = as3722_poweroff_probe,
 	.remove = as3722_poweroff_remove,
diff --git a/drivers/power/reset/axxia-reset.c b/drivers/power/reset/axxia-reset.c
index 3b1f8d601784..4e4cd1c8fe50 100644
--- a/drivers/power/reset/axxia-reset.c
+++ b/drivers/power/reset/axxia-reset.c
@@ -19,14 +19,12 @@
 #include <linux/kernel.h>
 #include <linux/mfd/syscon.h>
 #include <linux/module.h>
+#include <linux/notifier.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/reboot.h>
 #include <linux/regmap.h>
 
-#include <asm/system_misc.h>
-
-
 #define SC_CRIT_WRITE_KEY	0x1000
 #define SC_LATCH_ON_RESET	0x1004
 #define SC_RESET_CONTROL	0x1008
@@ -39,7 +37,8 @@
 
 static struct regmap *syscon;
 
-static void do_axxia_restart(enum reboot_mode reboot_mode, const char *cmd)
+static int axxia_restart_handler(struct notifier_block *this,
+				 unsigned long mode, void *cmd)
 {
 	/* Access Key (0xab) */
 	regmap_write(syscon, SC_CRIT_WRITE_KEY, 0xab);
@@ -50,11 +49,19 @@ static void do_axxia_restart(enum reboot_mode reboot_mode, const char *cmd)
 	/* Assert chip reset */
 	regmap_update_bits(syscon, SC_RESET_CONTROL,
 			   RSTCTL_RST_CHIP, RSTCTL_RST_CHIP);
+
+	return NOTIFY_DONE;
 }
 
+static struct notifier_block axxia_restart_nb = {
+	.notifier_call = axxia_restart_handler,
+	.priority = 128,
+};
+
 static int axxia_reset_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
+	int err;
 
 	syscon = syscon_regmap_lookup_by_phandle(dev->of_node, "syscon");
 	if (IS_ERR(syscon)) {
@@ -62,9 +69,11 @@ static int axxia_reset_probe(struct platform_device *pdev)
 		return PTR_ERR(syscon);
 	}
 
-	arm_pm_restart = do_axxia_restart;
+	err = register_restart_handler(&axxia_restart_nb);
+	if (err)
+		dev_err(dev, "cannot register restart handler (err=%d)\n", err);
 
-	return 0;
+	return err;
 }
 
 static const struct of_device_id of_axxia_reset_match[] = {
diff --git a/drivers/power/reset/brcmstb-reboot.c b/drivers/power/reset/brcmstb-reboot.c
index 3f236924742a..100606f9b3dc 100644
--- a/drivers/power/reset/brcmstb-reboot.c
+++ b/drivers/power/reset/brcmstb-reboot.c
@@ -16,6 +16,7 @@
 #include <linux/init.h>
 #include <linux/io.h>
 #include <linux/jiffies.h>
+#include <linux/notifier.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
 #include <linux/of_platform.h>
@@ -26,8 +27,6 @@
 #include <linux/smp.h>
 #include <linux/mfd/syscon.h>
 
-#include <asm/system_misc.h>
-
 #define RESET_SOURCE_ENABLE_REG 1
 #define SW_MASTER_RESET_REG 2
 
@@ -35,7 +34,8 @@ static struct regmap *regmap;
 static u32 rst_src_en;
 static u32 sw_mstr_rst;
 
-static void brcmstb_reboot(enum reboot_mode mode, const char *cmd)
+static int brcmstb_restart_handler(struct notifier_block *this,
+				   unsigned long mode, void *cmd)
 {
 	int rc;
 	u32 tmp;
@@ -43,31 +43,38 @@ static void brcmstb_reboot(enum reboot_mode mode, const char *cmd)
 	rc = regmap_write(regmap, rst_src_en, 1);
 	if (rc) {
 		pr_err("failed to write rst_src_en (%d)\n", rc);
-		return;
+		return NOTIFY_DONE;
 	}
 
 	rc = regmap_read(regmap, rst_src_en, &tmp);
 	if (rc) {
 		pr_err("failed to read rst_src_en (%d)\n", rc);
-		return;
+		return NOTIFY_DONE;
 	}
 
 	rc = regmap_write(regmap, sw_mstr_rst, 1);
 	if (rc) {
 		pr_err("failed to write sw_mstr_rst (%d)\n", rc);
-		return;
+		return NOTIFY_DONE;
 	}
 
 	rc = regmap_read(regmap, sw_mstr_rst, &tmp);
 	if (rc) {
 		pr_err("failed to read sw_mstr_rst (%d)\n", rc);
-		return;
+		return NOTIFY_DONE;
 	}
 
 	while (1)
 		;
+
+	return NOTIFY_DONE;
 }
 
+static struct notifier_block brcmstb_restart_nb = {
+	.notifier_call = brcmstb_restart_handler,
+	.priority = 128,
+};
+
 static int brcmstb_reboot_probe(struct platform_device *pdev)
 {
 	int rc;
@@ -93,9 +100,12 @@ static int brcmstb_reboot_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	arm_pm_restart = brcmstb_reboot;
+	rc = register_restart_handler(&brcmstb_restart_nb);
+	if (rc)
+		dev_err(&pdev->dev,
+			"cannot register restart handler (err=%d)\n", rc);
 
-	return 0;
+	return rc;
 }
 
 static const struct of_device_id of_match[] = {
@@ -107,7 +117,6 @@ static struct platform_driver brcmstb_reboot_driver = {
 	.probe = brcmstb_reboot_probe,
 	.driver = {
 		.name = "brcmstb-reboot",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match,
 	},
 };
diff --git a/drivers/power/reset/gpio-poweroff.c b/drivers/power/reset/gpio-poweroff.c
index ce849bc9b269..e5332f1db8a7 100644
--- a/drivers/power/reset/gpio-poweroff.c
+++ b/drivers/power/reset/gpio-poweroff.c
@@ -99,7 +99,6 @@ static struct platform_driver gpio_poweroff_driver = {
 	.remove = gpio_poweroff_remove,
 	.driver = {
 		.name = "poweroff-gpio",
-		.owner = THIS_MODULE,
 		.of_match_table = of_gpio_poweroff_match,
 	},
 };
diff --git a/drivers/power/reset/gpio-restart.c b/drivers/power/reset/gpio-restart.c
index a76829b3f1cd..edb327efee8b 100644
--- a/drivers/power/reset/gpio-restart.c
+++ b/drivers/power/reset/gpio-restart.c
@@ -137,7 +137,6 @@ static struct platform_driver gpio_restart_driver = {
 	.remove = gpio_restart_remove,
 	.driver = {
 		.name = "restart-gpio",
-		.owner = THIS_MODULE,
 		.of_match_table = of_gpio_restart_match,
 	},
 };
diff --git a/drivers/power/reset/hisi-reboot.c b/drivers/power/reset/hisi-reboot.c
index 0c91d0231d36..5385460e23bb 100644
--- a/drivers/power/reset/hisi-reboot.c
+++ b/drivers/power/reset/hisi-reboot.c
@@ -14,27 +14,36 @@
 #include <linux/delay.h>
 #include <linux/io.h>
 #include <linux/module.h>
+#include <linux/notifier.h>
 #include <linux/of_address.h>
 #include <linux/platform_device.h>
 #include <linux/reboot.h>
 
 #include <asm/proc-fns.h>
-#include <asm/system_misc.h>
 
 static void __iomem *base;
 static u32 reboot_offset;
 
-static void hisi_restart(enum reboot_mode mode, const char *cmd)
+static int hisi_restart_handler(struct notifier_block *this,
+				unsigned long mode, void *cmd)
 {
 	writel_relaxed(0xdeadbeef, base + reboot_offset);
 
 	while (1)
 		cpu_do_idle();
+
+	return NOTIFY_DONE;
 }
 
+static struct notifier_block hisi_restart_nb = {
+	.notifier_call = hisi_restart_handler,
+	.priority = 128,
+};
+
 static int hisi_reboot_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
+	int err;
 
 	base = of_iomap(np, 0);
 	if (!base) {
@@ -47,9 +56,12 @@ static int hisi_reboot_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	arm_pm_restart = hisi_restart;
+	err = register_restart_handler(&hisi_restart_nb);
+	if (err)
+		dev_err(&pdev->dev, "cannot register restart handler (err=%d)\n",
+			err);
 
-	return 0;
+	return err;
 }
 
 static struct of_device_id hisi_reboot_of_match[] = {
diff --git a/drivers/power/reset/keystone-reset.c b/drivers/power/reset/keystone-reset.c
index 408a18fd91cb..faedf16c8111 100644
--- a/drivers/power/reset/keystone-reset.c
+++ b/drivers/power/reset/keystone-reset.c
@@ -12,9 +12,9 @@
 
 #include <linux/io.h>
 #include <linux/module.h>
+#include <linux/notifier.h>
 #include <linux/reboot.h>
 #include <linux/regmap.h>
-#include <asm/system_misc.h>
 #include <linux/mfd/syscon.h>
 #include <linux/of_platform.h>
 
@@ -52,7 +52,8 @@ static inline int rsctrl_enable_rspll_write(void)
 				  RSCTRL_KEY_MASK, RSCTRL_KEY);
 }
 
-static void rsctrl_restart(enum reboot_mode mode, const char *cmd)
+static int rsctrl_restart_handler(struct notifier_block *this,
+				  unsigned long mode, void *cmd)
 {
 	/* enable write access to RSTCTRL */
 	rsctrl_enable_rspll_write();
@@ -60,8 +61,15 @@ static void rsctrl_restart(enum reboot_mode mode, const char *cmd)
 	/* reset the SOC */
 	regmap_update_bits(pllctrl_regs, rspll_offset + RSCTRL_RG,
 			   RSCTRL_RESET_MASK, 0);
+
+	return NOTIFY_DONE;
 }
 
+static struct notifier_block rsctrl_restart_nb = {
+	.notifier_call = rsctrl_restart_handler,
+	.priority = 128,
+};
+
 static struct of_device_id rsctrl_of_match[] = {
 	{.compatible = "ti,keystone-reset", },
 	{},
@@ -114,8 +122,6 @@ static int rsctrl_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	arm_pm_restart = rsctrl_restart;
-
 	/* disable a reset isolation for all module clocks */
 	ret = regmap_write(pllctrl_regs, rspll_offset + RSISO_RG, 0);
 	if (ret)
@@ -147,13 +153,16 @@ static int rsctrl_probe(struct platform_device *pdev)
 			return ret;
 	}
 
-	return 0;
+	ret = register_restart_handler(&rsctrl_restart_nb);
+	if (ret)
+		dev_err(dev, "cannot register restart handler (err=%d)\n", ret);
+
+	return ret;
 }
 
 static struct platform_driver rsctrl_driver = {
 	.probe = rsctrl_probe,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = KBUILD_MODNAME,
 		.of_match_table = rsctrl_of_match,
 	},
diff --git a/drivers/power/reset/ltc2952-poweroff.c b/drivers/power/reset/ltc2952-poweroff.c
index 116a1cef8f7b..34f38a3dc3ff 100644
--- a/drivers/power/reset/ltc2952-poweroff.c
+++ b/drivers/power/reset/ltc2952-poweroff.c
@@ -342,7 +342,6 @@ static struct platform_driver ltc2952_poweroff_driver = {
 	.remove = ltc2952_poweroff_remove,
 	.driver = {
 		.name = "ltc2952-poweroff",
-		.owner = THIS_MODULE,
 		.of_match_table = of_ltc2952_poweroff_match,
 	},
 	.suspend = ltc2952_poweroff_suspend,
diff --git a/drivers/power/reset/qnap-poweroff.c b/drivers/power/reset/qnap-poweroff.c
index a75db7f8a92f..2789a61cec68 100644
--- a/drivers/power/reset/qnap-poweroff.c
+++ b/drivers/power/reset/qnap-poweroff.c
@@ -129,7 +129,6 @@ static struct platform_driver qnap_power_off_driver = {
 	.probe	= qnap_power_off_probe,
 	.remove	= qnap_power_off_remove,
 	.driver	= {
-		.owner	= THIS_MODULE,
 		.name	= "qnap_power_off",
 		.of_match_table = of_match_ptr(qnap_power_off_of_match_table),
 	},
diff --git a/drivers/power/reset/restart-poweroff.c b/drivers/power/reset/restart-poweroff.c
index edd707ee7281..f46f2c2e4648 100644
--- a/drivers/power/reset/restart-poweroff.c
+++ b/drivers/power/reset/restart-poweroff.c
@@ -55,7 +55,6 @@ static struct platform_driver restart_poweroff_driver = {
 	.remove = restart_poweroff_remove,
 	.driver = {
 		.name = "poweroff-restart",
-		.owner = THIS_MODULE,
 		.of_match_table = of_restart_poweroff_match,
 	},
 };
diff --git a/drivers/power/reset/syscon-reboot.c b/drivers/power/reset/syscon-reboot.c
index 815b901822cf..c4049f45663f 100644
--- a/drivers/power/reset/syscon-reboot.c
+++ b/drivers/power/reset/syscon-reboot.c
@@ -68,7 +68,7 @@ static int syscon_reboot_probe(struct platform_device *pdev)
 		return -EINVAL;
 
 	ctx->restart_handler.notifier_call = syscon_restart_handle;
-	ctx->restart_handler.priority = 128;
+	ctx->restart_handler.priority = 192;
 	err = register_restart_handler(&ctx->restart_handler);
 	if (err)
 		dev_err(dev, "can't register restart notifier (err=%d)\n", err);
diff --git a/drivers/power/reset/vexpress-poweroff.c b/drivers/power/reset/vexpress-poweroff.c
index 4dc102e2b230..9dfc9cee3232 100644
--- a/drivers/power/reset/vexpress-poweroff.c
+++ b/drivers/power/reset/vexpress-poweroff.c
@@ -12,14 +12,14 @@
  */
 
 #include <linux/delay.h>
+#include <linux/notifier.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
 #include <linux/platform_device.h>
+#include <linux/reboot.h>
 #include <linux/stat.h>
 #include <linux/vexpress.h>
 
-#include <asm/system_misc.h>
-
 static void vexpress_reset_do(struct device *dev, const char *what)
 {
 	int err = -ENOENT;
@@ -43,11 +43,19 @@ static void vexpress_power_off(void)
 
 static struct device *vexpress_restart_device;
 
-static void vexpress_restart(enum reboot_mode reboot_mode, const char *cmd)
+static int vexpress_restart(struct notifier_block *this, unsigned long mode,
+			     void *cmd)
 {
 	vexpress_reset_do(vexpress_restart_device, "restart");
+
+	return NOTIFY_DONE;
 }
 
+static struct notifier_block vexpress_restart_nb = {
+	.notifier_call = vexpress_restart,
+	.priority = 128,
+};
+
 static ssize_t vexpress_reset_active_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
@@ -86,12 +94,28 @@ static struct of_device_id vexpress_reset_of_match[] = {
 	{}
 };
 
+static int _vexpress_register_restart_handler(struct device *dev)
+{
+	int err;
+
+	vexpress_restart_device = dev;
+	err = register_restart_handler(&vexpress_restart_nb);
+	if (err) {
+		dev_err(dev, "cannot register restart handler (err=%d)\n", err);
+		return err;
+	}
+	device_create_file(dev, &dev_attr_active);
+
+	return 0;
+}
+
 static int vexpress_reset_probe(struct platform_device *pdev)
 {
 	enum vexpress_reset_func func;
 	const struct of_device_id *match =
 			of_match_device(vexpress_reset_of_match, &pdev->dev);
 	struct regmap *regmap;
+	int ret = 0;
 
 	if (match)
 		func = (enum vexpress_reset_func)match->data;
@@ -110,18 +134,14 @@ static int vexpress_reset_probe(struct platform_device *pdev)
 		break;
 	case FUNC_RESET:
 		if (!vexpress_restart_device)
-			vexpress_restart_device = &pdev->dev;
-		arm_pm_restart = vexpress_restart;
-		device_create_file(&pdev->dev, &dev_attr_active);
+			ret = _vexpress_register_restart_handler(&pdev->dev);
 		break;
 	case FUNC_REBOOT:
-		vexpress_restart_device = &pdev->dev;
-		arm_pm_restart = vexpress_restart;
-		device_create_file(&pdev->dev, &dev_attr_active);
+		ret = _vexpress_register_restart_handler(&pdev->dev);
 		break;
 	};
 
-	return 0;
+	return ret;
 }
 
 static const struct platform_device_id vexpress_reset_id_table[] = {
diff --git a/drivers/power/reset/xgene-reboot.c b/drivers/power/reset/xgene-reboot.c
index 6b49be6867ab..b0e5002f8deb 100644
--- a/drivers/power/reset/xgene-reboot.c
+++ b/drivers/power/reset/xgene-reboot.c
@@ -24,63 +24,67 @@
  * For system shutdown, this is board specify. If a board designer
  * implements GPIO shutdown, use the gpio-poweroff.c driver.
  */
+#include <linux/delay.h>
 #include <linux/io.h>
+#include <linux/notifier.h>
 #include <linux/of_device.h>
 #include <linux/of_address.h>
 #include <linux/platform_device.h>
+#include <linux/reboot.h>
 #include <linux/stat.h>
 #include <linux/slab.h>
-#include <asm/system_misc.h>
 
 struct xgene_reboot_context {
-	struct platform_device *pdev;
+	struct device *dev;
 	void *csr;
 	u32 mask;
+	struct notifier_block restart_handler;
 };
 
-static struct xgene_reboot_context *xgene_restart_ctx;
-
-static void xgene_restart(enum reboot_mode mode, const char *cmd)
+static int xgene_restart_handler(struct notifier_block *this,
+				 unsigned long mode, void *cmd)
 {
-	struct xgene_reboot_context *ctx = xgene_restart_ctx;
-	unsigned long timeout;
+	struct xgene_reboot_context *ctx =
+		container_of(this, struct xgene_reboot_context,
+			     restart_handler);
 
 	/* Issue the reboot */
-	if (ctx)
-		writel(ctx->mask, ctx->csr);
+	writel(ctx->mask, ctx->csr);
+
+	mdelay(1000);
 
-	timeout = jiffies + HZ;
-	while (time_before(jiffies, timeout))
-		cpu_relax();
+	dev_emerg(ctx->dev, "Unable to restart system\n");
 
-	dev_emerg(&ctx->pdev->dev, "Unable to restart system\n");
+	return NOTIFY_DONE;
 }
 
 static int xgene_reboot_probe(struct platform_device *pdev)
 {
 	struct xgene_reboot_context *ctx;
+	struct device *dev = &pdev->dev;
+	int err;
 
-	ctx = devm_kzalloc(&pdev->dev, sizeof(*ctx), GFP_KERNEL);
-	if (!ctx) {
-		dev_err(&pdev->dev, "out of memory for context\n");
-		return -ENODEV;
-	}
+	ctx = devm_kzalloc(dev, sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
 
-	ctx->csr = of_iomap(pdev->dev.of_node, 0);
+	ctx->csr = of_iomap(dev->of_node, 0);
 	if (!ctx->csr) {
-		devm_kfree(&pdev->dev, ctx);
-		dev_err(&pdev->dev, "can not map resource\n");
+		dev_err(dev, "can not map resource\n");
 		return -ENODEV;
 	}
 
-	if (of_property_read_u32(pdev->dev.of_node, "mask", &ctx->mask))
+	if (of_property_read_u32(dev->of_node, "mask", &ctx->mask))
 		ctx->mask = 0xFFFFFFFF;
 
-	ctx->pdev = pdev;
-	arm_pm_restart = xgene_restart;
-	xgene_restart_ctx = ctx;
+	ctx->dev = dev;
+	ctx->restart_handler.notifier_call = xgene_restart_handler;
+	ctx->restart_handler.priority = 128;
+	err = register_restart_handler(&ctx->restart_handler);
+	if (err)
+		dev_err(dev, "cannot register restart handler (err=%d)\n", err);
 
-	return 0;
+	return err;
 }
 
 static struct of_device_id xgene_reboot_of_match[] = {
diff --git a/drivers/power/rx51_battery.c b/drivers/power/rx51_battery.c
index d5a2acfb8821..a01aacb32f59 100644
--- a/drivers/power/rx51_battery.c
+++ b/drivers/power/rx51_battery.c
@@ -281,7 +281,6 @@ static struct platform_driver rx51_battery_driver = {
 	.remove = rx51_battery_remove,
 	.driver = {
 		.name = "rx51-battery",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(n900_battery_of_match),
 	},
 };
diff --git a/drivers/power/tps65090-charger.c b/drivers/power/tps65090-charger.c
index 3e8ba97c8169..0f4e5971dff5 100644
--- a/drivers/power/tps65090-charger.c
+++ b/drivers/power/tps65090-charger.c
@@ -353,7 +353,6 @@ static struct platform_driver tps65090_charger_driver = {
 	.driver	= {
 		.name	= "tps65090-charger",
 		.of_match_table = of_tps65090_charger_match,
-		.owner	= THIS_MODULE,
 	},
 	.probe	= tps65090_charger_probe,
 	.remove = tps65090_charger_remove,
diff --git a/drivers/power/twl4030_charger.c b/drivers/power/twl4030_charger.c
index 2598c588006e..d35b83e635b5 100644
--- a/drivers/power/twl4030_charger.c
+++ b/drivers/power/twl4030_charger.c
@@ -716,7 +716,6 @@ MODULE_DEVICE_TABLE(of, twl_bci_of_match);
 static struct platform_driver twl4030_bci_driver = {
 	.driver	= {
 		.name	= "twl4030_bci",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(twl_bci_of_match),
 	},
 	.remove	= __exit_p(twl4030_bci_remove),
diff --git a/drivers/power/wm97xx_battery.c b/drivers/power/wm97xx_battery.c
index 58f7348e6c22..a8e6203673ad 100644
--- a/drivers/power/wm97xx_battery.c
+++ b/drivers/power/wm97xx_battery.c
@@ -281,7 +281,6 @@ static int wm97xx_bat_remove(struct platform_device *dev)
 static struct platform_driver wm97xx_bat_driver = {
 	.driver	= {
 		.name	= "wm97xx-battery",
-		.owner	= THIS_MODULE,
 #ifdef CONFIG_PM
 		.pm	= &wm97xx_bat_pm_ops,
 #endif
diff --git a/drivers/powercap/intel_rapl.c b/drivers/powercap/intel_rapl.c
index c71443c4f265..97b5e4ee1ca4 100644
--- a/drivers/powercap/intel_rapl.c
+++ b/drivers/powercap/intel_rapl.c
@@ -1041,6 +1041,7 @@ static const struct x86_cpu_id rapl_ids[] = {
 	RAPL_CPU(0x45, rapl_defaults_core),/* Haswell ULT */
 	RAPL_CPU(0x4C, rapl_defaults_atom),/* Braswell */
 	RAPL_CPU(0x4A, rapl_defaults_atom),/* Tangier */
+	RAPL_CPU(0x56, rapl_defaults_core),/* Future Xeon */
 	RAPL_CPU(0x5A, rapl_defaults_atom),/* Annidale */
 	{}
 };
diff --git a/drivers/pps/clients/pps-gpio.c b/drivers/pps/clients/pps-gpio.c
index f41bacfdc3dc..333ad7d5b45b 100644
--- a/drivers/pps/clients/pps-gpio.c
+++ b/drivers/pps/clients/pps-gpio.c
@@ -200,7 +200,6 @@ static struct platform_driver pps_gpio_driver = {
 	.remove		= pps_gpio_remove,
 	.driver		= {
 		.name	= PPS_GPIO_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table	= pps_gpio_dt_ids,
 	},
 };
diff --git a/drivers/pwm/Kconfig b/drivers/pwm/Kconfig
index ef2dd2e4754b..a3ecf5809634 100644
--- a/drivers/pwm/Kconfig
+++ b/drivers/pwm/Kconfig
@@ -50,6 +50,17 @@ config PWM_ATMEL
 	  To compile this driver as a module, choose M here: the module
 	  will be called pwm-atmel.
 
+config PWM_ATMEL_HLCDC_PWM
+	tristate "Atmel HLCDC PWM support"
+	depends on MFD_ATMEL_HLCDC
+	help
+	  Generic PWM framework driver for the PWM output of the HLCDC
+	  (Atmel High-end LCD Controller). This PWM output is mainly used
+	  to control the LCD backlight.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pwm-atmel-hlcdc.
+
 config PWM_ATMEL_TCB
 	tristate "Atmel TC Block PWM support"
 	depends on ATMEL_TCLIB && OF
@@ -71,6 +82,15 @@ config PWM_BCM_KONA
 	  To compile this driver as a module, choose M here: the module
 	  will be called pwm-bcm-kona.
 
+config PWM_BCM2835
+	tristate "BCM2835 PWM support"
+	depends on ARCH_BCM2835
+	help
+	  PWM framework driver for BCM2835 controller (Raspberry Pi)
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pwm-bcm2835.
+
 config PWM_BFIN
 	tristate "Blackfin PWM support"
 	depends on BFIN_GPTIMERS
@@ -235,7 +255,7 @@ config PWM_ROCKCHIP
 
 config PWM_SAMSUNG
 	tristate "Samsung PWM support"
-	depends on PLAT_SAMSUNG
+	depends on PLAT_SAMSUNG || ARCH_EXYNOS
 	help
 	  Generic PWM framework driver for Samsung.
 
diff --git a/drivers/pwm/Makefile b/drivers/pwm/Makefile
index c458606c3755..65259ac1e8de 100644
--- a/drivers/pwm/Makefile
+++ b/drivers/pwm/Makefile
@@ -2,8 +2,10 @@ obj-$(CONFIG_PWM)		+= core.o
 obj-$(CONFIG_PWM_SYSFS)		+= sysfs.o
 obj-$(CONFIG_PWM_AB8500)	+= pwm-ab8500.o
 obj-$(CONFIG_PWM_ATMEL)		+= pwm-atmel.o
+obj-$(CONFIG_PWM_ATMEL_HLCDC_PWM)	+= pwm-atmel-hlcdc.o
 obj-$(CONFIG_PWM_ATMEL_TCB)	+= pwm-atmel-tcb.o
 obj-$(CONFIG_PWM_BCM_KONA)	+= pwm-bcm-kona.o
+obj-$(CONFIG_PWM_BCM2835)	+= pwm-bcm2835.o
 obj-$(CONFIG_PWM_BFIN)		+= pwm-bfin.o
 obj-$(CONFIG_PWM_CLPS711X)	+= pwm-clps711x.o
 obj-$(CONFIG_PWM_EP93XX)	+= pwm-ep93xx.o
diff --git a/drivers/pwm/pwm-ab8500.c b/drivers/pwm/pwm-ab8500.c
index 4c07a8420b37..f39399273426 100644
--- a/drivers/pwm/pwm-ab8500.c
+++ b/drivers/pwm/pwm-ab8500.c
@@ -131,7 +131,6 @@ static int ab8500_pwm_remove(struct platform_device *pdev)
 static struct platform_driver ab8500_pwm_driver = {
 	.driver = {
 		.name = "ab8500-pwm",
-		.owner = THIS_MODULE,
 	},
 	.probe = ab8500_pwm_probe,
 	.remove = ab8500_pwm_remove,
diff --git a/drivers/pwm/pwm-atmel-hlcdc.c b/drivers/pwm/pwm-atmel-hlcdc.c
new file mode 100644
index 000000000000..e7a785fadcdf
--- /dev/null
+++ b/drivers/pwm/pwm-atmel-hlcdc.c
@@ -0,0 +1,299 @@
+/*
+ * Copyright (C) 2014 Free Electrons
+ * Copyright (C) 2014 Atmel
+ *
+ * Author: Boris BREZILLON <boris.brezillon@free-electrons.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/mfd/atmel-hlcdc.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pwm.h>
+#include <linux/regmap.h>
+
+#define ATMEL_HLCDC_PWMCVAL_MASK	GENMASK(15, 8)
+#define ATMEL_HLCDC_PWMCVAL(x)		(((x) << 8) & ATMEL_HLCDC_PWMCVAL_MASK)
+#define ATMEL_HLCDC_PWMPOL		BIT(4)
+#define ATMEL_HLCDC_PWMPS_MASK		GENMASK(2, 0)
+#define ATMEL_HLCDC_PWMPS_MAX		0x6
+#define ATMEL_HLCDC_PWMPS(x)		((x) & ATMEL_HLCDC_PWMPS_MASK)
+
+struct atmel_hlcdc_pwm_errata {
+	bool slow_clk_erratum;
+	bool div1_clk_erratum;
+};
+
+struct atmel_hlcdc_pwm {
+	struct pwm_chip chip;
+	struct atmel_hlcdc *hlcdc;
+	struct clk *cur_clk;
+	const struct atmel_hlcdc_pwm_errata *errata;
+};
+
+static inline struct atmel_hlcdc_pwm *to_atmel_hlcdc_pwm(struct pwm_chip *chip)
+{
+	return container_of(chip, struct atmel_hlcdc_pwm, chip);
+}
+
+static int atmel_hlcdc_pwm_config(struct pwm_chip *c,
+				  struct pwm_device *pwm,
+				  int duty_ns, int period_ns)
+{
+	struct atmel_hlcdc_pwm *chip = to_atmel_hlcdc_pwm(c);
+	struct atmel_hlcdc *hlcdc = chip->hlcdc;
+	struct clk *new_clk = hlcdc->slow_clk;
+	u64 pwmcval = duty_ns * 256;
+	unsigned long clk_freq;
+	u64 clk_period_ns;
+	u32 pwmcfg;
+	int pres;
+
+	if (!chip->errata || !chip->errata->slow_clk_erratum) {
+		clk_freq = clk_get_rate(new_clk);
+		clk_period_ns = (u64)NSEC_PER_SEC * 256;
+		do_div(clk_period_ns, clk_freq);
+	}
+
+	/* Errata: cannot use slow clk on some IP revisions */
+	if ((chip->errata && chip->errata->slow_clk_erratum) ||
+	    clk_period_ns > period_ns) {
+		new_clk = hlcdc->sys_clk;
+		clk_freq = clk_get_rate(new_clk);
+		clk_period_ns = (u64)NSEC_PER_SEC * 256;
+		do_div(clk_period_ns, clk_freq);
+	}
+
+	for (pres = 0; pres <= ATMEL_HLCDC_PWMPS_MAX; pres++) {
+		/* Errata: cannot divide by 1 on some IP revisions */
+		if (!pres && chip->errata && chip->errata->div1_clk_erratum)
+			continue;
+
+		if ((clk_period_ns << pres) >= period_ns)
+			break;
+	}
+
+	if (pres > ATMEL_HLCDC_PWMPS_MAX)
+		return -EINVAL;
+
+	pwmcfg = ATMEL_HLCDC_PWMPS(pres);
+
+	if (new_clk != chip->cur_clk) {
+		u32 gencfg = 0;
+		int ret;
+
+		ret = clk_prepare_enable(new_clk);
+		if (ret)
+			return ret;
+
+		clk_disable_unprepare(chip->cur_clk);
+		chip->cur_clk = new_clk;
+
+		if (new_clk == hlcdc->sys_clk)
+			gencfg = ATMEL_HLCDC_CLKPWMSEL;
+
+		ret = regmap_update_bits(hlcdc->regmap, ATMEL_HLCDC_CFG(0),
+					 ATMEL_HLCDC_CLKPWMSEL, gencfg);
+		if (ret)
+			return ret;
+	}
+
+	do_div(pwmcval, period_ns);
+
+	/*
+	 * The PWM duty cycle is configurable from 0/256 to 255/256 of the
+	 * period cycle. Hence we can't set a duty cycle occupying the
+	 * whole period cycle if we're asked to.
+	 * Set it to 255 if pwmcval is greater than 256.
+	 */
+	if (pwmcval > 255)
+		pwmcval = 255;
+
+	pwmcfg |= ATMEL_HLCDC_PWMCVAL(pwmcval);
+
+	return regmap_update_bits(hlcdc->regmap, ATMEL_HLCDC_CFG(6),
+				  ATMEL_HLCDC_PWMCVAL_MASK |
+				  ATMEL_HLCDC_PWMPS_MASK,
+				  pwmcfg);
+}
+
+static int atmel_hlcdc_pwm_set_polarity(struct pwm_chip *c,
+					struct pwm_device *pwm,
+					enum pwm_polarity polarity)
+{
+	struct atmel_hlcdc_pwm *chip = to_atmel_hlcdc_pwm(c);
+	struct atmel_hlcdc *hlcdc = chip->hlcdc;
+	u32 cfg = 0;
+
+	if (polarity == PWM_POLARITY_NORMAL)
+		cfg = ATMEL_HLCDC_PWMPOL;
+
+	return regmap_update_bits(hlcdc->regmap, ATMEL_HLCDC_CFG(6),
+				  ATMEL_HLCDC_PWMPOL, cfg);
+}
+
+static int atmel_hlcdc_pwm_enable(struct pwm_chip *c, struct pwm_device *pwm)
+{
+	struct atmel_hlcdc_pwm *chip = to_atmel_hlcdc_pwm(c);
+	struct atmel_hlcdc *hlcdc = chip->hlcdc;
+	u32 status;
+	int ret;
+
+	ret = regmap_write(hlcdc->regmap, ATMEL_HLCDC_EN, ATMEL_HLCDC_PWM);
+	if (ret)
+		return ret;
+
+	while (true) {
+		ret = regmap_read(hlcdc->regmap, ATMEL_HLCDC_SR, &status);
+		if (ret)
+			return ret;
+
+		if ((status & ATMEL_HLCDC_PWM) != 0)
+			break;
+
+		usleep_range(1, 10);
+	}
+
+	return 0;
+}
+
+static void atmel_hlcdc_pwm_disable(struct pwm_chip *c,
+				    struct pwm_device *pwm)
+{
+	struct atmel_hlcdc_pwm *chip = to_atmel_hlcdc_pwm(c);
+	struct atmel_hlcdc *hlcdc = chip->hlcdc;
+	u32 status;
+	int ret;
+
+	ret = regmap_write(hlcdc->regmap, ATMEL_HLCDC_DIS, ATMEL_HLCDC_PWM);
+	if (ret)
+		return;
+
+	while (true) {
+		ret = regmap_read(hlcdc->regmap, ATMEL_HLCDC_SR, &status);
+		if (ret)
+			return;
+
+		if ((status & ATMEL_HLCDC_PWM) == 0)
+			break;
+
+		usleep_range(1, 10);
+	}
+}
+
+static const struct pwm_ops atmel_hlcdc_pwm_ops = {
+	.config = atmel_hlcdc_pwm_config,
+	.set_polarity = atmel_hlcdc_pwm_set_polarity,
+	.enable = atmel_hlcdc_pwm_enable,
+	.disable = atmel_hlcdc_pwm_disable,
+	.owner = THIS_MODULE,
+};
+
+static const struct atmel_hlcdc_pwm_errata atmel_hlcdc_pwm_at91sam9x5_errata = {
+	.slow_clk_erratum = true,
+};
+
+static const struct atmel_hlcdc_pwm_errata atmel_hlcdc_pwm_sama5d3_errata = {
+	.div1_clk_erratum = true,
+};
+
+static const struct of_device_id atmel_hlcdc_dt_ids[] = {
+	{
+		.compatible = "atmel,at91sam9x5-hlcdc",
+		.data = &atmel_hlcdc_pwm_at91sam9x5_errata,
+	},
+	{
+		.compatible = "atmel,sama5d3-hlcdc",
+		.data = &atmel_hlcdc_pwm_sama5d3_errata,
+	},
+	{ /* sentinel */ },
+};
+
+static int atmel_hlcdc_pwm_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match;
+	struct device *dev = &pdev->dev;
+	struct atmel_hlcdc_pwm *chip;
+	struct atmel_hlcdc *hlcdc;
+	int ret;
+
+	hlcdc = dev_get_drvdata(dev->parent);
+
+	chip = devm_kzalloc(dev, sizeof(*chip), GFP_KERNEL);
+	if (!chip)
+		return -ENOMEM;
+
+	ret = clk_prepare_enable(hlcdc->periph_clk);
+	if (ret)
+		return ret;
+
+	match = of_match_node(atmel_hlcdc_dt_ids, dev->parent->of_node);
+	if (match)
+		chip->errata = match->data;
+
+	chip->hlcdc = hlcdc;
+	chip->chip.ops = &atmel_hlcdc_pwm_ops;
+	chip->chip.dev = dev;
+	chip->chip.base = -1;
+	chip->chip.npwm = 1;
+	chip->chip.of_xlate = of_pwm_xlate_with_flags;
+	chip->chip.of_pwm_n_cells = 3;
+	chip->chip.can_sleep = 1;
+
+	ret = pwmchip_add(&chip->chip);
+	if (ret) {
+		clk_disable_unprepare(hlcdc->periph_clk);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, chip);
+
+	return 0;
+}
+
+static int atmel_hlcdc_pwm_remove(struct platform_device *pdev)
+{
+	struct atmel_hlcdc_pwm *chip = platform_get_drvdata(pdev);
+	int ret;
+
+	ret = pwmchip_remove(&chip->chip);
+	if (ret)
+		return ret;
+
+	clk_disable_unprepare(chip->hlcdc->periph_clk);
+
+	return 0;
+}
+
+static const struct of_device_id atmel_hlcdc_pwm_dt_ids[] = {
+	{ .compatible = "atmel,hlcdc-pwm" },
+	{ /* sentinel */ },
+};
+
+static struct platform_driver atmel_hlcdc_pwm_driver = {
+	.driver = {
+		.name = "atmel-hlcdc-pwm",
+		.of_match_table = atmel_hlcdc_pwm_dt_ids,
+	},
+	.probe = atmel_hlcdc_pwm_probe,
+	.remove = atmel_hlcdc_pwm_remove,
+};
+module_platform_driver(atmel_hlcdc_pwm_driver);
+
+MODULE_ALIAS("platform:atmel-hlcdc-pwm");
+MODULE_AUTHOR("Boris Brezillon <boris.brezillon@free-electrons.com>");
+MODULE_DESCRIPTION("Atmel HLCDC PWM driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pwm/pwm-atmel-tcb.c b/drivers/pwm/pwm-atmel-tcb.c
index d56e5b717431..d14e0677c92d 100644
--- a/drivers/pwm/pwm-atmel-tcb.c
+++ b/drivers/pwm/pwm-atmel-tcb.c
@@ -436,7 +436,6 @@ MODULE_DEVICE_TABLE(of, atmel_tcb_pwm_dt_ids);
 static struct platform_driver atmel_tcb_pwm_driver = {
 	.driver = {
 		.name = "atmel-tcb-pwm",
-		.owner = THIS_MODULE,
 		.of_match_table = atmel_tcb_pwm_dt_ids,
 	},
 	.probe = atmel_tcb_pwm_probe,
diff --git a/drivers/pwm/pwm-bcm2835.c b/drivers/pwm/pwm-bcm2835.c
new file mode 100644
index 000000000000..b4c7f956b6fa
--- /dev/null
+++ b/drivers/pwm/pwm-bcm2835.c
@@ -0,0 +1,205 @@
+/*
+ * Copyright 2014 Bart Tanghe <bart.tanghe@thomasmore.be>
+ *
+ * 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/clk.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/pwm.h>
+
+#define PWM_CONTROL		0x000
+#define PWM_CONTROL_SHIFT(x)	((x) * 8)
+#define PWM_CONTROL_MASK	0xff
+#define PWM_MODE		0x80		/* set timer in PWM mode */
+#define PWM_ENABLE		(1 << 0)
+#define PWM_POLARITY		(1 << 4)
+
+#define PERIOD(x)		(((x) * 0x10) + 0x10)
+#define DUTY(x)			(((x) * 0x10) + 0x14)
+
+#define MIN_PERIOD		108		/* 9.2 MHz max. PWM clock */
+
+struct bcm2835_pwm {
+	struct pwm_chip chip;
+	struct device *dev;
+	unsigned long scaler;
+	void __iomem *base;
+	struct clk *clk;
+};
+
+static inline struct bcm2835_pwm *to_bcm2835_pwm(struct pwm_chip *chip)
+{
+	return container_of(chip, struct bcm2835_pwm, chip);
+}
+
+static int bcm2835_pwm_request(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct bcm2835_pwm *pc = to_bcm2835_pwm(chip);
+	u32 value;
+
+	value = readl(pc->base + PWM_CONTROL);
+	value &= ~(PWM_CONTROL_MASK << PWM_CONTROL_SHIFT(pwm->hwpwm));
+	value |= (PWM_MODE << PWM_CONTROL_SHIFT(pwm->hwpwm));
+	writel(value, pc->base + PWM_CONTROL);
+
+	return 0;
+}
+
+static void bcm2835_pwm_free(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct bcm2835_pwm *pc = to_bcm2835_pwm(chip);
+	u32 value;
+
+	value = readl(pc->base + PWM_CONTROL);
+	value &= ~(PWM_CONTROL_MASK << PWM_CONTROL_SHIFT(pwm->hwpwm));
+	writel(value, pc->base + PWM_CONTROL);
+}
+
+static int bcm2835_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
+			      int duty_ns, int period_ns)
+{
+	struct bcm2835_pwm *pc = to_bcm2835_pwm(chip);
+
+	if (period_ns <= MIN_PERIOD) {
+		dev_err(pc->dev, "period %d not supported, minimum %d\n",
+			period_ns, MIN_PERIOD);
+		return -EINVAL;
+	}
+
+	writel(duty_ns / pc->scaler, pc->base + DUTY(pwm->hwpwm));
+	writel(period_ns / pc->scaler, pc->base + PERIOD(pwm->hwpwm));
+
+	return 0;
+}
+
+static int bcm2835_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct bcm2835_pwm *pc = to_bcm2835_pwm(chip);
+	u32 value;
+
+	value = readl(pc->base + PWM_CONTROL);
+	value |= PWM_ENABLE << PWM_CONTROL_SHIFT(pwm->hwpwm);
+	writel(value, pc->base + PWM_CONTROL);
+
+	return 0;
+}
+
+static void bcm2835_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct bcm2835_pwm *pc = to_bcm2835_pwm(chip);
+	u32 value;
+
+	value = readl(pc->base + PWM_CONTROL);
+	value &= ~(PWM_ENABLE << PWM_CONTROL_SHIFT(pwm->hwpwm));
+	writel(value, pc->base + PWM_CONTROL);
+}
+
+static int bcm2835_set_polarity(struct pwm_chip *chip, struct pwm_device *pwm,
+				enum pwm_polarity polarity)
+{
+	struct bcm2835_pwm *pc = to_bcm2835_pwm(chip);
+	u32 value;
+
+	value = readl(pc->base + PWM_CONTROL);
+
+	if (polarity == PWM_POLARITY_NORMAL)
+		value &= ~(PWM_POLARITY << PWM_CONTROL_SHIFT(pwm->hwpwm));
+	else
+		value |= PWM_POLARITY << PWM_CONTROL_SHIFT(pwm->hwpwm);
+
+	writel(value, pc->base + PWM_CONTROL);
+
+	return 0;
+}
+
+static const struct pwm_ops bcm2835_pwm_ops = {
+	.request = bcm2835_pwm_request,
+	.free = bcm2835_pwm_free,
+	.config = bcm2835_pwm_config,
+	.enable = bcm2835_pwm_enable,
+	.disable = bcm2835_pwm_disable,
+	.set_polarity = bcm2835_set_polarity,
+	.owner = THIS_MODULE,
+};
+
+static int bcm2835_pwm_probe(struct platform_device *pdev)
+{
+	struct bcm2835_pwm *pc;
+	struct resource *res;
+	int ret;
+
+	pc = devm_kzalloc(&pdev->dev, sizeof(*pc), GFP_KERNEL);
+	if (!pc)
+		return -ENOMEM;
+
+	pc->dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	pc->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(pc->base))
+		return PTR_ERR(pc->base);
+
+	pc->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(pc->clk)) {
+		dev_err(&pdev->dev, "clock not found: %ld\n", PTR_ERR(pc->clk));
+		return PTR_ERR(pc->clk);
+	}
+
+	ret = clk_prepare_enable(pc->clk);
+	if (ret)
+		return ret;
+
+	pc->scaler = NSEC_PER_SEC / clk_get_rate(pc->clk);
+
+	pc->chip.dev = &pdev->dev;
+	pc->chip.ops = &bcm2835_pwm_ops;
+	pc->chip.npwm = 2;
+
+	platform_set_drvdata(pdev, pc);
+
+	ret = pwmchip_add(&pc->chip);
+	if (ret < 0)
+		goto add_fail;
+
+	return 0;
+
+add_fail:
+	clk_disable_unprepare(pc->clk);
+	return ret;
+}
+
+static int bcm2835_pwm_remove(struct platform_device *pdev)
+{
+	struct bcm2835_pwm *pc = platform_get_drvdata(pdev);
+
+	clk_disable_unprepare(pc->clk);
+
+	return pwmchip_remove(&pc->chip);
+}
+
+static const struct of_device_id bcm2835_pwm_of_match[] = {
+	{ .compatible = "brcm,bcm2835-pwm", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, bcm2835_pwm_of_match);
+
+static struct platform_driver bcm2835_pwm_driver = {
+	.driver = {
+		.name = "bcm2835-pwm",
+		.of_match_table = bcm2835_pwm_of_match,
+	},
+	.probe = bcm2835_pwm_probe,
+	.remove = bcm2835_pwm_remove,
+};
+module_platform_driver(bcm2835_pwm_driver);
+
+MODULE_AUTHOR("Bart Tanghe <bart.tanghe@thomasmore.be");
+MODULE_DESCRIPTION("Broadcom BCM2835 PWM driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pwm/pwm-bfin.c b/drivers/pwm/pwm-bfin.c
index 9985d830e554..7631ef194de7 100644
--- a/drivers/pwm/pwm-bfin.c
+++ b/drivers/pwm/pwm-bfin.c
@@ -149,7 +149,6 @@ static int bfin_pwm_remove(struct platform_device *pdev)
 static struct platform_driver bfin_pwm_driver = {
 	.driver = {
 		.name = "bfin-pwm",
-		.owner = THIS_MODULE,
 	},
 	.probe = bfin_pwm_probe,
 	.remove = bfin_pwm_remove,
diff --git a/drivers/pwm/pwm-clps711x.c b/drivers/pwm/pwm-clps711x.c
index fafb6a0111b0..a80c10803636 100644
--- a/drivers/pwm/pwm-clps711x.c
+++ b/drivers/pwm/pwm-clps711x.c
@@ -163,7 +163,6 @@ MODULE_DEVICE_TABLE(of, clps711x_pwm_dt_ids);
 static struct platform_driver clps711x_pwm_driver = {
 	.driver = {
 		.name = "clps711x-pwm",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(clps711x_pwm_dt_ids),
 	},
 	.probe = clps711x_pwm_probe,
diff --git a/drivers/pwm/pwm-fsl-ftm.c b/drivers/pwm/pwm-fsl-ftm.c
index 0f2cc7ef7784..f9dfc8b6407a 100644
--- a/drivers/pwm/pwm-fsl-ftm.c
+++ b/drivers/pwm/pwm-fsl-ftm.c
@@ -17,6 +17,7 @@
 #include <linux/mutex.h>
 #include <linux/of_address.h>
 #include <linux/platform_device.h>
+#include <linux/pm.h>
 #include <linux/pwm.h>
 #include <linux/regmap.h>
 #include <linux/slab.h>
@@ -299,7 +300,7 @@ static int fsl_counter_clock_enable(struct fsl_pwm_chip *fpc)
 {
 	int ret;
 
-	if (fpc->use_count != 0)
+	if (fpc->use_count++ != 0)
 		return 0;
 
 	/* select counter clock source */
@@ -316,8 +317,6 @@ static int fsl_counter_clock_enable(struct fsl_pwm_chip *fpc)
 		return ret;
 	}
 
-	fpc->use_count++;
-
 	return 0;
 }
 
@@ -399,12 +398,23 @@ static int fsl_pwm_init(struct fsl_pwm_chip *fpc)
 	return 0;
 }
 
+static bool fsl_pwm_volatile_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case FTM_CNT:
+		return true;
+	}
+	return false;
+}
+
 static const struct regmap_config fsl_pwm_regmap_config = {
 	.reg_bits = 32,
 	.reg_stride = 4,
 	.val_bits = 32,
 
 	.max_register = FTM_PWMLOAD,
+	.volatile_reg = fsl_pwm_volatile_reg,
+	.cache_type = REGCACHE_RBTREE,
 };
 
 static int fsl_pwm_probe(struct platform_device *pdev)
@@ -427,7 +437,7 @@ static int fsl_pwm_probe(struct platform_device *pdev)
 	if (IS_ERR(base))
 		return PTR_ERR(base);
 
-	fpc->regmap = devm_regmap_init_mmio_clk(&pdev->dev, NULL, base,
+	fpc->regmap = devm_regmap_init_mmio_clk(&pdev->dev, "ftm_sys", base,
 						&fsl_pwm_regmap_config);
 	if (IS_ERR(fpc->regmap)) {
 		dev_err(&pdev->dev, "regmap init failed\n");
@@ -478,6 +488,51 @@ static int fsl_pwm_remove(struct platform_device *pdev)
 	return pwmchip_remove(&fpc->chip);
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int fsl_pwm_suspend(struct device *dev)
+{
+	struct fsl_pwm_chip *fpc = dev_get_drvdata(dev);
+	u32 val;
+
+	regcache_cache_only(fpc->regmap, true);
+	regcache_mark_dirty(fpc->regmap);
+
+	/* read from cache */
+	regmap_read(fpc->regmap, FTM_OUTMASK, &val);
+	if ((val & 0xFF) != 0xFF) {
+		clk_disable_unprepare(fpc->clk[FSL_PWM_CLK_CNTEN]);
+		clk_disable_unprepare(fpc->clk[fpc->cnt_select]);
+		clk_disable_unprepare(fpc->clk[FSL_PWM_CLK_SYS]);
+	}
+
+	return 0;
+}
+
+static int fsl_pwm_resume(struct device *dev)
+{
+	struct fsl_pwm_chip *fpc = dev_get_drvdata(dev);
+	u32 val;
+
+	/* read from cache */
+	regmap_read(fpc->regmap, FTM_OUTMASK, &val);
+	if ((val & 0xFF) != 0xFF) {
+		clk_prepare_enable(fpc->clk[FSL_PWM_CLK_SYS]);
+		clk_prepare_enable(fpc->clk[fpc->cnt_select]);
+		clk_prepare_enable(fpc->clk[FSL_PWM_CLK_CNTEN]);
+	}
+
+	/* restore all registers from cache */
+	regcache_cache_only(fpc->regmap, false);
+	regcache_sync(fpc->regmap);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops fsl_pwm_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(fsl_pwm_suspend, fsl_pwm_resume)
+};
+
 static const struct of_device_id fsl_pwm_dt_ids[] = {
 	{ .compatible = "fsl,vf610-ftm-pwm", },
 	{ /* sentinel */ }
@@ -488,6 +543,7 @@ static struct platform_driver fsl_pwm_driver = {
 	.driver = {
 		.name = "fsl-ftm-pwm",
 		.of_match_table = fsl_pwm_dt_ids,
+		.pm = &fsl_pwm_pm_ops,
 	},
 	.probe = fsl_pwm_probe,
 	.remove = fsl_pwm_remove,
diff --git a/drivers/pwm/pwm-imx.c b/drivers/pwm/pwm-imx.c
index f8b5f109c1ab..66d6f0c5c421 100644
--- a/drivers/pwm/pwm-imx.c
+++ b/drivers/pwm/pwm-imx.c
@@ -336,7 +336,6 @@ static int imx_pwm_remove(struct platform_device *pdev)
 static struct platform_driver imx_pwm_driver = {
 	.driver		= {
 		.name	= "imx-pwm",
-		.owner = THIS_MODULE,
 		.of_match_table = imx_pwm_dt_ids,
 	},
 	.probe		= imx_pwm_probe,
diff --git a/drivers/pwm/pwm-jz4740.c b/drivers/pwm/pwm-jz4740.c
index 9c46209e1d02..76d13150283f 100644
--- a/drivers/pwm/pwm-jz4740.c
+++ b/drivers/pwm/pwm-jz4740.c
@@ -194,7 +194,6 @@ static int jz4740_pwm_remove(struct platform_device *pdev)
 static struct platform_driver jz4740_pwm_driver = {
 	.driver = {
 		.name = "jz4740-pwm",
-		.owner = THIS_MODULE,
 	},
 	.probe = jz4740_pwm_probe,
 	.remove = jz4740_pwm_remove,
diff --git a/drivers/pwm/pwm-lp3943.c b/drivers/pwm/pwm-lp3943.c
index 2c39b0e50fa4..872ea76a4f19 100644
--- a/drivers/pwm/pwm-lp3943.c
+++ b/drivers/pwm/pwm-lp3943.c
@@ -305,7 +305,6 @@ static struct platform_driver lp3943_pwm_driver = {
 	.remove = lp3943_pwm_remove,
 	.driver = {
 		.name = "lp3943-pwm",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(lp3943_pwm_of_match),
 	},
 };
diff --git a/drivers/pwm/pwm-lpc32xx.c b/drivers/pwm/pwm-lpc32xx.c
index 9dc0f9d42bfa..9fde60ce8e7b 100644
--- a/drivers/pwm/pwm-lpc32xx.c
+++ b/drivers/pwm/pwm-lpc32xx.c
@@ -168,7 +168,6 @@ MODULE_DEVICE_TABLE(of, lpc32xx_pwm_dt_ids);
 static struct platform_driver lpc32xx_pwm_driver = {
 	.driver = {
 		.name = "lpc32xx-pwm",
-		.owner = THIS_MODULE,
 		.of_match_table = lpc32xx_pwm_dt_ids,
 	},
 	.probe = lpc32xx_pwm_probe,
diff --git a/drivers/pwm/pwm-mxs.c b/drivers/pwm/pwm-mxs.c
index 4f1bb4e0a426..f75ecb09d97d 100644
--- a/drivers/pwm/pwm-mxs.c
+++ b/drivers/pwm/pwm-mxs.c
@@ -189,7 +189,6 @@ MODULE_DEVICE_TABLE(of, mxs_pwm_dt_ids);
 static struct platform_driver mxs_pwm_driver = {
 	.driver = {
 		.name = "mxs-pwm",
-		.owner = THIS_MODULE,
 		.of_match_table = mxs_pwm_dt_ids,
 	},
 	.probe = mxs_pwm_probe,
diff --git a/drivers/pwm/pwm-puv3.c b/drivers/pwm/pwm-puv3.c
index a9a28083f245..ed6007b27585 100644
--- a/drivers/pwm/pwm-puv3.c
+++ b/drivers/pwm/pwm-puv3.c
@@ -146,7 +146,6 @@ static int pwm_remove(struct platform_device *pdev)
 static struct platform_driver puv3_pwm_driver = {
 	.driver = {
 		.name = "PKUnity-v3-PWM",
-		.owner = THIS_MODULE,
 	},
 	.probe = pwm_probe,
 	.remove = pwm_remove,
diff --git a/drivers/pwm/pwm-pxa.c b/drivers/pwm/pwm-pxa.c
index 0b312ec420b6..cb2f7024cf68 100644
--- a/drivers/pwm/pwm-pxa.c
+++ b/drivers/pwm/pwm-pxa.c
@@ -225,7 +225,6 @@ static int pwm_remove(struct platform_device *pdev)
 static struct platform_driver pwm_driver = {
 	.driver		= {
 		.name	= "pxa25x-pwm",
-		.owner	= THIS_MODULE,
 		.of_match_table = pwm_of_match,
 	},
 	.probe		= pwm_probe,
diff --git a/drivers/pwm/pwm-renesas-tpu.c b/drivers/pwm/pwm-renesas-tpu.c
index 3b71b42e89d5..ee63f9e9d0fb 100644
--- a/drivers/pwm/pwm-renesas-tpu.c
+++ b/drivers/pwm/pwm-renesas-tpu.c
@@ -468,7 +468,6 @@ static struct platform_driver tpu_driver = {
 	.remove		= tpu_remove,
 	.driver		= {
 		.name	= "renesas-tpu-pwm",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(tpu_of_table),
 	}
 };
diff --git a/drivers/pwm/pwm-samsung.c b/drivers/pwm/pwm-samsung.c
index ba6b650cf8dc..3e9b5835a4af 100644
--- a/drivers/pwm/pwm-samsung.c
+++ b/drivers/pwm/pwm-samsung.c
@@ -601,7 +601,6 @@ static SIMPLE_DEV_PM_OPS(pwm_samsung_pm_ops, pwm_samsung_suspend,
 static struct platform_driver pwm_samsung_driver = {
 	.driver		= {
 		.name	= "samsung-pwm",
-		.owner	= THIS_MODULE,
 		.pm	= &pwm_samsung_pm_ops,
 		.of_match_table = of_match_ptr(samsung_pwm_matches),
 	},
diff --git a/drivers/pwm/pwm-spear.c b/drivers/pwm/pwm-spear.c
index 6fd93e6a4122..6c6b44fd3f43 100644
--- a/drivers/pwm/pwm-spear.c
+++ b/drivers/pwm/pwm-spear.c
@@ -252,7 +252,6 @@ MODULE_DEVICE_TABLE(of, spear_pwm_of_match);
 static struct platform_driver spear_pwm_driver = {
 	.driver = {
 		.name = "spear-pwm",
-		.owner = THIS_MODULE,
 		.of_match_table = spear_pwm_of_match,
 	},
 	.probe = spear_pwm_probe,
diff --git a/drivers/pwm/pwm-tegra.c b/drivers/pwm/pwm-tegra.c
index 61d86b9498ca..5b97cae5423a 100644
--- a/drivers/pwm/pwm-tegra.c
+++ b/drivers/pwm/pwm-tegra.c
@@ -237,7 +237,6 @@ MODULE_DEVICE_TABLE(of, tegra_pwm_of_match);
 static struct platform_driver tegra_pwm_driver = {
 	.driver = {
 		.name = "tegra-pwm",
-		.owner = THIS_MODULE,
 		.of_match_table = tegra_pwm_of_match,
 	},
 	.probe = tegra_pwm_probe,
diff --git a/drivers/pwm/pwm-tiecap.c b/drivers/pwm/pwm-tiecap.c
index 74efbe7f20c3..e557befdf4e6 100644
--- a/drivers/pwm/pwm-tiecap.c
+++ b/drivers/pwm/pwm-tiecap.c
@@ -331,7 +331,6 @@ static SIMPLE_DEV_PM_OPS(ecap_pwm_pm_ops, ecap_pwm_suspend, ecap_pwm_resume);
 static struct platform_driver ecap_pwm_driver = {
 	.driver = {
 		.name	= "ecap",
-		.owner	= THIS_MODULE,
 		.of_match_table = ecap_of_match,
 		.pm	= &ecap_pwm_pm_ops,
 	},
diff --git a/drivers/pwm/pwm-tiehrpwm.c b/drivers/pwm/pwm-tiehrpwm.c
index cb75133085a8..694b3cf7694b 100644
--- a/drivers/pwm/pwm-tiehrpwm.c
+++ b/drivers/pwm/pwm-tiehrpwm.c
@@ -599,7 +599,6 @@ static SIMPLE_DEV_PM_OPS(ehrpwm_pwm_pm_ops, ehrpwm_pwm_suspend,
 static struct platform_driver ehrpwm_pwm_driver = {
 	.driver = {
 		.name	= "ehrpwm",
-		.owner	= THIS_MODULE,
 		.of_match_table = ehrpwm_of_match,
 		.pm	= &ehrpwm_pwm_pm_ops,
 	},
diff --git a/drivers/pwm/pwm-tipwmss.c b/drivers/pwm/pwm-tipwmss.c
index 67481dc6da3f..5cf65a15d021 100644
--- a/drivers/pwm/pwm-tipwmss.c
+++ b/drivers/pwm/pwm-tipwmss.c
@@ -119,7 +119,6 @@ static SIMPLE_DEV_PM_OPS(pwmss_pm_ops, pwmss_suspend, pwmss_resume);
 static struct platform_driver pwmss_driver = {
 	.driver	= {
 		.name	= "pwmss",
-		.owner	= THIS_MODULE,
 		.pm	= &pwmss_pm_ops,
 		.of_match_table	= pwmss_of_match,
 	},
diff --git a/drivers/pwm/pwm-vt8500.c b/drivers/pwm/pwm-vt8500.c
index 652e6b5b859b..cdb58fd4619d 100644
--- a/drivers/pwm/pwm-vt8500.c
+++ b/drivers/pwm/pwm-vt8500.c
@@ -266,7 +266,6 @@ static struct platform_driver vt8500_pwm_driver = {
 	.remove		= vt8500_pwm_remove,
 	.driver		= {
 		.name	= "vt8500-pwm",
-		.owner	= THIS_MODULE,
 		.of_match_table = vt8500_pwm_dt_ids,
 	},
 };
diff --git a/drivers/regulator/88pm800.c b/drivers/regulator/88pm800.c
index 4e6c8c611905..832932bdc977 100644
--- a/drivers/regulator/88pm800.c
+++ b/drivers/regulator/88pm800.c
@@ -362,7 +362,6 @@ static int pm800_regulator_remove(struct platform_device *pdev)
 static struct platform_driver pm800_regulator_driver = {
 	.driver		= {
 		.name	= "88pm80x-regulator",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= pm800_regulator_probe,
 	.remove		= pm800_regulator_remove,
diff --git a/drivers/regulator/88pm8607.c b/drivers/regulator/88pm8607.c
index 3fe47bd66153..c3d15427adc7 100644
--- a/drivers/regulator/88pm8607.c
+++ b/drivers/regulator/88pm8607.c
@@ -419,7 +419,6 @@ MODULE_DEVICE_TABLE(platform, pm8607_regulator_driver_ids);
 static struct platform_driver pm8607_regulator_driver = {
 	.driver		= {
 		.name	= "88pm860x-regulator",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= pm8607_regulator_probe,
 	.id_table	= pm8607_regulator_driver_ids,
diff --git a/drivers/regulator/aat2870-regulator.c b/drivers/regulator/aat2870-regulator.c
index c873ee0082cf..9dfabda8f478 100644
--- a/drivers/regulator/aat2870-regulator.c
+++ b/drivers/regulator/aat2870-regulator.c
@@ -191,7 +191,6 @@ static int aat2870_regulator_probe(struct platform_device *pdev)
 static struct platform_driver aat2870_regulator_driver = {
 	.driver = {
 		.name	= "aat2870-regulator",
-		.owner	= THIS_MODULE,
 	},
 	.probe	= aat2870_regulator_probe,
 };
diff --git a/drivers/regulator/ab3100.c b/drivers/regulator/ab3100.c
index e10febe9ec34..de2644490f0d 100644
--- a/drivers/regulator/ab3100.c
+++ b/drivers/regulator/ab3100.c
@@ -721,7 +721,6 @@ static int ab3100_regulators_probe(struct platform_device *pdev)
 static struct platform_driver ab3100_regulators_driver = {
 	.driver = {
 		.name  = "ab3100-regulators",
-		.owner = THIS_MODULE,
 	},
 	.probe = ab3100_regulators_probe,
 	.remove = ab3100_regulators_remove,
diff --git a/drivers/regulator/ab8500-ext.c b/drivers/regulator/ab8500-ext.c
index 29c0faaf8eba..84c1ee39ddae 100644
--- a/drivers/regulator/ab8500-ext.c
+++ b/drivers/regulator/ab8500-ext.c
@@ -433,7 +433,6 @@ static struct platform_driver ab8500_ext_regulator_driver = {
 	.probe = ab8500_ext_regulator_probe,
 	.driver         = {
 		.name   = "ab8500-ext-regulator",
-		.owner  = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/regulator/ab8500.c b/drivers/regulator/ab8500.c
index 1fda14e12ea8..0f97514e3474 100644
--- a/drivers/regulator/ab8500.c
+++ b/drivers/regulator/ab8500.c
@@ -3075,7 +3075,6 @@ static struct platform_driver ab8500_regulator_driver = {
 	.probe = ab8500_regulator_probe,
 	.driver         = {
 		.name   = "ab8500-regulator",
-		.owner  = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/regulator/anatop-regulator.c b/drivers/regulator/anatop-regulator.c
index 3586571814b2..738adfa5332b 100644
--- a/drivers/regulator/anatop-regulator.c
+++ b/drivers/regulator/anatop-regulator.c
@@ -322,7 +322,6 @@ static const struct of_device_id of_anatop_regulator_match_tbl[] = {
 static struct platform_driver anatop_regulator_driver = {
 	.driver = {
 		.name	= "anatop_regulator",
-		.owner  = THIS_MODULE,
 		.of_match_table = of_anatop_regulator_match_tbl,
 	},
 	.probe	= anatop_regulator_probe,
diff --git a/drivers/regulator/arizona-ldo1.c b/drivers/regulator/arizona-ldo1.c
index d071b2119a60..8169165904c0 100644
--- a/drivers/regulator/arizona-ldo1.c
+++ b/drivers/regulator/arizona-ldo1.c
@@ -300,7 +300,6 @@ static struct platform_driver arizona_ldo1_driver = {
 	.probe = arizona_ldo1_probe,
 	.driver		= {
 		.name	= "arizona-ldo1",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/regulator/arizona-micsupp.c b/drivers/regulator/arizona-micsupp.c
index c313ef4c3a2f..20079006459a 100644
--- a/drivers/regulator/arizona-micsupp.c
+++ b/drivers/regulator/arizona-micsupp.c
@@ -302,7 +302,6 @@ static struct platform_driver arizona_micsupp_driver = {
 	.probe = arizona_micsupp_probe,
 	.driver		= {
 		.name	= "arizona-micsupp",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/regulator/as3711-regulator.c b/drivers/regulator/as3711-regulator.c
index 8459b0b648cd..c0e93b1332f7 100644
--- a/drivers/regulator/as3711-regulator.c
+++ b/drivers/regulator/as3711-regulator.c
@@ -267,7 +267,6 @@ static int as3711_regulator_probe(struct platform_device *pdev)
 static struct platform_driver as3711_regulator_driver = {
 	.driver	= {
 		.name	= "as3711-regulator",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= as3711_regulator_probe,
 };
diff --git a/drivers/regulator/as3722-regulator.c b/drivers/regulator/as3722-regulator.c
index b68f05f38537..8b046eec6ae0 100644
--- a/drivers/regulator/as3722-regulator.c
+++ b/drivers/regulator/as3722-regulator.c
@@ -916,7 +916,6 @@ MODULE_DEVICE_TABLE(of, of_as3722_regulator_match);
 static struct platform_driver as3722_regulator_driver = {
 	.driver = {
 		.name = "as3722-regulator",
-		.owner = THIS_MODULE,
 		.of_match_table = of_as3722_regulator_match,
 	},
 	.probe = as3722_regulator_probe,
diff --git a/drivers/regulator/axp20x-regulator.c b/drivers/regulator/axp20x-regulator.c
index 2e1010a34ddc..f23d7e1f2ee7 100644
--- a/drivers/regulator/axp20x-regulator.c
+++ b/drivers/regulator/axp20x-regulator.c
@@ -275,7 +275,6 @@ static struct platform_driver axp20x_regulator_driver = {
 	.probe	= axp20x_regulator_probe,
 	.driver	= {
 		.name		= "axp20x-regulator",
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/regulator/bcm590xx-regulator.c b/drivers/regulator/bcm590xx-regulator.c
index fe6ac69549a6..628430bdc312 100644
--- a/drivers/regulator/bcm590xx-regulator.c
+++ b/drivers/regulator/bcm590xx-regulator.c
@@ -453,7 +453,6 @@ static int bcm590xx_probe(struct platform_device *pdev)
 static struct platform_driver bcm590xx_regulator_driver = {
 	.driver = {
 		.name = "bcm590xx-vregs",
-		.owner = THIS_MODULE,
 	},
 	.probe = bcm590xx_probe,
 };
diff --git a/drivers/regulator/da903x.c b/drivers/regulator/da903x.c
index b431ae357fcd..affa1b191314 100644
--- a/drivers/regulator/da903x.c
+++ b/drivers/regulator/da903x.c
@@ -475,7 +475,6 @@ static int da903x_regulator_probe(struct platform_device *pdev)
 static struct platform_driver da903x_regulator_driver = {
 	.driver	= {
 		.name	= "da903x-regulator",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= da903x_regulator_probe,
 };
diff --git a/drivers/regulator/da9052-regulator.c b/drivers/regulator/da9052-regulator.c
index 3945f1006d23..8a4df7a1f2ee 100644
--- a/drivers/regulator/da9052-regulator.c
+++ b/drivers/regulator/da9052-regulator.c
@@ -464,7 +464,6 @@ static struct platform_driver da9052_regulator_driver = {
 	.probe = da9052_regulator_probe,
 	.driver = {
 		.name = "da9052-regulator",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/regulator/da9055-regulator.c b/drivers/regulator/da9055-regulator.c
index 9516317e1a9f..cafdafbffcaf 100644
--- a/drivers/regulator/da9055-regulator.c
+++ b/drivers/regulator/da9055-regulator.c
@@ -665,7 +665,6 @@ static struct platform_driver da9055_regulator_driver = {
 	.probe = da9055_regulator_probe,
 	.driver = {
 		.name = "da9055-regulator",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/regulator/da9063-regulator.c b/drivers/regulator/da9063-regulator.c
index 37dd42759ca9..31c2c593ae0b 100644
--- a/drivers/regulator/da9063-regulator.c
+++ b/drivers/regulator/da9063-regulator.c
@@ -893,7 +893,6 @@ static int da9063_regulator_remove(struct platform_device *pdev)
 static struct platform_driver da9063_regulator_driver = {
 	.driver = {
 		.name = DA9063_DRVNAME_REGULATORS,
-		.owner = THIS_MODULE,
 	},
 	.probe = da9063_regulator_probe,
 	.remove = da9063_regulator_remove,
diff --git a/drivers/regulator/db8500-prcmu.c b/drivers/regulator/db8500-prcmu.c
index 617c1adca816..7cec535cf0bc 100644
--- a/drivers/regulator/db8500-prcmu.c
+++ b/drivers/regulator/db8500-prcmu.c
@@ -534,7 +534,6 @@ static int db8500_regulator_remove(struct platform_device *pdev)
 static struct platform_driver db8500_regulator_driver = {
 	.driver = {
 		.name = "db8500-prcmu-regulators",
-		.owner = THIS_MODULE,
 	},
 	.probe = db8500_regulator_probe,
 	.remove = db8500_regulator_remove,
diff --git a/drivers/regulator/dummy.c b/drivers/regulator/dummy.c
index 7aef9e4c6fbf..cde749778774 100644
--- a/drivers/regulator/dummy.c
+++ b/drivers/regulator/dummy.c
@@ -63,7 +63,6 @@ static struct platform_driver dummy_regulator_driver = {
 	.probe		= dummy_regulator_probe,
 	.driver		= {
 		.name		= "reg-dummy",
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/regulator/fixed.c b/drivers/regulator/fixed.c
index d21da27c0eb6..ff62d69ba0be 100644
--- a/drivers/regulator/fixed.c
+++ b/drivers/regulator/fixed.c
@@ -213,7 +213,6 @@ static struct platform_driver regulator_fixed_voltage_driver = {
 	.probe		= reg_fixed_voltage_probe,
 	.driver		= {
 		.name		= "reg-fixed-voltage",
-		.owner		= THIS_MODULE,
 		.of_match_table = of_match_ptr(fixed_of_match),
 	},
 };
diff --git a/drivers/regulator/gpio-regulator.c b/drivers/regulator/gpio-regulator.c
index c888a9a9482c..464018de7e97 100644
--- a/drivers/regulator/gpio-regulator.c
+++ b/drivers/regulator/gpio-regulator.c
@@ -401,7 +401,6 @@ static struct platform_driver gpio_regulator_driver = {
 	.remove		= gpio_regulator_remove,
 	.driver		= {
 		.name		= "gpio-regulator",
-		.owner		= THIS_MODULE,
 		.of_match_table = of_match_ptr(regulator_gpio_of_match),
 	},
 };
diff --git a/drivers/regulator/hi6421-regulator.c b/drivers/regulator/hi6421-regulator.c
index 156d0d1a55f1..42dc5fb8c899 100644
--- a/drivers/regulator/hi6421-regulator.c
+++ b/drivers/regulator/hi6421-regulator.c
@@ -623,7 +623,6 @@ static int hi6421_regulator_probe(struct platform_device *pdev)
 static struct platform_driver hi6421_regulator_driver = {
 	.driver = {
 		.name	= "hi6421-regulator",
-		.owner  = THIS_MODULE,
 	},
 	.probe	= hi6421_regulator_probe,
 };
diff --git a/drivers/regulator/lp8788-buck.c b/drivers/regulator/lp8788-buck.c
index 948afc249e29..a97bed90d39b 100644
--- a/drivers/regulator/lp8788-buck.c
+++ b/drivers/regulator/lp8788-buck.c
@@ -533,7 +533,6 @@ static struct platform_driver lp8788_buck_driver = {
 	.probe = lp8788_buck_probe,
 	.driver = {
 		.name = LP8788_DEV_BUCK,
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/regulator/lp8788-ldo.c b/drivers/regulator/lp8788-ldo.c
index b9a29a29933f..9f22d079c8cc 100644
--- a/drivers/regulator/lp8788-ldo.c
+++ b/drivers/regulator/lp8788-ldo.c
@@ -561,7 +561,6 @@ static struct platform_driver lp8788_dldo_driver = {
 	.probe = lp8788_dldo_probe,
 	.driver = {
 		.name = LP8788_DEV_DLDO,
-		.owner = THIS_MODULE,
 	},
 };
 
@@ -611,7 +610,6 @@ static struct platform_driver lp8788_aldo_driver = {
 	.probe = lp8788_aldo_probe,
 	.driver = {
 		.name = LP8788_DEV_ALDO,
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/regulator/max14577.c b/drivers/regulator/max14577.c
index 0ff5a20ac958..bf9a44c5fdd2 100644
--- a/drivers/regulator/max14577.c
+++ b/drivers/regulator/max14577.c
@@ -371,7 +371,6 @@ MODULE_DEVICE_TABLE(platform, max14577_regulator_id);
 
 static struct platform_driver max14577_regulator_driver = {
 	.driver = {
-		   .owner = THIS_MODULE,
 		   .name = "max14577-regulator",
 		   },
 	.probe		= max14577_regulator_probe,
diff --git a/drivers/regulator/max77686.c b/drivers/regulator/max77686.c
index 871b96bcd2d0..10d206266ac2 100644
--- a/drivers/regulator/max77686.c
+++ b/drivers/regulator/max77686.c
@@ -488,7 +488,6 @@ MODULE_DEVICE_TABLE(platform, max77686_pmic_id);
 static struct platform_driver max77686_pmic_driver = {
 	.driver = {
 		.name = "max77686-pmic",
-		.owner = THIS_MODULE,
 	},
 	.probe = max77686_pmic_probe,
 	.id_table = max77686_pmic_id,
diff --git a/drivers/regulator/max77693.c b/drivers/regulator/max77693.c
index 7b9755a6c3b5..07b313e51b21 100644
--- a/drivers/regulator/max77693.c
+++ b/drivers/regulator/max77693.c
@@ -267,7 +267,6 @@ MODULE_DEVICE_TABLE(platform, max77693_pmic_id);
 static struct platform_driver max77693_pmic_driver = {
 	.driver = {
 		   .name = "max77693-pmic",
-		   .owner = THIS_MODULE,
 		   },
 	.probe = max77693_pmic_probe,
 	.id_table = max77693_pmic_id,
diff --git a/drivers/regulator/max77802.c b/drivers/regulator/max77802.c
index 0766615c60bc..6af41abccacb 100644
--- a/drivers/regulator/max77802.c
+++ b/drivers/regulator/max77802.c
@@ -597,7 +597,6 @@ MODULE_DEVICE_TABLE(platform, max77802_pmic_id);
 static struct platform_driver max77802_pmic_driver = {
 	.driver = {
 		.name = "max77802-pmic",
-		.owner = THIS_MODULE,
 	},
 	.probe = max77802_pmic_probe,
 	.id_table = max77802_pmic_id,
diff --git a/drivers/regulator/max8907-regulator.c b/drivers/regulator/max8907-regulator.c
index 3426be89c9f6..5e941db5ccaf 100644
--- a/drivers/regulator/max8907-regulator.c
+++ b/drivers/regulator/max8907-regulator.c
@@ -366,7 +366,6 @@ static int max8907_regulator_probe(struct platform_device *pdev)
 static struct platform_driver max8907_regulator_driver = {
 	.driver = {
 		   .name = "max8907-regulator",
-		   .owner = THIS_MODULE,
 		   },
 	.probe = max8907_regulator_probe,
 };
diff --git a/drivers/regulator/max8925-regulator.c b/drivers/regulator/max8925-regulator.c
index 7770777befc4..c802f0239dc7 100644
--- a/drivers/regulator/max8925-regulator.c
+++ b/drivers/regulator/max8925-regulator.c
@@ -324,7 +324,6 @@ static int max8925_regulator_probe(struct platform_device *pdev)
 static struct platform_driver max8925_regulator_driver = {
 	.driver		= {
 		.name	= "max8925-regulator",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= max8925_regulator_probe,
 };
diff --git a/drivers/regulator/max8997.c b/drivers/regulator/max8997.c
index 726fde1d883e..ea0196d4496b 100644
--- a/drivers/regulator/max8997.c
+++ b/drivers/regulator/max8997.c
@@ -1219,7 +1219,6 @@ MODULE_DEVICE_TABLE(platform, max8997_pmic_id);
 static struct platform_driver max8997_pmic_driver = {
 	.driver = {
 		.name = "max8997-pmic",
-		.owner = THIS_MODULE,
 	},
 	.probe = max8997_pmic_probe,
 	.id_table = max8997_pmic_id,
diff --git a/drivers/regulator/max8998.c b/drivers/regulator/max8998.c
index 59e34a05a4a2..3027e7ce100b 100644
--- a/drivers/regulator/max8998.c
+++ b/drivers/regulator/max8998.c
@@ -895,7 +895,6 @@ MODULE_DEVICE_TABLE(platform, max8998_pmic_id);
 static struct platform_driver max8998_pmic_driver = {
 	.driver = {
 		.name = "max8998-pmic",
-		.owner = THIS_MODULE,
 	},
 	.probe = max8998_pmic_probe,
 	.id_table = max8998_pmic_id,
diff --git a/drivers/regulator/mc13783-regulator.c b/drivers/regulator/mc13783-regulator.c
index 7f4a67edf780..fe4c7d677f9c 100644
--- a/drivers/regulator/mc13783-regulator.c
+++ b/drivers/regulator/mc13783-regulator.c
@@ -459,7 +459,6 @@ static int mc13783_regulator_probe(struct platform_device *pdev)
 static struct platform_driver mc13783_regulator_driver = {
 	.driver	= {
 		.name	= "mc13783-regulator",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= mc13783_regulator_probe,
 };
diff --git a/drivers/regulator/mc13892-regulator.c b/drivers/regulator/mc13892-regulator.c
index 793b662a1967..0d17c9206816 100644
--- a/drivers/regulator/mc13892-regulator.c
+++ b/drivers/regulator/mc13892-regulator.c
@@ -633,7 +633,6 @@ err_unlock:
 static struct platform_driver mc13892_regulator_driver = {
 	.driver	= {
 		.name	= "mc13892-regulator",
-		.owner	= THIS_MODULE,
 	},
 	.probe	= mc13892_regulator_probe,
 };
diff --git a/drivers/regulator/palmas-regulator.c b/drivers/regulator/palmas-regulator.c
index 1878e5b567ef..9205f433573c 100644
--- a/drivers/regulator/palmas-regulator.c
+++ b/drivers/regulator/palmas-regulator.c
@@ -1610,7 +1610,6 @@ static struct platform_driver palmas_driver = {
 	.driver = {
 		.name = "palmas-pmic",
 		.of_match_table = of_palmas_match_tbl,
-		.owner = THIS_MODULE,
 	},
 	.probe = palmas_regulators_probe,
 };
diff --git a/drivers/regulator/pbias-regulator.c b/drivers/regulator/pbias-regulator.c
index 6d02d68dfb46..bd2b75c0d1d1 100644
--- a/drivers/regulator/pbias-regulator.c
+++ b/drivers/regulator/pbias-regulator.c
@@ -185,7 +185,6 @@ static struct platform_driver pbias_regulator_driver = {
 	.probe		= pbias_regulator_probe,
 	.driver		= {
 		.name		= "pbias-regulator",
-		.owner		= THIS_MODULE,
 		.of_match_table = of_match_ptr(pbias_of_match),
 	},
 };
diff --git a/drivers/regulator/pcap-regulator.c b/drivers/regulator/pcap-regulator.c
index 3727b7d0e9ac..9b16e6158f15 100644
--- a/drivers/regulator/pcap-regulator.c
+++ b/drivers/regulator/pcap-regulator.c
@@ -259,7 +259,6 @@ static int pcap_regulator_probe(struct platform_device *pdev)
 static struct platform_driver pcap_regulator_driver = {
 	.driver = {
 		.name	= "pcap-regulator",
-		.owner	= THIS_MODULE,
 	},
 	.probe	= pcap_regulator_probe,
 };
diff --git a/drivers/regulator/pwm-regulator.c b/drivers/regulator/pwm-regulator.c
index 91f34ca3a9ac..253833ae35f3 100644
--- a/drivers/regulator/pwm-regulator.c
+++ b/drivers/regulator/pwm-regulator.c
@@ -184,7 +184,6 @@ MODULE_DEVICE_TABLE(of, pwm_of_match);
 static struct platform_driver pwm_regulator_driver = {
 	.driver = {
 		.name		= "pwm-regulator",
-		.owner		= THIS_MODULE,
 		.of_match_table = of_match_ptr(pwm_of_match),
 	},
 	.probe = pwm_regulator_probe,
diff --git a/drivers/regulator/qcom_rpm-regulator.c b/drivers/regulator/qcom_rpm-regulator.c
index 183598b111f9..8364ff331a81 100644
--- a/drivers/regulator/qcom_rpm-regulator.c
+++ b/drivers/regulator/qcom_rpm-regulator.c
@@ -797,7 +797,6 @@ static struct platform_driver rpm_reg_driver = {
 	.probe          = rpm_reg_probe,
 	.driver  = {
 		.name  = "qcom_rpm_reg",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(rpm_of_match),
 	},
 };
diff --git a/drivers/regulator/rc5t583-regulator.c b/drivers/regulator/rc5t583-regulator.c
index 4c414ae109ae..d2e67c512195 100644
--- a/drivers/regulator/rc5t583-regulator.c
+++ b/drivers/regulator/rc5t583-regulator.c
@@ -179,7 +179,6 @@ skip_ext_pwr_config:
 static struct platform_driver rc5t583_regulator_driver = {
 	.driver	= {
 		.name	= "rc5t583-regulator",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= rc5t583_regulator_probe,
 };
diff --git a/drivers/regulator/rk808-regulator.c b/drivers/regulator/rk808-regulator.c
index ea9d05eabd0a..c94a3e0f3b91 100644
--- a/drivers/regulator/rk808-regulator.c
+++ b/drivers/regulator/rk808-regulator.c
@@ -419,7 +419,6 @@ static struct platform_driver rk808_regulator_driver = {
 	.probe = rk808_regulator_probe,
 	.driver = {
 		.name = "rk808-regulator",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/regulator/s2mpa01.c b/drivers/regulator/s2mpa01.c
index 5db4e12a7e04..92f88753bfed 100644
--- a/drivers/regulator/s2mpa01.c
+++ b/drivers/regulator/s2mpa01.c
@@ -405,7 +405,6 @@ MODULE_DEVICE_TABLE(platform, s2mpa01_pmic_id);
 static struct platform_driver s2mpa01_pmic_driver = {
 	.driver = {
 		.name = "s2mpa01-pmic",
-		.owner = THIS_MODULE,
 	},
 	.probe = s2mpa01_pmic_probe,
 	.id_table = s2mpa01_pmic_id,
diff --git a/drivers/regulator/s2mps11.c b/drivers/regulator/s2mps11.c
index b345cf51225a..2809ae0d6bcd 100644
--- a/drivers/regulator/s2mps11.c
+++ b/drivers/regulator/s2mps11.c
@@ -570,7 +570,7 @@ static struct regulator_ops s2mps14_reg_ops = {
 	.enable_mask	= S2MPS14_ENABLE_MASK		\
 }
 
-#define regulator_desc_s2mps14_buck(num, min, step) {		\
+#define regulator_desc_s2mps14_buck(num, min, step, min_sel) {	\
 	.name		= "BUCK"#num,				\
 	.id		= S2MPS14_BUCK##num,			\
 	.ops		= &s2mps14_reg_ops,			\
@@ -579,7 +579,7 @@ static struct regulator_ops s2mps14_reg_ops = {
 	.min_uV		= min,					\
 	.uV_step	= step,					\
 	.n_voltages	= S2MPS14_BUCK_N_VOLTAGES,		\
-	.linear_min_sel = S2MPS14_BUCK1235_START_SEL,		\
+	.linear_min_sel = min_sel,				\
 	.ramp_delay	= S2MPS14_BUCK_RAMP_DELAY,		\
 	.vsel_reg	= S2MPS14_REG_B1CTRL2 + (num - 1) * 2,	\
 	.vsel_mask	= S2MPS14_BUCK_VSEL_MASK,		\
@@ -613,11 +613,16 @@ static const struct regulator_desc s2mps14_regulators[] = {
 	regulator_desc_s2mps14_ldo(23, MIN_800_MV, STEP_25_MV),
 	regulator_desc_s2mps14_ldo(24, MIN_1800_MV, STEP_25_MV),
 	regulator_desc_s2mps14_ldo(25, MIN_1800_MV, STEP_25_MV),
-	regulator_desc_s2mps14_buck(1, MIN_600_MV, STEP_6_25_MV),
-	regulator_desc_s2mps14_buck(2, MIN_600_MV, STEP_6_25_MV),
-	regulator_desc_s2mps14_buck(3, MIN_600_MV, STEP_6_25_MV),
-	regulator_desc_s2mps14_buck(4, MIN_1400_MV, STEP_12_5_MV),
-	regulator_desc_s2mps14_buck(5, MIN_600_MV, STEP_6_25_MV),
+	regulator_desc_s2mps14_buck(1, MIN_600_MV, STEP_6_25_MV,
+				    S2MPS14_BUCK1235_START_SEL),
+	regulator_desc_s2mps14_buck(2, MIN_600_MV, STEP_6_25_MV,
+				    S2MPS14_BUCK1235_START_SEL),
+	regulator_desc_s2mps14_buck(3, MIN_600_MV, STEP_6_25_MV,
+				    S2MPS14_BUCK1235_START_SEL),
+	regulator_desc_s2mps14_buck(4, MIN_1400_MV, STEP_12_5_MV,
+				    S2MPS14_BUCK4_START_SEL),
+	regulator_desc_s2mps14_buck(5, MIN_600_MV, STEP_6_25_MV,
+				    S2MPS14_BUCK1235_START_SEL),
 };
 
 static int s2mps14_pmic_enable_ext_control(struct s2mps11_info *s2mps11,
@@ -1033,7 +1038,6 @@ MODULE_DEVICE_TABLE(platform, s2mps11_pmic_id);
 static struct platform_driver s2mps11_pmic_driver = {
 	.driver = {
 		.name = "s2mps11-pmic",
-		.owner = THIS_MODULE,
 	},
 	.probe = s2mps11_pmic_probe,
 	.id_table = s2mps11_pmic_id,
diff --git a/drivers/regulator/s5m8767.c b/drivers/regulator/s5m8767.c
index dc1328c0c71c..58f5d3b8e981 100644
--- a/drivers/regulator/s5m8767.c
+++ b/drivers/regulator/s5m8767.c
@@ -988,7 +988,6 @@ MODULE_DEVICE_TABLE(platform, s5m8767_pmic_id);
 static struct platform_driver s5m8767_pmic_driver = {
 	.driver = {
 		.name = "s5m8767-pmic",
-		.owner = THIS_MODULE,
 	},
 	.probe = s5m8767_pmic_probe,
 	.id_table = s5m8767_pmic_id,
diff --git a/drivers/regulator/stw481x-vmmc.c b/drivers/regulator/stw481x-vmmc.c
index b4f1696456a7..89025f560259 100644
--- a/drivers/regulator/stw481x-vmmc.c
+++ b/drivers/regulator/stw481x-vmmc.c
@@ -95,7 +95,6 @@ static const struct of_device_id stw481x_vmmc_match[] = {
 static struct platform_driver stw481x_vmmc_regulator_driver = {
 	.driver = {
 		.name  = "stw481x-vmmc-regulator",
-		.owner = THIS_MODULE,
 		.of_match_table = stw481x_vmmc_match,
 	},
 	.probe = stw481x_vmmc_regulator_probe,
diff --git a/drivers/regulator/ti-abb-regulator.c b/drivers/regulator/ti-abb-regulator.c
index 1ef5aba96f17..d2f994298753 100644
--- a/drivers/regulator/ti-abb-regulator.c
+++ b/drivers/regulator/ti-abb-regulator.c
@@ -892,7 +892,6 @@ static struct platform_driver ti_abb_driver = {
 	.probe = ti_abb_probe,
 	.driver = {
 		   .name = "ti_abb",
-		   .owner = THIS_MODULE,
 		   .of_match_table = of_match_ptr(ti_abb_of_match),
 		   },
 };
diff --git a/drivers/regulator/tps6105x-regulator.c b/drivers/regulator/tps6105x-regulator.c
index c1e33a3d397b..3510b3e7330a 100644
--- a/drivers/regulator/tps6105x-regulator.c
+++ b/drivers/regulator/tps6105x-regulator.c
@@ -163,7 +163,6 @@ static int tps6105x_regulator_probe(struct platform_device *pdev)
 static struct platform_driver tps6105x_regulator_driver = {
 	.driver = {
 		.name  = "tps6105x-regulator",
-		.owner = THIS_MODULE,
 	},
 	.probe = tps6105x_regulator_probe,
 };
diff --git a/drivers/regulator/tps6507x-regulator.c b/drivers/regulator/tps6507x-regulator.c
index 98e66ce26723..dad0bac09ecf 100644
--- a/drivers/regulator/tps6507x-regulator.c
+++ b/drivers/regulator/tps6507x-regulator.c
@@ -519,7 +519,6 @@ static int tps6507x_pmic_probe(struct platform_device *pdev)
 static struct platform_driver tps6507x_pmic_driver = {
 	.driver = {
 		.name = "tps6507x-pmic",
-		.owner = THIS_MODULE,
 	},
 	.probe = tps6507x_pmic_probe,
 };
diff --git a/drivers/regulator/tps65090-regulator.c b/drivers/regulator/tps65090-regulator.c
index f1df4423d361..395f35dc8cdb 100644
--- a/drivers/regulator/tps65090-regulator.c
+++ b/drivers/regulator/tps65090-regulator.c
@@ -511,7 +511,6 @@ static int tps65090_regulator_probe(struct platform_device *pdev)
 static struct platform_driver tps65090_regulator_driver = {
 	.driver	= {
 		.name	= "tps65090-pmic",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= tps65090_regulator_probe,
 };
diff --git a/drivers/regulator/tps65218-regulator.c b/drivers/regulator/tps65218-regulator.c
index 263cc85d6202..7f97223f95c5 100644
--- a/drivers/regulator/tps65218-regulator.c
+++ b/drivers/regulator/tps65218-regulator.c
@@ -256,7 +256,6 @@ static int tps65218_regulator_probe(struct platform_device *pdev)
 static struct platform_driver tps65218_regulator_driver = {
 	.driver = {
 		.name = "tps65218-pmic",
-		.owner = THIS_MODULE,
 		.of_match_table = tps65218_of_match,
 	},
 	.probe = tps65218_regulator_probe,
diff --git a/drivers/regulator/tps6586x-regulator.c b/drivers/regulator/tps6586x-regulator.c
index ccbb9f150b4e..2852de05d64d 100644
--- a/drivers/regulator/tps6586x-regulator.c
+++ b/drivers/regulator/tps6586x-regulator.c
@@ -522,7 +522,6 @@ static int tps6586x_regulator_probe(struct platform_device *pdev)
 static struct platform_driver tps6586x_regulator_driver = {
 	.driver	= {
 		.name	= "tps6586x-regulator",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= tps6586x_regulator_probe,
 };
diff --git a/drivers/regulator/tps65910-regulator.c b/drivers/regulator/tps65910-regulator.c
index 18fc991175bc..e2cffe01b807 100644
--- a/drivers/regulator/tps65910-regulator.c
+++ b/drivers/regulator/tps65910-regulator.c
@@ -1245,7 +1245,6 @@ static void tps65910_shutdown(struct platform_device *pdev)
 static struct platform_driver tps65910_driver = {
 	.driver = {
 		.name = "tps65910-pmic",
-		.owner = THIS_MODULE,
 	},
 	.probe = tps65910_probe,
 	.shutdown = tps65910_shutdown,
diff --git a/drivers/regulator/tps65912-regulator.c b/drivers/regulator/tps65912-regulator.c
index 9cafaa0f9455..9503d5481a52 100644
--- a/drivers/regulator/tps65912-regulator.c
+++ b/drivers/regulator/tps65912-regulator.c
@@ -519,7 +519,6 @@ static int tps65912_probe(struct platform_device *pdev)
 static struct platform_driver tps65912_driver = {
 	.driver = {
 		.name = "tps65912-pmic",
-		.owner = THIS_MODULE,
 	},
 	.probe = tps65912_probe,
 };
diff --git a/drivers/regulator/tps80031-regulator.c b/drivers/regulator/tps80031-regulator.c
index 26aa6d9c308f..d4cc60ad18ae 100644
--- a/drivers/regulator/tps80031-regulator.c
+++ b/drivers/regulator/tps80031-regulator.c
@@ -746,7 +746,6 @@ static int tps80031_regulator_probe(struct platform_device *pdev)
 static struct platform_driver tps80031_regulator_driver = {
 	.driver	= {
 		.name	= "tps80031-pmic",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= tps80031_regulator_probe,
 };
diff --git a/drivers/regulator/twl-regulator.c b/drivers/regulator/twl-regulator.c
index dd727bca1983..955a6fb1355c 100644
--- a/drivers/regulator/twl-regulator.c
+++ b/drivers/regulator/twl-regulator.c
@@ -1221,7 +1221,6 @@ static struct platform_driver twlreg_driver = {
 	 */
 	.driver  = {
 		.name  = "twl_reg",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(twl_of_match),
 	},
 };
diff --git a/drivers/regulator/vexpress.c b/drivers/regulator/vexpress.c
index 5e7c789023a9..bed9d3ee4198 100644
--- a/drivers/regulator/vexpress.c
+++ b/drivers/regulator/vexpress.c
@@ -108,7 +108,6 @@ static struct platform_driver vexpress_regulator_driver = {
 	.probe = vexpress_regulator_probe,
 	.driver	= {
 		.name = DRVNAME,
-		.owner = THIS_MODULE,
 		.of_match_table = vexpress_regulator_of_match,
 	},
 };
diff --git a/drivers/regulator/virtual.c b/drivers/regulator/virtual.c
index 6ff95b045984..a6f1c7a9914f 100644
--- a/drivers/regulator/virtual.c
+++ b/drivers/regulator/virtual.c
@@ -338,7 +338,6 @@ static struct platform_driver regulator_virtual_consumer_driver = {
 	.remove		= regulator_virtual_remove,
 	.driver		= {
 		.name		= "reg-virt-consumer",
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/regulator/wm831x-dcdc.c b/drivers/regulator/wm831x-dcdc.c
index 0d88a82ab2a2..0d7e164a5e76 100644
--- a/drivers/regulator/wm831x-dcdc.c
+++ b/drivers/regulator/wm831x-dcdc.c
@@ -562,7 +562,6 @@ static struct platform_driver wm831x_buckv_driver = {
 	.probe = wm831x_buckv_probe,
 	.driver		= {
 		.name	= "wm831x-buckv",
-		.owner	= THIS_MODULE,
 	},
 };
 
@@ -689,7 +688,6 @@ static struct platform_driver wm831x_buckp_driver = {
 	.probe = wm831x_buckp_probe,
 	.driver		= {
 		.name	= "wm831x-buckp",
-		.owner	= THIS_MODULE,
 	},
 };
 
@@ -804,7 +802,6 @@ static struct platform_driver wm831x_boostp_driver = {
 	.probe = wm831x_boostp_probe,
 	.driver		= {
 		.name	= "wm831x-boostp",
-		.owner	= THIS_MODULE,
 	},
 };
 
@@ -880,7 +877,6 @@ static struct platform_driver wm831x_epe_driver = {
 	.probe = wm831x_epe_probe,
 	.driver		= {
 		.name	= "wm831x-epe",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/regulator/wm831x-isink.c b/drivers/regulator/wm831x-isink.c
index 72e385e76a9d..1e88391a1628 100644
--- a/drivers/regulator/wm831x-isink.c
+++ b/drivers/regulator/wm831x-isink.c
@@ -224,7 +224,6 @@ static struct platform_driver wm831x_isink_driver = {
 	.probe = wm831x_isink_probe,
 	.driver		= {
 		.name	= "wm831x-isink",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/regulator/wm831x-ldo.c b/drivers/regulator/wm831x-ldo.c
index eca0eeb78acd..7ae2dc82f636 100644
--- a/drivers/regulator/wm831x-ldo.c
+++ b/drivers/regulator/wm831x-ldo.c
@@ -307,7 +307,6 @@ static struct platform_driver wm831x_gp_ldo_driver = {
 	.probe = wm831x_gp_ldo_probe,
 	.driver		= {
 		.name	= "wm831x-ldo",
-		.owner	= THIS_MODULE,
 	},
 };
 
@@ -516,7 +515,6 @@ static struct platform_driver wm831x_aldo_driver = {
 	.probe = wm831x_aldo_probe,
 	.driver		= {
 		.name	= "wm831x-aldo",
-		.owner	= THIS_MODULE,
 	},
 };
 
@@ -650,7 +648,6 @@ static struct platform_driver wm831x_alive_ldo_driver = {
 	.probe = wm831x_alive_ldo_probe,
 	.driver		= {
 		.name	= "wm831x-alive-ldo",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/regulator/wm8994-regulator.c b/drivers/regulator/wm8994-regulator.c
index 88f5064e412b..750e0bd61f9f 100644
--- a/drivers/regulator/wm8994-regulator.c
+++ b/drivers/regulator/wm8994-regulator.c
@@ -187,7 +187,6 @@ static struct platform_driver wm8994_ldo_driver = {
 	.probe = wm8994_ldo_probe,
 	.driver		= {
 		.name	= "wm8994-ldo",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/remoteproc/da8xx_remoteproc.c b/drivers/remoteproc/da8xx_remoteproc.c
index 3841b9813109..89fd057e5f1d 100644
--- a/drivers/remoteproc/da8xx_remoteproc.c
+++ b/drivers/remoteproc/da8xx_remoteproc.c
@@ -300,7 +300,6 @@ static struct platform_driver da8xx_rproc_driver = {
 	.remove = da8xx_rproc_remove,
 	.driver = {
 		.name = "davinci-rproc",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/remoteproc/omap_remoteproc.c b/drivers/remoteproc/omap_remoteproc.c
index cf92f6e7c5dc..e85f30370760 100644
--- a/drivers/remoteproc/omap_remoteproc.c
+++ b/drivers/remoteproc/omap_remoteproc.c
@@ -231,7 +231,6 @@ static struct platform_driver omap_rproc_driver = {
 	.remove = omap_rproc_remove,
 	.driver = {
 		.name = "omap-rproc",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/remoteproc/ste_modem_rproc.c b/drivers/remoteproc/ste_modem_rproc.c
index c4ac9104dd8e..16b7b7bd805b 100644
--- a/drivers/remoteproc/ste_modem_rproc.c
+++ b/drivers/remoteproc/ste_modem_rproc.c
@@ -331,7 +331,6 @@ free_rproc:
 static struct platform_driver sproc_driver = {
 	.driver	= {
 		.name	= SPROC_MODEM_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe	= sproc_probe,
 	.remove	= sproc_drv_remove,
diff --git a/drivers/reset/reset-socfpga.c b/drivers/reset/reset-socfpga.c
index 40582089474a..0a8def35ea2e 100644
--- a/drivers/reset/reset-socfpga.c
+++ b/drivers/reset/reset-socfpga.c
@@ -150,7 +150,6 @@ static struct platform_driver socfpga_reset_driver = {
 	.remove	= socfpga_reset_remove,
 	.driver = {
 		.name		= "socfpga-reset",
-		.owner		= THIS_MODULE,
 		.of_match_table	= socfpga_reset_dt_ids,
 	},
 };
diff --git a/drivers/reset/reset-sunxi.c b/drivers/reset/reset-sunxi.c
index a94e7a7820b4..eebc52cb6984 100644
--- a/drivers/reset/reset-sunxi.c
+++ b/drivers/reset/reset-sunxi.c
@@ -179,7 +179,6 @@ static struct platform_driver sunxi_reset_driver = {
 	.remove	= sunxi_reset_remove,
 	.driver = {
 		.name		= "sunxi-reset",
-		.owner		= THIS_MODULE,
 		.of_match_table	= sunxi_reset_dt_ids,
 	},
 };
diff --git a/drivers/reset/sti/reset-stih415.c b/drivers/reset/sti/reset-stih415.c
index c93fd260447e..8dad603d863c 100644
--- a/drivers/reset/sti/reset-stih415.c
+++ b/drivers/reset/sti/reset-stih415.c
@@ -101,7 +101,6 @@ static struct platform_driver stih415_reset_driver = {
 	.probe = syscfg_reset_probe,
 	.driver = {
 		.name = "reset-stih415",
-		.owner = THIS_MODULE,
 		.of_match_table = stih415_reset_match,
 	},
 };
diff --git a/drivers/reset/sti/reset-stih416.c b/drivers/reset/sti/reset-stih416.c
index 5fc987076a90..79aed70a26c0 100644
--- a/drivers/reset/sti/reset-stih416.c
+++ b/drivers/reset/sti/reset-stih416.c
@@ -132,7 +132,6 @@ static struct platform_driver stih416_reset_driver = {
 	.probe = syscfg_reset_probe,
 	.driver = {
 		.name = "reset-stih416",
-		.owner = THIS_MODULE,
 		.of_match_table = stih416_reset_match,
 	},
 };
diff --git a/drivers/rpmsg/virtio_rpmsg_bus.c b/drivers/rpmsg/virtio_rpmsg_bus.c
index b6135d4d54eb..92f6af6da699 100644
--- a/drivers/rpmsg/virtio_rpmsg_bus.c
+++ b/drivers/rpmsg/virtio_rpmsg_bus.c
@@ -41,6 +41,7 @@
  * @svq:	tx virtqueue
  * @rbufs:	kernel address of rx buffers
  * @sbufs:	kernel address of tx buffers
+ * @num_bufs:	total number of buffers for rx and tx
  * @last_sbuf:	index of last tx buffer used
  * @bufs_dma:	dma base addr of the buffers
  * @tx_lock:	protects svq, sbufs and sleepers, to allow concurrent senders.
@@ -60,6 +61,7 @@ struct virtproc_info {
 	struct virtio_device *vdev;
 	struct virtqueue *rvq, *svq;
 	void *rbufs, *sbufs;
+	unsigned int num_bufs;
 	int last_sbuf;
 	dma_addr_t bufs_dma;
 	struct mutex tx_lock;
@@ -86,13 +88,14 @@ struct rpmsg_channel_info {
 #define to_rpmsg_driver(d) container_of(d, struct rpmsg_driver, drv)
 
 /*
- * We're allocating 512 buffers of 512 bytes for communications, and then
- * using the first 256 buffers for RX, and the last 256 buffers for TX.
+ * We're allocating buffers of 512 bytes each for communications. The
+ * number of buffers will be computed from the number of buffers supported
+ * by the vring, upto a maximum of 512 buffers (256 in each direction).
  *
  * Each buffer will have 16 bytes for the msg header and 496 bytes for
  * the payload.
  *
- * This will require a total space of 256KB for the buffers.
+ * This will utilize a maximum total space of 256KB for the buffers.
  *
  * We might also want to add support for user-provided buffers in time.
  * This will allow bigger buffer size flexibility, and can also be used
@@ -102,9 +105,8 @@ struct rpmsg_channel_info {
  * can change this without changing anything in the firmware of the remote
  * processor.
  */
-#define RPMSG_NUM_BUFS		(512)
+#define MAX_RPMSG_NUM_BUFS	(512)
 #define RPMSG_BUF_SIZE		(512)
-#define RPMSG_TOTAL_BUF_SPACE	(RPMSG_NUM_BUFS * RPMSG_BUF_SIZE)
 
 /*
  * Local addresses are dynamically allocated on-demand.
@@ -579,7 +581,7 @@ static void *get_a_tx_buf(struct virtproc_info *vrp)
 	 * either pick the next unused tx buffer
 	 * (half of our buffers are used for sending messages)
 	 */
-	if (vrp->last_sbuf < RPMSG_NUM_BUFS / 2)
+	if (vrp->last_sbuf < vrp->num_bufs / 2)
 		ret = vrp->sbufs + RPMSG_BUF_SIZE * vrp->last_sbuf++;
 	/* or recycle a used one */
 	else
@@ -948,6 +950,7 @@ static int rpmsg_probe(struct virtio_device *vdev)
 	struct virtproc_info *vrp;
 	void *bufs_va;
 	int err = 0, i;
+	size_t total_buf_space;
 
 	vrp = kzalloc(sizeof(*vrp), GFP_KERNEL);
 	if (!vrp)
@@ -968,10 +971,22 @@ static int rpmsg_probe(struct virtio_device *vdev)
 	vrp->rvq = vqs[0];
 	vrp->svq = vqs[1];
 
+	/* we expect symmetric tx/rx vrings */
+	WARN_ON(virtqueue_get_vring_size(vrp->rvq) !=
+		virtqueue_get_vring_size(vrp->svq));
+
+	/* we need less buffers if vrings are small */
+	if (virtqueue_get_vring_size(vrp->rvq) < MAX_RPMSG_NUM_BUFS / 2)
+		vrp->num_bufs = virtqueue_get_vring_size(vrp->rvq) * 2;
+	else
+		vrp->num_bufs = MAX_RPMSG_NUM_BUFS;
+
+	total_buf_space = vrp->num_bufs * RPMSG_BUF_SIZE;
+
 	/* allocate coherent memory for the buffers */
 	bufs_va = dma_alloc_coherent(vdev->dev.parent->parent,
-				RPMSG_TOTAL_BUF_SPACE,
-				&vrp->bufs_dma, GFP_KERNEL);
+				     total_buf_space, &vrp->bufs_dma,
+				     GFP_KERNEL);
 	if (!bufs_va) {
 		err = -ENOMEM;
 		goto vqs_del;
@@ -984,10 +999,10 @@ static int rpmsg_probe(struct virtio_device *vdev)
 	vrp->rbufs = bufs_va;
 
 	/* and half is dedicated for TX */
-	vrp->sbufs = bufs_va + RPMSG_TOTAL_BUF_SPACE / 2;
+	vrp->sbufs = bufs_va + total_buf_space / 2;
 
 	/* set up the receive buffers */
-	for (i = 0; i < RPMSG_NUM_BUFS / 2; i++) {
+	for (i = 0; i < vrp->num_bufs / 2; i++) {
 		struct scatterlist sg;
 		void *cpu_addr = vrp->rbufs + i * RPMSG_BUF_SIZE;
 
@@ -1023,8 +1038,8 @@ static int rpmsg_probe(struct virtio_device *vdev)
 	return 0;
 
 free_coherent:
-	dma_free_coherent(vdev->dev.parent->parent, RPMSG_TOTAL_BUF_SPACE,
-					bufs_va, vrp->bufs_dma);
+	dma_free_coherent(vdev->dev.parent->parent, total_buf_space,
+			  bufs_va, vrp->bufs_dma);
 vqs_del:
 	vdev->config->del_vqs(vrp->vdev);
 free_vrp:
@@ -1042,6 +1057,7 @@ static int rpmsg_remove_device(struct device *dev, void *data)
 static void rpmsg_remove(struct virtio_device *vdev)
 {
 	struct virtproc_info *vrp = vdev->priv;
+	size_t total_buf_space = vrp->num_bufs * RPMSG_BUF_SIZE;
 	int ret;
 
 	vdev->config->reset(vdev);
@@ -1057,8 +1073,8 @@ static void rpmsg_remove(struct virtio_device *vdev)
 
 	vdev->config->del_vqs(vrp->vdev);
 
-	dma_free_coherent(vdev->dev.parent->parent, RPMSG_TOTAL_BUF_SPACE,
-					vrp->rbufs, vrp->bufs_dma);
+	dma_free_coherent(vdev->dev.parent->parent, total_buf_space,
+			  vrp->rbufs, vrp->bufs_dma);
 
 	kfree(vrp);
 }
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index 4511ddc1ac31..f15cddfeb897 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -987,6 +987,17 @@ config RTC_DRV_NUC900
 	  If you say yes here you get support for the RTC subsystem of the
 	  NUC910/NUC920 used in embedded systems.
 
+config RTC_DRV_OPAL
+	tristate "IBM OPAL RTC driver"
+	depends on PPC_POWERNV
+	default y
+	help
+	  If you say yes here you get support for the PowerNV platform RTC
+	  driver based on OPAL interfaces.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called rtc-opal.
+
 comment "on-CPU RTC drivers"
 
 config RTC_DRV_DAVINCI
diff --git a/drivers/rtc/Makefile b/drivers/rtc/Makefile
index b188323c096a..c8ef3e1e6ccd 100644
--- a/drivers/rtc/Makefile
+++ b/drivers/rtc/Makefile
@@ -92,6 +92,7 @@ obj-$(CONFIG_RTC_DRV_MSM6242)	+= rtc-msm6242.o
 obj-$(CONFIG_RTC_DRV_MPC5121)	+= rtc-mpc5121.o
 obj-$(CONFIG_RTC_DRV_MV)	+= rtc-mv.o
 obj-$(CONFIG_RTC_DRV_NUC900)	+= rtc-nuc900.o
+obj-$(CONFIG_RTC_DRV_OPAL)	+= rtc-opal.o
 obj-$(CONFIG_RTC_DRV_OMAP)	+= rtc-omap.o
 obj-$(CONFIG_RTC_DRV_PALMAS)	+= rtc-palmas.o
 obj-$(CONFIG_RTC_DRV_PCAP)	+= rtc-pcap.o
diff --git a/drivers/rtc/rtc-88pm80x.c b/drivers/rtc/rtc-88pm80x.c
index 0916089c7c3e..7df0579d9852 100644
--- a/drivers/rtc/rtc-88pm80x.c
+++ b/drivers/rtc/rtc-88pm80x.c
@@ -352,7 +352,6 @@ static int pm80x_rtc_remove(struct platform_device *pdev)
 static struct platform_driver pm80x_rtc_driver = {
 	.driver = {
 		   .name = "88pm80x-rtc",
-		   .owner = THIS_MODULE,
 		   .pm = &pm80x_rtc_pm_ops,
 		   },
 	.probe = pm80x_rtc_probe,
diff --git a/drivers/rtc/rtc-88pm860x.c b/drivers/rtc/rtc-88pm860x.c
index 0c6add1a38dc..19e53b3b8e00 100644
--- a/drivers/rtc/rtc-88pm860x.c
+++ b/drivers/rtc/rtc-88pm860x.c
@@ -448,7 +448,6 @@ static SIMPLE_DEV_PM_OPS(pm860x_rtc_pm_ops, pm860x_rtc_suspend, pm860x_rtc_resum
 static struct platform_driver pm860x_rtc_driver = {
 	.driver		= {
 		.name	= "88pm860x-rtc",
-		.owner	= THIS_MODULE,
 		.pm	= &pm860x_rtc_pm_ops,
 	},
 	.probe		= pm860x_rtc_probe,
diff --git a/drivers/rtc/rtc-ab3100.c b/drivers/rtc/rtc-ab3100.c
index ff435343ba9f..1d0340fdb820 100644
--- a/drivers/rtc/rtc-ab3100.c
+++ b/drivers/rtc/rtc-ab3100.c
@@ -243,7 +243,6 @@ static int __init ab3100_rtc_probe(struct platform_device *pdev)
 static struct platform_driver ab3100_rtc_driver = {
 	.driver = {
 		.name = "ab3100-rtc",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-ab8500.c b/drivers/rtc/rtc-ab8500.c
index 866e0ef5122d..6856f0a3a3d5 100644
--- a/drivers/rtc/rtc-ab8500.c
+++ b/drivers/rtc/rtc-ab8500.c
@@ -519,7 +519,6 @@ static int ab8500_rtc_remove(struct platform_device *pdev)
 static struct platform_driver ab8500_rtc_driver = {
 	.driver = {
 		.name = "ab8500-rtc",
-		.owner = THIS_MODULE,
 	},
 	.probe	= ab8500_rtc_probe,
 	.remove = ab8500_rtc_remove,
diff --git a/drivers/rtc/rtc-at32ap700x.c b/drivers/rtc/rtc-at32ap700x.c
index aee3387fb099..d618d6c7ef93 100644
--- a/drivers/rtc/rtc-at32ap700x.c
+++ b/drivers/rtc/rtc-at32ap700x.c
@@ -277,7 +277,6 @@ static struct platform_driver at32_rtc_driver = {
 	.remove		= __exit_p(at32_rtc_remove),
 	.driver		= {
 		.name	= "at32ap700x_rtc",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-at91rm9200.c b/drivers/rtc/rtc-at91rm9200.c
index 44fe83ee9bee..70a5d94cc766 100644
--- a/drivers/rtc/rtc-at91rm9200.c
+++ b/drivers/rtc/rtc-at91rm9200.c
@@ -491,7 +491,6 @@ static struct platform_driver at91_rtc_driver = {
 	.shutdown	= at91_rtc_shutdown,
 	.driver		= {
 		.name	= "at91_rtc",
-		.owner	= THIS_MODULE,
 		.pm	= &at91_rtc_pm_ops,
 		.of_match_table = of_match_ptr(at91_rtc_dt_ids),
 	},
diff --git a/drivers/rtc/rtc-at91sam9.c b/drivers/rtc/rtc-at91sam9.c
index abac38abd38e..6b9aaf1afc72 100644
--- a/drivers/rtc/rtc-at91sam9.c
+++ b/drivers/rtc/rtc-at91sam9.c
@@ -525,7 +525,6 @@ static struct platform_driver at91_rtc_driver = {
 	.shutdown	= at91_rtc_shutdown,
 	.driver		= {
 		.name	= "rtc-at91sam9",
-		.owner	= THIS_MODULE,
 		.pm	= &at91_rtc_pm_ops,
 		.of_match_table = of_match_ptr(at91_rtc_dt_ids),
 	},
diff --git a/drivers/rtc/rtc-au1xxx.c b/drivers/rtc/rtc-au1xxx.c
index fd25e2374d4e..84d6e026784d 100644
--- a/drivers/rtc/rtc-au1xxx.c
+++ b/drivers/rtc/rtc-au1xxx.c
@@ -117,7 +117,6 @@ out_err:
 static struct platform_driver au1xrtc_driver = {
 	.driver		= {
 		.name	= "rtc-au1xxx",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-bfin.c b/drivers/rtc/rtc-bfin.c
index fe4bdb06a55a..3d44b11721ea 100644
--- a/drivers/rtc/rtc-bfin.c
+++ b/drivers/rtc/rtc-bfin.c
@@ -434,7 +434,6 @@ static SIMPLE_DEV_PM_OPS(bfin_rtc_pm_ops, bfin_rtc_suspend, bfin_rtc_resume);
 static struct platform_driver bfin_rtc_driver = {
 	.driver		= {
 		.name	= "rtc-bfin",
-		.owner	= THIS_MODULE,
 		.pm	= &bfin_rtc_pm_ops,
 	},
 	.probe		= bfin_rtc_probe,
diff --git a/drivers/rtc/rtc-bq4802.c b/drivers/rtc/rtc-bq4802.c
index fc0ff87aa5df..bd170cb3361c 100644
--- a/drivers/rtc/rtc-bq4802.c
+++ b/drivers/rtc/rtc-bq4802.c
@@ -192,7 +192,6 @@ MODULE_ALIAS("platform:rtc-bq4802");
 static struct platform_driver bq4802_driver = {
 	.driver		= {
 		.name	= "rtc-bq4802",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= bq4802_probe,
 };
diff --git a/drivers/rtc/rtc-coh901331.c b/drivers/rtc/rtc-coh901331.c
index 869cae273799..56343b2fbc68 100644
--- a/drivers/rtc/rtc-coh901331.c
+++ b/drivers/rtc/rtc-coh901331.c
@@ -267,7 +267,6 @@ static const struct of_device_id coh901331_dt_match[] = {
 static struct platform_driver coh901331_driver = {
 	.driver = {
 		.name = "rtc-coh901331",
-		.owner = THIS_MODULE,
 		.pm = &coh901331_pm_ops,
 		.of_match_table = coh901331_dt_match,
 	},
diff --git a/drivers/rtc/rtc-da9052.c b/drivers/rtc/rtc-da9052.c
index e5c9486cf452..613c43b7e9ae 100644
--- a/drivers/rtc/rtc-da9052.c
+++ b/drivers/rtc/rtc-da9052.c
@@ -265,7 +265,6 @@ static struct platform_driver da9052_rtc_driver = {
 	.probe	= da9052_rtc_probe,
 	.driver = {
 		.name	= "da9052-rtc",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-da9055.c b/drivers/rtc/rtc-da9055.c
index a825491331c8..7ec0872d5e3b 100644
--- a/drivers/rtc/rtc-da9055.c
+++ b/drivers/rtc/rtc-da9055.c
@@ -391,7 +391,6 @@ static struct platform_driver da9055_rtc_driver = {
 	.probe  = da9055_rtc_probe,
 	.driver = {
 		.name   = "da9055-rtc",
-		.owner  = THIS_MODULE,
 		.pm = &da9055_rtc_pm_ops,
 	},
 };
diff --git a/drivers/rtc/rtc-da9063.c b/drivers/rtc/rtc-da9063.c
index 731ed1a97f59..7ffc5707f8b9 100644
--- a/drivers/rtc/rtc-da9063.c
+++ b/drivers/rtc/rtc-da9063.c
@@ -341,7 +341,6 @@ static struct platform_driver da9063_rtc_driver = {
 	.probe		= da9063_rtc_probe,
 	.driver		= {
 		.name	= DA9063_DRVNAME_RTC,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-davinci.c b/drivers/rtc/rtc-davinci.c
index c0a3b59f65a2..c84f46168a52 100644
--- a/drivers/rtc/rtc-davinci.c
+++ b/drivers/rtc/rtc-davinci.c
@@ -550,7 +550,6 @@ static struct platform_driver davinci_rtc_driver = {
 	.remove		= __exit_p(davinci_rtc_remove),
 	.driver		= {
 		.name = "rtc_davinci",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-dm355evm.c b/drivers/rtc/rtc-dm355evm.c
index 1aca08394c47..94067f8eeb10 100644
--- a/drivers/rtc/rtc-dm355evm.c
+++ b/drivers/rtc/rtc-dm355evm.c
@@ -146,7 +146,6 @@ static int dm355evm_rtc_probe(struct platform_device *pdev)
 static struct platform_driver rtc_dm355evm_driver = {
 	.probe		= dm355evm_rtc_probe,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "rtc-dm355evm",
 	},
 };
diff --git a/drivers/rtc/rtc-ds1216.c b/drivers/rtc/rtc-ds1216.c
index 9c04fd2bc209..d16f550897b8 100644
--- a/drivers/rtc/rtc-ds1216.c
+++ b/drivers/rtc/rtc-ds1216.c
@@ -170,7 +170,6 @@ static int __init ds1216_rtc_probe(struct platform_device *pdev)
 static struct platform_driver ds1216_rtc_platform_driver = {
 	.driver		= {
 		.name	= "rtc-ds1216",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-ds1286.c b/drivers/rtc/rtc-ds1286.c
index 50e109b78252..2fe537f4e2bd 100644
--- a/drivers/rtc/rtc-ds1286.c
+++ b/drivers/rtc/rtc-ds1286.c
@@ -356,7 +356,6 @@ static int ds1286_probe(struct platform_device *pdev)
 static struct platform_driver ds1286_platform_driver = {
 	.driver		= {
 		.name	= "rtc-ds1286",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= ds1286_probe,
 };
diff --git a/drivers/rtc/rtc-ds1302.c b/drivers/rtc/rtc-ds1302.c
index 07e8d79b4a09..6bef7a5233c4 100644
--- a/drivers/rtc/rtc-ds1302.c
+++ b/drivers/rtc/rtc-ds1302.c
@@ -244,7 +244,6 @@ static int __init ds1302_rtc_probe(struct platform_device *pdev)
 static struct platform_driver ds1302_platform_driver = {
 	.driver		= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-ds1511.c b/drivers/rtc/rtc-ds1511.c
index b13d1399b81a..7415c2b4d6e8 100644
--- a/drivers/rtc/rtc-ds1511.c
+++ b/drivers/rtc/rtc-ds1511.c
@@ -561,7 +561,6 @@ static struct platform_driver ds1511_rtc_driver = {
 	.remove		= ds1511_rtc_remove,
 	.driver		= {
 		.name	= "ds1511",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-ds1553.c b/drivers/rtc/rtc-ds1553.c
index ab56893aac73..a24e091bcb41 100644
--- a/drivers/rtc/rtc-ds1553.c
+++ b/drivers/rtc/rtc-ds1553.c
@@ -351,7 +351,6 @@ static struct platform_driver ds1553_rtc_driver = {
 	.remove		= ds1553_rtc_remove,
 	.driver		= {
 		.name	= "rtc-ds1553",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-ds1742.c b/drivers/rtc/rtc-ds1742.c
index 9822715db8ba..0f8d8ace1515 100644
--- a/drivers/rtc/rtc-ds1742.c
+++ b/drivers/rtc/rtc-ds1742.c
@@ -230,7 +230,6 @@ static struct platform_driver ds1742_rtc_driver = {
 	.remove		= ds1742_rtc_remove,
 	.driver		= {
 		.name	= "rtc-ds1742",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(ds1742_rtc_of_match),
 	},
 };
diff --git a/drivers/rtc/rtc-ds2404.c b/drivers/rtc/rtc-ds2404.c
index fc209dc4e245..7885edd3d507 100644
--- a/drivers/rtc/rtc-ds2404.c
+++ b/drivers/rtc/rtc-ds2404.c
@@ -274,7 +274,6 @@ static struct platform_driver rtc_device_driver = {
 	.remove = rtc_remove,
 	.driver = {
 		.name	= "ds2404",
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(rtc_device_driver);
diff --git a/drivers/rtc/rtc-efi.c b/drivers/rtc/rtc-efi.c
index 53b589dc34eb..b37b0c80bd5a 100644
--- a/drivers/rtc/rtc-efi.c
+++ b/drivers/rtc/rtc-efi.c
@@ -226,7 +226,6 @@ static int __init efi_rtc_probe(struct platform_device *dev)
 static struct platform_driver efi_rtc_driver = {
 	.driver = {
 		.name = "rtc-efi",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-ep93xx.c b/drivers/rtc/rtc-ep93xx.c
index 5e4f5dc40ba5..de325d68c7e4 100644
--- a/drivers/rtc/rtc-ep93xx.c
+++ b/drivers/rtc/rtc-ep93xx.c
@@ -174,7 +174,6 @@ static int ep93xx_rtc_remove(struct platform_device *pdev)
 static struct platform_driver ep93xx_rtc_driver = {
 	.driver		= {
 		.name	= "ep93xx-rtc",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= ep93xx_rtc_probe,
 	.remove		= ep93xx_rtc_remove,
diff --git a/drivers/rtc/rtc-generic.c b/drivers/rtc/rtc-generic.c
index 9b6725ebbfb2..e782ebd719b2 100644
--- a/drivers/rtc/rtc-generic.c
+++ b/drivers/rtc/rtc-generic.c
@@ -51,7 +51,6 @@ static int __init generic_rtc_probe(struct platform_device *dev)
 static struct platform_driver generic_rtc_driver = {
 	.driver = {
 		.name = "rtc-generic",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-hid-sensor-time.c b/drivers/rtc/rtc-hid-sensor-time.c
index 965a9da70867..ae7c2ba440cf 100644
--- a/drivers/rtc/rtc-hid-sensor-time.c
+++ b/drivers/rtc/rtc-hid-sensor-time.c
@@ -331,7 +331,6 @@ static struct platform_driver hid_time_platform_driver = {
 	.id_table = hid_time_ids,
 	.driver = {
 		.name	= KBUILD_MODNAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= hid_time_probe,
 	.remove		= hid_time_remove,
diff --git a/drivers/rtc/rtc-imxdi.c b/drivers/rtc/rtc-imxdi.c
index cd741c77e085..42f5570f42f8 100644
--- a/drivers/rtc/rtc-imxdi.c
+++ b/drivers/rtc/rtc-imxdi.c
@@ -499,7 +499,6 @@ MODULE_DEVICE_TABLE(of, dryice_dt_ids);
 static struct platform_driver dryice_rtc_driver = {
 	.driver = {
 		   .name = "imxdi_rtc",
-		   .owner = THIS_MODULE,
 		   .of_match_table = of_match_ptr(dryice_dt_ids),
 		   },
 	.remove = __exit_p(dryice_rtc_remove),
diff --git a/drivers/rtc/rtc-jz4740.c b/drivers/rtc/rtc-jz4740.c
index 08f5160fb6d4..b2bcfc0bf2e5 100644
--- a/drivers/rtc/rtc-jz4740.c
+++ b/drivers/rtc/rtc-jz4740.c
@@ -299,7 +299,6 @@ static struct platform_driver jz4740_rtc_driver = {
 	.probe	 = jz4740_rtc_probe,
 	.driver	 = {
 		.name  = "jz4740-rtc",
-		.owner = THIS_MODULE,
 		.pm    = JZ4740_RTC_PM_OPS,
 	},
 };
diff --git a/drivers/rtc/rtc-lp8788.c b/drivers/rtc/rtc-lp8788.c
index 4ff6c73253b3..e20e7bd822e0 100644
--- a/drivers/rtc/rtc-lp8788.c
+++ b/drivers/rtc/rtc-lp8788.c
@@ -316,7 +316,6 @@ static struct platform_driver lp8788_rtc_driver = {
 	.probe = lp8788_rtc_probe,
 	.driver = {
 		.name = LP8788_DEV_RTC,
-		.owner = THIS_MODULE,
 	},
 };
 module_platform_driver(lp8788_rtc_driver);
diff --git a/drivers/rtc/rtc-lpc32xx.c b/drivers/rtc/rtc-lpc32xx.c
index f130c08c98f8..f923f7324788 100644
--- a/drivers/rtc/rtc-lpc32xx.c
+++ b/drivers/rtc/rtc-lpc32xx.c
@@ -379,7 +379,6 @@ static struct platform_driver lpc32xx_rtc_driver = {
 	.remove		= lpc32xx_rtc_remove,
 	.driver = {
 		.name	= RTC_NAME,
-		.owner	= THIS_MODULE,
 		.pm	= LPC32XX_RTC_PM_OPS,
 		.of_match_table = of_match_ptr(lpc32xx_rtc_match),
 	},
diff --git a/drivers/rtc/rtc-ls1x.c b/drivers/rtc/rtc-ls1x.c
index 682ecb094839..8445e564094a 100644
--- a/drivers/rtc/rtc-ls1x.c
+++ b/drivers/rtc/rtc-ls1x.c
@@ -188,7 +188,6 @@ err:
 static struct platform_driver  ls1x_rtc_driver = {
 	.driver		= {
 		.name	= "ls1x-rtc",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= ls1x_rtc_probe,
 };
diff --git a/drivers/rtc/rtc-m48t35.c b/drivers/rtc/rtc-m48t35.c
index 411adb3f86a1..c62b51217ecf 100644
--- a/drivers/rtc/rtc-m48t35.c
+++ b/drivers/rtc/rtc-m48t35.c
@@ -181,7 +181,6 @@ static int m48t35_probe(struct platform_device *pdev)
 static struct platform_driver m48t35_platform_driver = {
 	.driver		= {
 		.name	= "rtc-m48t35",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= m48t35_probe,
 };
diff --git a/drivers/rtc/rtc-m48t59.c b/drivers/rtc/rtc-m48t59.c
index 11880c1e9dac..90abb5bd589c 100644
--- a/drivers/rtc/rtc-m48t59.c
+++ b/drivers/rtc/rtc-m48t59.c
@@ -504,7 +504,6 @@ MODULE_ALIAS("platform:rtc-m48t59");
 static struct platform_driver m48t59_rtc_driver = {
 	.driver		= {
 		.name	= "rtc-m48t59",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= m48t59_rtc_probe,
 	.remove		= m48t59_rtc_remove,
diff --git a/drivers/rtc/rtc-m48t86.c b/drivers/rtc/rtc-m48t86.c
index 32f64c942621..a17b7a3ceece 100644
--- a/drivers/rtc/rtc-m48t86.c
+++ b/drivers/rtc/rtc-m48t86.c
@@ -169,7 +169,6 @@ static int m48t86_rtc_probe(struct platform_device *dev)
 static struct platform_driver m48t86_rtc_platform_driver = {
 	.driver		= {
 		.name	= "rtc-m48t86",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= m48t86_rtc_probe,
 };
diff --git a/drivers/rtc/rtc-max77686.c b/drivers/rtc/rtc-max77686.c
index cf73e969c8cc..9d71328e59b9 100644
--- a/drivers/rtc/rtc-max77686.c
+++ b/drivers/rtc/rtc-max77686.c
@@ -513,7 +513,6 @@ static const struct platform_device_id rtc_id[] = {
 static struct platform_driver max77686_rtc_driver = {
 	.driver		= {
 		.name	= "max77686-rtc",
-		.owner	= THIS_MODULE,
 		.pm	= &max77686_rtc_pm_ops,
 	},
 	.probe		= max77686_rtc_probe,
diff --git a/drivers/rtc/rtc-max77802.c b/drivers/rtc/rtc-max77802.c
index 566471335b33..7f8adf8d6feb 100644
--- a/drivers/rtc/rtc-max77802.c
+++ b/drivers/rtc/rtc-max77802.c
@@ -488,7 +488,6 @@ static const struct platform_device_id rtc_id[] = {
 static struct platform_driver max77802_rtc_driver = {
 	.driver		= {
 		.name	= "max77802-rtc",
-		.owner	= THIS_MODULE,
 		.pm	= &max77802_rtc_pm_ops,
 	},
 	.probe		= max77802_rtc_probe,
diff --git a/drivers/rtc/rtc-max8907.c b/drivers/rtc/rtc-max8907.c
index 3032178bd9e6..19c29b72598d 100644
--- a/drivers/rtc/rtc-max8907.c
+++ b/drivers/rtc/rtc-max8907.c
@@ -215,7 +215,6 @@ static int max8907_rtc_probe(struct platform_device *pdev)
 static struct platform_driver max8907_rtc_driver = {
 	.driver = {
 		.name = "max8907-rtc",
-		.owner = THIS_MODULE,
 	},
 	.probe = max8907_rtc_probe,
 };
diff --git a/drivers/rtc/rtc-max8925.c b/drivers/rtc/rtc-max8925.c
index 951d1a78e190..16d129a0bb3b 100644
--- a/drivers/rtc/rtc-max8925.c
+++ b/drivers/rtc/rtc-max8925.c
@@ -314,7 +314,6 @@ static SIMPLE_DEV_PM_OPS(max8925_rtc_pm_ops, max8925_rtc_suspend, max8925_rtc_re
 static struct platform_driver max8925_rtc_driver = {
 	.driver		= {
 		.name	= "max8925-rtc",
-		.owner	= THIS_MODULE,
 		.pm     = &max8925_rtc_pm_ops,
 	},
 	.probe		= max8925_rtc_probe,
diff --git a/drivers/rtc/rtc-max8997.c b/drivers/rtc/rtc-max8997.c
index 0777c01b58e0..67fbe559d535 100644
--- a/drivers/rtc/rtc-max8997.c
+++ b/drivers/rtc/rtc-max8997.c
@@ -523,7 +523,6 @@ static const struct platform_device_id rtc_id[] = {
 static struct platform_driver max8997_rtc_driver = {
 	.driver		= {
 		.name	= "max8997-rtc",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= max8997_rtc_probe,
 	.shutdown	= max8997_rtc_shutdown,
diff --git a/drivers/rtc/rtc-max8998.c b/drivers/rtc/rtc-max8998.c
index f098ad8382de..5726ef7bd56e 100644
--- a/drivers/rtc/rtc-max8998.c
+++ b/drivers/rtc/rtc-max8998.c
@@ -313,7 +313,6 @@ static const struct platform_device_id max8998_rtc_id[] = {
 static struct platform_driver max8998_rtc_driver = {
 	.driver		= {
 		.name	= "max8998-rtc",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= max8998_rtc_probe,
 	.id_table	= max8998_rtc_id,
diff --git a/drivers/rtc/rtc-mc13xxx.c b/drivers/rtc/rtc-mc13xxx.c
index 0765606a2d14..5bce904b7ee6 100644
--- a/drivers/rtc/rtc-mc13xxx.c
+++ b/drivers/rtc/rtc-mc13xxx.c
@@ -370,7 +370,6 @@ static struct platform_driver mc13xxx_rtc_driver = {
 	.remove = mc13xxx_rtc_remove,
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-moxart.c b/drivers/rtc/rtc-moxart.c
index c31846238871..73759c9a4527 100644
--- a/drivers/rtc/rtc-moxart.c
+++ b/drivers/rtc/rtc-moxart.c
@@ -317,7 +317,6 @@ static struct platform_driver moxart_rtc_driver = {
 	.probe	= moxart_rtc_probe,
 	.driver	= {
 		.name		= "moxart-rtc",
-		.owner		= THIS_MODULE,
 		.of_match_table	= moxart_rtc_match,
 	},
 };
diff --git a/drivers/rtc/rtc-mpc5121.c b/drivers/rtc/rtc-mpc5121.c
index 3b965ad6f4d5..1767e18d5bd4 100644
--- a/drivers/rtc/rtc-mpc5121.c
+++ b/drivers/rtc/rtc-mpc5121.c
@@ -411,7 +411,6 @@ static const struct of_device_id mpc5121_rtc_match[] = {
 static struct platform_driver mpc5121_rtc_driver = {
 	.driver = {
 		.name = "mpc5121-rtc",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(mpc5121_rtc_match),
 	},
 	.probe = mpc5121_rtc_probe,
diff --git a/drivers/rtc/rtc-msm6242.c b/drivers/rtc/rtc-msm6242.c
index 426cb5189daa..9bf877bdf836 100644
--- a/drivers/rtc/rtc-msm6242.c
+++ b/drivers/rtc/rtc-msm6242.c
@@ -225,7 +225,6 @@ static int __init msm6242_rtc_probe(struct platform_device *pdev)
 static struct platform_driver msm6242_rtc_driver = {
 	.driver	= {
 		.name	= "rtc-msm6242",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-mv.c b/drivers/rtc/rtc-mv.c
index 6aaec2fc7c0d..423762241042 100644
--- a/drivers/rtc/rtc-mv.c
+++ b/drivers/rtc/rtc-mv.c
@@ -329,7 +329,6 @@ static struct platform_driver mv_rtc_driver = {
 	.remove		= __exit_p(mv_rtc_remove),
 	.driver		= {
 		.name	= "rtc-mv",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(rtc_mv_of_match_table),
 	},
 };
diff --git a/drivers/rtc/rtc-mxc.c b/drivers/rtc/rtc-mxc.c
index 419874fefa4b..3c3f8d10ab43 100644
--- a/drivers/rtc/rtc-mxc.c
+++ b/drivers/rtc/rtc-mxc.c
@@ -489,7 +489,6 @@ static struct platform_driver mxc_rtc_driver = {
 	.driver = {
 		   .name	= "mxc_rtc",
 		   .pm		= &mxc_rtc_pm_ops,
-		   .owner	= THIS_MODULE,
 	},
 	.id_table = imx_rtc_devtype,
 	.probe = mxc_rtc_probe,
diff --git a/drivers/rtc/rtc-nuc900.c b/drivers/rtc/rtc-nuc900.c
index a53da0958e95..09fc1c19f0df 100644
--- a/drivers/rtc/rtc-nuc900.c
+++ b/drivers/rtc/rtc-nuc900.c
@@ -262,7 +262,6 @@ static int __init nuc900_rtc_probe(struct platform_device *pdev)
 static struct platform_driver nuc900_rtc_driver = {
 	.driver		= {
 		.name	= "nuc900-rtc",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-omap.c b/drivers/rtc/rtc-omap.c
index 4f1c6ca97211..8e5851aa4369 100644
--- a/drivers/rtc/rtc-omap.c
+++ b/drivers/rtc/rtc-omap.c
@@ -719,7 +719,6 @@ static struct platform_driver omap_rtc_driver = {
 	.shutdown	= omap_rtc_shutdown,
 	.driver		= {
 		.name	= "omap_rtc",
-		.owner	= THIS_MODULE,
 		.pm	= &omap_rtc_pm_ops,
 		.of_match_table = omap_rtc_of_match,
 	},
diff --git a/drivers/rtc/rtc-opal.c b/drivers/rtc/rtc-opal.c
new file mode 100644
index 000000000000..95f652165fe9
--- /dev/null
+++ b/drivers/rtc/rtc-opal.c
@@ -0,0 +1,261 @@
+/*
+ * IBM OPAL RTC driver
+ * Copyright (C) 2014 IBM
+ *
+ * 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.
+ */
+
+#define DRVNAME		"rtc-opal"
+#define pr_fmt(fmt)	DRVNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/rtc.h>
+#include <linux/delay.h>
+#include <linux/bcd.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <asm/opal.h>
+#include <asm/firmware.h>
+
+static void opal_to_tm(u32 y_m_d, u64 h_m_s_ms, struct rtc_time *tm)
+{
+	tm->tm_year = ((bcd2bin(y_m_d >> 24) * 100) +
+		       bcd2bin((y_m_d >> 16) & 0xff)) - 1900;
+	tm->tm_mon  = bcd2bin((y_m_d >> 8) & 0xff) - 1;
+	tm->tm_mday = bcd2bin(y_m_d & 0xff);
+	tm->tm_hour = bcd2bin((h_m_s_ms >> 56) & 0xff);
+	tm->tm_min  = bcd2bin((h_m_s_ms >> 48) & 0xff);
+	tm->tm_sec  = bcd2bin((h_m_s_ms >> 40) & 0xff);
+
+	GregorianDay(tm);
+}
+
+static void tm_to_opal(struct rtc_time *tm, u32 *y_m_d, u64 *h_m_s_ms)
+{
+	*y_m_d |= ((u32)bin2bcd((tm->tm_year + 1900) / 100)) << 24;
+	*y_m_d |= ((u32)bin2bcd((tm->tm_year + 1900) % 100)) << 16;
+	*y_m_d |= ((u32)bin2bcd((tm->tm_mon + 1))) << 8;
+	*y_m_d |= ((u32)bin2bcd(tm->tm_mday));
+
+	*h_m_s_ms |= ((u64)bin2bcd(tm->tm_hour)) << 56;
+	*h_m_s_ms |= ((u64)bin2bcd(tm->tm_min)) << 48;
+	*h_m_s_ms |= ((u64)bin2bcd(tm->tm_sec)) << 40;
+}
+
+static int opal_get_rtc_time(struct device *dev, struct rtc_time *tm)
+{
+	long rc = OPAL_BUSY;
+	u32 y_m_d;
+	u64 h_m_s_ms;
+	__be32 __y_m_d;
+	__be64 __h_m_s_ms;
+
+	while (rc == OPAL_BUSY || rc == OPAL_BUSY_EVENT) {
+		rc = opal_rtc_read(&__y_m_d, &__h_m_s_ms);
+		if (rc == OPAL_BUSY_EVENT)
+			opal_poll_events(NULL);
+		else
+			msleep(10);
+	}
+
+	if (rc != OPAL_SUCCESS)
+		return -EIO;
+
+	y_m_d = be32_to_cpu(__y_m_d);
+	h_m_s_ms = be64_to_cpu(__h_m_s_ms);
+	opal_to_tm(y_m_d, h_m_s_ms, tm);
+
+	return 0;
+}
+
+static int opal_set_rtc_time(struct device *dev, struct rtc_time *tm)
+{
+	long rc = OPAL_BUSY;
+	u32 y_m_d = 0;
+	u64 h_m_s_ms = 0;
+
+	tm_to_opal(tm, &y_m_d, &h_m_s_ms);
+	while (rc == OPAL_BUSY || rc == OPAL_BUSY_EVENT) {
+		rc = opal_rtc_write(y_m_d, h_m_s_ms);
+		if (rc == OPAL_BUSY_EVENT)
+			opal_poll_events(NULL);
+		else
+			msleep(10);
+	}
+
+	return rc == OPAL_SUCCESS ? 0 : -EIO;
+}
+
+/*
+ * TPO	Timed Power-On
+ *
+ * TPO get/set OPAL calls care about the hour and min and to make it consistent
+ * with the rtc utility time conversion functions, we use the 'u64' to store
+ * its value and perform bit shift by 32 before use..
+ */
+static int opal_get_tpo_time(struct device *dev, struct rtc_wkalrm *alarm)
+{
+	__be32 __y_m_d, __h_m;
+	struct opal_msg msg;
+	int rc, token;
+	u64 h_m_s_ms;
+	u32 y_m_d;
+
+	token = opal_async_get_token_interruptible();
+	if (token < 0) {
+		if (token != -ERESTARTSYS)
+			pr_err("Failed to get the async token\n");
+
+		return token;
+	}
+
+	rc = opal_tpo_read(token, &__y_m_d, &__h_m);
+	if (rc != OPAL_ASYNC_COMPLETION) {
+		rc = -EIO;
+		goto exit;
+	}
+
+	rc = opal_async_wait_response(token, &msg);
+	if (rc) {
+		rc = -EIO;
+		goto exit;
+	}
+
+	rc = be64_to_cpu(msg.params[1]);
+	if (rc != OPAL_SUCCESS) {
+		rc = -EIO;
+		goto exit;
+	}
+
+	y_m_d = be32_to_cpu(__y_m_d);
+	h_m_s_ms = ((u64)be32_to_cpu(__h_m) << 32);
+	opal_to_tm(y_m_d, h_m_s_ms, &alarm->time);
+
+exit:
+	opal_async_release_token(token);
+	return rc;
+}
+
+/* Set Timed Power-On */
+static int opal_set_tpo_time(struct device *dev, struct rtc_wkalrm *alarm)
+{
+	u64 h_m_s_ms = 0, token;
+	struct opal_msg msg;
+	u32 y_m_d = 0;
+	int rc;
+
+	tm_to_opal(&alarm->time, &y_m_d, &h_m_s_ms);
+
+	token = opal_async_get_token_interruptible();
+	if (token < 0) {
+		if (token != -ERESTARTSYS)
+			pr_err("Failed to get the async token\n");
+
+		return token;
+	}
+
+	/* TPO, we care about hour and minute */
+	rc = opal_tpo_write(token, y_m_d,
+			    (u32)((h_m_s_ms >> 32) & 0xffff0000));
+	if (rc != OPAL_ASYNC_COMPLETION) {
+		rc = -EIO;
+		goto exit;
+	}
+
+	rc = opal_async_wait_response(token, &msg);
+	if (rc) {
+		rc = -EIO;
+		goto exit;
+	}
+
+	rc = be64_to_cpu(msg.params[1]);
+	if (rc != OPAL_SUCCESS)
+		rc = -EIO;
+
+exit:
+	opal_async_release_token(token);
+	return rc;
+}
+
+static const struct rtc_class_ops opal_rtc_ops = {
+	.read_time	= opal_get_rtc_time,
+	.set_time	= opal_set_rtc_time,
+	.read_alarm	= opal_get_tpo_time,
+	.set_alarm	= opal_set_tpo_time,
+};
+
+static int opal_rtc_probe(struct platform_device *pdev)
+{
+	struct rtc_device *rtc;
+
+	if (pdev->dev.of_node && of_get_property(pdev->dev.of_node, "has-tpo",
+						 NULL))
+		device_set_wakeup_capable(&pdev->dev, true);
+
+	rtc = devm_rtc_device_register(&pdev->dev, DRVNAME, &opal_rtc_ops,
+				       THIS_MODULE);
+	if (IS_ERR(rtc))
+		return PTR_ERR(rtc);
+
+	rtc->uie_unsupported = 1;
+
+	return 0;
+}
+
+static const struct of_device_id opal_rtc_match[] = {
+	{
+		.compatible	= "ibm,opal-rtc",
+	},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, opal_rtc_match);
+
+static const struct platform_device_id opal_rtc_driver_ids[] = {
+	{
+		.name		= "opal-rtc",
+	},
+	{ }
+};
+MODULE_DEVICE_TABLE(platform, opal_rtc_driver_ids);
+
+static struct platform_driver opal_rtc_driver = {
+	.probe		= opal_rtc_probe,
+	.id_table	= opal_rtc_driver_ids,
+	.driver		= {
+		.name		= DRVNAME,
+		.owner		= THIS_MODULE,
+		.of_match_table	= opal_rtc_match,
+	},
+};
+
+static int __init opal_rtc_init(void)
+{
+	if (!firmware_has_feature(FW_FEATURE_OPAL))
+		return -ENODEV;
+
+	return platform_driver_register(&opal_rtc_driver);
+}
+
+static void __exit opal_rtc_exit(void)
+{
+	platform_driver_unregister(&opal_rtc_driver);
+}
+
+MODULE_AUTHOR("Neelesh Gupta <neelegup@linux.vnet.ibm.com>");
+MODULE_DESCRIPTION("IBM OPAL RTC driver");
+MODULE_LICENSE("GPL");
+
+module_init(opal_rtc_init);
+module_exit(opal_rtc_exit);
diff --git a/drivers/rtc/rtc-palmas.c b/drivers/rtc/rtc-palmas.c
index 4dfe2d793fa3..3b01d567496d 100644
--- a/drivers/rtc/rtc-palmas.c
+++ b/drivers/rtc/rtc-palmas.c
@@ -363,7 +363,6 @@ static struct platform_driver palmas_rtc_driver = {
 	.probe		= palmas_rtc_probe,
 	.remove		= palmas_rtc_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "palmas-rtc",
 		.pm	= &palmas_rtc_pm_ops,
 		.of_match_table = of_match_ptr(of_palmas_rtc_match),
diff --git a/drivers/rtc/rtc-pcap.c b/drivers/rtc/rtc-pcap.c
index 40b5c630bc7d..c4433240d8a9 100644
--- a/drivers/rtc/rtc-pcap.c
+++ b/drivers/rtc/rtc-pcap.c
@@ -184,7 +184,6 @@ static struct platform_driver pcap_rtc_driver = {
 	.remove = __exit_p(pcap_rtc_remove),
 	.driver = {
 		.name  = "pcap-rtc",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-pm8xxx.c b/drivers/rtc/rtc-pm8xxx.c
index 5adcf111fc14..795fcbd02ea3 100644
--- a/drivers/rtc/rtc-pm8xxx.c
+++ b/drivers/rtc/rtc-pm8xxx.c
@@ -531,7 +531,6 @@ static struct platform_driver pm8xxx_rtc_driver = {
 	.probe		= pm8xxx_rtc_probe,
 	.driver	= {
 		.name		= "rtc-pm8xxx",
-		.owner		= THIS_MODULE,
 		.pm		= &pm8xxx_rtc_pm_ops,
 		.of_match_table	= pm8xxx_id_table,
 	},
diff --git a/drivers/rtc/rtc-ps3.c b/drivers/rtc/rtc-ps3.c
index 554ada5e9b76..6a8f5d758eac 100644
--- a/drivers/rtc/rtc-ps3.c
+++ b/drivers/rtc/rtc-ps3.c
@@ -74,7 +74,6 @@ static int __init ps3_rtc_probe(struct platform_device *dev)
 static struct platform_driver ps3_rtc_driver = {
 	.driver = {
 		.name = "rtc-ps3",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-puv3.c b/drivers/rtc/rtc-puv3.c
index 1cff2a21db67..c0a6e638c672 100644
--- a/drivers/rtc/rtc-puv3.c
+++ b/drivers/rtc/rtc-puv3.c
@@ -328,7 +328,6 @@ static struct platform_driver puv3_rtc_driver = {
 	.remove		= puv3_rtc_remove,
 	.driver		= {
 		.name	= "PKUnity-v3-RTC",
-		.owner	= THIS_MODULE,
 		.pm	= &puv3_rtc_pm_ops,
 	}
 };
diff --git a/drivers/rtc/rtc-rc5t583.c b/drivers/rtc/rtc-rc5t583.c
index e53e9b1c69b3..f28d57788951 100644
--- a/drivers/rtc/rtc-rc5t583.c
+++ b/drivers/rtc/rtc-rc5t583.c
@@ -310,7 +310,6 @@ static struct platform_driver rc5t583_rtc_driver = {
 	.probe		= rc5t583_rtc_probe,
 	.remove		= rc5t583_rtc_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "rtc-rc5t583",
 		.pm	= &rc5t583_rtc_pm_ops,
 	},
diff --git a/drivers/rtc/rtc-rp5c01.c b/drivers/rtc/rtc-rp5c01.c
index 89d073679267..b548551f385c 100644
--- a/drivers/rtc/rtc-rp5c01.c
+++ b/drivers/rtc/rtc-rp5c01.c
@@ -273,7 +273,6 @@ static int __exit rp5c01_rtc_remove(struct platform_device *dev)
 static struct platform_driver rp5c01_rtc_driver = {
 	.driver	= {
 		.name	= "rtc-rp5c01",
-		.owner	= THIS_MODULE,
 	},
 	.remove	= __exit_p(rp5c01_rtc_remove),
 };
diff --git a/drivers/rtc/rtc-rs5c313.c b/drivers/rtc/rtc-rs5c313.c
index 68f7856422f1..5f48167c802a 100644
--- a/drivers/rtc/rtc-rs5c313.c
+++ b/drivers/rtc/rtc-rs5c313.c
@@ -381,7 +381,6 @@ static int rs5c313_rtc_probe(struct platform_device *pdev)
 static struct platform_driver rs5c313_rtc_platform_driver = {
 	.driver         = {
 		.name   = DRV_NAME,
-		.owner  = THIS_MODULE,
 	},
 	.probe	= rs5c313_rtc_probe,
 };
diff --git a/drivers/rtc/rtc-s3c.c b/drivers/rtc/rtc-s3c.c
index 806072238c00..4241eeab3386 100644
--- a/drivers/rtc/rtc-s3c.c
+++ b/drivers/rtc/rtc-s3c.c
@@ -896,7 +896,6 @@ static struct platform_driver s3c_rtc_driver = {
 	.remove		= s3c_rtc_remove,
 	.driver		= {
 		.name	= "s3c-rtc",
-		.owner	= THIS_MODULE,
 		.pm	= &s3c_rtc_pm_ops,
 		.of_match_table	= of_match_ptr(s3c_rtc_dt_match),
 	},
diff --git a/drivers/rtc/rtc-s5m.c b/drivers/rtc/rtc-s5m.c
index 8754c33361e8..b5e7c4670205 100644
--- a/drivers/rtc/rtc-s5m.c
+++ b/drivers/rtc/rtc-s5m.c
@@ -837,7 +837,6 @@ static const struct platform_device_id s5m_rtc_id[] = {
 static struct platform_driver s5m_rtc_driver = {
 	.driver		= {
 		.name	= "s5m-rtc",
-		.owner	= THIS_MODULE,
 		.pm	= &s5m_rtc_pm_ops,
 	},
 	.probe		= s5m_rtc_probe,
diff --git a/drivers/rtc/rtc-sh.c b/drivers/rtc/rtc-sh.c
index d0d2b047658b..2b81dd4baf17 100644
--- a/drivers/rtc/rtc-sh.c
+++ b/drivers/rtc/rtc-sh.c
@@ -782,7 +782,6 @@ static SIMPLE_DEV_PM_OPS(sh_rtc_pm_ops, sh_rtc_suspend, sh_rtc_resume);
 static struct platform_driver sh_rtc_platform_driver = {
 	.driver		= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 		.pm	= &sh_rtc_pm_ops,
 	},
 	.remove		= __exit_p(sh_rtc_remove),
diff --git a/drivers/rtc/rtc-sirfsoc.c b/drivers/rtc/rtc-sirfsoc.c
index d2ac6688e5c7..edc3b43282d4 100644
--- a/drivers/rtc/rtc-sirfsoc.c
+++ b/drivers/rtc/rtc-sirfsoc.c
@@ -444,7 +444,6 @@ static SIMPLE_DEV_PM_OPS(sirfsoc_rtc_pm_ops,
 static struct platform_driver sirfsoc_rtc_driver = {
 	.driver = {
 		.name = "sirfsoc-rtc",
-		.owner = THIS_MODULE,
 		.pm = &sirfsoc_rtc_pm_ops,
 		.of_match_table = sirfsoc_rtc_of_match,
 	},
diff --git a/drivers/rtc/rtc-snvs.c b/drivers/rtc/rtc-snvs.c
index 2cd8ffe5c698..0479e807a776 100644
--- a/drivers/rtc/rtc-snvs.c
+++ b/drivers/rtc/rtc-snvs.c
@@ -344,13 +344,20 @@ static int snvs_rtc_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static const struct dev_pm_ops snvs_rtc_pm_ops = {
 	.suspend_noirq = snvs_rtc_suspend,
 	.resume_noirq = snvs_rtc_resume,
 };
 
+#define SNVS_RTC_PM_OPS	(&snvs_rtc_pm_ops)
+
+#else
+
+#define SNVS_RTC_PM_OPS	NULL
+
+#endif
+
 static const struct of_device_id snvs_dt_ids[] = {
 	{ .compatible = "fsl,sec-v4.0-mon-rtc-lp", },
 	{ /* sentinel */ }
@@ -360,8 +367,7 @@ MODULE_DEVICE_TABLE(of, snvs_dt_ids);
 static struct platform_driver snvs_rtc_driver = {
 	.driver = {
 		.name	= "snvs_rtc",
-		.owner	= THIS_MODULE,
-		.pm	= &snvs_rtc_pm_ops,
+		.pm	= SNVS_RTC_PM_OPS,
 		.of_match_table = snvs_dt_ids,
 	},
 	.probe		= snvs_rtc_probe,
diff --git a/drivers/rtc/rtc-starfire.c b/drivers/rtc/rtc-starfire.c
index f7d8a6db8078..83a057a03060 100644
--- a/drivers/rtc/rtc-starfire.c
+++ b/drivers/rtc/rtc-starfire.c
@@ -54,7 +54,6 @@ static int __init starfire_rtc_probe(struct platform_device *pdev)
 static struct platform_driver starfire_rtc_driver = {
 	.driver		= {
 		.name	= "rtc-starfire",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-stk17ta8.c b/drivers/rtc/rtc-stk17ta8.c
index 35ed49ea1f81..0e93b714ee41 100644
--- a/drivers/rtc/rtc-stk17ta8.c
+++ b/drivers/rtc/rtc-stk17ta8.c
@@ -358,7 +358,6 @@ static struct platform_driver stk17ta8_rtc_driver = {
 	.remove		= stk17ta8_rtc_remove,
 	.driver		= {
 		.name	= "stk17ta8",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-stmp3xxx.c b/drivers/rtc/rtc-stmp3xxx.c
index ea96492357b0..2939cdcb2688 100644
--- a/drivers/rtc/rtc-stmp3xxx.c
+++ b/drivers/rtc/rtc-stmp3xxx.c
@@ -341,7 +341,6 @@ static struct platform_driver stmp3xxx_rtcdrv = {
 	.remove		= stmp3xxx_rtc_remove,
 	.driver		= {
 		.name	= "stmp3xxx-rtc",
-		.owner	= THIS_MODULE,
 		.pm	= &stmp3xxx_rtc_pm_ops,
 		.of_match_table = rtc_dt_ids,
 	},
diff --git a/drivers/rtc/rtc-sun4v.c b/drivers/rtc/rtc-sun4v.c
index bc97ff91341d..7c696c12f28f 100644
--- a/drivers/rtc/rtc-sun4v.c
+++ b/drivers/rtc/rtc-sun4v.c
@@ -95,7 +95,6 @@ static int __init sun4v_rtc_probe(struct platform_device *pdev)
 static struct platform_driver sun4v_rtc_driver = {
 	.driver		= {
 		.name	= "rtc-sun4v",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-sunxi.c b/drivers/rtc/rtc-sunxi.c
index b6f21f73d508..6e678fa4dfaf 100644
--- a/drivers/rtc/rtc-sunxi.c
+++ b/drivers/rtc/rtc-sunxi.c
@@ -511,7 +511,6 @@ static struct platform_driver sunxi_rtc_driver = {
 	.remove		= sunxi_rtc_remove,
 	.driver		= {
 		.name		= "sunxi-rtc",
-		.owner		= THIS_MODULE,
 		.of_match_table = sunxi_rtc_dt_ids,
 	},
 };
diff --git a/drivers/rtc/rtc-tegra.c b/drivers/rtc/rtc-tegra.c
index 76af92ad5a8a..d948277057d8 100644
--- a/drivers/rtc/rtc-tegra.c
+++ b/drivers/rtc/rtc-tegra.c
@@ -420,7 +420,6 @@ static struct platform_driver tegra_rtc_driver = {
 	.shutdown	= tegra_rtc_shutdown,
 	.driver		= {
 		.name	= "tegra_rtc",
-		.owner	= THIS_MODULE,
 		.of_match_table = tegra_rtc_dt_match,
 		.pm	= &tegra_rtc_pm_ops,
 	},
diff --git a/drivers/rtc/rtc-test.c b/drivers/rtc/rtc-test.c
index 6599c20bc454..8f86fa91de1a 100644
--- a/drivers/rtc/rtc-test.c
+++ b/drivers/rtc/rtc-test.c
@@ -129,7 +129,6 @@ static struct platform_driver test_driver = {
 	.remove = test_remove,
 	.driver = {
 		.name = "rtc-test",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-tile.c b/drivers/rtc/rtc-tile.c
index ff9632eb79f2..0b60867d8390 100644
--- a/drivers/rtc/rtc-tile.c
+++ b/drivers/rtc/rtc-tile.c
@@ -94,7 +94,6 @@ static int tile_rtc_probe(struct platform_device *dev)
 static struct platform_driver tile_rtc_platform_driver = {
 	.driver		= {
 		.name	= "rtc-tile",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= tile_rtc_probe,
 };
diff --git a/drivers/rtc/rtc-tps6586x.c b/drivers/rtc/rtc-tps6586x.c
index 426901cef14f..3b6ce80a769c 100644
--- a/drivers/rtc/rtc-tps6586x.c
+++ b/drivers/rtc/rtc-tps6586x.c
@@ -337,7 +337,6 @@ static SIMPLE_DEV_PM_OPS(tps6586x_pm_ops, tps6586x_rtc_suspend,
 static struct platform_driver tps6586x_rtc_driver = {
 	.driver	= {
 		.name	= "tps6586x-rtc",
-		.owner	= THIS_MODULE,
 		.pm	= &tps6586x_pm_ops,
 	},
 	.probe	= tps6586x_rtc_probe,
diff --git a/drivers/rtc/rtc-tps65910.c b/drivers/rtc/rtc-tps65910.c
index 2583349fbde5..f42aa2b2dcba 100644
--- a/drivers/rtc/rtc-tps65910.c
+++ b/drivers/rtc/rtc-tps65910.c
@@ -326,7 +326,6 @@ static struct platform_driver tps65910_rtc_driver = {
 	.probe		= tps65910_rtc_probe,
 	.remove		= tps65910_rtc_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "tps65910-rtc",
 		.pm	= &tps65910_rtc_pm_ops,
 	},
diff --git a/drivers/rtc/rtc-tps80031.c b/drivers/rtc/rtc-tps80031.c
index 3e400dce2d06..27e254cde715 100644
--- a/drivers/rtc/rtc-tps80031.c
+++ b/drivers/rtc/rtc-tps80031.c
@@ -324,7 +324,6 @@ static SIMPLE_DEV_PM_OPS(tps80031_pm_ops, tps80031_rtc_suspend,
 static struct platform_driver tps80031_rtc_driver = {
 	.driver	= {
 		.name	= "tps80031-rtc",
-		.owner	= THIS_MODULE,
 		.pm	= &tps80031_pm_ops,
 	},
 	.probe	= tps80031_rtc_probe,
diff --git a/drivers/rtc/rtc-twl.c b/drivers/rtc/rtc-twl.c
index 1915464e4cd6..5baea3f54926 100644
--- a/drivers/rtc/rtc-twl.c
+++ b/drivers/rtc/rtc-twl.c
@@ -608,7 +608,6 @@ static struct platform_driver twl4030rtc_driver = {
 	.remove		= twl_rtc_remove,
 	.shutdown	= twl_rtc_shutdown,
 	.driver		= {
-		.owner		= THIS_MODULE,
 		.name		= "twl_rtc",
 		.pm		= &twl_rtc_pm_ops,
 		.of_match_table = of_match_ptr(twl_rtc_of_match),
diff --git a/drivers/rtc/rtc-tx4939.c b/drivers/rtc/rtc-tx4939.c
index 2e678c681b13..cb7f94ede516 100644
--- a/drivers/rtc/rtc-tx4939.c
+++ b/drivers/rtc/rtc-tx4939.c
@@ -287,7 +287,6 @@ static struct platform_driver tx4939_rtc_driver = {
 	.remove		= __exit_p(tx4939_rtc_remove),
 	.driver		= {
 		.name	= "tx4939rtc",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-v3020.c b/drivers/rtc/rtc-v3020.c
index 25222cdccdc6..bfbfa7ed7bbf 100644
--- a/drivers/rtc/rtc-v3020.c
+++ b/drivers/rtc/rtc-v3020.c
@@ -382,7 +382,6 @@ static struct platform_driver rtc_device_driver = {
 	.remove = rtc_remove,
 	.driver = {
 		.name	= "v3020",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-vr41xx.c b/drivers/rtc/rtc-vr41xx.c
index 88c9c92e89fd..f64c282275b3 100644
--- a/drivers/rtc/rtc-vr41xx.c
+++ b/drivers/rtc/rtc-vr41xx.c
@@ -375,7 +375,6 @@ static struct platform_driver rtc_platform_driver = {
 	.probe		= rtc_probe,
 	.driver		= {
 		.name	= rtc_name,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/rtc/rtc-vt8500.c b/drivers/rtc/rtc-vt8500.c
index 051da968da6d..a58b6d17e6f0 100644
--- a/drivers/rtc/rtc-vt8500.c
+++ b/drivers/rtc/rtc-vt8500.c
@@ -277,7 +277,6 @@ static struct platform_driver vt8500_rtc_driver = {
 	.remove		= vt8500_rtc_remove,
 	.driver		= {
 		.name	= "vt8500-rtc",
-		.owner	= THIS_MODULE,
 		.of_match_table = wmt_dt_ids,
 	},
 };
diff --git a/drivers/rtc/rtc-xgene.c b/drivers/rtc/rtc-xgene.c
index 14129cc85bdb..65b432a096fe 100644
--- a/drivers/rtc/rtc-xgene.c
+++ b/drivers/rtc/rtc-xgene.c
@@ -264,7 +264,6 @@ static struct platform_driver xgene_rtc_driver = {
 	.probe		= xgene_rtc_probe,
 	.remove		= xgene_rtc_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "xgene-rtc",
 		.pm = &xgene_rtc_pm_ops,
 		.of_match_table	= of_match_ptr(xgene_rtc_of_match),
diff --git a/drivers/s390/block/dasd.c b/drivers/s390/block/dasd.c
index 329db997ee66..4abf11965484 100644
--- a/drivers/s390/block/dasd.c
+++ b/drivers/s390/block/dasd.c
@@ -1377,6 +1377,20 @@ int dasd_term_IO(struct dasd_ccw_req *cqr)
 				      "I/O error, retry");
 			break;
 		case -EINVAL:
+			/*
+			 * device not valid so no I/O could be running
+			 * handle CQR as termination successful
+			 */
+			cqr->status = DASD_CQR_CLEARED;
+			cqr->stopclk = get_tod_clock();
+			cqr->starttime = 0;
+			/* no retries for invalid devices */
+			cqr->retries = -1;
+			DBF_DEV_EVENT(DBF_ERR, device, "%s",
+				      "EINVAL, handle as terminated");
+			/* fake rc to success */
+			rc = 0;
+			break;
 		case -EBUSY:
 			DBF_DEV_EVENT(DBF_ERR, device, "%s",
 				      "device busy, retry later");
@@ -1683,11 +1697,8 @@ void dasd_int_handler(struct ccw_device *cdev, unsigned long intparm,
 	if (cqr->status == DASD_CQR_CLEAR_PENDING &&
 	    scsw_fctl(&irb->scsw) & SCSW_FCTL_CLEAR_FUNC) {
 		cqr->status = DASD_CQR_CLEARED;
-		if (cqr->callback_data == DASD_SLEEPON_START_TAG)
-			cqr->callback_data = DASD_SLEEPON_END_TAG;
 		dasd_device_clear_timer(device);
 		wake_up(&dasd_flush_wq);
-		wake_up(&generic_waitq);
 		dasd_schedule_device_bh(device);
 		return;
 	}
@@ -2326,21 +2337,11 @@ retry:
 			return -EAGAIN;
 
 		/* normal recovery for basedev IO */
-		if (__dasd_sleep_on_erp(cqr)) {
+		if (__dasd_sleep_on_erp(cqr))
+			/* handle erp first */
 			goto retry;
-			/* remember that ERP was needed */
-			rc = 1;
-			/* skip processing for active cqr */
-			if (cqr->status != DASD_CQR_TERMINATED &&
-			    cqr->status != DASD_CQR_NEED_ERP)
-				break;
-		}
 	}
 
-	/* start ERP requests in upper loop */
-	if (rc)
-		goto retry;
-
 	return 0;
 }
 
diff --git a/drivers/s390/block/dasd_genhd.c b/drivers/s390/block/dasd_genhd.c
index f224d59c4b6b..90f39f79f5d7 100644
--- a/drivers/s390/block/dasd_genhd.c
+++ b/drivers/s390/block/dasd_genhd.c
@@ -99,15 +99,37 @@ void dasd_gendisk_free(struct dasd_block *block)
 int dasd_scan_partitions(struct dasd_block *block)
 {
 	struct block_device *bdev;
+	int retry, rc;
 
+	retry = 5;
 	bdev = bdget_disk(block->gdp, 0);
-	if (!bdev || blkdev_get(bdev, FMODE_READ, NULL) < 0)
+	if (!bdev) {
+		DBF_DEV_EVENT(DBF_ERR, block->base, "%s",
+			      "scan partitions error, bdget returned NULL");
 		return -ENODEV;
+	}
+
+	rc = blkdev_get(bdev, FMODE_READ, NULL);
+	if (rc < 0) {
+		DBF_DEV_EVENT(DBF_ERR, block->base,
+			      "scan partitions error, blkdev_get returned %d",
+			      rc);
+		return -ENODEV;
+	}
 	/*
 	 * See fs/partition/check.c:register_disk,rescan_partitions
 	 * Can't call rescan_partitions directly. Use ioctl.
 	 */
-	ioctl_by_bdev(bdev, BLKRRPART, 0);
+	rc = ioctl_by_bdev(bdev, BLKRRPART, 0);
+	while (rc == -EBUSY && retry > 0) {
+		schedule();
+		rc = ioctl_by_bdev(bdev, BLKRRPART, 0);
+		retry--;
+		DBF_DEV_EVENT(DBF_ERR, block->base,
+			      "scan partitions error, retry %d rc %d",
+			      retry, rc);
+	}
+
 	/*
 	 * Since the matching blkdev_put call to the blkdev_get in
 	 * this function is not called before dasd_destroy_partitions
diff --git a/drivers/s390/block/dcssblk.c b/drivers/s390/block/dcssblk.c
index 0f471750327e..b550c8c8d010 100644
--- a/drivers/s390/block/dcssblk.c
+++ b/drivers/s390/block/dcssblk.c
@@ -1003,7 +1003,6 @@ static const struct dev_pm_ops dcssblk_pm_ops = {
 static struct platform_driver dcssblk_pdrv = {
 	.driver = {
 		.name	= "dcssblk",
-		.owner	= THIS_MODULE,
 		.pm	= &dcssblk_pm_ops,
 	},
 };
diff --git a/drivers/s390/block/scm_blk.c b/drivers/s390/block/scm_blk.c
index 56046ab39629..75d9896deccb 100644
--- a/drivers/s390/block/scm_blk.c
+++ b/drivers/s390/block/scm_blk.c
@@ -10,6 +10,7 @@
 
 #include <linux/interrupt.h>
 #include <linux/spinlock.h>
+#include <linux/mempool.h>
 #include <linux/module.h>
 #include <linux/blkdev.h>
 #include <linux/genhd.h>
@@ -20,13 +21,18 @@
 
 debug_info_t *scm_debug;
 static int scm_major;
+static mempool_t *aidaw_pool;
 static DEFINE_SPINLOCK(list_lock);
 static LIST_HEAD(inactive_requests);
 static unsigned int nr_requests = 64;
+static unsigned int nr_requests_per_io = 8;
 static atomic_t nr_devices = ATOMIC_INIT(0);
 module_param(nr_requests, uint, S_IRUGO);
 MODULE_PARM_DESC(nr_requests, "Number of parallel requests.");
 
+module_param(nr_requests_per_io, uint, S_IRUGO);
+MODULE_PARM_DESC(nr_requests_per_io, "Number of requests per IO.");
+
 MODULE_DESCRIPTION("Block driver for s390 storage class memory.");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("scm:scmdev*");
@@ -36,8 +42,8 @@ static void __scm_free_rq(struct scm_request *scmrq)
 	struct aob_rq_header *aobrq = to_aobrq(scmrq);
 
 	free_page((unsigned long) scmrq->aob);
-	free_page((unsigned long) scmrq->aidaw);
 	__scm_free_rq_cluster(scmrq);
+	kfree(scmrq->request);
 	kfree(aobrq);
 }
 
@@ -53,6 +59,8 @@ static void scm_free_rqs(void)
 		__scm_free_rq(scmrq);
 	}
 	spin_unlock_irq(&list_lock);
+
+	mempool_destroy(aidaw_pool);
 }
 
 static int __scm_alloc_rq(void)
@@ -65,17 +73,17 @@ static int __scm_alloc_rq(void)
 		return -ENOMEM;
 
 	scmrq = (void *) aobrq->data;
-	scmrq->aidaw = (void *) get_zeroed_page(GFP_DMA);
 	scmrq->aob = (void *) get_zeroed_page(GFP_DMA);
-	if (!scmrq->aob || !scmrq->aidaw) {
-		__scm_free_rq(scmrq);
-		return -ENOMEM;
-	}
+	if (!scmrq->aob)
+		goto free;
 
-	if (__scm_alloc_rq_cluster(scmrq)) {
-		__scm_free_rq(scmrq);
-		return -ENOMEM;
-	}
+	scmrq->request = kcalloc(nr_requests_per_io, sizeof(scmrq->request[0]),
+				 GFP_KERNEL);
+	if (!scmrq->request)
+		goto free;
+
+	if (__scm_alloc_rq_cluster(scmrq))
+		goto free;
 
 	INIT_LIST_HEAD(&scmrq->list);
 	spin_lock_irq(&list_lock);
@@ -83,12 +91,19 @@ static int __scm_alloc_rq(void)
 	spin_unlock_irq(&list_lock);
 
 	return 0;
+free:
+	__scm_free_rq(scmrq);
+	return -ENOMEM;
 }
 
 static int scm_alloc_rqs(unsigned int nrqs)
 {
 	int ret = 0;
 
+	aidaw_pool = mempool_create_page_pool(max(nrqs/8, 1U), 0);
+	if (!aidaw_pool)
+		return -ENOMEM;
+
 	while (nrqs-- && !ret)
 		ret = __scm_alloc_rq();
 
@@ -112,6 +127,18 @@ out:
 static void scm_request_done(struct scm_request *scmrq)
 {
 	unsigned long flags;
+	struct msb *msb;
+	u64 aidaw;
+	int i;
+
+	for (i = 0; i < nr_requests_per_io && scmrq->request[i]; i++) {
+		msb = &scmrq->aob->msb[i];
+		aidaw = msb->data_addr;
+
+		if ((msb->flags & MSB_FLAG_IDA) && aidaw &&
+		    IS_ALIGNED(aidaw, PAGE_SIZE))
+			mempool_free(virt_to_page(aidaw), aidaw_pool);
+	}
 
 	spin_lock_irqsave(&list_lock, flags);
 	list_add(&scmrq->list, &inactive_requests);
@@ -123,48 +150,90 @@ static bool scm_permit_request(struct scm_blk_dev *bdev, struct request *req)
 	return rq_data_dir(req) != WRITE || bdev->state != SCM_WR_PROHIBIT;
 }
 
-static void scm_request_prepare(struct scm_request *scmrq)
+static inline struct aidaw *scm_aidaw_alloc(void)
+{
+	struct page *page = mempool_alloc(aidaw_pool, GFP_ATOMIC);
+
+	return page ? page_address(page) : NULL;
+}
+
+static inline unsigned long scm_aidaw_bytes(struct aidaw *aidaw)
+{
+	unsigned long _aidaw = (unsigned long) aidaw;
+	unsigned long bytes = ALIGN(_aidaw, PAGE_SIZE) - _aidaw;
+
+	return (bytes / sizeof(*aidaw)) * PAGE_SIZE;
+}
+
+struct aidaw *scm_aidaw_fetch(struct scm_request *scmrq, unsigned int bytes)
+{
+	struct aidaw *aidaw;
+
+	if (scm_aidaw_bytes(scmrq->next_aidaw) >= bytes)
+		return scmrq->next_aidaw;
+
+	aidaw = scm_aidaw_alloc();
+	if (aidaw)
+		memset(aidaw, 0, PAGE_SIZE);
+	return aidaw;
+}
+
+static int scm_request_prepare(struct scm_request *scmrq)
 {
 	struct scm_blk_dev *bdev = scmrq->bdev;
 	struct scm_device *scmdev = bdev->gendisk->private_data;
-	struct aidaw *aidaw = scmrq->aidaw;
-	struct msb *msb = &scmrq->aob->msb[0];
+	int pos = scmrq->aob->request.msb_count;
+	struct msb *msb = &scmrq->aob->msb[pos];
+	struct request *req = scmrq->request[pos];
 	struct req_iterator iter;
+	struct aidaw *aidaw;
 	struct bio_vec bv;
 
+	aidaw = scm_aidaw_fetch(scmrq, blk_rq_bytes(req));
+	if (!aidaw)
+		return -ENOMEM;
+
 	msb->bs = MSB_BS_4K;
-	scmrq->aob->request.msb_count = 1;
-	msb->scm_addr = scmdev->address +
-		((u64) blk_rq_pos(scmrq->request) << 9);
-	msb->oc = (rq_data_dir(scmrq->request) == READ) ?
-		MSB_OC_READ : MSB_OC_WRITE;
+	scmrq->aob->request.msb_count++;
+	msb->scm_addr = scmdev->address + ((u64) blk_rq_pos(req) << 9);
+	msb->oc = (rq_data_dir(req) == READ) ? MSB_OC_READ : MSB_OC_WRITE;
 	msb->flags |= MSB_FLAG_IDA;
 	msb->data_addr = (u64) aidaw;
 
-	rq_for_each_segment(bv, scmrq->request, iter) {
+	rq_for_each_segment(bv, req, iter) {
 		WARN_ON(bv.bv_offset);
 		msb->blk_count += bv.bv_len >> 12;
 		aidaw->data_addr = (u64) page_address(bv.bv_page);
 		aidaw++;
 	}
+
+	scmrq->next_aidaw = aidaw;
+	return 0;
+}
+
+static inline void scm_request_set(struct scm_request *scmrq,
+				   struct request *req)
+{
+	scmrq->request[scmrq->aob->request.msb_count] = req;
 }
 
 static inline void scm_request_init(struct scm_blk_dev *bdev,
-				    struct scm_request *scmrq,
-				    struct request *req)
+				    struct scm_request *scmrq)
 {
 	struct aob_rq_header *aobrq = to_aobrq(scmrq);
 	struct aob *aob = scmrq->aob;
 
+	memset(scmrq->request, 0,
+	       nr_requests_per_io * sizeof(scmrq->request[0]));
 	memset(aob, 0, sizeof(*aob));
-	memset(scmrq->aidaw, 0, PAGE_SIZE);
 	aobrq->scmdev = bdev->scmdev;
 	aob->request.cmd_code = ARQB_CMD_MOVE;
 	aob->request.data = (u64) aobrq;
-	scmrq->request = req;
 	scmrq->bdev = bdev;
 	scmrq->retries = 4;
 	scmrq->error = 0;
+	/* We don't use all msbs - place aidaws at the end of the aob page. */
+	scmrq->next_aidaw = (void *) &aob->msb[nr_requests_per_io];
 	scm_request_cluster_init(scmrq);
 }
 
@@ -180,9 +249,12 @@ static void scm_ensure_queue_restart(struct scm_blk_dev *bdev)
 void scm_request_requeue(struct scm_request *scmrq)
 {
 	struct scm_blk_dev *bdev = scmrq->bdev;
+	int i;
 
 	scm_release_cluster(scmrq);
-	blk_requeue_request(bdev->rq, scmrq->request);
+	for (i = 0; i < nr_requests_per_io && scmrq->request[i]; i++)
+		blk_requeue_request(bdev->rq, scmrq->request[i]);
+
 	atomic_dec(&bdev->queued_reqs);
 	scm_request_done(scmrq);
 	scm_ensure_queue_restart(bdev);
@@ -191,20 +263,41 @@ void scm_request_requeue(struct scm_request *scmrq)
 void scm_request_finish(struct scm_request *scmrq)
 {
 	struct scm_blk_dev *bdev = scmrq->bdev;
+	int i;
 
 	scm_release_cluster(scmrq);
-	blk_end_request_all(scmrq->request, scmrq->error);
+	for (i = 0; i < nr_requests_per_io && scmrq->request[i]; i++)
+		blk_end_request_all(scmrq->request[i], scmrq->error);
+
 	atomic_dec(&bdev->queued_reqs);
 	scm_request_done(scmrq);
 }
 
+static int scm_request_start(struct scm_request *scmrq)
+{
+	struct scm_blk_dev *bdev = scmrq->bdev;
+	int ret;
+
+	atomic_inc(&bdev->queued_reqs);
+	if (!scmrq->aob->request.msb_count) {
+		scm_request_requeue(scmrq);
+		return -EINVAL;
+	}
+
+	ret = eadm_start_aob(scmrq->aob);
+	if (ret) {
+		SCM_LOG(5, "no subchannel");
+		scm_request_requeue(scmrq);
+	}
+	return ret;
+}
+
 static void scm_blk_request(struct request_queue *rq)
 {
 	struct scm_device *scmdev = rq->queuedata;
 	struct scm_blk_dev *bdev = dev_get_drvdata(&scmdev->dev);
-	struct scm_request *scmrq;
+	struct scm_request *scmrq = NULL;
 	struct request *req;
-	int ret;
 
 	while ((req = blk_peek_request(rq))) {
 		if (req->cmd_type != REQ_TYPE_FS) {
@@ -214,39 +307,64 @@ static void scm_blk_request(struct request_queue *rq)
 			continue;
 		}
 
-		if (!scm_permit_request(bdev, req)) {
-			scm_ensure_queue_restart(bdev);
-			return;
-		}
-		scmrq = scm_request_fetch();
+		if (!scm_permit_request(bdev, req))
+			goto out;
+
 		if (!scmrq) {
-			SCM_LOG(5, "no request");
-			scm_ensure_queue_restart(bdev);
-			return;
+			scmrq = scm_request_fetch();
+			if (!scmrq) {
+				SCM_LOG(5, "no request");
+				goto out;
+			}
+			scm_request_init(bdev, scmrq);
 		}
-		scm_request_init(bdev, scmrq, req);
+		scm_request_set(scmrq, req);
+
 		if (!scm_reserve_cluster(scmrq)) {
 			SCM_LOG(5, "cluster busy");
+			scm_request_set(scmrq, NULL);
+			if (scmrq->aob->request.msb_count)
+				goto out;
+
 			scm_request_done(scmrq);
 			return;
 		}
+
 		if (scm_need_cluster_request(scmrq)) {
-			atomic_inc(&bdev->queued_reqs);
-			blk_start_request(req);
-			scm_initiate_cluster_request(scmrq);
-			return;
+			if (scmrq->aob->request.msb_count) {
+				/* Start cluster requests separately. */
+				scm_request_set(scmrq, NULL);
+				if (scm_request_start(scmrq))
+					return;
+			} else {
+				atomic_inc(&bdev->queued_reqs);
+				blk_start_request(req);
+				scm_initiate_cluster_request(scmrq);
+			}
+			scmrq = NULL;
+			continue;
+		}
+
+		if (scm_request_prepare(scmrq)) {
+			SCM_LOG(5, "aidaw alloc failed");
+			scm_request_set(scmrq, NULL);
+			goto out;
 		}
-		scm_request_prepare(scmrq);
-		atomic_inc(&bdev->queued_reqs);
 		blk_start_request(req);
 
-		ret = eadm_start_aob(scmrq->aob);
-		if (ret) {
-			SCM_LOG(5, "no subchannel");
-			scm_request_requeue(scmrq);
+		if (scmrq->aob->request.msb_count < nr_requests_per_io)
+			continue;
+
+		if (scm_request_start(scmrq))
 			return;
-		}
+
+		scmrq = NULL;
 	}
+out:
+	if (scmrq)
+		scm_request_start(scmrq);
+	else
+		scm_ensure_queue_restart(bdev);
 }
 
 static void __scmrq_log_error(struct scm_request *scmrq)
@@ -443,11 +561,19 @@ void scm_blk_set_available(struct scm_blk_dev *bdev)
 	spin_unlock_irqrestore(&bdev->lock, flags);
 }
 
+static bool __init scm_blk_params_valid(void)
+{
+	if (!nr_requests_per_io || nr_requests_per_io > 64)
+		return false;
+
+	return scm_cluster_size_valid();
+}
+
 static int __init scm_blk_init(void)
 {
 	int ret = -EINVAL;
 
-	if (!scm_cluster_size_valid())
+	if (!scm_blk_params_valid())
 		goto out;
 
 	ret = register_blkdev(0, "scm");
diff --git a/drivers/s390/block/scm_blk.h b/drivers/s390/block/scm_blk.h
index e59331e6c2e5..09218cdc5129 100644
--- a/drivers/s390/block/scm_blk.h
+++ b/drivers/s390/block/scm_blk.h
@@ -30,8 +30,8 @@ struct scm_blk_dev {
 
 struct scm_request {
 	struct scm_blk_dev *bdev;
-	struct request *request;
-	struct aidaw *aidaw;
+	struct aidaw *next_aidaw;
+	struct request **request;
 	struct aob *aob;
 	struct list_head list;
 	u8 retries;
@@ -55,6 +55,8 @@ void scm_blk_irq(struct scm_device *, void *, int);
 void scm_request_finish(struct scm_request *);
 void scm_request_requeue(struct scm_request *);
 
+struct aidaw *scm_aidaw_fetch(struct scm_request *scmrq, unsigned int bytes);
+
 int scm_drv_init(void);
 void scm_drv_cleanup(void);
 
diff --git a/drivers/s390/block/scm_blk_cluster.c b/drivers/s390/block/scm_blk_cluster.c
index 9aae909d47a5..09db45296eed 100644
--- a/drivers/s390/block/scm_blk_cluster.c
+++ b/drivers/s390/block/scm_blk_cluster.c
@@ -57,39 +57,52 @@ void scm_request_cluster_init(struct scm_request *scmrq)
 	scmrq->cluster.state = CLUSTER_NONE;
 }
 
-static bool clusters_intersect(struct scm_request *A, struct scm_request *B)
+static bool clusters_intersect(struct request *A, struct request *B)
 {
 	unsigned long firstA, lastA, firstB, lastB;
 
-	firstA = ((u64) blk_rq_pos(A->request) << 9) / CLUSTER_SIZE;
-	lastA = (((u64) blk_rq_pos(A->request) << 9) +
-		    blk_rq_bytes(A->request) - 1) / CLUSTER_SIZE;
+	firstA = ((u64) blk_rq_pos(A) << 9) / CLUSTER_SIZE;
+	lastA = (((u64) blk_rq_pos(A) << 9) +
+		    blk_rq_bytes(A) - 1) / CLUSTER_SIZE;
 
-	firstB = ((u64) blk_rq_pos(B->request) << 9) / CLUSTER_SIZE;
-	lastB = (((u64) blk_rq_pos(B->request) << 9) +
-		    blk_rq_bytes(B->request) - 1) / CLUSTER_SIZE;
+	firstB = ((u64) blk_rq_pos(B) << 9) / CLUSTER_SIZE;
+	lastB = (((u64) blk_rq_pos(B) << 9) +
+		    blk_rq_bytes(B) - 1) / CLUSTER_SIZE;
 
 	return (firstB <= lastA && firstA <= lastB);
 }
 
 bool scm_reserve_cluster(struct scm_request *scmrq)
 {
+	struct request *req = scmrq->request[scmrq->aob->request.msb_count];
 	struct scm_blk_dev *bdev = scmrq->bdev;
 	struct scm_request *iter;
+	int pos, add = 1;
 
 	if (write_cluster_size == 0)
 		return true;
 
 	spin_lock(&bdev->lock);
 	list_for_each_entry(iter, &bdev->cluster_list, cluster.list) {
-		if (clusters_intersect(scmrq, iter) &&
-		    (rq_data_dir(scmrq->request) == WRITE ||
-		     rq_data_dir(iter->request) == WRITE)) {
-			spin_unlock(&bdev->lock);
-			return false;
+		if (iter == scmrq) {
+			/*
+			 * We don't have to use clusters_intersect here, since
+			 * cluster requests are always started separately.
+			 */
+			add = 0;
+			continue;
+		}
+		for (pos = 0; pos <= iter->aob->request.msb_count; pos++) {
+			if (clusters_intersect(req, iter->request[pos]) &&
+			    (rq_data_dir(req) == WRITE ||
+			     rq_data_dir(iter->request[pos]) == WRITE)) {
+				spin_unlock(&bdev->lock);
+				return false;
+			}
 		}
 	}
-	list_add(&scmrq->cluster.list, &bdev->cluster_list);
+	if (add)
+		list_add(&scmrq->cluster.list, &bdev->cluster_list);
 	spin_unlock(&bdev->lock);
 
 	return true;
@@ -114,14 +127,14 @@ void scm_blk_dev_cluster_setup(struct scm_blk_dev *bdev)
 	blk_queue_io_opt(bdev->rq, CLUSTER_SIZE);
 }
 
-static void scm_prepare_cluster_request(struct scm_request *scmrq)
+static int scm_prepare_cluster_request(struct scm_request *scmrq)
 {
 	struct scm_blk_dev *bdev = scmrq->bdev;
 	struct scm_device *scmdev = bdev->gendisk->private_data;
-	struct request *req = scmrq->request;
-	struct aidaw *aidaw = scmrq->aidaw;
+	struct request *req = scmrq->request[0];
 	struct msb *msb = &scmrq->aob->msb[0];
 	struct req_iterator iter;
+	struct aidaw *aidaw;
 	struct bio_vec bv;
 	int i = 0;
 	u64 addr;
@@ -131,11 +144,9 @@ static void scm_prepare_cluster_request(struct scm_request *scmrq)
 		scmrq->cluster.state = CLUSTER_READ;
 		/* fall through */
 	case CLUSTER_READ:
-		scmrq->aob->request.msb_count = 1;
 		msb->bs = MSB_BS_4K;
 		msb->oc = MSB_OC_READ;
 		msb->flags = MSB_FLAG_IDA;
-		msb->data_addr = (u64) aidaw;
 		msb->blk_count = write_cluster_size;
 
 		addr = scmdev->address + ((u64) blk_rq_pos(req) << 9);
@@ -146,6 +157,12 @@ static void scm_prepare_cluster_request(struct scm_request *scmrq)
 			       CLUSTER_SIZE))
 			msb->blk_count = 2 * write_cluster_size;
 
+		aidaw = scm_aidaw_fetch(scmrq, msb->blk_count * PAGE_SIZE);
+		if (!aidaw)
+			return -ENOMEM;
+
+		scmrq->aob->request.msb_count = 1;
+		msb->data_addr = (u64) aidaw;
 		for (i = 0; i < msb->blk_count; i++) {
 			aidaw->data_addr = (u64) scmrq->cluster.buf[i];
 			aidaw++;
@@ -153,6 +170,7 @@ static void scm_prepare_cluster_request(struct scm_request *scmrq)
 
 		break;
 	case CLUSTER_WRITE:
+		aidaw = (void *) msb->data_addr;
 		msb->oc = MSB_OC_WRITE;
 
 		for (addr = msb->scm_addr;
@@ -173,22 +191,29 @@ static void scm_prepare_cluster_request(struct scm_request *scmrq)
 		}
 		break;
 	}
+	return 0;
 }
 
 bool scm_need_cluster_request(struct scm_request *scmrq)
 {
-	if (rq_data_dir(scmrq->request) == READ)
+	int pos = scmrq->aob->request.msb_count;
+
+	if (rq_data_dir(scmrq->request[pos]) == READ)
 		return false;
 
-	return blk_rq_bytes(scmrq->request) < CLUSTER_SIZE;
+	return blk_rq_bytes(scmrq->request[pos]) < CLUSTER_SIZE;
 }
 
 /* Called with queue lock held. */
 void scm_initiate_cluster_request(struct scm_request *scmrq)
 {
-	scm_prepare_cluster_request(scmrq);
+	if (scm_prepare_cluster_request(scmrq))
+		goto requeue;
 	if (eadm_start_aob(scmrq->aob))
-		scm_request_requeue(scmrq);
+		goto requeue;
+	return;
+requeue:
+	scm_request_requeue(scmrq);
 }
 
 bool scm_test_cluster_request(struct scm_request *scmrq)
diff --git a/drivers/s390/block/xpram.c b/drivers/s390/block/xpram.c
index 9e0de9c9a6fc..7d4e9397ac31 100644
--- a/drivers/s390/block/xpram.c
+++ b/drivers/s390/block/xpram.c
@@ -417,7 +417,6 @@ static const struct dev_pm_ops xpram_pm_ops = {
 static struct platform_driver xpram_pdrv = {
 	.driver = {
 		.name	= XPRAM_NAME,
-		.owner	= THIS_MODULE,
 		.pm	= &xpram_pm_ops,
 	},
 };
diff --git a/drivers/s390/char/Kconfig b/drivers/s390/char/Kconfig
index db2cb1f8a1b5..a5c6f7e157aa 100644
--- a/drivers/s390/char/Kconfig
+++ b/drivers/s390/char/Kconfig
@@ -102,6 +102,16 @@ config SCLP_ASYNC
 	  want for inform other people about your kernel panics,
 	  need this feature and intend to run your kernel in LPAR.
 
+config SCLP_ASYNC_ID
+       string "Component ID for Call Home"
+       depends on SCLP_ASYNC
+       default "000000000"
+       help
+	 The Component ID for Call Home is used to identify the correct
+	 problem reporting queue the call home records should be sent to.
+
+	 If your are unsure, please use the default value "000000000".
+
 config HMC_DRV
 	def_tristate m
 	prompt "Support for file transfers from HMC drive CD/DVD-ROM"
diff --git a/drivers/s390/char/monwriter.c b/drivers/s390/char/monwriter.c
index 668b32b0dc1d..9b5d1138b2e2 100644
--- a/drivers/s390/char/monwriter.c
+++ b/drivers/s390/char/monwriter.c
@@ -334,7 +334,6 @@ static const struct dev_pm_ops monwriter_pm_ops = {
 static struct platform_driver monwriter_pdrv = {
 	.driver = {
 		.name	= "monwriter",
-		.owner	= THIS_MODULE,
 		.pm	= &monwriter_pm_ops,
 	},
 };
diff --git a/drivers/s390/char/sclp.c b/drivers/s390/char/sclp.c
index c316051d9bda..41ba56d2e752 100644
--- a/drivers/s390/char/sclp.c
+++ b/drivers/s390/char/sclp.c
@@ -1167,7 +1167,6 @@ static const struct attribute_group *sclp_drv_attr_groups[] = {
 static struct platform_driver sclp_pdrv = {
 	.driver = {
 		.name	= "sclp",
-		.owner	= THIS_MODULE,
 		.pm	= &sclp_pm_ops,
 		.groups = sclp_drv_attr_groups,
 	},
diff --git a/drivers/s390/char/sclp_async.c b/drivers/s390/char/sclp_async.c
index 5f9f929e891c..19c25427f27f 100644
--- a/drivers/s390/char/sclp_async.c
+++ b/drivers/s390/char/sclp_async.c
@@ -137,7 +137,8 @@ static int sclp_async_send_wait(char *message)
 	 * Retain Queue
 	 * e.g. 5639CC140 500 Red Hat RHEL5 Linux for zSeries (RHEL AS)
 	 */
-	strncpy(sccb->evbuf.comp_id, "000000000", sizeof(sccb->evbuf.comp_id));
+	strncpy(sccb->evbuf.comp_id, CONFIG_SCLP_ASYNC_ID,
+		sizeof(sccb->evbuf.comp_id));
 	sccb->evbuf.header.length = sizeof(sccb->evbuf);
 	sccb->header.length = sizeof(sccb->evbuf) + sizeof(sccb->header);
 	sccb->header.function_code = SCLP_NORMAL_WRITE;
diff --git a/drivers/s390/char/tape_3590.c b/drivers/s390/char/tape_3590.c
index 327cb19ad0b0..d3d1936057b4 100644
--- a/drivers/s390/char/tape_3590.c
+++ b/drivers/s390/char/tape_3590.c
@@ -1090,7 +1090,7 @@ tape_3590_print_io_sim_msg_f1(struct tape_device *device, struct irb *irb)
 				"channel path 0x%x on CU",
 				sense->fmt.f71.md[1]);
 		else
-			snprintf(service, BUFSIZE, "Repair will disable cannel"
+			snprintf(service, BUFSIZE, "Repair will disable channel"
 				" paths (0x%x-0x%x) on CU",
 				sense->fmt.f71.md[1], sense->fmt.f71.md[2]);
 		break;
@@ -1481,7 +1481,7 @@ tape_3590_irq(struct tape_device *device, struct tape_request *request,
 	}
 
 	if (irb->scsw.cmd.dstat & DEV_STAT_CHN_END) {
-		DBF_EVENT(2, "cannel end\n");
+		DBF_EVENT(2, "channel end\n");
 		return TAPE_IO_PENDING;
 	}
 
diff --git a/drivers/s390/cio/eadm_sch.c b/drivers/s390/cio/eadm_sch.c
index 37f0834300ea..bee8c11cd086 100644
--- a/drivers/s390/cio/eadm_sch.c
+++ b/drivers/s390/cio/eadm_sch.c
@@ -31,7 +31,7 @@
 MODULE_DESCRIPTION("driver for s390 eadm subchannels");
 MODULE_LICENSE("GPL");
 
-#define EADM_TIMEOUT (5 * HZ)
+#define EADM_TIMEOUT (7 * HZ)
 static DEFINE_SPINLOCK(list_lock);
 static LIST_HEAD(eadm_list);
 
diff --git a/drivers/s390/crypto/ap_bus.c b/drivers/s390/crypto/ap_bus.c
index 99485415dcc2..91e97ec01418 100644
--- a/drivers/s390/crypto/ap_bus.c
+++ b/drivers/s390/crypto/ap_bus.c
@@ -44,6 +44,7 @@
 #include <linux/hrtimer.h>
 #include <linux/ktime.h>
 #include <asm/facility.h>
+#include <linux/crypto.h>
 
 #include "ap_bus.h"
 
@@ -71,7 +72,7 @@ MODULE_AUTHOR("IBM Corporation");
 MODULE_DESCRIPTION("Adjunct Processor Bus driver, " \
 		   "Copyright IBM Corp. 2006, 2012");
 MODULE_LICENSE("GPL");
-MODULE_ALIAS("z90crypt");
+MODULE_ALIAS_CRYPTO("z90crypt");
 
 /*
  * Module parameter
diff --git a/drivers/sbus/char/bbc_i2c.c b/drivers/sbus/char/bbc_i2c.c
index 812b5f0361b6..129967ad345d 100644
--- a/drivers/sbus/char/bbc_i2c.c
+++ b/drivers/sbus/char/bbc_i2c.c
@@ -413,7 +413,6 @@ MODULE_DEVICE_TABLE(of, bbc_i2c_match);
 static struct platform_driver bbc_i2c_driver = {
 	.driver = {
 		.name = "bbc_i2c",
-		.owner = THIS_MODULE,
 		.of_match_table = bbc_i2c_match,
 	},
 	.probe		= bbc_i2c_probe,
diff --git a/drivers/sbus/char/display7seg.c b/drivers/sbus/char/display7seg.c
index 2b0ce7c350ee..33fbe8249fd5 100644
--- a/drivers/sbus/char/display7seg.c
+++ b/drivers/sbus/char/display7seg.c
@@ -263,7 +263,6 @@ MODULE_DEVICE_TABLE(of, d7s_match);
 static struct platform_driver d7s_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = d7s_match,
 	},
 	.probe		= d7s_probe,
diff --git a/drivers/sbus/char/envctrl.c b/drivers/sbus/char/envctrl.c
index af15a2fdab5e..e244cf3d9ec8 100644
--- a/drivers/sbus/char/envctrl.c
+++ b/drivers/sbus/char/envctrl.c
@@ -1130,7 +1130,6 @@ MODULE_DEVICE_TABLE(of, envctrl_match);
 static struct platform_driver envctrl_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = envctrl_match,
 	},
 	.probe		= envctrl_probe,
diff --git a/drivers/sbus/char/flash.c b/drivers/sbus/char/flash.c
index 25c738e9ef19..206ef4232adf 100644
--- a/drivers/sbus/char/flash.c
+++ b/drivers/sbus/char/flash.c
@@ -207,7 +207,6 @@ MODULE_DEVICE_TABLE(of, flash_match);
 static struct platform_driver flash_driver = {
 	.driver = {
 		.name = "flash",
-		.owner = THIS_MODULE,
 		.of_match_table = flash_match,
 	},
 	.probe		= flash_probe,
diff --git a/drivers/sbus/char/uctrl.c b/drivers/sbus/char/uctrl.c
index b7acafc85099..57696fc0b482 100644
--- a/drivers/sbus/char/uctrl.c
+++ b/drivers/sbus/char/uctrl.c
@@ -425,7 +425,6 @@ MODULE_DEVICE_TABLE(of, uctrl_match);
 static struct platform_driver uctrl_driver = {
 	.driver = {
 		.name = "uctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = uctrl_match,
 	},
 	.probe		= uctrl_probe,
diff --git a/drivers/scsi/53c700.c b/drivers/scsi/53c700.c
index aa915da2a5e5..82abfce1cb42 100644
--- a/drivers/scsi/53c700.c
+++ b/drivers/scsi/53c700.c
@@ -176,7 +176,6 @@ STATIC int NCR_700_slave_alloc(struct scsi_device *SDpnt);
 STATIC int NCR_700_slave_configure(struct scsi_device *SDpnt);
 STATIC void NCR_700_slave_destroy(struct scsi_device *SDpnt);
 static int NCR_700_change_queue_depth(struct scsi_device *SDpnt, int depth);
-static int NCR_700_change_queue_type(struct scsi_device *SDpnt, int depth);
 
 STATIC struct device_attribute *NCR_700_dev_attrs[];
 
@@ -326,7 +325,6 @@ NCR_700_detect(struct scsi_host_template *tpnt,
 	tpnt->slave_destroy = NCR_700_slave_destroy;
 	tpnt->slave_alloc = NCR_700_slave_alloc;
 	tpnt->change_queue_depth = NCR_700_change_queue_depth;
-	tpnt->change_queue_type = NCR_700_change_queue_type;
 	tpnt->use_blk_tags = 1;
 
 	if(tpnt->name == NULL)
@@ -904,8 +902,8 @@ process_message(struct Scsi_Host *host,	struct NCR_700_Host_Parameters *hostdata
 			hostdata->tag_negotiated &= ~(1<<scmd_id(SCp));
 
 			SCp->device->tagged_supported = 0;
+			SCp->device->simple_tags = 0;
 			scsi_change_queue_depth(SCp->device, host->cmd_per_lun);
-			scsi_set_tag_type(SCp->device, 0);
 		} else {
 			shost_printk(KERN_WARNING, host,
 				"(%d:%d) Unexpected REJECT Message %s\n",
@@ -1818,8 +1816,8 @@ NCR_700_queuecommand_lck(struct scsi_cmnd *SCp, void (*done)(struct scsi_cmnd *)
 		hostdata->tag_negotiated &= ~(1<<scmd_id(SCp));
 	}
 
-	if((hostdata->tag_negotiated &(1<<scmd_id(SCp)))
-	   && scsi_get_tag_type(SCp->device)) {
+	if ((hostdata->tag_negotiated & (1<<scmd_id(SCp))) &&
+	    SCp->device->simple_tags) {
 		slot->tag = SCp->request->tag;
 		CDEBUG(KERN_DEBUG, SCp, "sending out tag %d, slot %p\n",
 		       slot->tag, slot);
@@ -2082,39 +2080,6 @@ NCR_700_change_queue_depth(struct scsi_device *SDp, int depth)
 	return scsi_change_queue_depth(SDp, depth);
 }
 
-static int NCR_700_change_queue_type(struct scsi_device *SDp, int tag_type)
-{
-	int change_tag = ((tag_type ==0 &&  scsi_get_tag_type(SDp) != 0)
-			  || (tag_type != 0 && scsi_get_tag_type(SDp) == 0));
-	struct NCR_700_Host_Parameters *hostdata = 
-		(struct NCR_700_Host_Parameters *)SDp->host->hostdata[0];
-
-	/* We have a global (per target) flag to track whether TCQ is
-	 * enabled, so we'll be turning it off for the entire target here.
-	 * our tag algorithm will fail if we mix tagged and untagged commands,
-	 * so quiesce the device before doing this */
-	if (change_tag)
-		scsi_target_quiesce(SDp->sdev_target);
-
-	scsi_set_tag_type(SDp, tag_type);
-	if (!tag_type) {
-		/* shift back to the default unqueued number of commands
-		 * (the user can still raise this) */
-		scsi_change_queue_depth(SDp, SDp->host->cmd_per_lun);
-		hostdata->tag_negotiated &= ~(1 << sdev_id(SDp));
-	} else {
-		/* Here, we cleared the negotiation flag above, so this
-		 * will force the driver to renegotiate */
-		scsi_change_queue_depth(SDp, SDp->queue_depth);
-		if (change_tag)
-			NCR_700_set_tag_neg_state(SDp, NCR_700_START_TAG_NEGOTIATION);
-	}
-	if (change_tag)
-		scsi_target_resume(SDp->sdev_target);
-
-	return tag_type;
-}
-
 static ssize_t
 NCR_700_show_active_tags(struct device *dev, struct device_attribute *attr, char *buf)
 {
diff --git a/drivers/scsi/Kconfig b/drivers/scsi/Kconfig
index 86cf3d671eb9..9c92f415229f 100644
--- a/drivers/scsi/Kconfig
+++ b/drivers/scsi/Kconfig
@@ -1462,18 +1462,17 @@ config SCSI_WD719X
 	  SCSI controllers (based on WD33C296A chip).
 
 config SCSI_DEBUG
-	tristate "SCSI debugging host simulator"
+	tristate "SCSI debugging host and device simulator"
 	depends on SCSI
 	select CRC_T10DIF
 	help
-	  This is a host adapter simulator that can simulate multiple hosts
-	  each with multiple dummy SCSI devices (disks). It defaults to one
-	  host adapter with one dummy SCSI disk. Each dummy disk uses kernel
-	  RAM as storage (i.e. it is a ramdisk). To save space when multiple
-	  dummy disks are simulated, they share the same kernel RAM for 
-	  their storage. See <http://sg.danny.cz/sg/sdebug26.html> for more
-	  information. This driver is primarily of use to those testing the
-	  SCSI and block subsystems. If unsure, say N.
+	  This pseudo driver simulates one or more hosts (SCSI initiators),
+	  each with one or more targets, each with one or more logical units.
+	  Defaults to one of each, creating a small RAM disk device. Many
+	  parameters found in the /sys/bus/pseudo/drivers/scsi_debug
+	  directory can be tweaked at run time.
+	  See <http://sg.danny.cz/sg/sdebug26.html> for more information.
+	  Mainly used for testing and best as a module. If unsure, say N.
 
 config SCSI_MESH
 	tristate "MESH (Power Mac internal SCSI) support"
diff --git a/drivers/scsi/a3000.c b/drivers/scsi/a3000.c
index dd5b64726ddc..e6375b4de79e 100644
--- a/drivers/scsi/a3000.c
+++ b/drivers/scsi/a3000.c
@@ -276,7 +276,6 @@ static struct platform_driver amiga_a3000_scsi_driver = {
 	.remove = __exit_p(amiga_a3000_scsi_remove),
 	.driver   = {
 		.name	= "amiga-a3000-scsi",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/scsi/a4000t.c b/drivers/scsi/a4000t.c
index f5a2ab41543b..66c573093901 100644
--- a/drivers/scsi/a4000t.c
+++ b/drivers/scsi/a4000t.c
@@ -112,7 +112,6 @@ static struct platform_driver amiga_a4000t_scsi_driver = {
 	.remove = __exit_p(amiga_a4000t_scsi_remove),
 	.driver   = {
 		.name	= "amiga-a4000t-scsi",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/scsi/advansys.c b/drivers/scsi/advansys.c
index 6719a3390ebd..2c5ce48c8f95 100644
--- a/drivers/scsi/advansys.c
+++ b/drivers/scsi/advansys.c
@@ -7921,9 +7921,9 @@ static int asc_build_req(struct asc_board *boardp, struct scsi_cmnd *scp,
 	 */
 	if ((asc_dvc->cur_dvc_qng[scp->device->id] > 0) &&
 	    (boardp->reqcnt[scp->device->id] % 255) == 0) {
-		asc_scsi_q->q2.tag_code = MSG_ORDERED_TAG;
+		asc_scsi_q->q2.tag_code = ORDERED_QUEUE_TAG;
 	} else {
-		asc_scsi_q->q2.tag_code = MSG_SIMPLE_TAG;
+		asc_scsi_q->q2.tag_code = SIMPLE_QUEUE_TAG;
 	}
 
 	/* Build ASC_SCSI_Q */
@@ -8351,7 +8351,7 @@ static int AscPutReadyQueue(ASC_DVC_VAR *asc_dvc, ASC_SCSI_Q *scsiq, uchar q_no)
 	}
 	q_addr = ASC_QNO_TO_QADDR(q_no);
 	if ((scsiq->q1.target_id & asc_dvc->use_tagged_qng) == 0) {
-		scsiq->q2.tag_code &= ~MSG_SIMPLE_TAG;
+		scsiq->q2.tag_code &= ~SIMPLE_QUEUE_TAG;
 	}
 	scsiq->q1.status = QS_FREE;
 	AscMemWordCopyPtrToLram(iop_base,
@@ -8669,7 +8669,7 @@ static int AscExeScsiQueue(ASC_DVC_VAR *asc_dvc, ASC_SCSI_Q *scsiq)
 		}
 	}
 	if (disable_syn_offset_one_fix) {
-		scsiq->q2.tag_code &= ~MSG_SIMPLE_TAG;
+		scsiq->q2.tag_code &= ~SIMPLE_QUEUE_TAG;
 		scsiq->q2.tag_code |= (ASC_TAG_FLAG_DISABLE_ASYN_USE_SYN_FIX |
 				       ASC_TAG_FLAG_DISABLE_DISCONNECT);
 	} else {
diff --git a/drivers/scsi/aic94xx/aic94xx_init.c b/drivers/scsi/aic94xx/aic94xx_init.c
index 14fc018436c2..02a2512b76a8 100644
--- a/drivers/scsi/aic94xx/aic94xx_init.c
+++ b/drivers/scsi/aic94xx/aic94xx_init.c
@@ -63,7 +63,6 @@ static struct scsi_host_template aic94xx_sht = {
 	.scan_finished		= asd_scan_finished,
 	.scan_start		= asd_scan_start,
 	.change_queue_depth	= sas_change_queue_depth,
-	.change_queue_type	= sas_change_queue_type,
 	.bios_param		= sas_bios_param,
 	.can_queue		= 1,
 	.cmd_per_lun		= 1,
diff --git a/drivers/scsi/aic94xx/aic94xx_task.c b/drivers/scsi/aic94xx/aic94xx_task.c
index 5ff1ce7ba1f4..cdd4ab683be9 100644
--- a/drivers/scsi/aic94xx/aic94xx_task.c
+++ b/drivers/scsi/aic94xx/aic94xx_task.c
@@ -373,10 +373,10 @@ static int asd_build_ata_ascb(struct asd_ascb *ascb, struct sas_task *task,
 
 	if (unlikely(task->ata_task.device_control_reg_update))
 		scb->header.opcode = CONTROL_ATA_DEV;
-	else if (dev->sata_dev.command_set == ATA_COMMAND_SET)
-		scb->header.opcode = INITIATE_ATA_TASK;
-	else
+	else if (dev->sata_dev.class == ATA_DEV_ATAPI)
 		scb->header.opcode = INITIATE_ATAPI_TASK;
+	else
+		scb->header.opcode = INITIATE_ATA_TASK;
 
 	scb->ata_task.proto_conn_rate = (1 << 5); /* STP */
 	if (dev->port->oob_mode == SAS_OOB_MODE)
@@ -387,7 +387,7 @@ static int asd_build_ata_ascb(struct asd_ascb *ascb, struct sas_task *task,
 	if (likely(!task->ata_task.device_control_reg_update))
 		scb->ata_task.fis.flags |= 0x80; /* C=1: update ATA cmd reg */
 	scb->ata_task.fis.flags &= 0xF0; /* PM_PORT field shall be 0 */
-	if (dev->sata_dev.command_set == ATAPI_COMMAND_SET)
+	if (dev->sata_dev.class == ATA_DEV_ATAPI)
 		memcpy(scb->ata_task.atapi_packet, task->ata_task.atapi_packet,
 		       16);
 	scb->ata_task.sister_scb = cpu_to_le16(0xFFFF);
@@ -399,7 +399,7 @@ static int asd_build_ata_ascb(struct asd_ascb *ascb, struct sas_task *task,
 		if (task->ata_task.dma_xfer)
 			flags |= DATA_XFER_MODE_DMA;
 		if (task->ata_task.use_ncq &&
-		    dev->sata_dev.command_set != ATAPI_COMMAND_SET)
+		    dev->sata_dev.class != ATA_DEV_ATAPI)
 			flags |= ATA_Q_TYPE_NCQ;
 		flags |= data_dir_flags[task->data_dir];
 		scb->ata_task.ata_flags = flags;
diff --git a/drivers/scsi/bnx2fc/bnx2fc_fcoe.c b/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
index e861f286b42e..98d06d151958 100644
--- a/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
+++ b/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
@@ -2792,7 +2792,6 @@ static struct scsi_host_template bnx2fc_shost_template = {
 	.eh_host_reset_handler	= fc_eh_host_reset,
 	.slave_alloc		= fc_slave_alloc,
 	.change_queue_depth	= scsi_change_queue_depth,
-	.change_queue_type	= scsi_change_queue_type,
 	.this_id		= -1,
 	.cmd_per_lun		= 3,
 	.use_clustering		= ENABLE_CLUSTERING,
diff --git a/drivers/scsi/bnx2fc/bnx2fc_io.c b/drivers/scsi/bnx2fc/bnx2fc_io.c
index 4b56858c1df2..9ecca8504f60 100644
--- a/drivers/scsi/bnx2fc/bnx2fc_io.c
+++ b/drivers/scsi/bnx2fc/bnx2fc_io.c
@@ -1737,11 +1737,7 @@ void bnx2fc_build_fcp_cmnd(struct bnx2fc_cmd *io_req,
 	fcp_cmnd->fc_pri_ta = 0;
 	fcp_cmnd->fc_tm_flags = io_req->mp_req.tm_flags;
 	fcp_cmnd->fc_flags = io_req->io_req_flags;
-
-	if (sc_cmd->flags & SCMD_TAGGED)
-		fcp_cmnd->fc_pri_ta = FCP_PTA_SIMPLE;
-	else
-		fcp_cmnd->fc_pri_ta = 0;
+	fcp_cmnd->fc_pri_ta = FCP_PTA_SIMPLE;
 }
 
 static void bnx2fc_parse_fcp_rsp(struct bnx2fc_cmd *io_req,
diff --git a/drivers/scsi/bvme6000_scsi.c b/drivers/scsi/bvme6000_scsi.c
index 1e3f96adf9da..0f846ae2f918 100644
--- a/drivers/scsi/bvme6000_scsi.c
+++ b/drivers/scsi/bvme6000_scsi.c
@@ -105,7 +105,6 @@ bvme6000_device_remove(struct platform_device *dev)
 static struct platform_driver bvme6000_scsi_driver = {
 	.driver = {
 		.name		= "bvme6000-scsi",
-		.owner		= THIS_MODULE,
 	},
 	.probe		= bvme6000_probe,
 	.remove		= bvme6000_device_remove,
diff --git a/drivers/scsi/csiostor/csio_scsi.c b/drivers/scsi/csiostor/csio_scsi.c
index 51ea5dc5f084..3987284e0d2a 100644
--- a/drivers/scsi/csiostor/csio_scsi.c
+++ b/drivers/scsi/csiostor/csio_scsi.c
@@ -172,10 +172,7 @@ csio_scsi_fcp_cmnd(struct csio_ioreq *req, void *addr)
 		fcp_cmnd->fc_cmdref = 0;
 
 		memcpy(fcp_cmnd->fc_cdb, scmnd->cmnd, 16);
-		if (scmnd->flags & SCMD_TAGGED)
-			fcp_cmnd->fc_pri_ta = FCP_PTA_SIMPLE;
-		else
-			fcp_cmnd->fc_pri_ta = 0;
+		fcp_cmnd->fc_pri_ta = FCP_PTA_SIMPLE;
 		fcp_cmnd->fc_dl = cpu_to_be32(scsi_bufflen(scmnd));
 
 		if (req->nsge)
diff --git a/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c b/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c
index 69fbfc89efb6..a83d2ceded83 100644
--- a/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c
+++ b/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c
@@ -75,6 +75,7 @@ typedef void (*cxgb4i_cplhandler_func)(struct cxgbi_device *, struct sk_buff *);
 static void *t4_uld_add(const struct cxgb4_lld_info *);
 static int t4_uld_rx_handler(void *, const __be64 *, const struct pkt_gl *);
 static int t4_uld_state_change(void *, enum cxgb4_state state);
+static inline int send_tx_flowc_wr(struct cxgbi_sock *);
 
 static const struct cxgb4_uld_info cxgb4i_uld_info = {
 	.name = DRV_MODULE_NAME,
@@ -157,12 +158,6 @@ static struct scsi_transport_template *cxgb4i_stt;
 #define RCV_BUFSIZ_MASK		0x3FFU
 #define MAX_IMM_TX_PKT_LEN	128
 
-static inline void set_queue(struct sk_buff *skb, unsigned int queue,
-				const struct cxgbi_sock *csk)
-{
-	skb->queue_mapping = queue;
-}
-
 static int push_tx_frames(struct cxgbi_sock *, int);
 
 /*
@@ -172,10 +167,14 @@ static int push_tx_frames(struct cxgbi_sock *, int);
  * Returns true if a packet can be sent as an offload WR with immediate
  * data.  We currently use the same limit as for Ethernet packets.
  */
-static inline int is_ofld_imm(const struct sk_buff *skb)
+static inline bool is_ofld_imm(const struct sk_buff *skb)
 {
-	return skb->len <= (MAX_IMM_TX_PKT_LEN -
-			sizeof(struct fw_ofld_tx_data_wr));
+	int len = skb->len;
+
+	if (likely(cxgbi_skcb_test_flag(skb, SKCBF_TX_NEED_HDR)))
+		len += sizeof(struct fw_ofld_tx_data_wr);
+
+	return len <= MAX_IMM_TX_PKT_LEN;
 }
 
 static void send_act_open_req(struct cxgbi_sock *csk, struct sk_buff *skb,
@@ -388,13 +387,19 @@ static void send_abort_req(struct cxgbi_sock *csk)
 
 	if (unlikely(csk->state == CTP_ABORTING) || !skb || !csk->cdev)
 		return;
+
+	if (!cxgbi_sock_flag(csk, CTPF_TX_DATA_SENT)) {
+		send_tx_flowc_wr(csk);
+		cxgbi_sock_set_flag(csk, CTPF_TX_DATA_SENT);
+	}
+
 	cxgbi_sock_set_state(csk, CTP_ABORTING);
 	cxgbi_sock_set_flag(csk, CTPF_ABORT_RPL_PENDING);
 	cxgbi_sock_purge_write_queue(csk);
 
 	csk->cpl_abort_req = NULL;
 	req = (struct cpl_abort_req *)skb->head;
-	set_queue(skb, CPL_PRIORITY_DATA, csk);
+	set_wr_txq(skb, CPL_PRIORITY_DATA, csk->port_id);
 	req->cmd = CPL_ABORT_SEND_RST;
 	t4_set_arp_err_handler(skb, csk, abort_arp_failure);
 	INIT_TP_WR(req, csk->tid);
@@ -420,7 +425,7 @@ static void send_abort_rpl(struct cxgbi_sock *csk, int rst_status)
 		csk, csk->state, csk->flags, csk->tid, rst_status);
 
 	csk->cpl_abort_rpl = NULL;
-	set_queue(skb, CPL_PRIORITY_DATA, csk);
+	set_wr_txq(skb, CPL_PRIORITY_DATA, csk->port_id);
 	INIT_TP_WR(rpl, csk->tid);
 	OPCODE_TID(rpl) = cpu_to_be32(MK_OPCODE_TID(CPL_ABORT_RPL, csk->tid));
 	rpl->cmd = rst_status;
@@ -491,20 +496,40 @@ static inline unsigned int calc_tx_flits_ofld(const struct sk_buff *skb)
 	return flits + sgl_len(cnt);
 }
 
-static inline void send_tx_flowc_wr(struct cxgbi_sock *csk)
+#define FLOWC_WR_NPARAMS_MIN	9
+static inline int tx_flowc_wr_credits(int *nparamsp, int *flowclenp)
+{
+	int nparams, flowclen16, flowclen;
+
+	nparams = FLOWC_WR_NPARAMS_MIN;
+	flowclen = offsetof(struct fw_flowc_wr, mnemval[nparams]);
+	flowclen16 = DIV_ROUND_UP(flowclen, 16);
+	flowclen = flowclen16 * 16;
+	/*
+	 * Return the number of 16-byte credits used by the FlowC request.
+	 * Pass back the nparams and actual FlowC length if requested.
+	 */
+	if (nparamsp)
+		*nparamsp = nparams;
+	if (flowclenp)
+		*flowclenp = flowclen;
+
+	return flowclen16;
+}
+
+static inline int send_tx_flowc_wr(struct cxgbi_sock *csk)
 {
 	struct sk_buff *skb;
 	struct fw_flowc_wr *flowc;
-	int flowclen, i;
+	int nparams, flowclen16, flowclen;
 
-	flowclen = 80;
+	flowclen16 = tx_flowc_wr_credits(&nparams, &flowclen);
 	skb = alloc_wr(flowclen, 0, GFP_ATOMIC);
 	flowc = (struct fw_flowc_wr *)skb->head;
 	flowc->op_to_nparams =
-		htonl(FW_WR_OP_V(FW_FLOWC_WR) | FW_FLOWC_WR_NPARAMS_V(8));
+		htonl(FW_WR_OP_V(FW_FLOWC_WR) | FW_FLOWC_WR_NPARAMS_V(nparams));
 	flowc->flowid_len16 =
-		htonl(FW_WR_LEN16_V(DIV_ROUND_UP(72, 16)) |
-				FW_WR_FLOWID_V(csk->tid));
+		htonl(FW_WR_LEN16_V(flowclen16) | FW_WR_FLOWID_V(csk->tid));
 	flowc->mnemval[0].mnemonic = FW_FLOWC_MNEM_PFNVFN;
 	flowc->mnemval[0].val = htonl(csk->cdev->pfvf);
 	flowc->mnemval[1].mnemonic = FW_FLOWC_MNEM_CH;
@@ -523,12 +548,10 @@ static inline void send_tx_flowc_wr(struct cxgbi_sock *csk)
 	flowc->mnemval[7].val = htonl(csk->advmss);
 	flowc->mnemval[8].mnemonic = 0;
 	flowc->mnemval[8].val = 0;
-	for (i = 0; i < 9; i++) {
-		flowc->mnemval[i].r4[0] = 0;
-		flowc->mnemval[i].r4[1] = 0;
-		flowc->mnemval[i].r4[2] = 0;
-	}
-	set_queue(skb, CPL_PRIORITY_DATA, csk);
+	flowc->mnemval[8].mnemonic = FW_FLOWC_MNEM_TXDATAPLEN_MAX;
+	flowc->mnemval[8].val = 16384;
+
+	set_wr_txq(skb, CPL_PRIORITY_DATA, csk->port_id);
 
 	log_debug(1 << CXGBI_DBG_TOE | 1 << CXGBI_DBG_SOCK,
 		"csk 0x%p, tid 0x%x, %u,%u,%u,%u,%u,%u,%u.\n",
@@ -537,6 +560,8 @@ static inline void send_tx_flowc_wr(struct cxgbi_sock *csk)
 		csk->advmss);
 
 	cxgb4_ofld_send(csk->cdev->ports[csk->port_id], skb);
+
+	return flowclen16;
 }
 
 static inline void make_tx_data_wr(struct cxgbi_sock *csk, struct sk_buff *skb,
@@ -545,10 +570,11 @@ static inline void make_tx_data_wr(struct cxgbi_sock *csk, struct sk_buff *skb,
 	struct fw_ofld_tx_data_wr *req;
 	unsigned int submode = cxgbi_skcb_ulp_mode(skb) & 3;
 	unsigned int wr_ulp_mode = 0, val;
+	bool imm = is_ofld_imm(skb);
 
 	req = (struct fw_ofld_tx_data_wr *)__skb_push(skb, sizeof(*req));
 
-	if (is_ofld_imm(skb)) {
+	if (imm) {
 		req->op_to_immdlen = htonl(FW_WR_OP_V(FW_OFLD_TX_DATA_WR) |
 					FW_WR_COMPL_F |
 					FW_WR_IMMDLEN_V(dlen));
@@ -597,16 +623,32 @@ static int push_tx_frames(struct cxgbi_sock *csk, int req_completion)
 		int dlen = skb->len;
 		int len = skb->len;
 		unsigned int credits_needed;
+		int flowclen16 = 0;
 
 		skb_reset_transport_header(skb);
 		if (is_ofld_imm(skb))
-			credits_needed = DIV_ROUND_UP(dlen +
-					sizeof(struct fw_ofld_tx_data_wr), 16);
+			credits_needed = DIV_ROUND_UP(dlen, 16);
 		else
-			credits_needed = DIV_ROUND_UP(8*calc_tx_flits_ofld(skb)
-					+ sizeof(struct fw_ofld_tx_data_wr),
+			credits_needed = DIV_ROUND_UP(
+						8 * calc_tx_flits_ofld(skb),
+						16);
+
+		if (likely(cxgbi_skcb_test_flag(skb, SKCBF_TX_NEED_HDR)))
+			credits_needed += DIV_ROUND_UP(
+					sizeof(struct fw_ofld_tx_data_wr),
 					16);
 
+		/*
+		 * Assumes the initial credits is large enough to support
+		 * fw_flowc_wr plus largest possible first payload
+		 */
+		if (!cxgbi_sock_flag(csk, CTPF_TX_DATA_SENT)) {
+			flowclen16 = send_tx_flowc_wr(csk);
+			csk->wr_cred -= flowclen16;
+			csk->wr_una_cred += flowclen16;
+			cxgbi_sock_set_flag(csk, CTPF_TX_DATA_SENT);
+		}
+
 		if (csk->wr_cred < credits_needed) {
 			log_debug(1 << CXGBI_DBG_PDU_TX,
 				"csk 0x%p, skb %u/%u, wr %d < %u.\n",
@@ -615,8 +657,8 @@ static int push_tx_frames(struct cxgbi_sock *csk, int req_completion)
 			break;
 		}
 		__skb_unlink(skb, &csk->write_queue);
-		set_queue(skb, CPL_PRIORITY_DATA, csk);
-		skb->csum = credits_needed;
+		set_wr_txq(skb, CPL_PRIORITY_DATA, csk->port_id);
+		skb->csum = credits_needed + flowclen16;
 		csk->wr_cred -= credits_needed;
 		csk->wr_una_cred += credits_needed;
 		cxgbi_sock_enqueue_wr(csk, skb);
@@ -627,12 +669,6 @@ static int push_tx_frames(struct cxgbi_sock *csk, int req_completion)
 			csk->wr_cred, csk->wr_una_cred);
 
 		if (likely(cxgbi_skcb_test_flag(skb, SKCBF_TX_NEED_HDR))) {
-			if (!cxgbi_sock_flag(csk, CTPF_TX_DATA_SENT)) {
-				send_tx_flowc_wr(csk);
-				skb->csum += 5;
-				csk->wr_cred -= 5;
-				csk->wr_una_cred += 5;
-			}
 			len += cxgbi_ulp_extra_len(cxgbi_skcb_ulp_mode(skb));
 			make_tx_data_wr(csk, skb, dlen, len, credits_needed,
 					req_completion);
@@ -807,6 +843,13 @@ static void csk_act_open_retry_timer(unsigned long data)
 
 }
 
+static inline bool is_neg_adv(unsigned int status)
+{
+	return status == CPL_ERR_RTX_NEG_ADVICE ||
+		status == CPL_ERR_KEEPALV_NEG_ADVICE ||
+		status == CPL_ERR_PERSIST_NEG_ADVICE;
+}
+
 static void do_act_open_rpl(struct cxgbi_device *cdev, struct sk_buff *skb)
 {
 	struct cxgbi_sock *csk;
@@ -828,7 +871,7 @@ static void do_act_open_rpl(struct cxgbi_device *cdev, struct sk_buff *skb)
 		       "csk 0x%p,%u,0x%lx. ", (&csk->saddr), (&csk->daddr),
 		       atid, tid, status, csk, csk->state, csk->flags);
 
-	if (status == CPL_ERR_RTX_NEG_ADVICE)
+	if (is_neg_adv(status))
 		goto rel_skb;
 
 	module_put(THIS_MODULE);
@@ -934,8 +977,7 @@ static void do_abort_req_rss(struct cxgbi_device *cdev, struct sk_buff *skb)
 		       (&csk->saddr), (&csk->daddr),
 		       csk, csk->state, csk->flags, csk->tid, req->status);
 
-	if (req->status == CPL_ERR_RTX_NEG_ADVICE ||
-	    req->status == CPL_ERR_PERSIST_NEG_ADVICE)
+	if (is_neg_adv(req->status))
 		goto rel_skb;
 
 	cxgbi_sock_get(csk);
@@ -989,6 +1031,27 @@ rel_skb:
 	__kfree_skb(skb);
 }
 
+static void do_rx_data(struct cxgbi_device *cdev, struct sk_buff *skb)
+{
+	struct cxgbi_sock *csk;
+	struct cpl_rx_data *cpl = (struct cpl_rx_data *)skb->data;
+	unsigned int tid = GET_TID(cpl);
+	struct cxgb4_lld_info *lldi = cxgbi_cdev_priv(cdev);
+	struct tid_info *t = lldi->tids;
+
+	csk = lookup_tid(t, tid);
+	if (!csk) {
+		pr_err("can't find connection for tid %u.\n", tid);
+	} else {
+		/* not expecting this, reset the connection. */
+		pr_err("csk 0x%p, tid %u, rcv cpl_rx_data.\n", csk, tid);
+		spin_lock_bh(&csk->lock);
+		send_abort_req(csk);
+		spin_unlock_bh(&csk->lock);
+	}
+	__kfree_skb(skb);
+}
+
 static void do_rx_iscsi_hdr(struct cxgbi_device *cdev, struct sk_buff *skb)
 {
 	struct cxgbi_sock *csk;
@@ -1408,6 +1471,7 @@ cxgb4i_cplhandler_func cxgb4i_cplhandlers[NUM_CPL_CMDS] = {
 	[CPL_SET_TCB_RPL] = do_set_tcb_rpl,
 	[CPL_RX_DATA_DDP] = do_rx_data_ddp,
 	[CPL_RX_ISCSI_DDP] = do_rx_data_ddp,
+	[CPL_RX_DATA] = do_rx_data,
 };
 
 int cxgb4i_ofld_init(struct cxgbi_device *cdev)
@@ -1485,7 +1549,7 @@ static int ddp_ppod_write_idata(struct cxgbi_device *cdev, unsigned int port_id,
 		return -ENOMEM;
 	}
 	req = (struct ulp_mem_io *)skb->head;
-	set_queue(skb, CPL_PRIORITY_CONTROL, NULL);
+	set_wr_txq(skb, CPL_PRIORITY_CONTROL, 0);
 
 	ulp_mem_io_set_hdr(lldi, req, wr_len, dlen, pm_addr);
 	idata = (struct ulptx_idata *)(req + 1);
diff --git a/drivers/scsi/cxgbi/libcxgbi.c b/drivers/scsi/cxgbi/libcxgbi.c
index 7da59c38a69e..eb58afcfb73b 100644
--- a/drivers/scsi/cxgbi/libcxgbi.c
+++ b/drivers/scsi/cxgbi/libcxgbi.c
@@ -2294,10 +2294,12 @@ int cxgbi_conn_xmit_pdu(struct iscsi_task *task)
 		return err;
 	}
 
-	kfree_skb(skb);
 	log_debug(1 << CXGBI_DBG_ISCSI | 1 << CXGBI_DBG_PDU_TX,
 		"itt 0x%x, skb 0x%p, len %u/%u, xmit err %d.\n",
 		task->itt, skb, skb->len, skb->data_len, err);
+
+	kfree_skb(skb);
+
 	iscsi_conn_printk(KERN_ERR, task->conn, "xmit err %d.\n", err);
 	iscsi_conn_failure(task->conn, ISCSI_ERR_XMIT_FAILED);
 	return err;
diff --git a/drivers/scsi/cxgbi/libcxgbi.h b/drivers/scsi/cxgbi/libcxgbi.h
index 2c7cb1c0c453..aba1af720df6 100644
--- a/drivers/scsi/cxgbi/libcxgbi.h
+++ b/drivers/scsi/cxgbi/libcxgbi.h
@@ -317,8 +317,8 @@ static inline void cxgbi_skcb_clear_flag(struct sk_buff *skb,
 	__clear_bit(flag, &(cxgbi_skcb_flags(skb)));
 }
 
-static inline int cxgbi_skcb_test_flag(struct sk_buff *skb,
-					enum cxgbi_skcb_flags flag)
+static inline int cxgbi_skcb_test_flag(const struct sk_buff *skb,
+				       enum cxgbi_skcb_flags flag)
 {
 	return test_bit(flag, &(cxgbi_skcb_flags(skb)));
 }
diff --git a/drivers/scsi/esas2r/esas2r_flash.c b/drivers/scsi/esas2r/esas2r_flash.c
index b7dc59fca7a6..7bd376d95ed5 100644
--- a/drivers/scsi/esas2r/esas2r_flash.c
+++ b/drivers/scsi/esas2r/esas2r_flash.c
@@ -684,9 +684,9 @@ static u16 calc_fi_checksum(struct esas2r_flash_context *fc)
  *              1)  verify the fi_version is correct
  *              2)  verify the checksum of the entire image.
  *              3)  validate the adap_typ, action and length fields.
- *              4)  valdiate each component header. check the img_type and
+ *              4)  validate each component header. check the img_type and
  *                  length fields
- *              5)  valdiate each component image.  validate signatures and
+ *              5)  validate each component image.  validate signatures and
  *                  local checksums
  */
 static bool verify_fi(struct esas2r_adapter *a,
diff --git a/drivers/scsi/esas2r/esas2r_main.c b/drivers/scsi/esas2r/esas2r_main.c
index 593ff8a63c70..7e1c21e6736b 100644
--- a/drivers/scsi/esas2r/esas2r_main.c
+++ b/drivers/scsi/esas2r/esas2r_main.c
@@ -255,7 +255,6 @@ static struct scsi_host_template driver_template = {
 	.emulated			= 0,
 	.proc_name			= ESAS2R_DRVR_NAME,
 	.change_queue_depth		= scsi_change_queue_depth,
-	.change_queue_type		= scsi_change_queue_type,
 	.max_sectors			= 0xFFFF,
 	.use_blk_tags			= 1,
 };
diff --git a/drivers/scsi/fcoe/fcoe.c b/drivers/scsi/fcoe/fcoe.c
index cd00a6cdf55b..ec193a8357d7 100644
--- a/drivers/scsi/fcoe/fcoe.c
+++ b/drivers/scsi/fcoe/fcoe.c
@@ -281,7 +281,6 @@ static struct scsi_host_template fcoe_shost_template = {
 	.eh_host_reset_handler = fc_eh_host_reset,
 	.slave_alloc = fc_slave_alloc,
 	.change_queue_depth = scsi_change_queue_depth,
-	.change_queue_type = scsi_change_queue_type,
 	.this_id = -1,
 	.cmd_per_lun = 3,
 	.can_queue = FCOE_MAX_OUTSTANDING_COMMANDS,
diff --git a/drivers/scsi/fnic/fnic_main.c b/drivers/scsi/fnic/fnic_main.c
index 0c1f8177b5b7..8a0d4d7b3254 100644
--- a/drivers/scsi/fnic/fnic_main.c
+++ b/drivers/scsi/fnic/fnic_main.c
@@ -111,7 +111,6 @@ static struct scsi_host_template fnic_host_template = {
 	.eh_host_reset_handler = fnic_host_reset,
 	.slave_alloc = fnic_slave_alloc,
 	.change_queue_depth = scsi_change_queue_depth,
-	.change_queue_type = scsi_change_queue_type,
 	.this_id = -1,
 	.cmd_per_lun = 3,
 	.can_queue = FNIC_DFLT_IO_REQ,
diff --git a/drivers/scsi/ibmvscsi/ibmvfc.c b/drivers/scsi/ibmvscsi/ibmvfc.c
index f58c6d8e0264..057d27721d5b 100644
--- a/drivers/scsi/ibmvscsi/ibmvfc.c
+++ b/drivers/scsi/ibmvscsi/ibmvfc.c
@@ -1615,7 +1615,6 @@ static int ibmvfc_queuecommand_lck(struct scsi_cmnd *cmnd,
 	struct fc_rport *rport = starget_to_rport(scsi_target(cmnd->device));
 	struct ibmvfc_cmd *vfc_cmd;
 	struct ibmvfc_event *evt;
-	u8 tag[2];
 	int rc;
 
 	if (unlikely((rc = fc_remote_port_chkready(rport))) ||
@@ -3089,7 +3088,6 @@ static struct scsi_host_template driver_template = {
 	.target_alloc = ibmvfc_target_alloc,
 	.scan_finished = ibmvfc_scan_finished,
 	.change_queue_depth = ibmvfc_change_queue_depth,
-	.change_queue_type = scsi_change_queue_type,
 	.cmd_per_lun = 16,
 	.can_queue = IBMVFC_MAX_REQUESTS_DEFAULT,
 	.this_id = -1,
diff --git a/drivers/scsi/ipr.c b/drivers/scsi/ipr.c
index 540294389355..df4e27cd996a 100644
--- a/drivers/scsi/ipr.c
+++ b/drivers/scsi/ipr.c
@@ -1426,16 +1426,14 @@ static void ipr_handle_config_change(struct ipr_ioa_cfg *ioa_cfg,
 		if (res->sdev) {
 			res->del_from_ml = 1;
 			res->res_handle = IPR_INVALID_RES_HANDLE;
-			if (ioa_cfg->allow_ml_add_del)
-				schedule_work(&ioa_cfg->work_q);
+			schedule_work(&ioa_cfg->work_q);
 		} else {
 			ipr_clear_res_target(res);
 			list_move_tail(&res->queue, &ioa_cfg->free_res_q);
 		}
 	} else if (!res->sdev || res->del_from_ml) {
 		res->add_to_ml = 1;
-		if (ioa_cfg->allow_ml_add_del)
-			schedule_work(&ioa_cfg->work_q);
+		schedule_work(&ioa_cfg->work_q);
 	}
 
 	ipr_send_hcam(ioa_cfg, IPR_HCAM_CDB_OP_CODE_CONFIG_CHANGE, hostrcb);
@@ -3273,8 +3271,7 @@ static void ipr_worker_thread(struct work_struct *work)
 restart:
 	do {
 		did_work = 0;
-		if (!ioa_cfg->hrrq[IPR_INIT_HRRQ].allow_cmds ||
-		    !ioa_cfg->allow_ml_add_del) {
+		if (!ioa_cfg->hrrq[IPR_INIT_HRRQ].allow_cmds) {
 			spin_unlock_irqrestore(ioa_cfg->host->host_lock, lock_flags);
 			return;
 		}
@@ -3311,6 +3308,7 @@ restart:
 		}
 	}
 
+	ioa_cfg->scan_done = 1;
 	spin_unlock_irqrestore(ioa_cfg->host->host_lock, lock_flags);
 	kobject_uevent(&ioa_cfg->host->shost_dev.kobj, KOBJ_CHANGE);
 	LEAVE;
@@ -4346,30 +4344,6 @@ static int ipr_change_queue_depth(struct scsi_device *sdev, int qdepth)
 }
 
 /**
- * ipr_change_queue_type - Change the device's queue type
- * @dsev:		scsi device struct
- * @tag_type:	type of tags to use
- *
- * Return value:
- * 	actual queue type set
- **/
-static int ipr_change_queue_type(struct scsi_device *sdev, int tag_type)
-{
-	struct ipr_ioa_cfg *ioa_cfg = (struct ipr_ioa_cfg *)sdev->host->hostdata;
-	struct ipr_resource_entry *res;
-	unsigned long lock_flags = 0;
-
-	spin_lock_irqsave(ioa_cfg->host->host_lock, lock_flags);
-	res = (struct ipr_resource_entry *)sdev->hostdata;
-	if (res && ipr_is_gscsi(res))
-		tag_type = scsi_change_queue_type(sdev, tag_type);
-	else
-		tag_type = 0;
-	spin_unlock_irqrestore(ioa_cfg->host->host_lock, lock_flags);
-	return tag_type;
-}
-
-/**
  * ipr_show_adapter_handle - Show the adapter's resource handle for this device
  * @dev:	device struct
  * @attr:	device attribute structure
@@ -4739,6 +4713,7 @@ static int ipr_slave_configure(struct scsi_device *sdev)
 			sdev->no_uld_attach = 1;
 		}
 		if (ipr_is_vset_device(res)) {
+			sdev->scsi_level = SCSI_SPC_3;
 			blk_queue_rq_timeout(sdev->request_queue,
 					     IPR_VSET_RW_TIMEOUT);
 			blk_queue_max_hw_sectors(sdev->request_queue, IPR_VSET_MAX_SECTORS);
@@ -5231,6 +5206,28 @@ static int ipr_cancel_op(struct scsi_cmnd *scsi_cmd)
  * @scsi_cmd:	scsi command struct
  *
  * Return value:
+ *	0 if scan in progress / 1 if scan is complete
+ **/
+static int ipr_scan_finished(struct Scsi_Host *shost, unsigned long elapsed_time)
+{
+	unsigned long lock_flags;
+	struct ipr_ioa_cfg *ioa_cfg = (struct ipr_ioa_cfg *) shost->hostdata;
+	int rc = 0;
+
+	spin_lock_irqsave(shost->host_lock, lock_flags);
+	if (ioa_cfg->hrrq[IPR_INIT_HRRQ].ioa_is_dead || ioa_cfg->scan_done)
+		rc = 1;
+	if ((elapsed_time/HZ) > (ioa_cfg->transop_timeout * 2))
+		rc = 1;
+	spin_unlock_irqrestore(shost->host_lock, lock_flags);
+	return rc;
+}
+
+/**
+ * ipr_eh_host_reset - Reset the host adapter
+ * @scsi_cmd:	scsi command struct
+ *
+ * Return value:
  * 	SUCCESS / FAILED
  **/
 static int ipr_eh_abort(struct scsi_cmnd *scsi_cmd)
@@ -5779,7 +5776,7 @@ static void ipr_erp_cancel_all(struct ipr_cmnd *ipr_cmd)
 
 	ipr_reinit_ipr_cmnd_for_erp(ipr_cmd);
 
-	if (!scsi_get_tag_type(scsi_cmd->device)) {
+	if (!scsi_cmd->device->simple_tags) {
 		ipr_erp_request_sense(ipr_cmd);
 		return;
 	}
@@ -6299,10 +6296,10 @@ static struct scsi_host_template driver_template = {
 	.slave_alloc = ipr_slave_alloc,
 	.slave_configure = ipr_slave_configure,
 	.slave_destroy = ipr_slave_destroy,
+	.scan_finished = ipr_scan_finished,
 	.target_alloc = ipr_target_alloc,
 	.target_destroy = ipr_target_destroy,
 	.change_queue_depth = ipr_change_queue_depth,
-	.change_queue_type = ipr_change_queue_type,
 	.bios_param = ipr_biosparam,
 	.can_queue = IPR_MAX_COMMANDS,
 	.this_id = -1,
@@ -6841,7 +6838,7 @@ static int ipr_ioa_reset_done(struct ipr_cmnd *ipr_cmd)
 	ioa_cfg->doorbell |= IPR_RUNTIME_RESET;
 
 	list_for_each_entry(res, &ioa_cfg->used_res_q, queue) {
-		if (ioa_cfg->allow_ml_add_del && (res->add_to_ml || res->del_from_ml)) {
+		if (res->add_to_ml || res->del_from_ml) {
 			ipr_trace;
 			break;
 		}
@@ -6870,6 +6867,7 @@ static int ipr_ioa_reset_done(struct ipr_cmnd *ipr_cmd)
 	if (!ioa_cfg->hrrq[IPR_INIT_HRRQ].allow_cmds)
 		scsi_block_requests(ioa_cfg->host);
 
+	schedule_work(&ioa_cfg->work_q);
 	LEAVE;
 	return IPR_RC_JOB_RETURN;
 }
@@ -7610,6 +7608,19 @@ static int ipr_ioafp_page0_inquiry(struct ipr_cmnd *ipr_cmd)
 	type[4] = '\0';
 	ioa_cfg->type = simple_strtoul((char *)type, NULL, 16);
 
+	if (ipr_invalid_adapter(ioa_cfg)) {
+		dev_err(&ioa_cfg->pdev->dev,
+			"Adapter not supported in this hardware configuration.\n");
+
+		if (!ipr_testmode) {
+			ioa_cfg->reset_retries += IPR_NUM_RESET_RELOAD_RETRIES;
+			ipr_initiate_ioa_reset(ioa_cfg, IPR_SHUTDOWN_NONE);
+			list_add_tail(&ipr_cmd->queue,
+					&ioa_cfg->hrrq->hrrq_free_q);
+			return IPR_RC_JOB_RETURN;
+		}
+	}
+
 	ipr_cmd->job_step = ipr_ioafp_page3_inquiry;
 
 	ipr_ioafp_inquiry(ipr_cmd, 1, 0,
@@ -8797,20 +8808,6 @@ static int ipr_probe_ioa_part2(struct ipr_ioa_cfg *ioa_cfg)
 		_ipr_initiate_ioa_reset(ioa_cfg, ipr_reset_enable_ioa,
 					IPR_SHUTDOWN_NONE);
 	spin_unlock_irqrestore(ioa_cfg->host->host_lock, host_lock_flags);
-	wait_event(ioa_cfg->reset_wait_q, !ioa_cfg->in_reset_reload);
-	spin_lock_irqsave(ioa_cfg->host->host_lock, host_lock_flags);
-
-	if (ioa_cfg->hrrq[IPR_INIT_HRRQ].ioa_is_dead) {
-		rc = -EIO;
-	} else if (ipr_invalid_adapter(ioa_cfg)) {
-		if (!ipr_testmode)
-			rc = -EIO;
-
-		dev_err(&ioa_cfg->pdev->dev,
-			"Adapter not supported in this hardware configuration.\n");
-	}
-
-	spin_unlock_irqrestore(ioa_cfg->host->host_lock, host_lock_flags);
 
 	LEAVE;
 	return rc;
@@ -9264,7 +9261,7 @@ static void ipr_init_ioa_cfg(struct ipr_ioa_cfg *ioa_cfg,
 					       * ioa_cfg->max_devs_supported)));
 	}
 
-	host->max_channel = IPR_MAX_BUS_TO_SCAN;
+	host->max_channel = IPR_VSET_BUS;
 	host->unique_id = host->host_no;
 	host->max_cmd_len = IPR_MAX_CDB_LEN;
 	host->can_queue = ioa_cfg->max_cmds;
@@ -9764,25 +9761,6 @@ out_scsi_host_put:
 }
 
 /**
- * ipr_scan_vsets - Scans for VSET devices
- * @ioa_cfg:	ioa config struct
- *
- * Description: Since the VSET resources do not follow SAM in that we can have
- * sparse LUNs with no LUN 0, we have to scan for these ourselves.
- *
- * Return value:
- * 	none
- **/
-static void ipr_scan_vsets(struct ipr_ioa_cfg *ioa_cfg)
-{
-	int target, lun;
-
-	for (target = 0; target < IPR_MAX_NUM_TARGETS_PER_BUS; target++)
-		for (lun = 0; lun < IPR_MAX_NUM_VSET_LUNS_PER_TARGET; lun++)
-			scsi_add_device(ioa_cfg->host, IPR_VSET_BUS, target, lun);
-}
-
-/**
  * ipr_initiate_ioa_bringdown - Bring down an adapter
  * @ioa_cfg:		ioa config struct
  * @shutdown_type:	shutdown type
@@ -9937,10 +9915,6 @@ static int ipr_probe(struct pci_dev *pdev, const struct pci_device_id *dev_id)
 	}
 
 	scsi_scan_host(ioa_cfg->host);
-	ipr_scan_vsets(ioa_cfg);
-	scsi_add_device(ioa_cfg->host, IPR_IOA_BUS, IPR_IOA_TARGET, IPR_IOA_LUN);
-	ioa_cfg->allow_ml_add_del = 1;
-	ioa_cfg->host->max_channel = IPR_VSET_BUS;
 	ioa_cfg->iopoll_weight = ioa_cfg->chip_cfg->iopoll_weight;
 
 	if (ioa_cfg->iopoll_weight && ioa_cfg->sis64 && ioa_cfg->nvectors > 1) {
diff --git a/drivers/scsi/ipr.h b/drivers/scsi/ipr.h
index 9ebdebd944e7..b4f3eec51bc9 100644
--- a/drivers/scsi/ipr.h
+++ b/drivers/scsi/ipr.h
@@ -157,13 +157,11 @@
 
 #define IPR_MAX_NUM_TARGETS_PER_BUS			256
 #define IPR_MAX_NUM_LUNS_PER_TARGET			256
-#define IPR_MAX_NUM_VSET_LUNS_PER_TARGET	8
 #define IPR_VSET_BUS					0xff
 #define IPR_IOA_BUS						0xff
 #define IPR_IOA_TARGET					0xff
 #define IPR_IOA_LUN						0xff
 #define IPR_MAX_NUM_BUSES				16
-#define IPR_MAX_BUS_TO_SCAN				IPR_MAX_NUM_BUSES
 
 #define IPR_NUM_RESET_RELOAD_RETRIES		3
 
@@ -1453,7 +1451,7 @@ struct ipr_ioa_cfg {
 	u8 in_ioa_bringdown:1;
 	u8 ioa_unit_checked:1;
 	u8 dump_taken:1;
-	u8 allow_ml_add_del:1;
+	u8 scan_done:1;
 	u8 needs_hard_reset:1;
 	u8 dual_raid:1;
 	u8 needs_warm_reset:1;
diff --git a/drivers/scsi/isci/init.c b/drivers/scsi/isci/init.c
index 724c6265b667..cd41b63a2f10 100644
--- a/drivers/scsi/isci/init.c
+++ b/drivers/scsi/isci/init.c
@@ -158,7 +158,6 @@ static struct scsi_host_template isci_sht = {
 	.scan_finished			= isci_host_scan_finished,
 	.scan_start			= isci_host_start,
 	.change_queue_depth		= sas_change_queue_depth,
-	.change_queue_type		= sas_change_queue_type,
 	.bios_param			= sas_bios_param,
 	.can_queue			= ISCI_CAN_QUEUE_VAL,
 	.cmd_per_lun			= 1,
diff --git a/drivers/scsi/isci/request.c b/drivers/scsi/isci/request.c
index 56e38096f0c4..cfd0084f1cd2 100644
--- a/drivers/scsi/isci/request.c
+++ b/drivers/scsi/isci/request.c
@@ -694,7 +694,7 @@ sci_io_request_construct_sata(struct isci_request *ireq,
 	}
 
 	/* ATAPI */
-	if (dev->sata_dev.command_set == ATAPI_COMMAND_SET &&
+	if (dev->sata_dev.class == ATA_DEV_ATAPI &&
 	    task->ata_task.fis.command == ATA_CMD_PACKET) {
 		sci_atapi_construct(ireq);
 		return SCI_SUCCESS;
@@ -2980,7 +2980,7 @@ static void sci_request_started_state_enter(struct sci_base_state_machine *sm)
 		state = SCI_REQ_SMP_WAIT_RESP;
 	} else if (task && sas_protocol_ata(task->task_proto) &&
 		   !task->ata_task.use_ncq) {
-		if (dev->sata_dev.command_set == ATAPI_COMMAND_SET &&
+		if (dev->sata_dev.class == ATA_DEV_ATAPI &&
 			task->ata_task.fis.command == ATA_CMD_PACKET) {
 			state = SCI_REQ_ATAPI_WAIT_H2D;
 		} else if (task->data_dir == DMA_NONE) {
diff --git a/drivers/scsi/isci/task.c b/drivers/scsi/isci/task.c
index 3f63c6318b0d..6dcaed0c1fc8 100644
--- a/drivers/scsi/isci/task.c
+++ b/drivers/scsi/isci/task.c
@@ -588,7 +588,7 @@ int isci_task_abort_task(struct sas_task *task)
 
 		ret = TMF_RESP_FUNC_COMPLETE;
 	} else {
-		/* Fill in the tmf stucture */
+		/* Fill in the tmf structure */
 		isci_task_build_abort_task_tmf(&tmf, isci_tmf_ssp_task_abort,
 					       old_request);
 
diff --git a/drivers/scsi/jazz_esp.c b/drivers/scsi/jazz_esp.c
index 69efbf12b299..9aaa74e349cc 100644
--- a/drivers/scsi/jazz_esp.c
+++ b/drivers/scsi/jazz_esp.c
@@ -226,7 +226,6 @@ static struct platform_driver esp_jazz_driver = {
 	.remove		= esp_jazz_remove,
 	.driver	= {
 		.name	= "jazz_esp",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/scsi/libsas/sas_ata.c b/drivers/scsi/libsas/sas_ata.c
index 577770fdee86..932d9cc98d2f 100644
--- a/drivers/scsi/libsas/sas_ata.c
+++ b/drivers/scsi/libsas/sas_ata.c
@@ -138,7 +138,7 @@ static void sas_ata_task_done(struct sas_task *task)
 
 	if (stat->stat == SAS_PROTO_RESPONSE || stat->stat == SAM_STAT_GOOD ||
 	    ((stat->stat == SAM_STAT_CHECK_CONDITION &&
-	      dev->sata_dev.command_set == ATAPI_COMMAND_SET))) {
+	      dev->sata_dev.class == ATA_DEV_ATAPI))) {
 		memcpy(dev->sata_dev.fis, resp->ending_fis, ATA_RESP_FIS_SIZE);
 
 		if (!link->sactive) {
@@ -272,7 +272,7 @@ static struct sas_internal *dev_to_sas_internal(struct domain_device *dev)
 	return to_sas_internal(dev->port->ha->core.shost->transportt);
 }
 
-static void sas_get_ata_command_set(struct domain_device *dev);
+static int sas_get_ata_command_set(struct domain_device *dev);
 
 int sas_get_ata_info(struct domain_device *dev, struct ex_phy *phy)
 {
@@ -297,8 +297,7 @@ int sas_get_ata_info(struct domain_device *dev, struct ex_phy *phy)
 		}
 		memcpy(dev->frame_rcvd, &dev->sata_dev.rps_resp.rps.fis,
 		       sizeof(struct dev_to_host_fis));
-		/* TODO switch to ata_dev_classify() */
-		sas_get_ata_command_set(dev);
+		dev->sata_dev.class = sas_get_ata_command_set(dev);
 	}
 	return 0;
 }
@@ -419,18 +418,7 @@ static int sas_ata_hard_reset(struct ata_link *link, unsigned int *class,
 	if (ret && ret != -EAGAIN)
 		sas_ata_printk(KERN_ERR, dev, "reset failed (errno=%d)\n", ret);
 
-	/* XXX: if the class changes during the reset the upper layer
-	 * should be informed, if the device has gone away we assume
-	 * libsas will eventually delete it
-	 */
-	switch (dev->sata_dev.command_set) {
-	case ATA_COMMAND_SET:
-		*class = ATA_DEV_ATA;
-		break;
-	case ATAPI_COMMAND_SET:
-		*class = ATA_DEV_ATAPI;
-		break;
-	}
+	*class = dev->sata_dev.class;
 
 	ap->cbl = ATA_CBL_SATA;
 	return ret;
@@ -619,50 +607,18 @@ void sas_ata_task_abort(struct sas_task *task)
 	complete(waiting);
 }
 
-static void sas_get_ata_command_set(struct domain_device *dev)
+static int sas_get_ata_command_set(struct domain_device *dev)
 {
 	struct dev_to_host_fis *fis =
 		(struct dev_to_host_fis *) dev->frame_rcvd;
+	struct ata_taskfile tf;
 
 	if (dev->dev_type == SAS_SATA_PENDING)
-		return;
+		return ATA_DEV_UNKNOWN;
+
+	ata_tf_from_fis((const u8 *)fis, &tf);
 
-	if ((fis->sector_count == 1 && /* ATA */
-	     fis->lbal         == 1 &&
-	     fis->lbam         == 0 &&
-	     fis->lbah         == 0 &&
-	     fis->device       == 0)
-	    ||
-	    (fis->sector_count == 0 && /* CE-ATA (mATA) */
-	     fis->lbal         == 0 &&
-	     fis->lbam         == 0xCE &&
-	     fis->lbah         == 0xAA &&
-	     (fis->device & ~0x10) == 0))
-
-		dev->sata_dev.command_set = ATA_COMMAND_SET;
-
-	else if ((fis->interrupt_reason == 1 &&	/* ATAPI */
-		  fis->lbal             == 1 &&
-		  fis->byte_count_low   == 0x14 &&
-		  fis->byte_count_high  == 0xEB &&
-		  (fis->device & ~0x10) == 0))
-
-		dev->sata_dev.command_set = ATAPI_COMMAND_SET;
-
-	else if ((fis->sector_count == 1 && /* SEMB */
-		  fis->lbal         == 1 &&
-		  fis->lbam         == 0x3C &&
-		  fis->lbah         == 0xC3 &&
-		  fis->device       == 0)
-		||
-		 (fis->interrupt_reason == 1 &&	/* SATA PM */
-		  fis->lbal             == 1 &&
-		  fis->byte_count_low   == 0x69 &&
-		  fis->byte_count_high  == 0x96 &&
-		  (fis->device & ~0x10) == 0))
-
-		/* Treat it as a superset? */
-		dev->sata_dev.command_set = ATAPI_COMMAND_SET;
+	return ata_dev_classify(&tf);
 }
 
 void sas_probe_sata(struct asd_sas_port *port)
@@ -768,7 +724,7 @@ int sas_discover_sata(struct domain_device *dev)
 	if (dev->dev_type == SAS_SATA_PM)
 		return -ENODEV;
 
-	sas_get_ata_command_set(dev);
+	dev->sata_dev.class = sas_get_ata_command_set(dev);
 	sas_fill_in_rphy(dev, dev->rphy);
 
 	res = sas_notify_lldd_dev_found(dev);
diff --git a/drivers/scsi/libsas/sas_scsi_host.c b/drivers/scsi/libsas/sas_scsi_host.c
index 72918d227ead..519dac4e341e 100644
--- a/drivers/scsi/libsas/sas_scsi_host.c
+++ b/drivers/scsi/libsas/sas_scsi_host.c
@@ -906,13 +906,6 @@ int sas_change_queue_depth(struct scsi_device *sdev, int depth)
 	return scsi_change_queue_depth(sdev, depth);
 }
 
-int sas_change_queue_type(struct scsi_device *scsi_dev, int type)
-{
-	if (dev_is_sata(sdev_to_domain_dev(scsi_dev)))
-		return -EINVAL;
-	return scsi_change_queue_type(scsi_dev, type);
-}
-
 int sas_bios_param(struct scsi_device *scsi_dev,
 			  struct block_device *bdev,
 			  sector_t capacity, int *hsc)
@@ -1011,7 +1004,6 @@ EXPORT_SYMBOL_GPL(sas_queuecommand);
 EXPORT_SYMBOL_GPL(sas_target_alloc);
 EXPORT_SYMBOL_GPL(sas_slave_configure);
 EXPORT_SYMBOL_GPL(sas_change_queue_depth);
-EXPORT_SYMBOL_GPL(sas_change_queue_type);
 EXPORT_SYMBOL_GPL(sas_bios_param);
 EXPORT_SYMBOL_GPL(sas_task_abort);
 EXPORT_SYMBOL_GPL(sas_phy_reset);
diff --git a/drivers/scsi/lpfc/lpfc_scsi.c b/drivers/scsi/lpfc/lpfc_scsi.c
index fd85952b621d..4f9222eb2266 100644
--- a/drivers/scsi/lpfc/lpfc_scsi.c
+++ b/drivers/scsi/lpfc/lpfc_scsi.c
@@ -5879,7 +5879,6 @@ struct scsi_host_template lpfc_template = {
 	.max_sectors		= 0xFFFF,
 	.vendor_id		= LPFC_NL_VENDOR_ID,
 	.change_queue_depth	= scsi_change_queue_depth,
-	.change_queue_type	= scsi_change_queue_type,
 	.use_blk_tags		= 1,
 	.track_queue_depth	= 1,
 };
@@ -5904,7 +5903,6 @@ struct scsi_host_template lpfc_vport_template = {
 	.shost_attrs		= lpfc_vport_attrs,
 	.max_sectors		= 0xFFFF,
 	.change_queue_depth	= scsi_change_queue_depth,
-	.change_queue_type	= scsi_change_queue_type,
 	.use_blk_tags		= 1,
 	.track_queue_depth	= 1,
 };
diff --git a/drivers/scsi/mac_esp.c b/drivers/scsi/mac_esp.c
index 994fc5caf036..14c0334f41e4 100644
--- a/drivers/scsi/mac_esp.c
+++ b/drivers/scsi/mac_esp.c
@@ -617,7 +617,6 @@ static struct platform_driver esp_mac_driver = {
 	.remove   = esp_mac_remove,
 	.driver   = {
 		.name	= DRV_MODULE_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/scsi/mac_scsi.c b/drivers/scsi/mac_scsi.c
index 953fd9b953c7..1e85c07e3b62 100644
--- a/drivers/scsi/mac_scsi.c
+++ b/drivers/scsi/mac_scsi.c
@@ -166,7 +166,7 @@ static void mac_scsi_reset_boot(struct Scsi_Host *instance)
    XXX: Since bus errors in the PDMA routines never happen on my 
    computer, the bus error code is untested. 
    If the code works as intended, a bus error results in Pseudo-DMA 
-   beeing disabled, meaning that the driver switches to slow handshake. 
+   being disabled, meaning that the driver switches to slow handshake.
    If bus errors are NOT extremely rare, this has to be changed. 
 */
 
diff --git a/drivers/scsi/megaraid/megaraid_sas_base.c b/drivers/scsi/megaraid/megaraid_sas_base.c
index f05580e693d0..ff283d23788a 100644
--- a/drivers/scsi/megaraid/megaraid_sas_base.c
+++ b/drivers/scsi/megaraid/megaraid_sas_base.c
@@ -6783,7 +6783,7 @@ static int __init megasas_init(void)
 	rval = pci_register_driver(&megasas_pci_driver);
 
 	if (rval) {
-		printk(KERN_DEBUG "megasas: PCI hotplug regisration failed \n");
+		printk(KERN_DEBUG "megasas: PCI hotplug registration failed \n");
 		goto err_pcidrv;
 	}
 
diff --git a/drivers/scsi/mpt2sas/mpt2sas_ctl.h b/drivers/scsi/mpt2sas/mpt2sas_ctl.h
index fa0567c96050..7f842c88abd2 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_ctl.h
+++ b/drivers/scsi/mpt2sas/mpt2sas_ctl.h
@@ -224,7 +224,7 @@ struct mpt2_ioctl_eventreport {
 };
 
 /**
- * struct mpt2_ioctl_command - generic mpt firmware passthru ioclt
+ * struct mpt2_ioctl_command - generic mpt firmware passthru ioctl
  * @hdr - generic header
  * @timeout - command timeout in seconds. (if zero then use driver default
  *  value).
diff --git a/drivers/scsi/mpt2sas/mpt2sas_scsih.c b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
index 8431eb10bbb1..6a1c036a6f3f 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_scsih.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
@@ -7592,7 +7592,6 @@ static struct scsi_host_template scsih_driver_template = {
 	.scan_finished			= _scsih_scan_finished,
 	.scan_start			= _scsih_scan_start,
 	.change_queue_depth 		= _scsih_change_queue_depth,
-	.change_queue_type		= scsi_change_queue_type,
 	.eh_abort_handler		= _scsih_abort,
 	.eh_device_reset_handler	= _scsih_dev_reset,
 	.eh_target_reset_handler	= _scsih_target_reset,
diff --git a/drivers/scsi/mpt2sas/mpt2sas_transport.c b/drivers/scsi/mpt2sas/mpt2sas_transport.c
index 0d1d06488a28..e689bf20a3ea 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_transport.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_transport.c
@@ -1006,12 +1006,9 @@ mpt2sas_transport_update_links(struct MPT2SAS_ADAPTER *ioc,
 		    &mpt2sas_phy->remote_identify);
 		_transport_add_phy_to_an_existing_port(ioc, sas_node,
 		    mpt2sas_phy, mpt2sas_phy->remote_identify.sas_address);
-	} else {
+	} else
 		memset(&mpt2sas_phy->remote_identify, 0 , sizeof(struct
 		    sas_identify));
-		_transport_del_phy_from_an_existing_port(ioc, sas_node,
-		    mpt2sas_phy);
-	}
 
 	if (mpt2sas_phy->phy)
 		mpt2sas_phy->phy->negotiated_linkrate =
diff --git a/drivers/scsi/mpt3sas/mpt3sas_scsih.c b/drivers/scsi/mpt3sas/mpt3sas_scsih.c
index a2b60991efd4..94261ee9e72d 100644
--- a/drivers/scsi/mpt3sas/mpt3sas_scsih.c
+++ b/drivers/scsi/mpt3sas/mpt3sas_scsih.c
@@ -7229,7 +7229,6 @@ static struct scsi_host_template scsih_driver_template = {
 	.scan_finished			= _scsih_scan_finished,
 	.scan_start			= _scsih_scan_start,
 	.change_queue_depth		= _scsih_change_queue_depth,
-	.change_queue_type		= scsi_change_queue_type,
 	.eh_abort_handler		= _scsih_abort,
 	.eh_device_reset_handler	= _scsih_dev_reset,
 	.eh_target_reset_handler	= _scsih_target_reset,
diff --git a/drivers/scsi/mpt3sas/mpt3sas_transport.c b/drivers/scsi/mpt3sas/mpt3sas_transport.c
index d4bafaaebea9..3637ae6c0171 100644
--- a/drivers/scsi/mpt3sas/mpt3sas_transport.c
+++ b/drivers/scsi/mpt3sas/mpt3sas_transport.c
@@ -1003,12 +1003,9 @@ mpt3sas_transport_update_links(struct MPT3SAS_ADAPTER *ioc,
 		    &mpt3sas_phy->remote_identify);
 		_transport_add_phy_to_an_existing_port(ioc, sas_node,
 		    mpt3sas_phy, mpt3sas_phy->remote_identify.sas_address);
-	} else {
+	} else
 		memset(&mpt3sas_phy->remote_identify, 0 , sizeof(struct
 		    sas_identify));
-		_transport_del_phy_from_an_existing_port(ioc, sas_node,
-		    mpt3sas_phy);
-	}
 
 	if (mpt3sas_phy->phy)
 		mpt3sas_phy->phy->negotiated_linkrate =
diff --git a/drivers/scsi/mvme16x_scsi.c b/drivers/scsi/mvme16x_scsi.c
index 8fbb97a8bfd3..050c8c39d7ed 100644
--- a/drivers/scsi/mvme16x_scsi.c
+++ b/drivers/scsi/mvme16x_scsi.c
@@ -126,7 +126,6 @@ static int mvme16x_device_remove(struct platform_device *dev)
 static struct platform_driver mvme16x_scsi_driver = {
 	.driver = {
 		.name           = "mvme16x-scsi",
-		.owner          = THIS_MODULE,
 	},
 	.probe          = mvme16x_probe,
 	.remove         = mvme16x_device_remove,
diff --git a/drivers/scsi/mvsas/mv_init.c b/drivers/scsi/mvsas/mv_init.c
index f15df3de6790..53030b0e8015 100644
--- a/drivers/scsi/mvsas/mv_init.c
+++ b/drivers/scsi/mvsas/mv_init.c
@@ -54,7 +54,6 @@ static struct scsi_host_template mvs_sht = {
 	.scan_finished		= mvs_scan_finished,
 	.scan_start		= mvs_scan_start,
 	.change_queue_depth	= sas_change_queue_depth,
-	.change_queue_type	= sas_change_queue_type,
 	.bios_param		= sas_bios_param,
 	.can_queue		= 1,
 	.cmd_per_lun		= 1,
diff --git a/drivers/scsi/mvsas/mv_sas.c b/drivers/scsi/mvsas/mv_sas.c
index 85d86a5cdb60..2d5ab6d969ec 100644
--- a/drivers/scsi/mvsas/mv_sas.c
+++ b/drivers/scsi/mvsas/mv_sas.c
@@ -479,7 +479,7 @@ static int mvs_task_prep_ata(struct mvs_info *mvi,
 
 	if (task->ata_task.use_ncq)
 		flags |= MCH_FPDMA;
-	if (dev->sata_dev.command_set == ATAPI_COMMAND_SET) {
+	if (dev->sata_dev.class == ATA_DEV_ATAPI) {
 		if (task->ata_task.fis.command != ATA_CMD_ID_ATAPI)
 			flags |= MCH_ATAPI;
 	}
@@ -546,7 +546,7 @@ static int mvs_task_prep_ata(struct mvs_info *mvi,
 		task->ata_task.fis.flags |= 0x80; /* C=1: update ATA cmd reg */
 	/* fill in command FIS and ATAPI CDB */
 	memcpy(buf_cmd, &task->ata_task.fis, sizeof(struct host_to_dev_fis));
-	if (dev->sata_dev.command_set == ATAPI_COMMAND_SET)
+	if (dev->sata_dev.class == ATA_DEV_ATAPI)
 		memcpy(buf_cmd + STP_ATAPI_CMD,
 			task->ata_task.atapi_packet, 16);
 
diff --git a/drivers/scsi/pm8001/pm8001_hwi.c b/drivers/scsi/pm8001/pm8001_hwi.c
index 933f21471951..96dcc097a463 100644
--- a/drivers/scsi/pm8001/pm8001_hwi.c
+++ b/drivers/scsi/pm8001/pm8001_hwi.c
@@ -4367,7 +4367,7 @@ static int pm8001_chip_sata_req(struct pm8001_hba_info *pm8001_ha,
 			PM8001_IO_DBG(pm8001_ha, pm8001_printk("PIO\n"));
 		}
 		if (task->ata_task.use_ncq &&
-			dev->sata_dev.command_set != ATAPI_COMMAND_SET) {
+			dev->sata_dev.class != ATA_DEV_ATAPI) {
 			ATAP = 0x07; /* FPDMA */
 			PM8001_IO_DBG(pm8001_ha, pm8001_printk("FPDMA\n"));
 		}
diff --git a/drivers/scsi/pm8001/pm8001_init.c b/drivers/scsi/pm8001/pm8001_init.c
index 329aba0083ab..65555916d3b8 100644
--- a/drivers/scsi/pm8001/pm8001_init.c
+++ b/drivers/scsi/pm8001/pm8001_init.c
@@ -76,7 +76,6 @@ static struct scsi_host_template pm8001_sht = {
 	.scan_finished		= pm8001_scan_finished,
 	.scan_start		= pm8001_scan_start,
 	.change_queue_depth	= sas_change_queue_depth,
-	.change_queue_type	= sas_change_queue_type,
 	.bios_param		= sas_bios_param,
 	.can_queue		= 1,
 	.cmd_per_lun		= 1,
diff --git a/drivers/scsi/pm8001/pm80xx_hwi.c b/drivers/scsi/pm8001/pm80xx_hwi.c
index b06443a0db2d..05cce463ab01 100644
--- a/drivers/scsi/pm8001/pm80xx_hwi.c
+++ b/drivers/scsi/pm8001/pm80xx_hwi.c
@@ -4077,7 +4077,7 @@ static int pm80xx_chip_sata_req(struct pm8001_hba_info *pm8001_ha,
 			PM8001_IO_DBG(pm8001_ha, pm8001_printk("PIO\n"));
 		}
 		if (task->ata_task.use_ncq &&
-			dev->sata_dev.command_set != ATAPI_COMMAND_SET) {
+		    dev->sata_dev.class != ATA_DEV_ATAPI) {
 			ATAP = 0x07; /* FPDMA */
 			PM8001_IO_DBG(pm8001_ha, pm8001_printk("FPDMA\n"));
 		}
diff --git a/drivers/scsi/pmcraid.c b/drivers/scsi/pmcraid.c
index b1b1f66b1ab7..8c27b6a77ec4 100644
--- a/drivers/scsi/pmcraid.c
+++ b/drivers/scsi/pmcraid.c
@@ -4251,7 +4251,6 @@ static struct scsi_host_template pmcraid_host_template = {
 	.slave_configure = pmcraid_slave_configure,
 	.slave_destroy = pmcraid_slave_destroy,
 	.change_queue_depth = pmcraid_change_queue_depth,
-	.change_queue_type  = scsi_change_queue_type,
 	.can_queue = PMCRAID_MAX_IO_CMD,
 	.this_id = -1,
 	.sg_tablesize = PMCRAID_MAX_IOADLS,
diff --git a/drivers/scsi/ps3rom.c b/drivers/scsi/ps3rom.c
index b3b48b5a984c..5298def33733 100644
--- a/drivers/scsi/ps3rom.c
+++ b/drivers/scsi/ps3rom.c
@@ -383,6 +383,7 @@ static int ps3rom_probe(struct ps3_system_bus_device *_dev)
 	if (!host) {
 		dev_err(&dev->sbd.core, "%s:%u: scsi_host_alloc failed\n",
 			__func__, __LINE__);
+		error = -ENOMEM;
 		goto fail_teardown;
 	}
 
diff --git a/drivers/scsi/qla2xxx/qla_init.c b/drivers/scsi/qla2xxx/qla_init.c
index a4dde7e80dbd..e59f25bff7ab 100644
--- a/drivers/scsi/qla2xxx/qla_init.c
+++ b/drivers/scsi/qla2xxx/qla_init.c
@@ -3237,8 +3237,6 @@ qla2x00_reg_remote_port(scsi_qla_host_t *vha, fc_port_t *fcport)
 	struct fc_rport *rport;
 	unsigned long flags;
 
-	qla2x00_rport_del(fcport);
-
 	rport_ids.node_name = wwn_to_u64(fcport->node_name);
 	rport_ids.port_name = wwn_to_u64(fcport->port_name);
 	rport_ids.port_id = fcport->d_id.b.domain << 16 |
diff --git a/drivers/scsi/qla2xxx/qla_os.c b/drivers/scsi/qla2xxx/qla_os.c
index 6b4d9235368a..12ca291c1380 100644
--- a/drivers/scsi/qla2xxx/qla_os.c
+++ b/drivers/scsi/qla2xxx/qla_os.c
@@ -258,7 +258,6 @@ struct scsi_host_template qla2xxx_driver_template = {
 	.scan_finished		= qla2xxx_scan_finished,
 	.scan_start		= qla2xxx_scan_start,
 	.change_queue_depth	= scsi_change_queue_depth,
-	.change_queue_type	= scsi_change_queue_type,
 	.this_id		= -1,
 	.cmd_per_lun		= 3,
 	.use_clustering		= ENABLE_CLUSTERING,
diff --git a/drivers/scsi/qla2xxx/qla_target.c b/drivers/scsi/qla2xxx/qla_target.c
index a902fa1db7af..57418258c101 100644
--- a/drivers/scsi/qla2xxx/qla_target.c
+++ b/drivers/scsi/qla2xxx/qla_target.c
@@ -3218,25 +3218,25 @@ static inline int qlt_get_fcp_task_attr(struct scsi_qla_host *vha,
 
 	switch (task_codes) {
 	case ATIO_SIMPLE_QUEUE:
-		fcp_task_attr = MSG_SIMPLE_TAG;
+		fcp_task_attr = TCM_SIMPLE_TAG;
 		break;
 	case ATIO_HEAD_OF_QUEUE:
-		fcp_task_attr = MSG_HEAD_TAG;
+		fcp_task_attr = TCM_HEAD_TAG;
 		break;
 	case ATIO_ORDERED_QUEUE:
-		fcp_task_attr = MSG_ORDERED_TAG;
+		fcp_task_attr = TCM_ORDERED_TAG;
 		break;
 	case ATIO_ACA_QUEUE:
-		fcp_task_attr = MSG_ACA_TAG;
+		fcp_task_attr = TCM_ACA_TAG;
 		break;
 	case ATIO_UNTAGGED:
-		fcp_task_attr = MSG_SIMPLE_TAG;
+		fcp_task_attr = TCM_SIMPLE_TAG;
 		break;
 	default:
 		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf05d,
 		    "qla_target: unknown task code %x, use ORDERED instead\n",
 		    task_codes);
-		fcp_task_attr = MSG_ORDERED_TAG;
+		fcp_task_attr = TCM_ORDERED_TAG;
 		break;
 	}
 
diff --git a/drivers/scsi/qlogicpti.c b/drivers/scsi/qlogicpti.c
index 740ae495aa77..fe122700cad8 100644
--- a/drivers/scsi/qlogicpti.c
+++ b/drivers/scsi/qlogicpti.c
@@ -1455,7 +1455,6 @@ MODULE_DEVICE_TABLE(of, qpti_match);
 static struct platform_driver qpti_sbus_driver = {
 	.driver = {
 		.name = "qpti",
-		.owner = THIS_MODULE,
 		.of_match_table = qpti_match,
 	},
 	.probe		= qpti_sbus_probe,
diff --git a/drivers/scsi/scsi.c b/drivers/scsi/scsi.c
index 1ad0c36375b8..e02885451425 100644
--- a/drivers/scsi/scsi.c
+++ b/drivers/scsi/scsi.c
@@ -739,34 +739,12 @@ int scsi_track_queue_full(struct scsi_device *sdev, int depth)
 
 	if (sdev->last_queue_full_count <= 10)
 		return 0;
-	if (sdev->last_queue_full_depth < 8) {
-		/* Drop back to untagged */
-		scsi_set_tag_type(sdev, 0);
-		scsi_change_queue_depth(sdev, sdev->host->cmd_per_lun);
-		return -1;
-	}
 
 	return scsi_change_queue_depth(sdev, depth);
 }
 EXPORT_SYMBOL(scsi_track_queue_full);
 
 /**
- * scsi_change_queue_type() - Change a device's queue type
- * @sdev:     The SCSI device whose queue depth is to change
- * @tag_type: Identifier for queue type
- */
-int scsi_change_queue_type(struct scsi_device *sdev, int tag_type)
-{
-	if (!sdev->tagged_supported)
-		return 0;
-
-	scsi_set_tag_type(sdev, tag_type);
-	return tag_type;
-
-}
-EXPORT_SYMBOL(scsi_change_queue_type);
-
-/**
  * scsi_vpd_inquiry - Request a device provide us with a VPD page
  * @sdev: The device to ask
  * @buffer: Where to put the result
diff --git a/drivers/scsi/scsi_debug.c b/drivers/scsi/scsi_debug.c
index aa4b6b80aade..7b8b51bc29b4 100644
--- a/drivers/scsi/scsi_debug.c
+++ b/drivers/scsi/scsi_debug.c
@@ -128,7 +128,6 @@ static const char *scsi_debug_version_date = "20141022";
 #define DEF_REMOVABLE false
 #define DEF_SCSI_LEVEL   6    /* INQUIRY, byte2 [6->SPC-4] */
 #define DEF_SECTOR_SIZE 512
-#define DEF_TAGGED_QUEUING 0 /* 0 | MSG_SIMPLE_TAG | MSG_ORDERED_TAG */
 #define DEF_UNMAP_ALIGNMENT 0
 #define DEF_UNMAP_GRANULARITY 1
 #define DEF_UNMAP_MAX_BLOCKS 0xFFFFFFFF
@@ -817,6 +816,7 @@ static int check_readiness(struct scsi_cmnd *SCpnt, int uas_only,
 					UA_CHANGED_ASC, CAPACITY_CHANGED_ASCQ);
 			if (debug)
 				cp = "capacity data changed";
+			break;
 		default:
 			pr_warn("%s: unexpected unit attention code=%d\n",
 				__func__, k);
@@ -3045,18 +3045,12 @@ resp_comp_write(struct scsi_cmnd *scp, struct sdebug_dev_info *devip)
 	u8 num;
 	unsigned long iflags;
 	int ret;
+	int retval = 0;
 
-	lba = get_unaligned_be32(cmd + 2);
+	lba = get_unaligned_be64(cmd + 2);
 	num = cmd[13];		/* 1 to a maximum of 255 logical blocks */
 	if (0 == num)
 		return 0;	/* degenerate case, not an error */
-	dnum = 2 * num;
-	arr = kzalloc(dnum * lb_size, GFP_ATOMIC);
-	if (NULL == arr) {
-		mk_sense_buffer(scp, ILLEGAL_REQUEST, INSUFF_RES_ASC,
-				INSUFF_RES_ASCQ);
-		return check_condition_result;
-	}
 	if (scsi_debug_dif == SD_DIF_TYPE2_PROTECTION &&
 	    (cmd[1] & 0xe0)) {
 		mk_sense_invalid_opcode(scp);
@@ -3079,6 +3073,13 @@ resp_comp_write(struct scsi_cmnd *scp, struct sdebug_dev_info *devip)
 		mk_sense_buffer(scp, ILLEGAL_REQUEST, INVALID_FIELD_IN_CDB, 0);
 		return check_condition_result;
 	}
+	dnum = 2 * num;
+	arr = kzalloc(dnum * lb_size, GFP_ATOMIC);
+	if (NULL == arr) {
+		mk_sense_buffer(scp, ILLEGAL_REQUEST, INSUFF_RES_ASC,
+				INSUFF_RES_ASCQ);
+		return check_condition_result;
+	}
 
 	write_lock_irqsave(&atomic_rw, iflags);
 
@@ -3089,24 +3090,24 @@ resp_comp_write(struct scsi_cmnd *scp, struct sdebug_dev_info *devip)
 	ret = do_device_access(scp, 0, dnum, true);
 	fake_storep = fake_storep_hold;
 	if (ret == -1) {
-		write_unlock_irqrestore(&atomic_rw, iflags);
-		kfree(arr);
-		return DID_ERROR << 16;
+		retval = DID_ERROR << 16;
+		goto cleanup;
 	} else if ((ret < (dnum * lb_size)) &&
 		 (SCSI_DEBUG_OPT_NOISE & scsi_debug_opts))
 		sdev_printk(KERN_INFO, scp->device, "%s: compare_write: cdb "
 			    "indicated=%u, IO sent=%d bytes\n", my_name,
 			    dnum * lb_size, ret);
 	if (!comp_write_worker(lba, num, arr)) {
-		write_unlock_irqrestore(&atomic_rw, iflags);
-		kfree(arr);
 		mk_sense_buffer(scp, MISCOMPARE, MISCOMPARE_VERIFY_ASC, 0);
-		return check_condition_result;
+		retval = check_condition_result;
+		goto cleanup;
 	}
 	if (scsi_debug_lbp())
 		map_region(lba, num);
+cleanup:
 	write_unlock_irqrestore(&atomic_rw, iflags);
-	return 0;
+	kfree(arr);
+	return retval;
 }
 
 struct unmap_block_desc {
@@ -4438,6 +4439,7 @@ static ssize_t virtual_gb_store(struct device_driver *ddp, const char *buf,
 			struct sdebug_host_info *sdhp;
 			struct sdebug_dev_info *dp;
 
+			spin_lock(&sdebug_host_list_lock);
 			list_for_each_entry(sdhp, &sdebug_host_list,
 					    host_list) {
 				list_for_each_entry(dp, &sdhp->dev_info_list,
@@ -4446,6 +4448,7 @@ static ssize_t virtual_gb_store(struct device_driver *ddp, const char *buf,
 						dp->uas_bm);
 				}
 			}
+			spin_unlock(&sdebug_host_list_lock);
 		}
 		return count;
 	}
@@ -4988,32 +4991,6 @@ sdebug_change_qdepth(struct scsi_device *sdev, int qdepth)
 }
 
 static int
-sdebug_change_qtype(struct scsi_device *sdev, int qtype)
-{
-	qtype = scsi_change_queue_type(sdev, qtype);
-	if (SCSI_DEBUG_OPT_Q_NOISE & scsi_debug_opts) {
-		const char *cp;
-
-		switch (qtype) {
-		case 0:
-			cp = "untagged";
-			break;
-		case MSG_SIMPLE_TAG:
-			cp = "simple tags";
-			break;
-		case MSG_ORDERED_TAG:
-			cp = "ordered tags";
-			break;
-		default:
-			cp = "unknown";
-			break;
-		}
-		sdev_printk(KERN_INFO, sdev, "%s: to %s\n", __func__, cp);
-	}
-	return qtype;
-}
-
-static int
 check_inject(struct scsi_cmnd *scp)
 {
 	struct sdebug_scmd_extra_t *ep = scsi_cmd_priv(scp);
@@ -5212,7 +5189,6 @@ static struct scsi_host_template sdebug_driver_template = {
 	.ioctl =		scsi_debug_ioctl,
 	.queuecommand =		sdebug_queuecommand_lock_or_not,
 	.change_queue_depth =	sdebug_change_qdepth,
-	.change_queue_type =	sdebug_change_qtype,
 	.eh_abort_handler =	scsi_debug_abort,
 	.eh_device_reset_handler = scsi_debug_device_reset,
 	.eh_target_reset_handler = scsi_debug_target_reset,
diff --git a/drivers/scsi/scsi_devinfo.c b/drivers/scsi/scsi_devinfo.c
index c1d04d4d3c6c..262ab837a704 100644
--- a/drivers/scsi/scsi_devinfo.c
+++ b/drivers/scsi/scsi_devinfo.c
@@ -211,6 +211,7 @@ static struct {
 	{"Medion", "Flash XL  MMC/SD", "2.6D", BLIST_FORCELUN},
 	{"MegaRAID", "LD", NULL, BLIST_FORCELUN},
 	{"MICROP", "4110", NULL, BLIST_NOTQ},
+	{"MSFT", "Virtual HD", NULL, BLIST_NO_RSOC},
 	{"MYLEX", "DACARMRB", "*", BLIST_REPORTLUN2},
 	{"nCipher", "Fastness Crypto", NULL, BLIST_FORCELUN},
 	{"NAKAMICH", "MJ-4.8S", NULL, BLIST_FORCELUN | BLIST_SINGLELUN},
diff --git a/drivers/scsi/scsi_lib.c b/drivers/scsi/scsi_lib.c
index 7e3d954c9cac..9ea95dd3e260 100644
--- a/drivers/scsi/scsi_lib.c
+++ b/drivers/scsi/scsi_lib.c
@@ -1918,7 +1918,9 @@ static int scsi_mq_prep_fn(struct request *req)
 
 	if (scsi_host_get_prot(shost)) {
 		cmd->prot_sdb = (void *)sg +
-			shost->sg_tablesize * sizeof(struct scatterlist);
+			min_t(unsigned int,
+			      shost->sg_tablesize, SCSI_MAX_SG_SEGMENTS) *
+			sizeof(struct scatterlist);
 		memset(cmd->prot_sdb, 0, sizeof(struct scsi_data_buffer));
 
 		cmd->prot_sdb->table.sgl =
@@ -1947,9 +1949,10 @@ static void scsi_mq_done(struct scsi_cmnd *cmd)
 	blk_mq_complete_request(cmd->request);
 }
 
-static int scsi_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *req,
-		bool last)
+static int scsi_queue_rq(struct blk_mq_hw_ctx *hctx,
+			 const struct blk_mq_queue_data *bd)
 {
+	struct request *req = bd->rq;
 	struct request_queue *q = req->q;
 	struct scsi_device *sdev = q->queuedata;
 	struct Scsi_Host *shost = sdev->host;
diff --git a/drivers/scsi/scsi_pm.c b/drivers/scsi/scsi_pm.c
index 7454498c4091..9e43ae1d2163 100644
--- a/drivers/scsi/scsi_pm.c
+++ b/drivers/scsi/scsi_pm.c
@@ -213,8 +213,6 @@ static int scsi_bus_restore(struct device *dev)
 
 #endif /* CONFIG_PM_SLEEP */
 
-#ifdef CONFIG_PM_RUNTIME
-
 static int sdev_runtime_suspend(struct device *dev)
 {
 	const struct dev_pm_ops *pm = dev->driver ? dev->driver->pm : NULL;
@@ -332,14 +330,6 @@ void scsi_autopm_put_host(struct Scsi_Host *shost)
 	pm_runtime_put_sync(&shost->shost_gendev);
 }
 
-#else
-
-#define scsi_runtime_suspend	NULL
-#define scsi_runtime_resume	NULL
-#define scsi_runtime_idle	NULL
-
-#endif /* CONFIG_PM_RUNTIME */
-
 const struct dev_pm_ops scsi_bus_pm_ops = {
 	.prepare =		scsi_bus_prepare,
 	.suspend =		scsi_bus_suspend,
diff --git a/drivers/scsi/scsi_priv.h b/drivers/scsi/scsi_priv.h
index 2dc4a83fb84c..e3902fc66278 100644
--- a/drivers/scsi/scsi_priv.h
+++ b/drivers/scsi/scsi_priv.h
@@ -155,8 +155,7 @@ static inline void scsi_netlink_exit(void) {}
 /* scsi_pm.c */
 #ifdef CONFIG_PM
 extern const struct dev_pm_ops scsi_bus_pm_ops;
-#endif
-#ifdef CONFIG_PM_RUNTIME
+
 extern void scsi_autopm_get_target(struct scsi_target *);
 extern void scsi_autopm_put_target(struct scsi_target *);
 extern int scsi_autopm_get_host(struct Scsi_Host *);
@@ -166,7 +165,7 @@ static inline void scsi_autopm_get_target(struct scsi_target *t) {}
 static inline void scsi_autopm_put_target(struct scsi_target *t) {}
 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 */
+#endif /* CONFIG_PM */
 
 extern struct async_domain scsi_sd_pm_domain;
 extern struct async_domain scsi_sd_probe_domain;
diff --git a/drivers/scsi/scsi_sysfs.c b/drivers/scsi/scsi_sysfs.c
index 1cb64a8e18c9..1ac38e73df7e 100644
--- a/drivers/scsi/scsi_sysfs.c
+++ b/drivers/scsi/scsi_sysfs.c
@@ -738,30 +738,12 @@ store_queue_type_field(struct device *dev, struct device_attribute *attr,
 		       const char *buf, size_t count)
 {
 	struct scsi_device *sdev = to_scsi_device(dev);
-	struct scsi_host_template *sht = sdev->host->hostt;
-	int tag_type = 0, retval;
-	int prev_tag_type = scsi_get_tag_type(sdev);
-
-	if (!sdev->tagged_supported || !sht->change_queue_type)
-		return -EINVAL;
 
-	/*
-	 * We're never issueing order tags these days, but allow the value
-	 * for backwards compatibility.
-	 */
-	if (strncmp(buf, "ordered", 7) == 0 ||
-	    strncmp(buf, "simple", 6) == 0)
-		tag_type = MSG_SIMPLE_TAG;
-	else if (strncmp(buf, "none", 4) != 0)
+	if (!sdev->tagged_supported)
 		return -EINVAL;
-
-	if (tag_type == prev_tag_type)
-		return count;
-
-	retval = sht->change_queue_type(sdev, tag_type);
-	if (retval < 0)
-		return retval;
-
+		
+	sdev_printk(KERN_INFO, sdev,
+		    "ignoring write to deprecated queue_type attribute");
 	return count;
 }
 
@@ -938,10 +920,6 @@ static umode_t scsi_sdev_attr_is_visible(struct kobject *kobj,
 	    !sdev->host->hostt->change_queue_depth)
 		return 0;
 
-	if (attr == &dev_attr_queue_type.attr &&
-	    !sdev->host->hostt->change_queue_type)
-		return S_IRUGO;
-
 	return attr->mode;
 }
 
diff --git a/drivers/scsi/scsi_transport_spi.c b/drivers/scsi/scsi_transport_spi.c
index fa2aece76cc2..31bbb0da3397 100644
--- a/drivers/scsi/scsi_transport_spi.c
+++ b/drivers/scsi/scsi_transport_spi.c
@@ -1221,7 +1221,7 @@ EXPORT_SYMBOL_GPL(spi_populate_ppr_msg);
 int spi_populate_tag_msg(unsigned char *msg, struct scsi_cmnd *cmd)
 {
         if (cmd->flags & SCMD_TAGGED) {
-		*msg++ = MSG_SIMPLE_TAG;
+		*msg++ = SIMPLE_QUEUE_TAG;
         	*msg++ = cmd->request->tag;
         	return 2;
 	}
diff --git a/drivers/scsi/sgiwd93.c b/drivers/scsi/sgiwd93.c
index a464d959f66e..6d215e2fb46d 100644
--- a/drivers/scsi/sgiwd93.c
+++ b/drivers/scsi/sgiwd93.c
@@ -315,7 +315,6 @@ static struct platform_driver sgiwd93_driver = {
 	.remove = sgiwd93_remove,
 	.driver = {
 		.name   = "sgiwd93",
-		.owner	= THIS_MODULE,
 	}
 };
 
diff --git a/drivers/scsi/sni_53c710.c b/drivers/scsi/sni_53c710.c
index 52d54e7425db..76278072147e 100644
--- a/drivers/scsi/sni_53c710.c
+++ b/drivers/scsi/sni_53c710.c
@@ -137,7 +137,6 @@ static struct platform_driver snirm710_driver = {
 	.remove	= snirm710_driver_remove,
 	.driver	= {
 		.name	= "snirm_53c710",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/scsi/storvsc_drv.c b/drivers/scsi/storvsc_drv.c
index e3ba251fb6e7..4cff0ddc2c25 100644
--- a/drivers/scsi/storvsc_drv.c
+++ b/drivers/scsi/storvsc_drv.c
@@ -1688,13 +1688,12 @@ static int storvsc_queuecommand(struct Scsi_Host *host, struct scsi_cmnd *scmnd)
 	if (ret == -EAGAIN) {
 		/* no more space */
 
-		if (cmd_request->bounce_sgl_count) {
+		if (cmd_request->bounce_sgl_count)
 			destroy_bounce_buffer(cmd_request->bounce_sgl,
 					cmd_request->bounce_sgl_count);
 
-			ret = SCSI_MLQUEUE_DEVICE_BUSY;
-			goto queue_error;
-		}
+		ret = SCSI_MLQUEUE_DEVICE_BUSY;
+		goto queue_error;
 	}
 
 	return 0;
diff --git a/drivers/scsi/sun3x_esp.c b/drivers/scsi/sun3x_esp.c
index 534eb96fc3a7..e26e81de7c45 100644
--- a/drivers/scsi/sun3x_esp.c
+++ b/drivers/scsi/sun3x_esp.c
@@ -295,7 +295,6 @@ static struct platform_driver esp_sun3x_driver = {
 	.remove         = esp_sun3x_remove,
 	.driver = {
 		.name   = "sun3x_esp",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/scsi/sun_esp.c b/drivers/scsi/sun_esp.c
index f2e68459f7ea..7b6d4c2087d7 100644
--- a/drivers/scsi/sun_esp.c
+++ b/drivers/scsi/sun_esp.c
@@ -633,7 +633,6 @@ MODULE_DEVICE_TABLE(of, esp_match);
 static struct platform_driver esp_sbus_driver = {
 	.driver = {
 		.name = "esp",
-		.owner = THIS_MODULE,
 		.of_match_table = esp_match,
 	},
 	.probe		= esp_sbus_probe,
diff --git a/drivers/scsi/ufs/ufshcd-pci.c b/drivers/scsi/ufs/ufshcd-pci.c
index 955ed5587011..d15eaa466c59 100644
--- a/drivers/scsi/ufs/ufshcd-pci.c
+++ b/drivers/scsi/ufs/ufshcd-pci.c
@@ -62,12 +62,7 @@ static int ufshcd_pci_resume(struct device *dev)
 {
 	return ufshcd_system_resume(dev_get_drvdata(dev));
 }
-#else
-#define ufshcd_pci_suspend	NULL
-#define ufshcd_pci_resume	NULL
-#endif /* CONFIG_PM */
 
-#ifdef CONFIG_PM_RUNTIME
 static int ufshcd_pci_runtime_suspend(struct device *dev)
 {
 	return ufshcd_runtime_suspend(dev_get_drvdata(dev));
@@ -80,11 +75,13 @@ static int ufshcd_pci_runtime_idle(struct device *dev)
 {
 	return ufshcd_runtime_idle(dev_get_drvdata(dev));
 }
-#else /* !CONFIG_PM_RUNTIME */
+#else /* !CONFIG_PM */
+#define ufshcd_pci_suspend	NULL
+#define ufshcd_pci_resume	NULL
 #define ufshcd_pci_runtime_suspend	NULL
 #define ufshcd_pci_runtime_resume	NULL
 #define ufshcd_pci_runtime_idle	NULL
-#endif /* CONFIG_PM_RUNTIME */
+#endif /* CONFIG_PM */
 
 /**
  * ufshcd_pci_shutdown - main function to put the controller in reset state
diff --git a/drivers/scsi/ufs/ufshcd-pltfrm.c b/drivers/scsi/ufs/ufshcd-pltfrm.c
index 1c3467b82566..7db9564f507d 100644
--- a/drivers/scsi/ufs/ufshcd-pltfrm.c
+++ b/drivers/scsi/ufs/ufshcd-pltfrm.c
@@ -261,12 +261,7 @@ static int ufshcd_pltfrm_resume(struct device *dev)
 {
 	return ufshcd_system_resume(dev_get_drvdata(dev));
 }
-#else
-#define ufshcd_pltfrm_suspend	NULL
-#define ufshcd_pltfrm_resume	NULL
-#endif
 
-#ifdef CONFIG_PM_RUNTIME
 static int ufshcd_pltfrm_runtime_suspend(struct device *dev)
 {
 	return ufshcd_runtime_suspend(dev_get_drvdata(dev));
@@ -279,11 +274,13 @@ static int ufshcd_pltfrm_runtime_idle(struct device *dev)
 {
 	return ufshcd_runtime_idle(dev_get_drvdata(dev));
 }
-#else /* !CONFIG_PM_RUNTIME */
+#else /* !CONFIG_PM */
+#define ufshcd_pltfrm_suspend	NULL
+#define ufshcd_pltfrm_resume	NULL
 #define ufshcd_pltfrm_runtime_suspend	NULL
 #define ufshcd_pltfrm_runtime_resume	NULL
 #define ufshcd_pltfrm_runtime_idle	NULL
-#endif /* CONFIG_PM_RUNTIME */
+#endif /* CONFIG_PM */
 
 static void ufshcd_pltfrm_shutdown(struct platform_device *pdev)
 {
@@ -393,7 +390,6 @@ static struct platform_driver ufshcd_pltfrm_driver = {
 	.shutdown = ufshcd_pltfrm_shutdown,
 	.driver	= {
 		.name	= "ufshcd",
-		.owner	= THIS_MODULE,
 		.pm	= &ufshcd_dev_pm_ops,
 		.of_match_table = ufs_of_match,
 	},
diff --git a/drivers/soc/qcom/qcom_gsbi.c b/drivers/soc/qcom/qcom_gsbi.c
index 7e1f120f2b32..729425ddfd3e 100644
--- a/drivers/soc/qcom/qcom_gsbi.c
+++ b/drivers/soc/qcom/qcom_gsbi.c
@@ -91,7 +91,6 @@ MODULE_DEVICE_TABLE(of, gsbi_dt_match);
 static struct platform_driver gsbi_driver = {
 	.driver = {
 		.name		= "gsbi",
-		.owner		= THIS_MODULE,
 		.of_match_table	= gsbi_dt_match,
 	},
 	.probe = gsbi_probe,
diff --git a/drivers/soc/tegra/fuse/fuse-tegra.c b/drivers/soc/tegra/fuse/fuse-tegra.c
index 11a5043959dc..011a3363c265 100644
--- a/drivers/soc/tegra/fuse/fuse-tegra.c
+++ b/drivers/soc/tegra/fuse/fuse-tegra.c
@@ -31,6 +31,7 @@
 static u32 (*fuse_readl)(const unsigned int offset);
 static int fuse_size;
 struct tegra_sku_info tegra_sku_info;
+EXPORT_SYMBOL(tegra_sku_info);
 
 static const char *tegra_revision_name[TEGRA_REVISION_MAX] = {
 	[TEGRA_REVISION_UNKNOWN] = "unknown",
diff --git a/drivers/soc/tegra/fuse/fuse-tegra20.c b/drivers/soc/tegra/fuse/fuse-tegra20.c
index 7cb63ab6aac2..5eff6f097f98 100644
--- a/drivers/soc/tegra/fuse/fuse-tegra20.c
+++ b/drivers/soc/tegra/fuse/fuse-tegra20.c
@@ -158,7 +158,6 @@ static struct platform_driver tegra20_fuse_driver = {
 	.probe = tegra20_fuse_probe,
 	.driver = {
 		.name = "tegra20_fuse",
-		.owner = THIS_MODULE,
 		.of_match_table = tegra20_fuse_of_match,
 	}
 };
diff --git a/drivers/soc/tegra/fuse/fuse-tegra30.c b/drivers/soc/tegra/fuse/fuse-tegra30.c
index 5999cf34ab70..8646fa920d8d 100644
--- a/drivers/soc/tegra/fuse/fuse-tegra30.c
+++ b/drivers/soc/tegra/fuse/fuse-tegra30.c
@@ -130,7 +130,6 @@ static struct platform_driver tegra30_fuse_driver = {
 	.probe = tegra30_fuse_probe,
 	.driver = {
 		.name = "tegra_fuse",
-		.owner = THIS_MODULE,
 		.of_match_table = tegra30_fuse_of_match,
 	}
 };
diff --git a/drivers/soc/ti/knav_dma.c b/drivers/soc/ti/knav_dma.c
index 17264275f32b..bc1b80ec6afe 100644
--- a/drivers/soc/ti/knav_dma.c
+++ b/drivers/soc/ti/knav_dma.c
@@ -803,7 +803,6 @@ static struct platform_driver knav_dma_driver = {
 	.remove	= knav_dma_remove,
 	.driver = {
 		.name		= "keystone-navigator-dma",
-		.owner		= THIS_MODULE,
 		.of_match_table	= of_match,
 	},
 };
diff --git a/drivers/soc/ti/knav_qmss_queue.c b/drivers/soc/ti/knav_qmss_queue.c
index 9b8dd6732681..8e6a95d91d33 100644
--- a/drivers/soc/ti/knav_qmss_queue.c
+++ b/drivers/soc/ti/knav_qmss_queue.c
@@ -1805,7 +1805,6 @@ static struct platform_driver keystone_qmss_driver = {
 	.remove		= knav_queue_remove,
 	.driver		= {
 		.name	= "keystone-navigator-qmss",
-		.owner	= THIS_MODULE,
 		.of_match_table = keystone_qmss_of_match,
 	},
 };
diff --git a/drivers/spi/spi-adi-v3.c b/drivers/spi/spi-adi-v3.c
index 19ea8fb78cc7..a16b25dcd1e6 100644
--- a/drivers/spi/spi-adi-v3.c
+++ b/drivers/spi/spi-adi-v3.c
@@ -972,7 +972,6 @@ MODULE_ALIAS("platform:adi-spi3");
 static struct platform_driver adi_spi_driver = {
 	.driver	= {
 		.name	= "adi-spi3",
-		.owner	= THIS_MODULE,
 		.pm     = &adi_spi_pm_ops,
 	},
 	.remove		= adi_spi_remove,
diff --git a/drivers/spi/spi-altera.c b/drivers/spi/spi-altera.c
index 5b5709a5c957..b95010e72452 100644
--- a/drivers/spi/spi-altera.c
+++ b/drivers/spi/spi-altera.c
@@ -282,7 +282,6 @@ static struct platform_driver altera_spi_driver = {
 	.remove = altera_spi_remove,
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 		.pm = NULL,
 		.of_match_table = of_match_ptr(altera_spi_match),
 	},
diff --git a/drivers/spi/spi-ath79.c b/drivers/spi/spi-ath79.c
index 058db0fe8dc7..b02eb4ac0218 100644
--- a/drivers/spi/spi-ath79.c
+++ b/drivers/spi/spi-ath79.c
@@ -307,7 +307,6 @@ static struct platform_driver ath79_spi_driver = {
 	.shutdown	= ath79_spi_shutdown,
 	.driver		= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(ath79_spi_driver);
diff --git a/drivers/spi/spi-atmel.c b/drivers/spi/spi-atmel.c
index e4193ccc4970..23d8f5f56579 100644
--- a/drivers/spi/spi-atmel.c
+++ b/drivers/spi/spi-atmel.c
@@ -1522,7 +1522,6 @@ MODULE_DEVICE_TABLE(of, atmel_spi_dt_ids);
 static struct platform_driver atmel_spi_driver = {
 	.driver		= {
 		.name	= "atmel_spi",
-		.owner	= THIS_MODULE,
 		.pm	= ATMEL_SPI_PM_OPS,
 		.of_match_table	= of_match_ptr(atmel_spi_dt_ids),
 	},
diff --git a/drivers/spi/spi-au1550.c b/drivers/spi/spi-au1550.c
index f40b34cdf2fc..326f47973684 100644
--- a/drivers/spi/spi-au1550.c
+++ b/drivers/spi/spi-au1550.c
@@ -965,7 +965,6 @@ static struct platform_driver au1550_spi_drv = {
 	.remove = au1550_spi_remove,
 	.driver = {
 		.name = "au1550-spi",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/spi/spi-bcm2835.c b/drivers/spi/spi-bcm2835.c
index 69167456ec1e..98aab457b24d 100644
--- a/drivers/spi/spi-bcm2835.c
+++ b/drivers/spi/spi-bcm2835.c
@@ -395,7 +395,6 @@ MODULE_DEVICE_TABLE(of, bcm2835_spi_match);
 static struct platform_driver bcm2835_spi_driver = {
 	.driver		= {
 		.name		= DRV_NAME,
-		.owner		= THIS_MODULE,
 		.of_match_table	= bcm2835_spi_match,
 	},
 	.probe		= bcm2835_spi_probe,
diff --git a/drivers/spi/spi-bcm63xx-hsspi.c b/drivers/spi/spi-bcm63xx-hsspi.c
index 86f5a98aa7a2..f5ca6dc3a157 100644
--- a/drivers/spi/spi-bcm63xx-hsspi.c
+++ b/drivers/spi/spi-bcm63xx-hsspi.c
@@ -459,7 +459,6 @@ static SIMPLE_DEV_PM_OPS(bcm63xx_hsspi_pm_ops, bcm63xx_hsspi_suspend,
 static struct platform_driver bcm63xx_hsspi_driver = {
 	.driver = {
 		.name	= "bcm63xx-hsspi",
-		.owner	= THIS_MODULE,
 		.pm	= &bcm63xx_hsspi_pm_ops,
 	},
 	.probe		= bcm63xx_hsspi_probe,
diff --git a/drivers/spi/spi-bcm63xx.c b/drivers/spi/spi-bcm63xx.c
index 8510400e7867..c20530982e26 100644
--- a/drivers/spi/spi-bcm63xx.c
+++ b/drivers/spi/spi-bcm63xx.c
@@ -469,7 +469,6 @@ static const struct dev_pm_ops bcm63xx_spi_pm_ops = {
 static struct platform_driver bcm63xx_spi_driver = {
 	.driver = {
 		.name	= "bcm63xx-spi",
-		.owner	= THIS_MODULE,
 		.pm	= &bcm63xx_spi_pm_ops,
 	},
 	.probe		= bcm63xx_spi_probe,
diff --git a/drivers/spi/spi-bfin-sport.c b/drivers/spi/spi-bfin-sport.c
index f515c5e9db57..a78693189f45 100644
--- a/drivers/spi/spi-bfin-sport.c
+++ b/drivers/spi/spi-bfin-sport.c
@@ -921,7 +921,6 @@ static SIMPLE_DEV_PM_OPS(bfin_sport_spi_pm_ops, bfin_sport_spi_suspend,
 static struct platform_driver bfin_sport_spi_driver = {
 	.driver	= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 		.pm	= BFIN_SPORT_SPI_PM_OPS,
 	},
 	.probe   = bfin_sport_spi_probe,
diff --git a/drivers/spi/spi-bfin5xx.c b/drivers/spi/spi-bfin5xx.c
index ebf720b88a2a..37079937d2f7 100644
--- a/drivers/spi/spi-bfin5xx.c
+++ b/drivers/spi/spi-bfin5xx.c
@@ -1455,7 +1455,6 @@ MODULE_ALIAS("platform:bfin-spi");
 static struct platform_driver bfin_spi_driver = {
 	.driver	= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 		.pm	= BFIN_SPI_PM_OPS,
 	},
 	.probe		= bfin_spi_probe,
diff --git a/drivers/spi/spi-clps711x.c b/drivers/spi/spi-clps711x.c
index 181cf2262006..8c30de0315e7 100644
--- a/drivers/spi/spi-clps711x.c
+++ b/drivers/spi/spi-clps711x.c
@@ -203,7 +203,6 @@ err_out:
 static struct platform_driver clps711x_spi_driver = {
 	.driver	= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe	= spi_clps711x_probe,
 };
diff --git a/drivers/spi/spi-coldfire-qspi.c b/drivers/spi/spi-coldfire-qspi.c
index e2fa628e55e7..41b5dc4445f6 100644
--- a/drivers/spi/spi-coldfire-qspi.c
+++ b/drivers/spi/spi-coldfire-qspi.c
@@ -491,7 +491,7 @@ static int mcfqspi_resume(struct device *dev)
 }
 #endif
 
-#ifdef CONFIG_PM_RUNTIME
+#ifdef CONFIG_PM
 static int mcfqspi_runtime_suspend(struct device *dev)
 {
 	struct spi_master *master = dev_get_drvdata(dev);
diff --git a/drivers/spi/spi-davinci.c b/drivers/spi/spi-davinci.c
index 63700ab7bd9f..b3707badb1e5 100644
--- a/drivers/spi/spi-davinci.c
+++ b/drivers/spi/spi-davinci.c
@@ -1119,7 +1119,6 @@ static int davinci_spi_remove(struct platform_device *pdev)
 static struct platform_driver davinci_spi_driver = {
 	.driver = {
 		.name = "spi_davinci",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(davinci_spi_of_match),
 	},
 	.probe = davinci_spi_probe,
diff --git a/drivers/spi/spi-dw-mmio.c b/drivers/spi/spi-dw-mmio.c
index 21ce0e36fa00..eb03e1215195 100644
--- a/drivers/spi/spi-dw-mmio.c
+++ b/drivers/spi/spi-dw-mmio.c
@@ -135,7 +135,6 @@ static struct platform_driver dw_spi_mmio_driver = {
 	.remove		= dw_spi_mmio_remove,
 	.driver		= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = dw_spi_mmio_of_match,
 	},
 };
diff --git a/drivers/spi/spi-efm32.c b/drivers/spi/spi-efm32.c
index 6caeb1cac0f3..065fe8744989 100644
--- a/drivers/spi/spi-efm32.c
+++ b/drivers/spi/spi-efm32.c
@@ -494,7 +494,6 @@ static struct platform_driver efm32_spi_driver = {
 
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = efm32_spi_dt_ids,
 	},
 };
diff --git a/drivers/spi/spi-ep93xx.c b/drivers/spi/spi-ep93xx.c
index bf9728773247..bb00be8d1851 100644
--- a/drivers/spi/spi-ep93xx.c
+++ b/drivers/spi/spi-ep93xx.c
@@ -964,7 +964,6 @@ static int ep93xx_spi_remove(struct platform_device *pdev)
 static struct platform_driver ep93xx_spi_driver = {
 	.driver		= {
 		.name	= "ep93xx-spi",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= ep93xx_spi_probe,
 	.remove		= ep93xx_spi_remove,
diff --git a/drivers/spi/spi-falcon.c b/drivers/spi/spi-falcon.c
index f73b3004d6d3..912b9037e9cf 100644
--- a/drivers/spi/spi-falcon.c
+++ b/drivers/spi/spi-falcon.c
@@ -441,7 +441,6 @@ static struct platform_driver falcon_sflash_driver = {
 	.probe	= falcon_sflash_probe,
 	.driver = {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = falcon_sflash_match,
 	}
 };
diff --git a/drivers/spi/spi-fsl-espi.c b/drivers/spi/spi-fsl-espi.c
index 56cadf13519e..d0a73a09a9bd 100644
--- a/drivers/spi/spi-fsl-espi.c
+++ b/drivers/spi/spi-fsl-espi.c
@@ -850,7 +850,6 @@ MODULE_DEVICE_TABLE(of, of_fsl_espi_match);
 static struct platform_driver fsl_espi_driver = {
 	.driver = {
 		.name = "fsl_espi",
-		.owner = THIS_MODULE,
 		.of_match_table = of_fsl_espi_match,
 		.pm = &espi_pm,
 	},
diff --git a/drivers/spi/spi-fsl-spi.c b/drivers/spi/spi-fsl-spi.c
index 3b159364c459..60c590790854 100644
--- a/drivers/spi/spi-fsl-spi.c
+++ b/drivers/spi/spi-fsl-spi.c
@@ -879,7 +879,6 @@ static int of_fsl_spi_remove(struct platform_device *ofdev)
 static struct platform_driver of_fsl_spi_driver = {
 	.driver = {
 		.name = "fsl_spi",
-		.owner = THIS_MODULE,
 		.of_match_table = of_fsl_spi_match,
 	},
 	.probe		= of_fsl_spi_probe,
@@ -926,7 +925,6 @@ static struct platform_driver mpc8xxx_spi_driver = {
 	.remove = plat_mpc8xxx_spi_remove,
 	.driver = {
 		.name = "mpc8xxx_spi",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/spi/spi-gpio.c b/drivers/spi/spi-gpio.c
index 4b600d4f8548..aee4e7589568 100644
--- a/drivers/spi/spi-gpio.c
+++ b/drivers/spi/spi-gpio.c
@@ -539,7 +539,6 @@ MODULE_ALIAS("platform:" DRIVER_NAME);
 static struct platform_driver spi_gpio_driver = {
 	.driver = {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(spi_gpio_dt_ids),
 	},
 	.probe		= spi_gpio_probe,
diff --git a/drivers/spi/spi-img-spfi.c b/drivers/spi/spi-img-spfi.c
index 43781c9fe521..aad6683db81b 100644
--- a/drivers/spi/spi-img-spfi.c
+++ b/drivers/spi/spi-img-spfi.c
@@ -341,7 +341,7 @@ static int img_spfi_start_dma(struct spi_master *master,
 		default:
 			rxconf.src_addr = spfi->phys + SPFI_RX_8BIT_VALID_DATA;
 			rxconf.src_addr_width = 1;
-			rxconf.src_maxburst = 1;
+			rxconf.src_maxburst = 4;
 		}
 		dmaengine_slave_config(spfi->rx_ch, &rxconf);
 
@@ -368,7 +368,7 @@ static int img_spfi_start_dma(struct spi_master *master,
 		default:
 			txconf.dst_addr = spfi->phys + SPFI_TX_8BIT_VALID_DATA;
 			txconf.dst_addr_width = 1;
-			txconf.dst_maxburst = 1;
+			txconf.dst_maxburst = 4;
 			break;
 		}
 		dmaengine_slave_config(spfi->tx_ch, &txconf);
@@ -390,14 +390,14 @@ static int img_spfi_start_dma(struct spi_master *master,
 		dma_async_issue_pending(spfi->rx_ch);
 	}
 
+	spfi_start(spfi);
+
 	if (xfer->tx_buf) {
 		spfi->tx_dma_busy = true;
 		dmaengine_submit(txdesc);
 		dma_async_issue_pending(spfi->tx_ch);
 	}
 
-	spfi_start(spfi);
-
 	return 1;
 
 stop_dma:
@@ -663,7 +663,7 @@ static int img_spfi_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_RUNTIME
+#ifdef CONFIG_PM
 static int img_spfi_runtime_suspend(struct device *dev)
 {
 	struct spi_master *master = dev_get_drvdata(dev);
@@ -692,7 +692,7 @@ static int img_spfi_runtime_resume(struct device *dev)
 
 	return 0;
 }
-#endif /* CONFIG_PM_RUNTIME */
+#endif /* CONFIG_PM */
 
 #ifdef CONFIG_PM_SLEEP
 static int img_spfi_suspend(struct device *dev)
diff --git a/drivers/spi/spi-imx.c b/drivers/spi/spi-imx.c
index 3637847b5370..961b97d43b43 100644
--- a/drivers/spi/spi-imx.c
+++ b/drivers/spi/spi-imx.c
@@ -1231,7 +1231,6 @@ static int spi_imx_remove(struct platform_device *pdev)
 static struct platform_driver spi_imx_driver = {
 	.driver = {
 		   .name = DRIVER_NAME,
-		   .owner = THIS_MODULE,
 		   .of_match_table = spi_imx_dt_ids,
 		   },
 	.id_table = spi_imx_devtype,
diff --git a/drivers/spi/spi-meson-spifc.c b/drivers/spi/spi-meson-spifc.c
index 0e48f8c2037d..1bbac0378bf7 100644
--- a/drivers/spi/spi-meson-spifc.c
+++ b/drivers/spi/spi-meson-spifc.c
@@ -413,7 +413,7 @@ static int meson_spifc_resume(struct device *dev)
 }
 #endif /* CONFIG_PM_SLEEP */
 
-#ifdef CONFIG_PM_RUNTIME
+#ifdef CONFIG_PM
 static int meson_spifc_runtime_suspend(struct device *dev)
 {
 	struct spi_master *master = dev_get_drvdata(dev);
@@ -431,7 +431,7 @@ static int meson_spifc_runtime_resume(struct device *dev)
 
 	return clk_prepare_enable(spifc->clk);
 }
-#endif /* CONFIG_PM_RUNTIME */
+#endif /* CONFIG_PM */
 
 static const struct dev_pm_ops meson_spifc_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(meson_spifc_suspend, meson_spifc_resume)
diff --git a/drivers/spi/spi-mpc512x-psc.c b/drivers/spi/spi-mpc512x-psc.c
index 577d23a12763..ecae0d4e2945 100644
--- a/drivers/spi/spi-mpc512x-psc.c
+++ b/drivers/spi/spi-mpc512x-psc.c
@@ -600,7 +600,6 @@ static struct platform_driver mpc512x_psc_spi_of_driver = {
 	.remove = mpc512x_psc_spi_of_remove,
 	.driver = {
 		.name = "mpc512x-psc-spi",
-		.owner = THIS_MODULE,
 		.of_match_table = mpc512x_psc_spi_of_match,
 	},
 };
diff --git a/drivers/spi/spi-mpc52xx-psc.c b/drivers/spi/spi-mpc52xx-psc.c
index de532aa11d34..72d11ebefb28 100644
--- a/drivers/spi/spi-mpc52xx-psc.c
+++ b/drivers/spi/spi-mpc52xx-psc.c
@@ -508,7 +508,6 @@ static struct platform_driver mpc52xx_psc_spi_of_driver = {
 	.remove = mpc52xx_psc_spi_of_remove,
 	.driver = {
 		.name = "mpc52xx-psc-spi",
-		.owner = THIS_MODULE,
 		.of_match_table = mpc52xx_psc_spi_of_match,
 	},
 };
diff --git a/drivers/spi/spi-mpc52xx.c b/drivers/spi/spi-mpc52xx.c
index b07db4b62d80..c36002110c30 100644
--- a/drivers/spi/spi-mpc52xx.c
+++ b/drivers/spi/spi-mpc52xx.c
@@ -543,7 +543,6 @@ MODULE_DEVICE_TABLE(of, mpc52xx_spi_match);
 static struct platform_driver mpc52xx_spi_of_driver = {
 	.driver = {
 		.name = "mpc52xx-spi",
-		.owner = THIS_MODULE,
 		.of_match_table = mpc52xx_spi_match,
 	},
 	.probe = mpc52xx_spi_probe,
diff --git a/drivers/spi/spi-mxs.c b/drivers/spi/spi-mxs.c
index 06a11546a1a7..4045a1e580e1 100644
--- a/drivers/spi/spi-mxs.c
+++ b/drivers/spi/spi-mxs.c
@@ -572,7 +572,6 @@ static struct platform_driver mxs_spi_driver = {
 	.remove	= mxs_spi_remove,
 	.driver	= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = mxs_spi_dt_ids,
 	},
 };
diff --git a/drivers/spi/spi-nuc900.c b/drivers/spi/spi-nuc900.c
index 73e91d5a43df..f51a058e7678 100644
--- a/drivers/spi/spi-nuc900.c
+++ b/drivers/spi/spi-nuc900.c
@@ -423,7 +423,6 @@ static struct platform_driver nuc900_spi_driver = {
 	.remove		= nuc900_spi_remove,
 	.driver		= {
 		.name	= "nuc900-spi",
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(nuc900_spi_driver);
diff --git a/drivers/spi/spi-oc-tiny.c b/drivers/spi/spi-oc-tiny.c
index 8998d11c7238..76656a77ec12 100644
--- a/drivers/spi/spi-oc-tiny.c
+++ b/drivers/spi/spi-oc-tiny.c
@@ -351,7 +351,6 @@ static struct platform_driver tiny_spi_driver = {
 	.remove = tiny_spi_remove,
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 		.pm = NULL,
 		.of_match_table = of_match_ptr(tiny_spi_match),
 	},
diff --git a/drivers/spi/spi-octeon.c b/drivers/spi/spi-octeon.c
index c5e2f718eebd..b283d537d16a 100644
--- a/drivers/spi/spi-octeon.c
+++ b/drivers/spi/spi-octeon.c
@@ -247,7 +247,6 @@ MODULE_DEVICE_TABLE(of, octeon_spi_match);
 static struct platform_driver octeon_spi_driver = {
 	.driver = {
 		.name		= "spi-octeon",
-		.owner		= THIS_MODULE,
 		.of_match_table = octeon_spi_match,
 	},
 	.probe		= octeon_spi_probe,
diff --git a/drivers/spi/spi-omap-100k.c b/drivers/spi/spi-omap-100k.c
index fb522765ce5a..79399ae9c84c 100644
--- a/drivers/spi/spi-omap-100k.c
+++ b/drivers/spi/spi-omap-100k.c
@@ -453,7 +453,6 @@ err:
 static struct platform_driver omap1_spi100k_driver = {
 	.driver = {
 		.name		= "omap1_spi100k",
-		.owner		= THIS_MODULE,
 	},
 	.probe		= omap1_spi100k_probe,
 };
diff --git a/drivers/spi/spi-omap-uwire.c b/drivers/spi/spi-omap-uwire.c
index 8bca90a19dd1..daf1ada5cd11 100644
--- a/drivers/spi/spi-omap-uwire.c
+++ b/drivers/spi/spi-omap-uwire.c
@@ -527,7 +527,6 @@ MODULE_ALIAS("platform:omap_uwire");
 static struct platform_driver uwire_driver = {
 	.driver = {
 		.name		= "omap_uwire",
-		.owner		= THIS_MODULE,
 	},
 	.probe = uwire_probe,
 	.remove = uwire_remove,
diff --git a/drivers/spi/spi-omap2-mcspi.c b/drivers/spi/spi-omap2-mcspi.c
index 352eed7463ac..3bc3cbabbbc0 100644
--- a/drivers/spi/spi-omap2-mcspi.c
+++ b/drivers/spi/spi-omap2-mcspi.c
@@ -1521,7 +1521,6 @@ static const struct dev_pm_ops omap2_mcspi_pm_ops = {
 static struct platform_driver omap2_mcspi_driver = {
 	.driver = {
 		.name =		"omap2_mcspi",
-		.owner =	THIS_MODULE,
 		.pm =		&omap2_mcspi_pm_ops,
 		.of_match_table = omap_mcspi_of_match,
 	},
diff --git a/drivers/spi/spi-orion.c b/drivers/spi/spi-orion.c
index c76b7d7879df..3dec9e0b99b8 100644
--- a/drivers/spi/spi-orion.c
+++ b/drivers/spi/spi-orion.c
@@ -523,7 +523,7 @@ static int orion_spi_remove(struct platform_device *pdev)
 
 MODULE_ALIAS("platform:" DRIVER_NAME);
 
-#ifdef CONFIG_PM_RUNTIME
+#ifdef CONFIG_PM
 static int orion_spi_runtime_suspend(struct device *dev)
 {
 	struct spi_master *master = dev_get_drvdata(dev);
@@ -551,7 +551,6 @@ static const struct dev_pm_ops orion_spi_pm_ops = {
 static struct platform_driver orion_spi_driver = {
 	.driver = {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 		.pm	= &orion_spi_pm_ops,
 		.of_match_table = of_match_ptr(orion_spi_of_match_table),
 	},
diff --git a/drivers/spi/spi-ppc4xx.c b/drivers/spi/spi-ppc4xx.c
index 80b8408ac3e3..54fb984a3e17 100644
--- a/drivers/spi/spi-ppc4xx.c
+++ b/drivers/spi/spi-ppc4xx.c
@@ -575,7 +575,6 @@ static struct platform_driver spi_ppc4xx_of_driver = {
 	.remove = spi_ppc4xx_of_remove,
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = spi_ppc4xx_of_match,
 	},
 };
diff --git a/drivers/spi/spi-pxa2xx.c b/drivers/spi/spi-pxa2xx.c
index 1a1df5092aca..05c623cfb078 100644
--- a/drivers/spi/spi-pxa2xx.c
+++ b/drivers/spi/spi-pxa2xx.c
@@ -1531,7 +1531,7 @@ static int pxa2xx_spi_resume(struct device *dev)
 }
 #endif
 
-#ifdef CONFIG_PM_RUNTIME
+#ifdef CONFIG_PM
 static int pxa2xx_spi_runtime_suspend(struct device *dev)
 {
 	struct driver_data *drv_data = dev_get_drvdata(dev);
@@ -1558,7 +1558,6 @@ static const struct dev_pm_ops pxa2xx_spi_pm_ops = {
 static struct platform_driver driver = {
 	.driver = {
 		.name	= "pxa2xx-spi",
-		.owner	= THIS_MODULE,
 		.pm	= &pxa2xx_spi_pm_ops,
 		.acpi_match_table = ACPI_PTR(pxa2xx_spi_acpi_match),
 	},
diff --git a/drivers/spi/spi-qup.c b/drivers/spi/spi-qup.c
index 9f83d2950748..e7fb5a0d2e8d 100644
--- a/drivers/spi/spi-qup.c
+++ b/drivers/spi/spi-qup.c
@@ -646,7 +646,7 @@ error:
 	return ret;
 }
 
-#ifdef CONFIG_PM_RUNTIME
+#ifdef CONFIG_PM
 static int spi_qup_pm_suspend_runtime(struct device *device)
 {
 	struct spi_master *master = dev_get_drvdata(device);
@@ -672,7 +672,7 @@ static int spi_qup_pm_resume_runtime(struct device *device)
 	writel_relaxed(config, controller->base + QUP_CONFIG);
 	return 0;
 }
-#endif /* CONFIG_PM_RUNTIME */
+#endif /* CONFIG_PM */
 
 #ifdef CONFIG_PM_SLEEP
 static int spi_qup_suspend(struct device *device)
@@ -756,7 +756,6 @@ static const struct dev_pm_ops spi_qup_dev_pm_ops = {
 static struct platform_driver spi_qup_driver = {
 	.driver = {
 		.name		= "spi_qup",
-		.owner		= THIS_MODULE,
 		.pm		= &spi_qup_dev_pm_ops,
 		.of_match_table = spi_qup_dt_match,
 	},
diff --git a/drivers/spi/spi-rockchip.c b/drivers/spi/spi-rockchip.c
index 80637e9406e2..daabbabd26b0 100644
--- a/drivers/spi/spi-rockchip.c
+++ b/drivers/spi/spi-rockchip.c
@@ -799,7 +799,7 @@ static int rockchip_spi_resume(struct device *dev)
 }
 #endif /* CONFIG_PM_SLEEP */
 
-#ifdef CONFIG_PM_RUNTIME
+#ifdef CONFIG_PM
 static int rockchip_spi_runtime_suspend(struct device *dev)
 {
 	struct spi_master *master = dev_get_drvdata(dev);
@@ -827,7 +827,7 @@ static int rockchip_spi_runtime_resume(struct device *dev)
 
 	return ret;
 }
-#endif /* CONFIG_PM_RUNTIME */
+#endif /* CONFIG_PM */
 
 static const struct dev_pm_ops rockchip_spi_pm = {
 	SET_SYSTEM_SLEEP_PM_OPS(rockchip_spi_suspend, rockchip_spi_resume)
@@ -846,7 +846,6 @@ MODULE_DEVICE_TABLE(of, rockchip_spi_dt_match);
 static struct platform_driver rockchip_spi_driver = {
 	.driver = {
 		.name	= DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.pm = &rockchip_spi_pm,
 		.of_match_table = of_match_ptr(rockchip_spi_dt_match),
 	},
diff --git a/drivers/spi/spi-rspi.c b/drivers/spi/spi-rspi.c
index 54bb0faec155..2071f788c6fb 100644
--- a/drivers/spi/spi-rspi.c
+++ b/drivers/spi/spi-rspi.c
@@ -1211,7 +1211,6 @@ static struct platform_driver rspi_driver = {
 	.id_table =	spi_driver_ids,
 	.driver		= {
 		.name = "renesas_spi",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(rspi_of_match),
 	},
 };
diff --git a/drivers/spi/spi-s3c24xx.c b/drivers/spi/spi-s3c24xx.c
index e713737d784f..f747ca269986 100644
--- a/drivers/spi/spi-s3c24xx.c
+++ b/drivers/spi/spi-s3c24xx.c
@@ -663,7 +663,6 @@ static struct platform_driver s3c24xx_spi_driver = {
 	.remove		= s3c24xx_spi_remove,
 	.driver		= {
 		.name	= "s3c2410-spi",
-		.owner	= THIS_MODULE,
 		.pm	= S3C24XX_SPI_PMOPS,
 	},
 };
diff --git a/drivers/spi/spi-s3c64xx.c b/drivers/spi/spi-s3c64xx.c
index 0f602cba1989..37b19836f5cb 100644
--- a/drivers/spi/spi-s3c64xx.c
+++ b/drivers/spi/spi-s3c64xx.c
@@ -1267,7 +1267,7 @@ static int s3c64xx_spi_resume(struct device *dev)
 }
 #endif /* CONFIG_PM_SLEEP */
 
-#ifdef CONFIG_PM_RUNTIME
+#ifdef CONFIG_PM
 static int s3c64xx_spi_runtime_suspend(struct device *dev)
 {
 	struct spi_master *master = dev_get_drvdata(dev);
@@ -1297,7 +1297,7 @@ static int s3c64xx_spi_runtime_resume(struct device *dev)
 
 	return 0;
 }
-#endif /* CONFIG_PM_RUNTIME */
+#endif /* CONFIG_PM */
 
 static const struct dev_pm_ops s3c64xx_spi_pm = {
 	SET_SYSTEM_SLEEP_PM_OPS(s3c64xx_spi_suspend, s3c64xx_spi_resume)
@@ -1394,7 +1394,6 @@ MODULE_DEVICE_TABLE(of, s3c64xx_spi_dt_match);
 static struct platform_driver s3c64xx_spi_driver = {
 	.driver = {
 		.name	= "s3c64xx-spi",
-		.owner = THIS_MODULE,
 		.pm = &s3c64xx_spi_pm,
 		.of_match_table = of_match_ptr(s3c64xx_spi_dt_match),
 	},
diff --git a/drivers/spi/spi-sh-hspi.c b/drivers/spi/spi-sh-hspi.c
index 94b5faed21e2..fc29233d0650 100644
--- a/drivers/spi/spi-sh-hspi.c
+++ b/drivers/spi/spi-sh-hspi.c
@@ -315,7 +315,6 @@ static struct platform_driver hspi_driver = {
 	.remove = hspi_remove,
 	.driver = {
 		.name = "sh-hspi",
-		.owner = THIS_MODULE,
 		.of_match_table = hspi_of_match,
 	},
 };
diff --git a/drivers/spi/spi-sh-msiof.c b/drivers/spi/spi-sh-msiof.c
index 3f365402fcc0..96a5fc0878d8 100644
--- a/drivers/spi/spi-sh-msiof.c
+++ b/drivers/spi/spi-sh-msiof.c
@@ -480,6 +480,8 @@ static int sh_msiof_spi_setup(struct spi_device *spi)
 	struct device_node	*np = spi->master->dev.of_node;
 	struct sh_msiof_spi_priv *p = spi_master_get_devdata(spi->master);
 
+	pm_runtime_get_sync(&p->pdev->dev);
+
 	if (!np) {
 		/*
 		 * Use spi->controller_data for CS (same strategy as spi_gpio),
@@ -498,6 +500,9 @@ static int sh_msiof_spi_setup(struct spi_device *spi)
 	if (spi->cs_gpio >= 0)
 		gpio_set_value(spi->cs_gpio, !(spi->mode & SPI_CS_HIGH));
 
+
+	pm_runtime_put_sync(&p->pdev->dev);
+
 	return 0;
 }
 
@@ -1235,7 +1240,6 @@ static struct platform_driver sh_msiof_spi_drv = {
 	.id_table	= spi_driver_ids,
 	.driver		= {
 		.name		= "spi_sh_msiof",
-		.owner		= THIS_MODULE,
 		.of_match_table = of_match_ptr(sh_msiof_match),
 	},
 };
diff --git a/drivers/spi/spi-sh-sci.c b/drivers/spi/spi-sh-sci.c
index b83dd733684c..a9beeeed812c 100644
--- a/drivers/spi/spi-sh-sci.c
+++ b/drivers/spi/spi-sh-sci.c
@@ -187,7 +187,6 @@ static struct platform_driver sh_sci_spi_drv = {
 	.remove		= sh_sci_spi_remove,
 	.driver		= {
 		.name	= "spi_sh_sci",
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(sh_sci_spi_drv);
diff --git a/drivers/spi/spi-sh.c b/drivers/spi/spi-sh.c
index 8e171a76049f..1cfc906dd174 100644
--- a/drivers/spi/spi-sh.c
+++ b/drivers/spi/spi-sh.c
@@ -532,7 +532,6 @@ static struct platform_driver spi_sh_driver = {
 	.remove = spi_sh_remove,
 	.driver = {
 		.name = "sh_spi",
-		.owner = THIS_MODULE,
 	},
 };
 module_platform_driver(spi_sh_driver);
diff --git a/drivers/spi/spi-sirf.c b/drivers/spi/spi-sirf.c
index 73a88b33f810..d075191476f0 100644
--- a/drivers/spi/spi-sirf.c
+++ b/drivers/spi/spi-sirf.c
@@ -826,7 +826,6 @@ MODULE_DEVICE_TABLE(of, spi_sirfsoc_of_match);
 static struct platform_driver spi_sirfsoc_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.pm     = &spi_sirfsoc_pm_ops,
 		.of_match_table = spi_sirfsoc_of_match,
 	},
diff --git a/drivers/spi/spi-sun4i.c b/drivers/spi/spi-sun4i.c
index 85204c93f3d3..fbb0a4d74e91 100644
--- a/drivers/spi/spi-sun4i.c
+++ b/drivers/spi/spi-sun4i.c
@@ -464,7 +464,6 @@ static struct platform_driver sun4i_spi_driver = {
 	.remove	= sun4i_spi_remove,
 	.driver	= {
 		.name		= "sun4i-spi",
-		.owner		= THIS_MODULE,
 		.of_match_table	= sun4i_spi_match,
 		.pm		= &sun4i_spi_pm_ops,
 	},
diff --git a/drivers/spi/spi-sun6i.c b/drivers/spi/spi-sun6i.c
index bd24093f4038..ac48f59705a8 100644
--- a/drivers/spi/spi-sun6i.c
+++ b/drivers/spi/spi-sun6i.c
@@ -470,7 +470,6 @@ static struct platform_driver sun6i_spi_driver = {
 	.remove	= sun6i_spi_remove,
 	.driver	= {
 		.name		= "sun6i-spi",
-		.owner		= THIS_MODULE,
 		.of_match_table	= sun6i_spi_match,
 		.pm		= &sun6i_spi_pm_ops,
 	},
diff --git a/drivers/spi/spi-tegra114.c b/drivers/spi/spi-tegra114.c
index 795bcbc0131b..73779cecc3bb 100644
--- a/drivers/spi/spi-tegra114.c
+++ b/drivers/spi/spi-tegra114.c
@@ -1223,7 +1223,6 @@ static const struct dev_pm_ops tegra_spi_pm_ops = {
 static struct platform_driver tegra_spi_driver = {
 	.driver = {
 		.name		= "spi-tegra114",
-		.owner		= THIS_MODULE,
 		.pm		= &tegra_spi_pm_ops,
 		.of_match_table	= tegra_spi_of_match,
 	},
diff --git a/drivers/spi/spi-tegra20-sflash.c b/drivers/spi/spi-tegra20-sflash.c
index cd66fe7b78a9..b6558bb6f9df 100644
--- a/drivers/spi/spi-tegra20-sflash.c
+++ b/drivers/spi/spi-tegra20-sflash.c
@@ -608,7 +608,6 @@ static const struct dev_pm_ops slink_pm_ops = {
 static struct platform_driver tegra_sflash_driver = {
 	.driver = {
 		.name		= "spi-tegra-sflash",
-		.owner		= THIS_MODULE,
 		.pm		= &slink_pm_ops,
 		.of_match_table	= tegra_sflash_of_match,
 	},
diff --git a/drivers/spi/spi-tegra20-slink.c b/drivers/spi/spi-tegra20-slink.c
index 0b9e32e9f493..85c91f58b42f 100644
--- a/drivers/spi/spi-tegra20-slink.c
+++ b/drivers/spi/spi-tegra20-slink.c
@@ -1224,7 +1224,6 @@ static const struct dev_pm_ops slink_pm_ops = {
 static struct platform_driver tegra_slink_driver = {
 	.driver = {
 		.name		= "spi-tegra-slink",
-		.owner		= THIS_MODULE,
 		.pm		= &slink_pm_ops,
 		.of_match_table	= tegra_slink_of_match,
 	},
diff --git a/drivers/spi/spi-ti-qspi.c b/drivers/spi/spi-ti-qspi.c
index 6c211d1910b0..6146c4cd6583 100644
--- a/drivers/spi/spi-ti-qspi.c
+++ b/drivers/spi/spi-ti-qspi.c
@@ -567,7 +567,6 @@ static struct platform_driver ti_qspi_driver = {
 	.remove = ti_qspi_remove,
 	.driver = {
 		.name	= "ti-qspi",
-		.owner	= THIS_MODULE,
 		.pm =   &ti_qspi_pm_ops,
 		.of_match_table = ti_qspi_match,
 	}
diff --git a/drivers/spi/spi-topcliff-pch.c b/drivers/spi/spi-topcliff-pch.c
index 0a87ec39f619..be692ad50442 100644
--- a/drivers/spi/spi-topcliff-pch.c
+++ b/drivers/spi/spi-topcliff-pch.c
@@ -1561,7 +1561,6 @@ static int pch_spi_pd_resume(struct platform_device *pd_dev)
 static struct platform_driver pch_spi_pd_driver = {
 	.driver = {
 		.name = "pch-spi",
-		.owner = THIS_MODULE,
 	},
 	.probe = pch_spi_pd_probe,
 	.remove = pch_spi_pd_remove,
diff --git a/drivers/spi/spi-txx9.c b/drivers/spi/spi-txx9.c
index f2ab827c81bb..9190124b6d90 100644
--- a/drivers/spi/spi-txx9.c
+++ b/drivers/spi/spi-txx9.c
@@ -425,7 +425,6 @@ static struct platform_driver txx9spi_driver = {
 	.remove = txx9spi_remove,
 	.driver = {
 		.name = "spi_txx9",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/spi/spi-xtensa-xtfpga.c b/drivers/spi/spi-xtensa-xtfpga.c
index 0dc5df5233a9..2e32ea2f194f 100644
--- a/drivers/spi/spi-xtensa-xtfpga.c
+++ b/drivers/spi/spi-xtensa-xtfpga.c
@@ -160,7 +160,6 @@ static struct platform_driver xtfpga_spi_driver = {
 	.remove = xtfpga_spi_remove,
 	.driver = {
 		.name = XTFPGA_SPI_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(xtfpga_spi_of_match),
 	},
 };
diff --git a/drivers/spmi/spmi-pmic-arb.c b/drivers/spmi/spmi-pmic-arb.c
index 246e03a18c94..20559ab3466d 100644
--- a/drivers/spmi/spmi-pmic-arb.c
+++ b/drivers/spmi/spmi-pmic-arb.c
@@ -768,7 +768,6 @@ static struct platform_driver spmi_pmic_arb_driver = {
 	.remove		= spmi_pmic_arb_remove,
 	.driver		= {
 		.name	= "spmi_pmic_arb",
-		.owner	= THIS_MODULE,
 		.of_match_table = spmi_pmic_arb_match_table,
 	},
 };
diff --git a/drivers/ssb/driver_mipscore.c b/drivers/ssb/driver_mipscore.c
index 09077067b0c8..7b986f9f213f 100644
--- a/drivers/ssb/driver_mipscore.c
+++ b/drivers/ssb/driver_mipscore.c
@@ -15,6 +15,9 @@
 #include <linux/serial_core.h>
 #include <linux/serial_reg.h>
 #include <linux/time.h>
+#ifdef CONFIG_BCM47XX
+#include <bcm47xx_nvram.h>
+#endif
 
 #include "ssb_private.h"
 
@@ -210,6 +213,7 @@ static void ssb_mips_serial_init(struct ssb_mipscore *mcore)
 static void ssb_mips_flash_detect(struct ssb_mipscore *mcore)
 {
 	struct ssb_bus *bus = mcore->dev->bus;
+	struct ssb_sflash *sflash = &mcore->sflash;
 	struct ssb_pflash *pflash = &mcore->pflash;
 
 	/* When there is no chipcommon on the bus there is 4MB flash */
@@ -242,7 +246,15 @@ static void ssb_mips_flash_detect(struct ssb_mipscore *mcore)
 	}
 
 ssb_pflash:
-	if (pflash->present) {
+	if (sflash->present) {
+#ifdef CONFIG_BCM47XX
+		bcm47xx_nvram_init_from_mem(sflash->window, sflash->size);
+#endif
+	} else if (pflash->present) {
+#ifdef CONFIG_BCM47XX
+		bcm47xx_nvram_init_from_mem(pflash->window, pflash->window_size);
+#endif
+
 		ssb_pflash_data.width = pflash->buswidth;
 		ssb_pflash_resource.start = pflash->window;
 		ssb_pflash_resource.end = pflash->window + pflash->window_size;
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index 4690ae9a267f..815de379a130 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -62,8 +62,6 @@ source "drivers/staging/xgifb/Kconfig"
 
 source "drivers/staging/emxx_udc/Kconfig"
 
-source "drivers/staging/bcm/Kconfig"
-
 source "drivers/staging/ft1000/Kconfig"
 
 source "drivers/staging/speakup/Kconfig"
@@ -86,8 +84,6 @@ source "drivers/staging/gdm72xx/Kconfig"
 
 source "drivers/staging/gdm724x/Kconfig"
 
-source "drivers/staging/imx-drm/Kconfig"
-
 source "drivers/staging/fwserial/Kconfig"
 
 source "drivers/staging/goldfish/Kconfig"
@@ -108,4 +104,6 @@ source "drivers/staging/skein/Kconfig"
 
 source "drivers/staging/unisys/Kconfig"
 
+source "drivers/staging/clocking-wizard/Kconfig"
+
 endif # STAGING
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index c780a0e70e15..33c640b49566 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -25,7 +25,6 @@ obj-$(CONFIG_VME_BUS)		+= vme/
 obj-$(CONFIG_IIO)		+= iio/
 obj-$(CONFIG_FB_XGI)		+= xgifb/
 obj-$(CONFIG_USB_EMXX)		+= emxx_udc/
-obj-$(CONFIG_BCM_WIMAX)		+= bcm/
 obj-$(CONFIG_FT1000)		+= ft1000/
 obj-$(CONFIG_SPEAKUP)		+= speakup/
 obj-$(CONFIG_TOUCHSCREEN_CLEARPAD_TM1217)	+= cptm1217/
@@ -36,7 +35,6 @@ obj-$(CONFIG_STAGING_BOARD)	+= board/
 obj-$(CONFIG_USB_WPAN_HCD)	+= ozwpan/
 obj-$(CONFIG_WIMAX_GDM72XX)	+= gdm72xx/
 obj-$(CONFIG_LTE_GDM724X)	+= gdm724x/
-obj-$(CONFIG_DRM_IMX)		+= imx-drm/
 obj-$(CONFIG_FIREWIRE_SERIAL)	+= fwserial/
 obj-$(CONFIG_GOLDFISH)		+= goldfish/
 obj-$(CONFIG_LUSTRE_FS)		+= lustre/
@@ -46,3 +44,4 @@ obj-$(CONFIG_MTD_SPINAND_MT29F)	+= mt29f_spinand/
 obj-$(CONFIG_GS_FPGABOOT)	+= gs_fpgaboot/
 obj-$(CONFIG_CRYPTO_SKEIN)	+= skein/
 obj-$(CONFIG_UNISYSSPAR)	+= unisys/
+obj-$(CONFIG_COMMON_CLK_XLNX_CLKWZRD)	+= clocking-wizard/
diff --git a/drivers/staging/android/Kconfig b/drivers/staging/android/Kconfig
index 7a0e28852965..7e012f37792b 100644
--- a/drivers/staging/android/Kconfig
+++ b/drivers/staging/android/Kconfig
@@ -1,37 +1,7 @@
 menu "Android"
 
-config ANDROID
-	bool "Android Drivers"
-	---help---
-	  Enable support for various drivers needed on the Android platform
-
 if ANDROID
 
-config ANDROID_BINDER_IPC
-	bool "Android Binder IPC Driver"
-	depends on MMU
-	default n
-	---help---
-	  Binder is used in Android for both communication between processes,
-	  and remote method invocation.
-
-	  This means one Android process can call a method/routine in another
-	  Android process, using Binder to identify, invoke and pass arguments
-	  between said processes.
-
-config ANDROID_BINDER_IPC_32BIT
-	bool
-	depends on !64BIT && ANDROID_BINDER_IPC
-	default y
-	---help---
-	  The Binder API has been changed to support both 32 and 64bit
-	  applications in a mixed environment.
-
-	  Enable this to support an old 32-bit Android user-space (v4.4 and
-	  earlier).
-
-	  Note that enabling this will break newer Android user-space.
-
 config ASHMEM
 	bool "Enable the Anonymous Shared Memory Subsystem"
 	default n
diff --git a/drivers/staging/android/Makefile b/drivers/staging/android/Makefile
index 517ad5ffa429..479b2b86f8c8 100644
--- a/drivers/staging/android/Makefile
+++ b/drivers/staging/android/Makefile
@@ -2,7 +2,6 @@ ccflags-y += -I$(src)			# needed for trace events
 
 obj-y					+= ion/
 
-obj-$(CONFIG_ANDROID_BINDER_IPC)	+= binder.o
 obj-$(CONFIG_ASHMEM)			+= ashmem.o
 obj-$(CONFIG_ANDROID_LOGGER)		+= logger.o
 obj-$(CONFIG_ANDROID_TIMED_OUTPUT)	+= timed_output.o
diff --git a/drivers/staging/android/TODO b/drivers/staging/android/TODO
index b15fb0d6b152..06954cdf3dba 100644
--- a/drivers/staging/android/TODO
+++ b/drivers/staging/android/TODO
@@ -5,6 +5,13 @@ TODO:
 	- make sure things build as modules properly
 	- add proper arch dependencies as needed
 	- audit userspace interfaces to make sure they are sane
+	- kuid_t should never be exposed to user space as it is
+          kernel internal type. Data structure for this kuid_t is:
+          typedef struct {
+          	uid_t val;
+          } kuid_t;
+	- This bug is introduced by Xiong Zhou in the patch bd471258f2e09
+	- ("staging: android: logger: use kuid_t instead of uid_t")
 
 Please send patches to Greg Kroah-Hartman <greg@kroah.com> and Cc:
 Brian Swetland <swetland@google.com>
diff --git a/drivers/staging/android/ashmem.c b/drivers/staging/android/ashmem.c
index ad4f5790a76f..8c7852742f4b 100644
--- a/drivers/staging/android/ashmem.c
+++ b/drivers/staging/android/ashmem.c
@@ -418,7 +418,7 @@ out:
 }
 
 /*
- * ashmem_shrink - our cache shrinker, called from mm/vmscan.c :: shrink_slab
+ * ashmem_shrink - our cache shrinker, called from mm/vmscan.c
  *
  * 'nr_to_scan' is the number of objects to scan for freeing.
  *
@@ -446,7 +446,7 @@ ashmem_shrink_scan(struct shrinker *shrink, struct shrink_control *sc)
 		loff_t start = range->pgstart * PAGE_SIZE;
 		loff_t end = (range->pgend + 1) * PAGE_SIZE;
 
-		do_fallocate(range->asma->file,
+		vfs_fallocate(range->asma->file,
 				FALLOC_FL_PUNCH_HOLE | FALLOC_FL_KEEP_SIZE,
 				start, end - start);
 		range->purged = ASHMEM_WAS_PURGED;
@@ -785,7 +785,6 @@ static long ashmem_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 				.nr_to_scan = LONG_MAX,
 			};
 			ret = ashmem_shrink_count(&ashmem_shrinker, &sc);
-			nodes_setall(sc.nodes_to_scan);
 			ashmem_shrink_scan(&ashmem_shrinker, &sc);
 		}
 		break;
diff --git a/drivers/staging/android/ion/ion.c b/drivers/staging/android/ion/ion.c
index 56604f41ec48..296d347660fc 100644
--- a/drivers/staging/android/ion/ion.c
+++ b/drivers/staging/android/ion/ion.c
@@ -250,7 +250,7 @@ static struct ion_buffer *ion_buffer_create(struct ion_heap *heap,
 	   our systems the only dma_address space is physical addresses.
 	   Additionally, we can't afford the overhead of invalidating every
 	   allocation via dma_map_sg. The implicit contract here is that
-	   memory comming from the heaps is ready for dma, ie if it has a
+	   memory coming from the heaps is ready for dma, ie if it has a
 	   cached mapping that mapping has been invalidated */
 	for_each_sg(buffer->sg_table->sgl, sg, buffer->sg_table->nents, i)
 		sg_dma_address(sg) = sg_phys(sg);
@@ -263,8 +263,7 @@ err:
 	heap->ops->unmap_dma(heap, buffer);
 	heap->ops->free(buffer);
 err1:
-	if (buffer->pages)
-		vfree(buffer->pages);
+	vfree(buffer->pages);
 err2:
 	kfree(buffer);
 	return ERR_PTR(ret);
@@ -276,8 +275,7 @@ void ion_buffer_destroy(struct ion_buffer *buffer)
 		buffer->heap->ops->unmap_kernel(buffer->heap, buffer);
 	buffer->heap->ops->unmap_dma(buffer->heap, buffer);
 	buffer->heap->ops->free(buffer);
-	if (buffer->pages)
-		vfree(buffer->pages);
+	vfree(buffer->pages);
 	kfree(buffer);
 }
 
@@ -902,7 +900,7 @@ void ion_pages_sync_for_device(struct device *dev, struct page *page,
 	sg_set_page(&sg, page, size, 0);
 	/*
 	 * This is not correct - sg_dma_address needs a dma_addr_t that is valid
-	 * for the the targeted device, but this works on the currently targeted
+	 * for the targeted device, but this works on the currently targeted
 	 * hardware.
 	 */
 	sg_dma_address(&sg) = page_to_phys(page);
diff --git a/drivers/staging/android/ion/ion.h b/drivers/staging/android/ion/ion.h
index d305bb7e9a74..443db8459a9e 100644
--- a/drivers/staging/android/ion/ion.h
+++ b/drivers/staging/android/ion/ion.h
@@ -76,7 +76,7 @@ struct ion_platform_data {
  *		size
  *
  * Calls memblock reserve to set aside memory for heaps that are
- * located at specific memory addresses or of specfic sizes not
+ * located at specific memory addresses or of specific sizes not
  * managed by the kernel
  */
 void ion_reserve(struct ion_platform_data *data);
diff --git a/drivers/staging/android/ion/ion_dummy_driver.c b/drivers/staging/android/ion/ion_dummy_driver.c
index f3ea1c31e533..5678870bff48 100644
--- a/drivers/staging/android/ion/ion_dummy_driver.c
+++ b/drivers/staging/android/ion/ion_dummy_driver.c
@@ -112,10 +112,8 @@ static int __init ion_dummy_init(void)
 	}
 	return 0;
 err:
-	for (i = 0; i < dummy_ion_pdata.nr; i++) {
-		if (heaps[i])
-			ion_heap_destroy(heaps[i]);
-	}
+	for (i = 0; i < dummy_ion_pdata.nr; ++i)
+		ion_heap_destroy(heaps[i]);
 	kfree(heaps);
 
 	if (carveout_ptr) {
diff --git a/drivers/staging/android/ion/ion_page_pool.c b/drivers/staging/android/ion/ion_page_pool.c
index 5864f3dfcbc6..4b88f11e52d3 100644
--- a/drivers/staging/android/ion/ion_page_pool.c
+++ b/drivers/staging/android/ion/ion_page_pool.c
@@ -120,7 +120,7 @@ int ion_page_pool_shrink(struct ion_page_pool *pool, gfp_t gfp_mask,
 	bool high;
 
 	if (current_is_kswapd())
-		high = 1;
+		high = true;
 	else
 		high = !!(gfp_mask & __GFP_HIGHMEM);
 
diff --git a/drivers/staging/android/ion/ion_priv.h b/drivers/staging/android/ion/ion_priv.h
index c8f01757abfa..18a5f93e13b7 100644
--- a/drivers/staging/android/ion/ion_priv.h
+++ b/drivers/staging/android/ion/ion_priv.h
@@ -345,7 +345,7 @@ void ion_carveout_free(struct ion_heap *heap, ion_phys_addr_t addr,
  * functions for creating and destroying a heap pool -- allows you
  * to keep a pool of pre allocated memory to use from your heap.  Keeping
  * a pool of memory that is ready for dma, ie any cached mapping have been
- * invalidated from the cache, provides a significant peformance benefit on
+ * invalidated from the cache, provides a significant performance benefit on
  * many systems */
 
 /**
@@ -362,7 +362,7 @@ void ion_carveout_free(struct ion_heap *heap, ion_phys_addr_t addr,
  *
  * Allows you to keep a pool of pre allocated pages to use from your heap.
  * Keeping a pool of pages that is ready for dma, ie any cached mapping have
- * been invalidated from the cache, provides a significant peformance benefit
+ * been invalidated from the cache, provides a significant performance benefit
  * on many systems
  */
 struct ion_page_pool {
diff --git a/drivers/staging/android/ion/tegra/tegra_ion.c b/drivers/staging/android/ion/tegra/tegra_ion.c
index 11c7cceb3c7d..5b8ef0e66010 100644
--- a/drivers/staging/android/ion/tegra/tegra_ion.c
+++ b/drivers/staging/android/ion/tegra/tegra_ion.c
@@ -54,10 +54,8 @@ static int tegra_ion_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, idev);
 	return 0;
 err:
-	for (i = 0; i < num_heaps; i++) {
-		if (heaps[i])
-			ion_heap_destroy(heaps[i]);
-	}
+	for (i = 0; i < num_heaps; ++i)
+		ion_heap_destroy(heaps[i]);
 	return err;
 }
 
diff --git a/drivers/staging/android/sync_debug.c b/drivers/staging/android/sync_debug.c
index 257fc91bf02b..1532a86404be 100644
--- a/drivers/staging/android/sync_debug.c
+++ b/drivers/staging/android/sync_debug.c
@@ -25,6 +25,7 @@
 #include <linux/slab.h>
 #include <linux/uaccess.h>
 #include <linux/anon_inodes.h>
+#include <linux/time64.h>
 #include "sync.h"
 
 #ifdef CONFIG_DEBUG_FS
@@ -95,9 +96,9 @@ static void sync_print_pt(struct seq_file *s, struct sync_pt *pt, bool fence)
 		   sync_status_str(status));
 
 	if (status <= 0) {
-		struct timeval tv = ktime_to_timeval(pt->base.timestamp);
+		struct timespec64 ts64 = ktime_to_timespec64(pt->base.timestamp);
 
-		seq_printf(s, "@%ld.%06ld", tv.tv_sec, tv.tv_usec);
+		seq_printf(s, "@%lld.%09ld", (s64)ts64.tv_sec, ts64.tv_nsec);
 	}
 
 	if (parent->ops->timeline_value_str &&
diff --git a/drivers/staging/android/timed_gpio.c b/drivers/staging/android/timed_gpio.c
index 8fa4758517c0..938a35cd99bb 100644
--- a/drivers/staging/android/timed_gpio.c
+++ b/drivers/staging/android/timed_gpio.c
@@ -20,6 +20,7 @@
 #include <linux/hrtimer.h>
 #include <linux/err.h>
 #include <linux/gpio.h>
+#include <linux/ktime.h>
 
 #include "timed_output.h"
 #include "timed_gpio.h"
@@ -46,16 +47,16 @@ static enum hrtimer_restart gpio_timer_func(struct hrtimer *timer)
 static int gpio_get_time(struct timed_output_dev *dev)
 {
 	struct timed_gpio_data *data;
-	struct timeval t;
+	ktime_t t;
 
 	data = container_of(dev, struct timed_gpio_data, dev);
 
 	if (!hrtimer_active(&data->timer))
 		return 0;
 
-	t = ktime_to_timeval(hrtimer_get_remaining(&data->timer));
+	t = hrtimer_get_remaining(&data->timer);
 
-	return t.tv_sec * 1000 + t.tv_usec / 1000;
+	return ktime_to_ms(t);
 }
 
 static void gpio_enable(struct timed_output_dev *dev, int value)
@@ -157,7 +158,6 @@ static struct platform_driver timed_gpio_driver = {
 	.remove		= timed_gpio_remove,
 	.driver		= {
 		.name		= TIMED_GPIO_NAME,
-		.owner		= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/staging/android/uapi/binder.h b/drivers/staging/android/uapi/binder.h
deleted file mode 100644
index dba4cef3a8d3..000000000000
--- a/drivers/staging/android/uapi/binder.h
+++ /dev/null
@@ -1,351 +0,0 @@
-/*
- * Copyright (C) 2008 Google, Inc.
- *
- * Based on, but no longer compatible with, the original
- * OpenBinder.org binder driver interface, which is:
- *
- * Copyright (c) 2005 Palmsource, 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 _UAPI_LINUX_BINDER_H
-#define _UAPI_LINUX_BINDER_H
-
-#include <linux/ioctl.h>
-
-#define B_PACK_CHARS(c1, c2, c3, c4) \
-	((((c1)<<24)) | (((c2)<<16)) | (((c3)<<8)) | (c4))
-#define B_TYPE_LARGE 0x85
-
-enum {
-	BINDER_TYPE_BINDER	= B_PACK_CHARS('s', 'b', '*', B_TYPE_LARGE),
-	BINDER_TYPE_WEAK_BINDER	= B_PACK_CHARS('w', 'b', '*', B_TYPE_LARGE),
-	BINDER_TYPE_HANDLE	= B_PACK_CHARS('s', 'h', '*', B_TYPE_LARGE),
-	BINDER_TYPE_WEAK_HANDLE	= B_PACK_CHARS('w', 'h', '*', B_TYPE_LARGE),
-	BINDER_TYPE_FD		= B_PACK_CHARS('f', 'd', '*', B_TYPE_LARGE),
-};
-
-enum {
-	FLAT_BINDER_FLAG_PRIORITY_MASK = 0xff,
-	FLAT_BINDER_FLAG_ACCEPTS_FDS = 0x100,
-};
-
-#ifdef BINDER_IPC_32BIT
-typedef __u32 binder_size_t;
-typedef __u32 binder_uintptr_t;
-#else
-typedef __u64 binder_size_t;
-typedef __u64 binder_uintptr_t;
-#endif
-
-/*
- * This is the flattened representation of a Binder object for transfer
- * between processes.  The 'offsets' supplied as part of a binder transaction
- * contains offsets into the data where these structures occur.  The Binder
- * driver takes care of re-writing the structure type and data as it moves
- * between processes.
- */
-struct flat_binder_object {
-	/* 8 bytes for large_flat_header. */
-	__u32		type;
-	__u32		flags;
-
-	/* 8 bytes of data. */
-	union {
-		binder_uintptr_t	binder;	/* local object */
-		__u32			handle;	/* remote object */
-	};
-
-	/* extra data associated with local object */
-	binder_uintptr_t	cookie;
-};
-
-/*
- * On 64-bit platforms where user code may run in 32-bits the driver must
- * translate the buffer (and local binder) addresses appropriately.
- */
-
-struct binder_write_read {
-	binder_size_t		write_size;	/* bytes to write */
-	binder_size_t		write_consumed;	/* bytes consumed by driver */
-	binder_uintptr_t	write_buffer;
-	binder_size_t		read_size;	/* bytes to read */
-	binder_size_t		read_consumed;	/* bytes consumed by driver */
-	binder_uintptr_t	read_buffer;
-};
-
-/* Use with BINDER_VERSION, driver fills in fields. */
-struct binder_version {
-	/* driver protocol version -- increment with incompatible change */
-	__s32       protocol_version;
-};
-
-/* This is the current protocol version. */
-#ifdef BINDER_IPC_32BIT
-#define BINDER_CURRENT_PROTOCOL_VERSION 7
-#else
-#define BINDER_CURRENT_PROTOCOL_VERSION 8
-#endif
-
-#define BINDER_WRITE_READ		_IOWR('b', 1, struct binder_write_read)
-#define BINDER_SET_IDLE_TIMEOUT		_IOW('b', 3, __s64)
-#define BINDER_SET_MAX_THREADS		_IOW('b', 5, __u32)
-#define BINDER_SET_IDLE_PRIORITY	_IOW('b', 6, __s32)
-#define BINDER_SET_CONTEXT_MGR		_IOW('b', 7, __s32)
-#define BINDER_THREAD_EXIT		_IOW('b', 8, __s32)
-#define BINDER_VERSION			_IOWR('b', 9, struct binder_version)
-
-/*
- * NOTE: Two special error codes you should check for when calling
- * in to the driver are:
- *
- * EINTR -- The operation has been interupted.  This should be
- * handled by retrying the ioctl() until a different error code
- * is returned.
- *
- * ECONNREFUSED -- The driver is no longer accepting operations
- * from your process.  That is, the process is being destroyed.
- * You should handle this by exiting from your process.  Note
- * that once this error code is returned, all further calls to
- * the driver from any thread will return this same code.
- */
-
-enum transaction_flags {
-	TF_ONE_WAY	= 0x01,	/* this is a one-way call: async, no return */
-	TF_ROOT_OBJECT	= 0x04,	/* contents are the component's root object */
-	TF_STATUS_CODE	= 0x08,	/* contents are a 32-bit status code */
-	TF_ACCEPT_FDS	= 0x10,	/* allow replies with file descriptors */
-};
-
-struct binder_transaction_data {
-	/* The first two are only used for bcTRANSACTION and brTRANSACTION,
-	 * identifying the target and contents of the transaction.
-	 */
-	union {
-		/* target descriptor of command transaction */
-		__u32	handle;
-		/* target descriptor of return transaction */
-		binder_uintptr_t ptr;
-	} target;
-	binder_uintptr_t	cookie;	/* target object cookie */
-	__u32		code;		/* transaction command */
-
-	/* General information about the transaction. */
-	__u32	        flags;
-	pid_t		sender_pid;
-	uid_t		sender_euid;
-	binder_size_t	data_size;	/* number of bytes of data */
-	binder_size_t	offsets_size;	/* number of bytes of offsets */
-
-	/* If this transaction is inline, the data immediately
-	 * follows here; otherwise, it ends with a pointer to
-	 * the data buffer.
-	 */
-	union {
-		struct {
-			/* transaction data */
-			binder_uintptr_t	buffer;
-			/* offsets from buffer to flat_binder_object structs */
-			binder_uintptr_t	offsets;
-		} ptr;
-		__u8	buf[8];
-	} data;
-};
-
-struct binder_ptr_cookie {
-	binder_uintptr_t ptr;
-	binder_uintptr_t cookie;
-};
-
-struct binder_handle_cookie {
-	__u32 handle;
-	binder_uintptr_t cookie;
-} __packed;
-
-struct binder_pri_desc {
-	__s32 priority;
-	__u32 desc;
-};
-
-struct binder_pri_ptr_cookie {
-	__s32 priority;
-	binder_uintptr_t ptr;
-	binder_uintptr_t cookie;
-};
-
-enum binder_driver_return_protocol {
-	BR_ERROR = _IOR('r', 0, __s32),
-	/*
-	 * int: error code
-	 */
-
-	BR_OK = _IO('r', 1),
-	/* No parameters! */
-
-	BR_TRANSACTION = _IOR('r', 2, struct binder_transaction_data),
-	BR_REPLY = _IOR('r', 3, struct binder_transaction_data),
-	/*
-	 * binder_transaction_data: the received command.
-	 */
-
-	BR_ACQUIRE_RESULT = _IOR('r', 4, __s32),
-	/*
-	 * not currently supported
-	 * int: 0 if the last bcATTEMPT_ACQUIRE was not successful.
-	 * Else the remote object has acquired a primary reference.
-	 */
-
-	BR_DEAD_REPLY = _IO('r', 5),
-	/*
-	 * The target of the last transaction (either a bcTRANSACTION or
-	 * a bcATTEMPT_ACQUIRE) is no longer with us.  No parameters.
-	 */
-
-	BR_TRANSACTION_COMPLETE = _IO('r', 6),
-	/*
-	 * No parameters... always refers to the last transaction requested
-	 * (including replies).  Note that this will be sent even for
-	 * asynchronous transactions.
-	 */
-
-	BR_INCREFS = _IOR('r', 7, struct binder_ptr_cookie),
-	BR_ACQUIRE = _IOR('r', 8, struct binder_ptr_cookie),
-	BR_RELEASE = _IOR('r', 9, struct binder_ptr_cookie),
-	BR_DECREFS = _IOR('r', 10, struct binder_ptr_cookie),
-	/*
-	 * void *:	ptr to binder
-	 * void *: cookie for binder
-	 */
-
-	BR_ATTEMPT_ACQUIRE = _IOR('r', 11, struct binder_pri_ptr_cookie),
-	/*
-	 * not currently supported
-	 * int:	priority
-	 * void *: ptr to binder
-	 * void *: cookie for binder
-	 */
-
-	BR_NOOP = _IO('r', 12),
-	/*
-	 * No parameters.  Do nothing and examine the next command.  It exists
-	 * primarily so that we can replace it with a BR_SPAWN_LOOPER command.
-	 */
-
-	BR_SPAWN_LOOPER = _IO('r', 13),
-	/*
-	 * No parameters.  The driver has determined that a process has no
-	 * threads waiting to service incoming transactions.  When a process
-	 * receives this command, it must spawn a new service thread and
-	 * register it via bcENTER_LOOPER.
-	 */
-
-	BR_FINISHED = _IO('r', 14),
-	/*
-	 * not currently supported
-	 * stop threadpool thread
-	 */
-
-	BR_DEAD_BINDER = _IOR('r', 15, binder_uintptr_t),
-	/*
-	 * void *: cookie
-	 */
-	BR_CLEAR_DEATH_NOTIFICATION_DONE = _IOR('r', 16, binder_uintptr_t),
-	/*
-	 * void *: cookie
-	 */
-
-	BR_FAILED_REPLY = _IO('r', 17),
-	/*
-	 * The the last transaction (either a bcTRANSACTION or
-	 * a bcATTEMPT_ACQUIRE) failed (e.g. out of memory).  No parameters.
-	 */
-};
-
-enum binder_driver_command_protocol {
-	BC_TRANSACTION = _IOW('c', 0, struct binder_transaction_data),
-	BC_REPLY = _IOW('c', 1, struct binder_transaction_data),
-	/*
-	 * binder_transaction_data: the sent command.
-	 */
-
-	BC_ACQUIRE_RESULT = _IOW('c', 2, __s32),
-	/*
-	 * not currently supported
-	 * int:  0 if the last BR_ATTEMPT_ACQUIRE was not successful.
-	 * Else you have acquired a primary reference on the object.
-	 */
-
-	BC_FREE_BUFFER = _IOW('c', 3, binder_uintptr_t),
-	/*
-	 * void *: ptr to transaction data received on a read
-	 */
-
-	BC_INCREFS = _IOW('c', 4, __u32),
-	BC_ACQUIRE = _IOW('c', 5, __u32),
-	BC_RELEASE = _IOW('c', 6, __u32),
-	BC_DECREFS = _IOW('c', 7, __u32),
-	/*
-	 * int:	descriptor
-	 */
-
-	BC_INCREFS_DONE = _IOW('c', 8, struct binder_ptr_cookie),
-	BC_ACQUIRE_DONE = _IOW('c', 9, struct binder_ptr_cookie),
-	/*
-	 * void *: ptr to binder
-	 * void *: cookie for binder
-	 */
-
-	BC_ATTEMPT_ACQUIRE = _IOW('c', 10, struct binder_pri_desc),
-	/*
-	 * not currently supported
-	 * int: priority
-	 * int: descriptor
-	 */
-
-	BC_REGISTER_LOOPER = _IO('c', 11),
-	/*
-	 * No parameters.
-	 * Register a spawned looper thread with the device.
-	 */
-
-	BC_ENTER_LOOPER = _IO('c', 12),
-	BC_EXIT_LOOPER = _IO('c', 13),
-	/*
-	 * No parameters.
-	 * These two commands are sent as an application-level thread
-	 * enters and exits the binder loop, respectively.  They are
-	 * used so the binder can have an accurate count of the number
-	 * of looping threads it has available.
-	 */
-
-	BC_REQUEST_DEATH_NOTIFICATION = _IOW('c', 14,
-						struct binder_handle_cookie),
-	/*
-	 * int: handle
-	 * void *: cookie
-	 */
-
-	BC_CLEAR_DEATH_NOTIFICATION = _IOW('c', 15,
-						struct binder_handle_cookie),
-	/*
-	 * int: handle
-	 * void *: cookie
-	 */
-
-	BC_DEAD_BINDER_DONE = _IOW('c', 16, binder_uintptr_t),
-	/*
-	 * void *: cookie
-	 */
-};
-
-#endif /* _UAPI_LINUX_BINDER_H */
-
diff --git a/drivers/staging/bcm/Adapter.h b/drivers/staging/bcm/Adapter.h
deleted file mode 100644
index 940c852e17b7..000000000000
--- a/drivers/staging/bcm/Adapter.h
+++ /dev/null
@@ -1,474 +0,0 @@
-/***********************************
-*	Adapter.h
-************************************/
-#ifndef	__ADAPTER_H__
-#define	__ADAPTER_H__
-
-#define MAX_FRAGMENTEDIP_CLASSIFICATION_ENTRIES 256
-#include "Debug.h"
-
-struct bcm_leader {
-	USHORT	Vcid;
-	USHORT	PLength;
-	UCHAR	Status;
-	UCHAR	Unused[3];
-} __packed;
-
-struct bcm_packettosend {
-	struct bcm_leader Leader;
-	UCHAR	ucPayload;
-} __packed;
-
-struct bcm_control_packet {
-	PVOID	ControlBuff;
-	UINT	ControlBuffLen;
-	struct bcm_control_packet *next;
-} __packed;
-
-struct bcm_link_request {
-	struct bcm_leader Leader;
-	UCHAR	szData[4];
-} __packed;
-
-#define MAX_IP_RANGE_LENGTH 4
-#define MAX_PORT_RANGE 4
-#define MAX_PROTOCOL_LENGTH   32
-#define IPV6_ADDRESS_SIZEINBYTES 0x10
-
-union u_ip_address {
-	struct {
-		/* Source Ip Address Range */
-		ULONG ulIpv4Addr[MAX_IP_RANGE_LENGTH];
-		 /* Source Ip Mask Address Range */
-		ULONG ulIpv4Mask[MAX_IP_RANGE_LENGTH];
-	};
-	struct {
-		/* Source Ip Address Range */
-		ULONG ulIpv6Addr[MAX_IP_RANGE_LENGTH * 4];
-		/* Source Ip Mask Address Range */
-		ULONG ulIpv6Mask[MAX_IP_RANGE_LENGTH * 4];
-	};
-	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 bcm_hdr_suppression_contextinfo {
-	/* Intermediate buffer to accumulate pkt Header for PHS */
-	UCHAR ucaHdrSuppressionInBuf[MAX_PHS_LENGTHS];
-	/* Intermediate buffer containing pkt Header after PHS */
-	UCHAR ucaHdrSuppressionOutBuf[MAX_PHS_LENGTHS + PHSI_LEN];
-};
-
-struct bcm_classifier_rule {
-	ULONG		ulSFID;
-	UCHAR		ucReserved[2];
-	B_UINT16	uiClassifierRuleIndex;
-	bool		bUsed;
-	USHORT		usVCID_Value;
-	/* This field detemines the Classifier Priority */
-	B_UINT8		u8ClassifierRulePriority;
-	union u_ip_address	stSrcIpAddress;
-	UCHAR		ucIPSourceAddressLength; /* Ip Source Address Length */
-
-	union u_ip_address	stDestIpAddress;
-	/* Ip Destination Address Length */
-	UCHAR		ucIPDestinationAddressLength;
-	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;
-
-	bool		bProtocolValid;
-	bool		bTOSValid;
-	bool		bDestIpValid;
-	bool		bSrcIpValid;
-
-	/* For IPv6 Addressing */
-	UCHAR		ucDirection;
-	bool		bIpv6Protocol;
-	UINT32		u32PHSRuleID;
-	struct bcm_phs_rule sPhsRule;
-	UCHAR		u8AssociatedPHSI;
-
-	/* Classification fields for ETH CS */
-	UCHAR		ucEthCSSrcMACLen;
-	UCHAR		au8EThCSSrcMAC[MAC_ADDRESS_SIZE];
-	UCHAR		au8EThCSSrcMACMask[MAC_ADDRESS_SIZE];
-	UCHAR		ucEthCSDestMACLen;
-	UCHAR		au8EThCSDestMAC[MAC_ADDRESS_SIZE];
-	UCHAR		au8EThCSDestMACMask[MAC_ADDRESS_SIZE];
-	UCHAR		ucEtherTypeLen;
-	UCHAR		au8EthCSEtherType[NUM_ETHERTYPE_BYTES];
-	UCHAR		usUserPriority[2];
-	USHORT		usVLANID;
-	USHORT		usValidityBitMap;
-};
-
-struct bcm_fragmented_packet_info {
-	bool			bUsed;
-	ULONG			ulSrcIpAddress;
-	USHORT			usIpIdentification;
-	struct bcm_classifier_rule *pstMatchedClassifierEntry;
-	bool			bOutOfOrderFragment;
-};
-
-struct bcm_packet_info {
-	/* classification extension Rule */
-	ULONG		ulSFID;
-	USHORT		usVCID_Value;
-	UINT		uiThreshold;
-	/* This field determines the priority of the SF Queues */
-	B_UINT8		u8TrafficPriority;
-
-	bool		bValid;
-	bool		bActive;
-	bool		bActivateRequestSent;
-
-	B_UINT8		u8QueueType; /* BE or rtPS */
-
-	/* maximum size of the bucket for the queue */
-	UINT		uiMaxBucketSize;
-	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;
-	struct bcm_mibs_parameters stMibsExtServiceFlowTable;
-	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;
-		};
-	};
-
-	bool		bProtocolValid;
-	bool		bTOSValid;
-	bool		bDestIpValid;
-	bool		bSrcIpValid;
-
-	bool		bActiveSet;
-	bool		bAdmittedSet;
-	bool		bAuthorizedSet;
-	bool		bClassifierPriority;
-	UCHAR		ucServiceClassName[MAX_CLASS_NAME_LENGTH];
-	bool		bHeaderSuppressionEnabled;
-	spinlock_t	SFQueueLock;
-	void		*pstSFIndication;
-	struct timeval	stLastUpdateTokenAt;
-	atomic_t	uiPerSFTxResourceCount;
-	UINT		uiMaxLatency;
-	UCHAR		bIPCSSupport;
-	UCHAR		bEthCSSupport;
-};
-
-struct bcm_tarang_data {
-	struct bcm_tarang_data	*next;
-	struct bcm_mini_adapter	*Adapter;
-	struct sk_buff		*RxAppControlHead;
-	struct sk_buff		*RxAppControlTail;
-	int			AppCtrlQueueLen;
-	bool			MacTracingEnabled;
-	bool			bApplicationToExit;
-	struct bcm_mibs_dropped_cntrl_msg stDroppedAppCntrlMsgs;
-	ULONG			RxCntrlMsgBitMask;
-};
-
-struct bcm_targetdsx_buffer {
-	ULONG		ulTargetDsxBuffer;
-	B_UINT16	tid;
-	bool		valid;
-};
-
-typedef int (*FP_FLASH_WRITE)(struct bcm_mini_adapter *, UINT, PVOID);
-
-typedef int (*FP_FLASH_WRITE_STATUS)(struct bcm_mini_adapter *, UINT, PVOID);
-
-/*
- * Driver adapter data structure
- */
-struct bcm_mini_adapter {
-	struct bcm_mini_adapter	*next;
-	struct net_device	*dev;
-	u32			msg_enable;
-	CHAR			*caDsxReqResp;
-	atomic_t		ApplicationRunning;
-	bool			AppCtrlQueueOverFlow;
-	atomic_t		CurrentApplicationCount;
-	atomic_t		RegisteredApplicationCount;
-	bool			LinkUpStatus;
-	bool			TimerActive;
-	u32			StatisticsPointer;
-	struct sk_buff		*RxControlHead;
-	struct sk_buff		*RxControlTail;
-	struct semaphore	RxAppControlQueuelock;
-	struct semaphore	fw_download_sema;
-	struct bcm_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;
-	struct bcm_packet_info	PackInfo[NO_OF_QUEUES];
-	struct bcm_classifier_rule astClassifierTable[MAX_CLASSIFIERS];
-	bool			TransferMode;
-
-	/*************** qos ******************/
-	bool			bETHCSEnabled;
-	ULONG			BEBucketSize;
-	ULONG			rtPSBucketSize;
-	UCHAR			LinkStatus;
-	bool			AutoLinkUp;
-	bool			AutoSyncup;
-
-	int			major;
-	int			minor;
-	wait_queue_head_t	tx_packet_wait_queue;
-	wait_queue_head_t	process_rx_cntrlpkt;
-	atomic_t		process_waiting;
-	bool			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;
-
-	struct bcm_targetdsx_buffer	astTargetDsxBuffer[MAX_TARGET_DSX_BUFFERS];
-	ULONG			ulFreeTargetBufferCnt;
-	ULONG			ulCurrentTargetBuffer;
-	ULONG			ulTotalTargetBuffersAvailable;
-	unsigned long		chip_id;
-	wait_queue_head_t	lowpower_mode_wait_queue;
-	bool			bFlashBoot;
-	bool			bBinDownloaded;
-	bool			bCfgDownloaded;
-	bool			bSyncUpRequestSent;
-	USHORT			usBestEffortQueueIndex;
-	wait_queue_head_t	ioctl_fw_dnld_wait_queue;
-	bool			waiting_to_fw_download_done;
-	pid_t			fw_download_process_pid;
-	struct bcm_target_params *pstargetparams;
-	bool			device_removed;
-	bool			DeviceAccess;
-	bool			bIsAutoCorrectEnabled;
-	bool			bDDRInitDone;
-	int			DDRSetting;
-	ULONG			ulPowerSaveMode;
-	spinlock_t		txtransmitlock;
-	B_UINT8			txtransmit_running;
-	/* Thread for control packet handling */
-	struct task_struct	*control_packet_handler;
-	/* thread for transmitting packets. */
-	struct task_struct	*transmit_packet_thread;
-
-	/* LED Related Structures */
-	struct bcm_led_info	LEDInfo;
-
-	/* Driver State for LED Blinking */
-	enum bcm_led_events	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);
-	int (*interface_transmit)(PVOID, PVOID , UINT);
-	bool			IdleMode;
-	bool			bDregRequestSentInIdleMode;
-	bool			bTriedToWakeUpFromlowPowerMode;
-	bool			bShutStatus;
-	bool			bWakeUpDevice;
-	unsigned int		usIdleModePattern;
-	/* BOOLEAN			bTriedToWakeUpFromShutdown; */
-	bool			bLinkDownRequested;
-	int			downloadDDR;
-	struct bcm_phs_extension stBCMPhsContext;
-	struct bcm_hdr_suppression_contextinfo stPhsTxContextInfo;
-	uint8_t			ucaPHSPktRestoreBuf[2048];
-	uint8_t			bPHSEnabled;
-	bool			AutoFirmDld;
-	bool			bMipsConfig;
-	bool			bDPLLConfig;
-	UINT32			aTxPktSizeHist[MIBS_MAX_HIST_ENTRIES];
-	UINT32			aRxPktSizeHist[MIBS_MAX_HIST_ENTRIES];
-	struct bcm_fragmented_packet_info
-		astFragmentedPktClassifierTable[MAX_FRAGMENTEDIP_CLASSIFICATION_ENTRIES];
-	atomic_t		uiMBupdate;
-	UINT32			PmuMode;
-	enum bcm_nvm_type	eNVMType;
-	UINT			uiSectorSize;
-	UINT			uiSectorSizeInCFG;
-	bool			bSectorSizeOverride;
-	bool			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.
-	 */
-	UINT			ulFlashCalStart;
-	ULONG			ulFlashControlSectionStart;
-	ULONG			ulFlashWriteSize;
-	ULONG			ulFlashID;
-	FP_FLASH_WRITE		fpFlashWrite;
-	FP_FLASH_WRITE_STATUS	fpFlashWriteWithStatusCheck;
-
-	struct semaphore	NVMRdmWrmLock;
-	struct device		*pstCreatedClassDevice;
-
-	/*	BOOLEAN				InterfaceUpStatus; */
-	struct bcm_flash2x_cs_info *psFlash2xCSInfo;
-	struct bcm_flash_cs_info *psFlashCSInfo;
-	struct bcm_flash2x_vendor_info *psFlash2xVendorInfo;
-	UINT			uiFlashBaseAdd; /* Flash start address */
-	/* Active ISO offset chosen before f/w download */
-	UINT			uiActiveISOOffset;
-	enum bcm_flash2x_section_val eActiveISO; /* Active ISO section val */
-	/* Active DSD val chosen before f/w download */
-	enum bcm_flash2x_section_val eActiveDSD;
-	/* For accessing Active DSD chosen before f/w download */
-	UINT			uiActiveDSDOffsetAtFwDld;
-	UINT			uiFlashLayoutMajorVersion;
-	UINT			uiFlashLayoutMinorVersion;
-	bool			bAllDSDWriteAllow;
-	bool			bSigCorrupted;
-	/* this should be set who so ever want to change the Headers.
-	 * after Write it should be reset immediately.
-	 */
-	bool			bHeaderChangeAllowed;
-	int			SelectedChip;
-	bool			bEndPointHalted;
-	/* while bFlashRawRead will be true, Driver
-	 * ignore map lay out and consider flash as of without any map.
-	 */
-	bool			bFlashRawRead;
-	bool			bPreparingForLowPowerMode;
-	bool			bDoSuspend;
-	UINT			syscfgBefFwDld;
-	bool			StopAllXaction;
-	/* Used to Support extended CAPI requirements from */
-	UINT32			liTimeSinceLastNetEntry;
-	struct semaphore	LowPowerModeSync;
-	ULONG			liDrainCalculated;
-	UINT			gpioBitMap;
-	struct bcm_debug_state	stDebugState;
-};
-
-#define GET_BCM_ADAPTER(net_dev) netdev_priv(net_dev)
-
-struct bcm_eth_header {
-	UCHAR	au8DestinationAddress[6];
-	UCHAR	au8SourceAddress[6];
-	USHORT	u16Etype;
-} __packed;
-
-struct bcm_firmware_info {
-	void	__user *pvMappedFirmwareAddress;
-	ULONG	u32FirmwareLength;
-	ULONG	u32StartingAddress;
-} __packed;
-
-/* holds the value of net_device structure.. */
-extern struct net_device *gblpnetdev;
-
-struct bcm_ddr_setting {
-	UINT ulRegAddress;
-	UINT ulRegValue;
-};
-int InitAdapter(struct bcm_mini_adapter *psAdapter);
-
-/* =====================================================================
- * Beceem vendor request codes for EP0
- * =====================================================================
- */
-
-#define BCM_REQUEST_READ	0x2
-#define BCM_REQUEST_WRITE	0x1
-#define EP2_MPS_REG		0x0F0110A0
-#define EP2_MPS			0x40
-
-#define EP2_CFG_REG	0x0F0110A8
-#define EP2_CFG_INT	0x27
-#define EP2_CFG_BULK	0x25
-
-#define EP4_MPS_REG	0x0F0110F0
-#define EP4_MPS		0x8C
-
-#define EP4_CFG_REG	0x0F0110F8
-
-#define ISO_MPS_REG	0x0F0110C8
-#define ISO_MPS		0x00000000
-
-#define EP1 0
-#define EP2 1
-#define EP3 2
-#define EP4 3
-#define EP5 4
-#define EP6 5
-
-enum bcm_einterface_setting {
-	DEFAULT_SETTING_0  = 0,
-	ALTERNATE_SETTING_1 = 1,
-};
-
-#endif	/* __ADAPTER_H__ */
diff --git a/drivers/staging/bcm/Bcmchar.c b/drivers/staging/bcm/Bcmchar.c
deleted file mode 100644
index 88ce2da531c6..000000000000
--- a/drivers/staging/bcm/Bcmchar.c
+++ /dev/null
@@ -1,2652 +0,0 @@
-#include <linux/fs.h>
-
-#include "headers.h"
-
-static int bcm_handle_nvm_read_cmd(struct bcm_mini_adapter *ad,
-				   PUCHAR read_data,
-				   struct bcm_nvm_readwrite *nvm_rw)
-{
-	INT status = STATUS_FAILURE;
-
-	down(&ad->NVMRdmWrmLock);
-
-	if ((ad->IdleMode == TRUE) || (ad->bShutStatus == TRUE) ||
-			(ad->bPreparingForLowPowerMode == TRUE)) {
-
-		BCM_DEBUG_PRINT(ad,
-			DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"Device is in Idle/Shutdown Mode\n");
-		up(&ad->NVMRdmWrmLock);
-		kfree(read_data);
-		return -EACCES;
-	}
-
-	status = BeceemNVMRead(ad, (PUINT)read_data,
-			       nvm_rw->uiOffset,
-			       nvm_rw->uiNumBytes);
-	up(&ad->NVMRdmWrmLock);
-
-	if (status != STATUS_SUCCESS) {
-		kfree(read_data);
-		return status;
-	}
-
-	if (copy_to_user(nvm_rw->pBuffer, read_data, nvm_rw->uiNumBytes)) {
-		kfree(read_data);
-		return -EFAULT;
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int handle_flash2x_adapter(struct bcm_mini_adapter *ad,
-				  PUCHAR read_data,
-				  struct bcm_nvm_readwrite *nvm_rw)
-{
-	/*
-	 * New Requirement:-
-	 * DSD section updation will be allowed in two case:-
-	 * 1.  if DSD sig is present in DSD header means dongle
-	 * is ok and updation is fruitfull
-	 * 2.  if point 1 failes then user buff should have
-	 * DSD sig. this point ensures that if dongle is
-	 * corrupted then user space program first modify
-	 * the DSD header with valid DSD sig so that this
-	 * as well as further write may be worthwhile.
-	 *
-	 * This restriction has been put assuming that
-	 * if DSD sig is corrupted, DSD data won't be
-	 * considered valid.
-	 */
-	INT status;
-	ULONG dsd_magic_num_in_usr_buff = 0;
-
-	status = BcmFlash2xCorruptSig(ad, ad->eActiveDSD);
-	if (status == STATUS_SUCCESS)
-		return STATUS_SUCCESS;
-
-	if (((nvm_rw->uiOffset + nvm_rw->uiNumBytes) !=
-			ad->uiNVMDSDSize) ||
-			(nvm_rw->uiNumBytes < SIGNATURE_SIZE)) {
-
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"DSD Sig is present neither in Flash nor User provided Input..");
-		up(&ad->NVMRdmWrmLock);
-		kfree(read_data);
-		return status;
-	}
-
-	dsd_magic_num_in_usr_buff =
-		ntohl(*(PUINT)(read_data + nvm_rw->uiNumBytes -
-		      SIGNATURE_SIZE));
-	if (dsd_magic_num_in_usr_buff != DSD_IMAGE_MAGIC_NUMBER) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"DSD Sig is present neither in Flash nor User provided Input..");
-		up(&ad->NVMRdmWrmLock);
-		kfree(read_data);
-		return status;
-	}
-
-	return STATUS_SUCCESS;
-}
-
-/***************************************************************
-* Function	  - bcm_char_open()
-*
-* Description - This is the "open" entry point for the character
-*				driver.
-*
-* Parameters  - inode: Pointer to the Inode structure of char device
-*				filp : File pointer of the char device
-*
-* Returns	  - Zero(Success)
-****************************************************************/
-
-static int bcm_char_open(struct inode *inode, struct file *filp)
-{
-	struct bcm_mini_adapter *ad = NULL;
-	struct bcm_tarang_data *tarang = NULL;
-
-	ad = GET_BCM_ADAPTER(gblpnetdev);
-	tarang = kzalloc(sizeof(struct bcm_tarang_data), GFP_KERNEL);
-	if (!tarang)
-		return -ENOMEM;
-
-	tarang->Adapter = ad;
-	tarang->RxCntrlMsgBitMask = 0xFFFFFFFF & ~(1 << 0xB);
-
-	down(&ad->RxAppControlQueuelock);
-	tarang->next = ad->pTarangs;
-	ad->pTarangs = tarang;
-	up(&ad->RxAppControlQueuelock);
-
-	/* Store the Adapter structure */
-	filp->private_data = tarang;
-
-	/* Start Queuing the control response Packets */
-	atomic_inc(&ad->ApplicationRunning);
-
-	nonseekable_open(inode, filp);
-	return 0;
-}
-
-static int bcm_char_release(struct inode *inode, struct file *filp)
-{
-	struct bcm_tarang_data *tarang, *tmp, *ptmp;
-	struct bcm_mini_adapter *ad = NULL;
-	struct sk_buff *pkt, *npkt;
-
-	tarang = (struct bcm_tarang_data *)filp->private_data;
-
-	if (tarang == NULL)
-		return 0;
-
-	ad = tarang->Adapter;
-
-	down(&ad->RxAppControlQueuelock);
-
-	tmp = ad->pTarangs;
-	for (ptmp = NULL; tmp; ptmp = tmp, tmp = tmp->next) {
-		if (tmp == tarang)
-			break;
-	}
-
-	if (tmp) {
-		if (!ptmp)
-			ad->pTarangs = tmp->next;
-		else
-			ptmp->next = tmp->next;
-	} else {
-		up(&ad->RxAppControlQueuelock);
-		return 0;
-	}
-
-	pkt = tarang->RxAppControlHead;
-	while (pkt) {
-		npkt = pkt->next;
-		kfree_skb(pkt);
-		pkt = npkt;
-	}
-
-	up(&ad->RxAppControlQueuelock);
-
-	/* Stop Queuing the control response Packets */
-	atomic_dec(&ad->ApplicationRunning);
-
-	kfree(tarang);
-
-	/* remove this filp from the asynchronously notified filp's */
-	filp->private_data = NULL;
-	return 0;
-}
-
-static ssize_t bcm_char_read(struct file *filp,
-			     char __user *buf,
-			     size_t size,
-			     loff_t *f_pos)
-{
-	struct bcm_tarang_data *tarang = filp->private_data;
-	struct bcm_mini_adapter *ad = tarang->Adapter;
-	struct sk_buff *packet = NULL;
-	ssize_t pkt_len = 0;
-	int wait_ret_val = 0;
-	unsigned long ret = 0;
-
-	wait_ret_val = wait_event_interruptible(
-				ad->process_read_wait_queue,
-				(tarang->RxAppControlHead ||
-				ad->device_removed));
-
-	if ((wait_ret_val == -ERESTARTSYS)) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"Exiting as i've been asked to exit!!!\n");
-		return wait_ret_val;
-	}
-
-	if (ad->device_removed) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"Device Removed... Killing the Apps...\n");
-		return -ENODEV;
-	}
-
-	if (false == ad->fw_download_done)
-		return -EACCES;
-
-	down(&ad->RxAppControlQueuelock);
-
-	if (tarang->RxAppControlHead) {
-		packet = tarang->RxAppControlHead;
-		DEQUEUEPACKET(tarang->RxAppControlHead,
-			      tarang->RxAppControlTail);
-		tarang->AppCtrlQueueLen--;
-	}
-
-	up(&ad->RxAppControlQueuelock);
-
-	if (packet) {
-		pkt_len = packet->len;
-		ret = copy_to_user(buf, packet->data,
-				   min_t(size_t, pkt_len, size));
-		if (ret) {
-			dev_kfree_skb(packet);
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-					"Returning from copy to user failure\n");
-			return -EFAULT;
-		}
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"Read %zd Bytes From Adapter packet = %p by process %d!\n",
-				pkt_len, packet, current->pid);
-		dev_kfree_skb(packet);
-	}
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL, "<\n");
-	return pkt_len;
-}
-
-static int bcm_char_ioctl_reg_read_private(void __user *argp,
-					   struct bcm_mini_adapter *ad)
-{
-	struct bcm_rdm_buffer rdm_buff = {0};
-	struct bcm_ioctl_buffer io_buff;
-	PCHAR temp_buff;
-	INT status = STATUS_FAILURE;
-	UINT buff_len;
-	u16 temp_value;
-	int bytes;
-
-	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (io_buff.InputLength > sizeof(rdm_buff))
-		return -EINVAL;
-
-	if (copy_from_user(&rdm_buff, io_buff.InputBuffer,
-		io_buff.InputLength))
-		return -EFAULT;
-
-	if (io_buff.OutputLength > USHRT_MAX ||
-		io_buff.OutputLength == 0) {
-		return -EINVAL;
-	}
-
-	buff_len = io_buff.OutputLength;
-	temp_value = 4 - (buff_len % 4);
-	buff_len += temp_value % 4;
-
-	temp_buff = kmalloc(buff_len, GFP_KERNEL);
-	if (!temp_buff)
-		return -ENOMEM;
-
-	bytes = rdmalt(ad, (UINT)rdm_buff.Register,
-			(PUINT)temp_buff, buff_len);
-	if (bytes > 0) {
-		status = STATUS_SUCCESS;
-		if (copy_to_user(io_buff.OutputBuffer, temp_buff, bytes)) {
-			kfree(temp_buff);
-			return -EFAULT;
-		}
-	} else {
-		status = bytes;
-	}
-
-	kfree(temp_buff);
-	return status;
-}
-
-static int bcm_char_ioctl_reg_write_private(void __user *argp,
-					    struct bcm_mini_adapter *ad)
-{
-	struct bcm_wrm_buffer wrm_buff = {0};
-	struct bcm_ioctl_buffer io_buff;
-	UINT tmp = 0;
-	INT status;
-
-	/* Copy Ioctl Buffer structure */
-
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (io_buff.InputLength > sizeof(wrm_buff))
-		return -EINVAL;
-
-	/* Get WrmBuffer structure */
-	if (copy_from_user(&wrm_buff, io_buff.InputBuffer,
-		io_buff.InputLength))
-		return -EFAULT;
-
-	tmp = wrm_buff.Register & EEPROM_REJECT_MASK;
-	if (!((ad->pstargetparams->m_u32Customize) & VSG_MODE) &&
-		((tmp == EEPROM_REJECT_REG_1) ||
-			(tmp == EEPROM_REJECT_REG_2) ||
-			(tmp == EEPROM_REJECT_REG_3) ||
-			(tmp == EEPROM_REJECT_REG_4))) {
-
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"EEPROM Access Denied, not in VSG Mode\n");
-		return -EFAULT;
-	}
-
-	status = wrmalt(ad, (UINT)wrm_buff.Register,
-			(PUINT)wrm_buff.Data, sizeof(ULONG));
-
-	if (status == STATUS_SUCCESS) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
-				DBG_LVL_ALL, "WRM Done\n");
-	} else {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
-				DBG_LVL_ALL, "WRM Failed\n");
-		status = -EFAULT;
-	}
-	return status;
-}
-
-static int bcm_char_ioctl_eeprom_reg_read(void __user *argp,
-					  struct bcm_mini_adapter *ad)
-{
-	struct bcm_rdm_buffer rdm_buff = {0};
-	struct bcm_ioctl_buffer io_buff;
-	PCHAR temp_buff = NULL;
-	UINT tmp = 0;
-	INT status;
-	int bytes;
-
-	if ((ad->IdleMode == TRUE) ||
-		(ad->bShutStatus == TRUE) ||
-		(ad->bPreparingForLowPowerMode == TRUE)) {
-
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Device in Idle Mode, Blocking Rdms\n");
-		return -EACCES;
-	}
-
-	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (io_buff.InputLength > sizeof(rdm_buff))
-		return -EINVAL;
-
-	if (copy_from_user(&rdm_buff, io_buff.InputBuffer,
-		io_buff.InputLength))
-		return -EFAULT;
-
-	if (io_buff.OutputLength > USHRT_MAX ||
-		io_buff.OutputLength == 0) {
-		return -EINVAL;
-	}
-
-	temp_buff = kmalloc(io_buff.OutputLength, GFP_KERNEL);
-	if (!temp_buff)
-		return STATUS_FAILURE;
-
-	if ((((ULONG)rdm_buff.Register & 0x0F000000) != 0x0F000000) ||
-		((ULONG)rdm_buff.Register & 0x3)) {
-
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"RDM Done On invalid Address : %x Access Denied.\n",
-				(int)rdm_buff.Register);
-
-		kfree(temp_buff);
-		return -EINVAL;
-	}
-
-	tmp = rdm_buff.Register & EEPROM_REJECT_MASK;
-	bytes = rdmaltWithLock(ad, (UINT)rdm_buff.Register,
-			       (PUINT)temp_buff, io_buff.OutputLength);
-
-	if (bytes > 0) {
-		status = STATUS_SUCCESS;
-		if (copy_to_user(io_buff.OutputBuffer, temp_buff, bytes)) {
-			kfree(temp_buff);
-			return -EFAULT;
-		}
-	} else {
-		status = bytes;
-	}
-
-	kfree(temp_buff);
-	return status;
-}
-
-static int bcm_char_ioctl_eeprom_reg_write(void __user *argp,
-					   struct bcm_mini_adapter *ad,
-					   UINT cmd)
-{
-	struct bcm_wrm_buffer wrm_buff = {0};
-	struct bcm_ioctl_buffer io_buff;
-	UINT tmp = 0;
-	INT status;
-
-	if ((ad->IdleMode == TRUE) ||
-		(ad->bShutStatus == TRUE) ||
-		(ad->bPreparingForLowPowerMode == TRUE)) {
-
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Device in Idle Mode, Blocking Wrms\n");
-		return -EACCES;
-	}
-
-	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (io_buff.InputLength > sizeof(wrm_buff))
-		return -EINVAL;
-
-	/* Get WrmBuffer structure */
-	if (copy_from_user(&wrm_buff, io_buff.InputBuffer,
-		io_buff.InputLength))
-		return -EFAULT;
-
-	if ((((ULONG)wrm_buff.Register & 0x0F000000) != 0x0F000000) ||
-		((ULONG)wrm_buff.Register & 0x3)) {
-
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"WRM Done On invalid Address : %x Access Denied.\n",
-				(int)wrm_buff.Register);
-		return -EINVAL;
-	}
-
-	tmp = wrm_buff.Register & EEPROM_REJECT_MASK;
-	if (!((ad->pstargetparams->m_u32Customize) & VSG_MODE) &&
-			((tmp == EEPROM_REJECT_REG_1) ||
-			(tmp == EEPROM_REJECT_REG_2) ||
-			(tmp == EEPROM_REJECT_REG_3) ||
-			(tmp == EEPROM_REJECT_REG_4)) &&
-			(cmd == IOCTL_BCM_REGISTER_WRITE)) {
-
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-					"EEPROM Access Denied, not in VSG Mode\n");
-			return -EFAULT;
-	}
-
-	status = wrmaltWithLock(ad, (UINT)wrm_buff.Register,
-				(PUINT)wrm_buff.Data,
-				wrm_buff.Length);
-
-	if (status == STATUS_SUCCESS) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, OSAL_DBG,
-				DBG_LVL_ALL, "WRM Done\n");
-	} else {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
-				DBG_LVL_ALL, "WRM Failed\n");
-		status = -EFAULT;
-	}
-	return status;
-}
-
-static int bcm_char_ioctl_gpio_set_request(void __user *argp,
-					   struct bcm_mini_adapter *ad)
-{
-	struct bcm_gpio_info gpio_info = {0};
-	struct bcm_ioctl_buffer io_buff;
-	UCHAR reset_val[4];
-	UINT value = 0;
-	UINT bit = 0;
-	UINT operation = 0;
-	INT status;
-	int bytes;
-
-	if ((ad->IdleMode == TRUE) ||
-		(ad->bShutStatus == TRUE) ||
-		(ad->bPreparingForLowPowerMode == TRUE)) {
-
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
-				DBG_LVL_ALL,
-				"GPIO Can't be set/clear in Low power Mode");
-		return -EACCES;
-	}
-
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (io_buff.InputLength > sizeof(gpio_info))
-		return -EINVAL;
-
-	if (copy_from_user(&gpio_info, io_buff.InputBuffer,
-			   io_buff.InputLength))
-		return -EFAULT;
-
-	bit  = gpio_info.uiGpioNumber;
-	operation = gpio_info.uiGpioValue;
-	value = (1<<bit);
-
-	if (IsReqGpioIsLedInNVM(ad, value) == false) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
-				DBG_LVL_ALL,
-				"Sorry, Requested GPIO<0x%X> is not correspond to LED !!!",
-				value);
-		return -EINVAL;
-	}
-
-	/* Set - setting 1 */
-	if (operation) {
-		/* Set the gpio output register */
-		status = wrmaltWithLock(ad,
-					BCM_GPIO_OUTPUT_SET_REG,
-					(PUINT)(&value), sizeof(UINT));
-
-		if (status == STATUS_SUCCESS) {
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS,
-					OSAL_DBG, DBG_LVL_ALL,
-					"Set the GPIO bit\n");
-		} else {
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS,
-					OSAL_DBG, DBG_LVL_ALL,
-					"Failed to set the %dth GPIO\n",
-					bit);
-			return status;
-		}
-	} else {
-		/* Set the gpio output register */
-		status = wrmaltWithLock(ad,
-					BCM_GPIO_OUTPUT_CLR_REG,
-					(PUINT)(&value), sizeof(UINT));
-
-		if (status == STATUS_SUCCESS) {
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS,
-					OSAL_DBG, DBG_LVL_ALL,
-					"Set the GPIO bit\n");
-		} else {
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS,
-					OSAL_DBG, DBG_LVL_ALL,
-					"Failed to clear the %dth GPIO\n",
-					bit);
-			return status;
-		}
-	}
-
-	bytes = rdmaltWithLock(ad, (UINT)GPIO_MODE_REGISTER,
-			       (PUINT)reset_val, sizeof(UINT));
-	if (bytes < 0) {
-		status = bytes;
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"GPIO_MODE_REGISTER read failed");
-		return status;
-	}
-	status = STATUS_SUCCESS;
-
-	/* Set the gpio mode register to output */
-	*(UINT *)reset_val |= (1<<bit);
-	status = wrmaltWithLock(ad, GPIO_MODE_REGISTER,
-				(PUINT)reset_val, sizeof(UINT));
-
-	if (status == STATUS_SUCCESS) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
-				DBG_LVL_ALL,
-				"Set the GPIO to output Mode\n");
-	} else {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
-				DBG_LVL_ALL,
-				"Failed to put GPIO in Output Mode\n");
-	}
-
-	return status;
-}
-
-static int bcm_char_ioctl_led_thread_state_change_req(void __user *argp,
-		struct bcm_mini_adapter *ad)
-{
-	struct bcm_user_thread_req thread_req = {0};
-	struct bcm_ioctl_buffer io_buff;
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"User made LED thread InActive");
-
-	if ((ad->IdleMode == TRUE) ||
-		(ad->bShutStatus == TRUE) ||
-		(ad->bPreparingForLowPowerMode == TRUE)) {
-
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
-				DBG_LVL_ALL,
-				"GPIO Can't be set/clear in Low power Mode");
-		return -EACCES;
-	}
-
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (io_buff.InputLength > sizeof(thread_req))
-		return -EINVAL;
-
-	if (copy_from_user(&thread_req, io_buff.InputBuffer,
-			   io_buff.InputLength))
-		return -EFAULT;
-
-	/* if LED thread is running(Actively or Inactively)
-	 * set it state to make inactive
-	 */
-	if (ad->LEDInfo.led_thread_running) {
-		if (thread_req.ThreadState == LED_THREAD_ACTIVATION_REQ) {
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS,
-					OSAL_DBG, DBG_LVL_ALL,
-					"Activating thread req");
-			ad->DriverState = LED_THREAD_ACTIVE;
-		} else {
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS,
-					OSAL_DBG, DBG_LVL_ALL,
-					"DeActivating Thread req.....");
-			ad->DriverState = LED_THREAD_INACTIVE;
-		}
-
-		/* signal thread. */
-		wake_up(&ad->LEDInfo.notify_led_event);
-	}
-	return STATUS_SUCCESS;
-}
-
-static int bcm_char_ioctl_gpio_status_request(void __user *argp,
-					      struct bcm_mini_adapter *ad)
-{
-	struct bcm_gpio_info gpio_info = {0};
-	struct bcm_ioctl_buffer io_buff;
-	ULONG bit = 0;
-	UCHAR read[4];
-	INT status;
-	int bytes;
-
-	if ((ad->IdleMode == TRUE) ||
-		(ad->bShutStatus == TRUE) ||
-		(ad->bPreparingForLowPowerMode == TRUE))
-		return -EACCES;
-
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (io_buff.InputLength > sizeof(gpio_info))
-		return -EINVAL;
-
-	if (copy_from_user(&gpio_info, io_buff.InputBuffer,
-		io_buff.InputLength))
-		return -EFAULT;
-
-	bit = gpio_info.uiGpioNumber;
-
-	/* Set the gpio output register */
-	bytes = rdmaltWithLock(ad, (UINT)GPIO_PIN_STATE_REGISTER,
-				(PUINT)read, sizeof(UINT));
-
-	if (bytes < 0) {
-		status = bytes;
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"RDM Failed\n");
-		return status;
-	}
-	status = STATUS_SUCCESS;
-	return status;
-}
-
-static int bcm_char_ioctl_gpio_multi_request(void __user *argp,
-					     struct bcm_mini_adapter *ad)
-{
-	struct bcm_gpio_multi_info gpio_multi_info[MAX_IDX];
-	struct bcm_gpio_multi_info *pgpio_multi_info =
-		(struct bcm_gpio_multi_info *)gpio_multi_info;
-	struct bcm_ioctl_buffer io_buff;
-	UCHAR reset_val[4];
-	INT status = STATUS_FAILURE;
-	int bytes;
-
-	memset(pgpio_multi_info, 0,
-	       MAX_IDX * sizeof(struct bcm_gpio_multi_info));
-
-	if ((ad->IdleMode == TRUE) ||
-		(ad->bShutStatus == TRUE) ||
-		(ad->bPreparingForLowPowerMode == TRUE))
-		return -EINVAL;
-
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (io_buff.InputLength > sizeof(gpio_multi_info))
-		return -EINVAL;
-	if (io_buff.OutputLength > sizeof(gpio_multi_info))
-		io_buff.OutputLength = sizeof(gpio_multi_info);
-
-	if (copy_from_user(&gpio_multi_info, io_buff.InputBuffer,
-			   io_buff.InputLength))
-		return -EFAULT;
-
-	if (IsReqGpioIsLedInNVM(ad, pgpio_multi_info[WIMAX_IDX].uiGPIOMask)
-			== false) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
-				DBG_LVL_ALL,
-				"Sorry, Requested GPIO<0x%X> is not correspond to NVM LED bit map<0x%X>!!!",
-				pgpio_multi_info[WIMAX_IDX].uiGPIOMask,
-				ad->gpioBitMap);
-		return -EINVAL;
-	}
-
-	/* Set the gpio output register */
-	if ((pgpio_multi_info[WIMAX_IDX].uiGPIOMask) &
-		(pgpio_multi_info[WIMAX_IDX].uiGPIOCommand)) {
-		/* Set 1's in GPIO OUTPUT REGISTER */
-		*(UINT *)reset_val = pgpio_multi_info[WIMAX_IDX].uiGPIOMask &
-			pgpio_multi_info[WIMAX_IDX].uiGPIOCommand &
-			pgpio_multi_info[WIMAX_IDX].uiGPIOValue;
-
-		if (*(UINT *) reset_val)
-			status = wrmaltWithLock(ad,
-				BCM_GPIO_OUTPUT_SET_REG,
-				(PUINT)reset_val, sizeof(ULONG));
-
-		if (status != STATUS_SUCCESS) {
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"WRM to BCM_GPIO_OUTPUT_SET_REG Failed.");
-			return status;
-		}
-
-		/* Clear to 0's in GPIO OUTPUT REGISTER */
-		*(UINT *)reset_val =
-			(pgpio_multi_info[WIMAX_IDX].uiGPIOMask &
-			pgpio_multi_info[WIMAX_IDX].uiGPIOCommand &
-			(~(pgpio_multi_info[WIMAX_IDX].uiGPIOValue)));
-
-		if (*(UINT *) reset_val)
-			status = wrmaltWithLock(ad,
-				BCM_GPIO_OUTPUT_CLR_REG, (PUINT)reset_val,
-				sizeof(ULONG));
-
-		if (status != STATUS_SUCCESS) {
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-					"WRM to BCM_GPIO_OUTPUT_CLR_REG Failed.");
-			return status;
-		}
-	}
-
-	if (pgpio_multi_info[WIMAX_IDX].uiGPIOMask) {
-		bytes = rdmaltWithLock(ad, (UINT)GPIO_PIN_STATE_REGISTER,
-				       (PUINT)reset_val, sizeof(UINT));
-
-		if (bytes < 0) {
-			status = bytes;
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-					"RDM to GPIO_PIN_STATE_REGISTER Failed.");
-			return status;
-		}
-		status = STATUS_SUCCESS;
-
-		pgpio_multi_info[WIMAX_IDX].uiGPIOValue =
-			(*(UINT *)reset_val &
-			pgpio_multi_info[WIMAX_IDX].uiGPIOMask);
-	}
-
-	status = copy_to_user(io_buff.OutputBuffer, &gpio_multi_info,
-		io_buff.OutputLength);
-	if (status) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-			"Failed while copying Content to IOBufer for user space err:%d",
-			status);
-		return -EFAULT;
-	}
-	return status;
-}
-
-static int bcm_char_ioctl_gpio_mode_request(void __user *argp,
-					    struct bcm_mini_adapter *ad)
-{
-	struct bcm_gpio_multi_mode gpio_multi_mode[MAX_IDX];
-	struct bcm_gpio_multi_mode *pgpio_multi_mode =
-		(struct bcm_gpio_multi_mode *)gpio_multi_mode;
-	struct bcm_ioctl_buffer io_buff;
-	UCHAR reset_val[4];
-	INT status;
-	int bytes;
-
-	if ((ad->IdleMode == TRUE) ||
-		(ad->bShutStatus == TRUE) ||
-		(ad->bPreparingForLowPowerMode == TRUE))
-		return -EINVAL;
-
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (io_buff.InputLength > sizeof(gpio_multi_mode))
-		return -EINVAL;
-	if (io_buff.OutputLength > sizeof(gpio_multi_mode))
-		io_buff.OutputLength = sizeof(gpio_multi_mode);
-
-	if (copy_from_user(&gpio_multi_mode, io_buff.InputBuffer,
-		io_buff.InputLength))
-		return -EFAULT;
-
-	bytes = rdmaltWithLock(ad, (UINT)GPIO_MODE_REGISTER,
-		(PUINT)reset_val, sizeof(UINT));
-
-	if (bytes < 0) {
-		status = bytes;
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-			"Read of GPIO_MODE_REGISTER failed");
-		return status;
-	}
-	status = STATUS_SUCCESS;
-
-	/* Validating the request */
-	if (IsReqGpioIsLedInNVM(ad, pgpio_multi_mode[WIMAX_IDX].uiGPIOMask)
-			== false) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"Sorry, Requested GPIO<0x%X> is not correspond to NVM LED bit map<0x%X>!!!",
-				pgpio_multi_mode[WIMAX_IDX].uiGPIOMask,
-				ad->gpioBitMap);
-		return -EINVAL;
-	}
-
-	if (pgpio_multi_mode[WIMAX_IDX].uiGPIOMask) {
-		/* write all OUT's (1's) */
-		*(UINT *) reset_val |=
-			(pgpio_multi_mode[WIMAX_IDX].uiGPIOMode &
-					pgpio_multi_mode[WIMAX_IDX].uiGPIOMask);
-
-		/* write all IN's (0's) */
-		*(UINT *) reset_val &=
-			~((~pgpio_multi_mode[WIMAX_IDX].uiGPIOMode) &
-					pgpio_multi_mode[WIMAX_IDX].uiGPIOMask);
-
-		/* Currently implemented return the modes of all GPIO's
-		 * else needs to bit AND with  mask
-		 */
-		pgpio_multi_mode[WIMAX_IDX].uiGPIOMode = *(UINT *)reset_val;
-
-		status = wrmaltWithLock(ad, GPIO_MODE_REGISTER,
-			(PUINT)reset_val, sizeof(ULONG));
-		if (status == STATUS_SUCCESS) {
-			BCM_DEBUG_PRINT(ad,
-				DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"WRM to GPIO_MODE_REGISTER Done");
-		} else {
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-					"WRM to GPIO_MODE_REGISTER Failed");
-			return -EFAULT;
-		}
-	} else {
-		/* if uiGPIOMask is 0 then return mode register configuration */
-		pgpio_multi_mode[WIMAX_IDX].uiGPIOMode = *(UINT *)reset_val;
-	}
-
-	status = copy_to_user(io_buff.OutputBuffer, &gpio_multi_mode,
-		io_buff.OutputLength);
-	if (status) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-			"Failed while copying Content to IOBufer for user space err:%d",
-			status);
-		return -EFAULT;
-	}
-	return status;
-}
-
-static int bcm_char_ioctl_misc_request(void __user *argp,
-				       struct bcm_mini_adapter *ad)
-{
-	struct bcm_ioctl_buffer io_buff;
-	PVOID buff = NULL;
-	INT status;
-
-	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (io_buff.InputLength < sizeof(struct bcm_link_request))
-		return -EINVAL;
-
-	if (io_buff.InputLength > MAX_CNTL_PKT_SIZE)
-		return -EINVAL;
-
-	buff = memdup_user(io_buff.InputBuffer,
-			       io_buff.InputLength);
-	if (IS_ERR(buff))
-		return PTR_ERR(buff);
-
-	down(&ad->LowPowerModeSync);
-	status = wait_event_interruptible_timeout(
-			ad->lowpower_mode_wait_queue,
-			!ad->bPreparingForLowPowerMode,
-			(1 * HZ));
-
-	if (status == -ERESTARTSYS)
-		goto cntrlEnd;
-
-	if (ad->bPreparingForLowPowerMode) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"Preparing Idle Mode is still True - Hence Rejecting control message\n");
-		status = STATUS_FAILURE;
-		goto cntrlEnd;
-	}
-	status = CopyBufferToControlPacket(ad, (PVOID)buff);
-
-cntrlEnd:
-	up(&ad->LowPowerModeSync);
-	kfree(buff);
-	return status;
-}
-
-static int bcm_char_ioctl_buffer_download_start(
-		struct bcm_mini_adapter *ad)
-{
-	INT status;
-
-	if (down_trylock(&ad->NVMRdmWrmLock)) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"IOCTL_BCM_CHIP_RESET not allowed as EEPROM Read/Write is in progress\n");
-		return -EACCES;
-	}
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-			"Starting the firmware download PID =0x%x!!!!\n",
-			current->pid);
-
-	if (down_trylock(&ad->fw_download_sema))
-		return -EBUSY;
-
-	ad->bBinDownloaded = false;
-	ad->fw_download_process_pid = current->pid;
-	ad->bCfgDownloaded = false;
-	ad->fw_download_done = false;
-	netif_carrier_off(ad->dev);
-	netif_stop_queue(ad->dev);
-	status = reset_card_proc(ad);
-	if (status) {
-		pr_err(PFX "%s: reset_card_proc Failed!\n", ad->dev->name);
-		up(&ad->fw_download_sema);
-		up(&ad->NVMRdmWrmLock);
-		return status;
-	}
-	mdelay(10);
-
-	up(&ad->NVMRdmWrmLock);
-	return status;
-}
-
-static int bcm_char_ioctl_buffer_download(void __user *argp,
-					  struct bcm_mini_adapter *ad)
-{
-	struct bcm_firmware_info *fw_info = NULL;
-	struct bcm_ioctl_buffer io_buff;
-	INT status;
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-		"Starting the firmware download PID =0x%x!!!!\n", current->pid);
-
-	if (!down_trylock(&ad->fw_download_sema)) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Invalid way to download buffer. Use Start and then call this!!!\n");
-		up(&ad->fw_download_sema);
-		return -EINVAL;
-	}
-
-	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer))) {
-		up(&ad->fw_download_sema);
-		return -EFAULT;
-	}
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-			"Length for FW DLD is : %lx\n", io_buff.InputLength);
-
-	if (io_buff.InputLength > sizeof(struct bcm_firmware_info)) {
-		up(&ad->fw_download_sema);
-		return -EINVAL;
-	}
-
-	fw_info = kmalloc(sizeof(*fw_info), GFP_KERNEL);
-	if (!fw_info) {
-		up(&ad->fw_download_sema);
-		return -ENOMEM;
-	}
-
-	if (copy_from_user(fw_info, io_buff.InputBuffer,
-		io_buff.InputLength)) {
-		up(&ad->fw_download_sema);
-		kfree(fw_info);
-		return -EFAULT;
-	}
-
-	if (!fw_info->pvMappedFirmwareAddress ||
-		(fw_info->u32FirmwareLength == 0)) {
-
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Something else is wrong %lu\n",
-				fw_info->u32FirmwareLength);
-		up(&ad->fw_download_sema);
-		kfree(fw_info);
-		status = -EINVAL;
-		return status;
-	}
-
-	status = bcm_ioctl_fw_download(ad, fw_info);
-
-	if (status != STATUS_SUCCESS) {
-		if (fw_info->u32StartingAddress == CONFIG_BEGIN_ADDR)
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"IOCTL: Configuration File Upload Failed\n");
-		else
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"IOCTL: Firmware File Upload Failed\n");
-
-		/* up(&ad->fw_download_sema); */
-
-		if (ad->LEDInfo.led_thread_running &
-			BCM_LED_THREAD_RUNNING_ACTIVELY) {
-			ad->DriverState = DRIVER_INIT;
-			ad->LEDInfo.bLedInitDone = false;
-			wake_up(&ad->LEDInfo.notify_led_event);
-		}
-	}
-
-	if (status != STATUS_SUCCESS)
-		up(&ad->fw_download_sema);
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, OSAL_DBG, DBG_LVL_ALL,
-		"IOCTL: Firmware File Uploaded\n");
-	kfree(fw_info);
-	return status;
-}
-
-static int bcm_char_ioctl_buffer_download_stop(void __user *argp,
-					       struct bcm_mini_adapter *ad)
-{
-	INT status;
-	int timeout = 0;
-
-	if (!down_trylock(&ad->fw_download_sema)) {
-		up(&ad->fw_download_sema);
-		return -EINVAL;
-	}
-
-	if (down_trylock(&ad->NVMRdmWrmLock)) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"FW download blocked as EEPROM Read/Write is in progress\n");
-		up(&ad->fw_download_sema);
-		return -EACCES;
-	}
-
-	ad->bBinDownloaded = TRUE;
-	ad->bCfgDownloaded = TRUE;
-	atomic_set(&ad->CurrNumFreeTxDesc, 0);
-	ad->CurrNumRecvDescs = 0;
-	ad->downloadDDR = 0;
-
-	/* setting the Mips to Run */
-	status = run_card_proc(ad);
-
-	if (status) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-			"Firm Download Failed\n");
-		up(&ad->fw_download_sema);
-		up(&ad->NVMRdmWrmLock);
-		return status;
-	}
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
-			DBG_LVL_ALL, "Firm Download Over...\n");
-
-	mdelay(10);
-
-	/* Wait for MailBox Interrupt */
-	if (StartInterruptUrb((struct bcm_interface_adapter *)ad->pvInterfaceAdapter))
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-			"Unable to send interrupt...\n");
-
-	timeout = 5*HZ;
-	ad->waiting_to_fw_download_done = false;
-	wait_event_timeout(ad->ioctl_fw_dnld_wait_queue,
-			ad->waiting_to_fw_download_done, timeout);
-	ad->fw_download_process_pid = INVALID_PID;
-	ad->fw_download_done = TRUE;
-	atomic_set(&ad->CurrNumFreeTxDesc, 0);
-	ad->CurrNumRecvDescs = 0;
-	ad->PrevNumRecvDescs = 0;
-	atomic_set(&ad->cntrlpktCnt, 0);
-	ad->LinkUpStatus = 0;
-	ad->LinkStatus = 0;
-
-	if (ad->LEDInfo.led_thread_running &
-		BCM_LED_THREAD_RUNNING_ACTIVELY) {
-		ad->DriverState = FW_DOWNLOAD_DONE;
-		wake_up(&ad->LEDInfo.notify_led_event);
-	}
-
-	if (!timeout)
-		status = -ENODEV;
-
-	up(&ad->fw_download_sema);
-	up(&ad->NVMRdmWrmLock);
-	return status;
-}
-
-static int bcm_char_ioctl_chip_reset(struct bcm_mini_adapter *ad)
-{
-	INT status;
-	INT nvm_access;
-
-	nvm_access = down_trylock(&ad->NVMRdmWrmLock);
-	if (nvm_access) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-			" IOCTL_BCM_CHIP_RESET not allowed as EEPROM Read/Write is in progress\n");
-		return -EACCES;
-	}
-
-	down(&ad->RxAppControlQueuelock);
-	status = reset_card_proc(ad);
-	flushAllAppQ();
-	up(&ad->RxAppControlQueuelock);
-	up(&ad->NVMRdmWrmLock);
-	ResetCounters(ad);
-	return status;
-}
-
-static int bcm_char_ioctl_qos_threshold(ULONG arg,
-					struct bcm_mini_adapter *ad)
-{
-	USHORT i;
-
-	for (i = 0; i < NO_OF_QUEUES; i++) {
-		if (get_user(ad->PackInfo[i].uiThreshold,
-				(unsigned long __user *)arg)) {
-			return -EFAULT;
-		}
-	}
-	return 0;
-}
-
-static int bcm_char_ioctl_switch_transfer_mode(void __user *argp,
-					       struct bcm_mini_adapter *ad)
-{
-	UINT data = 0;
-
-	if (copy_from_user(&data, argp, sizeof(UINT)))
-		return -EFAULT;
-
-	if (data) {
-		/* Allow All Packets */
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"IOCTL_BCM_SWITCH_TRANSFER_MODE: ETH_PACKET_TUNNELING_MODE\n");
-			ad->TransferMode = ETH_PACKET_TUNNELING_MODE;
-	} else {
-		/* Allow IP only Packets */
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"IOCTL_BCM_SWITCH_TRANSFER_MODE: IP_PACKET_ONLY_MODE\n");
-		ad->TransferMode = IP_PACKET_ONLY_MODE;
-	}
-	return STATUS_SUCCESS;
-}
-
-static int bcm_char_ioctl_get_driver_version(void __user *argp)
-{
-	struct bcm_ioctl_buffer io_buff;
-	ulong len;
-
-	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	len = min_t(ulong, io_buff.OutputLength, strlen(DRV_VERSION) + 1);
-
-	if (copy_to_user(io_buff.OutputBuffer, DRV_VERSION, len))
-		return -EFAULT;
-
-	return STATUS_SUCCESS;
-}
-
-static int bcm_char_ioctl_get_current_status(void __user *argp,
-					     struct bcm_mini_adapter *ad)
-{
-	struct bcm_link_state link_state;
-	struct bcm_ioctl_buffer io_buff;
-
-	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer))) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-			"copy_from_user failed..\n");
-		return -EFAULT;
-	}
-
-	if (io_buff.OutputLength != sizeof(link_state))
-		return -EINVAL;
-
-	memset(&link_state, 0, sizeof(link_state));
-	link_state.bIdleMode = ad->IdleMode;
-	link_state.bShutdownMode = ad->bShutStatus;
-	link_state.ucLinkStatus = ad->LinkStatus;
-
-	if (copy_to_user(io_buff.OutputBuffer, &link_state, min_t(size_t,
-		sizeof(link_state), io_buff.OutputLength))) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-			"Copy_to_user Failed..\n");
-		return -EFAULT;
-	}
-	return STATUS_SUCCESS;
-}
-
-
-static int bcm_char_ioctl_set_mac_tracing(void __user *argp,
-					  struct bcm_mini_adapter *ad)
-{
-	struct bcm_ioctl_buffer io_buff;
-	UINT tracing_flag;
-
-	/* copy ioctl Buffer structure */
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (copy_from_user(&tracing_flag, io_buff.InputBuffer, sizeof(UINT)))
-		return -EFAULT;
-
-	if (tracing_flag)
-		ad->pTarangs->MacTracingEnabled = TRUE;
-	else
-		ad->pTarangs->MacTracingEnabled = false;
-
-	return STATUS_SUCCESS;
-}
-
-static int bcm_char_ioctl_get_dsx_indication(void __user *argp,
-					     struct bcm_mini_adapter *ad)
-{
-	struct bcm_ioctl_buffer io_buff;
-	ULONG sf_id = 0;
-
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (io_buff.OutputLength < sizeof(struct bcm_add_indication_alt)) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-			"Mismatch req: %lx needed is =0x%zx!!!",
-			io_buff.OutputLength,
-			sizeof(struct bcm_add_indication_alt));
-		return -EINVAL;
-	}
-
-	if (copy_from_user(&sf_id, io_buff.InputBuffer, sizeof(sf_id)))
-		return -EFAULT;
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"Get DSX Data SF ID is =%lx\n", sf_id);
-	get_dsx_sf_data_to_application(ad, sf_id, io_buff.OutputBuffer);
-	return STATUS_SUCCESS;
-}
-
-static int bcm_char_ioctl_get_host_mibs(void __user *argp,
-					struct bcm_mini_adapter *ad,
-					struct bcm_tarang_data *tarang)
-{
-	struct bcm_ioctl_buffer io_buff;
-	INT status = STATUS_FAILURE;
-	PVOID temp_buff;
-
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (io_buff.OutputLength != sizeof(struct bcm_host_stats_mibs)) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-			"Length Check failed %lu %zd\n", io_buff.OutputLength,
-			sizeof(struct bcm_host_stats_mibs));
-		return -EINVAL;
-	}
-
-	/* FIXME: HOST_STATS are too big for kmalloc (122048)! */
-	temp_buff = kzalloc(sizeof(struct bcm_host_stats_mibs), GFP_KERNEL);
-	if (!temp_buff)
-		return STATUS_FAILURE;
-
-	status = ProcessGetHostMibs(ad, temp_buff);
-	GetDroppedAppCntrlPktMibs(temp_buff, tarang);
-
-	if (status != STATUS_FAILURE) {
-		if (copy_to_user(io_buff.OutputBuffer, temp_buff,
-			sizeof(struct bcm_host_stats_mibs))) {
-			kfree(temp_buff);
-			return -EFAULT;
-		}
-	}
-
-	kfree(temp_buff);
-	return status;
-}
-
-static int bcm_char_ioctl_bulk_wrm(void __user *argp,
-				   struct bcm_mini_adapter *ad, UINT cmd)
-{
-	struct bcm_bulk_wrm_buffer *bulk_buff;
-	struct bcm_ioctl_buffer io_buff;
-	UINT tmp = 0;
-	INT status = STATUS_FAILURE;
-	PCHAR buff = NULL;
-
-	if ((ad->IdleMode == TRUE) ||
-		(ad->bShutStatus == TRUE) ||
-		(ad->bPreparingForLowPowerMode == TRUE)) {
-
-		BCM_DEBUG_PRINT (ad, DBG_TYPE_PRINTK, 0, 0,
-			"Device in Idle/Shutdown Mode, Blocking Wrms\n");
-		return -EACCES;
-	}
-
-	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (io_buff.InputLength < sizeof(ULONG) * 2)
-		return -EINVAL;
-
-	buff = memdup_user(io_buff.InputBuffer,
-			       io_buff.InputLength);
-	if (IS_ERR(buff))
-		return PTR_ERR(buff);
-
-	bulk_buff = (struct bcm_bulk_wrm_buffer *)buff;
-
-	if (((ULONG)bulk_buff->Register & 0x0F000000) != 0x0F000000 ||
-		((ULONG)bulk_buff->Register & 0x3)) {
-		BCM_DEBUG_PRINT (ad, DBG_TYPE_PRINTK, 0, 0,
-			"WRM Done On invalid Address : %x Access Denied.\n",
-			(int)bulk_buff->Register);
-		kfree(buff);
-		return -EINVAL;
-	}
-
-	tmp = bulk_buff->Register & EEPROM_REJECT_MASK;
-	if (!((ad->pstargetparams->m_u32Customize)&VSG_MODE) &&
-		((tmp == EEPROM_REJECT_REG_1) ||
-			(tmp == EEPROM_REJECT_REG_2) ||
-			(tmp == EEPROM_REJECT_REG_3) ||
-			(tmp == EEPROM_REJECT_REG_4)) &&
-		(cmd == IOCTL_BCM_REGISTER_WRITE)) {
-
-		kfree(buff);
-		BCM_DEBUG_PRINT (ad, DBG_TYPE_PRINTK, 0, 0,
-			"EEPROM Access Denied, not in VSG Mode\n");
-		return -EFAULT;
-	}
-
-	if (bulk_buff->SwapEndian == false)
-		status = wrmWithLock(ad, (UINT)bulk_buff->Register,
-			(PCHAR)bulk_buff->Values,
-			io_buff.InputLength - 2*sizeof(ULONG));
-	else
-		status = wrmaltWithLock(ad, (UINT)bulk_buff->Register,
-			(PUINT)bulk_buff->Values,
-			io_buff.InputLength - 2*sizeof(ULONG));
-
-	if (status != STATUS_SUCCESS)
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0, "WRM Failed\n");
-
-	kfree(buff);
-	return status;
-}
-
-static int bcm_char_ioctl_get_nvm_size(void __user *argp,
-				       struct bcm_mini_adapter *ad)
-{
-	struct bcm_ioctl_buffer io_buff;
-
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (ad->eNVMType == NVM_EEPROM || ad->eNVMType == NVM_FLASH) {
-		if (copy_to_user(io_buff.OutputBuffer, &ad->uiNVMDSDSize,
-			sizeof(UINT)))
-			return -EFAULT;
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int bcm_char_ioctl_cal_init(void __user *argp,
-				   struct bcm_mini_adapter *ad)
-{
-	struct bcm_ioctl_buffer io_buff;
-	UINT sector_size = 0;
-	INT status = STATUS_FAILURE;
-
-	if (ad->eNVMType == NVM_FLASH) {
-		if (copy_from_user(&io_buff, argp,
-			sizeof(struct bcm_ioctl_buffer)))
-			return -EFAULT;
-
-		if (copy_from_user(&sector_size, io_buff.InputBuffer,
-			sizeof(UINT)))
-			return -EFAULT;
-
-		if ((sector_size < MIN_SECTOR_SIZE) ||
-			(sector_size > MAX_SECTOR_SIZE)) {
-			if (copy_to_user(io_buff.OutputBuffer,
-				&ad->uiSectorSize, sizeof(UINT)))
-				return -EFAULT;
-		} else {
-			if (IsFlash2x(ad)) {
-				if (copy_to_user(io_buff.OutputBuffer,
-					&ad->uiSectorSize, sizeof(UINT)))
-					return -EFAULT;
-			} else {
-				if ((TRUE == ad->bShutStatus) ||
-					(TRUE == ad->IdleMode)) {
-					BCM_DEBUG_PRINT(ad,
-						DBG_TYPE_PRINTK, 0, 0,
-						"Device is in Idle/Shutdown Mode\n");
-					return -EACCES;
-				}
-
-				ad->uiSectorSize = sector_size;
-				BcmUpdateSectorSize(ad,
-					ad->uiSectorSize);
-			}
-		}
-		status = STATUS_SUCCESS;
-	} else {
-		status = STATUS_FAILURE;
-	}
-	return status;
-}
-
-static int bcm_char_ioctl_set_debug(void __user *argp,
-				    struct bcm_mini_adapter *ad)
-{
-#ifdef DEBUG
-	struct bcm_ioctl_buffer io_buff;
-	struct bcm_user_debug_state user_debug_state;
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"In SET_DEBUG ioctl\n");
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (copy_from_user(&user_debug_state, io_buff.InputBuffer,
-		sizeof(struct bcm_user_debug_state)))
-		return -EFAULT;
-
-	BCM_DEBUG_PRINT (ad, DBG_TYPE_PRINTK, 0, 0,
-			"IOCTL_BCM_SET_DEBUG: OnOff=%d Type = 0x%x ",
-			user_debug_state.OnOff, user_debug_state.Type);
-	/* user_debug_state.Subtype <<= 1; */
-	user_debug_state.Subtype = 1 << user_debug_state.Subtype;
-	BCM_DEBUG_PRINT (ad, DBG_TYPE_PRINTK, 0, 0,
-		"actual Subtype=0x%x\n", user_debug_state.Subtype);
-
-	/* Update new 'DebugState' in the ad */
-	ad->stDebugState.type |= user_debug_state.Type;
-	/* Subtype: A bitmap of 32 bits for Subtype per Type.
-	 * Valid indexes in 'subtype' array: 1,2,4,8
-	 * corresponding to valid Type values. Hence we can use the 'Type' field
-	 * as the index value, ignoring the array entries 0,3,5,6,7 !
-	 */
-	if (user_debug_state.OnOff)
-		ad->stDebugState.subtype[user_debug_state.Type] |=
-			user_debug_state.Subtype;
-	else
-		ad->stDebugState.subtype[user_debug_state.Type] &=
-			~user_debug_state.Subtype;
-
-	BCM_SHOW_DEBUG_BITMAP(ad);
-#endif
-	return STATUS_SUCCESS;
-}
-
-static int bcm_char_ioctl_nvm_rw(void __user *argp,
-				 struct bcm_mini_adapter *ad, UINT cmd)
-{
-	struct bcm_nvm_readwrite nvm_rw;
-	struct timeval tv0, tv1;
-	struct bcm_ioctl_buffer io_buff;
-	PUCHAR read_data = NULL;
-	INT status = STATUS_FAILURE;
-
-	memset(&tv0, 0, sizeof(struct timeval));
-	memset(&tv1, 0, sizeof(struct timeval));
-	if ((ad->eNVMType == NVM_FLASH) &&
-		(ad->uiFlashLayoutMajorVersion == 0)) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-			"The Flash Control Section is Corrupted. Hence Rejection on NVM Read/Write\n");
-		return -EFAULT;
-	}
-
-	if (IsFlash2x(ad)) {
-		if ((ad->eActiveDSD != DSD0) &&
-			(ad->eActiveDSD != DSD1) &&
-			(ad->eActiveDSD != DSD2)) {
-
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"No DSD is active..hence NVM Command is blocked");
-			return STATUS_FAILURE;
-		}
-	}
-
-	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (copy_from_user(&nvm_rw,
-				(IOCTL_BCM_NVM_READ == cmd) ?
-				io_buff.OutputBuffer : io_buff.InputBuffer,
-				sizeof(struct bcm_nvm_readwrite)))
-		return -EFAULT;
-
-	/*
-	 * Deny the access if the offset crosses the cal area limit.
-	 */
-	if (nvm_rw.uiNumBytes > ad->uiNVMDSDSize)
-		return STATUS_FAILURE;
-
-	if (nvm_rw.uiOffset >
-		ad->uiNVMDSDSize - nvm_rw.uiNumBytes)
-		return STATUS_FAILURE;
-
-	read_data = memdup_user(nvm_rw.pBuffer,
-				nvm_rw.uiNumBytes);
-	if (IS_ERR(read_data))
-		return PTR_ERR(read_data);
-
-	do_gettimeofday(&tv0);
-	if (IOCTL_BCM_NVM_READ == cmd) {
-		int ret = bcm_handle_nvm_read_cmd(ad, read_data,
-				&nvm_rw);
-		if (ret != STATUS_SUCCESS)
-			return ret;
-	} else {
-		down(&ad->NVMRdmWrmLock);
-
-		if ((ad->IdleMode == TRUE) ||
-			(ad->bShutStatus == TRUE) ||
-			(ad->bPreparingForLowPowerMode == TRUE)) {
-
-			BCM_DEBUG_PRINT(ad,
-				DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"Device is in Idle/Shutdown Mode\n");
-			up(&ad->NVMRdmWrmLock);
-			kfree(read_data);
-			return -EACCES;
-		}
-
-		ad->bHeaderChangeAllowed = TRUE;
-		if (IsFlash2x(ad)) {
-			int ret = handle_flash2x_adapter(ad,
-							read_data,
-							&nvm_rw);
-			if (ret != STATUS_SUCCESS)
-				return ret;
-		}
-
-		status = BeceemNVMWrite(ad, (PUINT)read_data,
-			nvm_rw.uiOffset, nvm_rw.uiNumBytes,
-			nvm_rw.bVerify);
-		if (IsFlash2x(ad))
-			BcmFlash2xWriteSig(ad, ad->eActiveDSD);
-
-		ad->bHeaderChangeAllowed = false;
-
-		up(&ad->NVMRdmWrmLock);
-
-		if (status != STATUS_SUCCESS) {
-			kfree(read_data);
-			return status;
-		}
-	}
-
-	do_gettimeofday(&tv1);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		" timetaken by Write/read :%ld msec\n",
-		(tv1.tv_sec - tv0.tv_sec)*1000 +
-		(tv1.tv_usec - tv0.tv_usec)/1000);
-
-	kfree(read_data);
-	return STATUS_SUCCESS;
-}
-
-static int bcm_char_ioctl_flash2x_section_read(void __user *argp,
-	struct bcm_mini_adapter *ad)
-{
-	struct bcm_flash2x_readwrite flash_2x_read = {0};
-	struct bcm_ioctl_buffer io_buff;
-	PUCHAR read_buff = NULL;
-	UINT nob = 0;
-	UINT buff_size = 0;
-	UINT read_bytes = 0;
-	UINT read_offset = 0;
-	INT status = STATUS_FAILURE;
-	void __user *OutPutBuff;
-
-	if (IsFlash2x(ad) != TRUE)	{
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-			"Flash Does not have 2.x map");
-		return -EINVAL;
-	}
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
-		DBG_LVL_ALL, "IOCTL_BCM_FLASH2X_SECTION_READ Called");
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	/* Reading FLASH 2.x READ structure */
-	if (copy_from_user(&flash_2x_read, io_buff.InputBuffer,
-		sizeof(struct bcm_flash2x_readwrite)))
-		return -EFAULT;
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"\nflash_2x_read.Section :%x",
-			flash_2x_read.Section);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"\nflash_2x_read.offset :%x",
-			flash_2x_read.offset);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"\nflash_2x_read.numOfBytes :%x",
-			flash_2x_read.numOfBytes);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"\nflash_2x_read.bVerify :%x\n",
-			flash_2x_read.bVerify);
-
-	/* This was internal to driver for raw read.
-	 * now it has ben exposed to user space app.
-	 */
-	if (validateFlash2xReadWrite(ad, &flash_2x_read) == false)
-		return STATUS_FAILURE;
-
-	nob = flash_2x_read.numOfBytes;
-	if (nob > ad->uiSectorSize)
-		buff_size = ad->uiSectorSize;
-	else
-		buff_size = nob;
-
-	read_offset = flash_2x_read.offset;
-	OutPutBuff = io_buff.OutputBuffer;
-	read_buff = kzalloc(buff_size , GFP_KERNEL);
-
-	if (read_buff == NULL) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Memory allocation failed for Flash 2.x Read Structure");
-		return -ENOMEM;
-	}
-	down(&ad->NVMRdmWrmLock);
-
-	if ((ad->IdleMode == TRUE) ||
-		(ad->bShutStatus == TRUE) ||
-		(ad->bPreparingForLowPowerMode == TRUE)) {
-
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
-				DBG_LVL_ALL,
-				"Device is in Idle/Shutdown Mode\n");
-		up(&ad->NVMRdmWrmLock);
-		kfree(read_buff);
-		return -EACCES;
-	}
-
-	while (nob) {
-		if (nob > ad->uiSectorSize)
-			read_bytes = ad->uiSectorSize;
-		else
-			read_bytes = nob;
-
-		/* Reading the data from Flash 2.x */
-		status = BcmFlash2xBulkRead(ad, (PUINT)read_buff,
-			flash_2x_read.Section, read_offset, read_bytes);
-		if (status) {
-			BCM_DEBUG_PRINT(ad,
-				DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"Flash 2x read err with status :%d",
-				status);
-			break;
-		}
-
-		BCM_DEBUG_PRINT_BUFFER(ad, DBG_TYPE_OTHERS, OSAL_DBG,
-			DBG_LVL_ALL, read_buff, read_bytes);
-
-		status = copy_to_user(OutPutBuff, read_buff, read_bytes);
-		if (status) {
-			BCM_DEBUG_PRINT(ad,
-				DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"Copy to use failed with status :%d", status);
-			up(&ad->NVMRdmWrmLock);
-			kfree(read_buff);
-			return -EFAULT;
-		}
-		nob = nob - read_bytes;
-		if (nob) {
-			read_offset = read_offset + read_bytes;
-			OutPutBuff = OutPutBuff + read_bytes;
-		}
-	}
-
-	up(&ad->NVMRdmWrmLock);
-	kfree(read_buff);
-	return status;
-}
-
-static int bcm_char_ioctl_flash2x_section_write(void __user *argp,
-	struct bcm_mini_adapter *ad)
-{
-	struct bcm_flash2x_readwrite sFlash2xWrite = {0};
-	struct bcm_ioctl_buffer io_buff;
-	PUCHAR write_buff;
-	void __user *input_addr;
-	UINT nob = 0;
-	UINT buff_size = 0;
-	UINT write_off = 0;
-	UINT write_bytes = 0;
-	INT status = STATUS_FAILURE;
-
-	if (IsFlash2x(ad) != TRUE) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-			"Flash Does not have 2.x map");
-		return -EINVAL;
-	}
-
-	/* First make this False so that we can enable the Sector
-	 * Permission Check in BeceemFlashBulkWrite
-	 */
-	ad->bAllDSDWriteAllow = false;
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"IOCTL_BCM_FLASH2X_SECTION_WRITE Called");
-
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	/* Reading FLASH 2.x READ structure */
-	if (copy_from_user(&sFlash2xWrite, io_buff.InputBuffer,
-		sizeof(struct bcm_flash2x_readwrite)))
-		return -EFAULT;
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"\nsFlash2xWrite.Section :%x", sFlash2xWrite.Section);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"\nsFlash2xWrite.offset :%d", sFlash2xWrite.offset);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"\nsFlash2xWrite.numOfBytes :%x", sFlash2xWrite.numOfBytes);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"\nsFlash2xWrite.bVerify :%x\n", sFlash2xWrite.bVerify);
-
-	if ((sFlash2xWrite.Section != VSA0) && (sFlash2xWrite.Section != VSA1)
-		&& (sFlash2xWrite.Section != VSA2)) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"Only VSA write is allowed");
-		return -EINVAL;
-	}
-
-	if (validateFlash2xReadWrite(ad, &sFlash2xWrite) == false)
-		return STATUS_FAILURE;
-
-	input_addr = sFlash2xWrite.pDataBuff;
-	write_off = sFlash2xWrite.offset;
-	nob = sFlash2xWrite.numOfBytes;
-
-	if (nob > ad->uiSectorSize)
-		buff_size = ad->uiSectorSize;
-	else
-		buff_size = nob;
-
-	write_buff = kmalloc(buff_size, GFP_KERNEL);
-
-	if (write_buff == NULL)
-		return -ENOMEM;
-
-	/* extracting the remainder of the given offset. */
-	write_bytes = ad->uiSectorSize;
-	if (write_off % ad->uiSectorSize) {
-		write_bytes = ad->uiSectorSize -
-			(write_off % ad->uiSectorSize);
-	}
-
-	if (nob < write_bytes)
-		write_bytes = nob;
-
-	down(&ad->NVMRdmWrmLock);
-
-	if ((ad->IdleMode == TRUE) ||
-		(ad->bShutStatus == TRUE) ||
-		(ad->bPreparingForLowPowerMode == TRUE)) {
-
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"Device is in Idle/Shutdown Mode\n");
-		up(&ad->NVMRdmWrmLock);
-		kfree(write_buff);
-		return -EACCES;
-	}
-
-	BcmFlash2xCorruptSig(ad, sFlash2xWrite.Section);
-	do {
-		status = copy_from_user(write_buff, input_addr, write_bytes);
-		if (status) {
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Copy to user failed with status :%d", status);
-			up(&ad->NVMRdmWrmLock);
-			kfree(write_buff);
-			return -EFAULT;
-		}
-		BCM_DEBUG_PRINT_BUFFER(ad, DBG_TYPE_OTHERS,
-			OSAL_DBG, DBG_LVL_ALL, write_buff, write_bytes);
-
-		/* Writing the data from Flash 2.x */
-		status = BcmFlash2xBulkWrite(ad, (PUINT)write_buff,
-					     sFlash2xWrite.Section,
-					     write_off,
-					     write_bytes,
-					     sFlash2xWrite.bVerify);
-
-		if (status) {
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Flash 2x read err with status :%d", status);
-			break;
-		}
-
-		nob = nob - write_bytes;
-		if (nob) {
-			write_off = write_off + write_bytes;
-			input_addr = input_addr + write_bytes;
-			if (nob > ad->uiSectorSize)
-				write_bytes = ad->uiSectorSize;
-			else
-				write_bytes = nob;
-		}
-	} while (nob > 0);
-
-	BcmFlash2xWriteSig(ad, sFlash2xWrite.Section);
-	up(&ad->NVMRdmWrmLock);
-	kfree(write_buff);
-	return status;
-}
-
-static int bcm_char_ioctl_flash2x_section_bitmap(void __user *argp,
-	struct bcm_mini_adapter *ad)
-{
-	struct bcm_flash2x_bitmap *flash_2x_bit_map;
-	struct bcm_ioctl_buffer io_buff;
-
-BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-	"IOCTL_BCM_GET_FLASH2X_SECTION_BITMAP Called");
-
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (io_buff.OutputLength != sizeof(struct bcm_flash2x_bitmap))
-		return -EINVAL;
-
-	flash_2x_bit_map = kzalloc(sizeof(struct bcm_flash2x_bitmap),
-			GFP_KERNEL);
-
-	if (flash_2x_bit_map == NULL) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-			"Memory is not available");
-		return -ENOMEM;
-	}
-
-	/* Reading the Flash Sectio Bit map */
-	down(&ad->NVMRdmWrmLock);
-
-	if ((ad->IdleMode == TRUE) ||
-		(ad->bShutStatus == TRUE) ||
-		(ad->bPreparingForLowPowerMode == TRUE)) {
-
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"Device is in Idle/Shutdown Mode\n");
-		up(&ad->NVMRdmWrmLock);
-		kfree(flash_2x_bit_map);
-		return -EACCES;
-	}
-
-	BcmGetFlash2xSectionalBitMap(ad, flash_2x_bit_map);
-	up(&ad->NVMRdmWrmLock);
-	if (copy_to_user(io_buff.OutputBuffer, flash_2x_bit_map,
-		sizeof(struct bcm_flash2x_bitmap))) {
-		kfree(flash_2x_bit_map);
-		return -EFAULT;
-	}
-
-	kfree(flash_2x_bit_map);
-	return STATUS_FAILURE;
-}
-
-static int bcm_char_ioctl_set_active_section(void __user *argp,
-					     struct bcm_mini_adapter *ad)
-{
-	enum bcm_flash2x_section_val flash_2x_section_val = 0;
-	INT status = STATUS_FAILURE;
-	struct bcm_ioctl_buffer io_buff;
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"IOCTL_BCM_SET_ACTIVE_SECTION Called");
-
-	if (IsFlash2x(ad) != TRUE) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Flash Does not have 2.x map");
-		return -EINVAL;
-	}
-
-	status = copy_from_user(&io_buff, argp,
-				sizeof(struct bcm_ioctl_buffer));
-	if (status) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Copy of IOCTL BUFFER failed");
-		return -EFAULT;
-	}
-
-	status = copy_from_user(&flash_2x_section_val,
-				io_buff.InputBuffer, sizeof(INT));
-	if (status) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-			"Copy of flash section val failed");
-		return -EFAULT;
-	}
-
-	down(&ad->NVMRdmWrmLock);
-
-	if ((ad->IdleMode == TRUE) ||
-		(ad->bShutStatus == TRUE) ||
-		(ad->bPreparingForLowPowerMode == TRUE)) {
-
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"Device is in Idle/Shutdown Mode\n");
-		up(&ad->NVMRdmWrmLock);
-		return -EACCES;
-	}
-
-	status = BcmSetActiveSection(ad, flash_2x_section_val);
-	if (status)
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Failed to make it's priority Highest. status %d",
-				status);
-
-	up(&ad->NVMRdmWrmLock);
-
-	return status;
-}
-
-static int bcm_char_ioctl_copy_section(void __user *argp,
-				       struct bcm_mini_adapter *ad)
-{
-	struct bcm_flash2x_copy_section copy_sect_strut = {0};
-	struct bcm_ioctl_buffer io_buff;
-	INT status = STATUS_SUCCESS;
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"IOCTL_BCM_COPY_SECTION  Called");
-
-	ad->bAllDSDWriteAllow = false;
-	if (IsFlash2x(ad) != TRUE) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Flash Does not have 2.x map");
-		return -EINVAL;
-	}
-
-	status = copy_from_user(&io_buff, argp,
-				sizeof(struct bcm_ioctl_buffer));
-	if (status) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Copy of IOCTL BUFFER failed status :%d",
-				status);
-		return -EFAULT;
-	}
-
-	status = copy_from_user(&copy_sect_strut, io_buff.InputBuffer,
-				sizeof(struct bcm_flash2x_copy_section));
-	if (status) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Copy of Copy_Section_Struct failed with status :%d",
-				status);
-		return -EFAULT;
-	}
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"Source SEction :%x", copy_sect_strut.SrcSection);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"Destination SEction :%x", copy_sect_strut.DstSection);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"offset :%x", copy_sect_strut.offset);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"nob :%x", copy_sect_strut.numOfBytes);
-
-	if (IsSectionExistInFlash(ad, copy_sect_strut.SrcSection) == false) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Source Section<%x> does not exist in Flash ",
-				copy_sect_strut.SrcSection);
-		return -EINVAL;
-	}
-
-	if (IsSectionExistInFlash(ad, copy_sect_strut.DstSection) == false) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Destinatio Section<%x> does not exist in Flash ",
-				copy_sect_strut.DstSection);
-		return -EINVAL;
-	}
-
-	if (copy_sect_strut.SrcSection == copy_sect_strut.DstSection) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"Source and Destination section should be different");
-		return -EINVAL;
-	}
-
-	down(&ad->NVMRdmWrmLock);
-
-	if ((ad->IdleMode == TRUE) ||
-		(ad->bShutStatus == TRUE) ||
-		(ad->bPreparingForLowPowerMode == TRUE)) {
-
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"Device is in Idle/Shutdown Mode\n");
-		up(&ad->NVMRdmWrmLock);
-		return -EACCES;
-	}
-
-	if (copy_sect_strut.SrcSection == ISO_IMAGE1 ||
-		copy_sect_strut.SrcSection == ISO_IMAGE2) {
-		if (IsNonCDLessDevice(ad)) {
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-					"Device is Non-CDLess hence won't have ISO !!");
-			status = -EINVAL;
-		} else if (copy_sect_strut.numOfBytes == 0) {
-			status = BcmCopyISO(ad, copy_sect_strut);
-		} else {
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-					"Partial Copy of ISO section is not Allowed..");
-			status = STATUS_FAILURE;
-		}
-		up(&ad->NVMRdmWrmLock);
-		return status;
-	}
-
-	status = BcmCopySection(ad, copy_sect_strut.SrcSection,
-				copy_sect_strut.DstSection,
-				copy_sect_strut.offset,
-				copy_sect_strut.numOfBytes);
-	up(&ad->NVMRdmWrmLock);
-	return status;
-}
-
-static int bcm_char_ioctl_get_flash_cs_info(void __user *argp,
-					    struct bcm_mini_adapter *ad)
-{
-	struct bcm_ioctl_buffer io_buff;
-	INT status = STATUS_SUCCESS;
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			" IOCTL_BCM_GET_FLASH_CS_INFO Called");
-
-	status = copy_from_user(&io_buff, argp,
-			sizeof(struct bcm_ioctl_buffer));
-	if (status) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Copy of IOCTL BUFFER failed");
-		return -EFAULT;
-	}
-
-	if (ad->eNVMType != NVM_FLASH) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Connected device does not have flash");
-		return -EINVAL;
-	}
-
-	if (IsFlash2x(ad) == TRUE) {
-		if (io_buff.OutputLength < sizeof(struct bcm_flash2x_cs_info))
-			return -EINVAL;
-
-		if (copy_to_user(io_buff.OutputBuffer,
-				 ad->psFlash2xCSInfo,
-				 sizeof(struct bcm_flash2x_cs_info)))
-			return -EFAULT;
-	} else {
-		if (io_buff.OutputLength < sizeof(struct bcm_flash_cs_info))
-			return -EINVAL;
-
-		if (copy_to_user(io_buff.OutputBuffer, ad->psFlashCSInfo,
-				 sizeof(struct bcm_flash_cs_info)))
-			return -EFAULT;
-	}
-	return status;
-}
-
-static int bcm_char_ioctl_select_dsd(void __user *argp,
-				     struct bcm_mini_adapter *ad)
-{
-	struct bcm_ioctl_buffer io_buff;
-	INT status = STATUS_FAILURE;
-	UINT sect_offset = 0;
-	enum bcm_flash2x_section_val flash_2x_section_val;
-
-	flash_2x_section_val = NO_SECTION_VAL;
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"IOCTL_BCM_SELECT_DSD Called");
-
-	if (IsFlash2x(ad) != TRUE) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Flash Does not have 2.x map");
-		return -EINVAL;
-	}
-
-	status = copy_from_user(&io_buff, argp,
-				sizeof(struct bcm_ioctl_buffer));
-	if (status) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Copy of IOCTL BUFFER failed");
-		return -EFAULT;
-	}
-	status = copy_from_user(&flash_2x_section_val, io_buff.InputBuffer,
-		sizeof(INT));
-	if (status) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Copy of flash section val failed");
-		return -EFAULT;
-	}
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"Read Section :%d", flash_2x_section_val);
-	if ((flash_2x_section_val != DSD0) &&
-		(flash_2x_section_val != DSD1) &&
-		(flash_2x_section_val != DSD2)) {
-
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Passed section<%x> is not DSD section",
-				flash_2x_section_val);
-		return STATUS_FAILURE;
-	}
-
-	sect_offset = BcmGetSectionValStartOffset(ad, flash_2x_section_val);
-	if (sect_offset == INVALID_OFFSET) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Provided Section val <%d> does not exist in Flash 2.x",
-				flash_2x_section_val);
-		return -EINVAL;
-	}
-
-	ad->bAllDSDWriteAllow = TRUE;
-	ad->ulFlashCalStart = sect_offset;
-	ad->eActiveDSD = flash_2x_section_val;
-
-	return STATUS_SUCCESS;
-}
-
-static int bcm_char_ioctl_nvm_raw_read(void __user *argp,
-				       struct bcm_mini_adapter *ad)
-{
-	struct bcm_nvm_readwrite nvm_read;
-	struct bcm_ioctl_buffer io_buff;
-	unsigned int nob;
-	INT buff_size;
-	INT read_offset = 0;
-	UINT read_bytes = 0;
-	PUCHAR read_buff;
-	void __user *OutPutBuff;
-	INT status = STATUS_FAILURE;
-
-	if (ad->eNVMType != NVM_FLASH) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"NVM TYPE is not Flash");
-		return -EINVAL;
-	}
-
-	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer))) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"copy_from_user 1 failed\n");
-		return -EFAULT;
-	}
-
-	if (copy_from_user(&nvm_read, io_buff.OutputBuffer,
-		sizeof(struct bcm_nvm_readwrite)))
-		return -EFAULT;
-
-	nob = nvm_read.uiNumBytes;
-	/* In Raw-Read max Buff size : 64MB */
-
-	if (nob > DEFAULT_BUFF_SIZE)
-		buff_size = DEFAULT_BUFF_SIZE;
-	else
-		buff_size = nob;
-
-	read_offset = nvm_read.uiOffset;
-	OutPutBuff = nvm_read.pBuffer;
-
-	read_buff = kzalloc(buff_size , GFP_KERNEL);
-	if (read_buff == NULL) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-				"Memory allocation failed for Flash 2.x Read Structure");
-		return -ENOMEM;
-	}
-	down(&ad->NVMRdmWrmLock);
-
-	if ((ad->IdleMode == TRUE) ||
-		(ad->bShutStatus == TRUE) ||
-		(ad->bPreparingForLowPowerMode == TRUE)) {
-
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"Device is in Idle/Shutdown Mode\n");
-		kfree(read_buff);
-		up(&ad->NVMRdmWrmLock);
-		return -EACCES;
-	}
-
-	ad->bFlashRawRead = TRUE;
-
-	while (nob) {
-		if (nob > DEFAULT_BUFF_SIZE)
-			read_bytes = DEFAULT_BUFF_SIZE;
-		else
-			read_bytes = nob;
-
-		/* Reading the data from Flash 2.x */
-		status = BeceemNVMRead(ad, (PUINT)read_buff,
-			read_offset, read_bytes);
-		if (status) {
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-					"Flash 2x read err with status :%d",
-					status);
-			break;
-		}
-
-		BCM_DEBUG_PRINT_BUFFER(ad, DBG_TYPE_OTHERS, OSAL_DBG,
-				       DBG_LVL_ALL, read_buff, read_bytes);
-
-		status = copy_to_user(OutPutBuff, read_buff, read_bytes);
-		if (status) {
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
-					"Copy to use failed with status :%d",
-					status);
-			up(&ad->NVMRdmWrmLock);
-			kfree(read_buff);
-			return -EFAULT;
-		}
-		nob = nob - read_bytes;
-		if (nob) {
-			read_offset = read_offset + read_bytes;
-			OutPutBuff = OutPutBuff + read_bytes;
-		}
-	}
-	ad->bFlashRawRead = false;
-	up(&ad->NVMRdmWrmLock);
-	kfree(read_buff);
-	return status;
-}
-
-static int bcm_char_ioctl_cntrlmsg_mask(void __user *argp,
-					struct bcm_mini_adapter *ad,
-					struct bcm_tarang_data *tarang)
-{
-	struct bcm_ioctl_buffer io_buff;
-	INT status = STATUS_FAILURE;
-	ULONG rx_cntrl_msg_bit_mask = 0;
-
-	/* Copy Ioctl Buffer structure */
-	status = copy_from_user(&io_buff, argp,
-			sizeof(struct bcm_ioctl_buffer));
-	if (status) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"copy of Ioctl buffer is failed from user space");
-		return -EFAULT;
-	}
-
-	if (io_buff.InputLength != sizeof(unsigned long))
-		return -EINVAL;
-
-	status = copy_from_user(&rx_cntrl_msg_bit_mask, io_buff.InputBuffer,
-				io_buff.InputLength);
-	if (status) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"copy of control bit mask failed from user space");
-		return -EFAULT;
-	}
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"\n Got user defined cntrl msg bit mask :%lx",
-			rx_cntrl_msg_bit_mask);
-	tarang->RxCntrlMsgBitMask = rx_cntrl_msg_bit_mask;
-
-	return status;
-}
-
-static int bcm_char_ioctl_get_device_driver_info(void __user *argp,
-	struct bcm_mini_adapter *ad)
-{
-	struct bcm_driver_info dev_info;
-	struct bcm_ioctl_buffer io_buff;
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"Called IOCTL_BCM_GET_DEVICE_DRIVER_INFO\n");
-
-	memset(&dev_info, 0, sizeof(dev_info));
-	dev_info.MaxRDMBufferSize = BUFFER_4K;
-	dev_info.u32DSDStartOffset = EEPROM_CALPARAM_START;
-	dev_info.u32RxAlignmentCorrection = 0;
-	dev_info.u32NVMType = ad->eNVMType;
-	dev_info.u32InterfaceType = BCM_USB;
-
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (io_buff.OutputLength < sizeof(dev_info))
-		return -EINVAL;
-
-	if (copy_to_user(io_buff.OutputBuffer, &dev_info, sizeof(dev_info)))
-		return -EFAULT;
-
-	return STATUS_SUCCESS;
-}
-
-static int bcm_char_ioctl_time_since_net_entry(void __user *argp,
-	struct bcm_mini_adapter *ad)
-{
-	struct bcm_time_elapsed time_elapsed_since_net_entry = {0};
-	struct bcm_ioctl_buffer io_buff;
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"IOCTL_BCM_TIME_SINCE_NET_ENTRY called");
-
-	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
-		return -EFAULT;
-
-	if (io_buff.OutputLength < sizeof(struct bcm_time_elapsed))
-		return -EINVAL;
-
-	time_elapsed_since_net_entry.ul64TimeElapsedSinceNetEntry =
-		get_seconds() - ad->liTimeSinceLastNetEntry;
-
-	if (copy_to_user(io_buff.OutputBuffer, &time_elapsed_since_net_entry,
-			 sizeof(struct bcm_time_elapsed)))
-		return -EFAULT;
-
-	return STATUS_SUCCESS;
-}
-
-
-static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
-{
-	struct bcm_tarang_data *tarang = filp->private_data;
-	void __user *argp = (void __user *)arg;
-	struct bcm_mini_adapter *ad = tarang->Adapter;
-	INT status = STATUS_FAILURE;
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"Parameters Passed to control IOCTL cmd=0x%X arg=0x%lX",
-			cmd, arg);
-
-	if (_IOC_TYPE(cmd) != BCM_IOCTL)
-		return -EFAULT;
-	if (_IOC_DIR(cmd) & _IOC_READ)
-		status = !access_ok(VERIFY_WRITE, argp, _IOC_SIZE(cmd));
-	else if (_IOC_DIR(cmd) & _IOC_WRITE)
-		status = !access_ok(VERIFY_READ, argp, _IOC_SIZE(cmd));
-	else if (_IOC_NONE == (_IOC_DIR(cmd) & _IOC_NONE))
-		status = STATUS_SUCCESS;
-
-	if (status)
-		return -EFAULT;
-
-	if (ad->device_removed)
-		return -EFAULT;
-
-	if (false == ad->fw_download_done) {
-		switch (cmd) {
-		case IOCTL_MAC_ADDR_REQ:
-		case IOCTL_LINK_REQ:
-		case IOCTL_CM_REQUEST:
-		case IOCTL_SS_INFO_REQ:
-		case IOCTL_SEND_CONTROL_MESSAGE:
-		case IOCTL_IDLE_REQ:
-		case IOCTL_BCM_GPIO_SET_REQUEST:
-		case IOCTL_BCM_GPIO_STATUS_REQUEST:
-			return -EACCES;
-		default:
-			break;
-		}
-	}
-
-	status = vendorextnIoctl(ad, cmd, arg);
-	if (status != CONTINUE_COMMON_PATH)
-		return status;
-
-	switch (cmd) {
-	/* Rdms for Swin Idle... */
-	case IOCTL_BCM_REGISTER_READ_PRIVATE:
-		status = bcm_char_ioctl_reg_read_private(argp, ad);
-		return status;
-
-	case IOCTL_BCM_REGISTER_WRITE_PRIVATE:
-		status = bcm_char_ioctl_reg_write_private(argp, ad);
-		return status;
-
-	case IOCTL_BCM_REGISTER_READ:
-	case IOCTL_BCM_EEPROM_REGISTER_READ:
-		status = bcm_char_ioctl_eeprom_reg_read(argp, ad);
-		return status;
-
-	case IOCTL_BCM_REGISTER_WRITE:
-	case IOCTL_BCM_EEPROM_REGISTER_WRITE:
-		status = bcm_char_ioctl_eeprom_reg_write(argp, ad, cmd);
-		return status;
-
-	case IOCTL_BCM_GPIO_SET_REQUEST:
-		status = bcm_char_ioctl_gpio_set_request(argp, ad);
-		return status;
-
-	case BCM_LED_THREAD_STATE_CHANGE_REQ:
-		status = bcm_char_ioctl_led_thread_state_change_req(argp,
-								    ad);
-		return status;
-
-	case IOCTL_BCM_GPIO_STATUS_REQUEST:
-		status = bcm_char_ioctl_gpio_status_request(argp, ad);
-		return status;
-
-	case IOCTL_BCM_GPIO_MULTI_REQUEST:
-		status = bcm_char_ioctl_gpio_multi_request(argp, ad);
-		return status;
-
-	case IOCTL_BCM_GPIO_MODE_REQUEST:
-		status = bcm_char_ioctl_gpio_mode_request(argp, ad);
-		return status;
-
-	case IOCTL_MAC_ADDR_REQ:
-	case IOCTL_LINK_REQ:
-	case IOCTL_CM_REQUEST:
-	case IOCTL_SS_INFO_REQ:
-	case IOCTL_SEND_CONTROL_MESSAGE:
-	case IOCTL_IDLE_REQ:
-		status = bcm_char_ioctl_misc_request(argp, ad);
-		return status;
-
-	case IOCTL_BCM_BUFFER_DOWNLOAD_START:
-		status = bcm_char_ioctl_buffer_download_start(ad);
-		return status;
-
-	case IOCTL_BCM_BUFFER_DOWNLOAD:
-		status = bcm_char_ioctl_buffer_download(argp, ad);
-		return status;
-
-	case IOCTL_BCM_BUFFER_DOWNLOAD_STOP:
-		status = bcm_char_ioctl_buffer_download_stop(argp, ad);
-		return status;
-
-
-	case IOCTL_BE_BUCKET_SIZE:
-		status = 0;
-		if (get_user(ad->BEBucketSize,
-			     (unsigned long __user *)arg))
-			status = -EFAULT;
-		break;
-
-	case IOCTL_RTPS_BUCKET_SIZE:
-		status = 0;
-		if (get_user(ad->rtPSBucketSize,
-			     (unsigned long __user *)arg))
-			status = -EFAULT;
-		break;
-
-	case IOCTL_CHIP_RESET:
-		status = bcm_char_ioctl_chip_reset(ad);
-		return status;
-
-	case IOCTL_QOS_THRESHOLD:
-		status = bcm_char_ioctl_qos_threshold(arg, ad);
-		return status;
-
-	case IOCTL_DUMP_PACKET_INFO:
-		DumpPackInfo(ad);
-		DumpPhsRules(&ad->stBCMPhsContext);
-		status = STATUS_SUCCESS;
-		break;
-
-	case IOCTL_GET_PACK_INFO:
-		if (copy_to_user(argp, &ad->PackInfo,
-				 sizeof(struct bcm_packet_info)*NO_OF_QUEUES))
-			return -EFAULT;
-		status = STATUS_SUCCESS;
-		break;
-
-	case IOCTL_BCM_SWITCH_TRANSFER_MODE:
-		status = bcm_char_ioctl_switch_transfer_mode(argp, ad);
-		return status;
-
-	case IOCTL_BCM_GET_DRIVER_VERSION:
-		status = bcm_char_ioctl_get_driver_version(argp);
-		return status;
-
-	case IOCTL_BCM_GET_CURRENT_STATUS:
-		status = bcm_char_ioctl_get_current_status(argp, ad);
-		return status;
-
-	case IOCTL_BCM_SET_MAC_TRACING:
-		status = bcm_char_ioctl_set_mac_tracing(argp, ad);
-		return status;
-
-	case IOCTL_BCM_GET_DSX_INDICATION:
-		status = bcm_char_ioctl_get_dsx_indication(argp, ad);
-		return status;
-
-	case IOCTL_BCM_GET_HOST_MIBS:
-		status = bcm_char_ioctl_get_host_mibs(argp, ad, tarang);
-		return status;
-
-	case IOCTL_BCM_WAKE_UP_DEVICE_FROM_IDLE:
-		if ((false == ad->bTriedToWakeUpFromlowPowerMode) &&
-				(TRUE == ad->IdleMode)) {
-			ad->usIdleModePattern = ABORT_IDLE_MODE;
-			ad->bWakeUpDevice = TRUE;
-			wake_up(&ad->process_rx_cntrlpkt);
-		}
-
-		status = STATUS_SUCCESS;
-		break;
-
-	case IOCTL_BCM_BULK_WRM:
-		status = bcm_char_ioctl_bulk_wrm(argp, ad, cmd);
-		return status;
-
-	case IOCTL_BCM_GET_NVM_SIZE:
-		status = bcm_char_ioctl_get_nvm_size(argp, ad);
-		return status;
-
-	case IOCTL_BCM_CAL_INIT:
-		status = bcm_char_ioctl_cal_init(argp, ad);
-		return status;
-
-	case IOCTL_BCM_SET_DEBUG:
-		status = bcm_char_ioctl_set_debug(argp, ad);
-		return status;
-
-	case IOCTL_BCM_NVM_READ:
-	case IOCTL_BCM_NVM_WRITE:
-		status = bcm_char_ioctl_nvm_rw(argp, ad, cmd);
-		return status;
-
-	case IOCTL_BCM_FLASH2X_SECTION_READ:
-		status = bcm_char_ioctl_flash2x_section_read(argp, ad);
-		return status;
-
-	case IOCTL_BCM_FLASH2X_SECTION_WRITE:
-		status = bcm_char_ioctl_flash2x_section_write(argp, ad);
-		return status;
-
-	case IOCTL_BCM_GET_FLASH2X_SECTION_BITMAP:
-		status = bcm_char_ioctl_flash2x_section_bitmap(argp, ad);
-		return status;
-
-	case IOCTL_BCM_SET_ACTIVE_SECTION:
-		status = bcm_char_ioctl_set_active_section(argp, ad);
-		return status;
-
-	case IOCTL_BCM_IDENTIFY_ACTIVE_SECTION:
-		/* Right Now we are taking care of only DSD */
-		ad->bAllDSDWriteAllow = false;
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"IOCTL_BCM_IDENTIFY_ACTIVE_SECTION called");
-		status = STATUS_SUCCESS;
-		break;
-
-	case IOCTL_BCM_COPY_SECTION:
-		status = bcm_char_ioctl_copy_section(argp, ad);
-		return status;
-
-	case IOCTL_BCM_GET_FLASH_CS_INFO:
-		status = bcm_char_ioctl_get_flash_cs_info(argp, ad);
-		return status;
-
-	case IOCTL_BCM_SELECT_DSD:
-		status = bcm_char_ioctl_select_dsd(argp, ad);
-		return status;
-
-	case IOCTL_BCM_NVM_RAW_READ:
-		status = bcm_char_ioctl_nvm_raw_read(argp, ad);
-		return status;
-
-	case IOCTL_BCM_CNTRLMSG_MASK:
-		status = bcm_char_ioctl_cntrlmsg_mask(argp, ad, tarang);
-		return status;
-
-	case IOCTL_BCM_GET_DEVICE_DRIVER_INFO:
-		status = bcm_char_ioctl_get_device_driver_info(argp, ad);
-		return status;
-
-	case IOCTL_BCM_TIME_SINCE_NET_ENTRY:
-		status = bcm_char_ioctl_time_since_net_entry(argp, ad);
-		return status;
-
-	case IOCTL_CLOSE_NOTIFICATION:
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"IOCTL_CLOSE_NOTIFICATION");
-		break;
-
-	default:
-		pr_info(DRV_NAME ": unknown ioctl cmd=%#x\n", cmd);
-		status = STATUS_FAILURE;
-		break;
-	}
-	return status;
-}
-
-
-static const struct file_operations bcm_fops = {
-	.owner    = THIS_MODULE,
-	.open     = bcm_char_open,
-	.release  = bcm_char_release,
-	.read     = bcm_char_read,
-	.unlocked_ioctl    = bcm_char_ioctl,
-	.llseek = no_llseek,
-};
-
-int register_control_device_interface(struct bcm_mini_adapter *ad)
-{
-
-	if (ad->major > 0)
-		return ad->major;
-
-	ad->major = register_chrdev(0, DEV_NAME, &bcm_fops);
-	if (ad->major < 0) {
-		pr_err(DRV_NAME ": could not created character device\n");
-		return ad->major;
-	}
-
-	ad->pstCreatedClassDevice = device_create(bcm_class, NULL,
-						       MKDEV(ad->major, 0),
-						       ad, DEV_NAME);
-
-	if (IS_ERR(ad->pstCreatedClassDevice)) {
-		pr_err(DRV_NAME ": class device create failed\n");
-		unregister_chrdev(ad->major, DEV_NAME);
-		return PTR_ERR(ad->pstCreatedClassDevice);
-	}
-
-	return 0;
-}
-
-void unregister_control_device_interface(struct bcm_mini_adapter *ad)
-{
-	if (ad->major > 0) {
-		device_destroy(bcm_class, MKDEV(ad->major, 0));
-		unregister_chrdev(ad->major, DEV_NAME);
-	}
-}
-
diff --git a/drivers/staging/bcm/Bcmnet.c b/drivers/staging/bcm/Bcmnet.c
deleted file mode 100644
index e57767684cee..000000000000
--- a/drivers/staging/bcm/Bcmnet.c
+++ /dev/null
@@ -1,240 +0,0 @@
-#include "headers.h"
-
-struct net_device *gblpnetdev;
-
-static INT bcm_open(struct net_device *dev)
-{
-	struct bcm_mini_adapter *ad = GET_BCM_ADAPTER(dev);
-
-	if (ad->fw_download_done == false) {
-		pr_notice(PFX "%s: link up failed (download in progress)\n",
-			  dev->name);
-		return -EBUSY;
-	}
-
-	if (netif_msg_ifup(ad))
-		pr_info(PFX "%s: enabling interface\n", dev->name);
-
-	if (ad->LinkUpStatus) {
-		if (netif_msg_link(ad))
-			pr_info(PFX "%s: link up\n", dev->name);
-
-		netif_carrier_on(ad->dev);
-		netif_start_queue(ad->dev);
-	}
-
-	return 0;
-}
-
-static INT bcm_close(struct net_device *dev)
-{
-	struct bcm_mini_adapter *ad = GET_BCM_ADAPTER(dev);
-
-	if (netif_msg_ifdown(ad))
-		pr_info(PFX "%s: disabling interface\n", dev->name);
-
-	netif_carrier_off(dev);
-	netif_stop_queue(dev);
-
-	return 0;
-}
-
-static u16 bcm_select_queue(struct net_device *dev, struct sk_buff *skb,
-			    void *accel_priv, select_queue_fallback_t fallback)
-{
-	return ClassifyPacket(netdev_priv(dev), skb);
-}
-
-/*******************************************************************
-* Function    -	bcm_transmit()
-*
-* Description - This is the main transmit function for our virtual
-*		interface(eth0). It handles the ARP packets. It
-*		clones this packet and then Queue it to a suitable
-*		Queue. Then calls the transmit_packet().
-*
-* Parameter   -	 skb - Pointer to the socket buffer structure
-*		 dev - Pointer to the virtual net device structure
-*
-*********************************************************************/
-
-static netdev_tx_t bcm_transmit(struct sk_buff *skb, struct net_device *dev)
-{
-	struct bcm_mini_adapter *ad = GET_BCM_ADAPTER(dev);
-	u16 qindex = skb_get_queue_mapping(skb);
-
-
-	if (ad->device_removed || !ad->LinkUpStatus)
-		goto drop;
-
-	if (ad->TransferMode != IP_PACKET_ONLY_MODE)
-		goto drop;
-
-	if (INVALID_QUEUE_INDEX == qindex)
-		goto drop;
-
-	if (ad->PackInfo[qindex].uiCurrentPacketsOnHost >=
-	    SF_MAX_ALLOWED_PACKETS_TO_BACKUP)
-		return NETDEV_TX_BUSY;
-
-	/* Now Enqueue the packet */
-	if (netif_msg_tx_queued(ad))
-		pr_info(PFX "%s: enqueueing packet to queue %d\n",
-			dev->name, qindex);
-
-	spin_lock(&ad->PackInfo[qindex].SFQueueLock);
-	ad->PackInfo[qindex].uiCurrentBytesOnHost += skb->len;
-	ad->PackInfo[qindex].uiCurrentPacketsOnHost++;
-
-	*((B_UINT32 *) skb->cb + SKB_CB_LATENCY_OFFSET) = jiffies;
-	ENQUEUEPACKET(ad->PackInfo[qindex].FirstTxQueue,
-		      ad->PackInfo[qindex].LastTxQueue, skb);
-	atomic_inc(&ad->TotalPacketCount);
-	spin_unlock(&ad->PackInfo[qindex].SFQueueLock);
-
-	/* FIXME - this is racy and incorrect, replace with work queue */
-	if (!atomic_read(&ad->TxPktAvail)) {
-		atomic_set(&ad->TxPktAvail, 1);
-		wake_up(&ad->tx_packet_wait_queue);
-	}
-	return NETDEV_TX_OK;
-
- drop:
-	dev_kfree_skb(skb);
-	return NETDEV_TX_OK;
-}
-
-
-
-/**
-@ingroup init_functions
-Register other driver entry points with the kernel
-*/
-static const struct net_device_ops bcmNetDevOps = {
-	.ndo_open		= bcm_open,
-	.ndo_stop		= bcm_close,
-	.ndo_start_xmit	        = bcm_transmit,
-	.ndo_change_mtu	        = eth_change_mtu,
-	.ndo_set_mac_address    = eth_mac_addr,
-	.ndo_validate_addr	= eth_validate_addr,
-	.ndo_select_queue	= bcm_select_queue,
-};
-
-static struct device_type wimax_type = {
-	.name	= "wimax",
-};
-
-static int bcm_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
-{
-	cmd->supported		= 0;
-	cmd->advertising	= 0;
-	cmd->speed		= SPEED_10000;
-	cmd->duplex		= DUPLEX_FULL;
-	cmd->port		= PORT_TP;
-	cmd->phy_address	= 0;
-	cmd->transceiver	= XCVR_INTERNAL;
-	cmd->autoneg		= AUTONEG_DISABLE;
-	cmd->maxtxpkt		= 0;
-	cmd->maxrxpkt		= 0;
-	return 0;
-}
-
-static void bcm_get_drvinfo(struct net_device *dev,
-			    struct ethtool_drvinfo *info)
-{
-	struct bcm_mini_adapter *ad = GET_BCM_ADAPTER(dev);
-	struct bcm_interface_adapter *intf_ad = ad->pvInterfaceAdapter;
-	struct usb_device *udev = interface_to_usbdev(intf_ad->interface);
-
-	strlcpy(info->driver, DRV_NAME, sizeof(info->driver));
-	strlcpy(info->version, DRV_VERSION, sizeof(info->version));
-	snprintf(info->fw_version, sizeof(info->fw_version), "%u.%u",
-		 ad->uiFlashLayoutMajorVersion,
-		 ad->uiFlashLayoutMinorVersion);
-
-	usb_make_path(udev, info->bus_info, sizeof(info->bus_info));
-}
-
-static u32 bcm_get_link(struct net_device *dev)
-{
-	struct bcm_mini_adapter *ad = GET_BCM_ADAPTER(dev);
-
-	return ad->LinkUpStatus;
-}
-
-static u32 bcm_get_msglevel(struct net_device *dev)
-{
-	struct bcm_mini_adapter *ad = GET_BCM_ADAPTER(dev);
-
-	return ad->msg_enable;
-}
-
-static void bcm_set_msglevel(struct net_device *dev, u32 level)
-{
-	struct bcm_mini_adapter *ad = GET_BCM_ADAPTER(dev);
-
-	ad->msg_enable = level;
-}
-
-static const struct ethtool_ops bcm_ethtool_ops = {
-	.get_settings	= bcm_get_settings,
-	.get_drvinfo	= bcm_get_drvinfo,
-	.get_link	= bcm_get_link,
-	.get_msglevel	= bcm_get_msglevel,
-	.set_msglevel	= bcm_set_msglevel,
-};
-
-int register_networkdev(struct bcm_mini_adapter *ad)
-{
-	struct net_device *net = ad->dev;
-	struct bcm_interface_adapter *intf_ad = ad->pvInterfaceAdapter;
-	struct usb_interface *udev = intf_ad->interface;
-	struct usb_device *xdev = intf_ad->udev;
-
-	int result;
-
-	net->netdev_ops = &bcmNetDevOps;
-	net->ethtool_ops = &bcm_ethtool_ops;
-	net->mtu = MTU_SIZE;	/* 1400 Bytes */
-	net->tx_queue_len = TX_QLEN;
-	net->flags |= IFF_NOARP;
-
-	netif_carrier_off(net);
-
-	SET_NETDEV_DEVTYPE(net, &wimax_type);
-
-	/* Read the MAC Address from EEPROM */
-	result = ReadMacAddressFromNVM(ad);
-	if (result != STATUS_SUCCESS) {
-		dev_err(&udev->dev,
-			PFX "Error in Reading the mac Address: %d", result);
-		return -EIO;
-	}
-
-	result = register_netdev(net);
-	if (result)
-		return result;
-
-	gblpnetdev = ad->dev;
-
-	if (netif_msg_probe(ad))
-		dev_info(&udev->dev, PFX "%s: register usb-%s-%s %pM\n",
-			 net->name, xdev->bus->bus_name, xdev->devpath,
-			 net->dev_addr);
-
-	return 0;
-}
-
-void unregister_networkdev(struct bcm_mini_adapter *ad)
-{
-	struct net_device *net = ad->dev;
-	struct bcm_interface_adapter *intf_ad = ad->pvInterfaceAdapter;
-	struct usb_interface *udev = intf_ad->interface;
-	struct usb_device *xdev = intf_ad->udev;
-
-	if (netif_msg_probe(ad))
-		dev_info(&udev->dev, PFX "%s: unregister usb-%s%s\n",
-			 net->name, xdev->bus->bus_name, xdev->devpath);
-
-	unregister_netdev(ad->dev);
-}
diff --git a/drivers/staging/bcm/CmHost.c b/drivers/staging/bcm/CmHost.c
deleted file mode 100644
index adca0ce4d05f..000000000000
--- a/drivers/staging/bcm/CmHost.c
+++ /dev/null
@@ -1,2254 +0,0 @@
-/************************************************************
- * CMHOST.C
- * This file contains the routines for handling Connection
- * Management.
- ************************************************************/
-
-#include "headers.h"
-
-enum E_CLASSIFIER_ACTION {
-	eInvalidClassifierAction,
-	eAddClassifier,
-	eReplaceClassifier,
-	eDeleteClassifier
-};
-
-static ULONG GetNextTargetBufferLocation(struct bcm_mini_adapter *Adapter,
-		B_UINT16 tid);
-static void restore_endianess_of_pstClassifierEntry(
-		struct bcm_classifier_rule *pstClassifierEntry,
-		enum bcm_ipaddr_context eIpAddrContext);
-
-static void apply_phs_rule_to_all_classifiers(
-		register struct bcm_mini_adapter *Adapter,
-		register UINT uiSearchRuleIndex,
-		USHORT uVCID,
-		struct bcm_phs_rule *sPhsRule,
-		struct bcm_phs_rules *cPhsRule,
-		struct bcm_add_indication_alt *pstAddIndication);
-
-/************************************************************
- * Function - SearchSfid
- *
- * Description - This routinue would search QOS queues having
- *  specified SFID as input parameter.
- *
- * Parameters -	Adapter: Pointer to the Adapter structure
- *  uiSfid : Given SFID for matching
- *
- * Returns - Queue index for this SFID(If matched)
- *  Else Invalid Queue Index(If Not matched)
- ************************************************************/
-int SearchSfid(struct bcm_mini_adapter *Adapter, UINT uiSfid)
-{
-	int i;
-
-	for (i = (NO_OF_QUEUES-1); i >= 0; i--)
-		if (Adapter->PackInfo[i].ulSFID == uiSfid)
-			return i;
-
-	return NO_OF_QUEUES+1;
-}
-
-/***************************************************************
- * Function -SearchFreeSfid
- *
- * Description - This routinue would search Free available SFID.
- *
- * Parameter - Adapter: Pointer to the Adapter structure
- *
- * Returns - Queue index for the free SFID
- *  Else returns Invalid Index.
- ****************************************************************/
-static int SearchFreeSfid(struct bcm_mini_adapter *Adapter)
-{
-	int i;
-
-	for (i = 0; i < (NO_OF_QUEUES-1); i++)
-		if (Adapter->PackInfo[i].ulSFID == 0)
-			return i;
-
-	return NO_OF_QUEUES+1;
-}
-
-/*
- * Function: SearchClsid
- * Description:	This routinue would search Classifier  having specified ClassifierID as input parameter
- * Input parameters: struct bcm_mini_adapter *Adapter - Adapter Context
- *  unsigned int uiSfid   - The SF in which the classifier is to searched
- *  B_UINT16  uiClassifierID - The classifier ID to be searched
- * Return: int :Classifier table index of matching entry
- */
-static int SearchClsid(struct bcm_mini_adapter *Adapter,
-		ULONG ulSFID,
-		B_UINT16 uiClassifierID)
-{
-	int i;
-
-	for (i = 0; i < MAX_CLASSIFIERS; i++) {
-		if ((Adapter->astClassifierTable[i].bUsed) &&
-			(Adapter->astClassifierTable[i].uiClassifierRuleIndex
-				== uiClassifierID) &&
-			(Adapter->astClassifierTable[i].ulSFID == ulSFID))
-			return i;
-	}
-
-	return MAX_CLASSIFIERS+1;
-}
-
-/*
- * @ingroup ctrl_pkt_functions
- * This routinue would search Free available Classifier entry in classifier table.
- * @return free Classifier Entry index in classifier table for specified SF
- */
-static int SearchFreeClsid(struct bcm_mini_adapter *Adapter /**Adapter Context*/)
-{
-	int i;
-
-	for (i = 0; i < MAX_CLASSIFIERS; i++) {
-		if (!Adapter->astClassifierTable[i].bUsed)
-			return i;
-	}
-
-	return MAX_CLASSIFIERS+1;
-}
-
-static VOID deleteSFBySfid(struct bcm_mini_adapter *Adapter,
-		UINT uiSearchRuleIndex)
-{
-	/* deleting all the packet held in the SF */
-	flush_queue(Adapter, uiSearchRuleIndex);
-
-	/* Deleting the all classifiers for this SF */
-	DeleteAllClassifiersForSF(Adapter, uiSearchRuleIndex);
-
-	/* Resetting only MIBS related entries in the SF */
-	memset((PVOID)&Adapter->PackInfo[uiSearchRuleIndex], 0,
-			sizeof(struct bcm_mibs_table));
-}
-
-static inline VOID
-CopyIpAddrToClassifier(struct bcm_classifier_rule *pstClassifierEntry,
-		B_UINT8 u8IpAddressLen, B_UINT8 *pu8IpAddressMaskSrc,
-		bool bIpVersion6, enum bcm_ipaddr_context eIpAddrContext)
-{
-	int i = 0;
-	UINT nSizeOfIPAddressInBytes = IP_LENGTH_OF_ADDRESS;
-	UCHAR *ptrClassifierIpAddress = NULL;
-	UCHAR *ptrClassifierIpMask = NULL;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	if (bIpVersion6)
-		nSizeOfIPAddressInBytes = IPV6_ADDRESS_SIZEINBYTES;
-
-	/* Destination Ip Address */
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"Ip Address Range Length:0x%X ", u8IpAddressLen);
-	if ((bIpVersion6 ? (IPV6_ADDRESS_SIZEINBYTES * MAX_IP_RANGE_LENGTH * 2) :
-			(TOTAL_MASKED_ADDRESS_IN_BYTES)) >= u8IpAddressLen) {
-
-		union u_ip_address *st_dest_ip =
-			&pstClassifierEntry->stDestIpAddress;
-
-		union u_ip_address *st_src_ip =
-			&pstClassifierEntry->stSrcIpAddress;
-
-		/*
-		 * checking both the mask and address togethor in Classification.
-		 * So length will be : TotalLengthInBytes/nSizeOfIPAddressInBytes * 2
-		 * (nSizeOfIPAddressInBytes for address and nSizeOfIPAddressInBytes for mask)
-		 */
-		if (eIpAddrContext == eDestIpAddress) {
-			pstClassifierEntry->ucIPDestinationAddressLength =
-				u8IpAddressLen/(nSizeOfIPAddressInBytes * 2);
-			if (bIpVersion6) {
-				ptrClassifierIpAddress =
-					st_dest_ip->ucIpv6Address;
-				ptrClassifierIpMask =
-					st_dest_ip->ucIpv6Mask;
-			} else {
-				ptrClassifierIpAddress =
-					st_dest_ip->ucIpv4Address;
-				ptrClassifierIpMask =
-					st_dest_ip->ucIpv4Mask;
-			}
-		} else if (eIpAddrContext == eSrcIpAddress) {
-			pstClassifierEntry->ucIPSourceAddressLength =
-				u8IpAddressLen/(nSizeOfIPAddressInBytes * 2);
-			if (bIpVersion6) {
-				ptrClassifierIpAddress =
-					st_src_ip->ucIpv6Address;
-				ptrClassifierIpMask = st_src_ip->ucIpv6Mask;
-			} else {
-				ptrClassifierIpAddress =
-					st_src_ip->ucIpv4Address;
-				ptrClassifierIpMask = st_src_ip->ucIpv4Mask;
-			}
-		}
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-				"Address Length:0x%X\n",
-				pstClassifierEntry->ucIPDestinationAddressLength);
-		while ((u8IpAddressLen >= nSizeOfIPAddressInBytes)
-				&& (i < MAX_IP_RANGE_LENGTH)) {
-			memcpy(ptrClassifierIpAddress +
-				(i * nSizeOfIPAddressInBytes),
-				(pu8IpAddressMaskSrc
-					+ (i * nSizeOfIPAddressInBytes * 2)),
-				nSizeOfIPAddressInBytes);
-
-			if (!bIpVersion6) {
-				if (eIpAddrContext == eSrcIpAddress) {
-					st_src_ip->ulIpv4Addr[i] =
-						ntohl(st_src_ip->ulIpv4Addr[i]);
-					BCM_DEBUG_PRINT(Adapter,
-							DBG_TYPE_OTHERS,
-							CONN_MSG,
-							DBG_LVL_ALL,
-							"Src Ip Address:0x%luX ",
-							st_src_ip->ulIpv4Addr[i]);
-				} else if (eIpAddrContext == eDestIpAddress) {
-					st_dest_ip->ulIpv4Addr[i] =
-						ntohl(st_dest_ip->ulIpv4Addr[i]);
-					BCM_DEBUG_PRINT(Adapter,
-							DBG_TYPE_OTHERS,
-							CONN_MSG,
-							DBG_LVL_ALL,
-							"Dest Ip Address:0x%luX ",
-							st_dest_ip->ulIpv4Addr[i]);
-				}
-			}
-			u8IpAddressLen -= nSizeOfIPAddressInBytes;
-			if (u8IpAddressLen >= nSizeOfIPAddressInBytes) {
-				memcpy(ptrClassifierIpMask +
-					(i * nSizeOfIPAddressInBytes),
-					(pu8IpAddressMaskSrc
-						+ nSizeOfIPAddressInBytes
-						+ (i * nSizeOfIPAddressInBytes * 2)),
-					nSizeOfIPAddressInBytes);
-
-				if (!bIpVersion6) {
-					if (eIpAddrContext == eSrcIpAddress) {
-						st_src_ip->ulIpv4Mask[i] =
-							ntohl(st_src_ip->ulIpv4Mask[i]);
-						BCM_DEBUG_PRINT(Adapter,
-								DBG_TYPE_OTHERS,
-								CONN_MSG,
-								DBG_LVL_ALL,
-								"Src Ip Mask Address:0x%luX ",
-								st_src_ip->ulIpv4Mask[i]);
-					} else if (eIpAddrContext == eDestIpAddress) {
-						st_dest_ip->ulIpv4Mask[i] =
-							ntohl(st_dest_ip->ulIpv4Mask[i]);
-						BCM_DEBUG_PRINT(Adapter,
-								DBG_TYPE_OTHERS,
-								CONN_MSG,
-								DBG_LVL_ALL,
-								"Dest Ip Mask Address:0x%luX ",
-								st_dest_ip->ulIpv4Mask[i]);
-					}
-				}
-				u8IpAddressLen -= nSizeOfIPAddressInBytes;
-			}
-			if (u8IpAddressLen == 0)
-				pstClassifierEntry->bDestIpValid = TRUE;
-
-			i++;
-		}
-		if (bIpVersion6) {
-			/* Restore EndianNess of Struct */
-			restore_endianess_of_pstClassifierEntry(
-					pstClassifierEntry,
-					eIpAddrContext
-					);
-		}
-	}
-}
-
-void ClearTargetDSXBuffer(struct bcm_mini_adapter *Adapter, B_UINT16 TID, bool bFreeAll)
-{
-	int i;
-	struct bcm_targetdsx_buffer *curr_buf;
-
-	for (i = 0; i < Adapter->ulTotalTargetBuffersAvailable; i++) {
-		curr_buf = &Adapter->astTargetDsxBuffer[i];
-
-		if (curr_buf->valid)
-			continue;
-
-		if ((bFreeAll) || (curr_buf->tid == TID)) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-					"ClearTargetDSXBuffer: found tid %d buffer cleared %lx\n",
-					TID, curr_buf->ulTargetDsxBuffer);
-			curr_buf->valid = 1;
-			curr_buf->tid = 0;
-			Adapter->ulFreeTargetBufferCnt++;
-		}
-	}
-}
-
-/*
- * @ingroup ctrl_pkt_functions
- * copy classifier rule into the specified SF index
- */
-static inline VOID CopyClassifierRuleToSF(struct bcm_mini_adapter *Adapter,
-		struct bcm_convergence_types *psfCSType,
-		UINT uiSearchRuleIndex,
-		UINT nClassifierIndex)
-{
-	struct bcm_classifier_rule *pstClassifierEntry = NULL;
-	/* VOID *pvPhsContext = NULL; */
-	int i;
-	/* UCHAR ucProtocolLength=0; */
-	/* ULONG ulPhsStatus; */
-
-	struct bcm_packet_class_rules *pack_class_rule =
-		&psfCSType->cCPacketClassificationRule;
-
-	if (Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value == 0 ||
-		nClassifierIndex > (MAX_CLASSIFIERS-1))
-		return;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"Storing Classifier Rule Index : %X",
-			ntohs(pack_class_rule->u16PacketClassificationRuleIndex));
-
-	if (nClassifierIndex > MAX_CLASSIFIERS-1)
-		return;
-
-	pstClassifierEntry = &Adapter->astClassifierTable[nClassifierIndex];
-	if (pstClassifierEntry) {
-		/* Store if Ipv6 */
-		pstClassifierEntry->bIpv6Protocol =
-			(Adapter->PackInfo[uiSearchRuleIndex].ucIpVersion == IPV6) ? TRUE : false;
-
-		/* Destinaiton Port */
-		pstClassifierEntry->ucDestPortRangeLength =
-			pack_class_rule->u8ProtocolDestPortRangeLength / 4;
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-				"Destination Port Range Length:0x%X ",
-				pstClassifierEntry->ucDestPortRangeLength);
-
-		if (pack_class_rule->u8ProtocolDestPortRangeLength <= MAX_PORT_RANGE) {
-			for (i = 0; i < (pstClassifierEntry->ucDestPortRangeLength); i++) {
-				pstClassifierEntry->usDestPortRangeLo[i] =
-					*((PUSHORT)(pack_class_rule->u8ProtocolDestPortRange+i));
-				pstClassifierEntry->usDestPortRangeHi[i] =
-					*((PUSHORT)(pack_class_rule->u8ProtocolDestPortRange+2+i));
-				pstClassifierEntry->usDestPortRangeLo[i] =
-					ntohs(pstClassifierEntry->usDestPortRangeLo[i]);
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
-						CONN_MSG, DBG_LVL_ALL,
-						"Destination Port Range Lo:0x%X ",
-						pstClassifierEntry->usDestPortRangeLo[i]);
-				pstClassifierEntry->usDestPortRangeHi[i] =
-					ntohs(pstClassifierEntry->usDestPortRangeHi[i]);
-			}
-		} else {
-			pstClassifierEntry->ucDestPortRangeLength = 0;
-		}
-
-		/* Source Port */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-				"Source Port Range Length:0x%X ",
-				pack_class_rule->u8ProtocolSourcePortRangeLength);
-		if (pack_class_rule->u8ProtocolSourcePortRangeLength <= MAX_PORT_RANGE) {
-			pstClassifierEntry->ucSrcPortRangeLength =
-				pack_class_rule->u8ProtocolSourcePortRangeLength/4;
-			for (i = 0; i < (pstClassifierEntry->ucSrcPortRangeLength); i++) {
-				pstClassifierEntry->usSrcPortRangeLo[i] =
-					*((PUSHORT)(pack_class_rule->
-							u8ProtocolSourcePortRange+i));
-				pstClassifierEntry->usSrcPortRangeHi[i] =
-					*((PUSHORT)(pack_class_rule->
-							u8ProtocolSourcePortRange+2+i));
-				pstClassifierEntry->usSrcPortRangeLo[i] =
-					ntohs(pstClassifierEntry->usSrcPortRangeLo[i]);
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
-						CONN_MSG, DBG_LVL_ALL,
-						"Source Port Range Lo:0x%X ",
-						pstClassifierEntry->usSrcPortRangeLo[i]);
-				pstClassifierEntry->usSrcPortRangeHi[i] =
-					ntohs(pstClassifierEntry->usSrcPortRangeHi[i]);
-			}
-		}
-		/* Destination Ip Address and Mask */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-				"Ip Destination Parameters : ");
-		CopyIpAddrToClassifier(pstClassifierEntry,
-				pack_class_rule->u8IPDestinationAddressLength,
-				pack_class_rule->u8IPDestinationAddress,
-				(Adapter->PackInfo[uiSearchRuleIndex].ucIpVersion == IPV6) ?
-			TRUE : false, eDestIpAddress);
-
-		/* Source Ip Address and Mask */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-				"Ip Source Parameters : ");
-
-		CopyIpAddrToClassifier(pstClassifierEntry,
-				pack_class_rule->u8IPMaskedSourceAddressLength,
-				pack_class_rule->u8IPMaskedSourceAddress,
-				(Adapter->PackInfo[uiSearchRuleIndex].ucIpVersion == IPV6) ? TRUE : false,
-				eSrcIpAddress);
-
-		/* TOS */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-				"TOS Length:0x%X ",
-				pack_class_rule->u8IPTypeOfServiceLength);
-		if (pack_class_rule->u8IPTypeOfServiceLength == 3) {
-			pstClassifierEntry->ucIPTypeOfServiceLength =
-				pack_class_rule->u8IPTypeOfServiceLength;
-			pstClassifierEntry->ucTosLow =
-				pack_class_rule->u8IPTypeOfService[0];
-			pstClassifierEntry->ucTosHigh =
-				pack_class_rule->u8IPTypeOfService[1];
-			pstClassifierEntry->ucTosMask =
-				pack_class_rule->u8IPTypeOfService[2];
-			pstClassifierEntry->bTOSValid = TRUE;
-		}
-		if (pack_class_rule->u8Protocol == 0) {
-			/* we didn't get protocol field filled in by the BS */
-			pstClassifierEntry->ucProtocolLength = 0;
-		} else {
-			pstClassifierEntry->ucProtocolLength = 1; /* 1 valid protocol */
-		}
-
-		pstClassifierEntry->ucProtocol[0] = pack_class_rule->u8Protocol;
-		pstClassifierEntry->u8ClassifierRulePriority =
-			pack_class_rule->u8ClassifierRulePriority;
-
-		/* store the classifier rule ID and set this classifier entry as valid */
-		pstClassifierEntry->ucDirection =
-			Adapter->PackInfo[uiSearchRuleIndex].ucDirection;
-		pstClassifierEntry->uiClassifierRuleIndex =
-			ntohs(pack_class_rule->u16PacketClassificationRuleIndex);
-		pstClassifierEntry->usVCID_Value =
-			Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value;
-		pstClassifierEntry->ulSFID =
-			Adapter->PackInfo[uiSearchRuleIndex].ulSFID;
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-				"Search Index %d Dir: %d, Index: %d, Vcid: %d\n",
-				uiSearchRuleIndex,
-				pstClassifierEntry->ucDirection,
-				pstClassifierEntry->uiClassifierRuleIndex,
-				pstClassifierEntry->usVCID_Value);
-
-		if (pack_class_rule->u8AssociatedPHSI)
-			pstClassifierEntry->u8AssociatedPHSI =
-				pack_class_rule->u8AssociatedPHSI;
-
-		/* Copy ETH CS Parameters */
-		pstClassifierEntry->ucEthCSSrcMACLen =
-			(pack_class_rule->u8EthernetSourceMACAddressLength);
-		memcpy(pstClassifierEntry->au8EThCSSrcMAC,
-				pack_class_rule->u8EthernetSourceMACAddress,
-				MAC_ADDRESS_SIZE);
-		memcpy(pstClassifierEntry->au8EThCSSrcMACMask,
-				pack_class_rule->u8EthernetSourceMACAddress
-				+ MAC_ADDRESS_SIZE, MAC_ADDRESS_SIZE);
-		pstClassifierEntry->ucEthCSDestMACLen =
-			(pack_class_rule->u8EthernetDestMacAddressLength);
-		memcpy(pstClassifierEntry->au8EThCSDestMAC,
-				pack_class_rule->u8EthernetDestMacAddress,
-				MAC_ADDRESS_SIZE);
-		memcpy(pstClassifierEntry->au8EThCSDestMACMask,
-				pack_class_rule->u8EthernetDestMacAddress
-				+ MAC_ADDRESS_SIZE, MAC_ADDRESS_SIZE);
-		pstClassifierEntry->ucEtherTypeLen =
-			(pack_class_rule->u8EthertypeLength);
-		memcpy(pstClassifierEntry->au8EthCSEtherType,
-				pack_class_rule->u8Ethertype,
-				NUM_ETHERTYPE_BYTES);
-		memcpy(pstClassifierEntry->usUserPriority,
-				&pack_class_rule->u16UserPriority, 2);
-		pstClassifierEntry->usVLANID =
-			ntohs(pack_class_rule->u16VLANID);
-		pstClassifierEntry->usValidityBitMap =
-			ntohs(pack_class_rule->u16ValidityBitMap);
-
-		pstClassifierEntry->bUsed = TRUE;
-	}
-}
-
-/*
- * @ingroup ctrl_pkt_functions
- */
-static inline VOID DeleteClassifierRuleFromSF(struct bcm_mini_adapter *Adapter,
-		UINT uiSearchRuleIndex, UINT nClassifierIndex)
-{
-	struct bcm_classifier_rule *pstClassifierEntry = NULL;
-	B_UINT16 u16PacketClassificationRuleIndex;
-	USHORT usVCID;
-	/* VOID *pvPhsContext = NULL; */
-	/*ULONG ulPhsStatus; */
-
-	usVCID = Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value;
-
-	if (nClassifierIndex > MAX_CLASSIFIERS-1)
-		return;
-
-	if (usVCID == 0)
-		return;
-
-	u16PacketClassificationRuleIndex =
-		Adapter->astClassifierTable[nClassifierIndex].uiClassifierRuleIndex;
-	pstClassifierEntry = &Adapter->astClassifierTable[nClassifierIndex];
-	if (pstClassifierEntry) {
-		pstClassifierEntry->bUsed = false;
-		pstClassifierEntry->uiClassifierRuleIndex = 0;
-		memset(pstClassifierEntry, 0,
-				sizeof(struct bcm_classifier_rule));
-
-		/* Delete the PHS Rule for this classifier */
-		PhsDeleteClassifierRule(&Adapter->stBCMPhsContext, usVCID,
-				u16PacketClassificationRuleIndex);
-	}
-}
-
-/*
- * @ingroup ctrl_pkt_functions
- */
-VOID DeleteAllClassifiersForSF(struct bcm_mini_adapter *Adapter,
-		UINT uiSearchRuleIndex)
-{
-	struct bcm_classifier_rule *pstClassifierEntry = NULL;
-	int i;
-	/* B_UINT16  u16PacketClassificationRuleIndex; */
-	USHORT ulVCID;
-	/* VOID *pvPhsContext = NULL; */
-	/* ULONG ulPhsStatus; */
-
-	ulVCID = Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value;
-
-	if (ulVCID == 0)
-		return;
-
-	for (i = 0; i < MAX_CLASSIFIERS; i++) {
-		if (Adapter->astClassifierTable[i].usVCID_Value == ulVCID) {
-			pstClassifierEntry = &Adapter->astClassifierTable[i];
-
-			if (pstClassifierEntry->bUsed)
-				DeleteClassifierRuleFromSF(Adapter,
-						uiSearchRuleIndex, i);
-		}
-	}
-
-	/* Delete All Phs Rules Associated with this SF */
-	PhsDeleteSFRules(&Adapter->stBCMPhsContext, ulVCID);
-}
-
-/*
- * This routinue  copies the Connection Management
- * related data into the Adapter structure.
- * @ingroup ctrl_pkt_functions
- */
-static VOID CopyToAdapter(register struct bcm_mini_adapter *Adapter, /* <Pointer to the Adapter structure */
-			register struct bcm_connect_mgr_params *psfLocalSet, /* Pointer to the connection manager parameters structure */
-			register UINT uiSearchRuleIndex, /* <Index of Queue, to which this data belongs */
-			register UCHAR ucDsxType,
-			struct bcm_add_indication_alt *pstAddIndication) {
-
-	/* UCHAR ucProtocolLength = 0; */
-	ULONG ulSFID;
-	UINT nClassifierIndex = 0;
-	enum E_CLASSIFIER_ACTION eClassifierAction = eInvalidClassifierAction;
-	B_UINT16 u16PacketClassificationRuleIndex = 0;
-	int i;
-	struct bcm_convergence_types *psfCSType = NULL;
-	struct bcm_phs_rule sPhsRule;
-	struct bcm_packet_info *curr_packinfo =
-		&Adapter->PackInfo[uiSearchRuleIndex];
-	USHORT uVCID = curr_packinfo->usVCID_Value;
-	UINT UGIValue = 0;
-
-	curr_packinfo->bValid = TRUE;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"Search Rule Index = %d\n", uiSearchRuleIndex);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"%s: SFID= %x ", __func__, ntohl(psfLocalSet->u32SFID));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"Updating Queue %d", uiSearchRuleIndex);
-
-	ulSFID = ntohl(psfLocalSet->u32SFID);
-	/* Store IP Version used */
-	/* Get The Version Of IP used (IPv6 or IPv4) from CSSpecification field of SF */
-
-	curr_packinfo->bIPCSSupport = 0;
-	curr_packinfo->bEthCSSupport = 0;
-
-	/* Enable IP/ETh CS Support As Required */
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"CopyToAdapter : u8CSSpecification : %X\n",
-			psfLocalSet->u8CSSpecification);
-	switch (psfLocalSet->u8CSSpecification) {
-	case eCSPacketIPV4:
-		curr_packinfo->bIPCSSupport = IPV4_CS;
-		break;
-	case eCSPacketIPV6:
-		curr_packinfo->bIPCSSupport = IPV6_CS;
-		break;
-	case eCS802_3PacketEthernet:
-	case eCS802_1QPacketVLAN:
-		curr_packinfo->bEthCSSupport = ETH_CS_802_3;
-		break;
-	case eCSPacketIPV4Over802_1QVLAN:
-	case eCSPacketIPV4Over802_3Ethernet:
-		curr_packinfo->bIPCSSupport = IPV4_CS;
-		curr_packinfo->bEthCSSupport = ETH_CS_802_3;
-		break;
-	case eCSPacketIPV6Over802_1QVLAN:
-	case eCSPacketIPV6Over802_3Ethernet:
-		curr_packinfo->bIPCSSupport = IPV6_CS;
-		curr_packinfo->bEthCSSupport = ETH_CS_802_3;
-		break;
-	default:
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-				"Error in value of CS Classification.. setting default to IP CS\n");
-		curr_packinfo->bIPCSSupport = IPV4_CS;
-		break;
-	}
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"CopyToAdapter : Queue No : %X ETH CS Support :  %X  , IP CS Support : %X\n",
-			uiSearchRuleIndex,
-			curr_packinfo->bEthCSSupport,
-			curr_packinfo->bIPCSSupport);
-
-	/* Store IP Version used */
-	/* Get The Version Of IP used (IPv6 or IPv4) from CSSpecification field of SF */
-	if (curr_packinfo->bIPCSSupport == IPV6_CS)
-		curr_packinfo->ucIpVersion = IPV6;
-	else
-		curr_packinfo->ucIpVersion = IPV4;
-
-	/* To ensure that the ETH CS code doesn't gets executed if the BS doesn't supports ETH CS */
-	if (!Adapter->bETHCSEnabled)
-		curr_packinfo->bEthCSSupport = 0;
-
-	if (psfLocalSet->u8ServiceClassNameLength > 0 && psfLocalSet->u8ServiceClassNameLength < 32)
-		memcpy(curr_packinfo->ucServiceClassName,
-				psfLocalSet->u8ServiceClassName,
-				psfLocalSet->u8ServiceClassNameLength);
-
-	curr_packinfo->u8QueueType = psfLocalSet->u8ServiceFlowSchedulingType;
-
-	if (curr_packinfo->u8QueueType == BE && curr_packinfo->ucDirection)
-		Adapter->usBestEffortQueueIndex = uiSearchRuleIndex;
-
-	curr_packinfo->ulSFID = ntohl(psfLocalSet->u32SFID);
-
-	curr_packinfo->u8TrafficPriority = psfLocalSet->u8TrafficPriority;
-
-	/* copy all the classifier in the Service Flow param  structure */
-	for (i = 0; i < psfLocalSet->u8TotalClassifiers; i++) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-				"Classifier index =%d", i);
-		psfCSType = &psfLocalSet->cConvergenceSLTypes[i];
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-				"Classifier index =%d", i);
-
-		if (psfCSType->cCPacketClassificationRule.u8ClassifierRulePriority)
-			curr_packinfo->bClassifierPriority = TRUE;
-
-		if (psfCSType->cCPacketClassificationRule.u8ClassifierRulePriority)
-			curr_packinfo->bClassifierPriority = TRUE;
-
-		if (ucDsxType == DSA_ACK) {
-			eClassifierAction = eAddClassifier;
-		} else if (ucDsxType == DSC_ACK) {
-			switch (psfCSType->u8ClassfierDSCAction) {
-			case 0: /* DSC Add Classifier */
-				eClassifierAction = eAddClassifier;
-				break;
-			case 1: /* DSC Replace Classifier */
-				eClassifierAction = eReplaceClassifier;
-				break;
-			case 2: /* DSC Delete Classifier */
-				eClassifierAction = eDeleteClassifier;
-				break;
-			default:
-				eClassifierAction = eInvalidClassifierAction;
-			}
-		}
-
-		u16PacketClassificationRuleIndex = ntohs(psfCSType->cCPacketClassificationRule.u16PacketClassificationRuleIndex);
-
-		switch (eClassifierAction) {
-		case eAddClassifier:
-			/* Get a Free Classifier Index From Classifier table for this SF to add the Classifier */
-			/* Contained in this message */
-			nClassifierIndex = SearchClsid(Adapter,
-					ulSFID,
-					u16PacketClassificationRuleIndex);
-
-			if (nClassifierIndex > MAX_CLASSIFIERS) {
-				nClassifierIndex = SearchFreeClsid(Adapter);
-				if (nClassifierIndex > MAX_CLASSIFIERS) {
-					/* Failed To get a free Entry */
-					BCM_DEBUG_PRINT(Adapter,
-							DBG_TYPE_OTHERS,
-							CONN_MSG,
-							DBG_LVL_ALL,
-							"Error Failed To get a free Classifier Entry");
-					break;
-				}
-				/* Copy the Classifier Rule for this service flow into our Classifier table maintained per SF. */
-				CopyClassifierRuleToSF(Adapter, psfCSType,
-						uiSearchRuleIndex,
-						nClassifierIndex);
-			} else {
-				/* This Classifier Already Exists and it is invalid to Add Classifier with existing PCRI */
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
-						CONN_MSG,
-						DBG_LVL_ALL,
-						"CopyToAdapter: Error The Specified Classifier Already Exists and attempted To Add Classifier with Same PCRI : 0x%x\n",
-						u16PacketClassificationRuleIndex);
-			}
-			break;
-		case eReplaceClassifier:
-			/* Get the Classifier Index From Classifier table for this SF and replace existing  Classifier */
-			/* with the new classifier Contained in this message */
-			nClassifierIndex = SearchClsid(Adapter, ulSFID,
-					u16PacketClassificationRuleIndex);
-			if (nClassifierIndex > MAX_CLASSIFIERS) {
-				/* Failed To search the classifier */
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
-						CONN_MSG, DBG_LVL_ALL,
-						"Error Search for Classifier To be replaced failed");
-				break;
-			}
-			/* Copy the Classifier Rule for this service flow into our Classifier table maintained per SF. */
-			CopyClassifierRuleToSF(Adapter, psfCSType,
-					uiSearchRuleIndex, nClassifierIndex);
-			break;
-		case eDeleteClassifier:
-			/* Get the Classifier Index From Classifier table for this SF and replace existing  Classifier */
-			/* with the new classifier Contained in this message */
-			nClassifierIndex = SearchClsid(Adapter, ulSFID,
-					u16PacketClassificationRuleIndex);
-			if (nClassifierIndex > MAX_CLASSIFIERS)	{
-				/* Failed To search the classifier */
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
-						CONN_MSG, DBG_LVL_ALL,
-						"Error Search for Classifier To be deleted failed");
-				break;
-			}
-
-			/* Delete This classifier */
-			DeleteClassifierRuleFromSF(Adapter, uiSearchRuleIndex,
-					nClassifierIndex);
-			break;
-		default:
-			/* Invalid Action for classifier */
-			break;
-		}
-	}
-
-	/* Repeat parsing Classification Entries to process PHS Rules */
-	for (i = 0; i < psfLocalSet->u8TotalClassifiers; i++) {
-		psfCSType = &psfLocalSet->cConvergenceSLTypes[i];
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-				"psfCSType->u8PhsDSCAction : 0x%x\n",
-				psfCSType->u8PhsDSCAction);
-
-		switch (psfCSType->u8PhsDSCAction) {
-		case eDeleteAllPHSRules:
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
-					DBG_LVL_ALL,
-					"Deleting All PHS Rules For VCID: 0x%X\n",
-					uVCID);
-
-			/* Delete All the PHS rules for this Service flow */
-			PhsDeleteSFRules(&Adapter->stBCMPhsContext, uVCID);
-			break;
-		case eDeletePHSRule:
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
-					DBG_LVL_ALL,
-					"PHS DSC Action = Delete PHS Rule\n");
-
-			if (psfCSType->cPhsRule.u8PHSI)
-				PhsDeletePHSRule(&Adapter->stBCMPhsContext,
-						uVCID,
-						psfCSType->cCPacketClassificationRule.u8AssociatedPHSI);
-
-			break;
-		default:
-			if (ucDsxType == DSC_ACK) {
-				/* BCM_DEBUG_PRINT(CONN_MSG,("Invalid PHS DSC Action For DSC\n",psfCSType->cPhsRule.u8PHSI)); */
-				break; /* FOr DSC ACK Case PHS DSC Action must be in valid set */
-			}
-		/* Proceed To Add PHS rule for DSA_ACK case even if PHS DSC action is unspecified */
-		/* No Break Here . Intentionally! */
-
-		case eAddPHSRule:
-		case eSetPHSRule:
-			if (psfCSType->cPhsRule.u8PHSI)	{
-				/* Apply This PHS Rule to all classifiers whose Associated PHSI Match */
-				apply_phs_rule_to_all_classifiers(Adapter,
-						uiSearchRuleIndex,
-						uVCID,
-						&sPhsRule,
-						&psfCSType->cPhsRule,
-						pstAddIndication);
-			}
-			break;
-		}
-	}
-
-	if (psfLocalSet->u32MaxSustainedTrafficRate == 0) {
-		/* No Rate Limit . Set Max Sustained Traffic Rate to Maximum */
-		curr_packinfo->uiMaxAllowedRate = WIMAX_MAX_ALLOWED_RATE;
-	} else if (ntohl(psfLocalSet->u32MaxSustainedTrafficRate) > WIMAX_MAX_ALLOWED_RATE) {
-		/* Too large Allowed Rate specified. Limiting to Wi Max  Allowed rate */
-		curr_packinfo->uiMaxAllowedRate = WIMAX_MAX_ALLOWED_RATE;
-	} else {
-		curr_packinfo->uiMaxAllowedRate =
-			ntohl(psfLocalSet->u32MaxSustainedTrafficRate);
-	}
-
-	curr_packinfo->uiMaxLatency = ntohl(psfLocalSet->u32MaximumLatency);
-	if (curr_packinfo->uiMaxLatency == 0) /* 0 should be treated as infinite */
-		curr_packinfo->uiMaxLatency = MAX_LATENCY_ALLOWED;
-
-	if ((curr_packinfo->u8QueueType == ERTPS ||
-			curr_packinfo->u8QueueType == UGS))
-		UGIValue = ntohs(psfLocalSet->u16UnsolicitedGrantInterval);
-
-	if (UGIValue == 0)
-		UGIValue = DEFAULT_UG_INTERVAL;
-
-	/*
-	 * For UGI based connections...
-	 * DEFAULT_UGI_FACTOR*UGIInterval worth of data is the max token count at host...
-	 * The extra amount of token is to ensure that a large amount of jitter won't have loss in throughput...
-	 * In case of non-UGI based connection, 200 frames worth of data is the max token count at host...
-	 */
-	curr_packinfo->uiMaxBucketSize =
-		(DEFAULT_UGI_FACTOR*curr_packinfo->uiMaxAllowedRate*UGIValue)/1000;
-
-	if (curr_packinfo->uiMaxBucketSize < WIMAX_MAX_MTU*8) {
-		UINT UGIFactor = 0;
-		/* Special Handling to ensure the biggest size of packet can go out from host to FW as follows:
-		 * 1. Any packet from Host to FW can go out in different packet size.
-		 * 2. So in case the Bucket count is smaller than MTU, the packets of size (Size > TokenCount), will get dropped.
-		 * 3. We can allow packets of MaxSize from Host->FW that can go out from FW in multiple SDUs by fragmentation at Wimax Layer
-		 */
-		UGIFactor = (curr_packinfo->uiMaxLatency/UGIValue + 1);
-
-		if (UGIFactor > DEFAULT_UGI_FACTOR)
-			curr_packinfo->uiMaxBucketSize =
-				(UGIFactor*curr_packinfo->uiMaxAllowedRate*UGIValue)/1000;
-
-		if (curr_packinfo->uiMaxBucketSize > WIMAX_MAX_MTU*8)
-			curr_packinfo->uiMaxBucketSize = WIMAX_MAX_MTU*8;
-	}
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"LAT: %d, UGI: %d\n", curr_packinfo->uiMaxLatency,
-			UGIValue);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"uiMaxAllowedRate: 0x%x, u32MaxSustainedTrafficRate: 0x%x ,uiMaxBucketSize: 0x%x",
-			curr_packinfo->uiMaxAllowedRate,
-			ntohl(psfLocalSet->u32MaxSustainedTrafficRate),
-			curr_packinfo->uiMaxBucketSize);
-
-	/* copy the extended SF Parameters to Support MIBS */
-	CopyMIBSExtendedSFParameters(Adapter, psfLocalSet, uiSearchRuleIndex);
-
-	/* store header suppression enabled flag per SF */
-	curr_packinfo->bHeaderSuppressionEnabled =
-		!(psfLocalSet->u8RequesttransmissionPolicy &
-			MASK_DISABLE_HEADER_SUPPRESSION);
-
-	kfree(curr_packinfo->pstSFIndication);
-	curr_packinfo->pstSFIndication = pstAddIndication;
-
-	/* Re Sort the SF list in PackInfo according to Traffic Priority */
-	SortPackInfo(Adapter);
-
-	/* Re Sort the Classifier Rules table and re - arrange
-	 * according to Classifier Rule Priority
-	 */
-	SortClassifiers(Adapter);
-	DumpPhsRules(&Adapter->stBCMPhsContext);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"%s <=====", __func__);
-}
-
-/***********************************************************************
- * Function - DumpCmControlPacket
- *
- * Description - This routinue Dumps the Contents of the AddIndication
- *  Structure in the Connection Management Control Packet
- *
- * Parameter - pvBuffer: Pointer to the buffer containing the
- *  AddIndication data.
- *
- * Returns - None
- *************************************************************************/
-static VOID DumpCmControlPacket(PVOID pvBuffer)
-{
-	int uiLoopIndex;
-	int nIndex;
-	struct bcm_add_indication_alt *pstAddIndication;
-	UINT nCurClassifierCnt;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	pstAddIndication = pvBuffer;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "======>");
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8Type: 0x%X", pstAddIndication->u8Type);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8Direction: 0x%X", pstAddIndication->u8Direction);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16TID: 0x%X", ntohs(pstAddIndication->u16TID));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16CID: 0x%X", ntohs(pstAddIndication->u16CID));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16VCID: 0x%X", ntohs(pstAddIndication->u16VCID));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " AuthorizedSet--->");
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u32SFID: 0x%X", htonl(pstAddIndication->sfAuthorizedSet.u32SFID));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16CID: 0x%X", htons(pstAddIndication->sfAuthorizedSet.u16CID));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ServiceClassNameLength: 0x%X",
-			pstAddIndication->sfAuthorizedSet.u8ServiceClassNameLength);
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ServiceClassName: 0x%X ,0x%X , 0x%X, 0x%X, 0x%X, 0x%X",
-			pstAddIndication->sfAuthorizedSet.u8ServiceClassName[0],
-			pstAddIndication->sfAuthorizedSet.u8ServiceClassName[1],
-			pstAddIndication->sfAuthorizedSet.u8ServiceClassName[2],
-			pstAddIndication->sfAuthorizedSet.u8ServiceClassName[3],
-			pstAddIndication->sfAuthorizedSet.u8ServiceClassName[4],
-			pstAddIndication->sfAuthorizedSet.u8ServiceClassName[5]);
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8MBSService: 0x%X", pstAddIndication->sfAuthorizedSet.u8MBSService);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8QosParamSet: 0x%X", pstAddIndication->sfAuthorizedSet.u8QosParamSet);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8TrafficPriority: 0x%X, %p",
-			pstAddIndication->sfAuthorizedSet.u8TrafficPriority, &pstAddIndication->sfAuthorizedSet.u8TrafficPriority);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u32MaxSustainedTrafficRate: 0x%X 0x%p",
-			pstAddIndication->sfAuthorizedSet.u32MaxSustainedTrafficRate,
-			&pstAddIndication->sfAuthorizedSet.u32MaxSustainedTrafficRate);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u32MaxTrafficBurst: 0x%X", pstAddIndication->sfAuthorizedSet.u32MaxTrafficBurst);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u32MinReservedTrafficRate	: 0x%X",
-			pstAddIndication->sfAuthorizedSet.u32MinReservedTrafficRate);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8VendorSpecificQoSParamLength: 0x%X",
-			pstAddIndication->sfAuthorizedSet.u8VendorSpecificQoSParamLength);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8VendorSpecificQoSParam: 0x%X",
-			pstAddIndication->sfAuthorizedSet.u8VendorSpecificQoSParam[0]);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ServiceFlowSchedulingType: 0x%X",
-			pstAddIndication->sfAuthorizedSet.u8ServiceFlowSchedulingType);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u32ToleratedJitter: 0x%X", pstAddIndication->sfAuthorizedSet.u32ToleratedJitter);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u32MaximumLatency: 0x%X", pstAddIndication->sfAuthorizedSet.u32MaximumLatency);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8FixedLengthVSVariableLengthSDUIndicator: 0x%X",
-			pstAddIndication->sfAuthorizedSet.u8FixedLengthVSVariableLengthSDUIndicator);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8SDUSize: 0x%X",	pstAddIndication->sfAuthorizedSet.u8SDUSize);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16TargetSAID: 0x%X", pstAddIndication->sfAuthorizedSet.u16TargetSAID);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ARQEnable: 0x%X", pstAddIndication->sfAuthorizedSet.u8ARQEnable);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16ARQWindowSize: 0x%X", pstAddIndication->sfAuthorizedSet.u16ARQWindowSize);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16ARQRetryTxTimeOut: 0x%X", pstAddIndication->sfAuthorizedSet.u16ARQRetryTxTimeOut);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16ARQRetryRxTimeOut: 0x%X", pstAddIndication->sfAuthorizedSet.u16ARQRetryRxTimeOut);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16ARQBlockLifeTime: 0x%X", pstAddIndication->sfAuthorizedSet.u16ARQBlockLifeTime);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16ARQSyncLossTimeOut: 0x%X", pstAddIndication->sfAuthorizedSet.u16ARQSyncLossTimeOut);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ARQDeliverInOrder: 0x%X", pstAddIndication->sfAuthorizedSet.u8ARQDeliverInOrder);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16ARQRxPurgeTimeOut: 0x%X", pstAddIndication->sfAuthorizedSet.u16ARQRxPurgeTimeOut);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16ARQBlockSize: 0x%X", pstAddIndication->sfAuthorizedSet.u16ARQBlockSize);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8CSSpecification: 0x%X",	pstAddIndication->sfAuthorizedSet.u8CSSpecification);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8TypeOfDataDeliveryService: 0x%X",
-			pstAddIndication->sfAuthorizedSet.u8TypeOfDataDeliveryService);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16SDUInterArrivalTime: 0x%X", pstAddIndication->sfAuthorizedSet.u16SDUInterArrivalTime);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16TimeBase: 0x%X", pstAddIndication->sfAuthorizedSet.u16TimeBase);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8PagingPreference: 0x%X", pstAddIndication->sfAuthorizedSet.u8PagingPreference);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16UnsolicitedPollingInterval: 0x%X",
-			pstAddIndication->sfAuthorizedSet.u16UnsolicitedPollingInterval);
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "sfAuthorizedSet.u8HARQChannelMapping %x  %x %x ",
-			*(unsigned int *)pstAddIndication->sfAuthorizedSet.u8HARQChannelMapping,
-			*(unsigned int *)&pstAddIndication->sfAuthorizedSet.u8HARQChannelMapping[4],
-			*(USHORT *)&pstAddIndication->sfAuthorizedSet.u8HARQChannelMapping[8]);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8TrafficIndicationPreference: 0x%X",
-			pstAddIndication->sfAuthorizedSet.u8TrafficIndicationPreference);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " Total Classifiers Received: 0x%X", pstAddIndication->sfAuthorizedSet.u8TotalClassifiers);
-
-	nCurClassifierCnt = pstAddIndication->sfAuthorizedSet.u8TotalClassifiers;
-	if (nCurClassifierCnt > MAX_CLASSIFIERS_IN_SF)
-		nCurClassifierCnt = MAX_CLASSIFIERS_IN_SF;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL,  "pstAddIndication->sfAuthorizedSet.bValid %d", pstAddIndication->sfAuthorizedSet.bValid);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL,  "pstAddIndication->sfAuthorizedSet.u16MacOverhead %x", pstAddIndication->sfAuthorizedSet.u16MacOverhead);
-	if (!pstAddIndication->sfAuthorizedSet.bValid)
-		pstAddIndication->sfAuthorizedSet.bValid = 1;
-	for (nIndex = 0; nIndex < nCurClassifierCnt; nIndex++) {
-		struct bcm_convergence_types *psfCSType = NULL;
-
-		psfCSType =  &pstAddIndication->sfAuthorizedSet.cConvergenceSLTypes[nIndex];
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "psfCSType = %p", psfCSType);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "CCPacketClassificationRuleSI====>");
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ClassifierRulePriority: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8ClassifierRulePriority);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL,  "u8IPTypeOfServiceLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8IPTypeOfServiceLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPTypeOfService[3]: 0x%X ,0x%X ,0x%X ",
-				psfCSType->cCPacketClassificationRule.u8IPTypeOfService[0],
-				psfCSType->cCPacketClassificationRule.u8IPTypeOfService[1],
-				psfCSType->cCPacketClassificationRule.u8IPTypeOfService[2]);
-
-		for (uiLoopIndex = 0; uiLoopIndex < 1; uiLoopIndex++)
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8Protocol: 0x%02X ",
-					psfCSType->cCPacketClassificationRule.u8Protocol);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPMaskedSourceAddressLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8IPMaskedSourceAddressLength);
-
-		for (uiLoopIndex = 0; uiLoopIndex < 32; uiLoopIndex++)
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPMaskedSourceAddress[32]: 0x%02X ",
-					psfCSType->cCPacketClassificationRule.u8IPMaskedSourceAddress[uiLoopIndex]);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPDestinationAddressLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8IPDestinationAddressLength);
-
-		for (uiLoopIndex = 0; uiLoopIndex < 32; uiLoopIndex++)
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPDestinationAddress[32]: 0x%02X ",
-					psfCSType->cCPacketClassificationRule.u8IPDestinationAddress[uiLoopIndex]);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ProtocolSourcePortRangeLength:0x%X ",
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRangeLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ProtocolSourcePortRange[4]: 0x%02X ,0x%02X ,0x%02X ,0x%02X ",
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRange[0],
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRange[1],
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRange[2],
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRange[3]);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ProtocolDestPortRangeLength: 0x%02X ",
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRangeLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ProtocolDestPortRange[4]: 0x%02X ,0x%02X ,0x%02X ,0x%02X ",
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange[0],
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange[1],
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange[2],
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange[3]);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthernetDestMacAddressLength: 0x%02X ",
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddressLength);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL, "u8EthernetDestMacAddress[6]: %pM",
-				psfCSType->cCPacketClassificationRule.
-						u8EthernetDestMacAddress);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthernetSourceMACAddressLength: 0x%02X ",
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddressLength);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL, "u8EthernetSourceMACAddress[6]: %pM",
-				psfCSType->cCPacketClassificationRule.
-						u8EthernetSourceMACAddress);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthertypeLength: 0x%02X ",
-				psfCSType->cCPacketClassificationRule.u8EthertypeLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8Ethertype[3]: 0x%02X ,0x%02X ,0x%02X ",
-				psfCSType->cCPacketClassificationRule.u8Ethertype[0],
-				psfCSType->cCPacketClassificationRule.u8Ethertype[1],
-				psfCSType->cCPacketClassificationRule.u8Ethertype[2]);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16UserPriority: 0x%X ", psfCSType->cCPacketClassificationRule.u16UserPriority);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16VLANID: 0x%X ", psfCSType->cCPacketClassificationRule.u16VLANID);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8AssociatedPHSI: 0x%02X ", psfCSType->cCPacketClassificationRule.u8AssociatedPHSI);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16PacketClassificationRuleIndex: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u16PacketClassificationRuleIndex);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8VendorSpecificClassifierParamLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8VendorSpecificClassifierParamLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8VendorSpecificClassifierParam[1]: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8VendorSpecificClassifierParam[0]);
-#ifdef VERSION_D5
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPv6FlowLableLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLableLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL, "u8IPv6FlowLable[6]: 0x%*ph ",
-				6, psfCSType->cCPacketClassificationRule.
-					      u8IPv6FlowLable);
-#endif
-	}
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "bValid: 0x%02X", pstAddIndication->sfAuthorizedSet.bValid);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "AdmittedSet--->");
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u32SFID: 0x%X", pstAddIndication->sfAdmittedSet.u32SFID);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16CID: 0x%X", pstAddIndication->sfAdmittedSet.u16CID);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ServiceClassNameLength: 0x%X",
-			pstAddIndication->sfAdmittedSet.u8ServiceClassNameLength);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL,
-			"u8ServiceClassName: 0x%*ph",
-			6, pstAddIndication->sfAdmittedSet.u8ServiceClassName);
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8MBSService: 0x%02X", pstAddIndication->sfAdmittedSet.u8MBSService);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8QosParamSet: 0x%02X", pstAddIndication->sfAdmittedSet.u8QosParamSet);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8TrafficPriority: 0x%02X", pstAddIndication->sfAdmittedSet.u8TrafficPriority);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u32MaxTrafficBurst: 0x%X", pstAddIndication->sfAdmittedSet.u32MaxTrafficBurst);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u32MinReservedTrafficRate: 0x%X",
-			pstAddIndication->sfAdmittedSet.u32MinReservedTrafficRate);
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8VendorSpecificQoSParamLength: 0x%02X",
-			pstAddIndication->sfAdmittedSet.u8VendorSpecificQoSParamLength);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8VendorSpecificQoSParam: 0x%02X",
-			pstAddIndication->sfAdmittedSet.u8VendorSpecificQoSParam[0]);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ServiceFlowSchedulingType: 0x%02X",
-			pstAddIndication->sfAdmittedSet.u8ServiceFlowSchedulingType);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u32ToleratedJitter: 0x%X", pstAddIndication->sfAdmittedSet.u32ToleratedJitter);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u32MaximumLatency: 0x%X", pstAddIndication->sfAdmittedSet.u32MaximumLatency);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8FixedLengthVSVariableLengthSDUIndicator: 0x%02X",
-			pstAddIndication->sfAdmittedSet.u8FixedLengthVSVariableLengthSDUIndicator);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8SDUSize: 0x%02X", pstAddIndication->sfAdmittedSet.u8SDUSize);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16TargetSAID: 0x%02X", pstAddIndication->sfAdmittedSet.u16TargetSAID);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ARQEnable: 0x%02X", pstAddIndication->sfAdmittedSet.u8ARQEnable);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16ARQWindowSize: 0x%X", pstAddIndication->sfAdmittedSet.u16ARQWindowSize);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16ARQRetryTxTimeOut: 0x%X", pstAddIndication->sfAdmittedSet.u16ARQRetryTxTimeOut);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16ARQRetryRxTimeOut: 0x%X", pstAddIndication->sfAdmittedSet.u16ARQRetryRxTimeOut);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16ARQBlockLifeTime: 0x%X", pstAddIndication->sfAdmittedSet.u16ARQBlockLifeTime);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16ARQSyncLossTimeOut: 0x%X", pstAddIndication->sfAdmittedSet.u16ARQSyncLossTimeOut);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ARQDeliverInOrder: 0x%02X", pstAddIndication->sfAdmittedSet.u8ARQDeliverInOrder);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16ARQRxPurgeTimeOut: 0x%X", pstAddIndication->sfAdmittedSet.u16ARQRxPurgeTimeOut);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16ARQBlockSize: 0x%X", pstAddIndication->sfAdmittedSet.u16ARQBlockSize);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8CSSpecification: 0x%02X", pstAddIndication->sfAdmittedSet.u8CSSpecification);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8TypeOfDataDeliveryService: 0x%02X",
-			pstAddIndication->sfAdmittedSet.u8TypeOfDataDeliveryService);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16SDUInterArrivalTime: 0x%X", pstAddIndication->sfAdmittedSet.u16SDUInterArrivalTime);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16TimeBase: 0x%X", pstAddIndication->sfAdmittedSet.u16TimeBase);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8PagingPreference: 0x%X", pstAddIndication->sfAdmittedSet.u8PagingPreference);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8TrafficIndicationPreference: 0x%02X",
-			pstAddIndication->sfAdmittedSet.u8TrafficIndicationPreference);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " Total Classifiers Received: 0x%X", pstAddIndication->sfAdmittedSet.u8TotalClassifiers);
-
-	nCurClassifierCnt = pstAddIndication->sfAdmittedSet.u8TotalClassifiers;
-	if (nCurClassifierCnt > MAX_CLASSIFIERS_IN_SF)
-		nCurClassifierCnt = MAX_CLASSIFIERS_IN_SF;
-
-	for (nIndex = 0; nIndex < nCurClassifierCnt; nIndex++) {
-		struct bcm_convergence_types *psfCSType = NULL;
-
-		psfCSType =  &pstAddIndication->sfAdmittedSet.cConvergenceSLTypes[nIndex];
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " CCPacketClassificationRuleSI====>");
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ClassifierRulePriority: 0x%02X ",
-				psfCSType->cCPacketClassificationRule.u8ClassifierRulePriority);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPTypeOfServiceLength: 0x%02X",
-				psfCSType->cCPacketClassificationRule.u8IPTypeOfServiceLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL, "u8IPTypeOfService[3]: 0x%*ph",
-				3, psfCSType->cCPacketClassificationRule.
-					      u8IPTypeOfService);
-		for (uiLoopIndex = 0; uiLoopIndex < 1; uiLoopIndex++)
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8Protocol: 0x%02X ", psfCSType->cCPacketClassificationRule.u8Protocol);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPMaskedSourceAddressLength: 0x%02X ",
-				psfCSType->cCPacketClassificationRule.u8IPMaskedSourceAddressLength);
-
-		for (uiLoopIndex = 0; uiLoopIndex < 32; uiLoopIndex++)
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPMaskedSourceAddress[32]: 0x%02X ",
-					psfCSType->cCPacketClassificationRule.u8IPMaskedSourceAddress[uiLoopIndex]);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPDestinationAddressLength: 0x%02X ",
-				psfCSType->cCPacketClassificationRule.u8IPDestinationAddressLength);
-
-		for (uiLoopIndex = 0; uiLoopIndex < 32; uiLoopIndex++)
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPDestinationAddress[32]: 0x%02X ",
-					psfCSType->cCPacketClassificationRule.u8IPDestinationAddress[uiLoopIndex]);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ProtocolSourcePortRangeLength: 0x%02X ",
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRangeLength);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL, "u8ProtocolSourcePortRange[4]: 0x%*ph ",
-				4, psfCSType->cCPacketClassificationRule.
-						u8ProtocolSourcePortRange);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ProtocolDestPortRangeLength: 0x%02X ",
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRangeLength);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL, "u8ProtocolDestPortRange[4]: 0x%*ph ",
-				4, psfCSType->cCPacketClassificationRule.
-						u8ProtocolDestPortRange);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthernetDestMacAddressLength: 0x%02X ",
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddressLength);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL, "u8EthernetDestMacAddress[6]: %pM",
-				psfCSType->cCPacketClassificationRule.
-						u8EthernetDestMacAddress);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthernetSourceMACAddressLength: 0x%02X ",
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddressLength);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL, "u8EthernetSourceMACAddress[6]: %pM",
-				psfCSType->cCPacketClassificationRule.
-						u8EthernetSourceMACAddress);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthertypeLength: 0x%02X ", psfCSType->cCPacketClassificationRule.u8EthertypeLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL, "u8Ethertype[3]: 0x%*ph",
-				3, psfCSType->cCPacketClassificationRule.
-					      u8Ethertype);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16UserPriority: 0x%X ", psfCSType->cCPacketClassificationRule.u16UserPriority);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16VLANID: 0x%X ", psfCSType->cCPacketClassificationRule.u16VLANID);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8AssociatedPHSI: 0x%02X ", psfCSType->cCPacketClassificationRule.u8AssociatedPHSI);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16PacketClassificationRuleIndex: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u16PacketClassificationRuleIndex);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8VendorSpecificClassifierParamLength: 0x%02X",
-				psfCSType->cCPacketClassificationRule.u8VendorSpecificClassifierParamLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8VendorSpecificClassifierParam[1]: 0x%02X ",
-				psfCSType->cCPacketClassificationRule.u8VendorSpecificClassifierParam[0]);
-#ifdef VERSION_D5
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPv6FlowLableLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLableLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL, "u8IPv6FlowLable[6]: 0x%*ph ",
-				6, psfCSType->cCPacketClassificationRule.
-					      u8IPv6FlowLable);
-#endif
-	}
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "bValid: 0x%X", pstAddIndication->sfAdmittedSet.bValid);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " ActiveSet--->");
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u32SFID: 0x%X", pstAddIndication->sfActiveSet.u32SFID);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u16CID: 0x%X", pstAddIndication->sfActiveSet.u16CID);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ServiceClassNameLength: 0x%X", pstAddIndication->sfActiveSet.u8ServiceClassNameLength);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL,
-			"u8ServiceClassName: 0x%*ph",
-			6, pstAddIndication->sfActiveSet.u8ServiceClassName);
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8MBSService: 0x%02X", pstAddIndication->sfActiveSet.u8MBSService);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8QosParamSet: 0x%02X", pstAddIndication->sfActiveSet.u8QosParamSet);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8TrafficPriority: 0x%02X", pstAddIndication->sfActiveSet.u8TrafficPriority);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u32MaxTrafficBurst: 0x%X", pstAddIndication->sfActiveSet.u32MaxTrafficBurst);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u32MinReservedTrafficRate: 0x%X",
-			pstAddIndication->sfActiveSet.u32MinReservedTrafficRate);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8VendorSpecificQoSParamLength: 0x%02X",
-			pstAddIndication->sfActiveSet.u8VendorSpecificQoSParamLength);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8VendorSpecificQoSParam: 0x%02X",
-			pstAddIndication->sfActiveSet.u8VendorSpecificQoSParam[0]);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8ServiceFlowSchedulingType: 0x%02X",
-			pstAddIndication->sfActiveSet.u8ServiceFlowSchedulingType);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u32ToleratedJitter: 0x%X", pstAddIndication->sfActiveSet.u32ToleratedJitter);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u32MaximumLatency: 0x%X",	pstAddIndication->sfActiveSet.u32MaximumLatency);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8FixedLengthVSVariableLengthSDUIndicator: 0x%02X",
-			pstAddIndication->sfActiveSet.u8FixedLengthVSVariableLengthSDUIndicator);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8SDUSize: 0x%X",	pstAddIndication->sfActiveSet.u8SDUSize);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u16TargetSAID: 0x%X", pstAddIndication->sfActiveSet.u16TargetSAID);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8ARQEnable: 0x%X", pstAddIndication->sfActiveSet.u8ARQEnable);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u16ARQWindowSize: 0x%X", pstAddIndication->sfActiveSet.u16ARQWindowSize);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u16ARQRetryTxTimeOut: 0x%X", pstAddIndication->sfActiveSet.u16ARQRetryTxTimeOut);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u16ARQRetryRxTimeOut: 0x%X", pstAddIndication->sfActiveSet.u16ARQRetryRxTimeOut);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u16ARQBlockLifeTime: 0x%X", pstAddIndication->sfActiveSet.u16ARQBlockLifeTime);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u16ARQSyncLossTimeOut: 0x%X", pstAddIndication->sfActiveSet.u16ARQSyncLossTimeOut);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8ARQDeliverInOrder: 0x%X", pstAddIndication->sfActiveSet.u8ARQDeliverInOrder);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u16ARQRxPurgeTimeOut: 0x%X", pstAddIndication->sfActiveSet.u16ARQRxPurgeTimeOut);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u16ARQBlockSize: 0x%X", pstAddIndication->sfActiveSet.u16ARQBlockSize);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8CSSpecification: 0x%X", pstAddIndication->sfActiveSet.u8CSSpecification);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8TypeOfDataDeliveryService: 0x%X",
-			pstAddIndication->sfActiveSet.u8TypeOfDataDeliveryService);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u16SDUInterArrivalTime: 0x%X", pstAddIndication->sfActiveSet.u16SDUInterArrivalTime);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u16TimeBase: 0x%X", pstAddIndication->sfActiveSet.u16TimeBase);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8PagingPreference: 0x%X", pstAddIndication->sfActiveSet.u8PagingPreference);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8TrafficIndicationPreference: 0x%X",
-			pstAddIndication->sfActiveSet.u8TrafficIndicationPreference);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " Total Classifiers Received: 0x%X", pstAddIndication->sfActiveSet.u8TotalClassifiers);
-
-	nCurClassifierCnt = pstAddIndication->sfActiveSet.u8TotalClassifiers;
-	if (nCurClassifierCnt > MAX_CLASSIFIERS_IN_SF)
-		nCurClassifierCnt = MAX_CLASSIFIERS_IN_SF;
-
-	for (nIndex = 0; nIndex < nCurClassifierCnt; nIndex++)	{
-		struct bcm_convergence_types *psfCSType = NULL;
-		struct bcm_packet_class_rules *clsRule = NULL;
-
-		psfCSType = &pstAddIndication->sfActiveSet.cConvergenceSLTypes[nIndex];
-		clsRule	= &psfCSType->cCPacketClassificationRule;
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL, " CCPacketClassificationRuleSI====>");
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL, " u8ClassifierRulePriority: 0x%X ",
-				clsRule->u8ClassifierRulePriority);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL, " u8IPTypeOfServiceLength: 0x%X ",
-				clsRule->u8IPTypeOfServiceLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL,
-				" u8IPTypeOfService[3]: 0x%X ,0x%X ,0x%X ",
-				clsRule->u8IPTypeOfService[0],
-				clsRule->u8IPTypeOfService[1],
-				clsRule->u8IPTypeOfService[2]);
-
-		for (uiLoopIndex = 0; uiLoopIndex < 1; uiLoopIndex++)
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-					DBG_LVL_ALL,
-					" u8Protocol: 0x%X ",
-					clsRule->u8Protocol);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL,
-				"u8IPMaskedSourceAddressLength: 0x%X ",
-				clsRule->u8IPMaskedSourceAddressLength);
-
-		for (uiLoopIndex = 0; uiLoopIndex < 32; uiLoopIndex++)
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-					DBG_LVL_ALL,
-					"u8IPMaskedSourceAddress[32]: 0x%X ",
-					clsRule->u8IPMaskedSourceAddress[uiLoopIndex]);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL,
-				"u8IPDestinationAddressLength: 0x%02X ",
-				clsRule->u8IPDestinationAddressLength);
-
-		for (uiLoopIndex = 0; uiLoopIndex < 32; uiLoopIndex++)
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-					DBG_LVL_ALL,
-					" u8IPDestinationAddress[32]:0x%X ",
-					clsRule->u8IPDestinationAddress[uiLoopIndex]);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL,
-				" u8ProtocolSourcePortRangeLength: 0x%X ",
-				clsRule->u8ProtocolSourcePortRangeLength);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL,
-				" u8ProtocolSourcePortRange[4]: 0x%X ,0x%X ,0x%X ,0x%X ",
-				clsRule->u8ProtocolSourcePortRange[0],
-				clsRule->u8ProtocolSourcePortRange[1],
-				clsRule->u8ProtocolSourcePortRange[2],
-				clsRule->u8ProtocolSourcePortRange[3]);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL,
-				" u8ProtocolDestPortRangeLength: 0x%X ",
-				clsRule->u8ProtocolDestPortRangeLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL,
-				" u8ProtocolDestPortRange[4]: 0x%X ,0x%X ,0x%X ,0x%X ",
-				clsRule->u8ProtocolDestPortRange[0],
-				clsRule->u8ProtocolDestPortRange[1],
-				clsRule->u8ProtocolDestPortRange[2],
-				clsRule->u8ProtocolDestPortRange[3]);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL,
-				" u8EthernetDestMacAddressLength: 0x%X ",
-				clsRule->u8EthernetDestMacAddressLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL,
-				" u8EthernetDestMacAddress[6]: 0x%X ,0x%X ,0x%X ,0x%X ,0x%X ,0x%X",
-				clsRule->u8EthernetDestMacAddress[0],
-				clsRule->u8EthernetDestMacAddress[1],
-				clsRule->u8EthernetDestMacAddress[2],
-				clsRule->u8EthernetDestMacAddress[3],
-				clsRule->u8EthernetDestMacAddress[4],
-				clsRule->u8EthernetDestMacAddress[5]);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL,
-				" u8EthernetSourceMACAddressLength: 0x%X ",
-				clsRule->u8EthernetDestMacAddressLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL,
-				"u8EthernetSourceMACAddress[6]: 0x%X ,0x%X ,0x%X ,0x%X ,0x%X ,0x%X",
-				clsRule->u8EthernetSourceMACAddress[0],
-				clsRule->u8EthernetSourceMACAddress[1],
-				clsRule->u8EthernetSourceMACAddress[2],
-				clsRule->u8EthernetSourceMACAddress[3],
-				clsRule->u8EthernetSourceMACAddress[4],
-				clsRule->u8EthernetSourceMACAddress[5]);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL, " u8EthertypeLength: 0x%X ",
-				clsRule->u8EthertypeLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL,
-				" u8Ethertype[3]: 0x%X ,0x%X ,0x%X ",
-				clsRule->u8Ethertype[0],
-				clsRule->u8Ethertype[1],
-				clsRule->u8Ethertype[2]);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL, " u16UserPriority: 0x%X ",
-				clsRule->u16UserPriority);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL, " u16VLANID: 0x%X ",
-				clsRule->u16VLANID);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL, " u8AssociatedPHSI: 0x%X ",
-				clsRule->u8AssociatedPHSI);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL,
-				" u16PacketClassificationRuleIndex:0x%X ",
-				clsRule->u16PacketClassificationRuleIndex);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL,
-				" u8VendorSpecificClassifierParamLength:0x%X ",
-				clsRule->u8VendorSpecificClassifierParamLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL,
-				" u8VendorSpecificClassifierParam[1]:0x%X ",
-				clsRule->u8VendorSpecificClassifierParam[0]);
-#ifdef VERSION_D5
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL, " u8IPv6FlowLableLength: 0x%X ",
-				clsRule->u8IPv6FlowLableLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
-				DBG_LVL_ALL,
-				" u8IPv6FlowLable[6]: 0x%X ,0x%X ,0x%X ,0x%X ,0x%X ,0x%X ",
-				clsRule->u8IPv6FlowLable[0],
-				clsRule->u8IPv6FlowLable[1],
-				clsRule->u8IPv6FlowLable[2],
-				clsRule->u8IPv6FlowLable[3],
-				clsRule->u8IPv6FlowLable[4],
-				clsRule->u8IPv6FlowLable[5]);
-#endif
-	}
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL,
-			" bValid: 0x%X", pstAddIndication->sfActiveSet.bValid);
-}
-
-static inline ULONG RestoreSFParam(struct bcm_mini_adapter *Adapter,
-		ULONG ulAddrSFParamSet, PUCHAR pucDestBuffer)
-{
-	UINT  nBytesToRead = sizeof(struct bcm_connect_mgr_params);
-
-	if (ulAddrSFParamSet == 0 || NULL == pucDestBuffer) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-				"Got Param address as 0!!");
-		return 0;
-	}
-	ulAddrSFParamSet = ntohl(ulAddrSFParamSet);
-
-	/* Read out the SF Param Set At the indicated Location */
-	if (rdm(Adapter, ulAddrSFParamSet, (PUCHAR)pucDestBuffer, nBytesToRead) < 0)
-		return STATUS_FAILURE;
-
-	return 1;
-}
-
-static ULONG StoreSFParam(struct bcm_mini_adapter *Adapter, PUCHAR pucSrcBuffer,
-		ULONG ulAddrSFParamSet)
-{
-	UINT nBytesToWrite = sizeof(struct bcm_connect_mgr_params);
-	int ret = 0;
-
-	if (ulAddrSFParamSet == 0 || NULL == pucSrcBuffer)
-		return 0;
-
-	ret = wrm(Adapter, ulAddrSFParamSet, (u8 *)pucSrcBuffer, nBytesToWrite);
-	if (ret < 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-				"%s:%d WRM failed", __func__, __LINE__);
-		return ret;
-	}
-	return 1;
-}
-
-ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter,
-		PVOID pvBuffer, UINT *puBufferLength)
-{
-	struct bcm_add_indication_alt *pstAddIndicationAlt = NULL;
-	struct bcm_add_indication *pstAddIndication = NULL;
-	struct bcm_del_request *pstDeletionRequest;
-	UINT uiSearchRuleIndex;
-	ULONG ulSFID;
-
-	pstAddIndicationAlt = pvBuffer;
-
-	/*
-	 * In case of DSD Req By MS, we should immediately delete this SF so that
-	 * we can stop the further classifying the pkt for this SF.
-	 */
-	if (pstAddIndicationAlt->u8Type == DSD_REQ) {
-		pstDeletionRequest = pvBuffer;
-
-		ulSFID = ntohl(pstDeletionRequest->u32SFID);
-		uiSearchRuleIndex = SearchSfid(Adapter, ulSFID);
-
-		if (uiSearchRuleIndex < NO_OF_QUEUES) {
-			deleteSFBySfid(Adapter, uiSearchRuleIndex);
-			Adapter->u32TotalDSD++;
-		}
-		return 1;
-	}
-
-	if ((pstAddIndicationAlt->u8Type == DSD_RSP) ||
-		(pstAddIndicationAlt->u8Type == DSD_ACK)) {
-		/* No Special handling send the message as it is */
-		return 1;
-	}
-	/* For DSA_REQ, only up to "psfAuthorizedSet" parameter should be accessed by driver! */
-
-	pstAddIndication = kmalloc(sizeof(struct bcm_add_indication),
-			GFP_KERNEL);
-	if (pstAddIndication == NULL)
-		return 0;
-
-	/* AUTHORIZED SET */
-	pstAddIndication->psfAuthorizedSet = (struct bcm_connect_mgr_params *)
-			GetNextTargetBufferLocation(Adapter,
-					pstAddIndicationAlt->u16TID);
-	if (!pstAddIndication->psfAuthorizedSet) {
-		kfree(pstAddIndication);
-		return 0;
-	}
-
-	if (StoreSFParam(Adapter, (PUCHAR)&pstAddIndicationAlt->sfAuthorizedSet,
-				(ULONG)pstAddIndication->psfAuthorizedSet) != 1) {
-		kfree(pstAddIndication);
-		return 0;
-	}
-
-	/* this can't possibly be right */
-	pstAddIndication->psfAuthorizedSet =
-		(struct bcm_connect_mgr_params *) ntohl(
-				(ULONG)pstAddIndication->psfAuthorizedSet);
-
-	if (pstAddIndicationAlt->u8Type == DSA_REQ) {
-		struct bcm_add_request AddRequest;
-
-		AddRequest.u8Type = pstAddIndicationAlt->u8Type;
-		AddRequest.eConnectionDir = pstAddIndicationAlt->u8Direction;
-		AddRequest.u16TID = pstAddIndicationAlt->u16TID;
-		AddRequest.u16CID = pstAddIndicationAlt->u16CID;
-		AddRequest.u16VCID = pstAddIndicationAlt->u16VCID;
-		AddRequest.psfParameterSet = pstAddIndication->psfAuthorizedSet;
-		(*puBufferLength) = sizeof(struct bcm_add_request);
-		memcpy(pvBuffer, &AddRequest, sizeof(struct bcm_add_request));
-		kfree(pstAddIndication);
-		return 1;
-	}
-
-	/* Since it's not DSA_REQ, we can access all field in pstAddIndicationAlt */
-	/* We need to extract the structure from the buffer and pack it differently */
-
-	pstAddIndication->u8Type = pstAddIndicationAlt->u8Type;
-	pstAddIndication->eConnectionDir = pstAddIndicationAlt->u8Direction;
-	pstAddIndication->u16TID = pstAddIndicationAlt->u16TID;
-	pstAddIndication->u16CID = pstAddIndicationAlt->u16CID;
-	pstAddIndication->u16VCID = pstAddIndicationAlt->u16VCID;
-	pstAddIndication->u8CC = pstAddIndicationAlt->u8CC;
-
-	/* ADMITTED SET */
-	pstAddIndication->psfAdmittedSet = (struct bcm_connect_mgr_params *)
-		GetNextTargetBufferLocation(Adapter,
-				pstAddIndicationAlt->u16TID);
-	if (!pstAddIndication->psfAdmittedSet) {
-		kfree(pstAddIndication);
-		return 0;
-	}
-	if (StoreSFParam(Adapter, (PUCHAR)&pstAddIndicationAlt->sfAdmittedSet,
-				(ULONG)pstAddIndication->psfAdmittedSet) != 1) {
-		kfree(pstAddIndication);
-		return 0;
-	}
-
-	pstAddIndication->psfAdmittedSet =
-		(struct bcm_connect_mgr_params *) ntohl(
-				(ULONG) pstAddIndication->psfAdmittedSet);
-
-	/* ACTIVE SET */
-	pstAddIndication->psfActiveSet = (struct bcm_connect_mgr_params *)
-		GetNextTargetBufferLocation(Adapter,
-				pstAddIndicationAlt->u16TID);
-	if (!pstAddIndication->psfActiveSet) {
-		kfree(pstAddIndication);
-		return 0;
-	}
-	if (StoreSFParam(Adapter, (PUCHAR)&pstAddIndicationAlt->sfActiveSet,
-				(ULONG)pstAddIndication->psfActiveSet) != 1) {
-		kfree(pstAddIndication);
-		return 0;
-	}
-
-	pstAddIndication->psfActiveSet =
-		(struct bcm_connect_mgr_params *) ntohl(
-				(ULONG)pstAddIndication->psfActiveSet);
-
-	(*puBufferLength) = sizeof(struct bcm_add_indication);
-	*(struct bcm_add_indication *)pvBuffer = *pstAddIndication;
-	kfree(pstAddIndication);
-	return 1;
-}
-
-static inline struct bcm_add_indication_alt
-*RestoreCmControlResponseMessage(register struct bcm_mini_adapter *Adapter,
-		register PVOID pvBuffer)
-{
-	ULONG ulStatus = 0;
-	struct bcm_add_indication *pstAddIndication = NULL;
-	struct bcm_add_indication_alt *pstAddIndicationDest = NULL;
-
-	pstAddIndication = pvBuffer;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"=====>");
-	if ((pstAddIndication->u8Type == DSD_REQ) ||
-		(pstAddIndication->u8Type == DSD_RSP) ||
-		(pstAddIndication->u8Type == DSD_ACK))
-		return pvBuffer;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"Inside RestoreCmControlResponseMessage ");
-	/*
-	 * Need to Allocate memory to contain the SUPER Large structures
-	 * Our driver can't create these structures on Stack :(
-	 */
-	pstAddIndicationDest = kmalloc(sizeof(struct bcm_add_indication_alt),
-			GFP_KERNEL);
-
-	if (pstAddIndicationDest) {
-		memset(pstAddIndicationDest, 0,
-				sizeof(struct bcm_add_indication_alt));
-	} else {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
-				DBG_LVL_ALL,
-				"Failed to allocate memory for SF Add Indication Structure ");
-		return NULL;
-	}
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"AddIndication-u8Type : 0x%X",
-			pstAddIndication->u8Type);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"AddIndication-u8Direction : 0x%X",
-			pstAddIndication->eConnectionDir);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"AddIndication-u8TID : 0x%X",
-			ntohs(pstAddIndication->u16TID));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"AddIndication-u8CID : 0x%X",
-			ntohs(pstAddIndication->u16CID));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"AddIndication-u16VCID : 0x%X",
-			ntohs(pstAddIndication->u16VCID));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"AddIndication-autorized set loc : %p",
-			pstAddIndication->psfAuthorizedSet);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"AddIndication-admitted set loc : %p",
-			pstAddIndication->psfAdmittedSet);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"AddIndication-Active set loc : %p",
-			pstAddIndication->psfActiveSet);
-
-	pstAddIndicationDest->u8Type = pstAddIndication->u8Type;
-	pstAddIndicationDest->u8Direction = pstAddIndication->eConnectionDir;
-	pstAddIndicationDest->u16TID = pstAddIndication->u16TID;
-	pstAddIndicationDest->u16CID = pstAddIndication->u16CID;
-	pstAddIndicationDest->u16VCID = pstAddIndication->u16VCID;
-	pstAddIndicationDest->u8CC = pstAddIndication->u8CC;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"Restoring Active Set ");
-	ulStatus = RestoreSFParam(Adapter,
-			(ULONG)pstAddIndication->psfActiveSet,
-			(PUCHAR)&pstAddIndicationDest->sfActiveSet);
-	if (ulStatus != 1)
-		goto failed_restore_sf_param;
-
-	if (pstAddIndicationDest->sfActiveSet.u8TotalClassifiers > MAX_CLASSIFIERS_IN_SF)
-		pstAddIndicationDest->sfActiveSet.u8TotalClassifiers =
-			MAX_CLASSIFIERS_IN_SF;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"Restoring Admitted Set ");
-	ulStatus = RestoreSFParam(Adapter,
-			(ULONG)pstAddIndication->psfAdmittedSet,
-			(PUCHAR)&pstAddIndicationDest->sfAdmittedSet);
-	if (ulStatus != 1)
-		goto failed_restore_sf_param;
-
-	if (pstAddIndicationDest->sfAdmittedSet.u8TotalClassifiers > MAX_CLASSIFIERS_IN_SF)
-		pstAddIndicationDest->sfAdmittedSet.u8TotalClassifiers =
-			MAX_CLASSIFIERS_IN_SF;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"Restoring Authorized Set ");
-	ulStatus = RestoreSFParam(Adapter,
-			(ULONG)pstAddIndication->psfAuthorizedSet,
-			(PUCHAR)&pstAddIndicationDest->sfAuthorizedSet);
-	if (ulStatus != 1)
-		goto failed_restore_sf_param;
-
-	if (pstAddIndicationDest->sfAuthorizedSet.u8TotalClassifiers > MAX_CLASSIFIERS_IN_SF)
-		pstAddIndicationDest->sfAuthorizedSet.u8TotalClassifiers =
-			MAX_CLASSIFIERS_IN_SF;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"Dumping the whole raw packet");
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-		"============================================================");
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			" pstAddIndicationDest->sfActiveSet size  %zx %p",
-			sizeof(*pstAddIndicationDest), pstAddIndicationDest);
-	/* BCM_DEBUG_PRINT_BUFFER(Adapter,DBG_TYPE_OTHERS, CONN_MSG,
-	 *		DBG_LVL_ALL, (unsigned char *)pstAddIndicationDest,
-	 *		sizeof(*pstAddIndicationDest));
-	 */
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"============================================================");
-	return pstAddIndicationDest;
-failed_restore_sf_param:
-	kfree(pstAddIndicationDest);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"<=====");
-	return NULL;
-}
-
-ULONG SetUpTargetDsxBuffers(struct bcm_mini_adapter *Adapter)
-{
-	ULONG ulTargetDsxBuffersBase = 0;
-	ULONG ulCntTargetBuffers;
-	ULONG i;
-	int Status;
-
-	if (!Adapter) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-				"Adapter was NULL!!!");
-		return 0;
-	}
-
-	if (Adapter->astTargetDsxBuffer[0].ulTargetDsxBuffer)
-		return 1;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"Size of Each DSX Buffer(Also size of connection manager parameters): %zx ",
-			sizeof(struct bcm_connect_mgr_params));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"Reading DSX buffer From Target location %x ",
-			DSX_MESSAGE_EXCHANGE_BUFFER);
-
-	Status = rdmalt(Adapter, DSX_MESSAGE_EXCHANGE_BUFFER,
-			(PUINT)&ulTargetDsxBuffersBase, sizeof(UINT));
-	if (Status < 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-				"RDM failed!!");
-		return 0;
-	}
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"Base Address Of DSX  Target Buffer : 0x%lx",
-			ulTargetDsxBuffersBase);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"Tgt Buffer is Now %lx :", ulTargetDsxBuffersBase);
-	ulCntTargetBuffers = DSX_MESSAGE_EXCHANGE_BUFFER_SIZE /
-		sizeof(struct bcm_connect_mgr_params);
-
-	Adapter->ulTotalTargetBuffersAvailable =
-		ulCntTargetBuffers > MAX_TARGET_DSX_BUFFERS ?
-		MAX_TARGET_DSX_BUFFERS : ulCntTargetBuffers;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			" Total Target DSX Buffer setup %lx ",
-			Adapter->ulTotalTargetBuffersAvailable);
-
-	for (i = 0; i < Adapter->ulTotalTargetBuffersAvailable; i++) {
-		Adapter->astTargetDsxBuffer[i].ulTargetDsxBuffer = ulTargetDsxBuffersBase;
-		Adapter->astTargetDsxBuffer[i].valid = 1;
-		Adapter->astTargetDsxBuffer[i].tid = 0;
-		ulTargetDsxBuffersBase += sizeof(struct bcm_connect_mgr_params);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "  Target DSX Buffer %lx setup at 0x%lx",
-				i, Adapter->astTargetDsxBuffer[i].ulTargetDsxBuffer);
-	}
-	Adapter->ulCurrentTargetBuffer = 0;
-	Adapter->ulFreeTargetBufferCnt = Adapter->ulTotalTargetBuffersAvailable;
-	return 1;
-}
-
-static ULONG GetNextTargetBufferLocation(struct bcm_mini_adapter *Adapter,
-		B_UINT16 tid)
-{
-	ULONG dsx_buf;
-	ULONG idx, max_try;
-
-	if ((Adapter->ulTotalTargetBuffersAvailable == 0)
-			|| (Adapter->ulFreeTargetBufferCnt == 0)) {
-		ClearTargetDSXBuffer(Adapter, tid, false);
-		return 0;
-	}
-
-	idx = Adapter->ulCurrentTargetBuffer;
-	max_try = Adapter->ulTotalTargetBuffersAvailable;
-	while ((max_try) && (Adapter->astTargetDsxBuffer[idx].valid != 1)) {
-		idx = (idx+1) % Adapter->ulTotalTargetBuffersAvailable;
-		max_try--;
-	}
-
-	if (max_try == 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-				"\n GetNextTargetBufferLocation : Error No Free Target DSX Buffers FreeCnt : %lx ",
-				Adapter->ulFreeTargetBufferCnt);
-		ClearTargetDSXBuffer(Adapter, tid, false);
-		return 0;
-	}
-
-	dsx_buf = Adapter->astTargetDsxBuffer[idx].ulTargetDsxBuffer;
-	Adapter->astTargetDsxBuffer[idx].valid = 0;
-	Adapter->astTargetDsxBuffer[idx].tid = tid;
-	Adapter->ulFreeTargetBufferCnt--;
-	idx = (idx+1)%Adapter->ulTotalTargetBuffersAvailable;
-	Adapter->ulCurrentTargetBuffer = idx;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"GetNextTargetBufferLocation :Returning address %lx tid %d\n",
-			dsx_buf, tid);
-
-	return dsx_buf;
-}
-
-int AllocAdapterDsxBuffer(struct bcm_mini_adapter *Adapter)
-{
-	/*
-	 * Need to Allocate memory to contain the SUPER Large structures
-	 * Our driver can't create these structures on Stack
-	 */
-	Adapter->caDsxReqResp = kmalloc(sizeof(struct bcm_add_indication_alt)
-			+ LEADER_SIZE, GFP_KERNEL);
-	if (!Adapter->caDsxReqResp)
-		return -ENOMEM;
-
-	return 0;
-}
-
-int FreeAdapterDsxBuffer(struct bcm_mini_adapter *Adapter)
-{
-	kfree(Adapter->caDsxReqResp);
-	return 0;
-}
-
-/*
- * @ingroup ctrl_pkt_functions
- * This routinue would process the Control responses
- * for the Connection Management.
- * @return - Queue index for the free SFID else returns Invalid Index.
- */
-bool CmControlResponseMessage(struct bcm_mini_adapter *Adapter,  /* <Pointer to the Adapter structure */
-				PVOID pvBuffer /* Starting Address of the Buffer, that contains the AddIndication Data */)
-{
-	struct bcm_connect_mgr_params *psfLocalSet = NULL;
-	struct bcm_add_indication_alt *pstAddIndication = NULL;
-	struct bcm_change_indication *pstChangeIndication = NULL;
-	struct bcm_leader *pLeader = NULL;
-	INT uiSearchRuleIndex = 0;
-	ULONG ulSFID;
-
-	/*
-	 * Otherwise the message contains a target address from where we need to
-	 * read out the rest of the service flow param structure
-	 */
-	pstAddIndication = RestoreCmControlResponseMessage(Adapter, pvBuffer);
-	if (pstAddIndication == NULL) {
-		ClearTargetDSXBuffer(Adapter, ((struct bcm_add_indication *)pvBuffer)->u16TID, false);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Error in restoring Service Flow param structure from DSx message");
-		return false;
-	}
-
-	DumpCmControlPacket(pstAddIndication);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "====>");
-	pLeader = (struct bcm_leader *)Adapter->caDsxReqResp;
-
-	pLeader->Status = CM_CONTROL_NEWDSX_MULTICLASSIFIER_REQ;
-	pLeader->Vcid = 0;
-
-	ClearTargetDSXBuffer(Adapter, pstAddIndication->u16TID, false);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "### TID RECEIVED %d\n", pstAddIndication->u16TID);
-	switch (pstAddIndication->u8Type) {
-	case DSA_REQ:
-		pLeader->PLength = sizeof(struct bcm_add_indication_alt);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Sending DSA Response....\n");
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "SENDING DSA RESPONSE TO MAC %d", pLeader->PLength);
-		*((struct bcm_add_indication_alt *)&(Adapter->caDsxReqResp[LEADER_SIZE]))
-			= *pstAddIndication;
-		((struct bcm_add_indication_alt *)&(Adapter->caDsxReqResp[LEADER_SIZE]))->u8Type = DSA_RSP;
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, " VCID = %x", ntohs(pstAddIndication->u16VCID));
-		CopyBufferToControlPacket(Adapter, (PVOID)Adapter->caDsxReqResp);
-		kfree(pstAddIndication);
-		break;
-	case DSA_RSP:
-		pLeader->PLength = sizeof(struct bcm_add_indication_alt);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "SENDING DSA ACK TO MAC %d",
-				pLeader->PLength);
-		*((struct bcm_add_indication_alt *)&(Adapter->caDsxReqResp[LEADER_SIZE]))
-			= *pstAddIndication;
-		((struct bcm_add_indication_alt *)&(Adapter->caDsxReqResp[LEADER_SIZE]))->u8Type = DSA_ACK;
-		/* FALLTHROUGH */
-	case DSA_ACK:
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "VCID:0x%X",
-				ntohs(pstAddIndication->u16VCID));
-		uiSearchRuleIndex = SearchFreeSfid(Adapter);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "uiSearchRuleIndex:0x%X ",
-				uiSearchRuleIndex);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Direction:0x%X ",
-				pstAddIndication->u8Direction);
-		if (uiSearchRuleIndex < NO_OF_QUEUES) {
-			Adapter->PackInfo[uiSearchRuleIndex].ucDirection =
-				pstAddIndication->u8Direction;
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "bValid:0x%X ",
-					pstAddIndication->sfActiveSet.bValid);
-			if (pstAddIndication->sfActiveSet.bValid == TRUE)
-				Adapter->PackInfo[uiSearchRuleIndex].bActiveSet = TRUE;
-
-			if (pstAddIndication->sfAuthorizedSet.bValid == TRUE)
-				Adapter->PackInfo[uiSearchRuleIndex].bAuthorizedSet = TRUE;
-
-			if (pstAddIndication->sfAdmittedSet.bValid == TRUE)
-				Adapter->PackInfo[uiSearchRuleIndex].bAdmittedSet = TRUE;
-
-			if (pstAddIndication->sfActiveSet.bValid == false) {
-				Adapter->PackInfo[uiSearchRuleIndex].bActive = false;
-				Adapter->PackInfo[uiSearchRuleIndex].bActivateRequestSent = false;
-				if (pstAddIndication->sfAdmittedSet.bValid)
-					psfLocalSet = &pstAddIndication->sfAdmittedSet;
-				else if (pstAddIndication->sfAuthorizedSet.bValid)
-					psfLocalSet = &pstAddIndication->sfAuthorizedSet;
-			} else {
-				psfLocalSet = &pstAddIndication->sfActiveSet;
-				Adapter->PackInfo[uiSearchRuleIndex].bActive = TRUE;
-			}
-
-			if (!psfLocalSet) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "No set is valid\n");
-				Adapter->PackInfo[uiSearchRuleIndex].bActive = false;
-				Adapter->PackInfo[uiSearchRuleIndex].bValid = false;
-				Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value = 0;
-				kfree(pstAddIndication);
-			} else if (psfLocalSet->bValid && (pstAddIndication->u8CC == 0)) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "DSA ACK");
-				Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value = ntohs(pstAddIndication->u16VCID);
-				Adapter->PackInfo[uiSearchRuleIndex].usCID = ntohs(pstAddIndication->u16CID);
-
-				if (UPLINK_DIR == pstAddIndication->u8Direction)
-					atomic_set(&Adapter->PackInfo[uiSearchRuleIndex].uiPerSFTxResourceCount, DEFAULT_PERSFCOUNT);
-
-				CopyToAdapter(Adapter, psfLocalSet, uiSearchRuleIndex, DSA_ACK, pstAddIndication);
-				/* don't free pstAddIndication */
-
-				/* Inside CopyToAdapter, Sorting of all the SFs take place.
-				 * Hence any access to the newly added SF through uiSearchRuleIndex is invalid.
-				 * SHOULD BE STRICTLY AVOIDED.
-				 */
-				/* *(PULONG)(((PUCHAR)pvBuffer)+1)=psfLocalSet->u32SFID; */
-				memcpy((((PUCHAR)pvBuffer)+1), &psfLocalSet->u32SFID, 4);
-
-				if (pstAddIndication->sfActiveSet.bValid == TRUE) {
-					if (UPLINK_DIR == pstAddIndication->u8Direction) {
-						if (!Adapter->LinkUpStatus) {
-							netif_carrier_on(Adapter->dev);
-							netif_start_queue(Adapter->dev);
-							Adapter->LinkUpStatus = 1;
-							if (netif_msg_link(Adapter))
-								pr_info(PFX "%s: link up\n", Adapter->dev->name);
-							atomic_set(&Adapter->TxPktAvail, 1);
-							wake_up(&Adapter->tx_packet_wait_queue);
-							Adapter->liTimeSinceLastNetEntry = get_seconds();
-						}
-					}
-				}
-			} else {
-				Adapter->PackInfo[uiSearchRuleIndex].bActive = false;
-				Adapter->PackInfo[uiSearchRuleIndex].bValid = false;
-				Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value = 0;
-				kfree(pstAddIndication);
-			}
-		} else {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "DSA ACK did not get valid SFID");
-			kfree(pstAddIndication);
-			return false;
-		}
-		break;
-	case DSC_REQ:
-		pLeader->PLength = sizeof(struct bcm_change_indication);
-		pstChangeIndication = (struct bcm_change_indication *)pstAddIndication;
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "SENDING DSC RESPONSE TO MAC %d", pLeader->PLength);
-
-		*((struct bcm_change_indication *)&(Adapter->caDsxReqResp[LEADER_SIZE])) = *pstChangeIndication;
-		((struct bcm_change_indication *)&(Adapter->caDsxReqResp[LEADER_SIZE]))->u8Type = DSC_RSP;
-
-		CopyBufferToControlPacket(Adapter, (PVOID)Adapter->caDsxReqResp);
-		kfree(pstAddIndication);
-		break;
-	case DSC_RSP:
-		pLeader->PLength = sizeof(struct bcm_change_indication);
-		pstChangeIndication = (struct bcm_change_indication *)pstAddIndication;
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "SENDING DSC ACK TO MAC %d", pLeader->PLength);
-		*((struct bcm_change_indication *)&(Adapter->caDsxReqResp[LEADER_SIZE])) = *pstChangeIndication;
-		((struct bcm_change_indication *)&(Adapter->caDsxReqResp[LEADER_SIZE]))->u8Type = DSC_ACK;
-		/* FALLTHROUGH */
-	case DSC_ACK:
-		pstChangeIndication = (struct bcm_change_indication *)pstAddIndication;
-		uiSearchRuleIndex = SearchSfid(Adapter, ntohl(pstChangeIndication->sfActiveSet.u32SFID));
-		if (uiSearchRuleIndex > NO_OF_QUEUES-1)
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "SF doesn't exist for which DSC_ACK is received");
-
-		if (uiSearchRuleIndex < NO_OF_QUEUES) {
-			Adapter->PackInfo[uiSearchRuleIndex].ucDirection = pstChangeIndication->u8Direction;
-			if (pstChangeIndication->sfActiveSet.bValid == TRUE)
-				Adapter->PackInfo[uiSearchRuleIndex].bActiveSet = TRUE;
-
-			if (pstChangeIndication->sfAuthorizedSet.bValid == TRUE)
-				Adapter->PackInfo[uiSearchRuleIndex].bAuthorizedSet = TRUE;
-
-			if (pstChangeIndication->sfAdmittedSet.bValid == TRUE)
-				Adapter->PackInfo[uiSearchRuleIndex].bAdmittedSet = TRUE;
-
-			if (pstChangeIndication->sfActiveSet.bValid == false) {
-				Adapter->PackInfo[uiSearchRuleIndex].bActive = false;
-				Adapter->PackInfo[uiSearchRuleIndex].bActivateRequestSent = false;
-
-				if (pstChangeIndication->sfAdmittedSet.bValid)
-					psfLocalSet = &pstChangeIndication->sfAdmittedSet;
-				else if (pstChangeIndication->sfAuthorizedSet.bValid)
-					psfLocalSet = &pstChangeIndication->sfAuthorizedSet;
-			} else {
-				psfLocalSet = &pstChangeIndication->sfActiveSet;
-				Adapter->PackInfo[uiSearchRuleIndex].bActive = TRUE;
-			}
-
-			if (!psfLocalSet) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "No set is valid\n");
-				Adapter->PackInfo[uiSearchRuleIndex].bActive = false;
-				Adapter->PackInfo[uiSearchRuleIndex].bValid = false;
-				Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value = 0;
-				kfree(pstAddIndication);
-			} else if (psfLocalSet->bValid && (pstChangeIndication->u8CC == 0)) {
-				Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value = ntohs(pstChangeIndication->u16VCID);
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "CC field is %d bvalid = %d\n",
-						pstChangeIndication->u8CC, psfLocalSet->bValid);
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "VCID= %d\n", ntohs(pstChangeIndication->u16VCID));
-				Adapter->PackInfo[uiSearchRuleIndex].usCID = ntohs(pstChangeIndication->u16CID);
-				CopyToAdapter(Adapter, psfLocalSet, uiSearchRuleIndex, DSC_ACK, pstAddIndication);
-
-				*(PULONG)(((PUCHAR)pvBuffer)+1) = psfLocalSet->u32SFID;
-			} else if (pstChangeIndication->u8CC == 6) {
-				deleteSFBySfid(Adapter, uiSearchRuleIndex);
-				kfree(pstAddIndication);
-			}
-		} else {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "DSC ACK did not get valid SFID");
-			kfree(pstAddIndication);
-			return false;
-		}
-		break;
-	case DSD_REQ:
-		pLeader->PLength = sizeof(struct bcm_del_indication);
-		*((struct bcm_del_indication *)&(Adapter->caDsxReqResp[LEADER_SIZE])) = *((struct bcm_del_indication *)pstAddIndication);
-
-		ulSFID = ntohl(((struct bcm_del_indication *)pstAddIndication)->u32SFID);
-		uiSearchRuleIndex = SearchSfid(Adapter, ulSFID);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "DSD - Removing connection %x", uiSearchRuleIndex);
-
-		if (uiSearchRuleIndex < NO_OF_QUEUES) {
-			/* Delete All Classifiers Associated with this SFID */
-			deleteSFBySfid(Adapter, uiSearchRuleIndex);
-			Adapter->u32TotalDSD++;
-		}
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "SENDING DSD RESPONSE TO MAC");
-		((struct bcm_del_indication *)&(Adapter->caDsxReqResp[LEADER_SIZE]))->u8Type = DSD_RSP;
-		CopyBufferToControlPacket(Adapter, (PVOID)Adapter->caDsxReqResp);
-		/* FALLTHROUGH */
-	case DSD_RSP:
-		/* Do nothing as SF has already got Deleted */
-		break;
-	case DSD_ACK:
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "DSD ACK Rcd, let App handle it\n");
-		break;
-	default:
-		kfree(pstAddIndication);
-		return false;
-	}
-	return TRUE;
-}
-
-int get_dsx_sf_data_to_application(struct bcm_mini_adapter *Adapter,
-		UINT uiSFId, void __user *user_buffer)
-{
-	int status = 0;
-	struct bcm_packet_info *psSfInfo = NULL;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"status =%d", status);
-	status = SearchSfid(Adapter, uiSFId);
-	if (status >= NO_OF_QUEUES) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-				"SFID %d not present in queue !!!", uiSFId);
-		return -EINVAL;
-	}
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"status =%d", status);
-	psSfInfo = &Adapter->PackInfo[status];
-	if (psSfInfo->pstSFIndication
-			&& copy_to_user(user_buffer, psSfInfo->pstSFIndication,
-				sizeof(struct bcm_add_indication_alt))) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-				"copy to user failed SFID %d, present in queue !!!",
-				uiSFId);
-		status = -EFAULT;
-		return status;
-	}
-	return STATUS_SUCCESS;
-}
-
-VOID OverrideServiceFlowParams(struct bcm_mini_adapter *Adapter,
-		PUINT puiBuffer)
-{
-	B_UINT32 u32NumofSFsinMsg = ntohl(*(puiBuffer + 1));
-	struct bcm_stim_sfhostnotify *pHostInfo = NULL;
-	UINT uiSearchRuleIndex = 0;
-	ULONG ulSFID = 0;
-
-	puiBuffer += 2;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-			"u32NumofSFsinMsg: 0x%x\n", u32NumofSFsinMsg);
-
-	while (u32NumofSFsinMsg != 0 && u32NumofSFsinMsg < NO_OF_QUEUES) {
-		u32NumofSFsinMsg--;
-		pHostInfo = (struct bcm_stim_sfhostnotify *)puiBuffer;
-		puiBuffer = (PUINT)(pHostInfo + 1);
-
-		ulSFID = ntohl(pHostInfo->SFID);
-		uiSearchRuleIndex = SearchSfid(Adapter, ulSFID);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-				"SFID: 0x%lx\n", ulSFID);
-
-		if (uiSearchRuleIndex >= NO_OF_QUEUES
-				|| uiSearchRuleIndex == HiPriority) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
-					DBG_LVL_ALL,
-					"The SFID <%lx> doesn't exist in host entry or is Invalid\n",
-					ulSFID);
-			continue;
-		}
-
-		if (pHostInfo->RetainSF == false) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
-					DBG_LVL_ALL, "Going to Delete SF");
-			deleteSFBySfid(Adapter, uiSearchRuleIndex);
-		} else {
-			struct bcm_packet_info *packinfo =
-				&Adapter->PackInfo[uiSearchRuleIndex];
-
-			packinfo->usVCID_Value = ntohs(pHostInfo->VCID);
-			packinfo->usCID = ntohs(pHostInfo->newCID);
-			packinfo->bActive = false;
-
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
-					DBG_LVL_ALL,
-					"pHostInfo->QoSParamSet: 0x%x\n",
-					pHostInfo->QoSParamSet);
-
-			if (pHostInfo->QoSParamSet & 0x1)
-				packinfo->bAuthorizedSet = TRUE;
-			if (pHostInfo->QoSParamSet & 0x2)
-				packinfo->bAdmittedSet = TRUE;
-			if (pHostInfo->QoSParamSet & 0x4) {
-				packinfo->bActiveSet = TRUE;
-				packinfo->bActive = TRUE;
-			}
-		}
-	}
-}
-
-static void restore_endianess_of_pstClassifierEntry(
-		struct bcm_classifier_rule *pstClassifierEntry,
-		enum bcm_ipaddr_context eIpAddrContext)
-{
-	int i;
-	union u_ip_address *stSrc  = &pstClassifierEntry->stSrcIpAddress;
-	union u_ip_address *stDest = &pstClassifierEntry->stDestIpAddress;
-
-	for (i = 0; i < MAX_IP_RANGE_LENGTH * 4; i++) {
-		if (eIpAddrContext == eSrcIpAddress) {
-			stSrc->ulIpv6Addr[i] = ntohl(stSrc->ulIpv6Addr[i]);
-			stSrc->ulIpv6Mask[i] = ntohl(stSrc->ulIpv6Mask[i]);
-		} else if (eIpAddrContext == eDestIpAddress) {
-			stDest->ulIpv6Addr[i] = ntohl(stDest->ulIpv6Addr[i]);
-			stDest->ulIpv6Mask[i] = ntohl(stDest->ulIpv6Mask[i]);
-		}
-	}
-}
-
-static void apply_phs_rule_to_all_classifiers(
-		register struct bcm_mini_adapter *Adapter,		/* <Pointer to the Adapter structure */
-		register UINT uiSearchRuleIndex,			/* <Index of Queue, to which this data belongs */
-		USHORT uVCID,
-		struct bcm_phs_rule *sPhsRule,
-		struct bcm_phs_rules *cPhsRule,
-		struct bcm_add_indication_alt *pstAddIndication)
-{
-	unsigned int uiClassifierIndex = 0;
-	struct bcm_classifier_rule *curr_classifier = NULL;
-
-	if (pstAddIndication->u8Direction == UPLINK_DIR) {
-		for (uiClassifierIndex = 0; uiClassifierIndex < MAX_CLASSIFIERS; uiClassifierIndex++) {
-			curr_classifier =
-				&Adapter->astClassifierTable[uiClassifierIndex];
-			if ((curr_classifier->bUsed) &&
-				(curr_classifier->ulSFID == Adapter->PackInfo[uiSearchRuleIndex].ulSFID) &&
-				(curr_classifier->u8AssociatedPHSI == cPhsRule->u8PHSI)) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-						"Adding PHS Rule For Classifier: 0x%x cPhsRule.u8PHSI: 0x%x\n",
-						curr_classifier->uiClassifierRuleIndex,
-						cPhsRule->u8PHSI);
-				/* Update The PHS Rule for this classifier as Associated PHSI id defined */
-
-				/* Copy the PHS Rule */
-				sPhsRule->u8PHSI = cPhsRule->u8PHSI;
-				sPhsRule->u8PHSFLength = cPhsRule->u8PHSFLength;
-				sPhsRule->u8PHSMLength = cPhsRule->u8PHSMLength;
-				sPhsRule->u8PHSS = cPhsRule->u8PHSS;
-				sPhsRule->u8PHSV = cPhsRule->u8PHSV;
-				memcpy(sPhsRule->u8PHSF, cPhsRule->u8PHSF, MAX_PHS_LENGTHS);
-				memcpy(sPhsRule->u8PHSM, cPhsRule->u8PHSM, MAX_PHS_LENGTHS);
-				sPhsRule->u8RefCnt = 0;
-				sPhsRule->bUnclassifiedPHSRule = false;
-				sPhsRule->PHSModifiedBytes = 0;
-				sPhsRule->PHSModifiedNumPackets = 0;
-				sPhsRule->PHSErrorNumPackets = 0;
-
-				/* bPHSRuleAssociated = TRUE; */
-				/* Store The PHS Rule for this classifier */
-
-				PhsUpdateClassifierRule(
-					&Adapter->stBCMPhsContext,
-					uVCID,
-					curr_classifier->uiClassifierRuleIndex,
-					sPhsRule,
-					curr_classifier->u8AssociatedPHSI);
-
-				/* Update PHS Rule For the Classifier */
-				if (sPhsRule->u8PHSI) {
-					curr_classifier->u32PHSRuleID = sPhsRule->u8PHSI;
-					memcpy(&curr_classifier->sPhsRule, sPhsRule, sizeof(struct bcm_phs_rule));
-				}
-			}
-		}
-	} else {
-		/* Error PHS Rule specified in signaling could not be applied to any classifier */
-
-		/* Copy the PHS Rule */
-		sPhsRule->u8PHSI = cPhsRule->u8PHSI;
-		sPhsRule->u8PHSFLength = cPhsRule->u8PHSFLength;
-		sPhsRule->u8PHSMLength = cPhsRule->u8PHSMLength;
-		sPhsRule->u8PHSS = cPhsRule->u8PHSS;
-		sPhsRule->u8PHSV = cPhsRule->u8PHSV;
-		memcpy(sPhsRule->u8PHSF, cPhsRule->u8PHSF, MAX_PHS_LENGTHS);
-		memcpy(sPhsRule->u8PHSM, cPhsRule->u8PHSM, MAX_PHS_LENGTHS);
-		sPhsRule->u8RefCnt = 0;
-		sPhsRule->bUnclassifiedPHSRule = TRUE;
-		sPhsRule->PHSModifiedBytes = 0;
-		sPhsRule->PHSModifiedNumPackets = 0;
-		sPhsRule->PHSErrorNumPackets = 0;
-		/* Store The PHS Rule for this classifier */
-
-		/*
-		 * Passing the argument u8PHSI instead of clsid. Because for DL with no classifier rule,
-		 * clsid will be zero hence we can't have multiple PHS rules for the same SF.
-		 * To support multiple PHS rule, passing u8PHSI.
-		 */
-		PhsUpdateClassifierRule(
-			&Adapter->stBCMPhsContext,
-			uVCID,
-			sPhsRule->u8PHSI,
-			sPhsRule,
-			sPhsRule->u8PHSI);
-	}
-}
diff --git a/drivers/staging/bcm/CmHost.h b/drivers/staging/bcm/CmHost.h
deleted file mode 100644
index 0887d3f49e2f..000000000000
--- a/drivers/staging/bcm/CmHost.h
+++ /dev/null
@@ -1,62 +0,0 @@
-/***************************************************************************
- * (c) Beceem Communications Inc.
- * All Rights Reserved
- *
- * file  : CmHost.h
- * author: Rajeev Tirumala
- * date  : September 8 , 2006
- * brief : Definitions for Connection Management Requests structure
- *          which we will use to setup our connection structures.Its high
- *          time we had a header file for CmHost.cpp to isolate the way
- *          f/w sends DSx messages and the way we interpret them in code.
- *          Revision History
- *
- *   Date       Author   Version   Description
- *   08-Sep-06    Rajeev       0.1      Created
- ***************************************************************************/
-#ifndef _CM_HOST_H
-#define _CM_HOST_H
-
-#pragma once
-#pragma pack(push, 4)
-
-#define DSX_MESSAGE_EXCHANGE_BUFFER        0xBF60AC84 /* This contains the pointer */
-#define DSX_MESSAGE_EXCHANGE_BUFFER_SIZE   72000      /* 24 K Bytes */
-
-struct bcm_add_indication_alt {
-	u8	u8Type;
-	u8	u8Direction;
-	u16	u16TID;
-	u16	u16CID;
-	u16	u16VCID;
-	struct bcm_connect_mgr_params sfAuthorizedSet;
-	struct bcm_connect_mgr_params sfAdmittedSet;
-	struct bcm_connect_mgr_params sfActiveSet;
-	u8	u8CC;    /* < Confirmation Code */
-	u8	u8Padd;
-	u16	u16Padd;
-};
-
-struct bcm_change_indication {
-	u8	u8Type;
-	u8	u8Direction;
-	u16	u16TID;
-	u16	u16CID;
-	u16	u16VCID;
-	struct bcm_connect_mgr_params sfAuthorizedSet;
-	struct bcm_connect_mgr_params sfAdmittedSet;
-	struct bcm_connect_mgr_params sfActiveSet;
-	u8	u8CC;    /* < Confirmation Code */
-	u8	u8Padd;
-	u16	u16Padd;
-};
-
-unsigned long StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, void *pvBuffer, unsigned int *puBufferLength);
-int AllocAdapterDsxBuffer(struct bcm_mini_adapter *Adapter);
-int FreeAdapterDsxBuffer(struct bcm_mini_adapter *Adapter);
-unsigned long SetUpTargetDsxBuffers(struct bcm_mini_adapter *Adapter);
-bool CmControlResponseMessage(struct bcm_mini_adapter *Adapter, void *pvBuffer);
-
-#pragma pack(pop)
-
-#endif
diff --git a/drivers/staging/bcm/DDRInit.c b/drivers/staging/bcm/DDRInit.c
deleted file mode 100644
index 4226c931cd45..000000000000
--- a/drivers/staging/bcm/DDRInit.c
+++ /dev/null
@@ -1,1355 +0,0 @@
-#include "headers.h"
-
-
-
-#define DDR_DUMP_INTERNAL_DEVICE_MEMORY 0xBFC02B00
-#define MIPS_CLOCK_REG 0x0f000820
-
-/* DDR INIT-133Mhz */
-#define T3_SKIP_CLOCK_PROGRAM_DUMP_133MHZ 12  /* index for 0x0F007000 */
-static struct bcm_ddr_setting asT3_DDRSetting133MHz[] = {
-	/* DPLL Clock Setting */
-	{0x0F000800, 0x00007212},
-	{0x0f000820, 0x07F13FFF},
-	{0x0f000810, 0x00000F95},
-	{0x0f000860, 0x00000000},
-	{0x0f000880, 0x000003DD},
-	/* Changed source for X-bar and MIPS clock to APLL */
-	{0x0f000840, 0x0FFF1B00},
-	{0x0f000870, 0x00000002},
-	{0x0F00a044, 0x1fffffff},
-	{0x0F00a040, 0x1f000000},
-	{0x0F00a084, 0x1Cffffff},
-	{0x0F00a080, 0x1C000000},
-	{0x0F00a04C, 0x0000000C},
-	/* Memcontroller Default values */
-	{0x0F007000, 0x00010001},
-	{0x0F007004, 0x01010100},
-	{0x0F007008, 0x01000001},
-	{0x0F00700c, 0x00000000},
-	{0x0F007010, 0x01000000},
-	{0x0F007014, 0x01000100},
-	{0x0F007018, 0x01000000},
-	{0x0F00701c, 0x01020001},
-	{0x0F007020, 0x04030107},
-	{0x0F007024, 0x02000007},
-	{0x0F007028, 0x02020202},
-	{0x0F00702c, 0x0206060a},
-	{0x0F007030, 0x05000000},
-	{0x0F007034, 0x00000003},
-	{0x0F007038, 0x110a0200},
-	{0x0F00703C, 0x02101010},
-	{0x0F007040, 0x45751200},
-	{0x0F007044, 0x110a0d00},
-	{0x0F007048, 0x081b0306},
-	{0x0F00704c, 0x00000000},
-	{0x0F007050, 0x0000001c},
-	{0x0F007054, 0x00000000},
-	{0x0F007058, 0x00000000},
-	{0x0F00705c, 0x00000000},
-	{0x0F007060, 0x0010246c},
-	{0x0F007064, 0x00000010},
-	{0x0F007068, 0x00000000},
-	{0x0F00706c, 0x00000001},
-	{0x0F007070, 0x00007000},
-	{0x0F007074, 0x00000000},
-	{0x0F007078, 0x00000000},
-	{0x0F00707C, 0x00000000},
-	{0x0F007080, 0x00000000},
-	{0x0F007084, 0x00000000},
-	/* Enable BW improvement within memory controller */
-	{0x0F007094, 0x00000104},
-	/* Enable 2 ports within X-bar */
-	{0x0F00A000, 0x00000016},
-	/* Enable start bit within memory controller */
-	{0x0F007018, 0x01010000}
-};
-/* 80Mhz */
-#define T3_SKIP_CLOCK_PROGRAM_DUMP_80MHZ 10  /* index for 0x0F007000 */
-static struct bcm_ddr_setting asT3_DDRSetting80MHz[] = {
-      /* DPLL Clock Setting */
-	{0x0f000810, 0x00000F95},
-	{0x0f000820, 0x07f1ffff},
-	{0x0f000860, 0x00000000},
-	{0x0f000880, 0x000003DD},
-	{0x0F00a044, 0x1fffffff},
-	{0x0F00a040, 0x1f000000},
-	{0x0F00a084, 0x1Cffffff},
-	{0x0F00a080, 0x1C000000},
-	{0x0F00a000, 0x00000016},
-	{0x0F00a04C, 0x0000000C},
-	/* Memcontroller Default values */
-	{0x0F007000, 0x00010001},
-	{0x0F007004, 0x01000000},
-	{0x0F007008, 0x01000001},
-	{0x0F00700c, 0x00000000},
-	{0x0F007010, 0x01000000},
-	{0x0F007014, 0x01000100},
-	{0x0F007018, 0x01000000},
-	{0x0F00701c, 0x01020000},
-	{0x0F007020, 0x04020107},
-	{0x0F007024, 0x00000007},
-	{0x0F007028, 0x02020201},
-	{0x0F00702c, 0x0204040a},
-	{0x0F007030, 0x04000000},
-	{0x0F007034, 0x00000002},
-	{0x0F007038, 0x1F060200},
-	{0x0F00703C, 0x1C22221F},
-	{0x0F007040, 0x8A006600},
-	{0x0F007044, 0x221a0800},
-	{0x0F007048, 0x02690204},
-	{0x0F00704c, 0x00000000},
-	{0x0F007050, 0x0000001c},
-	{0x0F007054, 0x00000000},
-	{0x0F007058, 0x00000000},
-	{0x0F00705c, 0x00000000},
-	{0x0F007060, 0x000A15D6},
-	{0x0F007064, 0x0000000A},
-	{0x0F007068, 0x00000000},
-	{0x0F00706c, 0x00000001},
-	{0x0F007070, 0x00004000},
-	{0x0F007074, 0x00000000},
-	{0x0F007078, 0x00000000},
-	{0x0F00707C, 0x00000000},
-	{0x0F007080, 0x00000000},
-	{0x0F007084, 0x00000000},
-	{0x0F007094, 0x00000104},
-	/* Enable start bit within memory controller */
-	{0x0F007018, 0x01010000}
-};
-/* 100Mhz */
-#define T3_SKIP_CLOCK_PROGRAM_DUMP_100MHZ 13  /* index for 0x0F007000 */
-static struct bcm_ddr_setting asT3_DDRSetting100MHz[] = {
-      /* DPLL Clock Setting */
-	{0x0F000800, 0x00007008},
-	{0x0f000810, 0x00000F95},
-	{0x0f000820, 0x07F13E3F},
-	{0x0f000860, 0x00000000},
-	{0x0f000880, 0x000003DD},
-	/* Changed source for X-bar and MIPS clock to APLL */
-	{0x0f000840, 0x0FFF1B00},
-	{0x0f000870, 0x00000002},
-	{0x0F00a044, 0x1fffffff},
-	{0x0F00a040, 0x1f000000},
-	{0x0F00a084, 0x1Cffffff},
-	{0x0F00a080, 0x1C000000},
-	{0x0F00a04C, 0x0000000C},
-	/* Enable 2 ports within X-bar */
-	{0x0F00A000, 0x00000016},
-	/* Memcontroller Default values */
-	{0x0F007000, 0x00010001},
-	{0x0F007004, 0x01010100},
-	{0x0F007008, 0x01000001},
-	{0x0F00700c, 0x00000000},
-	{0x0F007010, 0x01000000},
-	{0x0F007014, 0x01000100},
-	{0x0F007018, 0x01000000},
-	{0x0F00701c, 0x01020001},
-	{0x0F007020, 0x04020107},
-	{0x0F007024, 0x00000007},
-	{0x0F007028, 0x01020201},
-	{0x0F00702c, 0x0204040A},
-	{0x0F007030, 0x06000000},
-	{0x0F007034, 0x00000004},
-	{0x0F007038, 0x20080200},
-	{0x0F00703C, 0x02030320},
-	{0x0F007040, 0x6E7F1200},
-	{0x0F007044, 0x01190A00},
-	{0x0F007048, 0x06120305},
-	{0x0F00704c, 0x00000000},
-	{0x0F007050, 0x0000001C},
-	{0x0F007054, 0x00000000},
-	{0x0F007058, 0x00000000},
-	{0x0F00705c, 0x00000000},
-	{0x0F007060, 0x00082ED6},
-	{0x0F007064, 0x0000000A},
-	{0x0F007068, 0x00000000},
-	{0x0F00706c, 0x00000001},
-	{0x0F007070, 0x00005000},
-	{0x0F007074, 0x00000000},
-	{0x0F007078, 0x00000000},
-	{0x0F00707C, 0x00000000},
-	{0x0F007080, 0x00000000},
-	{0x0F007084, 0x00000000},
-	/* Enable BW improvement within memory controller */
-	{0x0F007094, 0x00000104},
-	/* Enable start bit within memory controller */
-	{0x0F007018, 0x01010000}
-};
-
-/* Net T3B DDR Settings
- * DDR INIT-133Mhz
- */
-static struct bcm_ddr_setting asDPLL_266MHZ[] = {
-	{0x0F000800, 0x00007212},
-	{0x0f000820, 0x07F13FFF},
-	{0x0f000810, 0x00000F95},
-	{0x0f000860, 0x00000000},
-	{0x0f000880, 0x000003DD},
-	/* Changed source for X-bar and MIPS clock to APLL */
-	{0x0f000840, 0x0FFF1B00},
-	{0x0f000870, 0x00000002}
-};
-
-#define T3B_SKIP_CLOCK_PROGRAM_DUMP_133MHZ 11  /* index for 0x0F007000 */
-static struct bcm_ddr_setting asT3B_DDRSetting133MHz[] = {
-      /* DPLL Clock Setting */
-	{0x0f000810, 0x00000F95},
-	{0x0f000810, 0x00000F95},
-	{0x0f000810, 0x00000F95},
-	{0x0f000820, 0x07F13652},
-	{0x0f000840, 0x0FFF0800},
-	/* Changed source for X-bar and MIPS clock to APLL */
-	{0x0f000880, 0x000003DD},
-	{0x0f000860, 0x00000000},
-	/* Changed source for X-bar and MIPS clock to APLL */
-	{0x0F00a044, 0x1fffffff},
-	{0x0F00a040, 0x1f000000},
-	{0x0F00a084, 0x1Cffffff},
-	{0x0F00a080, 0x1C000000},
-	/* Enable 2 ports within X-bar */
-	{0x0F00A000, 0x00000016},
-	/* Memcontroller Default values */
-	{0x0F007000, 0x00010001},
-	{0x0F007004, 0x01010100},
-	{0x0F007008, 0x01000001},
-	{0x0F00700c, 0x00000000},
-	{0x0F007010, 0x01000000},
-	{0x0F007014, 0x01000100},
-	{0x0F007018, 0x01000000},
-	{0x0F00701c, 0x01020001},
-	{0x0F007020, 0x04030107},
-	{0x0F007024, 0x02000007},
-	{0x0F007028, 0x02020202},
-	{0x0F00702c, 0x0206060a},
-	{0x0F007030, 0x05000000},
-	{0x0F007034, 0x00000003},
-	{0x0F007038, 0x130a0200},
-	{0x0F00703C, 0x02101012},
-	{0x0F007040, 0x457D1200},
-	{0x0F007044, 0x11130d00},
-	{0x0F007048, 0x040D0306},
-	{0x0F00704c, 0x00000000},
-	{0x0F007050, 0x0000001c},
-	{0x0F007054, 0x00000000},
-	{0x0F007058, 0x00000000},
-	{0x0F00705c, 0x00000000},
-	{0x0F007060, 0x0010246c},
-	{0x0F007064, 0x00000012},
-	{0x0F007068, 0x00000000},
-	{0x0F00706c, 0x00000001},
-	{0x0F007070, 0x00007000},
-	{0x0F007074, 0x00000000},
-	{0x0F007078, 0x00000000},
-	{0x0F00707C, 0x00000000},
-	{0x0F007080, 0x00000000},
-	{0x0F007084, 0x00000000},
-	/* Enable BW improvement within memory controller */
-	{0x0F007094, 0x00000104},
-	/* Enable start bit within memory controller */
-	{0x0F007018, 0x01010000},
-	};
-
-#define T3B_SKIP_CLOCK_PROGRAM_DUMP_80MHZ 9  /* index for 0x0F007000 */
-static struct bcm_ddr_setting asT3B_DDRSetting80MHz[] = {
-      /* DPLL Clock Setting */
-	{0x0f000810, 0x00000F95},
-	{0x0f000820, 0x07F13FFF},
-	{0x0f000840, 0x0FFF1F00},
-	{0x0f000880, 0x000003DD},
-	{0x0f000860, 0x00000000},
-
-	{0x0F00a044, 0x1fffffff},
-	{0x0F00a040, 0x1f000000},
-	{0x0F00a084, 0x1Cffffff},
-	{0x0F00a080, 0x1C000000},
-	{0x0F00a000, 0x00000016},
-	/* Memcontroller Default values */
-	{0x0F007000, 0x00010001},
-	{0x0F007004, 0x01000000},
-	{0x0F007008, 0x01000001},
-	{0x0F00700c, 0x00000000},
-	{0x0F007010, 0x01000000},
-	{0x0F007014, 0x01000100},
-	{0x0F007018, 0x01000000},
-	{0x0F00701c, 0x01020000},
-	{0x0F007020, 0x04020107},
-	{0x0F007024, 0x00000007},
-	{0x0F007028, 0x02020201},
-	{0x0F00702c, 0x0204040a},
-	{0x0F007030, 0x04000000},
-	{0x0F007034, 0x02000002},
-	{0x0F007038, 0x1F060202},
-	{0x0F00703C, 0x1C22221F},
-	{0x0F007040, 0x8A006600},
-	{0x0F007044, 0x221a0800},
-	{0x0F007048, 0x02690204},
-	{0x0F00704c, 0x00000000},
-	{0x0F007050, 0x0100001c},
-	{0x0F007054, 0x00000000},
-	{0x0F007058, 0x00000000},
-	{0x0F00705c, 0x00000000},
-	{0x0F007060, 0x000A15D6},
-	{0x0F007064, 0x0000000A},
-	{0x0F007068, 0x00000000},
-	{0x0F00706c, 0x00000001},
-	{0x0F007070, 0x00004000},
-	{0x0F007074, 0x00000000},
-	{0x0F007078, 0x00000000},
-	{0x0F00707C, 0x00000000},
-	{0x0F007080, 0x00000000},
-	{0x0F007084, 0x00000000},
-	{0x0F007094, 0x00000104},
-	/* Enable start bit within memory controller */
-	{0x0F007018, 0x01010000}
-};
-
-/* 100Mhz */
-#define T3B_SKIP_CLOCK_PROGRAM_DUMP_100MHZ 9  /* index for 0x0F007000 */
-static struct bcm_ddr_setting asT3B_DDRSetting100MHz[] = {
-      /* DPLL Clock Setting */
-	{0x0f000810, 0x00000F95},
-	{0x0f000820, 0x07F1369B},
-	{0x0f000840, 0x0FFF0800},
-	{0x0f000880, 0x000003DD},
-	{0x0f000860, 0x00000000},
-	{0x0F00a044, 0x1fffffff},
-	{0x0F00a040, 0x1f000000},
-	{0x0F00a084, 0x1Cffffff},
-	{0x0F00a080, 0x1C000000},
-	/* Enable 2 ports within X-bar */
-	{0x0F00A000, 0x00000016},
-	/* Memcontroller Default values */
-	{0x0F007000, 0x00010001},
-	{0x0F007004, 0x01010100},
-	{0x0F007008, 0x01000001},
-	{0x0F00700c, 0x00000000},
-	{0x0F007010, 0x01000000},
-	{0x0F007014, 0x01000100},
-	{0x0F007018, 0x01000000},
-	{0x0F00701c, 0x01020000},
-	{0x0F007020, 0x04020107},
-	{0x0F007024, 0x00000007},
-	{0x0F007028, 0x01020201},
-	{0x0F00702c, 0x0204040A},
-	{0x0F007030, 0x06000000},
-	{0x0F007034, 0x02000004},
-	{0x0F007038, 0x20080200},
-	{0x0F00703C, 0x02030320},
-	{0x0F007040, 0x6E7F1200},
-	{0x0F007044, 0x01190A00},
-	{0x0F007048, 0x06120305},
-	{0x0F00704c, 0x00000000},
-	{0x0F007050, 0x0100001C},
-	{0x0F007054, 0x00000000},
-	{0x0F007058, 0x00000000},
-	{0x0F00705c, 0x00000000},
-	{0x0F007060, 0x00082ED6},
-	{0x0F007064, 0x0000000A},
-	{0x0F007068, 0x00000000},
-	{0x0F00706c, 0x00000001},
-	{0x0F007070, 0x00005000},
-	{0x0F007074, 0x00000000},
-	{0x0F007078, 0x00000000},
-	{0x0F00707C, 0x00000000},
-	{0x0F007080, 0x00000000},
-	{0x0F007084, 0x00000000},
-	/* Enable BW improvement within memory controller */
-	{0x0F007094, 0x00000104},
-	/* Enable start bit within memory controller */
-	{0x0F007018, 0x01010000}
-};
-
-
-#define T3LP_SKIP_CLOCK_PROGRAM_DUMP_133MHZ 9  /* index for 0x0F007000 */
-static struct bcm_ddr_setting asT3LP_DDRSetting133MHz[] = {
-      /* DPLL Clock Setting */
-	{0x0f000820, 0x03F1365B},
-	{0x0f000810, 0x00002F95},
-	{0x0f000880, 0x000003DD},
-	/* Changed source for X-bar and MIPS clock to APLL */
-	{0x0f000840, 0x0FFF0000},
-	{0x0f000860, 0x00000000},
-	{0x0F00a044, 0x1fffffff},
-	{0x0F00a040, 0x1f000000},
-	{0x0F00a084, 0x1Cffffff},
-	{0x0F00a080, 0x1C000000},
-	{0x0F00A000, 0x00000016},
-	/* Memcontroller Default values */
-	{0x0F007000, 0x00010001},
-	{0x0F007004, 0x01010100},
-	{0x0F007008, 0x01000001},
-	{0x0F00700c, 0x00000000},
-	{0x0F007010, 0x01000000},
-	{0x0F007014, 0x01000100},
-	{0x0F007018, 0x01000000},
-	{0x0F00701c, 0x01020001},
-	{0x0F007020, 0x04030107},
-	{0x0F007024, 0x02000007},
-	{0x0F007028, 0x02020200},
-	{0x0F00702c, 0x0206060a},
-	{0x0F007030, 0x05000000},
-	{0x0F007034, 0x00000003},
-	{0x0F007038, 0x200a0200},
-	{0x0F00703C, 0x02101020},
-	{0x0F007040, 0x45711200},
-	{0x0F007044, 0x110D0D00},
-	{0x0F007048, 0x04080306},
-	{0x0F00704c, 0x00000000},
-	{0x0F007050, 0x0100001c},
-	{0x0F007054, 0x00000000},
-	{0x0F007058, 0x00000000},
-	{0x0F00705c, 0x00000000},
-	{0x0F007060, 0x0010245F},
-	{0x0F007064, 0x00000010},
-	{0x0F007068, 0x00000000},
-	{0x0F00706c, 0x00000001},
-	{0x0F007070, 0x00007000},
-	{0x0F007074, 0x00000000},
-	{0x0F007078, 0x00000000},
-	{0x0F00707C, 0x00000000},
-	{0x0F007080, 0x00000000},
-	{0x0F007084, 0x00000000},
-	{0x0F007088, 0x01000001},
-	{0x0F00708c, 0x00000101},
-	{0x0F007090, 0x00000000},
-	/* Enable BW improvement within memory controller */
-	{0x0F007094, 0x00040000},
-	{0x0F007098, 0x00000000},
-	{0x0F0070c8, 0x00000104},
-	/* Enable 2 ports within X-bar */
-	/* Enable start bit within memory controller */
-	{0x0F007018, 0x01010000}
-};
-
-#define T3LP_SKIP_CLOCK_PROGRAM_DUMP_100MHZ 11  /* index for 0x0F007000 */
-static struct bcm_ddr_setting asT3LP_DDRSetting100MHz[] = {
-      /* DPLL Clock Setting */
-	{0x0f000810, 0x00002F95},
-	{0x0f000820, 0x03F1369B},
-	{0x0f000840, 0x0fff0000},
-	{0x0f000860, 0x00000000},
-	{0x0f000880, 0x000003DD},
-	/* Changed source for X-bar and MIPS clock to APLL */
-	{0x0f000840, 0x0FFF0000},
-	{0x0F00a044, 0x1fffffff},
-	{0x0F00a040, 0x1f000000},
-	{0x0F00a084, 0x1Cffffff},
-	{0x0F00a080, 0x1C000000},
-	/* Memcontroller Default values */
-	{0x0F007000, 0x00010001},
-	{0x0F007004, 0x01010100},
-	{0x0F007008, 0x01000001},
-	{0x0F00700c, 0x00000000},
-	{0x0F007010, 0x01000000},
-	{0x0F007014, 0x01000100},
-	{0x0F007018, 0x01000000},
-	{0x0F00701c, 0x01020000},
-	{0x0F007020, 0x04020107},
-	{0x0F007024, 0x00000007},
-	{0x0F007028, 0x01020200},
-	{0x0F00702c, 0x0204040a},
-	{0x0F007030, 0x06000000},
-	{0x0F007034, 0x00000004},
-	{0x0F007038, 0x1F080200},
-	{0x0F00703C, 0x0203031F},
-	{0x0F007040, 0x6e001200},
-	{0x0F007044, 0x011a0a00},
-	{0x0F007048, 0x03000305},
-	{0x0F00704c, 0x00000000},
-	{0x0F007050, 0x0100001c},
-	{0x0F007054, 0x00000000},
-	{0x0F007058, 0x00000000},
-	{0x0F00705c, 0x00000000},
-	{0x0F007060, 0x00082ED6},
-	{0x0F007064, 0x0000000A},
-	{0x0F007068, 0x00000000},
-	{0x0F00706c, 0x00000001},
-	{0x0F007070, 0x00005000},
-	{0x0F007074, 0x00000000},
-	{0x0F007078, 0x00000000},
-	{0x0F00707C, 0x00000000},
-	{0x0F007080, 0x00000000},
-	{0x0F007084, 0x00000000},
-	{0x0F007088, 0x01000001},
-	{0x0F00708c, 0x00000101},
-	{0x0F007090, 0x00000000},
-	{0x0F007094, 0x00010000},
-	{0x0F007098, 0x00000000},
-	{0x0F0070C8, 0x00000104},
-	/* Enable 2 ports within X-bar */
-	{0x0F00A000, 0x00000016},
-	/* Enable start bit within memory controller */
-	{0x0F007018, 0x01010000}
-};
-
-#define T3LP_SKIP_CLOCK_PROGRAM_DUMP_80MHZ 9  /* index for 0x0F007000 */
-static struct bcm_ddr_setting asT3LP_DDRSetting80MHz[] = {
-      /* DPLL Clock Setting */
-	{0x0f000820, 0x07F13FFF},
-	{0x0f000810, 0x00002F95},
-	{0x0f000860, 0x00000000},
-	{0x0f000880, 0x000003DD},
-	{0x0f000840, 0x0FFF1F00},
-	{0x0F00a044, 0x1fffffff},
-	{0x0F00a040, 0x1f000000},
-	{0x0F00a084, 0x1Cffffff},
-	{0x0F00a080, 0x1C000000},
-	{0x0F00A000, 0x00000016},
-	{0x0f007000, 0x00010001},
-	{0x0f007004, 0x01000000},
-	{0x0f007008, 0x01000001},
-	{0x0f00700c, 0x00000000},
-	{0x0f007010, 0x01000000},
-	{0x0f007014, 0x01000100},
-	{0x0f007018, 0x01000000},
-	{0x0f00701c, 0x01020000},
-	{0x0f007020, 0x04020107},
-	{0x0f007024, 0x00000007},
-	{0x0f007028, 0x02020200},
-	{0x0f00702c, 0x0204040a},
-	{0x0f007030, 0x04000000},
-	{0x0f007034, 0x00000002},
-	{0x0f007038, 0x1d060200},
-	{0x0f00703c, 0x1c22221d},
-	{0x0f007040, 0x8A116600},
-	{0x0f007044, 0x222d0800},
-	{0x0f007048, 0x02690204},
-	{0x0f00704c, 0x00000000},
-	{0x0f007050, 0x0100001c},
-	{0x0f007054, 0x00000000},
-	{0x0f007058, 0x00000000},
-	{0x0f00705c, 0x00000000},
-	{0x0f007060, 0x000A15D6},
-	{0x0f007064, 0x0000000A},
-	{0x0f007068, 0x00000000},
-	{0x0f00706c, 0x00000001},
-	{0x0f007070, 0x00004000},
-	{0x0f007074, 0x00000000},
-	{0x0f007078, 0x00000000},
-	{0x0f00707c, 0x00000000},
-	{0x0f007080, 0x00000000},
-	{0x0f007084, 0x00000000},
-	{0x0f007088, 0x01000001},
-	{0x0f00708c, 0x00000101},
-	{0x0f007090, 0x00000000},
-	{0x0f007094, 0x00010000},
-	{0x0f007098, 0x00000000},
-	{0x0F0070C8, 0x00000104},
-	{0x0F007018, 0x01010000}
-};
-
-
-
-
-/* T3 LP-B (UMA-B) */
-
-#define T3LPB_SKIP_CLOCK_PROGRAM_DUMP_160MHZ 7  /* index for 0x0F007000 */
-static struct bcm_ddr_setting asT3LPB_DDRSetting160MHz[] = {
-      /* DPLL Clock Setting */
-	{0x0f000820, 0x03F137DB},
-	{0x0f000810, 0x01842795},
-	{0x0f000860, 0x00000000},
-	{0x0f000880, 0x000003DD},
-	{0x0f000840, 0x0FFF0400},
-	{0x0F00a044, 0x1fffffff},
-	{0x0F00a040, 0x1f000000},
-	{0x0f003050, 0x00000021},  /* this is flash/eeprom clock divisor which
-				    * set the flash clock to 20 MHz */
-	{0x0F00a084, 0x1Cffffff},  /* Now dump from her in internal memory */
-	{0x0F00a080, 0x1C000000},
-	{0x0F00A000, 0x00000016},
-	{0x0f007000, 0x00010001},
-	{0x0f007004, 0x01000001},
-	{0x0f007008, 0x01000101},
-	{0x0f00700c, 0x00000000},
-	{0x0f007010, 0x01000100},
-	{0x0f007014, 0x01000100},
-	{0x0f007018, 0x01000000},
-	{0x0f00701c, 0x01020000},
-	{0x0f007020, 0x04030107},
-	{0x0f007024, 0x02000007},
-	{0x0f007028, 0x02020200},
-	{0x0f00702c, 0x0206060a},
-	{0x0f007030, 0x050d0d00},
-	{0x0f007034, 0x00000003},
-	{0x0f007038, 0x170a0200},
-	{0x0f00703c, 0x02101012},
-	{0x0f007040, 0x45161200},
-	{0x0f007044, 0x11250c00},
-	{0x0f007048, 0x04da0307},
-	{0x0f00704c, 0x00000000},
-	{0x0f007050, 0x0000001c},
-	{0x0f007054, 0x00000000},
-	{0x0f007058, 0x00000000},
-	{0x0f00705c, 0x00000000},
-	{0x0f007060, 0x00142bb6},
-	{0x0f007064, 0x20430014},
-	{0x0f007068, 0x00000000},
-	{0x0f00706c, 0x00000001},
-	{0x0f007070, 0x00009000},
-	{0x0f007074, 0x00000000},
-	{0x0f007078, 0x00000000},
-	{0x0f00707c, 0x00000000},
-	{0x0f007080, 0x00000000},
-	{0x0f007084, 0x00000000},
-	{0x0f007088, 0x01000001},
-	{0x0f00708c, 0x00000101},
-	{0x0f007090, 0x00000000},
-	{0x0f007094, 0x00040000},
-	{0x0f007098, 0x00000000},
-	{0x0F0070C8, 0x00000104},
-	{0x0F007018, 0x01010000}
-};
-
-
-#define T3LPB_SKIP_CLOCK_PROGRAM_DUMP_133MHZ 7  /* index for 0x0F007000 */
-static struct bcm_ddr_setting asT3LPB_DDRSetting133MHz[] = {
-      /* DPLL Clock Setting */
-	{0x0f000820, 0x03F1365B},
-	{0x0f000810, 0x00002F95},
-	{0x0f000880, 0x000003DD},
-	/* Changed source for X-bar and MIPS clock to APLL */
-	{0x0f000840, 0x0FFF0000},
-	{0x0f000860, 0x00000000},
-	{0x0F00a044, 0x1fffffff},
-	{0x0F00a040, 0x1f000000},
-	{0x0f003050, 0x00000021},  /* flash/eeprom clock divisor which
-				    * set the flash clock to 20 MHz */
-	{0x0F00a084, 0x1Cffffff},  /* dump from here in internal memory */
-	{0x0F00a080, 0x1C000000},
-	{0x0F00A000, 0x00000016},
-	/* Memcontroller Default values */
-	{0x0F007000, 0x00010001},
-	{0x0F007004, 0x01010100},
-	{0x0F007008, 0x01000001},
-	{0x0F00700c, 0x00000000},
-	{0x0F007010, 0x01000000},
-	{0x0F007014, 0x01000100},
-	{0x0F007018, 0x01000000},
-	{0x0F00701c, 0x01020001},
-	{0x0F007020, 0x04030107},
-	{0x0F007024, 0x02000007},
-	{0x0F007028, 0x02020200},
-	{0x0F00702c, 0x0206060a},
-	{0x0F007030, 0x05000000},
-	{0x0F007034, 0x00000003},
-	{0x0F007038, 0x190a0200},
-	{0x0F00703C, 0x02101017},
-	{0x0F007040, 0x45171200},
-	{0x0F007044, 0x11290D00},
-	{0x0F007048, 0x04080306},
-	{0x0F00704c, 0x00000000},
-	{0x0F007050, 0x0100001c},
-	{0x0F007054, 0x00000000},
-	{0x0F007058, 0x00000000},
-	{0x0F00705c, 0x00000000},
-	{0x0F007060, 0x0010245F},
-	{0x0F007064, 0x00000010},
-	{0x0F007068, 0x00000000},
-	{0x0F00706c, 0x00000001},
-	{0x0F007070, 0x00007000},
-	{0x0F007074, 0x00000000},
-	{0x0F007078, 0x00000000},
-	{0x0F00707C, 0x00000000},
-	{0x0F007080, 0x00000000},
-	{0x0F007084, 0x00000000},
-	{0x0F007088, 0x01000001},
-	{0x0F00708c, 0x00000101},
-	{0x0F007090, 0x00000000},
-	/* Enable BW improvement within memory controller */
-	{0x0F007094, 0x00040000},
-	{0x0F007098, 0x00000000},
-	{0x0F0070c8, 0x00000104},
-	/* Enable 2 ports within X-bar */
-	/* Enable start bit within memory controller */
-	{0x0F007018, 0x01010000}
-};
-
-#define T3LPB_SKIP_CLOCK_PROGRAM_DUMP_100MHZ 8  /* index for 0x0F007000 */
-static struct bcm_ddr_setting asT3LPB_DDRSetting100MHz[] = {
-      /* DPLL Clock Setting */
-	{0x0f000810, 0x00002F95},
-	{0x0f000820, 0x03F1369B},
-	{0x0f000840, 0x0fff0000},
-	{0x0f000860, 0x00000000},
-	{0x0f000880, 0x000003DD},
-	/* Changed source for X-bar and MIPS clock to APLL */
-	{0x0f000840, 0x0FFF0000},
-	{0x0F00a044, 0x1fffffff},
-	{0x0F00a040, 0x1f000000},
-	{0x0f003050, 0x00000021},  /* flash/eeprom clock divisor which
-				    * set the flash clock to 20 MHz */
-	{0x0F00a084, 0x1Cffffff},  /* dump from here in internal memory */
-	{0x0F00a080, 0x1C000000},
-	/* Memcontroller Default values */
-	{0x0F007000, 0x00010001},
-	{0x0F007004, 0x01010100},
-	{0x0F007008, 0x01000001},
-	{0x0F00700c, 0x00000000},
-	{0x0F007010, 0x01000000},
-	{0x0F007014, 0x01000100},
-	{0x0F007018, 0x01000000},
-	{0x0F00701c, 0x01020000},
-	{0x0F007020, 0x04020107},
-	{0x0F007024, 0x00000007},
-	{0x0F007028, 0x01020200},
-	{0x0F00702c, 0x0204040a},
-	{0x0F007030, 0x06000000},
-	{0x0F007034, 0x00000004},
-	{0x0F007038, 0x1F080200},
-	{0x0F00703C, 0x0203031F},
-	{0x0F007040, 0x6e001200},
-	{0x0F007044, 0x011a0a00},
-	{0x0F007048, 0x03000305},
-	{0x0F00704c, 0x00000000},
-	{0x0F007050, 0x0100001c},
-	{0x0F007054, 0x00000000},
-	{0x0F007058, 0x00000000},
-	{0x0F00705c, 0x00000000},
-	{0x0F007060, 0x00082ED6},
-	{0x0F007064, 0x0000000A},
-	{0x0F007068, 0x00000000},
-	{0x0F00706c, 0x00000001},
-	{0x0F007070, 0x00005000},
-	{0x0F007074, 0x00000000},
-	{0x0F007078, 0x00000000},
-	{0x0F00707C, 0x00000000},
-	{0x0F007080, 0x00000000},
-	{0x0F007084, 0x00000000},
-	{0x0F007088, 0x01000001},
-	{0x0F00708c, 0x00000101},
-	{0x0F007090, 0x00000000},
-	{0x0F007094, 0x00010000},
-	{0x0F007098, 0x00000000},
-	{0x0F0070C8, 0x00000104},
-	/* Enable 2 ports within X-bar */
-	{0x0F00A000, 0x00000016},
-	/* Enable start bit within memory controller */
-	{0x0F007018, 0x01010000}
-};
-
-#define T3LPB_SKIP_CLOCK_PROGRAM_DUMP_80MHZ 7  /* index for 0x0F007000 */
-static struct bcm_ddr_setting asT3LPB_DDRSetting80MHz[] = {
-      /* DPLL Clock Setting */
-	{0x0f000820, 0x07F13FFF},
-	{0x0f000810, 0x00002F95},
-	{0x0f000860, 0x00000000},
-	{0x0f000880, 0x000003DD},
-	{0x0f000840, 0x0FFF1F00},
-	{0x0F00a044, 0x1fffffff},
-	{0x0F00a040, 0x1f000000},
-	{0x0f003050, 0x00000021},  /* flash/eeprom clock divisor
-				    * which set the flash clock to 20 MHz */
-	{0x0F00a084, 0x1Cffffff},  /* dump from here in internal memory */
-	{0x0F00a080, 0x1C000000},
-	{0x0F00A000, 0x00000016},
-	{0x0f007000, 0x00010001},
-	{0x0f007004, 0x01000000},
-	{0x0f007008, 0x01000001},
-	{0x0f00700c, 0x00000000},
-	{0x0f007010, 0x01000000},
-	{0x0f007014, 0x01000100},
-	{0x0f007018, 0x01000000},
-	{0x0f00701c, 0x01020000},
-	{0x0f007020, 0x04020107},
-	{0x0f007024, 0x00000007},
-	{0x0f007028, 0x02020200},
-	{0x0f00702c, 0x0204040a},
-	{0x0f007030, 0x04000000},
-	{0x0f007034, 0x00000002},
-	{0x0f007038, 0x1d060200},
-	{0x0f00703c, 0x1c22221d},
-	{0x0f007040, 0x8A116600},
-	{0x0f007044, 0x222d0800},
-	{0x0f007048, 0x02690204},
-	{0x0f00704c, 0x00000000},
-	{0x0f007050, 0x0100001c},
-	{0x0f007054, 0x00000000},
-	{0x0f007058, 0x00000000},
-	{0x0f00705c, 0x00000000},
-	{0x0f007060, 0x000A15D6},
-	{0x0f007064, 0x0000000A},
-	{0x0f007068, 0x00000000},
-	{0x0f00706c, 0x00000001},
-	{0x0f007070, 0x00004000},
-	{0x0f007074, 0x00000000},
-	{0x0f007078, 0x00000000},
-	{0x0f00707c, 0x00000000},
-	{0x0f007080, 0x00000000},
-	{0x0f007084, 0x00000000},
-	{0x0f007088, 0x01000001},
-	{0x0f00708c, 0x00000101},
-	{0x0f007090, 0x00000000},
-	{0x0f007094, 0x00010000},
-	{0x0f007098, 0x00000000},
-	{0x0F0070C8, 0x00000104},
-	{0x0F007018, 0x01010000}
-};
-
-
-int ddr_init(struct bcm_mini_adapter *Adapter)
-{
-	struct bcm_ddr_setting *psDDRSetting = NULL;
-	ULONG RegCount = 0;
-	UINT value = 0;
-	UINT uiResetValue = 0;
-	UINT uiClockSetting = 0;
-	int retval = STATUS_SUCCESS;
-
-	switch (Adapter->chip_id) {
-	case 0xbece3200:
-		switch (Adapter->DDRSetting) {
-		case DDR_80_MHZ:
-			psDDRSetting = asT3LP_DDRSetting80MHz;
-			RegCount = (sizeof(asT3LP_DDRSetting80MHz) /
-				    sizeof(struct bcm_ddr_setting));
-			break;
-		case DDR_100_MHZ:
-			psDDRSetting = asT3LP_DDRSetting100MHz;
-			RegCount = (sizeof(asT3LP_DDRSetting100MHz) /
-				    sizeof(struct bcm_ddr_setting));
-			break;
-		case DDR_133_MHZ:
-			psDDRSetting = asT3LP_DDRSetting133MHz;
-			RegCount = (sizeof(asT3LP_DDRSetting133MHz) /
-				    sizeof(struct bcm_ddr_setting));
-			if (Adapter->bMipsConfig == MIPS_200_MHZ)
-				uiClockSetting = 0x03F13652;
-			else
-				uiClockSetting = 0x03F1365B;
-			break;
-		default:
-			return -EINVAL;
-		}
-
-		break;
-	case T3LPB:
-	case BCS220_2:
-	case BCS220_2BC:
-	case BCS250_BC:
-	case BCS220_3:
-		/* Set bit 2 and bit 6 to 1 for BBIC 2mA drive
-		 * (please check current value and additionally set these bits)
-		 */
-	if ((Adapter->chip_id !=  BCS220_2) &&
-		(Adapter->chip_id !=  BCS220_2BC) &&
-		(Adapter->chip_id != BCS220_3)) {
-		retval = rdmalt(Adapter, (UINT)0x0f000830, &uiResetValue,
-				sizeof(uiResetValue));
-		if (retval < 0) {
-			BCM_DEBUG_PRINT(Adapter, CMHOST, RDM, DBG_LVL_ALL,
-					"%s:%d RDM failed\n",
-					__func__, __LINE__);
-			return retval;
-		}
-		uiResetValue |= 0x44;
-		retval = wrmalt(Adapter, (UINT)0x0f000830, &uiResetValue,
-				sizeof(uiResetValue));
-		if (retval < 0) {
-			BCM_DEBUG_PRINT(Adapter, CMHOST, RDM, DBG_LVL_ALL,
-					"%s:%d RDM failed\n",
-					__func__, __LINE__);
-			return retval;
-		}
-	}
-		switch (Adapter->DDRSetting) {
-
-
-
-		case DDR_80_MHZ:
-			psDDRSetting = asT3LPB_DDRSetting80MHz;
-			RegCount = (sizeof(asT3B_DDRSetting80MHz) /
-				    sizeof(struct bcm_ddr_setting));
-			break;
-		case DDR_100_MHZ:
-			psDDRSetting = asT3LPB_DDRSetting100MHz;
-			RegCount = (sizeof(asT3B_DDRSetting100MHz) /
-				    sizeof(struct bcm_ddr_setting));
-			break;
-		case DDR_133_MHZ:
-			psDDRSetting = asT3LPB_DDRSetting133MHz;
-			RegCount = (sizeof(asT3B_DDRSetting133MHz) /
-				    sizeof(struct bcm_ddr_setting));
-
-			if (Adapter->bMipsConfig == MIPS_200_MHZ)
-				uiClockSetting = 0x03F13652;
-			else
-				uiClockSetting = 0x03F1365B;
-			break;
-
-		case DDR_160_MHZ:
-			psDDRSetting = asT3LPB_DDRSetting160MHz;
-			RegCount = sizeof(asT3LPB_DDRSetting160MHz) /
-				   sizeof(struct bcm_ddr_setting);
-
-			if (Adapter->bMipsConfig == MIPS_200_MHZ)
-				uiClockSetting = 0x03F137D2;
-			else
-				uiClockSetting = 0x03F137DB;
-		}
-			break;
-
-	case 0xbece0110:
-	case 0xbece0120:
-	case 0xbece0121:
-	case 0xbece0130:
-	case 0xbece0300:
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, DRV_ENTRY, DBG_LVL_ALL,
-			"DDR Setting: %x\n", Adapter->DDRSetting);
-		switch (Adapter->DDRSetting) {
-		case DDR_80_MHZ:
-			psDDRSetting = asT3_DDRSetting80MHz;
-			RegCount = (sizeof(asT3_DDRSetting80MHz) /
-				    sizeof(struct bcm_ddr_setting));
-			break;
-		case DDR_100_MHZ:
-			psDDRSetting = asT3_DDRSetting100MHz;
-			RegCount = (sizeof(asT3_DDRSetting100MHz) /
-				    sizeof(struct bcm_ddr_setting));
-			break;
-		case DDR_133_MHZ:
-			psDDRSetting = asT3_DDRSetting133MHz;
-			RegCount = (sizeof(asT3_DDRSetting133MHz) /
-				    sizeof(struct bcm_ddr_setting));
-			break;
-		default:
-			return -EINVAL;
-		}
-	case 0xbece0310:
-	{
-		switch (Adapter->DDRSetting) {
-		case DDR_80_MHZ:
-			psDDRSetting = asT3B_DDRSetting80MHz;
-			RegCount = (sizeof(asT3B_DDRSetting80MHz) /
-				    sizeof(struct bcm_ddr_setting));
-			break;
-		case DDR_100_MHZ:
-			psDDRSetting = asT3B_DDRSetting100MHz;
-			RegCount = (sizeof(asT3B_DDRSetting100MHz) /
-				    sizeof(struct bcm_ddr_setting));
-			break;
-		case DDR_133_MHZ:
-
-			/* 266Mhz PLL selected. */
-			if (Adapter->bDPLLConfig == PLL_266_MHZ) {
-				memcpy(asT3B_DDRSetting133MHz, asDPLL_266MHZ,
-				       sizeof(asDPLL_266MHZ));
-				psDDRSetting = asT3B_DDRSetting133MHz;
-				RegCount = (sizeof(asT3B_DDRSetting133MHz) /
-					    sizeof(struct bcm_ddr_setting));
-			} else {
-				psDDRSetting = asT3B_DDRSetting133MHz;
-				RegCount = (sizeof(asT3B_DDRSetting133MHz) /
-					    sizeof(struct bcm_ddr_setting));
-				if (Adapter->bMipsConfig == MIPS_200_MHZ)
-					uiClockSetting = 0x07F13652;
-				else
-					uiClockSetting = 0x07F1365B;
-			}
-			break;
-		default:
-			return -EINVAL;
-		}
-		break;
-
-	}
-	default:
-		return -EINVAL;
-	}
-
-	value = 0;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, DRV_ENTRY, DBG_LVL_ALL,
-			"Register Count is =%lu\n", RegCount);
-	while (RegCount && !retval) {
-		if (uiClockSetting
-				&& psDDRSetting->ulRegAddress == MIPS_CLOCK_REG)
-			value = uiClockSetting;
-		else
-			value = psDDRSetting->ulRegValue;
-		retval = wrmalt(Adapter, psDDRSetting->ulRegAddress, &value,
-				sizeof(value));
-		if (STATUS_SUCCESS != retval) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-					"%s:%d\n", __func__, __LINE__);
-			break;
-		}
-
-		RegCount--;
-		psDDRSetting++;
-	}
-
-	if (Adapter->chip_id >= 0xbece3300) {
-
-		mdelay(3);
-		if ((Adapter->chip_id != BCS220_2) &&
-			(Adapter->chip_id != BCS220_2BC) &&
-			(Adapter->chip_id != BCS220_3)) {
-			/* drive MDDR to half in case of UMA-B:	*/
-			uiResetValue = 0x01010001;
-			retval = wrmalt(Adapter, (UINT)0x0F007018,
-					&uiResetValue, sizeof(uiResetValue));
-			if (retval < 0) {
-				BCM_DEBUG_PRINT(Adapter, CMHOST, RDM,
-						DBG_LVL_ALL,
-						"%s:%d RDM failed\n",
-						__func__,
-						__LINE__);
-				return retval;
-			}
-			uiResetValue = 0x00040020;
-			retval = wrmalt(Adapter, (UINT)0x0F007094,
-					&uiResetValue, sizeof(uiResetValue));
-			if (retval < 0) {
-				BCM_DEBUG_PRINT(Adapter, CMHOST, RDM,
-						DBG_LVL_ALL,
-						"%s:%d RDM failed\n",
-						__func__,
-						__LINE__);
-				return retval;
-			}
-			uiResetValue = 0x01020101;
-			retval = wrmalt(Adapter, (UINT)0x0F00701c,
-					&uiResetValue, sizeof(uiResetValue));
-			if (retval < 0) {
-				BCM_DEBUG_PRINT(Adapter, CMHOST, RDM,
-						DBG_LVL_ALL,
-						"%s:%d RDM failed\n",
-						__func__,
-						__LINE__);
-				return retval;
-			}
-			uiResetValue = 0x01010000;
-			retval = wrmalt(Adapter, (UINT)0x0F007018,
-					&uiResetValue, sizeof(uiResetValue));
-			if (retval < 0) {
-				BCM_DEBUG_PRINT(Adapter, CMHOST, RDM,
-						DBG_LVL_ALL,
-						"%s:%d RDM failed\n",
-						__func__,
-						__LINE__);
-				return retval;
-			}
-		}
-		mdelay(3);
-
-		/* DC/DC standby change...
-		 * This is to be done only for Hybrid PMU mode.
-		 * with the current h/w there is no way to detect this.
-		 * and since we dont have internal PMU lets do it under
-		 * UMA-B chip id. we will change this when we will have
-		 * internal PMU.
-		 */
-		if (Adapter->PmuMode == HYBRID_MODE_7C) {
-			retval = rdmalt(Adapter, (UINT)0x0f000c00,
-					&uiResetValue, sizeof(uiResetValue));
-			if (retval < 0) {
-				BCM_DEBUG_PRINT(Adapter, CMHOST, RDM,
-						DBG_LVL_ALL,
-						"%s:%d RDM failed\n",
-						__func__,
-						__LINE__);
-				return retval;
-			}
-			retval = rdmalt(Adapter, (UINT)0x0f000c00,
-					&uiResetValue, sizeof(uiResetValue));
-			if (retval < 0) {
-				BCM_DEBUG_PRINT(Adapter, CMHOST, RDM,
-						DBG_LVL_ALL,
-						"%s:%d RDM failed\n",
-						__func__,
-						__LINE__);
-				return retval;
-			}
-			uiResetValue = 0x1322a8;
-			retval = wrmalt(Adapter, (UINT)0x0f000d1c,
-					&uiResetValue, sizeof(uiResetValue));
-			if (retval < 0) {
-				BCM_DEBUG_PRINT(Adapter, CMHOST, RDM,
-						DBG_LVL_ALL,
-						"%s:%d RDM failed\n",
-						__func__,
-						__LINE__);
-				return retval;
-			}
-			retval = rdmalt(Adapter, (UINT)0x0f000c00,
-					&uiResetValue, sizeof(uiResetValue));
-			if (retval < 0) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, RDM,
-						DBG_LVL_ALL,
-						"%s:%d RDM failed\n",
-						__func__,
-						__LINE__);
-				return retval;
-			}
-			retval = rdmalt(Adapter, (UINT)0x0f000c00,
-					&uiResetValue, sizeof(uiResetValue));
-			if (retval < 0) {
-				BCM_DEBUG_PRINT(Adapter, CMHOST, RDM,
-						DBG_LVL_ALL,
-						"%s:%d RDM failed\n",
-						__func__,
-						__LINE__);
-				return retval;
-			}
-			uiResetValue = 0x132296;
-			retval = wrmalt(Adapter, (UINT)0x0f000d14,
-					&uiResetValue, sizeof(uiResetValue));
-			if (retval < 0) {
-				BCM_DEBUG_PRINT(Adapter, CMHOST, RDM,
-						DBG_LVL_ALL,
-						"%s:%d RDM failed\n",
-						__func__,
-						__LINE__);
-				return retval;
-			}
-		} else if (Adapter->PmuMode == HYBRID_MODE_6) {
-
-			retval = rdmalt(Adapter, (UINT)0x0f000c00,
-					&uiResetValue, sizeof(uiResetValue));
-			if (retval < 0) {
-				BCM_DEBUG_PRINT(Adapter, CMHOST, RDM,
-						DBG_LVL_ALL,
-						"%s:%d RDM failed\n",
-						__func__,
-						__LINE__);
-				return retval;
-			}
-			retval = rdmalt(Adapter, (UINT)0x0f000c00,
-					&uiResetValue, sizeof(uiResetValue));
-			if (retval < 0) {
-				BCM_DEBUG_PRINT(Adapter, CMHOST, RDM,
-						DBG_LVL_ALL,
-						"%s:%d RDM failed\n",
-						__func__,
-						__LINE__);
-				return retval;
-			}
-			uiResetValue = 0x6003229a;
-			retval = wrmalt(Adapter, (UINT)0x0f000d14,
-					&uiResetValue, sizeof(uiResetValue));
-			if (retval < 0) {
-				BCM_DEBUG_PRINT(Adapter, CMHOST, RDM,
-						DBG_LVL_ALL,
-						"%s:%d RDM failed\n",
-						__func__,
-						__LINE__);
-				return retval;
-			}
-			retval = rdmalt(Adapter, (UINT)0x0f000c00,
-					&uiResetValue, sizeof(uiResetValue));
-			if (retval < 0) {
-				BCM_DEBUG_PRINT(Adapter, CMHOST, RDM,
-						DBG_LVL_ALL,
-						"%s:%d RDM failed\n",
-						__func__,
-						__LINE__);
-				return retval;
-			}
-			retval = rdmalt(Adapter, (UINT)0x0f000c00,
-					&uiResetValue, sizeof(uiResetValue));
-			if (retval < 0) {
-				BCM_DEBUG_PRINT(Adapter, CMHOST, RDM,
-						DBG_LVL_ALL,
-						"%s:%d RDM failed\n",
-						__func__,
-						__LINE__);
-				return retval;
-			}
-			uiResetValue = 0x1322a8;
-			retval = wrmalt(Adapter, (UINT)0x0f000d1c,
-					&uiResetValue, sizeof(uiResetValue));
-			if (retval < 0) {
-				BCM_DEBUG_PRINT(Adapter, CMHOST, RDM,
-						DBG_LVL_ALL,
-						"%s:%d RDM failed\n",
-						__func__,
-						__LINE__);
-				return retval;
-			}
-		}
-
-	}
-	Adapter->bDDRInitDone = TRUE;
-	return retval;
-}
-
-int download_ddr_settings(struct bcm_mini_adapter *Adapter)
-{
-	struct bcm_ddr_setting *psDDRSetting = NULL;
-	ULONG RegCount = 0;
-	unsigned long ul_ddr_setting_load_addr =
-		DDR_DUMP_INTERNAL_DEVICE_MEMORY;
-	UINT value = 0;
-	int retval = STATUS_SUCCESS;
-	bool bOverrideSelfRefresh = false;
-
-	switch (Adapter->chip_id) {
-	case 0xbece3200:
-		switch (Adapter->DDRSetting) {
-		case DDR_80_MHZ:
-			psDDRSetting = asT3LP_DDRSetting80MHz;
-			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 = 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 = ARRAY_SIZE(asT3LP_DDRSetting133MHz);
-			RegCount -= T3LP_SKIP_CLOCK_PROGRAM_DUMP_133MHZ;
-			psDDRSetting += T3LP_SKIP_CLOCK_PROGRAM_DUMP_133MHZ;
-			break;
-		default:
-			return -EINVAL;
-		}
-		break;
-
-	case T3LPB:
-	case BCS220_2:
-	case BCS220_2BC:
-	case BCS250_BC:
-	case BCS220_3:
-		switch (Adapter->DDRSetting) {
-		case DDR_80_MHZ:
-			psDDRSetting = asT3LPB_DDRSetting80MHz;
-			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 = 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 = ARRAY_SIZE(asT3LPB_DDRSetting133MHz);
-			RegCount -= T3LPB_SKIP_CLOCK_PROGRAM_DUMP_133MHZ;
-			psDDRSetting += T3LPB_SKIP_CLOCK_PROGRAM_DUMP_133MHZ;
-			break;
-
-		case DDR_160_MHZ:
-			bOverrideSelfRefresh = TRUE;
-			psDDRSetting = asT3LPB_DDRSetting160MHz;
-			RegCount = ARRAY_SIZE(asT3LPB_DDRSetting160MHz);
-			RegCount -= T3LPB_SKIP_CLOCK_PROGRAM_DUMP_160MHZ;
-			psDDRSetting += T3LPB_SKIP_CLOCK_PROGRAM_DUMP_160MHZ;
-
-			break;
-		default:
-			return -EINVAL;
-		}
-		break;
-	case 0xbece0300:
-		switch (Adapter->DDRSetting) {
-		case DDR_80_MHZ:
-			psDDRSetting = asT3_DDRSetting80MHz;
-			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 = 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 = ARRAY_SIZE(asT3_DDRSetting133MHz);
-			RegCount -= T3_SKIP_CLOCK_PROGRAM_DUMP_133MHZ;
-			psDDRSetting += T3_SKIP_CLOCK_PROGRAM_DUMP_133MHZ;
-			break;
-		default:
-			return -EINVAL;
-		}
-	break;
-	case 0xbece0310:
-	    {
-		switch (Adapter->DDRSetting) {
-		case DDR_80_MHZ:
-			psDDRSetting = asT3B_DDRSetting80MHz;
-			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 = 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 = ARRAY_SIZE(asT3B_DDRSetting133MHz);
-			RegCount -= T3B_SKIP_CLOCK_PROGRAM_DUMP_133MHZ;
-			psDDRSetting += T3B_SKIP_CLOCK_PROGRAM_DUMP_133MHZ;
-		break;
-		}
-		break;
-	     }
-	default:
-		return -EINVAL;
-	}
-	/* total number of Register that has to be dumped */
-	value = RegCount;
-	retval = wrmalt(Adapter, ul_ddr_setting_load_addr, &value,
-			sizeof(value));
-	if (retval) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-				"%s:%d\n", __func__, __LINE__);
-
-		return retval;
-	}
-	ul_ddr_setting_load_addr += sizeof(ULONG);
-	/* signature */
-	value = (0x1d1e0dd0);
-	retval = wrmalt(Adapter, ul_ddr_setting_load_addr, &value,
-			sizeof(value));
-	if (retval) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-				"%s:%d\n", __func__, __LINE__);
-		return retval;
-	}
-
-	ul_ddr_setting_load_addr += sizeof(ULONG);
-	RegCount *= (sizeof(struct bcm_ddr_setting)/sizeof(ULONG));
-
-	while (RegCount && !retval) {
-		value = psDDRSetting->ulRegAddress;
-		retval = wrmalt(Adapter, ul_ddr_setting_load_addr, &value,
-				sizeof(value));
-		ul_ddr_setting_load_addr += sizeof(ULONG);
-		if (!retval) {
-			if (bOverrideSelfRefresh
-					&& (psDDRSetting->ulRegAddress
-						== 0x0F007018))
-				value = (psDDRSetting->ulRegValue | (1<<8));
-			else
-				value = psDDRSetting->ulRegValue;
-
-			if (STATUS_SUCCESS != wrmalt(Adapter,
-						     ul_ddr_setting_load_addr,
-						     &value,
-						     sizeof(value))) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-						"%s:%d\n", __func__, __LINE__);
-				break;
-			}
-		}
-		ul_ddr_setting_load_addr += sizeof(ULONG);
-		RegCount--;
-		psDDRSetting++;
-	}
-	return retval;
-}
diff --git a/drivers/staging/bcm/DDRInit.h b/drivers/staging/bcm/DDRInit.h
deleted file mode 100644
index b0196fce9255..000000000000
--- a/drivers/staging/bcm/DDRInit.h
+++ /dev/null
@@ -1,9 +0,0 @@
-#ifndef _DDR_INIT_H_
-#define _DDR_INIT_H_
-
-
-
-int ddr_init(struct bcm_mini_adapter *psAdapter);
-int download_ddr_settings(struct bcm_mini_adapter *psAdapter);
-
-#endif
diff --git a/drivers/staging/bcm/Debug.h b/drivers/staging/bcm/Debug.h
deleted file mode 100644
index 7b331215c1ac..000000000000
--- a/drivers/staging/bcm/Debug.h
+++ /dev/null
@@ -1,242 +0,0 @@
-/*
- * Debug.h
- *
- * Dynamic (runtime) debug framework implementation.
- * -kaiwan.
- */
-#ifndef _DEBUG_H
-#define _DEBUG_H
-#include <linux/string.h>
-#define NONE 0xFFFF
-
-/* TYPE and SUBTYPE
- * Define valid TYPE (or category or code-path, however you like to think of it)
- * and SUBTYPE s.
- * Type and SubType are treated as bitmasks.
- */
-#define DBG_TYPE_INITEXIT	(1 << 0)	/* 1 */
-#define DBG_TYPE_TX		(1 << 1)	/* 2 */
-#define DBG_TYPE_RX		(1 << 2)	/* 4 */
-#define DBG_TYPE_OTHERS		(1 << 3)	/* 8 */
-#define NUMTYPES		4
-
-/* -SUBTYPEs for TX :  TYPE is DBG_TYPE_TX -----//
- * Transmit.c ,Arp.c, LeakyBucket.c, And Qos.c
- * total 17 macros
- */
-/* Transmit.c */
-#define TX		1
-#define MP_SEND		(TX << 0)
-#define NEXT_SEND	(TX << 1)
-#define TX_FIFO		(TX << 2)
-#define TX_CONTROL	(TX << 3)
-
-/* Arp.c */
-#define IP_ADDR		(TX << 4)
-#define ARP_REQ		(TX << 5)
-#define ARP_RESP	(TX << 6)
-
-/* Leakybucket.c */
-#define TOKEN_COUNTS	(TX << 8)
-#define CHECK_TOKENS	(TX << 9)
-#define TX_PACKETS	(TX << 10)
-#define TIMER		(TX << 11)
-
-/* Qos.c */
-#define QOS		TX
-#define QUEUE_INDEX	(QOS << 12)
-#define IPV4_DBG	(QOS << 13)
-#define IPV6_DBG	(QOS << 14)
-#define PRUNE_QUEUE	(QOS << 15)
-#define SEND_QUEUE	(QOS << 16)
-
-/* TX_Misc */
-#define TX_OSAL_DBG	(TX << 17)
-
-/* --SUBTYPEs for ------INIT & EXIT---------------------
- * ------------ TYPE is DBG_TYPE_INITEXIT -----//
- * DriverEntry.c, bcmfwup.c, ChipDetectTask.c, HaltnReset.c, InterfaceDDR.c
- */
-#define MP		1
-#define DRV_ENTRY	(MP << 0)
-#define MP_INIT		(MP << 1)
-#define READ_REG	(MP << 3)
-#define DISPATCH	(MP << 2)
-#define CLAIM_ADAP	(MP << 4)
-#define REG_IO_PORT	(MP << 5)
-#define INIT_DISP	(MP << 6)
-#define RX_INIT		(MP << 7)
-
-/* -SUBTYPEs for --RX----------------------------------
- * ------------RX  :  TYPE is DBG_TYPE_RX -----//
- * Receive.c
- */
-#define RX		1
-#define RX_DPC		(RX << 0)
-#define RX_CTRL		(RX << 3)
-#define RX_DATA		(RX << 4)
-#define MP_RETURN	(RX << 1)
-#define LINK_MSG	(RX << 2)
-
-/* -SUBTYPEs for ----OTHER ROUTINES------------------
- * ------------OTHERS  :  TYPE is DBG_TYPE_OTHER -----//
- * HaltnReset,CheckForHang,PnP,Misc,CmHost
- * total 12 macros
- */
-#define OTHERS		1
-#define ISR		OTHERS
-#define MP_DPC		(ISR << 0)
-
-/* HaltnReset.c */
-#define HALT		OTHERS
-#define MP_HALT		(HALT << 1)
-#define CHECK_HANG	(HALT << 2)
-#define MP_RESET	(HALT << 3)
-#define MP_SHUTDOWN	(HALT << 4)
-
-/* pnp.c */
-#define PNP		OTHERS
-#define MP_PNP		(PNP << 5)
-
-/* Misc.c */
-#define MISC		OTHERS
-#define DUMP_INFO	(MISC << 6)
-#define CLASSIFY	(MISC << 7)
-#define LINK_UP_MSG	(MISC << 8)
-#define CP_CTRL_PKT	(MISC << 9)
-#define DUMP_CONTROL	(MISC << 10)
-#define LED_DUMP_INFO	(MISC << 11)
-
-/* CmHost.c */
-#define CMHOST		OTHERS
-#define SERIAL		(OTHERS << 12)
-#define IDLE_MODE	(OTHERS << 13)
-#define WRM		(OTHERS << 14)
-#define RDM		(OTHERS << 15)
-
-/* TODO - put PHS_SEND in Tx PHS_RECEIVE in Rx path ? */
-#define PHS_SEND	(OTHERS << 16)
-#define PHS_RECEIVE	(OTHERS << 17)
-#define PHS_MODULE	(OTHERS << 18)
-
-#define INTF_INIT	(OTHERS << 19)
-#define INTF_ERR	(OTHERS << 20)
-#define INTF_WARN	(OTHERS << 21)
-#define INTF_NORM	(OTHERS << 22)
-
-#define IRP_COMPLETION		(OTHERS << 23)
-#define SF_DESCRIPTOR_CNTS	(OTHERS << 24)
-#define PHS_DISPATCH		(OTHERS << 25)
-#define OSAL_DBG		(OTHERS << 26)
-#define NVM_RW			(OTHERS << 27)
-
-#define HOST_MIBS	(OTHERS << 28)
-#define CONN_MSG	(CMHOST << 29)
-
-/* Debug level
- * We have 8 debug levels, in (numerical) increasing order of verbosity.
- * IMP: Currently implementing ONLY DBG_LVL_ALL , i.e. , all debug prints will
- * appear (of course, iff global debug flag is ON and we match the Type and SubType).
- * Finer granularity debug levels are currently not in use, although the feature exists.
- *
- * Another way to say this:
- * All the debug prints currently have 'debug_level' set to DBG_LVL_ALL .
- * You can compile-time change that to any of the below, if you wish to. However, as of now, there's
- * no dynamic facility to have the userspace 'TestApp' set debug_level. Slated for future expansion.
- */
-#define BCM_ALL		7
-#define	BCM_LOW		6
-#define	BCM_PRINT	5
-#define	BCM_NORMAL	4
-#define	BCM_MEDIUM	3
-#define	BCM_SCREAM	2
-#define	BCM_ERR		1
-/* Not meant for developer in debug prints.
- * To be used to disable all prints by setting the DBG_LVL_CURR to this value
- */
-#define	BCM_NONE	0
-
-/* The current driver logging level.
- * Everything at this level and (numerically) lower (meaning higher prio)
- * is logged.
- * Replace 'BCM_ALL' in the DBG_LVL_CURR macro with the logging level desired.
- * For eg. to set the logging level to 'errors only' use:
- *	 #define DBG_LVL_CURR	(BCM_ERR)
- */
-
-#define DBG_LVL_CURR	(BCM_ALL)
-#define DBG_LVL_ALL	BCM_ALL
-
-/* ---Userspace mapping of Debug State.
- * Delibrately matches that of the Windows driver..
- * The TestApp's ioctl passes this struct to us.
- */
-struct bcm_user_debug_state {
-	unsigned int Subtype, Type;
-	unsigned int OnOff;
-/*	unsigned int debug_level; future expansion */
-} __packed;
-
-/* ---Kernel-space mapping of Debug State */
-struct bcm_debug_state {
-	unsigned int type;
-	/* A bitmap of 32 bits for Subtype per Type.
-	 * Valid indexes in 'subtype' array are *only* 1,2,4 and 8,
-	 * corresponding to valid Type values. Hence we use the 'Type' field
-	 * as the index value, ignoring the array entries 0,3,5,6,7 !
-	 */
-	unsigned int subtype[(NUMTYPES*2)+1];
-	unsigned int debug_level;
-};
-/* Instantiated in the Adapter structure
- * We'll reuse the debug level parameter to include a bit (the MSB) to indicate whether or not
- * we want the function's name printed.
- */
-#define DBG_NO_FUNC_PRINT	(1 << 31)
-#define DBG_LVL_BITMASK		0xFF
-
-/* --- Only for direct printk's; "hidden" to API. */
-#define DBG_TYPE_PRINTK		3
-
-#define BCM_DEBUG_PRINT(Adapter, Type, SubType, dbg_level, string, args...) \
-	do {								\
-		if (DBG_TYPE_PRINTK == Type)				\
-			pr_info("%s:" string, __func__, ##args);	\
-		else if (Adapter &&					\
-			(dbg_level & DBG_LVL_BITMASK) <= Adapter->stDebugState.debug_level && \
-			(Type & Adapter->stDebugState.type) &&		\
-			(SubType & Adapter->stDebugState.subtype[Type])) { \
-			if (dbg_level & DBG_NO_FUNC_PRINT)		\
-				pr_debug("%s:\n", string);	\
-			else						\
-				pr_debug("%s:\n" string, __func__, ##args); \
-		}							\
-	} while (0)
-
-#define BCM_DEBUG_PRINT_BUFFER(Adapter, Type, SubType, dbg_level,  buffer, bufferlen) \
-	do {								\
-		if (DBG_TYPE_PRINTK == Type ||				\
-			(Adapter &&					\
-				(dbg_level & DBG_LVL_BITMASK) <= Adapter->stDebugState.debug_level  && \
-				(Type & Adapter->stDebugState.type) &&	\
-				(SubType & Adapter->stDebugState.subtype[Type]))) { \
-			pr_debug("%s:\n", __func__);			\
-			print_hex_dump(KERN_DEBUG, " ", DUMP_PREFIX_OFFSET, \
-				16, 1, buffer, bufferlen, false);	\
-		}							\
-	} while (0)
-
-#define BCM_SHOW_DEBUG_BITMAP(Adapter) do {			\
-	int i;							\
-	for (i = 0; i < (NUMTYPES * 2) + 1; i++) {		\
-		if ((i == 1) || (i == 2) || (i == 4) || (i == 8)) {		\
-			/* CAUTION! Forcefully turn on ALL debug paths and subpaths! \
-			 * Adapter->stDebugState.subtype[i] = 0xffffffff; \
-			 */ \
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "subtype[%d] = 0x%08x\n",	\
-					i, Adapter->stDebugState.subtype[i]); \
-		}	\
-	}		\
-} while (0)
-
-#endif
diff --git a/drivers/staging/bcm/HandleControlPacket.c b/drivers/staging/bcm/HandleControlPacket.c
deleted file mode 100644
index dd5d138a6528..000000000000
--- a/drivers/staging/bcm/HandleControlPacket.c
+++ /dev/null
@@ -1,241 +0,0 @@
-/**
- * @file HandleControlPacket.c
- * This file contains the routines to deal with
- * sending and receiving of control packets.
- */
-#include "headers.h"
-
-/**
- * When a control packet is received, analyze the
- * "status" and call appropriate response function.
- * Enqueue the control packet for Application.
- * @return None
- */
-static VOID handle_rx_control_packet(struct bcm_mini_adapter *Adapter,
-				     struct sk_buff *skb)
-{
-	struct bcm_tarang_data *pTarang = NULL;
-	bool HighPriorityMessage = false;
-	struct sk_buff *newPacket = NULL;
-	CHAR cntrl_msg_mask_bit = 0;
-	bool drop_pkt_flag = TRUE;
-	USHORT usStatus = *(PUSHORT)(skb->data);
-
-	if (netif_msg_pktdata(Adapter))
-		print_hex_dump(KERN_DEBUG, PFX "rx control: ", DUMP_PREFIX_NONE,
-			       16, 1, skb->data, skb->len, 0);
-
-	switch (usStatus) {
-	case CM_RESPONSES:               /* 0xA0 */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CP_CTRL_PKT,
-			DBG_LVL_ALL,
-			"MAC Version Seems to be Non Multi-Classifier, rejected by Driver");
-		HighPriorityMessage = TRUE;
-		break;
-	case CM_CONTROL_NEWDSX_MULTICLASSIFIER_RESP:
-		HighPriorityMessage = TRUE;
-		if (Adapter->LinkStatus == LINKUP_DONE)
-			CmControlResponseMessage(Adapter,
-				(skb->data + sizeof(USHORT)));
-		break;
-	case LINK_CONTROL_RESP:          /* 0xA2 */
-	case STATUS_RSP:                 /* 0xA1 */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CP_CTRL_PKT,
-			DBG_LVL_ALL, "LINK_CONTROL_RESP");
-		HighPriorityMessage = TRUE;
-		LinkControlResponseMessage(Adapter,
-			(skb->data + sizeof(USHORT)));
-		break;
-	case STATS_POINTER_RESP:         /* 0xA6 */
-		HighPriorityMessage = TRUE;
-		StatisticsResponse(Adapter, (skb->data + sizeof(USHORT)));
-		break;
-	case IDLE_MODE_STATUS:           /* 0xA3 */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CP_CTRL_PKT,
-			DBG_LVL_ALL,
-			"IDLE_MODE_STATUS Type Message Got from F/W");
-		InterfaceIdleModeRespond(Adapter, (PUINT)(skb->data +
-					sizeof(USHORT)));
-		HighPriorityMessage = TRUE;
-		break;
-
-	case AUTH_SS_HOST_MSG:
-		HighPriorityMessage = TRUE;
-		break;
-
-	default:
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CP_CTRL_PKT,
-			DBG_LVL_ALL, "Got Default Response");
-		/* Let the Application Deal with This Packet */
-		break;
-	}
-
-	/* Queue The Control Packet to The Application Queues */
-	down(&Adapter->RxAppControlQueuelock);
-
-	for (pTarang = Adapter->pTarangs; pTarang; pTarang = pTarang->next) {
-		if (Adapter->device_removed)
-			break;
-
-		drop_pkt_flag = TRUE;
-		/*
-		 * There are cntrl msg from A0 to AC. It has been mapped to 0 to
-		 * C bit in the cntrl mask.
-		 * Also, by default AD to BF has been masked to the rest of the
-		 * bits... which wil be ON by default.
-		 * if mask bit is enable to particular pkt status, send it out
-		 * to app else stop it.
-		 */
-		cntrl_msg_mask_bit = (usStatus & 0x1F);
-		/*
-		 * printk("\ninew  msg  mask bit which is disable in mask:%X",
-		 *	cntrl_msg_mask_bit);
-		 */
-		if (pTarang->RxCntrlMsgBitMask & (1 << cntrl_msg_mask_bit))
-			drop_pkt_flag = false;
-
-		if ((drop_pkt_flag == TRUE) ||
-				(pTarang->AppCtrlQueueLen > MAX_APP_QUEUE_LEN)
-				|| ((pTarang->AppCtrlQueueLen >
-					MAX_APP_QUEUE_LEN / 2) &&
-				    (HighPriorityMessage == false))) {
-			/*
-			 * Assumption:-
-			 * 1. every tarang manages it own dropped pkt
-			 *    statitistics
-			 * 2. Total packet dropped per tarang will be equal to
-			 *    the sum of all types of dropped pkt by that
-			 *    tarang only.
-			 */
-			struct bcm_mibs_dropped_cntrl_msg *msg =
-				&pTarang->stDroppedAppCntrlMsgs;
-			switch (*(PUSHORT)skb->data) {
-			case CM_RESPONSES:
-				msg->cm_responses++;
-				break;
-			case CM_CONTROL_NEWDSX_MULTICLASSIFIER_RESP:
-				msg->cm_control_newdsx_multiclassifier_resp++;
-				break;
-			case LINK_CONTROL_RESP:
-				msg->link_control_resp++;
-				break;
-			case STATUS_RSP:
-				msg->status_rsp++;
-				break;
-			case STATS_POINTER_RESP:
-				msg->stats_pointer_resp++;
-				break;
-			case IDLE_MODE_STATUS:
-				msg->idle_mode_status++;
-				break;
-			case AUTH_SS_HOST_MSG:
-				msg->auth_ss_host_msg++;
-				break;
-			default:
-				msg->low_priority_message++;
-				break;
-			}
-
-			continue;
-		}
-
-		newPacket = skb_clone(skb, GFP_KERNEL);
-		if (!newPacket)
-			break;
-		ENQUEUEPACKET(pTarang->RxAppControlHead,
-				pTarang->RxAppControlTail, newPacket);
-		pTarang->AppCtrlQueueLen++;
-	}
-	up(&Adapter->RxAppControlQueuelock);
-	wake_up(&Adapter->process_read_wait_queue);
-	dev_kfree_skb(skb);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CP_CTRL_PKT, DBG_LVL_ALL,
-			"After wake_up_interruptible");
-}
-
-/**
- * @ingroup ctrl_pkt_functions
- * Thread to handle control pkt reception
- */
-
-/* pointer to adapter object*/
-int control_packet_handler(struct bcm_mini_adapter *Adapter)
-{
-	struct sk_buff *ctrl_packet = NULL;
-	unsigned long flags = 0;
-	/* struct timeval tv; */
-	/* int *puiBuffer = NULL; */
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CP_CTRL_PKT, DBG_LVL_ALL,
-		"Entering to make thread wait on control packet event!");
-	while (1) {
-		wait_event_interruptible(Adapter->process_rx_cntrlpkt,
-			atomic_read(&Adapter->cntrlpktCnt) ||
-			Adapter->bWakeUpDevice ||
-			kthread_should_stop());
-
-
-		if (kthread_should_stop()) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CP_CTRL_PKT,
-				DBG_LVL_ALL, "Exiting\n");
-			return 0;
-		}
-		if (TRUE == Adapter->bWakeUpDevice) {
-			Adapter->bWakeUpDevice = false;
-			if ((false == Adapter->bTriedToWakeUpFromlowPowerMode)
-					&& ((TRUE == Adapter->IdleMode) ||
-					    (TRUE == Adapter->bShutStatus))) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
-					CP_CTRL_PKT, DBG_LVL_ALL,
-					"Calling InterfaceAbortIdlemode\n");
-				/*
-				 * Adapter->bTriedToWakeUpFromlowPowerMode
-				 *					= TRUE;
-				 */
-				InterfaceIdleModeWakeup(Adapter);
-			}
-			continue;
-		}
-
-		while (atomic_read(&Adapter->cntrlpktCnt)) {
-			spin_lock_irqsave(&Adapter->control_queue_lock, flags);
-			ctrl_packet = Adapter->RxControlHead;
-			if (ctrl_packet) {
-				DEQUEUEPACKET(Adapter->RxControlHead,
-					Adapter->RxControlTail);
-				/* Adapter->RxControlHead=ctrl_packet->next; */
-			}
-
-			spin_unlock_irqrestore(&Adapter->control_queue_lock,
-						flags);
-			handle_rx_control_packet(Adapter, ctrl_packet);
-			atomic_dec(&Adapter->cntrlpktCnt);
-		}
-
-		SetUpTargetDsxBuffers(Adapter);
-	}
-	return STATUS_SUCCESS;
-}
-
-INT flushAllAppQ(void)
-{
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	struct bcm_tarang_data *pTarang = NULL;
-	struct sk_buff *PacketToDrop = NULL;
-
-	for (pTarang = Adapter->pTarangs; pTarang; pTarang = pTarang->next) {
-		while (pTarang->RxAppControlHead != NULL) {
-			PacketToDrop = pTarang->RxAppControlHead;
-			DEQUEUEPACKET(pTarang->RxAppControlHead,
-					pTarang->RxAppControlTail);
-			dev_kfree_skb(PacketToDrop);
-		}
-		pTarang->AppCtrlQueueLen = 0;
-		/* dropped contrl packet statistics also should be reset. */
-		memset((PVOID)&pTarang->stDroppedAppCntrlMsgs, 0,
-			sizeof(struct bcm_mibs_dropped_cntrl_msg));
-
-	}
-	return STATUS_SUCCESS;
-}
-
-
diff --git a/drivers/staging/bcm/HostMIBSInterface.h b/drivers/staging/bcm/HostMIBSInterface.h
deleted file mode 100644
index f922ac49b70e..000000000000
--- a/drivers/staging/bcm/HostMIBSInterface.h
+++ /dev/null
@@ -1,192 +0,0 @@
-#ifndef _HOST_MIBSINTERFACE_H
-#define _HOST_MIBSINTERFACE_H
-
-/*
- * Copyright (c) 2007 Beceem Communications Pvt. Ltd
- * File Name: HostMIBSInterface.h
- * Abstract: This file contains DS used by the Host to update the Host
- * statistics used for the MIBS.
- */
-
-#define MIBS_MAX_CLASSIFIERS		100
-#define MIBS_MAX_PHSRULES		100
-#define MIBS_MAX_SERVICEFLOWS		17
-#define MIBS_MAX_IP_RANGE_LENGTH	4
-#define MIBS_MAX_PORT_RANGE		4
-#define MIBS_MAX_PROTOCOL_LENGTH	32
-#define MIBS_MAX_PHS_LENGTHS		255
-#define MIBS_IPV6_ADDRESS_SIZEINBYTES	0x10
-#define MIBS_IP_LENGTH_OF_ADDRESS	4
-#define MIBS_MAX_HIST_ENTRIES		12
-#define MIBS_PKTSIZEHIST_RANGE		128
-
-union bcm_mibs_ip_addr {
-	struct {
-		/* Source Ip Address Range */
-		unsigned long ulIpv4Addr[MIBS_MAX_IP_RANGE_LENGTH];
-		/* Source Ip Mask Address Range */
-		unsigned long ulIpv4Mask[MIBS_MAX_IP_RANGE_LENGTH];
-	};
-	struct {
-		/* Source Ip Address Range */
-		unsigned long ulIpv6Addr[MIBS_MAX_IP_RANGE_LENGTH * 4];
-		/* Source Ip Mask Address Range */
-		unsigned long ulIpv6Mask[MIBS_MAX_IP_RANGE_LENGTH * 4];
-	};
-	struct {
-		unsigned char ucIpv4Address[MIBS_MAX_IP_RANGE_LENGTH * MIBS_IP_LENGTH_OF_ADDRESS];
-		unsigned char ucIpv4Mask[MIBS_MAX_IP_RANGE_LENGTH * MIBS_IP_LENGTH_OF_ADDRESS];
-	};
-	struct {
-		unsigned char ucIpv6Address[MIBS_MAX_IP_RANGE_LENGTH * MIBS_IPV6_ADDRESS_SIZEINBYTES];
-		unsigned char ucIpv6Mask[MIBS_MAX_IP_RANGE_LENGTH * MIBS_IPV6_ADDRESS_SIZEINBYTES];
-	};
-};
-
-struct bcm_mibs_host_info {
-	u64	GoodTransmits;
-	u64	GoodReceives;
-	/* this to keep track of the Tx and Rx MailBox Registers. */
-	unsigned long	NumDesUsed;
-	unsigned long	CurrNumFreeDesc;
-	unsigned long	PrevNumFreeDesc;
-	/* to keep track the no of byte received */
-	unsigned long	PrevNumRcevBytes;
-	unsigned long	CurrNumRcevBytes;
-	/* QOS Related */
-	unsigned long	BEBucketSize;
-	unsigned long	rtPSBucketSize;
-	unsigned long	LastTxQueueIndex;
-	bool	TxOutofDescriptors;
-	bool	TimerActive;
-	u32	u32TotalDSD;
-	u32	aTxPktSizeHist[MIBS_MAX_HIST_ENTRIES];
-	u32	aRxPktSizeHist[MIBS_MAX_HIST_ENTRIES];
-};
-
-struct bcm_mibs_classifier_rule {
-	unsigned long	ulSFID;
-	unsigned char	ucReserved[2];
-	u16	uiClassifierRuleIndex;
-	bool	bUsed;
-	unsigned short	usVCID_Value;
-	u8	u8ClassifierRulePriority;
-	union bcm_mibs_ip_addr stSrcIpAddress;
-	/* IP Source Address Length */
-	unsigned char	ucIPSourceAddressLength;
-	union bcm_mibs_ip_addr stDestIpAddress;
-	/* IP Destination Address Length */
-	unsigned char	ucIPDestinationAddressLength;
-	unsigned char	ucIPTypeOfServiceLength;
-	unsigned char	ucTosLow;
-	unsigned char	ucTosHigh;
-	unsigned char	ucTosMask;
-	unsigned char	ucProtocolLength;
-	unsigned char	ucProtocol[MIBS_MAX_PROTOCOL_LENGTH];
-	unsigned short	usSrcPortRangeLo[MIBS_MAX_PORT_RANGE];
-	unsigned short	usSrcPortRangeHi[MIBS_MAX_PORT_RANGE];
-	unsigned char	ucSrcPortRangeLength;
-	unsigned short	usDestPortRangeLo[MIBS_MAX_PORT_RANGE];
-	unsigned short	usDestPortRangeHi[MIBS_MAX_PORT_RANGE];
-	unsigned char	ucDestPortRangeLength;
-	bool	bProtocolValid;
-	bool	bTOSValid;
-	bool	bDestIpValid;
-	bool	bSrcIpValid;
-	unsigned char	ucDirection;
-	bool	bIpv6Protocol;
-	u32	u32PHSRuleID;
-};
-
-struct bcm_mibs_phs_rule {
-	unsigned long	ulSFID;
-	u8	u8PHSI;
-	u8	u8PHSFLength;
-	u8	u8PHSF[MIBS_MAX_PHS_LENGTHS];
-	u8	u8PHSMLength;
-	u8	u8PHSM[MIBS_MAX_PHS_LENGTHS];
-	u8	u8PHSS;
-	u8	u8PHSV;
-	u8	reserved[5];
-	long	PHSModifiedBytes;
-	unsigned long	PHSModifiedNumPackets;
-	unsigned long	PHSErrorNumPackets;
-};
-
-struct bcm_mibs_parameters {
-	u32 wmanIfSfid;
-	u32 wmanIfCmnCpsSfState;
-	u32 wmanIfCmnCpsMaxSustainedRate;
-	u32 wmanIfCmnCpsMaxTrafficBurst;
-	u32 wmanIfCmnCpsMinReservedRate;
-	u32 wmanIfCmnCpsToleratedJitter;
-	u32 wmanIfCmnCpsMaxLatency;
-	u32 wmanIfCmnCpsFixedVsVariableSduInd;
-	u32 wmanIfCmnCpsSduSize;
-	u32 wmanIfCmnCpsSfSchedulingType;
-	u32 wmanIfCmnCpsArqEnable;
-	u32 wmanIfCmnCpsArqWindowSize;
-	u32 wmanIfCmnCpsArqBlockLifetime;
-	u32 wmanIfCmnCpsArqSyncLossTimeout;
-	u32 wmanIfCmnCpsArqDeliverInOrder;
-	u32 wmanIfCmnCpsArqRxPurgeTimeout;
-	u32 wmanIfCmnCpsArqBlockSize;
-	u32 wmanIfCmnCpsMinRsvdTolerableRate;
-	u32 wmanIfCmnCpsReqTxPolicy;
-	u32 wmanIfCmnSfCsSpecification;
-	u32 wmanIfCmnCpsTargetSaid;
-};
-
-struct bcm_mibs_table {
-	unsigned long	ulSFID;
-	unsigned short	usVCID_Value;
-	unsigned int	uiThreshold;
-	u8	u8TrafficPriority;
-	bool	bValid;
-	bool	bActive;
-	bool	bActivateRequestSent;
-	u8	u8QueueType;
-	unsigned int	uiMaxBucketSize;
-	unsigned int	uiCurrentQueueDepthOnTarget;
-	unsigned int	uiCurrentBytesOnHost;
-	unsigned int	uiCurrentPacketsOnHost;
-	unsigned int	uiDroppedCountBytes;
-	unsigned int	uiDroppedCountPackets;
-	unsigned int	uiSentBytes;
-	unsigned int	uiSentPackets;
-	unsigned int	uiCurrentDrainRate;
-	unsigned int	uiThisPeriodSentBytes;
-	u64	liDrainCalculated;
-	unsigned int	uiCurrentTokenCount;
-	u64	liLastUpdateTokenAt;
-	unsigned int	uiMaxAllowedRate;
-	unsigned int	NumOfPacketsSent;
-	unsigned char ucDirection;
-	unsigned short	usCID;
-	struct bcm_mibs_parameters stMibsExtServiceFlowTable;
-	unsigned int	uiCurrentRxRate;
-	unsigned int	uiThisPeriodRxBytes;
-	unsigned int	uiTotalRxBytes;
-	unsigned int	uiTotalTxBytes;
-};
-
-struct bcm_mibs_dropped_cntrl_msg {
-	unsigned long cm_responses;
-	unsigned long cm_control_newdsx_multiclassifier_resp;
-	unsigned long link_control_resp;
-	unsigned long status_rsp;
-	unsigned long stats_pointer_resp;
-	unsigned long idle_mode_status;
-	unsigned long auth_ss_host_msg;
-	unsigned long low_priority_message;
-};
-
-struct bcm_host_stats_mibs {
-	struct bcm_mibs_host_info stHostInfo;
-	struct bcm_mibs_classifier_rule astClassifierTable[MIBS_MAX_CLASSIFIERS];
-	struct bcm_mibs_table	astSFtable[MIBS_MAX_SERVICEFLOWS];
-	struct bcm_mibs_phs_rule astPhsRulesTable[MIBS_MAX_PHSRULES];
-	struct bcm_mibs_dropped_cntrl_msg stDroppedAppCntrlMsgs;
-};
-
-#endif
diff --git a/drivers/staging/bcm/IPv6Protocol.c b/drivers/staging/bcm/IPv6Protocol.c
deleted file mode 100644
index 27f3f416f184..000000000000
--- a/drivers/staging/bcm/IPv6Protocol.c
+++ /dev/null
@@ -1,476 +0,0 @@
-#include "headers.h"
-
-static bool MatchSrcIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
-	struct bcm_ipv6_hdr *pstIpv6Header);
-static bool MatchDestIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
-	struct bcm_ipv6_hdr *pstIpv6Header);
-static VOID DumpIpv6Header(struct bcm_ipv6_hdr *pstIpv6Header);
-
-static UCHAR *GetNextIPV6ChainedHeader(UCHAR **ppucPayload,
-	UCHAR *pucNextHeader, bool *bParseDone, USHORT *pusPayloadLength)
-{
-	UCHAR *pucRetHeaderPtr = NULL;
-	UCHAR *pucPayloadPtr = NULL;
-	USHORT  usNextHeaderOffset = 0;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	if ((ppucPayload == NULL) || (*pusPayloadLength == 0) ||
-		(*bParseDone)) {
-		*bParseDone = TRUE;
-		return NULL;
-	}
-
-	pucRetHeaderPtr = *ppucPayload;
-	pucPayloadPtr = *ppucPayload;
-
-	if (!pucRetHeaderPtr || !pucPayloadPtr) {
-		*bParseDone = TRUE;
-		return NULL;
-	}
-
-	/* Get the Nextt Header Type */
-	*bParseDone = false;
-
-
-	switch (*pucNextHeader) {
-	case IPV6HDR_TYPE_HOPBYHOP:
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
-				DBG_LVL_ALL, "\nIPv6 HopByHop Header");
-		usNextHeaderOffset += sizeof(struct bcm_ipv6_options_hdr);
-		break;
-
-	case IPV6HDR_TYPE_ROUTING:
-		{
-			struct bcm_ipv6_routing_hdr *pstIpv6RoutingHeader;
-
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
-					DBG_LVL_ALL, "\nIPv6 Routing Header");
-			pstIpv6RoutingHeader =
-				(struct bcm_ipv6_routing_hdr *)pucPayloadPtr;
-			usNextHeaderOffset += sizeof(struct bcm_ipv6_routing_hdr);
-			usNextHeaderOffset += pstIpv6RoutingHeader->ucNumAddresses *
-					      IPV6_ADDRESS_SIZEINBYTES;
-		}
-		break;
-
-	case IPV6HDR_TYPE_FRAGMENTATION:
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
-				DBG_LVL_ALL,
-				"\nIPv6 Fragmentation Header");
-		usNextHeaderOffset += sizeof(struct bcm_ipv6_fragment_hdr);
-		break;
-
-	case IPV6HDR_TYPE_DESTOPTS:
-		{
-			struct bcm_ipv6_dest_options_hdr *pstIpv6DestOptsHdr =
-				(struct bcm_ipv6_dest_options_hdr *)pucPayloadPtr;
-			int nTotalOptions = pstIpv6DestOptsHdr->ucHdrExtLen;
-
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
-					DBG_LVL_ALL,
-					"\nIPv6 DestOpts Header Header");
-			usNextHeaderOffset += sizeof(struct bcm_ipv6_dest_options_hdr);
-			usNextHeaderOffset += nTotalOptions *
-					      IPV6_DESTOPTS_HDR_OPTIONSIZE;
-		}
-		break;
-
-
-	case IPV6HDR_TYPE_AUTHENTICATION:
-		{
-			struct bcm_ipv6_authentication_hdr *pstIpv6AuthHdr =
-				(struct bcm_ipv6_authentication_hdr *)pucPayloadPtr;
-			int nHdrLen = pstIpv6AuthHdr->ucLength;
-
-			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");
-		*bParseDone = TRUE;
-		break;
-
-	case IPV6_ICMP_HDR_TYPE:
-		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");
-		*bParseDone = TRUE;
-		break;
-
-	case UDP_HEADER_TYPE:
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
-				DBG_LVL_ALL, "\nUDP Header");
-		*bParseDone = TRUE;
-		break;
-
-	default:
-		*bParseDone = TRUE;
-		break;
-	}
-
-	if (*bParseDone == false) {
-		if (*pusPayloadLength <= usNextHeaderOffset) {
-			*bParseDone = TRUE;
-		} else {
-			*pucNextHeader = *pucPayloadPtr;
-			pucPayloadPtr += usNextHeaderOffset;
-			(*pusPayloadLength) -= usNextHeaderOffset;
-		}
-
-	}
-
-	*ppucPayload = pucPayloadPtr;
-	return pucRetHeaderPtr;
-}
-
-
-static UCHAR GetIpv6ProtocolPorts(UCHAR *pucPayload, USHORT *pusSrcPort,
-	USHORT *pusDestPort, USHORT usPayloadLength, UCHAR ucNextHeader)
-{
-	UCHAR *pIpv6HdrScanContext = pucPayload;
-	bool bDone = false;
-	UCHAR ucHeaderType = 0;
-	UCHAR *pucNextHeader = NULL;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	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));
-			}
-			break;
-
-		}
-	}
-	return ucHeaderType;
-}
-
-
-/*
- * Arg 1 struct bcm_mini_adapter *Adapter is a pointer ot the driver control
- * structure
- * Arg 2 PVOID pcIpHeader is a pointer to the IP header of the packet
- */
-USHORT	IpVersion6(struct bcm_mini_adapter *Adapter, PVOID pcIpHeader,
-		   struct bcm_classifier_rule *pstClassifierRule)
-{
-	USHORT	ushDestPort = 0;
-	USHORT	ushSrcPort = 0;
-	UCHAR   ucNextProtocolAboveIP = 0;
-	struct bcm_ipv6_hdr *pstIpv6Header = NULL;
-	bool bClassificationSucceed = false;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
-			DBG_LVL_ALL, "IpVersion6 ==========>\n");
-
-	pstIpv6Header = pcIpHeader;
-
-	DumpIpv6Header(pstIpv6Header);
-
-	/*
-	 * Try to get the next higher layer protocol
-	 * and the Ports Nos if TCP or UDP
-	 */
-	ucNextProtocolAboveIP = GetIpv6ProtocolPorts((UCHAR *)(pcIpHeader +
-						     sizeof(struct bcm_ipv6_hdr)),
-						     &ushSrcPort,
-						     &ushDestPort,
-						     pstIpv6Header->usPayloadLength,
-						     pstIpv6Header->ucNextHeader);
-
-	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
-			 */
-			break;
-		}
-
-		bClassificationSucceed = MatchSrcIpv6Address(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");
-
-		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");
-
-			/* 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");
-		}
-	} while (0);
-
-	if (bClassificationSucceed == TRUE) {
-		INT iMatchedSFQueueIndex = 0;
-
-		iMatchedSFQueueIndex = SearchSfid(Adapter,
-						  pstClassifierRule->ulSFID);
-		if ((iMatchedSFQueueIndex >= NO_OF_QUEUES) ||
-		    (Adapter->PackInfo[iMatchedSFQueueIndex].bActive == false))
-			bClassificationSucceed = false;
-	}
-
-	return bClassificationSucceed;
-}
-
-
-static bool MatchSrcIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
-				struct bcm_ipv6_hdr *pstIpv6Header)
-{
-	UINT uiLoopIndex = 0;
-	UINT uiIpv6AddIndex = 0;
-	UINT uiIpv6AddrNoLongWords = 4;
-	ULONG aulSrcIP[4];
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	union u_ip_address *src_addr = &pstClassifierRule->stSrcIpAddress;
-
-	/*
-	 * 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 (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]);
-
-	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");
-		DumpIpv6Address(&src_addr->ulIpv6Mask[uiLoopIndex]);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
-				"\n Src Ipv6 Address In Classifier Rule :\n");
-		DumpIpv6Address(&src_addr->ulIpv6Addr[uiLoopIndex]);
-
-		for (uiIpv6AddIndex = 0;
-		     uiIpv6AddIndex < uiIpv6AddrNoLongWords;
-		     uiIpv6AddIndex++) {
-			if ((src_addr->ulIpv6Mask[uiLoopIndex+uiIpv6AddIndex] &
-				aulSrcIP[uiIpv6AddIndex]) !=
-			    src_addr->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");
-				return TRUE;
-			}
-		}
-	}
-	return false;
-}
-
-static bool MatchDestIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
-				 struct bcm_ipv6_hdr *pstIpv6Header)
-{
-	UINT uiLoopIndex = 0;
-	UINT uiIpv6AddIndex = 0;
-	UINT uiIpv6AddrNoLongWords = 4;
-	ULONG aulDestIP[4];
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	union u_ip_address *dest_addr = &pstClassifierRule->stDestIpAddress;
-
-	/*
-	 * 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 (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]);
-
-	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");
-		DumpIpv6Address(&dest_addr->ulIpv6Mask[uiLoopIndex]);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
-				"\n Destination Ipv6 Address In Classifier Rule :\n");
-		DumpIpv6Address(&dest_addr->ulIpv6Addr[uiLoopIndex]);
-
-		for (uiIpv6AddIndex = 0;
-		     uiIpv6AddIndex < uiIpv6AddrNoLongWords;
-		     uiIpv6AddIndex++) {
-			if ((dest_addr->ulIpv6Mask[uiLoopIndex+uiIpv6AddIndex] &
-				aulDestIP[uiIpv6AddIndex]) !=
-			    dest_addr->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");
-				return TRUE;
-			}
-		}
-	}
-	return false;
-
-}
-
-VOID DumpIpv6Address(ULONG *puIpv6Address)
-{
-	UINT uiIpv6AddrNoLongWords = 4;
-	UINT uiIpv6AddIndex = 0;
-	struct bcm_mini_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]);
-	}
-
-}
-
-static VOID DumpIpv6Header(struct bcm_ipv6_hdr *pstIpv6Header)
-{
-	UCHAR ucVersion;
-	UCHAR ucPrio;
-	struct bcm_mini_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);
-	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");
-	DumpIpv6Address(pstIpv6Header->ulSrcIpAddress);
-	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---");
-
-
-}
diff --git a/drivers/staging/bcm/IPv6ProtocolHdr.h b/drivers/staging/bcm/IPv6ProtocolHdr.h
deleted file mode 100644
index 96b36a579af2..000000000000
--- a/drivers/staging/bcm/IPv6ProtocolHdr.h
+++ /dev/null
@@ -1,85 +0,0 @@
-#ifndef _IPV6_PROTOCOL_DEFINES_
-#define _IPV6_PROTOCOL_DEFINES_
-
-#define IPV6HDR_TYPE_HOPBYHOP 0x0
-#define IPV6HDR_TYPE_ROUTING 0x2B
-#define IPV6HDR_TYPE_FRAGMENTATION 0x2C
-#define IPV6HDR_TYPE_DESTOPTS 0x3c
-#define IPV6HDR_TYPE_AUTHENTICATION 0x33
-#define IPV6HDR_TYPE_ENCRYPTEDSECURITYPAYLOAD 0x34
-#define MASK_IPV6_CS_SPEC 0x2
-
-#define TCP_HEADER_TYPE	0x6
-#define UDP_HEADER_TYPE	0x11
-#define IPV6_ICMP_HDR_TYPE 0x2
-#define IPV6_FLOWLABEL_BITOFFSET 9
-
-#define IPV6_MAX_CHAINEDHDR_BUFFBYTES 0x64
-/*
- * Size of Dest Options field of Destinations Options Header
- * in bytes.
- */
-#define IPV6_DESTOPTS_HDR_OPTIONSIZE 0x8
-
-struct bcm_ipv6_hdr {
-	unsigned char  ucVersionPrio;
-	unsigned char  aucFlowLabel[3];
-	unsigned short usPayloadLength;
-	unsigned char  ucNextHeader;
-	unsigned char  ucHopLimit;
-	unsigned long  ulSrcIpAddress[4];
-	unsigned long  ulDestIpAddress[4];
-};
-
-struct bcm_ipv6_routing_hdr {
-	unsigned char ucNextHeader;
-	unsigned char ucRoutingType;
-	unsigned char ucNumAddresses;
-	unsigned char ucNextAddress;
-	unsigned long ulReserved;
-};
-
-struct bcm_ipv6_fragment_hdr {
-	unsigned char  ucNextHeader;
-	unsigned char  ucReserved;
-	unsigned short usFragmentOffset;
-	unsigned long  ulIdentification;
-};
-
-struct bcm_ipv6_dest_options_hdr {
-	unsigned char ucNextHeader;
-	unsigned char ucHdrExtLen;
-	unsigned char ucDestOptions[6];
-};
-
-struct bcm_ipv6_options_hdr {
-	unsigned char ucNextHeader;
-	unsigned char ucMisc[3];
-	unsigned long ulJumboPayloadLen;
-};
-
-struct bcm_ipv6_authentication_hdr {
-	unsigned char  ucNextHeader;
-	unsigned char  ucLength;
-	unsigned short usReserved;
-	unsigned long  ulSecurityParametersIndex;
-};
-
-enum bcm_ipaddr_context {
-	eSrcIpAddress,
-	eDestIpAddress
-};
-
-/* Function Prototypes */
-
-unsigned short IpVersion6(struct bcm_mini_adapter *Adapter, /* < Pointer to the driver control structure */
-					void *pcIpHeader, /* <Pointer to the IP Hdr of the packet */
-					struct bcm_classifier_rule *pstClassifierRule);
-
-void DumpIpv6Address(unsigned long *puIpv6Address);
-
-extern bool MatchSrcPort(struct bcm_classifier_rule *pstClassifierRule, unsigned short ushSrcPort);
-extern bool MatchDestPort(struct bcm_classifier_rule *pstClassifierRule, unsigned short ushSrcPort);
-extern bool MatchProtocol(struct bcm_classifier_rule *pstClassifierRule, unsigned char ucProtocol);
-
-#endif
diff --git a/drivers/staging/bcm/InterfaceAdapter.h b/drivers/staging/bcm/InterfaceAdapter.h
deleted file mode 100644
index 06a6b18bca48..000000000000
--- a/drivers/staging/bcm/InterfaceAdapter.h
+++ /dev/null
@@ -1,79 +0,0 @@
-#ifndef _INTERFACE_ADAPTER_H
-#define _INTERFACE_ADAPTER_H
-
-struct bcm_bulk_endpoint_in {
-	char	*bulk_in_buffer;
-	size_t	bulk_in_size;
-	unsigned char	bulk_in_endpointAddr;
-	unsigned int	bulk_in_pipe;
-};
-
-struct bcm_bulk_endpoint_out {
-	unsigned char	bulk_out_buffer;
-	size_t	bulk_out_size;
-	unsigned char	bulk_out_endpointAddr;
-	unsigned int	bulk_out_pipe;
-	/* this is used when int out endpoint is used as bulk out end point */
-	unsigned char	int_out_interval;
-};
-
-struct bcm_intr_endpoint_in {
-	char	*int_in_buffer;
-	size_t	int_in_size;
-	unsigned char	int_in_endpointAddr;
-	unsigned char	int_in_interval;
-	unsigned int	int_in_pipe;
-};
-
-struct bcm_intr_endpoint_out {
-	char	*int_out_buffer;
-	size_t	int_out_size;
-	unsigned char	int_out_endpointAddr;
-	unsigned char	int_out_interval;
-	unsigned int	int_out_pipe;
-};
-
-struct bcm_usb_tcb {
-	struct urb *urb;
-	void *psIntfAdapter;
-	bool bUsed;
-};
-
-struct bcm_usb_rcb {
-	struct urb *urb;
-	void *psIntfAdapter;
-	bool bUsed;
-};
-
-/*
- * This is the interface specific Sub-Adapter
- * Structure.
- */
-struct bcm_interface_adapter {
-	struct usb_device *udev;
-	struct usb_interface *interface;
-	/* Bulk endpoint in info */
-	struct bcm_bulk_endpoint_in	sBulkIn;
-	/* Bulk endpoint out info */
-	struct bcm_bulk_endpoint_out	sBulkOut;
-	/* Interrupt endpoint in info */
-	struct bcm_intr_endpoint_in	sIntrIn;
-	/* Interrupt endpoint out info */
-	struct bcm_intr_endpoint_out	sIntrOut;
-	unsigned long		ulInterruptData[2];
-	struct urb *psInterruptUrb;
-	struct bcm_usb_tcb	asUsbTcb[MAXIMUM_USB_TCB];
-	struct bcm_usb_rcb	asUsbRcb[MAXIMUM_USB_RCB];
-	atomic_t	uNumTcbUsed;
-	atomic_t	uCurrTcb;
-	atomic_t	uNumRcbUsed;
-	atomic_t	uCurrRcb;
-	struct bcm_mini_adapter *psAdapter;
-	bool		bFlashBoot;
-	bool		bHighSpeedDevice;
-	bool		bSuspended;
-	bool		bPreparingForBusSuspend;
-	struct work_struct usbSuspendWork;
-};
-
-#endif
diff --git a/drivers/staging/bcm/InterfaceDld.c b/drivers/staging/bcm/InterfaceDld.c
deleted file mode 100644
index abc7a7ab782a..000000000000
--- a/drivers/staging/bcm/InterfaceDld.c
+++ /dev/null
@@ -1,317 +0,0 @@
-#include "headers.h"
-
-int InterfaceFileDownload(PVOID arg, struct file *flp, unsigned int on_chip_loc)
-{
-	/* unsigned int reg = 0; */
-	mm_segment_t oldfs = {0};
-	int errno = 0, len = 0; /* ,is_config_file = 0 */
-	loff_t pos = 0;
-	struct bcm_interface_adapter *psIntfAdapter = arg;
-	/* struct bcm_mini_adapter *Adapter = psIntfAdapter->psAdapter; */
-	char *buff = kmalloc(MAX_TRANSFER_CTRL_BYTE_USB, GFP_KERNEL);
-
-	if (!buff)
-		return -ENOMEM;
-
-	while (1) {
-		oldfs = get_fs();
-		set_fs(get_ds());
-		len = vfs_read(flp, (void __force __user *)buff,
-			MAX_TRANSFER_CTRL_BYTE_USB, &pos);
-		set_fs(oldfs);
-		if (len <= 0) {
-			if (len < 0)
-				errno = len;
-			else
-				errno = 0;
-			break;
-		}
-		/* BCM_DEBUG_PRINT_BUFFER(Adapter,DBG_TYPE_INITEXIT, MP_INIT,
-		 *			  DBG_LVL_ALL, buff,
-		 *			  MAX_TRANSFER_CTRL_BYTE_USB);
-		 */
-		errno = InterfaceWRM(psIntfAdapter, on_chip_loc, buff, len);
-		if (errno)
-			break;
-		on_chip_loc += MAX_TRANSFER_CTRL_BYTE_USB;
-	}
-
-	kfree(buff);
-	return errno;
-}
-
-int InterfaceFileReadbackFromChip(PVOID arg, struct file *flp,
-				unsigned int on_chip_loc)
-{
-	char *buff, *buff_readback;
-	unsigned int reg = 0;
-	mm_segment_t oldfs = {0};
-	int errno = 0, len = 0, is_config_file = 0;
-	loff_t pos = 0;
-	static int fw_down;
-	INT Status = STATUS_SUCCESS;
-	struct bcm_interface_adapter *psIntfAdapter = arg;
-	int bytes;
-
-	buff = kzalloc(MAX_TRANSFER_CTRL_BYTE_USB, GFP_DMA);
-	buff_readback = kzalloc(MAX_TRANSFER_CTRL_BYTE_USB , GFP_DMA);
-	if (!buff || !buff_readback) {
-		kfree(buff);
-		kfree(buff_readback);
-
-		return -ENOMEM;
-	}
-
-	is_config_file = (on_chip_loc == CONFIG_BEGIN_ADDR) ? 1 : 0;
-
-	while (1) {
-		oldfs = get_fs();
-		set_fs(get_ds());
-		len = vfs_read(flp, (void __force __user *)buff,
-				MAX_TRANSFER_CTRL_BYTE_USB, &pos);
-		set_fs(oldfs);
-		fw_down++;
-
-		if (len <= 0) {
-			if (len < 0)
-				errno = len;
-			else
-				errno = 0;
-			break;
-		}
-
-		bytes = InterfaceRDM(psIntfAdapter, on_chip_loc,
-					buff_readback, len);
-		if (bytes < 0) {
-			Status = bytes;
-			goto exit;
-		}
-		reg++;
-		if ((len-sizeof(unsigned int)) < 4) {
-			if (memcmp(buff_readback, buff, len)) {
-				Status = -EIO;
-				goto exit;
-			}
-		} else {
-			len -= 4;
-
-			while (len) {
-				if (*(unsigned int *)&buff_readback[len] !=
-						 *(unsigned int *)&buff[len]) {
-					Status = -EIO;
-					goto exit;
-				}
-				len -= 4;
-			}
-		}
-		on_chip_loc += MAX_TRANSFER_CTRL_BYTE_USB;
-	} /* End of while(1) */
-
-exit:
-	kfree(buff);
-	kfree(buff_readback);
-	return Status;
-}
-
-static int bcm_download_config_file(struct bcm_mini_adapter *Adapter,
-				struct bcm_firmware_info *psFwInfo)
-{
-	int retval = STATUS_SUCCESS;
-	B_UINT32 value = 0;
-
-	if (Adapter->pstargetparams == NULL) {
-		Adapter->pstargetparams =
-			kmalloc(sizeof(struct bcm_target_params), GFP_KERNEL);
-		if (Adapter->pstargetparams == NULL)
-			return -ENOMEM;
-	}
-
-	if (psFwInfo->u32FirmwareLength != sizeof(struct bcm_target_params))
-		return -EIO;
-
-	retval = copy_from_user(Adapter->pstargetparams,
-			psFwInfo->pvMappedFirmwareAddress,
-			psFwInfo->u32FirmwareLength);
-	if (retval) {
-		kfree(Adapter->pstargetparams);
-		Adapter->pstargetparams = NULL;
-		return -EFAULT;
-	}
-
-	/* Parse the structure and then Download the Firmware */
-	beceem_parse_target_struct(Adapter);
-
-	/* Initializing the NVM. */
-	BcmInitNVM(Adapter);
-	retval = InitLedSettings(Adapter);
-
-	if (retval)
-		return retval;
-
-	if (Adapter->LEDInfo.led_thread_running &
-			BCM_LED_THREAD_RUNNING_ACTIVELY) {
-		Adapter->LEDInfo.bLedInitDone = false;
-		Adapter->DriverState = DRIVER_INIT;
-		wake_up(&Adapter->LEDInfo.notify_led_event);
-	}
-
-	if (Adapter->LEDInfo.led_thread_running &
-			BCM_LED_THREAD_RUNNING_ACTIVELY) {
-		Adapter->DriverState = FW_DOWNLOAD;
-		wake_up(&Adapter->LEDInfo.notify_led_event);
-	}
-
-	/* Initialize the DDR Controller */
-	retval = ddr_init(Adapter);
-	if (retval)
-		return retval;
-
-	value = 0;
-	wrmalt(Adapter, EEPROM_CAL_DATA_INTERNAL_LOC - 4,
-				&value, sizeof(value));
-	wrmalt(Adapter, EEPROM_CAL_DATA_INTERNAL_LOC - 8,
-				&value, sizeof(value));
-
-	if (Adapter->eNVMType == NVM_FLASH) {
-		retval = PropagateCalParamsFromFlashToMemory(Adapter);
-		if (retval)
-			return retval;
-	}
-
-	retval = buffDnldVerify(Adapter, (PUCHAR)Adapter->pstargetparams,
-			sizeof(struct bcm_target_params), CONFIG_BEGIN_ADDR);
-
-	if (retval)
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT,
-				MP_INIT, DBG_LVL_ALL,
-				"configuration file not downloaded properly");
-	else
-		Adapter->bCfgDownloaded = TRUE;
-
-	return retval;
-}
-
-int bcm_ioctl_fw_download(struct bcm_mini_adapter *Adapter,
-			struct bcm_firmware_info *psFwInfo)
-{
-	int retval = STATUS_SUCCESS;
-	PUCHAR buff = NULL;
-
-	/* Config File is needed for the Driver to download the Config file and
-	 * Firmware. Check for the Config file to be first to be sent from the
-	 * Application
-	 */
-	atomic_set(&Adapter->uiMBupdate, false);
-	if (!Adapter->bCfgDownloaded &&
-		psFwInfo->u32StartingAddress != CONFIG_BEGIN_ADDR) {
-		/* Can't Download Firmware. */
-		return -EINVAL;
-	}
-
-	/* If Config File, Finish the DDR Settings and then Download CFG File */
-	if (psFwInfo->u32StartingAddress == CONFIG_BEGIN_ADDR) {
-		retval = bcm_download_config_file(Adapter, psFwInfo);
-	} else {
-		buff = kzalloc(psFwInfo->u32FirmwareLength, GFP_KERNEL);
-		if (buff == NULL)
-			return -ENOMEM;
-
-		retval = copy_from_user(buff,
-			psFwInfo->pvMappedFirmwareAddress,
-			psFwInfo->u32FirmwareLength);
-		if (retval != STATUS_SUCCESS) {
-			retval = -EFAULT;
-			goto error;
-		}
-
-		retval = buffDnldVerify(Adapter,
-					buff,
-					psFwInfo->u32FirmwareLength,
-					psFwInfo->u32StartingAddress);
-
-		if (retval != STATUS_SUCCESS)
-			goto error;
-	}
-
-error:
-	kfree(buff);
-	return retval;
-}
-
-static INT buffDnld(struct bcm_mini_adapter *Adapter,
-			PUCHAR mappedbuffer, UINT u32FirmwareLength,
-			ULONG u32StartingAddress)
-{
-	unsigned int len = 0;
-	int retval = STATUS_SUCCESS;
-
-	len = u32FirmwareLength;
-
-	while (u32FirmwareLength) {
-		len = MIN_VAL(u32FirmwareLength, MAX_TRANSFER_CTRL_BYTE_USB);
-		retval = wrm(Adapter, u32StartingAddress, mappedbuffer, len);
-
-		if (retval)
-			break;
-		u32StartingAddress += len;
-		u32FirmwareLength -= len;
-		mappedbuffer += len;
-	}
-	return retval;
-}
-
-static INT buffRdbkVerify(struct bcm_mini_adapter *Adapter,
-			PUCHAR mappedbuffer, UINT u32FirmwareLength,
-			ULONG u32StartingAddress)
-{
-	UINT len = u32FirmwareLength;
-	INT retval = STATUS_SUCCESS;
-	PUCHAR readbackbuff = kzalloc(MAX_TRANSFER_CTRL_BYTE_USB, GFP_KERNEL);
-	int bytes;
-
-	if (NULL == readbackbuff)
-		return -ENOMEM;
-
-	while (u32FirmwareLength && !retval) {
-		len = MIN_VAL(u32FirmwareLength, MAX_TRANSFER_CTRL_BYTE_USB);
-		bytes = rdm(Adapter, u32StartingAddress, readbackbuff, len);
-
-		if (bytes < 0) {
-			retval = bytes;
-			break;
-		}
-
-		if (memcmp(readbackbuff, mappedbuffer, len) != 0) {
-			pr_err("%s() failed.  The firmware doesn't match what was written",
-			       __func__);
-			retval = -EIO;
-		}
-
-		u32StartingAddress += len;
-		u32FirmwareLength -= len;
-		mappedbuffer += len;
-
-	} /* end of while (u32FirmwareLength && !retval) */
-	kfree(readbackbuff);
-	return retval;
-}
-
-INT buffDnldVerify(struct bcm_mini_adapter *Adapter,
-			unsigned char *mappedbuffer,
-			unsigned int u32FirmwareLength,
-			unsigned long u32StartingAddress)
-{
-	INT status = STATUS_SUCCESS;
-
-	status = buffDnld(Adapter, mappedbuffer,
-			u32FirmwareLength, u32StartingAddress);
-	if (status != STATUS_SUCCESS)
-		goto error;
-
-	status = buffRdbkVerify(Adapter, mappedbuffer,
-			u32FirmwareLength, u32StartingAddress);
-	if (status != STATUS_SUCCESS)
-		goto error;
-error:
-	return status;
-}
diff --git a/drivers/staging/bcm/InterfaceIdleMode.c b/drivers/staging/bcm/InterfaceIdleMode.c
deleted file mode 100644
index 612c89fba341..000000000000
--- a/drivers/staging/bcm/InterfaceIdleMode.c
+++ /dev/null
@@ -1,274 +0,0 @@
-#include "headers.h"
-
-/*
-Function:	InterfaceIdleModeWakeup
-
-Description:	This is the hardware specific Function for
-		waking up HW device from Idle mode.
-		A software abort pattern is written to the
-		device to wake it and necessary power state
-		transitions from host are performed here.
-
-Input parameters: IN struct bcm_mini_adapter *Adapter
-		  - Miniport Adapter Context
-
-Return:		BCM_STATUS_SUCCESS - If Wakeup of the HW Interface
-				     was successful.
-		Other              - If an error occurred.
-*/
-
-/*
-Function:	InterfaceIdleModeRespond
-
-Description:	This is the hardware specific Function for
-		responding to Idle mode request from target.
-		Necessary power state transitions from host for
-		idle mode or other device specific initializations
-		are performed here.
-
-Input parameters: IN struct bcm_mini_adapter * Adapter
-		  - Miniport Adapter Context
-
-Return:		BCM_STATUS_SUCCESS - If Idle mode response related
-				     HW configuration was successful.
-		Other              - If an error occurred.
-*/
-
-/*
-"dmem bfc02f00  100" tells how many time device went in Idle mode.
-this value will be at address bfc02fa4.just before value d0ea1dle.
-
-Set time value by writing at bfc02f98 7d0
-
-checking the Ack timer expire on kannon by running command
-d qcslog .. if it shows e means host has not send response
-to f/w with in 200 ms. Response should be
-send to f/w with in 200 ms after the Idle/Shutdown req issued
-
-*/
-
-
-int InterfaceIdleModeRespond(struct bcm_mini_adapter *Adapter,
-			unsigned int *puiBuffer)
-{
-	int	status = STATUS_SUCCESS;
-	unsigned int	uiRegRead = 0;
-	int bytes;
-
-	if (ntohl(*puiBuffer) == GO_TO_IDLE_MODE_PAYLOAD) {
-		if (ntohl(*(puiBuffer+1)) == 0) {
-
-			status = wrmalt(Adapter, SW_ABORT_IDLEMODE_LOC,
-					&uiRegRead, sizeof(uiRegRead));
-			if (status)
-				return status;
-
-			if (Adapter->ulPowerSaveMode ==
-				DEVICE_POWERSAVE_MODE_AS_MANUAL_CLOCK_GATING) {
-				uiRegRead = 0x00000000;
-				status = wrmalt(Adapter,
-					DEBUG_INTERRUPT_GENERATOR_REGISTOR,
-					&uiRegRead, sizeof(uiRegRead));
-				if (status)
-					return status;
-			}
-			/* Below Register should not br read in case of
-			 * Manual and Protocol Idle mode */
-			else if (Adapter->ulPowerSaveMode !=
-				DEVICE_POWERSAVE_MODE_AS_PROTOCOL_IDLE_MODE) {
-				/* clear on read Register */
-				bytes = rdmalt(Adapter, DEVICE_INT_OUT_EP_REG0,
-					&uiRegRead, sizeof(uiRegRead));
-				if (bytes < 0) {
-					status = bytes;
-					return status;
-				}
-				/* clear on read Register */
-				bytes = rdmalt(Adapter, DEVICE_INT_OUT_EP_REG1,
-					&uiRegRead, sizeof(uiRegRead));
-				if (bytes < 0) {
-					status = bytes;
-					return status;
-				}
-			}
-
-			/* Set Idle Mode Flag to False and
-			 * Clear IdleMode reg. */
-			Adapter->IdleMode = false;
-			Adapter->bTriedToWakeUpFromlowPowerMode = false;
-
-			wake_up(&Adapter->lowpower_mode_wait_queue);
-
-		} else {
-			if (TRUE == Adapter->IdleMode)
-				return status;
-
-			uiRegRead = 0;
-
-			if (Adapter->chip_id == BCS220_2 ||
-				Adapter->chip_id == BCS220_2BC ||
-					Adapter->chip_id == BCS250_BC ||
-					Adapter->chip_id == BCS220_3) {
-
-				bytes = rdmalt(Adapter, HPM_CONFIG_MSW,
-					&uiRegRead, sizeof(uiRegRead));
-				if (bytes < 0) {
-					status = bytes;
-					return status;
-				}
-
-
-				uiRegRead |= (1<<17);
-
-				status = wrmalt(Adapter, HPM_CONFIG_MSW,
-					&uiRegRead, sizeof(uiRegRead));
-				if (status)
-					return status;
-			}
-			SendIdleModeResponse(Adapter);
-		}
-	} else if (ntohl(*puiBuffer) == IDLE_MODE_SF_UPDATE_MSG) {
-		OverrideServiceFlowParams(Adapter, puiBuffer);
-	}
-	return status;
-}
-
-static int InterfaceAbortIdlemode(struct bcm_mini_adapter *Adapter,
-				unsigned int Pattern)
-{
-	int status = STATUS_SUCCESS;
-	unsigned int value;
-	unsigned int chip_id;
-	unsigned long timeout = 0, itr = 0;
-
-	int lenwritten = 0;
-	unsigned char aucAbortPattern[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
-						0xFF, 0xFF, 0xFF};
-	struct bcm_interface_adapter *psInterfaceAdapter =
-				Adapter->pvInterfaceAdapter;
-
-	/* Abort Bus suspend if its already suspended */
-	if ((TRUE == psInterfaceAdapter->bSuspended) &&
-			(TRUE == Adapter->bDoSuspend))
-		status = usb_autopm_get_interface(
-				psInterfaceAdapter->interface);
-
-	if ((Adapter->ulPowerSaveMode ==
-			DEVICE_POWERSAVE_MODE_AS_MANUAL_CLOCK_GATING) ||
-	   (Adapter->ulPowerSaveMode ==
-			DEVICE_POWERSAVE_MODE_AS_PROTOCOL_IDLE_MODE)) {
-		/* write the SW abort pattern. */
-		status = wrmalt(Adapter, SW_ABORT_IDLEMODE_LOC,
-				&Pattern, sizeof(Pattern));
-		if (status)
-			return status;
-	}
-
-	if (Adapter->ulPowerSaveMode ==
-		DEVICE_POWERSAVE_MODE_AS_MANUAL_CLOCK_GATING) {
-		value = 0x80000000;
-		status = wrmalt(Adapter,
-				DEBUG_INTERRUPT_GENERATOR_REGISTOR,
-				&value, sizeof(value));
-		if (status)
-			return status;
-	} else if (Adapter->ulPowerSaveMode !=
-			DEVICE_POWERSAVE_MODE_AS_PROTOCOL_IDLE_MODE) {
-		/*
-		 * Get a Interrupt Out URB and send 8 Bytes Down
-		 * To be Done in Thread Context.
-		 * Not using Asynchronous Mechanism.
-		 */
-		status = usb_interrupt_msg(psInterfaceAdapter->udev,
-			usb_sndintpipe(psInterfaceAdapter->udev,
-			psInterfaceAdapter->sIntrOut.int_out_endpointAddr),
-			aucAbortPattern,
-			8,
-			&lenwritten,
-			5000);
-		if (status)
-			return status;
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
-				IDLE_MODE, DBG_LVL_ALL,
-				"NOB Sent down :%d", lenwritten);
-
-		/* mdelay(25); */
-
-		timeout = jiffies +  msecs_to_jiffies(50);
-		while (time_after(timeout, jiffies)) {
-			itr++;
-			rdmalt(Adapter, CHIP_ID_REG, &chip_id, sizeof(UINT));
-			if (0xbece3200 == (chip_id&~(0xF0)))
-				chip_id = chip_id&~(0xF0);
-			if (chip_id == Adapter->chip_id)
-				break;
-		}
-		if (time_before(timeout, jiffies))
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
-				IDLE_MODE, DBG_LVL_ALL,
-				"Not able to read chip-id even after 25 msec");
-		else
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
-				IDLE_MODE, DBG_LVL_ALL,
-				"Number of completed iteration to read chip-id :%lu", itr);
-
-		status = wrmalt(Adapter, SW_ABORT_IDLEMODE_LOC,
-				&Pattern, sizeof(status));
-		if (status)
-			return status;
-	}
-	return status;
-}
-int InterfaceIdleModeWakeup(struct bcm_mini_adapter *Adapter)
-{
-	if (Adapter->bTriedToWakeUpFromlowPowerMode) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
-		IDLE_MODE, DBG_LVL_ALL,
-		"Wake up already attempted.. ignoring\n");
-	} else {
-		Adapter->bTriedToWakeUpFromlowPowerMode = TRUE;
-		InterfaceAbortIdlemode(Adapter, Adapter->usIdleModePattern);
-
-	}
-	return 0;
-}
-
-void InterfaceHandleShutdownModeWakeup(struct bcm_mini_adapter *Adapter)
-{
-	unsigned int uiRegVal = 0;
-	INT Status = 0;
-	int bytes;
-
-	if (Adapter->ulPowerSaveMode ==
-		DEVICE_POWERSAVE_MODE_AS_MANUAL_CLOCK_GATING) {
-		/* clear idlemode interrupt. */
-		uiRegVal = 0;
-		Status = wrmalt(Adapter,
-			DEBUG_INTERRUPT_GENERATOR_REGISTOR,
-			&uiRegVal, sizeof(uiRegVal));
-		if (Status)
-			return;
-	}
-
-	else {
-
-/* clear Interrupt EP registers. */
-		bytes = rdmalt(Adapter,
-			DEVICE_INT_OUT_EP_REG0,
-			&uiRegVal, sizeof(uiRegVal));
-		if (bytes < 0) {
-			Status = bytes;
-			return;
-		}
-
-		bytes = rdmalt(Adapter,
-			DEVICE_INT_OUT_EP_REG1,
-			&uiRegVal, sizeof(uiRegVal));
-		if (bytes < 0) {
-			Status = bytes;
-			return;
-		}
-	}
-}
-
diff --git a/drivers/staging/bcm/InterfaceIdleMode.h b/drivers/staging/bcm/InterfaceIdleMode.h
deleted file mode 100644
index 2ef64003aa89..000000000000
--- a/drivers/staging/bcm/InterfaceIdleMode.h
+++ /dev/null
@@ -1,15 +0,0 @@
-#ifndef _INTERFACE_IDLEMODE_H
-#define _INTERFACE_IDLEMODE_H
-
-INT InterfaceIdleModeWakeup(struct bcm_mini_adapter *Adapter);
-
-INT InterfaceIdleModeRespond(struct bcm_mini_adapter *Adapter,
-				unsigned int *puiBuffer);
-
-VOID InterfaceWriteIdleModeWakePattern(struct bcm_mini_adapter *Adapter);
-
-INT InterfaceWakeUp(struct bcm_mini_adapter *Adapter);
-
-VOID InterfaceHandleShutdownModeWakeup(struct bcm_mini_adapter *Adapter);
-#endif
-
diff --git a/drivers/staging/bcm/InterfaceInit.c b/drivers/staging/bcm/InterfaceInit.c
deleted file mode 100644
index bb61d34886b3..000000000000
--- a/drivers/staging/bcm/InterfaceInit.c
+++ /dev/null
@@ -1,729 +0,0 @@
-#include "headers.h"
-#include <linux/usb/ch9.h>
-static struct usb_device_id InterfaceUsbtable[] = {
-	{ USB_DEVICE(BCM_USB_VENDOR_ID_T3, BCM_USB_PRODUCT_ID_T3) },
-	{ USB_DEVICE(BCM_USB_VENDOR_ID_T3, BCM_USB_PRODUCT_ID_T3B) },
-	{ USB_DEVICE(BCM_USB_VENDOR_ID_T3, BCM_USB_PRODUCT_ID_T3L) },
-	{ USB_DEVICE(BCM_USB_VENDOR_ID_T3, BCM_USB_PRODUCT_ID_SYM) },
-	{ USB_DEVICE(BCM_USB_VENDOR_ID_ZTE, BCM_USB_PRODUCT_ID_226) },
-	{ USB_DEVICE(BCM_USB_VENDOR_ID_FOXCONN, BCM_USB_PRODUCT_ID_1901) },
-	{ USB_DEVICE(BCM_USB_VENDOR_ID_ZTE, BCM_USB_PRODUCT_ID_ZTE_TU25) },
-	{ USB_DEVICE(BCM_USB_VENDOR_ID_ZTE, BCM_USB_PRODUCT_ID_ZTE_226) },
-	{ USB_DEVICE(BCM_USB_VENDOR_ID_ZTE, BCM_USB_PRODUCT_ID_ZTE_326) },
-	{ }
-};
-MODULE_DEVICE_TABLE(usb, InterfaceUsbtable);
-
-static int debug = -1;
-module_param(debug, uint, 0600);
-MODULE_PARM_DESC(debug, "Debug level (0=none,...,16=all)");
-
-static const u32 default_msg =
-	NETIF_MSG_DRV | NETIF_MSG_PROBE | NETIF_MSG_LINK
-	| NETIF_MSG_TIMER | NETIF_MSG_TX_ERR | NETIF_MSG_RX_ERR
-	| NETIF_MSG_IFUP | NETIF_MSG_IFDOWN;
-
-static int InterfaceAdapterInit(struct bcm_interface_adapter *Adapter);
-
-static void InterfaceAdapterFree(struct bcm_interface_adapter *psIntfAdapter)
-{
-	int i = 0;
-	struct bcm_mini_adapter *ps_ad = psIntfAdapter->psAdapter;
-
-	/* Wake up the wait_queue... */
-	if (ps_ad->LEDInfo.led_thread_running &
-			BCM_LED_THREAD_RUNNING_ACTIVELY) {
-		ps_ad->DriverState = DRIVER_HALT;
-		wake_up(&ps_ad->LEDInfo.notify_led_event);
-	}
-	reset_card_proc(ps_ad);
-
-	/*
-	 * worst case time taken by the RDM/WRM will be 5 sec. will check after
-	 * every 100 ms to accertain the device is not being accessed. After
-	 * this No RDM/WRM should be made.
-	 */
-	while (ps_ad->DeviceAccess) {
-		BCM_DEBUG_PRINT(ps_ad, DBG_TYPE_INITEXIT, DRV_ENTRY,
-				DBG_LVL_ALL, "Device is being accessed.\n");
-		msleep(100);
-	}
-	/* Free interrupt URB */
-	/* ps_ad->device_removed = TRUE; */
-	usb_free_urb(psIntfAdapter->psInterruptUrb);
-
-	/* Free transmit URBs */
-	for (i = 0; i < MAXIMUM_USB_TCB; i++) {
-		if (psIntfAdapter->asUsbTcb[i].urb  != NULL) {
-			usb_free_urb(psIntfAdapter->asUsbTcb[i].urb);
-			psIntfAdapter->asUsbTcb[i].urb = NULL;
-		}
-	}
-	/* Free receive URB and buffers */
-	for (i = 0; i < MAXIMUM_USB_RCB; i++) {
-		if (psIntfAdapter->asUsbRcb[i].urb != NULL) {
-			kfree(psIntfAdapter->asUsbRcb[i].urb->transfer_buffer);
-			usb_free_urb(psIntfAdapter->asUsbRcb[i].urb);
-			psIntfAdapter->asUsbRcb[i].urb = NULL;
-		}
-	}
-	AdapterFree(ps_ad);
-}
-
-static void ConfigureEndPointTypesThroughEEPROM(
-		struct bcm_mini_adapter *Adapter)
-{
-	u32 ulReg;
-	int bytes;
-	struct bcm_interface_adapter *interfaceAdapter;
-
-	/* Program EP2 MAX_PKT_SIZE */
-	ulReg = ntohl(EP2_MPS_REG);
-	BeceemEEPROMBulkWrite(Adapter, (PUCHAR)&ulReg, 0x128, 4, TRUE);
-	ulReg = ntohl(EP2_MPS);
-	BeceemEEPROMBulkWrite(Adapter, (PUCHAR)&ulReg, 0x12C, 4, TRUE);
-
-	ulReg = ntohl(EP2_CFG_REG);
-	BeceemEEPROMBulkWrite(Adapter, (PUCHAR)&ulReg, 0x132, 4, TRUE);
-	interfaceAdapter =
-		(struct bcm_interface_adapter *)(Adapter->pvInterfaceAdapter);
-	if (interfaceAdapter->bHighSpeedDevice) {
-		ulReg = ntohl(EP2_CFG_INT);
-		BeceemEEPROMBulkWrite(Adapter, (PUCHAR)&ulReg, 0x136, 4, TRUE);
-	} else {
-		/* USE BULK EP as TX in FS mode. */
-		ulReg = ntohl(EP2_CFG_BULK);
-		BeceemEEPROMBulkWrite(Adapter, (PUCHAR)&ulReg, 0x136, 4, TRUE);
-	}
-
-	/* Program EP4 MAX_PKT_SIZE. */
-	ulReg = ntohl(EP4_MPS_REG);
-	BeceemEEPROMBulkWrite(Adapter, (PUCHAR)&ulReg, 0x13C, 4, TRUE);
-	ulReg = ntohl(EP4_MPS);
-	BeceemEEPROMBulkWrite(Adapter, (PUCHAR)&ulReg, 0x140, 4, TRUE);
-
-	/* Program TX EP as interrupt(Alternate Setting) */
-	bytes = rdmalt(Adapter, 0x0F0110F8, &ulReg, sizeof(u32));
-	if (bytes < 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, DRV_ENTRY,
-				DBG_LVL_ALL, "reading of Tx EP failed\n");
-		return;
-	}
-	ulReg |= 0x6;
-
-	ulReg = ntohl(ulReg);
-	BeceemEEPROMBulkWrite(Adapter, (PUCHAR)&ulReg, 0x1CC, 4, TRUE);
-
-	ulReg = ntohl(EP4_CFG_REG);
-	BeceemEEPROMBulkWrite(Adapter, (PUCHAR)&ulReg, 0x1C8, 4, TRUE);
-	/* Program ISOCHRONOUS EP size to zero. */
-	ulReg = ntohl(ISO_MPS_REG);
-	BeceemEEPROMBulkWrite(Adapter, (PUCHAR)&ulReg, 0x1D2, 4, TRUE);
-	ulReg = ntohl(ISO_MPS);
-	BeceemEEPROMBulkWrite(Adapter, (PUCHAR)&ulReg, 0x1D6, 4, TRUE);
-
-	/*
-	 * Update EEPROM Version.
-	 * Read 4 bytes from 508 and modify 511 and 510.
-	 */
-	ReadBeceemEEPROM(Adapter, 0x1FC, &ulReg);
-	ulReg &= 0x0101FFFF;
-	BeceemEEPROMBulkWrite(Adapter, (PUCHAR)&ulReg, 0x1FC, 4, TRUE);
-
-	/*
-	 * Update length field if required.
-	 * Also make the string NULL terminated.
-	 */
-
-	ReadBeceemEEPROM(Adapter, 0xA8, &ulReg);
-	if ((ulReg&0x00FF0000)>>16 > 0x30) {
-		ulReg = (ulReg&0xFF00FFFF)|(0x30<<16);
-		BeceemEEPROMBulkWrite(Adapter, (PUCHAR)&ulReg, 0xA8, 4, TRUE);
-	}
-	ReadBeceemEEPROM(Adapter, 0x148, &ulReg);
-	if ((ulReg&0x00FF0000)>>16 > 0x30) {
-		ulReg = (ulReg&0xFF00FFFF)|(0x30<<16);
-		BeceemEEPROMBulkWrite(Adapter, (PUCHAR)&ulReg, 0x148, 4, TRUE);
-	}
-	ulReg = 0;
-	BeceemEEPROMBulkWrite(Adapter, (PUCHAR)&ulReg, 0x122, 4, TRUE);
-	ulReg = 0;
-	BeceemEEPROMBulkWrite(Adapter, (PUCHAR)&ulReg, 0x1C2, 4, TRUE);
-}
-
-static int usbbcm_device_probe(struct usb_interface *intf,
-			       const struct usb_device_id *id)
-{
-	struct usb_device *udev = interface_to_usbdev(intf);
-	int retval;
-	struct bcm_mini_adapter *psAdapter;
-	struct bcm_interface_adapter *psIntfAdapter;
-	struct net_device *ndev;
-
-	/* Reserve one extra queue for the bit-bucket */
-	ndev = alloc_etherdev_mq(sizeof(struct bcm_mini_adapter),
-			NO_OF_QUEUES + 1);
-	if (ndev == NULL) {
-		dev_err(&udev->dev, DRV_NAME ": no memory for device\n");
-		return -ENOMEM;
-	}
-
-	SET_NETDEV_DEV(ndev, &intf->dev);
-
-	psAdapter = netdev_priv(ndev);
-	psAdapter->dev = ndev;
-	psAdapter->msg_enable = netif_msg_init(debug, default_msg);
-
-	/* Init default driver debug state */
-
-	psAdapter->stDebugState.debug_level = DBG_LVL_CURR;
-	psAdapter->stDebugState.type = DBG_TYPE_INITEXIT;
-
-	/*
-	 * Technically, one can start using BCM_DEBUG_PRINT after this point.
-	 * However, realize that by default the Type/Subtype bitmaps are all
-	 * zero now; so no prints will actually appear until the TestApp turns
-	 * on debug paths via the ioctl(); so practically speaking, in early
-	 * init, no logging happens.
-	 *
-	 * A solution (used below): we explicitly set the bitmaps to 1 for
-	 * Type=DBG_TYPE_INITEXIT and ALL subtype's of the same. Now all bcm
-	 * debug statements get logged, enabling debug during early init.
-	 * Further, we turn this OFF once init_module() completes.
-	 */
-
-	psAdapter->stDebugState.subtype[DBG_TYPE_INITEXIT] = 0xff;
-	BCM_SHOW_DEBUG_BITMAP(psAdapter);
-
-	retval = InitAdapter(psAdapter);
-	if (retval) {
-		dev_err(&udev->dev, DRV_NAME ": InitAdapter Failed\n");
-		AdapterFree(psAdapter);
-		return retval;
-	}
-
-	/* Allocate interface adapter structure */
-	psIntfAdapter = kzalloc(sizeof(struct bcm_interface_adapter),
-			GFP_KERNEL);
-	if (psIntfAdapter == NULL) {
-		AdapterFree(psAdapter);
-		return -ENOMEM;
-	}
-
-	psAdapter->pvInterfaceAdapter = psIntfAdapter;
-	psIntfAdapter->psAdapter = psAdapter;
-
-	/* Store usb interface in Interface Adapter */
-	psIntfAdapter->interface = intf;
-	usb_set_intfdata(intf, psIntfAdapter);
-
-	BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_INITEXIT, DRV_ENTRY, DBG_LVL_ALL,
-			"psIntfAdapter 0x%p\n", psIntfAdapter);
-	retval = InterfaceAdapterInit(psIntfAdapter);
-	if (retval) {
-		/* If the Firmware/Cfg File is not present
-		 * then return success, let the application
-		 * download the files.
-		 */
-		if (-ENOENT == retval) {
-			BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_INITEXIT, DRV_ENTRY,
-					DBG_LVL_ALL,
-					"File Not Found.  Use app to download.\n");
-			return STATUS_SUCCESS;
-		}
-		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_INITEXIT, DRV_ENTRY,
-				DBG_LVL_ALL, "InterfaceAdapterInit failed.\n");
-		usb_set_intfdata(intf, NULL);
-		udev = interface_to_usbdev(intf);
-		usb_put_dev(udev);
-		InterfaceAdapterFree(psIntfAdapter);
-		return retval;
-	}
-	if (psAdapter->chip_id > T3) {
-		uint32_t uiNackZeroLengthInt = 4;
-
-		retval =
-			wrmalt(psAdapter, DISABLE_USB_ZERO_LEN_INT,
-					&uiNackZeroLengthInt,
-					sizeof(uiNackZeroLengthInt));
-		if (retval)
-			return retval;
-	}
-
-	/* Check whether the USB-Device Supports remote Wake-Up */
-	if (USB_CONFIG_ATT_WAKEUP & udev->actconfig->desc.bmAttributes) {
-		/* If Suspend then only support dynamic suspend */
-		if (psAdapter->bDoSuspend) {
-#ifdef CONFIG_PM
-			pm_runtime_set_autosuspend_delay(&udev->dev, 0);
-			intf->needs_remote_wakeup = 1;
-			usb_enable_autosuspend(udev);
-			device_init_wakeup(&intf->dev, 1);
-			INIT_WORK(&psIntfAdapter->usbSuspendWork,
-					putUsbSuspend);
-			BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_INITEXIT, DRV_ENTRY,
-					DBG_LVL_ALL,
-					"Enabling USB Auto-Suspend\n");
-#endif
-		} else {
-			intf->needs_remote_wakeup = 0;
-			usb_disable_autosuspend(udev);
-		}
-	}
-
-	psAdapter->stDebugState.subtype[DBG_TYPE_INITEXIT] = 0x0;
-	return retval;
-}
-
-static void usbbcm_disconnect(struct usb_interface *intf)
-{
-	struct bcm_interface_adapter *psIntfAdapter = usb_get_intfdata(intf);
-	struct bcm_mini_adapter *psAdapter;
-	struct usb_device  *udev = interface_to_usbdev(intf);
-
-	if (psIntfAdapter == NULL)
-		return;
-
-	psAdapter = psIntfAdapter->psAdapter;
-	netif_device_detach(psAdapter->dev);
-
-	if (psAdapter->bDoSuspend)
-		intf->needs_remote_wakeup = 0;
-
-	psAdapter->device_removed = TRUE;
-	usb_set_intfdata(intf, NULL);
-	InterfaceAdapterFree(psIntfAdapter);
-	usb_put_dev(udev);
-}
-
-static int AllocUsbCb(struct bcm_interface_adapter *psIntfAdapter)
-{
-	int i = 0;
-
-	for (i = 0; i < MAXIMUM_USB_TCB; i++) {
-		psIntfAdapter->asUsbTcb[i].urb = usb_alloc_urb(0, GFP_KERNEL);
-
-		if (psIntfAdapter->asUsbTcb[i].urb == NULL) {
-			BCM_DEBUG_PRINT(psIntfAdapter->psAdapter,
-					DBG_TYPE_PRINTK, 0, 0,
-					"Can't allocate Tx urb for index %d\n",
-					i);
-			return -ENOMEM;
-		}
-	}
-
-	for (i = 0; i < MAXIMUM_USB_RCB; i++) {
-		psIntfAdapter->asUsbRcb[i].urb = usb_alloc_urb(0, GFP_KERNEL);
-
-		if (psIntfAdapter->asUsbRcb[i].urb == NULL) {
-			BCM_DEBUG_PRINT(psIntfAdapter->psAdapter,
-					DBG_TYPE_PRINTK, 0, 0,
-					"Can't allocate Rx urb for index %d\n",
-					i);
-			return -ENOMEM;
-		}
-
-		psIntfAdapter->asUsbRcb[i].urb->transfer_buffer =
-			kmalloc(MAX_DATA_BUFFER_SIZE, GFP_KERNEL);
-
-		if (psIntfAdapter->asUsbRcb[i].urb->transfer_buffer == NULL) {
-			BCM_DEBUG_PRINT(psIntfAdapter->psAdapter,
-					DBG_TYPE_PRINTK, 0, 0,
-					"Can't allocate Rx buffer for index %d\n",
-					i);
-			return -ENOMEM;
-		}
-		psIntfAdapter->asUsbRcb[i].urb->transfer_buffer_length =
-			MAX_DATA_BUFFER_SIZE;
-	}
-	return 0;
-}
-
-static int device_run(struct bcm_interface_adapter *psIntfAdapter)
-{
-	int value = 0;
-	UINT status = STATUS_SUCCESS;
-	struct bcm_mini_adapter *psAd = psIntfAdapter->psAdapter;
-
-	status = InitCardAndDownloadFirmware(psAd);
-	if (status != STATUS_SUCCESS) {
-		pr_err(DRV_NAME "InitCardAndDownloadFirmware failed.\n");
-		return status;
-	}
-	if (psAd->fw_download_done) {
-		if (StartInterruptUrb(psIntfAdapter)) {
-			BCM_DEBUG_PRINT(psAd, DBG_TYPE_INITEXIT, DRV_ENTRY,
-					DBG_LVL_ALL,
-					"Cannot send interrupt in URB\n");
-		}
-
-		/*
-		 * now register the cntrl interface.  after downloading the f/w
-		 * waiting for 5 sec to get the mailbox interrupt.
-		 */
-		psAd->waiting_to_fw_download_done = false;
-		value = wait_event_timeout(psAd->ioctl_fw_dnld_wait_queue,
-					   psAd->waiting_to_fw_download_done,
-					   5 * HZ);
-
-		if (value == 0)
-			pr_err(DRV_NAME ": Timeout waiting for mailbox interrupt.\n");
-
-		if (register_control_device_interface(psAd) < 0) {
-			pr_err(DRV_NAME ": Register Control Device failed.\n");
-			return -EIO;
-		}
-	}
-	return 0;
-}
-
-static int select_alternate_setting_for_highspeed_modem(
-		struct bcm_interface_adapter *psIntfAdapter,
-		struct usb_endpoint_descriptor **endpoint,
-		const struct usb_host_interface *iface_desc,
-		int *usedIntOutForBulkTransfer)
-{
-	int retval = 0;
-	struct bcm_mini_adapter *psAd = psIntfAdapter->psAdapter;
-
-	/* selecting alternate setting one as a default setting
-	 * for High Speed  modem. */
-	if (psIntfAdapter->bHighSpeedDevice)
-		retval = usb_set_interface(psIntfAdapter->udev,
-					   DEFAULT_SETTING_0,
-					   ALTERNATE_SETTING_1);
-	BCM_DEBUG_PRINT(psAd, DBG_TYPE_INITEXIT, DRV_ENTRY, DBG_LVL_ALL,
-			"BCM16 is applicable on this dongle\n");
-	if (retval || !psIntfAdapter->bHighSpeedDevice) {
-		*usedIntOutForBulkTransfer = EP2;
-		*endpoint = &iface_desc->endpoint[EP2].desc;
-		BCM_DEBUG_PRINT(psAd, DBG_TYPE_INITEXIT, DRV_ENTRY, DBG_LVL_ALL,
-				"Interface altsetting failed or modem is configured to Full Speed, hence will work on default setting 0\n");
-		/*
-		 * If Modem is high speed device EP2 should be
-		 * INT OUT End point
-		 *
-		 * If Mode is FS then EP2 should be bulk end
-		 * point
-		 */
-		if ((psIntfAdapter->bHighSpeedDevice &&
-					!usb_endpoint_is_int_out(*endpoint)) ||
-				(!psIntfAdapter->bHighSpeedDevice &&
-				 !usb_endpoint_is_bulk_out(*endpoint))) {
-			BCM_DEBUG_PRINT(psAd, DBG_TYPE_INITEXIT, DRV_ENTRY,
-					DBG_LVL_ALL,
-					"Configuring the EEPROM\n");
-			/* change the EP2, EP4 to INT OUT end point */
-			ConfigureEndPointTypesThroughEEPROM(
-					psAd);
-
-			/*
-			 * It resets the device and if any thing
-			 * gets changed in USB descriptor it
-			 * will show fail and re-enumerate the
-			 * device
-			 */
-			retval = usb_reset_device(psIntfAdapter->udev);
-			if (retval) {
-				BCM_DEBUG_PRINT(psAd, DBG_TYPE_INITEXIT,
-						DRV_ENTRY, DBG_LVL_ALL,
-						"reset failed.  Re-enumerating the device.\n");
-				return retval;
-			}
-
-		}
-		if (!psIntfAdapter->bHighSpeedDevice &&
-		    usb_endpoint_is_bulk_out(*endpoint)) {
-			/*
-			 * Once BULK is selected in FS mode.
-			 * Revert it back to INT.
-			 * Else USB_IF will fail.
-			 */
-			UINT _uiData = ntohl(EP2_CFG_INT);
-
-			BCM_DEBUG_PRINT(psAd, DBG_TYPE_INITEXIT, DRV_ENTRY,
-					DBG_LVL_ALL,
-					"Reverting Bulk to INT as it is in Full Speed mode.\n");
-			BeceemEEPROMBulkWrite(psAd, (PUCHAR) & _uiData, 0x136,
-					      4, TRUE);
-		}
-	} else {
-		*usedIntOutForBulkTransfer = EP4;
-		*endpoint = &iface_desc->endpoint[EP4].desc;
-		BCM_DEBUG_PRINT(psAd, DBG_TYPE_INITEXIT, DRV_ENTRY, DBG_LVL_ALL,
-				"Choosing AltSetting as a default setting.\n");
-		if (!usb_endpoint_is_int_out(*endpoint)) {
-			BCM_DEBUG_PRINT(psAd, DBG_TYPE_INITEXIT, DRV_ENTRY,
-					DBG_LVL_ALL,
-					"Dongle does not have BCM16 Fix.\n");
-			/*
-			 * change the EP2, EP4 to INT OUT end point and use EP4
-			 * in altsetting
-			 */
-			ConfigureEndPointTypesThroughEEPROM(psAd);
-
-			/*
-			 * It resets the device and if any thing
-			 * gets changed in USB descriptor it
-			 * will show fail and re-enumerate the
-			 * device
-			 */
-			retval = usb_reset_device(psIntfAdapter->udev);
-			if (retval) {
-				BCM_DEBUG_PRINT(psAd, DBG_TYPE_INITEXIT,
-						DRV_ENTRY, DBG_LVL_ALL,
-						"reset failed.  Re-enumerating the device.\n");
-				return retval;
-			}
-		}
-	}
-
-	return 0;
-}
-
-static int InterfaceAdapterInit(struct bcm_interface_adapter *psIntfAdapter)
-{
-	struct usb_host_interface *iface_desc;
-	struct usb_endpoint_descriptor *endpoint;
-	size_t buffer_size;
-	unsigned long value;
-	int retval = 0;
-	int usedIntOutForBulkTransfer = 0;
-	bool bBcm16 = false;
-	UINT uiData = 0;
-	int bytes;
-	struct bcm_mini_adapter *psAd = psIntfAdapter->psAdapter;
-
-	/* Store the usb dev into interface adapter */
-	psIntfAdapter->udev =
-		usb_get_dev(interface_to_usbdev(psIntfAdapter->interface));
-
-	psIntfAdapter->bHighSpeedDevice =
-		(psIntfAdapter->udev->speed == USB_SPEED_HIGH);
-	psAd->interface_rdm = BcmRDM;
-	psAd->interface_wrm = BcmWRM;
-
-	bytes = rdmalt(psAd, CHIP_ID_REG, (u32 *) &(psAd->chip_id),
-		       sizeof(u32));
-	if (bytes < 0) {
-		retval = bytes;
-		BCM_DEBUG_PRINT(psAd, DBG_TYPE_PRINTK, 0, 0,
-				"CHIP ID Read Failed\n");
-		return retval;
-	}
-
-	if (0xbece3200 == (psAd->chip_id & ~(0xF0)))
-		psAd->chip_id &= ~0xF0;
-
-	dev_info(&psIntfAdapter->udev->dev, "RDM Chip ID 0x%lx\n",
-		 psAd->chip_id);
-
-	iface_desc = psIntfAdapter->interface->cur_altsetting;
-
-	if (psAd->chip_id == T3B) {
-		/* T3B device will have EEPROM, check if EEPROM is proper and
-		 * BCM16 can be done or not. */
-		BeceemEEPROMBulkRead(psAd, &uiData, 0x0, 4);
-		if (uiData == BECM)
-			bBcm16 = TRUE;
-
-		dev_info(&psIntfAdapter->udev->dev,
-			 "number of alternate setting %d\n",
-			 psIntfAdapter->interface->num_altsetting);
-
-		if (bBcm16 == TRUE) {
-			retval = select_alternate_setting_for_highspeed_modem(
-					psIntfAdapter, &endpoint, iface_desc,
-					&usedIntOutForBulkTransfer);
-			if (retval)
-				return retval;
-		}
-	}
-
-	iface_desc = psIntfAdapter->interface->cur_altsetting;
-
-	for (value = 0; value < iface_desc->desc.bNumEndpoints; ++value) {
-		endpoint = &iface_desc->endpoint[value].desc;
-
-		if (!psIntfAdapter->sBulkIn.bulk_in_endpointAddr &&
-				usb_endpoint_is_bulk_in(endpoint)) {
-			buffer_size = le16_to_cpu(endpoint->wMaxPacketSize);
-			psIntfAdapter->sBulkIn.bulk_in_size = buffer_size;
-			psIntfAdapter->sBulkIn.bulk_in_endpointAddr =
-				endpoint->bEndpointAddress;
-			psIntfAdapter->sBulkIn.bulk_in_pipe = usb_rcvbulkpipe(
-					psIntfAdapter->udev,
-					psIntfAdapter->sBulkIn.bulk_in_endpointAddr);
-		}
-
-		if (!psIntfAdapter->sBulkOut.bulk_out_endpointAddr &&
-				usb_endpoint_is_bulk_out(endpoint)) {
-			psIntfAdapter->sBulkOut.bulk_out_endpointAddr =
-				endpoint->bEndpointAddress;
-			psIntfAdapter->sBulkOut.bulk_out_pipe = usb_sndbulkpipe(
-					psIntfAdapter->udev,
-					psIntfAdapter->sBulkOut.bulk_out_endpointAddr);
-		}
-
-		if (!psIntfAdapter->sIntrIn.int_in_endpointAddr &&
-				usb_endpoint_is_int_in(endpoint)) {
-			buffer_size = le16_to_cpu(endpoint->wMaxPacketSize);
-			psIntfAdapter->sIntrIn.int_in_size = buffer_size;
-			psIntfAdapter->sIntrIn.int_in_endpointAddr =
-				endpoint->bEndpointAddress;
-			psIntfAdapter->sIntrIn.int_in_interval =
-				endpoint->bInterval;
-			psIntfAdapter->sIntrIn.int_in_buffer =
-				kmalloc(buffer_size, GFP_KERNEL);
-			if (!psIntfAdapter->sIntrIn.int_in_buffer)
-				return -EINVAL;
-		}
-
-		if (!psIntfAdapter->sIntrOut.int_out_endpointAddr &&
-				usb_endpoint_is_int_out(endpoint)) {
-			if (!psIntfAdapter->sBulkOut.bulk_out_endpointAddr &&
-					(psAd->chip_id == T3B) &&
-					(value == usedIntOutForBulkTransfer)) {
-				/*
-				 * use first intout end point as a bulk out end
-				 * point
-				 */
-				buffer_size =
-					le16_to_cpu(endpoint->wMaxPacketSize);
-				psIntfAdapter->sBulkOut.bulk_out_size =
-					buffer_size;
-				psIntfAdapter->sBulkOut.bulk_out_endpointAddr =
-					endpoint->bEndpointAddress;
-				psIntfAdapter->sBulkOut.bulk_out_pipe =
-					usb_sndintpipe(psIntfAdapter->udev,
-							psIntfAdapter->sBulkOut
-							.bulk_out_endpointAddr);
-				psIntfAdapter->sBulkOut.int_out_interval =
-					endpoint->bInterval;
-			} else if (value == EP6) {
-				buffer_size =
-					le16_to_cpu(endpoint->wMaxPacketSize);
-				psIntfAdapter->sIntrOut.int_out_size =
-					buffer_size;
-				psIntfAdapter->sIntrOut.int_out_endpointAddr =
-					endpoint->bEndpointAddress;
-				psIntfAdapter->sIntrOut.int_out_interval =
-					endpoint->bInterval;
-				psIntfAdapter->sIntrOut.int_out_buffer =
-					kmalloc(buffer_size, GFP_KERNEL);
-				if (!psIntfAdapter->sIntrOut.int_out_buffer)
-					return -EINVAL;
-			}
-		}
-	}
-
-	usb_set_intfdata(psIntfAdapter->interface, psIntfAdapter);
-
-	psAd->bcm_file_download = InterfaceFileDownload;
-	psAd->bcm_file_readback_from_chip = InterfaceFileReadbackFromChip;
-	psAd->interface_transmit = InterfaceTransmitPacket;
-
-	retval = CreateInterruptUrb(psIntfAdapter);
-
-	if (retval) {
-		BCM_DEBUG_PRINT(psAd, DBG_TYPE_PRINTK, 0, 0,
-				"Cannot create interrupt urb\n");
-		return retval;
-	}
-
-	retval = AllocUsbCb(psIntfAdapter);
-	if (retval)
-		return retval;
-
-	return device_run(psIntfAdapter);
-}
-
-static int InterfaceSuspend(struct usb_interface *intf, pm_message_t message)
-{
-	struct bcm_interface_adapter *psIntfAdapter = usb_get_intfdata(intf);
-
-	psIntfAdapter->bSuspended = TRUE;
-
-	if (psIntfAdapter->bPreparingForBusSuspend) {
-		psIntfAdapter->bPreparingForBusSuspend = false;
-
-		if (psIntfAdapter->psAdapter->LinkStatus == LINKUP_DONE) {
-			psIntfAdapter->psAdapter->IdleMode = TRUE;
-			BCM_DEBUG_PRINT(psIntfAdapter->psAdapter,
-					DBG_TYPE_INITEXIT, DRV_ENTRY,
-					DBG_LVL_ALL,
-					"Host Entered in PMU Idle Mode.\n");
-		} else {
-			psIntfAdapter->psAdapter->bShutStatus = TRUE;
-			BCM_DEBUG_PRINT(psIntfAdapter->psAdapter,
-					DBG_TYPE_INITEXIT, DRV_ENTRY,
-					DBG_LVL_ALL,
-					"Host Entered in PMU Shutdown Mode.\n");
-		}
-	}
-	psIntfAdapter->psAdapter->bPreparingForLowPowerMode = false;
-
-	/* Signaling the control pkt path */
-	wake_up(&psIntfAdapter->psAdapter->lowpower_mode_wait_queue);
-
-	return 0;
-}
-
-static int InterfaceResume(struct usb_interface *intf)
-{
-	struct bcm_interface_adapter *psIntfAdapter = usb_get_intfdata(intf);
-
-	mdelay(100);
-	psIntfAdapter->bSuspended = false;
-
-	StartInterruptUrb(psIntfAdapter);
-	InterfaceRx(psIntfAdapter);
-	return 0;
-}
-
-static struct usb_driver usbbcm_driver = {
-	.name = "usbbcm",
-	.probe = usbbcm_device_probe,
-	.disconnect = usbbcm_disconnect,
-	.suspend = InterfaceSuspend,
-	.resume = InterfaceResume,
-	.id_table = InterfaceUsbtable,
-	.supports_autosuspend = 1,
-};
-
-struct class *bcm_class;
-
-static __init int bcm_init(void)
-{
-	int retval;
-
-	pr_info("%s: %s, %s\n", DRV_NAME, DRV_DESCRIPTION, DRV_VERSION);
-	pr_info("%s\n", DRV_COPYRIGHT);
-
-	bcm_class = class_create(THIS_MODULE, DRV_NAME);
-	if (IS_ERR(bcm_class)) {
-		pr_err(DRV_NAME ": could not create class\n");
-		return PTR_ERR(bcm_class);
-	}
-
-	retval = usb_register(&usbbcm_driver);
-	if (retval < 0) {
-		pr_err(DRV_NAME ": could not register usb driver\n");
-		class_destroy(bcm_class);
-		return retval;
-	}
-	return 0;
-}
-
-static __exit void bcm_exit(void)
-{
-	usb_deregister(&usbbcm_driver);
-	class_destroy(bcm_class);
-}
-
-module_init(bcm_init);
-module_exit(bcm_exit);
-
-MODULE_DESCRIPTION(DRV_DESCRIPTION);
-MODULE_VERSION(DRV_VERSION);
-MODULE_LICENSE("GPL");
diff --git a/drivers/staging/bcm/InterfaceInit.h b/drivers/staging/bcm/InterfaceInit.h
deleted file mode 100644
index ffa6e9667ec4..000000000000
--- a/drivers/staging/bcm/InterfaceInit.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#ifndef _INTERFACE_INIT_H
-#define _INTERFACE_INIT_H
-
-#define BCM_USB_VENDOR_ID_T3	0x198f
-#define BCM_USB_VENDOR_ID_FOXCONN	0x0489
-#define BCM_USB_VENDOR_ID_ZTE	0x19d2
-
-#define BCM_USB_PRODUCT_ID_T3	0x0300
-#define BCM_USB_PRODUCT_ID_T3B	0x0210
-#define BCM_USB_PRODUCT_ID_T3L	0x0220
-#define BCM_USB_PRODUCT_ID_SYM	0x15E
-#define BCM_USB_PRODUCT_ID_1901	0xe017
-#define BCM_USB_PRODUCT_ID_226	0x0132 /* not sure if this is valid */
-#define BCM_USB_PRODUCT_ID_ZTE_226 0x172
-#define BCM_USB_PRODUCT_ID_ZTE_326 0x173 /* ZTE AX326 */
-#define BCM_USB_PRODUCT_ID_ZTE_TU25	0x0007
-
-#define BCM_USB_MINOR_BASE	192
-
-int InterfaceInitialize(void);
-
-int InterfaceExit(void);
-
-int usbbcm_worker_thread(struct bcm_interface_adapter *psIntfAdapter);
-
-#endif
diff --git a/drivers/staging/bcm/InterfaceIsr.c b/drivers/staging/bcm/InterfaceIsr.c
deleted file mode 100644
index b9f8a7aa24fe..000000000000
--- a/drivers/staging/bcm/InterfaceIsr.c
+++ /dev/null
@@ -1,190 +0,0 @@
-#include "headers.h"
-
-
-static void read_int_callback(struct urb *urb/*, struct pt_regs *regs*/)
-{
-	int		status = urb->status;
-	struct bcm_interface_adapter *psIntfAdapter =
-		(struct bcm_interface_adapter *)urb->context;
-	struct bcm_mini_adapter *Adapter = psIntfAdapter->psAdapter;
-
-	if (netif_msg_intr(Adapter))
-		pr_info(PFX "%s: interrupt status %d\n",
-				Adapter->dev->name, status);
-
-	if (Adapter->device_removed) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, INTF_INIT,
-				DBG_LVL_ALL, "Device has Got Removed.");
-		return;
-	}
-
-	if ((Adapter->bPreparingForLowPowerMode && Adapter->bDoSuspend) ||
-			psIntfAdapter->bSuspended ||
-			psIntfAdapter->bPreparingForBusSuspend) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, INTF_INIT,
-				DBG_LVL_ALL,
-				"Interrupt call back is called while suspending the device");
-		return;
-	}
-
-	switch (status) {
-	/* success */
-	case STATUS_SUCCESS:
-		if (urb->actual_length) {
-
-			if (psIntfAdapter->ulInterruptData[1] & 0xFF) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
-						INTF_INIT, DBG_LVL_ALL,
-						"Got USIM interrupt");
-			}
-
-			if (psIntfAdapter->ulInterruptData[1] & 0xFF00) {
-				atomic_set(&Adapter->CurrNumFreeTxDesc,
-					(psIntfAdapter->ulInterruptData[1] &
-					 0xFF00) >> 8);
-				atomic_set(&Adapter->uiMBupdate, TRUE);
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
-					INTF_INIT, DBG_LVL_ALL,
-					"TX mailbox contains %d",
-					atomic_read(&Adapter->CurrNumFreeTxDesc));
-			}
-			if (psIntfAdapter->ulInterruptData[1] >> 16) {
-				Adapter->CurrNumRecvDescs =
-					(psIntfAdapter->ulInterruptData[1]  >> 16);
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
-						INTF_INIT, DBG_LVL_ALL,
-						"RX mailbox contains %d",
-						Adapter->CurrNumRecvDescs);
-				InterfaceRx(psIntfAdapter);
-			}
-			if (Adapter->fw_download_done &&
-				!Adapter->downloadDDR &&
-				atomic_read(&Adapter->CurrNumFreeTxDesc)) {
-
-				psIntfAdapter->psAdapter->downloadDDR += 1;
-				wake_up(&Adapter->tx_packet_wait_queue);
-			}
-			if (!Adapter->waiting_to_fw_download_done) {
-				Adapter->waiting_to_fw_download_done = TRUE;
-				wake_up(&Adapter->ioctl_fw_dnld_wait_queue);
-			}
-			if (!atomic_read(&Adapter->TxPktAvail)) {
-				atomic_set(&Adapter->TxPktAvail, 1);
-				wake_up(&Adapter->tx_packet_wait_queue);
-			}
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, INTF_INIT,
-					DBG_LVL_ALL, "Firing interrupt in URB");
-		}
-		break;
-	case -ENOENT:
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, INTF_INIT,
-				DBG_LVL_ALL, "URB has got disconnected....");
-		return;
-	case -EINPROGRESS:
-		/*
-		 * This situation may happened when URBunlink is used.  for
-		 * detail check usb_unlink_urb documentation.
-		 */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, INTF_INIT,
-				DBG_LVL_ALL,
-				"Impossibe condition has occurred... something very bad is going on");
-		break;
-		/* return; */
-	case -EPIPE:
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, INTF_INIT,
-				DBG_LVL_ALL,
-				"Interrupt IN endPoint has got halted/stalled...need to clear this");
-		Adapter->bEndPointHalted = TRUE;
-		wake_up(&Adapter->tx_packet_wait_queue);
-		urb->status = STATUS_SUCCESS;
-		return;
-	/* software-driven interface shutdown */
-	case -ECONNRESET:	/* URB got unlinked */
-	case -ESHUTDOWN:	/* hardware gone. this is the serious problem */
-		/*
-		 * Occurs only when something happens with the
-		 * host controller device
-		 */
-	case -ENODEV: /* Device got removed */
-	case -EINVAL:
-		/*
-		 * Some thing very bad happened with the URB. No
-		 * description is available.
-		 */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, INTF_INIT,
-				DBG_LVL_ALL, "interrupt urb error %d", status);
-		urb->status = STATUS_SUCCESS;
-		break;
-		/* return; */
-	default:
-		/*
-		 * This is required to check what is the defaults conditions
-		 * when it occurs..
-		 */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, NEXT_SEND, DBG_LVL_ALL,
-				"GOT DEFAULT INTERRUPT URB STATUS :%d..Please Analyze it...",
-				status);
-		break;
-	}
-
-	StartInterruptUrb(psIntfAdapter);
-
-
-}
-
-int CreateInterruptUrb(struct bcm_interface_adapter *psIntfAdapter)
-{
-	psIntfAdapter->psInterruptUrb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!psIntfAdapter->psInterruptUrb) {
-		BCM_DEBUG_PRINT(psIntfAdapter->psAdapter, DBG_TYPE_OTHERS,
-				INTF_INIT, DBG_LVL_ALL,
-				"Cannot allocate interrupt urb");
-		return -ENOMEM;
-	}
-	psIntfAdapter->psInterruptUrb->transfer_buffer =
-		psIntfAdapter->ulInterruptData;
-	psIntfAdapter->psInterruptUrb->transfer_buffer_length =
-		sizeof(psIntfAdapter->ulInterruptData);
-
-	psIntfAdapter->sIntrIn.int_in_pipe = usb_rcvintpipe(psIntfAdapter->udev,
-			psIntfAdapter->sIntrIn.int_in_endpointAddr);
-
-	usb_fill_int_urb(psIntfAdapter->psInterruptUrb, psIntfAdapter->udev,
-			psIntfAdapter->sIntrIn.int_in_pipe,
-			psIntfAdapter->psInterruptUrb->transfer_buffer,
-			psIntfAdapter->psInterruptUrb->transfer_buffer_length,
-			read_int_callback, psIntfAdapter,
-			psIntfAdapter->sIntrIn.int_in_interval);
-
-	BCM_DEBUG_PRINT(psIntfAdapter->psAdapter, DBG_TYPE_OTHERS, INTF_INIT,
-			DBG_LVL_ALL, "Interrupt Interval: %d\n",
-			psIntfAdapter->sIntrIn.int_in_interval);
-	return 0;
-}
-
-
-INT StartInterruptUrb(struct bcm_interface_adapter *psIntfAdapter)
-{
-	INT status = 0;
-
-	if (!(psIntfAdapter->psAdapter->device_removed ||
-				psIntfAdapter->psAdapter->bEndPointHalted ||
-				psIntfAdapter->bSuspended ||
-				psIntfAdapter->bPreparingForBusSuspend ||
-				psIntfAdapter->psAdapter->StopAllXaction)) {
-		status =
-			usb_submit_urb(psIntfAdapter->psInterruptUrb, GFP_ATOMIC);
-		if (status) {
-			BCM_DEBUG_PRINT(psIntfAdapter->psAdapter,
-					DBG_TYPE_OTHERS, INTF_INIT, DBG_LVL_ALL,
-					"Cannot send inturb %d\n", status);
-			if (status == -EPIPE) {
-				psIntfAdapter->psAdapter->bEndPointHalted =
-					TRUE;
-				wake_up(&psIntfAdapter->psAdapter->tx_packet_wait_queue);
-			}
-		}
-	}
-	return status;
-}
-
diff --git a/drivers/staging/bcm/InterfaceIsr.h b/drivers/staging/bcm/InterfaceIsr.h
deleted file mode 100644
index 3073bd71cfeb..000000000000
--- a/drivers/staging/bcm/InterfaceIsr.h
+++ /dev/null
@@ -1,15 +0,0 @@
-#ifndef _INTERFACE_ISR_H
-#define _INTERFACE_ISR_H
-
-int CreateInterruptUrb(struct bcm_interface_adapter *psIntfAdapter);
-
-
-INT StartInterruptUrb(struct bcm_interface_adapter *psIntfAdapter);
-
-
-VOID InterfaceEnableInterrupt(struct bcm_mini_adapter *Adapter);
-
-VOID InterfaceDisableInterrupt(struct bcm_mini_adapter *Adapter);
-
-#endif
-
diff --git a/drivers/staging/bcm/InterfaceMacros.h b/drivers/staging/bcm/InterfaceMacros.h
deleted file mode 100644
index fedb79437f33..000000000000
--- a/drivers/staging/bcm/InterfaceMacros.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#ifndef _INTERFACE_MACROS_H
-#define _INTERFACE_MACROS_H
-
-#define BCM_USB_MAX_READ_LENGTH 2048
-
-#define MAXIMUM_USB_TCB      128
-#define MAXIMUM_USB_RCB 	 128
-
-#define MAX_BUFFERS_PER_QUEUE   256
-
-#define MAX_DATA_BUFFER_SIZE    2048
-
-/* Num of Asynchronous reads pending */
-#define NUM_RX_DESC 64
-
-#define SYS_CFG 0x0F000C00
-
-#endif
diff --git a/drivers/staging/bcm/InterfaceMisc.c b/drivers/staging/bcm/InterfaceMisc.c
deleted file mode 100644
index e5bcfec2a6cf..000000000000
--- a/drivers/staging/bcm/InterfaceMisc.c
+++ /dev/null
@@ -1,247 +0,0 @@
-#include "headers.h"
-
-static int adapter_err_occurred(const struct bcm_interface_adapter *ad)
-{
-	if (ad->psAdapter->device_removed == TRUE) {
-		BCM_DEBUG_PRINT(ad->psAdapter, DBG_TYPE_PRINTK, 0, 0,
-				"Device got removed");
-		return -ENODEV;
-	}
-
-	if ((ad->psAdapter->StopAllXaction == TRUE) &&
-	    (ad->psAdapter->chip_id >= T3LPB)) {
-		BCM_DEBUG_PRINT(ad->psAdapter, DBG_TYPE_OTHERS, RDM,
-				DBG_LVL_ALL,
-				"Currently Xaction is not allowed on the bus");
-		return -EACCES;
-	}
-
-	if (ad->bSuspended == TRUE || ad->bPreparingForBusSuspend == TRUE) {
-		BCM_DEBUG_PRINT(ad->psAdapter, DBG_TYPE_OTHERS, RDM,
-				DBG_LVL_ALL,
-				"Bus is in suspended states hence RDM not allowed..");
-		return -EACCES;
-	}
-
-	return 0;
-}
-
-int InterfaceRDM(struct bcm_interface_adapter *psIntfAdapter,
-		unsigned int addr,
-		void *buff,
-		int len)
-{
-	int bytes;
-	int err = 0;
-
-	if (!psIntfAdapter)
-		return -EINVAL;
-
-	err = adapter_err_occurred(psIntfAdapter);
-	if (err)
-		return err;
-
-	psIntfAdapter->psAdapter->DeviceAccess = TRUE;
-
-	bytes = usb_control_msg(psIntfAdapter->udev,
-				usb_rcvctrlpipe(psIntfAdapter->udev, 0),
-				0x02,
-				0xC2,
-				(addr & 0xFFFF),
-				((addr >> 16) & 0xFFFF),
-				buff,
-				len,
-				5000);
-
-	if (-ENODEV == bytes)
-		psIntfAdapter->psAdapter->device_removed = TRUE;
-
-	if (bytes < 0)
-		BCM_DEBUG_PRINT(psIntfAdapter->psAdapter, DBG_TYPE_OTHERS, RDM,
-				DBG_LVL_ALL, "RDM failed status :%d", bytes);
-	else
-		BCM_DEBUG_PRINT(psIntfAdapter->psAdapter, DBG_TYPE_OTHERS, RDM,
-				DBG_LVL_ALL, "RDM sent %d", bytes);
-
-	psIntfAdapter->psAdapter->DeviceAccess = false;
-	return bytes;
-}
-
-int InterfaceWRM(struct bcm_interface_adapter *psIntfAdapter,
-		unsigned int addr,
-		void *buff,
-		int len)
-{
-	int retval = 0;
-	int err = 0;
-
-	if (!psIntfAdapter)
-		return -EINVAL;
-
-	err = adapter_err_occurred(psIntfAdapter);
-	if (err)
-		return err;
-
-	psIntfAdapter->psAdapter->DeviceAccess = TRUE;
-
-	retval = usb_control_msg(psIntfAdapter->udev,
-				usb_sndctrlpipe(psIntfAdapter->udev, 0),
-				0x01,
-				0x42,
-				(addr & 0xFFFF),
-				((addr >> 16) & 0xFFFF),
-				buff,
-				len,
-				5000);
-
-	if (-ENODEV == retval)
-		psIntfAdapter->psAdapter->device_removed = TRUE;
-
-	if (retval < 0)	{
-		BCM_DEBUG_PRINT(psIntfAdapter->psAdapter, DBG_TYPE_OTHERS, WRM,
-				DBG_LVL_ALL, "WRM failed status :%d", retval);
-		psIntfAdapter->psAdapter->DeviceAccess = false;
-		return retval;
-	} else {
-		psIntfAdapter->psAdapter->DeviceAccess = false;
-		BCM_DEBUG_PRINT(psIntfAdapter->psAdapter, DBG_TYPE_OTHERS, WRM,
-				DBG_LVL_ALL, "WRM sent %d", retval);
-		return STATUS_SUCCESS;
-	}
-}
-
-int BcmRDM(void *arg,
-	unsigned int addr,
-	void *buff,
-	int len)
-{
-	return InterfaceRDM((struct bcm_interface_adapter *)arg, addr, buff,
-			    len);
-}
-
-int BcmWRM(void *arg,
-	unsigned int addr,
-	void *buff,
-	int len)
-{
-	return InterfaceWRM((struct bcm_interface_adapter *)arg, addr, buff,
-			    len);
-}
-
-int Bcm_clear_halt_of_endpoints(struct bcm_mini_adapter *Adapter)
-{
-	struct bcm_interface_adapter *psIntfAdapter =
-		(struct bcm_interface_adapter *)(Adapter->pvInterfaceAdapter);
-	int status = STATUS_SUCCESS;
-
-	/*
-	 * usb_clear_halt - tells device to clear endpoint halt/stall condition
-	 * @dev: device whose endpoint is halted
-	 * @pipe: endpoint "pipe" being cleared
-	 * @ Context: !in_interrupt ()
-	 *
-	 * usb_clear_halt is the synchrnous call and returns 0 on success else
-	 * returns with error code.
-	 * This is used to clear halt conditions for bulk and interrupt
-	 * endpoints only.
-	 * Control and isochronous endpoints never halts.
-	 *
-	 * Any URBs  queued for such an endpoint should normally be unlinked by
-	 * the driver before clearing the halt condition.
-	 *
-	 */
-
-	/* Killing all the submitted urbs to different end points. */
-	Bcm_kill_all_URBs(psIntfAdapter);
-
-	/* clear the halted/stalled state for every end point */
-	status = usb_clear_halt(psIntfAdapter->udev,
-				psIntfAdapter->sIntrIn.int_in_pipe);
-	if (status != STATUS_SUCCESS)
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, INTF_INIT,
-				DBG_LVL_ALL,
-				"Unable to Clear Halt of Interrupt IN end point. :%d ",
-				status);
-
-	status = usb_clear_halt(psIntfAdapter->udev,
-				psIntfAdapter->sBulkIn.bulk_in_pipe);
-	if (status != STATUS_SUCCESS)
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, INTF_INIT,
-				DBG_LVL_ALL,
-				"Unable to Clear Halt of Bulk IN end point. :%d ",
-				status);
-
-	status = usb_clear_halt(psIntfAdapter->udev,
-				psIntfAdapter->sBulkOut.bulk_out_pipe);
-	if (status != STATUS_SUCCESS)
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, INTF_INIT,
-				DBG_LVL_ALL,
-				"Unable to Clear Halt of Bulk OUT end point. :%d ",
-				status);
-
-	return status;
-}
-
-void Bcm_kill_all_URBs(struct bcm_interface_adapter *psIntfAdapter)
-{
-	struct urb *tempUrb = NULL;
-	unsigned int i;
-
-	/*
-	 * usb_kill_urb - cancel a transfer request and wait for it to finish
-	 * @urb: pointer to URB describing a previously submitted request,
-	 * returns nothing as it is void returned API.
-	 *
-	 * This routine cancels an in-progress request. It is guaranteed that
-	 * upon return all completion handlers will have finished and the URB
-	 * will be totally idle and available for reuse
-	 *
-	 * This routine may not be used in an interrupt context (such as a
-	 * bottom half or a completion handler), or when holding a spinlock, or
-	 * in other situations where the caller can't schedule().
-	 *
-	 */
-
-	/* Cancel submitted Interrupt-URB's */
-	if (psIntfAdapter->psInterruptUrb) {
-		if (psIntfAdapter->psInterruptUrb->status == -EINPROGRESS)
-			usb_kill_urb(psIntfAdapter->psInterruptUrb);
-	}
-
-	/* Cancel All submitted TX URB's */
-	for (i = 0; i < MAXIMUM_USB_TCB; i++) {
-		tempUrb = psIntfAdapter->asUsbTcb[i].urb;
-		if (tempUrb) {
-			if (tempUrb->status == -EINPROGRESS)
-				usb_kill_urb(tempUrb);
-		}
-	}
-
-	for (i = 0; i < MAXIMUM_USB_RCB; i++) {
-		tempUrb = psIntfAdapter->asUsbRcb[i].urb;
-		if (tempUrb) {
-			if (tempUrb->status == -EINPROGRESS)
-				usb_kill_urb(tempUrb);
-		}
-	}
-
-	atomic_set(&psIntfAdapter->uNumTcbUsed, 0);
-	atomic_set(&psIntfAdapter->uCurrTcb, 0);
-
-	atomic_set(&psIntfAdapter->uNumRcbUsed, 0);
-	atomic_set(&psIntfAdapter->uCurrRcb, 0);
-}
-
-void putUsbSuspend(struct work_struct *work)
-{
-	struct bcm_interface_adapter *psIntfAdapter = NULL;
-	struct usb_interface *intf = NULL;
-
-	psIntfAdapter = container_of(work, struct bcm_interface_adapter,
-				     usbSuspendWork);
-	intf = psIntfAdapter->interface;
-
-	if (psIntfAdapter->bSuspended == false)
-		usb_autopm_put_interface(intf);
-}
-
diff --git a/drivers/staging/bcm/InterfaceMisc.h b/drivers/staging/bcm/InterfaceMisc.h
deleted file mode 100644
index 0e5e38b26329..000000000000
--- a/drivers/staging/bcm/InterfaceMisc.h
+++ /dev/null
@@ -1,42 +0,0 @@
-#ifndef __INTERFACE_MISC_H
-#define __INTERFACE_MISC_H
-
-INT
-InterfaceRDM(struct bcm_interface_adapter *psIntfAdapter,
-			UINT addr,
-			PVOID buff,
-			INT len);
-
-INT
-InterfaceWRM(struct bcm_interface_adapter *psIntfAdapter,
-			UINT addr,
-			PVOID buff,
-			INT len);
-
-
-int InterfaceFileDownload(PVOID psIntfAdapter,
-			struct file *flp,
-			unsigned int on_chip_loc);
-
-int InterfaceFileReadbackFromChip(PVOID psIntfAdapter,
-			struct file *flp,
-			unsigned int on_chip_loc);
-
-
-int BcmRDM(PVOID arg,
-			UINT addr,
-			PVOID buff,
-			INT len);
-
-int BcmWRM(PVOID arg,
-			UINT addr,
-			PVOID buff,
-			INT len);
-
-INT Bcm_clear_halt_of_endpoints(struct bcm_mini_adapter *Adapter);
-
-VOID Bcm_kill_all_URBs(struct bcm_interface_adapter *psIntfAdapter);
-
-#define DISABLE_USB_ZERO_LEN_INT 0x0F011878
-
-#endif /* __INTERFACE_MISC_H */
diff --git a/drivers/staging/bcm/InterfaceRx.c b/drivers/staging/bcm/InterfaceRx.c
deleted file mode 100644
index 0f179b9382d3..000000000000
--- a/drivers/staging/bcm/InterfaceRx.c
+++ /dev/null
@@ -1,289 +0,0 @@
-#include "headers.h"
-
-static void handle_control_packet(struct bcm_interface_adapter *interface,
-				  struct bcm_mini_adapter *ad,
-				  struct bcm_leader *leader,
-				  struct sk_buff *skb,
-				  struct urb *urb)
-{
-	BCM_DEBUG_PRINT(interface->psAdapter, DBG_TYPE_RX, RX_CTRL, DBG_LVL_ALL,
-			"Received control pkt...");
-	*(PUSHORT)skb->data = leader->Status;
-	memcpy(skb->data+sizeof(USHORT), urb->transfer_buffer +
-	       (sizeof(struct bcm_leader)), leader->PLength);
-	skb->len = leader->PLength + sizeof(USHORT);
-
-	spin_lock(&ad->control_queue_lock);
-	ENQUEUEPACKET(ad->RxControlHead, ad->RxControlTail, skb);
-	spin_unlock(&ad->control_queue_lock);
-
-	atomic_inc(&ad->cntrlpktCnt);
-	wake_up(&ad->process_rx_cntrlpkt);
-}
-
-static void format_eth_hdr_to_stack(struct bcm_interface_adapter *interface,
-				    struct bcm_mini_adapter *ad,
-				    struct bcm_leader *p_leader,
-				    struct sk_buff *skb,
-				    struct urb *urb,
-				    UINT ui_index,
-				    int queue_index,
-				    bool b_header_supression_endabled)
-{
-	/*
-	 * Data Packet, Format a proper Ethernet Header
-	 * and give it to the stack
-	 */
-	BCM_DEBUG_PRINT(interface->psAdapter, DBG_TYPE_RX, RX_DATA,
-			DBG_LVL_ALL, "Received Data pkt...");
-	skb_reserve(skb, 2 + SKB_RESERVE_PHS_BYTES);
-	memcpy(skb->data+ETH_HLEN, (PUCHAR)urb->transfer_buffer +
-	       sizeof(struct bcm_leader), p_leader->PLength);
-	skb->dev = ad->dev;
-
-	/* currently skb->len has extra ETH_HLEN bytes in the beginning */
-	skb_put(skb, p_leader->PLength + ETH_HLEN);
-	ad->PackInfo[queue_index].uiTotalRxBytes += p_leader->PLength;
-	ad->PackInfo[queue_index].uiThisPeriodRxBytes += p_leader->PLength;
-	BCM_DEBUG_PRINT(interface->psAdapter, DBG_TYPE_RX, RX_DATA,
-			DBG_LVL_ALL, "Received Data pkt of len :0x%X",
-			p_leader->PLength);
-
-	if (netif_running(ad->dev)) {
-		/* Moving ahead by ETH_HLEN to the data ptr as received from FW */
-		skb_pull(skb, ETH_HLEN);
-		PHSReceive(ad, p_leader->Vcid, skb, &skb->len,
-			   NULL, b_header_supression_endabled);
-
-		if (!ad->PackInfo[queue_index].bEthCSSupport) {
-			skb_push(skb, ETH_HLEN);
-
-			memcpy(skb->data, skb->dev->dev_addr, 6);
-			memcpy(skb->data+6, skb->dev->dev_addr, 6);
-			(*(skb->data+11))++;
-			*(skb->data+12) = 0x08;
-			*(skb->data+13) = 0x00;
-			p_leader->PLength += ETH_HLEN;
-		}
-
-		skb->protocol = eth_type_trans(skb, ad->dev);
-		netif_rx(skb);
-	} else {
-		BCM_DEBUG_PRINT(interface->psAdapter, DBG_TYPE_RX,
-				RX_DATA, DBG_LVL_ALL,
-				"i/f not up hance freeing SKB...");
-		dev_kfree_skb(skb);
-	}
-
-	++ad->dev->stats.rx_packets;
-	ad->dev->stats.rx_bytes += p_leader->PLength;
-
-	for (ui_index = 0; ui_index < MIBS_MAX_HIST_ENTRIES; ui_index++) {
-		if ((p_leader->PLength <=
-		    MIBS_PKTSIZEHIST_RANGE*(ui_index+1)) &&
-			(p_leader->PLength > MIBS_PKTSIZEHIST_RANGE*(ui_index)))
-
-			ad->aRxPktSizeHist[ui_index]++;
-	}
-}
-
-static int SearchVcid(struct bcm_mini_adapter *Adapter, unsigned short usVcid)
-{
-	int iIndex = 0;
-
-	for (iIndex = (NO_OF_QUEUES-1); iIndex >= 0; iIndex--)
-		if (Adapter->PackInfo[iIndex].usVCID_Value == usVcid)
-			return iIndex;
-	return NO_OF_QUEUES+1;
-
-}
-
-
-static struct bcm_usb_rcb *
-GetBulkInRcb(struct bcm_interface_adapter *psIntfAdapter)
-{
-	struct bcm_usb_rcb *pRcb = NULL;
-	UINT index = 0;
-
-	if ((atomic_read(&psIntfAdapter->uNumRcbUsed) < MAXIMUM_USB_RCB) &&
-	    (psIntfAdapter->psAdapter->StopAllXaction == false)) {
-		index = atomic_read(&psIntfAdapter->uCurrRcb);
-		pRcb = &psIntfAdapter->asUsbRcb[index];
-		pRcb->bUsed = TRUE;
-		pRcb->psIntfAdapter = psIntfAdapter;
-		BCM_DEBUG_PRINT(psIntfAdapter->psAdapter, DBG_TYPE_RX, RX_DPC,
-				DBG_LVL_ALL, "Got Rx desc %d used %d", index,
-				atomic_read(&psIntfAdapter->uNumRcbUsed));
-		index = (index + 1) % MAXIMUM_USB_RCB;
-		atomic_set(&psIntfAdapter->uCurrRcb, index);
-		atomic_inc(&psIntfAdapter->uNumRcbUsed);
-	}
-	return pRcb;
-}
-
-/*this is receive call back - when pkt available for receive (BULK IN- end point)*/
-static void read_bulk_callback(struct urb *urb)
-{
-	struct sk_buff *skb = NULL;
-	bool bHeaderSupressionEnabled = false;
-	int QueueIndex = NO_OF_QUEUES + 1;
-	UINT uiIndex = 0;
-	struct bcm_usb_rcb *pRcb = (struct bcm_usb_rcb *)urb->context;
-	struct bcm_interface_adapter *psIntfAdapter = pRcb->psIntfAdapter;
-	struct bcm_mini_adapter *Adapter = psIntfAdapter->psAdapter;
-	struct bcm_leader *pLeader = urb->transfer_buffer;
-
-	if (unlikely(netif_msg_rx_status(Adapter)))
-		pr_info(PFX "%s: rx urb status %d length %d\n",
-			Adapter->dev->name, urb->status, urb->actual_length);
-
-	if ((Adapter->device_removed == TRUE) ||
-	    (TRUE == Adapter->bEndPointHalted) ||
-	    (0 == urb->actual_length)) {
-		pRcb->bUsed = false;
-		atomic_dec(&psIntfAdapter->uNumRcbUsed);
-		return;
-	}
-
-	if (urb->status != STATUS_SUCCESS) {
-		if (urb->status == -EPIPE) {
-			Adapter->bEndPointHalted = TRUE;
-			wake_up(&Adapter->tx_packet_wait_queue);
-		} else {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_RX, RX_DPC,
-					DBG_LVL_ALL,
-					"Rx URB has got cancelled. status :%d",
-					urb->status);
-		}
-		pRcb->bUsed = false;
-		atomic_dec(&psIntfAdapter->uNumRcbUsed);
-		urb->status = STATUS_SUCCESS;
-		return;
-	}
-
-	if (Adapter->bDoSuspend && (Adapter->bPreparingForLowPowerMode)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_RX, RX_DPC, DBG_LVL_ALL,
-				"device is going in low power mode while PMU option selected..hence rx packet should not be process");
-		return;
-	}
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_RX, RX_DPC, DBG_LVL_ALL,
-			"Read back done len %d\n", pLeader->PLength);
-	if (!pLeader->PLength) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_RX, RX_DPC, DBG_LVL_ALL,
-				"Leader Length 0");
-		atomic_dec(&psIntfAdapter->uNumRcbUsed);
-		return;
-	}
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_RX, RX_DPC, DBG_LVL_ALL,
-			"Leader Status:0x%hX, Length:0x%hX, VCID:0x%hX",
-			pLeader->Status, pLeader->PLength, pLeader->Vcid);
-	if (MAX_CNTL_PKT_SIZE < pLeader->PLength) {
-		if (netif_msg_rx_err(Adapter))
-			pr_info(PFX "%s: corrupted leader length...%d\n",
-				Adapter->dev->name, pLeader->PLength);
-		++Adapter->dev->stats.rx_dropped;
-		atomic_dec(&psIntfAdapter->uNumRcbUsed);
-		return;
-	}
-
-	QueueIndex = SearchVcid(Adapter, pLeader->Vcid);
-	if (QueueIndex < NO_OF_QUEUES) {
-		bHeaderSupressionEnabled =
-			Adapter->PackInfo[QueueIndex].bHeaderSuppressionEnabled;
-		bHeaderSupressionEnabled =
-			bHeaderSupressionEnabled & Adapter->bPHSEnabled;
-	}
-
-	skb = dev_alloc_skb(pLeader->PLength + SKB_RESERVE_PHS_BYTES +
-			    SKB_RESERVE_ETHERNET_HEADER);
-	if (!skb) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-				"NO SKBUFF!!! Dropping the Packet");
-		atomic_dec(&psIntfAdapter->uNumRcbUsed);
-		return;
-	}
-	/* If it is a control Packet, then call handle_bcm_packet ()*/
-	if ((ntohs(pLeader->Vcid) == VCID_CONTROL_PACKET) ||
-	    (!(pLeader->Status >= 0x20  &&  pLeader->Status <= 0x3F))) {
-		handle_control_packet(psIntfAdapter, Adapter, pLeader, skb,
-				      urb);
-	} else {
-		format_eth_hdr_to_stack(psIntfAdapter, Adapter, pLeader, skb,
-					urb, uiIndex, QueueIndex,
-					bHeaderSupressionEnabled);
-	}
-	Adapter->PrevNumRecvDescs++;
-	pRcb->bUsed = false;
-	atomic_dec(&psIntfAdapter->uNumRcbUsed);
-}
-
-static int ReceiveRcb(struct bcm_interface_adapter *psIntfAdapter,
-		      struct bcm_usb_rcb *pRcb)
-{
-	struct urb *urb = pRcb->urb;
-	int retval = 0;
-
-	usb_fill_bulk_urb(urb, psIntfAdapter->udev,
-			  usb_rcvbulkpipe(psIntfAdapter->udev,
-					  psIntfAdapter->sBulkIn.bulk_in_endpointAddr),
-			  urb->transfer_buffer,
-			  BCM_USB_MAX_READ_LENGTH,
-			  read_bulk_callback, pRcb);
-
-	if (false == psIntfAdapter->psAdapter->device_removed &&
-	    false == psIntfAdapter->psAdapter->bEndPointHalted &&
-	    false == psIntfAdapter->bSuspended &&
-	    false == psIntfAdapter->bPreparingForBusSuspend) {
-		retval = usb_submit_urb(urb, GFP_ATOMIC);
-		if (retval) {
-			BCM_DEBUG_PRINT(psIntfAdapter->psAdapter, DBG_TYPE_RX,
-					RX_DPC, DBG_LVL_ALL,
-					"failed submitting read urb, error %d",
-					retval);
-			/* if this return value is because of pipe halt. need to clear this. */
-			if (retval == -EPIPE) {
-				psIntfAdapter->psAdapter->bEndPointHalted = TRUE;
-				wake_up(&psIntfAdapter->psAdapter->tx_packet_wait_queue);
-			}
-
-		}
-	}
-	return retval;
-}
-
-/*
-Function:				InterfaceRx
-
-Description:			This is the hardware specific Function for Receiving
-						data packet/control packets from the device.
-
-Input parameters:		IN struct bcm_mini_adapter *Adapter   - Miniport Adapter Context
-
-
-
-Return:				TRUE  - If Rx was successful.
-					Other - If an error occurred.
-*/
-
-bool InterfaceRx(struct bcm_interface_adapter *psIntfAdapter)
-{
-	USHORT RxDescCount = NUM_RX_DESC -
-		atomic_read(&psIntfAdapter->uNumRcbUsed);
-
-	struct bcm_usb_rcb *pRcb = NULL;
-
-	while (RxDescCount) {
-		pRcb = GetBulkInRcb(psIntfAdapter);
-		if (pRcb == NULL) {
-			BCM_DEBUG_PRINT(psIntfAdapter->psAdapter,
-					DBG_TYPE_PRINTK, 0, 0,
-					"Unable to get Rcb pointer");
-			return false;
-		}
-		ReceiveRcb(psIntfAdapter, pRcb);
-		RxDescCount--;
-	}
-	return TRUE;
-}
-
diff --git a/drivers/staging/bcm/InterfaceRx.h b/drivers/staging/bcm/InterfaceRx.h
deleted file mode 100644
index b4e858bcda34..000000000000
--- a/drivers/staging/bcm/InterfaceRx.h
+++ /dev/null
@@ -1,7 +0,0 @@
-#ifndef _INTERFACE_RX_H
-#define _INTERFACE_RX_H
-
-bool InterfaceRx(struct bcm_interface_adapter *Adapter);
-
-#endif
-
diff --git a/drivers/staging/bcm/InterfaceTx.c b/drivers/staging/bcm/InterfaceTx.c
deleted file mode 100644
index 9b3f64b821ed..000000000000
--- a/drivers/staging/bcm/InterfaceTx.c
+++ /dev/null
@@ -1,213 +0,0 @@
-#include "headers.h"
-
-static void prepare_low_power_mode(struct urb *urb,
-				   struct bcm_interface_adapter *interface,
-				   struct bcm_mini_adapter *ps_adapter,
-				   struct bcm_mini_adapter *ad,
-				   struct bcm_link_request *p_control_msg,
-				   bool *b_power_down_msg)
-{
-	if (((p_control_msg->szData[0] == GO_TO_IDLE_MODE_PAYLOAD) &&
-		(p_control_msg->szData[1] == TARGET_CAN_GO_TO_IDLE_MODE))) {
-
-		*b_power_down_msg = TRUE;
-		/*
-		 * This covers the bus err while Idle Request msg
-		 * sent down.
-		 */
-		if (urb->status != STATUS_SUCCESS) {
-			ps_adapter->bPreparingForLowPowerMode = false;
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_TX, NEXT_SEND,
-					DBG_LVL_ALL,
-					"Idle Mode Request msg failed to reach to Modem");
-			/* Signalling the cntrl pkt path in Ioctl */
-			wake_up(&ps_adapter->lowpower_mode_wait_queue);
-			StartInterruptUrb(interface);
-			return;
-		}
-
-		if (ps_adapter->bDoSuspend == false) {
-			ps_adapter->IdleMode = TRUE;
-			/* since going in Idle mode completed hence making this var false */
-			ps_adapter->bPreparingForLowPowerMode = false;
-
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_TX, NEXT_SEND,
-					DBG_LVL_ALL,
-					"Host Entered in Idle Mode State...");
-			/* Signalling the cntrl pkt path in Ioctl*/
-			wake_up(&ps_adapter->lowpower_mode_wait_queue);
-		}
-
-	} else if ((p_control_msg->Leader.Status == LINK_UP_CONTROL_REQ) &&
-		(p_control_msg->szData[0] == LINK_UP_ACK) &&
-		(p_control_msg->szData[1] == LINK_SHUTDOWN_REQ_FROM_FIRMWARE)  &&
-		(p_control_msg->szData[2] == SHUTDOWN_ACK_FROM_DRIVER)) {
-		/*
-		 * This covers the bus err while shutdown Request
-		 * msg sent down.
-		 */
-		if (urb->status != STATUS_SUCCESS) {
-			ps_adapter->bPreparingForLowPowerMode = false;
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_TX, NEXT_SEND,
-					DBG_LVL_ALL,
-					"Shutdown Request Msg failed to reach to Modem");
-			/* Signalling the cntrl pkt path in Ioctl */
-			wake_up(&ps_adapter->lowpower_mode_wait_queue);
-			StartInterruptUrb(interface);
-			return;
-		}
-
-		*b_power_down_msg = TRUE;
-		if (ps_adapter->bDoSuspend == false) {
-			ps_adapter->bShutStatus = TRUE;
-			/*
-			 * since going in shutdown mode completed hence
-			 * making this var false
-			 */
-			ps_adapter->bPreparingForLowPowerMode = false;
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_TX, NEXT_SEND,
-					DBG_LVL_ALL,
-					"Host Entered in shutdown Mode State...");
-			/* Signalling the cntrl pkt path in Ioctl */
-			wake_up(&ps_adapter->lowpower_mode_wait_queue);
-		}
-	}
-
-	if (ps_adapter->bDoSuspend && *b_power_down_msg) {
-		/* issuing bus suspend request */
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_TX, NEXT_SEND, DBG_LVL_ALL,
-				"Issuing the Bus suspend request to USB stack");
-		interface->bPreparingForBusSuspend = TRUE;
-		schedule_work(&interface->usbSuspendWork);
-	}
-}
-
-/*this is transmit call-back(BULK OUT)*/
-static void write_bulk_callback(struct urb *urb/*, struct pt_regs *regs*/)
-{
-	struct bcm_usb_tcb *pTcb = (struct bcm_usb_tcb *)urb->context;
-	struct bcm_interface_adapter *psIntfAdapter = pTcb->psIntfAdapter;
-	struct bcm_link_request *pControlMsg =
-		(struct bcm_link_request *)urb->transfer_buffer;
-	struct bcm_mini_adapter *psAdapter = psIntfAdapter->psAdapter;
-	bool bpowerDownMsg = false;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	if (unlikely(netif_msg_tx_done(Adapter)))
-		pr_info(PFX "%s: transmit status %d\n", Adapter->dev->name,
-			urb->status);
-
-	if (urb->status != STATUS_SUCCESS) {
-		if (urb->status == -EPIPE) {
-			psIntfAdapter->psAdapter->bEndPointHalted = TRUE;
-			wake_up(&psIntfAdapter->psAdapter->tx_packet_wait_queue);
-		} else {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, NEXT_SEND,
-					DBG_LVL_ALL,
-					"Tx URB has got cancelled. status :%d",
-					urb->status);
-		}
-	}
-
-	pTcb->bUsed = false;
-	atomic_dec(&psIntfAdapter->uNumTcbUsed);
-
-	if (TRUE == psAdapter->bPreparingForLowPowerMode) {
-		prepare_low_power_mode(urb, psIntfAdapter, psAdapter, Adapter,
-				       pControlMsg, &bpowerDownMsg);
-	}
-
-	usb_free_coherent(urb->dev, urb->transfer_buffer_length,
-			urb->transfer_buffer, urb->transfer_dma);
-}
-
-
-static struct bcm_usb_tcb *GetBulkOutTcb(struct bcm_interface_adapter *psIntfAdapter)
-{
-	struct bcm_usb_tcb *pTcb = NULL;
-	UINT index = 0;
-
-	if ((atomic_read(&psIntfAdapter->uNumTcbUsed) < MAXIMUM_USB_TCB) &&
-		(psIntfAdapter->psAdapter->StopAllXaction == false)) {
-		index = atomic_read(&psIntfAdapter->uCurrTcb);
-		pTcb = &psIntfAdapter->asUsbTcb[index];
-		pTcb->bUsed = TRUE;
-		pTcb->psIntfAdapter = psIntfAdapter;
-		BCM_DEBUG_PRINT(psIntfAdapter->psAdapter, DBG_TYPE_TX,
-				NEXT_SEND, DBG_LVL_ALL,
-				"Got Tx desc %d used %d",
-				index,
-				atomic_read(&psIntfAdapter->uNumTcbUsed));
-		index = (index + 1) % MAXIMUM_USB_TCB;
-		atomic_set(&psIntfAdapter->uCurrTcb, index);
-		atomic_inc(&psIntfAdapter->uNumTcbUsed);
-	}
-	return pTcb;
-}
-
-static int TransmitTcb(struct bcm_interface_adapter *psIntfAdapter,
-		       struct bcm_usb_tcb *pTcb, PVOID data, int len)
-{
-
-	struct urb *urb = pTcb->urb;
-	int retval = 0;
-
-	urb->transfer_buffer = usb_alloc_coherent(psIntfAdapter->udev, len,
-						GFP_ATOMIC, &urb->transfer_dma);
-	if (!urb->transfer_buffer) {
-		BCM_DEBUG_PRINT(psIntfAdapter->psAdapter, DBG_TYPE_PRINTK, 0, 0,
-				"Error allocating memory\n");
-		return  -ENOMEM;
-	}
-	memcpy(urb->transfer_buffer, data, len);
-	urb->transfer_buffer_length = len;
-
-	BCM_DEBUG_PRINT(psIntfAdapter->psAdapter, DBG_TYPE_TX, NEXT_SEND,
-			DBG_LVL_ALL, "Sending Bulk out packet\n");
-	/* For T3B,INT OUT end point will be used as bulk out end point */
-	if ((psIntfAdapter->psAdapter->chip_id == T3B) &&
-			(psIntfAdapter->bHighSpeedDevice == TRUE)) {
-		usb_fill_int_urb(urb, psIntfAdapter->udev,
-			psIntfAdapter->sBulkOut.bulk_out_pipe,
-			urb->transfer_buffer, len, write_bulk_callback, pTcb,
-			psIntfAdapter->sBulkOut.int_out_interval);
-	} else {
-	usb_fill_bulk_urb(urb, psIntfAdapter->udev,
-		  psIntfAdapter->sBulkOut.bulk_out_pipe,
-		  urb->transfer_buffer, len, write_bulk_callback, pTcb);
-	}
-	urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP; /* For DMA transfer */
-
-	if (false == psIntfAdapter->psAdapter->device_removed &&
-	   false == psIntfAdapter->psAdapter->bEndPointHalted &&
-	   false == psIntfAdapter->bSuspended &&
-	   false == psIntfAdapter->bPreparingForBusSuspend) {
-		retval = usb_submit_urb(urb, GFP_ATOMIC);
-		if (retval) {
-			BCM_DEBUG_PRINT(psIntfAdapter->psAdapter, DBG_TYPE_TX,
-					NEXT_SEND, DBG_LVL_ALL,
-					"failed submitting write urb, error %d",
-					retval);
-			if (retval == -EPIPE) {
-				psIntfAdapter->psAdapter->bEndPointHalted = TRUE;
-				wake_up(&psIntfAdapter->psAdapter->tx_packet_wait_queue);
-			}
-		}
-	}
-	return retval;
-}
-
-int InterfaceTransmitPacket(PVOID arg, PVOID data, UINT len)
-{
-	struct bcm_usb_tcb *pTcb = NULL;
-	struct bcm_interface_adapter *psIntfAdapter = arg;
-
-	pTcb = GetBulkOutTcb(psIntfAdapter);
-	if (pTcb == NULL) {
-		BCM_DEBUG_PRINT(psIntfAdapter->psAdapter, DBG_TYPE_PRINTK, 0, 0,
-				"No URB to transmit packet, dropping packet");
-		return -EFAULT;
-	}
-	return TransmitTcb(psIntfAdapter, pTcb, data, len);
-}
-
diff --git a/drivers/staging/bcm/InterfaceTx.h b/drivers/staging/bcm/InterfaceTx.h
deleted file mode 100644
index 273147577c17..000000000000
--- a/drivers/staging/bcm/InterfaceTx.h
+++ /dev/null
@@ -1,7 +0,0 @@
-#ifndef _INTERFACE_TX_H
-#define _INTERFACE_TX_H
-
-INT InterfaceTransmitPacket(PVOID arg, PVOID data, UINT len);
-
-#endif
-
diff --git a/drivers/staging/bcm/Ioctl.h b/drivers/staging/bcm/Ioctl.h
deleted file mode 100644
index fa5f8671612e..000000000000
--- a/drivers/staging/bcm/Ioctl.h
+++ /dev/null
@@ -1,226 +0,0 @@
-#ifndef _IOCTL_H_
-#define _IOCTL_H_
-
-struct bcm_rdm_buffer {
-	unsigned long Register;
-	unsigned long Length;
-} __packed;
-
-struct bcm_wrm_buffer {
-	unsigned long Register;
-	unsigned long Length;
-	unsigned char Data[4];
-} __packed;
-
-struct bcm_ioctl_buffer {
-	void __user *InputBuffer;
-	unsigned long InputLength;
-	void __user *OutputBuffer;
-	unsigned long OutputLength;
-} __packed;
-
-struct bcm_gpio_info {
-	unsigned int uiGpioNumber; /* valid numbers 0-15 */
-	unsigned int uiGpioValue; /* 1 set ; 0 not  set */
-} __packed;
-
-struct bcm_user_thread_req {
-	/* 0->Inactivate LED thread. */
-	/* 1->Activate the LED thread */
-	unsigned int ThreadState;
-} __packed;
-
-#define LED_THREAD_ACTIVATION_REQ  1
-#define BCM_IOCTL				'k'
-#define IOCTL_SEND_CONTROL_MESSAGE		_IOW(BCM_IOCTL,	0x801, int)
-#define IOCTL_BCM_REGISTER_WRITE		_IOW(BCM_IOCTL, 0x802, int)
-#define IOCTL_BCM_REGISTER_READ			_IOR(BCM_IOCTL, 0x803, int)
-#define IOCTL_BCM_COMMON_MEMORY_WRITE		_IOW(BCM_IOCTL, 0x804, int)
-#define IOCTL_BCM_COMMON_MEMORY_READ		_IOR(BCM_IOCTL, 0x805, int)
-#define IOCTL_GET_CONTROL_MESSAGE		_IOR(BCM_IOCTL,	0x806, int)
-#define IOCTL_BCM_FIRMWARE_DOWNLOAD		_IOW(BCM_IOCTL, 0x807, int)
-#define IOCTL_BCM_SET_SEND_VCID			_IOW(BCM_IOCTL,	0x808, int)
-#define IOCTL_BCM_SWITCH_TRANSFER_MODE		_IOW(BCM_IOCTL, 0x809, int)
-#define IOCTL_LINK_REQ				_IOW(BCM_IOCTL, 0x80A, int)
-#define IOCTL_RSSI_LEVEL_REQ			_IOW(BCM_IOCTL, 0x80B, int)
-#define IOCTL_IDLE_REQ				_IOW(BCM_IOCTL, 0x80C, int)
-#define IOCTL_SS_INFO_REQ			_IOW(BCM_IOCTL, 0x80D, int)
-#define IOCTL_GET_STATISTICS_POINTER		_IOW(BCM_IOCTL, 0x80E, int)
-#define IOCTL_CM_REQUEST			_IOW(BCM_IOCTL, 0x80F, int)
-#define IOCTL_INIT_PARAM_REQ			_IOW(BCM_IOCTL, 0x810, int)
-#define IOCTL_MAC_ADDR_REQ			_IOW(BCM_IOCTL, 0x811, int)
-#define IOCTL_MAC_ADDR_RESP			_IOWR(BCM_IOCTL, 0x812, int)
-#define IOCTL_CLASSIFICATION_RULE		_IOW(BCM_IOCTL, 0x813, char)
-#define IOCTL_CLOSE_NOTIFICATION		_IO(BCM_IOCTL, 0x814)
-#define IOCTL_LINK_UP				_IO(BCM_IOCTL, 0x815)
-#define IOCTL_LINK_DOWN				_IO(BCM_IOCTL, 0x816, struct bcm_ioctl_buffer)
-#define IOCTL_CHIP_RESET			_IO(BCM_IOCTL, 0x816)
-#define IOCTL_CINR_LEVEL_REQ			_IOW(BCM_IOCTL, 0x817, char)
-#define IOCTL_WTM_CONTROL_REQ			_IOW(BCM_IOCTL, 0x817, char)
-#define IOCTL_BE_BUCKET_SIZE			_IOW(BCM_IOCTL, 0x818, unsigned long)
-#define IOCTL_RTPS_BUCKET_SIZE			_IOW(BCM_IOCTL, 0x819, unsigned long)
-#define IOCTL_QOS_THRESHOLD			_IOW(BCM_IOCTL, 0x820, unsigned long)
-#define IOCTL_DUMP_PACKET_INFO			_IO(BCM_IOCTL, 0x821)
-#define IOCTL_GET_PACK_INFO			_IOR(BCM_IOCTL, 0x823, int)
-#define IOCTL_BCM_GET_DRIVER_VERSION		_IOR(BCM_IOCTL, 0x829, int)
-#define IOCTL_BCM_GET_CURRENT_STATUS		_IOW(BCM_IOCTL, 0x828, int)
-#define IOCTL_BCM_GPIO_SET_REQUEST		_IOW(BCM_IOCTL, 0x82A, int)
-#define IOCTL_BCM_GPIO_STATUS_REQUEST		_IOW(BCM_IOCTL, 0x82b, int)
-#define IOCTL_BCM_GET_DSX_INDICATION		_IOR(BCM_IOCTL, 0x854, int)
-#define IOCTL_BCM_BUFFER_DOWNLOAD_START		_IOW(BCM_IOCTL, 0x855, int)
-#define IOCTL_BCM_BUFFER_DOWNLOAD		_IOW(BCM_IOCTL, 0x856, int)
-#define IOCTL_BCM_BUFFER_DOWNLOAD_STOP		_IOW(BCM_IOCTL, 0x857, int)
-#define IOCTL_BCM_REGISTER_WRITE_PRIVATE	_IOW(BCM_IOCTL, 0x826, char)
-#define IOCTL_BCM_REGISTER_READ_PRIVATE		_IOW(BCM_IOCTL, 0x827, char)
-#define IOCTL_BCM_SET_DEBUG			_IOW(BCM_IOCTL, 0x824, struct bcm_ioctl_buffer)
-#define IOCTL_BCM_EEPROM_REGISTER_WRITE		_IOW(BCM_IOCTL, 0x858, int)
-#define IOCTL_BCM_EEPROM_REGISTER_READ		_IOR(BCM_IOCTL, 0x859, int)
-#define IOCTL_BCM_WAKE_UP_DEVICE_FROM_IDLE	_IOR(BCM_IOCTL, 0x860, int)
-#define IOCTL_BCM_SET_MAC_TRACING		_IOW(BCM_IOCTL, 0x82c, int)
-#define IOCTL_BCM_GET_HOST_MIBS			_IOW(BCM_IOCTL, 0x853, int)
-#define IOCTL_BCM_NVM_READ			_IOR(BCM_IOCTL, 0x861, int)
-#define IOCTL_BCM_NVM_WRITE			_IOW(BCM_IOCTL, 0x862, int)
-#define IOCTL_BCM_GET_NVM_SIZE			_IOR(BCM_IOCTL, 0x863, int)
-#define IOCTL_BCM_CAL_INIT			_IOR(BCM_IOCTL, 0x864, int)
-#define IOCTL_BCM_BULK_WRM			_IOW(BCM_IOCTL, 0x90B, int)
-#define IOCTL_BCM_FLASH2X_SECTION_READ		_IOR(BCM_IOCTL, 0x865, int)
-#define IOCTL_BCM_FLASH2X_SECTION_WRITE		_IOW(BCM_IOCTL, 0x866, int)
-#define IOCTL_BCM_GET_FLASH2X_SECTION_BITMAP	_IOR(BCM_IOCTL, 0x867, int)
-#define IOCTL_BCM_SET_ACTIVE_SECTION		_IOW(BCM_IOCTL, 0x868, int)
-#define IOCTL_BCM_IDENTIFY_ACTIVE_SECTION	_IO(BCM_IOCTL, 0x869)
-#define IOCTL_BCM_COPY_SECTION			_IOW(BCM_IOCTL, 0x870, int)
-#define IOCTL_BCM_GET_FLASH_CS_INFO		_IOR(BCM_IOCTL, 0x871, int)
-#define IOCTL_BCM_SELECT_DSD			_IOW(BCM_IOCTL, 0x872, int)
-#define IOCTL_BCM_NVM_RAW_READ			_IOR(BCM_IOCTL, 0x875, int)
-#define IOCTL_BCM_CNTRLMSG_MASK			_IOW(BCM_IOCTL, 0x874, int)
-#define IOCTL_BCM_GET_DEVICE_DRIVER_INFO	_IOR(BCM_IOCTL, 0x877, int)
-#define IOCTL_BCM_TIME_SINCE_NET_ENTRY		_IOR(BCM_IOCTL, 0x876, int)
-#define BCM_LED_THREAD_STATE_CHANGE_REQ		_IOW(BCM_IOCTL, 0x878, int)
-#define IOCTL_BCM_GPIO_MULTI_REQUEST		_IOW(BCM_IOCTL, 0x82D, struct bcm_ioctl_buffer)
-#define IOCTL_BCM_GPIO_MODE_REQUEST		_IOW(BCM_IOCTL, 0x82E, struct bcm_ioctl_buffer)
-
-enum bcm_interface_type {
-	BCM_MII,
-	BCM_CARDBUS,
-	BCM_USB,
-	BCM_SDIO,
-	BCM_PCMCIA
-};
-
-struct bcm_driver_info {
-	enum bcm_nvm_type	u32NVMType;
-	unsigned int		MaxRDMBufferSize;
-	enum bcm_interface_type	u32InterfaceType;
-	unsigned int		u32DSDStartOffset;
-	unsigned int		u32RxAlignmentCorrection;
-	unsigned int		u32Reserved[10];
-};
-
-struct bcm_nvm_readwrite {
-	void __user *pBuffer;
-	uint32_t  uiOffset;
-	uint32_t uiNumBytes;
-	bool bVerify;
-};
-
-struct bcm_bulk_wrm_buffer {
-	unsigned long Register;
-	unsigned long SwapEndian;
-	unsigned long Values[1];
-};
-
-enum bcm_flash2x_section_val {
-	NO_SECTION_VAL = 0, /* no section chosen when absolute offset is given for RD/WR */
-	ISO_IMAGE1,
-	ISO_IMAGE2,
-	DSD0,
-	DSD1,
-	DSD2,
-	VSA0,
-	VSA1,
-	VSA2,
-	SCSI,
-	CONTROL_SECTION,
-	ISO_IMAGE1_PART2,
-	ISO_IMAGE1_PART3,
-	ISO_IMAGE2_PART2,
-	ISO_IMAGE2_PART3,
-	TOTAL_SECTIONS
-};
-
-/*
- * Structure used for READ/WRITE Flash Map2.x
- */
-struct bcm_flash2x_readwrite {
-	enum bcm_flash2x_section_val Section; /* section to be read/written */
-	u32 offset; /* offset within section. */
-	u32 numOfBytes; /* number of bytes from the offset */
-	u32 bVerify;
-	void __user *pDataBuff; /* buffer for reading/writing */
-};
-
-/*
- * This structure is used for coping one section to other.
- * there are two ways to copy one section to other.
- * it NOB =0, complete section will be copied on to other.
- * if NOB !=0, only NOB will be copied from the given offset.
- */
-
-struct bcm_flash2x_copy_section {
-	enum bcm_flash2x_section_val SrcSection;
-	enum bcm_flash2x_section_val DstSection;
-	u32 offset;
-	u32 numOfBytes;
-};
-
-/*
- * This section provide the complete bitmap of the Flash.
- * using this map lib/APP will issue read/write command.
- * Fields are defined as :
- * Bit [0] = section is present  //1:present, 0: Not present
- * Bit [1] = section is valid  //1: valid, 0: not valid
- * Bit [2] = Section is R/W  //0: RW, 1: RO
- * Bit [3] = Section is Active or not 1 means Active, 0->inactive
- * Bit [7...3] = Reserved
- */
-
-struct bcm_flash2x_bitmap {
-	unsigned char ISO_IMAGE1;
-	unsigned char ISO_IMAGE2;
-	unsigned char DSD0;
-	unsigned char DSD1;
-	unsigned char DSD2;
-	unsigned char VSA0;
-	unsigned char VSA1;
-	unsigned char VSA2;
-	unsigned char SCSI;
-	unsigned char CONTROL_SECTION;
-	/* Reserved for future use */
-	unsigned char Reserved0;
-	unsigned char Reserved1;
-	unsigned char Reserved2;
-};
-
-struct bcm_time_elapsed {
-	u64 ul64TimeElapsedSinceNetEntry;
-	u32 uiReserved[4];
-};
-
-enum {
-	WIMAX_IDX = 0, /* To access WiMAX chip GPIO's for GPIO_MULTI_INFO or GPIO_MULTI_MODE */
-	HOST_IDX, /* To access Host chip GPIO's for GPIO_MULTI_INFO or GPIO_MULTI_MODE */
-	MAX_IDX
-};
-
-struct bcm_gpio_multi_info {
-	unsigned int uiGPIOCommand; /* 1 for set and 0 for get */
-	unsigned int uiGPIOMask; /* set the corresponding bit to 1 to access GPIO */
-	unsigned int uiGPIOValue; /* 0 or 1; value to be set when command is 1. */
-} __packed;
-
-struct bcm_gpio_multi_mode {
-	unsigned int uiGPIOMode; /* 1 for OUT mode, 0 for IN mode */
-	unsigned int uiGPIOMask; /* GPIO mask to set mode */
-} __packed;
-
-#endif
diff --git a/drivers/staging/bcm/Kconfig b/drivers/staging/bcm/Kconfig
deleted file mode 100644
index 8acf4b24a7c9..000000000000
--- a/drivers/staging/bcm/Kconfig
+++ /dev/null
@@ -1,6 +0,0 @@
-config BCM_WIMAX
-       tristate "Beceem BCS200/BCS220-3 and BCSM250 wimax support"
-       depends on USB && NET
-       help
-         This is an experimental driver for the Beceem WIMAX chipset used
-	 by Sprint 4G.
diff --git a/drivers/staging/bcm/LeakyBucket.c b/drivers/staging/bcm/LeakyBucket.c
deleted file mode 100644
index d6b55f993b57..000000000000
--- a/drivers/staging/bcm/LeakyBucket.c
+++ /dev/null
@@ -1,364 +0,0 @@
-/**********************************************************************
-*			LEAKYBUCKET.C
-*	This file contains the routines related to Leaky Bucket Algorithm.
-***********************************************************************/
-#include "headers.h"
-
-/**
- * UpdateTokenCount() - Calculates the token count for each channel
- * and updates the same in Adapter structure
- * @Adapter:	Pointer to the Adapter structure.
- *
- * Return: None
- */
-static VOID UpdateTokenCount(register struct bcm_mini_adapter *Adapter)
-{
-	ULONG liCurrentTime;
-	INT i = 0;
-	struct timeval tv;
-	struct bcm_packet_info *curr_pi;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TOKEN_COUNTS, DBG_LVL_ALL,
-			"=====>\n");
-	if (NULL == Adapter) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TOKEN_COUNTS,
-				DBG_LVL_ALL, "Adapter found NULL!\n");
-		return;
-	}
-
-	do_gettimeofday(&tv);
-	for (i = 0; i < NO_OF_QUEUES; i++) {
-		curr_pi = &Adapter->PackInfo[i];
-
-		if (TRUE == curr_pi->bValid && (1 == curr_pi->ucDirection)) {
-			liCurrentTime = ((tv.tv_sec -
-				curr_pi->stLastUpdateTokenAt.tv_sec)*1000 +
-				(tv.tv_usec - curr_pi->stLastUpdateTokenAt.tv_usec) /
-				1000);
-			if (0 != liCurrentTime) {
-				curr_pi->uiCurrentTokenCount += (ULONG)
-					((curr_pi->uiMaxAllowedRate) *
-					((ULONG)((liCurrentTime)))/1000);
-				memcpy(&curr_pi->stLastUpdateTokenAt, &tv,
-				       sizeof(struct timeval));
-				curr_pi->liLastUpdateTokenAt = liCurrentTime;
-				if (curr_pi->uiCurrentTokenCount >=
-				    curr_pi->uiMaxBucketSize) {
-					curr_pi->uiCurrentTokenCount =
-						curr_pi->uiMaxBucketSize;
-				}
-			}
-		}
-	}
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TOKEN_COUNTS, DBG_LVL_ALL,
-			"<=====\n");
-}
-
-
-/**
- * IsPacketAllowedForFlow() - This function checks whether the given
- * packet from the specified queue can be allowed for transmission by
- * checking the token count.
- * @Adapter:		Pointer to the Adpater structure.
- * @iQIndex:		The queue Identifier.
- * @ulPacketLength:	Number of bytes to be transmitted.
- *
- * Returns: The number of bytes allowed for transmission.
- */
-static ULONG GetSFTokenCount(struct bcm_mini_adapter *Adapter, struct bcm_packet_info *psSF)
-{
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TOKEN_COUNTS, DBG_LVL_ALL,
-			"IsPacketAllowedForFlow ===>");
-
-	/* Validate the parameters */
-	if (NULL == Adapter || (psSF < Adapter->PackInfo &&
-	    (uintptr_t)psSF > (uintptr_t) &Adapter->PackInfo[HiPriority])) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TOKEN_COUNTS, DBG_LVL_ALL,
-				"IPAFF: Got wrong Parameters:Adapter: %p, QIndex: %zd\n",
-				Adapter, (psSF-Adapter->PackInfo));
-		return 0;
-	}
-
-	if (false != psSF->bValid && psSF->ucDirection) {
-		if (0 != psSF->uiCurrentTokenCount) {
-			return psSF->uiCurrentTokenCount;
-		} else {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TOKEN_COUNTS,
-					DBG_LVL_ALL,
-					"Not enough tokens in queue %zd Available %u\n",
-					psSF-Adapter->PackInfo, psSF->uiCurrentTokenCount);
-			psSF->uiPendedLast = 1;
-		}
-	} else {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TOKEN_COUNTS, DBG_LVL_ALL,
-				"IPAFF: Queue %zd not valid\n",
-				psSF-Adapter->PackInfo);
-	}
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TOKEN_COUNTS, DBG_LVL_ALL,
-			"IsPacketAllowedForFlow <===");
-	return 0;
-}
-
-/**
-@ingroup tx_functions
-This function despatches packet from the specified queue.
-@return Zero(success) or Negative value(failure)
-*/
-static INT SendPacketFromQueue(struct bcm_mini_adapter *Adapter,/**<Logical Adapter*/
-			       struct bcm_packet_info *psSF, /**<Queue identifier*/
-			       struct sk_buff *Packet)	/**<Pointer to the packet to be sent*/
-{
-	INT Status = STATUS_FAILURE;
-	UINT uiIndex = 0, PktLen = 0;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, SEND_QUEUE, DBG_LVL_ALL,
-			"=====>");
-	if (!Adapter || !Packet || !psSF) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, SEND_QUEUE, DBG_LVL_ALL,
-				"Got NULL Adapter or Packet");
-		return -EINVAL;
-	}
-
-	if (psSF->liDrainCalculated == 0)
-		psSF->liDrainCalculated = jiffies;
-	/* send the packet to the fifo.. */
-	PktLen = Packet->len;
-	Status = SetupNextSend(Adapter, Packet, psSF->usVCID_Value);
-	if (Status == 0) {
-		for (uiIndex = 0; uiIndex < MIBS_MAX_HIST_ENTRIES; uiIndex++) {
-			if ((PktLen <= MIBS_PKTSIZEHIST_RANGE*(uiIndex+1)) &&
-			    (PktLen > MIBS_PKTSIZEHIST_RANGE*(uiIndex)))
-				Adapter->aTxPktSizeHist[uiIndex]++;
-		}
-	}
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, SEND_QUEUE, DBG_LVL_ALL,
-			"<=====");
-	return Status;
-}
-
-static void get_data_packet(struct bcm_mini_adapter *ad,
-			    struct bcm_packet_info *ps_sf)
-{
-	int packet_len;
-	struct sk_buff *qpacket;
-
-	if (!ps_sf->ucDirection)
-		return;
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL,
-			"UpdateTokenCount ");
-	if (ad->IdleMode || ad->bPreparingForLowPowerMode)
-		return; /* in idle mode */
-
-	/* Check for Free Descriptors */
-	if (atomic_read(&ad->CurrNumFreeTxDesc) <=
-	    MINIMUM_PENDING_DESCRIPTORS) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL,
-				" No Free Tx Descriptor(%d) is available for Data pkt..",
-				atomic_read(&ad->CurrNumFreeTxDesc));
-		return;
-	}
-
-	spin_lock_bh(&ps_sf->SFQueueLock);
-	qpacket = ps_sf->FirstTxQueue;
-
-	if (qpacket) {
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL,
-				"Dequeuing Data Packet");
-
-		if (ps_sf->bEthCSSupport)
-			packet_len = qpacket->len;
-		else
-			packet_len = qpacket->len - ETH_HLEN;
-
-		packet_len <<= 3;
-		if (packet_len <= GetSFTokenCount(ad, ps_sf)) {
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_TX, TX_PACKETS,
-					DBG_LVL_ALL, "Allowed bytes %d",
-					(packet_len >> 3));
-
-			DEQUEUEPACKET(ps_sf->FirstTxQueue, ps_sf->LastTxQueue);
-			ps_sf->uiCurrentBytesOnHost -= (qpacket->len);
-			ps_sf->uiCurrentPacketsOnHost--;
-				atomic_dec(&ad->TotalPacketCount);
-			spin_unlock_bh(&ps_sf->SFQueueLock);
-
-			SendPacketFromQueue(ad, ps_sf, qpacket);
-			ps_sf->uiPendedLast = false;
-		} else {
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_TX, TX_PACKETS,
-					DBG_LVL_ALL, "For Queue: %zd\n",
-					ps_sf - ad->PackInfo);
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_TX, TX_PACKETS,
-					DBG_LVL_ALL,
-					"\nAvailable Tokens = %d required = %d\n",
-					ps_sf->uiCurrentTokenCount,
-					packet_len);
-			/*
-			this part indicates that because of
-			non-availability of the tokens
-			pkt has not been send out hence setting the
-			pending flag indicating the host to send it out
-			first next iteration.
-			*/
-			ps_sf->uiPendedLast = TRUE;
-			spin_unlock_bh(&ps_sf->SFQueueLock);
-		}
-	} else {
-		spin_unlock_bh(&ps_sf->SFQueueLock);
-	}
-}
-
-static void send_control_packet(struct bcm_mini_adapter *ad,
-				struct bcm_packet_info *ps_sf)
-{
-	char *ctrl_packet = NULL;
-	INT status = 0;
-
-	if ((atomic_read(&ad->CurrNumFreeTxDesc) > 0) &&
-	    (atomic_read(&ad->index_rd_txcntrlpkt) !=
-	     atomic_read(&ad->index_wr_txcntrlpkt))) {
-		ctrl_packet = ad->txctlpacket
-		[(atomic_read(&ad->index_rd_txcntrlpkt)%MAX_CNTRL_PKTS)];
-		if (ctrl_packet) {
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_TX, TX_PACKETS,
-					DBG_LVL_ALL,
-					"Sending Control packet");
-			status = SendControlPacket(ad, ctrl_packet);
-			if (STATUS_SUCCESS == status) {
-				spin_lock_bh(&ps_sf->SFQueueLock);
-				ps_sf->NumOfPacketsSent++;
-				ps_sf->uiSentBytes += ((struct bcm_leader *)ctrl_packet)->PLength;
-				ps_sf->uiSentPackets++;
-				atomic_dec(&ad->TotalPacketCount);
-				ps_sf->uiCurrentBytesOnHost -= ((struct bcm_leader *)ctrl_packet)->PLength;
-				ps_sf->uiCurrentPacketsOnHost--;
-				atomic_inc(&ad->index_rd_txcntrlpkt);
-				spin_unlock_bh(&ps_sf->SFQueueLock);
-			} else {
-				BCM_DEBUG_PRINT(ad, DBG_TYPE_TX, TX_PACKETS,
-						DBG_LVL_ALL,
-						"SendControlPacket Failed\n");
-			}
-		} else {
-			BCM_DEBUG_PRINT(ad, DBG_TYPE_TX, TX_PACKETS,
-					DBG_LVL_ALL,
-					" Control Pkt is not available, Indexing is wrong....");
-		}
-	}
-}
-
-/**
- * CheckAndSendPacketFromIndex() - This function dequeues the
- * data/control packet from the specified queue for transmission.
- * @Adapter:	Pointer to the driver control structure.
- * @iQIndex:	The queue Identifier.
- *
- * Returns: None.
- */
-static VOID CheckAndSendPacketFromIndex(struct bcm_mini_adapter *Adapter,
-					struct bcm_packet_info *psSF)
-{
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL,
-			"%zd ====>", (psSF-Adapter->PackInfo));
-	if ((psSF != &Adapter->PackInfo[HiPriority]) &&
-	    Adapter->LinkUpStatus &&
-	    atomic_read(&psSF->uiPerSFTxResourceCount)) { /* Get data packet */
-
-		get_data_packet(Adapter, psSF);
-	} else {
-		send_control_packet(Adapter, psSF);
-	}
-}
-
-
-/**
- * transmit_packets() - This function transmits the packets from
- * different queues, if free descriptors are available on target.
- * @Adapter:	Pointer to the Adapter structure.
- *
- * Returns: None.
- */
-VOID transmit_packets(struct bcm_mini_adapter *Adapter)
-{
-	UINT uiPrevTotalCount = 0;
-	int iIndex = 0;
-
-	bool exit_flag = TRUE;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL,
-			"=====>");
-
-	if (NULL == Adapter) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL,
-				"Got NULL Adapter");
-		return;
-	}
-	if (Adapter->device_removed == TRUE) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL,
-				"Device removed");
-		return;
-	}
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL,
-			"\nUpdateTokenCount ====>\n");
-
-	UpdateTokenCount(Adapter);
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL,
-			"\nPruneQueueAllSF ====>\n");
-
-	PruneQueueAllSF(Adapter);
-
-	uiPrevTotalCount = atomic_read(&Adapter->TotalPacketCount);
-
-	for (iIndex = HiPriority; iIndex >= 0; iIndex--) {
-		if (!uiPrevTotalCount || (TRUE == Adapter->device_removed))
-				break;
-
-		if (Adapter->PackInfo[iIndex].bValid &&
-		    Adapter->PackInfo[iIndex].uiPendedLast &&
-		    Adapter->PackInfo[iIndex].uiCurrentBytesOnHost) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS,
-					DBG_LVL_ALL,
-					"Calling CheckAndSendPacketFromIndex..");
-			CheckAndSendPacketFromIndex(Adapter,
-						    &Adapter->PackInfo[iIndex]);
-			uiPrevTotalCount--;
-		}
-	}
-
-	while (uiPrevTotalCount > 0 && !Adapter->device_removed) {
-		exit_flag = TRUE;
-		/* second iteration to parse non-pending queues */
-		for (iIndex = HiPriority; iIndex >= 0; iIndex--) {
-			if (!uiPrevTotalCount ||
-			    (TRUE == Adapter->device_removed))
-				break;
-
-			if (Adapter->PackInfo[iIndex].bValid &&
-			    Adapter->PackInfo[iIndex].uiCurrentBytesOnHost &&
-			    !Adapter->PackInfo[iIndex].uiPendedLast) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX,
-						TX_PACKETS, DBG_LVL_ALL,
-						"Calling CheckAndSendPacketFromIndex..");
-				CheckAndSendPacketFromIndex(Adapter, &Adapter->PackInfo[iIndex]);
-				uiPrevTotalCount--;
-				exit_flag = false;
-			}
-		}
-
-		if (Adapter->IdleMode || Adapter->bPreparingForLowPowerMode) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS,
-					DBG_LVL_ALL, "In Idle Mode\n");
-			break;
-		}
-		if (exit_flag == TRUE)
-			break;
-	} /* end of inner while loop */
-
-	update_per_cid_rx(Adapter);
-	Adapter->txtransmit_running = 0;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL,
-			"<======");
-}
diff --git a/drivers/staging/bcm/Macros.h b/drivers/staging/bcm/Macros.h
deleted file mode 100644
index dc01e3016d4f..000000000000
--- a/drivers/staging/bcm/Macros.h
+++ /dev/null
@@ -1,352 +0,0 @@
-/*************************************
-* Macros.h
-**************************************/
-#ifndef __MACROS_H__
-#define __MACROS_H__
-
-#define TX_TIMER_PERIOD 10 /*10 msec*/
-#define MAX_CLASSIFIERS 100
-#define MAX_TARGET_DSX_BUFFERS 24
-
-#define MAX_CNTRL_PKTS    100
-#define MAX_DATA_PKTS     200
-#define MAX_ETH_SIZE      1536
-#define MAX_CNTL_PKT_SIZE 2048
-
-#define MTU_SIZE 1400
-#define TX_QLEN  5
-
-#define MAC_ADDR_REGISTER 0xbf60d000
-
-
-/* Quality of Service */
-#define NO_OF_QUEUES 17
-#define HiPriority (NO_OF_QUEUES-1)
-#define LowPriority 0
-#define BE          2
-#define rtPS        4
-#define ERTPS       5
-#define UGS         6
-
-#define BE_BUCKET_SIZE       (1024*1024*100)  /* 32kb */
-#define rtPS_BUCKET_SIZE     (1024*1024*100) /*  8kb */
-#define MAX_ALLOWED_RATE     (1024*1024*100)
-#define TX_PACKET_THRESHOLD  10
-#define XSECONDS             (1*HZ)
-#define DSC_ACTIVATE_REQUEST 248
-#define QUEUE_DEPTH_OFFSET   0x1fc01000
-#define MAX_DEVICE_DESC_SIZE 2040
-#define MAX_CTRL_QUEUE_LEN   100
-#define MAX_APP_QUEUE_LEN    200
-#define MAX_LATENCY_ALLOWED  0xFFFFFFFF
-#define DEFAULT_UG_INTERVAL  250
-#define DEFAULT_UGI_FACTOR   4
-
-#define DEFAULT_PERSFCOUNT    60
-#define MAX_CONNECTIONS       10
-#define MAX_CLASS_NAME_LENGTH 32
-
-#define ETH_LENGTH_OF_ADDRESS   6
-#define MAX_MULTICAST_ADDRESSES 32
-#define IP_LENGTH_OF_ADDRESS    4
-
-#define IP_PACKET_ONLY_MODE       0
-#define ETH_PACKET_TUNNELING_MODE 1
-
-/* Link Request */
-#define SET_MAC_ADDRESS_REQUEST 0
-#define SYNC_UP_REQUEST         1
-#define SYNCED_UP               2
-#define LINK_UP_REQUEST         3
-#define LINK_CONNECTED          4
-#define SYNC_UP_NOTIFICATION    2
-#define LINK_UP_NOTIFICATION    4
-
-
-#define LINK_NET_ENTRY              0x0002
-#define HMC_STATUS                  0x0004
-#define LINK_UP_CONTROL_REQ         0x83
-
-#define STATS_POINTER_REQ_STATUS    0x86
-#define NETWORK_ENTRY_REQ_PAYLOAD   198
-#define LINK_DOWN_REQ_PAYLOAD       226
-#define SYNC_UP_REQ_PAYLOAD         228
-#define STATISTICS_POINTER_REQ      237
-#define LINK_UP_REQ_PAYLOAD         245
-#define LINK_UP_ACK                 246
-
-#define STATS_MSG_SIZE              4
-#define INDEX_TO_DATA               4
-
-#define GO_TO_IDLE_MODE_PAYLOAD         210
-#define COME_UP_FROM_IDLE_MODE_PAYLOAD  211
-#define IDLE_MODE_SF_UPDATE_MSG         187
-
-#define SKB_RESERVE_ETHERNET_HEADER 16
-#define SKB_RESERVE_PHS_BYTES       32
-
-#define IP_PACKET_ONLY_MODE       0
-#define ETH_PACKET_TUNNELING_MODE 1
-
-#define ETH_CS_802_3       1
-#define ETH_CS_802_1Q_VLAN 3
-#define IPV4_CS            1
-#define IPV6_CS            2
-#define ETH_CS_MASK        0x3f
-
-/** \brief Validity bit maps for TLVs in packet classification rule */
-
-#define PKT_CLASSIFICATION_USER_PRIORITY_VALID 0
-#define PKT_CLASSIFICATION_VLANID_VALID        1
-
-#ifndef MIN
-#define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b))
-#endif
-
-
-/*Leader related terms */
-#define LEADER_STATUS         0x00
-#define LEADER_STATUS_TCP_ACK 0x1
-#define LEADER_SIZE           sizeof(struct bcm_leader)
-#define MAC_ADDR_REQ_SIZE     sizeof(struct bcm_packettosend)
-#define SS_INFO_REQ_SIZE      sizeof(struct bcm_packettosend)
-#define CM_REQUEST_SIZE       (LEADER_SIZE + sizeof(stLocalSFChangeRequest))
-#define IDLE_REQ_SIZE         sizeof(struct bcm_packettosend)
-
-
-#define MAX_TRANSFER_CTRL_BYTE_USB (2*1024)
-
-#define GET_MAILBOX1_REG_REQUEST        0x87
-#define GET_MAILBOX1_REG_RESPONSE       0x67
-#define VCID_CONTROL_PACKET             0x00
-
-#define TRANSMIT_NETWORK_DATA           0x00
-#define RECEIVED_NETWORK_DATA           0x20
-
-#define CM_RESPONSES            0xA0
-#define STATUS_RSP              0xA1
-#define LINK_CONTROL_RESP       0xA2
-#define IDLE_MODE_STATUS        0xA3
-#define STATS_POINTER_RESP      0xA6
-#define MGMT_MSG_INFO_SW_STATUS 0xA7
-#define AUTH_SS_HOST_MSG        0xA8
-
-#define CM_DSA_ACK_PAYLOAD            247
-#define CM_DSC_ACK_PAYLOAD            248
-#define CM_DSD_ACK_PAYLOAD            249
-#define CM_DSDEACTVATE                250
-#define TOTAL_MASKED_ADDRESS_IN_BYTES 32
-
-#define MAC_REQ         0
-#define LINK_RESP       1
-#define RSSI_INDICATION 2
-
-#define SS_INFO         4
-#define STATISTICS_INFO 5
-#define CM_INDICATION   6
-#define PARAM_RESP      7
-#define BUFFER_1K       1024
-#define BUFFER_2K       (BUFFER_1K*2)
-#define BUFFER_4K       (BUFFER_2K*2)
-#define BUFFER_8K       (BUFFER_4K*2)
-#define BUFFER_16K      (BUFFER_8K*2)
-#define DOWNLINK_DIR    0
-#define UPLINK_DIR      1
-
-#define BCM_SIGNATURE   "BECEEM"
-
-
-#define GPIO_OUTPUT_REGISTER     0x0F00003C
-#define BCM_GPIO_OUTPUT_SET_REG  0x0F000040
-#define BCM_GPIO_OUTPUT_CLR_REG  0x0F000044
-#define GPIO_MODE_REGISTER       0x0F000034
-#define GPIO_PIN_STATE_REGISTER  0x0F000038
-
-struct bcm_link_state {
-	unsigned char ucLinkStatus;
-	unsigned char bIdleMode;
-	unsigned char bShutdownMode;
-};
-
-enum enLinkStatus {
-	WAIT_FOR_SYNC = 1,
-	PHY_SYNC_ACHIVED = 2,
-	LINKUP_IN_PROGRESS = 3,
-	LINKUP_DONE = 4,
-	DREG_RECEIVED = 5,
-	LINK_STATUS_RESET_RECEIVED = 6,
-	PERIODIC_WAKE_UP_NOTIFICATION_FRM_FW  = 7,
-	LINK_SHUTDOWN_REQ_FROM_FIRMWARE = 8,
-	COMPLETE_WAKE_UP_NOTIFICATION_FRM_FW = 9
-};
-
-enum bcm_phs_dsc_action {
-	eAddPHSRule = 0,
-	eSetPHSRule,
-	eDeletePHSRule,
-	eDeleteAllPHSRules
-};
-
-#define CM_CONTROL_NEWDSX_MULTICLASSIFIER_REQ  0x89 /* Host to Mac */
-#define CM_CONTROL_NEWDSX_MULTICLASSIFIER_RESP 0xA9 /* Mac to Host */
-#define MASK_DISABLE_HEADER_SUPPRESSION        0x10 /* 0b000010000 */
-#define MINIMUM_PENDING_DESCRIPTORS            5
-
-#define SHUTDOWN_HOSTINITIATED_REQUESTPAYLOAD 0xCC
-#define SHUTDOWN_ACK_FROM_DRIVER 0x1
-#define SHUTDOWN_NACK_FROM_DRIVER 0x2
-
-#define LINK_SYNC_UP_SUBTYPE   0x0001
-#define LINK_SYNC_DOWN_SUBTYPE 0x0001
-
-
-
-#define CONT_MODE 1
-#define SINGLE_DESCRIPTOR 1
-
-
-#define DESCRIPTOR_LENGTH 0x30
-#define FIRMWARE_DESCS_ADDRESS 0x1F100000
-
-
-#define CLOCK_RESET_CNTRL_REG_1 0x0F00000C
-#define CLOCK_RESET_CNTRL_REG_2 0x0F000840
-
-
-
-#define TX_DESCRIPTOR_HEAD_REGISTER 0x0F010034
-#define RX_DESCRIPTOR_HEAD_REGISTER 0x0F010094
-
-#define STATISTICS_BEGIN_ADDR        0xbf60f02c
-
-#define MAX_PENDING_CTRL_PACKET (MAX_CTRL_QUEUE_LEN-10)
-
-#define WIMAX_MAX_MTU                   (MTU_SIZE + ETH_HLEN)
-#define AUTO_LINKUP_ENABLE              0x2
-#define AUTO_SYNC_DISABLE               0x1
-#define AUTO_FIRM_DOWNLOAD              0x1
-#define SETTLE_DOWN_TIME                50
-
-#define HOST_BUS_SUSPEND_BIT            16
-
-#define IDLE_MESSAGE 0x81
-
-#define MIPS_CLOCK_133MHz 1
-
-#define TARGET_CAN_GO_TO_IDLE_MODE 2
-#define TARGET_CAN_NOT_GO_TO_IDLE_MODE 3
-#define IDLE_MODE_PAYLOAD_LENGTH 8
-
-#define IP_HEADER(Buffer) ((IPHeaderFormat *)(Buffer))
-#define IPV4 4
-#define IP_VERSION(byte) (((byte&0xF0)>>4))
-
-#define SET_MAC_ADDRESS  193
-#define SET_MAC_ADDRESS_RESPONSE 236
-
-#define IDLE_MODE_WAKEUP_PATTERN 0xd0ea1d1e
-#define IDLE_MODE_WAKEUP_NOTIFIER_ADDRESS 0x1FC02FA8
-#define IDLE_MODE_MAX_RETRY_COUNT 1000
-
-#define CONFIG_BEGIN_ADDR 0xBF60B000
-
-#define FIRMWARE_BEGIN_ADDR 0xBFC00000
-
-#define INVALID_QUEUE_INDEX NO_OF_QUEUES
-
-#define INVALID_PID ((pid_t)-1)
-#define DDR_80_MHZ  0
-#define DDR_100_MHZ 1
-#define DDR_120_MHZ 2 /* Additional Frequency for T3LP */
-#define DDR_133_MHZ 3
-#define DDR_140_MHZ 4 /* Not Used (Reserved for future) */
-#define DDR_160_MHZ 5 /* Additional Frequency for T3LP  */
-#define DDR_180_MHZ 6 /* Not Used (Reserved for future) */
-#define DDR_200_MHZ 7 /* Not Used (Reserved for future) */
-
-#define MIPS_200_MHZ   0
-#define MIPS_160_MHZ   1
-
-#define PLL_800_MHZ    0
-#define PLL_266_MHZ    1
-
-#define DEVICE_POWERSAVE_MODE_AS_MANUAL_CLOCK_GATING        0
-#define DEVICE_POWERSAVE_MODE_AS_PMU_CLOCK_GATING           1
-#define DEVICE_POWERSAVE_MODE_AS_PMU_SHUTDOWN               2
-#define DEVICE_POWERSAVE_MODE_AS_RESERVED                   3
-#define DEVICE_POWERSAVE_MODE_AS_PROTOCOL_IDLE_MODE         4
-
-
-#define EEPROM_REJECT_REG_1 0x0f003018
-#define EEPROM_REJECT_REG_2 0x0f00301c
-#define EEPROM_REJECT_REG_3 0x0f003008
-#define EEPROM_REJECT_REG_4 0x0f003020
-#define EEPROM_REJECT_MASK  0x0fffffff
-#define VSG_MODE            0x3
-
-/* Idle Mode Related Registers */
-#define DEBUG_INTERRUPT_GENERATOR_REGISTOR 0x0F00007C
-#define SW_ABORT_IDLEMODE_LOC 0x0FF01FFC
-
-#define SW_ABORT_IDLEMODE_PATTERN 0xd0ea1d1e
-#define DEVICE_INT_OUT_EP_REG0    0x0F011870
-#define DEVICE_INT_OUT_EP_REG1    0x0F011874
-
-#define BIN_FILE "/lib/firmware/macxvi200.bin"
-#define CFG_FILE "/lib/firmware/macxvi.cfg"
-#define SF_MAX_ALLOWED_PACKETS_TO_BACKUP 128
-#define MIN_VAL(x, y) ((x) < (y) ? (x) : (y))
-#define MAC_ADDRESS_SIZE 6
-#define EEPROM_COMMAND_Q_REG    0x0F003018
-#define EEPROM_READ_DATA_Q_REG  0x0F003020
-#define CHIP_ID_REG             0x0F000000
-#define GPIO_MODE_REG           0x0F000034
-#define GPIO_OUTPUT_REG         0x0F00003C
-#define WIMAX_MAX_ALLOWED_RATE  (1024*1024*50)
-
-#define T3 0xbece0300
-#define TARGET_SFID_TXDESC_MAP_LOC 0xBFFFF400
-
-#define RWM_READ 0
-#define RWM_WRITE 1
-
-#define T3LPB      0xbece3300
-#define BCS220_2   0xbece3311
-#define BCS220_2BC 0xBECE3310
-#define BCS250_BC  0xbece3301
-#define BCS220_3   0xbece3321
-
-
-#define HPM_CONFIG_LDO145 0x0F000D54
-#define HPM_CONFIG_MSW    0x0F000D58
-
-#define T3B 0xbece0310
-enum bcm_nvm_type {
-	NVM_AUTODETECT = 0,
-	NVM_EEPROM,
-	NVM_FLASH,
-	NVM_UNKNOWN
-};
-
-enum bcm_pmu_modes {
-	HYBRID_MODE_7C  = 0,
-	INTERNAL_MODE_6 = 1,
-	HYBRID_MODE_6   = 2
-};
-
-#define MAX_RDM_WRM_RETIRES 1
-
-enum eAbortPattern {
-	ABORT_SHUTDOWN_MODE = 1,
-	ABORT_IDLE_REG = 1,
-	ABORT_IDLE_MODE = 2,
-	ABORT_IDLE_SYNCDOWN = 3
-};
-
-
-/* Offsets used by driver in skb cb variable */
-#define SKB_CB_CLASSIFICATION_OFFSET    0
-#define SKB_CB_LATENCY_OFFSET           1
-#define SKB_CB_TCPACK_OFFSET            2
-
-#endif /* __MACROS_H__ */
diff --git a/drivers/staging/bcm/Makefile b/drivers/staging/bcm/Makefile
deleted file mode 100644
index 652b7f87737c..000000000000
--- a/drivers/staging/bcm/Makefile
+++ /dev/null
@@ -1,12 +0,0 @@
-#
-# Makefile for Beceem USB Wimax card
-#
-
-obj-$(CONFIG_BCM_WIMAX) +=	bcm_wimax.o
-
-bcm_wimax-y :=  InterfaceDld.o InterfaceIdleMode.o InterfaceInit.o InterfaceRx.o \
-		InterfaceIsr.o InterfaceMisc.o InterfaceTx.o \
-		CmHost.o IPv6Protocol.o Qos.o Transmit.o\
-		Bcmnet.o DDRInit.o HandleControlPacket.o\
-		LeakyBucket.o Misc.o sort.o Bcmchar.o hostmibs.o PHSModule.o\
-		led_control.o nvm.o vendorspecificextn.o
diff --git a/drivers/staging/bcm/Misc.c b/drivers/staging/bcm/Misc.c
deleted file mode 100644
index 883f7394dee6..000000000000
--- a/drivers/staging/bcm/Misc.c
+++ /dev/null
@@ -1,1587 +0,0 @@
-#include "headers.h"
-
-static int BcmFileDownload(struct bcm_mini_adapter *Adapter, const char *path, unsigned int loc);
-static void doPowerAutoCorrection(struct bcm_mini_adapter *psAdapter);
-static void HandleShutDownModeRequest(struct bcm_mini_adapter *Adapter, PUCHAR pucBuffer);
-static int bcm_parse_target_params(struct bcm_mini_adapter *Adapter);
-static void beceem_protocol_reset(struct bcm_mini_adapter *Adapter);
-
-static void default_wimax_protocol_initialize(struct bcm_mini_adapter *Adapter)
-{
-	unsigned int uiLoopIndex;
-
-	for (uiLoopIndex = 0; uiLoopIndex < NO_OF_QUEUES-1; uiLoopIndex++) {
-		Adapter->PackInfo[uiLoopIndex].uiThreshold = TX_PACKET_THRESHOLD;
-		Adapter->PackInfo[uiLoopIndex].uiMaxAllowedRate = MAX_ALLOWED_RATE;
-		Adapter->PackInfo[uiLoopIndex].uiMaxBucketSize = 20*1024*1024;
-	}
-
-	Adapter->BEBucketSize = BE_BUCKET_SIZE;
-	Adapter->rtPSBucketSize = rtPS_BUCKET_SIZE;
-	Adapter->LinkStatus = SYNC_UP_REQUEST;
-	Adapter->TransferMode = IP_PACKET_ONLY_MODE;
-	Adapter->usBestEffortQueueIndex = -1;
-}
-
-int InitAdapter(struct bcm_mini_adapter *psAdapter)
-{
-	int i = 0;
-	int Status = STATUS_SUCCESS;
-
-	BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Initialising Adapter = %p", psAdapter);
-
-	if (psAdapter == NULL) {
-		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Adapter is NULL");
-		return -EINVAL;
-	}
-
-	sema_init(&psAdapter->NVMRdmWrmLock, 1);
-	sema_init(&psAdapter->rdmwrmsync, 1);
-	spin_lock_init(&psAdapter->control_queue_lock);
-	spin_lock_init(&psAdapter->txtransmitlock);
-	sema_init(&psAdapter->RxAppControlQueuelock, 1);
-	sema_init(&psAdapter->fw_download_sema, 1);
-	sema_init(&psAdapter->LowPowerModeSync, 1);
-
-	for (i = 0; i < NO_OF_QUEUES; i++)
-		spin_lock_init(&psAdapter->PackInfo[i].SFQueueLock);
-	i = 0;
-
-	init_waitqueue_head(&psAdapter->process_rx_cntrlpkt);
-	init_waitqueue_head(&psAdapter->tx_packet_wait_queue);
-	init_waitqueue_head(&psAdapter->process_read_wait_queue);
-	init_waitqueue_head(&psAdapter->ioctl_fw_dnld_wait_queue);
-	init_waitqueue_head(&psAdapter->lowpower_mode_wait_queue);
-	psAdapter->waiting_to_fw_download_done = TRUE;
-	psAdapter->fw_download_done = false;
-
-	default_wimax_protocol_initialize(psAdapter);
-	for (i = 0; i < MAX_CNTRL_PKTS; i++) {
-		psAdapter->txctlpacket[i] = kmalloc(MAX_CNTL_PKT_SIZE, GFP_KERNEL);
-		if (!psAdapter->txctlpacket[i]) {
-			BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "No More Cntl pkts got, max got is %d", i);
-			return -ENOMEM;
-		}
-	}
-
-	if (AllocAdapterDsxBuffer(psAdapter)) {
-		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Failed to allocate DSX buffers");
-		return -EINVAL;
-	}
-
-	/* Initialize PHS interface */
-	if (phs_init(&psAdapter->stBCMPhsContext, psAdapter) != 0) {
-		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "%s:%s:%d:Error PHS Init Failed=====>\n", __FILE__, __func__, __LINE__);
-		return -ENOMEM;
-	}
-
-	Status = BcmAllocFlashCSStructure(psAdapter);
-	if (Status) {
-		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Memory Allocation for Flash structure failed");
-		return Status;
-	}
-
-	Status = vendorextnInit(psAdapter);
-
-	if (STATUS_SUCCESS != Status) {
-		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Vendor Init Failed");
-		return Status;
-	}
-
-	BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Adapter initialised");
-
-	return STATUS_SUCCESS;
-}
-
-void AdapterFree(struct bcm_mini_adapter *Adapter)
-{
-	int count;
-
-	beceem_protocol_reset(Adapter);
-	vendorextnExit(Adapter);
-
-	if (Adapter->control_packet_handler && !IS_ERR(Adapter->control_packet_handler))
-		kthread_stop(Adapter->control_packet_handler);
-
-	if (Adapter->transmit_packet_thread && !IS_ERR(Adapter->transmit_packet_thread))
-		kthread_stop(Adapter->transmit_packet_thread);
-
-	wake_up(&Adapter->process_read_wait_queue);
-
-	if (Adapter->LEDInfo.led_thread_running & (BCM_LED_THREAD_RUNNING_ACTIVELY | BCM_LED_THREAD_RUNNING_INACTIVELY))
-		kthread_stop(Adapter->LEDInfo.led_cntrl_threadid);
-
-	unregister_networkdev(Adapter);
-
-	/* FIXME: use proper wait_event and refcounting */
-	while (atomic_read(&Adapter->ApplicationRunning)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Waiting for Application to close.. %d\n", atomic_read(&Adapter->ApplicationRunning));
-		msleep(100);
-	}
-	unregister_control_device_interface(Adapter);
-	kfree(Adapter->pstargetparams);
-
-	for (count = 0; count < MAX_CNTRL_PKTS; count++)
-		kfree(Adapter->txctlpacket[count]);
-
-	FreeAdapterDsxBuffer(Adapter);
-	kfree(Adapter->pvInterfaceAdapter);
-
-	/* Free the PHS Interface */
-	PhsCleanup(&Adapter->stBCMPhsContext);
-
-	BcmDeAllocFlashCSStructure(Adapter);
-
-	free_netdev(Adapter->dev);
-}
-
-static int create_worker_threads(struct bcm_mini_adapter *psAdapter)
-{
-	/* Rx Control Packets Processing */
-	psAdapter->control_packet_handler = kthread_run((int (*)(void *))
-							control_packet_handler, psAdapter, "%s-rx", DRV_NAME);
-	if (IS_ERR(psAdapter->control_packet_handler)) {
-		pr_notice(DRV_NAME ": could not create control thread\n");
-		return PTR_ERR(psAdapter->control_packet_handler);
-	}
-
-	/* Tx Thread */
-	psAdapter->transmit_packet_thread = kthread_run((int (*)(void *))
-							tx_pkt_handler, psAdapter, "%s-tx", DRV_NAME);
-	if (IS_ERR(psAdapter->transmit_packet_thread)) {
-		pr_notice(DRV_NAME ": could not creat transmit thread\n");
-		kthread_stop(psAdapter->control_packet_handler);
-		return PTR_ERR(psAdapter->transmit_packet_thread);
-	}
-	return 0;
-}
-
-static struct file *open_firmware_file(struct bcm_mini_adapter *Adapter, const char *path)
-{
-	struct file *flp = filp_open(path, O_RDONLY, S_IRWXU);
-
-	if (IS_ERR(flp)) {
-		pr_err(DRV_NAME "Unable To Open File %s, err %ld", path, PTR_ERR(flp));
-		flp = NULL;
-	}
-
-	if (Adapter->device_removed)
-		flp = NULL;
-
-	return flp;
-}
-
-/* Arguments:
- * Logical Adapter
- * Path to image file
- * Download Address on the chip
- */
-static int BcmFileDownload(struct bcm_mini_adapter *Adapter, const char *path, unsigned int loc)
-{
-	int errorno = 0;
-	struct file *flp = NULL;
-	struct timeval tv = {0};
-
-	flp = open_firmware_file(Adapter, path);
-	if (!flp) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Unable to Open %s\n", path);
-		return -ENOENT;
-	}
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Opened file is = %s and length =0x%lx to be downloaded at =0x%x", path, (unsigned long)file_inode(flp)->i_size, loc);
-	do_gettimeofday(&tv);
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "download start %lx", ((tv.tv_sec * 1000) + (tv.tv_usec / 1000)));
-	if (Adapter->bcm_file_download(Adapter->pvInterfaceAdapter, flp, loc)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Failed to download the firmware with error %x!!!", -EIO);
-		errorno = -EIO;
-		goto exit_download;
-	}
-	vfs_llseek(flp, 0, 0);
-	if (Adapter->bcm_file_readback_from_chip(Adapter->pvInterfaceAdapter, flp, loc)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Failed to read back firmware!");
-		errorno = -EIO;
-		goto exit_download;
-	}
-
-exit_download:
-	filp_close(flp, NULL);
-	return errorno;
-}
-
-/**
- * @ingroup ctrl_pkt_functions
- * This function copies the contents of given buffer
- * to the control packet and queues it for transmission.
- * @note Do not acquire the spinlock, as it it already acquired.
- * @return  SUCCESS/FAILURE.
- * Arguments:
- * Logical Adapter
- * Control Packet Buffer
- */
-int CopyBufferToControlPacket(struct bcm_mini_adapter *Adapter, void *ioBuffer)
-{
-	struct bcm_leader *pLeader = NULL;
-	int Status = 0;
-	unsigned char *ctrl_buff;
-	unsigned int pktlen = 0;
-	struct bcm_link_request *pLinkReq = NULL;
-	PUCHAR pucAddIndication = NULL;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "======>");
-	if (!ioBuffer) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Got Null Buffer\n");
-		return -EINVAL;
-	}
-
-	pLinkReq = (struct bcm_link_request *)ioBuffer;
-	pLeader = (struct bcm_leader *)ioBuffer; /* ioBuffer Contains sw_Status and Payload */
-
-	if (Adapter->bShutStatus == TRUE &&
-		pLinkReq->szData[0] == LINK_DOWN_REQ_PAYLOAD &&
-		pLinkReq->szData[1] == LINK_SYNC_UP_SUBTYPE) {
-
-		/* Got sync down in SHUTDOWN..we could not process this. */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "SYNC DOWN Request in Shut Down Mode..\n");
-		return STATUS_FAILURE;
-	}
-
-	if ((pLeader->Status == LINK_UP_CONTROL_REQ) &&
-		((pLinkReq->szData[0] == LINK_UP_REQ_PAYLOAD &&
-			(pLinkReq->szData[1] == LINK_SYNC_UP_SUBTYPE)) || /* Sync Up Command */
-			pLinkReq->szData[0] == NETWORK_ENTRY_REQ_PAYLOAD)) /* Net Entry Command */ {
-
-		if (Adapter->LinkStatus > PHY_SYNC_ACHIVED) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "LinkStatus is Greater than PHY_SYN_ACHIEVED");
-			return STATUS_FAILURE;
-		}
-
-		if (Adapter->bShutStatus == TRUE) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "SYNC UP IN SHUTDOWN..Device WakeUp\n");
-			if (Adapter->bTriedToWakeUpFromlowPowerMode == false) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Waking up for the First Time..\n");
-				Adapter->usIdleModePattern = ABORT_SHUTDOWN_MODE; /* change it to 1 for current support. */
-				Adapter->bWakeUpDevice = TRUE;
-				wake_up(&Adapter->process_rx_cntrlpkt);
-				Status = wait_event_interruptible_timeout(Adapter->lowpower_mode_wait_queue, !Adapter->bShutStatus, (5 * HZ));
-
-				if (Status == -ERESTARTSYS)
-					return Status;
-
-				if (Adapter->bShutStatus) {
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Shutdown Mode Wake up Failed - No Wake Up Received\n");
-					return STATUS_FAILURE;
-				}
-			} else {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Wakeup has been tried already...\n");
-			}
-		}
-	}
-
-	if (Adapter->IdleMode == TRUE) {
-		/* BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Device is in Idle mode ... hence\n"); */
-		if (pLeader->Status == LINK_UP_CONTROL_REQ || pLeader->Status == 0x80 ||
-			pLeader->Status == CM_CONTROL_NEWDSX_MULTICLASSIFIER_REQ) {
-
-			if ((pLeader->Status == LINK_UP_CONTROL_REQ) && (pLinkReq->szData[0] == LINK_DOWN_REQ_PAYLOAD))	{
-				if (pLinkReq->szData[1] == LINK_SYNC_DOWN_SUBTYPE) {
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Link Down Sent in Idle Mode\n");
-					Adapter->usIdleModePattern = ABORT_IDLE_SYNCDOWN; /* LINK DOWN sent in Idle Mode */
-				} else {
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "ABORT_IDLE_MODE pattern is being written\n");
-					Adapter->usIdleModePattern = ABORT_IDLE_REG;
-				}
-			} else {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "ABORT_IDLE_MODE pattern is being written\n");
-				Adapter->usIdleModePattern = ABORT_IDLE_MODE;
-			}
-
-			/*Setting bIdleMode_tx_from_host to TRUE to indicate LED control thread to represent
-			 *  the wake up from idlemode is from host
-			 */
-			/* Adapter->LEDInfo.bIdleMode_tx_from_host = TRUE; */
-			Adapter->bWakeUpDevice = TRUE;
-			wake_up(&Adapter->process_rx_cntrlpkt);
-
-			/* We should not send DREG message down while in idlemode. */
-			if (LINK_DOWN_REQ_PAYLOAD == pLinkReq->szData[0])
-				return STATUS_SUCCESS;
-
-			Status = wait_event_interruptible_timeout(Adapter->lowpower_mode_wait_queue, !Adapter->IdleMode, (5 * HZ));
-
-			if (Status == -ERESTARTSYS)
-				return Status;
-
-			if (Adapter->IdleMode) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Idle Mode Wake up Failed - No Wake Up Received\n");
-				return STATUS_FAILURE;
-			}
-		} else {
-			return STATUS_SUCCESS;
-		}
-	}
-
-	/* The Driver has to send control messages with a particular VCID */
-	pLeader->Vcid = VCID_CONTROL_PACKET; /* VCID for control packet. */
-
-	/* Allocate skb for Control Packet */
-	pktlen = pLeader->PLength;
-	ctrl_buff = (char *)Adapter->txctlpacket[atomic_read(&Adapter->index_wr_txcntrlpkt)%MAX_CNTRL_PKTS];
-
-	if (!ctrl_buff) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "mem allocation Failed");
-		return -ENOMEM;
-	}
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Control packet to be taken =%d and address is =%pincoming address is =%p and packet len=%x",
-			atomic_read(&Adapter->index_wr_txcntrlpkt), ctrl_buff, ioBuffer, pktlen);
-
-	if (pLeader) {
-		if ((pLeader->Status == 0x80) ||
-			(pLeader->Status == CM_CONTROL_NEWDSX_MULTICLASSIFIER_REQ)) {
-			/*
-			 * Restructure the DSX message to handle Multiple classifier Support
-			 * Write the Service Flow param Structures directly to the target
-			 * and embed the pointers in the DSX messages sent to target.
-			 */
-			/* Lets store the current length of the control packet we are transmitting */
-			pucAddIndication = (PUCHAR)ioBuffer + LEADER_SIZE;
-			pktlen = pLeader->PLength;
-			Status = StoreCmControlResponseMessage(Adapter, pucAddIndication, &pktlen);
-			if (Status != 1) {
-				ClearTargetDSXBuffer(Adapter, ((struct bcm_add_indication_alt *)pucAddIndication)->u16TID, false);
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, " Error Restoring The DSX Control Packet. Dsx Buffers on Target may not be Setup Properly ");
-				return STATUS_FAILURE;
-			}
-			/*
-			 * update the leader to use the new length
-			 * The length of the control packet is length of message being sent + Leader length
-			 */
-			pLeader->PLength = pktlen;
-		}
-	}
-
-	if (pktlen + LEADER_SIZE > MAX_CNTL_PKT_SIZE)
-		return -EINVAL;
-
-	memset(ctrl_buff, 0, pktlen+LEADER_SIZE);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Copying the Control Packet Buffer with length=%d\n", pLeader->PLength);
-	*(struct bcm_leader *)ctrl_buff = *pLeader;
-	memcpy(ctrl_buff + LEADER_SIZE, ((PUCHAR)ioBuffer + LEADER_SIZE), pLeader->PLength);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Enqueuing the Control Packet");
-
-	/* Update the statistics counters */
-	spin_lock_bh(&Adapter->PackInfo[HiPriority].SFQueueLock);
-	Adapter->PackInfo[HiPriority].uiCurrentBytesOnHost += pLeader->PLength;
-	Adapter->PackInfo[HiPriority].uiCurrentPacketsOnHost++;
-	atomic_inc(&Adapter->TotalPacketCount);
-	spin_unlock_bh(&Adapter->PackInfo[HiPriority].SFQueueLock);
-	Adapter->PackInfo[HiPriority].bValid = TRUE;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "CurrBytesOnHost: %x bValid: %x",
-			Adapter->PackInfo[HiPriority].uiCurrentBytesOnHost,
-			Adapter->PackInfo[HiPriority].bValid);
-	Status = STATUS_SUCCESS;
-	/*Queue the packet for transmission */
-	atomic_inc(&Adapter->index_wr_txcntrlpkt);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Calling transmit_packets");
-	atomic_set(&Adapter->TxPktAvail, 1);
-	wake_up(&Adapter->tx_packet_wait_queue);
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "<====");
-	return Status;
-}
-
-/******************************************************************
-* Function    - LinkMessage()
-*
-* Description - This function builds the Sync-up and Link-up request
-* packet messages depending on the device Link status.
-*
-* Parameters  - Adapter:	Pointer to the Adapter structure.
-*
-* Returns     - None.
-*******************************************************************/
-void LinkMessage(struct bcm_mini_adapter *Adapter)
-{
-	struct bcm_link_request *pstLinkRequest = NULL;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LINK_UP_MSG, DBG_LVL_ALL, "=====>");
-	if (Adapter->LinkStatus == SYNC_UP_REQUEST && Adapter->AutoSyncup) {
-		pstLinkRequest = kzalloc(sizeof(struct bcm_link_request), GFP_ATOMIC);
-		if (!pstLinkRequest) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LINK_UP_MSG, DBG_LVL_ALL, "Can not allocate memory for Link request!");
-			return;
-		}
-		/* sync up request... */
-		Adapter->LinkStatus = WAIT_FOR_SYNC; /* current link status */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LINK_UP_MSG, DBG_LVL_ALL, "Requesting For SyncUp...");
-		pstLinkRequest->szData[0] = LINK_UP_REQ_PAYLOAD;
-		pstLinkRequest->szData[1] = LINK_SYNC_UP_SUBTYPE;
-		pstLinkRequest->Leader.Status = LINK_UP_CONTROL_REQ;
-		pstLinkRequest->Leader.PLength = sizeof(ULONG);
-		Adapter->bSyncUpRequestSent = TRUE;
-
-	} else if (Adapter->LinkStatus == PHY_SYNC_ACHIVED && Adapter->AutoLinkUp) {
-		pstLinkRequest = kzalloc(sizeof(struct bcm_link_request), GFP_ATOMIC);
-		if (!pstLinkRequest) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LINK_UP_MSG, DBG_LVL_ALL, "Can not allocate memory for Link request!");
-			return;
-		}
-		/* LINK_UP_REQUEST */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LINK_UP_MSG, DBG_LVL_ALL, "Requesting For LinkUp...");
-		pstLinkRequest->szData[0] = LINK_UP_REQ_PAYLOAD;
-		pstLinkRequest->szData[1] = LINK_NET_ENTRY;
-		pstLinkRequest->Leader.Status = LINK_UP_CONTROL_REQ;
-		pstLinkRequest->Leader.PLength = sizeof(ULONG);
-	}
-	if (pstLinkRequest) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LINK_UP_MSG, DBG_LVL_ALL, "Calling CopyBufferToControlPacket");
-		CopyBufferToControlPacket(Adapter, pstLinkRequest);
-		kfree(pstLinkRequest);
-	}
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LINK_UP_MSG, DBG_LVL_ALL, "LinkMessage <=====");
-	return;
-}
-
-/**********************************************************************
-* Function    - StatisticsResponse()
-*
-* Description - This function handles the Statistics response packet.
-*
-* Parameters  - Adapter	: Pointer to the Adapter structure.
-* - pvBuffer: Starting address of Statistic response data.
-*
-* Returns     - None.
-************************************************************************/
-void StatisticsResponse(struct bcm_mini_adapter *Adapter, void *pvBuffer)
-{
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "%s====>", __func__);
-	Adapter->StatisticsPointer = ntohl(*(__be32 *)pvBuffer);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "Stats at %x", (unsigned int)Adapter->StatisticsPointer);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "%s <====", __func__);
-}
-
-/**********************************************************************
-* Function    - LinkControlResponseMessage()
-*
-* Description - This function handles the Link response packets.
-*
-* Parameters  - Adapter	 : Pointer to the Adapter structure.
-* - pucBuffer: Starting address of Link response data.
-*
-* Returns     - None.
-***********************************************************************/
-void LinkControlResponseMessage(struct bcm_mini_adapter *Adapter, PUCHAR pucBuffer)
-{
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_RX, RX_DPC, DBG_LVL_ALL, "=====>");
-
-	if (*pucBuffer == LINK_UP_ACK) {
-		switch (*(pucBuffer+1)) {
-		case PHY_SYNC_ACHIVED: /* SYNCed UP */
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "PHY_SYNC_ACHIVED");
-
-				if (Adapter->LinkStatus == LINKUP_DONE)
-					beceem_protocol_reset(Adapter);
-
-				Adapter->usBestEffortQueueIndex = INVALID_QUEUE_INDEX;
-				Adapter->LinkStatus = PHY_SYNC_ACHIVED;
-
-				if (Adapter->LEDInfo.led_thread_running & BCM_LED_THREAD_RUNNING_ACTIVELY) {
-					Adapter->DriverState = NO_NETWORK_ENTRY;
-					wake_up(&Adapter->LEDInfo.notify_led_event);
-				}
-
-				LinkMessage(Adapter);
-				break;
-
-		case LINKUP_DONE:
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_RX, RX_DPC, DBG_LVL_ALL, "LINKUP_DONE");
-			Adapter->LinkStatus = LINKUP_DONE;
-			Adapter->bPHSEnabled = *(pucBuffer+3);
-			Adapter->bETHCSEnabled = *(pucBuffer+4) & ETH_CS_MASK;
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "PHS Support Status Received In LinkUp Ack : %x\n", Adapter->bPHSEnabled);
-
-			if ((false == Adapter->bShutStatus) && (false == Adapter->IdleMode)) {
-				if (Adapter->LEDInfo.led_thread_running & BCM_LED_THREAD_RUNNING_ACTIVELY) {
-					Adapter->DriverState = NORMAL_OPERATION;
-					wake_up(&Adapter->LEDInfo.notify_led_event);
-				}
-			}
-			LinkMessage(Adapter);
-			break;
-
-		case WAIT_FOR_SYNC:
-			/*
-			 * Driver to ignore the DREG_RECEIVED
-			 * WiMAX Application should handle this Message
-			 */
-			/* Adapter->liTimeSinceLastNetEntry = 0; */
-			Adapter->LinkUpStatus = 0;
-			Adapter->LinkStatus = 0;
-			Adapter->usBestEffortQueueIndex = INVALID_QUEUE_INDEX;
-			Adapter->bTriedToWakeUpFromlowPowerMode = false;
-			Adapter->IdleMode = false;
-			beceem_protocol_reset(Adapter);
-
-			break;
-		case LINK_SHUTDOWN_REQ_FROM_FIRMWARE:
-		case COMPLETE_WAKE_UP_NOTIFICATION_FRM_FW:
-		{
-			HandleShutDownModeRequest(Adapter, pucBuffer);
-		}
-		break;
-		default:
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "default case:LinkResponse %x", *(pucBuffer + 1));
-			break;
-		}
-	} else if (SET_MAC_ADDRESS_RESPONSE == *pucBuffer) {
-		PUCHAR puMacAddr = (pucBuffer + 1);
-
-		Adapter->LinkStatus = SYNC_UP_REQUEST;
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_RX, RX_DPC, DBG_LVL_ALL, "MAC address response, sending SYNC_UP");
-		LinkMessage(Adapter);
-		memcpy(Adapter->dev->dev_addr, puMacAddr, MAC_ADDRESS_SIZE);
-	}
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_RX, RX_DPC, DBG_LVL_ALL, "%s <=====", __func__);
-}
-
-void SendIdleModeResponse(struct bcm_mini_adapter *Adapter)
-{
-	int status = 0, NVMAccess = 0, lowPwrAbortMsg = 0;
-	struct timeval tv;
-	struct bcm_link_request stIdleResponse = {{0} };
-
-	memset(&tv, 0, sizeof(tv));
-	stIdleResponse.Leader.Status = IDLE_MESSAGE;
-	stIdleResponse.Leader.PLength = IDLE_MODE_PAYLOAD_LENGTH;
-	stIdleResponse.szData[0] = GO_TO_IDLE_MODE_PAYLOAD;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_RX, RX_DPC, DBG_LVL_ALL, " ============>");
-
-	/*********************************
-	 *down_trylock -
-	 * if [ semaphore is available ]
-	 *		 acquire semaphone and return value 0 ;
-	 *   else
-	 *		 return non-zero value ;
-	 *
-	 ***********************************/
-
-	NVMAccess = down_trylock(&Adapter->NVMRdmWrmLock);
-	lowPwrAbortMsg = down_trylock(&Adapter->LowPowerModeSync);
-
-
-	if ((NVMAccess || lowPwrAbortMsg || atomic_read(&Adapter->TotalPacketCount)) &&
-		(Adapter->ulPowerSaveMode != DEVICE_POWERSAVE_MODE_AS_PROTOCOL_IDLE_MODE)) {
-
-		if (!NVMAccess)
-			up(&Adapter->NVMRdmWrmLock);
-
-		if (!lowPwrAbortMsg)
-			up(&Adapter->LowPowerModeSync);
-
-		stIdleResponse.szData[1] = TARGET_CAN_NOT_GO_TO_IDLE_MODE; /* NACK- device access is going on. */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_RX, RX_DPC, DBG_LVL_ALL, "HOST IS NACKING Idle mode To F/W!!!!!!!!");
-		Adapter->bPreparingForLowPowerMode = false;
-	} else {
-		stIdleResponse.szData[1] = TARGET_CAN_GO_TO_IDLE_MODE; /* 2; Idle ACK */
-		Adapter->StatisticsPointer = 0;
-
-		/* Wait for the LED to TURN OFF before sending ACK response */
-		if (Adapter->LEDInfo.led_thread_running & BCM_LED_THREAD_RUNNING_ACTIVELY) {
-			int iRetVal = 0;
-
-			/* Wake the LED Thread with IDLEMODE_ENTER State */
-			Adapter->DriverState = LOWPOWER_MODE_ENTER;
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_RX, RX_DPC, DBG_LVL_ALL, "LED Thread is Running..Hence Setting LED Event as IDLEMODE_ENTER jiffies:%ld", jiffies);
-			wake_up(&Adapter->LEDInfo.notify_led_event);
-
-			/* Wait for 1 SEC for LED to OFF */
-			iRetVal = wait_event_timeout(Adapter->LEDInfo.idleModeSyncEvent, Adapter->LEDInfo.bIdle_led_off, msecs_to_jiffies(1000));
-
-			/* If Timed Out to Sync IDLE MODE Enter, do IDLE mode Exit and Send NACK to device */
-			if (iRetVal <= 0) {
-				stIdleResponse.szData[1] = TARGET_CAN_NOT_GO_TO_IDLE_MODE; /* NACK- device access is going on. */
-				Adapter->DriverState = NORMAL_OPERATION;
-				wake_up(&Adapter->LEDInfo.notify_led_event);
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_RX, RX_DPC, DBG_LVL_ALL, "NACKING Idle mode as time out happen from LED side!!!!!!!!");
-			}
-		}
-
-		if (stIdleResponse.szData[1] == TARGET_CAN_GO_TO_IDLE_MODE) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_RX, RX_DPC, DBG_LVL_ALL, "ACKING IDLE MODE !!!!!!!!!");
-			down(&Adapter->rdmwrmsync);
-			Adapter->bPreparingForLowPowerMode = TRUE;
-			up(&Adapter->rdmwrmsync);
-			/* Killing all URBS. */
-			if (Adapter->bDoSuspend == TRUE)
-				Bcm_kill_all_URBs((struct bcm_interface_adapter *)(Adapter->pvInterfaceAdapter));
-		} else {
-			Adapter->bPreparingForLowPowerMode = false;
-		}
-
-		if (!NVMAccess)
-			up(&Adapter->NVMRdmWrmLock);
-
-		if (!lowPwrAbortMsg)
-			up(&Adapter->LowPowerModeSync);
-	}
-
-	status = CopyBufferToControlPacket(Adapter, &stIdleResponse);
-	if (status != STATUS_SUCCESS) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "fail to send the Idle mode Request\n");
-		Adapter->bPreparingForLowPowerMode = false;
-		StartInterruptUrb((struct bcm_interface_adapter *)(Adapter->pvInterfaceAdapter));
-	}
-	do_gettimeofday(&tv);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_RX, RX_DPC, DBG_LVL_ALL, "IdleMode Msg submitter to Q :%ld ms", tv.tv_sec * 1000 + tv.tv_usec / 1000);
-}
-
-/******************************************************************
-* Function    - DumpPackInfo()
-*
-* Description - This function dumps the all Queue(PackInfo[]) details.
-*
-* Parameters  - Adapter: Pointer to the Adapter structure.
-*
-* Returns     - None.
-*******************************************************************/
-void DumpPackInfo(struct bcm_mini_adapter *Adapter)
-{
-	unsigned int uiLoopIndex = 0;
-	unsigned int uiIndex = 0;
-	unsigned int uiClsfrIndex = 0;
-	struct bcm_classifier_rule *pstClassifierEntry = NULL;
-
-	for (uiLoopIndex = 0; uiLoopIndex < NO_OF_QUEUES; uiLoopIndex++) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "*********** Showing Details Of Queue %d***** ******", uiLoopIndex);
-		if (false == Adapter->PackInfo[uiLoopIndex].bValid) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "bValid is false for %X index\n", uiLoopIndex);
-			continue;
-		}
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, " Dumping	SF Rule Entry For SFID %lX\n", Adapter->PackInfo[uiLoopIndex].ulSFID);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, " ucDirection %X\n", Adapter->PackInfo[uiLoopIndex].ucDirection);
-
-		if (Adapter->PackInfo[uiLoopIndex].ucIpVersion == IPV6)
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "Ipv6 Service Flow\n");
-		else
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "Ipv4 Service Flow\n");
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "SF Traffic Priority %X\n", Adapter->PackInfo[uiLoopIndex].u8TrafficPriority);
-
-		for (uiClsfrIndex = 0; uiClsfrIndex < MAX_CLASSIFIERS; uiClsfrIndex++) {
-			pstClassifierEntry = &Adapter->astClassifierTable[uiClsfrIndex];
-			if (!pstClassifierEntry->bUsed)
-				continue;
-
-			if (pstClassifierEntry->ulSFID != Adapter->PackInfo[uiLoopIndex].ulSFID)
-				continue;
-
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tDumping Classifier Rule Entry For Index: %X Classifier Rule ID : %X\n", uiClsfrIndex, pstClassifierEntry->uiClassifierRuleIndex);
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tDumping Classifier Rule Entry For Index: %X usVCID_Value : %X\n", uiClsfrIndex, pstClassifierEntry->usVCID_Value);
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tDumping Classifier Rule Entry For Index: %X bProtocolValid : %X\n", uiClsfrIndex, pstClassifierEntry->bProtocolValid);
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tDumping Classifier Rule Entry For Index: %X bTOSValid : %X\n", uiClsfrIndex, pstClassifierEntry->bTOSValid);
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tDumping Classifier Rule Entry For Index: %X bDestIpValid : %X\n", uiClsfrIndex, pstClassifierEntry->bDestIpValid);
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tDumping Classifier Rule Entry For Index: %X bSrcIpValid : %X\n", uiClsfrIndex, pstClassifierEntry->bSrcIpValid);
-
-			for (uiIndex = 0; uiIndex < MAX_PORT_RANGE; uiIndex++) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tusSrcPortRangeLo:%X\n", pstClassifierEntry->usSrcPortRangeLo[uiIndex]);
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tusSrcPortRangeHi:%X\n", pstClassifierEntry->usSrcPortRangeHi[uiIndex]);
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tusDestPortRangeLo:%X\n", pstClassifierEntry->usDestPortRangeLo[uiIndex]);
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tusDestPortRangeHi:%X\n", pstClassifierEntry->usDestPortRangeHi[uiIndex]);
-			}
-
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tucIPSourceAddressLength : 0x%x\n", pstClassifierEntry->ucIPSourceAddressLength);
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tucIPDestinationAddressLength : 0x%x\n", pstClassifierEntry->ucIPDestinationAddressLength);
-			for (uiIndex = 0; uiIndex < pstClassifierEntry->ucIPSourceAddressLength; uiIndex++) {
-				if (Adapter->PackInfo[uiLoopIndex].ucIpVersion == IPV6)	{
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tIpv6 ulSrcIpAddr :\n");
-					DumpIpv6Address(pstClassifierEntry->stSrcIpAddress.ulIpv6Addr);
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tIpv6 ulSrcIpMask :\n");
-					DumpIpv6Address(pstClassifierEntry->stSrcIpAddress.ulIpv6Mask);
-				} else {
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tulSrcIpAddr:%lX\n", pstClassifierEntry->stSrcIpAddress.ulIpv4Addr[uiIndex]);
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tulSrcIpMask:%lX\n", pstClassifierEntry->stSrcIpAddress.ulIpv4Mask[uiIndex]);
-				}
-			}
-
-			for (uiIndex = 0; uiIndex < pstClassifierEntry->ucIPDestinationAddressLength; uiIndex++) {
-				if (Adapter->PackInfo[uiLoopIndex].ucIpVersion == IPV6) {
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tIpv6 ulDestIpAddr :\n");
-					DumpIpv6Address(pstClassifierEntry->stDestIpAddress.ulIpv6Addr);
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tIpv6 ulDestIpMask :\n");
-					DumpIpv6Address(pstClassifierEntry->stDestIpAddress.ulIpv6Mask);
-				} else {
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tulDestIpAddr:%lX\n", pstClassifierEntry->stDestIpAddress.ulIpv4Addr[uiIndex]);
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tulDestIpMask:%lX\n", pstClassifierEntry->stDestIpAddress.ulIpv4Mask[uiIndex]);
-				}
-			}
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tucProtocol:0x%X\n", pstClassifierEntry->ucProtocol[0]);
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\tu8ClassifierRulePriority:%X\n", pstClassifierEntry->u8ClassifierRulePriority);
-		}
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "ulSFID:%lX\n", Adapter->PackInfo[uiLoopIndex].ulSFID);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "usVCID_Value:%X\n", Adapter->PackInfo[uiLoopIndex].usVCID_Value);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "PhsEnabled: 0x%X\n", Adapter->PackInfo[uiLoopIndex].bHeaderSuppressionEnabled);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiThreshold:%X\n", Adapter->PackInfo[uiLoopIndex].uiThreshold);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "bValid:%X\n", Adapter->PackInfo[uiLoopIndex].bValid);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "bActive:%X\n", Adapter->PackInfo[uiLoopIndex].bActive);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "ActivateReqSent: %x", Adapter->PackInfo[uiLoopIndex].bActivateRequestSent);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "u8QueueType:%X\n", Adapter->PackInfo[uiLoopIndex].u8QueueType);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiMaxBucketSize:%X\n", Adapter->PackInfo[uiLoopIndex].uiMaxBucketSize);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiPerSFTxResourceCount:%X\n", atomic_read(&Adapter->PackInfo[uiLoopIndex].uiPerSFTxResourceCount));
-		/* DumpDebug(DUMP_INFO,("bCSSupport:%X\n",Adapter->PackInfo[uiLoopIndex].bCSSupport)); */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "CurrQueueDepthOnTarget: %x\n", Adapter->PackInfo[uiLoopIndex].uiCurrentQueueDepthOnTarget);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiCurrentBytesOnHost:%X\n", Adapter->PackInfo[uiLoopIndex].uiCurrentBytesOnHost);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiCurrentPacketsOnHost:%X\n", Adapter->PackInfo[uiLoopIndex].uiCurrentPacketsOnHost);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiDroppedCountBytes:%X\n", Adapter->PackInfo[uiLoopIndex].uiDroppedCountBytes);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiDroppedCountPackets:%X\n", Adapter->PackInfo[uiLoopIndex].uiDroppedCountPackets);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiSentBytes:%X\n", Adapter->PackInfo[uiLoopIndex].uiSentBytes);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiSentPackets:%X\n", Adapter->PackInfo[uiLoopIndex].uiSentPackets);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiCurrentDrainRate:%X\n", Adapter->PackInfo[uiLoopIndex].uiCurrentDrainRate);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiThisPeriodSentBytes:%X\n", Adapter->PackInfo[uiLoopIndex].uiThisPeriodSentBytes);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "liDrainCalculated:%llX\n", Adapter->PackInfo[uiLoopIndex].liDrainCalculated);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiCurrentTokenCount:%X\n", Adapter->PackInfo[uiLoopIndex].uiCurrentTokenCount);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "liLastUpdateTokenAt:%llX\n", Adapter->PackInfo[uiLoopIndex].liLastUpdateTokenAt);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiMaxAllowedRate:%X\n", Adapter->PackInfo[uiLoopIndex].uiMaxAllowedRate);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiPendedLast:%X\n", Adapter->PackInfo[uiLoopIndex].uiPendedLast);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "NumOfPacketsSent:%X\n", Adapter->PackInfo[uiLoopIndex].NumOfPacketsSent);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "Direction: %x\n", Adapter->PackInfo[uiLoopIndex].ucDirection);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "CID: %x\n", Adapter->PackInfo[uiLoopIndex].usCID);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "ProtocolValid: %x\n", Adapter->PackInfo[uiLoopIndex].bProtocolValid);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "TOSValid: %x\n", Adapter->PackInfo[uiLoopIndex].bTOSValid);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "DestIpValid: %x\n", Adapter->PackInfo[uiLoopIndex].bDestIpValid);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "SrcIpValid: %x\n", Adapter->PackInfo[uiLoopIndex].bSrcIpValid);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "ActiveSet: %x\n", Adapter->PackInfo[uiLoopIndex].bActiveSet);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "AdmittedSet: %x\n", Adapter->PackInfo[uiLoopIndex].bAdmittedSet);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "AuthzSet: %x\n", Adapter->PackInfo[uiLoopIndex].bAuthorizedSet);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "ClassifyPrority: %x\n", Adapter->PackInfo[uiLoopIndex].bClassifierPriority);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiMaxLatency: %x\n", Adapter->PackInfo[uiLoopIndex].uiMaxLatency);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO,
-				DBG_LVL_ALL, "ServiceClassName: %*ph\n",
-				4, Adapter->PackInfo[uiLoopIndex].
-					    ucServiceClassName);
-/* BCM_DEBUG_PRINT (Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "bHeaderSuppressionEnabled :%X\n", Adapter->PackInfo[uiLoopIndex].bHeaderSuppressionEnabled);
- * BCM_DEBUG_PRINT (Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiTotalTxBytes:%X\n", Adapter->PackInfo[uiLoopIndex].uiTotalTxBytes);
- * BCM_DEBUG_PRINT (Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "uiTotalRxBytes:%X\n", Adapter->PackInfo[uiLoopIndex].uiTotalRxBytes);
- *		DumpDebug(DUMP_INFO,("				uiRanOutOfResCount:%X\n",Adapter->PackInfo[uiLoopIndex].uiRanOutOfResCount));
- */
-	}
-
-	for (uiLoopIndex = 0; uiLoopIndex < MIBS_MAX_HIST_ENTRIES; uiLoopIndex++)
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "Adapter->aRxPktSizeHist[%x] = %x\n", uiLoopIndex, Adapter->aRxPktSizeHist[uiLoopIndex]);
-
-	for (uiLoopIndex = 0; uiLoopIndex < MIBS_MAX_HIST_ENTRIES; uiLoopIndex++)
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "Adapter->aTxPktSizeHist[%x] = %x\n", uiLoopIndex, Adapter->aTxPktSizeHist[uiLoopIndex]);
-}
-
-int reset_card_proc(struct bcm_mini_adapter *ps_adapter)
-{
-	int retval = STATUS_SUCCESS;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	struct bcm_interface_adapter *psIntfAdapter = NULL;
-	unsigned int value = 0, uiResetValue = 0;
-	int bytes;
-
-	psIntfAdapter = ((struct bcm_interface_adapter *)(ps_adapter->pvInterfaceAdapter));
-	ps_adapter->bDDRInitDone = false;
-
-	if (ps_adapter->chip_id >= T3LPB) {
-		/* SYS_CFG register is write protected hence for modifying this reg value, it should be read twice before */
-		rdmalt(ps_adapter, SYS_CFG, &value, sizeof(value));
-		rdmalt(ps_adapter, SYS_CFG, &value, sizeof(value));
-
-		/* making bit[6...5] same as was before f/w download. this setting force the h/w to */
-		/* re-populated the SP RAM area with the string descriptor. */
-		value = value | (ps_adapter->syscfgBefFwDld & 0x00000060);
-		wrmalt(ps_adapter, SYS_CFG, &value, sizeof(value));
-	}
-
-	/* killing all submitted URBs. */
-	psIntfAdapter->psAdapter->StopAllXaction = TRUE;
-	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, "Resetting UMA-B\n");
-		retval = usb_reset_device(psIntfAdapter->udev);
-		psIntfAdapter->psAdapter->StopAllXaction = false;
-
-		if (retval != STATUS_SUCCESS) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Reset failed with ret value :%d", retval);
-			goto err_exit;
-		}
-
-		if (ps_adapter->chip_id == BCS220_2 ||
-			ps_adapter->chip_id == BCS220_2BC ||
-			ps_adapter->chip_id == BCS250_BC ||
-			ps_adapter->chip_id == BCS220_3) {
-
-			bytes = rdmalt(ps_adapter, HPM_CONFIG_LDO145, &value, sizeof(value));
-			if (bytes < 0) {
-				retval = bytes;
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "read failed with status :%d", retval);
-				goto err_exit;
-			}
-			/* setting 0th bit */
-			value |= (1<<0);
-			retval = wrmalt(ps_adapter, HPM_CONFIG_LDO145, &value, sizeof(value));
-			if (retval < 0) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "write failed with status :%d", retval);
-				goto err_exit;
-			}
-		}
-	} else {
-		bytes = rdmalt(ps_adapter, 0x0f007018, &value, sizeof(value));
-		if (bytes < 0) {
-			retval = bytes;
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "read failed with status :%d", retval);
-			goto err_exit;
-		}
-		value &= (~(1<<16));
-		retval = wrmalt(ps_adapter, 0x0f007018, &value, sizeof(value));
-		if (retval < 0) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "write failed with status :%d", retval);
-			goto err_exit;
-		}
-
-		/* Toggling the GPIO 8, 9 */
-		value = 0;
-		retval = wrmalt(ps_adapter, GPIO_OUTPUT_REGISTER, &value, sizeof(value));
-		if (retval < 0) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "write failed with status :%d", retval);
-			goto err_exit;
-		}
-		value = 0x300;
-		retval = wrmalt(ps_adapter, GPIO_MODE_REGISTER, &value, sizeof(value));
-		if (retval < 0) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "write failed with status :%d", retval);
-			goto err_exit;
-		}
-		mdelay(50);
-	}
-
-	/* ps_adapter->downloadDDR = false; */
-	if (ps_adapter->bFlashBoot) {
-		/* In flash boot mode MIPS state register has reverse polarity.
-		 * So just or with setting bit 30.
-		 * Make the MIPS in Reset state.
-		 */
-		rdmalt(ps_adapter, CLOCK_RESET_CNTRL_REG_1, &uiResetValue, sizeof(uiResetValue));
-		uiResetValue |= (1<<30);
-		wrmalt(ps_adapter, CLOCK_RESET_CNTRL_REG_1, &uiResetValue, sizeof(uiResetValue));
-	}
-
-	if (ps_adapter->chip_id >= T3LPB) {
-		uiResetValue = 0;
-		/*
-		 * WA for SYSConfig Issue.
-		 * Read SYSCFG Twice to make it writable.
-		 */
-		rdmalt(ps_adapter, SYS_CFG, &uiResetValue, sizeof(uiResetValue));
-		if (uiResetValue & (1<<4)) {
-			uiResetValue = 0;
-			rdmalt(ps_adapter, SYS_CFG, &uiResetValue, sizeof(uiResetValue)); /* 2nd read to make it writable. */
-			uiResetValue &= (~(1<<4));
-			wrmalt(ps_adapter, SYS_CFG, &uiResetValue, sizeof(uiResetValue));
-		}
-	}
-	uiResetValue = 0;
-	wrmalt(ps_adapter, 0x0f01186c, &uiResetValue, sizeof(uiResetValue));
-
-err_exit:
-	psIntfAdapter->psAdapter->StopAllXaction = false;
-	return retval;
-}
-
-int run_card_proc(struct bcm_mini_adapter *ps_adapter)
-{
-	int status = STATUS_SUCCESS;
-	int bytes;
-
-	unsigned int value = 0;
-	{
-		bytes = rdmalt(ps_adapter, CLOCK_RESET_CNTRL_REG_1, &value, sizeof(value));
-		if (bytes < 0) {
-			status = bytes;
-			BCM_DEBUG_PRINT(ps_adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "%s:%d\n", __func__, __LINE__);
-			return status;
-		}
-
-		if (ps_adapter->bFlashBoot)
-			value &= (~(1<<30));
-		else
-			value |= (1<<30);
-
-		if (wrmalt(ps_adapter, CLOCK_RESET_CNTRL_REG_1, &value, sizeof(value)) < 0) {
-			BCM_DEBUG_PRINT(ps_adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "%s:%d\n", __func__, __LINE__);
-			return STATUS_FAILURE;
-		}
-	}
-	return status;
-}
-
-int InitCardAndDownloadFirmware(struct bcm_mini_adapter *ps_adapter)
-{
-	int status;
-	unsigned int value = 0;
-	/*
-	 * Create the threads first and then download the
-	 * Firm/DDR Settings..
-	 */
-	status = create_worker_threads(ps_adapter);
-	if (status < 0)
-		return status;
-
-	status = bcm_parse_target_params(ps_adapter);
-	if (status)
-		return status;
-
-	if (ps_adapter->chip_id >= T3LPB) {
-		rdmalt(ps_adapter, SYS_CFG, &value, sizeof(value));
-		ps_adapter->syscfgBefFwDld = value;
-
-		if ((value & 0x60) == 0)
-			ps_adapter->bFlashBoot = TRUE;
-	}
-
-	reset_card_proc(ps_adapter);
-
-	/* Initializing the NVM. */
-	BcmInitNVM(ps_adapter);
-	status = ddr_init(ps_adapter);
-	if (status) {
-		pr_err(DRV_NAME "ddr_init Failed\n");
-		return status;
-	}
-
-	/* Download cfg file */
-	status = buffDnldVerify(ps_adapter,
-				(PUCHAR)ps_adapter->pstargetparams,
-				sizeof(struct bcm_target_params),
-				CONFIG_BEGIN_ADDR);
-	if (status) {
-		BCM_DEBUG_PRINT(ps_adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Error downloading CFG file");
-		goto OUT;
-	}
-
-	if (register_networkdev(ps_adapter)) {
-		BCM_DEBUG_PRINT(ps_adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Register Netdevice failed. Cleanup needs to be performed.");
-		return -EIO;
-	}
-
-	if (false == ps_adapter->AutoFirmDld) {
-		BCM_DEBUG_PRINT(ps_adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "AutoFirmDld Disabled in CFG File..\n");
-		/* If Auto f/w download is disable, register the control interface, */
-		/* register the control interface after the mailbox. */
-		if (register_control_device_interface(ps_adapter) < 0) {
-			BCM_DEBUG_PRINT(ps_adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Register Control Device failed. Cleanup needs to be performed.");
-			return -EIO;
-		}
-		return STATUS_SUCCESS;
-	}
-
-	/*
-	 * Do the LED Settings here. It will be used by the Firmware Download
-	 * Thread.
-	 */
-
-	/*
-	 * 1. If the LED Settings fails, do not stop and do the Firmware download.
-	 * 2. This init would happened only if the cfg file is present, else
-	 *    call from the ioctl context.
-	 */
-
-	status = InitLedSettings(ps_adapter);
-	if (status) {
-		BCM_DEBUG_PRINT(ps_adapter, DBG_TYPE_PRINTK, 0, 0, "INIT LED FAILED\n");
-		return status;
-	}
-
-	if (ps_adapter->LEDInfo.led_thread_running & BCM_LED_THREAD_RUNNING_ACTIVELY) {
-		ps_adapter->DriverState = DRIVER_INIT;
-		wake_up(&ps_adapter->LEDInfo.notify_led_event);
-	}
-
-	if (ps_adapter->LEDInfo.led_thread_running & BCM_LED_THREAD_RUNNING_ACTIVELY) {
-		ps_adapter->DriverState = FW_DOWNLOAD;
-		wake_up(&ps_adapter->LEDInfo.notify_led_event);
-	}
-
-	value = 0;
-	wrmalt(ps_adapter, EEPROM_CAL_DATA_INTERNAL_LOC - 4, &value, sizeof(value));
-	wrmalt(ps_adapter, EEPROM_CAL_DATA_INTERNAL_LOC - 8, &value, sizeof(value));
-
-	if (ps_adapter->eNVMType == NVM_FLASH) {
-		status = PropagateCalParamsFromFlashToMemory(ps_adapter);
-		if (status) {
-			BCM_DEBUG_PRINT(ps_adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Propagation of Cal param failed ..");
-			goto OUT;
-		}
-	}
-
-	/* Download Firmare */
-	status = BcmFileDownload(ps_adapter, BIN_FILE, FIRMWARE_BEGIN_ADDR);
-	if (status != 0) {
-		BCM_DEBUG_PRINT(ps_adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "No Firmware File is present...\n");
-		goto OUT;
-	}
-
-	status = run_card_proc(ps_adapter);
-	if (status) {
-		BCM_DEBUG_PRINT(ps_adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "run_card_proc Failed\n");
-		goto OUT;
-	}
-
-	ps_adapter->fw_download_done = TRUE;
-	mdelay(10);
-
-OUT:
-	if (ps_adapter->LEDInfo.led_thread_running & BCM_LED_THREAD_RUNNING_ACTIVELY) {
-		ps_adapter->DriverState = FW_DOWNLOAD_DONE;
-		wake_up(&ps_adapter->LEDInfo.notify_led_event);
-	}
-
-	return status;
-}
-
-static int bcm_parse_target_params(struct bcm_mini_adapter *Adapter)
-{
-	struct file *flp = NULL;
-	char *buff;
-	int len = 0;
-
-	buff = kmalloc(BUFFER_1K, GFP_KERNEL);
-	if (!buff)
-		return -ENOMEM;
-
-	Adapter->pstargetparams = kmalloc(sizeof(struct bcm_target_params), GFP_KERNEL);
-	if (Adapter->pstargetparams == NULL) {
-		kfree(buff);
-		return -ENOMEM;
-	}
-
-	flp = open_firmware_file(Adapter, CFG_FILE);
-	if (!flp) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "NOT ABLE TO OPEN THE %s FILE\n", CFG_FILE);
-		kfree(buff);
-		kfree(Adapter->pstargetparams);
-		Adapter->pstargetparams = NULL;
-		return -ENOENT;
-	}
-	len = kernel_read(flp, 0, buff, BUFFER_1K);
-	filp_close(flp, NULL);
-
-	if (len != sizeof(struct bcm_target_params)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Mismatch in Target Param Structure!\n");
-		kfree(buff);
-		kfree(Adapter->pstargetparams);
-		Adapter->pstargetparams = NULL;
-		return -ENOENT;
-	}
-
-	/* Check for autolink in config params */
-	/*
-	 * Values in Adapter->pstargetparams are in network byte order
-	 */
-	memcpy(Adapter->pstargetparams, buff, sizeof(struct bcm_target_params));
-	kfree(buff);
-	beceem_parse_target_struct(Adapter);
-	return STATUS_SUCCESS;
-}
-
-void beceem_parse_target_struct(struct bcm_mini_adapter *Adapter)
-{
-	unsigned int uiHostDrvrCfg6 = 0, uiEEPROMFlag = 0;
-
-	if (ntohl(Adapter->pstargetparams->m_u32PhyParameter2) & AUTO_SYNC_DISABLE) {
-		pr_info(DRV_NAME ": AutoSyncup is Disabled\n");
-		Adapter->AutoSyncup = false;
-	} else {
-		pr_info(DRV_NAME ": AutoSyncup is Enabled\n");
-		Adapter->AutoSyncup = TRUE;
-	}
-
-	if (ntohl(Adapter->pstargetparams->HostDrvrConfig6) & AUTO_LINKUP_ENABLE) {
-		pr_info(DRV_NAME ": Enabling autolink up");
-		Adapter->AutoLinkUp = TRUE;
-	} else {
-		pr_info(DRV_NAME ": Disabling autolink up");
-		Adapter->AutoLinkUp = false;
-	}
-	/* Setting the DDR Setting.. */
-	Adapter->DDRSetting = (ntohl(Adapter->pstargetparams->HostDrvrConfig6) >> 8)&0x0F;
-	Adapter->ulPowerSaveMode = (ntohl(Adapter->pstargetparams->HostDrvrConfig6)>>12)&0x0F;
-	pr_info(DRV_NAME ": DDR Setting: %x\n", Adapter->DDRSetting);
-	pr_info(DRV_NAME ": Power Save Mode: %lx\n", Adapter->ulPowerSaveMode);
-	if (ntohl(Adapter->pstargetparams->HostDrvrConfig6) & AUTO_FIRM_DOWNLOAD) {
-		pr_info(DRV_NAME ": Enabling Auto Firmware Download\n");
-		Adapter->AutoFirmDld = TRUE;
-	} else {
-		pr_info(DRV_NAME ": Disabling Auto Firmware Download\n");
-		Adapter->AutoFirmDld = false;
-	}
-	uiHostDrvrCfg6 = ntohl(Adapter->pstargetparams->HostDrvrConfig6);
-	Adapter->bMipsConfig = (uiHostDrvrCfg6>>20)&0x01;
-	pr_info(DRV_NAME ": MIPSConfig   : 0x%X\n", Adapter->bMipsConfig);
-	/* used for backward compatibility. */
-	Adapter->bDPLLConfig = (uiHostDrvrCfg6>>19)&0x01;
-	Adapter->PmuMode = (uiHostDrvrCfg6 >> 24) & 0x03;
-	pr_info(DRV_NAME ": PMU MODE: %x", Adapter->PmuMode);
-
-	if ((uiHostDrvrCfg6 >> HOST_BUS_SUSPEND_BIT) & (0x01)) {
-		Adapter->bDoSuspend = TRUE;
-		pr_info(DRV_NAME ": Making DoSuspend TRUE as per configFile");
-	}
-
-	uiEEPROMFlag = ntohl(Adapter->pstargetparams->m_u32EEPROMFlag);
-	pr_info(DRV_NAME ": uiEEPROMFlag  : 0x%X\n", uiEEPROMFlag);
-	Adapter->eNVMType = (enum bcm_nvm_type)((uiEEPROMFlag>>4)&0x3);
-	Adapter->bStatusWrite = (uiEEPROMFlag>>6)&0x1;
-	Adapter->uiSectorSizeInCFG = 1024*(0xFFFF & ntohl(Adapter->pstargetparams->HostDrvrConfig4));
-	Adapter->bSectorSizeOverride = (bool) ((ntohl(Adapter->pstargetparams->HostDrvrConfig4))>>16)&0x1;
-
-	if (ntohl(Adapter->pstargetparams->m_u32PowerSavingModeOptions) & 0x01)
-		Adapter->ulPowerSaveMode = DEVICE_POWERSAVE_MODE_AS_PROTOCOL_IDLE_MODE;
-
-	if (Adapter->ulPowerSaveMode != DEVICE_POWERSAVE_MODE_AS_PROTOCOL_IDLE_MODE)
-		doPowerAutoCorrection(Adapter);
-}
-
-static void doPowerAutoCorrection(struct bcm_mini_adapter *psAdapter)
-{
-	unsigned int reporting_mode;
-
-	reporting_mode = ntohl(psAdapter->pstargetparams->m_u32PowerSavingModeOptions) & 0x02;
-	psAdapter->bIsAutoCorrectEnabled = !((char)(psAdapter->ulPowerSaveMode >> 3) & 0x1);
-
-	if (reporting_mode) {
-		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "can't do suspen/resume as reporting mode is enable");
-		psAdapter->bDoSuspend = false;
-	}
-
-	if (psAdapter->bIsAutoCorrectEnabled && (psAdapter->chip_id >= T3LPB)) {
-		/* If reporting mode is enable, switch PMU to PMC */
-		{
-			psAdapter->ulPowerSaveMode = DEVICE_POWERSAVE_MODE_AS_PMU_CLOCK_GATING;
-			psAdapter->bDoSuspend = false;
-		}
-
-		/* clearing space bit[15..12] */
-		psAdapter->pstargetparams->HostDrvrConfig6 &= ~(htonl((0xF << 12)));
-		/* placing the power save mode option */
-		psAdapter->pstargetparams->HostDrvrConfig6 |= htonl((psAdapter->ulPowerSaveMode << 12));
-	} else if (psAdapter->bIsAutoCorrectEnabled == false) {
-		/* remove the autocorrect disable bit set before dumping. */
-		psAdapter->ulPowerSaveMode &= ~(1 << 3);
-		psAdapter->pstargetparams->HostDrvrConfig6 &= ~(htonl(1 << 15));
-		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Using Forced User Choice: %lx\n", psAdapter->ulPowerSaveMode);
-	}
-}
-
-static void convertEndian(unsigned char rwFlag, unsigned int *puiBuffer, unsigned int uiByteCount)
-{
-	unsigned int uiIndex = 0;
-
-	if (RWM_WRITE == rwFlag) {
-		for (uiIndex = 0; uiIndex < (uiByteCount/sizeof(unsigned int)); uiIndex++)
-			puiBuffer[uiIndex] = htonl(puiBuffer[uiIndex]);
-	} else {
-		for (uiIndex = 0; uiIndex < (uiByteCount/sizeof(unsigned int)); uiIndex++)
-			puiBuffer[uiIndex] = ntohl(puiBuffer[uiIndex]);
-	}
-}
-
-int rdm(struct bcm_mini_adapter *Adapter, unsigned int uiAddress, PCHAR pucBuff, size_t sSize)
-{
-	return Adapter->interface_rdm(Adapter->pvInterfaceAdapter,
-				uiAddress, pucBuff, sSize);
-}
-
-int wrm(struct bcm_mini_adapter *Adapter, unsigned int uiAddress, PCHAR pucBuff, size_t sSize)
-{
-	int iRetVal;
-
-	iRetVal = Adapter->interface_wrm(Adapter->pvInterfaceAdapter,
-					uiAddress, pucBuff, sSize);
-	return iRetVal;
-}
-
-int wrmalt(struct bcm_mini_adapter *Adapter, unsigned int uiAddress, unsigned int *pucBuff, size_t size)
-{
-	convertEndian(RWM_WRITE, pucBuff, size);
-	return wrm(Adapter, uiAddress, (PUCHAR)pucBuff, size);
-}
-
-int rdmalt(struct bcm_mini_adapter *Adapter, unsigned int uiAddress, unsigned int *pucBuff, size_t size)
-{
-	int uiRetVal = 0;
-
-	uiRetVal = rdm(Adapter, uiAddress, (PUCHAR)pucBuff, size);
-	convertEndian(RWM_READ, (unsigned int *)pucBuff, size);
-
-	return uiRetVal;
-}
-
-int wrmWithLock(struct bcm_mini_adapter *Adapter, unsigned int uiAddress, PCHAR pucBuff, size_t sSize)
-{
-	int status = STATUS_SUCCESS;
-
-	down(&Adapter->rdmwrmsync);
-
-	if ((Adapter->IdleMode == TRUE) ||
-		(Adapter->bShutStatus == TRUE) ||
-		(Adapter->bPreparingForLowPowerMode == TRUE)) {
-
-		status = -EACCES;
-		goto exit;
-	}
-
-	status = wrm(Adapter, uiAddress, pucBuff, sSize);
-exit:
-	up(&Adapter->rdmwrmsync);
-	return status;
-}
-
-int wrmaltWithLock(struct bcm_mini_adapter *Adapter, unsigned int uiAddress, unsigned int *pucBuff, size_t size)
-{
-	int iRetVal = STATUS_SUCCESS;
-
-	down(&Adapter->rdmwrmsync);
-
-	if ((Adapter->IdleMode == TRUE) ||
-		(Adapter->bShutStatus == TRUE) ||
-		(Adapter->bPreparingForLowPowerMode == TRUE)) {
-
-		iRetVal = -EACCES;
-		goto exit;
-	}
-
-	iRetVal = wrmalt(Adapter, uiAddress, pucBuff, size);
-exit:
-	up(&Adapter->rdmwrmsync);
-	return iRetVal;
-}
-
-int rdmaltWithLock(struct bcm_mini_adapter *Adapter, unsigned int uiAddress, unsigned int *pucBuff, size_t size)
-{
-	int uiRetVal = STATUS_SUCCESS;
-
-	down(&Adapter->rdmwrmsync);
-	if ((Adapter->IdleMode == TRUE) ||
-		(Adapter->bShutStatus == TRUE) ||
-		(Adapter->bPreparingForLowPowerMode == TRUE)) {
-
-		uiRetVal = -EACCES;
-		goto exit;
-	}
-
-	uiRetVal = rdmalt(Adapter, uiAddress, pucBuff, size);
-exit:
-	up(&Adapter->rdmwrmsync);
-	return uiRetVal;
-}
-
-static void HandleShutDownModeWakeup(struct bcm_mini_adapter *Adapter)
-{
-	int clear_abort_pattern = 0, Status = 0;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, MP_SHUTDOWN, DBG_LVL_ALL, "====>\n");
-	/* target has woken up From Shut Down */
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, MP_SHUTDOWN, DBG_LVL_ALL, "Clearing Shut Down Software abort pattern\n");
-	Status = wrmalt(Adapter, SW_ABORT_IDLEMODE_LOC, (unsigned int *)&clear_abort_pattern, sizeof(clear_abort_pattern));
-	if (Status) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, MP_SHUTDOWN, DBG_LVL_ALL, "WRM to SW_ABORT_IDLEMODE_LOC failed with err:%d", Status);
-		return;
-	}
-
-	if (Adapter->ulPowerSaveMode != DEVICE_POWERSAVE_MODE_AS_PROTOCOL_IDLE_MODE) {
-		msleep(100);
-		InterfaceHandleShutdownModeWakeup(Adapter);
-		msleep(100);
-	}
-
-	if (Adapter->LEDInfo.led_thread_running & BCM_LED_THREAD_RUNNING_ACTIVELY) {
-		Adapter->DriverState = NO_NETWORK_ENTRY;
-		wake_up(&Adapter->LEDInfo.notify_led_event);
-	}
-
-	Adapter->bTriedToWakeUpFromlowPowerMode = false;
-	Adapter->bShutStatus = false;
-	wake_up(&Adapter->lowpower_mode_wait_queue);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, MP_SHUTDOWN, DBG_LVL_ALL, "<====\n");
-}
-
-static void SendShutModeResponse(struct bcm_mini_adapter *Adapter)
-{
-	struct bcm_link_request stShutdownResponse;
-	unsigned int NVMAccess = 0, lowPwrAbortMsg = 0;
-	unsigned int Status = 0;
-
-	memset(&stShutdownResponse, 0, sizeof(struct bcm_link_request));
-	stShutdownResponse.Leader.Status  = LINK_UP_CONTROL_REQ;
-	stShutdownResponse.Leader.PLength = 8; /* 8 bytes; */
-	stShutdownResponse.szData[0] = LINK_UP_ACK;
-	stShutdownResponse.szData[1] = LINK_SHUTDOWN_REQ_FROM_FIRMWARE;
-
-	/*********************************
-	 * down_trylock -
-	 * if [ semaphore is available ]
-	 *		 acquire semaphone and return value 0 ;
-	 *   else
-	 *		 return non-zero value ;
-	 *
-	 ***********************************/
-
-	NVMAccess = down_trylock(&Adapter->NVMRdmWrmLock);
-	lowPwrAbortMsg = down_trylock(&Adapter->LowPowerModeSync);
-
-	if (NVMAccess || lowPwrAbortMsg || atomic_read(&Adapter->TotalPacketCount)) {
-		if (!NVMAccess)
-			up(&Adapter->NVMRdmWrmLock);
-
-		if (!lowPwrAbortMsg)
-			up(&Adapter->LowPowerModeSync);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, MP_SHUTDOWN, DBG_LVL_ALL, "Device Access is going on NACK the Shut Down MODE\n");
-		stShutdownResponse.szData[2] = SHUTDOWN_NACK_FROM_DRIVER; /* NACK- device access is going on. */
-		Adapter->bPreparingForLowPowerMode = false;
-	} else {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, MP_SHUTDOWN, DBG_LVL_ALL, "Sending SHUTDOWN MODE ACK\n");
-		stShutdownResponse.szData[2] = SHUTDOWN_ACK_FROM_DRIVER; /* ShutDown ACK */
-
-		/* Wait for the LED to TURN OFF before sending ACK response */
-		if (Adapter->LEDInfo.led_thread_running & BCM_LED_THREAD_RUNNING_ACTIVELY) {
-			int iRetVal = 0;
-
-			/* Wake the LED Thread with LOWPOWER_MODE_ENTER State */
-			Adapter->DriverState = LOWPOWER_MODE_ENTER;
-			wake_up(&Adapter->LEDInfo.notify_led_event);
-
-			/* Wait for 1 SEC for LED to OFF */
-			iRetVal = wait_event_timeout(Adapter->LEDInfo.idleModeSyncEvent, Adapter->LEDInfo.bIdle_led_off, msecs_to_jiffies(1000));
-
-			/* If Timed Out to Sync IDLE MODE Enter, do IDLE mode Exit and Send NACK to device */
-			if (iRetVal <= 0) {
-				stShutdownResponse.szData[1] = SHUTDOWN_NACK_FROM_DRIVER; /* NACK- device access is going on. */
-				Adapter->DriverState = NO_NETWORK_ENTRY;
-				wake_up(&Adapter->LEDInfo.notify_led_event);
-			}
-		}
-
-		if (stShutdownResponse.szData[2] == SHUTDOWN_ACK_FROM_DRIVER) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, MP_SHUTDOWN, DBG_LVL_ALL, "ACKING SHUTDOWN MODE !!!!!!!!!");
-			down(&Adapter->rdmwrmsync);
-			Adapter->bPreparingForLowPowerMode = TRUE;
-			up(&Adapter->rdmwrmsync);
-			/* Killing all URBS. */
-			if (Adapter->bDoSuspend == TRUE)
-				Bcm_kill_all_URBs((struct bcm_interface_adapter *)(Adapter->pvInterfaceAdapter));
-		} else {
-			Adapter->bPreparingForLowPowerMode = false;
-		}
-
-		if (!NVMAccess)
-			up(&Adapter->NVMRdmWrmLock);
-
-		if (!lowPwrAbortMsg)
-			up(&Adapter->LowPowerModeSync);
-	}
-
-	Status = CopyBufferToControlPacket(Adapter, &stShutdownResponse);
-	if (Status != STATUS_SUCCESS) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, MP_SHUTDOWN, DBG_LVL_ALL, "fail to send the Idle mode Request\n");
-		Adapter->bPreparingForLowPowerMode = false;
-		StartInterruptUrb((struct bcm_interface_adapter *)(Adapter->pvInterfaceAdapter));
-	}
-}
-
-static void HandleShutDownModeRequest(struct bcm_mini_adapter *Adapter, PUCHAR pucBuffer)
-{
-	unsigned int uiResetValue = 0;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, MP_SHUTDOWN, DBG_LVL_ALL, "====>\n");
-
-	if (*(pucBuffer+1) ==  COMPLETE_WAKE_UP_NOTIFICATION_FRM_FW) {
-		HandleShutDownModeWakeup(Adapter);
-	} else if (*(pucBuffer+1) ==  LINK_SHUTDOWN_REQ_FROM_FIRMWARE) {
-		/* Target wants to go to Shut Down Mode */
-		/* InterfacePrepareForShutdown(Adapter); */
-		if (Adapter->chip_id == BCS220_2 ||
-			Adapter->chip_id == BCS220_2BC ||
-			Adapter->chip_id == BCS250_BC ||
-			Adapter->chip_id == BCS220_3) {
-
-			rdmalt(Adapter, HPM_CONFIG_MSW, &uiResetValue, 4);
-			uiResetValue |= (1<<17);
-			wrmalt(Adapter, HPM_CONFIG_MSW, &uiResetValue, 4);
-		}
-
-		SendShutModeResponse(Adapter);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, MP_SHUTDOWN, DBG_LVL_ALL, "ShutDownModeResponse:Notification received: Sending the response(Ack/Nack)\n");
-	}
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, MP_SHUTDOWN, DBG_LVL_ALL, "<====\n");
-}
-
-void ResetCounters(struct bcm_mini_adapter *Adapter)
-{
-	beceem_protocol_reset(Adapter);
-	Adapter->CurrNumRecvDescs = 0;
-	Adapter->PrevNumRecvDescs = 0;
-	Adapter->LinkUpStatus = 0;
-	Adapter->LinkStatus = 0;
-	atomic_set(&Adapter->cntrlpktCnt, 0);
-	atomic_set(&Adapter->TotalPacketCount, 0);
-	Adapter->fw_download_done = false;
-	Adapter->LinkStatus = 0;
-	Adapter->AutoLinkUp = false;
-	Adapter->IdleMode = false;
-	Adapter->bShutStatus = false;
-}
-
-struct bcm_classifier_rule *GetFragIPClsEntry(struct bcm_mini_adapter *Adapter, USHORT usIpIdentification, ULONG SrcIP)
-{
-	unsigned int uiIndex = 0;
-
-	for (uiIndex = 0; uiIndex < MAX_FRAGMENTEDIP_CLASSIFICATION_ENTRIES; uiIndex++) {
-		if ((Adapter->astFragmentedPktClassifierTable[uiIndex].bUsed) &&
-			(Adapter->astFragmentedPktClassifierTable[uiIndex].usIpIdentification == usIpIdentification) &&
-			(Adapter->astFragmentedPktClassifierTable[uiIndex].ulSrcIpAddress == SrcIP) &&
-			!Adapter->astFragmentedPktClassifierTable[uiIndex].bOutOfOrderFragment)
-
-			return Adapter->astFragmentedPktClassifierTable[uiIndex].pstMatchedClassifierEntry;
-	}
-	return NULL;
-}
-
-void AddFragIPClsEntry(struct bcm_mini_adapter *Adapter, struct bcm_fragmented_packet_info *psFragPktInfo)
-{
-	unsigned int uiIndex = 0;
-
-	for (uiIndex = 0; uiIndex < MAX_FRAGMENTEDIP_CLASSIFICATION_ENTRIES; uiIndex++) {
-		if (!Adapter->astFragmentedPktClassifierTable[uiIndex].bUsed) {
-			memcpy(&Adapter->astFragmentedPktClassifierTable[uiIndex], psFragPktInfo, sizeof(struct bcm_fragmented_packet_info));
-			break;
-		}
-	}
-}
-
-void DelFragIPClsEntry(struct bcm_mini_adapter *Adapter, USHORT usIpIdentification, ULONG SrcIp)
-{
-	unsigned int uiIndex = 0;
-
-	for (uiIndex = 0; uiIndex < MAX_FRAGMENTEDIP_CLASSIFICATION_ENTRIES; uiIndex++) {
-		if ((Adapter->astFragmentedPktClassifierTable[uiIndex].bUsed) &&
-			(Adapter->astFragmentedPktClassifierTable[uiIndex].usIpIdentification == usIpIdentification) &&
-			(Adapter->astFragmentedPktClassifierTable[uiIndex].ulSrcIpAddress == SrcIp))
-
-			memset(&Adapter->astFragmentedPktClassifierTable[uiIndex], 0, sizeof(struct bcm_fragmented_packet_info));
-	}
-}
-
-void update_per_cid_rx(struct bcm_mini_adapter *Adapter)
-{
-	unsigned int qindex = 0;
-
-	if ((jiffies - Adapter->liDrainCalculated) < XSECONDS)
-		return;
-
-	for (qindex = 0; qindex < HiPriority; qindex++) {
-		if (Adapter->PackInfo[qindex].ucDirection == 0) {
-			Adapter->PackInfo[qindex].uiCurrentRxRate =
-				(Adapter->PackInfo[qindex].uiCurrentRxRate +
-					Adapter->PackInfo[qindex].uiThisPeriodRxBytes) / 2;
-
-			Adapter->PackInfo[qindex].uiThisPeriodRxBytes = 0;
-		} else {
-			Adapter->PackInfo[qindex].uiCurrentDrainRate =
-				(Adapter->PackInfo[qindex].uiCurrentDrainRate +
-					Adapter->PackInfo[qindex].uiThisPeriodSentBytes) / 2;
-			Adapter->PackInfo[qindex].uiThisPeriodSentBytes = 0;
-		}
-	}
-	Adapter->liDrainCalculated = jiffies;
-}
-
-void update_per_sf_desc_cnts(struct bcm_mini_adapter *Adapter)
-{
-	int iIndex = 0;
-	u32 uibuff[MAX_TARGET_DSX_BUFFERS];
-	int bytes;
-
-	if (!atomic_read(&Adapter->uiMBupdate))
-		return;
-
-	bytes = rdmaltWithLock(Adapter, TARGET_SFID_TXDESC_MAP_LOC, (unsigned int *)uibuff, sizeof(unsigned int) * MAX_TARGET_DSX_BUFFERS);
-	if (bytes < 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "rdm failed\n");
-		return;
-	}
-
-	for (iIndex = 0; iIndex < HiPriority; iIndex++) {
-		if (Adapter->PackInfo[iIndex].bValid && Adapter->PackInfo[iIndex].ucDirection) {
-			if (Adapter->PackInfo[iIndex].usVCID_Value < MAX_TARGET_DSX_BUFFERS)
-				atomic_set(&Adapter->PackInfo[iIndex].uiPerSFTxResourceCount, uibuff[Adapter->PackInfo[iIndex].usVCID_Value]);
-			else
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Invalid VCID : %x\n", Adapter->PackInfo[iIndex].usVCID_Value);
-		}
-	}
-	atomic_set(&Adapter->uiMBupdate, false);
-}
-
-void flush_queue(struct bcm_mini_adapter *Adapter, unsigned int iQIndex)
-{
-	struct sk_buff *PacketToDrop = NULL;
-	struct net_device_stats *netstats = &Adapter->dev->stats;
-
-	spin_lock_bh(&Adapter->PackInfo[iQIndex].SFQueueLock);
-
-	while (Adapter->PackInfo[iQIndex].FirstTxQueue && atomic_read(&Adapter->TotalPacketCount)) {
-		PacketToDrop = Adapter->PackInfo[iQIndex].FirstTxQueue;
-		if (PacketToDrop && PacketToDrop->len) {
-			netstats->tx_dropped++;
-			DEQUEUEPACKET(Adapter->PackInfo[iQIndex].FirstTxQueue, Adapter->PackInfo[iQIndex].LastTxQueue);
-			Adapter->PackInfo[iQIndex].uiCurrentPacketsOnHost--;
-			Adapter->PackInfo[iQIndex].uiCurrentBytesOnHost -= PacketToDrop->len;
-
-			/* Adding dropped statistics */
-			Adapter->PackInfo[iQIndex].uiDroppedCountBytes += PacketToDrop->len;
-			Adapter->PackInfo[iQIndex].uiDroppedCountPackets++;
-			dev_kfree_skb(PacketToDrop);
-			atomic_dec(&Adapter->TotalPacketCount);
-		}
-	}
-	spin_unlock_bh(&Adapter->PackInfo[iQIndex].SFQueueLock);
-}
-
-static void beceem_protocol_reset(struct bcm_mini_adapter *Adapter)
-{
-	int i;
-
-	if (netif_msg_link(Adapter))
-		pr_notice(PFX "%s: protocol reset\n", Adapter->dev->name);
-
-	netif_carrier_off(Adapter->dev);
-	netif_stop_queue(Adapter->dev);
-
-	Adapter->IdleMode = false;
-	Adapter->LinkUpStatus = false;
-	ClearTargetDSXBuffer(Adapter, 0, TRUE);
-	/* Delete All Classifier Rules */
-
-	for (i = 0; i < HiPriority; i++)
-		DeleteAllClassifiersForSF(Adapter, i);
-
-	flush_all_queues(Adapter);
-
-	if (Adapter->TimerActive == TRUE)
-		Adapter->TimerActive = false;
-
-	memset(Adapter->astFragmentedPktClassifierTable, 0, sizeof(struct bcm_fragmented_packet_info) * MAX_FRAGMENTEDIP_CLASSIFICATION_ENTRIES);
-
-	for (i = 0; i < HiPriority; i++) {
-		/* resetting only the first size (S_MIBS_SERVICEFLOW_TABLE) for the SF. */
-		/* It is same between MIBs and SF. */
-		memset(&Adapter->PackInfo[i].stMibsExtServiceFlowTable, 0, sizeof(struct bcm_mibs_parameters));
-	}
-}
diff --git a/drivers/staging/bcm/PHSDefines.h b/drivers/staging/bcm/PHSDefines.h
deleted file mode 100644
index cd78ee4ffa22..000000000000
--- a/drivers/staging/bcm/PHSDefines.h
+++ /dev/null
@@ -1,94 +0,0 @@
-#ifndef BCM_PHS_DEFINES_H
-#define BCM_PHS_DEFINES_H
-
-#define PHS_INVALID_TABLE_INDEX	0xffffffff
-#define PHS_MEM_TAG "_SHP"
-
-/* PHS Defines */
-#define STATUS_PHS_COMPRESSED		0xa1
-#define STATUS_PHS_NOCOMPRESSION	0xa2
-#define APPLY_PHS			1
-#define MAX_NO_BIT			7
-#define ZERO_PHSI			0
-#define VERIFY				0
-#define SIZE_MULTIPLE_32		4
-#define UNCOMPRESSED_PACKET		0
-#define DYNAMIC				0
-#define SUPPRESS			0x80
-#define NO_CLASSIFIER_MATCH		0
-#define SEND_PACKET_UNCOMPRESSED	0
-#define PHSI_IS_ZERO			0
-#define PHSI_LEN			1
-#define ERROR_LEN			0
-#define PHS_BUFFER_SIZE			1532
-#define MAX_PHSRULE_PER_SF		20
-#define MAX_SERVICEFLOWS		17
-
-/* PHS Error Defines */
-#define PHS_SUCCESS				0
-#define ERR_PHS_INVALID_DEVICE_EXETENSION	0x800
-#define ERR_PHS_INVALID_PHS_RULE		0x801
-#define ERR_PHS_RULE_ALREADY_EXISTS		0x802
-#define ERR_SF_MATCH_FAIL			0x803
-#define ERR_INVALID_CLASSIFIERTABLE_FOR_SF	0x804
-#define ERR_SFTABLE_FULL			0x805
-#define ERR_CLSASSIFIER_TABLE_FULL		0x806
-#define ERR_PHSRULE_MEMALLOC_FAIL		0x807
-#define ERR_CLSID_MATCH_FAIL			0x808
-#define ERR_PHSRULE_MATCH_FAIL			0x809
-
-struct bcm_phs_rule {
-	u8 u8PHSI;
-	u8 u8PHSFLength;
-	u8 u8PHSF[MAX_PHS_LENGTHS];
-	u8 u8PHSMLength;
-	u8 u8PHSM[MAX_PHS_LENGTHS];
-	u8 u8PHSS;
-	u8 u8PHSV;
-	u8 u8RefCnt;
-	u8 bUnclassifiedPHSRule;
-	u8 u8Reserved[3];
-	long PHSModifiedBytes;
-	unsigned long PHSModifiedNumPackets;
-	unsigned long PHSErrorNumPackets;
-};
-
-enum bcm_phs_classifier_context {
-	eActiveClassifierRuleContext,
-	eOldClassifierRuleContext
-};
-
-struct bcm_phs_classifier_entry {
-	u8  bUsed;
-	u16 uiClassifierRuleId;
-	u8  u8PHSI;
-	struct bcm_phs_rule *pstPhsRule;
-	u8  bUnclassifiedPHSRule;
-};
-
-struct bcm_phs_classifier_table {
-	u16 uiTotalClassifiers;
-	struct bcm_phs_classifier_entry stActivePhsRulesList[MAX_PHSRULE_PER_SF];
-	struct bcm_phs_classifier_entry stOldPhsRulesList[MAX_PHSRULE_PER_SF];
-	u16 uiOldestPhsRuleIndex;
-};
-
-struct bcm_phs_entry {
-	u8  bUsed;
-	u16 uiVcid;
-	struct bcm_phs_classifier_table *pstClassifierTable;
-};
-
-struct bcm_phs_table {
-	u16 uiTotalServiceFlows;
-	struct bcm_phs_entry stSFList[MAX_SERVICEFLOWS];
-};
-
-struct bcm_phs_extension {
-	/* PHS Specific data */
-	struct bcm_phs_table *pstServiceFlowPhsRulesTable;
-	void *CompressedTxBuffer;
-	void *UnCompressedRxBuffer;
-};
-
-#endif
diff --git a/drivers/staging/bcm/PHSModule.c b/drivers/staging/bcm/PHSModule.c
deleted file mode 100644
index 5f4e503d54ec..000000000000
--- a/drivers/staging/bcm/PHSModule.c
+++ /dev/null
@@ -1,1703 +0,0 @@
-#include "headers.h"
-
-static UINT CreateSFToClassifierRuleMapping(B_UINT16 uiVcid,
-					    B_UINT16 uiClsId,
-					    struct bcm_phs_table *psServiceFlowTable,
-					    struct bcm_phs_rule *psPhsRule,
-					    B_UINT8 u8AssociatedPHSI);
-
-static UINT CreateClassiferToPHSRuleMapping(B_UINT16 uiVcid,
-					    B_UINT16  uiClsId,
-					    struct bcm_phs_entry *pstServiceFlowEntry,
-					    struct bcm_phs_rule *psPhsRule,
-					    B_UINT8 u8AssociatedPHSI);
-
-static UINT CreateClassifierPHSRule(B_UINT16  uiClsId,
-				    struct bcm_phs_classifier_table *psaClassifiertable,
-				    struct bcm_phs_rule *psPhsRule,
-				    enum bcm_phs_classifier_context eClsContext,
-				    B_UINT8 u8AssociatedPHSI);
-
-static UINT UpdateClassifierPHSRule(B_UINT16 uiClsId,
-				    struct bcm_phs_classifier_entry *pstClassifierEntry,
-				    struct bcm_phs_classifier_table *psaClassifiertable,
-				    struct bcm_phs_rule *psPhsRule,
-				    B_UINT8 u8AssociatedPHSI);
-
-static bool ValidatePHSRuleComplete(const struct bcm_phs_rule *psPhsRule);
-
-static bool DerefPhsRule(B_UINT16 uiClsId,
-			 struct bcm_phs_classifier_table *psaClassifiertable,
-			 struct bcm_phs_rule *pstPhsRule);
-
-static UINT GetClassifierEntry(struct bcm_phs_classifier_table *pstClassifierTable,
-			       B_UINT32 uiClsid,
-			       enum bcm_phs_classifier_context eClsContext,
-			       struct bcm_phs_classifier_entry **ppstClassifierEntry);
-
-static UINT GetPhsRuleEntry(struct bcm_phs_classifier_table *pstClassifierTable,
-			    B_UINT32 uiPHSI,
-			    enum bcm_phs_classifier_context eClsContext,
-			    struct bcm_phs_rule **ppstPhsRule);
-
-static void free_phs_serviceflow_rules(struct bcm_phs_table *psServiceFlowRulesTable);
-
-static int phs_compress(struct bcm_phs_rule *phs_members,
-			unsigned char *in_buf,
-			unsigned char *out_buf,
-			unsigned int *header_size,
-			UINT *new_header_size);
-
-static int verify_suppress_phsf(unsigned char *in_buffer,
-				unsigned char *out_buffer,
-				unsigned char *phsf,
-				unsigned char *phsm,
-				unsigned int phss,
-				unsigned int phsv,
-				UINT *new_header_size);
-
-static int phs_decompress(unsigned char *in_buf,
-			  unsigned char *out_buf,
-			  struct bcm_phs_rule *phs_rules,
-			  UINT *header_size);
-
-static ULONG PhsCompress(void *pvContext,
-			 B_UINT16 uiVcid,
-			 B_UINT16 uiClsId,
-			 void *pvInputBuffer,
-			 void *pvOutputBuffer,
-			 UINT *pOldHeaderSize,
-			 UINT *pNewHeaderSize);
-
-static ULONG PhsDeCompress(void *pvContext,
-			   B_UINT16 uiVcid,
-			   void *pvInputBuffer,
-			   void *pvOutputBuffer,
-			   UINT *pInHeaderSize,
-			   UINT *pOutHeaderSize);
-
-#define IN
-#define OUT
-
-/*
- * Function: PHSTransmit
- * Description:	This routine handle PHS(Payload Header Suppression for Tx path.
- *	It extracts a fragment of the NDIS_PACKET containing the header
- *	to be suppressed. It then suppresses the header by invoking PHS exported compress routine.
- *	The header data after suppression is copied back to the NDIS_PACKET.
- *
- * Input parameters: IN struct bcm_mini_adapter *Adapter         - Miniport Adapter Context
- *	IN Packet - NDIS packet containing data to be transmitted
- *	IN USHORT Vcid - vcid pertaining to connection on which the packet is being sent.Used to
- *		identify PHS rule to be applied.
- *	B_UINT16 uiClassifierRuleID - Classifier Rule ID
- *	BOOLEAN bHeaderSuppressionEnabled - indicates if header suprression is enabled for SF.
- *
- * Return:	STATUS_SUCCESS - If the send was successful.
- *	Other  - If an error occurred.
- */
-
-int PHSTransmit(struct bcm_mini_adapter *Adapter,
-		struct sk_buff **pPacket,
-		USHORT Vcid,
-		B_UINT16 uiClassifierRuleID,
-		bool bHeaderSuppressionEnabled,
-		UINT *PacketLen,
-		UCHAR bEthCSSupport)
-{
-	/* PHS Sepcific */
-	UINT unPHSPktHdrBytesCopied = 0;
-	UINT unPhsOldHdrSize = 0;
-	UINT unPHSNewPktHeaderLen = 0;
-	/* Pointer to PHS IN Hdr Buffer */
-	PUCHAR pucPHSPktHdrInBuf =
-		Adapter->stPhsTxContextInfo.ucaHdrSuppressionInBuf;
-	/* Pointer to PHS OUT Hdr Buffer */
-	PUCHAR pucPHSPktHdrOutBuf =
-		Adapter->stPhsTxContextInfo.ucaHdrSuppressionOutBuf;
-	UINT usPacketType;
-	UINT BytesToRemove = 0;
-	bool bPHSI = 0;
-	LONG ulPhsStatus = 0;
-	UINT numBytesCompressed = 0;
-	struct sk_buff *newPacket = NULL;
-	struct sk_buff *Packet = *pPacket;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,
-			"In PHSTransmit");
-
-	if (!bEthCSSupport)
-		BytesToRemove = ETH_HLEN;
-	/*
-	 * Accumulate the header upto the size we support suppression
-	 * from NDIS packet
-	 */
-
-	usPacketType = ((struct ethhdr *)(Packet->data))->h_proto;
-
-	pucPHSPktHdrInBuf = Packet->data + BytesToRemove;
-	/* considering data after ethernet header */
-	if ((*PacketLen - BytesToRemove) < MAX_PHS_LENGTHS)
-		unPHSPktHdrBytesCopied = (*PacketLen - BytesToRemove);
-	else
-		unPHSPktHdrBytesCopied = MAX_PHS_LENGTHS;
-
-	if ((unPHSPktHdrBytesCopied > 0) &&
-		(unPHSPktHdrBytesCopied <= MAX_PHS_LENGTHS)) {
-
-		/*
-		 * Step 2 Suppress Header using PHS and fill into intermediate
-		 * ucaPHSPktHdrOutBuf.
-		 * Suppress only if IP Header and PHS Enabled For the
-		 * Service Flow
-		 */
-		if (((usPacketType == ETHERNET_FRAMETYPE_IPV4) ||
-				(usPacketType == ETHERNET_FRAMETYPE_IPV6)) &&
-			(bHeaderSuppressionEnabled)) {
-
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND,
-					DBG_LVL_ALL,
-					"\nTrying to PHS Compress Using Classifier rule 0x%X",
-					uiClassifierRuleID);
-			unPHSNewPktHeaderLen = unPHSPktHdrBytesCopied;
-			ulPhsStatus = PhsCompress(&Adapter->stBCMPhsContext,
-						  Vcid,
-						  uiClassifierRuleID,
-						  pucPHSPktHdrInBuf,
-						  pucPHSPktHdrOutBuf,
-						  &unPhsOldHdrSize,
-						  &unPHSNewPktHeaderLen);
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND,
-					DBG_LVL_ALL,
-					"\nPHS Old header Size : %d New Header Size  %d\n",
-					unPhsOldHdrSize, unPHSNewPktHeaderLen);
-
-			if (unPHSNewPktHeaderLen == unPhsOldHdrSize) {
-
-				if (ulPhsStatus == STATUS_PHS_COMPRESSED)
-					bPHSI = *pucPHSPktHdrOutBuf;
-
-				ulPhsStatus = STATUS_PHS_NOCOMPRESSION;
-			}
-
-			if (ulPhsStatus == STATUS_PHS_COMPRESSED) {
-
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
-						PHS_SEND, DBG_LVL_ALL,
-						"PHS Sending packet Compressed");
-
-				if (skb_cloned(Packet)) {
-					newPacket =
-						skb_copy(Packet, GFP_ATOMIC);
-
-					if (newPacket == NULL)
-						return STATUS_FAILURE;
-
-					dev_kfree_skb(Packet);
-					*pPacket = Packet = newPacket;
-					pucPHSPktHdrInBuf =
-						Packet->data + BytesToRemove;
-				}
-
-				numBytesCompressed = unPhsOldHdrSize -
-					(unPHSNewPktHeaderLen + PHSI_LEN);
-
-				memcpy(pucPHSPktHdrInBuf + numBytesCompressed,
-				       pucPHSPktHdrOutBuf,
-				       unPHSNewPktHeaderLen + PHSI_LEN);
-				memcpy(Packet->data + numBytesCompressed,
-				       Packet->data, BytesToRemove);
-				skb_pull(Packet, numBytesCompressed);
-
-				return STATUS_SUCCESS;
-			} else {
-				/* if one byte headroom is not available,
-				 * increase it through skb_cow
-				 */
-				if (!(skb_headroom(Packet) > 0)) {
-
-					if (skb_cow(Packet, 1)) {
-						BCM_DEBUG_PRINT(Adapter,
-								DBG_TYPE_PRINTK,
-								0, 0,
-								"SKB Cow Failed\n");
-						return STATUS_FAILURE;
-					}
-				}
-				skb_push(Packet, 1);
-
-				/*
-				 * CAUTION: The MAC Header is getting corrupted
-				 * here for IP CS - can be saved by copying 14
-				 * Bytes.  not needed .... hence corrupting it.
-				 */
-				*(Packet->data + BytesToRemove) = bPHSI;
-				return STATUS_SUCCESS;
-			}
-		} else {
-
-			if (!bHeaderSuppressionEnabled)
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
-						PHS_SEND, DBG_LVL_ALL,
-						"\nHeader Suppression Disabled For SF: No PHS\n");
-
-			return STATUS_SUCCESS;
-		}
-	}
-
-	/* BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,
-	 * "PHSTransmit : Dumping data packet After PHS"); */
-	return STATUS_SUCCESS;
-}
-
-int PHSReceive(struct bcm_mini_adapter *Adapter,
-	       USHORT usVcid,
-	       struct sk_buff *packet,
-	       UINT *punPacketLen,
-	       UCHAR *pucEthernetHdr,
-	       UINT bHeaderSuppressionEnabled)
-{
-	u32 nStandardPktHdrLen = 0;
-	u32 nTotalsuppressedPktHdrBytes = 0;
-	int ulPhsStatus	= 0;
-	PUCHAR pucInBuff = NULL;
-	UINT TotalBytesAdded = 0;
-
-	if (!bHeaderSuppressionEnabled) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_RECEIVE,
-				DBG_LVL_ALL,
-				"\nPhs Disabled for incoming packet");
-		return ulPhsStatus;
-	}
-
-	pucInBuff = packet->data;
-
-	/* Restore PHS suppressed header */
-	nStandardPktHdrLen = packet->len;
-	ulPhsStatus = PhsDeCompress(&Adapter->stBCMPhsContext,
-				    usVcid,
-				    pucInBuff,
-				    Adapter->ucaPHSPktRestoreBuf,
-				    &nTotalsuppressedPktHdrBytes,
-				    &nStandardPktHdrLen);
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_RECEIVE, DBG_LVL_ALL,
-			"\nSuppressed PktHdrLen : 0x%x Restored PktHdrLen : 0x%x",
-			nTotalsuppressedPktHdrBytes, nStandardPktHdrLen);
-
-	if (ulPhsStatus != STATUS_PHS_COMPRESSED) {
-		skb_pull(packet, 1);
-		return STATUS_SUCCESS;
-	} else {
-		TotalBytesAdded = nStandardPktHdrLen -
-			nTotalsuppressedPktHdrBytes - PHSI_LEN;
-
-		if (TotalBytesAdded) {
-			if (skb_headroom(packet) >= (SKB_RESERVE_ETHERNET_HEADER + TotalBytesAdded))
-				skb_push(packet, TotalBytesAdded);
-			else {
-				if (skb_cow(packet, skb_headroom(packet) + TotalBytesAdded)) {
-					BCM_DEBUG_PRINT(Adapter,
-							DBG_TYPE_PRINTK, 0, 0,
-							"cow failed in receive\n");
-					return STATUS_FAILURE;
-				}
-
-				skb_push(packet, TotalBytesAdded);
-			}
-		}
-
-		memcpy(packet->data, Adapter->ucaPHSPktRestoreBuf,
-		       nStandardPktHdrLen);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-void DumpFullPacket(UCHAR *pBuf, UINT nPktLen)
-{
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,
-			"Dumping Data Packet");
-	BCM_DEBUG_PRINT_BUFFER(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,
-			       pBuf, nPktLen);
-}
-
-/*
- * Procedure:   phs_init
- *
- * Description: This routine is responsible for allocating memory for classifier
- * and PHS rules.
- *
- * Arguments:
- * pPhsdeviceExtension - ptr to Device extension containing PHS Classifier rules
- * and PHS Rules , RX, TX buffer etc
- *
- * Returns:
- * TRUE(1)	-If allocation of memory was successful.
- * FALSE	-If allocation of memory fails.
- */
-int phs_init(struct bcm_phs_extension *pPhsdeviceExtension,
-	     struct bcm_mini_adapter *Adapter)
-{
-	int i;
-	struct bcm_phs_table *pstServiceFlowTable;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL,
-			"\nPHS:phs_init function");
-
-	if (pPhsdeviceExtension->pstServiceFlowPhsRulesTable)
-		return -EINVAL;
-
-	pPhsdeviceExtension->pstServiceFlowPhsRulesTable =
-		kzalloc(sizeof(struct bcm_phs_table), GFP_KERNEL);
-
-	if (!pPhsdeviceExtension->pstServiceFlowPhsRulesTable) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH,
-				DBG_LVL_ALL,
-				"\nAllocation ServiceFlowPhsRulesTable failed");
-		return -ENOMEM;
-	}
-
-	pstServiceFlowTable = pPhsdeviceExtension->pstServiceFlowPhsRulesTable;
-	for (i = 0; i < MAX_SERVICEFLOWS; i++) {
-		struct bcm_phs_entry sServiceFlow =
-			pstServiceFlowTable->stSFList[i];
-		sServiceFlow.pstClassifierTable =
-			kzalloc(sizeof(struct bcm_phs_classifier_table),
-				GFP_KERNEL);
-		if (!sServiceFlow.pstClassifierTable) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH,
-					DBG_LVL_ALL, "\nAllocation failed");
-			free_phs_serviceflow_rules(pPhsdeviceExtension->pstServiceFlowPhsRulesTable);
-			pPhsdeviceExtension->pstServiceFlowPhsRulesTable = NULL;
-			return -ENOMEM;
-		}
-	}
-
-	pPhsdeviceExtension->CompressedTxBuffer = kmalloc(PHS_BUFFER_SIZE, GFP_KERNEL);
-	if (pPhsdeviceExtension->CompressedTxBuffer == NULL) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH,
-				DBG_LVL_ALL, "\nAllocation failed");
-		free_phs_serviceflow_rules(pPhsdeviceExtension->pstServiceFlowPhsRulesTable);
-		pPhsdeviceExtension->pstServiceFlowPhsRulesTable = NULL;
-		return -ENOMEM;
-	}
-
-	pPhsdeviceExtension->UnCompressedRxBuffer =
-		kmalloc(PHS_BUFFER_SIZE, GFP_KERNEL);
-	if (pPhsdeviceExtension->UnCompressedRxBuffer == NULL) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH,
-				DBG_LVL_ALL, "\nAllocation failed");
-		kfree(pPhsdeviceExtension->CompressedTxBuffer);
-		free_phs_serviceflow_rules(pPhsdeviceExtension->pstServiceFlowPhsRulesTable);
-		pPhsdeviceExtension->pstServiceFlowPhsRulesTable = NULL;
-		return -ENOMEM;
-	}
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL,
-			"\n phs_init Successful");
-	return STATUS_SUCCESS;
-}
-
-int PhsCleanup(IN struct bcm_phs_extension *pPHSDeviceExt)
-{
-	if (pPHSDeviceExt->pstServiceFlowPhsRulesTable) {
-		free_phs_serviceflow_rules(pPHSDeviceExt->pstServiceFlowPhsRulesTable);
-		pPHSDeviceExt->pstServiceFlowPhsRulesTable = NULL;
-	}
-
-	kfree(pPHSDeviceExt->CompressedTxBuffer);
-	pPHSDeviceExt->CompressedTxBuffer = NULL;
-
-	kfree(pPHSDeviceExt->UnCompressedRxBuffer);
-	pPHSDeviceExt->UnCompressedRxBuffer = NULL;
-
-	return 0;
-}
-
-/*
- * PHS functions
- * PhsUpdateClassifierRule
- *
- * Routine Description:
- *   Exported function to add or modify a PHS Rule.
- *
- * Arguments:
- *	IN void* pvContext - PHS Driver Specific Context
- *	IN B_UINT16 uiVcid    - The Service Flow ID for which the PHS rule applies
- *	IN B_UINT16  uiClsId   - The Classifier ID within the Service Flow for which the PHS rule applies.
- *	IN struct bcm_phs_rule *psPhsRule - The PHS Rule strcuture to be added to the PHS Rule table.
- *
- * Return Value:
- *
- * 0 if successful,
- * >0 Error.
- */
-ULONG PhsUpdateClassifierRule(IN void *pvContext,
-			      IN B_UINT16 uiVcid ,
-			      IN B_UINT16 uiClsId   ,
-			      IN struct bcm_phs_rule *psPhsRule,
-			      IN B_UINT8 u8AssociatedPHSI)
-{
-	ULONG lStatus = 0;
-	UINT nSFIndex = 0;
-	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	struct bcm_phs_extension *pDeviceExtension =
-		(struct bcm_phs_extension *)pvContext;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL,
-			"PHS With Corr2 Changes\n");
-
-	if (pDeviceExtension == NULL) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH,
-				DBG_LVL_ALL, "Invalid Device Extension\n");
-		return ERR_PHS_INVALID_DEVICE_EXETENSION;
-	}
-
-	if (u8AssociatedPHSI == 0)
-		return ERR_PHS_INVALID_PHS_RULE;
-
-	/* Retrieve the SFID Entry Index for requested Service Flow */
-	nSFIndex = GetServiceFlowEntry(pDeviceExtension->pstServiceFlowPhsRulesTable,
-				       uiVcid, &pstServiceFlowEntry);
-
-	if (nSFIndex == PHS_INVALID_TABLE_INDEX) {
-		/* This is a new SF. Create a mapping entry for this */
-		lStatus = CreateSFToClassifierRuleMapping(uiVcid, uiClsId,
-							  pDeviceExtension->pstServiceFlowPhsRulesTable,
-							  psPhsRule,
-							  u8AssociatedPHSI);
-		return lStatus;
-	}
-
-	/* SF already Exists Add PHS Rule to existing SF */
-	lStatus = CreateClassiferToPHSRuleMapping(uiVcid, uiClsId,
-						  pstServiceFlowEntry,
-						  psPhsRule,
-						  u8AssociatedPHSI);
-
-	return lStatus;
-}
-
-/*
- * PhsDeletePHSRule
- *
- * Routine Description:
- *   Deletes the specified phs Rule within Vcid
- *
- * Arguments:
- *	IN void* pvContext - PHS Driver Specific Context
- *	IN B_UINT16  uiVcid    - The Service Flow ID for which the PHS rule applies
- *	IN B_UINT8  u8PHSI   - the PHS Index identifying PHS rule to be deleted.
- *
- * Return Value:
- *
- * 0 if successful,
- * >0 Error.
- */
-ULONG PhsDeletePHSRule(IN void *pvContext,
-		       IN B_UINT16 uiVcid,
-		       IN B_UINT8 u8PHSI)
-{
-	UINT nSFIndex = 0, nClsidIndex = 0;
-	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
-	struct bcm_phs_classifier_table *pstClassifierRulesTable = NULL;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	struct bcm_phs_extension *pDeviceExtension = (struct bcm_phs_extension *)pvContext;
-	struct bcm_phs_classifier_entry *curr_entry;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL,
-			"======>\n");
-
-	if (pDeviceExtension) {
-		/* Retrieve the SFID Entry Index for requested Service Flow */
-		nSFIndex = GetServiceFlowEntry(pDeviceExtension->pstServiceFlowPhsRulesTable,
-					       uiVcid, &pstServiceFlowEntry);
-
-		if (nSFIndex == PHS_INVALID_TABLE_INDEX) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH,
-					DBG_LVL_ALL, "SFID Match Failed\n");
-			return ERR_SF_MATCH_FAIL;
-		}
-
-		pstClassifierRulesTable = pstServiceFlowEntry->pstClassifierTable;
-		if (pstClassifierRulesTable) {
-			for (nClsidIndex = 0; nClsidIndex < MAX_PHSRULE_PER_SF; nClsidIndex++) {
-				curr_entry = &pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex];
-				if (curr_entry->bUsed &&
-				    curr_entry->pstPhsRule &&
-				    (curr_entry->pstPhsRule->u8PHSI == u8PHSI)) {
-
-					if (curr_entry->pstPhsRule->u8RefCnt)
-						curr_entry->pstPhsRule->u8RefCnt--;
-
-					if (0 == curr_entry->pstPhsRule->u8RefCnt)
-						kfree(curr_entry->pstPhsRule);
-
-					memset(curr_entry,
-					       0,
-					       sizeof(struct bcm_phs_classifier_entry));
-				}
-			}
-		}
-	}
-	return 0;
-}
-
-/*
- * PhsDeleteClassifierRule
- *
- * Routine Description:
- *    Exported function to Delete a PHS Rule for the SFID,CLSID Pair.
- *
- * Arguments:
- *	IN void* pvContext - PHS Driver Specific Context
- *	IN B_UINT16  uiVcid    - The Service Flow ID for which the PHS rule applies
- *	IN B_UINT16  uiClsId   - The Classifier ID within the Service Flow for which the PHS rule applies.
- *
- * Return Value:
- *
- * 0 if successful,
- * >0 Error.
- */
-ULONG PhsDeleteClassifierRule(IN void *pvContext,
-			      IN B_UINT16 uiVcid,
-			      IN B_UINT16 uiClsId)
-{
-	UINT nSFIndex = 0, nClsidIndex = 0;
-	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
-	struct bcm_phs_classifier_entry *pstClassifierEntry = NULL;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	struct bcm_phs_extension *pDeviceExtension =
-		(struct bcm_phs_extension *)pvContext;
-
-	if (!pDeviceExtension)
-		goto out;
-
-	/* Retrieve the SFID Entry Index for requested Service Flow */
-	nSFIndex = GetServiceFlowEntry(pDeviceExtension->pstServiceFlowPhsRulesTable,
-				       uiVcid, &pstServiceFlowEntry);
-	if (nSFIndex == PHS_INVALID_TABLE_INDEX) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH,
-				DBG_LVL_ALL, "SFID Match Failed\n");
-		return ERR_SF_MATCH_FAIL;
-	}
-
-	nClsidIndex =
-		GetClassifierEntry(pstServiceFlowEntry->pstClassifierTable,
-				   uiClsId,
-				   eActiveClassifierRuleContext,
-				   &pstClassifierEntry);
-
-	if ((nClsidIndex != PHS_INVALID_TABLE_INDEX) &&
-			(!pstClassifierEntry->bUnclassifiedPHSRule)) {
-		if (pstClassifierEntry->pstPhsRule) {
-			if (pstClassifierEntry->pstPhsRule->u8RefCnt)
-				pstClassifierEntry->pstPhsRule->u8RefCnt--;
-
-			if (0 == pstClassifierEntry->pstPhsRule->u8RefCnt)
-				kfree(pstClassifierEntry->pstPhsRule);
-		}
-		memset(pstClassifierEntry, 0,
-		       sizeof(struct bcm_phs_classifier_entry));
-	}
-
-	nClsidIndex =
-		GetClassifierEntry(pstServiceFlowEntry->pstClassifierTable,
-				   uiClsId,
-				   eOldClassifierRuleContext,
-				   &pstClassifierEntry);
-
-	if ((nClsidIndex != PHS_INVALID_TABLE_INDEX) &&
-			(!pstClassifierEntry->bUnclassifiedPHSRule)) {
-		kfree(pstClassifierEntry->pstPhsRule);
-		memset(pstClassifierEntry, 0,
-		       sizeof(struct bcm_phs_classifier_entry));
-	}
-
-out:
-	return 0;
-}
-
-/*
- * PhsDeleteSFRules
- *
- * Routine Description:
- *    Exported function to Delete a all PHS Rules for the SFID.
- *
- * Arguments:
- *	IN void* pvContext - PHS Driver Specific Context
- *	IN B_UINT16 uiVcid   - The Service Flow ID for which the PHS rules need to be deleted
- *
- * Return Value:
- *
- * 0 if successful,
- * >0 Error.
- */
-ULONG PhsDeleteSFRules(IN void *pvContext, IN B_UINT16 uiVcid)
-{
-	UINT nSFIndex = 0, nClsidIndex = 0;
-	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
-	struct bcm_phs_classifier_table *pstClassifierRulesTable = NULL;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	struct bcm_phs_extension *pDeviceExtension =
-		(struct bcm_phs_extension *)pvContext;
-	struct bcm_phs_classifier_entry *curr_clsf_entry;
-	struct bcm_phs_classifier_entry *curr_rules_list;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL,
-			"====>\n");
-
-	if (!pDeviceExtension)
-		goto out;
-
-	/* Retrieve the SFID Entry Index for requested Service Flow */
-	nSFIndex = GetServiceFlowEntry(pDeviceExtension->pstServiceFlowPhsRulesTable,
-				       uiVcid, &pstServiceFlowEntry);
-	if (nSFIndex == PHS_INVALID_TABLE_INDEX) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH,
-				DBG_LVL_ALL, "SFID Match Failed\n");
-		return ERR_SF_MATCH_FAIL;
-	}
-
-	pstClassifierRulesTable = pstServiceFlowEntry->pstClassifierTable;
-	if (pstClassifierRulesTable) {
-		for (nClsidIndex = 0; nClsidIndex < MAX_PHSRULE_PER_SF; nClsidIndex++) {
-			curr_clsf_entry =
-				&pstClassifierRulesTable->stActivePhsRulesList[nClsidIndex];
-
-			curr_rules_list =
-				&pstClassifierRulesTable->stOldPhsRulesList[nClsidIndex];
-
-			if (curr_clsf_entry->pstPhsRule) {
-
-				if (curr_clsf_entry->pstPhsRule->u8RefCnt)
-					curr_clsf_entry->pstPhsRule->u8RefCnt--;
-
-				if (0 == curr_clsf_entry->pstPhsRule->u8RefCnt)
-					kfree(curr_clsf_entry->pstPhsRule);
-
-				curr_clsf_entry->pstPhsRule = NULL;
-			}
-			memset(curr_clsf_entry, 0,
-			       sizeof(struct bcm_phs_classifier_entry));
-			if (curr_rules_list->pstPhsRule) {
-
-				if (curr_rules_list->pstPhsRule->u8RefCnt)
-					curr_rules_list->pstPhsRule->u8RefCnt--;
-
-				if (0 == curr_rules_list->pstPhsRule->u8RefCnt)
-					kfree(curr_rules_list->pstPhsRule);
-
-				curr_rules_list->pstPhsRule = NULL;
-			}
-			memset(curr_rules_list, 0,
-			       sizeof(struct bcm_phs_classifier_entry));
-		}
-	}
-	pstServiceFlowEntry->bUsed = false;
-	pstServiceFlowEntry->uiVcid = 0;
-
-out:
-	return 0;
-}
-
-/*
- * PhsCompress
- *
- * Routine Description:
- *    Exported function to compress the data using PHS.
- *
- * Arguments:
- *	IN void* pvContext	    - PHS Driver Specific Context.
- *	IN B_UINT16 uiVcid	    - The Service Flow ID to which current
- *				      packet header compression applies.
- *	IN UINT  uiClsId	    - The Classifier ID to which current packet
- *				      header compression applies.
- *	IN void *pvInputBuffer	    - The Input buffer containg packet header
- *				      data
- *	IN void *pvOutputBuffer     - The output buffer returned by this
- *				      function after PHS
- *	IN UINT *pOldHeaderSize	    - The actual size of the header before PHS
- *	IN UINT *pNewHeaderSize	    - The new size of the header after applying
- *				      PHS
- *
- * Return Value:
- *
- * 0 if successful,
- * >0 Error.
- */
-static ULONG PhsCompress(IN void *pvContext,
-			 IN B_UINT16 uiVcid,
-			 IN B_UINT16 uiClsId,
-			 IN void *pvInputBuffer,
-			 OUT void *pvOutputBuffer,
-			 OUT UINT *pOldHeaderSize,
-			 OUT UINT *pNewHeaderSize)
-{
-	UINT nSFIndex = 0, nClsidIndex = 0;
-	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
-	struct bcm_phs_classifier_entry *pstClassifierEntry = NULL;
-	struct bcm_phs_rule *pstPhsRule = NULL;
-	ULONG lStatus = 0;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	struct bcm_phs_extension *pDeviceExtension =
-		(struct bcm_phs_extension *)pvContext;
-
-	if (pDeviceExtension == NULL) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,
-				"Invalid Device Extension\n");
-		lStatus = STATUS_PHS_NOCOMPRESSION;
-		return lStatus;
-	}
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,
-			"Suppressing header\n");
-
-	/* Retrieve the SFID Entry Index for requested Service Flow */
-	nSFIndex = GetServiceFlowEntry(pDeviceExtension->pstServiceFlowPhsRulesTable,
-				       uiVcid, &pstServiceFlowEntry);
-	if (nSFIndex == PHS_INVALID_TABLE_INDEX) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,
-				"SFID Match Failed\n");
-		lStatus = STATUS_PHS_NOCOMPRESSION;
-		return lStatus;
-	}
-
-	nClsidIndex = GetClassifierEntry(pstServiceFlowEntry->pstClassifierTable,
-					 uiClsId, eActiveClassifierRuleContext,
-					 &pstClassifierEntry);
-
-	if (nClsidIndex == PHS_INVALID_TABLE_INDEX) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,
-				"No PHS Rule Defined For Classifier\n");
-		lStatus =  STATUS_PHS_NOCOMPRESSION;
-		return lStatus;
-	}
-
-	/* get rule from SF id,Cls ID pair and proceed */
-	pstPhsRule = pstClassifierEntry->pstPhsRule;
-	if (!ValidatePHSRuleComplete(pstPhsRule)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL,
-				"PHS Rule Defined For Classifier But Not Complete\n");
-		lStatus = STATUS_PHS_NOCOMPRESSION;
-		return lStatus;
-	}
-
-	/* Compress Packet */
-	lStatus = phs_compress(pstPhsRule,
-			       (PUCHAR)pvInputBuffer,
-			       (PUCHAR)pvOutputBuffer,
-			       pOldHeaderSize,
-			       pNewHeaderSize);
-
-	if (lStatus == STATUS_PHS_COMPRESSED) {
-		pstPhsRule->PHSModifiedBytes +=
-			*pOldHeaderSize - *pNewHeaderSize - 1;
-		pstPhsRule->PHSModifiedNumPackets++;
-	} else {
-		pstPhsRule->PHSErrorNumPackets++;
-	}
-
-	return lStatus;
-}
-
-/*
- * PhsDeCompress
- *
- * Routine Description:
- *    Exported function to restore the packet header in Rx path.
- *
- * Arguments:
- *	IN void* pvContext	    - PHS Driver Specific Context.
- *	IN B_UINT16 uiVcid	    - The Service Flow ID to which current
- *				      packet header restoration applies.
- *	IN  void *pvInputBuffer	    - The Input buffer containg suppressed
- *				      packet header data
- *	OUT void *pvOutputBuffer    - The output buffer returned by this
- *				      function after restoration
- *	OUT UINT *pHeaderSize	    - The packet header size after restoration
- *				      is returned in this parameter.
- *
- * Return Value:
- *
- * 0 if successful,
- * >0 Error.
- */
-static ULONG PhsDeCompress(IN void *pvContext,
-			   IN B_UINT16 uiVcid,
-			   IN void *pvInputBuffer,
-			   OUT void *pvOutputBuffer,
-			   OUT UINT *pInHeaderSize,
-			   OUT UINT *pOutHeaderSize)
-{
-	UINT nSFIndex = 0, nPhsRuleIndex = 0;
-	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
-	struct bcm_phs_rule *pstPhsRule = NULL;
-	UINT phsi;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	struct bcm_phs_extension *pDeviceExtension =
-		(struct bcm_phs_extension *)pvContext;
-
-	*pInHeaderSize = 0;
-	if (pDeviceExtension == NULL) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_RECEIVE,
-				DBG_LVL_ALL, "Invalid Device Extension\n");
-		return ERR_PHS_INVALID_DEVICE_EXETENSION;
-	}
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_RECEIVE, DBG_LVL_ALL,
-			"Restoring header\n");
-
-	phsi = *((unsigned char *)(pvInputBuffer));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_RECEIVE, DBG_LVL_ALL,
-			"PHSI To Be Used For restore : %x\n", phsi);
-	if (phsi == UNCOMPRESSED_PACKET)
-		return STATUS_PHS_NOCOMPRESSION;
-
-	/* Retrieve the SFID Entry Index for requested Service Flow */
-	nSFIndex = GetServiceFlowEntry(pDeviceExtension->pstServiceFlowPhsRulesTable,
-				       uiVcid, &pstServiceFlowEntry);
-	if (nSFIndex == PHS_INVALID_TABLE_INDEX) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_RECEIVE,
-				DBG_LVL_ALL,
-				"SFID Match Failed During Lookup\n");
-		return ERR_SF_MATCH_FAIL;
-	}
-
-	nPhsRuleIndex = GetPhsRuleEntry(pstServiceFlowEntry->pstClassifierTable,
-					phsi,
-					eActiveClassifierRuleContext,
-					&pstPhsRule);
-	if (nPhsRuleIndex == PHS_INVALID_TABLE_INDEX) {
-		/* Phs Rule does not exist in  active rules table. Lets try
-		 * in the old rules table. */
-		nPhsRuleIndex = GetPhsRuleEntry(pstServiceFlowEntry->pstClassifierTable,
-						phsi,
-						eOldClassifierRuleContext,
-						&pstPhsRule);
-		if (nPhsRuleIndex == PHS_INVALID_TABLE_INDEX)
-			return ERR_PHSRULE_MATCH_FAIL;
-	}
-
-	*pInHeaderSize = phs_decompress((PUCHAR)pvInputBuffer,
-					(PUCHAR)pvOutputBuffer,
-					pstPhsRule,
-					pOutHeaderSize);
-
-	pstPhsRule->PHSModifiedBytes += *pOutHeaderSize - *pInHeaderSize - 1;
-
-	pstPhsRule->PHSModifiedNumPackets++;
-	return STATUS_PHS_COMPRESSED;
-}
-
-/*
- * Procedure:   free_phs_serviceflow_rules
- *
- * Description: This routine is responsible for freeing memory allocated for
- * PHS rules.
- *
- * Arguments:
- * rules	- ptr to S_SERVICEFLOW_TABLE structure.
- *
- * Returns:
- * Does not return any value.
- */
-static void free_phs_serviceflow_rules(struct bcm_phs_table *psServiceFlowRulesTable)
-{
-	int i, j;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	struct bcm_phs_classifier_entry *curr_act_rules_list;
-	struct bcm_phs_classifier_entry *curr_old_rules_list;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL,
-			"=======>\n");
-
-	if (!psServiceFlowRulesTable)
-		goto out;
-
-	for (i = 0; i < MAX_SERVICEFLOWS; i++) {
-		struct bcm_phs_entry stServiceFlowEntry =
-			psServiceFlowRulesTable->stSFList[i];
-		struct bcm_phs_classifier_table *pstClassifierRulesTable =
-			stServiceFlowEntry.pstClassifierTable;
-
-		if (pstClassifierRulesTable) {
-			for (j = 0; j < MAX_PHSRULE_PER_SF; j++) {
-				curr_act_rules_list =
-					&pstClassifierRulesTable->stActivePhsRulesList[j];
-
-				curr_old_rules_list =
-					&pstClassifierRulesTable->stOldPhsRulesList[j];
-
-				if (curr_act_rules_list->pstPhsRule) {
-
-					if (curr_act_rules_list->pstPhsRule->u8RefCnt)
-						curr_act_rules_list->pstPhsRule->u8RefCnt--;
-
-					if (0 == curr_act_rules_list->pstPhsRule->u8RefCnt)
-						kfree(curr_act_rules_list->pstPhsRule);
-
-					curr_act_rules_list->pstPhsRule = NULL;
-				}
-
-				if (curr_old_rules_list->pstPhsRule) {
-
-					if (curr_old_rules_list->pstPhsRule->u8RefCnt)
-						curr_old_rules_list->pstPhsRule->u8RefCnt--;
-
-					if (0 == curr_old_rules_list->pstPhsRule->u8RefCnt)
-						kfree(curr_old_rules_list->pstPhsRule);
-
-					curr_old_rules_list->pstPhsRule = NULL;
-				}
-			}
-			kfree(pstClassifierRulesTable);
-			stServiceFlowEntry.pstClassifierTable =
-				pstClassifierRulesTable = NULL;
-		}
-	}
-
-out:
-
-	kfree(psServiceFlowRulesTable);
-	psServiceFlowRulesTable = NULL;
-}
-
-static bool ValidatePHSRuleComplete(IN const struct bcm_phs_rule *psPhsRule)
-{
-	return (psPhsRule &&
-		psPhsRule->u8PHSI &&
-		psPhsRule->u8PHSS &&
-		psPhsRule->u8PHSFLength);
-}
-
-UINT GetServiceFlowEntry(IN struct bcm_phs_table *psServiceFlowTable,
-			 IN B_UINT16 uiVcid,
-			 struct bcm_phs_entry **ppstServiceFlowEntry)
-{
-	int i;
-	struct bcm_phs_entry *curr_sf_list;
-
-	for (i = 0; i < MAX_SERVICEFLOWS; i++) {
-		curr_sf_list = &psServiceFlowTable->stSFList[i];
-		if (curr_sf_list->bUsed && (curr_sf_list->uiVcid == uiVcid)) {
-			*ppstServiceFlowEntry = curr_sf_list;
-			return i;
-		}
-	}
-
-	*ppstServiceFlowEntry = NULL;
-	return PHS_INVALID_TABLE_INDEX;
-}
-
-static UINT GetClassifierEntry(IN struct bcm_phs_classifier_table *pstClassifierTable,
-			       IN B_UINT32 uiClsid,
-			       enum bcm_phs_classifier_context eClsContext,
-			       OUT struct bcm_phs_classifier_entry **ppstClassifierEntry)
-{
-	int  i;
-	struct bcm_phs_classifier_entry *psClassifierRules = NULL;
-
-	for (i = 0; i < MAX_PHSRULE_PER_SF; i++) {
-
-		if (eClsContext == eActiveClassifierRuleContext)
-			psClassifierRules =
-				&pstClassifierTable->stActivePhsRulesList[i];
-		else
-			psClassifierRules =
-				&pstClassifierTable->stOldPhsRulesList[i];
-
-		if (psClassifierRules->bUsed &&
-		   (psClassifierRules->uiClassifierRuleId == uiClsid)) {
-			*ppstClassifierEntry = psClassifierRules;
-			return i;
-		}
-	}
-
-	*ppstClassifierEntry = NULL;
-	return PHS_INVALID_TABLE_INDEX;
-}
-
-static UINT GetPhsRuleEntry(IN struct bcm_phs_classifier_table *pstClassifierTable,
-			    IN B_UINT32 uiPHSI,
-			    enum bcm_phs_classifier_context eClsContext,
-			    OUT struct bcm_phs_rule **ppstPhsRule)
-{
-	int  i;
-	struct bcm_phs_classifier_entry *pstClassifierRule = NULL;
-
-	for (i = 0; i < MAX_PHSRULE_PER_SF; i++) {
-		if (eClsContext == eActiveClassifierRuleContext)
-			pstClassifierRule =
-				&pstClassifierTable->stActivePhsRulesList[i];
-		else
-			pstClassifierRule =
-				&pstClassifierTable->stOldPhsRulesList[i];
-
-		if (pstClassifierRule->bUsed &&
-		   (pstClassifierRule->u8PHSI == uiPHSI)) {
-			*ppstPhsRule = pstClassifierRule->pstPhsRule;
-			return i;
-		}
-	}
-
-	*ppstPhsRule = NULL;
-	return PHS_INVALID_TABLE_INDEX;
-}
-
-static UINT CreateSFToClassifierRuleMapping(IN B_UINT16 uiVcid,
-					    IN B_UINT16  uiClsId,
-					    IN struct bcm_phs_table *psServiceFlowTable,
-					    struct bcm_phs_rule *psPhsRule,
-					    B_UINT8 u8AssociatedPHSI)
-{
-	struct bcm_phs_classifier_table *psaClassifiertable = NULL;
-	UINT uiStatus = 0;
-	int iSfIndex;
-	bool bFreeEntryFound = false;
-	struct bcm_phs_entry *curr_list;
-
-	/* Check for a free entry in SFID table */
-	for (iSfIndex = 0; iSfIndex < MAX_SERVICEFLOWS; iSfIndex++) {
-		curr_list = &psServiceFlowTable->stSFList[iSfIndex];
-		if (!curr_list->bUsed) {
-			bFreeEntryFound = TRUE;
-			break;
-		}
-	}
-
-	if (!bFreeEntryFound)
-		return ERR_SFTABLE_FULL;
-
-	psaClassifiertable = curr_list->pstClassifierTable;
-	uiStatus = CreateClassifierPHSRule(uiClsId,
-					   psaClassifiertable,
-					   psPhsRule,
-					   eActiveClassifierRuleContext,
-					   u8AssociatedPHSI);
-	if (uiStatus == PHS_SUCCESS) {
-		/* Add entry at free index to the SF */
-		curr_list->bUsed = TRUE;
-		curr_list->uiVcid = uiVcid;
-	}
-
-	return uiStatus;
-}
-
-static UINT CreateClassiferToPHSRuleMapping(IN B_UINT16 uiVcid,
-					    IN B_UINT16 uiClsId,
-					    IN struct bcm_phs_entry *pstServiceFlowEntry,
-					    struct bcm_phs_rule *psPhsRule,
-					    B_UINT8 u8AssociatedPHSI)
-{
-	struct bcm_phs_classifier_entry *pstClassifierEntry = NULL;
-	UINT uiStatus = PHS_SUCCESS;
-	UINT nClassifierIndex = 0;
-	struct bcm_phs_classifier_table *psaClassifiertable = NULL;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	psaClassifiertable = pstServiceFlowEntry->pstClassifierTable;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL,
-			"==>");
-
-	/* Check if the supplied Classifier already exists */
-	nClassifierIndex = GetClassifierEntry(
-		pstServiceFlowEntry->pstClassifierTable,
-		uiClsId,
-		eActiveClassifierRuleContext,
-		&pstClassifierEntry);
-
-	if (nClassifierIndex == PHS_INVALID_TABLE_INDEX) {
-		/*
-		 * The Classifier doesn't exist. So its a new classifier being
-		 * added.
-		 * Add new entry to associate PHS Rule to the Classifier
-		 */
-
-		uiStatus = CreateClassifierPHSRule(uiClsId, psaClassifiertable,
-						   psPhsRule,
-						   eActiveClassifierRuleContext,
-						   u8AssociatedPHSI);
-		return uiStatus;
-	}
-
-	/*
-	 * The Classifier exists.The PHS Rule for this classifier
-	 * is being modified
-	 */
-
-	if (pstClassifierEntry->u8PHSI == psPhsRule->u8PHSI) {
-		if (pstClassifierEntry->pstPhsRule == NULL)
-			return ERR_PHS_INVALID_PHS_RULE;
-
-		/*
-		 * This rule already exists if any fields are changed for this
-		 * PHS rule update them.
-		 */
-		/* If any part of PHSF is valid then we update PHSF */
-		if (psPhsRule->u8PHSFLength) {
-			/* update PHSF */
-			memcpy(pstClassifierEntry->pstPhsRule->u8PHSF,
-			       psPhsRule->u8PHSF,
-			       MAX_PHS_LENGTHS);
-		}
-
-		if (psPhsRule->u8PHSFLength) {
-			/* update PHSFLen */
-			pstClassifierEntry->pstPhsRule->u8PHSFLength =
-				psPhsRule->u8PHSFLength;
-		}
-
-		if (psPhsRule->u8PHSMLength) {
-			/* update PHSM */
-			memcpy(pstClassifierEntry->pstPhsRule->u8PHSM,
-			       psPhsRule->u8PHSM,
-			       MAX_PHS_LENGTHS);
-		}
-
-		if (psPhsRule->u8PHSMLength) {
-			/* update PHSM Len */
-			pstClassifierEntry->pstPhsRule->u8PHSMLength =
-				psPhsRule->u8PHSMLength;
-		}
-
-		if (psPhsRule->u8PHSS) {
-			/* update PHSS */
-			pstClassifierEntry->pstPhsRule->u8PHSS =
-				psPhsRule->u8PHSS;
-		}
-
-		/* update PHSV */
-		pstClassifierEntry->pstPhsRule->u8PHSV = psPhsRule->u8PHSV;
-	} else {
-		/* A new rule is being set for this classifier. */
-		uiStatus = UpdateClassifierPHSRule(uiClsId,
-						   pstClassifierEntry,
-						   psaClassifiertable,
-						   psPhsRule,
-						   u8AssociatedPHSI);
-	}
-
-	return uiStatus;
-}
-
-static UINT CreateClassifierPHSRule(IN B_UINT16  uiClsId,
-				    struct bcm_phs_classifier_table *psaClassifiertable,
-				    struct bcm_phs_rule *psPhsRule,
-				    enum bcm_phs_classifier_context eClsContext,
-				    B_UINT8 u8AssociatedPHSI)
-{
-	UINT iClassifierIndex = 0;
-	bool bFreeEntryFound = false;
-	struct bcm_phs_classifier_entry *psClassifierRules = NULL;
-	UINT nStatus = PHS_SUCCESS;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL,
-			"Inside CreateClassifierPHSRule");
-
-	if (psaClassifiertable == NULL)
-		return ERR_INVALID_CLASSIFIERTABLE_FOR_SF;
-
-	if (eClsContext == eOldClassifierRuleContext) {
-		/*
-		 * If An Old Entry for this classifier ID already exists in the
-		 * old rules table replace it.
-		 */
-
-		iClassifierIndex = GetClassifierEntry(psaClassifiertable,
-						      uiClsId,
-						      eClsContext,
-						      &psClassifierRules);
-
-		if (iClassifierIndex != PHS_INVALID_TABLE_INDEX) {
-			/*
-			 * The Classifier already exists in the old rules table
-			 * Lets replace the old classifier with the new one.
-			 */
-			bFreeEntryFound = TRUE;
-		}
-	}
-
-	if (!bFreeEntryFound) {
-		/* Continue to search for a free location to add the rule */
-		for (iClassifierIndex = 0; iClassifierIndex <
-			     MAX_PHSRULE_PER_SF; iClassifierIndex++) {
-			if (eClsContext == eActiveClassifierRuleContext)
-				psClassifierRules = &psaClassifiertable->stActivePhsRulesList[iClassifierIndex];
-			else
-				psClassifierRules = &psaClassifiertable->stOldPhsRulesList[iClassifierIndex];
-
-			if (!psClassifierRules->bUsed) {
-				bFreeEntryFound = TRUE;
-				break;
-			}
-		}
-	}
-
-	if (!bFreeEntryFound) {
-
-		if (eClsContext == eActiveClassifierRuleContext)
-			return ERR_CLSASSIFIER_TABLE_FULL;
-		else {
-			/* Lets replace the oldest rule if we are looking in
-			 * old Rule table */
-			if (psaClassifiertable->uiOldestPhsRuleIndex >= MAX_PHSRULE_PER_SF)
-				psaClassifiertable->uiOldestPhsRuleIndex = 0;
-
-			iClassifierIndex =
-				psaClassifiertable->uiOldestPhsRuleIndex;
-			psClassifierRules =
-				&psaClassifiertable->stOldPhsRulesList[iClassifierIndex];
-
-			(psaClassifiertable->uiOldestPhsRuleIndex)++;
-		}
-	}
-
-	if (eClsContext == eOldClassifierRuleContext) {
-
-		if (psClassifierRules->pstPhsRule == NULL) {
-
-			psClassifierRules->pstPhsRule =
-				kmalloc(sizeof(struct bcm_phs_rule),
-					GFP_KERNEL);
-
-			if (NULL == psClassifierRules->pstPhsRule)
-				return ERR_PHSRULE_MEMALLOC_FAIL;
-		}
-
-		psClassifierRules->bUsed = TRUE;
-		psClassifierRules->uiClassifierRuleId = uiClsId;
-		psClassifierRules->u8PHSI = psPhsRule->u8PHSI;
-		psClassifierRules->bUnclassifiedPHSRule =
-			psPhsRule->bUnclassifiedPHSRule;
-
-		/* Update The PHS rule */
-		memcpy(psClassifierRules->pstPhsRule, psPhsRule,
-		       sizeof(struct bcm_phs_rule));
-	} else
-		nStatus = UpdateClassifierPHSRule(uiClsId,
-						  psClassifierRules,
-						  psaClassifiertable,
-						  psPhsRule,
-						  u8AssociatedPHSI);
-
-	return nStatus;
-}
-
-static UINT UpdateClassifierPHSRule(IN B_UINT16  uiClsId,
-				    IN struct bcm_phs_classifier_entry *pstClassifierEntry,
-				    struct bcm_phs_classifier_table *psaClassifiertable,
-				    struct bcm_phs_rule *psPhsRule,
-				    B_UINT8 u8AssociatedPHSI)
-{
-	struct bcm_phs_rule *pstAddPhsRule = NULL;
-	UINT nPhsRuleIndex = 0;
-	bool bPHSRuleOrphaned = false;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	psPhsRule->u8RefCnt = 0;
-
-	/* Step 1 Deref Any Exisiting PHS Rule in this classifier Entry */
-	bPHSRuleOrphaned = DerefPhsRule(uiClsId, psaClassifiertable,
-					pstClassifierEntry->pstPhsRule);
-
-	/* Step 2 Search if there is a PHS Rule with u8AssociatedPHSI in
-	 * Classifier table for this SF */
-	nPhsRuleIndex = GetPhsRuleEntry(psaClassifiertable, u8AssociatedPHSI,
-					eActiveClassifierRuleContext,
-					&pstAddPhsRule);
-	if (PHS_INVALID_TABLE_INDEX == nPhsRuleIndex) {
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH,
-				DBG_LVL_ALL,
-				"\nAdding New PHSRuleEntry For Classifier");
-
-		if (psPhsRule->u8PHSI == 0) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH,
-					DBG_LVL_ALL, "\nError PHSI is Zero\n");
-			return ERR_PHS_INVALID_PHS_RULE;
-		}
-
-		/* Step 2.a PHS Rule Does Not Exist .Create New PHS Rule for
-		 * uiClsId */
-		if (false == bPHSRuleOrphaned) {
-
-			pstClassifierEntry->pstPhsRule =
-				kmalloc(sizeof(struct bcm_phs_rule),
-					GFP_KERNEL);
-			if (NULL == pstClassifierEntry->pstPhsRule)
-				return ERR_PHSRULE_MEMALLOC_FAIL;
-		}
-		memcpy(pstClassifierEntry->pstPhsRule, psPhsRule,
-		       sizeof(struct bcm_phs_rule));
-	} else {
-		/* Step 2.b PHS Rule  Exists Tie uiClsId with the existing
-		 * PHS Rule */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_DISPATCH,
-				DBG_LVL_ALL,
-				"\nTying Classifier to Existing PHS Rule");
-		if (bPHSRuleOrphaned) {
-			kfree(pstClassifierEntry->pstPhsRule);
-			pstClassifierEntry->pstPhsRule = NULL;
-		}
-		pstClassifierEntry->pstPhsRule = pstAddPhsRule;
-	}
-
-	pstClassifierEntry->bUsed = TRUE;
-	pstClassifierEntry->u8PHSI = pstClassifierEntry->pstPhsRule->u8PHSI;
-	pstClassifierEntry->uiClassifierRuleId = uiClsId;
-	pstClassifierEntry->pstPhsRule->u8RefCnt++;
-	pstClassifierEntry->bUnclassifiedPHSRule =
-		pstClassifierEntry->pstPhsRule->bUnclassifiedPHSRule;
-
-	return PHS_SUCCESS;
-}
-
-static bool DerefPhsRule(IN B_UINT16  uiClsId,
-			 struct bcm_phs_classifier_table *psaClassifiertable,
-			 struct bcm_phs_rule *pstPhsRule)
-{
-	if (pstPhsRule == NULL)
-		return false;
-
-	if (pstPhsRule->u8RefCnt)
-		pstPhsRule->u8RefCnt--;
-
-	return (0 == pstPhsRule->u8RefCnt);
-}
-
-static void dbg_print_st_cls_entry(struct bcm_mini_adapter *ad,
-				   struct bcm_phs_entry *st_serv_flow_entry,
-				   struct bcm_phs_classifier_entry *st_cls_entry)
-{
-	int k;
-
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\n VCID  : %#X", st_serv_flow_entry->uiVcid);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n ClassifierID  : %#X", st_cls_entry->uiClassifierRuleId);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSRuleID  : %#X", st_cls_entry->u8PHSI);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n****************PHS Rule********************\n");
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSI  : %#X", st_cls_entry->pstPhsRule->u8PHSI);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSFLength : %#X ", st_cls_entry->pstPhsRule->u8PHSFLength);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSF : ");
-
-	for (k = 0 ; k < st_cls_entry->pstPhsRule->u8PHSFLength; k++)
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "%#X  ", st_cls_entry->pstPhsRule->u8PHSF[k]);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSMLength  : %#X", st_cls_entry->pstPhsRule->u8PHSMLength);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSM :");
-
-	for (k = 0; k < st_cls_entry->pstPhsRule->u8PHSMLength; k++)
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "%#X  ", st_cls_entry->pstPhsRule->u8PHSM[k]);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSS : %#X ", st_cls_entry->pstPhsRule->u8PHSS);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, DUMP_INFO, (DBG_LVL_ALL|DBG_NO_FUNC_PRINT), "\n PHSV  : %#X", st_cls_entry->pstPhsRule->u8PHSV);
-	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\n********************************************\n");
-}
-
-static void phsrules_per_sf_dbg_print(struct bcm_mini_adapter *ad,
-				      struct bcm_phs_entry *st_serv_flow_entry)
-{
-	int j, l;
-	struct bcm_phs_classifier_entry st_cls_entry;
-
-	for (j = 0; j < MAX_PHSRULE_PER_SF; j++) {
-
-		for (l = 0; l < 2; l++) {
-
-			if (l == 0) {
-				st_cls_entry = st_serv_flow_entry->pstClassifierTable->stActivePhsRulesList[j];
-				if (st_cls_entry.bUsed)
-					BCM_DEBUG_PRINT(ad,
-							DBG_TYPE_OTHERS,
-							DUMP_INFO,
-							(DBG_LVL_ALL | DBG_NO_FUNC_PRINT),
-							"\n Active PHS Rule :\n");
-			} else {
-				st_cls_entry = st_serv_flow_entry->pstClassifierTable->stOldPhsRulesList[j];
-				if (st_cls_entry.bUsed)
-					BCM_DEBUG_PRINT(ad,
-							DBG_TYPE_OTHERS,
-							DUMP_INFO,
-							(DBG_LVL_ALL | DBG_NO_FUNC_PRINT),
-							"\n Old PHS Rule :\n");
-			}
-
-			if (st_cls_entry.bUsed) {
-				dbg_print_st_cls_entry(ad,
-						       st_serv_flow_entry,
-						       &st_cls_entry);
-			}
-		}
-	}
-}
-
-void DumpPhsRules(struct bcm_phs_extension *pDeviceExtension)
-{
-	int i;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL,
-			"\n Dumping PHS Rules :\n");
-
-	for (i = 0; i < MAX_SERVICEFLOWS; i++) {
-
-		struct bcm_phs_entry stServFlowEntry =
-			pDeviceExtension->pstServiceFlowPhsRulesTable->stSFList[i];
-
-		if (!stServFlowEntry.bUsed)
-			continue;
-
-		phsrules_per_sf_dbg_print(Adapter, &stServFlowEntry);
-	}
-}
-
-/*
- * Procedure:   phs_decompress
- *
- * Description: This routine restores the static fields within the packet.
- *
- * Arguments:
- *	in_buf			- ptr to incoming packet buffer.
- *	out_buf			- ptr to output buffer where the suppressed
- *				  header is copied.
- *	decomp_phs_rules	- ptr to PHS rule.
- *	header_size		- ptr to field which holds the phss or
- *				  phsf_length.
- *
- * Returns:
- *	size	- The number of bytes of dynamic fields present with in the
- *		  incoming packet header.
- *	0	- If PHS rule is NULL.If PHSI is 0 indicateing packet as
- *		  uncompressed.
- */
-static int phs_decompress(unsigned char *in_buf,
-			  unsigned char *out_buf,
-			  struct bcm_phs_rule *decomp_phs_rules,
-			  UINT *header_size)
-{
-	int phss, size = 0;
-	struct bcm_phs_rule *tmp_memb;
-	int bit, i = 0;
-	unsigned char *phsf, *phsm;
-	int in_buf_len = *header_size - 1;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	in_buf++;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_RECEIVE, DBG_LVL_ALL,
-			"====>\n");
-	*header_size = 0;
-
-	if (decomp_phs_rules == NULL)
-		return 0;
-
-	tmp_memb = decomp_phs_rules;
-	/*
-	 * BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_RECEIVE,DBG_LVL_ALL,
-	 * "\nDECOMP:In phs_decompress PHSI 1  %d",phsi));
-	 * header_size = tmp_memb->u8PHSFLength;
-	 */
-	phss = tmp_memb->u8PHSS;
-	phsf = tmp_memb->u8PHSF;
-	phsm = tmp_memb->u8PHSM;
-
-	if (phss > MAX_PHS_LENGTHS)
-		phss = MAX_PHS_LENGTHS;
-
-	/*
-	 * BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_RECEIVE,DBG_LVL_ALL,
-	 * "\nDECOMP:
-	 * In phs_decompress PHSI  %d phss %d index %d",phsi,phss,index));
-	 */
-	while ((phss > 0) && (size < in_buf_len)) {
-		bit = ((*phsm << i) & SUPPRESS);
-
-		if (bit == SUPPRESS) {
-			*out_buf = *phsf;
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_RECEIVE,
-					DBG_LVL_ALL,
-					"\nDECOMP:In phss  %d phsf %d output %d",
-					phss, *phsf, *out_buf);
-		} else {
-			*out_buf = *in_buf;
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_RECEIVE,
-					DBG_LVL_ALL,
-					"\nDECOMP:In phss  %d input %d output %d",
-					phss, *in_buf, *out_buf);
-			in_buf++;
-			size++;
-		}
-		out_buf++;
-		phsf++;
-		phss--;
-		i++;
-		*header_size = *header_size + 1;
-
-		if (i > MAX_NO_BIT) {
-			i = 0;
-			phsm++;
-		}
-	}
-
-	return size;
-}
-
-/*
- * Procedure:   phs_compress
- *
- * Description: This routine suppresses the static fields within the packet.
- * Before that it will verify the fields to be suppressed with the corresponding
- * fields in the phsf. For verification it checks the phsv field of PHS rule.
- * If set and verification succeeds it suppresses the field.If any one static
- * field is found different none of the static fields are suppressed then the
- * packet is sent as uncompressed packet with phsi=0.
- *
- * Arguments:
- *	phs_rule - ptr to PHS rule.
- *	in_buf		- ptr to incoming packet buffer.
- *	out_buf		- ptr to output buffer where the suppressed header is
- *			  copied.
- *	header_size	- ptr to field which holds the phss.
- *
- * Returns:
- *	size	- The number of bytes copied into the output buffer i.e
- *		  dynamic fields
- *	0	- If PHS rule is NULL.If PHSV field is not set. If the
- *		  verification fails.
- */
-static int phs_compress(struct bcm_phs_rule *phs_rule,
-			unsigned char *in_buf,
-			unsigned char *out_buf,
-			UINT *header_size,
-			UINT *new_header_size)
-{
-	unsigned char *old_addr = out_buf;
-	int suppress = 0;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	if (phs_rule == NULL) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,
-				"\nphs_compress(): phs_rule null!");
-		*out_buf = ZERO_PHSI;
-		return STATUS_PHS_NOCOMPRESSION;
-	}
-
-	if (phs_rule->u8PHSS <= *new_header_size)
-		*header_size = phs_rule->u8PHSS;
-	else
-		*header_size = *new_header_size;
-
-	/* To copy PHSI */
-	out_buf++;
-	suppress = verify_suppress_phsf(in_buf, out_buf, phs_rule->u8PHSF,
-					phs_rule->u8PHSM, phs_rule->u8PHSS,
-					phs_rule->u8PHSV, new_header_size);
-
-	if (suppress == STATUS_PHS_COMPRESSED) {
-		*old_addr = (unsigned char)phs_rule->u8PHSI;
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,
-				"\nCOMP:In phs_compress phsi %d",
-				phs_rule->u8PHSI);
-	} else {
-		*old_addr = ZERO_PHSI;
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,
-				"\nCOMP:In phs_compress PHSV Verification failed");
-	}
-
-	return suppress;
-}
-
-/*
- * Procedure:	verify_suppress_phsf
- *
- * Description: This routine verifies the fields of the packet and if all the
- * static fields are equal it adds the phsi of that PHS rule.If any static
- * field differs it woun't suppress any field.
- *
- * Arguments:
- * rules_set	- ptr to classifier_rules.
- * in_buffer	- ptr to incoming packet buffer.
- * out_buffer	- ptr to output buffer where the suppressed header is copied.
- * phsf		- ptr to phsf.
- * phsm		- ptr to phsm.
- * phss		- variable holding phss.
- *
- * Returns:
- *	size    - The number of bytes copied into the output buffer i.e dynamic
- *		  fields.
- *	0	- Packet has failed the verification.
- */
-static int verify_suppress_phsf(unsigned char *in_buffer,
-				unsigned char *out_buffer,
-				unsigned char *phsf,
-				unsigned char *phsm,
-				unsigned int phss,
-				unsigned int phsv,
-				UINT *new_header_size)
-{
-	unsigned int size = 0;
-	int bit, i = 0;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,
-			"\nCOMP:In verify_phsf PHSM - 0x%X", *phsm);
-
-	if (phss > (*new_header_size))
-		phss = *new_header_size;
-
-	while (phss > 0) {
-		bit = ((*phsm << i) & SUPPRESS);
-		if (bit == SUPPRESS) {
-			if (*in_buffer != *phsf) {
-				if (phsv == VERIFY) {
-					BCM_DEBUG_PRINT(Adapter,
-							DBG_TYPE_OTHERS,
-							PHS_SEND,
-							DBG_LVL_ALL,
-							"\nCOMP:In verify_phsf failed for field  %d buf  %d phsf %d",
-							phss,
-							*in_buffer,
-							*phsf);
-					return STATUS_PHS_NOCOMPRESSION;
-				}
-			} else
-				BCM_DEBUG_PRINT(Adapter,
-						DBG_TYPE_OTHERS,
-						PHS_SEND,
-						DBG_LVL_ALL,
-						"\nCOMP:In verify_phsf success for field  %d buf  %d phsf %d",
-						phss,
-						*in_buffer,
-						*phsf);
-		} else {
-			*out_buffer = *in_buffer;
-			BCM_DEBUG_PRINT(Adapter,
-					DBG_TYPE_OTHERS,
-					PHS_SEND,
-					DBG_LVL_ALL,
-					"\nCOMP:In copying_header input %d  out %d",
-					*in_buffer,
-					*out_buffer);
-			out_buffer++;
-			size++;
-		}
-
-		in_buffer++;
-		phsf++;
-		phss--;
-		i++;
-
-		if (i > MAX_NO_BIT) {
-			i = 0;
-			phsm++;
-		}
-	}
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,
-			"\nCOMP:In verify_phsf success");
-	*new_header_size = size;
-	return STATUS_PHS_COMPRESSED;
-}
diff --git a/drivers/staging/bcm/PHSModule.h b/drivers/staging/bcm/PHSModule.h
deleted file mode 100644
index d84d60ba48f9..000000000000
--- a/drivers/staging/bcm/PHSModule.h
+++ /dev/null
@@ -1,59 +0,0 @@
-#ifndef BCM_MINIPORT_PHSMODULE_H
-#define BCM_MINIPORT_PHSMODULE_H
-
-int PHSTransmit(struct bcm_mini_adapter *Adapter,
-					struct sk_buff **pPacket,
-					 USHORT Vcid,
-					 B_UINT16 uiClassifierRuleID,
-					 bool bHeaderSuppressionEnabled,
-					 PUINT PacketLen,
-					 UCHAR bEthCSSupport);
-
-int PHSReceive(struct bcm_mini_adapter *Adapter,
-					USHORT usVcid,
-					struct sk_buff *packet,
-					UINT *punPacketLen,
-					UCHAR *pucEthernetHdr,
-					UINT
-					);
-
-
-void DumpDataPacketHeader(PUCHAR pPkt);
-
-void DumpFullPacket(UCHAR *pBuf, UINT nPktLen);
-
-void DumpPhsRules(struct bcm_phs_extension *pDeviceExtension);
-
-
-int phs_init(struct bcm_phs_extension *pPhsdeviceExtension,
-	     struct bcm_mini_adapter *Adapter);
-
-int PhsCleanup(struct bcm_phs_extension *pPHSDeviceExt);
-
-/* Utility Functions */
-ULONG PhsUpdateClassifierRule(void *pvContext,
-			      B_UINT16 uiVcid,
-			      B_UINT16 uiClsId,
-			      struct bcm_phs_rule *psPhsRule,
-			      B_UINT8  u8AssociatedPHSI);
-
-ULONG PhsDeletePHSRule(void *pvContext, B_UINT16 uiVcid, B_UINT8 u8PHSI);
-
-ULONG PhsDeleteClassifierRule(void *pvContext,
-			      B_UINT16 uiVcid,
-			      B_UINT16  uiClsId);
-
-ULONG PhsDeleteSFRules(void *pvContext, B_UINT16 uiVcid);
-
-
-bool ValidatePHSRule(struct bcm_phs_rule *psPhsRule);
-
-UINT GetServiceFlowEntry(struct bcm_phs_table *psServiceFlowTable,
-			 B_UINT16 uiVcid,
-			 struct bcm_phs_entry **ppstServiceFlowEntry);
-
-
-void DumpPhsRules(struct bcm_phs_extension *pDeviceExtension);
-
-
-#endif
diff --git a/drivers/staging/bcm/Protocol.h b/drivers/staging/bcm/Protocol.h
deleted file mode 100644
index 9818128d9320..000000000000
--- a/drivers/staging/bcm/Protocol.h
+++ /dev/null
@@ -1,128 +0,0 @@
-/************************************
-*	Protocol.h
-*************************************/
-#ifndef	__PROTOCOL_H__
-#define	__PROTOCOL_H__
-
-#define IPV4 4
-#define IPV6 6
-
-struct ArpHeader {
-	struct arphdr arp;
-	unsigned char ar_sha[ETH_ALEN];	/* sender hardware address  */
-	unsigned char ar_sip[4];	/* sender IP address        */
-	unsigned char ar_tha[ETH_ALEN];	/* target hardware address  */
-	unsigned char ar_tip[4];	/* target IP address        */
-};
-
-struct bcm_transport_header {
-	union {
-		struct udphdr uhdr;
-		struct tcphdr thdr;
-	};
-} __packed;
-
-enum bcm_ip_frame_type {
-	eNonIPPacket,
-	eIPv4Packet,
-	eIPv6Packet
-};
-
-enum bcm_eth_frame_type {
-	eEthUnsupportedFrame,
-	eEth802LLCFrame,
-	eEth802LLCSNAPFrame,
-	eEth802QVLANFrame,
-	eEthOtherFrame
-};
-
-struct bcm_eth_packet_info {
-	enum bcm_ip_frame_type  eNwpktIPFrameType;
-	enum bcm_eth_frame_type eNwpktEthFrameType;
-	unsigned short	usEtherType;
-	unsigned char	ucDSAP;
-};
-
-struct bcm_eth_q_frame {
-	struct bcm_eth_header EThHdr;
-	unsigned short UserPriority:3;
-	unsigned short CFI:1;
-	unsigned short VLANID:12;
-	unsigned short EthType;
-} __packed;
-
-struct bcm_eth_llc_frame {
-	struct bcm_eth_header EThHdr;
-	unsigned char DSAP;
-	unsigned char SSAP;
-	unsigned char Control;
-} __packed;
-
-struct bcm_eth_llc_snap_frame {
-	struct bcm_eth_header EThHdr;
-	unsigned char DSAP;
-	unsigned char SSAP;
-	unsigned char Control;
-	unsigned char OUI[3];
-	unsigned short usEtherType;
-} __packed;
-
-struct bcm_ethernet2_frame {
-	struct bcm_eth_header EThHdr;
-} __packed;
-
-#define ETHERNET_FRAMETYPE_IPV4		ntohs(0x0800)
-#define ETHERNET_FRAMETYPE_IPV6		ntohs(0x86dd)
-#define ETHERNET_FRAMETYPE_802QVLAN	ntohs(0x8100)
-
-/* Per SF CS Specification Encodings */
-enum bcm_spec_encoding {
-	eCSSpecUnspecified = 0,
-	eCSPacketIPV4,
-	eCSPacketIPV6,
-	eCS802_3PacketEthernet,
-	eCS802_1QPacketVLAN,
-	eCSPacketIPV4Over802_3Ethernet,
-	eCSPacketIPV6Over802_3Ethernet,
-	eCSPacketIPV4Over802_1QVLAN,
-	eCSPacketIPV6Over802_1QVLAN,
-	eCSPacketUnsupported
-};
-
-#define	IP6_HEADER_LEN		40
-#define IP_VERSION(byte)	(((byte&0xF0)>>4))
-
-#define MAC_ADDRESS_SIZE	6
-#define	ETH_AND_IP_HEADER_LEN	(14 + 20)
-#define L4_SRC_PORT_LEN		2
-#define L4_DEST_PORT_LEN	2
-#define	CTRL_PKT_LEN		(8 + ETH_AND_IP_HEADER_LEN)
-
-#define	ETH_ARP_FRAME		0x806
-#define	ETH_IPV4_FRAME		0x800
-#define	ETH_IPV6_FRAME		0x86DD
-#define UDP			0x11
-#define TCP			0x06
-
-#define	ARP_OP_REQUEST		0x01
-#define	ARP_OP_REPLY		0x02
-#define	ARP_PKT_SIZE		60
-
-/* This is the format for the TCP packet header */
-struct bcm_tcp_header {
-	unsigned short usSrcPort;
-	unsigned short usDestPort;
-	unsigned long  ulSeqNumber;
-	unsigned long  ulAckNumber;
-	unsigned char  HeaderLength;
-	unsigned char  ucFlags;
-	unsigned short usWindowsSize;
-	unsigned short usChkSum;
-	unsigned short usUrgetPtr;
-};
-
-#define TCP_HEADER_LEN		sizeof(struct bcm_tcp_header)
-#define TCP_ACK			0x10  /* Bit 4 in tcpflags field. */
-#define GET_TCP_HEADER_LEN(byte) ((byte&0xF0)>>4)
-
-#endif /* __PROTOCOL_H__ */
diff --git a/drivers/staging/bcm/Prototypes.h b/drivers/staging/bcm/Prototypes.h
deleted file mode 100644
index 1ddc8b2539f6..000000000000
--- a/drivers/staging/bcm/Prototypes.h
+++ /dev/null
@@ -1,217 +0,0 @@
-#ifndef _PROTOTYPES_H_
-#define _PROTOTYPES_H_
-
-VOID LinkControlResponseMessage(struct bcm_mini_adapter *Adapter, PUCHAR pucBuffer);
-
-VOID StatisticsResponse(struct bcm_mini_adapter *Adapter, PVOID pvBuffer);
-
-VOID IdleModeResponse(struct bcm_mini_adapter *Adapter, PUINT puiBuffer);
-
-int control_packet_handler(struct bcm_mini_adapter *Adapter);
-
-VOID DeleteAllClassifiersForSF(struct bcm_mini_adapter *Adapter, UINT uiSearchRuleIndex);
-
-VOID flush_all_queues(struct bcm_mini_adapter *Adapter);
-
-int register_control_device_interface(struct bcm_mini_adapter *ps_adapter);
-
-void unregister_control_device_interface(struct bcm_mini_adapter *Adapter);
-
-INT CopyBufferToControlPacket(struct bcm_mini_adapter *Adapter,/**<Logical Adapter*/
-									  PVOID ioBuffer/**<Control Packet Buffer*/
-									  );
-
-VOID SortPackInfo(struct bcm_mini_adapter *Adapter);
-
-VOID SortClassifiers(struct bcm_mini_adapter *Adapter);
-
-VOID flush_all_queues(struct bcm_mini_adapter *Adapter);
-
-VOID PruneQueueAllSF(struct bcm_mini_adapter *Adapter);
-
-INT SearchSfid(struct bcm_mini_adapter *Adapter, UINT uiSfid);
-
-USHORT ClassifyPacket(struct bcm_mini_adapter *Adapter, struct sk_buff *skb);
-
-bool MatchSrcPort(struct bcm_classifier_rule *pstClassifierRule, USHORT ushSrcPort);
-
-bool MatchDestPort(struct bcm_classifier_rule *pstClassifierRule, USHORT ushSrcPort);
-
-bool MatchProtocol(struct bcm_classifier_rule *pstClassifierRule, UCHAR ucProtocol);
-
-INT SetupNextSend(struct bcm_mini_adapter *Adapter, /**<Logical Adapter*/
-					struct sk_buff *Packet, /**<data buffer*/
-					USHORT Vcid);
-
-VOID LinkMessage(struct bcm_mini_adapter *Adapter);
-
-VOID transmit_packets(struct bcm_mini_adapter *Adapter);
-
-INT SendControlPacket(struct bcm_mini_adapter *Adapter, /**<Logical Adapter*/
-							char *pControlPacket/**<Control Packet*/
-							);
-
-int register_networkdev(struct bcm_mini_adapter *Adapter);
-
-void unregister_networkdev(struct bcm_mini_adapter *Adapter);
-
-INT AllocAdapterDsxBuffer(struct bcm_mini_adapter *Adapter);
-
-VOID AdapterFree(struct bcm_mini_adapter *Adapter);
-
-INT FreeAdapterDsxBuffer(struct bcm_mini_adapter *Adapter);
-
-int tx_pkt_handler(struct bcm_mini_adapter *Adapter);
-
-int reset_card_proc(struct bcm_mini_adapter *Adapter);
-
-int run_card_proc(struct bcm_mini_adapter *Adapter);
-
-int InitCardAndDownloadFirmware(struct bcm_mini_adapter *ps_adapter);
-
-INT ReadMacAddressFromNVM(struct bcm_mini_adapter *Adapter);
-
-int register_control_device_interface(struct bcm_mini_adapter *ps_adapter);
-
-void DumpPackInfo(struct bcm_mini_adapter *Adapter);
-
-int rdm(struct bcm_mini_adapter *Adapter, UINT uiAddress, PCHAR pucBuff, size_t size);
-
-int wrm(struct bcm_mini_adapter *Adapter, UINT uiAddress, PCHAR pucBuff, size_t size);
-
-int wrmalt(struct bcm_mini_adapter *Adapter, UINT uiAddress, unsigned int *pucBuff, size_t sSize);
-
-int rdmalt(struct bcm_mini_adapter *Adapter, UINT uiAddress, unsigned int *pucBuff, size_t sSize);
-
-int get_dsx_sf_data_to_application(struct bcm_mini_adapter *Adapter, UINT uiSFId, void __user *user_buffer);
-
-void SendIdleModeResponse(struct bcm_mini_adapter *Adapter);
-
-int ProcessGetHostMibs(struct bcm_mini_adapter *Adapter, struct bcm_host_stats_mibs *buf);
-
-void GetDroppedAppCntrlPktMibs(struct bcm_host_stats_mibs *ioBuffer, struct bcm_tarang_data *pTarang);
-
-void beceem_parse_target_struct(struct bcm_mini_adapter *Adapter);
-
-int bcm_ioctl_fw_download(struct bcm_mini_adapter *Adapter, struct bcm_firmware_info *psFwInfo);
-
-void CopyMIBSExtendedSFParameters(struct bcm_mini_adapter *Adapter,
-		struct bcm_connect_mgr_params *psfLocalSet, UINT uiSearchRuleIndex);
-
-VOID ResetCounters(struct bcm_mini_adapter *Adapter);
-
-int InitLedSettings(struct bcm_mini_adapter *Adapter);
-
-struct bcm_classifier_rule *GetFragIPClsEntry(struct bcm_mini_adapter *Adapter, USHORT usIpIdentification, ULONG SrcIP);
-
-void AddFragIPClsEntry(struct bcm_mini_adapter *Adapter, struct bcm_fragmented_packet_info *psFragPktInfo);
-
-void DelFragIPClsEntry(struct bcm_mini_adapter *Adapter, USHORT usIpIdentification, ULONG SrcIp);
-
-void update_per_cid_rx(struct bcm_mini_adapter *Adapter);
-
-void update_per_sf_desc_cnts(struct bcm_mini_adapter *Adapter);
-
-void ClearTargetDSXBuffer(struct bcm_mini_adapter *Adapter, B_UINT16 TID, bool bFreeAll);
-
-void flush_queue(struct bcm_mini_adapter *Adapter, UINT iQIndex);
-
-INT flushAllAppQ(VOID);
-
-INT BeceemEEPROMBulkRead(
-	struct bcm_mini_adapter *Adapter,
-	PUINT pBuffer,
-	UINT uiOffset,
-	UINT uiNumBytes);
-
-INT WriteBeceemEEPROM(struct bcm_mini_adapter *Adapter, UINT uiEEPROMOffset, UINT uiData);
-
-INT PropagateCalParamsFromFlashToMemory(struct bcm_mini_adapter *Adapter);
-
-INT BeceemEEPROMBulkWrite(
-	struct bcm_mini_adapter *Adapter,
-	PUCHAR pBuffer,
-	UINT uiOffset,
-	UINT uiNumBytes,
-	bool bVerify);
-
-INT ReadBeceemEEPROM(struct bcm_mini_adapter *Adapter, UINT dwAddress, UINT *pdwData);
-
-INT BeceemNVMRead(
-	struct bcm_mini_adapter *Adapter,
-	PUINT pBuffer,
-	UINT uiOffset,
-	UINT uiNumBytes);
-
-INT BeceemNVMWrite(
-	struct bcm_mini_adapter *Adapter,
-	PUINT pBuffer,
-	UINT uiOffset,
-	UINT uiNumBytes,
-	bool bVerify);
-
-INT BcmInitNVM(struct bcm_mini_adapter *Adapter);
-
-INT BcmUpdateSectorSize(struct bcm_mini_adapter *Adapter, UINT uiSectorSize);
-
-bool IsSectionExistInFlash(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_section_val section);
-
-INT BcmGetFlash2xSectionalBitMap(struct bcm_mini_adapter *Adapter, struct bcm_flash2x_bitmap *psFlash2xBitMap);
-
-INT BcmFlash2xBulkWrite(
-	struct bcm_mini_adapter *Adapter,
-	PUINT pBuffer,
-	enum bcm_flash2x_section_val eFlashSectionVal,
-	UINT uiOffset,
-	UINT uiNumBytes,
-	UINT bVerify);
-
-INT BcmFlash2xBulkRead(
-	struct bcm_mini_adapter *Adapter,
-	PUINT pBuffer,
-	enum bcm_flash2x_section_val eFlashSectionVal,
-	UINT uiOffsetWithinSectionVal,
-	UINT uiNumBytes);
-
-INT BcmGetSectionValStartOffset(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_section_val eFlashSectionVal);
-
-INT BcmSetActiveSection(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_section_val eFlash2xSectVal);
-
-INT BcmAllocFlashCSStructure(struct bcm_mini_adapter *psAdapter);
-
-INT BcmDeAllocFlashCSStructure(struct bcm_mini_adapter *psAdapter);
-
-INT BcmCopyISO(struct bcm_mini_adapter *Adapter, struct bcm_flash2x_copy_section sCopySectStrut);
-
-INT BcmFlash2xCorruptSig(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_section_val eFlash2xSectionVal);
-
-INT BcmFlash2xWriteSig(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_section_val eFlashSectionVal);
-
-INT validateFlash2xReadWrite(struct bcm_mini_adapter *Adapter, struct bcm_flash2x_readwrite *psFlash2xReadWrite);
-
-INT IsFlash2x(struct bcm_mini_adapter *Adapter);
-
-INT BcmCopySection(struct bcm_mini_adapter *Adapter,
-						enum bcm_flash2x_section_val SrcSection,
-						enum bcm_flash2x_section_val DstSection,
-						UINT offset,
-						UINT numOfBytes);
-
-bool IsNonCDLessDevice(struct bcm_mini_adapter *Adapter);
-
-VOID OverrideServiceFlowParams(struct bcm_mini_adapter *Adapter, PUINT puiBuffer);
-
-int wrmaltWithLock(struct bcm_mini_adapter *Adapter, UINT uiAddress, unsigned int *pucBuff, size_t sSize);
-
-int rdmaltWithLock(struct bcm_mini_adapter *Adapter, UINT uiAddress, unsigned int *pucBuff, size_t sSize);
-
-int wrmWithLock(struct bcm_mini_adapter *Adapter, UINT uiAddress, PCHAR pucBuff, size_t size);
-
-INT buffDnldVerify(struct bcm_mini_adapter *Adapter, unsigned char *mappedbuffer, unsigned int u32FirmwareLength,
-		unsigned long u32StartingAddress);
-
-VOID putUsbSuspend(struct work_struct *work);
-
-bool IsReqGpioIsLedInNVM(struct bcm_mini_adapter *Adapter, UINT gpios);
-
-#endif
diff --git a/drivers/staging/bcm/Qos.c b/drivers/staging/bcm/Qos.c
deleted file mode 100644
index b3ac614cd35f..000000000000
--- a/drivers/staging/bcm/Qos.c
+++ /dev/null
@@ -1,1200 +0,0 @@
-/**
- * @file Qos.C
- * This file contains the routines related to Quality of Service.
-*/
-#include "headers.h"
-
-static void EThCSGetPktInfo(struct bcm_mini_adapter *Adapter,
-			    PVOID pvEthPayload,
-			    struct bcm_eth_packet_info *pstEthCsPktInfo);
-
-static bool EThCSClassifyPkt(struct bcm_mini_adapter *Adapter,
-			     struct sk_buff *skb,
-			     struct bcm_eth_packet_info *pstEthCsPktInfo,
-			     struct bcm_classifier_rule *pstClassifierRule,
-			     B_UINT8 EthCSCupport);
-
-static USHORT IpVersion4(struct bcm_mini_adapter *Adapter, struct iphdr *iphd,
-			 struct bcm_classifier_rule *pstClassifierRule);
-
-static VOID PruneQueue(struct bcm_mini_adapter *Adapter, INT iIndex);
-
-
-/*******************************************************************
-* Function    - MatchSrcIpAddress()
-*
-* Description - Checks whether the Source IP address from the packet
-*				matches with that of Queue.
-*
-* Parameters  - pstClassifierRule: Pointer to the packet info structure.
-*		- ulSrcIP	    : Source IP address from the packet.
-*
-* Returns     - TRUE(If address matches) else FAIL .
-*********************************************************************/
-static bool MatchSrcIpAddress(struct bcm_classifier_rule *pstClassifierRule,
-			      ULONG ulSrcIP)
-{
-	UCHAR ucLoopIndex = 0;
-
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	union u_ip_address	*src_addr;
-
-	ulSrcIP = ntohl(ulSrcIP);
-	if (0 == pstClassifierRule->ucIPSourceAddressLength)
-		return TRUE;
-	for (ucLoopIndex = 0;
-	     ucLoopIndex < (pstClassifierRule->ucIPSourceAddressLength);
-	     ucLoopIndex++) {
-		src_addr = &pstClassifierRule->stSrcIpAddress;
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"Src Ip Address Mask:0x%x PacketIp:0x%x and Classification:0x%x",
-				(UINT)src_addr->ulIpv4Mask[ucLoopIndex],
-				(UINT)ulSrcIP,
-				(UINT)src_addr->ulIpv6Addr[ucLoopIndex]);
-
-		if ((src_addr->ulIpv4Mask[ucLoopIndex] & ulSrcIP) ==
-				(src_addr->ulIpv4Addr[ucLoopIndex] &
-				 src_addr->ulIpv4Mask[ucLoopIndex]))
-			return TRUE;
-	}
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"Src Ip Address Not Matched");
-	return false;
-}
-
-
-/*******************************************************************
-* Function    - MatchDestIpAddress()
-*
-* Description - Checks whether the Destination IP address from the packet
-*				matches with that of Queue.
-*
-* Parameters  - pstClassifierRule: Pointer to the packet info structure.
-*		- ulDestIP    : Destination IP address from the packet.
-*
-* Returns     - TRUE(If address matches) else FAIL .
-*********************************************************************/
-static bool MatchDestIpAddress(struct bcm_classifier_rule *pstClassifierRule, ULONG ulDestIP)
-{
-	UCHAR ucLoopIndex = 0;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	union u_ip_address	*dest_addr = &pstClassifierRule->stDestIpAddress;
-
-	ulDestIP = ntohl(ulDestIP);
-	if (0 == pstClassifierRule->ucIPDestinationAddressLength)
-		return TRUE;
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"Destination Ip Address 0x%x 0x%x 0x%x  ",
-			(UINT)ulDestIP,
-			(UINT)dest_addr->ulIpv4Mask[ucLoopIndex],
-			(UINT)dest_addr->ulIpv4Addr[ucLoopIndex]);
-
-	for (ucLoopIndex = 0;
-	     ucLoopIndex < (pstClassifierRule->ucIPDestinationAddressLength);
-	     ucLoopIndex++) {
-		if ((dest_addr->ulIpv4Mask[ucLoopIndex] & ulDestIP) ==
-				(dest_addr->ulIpv4Addr[ucLoopIndex] &
-				 dest_addr->ulIpv4Mask[ucLoopIndex]))
-			return TRUE;
-	}
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"Destination Ip Address Not Matched");
-	return false;
-}
-
-
-/************************************************************************
-* Function    - MatchTos()
-*
-* Description - Checks the TOS from the packet matches with that of queue.
-*
-* Parameters  - pstClassifierRule   : Pointer to the packet info structure.
-*		- ucTypeOfService: TOS from the packet.
-*
-* Returns     - TRUE(If address matches) else FAIL.
-**************************************************************************/
-static bool MatchTos(struct bcm_classifier_rule *pstClassifierRule,
-		     UCHAR ucTypeOfService)
-{
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	if (3 != pstClassifierRule->ucIPTypeOfServiceLength)
-		return TRUE;
-
-	if (((pstClassifierRule->ucTosMask & ucTypeOfService) <=
-				pstClassifierRule->ucTosHigh) &&
-			((pstClassifierRule->ucTosMask & ucTypeOfService) >=
-				pstClassifierRule->ucTosLow))
-		return TRUE;
-
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"Type Of Service Not Matched");
-	return false;
-}
-
-
-/***************************************************************************
-* Function    - MatchProtocol()
-*
-* Description - Checks the protocol from the packet matches with that of queue.
-*
-* Parameters  - pstClassifierRule: Pointer to the packet info structure.
-*		- ucProtocol	: Protocol from the packet.
-*
-* Returns     - TRUE(If address matches) else FAIL.
-****************************************************************************/
-bool MatchProtocol(struct bcm_classifier_rule *pstClassifierRule,
-		   UCHAR ucProtocol)
-{
-	UCHAR ucLoopIndex = 0;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	if (0 == pstClassifierRule->ucProtocolLength)
-		return TRUE;
-	for (ucLoopIndex = 0;
-	     ucLoopIndex < pstClassifierRule->ucProtocolLength;
-	     ucLoopIndex++) {
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"Protocol:0x%X Classification Protocol:0x%X",
-				ucProtocol,
-				pstClassifierRule->ucProtocol[ucLoopIndex]);
-		if (pstClassifierRule->ucProtocol[ucLoopIndex] == ucProtocol)
-			return TRUE;
-	}
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"Protocol Not Matched");
-	return false;
-}
-
-
-/***********************************************************************
-* Function    - MatchSrcPort()
-*
-* Description - Checks, Source port from the packet matches with that of queue.
-*
-* Parameters  - pstClassifierRule: Pointer to the packet info structure.
-*		- ushSrcPort	: Source port from the packet.
-*
-* Returns     - TRUE(If address matches) else FAIL.
-***************************************************************************/
-bool MatchSrcPort(struct bcm_classifier_rule *pstClassifierRule,
-		  USHORT ushSrcPort)
-{
-	UCHAR ucLoopIndex = 0;
-
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-
-	if (0 == pstClassifierRule->ucSrcPortRangeLength)
-		return TRUE;
-	for (ucLoopIndex = 0;
-	     ucLoopIndex < pstClassifierRule->ucSrcPortRangeLength;
-	     ucLoopIndex++) {
-		if (ushSrcPort <= pstClassifierRule->usSrcPortRangeHi[ucLoopIndex] &&
-			ushSrcPort >= pstClassifierRule->usSrcPortRangeLo[ucLoopIndex])
-			return TRUE;
-	}
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"Src Port: %x Not Matched ",
-			ushSrcPort);
-	return false;
-}
-
-
-/***********************************************************************
-* Function    - MatchDestPort()
-*
-* Description - Checks, Destination port from packet matches with that of queue.
-*
-* Parameters  - pstClassifierRule: Pointer to the packet info structure.
-*		- ushDestPort	: Destination port from the packet.
-*
-* Returns     - TRUE(If address matches) else FAIL.
-***************************************************************************/
-bool MatchDestPort(struct bcm_classifier_rule *pstClassifierRule,
-		   USHORT ushDestPort)
-{
-	UCHAR ucLoopIndex = 0;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	if (0 == pstClassifierRule->ucDestPortRangeLength)
-		return TRUE;
-
-	for (ucLoopIndex = 0;
-	     ucLoopIndex < pstClassifierRule->ucDestPortRangeLength;
-	     ucLoopIndex++) {
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"Matching Port:0x%X   0x%X  0x%X",
-				ushDestPort,
-				pstClassifierRule->usDestPortRangeLo[ucLoopIndex],
-				pstClassifierRule->usDestPortRangeHi[ucLoopIndex]);
-
-		if (ushDestPort <= pstClassifierRule->usDestPortRangeHi[ucLoopIndex] &&
-			ushDestPort >= pstClassifierRule->usDestPortRangeLo[ucLoopIndex])
-			return TRUE;
-	}
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"Dest Port: %x Not Matched",
-			ushDestPort);
-	return false;
-}
-/**
- * @ingroup tx_functions
- * Compares IPV4 Ip address and port number
- * @return Queue Index.
-*/
-static USHORT	IpVersion4(struct bcm_mini_adapter *Adapter,
-			   struct iphdr *iphd,
-			   struct bcm_classifier_rule *pstClassifierRule)
-{
-	struct bcm_transport_header *xprt_hdr = NULL;
-	bool	bClassificationSucceed = false;
-
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"========>");
-
-	xprt_hdr = (struct bcm_transport_header *)((PUCHAR)iphd + sizeof(struct iphdr));
-
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"Trying to see Direction = %d %d",
-			pstClassifierRule->ucDirection,
-			pstClassifierRule->usVCID_Value);
-
-	/* Checking classifier validity */
-	if (!pstClassifierRule->bUsed ||
-			pstClassifierRule->ucDirection == DOWNLINK_DIR)
-		goto out;
-
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"is IPv6 check!");
-	if (pstClassifierRule->bIpv6Protocol)
-		goto out;
-
-	/* Checking IP header parameter */
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"Trying to match Source IP Address");
-	if (!MatchSrcIpAddress(pstClassifierRule, iphd->saddr))
-		goto out;
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"Source IP Address Matched");
-
-	if (!MatchDestIpAddress(pstClassifierRule, iphd->daddr))
-		goto out;
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"Destination IP Address Matched");
-
-	if (!MatchTos(pstClassifierRule, iphd->tos)) {
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"TOS Match failed\n");
-		goto out;
-	}
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"TOS Matched");
-
-	if (!MatchProtocol(pstClassifierRule, iphd->protocol))
-		goto out;
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"Protocol Matched");
-
-	/*
-	 * if protocol is not TCP or UDP then no
-	 * need of comparing source port and destination port
-	 */
-	if (iphd->protocol != TCP && iphd->protocol != UDP) {
-		bClassificationSucceed = TRUE;
-		goto out;
-	}
-	/* Checking Transport Layer Header field if present */
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"Source Port %04x",
-			(iphd->protocol == UDP) ? xprt_hdr->uhdr.source : xprt_hdr->thdr.source);
-
-	if (!MatchSrcPort(pstClassifierRule,
-			  ntohs((iphd->protocol == UDP) ?
-			  xprt_hdr->uhdr.source : xprt_hdr->thdr.source)))
-		goto out;
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"Src Port Matched");
-
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"Destination Port %04x",
-			(iphd->protocol == UDP) ? xprt_hdr->uhdr.dest :
-				xprt_hdr->thdr.dest);
-
-	if (!MatchDestPort(pstClassifierRule,
-			   ntohs((iphd->protocol == UDP) ?
-			   xprt_hdr->uhdr.dest : xprt_hdr->thdr.dest)))
-		goto out;
-	bClassificationSucceed = TRUE;
-
-out:
-	if (TRUE == bClassificationSucceed) {
-		INT iMatchedSFQueueIndex = 0;
-
-		iMatchedSFQueueIndex =
-			SearchSfid(Adapter, pstClassifierRule->ulSFID);
-		if (iMatchedSFQueueIndex >= NO_OF_QUEUES)
-			bClassificationSucceed = false;
-		else if (false == Adapter->PackInfo[iMatchedSFQueueIndex].bActive)
-			bClassificationSucceed = false;
-	}
-
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"IpVersion4 <==========");
-
-	return bClassificationSucceed;
-}
-
-VOID PruneQueueAllSF(struct bcm_mini_adapter *Adapter)
-{
-	UINT iIndex = 0;
-
-	for (iIndex = 0; iIndex < HiPriority; iIndex++) {
-		if (!Adapter->PackInfo[iIndex].bValid)
-			continue;
-
-		PruneQueue(Adapter, iIndex);
-	}
-}
-
-
-/**
- * @ingroup tx_functions
- * This function checks if the max queue size for a queue
- * is less than number of bytes in the queue. If so -
- * drops packets from the Head till the number of bytes is
- * less than or equal to max queue size for the queue.
- */
-static VOID PruneQueue(struct bcm_mini_adapter *Adapter, INT iIndex)
-{
-	struct sk_buff *PacketToDrop = NULL;
-	struct net_device_stats *netstats;
-	struct bcm_packet_info	*curr_pack_info = &Adapter->PackInfo[iIndex];
-
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			PRUNE_QUEUE,
-			DBG_LVL_ALL,
-			"=====> Index %d",
-			iIndex);
-
-	if (iIndex == HiPriority)
-		return;
-
-	if (!Adapter || (iIndex < 0) || (iIndex > HiPriority))
-		return;
-
-	/* To Store the netdevice statistic */
-	netstats = &Adapter->dev->stats;
-
-	spin_lock_bh(&curr_pack_info->SFQueueLock);
-
-	while (1) {
-/*	while((UINT)curr_pack_info->uiCurrentPacketsOnHost >
-		SF_MAX_ALLOWED_PACKETS_TO_BACKUP) { */
-
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				PRUNE_QUEUE,
-				DBG_LVL_ALL,
-				"uiCurrentBytesOnHost:%x uiMaxBucketSize :%x",
-				curr_pack_info->uiCurrentBytesOnHost,
-				curr_pack_info->uiMaxBucketSize);
-
-		PacketToDrop = curr_pack_info->FirstTxQueue;
-
-		if (PacketToDrop == NULL)
-			break;
-		if ((curr_pack_info->uiCurrentPacketsOnHost <
-					SF_MAX_ALLOWED_PACKETS_TO_BACKUP) &&
-			((1000*(jiffies - *((B_UINT32 *)(PacketToDrop->cb) +
-					    SKB_CB_LATENCY_OFFSET))/HZ) <=
-				curr_pack_info->uiMaxLatency))
-			break;
-
-		if (PacketToDrop) {
-			if (netif_msg_tx_err(Adapter))
-				pr_info(PFX "%s: tx queue %d overlimit\n",
-					Adapter->dev->name, iIndex);
-
-			netstats->tx_dropped++;
-
-			DEQUEUEPACKET(curr_pack_info->FirstTxQueue,
-				      curr_pack_info->LastTxQueue);
-			/* update current bytes and packets count */
-			curr_pack_info->uiCurrentBytesOnHost -=
-				PacketToDrop->len;
-			curr_pack_info->uiCurrentPacketsOnHost--;
-			/* update dropped bytes and packets counts */
-			curr_pack_info->uiDroppedCountBytes += PacketToDrop->len;
-			curr_pack_info->uiDroppedCountPackets++;
-			dev_kfree_skb(PacketToDrop);
-
-		}
-
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				PRUNE_QUEUE,
-				DBG_LVL_ALL,
-				"Dropped Bytes:%x Dropped Packets:%x",
-				curr_pack_info->uiDroppedCountBytes,
-				curr_pack_info->uiDroppedCountPackets);
-
-		atomic_dec(&Adapter->TotalPacketCount);
-	}
-
-	spin_unlock_bh(&curr_pack_info->SFQueueLock);
-
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			PRUNE_QUEUE,
-			DBG_LVL_ALL,
-			"TotalPacketCount:%x",
-			atomic_read(&Adapter->TotalPacketCount));
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			PRUNE_QUEUE,
-			DBG_LVL_ALL,
-			"<=====");
-}
-
-VOID flush_all_queues(struct bcm_mini_adapter *Adapter)
-{
-	INT	iQIndex;
-	UINT uiTotalPacketLength;
-	struct sk_buff *PacketToDrop = NULL;
-	struct bcm_packet_info *curr_packet_info;
-
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_OTHERS,
-			DUMP_INFO,
-			DBG_LVL_ALL,
-			"=====>");
-
-	/* down(&Adapter->data_packet_queue_lock); */
-	for (iQIndex = LowPriority; iQIndex < HiPriority; iQIndex++) {
-		struct net_device_stats *netstats = &Adapter->dev->stats;
-
-		curr_packet_info = &Adapter->PackInfo[iQIndex];
-
-		spin_lock_bh(&curr_packet_info->SFQueueLock);
-		while (curr_packet_info->FirstTxQueue) {
-			PacketToDrop = curr_packet_info->FirstTxQueue;
-			if (PacketToDrop) {
-				uiTotalPacketLength = PacketToDrop->len;
-				netstats->tx_dropped++;
-			} else
-				uiTotalPacketLength = 0;
-
-			DEQUEUEPACKET(curr_packet_info->FirstTxQueue,
-				      curr_packet_info->LastTxQueue);
-
-			/* Free the skb */
-			dev_kfree_skb(PacketToDrop);
-
-			/* update current bytes and packets count */
-			curr_packet_info->uiCurrentBytesOnHost -= uiTotalPacketLength;
-			curr_packet_info->uiCurrentPacketsOnHost--;
-
-			/* update dropped bytes and packets counts */
-			curr_packet_info->uiDroppedCountBytes += uiTotalPacketLength;
-			curr_packet_info->uiDroppedCountPackets++;
-
-			BCM_DEBUG_PRINT(Adapter,
-					DBG_TYPE_OTHERS,
-					DUMP_INFO,
-					DBG_LVL_ALL,
-					"Dropped Bytes:%x Dropped Packets:%x",
-					curr_packet_info->uiDroppedCountBytes,
-					curr_packet_info->uiDroppedCountPackets);
-			atomic_dec(&Adapter->TotalPacketCount);
-		}
-		spin_unlock_bh(&curr_packet_info->SFQueueLock);
-	}
-	/* up(&Adapter->data_packet_queue_lock); */
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_OTHERS,
-			DUMP_INFO,
-			DBG_LVL_ALL,
-			"<=====");
-}
-
-USHORT ClassifyPacket(struct bcm_mini_adapter *Adapter, struct sk_buff *skb)
-{
-	INT uiLoopIndex = 0;
-	struct bcm_classifier_rule *pstClassifierRule = NULL;
-	struct bcm_eth_packet_info stEthCsPktInfo;
-	PVOID pvEThPayload = NULL;
-	struct iphdr *pIpHeader = NULL;
-	INT uiSfIndex = 0;
-	USHORT usIndex = Adapter->usBestEffortQueueIndex;
-	bool bFragmentedPkt = false, bClassificationSucceed = false;
-	USHORT usCurrFragment = 0;
-
-	struct bcm_tcp_header *pTcpHeader;
-	UCHAR IpHeaderLength;
-	UCHAR TcpHeaderLength;
-
-	pvEThPayload = skb->data;
-	*((UINT32 *) (skb->cb) + SKB_CB_TCPACK_OFFSET) = 0;
-	EThCSGetPktInfo(Adapter, pvEThPayload, &stEthCsPktInfo);
-
-	switch (stEthCsPktInfo.eNwpktEthFrameType) {
-	case eEth802LLCFrame:
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"ClassifyPacket : 802LLCFrame\n");
-		pIpHeader = pvEThPayload + sizeof(struct bcm_eth_llc_frame);
-		break;
-	case eEth802LLCSNAPFrame:
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"ClassifyPacket : 802LLC SNAP Frame\n");
-		pIpHeader = pvEThPayload +
-			sizeof(struct bcm_eth_llc_snap_frame);
-		break;
-	case eEth802QVLANFrame:
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"ClassifyPacket : 802.1Q VLANFrame\n");
-		pIpHeader = pvEThPayload + sizeof(struct bcm_eth_q_frame);
-		break;
-	case eEthOtherFrame:
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"ClassifyPacket : ETH Other Frame\n");
-		pIpHeader = pvEThPayload + sizeof(struct bcm_ethernet2_frame);
-		break;
-	default:
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"ClassifyPacket : Unrecognized ETH Frame\n");
-		pIpHeader = pvEThPayload + sizeof(struct bcm_ethernet2_frame);
-		break;
-	}
-
-	if (stEthCsPktInfo.eNwpktIPFrameType == eIPv4Packet) {
-		usCurrFragment = (ntohs(pIpHeader->frag_off) & IP_OFFSET);
-		if ((ntohs(pIpHeader->frag_off) & IP_MF) || usCurrFragment)
-			bFragmentedPkt = TRUE;
-
-		if (bFragmentedPkt) {
-			/* Fragmented  Packet. Get Frag Classifier Entry. */
-			pstClassifierRule = GetFragIPClsEntry(Adapter,
-							      pIpHeader->id,
-							      pIpHeader->saddr);
-			if (pstClassifierRule) {
-					BCM_DEBUG_PRINT(Adapter,
-							DBG_TYPE_TX,
-							IPV4_DBG,
-							DBG_LVL_ALL,
-							"It is next Fragmented pkt");
-					bClassificationSucceed = TRUE;
-			}
-			if (!(ntohs(pIpHeader->frag_off) & IP_MF)) {
-				/* Fragmented Last packet . Remove Frag Classifier Entry */
-				BCM_DEBUG_PRINT(Adapter,
-						DBG_TYPE_TX,
-						IPV4_DBG,
-						DBG_LVL_ALL,
-						"This is the last fragmented Pkt");
-				DelFragIPClsEntry(Adapter,
-						  pIpHeader->id,
-						  pIpHeader->saddr);
-			}
-		}
-	}
-
-	for (uiLoopIndex = MAX_CLASSIFIERS - 1; uiLoopIndex >= 0; uiLoopIndex--) {
-		if (bClassificationSucceed)
-			break;
-		/*
-		 * Iterate through all classifiers which are already in order of priority
-		 * to classify the packet until match found
-		 */
-		if (false == Adapter->astClassifierTable[uiLoopIndex].bUsed) {
-			bClassificationSucceed = false;
-			continue;
-		}
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"Adapter->PackInfo[%d].bvalid=True\n",
-				uiLoopIndex);
-
-		if (0 == Adapter->astClassifierTable[uiLoopIndex].ucDirection) {
-			bClassificationSucceed = false; /* cannot be processed for classification. */
-			continue;	/* it is a down link connection */
-		}
-
-		pstClassifierRule = &Adapter->astClassifierTable[uiLoopIndex];
-
-		uiSfIndex = SearchSfid(Adapter, pstClassifierRule->ulSFID);
-		if (uiSfIndex >= NO_OF_QUEUES) {
-			BCM_DEBUG_PRINT(Adapter,
-					DBG_TYPE_TX,
-					IPV4_DBG,
-					DBG_LVL_ALL,
-					"Queue Not Valid. SearchSfid for this classifier Failed\n");
-			continue;
-		}
-
-		if (Adapter->PackInfo[uiSfIndex].bEthCSSupport) {
-
-			if (eEthUnsupportedFrame == stEthCsPktInfo.eNwpktEthFrameType) {
-				BCM_DEBUG_PRINT(Adapter,
-						DBG_TYPE_TX,
-						IPV4_DBG,
-						DBG_LVL_ALL,
-						" ClassifyPacket : Packet Not a Valid Supported Ethernet Frame\n");
-				bClassificationSucceed = false;
-				continue;
-			}
-
-
-
-			BCM_DEBUG_PRINT(Adapter,
-					DBG_TYPE_TX,
-					IPV4_DBG,
-					DBG_LVL_ALL,
-					"Performing ETH CS Classification on Classifier Rule ID : %x Service Flow ID : %lx\n",
-					pstClassifierRule->uiClassifierRuleIndex,
-					Adapter->PackInfo[uiSfIndex].ulSFID);
-			bClassificationSucceed = EThCSClassifyPkt(Adapter,
-								  skb,
-								  &stEthCsPktInfo,
-								  pstClassifierRule,
-								  Adapter->PackInfo[uiSfIndex].bEthCSSupport);
-
-			if (!bClassificationSucceed) {
-				BCM_DEBUG_PRINT(Adapter,
-						DBG_TYPE_TX,
-						IPV4_DBG,
-						DBG_LVL_ALL,
-						"ClassifyPacket : Ethernet CS Classification Failed\n");
-				continue;
-			}
-		} else { /* No ETH Supported on this SF */
-			if (eEthOtherFrame != stEthCsPktInfo.eNwpktEthFrameType) {
-				BCM_DEBUG_PRINT(Adapter,
-						DBG_TYPE_TX,
-						IPV4_DBG,
-						DBG_LVL_ALL,
-						" ClassifyPacket : Packet Not a 802.3 Ethernet Frame... hence not allowed over non-ETH CS SF\n");
-				bClassificationSucceed = false;
-				continue;
-			}
-		}
-
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"Proceeding to IP CS Clasification");
-
-		if (Adapter->PackInfo[uiSfIndex].bIPCSSupport) {
-
-			if (stEthCsPktInfo.eNwpktIPFrameType == eNonIPPacket) {
-				BCM_DEBUG_PRINT(Adapter,
-						DBG_TYPE_TX,
-						IPV4_DBG,
-						DBG_LVL_ALL,
-						" ClassifyPacket : Packet is Not an IP Packet\n");
-				bClassificationSucceed = false;
-				continue;
-			}
-			BCM_DEBUG_PRINT(Adapter,
-					DBG_TYPE_TX,
-					IPV4_DBG,
-					DBG_LVL_ALL,
-					"Dump IP Header :\n");
-			DumpFullPacket((PUCHAR)pIpHeader, 20);
-
-			if (stEthCsPktInfo.eNwpktIPFrameType == eIPv4Packet)
-				bClassificationSucceed = IpVersion4(Adapter,
-								    pIpHeader,
-								    pstClassifierRule);
-			else if (stEthCsPktInfo.eNwpktIPFrameType == eIPv6Packet)
-				bClassificationSucceed = IpVersion6(Adapter,
-								    pIpHeader,
-								    pstClassifierRule);
-		}
-	}
-
-	if (bClassificationSucceed == TRUE) {
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"CF id : %d, SF ID is =%lu",
-				pstClassifierRule->uiClassifierRuleIndex,
-				pstClassifierRule->ulSFID);
-
-		/* Store The matched Classifier in SKB */
-		*((UINT32 *)(skb->cb)+SKB_CB_CLASSIFICATION_OFFSET) =
-			pstClassifierRule->uiClassifierRuleIndex;
-		if ((TCP == pIpHeader->protocol) && !bFragmentedPkt &&
-				(ETH_AND_IP_HEADER_LEN + TCP_HEADER_LEN <=
-					skb->len)) {
-			IpHeaderLength = pIpHeader->ihl;
-			pTcpHeader =
-				(struct bcm_tcp_header *)(((PUCHAR)pIpHeader) +
-						(IpHeaderLength*4));
-			TcpHeaderLength = GET_TCP_HEADER_LEN(pTcpHeader->HeaderLength);
-
-			if ((pTcpHeader->ucFlags & TCP_ACK) &&
-				   (ntohs(pIpHeader->tot_len) ==
-				    (IpHeaderLength*4)+(TcpHeaderLength*4)))
-				*((UINT32 *) (skb->cb) + SKB_CB_TCPACK_OFFSET) =
-					TCP_ACK;
-		}
-
-		usIndex = SearchSfid(Adapter, pstClassifierRule->ulSFID);
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"index is =%d",
-				usIndex);
-
-		/*
-		 * If this is the first fragment of a Fragmented pkt,
-		 * add this CF. Only This CF should be used for all other
-		 * fragment of this Pkt.
-		 */
-		if (bFragmentedPkt && (usCurrFragment == 0)) {
-			/*
-			 * First Fragment of Fragmented Packet.
-			 * Create Frag CLS Entry
-			 */
-			struct bcm_fragmented_packet_info stFragPktInfo;
-
-			stFragPktInfo.bUsed = TRUE;
-			stFragPktInfo.ulSrcIpAddress = pIpHeader->saddr;
-			stFragPktInfo.usIpIdentification = pIpHeader->id;
-			stFragPktInfo.pstMatchedClassifierEntry =
-				pstClassifierRule;
-			stFragPktInfo.bOutOfOrderFragment = false;
-			AddFragIPClsEntry(Adapter, &stFragPktInfo);
-		}
-
-
-	}
-
-	return bClassificationSucceed ? usIndex : INVALID_QUEUE_INDEX;
-}
-
-static bool EthCSMatchSrcMACAddress(struct bcm_classifier_rule *pstClassifierRule,
-				    PUCHAR Mac)
-{
-	UINT i = 0;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	if (pstClassifierRule->ucEthCSSrcMACLen == 0)
-		return TRUE;
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"%s\n", __func__);
-	for (i = 0; i < MAC_ADDRESS_SIZE; i++) {
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"SRC MAC[%x] = %x ClassifierRuleSrcMAC = %x Mask : %x\n",
-				i,
-				Mac[i],
-				pstClassifierRule->au8EThCSSrcMAC[i],
-				pstClassifierRule->au8EThCSSrcMACMask[i]);
-		if ((pstClassifierRule->au8EThCSSrcMAC[i] &
-					pstClassifierRule->au8EThCSSrcMACMask[i]) !=
-				(Mac[i] & pstClassifierRule->au8EThCSSrcMACMask[i]))
-			return false;
-	}
-	return TRUE;
-}
-
-static bool EthCSMatchDestMACAddress(struct bcm_classifier_rule *pstClassifierRule,
-				     PUCHAR Mac)
-{
-	UINT i = 0;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	if (pstClassifierRule->ucEthCSDestMACLen == 0)
-		return TRUE;
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"%s\n",
-			__func__);
-	for (i = 0; i < MAC_ADDRESS_SIZE; i++) {
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"SRC MAC[%x] = %x ClassifierRuleSrcMAC = %x Mask : %x\n",
-				i,
-				Mac[i],
-				pstClassifierRule->au8EThCSDestMAC[i],
-				pstClassifierRule->au8EThCSDestMACMask[i]);
-		if ((pstClassifierRule->au8EThCSDestMAC[i] &
-					pstClassifierRule->au8EThCSDestMACMask[i]) !=
-				(Mac[i] & pstClassifierRule->au8EThCSDestMACMask[i]))
-			return false;
-	}
-	return TRUE;
-}
-
-static bool EthCSMatchEThTypeSAP(struct bcm_classifier_rule *pstClassifierRule,
-				 struct sk_buff *skb,
-				 struct bcm_eth_packet_info *pstEthCsPktInfo)
-{
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	if ((pstClassifierRule->ucEtherTypeLen == 0) ||
-		(pstClassifierRule->au8EthCSEtherType[0] == 0))
-		return TRUE;
-
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"%s SrcEtherType:%x CLS EtherType[0]:%x\n",
-			__func__,
-			pstEthCsPktInfo->usEtherType,
-			pstClassifierRule->au8EthCSEtherType[0]);
-	if (pstClassifierRule->au8EthCSEtherType[0] == 1) {
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"%s  CLS EtherType[1]:%x EtherType[2]:%x\n",
-				__func__,
-				pstClassifierRule->au8EthCSEtherType[1],
-				pstClassifierRule->au8EthCSEtherType[2]);
-
-		if (memcmp(&pstEthCsPktInfo->usEtherType,
-			   &pstClassifierRule->au8EthCSEtherType[1],
-			   2) == 0)
-			return TRUE;
-		else
-			return false;
-	}
-
-	if (pstClassifierRule->au8EthCSEtherType[0] == 2) {
-		if (eEth802LLCFrame != pstEthCsPktInfo->eNwpktEthFrameType)
-			return false;
-
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"%s  EthCS DSAP:%x EtherType[2]:%x\n",
-				__func__,
-				pstEthCsPktInfo->ucDSAP,
-				pstClassifierRule->au8EthCSEtherType[2]);
-		if (pstEthCsPktInfo->ucDSAP ==
-				pstClassifierRule->au8EthCSEtherType[2])
-			return TRUE;
-		else
-			return false;
-
-	}
-
-	return false;
-
-}
-
-static bool EthCSMatchVLANRules(struct bcm_classifier_rule *pstClassifierRule,
-				struct sk_buff *skb,
-				struct bcm_eth_packet_info *pstEthCsPktInfo)
-{
-	bool bClassificationSucceed = false;
-	USHORT usVLANID;
-	B_UINT8 uPriority = 0;
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"%s  CLS UserPrio:%x CLS VLANID:%x\n",
-			__func__,
-			ntohs(*((USHORT *)pstClassifierRule->usUserPriority)),
-			pstClassifierRule->usVLANID);
-
-	/*
-	 * In case FW didn't receive the TLV,
-	 * the priority field should be ignored
-	 */
-	if (pstClassifierRule->usValidityBitMap &
-			(1<<PKT_CLASSIFICATION_USER_PRIORITY_VALID)) {
-		if (pstEthCsPktInfo->eNwpktEthFrameType != eEth802QVLANFrame)
-				return false;
-
-		uPriority = (ntohs(*(USHORT *)(skb->data +
-				   sizeof(struct bcm_eth_header))) &
-				   0xF000) >> 13;
-
-		if ((uPriority >= pstClassifierRule->usUserPriority[0]) &&
-				(uPriority <=
-				 pstClassifierRule->usUserPriority[1]))
-			bClassificationSucceed = TRUE;
-
-		if (!bClassificationSucceed)
-			return false;
-	}
-
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"ETH CS 802.1 D  User Priority Rule Matched\n");
-
-	bClassificationSucceed = false;
-
-	if (pstClassifierRule->usValidityBitMap &
-			(1<<PKT_CLASSIFICATION_VLANID_VALID)) {
-		if (pstEthCsPktInfo->eNwpktEthFrameType != eEth802QVLANFrame)
-				return false;
-
-		usVLANID = ntohs(*(USHORT *)(skb->data +
-					sizeof(struct bcm_eth_header))) & 0xFFF;
-
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"%s  Pkt VLANID %x Priority: %d\n",
-				__func__,
-				usVLANID,
-				uPriority);
-
-		if (usVLANID == ((pstClassifierRule->usVLANID & 0xFFF0) >> 4))
-			bClassificationSucceed = TRUE;
-
-		if (!bClassificationSucceed)
-			return false;
-	}
-
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"ETH CS 802.1 Q VLAN ID Rule Matched\n");
-
-	return TRUE;
-}
-
-
-static bool EThCSClassifyPkt(struct bcm_mini_adapter *Adapter,
-			     struct sk_buff *skb,
-			     struct bcm_eth_packet_info *pstEthCsPktInfo,
-			     struct bcm_classifier_rule *pstClassifierRule,
-			     B_UINT8 EthCSCupport)
-{
-	bool bClassificationSucceed = false;
-
-	bClassificationSucceed = EthCSMatchSrcMACAddress(pstClassifierRule,
-			((struct bcm_eth_header *)(skb->data))->au8SourceAddress);
-	if (!bClassificationSucceed)
-		return false;
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"ETH CS SrcMAC Matched\n");
-
-	bClassificationSucceed = EthCSMatchDestMACAddress(pstClassifierRule,
-			((struct bcm_eth_header *)(skb->data))->au8DestinationAddress);
-	if (!bClassificationSucceed)
-		return false;
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"ETH CS DestMAC Matched\n");
-
-	/* classify on ETHType/802.2SAP TLV */
-	bClassificationSucceed = EthCSMatchEThTypeSAP(pstClassifierRule,
-						      skb,
-						      pstEthCsPktInfo);
-	if (!bClassificationSucceed)
-		return false;
-
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"ETH CS EthType/802.2SAP Matched\n");
-
-	/* classify on 802.1VLAN Header Parameters */
-	bClassificationSucceed = EthCSMatchVLANRules(pstClassifierRule,
-						     skb,
-						     pstEthCsPktInfo);
-	if (!bClassificationSucceed)
-		return false;
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"ETH CS 802.1 VLAN Rules Matched\n");
-
-	return bClassificationSucceed;
-}
-
-static void EThCSGetPktInfo(struct bcm_mini_adapter *Adapter,
-			    PVOID pvEthPayload,
-			    struct bcm_eth_packet_info *pstEthCsPktInfo)
-{
-	USHORT u16Etype = ntohs(
-			((struct bcm_eth_header *)pvEthPayload)->u16Etype);
-
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"EthCSGetPktInfo : Eth Hdr Type : %X\n",
-			u16Etype);
-	if (u16Etype > 0x5dc) {
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"EthCSGetPktInfo : ETH2 Frame\n");
-		/* ETH2 Frame */
-		if (u16Etype == ETHERNET_FRAMETYPE_802QVLAN) {
-			/* 802.1Q VLAN Header */
-			pstEthCsPktInfo->eNwpktEthFrameType = eEth802QVLANFrame;
-			u16Etype = ((struct bcm_eth_q_frame *)pvEthPayload)->EthType;
-			/* ((ETH_CS_802_Q_FRAME*)pvEthPayload)->UserPriority */
-		} else {
-			pstEthCsPktInfo->eNwpktEthFrameType = eEthOtherFrame;
-			u16Etype = ntohs(u16Etype);
-		}
-	} else {
-		/* 802.2 LLC */
-		BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_TX,
-				IPV4_DBG,
-				DBG_LVL_ALL,
-				"802.2 LLC Frame\n");
-		pstEthCsPktInfo->eNwpktEthFrameType = eEth802LLCFrame;
-		pstEthCsPktInfo->ucDSAP =
-			((struct bcm_eth_llc_frame *)pvEthPayload)->DSAP;
-		if (pstEthCsPktInfo->ucDSAP == 0xAA && ((struct bcm_eth_llc_frame *)pvEthPayload)->SSAP == 0xAA) {
-			/* SNAP Frame */
-			pstEthCsPktInfo->eNwpktEthFrameType = eEth802LLCSNAPFrame;
-			u16Etype = ((struct bcm_eth_llc_snap_frame *)pvEthPayload)->usEtherType;
-		}
-	}
-	if (u16Etype == ETHERNET_FRAMETYPE_IPV4)
-		pstEthCsPktInfo->eNwpktIPFrameType = eIPv4Packet;
-	else if (u16Etype == ETHERNET_FRAMETYPE_IPV6)
-		pstEthCsPktInfo->eNwpktIPFrameType = eIPv6Packet;
-	else
-		pstEthCsPktInfo->eNwpktIPFrameType = eNonIPPacket;
-
-	pstEthCsPktInfo->usEtherType = ((struct bcm_eth_header *)pvEthPayload)->u16Etype;
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"EthCsPktInfo->eNwpktIPFrameType : %x\n",
-			pstEthCsPktInfo->eNwpktIPFrameType);
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"EthCsPktInfo->eNwpktEthFrameType : %x\n",
-			pstEthCsPktInfo->eNwpktEthFrameType);
-	BCM_DEBUG_PRINT(Adapter,
-			DBG_TYPE_TX,
-			IPV4_DBG,
-			DBG_LVL_ALL,
-			"EthCsPktInfo->usEtherType : %x\n",
-			pstEthCsPktInfo->usEtherType);
-}
-
diff --git a/drivers/staging/bcm/Queue.h b/drivers/staging/bcm/Queue.h
deleted file mode 100644
index 460c0aee67f6..000000000000
--- a/drivers/staging/bcm/Queue.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*************************************
-* Queue.h
-**************************************/
-#ifndef	__QUEUE_H__
-#define	__QUEUE_H__
-
-
-
-#define ENQUEUEPACKET(_Head, _Tail, _Packet)	\
-do {						\
-	if (!_Head) {				\
-		_Head = _Packet;		\
-	}					\
-	else {					\
-		(_Tail)->next = _Packet;	\
-	}					\
-	(_Packet)->next = NULL;			\
-	_Tail = _Packet;			\
-} while (0)
-#define DEQUEUEPACKET(Head, Tail)		\
-do {						\
-	if (Head) {				\
-		if (!Head->next) {		\
-			Tail = NULL;		\
-		}				\
-		Head = Head->next;		\
-	}					\
-} while (0)
-#endif /* __QUEUE_H__ */
diff --git a/drivers/staging/bcm/TODO b/drivers/staging/bcm/TODO
deleted file mode 100644
index 8467f45d08a6..000000000000
--- a/drivers/staging/bcm/TODO
+++ /dev/null
@@ -1,26 +0,0 @@
-This driver is barely functional in its current state.
-
-Kevin McKinney(klmckinney1@gmail.com) and Matthias Beyer(mail@beyermatthias.de)
-are currently maintaining/cleaning up this driver.  Please copy us on all
-patches.  More maintainers are aways welcomed.
-
-BIG:
-	- existing API is (/dev/tarang) should be replaced
-	  Is it possible to use same API as Intel Wimax stack and
-	  have same user level components.
-	- Qos and queue model is non-standard and inflexible.
-	  Use existing TC Qos?
-
-TODO:
-	- support more than one board - eliminate global variables
-	- remove developer debug BCM_DEBUG() macros
-	  add a limited number of messages through netif_msg()
-	- fix non-standard kernel style
-	- checkpatch warnings
-	- use request firmware
-	- fix use of file I/O to load config with better API
-	- merge some files together?
-	- cleanup/eliminate debug messages
-
-
-
diff --git a/drivers/staging/bcm/Transmit.c b/drivers/staging/bcm/Transmit.c
deleted file mode 100644
index 622a482e9826..000000000000
--- a/drivers/staging/bcm/Transmit.c
+++ /dev/null
@@ -1,271 +0,0 @@
-/**
- * @file Transmit.c
- * @defgroup tx_functions Transmission
- * @section Queueing
- * @dot
- * digraph transmit1 {
- * node[shape=box]
- * edge[weight=5;color=red]
- *
- * bcm_transmit->GetPacketQueueIndex[label="IP Packet"]
- * GetPacketQueueIndex->IpVersion4[label="IPV4"]
- * GetPacketQueueIndex->IpVersion6[label="IPV6"]
- * }
- *
- * @enddot
- *
- * @section De-Queueing
- * @dot
- * digraph transmit2 {
- * node[shape=box]
- * edge[weight=5;color=red]
- * interrupt_service_thread->transmit_packets
- * tx_pkt_hdler->transmit_packets
- * transmit_packets->CheckAndSendPacketFromIndex
- * transmit_packets->UpdateTokenCount
- * CheckAndSendPacketFromIndex->PruneQueue
- * CheckAndSendPacketFromIndex->IsPacketAllowedForFlow
- * CheckAndSendPacketFromIndex->SendControlPacket[label="control pkt"]
- * SendControlPacket->bcm_cmd53
- * CheckAndSendPacketFromIndex->SendPacketFromQueue[label="data pkt"]
- * SendPacketFromQueue->SetupNextSend->bcm_cmd53
- * }
- * @enddot
- */
-
-#include "headers.h"
-
-/**
- * @ingroup ctrl_pkt_functions
- * This function dispatches control packet to the h/w interface
- * @return zero(success) or -ve value(failure)
- */
-int SendControlPacket(struct bcm_mini_adapter *Adapter, char *pControlPacket)
-{
-	struct bcm_leader *PLeader = (struct bcm_leader *)pControlPacket;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Tx");
-	if (!pControlPacket || !Adapter) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL,
-				"Got NULL Control Packet or Adapter");
-		return STATUS_FAILURE;
-	}
-	if ((atomic_read(&Adapter->CurrNumFreeTxDesc) <
-			((PLeader->PLength-1)/MAX_DEVICE_DESC_SIZE)+1))	{
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL,
-				"NO FREE DESCRIPTORS TO SEND CONTROL PACKET");
-		return STATUS_FAILURE;
-	}
-
-	/* Update the netdevice statistics */
-	/* Dump Packet  */
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL,
-			"Leader Status: %x", PLeader->Status);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL,
-			"Leader VCID: %x", PLeader->Vcid);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL,
-			"Leader Length: %x", PLeader->PLength);
-	if (Adapter->device_removed)
-		return 0;
-
-	if (netif_msg_pktdata(Adapter))
-		print_hex_dump(KERN_DEBUG, PFX "tx control: ", DUMP_PREFIX_NONE,
-			       16, 1, pControlPacket,
-			       PLeader->PLength + LEADER_SIZE, 0);
-
-	Adapter->interface_transmit(Adapter->pvInterfaceAdapter,
-				    pControlPacket,
-				    (PLeader->PLength + LEADER_SIZE));
-
-	atomic_dec(&Adapter->CurrNumFreeTxDesc);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL,
-			"<=========");
-	return STATUS_SUCCESS;
-}
-
-/**
- * @ingroup tx_functions
- * This function despatches the IP packets with the given vcid
- * to the target via the host h/w interface.
- * @return  zero(success) or -ve value(failure)
- */
-int SetupNextSend(struct bcm_mini_adapter *Adapter,
-		struct sk_buff *Packet, USHORT Vcid)
-{
-	int	status = 0;
-	bool	bHeaderSupressionEnabled = false;
-	B_UINT16 uiClassifierRuleID;
-	u16	QueueIndex = skb_get_queue_mapping(Packet);
-	struct bcm_packet_info *curr_packet_info =
-		&Adapter->PackInfo[QueueIndex];
-	struct bcm_leader Leader = {0};
-
-	if (Packet->len > MAX_DEVICE_DESC_SIZE) {
-		status = STATUS_FAILURE;
-		goto errExit;
-	}
-
-	/* Get the Classifier Rule ID */
-	uiClassifierRuleID = *((UINT32 *) (Packet->cb) +
-			       SKB_CB_CLASSIFICATION_OFFSET);
-
-	bHeaderSupressionEnabled = curr_packet_info->bHeaderSuppressionEnabled &
-		Adapter->bPHSEnabled;
-
-	if (Adapter->device_removed) {
-		status = STATUS_FAILURE;
-		goto errExit;
-	}
-
-	status = PHSTransmit(Adapter, &Packet, Vcid, uiClassifierRuleID,
-			     bHeaderSupressionEnabled,
-			     (UINT *)&Packet->len,
-			     curr_packet_info->bEthCSSupport);
-
-	if (status != STATUS_SUCCESS) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, NEXT_SEND, DBG_LVL_ALL,
-				"PHS Transmit failed..\n");
-		goto errExit;
-	}
-
-	Leader.Vcid = Vcid;
-
-	if (TCP_ACK == *((UINT32 *) (Packet->cb) + SKB_CB_TCPACK_OFFSET))
-		Leader.Status = LEADER_STATUS_TCP_ACK;
-	else
-		Leader.Status = LEADER_STATUS;
-
-	if (curr_packet_info->bEthCSSupport) {
-		Leader.PLength = Packet->len;
-		if (skb_headroom(Packet) < LEADER_SIZE) {
-			status = skb_cow(Packet, LEADER_SIZE);
-			if (status) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, NEXT_SEND,
-						DBG_LVL_ALL,
-						"bcm_transmit : Failed To Increase headRoom\n");
-				goto errExit;
-			}
-		}
-		skb_push(Packet, LEADER_SIZE);
-		memcpy(Packet->data, &Leader, LEADER_SIZE);
-	} else {
-		Leader.PLength = Packet->len - ETH_HLEN;
-		memcpy((struct bcm_leader *)skb_pull(Packet,
-						     (ETH_HLEN - LEADER_SIZE)),
-			&Leader,
-			LEADER_SIZE);
-	}
-
-	status = Adapter->interface_transmit(Adapter->pvInterfaceAdapter,
-					     Packet->data,
-					     (Leader.PLength + LEADER_SIZE));
-	if (status) {
-		++Adapter->dev->stats.tx_errors;
-		if (netif_msg_tx_err(Adapter))
-			pr_info(PFX "%s: transmit error %d\n",
-				Adapter->dev->name,
-				status);
-	} else {
-		struct net_device_stats *netstats = &Adapter->dev->stats;
-
-		curr_packet_info->uiTotalTxBytes += Leader.PLength;
-
-		netstats->tx_bytes += Leader.PLength;
-		++netstats->tx_packets;
-
-		curr_packet_info->uiCurrentTokenCount -= Leader.PLength << 3;
-		curr_packet_info->uiSentBytes += (Packet->len);
-		curr_packet_info->uiSentPackets++;
-		curr_packet_info->NumOfPacketsSent++;
-
-		atomic_dec(&curr_packet_info->uiPerSFTxResourceCount);
-		curr_packet_info->uiThisPeriodSentBytes += Leader.PLength;
-	}
-
-	atomic_dec(&Adapter->CurrNumFreeTxDesc);
-
-errExit:
-	dev_kfree_skb(Packet);
-	return status;
-}
-
-static int tx_pending(struct bcm_mini_adapter *Adapter)
-{
-	return (atomic_read(&Adapter->TxPktAvail)
-		&& MINIMUM_PENDING_DESCRIPTORS <
-			atomic_read(&Adapter->CurrNumFreeTxDesc))
-		|| Adapter->device_removed || (1 == Adapter->downloadDDR);
-}
-
-/**
- * @ingroup tx_functions
- * Transmit thread
- */
-int tx_pkt_handler(struct bcm_mini_adapter *Adapter)
-{
-	int status = 0;
-
-	while (!kthread_should_stop()) {
-		/* FIXME - the timeout looks like workaround
-		 *  for racey usage of TxPktAvail
-		*/
-		if (Adapter->LinkUpStatus)
-			wait_event_timeout(Adapter->tx_packet_wait_queue,
-					   tx_pending(Adapter),
-					   msecs_to_jiffies(10));
-		else
-			wait_event_interruptible(Adapter->tx_packet_wait_queue,
-						 tx_pending(Adapter));
-
-		if (Adapter->device_removed)
-			break;
-
-		if (Adapter->downloadDDR == 1) {
-			Adapter->downloadDDR += 1;
-			status = download_ddr_settings(Adapter);
-			if (status)
-				pr_err(PFX "DDR DOWNLOAD FAILED! %d\n", status);
-			continue;
-		}
-
-		/* Check end point for halt/stall. */
-		if (Adapter->bEndPointHalted == TRUE) {
-			Bcm_clear_halt_of_endpoints(Adapter);
-			Adapter->bEndPointHalted = false;
-			StartInterruptUrb((struct bcm_interface_adapter *)
-					(Adapter->pvInterfaceAdapter));
-		}
-
-		if (Adapter->LinkUpStatus && !Adapter->IdleMode) {
-			if (atomic_read(&Adapter->TotalPacketCount))
-				update_per_sf_desc_cnts(Adapter);
-		}
-
-		if (atomic_read(&Adapter->CurrNumFreeTxDesc) &&
-			Adapter->LinkStatus == SYNC_UP_REQUEST &&
-			!Adapter->bSyncUpRequestSent) {
-
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS,
-					DBG_LVL_ALL, "Calling LinkMessage");
-			LinkMessage(Adapter);
-		}
-
-		if ((Adapter->IdleMode || Adapter->bShutStatus) &&
-				atomic_read(&Adapter->TotalPacketCount)) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX,
-					TX_PACKETS, DBG_LVL_ALL,
-					"Device in Low Power mode...waking up");
-			Adapter->usIdleModePattern = ABORT_IDLE_MODE;
-			Adapter->bWakeUpDevice = TRUE;
-			wake_up(&Adapter->process_rx_cntrlpkt);
-		}
-
-		transmit_packets(Adapter);
-		atomic_set(&Adapter->TxPktAvail, 0);
-	}
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL,
-			"Exiting the tx thread..\n");
-	Adapter->transmit_packet_thread = NULL;
-	return 0;
-}
diff --git a/drivers/staging/bcm/Typedefs.h b/drivers/staging/bcm/Typedefs.h
deleted file mode 100644
index 90b3b25dd606..000000000000
--- a/drivers/staging/bcm/Typedefs.h
+++ /dev/null
@@ -1,47 +0,0 @@
-/****************************
-*	Typedefs.h
-****************************/
-#ifndef	__TYPEDEFS_H__
-#define	__TYPEDEFS_H__
-#define  STATUS_SUCCESS	0
-#define  STATUS_FAILURE -1
-
-
-#define	 TRUE		1
-
-
-typedef char CHAR;
-typedef int INT;
-typedef short SHORT;
-typedef long LONG;
-typedef void VOID;
-
-typedef unsigned char UCHAR;
-typedef unsigned char B_UINT8;
-typedef unsigned short USHORT;
-typedef unsigned short B_UINT16;
-typedef unsigned int UINT;
-typedef unsigned int B_UINT32;
-typedef unsigned long ULONG;
-typedef unsigned long DWORD;
-
-typedef char *PCHAR;
-typedef short *PSHORT;
-typedef int *PINT;
-typedef long *PLONG;
-typedef void *PVOID;
-
-typedef unsigned char *PUCHAR;
-typedef unsigned short *PUSHORT;
-typedef unsigned int *PUINT;
-typedef unsigned long *PULONG;
-typedef unsigned long long ULONG64;
-typedef unsigned long long LARGE_INTEGER;
-typedef unsigned int UINT32;
-#ifndef NULL
-#define NULL 0
-#endif
-
-
-#endif	/* __TYPEDEFS_H__ */
-
diff --git a/drivers/staging/bcm/cntrl_SignalingInterface.h b/drivers/staging/bcm/cntrl_SignalingInterface.h
deleted file mode 100644
index 8683c2d4276e..000000000000
--- a/drivers/staging/bcm/cntrl_SignalingInterface.h
+++ /dev/null
@@ -1,311 +0,0 @@
-#ifndef CNTRL_SIGNALING_INTERFACE_
-#define CNTRL_SIGNALING_INTERFACE_
-
-#define DSA_REQ			11
-#define DSA_RSP			12
-#define DSA_ACK			13
-#define DSC_REQ			14
-#define DSC_RSP			15
-#define DSC_ACK			16
-#define DSD_REQ			17
-#define DSD_RSP			18
-#define DSD_ACK			19
-#define MAX_CLASSIFIERS_IN_SF	4
-
-#define MAX_STRING_LEN			20
-#define MAX_PHS_LENGTHS			255
-#define VENDOR_PHS_PARAM_LENGTH		10
-#define MAX_NUM_ACTIVE_BS		10
-#define AUTH_TOKEN_LENGTH		10
-#define NUM_HARQ_CHANNELS		16 /* Changed from 10 to 16 to accommodate all HARQ channels */
-#define VENDOR_CLASSIFIER_PARAM_LENGTH	1  /* Changed the size to 1 byte since we dnt use it */
-#define  VENDOR_SPECIF_QOS_PARAM	1
-#define VENDOR_PHS_PARAM_LENGTH		10
-#define MBS_CONTENTS_ID_LENGTH		10
-#define GLOBAL_SF_CLASSNAME_LENGTH	6
-
-#define TYPE_OF_SERVICE_LENGTH		3
-#define IP_MASKED_SRC_ADDRESS_LENGTH	32
-#define IP_MASKED_DEST_ADDRESS_LENGTH	32
-#define PROTOCOL_SRC_PORT_RANGE_LENGTH	4
-#define PROTOCOL_DEST_PORT_RANGE_LENGTH	4
-#define ETHERNET_DEST_MAC_ADDR_LENGTH	12
-#define ETHERNET_SRC_MAC_ADDR_LENGTH	12
-#define NUM_ETHERTYPE_BYTES		3
-#define NUM_IPV6_FLOWLABLE_BYTES	3
-
-struct bcm_packet_class_rules {
-	/* 16bit UserPriority Of The Service Flow */
-	u16 u16UserPriority;
-	/* 16bit VLANID Of The Service Flow */
-	u16 u16VLANID;
-	/* 16bit Packet Classification RuleIndex Of The Service Flow */
-	u16 u16PacketClassificationRuleIndex;
-	/* 8bit Classifier Rule Priority Of The Service Flow */
-	u8 u8ClassifierRulePriority;
-	/* Length of IP TypeOfService field */
-	u8 u8IPTypeOfServiceLength;
-	/* 3bytes IP TypeOfService */
-	u8 u8IPTypeOfService[TYPE_OF_SERVICE_LENGTH];
-	/* Protocol used in classification of Service Flow */
-	u8 u8Protocol;
-	/* Length of IP Masked Source Address */
-	u8 u8IPMaskedSourceAddressLength;
-	/* IP Masked Source Address used in classification for the Service Flow */
-	u8 u8IPMaskedSourceAddress[IP_MASKED_SRC_ADDRESS_LENGTH];
-	/* Length of IP Destination Address */
-	u8 u8IPDestinationAddressLength;
-	/* IP Destination Address used in classification for the Service Flow */
-	u8 u8IPDestinationAddress[IP_MASKED_DEST_ADDRESS_LENGTH];
-	/* Length of Protocol Source Port Range */
-	u8 u8ProtocolSourcePortRangeLength;
-	/* Protocol Source Port Range used in the Service Flow */
-	u8 u8ProtocolSourcePortRange[PROTOCOL_SRC_PORT_RANGE_LENGTH];
-	/* Length of Protocol Dest Port Range */
-	u8 u8ProtocolDestPortRangeLength;
-	/* Protocol Dest Port Range used in the Service Flow */
-	u8 u8ProtocolDestPortRange[PROTOCOL_DEST_PORT_RANGE_LENGTH];
-	/* Length of Ethernet Destination MAC Address */
-	u8 u8EthernetDestMacAddressLength;
-	/* Ethernet Destination MAC Address  used in classification of the Service Flow */
-	u8 u8EthernetDestMacAddress[ETHERNET_DEST_MAC_ADDR_LENGTH];
-	/* Length of Ethernet Source MAC Address */
-	u8 u8EthernetSourceMACAddressLength;
-	/* Ethernet Source MAC Address  used in classification of the Service Flow */
-	u8 u8EthernetSourceMACAddress[ETHERNET_SRC_MAC_ADDR_LENGTH];
-	/* Length of Ethertype */
-	u8 u8EthertypeLength;
-	/* 3bytes Ethertype Of The Service Flow */
-	u8 u8Ethertype[NUM_ETHERTYPE_BYTES];
-	/* 8bit Associated PHSI Of The Service Flow */
-	u8 u8AssociatedPHSI;
-	/* Length of Vendor Specific Classifier Param length Of The Service Flow */
-	u8 u8VendorSpecificClassifierParamLength;
-	/* Vendor Specific Classifier Param Of The Service Flow */
-	u8 u8VendorSpecificClassifierParam[VENDOR_CLASSIFIER_PARAM_LENGTH];
-	/* Length Of IPv6 Flow Lable of the Service Flow */
-	u8 u8IPv6FlowLableLength;
-	/* IPv6 Flow Lable Of The Service Flow */
-	u8 u8IPv6FlowLable[NUM_IPV6_FLOWLABLE_BYTES];
-	/* Action associated with the classifier rule */
-	u8 u8ClassifierActionRule;
-	u16 u16ValidityBitMap;
-};
-
-struct bcm_phs_rules {
-	/* 8bit PHS Index Of The Service Flow */
-	u8 u8PHSI;
-	/* PHSF Length Of The Service Flow */
-	u8 u8PHSFLength;
-	/* String of bytes containing header information to be suppressed by the sending CS and reconstructed by the receiving CS */
-	u8 u8PHSF[MAX_PHS_LENGTHS];
-	/* PHSM Length Of The Service Flow */
-	u8 u8PHSMLength;
-	/* PHS Mask for the SF */
-	u8 u8PHSM[MAX_PHS_LENGTHS];
-	/* 8bit Total number of bytes to be suppressed for the Service Flow */
-	u8 u8PHSS;
-	/* 8bit Indicates whether or not Packet Header contents need to be verified prior to suppression */
-	u8 u8PHSV;
-	/* Vendor Specific PHS param Length Of The Service Flow */
-	u8 u8VendorSpecificPHSParamsLength;
-	/* Vendor Specific PHS param Of The Service Flow */
-	u8 u8VendorSpecificPHSParams[VENDOR_PHS_PARAM_LENGTH];
-	u8 u8Padding[2];
-};
-
-struct bcm_convergence_types {
-	/* 8bit Phs Classfier Action Of The Service Flow */
-	u8 u8ClassfierDSCAction;
-	/* 8bit Phs DSC Action Of The Service Flow */
-	u8 u8PhsDSCAction;
-	/* 16bit Padding */
-	u8 u8Padding[2];
-	/* Packet classification rules structure */
-	struct bcm_packet_class_rules cCPacketClassificationRule;
-	/* Payload header suppression rules structure */
-	struct bcm_phs_rules cPhsRule;
-};
-
-struct bcm_connect_mgr_params {
-	/* 32bitSFID Of The Service Flow */
-	u32 u32SFID;
-	/* 32bit Maximum Sustained Traffic Rate of the Service Flow */
-	u32 u32MaxSustainedTrafficRate;
-	/* 32bit Maximum Traffic Burst allowed for the Service Flow */
-	u32 u32MaxTrafficBurst;
-	/* 32bit Minimum Reserved Traffic Rate of the Service Flow */
-	u32 u32MinReservedTrafficRate;
-	/* 32bit Tolerated Jitter of the Service Flow */
-	u32 u32ToleratedJitter;
-	/* 32bit Maximum Latency of the Service Flow */
-	u32 u32MaximumLatency;
-	/* 16bitCID Of The Service Flow */
-	u16 u16CID;
-	/* 16bit SAID on which the service flow being set up shall be mapped */
-	u16 u16TargetSAID;
-	/* 16bit  ARQ window size negotiated */
-	u16 u16ARQWindowSize;
-	/* 16bit Total Tx delay incl sending, receiving & processing delays */
-	u16 u16ARQRetryTxTimeOut;
-	/* 16bit Total Rx delay incl sending, receiving & processing delays */
-	u16 u16ARQRetryRxTimeOut;
-	/* 16bit ARQ block lifetime */
-	u16 u16ARQBlockLifeTime;
-	/* 16bit ARQ Sync loss timeout */
-	u16 u16ARQSyncLossTimeOut;
-	/* 16bit ARQ Purge timeout */
-	u16 u16ARQRxPurgeTimeOut;
-	/* TODO::Remove this once we move to a new CORR2 driver
-	 * brief Size of an ARQ block
-	 */
-	u16 u16ARQBlockSize;
-	/* #endif */
-	/* 16bit Nominal interval b/w consecutive SDU arrivals at MAC SAP */
-	u16 u16SDUInterArrivalTime;
-	/* 16bit Specifies the time base for rate measurement */
-	u16 u16TimeBase;
-	/* 16bit Interval b/w Successive Grant oppurtunities */
-	u16 u16UnsolicitedGrantInterval;
-	/* 16bit Interval b/w Successive Polling grant oppurtunities */
-	u16 u16UnsolicitedPollingInterval;
-	/* internal var to get the overhead */
-	u16 u16MacOverhead;
-	/* MBS contents Identifier */
-	u16 u16MBSContentsID[MBS_CONTENTS_ID_LENGTH];
-	/* MBS contents Identifier length */
-	u8 u8MBSContentsIDLength;
-	/* ServiceClassName Length Of The Service Flow */
-	u8 u8ServiceClassNameLength;
-	/* 32bytes ServiceClassName Of The Service Flow */
-	u8 u8ServiceClassName[32];
-	/* 8bit Indicates whether or not MBS service is requested for this Serivce Flow */
-	u8 u8MBSService;
-	/* 8bit QOS Parameter Set specifies proper application of QoS parameters to Provisioned, Admitted and Active sets */
-	u8 u8QosParamSet;
-	/* 8bit Traffic Priority Of the Service Flow */
-	u8 u8TrafficPriority;
-	/* 8bit Uplink Grant Scheduling Type of The Service Flow */
-	u8 u8ServiceFlowSchedulingType;
-	/* 8bit Request transmission Policy of the Service Flow */
-	u8 u8RequesttransmissionPolicy;
-	/* 8bit Specifies whether SDUs for this Service flow are of FixedLength or Variable length */
-	u8 u8FixedLengthVSVariableLengthSDUIndicator;
-	/* 8bit Length of the SDU for a fixed length SDU service flow */
-	u8 u8SDUSize;
-	/* 8bit Indicates whether or not ARQ is requested for this connection */
-	u8 u8ARQEnable;
-	/* < 8bit Indicates whether or not data has tobe delivered in order to higher layer */
-	u8 u8ARQDeliverInOrder;
-	/* 8bit Receiver ARQ ACK processing time */
-	u8 u8RxARQAckProcessingTime;
-	/* 8bit Convergence Sublayer Specification Of The Service Flow */
-	u8 u8CSSpecification;
-	/* 8 bit Type of data delivery service */
-	u8 u8TypeOfDataDeliveryService;
-	/* 8bit Specifies whether a service flow may generate Paging */
-	u8 u8PagingPreference;
-	/* 8bit Indicates the MBS Zone through which the connection or virtual connection is valid */
-	u8 u8MBSZoneIdentifierassignment;
-	/* 8bit Specifies whether traffic on SF should generate MOB_TRF_IND to MS in sleep mode */
-	u8 u8TrafficIndicationPreference;
-	/* 8bit Speciifes the length of predefined Global QoS parameter set encoding for this SF */
-	u8 u8GlobalServicesClassNameLength;
-	/* 6 byte Speciifes the predefined Global QoS parameter set encoding for this SF */
-	u8 u8GlobalServicesClassName[GLOBAL_SF_CLASSNAME_LENGTH];
-	/* 8bit Indicates whether or not SN feedback is enabled for the conn */
-	u8 u8SNFeedbackEnabled;
-	/* Indicates the size of the Fragment Sequence Number for the connection */
-	u8 u8FSNSize;
-	/* 8bit Number of CIDs in active BS list */
-	u8 u8CIDAllocation4activeBSsLength;
-	/* CIDs of BS in the active list */
-	u8 u8CIDAllocation4activeBSs[MAX_NUM_ACTIVE_BS];
-	/* Specifies if PDU extended subheader should be applied on every PDU on this conn */
-	u8 u8PDUSNExtendedSubheader4HarqReordering;
-	/* 8bit Specifies whether the connection uses HARQ or not */
-	u8 u8HARQServiceFlows;
-	/* Specifies the length of Authorization token */
-	u8 u8AuthTokenLength;
-	/* Specifies the Authorization token */
-	u8 u8AuthToken[AUTH_TOKEN_LENGTH];
-	/* specifes Number of HARQ channels used to carry data length */
-	u8 u8HarqChannelMappingLength;
-	/* specifes HARQ channels used to carry data */
-	u8 u8HARQChannelMapping[NUM_HARQ_CHANNELS];
-	/* 8bit Length of Vendor Specific QoS Params */
-	u8 u8VendorSpecificQoSParamLength;
-	/* 1byte  Vendor Specific QoS Param Of The Service Flow */
-	u8 u8VendorSpecificQoSParam[VENDOR_SPECIF_QOS_PARAM];
-	/* indicates total classifiers in the SF */
-	u8 u8TotalClassifiers;  /* < Total number of valid classifiers */
-	u8 bValid;	/* < Validity flag */
-	u8 u8Padding;	 /* < Padding byte */
-	/*
-	 * Structure for Convergence SubLayer Types with a maximum of 4 classifiers
-	 */
-	struct bcm_convergence_types cConvergenceSLTypes[MAX_CLASSIFIERS_IN_SF];
-};
-
-struct bcm_add_request {
-	u8 u8Type;	/* < Type */
-	u8 eConnectionDir; /* < Connection direction */
-	/* brief 16 bit TID */
-	u16 u16TID; /* < 16bit TID */
-	/* brief 16bitCID */
-	u16 u16CID; /* < 16bit CID */
-	/* brief 16bitVCID */
-	u16 u16VCID; /* < 16bit VCID */
-	struct bcm_connect_mgr_params *psfParameterSet; /* < connection manager parameters */
-};
-
-struct bcm_add_indication {
-	u8 u8Type;	/* < Type */
-	u8 eConnectionDir;	/* < Connection Direction */
-	/* brief 16 bit TID */
-	u16 u16TID; /* < TID */
-	/* brief 16bitCID */
-	u16 u16CID; /* < 16bitCID */
-	/* brief 16bitVCID */
-	u16 u16VCID; /* < 16bitVCID */
-	struct bcm_connect_mgr_params *psfAuthorizedSet; /* Authorized set of connection manager parameters */
-	struct bcm_connect_mgr_params *psfAdmittedSet; /* Admitted set of connection manager parameters */
-	struct bcm_connect_mgr_params *psfActiveSet; /* Activeset of connection manager parameters */
-	u8 u8CC; /* <Confirmation Code */
-	u8 u8Padd; /* < 8-bit Padding */
-	u16 u16Padd; /* < 16 bit Padding */
-};
-
-struct bcm_del_request {
-	u8 u8Type; /* < Type */
-	u8 u8Padding; /* < Padding byte */
-	u16 u16TID; /* < TID */
-	/* brief 32bitSFID */
-	u32 u32SFID; /* < SFID */
-};
-
-struct bcm_del_indication {
-	u8 u8Type;	/* < Type */
-	u8 u8Padding; /* < Padding */
-	u16 u16TID; /* < TID */
-	/* brief 16bitCID */
-	u16 u16CID; /* < CID */
-	/* brief 16bitVCID */
-	u16 u16VCID; /* < VCID */
-	/* brief 32bitSFID */
-	u32 u32SFID; /* < SFID */
-	/* brief 8bit Confirmation code */
-	u8 u8ConfirmationCode; /* < Confirmation code */
-	u8 u8Padding1[3]; /* < 3 byte Padding */
-};
-
-struct bcm_stim_sfhostnotify {
-	u32 SFID; /* SFID of the service flow */
-	u16 newCID; /* the new/changed CID */
-	u16 VCID; /* Get new Vcid if the flow has been made active in CID update TLV, but was inactive earlier or the orig vcid */
-	u8 RetainSF; /* Indication to Host if the SF is to be retained or deleted; if TRUE-retain else delete */
-	u8 QoSParamSet; /* QoS paramset of the retained SF */
-	u16 u16reserved; /* For byte alignment */
-};
-
-#endif
diff --git a/drivers/staging/bcm/headers.h b/drivers/staging/bcm/headers.h
deleted file mode 100644
index a7d4af5ea9a7..000000000000
--- a/drivers/staging/bcm/headers.h
+++ /dev/null
@@ -1,78 +0,0 @@
-
-/*******************************************************************
-*		Headers.h
-*******************************************************************/
-#ifndef __HEADERS_H__
-#define __HEADERS_H__
-
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/types.h>
-#include <linux/netdevice.h>
-#include <linux/skbuff.h>
-#include <linux/socket.h>
-#include <linux/netfilter.h>
-#include <linux/netfilter_ipv4.h>
-#include <linux/if_arp.h>
-#include <linux/delay.h>
-#include <linux/spinlock.h>
-#include <linux/fs.h>
-#include <linux/file.h>
-#include <linux/string.h>
-#include <linux/etherdevice.h>
-#include <linux/wait.h>
-#include <linux/proc_fs.h>
-#include <linux/interrupt.h>
-#include <linux/stddef.h>
-#include <linux/stat.h>
-#include <linux/fcntl.h>
-#include <linux/unistd.h>
-#include <linux/sched.h>
-#include <linux/mm.h>
-#include <linux/pagemap.h>
-#include <linux/kthread.h>
-#include <linux/tcp.h>
-#include <linux/udp.h>
-#include <linux/usb.h>
-#include <linux/uaccess.h>
-#include <net/ip.h>
-
-#include "Typedefs.h"
-#include "Macros.h"
-#include "HostMIBSInterface.h"
-#include "cntrl_SignalingInterface.h"
-#include "PHSDefines.h"
-#include "led_control.h"
-#include "Ioctl.h"
-#include "nvm.h"
-#include "target_params.h"
-#include "Adapter.h"
-#include "CmHost.h"
-#include "DDRInit.h"
-#include "Debug.h"
-#include "IPv6ProtocolHdr.h"
-#include "PHSModule.h"
-#include "Protocol.h"
-#include "Prototypes.h"
-#include "Queue.h"
-#include "vendorspecificextn.h"
-
-#include "InterfaceMacros.h"
-#include "InterfaceAdapter.h"
-#include "InterfaceIsr.h"
-#include "InterfaceMisc.h"
-#include "InterfaceRx.h"
-#include "InterfaceTx.h"
-#include "InterfaceIdleMode.h"
-#include "InterfaceInit.h"
-
-#define DRV_NAME	"beceem"
-#define DEV_NAME	"tarang"
-#define DRV_DESCRIPTION "Beceem Communications Inc. WiMAX driver"
-#define DRV_COPYRIGHT	"Copyright 2010. Beceem Communications Inc"
-#define DRV_VERSION	"5.2.45"
-#define PFX		DRV_NAME " "
-
-extern struct class *bcm_class;
-
-#endif
diff --git a/drivers/staging/bcm/hostmibs.c b/drivers/staging/bcm/hostmibs.c
deleted file mode 100644
index f9b08a5d8ce8..000000000000
--- a/drivers/staging/bcm/hostmibs.c
+++ /dev/null
@@ -1,164 +0,0 @@
-/*
- * File Name: hostmibs.c
- *
- * Author: Beceem Communications Pvt. Ltd
- *
- * Abstract: This file contains the routines to copy the statistics used by
- * the driver to the Host MIBS structure and giving the same to Application.
- */
-
-#include "headers.h"
-
-INT ProcessGetHostMibs(struct bcm_mini_adapter *Adapter,
-		       struct bcm_host_stats_mibs *pstHostMibs)
-{
-	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
-	struct bcm_phs_rule *pstPhsRule = NULL;
-	struct bcm_phs_classifier_table *pstClassifierTable = NULL;
-	struct bcm_phs_classifier_entry *pstClassifierRule = NULL;
-	struct bcm_phs_extension *pDeviceExtension = &Adapter->stBCMPhsContext;
-	struct bcm_mibs_host_info *host_info;
-	UINT nClassifierIndex = 0;
-	UINT nPhsTableIndex = 0;
-	UINT nSfIndex = 0;
-	UINT uiIndex = 0;
-
-	if (pDeviceExtension == NULL) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, HOST_MIBS,
-				DBG_LVL_ALL, "Invalid Device Extension\n");
-		return STATUS_FAILURE;
-	}
-
-	/* Copy the classifier Table */
-	for (nClassifierIndex = 0; nClassifierIndex < MAX_CLASSIFIERS;
-							nClassifierIndex++) {
-		if (Adapter->astClassifierTable[nClassifierIndex].bUsed == TRUE)
-			memcpy(&pstHostMibs->astClassifierTable[nClassifierIndex],
-			       &Adapter->astClassifierTable[nClassifierIndex],
-			       sizeof(struct bcm_mibs_classifier_rule));
-	}
-
-	/* Copy the SF Table */
-	for (nSfIndex = 0; nSfIndex < NO_OF_QUEUES; nSfIndex++) {
-		if (Adapter->PackInfo[nSfIndex].bValid) {
-			memcpy(&pstHostMibs->astSFtable[nSfIndex],
-			       &Adapter->PackInfo[nSfIndex],
-			       sizeof(struct bcm_mibs_table));
-		} else {
-			/* If index in not valid,
-			 * don't process this for the PHS table.
-			 * Go For the next entry.
-			 */
-			continue;
-		}
-
-		/* Retrieve the SFID Entry Index for requested Service Flow */
-		if (PHS_INVALID_TABLE_INDEX ==
-		    GetServiceFlowEntry(pDeviceExtension->
-					pstServiceFlowPhsRulesTable,
-					Adapter->PackInfo[nSfIndex].
-					usVCID_Value, &pstServiceFlowEntry))
-
-			continue;
-
-		pstClassifierTable = pstServiceFlowEntry->pstClassifierTable;
-
-		for (uiIndex = 0; uiIndex < MAX_PHSRULE_PER_SF; uiIndex++) {
-			pstClassifierRule = &pstClassifierTable->stActivePhsRulesList[uiIndex];
-
-			if (pstClassifierRule->bUsed) {
-				pstPhsRule = pstClassifierRule->pstPhsRule;
-
-				pstHostMibs->astPhsRulesTable[nPhsTableIndex].
-				    ulSFID = Adapter->PackInfo[nSfIndex].ulSFID;
-
-				memcpy(&pstHostMibs->astPhsRulesTable[nPhsTableIndex].u8PHSI,
-				       &pstPhsRule->u8PHSI,
-				       sizeof(struct bcm_phs_rule));
-				nPhsTableIndex++;
-
-			}
-
-		}
-
-	}
-
-	/* Copy other Host Statistics parameters */
-	host_info = &pstHostMibs->stHostInfo;
-	host_info->GoodTransmits    = Adapter->dev->stats.tx_packets;
-	host_info->GoodReceives	    = Adapter->dev->stats.rx_packets;
-	host_info->CurrNumFreeDesc  = atomic_read(&Adapter->CurrNumFreeTxDesc);
-	host_info->BEBucketSize	    = Adapter->BEBucketSize;
-	host_info->rtPSBucketSize   = Adapter->rtPSBucketSize;
-	host_info->TimerActive	    = Adapter->TimerActive;
-	host_info->u32TotalDSD	    = Adapter->u32TotalDSD;
-
-	memcpy(host_info->aTxPktSizeHist, Adapter->aTxPktSizeHist,
-	       sizeof(UINT32) * MIBS_MAX_HIST_ENTRIES);
-	memcpy(host_info->aRxPktSizeHist, Adapter->aRxPktSizeHist,
-	       sizeof(UINT32) * MIBS_MAX_HIST_ENTRIES);
-
-	return STATUS_SUCCESS;
-}
-
-VOID GetDroppedAppCntrlPktMibs(struct bcm_host_stats_mibs *pstHostMibs,
-			       struct bcm_tarang_data *pTarang)
-{
-	memcpy(&(pstHostMibs->stDroppedAppCntrlMsgs),
-	       &(pTarang->stDroppedAppCntrlMsgs),
-	       sizeof(struct bcm_mibs_dropped_cntrl_msg));
-}
-
-VOID CopyMIBSExtendedSFParameters(struct bcm_mini_adapter *Adapter,
-				  struct bcm_connect_mgr_params *psfLocalSet,
-				  UINT uiSearchRuleIndex)
-{
-	struct bcm_mibs_parameters *t =
-		&Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable;
-
-	t->wmanIfSfid = psfLocalSet->u32SFID;
-	t->wmanIfCmnCpsMaxSustainedRate =
-		psfLocalSet->u32MaxSustainedTrafficRate;
-	t->wmanIfCmnCpsMaxTrafficBurst = psfLocalSet->u32MaxTrafficBurst;
-	t->wmanIfCmnCpsMinReservedRate = psfLocalSet->u32MinReservedTrafficRate;
-	t->wmanIfCmnCpsToleratedJitter = psfLocalSet->u32ToleratedJitter;
-	t->wmanIfCmnCpsMaxLatency = psfLocalSet->u32MaximumLatency;
-	t->wmanIfCmnCpsFixedVsVariableSduInd =
-		psfLocalSet->u8FixedLengthVSVariableLengthSDUIndicator;
-	t->wmanIfCmnCpsFixedVsVariableSduInd =
-		ntohl(t->wmanIfCmnCpsFixedVsVariableSduInd);
-	t->wmanIfCmnCpsSduSize = psfLocalSet->u8SDUSize;
-	t->wmanIfCmnCpsSduSize = ntohl(t->wmanIfCmnCpsSduSize);
-	t->wmanIfCmnCpsSfSchedulingType =
-		psfLocalSet->u8ServiceFlowSchedulingType;
-	t->wmanIfCmnCpsSfSchedulingType =
-		ntohl(t->wmanIfCmnCpsSfSchedulingType);
-	t->wmanIfCmnCpsArqEnable = psfLocalSet->u8ARQEnable;
-	t->wmanIfCmnCpsArqEnable = ntohl(t->wmanIfCmnCpsArqEnable);
-	t->wmanIfCmnCpsArqWindowSize = ntohs(psfLocalSet->u16ARQWindowSize);
-	t->wmanIfCmnCpsArqWindowSize = ntohl(t->wmanIfCmnCpsArqWindowSize);
-	t->wmanIfCmnCpsArqBlockLifetime =
-		ntohs(psfLocalSet->u16ARQBlockLifeTime);
-	t->wmanIfCmnCpsArqBlockLifetime =
-		ntohl(t->wmanIfCmnCpsArqBlockLifetime);
-	t->wmanIfCmnCpsArqSyncLossTimeout =
-		ntohs(psfLocalSet->u16ARQSyncLossTimeOut);
-	t->wmanIfCmnCpsArqSyncLossTimeout =
-		ntohl(t->wmanIfCmnCpsArqSyncLossTimeout);
-	t->wmanIfCmnCpsArqDeliverInOrder = psfLocalSet->u8ARQDeliverInOrder;
-	t->wmanIfCmnCpsArqDeliverInOrder =
-		ntohl(t->wmanIfCmnCpsArqDeliverInOrder);
-	t->wmanIfCmnCpsArqRxPurgeTimeout =
-		ntohs(psfLocalSet->u16ARQRxPurgeTimeOut);
-	t->wmanIfCmnCpsArqRxPurgeTimeout =
-		ntohl(t->wmanIfCmnCpsArqRxPurgeTimeout);
-	t->wmanIfCmnCpsArqBlockSize = ntohs(psfLocalSet->u16ARQBlockSize);
-	t->wmanIfCmnCpsArqBlockSize = ntohl(t->wmanIfCmnCpsArqBlockSize);
-	t->wmanIfCmnCpsReqTxPolicy = psfLocalSet->u8RequesttransmissionPolicy;
-	t->wmanIfCmnCpsReqTxPolicy = ntohl(t->wmanIfCmnCpsReqTxPolicy);
-	t->wmanIfCmnSfCsSpecification = psfLocalSet->u8CSSpecification;
-	t->wmanIfCmnSfCsSpecification = ntohl(t->wmanIfCmnSfCsSpecification);
-	t->wmanIfCmnCpsTargetSaid = ntohs(psfLocalSet->u16TargetSAID);
-	t->wmanIfCmnCpsTargetSaid = ntohl(t->wmanIfCmnCpsTargetSaid);
-
-}
diff --git a/drivers/staging/bcm/led_control.c b/drivers/staging/bcm/led_control.c
deleted file mode 100644
index 074fc39ed678..000000000000
--- a/drivers/staging/bcm/led_control.c
+++ /dev/null
@@ -1,952 +0,0 @@
-#include "headers.h"
-
-#define STATUS_IMAGE_CHECKSUM_MISMATCH -199
-#define EVENT_SIGNALED 1
-
-static B_UINT16 CFG_CalculateChecksum(B_UINT8 *pu8Buffer, B_UINT32 u32Size)
-{
-	B_UINT16 u16CheckSum = 0;
-
-	while (u32Size--) {
-		u16CheckSum += (B_UINT8)~(*pu8Buffer);
-		pu8Buffer++;
-	}
-	return u16CheckSum;
-}
-
-bool IsReqGpioIsLedInNVM(struct bcm_mini_adapter *Adapter, UINT gpios)
-{
-	INT Status;
-
-	Status = (Adapter->gpioBitMap & gpios) ^ gpios;
-	if (Status)
-		return false;
-	else
-		return TRUE;
-}
-
-static INT LED_Blink(struct bcm_mini_adapter *Adapter,
-		     UINT GPIO_Num,
-		     UCHAR uiLedIndex,
-		     ULONG timeout,
-		     INT num_of_time,
-		     enum bcm_led_events currdriverstate)
-{
-	int Status = STATUS_SUCCESS;
-	bool bInfinite = false;
-
-	/* Check if num_of_time is -ve. If yes, blink led in infinite loop */
-	if (num_of_time < 0) {
-		bInfinite = TRUE;
-		num_of_time = 1;
-	}
-	while (num_of_time) {
-		if (currdriverstate == Adapter->DriverState)
-			TURN_ON_LED(Adapter, GPIO_Num, uiLedIndex);
-
-		/* Wait for timeout after setting on the LED */
-		Status = wait_event_interruptible_timeout(
-				Adapter->LEDInfo.notify_led_event,
-				currdriverstate != Adapter->DriverState ||
-					kthread_should_stop(),
-				msecs_to_jiffies(timeout));
-
-		if (kthread_should_stop()) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
-				DBG_LVL_ALL,
-				"Led thread got signal to exit..hence exiting");
-			Adapter->LEDInfo.led_thread_running =
-					BCM_LED_THREAD_DISABLED;
-			TURN_OFF_LED(Adapter, GPIO_Num, uiLedIndex);
-			Status = EVENT_SIGNALED;
-			break;
-		}
-		if (Status) {
-			TURN_OFF_LED(Adapter, GPIO_Num, uiLedIndex);
-			Status = EVENT_SIGNALED;
-			break;
-		}
-
-		TURN_OFF_LED(Adapter, GPIO_Num, uiLedIndex);
-		Status = wait_event_interruptible_timeout(
-				Adapter->LEDInfo.notify_led_event,
-				currdriverstate != Adapter->DriverState ||
-					kthread_should_stop(),
-				msecs_to_jiffies(timeout));
-		if (bInfinite == false)
-			num_of_time--;
-	}
-	return Status;
-}
-
-static INT ScaleRateofTransfer(ULONG rate)
-{
-	if (rate <= 3)
-		return rate;
-	else if ((rate > 3) && (rate <= 100))
-		return 5;
-	else if ((rate > 100) && (rate <= 200))
-		return 6;
-	else if ((rate > 200) && (rate <= 300))
-		return 7;
-	else if ((rate > 300) && (rate <= 400))
-		return 8;
-	else if ((rate > 400) && (rate <= 500))
-		return 9;
-	else if ((rate > 500) && (rate <= 600))
-		return 10;
-	else
-		return MAX_NUM_OF_BLINKS;
-}
-
-static INT blink_in_normal_bandwidth(struct bcm_mini_adapter *ad,
-				     INT *time,
-				     INT *time_tx,
-				     INT *time_rx,
-				     UCHAR GPIO_Num_tx,
-				     UCHAR uiTxLedIndex,
-				     UCHAR GPIO_Num_rx,
-				     UCHAR uiRxLedIndex,
-				     enum bcm_led_events currdriverstate,
-				     ulong *timeout)
-{
-	/*
-	 * Assign minimum number of blinks of
-	 * either Tx or Rx.
-	 */
-	*time = (*time_tx > *time_rx ? *time_rx : *time_tx);
-
-	if (*time > 0) {
-		/* Blink both Tx and Rx LEDs */
-		if ((LED_Blink(ad, 1 << GPIO_Num_tx, uiTxLedIndex, *timeout,
-			      *time, currdriverstate) == EVENT_SIGNALED) ||
-		    (LED_Blink(ad, 1 << GPIO_Num_rx, uiRxLedIndex, *timeout,
-			      *time, currdriverstate) == EVENT_SIGNALED))
-			return EVENT_SIGNALED;
-	}
-
-	if (*time == *time_tx) {
-		/* Blink pending rate of Rx */
-		if (LED_Blink(ad, (1 << GPIO_Num_rx), uiRxLedIndex, *timeout,
-			      *time_rx - *time,
-			      currdriverstate) == EVENT_SIGNALED)
-			return EVENT_SIGNALED;
-
-		*time = *time_rx;
-	} else {
-		/* Blink pending rate of Tx */
-		if (LED_Blink(ad, 1 << GPIO_Num_tx, uiTxLedIndex, *timeout,
-			      *time_tx - *time,
-			      currdriverstate) == EVENT_SIGNALED)
-			return EVENT_SIGNALED;
-
-		*time = *time_tx;
-	}
-
-	return 0;
-}
-
-static INT LED_Proportional_Blink(struct bcm_mini_adapter *Adapter,
-				  UCHAR GPIO_Num_tx,
-				  UCHAR uiTxLedIndex,
-				  UCHAR GPIO_Num_rx,
-				  UCHAR uiRxLedIndex,
-				  enum bcm_led_events currdriverstate)
-{
-	/* Initial values of TX and RX packets */
-	ULONG64 Initial_num_of_packts_tx = 0, Initial_num_of_packts_rx = 0;
-	/* values of TX and RX packets after 1 sec */
-	ULONG64 Final_num_of_packts_tx = 0, Final_num_of_packts_rx = 0;
-	/* Rate of transfer of Tx and Rx in 1 sec */
-	ULONG64 rate_of_transfer_tx = 0, rate_of_transfer_rx = 0;
-	int Status = STATUS_SUCCESS;
-	INT num_of_time = 0, num_of_time_tx = 0, num_of_time_rx = 0;
-	UINT remDelay = 0;
-	/* UINT GPIO_num = DISABLE_GPIO_NUM; */
-	ulong timeout = 0;
-
-	/* Read initial value of packets sent/received */
-	Initial_num_of_packts_tx = Adapter->dev->stats.tx_packets;
-	Initial_num_of_packts_rx = Adapter->dev->stats.rx_packets;
-
-	/* Scale the rate of transfer to no of blinks. */
-	num_of_time_tx = ScaleRateofTransfer((ULONG)rate_of_transfer_tx);
-	num_of_time_rx = ScaleRateofTransfer((ULONG)rate_of_transfer_rx);
-
-	while ((Adapter->device_removed == false)) {
-		timeout = 50;
-
-		if (EVENT_SIGNALED == blink_in_normal_bandwidth(Adapter,
-								&num_of_time,
-								&num_of_time_tx,
-								&num_of_time_rx,
-								GPIO_Num_tx,
-								uiTxLedIndex,
-								GPIO_Num_rx,
-								uiRxLedIndex,
-								currdriverstate,
-								&timeout))
-			return EVENT_SIGNALED;
-
-
-		/*
-		 * If Tx/Rx rate is less than maximum blinks per second,
-		 * wait till delay completes to 1 second
-		 */
-		remDelay = MAX_NUM_OF_BLINKS - num_of_time;
-		if (remDelay > 0) {
-			timeout = 100 * remDelay;
-			Status = wait_event_interruptible_timeout(
-					Adapter->LEDInfo.notify_led_event,
-					currdriverstate != Adapter->DriverState
-						|| kthread_should_stop(),
-					msecs_to_jiffies(timeout));
-
-			if (kthread_should_stop()) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
-					LED_DUMP_INFO, DBG_LVL_ALL,
-					"Led thread got signal to exit..hence exiting");
-				Adapter->LEDInfo.led_thread_running =
-						BCM_LED_THREAD_DISABLED;
-				return EVENT_SIGNALED;
-			}
-			if (Status)
-				return EVENT_SIGNALED;
-		}
-
-		/* Turn off both Tx and Rx LEDs before next second */
-		TURN_OFF_LED(Adapter, 1 << GPIO_Num_tx, uiTxLedIndex);
-		TURN_OFF_LED(Adapter, 1 << GPIO_Num_rx, uiTxLedIndex);
-
-		/*
-		 * Read the Tx & Rx packets transmission after 1 second and
-		 * calculate rate of transfer
-		 */
-		Final_num_of_packts_tx = Adapter->dev->stats.tx_packets;
-		Final_num_of_packts_rx = Adapter->dev->stats.rx_packets;
-
-		rate_of_transfer_tx = Final_num_of_packts_tx -
-						Initial_num_of_packts_tx;
-		rate_of_transfer_rx = Final_num_of_packts_rx -
-						Initial_num_of_packts_rx;
-
-		/* Read initial value of packets sent/received */
-		Initial_num_of_packts_tx = Final_num_of_packts_tx;
-		Initial_num_of_packts_rx = Final_num_of_packts_rx;
-
-		/* Scale the rate of transfer to no of blinks. */
-		num_of_time_tx =
-			ScaleRateofTransfer((ULONG)rate_of_transfer_tx);
-		num_of_time_rx =
-			ScaleRateofTransfer((ULONG)rate_of_transfer_rx);
-
-	}
-	return Status;
-}
-
-/*
- * -----------------------------------------------------------------------------
- * Procedure:   ValidateDSDParamsChecksum
- *
- * Description: Reads DSD Params and validates checkusm.
- *
- * Arguments:
- *      Adapter - Pointer to Adapter structure.
- *      ulParamOffset - Start offset of the DSD parameter to be read and
- *			validated.
- *      usParamLen - Length of the DSD Parameter.
- *
- * Returns:
- *  <OSAL_STATUS_CODE>
- * -----------------------------------------------------------------------------
- */
-static INT ValidateDSDParamsChecksum(struct bcm_mini_adapter *Adapter,
-				     ULONG ulParamOffset,
-				     USHORT usParamLen)
-{
-	INT Status = STATUS_SUCCESS;
-	PUCHAR puBuffer = NULL;
-	USHORT usChksmOrg = 0;
-	USHORT usChecksumCalculated = 0;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,
-			"LED Thread:ValidateDSDParamsChecksum: 0x%lx 0x%X",
-			ulParamOffset, usParamLen);
-
-	puBuffer = kmalloc(usParamLen, GFP_KERNEL);
-	if (!puBuffer) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
-				DBG_LVL_ALL,
-				"LED Thread: ValidateDSDParamsChecksum Allocation failed");
-		return -ENOMEM;
-
-	}
-
-	/* Read the DSD data from the parameter offset. */
-	if (STATUS_SUCCESS != BeceemNVMRead(Adapter, (PUINT)puBuffer,
-					    ulParamOffset, usParamLen)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
-				DBG_LVL_ALL,
-				"LED Thread: ValidateDSDParamsChecksum BeceemNVMRead failed");
-		Status = STATUS_IMAGE_CHECKSUM_MISMATCH;
-		goto exit;
-	}
-
-	/* Calculate the checksum of the data read from the DSD parameter. */
-	usChecksumCalculated = CFG_CalculateChecksum(puBuffer, usParamLen);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,
-			"LED Thread: usCheckSumCalculated = 0x%x\n",
-			usChecksumCalculated);
-
-	/*
-	 * End of the DSD parameter will have a TWO bytes checksum stored in it.
-	 * Read it and compare with the calculated Checksum.
-	 */
-	if (STATUS_SUCCESS != BeceemNVMRead(Adapter, (PUINT)&usChksmOrg,
-					    ulParamOffset+usParamLen, 2)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
-				DBG_LVL_ALL,
-				"LED Thread: ValidateDSDParamsChecksum BeceemNVMRead failed");
-		Status = STATUS_IMAGE_CHECKSUM_MISMATCH;
-		goto exit;
-	}
-	usChksmOrg = ntohs(usChksmOrg);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,
-			"LED Thread: usChksmOrg = 0x%x", usChksmOrg);
-
-	/*
-	 * Compare the checksum calculated with the checksum read
-	 * from DSD section
-	 */
-	if (usChecksumCalculated ^ usChksmOrg) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
-				DBG_LVL_ALL,
-				"LED Thread: ValidateDSDParamsChecksum: Checksums don't match");
-		Status = STATUS_IMAGE_CHECKSUM_MISMATCH;
-		goto exit;
-	}
-
-exit:
-	kfree(puBuffer);
-	return Status;
-}
-
-
-/*
- * -----------------------------------------------------------------------------
- * Procedure:   ValidateHWParmStructure
- *
- * Description: Validates HW Parameters.
- *
- * Arguments:
- *      Adapter - Pointer to Adapter structure.
- *      ulHwParamOffset - Start offset of the HW parameter Section to be read
- *				and validated.
- *
- * Returns:
- *  <OSAL_STATUS_CODE>
- * -----------------------------------------------------------------------------
- */
-static INT ValidateHWParmStructure(struct bcm_mini_adapter *Adapter,
-				   ULONG ulHwParamOffset)
-{
-
-	INT Status = STATUS_SUCCESS;
-	USHORT HwParamLen = 0;
-	/*
-	 * Add DSD start offset to the hwParamOffset to get
-	 * the actual address.
-	 */
-	ulHwParamOffset += DSD_START_OFFSET;
-
-	/* Read the Length of HW_PARAM structure */
-	BeceemNVMRead(Adapter, (PUINT)&HwParamLen, ulHwParamOffset, 2);
-	HwParamLen = ntohs(HwParamLen);
-	if (0 == HwParamLen || HwParamLen > Adapter->uiNVMDSDSize)
-		return STATUS_IMAGE_CHECKSUM_MISMATCH;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,
-			"LED Thread:HwParamLen = 0x%x", HwParamLen);
-	Status = ValidateDSDParamsChecksum(Adapter, ulHwParamOffset,
-					   HwParamLen);
-	return Status;
-} /* ValidateHWParmStructure() */
-
-static int ReadLEDInformationFromEEPROM(struct bcm_mini_adapter *Adapter,
-					UCHAR GPIO_Array[])
-{
-	int Status = STATUS_SUCCESS;
-
-	ULONG  dwReadValue	= 0;
-	USHORT usHwParamData	= 0;
-	USHORT usEEPROMVersion	= 0;
-	UCHAR  ucIndex		= 0;
-	UCHAR  ucGPIOInfo[32]	= {0};
-
-	BeceemNVMRead(Adapter, (PUINT)&usEEPROMVersion,
-		      EEPROM_VERSION_OFFSET, 2);
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,
-			"usEEPROMVersion: Minor:0x%X Major:0x%x",
-			usEEPROMVersion & 0xFF,
-			((usEEPROMVersion >> 8) & 0xFF));
-
-
-	if (((usEEPROMVersion>>8)&0xFF) < EEPROM_MAP5_MAJORVERSION) {
-		BeceemNVMRead(Adapter, (PUINT)&usHwParamData,
-			      EEPROM_HW_PARAM_POINTER_ADDRESS, 2);
-		usHwParamData = ntohs(usHwParamData);
-		dwReadValue   = usHwParamData;
-	} else {
-		/*
-		 * Validate Compatibility section and then read HW param
-		 * if compatibility section is valid.
-		 */
-		Status = ValidateDSDParamsChecksum(Adapter,
-						   DSD_START_OFFSET,
-						   COMPATIBILITY_SECTION_LENGTH_MAP5);
-
-		if (Status != STATUS_SUCCESS)
-			return Status;
-
-		BeceemNVMRead(Adapter, (PUINT)&dwReadValue,
-			      EEPROM_HW_PARAM_POINTER_ADDRRES_MAP5, 4);
-		dwReadValue = ntohl(dwReadValue);
-	}
-
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,
-			"LED Thread: Start address of HW_PARAM structure = 0x%lx",
-			dwReadValue);
-
-	/*
-	 * Validate if the address read out is within the DSD.
-	 * Adapter->uiNVMDSDSize gives whole DSD size inclusive of Autoinit.
-	 * lower limit should be above DSD_START_OFFSET and
-	 * upper limit should be below (Adapter->uiNVMDSDSize-DSD_START_OFFSET)
-	 */
-	if (dwReadValue < DSD_START_OFFSET ||
-			dwReadValue > (Adapter->uiNVMDSDSize-DSD_START_OFFSET))
-		return STATUS_IMAGE_CHECKSUM_MISMATCH;
-
-	Status = ValidateHWParmStructure(Adapter, dwReadValue);
-	if (Status)
-		return Status;
-
-	/*
-	 * Add DSD_START_OFFSET to the offset read from the EEPROM.
-	 * This will give the actual start HW Parameters start address.
-	 * To read GPIO section, add GPIO offset further.
-	 */
-
-	dwReadValue += DSD_START_OFFSET;
-			/* = start address of hw param section. */
-	dwReadValue += GPIO_SECTION_START_OFFSET;
-			/* = GPIO start offset within HW Param section. */
-
-	/*
-	 * Read the GPIO values for 32 GPIOs from EEPROM and map the function
-	 * number to GPIO pin number to GPIO_Array
-	 */
-	BeceemNVMRead(Adapter, (UINT *)ucGPIOInfo, dwReadValue, 32);
-	for (ucIndex = 0; ucIndex < 32; ucIndex++) {
-
-		switch (ucGPIOInfo[ucIndex]) {
-		case RED_LED:
-			GPIO_Array[RED_LED] = ucIndex;
-			Adapter->gpioBitMap |= (1 << ucIndex);
-			break;
-		case BLUE_LED:
-			GPIO_Array[BLUE_LED] = ucIndex;
-			Adapter->gpioBitMap |= (1 << ucIndex);
-			break;
-		case YELLOW_LED:
-			GPIO_Array[YELLOW_LED] = ucIndex;
-			Adapter->gpioBitMap |= (1 << ucIndex);
-			break;
-		case GREEN_LED:
-			GPIO_Array[GREEN_LED] = ucIndex;
-			Adapter->gpioBitMap |= (1 << ucIndex);
-			break;
-		default:
-			break;
-		}
-
-	}
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,
-			"GPIO's bit map correspond to LED :0x%X",
-			Adapter->gpioBitMap);
-	return Status;
-}
-
-
-static int ReadConfigFileStructure(struct bcm_mini_adapter *Adapter,
-				   bool *bEnableThread)
-{
-	int Status = STATUS_SUCCESS;
-	/* Array to store GPIO numbers from EEPROM */
-	UCHAR GPIO_Array[NUM_OF_LEDS+1];
-	UINT uiIndex = 0;
-	UINT uiNum_of_LED_Type = 0;
-	PUCHAR puCFGData	= NULL;
-	UCHAR bData = 0;
-	struct bcm_led_state_info *curr_led_state;
-
-	memset(GPIO_Array, DISABLE_GPIO_NUM, NUM_OF_LEDS+1);
-
-	if (!Adapter->pstargetparams || IS_ERR(Adapter->pstargetparams)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
-				DBG_LVL_ALL, "Target Params not Avail.\n");
-		return -ENOENT;
-	}
-
-	/* Populate GPIO_Array with GPIO numbers for LED functions */
-	/* Read the GPIO numbers from EEPROM */
-	Status = ReadLEDInformationFromEEPROM(Adapter, GPIO_Array);
-	if (Status == STATUS_IMAGE_CHECKSUM_MISMATCH) {
-		*bEnableThread = false;
-		return STATUS_SUCCESS;
-	} else if (Status) {
-		*bEnableThread = false;
-		return Status;
-	}
-
-	/*
-	 * CONFIG file read successfully. Deallocate the memory of
-	 * uiFileNameBufferSize
-	 */
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,
-			"LED Thread: Config file read successfully\n");
-	puCFGData = (PUCHAR) &Adapter->pstargetparams->HostDrvrConfig1;
-
-	/*
-	 * Offset for HostDrvConfig1, HostDrvConfig2, HostDrvConfig3 which
-	 * will have the information of LED type, LED on state for different
-	 * driver state and LED blink state.
-	 */
-
-	for (uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++) {
-		bData = *puCFGData;
-		curr_led_state = &Adapter->LEDInfo.LEDState[uiIndex];
-
-		/*
-		 * Check Bit 8 for polarity. If it is set,
-		 * polarity is reverse polarity
-		 */
-		if (bData & 0x80) {
-			curr_led_state->BitPolarity = 0;
-			/* unset the bit 8 */
-			bData = bData & 0x7f;
-		}
-
-		curr_led_state->LED_Type = bData;
-		if (bData <= NUM_OF_LEDS)
-			curr_led_state->GPIO_Num = GPIO_Array[bData];
-		else
-			curr_led_state->GPIO_Num = DISABLE_GPIO_NUM;
-
-		puCFGData++;
-		bData = *puCFGData;
-		curr_led_state->LED_On_State = bData;
-		puCFGData++;
-		bData = *puCFGData;
-		curr_led_state->LED_Blink_State = bData;
-		puCFGData++;
-	}
-
-	/*
-	 * Check if all the LED settings are disabled. If it is disabled,
-	 * dont launch the LED control thread.
-	 */
-	for (uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++) {
-		curr_led_state = &Adapter->LEDInfo.LEDState[uiIndex];
-
-		if ((curr_led_state->LED_Type == DISABLE_GPIO_NUM) ||
-			(curr_led_state->LED_Type == 0x7f) ||
-			(curr_led_state->LED_Type == 0))
-			uiNum_of_LED_Type++;
-	}
-	if (uiNum_of_LED_Type >= NUM_OF_LEDS)
-		*bEnableThread = false;
-
-	return Status;
-}
-
-/*
- * -----------------------------------------------------------------------------
- * Procedure:   LedGpioInit
- *
- * Description: Initializes LED GPIOs. Makes the LED GPIOs to OUTPUT mode
- *			  and make the initial state to be OFF.
- *
- * Arguments:
- *      Adapter - Pointer to MINI_ADAPTER structure.
- *
- * Returns: VOID
- *
- * -----------------------------------------------------------------------------
- */
-static VOID LedGpioInit(struct bcm_mini_adapter *Adapter)
-{
-	UINT uiResetValue = 0;
-	UINT uiIndex      = 0;
-	struct bcm_led_state_info *curr_led_state;
-
-	/* Set all LED GPIO Mode to output mode */
-	if (rdmalt(Adapter, GPIO_MODE_REGISTER, &uiResetValue,
-		   sizeof(uiResetValue)) < 0)
-		BCM_DEBUG_PRINT (Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
-			DBG_LVL_ALL, "LED Thread: RDM Failed\n");
-	for (uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++) {
-		curr_led_state = &Adapter->LEDInfo.LEDState[uiIndex];
-
-		if (curr_led_state->GPIO_Num != DISABLE_GPIO_NUM)
-			uiResetValue |= (1 << curr_led_state->GPIO_Num);
-
-		TURN_OFF_LED(Adapter, 1 << curr_led_state->GPIO_Num, uiIndex);
-
-	}
-	if (wrmalt(Adapter, GPIO_MODE_REGISTER, &uiResetValue,
-		   sizeof(uiResetValue)) < 0)
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
-				DBG_LVL_ALL, "LED Thread: WRM Failed\n");
-
-	Adapter->LEDInfo.bIdle_led_off = false;
-}
-
-static INT BcmGetGPIOPinInfo(struct bcm_mini_adapter *Adapter,
-			     UCHAR *GPIO_num_tx,
-			     UCHAR *GPIO_num_rx,
-			     UCHAR *uiLedTxIndex,
-			     UCHAR *uiLedRxIndex,
-			     enum bcm_led_events currdriverstate)
-{
-	UINT uiIndex = 0;
-	struct bcm_led_state_info *led_state_info;
-
-	*GPIO_num_tx = DISABLE_GPIO_NUM;
-	*GPIO_num_rx = DISABLE_GPIO_NUM;
-
-	for (uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++) {
-		led_state_info = &Adapter->LEDInfo.LEDState[uiIndex];
-
-		if (((currdriverstate == NORMAL_OPERATION) ||
-			(currdriverstate == IDLEMODE_EXIT) ||
-			(currdriverstate == FW_DOWNLOAD)) &&
-		    (led_state_info->LED_Blink_State & currdriverstate) &&
-		    (led_state_info->GPIO_Num != DISABLE_GPIO_NUM)) {
-			if (*GPIO_num_tx == DISABLE_GPIO_NUM) {
-				*GPIO_num_tx = led_state_info->GPIO_Num;
-				*uiLedTxIndex = uiIndex;
-			} else {
-				*GPIO_num_rx = led_state_info->GPIO_Num;
-				*uiLedRxIndex = uiIndex;
-			}
-		} else {
-			if ((led_state_info->LED_On_State & currdriverstate) &&
-			    (led_state_info->GPIO_Num != DISABLE_GPIO_NUM)) {
-				*GPIO_num_tx = led_state_info->GPIO_Num;
-				*uiLedTxIndex = uiIndex;
-			}
-		}
-	}
-	return STATUS_SUCCESS;
-}
-
-static void handle_adapter_driver_state(struct bcm_mini_adapter *ad,
-					enum bcm_led_events currdriverstate,
-					UCHAR GPIO_num,
-					UCHAR dummyGPIONum,
-					UCHAR uiLedIndex,
-					UCHAR dummyIndex,
-					ulong timeout,
-					UINT uiResetValue,
-					UINT uiIndex)
-{
-	switch (ad->DriverState) {
-	case DRIVER_INIT:
-		currdriverstate = DRIVER_INIT;
-				/* ad->DriverState; */
-		BcmGetGPIOPinInfo(ad, &GPIO_num, &dummyGPIONum,
-				  &uiLedIndex, &dummyIndex,
-				  currdriverstate);
-
-		if (GPIO_num != DISABLE_GPIO_NUM)
-			TURN_ON_LED(ad, 1 << GPIO_num, uiLedIndex);
-
-		break;
-	case FW_DOWNLOAD:
-		/*
-		 * BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS,
-		 *	LED_DUMP_INFO, DBG_LVL_ALL,
-		 *	"LED Thread: FW_DN_DONE called\n");
-		 */
-		currdriverstate = FW_DOWNLOAD;
-		BcmGetGPIOPinInfo(ad, &GPIO_num, &dummyGPIONum,
-				  &uiLedIndex, &dummyIndex,
-				  currdriverstate);
-
-		if (GPIO_num != DISABLE_GPIO_NUM) {
-			timeout = 50;
-			LED_Blink(ad, 1 << GPIO_num, uiLedIndex, timeout,
-				  -1, currdriverstate);
-		}
-		break;
-	case FW_DOWNLOAD_DONE:
-		currdriverstate = FW_DOWNLOAD_DONE;
-		BcmGetGPIOPinInfo(ad, &GPIO_num, &dummyGPIONum,
-				  &uiLedIndex, &dummyIndex, currdriverstate);
-		if (GPIO_num != DISABLE_GPIO_NUM)
-			TURN_ON_LED(ad, 1 << GPIO_num, uiLedIndex);
-		break;
-
-	case SHUTDOWN_EXIT:
-		/*
-		 * no break, continue to NO_NETWORK_ENTRY
-		 * state as well.
-		 */
-	case NO_NETWORK_ENTRY:
-		currdriverstate = NO_NETWORK_ENTRY;
-		BcmGetGPIOPinInfo(ad, &GPIO_num, &dummyGPIONum,
-				  &uiLedIndex, &dummyGPIONum, currdriverstate);
-		if (GPIO_num != DISABLE_GPIO_NUM)
-			TURN_ON_LED(ad, 1 << GPIO_num, uiLedIndex);
-		break;
-	case NORMAL_OPERATION:
-		{
-			UCHAR GPIO_num_tx = DISABLE_GPIO_NUM;
-			UCHAR GPIO_num_rx = DISABLE_GPIO_NUM;
-			UCHAR uiLEDTx = 0;
-			UCHAR uiLEDRx = 0;
-
-			currdriverstate = NORMAL_OPERATION;
-			ad->LEDInfo.bIdle_led_off = false;
-
-			BcmGetGPIOPinInfo(ad, &GPIO_num_tx, &GPIO_num_rx,
-					  &uiLEDTx, &uiLEDRx, currdriverstate);
-			if ((GPIO_num_tx == DISABLE_GPIO_NUM) &&
-					(GPIO_num_rx == DISABLE_GPIO_NUM)) {
-				GPIO_num = DISABLE_GPIO_NUM;
-			} else {
-				/*
-				 * If single LED is selected, use same
-				 * for both Tx and Rx
-				 */
-				if (GPIO_num_tx == DISABLE_GPIO_NUM) {
-					GPIO_num_tx = GPIO_num_rx;
-					uiLEDTx = uiLEDRx;
-				} else if (GPIO_num_rx == DISABLE_GPIO_NUM) {
-					GPIO_num_rx = GPIO_num_tx;
-					uiLEDRx = uiLEDTx;
-				}
-				/*
-				 * Blink the LED in proportionate
-				 * to Tx and Rx transmissions.
-				 */
-				LED_Proportional_Blink(ad,
-						       GPIO_num_tx, uiLEDTx,
-						       GPIO_num_rx, uiLEDRx,
-						       currdriverstate);
-			}
-		}
-		break;
-	case LOWPOWER_MODE_ENTER:
-		currdriverstate = LOWPOWER_MODE_ENTER;
-		if (DEVICE_POWERSAVE_MODE_AS_MANUAL_CLOCK_GATING ==
-				ad->ulPowerSaveMode) {
-			/* Turn OFF all the LED */
-			uiResetValue = 0;
-			for (uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++) {
-				if (ad->LEDInfo.LEDState[uiIndex].GPIO_Num != DISABLE_GPIO_NUM)
-					TURN_OFF_LED(ad,
-						     (1 << ad->LEDInfo.LEDState[uiIndex].GPIO_Num),
-						     uiIndex);
-			}
-
-		}
-		/* Turn off LED And WAKE-UP for Sendinf IDLE mode ACK */
-		ad->LEDInfo.bLedInitDone = false;
-		ad->LEDInfo.bIdle_led_off = TRUE;
-		wake_up(&ad->LEDInfo.idleModeSyncEvent);
-		GPIO_num = DISABLE_GPIO_NUM;
-		break;
-	case IDLEMODE_CONTINUE:
-		currdriverstate = IDLEMODE_CONTINUE;
-		GPIO_num = DISABLE_GPIO_NUM;
-		break;
-	case IDLEMODE_EXIT:
-		break;
-	case DRIVER_HALT:
-		currdriverstate = DRIVER_HALT;
-		GPIO_num = DISABLE_GPIO_NUM;
-		for (uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++) {
-			if (ad->LEDInfo.LEDState[uiIndex].GPIO_Num !=
-					DISABLE_GPIO_NUM)
-				TURN_OFF_LED(ad,
-					     (1 << ad->LEDInfo.LEDState[uiIndex].GPIO_Num),
-					     uiIndex);
-		}
-		/* ad->DriverState = DRIVER_INIT; */
-		break;
-	case LED_THREAD_INACTIVE:
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, LED_DUMP_INFO,
-				DBG_LVL_ALL, "InActivating LED thread...");
-		currdriverstate = LED_THREAD_INACTIVE;
-		ad->LEDInfo.led_thread_running =
-				BCM_LED_THREAD_RUNNING_INACTIVELY;
-		ad->LEDInfo.bLedInitDone = false;
-		/* disable ALL LED */
-		for (uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++) {
-			if (ad->LEDInfo.LEDState[uiIndex].GPIO_Num !=
-					DISABLE_GPIO_NUM)
-				TURN_OFF_LED(ad,
-					     (1 << ad->LEDInfo.LEDState[uiIndex].GPIO_Num),
-					     uiIndex);
-		}
-		break;
-	case LED_THREAD_ACTIVE:
-		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, LED_DUMP_INFO,
-				DBG_LVL_ALL, "Activating LED thread again...");
-		if (ad->LinkUpStatus == false)
-			ad->DriverState = NO_NETWORK_ENTRY;
-		else
-			ad->DriverState = NORMAL_OPERATION;
-
-		ad->LEDInfo.led_thread_running =
-				BCM_LED_THREAD_RUNNING_ACTIVELY;
-		break;
-		/* return; */
-	default:
-		break;
-	}
-}
-
-static VOID LEDControlThread(struct bcm_mini_adapter *Adapter)
-{
-	UINT uiIndex = 0;
-	UCHAR GPIO_num = 0;
-	UCHAR uiLedIndex = 0;
-	UINT uiResetValue = 0;
-	enum bcm_led_events currdriverstate = 0;
-	ulong timeout = 0;
-
-	INT Status = 0;
-
-	UCHAR dummyGPIONum = 0;
-	UCHAR dummyIndex = 0;
-
-	/* currdriverstate = Adapter->DriverState; */
-	Adapter->LEDInfo.bIdleMode_tx_from_host = false;
-
-	/*
-	 * Wait till event is triggered
-	 *
-	 * wait_event(Adapter->LEDInfo.notify_led_event,
-	 *	currdriverstate!= Adapter->DriverState);
-	 */
-
-	GPIO_num = DISABLE_GPIO_NUM;
-
-	while (TRUE) {
-		/* Wait till event is triggered */
-		if ((GPIO_num == DISABLE_GPIO_NUM)
-						||
-				((currdriverstate != FW_DOWNLOAD) &&
-				 (currdriverstate != NORMAL_OPERATION) &&
-				 (currdriverstate != LOWPOWER_MODE_ENTER))
-						||
-				(currdriverstate == LED_THREAD_INACTIVE))
-			Status = wait_event_interruptible(
-					Adapter->LEDInfo.notify_led_event,
-					currdriverstate != Adapter->DriverState
-						|| kthread_should_stop());
-
-		if (kthread_should_stop() || Adapter->device_removed) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
-				DBG_LVL_ALL,
-				"Led thread got signal to exit..hence exiting");
-			Adapter->LEDInfo.led_thread_running =
-						BCM_LED_THREAD_DISABLED;
-			TURN_OFF_LED(Adapter, 1 << GPIO_num, uiLedIndex);
-			return; /* STATUS_FAILURE; */
-		}
-
-		if (GPIO_num != DISABLE_GPIO_NUM)
-			TURN_OFF_LED(Adapter, 1 << GPIO_num, uiLedIndex);
-
-		if (Adapter->LEDInfo.bLedInitDone == false) {
-			LedGpioInit(Adapter);
-			Adapter->LEDInfo.bLedInitDone = TRUE;
-		}
-
-		handle_adapter_driver_state(Adapter,
-					    currdriverstate,
-					    GPIO_num,
-					    dummyGPIONum,
-					    uiLedIndex,
-					    dummyIndex,
-					    timeout,
-					    uiResetValue,
-					    uiIndex
-					    );
-	}
-	Adapter->LEDInfo.led_thread_running = BCM_LED_THREAD_DISABLED;
-}
-
-int InitLedSettings(struct bcm_mini_adapter *Adapter)
-{
-	int Status = STATUS_SUCCESS;
-	bool bEnableThread = TRUE;
-	UCHAR uiIndex = 0;
-
-	/*
-	 * Initially set BitPolarity to normal polarity. The bit 8 of LED type
-	 * is used to change the polarity of the LED.
-	 */
-
-	for (uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++)
-		Adapter->LEDInfo.LEDState[uiIndex].BitPolarity = 1;
-
-	/*
-	 * Read the LED settings of CONFIG file and map it
-	 * to GPIO numbers in EEPROM
-	 */
-	Status = ReadConfigFileStructure(Adapter, &bEnableThread);
-	if (STATUS_SUCCESS != Status) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
-				DBG_LVL_ALL,
-				"LED Thread: FAILED in ReadConfigFileStructure\n");
-		return Status;
-	}
-
-	if (Adapter->LEDInfo.led_thread_running) {
-		if (bEnableThread) {
-			;
-		} else {
-			Adapter->DriverState = DRIVER_HALT;
-			wake_up(&Adapter->LEDInfo.notify_led_event);
-			Adapter->LEDInfo.led_thread_running =
-						BCM_LED_THREAD_DISABLED;
-		}
-
-	} else if (bEnableThread) {
-		/* Create secondary thread to handle the LEDs */
-		init_waitqueue_head(&Adapter->LEDInfo.notify_led_event);
-		init_waitqueue_head(&Adapter->LEDInfo.idleModeSyncEvent);
-		Adapter->LEDInfo.led_thread_running =
-					BCM_LED_THREAD_RUNNING_ACTIVELY;
-		Adapter->LEDInfo.bIdle_led_off = false;
-		Adapter->LEDInfo.led_cntrl_threadid =
-			kthread_run((int (*)(void *)) LEDControlThread,
-				    Adapter, "led_control_thread");
-		if (IS_ERR(Adapter->LEDInfo.led_cntrl_threadid)) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
-					DBG_LVL_ALL,
-					"Not able to spawn Kernel Thread\n");
-			Adapter->LEDInfo.led_thread_running =
-				BCM_LED_THREAD_DISABLED;
-			return PTR_ERR(Adapter->LEDInfo.led_cntrl_threadid);
-		}
-	}
-	return Status;
-}
diff --git a/drivers/staging/bcm/led_control.h b/drivers/staging/bcm/led_control.h
deleted file mode 100644
index 1b24bf4658af..000000000000
--- a/drivers/staging/bcm/led_control.h
+++ /dev/null
@@ -1,84 +0,0 @@
-#ifndef _LED_CONTROL_H
-#define _LED_CONTROL_H
-
-#define NUM_OF_LEDS				4
-#define DSD_START_OFFSET			0x0200
-#define EEPROM_VERSION_OFFSET			0x020E
-#define EEPROM_HW_PARAM_POINTER_ADDRESS		0x0218
-#define EEPROM_HW_PARAM_POINTER_ADDRRES_MAP5	0x0220
-#define GPIO_SECTION_START_OFFSET		0x03
-#define COMPATIBILITY_SECTION_LENGTH		42
-#define COMPATIBILITY_SECTION_LENGTH_MAP5	84
-#define EEPROM_MAP5_MAJORVERSION		5
-#define EEPROM_MAP5_MINORVERSION		0
-#define MAX_NUM_OF_BLINKS			10
-#define NUM_OF_GPIO_PINS			16
-#define DISABLE_GPIO_NUM			0xFF
-#define EVENT_SIGNALED				1
-#define MAX_FILE_NAME_BUFFER_SIZE		100
-
-#define TURN_ON_LED(ad, GPIO, index) do {					\
-		unsigned int gpio_val = GPIO;					\
-		(ad->LEDInfo.LEDState[index].BitPolarity == 1) ?	\
-			wrmaltWithLock(ad, BCM_GPIO_OUTPUT_SET_REG, &gpio_val, sizeof(gpio_val)) : \
-			wrmaltWithLock(ad, BCM_GPIO_OUTPUT_CLR_REG, &gpio_val, sizeof(gpio_val)); \
-	} while (0)
-
-#define TURN_OFF_LED(ad, GPIO, index)  do {					\
-		unsigned int gpio_val = GPIO;					\
-		(ad->LEDInfo.LEDState[index].BitPolarity == 1) ?	\
-			wrmaltWithLock(ad, BCM_GPIO_OUTPUT_CLR_REG, &gpio_val, sizeof(gpio_val)) : \
-			wrmaltWithLock(ad, BCM_GPIO_OUTPUT_SET_REG, &gpio_val, sizeof(gpio_val)); \
-	} while (0)
-
-enum bcm_led_colors {
-	RED_LED		= 1,
-	BLUE_LED	= 2,
-	YELLOW_LED	= 3,
-	GREEN_LED	= 4
-};
-
-enum bcm_led_events {
-	SHUTDOWN_EXIT		= 0x00,
-	DRIVER_INIT		= 0x1,
-	FW_DOWNLOAD		= 0x2,
-	FW_DOWNLOAD_DONE	= 0x4,
-	NO_NETWORK_ENTRY	= 0x8,
-	NORMAL_OPERATION	= 0x10,
-	LOWPOWER_MODE_ENTER	= 0x20,
-	IDLEMODE_CONTINUE	= 0x40,
-	IDLEMODE_EXIT		= 0x80,
-	LED_THREAD_INACTIVE	= 0x100,  /* Makes the LED thread Inactivce. It wil be equivallent to putting the thread on hold. */
-	LED_THREAD_ACTIVE	= 0x200,  /* Makes the LED Thread Active back. */
-	DRIVER_HALT		= 0xff
-}; /* Enumerated values of different driver states */
-
-/*
- * Structure which stores the information of different LED types
- * and corresponding LED state information of driver states
- */
-struct bcm_led_state_info {
-	unsigned char LED_Type; /* specify GPIO number - use 0xFF if not used */
-	unsigned char LED_On_State; /* Bits set or reset for different states */
-	unsigned char LED_Blink_State; /* Bits set or reset for blinking LEDs for different states */
-	unsigned char GPIO_Num;
-	unsigned char BitPolarity; /* To represent whether H/W is normal polarity or reverse polarity */
-};
-
-struct bcm_led_info {
-	struct bcm_led_state_info LEDState[NUM_OF_LEDS];
-	bool		bIdleMode_tx_from_host; /* Variable to notify whether driver came out from idlemode due to Host or target */
-	bool		bIdle_led_off;
-	wait_queue_head_t	notify_led_event;
-	wait_queue_head_t	idleModeSyncEvent;
-	struct task_struct	*led_cntrl_threadid;
-	int		led_thread_running;
-	bool		bLedInitDone;
-};
-
-/* LED Thread state. */
-#define BCM_LED_THREAD_DISABLED		0   /* LED Thread is not running. */
-#define BCM_LED_THREAD_RUNNING_ACTIVELY	1   /* LED thread is running. */
-#define BCM_LED_THREAD_RUNNING_INACTIVELY 2 /* LED thread has been put on hold */
-
-#endif
diff --git a/drivers/staging/bcm/nvm.c b/drivers/staging/bcm/nvm.c
deleted file mode 100644
index ce09473fbb1f..000000000000
--- a/drivers/staging/bcm/nvm.c
+++ /dev/null
@@ -1,4661 +0,0 @@
-#include "headers.h"
-
-#define DWORD unsigned int
-
-static int BcmDoChipSelect(struct bcm_mini_adapter *Adapter,
-			   unsigned int offset);
-static int BcmGetActiveDSD(struct bcm_mini_adapter *Adapter);
-static int BcmGetActiveISO(struct bcm_mini_adapter *Adapter);
-static unsigned int BcmGetEEPROMSize(struct bcm_mini_adapter *Adapter);
-static int BcmGetFlashCSInfo(struct bcm_mini_adapter *Adapter);
-static unsigned int BcmGetFlashSectorSize(struct bcm_mini_adapter *Adapter,
-					  unsigned int FlashSectorSizeSig,
-					  unsigned int FlashSectorSize);
-
-static VOID BcmValidateNvmType(struct bcm_mini_adapter *Adapter);
-static int BcmGetNvmSize(struct bcm_mini_adapter *Adapter);
-static unsigned int BcmGetFlashSize(struct bcm_mini_adapter *Adapter);
-static enum bcm_nvm_type BcmGetNvmType(struct bcm_mini_adapter *Adapter);
-
-static int BcmGetSectionValEndOffset(struct bcm_mini_adapter *Adapter,
-				     enum bcm_flash2x_section_val eFlash2xSectionVal);
-
-static B_UINT8 IsOffsetWritable(struct bcm_mini_adapter *Adapter,
-				unsigned int uiOffset);
-static int IsSectionWritable(struct bcm_mini_adapter *Adapter,
-			     enum bcm_flash2x_section_val Section);
-static int IsSectionExistInVendorInfo(struct bcm_mini_adapter *Adapter,
-				      enum bcm_flash2x_section_val section);
-
-static int ReadDSDPriority(struct bcm_mini_adapter *Adapter,
-			   enum bcm_flash2x_section_val dsd);
-static int ReadDSDSignature(struct bcm_mini_adapter *Adapter,
-			    enum bcm_flash2x_section_val dsd);
-static int ReadISOPriority(struct bcm_mini_adapter *Adapter,
-			   enum bcm_flash2x_section_val iso);
-static int ReadISOSignature(struct bcm_mini_adapter *Adapter,
-			    enum bcm_flash2x_section_val iso);
-
-static int CorruptDSDSig(struct bcm_mini_adapter *Adapter,
-			 enum bcm_flash2x_section_val eFlash2xSectionVal);
-static int CorruptISOSig(struct bcm_mini_adapter *Adapter,
-			 enum bcm_flash2x_section_val eFlash2xSectionVal);
-static int SaveHeaderIfPresent(struct bcm_mini_adapter *Adapter,
-			       PUCHAR pBuff,
-			       unsigned int uiSectAlignAddr);
-static int WriteToFlashWithoutSectorErase(struct bcm_mini_adapter *Adapter,
-					  PUINT pBuff,
-					  enum bcm_flash2x_section_val eFlash2xSectionVal,
-					  unsigned int uiOffset,
-					  unsigned int uiNumBytes);
-static enum bcm_flash2x_section_val getHighestPriDSD(struct bcm_mini_adapter *Adapter);
-static enum bcm_flash2x_section_val getHighestPriISO(struct bcm_mini_adapter *Adapter);
-
-static int BeceemFlashBulkRead(
-	struct bcm_mini_adapter *Adapter,
-	PUINT pBuffer,
-	unsigned int uiOffset,
-	unsigned int uiNumBytes);
-
-static int BeceemFlashBulkWrite(
-	struct bcm_mini_adapter *Adapter,
-	PUINT pBuffer,
-	unsigned int uiOffset,
-	unsigned int uiNumBytes,
-	bool bVerify);
-
-static int GetFlashBaseAddr(struct bcm_mini_adapter *Adapter);
-
-static int ReadBeceemEEPROMBulk(struct bcm_mini_adapter *Adapter, unsigned int dwAddress, unsigned int *pdwData, unsigned int dwNumData);
-
-/* Procedure:	ReadEEPROMStatusRegister
- *
- * Description: Reads the standard EEPROM Status Register.
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- * Returns:
- *		OSAL_STATUS_CODE
- */
-static UCHAR ReadEEPROMStatusRegister(struct bcm_mini_adapter *Adapter)
-{
-	UCHAR uiData = 0;
-	DWORD dwRetries = MAX_EEPROM_RETRIES * RETRIES_PER_DELAY;
-	unsigned int uiStatus = 0;
-	unsigned int value = 0;
-	unsigned int value1 = 0;
-
-	/* Read the EEPROM status register */
-	value = EEPROM_READ_STATUS_REGISTER;
-	wrmalt(Adapter, EEPROM_CMDQ_SPI_REG, &value, sizeof(value));
-
-	while (dwRetries != 0) {
-		value = 0;
-		uiStatus = 0;
-		rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &uiStatus, sizeof(uiStatus));
-		if (Adapter->device_removed == TRUE) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Modem has got removed hence exiting....");
-			break;
-		}
-
-		/* Wait for Avail bit to be set. */
-		if ((uiStatus & EEPROM_READ_DATA_AVAIL) != 0) {
-			/* Clear the Avail/Full bits - which ever is set. */
-			value = uiStatus & (EEPROM_READ_DATA_AVAIL | EEPROM_READ_DATA_FULL);
-			wrmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
-
-			value = 0;
-			rdmalt(Adapter, EEPROM_READ_DATAQ_REG, &value, sizeof(value));
-			uiData = (UCHAR)value;
-
-			break;
-		}
-
-		dwRetries--;
-		if (dwRetries == 0) {
-			rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
-			rdmalt(Adapter, EEPROM_SPI_Q_STATUS_REG, &value1, sizeof(value1));
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "0x3004 = %x 0x3008 = %x, retries = %d failed.\n", value, value1, MAX_EEPROM_RETRIES * RETRIES_PER_DELAY);
-			return uiData;
-		}
-		if (!(dwRetries%RETRIES_PER_DELAY))
-			udelay(1000);
-		uiStatus = 0;
-	}
-	return uiData;
-} /* ReadEEPROMStatusRegister */
-
-/*
- * Procedure:	ReadBeceemEEPROMBulk
- *
- * Description: This routine reads 16Byte data from EEPROM
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- *      dwAddress   - EEPROM Offset to read the data from.
- *      pdwData     - Pointer to double word where data needs to be stored in.  //		dwNumWords  - Number of words.  Valid values are 4 ONLY.
- *
- * Returns:
- *		OSAL_STATUS_CODE:
- */
-
-static int ReadBeceemEEPROMBulk(struct bcm_mini_adapter *Adapter,
-			DWORD dwAddress,
-			DWORD *pdwData,
-			DWORD dwNumWords)
-{
-	DWORD dwIndex = 0;
-	DWORD dwRetries = MAX_EEPROM_RETRIES * RETRIES_PER_DELAY;
-	unsigned int uiStatus  = 0;
-	unsigned int value = 0;
-	unsigned int value1 = 0;
-	UCHAR *pvalue;
-
-	/* Flush the read and cmd queue. */
-	value = (EEPROM_READ_QUEUE_FLUSH | EEPROM_CMD_QUEUE_FLUSH);
-	wrmalt(Adapter, SPI_FLUSH_REG, &value, sizeof(value));
-	value = 0;
-	wrmalt(Adapter, SPI_FLUSH_REG, &value, sizeof(value));
-
-	/* Clear the Avail/Full bits. */
-	value = (EEPROM_READ_DATA_AVAIL | EEPROM_READ_DATA_FULL);
-	wrmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
-
-	value = dwAddress | ((dwNumWords == 4) ? EEPROM_16_BYTE_PAGE_READ : EEPROM_4_BYTE_PAGE_READ);
-	wrmalt(Adapter, EEPROM_CMDQ_SPI_REG, &value, sizeof(value));
-
-	while (dwRetries != 0) {
-		uiStatus = 0;
-		rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &uiStatus, sizeof(uiStatus));
-		if (Adapter->device_removed == TRUE) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Modem has got Removed.hence exiting from loop...");
-			return -ENODEV;
-		}
-
-		/* If we are reading 16 bytes we want to be sure that the queue
-		 * is full before we read.  In the other cases we are ok if the
-		 * queue has data available
-		 */
-		if (dwNumWords == 4) {
-			if ((uiStatus & EEPROM_READ_DATA_FULL) != 0) {
-				/* Clear the Avail/Full bits - which ever is set. */
-				value = (uiStatus & (EEPROM_READ_DATA_AVAIL | EEPROM_READ_DATA_FULL));
-				wrmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
-				break;
-			}
-		} else if (dwNumWords == 1) {
-			if ((uiStatus & EEPROM_READ_DATA_AVAIL) != 0) {
-				/* We just got Avail and we have to read 32bits so we
-				 * need this sleep for Cardbus kind of devices.
-				 */
-				if (Adapter->chip_id == 0xBECE0210)
-					udelay(800);
-
-				/* Clear the Avail/Full bits - which ever is set. */
-				value = (uiStatus & (EEPROM_READ_DATA_AVAIL | EEPROM_READ_DATA_FULL));
-				wrmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
-				break;
-			}
-		}
-
-		uiStatus = 0;
-
-		dwRetries--;
-		if (dwRetries == 0) {
-			value = 0;
-			value1 = 0;
-			rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
-			rdmalt(Adapter, EEPROM_SPI_Q_STATUS_REG, &value1, sizeof(value1));
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "dwNumWords %d 0x3004 = %x 0x3008 = %x  retries = %d failed.\n",
-					dwNumWords, value,  value1,  MAX_EEPROM_RETRIES * RETRIES_PER_DELAY);
-			return STATUS_FAILURE;
-		}
-
-		if (!(dwRetries%RETRIES_PER_DELAY))
-			udelay(1000);
-	}
-
-	for (dwIndex = 0; dwIndex < dwNumWords; dwIndex++) {
-		/* We get only a byte at a time - from LSB to MSB. We shift it into an integer. */
-		pvalue = (PUCHAR)(pdwData + dwIndex);
-
-		value = 0;
-		rdmalt(Adapter, EEPROM_READ_DATAQ_REG, &value, sizeof(value));
-
-		pvalue[0] = value;
-
-		value = 0;
-		rdmalt(Adapter, EEPROM_READ_DATAQ_REG, &value, sizeof(value));
-
-		pvalue[1] = value;
-
-		value = 0;
-		rdmalt(Adapter, EEPROM_READ_DATAQ_REG, &value, sizeof(value));
-
-		pvalue[2] = value;
-
-		value = 0;
-		rdmalt(Adapter, EEPROM_READ_DATAQ_REG, &value, sizeof(value));
-
-		pvalue[3] = value;
-	}
-
-	return STATUS_SUCCESS;
-} /* ReadBeceemEEPROMBulk() */
-
-/*
- * Procedure:	ReadBeceemEEPROM
- *
- * Description: This routine reads 4 data from EEPROM.  It uses 1 or 2 page
- *				reads to do this operation.
- *
- * Arguments:
- *		Adapter     - ptr to Adapter object instance
- *      uiOffset	- EEPROM Offset to read the data from.
- *      pBuffer		- Pointer to word where data needs to be stored in.
- *
- * Returns:
- *		OSAL_STATUS_CODE:
- */
-
-int ReadBeceemEEPROM(struct bcm_mini_adapter *Adapter,
-		DWORD uiOffset,
-		DWORD *pBuffer)
-{
-	unsigned int uiData[8]		= {0};
-	unsigned int uiByteOffset	= 0;
-	unsigned int uiTempOffset	= 0;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, " ====> ");
-
-	uiTempOffset = uiOffset - (uiOffset % MAX_RW_SIZE);
-	uiByteOffset = uiOffset - uiTempOffset;
-
-	ReadBeceemEEPROMBulk(Adapter, uiTempOffset, (PUINT)&uiData[0], 4);
-
-	/* A word can overlap at most over 2 pages. In that case we read the
-	 * next page too.
-	 */
-	if (uiByteOffset > 12)
-		ReadBeceemEEPROMBulk(Adapter, uiTempOffset + MAX_RW_SIZE, (PUINT)&uiData[4], 4);
-
-	memcpy((PUCHAR)pBuffer, (((PUCHAR)&uiData[0]) + uiByteOffset), 4);
-
-	return STATUS_SUCCESS;
-} /* ReadBeceemEEPROM() */
-
-int ReadMacAddressFromNVM(struct bcm_mini_adapter *Adapter)
-{
-	int Status;
-	unsigned char puMacAddr[6];
-
-	Status = BeceemNVMRead(Adapter,
-			(PUINT)&puMacAddr[0],
-			INIT_PARAMS_1_MACADDRESS_ADDRESS,
-			MAC_ADDRESS_SIZE);
-
-	if (Status == STATUS_SUCCESS)
-		memcpy(Adapter->dev->dev_addr, puMacAddr, MAC_ADDRESS_SIZE);
-
-	return Status;
-}
-
-/*
- * Procedure:	BeceemEEPROMBulkRead
- *
- * Description: Reads the EEPROM and returns the Data.
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- *		pBuffer    - Buffer to store the data read from EEPROM
- *		uiOffset   - Offset of EEPROM from where data should be read
- *		uiNumBytes - Number of bytes to be read from the EEPROM.
- *
- * Returns:
- *		OSAL_STATUS_SUCCESS - if EEPROM read is successful.
- *		<FAILURE>			- if failed.
- */
-
-int BeceemEEPROMBulkRead(struct bcm_mini_adapter *Adapter,
-			PUINT pBuffer,
-			unsigned int uiOffset,
-			unsigned int uiNumBytes)
-{
-	unsigned int uiData[4]		= {0};
-	/* unsigned int uiAddress	= 0; */
-	unsigned int uiBytesRemaining	= uiNumBytes;
-	unsigned int uiIndex		= 0;
-	unsigned int uiTempOffset	= 0;
-	unsigned int uiExtraBytes	= 0;
-	unsigned int uiFailureRetries	= 0;
-	PUCHAR pcBuff = (PUCHAR)pBuffer;
-
-	if (uiOffset % MAX_RW_SIZE && uiBytesRemaining) {
-		uiTempOffset = uiOffset - (uiOffset % MAX_RW_SIZE);
-		uiExtraBytes = uiOffset - uiTempOffset;
-		ReadBeceemEEPROMBulk(Adapter, uiTempOffset, (PUINT)&uiData[0], 4);
-		if (uiBytesRemaining >= (MAX_RW_SIZE - uiExtraBytes)) {
-			memcpy(pBuffer, (((PUCHAR)&uiData[0]) + uiExtraBytes), MAX_RW_SIZE - uiExtraBytes);
-			uiBytesRemaining -= (MAX_RW_SIZE - uiExtraBytes);
-			uiIndex += (MAX_RW_SIZE - uiExtraBytes);
-			uiOffset += (MAX_RW_SIZE - uiExtraBytes);
-		} else {
-			memcpy(pBuffer, (((PUCHAR)&uiData[0]) + uiExtraBytes), uiBytesRemaining);
-			uiIndex += uiBytesRemaining;
-			uiOffset += uiBytesRemaining;
-			uiBytesRemaining = 0;
-		}
-	}
-
-	while (uiBytesRemaining && uiFailureRetries != 128) {
-		if (Adapter->device_removed)
-			return -1;
-
-		if (uiBytesRemaining >= MAX_RW_SIZE) {
-			/* For the requests more than or equal to 16 bytes, use bulk
-			 * read function to make the access faster.
-			 * We read 4 Dwords of data
-			 */
-			if (ReadBeceemEEPROMBulk(Adapter, uiOffset, &uiData[0], 4) == 0) {
-				memcpy(pcBuff + uiIndex, &uiData[0], MAX_RW_SIZE);
-				uiOffset += MAX_RW_SIZE;
-				uiBytesRemaining -= MAX_RW_SIZE;
-				uiIndex += MAX_RW_SIZE;
-			} else {
-				uiFailureRetries++;
-				mdelay(3); /* sleep for a while before retry... */
-			}
-		} else if (uiBytesRemaining >= 4) {
-			if (ReadBeceemEEPROM(Adapter, uiOffset, &uiData[0]) == 0) {
-				memcpy(pcBuff + uiIndex, &uiData[0], 4);
-				uiOffset += 4;
-				uiBytesRemaining -= 4;
-				uiIndex += 4;
-			} else {
-				uiFailureRetries++;
-				mdelay(3); /* sleep for a while before retry... */
-			}
-		} else {
-			/* Handle the reads less than 4 bytes... */
-			PUCHAR pCharBuff = (PUCHAR)pBuffer;
-
-			pCharBuff += uiIndex;
-			if (ReadBeceemEEPROM(Adapter, uiOffset, &uiData[0]) == 0) {
-				memcpy(pCharBuff, &uiData[0], uiBytesRemaining); /* copy only bytes requested. */
-				uiBytesRemaining = 0;
-			} else {
-				uiFailureRetries++;
-				mdelay(3); /* sleep for a while before retry... */
-			}
-		}
-	}
-
-	return 0;
-}
-
-/*
- * Procedure:	BeceemFlashBulkRead
- *
- * Description: Reads the FLASH and returns the Data.
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- *		pBuffer    - Buffer to store the data read from FLASH
- *		uiOffset   - Offset of FLASH from where data should be read
- *		uiNumBytes - Number of bytes to be read from the FLASH.
- *
- * Returns:
- *		OSAL_STATUS_SUCCESS - if FLASH read is successful.
- *		<FAILURE>			- if failed.
- */
-
-static int BeceemFlashBulkRead(struct bcm_mini_adapter *Adapter,
-			PUINT pBuffer,
-			unsigned int uiOffset,
-			unsigned int uiNumBytes)
-{
-	unsigned int uiIndex = 0;
-	unsigned int uiBytesToRead = uiNumBytes;
-	int Status = 0;
-	unsigned int uiPartOffset = 0;
-	int bytes;
-
-	if (Adapter->device_removed) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Device Got Removed");
-		return -ENODEV;
-	}
-
-	/* Adding flash Base address
-	 * uiOffset = uiOffset + GetFlashBaseAddr(Adapter);
-	 */
-	#if defined(BCM_SHM_INTERFACE) && !defined(FLASH_DIRECT_ACCESS)
-		Status = bcmflash_raw_read((uiOffset/FLASH_PART_SIZE), (uiOffset % FLASH_PART_SIZE), (unsigned char *)pBuffer, uiNumBytes);
-		return Status;
-	#endif
-
-	Adapter->SelectedChip = RESET_CHIP_SELECT;
-
-	if (uiOffset % MAX_RW_SIZE) {
-		BcmDoChipSelect(Adapter, uiOffset);
-		uiPartOffset = (uiOffset & (FLASH_PART_SIZE - 1)) + GetFlashBaseAddr(Adapter);
-
-		uiBytesToRead = MAX_RW_SIZE - (uiOffset % MAX_RW_SIZE);
-		uiBytesToRead = MIN(uiNumBytes, uiBytesToRead);
-
-		bytes = rdm(Adapter, uiPartOffset, (PCHAR)pBuffer + uiIndex, uiBytesToRead);
-		if (bytes < 0) {
-			Status = bytes;
-			Adapter->SelectedChip = RESET_CHIP_SELECT;
-			return Status;
-		}
-
-		uiIndex += uiBytesToRead;
-		uiOffset += uiBytesToRead;
-		uiNumBytes -= uiBytesToRead;
-	}
-
-	while (uiNumBytes) {
-		BcmDoChipSelect(Adapter, uiOffset);
-		uiPartOffset = (uiOffset & (FLASH_PART_SIZE - 1)) + GetFlashBaseAddr(Adapter);
-
-		uiBytesToRead = MIN(uiNumBytes, MAX_RW_SIZE);
-
-		bytes = rdm(Adapter, uiPartOffset, (PCHAR)pBuffer + uiIndex, uiBytesToRead);
-		if (bytes < 0) {
-			Status = bytes;
-			break;
-		}
-
-		uiIndex += uiBytesToRead;
-		uiOffset += uiBytesToRead;
-		uiNumBytes -= uiBytesToRead;
-	}
-	Adapter->SelectedChip = RESET_CHIP_SELECT;
-	return Status;
-}
-
-/*
- * Procedure:	BcmGetFlashSize
- *
- * Description: Finds the size of FLASH.
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- *
- * Returns:
- *		unsigned int - size of the FLASH Storage.
- *
- */
-
-static unsigned int BcmGetFlashSize(struct bcm_mini_adapter *Adapter)
-{
-	if (IsFlash2x(Adapter))
-		return Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + sizeof(struct bcm_dsd_header);
-	else
-		return 32 * 1024;
-}
-
-/*
- * Procedure:	BcmGetEEPROMSize
- *
- * Description: Finds the size of EEPROM.
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- *
- * Returns:
- *		unsigned int - size of the EEPROM Storage.
- *
- */
-
-static unsigned int BcmGetEEPROMSize(struct bcm_mini_adapter *Adapter)
-{
-	unsigned int uiData = 0;
-	unsigned int uiIndex = 0;
-
-	/*
-	 * if EEPROM is present and already Calibrated,it will have
-	 * 'BECM' string at 0th offset.
-	 * To find the EEPROM size read the possible boundaries of the
-	 * EEPROM like 4K,8K etc..accessing the EEPROM beyond its size will
-	 * result in wrap around. So when we get the End of the EEPROM we will
-	 * get 'BECM' string which is indeed at offset 0.
-	 */
-	BeceemEEPROMBulkRead(Adapter, &uiData, 0x0, 4);
-	if (uiData == BECM) {
-		for (uiIndex = 2; uiIndex <= 256; uiIndex *= 2)	{
-			BeceemEEPROMBulkRead(Adapter, &uiData, uiIndex * 1024, 4);
-			if (uiData == BECM)
-				return uiIndex * 1024;
-		}
-	} else {
-		/*
-		 * EEPROM may not be present or not programmed
-		 */
-		uiData = 0xBABEFACE;
-		if (BeceemEEPROMBulkWrite(Adapter, (PUCHAR)&uiData, 0, 4, TRUE) == 0) {
-			uiData = 0;
-			for (uiIndex = 2; uiIndex <= 256; uiIndex *= 2) {
-				BeceemEEPROMBulkRead(Adapter, &uiData, uiIndex * 1024, 4);
-				if (uiData == 0xBABEFACE)
-					return uiIndex * 1024;
-			}
-		}
-	}
-	return 0;
-}
-
-/*
- * Procedure:	FlashSectorErase
- *
- * Description: Finds the sector size of the FLASH.
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- *		addr	   - sector start address
- *		numOfSectors - number of sectors to  be erased.
- *
- * Returns:
- *		OSAL_STATUS_CODE
- *
- */
-
-static int FlashSectorErase(struct bcm_mini_adapter *Adapter,
-			unsigned int addr,
-			unsigned int numOfSectors)
-{
-	unsigned int iIndex = 0, iRetries = 0;
-	unsigned int uiStatus = 0;
-	unsigned int value;
-	int bytes;
-
-	for (iIndex = 0; iIndex < numOfSectors; iIndex++) {
-		value = 0x06000000;
-		wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value));
-
-		value = (0xd8000000 | (addr & 0xFFFFFF));
-		wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value));
-		iRetries = 0;
-
-		do {
-			value = (FLASH_CMD_STATUS_REG_READ << 24);
-			if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Programing of FLASH_SPI_CMDQ_REG fails");
-				return STATUS_FAILURE;
-			}
-
-			bytes = rdmalt(Adapter, FLASH_SPI_READQ_REG, &uiStatus, sizeof(uiStatus));
-			if (bytes < 0) {
-				uiStatus = bytes;
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Reading status of FLASH_SPI_READQ_REG fails");
-				return uiStatus;
-			}
-			iRetries++;
-			/* After every try lets make the CPU free for 10 ms. generally time taken by the
-			 * the sector erase cycle is 500 ms to 40000 msec. hence sleeping 10 ms
-			 * won't hamper performance in any case.
-			 */
-			mdelay(10);
-		} while ((uiStatus & 0x1) && (iRetries < 400));
-
-		if (uiStatus & 0x1) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "iRetries crossing the limit of 80000\n");
-			return STATUS_FAILURE;
-		}
-
-		addr += Adapter->uiSectorSize;
-	}
-	return 0;
-}
-/*
- * Procedure:	flashByteWrite
- *
- * Description: Performs Byte by Byte write to flash
- *
- * Arguments:
- *		Adapter   - ptr to Adapter object instance
- *		uiOffset   - Offset of the flash where data needs to be written to.
- *		pData	- Address of Data to be written.
- * Returns:
- *		OSAL_STATUS_CODE
- *
- */
-
-static int flashByteWrite(struct bcm_mini_adapter *Adapter,
-			unsigned int uiOffset,
-			PVOID pData)
-{
-	unsigned int uiStatus = 0;
-	int  iRetries = MAX_FLASH_RETRIES * FLASH_PER_RETRIES_DELAY; /* 3 */
-	unsigned int value;
-	ULONG ulData = *(PUCHAR)pData;
-	int bytes;
-	/*
-	 * need not write 0xFF because write requires an erase and erase will
-	 * make whole sector 0xFF.
-	 */
-
-	if (0xFF == ulData)
-		return STATUS_SUCCESS;
-
-	/* DumpDebug(NVM_RW,("flashWrite ====>\n")); */
-	value = (FLASH_CMD_WRITE_ENABLE << 24);
-	if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Write enable in FLASH_SPI_CMDQ_REG register fails");
-		return STATUS_FAILURE;
-	}
-
-	if (wrm(Adapter, FLASH_SPI_WRITEQ_REG, (PCHAR)&ulData, 4) < 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "DATA Write on FLASH_SPI_WRITEQ_REG fails");
-		return STATUS_FAILURE;
-	}
-	value = (0x02000000 | (uiOffset & 0xFFFFFF));
-	if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Programming of FLASH_SPI_CMDQ_REG fails");
-		return STATUS_FAILURE;
-	}
-
-	/* __udelay(950); */
-
-	do {
-		value = (FLASH_CMD_STATUS_REG_READ << 24);
-		if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Programing of FLASH_SPI_CMDQ_REG fails");
-			return STATUS_FAILURE;
-		}
-		/* __udelay(1); */
-		bytes = rdmalt(Adapter, FLASH_SPI_READQ_REG, &uiStatus, sizeof(uiStatus));
-		if (bytes < 0) {
-			uiStatus = bytes;
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Reading status of FLASH_SPI_READQ_REG fails");
-			return uiStatus;
-		}
-		iRetries--;
-		if (iRetries && ((iRetries % FLASH_PER_RETRIES_DELAY) == 0))
-			udelay(1000);
-
-	} while ((uiStatus & 0x1) && (iRetries  > 0));
-
-	if (uiStatus & 0x1) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Flash Write fails even after checking status for 200 times.");
-		return STATUS_FAILURE;
-	}
-
-	return STATUS_SUCCESS;
-}
-
-/*
- * Procedure:	flashWrite
- *
- * Description: Performs write to flash
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- *		uiOffset   - Offset of the flash where data needs to be written to.
- *		pData	- Address of Data to be written.
- * Returns:
- *		OSAL_STATUS_CODE
- *
- */
-
-static int flashWrite(struct bcm_mini_adapter *Adapter,
-		unsigned int uiOffset,
-		PVOID pData)
-{
-	/* unsigned int uiStatus = 0;
-	 * int  iRetries = 0;
-	 * unsigned int uiReadBack = 0;
-	 */
-	unsigned int uiStatus = 0;
-	int  iRetries = MAX_FLASH_RETRIES * FLASH_PER_RETRIES_DELAY; /* 3 */
-	unsigned int value;
-	unsigned int uiErasePattern[4] = {0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF};
-	int bytes;
-	/*
-	 * need not write 0xFFFFFFFF because write requires an erase and erase will
-	 * make whole sector 0xFFFFFFFF.
-	 */
-	if (!memcmp(pData, uiErasePattern, MAX_RW_SIZE))
-		return 0;
-
-	value = (FLASH_CMD_WRITE_ENABLE << 24);
-
-	if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Write Enable of FLASH_SPI_CMDQ_REG fails");
-		return STATUS_FAILURE;
-	}
-
-	if (wrm(Adapter, uiOffset, (PCHAR)pData, MAX_RW_SIZE) < 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Data write fails...");
-		return STATUS_FAILURE;
-	}
-
-	/* __udelay(950); */
-	do {
-		value = (FLASH_CMD_STATUS_REG_READ << 24);
-		if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Programing of FLASH_SPI_CMDQ_REG fails");
-			return STATUS_FAILURE;
-		}
-		/* __udelay(1); */
-		bytes = rdmalt(Adapter, FLASH_SPI_READQ_REG, &uiStatus, sizeof(uiStatus));
-		if (bytes < 0) {
-			uiStatus = bytes;
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Reading status of FLASH_SPI_READQ_REG fails");
-			return uiStatus;
-		}
-
-		iRetries--;
-		/* this will ensure that in there will be no changes in the current path.
-		 * currently one rdm/wrm takes 125 us.
-		 * Hence  125 *2 * FLASH_PER_RETRIES_DELAY > 3 ms(worst case delay)
-		 * Hence current implementation cycle will intoduce no delay in current path
-		 */
-		if (iRetries && ((iRetries % FLASH_PER_RETRIES_DELAY) == 0))
-			udelay(1000);
-	} while ((uiStatus & 0x1) && (iRetries > 0));
-
-	if (uiStatus & 0x1) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Flash Write fails even after checking status for 200 times.");
-		return STATUS_FAILURE;
-	}
-
-	return STATUS_SUCCESS;
-}
-
-/*-----------------------------------------------------------------------------
- * Procedure:	flashByteWriteStatus
- *
- * Description: Performs byte by byte write to flash with write done status check
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- *		uiOffset    - Offset of the flash where data needs to be written to.
- *		pData	 - Address of the Data to be written.
- * Returns:
- *		OSAL_STATUS_CODE
- *
- */
-static int flashByteWriteStatus(struct bcm_mini_adapter *Adapter,
-				unsigned int uiOffset,
-				PVOID pData)
-{
-	unsigned int uiStatus = 0;
-	int  iRetries = MAX_FLASH_RETRIES * FLASH_PER_RETRIES_DELAY; /* 3 */
-	ULONG ulData  = *(PUCHAR)pData;
-	unsigned int value;
-	int bytes;
-
-	/*
-	 * need not write 0xFFFFFFFF because write requires an erase and erase will
-	 * make whole sector 0xFFFFFFFF.
-	 */
-
-	if (0xFF == ulData)
-		return STATUS_SUCCESS;
-
-	/* DumpDebug(NVM_RW,("flashWrite ====>\n")); */
-
-	value = (FLASH_CMD_WRITE_ENABLE << 24);
-	if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Write enable in FLASH_SPI_CMDQ_REG register fails");
-		return STATUS_SUCCESS;
-	}
-	if (wrm(Adapter, FLASH_SPI_WRITEQ_REG, (PCHAR)&ulData, 4) < 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "DATA Write on FLASH_SPI_WRITEQ_REG fails");
-		return STATUS_FAILURE;
-	}
-	value = (0x02000000 | (uiOffset & 0xFFFFFF));
-	if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Programming of FLASH_SPI_CMDQ_REG fails");
-		return STATUS_FAILURE;
-	}
-
-	/* msleep(1); */
-
-	do {
-		value = (FLASH_CMD_STATUS_REG_READ << 24);
-		if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Programing of FLASH_SPI_CMDQ_REG fails");
-			return STATUS_FAILURE;
-		}
-		/* __udelay(1); */
-		bytes = rdmalt(Adapter, FLASH_SPI_READQ_REG, &uiStatus, sizeof(uiStatus));
-		if (bytes < 0) {
-			uiStatus = bytes;
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Reading status of FLASH_SPI_READQ_REG fails");
-			return uiStatus;
-		}
-
-		iRetries--;
-		if (iRetries && ((iRetries % FLASH_PER_RETRIES_DELAY) == 0))
-			udelay(1000);
-
-	} while ((uiStatus & 0x1) && (iRetries > 0));
-
-	if (uiStatus & 0x1) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Flash Write fails even after checking status for 200 times.");
-		return STATUS_FAILURE;
-	}
-
-	return STATUS_SUCCESS;
-}
-/*
- * Procedure:	flashWriteStatus
- *
- * Description: Performs write to flash with write done status check
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- *		uiOffset    - Offset of the flash where data needs to be written to.
- *		pData	 - Address of the Data to be written.
- * Returns:
- *		OSAL_STATUS_CODE
- *
- */
-
-static int flashWriteStatus(struct bcm_mini_adapter *Adapter,
-			unsigned int uiOffset,
-			PVOID pData)
-{
-	unsigned int uiStatus = 0;
-	int  iRetries = MAX_FLASH_RETRIES * FLASH_PER_RETRIES_DELAY; /* 3 */
-	/* unsigned int uiReadBack = 0; */
-	unsigned int value;
-	unsigned int uiErasePattern[4] = {0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF};
-	int bytes;
-
-	/*
-	 * need not write 0xFFFFFFFF because write requires an erase and erase will
-	 * make whole sector 0xFFFFFFFF.
-	 */
-	if (!memcmp(pData, uiErasePattern, MAX_RW_SIZE))
-		return 0;
-
-	value = (FLASH_CMD_WRITE_ENABLE << 24);
-	if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Write Enable of FLASH_SPI_CMDQ_REG fails");
-		return STATUS_FAILURE;
-	}
-
-	if (wrm(Adapter, uiOffset, (PCHAR)pData, MAX_RW_SIZE) < 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Data write fails...");
-		return STATUS_FAILURE;
-	}
-	/* __udelay(1); */
-
-	do {
-		value = (FLASH_CMD_STATUS_REG_READ << 24);
-		if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Programing of FLASH_SPI_CMDQ_REG fails");
-			return STATUS_FAILURE;
-		}
-		/* __udelay(1); */
-		bytes = rdmalt(Adapter, FLASH_SPI_READQ_REG, &uiStatus, sizeof(uiStatus));
-		if (bytes < 0) {
-			uiStatus = bytes;
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Reading status of FLASH_SPI_READQ_REG fails");
-			return uiStatus;
-		}
-		iRetries--;
-		/* this will ensure that in there will be no changes in the current path.
-		 * currently one rdm/wrm takes 125 us.
-		 * Hence  125 *2  * FLASH_PER_RETRIES_DELAY  >3 ms(worst case delay)
-		 * Hence current implementation cycle will intoduce no delay in current path
-		 */
-		if (iRetries && ((iRetries % FLASH_PER_RETRIES_DELAY) == 0))
-			udelay(1000);
-
-	} while ((uiStatus & 0x1) && (iRetries > 0));
-
-	if (uiStatus & 0x1) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Flash Write fails even after checking status for 200 times.");
-		return STATUS_FAILURE;
-	}
-
-	return STATUS_SUCCESS;
-}
-
-/*
- * Procedure:	BcmRestoreBlockProtectStatus
- *
- * Description: Restores the original block protection status.
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- *		ulWriteStatus   -Original status
- * Returns:
- *		<VOID>
- *
- */
-
-static VOID BcmRestoreBlockProtectStatus(struct bcm_mini_adapter *Adapter, ULONG ulWriteStatus)
-{
-	unsigned int value;
-
-	value = (FLASH_CMD_WRITE_ENABLE << 24);
-	wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value));
-
-	udelay(20);
-	value = (FLASH_CMD_STATUS_REG_WRITE << 24) | (ulWriteStatus << 16);
-	wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value));
-	udelay(20);
-}
-
-/*
- * Procedure:	BcmFlashUnProtectBlock
- *
- * Description: UnProtects appropriate blocks for writing.
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- *		uiOffset   - Offset of the flash where data needs to be written to. This should be Sector aligned.
- * Returns:
- *		ULONG   - Status value before UnProtect.
- *
- */
-
-static ULONG BcmFlashUnProtectBlock(struct bcm_mini_adapter *Adapter, unsigned int uiOffset, unsigned int uiLength)
-{
-	ULONG ulStatus		= 0;
-	ULONG ulWriteStatus	= 0;
-	unsigned int value;
-
-	uiOffset = uiOffset&0x000FFFFF;
-	/*
-	 * Implemented only for 1MB Flash parts.
-	 */
-	if (FLASH_PART_SST25VF080B == Adapter->ulFlashID) {
-		/*
-		 * Get Current BP status.
-		 */
-		value = (FLASH_CMD_STATUS_REG_READ << 24);
-		wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value));
-		udelay(10);
-		/*
-		 * Read status will be WWXXYYZZ. We have to take only WW.
-		 */
-		rdmalt(Adapter, FLASH_SPI_READQ_REG, (PUINT)&ulStatus, sizeof(ulStatus));
-		ulStatus >>= 24;
-		ulWriteStatus = ulStatus;
-		/*
-		 * Bits [5-2] give current block level protection status.
-		 * Bit5: BP3 - DONT CARE
-		 * BP2-BP0: 0 - NO PROTECTION, 1 - UPPER 1/16, 2 - UPPER 1/8, 3 - UPPER 1/4
-		 *                4 - UPPER 1/2. 5 to 7 - ALL BLOCKS
-		 */
-
-		if (ulStatus) {
-			if ((uiOffset+uiLength) <= 0x80000) {
-				/*
-				 * Offset comes in lower half of 1MB. Protect the upper half.
-				 * Clear BP1 and BP0 and set BP2.
-				 */
-				ulWriteStatus |= (0x4<<2);
-				ulWriteStatus &= ~(0x3<<2);
-			} else if ((uiOffset + uiLength) <= 0xC0000) {
-				/*
-				 * Offset comes below Upper 1/4. Upper 1/4 can be protected.
-				 *  Clear BP2 and set BP1 and BP0.
-				 */
-				ulWriteStatus |= (0x3<<2);
-				ulWriteStatus &= ~(0x1<<4);
-			} else if ((uiOffset + uiLength) <= 0xE0000) {
-				/*
-				 * Offset comes below Upper 1/8. Upper 1/8 can be protected.
-				 * Clear BP2 and BP0  and set BP1
-				 */
-				ulWriteStatus |= (0x1<<3);
-				ulWriteStatus &= ~(0x5<<2);
-			} else if ((uiOffset + uiLength) <= 0xF0000) {
-				/*
-				 * Offset comes below Upper 1/16. Only upper 1/16 can be protected.
-				 * Set BP0 and Clear BP2,BP1.
-				 */
-				ulWriteStatus |= (0x1<<2);
-				ulWriteStatus &= ~(0x3<<3);
-			} else {
-				/*
-				 * Unblock all.
-				 * Clear BP2,BP1 and BP0.
-				 */
-				ulWriteStatus &= ~(0x7<<2);
-			}
-
-			value = (FLASH_CMD_WRITE_ENABLE << 24);
-			wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value));
-			udelay(20);
-			value = (FLASH_CMD_STATUS_REG_WRITE << 24) | (ulWriteStatus << 16);
-			wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value));
-			udelay(20);
-		}
-	}
-	return ulStatus;
-}
-
-static int bulk_read_complete_sector(struct bcm_mini_adapter *ad,
-				     UCHAR read_bk[],
-				     PCHAR tmpbuff,
-				     unsigned int offset,
-				     unsigned int partoff)
-{
-	unsigned int i;
-	int j;
-	int bulk_read_stat;
-	FP_FLASH_WRITE_STATUS writef =
-		ad->fpFlashWriteWithStatusCheck;
-
-	for (i = 0; i < ad->uiSectorSize; i += MAX_RW_SIZE) {
-		bulk_read_stat = BeceemFlashBulkRead(ad,
-						     (PUINT)read_bk,
-						     offset + i,
-						     MAX_RW_SIZE);
-
-		if (bulk_read_stat != STATUS_SUCCESS)
-			continue;
-
-		if (ad->ulFlashWriteSize == 1) {
-			for (j = 0; j < 16; j++) {
-				if ((read_bk[j] != tmpbuff[i + j]) &&
-				    (STATUS_SUCCESS != (*writef)(ad, partoff + i + j, &tmpbuff[i + j]))) {
-					return STATUS_FAILURE;
-				}
-			}
-		} else {
-			if ((memcmp(read_bk, &tmpbuff[i], MAX_RW_SIZE)) &&
-			    (STATUS_SUCCESS != (*writef)(ad, partoff + i, &tmpbuff[i]))) {
-				return STATUS_FAILURE;
-			}
-		}
-	}
-
-	return STATUS_SUCCESS;
-}
-
-/*
- * Procedure:	BeceemFlashBulkWrite
- *
- * Description: Performs write to the flash
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- * pBuffer - Data to be written.
- *		uiOffset   - Offset of the flash where data needs to be written to.
- *		uiNumBytes - Number of bytes to be written.
- *		bVerify    - read verify flag.
- * Returns:
- *		OSAL_STATUS_CODE
- *
- */
-
-static int BeceemFlashBulkWrite(struct bcm_mini_adapter *Adapter,
-				PUINT pBuffer,
-				unsigned int uiOffset,
-				unsigned int uiNumBytes,
-				bool bVerify)
-{
-	PCHAR pTempBuff			= NULL;
-	PUCHAR pcBuffer			= (PUCHAR)pBuffer;
-	unsigned int uiIndex			= 0;
-	unsigned int uiOffsetFromSectStart	= 0;
-	unsigned int uiSectAlignAddr		= 0;
-	unsigned int uiCurrSectOffsetAddr	= 0;
-	unsigned int uiSectBoundary		= 0;
-	unsigned int uiNumSectTobeRead		= 0;
-	UCHAR ucReadBk[16]		= {0};
-	ULONG ulStatus			= 0;
-	int Status			= STATUS_SUCCESS;
-	unsigned int uiTemp			= 0;
-	unsigned int index			= 0;
-	unsigned int uiPartOffset		= 0;
-
-	#if defined(BCM_SHM_INTERFACE) && !defined(FLASH_DIRECT_ACCESS)
-		Status = bcmflash_raw_write((uiOffset / FLASH_PART_SIZE), (uiOffset % FLASH_PART_SIZE), (unsigned char *)pBuffer, uiNumBytes);
-		return Status;
-	#endif
-
-	uiOffsetFromSectStart = uiOffset & ~(Adapter->uiSectorSize - 1);
-
-	/* Adding flash Base address
-	 * uiOffset = uiOffset + GetFlashBaseAddr(Adapter);
-	 */
-
-	uiSectAlignAddr	= uiOffset & ~(Adapter->uiSectorSize - 1);
-	uiCurrSectOffsetAddr = uiOffset & (Adapter->uiSectorSize - 1);
-	uiSectBoundary = uiSectAlignAddr + Adapter->uiSectorSize;
-
-	pTempBuff = kmalloc(Adapter->uiSectorSize, GFP_KERNEL);
-	if (!pTempBuff)
-		goto BeceemFlashBulkWrite_EXIT;
-	/*
-	 * check if the data to be written is overlapped across sectors
-	 */
-	if (uiOffset+uiNumBytes < uiSectBoundary) {
-		uiNumSectTobeRead = 1;
-	} else {
-		/* Number of sectors  = Last sector start address/First sector start address */
-		uiNumSectTobeRead =  (uiCurrSectOffsetAddr + uiNumBytes) / Adapter->uiSectorSize;
-		if ((uiCurrSectOffsetAddr + uiNumBytes)%Adapter->uiSectorSize)
-			uiNumSectTobeRead++;
-	}
-	/* Check whether Requested sector is writable or not in case of flash2x write. But if  write call is
-	 * for DSD calibration, allow it without checking of sector permission
-	 */
-
-	if (IsFlash2x(Adapter) && (Adapter->bAllDSDWriteAllow == false)) {
-		index = 0;
-		uiTemp = uiNumSectTobeRead;
-		while (uiTemp) {
-			if (IsOffsetWritable(Adapter, uiOffsetFromSectStart + index * Adapter->uiSectorSize) == false) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Sector Starting at offset <0X%X> is not writable",
-						(uiOffsetFromSectStart + index * Adapter->uiSectorSize));
-				Status = SECTOR_IS_NOT_WRITABLE;
-				goto BeceemFlashBulkWrite_EXIT;
-			}
-			uiTemp = uiTemp - 1;
-			index = index + 1;
-		}
-	}
-	Adapter->SelectedChip = RESET_CHIP_SELECT;
-	while (uiNumSectTobeRead) {
-		/* do_gettimeofday(&tv1);
-		 * BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "\nTime In start of write :%ld ms\n",(tv1.tv_sec *1000 + tv1.tv_usec /1000));
-		 */
-		uiPartOffset = (uiSectAlignAddr & (FLASH_PART_SIZE - 1)) + GetFlashBaseAddr(Adapter);
-
-		BcmDoChipSelect(Adapter, uiSectAlignAddr);
-
-		if (0 != BeceemFlashBulkRead(Adapter,
-						(PUINT)pTempBuff,
-						uiOffsetFromSectStart,
-						Adapter->uiSectorSize)) {
-			Status = -1;
-			goto BeceemFlashBulkWrite_EXIT;
-		}
-
-		/* do_gettimeofday(&tr);
-		 * BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Total time taken by Read :%ld ms\n", (tr.tv_sec *1000 + tr.tv_usec/1000) - (tv1.tv_sec *1000 + tv1.tv_usec/1000));
-		 */
-		ulStatus = BcmFlashUnProtectBlock(Adapter, uiSectAlignAddr, Adapter->uiSectorSize);
-
-		if (uiNumSectTobeRead > 1) {
-			memcpy(&pTempBuff[uiCurrSectOffsetAddr], pcBuffer, uiSectBoundary - (uiSectAlignAddr + uiCurrSectOffsetAddr));
-			pcBuffer += ((uiSectBoundary - (uiSectAlignAddr + uiCurrSectOffsetAddr)));
-			uiNumBytes -= (uiSectBoundary - (uiSectAlignAddr + uiCurrSectOffsetAddr));
-		} else {
-			memcpy(&pTempBuff[uiCurrSectOffsetAddr], pcBuffer, uiNumBytes);
-		}
-
-		if (IsFlash2x(Adapter))
-			SaveHeaderIfPresent(Adapter, (PUCHAR)pTempBuff, uiOffsetFromSectStart);
-
-		FlashSectorErase(Adapter, uiPartOffset, 1);
-		/* do_gettimeofday(&te);
-		 * BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Total time taken by Erase :%ld ms\n", (te.tv_sec *1000 + te.tv_usec/1000) - (tr.tv_sec *1000 + tr.tv_usec/1000));
-		 */
-		for (uiIndex = 0; uiIndex < Adapter->uiSectorSize; uiIndex += Adapter->ulFlashWriteSize) {
-			if (Adapter->device_removed) {
-				Status = -1;
-				goto BeceemFlashBulkWrite_EXIT;
-			}
-
-			if (STATUS_SUCCESS != (*Adapter->fpFlashWrite)(Adapter, uiPartOffset + uiIndex, (&pTempBuff[uiIndex]))) {
-				Status = -1;
-				goto BeceemFlashBulkWrite_EXIT;
-			}
-		}
-
-		/* do_gettimeofday(&tw);
-		 * BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Total time taken in Write  to Flash :%ld ms\n", (tw.tv_sec *1000 + tw.tv_usec/1000) - (te.tv_sec *1000 + te.tv_usec/1000));
-		 */
-
-		if (STATUS_FAILURE == bulk_read_complete_sector(Adapter,
-								ucReadBk,
-								pTempBuff,
-								uiOffsetFromSectStart,
-								uiPartOffset)) {
-			Status = STATUS_FAILURE;
-			goto BeceemFlashBulkWrite_EXIT;
-		}
-
-		/* do_gettimeofday(&twv);
-		 * BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Total time taken in Write  to Flash verification :%ld ms\n", (twv.tv_sec *1000 + twv.tv_usec/1000) - (tw.tv_sec *1000 + tw.tv_usec/1000));
-		 */
-		if (ulStatus) {
-			BcmRestoreBlockProtectStatus(Adapter, ulStatus);
-			ulStatus = 0;
-		}
-
-		uiCurrSectOffsetAddr = 0;
-		uiSectAlignAddr = uiSectBoundary;
-		uiSectBoundary += Adapter->uiSectorSize;
-		uiOffsetFromSectStart += Adapter->uiSectorSize;
-		uiNumSectTobeRead--;
-	}
-	/* do_gettimeofday(&tv2);
-	 * BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Time after Write :%ld ms\n",(tv2.tv_sec *1000 + tv2.tv_usec/1000));
-	 * BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Total time taken by in Write is :%ld ms\n", (tv2.tv_sec *1000 + tv2.tv_usec/1000) - (tv1.tv_sec *1000 + tv1.tv_usec/1000));
-	 *
-	 * Cleanup.
-	 */
-BeceemFlashBulkWrite_EXIT:
-	if (ulStatus)
-		BcmRestoreBlockProtectStatus(Adapter, ulStatus);
-
-	kfree(pTempBuff);
-
-	Adapter->SelectedChip = RESET_CHIP_SELECT;
-	return Status;
-}
-
-/*
- * Procedure:	BeceemFlashBulkWriteStatus
- *
- * Description: Writes to Flash. Checks the SPI status after each write.
- *
- * Arguments:
- *		Adapter		- ptr to Adapter object instance
- *		pBuffer		- Data to be written.
- *		uiOffset	- Offset of the flash where data needs to be written to.
- *		uiNumBytes	- Number of bytes to be written.
- *		bVerify		- read verify flag.
- * Returns:
- *		OSAL_STATUS_CODE
- *
- */
-
-static int BeceemFlashBulkWriteStatus(struct bcm_mini_adapter *Adapter,
-				PUINT pBuffer,
-				unsigned int uiOffset,
-				unsigned int uiNumBytes,
-				bool bVerify)
-{
-	PCHAR pTempBuff			= NULL;
-	PUCHAR pcBuffer			= (PUCHAR)pBuffer;
-	unsigned int uiIndex			= 0;
-	unsigned int uiOffsetFromSectStart	= 0;
-	unsigned int uiSectAlignAddr		= 0;
-	unsigned int uiCurrSectOffsetAddr	= 0;
-	unsigned int uiSectBoundary		= 0;
-	unsigned int uiNumSectTobeRead		= 0;
-	UCHAR ucReadBk[16]		= {0};
-	ULONG ulStatus			= 0;
-	unsigned int Status			= STATUS_SUCCESS;
-	unsigned int uiTemp			= 0;
-	unsigned int index			= 0;
-	unsigned int uiPartOffset		= 0;
-
-	uiOffsetFromSectStart = uiOffset & ~(Adapter->uiSectorSize - 1);
-
-	/* uiOffset += Adapter->ulFlashCalStart;
-	 * Adding flash Base address
-	 * uiOffset = uiOffset + GetFlashBaseAddr(Adapter);
-	 */
-	uiSectAlignAddr = uiOffset & ~(Adapter->uiSectorSize - 1);
-	uiCurrSectOffsetAddr = uiOffset & (Adapter->uiSectorSize - 1);
-	uiSectBoundary = uiSectAlignAddr + Adapter->uiSectorSize;
-
-	pTempBuff = kmalloc(Adapter->uiSectorSize, GFP_KERNEL);
-	if (!pTempBuff)
-		goto BeceemFlashBulkWriteStatus_EXIT;
-
-	/*
-	 * check if the data to be written is overlapped across sectors
-	 */
-	if (uiOffset+uiNumBytes < uiSectBoundary) {
-		uiNumSectTobeRead = 1;
-	} else {
-		/* Number of sectors  = Last sector start address/First sector start address */
-		uiNumSectTobeRead =  (uiCurrSectOffsetAddr + uiNumBytes) / Adapter->uiSectorSize;
-		if ((uiCurrSectOffsetAddr + uiNumBytes)%Adapter->uiSectorSize)
-			uiNumSectTobeRead++;
-	}
-
-	if (IsFlash2x(Adapter) && (Adapter->bAllDSDWriteAllow == false)) {
-		index = 0;
-		uiTemp = uiNumSectTobeRead;
-		while (uiTemp) {
-			if (IsOffsetWritable(Adapter, uiOffsetFromSectStart + index * Adapter->uiSectorSize) == false) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Sector Starting at offset <0X%x> is not writable",
-						(uiOffsetFromSectStart + index * Adapter->uiSectorSize));
-				Status = SECTOR_IS_NOT_WRITABLE;
-				goto BeceemFlashBulkWriteStatus_EXIT;
-			}
-			uiTemp = uiTemp - 1;
-			index = index + 1;
-		}
-	}
-
-	Adapter->SelectedChip = RESET_CHIP_SELECT;
-	while (uiNumSectTobeRead) {
-		uiPartOffset = (uiSectAlignAddr & (FLASH_PART_SIZE - 1)) + GetFlashBaseAddr(Adapter);
-
-		BcmDoChipSelect(Adapter, uiSectAlignAddr);
-		if (0 != BeceemFlashBulkRead(Adapter,
-						(PUINT)pTempBuff,
-						uiOffsetFromSectStart,
-						Adapter->uiSectorSize))	{
-			Status = -1;
-			goto BeceemFlashBulkWriteStatus_EXIT;
-		}
-
-		ulStatus = BcmFlashUnProtectBlock(Adapter, uiOffsetFromSectStart, Adapter->uiSectorSize);
-
-		if (uiNumSectTobeRead > 1) {
-			memcpy(&pTempBuff[uiCurrSectOffsetAddr], pcBuffer, uiSectBoundary - (uiSectAlignAddr + uiCurrSectOffsetAddr));
-			pcBuffer += ((uiSectBoundary - (uiSectAlignAddr + uiCurrSectOffsetAddr)));
-			uiNumBytes -= (uiSectBoundary - (uiSectAlignAddr + uiCurrSectOffsetAddr));
-		} else {
-			memcpy(&pTempBuff[uiCurrSectOffsetAddr], pcBuffer, uiNumBytes);
-		}
-
-		if (IsFlash2x(Adapter))
-			SaveHeaderIfPresent(Adapter, (PUCHAR)pTempBuff, uiOffsetFromSectStart);
-
-		FlashSectorErase(Adapter, uiPartOffset, 1);
-
-		for (uiIndex = 0; uiIndex < Adapter->uiSectorSize; uiIndex += Adapter->ulFlashWriteSize) {
-			if (Adapter->device_removed) {
-				Status = -1;
-				goto BeceemFlashBulkWriteStatus_EXIT;
-			}
-
-			if (STATUS_SUCCESS != (*Adapter->fpFlashWriteWithStatusCheck)(Adapter, uiPartOffset+uiIndex, &pTempBuff[uiIndex])) {
-				Status = -1;
-				goto BeceemFlashBulkWriteStatus_EXIT;
-			}
-		}
-
-		if (bVerify) {
-			for (uiIndex = 0; uiIndex < Adapter->uiSectorSize; uiIndex += MAX_RW_SIZE) {
-				if (STATUS_SUCCESS == BeceemFlashBulkRead(Adapter, (PUINT)ucReadBk, uiOffsetFromSectStart + uiIndex, MAX_RW_SIZE)) {
-					if (memcmp(ucReadBk, &pTempBuff[uiIndex], MAX_RW_SIZE)) {
-						Status = STATUS_FAILURE;
-						goto BeceemFlashBulkWriteStatus_EXIT;
-					}
-				}
-			}
-		}
-
-		if (ulStatus) {
-			BcmRestoreBlockProtectStatus(Adapter, ulStatus);
-			ulStatus = 0;
-		}
-
-		uiCurrSectOffsetAddr = 0;
-		uiSectAlignAddr = uiSectBoundary;
-		uiSectBoundary += Adapter->uiSectorSize;
-		uiOffsetFromSectStart += Adapter->uiSectorSize;
-		uiNumSectTobeRead--;
-	}
-/*
- * Cleanup.
- */
-BeceemFlashBulkWriteStatus_EXIT:
-	if (ulStatus)
-		BcmRestoreBlockProtectStatus(Adapter, ulStatus);
-
-	kfree(pTempBuff);
-	Adapter->SelectedChip = RESET_CHIP_SELECT;
-	return Status;
-}
-
-/*
- * Procedure:	PropagateCalParamsFromFlashToMemory
- *
- * Description: Dumps the calibration section of EEPROM to DDR.
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- * Returns:
- *		OSAL_STATUS_CODE
- *
- */
-
-int PropagateCalParamsFromFlashToMemory(struct bcm_mini_adapter *Adapter)
-{
-	PCHAR pBuff, pPtr;
-	unsigned int uiEepromSize = 0;
-	unsigned int uiBytesToCopy = 0;
-	/* unsigned int uiIndex = 0; */
-	unsigned int uiCalStartAddr = EEPROM_CALPARAM_START;
-	unsigned int uiMemoryLoc = EEPROM_CAL_DATA_INTERNAL_LOC;
-	unsigned int value;
-	int Status = 0;
-
-	/*
-	 * Write the signature first. This will ensure firmware does not access EEPROM.
-	 */
-	value = 0xbeadbead;
-	wrmalt(Adapter, EEPROM_CAL_DATA_INTERNAL_LOC - 4, &value, sizeof(value));
-	value = 0xbeadbead;
-	wrmalt(Adapter, EEPROM_CAL_DATA_INTERNAL_LOC - 8, &value, sizeof(value));
-
-	if (0 != BeceemNVMRead(Adapter, &uiEepromSize, EEPROM_SIZE_OFFSET, 4))
-		return -1;
-
-	uiEepromSize = ntohl(uiEepromSize);
-	uiEepromSize >>= 16;
-
-	/*
-	 * subtract the auto init section size
-	 */
-	uiEepromSize -= EEPROM_CALPARAM_START;
-
-	if (uiEepromSize > 1024 * 1024)
-		return -1;
-
-	pBuff = kmalloc(uiEepromSize, GFP_KERNEL);
-	if (pBuff == NULL)
-		return -ENOMEM;
-
-	if (0 != BeceemNVMRead(Adapter, (PUINT)pBuff, uiCalStartAddr, uiEepromSize)) {
-		kfree(pBuff);
-		return -1;
-	}
-
-	pPtr = pBuff;
-
-	uiBytesToCopy = MIN(BUFFER_4K, uiEepromSize);
-
-	while (uiBytesToCopy) {
-		Status = wrm(Adapter, uiMemoryLoc, (PCHAR)pPtr, uiBytesToCopy);
-		if (Status) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "wrm failed with status :%d", Status);
-			break;
-		}
-
-		pPtr += uiBytesToCopy;
-		uiEepromSize -= uiBytesToCopy;
-		uiMemoryLoc += uiBytesToCopy;
-		uiBytesToCopy = MIN(BUFFER_4K, uiEepromSize);
-	}
-
-	kfree(pBuff);
-	return Status;
-}
-
-/*
- * Procedure:	BeceemEEPROMReadBackandVerify
- *
- * Description: Read back the data written and verifies.
- *
- * Arguments:
- *		Adapter		- ptr to Adapter object instance
- *		pBuffer		- Data to be written.
- *		uiOffset	- Offset of the flash where data needs to be written to.
- *		uiNumBytes	- Number of bytes to be written.
- * Returns:
- *		OSAL_STATUS_CODE
- *
- */
-
-static int BeceemEEPROMReadBackandVerify(struct bcm_mini_adapter *Adapter,
-					PUINT pBuffer,
-					unsigned int uiOffset,
-					unsigned int uiNumBytes)
-{
-	unsigned int uiRdbk	= 0;
-	unsigned int uiIndex	= 0;
-	unsigned int uiData	= 0;
-	unsigned int auiData[4]	= {0};
-
-	while (uiNumBytes) {
-		if (Adapter->device_removed)
-			return -1;
-
-		if (uiNumBytes >= MAX_RW_SIZE) {
-			/* for the requests more than or equal to MAX_RW_SIZE bytes, use bulk read function to make the access faster. */
-			BeceemEEPROMBulkRead(Adapter, &auiData[0], uiOffset, MAX_RW_SIZE);
-
-			if (memcmp(&pBuffer[uiIndex], &auiData[0], MAX_RW_SIZE)) {
-				/* re-write */
-				BeceemEEPROMBulkWrite(Adapter, (PUCHAR)(pBuffer + uiIndex), uiOffset, MAX_RW_SIZE, false);
-				mdelay(3);
-				BeceemEEPROMBulkRead(Adapter, &auiData[0], uiOffset, MAX_RW_SIZE);
-
-				if (memcmp(&pBuffer[uiIndex], &auiData[0], MAX_RW_SIZE))
-					return -1;
-			}
-			uiOffset += MAX_RW_SIZE;
-			uiNumBytes -= MAX_RW_SIZE;
-			uiIndex += 4;
-		} else if (uiNumBytes >= 4) {
-			BeceemEEPROMBulkRead(Adapter, &uiData, uiOffset, 4);
-			if (uiData != pBuffer[uiIndex]) {
-				/* re-write */
-				BeceemEEPROMBulkWrite(Adapter, (PUCHAR)(pBuffer + uiIndex), uiOffset, 4, false);
-				mdelay(3);
-				BeceemEEPROMBulkRead(Adapter, &uiData, uiOffset, 4);
-				if (uiData != pBuffer[uiIndex])
-					return -1;
-			}
-			uiOffset += 4;
-			uiNumBytes -= 4;
-			uiIndex++;
-		} else {
-			/* Handle the reads less than 4 bytes... */
-			uiData = 0;
-			memcpy(&uiData, ((PUCHAR)pBuffer) + (uiIndex * sizeof(unsigned int)), uiNumBytes);
-			BeceemEEPROMBulkRead(Adapter, &uiRdbk, uiOffset, 4);
-
-			if (memcmp(&uiData, &uiRdbk, uiNumBytes))
-				return -1;
-
-			uiNumBytes = 0;
-		}
-	}
-
-	return 0;
-}
-
-static VOID BcmSwapWord(unsigned int *ptr1)
-{
-	unsigned int tempval = (unsigned int)*ptr1;
-	char *ptr2 = (char *)&tempval;
-	char *ptr = (char *)ptr1;
-
-	ptr[0] = ptr2[3];
-	ptr[1] = ptr2[2];
-	ptr[2] = ptr2[1];
-	ptr[3] = ptr2[0];
-}
-
-/*
- * Procedure:	BeceemEEPROMWritePage
- *
- * Description: Performs page write (16bytes) to the EEPROM
- *
- * Arguments:
- *		Adapter		- ptr to Adapter object instance
- *		uiData		- Data to be written.
- *		uiOffset	- Offset of the EEPROM where data needs to be written to.
- * Returns:
- *		OSAL_STATUS_CODE
- *
- */
-
-static int BeceemEEPROMWritePage(struct bcm_mini_adapter *Adapter, unsigned int uiData[], unsigned int uiOffset)
-{
-	unsigned int uiRetries = MAX_EEPROM_RETRIES * RETRIES_PER_DELAY;
-	unsigned int uiStatus = 0;
-	UCHAR uiEpromStatus = 0;
-	unsigned int value = 0;
-
-	/* Flush the Write/Read/Cmd queues. */
-	value = (EEPROM_WRITE_QUEUE_FLUSH | EEPROM_CMD_QUEUE_FLUSH | EEPROM_READ_QUEUE_FLUSH);
-	wrmalt(Adapter, SPI_FLUSH_REG, &value, sizeof(value));
-	value = 0;
-	wrmalt(Adapter, SPI_FLUSH_REG, &value, sizeof(value));
-
-	/* Clear the Empty/Avail/Full bits.  After this it has been confirmed
-	 * that the bit was cleared by reading back the register. See NOTE below.
-	 * We also clear the Read queues as we do a EEPROM status register read
-	 * later.
-	 */
-	value = (EEPROM_WRITE_QUEUE_EMPTY | EEPROM_WRITE_QUEUE_AVAIL | EEPROM_WRITE_QUEUE_FULL | EEPROM_READ_DATA_AVAIL | EEPROM_READ_DATA_FULL);
-	wrmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
-
-	/* Enable write */
-	value = EEPROM_WRITE_ENABLE;
-	wrmalt(Adapter, EEPROM_CMDQ_SPI_REG, &value, sizeof(value));
-
-	/* We can write back to back 8bits * 16 into the queue and as we have
-	 * checked for the queue to be empty we can write in a burst.
-	 */
-
-	value = uiData[0];
-	BcmSwapWord(&value);
-	wrm(Adapter, EEPROM_WRITE_DATAQ_REG, (PUCHAR)&value, 4);
-
-	value = uiData[1];
-	BcmSwapWord(&value);
-	wrm(Adapter, EEPROM_WRITE_DATAQ_REG, (PUCHAR)&value, 4);
-
-	value = uiData[2];
-	BcmSwapWord(&value);
-	wrm(Adapter, EEPROM_WRITE_DATAQ_REG, (PUCHAR)&value, 4);
-
-	value = uiData[3];
-	BcmSwapWord(&value);
-	wrm(Adapter, EEPROM_WRITE_DATAQ_REG, (PUCHAR)&value, 4);
-
-	/* NOTE : After this write, on readback of EEPROM_SPI_Q_STATUS1_REG
-	 * shows that we see 7 for the EEPROM data write.  Which means that
-	 * queue got full, also space is available as well as the queue is empty.
-	 * This may happen in sequence.
-	 */
-	value =  EEPROM_16_BYTE_PAGE_WRITE | uiOffset;
-	wrmalt(Adapter, EEPROM_CMDQ_SPI_REG, &value, sizeof(value));
-
-	/* Ideally we should loop here without tries and eventually succeed.
-	 * What we are checking if the previous write has completed, and this
-	 * may take time. We should wait till the Empty bit is set.
-	 */
-	uiStatus = 0;
-	rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &uiStatus, sizeof(uiStatus));
-	while ((uiStatus & EEPROM_WRITE_QUEUE_EMPTY) == 0) {
-		uiRetries--;
-		if (uiRetries == 0) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "0x0f003004 = %x, %d retries failed.\n", uiStatus, MAX_EEPROM_RETRIES * RETRIES_PER_DELAY);
-			return STATUS_FAILURE;
-		}
-
-		if (!(uiRetries%RETRIES_PER_DELAY))
-			udelay(1000);
-
-		uiStatus = 0;
-		rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &uiStatus, sizeof(uiStatus));
-		if (Adapter->device_removed == TRUE) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Modem got removed hence exiting from loop....");
-			return -ENODEV;
-		}
-	}
-
-	if (uiRetries != 0) {
-		/* Clear the ones that are set - either, Empty/Full/Avail bits */
-		value = (uiStatus & (EEPROM_WRITE_QUEUE_EMPTY | EEPROM_WRITE_QUEUE_AVAIL | EEPROM_WRITE_QUEUE_FULL));
-		wrmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
-	}
-
-	/* Here we should check if the EEPROM status register is correct before
-	 * proceeding. Bit 0 in the EEPROM Status register should be 0 before
-	 * we proceed further.  A 1 at Bit 0 indicates that the EEPROM is busy
-	 * with the previous write. Note also that issuing this read finally
-	 * means the previous write to the EEPROM has completed.
-	 */
-	uiRetries = MAX_EEPROM_RETRIES * RETRIES_PER_DELAY;
-	uiEpromStatus = 0;
-	while (uiRetries != 0) {
-		uiEpromStatus = ReadEEPROMStatusRegister(Adapter);
-		if (Adapter->device_removed == TRUE) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Modem has got removed hence exiting from loop...");
-			return -ENODEV;
-		}
-		if ((EEPROM_STATUS_REG_WRITE_BUSY & uiEpromStatus) == 0) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "EEPROM status register = %x tries = %d\n", uiEpromStatus, (MAX_EEPROM_RETRIES * RETRIES_PER_DELAY - uiRetries));
-			return STATUS_SUCCESS;
-		}
-		uiRetries--;
-		if (uiRetries == 0) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "0x0f003004 = %x, for EEPROM status read %d retries failed.\n", uiEpromStatus, MAX_EEPROM_RETRIES * RETRIES_PER_DELAY);
-			return STATUS_FAILURE;
-		}
-		uiEpromStatus = 0;
-		if (!(uiRetries%RETRIES_PER_DELAY))
-			udelay(1000);
-	}
-
-	return STATUS_SUCCESS;
-} /* BeceemEEPROMWritePage */
-
-/*
- * Procedure:	BeceemEEPROMBulkWrite
- *
- * Description: Performs write to the EEPROM
- *
- * Arguments:
- *		Adapter		- ptr to Adapter object instance
- *		pBuffer		- Data to be written.
- *		uiOffset	- Offset of the EEPROM where data needs to be written to.
- *		uiNumBytes	- Number of bytes to be written.
- *		bVerify		- read verify flag.
- * Returns:
- *		OSAL_STATUS_CODE
- *
- */
-
-int BeceemEEPROMBulkWrite(struct bcm_mini_adapter *Adapter,
-			PUCHAR pBuffer,
-			unsigned int uiOffset,
-			unsigned int uiNumBytes,
-			bool bVerify)
-{
-	unsigned int uiBytesToCopy	= uiNumBytes;
-	/* unsigned int uiRdbk		= 0; */
-	unsigned int uiData[4]		= {0};
-	unsigned int uiIndex		= 0;
-	unsigned int uiTempOffset	= 0;
-	unsigned int uiExtraBytes	= 0;
-	/* PUINT puiBuffer	= (PUINT)pBuffer;
-	 * int value;
-	 */
-
-	if (uiOffset % MAX_RW_SIZE && uiBytesToCopy) {
-		uiTempOffset = uiOffset - (uiOffset % MAX_RW_SIZE);
-		uiExtraBytes = uiOffset - uiTempOffset;
-
-		BeceemEEPROMBulkRead(Adapter, &uiData[0], uiTempOffset, MAX_RW_SIZE);
-
-		if (uiBytesToCopy >= (16 - uiExtraBytes)) {
-			memcpy((((PUCHAR)&uiData[0]) + uiExtraBytes), pBuffer, MAX_RW_SIZE - uiExtraBytes);
-
-			if (STATUS_FAILURE == BeceemEEPROMWritePage(Adapter, uiData, uiTempOffset))
-				return STATUS_FAILURE;
-
-			uiBytesToCopy -= (MAX_RW_SIZE - uiExtraBytes);
-			uiIndex += (MAX_RW_SIZE - uiExtraBytes);
-			uiOffset += (MAX_RW_SIZE - uiExtraBytes);
-		} else {
-			memcpy((((PUCHAR)&uiData[0]) + uiExtraBytes), pBuffer, uiBytesToCopy);
-
-			if (STATUS_FAILURE == BeceemEEPROMWritePage(Adapter, uiData, uiTempOffset))
-				return STATUS_FAILURE;
-
-			uiIndex += uiBytesToCopy;
-			uiOffset += uiBytesToCopy;
-			uiBytesToCopy = 0;
-		}
-	}
-
-	while (uiBytesToCopy) {
-		if (Adapter->device_removed)
-			return -1;
-
-		if (uiBytesToCopy >= MAX_RW_SIZE) {
-			if (STATUS_FAILURE == BeceemEEPROMWritePage(Adapter, (PUINT) &pBuffer[uiIndex], uiOffset))
-				return STATUS_FAILURE;
-
-			uiIndex += MAX_RW_SIZE;
-			uiOffset += MAX_RW_SIZE;
-			uiBytesToCopy -= MAX_RW_SIZE;
-		} else {
-			/*
-			 * To program non 16byte aligned data, read 16byte and then update.
-			 */
-			BeceemEEPROMBulkRead(Adapter, &uiData[0], uiOffset, 16);
-			memcpy(&uiData[0], pBuffer + uiIndex, uiBytesToCopy);
-
-			if (STATUS_FAILURE == BeceemEEPROMWritePage(Adapter, uiData, uiOffset))
-				return STATUS_FAILURE;
-
-			uiBytesToCopy = 0;
-		}
-	}
-
-	return 0;
-}
-
-/*
- * Procedure:	BeceemNVMRead
- *
- * Description: Reads n number of bytes from NVM.
- *
- * Arguments:
- *		Adapter      - ptr to Adapter object instance
- *		pBuffer       - Buffer to store the data read from NVM
- *		uiOffset       - Offset of NVM from where data should be read
- *		uiNumBytes - Number of bytes to be read from the NVM.
- *
- * Returns:
- *		OSAL_STATUS_SUCCESS - if NVM read is successful.
- *		<FAILURE>			- if failed.
- */
-
-int BeceemNVMRead(struct bcm_mini_adapter *Adapter,
-		PUINT pBuffer,
-		unsigned int uiOffset,
-		unsigned int uiNumBytes)
-{
-	int Status = 0;
-
-	#if !defined(BCM_SHM_INTERFACE) || defined(FLASH_DIRECT_ACCESS)
-		unsigned int uiTemp = 0, value;
-	#endif
-
-	if (Adapter->eNVMType == NVM_FLASH) {
-		if (Adapter->bFlashRawRead == false) {
-			if (IsSectionExistInVendorInfo(Adapter, Adapter->eActiveDSD))
-				return vendorextnReadSection(Adapter, (PUCHAR)pBuffer, Adapter->eActiveDSD, uiOffset, uiNumBytes);
-
-			uiOffset = uiOffset + Adapter->ulFlashCalStart;
-		}
-
-		#if defined(BCM_SHM_INTERFACE) && !defined(FLASH_DIRECT_ACCESS)
-			Status = bcmflash_raw_read((uiOffset / FLASH_PART_SIZE), (uiOffset % FLASH_PART_SIZE), (unsigned char *)pBuffer, uiNumBytes);
-		#else
-			rdmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
-			value = 0;
-			wrmalt(Adapter, 0x0f000C80, &value, sizeof(value));
-			Status = BeceemFlashBulkRead(Adapter,
-						pBuffer,
-						uiOffset,
-						uiNumBytes);
-			wrmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
-		#endif
-	} else if (Adapter->eNVMType == NVM_EEPROM) {
-		Status = BeceemEEPROMBulkRead(Adapter,
-					pBuffer,
-					uiOffset,
-					uiNumBytes);
-	} else {
-		Status = -1;
-	}
-
-	return Status;
-}
-
-/*
- * Procedure:	BeceemNVMWrite
- *
- * Description: Writes n number of bytes to NVM.
- *
- * Arguments:
- *		Adapter      - ptr to Adapter object instance
- *		pBuffer       - Buffer contains the data to be written.
- *		uiOffset       - Offset of NVM where data to be written to.
- *		uiNumBytes - Number of bytes to be written..
- *
- * Returns:
- *		OSAL_STATUS_SUCCESS - if NVM write is successful.
- *		<FAILURE>			- if failed.
- */
-
-int BeceemNVMWrite(struct bcm_mini_adapter *Adapter,
-		PUINT pBuffer,
-		unsigned int uiOffset,
-		unsigned int uiNumBytes,
-		bool bVerify)
-{
-	int Status = 0;
-	unsigned int uiTemp = 0;
-	unsigned int uiMemoryLoc = EEPROM_CAL_DATA_INTERNAL_LOC;
-	unsigned int uiIndex = 0;
-
-	#if !defined(BCM_SHM_INTERFACE) || defined(FLASH_DIRECT_ACCESS)
-		unsigned int value;
-	#endif
-
-	unsigned int uiFlashOffset = 0;
-
-	if (Adapter->eNVMType == NVM_FLASH) {
-		if (IsSectionExistInVendorInfo(Adapter, Adapter->eActiveDSD))
-			Status = vendorextnWriteSection(Adapter, (PUCHAR)pBuffer, Adapter->eActiveDSD, uiOffset, uiNumBytes, bVerify);
-		else {
-			uiFlashOffset = uiOffset + Adapter->ulFlashCalStart;
-
-			#if defined(BCM_SHM_INTERFACE) && !defined(FLASH_DIRECT_ACCESS)
-				Status = bcmflash_raw_write((uiFlashOffset / FLASH_PART_SIZE), (uiFlashOffset % FLASH_PART_SIZE), (unsigned char *)pBuffer, uiNumBytes);
-			#else
-				rdmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
-				value = 0;
-				wrmalt(Adapter, 0x0f000C80, &value, sizeof(value));
-
-				if (Adapter->bStatusWrite == TRUE)
-					Status = BeceemFlashBulkWriteStatus(Adapter,
-									pBuffer,
-									uiFlashOffset,
-									uiNumBytes ,
-									bVerify);
-				else
-
-					Status = BeceemFlashBulkWrite(Adapter,
-								pBuffer,
-								uiFlashOffset,
-								uiNumBytes,
-								bVerify);
-			#endif
-		}
-
-		if (uiOffset >= EEPROM_CALPARAM_START) {
-			uiMemoryLoc += (uiOffset - EEPROM_CALPARAM_START);
-			while (uiNumBytes) {
-				if (uiNumBytes > BUFFER_4K) {
-					wrm(Adapter, (uiMemoryLoc+uiIndex), (PCHAR)(pBuffer + (uiIndex / 4)), BUFFER_4K);
-					uiNumBytes -= BUFFER_4K;
-					uiIndex += BUFFER_4K;
-				} else {
-					wrm(Adapter, uiMemoryLoc+uiIndex, (PCHAR)(pBuffer + (uiIndex / 4)), uiNumBytes);
-					uiNumBytes = 0;
-					break;
-				}
-			}
-		} else {
-			if ((uiOffset + uiNumBytes) > EEPROM_CALPARAM_START) {
-				ULONG ulBytesTobeSkipped = 0;
-				PUCHAR pcBuffer = (PUCHAR)pBuffer; /* char pointer to take care of odd byte cases. */
-
-				uiNumBytes -= (EEPROM_CALPARAM_START - uiOffset);
-				ulBytesTobeSkipped += (EEPROM_CALPARAM_START - uiOffset);
-				uiOffset += (EEPROM_CALPARAM_START - uiOffset);
-				while (uiNumBytes) {
-					if (uiNumBytes > BUFFER_4K) {
-						wrm(Adapter, uiMemoryLoc + uiIndex, (PCHAR)&pcBuffer[ulBytesTobeSkipped + uiIndex], BUFFER_4K);
-						uiNumBytes -= BUFFER_4K;
-						uiIndex += BUFFER_4K;
-					} else {
-						wrm(Adapter, uiMemoryLoc + uiIndex, (PCHAR)&pcBuffer[ulBytesTobeSkipped + uiIndex], uiNumBytes);
-						uiNumBytes = 0;
-						break;
-					}
-				}
-			}
-		}
-		/* restore the values. */
-		wrmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
-	} else if (Adapter->eNVMType == NVM_EEPROM) {
-		Status = BeceemEEPROMBulkWrite(Adapter,
-					(PUCHAR)pBuffer,
-					uiOffset,
-					uiNumBytes,
-					bVerify);
-		if (bVerify)
-			Status = BeceemEEPROMReadBackandVerify(Adapter, (PUINT)pBuffer, uiOffset, uiNumBytes);
-	} else {
-		Status = -1;
-	}
-	return Status;
-}
-
-/*
- * Procedure:	BcmUpdateSectorSize
- *
- * Description: Updates the sector size to FLASH.
- *
- * Arguments:
- *		Adapter       - ptr to Adapter object instance
- *          uiSectorSize - sector size
- *
- * Returns:
- *		OSAL_STATUS_SUCCESS - if NVM write is successful.
- *		<FAILURE>			- if failed.
- */
-
-int BcmUpdateSectorSize(struct bcm_mini_adapter *Adapter, unsigned int uiSectorSize)
-{
-	int Status = -1;
-	struct bcm_flash_cs_info sFlashCsInfo = {0};
-	unsigned int uiTemp = 0;
-	unsigned int uiSectorSig = 0;
-	unsigned int uiCurrentSectorSize = 0;
-	unsigned int value;
-
-	rdmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
-	value = 0;
-	wrmalt(Adapter, 0x0f000C80, &value, sizeof(value));
-
-	/*
-	 * Before updating the sector size in the reserved area, check if already present.
-	 */
-	BeceemFlashBulkRead(Adapter, (PUINT)&sFlashCsInfo, Adapter->ulFlashControlSectionStart, sizeof(sFlashCsInfo));
-	uiSectorSig = ntohl(sFlashCsInfo.FlashSectorSizeSig);
-	uiCurrentSectorSize = ntohl(sFlashCsInfo.FlashSectorSize);
-
-	if (uiSectorSig == FLASH_SECTOR_SIZE_SIG) {
-		if ((uiCurrentSectorSize <= MAX_SECTOR_SIZE) && (uiCurrentSectorSize >= MIN_SECTOR_SIZE)) {
-			if (uiSectorSize == uiCurrentSectorSize) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Provided sector size is same as programmed in Flash");
-				Status = STATUS_SUCCESS;
-				goto Restore;
-			}
-		}
-	}
-
-	if ((uiSectorSize <= MAX_SECTOR_SIZE) && (uiSectorSize >= MIN_SECTOR_SIZE)) {
-		sFlashCsInfo.FlashSectorSize = htonl(uiSectorSize);
-		sFlashCsInfo.FlashSectorSizeSig = htonl(FLASH_SECTOR_SIZE_SIG);
-
-		Status = BeceemFlashBulkWrite(Adapter,
-					(PUINT)&sFlashCsInfo,
-					Adapter->ulFlashControlSectionStart,
-					sizeof(sFlashCsInfo),
-					TRUE);
-	}
-
-Restore:
-	/* restore the values. */
-	wrmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
-
-	return Status;
-}
-
-/*
- * Procedure:	BcmGetFlashSectorSize
- *
- * Description: Finds the sector size of the FLASH.
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- *
- * Returns:
- *		unsigned int - sector size.
- *
- */
-
-static unsigned int BcmGetFlashSectorSize(struct bcm_mini_adapter *Adapter, unsigned int FlashSectorSizeSig, unsigned int FlashSectorSize)
-{
-	unsigned int uiSectorSize = 0;
-	unsigned int uiSectorSig = 0;
-
-	if (Adapter->bSectorSizeOverride &&
-		(Adapter->uiSectorSizeInCFG <= MAX_SECTOR_SIZE &&
-			Adapter->uiSectorSizeInCFG >= MIN_SECTOR_SIZE)) {
-		Adapter->uiSectorSize = Adapter->uiSectorSizeInCFG;
-	} else {
-		uiSectorSig = FlashSectorSizeSig;
-
-		if (uiSectorSig == FLASH_SECTOR_SIZE_SIG) {
-			uiSectorSize = FlashSectorSize;
-			/*
-			 * If the sector size stored in the FLASH makes sense then use it.
-			 */
-			if (uiSectorSize <= MAX_SECTOR_SIZE && uiSectorSize >= MIN_SECTOR_SIZE) {
-				Adapter->uiSectorSize = uiSectorSize;
-			} else if (Adapter->uiSectorSizeInCFG <= MAX_SECTOR_SIZE &&
-				Adapter->uiSectorSizeInCFG >= MIN_SECTOR_SIZE) {
-				/* No valid size in FLASH, check if Config file has it. */
-				Adapter->uiSectorSize = Adapter->uiSectorSizeInCFG;
-			} else {
-				/* Init to Default, if none of the above works. */
-				Adapter->uiSectorSize = DEFAULT_SECTOR_SIZE;
-			}
-		} else {
-			if (Adapter->uiSectorSizeInCFG <= MAX_SECTOR_SIZE &&
-				Adapter->uiSectorSizeInCFG >= MIN_SECTOR_SIZE)
-				Adapter->uiSectorSize = Adapter->uiSectorSizeInCFG;
-			else
-				Adapter->uiSectorSize = DEFAULT_SECTOR_SIZE;
-		}
-	}
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Sector size  :%x\n", Adapter->uiSectorSize);
-
-	return Adapter->uiSectorSize;
-}
-
-/*
- * Procedure:	BcmInitEEPROMQueues
- *
- * Description: Initialization of EEPROM queues.
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- *
- * Returns:
- *		<OSAL_STATUS_CODE>
- */
-
-static int BcmInitEEPROMQueues(struct bcm_mini_adapter *Adapter)
-{
-	unsigned int value = 0;
-	/* CHIP Bug : Clear the Avail bits on the Read queue. The default
-	 * value on this register is supposed to be 0x00001102.
-	 * But we get 0x00001122.
-	 */
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Fixing reset value on 0x0f003004 register\n");
-	value = EEPROM_READ_DATA_AVAIL;
-	wrmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
-
-	/* Flush the all the EEPROM queues. */
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, " Flushing the queues\n");
-	value = EEPROM_ALL_QUEUE_FLUSH;
-	wrmalt(Adapter, SPI_FLUSH_REG, &value, sizeof(value));
-
-	value = 0;
-	wrmalt(Adapter, SPI_FLUSH_REG, &value, sizeof(value));
-
-	/* Read the EEPROM Status Register. Just to see, no real purpose. */
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "EEPROM Status register value = %x\n", ReadEEPROMStatusRegister(Adapter));
-
-	return STATUS_SUCCESS;
-} /* BcmInitEEPROMQueues() */
-
-/*
- * Procedure:	BcmInitNVM
- *
- * Description: Initialization of NVM, EEPROM size,FLASH size, sector size etc.
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- *
- * Returns:
- *		<OSAL_STATUS_CODE>
- */
-
-int BcmInitNVM(struct bcm_mini_adapter *ps_adapter)
-{
-	BcmValidateNvmType(ps_adapter);
-	BcmInitEEPROMQueues(ps_adapter);
-
-	if (ps_adapter->eNVMType == NVM_AUTODETECT) {
-		ps_adapter->eNVMType = BcmGetNvmType(ps_adapter);
-		if (ps_adapter->eNVMType == NVM_UNKNOWN)
-			BCM_DEBUG_PRINT(ps_adapter, DBG_TYPE_PRINTK, 0, 0, "NVM Type is unknown!!\n");
-	} else if (ps_adapter->eNVMType == NVM_FLASH) {
-		BcmGetFlashCSInfo(ps_adapter);
-	}
-
-	BcmGetNvmSize(ps_adapter);
-
-	return STATUS_SUCCESS;
-}
-
-/* BcmGetNvmSize : set the EEPROM or flash size in Adapter.
- *
- * Input Parameter:
- *		Adapter data structure
- * Return Value :
- *		0. means success;
- */
-
-static int BcmGetNvmSize(struct bcm_mini_adapter *Adapter)
-{
-	if (Adapter->eNVMType == NVM_EEPROM)
-		Adapter->uiNVMDSDSize = BcmGetEEPROMSize(Adapter);
-	else if (Adapter->eNVMType == NVM_FLASH)
-		Adapter->uiNVMDSDSize = BcmGetFlashSize(Adapter);
-
-	return 0;
-}
-
-/*
- * Procedure:	BcmValidateNvm
- *
- * Description: Validates the NVM Type option selected against the device
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- *
- * Returns:
- *		<VOID>
- */
-
-static VOID BcmValidateNvmType(struct bcm_mini_adapter *Adapter)
-{
-	/*
-	 * if forcing the FLASH through CFG file, we should ensure device really has a FLASH.
-	 * Accessing the FLASH address without the FLASH being present can cause hang/freeze etc.
-	 * So if NVM_FLASH is selected for older chipsets, change it to AUTODETECT where EEPROM is 1st choice.
-	 */
-
-	if (Adapter->eNVMType == NVM_FLASH &&
-		Adapter->chip_id < 0xBECE3300)
-		Adapter->eNVMType = NVM_AUTODETECT;
-}
-
-/*
- * Procedure:	BcmReadFlashRDID
- *
- * Description: Reads ID from Serial Flash
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- *
- * Returns:
- *		Flash ID
- */
-
-static ULONG BcmReadFlashRDID(struct bcm_mini_adapter *Adapter)
-{
-	ULONG ulRDID = 0;
-	unsigned int value;
-
-	/*
-	 * Read ID Instruction.
-	 */
-	value = (FLASH_CMD_READ_ID << 24);
-	wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value));
-
-	/* Delay */
-	udelay(10);
-
-	/*
-	 * Read SPI READQ REG. The output will be WWXXYYZZ.
-	 * The ID is 3Bytes long and is WWXXYY. ZZ needs to be Ignored.
-	 */
-	rdmalt(Adapter, FLASH_SPI_READQ_REG, (PUINT)&ulRDID, sizeof(ulRDID));
-
-	return ulRDID >> 8;
-}
-
-int BcmAllocFlashCSStructure(struct bcm_mini_adapter *psAdapter)
-{
-	if (!psAdapter) {
-		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_PRINTK, 0, 0, "Adapter structure point is NULL");
-		return -EINVAL;
-	}
-	psAdapter->psFlashCSInfo = kzalloc(sizeof(struct bcm_flash_cs_info), GFP_KERNEL);
-	if (psAdapter->psFlashCSInfo == NULL) {
-		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_PRINTK, 0, 0, "Can't Allocate memory for Flash 1.x");
-		return -ENOMEM;
-	}
-
-	psAdapter->psFlash2xCSInfo = kzalloc(sizeof(struct bcm_flash2x_cs_info), GFP_KERNEL);
-	if (!psAdapter->psFlash2xCSInfo) {
-		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_PRINTK, 0, 0, "Can't Allocate memory for Flash 2.x");
-		kfree(psAdapter->psFlashCSInfo);
-		return -ENOMEM;
-	}
-
-	psAdapter->psFlash2xVendorInfo = kzalloc(sizeof(struct bcm_flash2x_vendor_info), GFP_KERNEL);
-	if (!psAdapter->psFlash2xVendorInfo) {
-		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_PRINTK, 0, 0, "Can't Allocate Vendor Info Memory for Flash 2.x");
-		kfree(psAdapter->psFlashCSInfo);
-		kfree(psAdapter->psFlash2xCSInfo);
-		return -ENOMEM;
-	}
-
-	return STATUS_SUCCESS;
-}
-
-int BcmDeAllocFlashCSStructure(struct bcm_mini_adapter *psAdapter)
-{
-	if (!psAdapter) {
-		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_PRINTK, 0, 0, "Adapter structure point is NULL");
-		return -EINVAL;
-	}
-	kfree(psAdapter->psFlashCSInfo);
-	kfree(psAdapter->psFlash2xCSInfo);
-	kfree(psAdapter->psFlash2xVendorInfo);
-	return STATUS_SUCCESS;
-}
-
-static int BcmDumpFlash2XCSStructure(struct bcm_flash2x_cs_info *psFlash2xCSInfo, struct bcm_mini_adapter *Adapter)
-{
-	unsigned int Index = 0;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "**********************FLASH2X CS Structure *******************");
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Signature is  :%x", (psFlash2xCSInfo->MagicNumber));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Flash Major Version :%d", MAJOR_VERSION(psFlash2xCSInfo->FlashLayoutVersion));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Flash Minor Version :%d", MINOR_VERSION(psFlash2xCSInfo->FlashLayoutVersion));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, " ISOImageMajorVersion:0x%x", (psFlash2xCSInfo->ISOImageVersion));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "SCSIFirmwareMajorVersion :0x%x", (psFlash2xCSInfo->SCSIFirmwareVersion));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForPart1ISOImage :0x%x", (psFlash2xCSInfo->OffsetFromZeroForPart1ISOImage));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForScsiFirmware :0x%x", (psFlash2xCSInfo->OffsetFromZeroForScsiFirmware));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "SizeOfScsiFirmware  :0x%x", (psFlash2xCSInfo->SizeOfScsiFirmware));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForPart2ISOImage :0x%x", (psFlash2xCSInfo->OffsetFromZeroForPart2ISOImage));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForDSDStart :0x%x", (psFlash2xCSInfo->OffsetFromZeroForDSDStart));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForDSDEnd :0x%x", (psFlash2xCSInfo->OffsetFromZeroForDSDEnd));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForVSAStart :0x%x", (psFlash2xCSInfo->OffsetFromZeroForVSAStart));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForVSAEnd :0x%x", (psFlash2xCSInfo->OffsetFromZeroForVSAEnd));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForControlSectionStart :0x%x", (psFlash2xCSInfo->OffsetFromZeroForControlSectionStart));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForControlSectionData :0x%x", (psFlash2xCSInfo->OffsetFromZeroForControlSectionData));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "CDLessInactivityTimeout :0x%x", (psFlash2xCSInfo->CDLessInactivityTimeout));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "NewImageSignature :0x%x", (psFlash2xCSInfo->NewImageSignature));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "FlashSectorSizeSig :0x%x", (psFlash2xCSInfo->FlashSectorSizeSig));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "FlashSectorSize :0x%x", (psFlash2xCSInfo->FlashSectorSize));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "FlashWriteSupportSize :0x%x", (psFlash2xCSInfo->FlashWriteSupportSize));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "TotalFlashSize :0x%X", (psFlash2xCSInfo->TotalFlashSize));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "FlashBaseAddr :0x%x", (psFlash2xCSInfo->FlashBaseAddr));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "FlashPartMaxSize :0x%x", (psFlash2xCSInfo->FlashPartMaxSize));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "IsCDLessDeviceBootSig :0x%x", (psFlash2xCSInfo->IsCDLessDeviceBootSig));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "MassStorageTimeout :0x%x", (psFlash2xCSInfo->MassStorageTimeout));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage1Part1Start :0x%x", (psFlash2xCSInfo->OffsetISOImage1Part1Start));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage1Part1End :0x%x", (psFlash2xCSInfo->OffsetISOImage1Part1End));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage1Part2Start :0x%x", (psFlash2xCSInfo->OffsetISOImage1Part2Start));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage1Part2End :0x%x", (psFlash2xCSInfo->OffsetISOImage1Part2End));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage1Part3Start :0x%x", (psFlash2xCSInfo->OffsetISOImage1Part3Start));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage1Part3End :0x%x", (psFlash2xCSInfo->OffsetISOImage1Part3End));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage2Part1Start :0x%x", (psFlash2xCSInfo->OffsetISOImage2Part1Start));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage2Part1End	:0x%x", (psFlash2xCSInfo->OffsetISOImage2Part1End));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage2Part2Start :0x%x", (psFlash2xCSInfo->OffsetISOImage2Part2Start));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage2Part2End :0x%x", (psFlash2xCSInfo->OffsetISOImage2Part2End));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage2Part3Start :0x%x", (psFlash2xCSInfo->OffsetISOImage2Part3Start));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage2Part3End :0x%x", (psFlash2xCSInfo->OffsetISOImage2Part3End));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromDSDStartForDSDHeader :0x%x", (psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForDSD1Start :0x%x", (psFlash2xCSInfo->OffsetFromZeroForDSD1Start));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForDSD1End :0x%x", (psFlash2xCSInfo->OffsetFromZeroForDSD1End));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForDSD2Start :0x%x", (psFlash2xCSInfo->OffsetFromZeroForDSD2Start));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForDSD2End :0x%x", (psFlash2xCSInfo->OffsetFromZeroForDSD2End));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForVSA1Start :0x%x", (psFlash2xCSInfo->OffsetFromZeroForVSA1Start));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForVSA1End :0x%x", (psFlash2xCSInfo->OffsetFromZeroForVSA1End));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForVSA2Start :0x%x", (psFlash2xCSInfo->OffsetFromZeroForVSA2Start));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForVSA2End :0x%x", (psFlash2xCSInfo->OffsetFromZeroForVSA2End));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Sector Access Bit Map is Defined as :");
-
-	for (Index = 0; Index < (FLASH2X_TOTAL_SIZE / (DEFAULT_SECTOR_SIZE * 16)); Index++)
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "SectorAccessBitMap[%d] :0x%x", Index,
-				(psFlash2xCSInfo->SectorAccessBitMap[Index]));
-
-	return STATUS_SUCCESS;
-}
-
-static int ConvertEndianOf2XCSStructure(struct bcm_flash2x_cs_info *psFlash2xCSInfo)
-{
-	unsigned int Index = 0;
-
-	psFlash2xCSInfo->MagicNumber = ntohl(psFlash2xCSInfo->MagicNumber);
-	psFlash2xCSInfo->FlashLayoutVersion = ntohl(psFlash2xCSInfo->FlashLayoutVersion);
-	/* psFlash2xCSInfo->FlashLayoutMinorVersion = ntohs(psFlash2xCSInfo->FlashLayoutMinorVersion); */
-	psFlash2xCSInfo->ISOImageVersion = ntohl(psFlash2xCSInfo->ISOImageVersion);
-	psFlash2xCSInfo->SCSIFirmwareVersion = ntohl(psFlash2xCSInfo->SCSIFirmwareVersion);
-	psFlash2xCSInfo->OffsetFromZeroForPart1ISOImage = ntohl(psFlash2xCSInfo->OffsetFromZeroForPart1ISOImage);
-	psFlash2xCSInfo->OffsetFromZeroForScsiFirmware = ntohl(psFlash2xCSInfo->OffsetFromZeroForScsiFirmware);
-	psFlash2xCSInfo->SizeOfScsiFirmware = ntohl(psFlash2xCSInfo->SizeOfScsiFirmware);
-	psFlash2xCSInfo->OffsetFromZeroForPart2ISOImage = ntohl(psFlash2xCSInfo->OffsetFromZeroForPart2ISOImage);
-	psFlash2xCSInfo->OffsetFromZeroForDSDStart = ntohl(psFlash2xCSInfo->OffsetFromZeroForDSDStart);
-	psFlash2xCSInfo->OffsetFromZeroForDSDEnd = ntohl(psFlash2xCSInfo->OffsetFromZeroForDSDEnd);
-	psFlash2xCSInfo->OffsetFromZeroForVSAStart = ntohl(psFlash2xCSInfo->OffsetFromZeroForVSAStart);
-	psFlash2xCSInfo->OffsetFromZeroForVSAEnd = ntohl(psFlash2xCSInfo->OffsetFromZeroForVSAEnd);
-	psFlash2xCSInfo->OffsetFromZeroForControlSectionStart = ntohl(psFlash2xCSInfo->OffsetFromZeroForControlSectionStart);
-	psFlash2xCSInfo->OffsetFromZeroForControlSectionData = ntohl(psFlash2xCSInfo->OffsetFromZeroForControlSectionData);
-	psFlash2xCSInfo->CDLessInactivityTimeout = ntohl(psFlash2xCSInfo->CDLessInactivityTimeout);
-	psFlash2xCSInfo->NewImageSignature = ntohl(psFlash2xCSInfo->NewImageSignature);
-	psFlash2xCSInfo->FlashSectorSizeSig = ntohl(psFlash2xCSInfo->FlashSectorSizeSig);
-	psFlash2xCSInfo->FlashSectorSize = ntohl(psFlash2xCSInfo->FlashSectorSize);
-	psFlash2xCSInfo->FlashWriteSupportSize = ntohl(psFlash2xCSInfo->FlashWriteSupportSize);
-	psFlash2xCSInfo->TotalFlashSize = ntohl(psFlash2xCSInfo->TotalFlashSize);
-	psFlash2xCSInfo->FlashBaseAddr = ntohl(psFlash2xCSInfo->FlashBaseAddr);
-	psFlash2xCSInfo->FlashPartMaxSize = ntohl(psFlash2xCSInfo->FlashPartMaxSize);
-	psFlash2xCSInfo->IsCDLessDeviceBootSig = ntohl(psFlash2xCSInfo->IsCDLessDeviceBootSig);
-	psFlash2xCSInfo->MassStorageTimeout = ntohl(psFlash2xCSInfo->MassStorageTimeout);
-	psFlash2xCSInfo->OffsetISOImage1Part1Start = ntohl(psFlash2xCSInfo->OffsetISOImage1Part1Start);
-	psFlash2xCSInfo->OffsetISOImage1Part1End = ntohl(psFlash2xCSInfo->OffsetISOImage1Part1End);
-	psFlash2xCSInfo->OffsetISOImage1Part2Start = ntohl(psFlash2xCSInfo->OffsetISOImage1Part2Start);
-	psFlash2xCSInfo->OffsetISOImage1Part2End = ntohl(psFlash2xCSInfo->OffsetISOImage1Part2End);
-	psFlash2xCSInfo->OffsetISOImage1Part3Start = ntohl(psFlash2xCSInfo->OffsetISOImage1Part3Start);
-	psFlash2xCSInfo->OffsetISOImage1Part3End = ntohl(psFlash2xCSInfo->OffsetISOImage1Part3End);
-	psFlash2xCSInfo->OffsetISOImage2Part1Start = ntohl(psFlash2xCSInfo->OffsetISOImage2Part1Start);
-	psFlash2xCSInfo->OffsetISOImage2Part1End = ntohl(psFlash2xCSInfo->OffsetISOImage2Part1End);
-	psFlash2xCSInfo->OffsetISOImage2Part2Start = ntohl(psFlash2xCSInfo->OffsetISOImage2Part2Start);
-	psFlash2xCSInfo->OffsetISOImage2Part2End = ntohl(psFlash2xCSInfo->OffsetISOImage2Part2End);
-	psFlash2xCSInfo->OffsetISOImage2Part3Start = ntohl(psFlash2xCSInfo->OffsetISOImage2Part3Start);
-	psFlash2xCSInfo->OffsetISOImage2Part3End = ntohl(psFlash2xCSInfo->OffsetISOImage2Part3End);
-	psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader = ntohl(psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader);
-	psFlash2xCSInfo->OffsetFromZeroForDSD1Start = ntohl(psFlash2xCSInfo->OffsetFromZeroForDSD1Start);
-	psFlash2xCSInfo->OffsetFromZeroForDSD1End = ntohl(psFlash2xCSInfo->OffsetFromZeroForDSD1End);
-	psFlash2xCSInfo->OffsetFromZeroForDSD2Start = ntohl(psFlash2xCSInfo->OffsetFromZeroForDSD2Start);
-	psFlash2xCSInfo->OffsetFromZeroForDSD2End = ntohl(psFlash2xCSInfo->OffsetFromZeroForDSD2End);
-	psFlash2xCSInfo->OffsetFromZeroForVSA1Start = ntohl(psFlash2xCSInfo->OffsetFromZeroForVSA1Start);
-	psFlash2xCSInfo->OffsetFromZeroForVSA1End = ntohl(psFlash2xCSInfo->OffsetFromZeroForVSA1End);
-	psFlash2xCSInfo->OffsetFromZeroForVSA2Start = ntohl(psFlash2xCSInfo->OffsetFromZeroForVSA2Start);
-	psFlash2xCSInfo->OffsetFromZeroForVSA2End = ntohl(psFlash2xCSInfo->OffsetFromZeroForVSA2End);
-
-	for (Index = 0; Index < (FLASH2X_TOTAL_SIZE / (DEFAULT_SECTOR_SIZE * 16)); Index++)
-		psFlash2xCSInfo->SectorAccessBitMap[Index] = ntohl(psFlash2xCSInfo->SectorAccessBitMap[Index]);
-
-	return STATUS_SUCCESS;
-}
-
-static int ConvertEndianOfCSStructure(struct bcm_flash_cs_info *psFlashCSInfo)
-{
-	/* unsigned int Index = 0; */
-	psFlashCSInfo->MagicNumber				= ntohl(psFlashCSInfo->MagicNumber);
-	psFlashCSInfo->FlashLayoutVersion			= ntohl(psFlashCSInfo->FlashLayoutVersion);
-	psFlashCSInfo->ISOImageVersion				= ntohl(psFlashCSInfo->ISOImageVersion);
-	/* won't convert according to old assumption */
-	psFlashCSInfo->SCSIFirmwareVersion			= (psFlashCSInfo->SCSIFirmwareVersion);
-	psFlashCSInfo->OffsetFromZeroForPart1ISOImage		= ntohl(psFlashCSInfo->OffsetFromZeroForPart1ISOImage);
-	psFlashCSInfo->OffsetFromZeroForScsiFirmware		= ntohl(psFlashCSInfo->OffsetFromZeroForScsiFirmware);
-	psFlashCSInfo->SizeOfScsiFirmware			= ntohl(psFlashCSInfo->SizeOfScsiFirmware);
-	psFlashCSInfo->OffsetFromZeroForPart2ISOImage		= ntohl(psFlashCSInfo->OffsetFromZeroForPart2ISOImage);
-	psFlashCSInfo->OffsetFromZeroForCalibrationStart	= ntohl(psFlashCSInfo->OffsetFromZeroForCalibrationStart);
-	psFlashCSInfo->OffsetFromZeroForCalibrationEnd		= ntohl(psFlashCSInfo->OffsetFromZeroForCalibrationEnd);
-	psFlashCSInfo->OffsetFromZeroForVSAStart		= ntohl(psFlashCSInfo->OffsetFromZeroForVSAStart);
-	psFlashCSInfo->OffsetFromZeroForVSAEnd			= ntohl(psFlashCSInfo->OffsetFromZeroForVSAEnd);
-	psFlashCSInfo->OffsetFromZeroForControlSectionStart	= ntohl(psFlashCSInfo->OffsetFromZeroForControlSectionStart);
-	psFlashCSInfo->OffsetFromZeroForControlSectionData	= ntohl(psFlashCSInfo->OffsetFromZeroForControlSectionData);
-	psFlashCSInfo->CDLessInactivityTimeout			= ntohl(psFlashCSInfo->CDLessInactivityTimeout);
-	psFlashCSInfo->NewImageSignature			= ntohl(psFlashCSInfo->NewImageSignature);
-	psFlashCSInfo->FlashSectorSizeSig			= ntohl(psFlashCSInfo->FlashSectorSizeSig);
-	psFlashCSInfo->FlashSectorSize				= ntohl(psFlashCSInfo->FlashSectorSize);
-	psFlashCSInfo->FlashWriteSupportSize			= ntohl(psFlashCSInfo->FlashWriteSupportSize);
-	psFlashCSInfo->TotalFlashSize				= ntohl(psFlashCSInfo->TotalFlashSize);
-	psFlashCSInfo->FlashBaseAddr				= ntohl(psFlashCSInfo->FlashBaseAddr);
-	psFlashCSInfo->FlashPartMaxSize				= ntohl(psFlashCSInfo->FlashPartMaxSize);
-	psFlashCSInfo->IsCDLessDeviceBootSig			= ntohl(psFlashCSInfo->IsCDLessDeviceBootSig);
-	psFlashCSInfo->MassStorageTimeout			= ntohl(psFlashCSInfo->MassStorageTimeout);
-
-	return STATUS_SUCCESS;
-}
-
-static int IsSectionExistInVendorInfo(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_section_val section)
-{
-	return (Adapter->uiVendorExtnFlag &&
-		(Adapter->psFlash2xVendorInfo->VendorSection[section].AccessFlags & FLASH2X_SECTION_PRESENT) &&
-		(Adapter->psFlash2xVendorInfo->VendorSection[section].OffsetFromZeroForSectionStart != UNINIT_PTR_IN_CS));
-}
-
-static VOID UpdateVendorInfo(struct bcm_mini_adapter *Adapter)
-{
-	B_UINT32 i = 0;
-	unsigned int uiSizeSection = 0;
-
-	Adapter->uiVendorExtnFlag = false;
-
-	for (i = 0; i < TOTAL_SECTIONS; i++)
-		Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart = UNINIT_PTR_IN_CS;
-
-	if (STATUS_SUCCESS != vendorextnGetSectionInfo(Adapter, Adapter->psFlash2xVendorInfo))
-		return;
-
-	i = 0;
-	while (i < TOTAL_SECTIONS) {
-		if (!(Adapter->psFlash2xVendorInfo->VendorSection[i].AccessFlags & FLASH2X_SECTION_PRESENT)) {
-			i++;
-			continue;
-		}
-
-		Adapter->uiVendorExtnFlag = TRUE;
-		uiSizeSection = (Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionEnd -
-				Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart);
-
-		switch (i) {
-		case DSD0:
-			if ((uiSizeSection >= (Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + sizeof(struct bcm_dsd_header))) &&
-				(UNINIT_PTR_IN_CS != Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart))
-				Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDEnd = VENDOR_PTR_IN_CS;
-			else
-				Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDEnd = UNINIT_PTR_IN_CS;
-			break;
-
-		case DSD1:
-			if ((uiSizeSection >= (Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + sizeof(struct bcm_dsd_header))) &&
-				(UNINIT_PTR_IN_CS != Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart))
-				Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1Start = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1End = VENDOR_PTR_IN_CS;
-			else
-				Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1Start = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1End = UNINIT_PTR_IN_CS;
-			break;
-
-		case DSD2:
-			if ((uiSizeSection >= (Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + sizeof(struct bcm_dsd_header))) &&
-				(UNINIT_PTR_IN_CS != Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart))
-				Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2Start = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2End = VENDOR_PTR_IN_CS;
-			else
-				Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2Start = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2End = UNINIT_PTR_IN_CS;
-			break;
-		case VSA0:
-			if (UNINIT_PTR_IN_CS != Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart)
-				Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAEnd = VENDOR_PTR_IN_CS;
-			else
-				Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAEnd = UNINIT_PTR_IN_CS;
-			break;
-
-		case VSA1:
-			if (UNINIT_PTR_IN_CS != Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart)
-				Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1Start = Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1End = VENDOR_PTR_IN_CS;
-			else
-				Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1Start = Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1End = UNINIT_PTR_IN_CS;
-			break;
-		case VSA2:
-			if (UNINIT_PTR_IN_CS != Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart)
-				Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2Start = Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2End = VENDOR_PTR_IN_CS;
-			else
-				Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2Start = Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2End = UNINIT_PTR_IN_CS;
-			break;
-
-		default:
-			break;
-		}
-		i++;
-	}
-}
-
-/*
- * Procedure:	BcmGetFlashCSInfo
- *
- * Description: Reads control structure and gets Cal section addresses.
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- *
- * Returns:
- *		<VOID>
- */
-
-static int BcmGetFlashCSInfo(struct bcm_mini_adapter *Adapter)
-{
-	/* struct bcm_flash_cs_info sFlashCsInfo = {0}; */
-
-	#if !defined(BCM_SHM_INTERFACE) || defined(FLASH_DIRECT_ACCESS)
-		unsigned int value;
-	#endif
-
-	unsigned int uiFlashLayoutMajorVersion;
-
-	Adapter->uiFlashLayoutMinorVersion = 0;
-	Adapter->uiFlashLayoutMajorVersion = 0;
-	Adapter->ulFlashControlSectionStart = FLASH_CS_INFO_START_ADDR;
-
-	Adapter->uiFlashBaseAdd = 0;
-	Adapter->ulFlashCalStart = 0;
-	memset(Adapter->psFlashCSInfo, 0 , sizeof(struct bcm_flash_cs_info));
-	memset(Adapter->psFlash2xCSInfo, 0 , sizeof(struct bcm_flash2x_cs_info));
-
-	if (!Adapter->bDDRInitDone) {
-		value = FLASH_CONTIGIOUS_START_ADDR_BEFORE_INIT;
-		wrmalt(Adapter, 0xAF00A080, &value, sizeof(value));
-	}
-
-	/* Reading first 8 Bytes to get the Flash Layout
-	 * MagicNumber(4 bytes) +FlashLayoutMinorVersion(2 Bytes) +FlashLayoutMajorVersion(2 Bytes)
-	 */
-	BeceemFlashBulkRead(Adapter, (PUINT)Adapter->psFlashCSInfo, Adapter->ulFlashControlSectionStart, 8);
-
-	Adapter->psFlashCSInfo->FlashLayoutVersion =  ntohl(Adapter->psFlashCSInfo->FlashLayoutVersion);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Flash Layout Version :%X", (Adapter->psFlashCSInfo->FlashLayoutVersion));
-	/* BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Flash Layout Minor Version :%d\n", ntohs(sFlashCsInfo.FlashLayoutMinorVersion)); */
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Signature is  :%x\n", ntohl(Adapter->psFlashCSInfo->MagicNumber));
-
-	if (FLASH_CONTROL_STRUCT_SIGNATURE == ntohl(Adapter->psFlashCSInfo->MagicNumber)) {
-		uiFlashLayoutMajorVersion = MAJOR_VERSION((Adapter->psFlashCSInfo->FlashLayoutVersion));
-		Adapter->uiFlashLayoutMinorVersion = MINOR_VERSION((Adapter->psFlashCSInfo->FlashLayoutVersion));
-	} else {
-		Adapter->uiFlashLayoutMinorVersion = 0;
-		uiFlashLayoutMajorVersion = 0;
-	}
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "FLASH LAYOUT MAJOR VERSION :%X", uiFlashLayoutMajorVersion);
-
-	if (uiFlashLayoutMajorVersion < FLASH_2X_MAJOR_NUMBER) {
-		BeceemFlashBulkRead(Adapter, (PUINT)Adapter->psFlashCSInfo, Adapter->ulFlashControlSectionStart, sizeof(struct bcm_flash_cs_info));
-		ConvertEndianOfCSStructure(Adapter->psFlashCSInfo);
-		Adapter->ulFlashCalStart = (Adapter->psFlashCSInfo->OffsetFromZeroForCalibrationStart);
-
-		if (!((Adapter->uiFlashLayoutMajorVersion == 1) && (Adapter->uiFlashLayoutMinorVersion == 1)))
-			Adapter->ulFlashControlSectionStart = Adapter->psFlashCSInfo->OffsetFromZeroForControlSectionStart;
-
-		if ((FLASH_CONTROL_STRUCT_SIGNATURE == (Adapter->psFlashCSInfo->MagicNumber)) &&
-			(SCSI_FIRMWARE_MINOR_VERSION <= MINOR_VERSION(Adapter->psFlashCSInfo->SCSIFirmwareVersion)) &&
-			(FLASH_SECTOR_SIZE_SIG == (Adapter->psFlashCSInfo->FlashSectorSizeSig)) &&
-			(BYTE_WRITE_SUPPORT == (Adapter->psFlashCSInfo->FlashWriteSupportSize))) {
-			Adapter->ulFlashWriteSize = (Adapter->psFlashCSInfo->FlashWriteSupportSize);
-			Adapter->fpFlashWrite = flashByteWrite;
-			Adapter->fpFlashWriteWithStatusCheck = flashByteWriteStatus;
-		} else {
-			Adapter->ulFlashWriteSize = MAX_RW_SIZE;
-			Adapter->fpFlashWrite = flashWrite;
-			Adapter->fpFlashWriteWithStatusCheck = flashWriteStatus;
-		}
-
-		BcmGetFlashSectorSize(Adapter, (Adapter->psFlashCSInfo->FlashSectorSizeSig),
-				(Adapter->psFlashCSInfo->FlashSectorSize));
-		Adapter->uiFlashBaseAdd = Adapter->psFlashCSInfo->FlashBaseAddr & 0xFCFFFFFF;
-	} else {
-		if (BcmFlash2xBulkRead(Adapter, (PUINT)Adapter->psFlash2xCSInfo, NO_SECTION_VAL,
-					Adapter->ulFlashControlSectionStart, sizeof(struct bcm_flash2x_cs_info))) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Unable to read CS structure\n");
-			return STATUS_FAILURE;
-		}
-
-		ConvertEndianOf2XCSStructure(Adapter->psFlash2xCSInfo);
-		BcmDumpFlash2XCSStructure(Adapter->psFlash2xCSInfo, Adapter);
-		if ((FLASH_CONTROL_STRUCT_SIGNATURE == Adapter->psFlash2xCSInfo->MagicNumber) &&
-			(SCSI_FIRMWARE_MINOR_VERSION <= MINOR_VERSION(Adapter->psFlash2xCSInfo->SCSIFirmwareVersion)) &&
-			(FLASH_SECTOR_SIZE_SIG == Adapter->psFlash2xCSInfo->FlashSectorSizeSig) &&
-			(BYTE_WRITE_SUPPORT == Adapter->psFlash2xCSInfo->FlashWriteSupportSize)) {
-			Adapter->ulFlashWriteSize = Adapter->psFlash2xCSInfo->FlashWriteSupportSize;
-			Adapter->fpFlashWrite = flashByteWrite;
-			Adapter->fpFlashWriteWithStatusCheck = flashByteWriteStatus;
-		} else {
-			Adapter->ulFlashWriteSize = MAX_RW_SIZE;
-			Adapter->fpFlashWrite = flashWrite;
-			Adapter->fpFlashWriteWithStatusCheck = flashWriteStatus;
-		}
-
-		BcmGetFlashSectorSize(Adapter, Adapter->psFlash2xCSInfo->FlashSectorSizeSig,
-				Adapter->psFlash2xCSInfo->FlashSectorSize);
-
-		UpdateVendorInfo(Adapter);
-
-		BcmGetActiveDSD(Adapter);
-		BcmGetActiveISO(Adapter);
-		Adapter->uiFlashBaseAdd = Adapter->psFlash2xCSInfo->FlashBaseAddr & 0xFCFFFFFF;
-		Adapter->ulFlashControlSectionStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForControlSectionStart;
-	}
-	/*
-	 * Concerns: what if CS sector size does not match with this sector size ???
-	 * what is the indication of AccessBitMap  in CS in flash 2.x ????
-	 */
-	Adapter->ulFlashID = BcmReadFlashRDID(Adapter);
-	Adapter->uiFlashLayoutMajorVersion = uiFlashLayoutMajorVersion;
-
-	return STATUS_SUCCESS;
-}
-
-/*
- * Procedure:	BcmGetNvmType
- *
- * Description: Finds the type of NVM used.
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- *
- * Returns:
- *		NVM_TYPE
- *
- */
-
-static enum bcm_nvm_type BcmGetNvmType(struct bcm_mini_adapter *Adapter)
-{
-	unsigned int uiData = 0;
-
-	BeceemEEPROMBulkRead(Adapter, &uiData, 0x0, 4);
-	if (uiData == BECM)
-		return NVM_EEPROM;
-
-	/*
-	 * Read control struct and get cal addresses before accessing the flash
-	 */
-	BcmGetFlashCSInfo(Adapter);
-
-	BeceemFlashBulkRead(Adapter, &uiData, 0x0 + Adapter->ulFlashCalStart, 4);
-	if (uiData == BECM)
-		return NVM_FLASH;
-
-	/*
-	 * even if there is no valid signature on EEPROM/FLASH find out if they really exist.
-	 * if exist select it.
-	 */
-	if (BcmGetEEPROMSize(Adapter))
-		return NVM_EEPROM;
-
-	/* TBD for Flash. */
-	return NVM_UNKNOWN;
-}
-
-/*
- * BcmGetSectionValStartOffset - this will calculate the section's starting offset if section val is given
- * @Adapter : Drivers Private Data structure
- * @eFlashSectionVal : Flash secion value defined in enum bcm_flash2x_section_val
- *
- * Return value:-
- * On success it return the start offset of the provided section val
- * On Failure -returns STATUS_FAILURE
- */
-
-int BcmGetSectionValStartOffset(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_section_val eFlashSectionVal)
-{
-	/*
-	 * Considering all the section for which end offset can be calculated or directly given
-	 * in CS Structure. if matching case does not exist, return STATUS_FAILURE indicating section
-	 * endoffset can't be calculated or given in CS Structure.
-	 */
-
-	int SectStartOffset = 0;
-
-	SectStartOffset = INVALID_OFFSET;
-
-	if (IsSectionExistInVendorInfo(Adapter, eFlashSectionVal))
-		return Adapter->psFlash2xVendorInfo->VendorSection[eFlashSectionVal].OffsetFromZeroForSectionStart;
-
-	switch (eFlashSectionVal) {
-	case ISO_IMAGE1:
-		if ((Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start != UNINIT_PTR_IN_CS) &&
-			(IsNonCDLessDevice(Adapter) == false))
-			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start);
-		break;
-	case ISO_IMAGE2:
-		if ((Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start != UNINIT_PTR_IN_CS) &&
-			(IsNonCDLessDevice(Adapter) == false))
-			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start);
-		break;
-	case DSD0:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDStart != UNINIT_PTR_IN_CS)
-			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDStart);
-		break;
-	case DSD1:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1Start != UNINIT_PTR_IN_CS)
-			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1Start);
-		break;
-	case DSD2:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2Start != UNINIT_PTR_IN_CS)
-			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2Start);
-		break;
-	case VSA0:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAStart != UNINIT_PTR_IN_CS)
-			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAStart);
-		break;
-	case VSA1:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1Start != UNINIT_PTR_IN_CS)
-			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1Start);
-		break;
-	case VSA2:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2Start != UNINIT_PTR_IN_CS)
-			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2Start);
-		break;
-	case SCSI:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForScsiFirmware != UNINIT_PTR_IN_CS)
-			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForScsiFirmware);
-		break;
-	case CONTROL_SECTION:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForControlSectionStart != UNINIT_PTR_IN_CS)
-			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForControlSectionStart);
-		break;
-	case ISO_IMAGE1_PART2:
-		if (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2Start != UNINIT_PTR_IN_CS)
-			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2Start);
-		break;
-	case ISO_IMAGE1_PART3:
-		if (Adapter->psFlash2xCSInfo->OffsetISOImage1Part3Start != UNINIT_PTR_IN_CS)
-			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part3Start);
-		break;
-	case ISO_IMAGE2_PART2:
-		if (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2Start != UNINIT_PTR_IN_CS)
-			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2Start);
-		break;
-	case ISO_IMAGE2_PART3:
-		if (Adapter->psFlash2xCSInfo->OffsetISOImage2Part3Start != UNINIT_PTR_IN_CS)
-			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part3Start);
-		break;
-	default:
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Section Does not exist in Flash 2.x");
-		SectStartOffset = INVALID_OFFSET;
-	}
-
-	return SectStartOffset;
-}
-
-/*
- * BcmGetSectionValEndOffset - this will calculate the section's Ending offset if section val is given
- * @Adapter : Drivers Private Data structure
- * @eFlashSectionVal : Flash secion value defined in enum bcm_flash2x_section_val
- *
- * Return value:-
- * On success it return the end offset of the provided section val
- * On Failure -returns STATUS_FAILURE
- */
-
-static int BcmGetSectionValEndOffset(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_section_val eFlash2xSectionVal)
-{
-	int SectEndOffset = 0;
-
-	SectEndOffset = INVALID_OFFSET;
-	if (IsSectionExistInVendorInfo(Adapter, eFlash2xSectionVal))
-		return Adapter->psFlash2xVendorInfo->VendorSection[eFlash2xSectionVal].OffsetFromZeroForSectionEnd;
-
-	switch (eFlash2xSectionVal) {
-	case ISO_IMAGE1:
-		if ((Adapter->psFlash2xCSInfo->OffsetISOImage1Part1End != UNINIT_PTR_IN_CS) &&
-			(IsNonCDLessDevice(Adapter) == false))
-			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1End);
-		break;
-	case ISO_IMAGE2:
-		if ((Adapter->psFlash2xCSInfo->OffsetISOImage2Part1End != UNINIT_PTR_IN_CS) &&
-			(IsNonCDLessDevice(Adapter) == false))
-			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1End);
-		break;
-	case DSD0:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDEnd != UNINIT_PTR_IN_CS)
-			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDEnd);
-		break;
-	case DSD1:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1End != UNINIT_PTR_IN_CS)
-			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1End);
-		break;
-	case DSD2:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2End != UNINIT_PTR_IN_CS)
-			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2End);
-		break;
-	case VSA0:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAEnd != UNINIT_PTR_IN_CS)
-			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAEnd);
-		break;
-	case VSA1:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1End != UNINIT_PTR_IN_CS)
-			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1End);
-		break;
-	case VSA2:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2End != UNINIT_PTR_IN_CS)
-			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2End);
-		break;
-	case SCSI:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForScsiFirmware != UNINIT_PTR_IN_CS)
-			SectEndOffset = ((Adapter->psFlash2xCSInfo->OffsetFromZeroForScsiFirmware) +
-					(Adapter->psFlash2xCSInfo->SizeOfScsiFirmware));
-		break;
-	case CONTROL_SECTION:
-		/* Not Clear So Putting failure. confirm and fix it. */
-		SectEndOffset = STATUS_FAILURE;
-		break;
-	case ISO_IMAGE1_PART2:
-		if (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2End != UNINIT_PTR_IN_CS)
-			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2End);
-		break;
-	case ISO_IMAGE1_PART3:
-		if (Adapter->psFlash2xCSInfo->OffsetISOImage1Part3End != UNINIT_PTR_IN_CS)
-			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part3End);
-		break;
-	case ISO_IMAGE2_PART2:
-		if (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2End != UNINIT_PTR_IN_CS)
-			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2End);
-		break;
-	case ISO_IMAGE2_PART3:
-		if (Adapter->psFlash2xCSInfo->OffsetISOImage2Part3End != UNINIT_PTR_IN_CS)
-			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part3End);
-		break;
-	default:
-		SectEndOffset = INVALID_OFFSET;
-	}
-
-	return SectEndOffset;
-}
-
-/*
- * BcmFlash2xBulkRead:- Read API for Flash Map 2.x .
- * @Adapter :Driver Private Data Structure
- * @pBuffer : Buffer where data has to be put after reading
- * @eFlashSectionVal :Flash Section Val defined in enum bcm_flash2x_section_val
- * @uiOffsetWithinSectionVal :- Offset with in provided section
- * @uiNumBytes : Number of Bytes for Read
- *
- * Return value:-
- * return true on success and STATUS_FAILURE on fail.
- */
-
-int BcmFlash2xBulkRead(struct bcm_mini_adapter *Adapter,
-		PUINT pBuffer,
-		enum bcm_flash2x_section_val eFlash2xSectionVal,
-		unsigned int uiOffsetWithinSectionVal,
-		unsigned int uiNumBytes)
-{
-	int Status = STATUS_SUCCESS;
-	int SectionStartOffset = 0;
-	unsigned int uiAbsoluteOffset = 0;
-	unsigned int uiTemp = 0, value = 0;
-
-	if (!Adapter) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Adapter structure is NULL");
-		return -EINVAL;
-	}
-	if (Adapter->device_removed) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Device has been removed");
-		return -ENODEV;
-	}
-
-	/* NO_SECTION_VAL means absolute offset is given. */
-	if (eFlash2xSectionVal == NO_SECTION_VAL)
-		SectionStartOffset = 0;
-	else
-		SectionStartOffset = BcmGetSectionValStartOffset(Adapter, eFlash2xSectionVal);
-
-	if (SectionStartOffset == STATUS_FAILURE) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "This Section<%d> does not exist in Flash 2.x Map ", eFlash2xSectionVal);
-		return -EINVAL;
-	}
-
-	if (IsSectionExistInVendorInfo(Adapter, eFlash2xSectionVal))
-		return vendorextnReadSection(Adapter, (PUCHAR)pBuffer, eFlash2xSectionVal, uiOffsetWithinSectionVal, uiNumBytes);
-
-	/* calculating  the absolute offset from FLASH; */
-	uiAbsoluteOffset = uiOffsetWithinSectionVal + SectionStartOffset;
-	rdmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
-	value = 0;
-	wrmalt(Adapter, 0x0f000C80, &value, sizeof(value));
-	Status = BeceemFlashBulkRead(Adapter, pBuffer, uiAbsoluteOffset, uiNumBytes);
-	wrmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
-	if (Status) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Flash Read Failed with Status :%d", Status);
-		return Status;
-	}
-
-	return Status;
-}
-
-/*
- * BcmFlash2xBulkWrite :-API for Writing on the Flash Map 2.x.
- * @Adapter :Driver Private Data Structure
- * @pBuffer : Buffer From where data has to taken for writing
- * @eFlashSectionVal :Flash Section Val defined in enum bcm_flash2x_section_val
- * @uiOffsetWithinSectionVal :- Offset with in provided section
- * @uiNumBytes : Number of Bytes for Write
- *
- * Return value:-
- * return true on success and STATUS_FAILURE on fail.
- *
- */
-
-int BcmFlash2xBulkWrite(struct bcm_mini_adapter *Adapter,
-			PUINT pBuffer,
-			enum bcm_flash2x_section_val eFlash2xSectVal,
-			unsigned int uiOffset,
-			unsigned int uiNumBytes,
-			unsigned int bVerify)
-{
-	int Status = STATUS_SUCCESS;
-	unsigned int FlashSectValStartOffset = 0;
-	unsigned int uiTemp = 0, value = 0;
-
-	if (!Adapter) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Adapter structure is NULL");
-		return -EINVAL;
-	}
-
-	if (Adapter->device_removed) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Device has been removed");
-		return -ENODEV;
-	}
-
-	/* NO_SECTION_VAL means absolute offset is given. */
-	if (eFlash2xSectVal == NO_SECTION_VAL)
-		FlashSectValStartOffset = 0;
-	else
-		FlashSectValStartOffset = BcmGetSectionValStartOffset(Adapter, eFlash2xSectVal);
-
-	if (FlashSectValStartOffset == STATUS_FAILURE) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "This Section<%d> does not exist in Flash Map 2.x", eFlash2xSectVal);
-		return -EINVAL;
-	}
-
-	if (IsSectionExistInVendorInfo(Adapter, eFlash2xSectVal))
-		return vendorextnWriteSection(Adapter, (PUCHAR)pBuffer, eFlash2xSectVal, uiOffset, uiNumBytes, bVerify);
-
-	/* calculating  the absolute offset from FLASH; */
-	uiOffset = uiOffset + FlashSectValStartOffset;
-
-	rdmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
-	value = 0;
-	wrmalt(Adapter, 0x0f000C80, &value, sizeof(value));
-
-	Status = BeceemFlashBulkWrite(Adapter, pBuffer, uiOffset, uiNumBytes, bVerify);
-
-	wrmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
-	if (Status) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Flash Write failed with Status :%d", Status);
-		return Status;
-	}
-
-	return Status;
-}
-
-/*
- * BcmGetActiveDSD : Set the Active DSD in Adapter Structure which has to be dumped in DDR
- * @Adapter :-Drivers private Data Structure
- *
- * Return Value:-
- * Return STATUS_SUCESS if get success in setting the right DSD else negative error code
- *
- */
-
-static int BcmGetActiveDSD(struct bcm_mini_adapter *Adapter)
-{
-	enum bcm_flash2x_section_val uiHighestPriDSD = 0;
-
-	uiHighestPriDSD = getHighestPriDSD(Adapter);
-	Adapter->eActiveDSD = uiHighestPriDSD;
-
-	if (DSD0  == uiHighestPriDSD)
-		Adapter->ulFlashCalStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDStart;
-	if (DSD1 == uiHighestPriDSD)
-		Adapter->ulFlashCalStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1Start;
-	if (DSD2 == uiHighestPriDSD)
-		Adapter->ulFlashCalStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2Start;
-	if (Adapter->eActiveDSD)
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Active DSD :%d", Adapter->eActiveDSD);
-	if (Adapter->eActiveDSD == 0) {
-		/* if No DSD gets Active, Make Active the DSD with WR  permission */
-		if (IsSectionWritable(Adapter, DSD2)) {
-			Adapter->eActiveDSD = DSD2;
-			Adapter->ulFlashCalStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2Start;
-		} else if (IsSectionWritable(Adapter, DSD1)) {
-			Adapter->eActiveDSD = DSD1;
-			Adapter->ulFlashCalStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1Start;
-		} else if (IsSectionWritable(Adapter, DSD0)) {
-			Adapter->eActiveDSD = DSD0;
-			Adapter->ulFlashCalStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDStart;
-		}
-	}
-
-	return STATUS_SUCCESS;
-}
-
-/*
- * BcmGetActiveISO :- Set the Active ISO in Adapter Data Structue
- * @Adapter : Driver private Data Structure
- *
- * Return Value:-
- * Sucsess:- STATUS_SUCESS
- * Failure- : negative erro code
- *
- */
-
-static int BcmGetActiveISO(struct bcm_mini_adapter *Adapter)
-{
-	int HighestPriISO = 0;
-
-	HighestPriISO = getHighestPriISO(Adapter);
-
-	Adapter->eActiveISO = HighestPriISO;
-	if (Adapter->eActiveISO == ISO_IMAGE2)
-		Adapter->uiActiveISOOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start);
-	else if (Adapter->eActiveISO == ISO_IMAGE1)
-		Adapter->uiActiveISOOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start);
-
-	if (Adapter->eActiveISO)
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Active ISO :%x", Adapter->eActiveISO);
-
-	return STATUS_SUCCESS;
-}
-
-/*
- * IsOffsetWritable :- it will tell the access permission of the sector having passed offset
- * @Adapter : Drivers Private Data Structure
- * @uiOffset : Offset provided in the Flash
- *
- * Return Value:-
- * Success:-TRUE ,  offset is writable
- * Failure:-false, offset is RO
- *
- */
-
-static B_UINT8 IsOffsetWritable(struct bcm_mini_adapter *Adapter, unsigned int uiOffset)
-{
-	unsigned int uiSectorNum = 0;
-	unsigned int uiWordOfSectorPermission = 0;
-	unsigned int uiBitofSectorePermission = 0;
-	B_UINT32 permissionBits = 0;
-
-	uiSectorNum = uiOffset/Adapter->uiSectorSize;
-
-	/* calculating the word having this Sector Access permission from SectorAccessBitMap Array */
-	uiWordOfSectorPermission = Adapter->psFlash2xCSInfo->SectorAccessBitMap[uiSectorNum / 16];
-
-	/* calculating the bit index inside the word for  this sector */
-	uiBitofSectorePermission = 2 * (15 - uiSectorNum % 16);
-
-	/* Setting Access permission */
-	permissionBits = uiWordOfSectorPermission & (0x3 << uiBitofSectorePermission);
-	permissionBits = (permissionBits >> uiBitofSectorePermission) & 0x3;
-	if (permissionBits == SECTOR_READWRITE_PERMISSION)
-		return TRUE;
-	else
-		return false;
-}
-
-static int BcmDumpFlash2xSectionBitMap(struct bcm_flash2x_bitmap *psFlash2xBitMap)
-{
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "***************Flash 2.x Section Bitmap***************");
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "ISO_IMAGE1  :0X%x", psFlash2xBitMap->ISO_IMAGE1);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "ISO_IMAGE2  :0X%x", psFlash2xBitMap->ISO_IMAGE2);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "DSD0  :0X%x", psFlash2xBitMap->DSD0);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "DSD1  :0X%x", psFlash2xBitMap->DSD1);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "DSD2  :0X%x", psFlash2xBitMap->DSD2);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "VSA0  :0X%x", psFlash2xBitMap->VSA0);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "VSA1  :0X%x", psFlash2xBitMap->VSA1);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "VSA2  :0X%x", psFlash2xBitMap->VSA2);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "SCSI  :0X%x", psFlash2xBitMap->SCSI);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "CONTROL_SECTION  :0X%x", psFlash2xBitMap->CONTROL_SECTION);
-
-	return STATUS_SUCCESS;
-}
-
-/*
- * BcmGetFlash2xSectionalBitMap :- It will provide the bit map of all the section present in Flash
- * 8bit has been assigned to every section.
- * bit[0] :Section present or not
- * bit[1] :section is valid or not
- * bit[2] : Secton is read only or has write permission too.
- * bit[3] : Active Section -
- * bit[7...4] = Reserved .
- *
- * @Adapter:-Driver private Data Structure
- *
- * Return value:-
- * Success:- STATUS_SUCESS
- * Failure:- negative error code
- */
-
-int BcmGetFlash2xSectionalBitMap(struct bcm_mini_adapter *Adapter, struct bcm_flash2x_bitmap *psFlash2xBitMap)
-{
-	struct bcm_flash2x_cs_info *psFlash2xCSInfo = Adapter->psFlash2xCSInfo;
-	enum bcm_flash2x_section_val uiHighestPriDSD = 0;
-	enum bcm_flash2x_section_val uiHighestPriISO = 0;
-	bool SetActiveDSDDone = false;
-	bool SetActiveISODone = false;
-
-	/* For 1.x map all the section except DSD0 will be shown as not present
-	 * This part will be used by calibration tool to detect the number of DSD present in Flash.
-	 */
-	if (IsFlash2x(Adapter) == false) {
-		psFlash2xBitMap->ISO_IMAGE2 = 0;
-		psFlash2xBitMap->ISO_IMAGE1 = 0;
-		psFlash2xBitMap->DSD0 = FLASH2X_SECTION_VALID | FLASH2X_SECTION_ACT | FLASH2X_SECTION_PRESENT; /* 0xF; 0000(Reseved)1(Active)0(RW)1(valid)1(present) */
-		psFlash2xBitMap->DSD1  = 0;
-		psFlash2xBitMap->DSD2 = 0;
-		psFlash2xBitMap->VSA0 = 0;
-		psFlash2xBitMap->VSA1 = 0;
-		psFlash2xBitMap->VSA2 = 0;
-		psFlash2xBitMap->CONTROL_SECTION = 0;
-		psFlash2xBitMap->SCSI = 0;
-		psFlash2xBitMap->Reserved0 = 0;
-		psFlash2xBitMap->Reserved1 = 0;
-		psFlash2xBitMap->Reserved2 = 0;
-
-		return STATUS_SUCCESS;
-	}
-
-	uiHighestPriDSD = getHighestPriDSD(Adapter);
-	uiHighestPriISO = getHighestPriISO(Adapter);
-
-	/*
-	 * IS0 IMAGE 2
-	 */
-	if ((psFlash2xCSInfo->OffsetISOImage2Part1Start) != UNINIT_PTR_IN_CS) {
-		/* Setting the 0th Bit representing the Section is present or not. */
-		psFlash2xBitMap->ISO_IMAGE2 = psFlash2xBitMap->ISO_IMAGE2 | FLASH2X_SECTION_PRESENT;
-
-		if (ReadISOSignature(Adapter, ISO_IMAGE2) == ISO_IMAGE_MAGIC_NUMBER)
-			psFlash2xBitMap->ISO_IMAGE2 |= FLASH2X_SECTION_VALID;
-
-		/* Calculation for extrating the Access permission */
-		if (IsSectionWritable(Adapter, ISO_IMAGE2) == false)
-			psFlash2xBitMap->ISO_IMAGE2 |= FLASH2X_SECTION_RO;
-
-		if (SetActiveISODone == false && uiHighestPriISO == ISO_IMAGE2) {
-			psFlash2xBitMap->ISO_IMAGE2 |= FLASH2X_SECTION_ACT;
-			SetActiveISODone = TRUE;
-		}
-	}
-
-	/*
-	 * IS0 IMAGE 1
-	 */
-	if ((psFlash2xCSInfo->OffsetISOImage1Part1Start) != UNINIT_PTR_IN_CS) {
-		/* Setting the 0th Bit representing the Section is present or not. */
-		psFlash2xBitMap->ISO_IMAGE1 = psFlash2xBitMap->ISO_IMAGE1 | FLASH2X_SECTION_PRESENT;
-
-		if (ReadISOSignature(Adapter, ISO_IMAGE1) == ISO_IMAGE_MAGIC_NUMBER)
-			psFlash2xBitMap->ISO_IMAGE1 |= FLASH2X_SECTION_VALID;
-
-		/* Calculation for extrating the Access permission */
-		if (IsSectionWritable(Adapter, ISO_IMAGE1) == false)
-			psFlash2xBitMap->ISO_IMAGE1 |= FLASH2X_SECTION_RO;
-
-		if (SetActiveISODone == false && uiHighestPriISO == ISO_IMAGE1) {
-			psFlash2xBitMap->ISO_IMAGE1 |= FLASH2X_SECTION_ACT;
-			SetActiveISODone = TRUE;
-		}
-	}
-
-	/*
-	 * DSD2
-	 */
-	if ((psFlash2xCSInfo->OffsetFromZeroForDSD2Start) != UNINIT_PTR_IN_CS) {
-		/* Setting the 0th Bit representing the Section is present or not. */
-		psFlash2xBitMap->DSD2 = psFlash2xBitMap->DSD2 | FLASH2X_SECTION_PRESENT;
-
-		if (ReadDSDSignature(Adapter, DSD2) == DSD_IMAGE_MAGIC_NUMBER)
-			psFlash2xBitMap->DSD2 |= FLASH2X_SECTION_VALID;
-
-		/* Calculation for extrating the Access permission */
-		if (IsSectionWritable(Adapter, DSD2) == false) {
-			psFlash2xBitMap->DSD2 |= FLASH2X_SECTION_RO;
-		} else {
-			/* Means section is writable */
-			if ((SetActiveDSDDone == false) && (uiHighestPriDSD == DSD2)) {
-				psFlash2xBitMap->DSD2 |= FLASH2X_SECTION_ACT;
-				SetActiveDSDDone = TRUE;
-			}
-		}
-	}
-
-	/*
-	 * DSD 1
-	 */
-	if ((psFlash2xCSInfo->OffsetFromZeroForDSD1Start) != UNINIT_PTR_IN_CS) {
-		/* Setting the 0th Bit representing the Section is present or not. */
-		psFlash2xBitMap->DSD1 = psFlash2xBitMap->DSD1 | FLASH2X_SECTION_PRESENT;
-
-		if (ReadDSDSignature(Adapter, DSD1) == DSD_IMAGE_MAGIC_NUMBER)
-			psFlash2xBitMap->DSD1 |= FLASH2X_SECTION_VALID;
-
-		/* Calculation for extrating the Access permission */
-		if (IsSectionWritable(Adapter, DSD1) == false) {
-			psFlash2xBitMap->DSD1 |= FLASH2X_SECTION_RO;
-		} else {
-			/* Means section is writable */
-			if ((SetActiveDSDDone == false) && (uiHighestPriDSD == DSD1)) {
-				psFlash2xBitMap->DSD1 |= FLASH2X_SECTION_ACT;
-				SetActiveDSDDone = TRUE;
-			}
-		}
-	}
-
-	/*
-	 * For DSD 0
-	 */
-	if ((psFlash2xCSInfo->OffsetFromZeroForDSDStart) != UNINIT_PTR_IN_CS) {
-		/* Setting the 0th Bit representing the Section is present or not. */
-		psFlash2xBitMap->DSD0 = psFlash2xBitMap->DSD0 | FLASH2X_SECTION_PRESENT;
-
-		if (ReadDSDSignature(Adapter, DSD0) == DSD_IMAGE_MAGIC_NUMBER)
-			psFlash2xBitMap->DSD0 |= FLASH2X_SECTION_VALID;
-
-		/* Setting Access permission */
-		if (IsSectionWritable(Adapter, DSD0) == false) {
-			psFlash2xBitMap->DSD0 |= FLASH2X_SECTION_RO;
-		} else {
-			/* Means section is writable */
-			if ((SetActiveDSDDone == false) && (uiHighestPriDSD == DSD0)) {
-				psFlash2xBitMap->DSD0 |= FLASH2X_SECTION_ACT;
-				SetActiveDSDDone = TRUE;
-			}
-		}
-	}
-
-	/*
-	 * VSA 0
-	 */
-	if ((psFlash2xCSInfo->OffsetFromZeroForVSAStart) != UNINIT_PTR_IN_CS) {
-		/* Setting the 0th Bit representing the Section is present or not. */
-		psFlash2xBitMap->VSA0 = psFlash2xBitMap->VSA0 | FLASH2X_SECTION_PRESENT;
-
-		/* Setting the Access Bit. Map is not defined hece setting it always valid */
-		psFlash2xBitMap->VSA0 |= FLASH2X_SECTION_VALID;
-
-		/* Calculation for extrating the Access permission */
-		if (IsSectionWritable(Adapter, VSA0) == false)
-			psFlash2xBitMap->VSA0 |=  FLASH2X_SECTION_RO;
-
-		/* By Default section is Active */
-		psFlash2xBitMap->VSA0 |= FLASH2X_SECTION_ACT;
-	}
-
-	/*
-	 * VSA 1
-	 */
-	if ((psFlash2xCSInfo->OffsetFromZeroForVSA1Start) != UNINIT_PTR_IN_CS) {
-		/* Setting the 0th Bit representing the Section is present or not. */
-		psFlash2xBitMap->VSA1 = psFlash2xBitMap->VSA1 | FLASH2X_SECTION_PRESENT;
-
-		/* Setting the Access Bit. Map is not defined hece setting it always valid */
-		psFlash2xBitMap->VSA1 |= FLASH2X_SECTION_VALID;
-
-		/* Checking For Access permission */
-		if (IsSectionWritable(Adapter, VSA1) == false)
-			psFlash2xBitMap->VSA1 |= FLASH2X_SECTION_RO;
-
-		/* By Default section is Active */
-		psFlash2xBitMap->VSA1 |= FLASH2X_SECTION_ACT;
-	}
-
-	/*
-	 * VSA 2
-	 */
-	if ((psFlash2xCSInfo->OffsetFromZeroForVSA2Start) != UNINIT_PTR_IN_CS) {
-		/* Setting the 0th Bit representing the Section is present or not. */
-		psFlash2xBitMap->VSA2 = psFlash2xBitMap->VSA2 | FLASH2X_SECTION_PRESENT;
-
-		/* Setting the Access Bit. Map is not defined hece setting it always valid */
-		psFlash2xBitMap->VSA2 |= FLASH2X_SECTION_VALID;
-
-		/* Checking For Access permission */
-		if (IsSectionWritable(Adapter, VSA2) == false)
-			psFlash2xBitMap->VSA2 |= FLASH2X_SECTION_RO;
-
-		/* By Default section is Active */
-		psFlash2xBitMap->VSA2 |= FLASH2X_SECTION_ACT;
-	}
-
-	/*
-	 * SCSI Section
-	 */
-	if ((psFlash2xCSInfo->OffsetFromZeroForScsiFirmware) != UNINIT_PTR_IN_CS) {
-		/* Setting the 0th Bit representing the Section is present or not. */
-		psFlash2xBitMap->SCSI = psFlash2xBitMap->SCSI | FLASH2X_SECTION_PRESENT;
-
-		/* Setting the Access Bit. Map is not defined hece setting it always valid */
-		psFlash2xBitMap->SCSI |= FLASH2X_SECTION_VALID;
-
-		/* Checking For Access permission */
-		if (IsSectionWritable(Adapter, SCSI) == false)
-			psFlash2xBitMap->SCSI |= FLASH2X_SECTION_RO;
-
-		/* By Default section is Active */
-		psFlash2xBitMap->SCSI |= FLASH2X_SECTION_ACT;
-	}
-
-	/*
-	 * Control Section
-	 */
-	if ((psFlash2xCSInfo->OffsetFromZeroForControlSectionStart) != UNINIT_PTR_IN_CS) {
-		/* Setting the 0th Bit representing the Section is present or not. */
-		psFlash2xBitMap->CONTROL_SECTION = psFlash2xBitMap->CONTROL_SECTION | (FLASH2X_SECTION_PRESENT);
-
-		/* Setting the Access Bit. Map is not defined hece setting it always valid */
-		psFlash2xBitMap->CONTROL_SECTION |= FLASH2X_SECTION_VALID;
-
-		/* Checking For Access permission */
-		if (IsSectionWritable(Adapter, CONTROL_SECTION) == false)
-			psFlash2xBitMap->CONTROL_SECTION |= FLASH2X_SECTION_RO;
-
-		/* By Default section is Active */
-		psFlash2xBitMap->CONTROL_SECTION |= FLASH2X_SECTION_ACT;
-	}
-
-	/*
-	 * For Reserved Sections
-	 */
-	psFlash2xBitMap->Reserved0 = 0;
-	psFlash2xBitMap->Reserved0 = 0;
-	psFlash2xBitMap->Reserved0 = 0;
-	BcmDumpFlash2xSectionBitMap(psFlash2xBitMap);
-
-	return STATUS_SUCCESS;
-}
-
-/*
- * BcmSetActiveSection :- Set Active section is used to make priority field highest over other
- * section of same type.
- *
- * @Adapater :- Bcm Driver Private Data Structure
- * @eFlash2xSectionVal :- Flash section val whose priority has to be made highest.
- *
- * Return Value:- Make the priorit highest else return erorr code
- *
- */
-
-int BcmSetActiveSection(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_section_val eFlash2xSectVal)
-{
-	unsigned int SectImagePriority = 0;
-	int Status = STATUS_SUCCESS;
-
-	/* struct bcm_dsd_header sDSD = {0};
-	 * struct bcm_iso_header sISO = {0};
-	 */
-	int HighestPriDSD = 0;
-	int HighestPriISO = 0;
-
-	Status = IsSectionWritable(Adapter, eFlash2xSectVal);
-	if (Status != TRUE) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Provided Section <%d> is not writable", eFlash2xSectVal);
-		return STATUS_FAILURE;
-	}
-
-	Adapter->bHeaderChangeAllowed = TRUE;
-	switch (eFlash2xSectVal) {
-	case ISO_IMAGE1:
-	case ISO_IMAGE2:
-		if (ReadISOSignature(Adapter, eFlash2xSectVal) == ISO_IMAGE_MAGIC_NUMBER) {
-			HighestPriISO = getHighestPriISO(Adapter);
-
-			if (HighestPriISO == eFlash2xSectVal) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Given ISO<%x> already has highest priority", eFlash2xSectVal);
-				Status = STATUS_SUCCESS;
-				break;
-			}
-
-			SectImagePriority = ReadISOPriority(Adapter, HighestPriISO) + 1;
-
-			if ((SectImagePriority == 0) && IsSectionWritable(Adapter, HighestPriISO)) {
-				/* This is a SPECIAL Case which will only happen if the current highest priority ISO has priority value = 0x7FFFFFFF.
-				 * We will write 1 to the current Highest priority ISO And then shall increase the priority of the requested ISO
-				 * by user
-				 */
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "SectImagePriority wraparound happened, eFlash2xSectVal: 0x%x\n", eFlash2xSectVal);
-				SectImagePriority = htonl(0x1);
-				Status = BcmFlash2xBulkWrite(Adapter,
-							&SectImagePriority,
-							HighestPriISO,
-							0 + FIELD_OFFSET_IN_HEADER(struct bcm_iso_header *, ISOImagePriority),
-							SIGNATURE_SIZE,
-							TRUE);
-				if (Status) {
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Priority has not been written properly");
-					Status = STATUS_FAILURE;
-					break;
-				}
-
-				HighestPriISO = getHighestPriISO(Adapter);
-
-				if (HighestPriISO == eFlash2xSectVal) {
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Given ISO<%x> already has highest priority", eFlash2xSectVal);
-					Status = STATUS_SUCCESS;
-					break;
-				}
-
-				SectImagePriority = 2;
-			}
-
-			SectImagePriority = htonl(SectImagePriority);
-
-			Status = BcmFlash2xBulkWrite(Adapter,
-						&SectImagePriority,
-						eFlash2xSectVal,
-						0 + FIELD_OFFSET_IN_HEADER(struct bcm_iso_header *, ISOImagePriority),
-						SIGNATURE_SIZE,
-						TRUE);
-			if (Status) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Priority has not been written properly");
-				break;
-			}
-		} else {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Signature is currupted. Hence can't increase the priority");
-			Status = STATUS_FAILURE;
-			break;
-		}
-		break;
-	case DSD0:
-	case DSD1:
-	case DSD2:
-		if (ReadDSDSignature(Adapter, eFlash2xSectVal) == DSD_IMAGE_MAGIC_NUMBER) {
-			HighestPriDSD = getHighestPriDSD(Adapter);
-			if (HighestPriDSD == eFlash2xSectVal) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Given DSD<%x> already has highest priority", eFlash2xSectVal);
-				Status = STATUS_SUCCESS;
-				break;
-			}
-
-			SectImagePriority = ReadDSDPriority(Adapter, HighestPriDSD) + 1;
-			if (SectImagePriority == 0) {
-				/* This is a SPECIAL Case which will only happen if the current highest priority DSD has priority value = 0x7FFFFFFF.
-				 * We will write 1 to the current Highest priority DSD And then shall increase the priority of the requested DSD
-				 * by user
-				 */
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, NVM_RW, DBG_LVL_ALL, "SectImagePriority wraparound happened, eFlash2xSectVal: 0x%x\n", eFlash2xSectVal);
-				SectImagePriority = htonl(0x1);
-
-				Status = BcmFlash2xBulkWrite(Adapter,
-							&SectImagePriority,
-							HighestPriDSD,
-							Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + FIELD_OFFSET_IN_HEADER(struct bcm_dsd_header *, DSDImagePriority),
-							SIGNATURE_SIZE,
-							TRUE);
-				if (Status) {
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Priority has not been written properly");
-					break;
-				}
-
-				HighestPriDSD = getHighestPriDSD(Adapter);
-
-				if (HighestPriDSD == eFlash2xSectVal) {
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Made the DSD: %x highest by reducing priority of other\n", eFlash2xSectVal);
-					Status = STATUS_SUCCESS;
-					break;
-				}
-
-				SectImagePriority = htonl(0x2);
-				Status = BcmFlash2xBulkWrite(Adapter,
-							&SectImagePriority,
-							HighestPriDSD,
-							Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + FIELD_OFFSET_IN_HEADER(struct bcm_dsd_header *, DSDImagePriority),
-							SIGNATURE_SIZE,
-							TRUE);
-				if (Status) {
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Priority has not been written properly");
-					break;
-				}
-
-				HighestPriDSD = getHighestPriDSD(Adapter);
-				if (HighestPriDSD == eFlash2xSectVal) {
-					Status = STATUS_SUCCESS;
-					break;
-				}
-
-				SectImagePriority = 3;
-			}
-			SectImagePriority = htonl(SectImagePriority);
-			Status = BcmFlash2xBulkWrite(Adapter,
-						&SectImagePriority,
-						eFlash2xSectVal,
-						Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + FIELD_OFFSET_IN_HEADER(struct bcm_dsd_header *, DSDImagePriority),
-						SIGNATURE_SIZE,
-						TRUE);
-			if (Status) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Priority has not been written properly");
-				Status = STATUS_FAILURE;
-				break;
-			}
-		} else {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Signature is currupted. Hence can't increase the priority");
-			Status = STATUS_FAILURE;
-			break;
-		}
-		break;
-	case VSA0:
-	case VSA1:
-	case VSA2:
-		/* Has to be decided */
-		break;
-	default:
-		Status = STATUS_FAILURE;
-		break;
-	}
-
-	Adapter->bHeaderChangeAllowed = false;
-	return Status;
-}
-
-/*
- * BcmCopyISO - Used only for copying the ISO section
- * @Adapater :- Bcm Driver Private Data Structure
- * @sCopySectStrut :- Section copy structure
- *
- * Return value:- SUCCESS if copies successfully else negative error code
- *
- */
-
-int BcmCopyISO(struct bcm_mini_adapter *Adapter, struct bcm_flash2x_copy_section sCopySectStrut)
-{
-	PCHAR Buff = NULL;
-	enum bcm_flash2x_section_val eISOReadPart = 0, eISOWritePart = 0;
-	unsigned int uiReadOffsetWithinPart = 0, uiWriteOffsetWithinPart = 0;
-	unsigned int uiTotalDataToCopy = 0;
-	bool IsThisHeaderSector = false;
-	unsigned int sigOffset = 0;
-	unsigned int ISOLength = 0;
-	unsigned int Status = STATUS_SUCCESS;
-	unsigned int SigBuff[MAX_RW_SIZE];
-	unsigned int i = 0;
-
-	if (ReadISOSignature(Adapter, sCopySectStrut.SrcSection) != ISO_IMAGE_MAGIC_NUMBER) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "error as Source ISO Section does not have valid signature");
-		return STATUS_FAILURE;
-	}
-
-	Status = BcmFlash2xBulkRead(Adapter, &ISOLength,
-				    sCopySectStrut.SrcSection,
-				    0 + FIELD_OFFSET_IN_HEADER(struct bcm_iso_header *, ISOImageSize),
-				    4);
-	if (Status) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Read failed while copying ISO\n");
-		return Status;
-	}
-
-	ISOLength = htonl(ISOLength);
-	if (ISOLength % Adapter->uiSectorSize)
-		ISOLength = Adapter->uiSectorSize * (1 + ISOLength/Adapter->uiSectorSize);
-
-	sigOffset = FIELD_OFFSET_IN_HEADER(struct bcm_iso_header *, ISOImageMagicNumber);
-
-	Buff = kzalloc(Adapter->uiSectorSize, GFP_KERNEL);
-
-	if (!Buff) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Memory allocation failed for section size");
-		return -ENOMEM;
-	}
-
-	if (sCopySectStrut.SrcSection == ISO_IMAGE1 && sCopySectStrut.DstSection == ISO_IMAGE2) {
-		eISOReadPart = ISO_IMAGE1;
-		eISOWritePart = ISO_IMAGE2;
-		uiReadOffsetWithinPart =  0;
-		uiWriteOffsetWithinPart = 0;
-
-		uiTotalDataToCopy = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1End) -
-			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start) +
-			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part2End) -
-			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part2Start) +
-			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part3End) -
-			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part3Start);
-
-		if (uiTotalDataToCopy < ISOLength) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "error as Source ISO Section does not have valid signature");
-			Status = STATUS_FAILURE;
-			goto out;
-		}
-
-		uiTotalDataToCopy = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1End) -
-			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start) +
-			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part2End) -
-			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part2Start) +
-			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part3End) -
-			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part3Start);
-
-		if (uiTotalDataToCopy < ISOLength) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "error as Dest ISO Section does not have enough section size");
-			Status = STATUS_FAILURE;
-			goto out;
-		}
-
-		uiTotalDataToCopy = ISOLength;
-
-		CorruptISOSig(Adapter, ISO_IMAGE2);
-		while (uiTotalDataToCopy) {
-			if (uiTotalDataToCopy == Adapter->uiSectorSize) {
-				/* Setting for write of first sector. First sector is assumed to be written in last */
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Writing the signature sector");
-				eISOReadPart = ISO_IMAGE1;
-				uiReadOffsetWithinPart = 0;
-				eISOWritePart = ISO_IMAGE2;
-				uiWriteOffsetWithinPart = 0;
-				IsThisHeaderSector = TRUE;
-			} else {
-				uiReadOffsetWithinPart = uiReadOffsetWithinPart + Adapter->uiSectorSize;
-				uiWriteOffsetWithinPart = uiWriteOffsetWithinPart + Adapter->uiSectorSize;
-
-				if ((eISOReadPart == ISO_IMAGE1) && (uiReadOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1End - Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start))) {
-					eISOReadPart = ISO_IMAGE1_PART2;
-					uiReadOffsetWithinPart = 0;
-				}
-
-				if ((eISOReadPart == ISO_IMAGE1_PART2) && (uiReadOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2End - Adapter->psFlash2xCSInfo->OffsetISOImage1Part2Start))) {
-					eISOReadPart = ISO_IMAGE1_PART3;
-					uiReadOffsetWithinPart = 0;
-				}
-
-				if ((eISOWritePart == ISO_IMAGE2) && (uiWriteOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1End - Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start))) {
-					eISOWritePart = ISO_IMAGE2_PART2;
-					uiWriteOffsetWithinPart = 0;
-				}
-
-				if ((eISOWritePart == ISO_IMAGE2_PART2) && (uiWriteOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2End - Adapter->psFlash2xCSInfo->OffsetISOImage2Part2Start))) {
-					eISOWritePart = ISO_IMAGE2_PART3;
-					uiWriteOffsetWithinPart = 0;
-				}
-			}
-
-			Status = BcmFlash2xBulkRead(Adapter,
-						(PUINT)Buff,
-						eISOReadPart,
-						uiReadOffsetWithinPart,
-						Adapter->uiSectorSize);
-			if (Status) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Read failed while copying ISO: Part: %x, OffsetWithinPart: %x\n", eISOReadPart, uiReadOffsetWithinPart);
-				break;
-			}
-
-			if (IsThisHeaderSector == TRUE) {
-				/* If this is header sector write 0xFFFFFFFF at the sig time and in last write sig */
-				memcpy(SigBuff, Buff + sigOffset, sizeof(SigBuff));
-
-				for (i = 0; i < MAX_RW_SIZE; i++)
-					*(Buff + sigOffset + i) = 0xFF;
-			}
-			Adapter->bHeaderChangeAllowed = TRUE;
-			Status = BcmFlash2xBulkWrite(Adapter,
-						(PUINT)Buff,
-						eISOWritePart,
-						uiWriteOffsetWithinPart,
-						Adapter->uiSectorSize,
-						TRUE);
-			if (Status) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Write failed while copying ISO: Part: %x, OffsetWithinPart: %x\n", eISOWritePart, uiWriteOffsetWithinPart);
-				break;
-			}
-
-			Adapter->bHeaderChangeAllowed = false;
-			if (IsThisHeaderSector == TRUE) {
-				WriteToFlashWithoutSectorErase(Adapter,
-							SigBuff,
-							eISOWritePart,
-							sigOffset,
-							MAX_RW_SIZE);
-				IsThisHeaderSector = false;
-			}
-			/* subtracting the written Data */
-			uiTotalDataToCopy = uiTotalDataToCopy - Adapter->uiSectorSize;
-		}
-	}
-
-	if (sCopySectStrut.SrcSection == ISO_IMAGE2 && sCopySectStrut.DstSection == ISO_IMAGE1) {
-		eISOReadPart = ISO_IMAGE2;
-		eISOWritePart = ISO_IMAGE1;
-		uiReadOffsetWithinPart = 0;
-		uiWriteOffsetWithinPart = 0;
-
-		uiTotalDataToCopy = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1End) -
-			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start) +
-			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part2End) -
-			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part2Start) +
-			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part3End) -
-			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part3Start);
-
-		if (uiTotalDataToCopy < ISOLength) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "error as Source ISO Section does not have valid signature");
-			Status = STATUS_FAILURE;
-			goto out;
-		}
-
-		uiTotalDataToCopy = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1End) -
-			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start) +
-			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part2End) -
-			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part2Start) +
-			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part3End) -
-			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part3Start);
-
-		if (uiTotalDataToCopy < ISOLength) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "error as Dest ISO Section does not have enough section size");
-			Status = STATUS_FAILURE;
-			goto out;
-		}
-
-		uiTotalDataToCopy = ISOLength;
-
-		CorruptISOSig(Adapter, ISO_IMAGE1);
-
-		while (uiTotalDataToCopy) {
-			if (uiTotalDataToCopy == Adapter->uiSectorSize) {
-				/* Setting for write of first sector. First sector is assumed to be written in last */
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Writing the signature sector");
-				eISOReadPart = ISO_IMAGE2;
-				uiReadOffsetWithinPart = 0;
-				eISOWritePart = ISO_IMAGE1;
-				uiWriteOffsetWithinPart = 0;
-				IsThisHeaderSector = TRUE;
-			} else {
-				uiReadOffsetWithinPart = uiReadOffsetWithinPart + Adapter->uiSectorSize;
-				uiWriteOffsetWithinPart = uiWriteOffsetWithinPart + Adapter->uiSectorSize;
-
-				if ((eISOReadPart == ISO_IMAGE2) && (uiReadOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1End - Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start))) {
-					eISOReadPart = ISO_IMAGE2_PART2;
-					uiReadOffsetWithinPart = 0;
-				}
-
-				if ((eISOReadPart == ISO_IMAGE2_PART2) && (uiReadOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2End - Adapter->psFlash2xCSInfo->OffsetISOImage2Part2Start))) {
-					eISOReadPart = ISO_IMAGE2_PART3;
-					uiReadOffsetWithinPart = 0;
-				}
-
-				if ((eISOWritePart == ISO_IMAGE1) && (uiWriteOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1End - Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start))) {
-					eISOWritePart = ISO_IMAGE1_PART2;
-					uiWriteOffsetWithinPart = 0;
-				}
-
-				if ((eISOWritePart == ISO_IMAGE1_PART2) && (uiWriteOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2End - Adapter->psFlash2xCSInfo->OffsetISOImage1Part2Start))) {
-					eISOWritePart = ISO_IMAGE1_PART3;
-					uiWriteOffsetWithinPart = 0;
-				}
-			}
-
-			Status = BcmFlash2xBulkRead(Adapter,
-						(PUINT)Buff,
-						eISOReadPart,
-						uiReadOffsetWithinPart,
-						Adapter->uiSectorSize);
-			if (Status) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Read failed while copying ISO: Part: %x, OffsetWithinPart: %x\n", eISOReadPart, uiReadOffsetWithinPart);
-				break;
-			}
-
-			if (IsThisHeaderSector == TRUE) {
-				/* If this is header sector write 0xFFFFFFFF at the sig time and in last write sig */
-				memcpy(SigBuff, Buff + sigOffset, sizeof(SigBuff));
-
-				for (i = 0; i < MAX_RW_SIZE; i++)
-					*(Buff + sigOffset + i) = 0xFF;
-			}
-			Adapter->bHeaderChangeAllowed = TRUE;
-			Status = BcmFlash2xBulkWrite(Adapter,
-						(PUINT)Buff,
-						eISOWritePart,
-						uiWriteOffsetWithinPart,
-						Adapter->uiSectorSize,
-						TRUE);
-			if (Status) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Write failed while copying ISO: Part: %x, OffsetWithinPart: %x\n", eISOWritePart, uiWriteOffsetWithinPart);
-				break;
-			}
-
-			Adapter->bHeaderChangeAllowed = false;
-			if (IsThisHeaderSector == TRUE) {
-				WriteToFlashWithoutSectorErase(Adapter,
-							SigBuff,
-							eISOWritePart,
-							sigOffset,
-							MAX_RW_SIZE);
-
-				IsThisHeaderSector = false;
-			}
-
-			/* subtracting the written Data */
-			uiTotalDataToCopy = uiTotalDataToCopy - Adapter->uiSectorSize;
-		}
-	}
-out:
-	kfree(Buff);
-
-	return Status;
-}
-
-/*
- * BcmFlash2xCorruptSig : this API is used to corrupt the written sig in Bcm Header present in flash section.
- * It will corrupt the sig, if Section is writable, by making first bytes as zero.
- * @Adapater :- Bcm Driver Private Data Structure
- * @eFlash2xSectionVal :- Flash section val which has header
- *
- * Return Value :-
- *	Success :- If Section is present and writable, corrupt the sig and return STATUS_SUCCESS
- *	Failure :-Return negative error code
- */
-
-int BcmFlash2xCorruptSig(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_section_val eFlash2xSectionVal)
-{
-	int Status = STATUS_SUCCESS;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Section Value :%x\n", eFlash2xSectionVal);
-
-	if ((eFlash2xSectionVal == DSD0) || (eFlash2xSectionVal == DSD1) || (eFlash2xSectionVal == DSD2)) {
-		Status = CorruptDSDSig(Adapter, eFlash2xSectionVal);
-	} else if (eFlash2xSectionVal == ISO_IMAGE1 || eFlash2xSectionVal == ISO_IMAGE2) {
-		Status = CorruptISOSig(Adapter, eFlash2xSectionVal);
-	} else {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Given Section <%d>does not have Header", eFlash2xSectionVal);
-		return STATUS_SUCCESS;
-	}
-	return Status;
-}
-
-/*
- *BcmFlash2xWriteSig :-this API is used to Write the sig if requested Section has
- *					  header and  Write Permission.
- * @Adapater :- Bcm Driver Private Data Structure
- * @eFlashSectionVal :- Flash section val which has header
- *
- * Return Value :-
- *	Success :- If Section is present and writable write the sig and return STATUS_SUCCESS
- *	Failure :-Return negative error code
- */
-
-int BcmFlash2xWriteSig(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_section_val eFlashSectionVal)
-{
-	unsigned int uiSignature = 0;
-	unsigned int uiOffset = 0;
-
-	/* struct bcm_dsd_header dsdHeader = {0}; */
-	if (Adapter->bSigCorrupted == false) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Signature is not corrupted by driver, hence not restoring\n");
-		return STATUS_SUCCESS;
-	}
-
-	if (Adapter->bAllDSDWriteAllow == false) {
-		if (IsSectionWritable(Adapter, eFlashSectionVal) == false) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Section is not Writable...Hence can't Write signature");
-			return SECTOR_IS_NOT_WRITABLE;
-		}
-	}
-
-	if ((eFlashSectionVal == DSD0) || (eFlashSectionVal == DSD1) || (eFlashSectionVal == DSD2)) {
-		uiSignature = htonl(DSD_IMAGE_MAGIC_NUMBER);
-		uiOffset = Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader;
-
-		uiOffset += FIELD_OFFSET_IN_HEADER(struct bcm_dsd_header *, DSDImageMagicNumber);
-
-		if ((ReadDSDSignature(Adapter, eFlashSectionVal) & 0xFF000000) != CORRUPTED_PATTERN) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Corrupted Pattern is not there. Hence won't write sig");
-			return STATUS_FAILURE;
-		}
-	} else if ((eFlashSectionVal == ISO_IMAGE1) || (eFlashSectionVal == ISO_IMAGE2)) {
-		uiSignature = htonl(ISO_IMAGE_MAGIC_NUMBER);
-		/* uiOffset = 0; */
-		uiOffset = FIELD_OFFSET_IN_HEADER(struct bcm_iso_header *, ISOImageMagicNumber);
-		if ((ReadISOSignature(Adapter, eFlashSectionVal) & 0xFF000000) != CORRUPTED_PATTERN) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Currupted Pattern is not there. Hence won't write sig");
-			return STATUS_FAILURE;
-		}
-	} else {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "GIVEN SECTION< %d > IS NOT VALID FOR SIG WRITE...", eFlashSectionVal);
-		return STATUS_FAILURE;
-	}
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Restoring the signature");
-
-	Adapter->bHeaderChangeAllowed = TRUE;
-	Adapter->bSigCorrupted = false;
-	BcmFlash2xBulkWrite(Adapter, &uiSignature, eFlashSectionVal, uiOffset, SIGNATURE_SIZE, TRUE);
-	Adapter->bHeaderChangeAllowed = false;
-
-	return STATUS_SUCCESS;
-}
-
-/*
- * validateFlash2xReadWrite :- This API is used to validate the user request for Read/Write.
- *						      if requested Bytes goes beyond the Requested section, it reports error.
- * @Adapater :- Bcm Driver Private Data Structure
- * @psFlash2xReadWrite :-Flash2x Read/write structure pointer
- *
- * Return values:-Return TRUE is request is valid else false.
- */
-
-int validateFlash2xReadWrite(struct bcm_mini_adapter *Adapter, struct bcm_flash2x_readwrite *psFlash2xReadWrite)
-{
-	unsigned int uiNumOfBytes = 0;
-	unsigned int uiSectStartOffset = 0;
-	unsigned int uiSectEndOffset = 0;
-
-	uiNumOfBytes = psFlash2xReadWrite->numOfBytes;
-
-	if (IsSectionExistInFlash(Adapter, psFlash2xReadWrite->Section) != TRUE) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Section<%x> does not exist in Flash", psFlash2xReadWrite->Section);
-		return false;
-	}
-	uiSectStartOffset = BcmGetSectionValStartOffset(Adapter, psFlash2xReadWrite->Section);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Start offset :%x ,section :%d\n", uiSectStartOffset, psFlash2xReadWrite->Section);
-	if ((psFlash2xReadWrite->Section == ISO_IMAGE1) || (psFlash2xReadWrite->Section == ISO_IMAGE2)) {
-		if (psFlash2xReadWrite->Section == ISO_IMAGE1) {
-			uiSectEndOffset = BcmGetSectionValEndOffset(Adapter, ISO_IMAGE1) -
-				BcmGetSectionValStartOffset(Adapter, ISO_IMAGE1) +
-				BcmGetSectionValEndOffset(Adapter, ISO_IMAGE1_PART2) -
-				BcmGetSectionValStartOffset(Adapter, ISO_IMAGE1_PART2) +
-				BcmGetSectionValEndOffset(Adapter, ISO_IMAGE1_PART3) -
-				BcmGetSectionValStartOffset(Adapter, ISO_IMAGE1_PART3);
-		} else if (psFlash2xReadWrite->Section == ISO_IMAGE2) {
-			uiSectEndOffset = BcmGetSectionValEndOffset(Adapter, ISO_IMAGE2) -
-				BcmGetSectionValStartOffset(Adapter, ISO_IMAGE2) +
-				BcmGetSectionValEndOffset(Adapter, ISO_IMAGE2_PART2) -
-				BcmGetSectionValStartOffset(Adapter, ISO_IMAGE2_PART2) +
-				BcmGetSectionValEndOffset(Adapter, ISO_IMAGE2_PART3) -
-				BcmGetSectionValStartOffset(Adapter, ISO_IMAGE2_PART3);
-		}
-
-		/* since this uiSectEndoffset is the size of iso Image. hence for calculating the virtual endoffset
-		 * it should be added in startoffset. so that check done in last of this function can be valued.
-		 */
-		uiSectEndOffset = uiSectStartOffset + uiSectEndOffset;
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Total size of the ISO Image :%x", uiSectEndOffset);
-	} else
-		uiSectEndOffset = BcmGetSectionValEndOffset(Adapter, psFlash2xReadWrite->Section);
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "End offset :%x\n", uiSectEndOffset);
-
-	/* psFlash2xReadWrite->offset and uiNumOfBytes are user controlled and can lead to integer overflows */
-	if (psFlash2xReadWrite->offset > uiSectEndOffset) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Invalid Request....");
-		return false;
-	}
-	if (uiNumOfBytes > uiSectEndOffset) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Invalid Request....");
-		return false;
-	}
-	/* Checking the boundary condition */
-	if ((uiSectStartOffset + psFlash2xReadWrite->offset + uiNumOfBytes) <= uiSectEndOffset)
-		return TRUE;
-	else {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Invalid Request....");
-		return false;
-	}
-}
-
-/*
- * IsFlash2x :- check for Flash 2.x
- * Adapater :- Bcm Driver Private Data Structure
- *
- * Return value:-
- *	return TRUE if flah2.x of hgher version else return false.
- */
-
-int IsFlash2x(struct bcm_mini_adapter *Adapter)
-{
-	if (Adapter->uiFlashLayoutMajorVersion >= FLASH_2X_MAJOR_NUMBER)
-		return TRUE;
-	else
-		return false;
-}
-
-/*
- * GetFlashBaseAddr :- Calculate the Flash Base address
- * @Adapater :- Bcm Driver Private Data Structure
- *
- * Return Value:-
- *	Success :- Base Address of the Flash
- */
-
-static int GetFlashBaseAddr(struct bcm_mini_adapter *Adapter)
-{
-	unsigned int uiBaseAddr = 0;
-
-	if (Adapter->bDDRInitDone) {
-		/*
-		 * For All Valid Flash Versions... except 1.1, take the value from FlashBaseAddr
-		 * In case of Raw Read... use the default value
-		 */
-		if (Adapter->uiFlashLayoutMajorVersion && (Adapter->bFlashRawRead == false) &&
-			!((Adapter->uiFlashLayoutMajorVersion == 1) && (Adapter->uiFlashLayoutMinorVersion == 1)))
-			uiBaseAddr = Adapter->uiFlashBaseAdd;
-		else
-			uiBaseAddr = FLASH_CONTIGIOUS_START_ADDR_AFTER_INIT;
-	} else {
-		/*
-		 * For All Valid Flash Versions... except 1.1, take the value from FlashBaseAddr
-		 * In case of Raw Read... use the default value
-		 */
-		if (Adapter->uiFlashLayoutMajorVersion && (Adapter->bFlashRawRead == false) &&
-			!((Adapter->uiFlashLayoutMajorVersion == 1) && (Adapter->uiFlashLayoutMinorVersion == 1)))
-			uiBaseAddr = Adapter->uiFlashBaseAdd | FLASH_CONTIGIOUS_START_ADDR_BEFORE_INIT;
-		else
-			uiBaseAddr = FLASH_CONTIGIOUS_START_ADDR_BEFORE_INIT;
-	}
-
-	return uiBaseAddr;
-}
-
-/*
- * BcmCopySection :- This API is used to copy the One section in another. Both section should
- *				    be contiuous and of same size. Hence this Will not be applicabe to copy ISO.
- *
- * @Adapater :- Bcm Driver Private Data Structure
- * @SrcSection :- Source section From where data has to be copied
- * @DstSection :- Destination section to which data has to be copied
- * @offset :- Offset from/to  where data has to be copied from one section to another.
- * @numOfBytes :- number of byes that has to be copyed from one section to another at given offset.
- *			     in case of numofBytes  equal zero complete section will be copied.
- * Return Values-
- *	Success : Return STATUS_SUCCESS
- *	Faillure :- return negative error code
- */
-
-int BcmCopySection(struct bcm_mini_adapter *Adapter,
-		enum bcm_flash2x_section_val SrcSection,
-		enum bcm_flash2x_section_val DstSection,
-		unsigned int offset,
-		unsigned int numOfBytes)
-{
-	unsigned int BuffSize = 0;
-	unsigned int BytesToBeCopied = 0;
-	PUCHAR pBuff = NULL;
-	int Status = STATUS_SUCCESS;
-
-	if (SrcSection == DstSection) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Source and Destination should be different ...try again");
-		return -EINVAL;
-	}
-
-	if ((SrcSection != DSD0) && (SrcSection != DSD1) && (SrcSection != DSD2)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Source should be DSD subsection");
-		return -EINVAL;
-	}
-
-	if ((DstSection != DSD0) && (DstSection != DSD1) && (DstSection != DSD2)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Destination should be DSD subsection");
-		return -EINVAL;
-	}
-
-	/* if offset zero means have to copy complete secton */
-	if (numOfBytes == 0) {
-		numOfBytes = BcmGetSectionValEndOffset(Adapter, SrcSection)
-			- BcmGetSectionValStartOffset(Adapter, SrcSection);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Section Size :0x%x", numOfBytes);
-	}
-
-	if ((offset + numOfBytes) > BcmGetSectionValEndOffset(Adapter, SrcSection)
-		- BcmGetSectionValStartOffset(Adapter, SrcSection)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, " Input parameters going beyond the section offS: %x numB: %x of Source Section\n",
-				offset, numOfBytes);
-		return -EINVAL;
-	}
-
-	if ((offset + numOfBytes) > BcmGetSectionValEndOffset(Adapter, DstSection)
-		- BcmGetSectionValStartOffset(Adapter, DstSection)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Input parameters going beyond the section offS: %x numB: %x of Destination Section\n",
-				offset, numOfBytes);
-		return -EINVAL;
-	}
-
-	if (numOfBytes > Adapter->uiSectorSize)
-		BuffSize = Adapter->uiSectorSize;
-	else
-		BuffSize = numOfBytes;
-
-	pBuff = kzalloc(BuffSize, GFP_KERNEL);
-	if (!pBuff) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Memory allocation failed.. ");
-		return -ENOMEM;
-	}
-
-	BytesToBeCopied = Adapter->uiSectorSize;
-	if (offset % Adapter->uiSectorSize)
-		BytesToBeCopied = Adapter->uiSectorSize - (offset % Adapter->uiSectorSize);
-	if (BytesToBeCopied > numOfBytes)
-		BytesToBeCopied = numOfBytes;
-
-	Adapter->bHeaderChangeAllowed = TRUE;
-
-	do {
-		Status = BcmFlash2xBulkRead(Adapter, (PUINT)pBuff, SrcSection , offset, BytesToBeCopied);
-		if (Status) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Read failed at offset :%d for NOB :%d", SrcSection, BytesToBeCopied);
-			break;
-		}
-		Status = BcmFlash2xBulkWrite(Adapter, (PUINT)pBuff, DstSection, offset, BytesToBeCopied, false);
-		if (Status) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Write failed at offset :%d for NOB :%d", DstSection, BytesToBeCopied);
-			break;
-		}
-		offset = offset + BytesToBeCopied;
-		numOfBytes = numOfBytes - BytesToBeCopied;
-		if (numOfBytes) {
-			if (numOfBytes > Adapter->uiSectorSize)
-				BytesToBeCopied = Adapter->uiSectorSize;
-			else
-				BytesToBeCopied = numOfBytes;
-		}
-	} while (numOfBytes > 0);
-
-	kfree(pBuff);
-	Adapter->bHeaderChangeAllowed = false;
-
-	return Status;
-}
-
-/*
- * SaveHeaderIfPresent :- This API is use to Protect the Header in case of Header Sector write
- * @Adapater :- Bcm Driver Private Data Structure
- * @pBuff :- Data buffer that has to be written in sector having the header map.
- * @uiOffset :- Flash offset that has to be written.
- *
- * Return value :-
- *	Success :- On success return STATUS_SUCCESS
- *	Faillure :- Return negative error code
- */
-
-static int SaveHeaderIfPresent(struct bcm_mini_adapter *Adapter, PUCHAR pBuff, unsigned int uiOffset)
-{
-	unsigned int offsetToProtect = 0, HeaderSizeToProtect = 0;
-	bool bHasHeader = false;
-	PUCHAR pTempBuff = NULL;
-	unsigned int uiSectAlignAddr = 0;
-	unsigned int sig = 0;
-
-	/* making the offset sector aligned */
-	uiSectAlignAddr = uiOffset & ~(Adapter->uiSectorSize - 1);
-
-	if ((uiSectAlignAddr == BcmGetSectionValEndOffset(Adapter, DSD2) - Adapter->uiSectorSize) ||
-		(uiSectAlignAddr == BcmGetSectionValEndOffset(Adapter, DSD1) - Adapter->uiSectorSize) ||
-		(uiSectAlignAddr == BcmGetSectionValEndOffset(Adapter, DSD0) - Adapter->uiSectorSize)) {
-		/* offset from the sector boundary having the header map */
-		offsetToProtect = Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader % Adapter->uiSectorSize;
-		HeaderSizeToProtect = sizeof(struct bcm_dsd_header);
-		bHasHeader = TRUE;
-	}
-
-	if (uiSectAlignAddr == BcmGetSectionValStartOffset(Adapter, ISO_IMAGE1) ||
-		uiSectAlignAddr == BcmGetSectionValStartOffset(Adapter, ISO_IMAGE2)) {
-		offsetToProtect = 0;
-		HeaderSizeToProtect = sizeof(struct bcm_iso_header);
-		bHasHeader = TRUE;
-	}
-	/* If Header is present overwrite passed buffer with this */
-	if (bHasHeader && (Adapter->bHeaderChangeAllowed == false)) {
-		pTempBuff = kzalloc(HeaderSizeToProtect, GFP_KERNEL);
-		if (!pTempBuff) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Memory allocation failed");
-			return -ENOMEM;
-		}
-		/* Read header */
-		BeceemFlashBulkRead(Adapter, (PUINT)pTempBuff, (uiSectAlignAddr + offsetToProtect), HeaderSizeToProtect);
-		BCM_DEBUG_PRINT_BUFFER(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, pTempBuff, HeaderSizeToProtect);
-		/* Replace Buffer content with Header */
-		memcpy(pBuff + offsetToProtect, pTempBuff, HeaderSizeToProtect);
-
-		kfree(pTempBuff);
-	}
-	if (bHasHeader && Adapter->bSigCorrupted) {
-		sig = *((PUINT)(pBuff + offsetToProtect + FIELD_OFFSET_IN_HEADER(struct bcm_dsd_header *, DSDImageMagicNumber)));
-		sig = ntohl(sig);
-		if ((sig & 0xFF000000) != CORRUPTED_PATTERN) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Desired pattern is not at sig offset. Hence won't restore");
-			Adapter->bSigCorrupted = false;
-			return STATUS_SUCCESS;
-		}
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, " Corrupted sig is :%X", sig);
-		*((PUINT)(pBuff + offsetToProtect + FIELD_OFFSET_IN_HEADER(struct bcm_dsd_header *, DSDImageMagicNumber))) = htonl(DSD_IMAGE_MAGIC_NUMBER);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Restoring the signature in Header Write only");
-		Adapter->bSigCorrupted = false;
-	}
-
-	return STATUS_SUCCESS;
-}
-
-/*
- * BcmDoChipSelect : This will selcet the appropriate chip for writing.
- * @Adapater :- Bcm Driver Private Data Structure
- *
- * OutPut:-
- *	Select the Appropriate chip and retrn status Success
- */
-static int BcmDoChipSelect(struct bcm_mini_adapter *Adapter, unsigned int offset)
-{
-	unsigned int FlashConfig = 0;
-	int ChipNum = 0;
-	unsigned int GPIOConfig = 0;
-	unsigned int PartNum = 0;
-
-	ChipNum = offset / FLASH_PART_SIZE;
-
-	/*
-	 * Chip Select mapping to enable flash0.
-	 * To select flash 0, we have to OR with (0<<12).
-	 * ORing 0 will have no impact so not doing that part.
-	 * In future if Chip select value changes from 0 to non zero,
-	 * That needs be taken care with backward comaptibility. No worries for now.
-	 */
-
-	/*
-	 * SelectedChip Variable is the selection that the host is 100% Sure the same as what the register will hold. This can be ONLY ensured
-	 * if the Chip doesn't goes to low power mode while the flash operation is in progress (NVMRdmWrmLock is taken)
-	 * Before every new Flash Write operation, we reset the variable. This is to ensure that after any wake-up from
-	 * power down modes (Idle mode/shutdown mode), the values in the register will be different.
-	 */
-
-	if (Adapter->SelectedChip == ChipNum)
-		return STATUS_SUCCESS;
-
-	/* BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Selected Chip :%x", ChipNum); */
-	Adapter->SelectedChip = ChipNum;
-
-	/* bit[13..12]  will select the appropriate chip */
-	rdmalt(Adapter, FLASH_CONFIG_REG, &FlashConfig, 4);
-	rdmalt(Adapter, FLASH_GPIO_CONFIG_REG, &GPIOConfig, 4);
-	{
-		switch (ChipNum) {
-		case 0:
-			PartNum = 0;
-			break;
-		case 1:
-			PartNum = 3;
-			GPIOConfig |= (0x4 << CHIP_SELECT_BIT12);
-			break;
-		case 2:
-			PartNum = 1;
-			GPIOConfig |= (0x1 << CHIP_SELECT_BIT12);
-			break;
-		case 3:
-			PartNum = 2;
-			GPIOConfig |= (0x2 << CHIP_SELECT_BIT12);
-			break;
-		}
-	}
-	/* In case the bits already written in the FLASH_CONFIG_REG is same as what the user desired,
-	 * nothing to do... can return immediately.
-	 * ASSUMPTION: FLASH_GPIO_CONFIG_REG will be in sync with FLASH_CONFIG_REG.
-	 * Even if the chip goes to low power mode, it should wake with values in each register in sync with each other.
-	 * These values are not written by host other than during CHIP_SELECT.
-	 */
-	if (PartNum == ((FlashConfig >> CHIP_SELECT_BIT12) & 0x3))
-		return STATUS_SUCCESS;
-
-	/* clearing the bit[13..12] */
-	FlashConfig &= 0xFFFFCFFF;
-	FlashConfig = (FlashConfig | (PartNum<<CHIP_SELECT_BIT12)); /* 00 */
-
-	wrmalt(Adapter, FLASH_GPIO_CONFIG_REG, &GPIOConfig, 4);
-	udelay(100);
-
-	wrmalt(Adapter, FLASH_CONFIG_REG, &FlashConfig, 4);
-	udelay(100);
-
-	return STATUS_SUCCESS;
-}
-
-static int ReadDSDSignature(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_section_val dsd)
-{
-	unsigned int uiDSDsig = 0;
-	/* unsigned int sigoffsetInMap = 0;
-	 * struct bcm_dsd_header dsdHeader = {0};
-	 */
-
-	/* sigoffsetInMap =(PUCHAR)&(dsdHeader.DSDImageMagicNumber) -(PUCHAR)&dsdHeader; */
-
-	if (dsd != DSD0 && dsd != DSD1 && dsd != DSD2) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "passed section value is not for DSDs");
-		return STATUS_FAILURE;
-	}
-	BcmFlash2xBulkRead(Adapter,
-			&uiDSDsig,
-			dsd,
-			Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + FIELD_OFFSET_IN_HEADER(struct bcm_dsd_header *, DSDImageMagicNumber),
-			SIGNATURE_SIZE);
-
-	uiDSDsig = ntohl(uiDSDsig);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "DSD SIG :%x", uiDSDsig);
-
-	return uiDSDsig;
-}
-
-static int ReadDSDPriority(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_section_val dsd)
-{
-	/* unsigned int priOffsetInMap = 0 ; */
-	unsigned int uiDSDPri = STATUS_FAILURE;
-	/* struct bcm_dsd_header dsdHeader = {0};
-	 * priOffsetInMap = (PUCHAR)&(dsdHeader.DSDImagePriority) -(PUCHAR)&dsdHeader;
-	 */
-	if (IsSectionWritable(Adapter, dsd)) {
-		if (ReadDSDSignature(Adapter, dsd) == DSD_IMAGE_MAGIC_NUMBER) {
-			BcmFlash2xBulkRead(Adapter,
-					&uiDSDPri,
-					dsd,
-					Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + FIELD_OFFSET_IN_HEADER(struct bcm_dsd_header *, DSDImagePriority),
-					4);
-
-			uiDSDPri = ntohl(uiDSDPri);
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "DSD<%x> Priority :%x", dsd, uiDSDPri);
-		}
-	}
-
-	return uiDSDPri;
-}
-
-static enum bcm_flash2x_section_val getHighestPriDSD(struct bcm_mini_adapter *Adapter)
-{
-	int DSDHighestPri = STATUS_FAILURE;
-	int DsdPri = 0;
-	enum bcm_flash2x_section_val HighestPriDSD = 0;
-
-	if (IsSectionWritable(Adapter, DSD2)) {
-		DSDHighestPri = ReadDSDPriority(Adapter, DSD2);
-		HighestPriDSD = DSD2;
-	}
-
-	if (IsSectionWritable(Adapter, DSD1)) {
-		DsdPri = ReadDSDPriority(Adapter, DSD1);
-		if (DSDHighestPri  < DsdPri) {
-			DSDHighestPri = DsdPri;
-			HighestPriDSD = DSD1;
-		}
-	}
-
-	if (IsSectionWritable(Adapter, DSD0)) {
-		DsdPri = ReadDSDPriority(Adapter, DSD0);
-		if (DSDHighestPri  < DsdPri) {
-			DSDHighestPri = DsdPri;
-			HighestPriDSD = DSD0;
-		}
-	}
-	if (HighestPriDSD)
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Highest DSD :%x , and its  Pri :%x", HighestPriDSD, DSDHighestPri);
-
-	return  HighestPriDSD;
-}
-
-static int ReadISOSignature(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_section_val iso)
-{
-	unsigned int uiISOsig = 0;
-	/* unsigned int sigoffsetInMap = 0;
-	 * struct bcm_iso_header ISOHeader = {0};
-	 * sigoffsetInMap =(PUCHAR)&(ISOHeader.ISOImageMagicNumber) -(PUCHAR)&ISOHeader;
-	 */
-	if (iso != ISO_IMAGE1 && iso != ISO_IMAGE2) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "passed section value is not for ISOs");
-		return STATUS_FAILURE;
-	}
-	BcmFlash2xBulkRead(Adapter,
-			&uiISOsig,
-			iso,
-			0 + FIELD_OFFSET_IN_HEADER(struct bcm_iso_header *, ISOImageMagicNumber),
-			SIGNATURE_SIZE);
-
-	uiISOsig = ntohl(uiISOsig);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "ISO SIG :%x", uiISOsig);
-
-	return uiISOsig;
-}
-
-static int ReadISOPriority(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_section_val iso)
-{
-	unsigned int ISOPri = STATUS_FAILURE;
-
-	if (IsSectionWritable(Adapter, iso)) {
-		if (ReadISOSignature(Adapter, iso) == ISO_IMAGE_MAGIC_NUMBER) {
-			BcmFlash2xBulkRead(Adapter,
-					&ISOPri,
-					iso,
-					0 + FIELD_OFFSET_IN_HEADER(struct bcm_iso_header *, ISOImagePriority),
-					4);
-
-			ISOPri = ntohl(ISOPri);
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "ISO<%x> Priority :%x", iso, ISOPri);
-		}
-	}
-
-	return ISOPri;
-}
-
-static enum bcm_flash2x_section_val getHighestPriISO(struct bcm_mini_adapter *Adapter)
-{
-	int ISOHighestPri = STATUS_FAILURE;
-	int ISOPri = 0;
-	enum bcm_flash2x_section_val HighestPriISO = NO_SECTION_VAL;
-
-	if (IsSectionWritable(Adapter, ISO_IMAGE2)) {
-		ISOHighestPri = ReadISOPriority(Adapter, ISO_IMAGE2);
-		HighestPriISO = ISO_IMAGE2;
-	}
-
-	if (IsSectionWritable(Adapter, ISO_IMAGE1)) {
-		ISOPri = ReadISOPriority(Adapter, ISO_IMAGE1);
-		if (ISOHighestPri  < ISOPri) {
-			ISOHighestPri = ISOPri;
-			HighestPriISO = ISO_IMAGE1;
-		}
-	}
-	if (HighestPriISO)
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Highest ISO :%x and its Pri :%x", HighestPriISO, ISOHighestPri);
-
-	return HighestPriISO;
-}
-
-static int WriteToFlashWithoutSectorErase(struct bcm_mini_adapter *Adapter,
-				PUINT pBuff,
-				enum bcm_flash2x_section_val eFlash2xSectionVal,
-				unsigned int uiOffset,
-				unsigned int uiNumBytes)
-{
-	#if !defined(BCM_SHM_INTERFACE) || defined(FLASH_DIRECT_ACCESS)
-		unsigned int uiTemp = 0, value = 0;
-		unsigned int i = 0;
-		unsigned int uiPartOffset = 0;
-	#endif
-	unsigned int uiStartOffset = 0;
-	/* Adding section start address */
-	int Status = STATUS_SUCCESS;
-	PUCHAR pcBuff = (PUCHAR)pBuff;
-
-	if (uiNumBytes % Adapter->ulFlashWriteSize) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Writing without Sector Erase for non-FlashWriteSize number of bytes 0x%x\n", uiNumBytes);
-		return STATUS_FAILURE;
-	}
-
-	uiStartOffset = BcmGetSectionValStartOffset(Adapter, eFlash2xSectionVal);
-
-	if (IsSectionExistInVendorInfo(Adapter, eFlash2xSectionVal))
-		return vendorextnWriteSectionWithoutErase(Adapter, pcBuff, eFlash2xSectionVal, uiOffset, uiNumBytes);
-
-	uiOffset = uiOffset + uiStartOffset;
-
-	#if defined(BCM_SHM_INTERFACE) && !defined(FLASH_DIRECT_ACCESS)
-		Status = bcmflash_raw_writenoerase((uiOffset / FLASH_PART_SIZE), (uiOffset % FLASH_PART_SIZE), pcBuff, uiNumBytes);
-	#else
-		rdmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
-		value = 0;
-		wrmalt(Adapter, 0x0f000C80, &value, sizeof(value));
-
-		Adapter->SelectedChip = RESET_CHIP_SELECT;
-		BcmDoChipSelect(Adapter, uiOffset);
-		uiPartOffset = (uiOffset & (FLASH_PART_SIZE - 1)) + GetFlashBaseAddr(Adapter);
-
-		for (i = 0; i < uiNumBytes; i += Adapter->ulFlashWriteSize) {
-			if (Adapter->ulFlashWriteSize == BYTE_WRITE_SUPPORT)
-				Status = flashByteWrite(Adapter, uiPartOffset, pcBuff);
-			else
-				Status = flashWrite(Adapter, uiPartOffset, pcBuff);
-
-			if (Status != STATUS_SUCCESS)
-				break;
-
-			pcBuff = pcBuff + Adapter->ulFlashWriteSize;
-			uiPartOffset = uiPartOffset +  Adapter->ulFlashWriteSize;
-		}
-		wrmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
-		Adapter->SelectedChip = RESET_CHIP_SELECT;
-	#endif
-
-	return Status;
-}
-
-bool IsSectionExistInFlash(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_section_val section)
-{
-	bool SectionPresent = false;
-
-	switch (section) {
-	case ISO_IMAGE1:
-		if ((Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start != UNINIT_PTR_IN_CS) &&
-			(IsNonCDLessDevice(Adapter) == false))
-			SectionPresent = TRUE;
-		break;
-	case ISO_IMAGE2:
-		if ((Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start != UNINIT_PTR_IN_CS) &&
-			(IsNonCDLessDevice(Adapter) == false))
-			SectionPresent = TRUE;
-		break;
-	case DSD0:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDStart != UNINIT_PTR_IN_CS)
-			SectionPresent = TRUE;
-		break;
-	case DSD1:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1Start != UNINIT_PTR_IN_CS)
-			SectionPresent = TRUE;
-		break;
-	case DSD2:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2Start != UNINIT_PTR_IN_CS)
-			SectionPresent = TRUE;
-		break;
-	case VSA0:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAStart != UNINIT_PTR_IN_CS)
-			SectionPresent = TRUE;
-		break;
-	case VSA1:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1Start != UNINIT_PTR_IN_CS)
-			SectionPresent = TRUE;
-		break;
-	case VSA2:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2Start != UNINIT_PTR_IN_CS)
-			SectionPresent = TRUE;
-		break;
-	case SCSI:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForScsiFirmware != UNINIT_PTR_IN_CS)
-			SectionPresent = TRUE;
-		break;
-	case CONTROL_SECTION:
-		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForControlSectionStart != UNINIT_PTR_IN_CS)
-			SectionPresent = TRUE;
-		break;
-	default:
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Section Does not exist in Flash 2.x");
-		SectionPresent =  false;
-	}
-
-	return SectionPresent;
-}
-
-static int IsSectionWritable(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_section_val Section)
-{
-	int offset = STATUS_FAILURE;
-	int Status = false;
-
-	if (IsSectionExistInFlash(Adapter, Section) == false) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Section <%d> does not exist", Section);
-		return false;
-	}
-
-	offset = BcmGetSectionValStartOffset(Adapter, Section);
-	if (offset == INVALID_OFFSET) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Section<%d> does not exist", Section);
-		return false;
-	}
-
-	if (IsSectionExistInVendorInfo(Adapter, Section))
-		return !(Adapter->psFlash2xVendorInfo->VendorSection[Section].AccessFlags & FLASH2X_SECTION_RO);
-
-	Status = IsOffsetWritable(Adapter, offset);
-	return Status;
-}
-
-static int CorruptDSDSig(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_section_val eFlash2xSectionVal)
-{
-	PUCHAR pBuff = NULL;
-	unsigned int sig = 0;
-	unsigned int uiOffset = 0;
-	unsigned int BlockStatus = 0;
-	unsigned int uiSectAlignAddr = 0;
-
-	Adapter->bSigCorrupted = false;
-	if (Adapter->bAllDSDWriteAllow == false) {
-		if (IsSectionWritable(Adapter, eFlash2xSectionVal) != TRUE) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Section is not Writable...Hence can't Corrupt signature");
-			return SECTOR_IS_NOT_WRITABLE;
-		}
-	}
-
-	pBuff = kzalloc(MAX_RW_SIZE, GFP_KERNEL);
-	if (!pBuff) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Can't allocate memorey");
-		return -ENOMEM;
-	}
-
-	uiOffset = Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + sizeof(struct bcm_dsd_header);
-	uiOffset -= MAX_RW_SIZE;
-
-	BcmFlash2xBulkRead(Adapter, (PUINT)pBuff, eFlash2xSectionVal, uiOffset, MAX_RW_SIZE);
-
-	sig = *((PUINT)(pBuff + 12));
-	sig = ntohl(sig);
-	BCM_DEBUG_PRINT_BUFFER(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, pBuff, MAX_RW_SIZE);
-	/* Now corrupting the sig by corrupting 4th last Byte. */
-	*(pBuff + 12) = 0;
-
-	if (sig == DSD_IMAGE_MAGIC_NUMBER) {
-		Adapter->bSigCorrupted = TRUE;
-		if (Adapter->ulFlashWriteSize == BYTE_WRITE_SUPPORT) {
-			uiSectAlignAddr = uiOffset & ~(Adapter->uiSectorSize - 1);
-			BlockStatus = BcmFlashUnProtectBlock(Adapter, uiSectAlignAddr, Adapter->uiSectorSize);
-
-			WriteToFlashWithoutSectorErase(Adapter, (PUINT)(pBuff + 12), eFlash2xSectionVal,
-						(uiOffset + 12), BYTE_WRITE_SUPPORT);
-			if (BlockStatus) {
-				BcmRestoreBlockProtectStatus(Adapter, BlockStatus);
-				BlockStatus = 0;
-			}
-		} else {
-			WriteToFlashWithoutSectorErase(Adapter, (PUINT)pBuff, eFlash2xSectionVal,
-						uiOffset, MAX_RW_SIZE);
-		}
-	} else {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "BCM Signature is not present in header");
-		kfree(pBuff);
-
-		return STATUS_FAILURE;
-	}
-
-	kfree(pBuff);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Corrupted the signature");
-
-	return STATUS_SUCCESS;
-}
-
-static int CorruptISOSig(struct bcm_mini_adapter *Adapter, enum bcm_flash2x_section_val eFlash2xSectionVal)
-{
-	PUCHAR pBuff = NULL;
-	unsigned int sig = 0;
-	unsigned int uiOffset = 0;
-
-	Adapter->bSigCorrupted = false;
-
-	if (IsSectionWritable(Adapter, eFlash2xSectionVal) != TRUE) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Section is not Writable...Hence can't Corrupt signature");
-		return SECTOR_IS_NOT_WRITABLE;
-	}
-
-	pBuff = kzalloc(MAX_RW_SIZE, GFP_KERNEL);
-	if (!pBuff) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Can't allocate memorey");
-		return -ENOMEM;
-	}
-
-	uiOffset = 0;
-
-	BcmFlash2xBulkRead(Adapter, (PUINT)pBuff, eFlash2xSectionVal, uiOffset, MAX_RW_SIZE);
-
-	sig = *((PUINT)pBuff);
-	sig = ntohl(sig);
-
-	/* corrupt signature */
-	*pBuff = 0;
-
-	if (sig == ISO_IMAGE_MAGIC_NUMBER) {
-		Adapter->bSigCorrupted = TRUE;
-		WriteToFlashWithoutSectorErase(Adapter, (PUINT)pBuff, eFlash2xSectionVal,
-					uiOffset, Adapter->ulFlashWriteSize);
-	} else {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "BCM Signature is not present in header");
-		kfree(pBuff);
-
-		return STATUS_FAILURE;
-	}
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Corrupted the signature");
-	BCM_DEBUG_PRINT_BUFFER(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, pBuff, MAX_RW_SIZE);
-
-	kfree(pBuff);
-	return STATUS_SUCCESS;
-}
-
-bool IsNonCDLessDevice(struct bcm_mini_adapter *Adapter)
-{
-	if (Adapter->psFlash2xCSInfo->IsCDLessDeviceBootSig == NON_CDLESS_DEVICE_BOOT_SIG)
-		return TRUE;
-	else
-		return false;
-}
diff --git a/drivers/staging/bcm/nvm.h b/drivers/staging/bcm/nvm.h
deleted file mode 100644
index e765cca5d966..000000000000
--- a/drivers/staging/bcm/nvm.h
+++ /dev/null
@@ -1,286 +0,0 @@
-/***************************************************************************************
- *
- * Copyright (c) Beceem Communications Inc.
- *
- * Module Name:
- *	NVM.h
- *
- * Abstract:
- *	This file has the prototypes,preprocessors and definitions various NVM libraries.
- *
- *
- * Revision History:
- *	Who		When		What
- *	--------	--------	----------------------------------------------
- *	Name		Date		Created/reviewed/modified
- *
- * Notes:
- *
- ****************************************************************************************/
-
-#ifndef _NVM_H_
-#define _NVM_H_
-
-struct bcm_flash_cs_info {
-	u32 MagicNumber;
-	/* let the magic number be 0xBECE-F1A5 - F1A5 for "flas-h" */
-	u32 FlashLayoutVersion;
-	u32 ISOImageVersion;
-	u32 SCSIFirmwareVersion;
-	u32 OffsetFromZeroForPart1ISOImage;
-	u32 OffsetFromZeroForScsiFirmware;
-	u32 SizeOfScsiFirmware;
-	u32 OffsetFromZeroForPart2ISOImage;
-	u32 OffsetFromZeroForCalibrationStart;
-	u32 OffsetFromZeroForCalibrationEnd;
-	u32 OffsetFromZeroForVSAStart;
-	u32 OffsetFromZeroForVSAEnd;
-	u32 OffsetFromZeroForControlSectionStart;
-	u32 OffsetFromZeroForControlSectionData;
-	u32 CDLessInactivityTimeout;
-	u32 NewImageSignature;
-	u32 FlashSectorSizeSig;
-	u32 FlashSectorSize;
-	u32 FlashWriteSupportSize;
-	u32 TotalFlashSize;
-	u32 FlashBaseAddr;
-	u32 FlashPartMaxSize;
-	u32 IsCDLessDeviceBootSig;
-	/* MSC Timeout after reset to switch from MSC to NW Mode */
-	u32 MassStorageTimeout;
-};
-
-#define FLASH2X_TOTAL_SIZE	(64 * 1024 * 1024)
-#define DEFAULT_SECTOR_SIZE	(64 * 1024)
-
-struct bcm_flash2x_cs_info {
-	/* magic number as 0xBECE-F1A5 - F1A5 for "flas-h" */
-	u32 MagicNumber;
-	u32 FlashLayoutVersion;
-	u32 ISOImageVersion;
-	u32 SCSIFirmwareVersion;
-	u32 OffsetFromZeroForPart1ISOImage;
-	u32 OffsetFromZeroForScsiFirmware;
-	u32 SizeOfScsiFirmware;
-	u32 OffsetFromZeroForPart2ISOImage;
-	u32 OffsetFromZeroForDSDStart;
-	u32 OffsetFromZeroForDSDEnd;
-	u32 OffsetFromZeroForVSAStart;
-	u32 OffsetFromZeroForVSAEnd;
-	u32 OffsetFromZeroForControlSectionStart;
-	u32 OffsetFromZeroForControlSectionData;
-	/* NO Data Activity timeout to switch from MSC to NW Mode */
-	u32 CDLessInactivityTimeout;
-	u32 NewImageSignature;
-	u32 FlashSectorSizeSig;
-	u32 FlashSectorSize;
-	u32 FlashWriteSupportSize;
-	u32 TotalFlashSize;
-	u32 FlashBaseAddr;
-	u32 FlashPartMaxSize;
-	u32 IsCDLessDeviceBootSig;
-	/* MSC Timeout after reset to switch from MSC to NW Mode */
-	u32 MassStorageTimeout;
-	/* Flash Map 2.0 Field */
-	u32 OffsetISOImage1Part1Start;
-	u32 OffsetISOImage1Part1End;
-	u32 OffsetISOImage1Part2Start;
-	u32 OffsetISOImage1Part2End;
-	u32 OffsetISOImage1Part3Start;
-	u32 OffsetISOImage1Part3End;
-	u32 OffsetISOImage2Part1Start;
-	u32 OffsetISOImage2Part1End;
-	u32 OffsetISOImage2Part2Start;
-	u32 OffsetISOImage2Part2End;
-	u32 OffsetISOImage2Part3Start;
-	u32 OffsetISOImage2Part3End;
-	/* DSD Header offset from start of DSD */
-	u32 OffsetFromDSDStartForDSDHeader;
-	u32 OffsetFromZeroForDSD1Start;
-	u32 OffsetFromZeroForDSD1End;
-	u32 OffsetFromZeroForDSD2Start;
-	u32 OffsetFromZeroForDSD2End;
-	u32 OffsetFromZeroForVSA1Start;
-	u32 OffsetFromZeroForVSA1End;
-	u32 OffsetFromZeroForVSA2Start;
-	u32 OffsetFromZeroForVSA2End;
-	/*
-	 * ACCESS_BITS_PER_SECTOR	2
-	 * ACCESS_RW			0
-	 * ACCESS_RO			1
-	 * ACCESS_RESVD			2
-	 * ACCESS_RESVD			3
-	 */
-	u32 SectorAccessBitMap[FLASH2X_TOTAL_SIZE / (DEFAULT_SECTOR_SIZE * 16)];
-	/* All expansions to the control data structure should add here */
-};
-
-struct bcm_vendor_section_info {
-	u32 OffsetFromZeroForSectionStart;
-	u32 OffsetFromZeroForSectionEnd;
-	u32 AccessFlags;
-	u32 Reserved[16];
-};
-
-struct bcm_flash2x_vendor_info {
-	struct bcm_vendor_section_info VendorSection[TOTAL_SECTIONS];
-	u32 Reserved[16];
-};
-
-struct bcm_dsd_header {
-	u32 DSDImageSize;
-	u32 DSDImageCRC;
-	u32 DSDImagePriority;
-	/* We should not consider right now. Reading reserve is worthless. */
-	u32 Reserved[252]; /* Resvd for DSD Header */
-	u32 DSDImageMagicNumber;
-};
-
-struct bcm_iso_header {
-	u32 ISOImageMagicNumber;
-	u32 ISOImageSize;
-	u32 ISOImageCRC;
-	u32 ISOImagePriority;
-	/* We should not consider right now. Reading reserve is worthless. */
-	u32 Reserved[60]; /* Resvd for ISO Header extension */
-};
-
-#define EEPROM_BEGIN_CIS	(0)
-#define EEPROM_BEGIN_NON_CIS	(0x200)
-#define EEPROM_END		(0x2000)
-#define INIT_PARAMS_SIGNATURE	(0x95a7a597)
-#define MAX_INIT_PARAMS_LENGTH	(2048)
-#define MAC_ADDRESS_OFFSET	0x200
-
-#define INIT_PARAMS_1_SIGNATURE_ADDRESS		EEPROM_BEGIN_NON_CIS
-#define INIT_PARAMS_1_DATA_ADDRESS		(INIT_PARAMS_1_SIGNATURE_ADDRESS+16)
-#define INIT_PARAMS_1_MACADDRESS_ADDRESS	(MAC_ADDRESS_OFFSET)
-#define INIT_PARAMS_1_LENGTH_ADDRESS		(INIT_PARAMS_1_SIGNATURE_ADDRESS+4)
-
-#define INIT_PARAMS_2_SIGNATURE_ADDRESS		(EEPROM_BEGIN_NON_CIS + 2048 + 16)
-#define INIT_PARAMS_2_DATA_ADDRESS		(INIT_PARAMS_2_SIGNATURE_ADDRESS + 16)
-#define INIT_PARAMS_2_MACADDRESS_ADDRESS	(INIT_PARAMS_2_SIGNATURE_ADDRESS + 8)
-#define INIT_PARAMS_2_LENGTH_ADDRESS		(INIT_PARAMS_2_SIGNATURE_ADDRESS + 4)
-
-#define EEPROM_SPI_DEV_CONFIG_REG		0x0F003000
-#define EEPROM_SPI_Q_STATUS1_REG		0x0F003004
-#define EEPROM_SPI_Q_STATUS1_MASK_REG		0x0F00300C
-
-#define EEPROM_SPI_Q_STATUS_REG			0x0F003008
-#define EEPROM_CMDQ_SPI_REG			0x0F003018
-#define EEPROM_WRITE_DATAQ_REG			0x0F00301C
-#define EEPROM_READ_DATAQ_REG			0x0F003020
-#define SPI_FLUSH_REG				0x0F00304C
-
-#define EEPROM_WRITE_ENABLE			0x06000000
-#define EEPROM_READ_STATUS_REGISTER		0x05000000
-#define EEPROM_16_BYTE_PAGE_WRITE		0xFA000000
-#define EEPROM_WRITE_QUEUE_EMPTY		0x00001000
-#define EEPROM_WRITE_QUEUE_AVAIL		0x00002000
-#define EEPROM_WRITE_QUEUE_FULL			0x00004000
-#define EEPROM_16_BYTE_PAGE_READ		0xFB000000
-#define EEPROM_4_BYTE_PAGE_READ			0x3B000000
-
-#define EEPROM_CMD_QUEUE_FLUSH			0x00000001
-#define EEPROM_WRITE_QUEUE_FLUSH		0x00000002
-#define EEPROM_READ_QUEUE_FLUSH			0x00000004
-#define EEPROM_ETH_QUEUE_FLUSH			0x00000008
-#define EEPROM_ALL_QUEUE_FLUSH			0x0000000f
-#define EEPROM_READ_ENABLE			0x06000000
-#define EEPROM_16_BYTE_PAGE_WRITE		0xFA000000
-#define EEPROM_READ_DATA_FULL			0x00000010
-#define EEPROM_READ_DATA_AVAIL			0x00000020
-#define EEPROM_READ_QUEUE_EMPTY			0x00000002
-#define EEPROM_CMD_QUEUE_EMPTY			0x00000100
-#define EEPROM_CMD_QUEUE_AVAIL			0x00000200
-#define EEPROM_CMD_QUEUE_FULL			0x00000400
-
-/* Most EEPROM status register bit 0 indicates if the EEPROM is busy
- * with a write if set 1. See the details of the EEPROM Status Register
- * in the EEPROM data sheet.
- */
-#define EEPROM_STATUS_REG_WRITE_BUSY		0x00000001
-
-/* We will have 1 mSec for every RETRIES_PER_DELAY count and have a max attempts of MAX_EEPROM_RETRIES
- * This will give us 80 mSec minimum of delay = 80mSecs
- */
-#define MAX_EEPROM_RETRIES			80
-#define RETRIES_PER_DELAY			64
-#define MAX_RW_SIZE				0x10
-#define MAX_READ_SIZE				0x10
-#define MAX_SECTOR_SIZE				(512 * 1024)
-#define MIN_SECTOR_SIZE				(1024)
-#define FLASH_SECTOR_SIZE_OFFSET		0xEFFFC
-#define FLASH_SECTOR_SIZE_SIG_OFFSET		0xEFFF8
-#define FLASH_SECTOR_SIZE_SIG			0xCAFEBABE
-#define FLASH_CS_INFO_START_ADDR		0xFF0000
-#define FLASH_CONTROL_STRUCT_SIGNATURE		0xBECEF1A5
-#define SCSI_FIRMWARE_MAJOR_VERSION		0x1
-#define SCSI_FIRMWARE_MINOR_VERSION		0x5
-#define BYTE_WRITE_SUPPORT			0x1
-#define FLASH_AUTO_INIT_BASE_ADDR		0xF00000
-#define FLASH_CONTIGIOUS_START_ADDR_AFTER_INIT	0x1C000000
-#define FLASH_CONTIGIOUS_START_ADDR_BEFORE_INIT	0x1F000000
-#define FLASH_CONTIGIOUS_START_ADDR_BCS350	0x08000000
-#define FLASH_CONTIGIOUS_END_ADDR_BCS350	0x08FFFFFF
-#define FLASH_SIZE_ADDR				0xFFFFEC
-#define FLASH_SPI_CMDQ_REG			0xAF003040
-#define FLASH_SPI_WRITEQ_REG			0xAF003044
-#define FLASH_SPI_READQ_REG			0xAF003048
-#define FLASH_CONFIG_REG			0xAF003050
-#define FLASH_GPIO_CONFIG_REG			0xAF000030
-#define FLASH_CMD_WRITE_ENABLE			0x06
-#define FLASH_CMD_READ_ENABLE			0x03
-#define FLASH_CMD_RESET_WRITE_ENABLE		0x04
-#define FLASH_CMD_STATUS_REG_READ		0x05
-#define FLASH_CMD_STATUS_REG_WRITE		0x01
-#define FLASH_CMD_READ_ID			0x9F
-#define PAD_SELECT_REGISTER			0xAF000410
-#define FLASH_PART_SST25VF080B			0xBF258E
-#define EEPROM_CAL_DATA_INTERNAL_LOC		0xbFB00008
-#define EEPROM_CALPARAM_START			0x200
-#define EEPROM_SIZE_OFFSET			524
-
-/* As Read/Write time vaires from 1.5 to 3.0 ms.
- * so After Ignoring the rdm/wrm time(that is dependent on many factor like interface etc.),
- * here time calculated meets the worst case delay, 3.0 ms
- */
-#define MAX_FLASH_RETRIES		4
-#define FLASH_PER_RETRIES_DELAY		16
-#define EEPROM_MAX_CAL_AREA_SIZE	0xF0000
-#define BECM				ntohl(0x4245434d)
-#define FLASH_2X_MAJOR_NUMBER		0x2
-#define DSD_IMAGE_MAGIC_NUMBER		0xBECE0D5D
-#define ISO_IMAGE_MAGIC_NUMBER		0xBECE0150
-#define NON_CDLESS_DEVICE_BOOT_SIG	0xBECEB007
-
-#define MINOR_VERSION(x) ((x >> 16) & 0xFFFF)
-#define MAJOR_VERSION(x) (x & 0xFFFF)
-
-#define CORRUPTED_PATTERN		0x0
-#define UNINIT_PTR_IN_CS		0xBBBBDDDD
-#define VENDOR_PTR_IN_CS		0xAAAACCCC
-#define FLASH2X_SECTION_PRESENT		(1 << 0)
-#define FLASH2X_SECTION_VALID		(1 << 1)
-#define FLASH2X_SECTION_RO		(1 << 2)
-#define FLASH2X_SECTION_ACT		(1 << 3)
-#define SECTOR_IS_NOT_WRITABLE		STATUS_FAILURE
-#define INVALID_OFFSET			STATUS_FAILURE
-#define INVALID_SECTION			STATUS_FAILURE
-#define SECTOR_1K			1024
-#define SECTOR_64K			(64 * SECTOR_1K)
-#define SECTOR_128K			(2 * SECTOR_64K)
-#define SECTOR_256k			(2 * SECTOR_128K)
-#define SECTOR_512K			(2 * SECTOR_256k)
-#define FLASH_PART_SIZE			(16 * 1024 * 1024)
-#define RESET_CHIP_SELECT		-1
-#define CHIP_SELECT_BIT12		12
-#define SECTOR_READWRITE_PERMISSION	0
-#define SECTOR_READONLY			1
-#define SIGNATURE_SIZE			4
-#define DEFAULT_BUFF_SIZE		0x10000
-
-#define FIELD_OFFSET_IN_HEADER(HeaderPointer, Field) ((u8 *)&((HeaderPointer)(NULL))->Field - (u8 *)(NULL))
-
-#endif
-
diff --git a/drivers/staging/bcm/sort.c b/drivers/staging/bcm/sort.c
deleted file mode 100644
index ca0b17991512..000000000000
--- a/drivers/staging/bcm/sort.c
+++ /dev/null
@@ -1,52 +0,0 @@
-#include "headers.h"
-#include <linux/sort.h>
-
-/*
- * File Name: sort.c
- *
- * Author: Beceem Communications Pvt. Ltd
- *
- * Abstract: This file contains the routines sorting the classification rules.
- *
- * Copyright (c) 2007 Beceem Communications Pvt. Ltd
- */
-
-static int compare_packet_info(void const *a, void const *b)
-{
-	struct bcm_packet_info const *pa = a;
-	struct bcm_packet_info const *pb = b;
-
-	if (!pa->bValid || !pb->bValid)
-		return 0;
-
-	return pa->u8TrafficPriority - pb->u8TrafficPriority;
-}
-
-VOID SortPackInfo(struct bcm_mini_adapter *Adapter)
-{
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
-			DBG_LVL_ALL, "<=======");
-
-	sort(Adapter->PackInfo, NO_OF_QUEUES, sizeof(struct bcm_packet_info),
-	     compare_packet_info, NULL);
-}
-
-static int compare_classifiers(void const *a, void const *b)
-{
-	struct bcm_classifier_rule const *pa = a;
-	struct bcm_classifier_rule const *pb = b;
-
-	if (!pa->bUsed || !pb->bUsed)
-		return 0;
-
-	return pa->u8ClassifierRulePriority - pb->u8ClassifierRulePriority;
-}
-
-VOID SortClassifiers(struct bcm_mini_adapter *Adapter)
-{
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
-			DBG_LVL_ALL, "<=======");
-
-	sort(Adapter->astClassifierTable, MAX_CLASSIFIERS,
-	     sizeof(struct bcm_classifier_rule), compare_classifiers, NULL);
-}
diff --git a/drivers/staging/bcm/target_params.h b/drivers/staging/bcm/target_params.h
deleted file mode 100644
index dc45f9ab854d..000000000000
--- a/drivers/staging/bcm/target_params.h
+++ /dev/null
@@ -1,57 +0,0 @@
-#ifndef TARGET_PARAMS_H
-#define TARGET_PARAMS_H
-
-struct bcm_target_params {
-	u32 m_u32CfgVersion;
-	u32 m_u32CenterFrequency;
-	u32 m_u32BandAScan;
-	u32 m_u32BandBScan;
-	u32 m_u32BandCScan;
-	u32 m_u32ErtpsOptions;
-	u32 m_u32PHSEnable;
-	u32 m_u32HoEnable;
-	u32 m_u32HoReserved1;
-	u32 m_u32HoReserved2;
-	u32 m_u32MimoEnable;
-	u32 m_u32SecurityEnable;
-	u32 m_u32PowerSavingModesEnable; /* bit 1: 1 Idlemode enable; bit2: 1 Sleepmode Enable */
-	/* PowerSaving Mode Options:
-	 * bit 0 = 1: CPE mode - to keep pcmcia if alive;
-	 * bit 1 = 1: CINR reporting in Idlemode Msg
-	 * bit 2 = 1: Default PSC Enable in sleepmode
-	 */
-	u32 m_u32PowerSavingModeOptions;
-	u32 m_u32ArqEnable;
-	/* From Version #3, the HARQ section renamed as general */
-	u32 m_u32HarqEnable;
-	u32 m_u32EEPROMFlag;
-	/* BINARY TYPE - 4th MSByte: Interface Type -  3rd MSByte: Vendor Type - 2nd MSByte
-	 * Unused - LSByte
-	 */
-	u32 m_u32Customize;
-	u32 m_u32ConfigBW;  /* In Hz */
-	u32 m_u32ShutDownInitThresholdTimer;
-	u32 m_u32RadioParameter;
-	u32 m_u32PhyParameter1;
-	u32 m_u32PhyParameter2;
-	u32 m_u32PhyParameter3;
-	u32 m_u32TestOptions; /* 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 and bit 19 is test harq ack/nack */
-	u32 m_u32MaxMACDataperDLFrame;
-	u32 m_u32MaxMACDataperULFrame;
-	u32 m_u32Corr2MacFlags;
-	u32 HostDrvrConfig1;
-	u32 HostDrvrConfig2;
-	u32 HostDrvrConfig3;
-	u32 HostDrvrConfig4;
-	u32 HostDrvrConfig5;
-	u32 HostDrvrConfig6;
-	u32 m_u32SegmentedPUSCenable;
-	/* removed SHUT down related 'unused' params from here to sync 4.x and 5.x CFG files..
-	 * BAMC Related Parameters
-	 * Bit 0-15 Band AMC signaling configuration: Bit 1 = 1 – Enable Band AMC signaling.
-	 * bit 16-31 Band AMC Data configuration: Bit 16 = 1 – Band AMC 2x3 support.
-	 */
-	u32 m_u32BandAMCEnable;
-};
-
-#endif
diff --git a/drivers/staging/bcm/vendorspecificextn.c b/drivers/staging/bcm/vendorspecificextn.c
deleted file mode 100644
index 1d9bef6e4273..000000000000
--- a/drivers/staging/bcm/vendorspecificextn.c
+++ /dev/null
@@ -1,145 +0,0 @@
-#include "headers.h"
-/*
- * Procedure:	vendorextnGetSectionInfo
- *
- * Description: Finds the type of NVM used.
- *
- * Arguments:
- *		Adapter    - ptr to Adapter object instance
- *		pNVMType   - ptr to NVM type.
- * Returns:
- *		STATUS_SUCCESS/STATUS_FAILURE
- *
- */
-INT vendorextnGetSectionInfo(PVOID  pContext,
-			struct bcm_flash2x_vendor_info *pVendorInfo)
-{
-	return STATUS_FAILURE;
-}
-
-/*
- * Procedure:   vendorextnInit
- *
- * Description: Initializing the vendor extension NVM interface
- *
- * Arguments:
- *              Adapter   - Pointer to MINI Adapter Structure
- * Returns:
- *             STATUS_SUCCESS/STATUS_FAILURE
- *
- *
- */
-INT vendorextnInit(struct bcm_mini_adapter *Adapter)
-{
-	return STATUS_SUCCESS;
-}
-
-/*
- * Procedure:   vendorextnExit
- *
- * Description: Free the resource associated with vendor extension NVM interface
- *
- * Arguments:
- *
- * Returns:
- *              STATUS_SUCCESS/STATUS_FAILURE
- *
- *
- */
-INT vendorextnExit(struct bcm_mini_adapter *Adapter)
-{
-	return STATUS_SUCCESS;
-}
-
-/*
- * Procedure:	vendorextnIoctl
- *
- * Description: execute the vendor extension specific ioctl
- *
- * Arguments:
- *		Adapter -Beceem private Adapter Structure
- *		cmd	-vendor extension specific Ioctl commad
- *		arg	-input parameter sent by vendor
- *
- * Returns:
- *		CONTINUE_COMMON_PATH in case it is not meant to be processed
- *		by vendor ioctls
- *		STATUS_SUCCESS/STATUS_FAILURE as per the IOCTL return value
- */
-
-INT vendorextnIoctl(struct bcm_mini_adapter *Adapter, UINT cmd, ULONG arg)
-{
-	return CONTINUE_COMMON_PATH;
-}
-
-
-
-/*
- * Procedure:	vendorextnReadSection
- *
- * Description: Reads from a section of NVM
- *
- * Arguments:
- *		pContext - ptr to Adapter object instance
- *		pBuffer - Read the data from Vendor Area to this buffer
- *		SectionVal   - Value of type of Section
- *		Offset - Read from the Offset of the Vendor Section.
- *		numOfBytes - Read numOfBytes from the Vendor section to Buffer
- *
- * Returns:
- *		STATUS_SUCCESS/STATUS_FAILURE
- */
-
-INT vendorextnReadSection(PVOID  pContext, PUCHAR pBuffer,
-	enum bcm_flash2x_section_val SectionVal, UINT offset, UINT numOfBytes)
-{
-	return STATUS_FAILURE;
-}
-
-
-
-/*
- * Procedure:	vendorextnWriteSection
- *
- * Description: Write to a Section of NVM
- *
- * Arguments:
- *		pContext - ptr to Adapter object instance
- *		pBuffer - Write the data provided in the buffer
- *		SectionVal   - Value of type of Section
- *		Offset - Writes to the Offset of the Vendor Section.
- *		numOfBytes - Write num Bytes after reading from pBuffer.
- *		bVerify - the Buffer Written should be verified.
- *
- * Returns:
- *		STATUS_SUCCESS/STATUS_FAILURE
- */
-INT vendorextnWriteSection(PVOID  pContext, PUCHAR pBuffer,
-	enum bcm_flash2x_section_val SectionVal, UINT offset,
-	UINT numOfBytes, bool bVerify)
-{
-	return STATUS_FAILURE;
-}
-
-
-
-/*
- * Procedure:	vendorextnWriteSectionWithoutErase
- *
- * Description: Write to a Section of NVM without erasing the sector
- *
- * Arguments:
- *		pContext - ptr to Adapter object instance
- *		pBuffer - Write the data provided in the buffer
- *		SectionVal   - Value of type of Section
- *		Offset - Writes to the Offset of the Vendor Section.
- *		numOfBytes - Write num Bytes after reading from pBuffer.
- *
- * Returns:
- *		STATUS_SUCCESS/STATUS_FAILURE
- */
-INT vendorextnWriteSectionWithoutErase(PVOID  pContext, PUCHAR pBuffer,
-	enum bcm_flash2x_section_val SectionVal, UINT offset, UINT numOfBytes)
-{
-	return STATUS_FAILURE;
-}
diff --git a/drivers/staging/bcm/vendorspecificextn.h b/drivers/staging/bcm/vendorspecificextn.h
deleted file mode 100644
index ff57f0570451..000000000000
--- a/drivers/staging/bcm/vendorspecificextn.h
+++ /dev/null
@@ -1,18 +0,0 @@
-
-#ifndef __VENDOR_EXTN_NVM_H__
-#define __VENDOR_EXTN_NVM_H__
-
-#define CONTINUE_COMMON_PATH 0xFFFF
-
-INT vendorextnGetSectionInfo(PVOID  pContext, struct bcm_flash2x_vendor_info *pVendorInfo);
-INT vendorextnExit(struct bcm_mini_adapter *Adapter);
-INT vendorextnInit(struct bcm_mini_adapter *Adapter);
-INT vendorextnIoctl(struct bcm_mini_adapter *Adapter, UINT cmd, ULONG arg);
-INT vendorextnReadSection(PVOID  pContext, PUCHAR pBuffer, enum bcm_flash2x_section_val SectionVal,
-			UINT offset, UINT numOfBytes);
-INT vendorextnWriteSection(PVOID  pContext, PUCHAR pBuffer, enum bcm_flash2x_section_val SectionVal,
-			UINT offset, UINT numOfBytes, bool bVerify);
-INT vendorextnWriteSectionWithoutErase(PVOID  pContext, PUCHAR pBuffer, enum bcm_flash2x_section_val SectionVal,
-			UINT offset, UINT numOfBytes);
-
-#endif /*  */
diff --git a/drivers/staging/clocking-wizard/Kconfig b/drivers/staging/clocking-wizard/Kconfig
new file mode 100644
index 000000000000..357af02c562c
--- /dev/null
+++ b/drivers/staging/clocking-wizard/Kconfig
@@ -0,0 +1,9 @@
+#
+# Xilinx Clocking Wizard Driver
+#
+
+config COMMON_CLK_XLNX_CLKWZRD
+	tristate "Xilinx Clocking Wizard"
+	depends on COMMON_CLK && OF
+	---help---
+	  Support for the Xilinx Clocking Wizard IP core clock generator.
diff --git a/drivers/staging/clocking-wizard/Makefile b/drivers/staging/clocking-wizard/Makefile
new file mode 100644
index 000000000000..5ad352f521fe
--- /dev/null
+++ b/drivers/staging/clocking-wizard/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_COMMON_CLK_XLNX_CLKWZRD)	+= clk-xlnx-clock-wizard.o
diff --git a/drivers/staging/clocking-wizard/TODO b/drivers/staging/clocking-wizard/TODO
new file mode 100644
index 000000000000..ebe99db7d153
--- /dev/null
+++ b/drivers/staging/clocking-wizard/TODO
@@ -0,0 +1,12 @@
+TODO:
+	- support for fractional multiplier
+	- support for fractional divider (output 0 only)
+	- support for set_rate() operations (may benefit from Stephen Boyd's
+	  refactoring of the clk primitives: https://lkml.org/lkml/2014/9/5/766)
+	- review arithmetic
+	  - overflow after multiplication?
+	  - maximize accuracy before divisions
+
+Patches to:
+	Greg Kroah-Hartman <gregkh@linuxfoundation.org>
+	Sören Brinkmann <soren.brinkmann@xilinx.com>
diff --git a/drivers/staging/clocking-wizard/clk-xlnx-clock-wizard.c b/drivers/staging/clocking-wizard/clk-xlnx-clock-wizard.c
new file mode 100644
index 000000000000..471d0877f382
--- /dev/null
+++ b/drivers/staging/clocking-wizard/clk-xlnx-clock-wizard.c
@@ -0,0 +1,341 @@
+/*
+ * Xilinx 'Clocking Wizard' driver
+ *
+ *  Copyright (C) 2013 - 2014 Xilinx
+ *
+ *  Sören Brinkmann <soren.brinkmann@xilinx.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License v2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/platform_device.h>
+#include <linux/clk-provider.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/module.h>
+#include <linux/err.h>
+
+#define WZRD_NUM_OUTPUTS	7
+#define WZRD_ACLK_MAX_FREQ	250000000UL
+
+#define WZRD_CLK_CFG_REG(n)	(0x200 + 4 * (n))
+
+#define WZRD_CLkOUT0_FRAC_EN	BIT(18)
+#define WZRD_CLkFBOUT_FRAC_EN	BIT(26)
+
+#define WZRD_CLKFBOUT_MULT_SHIFT	8
+#define WZRD_CLKFBOUT_MULT_MASK		(0xff << WZRD_CLKFBOUT_MULT_SHIFT)
+#define WZRD_DIVCLK_DIVIDE_SHIFT	0
+#define WZRD_DIVCLK_DIVIDE_MASK		(0xff << WZRD_DIVCLK_DIVIDE_SHIFT)
+#define WZRD_CLKOUT_DIVIDE_SHIFT	0
+#define WZRD_CLKOUT_DIVIDE_MASK		(0xff << WZRD_DIVCLK_DIVIDE_SHIFT)
+
+enum clk_wzrd_int_clks {
+	wzrd_clk_mul,
+	wzrd_clk_mul_div,
+	wzrd_clk_int_max
+};
+
+/**
+ * struct clk_wzrd:
+ * @clk_data:		Clock data
+ * @nb:			Notifier block
+ * @base:		Memory base
+ * @clk_in1:		Handle to input clock 'clk_in1'
+ * @axi_clk:		Handle to input clock 's_axi_aclk'
+ * @clks_internal:	Internal clocks
+ * @clkout:		Output clocks
+ * @speed_grade:	Speed grade of the device
+ * @suspended:		Flag indicating power state of the device
+ */
+struct clk_wzrd {
+	struct clk_onecell_data clk_data;
+	struct notifier_block nb;
+	void __iomem *base;
+	struct clk *clk_in1;
+	struct clk *axi_clk;
+	struct clk *clks_internal[wzrd_clk_int_max];
+	struct clk *clkout[WZRD_NUM_OUTPUTS];
+	int speed_grade;
+	bool suspended;
+};
+#define to_clk_wzrd(_nb) container_of(_nb, struct clk_wzrd, nb)
+
+/* maximum frequencies for input/output clocks per speed grade */
+static const unsigned long clk_wzrd_max_freq[] = {
+	800000000UL,
+	933000000UL,
+	1066000000UL
+};
+
+static int clk_wzrd_clk_notifier(struct notifier_block *nb, unsigned long event,
+				 void *data)
+{
+	unsigned long max;
+	struct clk_notifier_data *ndata = data;
+	struct clk_wzrd *clk_wzrd = to_clk_wzrd(nb);
+
+	if (clk_wzrd->suspended)
+		return NOTIFY_OK;
+
+	if (ndata->clk == clk_wzrd->clk_in1)
+		max = clk_wzrd_max_freq[clk_wzrd->speed_grade - 1];
+	if (ndata->clk == clk_wzrd->axi_clk)
+		max = WZRD_ACLK_MAX_FREQ;
+
+	switch (event) {
+	case PRE_RATE_CHANGE:
+		if (ndata->new_rate > max)
+			return NOTIFY_BAD;
+		return NOTIFY_OK;
+	case POST_RATE_CHANGE:
+	case ABORT_RATE_CHANGE:
+	default:
+		return NOTIFY_DONE;
+	}
+}
+
+static int __maybe_unused clk_wzrd_suspend(struct device *dev)
+{
+	struct clk_wzrd *clk_wzrd = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(clk_wzrd->axi_clk);
+	clk_wzrd->suspended = true;
+
+	return 0;
+}
+
+static int __maybe_unused clk_wzrd_resume(struct device *dev)
+{
+	int ret;
+	struct clk_wzrd *clk_wzrd = dev_get_drvdata(dev);
+
+	ret = clk_prepare_enable(clk_wzrd->axi_clk);
+	if (ret) {
+		dev_err(dev, "unable to enable s_axi_aclk\n");
+		return ret;
+	}
+
+	clk_wzrd->suspended = false;
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(clk_wzrd_dev_pm_ops, clk_wzrd_suspend,
+			 clk_wzrd_resume);
+
+static int clk_wzrd_probe(struct platform_device *pdev)
+{
+	int i, ret;
+	u32 reg;
+	unsigned long rate;
+	const char *clk_name;
+	struct clk_wzrd *clk_wzrd;
+	struct resource *mem;
+	struct device_node *np = pdev->dev.of_node;
+
+	clk_wzrd = devm_kzalloc(&pdev->dev, sizeof(*clk_wzrd), GFP_KERNEL);
+	if (!clk_wzrd)
+		return -ENOMEM;
+	platform_set_drvdata(pdev, clk_wzrd);
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	clk_wzrd->base = devm_ioremap_resource(&pdev->dev, mem);
+	if (IS_ERR(clk_wzrd->base))
+		return PTR_ERR(clk_wzrd->base);
+
+	ret = of_property_read_u32(np, "speed-grade", &clk_wzrd->speed_grade);
+	if (!ret) {
+		if (clk_wzrd->speed_grade < 1 || clk_wzrd->speed_grade > 3) {
+			dev_warn(&pdev->dev, "invalid speed grade '%d'\n",
+				 clk_wzrd->speed_grade);
+			clk_wzrd->speed_grade = 0;
+		}
+	}
+
+	clk_wzrd->clk_in1 = devm_clk_get(&pdev->dev, "clk_in1");
+	if (IS_ERR(clk_wzrd->clk_in1)) {
+		if (clk_wzrd->clk_in1 != ERR_PTR(-EPROBE_DEFER))
+			dev_err(&pdev->dev, "clk_in1 not found\n");
+		return PTR_ERR(clk_wzrd->clk_in1);
+	}
+
+	clk_wzrd->axi_clk = devm_clk_get(&pdev->dev, "s_axi_aclk");
+	if (IS_ERR(clk_wzrd->axi_clk)) {
+		if (clk_wzrd->axi_clk != ERR_PTR(-EPROBE_DEFER))
+			dev_err(&pdev->dev, "s_axi_aclk not found\n");
+		return PTR_ERR(clk_wzrd->axi_clk);
+	}
+	ret = clk_prepare_enable(clk_wzrd->axi_clk);
+	if (ret) {
+		dev_err(&pdev->dev, "enabling s_axi_aclk failed\n");
+		return ret;
+	}
+	rate = clk_get_rate(clk_wzrd->axi_clk);
+	if (rate > WZRD_ACLK_MAX_FREQ) {
+		dev_err(&pdev->dev, "s_axi_aclk frequency (%lu) too high\n",
+			rate);
+		ret = -EINVAL;
+		goto err_disable_clk;
+	}
+
+	/* we don't support fractional div/mul yet */
+	reg = readl(clk_wzrd->base + WZRD_CLK_CFG_REG(0)) &
+		    WZRD_CLkFBOUT_FRAC_EN;
+	reg |= readl(clk_wzrd->base + WZRD_CLK_CFG_REG(2)) &
+		     WZRD_CLkOUT0_FRAC_EN;
+	if (reg)
+		dev_warn(&pdev->dev, "fractional div/mul not supported\n");
+
+	/* register multiplier */
+	reg = (readl(clk_wzrd->base + WZRD_CLK_CFG_REG(0)) &
+		     WZRD_CLKFBOUT_MULT_MASK) >> WZRD_CLKFBOUT_MULT_SHIFT;
+	clk_name = kasprintf(GFP_KERNEL, "%s_mul", dev_name(&pdev->dev));
+	if (!clk_name) {
+		ret = -ENOMEM;
+		goto err_disable_clk;
+	}
+	clk_wzrd->clks_internal[wzrd_clk_mul] = clk_register_fixed_factor(
+			&pdev->dev, clk_name,
+			__clk_get_name(clk_wzrd->clk_in1),
+			0, reg, 1);
+	kfree(clk_name);
+	if (IS_ERR(clk_wzrd->clks_internal[wzrd_clk_mul])) {
+		dev_err(&pdev->dev, "unable to register fixed-factor clock\n");
+		ret = PTR_ERR(clk_wzrd->clks_internal[wzrd_clk_mul]);
+		goto err_disable_clk;
+	}
+
+	/* register div */
+	reg = (readl(clk_wzrd->base + WZRD_CLK_CFG_REG(0)) &
+			WZRD_DIVCLK_DIVIDE_MASK) >> WZRD_DIVCLK_DIVIDE_SHIFT;
+	clk_name = kasprintf(GFP_KERNEL, "%s_mul_div", dev_name(&pdev->dev));
+	if (!clk_name) {
+		ret = -ENOMEM;
+		goto err_rm_int_clk;
+	}
+
+	clk_wzrd->clks_internal[wzrd_clk_mul_div] = clk_register_fixed_factor(
+			&pdev->dev, clk_name,
+			__clk_get_name(clk_wzrd->clks_internal[wzrd_clk_mul]),
+			0, 1, reg);
+	if (IS_ERR(clk_wzrd->clks_internal[wzrd_clk_mul_div])) {
+		dev_err(&pdev->dev, "unable to register divider clock\n");
+		ret = PTR_ERR(clk_wzrd->clks_internal[wzrd_clk_mul_div]);
+		goto err_rm_int_clk;
+	}
+
+	/* register div per output */
+	for (i = WZRD_NUM_OUTPUTS - 1; i >= 0 ; i--) {
+		const char *clkout_name;
+		if (of_property_read_string_index(np, "clock-output-names", i,
+						  &clkout_name)) {
+			dev_err(&pdev->dev,
+				"clock output name not specified\n");
+			ret = -EINVAL;
+			goto err_rm_int_clks;
+		}
+		reg = readl(clk_wzrd->base + WZRD_CLK_CFG_REG(2) + i * 12);
+		reg &= WZRD_CLKOUT_DIVIDE_MASK;
+		reg >>= WZRD_CLKOUT_DIVIDE_SHIFT;
+		clk_wzrd->clkout[i] = clk_register_fixed_factor(&pdev->dev,
+				clkout_name, clk_name, 0, 1, reg);
+		if (IS_ERR(clk_wzrd->clkout[i])) {
+			int j;
+
+			for (j = i + 1; j < WZRD_NUM_OUTPUTS; j++)
+				clk_unregister(clk_wzrd->clkout[j]);
+			dev_err(&pdev->dev,
+				"unable to register divider clock\n");
+			ret = PTR_ERR(clk_wzrd->clkout[i]);
+			goto err_rm_int_clks;
+		}
+	}
+
+	kfree(clk_name);
+
+	clk_wzrd->clk_data.clks = clk_wzrd->clkout;
+	clk_wzrd->clk_data.clk_num = ARRAY_SIZE(clk_wzrd->clkout);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_wzrd->clk_data);
+
+	if (clk_wzrd->speed_grade) {
+		clk_wzrd->nb.notifier_call = clk_wzrd_clk_notifier;
+
+		ret = clk_notifier_register(clk_wzrd->clk_in1,
+					    &clk_wzrd->nb);
+		if (ret)
+			dev_warn(&pdev->dev,
+				 "unable to register clock notifier\n");
+
+		ret = clk_notifier_register(clk_wzrd->axi_clk, &clk_wzrd->nb);
+		if (ret)
+			dev_warn(&pdev->dev,
+				 "unable to register clock notifier\n");
+	}
+
+	return 0;
+
+err_rm_int_clks:
+	clk_unregister(clk_wzrd->clks_internal[1]);
+err_rm_int_clk:
+	kfree(clk_name);
+	clk_unregister(clk_wzrd->clks_internal[0]);
+err_disable_clk:
+	clk_disable_unprepare(clk_wzrd->axi_clk);
+
+	return ret;
+}
+
+static int clk_wzrd_remove(struct platform_device *pdev)
+{
+	int i;
+	struct clk_wzrd *clk_wzrd = platform_get_drvdata(pdev);
+
+	of_clk_del_provider(pdev->dev.of_node);
+
+	for (i = 0; i < WZRD_NUM_OUTPUTS; i++)
+		clk_unregister(clk_wzrd->clkout[i]);
+	for (i = 0; i < wzrd_clk_int_max; i++)
+		clk_unregister(clk_wzrd->clks_internal[i]);
+
+	if (clk_wzrd->speed_grade) {
+		clk_notifier_unregister(clk_wzrd->axi_clk, &clk_wzrd->nb);
+		clk_notifier_unregister(clk_wzrd->clk_in1, &clk_wzrd->nb);
+	}
+
+	clk_disable_unprepare(clk_wzrd->axi_clk);
+
+	return 0;
+}
+
+static const struct of_device_id clk_wzrd_ids[] = {
+	{ .compatible = "xlnx,clocking-wizard" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, clk_wzrd_ids);
+
+static struct platform_driver clk_wzrd_driver = {
+	.driver = {
+		.name = "clk-wizard",
+		.of_match_table = clk_wzrd_ids,
+		.pm = &clk_wzrd_dev_pm_ops,
+	},
+	.probe = clk_wzrd_probe,
+	.remove = clk_wzrd_remove,
+};
+module_platform_driver(clk_wzrd_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Soeren Brinkmann <soren.brinkmann@xilinx.com");
+MODULE_DESCRIPTION("Driver for the Xilinx Clocking Wizard IP core");
diff --git a/drivers/staging/clocking-wizard/dt-binding.txt b/drivers/staging/clocking-wizard/dt-binding.txt
new file mode 100644
index 000000000000..723271e93316
--- /dev/null
+++ b/drivers/staging/clocking-wizard/dt-binding.txt
@@ -0,0 +1,30 @@
+Binding for Xilinx Clocking Wizard IP Core
+
+This binding uses the common clock binding[1]. Details about the devices can be
+found in the product guide[2].
+
+[1] Documentation/devicetree/bindings/clock/clock-bindings.txt
+[2] Clocking Wizard Product Guide
+http://www.xilinx.com/support/documentation/ip_documentation/clk_wiz/v5_1/pg065-clk-wiz.pdf
+
+Required properties:
+ - compatible: Must be 'xlnx,clocking-wizard'
+ - reg: Base and size of the cores register space
+ - clocks: Handle to input clock
+ - clock-names: Tuple containing 'clk_in1' and 's_axi_aclk'
+ - clock-output-names: Names for the output clocks
+
+Optional properties:
+ - speed-grade: Speed grade of the device (valid values are 1..3)
+
+Example:
+	clock-generator@40040000 {
+		reg = <0x40040000 0x1000>;
+		compatible = "xlnx,clocking-wizard";
+		speed-grade = <1>;
+		clock-names = "clk_in1", "s_axi_aclk";
+		clocks = <&clkc 15>, <&clkc 15>;
+		clock-output-names = "clk_out0", "clk_out1", "clk_out2",
+				     "clk_out3", "clk_out4", "clk_out5",
+				     "clk_out6", "clk_out7";
+	};
diff --git a/drivers/staging/comedi/Kconfig b/drivers/staging/comedi/Kconfig
index 152f4c12ea43..a8201fe87512 100644
--- a/drivers/staging/comedi/Kconfig
+++ b/drivers/staging/comedi/Kconfig
@@ -384,6 +384,7 @@ config COMEDI_DT282X
 
 config COMEDI_DMM32AT
 	tristate "Diamond Systems MM-32-AT PC/104 board support"
+	select COMEDI_8255
 	---help---
 	  Enable support for Diamond Systems MM-32-AT PC/104 boards
 
@@ -564,11 +565,14 @@ config COMEDI_S526
 endif # COMEDI_ISA_DRIVERS
 
 menuconfig COMEDI_PCI_DRIVERS
-	bool "Comedi PCI drivers"
+	tristate "Comedi PCI drivers"
 	depends on PCI
 	---help---
 	  Enable support for comedi PCI drivers.
 
+	  To compile this support as a module, choose M here: the module will
+	  be called comedi_pci.
+
 if COMEDI_PCI_DRIVERS
 
 config COMEDI_8255_PCI
@@ -595,14 +599,6 @@ config COMEDI_ADDI_WATCHDOG
 	  boards. This module will be automatically selected when needed. The
 	  module will be called addi_watchdog.
 
-config COMEDI_ADDI_APCI_035
-	tristate "ADDI-DATA APCI_035 support"
-	---help---
-	  Enable support for ADDI-DATA APCI_035 cards
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called addi_apci_035.
-
 config COMEDI_ADDI_APCI_1032
 	tristate "ADDI-DATA APCI_1032 support"
 	---help---
@@ -939,11 +935,11 @@ config COMEDI_CB_PCIDDA
 	  called cb_pcidda.
 
 config COMEDI_CB_PCIMDAS
-	tristate "MeasurementComputing PCIM-DAS1602/16 support"
+	tristate "MeasurementComputing PCIM-DAS1602/16, PCIe-DAS1602/16 support"
 	select COMEDI_8255
 	---help---
 	  Enable support for ComputerBoards/MeasurementComputing PCI Migration
-	  series PCIM-DAS1602/16
+	  series PCIM-DAS1602/16 and PCIe-DAS1602/16.
 
 	  To compile this driver as a module, choose M here: the module will be
 	  called cb_pcimdas.
@@ -1084,11 +1080,14 @@ config COMEDI_NI_TIOCMD
 endif # COMEDI_PCI_DRIVERS
 
 menuconfig COMEDI_PCMCIA_DRIVERS
-	bool "Comedi PCMCIA drivers"
+	tristate "Comedi PCMCIA drivers"
 	depends on PCMCIA
 	---help---
 	  Enable support for comedi PCMCIA drivers.
 
+	  To compile this support as a module, choose M here: the module will
+	  be called comedi_pcmcia.
+
 if COMEDI_PCMCIA_DRIVERS
 
 config COMEDI_CB_DAS16_CS
@@ -1160,11 +1159,14 @@ config COMEDI_QUATECH_DAQP_CS
 endif # COMEDI_PCMCIA_DRIVERS
 
 menuconfig COMEDI_USB_DRIVERS
-	bool "Comedi USB drivers"
+	tristate "Comedi USB drivers"
 	depends on USB
 	---help---
 	  Enable support for comedi USB drivers.
 
+	  To compile this support as a module, choose M here: the module will
+	  be called comedi_usb.
+
 if COMEDI_USB_DRIVERS
 
 config COMEDI_DT9812
diff --git a/drivers/staging/comedi/Makefile b/drivers/staging/comedi/Makefile
index fae2d9090006..7f9dfb3923ab 100644
--- a/drivers/staging/comedi/Makefile
+++ b/drivers/staging/comedi/Makefile
@@ -2,12 +2,13 @@ ccflags-$(CONFIG_COMEDI_DEBUG)		:= -DDEBUG
 
 comedi-y				:= comedi_fops.o range.o drivers.o \
 					   comedi_buf.o
-comedi-$(CONFIG_COMEDI_PCI_DRIVERS)	+= comedi_pci.o
-comedi-$(CONFIG_COMEDI_PCMCIA_DRIVERS)	+= comedi_pcmcia.o
-comedi-$(CONFIG_COMEDI_USB_DRIVERS)	+= comedi_usb.o
 comedi-$(CONFIG_PROC_FS)		+= proc.o
 comedi-$(CONFIG_COMPAT)			+= comedi_compat32.o
 
+obj-$(CONFIG_COMEDI_PCI_DRIVERS)	+= comedi_pci.o
+obj-$(CONFIG_COMEDI_PCMCIA_DRIVERS)	+= comedi_pcmcia.o
+obj-$(CONFIG_COMEDI_USB_DRIVERS)	+= comedi_usb.o
+
 obj-$(CONFIG_COMEDI)			+= comedi.o
 
 obj-$(CONFIG_COMEDI)			+= kcomedilib/
diff --git a/drivers/staging/comedi/comedi.h b/drivers/staging/comedi/comedi.h
index c8c99e65423b..745574077352 100644
--- a/drivers/staging/comedi/comedi.h
+++ b/drivers/staging/comedi/comedi.h
@@ -367,6 +367,8 @@ enum comedi_support_level {
 #define COMEDI_BUFCONFIG _IOR(CIO, 13, struct comedi_bufconfig)
 #define COMEDI_BUFINFO _IOWR(CIO, 14, struct comedi_bufinfo)
 #define COMEDI_POLL _IO(CIO, 15)
+#define COMEDI_SETRSUBD _IO(CIO, 16)
+#define COMEDI_SETWSUBD _IO(CIO, 17)
 
 /* structures */
 
@@ -514,17 +516,6 @@ struct comedi_bufinfo {
 
 #define COMEDI_MIN_SPEED	((unsigned int)0xffffffff)
 
-/* callback stuff */
-/* only relevant to kernel modules. */
-
-#define COMEDI_CB_EOS		1	/* end of scan */
-#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 */
-#define COMEDI_CB_ERROR		16	/* card error during acquisition */
-#define COMEDI_CB_OVERFLOW	32	/* buffer overflow/underflow */
-
 /**********************************************************/
 /* everything after this line is ALPHA */
 /**********************************************************/
diff --git a/drivers/staging/comedi/comedi_buf.c b/drivers/staging/comedi/comedi_buf.c
index c60a45ad12b9..19e7b229d15e 100644
--- a/drivers/staging/comedi/comedi_buf.c
+++ b/drivers/staging/comedi/comedi_buf.c
@@ -236,6 +236,7 @@ void comedi_buf_reset(struct comedi_subdevice *s)
 	async->buf_read_ptr = 0;
 
 	async->cur_chan = 0;
+	async->scans_done = 0;
 	async->scan_progress = 0;
 	async->munge_chan = 0;
 	async->munge_count = 0;
@@ -252,15 +253,15 @@ static unsigned int comedi_buf_write_n_available(struct comedi_subdevice *s)
 	return free_end - async->buf_write_alloc_count;
 }
 
-static unsigned int __comedi_buf_write_alloc(struct comedi_subdevice *s,
-					     unsigned int nbytes,
-					     int strict)
+/* allocates chunk for the writer from free buffer space */
+unsigned int comedi_buf_write_alloc(struct comedi_subdevice *s,
+				    unsigned int nbytes)
 {
 	struct comedi_async *async = s->async;
 	unsigned int available = comedi_buf_write_n_available(s);
 
 	if (nbytes > available)
-		nbytes = strict ? 0 : available;
+		nbytes = available;
 
 	async->buf_write_alloc_count += nbytes;
 
@@ -272,13 +273,6 @@ static unsigned int __comedi_buf_write_alloc(struct comedi_subdevice *s,
 
 	return nbytes;
 }
-
-/* allocates chunk for the writer from free buffer space */
-unsigned int comedi_buf_write_alloc(struct comedi_subdevice *s,
-				    unsigned int nbytes)
-{
-	return __comedi_buf_write_alloc(s, nbytes, 0);
-}
 EXPORT_SYMBOL_GPL(comedi_buf_write_alloc);
 
 /*
@@ -290,7 +284,7 @@ static unsigned int comedi_buf_munge(struct comedi_subdevice *s,
 {
 	struct comedi_async *async = s->async;
 	unsigned int count = 0;
-	const unsigned num_sample_bytes = bytes_per_sample(s);
+	const unsigned num_sample_bytes = comedi_bytes_per_sample(s);
 
 	if (!s->munge || (async->cmd.flags & CMDF_RAWDATA)) {
 		async->munge_count += num_bytes;
@@ -427,43 +421,11 @@ unsigned int comedi_buf_read_free(struct comedi_subdevice *s,
 }
 EXPORT_SYMBOL_GPL(comedi_buf_read_free);
 
-int comedi_buf_put(struct comedi_subdevice *s, unsigned short x)
-{
-	struct comedi_async *async = s->async;
-	unsigned int n = __comedi_buf_write_alloc(s, sizeof(short), 1);
-
-	if (n < sizeof(short)) {
-		async->events |= COMEDI_CB_ERROR;
-		return 0;
-	}
-	*(unsigned short *)(async->prealloc_buf + async->buf_write_ptr) = x;
-	comedi_buf_write_free(s, sizeof(short));
-	return 1;
-}
-EXPORT_SYMBOL_GPL(comedi_buf_put);
-
-int comedi_buf_get(struct comedi_subdevice *s, unsigned short *x)
+static void comedi_buf_memcpy_to(struct comedi_subdevice *s,
+				 const void *data, unsigned int num_bytes)
 {
 	struct comedi_async *async = s->async;
-	unsigned int n = comedi_buf_read_n_available(s);
-
-	if (n < sizeof(short))
-		return 0;
-	comedi_buf_read_alloc(s, sizeof(short));
-	*x = *(unsigned short *)(async->prealloc_buf + async->buf_read_ptr);
-	comedi_buf_read_free(s, sizeof(short));
-	return 1;
-}
-EXPORT_SYMBOL_GPL(comedi_buf_get);
-
-void comedi_buf_memcpy_to(struct comedi_subdevice *s, unsigned int offset,
-			  const void *data, unsigned int num_bytes)
-{
-	struct comedi_async *async = s->async;
-	unsigned int write_ptr = async->buf_write_ptr + offset;
-
-	if (write_ptr >= async->prealloc_bufsz)
-		write_ptr %= async->prealloc_bufsz;
+	unsigned int write_ptr = async->buf_write_ptr;
 
 	while (num_bytes) {
 		unsigned int block_size;
@@ -481,17 +443,13 @@ void comedi_buf_memcpy_to(struct comedi_subdevice *s, unsigned int offset,
 		write_ptr = 0;
 	}
 }
-EXPORT_SYMBOL_GPL(comedi_buf_memcpy_to);
 
-void comedi_buf_memcpy_from(struct comedi_subdevice *s, unsigned int offset,
-			    void *dest, unsigned int nbytes)
+static void comedi_buf_memcpy_from(struct comedi_subdevice *s,
+				   void *dest, unsigned int nbytes)
 {
 	void *src;
 	struct comedi_async *async = s->async;
-	unsigned int read_ptr = async->buf_read_ptr + offset;
-
-	if (read_ptr >= async->prealloc_bufsz)
-		read_ptr %= async->prealloc_bufsz;
+	unsigned int read_ptr = async->buf_read_ptr;
 
 	while (nbytes) {
 		unsigned int block_size;
@@ -509,69 +467,84 @@ void comedi_buf_memcpy_from(struct comedi_subdevice *s, unsigned int offset,
 		read_ptr = 0;
 	}
 }
-EXPORT_SYMBOL_GPL(comedi_buf_memcpy_from);
 
 /**
- * comedi_write_array_to_buffer - write data to comedi buffer
+ * comedi_buf_write_samples - write sample data to comedi buffer
  * @s: comedi_subdevice struct
- * @data: destination
- * @num_bytes: number of bytes to write
+ * @data: samples
+ * @nsamples: number of samples
  *
- * Writes up to num_bytes bytes of data to the comedi buffer associated with
- * the subdevice, marks it as written and updates the acquisition scan
- * progress.
+ * Writes nsamples to the comedi buffer associated with the subdevice, marks
+ * it as written and updates the acquisition scan progress.
  *
  * Returns the amount of data written in bytes.
  */
-unsigned int comedi_write_array_to_buffer(struct comedi_subdevice *s,
-					  const void *data,
-					  unsigned int num_bytes)
+unsigned int comedi_buf_write_samples(struct comedi_subdevice *s,
+				      const void *data, unsigned int nsamples)
 {
-	struct comedi_async *async = s->async;
-	unsigned int retval;
-
-	if (num_bytes == 0)
-		return 0;
+	unsigned int max_samples;
+	unsigned int nbytes;
 
-	retval = comedi_buf_write_alloc(s, num_bytes);
-	if (retval != num_bytes) {
+	/*
+	 * Make sure there is enough room in the buffer for all the samples.
+	 * If not, clamp the nsamples to the number that will fit, flag the
+	 * buffer overrun and add the samples that fit.
+	 */
+	max_samples = comedi_bytes_to_samples(s,
+					      comedi_buf_write_n_available(s));
+	if (nsamples > max_samples) {
 		dev_warn(s->device->class_dev, "buffer overrun\n");
-		async->events |= COMEDI_CB_OVERFLOW;
-		return 0;
+		s->async->events |= COMEDI_CB_OVERFLOW;
+		nsamples = max_samples;
 	}
 
-	comedi_buf_memcpy_to(s, 0, data, num_bytes);
-	comedi_buf_write_free(s, num_bytes);
-	comedi_inc_scan_progress(s, num_bytes);
-	async->events |= COMEDI_CB_BLOCK;
+	if (nsamples == 0)
+		return 0;
 
-	return num_bytes;
+	nbytes = comedi_buf_write_alloc(s,
+					comedi_samples_to_bytes(s, nsamples));
+	comedi_buf_memcpy_to(s, data, nbytes);
+	comedi_buf_write_free(s, nbytes);
+	comedi_inc_scan_progress(s, nbytes);
+	s->async->events |= COMEDI_CB_BLOCK;
+
+	return nbytes;
 }
-EXPORT_SYMBOL_GPL(comedi_write_array_to_buffer);
+EXPORT_SYMBOL_GPL(comedi_buf_write_samples);
 
 /**
- * comedi_read_array_from_buffer - read data from comedi buffer
+ * comedi_buf_read_samples - read sample data from comedi buffer
  * @s: comedi_subdevice struct
  * @data: destination
- * @num_bytes: number of bytes to read
+ * @nsamples: maximum number of samples to read
  *
- * Reads up to num_bytes bytes of data from the comedi buffer associated with
- * the subdevice, marks it as read and updates the acquisition scan progress.
+ * Reads up to nsamples from the comedi buffer associated with the subdevice,
+ * marks it as read and updates the acquisition scan progress.
  *
  * Returns the amount of data read in bytes.
  */
-unsigned int comedi_read_array_from_buffer(struct comedi_subdevice *s,
-					   void *data, unsigned int num_bytes)
+unsigned int comedi_buf_read_samples(struct comedi_subdevice *s,
+				     void *data, unsigned int nsamples)
 {
-	if (num_bytes == 0)
+	unsigned int max_samples;
+	unsigned int nbytes;
+
+	/* clamp nsamples to the number of full samples available */
+	max_samples = comedi_bytes_to_samples(s,
+					      comedi_buf_read_n_available(s));
+	if (nsamples > max_samples)
+		nsamples = max_samples;
+
+	if (nsamples == 0)
 		return 0;
 
-	num_bytes = comedi_buf_read_alloc(s, num_bytes);
-	comedi_buf_memcpy_from(s, 0, data, num_bytes);
-	comedi_buf_read_free(s, num_bytes);
-	comedi_inc_scan_progress(s, num_bytes);
+	nbytes = comedi_buf_read_alloc(s,
+				       comedi_samples_to_bytes(s, nsamples));
+	comedi_buf_memcpy_from(s, data, nbytes);
+	comedi_buf_read_free(s, nbytes);
+	comedi_inc_scan_progress(s, nbytes);
 	s->async->events |= COMEDI_CB_BLOCK;
 
-	return num_bytes;
+	return nbytes;
 }
-EXPORT_SYMBOL_GPL(comedi_read_array_from_buffer);
+EXPORT_SYMBOL_GPL(comedi_buf_read_samples);
diff --git a/drivers/staging/comedi/comedi_compat32.c b/drivers/staging/comedi/comedi_compat32.c
index 9b6f96f1591c..5a4c74f703b3 100644
--- a/drivers/staging/comedi/comedi_compat32.c
+++ b/drivers/staging/comedi/comedi_compat32.c
@@ -416,6 +416,8 @@ static inline int raw_ioctl(struct file *file, unsigned int cmd,
 	case COMEDI_UNLOCK:
 	case COMEDI_CANCEL:
 	case COMEDI_POLL:
+	case COMEDI_SETRSUBD:
+	case COMEDI_SETWSUBD:
 		/* No translation needed. */
 		rc = translated_ioctl(file, cmd, arg);
 		break;
diff --git a/drivers/staging/comedi/comedi_fops.c b/drivers/staging/comedi/comedi_fops.c
index 9c32f0276009..f143cb64d69e 100644
--- a/drivers/staging/comedi/comedi_fops.c
+++ b/drivers/staging/comedi/comedi_fops.c
@@ -43,6 +43,22 @@
 
 #include "comedi_internal.h"
 
+/**
+ * struct comedi_file - per-file private data for comedi device
+ * @dev: comedi_device struct
+ * @read_subdev: current "read" subdevice
+ * @write_subdev: current "write" subdevice
+ * @last_detach_count: last known detach count
+ * @last_attached: last known attached/detached state
+ */
+struct comedi_file {
+	struct comedi_device *dev;
+	struct comedi_subdevice *read_subdev;
+	struct comedi_subdevice *write_subdev;
+	unsigned int last_detach_count;
+	bool last_attached:1;
+};
+
 #define COMEDI_NUM_MINORS 0x100
 #define COMEDI_NUM_SUBDEVICE_MINORS	\
 	(COMEDI_NUM_MINORS - COMEDI_NUM_BOARD_MINORS)
@@ -239,6 +255,54 @@ comedi_write_subdevice(const struct comedi_device *dev, unsigned int minor)
 	return dev->write_subdev;
 }
 
+static void comedi_file_reset(struct file *file)
+{
+	struct comedi_file *cfp = file->private_data;
+	struct comedi_device *dev = cfp->dev;
+	struct comedi_subdevice *s, *read_s, *write_s;
+	unsigned int minor = iminor(file_inode(file));
+
+	read_s = dev->read_subdev;
+	write_s = dev->write_subdev;
+	if (minor >= COMEDI_NUM_BOARD_MINORS) {
+		s = comedi_subdevice_from_minor(dev, minor);
+		if (s == NULL || s->subdev_flags & SDF_CMD_READ)
+			read_s = s;
+		if (s == NULL || s->subdev_flags & SDF_CMD_WRITE)
+			write_s = s;
+	}
+	cfp->last_attached = dev->attached;
+	cfp->last_detach_count = dev->detach_count;
+	ACCESS_ONCE(cfp->read_subdev) = read_s;
+	ACCESS_ONCE(cfp->write_subdev) = write_s;
+}
+
+static void comedi_file_check(struct file *file)
+{
+	struct comedi_file *cfp = file->private_data;
+	struct comedi_device *dev = cfp->dev;
+
+	if (cfp->last_attached != dev->attached ||
+	    cfp->last_detach_count != dev->detach_count)
+		comedi_file_reset(file);
+}
+
+static struct comedi_subdevice *comedi_file_read_subdevice(struct file *file)
+{
+	struct comedi_file *cfp = file->private_data;
+
+	comedi_file_check(file);
+	return ACCESS_ONCE(cfp->read_subdev);
+}
+
+static struct comedi_subdevice *comedi_file_write_subdevice(struct file *file)
+{
+	struct comedi_file *cfp = file->private_data;
+
+	comedi_file_check(file);
+	return ACCESS_ONCE(cfp->write_subdev);
+}
+
 static int resize_async_buffer(struct comedi_device *dev,
 			       struct comedi_subdevice *s, unsigned new_size)
 {
@@ -776,7 +840,6 @@ static int do_devinfo_ioctl(struct comedi_device *dev,
 			    struct comedi_devinfo __user *arg,
 			    struct file *file)
 {
-	const unsigned minor = iminor(file_inode(file));
 	struct comedi_subdevice *s;
 	struct comedi_devinfo devinfo;
 
@@ -788,13 +851,13 @@ static int do_devinfo_ioctl(struct comedi_device *dev,
 	strlcpy(devinfo.driver_name, dev->driver->driver_name, COMEDI_NAMELEN);
 	strlcpy(devinfo.board_name, dev->board_name, COMEDI_NAMELEN);
 
-	s = comedi_read_subdevice(dev, minor);
+	s = comedi_file_read_subdevice(file);
 	if (s)
 		devinfo.read_subdevice = s->index;
 	else
 		devinfo.read_subdevice = -1;
 
-	s = comedi_write_subdevice(dev, minor);
+	s = comedi_file_write_subdevice(file);
 	if (s)
 		devinfo.write_subdevice = s->index;
 	else
@@ -991,7 +1054,7 @@ static int do_bufinfo_ioctl(struct comedi_device *dev,
 	if (s->busy != file)
 		return -EACCES;
 
-	if (bi.bytes_read && (s->subdev_flags & SDF_CMD_READ)) {
+	if (bi.bytes_read && !(async->cmd.flags & CMDF_WRITE)) {
 		bi.bytes_read = comedi_buf_read_alloc(s, bi.bytes_read);
 		comedi_buf_read_free(s, bi.bytes_read);
 
@@ -1001,7 +1064,7 @@ static int do_bufinfo_ioctl(struct comedi_device *dev,
 		}
 	}
 
-	if (bi.bytes_written && (s->subdev_flags & SDF_CMD_WRITE)) {
+	if (bi.bytes_written && (async->cmd.flags & CMDF_WRITE)) {
 		bi.bytes_written =
 		    comedi_buf_write_alloc(s, bi.bytes_written);
 		comedi_buf_write_free(s, bi.bytes_written);
@@ -1451,6 +1514,21 @@ static int __comedi_get_user_cmd(struct comedi_device *dev,
 		return -EINVAL;
 	}
 
+	/*
+	 * Set the CMDF_WRITE flag to the correct state if the subdevice
+	 * supports only "read" commands or only "write" commands.
+	 */
+	switch (s->subdev_flags & (SDF_CMD_READ | SDF_CMD_WRITE)) {
+	case SDF_CMD_READ:
+		cmd->flags &= ~CMDF_WRITE;
+		break;
+	case SDF_CMD_WRITE:
+		cmd->flags |= CMDF_WRITE;
+		break;
+	default:
+		break;
+	}
+
 	return 0;
 }
 
@@ -1552,9 +1630,7 @@ static int do_cmd_ioctl(struct comedi_device *dev,
 
 	comedi_buf_reset(s);
 
-	async->cb_mask =
-	    COMEDI_CB_EOA | COMEDI_CB_BLOCK | COMEDI_CB_ERROR |
-	    COMEDI_CB_OVERFLOW;
+	async->cb_mask = COMEDI_CB_BLOCK | COMEDI_CB_CANCEL_MASK;
 	if (async->cmd.flags & CMDF_WAKE_EOS)
 		async->cb_mask |= COMEDI_CB_EOS;
 
@@ -1720,7 +1796,6 @@ static int do_cancel_ioctl(struct comedi_device *dev, unsigned long arg,
 			   void *file)
 {
 	struct comedi_subdevice *s;
-	int ret;
 
 	if (arg >= dev->n_subdevices)
 		return -EINVAL;
@@ -1734,9 +1809,7 @@ static int do_cancel_ioctl(struct comedi_device *dev, unsigned long arg,
 	if (s->busy != file)
 		return -EBUSY;
 
-	ret = do_cancel(dev, s);
-
-	return ret;
+	return do_cancel(dev, s);
 }
 
 /*
@@ -1774,11 +1847,96 @@ static int do_poll_ioctl(struct comedi_device *dev, unsigned long arg,
 	return -EINVAL;
 }
 
+/*
+ * COMEDI_SETRSUBD ioctl
+ * sets the current "read" subdevice on a per-file basis
+ *
+ * arg:
+ *	subdevice number
+ *
+ * reads:
+ *	nothing
+ *
+ * writes:
+ *	nothing
+ */
+static int do_setrsubd_ioctl(struct comedi_device *dev, unsigned long arg,
+			     struct file *file)
+{
+	struct comedi_file *cfp = file->private_data;
+	struct comedi_subdevice *s_old, *s_new;
+
+	if (arg >= dev->n_subdevices)
+		return -EINVAL;
+
+	s_new = &dev->subdevices[arg];
+	s_old = comedi_file_read_subdevice(file);
+	if (s_old == s_new)
+		return 0;	/* no change */
+
+	if (!(s_new->subdev_flags & SDF_CMD_READ))
+		return -EINVAL;
+
+	/*
+	 * Check the file isn't still busy handling a "read" command on the
+	 * old subdevice (if any).
+	 */
+	if (s_old && s_old->busy == file && s_old->async &&
+	    !(s_old->async->cmd.flags & CMDF_WRITE))
+		return -EBUSY;
+
+	ACCESS_ONCE(cfp->read_subdev) = s_new;
+	return 0;
+}
+
+/*
+ * COMEDI_SETWSUBD ioctl
+ * sets the current "write" subdevice on a per-file basis
+ *
+ * arg:
+ *	subdevice number
+ *
+ * reads:
+ *	nothing
+ *
+ * writes:
+ *	nothing
+ */
+static int do_setwsubd_ioctl(struct comedi_device *dev, unsigned long arg,
+			     struct file *file)
+{
+	struct comedi_file *cfp = file->private_data;
+	struct comedi_subdevice *s_old, *s_new;
+
+	if (arg >= dev->n_subdevices)
+		return -EINVAL;
+
+	s_new = &dev->subdevices[arg];
+	s_old = comedi_file_write_subdevice(file);
+	if (s_old == s_new)
+		return 0;	/* no change */
+
+	if (!(s_new->subdev_flags & SDF_CMD_WRITE))
+		return -EINVAL;
+
+	/*
+	 * Check the file isn't still busy handling a "write" command on the
+	 * old subdevice (if any).
+	 */
+	if (s_old && s_old->busy == file && s_old->async &&
+	    (s_old->async->cmd.flags & CMDF_WRITE))
+		return -EBUSY;
+
+	ACCESS_ONCE(cfp->write_subdev) = s_new;
+	return 0;
+}
+
 static long comedi_unlocked_ioctl(struct file *file, unsigned int cmd,
 				  unsigned long arg)
 {
-	const unsigned minor = iminor(file_inode(file));
-	struct comedi_device *dev = file->private_data;
+	unsigned minor = iminor(file_inode(file));
+	struct comedi_file *cfp = file->private_data;
+	struct comedi_device *dev = cfp->dev;
 	int rc;
 
 	mutex_lock(&dev->mutex);
@@ -1867,6 +2025,12 @@ static long comedi_unlocked_ioctl(struct file *file, unsigned int cmd,
 	case COMEDI_POLL:
 		rc = do_poll_ioctl(dev, arg, file);
 		break;
+	case COMEDI_SETRSUBD:
+		rc = do_setrsubd_ioctl(dev, arg, file);
+		break;
+	case COMEDI_SETWSUBD:
+		rc = do_setwsubd_ioctl(dev, arg, file);
+		break;
 	default:
 		rc = -ENOTTY;
 		break;
@@ -1900,8 +2064,8 @@ static struct vm_operations_struct comedi_vm_ops = {
 
 static int comedi_mmap(struct file *file, struct vm_area_struct *vma)
 {
-	const unsigned minor = iminor(file_inode(file));
-	struct comedi_device *dev = file->private_data;
+	struct comedi_file *cfp = file->private_data;
+	struct comedi_device *dev = cfp->dev;
 	struct comedi_subdevice *s;
 	struct comedi_async *async;
 	struct comedi_buf_map *bm = NULL;
@@ -1927,9 +2091,9 @@ static int comedi_mmap(struct file *file, struct vm_area_struct *vma)
 	}
 
 	if (vma->vm_flags & VM_WRITE)
-		s = comedi_write_subdevice(dev, minor);
+		s = comedi_file_write_subdevice(file);
 	else
-		s = comedi_read_subdevice(dev, minor);
+		s = comedi_file_read_subdevice(file);
 	if (!s) {
 		retval = -EINVAL;
 		goto done;
@@ -1992,8 +2156,8 @@ done:
 static unsigned int comedi_poll(struct file *file, poll_table *wait)
 {
 	unsigned int mask = 0;
-	const unsigned minor = iminor(file_inode(file));
-	struct comedi_device *dev = file->private_data;
+	struct comedi_file *cfp = file->private_data;
+	struct comedi_device *dev = cfp->dev;
 	struct comedi_subdevice *s;
 
 	mutex_lock(&dev->mutex);
@@ -2003,21 +2167,23 @@ static unsigned int comedi_poll(struct file *file, poll_table *wait)
 		goto done;
 	}
 
-	s = comedi_read_subdevice(dev, minor);
+	s = comedi_file_read_subdevice(file);
 	if (s && s->async) {
 		poll_wait(file, &s->async->wait_head, wait);
 		if (!s->busy || !comedi_is_subdevice_running(s) ||
+		    (s->async->cmd.flags & CMDF_WRITE) ||
 		    comedi_buf_read_n_available(s) > 0)
 			mask |= POLLIN | POLLRDNORM;
 	}
 
-	s = comedi_write_subdevice(dev, minor);
+	s = comedi_file_write_subdevice(file);
 	if (s && s->async) {
-		unsigned int bps = bytes_per_sample(s);
+		unsigned int bps = comedi_bytes_per_sample(s);
 
 		poll_wait(file, &s->async->wait_head, wait);
 		comedi_buf_write_alloc(s, s->async->prealloc_bufsz);
 		if (!s->busy || !comedi_is_subdevice_running(s) ||
+		    !(s->async->cmd.flags & CMDF_WRITE) ||
 		    comedi_buf_write_n_allocated(s) >= bps)
 			mask |= POLLOUT | POLLWRNORM;
 	}
@@ -2034,8 +2200,8 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
 	struct comedi_async *async;
 	int n, m, count = 0, retval = 0;
 	DECLARE_WAITQUEUE(wait, current);
-	const unsigned minor = iminor(file_inode(file));
-	struct comedi_device *dev = file->private_data;
+	struct comedi_file *cfp = file->private_data;
+	struct comedi_device *dev = cfp->dev;
 	bool on_wait_queue = false;
 	bool attach_locked;
 	unsigned int old_detach_count;
@@ -2051,7 +2217,7 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
 		goto out;
 	}
 
-	s = comedi_write_subdevice(dev, minor);
+	s = comedi_file_write_subdevice(file);
 	if (!s || !s->async) {
 		retval = -EIO;
 		goto out;
@@ -2065,6 +2231,10 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
 		retval = -EACCES;
 		goto out;
 	}
+	if (!(async->cmd.flags & CMDF_WRITE)) {
+		retval = -EINVAL;
+		goto out;
+	}
 
 	add_wait_queue(&async->wait_head, &wait);
 	on_wait_queue = true;
@@ -2099,7 +2269,7 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
 				 * meantime!), but check the subdevice pointer
 				 * as well just in case.
 				 */
-				new_s = comedi_write_subdevice(dev, minor);
+				new_s = comedi_file_write_subdevice(file);
 				if (dev->attached &&
 				    old_detach_count == dev->detach_count &&
 				    s == new_s && new_s->async == async)
@@ -2136,6 +2306,10 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
 				retval = -EACCES;
 				break;
 			}
+			if (!(async->cmd.flags & CMDF_WRITE)) {
+				retval = -EINVAL;
+				break;
+			}
 			continue;
 		}
 
@@ -2170,8 +2344,8 @@ static ssize_t comedi_read(struct file *file, char __user *buf, size_t nbytes,
 	struct comedi_async *async;
 	int n, m, count = 0, retval = 0;
 	DECLARE_WAITQUEUE(wait, current);
-	const unsigned minor = iminor(file_inode(file));
-	struct comedi_device *dev = file->private_data;
+	struct comedi_file *cfp = file->private_data;
+	struct comedi_device *dev = cfp->dev;
 	unsigned int old_detach_count;
 	bool become_nonbusy = false;
 	bool attach_locked;
@@ -2187,7 +2361,7 @@ static ssize_t comedi_read(struct file *file, char __user *buf, size_t nbytes,
 		goto out;
 	}
 
-	s = comedi_read_subdevice(dev, minor);
+	s = comedi_file_read_subdevice(file);
 	if (!s || !s->async) {
 		retval = -EIO;
 		goto out;
@@ -2200,6 +2374,10 @@ static ssize_t comedi_read(struct file *file, char __user *buf, size_t nbytes,
 		retval = -EACCES;
 		goto out;
 	}
+	if (async->cmd.flags & CMDF_WRITE) {
+		retval = -EINVAL;
+		goto out;
+	}
 
 	add_wait_queue(&async->wait_head, &wait);
 	while (nbytes > 0 && !retval) {
@@ -2239,6 +2417,10 @@ static ssize_t comedi_read(struct file *file, char __user *buf, size_t nbytes,
 				retval = -EACCES;
 				break;
 			}
+			if (async->cmd.flags & CMDF_WRITE) {
+				retval = -EINVAL;
+				break;
+			}
 			continue;
 		}
 		m = copy_to_user(buf, async->prealloc_buf +
@@ -2276,7 +2458,7 @@ static ssize_t comedi_read(struct file *file, char __user *buf, size_t nbytes,
 		 * meantime!), but check the subdevice pointer as well just in
 		 * case.
 		 */
-		new_s = comedi_read_subdevice(dev, minor);
+		new_s = comedi_file_read_subdevice(file);
 		if (dev->attached && old_detach_count == dev->detach_count &&
 		    s == new_s && new_s->async == async) {
 			if (become_nonbusy || comedi_buf_n_bytes_ready(s) == 0)
@@ -2294,6 +2476,7 @@ out:
 static int comedi_open(struct inode *inode, struct file *file)
 {
 	const unsigned minor = iminor(inode);
+	struct comedi_file *cfp;
 	struct comedi_device *dev = comedi_dev_get_from_minor(minor);
 	int rc;
 
@@ -2302,6 +2485,12 @@ static int comedi_open(struct inode *inode, struct file *file)
 		return -ENODEV;
 	}
 
+	cfp = kzalloc(sizeof(*cfp), GFP_KERNEL);
+	if (!cfp)
+		return -ENOMEM;
+
+	cfp->dev = dev;
+
 	mutex_lock(&dev->mutex);
 	if (!dev->attached && !capable(CAP_NET_ADMIN)) {
 		dev_dbg(dev->class_dev, "not attached and not CAP_NET_ADMIN\n");
@@ -2323,26 +2512,31 @@ static int comedi_open(struct inode *inode, struct file *file)
 	}
 
 	dev->use_count++;
-	file->private_data = dev;
+	file->private_data = cfp;
+	comedi_file_reset(file);
 	rc = 0;
 
 out:
 	mutex_unlock(&dev->mutex);
-	if (rc)
+	if (rc) {
 		comedi_dev_put(dev);
+		kfree(cfp);
+	}
 	return rc;
 }
 
 static int comedi_fasync(int fd, struct file *file, int on)
 {
-	struct comedi_device *dev = file->private_data;
+	struct comedi_file *cfp = file->private_data;
+	struct comedi_device *dev = cfp->dev;
 
 	return fasync_helper(fd, file, on, &dev->async_queue);
 }
 
 static int comedi_close(struct inode *inode, struct file *file)
 {
-	struct comedi_device *dev = file->private_data;
+	struct comedi_file *cfp = file->private_data;
+	struct comedi_device *dev = cfp->dev;
 	struct comedi_subdevice *s = NULL;
 	int i;
 
@@ -2368,6 +2562,7 @@ static int comedi_close(struct inode *inode, struct file *file)
 
 	mutex_unlock(&dev->mutex);
 	comedi_dev_put(dev);
+	kfree(cfp);
 
 	return 0;
 }
@@ -2395,14 +2590,14 @@ void comedi_event(struct comedi_device *dev, struct comedi_subdevice *s)
 	if (!comedi_is_subdevice_running(s))
 		return;
 
-	if (s->
-	    async->events & (COMEDI_CB_EOA | COMEDI_CB_ERROR |
-			     COMEDI_CB_OVERFLOW)) {
+	if (s->async->events & COMEDI_CB_CANCEL_MASK)
 		runflags_mask |= SRF_RUNNING;
-	}
-	/* remember if an error event has occurred, so an error
-	 * can be returned the next time the user does a read() */
-	if (s->async->events & (COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW)) {
+
+	/*
+	 * Remember if an error event has occurred, so an error
+	 * can be returned the next time the user does a read().
+	 */
+	if (s->async->events & COMEDI_CB_ERROR_MASK) {
 		runflags_mask |= SRF_ERROR;
 		runflags |= SRF_ERROR;
 	}
diff --git a/drivers/staging/comedi/comedi_pci.c b/drivers/staging/comedi/comedi_pci.c
index aa0795a2660e..6ba59c977006 100644
--- a/drivers/staging/comedi/comedi_pci.c
+++ b/drivers/staging/comedi/comedi_pci.c
@@ -16,6 +16,7 @@
  * GNU General Public License for more details.
  */
 
+#include <linux/module.h>
 #include <linux/pci.h>
 #include <linux/interrupt.h>
 
@@ -168,3 +169,18 @@ void comedi_pci_driver_unregister(struct comedi_driver *comedi_driver,
 	comedi_driver_unregister(comedi_driver);
 }
 EXPORT_SYMBOL_GPL(comedi_pci_driver_unregister);
+
+static int __init comedi_pci_init(void)
+{
+	return 0;
+}
+module_init(comedi_pci_init);
+
+static void __exit comedi_pci_exit(void)
+{
+}
+module_exit(comedi_pci_exit);
+
+MODULE_AUTHOR("http://www.comedi.org");
+MODULE_DESCRIPTION("Comedi PCI interface module");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/comedi_pcmcia.c b/drivers/staging/comedi/comedi_pcmcia.c
index 9d49d5d01ad9..0529bae8e5ac 100644
--- a/drivers/staging/comedi/comedi_pcmcia.c
+++ b/drivers/staging/comedi/comedi_pcmcia.c
@@ -16,6 +16,7 @@
  * GNU General Public License for more details.
  */
 
+#include <linux/module.h>
 #include <linux/kernel.h>
 
 #include <pcmcia/cistpl.h>
@@ -154,3 +155,18 @@ void comedi_pcmcia_driver_unregister(struct comedi_driver *comedi_driver,
 	comedi_driver_unregister(comedi_driver);
 }
 EXPORT_SYMBOL_GPL(comedi_pcmcia_driver_unregister);
+
+static int __init comedi_pcmcia_init(void)
+{
+	return 0;
+}
+module_init(comedi_pcmcia_init);
+
+static void __exit comedi_pcmcia_exit(void)
+{
+}
+module_exit(comedi_pcmcia_exit);
+
+MODULE_AUTHOR("http://www.comedi.org");
+MODULE_DESCRIPTION("Comedi PCMCIA interface module");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/comedi_usb.c b/drivers/staging/comedi/comedi_usb.c
index 13f18bef6091..0b862a64c049 100644
--- a/drivers/staging/comedi/comedi_usb.c
+++ b/drivers/staging/comedi/comedi_usb.c
@@ -16,6 +16,7 @@
  * GNU General Public License for more details.
  */
 
+#include <linux/module.h>
 #include <linux/usb.h>
 
 #include "comedidev.h"
@@ -114,3 +115,18 @@ void comedi_usb_driver_unregister(struct comedi_driver *comedi_driver,
 	comedi_driver_unregister(comedi_driver);
 }
 EXPORT_SYMBOL_GPL(comedi_usb_driver_unregister);
+
+static int __init comedi_usb_init(void)
+{
+	return 0;
+}
+module_init(comedi_usb_init);
+
+static void __exit comedi_usb_exit(void)
+{
+}
+module_exit(comedi_usb_exit);
+
+MODULE_AUTHOR("http://www.comedi.org");
+MODULE_DESCRIPTION("Comedi USB interface module");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/comedidev.h b/drivers/staging/comedi/comedidev.h
index 1b2bbd56f6ef..77be191988ca 100644
--- a/drivers/staging/comedi/comedidev.h
+++ b/drivers/staging/comedi/comedidev.h
@@ -121,6 +121,7 @@ struct comedi_buf_map {
  * @buf_read_ptr:	buffer position for reader
  * @cur_chan:		current position in chanlist for scan (for those
  *			drivers that use it)
+ * @scans_done:		the number of scans completed (COMEDI_CB_EOS)
  * @scan_progress:	amount received or sent for current scan (in bytes)
  * @munge_chan:		current position in chanlist for "munging"
  * @munge_count:	"munge" count (in bytes, modulo 2**32)
@@ -201,6 +202,7 @@ struct comedi_async {
 	unsigned int buf_write_ptr;
 	unsigned int buf_read_ptr;
 	unsigned int cur_chan;
+	unsigned int scans_done;
 	unsigned int scan_progress;
 	unsigned int munge_chan;
 	unsigned int munge_count;
@@ -213,6 +215,28 @@ struct comedi_async {
 		       unsigned int x);
 };
 
+/**
+ * comedi_async callback "events"
+ * @COMEDI_CB_EOS:		end-of-scan
+ * @COMEDI_CB_EOA:		end-of-acquisition/output
+ * @COMEDI_CB_BLOCK:		data has arrived, wakes up read() / write()
+ * @COMEDI_CB_EOBUF:		DEPRECATED: end of buffer
+ * @COMEDI_CB_ERROR:		card error during acquisition
+ * @COMEDI_CB_OVERFLOW:		buffer overflow/underflow
+ *
+ * @COMEDI_CB_ERROR_MASK:	events that indicate an error has occurred
+ * @COMEDI_CB_CANCEL_MASK:	events that will cancel an async command
+ */
+#define COMEDI_CB_EOS		(1 << 0)
+#define COMEDI_CB_EOA		(1 << 1)
+#define COMEDI_CB_BLOCK		(1 << 2)
+#define COMEDI_CB_EOBUF		(1 << 3)
+#define COMEDI_CB_ERROR		(1 << 4)
+#define COMEDI_CB_OVERFLOW	(1 << 5)
+
+#define COMEDI_CB_ERROR_MASK	(COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW)
+#define COMEDI_CB_CANCEL_MASK	(COMEDI_CB_EOA | COMEDI_CB_ERROR_MASK)
+
 struct comedi_driver {
 	struct comedi_driver *next;
 
@@ -391,12 +415,61 @@ static inline unsigned int comedi_offset_munge(struct comedi_subdevice *s,
 	return val ^ s->maxdata ^ (s->maxdata >> 1);
 }
 
-static inline unsigned int bytes_per_sample(const struct comedi_subdevice *subd)
+/**
+ * comedi_bytes_per_sample - determine subdevice sample size
+ * @s:		comedi_subdevice struct
+ *
+ * The sample size will be 4 (sizeof int) or 2 (sizeof short) depending on
+ * whether the SDF_LSAMPL subdevice flag is set or not.
+ *
+ * Returns the subdevice sample size.
+ */
+static inline unsigned int comedi_bytes_per_sample(struct comedi_subdevice *s)
+{
+	return s->subdev_flags & SDF_LSAMPL ? sizeof(int) : sizeof(short);
+}
+
+/**
+ * comedi_sample_shift - determine log2 of subdevice sample size
+ * @s:		comedi_subdevice struct
+ *
+ * The sample size will be 4 (sizeof int) or 2 (sizeof short) depending on
+ * whether the SDF_LSAMPL subdevice flag is set or not.  The log2 of the
+ * sample size will be 2 or 1 and can be used as the right operand of a
+ * bit-shift operator to multiply or divide something by the sample size.
+ *
+ * Returns log2 of the subdevice sample size.
+ */
+static inline unsigned int comedi_sample_shift(struct comedi_subdevice *s)
 {
-	if (subd->subdev_flags & SDF_LSAMPL)
-		return sizeof(unsigned int);
+	return s->subdev_flags & SDF_LSAMPL ? 2 : 1;
+}
 
-	return sizeof(short);
+/**
+ * comedi_bytes_to_samples - converts a number of bytes to a number of samples
+ * @s:		comedi_subdevice struct
+ * @nbytes:	number of bytes
+ *
+ * Returns the number of bytes divided by the subdevice sample size.
+ */
+static inline unsigned int comedi_bytes_to_samples(struct comedi_subdevice *s,
+						   unsigned int nbytes)
+{
+	return nbytes >> comedi_sample_shift(s);
+}
+
+/**
+ * comedi_samples_to_bytes - converts a number of samples to a number of bytes
+ * @s:		comedi_subdevice struct
+ * @nsamples:	number of samples
+ *
+ * Returns the number of samples multiplied by the subdevice sample size.
+ * Does not check for arithmetic overflow.
+ */
+static inline unsigned int comedi_samples_to_bytes(struct comedi_subdevice *s,
+						   unsigned int nsamples)
+{
+	return nsamples << comedi_sample_shift(s);
 }
 
 /*
@@ -419,18 +492,10 @@ unsigned int comedi_buf_read_n_available(struct comedi_subdevice *s);
 unsigned int comedi_buf_read_alloc(struct comedi_subdevice *s, unsigned int n);
 unsigned int comedi_buf_read_free(struct comedi_subdevice *s, unsigned int n);
 
-int comedi_buf_put(struct comedi_subdevice *s, unsigned short x);
-int comedi_buf_get(struct comedi_subdevice *s, unsigned short *x);
-
-void comedi_buf_memcpy_to(struct comedi_subdevice *s, unsigned int offset,
-			  const void *source, unsigned int num_bytes);
-void comedi_buf_memcpy_from(struct comedi_subdevice *s, unsigned int offset,
-			    void *destination, unsigned int num_bytes);
-unsigned int comedi_write_array_to_buffer(struct comedi_subdevice *s,
-					  const void *data,
-					  unsigned int num_bytes);
-unsigned int comedi_read_array_from_buffer(struct comedi_subdevice *s,
-					   void *data, unsigned int num_bytes);
+unsigned int comedi_buf_write_samples(struct comedi_subdevice *s,
+				      const void *data, unsigned int nsamples);
+unsigned int comedi_buf_read_samples(struct comedi_subdevice *s,
+				     void *data, unsigned int nsamples);
 
 /* drivers.c - general comedi driver functions */
 
@@ -451,6 +516,10 @@ int comedi_dio_insn_config(struct comedi_device *, struct comedi_subdevice *,
 unsigned int comedi_dio_update_state(struct comedi_subdevice *,
 				     unsigned int *data);
 unsigned int comedi_bytes_per_scan(struct comedi_subdevice *s);
+unsigned int comedi_nscans_left(struct comedi_subdevice *s,
+				unsigned int nscans);
+unsigned int comedi_nsamples_left(struct comedi_subdevice *s,
+				  unsigned int nsamples);
 void comedi_inc_scan_progress(struct comedi_subdevice *s,
 			      unsigned int num_bytes);
 
@@ -493,8 +562,6 @@ void comedi_driver_unregister(struct comedi_driver *);
 	module_driver(__comedi_driver, comedi_driver_register, \
 			comedi_driver_unregister)
 
-#ifdef CONFIG_COMEDI_PCI_DRIVERS
-
 /* comedi_pci.c - comedi PCI driver specific functions */
 
 /*
@@ -538,36 +605,6 @@ void comedi_pci_driver_unregister(struct comedi_driver *, struct pci_driver *);
 	module_driver(__comedi_driver, comedi_pci_driver_register, \
 			comedi_pci_driver_unregister, &(__pci_driver))
 
-#else
-
-/*
- * Some of the comedi mixed ISA/PCI drivers call the PCI specific
- * functions. Provide some dummy functions if CONFIG_COMEDI_PCI_DRIVERS
- * is not enabled.
- */
-
-static inline struct pci_dev *comedi_to_pci_dev(struct comedi_device *dev)
-{
-	return NULL;
-}
-
-static inline int comedi_pci_enable(struct comedi_device *dev)
-{
-	return -ENOSYS;
-}
-
-static inline void comedi_pci_disable(struct comedi_device *dev)
-{
-}
-
-static inline void comedi_pci_detach(struct comedi_device *dev)
-{
-}
-
-#endif /* CONFIG_COMEDI_PCI_DRIVERS */
-
-#ifdef CONFIG_COMEDI_PCMCIA_DRIVERS
-
 /* comedi_pcmcia.c - comedi PCMCIA driver specific functions */
 
 struct pcmcia_driver;
@@ -601,10 +638,6 @@ void comedi_pcmcia_driver_unregister(struct comedi_driver *,
 	module_driver(__comedi_driver, comedi_pcmcia_driver_register, \
 			comedi_pcmcia_driver_unregister, &(__pcmcia_driver))
 
-#endif /* CONFIG_COMEDI_PCMCIA_DRIVERS */
-
-#ifdef CONFIG_COMEDI_USB_DRIVERS
-
 /* comedi_usb.c - comedi USB driver specific functions */
 
 struct usb_driver;
@@ -634,6 +667,4 @@ void comedi_usb_driver_unregister(struct comedi_driver *, struct usb_driver *);
 	module_driver(__comedi_driver, comedi_usb_driver_register, \
 			comedi_usb_driver_unregister, &(__usb_driver))
 
-#endif /* CONFIG_COMEDI_USB_DRIVERS */
-
 #endif /* _COMEDIDEV_H */
diff --git a/drivers/staging/comedi/drivers.c b/drivers/staging/comedi/drivers.c
index 3e5bccbc9c39..61802d7947ae 100644
--- a/drivers/staging/comedi/drivers.c
+++ b/drivers/staging/comedi/drivers.c
@@ -109,6 +109,10 @@ int comedi_alloc_subdev_readback(struct comedi_subdevice *s)
 	s->readback = kcalloc(s->n_chan, sizeof(*s->readback), GFP_KERNEL);
 	if (!s->readback)
 		return -ENOMEM;
+
+	if (!s->insn_read)
+		s->insn_read = comedi_readback_insn_read;
+
 	return 0;
 }
 EXPORT_SYMBOL_GPL(comedi_alloc_subdev_readback);
@@ -316,19 +320,91 @@ unsigned int comedi_bytes_per_scan(struct comedi_subdevice *s)
 	case COMEDI_SUBD_DI:
 	case COMEDI_SUBD_DO:
 	case COMEDI_SUBD_DIO:
-		bits_per_sample = 8 * bytes_per_sample(s);
-		num_samples = (cmd->chanlist_len + bits_per_sample - 1) /
-				bits_per_sample;
+		bits_per_sample = 8 * comedi_bytes_per_sample(s);
+		num_samples = DIV_ROUND_UP(cmd->scan_end_arg, bits_per_sample);
 		break;
 	default:
-		num_samples = cmd->chanlist_len;
+		num_samples = cmd->scan_end_arg;
 		break;
 	}
-	return num_samples * bytes_per_sample(s);
+	return comedi_samples_to_bytes(s, num_samples);
 }
 EXPORT_SYMBOL_GPL(comedi_bytes_per_scan);
 
 /**
+ * comedi_nscans_left - return the number of scans left in the command
+ * @s: comedi_subdevice struct
+ * @nscans: the expected number of scans
+ *
+ * If nscans is 0, the number of scans available in the async buffer will be
+ * used. Otherwise the expected number of scans will be used.
+ *
+ * If the async command has a stop_src of TRIG_COUNT, the nscans will be
+ * checked against the number of scans left in the command.
+ *
+ * The return value will then be either the expected number of scans or the
+ * number of scans remaining in the command.
+ */
+unsigned int comedi_nscans_left(struct comedi_subdevice *s,
+				unsigned int nscans)
+{
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
+
+	if (nscans == 0) {
+		unsigned int nbytes = comedi_buf_read_n_available(s);
+
+		nscans = nbytes / comedi_bytes_per_scan(s);
+	}
+
+	if (cmd->stop_src == TRIG_COUNT) {
+		unsigned int scans_left = 0;
+
+		if (async->scans_done < cmd->stop_arg)
+			scans_left = cmd->stop_arg - async->scans_done;
+
+		if (nscans > scans_left)
+			nscans = scans_left;
+	}
+	return nscans;
+}
+EXPORT_SYMBOL_GPL(comedi_nscans_left);
+
+/**
+ * comedi_nsamples_left - return the number of samples left in the command
+ * @s: comedi_subdevice struct
+ * @nsamples: the expected number of samples
+ *
+ * Returns the expected number of samples of the number of samples remaining
+ * in the command.
+ */
+unsigned int comedi_nsamples_left(struct comedi_subdevice *s,
+				  unsigned int nsamples)
+{
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
+
+	if (cmd->stop_src == TRIG_COUNT) {
+		/* +1 to force comedi_nscans_left() to return the scans left */
+		unsigned int nscans = (nsamples / cmd->scan_end_arg) + 1;
+		unsigned int scans_left = comedi_nscans_left(s, nscans);
+		unsigned int scan_pos =
+		    comedi_bytes_to_samples(s, async->scan_progress);
+		unsigned long long samples_left = 0;
+
+		if (scans_left) {
+			samples_left = ((unsigned long long)scans_left *
+					cmd->scan_end_arg) - scan_pos;
+		}
+
+		if (samples_left < nsamples)
+			nsamples = samples_left;
+	}
+	return nsamples;
+}
+EXPORT_SYMBOL_GPL(comedi_nsamples_left);
+
+/**
  * comedi_inc_scan_progress - update scan progress in asynchronous command
  * @s: comedi_subdevice struct
  * @num_bytes: amount of data in bytes to increment scan progress
@@ -342,10 +418,24 @@ void comedi_inc_scan_progress(struct comedi_subdevice *s,
 			      unsigned int num_bytes)
 {
 	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
 	unsigned int scan_length = comedi_bytes_per_scan(s);
 
+	/* track the 'cur_chan' for non-SDF_PACKED subdevices */
+	if (!(s->subdev_flags & SDF_PACKED)) {
+		async->cur_chan += comedi_bytes_to_samples(s, num_bytes);
+		async->cur_chan %= cmd->chanlist_len;
+	}
+
 	async->scan_progress += num_bytes;
 	if (async->scan_progress >= scan_length) {
+		unsigned int nscans = async->scan_progress / scan_length;
+
+		if (async->scans_done < (UINT_MAX - nscans))
+			async->scans_done += nscans;
+		else
+			async->scans_done = UINT_MAX;
+
 		async->scan_progress %= scan_length;
 		async->events |= COMEDI_CB_EOS;
 	}
@@ -376,7 +466,7 @@ unsigned int comedi_handle_events(struct comedi_device *dev,
 	if (events == 0)
 		return events;
 
-	if (events & (COMEDI_CB_EOA | COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW))
+	if (events & COMEDI_CB_CANCEL_MASK)
 		s->cancel(dev, s);
 
 	comedi_event(dev, s);
diff --git a/drivers/staging/comedi/drivers/Makefile b/drivers/staging/comedi/drivers/Makefile
index 6bc9ef3b25b3..84fdf20ca986 100644
--- a/drivers/staging/comedi/drivers/Makefile
+++ b/drivers/staging/comedi/drivers/Makefile
@@ -60,7 +60,6 @@ obj-$(CONFIG_COMEDI_S526)		+= s526.o
 # Comedi PCI drivers
 obj-$(CONFIG_COMEDI_8255_PCI)		+= 8255_pci.o
 obj-$(CONFIG_COMEDI_ADDI_WATCHDOG)	+= addi_watchdog.o
-obj-$(CONFIG_COMEDI_ADDI_APCI_035)	+= addi_apci_035.o
 obj-$(CONFIG_COMEDI_ADDI_APCI_1032)	+= addi_apci_1032.o
 obj-$(CONFIG_COMEDI_ADDI_APCI_1500)	+= addi_apci_1500.o
 obj-$(CONFIG_COMEDI_ADDI_APCI_1516)	+= addi_apci_1516.o
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_common.c b/drivers/staging/comedi/drivers/addi-data/addi_common.c
deleted file mode 100644
index 2e7fb218340f..000000000000
--- a/drivers/staging/comedi/drivers/addi-data/addi_common.c
+++ /dev/null
@@ -1,274 +0,0 @@
-/**
-@verbatim
-
-Copyright (C) 2004,2005  ADDI-DATA GmbH for the source code of this module.
-
-	ADDI-DATA GmbH
-	Dieselstrasse 3
-	D-77833 Ottersweier
-	Tel: +19(0)7223/9493-0
-	Fax: +49(0)7223/9493-92
-	http://www.addi-data.com
-	info@addi-data.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.
-
-@endverbatim
-*/
-/*
-
-  +-----------------------------------------------------------------------+
-  | (C) ADDI-DATA GmbH          Dieselstrasse 3      D-77833 Ottersweier  |
-  +-----------------------------------------------------------------------+
-  | Tel : +49 (0) 7223/9493-0     | email    : info@addi-data.com         |
-  | Fax : +49 (0) 7223/9493-92    | Internet : http://www.addi-data.com   |
-  +-----------------------------------------------------------------------+
-  | Project   : ADDI DATA         | Compiler : GCC                        |
-  | Modulname : addi_common.c     | Version  : 2.96                       |
-  +-------------------------------+---------------------------------------+
-  | Author    :           | Date     :                                    |
-  +-----------------------------------------------------------------------+
-  | Description : ADDI COMMON Main Module                                 |
-  +-----------------------------------------------------------------------+
-*/
-
-static int i_ADDIDATA_InsnReadEeprom(struct comedi_device *dev,
-				     struct comedi_subdevice *s,
-				     struct comedi_insn *insn,
-				     unsigned int *data)
-{
-	const struct addi_board *this_board = dev->board_ptr;
-	struct addi_private *devpriv = dev->private;
-	unsigned short w_Address = CR_CHAN(insn->chanspec);
-	unsigned short w_Data;
-
-	w_Data = addi_eeprom_readw(devpriv->i_IobaseAmcc,
-		this_board->pc_EepromChip, 2 * w_Address);
-	data[0] = w_Data;
-
-	return insn->n;
-}
-
-static irqreturn_t v_ADDI_Interrupt(int irq, void *d)
-{
-	struct comedi_device *dev = d;
-	const struct addi_board *this_board = dev->board_ptr;
-
-	this_board->interrupt(irq, d);
-	return IRQ_RETVAL(1);
-}
-
-static int i_ADDI_Reset(struct comedi_device *dev)
-{
-	const struct addi_board *this_board = dev->board_ptr;
-
-	this_board->reset(dev);
-	return 0;
-}
-
-static int addi_auto_attach(struct comedi_device *dev,
-				      unsigned long context_unused)
-{
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct addi_board *this_board = dev->board_ptr;
-	struct addi_private *devpriv;
-	struct comedi_subdevice *s;
-	int ret, n_subdevices;
-	unsigned int dw_Dummy;
-
-	dev->board_name = this_board->pc_DriverName;
-
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
-
-	ret = comedi_pci_enable(dev);
-	if (ret)
-		return ret;
-
-	if (this_board->i_IorangeBase1)
-		dev->iobase = pci_resource_start(pcidev, 1);
-	else
-		dev->iobase = pci_resource_start(pcidev, 0);
-
-	devpriv->iobase = dev->iobase;
-	devpriv->i_IobaseAmcc = pci_resource_start(pcidev, 0);
-	devpriv->i_IobaseAddon = pci_resource_start(pcidev, 2);
-	devpriv->i_IobaseReserved = pci_resource_start(pcidev, 3);
-
-	/* Initialize parameters that can be overridden in EEPROM */
-	devpriv->s_EeParameters.i_NbrAiChannel = this_board->i_NbrAiChannel;
-	devpriv->s_EeParameters.i_NbrAoChannel = this_board->i_NbrAoChannel;
-	devpriv->s_EeParameters.i_AiMaxdata = this_board->i_AiMaxdata;
-	devpriv->s_EeParameters.i_AoMaxdata = this_board->i_AoMaxdata;
-	devpriv->s_EeParameters.i_NbrDiChannel = this_board->i_NbrDiChannel;
-	devpriv->s_EeParameters.i_NbrDoChannel = this_board->i_NbrDoChannel;
-	devpriv->s_EeParameters.i_DoMaxdata = this_board->i_DoMaxdata;
-	devpriv->s_EeParameters.i_Timer = this_board->i_Timer;
-	devpriv->s_EeParameters.ui_MinAcquisitiontimeNs =
-		this_board->ui_MinAcquisitiontimeNs;
-	devpriv->s_EeParameters.ui_MinDelaytimeNs =
-		this_board->ui_MinDelaytimeNs;
-
-	/* ## */
-
-	if (pcidev->irq > 0) {
-		ret = request_irq(pcidev->irq, v_ADDI_Interrupt, IRQF_SHARED,
-				  dev->board_name, dev);
-		if (ret == 0)
-			dev->irq = pcidev->irq;
-	}
-
-	/*  Read eepeom and fill addi_board Structure */
-
-	if (this_board->i_PCIEeprom) {
-		if (!(strcmp(this_board->pc_EepromChip, "S5920"))) {
-			/*  Set 3 wait stait */
-			if (!(strcmp(dev->board_name, "apci035")))
-				outl(0x80808082, devpriv->i_IobaseAmcc + 0x60);
-			else
-				outl(0x83838383, devpriv->i_IobaseAmcc + 0x60);
-
-			/*  Enable the interrupt for the controller */
-			dw_Dummy = inl(devpriv->i_IobaseAmcc + 0x38);
-			outl(dw_Dummy | 0x2000, devpriv->i_IobaseAmcc + 0x38);
-		}
-		addi_eeprom_read_info(dev, pci_resource_start(pcidev, 0));
-	}
-
-	n_subdevices = 7;
-	ret = comedi_alloc_subdevices(dev, n_subdevices);
-	if (ret)
-		return ret;
-
-	/*  Allocate and Initialise AI Subdevice Structures */
-	s = &dev->subdevices[0];
-	if ((devpriv->s_EeParameters.i_NbrAiChannel)
-		|| (this_board->i_NbrAiChannelDiff)) {
-		dev->read_subdev = s;
-		s->type = COMEDI_SUBD_AI;
-		s->subdev_flags =
-			SDF_READABLE | SDF_COMMON | SDF_GROUND
-			| SDF_DIFF;
-		if (devpriv->s_EeParameters.i_NbrAiChannel)
-			s->n_chan = devpriv->s_EeParameters.i_NbrAiChannel;
-		else
-			s->n_chan = this_board->i_NbrAiChannelDiff;
-		s->maxdata = devpriv->s_EeParameters.i_AiMaxdata;
-		s->len_chanlist = this_board->i_AiChannelList;
-		s->range_table = this_board->pr_AiRangelist;
-
-		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;
-	}
-
-	/*  Allocate and Initialise AO Subdevice Structures */
-	s = &dev->subdevices[1];
-	if (devpriv->s_EeParameters.i_NbrAoChannel) {
-		s->type = COMEDI_SUBD_AO;
-		s->subdev_flags = SDF_WRITEABLE | SDF_GROUND | SDF_COMMON;
-		s->n_chan = devpriv->s_EeParameters.i_NbrAoChannel;
-		s->maxdata = devpriv->s_EeParameters.i_AoMaxdata;
-		s->len_chanlist =
-			devpriv->s_EeParameters.i_NbrAoChannel;
-		s->insn_write = this_board->ao_write;
-	} else {
-		s->type = COMEDI_SUBD_UNUSED;
-	}
-	/*  Allocate and Initialise DI Subdevice Structures */
-	s = &dev->subdevices[2];
-	if (devpriv->s_EeParameters.i_NbrDiChannel) {
-		s->type = COMEDI_SUBD_DI;
-		s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_COMMON;
-		s->n_chan = devpriv->s_EeParameters.i_NbrDiChannel;
-		s->maxdata = 1;
-		s->len_chanlist =
-			devpriv->s_EeParameters.i_NbrDiChannel;
-		s->range_table = &range_digital;
-		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;
-	}
-	/*  Allocate and Initialise DO Subdevice Structures */
-	s = &dev->subdevices[3];
-	if (devpriv->s_EeParameters.i_NbrDoChannel) {
-		s->type = COMEDI_SUBD_DO;
-		s->subdev_flags =
-			SDF_READABLE | SDF_WRITEABLE | SDF_GROUND | SDF_COMMON;
-		s->n_chan = devpriv->s_EeParameters.i_NbrDoChannel;
-		s->maxdata = devpriv->s_EeParameters.i_DoMaxdata;
-		s->len_chanlist =
-			devpriv->s_EeParameters.i_NbrDoChannel;
-		s->range_table = &range_digital;
-
-		/* 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;
-	}
-
-	/*  Allocate and Initialise Timer Subdevice Structures */
-	s = &dev->subdevices[4];
-	if (devpriv->s_EeParameters.i_Timer) {
-		s->type = COMEDI_SUBD_TIMER;
-		s->subdev_flags = SDF_WRITEABLE | SDF_GROUND | SDF_COMMON;
-		s->n_chan = 1;
-		s->maxdata = 0;
-		s->len_chanlist = 1;
-		s->range_table = &range_digital;
-
-		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;
-	}
-
-	/*  Allocate and Initialise TTL */
-	s = &dev->subdevices[5];
-	s->type = COMEDI_SUBD_UNUSED;
-
-	/* EEPROM */
-	s = &dev->subdevices[6];
-	if (this_board->i_PCIEeprom) {
-		s->type = COMEDI_SUBD_MEMORY;
-		s->subdev_flags = SDF_READABLE | SDF_INTERNAL;
-		s->n_chan = 256;
-		s->maxdata = 0xffff;
-		s->insn_read = i_ADDIDATA_InsnReadEeprom;
-	} else {
-		s->type = COMEDI_SUBD_UNUSED;
-	}
-
-	i_ADDI_Reset(dev);
-	return 0;
-}
-
-static void i_ADDI_Detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		i_ADDI_Reset(dev);
-	comedi_pci_detach(dev);
-}
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_common.h b/drivers/staging/comedi/drivers/addi-data/addi_common.h
deleted file mode 100644
index e2a3ffeee5cf..000000000000
--- a/drivers/staging/comedi/drivers/addi-data/addi_common.h
+++ /dev/null
@@ -1,144 +0,0 @@
-/*
- *  Copyright (C) 2004,2005  ADDI-DATA GmbH for the source code of this module.
- *
- *	ADDI-DATA GmbH
- *	Dieselstrasse 3
- *	D-77833 Ottersweier
- *	Tel: +19(0)7223/9493-0
- *	Fax: +49(0)7223/9493-92
- *	http://www.addi-data.com
- *	info@addi-data.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/sched.h>
-#include <linux/interrupt.h>
-
-struct addi_board {
-	const char *pc_DriverName;	/*  driver name */
-	int i_IorangeBase1;
-	int i_PCIEeprom;	/*  eeprom present or not */
-	char *pc_EepromChip;	/*  type of chip */
-	int i_NbrAiChannel;	/*  num of A/D chans */
-	int i_NbrAiChannelDiff;	/*  num of A/D chans in diff mode */
-	int i_AiChannelList;	/*  len of chanlist */
-	int i_NbrAoChannel;	/*  num of D/A chans */
-	int i_AiMaxdata;	/*  resolution of A/D */
-	int i_AoMaxdata;	/*  resolution of D/A */
-	const struct comedi_lrange *pr_AiRangelist;	/* rangelist for A/D */
-
-	int i_NbrDiChannel;	/*  Number of DI channels */
-	int i_NbrDoChannel;	/*  Number of DO channels */
-	int i_DoMaxdata;	/*  data to set all channels high */
-
-	int i_Timer;		/*    timer subdevice present or not */
-	unsigned int ui_MinAcquisitiontimeNs;	/*  Minimum Acquisition in Nano secs */
-	unsigned int ui_MinDelaytimeNs;	/*  Minimum Delay in Nano secs */
-
-	/* interrupt and reset */
-	void (*interrupt)(int irq, void *d);
-	int (*reset)(struct comedi_device *);
-
-	/* Subdevice functions */
-
-	/* ANALOG INPUT */
-	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 (*ao_write)(struct comedi_device *, struct comedi_subdevice *,
-			struct comedi_insn *, unsigned int *);
-
-	/* Digital Input */
-	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 (*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 (*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 *);
-};
-
-struct addi_private {
-	int iobase;
-	int i_IobaseAmcc;	/*  base+size for AMCC chip */
-	int i_IobaseAddon;	/* addon base address */
-	int i_IobaseReserved;
-	unsigned int ui_AiActualScan;	/* how many scans we finished */
-	unsigned int ui_AiNbrofChannels;	/*  how many channels is measured */
-	unsigned int ui_AiChannelList[32];	/*  actual chanlist */
-	unsigned int ui_AiReadData[32];
-	unsigned short us_UseDma;	/*  To use Dma or not */
-	unsigned char b_DmaDoubleBuffer;	/*  we can use double buffering */
-	unsigned int ui_DmaActualBuffer;	/*  which buffer is used now */
-	unsigned short *ul_DmaBufferVirtual[2];	/*  pointers to DMA buffer */
-	dma_addr_t ul_DmaBufferHw[2];		/*  hw address of DMA buff */
-	unsigned int ui_DmaBufferSize[2];	/*  size of dma buffer in bytes */
-	unsigned int ui_DmaBufferUsesize[2];	/*  which size we may now used for transfer */
-	unsigned char b_DigitalOutputRegister;	/*  Digital Output Register */
-	unsigned char b_OutputMemoryStatus;
-	unsigned char b_TimerSelectMode;	/*  Contain data written at iobase + 0C */
-	unsigned char b_ModeSelectRegister;	/*  Contain data written at iobase + 0E */
-	unsigned short us_OutputRegister;	/*  Contain data written at iobase + 0 */
-	unsigned char b_Timer2Mode;	/*  Specify the timer 2 mode */
-	unsigned char b_Timer2Interrupt;	/* Timer2  interrupt enable or disable */
-	unsigned int ai_running:1;
-	unsigned char b_InterruptMode;	/*  eoc eos or dma */
-	unsigned char b_EocEosInterrupt;	/*  Enable disable eoc eos interrupt */
-	unsigned int ui_EocEosConversionTime;
-	unsigned char b_ExttrigEnable;	/* To enable or disable external trigger */
-
-	/* Pointer to the current process */
-	struct task_struct *tsk_Current;
-
-	/* Parameters read from EEPROM overriding static board info */
-	struct {
-		int i_NbrAiChannel;	/*  num of A/D chans */
-		int i_NbrAoChannel;	/*  num of D/A chans */
-		int i_AiMaxdata;	/*  resolution of A/D */
-		int i_AoMaxdata;	/*  resolution of D/A */
-		int i_NbrDiChannel;	/*  Number of DI channels */
-		int i_NbrDoChannel;	/*  Number of DO channels */
-		int i_DoMaxdata;	/*  data to set all channels high */
-		int i_Timer;		/*  timer subdevice present or not */
-		unsigned int ui_MinAcquisitiontimeNs;
-					/*  Minimum Acquisition in Nano secs */
-		unsigned int ui_MinDelaytimeNs;
-					/*  Minimum Delay in Nano secs */
-	} s_EeParameters;
-};
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_eeprom.c b/drivers/staging/comedi/drivers/addi-data/addi_eeprom.c
deleted file mode 100644
index b731856c27da..000000000000
--- a/drivers/staging/comedi/drivers/addi-data/addi_eeprom.c
+++ /dev/null
@@ -1,360 +0,0 @@
-/*
- * addi_eeprom.c - ADDI EEPROM Module
- * Copyright (C) 2004,2005  ADDI-DATA GmbH for the source code of this module.
- * Project manager: Eric Stolz
- *
- *	ADDI-DATA GmbH
- *	Dieselstrasse 3
- *	D-77833 Ottersweier
- *	Tel: +19(0)7223/9493-0
- *	Fax: +49(0)7223/9493-92
- *	http://www.addi-data.com
- *	info@addi-data.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.
- */
-
-#include <linux/delay.h>
-
-#define NVRAM_USER_DATA_START	0x100
-
-#define NVCMD_BEGIN_READ	(0x7 << 5)	/* nvRam begin read command */
-#define NVCMD_LOAD_LOW		(0x4 << 5)	/* nvRam load low command */
-#define NVCMD_LOAD_HIGH		(0x5 << 5)	/* nvRam load high command */
-
-#define EE93C76_CLK_BIT		(1 << 0)
-#define EE93C76_CS_BIT		(1 << 1)
-#define EE93C76_DOUT_BIT	(1 << 2)
-#define EE93C76_DIN_BIT		(1 << 3)
-#define EE93C76_READ_CMD	(0x0180 << 4)
-#define EE93C76_CMD_LEN		13
-
-#define EEPROM_DIGITALINPUT		0
-#define EEPROM_DIGITALOUTPUT		1
-#define EEPROM_ANALOGINPUT		2
-#define EEPROM_ANALOGOUTPUT		3
-#define EEPROM_TIMER			4
-#define EEPROM_WATCHDOG			5
-#define EEPROM_TIMER_WATCHDOG_COUNTER	10
-
-static void addi_eeprom_clk_93c76(unsigned long iobase, unsigned int val)
-{
-	outl(val & ~EE93C76_CLK_BIT, iobase);
-	udelay(100);
-
-	outl(val | EE93C76_CLK_BIT, iobase);
-	udelay(100);
-}
-
-static unsigned int addi_eeprom_cmd_93c76(unsigned long iobase,
-					  unsigned int cmd,
-					  unsigned char len)
-{
-	unsigned int val = EE93C76_CS_BIT;
-	int i;
-
-	/* Toggle EEPROM's Chip select to get it out of Shift Register Mode */
-	outl(val, iobase);
-	udelay(100);
-
-	/* Send EEPROM command - one bit at a time */
-	for (i = (len - 1); i >= 0; i--) {
-		if (cmd & (1 << i))
-			val |= EE93C76_DOUT_BIT;
-		else
-			val &= ~EE93C76_DOUT_BIT;
-
-		/* Write the command */
-		outl(val, iobase);
-		udelay(100);
-
-		addi_eeprom_clk_93c76(iobase, val);
-	}
-	return val;
-}
-
-static unsigned short addi_eeprom_readw_93c76(unsigned long iobase,
-					      unsigned short addr)
-{
-	unsigned short val = 0;
-	unsigned int cmd;
-	unsigned int tmp;
-	int i;
-
-	/* Send EEPROM read command and offset to EEPROM */
-	cmd = EE93C76_READ_CMD | (addr / 2);
-	cmd = addi_eeprom_cmd_93c76(iobase, cmd, EE93C76_CMD_LEN);
-
-	/* Get the 16-bit value */
-	for (i = 0; i < 16; i++) {
-		addi_eeprom_clk_93c76(iobase, cmd);
-
-		tmp = inl(iobase);
-		udelay(100);
-
-		val <<= 1;
-		if (tmp & EE93C76_DIN_BIT)
-			val |= 0x1;
-	}
-
-	/* Toggle EEPROM's Chip select to get it out of Shift Register Mode */
-	outl(0, iobase);
-	udelay(100);
-
-	return val;
-}
-
-static void addi_eeprom_nvram_wait(unsigned long iobase)
-{
-	unsigned char val;
-
-	do {
-		val = inb(iobase + AMCC_OP_REG_MCSR_NVCMD);
-	} while (val & 0x80);
-}
-
-static unsigned short addi_eeprom_readw_nvram(unsigned long iobase,
-					      unsigned short addr)
-{
-	unsigned short val = 0;
-	unsigned char tmp;
-	unsigned char i;
-
-	for (i = 0; i < 2; i++) {
-		/* Load the low 8 bit address */
-		outb(NVCMD_LOAD_LOW, iobase + AMCC_OP_REG_MCSR_NVCMD);
-		addi_eeprom_nvram_wait(iobase);
-		outb((addr + i) & 0xff, iobase + AMCC_OP_REG_MCSR_NVDATA);
-		addi_eeprom_nvram_wait(iobase);
-
-		/* Load the high 8 bit address */
-		outb(NVCMD_LOAD_HIGH, iobase + AMCC_OP_REG_MCSR_NVCMD);
-		addi_eeprom_nvram_wait(iobase);
-		outb(((addr + i) >> 8) & 0xff,
-			iobase + AMCC_OP_REG_MCSR_NVDATA);
-		addi_eeprom_nvram_wait(iobase);
-
-		/* Read the eeprom data byte */
-		outb(NVCMD_BEGIN_READ, iobase + AMCC_OP_REG_MCSR_NVCMD);
-		addi_eeprom_nvram_wait(iobase);
-		tmp = inb(iobase + AMCC_OP_REG_MCSR_NVDATA);
-		addi_eeprom_nvram_wait(iobase);
-
-		if (i == 0)
-			val |= tmp;
-		else
-			val |= (tmp << 8);
-	}
-
-	return val;
-}
-
-static unsigned short addi_eeprom_readw(unsigned long iobase,
-					char *type,
-					unsigned short addr)
-{
-	unsigned short val = 0;
-
-	/* Add the offset to the start of the user data */
-	addr += NVRAM_USER_DATA_START;
-
-	if (!strcmp(type, "S5920") || !strcmp(type, "S5933"))
-		val = addi_eeprom_readw_nvram(iobase, addr);
-
-	if (!strcmp(type, "93C76"))
-		val = addi_eeprom_readw_93c76(iobase, addr);
-
-	return val;
-}
-
-static void addi_eeprom_read_di_info(struct comedi_device *dev,
-				     unsigned long iobase,
-				     unsigned short addr)
-{
-	const struct addi_board *this_board = dev->board_ptr;
-	struct addi_private *devpriv = dev->private;
-	char *type = this_board->pc_EepromChip;
-	unsigned short tmp;
-
-	/* Number of channels */
-	tmp = addi_eeprom_readw(iobase, type, addr + 6);
-	devpriv->s_EeParameters.i_NbrDiChannel = tmp;
-
-	/* Interruptible or not */
-	tmp = addi_eeprom_readw(iobase, type, addr + 8);
-	tmp = (tmp >> 7) & 0x01;
-
-	/* How many interruptible logic */
-	tmp = addi_eeprom_readw(iobase, type, addr + 10);
-}
-
-static void addi_eeprom_read_do_info(struct comedi_device *dev,
-				     unsigned long iobase,
-				     unsigned short addr)
-{
-	const struct addi_board *this_board = dev->board_ptr;
-	struct addi_private *devpriv = dev->private;
-	char *type = this_board->pc_EepromChip;
-	unsigned short tmp;
-
-	/* Number of channels */
-	tmp = addi_eeprom_readw(iobase, type, addr + 6);
-	devpriv->s_EeParameters.i_NbrDoChannel = tmp;
-
-	devpriv->s_EeParameters.i_DoMaxdata = 0xffffffff >> (32 - tmp);
-}
-
-static void addi_eeprom_read_timer_info(struct comedi_device *dev,
-					unsigned long iobase,
-					unsigned short addr)
-{
-	struct addi_private *devpriv = dev->private;
-#if 0
-	const struct addi_board *this_board = dev->board_ptr;
-	char *type = this_board->pc_EepromChip;
-	unsigned short offset = 0;
-	unsigned short ntimers;
-	unsigned short tmp;
-	int i;
-
-	/* Number of Timers */
-	ntimers = addi_eeprom_readw(iobase, type, addr + 6);
-
-	/* Read header size */
-	for (i = 0; i < ntimers; i++) {
-		unsigned short size;
-		unsigned short res;
-		unsigned short mode;
-		unsigned short min_timing;
-		unsigned short timebase;
-
-		size = addi_eeprom_readw(iobase, type, addr + 8 + offset + 0);
-
-		/* Resolution / Mode */
-		tmp = addi_eeprom_readw(iobase, type, addr + 8 + offset + 2);
-		res = (tmp >> 10) & 0x3f;
-		mode = (tmp >> 4) & 0x3f;
-
-		/* MinTiming / Timebase */
-		tmp = addi_eeprom_readw(iobase, type, addr + 8 + offset + 4);
-		min_timing = (tmp  >> 6) & 0x3ff;
-		Timebase = tmp & 0x3f;
-
-		offset += size;
-	}
-#endif
-	/* Timer subdevice present */
-	devpriv->s_EeParameters.i_Timer = 1;
-}
-
-static void addi_eeprom_read_ao_info(struct comedi_device *dev,
-				     unsigned long iobase,
-				     unsigned short addr)
-{
-	const struct addi_board *this_board = dev->board_ptr;
-	struct addi_private *devpriv = dev->private;
-	char *type = this_board->pc_EepromChip;
-	unsigned short tmp;
-
-	/* No of channels for 1st hard component */
-	tmp = addi_eeprom_readw(iobase, type, addr + 10);
-	devpriv->s_EeParameters.i_NbrAoChannel = (tmp >> 4) & 0x3ff;
-
-	/* Resolution for 1st hard component */
-	tmp = addi_eeprom_readw(iobase, type, addr + 16);
-	tmp = (tmp >> 8) & 0xff;
-	devpriv->s_EeParameters.i_AoMaxdata = 0xfff >> (16 - tmp);
-}
-
-static void addi_eeprom_read_ai_info(struct comedi_device *dev,
-				     unsigned long iobase,
-				     unsigned short addr)
-{
-	const struct addi_board *this_board = dev->board_ptr;
-	struct addi_private *devpriv = dev->private;
-	char *type = this_board->pc_EepromChip;
-	unsigned short offset;
-	unsigned short tmp;
-
-	/* No of channels for 1st hard component */
-	tmp = addi_eeprom_readw(iobase, type, addr + 10);
-	devpriv->s_EeParameters.i_NbrAiChannel = (tmp >> 4) & 0x3ff;
-	if (!strcmp(this_board->pc_DriverName, "apci3200"))
-		devpriv->s_EeParameters.i_NbrAiChannel *= 4;
-
-	tmp = addi_eeprom_readw(iobase, type, addr + 16);
-	devpriv->s_EeParameters.ui_MinAcquisitiontimeNs = tmp * 1000;
-
-	tmp = addi_eeprom_readw(iobase, type, addr + 30);
-	devpriv->s_EeParameters.ui_MinDelaytimeNs = tmp * 1000;
-
-	tmp = addi_eeprom_readw(iobase, type, addr + 20);
-	/* dma = (tmp >> 13) & 0x01; */
-
-	tmp = addi_eeprom_readw(iobase, type, addr + 72) & 0xff;
-	if (tmp) {		/* > 0 */
-		/* offset of first analog input single header */
-		offset = 74 + (2 * tmp) + (10 * (1 + (tmp / 16)));
-	} else {		/* = 0 */
-		offset = 74;
-	}
-
-	/* Resolution */
-	tmp = addi_eeprom_readw(iobase, type, addr + offset + 2) & 0x1f;
-	devpriv->s_EeParameters.i_AiMaxdata = 0xffff >> (16 - tmp);
-}
-
-static void addi_eeprom_read_info(struct comedi_device *dev,
-				  unsigned long iobase)
-{
-	const struct addi_board *this_board = dev->board_ptr;
-	char *type = this_board->pc_EepromChip;
-	unsigned short size;
-	unsigned char nfuncs;
-	int i;
-
-	size = addi_eeprom_readw(iobase, type, 8);
-	nfuncs = addi_eeprom_readw(iobase, type, 10) & 0xff;
-
-	/* Read functionality details */
-	for (i = 0; i < nfuncs; i++) {
-		unsigned short offset = i * 4;
-		unsigned short addr;
-		unsigned char func;
-
-		func = addi_eeprom_readw(iobase, type, 12 + offset) & 0x3f;
-		addr = addi_eeprom_readw(iobase, type, 14 + offset);
-
-		switch (func) {
-		case EEPROM_DIGITALINPUT:
-			addi_eeprom_read_di_info(dev, iobase, addr);
-			break;
-
-		case EEPROM_DIGITALOUTPUT:
-			addi_eeprom_read_do_info(dev, iobase, addr);
-			break;
-
-		case EEPROM_ANALOGINPUT:
-			addi_eeprom_read_ai_info(dev, iobase, addr);
-			break;
-
-		case EEPROM_ANALOGOUTPUT:
-			addi_eeprom_read_ao_info(dev, iobase, addr);
-			break;
-
-		case EEPROM_TIMER:
-		case EEPROM_WATCHDOG:
-		case EEPROM_TIMER_WATCHDOG_COUNTER:
-			addi_eeprom_read_timer_info(dev, iobase, addr);
-			break;
-		}
-	}
-}
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci035.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci035.c
deleted file mode 100644
index 53bb51bd77b5..000000000000
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci035.c
+++ /dev/null
@@ -1,480 +0,0 @@
-/*
- * Copyright (C) 2004,2005  ADDI-DATA GmbH for the source code of this module.
- *
- *	ADDI-DATA GmbH
- *	Dieselstrasse 3
- *	D-77833 Ottersweier
- *	Tel: +19(0)7223/9493-0
- *	Fax: +49(0)7223/9493-92
- *	http://www.addi-data.com
- *	info@addi-data.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.
- */
-
-/* Card Specific information */
-#define APCI035_ADDRESS_RANGE		255
-
-/* Timer / Watchdog Related Defines */
-#define APCI035_TCW_SYNC_ENABLEDISABLE	0
-#define APCI035_TCW_RELOAD_VALUE	4
-#define APCI035_TCW_TIMEBASE		8
-#define APCI035_TCW_PROG		12
-#define APCI035_TCW_TRIG_STATUS		16
-#define APCI035_TCW_IRQ			20
-#define APCI035_TCW_WARN_TIMEVAL	24
-#define APCI035_TCW_WARN_TIMEBASE	28
-
-#define ADDIDATA_TIMER			0
-/* #define ADDIDATA_WATCHDOG		1 */
-
-#define APCI035_TW1			0
-#define APCI035_TW2			32
-#define APCI035_TW3			64
-#define APCI035_TW4			96
-
-#define APCI035_AI_OFFSET		0
-#define APCI035_TEMP			128
-#define APCI035_ALR_SEQ			4
-#define APCI035_START_STOP_INDEX	8
-#define APCI035_ALR_START_STOP		12
-#define APCI035_ALR_IRQ			16
-#define APCI035_EOS			20
-#define APCI035_CHAN_NO			24
-#define APCI035_CHAN_VAL		28
-#define APCI035_CONV_TIME_TIME_BASE	36
-#define APCI035_RELOAD_CONV_TIME_VAL	32
-#define APCI035_DELAY_TIME_TIME_BASE	44
-#define APCI035_RELOAD_DELAY_TIME_VAL	40
-#define ENABLE_EXT_TRIG			1
-#define ENABLE_EXT_GATE			2
-#define ENABLE_EXT_TRIG_GATE		3
-
-#define ANALOG_INPUT			0
-#define TEMPERATURE			1
-#define RESISTANCE			2
-
-#define ADDIDATA_GREATER_THAN_TEST	0
-#define ADDIDATA_LESS_THAN_TEST		1
-
-#define APCI035_MAXVOLT			2.5
-
-#define ADDIDATA_UNIPOLAR		1
-#define ADDIDATA_BIPOLAR		2
-
-/* ANALOG INPUT RANGE */
-static struct comedi_lrange range_apci035_ai = {
-	8, {
-		BIP_RANGE(10),
-		BIP_RANGE(5),
-		BIP_RANGE(2),
-		BIP_RANGE(1),
-		UNI_RANGE(10),
-		UNI_RANGE(5),
-		UNI_RANGE(2),
-		UNI_RANGE(1)
-	}
-};
-
-static int i_WatchdogNbr;
-static int i_Temp;
-static int i_Flag = 1;
-
-/*
- * Configures The Timer , Counter or Watchdog
- *
- * data[0] 0 = Configure As Timer, 1 = Configure As Watchdog
- * data[1] Watchdog number
- * data[2] Time base Unit
- * data[3] Reload Value
- * data[4] External Trigger, 1 = Enable, 0 = Disable
- * data[5] External Trigger Level
- *	00 = Trigger Disabled
- *	01 = Trigger Enabled (Low level)
- *	10 = Trigger Enabled (High Level)
- *	11 = Trigger Enabled (High/Low level)
- * data[6] External Gate, 1 = Enable, 0 = Disable
- * data[7] External Gate level
- *	00 = Gate Disabled
- *	01 = Gate Enabled (Low level)
- *	10 = Gate Enabled (High Level)
- * data[8] Warning Relay, 1 = Enable, 0 = Disable
- * data[9] Warning Delay available
- * data[10] Warning Relay Time unit
- * data[11] Warning Relay Time Reload value
- * data[12] Reset Relay, 1 = Enable, 0 = Disable
- * data[13] Interrupt, 1 = Enable, 0 = Disable
- */
-static int apci035_timer_config(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn,
-				unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-	unsigned int ui_Status;
-	unsigned int ui_Command;
-	unsigned int ui_Mode;
-
-	i_Temp = 0;
-	devpriv->tsk_Current = current;
-	devpriv->b_TimerSelectMode = data[0];
-	i_WatchdogNbr = data[1];
-	if (data[0] == 0)
-		ui_Mode = 2;
-	else
-		ui_Mode = 0;
-
-	ui_Command = 0;
-	outl(ui_Command, devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 12);
-
-	ui_Command = inl(devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 12);
-
-	/* Set the reload value */
-	outl(data[3], devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 4);
-
-	/* Set the time unit */
-	outl(data[2], devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 8);
-	if (data[0] == ADDIDATA_TIMER) {
-
-		/* Set the mode :             */
-		/* - Disable the hardware     */
-		/* - Disable the counter mode */
-		/* - Disable the warning      */
-		/* - Disable the reset        */
-		/* - Enable the timer mode    */
-		/* - Set the timer mode       */
-
-		ui_Command =
-			(ui_Command & 0xFFF719E2UL) | ui_Mode << 13UL | 0x10UL;
-
-	} else if (data[0] == ADDIDATA_WATCHDOG) {
-
-		/* Set the mode :             */
-		/* - Disable the hardware     */
-		/* - Disable the counter mode */
-		/* - Disable the warning      */
-		/* - Disable the reset        */
-		/* - Disable the timer mode   */
-
-		ui_Command = ui_Command & 0xFFF819E2UL;
-
-	} else {
-		dev_err(dev->class_dev, "The parameter for Timer/watchdog selection is in error\n");
-		return -EINVAL;
-	}
-
-	outl(ui_Command, devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 12);
-
-	ui_Command = inl(devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 12);
-
-	/* Disable the hardware trigger */
-	ui_Command = ui_Command & 0xFFFFF89FUL;
-	if (data[4] == 1) {
-		/* Set the hardware trigger level */
-		ui_Command = ui_Command | (data[5] << 5);
-	}
-	outl(ui_Command, devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 12);
-
-	ui_Command = inl(devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 12);
-
-	/* Disable the hardware gate */
-	ui_Command = ui_Command & 0xFFFFF87FUL;
-	if (data[6] == 1) {
-		/* Set the hardware gate level */
-		ui_Command = ui_Command | (data[7] << 7);
-	}
-	outl(ui_Command, devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 12);
-
-	ui_Command = inl(devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 12);
-
-	/* Disable the hardware output */
-	ui_Command = ui_Command & 0xFFFFF9FBUL;
-
-	/* Set the hardware output level */
-	ui_Command = ui_Command | (data[8] << 2);
-	outl(ui_Command, devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 12);
-	if (data[9] == 1) {
-		/* Set the reload value */
-		outl(data[11],
-			devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 24);
-
-		/* Set the time unite */
-		outl(data[10],
-			devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 28);
-	}
-
-	ui_Command = inl(devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 12);
-
-	/* Disable the hardware output */
-	ui_Command = ui_Command & 0xFFFFF9F7UL;
-
-	/* Set the hardware output level */
-	ui_Command = ui_Command | (data[12] << 3);
-	outl(ui_Command, devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 12);
-
-	/* Enable the watchdog interrupt */
-	ui_Command = inl(devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 12);
-
-	/* Set the interrupt selection */
-	ui_Status = inl(devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 16);
-
-	ui_Command = (ui_Command & 0xFFFFF9FDUL) | (data[13] << 1);
-	outl(ui_Command, devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 12);
-
-	return insn->n;
-}
-
-/*
- * Start / Stop The Selected Timer , or Watchdog
- *
- * data[0]
- *	0 - Stop Selected Timer/Watchdog
- *	1 - Start Selected Timer/Watch*dog
- *	2 - Trigger Selected Timer/Watchdog
- *	3 - Stop All Timer/Watchdog
- *	4 - Start All Timer/Watchdog
- *	5 - Trigger All Timer/Watchdog
- */
-static int apci035_timer_write(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn,
-			       unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-	unsigned int ui_Command;
-	int i_Count;
-
-	if (data[0] == 1) {
-		ui_Command =
-			inl(devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 12);
-
-		/* Start the hardware */
-		ui_Command = (ui_Command & 0xFFFFF9FFUL) | 0x1UL;
-		outl(ui_Command,
-			devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 12);
-	}
-	if (data[0] == 2) {
-		ui_Command =
-			inl(devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 12);
-
-		/* Set the trigger command */
-		ui_Command = (ui_Command & 0xFFFFF9FFUL) | 0x200UL;
-		outl(ui_Command,
-			devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 12);
-	}
-
-	if (data[0] == 0) {
-		/* Stop The Watchdog */
-		ui_Command = 0;
-		/*
-		* ui_Command = inl(devpriv->iobase+((i_WatchdogNbr-1)*32)+12);
-		* ui_Command = ui_Command & 0xFFFFF9FEUL;
-		*/
-		outl(ui_Command,
-			devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 12);
-	}
-	if (data[0] == 3) {
-		/* stop all Watchdogs */
-		ui_Command = 0;
-		for (i_Count = 1; i_Count <= 4; i_Count++) {
-			if (devpriv->b_TimerSelectMode == ADDIDATA_WATCHDOG)
-				ui_Command = 0x2UL;
-			else
-				ui_Command = 0x10UL;
-
-			i_WatchdogNbr = i_Count;
-			outl(ui_Command,
-				devpriv->iobase + ((i_WatchdogNbr - 1) * 32) +
-				0);
-		}
-
-	}
-	if (data[0] == 4) {
-		/* start all Watchdogs */
-		ui_Command = 0;
-		for (i_Count = 1; i_Count <= 4; i_Count++) {
-			if (devpriv->b_TimerSelectMode == ADDIDATA_WATCHDOG)
-				ui_Command = 0x1UL;
-			else
-				ui_Command = 0x8UL;
-
-			i_WatchdogNbr = i_Count;
-			outl(ui_Command,
-				devpriv->iobase + ((i_WatchdogNbr - 1) * 32) +
-				0);
-		}
-	}
-	if (data[0] == 5) {
-		/* trigger all Watchdogs */
-		ui_Command = 0;
-		for (i_Count = 1; i_Count <= 4; i_Count++) {
-			if (devpriv->b_TimerSelectMode == ADDIDATA_WATCHDOG)
-				ui_Command = 0x4UL;
-			else
-				ui_Command = 0x20UL;
-
-			i_WatchdogNbr = i_Count;
-			outl(ui_Command,
-				devpriv->iobase + ((i_WatchdogNbr - 1) * 32) +
-				0);
-		}
-		i_Temp = 1;
-	}
-	return insn->n;
-}
-
-/*
- * Read The Selected Timer , Counter or Watchdog
- *
- * data[0] software trigger status
- * data[1] hardware trigger status
- * data[2] Software clear status
- * data[3] Overflow status
- * data[4] Timer actual value
- */
-static int apci035_timer_read(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn,
-			      unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-	unsigned int ui_Status;	/*  Status register */
-
-	i_WatchdogNbr = insn->unused[0];
-
-	/* Get the status */
-	ui_Status = inl(devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 16);
-
-	/* Get the software trigger status */
-	data[0] = ((ui_Status >> 1) & 1);
-
-	/* Get the hardware trigger status */
-	data[1] = ((ui_Status >> 2) & 1);
-
-	/* Get the software clear status */
-	data[2] = ((ui_Status >> 3) & 1);
-
-	/* Get the overflow status */
-	data[3] = ((ui_Status >> 0) & 1);
-	if (devpriv->b_TimerSelectMode == ADDIDATA_TIMER)
-		data[4] = inl(devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 0);
-
-	return insn->n;
-}
-
-/*
- * Configures The Analog Input Subdevice
- *
- * data[0] Warning delay value
- */
-static int apci035_ai_config(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_insn *insn,
-			     unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-
-	devpriv->tsk_Current = current;
-	outl(0x200 | 0, devpriv->iobase + 128 + 0x4);
-	outl(0, devpriv->iobase + 128 + 0);
-
-	/* Initialise the warning value */
-	outl(0x300 | 0, devpriv->iobase + 128 + 0x4);
-	outl((data[0] << 8), devpriv->iobase + 128 + 0);
-	outl(0x200000UL, devpriv->iobase + 128 + 12);
-
-	return insn->n;
-}
-
-/*
- * Read value of the selected channel
- *
- * data[0] Digital Value Of Input
- */
-static int apci035_ai_read(struct comedi_device *dev,
-			   struct comedi_subdevice *s,
-			   struct comedi_insn *insn,
-			   unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-	unsigned int ui_CommandRegister;
-
-	/*  Set the start */
-	ui_CommandRegister = 0x80000;
-
-	/* Write the command register */
-	outl(ui_CommandRegister, devpriv->iobase + 128 + 8);
-
-	/* Read the digital value of the input */
-	data[0] = inl(devpriv->iobase + 128 + 28);
-	return insn->n;
-}
-
-static int apci035_reset(struct comedi_device *dev)
-{
-	struct addi_private *devpriv = dev->private;
-	int i_Count;
-
-	for (i_Count = 1; i_Count <= 4; i_Count++) {
-		i_WatchdogNbr = i_Count;
-
-		/* stop all timers */
-		outl(0x0, devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 0);
-	}
-	outl(0x0, devpriv->iobase + 128 + 12);	/* Disable the warning delay */
-
-	return 0;
-}
-
-static void apci035_interrupt(int irq, void *d)
-{
-	struct comedi_device *dev = d;
-	struct addi_private *devpriv = dev->private;
-	unsigned int ui_StatusRegister1;
-	unsigned int ui_StatusRegister2;
-	unsigned int ui_ReadCommand;
-	unsigned int ui_ChannelNumber;
-	unsigned int ui_DigitalTemperature;
-
-	if (i_Temp == 1) {
-		i_WatchdogNbr = i_Flag;
-		i_Flag = i_Flag + 1;
-	}
-
-	/* Read the interrupt status register of temperature Warning */
-	ui_StatusRegister1 = inl(devpriv->iobase + 128 + 16);
-
-	/* Read the interrupt status register for Watchdog/timer */
-	ui_StatusRegister2 =
-		inl(devpriv->iobase + ((i_WatchdogNbr - 1) * 32) + 20);
-
-	/* Test if warning relay interrupt */
-	if ((((ui_StatusRegister1) & 0x8) == 0x8)) {
-
-		/* Disable the temperature warning */
-		ui_ReadCommand = inl(devpriv->iobase + 128 + 12);
-		ui_ReadCommand = ui_ReadCommand & 0xFFDF0000UL;
-		outl(ui_ReadCommand, devpriv->iobase + 128 + 12);
-
-		/* Read the channel number */
-		ui_ChannelNumber = inl(devpriv->iobase + 128 + 60);
-
-		/* Read the digital temperature value */
-		ui_DigitalTemperature = inl(devpriv->iobase + 128 + 60);
-
-		/*  send signal to the sample */
-		send_sig(SIGIO, devpriv->tsk_Current, 0);
-
-	} else if ((ui_StatusRegister2 & 0x1) == 0x1) {
-		/*  send signal to the sample */
-		send_sig(SIGIO, devpriv->tsk_Current, 0);
-	}
-}
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1500.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1500.c
index 0ea081e1e119..bfa9228c833f 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1500.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1500.c
@@ -158,7 +158,7 @@ static int apci1500_di_config(struct comedi_device *dev,
 			      struct comedi_insn *insn,
 			      unsigned int *data)
 {
-	struct addi_private *devpriv = dev->private;
+	struct apci1500_private *devpriv = dev->private;
 	int i_PatternPolarity = 0, i_PatternTransition = 0, i_PatternMask = 0;
 	int i_MaxChannel = 0, i_Count = 0, i_EventMask = 0;
 	int i_PatternTransitionCount = 0, i_RegValue;
@@ -466,7 +466,7 @@ static int apci1500_di_write(struct comedi_device *dev,
 			     struct comedi_insn *insn,
 			     unsigned int *data)
 {
-	struct addi_private *devpriv = dev->private;
+	struct apci1500_private *devpriv = dev->private;
 	int i_Event1InterruptStatus = 0, i_Event2InterruptStatus =
 		0, i_RegValue;
 
@@ -653,7 +653,7 @@ static int apci1500_di_read(struct comedi_device *dev,
 			    struct comedi_insn *insn,
 			    unsigned int *data)
 {
-	struct addi_private *devpriv = dev->private;
+	struct apci1500_private *devpriv = dev->private;
 	int i_DummyRead = 0;
 
 	/* Software reset */
@@ -789,7 +789,7 @@ static int apci1500_di_insn_bits(struct comedi_device *dev,
 				 struct comedi_insn *insn,
 				 unsigned int *data)
 {
-	struct addi_private *devpriv = dev->private;
+	struct apci1500_private *devpriv = dev->private;
 
 	data[1] = inw(devpriv->i_IobaseAddon + APCI1500_DIGITAL_IP);
 
@@ -807,7 +807,7 @@ static int apci1500_do_config(struct comedi_device *dev,
 			      struct comedi_insn *insn,
 			      unsigned int *data)
 {
-	struct addi_private *devpriv = dev->private;
+	struct apci1500_private *devpriv = dev->private;
 
 	devpriv->b_OutputMemoryStatus = data[0];
 	return insn->n;
@@ -821,7 +821,7 @@ static int apci1500_do_write(struct comedi_device *dev,
 			     struct comedi_insn *insn,
 			     unsigned int *data)
 {
-	struct addi_private *devpriv = dev->private;
+	struct apci1500_private *devpriv = dev->private;
 	static unsigned int ui_Temp;
 	unsigned int ui_Temp1;
 	unsigned int ui_NoOfChannel = CR_CHAN(insn->chanspec);	/*  get the channel */
@@ -981,7 +981,7 @@ static int apci1500_timer_config(struct comedi_device *dev,
 				 struct comedi_insn *insn,
 				 unsigned int *data)
 {
-	struct addi_private *devpriv = dev->private;
+	struct apci1500_private *devpriv = dev->private;
 	int i_TimerCounterMode, i_MasterConfiguration;
 
 	devpriv->tsk_Current = current;
@@ -1471,7 +1471,7 @@ static int apci1500_timer_write(struct comedi_device *dev,
 				struct comedi_insn *insn,
 				unsigned int *data)
 {
-	struct addi_private *devpriv = dev->private;
+	struct apci1500_private *devpriv = dev->private;
 	int i_CommandAndStatusValue;
 
 	switch (data[0]) {
@@ -1731,7 +1731,7 @@ static int apci1500_timer_bits(struct comedi_device *dev,
 			       struct comedi_insn *insn,
 			       unsigned int *data)
 {
-	struct addi_private *devpriv = dev->private;
+	struct apci1500_private *devpriv = dev->private;
 	int i_CommandAndStatusValue;
 
 	switch (data[0]) {
@@ -1895,7 +1895,7 @@ static int apci1500_do_bits(struct comedi_device *dev,
 			    struct comedi_insn *insn,
 			    unsigned int *data)
 {
-	struct addi_private *devpriv = dev->private;
+	struct apci1500_private *devpriv = dev->private;
 	unsigned int ui_Status;
 	int i_RegValue;
 	int i_Constant;
@@ -2011,11 +2011,11 @@ static int apci1500_do_bits(struct comedi_device *dev,
 	return insn->n;
 }
 
-static void apci1500_interrupt(int irq, void *d)
+static irqreturn_t apci1500_interrupt(int irq, void *d)
 {
 
 	struct comedi_device *dev = d;
-	struct addi_private *devpriv = dev->private;
+	struct apci1500_private *devpriv = dev->private;
 	unsigned int ui_InterruptStatus = 0;
 	int i_RegValue = 0;
 
@@ -2180,11 +2180,13 @@ static void apci1500_interrupt(int irq, void *d)
 			"Interrupt from unknown source\n");
 
 	}
+
+	return IRQ_HANDLED;
 }
 
 static int apci1500_reset(struct comedi_device *dev)
 {
-	struct addi_private *devpriv = dev->private;
+	struct apci1500_private *devpriv = dev->private;
 	int i_DummyRead = 0;
 
 	i_TimerCounter1Init = 0;
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1564.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1564.c
index 98de96953a29..fa99c8ca4f95 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1564.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1564.c
@@ -16,244 +16,189 @@
 #define ADDIDATA_TIMER					0
 #define ADDIDATA_COUNTER				1
 #define ADDIDATA_WATCHDOG				2
-#define APCI1564_COUNTER1				0
-#define APCI1564_COUNTER2				1
-#define APCI1564_COUNTER3				2
-#define APCI1564_COUNTER4				3
-
-/*
- * devpriv->amcc_iobase Register Map
- */
-#define APCI1564_DI_REG					0x04
-#define APCI1564_DI_INT_MODE1_REG			0x08
-#define APCI1564_DI_INT_MODE2_REG			0x0c
-#define APCI1564_DI_INT_STATUS_REG			0x10
-#define APCI1564_DI_IRQ_REG				0x14
-#define APCI1564_DO_REG					0x18
-#define APCI1564_DO_INT_CTRL_REG			0x1c
-#define APCI1564_DO_INT_STATUS_REG			0x20
-#define APCI1564_DO_IRQ_REG				0x24
-#define APCI1564_WDOG_REG				0x28
-#define APCI1564_WDOG_RELOAD_REG			0x2c
-#define APCI1564_WDOG_TIMEBASE_REG			0x30
-#define APCI1564_WDOG_CTRL_REG				0x34
-#define APCI1564_WDOG_STATUS_REG			0x38
-#define APCI1564_WDOG_IRQ_REG				0x3c
-#define APCI1564_WDOG_WARN_TIMEVAL_REG			0x40
-#define APCI1564_WDOG_WARN_TIMEBASE_REG			0x44
-#define APCI1564_TIMER_REG				0x48
-#define APCI1564_TIMER_RELOAD_REG			0x4c
-#define APCI1564_TIMER_TIMEBASE_REG			0x50
-#define APCI1564_TIMER_CTRL_REG				0x54
-#define APCI1564_TIMER_STATUS_REG			0x58
-#define APCI1564_TIMER_IRQ_REG				0x5c
-#define APCI1564_TIMER_WARN_TIMEVAL_REG			0x60
-#define APCI1564_TIMER_WARN_TIMEBASE_REG		0x64
-
-/*
- * dev->iobase Register Map
- */
-#define APCI1564_COUNTER_REG(x)				(0x00 + ((x) * 0x20))
-#define APCI1564_COUNTER_RELOAD_REG(x)			(0x04 + ((x) * 0x20))
-#define APCI1564_COUNTER_TIMEBASE_REG(x)		(0x08 + ((x) * 0x20))
-#define APCI1564_COUNTER_CTRL_REG(x)			(0x0c + ((x) * 0x20))
-#define APCI1564_COUNTER_STATUS_REG(x)			(0x10 + ((x) * 0x20))
-#define APCI1564_COUNTER_IRQ_REG(x)			(0x14 + ((x) * 0x20))
-#define APCI1564_COUNTER_WARN_TIMEVAL_REG(x)		(0x18 + ((x) * 0x20))
-#define APCI1564_COUNTER_WARN_TIMEBASE_REG(x)		(0x1c + ((x) * 0x20))
-
-/*
- * Configures The Timer or Counter
- *
- * data[0] Configure as: 0 = Timer, 1 = Counter
- * data[1] 1 = Enable Interrupt, 0 = Disable Interrupt
- * data[2] Time Unit
- * data[3] Reload Value
- * data[4] Timer Mode
- * data[5] Timer Counter Watchdog Number
- * data[6] Counter Direction
- */
-static int apci1564_timer_config(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn,
-				 unsigned int *data)
+
+static int apci1564_timer_insn_config(struct comedi_device *dev,
+				      struct comedi_subdevice *s,
+				      struct comedi_insn *insn,
+				      unsigned int *data)
 {
 	struct apci1564_private *devpriv = dev->private;
-	unsigned int ul_Command1 = 0;
+	unsigned int ctrl;
 
 	devpriv->tsk_current = current;
-	if (data[0] == ADDIDATA_TIMER) {
-		/* First Stop The Timer */
-		ul_Command1 = inl(devpriv->amcc_iobase + APCI1564_TIMER_CTRL_REG);
-		ul_Command1 = ul_Command1 & 0xFFFFF9FEUL;
-		/* Stop The Timer */
-		outl(ul_Command1, devpriv->amcc_iobase + APCI1564_TIMER_CTRL_REG);
-
-		devpriv->timer_select_mode = ADDIDATA_TIMER;
-		if (data[1] == 1) {
-			/* Enable TIMER int & DISABLE ALL THE OTHER int SOURCES */
-			outl(0x02, devpriv->amcc_iobase + APCI1564_TIMER_CTRL_REG);
-			outl(0x0, devpriv->amcc_iobase + APCI1564_DI_IRQ_REG);
-			outl(0x0, devpriv->amcc_iobase + APCI1564_DO_IRQ_REG);
-			outl(0x0, devpriv->amcc_iobase + APCI1564_WDOG_IRQ_REG);
-			outl(0x0, dev->iobase +
-			    APCI1564_COUNTER_IRQ_REG(APCI1564_COUNTER1));
-			outl(0x0, dev->iobase +
-			    APCI1564_COUNTER_IRQ_REG(APCI1564_COUNTER2));
-			outl(0x0, dev->iobase +
-			    APCI1564_COUNTER_IRQ_REG(APCI1564_COUNTER3));
-			outl(0x0, dev->iobase +
-			    APCI1564_COUNTER_IRQ_REG(APCI1564_COUNTER4));
-		} else {
-			/* disable Timer interrupt */
-			outl(0x0, devpriv->amcc_iobase + APCI1564_TIMER_CTRL_REG);
-		}
 
-		/*  Loading Timebase */
-		outl(data[2], devpriv->amcc_iobase + APCI1564_TIMER_TIMEBASE_REG);
-
-		/* Loading the Reload value */
-		outl(data[3], devpriv->amcc_iobase + APCI1564_TIMER_RELOAD_REG);
-
-		ul_Command1 = inl(devpriv->amcc_iobase + APCI1564_TIMER_CTRL_REG);
-		ul_Command1 = (ul_Command1 & 0xFFF719E2UL) | 2UL << 13UL | 0x10UL;
-		/* mode 2 */
-		outl(ul_Command1, devpriv->amcc_iobase + APCI1564_TIMER_CTRL_REG);
-	} else if (data[0] == ADDIDATA_COUNTER) {
-		devpriv->timer_select_mode = ADDIDATA_COUNTER;
-		devpriv->mode_select_register = data[5];
-
-		/* First Stop The Counter */
-		ul_Command1 = inl(dev->iobase +
-				 APCI1564_COUNTER_CTRL_REG(data[5] - 1));
-		ul_Command1 = ul_Command1 & 0xFFFFF9FEUL;
-		/* Stop The Timer */
-		outl(ul_Command1, dev->iobase +
-					APCI1564_COUNTER_CTRL_REG(data[5] - 1));
-
-		/* Set the reload value */
-		outl(data[3], dev->iobase +
-					APCI1564_COUNTER_RELOAD_REG(data[5] - 1));
-
-		/* Set the mode :             */
-		/* - Disable the hardware     */
-		/* - Disable the counter mode */
-		/* - Disable the warning      */
-		/* - Disable the reset        */
-		/* - Disable the timer mode   */
-		/* - Enable the counter mode  */
-
-		ul_Command1 =
-			(ul_Command1 & 0xFFFC19E2UL) | 0x80000UL |
-			(unsigned int) ((unsigned int) data[4] << 16UL);
-		outl(ul_Command1, dev->iobase +
-					APCI1564_COUNTER_CTRL_REG(data[5] - 1));
-
-		/*  Enable or Disable Interrupt */
-		ul_Command1 = (ul_Command1 & 0xFFFFF9FD) | (data[1] << 1);
-		outl(ul_Command1, dev->iobase +
-					APCI1564_COUNTER_CTRL_REG(data[5] - 1));
-
-		/* Set the Up/Down selection */
-		ul_Command1 = (ul_Command1 & 0xFFFBF9FFUL) | (data[6] << 18);
-		outl(ul_Command1, dev->iobase +
-					APCI1564_COUNTER_CTRL_REG(data[5] - 1));
+	/* First Stop The Timer */
+	ctrl = inl(devpriv->timer + ADDI_TCW_CTRL_REG);
+	ctrl &= 0xfffff9fe;
+	/* Stop The Timer */
+	outl(ctrl, devpriv->timer + ADDI_TCW_CTRL_REG);
+
+	if (data[1] == 1) {
+		/* Enable timer int & disable all the other int sources */
+		outl(0x02, devpriv->timer + ADDI_TCW_CTRL_REG);
+		outl(0x0, dev->iobase + APCI1564_DI_IRQ_REG);
+		outl(0x0, dev->iobase + APCI1564_DO_IRQ_REG);
+		outl(0x0, dev->iobase + APCI1564_WDOG_IRQ_REG);
+		if (devpriv->counters) {
+			unsigned long iobase;
+
+			iobase = devpriv->counters + ADDI_TCW_IRQ_REG;
+			outl(0x0, iobase + APCI1564_COUNTER(0));
+			outl(0x0, iobase + APCI1564_COUNTER(1));
+			outl(0x0, iobase + APCI1564_COUNTER(2));
+		}
 	} else {
-		dev_err(dev->class_dev, "Invalid subdevice.\n");
+		/* disable Timer interrupt */
+		outl(0x0, devpriv->timer + ADDI_TCW_CTRL_REG);
 	}
+
+	/* Loading Timebase */
+	outl(data[2], devpriv->timer + ADDI_TCW_TIMEBASE_REG);
+
+	/* Loading the Reload value */
+	outl(data[3], devpriv->timer + ADDI_TCW_RELOAD_REG);
+
+	ctrl = inl(devpriv->timer + ADDI_TCW_CTRL_REG);
+	ctrl &= 0xfff719e2;
+	ctrl |= (2 << 13) | 0x10;
+	/* mode 2 */
+	outl(ctrl, devpriv->timer + ADDI_TCW_CTRL_REG);
+
 	return insn->n;
 }
 
-/*
- * Start / Stop The Selected Timer or Counter
- *
- * data[0] Configure as: 0 = Timer, 1 = Counter
- * data[1] 0 = Stop, 1 = Start, 2 = Trigger Clear (Only Counter)
- */
-static int apci1564_timer_write(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn,
-				unsigned int *data)
+static int apci1564_timer_insn_write(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     struct comedi_insn *insn,
+				     unsigned int *data)
 {
 	struct apci1564_private *devpriv = dev->private;
-	unsigned int ul_Command1 = 0;
+	unsigned int ctrl;
+
+	ctrl = inl(devpriv->timer + ADDI_TCW_CTRL_REG);
+	switch (data[1]) {
+	case 0:	/* Stop The Timer */
+		ctrl &= 0xfffff9fe;
+		break;
+	case 1:	/* Enable the Timer */
+		ctrl &= 0xfffff9ff;
+		ctrl |= 0x1;
+		break;
+	}
+	outl(ctrl, devpriv->timer + ADDI_TCW_CTRL_REG);
 
-	if (devpriv->timer_select_mode == ADDIDATA_TIMER) {
-		if (data[1] == 1) {
-			ul_Command1 = inl(devpriv->amcc_iobase + APCI1564_TIMER_CTRL_REG);
-			ul_Command1 = (ul_Command1 & 0xFFFFF9FFUL) | 0x1UL;
+	return insn->n;
+}
 
-			/* Enable the Timer */
-			outl(ul_Command1, devpriv->amcc_iobase + APCI1564_TIMER_CTRL_REG);
-		} else if (data[1] == 0) {
-			/* Stop The Timer */
+static int apci1564_timer_insn_read(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_insn *insn,
+				    unsigned int *data)
+{
+	struct apci1564_private *devpriv = dev->private;
+
+	/* Stores the status of the Timer */
+	data[0] = inl(devpriv->timer + ADDI_TCW_STATUS_REG) & 0x1;
+
+	/* Stores the Actual value of the Timer */
+	data[1] = inl(devpriv->timer + ADDI_TCW_VAL_REG);
 
-			ul_Command1 = inl(devpriv->amcc_iobase + APCI1564_TIMER_CTRL_REG);
-			ul_Command1 = ul_Command1 & 0xFFFFF9FEUL;
-			outl(ul_Command1, devpriv->amcc_iobase + APCI1564_TIMER_CTRL_REG);
-		}
-	} else if (devpriv->timer_select_mode == ADDIDATA_COUNTER) {
-		ul_Command1 =
-			inl(dev->iobase +
-			   APCI1564_COUNTER_CTRL_REG(devpriv->mode_select_register - 1));
-		if (data[1] == 1) {
-			/* Start the Counter subdevice */
-			ul_Command1 = (ul_Command1 & 0xFFFFF9FFUL) | 0x1UL;
-		} else if (data[1] == 0) {
-			/*  Stops the Counter subdevice */
-			ul_Command1 = 0;
-
-		} else if (data[1] == 2) {
-			/*  Clears the Counter subdevice */
-			ul_Command1 = (ul_Command1 & 0xFFFFF9FFUL) | 0x400;
-		}
-		outl(ul_Command1, dev->iobase +
-		     APCI1564_COUNTER_CTRL_REG(devpriv->mode_select_register - 1));
-	} else {
-		dev_err(dev->class_dev, "Invalid subdevice.\n");
-	}
 	return insn->n;
 }
 
-/*
- * Read The Selected Timer or Counter
- */
-static int apci1564_timer_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn,
-			       unsigned int *data)
+static int apci1564_counter_insn_config(struct comedi_device *dev,
+					struct comedi_subdevice *s,
+					struct comedi_insn *insn,
+					unsigned int *data)
 {
 	struct apci1564_private *devpriv = dev->private;
-	unsigned int ul_Command1 = 0;
-
-	if (devpriv->timer_select_mode == ADDIDATA_TIMER) {
-		/*  Stores the status of the Timer */
-		data[0] = inl(devpriv->amcc_iobase + APCI1564_TIMER_STATUS_REG) & 0x1;
-
-		/*  Stores the Actual value of the Timer */
-		data[1] = inl(devpriv->amcc_iobase + APCI1564_TIMER_REG);
-	} else if (devpriv->timer_select_mode == ADDIDATA_COUNTER) {
-		/*  Read the Counter Actual Value. */
-		data[0] =
-			inl(dev->iobase +
-			    APCI1564_COUNTER_REG(devpriv->mode_select_register - 1));
-		ul_Command1 =
-			inl(dev->iobase +
-			    APCI1564_COUNTER_STATUS_REG(devpriv->mode_select_register - 1));
-
-		/* Get the software trigger status */
-		data[1] = (unsigned char) ((ul_Command1 >> 1) & 1);
-
-		/* Get the hardware trigger status */
-		data[2] = (unsigned char) ((ul_Command1 >> 2) & 1);
-
-		/* Get the software clear status */
-		data[3] = (unsigned char) ((ul_Command1 >> 3) & 1);
-
-		/* Get the overflow status */
-		data[4] = (unsigned char) ((ul_Command1 >> 0) & 1);
-	} else {
-		dev_err(dev->class_dev, "Invalid subdevice.\n");
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned long iobase = devpriv->counters + APCI1564_COUNTER(chan);
+	unsigned int ctrl;
+
+	devpriv->tsk_current = current;
+
+	/* First Stop The Counter */
+	ctrl = inl(iobase + ADDI_TCW_CTRL_REG);
+	ctrl &= 0xfffff9fe;
+	/* Stop The Timer */
+	outl(ctrl, iobase + ADDI_TCW_CTRL_REG);
+
+	/* Set the reload value */
+	outl(data[3], iobase + ADDI_TCW_RELOAD_REG);
+
+	/* Set the mode :             */
+	/* - Disable the hardware     */
+	/* - Disable the counter mode */
+	/* - Disable the warning      */
+	/* - Disable the reset        */
+	/* - Disable the timer mode   */
+	/* - Enable the counter mode  */
+
+	ctrl &= 0xfffc19e2;
+	ctrl |= 0x80000 | (data[4] << 16);
+	outl(ctrl, iobase + ADDI_TCW_CTRL_REG);
+
+	/* Enable or Disable Interrupt */
+	ctrl &= 0xfffff9fd;
+	ctrl |= (data[1] << 1);
+	outl(ctrl, iobase + ADDI_TCW_CTRL_REG);
+
+	/* Set the Up/Down selection */
+	ctrl &= 0xfffbf9ff;
+	ctrl |= (data[6] << 18);
+	outl(ctrl, iobase + ADDI_TCW_CTRL_REG);
+
+	return insn->n;
+}
+
+static int apci1564_counter_insn_write(struct comedi_device *dev,
+				       struct comedi_subdevice *s,
+				       struct comedi_insn *insn,
+				       unsigned int *data)
+{
+	struct apci1564_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned long iobase = devpriv->counters + APCI1564_COUNTER(chan);
+	unsigned int ctrl;
+
+	ctrl = inl(iobase + ADDI_TCW_CTRL_REG);
+	switch (data[1]) {
+	case 0:	/* Stops the Counter subdevice */
+		ctrl = 0;
+		break;
+	case 1:	/* Start the Counter subdevice */
+		ctrl &= 0xfffff9ff;
+		ctrl |= 0x1;
+		break;
+	case 2:	/* Clears the Counter subdevice */
+		ctrl &= 0xfffff9ff;
+		ctrl |= 0x400;
+		break;
 	}
+	outl(ctrl, iobase + ADDI_TCW_CTRL_REG);
+
+	return insn->n;
+}
+
+static int apci1564_counter_insn_read(struct comedi_device *dev,
+				      struct comedi_subdevice *s,
+				      struct comedi_insn *insn,
+				      unsigned int *data)
+{
+	struct apci1564_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned long iobase = devpriv->counters + APCI1564_COUNTER(chan);
+	unsigned int status;
+
+	/* Read the Counter Actual Value. */
+	data[0] = inl(iobase + ADDI_TCW_VAL_REG);
+
+	status = inl(iobase + ADDI_TCW_STATUS_REG);
+	data[1] = (status >> 1) & 1;	/* software trigger status */
+	data[2] = (status >> 2) & 1;	/* hardware trigger status */
+	data[3] = (status >> 3) & 1;	/* software clear status */
+	data[4] = (status >> 0) & 1;	/* overflow status */
+
 	return insn->n;
 }
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c
deleted file mode 100644
index 2950815b65f4..000000000000
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c
+++ /dev/null
@@ -1,2050 +0,0 @@
-/**
-@verbatim
-
-Copyright (C) 2004,2005  ADDI-DATA GmbH for the source code of this module.
-
-	ADDI-DATA GmbH
-	Dieselstrasse 3
-	D-77833 Ottersweier
-	Tel: +19(0)7223/9493-0
-	Fax: +49(0)7223/9493-92
-	http://www.addi-data.com
-	info@addi-data.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.
-
-@endverbatim
-*/
-/*
-  +-----------------------------------------------------------------------+
-  | (C) ADDI-DATA GmbH          Dieselstrasse 3      D-77833 Ottersweier  |
-  +-----------------------------------------------------------------------+
-  | Tel : +49 (0) 7223/9493-0     | email    : info@addi-data.com         |
-  | Fax : +49 (0) 7223/9493-92    | Internet : http://www.addi-data.com   |
-  +-----------------------------------------------------------------------+
-  | Project     : APCI-3120       | Compiler   : GCC                      |
-  | Module name : hwdrv_apci3120.c| Version    : 2.96                     |
-  +-------------------------------+---------------------------------------+
-  | Project manager: Eric Stolz   | Date       :  02/12/2002              |
-  +-----------------------------------------------------------------------+
-  | Description :APCI3120 Module.  Hardware abstraction Layer for APCI3120|
-  +-----------------------------------------------------------------------+
-  |                             UPDATE'S                                  |
-  +-----------------------------------------------------------------------+
-  |   Date   |   Author  |          Description of updates                |
-  +----------+-----------+------------------------------------------------+
-  |          | 		 | 						  |
-  |          |           |						  |
-  +----------+-----------+------------------------------------------------+
-*/
-
-#include <linux/delay.h>
-
-/*
- * ADDON RELATED ADDITIONS
- */
-/* Constant */
-#define APCI3120_ENABLE_TRANSFER_ADD_ON_LOW		0x00
-#define APCI3120_ENABLE_TRANSFER_ADD_ON_HIGH		0x1200
-#define APCI3120_A2P_FIFO_MANAGEMENT			0x04000400L
-#define APCI3120_AMWEN_ENABLE				0x02
-#define APCI3120_A2P_FIFO_WRITE_ENABLE			0x01
-#define APCI3120_FIFO_ADVANCE_ON_BYTE_2			0x20000000L
-#define APCI3120_ENABLE_WRITE_TC_INT			0x00004000L
-#define APCI3120_CLEAR_WRITE_TC_INT			0x00040000L
-#define APCI3120_DISABLE_AMWEN_AND_A2P_FIFO_WRITE	0x0
-#define APCI3120_DISABLE_BUS_MASTER_ADD_ON		0x0
-#define APCI3120_DISABLE_BUS_MASTER_PCI			0x0
-
-/* ADD_ON ::: this needed since apci supports 16 bit interface to add on */
-#define APCI3120_ADD_ON_AGCSTS_LOW	0x3C
-#define APCI3120_ADD_ON_AGCSTS_HIGH	(APCI3120_ADD_ON_AGCSTS_LOW + 2)
-#define APCI3120_ADD_ON_MWAR_LOW	0x24
-#define APCI3120_ADD_ON_MWAR_HIGH	(APCI3120_ADD_ON_MWAR_LOW + 2)
-#define APCI3120_ADD_ON_MWTC_LOW	0x058
-#define APCI3120_ADD_ON_MWTC_HIGH	(APCI3120_ADD_ON_MWTC_LOW + 2)
-
-/* AMCC */
-#define APCI3120_AMCC_OP_MCSR		0x3C
-#define APCI3120_AMCC_OP_REG_INTCSR	0x38
-
-/* for transfer count enable bit */
-#define AGCSTS_TC_ENABLE	0x10000000
-
-/* used for test on mixture of BIP/UNI ranges */
-#define APCI3120_BIPOLAR_RANGES		4
-
-#define APCI3120_ADDRESS_RANGE		16
-
-#define APCI3120_DISABLE		0
-#define APCI3120_ENABLE			1
-
-#define APCI3120_START			1
-#define APCI3120_STOP			0
-
-#define APCI3120_EOC_MODE		1
-#define APCI3120_EOS_MODE		2
-#define APCI3120_DMA_MODE		3
-
-/* DIGITAL INPUT-OUTPUT DEFINE */
-
-#define APCI3120_DIGITAL_OUTPUT		0x0d
-#define APCI3120_RD_STATUS		0x02
-#define APCI3120_RD_FIFO		0x00
-
-/* digital output insn_write ON /OFF selection */
-#define	APCI3120_SET4DIGITALOUTPUTON	1
-#define APCI3120_SET4DIGITALOUTPUTOFF	0
-
-/* analog output SELECT BIT */
-#define APCI3120_ANALOG_OP_CHANNEL_1	0x0000
-#define APCI3120_ANALOG_OP_CHANNEL_2	0x4000
-#define APCI3120_ANALOG_OP_CHANNEL_3	0x8000
-#define APCI3120_ANALOG_OP_CHANNEL_4	0xc000
-#define APCI3120_ANALOG_OP_CHANNEL_5	0x0000
-#define APCI3120_ANALOG_OP_CHANNEL_6	0x4000
-#define APCI3120_ANALOG_OP_CHANNEL_7	0x8000
-#define APCI3120_ANALOG_OP_CHANNEL_8	0xc000
-
-/* Enable external trigger bit in nWrAddress */
-#define APCI3120_ENABLE_EXT_TRIGGER	0x8000
-
-/* ANALOG OUTPUT AND INPUT DEFINE */
-#define APCI3120_UNIPOLAR		0x80
-#define APCI3120_BIPOLAR		0x00
-#define APCI3120_ANALOG_OUTPUT_1	0x08
-#define APCI3120_ANALOG_OUTPUT_2	0x0a
-#define APCI3120_1_GAIN			0x00
-#define APCI3120_2_GAIN			0x10
-#define APCI3120_5_GAIN			0x20
-#define APCI3120_10_GAIN		0x30
-#define APCI3120_SEQ_RAM_ADDRESS	0x06
-#define APCI3120_RESET_FIFO		0x0c
-#define APCI3120_TIMER_0_MODE_2		0x01
-#define APCI3120_TIMER_0_MODE_4		0x2
-#define APCI3120_SELECT_TIMER_0_WORD	0x00
-#define APCI3120_ENABLE_TIMER0		0x1000
-#define APCI3120_CLEAR_PR		0xf0ff
-#define APCI3120_CLEAR_PA		0xfff0
-#define APCI3120_CLEAR_PA_PR		(APCI3120_CLEAR_PR & APCI3120_CLEAR_PA)
-
-/* nWrMode_Select */
-#define APCI3120_ENABLE_SCAN		0x8
-#define APCI3120_DISABLE_SCAN		(~APCI3120_ENABLE_SCAN)
-#define APCI3120_ENABLE_EOS_INT		0x2
-
-#define APCI3120_DISABLE_EOS_INT	(~APCI3120_ENABLE_EOS_INT)
-#define APCI3120_ENABLE_EOC_INT		0x1
-#define APCI3120_DISABLE_EOC_INT	(~APCI3120_ENABLE_EOC_INT)
-#define APCI3120_DISABLE_ALL_INTERRUPT_WITHOUT_TIMER	\
-	(APCI3120_DISABLE_EOS_INT & APCI3120_DISABLE_EOC_INT)
-#define APCI3120_DISABLE_ALL_INTERRUPT			\
-	(APCI3120_DISABLE_TIMER_INT & APCI3120_DISABLE_EOS_INT & APCI3120_DISABLE_EOC_INT)
-
-/* status register bits */
-#define APCI3120_EOC			0x8000
-#define APCI3120_EOS			0x2000
-
-/* software trigger dummy register */
-#define APCI3120_START_CONVERSION	0x02
-
-/* TIMER DEFINE */
-#define APCI3120_QUARTZ_A		70
-#define APCI3120_QUARTZ_B		50
-#define APCI3120_TIMER			1
-#define APCI3120_WATCHDOG		2
-#define APCI3120_TIMER_DISABLE		0
-#define APCI3120_TIMER_ENABLE		1
-#define APCI3120_ENABLE_TIMER2		0x4000
-#define APCI3120_DISABLE_TIMER2		(~APCI3120_ENABLE_TIMER2)
-#define APCI3120_ENABLE_TIMER_INT	0x04
-#define APCI3120_DISABLE_TIMER_INT	(~APCI3120_ENABLE_TIMER_INT)
-#define APCI3120_WRITE_MODE_SELECT	0x0e
-#define APCI3120_SELECT_TIMER_0_WORD	0x00
-#define APCI3120_SELECT_TIMER_1_WORD	0x01
-#define APCI3120_TIMER_1_MODE_2		0x4
-
-/* $$ BIT FOR MODE IN nCsTimerCtr1 */
-#define APCI3120_TIMER_2_MODE_0		0x0
-#define APCI3120_TIMER_2_MODE_2		0x10
-#define APCI3120_TIMER_2_MODE_5		0x30
-
-/* $$ BIT FOR MODE IN nCsTimerCtr0 */
-#define APCI3120_SELECT_TIMER_2_LOW_WORD	0x02
-#define APCI3120_SELECT_TIMER_2_HIGH_WORD	0x03
-
-#define APCI3120_TIMER_CRT0		0x0d
-#define APCI3120_TIMER_CRT1		0x0c
-
-#define APCI3120_TIMER_VALUE		0x04
-#define APCI3120_TIMER_STATUS_REGISTER	0x0d
-#define APCI3120_RD_STATUS		0x02
-#define APCI3120_WR_ADDRESS		0x00
-#define APCI3120_ENABLE_WATCHDOG	0x20
-#define APCI3120_DISABLE_WATCHDOG	(~APCI3120_ENABLE_WATCHDOG)
-#define APCI3120_ENABLE_TIMER_COUNTER	0x10
-#define APCI3120_DISABLE_TIMER_COUNTER	(~APCI3120_ENABLE_TIMER_COUNTER)
-#define APCI3120_FC_TIMER		0x1000
-#define APCI3120_ENABLE_TIMER0		0x1000
-#define APCI3120_ENABLE_TIMER1		0x2000
-#define APCI3120_ENABLE_TIMER2		0x4000
-#define APCI3120_DISABLE_TIMER0		(~APCI3120_ENABLE_TIMER0)
-#define APCI3120_DISABLE_TIMER1		(~APCI3120_ENABLE_TIMER1)
-#define APCI3120_DISABLE_TIMER2		(~APCI3120_ENABLE_TIMER2)
-
-#define APCI3120_TIMER2_SELECT_EOS	0xc0
-#define APCI3120_COUNTER		3
-#define APCI3120_DISABLE_ALL_TIMER	(APCI3120_DISABLE_TIMER0 &	\
-					 APCI3120_DISABLE_TIMER1 &	\
-					 APCI3120_DISABLE_TIMER2)
-
-#define MAX_ANALOGINPUT_CHANNELS	32
-
-struct str_AnalogReadInformation {
-	/* EOC or EOS */
-	unsigned char b_Type;
-	/* Interrupt use or not */
-	unsigned char b_InterruptFlag;
-	/* Selection of the conversion time */
-	unsigned int ui_ConvertTiming;
-	/* Number of channel to read */
-	unsigned char b_NbrOfChannel;
-	/* Number of the channel to be read */
-	unsigned int ui_ChannelList[MAX_ANALOGINPUT_CHANNELS];
-	/* Gain of each channel */
-	unsigned int ui_RangeList[MAX_ANALOGINPUT_CHANNELS];
-};
-
-/* ANALOG INPUT RANGE */
-static const struct comedi_lrange range_apci3120_ai = {
-	8, {
-		BIP_RANGE(10),
-		BIP_RANGE(5),
-		BIP_RANGE(2),
-		BIP_RANGE(1),
-		UNI_RANGE(10),
-		UNI_RANGE(5),
-		UNI_RANGE(2),
-		UNI_RANGE(1)
-	}
-};
-
-/* ANALOG OUTPUT RANGE */
-static const struct comedi_lrange range_apci3120_ao = {
-	2, {
-		BIP_RANGE(10),
-		UNI_RANGE(10)
-	}
-};
-
-
-/* FUNCTION DEFINITIONS */
-static int apci3120_ai_insn_config(struct comedi_device *dev,
-				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn,
-				   unsigned int *data)
-{
-	const struct addi_board *this_board = dev->board_ptr;
-	struct addi_private *devpriv = dev->private;
-	unsigned int i;
-
-	if ((data[0] != APCI3120_EOC_MODE) && (data[0] != APCI3120_EOS_MODE))
-		return -1;
-
-	/*  Check for Conversion time to be added */
-	devpriv->ui_EocEosConversionTime = data[2];
-
-	if (data[0] == APCI3120_EOS_MODE) {
-
-		/* Test the number of the channel */
-		for (i = 0; i < data[3]; i++) {
-
-			if (CR_CHAN(data[4 + i]) >=
-				this_board->i_NbrAiChannel) {
-				dev_err(dev->class_dev, "bad channel list\n");
-				return -2;
-			}
-		}
-
-		devpriv->b_InterruptMode = APCI3120_EOS_MODE;
-
-		if (data[1])
-			devpriv->b_EocEosInterrupt = APCI3120_ENABLE;
-		else
-			devpriv->b_EocEosInterrupt = APCI3120_DISABLE;
-		/*  Copy channel list and Range List to devpriv */
-		devpriv->ui_AiNbrofChannels = data[3];
-		for (i = 0; i < devpriv->ui_AiNbrofChannels; i++)
-			devpriv->ui_AiChannelList[i] = data[4 + i];
-
-	} else {			/*  EOC */
-		devpriv->b_InterruptMode = APCI3120_EOC_MODE;
-		if (data[1])
-			devpriv->b_EocEosInterrupt = APCI3120_ENABLE;
-		else
-			devpriv->b_EocEosInterrupt = APCI3120_DISABLE;
-	}
-
-	return insn->n;
-}
-
-/*
- * This function will first check channel list is ok or not and then
- * initialize the sequence RAM with the polarity, Gain,Channel number.
- * If the last argument of function "check"is 1 then it only checks
- * the channel list is ok or not.
- */
-static int apci3120_setup_chan_list(struct comedi_device *dev,
-				    struct comedi_subdevice *s,
-				    int n_chan,
-				    unsigned int *chanlist,
-				    char check)
-{
-	struct addi_private *devpriv = dev->private;
-	unsigned int i;
-	unsigned int gain;
-	unsigned short us_TmpValue;
-
-	/* correct channel and range number check itself comedi/range.c */
-	if (n_chan < 1) {
-		if (!check)
-			dev_err(dev->class_dev,
-				"range/channel list is empty!\n");
-		return 0;
-	}
-	/*  All is ok, so we can setup channel/range list */
-	if (check)
-		return 1;
-
-	/* Code  to set the PA and PR...Here it set PA to 0 */
-	devpriv->us_OutputRegister =
-		devpriv->us_OutputRegister & APCI3120_CLEAR_PA_PR;
-	devpriv->us_OutputRegister = ((n_chan - 1) & 0xf) << 8;
-	outw(devpriv->us_OutputRegister, dev->iobase + APCI3120_WR_ADDRESS);
-
-	for (i = 0; i < n_chan; i++) {
-		/*  store range list to card */
-		us_TmpValue = CR_CHAN(chanlist[i]);	/*  get channel number */
-
-		if (CR_RANGE(chanlist[i]) < APCI3120_BIPOLAR_RANGES)
-			us_TmpValue &= ((~APCI3120_UNIPOLAR) & 0xff);	/*  set bipolar */
-		else
-			us_TmpValue |= APCI3120_UNIPOLAR;	/*  enable unipolar */
-
-		gain = CR_RANGE(chanlist[i]);	/*  get gain number */
-		us_TmpValue |= ((gain & 0x03) << 4);	/* <<4 for G0 and G1 bit in RAM */
-		us_TmpValue |= i << 8;	/* To select the RAM LOCATION */
-		outw(us_TmpValue, dev->iobase + APCI3120_SEQ_RAM_ADDRESS);
-	}
-	return 1;		/*  we can serve this with scan logic */
-}
-
-/*
- * Reads analog input in synchronous mode EOC and EOS is selected
- * as per configured if no conversion time is set uses default
- * conversion time 10 microsec.
- */
-static int apci3120_ai_insn_read(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn,
-				 unsigned int *data)
-{
-	const struct addi_board *this_board = dev->board_ptr;
-	struct addi_private *devpriv = dev->private;
-	unsigned short us_ConvertTiming, us_TmpValue, i;
-	unsigned char b_Tmp;
-
-	/*  fix conversion time to 10 us */
-	if (!devpriv->ui_EocEosConversionTime)
-		us_ConvertTiming = 10;
-	else
-		us_ConvertTiming = (unsigned short) (devpriv->ui_EocEosConversionTime / 1000);	/*  nano to useconds */
-
-	/*  Clear software registers */
-	devpriv->b_TimerSelectMode = 0;
-	devpriv->b_ModeSelectRegister = 0;
-	devpriv->us_OutputRegister = 0;
-
-	if (insn->unused[0] == 222) {	/*  second insn read */
-		for (i = 0; i < insn->n; i++)
-			data[i] = devpriv->ui_AiReadData[i];
-	} else {
-		devpriv->tsk_Current = current;	/*  Save the current process task structure */
-
-		/*
-		 * Testing if board have the new Quartz and calculate the time value
-		 * to set in the timer
-		 */
-		us_TmpValue =
-			(unsigned short) inw(devpriv->iobase + APCI3120_RD_STATUS);
-
-		/* EL250804: Testing if board APCI3120 have the new Quartz or if it is an APCI3001 */
-		if ((us_TmpValue & 0x00B0) == 0x00B0
-			|| !strcmp(this_board->pc_DriverName, "apci3001")) {
-			us_ConvertTiming = (us_ConvertTiming * 2) - 2;
-		} else {
-			us_ConvertTiming =
-				((us_ConvertTiming * 12926) / 10000) - 1;
-		}
-
-		us_TmpValue = (unsigned short) devpriv->b_InterruptMode;
-
-		switch (us_TmpValue) {
-
-		case APCI3120_EOC_MODE:
-
-			/*
-			 * Testing the interrupt flag and set the EOC bit Clears the FIFO
-			 */
-			inw(devpriv->iobase + APCI3120_RESET_FIFO);
-
-			/*  Initialize the sequence array */
-			if (!apci3120_setup_chan_list(dev, s, 1,
-					&insn->chanspec, 0))
-				return -EINVAL;
-
-			/* Initialize Timer 0 mode 4 */
-			devpriv->b_TimerSelectMode =
-				(devpriv->
-				b_TimerSelectMode & 0xFC) |
-				APCI3120_TIMER_0_MODE_4;
-			outb(devpriv->b_TimerSelectMode,
-				devpriv->iobase + APCI3120_TIMER_CRT1);
-
-			/*  Reset the scan bit and Disables the  EOS, DMA, EOC interrupt */
-			devpriv->b_ModeSelectRegister =
-				devpriv->
-				b_ModeSelectRegister & APCI3120_DISABLE_SCAN;
-
-			if (devpriv->b_EocEosInterrupt == APCI3120_ENABLE) {
-
-				/* Disables the EOS,DMA and enables the EOC interrupt */
-				devpriv->b_ModeSelectRegister =
-					(devpriv->
-					b_ModeSelectRegister &
-					APCI3120_DISABLE_EOS_INT) |
-					APCI3120_ENABLE_EOC_INT;
-				inw(devpriv->iobase);
-
-			} else {
-				devpriv->b_ModeSelectRegister =
-					devpriv->
-					b_ModeSelectRegister &
-					APCI3120_DISABLE_ALL_INTERRUPT_WITHOUT_TIMER;
-			}
-
-			outb(devpriv->b_ModeSelectRegister,
-				devpriv->iobase + APCI3120_WRITE_MODE_SELECT);
-
-			/*  Sets gate 0 */
-			devpriv->us_OutputRegister =
-				(devpriv->
-				us_OutputRegister & APCI3120_CLEAR_PA_PR) |
-				APCI3120_ENABLE_TIMER0;
-			outw(devpriv->us_OutputRegister,
-				devpriv->iobase + APCI3120_WR_ADDRESS);
-
-			/*  Select Timer 0 */
-			b_Tmp = ((devpriv->
-					b_DigitalOutputRegister) & 0xF0) |
-				APCI3120_SELECT_TIMER_0_WORD;
-			outb(b_Tmp, devpriv->iobase + APCI3120_TIMER_CRT0);
-
-			/* Set the conversion time */
-			outw(us_ConvertTiming,
-				devpriv->iobase + APCI3120_TIMER_VALUE);
-
-			us_TmpValue =
-				(unsigned short) inw(dev->iobase + APCI3120_RD_STATUS);
-
-			if (devpriv->b_EocEosInterrupt == APCI3120_DISABLE) {
-
-				do {
-					/*  Waiting for the end of conversion */
-					us_TmpValue =
-						inw(devpriv->iobase +
-						APCI3120_RD_STATUS);
-				} while ((us_TmpValue & APCI3120_EOC) ==
-					APCI3120_EOC);
-
-				/* Read the result in FIFO  and put it in insn data pointer */
-				us_TmpValue = inw(devpriv->iobase + 0);
-				*data = us_TmpValue;
-
-				inw(devpriv->iobase + APCI3120_RESET_FIFO);
-			}
-
-			break;
-
-		case APCI3120_EOS_MODE:
-
-			inw(devpriv->iobase);
-			/*  Clears the FIFO */
-			inw(devpriv->iobase + APCI3120_RESET_FIFO);
-			/*  clear PA PR  and disable timer 0 */
-
-			devpriv->us_OutputRegister =
-				(devpriv->
-				us_OutputRegister & APCI3120_CLEAR_PA_PR) |
-				APCI3120_DISABLE_TIMER0;
-
-			outw(devpriv->us_OutputRegister,
-				devpriv->iobase + APCI3120_WR_ADDRESS);
-
-			if (!apci3120_setup_chan_list(dev, s,
-					devpriv->ui_AiNbrofChannels,
-					devpriv->ui_AiChannelList, 0))
-				return -EINVAL;
-
-			/* Initialize Timer 0 mode 2 */
-			devpriv->b_TimerSelectMode =
-				(devpriv->
-				b_TimerSelectMode & 0xFC) |
-				APCI3120_TIMER_0_MODE_2;
-			outb(devpriv->b_TimerSelectMode,
-				devpriv->iobase + APCI3120_TIMER_CRT1);
-
-			/* Select Timer 0 */
-			b_Tmp = ((devpriv->
-					b_DigitalOutputRegister) & 0xF0) |
-				APCI3120_SELECT_TIMER_0_WORD;
-			outb(b_Tmp, devpriv->iobase + APCI3120_TIMER_CRT0);
-
-			/* Set the conversion time */
-			outw(us_ConvertTiming,
-				devpriv->iobase + APCI3120_TIMER_VALUE);
-
-			/* Set the scan bit */
-			devpriv->b_ModeSelectRegister =
-				devpriv->
-				b_ModeSelectRegister | APCI3120_ENABLE_SCAN;
-			outb(devpriv->b_ModeSelectRegister,
-				devpriv->iobase + APCI3120_WRITE_MODE_SELECT);
-
-			/* If Interrupt function is loaded */
-			if (devpriv->b_EocEosInterrupt == APCI3120_ENABLE) {
-				/* Disables the EOC,DMA and enables the EOS interrupt */
-				devpriv->b_ModeSelectRegister =
-					(devpriv->
-					b_ModeSelectRegister &
-					APCI3120_DISABLE_EOC_INT) |
-					APCI3120_ENABLE_EOS_INT;
-				inw(devpriv->iobase);
-
-			} else
-				devpriv->b_ModeSelectRegister =
-					devpriv->
-					b_ModeSelectRegister &
-					APCI3120_DISABLE_ALL_INTERRUPT_WITHOUT_TIMER;
-
-			outb(devpriv->b_ModeSelectRegister,
-				devpriv->iobase + APCI3120_WRITE_MODE_SELECT);
-
-			inw(devpriv->iobase + APCI3120_RD_STATUS);
-
-			/* Sets gate 0 */
-			devpriv->us_OutputRegister =
-				devpriv->
-				us_OutputRegister | APCI3120_ENABLE_TIMER0;
-			outw(devpriv->us_OutputRegister,
-				devpriv->iobase + APCI3120_WR_ADDRESS);
-
-			/* Start conversion */
-			outw(0, devpriv->iobase + APCI3120_START_CONVERSION);
-
-			/* Waiting of end of conversion if interrupt is not installed */
-			if (devpriv->b_EocEosInterrupt == APCI3120_DISABLE) {
-				/* Waiting the end of conversion */
-				do {
-					us_TmpValue =
-						inw(devpriv->iobase +
-						APCI3120_RD_STATUS);
-				} while ((us_TmpValue & APCI3120_EOS) !=
-					 APCI3120_EOS);
-
-				for (i = 0; i < devpriv->ui_AiNbrofChannels;
-					i++) {
-					/* Read the result in FIFO and write them in shared memory */
-					us_TmpValue = inw(devpriv->iobase);
-					data[i] = (unsigned int) us_TmpValue;
-				}
-
-				devpriv->b_InterruptMode = APCI3120_EOC_MODE;	/*  Restore defaults */
-			}
-			break;
-
-		default:
-			dev_err(dev->class_dev, "inputs wrong\n");
-
-		}
-		devpriv->ui_EocEosConversionTime = 0;	/*  re initializing the variable */
-	}
-
-	return insn->n;
-
-}
-
-static int apci3120_reset(struct comedi_device *dev)
-{
-	struct addi_private *devpriv = dev->private;
-	unsigned int i;
-	unsigned short us_TmpValue;
-
-	devpriv->ai_running = 0;
-	devpriv->b_EocEosInterrupt = APCI3120_DISABLE;
-	devpriv->b_InterruptMode = APCI3120_EOC_MODE;
-	devpriv->ui_EocEosConversionTime = 0;	/*  set eoc eos conv time to 0 */
-
-	/*  variables used in timer subdevice */
-	devpriv->b_Timer2Mode = 0;
-	devpriv->b_Timer2Interrupt = 0;
-	devpriv->b_ExttrigEnable = 0;	/*  Disable ext trigger */
-
-	/* Disable all interrupts, watchdog for the anolog output */
-	devpriv->b_ModeSelectRegister = 0;
-	outb(devpriv->b_ModeSelectRegister,
-		dev->iobase + APCI3120_WRITE_MODE_SELECT);
-
-	/*  Disables all counters, ext trigger and clears PA, PR */
-	devpriv->us_OutputRegister = 0;
-	outw(devpriv->us_OutputRegister, dev->iobase + APCI3120_WR_ADDRESS);
-
-	/*
-	 * Code to set the all anolog o/p channel to 0v 8191 is decimal
-	 * value for zero(0 v)volt in bipolar mode(default)
-	 */
-	outw(8191 | APCI3120_ANALOG_OP_CHANNEL_1, dev->iobase + APCI3120_ANALOG_OUTPUT_1);	/* channel 1 */
-	outw(8191 | APCI3120_ANALOG_OP_CHANNEL_2, dev->iobase + APCI3120_ANALOG_OUTPUT_1);	/* channel 2 */
-	outw(8191 | APCI3120_ANALOG_OP_CHANNEL_3, dev->iobase + APCI3120_ANALOG_OUTPUT_1);	/* channel 3 */
-	outw(8191 | APCI3120_ANALOG_OP_CHANNEL_4, dev->iobase + APCI3120_ANALOG_OUTPUT_1);	/* channel 4 */
-
-	outw(8191 | APCI3120_ANALOG_OP_CHANNEL_5, dev->iobase + APCI3120_ANALOG_OUTPUT_2);	/* channel 5 */
-	outw(8191 | APCI3120_ANALOG_OP_CHANNEL_6, dev->iobase + APCI3120_ANALOG_OUTPUT_2);	/* channel 6 */
-	outw(8191 | APCI3120_ANALOG_OP_CHANNEL_7, dev->iobase + APCI3120_ANALOG_OUTPUT_2);	/* channel 7 */
-	outw(8191 | APCI3120_ANALOG_OP_CHANNEL_8, dev->iobase + APCI3120_ANALOG_OUTPUT_2);	/* channel 8 */
-
-	udelay(10);
-
-	inw(dev->iobase + 0);	/* make a dummy read */
-	inb(dev->iobase + APCI3120_RESET_FIFO);	/*  flush FIFO */
-	inw(dev->iobase + APCI3120_RD_STATUS);	/*  flush A/D status register */
-
-	/* code to reset the RAM sequence */
-	for (i = 0; i < 16; i++) {
-		us_TmpValue = i << 8;	/* select the location */
-		outw(us_TmpValue, dev->iobase + APCI3120_SEQ_RAM_ADDRESS);
-	}
-	return 0;
-}
-
-static int apci3120_exttrig_enable(struct comedi_device *dev)
-{
-	struct addi_private *devpriv = dev->private;
-
-	devpriv->us_OutputRegister |= APCI3120_ENABLE_EXT_TRIGGER;
-	outw(devpriv->us_OutputRegister, dev->iobase + APCI3120_WR_ADDRESS);
-	return 0;
-}
-
-static int apci3120_exttrig_disable(struct comedi_device *dev)
-{
-	struct addi_private *devpriv = dev->private;
-
-	devpriv->us_OutputRegister &= ~APCI3120_ENABLE_EXT_TRIGGER;
-	outw(devpriv->us_OutputRegister, dev->iobase + APCI3120_WR_ADDRESS);
-	return 0;
-}
-
-static int apci3120_cancel(struct comedi_device *dev,
-			   struct comedi_subdevice *s)
-{
-	struct addi_private *devpriv = dev->private;
-
-	/*  Disable A2P Fifo write and AMWEN signal */
-	outw(0, devpriv->i_IobaseAddon + 4);
-
-	/* Disable Bus Master ADD ON */
-	outw(APCI3120_ADD_ON_AGCSTS_LOW, devpriv->i_IobaseAddon + 0);
-	outw(0, devpriv->i_IobaseAddon + 2);
-	outw(APCI3120_ADD_ON_AGCSTS_HIGH, devpriv->i_IobaseAddon + 0);
-	outw(0, devpriv->i_IobaseAddon + 2);
-
-	/* Disable BUS Master PCI */
-	outl(0, devpriv->i_IobaseAmcc + AMCC_OP_REG_MCSR);
-
-	/* Disable ext trigger */
-	apci3120_exttrig_disable(dev);
-
-	devpriv->us_OutputRegister = 0;
-	/* stop  counters */
-	outw(devpriv->
-		us_OutputRegister & APCI3120_DISABLE_TIMER0 &
-		APCI3120_DISABLE_TIMER1, dev->iobase + APCI3120_WR_ADDRESS);
-
-	outw(APCI3120_DISABLE_ALL_TIMER, dev->iobase + APCI3120_WR_ADDRESS);
-
-	/* DISABLE_ALL_INTERRUPT */
-	outb(APCI3120_DISABLE_ALL_INTERRUPT,
-		dev->iobase + APCI3120_WRITE_MODE_SELECT);
-	/* Flush FIFO */
-	inb(dev->iobase + APCI3120_RESET_FIFO);
-	inw(dev->iobase + APCI3120_RD_STATUS);
-	devpriv->ui_AiActualScan = 0;
-	s->async->cur_chan = 0;
-	devpriv->ui_DmaActualBuffer = 0;
-
-	devpriv->ai_running = 0;
-	devpriv->b_InterruptMode = APCI3120_EOC_MODE;
-	devpriv->b_EocEosInterrupt = APCI3120_DISABLE;
-	apci3120_reset(dev);
-	return 0;
-}
-
-static int apci3120_ai_cmdtest(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_cmd *cmd)
-{
-	int err = 0;
-
-	/* Step 1 : check if triggers are trivially valid */
-
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
-					TRIG_TIMER | TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
-
-	if (err)
-		return 1;
-
-	/* Step 2a : make sure trigger sources are unique */
-
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
-
-	/* Step 2b : and mutually compatible */
-
-	if (err)
-		return 2;
-
-	/* Step 3: check if arguments are trivially valid */
-
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-
-	if (cmd->scan_begin_src == TRIG_TIMER)	/* Test Delay timing */
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg, 100000);
-
-	if (cmd->scan_begin_src == TRIG_TIMER) {
-		if (cmd->convert_arg)
-			err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-							 10000);
-	} else {
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg, 10000);
-	}
-
-	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
-
-	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
-	else	/*  TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-
-	if (err)
-		return 3;
-
-	/*  step 4: fix up any arguments */
-
-	if (cmd->scan_begin_src == TRIG_TIMER &&
-	    cmd->scan_begin_arg < cmd->convert_arg * cmd->scan_end_arg) {
-		cmd->scan_begin_arg = cmd->convert_arg * cmd->scan_end_arg;
-		err |= -EINVAL;
-	}
-
-	if (err)
-		return 4;
-
-	return 0;
-}
-
-/*
- * This is used for analog input cyclic acquisition.
- * Performs the command operations.
- * If DMA is configured does DMA initialization otherwise does the
- * acquisition with EOS interrupt.
- */
-static int apci3120_cyclic_ai(int mode,
-			      struct comedi_device *dev,
-			      struct comedi_subdevice *s)
-{
-	const struct addi_board *this_board = dev->board_ptr;
-	struct addi_private *devpriv = dev->private;
-	struct comedi_cmd *cmd = &s->async->cmd;
-	unsigned char b_Tmp;
-	unsigned int ui_Tmp, ui_DelayTiming = 0, ui_TimerValue1 = 0, dmalen0 =
-		0, dmalen1 = 0, ui_TimerValue2 =
-		0, ui_TimerValue0, ui_ConvertTiming;
-	unsigned short us_TmpValue;
-
-	/* Resets the FIFO */
-	inb(dev->iobase + APCI3120_RESET_FIFO);
-
-	devpriv->ai_running = 1;
-
-	/*  clear software  registers */
-	devpriv->b_TimerSelectMode = 0;
-	devpriv->us_OutputRegister = 0;
-	devpriv->b_ModeSelectRegister = 0;
-
-	/* Clear Timer Write TC int */
-	outl(APCI3120_CLEAR_WRITE_TC_INT,
-		devpriv->i_IobaseAmcc + APCI3120_AMCC_OP_REG_INTCSR);
-
-	/* Disables All Timer     */
-	/* Sets PR and PA to 0    */
-	devpriv->us_OutputRegister = devpriv->us_OutputRegister &
-		APCI3120_DISABLE_TIMER0 &
-		APCI3120_DISABLE_TIMER1 & APCI3120_CLEAR_PA_PR;
-
-	outw(devpriv->us_OutputRegister, dev->iobase + APCI3120_WR_ADDRESS);
-
-	/* Resets the FIFO */
-	/* BEGIN JK 07.05.04: Comparison between WIN32 and Linux driver */
-	inb(devpriv->iobase + APCI3120_RESET_FIFO);
-	/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
-
-	devpriv->ui_AiActualScan = 0;
-	s->async->cur_chan = 0;
-	devpriv->ui_DmaActualBuffer = 0;
-
-	/* value for timer2  minus -2 has to be done */
-	ui_TimerValue2 = cmd->stop_arg - 2;
-	ui_ConvertTiming = cmd->convert_arg;
-
-	if (mode == 2)
-		ui_DelayTiming = cmd->scan_begin_arg;
-
-	/* Initializes the sequence array */
-	if (!apci3120_setup_chan_list(dev, s, devpriv->ui_AiNbrofChannels,
-			cmd->chanlist, 0))
-		return -EINVAL;
-
-	us_TmpValue = (unsigned short) inw(dev->iobase + APCI3120_RD_STATUS);
-
-	/* EL241003 Begin: add this section to replace floats calculation by integer calculations */
-	/* EL250804: Testing if board APCI3120 have the new Quartz or if it is an APCI3001 */
-	if ((us_TmpValue & 0x00B0) == 0x00B0
-		|| !strcmp(this_board->pc_DriverName, "apci3001")) {
-		ui_TimerValue0 = ui_ConvertTiming * 2 - 2000;
-		ui_TimerValue0 = ui_TimerValue0 / 1000;
-
-		if (mode == 2) {
-			ui_DelayTiming = ui_DelayTiming / 1000;
-			ui_TimerValue1 = ui_DelayTiming * 2 - 200;
-			ui_TimerValue1 = ui_TimerValue1 / 100;
-		}
-	} else {
-		ui_ConvertTiming = ui_ConvertTiming / 1000;
-		ui_TimerValue0 = ui_ConvertTiming * 12926 - 10000;
-		ui_TimerValue0 = ui_TimerValue0 / 10000;
-
-		if (mode == 2) {
-			ui_DelayTiming = ui_DelayTiming / 1000;
-			ui_TimerValue1 = ui_DelayTiming * 12926 - 1;
-			ui_TimerValue1 = ui_TimerValue1 / 1000000;
-		}
-	}
-	/* EL241003 End */
-
-	if (devpriv->b_ExttrigEnable == APCI3120_ENABLE)
-		apci3120_exttrig_enable(dev);	/*  activate EXT trigger */
-	switch (mode) {
-	case 1:
-		/*  init timer0 in mode 2 */
-		devpriv->b_TimerSelectMode =
-			(devpriv->
-			b_TimerSelectMode & 0xFC) | APCI3120_TIMER_0_MODE_2;
-		outb(devpriv->b_TimerSelectMode,
-			dev->iobase + APCI3120_TIMER_CRT1);
-
-		/* Select Timer 0 */
-		b_Tmp = ((devpriv->
-				b_DigitalOutputRegister) & 0xF0) |
-			APCI3120_SELECT_TIMER_0_WORD;
-		outb(b_Tmp, dev->iobase + APCI3120_TIMER_CRT0);
-		/* Set the conversion time */
-		outw(((unsigned short) ui_TimerValue0),
-			dev->iobase + APCI3120_TIMER_VALUE);
-		break;
-
-	case 2:
-		/*  init timer1 in mode 2 */
-		devpriv->b_TimerSelectMode =
-			(devpriv->
-			b_TimerSelectMode & 0xF3) | APCI3120_TIMER_1_MODE_2;
-		outb(devpriv->b_TimerSelectMode,
-			dev->iobase + APCI3120_TIMER_CRT1);
-
-		/* Select Timer 1 */
-		b_Tmp = ((devpriv->
-				b_DigitalOutputRegister) & 0xF0) |
-			APCI3120_SELECT_TIMER_1_WORD;
-		outb(b_Tmp, dev->iobase + APCI3120_TIMER_CRT0);
-		/* Set the conversion time */
-		outw(((unsigned short) ui_TimerValue1),
-			dev->iobase + APCI3120_TIMER_VALUE);
-
-		/*  init timer0 in mode 2 */
-		devpriv->b_TimerSelectMode =
-			(devpriv->
-			b_TimerSelectMode & 0xFC) | APCI3120_TIMER_0_MODE_2;
-		outb(devpriv->b_TimerSelectMode,
-			dev->iobase + APCI3120_TIMER_CRT1);
-
-		/* Select Timer 0 */
-		b_Tmp = ((devpriv->
-				b_DigitalOutputRegister) & 0xF0) |
-			APCI3120_SELECT_TIMER_0_WORD;
-		outb(b_Tmp, dev->iobase + APCI3120_TIMER_CRT0);
-
-		/* Set the conversion time */
-		outw(((unsigned short) ui_TimerValue0),
-			dev->iobase + APCI3120_TIMER_VALUE);
-		break;
-
-	}
-	/* common for all modes */
-	/* BEGIN JK 07.05.04: Comparison between WIN32 and Linux driver */
-	devpriv->b_ModeSelectRegister = devpriv->b_ModeSelectRegister &
-		APCI3120_DISABLE_SCAN;
-	/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
-
-	outb(devpriv->b_ModeSelectRegister,
-		dev->iobase + APCI3120_WRITE_MODE_SELECT);
-
-	/*  If DMA is disabled */
-	if (devpriv->us_UseDma == APCI3120_DISABLE) {
-		/*  disable EOC and enable EOS */
-		devpriv->b_InterruptMode = APCI3120_EOS_MODE;
-		devpriv->b_EocEosInterrupt = APCI3120_ENABLE;
-
-		devpriv->b_ModeSelectRegister =
-			(devpriv->
-			b_ModeSelectRegister & APCI3120_DISABLE_EOC_INT) |
-			APCI3120_ENABLE_EOS_INT;
-		outb(devpriv->b_ModeSelectRegister,
-			dev->iobase + APCI3120_WRITE_MODE_SELECT);
-
-		if (cmd->stop_src == TRIG_COUNT) {
-			/*
-			 * configure Timer2 For counting EOS Reset gate 2 of Timer 2 to
-			 * disable it (Set Bit D14 to 0)
-			 */
-			devpriv->us_OutputRegister =
-				devpriv->
-				us_OutputRegister & APCI3120_DISABLE_TIMER2;
-			outw(devpriv->us_OutputRegister,
-				dev->iobase + APCI3120_WR_ADDRESS);
-
-			/*  DISABLE TIMER intERRUPT */
-			devpriv->b_ModeSelectRegister =
-				devpriv->
-				b_ModeSelectRegister &
-				APCI3120_DISABLE_TIMER_INT & 0xEF;
-			outb(devpriv->b_ModeSelectRegister,
-				dev->iobase + APCI3120_WRITE_MODE_SELECT);
-
-			/* (1) Init timer 2 in mode 0 and write timer value */
-			devpriv->b_TimerSelectMode =
-				(devpriv->
-				b_TimerSelectMode & 0x0F) |
-				APCI3120_TIMER_2_MODE_0;
-			outb(devpriv->b_TimerSelectMode,
-				dev->iobase + APCI3120_TIMER_CRT1);
-
-			/* Writing LOW unsigned short */
-			b_Tmp = ((devpriv->
-					b_DigitalOutputRegister) & 0xF0) |
-				APCI3120_SELECT_TIMER_2_LOW_WORD;
-			outb(b_Tmp, dev->iobase + APCI3120_TIMER_CRT0);
-			outw(ui_TimerValue2 & 0xffff,
-				dev->iobase + APCI3120_TIMER_VALUE);
-
-			/* Writing HIGH unsigned short */
-			b_Tmp = ((devpriv->
-					b_DigitalOutputRegister) & 0xF0) |
-				APCI3120_SELECT_TIMER_2_HIGH_WORD;
-			outb(b_Tmp, dev->iobase + APCI3120_TIMER_CRT0);
-			outw((ui_TimerValue2 >> 16) & 0xffff,
-				dev->iobase + APCI3120_TIMER_VALUE);
-
-			/* (2) Reset FC_TIMER BIT  Clearing timer status register */
-			inb(dev->iobase + APCI3120_TIMER_STATUS_REGISTER);
-			/*  enable timer counter and disable watch dog */
-			devpriv->b_ModeSelectRegister =
-				(devpriv->
-				b_ModeSelectRegister |
-				APCI3120_ENABLE_TIMER_COUNTER) &
-				APCI3120_DISABLE_WATCHDOG;
-			/*  select EOS clock input for timer 2 */
-			devpriv->b_ModeSelectRegister =
-				devpriv->
-				b_ModeSelectRegister |
-				APCI3120_TIMER2_SELECT_EOS;
-			/*  Enable timer2  interrupt */
-			devpriv->b_ModeSelectRegister =
-				devpriv->
-				b_ModeSelectRegister |
-				APCI3120_ENABLE_TIMER_INT;
-			outb(devpriv->b_ModeSelectRegister,
-				dev->iobase + APCI3120_WRITE_MODE_SELECT);
-			devpriv->b_Timer2Mode = APCI3120_COUNTER;
-			devpriv->b_Timer2Interrupt = APCI3120_ENABLE;
-		}
-	} else {
-		/* If DMA Enabled */
-		unsigned int scan_bytes = cmd->scan_end_arg * sizeof(short);
-
-		devpriv->b_InterruptMode = APCI3120_DMA_MODE;
-
-		/* Disables the EOC, EOS interrupt  */
-		devpriv->b_ModeSelectRegister = devpriv->b_ModeSelectRegister &
-			APCI3120_DISABLE_EOC_INT & APCI3120_DISABLE_EOS_INT;
-
-		outb(devpriv->b_ModeSelectRegister,
-			dev->iobase + APCI3120_WRITE_MODE_SELECT);
-
-		dmalen0 = devpriv->ui_DmaBufferSize[0];
-		dmalen1 = devpriv->ui_DmaBufferSize[1];
-
-		if (cmd->stop_src == TRIG_COUNT) {
-			/*
-			 * Must we fill full first buffer? And must we fill
-			 * full second buffer when first is once filled?
-			 */
-			if (dmalen0 > (cmd->stop_arg * scan_bytes)) {
-				dmalen0 = cmd->stop_arg * scan_bytes;
-			} else if (dmalen1 > (cmd->stop_arg * scan_bytes -
-					      dmalen0))
-				dmalen1 = cmd->stop_arg * scan_bytes -
-					  dmalen0;
-		}
-
-		if (cmd->flags & CMDF_WAKE_EOS) {
-			/*  don't we want wake up every scan? */
-			if (dmalen0 > scan_bytes) {
-				dmalen0 = scan_bytes;
-				if (cmd->scan_end_arg & 1)
-					dmalen0 += 2;
-			}
-			if (dmalen1 > scan_bytes) {
-				dmalen1 = scan_bytes;
-				if (cmd->scan_end_arg & 1)
-					dmalen1 -= 2;
-				if (dmalen1 < 4)
-					dmalen1 = 4;
-			}
-		} else {	/*  isn't output buff smaller that our DMA buff? */
-			if (dmalen0 > s->async->prealloc_bufsz)
-				dmalen0 = s->async->prealloc_bufsz;
-			if (dmalen1 > s->async->prealloc_bufsz)
-				dmalen1 = s->async->prealloc_bufsz;
-		}
-		devpriv->ui_DmaBufferUsesize[0] = dmalen0;
-		devpriv->ui_DmaBufferUsesize[1] = dmalen1;
-
-		/* Initialize DMA */
-
-		/*
-		 * Set Transfer count enable bit and A2P_fifo reset bit in AGCSTS
-		 * register 1
-		 */
-		ui_Tmp = AGCSTS_TC_ENABLE | AGCSTS_RESET_A2P_FIFO;
-		outl(ui_Tmp, devpriv->i_IobaseAmcc + AMCC_OP_REG_AGCSTS);
-
-		/*  changed  since 16 bit interface for add on */
-		/* ENABLE BUS MASTER */
-		outw(APCI3120_ADD_ON_AGCSTS_LOW, devpriv->i_IobaseAddon + 0);
-		outw(APCI3120_ENABLE_TRANSFER_ADD_ON_LOW,
-			devpriv->i_IobaseAddon + 2);
-
-		outw(APCI3120_ADD_ON_AGCSTS_HIGH, devpriv->i_IobaseAddon + 0);
-		outw(APCI3120_ENABLE_TRANSFER_ADD_ON_HIGH,
-			devpriv->i_IobaseAddon + 2);
-
-		/*
-		 * TO VERIFIED BEGIN JK 07.05.04: Comparison between WIN32 and Linux
-		 * driver
-		 */
-		outw(0x1000, devpriv->i_IobaseAddon + 2);
-		/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
-
-		/* 2 No change */
-		/* A2P FIFO MANAGEMENT */
-		/* A2P fifo reset & transfer control enable */
-		outl(APCI3120_A2P_FIFO_MANAGEMENT, devpriv->i_IobaseAmcc +
-			APCI3120_AMCC_OP_MCSR);
-
-		/*
-		 * 3
-		 * beginning address of dma buf The 32 bit address of dma buffer
-		 * is converted into two 16 bit addresses Can done by using _attach
-		 * and put into into an array array used may be for differnet pages
-		 */
-
-		/*  DMA Start Address Low */
-		outw(APCI3120_ADD_ON_MWAR_LOW, devpriv->i_IobaseAddon + 0);
-		outw((devpriv->ul_DmaBufferHw[0] & 0xFFFF),
-			devpriv->i_IobaseAddon + 2);
-
-		/* DMA Start Address High */
-		outw(APCI3120_ADD_ON_MWAR_HIGH, devpriv->i_IobaseAddon + 0);
-		outw((devpriv->ul_DmaBufferHw[0] / 65536),
-			devpriv->i_IobaseAddon + 2);
-
-		/*
-		 * 4
-		 * amount of bytes to be transferred set transfer count used ADDON
-		 * MWTC register commented testing
-		 */
-
-		/* Nbr of acquisition LOW */
-		outw(APCI3120_ADD_ON_MWTC_LOW, devpriv->i_IobaseAddon + 0);
-		outw((devpriv->ui_DmaBufferUsesize[0] & 0xFFFF),
-			devpriv->i_IobaseAddon + 2);
-
-		/* Nbr of acquisition HIGH */
-		outw(APCI3120_ADD_ON_MWTC_HIGH, devpriv->i_IobaseAddon + 0);
-		outw((devpriv->ui_DmaBufferUsesize[0] / 65536),
-			devpriv->i_IobaseAddon + 2);
-
-		/*
-		 * 5
-		 * To configure A2P FIFO testing outl(
-		 * FIFO_ADVANCE_ON_BYTE_2,devpriv->i_IobaseAmcc+AMCC_OP_REG_INTCSR);
-		 */
-
-		/* A2P FIFO RESET */
-		/*
-		 * TO VERIFY BEGIN JK 07.05.04: Comparison between WIN32 and Linux
-		 * driver
-		 */
-		outl(0x04000000UL, devpriv->i_IobaseAmcc + AMCC_OP_REG_MCSR);
-		/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
-
-		/*
-		 * 6
-		 * ENABLE A2P FIFO WRITE AND ENABLE AMWEN AMWEN_ENABLE |
-		 * A2P_FIFO_WRITE_ENABLE (0x01|0x02)=0x03
-		 */
-
-		/*
-		 * 7
-		 * initialise end of dma interrupt AINT_WRITE_COMPL =
-		 * ENABLE_WRITE_TC_INT(ADDI)
-		 */
-		/* A2P FIFO CONFIGURATE, END OF DMA intERRUPT INIT */
-		outl((APCI3120_FIFO_ADVANCE_ON_BYTE_2 |
-				APCI3120_ENABLE_WRITE_TC_INT),
-			devpriv->i_IobaseAmcc + AMCC_OP_REG_INTCSR);
-
-		/* BEGIN JK 07.05.04: Comparison between WIN32 and Linux driver */
-		/* ENABLE A2P FIFO WRITE AND ENABLE AMWEN */
-		outw(3, devpriv->i_IobaseAddon + 4);
-		/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
-
-		/* A2P FIFO RESET */
-		/* BEGIN JK 07.05.04: Comparison between WIN32 and Linux driver */
-		outl(0x04000000UL,
-			devpriv->i_IobaseAmcc + APCI3120_AMCC_OP_MCSR);
-		/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
-	}
-
-	if (devpriv->us_UseDma == APCI3120_DISABLE &&
-	    cmd->stop_src == TRIG_COUNT) {
-		/*  set gate 2   to start conversion */
-		devpriv->us_OutputRegister =
-			devpriv->us_OutputRegister | APCI3120_ENABLE_TIMER2;
-		outw(devpriv->us_OutputRegister,
-			dev->iobase + APCI3120_WR_ADDRESS);
-	}
-
-	switch (mode) {
-	case 1:
-		/*  set gate 0   to start conversion */
-		devpriv->us_OutputRegister =
-			devpriv->us_OutputRegister | APCI3120_ENABLE_TIMER0;
-		outw(devpriv->us_OutputRegister,
-			dev->iobase + APCI3120_WR_ADDRESS);
-		break;
-	case 2:
-		/*  set  gate 0 and gate 1 */
-		devpriv->us_OutputRegister =
-			devpriv->us_OutputRegister | APCI3120_ENABLE_TIMER1;
-		devpriv->us_OutputRegister =
-			devpriv->us_OutputRegister | APCI3120_ENABLE_TIMER0;
-		outw(devpriv->us_OutputRegister,
-			dev->iobase + APCI3120_WR_ADDRESS);
-		break;
-
-	}
-
-	return 0;
-
-}
-
-/*
- * Does asynchronous acquisition.
- * Determines the mode 1 or 2.
- */
-static int apci3120_ai_cmd(struct comedi_device *dev,
-			   struct comedi_subdevice *s)
-{
-	struct addi_private *devpriv = dev->private;
-	struct comedi_cmd *cmd = &s->async->cmd;
-
-	/* loading private structure with cmd structure inputs */
-	devpriv->ui_AiNbrofChannels = cmd->chanlist_len;
-
-	if (cmd->start_src == TRIG_EXT)
-		devpriv->b_ExttrigEnable = APCI3120_ENABLE;
-	else
-		devpriv->b_ExttrigEnable = APCI3120_DISABLE;
-
-	if (cmd->scan_begin_src == TRIG_FOLLOW)
-		return apci3120_cyclic_ai(1, dev, s);
-	/* TRIG_TIMER */
-	return apci3120_cyclic_ai(2, dev, s);
-}
-
-/*
- * This function copies the data from DMA buffer to the Comedi buffer.
- */
-static void v_APCI3120_InterruptDmaMoveBlock16bit(struct comedi_device *dev,
-						  struct comedi_subdevice *s,
-						  unsigned short *dma_buffer,
-						  unsigned int num_samples)
-{
-	struct addi_private *devpriv = dev->private;
-	struct comedi_cmd *cmd = &s->async->cmd;
-
-	devpriv->ui_AiActualScan +=
-		(s->async->cur_chan + num_samples) / cmd->scan_end_arg;
-	s->async->cur_chan += num_samples;
-	s->async->cur_chan %= cmd->scan_end_arg;
-
-	cfc_write_array_to_buffer(s, dma_buffer, num_samples * sizeof(short));
-}
-
-/*
- * This is a handler for the DMA interrupt.
- * This function copies the data to Comedi Buffer.
- * For continuous DMA it reinitializes the DMA operation.
- * For single mode DMA it stop the acquisition.
- */
-static void apci3120_interrupt_dma(int irq, void *d)
-{
-	struct comedi_device *dev = d;
-	struct addi_private *devpriv = dev->private;
-	struct comedi_subdevice *s = dev->read_subdev;
-	struct comedi_cmd *cmd = &s->async->cmd;
-	unsigned int next_dma_buf, samplesinbuf;
-	unsigned long low_word, high_word, var;
-	unsigned int ui_Tmp;
-
-	samplesinbuf =
-		devpriv->ui_DmaBufferUsesize[devpriv->ui_DmaActualBuffer] -
-		inl(devpriv->i_IobaseAmcc + AMCC_OP_REG_MWTC);
-
-	if (samplesinbuf <
-		devpriv->ui_DmaBufferUsesize[devpriv->ui_DmaActualBuffer]) {
-		dev_err(dev->class_dev, "Interrupted DMA transfer!\n");
-	}
-	if (samplesinbuf & 1) {
-		dev_err(dev->class_dev, "Odd count of bytes in DMA ring!\n");
-		apci3120_cancel(dev, s);
-		return;
-	}
-	samplesinbuf = samplesinbuf >> 1;	/*  number of received samples */
-	if (devpriv->b_DmaDoubleBuffer) {
-		/*  switch DMA buffers if is used double buffering */
-		next_dma_buf = 1 - devpriv->ui_DmaActualBuffer;
-
-		ui_Tmp = AGCSTS_TC_ENABLE | AGCSTS_RESET_A2P_FIFO;
-		outl(ui_Tmp, devpriv->i_IobaseAddon + AMCC_OP_REG_AGCSTS);
-
-		/*  changed  since 16 bit interface for add on */
-		outw(APCI3120_ADD_ON_AGCSTS_LOW, devpriv->i_IobaseAddon + 0);
-		outw(APCI3120_ENABLE_TRANSFER_ADD_ON_LOW,
-			devpriv->i_IobaseAddon + 2);
-		outw(APCI3120_ADD_ON_AGCSTS_HIGH, devpriv->i_IobaseAddon + 0);
-		outw(APCI3120_ENABLE_TRANSFER_ADD_ON_HIGH, devpriv->i_IobaseAddon + 2);	/*  0x1000 is out putted in windows driver */
-
-		var = devpriv->ul_DmaBufferHw[next_dma_buf];
-		low_word = var & 0xffff;
-		var = devpriv->ul_DmaBufferHw[next_dma_buf];
-		high_word = var / 65536;
-
-		/* DMA Start Address Low */
-		outw(APCI3120_ADD_ON_MWAR_LOW, devpriv->i_IobaseAddon + 0);
-		outw(low_word, devpriv->i_IobaseAddon + 2);
-
-		/* DMA Start Address High */
-		outw(APCI3120_ADD_ON_MWAR_HIGH, devpriv->i_IobaseAddon + 0);
-		outw(high_word, devpriv->i_IobaseAddon + 2);
-
-		var = devpriv->ui_DmaBufferUsesize[next_dma_buf];
-		low_word = var & 0xffff;
-		var = devpriv->ui_DmaBufferUsesize[next_dma_buf];
-		high_word = var / 65536;
-
-		/* Nbr of acquisition LOW */
-		outw(APCI3120_ADD_ON_MWTC_LOW, devpriv->i_IobaseAddon + 0);
-		outw(low_word, devpriv->i_IobaseAddon + 2);
-
-		/* Nbr of acquisition HIGH */
-		outw(APCI3120_ADD_ON_MWTC_HIGH, devpriv->i_IobaseAddon + 0);
-		outw(high_word, devpriv->i_IobaseAddon + 2);
-
-		/*
-		 * To configure A2P FIFO
-		 * ENABLE A2P FIFO WRITE AND ENABLE AMWEN
-		 * AMWEN_ENABLE | A2P_FIFO_WRITE_ENABLE (0x01|0x02)=0x03
-		 */
-		outw(3, devpriv->i_IobaseAddon + 4);
-		/* initialise end of dma interrupt  AINT_WRITE_COMPL = ENABLE_WRITE_TC_INT(ADDI) */
-		outl((APCI3120_FIFO_ADVANCE_ON_BYTE_2 |
-				APCI3120_ENABLE_WRITE_TC_INT),
-			devpriv->i_IobaseAmcc + AMCC_OP_REG_INTCSR);
-
-	}
-	if (samplesinbuf) {
-		v_APCI3120_InterruptDmaMoveBlock16bit(dev, s,
-			devpriv->ul_DmaBufferVirtual[devpriv->
-				ui_DmaActualBuffer], samplesinbuf);
-
-		if (!(cmd->flags & CMDF_WAKE_EOS)) {
-			s->async->events |= COMEDI_CB_EOS;
-			comedi_event(dev, s);
-		}
-	}
-	if (cmd->stop_src == TRIG_COUNT)
-		if (devpriv->ui_AiActualScan >= cmd->stop_arg) {
-			/*  all data sampled */
-			apci3120_cancel(dev, s);
-			s->async->events |= COMEDI_CB_EOA;
-			comedi_event(dev, s);
-			return;
-		}
-
-	if (devpriv->b_DmaDoubleBuffer) {	/*  switch dma buffers */
-		devpriv->ui_DmaActualBuffer = 1 - devpriv->ui_DmaActualBuffer;
-	} else {
-		/*
-		 * restart DMA if is not used double buffering
-		 * ADDED REINITIALISE THE DMA
-		 */
-		ui_Tmp = AGCSTS_TC_ENABLE | AGCSTS_RESET_A2P_FIFO;
-		outl(ui_Tmp, devpriv->i_IobaseAddon + AMCC_OP_REG_AGCSTS);
-
-		/*  changed  since 16 bit interface for add on */
-		outw(APCI3120_ADD_ON_AGCSTS_LOW, devpriv->i_IobaseAddon + 0);
-		outw(APCI3120_ENABLE_TRANSFER_ADD_ON_LOW,
-			devpriv->i_IobaseAddon + 2);
-		outw(APCI3120_ADD_ON_AGCSTS_HIGH, devpriv->i_IobaseAddon + 0);
-		outw(APCI3120_ENABLE_TRANSFER_ADD_ON_HIGH, devpriv->i_IobaseAddon + 2);
-		/*
-		 * A2P FIFO MANAGEMENT
-		 * A2P fifo reset & transfer control enable
-		 */
-		outl(APCI3120_A2P_FIFO_MANAGEMENT,
-			devpriv->i_IobaseAmcc + AMCC_OP_REG_MCSR);
-
-		var = devpriv->ul_DmaBufferHw[0];
-		low_word = var & 0xffff;
-		var = devpriv->ul_DmaBufferHw[0];
-		high_word = var / 65536;
-		outw(APCI3120_ADD_ON_MWAR_LOW, devpriv->i_IobaseAddon + 0);
-		outw(low_word, devpriv->i_IobaseAddon + 2);
-		outw(APCI3120_ADD_ON_MWAR_HIGH, devpriv->i_IobaseAddon + 0);
-		outw(high_word, devpriv->i_IobaseAddon + 2);
-
-		var = devpriv->ui_DmaBufferUsesize[0];
-		low_word = var & 0xffff;	/* changed */
-		var = devpriv->ui_DmaBufferUsesize[0];
-		high_word = var / 65536;
-		outw(APCI3120_ADD_ON_MWTC_LOW, devpriv->i_IobaseAddon + 0);
-		outw(low_word, devpriv->i_IobaseAddon + 2);
-		outw(APCI3120_ADD_ON_MWTC_HIGH, devpriv->i_IobaseAddon + 0);
-		outw(high_word, devpriv->i_IobaseAddon + 2);
-
-		/*
-		 * To configure A2P FIFO
-		 * ENABLE A2P FIFO WRITE AND ENABLE AMWEN
-		 * AMWEN_ENABLE | A2P_FIFO_WRITE_ENABLE (0x01|0x02)=0x03
-		 */
-		outw(3, devpriv->i_IobaseAddon + 4);
-		/* initialise end of dma interrupt  AINT_WRITE_COMPL = ENABLE_WRITE_TC_INT(ADDI) */
-		outl((APCI3120_FIFO_ADVANCE_ON_BYTE_2 |
-				APCI3120_ENABLE_WRITE_TC_INT),
-			devpriv->i_IobaseAmcc + AMCC_OP_REG_INTCSR);
-	}
-}
-
-/*
- * This function handles EOS interrupt.
- * This function copies the acquired data(from FIFO) to Comedi buffer.
- */
-static int apci3120_interrupt_handle_eos(struct comedi_device *dev)
-{
-	struct addi_private *devpriv = dev->private;
-	struct comedi_subdevice *s = dev->read_subdev;
-	int n_chan, i;
-	int err = 1;
-
-	n_chan = devpriv->ui_AiNbrofChannels;
-
-	for (i = 0; i < n_chan; i++)
-		err &= comedi_buf_put(s, inw(dev->iobase + 0));
-
-	s->async->events |= COMEDI_CB_EOS;
-
-	if (err == 0)
-		s->async->events |= COMEDI_CB_OVERFLOW;
-
-	comedi_event(dev, s);
-
-	return 0;
-}
-
-static void apci3120_interrupt(int irq, void *d)
-{
-	struct comedi_device *dev = d;
-	struct addi_private *devpriv = dev->private;
-	struct comedi_subdevice *s = dev->read_subdev;
-	unsigned short int_daq;
-	unsigned int int_amcc, ui_Check, i;
-	unsigned short us_TmpValue;
-	unsigned char b_DummyRead;
-
-	ui_Check = 1;
-
-	int_daq = inw(dev->iobase + APCI3120_RD_STATUS) & 0xf000;	/*  get IRQ reasons */
-	int_amcc = inl(devpriv->i_IobaseAmcc + AMCC_OP_REG_INTCSR);	/*  get AMCC int register */
-
-	if ((!int_daq) && (!(int_amcc & ANY_S593X_INT))) {
-		dev_err(dev->class_dev, "IRQ from unknown source\n");
-		return;
-	}
-
-	outl(int_amcc | 0x00ff0000, devpriv->i_IobaseAmcc + AMCC_OP_REG_INTCSR);	/*  shutdown IRQ reasons in AMCC */
-
-	int_daq = (int_daq >> 12) & 0xF;
-
-	if (devpriv->b_ExttrigEnable == APCI3120_ENABLE) {
-		/* Disable ext trigger */
-		apci3120_exttrig_disable(dev);
-		devpriv->b_ExttrigEnable = APCI3120_DISABLE;
-	}
-	/* clear the timer 2 interrupt */
-	inb(devpriv->i_IobaseAmcc + APCI3120_TIMER_STATUS_REGISTER);
-
-	if (int_amcc & MASTER_ABORT_INT)
-		dev_err(dev->class_dev, "AMCC IRQ - MASTER DMA ABORT!\n");
-	if (int_amcc & TARGET_ABORT_INT)
-		dev_err(dev->class_dev, "AMCC IRQ - TARGET DMA ABORT!\n");
-
-	/*  Ckeck if EOC interrupt */
-	if (((int_daq & 0x8) == 0)
-		&& (devpriv->b_InterruptMode == APCI3120_EOC_MODE)) {
-		if (devpriv->b_EocEosInterrupt == APCI3120_ENABLE) {
-
-			/*  Read the AI Value */
-			devpriv->ui_AiReadData[0] =
-				(unsigned int) inw(devpriv->iobase + 0);
-			devpriv->b_EocEosInterrupt = APCI3120_DISABLE;
-			send_sig(SIGIO, devpriv->tsk_Current, 0);	/*  send signal to the sample */
-		} else {
-			/* Disable EOC Interrupt */
-			devpriv->b_ModeSelectRegister =
-				devpriv->
-				b_ModeSelectRegister & APCI3120_DISABLE_EOC_INT;
-			outb(devpriv->b_ModeSelectRegister,
-				devpriv->iobase + APCI3120_WRITE_MODE_SELECT);
-
-		}
-	}
-
-	/*  Check If EOS interrupt */
-	if ((int_daq & 0x2) && (devpriv->b_InterruptMode == APCI3120_EOS_MODE)) {
-
-		if (devpriv->b_EocEosInterrupt == APCI3120_ENABLE) {	/*  enable this in without DMA ??? */
-
-			if (devpriv->ai_running) {
-				ui_Check = 0;
-				apci3120_interrupt_handle_eos(dev);
-				devpriv->ui_AiActualScan++;
-				devpriv->b_ModeSelectRegister =
-					devpriv->
-					b_ModeSelectRegister |
-					APCI3120_ENABLE_EOS_INT;
-				outb(devpriv->b_ModeSelectRegister,
-					dev->iobase +
-					APCI3120_WRITE_MODE_SELECT);
-			} else {
-				ui_Check = 0;
-				for (i = 0; i < devpriv->ui_AiNbrofChannels;
-					i++) {
-					us_TmpValue = inw(devpriv->iobase + 0);
-					devpriv->ui_AiReadData[i] =
-						(unsigned int) us_TmpValue;
-				}
-				devpriv->b_EocEosInterrupt = APCI3120_DISABLE;
-				devpriv->b_InterruptMode = APCI3120_EOC_MODE;
-
-				send_sig(SIGIO, devpriv->tsk_Current, 0);	/*  send signal to the sample */
-
-			}
-
-		} else {
-			devpriv->b_ModeSelectRegister =
-				devpriv->
-				b_ModeSelectRegister & APCI3120_DISABLE_EOS_INT;
-			outb(devpriv->b_ModeSelectRegister,
-				dev->iobase + APCI3120_WRITE_MODE_SELECT);
-			devpriv->b_EocEosInterrupt = APCI3120_DISABLE;	/* Default settings */
-			devpriv->b_InterruptMode = APCI3120_EOC_MODE;
-		}
-
-	}
-	/* Timer2 interrupt */
-	if (int_daq & 0x1) {
-
-		switch (devpriv->b_Timer2Mode) {
-		case APCI3120_COUNTER:
-			devpriv->b_ModeSelectRegister =
-				devpriv->
-				b_ModeSelectRegister & APCI3120_DISABLE_EOS_INT;
-			outb(devpriv->b_ModeSelectRegister,
-				dev->iobase + APCI3120_WRITE_MODE_SELECT);
-
-			/*  stop timer 2 */
-			devpriv->us_OutputRegister =
-				devpriv->
-				us_OutputRegister & APCI3120_DISABLE_ALL_TIMER;
-			outw(devpriv->us_OutputRegister,
-				dev->iobase + APCI3120_WR_ADDRESS);
-
-			/* stop timer 0 and timer 1 */
-			apci3120_cancel(dev, s);
-
-			/* UPDATE-0.7.57->0.7.68comedi_done(dev,s); */
-			s->async->events |= COMEDI_CB_EOA;
-			comedi_event(dev, s);
-
-			break;
-
-		case APCI3120_TIMER:
-
-			/* Send a signal to from kernel to user space */
-			send_sig(SIGIO, devpriv->tsk_Current, 0);
-			break;
-
-		case APCI3120_WATCHDOG:
-
-			/* Send a signal to from kernel to user space */
-			send_sig(SIGIO, devpriv->tsk_Current, 0);
-			break;
-
-		default:
-
-			/*  disable Timer Interrupt */
-			devpriv->b_ModeSelectRegister =
-				devpriv->
-				b_ModeSelectRegister &
-				APCI3120_DISABLE_TIMER_INT;
-
-			outb(devpriv->b_ModeSelectRegister,
-				dev->iobase + APCI3120_WRITE_MODE_SELECT);
-
-		}
-
-		b_DummyRead = inb(dev->iobase + APCI3120_TIMER_STATUS_REGISTER);
-
-	}
-
-	if ((int_daq & 0x4) && (devpriv->b_InterruptMode == APCI3120_DMA_MODE)) {
-		if (devpriv->ai_running) {
-
-			/* Clear Timer Write TC int */
-			outl(APCI3120_CLEAR_WRITE_TC_INT,
-				devpriv->i_IobaseAmcc +
-				APCI3120_AMCC_OP_REG_INTCSR);
-
-			/* Clears the timer status register */
-			inw(dev->iobase + APCI3120_TIMER_STATUS_REGISTER);
-			/* do some data transfer */
-			apci3120_interrupt_dma(irq, d);
-		} else {
-			/* Stops the Timer */
-			outw(devpriv->
-				us_OutputRegister & APCI3120_DISABLE_TIMER0 &
-				APCI3120_DISABLE_TIMER1,
-				dev->iobase + APCI3120_WR_ADDRESS);
-		}
-
-	}
-}
-
-/*
- * Configure Timer 2
- *
- * data[0] = TIMER configure as timer
- *	   = WATCHDOG configure as watchdog
- * data[1] = Timer constant
- * data[2] = Timer2 interrupt (1)enable or(0) disable
- */
-static int apci3120_config_insn_timer(struct comedi_device *dev,
-				      struct comedi_subdevice *s,
-				      struct comedi_insn *insn,
-				      unsigned int *data)
-{
-	const struct addi_board *this_board = dev->board_ptr;
-	struct addi_private *devpriv = dev->private;
-	unsigned int ui_Timervalue2;
-	unsigned short us_TmpValue;
-	unsigned char b_Tmp;
-
-	if (!data[1])
-		dev_err(dev->class_dev, "No timer constant!\n");
-
-	devpriv->b_Timer2Interrupt = (unsigned char) data[2];	/*  save info whether to enable or disable interrupt */
-
-	ui_Timervalue2 = data[1] / 1000;	/*  convert nano seconds  to u seconds */
-
-	us_TmpValue = (unsigned short) inw(devpriv->iobase + APCI3120_RD_STATUS);
-
-	/*
-	 * EL250804: Testing if board APCI3120 have the new Quartz or if it
-	 * is an APCI3001 and calculate the time value to set in the timer
-	 */
-	if ((us_TmpValue & 0x00B0) == 0x00B0
-		|| !strcmp(this_board->pc_DriverName, "apci3001")) {
-		/* Calculate the time value to set in the timer */
-		ui_Timervalue2 = ui_Timervalue2 / 50;
-	} else {
-		/* Calculate the time value to set in the timer */
-		ui_Timervalue2 = ui_Timervalue2 / 70;
-	}
-
-	/* Reset gate 2 of Timer 2 to disable it (Set Bit D14 to 0) */
-	devpriv->us_OutputRegister =
-		devpriv->us_OutputRegister & APCI3120_DISABLE_TIMER2;
-	outw(devpriv->us_OutputRegister, devpriv->iobase + APCI3120_WR_ADDRESS);
-
-	/*  Disable TIMER Interrupt */
-	devpriv->b_ModeSelectRegister =
-		devpriv->
-		b_ModeSelectRegister & APCI3120_DISABLE_TIMER_INT & 0xEF;
-
-	/*  Disable Eoc and Eos Interrupts */
-	devpriv->b_ModeSelectRegister =
-		devpriv->
-		b_ModeSelectRegister & APCI3120_DISABLE_EOC_INT &
-		APCI3120_DISABLE_EOS_INT;
-	outb(devpriv->b_ModeSelectRegister,
-		devpriv->iobase + APCI3120_WRITE_MODE_SELECT);
-	if (data[0] == APCI3120_TIMER) {	/* initialize timer */
-		/* Set the Timer 2 in mode 2(Timer) */
-		devpriv->b_TimerSelectMode =
-			(devpriv->
-			b_TimerSelectMode & 0x0F) | APCI3120_TIMER_2_MODE_2;
-		outb(devpriv->b_TimerSelectMode,
-			devpriv->iobase + APCI3120_TIMER_CRT1);
-
-		/*
-		 * Configure the timer 2 for writing the LOW unsigned short of timer
-		 * is Delay value You must make a b_tmp variable with
-		 * DigitalOutPutRegister because at Address_1+APCI3120_TIMER_CRT0
-		 * you can set the digital output and configure the timer 2,and if
-		 * you don't make this, digital output are erase (Set to 0)
-		 */
-
-		/* Writing LOW unsigned short */
-		b_Tmp = ((devpriv->
-				b_DigitalOutputRegister) & 0xF0) |
-			APCI3120_SELECT_TIMER_2_LOW_WORD;
-		outb(b_Tmp, devpriv->iobase + APCI3120_TIMER_CRT0);
-		outw(ui_Timervalue2 & 0xffff,
-			devpriv->iobase + APCI3120_TIMER_VALUE);
-
-		/* Writing HIGH unsigned short */
-		b_Tmp = ((devpriv->
-				b_DigitalOutputRegister) & 0xF0) |
-			APCI3120_SELECT_TIMER_2_HIGH_WORD;
-		outb(b_Tmp, devpriv->iobase + APCI3120_TIMER_CRT0);
-		outw((ui_Timervalue2 >> 16) & 0xffff,
-			devpriv->iobase + APCI3120_TIMER_VALUE);
-		/*  timer2 in Timer mode enabled */
-		devpriv->b_Timer2Mode = APCI3120_TIMER;
-
-	} else {			/*  Initialize Watch dog */
-
-		/* Set the Timer 2 in mode 5(Watchdog) */
-		devpriv->b_TimerSelectMode =
-			(devpriv->
-			b_TimerSelectMode & 0x0F) | APCI3120_TIMER_2_MODE_5;
-		outb(devpriv->b_TimerSelectMode,
-			devpriv->iobase + APCI3120_TIMER_CRT1);
-
-		/*
-		 * Configure the timer 2 for writing the LOW unsigned short of timer
-		 * is Delay value You must make a b_tmp variable with
-		 * DigitalOutPutRegister because at Address_1+APCI3120_TIMER_CRT0
-		 * you can set the digital output and configure the timer 2,and if
-		 * you don't make this, digital output are erase (Set to 0)
-		 */
-
-		/* Writing LOW unsigned short */
-		b_Tmp = ((devpriv->
-				b_DigitalOutputRegister) & 0xF0) |
-			APCI3120_SELECT_TIMER_2_LOW_WORD;
-		outb(b_Tmp, devpriv->iobase + APCI3120_TIMER_CRT0);
-		outw(ui_Timervalue2 & 0xffff,
-			devpriv->iobase + APCI3120_TIMER_VALUE);
-
-		/* Writing HIGH unsigned short */
-		b_Tmp = ((devpriv->
-				b_DigitalOutputRegister) & 0xF0) |
-			APCI3120_SELECT_TIMER_2_HIGH_WORD;
-		outb(b_Tmp, devpriv->iobase + APCI3120_TIMER_CRT0);
-
-		outw((ui_Timervalue2 >> 16) & 0xffff,
-			devpriv->iobase + APCI3120_TIMER_VALUE);
-		/* watchdog enabled */
-		devpriv->b_Timer2Mode = APCI3120_WATCHDOG;
-
-	}
-
-	return insn->n;
-
-}
-
-/*
- * To start and stop the timer
- *
- * data[0] = 1 (start)
- *	   = 0 (stop)
- *	   = 2 (write new value)
- * data[1] = new value
- *
- * devpriv->b_Timer2Mode = 0 DISABLE
- *			 = 1 Timer
- *			 = 2 Watch dog
- */
-static int apci3120_write_insn_timer(struct comedi_device *dev,
-				     struct comedi_subdevice *s,
-				     struct comedi_insn *insn,
-				     unsigned int *data)
-{
-	const struct addi_board *this_board = dev->board_ptr;
-	struct addi_private *devpriv = dev->private;
-	unsigned int ui_Timervalue2 = 0;
-	unsigned short us_TmpValue;
-	unsigned char b_Tmp;
-
-	if ((devpriv->b_Timer2Mode != APCI3120_WATCHDOG)
-		&& (devpriv->b_Timer2Mode != APCI3120_TIMER)) {
-		dev_err(dev->class_dev, "timer2 not configured\n");
-		return -EINVAL;
-	}
-
-	if (data[0] == 2) {	/*  write new value */
-		if (devpriv->b_Timer2Mode != APCI3120_TIMER) {
-			dev_err(dev->class_dev,
-				"timer2 not configured in TIMER MODE\n");
-			return -EINVAL;
-		}
-
-		if (data[1])
-			ui_Timervalue2 = data[1];
-		else
-			ui_Timervalue2 = 0;
-	}
-
-	switch (data[0]) {
-	case APCI3120_START:
-
-		/*  Reset FC_TIMER BIT */
-		inb(devpriv->iobase + APCI3120_TIMER_STATUS_REGISTER);
-		if (devpriv->b_Timer2Mode == APCI3120_TIMER) {	/* start timer */
-			/* Enable Timer */
-			devpriv->b_ModeSelectRegister =
-				devpriv->b_ModeSelectRegister & 0x0B;
-		} else {		/* start watch dog */
-			/* Enable WatchDog */
-			devpriv->b_ModeSelectRegister =
-				(devpriv->
-				b_ModeSelectRegister & 0x0B) |
-				APCI3120_ENABLE_WATCHDOG;
-		}
-
-		/* enable disable interrupt */
-		if ((devpriv->b_Timer2Interrupt) == APCI3120_ENABLE) {
-
-			devpriv->b_ModeSelectRegister =
-				devpriv->
-				b_ModeSelectRegister |
-				APCI3120_ENABLE_TIMER_INT;
-			/*  save the task structure to pass info to user */
-			devpriv->tsk_Current = current;
-		} else {
-
-			devpriv->b_ModeSelectRegister =
-				devpriv->
-				b_ModeSelectRegister &
-				APCI3120_DISABLE_TIMER_INT;
-		}
-		outb(devpriv->b_ModeSelectRegister,
-			devpriv->iobase + APCI3120_WRITE_MODE_SELECT);
-
-		if (devpriv->b_Timer2Mode == APCI3120_TIMER) {	/* start timer */
-			/* For Timer mode is  Gate2 must be activated	timer started */
-			devpriv->us_OutputRegister =
-				devpriv->
-				us_OutputRegister | APCI3120_ENABLE_TIMER2;
-			outw(devpriv->us_OutputRegister,
-				devpriv->iobase + APCI3120_WR_ADDRESS);
-		}
-
-		break;
-
-	case APCI3120_STOP:
-		if (devpriv->b_Timer2Mode == APCI3120_TIMER) {
-			/* Disable timer */
-			devpriv->b_ModeSelectRegister =
-				devpriv->
-				b_ModeSelectRegister &
-				APCI3120_DISABLE_TIMER_COUNTER;
-		} else {
-			/* Disable WatchDog */
-			devpriv->b_ModeSelectRegister =
-				devpriv->
-				b_ModeSelectRegister &
-				APCI3120_DISABLE_WATCHDOG;
-		}
-		/*  Disable timer interrupt */
-		devpriv->b_ModeSelectRegister =
-			devpriv->
-			b_ModeSelectRegister & APCI3120_DISABLE_TIMER_INT;
-
-		/*  Write above states  to register */
-		outb(devpriv->b_ModeSelectRegister,
-			devpriv->iobase + APCI3120_WRITE_MODE_SELECT);
-
-		/*  Reset Gate 2 */
-		devpriv->us_OutputRegister =
-			devpriv->us_OutputRegister & APCI3120_DISABLE_TIMER_INT;
-		outw(devpriv->us_OutputRegister,
-			devpriv->iobase + APCI3120_WR_ADDRESS);
-
-		/*  Reset FC_TIMER BIT */
-		inb(devpriv->iobase + APCI3120_TIMER_STATUS_REGISTER);
-
-		break;
-
-	case 2:		/* write new value to Timer */
-		if (devpriv->b_Timer2Mode != APCI3120_TIMER) {
-			dev_err(dev->class_dev,
-				"timer2 not configured in TIMER MODE\n");
-			return -EINVAL;
-		}
-		us_TmpValue =
-			(unsigned short) inw(devpriv->iobase + APCI3120_RD_STATUS);
-
-		/*
-		 * EL250804: Testing if board APCI3120 have the new Quartz or if it
-		 * is an APCI3001 and calculate the time value to set in the timer
-		 */
-		if ((us_TmpValue & 0x00B0) == 0x00B0
-			|| !strcmp(this_board->pc_DriverName, "apci3001")) {
-			/* Calculate the time value to set in the timer */
-			ui_Timervalue2 = ui_Timervalue2 / 50;
-		} else {
-			/* Calculate the time value to set in the timer */
-			ui_Timervalue2 = ui_Timervalue2 / 70;
-		}
-		/* Writing LOW unsigned short */
-		b_Tmp = ((devpriv->
-				b_DigitalOutputRegister) & 0xF0) |
-			APCI3120_SELECT_TIMER_2_LOW_WORD;
-		outb(b_Tmp, devpriv->iobase + APCI3120_TIMER_CRT0);
-
-		outw(ui_Timervalue2 & 0xffff,
-			devpriv->iobase + APCI3120_TIMER_VALUE);
-
-		/* Writing HIGH unsigned short */
-		b_Tmp = ((devpriv->
-				b_DigitalOutputRegister) & 0xF0) |
-			APCI3120_SELECT_TIMER_2_HIGH_WORD;
-		outb(b_Tmp, devpriv->iobase + APCI3120_TIMER_CRT0);
-
-		outw((ui_Timervalue2 >> 16) & 0xffff,
-			devpriv->iobase + APCI3120_TIMER_VALUE);
-
-		break;
-	default:
-		return -EINVAL;	/*  Not a valid input */
-	}
-
-	return insn->n;
-}
-
-/*
- * Read the Timer value
- *
- * for Timer: data[0]= Timer constant
- *
- * for watchdog: data[0] = 0 (still running)
- *			 = 1 (run down)
- */
-static int apci3120_read_insn_timer(struct comedi_device *dev,
-				    struct comedi_subdevice *s,
-				    struct comedi_insn *insn,
-				    unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-	unsigned char b_Tmp;
-	unsigned short us_TmpValue, us_TmpValue_2, us_StatusValue;
-
-	if ((devpriv->b_Timer2Mode != APCI3120_WATCHDOG)
-		&& (devpriv->b_Timer2Mode != APCI3120_TIMER)) {
-		dev_err(dev->class_dev, "timer2 not configured\n");
-	}
-	if (devpriv->b_Timer2Mode == APCI3120_TIMER) {
-
-		/* Read the LOW unsigned short of Timer 2 register */
-		b_Tmp = ((devpriv->
-				b_DigitalOutputRegister) & 0xF0) |
-			APCI3120_SELECT_TIMER_2_LOW_WORD;
-		outb(b_Tmp, devpriv->iobase + APCI3120_TIMER_CRT0);
-
-		us_TmpValue = inw(devpriv->iobase + APCI3120_TIMER_VALUE);
-
-		/* Read the HIGH unsigned short of Timer 2 register */
-		b_Tmp = ((devpriv->
-				b_DigitalOutputRegister) & 0xF0) |
-			APCI3120_SELECT_TIMER_2_HIGH_WORD;
-		outb(b_Tmp, devpriv->iobase + APCI3120_TIMER_CRT0);
-
-		us_TmpValue_2 = inw(devpriv->iobase + APCI3120_TIMER_VALUE);
-
-		/*  combining both words */
-		data[0] = (unsigned int) ((us_TmpValue) | ((us_TmpValue_2) << 16));
-
-	} else {			/*  Read watch dog status */
-
-		us_StatusValue = inw(devpriv->iobase + APCI3120_RD_STATUS);
-		us_StatusValue =
-			((us_StatusValue & APCI3120_FC_TIMER) >> 12) & 1;
-		if (us_StatusValue == 1) {
-			/*  RESET FC_TIMER BIT */
-			inb(devpriv->iobase + APCI3120_TIMER_STATUS_REGISTER);
-		}
-		data[0] = us_StatusValue;	/*  when data[0] = 1 then the watch dog has rundown */
-	}
-	return insn->n;
-}
-
-static int apci3120_di_insn_bits(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn,
-				 unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-	unsigned int val;
-
-	/* the input channels are bits 11:8 of the status reg */
-	val = inw(devpriv->iobase + APCI3120_RD_STATUS);
-	data[1] = (val >> 8) & 0xf;
-
-	return insn->n;
-}
-
-static int apci3120_do_insn_bits(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn,
-				 unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-
-	if (comedi_dio_update_state(s, data)) {
-		/* The do channels are bits 7:4 of the do register */
-		devpriv->b_DigitalOutputRegister = s->state << 4;
-
-		outb(devpriv->b_DigitalOutputRegister,
-		     devpriv->iobase + APCI3120_DIGITAL_OUTPUT);
-	}
-
-	data[1] = s->state;
-
-	return insn->n;
-}
-
-static int apci3120_ao_insn_write(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn,
-				  unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-	unsigned int ui_Range, ui_Channel;
-	unsigned short us_TmpValue;
-
-	ui_Range = CR_RANGE(insn->chanspec);
-	ui_Channel = CR_CHAN(insn->chanspec);
-
-	if (ui_Range) {		/*  if 1 then unipolar */
-
-		if (data[0] != 0)
-			data[0] =
-				((((ui_Channel & 0x03) << 14) & 0xC000) | (1 <<
-					13) | (data[0] + 8191));
-		else
-			data[0] =
-				((((ui_Channel & 0x03) << 14) & 0xC000) | (1 <<
-					13) | 8192);
-
-	} else {			/*  if 0 then   bipolar */
-		data[0] =
-			((((ui_Channel & 0x03) << 14) & 0xC000) | (0 << 13) |
-			data[0]);
-
-	}
-
-	do {			/* Waiting of DA_READY BIT */
-		us_TmpValue =
-			((unsigned short) inw(devpriv->iobase +
-				APCI3120_RD_STATUS)) & 0x0001;
-	} while (us_TmpValue != 0x0001);
-
-	if (ui_Channel <= 3)
-		/*
-		 * for channel 0-3 out at the register 1 (wrDac1-8) data[i]
-		 * typecasted to ushort since word write is to be done
-		 */
-		outw((unsigned short) data[0],
-			devpriv->iobase + APCI3120_ANALOG_OUTPUT_1);
-	else
-		/*
-		 * for channel 4-7 out at the register 2 (wrDac5-8) data[i]
-		 * typecasted to ushort since word write is to be done
-		 */
-		outw((unsigned short) data[0],
-			devpriv->iobase + APCI3120_ANALOG_OUTPUT_2);
-
-	return insn->n;
-}
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3200.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3200.c
deleted file mode 100644
index 5e321f91172f..000000000000
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3200.c
+++ /dev/null
@@ -1,3003 +0,0 @@
-/**
-@verbatim
-
-Copyright (C) 2004,2005  ADDI-DATA GmbH for the source code of this module.
-
-	ADDI-DATA GmbH
-	Dieselstrasse 3
-	D-77833 Ottersweier
-	Tel: +19(0)7223/9493-0
-	Fax: +49(0)7223/9493-92
-	http://www.addi-data.com
-	info@addi-data.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.
-
-@endverbatim
-*/
-/*
-
-  +-----------------------------------------------------------------------+
-  | (C) ADDI-DATA GmbH          Dieselstraße 3       D-77833 Ottersweier  |
-  +-----------------------------------------------------------------------+
-  | Tel : +49 (0) 7223/9493-0     | email    : info@addi-data.com         |
-  | Fax : +49 (0) 7223/9493-92    | Internet : http://www.addi-data.com   |
-  +-------------------------------+---------------------------------------+
-  | Project     : APCI-3200       | Compiler   : GCC                      |
-  | Module name : hwdrv_apci3200.c| Version    : 2.96                     |
-  +-------------------------------+---------------------------------------+
-  | Project manager: Eric Stolz   | Date       :  02/12/2002              |
-  +-------------------------------+---------------------------------------+
-  | Description :   Hardware Layer Access For APCI-3200                   |
-  +-----------------------------------------------------------------------+
-  |                             UPDATES                                   |
-  +----------+-----------+------------------------------------------------+
-  |   Date   |   Author  |          Description of updates                |
-  +----------+-----------+------------------------------------------------+
-  | 02.07.04 | J. Krauth | Modification from the driver in order to       |
-  |          |           | correct some errors when using several boards. |
-  |          |           |                                                |
-  |          |           |                                                |
-  +----------+-----------+------------------------------------------------+
-  | 26.10.04 | J. Krauth | - Update for COMEDI 0.7.68                     |
-  |          |           | - Read eeprom value                            |
-  |          |           | - Append APCI-3300                             |
-  +----------+-----------+------------------------------------------------+
-*/
-
-/* Card Specific information */
-/* #define APCI3200_ADDRESS_RANGE	264 */
-
-/* Analog Input related Defines */
-#define APCI3200_AI_OFFSET_GAIN		0
-#define APCI3200_AI_SC_TEST		4
-#define APCI3200_AI_IRQ			8
-#define APCI3200_AI_AUTOCAL		12
-#define APCI3200_RELOAD_CONV_TIME_VAL	32
-#define APCI3200_CONV_TIME_TIME_BASE	36
-#define APCI3200_RELOAD_DELAY_TIME_VAL	40
-#define APCI3200_DELAY_TIME_TIME_BASE	44
-#define APCI3200_AI_MODULE1		0
-#define APCI3200_AI_MODULE2		64
-#define APCI3200_AI_MODULE3		128
-#define APCI3200_AI_MODULE4		192
-#define TRUE				1
-#define FALSE				0
-#define APCI3200_AI_EOSIRQ		16
-#define APCI3200_AI_EOS			20
-#define APCI3200_AI_CHAN_ID		24
-#define APCI3200_AI_CHAN_VAL		28
-#define ANALOG_INPUT			0
-#define TEMPERATURE			1
-#define RESISTANCE			2
-
-#define ENABLE_EXT_TRIG			1
-#define ENABLE_EXT_GATE			2
-#define ENABLE_EXT_TRIG_GATE		3
-
-#define APCI3200_MAXVOLT		2.5
-#define ADDIDATA_GREATER_THAN_TEST	0
-#define ADDIDATA_LESS_THAN_TEST		1
-
-#define ADDIDATA_UNIPOLAR		1
-#define ADDIDATA_BIPOLAR		2
-
-#define MAX_MODULE			4
-
-/* ANALOG INPUT RANGE */
-static const struct comedi_lrange range_apci3200_ai = {
-	8, {
-		BIP_RANGE(10),
-		BIP_RANGE(5),
-		BIP_RANGE(2),
-		BIP_RANGE(1),
-		UNI_RANGE(10),
-		UNI_RANGE(5),
-		UNI_RANGE(2),
-		UNI_RANGE(1)
-	}
-};
-
-static const struct comedi_lrange range_apci3300_ai = {
-	4, {
-		UNI_RANGE(10),
-		UNI_RANGE(5),
-		UNI_RANGE(2),
-		UNI_RANGE(1)
-	}
-};
-
-int MODULE_NO;
-struct {
-	int i_Gain;
-	int i_Polarity;
-	int i_OffsetRange;
-	int i_Coupling;
-	int i_SingleDiff;
-	int i_AutoCalibration;
-	unsigned int ui_ReloadValue;
-	unsigned int ui_TimeUnitReloadVal;
-	int i_Interrupt;
-	int i_ModuleSelection;
-} Config_Parameters_Module1, Config_Parameters_Module2,
-    Config_Parameters_Module3, Config_Parameters_Module4;
-
-
-struct str_ADDIDATA_RTDStruct {
-	unsigned int ul_NumberOfValue;
-	unsigned int *pul_ResistanceValue;
-	unsigned int *pul_TemperatureValue;
-};
-
-struct str_Module {
-	unsigned long ul_CurrentSourceCJC;
-	unsigned long ul_CurrentSource[5];
-	unsigned long ul_GainFactor[8];	/*  Gain Factor */
-	unsigned int w_GainValue[10];
-};
-
-struct str_BoardInfos {
-
-	int i_CJCAvailable;
-	int i_CJCPolarity;
-	int i_CJCGain;
-	int i_InterruptFlag;
-	int i_ADDIDATAPolarity;
-	int i_ADDIDATAGain;
-	int i_AutoCalibration;
-	int i_ADDIDATAConversionTime;
-	int i_ADDIDATAConversionTimeUnit;
-	int i_ADDIDATAType;
-	int i_ChannelNo;
-	int i_ChannelCount;
-	int i_ScanType;
-	int i_FirstChannel;
-	int i_LastChannel;
-	int i_Sum;
-	int i_Offset;
-	unsigned int ui_Channel_num;
-	int i_Count;
-	int i_Initialised;
-	unsigned int ui_InterruptChannelValue[144];	/* Buffer */
-	unsigned char b_StructInitialized;
-	/* 7 is the maximal number of channels */
-	unsigned int ui_ScanValueArray[7 + 12];	
-
-	int i_ConnectionType;
-	int i_NbrOfModule;
-	struct str_Module s_Module[MAX_MODULE];
-};
-
-/* BEGIN JK 06.07.04: Management of sevrals boards */
-/*
-  int i_CJCAvailable=1;
-  int i_CJCPolarity=0;
-  int i_CJCGain=2;/* changed from 0 to 2 */
-  int i_InterruptFlag=0;
-  int i_ADDIDATAPolarity;
-  int i_ADDIDATAGain;
-  int i_AutoCalibration=0;   /* : auto calibration */
-  int i_ADDIDATAConversionTime;
-  int i_ADDIDATAConversionTimeUnit;
-  int i_ADDIDATAType;
-  int i_ChannelNo;
-  int i_ChannelCount=0;
-  int i_ScanType;
-  int i_FirstChannel;
-  int i_LastChannel;
-  int i_Sum=0;
-  int i_Offset;
-  unsigned int ui_Channel_num=0;
-  static int i_Count=0;
-  int i_Initialised=0;
-  unsigned int ui_InterruptChannelValue[96]; /* Buffer */
-*/
-struct str_BoardInfos s_BoardInfos[100];	/*  100 will be the max number of boards to be used */
-/* END JK 06.07.04: Management of sevrals boards */
-
-#define AMCC_OP_REG_MCSR	0x3c
-#define EEPROM_BUSY		0x80000000
-#define NVCMD_LOAD_LOW		(0x4 << 5)	/* nvRam load low command */
-#define NVCMD_LOAD_HIGH		(0x5 << 5)	/* nvRam load high command */
-#define NVCMD_BEGIN_READ	(0x7 << 5)	/* nvRam begin read command */
-#define NVCMD_BEGIN_WRITE	(0x6 << 5)	/* EEPROM begin write command */
-
-static int i_AddiHeaderRW_ReadEeprom(int i_NbOfWordsToRead,
-				     unsigned int dw_PCIBoardEepromAddress,
-				     unsigned short w_EepromStartAddress,
-				     unsigned short *pw_DataRead)
-{
-	unsigned int dw_eeprom_busy = 0;
-	int i_Counter = 0;
-	int i_WordCounter;
-	int i;
-	unsigned char pb_ReadByte[1];
-	unsigned char b_ReadLowByte = 0;
-	unsigned char b_ReadHighByte = 0;
-	unsigned char b_SelectedAddressLow = 0;
-	unsigned char b_SelectedAddressHigh = 0;
-	unsigned short w_ReadWord = 0;
-
-	for (i_WordCounter = 0; i_WordCounter < i_NbOfWordsToRead;
-		i_WordCounter++) {
-		do {
-			dw_eeprom_busy =
-				inl(dw_PCIBoardEepromAddress +
-				AMCC_OP_REG_MCSR);
-			dw_eeprom_busy = dw_eeprom_busy & EEPROM_BUSY;
-		} while (dw_eeprom_busy == EEPROM_BUSY);
-
-		for (i_Counter = 0; i_Counter < 2; i_Counter++) {
-			b_SelectedAddressLow = (w_EepromStartAddress + i_Counter) % 256;	/* Read the low 8 bit part */
-			b_SelectedAddressHigh = (w_EepromStartAddress + i_Counter) / 256;	/* Read the high 8 bit part */
-
-			/* Select the load low address mode */
-			outb(NVCMD_LOAD_LOW,
-				dw_PCIBoardEepromAddress + AMCC_OP_REG_MCSR +
-				3);
-
-			/* Wait on busy */
-			do {
-				dw_eeprom_busy =
-					inl(dw_PCIBoardEepromAddress +
-					AMCC_OP_REG_MCSR);
-				dw_eeprom_busy = dw_eeprom_busy & EEPROM_BUSY;
-			} while (dw_eeprom_busy == EEPROM_BUSY);
-
-			/* Load the low address */
-			outb(b_SelectedAddressLow,
-				dw_PCIBoardEepromAddress + AMCC_OP_REG_MCSR +
-				2);
-
-			/* Wait on busy */
-			do {
-				dw_eeprom_busy =
-					inl(dw_PCIBoardEepromAddress +
-					AMCC_OP_REG_MCSR);
-				dw_eeprom_busy = dw_eeprom_busy & EEPROM_BUSY;
-			} while (dw_eeprom_busy == EEPROM_BUSY);
-
-			/* Select the load high address mode */
-			outb(NVCMD_LOAD_HIGH,
-				dw_PCIBoardEepromAddress + AMCC_OP_REG_MCSR +
-				3);
-
-			/* Wait on busy */
-			do {
-				dw_eeprom_busy =
-					inl(dw_PCIBoardEepromAddress +
-					AMCC_OP_REG_MCSR);
-				dw_eeprom_busy = dw_eeprom_busy & EEPROM_BUSY;
-			} while (dw_eeprom_busy == EEPROM_BUSY);
-
-			/* Load the high address */
-			outb(b_SelectedAddressHigh,
-				dw_PCIBoardEepromAddress + AMCC_OP_REG_MCSR +
-				2);
-
-			/* Wait on busy */
-			do {
-				dw_eeprom_busy =
-					inl(dw_PCIBoardEepromAddress +
-					AMCC_OP_REG_MCSR);
-				dw_eeprom_busy = dw_eeprom_busy & EEPROM_BUSY;
-			} while (dw_eeprom_busy == EEPROM_BUSY);
-
-			/* Select the READ mode */
-			outb(NVCMD_BEGIN_READ,
-				dw_PCIBoardEepromAddress + AMCC_OP_REG_MCSR +
-				3);
-
-			/* Wait on busy */
-			do {
-				dw_eeprom_busy =
-					inl(dw_PCIBoardEepromAddress +
-					AMCC_OP_REG_MCSR);
-				dw_eeprom_busy = dw_eeprom_busy & EEPROM_BUSY;
-			} while (dw_eeprom_busy == EEPROM_BUSY);
-
-			/* Read data into the EEPROM */
-			*pb_ReadByte =
-				inb(dw_PCIBoardEepromAddress +
-				AMCC_OP_REG_MCSR + 2);
-
-			/* Wait on busy */
-			do {
-				dw_eeprom_busy =
-					inl(dw_PCIBoardEepromAddress +
-					AMCC_OP_REG_MCSR);
-				dw_eeprom_busy = dw_eeprom_busy & EEPROM_BUSY;
-			} while (dw_eeprom_busy == EEPROM_BUSY);
-
-			/* Select the upper address part */
-			if (i_Counter == 0)
-				b_ReadLowByte = pb_ReadByte[0];
-			else
-				b_ReadHighByte = pb_ReadByte[0];
-
-
-			/* Sleep */
-			msleep(1);
-
-		}
-		w_ReadWord =
-			(b_ReadLowByte | (((unsigned short)b_ReadHighByte) *
-				256));
-
-		pw_DataRead[i_WordCounter] = w_ReadWord;
-
-		w_EepromStartAddress += 2;	/*  to read the next word */
-
-	}			/*  for (...) i_NbOfWordsToRead */
-	return 0;
-}
-
-static void v_GetAPCI3200EepromCalibrationValue(unsigned int dw_PCIBoardEepromAddress,
-						struct str_BoardInfos *BoardInformations)
-{
-	unsigned short w_AnalogInputMainHeaderAddress;
-	unsigned short w_AnalogInputComponentAddress;
-	unsigned short w_NumberOfModuls = 0;
-	unsigned short w_CurrentSources[2];
-	unsigned short w_ModulCounter = 0;
-	unsigned short w_FirstHeaderSize = 0;
-	unsigned short w_NumberOfInputs = 0;
-	unsigned short w_CJCFlag = 0;
-	unsigned short w_NumberOfGainValue = 0;
-	unsigned short w_SingleHeaderAddress = 0;
-	unsigned short w_SingleHeaderSize = 0;
-	unsigned short w_Input = 0;
-	unsigned short w_GainFactorAddress = 0;
-	unsigned short w_GainFactorValue[2];
-	unsigned short w_GainIndex = 0;
-	unsigned short w_GainValue = 0;
-
-  /*****************************************/
-  /** Get the Analog input header address **/
-  /*****************************************/
-	i_AddiHeaderRW_ReadEeprom(1,	/* i_NbOfWordsToRead */
-		dw_PCIBoardEepromAddress, 0x116,	/* w_EepromStartAddress: Analog input header address */
-		&w_AnalogInputMainHeaderAddress);
-
-  /*******************************************/
-  /** Compute the real analog input address **/
-  /*******************************************/
-	w_AnalogInputMainHeaderAddress = w_AnalogInputMainHeaderAddress + 0x100;
-
-  /******************************/
-  /** Get the number of moduls **/
-  /******************************/
-	i_AddiHeaderRW_ReadEeprom(1,	/* i_NbOfWordsToRead */
-		dw_PCIBoardEepromAddress, w_AnalogInputMainHeaderAddress + 0x02,	/* w_EepromStartAddress: Number of conponment */
-		&w_NumberOfModuls);
-
-	for (w_ModulCounter = 0; w_ModulCounter < w_NumberOfModuls;
-		w_ModulCounter++) {
-      /***********************************/
-      /** Compute the component address **/
-      /***********************************/
-		w_AnalogInputComponentAddress =
-			w_AnalogInputMainHeaderAddress +
-			(w_FirstHeaderSize * w_ModulCounter) + 0x04;
-
-      /****************************/
-      /** Read first header size **/
-      /****************************/
-		i_AddiHeaderRW_ReadEeprom(1,	/* i_NbOfWordsToRead */
-			dw_PCIBoardEepromAddress, w_AnalogInputComponentAddress,	/*  Address of the first header */
-			&w_FirstHeaderSize);
-
-		w_FirstHeaderSize = w_FirstHeaderSize >> 4;
-
-      /***************************/
-      /** Read number of inputs **/
-      /***************************/
-		i_AddiHeaderRW_ReadEeprom(1,	/* i_NbOfWordsToRead */
-			dw_PCIBoardEepromAddress, w_AnalogInputComponentAddress + 0x06,	/*  Number of inputs for the first modul */
-			&w_NumberOfInputs);
-
-		w_NumberOfInputs = w_NumberOfInputs >> 4;
-
-      /***********************/
-      /** Read the CJC flag **/
-      /***********************/
-		i_AddiHeaderRW_ReadEeprom(1,	/* i_NbOfWordsToRead */
-			dw_PCIBoardEepromAddress, w_AnalogInputComponentAddress + 0x08,	/*  CJC flag */
-			&w_CJCFlag);
-
-		w_CJCFlag = (w_CJCFlag >> 3) & 0x1;	/*  Get only the CJC flag */
-
-      /*******************************/
-      /** Read number of gain value **/
-      /*******************************/
-		i_AddiHeaderRW_ReadEeprom(1,	/* i_NbOfWordsToRead */
-			dw_PCIBoardEepromAddress, w_AnalogInputComponentAddress + 0x44,	/*  Number of gain value */
-			&w_NumberOfGainValue);
-
-		w_NumberOfGainValue = w_NumberOfGainValue & 0xFF;
-
-      /***********************************/
-      /** Compute single header address **/
-      /***********************************/
-		w_SingleHeaderAddress =
-			w_AnalogInputComponentAddress + 0x46 +
-			(((w_NumberOfGainValue / 16) + 1) * 2) +
-			(6 * w_NumberOfGainValue) +
-			(4 * (((w_NumberOfGainValue / 16) + 1) * 2));
-
-      /********************************************/
-      /** Read current sources value for input 1 **/
-      /********************************************/
-		i_AddiHeaderRW_ReadEeprom(1,	/* i_NbOfWordsToRead */
-			dw_PCIBoardEepromAddress, w_SingleHeaderAddress,	/* w_EepromStartAddress: Single header address */
-			&w_SingleHeaderSize);
-
-		w_SingleHeaderSize = w_SingleHeaderSize >> 4;
-
-      /*************************************/
-      /** Read gain factor for the module **/
-      /*************************************/
-		w_GainFactorAddress = w_AnalogInputComponentAddress;
-
-		for (w_GainIndex = 0; w_GainIndex < w_NumberOfGainValue;
-			w_GainIndex++) {
-	  /************************************/
-	  /** Read gain value for the module **/
-	  /************************************/
-			i_AddiHeaderRW_ReadEeprom(1,	/* i_NbOfWordsToRead */
-				dw_PCIBoardEepromAddress, w_AnalogInputComponentAddress + 70 + (2 * (1 + (w_NumberOfGainValue / 16))) + (0x02 * w_GainIndex),	/*  Gain value */
-				&w_GainValue);
-
-			BoardInformations->s_Module[w_ModulCounter].
-				w_GainValue[w_GainIndex] = w_GainValue;
-
-	  /*************************************/
-	  /** Read gain factor for the module **/
-	  /*************************************/
-			i_AddiHeaderRW_ReadEeprom(2,	/* i_NbOfWordsToRead */
-				dw_PCIBoardEepromAddress, w_AnalogInputComponentAddress + 70 + ((2 * w_NumberOfGainValue) + (2 * (1 + (w_NumberOfGainValue / 16)))) + (0x04 * w_GainIndex),	/*  Gain factor */
-				w_GainFactorValue);
-
-			BoardInformations->s_Module[w_ModulCounter].
-				ul_GainFactor[w_GainIndex] =
-				(w_GainFactorValue[1] << 16) +
-				w_GainFactorValue[0];
-		}
-
-      /***************************************************************/
-      /** Read current source value for each channels of the module **/
-      /***************************************************************/
-		for (w_Input = 0; w_Input < w_NumberOfInputs; w_Input++) {
-	  /********************************************/
-	  /** Read current sources value for input 1 **/
-	  /********************************************/
-			i_AddiHeaderRW_ReadEeprom(2,	/* i_NbOfWordsToRead */
-				dw_PCIBoardEepromAddress,
-				(w_Input * w_SingleHeaderSize) +
-				w_SingleHeaderAddress + 0x0C, w_CurrentSources);
-
-	  /************************************/
-	  /** Save the current sources value **/
-	  /************************************/
-			BoardInformations->s_Module[w_ModulCounter].
-				ul_CurrentSource[w_Input] =
-				(w_CurrentSources[0] +
-				((w_CurrentSources[1] & 0xFFF) << 16));
-		}
-
-      /***************************************/
-      /** Read the CJC current source value **/
-      /***************************************/
-		i_AddiHeaderRW_ReadEeprom(2,	/* i_NbOfWordsToRead */
-			dw_PCIBoardEepromAddress,
-			(w_Input * w_SingleHeaderSize) + w_SingleHeaderAddress +
-			0x0C, w_CurrentSources);
-
-      /************************************/
-      /** Save the current sources value **/
-      /************************************/
-		BoardInformations->s_Module[w_ModulCounter].
-			ul_CurrentSourceCJC =
-			(w_CurrentSources[0] +
-			((w_CurrentSources[1] & 0xFFF) << 16));
-	}
-}
-
-static int i_APCI3200_GetChannelCalibrationValue(struct comedi_device *dev,
-						 unsigned int ui_Channel_num,
-						 unsigned int *CJCCurrentSource,
-						 unsigned int *ChannelCurrentSource,
-						 unsigned int *ChannelGainFactor)
-{
-	int i_DiffChannel = 0;
-	int i_Module = 0;
-
-	/* Test if single or differential mode */
-	if (s_BoardInfos[dev->minor].i_ConnectionType == 1) {
-		/* if diff */
-
-		if (ui_Channel_num <= 1)
-			i_DiffChannel = ui_Channel_num, i_Module = 0;
-		else if ((ui_Channel_num >= 2) && (ui_Channel_num <= 3))
-			i_DiffChannel = ui_Channel_num - 2, i_Module = 1;
-		else if ((ui_Channel_num >= 4) && (ui_Channel_num <= 5))
-			i_DiffChannel = ui_Channel_num - 4, i_Module = 2;
-		else if ((ui_Channel_num >= 6) && (ui_Channel_num <= 7))
-			i_DiffChannel = ui_Channel_num - 6, i_Module = 3;
-
-	} else {
-		/*  if single */
-		if ((ui_Channel_num == 0) || (ui_Channel_num == 1))
-			i_DiffChannel = 0, i_Module = 0;
-		else if ((ui_Channel_num == 2) || (ui_Channel_num == 3))
-			i_DiffChannel = 1, i_Module = 0;
-		else if ((ui_Channel_num == 4) || (ui_Channel_num == 5))
-			i_DiffChannel = 0, i_Module = 1;
-		else if ((ui_Channel_num == 6) || (ui_Channel_num == 7))
-			i_DiffChannel = 1, i_Module = 1;
-		else if ((ui_Channel_num == 8) || (ui_Channel_num == 9))
-			i_DiffChannel = 0, i_Module = 2;
-		else if ((ui_Channel_num == 10) || (ui_Channel_num == 11))
-			i_DiffChannel = 1, i_Module = 2;
-		else if ((ui_Channel_num == 12) || (ui_Channel_num == 13))
-			i_DiffChannel = 0, i_Module = 3;
-		else if ((ui_Channel_num == 14) || (ui_Channel_num == 15))
-			i_DiffChannel = 1, i_Module = 3;
-	}
-
-	/* Test if thermocouple or RTD mode */
-	*CJCCurrentSource =
-		s_BoardInfos[dev->minor].s_Module[i_Module].ul_CurrentSourceCJC;
-
-	*ChannelCurrentSource =
-		s_BoardInfos[dev->minor].s_Module[i_Module].
-		ul_CurrentSource[i_DiffChannel];
-	/*       } */
-	/*    } */
-
-	/* Channle gain factor */
-	*ChannelGainFactor =
-		s_BoardInfos[dev->minor].s_Module[i_Module].
-		ul_GainFactor[s_BoardInfos[dev->minor].i_ADDIDATAGain];
-	/* End JK 21.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-
-	return 0;
-}
-
-static int apci3200_di_insn_bits(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn,
-				 unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-
-	data[1] = inl(devpriv->i_IobaseReserved) & 0xf;
-
-	return insn->n;
-}
-
-static int apci3200_do_insn_bits(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn,
-				 unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-
-	s->state = inl(devpriv->i_IobaseAddon) & 0xf;
-
-	if (comedi_dio_update_state(s, data))
-		outl(s->state, devpriv->i_IobaseAddon);
-
-	data[1] = s->state;
-
-	return insn->n;
-}
-
-static int i_APCI3200_Read1AnalogInputChannel(struct comedi_device *dev,
-					      struct comedi_subdevice *s,
-					      struct comedi_insn *insn,
-					      unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-	unsigned int ui_EOC = 0;
-	unsigned int ui_ChannelNo = 0;
-	unsigned int ui_CommandRegister = 0;
-
-	/* BEGIN JK 06.07.04: Management of sevrals boards */
-	/* ui_ChannelNo=i_ChannelNo; */
-	ui_ChannelNo = s_BoardInfos[dev->minor].i_ChannelNo;
-
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-  /*********************************/
-	/* Write the channel to configure */
-  /*********************************/
-	/* Begin JK 20.10.2004: Bad channel value is used when using differential mode */
-	/* outl(0 | ui_Channel_num , devpriv->iobase+i_Offset + 0x4); */
-	/* outl(0 | s_BoardInfos [dev->minor].ui_Channel_num , devpriv->iobase+s_BoardInfos [dev->minor].i_Offset + 0x4); */
-	outl(0 | s_BoardInfos[dev->minor].i_ChannelNo,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 0x4);
-	/* End JK 20.10.2004: Bad channel value is used when using differential mode */
-
-  /*******************************/
-	/* Set the convert timing unit */
-  /*******************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-
-	/* outl(i_ADDIDATAConversionTimeUnit , devpriv->iobase+i_Offset + 36); */
-	outl(s_BoardInfos[dev->minor].i_ADDIDATAConversionTimeUnit,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 36);
-
-  /**************************/
-	/* Set the convert timing */
-  /**************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-
-	/* outl(i_ADDIDATAConversionTime , devpriv->iobase+i_Offset + 32); */
-	outl(s_BoardInfos[dev->minor].i_ADDIDATAConversionTime,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 32);
-
-  /**************************************************************************/
-	/* Set the start end stop index to the selected channel and set the start */
-  /**************************************************************************/
-
-	ui_CommandRegister = ui_ChannelNo | (ui_ChannelNo << 8) | 0x80000;
-
-	/*Test if the interrupt is enable */
-	if (s_BoardInfos[dev->minor].i_InterruptFlag == 1) {
-		/* Enable the interrupt */
-		ui_CommandRegister = ui_CommandRegister | 0x00100000;
-	}
-
-  /******************************/
-	/* Write the command register */
-  /******************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-
-	/* outl(ui_CommandRegister, devpriv->iobase+i_Offset + 8); */
-	outl(ui_CommandRegister,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 8);
-
-	/*Test if interrupt is enable */
-	if (s_BoardInfos[dev->minor].i_InterruptFlag == 0) {
-		do {
-	  /*************************/
-			/*Read the EOC Status bit */
-	  /*************************/
-
-			/* ui_EOC = inl(devpriv->iobase+i_Offset + 20) & 1; */
-			ui_EOC = inl(devpriv->iobase +
-				s_BoardInfos[dev->minor].i_Offset + 20) & 1;
-
-		} while (ui_EOC != 1);
-
-      /***************************************/
-		/* Read the digital value of the input */
-      /***************************************/
-
-		/* data[0] = inl (devpriv->iobase+i_Offset + 28); */
-		data[0] =
-			inl(devpriv->iobase +
-			s_BoardInfos[dev->minor].i_Offset + 28);
-		/* END JK 06.07.04: Management of sevrals boards */
-
-	}
-	return 0;
-}
-
-static int i_APCI3200_ReadCalibrationOffsetValue(struct comedi_device *dev,
-						 unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-	unsigned int ui_Temp = 0, ui_EOC = 0;
-	unsigned int ui_CommandRegister = 0;
-
-	/* BEGIN JK 06.07.04: Management of sevrals boards */
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-  /*********************************/
-	/* Write the channel to configure */
-  /*********************************/
-	/* Begin JK 20.10.2004: This seems not necessary ! */
-	/* outl(0 | ui_Channel_num , devpriv->iobase+i_Offset + 0x4); */
-	/* outl(0 | s_BoardInfos [dev->minor].ui_Channel_num , devpriv->iobase+s_BoardInfos [dev->minor].i_Offset + 0x4); */
-	/* End JK 20.10.2004: This seems not necessary ! */
-
-  /*******************************/
-	/* Set the convert timing unit */
-  /*******************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(i_ADDIDATAConversionTimeUnit , devpriv->iobase+i_Offset + 36); */
-	outl(s_BoardInfos[dev->minor].i_ADDIDATAConversionTimeUnit,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 36);
-  /**************************/
-	/* Set the convert timing */
-  /**************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(i_ADDIDATAConversionTime , devpriv->iobase+i_Offset + 32); */
-	outl(s_BoardInfos[dev->minor].i_ADDIDATAConversionTime,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 32);
-  /*****************************/
-	/*Read the calibration offset */
-  /*****************************/
-	/* ui_Temp = inl(devpriv->iobase+i_Offset + 12); */
-	ui_Temp = inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 12);
-
-  /*********************************/
-	/*Configure the Offset Conversion */
-  /*********************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl((ui_Temp | 0x00020000), devpriv->iobase+i_Offset + 12); */
-	outl((ui_Temp | 0x00020000),
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 12);
-  /*******************************/
-	/*Initialise ui_CommandRegister */
-  /*******************************/
-
-	ui_CommandRegister = 0;
-
-	/*Test if the interrupt is enable */
-	if (s_BoardInfos[dev->minor].i_InterruptFlag == 1) {
-		/*Enable the interrupt */
-		ui_CommandRegister = ui_CommandRegister | 0x00100000;
-	}
-
-  /**********************/
-	/*Start the conversion */
-  /**********************/
-	ui_CommandRegister = ui_CommandRegister | 0x00080000;
-
-  /***************************/
-	/*Write the command regiter */
-  /***************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(ui_CommandRegister, devpriv->iobase+i_Offset + 8); */
-	outl(ui_CommandRegister,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 8);
-
-	/*Test if interrupt is enable */
-	if (s_BoardInfos[dev->minor].i_InterruptFlag == 0) {
-		do {
-	  /*******************/
-			/*Read the EOC flag */
-	  /*******************/
-
-			/* ui_EOC = inl (devpriv->iobase+i_Offset + 20) & 1; */
-			ui_EOC = inl(devpriv->iobase +
-				s_BoardInfos[dev->minor].i_Offset + 20) & 1;
-
-		} while (ui_EOC != 1);
-
-      /**************************************************/
-		/*Read the digital value of the calibration Offset */
-      /**************************************************/
-
-		/* data[0] = inl(devpriv->iobase+i_Offset+ 28); */
-		data[0] =
-			inl(devpriv->iobase +
-			s_BoardInfos[dev->minor].i_Offset + 28);
-	}
-	return 0;
-}
-
-static int i_APCI3200_ReadCalibrationGainValue(struct comedi_device *dev,
-					       unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-	unsigned int ui_EOC = 0;
-	int ui_CommandRegister = 0;
-
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-  /*********************************/
-	/* Write the channel to configure */
-  /*********************************/
-	/* Begin JK 20.10.2004: This seems not necessary ! */
-	/* outl(0 | ui_Channel_num , devpriv->iobase+i_Offset + 0x4); */
-	/* outl(0 | s_BoardInfos [dev->minor].ui_Channel_num , devpriv->iobase+s_BoardInfos [dev->minor].i_Offset + 0x4); */
-	/* End JK 20.10.2004: This seems not necessary ! */
-
-  /***************************/
-	/*Read the calibration gain */
-  /***************************/
-  /*******************************/
-	/* Set the convert timing unit */
-  /*******************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(i_ADDIDATAConversionTimeUnit , devpriv->iobase+i_Offset + 36); */
-	outl(s_BoardInfos[dev->minor].i_ADDIDATAConversionTimeUnit,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 36);
-  /**************************/
-	/* Set the convert timing */
-  /**************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(i_ADDIDATAConversionTime , devpriv->iobase+i_Offset + 32); */
-	outl(s_BoardInfos[dev->minor].i_ADDIDATAConversionTime,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 32);
-  /*******************************/
-	/*Configure the Gain Conversion */
-  /*******************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(0x00040000 , devpriv->iobase+i_Offset + 12); */
-	outl(0x00040000,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 12);
-
-  /*******************************/
-	/*Initialise ui_CommandRegister */
-  /*******************************/
-
-	ui_CommandRegister = 0;
-
-	/*Test if the interrupt is enable */
-	if (s_BoardInfos[dev->minor].i_InterruptFlag == 1) {
-		/*Enable the interrupt */
-		ui_CommandRegister = ui_CommandRegister | 0x00100000;
-	}
-
-  /**********************/
-	/*Start the conversion */
-  /**********************/
-
-	ui_CommandRegister = ui_CommandRegister | 0x00080000;
-  /***************************/
-	/*Write the command regiter */
-  /***************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(ui_CommandRegister , devpriv->iobase+i_Offset + 8); */
-	outl(ui_CommandRegister,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 8);
-
-	/*Test if interrupt is enable */
-	if (s_BoardInfos[dev->minor].i_InterruptFlag == 0) {
-		do {
-
-	  /*******************/
-			/*Read the EOC flag */
-	  /*******************/
-
-			/* ui_EOC = inl(devpriv->iobase+i_Offset + 20) & 1; */
-			ui_EOC = inl(devpriv->iobase +
-				s_BoardInfos[dev->minor].i_Offset + 20) & 1;
-
-		} while (ui_EOC != 1);
-
-      /************************************************/
-		/*Read the digital value of the calibration Gain */
-      /************************************************/
-
-		/* data[0] = inl(devpriv->iobase+i_Offset + 28); */
-		data[0] =
-			inl(devpriv->iobase +
-			s_BoardInfos[dev->minor].i_Offset + 28);
-
-	}
-	return 0;
-}
-
-static int i_APCI3200_ReadCJCValue(struct comedi_device *dev,
-				   unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-	unsigned int ui_EOC = 0;
-	int ui_CommandRegister = 0;
-
-  /******************************/
-	/*Set the converting time unit */
-  /******************************/
-
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-
-	/* outl(i_ADDIDATAConversionTimeUnit , devpriv->iobase+i_Offset + 36); */
-	outl(s_BoardInfos[dev->minor].i_ADDIDATAConversionTimeUnit,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 36);
-  /**************************/
-	/* Set the convert timing */
-  /**************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-
-	/* outl(i_ADDIDATAConversionTime , devpriv->iobase+i_Offset + 32); */
-	outl(s_BoardInfos[dev->minor].i_ADDIDATAConversionTime,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 32);
-
-  /******************************/
-	/*Configure the CJC Conversion */
-  /******************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-
-	/* outl( 0x00000400 , devpriv->iobase+i_Offset + 4); */
-	outl(0x00000400,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 4);
-  /*******************************/
-	/*Initialise dw_CommandRegister */
-  /*******************************/
-	ui_CommandRegister = 0;
-	/*Test if the interrupt is enable */
-	if (s_BoardInfos[dev->minor].i_InterruptFlag == 1) {
-		/*Enable the interrupt */
-		ui_CommandRegister = ui_CommandRegister | 0x00100000;
-	}
-
-  /**********************/
-	/*Start the conversion */
-  /**********************/
-
-	ui_CommandRegister = ui_CommandRegister | 0x00080000;
-
-  /***************************/
-	/*Write the command regiter */
-  /***************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(ui_CommandRegister , devpriv->iobase+i_Offset + 8); */
-	outl(ui_CommandRegister,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 8);
-
-	/*Test if interrupt is enable */
-	if (s_BoardInfos[dev->minor].i_InterruptFlag == 0) {
-		do {
-
-	  /*******************/
-			/*Read the EOC flag */
-	  /*******************/
-
-			/* ui_EOC = inl(devpriv->iobase+i_Offset + 20) & 1; */
-			ui_EOC = inl(devpriv->iobase +
-				s_BoardInfos[dev->minor].i_Offset + 20) & 1;
-
-		} while (ui_EOC != 1);
-
-      /***********************************/
-		/*Read the digital value of the CJC */
-      /***********************************/
-
-		/* data[0] = inl(devpriv->iobase+i_Offset + 28); */
-		data[0] =
-			inl(devpriv->iobase +
-			s_BoardInfos[dev->minor].i_Offset + 28);
-	}
-	return 0;
-}
-
-static int i_APCI3200_ReadCJCCalOffset(struct comedi_device *dev,
-				       unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-	unsigned int ui_EOC = 0;
-	int ui_CommandRegister = 0;
-
-  /*******************************************/
-	/*Read calibration offset value for the CJC */
-  /*******************************************/
-  /*******************************/
-	/* Set the convert timing unit */
-  /*******************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(i_ADDIDATAConversionTimeUnit , devpriv->iobase+i_Offset + 36); */
-	outl(s_BoardInfos[dev->minor].i_ADDIDATAConversionTimeUnit,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 36);
-  /**************************/
-	/* Set the convert timing */
-  /**************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(i_ADDIDATAConversionTime , devpriv->iobase+i_Offset + 32); */
-	outl(s_BoardInfos[dev->minor].i_ADDIDATAConversionTime,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 32);
-  /******************************/
-	/*Configure the CJC Conversion */
-  /******************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(0x00000400 , devpriv->iobase+i_Offset + 4); */
-	outl(0x00000400,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 4);
-  /*********************************/
-	/*Configure the Offset Conversion */
-  /*********************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(0x00020000, devpriv->iobase+i_Offset + 12); */
-	outl(0x00020000,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 12);
-  /*******************************/
-	/*Initialise ui_CommandRegister */
-  /*******************************/
-	ui_CommandRegister = 0;
-	/*Test if the interrupt is enable */
-	if (s_BoardInfos[dev->minor].i_InterruptFlag == 1) {
-		/*Enable the interrupt */
-		ui_CommandRegister = ui_CommandRegister | 0x00100000;
-	}
-
-  /**********************/
-	/*Start the conversion */
-  /**********************/
-	ui_CommandRegister = ui_CommandRegister | 0x00080000;
-  /***************************/
-	/*Write the command regiter */
-  /***************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(ui_CommandRegister,devpriv->iobase+i_Offset + 8); */
-	outl(ui_CommandRegister,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 8);
-	if (s_BoardInfos[dev->minor].i_InterruptFlag == 0) {
-		do {
-	  /*******************/
-			/*Read the EOC flag */
-	  /*******************/
-			/* ui_EOC = inl(devpriv->iobase+i_Offset + 20) & 1; */
-			ui_EOC = inl(devpriv->iobase +
-				s_BoardInfos[dev->minor].i_Offset + 20) & 1;
-		} while (ui_EOC != 1);
-
-      /**************************************************/
-		/*Read the digital value of the calibration Offset */
-      /**************************************************/
-		/* data[0] = inl(devpriv->iobase+i_Offset + 28); */
-		data[0] =
-			inl(devpriv->iobase +
-			s_BoardInfos[dev->minor].i_Offset + 28);
-	}
-	return 0;
-}
-
-static int i_APCI3200_ReadCJCCalGain(struct comedi_device *dev,
-				     unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-	unsigned int ui_EOC = 0;
-	int ui_CommandRegister = 0;
-
-  /*******************************/
-	/* Set the convert timing unit */
-  /*******************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(i_ADDIDATAConversionTimeUnit , devpriv->iobase+i_Offset + 36); */
-	outl(s_BoardInfos[dev->minor].i_ADDIDATAConversionTimeUnit,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 36);
-  /**************************/
-	/* Set the convert timing */
-  /**************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(i_ADDIDATAConversionTime , devpriv->iobase+i_Offset + 32); */
-	outl(s_BoardInfos[dev->minor].i_ADDIDATAConversionTime,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 32);
-  /******************************/
-	/*Configure the CJC Conversion */
-  /******************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(0x00000400,devpriv->iobase+i_Offset + 4); */
-	outl(0x00000400,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 4);
-  /*******************************/
-	/*Configure the Gain Conversion */
-  /*******************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(0x00040000,devpriv->iobase+i_Offset + 12); */
-	outl(0x00040000,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 12);
-
-  /*******************************/
-	/*Initialise dw_CommandRegister */
-  /*******************************/
-	ui_CommandRegister = 0;
-	/*Test if the interrupt is enable */
-	if (s_BoardInfos[dev->minor].i_InterruptFlag == 1) {
-		/*Enable the interrupt */
-		ui_CommandRegister = ui_CommandRegister | 0x00100000;
-	}
-  /**********************/
-	/*Start the conversion */
-  /**********************/
-	ui_CommandRegister = ui_CommandRegister | 0x00080000;
-  /***************************/
-	/*Write the command regiter */
-  /***************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(ui_CommandRegister ,devpriv->iobase+i_Offset + 8); */
-	outl(ui_CommandRegister,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 8);
-	if (s_BoardInfos[dev->minor].i_InterruptFlag == 0) {
-		do {
-	  /*******************/
-			/*Read the EOC flag */
-	  /*******************/
-			/* ui_EOC = inl(devpriv->iobase+i_Offset + 20) & 1; */
-			ui_EOC = inl(devpriv->iobase +
-				s_BoardInfos[dev->minor].i_Offset + 20) & 1;
-		} while (ui_EOC != 1);
-      /************************************************/
-		/*Read the digital value of the calibration Gain */
-      /************************************************/
-		/* data[0] = inl (devpriv->iobase+i_Offset + 28); */
-		data[0] =
-			inl(devpriv->iobase +
-			s_BoardInfos[dev->minor].i_Offset + 28);
-	}
-	return 0;
-}
-
-static int apci3200_reset(struct comedi_device *dev)
-{
-	struct addi_private *devpriv = dev->private;
-	int i_Temp;
-	unsigned int dw_Dummy;
-
-	/* i_InterruptFlag=0; */
-	/* i_Initialised==0; */
-	/* i_Count=0; */
-	/* i_Sum=0; */
-
-	s_BoardInfos[dev->minor].i_InterruptFlag = 0;
-	s_BoardInfos[dev->minor].i_Initialised = 0;
-	s_BoardInfos[dev->minor].i_Count = 0;
-	s_BoardInfos[dev->minor].i_Sum = 0;
-	s_BoardInfos[dev->minor].b_StructInitialized = 0;
-
-	outl(0x83838383, devpriv->i_IobaseAmcc + 0x60);
-
-	/*  Enable the interrupt for the controller */
-	dw_Dummy = inl(devpriv->i_IobaseAmcc + 0x38);
-	outl(dw_Dummy | 0x2000, devpriv->i_IobaseAmcc + 0x38);
-	outl(0, devpriv->i_IobaseAddon);	/* Resets the output */
-  /***************/
-	/*Empty the buffer */
-  /**************/
-	for (i_Temp = 0; i_Temp <= 95; i_Temp++) {
-		/* ui_InterruptChannelValue[i_Temp]=0; */
-		s_BoardInfos[dev->minor].ui_InterruptChannelValue[i_Temp] = 0;
-	}			/* for(i_Temp=0;i_Temp<=95;i_Temp++) */
-  /*****************************/
-	/*Reset the START and IRQ bit */
-  /*****************************/
-	for (i_Temp = 0; i_Temp <= 192;) {
-		while (((inl(devpriv->iobase + i_Temp + 12) >> 19) & 1) != 1) ;
-		outl(0, devpriv->iobase + i_Temp + 8);
-		i_Temp = i_Temp + 64;
-	}			/* for(i_Temp=0;i_Temp<=192;i_Temp+64) */
-	return 0;
-}
-
-/*
- * Read value of the selected channel
- *
- * data[0]  : Digital Value Of Input
- * data[1]  : Calibration Offset Value
- * data[2]  : Calibration Gain Value
- * data[3]  : CJC value
- * data[4]  : CJC offset value
- * data[5]  : CJC gain value
- * data[6] : CJC current source from eeprom
- * data[7] : Channel current source from eeprom
- * data[8] : Channle gain factor from eeprom
- */
-static int apci3200_ai_read(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn,
-			    unsigned int *data)
-{
-	unsigned int ui_DummyValue = 0;
-	int i_ConvertCJCCalibration;
-	int i = 0;
-
-	/* BEGIN JK 06.07.04: Management of sevrals boards */
-	/* if(i_Initialised==0) */
-	if (s_BoardInfos[dev->minor].i_Initialised == 0)
-		/* END JK 06.07.04: Management of sevrals boards */
-	{
-		apci3200_reset(dev);
-		return -EINVAL;
-	}			/* if(i_Initialised==0); */
-
-	switch (insn->unused[0]) {
-	case 0:
-
-		i_APCI3200_Read1AnalogInputChannel(dev, s, insn,
-			&ui_DummyValue);
-		/* BEGIN JK 06.07.04: Management of sevrals boards */
-		/* ui_InterruptChannelValue[i_Count+0]=ui_DummyValue; */
-		s_BoardInfos[dev->minor].
-			ui_InterruptChannelValue[s_BoardInfos[dev->minor].
-			i_Count + 0] = ui_DummyValue;
-		/* END JK 06.07.04: Management of sevrals boards */
-
-		/* Begin JK 25.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-		i_APCI3200_GetChannelCalibrationValue(dev,
-			s_BoardInfos[dev->minor].ui_Channel_num,
-			&s_BoardInfos[dev->minor].
-			ui_InterruptChannelValue[s_BoardInfos[dev->minor].
-				i_Count + 6],
-			&s_BoardInfos[dev->minor].
-			ui_InterruptChannelValue[s_BoardInfos[dev->minor].
-				i_Count + 7],
-			&s_BoardInfos[dev->minor].
-			ui_InterruptChannelValue[s_BoardInfos[dev->minor].
-				i_Count + 8]);
-		/* End JK 25.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-
-		/* BEGIN JK 06.07.04: Management of sevrals boards */
-		/* if((i_ADDIDATAType==2) && (i_InterruptFlag == FALSE) && (i_CJCAvailable==1)) */
-		if ((s_BoardInfos[dev->minor].i_ADDIDATAType == 2)
-			&& (s_BoardInfos[dev->minor].i_InterruptFlag == FALSE)
-			&& (s_BoardInfos[dev->minor].i_CJCAvailable == 1))
-			/* END JK 06.07.04: Management of sevrals boards */
-		{
-			i_APCI3200_ReadCJCValue(dev, &ui_DummyValue);
-			/* BEGIN JK 06.07.04: Management of sevrals boards */
-			/* ui_InterruptChannelValue[i_Count + 3]=ui_DummyValue; */
-			s_BoardInfos[dev->minor].
-				ui_InterruptChannelValue[s_BoardInfos[dev->
-					minor].i_Count + 3] = ui_DummyValue;
-			/* END JK 06.07.04: Management of sevrals boards */
-		}		/* if((i_ADDIDATAType==2) && (i_InterruptFlag == FALSE)) */
-		else {
-			/* BEGIN JK 06.07.04: Management of sevrals boards */
-			/* ui_InterruptChannelValue[i_Count + 3]=0; */
-			s_BoardInfos[dev->minor].
-				ui_InterruptChannelValue[s_BoardInfos[dev->
-					minor].i_Count + 3] = 0;
-			/* END JK 06.07.04: Management of sevrals boards */
-		}		/* elseif((i_ADDIDATAType==2) && (i_InterruptFlag == FALSE) && (i_CJCAvailable==1)) */
-
-		/* BEGIN JK 06.07.04: Management of sevrals boards */
-		/* if (( i_AutoCalibration == FALSE) && (i_InterruptFlag == FALSE)) */
-		if ((s_BoardInfos[dev->minor].i_AutoCalibration == FALSE)
-			&& (s_BoardInfos[dev->minor].i_InterruptFlag == FALSE))
-			/* END JK 06.07.04: Management of sevrals boards */
-		{
-			i_APCI3200_ReadCalibrationOffsetValue(dev,
-				&ui_DummyValue);
-			/* BEGIN JK 06.07.04: Management of sevrals boards */
-			/* ui_InterruptChannelValue[i_Count + 1]=ui_DummyValue; */
-			s_BoardInfos[dev->minor].
-				ui_InterruptChannelValue[s_BoardInfos[dev->
-					minor].i_Count + 1] = ui_DummyValue;
-			/* END JK 06.07.04: Management of sevrals boards */
-			i_APCI3200_ReadCalibrationGainValue(dev,
-				&ui_DummyValue);
-			/* BEGIN JK 06.07.04: Management of sevrals boards */
-			/* ui_InterruptChannelValue[i_Count + 2]=ui_DummyValue; */
-			s_BoardInfos[dev->minor].
-				ui_InterruptChannelValue[s_BoardInfos[dev->
-					minor].i_Count + 2] = ui_DummyValue;
-			/* END JK 06.07.04: Management of sevrals boards */
-		}		/* if (( i_AutoCalibration == FALSE) && (i_InterruptFlag == FALSE)) */
-
-		/* BEGIN JK 06.07.04: Management of sevrals boards */
-		/* if((i_ADDIDATAType==2) && (i_InterruptFlag == FALSE)&& (i_CJCAvailable==1)) */
-		if ((s_BoardInfos[dev->minor].i_ADDIDATAType == 2)
-			&& (s_BoardInfos[dev->minor].i_InterruptFlag == FALSE)
-			&& (s_BoardInfos[dev->minor].i_CJCAvailable == 1))
-			/* END JK 06.07.04: Management of sevrals boards */
-		{
-	  /**********************************************************/
-			/*Test if the Calibration channel must be read for the CJC */
-	  /**********************************************************/
-	  /**********************************/
-			/*Test if the polarity is the same */
-	  /**********************************/
-			/* BEGIN JK 06.07.04: Management of sevrals boards */
-			/* if(i_CJCPolarity!=i_ADDIDATAPolarity) */
-			if (s_BoardInfos[dev->minor].i_CJCPolarity !=
-				s_BoardInfos[dev->minor].i_ADDIDATAPolarity)
-				/* END JK 06.07.04: Management of sevrals boards */
-			{
-				i_ConvertCJCCalibration = 1;
-			}	/* if(i_CJCPolarity!=i_ADDIDATAPolarity) */
-			else {
-				/* BEGIN JK 06.07.04: Management of sevrals boards */
-				/* if(i_CJCGain==i_ADDIDATAGain) */
-				if (s_BoardInfos[dev->minor].i_CJCGain ==
-					s_BoardInfos[dev->minor].i_ADDIDATAGain)
-					/* END JK 06.07.04: Management of sevrals boards */
-				{
-					i_ConvertCJCCalibration = 0;
-				}	/* if(i_CJCGain==i_ADDIDATAGain) */
-				else {
-					i_ConvertCJCCalibration = 1;
-				}	/* elseif(i_CJCGain==i_ADDIDATAGain) */
-			}	/* elseif(i_CJCPolarity!=i_ADDIDATAPolarity) */
-			if (i_ConvertCJCCalibration == 1) {
-				i_APCI3200_ReadCJCCalOffset(dev,
-					&ui_DummyValue);
-				/* BEGIN JK 06.07.04: Management of sevrals boards */
-				/* ui_InterruptChannelValue[i_Count+4]=ui_DummyValue; */
-				s_BoardInfos[dev->minor].
-					ui_InterruptChannelValue[s_BoardInfos
-					[dev->minor].i_Count + 4] =
-					ui_DummyValue;
-				/* END JK 06.07.04: Management of sevrals boards */
-
-				i_APCI3200_ReadCJCCalGain(dev, &ui_DummyValue);
-
-				/* BEGIN JK 06.07.04: Management of sevrals boards */
-				/* ui_InterruptChannelValue[i_Count+5]=ui_DummyValue; */
-				s_BoardInfos[dev->minor].
-					ui_InterruptChannelValue[s_BoardInfos
-					[dev->minor].i_Count + 5] =
-					ui_DummyValue;
-				/* END JK 06.07.04: Management of sevrals boards */
-			}	/* if(i_ConvertCJCCalibration==1) */
-			else {
-				/* BEGIN JK 06.07.04: Management of sevrals boards */
-				/* ui_InterruptChannelValue[i_Count+4]=0; */
-				/* ui_InterruptChannelValue[i_Count+5]=0; */
-
-				s_BoardInfos[dev->minor].
-					ui_InterruptChannelValue[s_BoardInfos
-					[dev->minor].i_Count + 4] = 0;
-				s_BoardInfos[dev->minor].
-					ui_InterruptChannelValue[s_BoardInfos
-					[dev->minor].i_Count + 5] = 0;
-				/* END JK 06.07.04: Management of sevrals boards */
-			}	/* elseif(i_ConvertCJCCalibration==1) */
-		}		/* if((i_ADDIDATAType==2) && (i_InterruptFlag == FALSE)) */
-
-		/* BEGIN JK 06.07.04: Management of sevrals boards */
-		/* if(i_ScanType!=1) */
-		if (s_BoardInfos[dev->minor].i_ScanType != 1) {
-			/* i_Count=0; */
-			s_BoardInfos[dev->minor].i_Count = 0;
-		}		/* if(i_ScanType!=1) */
-		else {
-			/* i_Count=i_Count +6; */
-			/* Begin JK 22.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-			/* s_BoardInfos [dev->minor].i_Count=s_BoardInfos [dev->minor].i_Count +6; */
-			s_BoardInfos[dev->minor].i_Count =
-				s_BoardInfos[dev->minor].i_Count + 9;
-			/* End JK 22.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-		}		/* else if(i_ScanType!=1) */
-
-		/* if((i_ScanType==1) &&(i_InterruptFlag==1)) */
-		if ((s_BoardInfos[dev->minor].i_ScanType == 1)
-			&& (s_BoardInfos[dev->minor].i_InterruptFlag == 1)) {
-			/* i_Count=i_Count-6; */
-			/* Begin JK 22.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-			/* s_BoardInfos [dev->minor].i_Count=s_BoardInfos [dev->minor].i_Count-6; */
-			s_BoardInfos[dev->minor].i_Count =
-				s_BoardInfos[dev->minor].i_Count - 9;
-			/* End JK 22.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-		}
-		/* if(i_ScanType==0) */
-		if (s_BoardInfos[dev->minor].i_ScanType == 0) {
-			/*
-			   data[0]= ui_InterruptChannelValue[0];
-			   data[1]= ui_InterruptChannelValue[1];
-			   data[2]= ui_InterruptChannelValue[2];
-			   data[3]= ui_InterruptChannelValue[3];
-			   data[4]= ui_InterruptChannelValue[4];
-			   data[5]= ui_InterruptChannelValue[5];
-			 */
-			data[0] =
-				s_BoardInfos[dev->minor].
-				ui_InterruptChannelValue[0];
-			data[1] =
-				s_BoardInfos[dev->minor].
-				ui_InterruptChannelValue[1];
-			data[2] =
-				s_BoardInfos[dev->minor].
-				ui_InterruptChannelValue[2];
-			data[3] =
-				s_BoardInfos[dev->minor].
-				ui_InterruptChannelValue[3];
-			data[4] =
-				s_BoardInfos[dev->minor].
-				ui_InterruptChannelValue[4];
-			data[5] =
-				s_BoardInfos[dev->minor].
-				ui_InterruptChannelValue[5];
-
-			/* Begin JK 22.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-			i_APCI3200_GetChannelCalibrationValue(dev,
-				s_BoardInfos[dev->minor].ui_Channel_num,
-				&data[6], &data[7], &data[8]);
-			/* End JK 22.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-		}
-		break;
-	case 1:
-
-		for (i = 0; i < insn->n; i++) {
-			/* data[i]=ui_InterruptChannelValue[i]; */
-			data[i] =
-				s_BoardInfos[dev->minor].
-				ui_InterruptChannelValue[i];
-		}
-
-		/* i_Count=0; */
-		/* i_Sum=0; */
-		/* if(i_ScanType==1) */
-		s_BoardInfos[dev->minor].i_Count = 0;
-		s_BoardInfos[dev->minor].i_Sum = 0;
-		if (s_BoardInfos[dev->minor].i_ScanType == 1) {
-			/* i_Initialised=0; */
-			/* i_InterruptFlag=0; */
-			s_BoardInfos[dev->minor].i_Initialised = 0;
-			s_BoardInfos[dev->minor].i_InterruptFlag = 0;
-			/* END JK 06.07.04: Management of sevrals boards */
-		}
-		break;
-	default:
-		printk("\nThe parameters passed are in error\n");
-		apci3200_reset(dev);
-		return -EINVAL;
-	}			/* switch(insn->unused[0]) */
-
-	return insn->n;
-}
-
-/*
- * Configures The Analog Input Subdevice
- *
- * data[0]  = 0  Normal AI
- *	    = 1  RTD
- *	    = 2  THERMOCOUPLE
- * data[1]  = Gain To Use
- * data[2]  = 0  Bipolar
- *	    = 1  Unipolar
- * data[3]  = Offset Range
- * data[4]  = 0  DC Coupling
- *	    = 1  AC Coupling
- * data[5]  = 0  Single
- *	    = 1  Differential
- * data[6]  = TimerReloadValue
- * data[7]  = ConvertingTimeUnit
- * data[8]  = 0  Analog voltage measurement
- *	    = 1  Resistance measurement
- *	    = 2  Temperature measurement
- * data[9]  = 0  Interrupt Disable
- *	    = 1  INterrupt Enable
- * data[10] = Type of Thermocouple
- * data[11] = single channel Module Number
- * data[12] = 0  Single Read
- *	    = 1  Read more channel
- *	    = 2  Single scan
- *	    = 3  Continuous Scan
- * data[13] = Number of channels to read
- * data[14] = 0  RTD not used
- *	    = 1  RTD 2 wire connection
- *	    = 2  RTD 3 wire connection
- *	    = 3  RTD 4 wire connection
- */
-static int apci3200_ai_config(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn,
-			      unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-	unsigned int ul_Config = 0, ul_Temp = 0;
-	unsigned int ui_ChannelNo = 0;
-	unsigned int ui_Dummy = 0;
-	int i_err = 0;
-
-	/* BEGIN JK 06.07.04: Management of sevrals boards */
-	/*  Initialize the structure */
-	if (s_BoardInfos[dev->minor].b_StructInitialized != 1) {
-		s_BoardInfos[dev->minor].i_CJCAvailable = 1;
-		s_BoardInfos[dev->minor].i_CJCPolarity = 0;
-		s_BoardInfos[dev->minor].i_CJCGain = 2;	/* changed from 0 to 2 */
-		s_BoardInfos[dev->minor].i_InterruptFlag = 0;
-		s_BoardInfos[dev->minor].i_AutoCalibration = 0;	/* : auto calibration */
-		s_BoardInfos[dev->minor].i_ChannelCount = 0;
-		s_BoardInfos[dev->minor].i_Sum = 0;
-		s_BoardInfos[dev->minor].ui_Channel_num = 0;
-		s_BoardInfos[dev->minor].i_Count = 0;
-		s_BoardInfos[dev->minor].i_Initialised = 0;
-		s_BoardInfos[dev->minor].b_StructInitialized = 1;
-
-		/* Begin JK 21.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-		s_BoardInfos[dev->minor].i_ConnectionType = 0;
-		/* End JK 21.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-
-		/* Begin JK 21.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-		memset(s_BoardInfos[dev->minor].s_Module, 0,
-			sizeof(s_BoardInfos[dev->minor].s_Module[MAX_MODULE]));
-
-		v_GetAPCI3200EepromCalibrationValue(devpriv->i_IobaseAmcc,
-			&s_BoardInfos[dev->minor]);
-		/* End JK 21.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-	}
-
-	if (data[0] != 0 && data[0] != 1 && data[0] != 2) {
-		printk("\nThe selection of acquisition type is in error\n");
-		i_err++;
-	}			/* if(data[0]!=0 && data[0]!=1 && data[0]!=2) */
-	if (data[0] == 1) {
-		if (data[14] != 0 && data[14] != 1 && data[14] != 2
-			&& data[14] != 4) {
-			printk("\n Error in selection of RTD connection type\n");
-			i_err++;
-		}		/* if(data[14]!=0 && data[14]!=1 && data[14]!=2 && data[14]!=4) */
-	}			/* if(data[0]==1 ) */
-	if (data[1] < 0 || data[1] > 7) {
-		printk("\nThe selection of gain is in error\n");
-		i_err++;
-	}			/*  if(data[1]<0 || data[1]>7) */
-	if (data[2] != 0 && data[2] != 1) {
-		printk("\nThe selection of polarity is in error\n");
-		i_err++;
-	}			/* if(data[2]!=0 &&  data[2]!=1) */
-	if (data[3] != 0) {
-		printk("\nThe selection of offset range  is in error\n");
-		i_err++;
-	}			/*  if(data[3]!=0) */
-	if (data[4] != 0 && data[4] != 1) {
-		printk("\nThe selection of coupling is in error\n");
-		i_err++;
-	}			/* if(data[4]!=0 &&  data[4]!=1) */
-	if (data[5] != 0 && data[5] != 1) {
-		printk("\nThe selection of single/differential mode is in error\n");
-		i_err++;
-	}			/* if(data[5]!=0 &&  data[5]!=1) */
-	if (data[8] != 0 && data[8] != 1 && data[2] != 2) {
-		printk("\nError in selection of functionality\n");
-	}			/* if(data[8]!=0 && data[8]!=1 && data[2]!=2) */
-	if (data[12] == 0 || data[12] == 1) {
-		if (data[6] != 20 && data[6] != 40 && data[6] != 80
-			&& data[6] != 160) {
-			printk("\nThe selection of conversion time reload value is in error\n");
-			i_err++;
-		}		/*  if (data[6]!=20 && data[6]!=40 && data[6]!=80 && data[6]!=160 ) */
-		if (data[7] != 2) {
-			printk("\nThe selection of conversion time unit  is in error\n");
-			i_err++;
-		}		/*  if(data[7]!=2) */
-	}
-	if (data[9] != 0 && data[9] != 1) {
-		printk("\nThe selection of interrupt enable is in error\n");
-		i_err++;
-	}			/* if(data[9]!=0 &&  data[9]!=1) */
-	if (data[11] < 0 || data[11] > 4) {
-		printk("\nThe selection of module is in error\n");
-		i_err++;
-	}			/* if(data[11] <0 ||  data[11]>1) */
-	if (data[12] < 0 || data[12] > 3) {
-		printk("\nThe selection of singlechannel/scan selection is in error\n");
-		i_err++;
-	}			/* if(data[12] < 0 ||  data[12]> 3) */
-	if (data[13] < 0 || data[13] > 16) {
-		printk("\nThe selection of number of channels is in error\n");
-		i_err++;
-	}			/*  if(data[13] <0 ||data[13] >15) */
-
-	/* BEGIN JK 06.07.04: Management of sevrals boards */
-	/*
-	   i_ChannelCount=data[13];
-	   i_ScanType=data[12];
-	   i_ADDIDATAPolarity = data[2];
-	   i_ADDIDATAGain=data[1];
-	   i_ADDIDATAConversionTime=data[6];
-	   i_ADDIDATAConversionTimeUnit=data[7];
-	   i_ADDIDATAType=data[0];
-	 */
-
-	/*  Save acquisition configuration for the actual board */
-	s_BoardInfos[dev->minor].i_ChannelCount = data[13];
-	s_BoardInfos[dev->minor].i_ScanType = data[12];
-	s_BoardInfos[dev->minor].i_ADDIDATAPolarity = data[2];
-	s_BoardInfos[dev->minor].i_ADDIDATAGain = data[1];
-	s_BoardInfos[dev->minor].i_ADDIDATAConversionTime = data[6];
-	s_BoardInfos[dev->minor].i_ADDIDATAConversionTimeUnit = data[7];
-	s_BoardInfos[dev->minor].i_ADDIDATAType = data[0];
-	/* Begin JK 19.10.2004: APCI-3200 Driver update 0.7.57 -> 0.7.68 */
-	s_BoardInfos[dev->minor].i_ConnectionType = data[5];
-	/* End JK 19.10.2004: APCI-3200 Driver update 0.7.57 -> 0.7.68 */
-	/* END JK 06.07.04: Management of sevrals boards */
-
-	/* Begin JK 19.10.2004: APCI-3200 Driver update 0.7.57 -> 0.7.68 */
-	memset(s_BoardInfos[dev->minor].ui_ScanValueArray, 0, (7 + 12) * sizeof(unsigned int));	/*  7 is the maximal number of channels */
-	/* End JK 19.10.2004: APCI-3200 Driver update 0.7.57 -> 0.7.68 */
-
-	/* BEGIN JK 02.07.04 : This while can't be do, it block the process when using severals boards */
-	/* while(i_InterruptFlag==1) */
-	while (s_BoardInfos[dev->minor].i_InterruptFlag == 1) {
-#ifndef MSXBOX
-		udelay(1);
-#else
-		/*  In the case where the driver is compiled for the MSX-Box */
-		/*  we used a printk to have a little delay because udelay */
-		/*  seems to be broken under the MSX-Box. */
-		/*  This solution hat to be studied. */
-		printk("");
-#endif
-	}
-	/* END JK 02.07.04 : This while can't be do, it block the process when using severals boards */
-
-	ui_ChannelNo = CR_CHAN(insn->chanspec);	/*  get the channel */
-	/* BEGIN JK 06.07.04: Management of sevrals boards */
-	/* i_ChannelNo=ui_ChannelNo; */
-	/* ui_Channel_num =ui_ChannelNo; */
-
-	s_BoardInfos[dev->minor].i_ChannelNo = ui_ChannelNo;
-	s_BoardInfos[dev->minor].ui_Channel_num = ui_ChannelNo;
-
-	/* END JK 06.07.04: Management of sevrals boards */
-
-	if (data[5] == 0) {
-		if (ui_ChannelNo > 15) {
-			printk("\nThe Selection of the channel is in error\n");
-			i_err++;
-		}		/*  if(ui_ChannelNo>15) */
-	}			/* if(data[5]==0) */
-	else {
-		if (data[14] == 2) {
-			if (ui_ChannelNo > 3) {
-				printk("\nThe Selection of the channel is in error\n");
-				i_err++;
-			}	/*  if(ui_ChannelNo>3) */
-		}		/* if(data[14]==2) */
-		else {
-			if (ui_ChannelNo > 7) {
-				printk("\nThe Selection of the channel is in error\n");
-				i_err++;
-			}	/*  if(ui_ChannelNo>7) */
-		}		/* elseif(data[14]==2) */
-	}			/* elseif(data[5]==0) */
-	if (data[12] == 0 || data[12] == 1) {
-		switch (data[5]) {
-		case 0:
-			if (ui_ChannelNo <= 3) {
-				/* BEGIN JK 06.07.04: Management of sevrals boards */
-				/* i_Offset=0; */
-				s_BoardInfos[dev->minor].i_Offset = 0;
-				/* END JK 06.07.04: Management of sevrals boards */
-			}	/* if(ui_ChannelNo <=3) */
-			if (ui_ChannelNo >= 4 && ui_ChannelNo <= 7) {
-				/* BEGIN JK 06.07.04: Management of sevrals boards */
-				/* i_Offset=64; */
-				s_BoardInfos[dev->minor].i_Offset = 64;
-				/* END JK 06.07.04: Management of sevrals boards */
-			}	/* if(ui_ChannelNo >=4 && ui_ChannelNo <=7) */
-			if (ui_ChannelNo >= 8 && ui_ChannelNo <= 11) {
-				/* BEGIN JK 06.07.04: Management of sevrals boards */
-				/* i_Offset=128; */
-				s_BoardInfos[dev->minor].i_Offset = 128;
-				/* END JK 06.07.04: Management of sevrals boards */
-			}	/* if(ui_ChannelNo >=8 && ui_ChannelNo <=11) */
-			if (ui_ChannelNo >= 12 && ui_ChannelNo <= 15) {
-				/* BEGIN JK 06.07.04: Management of sevrals boards */
-				/* i_Offset=192; */
-				s_BoardInfos[dev->minor].i_Offset = 192;
-				/* END JK 06.07.04: Management of sevrals boards */
-			}	/* if(ui_ChannelNo >=12 && ui_ChannelNo <=15) */
-			break;
-		case 1:
-			if (data[14] == 2) {
-				if (ui_ChannelNo == 0) {
-					/* BEGIN JK 06.07.04: Management of sevrals boards */
-					/* i_Offset=0; */
-					s_BoardInfos[dev->minor].i_Offset = 0;
-					/* END JK 06.07.04: Management of sevrals boards */
-				}	/* if(ui_ChannelNo ==0 ) */
-				if (ui_ChannelNo == 1) {
-					/* BEGIN JK 06.07.04: Management of sevrals boards */
-					/* i_Offset=0; */
-					s_BoardInfos[dev->minor].i_Offset = 64;
-					/* END JK 06.07.04: Management of sevrals boards */
-				}	/*  if(ui_ChannelNo ==1) */
-				if (ui_ChannelNo == 2) {
-					/* BEGIN JK 06.07.04: Management of sevrals boards */
-					/* i_Offset=128; */
-					s_BoardInfos[dev->minor].i_Offset = 128;
-					/* END JK 06.07.04: Management of sevrals boards */
-				}	/* if(ui_ChannelNo ==2 ) */
-				if (ui_ChannelNo == 3) {
-					/* BEGIN JK 06.07.04: Management of sevrals boards */
-					/* i_Offset=192; */
-					s_BoardInfos[dev->minor].i_Offset = 192;
-					/* END JK 06.07.04: Management of sevrals boards */
-				}	/* if(ui_ChannelNo ==3) */
-
-				/* BEGIN JK 06.07.04: Management of sevrals boards */
-				/* i_ChannelNo=0; */
-				s_BoardInfos[dev->minor].i_ChannelNo = 0;
-				/* END JK 06.07.04: Management of sevrals boards */
-				ui_ChannelNo = 0;
-				break;
-			}	/* if(data[14]==2) */
-			if (ui_ChannelNo <= 1) {
-				/* BEGIN JK 06.07.04: Management of sevrals boards */
-				/* i_Offset=0; */
-				s_BoardInfos[dev->minor].i_Offset = 0;
-				/* END JK 06.07.04: Management of sevrals boards */
-			}	/* if(ui_ChannelNo <=1) */
-			if (ui_ChannelNo >= 2 && ui_ChannelNo <= 3) {
-				/* BEGIN JK 06.07.04: Management of sevrals boards */
-				/* i_ChannelNo=i_ChannelNo-2; */
-				/* i_Offset=64; */
-				s_BoardInfos[dev->minor].i_ChannelNo =
-					s_BoardInfos[dev->minor].i_ChannelNo -
-					2;
-				s_BoardInfos[dev->minor].i_Offset = 64;
-				/* END JK 06.07.04: Management of sevrals boards */
-				ui_ChannelNo = ui_ChannelNo - 2;
-			}	/* if(ui_ChannelNo >=2 && ui_ChannelNo <=3) */
-			if (ui_ChannelNo >= 4 && ui_ChannelNo <= 5) {
-				/* BEGIN JK 06.07.04: Management of sevrals boards */
-				/* i_ChannelNo=i_ChannelNo-4; */
-				/* i_Offset=128; */
-				s_BoardInfos[dev->minor].i_ChannelNo =
-					s_BoardInfos[dev->minor].i_ChannelNo -
-					4;
-				s_BoardInfos[dev->minor].i_Offset = 128;
-				/* END JK 06.07.04: Management of sevrals boards */
-				ui_ChannelNo = ui_ChannelNo - 4;
-			}	/* if(ui_ChannelNo >=4 && ui_ChannelNo <=5) */
-			if (ui_ChannelNo >= 6 && ui_ChannelNo <= 7) {
-				/* BEGIN JK 06.07.04: Management of sevrals boards */
-				/* i_ChannelNo=i_ChannelNo-6; */
-				/* i_Offset=192; */
-				s_BoardInfos[dev->minor].i_ChannelNo =
-					s_BoardInfos[dev->minor].i_ChannelNo -
-					6;
-				s_BoardInfos[dev->minor].i_Offset = 192;
-				/* END JK 06.07.04: Management of sevrals boards */
-				ui_ChannelNo = ui_ChannelNo - 6;
-			}	/* if(ui_ChannelNo >=6 && ui_ChannelNo <=7) */
-			break;
-
-		default:
-			printk("\n This selection of polarity does not exist\n");
-			i_err++;
-		}		/* switch(data[2]) */
-	}			/* if(data[12]==0 || data[12]==1) */
-	else {
-		switch (data[11]) {
-		case 1:
-			/* BEGIN JK 06.07.04: Management of sevrals boards */
-			/* i_Offset=0; */
-			s_BoardInfos[dev->minor].i_Offset = 0;
-			/* END JK 06.07.04: Management of sevrals boards */
-			break;
-		case 2:
-			/* BEGIN JK 06.07.04: Management of sevrals boards */
-			/* i_Offset=64; */
-			s_BoardInfos[dev->minor].i_Offset = 64;
-			/* END JK 06.07.04: Management of sevrals boards */
-			break;
-		case 3:
-			/* BEGIN JK 06.07.04: Management of sevrals boards */
-			/* i_Offset=128; */
-			s_BoardInfos[dev->minor].i_Offset = 128;
-			/* END JK 06.07.04: Management of sevrals boards */
-			break;
-		case 4:
-			/* BEGIN JK 06.07.04: Management of sevrals boards */
-			/* i_Offset=192; */
-			s_BoardInfos[dev->minor].i_Offset = 192;
-			/* END JK 06.07.04: Management of sevrals boards */
-			break;
-		default:
-			printk("\nError in module selection\n");
-			i_err++;
-		}		/*  switch(data[11]) */
-	}			/*  elseif(data[12]==0 || data[12]==1) */
-	if (i_err) {
-		apci3200_reset(dev);
-		return -EINVAL;
-	}
-	/* if(i_ScanType!=1) */
-	if (s_BoardInfos[dev->minor].i_ScanType != 1) {
-		/* BEGIN JK 06.07.04: Management of sevrals boards */
-		/* i_Count=0; */
-		/* i_Sum=0; */
-		s_BoardInfos[dev->minor].i_Count = 0;
-		s_BoardInfos[dev->minor].i_Sum = 0;
-		/* END JK 06.07.04: Management of sevrals boards */
-	}			/* if(i_ScanType!=1) */
-
-	ul_Config =
-		data[1] | (data[2] << 6) | (data[5] << 7) | (data[3] << 8) |
-		(data[4] << 9);
-	/* BEGIN JK 06.07.04: Management of sevrals boards */
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* END JK 06.07.04: Management of sevrals boards */
-  /*********************************/
-	/* Write the channel to configure */
-  /*********************************/
-	/* BEGIN JK 06.07.04: Management of sevrals boards */
-	/* outl(0 | ui_ChannelNo , devpriv->iobase+i_Offset + 0x4); */
-	outl(0 | ui_ChannelNo,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 0x4);
-	/* END JK 06.07.04: Management of sevrals boards */
-
-	/* BEGIN JK 06.07.04: Management of sevrals boards */
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* END JK 06.07.04: Management of sevrals boards */
-  /**************************/
-	/* Reset the configuration */
-  /**************************/
-	/* BEGIN JK 06.07.04: Management of sevrals boards */
-	/* outl(0 , devpriv->iobase+i_Offset + 0x0); */
-	outl(0, devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 0x0);
-	/* END JK 06.07.04: Management of sevrals boards */
-
-	/* BEGIN JK 06.07.04: Management of sevrals boards */
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* END JK 06.07.04: Management of sevrals boards */
-
-  /***************************/
-	/* Write the configuration */
-  /***************************/
-	/* BEGIN JK 06.07.04: Management of sevrals boards */
-	/* outl(ul_Config , devpriv->iobase+i_Offset + 0x0); */
-	outl(ul_Config,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 0x0);
-	/* END JK 06.07.04: Management of sevrals boards */
-
-  /***************************/
-	/*Reset the calibration bit */
-  /***************************/
-	/* BEGIN JK 06.07.04: Management of sevrals boards */
-	/* ul_Temp = inl(devpriv->iobase+i_Offset + 12); */
-	ul_Temp = inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 12);
-	/* END JK 06.07.04: Management of sevrals boards */
-
-	/* BEGIN JK 06.07.04: Management of sevrals boards */
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* END JK 06.07.04: Management of sevrals boards */
-
-	/* BEGIN JK 06.07.04: Management of sevrals boards */
-	/* outl((ul_Temp & 0xFFF9FFFF) , devpriv->iobase+.i_Offset + 12); */
-	outl((ul_Temp & 0xFFF9FFFF),
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 12);
-	/* END JK 06.07.04: Management of sevrals boards */
-
-	if (data[9] == 1) {
-		devpriv->tsk_Current = current;
-		/* BEGIN JK 06.07.04: Management of sevrals boards */
-		/* i_InterruptFlag=1; */
-		s_BoardInfos[dev->minor].i_InterruptFlag = 1;
-		/* END JK 06.07.04: Management of sevrals boards */
-	}			/*  if(data[9]==1) */
-	else {
-		/* BEGIN JK 06.07.04: Management of sevrals boards */
-		/* i_InterruptFlag=0; */
-		s_BoardInfos[dev->minor].i_InterruptFlag = 0;
-		/* END JK 06.07.04: Management of sevrals boards */
-	}			/* else  if(data[9]==1) */
-
-	/* BEGIN JK 06.07.04: Management of sevrals boards */
-	/* i_Initialised=1; */
-	s_BoardInfos[dev->minor].i_Initialised = 1;
-	/* END JK 06.07.04: Management of sevrals boards */
-
-	/* BEGIN JK 06.07.04: Management of sevrals boards */
-	/* if(i_ScanType==1) */
-	if (s_BoardInfos[dev->minor].i_ScanType == 1)
-		/* END JK 06.07.04: Management of sevrals boards */
-	{
-		/* BEGIN JK 06.07.04: Management of sevrals boards */
-		/* i_Sum=i_Sum+1; */
-		s_BoardInfos[dev->minor].i_Sum =
-			s_BoardInfos[dev->minor].i_Sum + 1;
-		/* END JK 06.07.04: Management of sevrals boards */
-
-		insn->unused[0] = 0;
-		apci3200_ai_read(dev, s, insn, &ui_Dummy);
-	}
-
-	return insn->n;
-}
-
-/*
- * Tests the Selected Anlog Input Channel
- *
- * data[0] = 0  TestAnalogInputShortCircuit
- *	   = 1  TestAnalogInputConnection
- *
- * data[0] : Digital value obtained
- * data[1] : calibration offset
- * data[2] : calibration gain
- */
-static int apci3200_ai_bits_test(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn,
-				 unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-	unsigned int ui_Configuration = 0;
-	int i_Temp;		/* ,i_TimeUnit; */
-
-	/* if(i_Initialised==0) */
-
-	if (s_BoardInfos[dev->minor].i_Initialised == 0) {
-		apci3200_reset(dev);
-		return -EINVAL;
-	}			/* if(i_Initialised==0); */
-	if (data[0] != 0 && data[0] != 1) {
-		printk("\nError in selection of functionality\n");
-		apci3200_reset(dev);
-		return -EINVAL;
-	}			/* if(data[0]!=0 && data[0]!=1) */
-
-	if (data[0] == 1)	/* Perform Short Circuit TEST */
-	{
-      /**************************/
-		/*Set the short-cicuit bit */
-      /**************************/
-		/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-		while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].
-						i_Offset + 12) >> 19) & 1) !=
-			1) ;
-		/* outl((0x00001000 |i_ChannelNo) , devpriv->iobase+i_Offset + 4); */
-		outl((0x00001000 | s_BoardInfos[dev->minor].i_ChannelNo),
-			devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-			4);
-      /*************************/
-		/*Set the time unit to ns */
-      /*************************/
-		/* i_TimeUnit= i_ADDIDATAConversionTimeUnit;
-		   i_ADDIDATAConversionTimeUnit= 1; */
-		/* i_Temp= i_InterruptFlag ; */
-		i_Temp = s_BoardInfos[dev->minor].i_InterruptFlag;
-		s_BoardInfos[dev->minor].i_InterruptFlag = 0;
-		i_APCI3200_Read1AnalogInputChannel(dev, s, insn, data);
-		/* if(i_AutoCalibration == FALSE) */
-		if (s_BoardInfos[dev->minor].i_AutoCalibration == FALSE) {
-			/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-			while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].
-							i_Offset +
-							12) >> 19) & 1) != 1) ;
-
-			/* outl((0x00001000 |i_ChannelNo) , devpriv->iobase+i_Offset + 4); */
-			outl((0x00001000 | s_BoardInfos[dev->minor].
-					i_ChannelNo),
-				devpriv->iobase +
-				s_BoardInfos[dev->minor].i_Offset + 4);
-			data++;
-			i_APCI3200_ReadCalibrationOffsetValue(dev, data);
-			data++;
-			i_APCI3200_ReadCalibrationGainValue(dev, data);
-		}
-	} else {
-		/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-		while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].
-						i_Offset + 12) >> 19) & 1) !=
-			1) ;
-		/* outl((0x00000800|i_ChannelNo) , devpriv->iobase+i_Offset + 4); */
-		outl((0x00000800 | s_BoardInfos[dev->minor].i_ChannelNo),
-			devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-			4);
-		/* ui_Configuration = inl(devpriv->iobase+i_Offset + 0); */
-		ui_Configuration =
-			inl(devpriv->iobase +
-			s_BoardInfos[dev->minor].i_Offset + 0);
-      /*************************/
-		/*Set the time unit to ns */
-      /*************************/
-		/* i_TimeUnit= i_ADDIDATAConversionTimeUnit;
-		   i_ADDIDATAConversionTimeUnit= 1; */
-		/* i_Temp= i_InterruptFlag ; */
-		i_Temp = s_BoardInfos[dev->minor].i_InterruptFlag;
-		s_BoardInfos[dev->minor].i_InterruptFlag = 0;
-		i_APCI3200_Read1AnalogInputChannel(dev, s, insn, data);
-		/* if(i_AutoCalibration == FALSE) */
-		if (s_BoardInfos[dev->minor].i_AutoCalibration == FALSE) {
-			/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-			while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].
-							i_Offset +
-							12) >> 19) & 1) != 1) ;
-			/* outl((0x00000800|i_ChannelNo) , devpriv->iobase+i_Offset + 4); */
-			outl((0x00000800 | s_BoardInfos[dev->minor].
-					i_ChannelNo),
-				devpriv->iobase +
-				s_BoardInfos[dev->minor].i_Offset + 4);
-			data++;
-			i_APCI3200_ReadCalibrationOffsetValue(dev, data);
-			data++;
-			i_APCI3200_ReadCalibrationGainValue(dev, data);
-		}
-	}
-	/* i_InterruptFlag=i_Temp ; */
-	s_BoardInfos[dev->minor].i_InterruptFlag = i_Temp;
-	return insn->n;
-}
-
-static int apci3200_ai_write(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_insn *insn,
-			     unsigned int *data)
-{
-	apci3200_reset(dev);
-	return insn->n;
-}
-
-static int apci3200_ai_cmdtest(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_cmd *cmd)
-{
-
-	int err = 0;
-	unsigned int ui_ConvertTime = 0;
-	unsigned int ui_ConvertTimeBase = 0;
-	unsigned int ui_DelayTime = 0;
-	unsigned int ui_DelayTimeBase = 0;
-	int i_NbrOfChannel = 0;
-	int i_Cpt = 0;
-	double d_ConversionTimeForAllChannels = 0.0;
-	double d_SCANTimeNewUnit = 0.0;
-	unsigned int arg;
-
-	/* Step 1 : check if triggers are trivially valid */
-
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
-					TRIG_TIMER | TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
-
-	if (s_BoardInfos[dev->minor].i_InterruptFlag == 0)
-		err |= -EINVAL;
-
-	if (err) {
-		apci3200_reset(dev);
-		return 1;
-	}
-
-	/* Step 2a : make sure trigger sources are unique */
-
-	err |= cfc_check_trigger_is_unique(&cmd->start_src);
-	err |= cfc_check_trigger_is_unique(&cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(&cmd->stop_src);
-
-	/* Step 2b : and mutually compatible */
-
-	if (err) {
-		apci3200_reset(dev);
-		return 2;
-	}
-
-	/* Step 3: check if arguments are trivially valid */
-
-	switch (cmd->start_src) {
-	case TRIG_NOW:
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-		break;
-	case TRIG_EXT:
-		/* validate the trigger edge selection */
-		arg = cmd->start_arg & 0xffff;
-		if (arg < 1 || arg > 3) {
-			cmd->start_arg &= ~0xffff;
-			cmd->start_arg |= 1;
-			err |= -EINVAL;
-		}
-		/* validate the trigger mode selection */
-		arg = cmd->start_arg >> 16;
-		if (arg != 2) {
-			cmd->start_arg &= ~(0xffff << 16);
-			cmd->start_arg |= (2 << 16);
-			err |= -EINVAL;
-		}
-		break;
-	}
-
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
-
-	/* i_FirstChannel=cmd->chanlist[0]; */
-	s_BoardInfos[dev->minor].i_FirstChannel = cmd->chanlist[0];
-	/* i_LastChannel=cmd->chanlist[1]; */
-	s_BoardInfos[dev->minor].i_LastChannel = cmd->chanlist[1];
-
-	if (cmd->convert_src == TRIG_TIMER) {
-		ui_ConvertTime = cmd->convert_arg & 0xFFFF;
-		ui_ConvertTimeBase = cmd->convert_arg >> 16;
-		if (ui_ConvertTime != 20 && ui_ConvertTime != 40
-			&& ui_ConvertTime != 80 && ui_ConvertTime != 160)
-		{
-			printk("\nThe selection of conversion time reload value is in error\n");
-			err++;
-		}		/*  if (ui_ConvertTime!=20 && ui_ConvertTime!=40 && ui_ConvertTime!=80 && ui_ConvertTime!=160 ) */
-		if (ui_ConvertTimeBase != 2) {
-			printk("\nThe selection of conversion time unit  is in error\n");
-			err++;
-		}		/* if(ui_ConvertTimeBase!=2) */
-	} else {
-		ui_ConvertTime = 0;
-		ui_ConvertTimeBase = 0;
-	}
-	if (cmd->scan_begin_src == TRIG_FOLLOW) {
-		ui_DelayTime = 0;
-		ui_DelayTimeBase = 0;
-	}			/* if(cmd->scan_begin_src==TRIG_FOLLOW) */
-	else {
-		ui_DelayTime = cmd->scan_begin_arg & 0xFFFF;
-		ui_DelayTimeBase = cmd->scan_begin_arg >> 16;
-		if (ui_DelayTimeBase != 2 && ui_DelayTimeBase != 3) {
-			err++;
-			printk("\nThe Delay time base selection is in error\n");
-		}
-		if (ui_DelayTime < 1 || ui_DelayTime > 1023) {
-			err++;
-			printk("\nThe Delay time value is in error\n");
-		}
-		if (err) {
-			apci3200_reset(dev);
-			return 3;
-		}
-		fpu_begin();
-		d_SCANTimeNewUnit = (double)ui_DelayTime;
-		/* i_NbrOfChannel= i_LastChannel-i_FirstChannel + 4; */
-		i_NbrOfChannel =
-			s_BoardInfos[dev->minor].i_LastChannel -
-			s_BoardInfos[dev->minor].i_FirstChannel + 4;
-      /**********************************************************/
-		/*calculate the total conversion time for all the channels */
-      /**********************************************************/
-		d_ConversionTimeForAllChannels =
-			(double)((double)ui_ConvertTime /
-			(double)i_NbrOfChannel);
-
-      /*******************************/
-		/*Convert the frequence in time */
-      /*******************************/
-		d_ConversionTimeForAllChannels =
-			(double)1.0 / d_ConversionTimeForAllChannels;
-		ui_ConvertTimeBase = 3;
-      /***********************************/
-		/*Test if the time unit is the same */
-      /***********************************/
-
-		if (ui_DelayTimeBase <= ui_ConvertTimeBase) {
-
-			for (i_Cpt = 0;
-				i_Cpt < (ui_ConvertTimeBase - ui_DelayTimeBase);
-				i_Cpt++) {
-
-				d_ConversionTimeForAllChannels =
-					d_ConversionTimeForAllChannels * 1000;
-				d_ConversionTimeForAllChannels =
-					d_ConversionTimeForAllChannels + 1;
-			}
-		} else {
-			for (i_Cpt = 0;
-				i_Cpt < (ui_DelayTimeBase - ui_ConvertTimeBase);
-				i_Cpt++) {
-				d_SCANTimeNewUnit = d_SCANTimeNewUnit * 1000;
-
-			}
-		}
-
-		if (d_ConversionTimeForAllChannels >= d_SCANTimeNewUnit) {
-
-			printk("\nSCAN Delay value cannot be used\n");
-	  /*********************************/
-			/*SCAN Delay value cannot be used */
-	  /*********************************/
-			err++;
-		}
-		fpu_end();
-	}			/* else if(cmd->scan_begin_src==TRIG_FOLLOW) */
-
-	if (err) {
-		apci3200_reset(dev);
-		return 4;
-	}
-
-	return 0;
-}
-
-static int apci3200_cancel(struct comedi_device *dev,
-			   struct comedi_subdevice *s)
-{
-	struct addi_private *devpriv = dev->private;
-	unsigned int ui_Configuration = 0;
-
-	/* i_InterruptFlag=0; */
-	/* i_Initialised=0; */
-	/* i_Count=0; */
-	/* i_Sum=0; */
-	s_BoardInfos[dev->minor].i_InterruptFlag = 0;
-	s_BoardInfos[dev->minor].i_Initialised = 0;
-	s_BoardInfos[dev->minor].i_Count = 0;
-	s_BoardInfos[dev->minor].i_Sum = 0;
-
-  /*******************/
-	/*Read the register */
-  /*******************/
-	/* ui_Configuration = inl(devpriv->iobase+i_Offset + 8); */
-	ui_Configuration =
-		inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 8);
-  /*****************************/
-	/*Reset the START and IRQ bit */
-  /*****************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl((ui_Configuration & 0xFFE7FFFF),devpriv->iobase+i_Offset + 8); */
-	outl((ui_Configuration & 0xFFE7FFFF),
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 8);
-	return 0;
-}
-
-/*
- * Does asynchronous acquisition
- * Determines the mode 1 or 2.
- */
-static int apci3200_ai_cmd(struct comedi_device *dev,
-			   struct comedi_subdevice *s)
-{
-	struct addi_private *devpriv = dev->private;
-	struct comedi_cmd *cmd = &s->async->cmd;
-	unsigned int ui_Configuration = 0;
-	/* INT  i_CurrentSource = 0; */
-	unsigned int ui_Trigger = 0;
-	unsigned int ui_TriggerEdge = 0;
-	unsigned int ui_Triggermode = 0;
-	unsigned int ui_ScanMode = 0;
-	unsigned int ui_ConvertTime = 0;
-	unsigned int ui_ConvertTimeBase = 0;
-	unsigned int ui_DelayTime = 0;
-	unsigned int ui_DelayTimeBase = 0;
-	unsigned int ui_DelayMode = 0;
-
-	/* i_FirstChannel=cmd->chanlist[0]; */
-	/* i_LastChannel=cmd->chanlist[1]; */
-	s_BoardInfos[dev->minor].i_FirstChannel = cmd->chanlist[0];
-	s_BoardInfos[dev->minor].i_LastChannel = cmd->chanlist[1];
-	if (cmd->start_src == TRIG_EXT) {
-		ui_Trigger = 1;
-		ui_TriggerEdge = cmd->start_arg & 0xFFFF;
-		ui_Triggermode = cmd->start_arg >> 16;
-	}			/* if(cmd->start_src==TRIG_EXT) */
-	else {
-		ui_Trigger = 0;
-	}			/* elseif(cmd->start_src==TRIG_EXT) */
-
-	if (cmd->stop_src == TRIG_COUNT) {
-		ui_ScanMode = 0;
-	}			/*  if (cmd->stop_src==TRIG_COUNT) */
-	else {
-		ui_ScanMode = 2;
-	}			/* else if (cmd->stop_src==TRIG_COUNT) */
-
-	if (cmd->scan_begin_src == TRIG_FOLLOW) {
-		ui_DelayTime = 0;
-		ui_DelayTimeBase = 0;
-		ui_DelayMode = 0;
-	}			/* if(cmd->scan_begin_src==TRIG_FOLLOW) */
-	else {
-		ui_DelayTime = cmd->scan_begin_arg & 0xFFFF;
-		ui_DelayTimeBase = cmd->scan_begin_arg >> 16;
-		ui_DelayMode = 1;
-	}			/* else if(cmd->scan_begin_src==TRIG_FOLLOW) */
-	if (cmd->convert_src == TRIG_TIMER) {
-		ui_ConvertTime = cmd->convert_arg & 0xFFFF;
-		ui_ConvertTimeBase = cmd->convert_arg >> 16;
-	} else {
-		ui_ConvertTime = 0;
-		ui_ConvertTimeBase = 0;
-	}
-
-	/*  if(i_ADDIDATAType ==1 || ((i_ADDIDATAType==2))) */
-	/*    { */
-  /**************************************************/
-	/*Read the old configuration of the current source */
-  /**************************************************/
-	/* ui_Configuration = inl(devpriv->iobase+i_Offset + 12); */
-	ui_Configuration =
-		inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 12);
-  /***********************************************/
-	/*Write the configuration of the current source */
-  /***********************************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl((ui_Configuration & 0xFFC00000 ), devpriv->iobase+i_Offset +12); */
-	outl((ui_Configuration & 0xFFC00000),
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 12);
-	/*  } */
-	ui_Configuration = 0;
-
-	/* ui_Configuration = i_FirstChannel |(i_LastChannel << 8)| 0x00100000 | */
-	ui_Configuration =
-		s_BoardInfos[dev->minor].i_FirstChannel | (s_BoardInfos[dev->
-			minor].
-		i_LastChannel << 8) | 0x00100000 | (ui_Trigger << 24) |
-		(ui_TriggerEdge << 25) | (ui_Triggermode << 27) | (ui_DelayMode
-		<< 18) | (ui_ScanMode << 16);
-
-  /*************************/
-	/*Write the Configuration */
-  /*************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl( ui_Configuration, devpriv->iobase+i_Offset + 0x8); */
-	outl(ui_Configuration,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 0x8);
-  /***********************/
-	/*Write the Delay Value */
-  /***********************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(ui_DelayTime,devpriv->iobase+i_Offset + 40); */
-	outl(ui_DelayTime,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 40);
-  /***************************/
-	/*Write the Delay time base */
-  /***************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(ui_DelayTimeBase,devpriv->iobase+i_Offset + 44); */
-	outl(ui_DelayTimeBase,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 44);
-  /*********************************/
-	/*Write the conversion time value */
-  /*********************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(ui_ConvertTime,devpriv->iobase+i_Offset + 32); */
-	outl(ui_ConvertTime,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 32);
-
-  /********************************/
-	/*Write the conversion time base */
-  /********************************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl(ui_ConvertTimeBase,devpriv->iobase+i_Offset + 36); */
-	outl(ui_ConvertTimeBase,
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 36);
-  /*******************/
-	/*Read the register */
-  /*******************/
-	/* ui_Configuration = inl(devpriv->iobase+i_Offset + 4); */
-	ui_Configuration =
-		inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 4);
-  /******************/
-	/*Set the SCAN bit */
-  /******************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-
-	/* outl(((ui_Configuration & 0x1E0FF) | 0x00002000),devpriv->iobase+i_Offset + 4); */
-	outl(((ui_Configuration & 0x1E0FF) | 0x00002000),
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 4);
-  /*******************/
-	/*Read the register */
-  /*******************/
-	ui_Configuration = 0;
-	/* ui_Configuration = inl(devpriv->iobase+i_Offset + 8); */
-	ui_Configuration =
-		inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 8);
-
-  /*******************/
-	/*Set the START bit */
-  /*******************/
-	/* while (((inl(devpriv->iobase+i_Offset+12)>>19) & 1) != 1); */
-	while (((inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset +
-					12) >> 19) & 1) != 1) ;
-	/* outl((ui_Configuration | 0x00080000),devpriv->iobase+i_Offset + 8); */
-	outl((ui_Configuration | 0x00080000),
-		devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 8);
-	return 0;
-}
-
-/*
- * This function copies the acquired data(from FIFO) to Comedi buffer.
- */
-static int i_APCI3200_InterruptHandleEos(struct comedi_device *dev)
-{
-	struct addi_private *devpriv = dev->private;
-	struct comedi_subdevice *s = dev->read_subdev;
-	unsigned int ui_StatusRegister = 0;
-
-	/* BEGIN JK 18.10.2004: APCI-3200 Driver update 0.7.57 -> 0.7.68 */
-	/* comedi_async *async = s->async; */
-	/* UINT *data; */
-	/* data=async->data+async->buf_int_ptr;//new samples added from here onwards */
-	int n = 0, i = 0;
-	/* END JK 18.10.2004: APCI-3200 Driver update 0.7.57 -> 0.7.68 */
-
-  /************************************/
-	/*Read the interrupt status register */
-  /************************************/
-	/* ui_StatusRegister = inl(devpriv->iobase+i_Offset + 16); */
-	ui_StatusRegister =
-		inl(devpriv->iobase + s_BoardInfos[dev->minor].i_Offset + 16);
-
-  /*************************/
-	/*Test if interrupt occur */
-  /*************************/
-
-	if ((ui_StatusRegister & 0x2) == 0x2) {
-      /*************************/
-		/*Read the channel number */
-      /*************************/
-		/* ui_ChannelNumber = inl(devpriv->iobase+i_Offset + 24); */
-		/* BEGIN JK 18.10.2004: APCI-3200 Driver update 0.7.57 -> 0.7.68 */
-		/* This value is not used */
-		/* ui_ChannelNumber = inl(devpriv->iobase+s_BoardInfos [dev->minor].i_Offset + 24); */
-		/* END JK 18.10.2004: APCI-3200 Driver update 0.7.57 -> 0.7.68 */
-
-      /*************************************/
-		/*Read the digital Analog Input value */
-      /*************************************/
-
-		/* data[i_Count] = inl(devpriv->iobase+i_Offset + 28); */
-		/* Begin JK 18.10.2004: APCI-3200 Driver update 0.7.57 -> 0.7.68 */
-		/* data[s_BoardInfos [dev->minor].i_Count] = inl(devpriv->iobase+s_BoardInfos [dev->minor].i_Offset + 28); */
-		s_BoardInfos[dev->minor].ui_ScanValueArray[s_BoardInfos[dev->
-				minor].i_Count] =
-			inl(devpriv->iobase +
-			s_BoardInfos[dev->minor].i_Offset + 28);
-		/* End JK 18.10.2004: APCI-3200 Driver update 0.7.57 -> 0.7.68 */
-
-		/* if((i_Count == (i_LastChannel-i_FirstChannel+3))) */
-		if ((s_BoardInfos[dev->minor].i_Count ==
-				(s_BoardInfos[dev->minor].i_LastChannel -
-					s_BoardInfos[dev->minor].
-					i_FirstChannel + 3))) {
-
-			/* Begin JK 22.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-			s_BoardInfos[dev->minor].i_Count++;
-
-			for (i = s_BoardInfos[dev->minor].i_FirstChannel;
-				i <= s_BoardInfos[dev->minor].i_LastChannel;
-				i++) {
-				i_APCI3200_GetChannelCalibrationValue(dev, i,
-					&s_BoardInfos[dev->minor].
-					ui_ScanValueArray[s_BoardInfos[dev->
-							minor].i_Count + ((i -
-								s_BoardInfos
-								[dev->minor].
-								i_FirstChannel)
-							* 3)],
-					&s_BoardInfos[dev->minor].
-					ui_ScanValueArray[s_BoardInfos[dev->
-							minor].i_Count + ((i -
-								s_BoardInfos
-								[dev->minor].
-								i_FirstChannel)
-							* 3) + 1],
-					&s_BoardInfos[dev->minor].
-					ui_ScanValueArray[s_BoardInfos[dev->
-							minor].i_Count + ((i -
-								s_BoardInfos
-								[dev->minor].
-								i_FirstChannel)
-							* 3) + 2]);
-			}
-
-			/* End JK 22.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-
-			/* i_Count=-1; */
-
-			s_BoardInfos[dev->minor].i_Count = -1;
-
-			/* async->buf_int_count+=(i_LastChannel-i_FirstChannel+4)*sizeof(unsigned int); */
-			/* Begin JK 18.10.2004: APCI-3200 Driver update 0.7.57 -> 0.7.68 */
-			/* async->buf_int_count+=(s_BoardInfos [dev->minor].i_LastChannel-s_BoardInfos [dev->minor].i_FirstChannel+4)*sizeof(unsigned int); */
-			/* End JK 18.10.2004: APCI-3200 Driver update 0.7.57 -> 0.7.68 */
-			/* async->buf_int_ptr+=(i_LastChannel-i_FirstChannel+4)*sizeof(unsigned int); */
-			/* Begin JK 18.10.2004: APCI-3200 Driver update 0.7.57 -> 0.7.68 */
-			/* async->buf_int_ptr+=(s_BoardInfos [dev->minor].i_LastChannel-s_BoardInfos [dev->minor].i_FirstChannel+4)*sizeof(unsigned int); */
-			/* comedi_eos(dev,s); */
-
-			/*  Set the event type (Comedi Buffer End Of Scan) */
-			s->async->events |= COMEDI_CB_EOS;
-
-			/*  Test if enougth memory is available and allocate it for 7 values */
-			n = comedi_buf_write_alloc(s,
-				(7 + 12) * sizeof(unsigned int));
-
-			/*  If not enough memory available, event is set to Comedi Buffer Error */
-			if (n > ((7 + 12) * sizeof(unsigned int))) {
-				printk("\ncomedi_buf_write_alloc n = %i", n);
-				s->async->events |= COMEDI_CB_ERROR;
-			}
-			/*  Write all 7 scan values in the comedi buffer */
-			comedi_buf_memcpy_to(s, 0,
-				(unsigned int *) s_BoardInfos[dev->minor].
-				ui_ScanValueArray, (7 + 12) * sizeof(unsigned int));
-
-			/*  Update comedi buffer pinters indexes */
-			comedi_buf_write_free(s,
-				(7 + 12) * sizeof(unsigned int));
-
-			/*  Send events */
-			comedi_event(dev, s);
-			/* End JK 18.10.2004: APCI-3200 Driver update 0.7.57 -> 0.7.68 */
-
-			/* BEGIN JK 18.10.2004: APCI-3200 Driver update 0.7.57 -> 0.7.68 */
-			/*  */
-			/* if (s->async->buf_int_ptr>=s->async->data_len) //  for buffer rool over */
-			/*   { */
-			/*     /* buffer rollover */ */
-			/*     s->async->buf_int_ptr=0; */
-			/*     comedi_eobuf(dev,s); */
-			/*   } */
-			/* End JK 18.10.2004: APCI-3200 Driver update 0.7.57 -> 0.7.68 */
-		}
-		/* i_Count++; */
-		s_BoardInfos[dev->minor].i_Count++;
-	}
-	/* i_InterruptFlag=0; */
-	s_BoardInfos[dev->minor].i_InterruptFlag = 0;
-	return 0;
-}
-
-static void apci3200_interrupt(int irq, void *d)
-{
-	struct comedi_device *dev = d;
-	struct addi_private *devpriv = dev->private;
-	unsigned int ui_StatusRegister = 0;
-	unsigned int ui_ChannelNumber = 0;
-	int i_CalibrationFlag = 0;
-	int i_CJCFlag = 0;
-	unsigned int ui_DummyValue = 0;
-	unsigned int ui_DigitalTemperature = 0;
-	unsigned int ui_DigitalInput = 0;
-	int i_ConvertCJCCalibration;
-	/* BEGIN JK TEST */
-	int i_ReturnValue = 0;
-	/* END JK TEST */
-
-	/* switch(i_ScanType) */
-	switch (s_BoardInfos[dev->minor].i_ScanType) {
-	case 0:
-	case 1:
-		/* switch(i_ADDIDATAType) */
-		switch (s_BoardInfos[dev->minor].i_ADDIDATAType) {
-		case 0:
-		case 1:
-
-	  /************************************/
-			/*Read the interrupt status register */
-	  /************************************/
-			/* ui_StatusRegister = inl(devpriv->iobase+i_Offset + 16); */
-			ui_StatusRegister =
-				inl(devpriv->iobase +
-				s_BoardInfos[dev->minor].i_Offset + 16);
-			if ((ui_StatusRegister & 0x2) == 0x2) {
-				/* i_CalibrationFlag = ((inl(devpriv->iobase+i_Offset + 12) & 0x00060000) >> 17); */
-				i_CalibrationFlag =
-					((inl(devpriv->iobase +
-							s_BoardInfos[dev->
-								minor].
-							i_Offset +
-							12) & 0x00060000) >>
-					17);
-	      /*************************/
-				/*Read the channel number */
-	      /*************************/
-				/* ui_ChannelNumber = inl(devpriv->iobase+i_Offset + 24); */
-
-	      /*************************************/
-				/*Read the digital analog input value */
-	      /*************************************/
-				/* ui_DigitalInput = inl(devpriv->iobase+i_Offset + 28); */
-				ui_DigitalInput =
-					inl(devpriv->iobase +
-					s_BoardInfos[dev->minor].i_Offset + 28);
-
-	      /***********************************************/
-				/* Test if the value read is the channel value */
-	      /***********************************************/
-				if (i_CalibrationFlag == 0) {
-					/* ui_InterruptChannelValue[i_Count + 0] = ui_DigitalInput; */
-					s_BoardInfos[dev->minor].
-						ui_InterruptChannelValue
-						[s_BoardInfos[dev->minor].
-						i_Count + 0] = ui_DigitalInput;
-
-					/* Begin JK 22.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-					/*
-					   i_APCI3200_GetChannelCalibrationValue (dev, s_BoardInfos [dev->minor].ui_Channel_num,
-					   &s_BoardInfos [dev->minor].ui_InterruptChannelValue[s_BoardInfos [dev->minor].i_Count + 6],
-					   &s_BoardInfos [dev->minor].ui_InterruptChannelValue[s_BoardInfos [dev->minor].i_Count + 7],
-					   &s_BoardInfos [dev->minor].ui_InterruptChannelValue[s_BoardInfos [dev->minor].i_Count + 8]);
-					 */
-					/* End JK 22.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-
-		  /******************************************************/
-					/*Start the conversion of the calibration offset value */
-		  /******************************************************/
-					i_APCI3200_ReadCalibrationOffsetValue
-						(dev, &ui_DummyValue);
-				}	/* if (i_CalibrationFlag == 0) */
-	      /**********************************************************/
-				/* Test if the value read is the calibration offset value */
-	      /**********************************************************/
-
-				if (i_CalibrationFlag == 1) {
-
-		  /******************/
-					/* Save the value */
-		  /******************/
-
-					/* ui_InterruptChannelValue[i_Count + 1] = ui_DigitalInput; */
-					s_BoardInfos[dev->minor].
-						ui_InterruptChannelValue
-						[s_BoardInfos[dev->minor].
-						i_Count + 1] = ui_DigitalInput;
-
-		  /******************************************************/
-					/* Start the conversion of the calibration gain value */
-		  /******************************************************/
-					i_APCI3200_ReadCalibrationGainValue(dev,
-						&ui_DummyValue);
-				}	/* if (i_CalibrationFlag == 1) */
-	      /******************************************************/
-				/*Test if the value read is the calibration gain value */
-	      /******************************************************/
-
-				if (i_CalibrationFlag == 2) {
-
-		  /****************/
-					/*Save the value */
-		  /****************/
-					/* ui_InterruptChannelValue[i_Count + 2] = ui_DigitalInput; */
-					s_BoardInfos[dev->minor].
-						ui_InterruptChannelValue
-						[s_BoardInfos[dev->minor].
-						i_Count + 2] = ui_DigitalInput;
-					/* if(i_ScanType==1) */
-					if (s_BoardInfos[dev->minor].
-						i_ScanType == 1) {
-
-						/* i_InterruptFlag=0; */
-						s_BoardInfos[dev->minor].
-							i_InterruptFlag = 0;
-						/* i_Count=i_Count + 6; */
-						/* Begin JK 22.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-						/* s_BoardInfos [dev->minor].i_Count=s_BoardInfos [dev->minor].i_Count + 6; */
-						s_BoardInfos[dev->minor].
-							i_Count =
-							s_BoardInfos[dev->
-							minor].i_Count + 9;
-						/* End JK 22.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-					}	/* if(i_ScanType==1) */
-					else {
-						/* i_Count=0; */
-						s_BoardInfos[dev->minor].
-							i_Count = 0;
-					}	/* elseif(i_ScanType==1) */
-					/* if(i_ScanType!=1) */
-					if (s_BoardInfos[dev->minor].
-						i_ScanType != 1) {
-						i_ReturnValue = send_sig(SIGIO, devpriv->tsk_Current, 0);	/*  send signal to the sample */
-					}	/* if(i_ScanType!=1) */
-					else {
-						/* if(i_ChannelCount==i_Sum) */
-						if (s_BoardInfos[dev->minor].
-							i_ChannelCount ==
-							s_BoardInfos[dev->
-								minor].i_Sum) {
-							send_sig(SIGIO, devpriv->tsk_Current, 0);	/*  send signal to the sample */
-						}
-					}	/* if(i_ScanType!=1) */
-				}	/* if (i_CalibrationFlag == 2) */
-			}	/*  if ((ui_StatusRegister & 0x2) == 0x2) */
-
-			break;
-
-		case 2:
-	  /************************************/
-			/*Read the interrupt status register */
-	  /************************************/
-
-			/* ui_StatusRegister = inl(devpriv->iobase+i_Offset + 16); */
-			ui_StatusRegister =
-				inl(devpriv->iobase +
-				s_BoardInfos[dev->minor].i_Offset + 16);
-	  /*************************/
-			/*Test if interrupt occur */
-	  /*************************/
-
-			if ((ui_StatusRegister & 0x2) == 0x2) {
-
-				/* i_CJCFlag = ((inl(devpriv->iobase+i_Offset + 4) & 0x00000400) >> 10); */
-				i_CJCFlag =
-					((inl(devpriv->iobase +
-							s_BoardInfos[dev->
-								minor].
-							i_Offset +
-							4) & 0x00000400) >> 10);
-
-				/* i_CalibrationFlag = ((inl(devpriv->iobase+i_Offset + 12) & 0x00060000) >> 17); */
-				i_CalibrationFlag =
-					((inl(devpriv->iobase +
-							s_BoardInfos[dev->
-								minor].
-							i_Offset +
-							12) & 0x00060000) >>
-					17);
-
-	      /*************************/
-				/*Read the channel number */
-	      /*************************/
-
-				/* ui_ChannelNumber = inl(devpriv->iobase+i_Offset + 24); */
-				ui_ChannelNumber =
-					inl(devpriv->iobase +
-					s_BoardInfos[dev->minor].i_Offset + 24);
-				/* Begin JK 22.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-				s_BoardInfos[dev->minor].ui_Channel_num =
-					ui_ChannelNumber;
-				/* End JK 22.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-
-	      /************************************/
-				/*Read the digital temperature value */
-	      /************************************/
-				/* ui_DigitalTemperature = inl(devpriv->iobase+i_Offset + 28); */
-				ui_DigitalTemperature =
-					inl(devpriv->iobase +
-					s_BoardInfos[dev->minor].i_Offset + 28);
-
-	      /*********************************************/
-				/*Test if the value read is the channel value */
-	      /*********************************************/
-
-				if ((i_CalibrationFlag == 0)
-					&& (i_CJCFlag == 0)) {
-					/* ui_InterruptChannelValue[i_Count + 0]=ui_DigitalTemperature; */
-					s_BoardInfos[dev->minor].
-						ui_InterruptChannelValue
-						[s_BoardInfos[dev->minor].
-						i_Count + 0] =
-						ui_DigitalTemperature;
-
-		  /*********************************/
-					/*Start the conversion of the CJC */
-		  /*********************************/
-					i_APCI3200_ReadCJCValue(dev,
-						&ui_DummyValue);
-
-				}	/* if ((i_CalibrationFlag == 0) && (i_CJCFlag == 0)) */
-
-		 /*****************************************/
-				/*Test if the value read is the CJC value */
-		 /*****************************************/
-
-				if ((i_CJCFlag == 1)
-					&& (i_CalibrationFlag == 0)) {
-					/* ui_InterruptChannelValue[i_Count + 3]=ui_DigitalTemperature; */
-					s_BoardInfos[dev->minor].
-						ui_InterruptChannelValue
-						[s_BoardInfos[dev->minor].
-						i_Count + 3] =
-						ui_DigitalTemperature;
-
-		  /******************************************************/
-					/*Start the conversion of the calibration offset value */
-		  /******************************************************/
-					i_APCI3200_ReadCalibrationOffsetValue
-						(dev, &ui_DummyValue);
-				}	/*  if ((i_CJCFlag == 1) && (i_CalibrationFlag == 0)) */
-
-		 /********************************************************/
-				/*Test if the value read is the calibration offset value */
-		 /********************************************************/
-
-				if ((i_CalibrationFlag == 1)
-					&& (i_CJCFlag == 0)) {
-					/* ui_InterruptChannelValue[i_Count + 1]=ui_DigitalTemperature; */
-					s_BoardInfos[dev->minor].
-						ui_InterruptChannelValue
-						[s_BoardInfos[dev->minor].
-						i_Count + 1] =
-						ui_DigitalTemperature;
-
-		  /****************************************************/
-					/*Start the conversion of the calibration gain value */
-		  /****************************************************/
-					i_APCI3200_ReadCalibrationGainValue(dev,
-						&ui_DummyValue);
-
-				}	/* if ((i_CalibrationFlag == 1) && (i_CJCFlag == 0)) */
-
-	      /******************************************************/
-				/*Test if the value read is the calibration gain value */
-	      /******************************************************/
-
-				if ((i_CalibrationFlag == 2)
-					&& (i_CJCFlag == 0)) {
-					/* ui_InterruptChannelValue[i_Count + 2]=ui_DigitalTemperature; */
-					s_BoardInfos[dev->minor].
-						ui_InterruptChannelValue
-						[s_BoardInfos[dev->minor].
-						i_Count + 2] =
-						ui_DigitalTemperature;
-
-		  /**********************************************************/
-					/*Test if the Calibration channel must be read for the CJC */
-		  /**********************************************************/
-
-					/*Test if the polarity is the same */
-		  /**********************************/
-					/* if(i_CJCPolarity!=i_ADDIDATAPolarity) */
-					if (s_BoardInfos[dev->minor].
-						i_CJCPolarity !=
-						s_BoardInfos[dev->minor].
-						i_ADDIDATAPolarity) {
-						i_ConvertCJCCalibration = 1;
-					}	/* if(i_CJCPolarity!=i_ADDIDATAPolarity) */
-					else {
-						/* if(i_CJCGain==i_ADDIDATAGain) */
-						if (s_BoardInfos[dev->minor].
-							i_CJCGain ==
-							s_BoardInfos[dev->
-								minor].
-							i_ADDIDATAGain) {
-							i_ConvertCJCCalibration
-								= 0;
-						}	/* if(i_CJCGain==i_ADDIDATAGain) */
-						else {
-							i_ConvertCJCCalibration
-								= 1;
-						}	/* elseif(i_CJCGain==i_ADDIDATAGain) */
-					}	/* elseif(i_CJCPolarity!=i_ADDIDATAPolarity) */
-					if (i_ConvertCJCCalibration == 1) {
-		      /****************************************************************/
-						/*Start the conversion of the calibration gain value for the CJC */
-		      /****************************************************************/
-						i_APCI3200_ReadCJCCalOffset(dev,
-							&ui_DummyValue);
-
-					}	/* if(i_ConvertCJCCalibration==1) */
-					else {
-						/* ui_InterruptChannelValue[i_Count + 4]=0; */
-						/* ui_InterruptChannelValue[i_Count + 5]=0; */
-						s_BoardInfos[dev->minor].
-							ui_InterruptChannelValue
-							[s_BoardInfos[dev->
-								minor].i_Count +
-							4] = 0;
-						s_BoardInfos[dev->minor].
-							ui_InterruptChannelValue
-							[s_BoardInfos[dev->
-								minor].i_Count +
-							5] = 0;
-					}	/* elseif(i_ConvertCJCCalibration==1) */
-				}	/* else if ((i_CalibrationFlag == 2) && (i_CJCFlag == 0)) */
-
-		 /********************************************************************/
-				/*Test if the value read is the calibration offset value for the CJC */
-		 /********************************************************************/
-
-				if ((i_CalibrationFlag == 1)
-					&& (i_CJCFlag == 1)) {
-					/* ui_InterruptChannelValue[i_Count + 4]=ui_DigitalTemperature; */
-					s_BoardInfos[dev->minor].
-						ui_InterruptChannelValue
-						[s_BoardInfos[dev->minor].
-						i_Count + 4] =
-						ui_DigitalTemperature;
-
-		  /****************************************************************/
-					/*Start the conversion of the calibration gain value for the CJC */
-		  /****************************************************************/
-					i_APCI3200_ReadCJCCalGain(dev,
-						&ui_DummyValue);
-
-				}	/* if ((i_CalibrationFlag == 1) && (i_CJCFlag == 1)) */
-
-	      /******************************************************************/
-				/*Test if the value read is the calibration gain value for the CJC */
-	      /******************************************************************/
-
-				if ((i_CalibrationFlag == 2)
-					&& (i_CJCFlag == 1)) {
-					/* ui_InterruptChannelValue[i_Count + 5]=ui_DigitalTemperature; */
-					s_BoardInfos[dev->minor].
-						ui_InterruptChannelValue
-						[s_BoardInfos[dev->minor].
-						i_Count + 5] =
-						ui_DigitalTemperature;
-
-					/* if(i_ScanType==1) */
-					if (s_BoardInfos[dev->minor].
-						i_ScanType == 1) {
-
-						/* i_InterruptFlag=0; */
-						s_BoardInfos[dev->minor].
-							i_InterruptFlag = 0;
-						/* i_Count=i_Count + 6; */
-						/* Begin JK 22.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-						/* s_BoardInfos [dev->minor].i_Count=s_BoardInfos [dev->minor].i_Count + 6; */
-						s_BoardInfos[dev->minor].
-							i_Count =
-							s_BoardInfos[dev->
-							minor].i_Count + 9;
-						/* End JK 22.10.2004: APCI-3200 / APCI-3300 Reading of EEPROM values */
-					}	/* if(i_ScanType==1) */
-					else {
-						/* i_Count=0; */
-						s_BoardInfos[dev->minor].
-							i_Count = 0;
-					}	/* elseif(i_ScanType==1) */
-
-					/* if(i_ScanType!=1) */
-					if (s_BoardInfos[dev->minor].
-						i_ScanType != 1) {
-						send_sig(SIGIO, devpriv->tsk_Current, 0);	/*  send signal to the sample */
-					}	/* if(i_ScanType!=1) */
-					else {
-						/* if(i_ChannelCount==i_Sum) */
-						if (s_BoardInfos[dev->minor].
-							i_ChannelCount ==
-							s_BoardInfos[dev->
-								minor].i_Sum) {
-							send_sig(SIGIO, devpriv->tsk_Current, 0);	/*  send signal to the sample */
-
-						}	/* if(i_ChannelCount==i_Sum) */
-					}	/* else if(i_ScanType!=1) */
-				}	/* if ((i_CalibrationFlag == 2) && (i_CJCFlag == 1)) */
-
-			}	/* else if ((ui_StatusRegister & 0x2) == 0x2) */
-			break;
-		}		/* switch(i_ADDIDATAType) */
-		break;
-	case 2:
-	case 3:
-		i_APCI3200_InterruptHandleEos(dev);
-		break;
-	}			/* switch(i_ScanType) */
-	return;
-}
diff --git a/drivers/staging/comedi/drivers/addi_apci_035.c b/drivers/staging/comedi/drivers/addi_apci_035.c
deleted file mode 100644
index af70c8401880..000000000000
--- a/drivers/staging/comedi/drivers/addi_apci_035.c
+++ /dev/null
@@ -1,77 +0,0 @@
-#include <linux/module.h>
-#include <linux/pci.h>
-
-#include "../comedidev.h"
-#include "comedi_fc.h"
-#include "amcc_s5933.h"
-
-#include "addi-data/addi_common.h"
-
-#define ADDIDATA_WATCHDOG 2	/*  Or shold it be something else */
-
-#include "addi-data/addi_eeprom.c"
-#include "addi-data/hwdrv_apci035.c"
-#include "addi-data/addi_common.c"
-
-static const struct addi_board apci035_boardtypes[] = {
-	{
-		.pc_DriverName		= "apci035",
-		.i_IorangeBase1		= APCI035_ADDRESS_RANGE,
-		.i_PCIEeprom		= 1,
-		.pc_EepromChip		= "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		= apci035_interrupt,
-		.reset			= apci035_reset,
-		.ai_config		= apci035_ai_config,
-		.ai_read		= apci035_ai_read,
-		.timer_config		= apci035_timer_config,
-		.timer_write		= apci035_timer_write,
-		.timer_read		= apci035_timer_read,
-	},
-};
-
-static int apci035_auto_attach(struct comedi_device *dev,
-			       unsigned long context)
-{
-	dev->board_ptr = &apci035_boardtypes[0];
-
-	return addi_auto_attach(dev, context);
-}
-
-static struct comedi_driver apci035_driver = {
-	.driver_name	= "addi_apci_035",
-	.module		= THIS_MODULE,
-	.auto_attach	= apci035_auto_attach,
-	.detach		= i_ADDI_Detach,
-};
-
-static int apci035_pci_probe(struct pci_dev *dev,
-			     const struct pci_device_id *id)
-{
-	return comedi_pci_auto_config(dev, &apci035_driver, id->driver_data);
-}
-
-static const struct pci_device_id apci035_pci_table[] = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA,  0x0300) },
-	{ 0 }
-};
-MODULE_DEVICE_TABLE(pci, apci035_pci_table);
-
-static struct pci_driver apci035_pci_driver = {
-	.name		= "addi_apci_035",
-	.id_table	= apci035_pci_table,
-	.probe		= apci035_pci_probe,
-	.remove		= comedi_pci_auto_unconfig,
-};
-module_comedi_pci_driver(apci035_driver, apci035_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/addi_apci_1032.c b/drivers/staging/comedi/drivers/addi_apci_1032.c
index 840cb289507a..bf14165297b7 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1032.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1032.c
@@ -258,9 +258,8 @@ static irqreturn_t apci1032_interrupt(int irq, void *d)
 	outl(ctrl & ~APCI1032_CTRL_INT_ENA, dev->iobase + APCI1032_CTRL_REG);
 
 	s->state = inl(dev->iobase + APCI1032_STATUS_REG) & 0xffff;
-	comedi_buf_put(s, s->state);
-	s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
-	comedi_event(dev, s);
+	comedi_buf_write_samples(s, &s->state, 1);
+	comedi_handle_events(dev, s);
 
 	/* enable the interrupt */
 	outl(ctrl, dev->iobase + APCI1032_CTRL_REG);
diff --git a/drivers/staging/comedi/drivers/addi_apci_1500.c b/drivers/staging/comedi/drivers/addi_apci_1500.c
index b7a284ac6649..30b132c3d092 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1500.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1500.c
@@ -1,54 +1,109 @@
 #include <linux/module.h>
 #include <linux/pci.h>
+#include <linux/sched.h>
+#include <linux/interrupt.h>
 
 #include "../comedidev.h"
 #include "comedi_fc.h"
 #include "amcc_s5933.h"
 
-#include "addi-data/addi_common.h"
+struct apci1500_private {
+	int iobase;
+	int i_IobaseAmcc;
+	int i_IobaseAddon;
+	int i_IobaseReserved;
+	unsigned char b_OutputMemoryStatus;
+	struct task_struct *tsk_Current;
+};
 
-#include "addi-data/addi_eeprom.c"
 #include "addi-data/hwdrv_apci1500.c"
-#include "addi-data/addi_common.c"
-
-static const struct addi_board apci1500_boardtypes[] = {
-	{
-		.pc_DriverName		= "apci1500",
-		.i_IorangeBase1		= APCI1500_ADDRESS_RANGE,
-		.i_PCIEeprom		= 0,
-		.i_NbrDiChannel		= 16,
-		.i_NbrDoChannel		= 16,
-		.i_DoMaxdata		= 0xffff,
-		.i_Timer		= 1,
-		.interrupt		= apci1500_interrupt,
-		.reset			= apci1500_reset,
-		.di_config		= apci1500_di_config,
-		.di_read		= apci1500_di_read,
-		.di_write		= apci1500_di_write,
-		.di_bits		= apci1500_di_insn_bits,
-		.do_config		= apci1500_do_config,
-		.do_write		= apci1500_do_write,
-		.do_bits		= apci1500_do_bits,
-		.timer_config		= apci1500_timer_config,
-		.timer_write		= apci1500_timer_write,
-		.timer_read		= apci1500_timer_read,
-		.timer_bits		= apci1500_timer_bits,
-	},
-};
 
 static int apci1500_auto_attach(struct comedi_device *dev,
 				unsigned long context)
 {
-	dev->board_ptr = &apci1500_boardtypes[0];
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	struct apci1500_private *devpriv;
+	struct comedi_subdevice *s;
+	int ret;
+
+	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
+	if (!devpriv)
+		return -ENOMEM;
+
+	ret = comedi_pci_enable(dev);
+	if (ret)
+		return ret;
+
+	dev->iobase = pci_resource_start(pcidev, 1);
+	devpriv->iobase = dev->iobase;
+	devpriv->i_IobaseAmcc = pci_resource_start(pcidev, 0);
+	devpriv->i_IobaseAddon = pci_resource_start(pcidev, 2);
+	devpriv->i_IobaseReserved = pci_resource_start(pcidev, 3);
+
+	if (pcidev->irq > 0) {
+		ret = request_irq(pcidev->irq, apci1500_interrupt, IRQF_SHARED,
+				  dev->board_name, dev);
+		if (ret == 0)
+			dev->irq = pcidev->irq;
+	}
+
+	ret = comedi_alloc_subdevices(dev, 3);
+	if (ret)
+		return ret;
 
-	return addi_auto_attach(dev, context);
+	/*  Allocate and Initialise DI Subdevice Structures */
+	s = &dev->subdevices[0];
+	s->type = COMEDI_SUBD_DI;
+	s->subdev_flags = SDF_READABLE;
+	s->n_chan = 16;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_config = apci1500_di_config;
+	s->insn_read = apci1500_di_read;
+	s->insn_write = apci1500_di_write;
+	s->insn_bits = apci1500_di_insn_bits;
+
+	/*  Allocate and Initialise DO Subdevice Structures */
+	s = &dev->subdevices[1];
+	s->type = COMEDI_SUBD_DO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 16;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_config = apci1500_do_config;
+	s->insn_write = apci1500_do_write;
+	s->insn_bits = apci1500_do_bits;
+
+	/*  Allocate and Initialise Timer Subdevice Structures */
+	s = &dev->subdevices[2];
+	s->type = COMEDI_SUBD_TIMER;
+	s->subdev_flags = SDF_WRITABLE;
+	s->n_chan = 1;
+	s->maxdata = 0;
+	s->len_chanlist = 1;
+	s->range_table = &range_digital;
+	s->insn_write = apci1500_timer_write;
+	s->insn_read = apci1500_timer_read;
+	s->insn_config = apci1500_timer_config;
+	s->insn_bits = apci1500_timer_bits;
+
+	apci1500_reset(dev);
+
+	return 0;
+}
+
+static void apci1500_detach(struct comedi_device *dev)
+{
+	if (dev->iobase)
+		apci1500_reset(dev);
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver apci1500_driver = {
 	.driver_name	= "addi_apci_1500",
 	.module		= THIS_MODULE,
 	.auto_attach	= apci1500_auto_attach,
-	.detach		= i_ADDI_Detach,
+	.detach		= apci1500_detach,
 };
 
 static int apci1500_pci_probe(struct pci_dev *dev,
diff --git a/drivers/staging/comedi/drivers/addi_apci_1516.c b/drivers/staging/comedi/drivers/addi_apci_1516.c
index 55d00fd94c91..d8410415cc90 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1516.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1516.c
@@ -163,7 +163,7 @@ static int apci1516_auto_attach(struct comedi_device *dev,
 	s = &dev->subdevices[1];
 	if (this_board->do_nchan) {
 		s->type		= COMEDI_SUBD_DO;
-		s->subdev_flags	= SDF_WRITEABLE;
+		s->subdev_flags	= SDF_WRITABLE;
 		s->n_chan	= this_board->do_nchan;
 		s->maxdata	= 1;
 		s->range_table	= &range_digital;
diff --git a/drivers/staging/comedi/drivers/addi_apci_1564.c b/drivers/staging/comedi/drivers/addi_apci_1564.c
index 688b015a834e..6872b69da5db 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1564.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1564.c
@@ -28,16 +28,91 @@
 
 #include "../comedidev.h"
 #include "comedi_fc.h"
-#include "amcc_s5933.h"
+#include "addi_tcw.h"
 #include "addi_watchdog.h"
 
+/*
+ * PCI BAR 0
+ *
+ * PLD Revision 1.0 I/O Mapping
+ *   0x00         93C76 EEPROM
+ *   0x04 - 0x18  Timer 12-Bit
+ *
+ * PLD Revision 2.x I/O Mapping
+ *   0x00         93C76 EEPROM
+ *   0x04 - 0x14  Digital Input
+ *   0x18 - 0x25  Digital Output
+ *   0x28 - 0x44  Watchdog 8-Bit
+ *   0x48 - 0x64  Timer 12-Bit
+ */
+#define APCI1564_EEPROM_REG			0x00
+#define APCI1564_EEPROM_VCC_STATUS		(1 << 8)
+#define APCI1564_EEPROM_TO_REV(x)		(((x) >> 4) & 0xf)
+#define APCI1564_EEPROM_DI			(1 << 3)
+#define APCI1564_EEPROM_DO			(1 << 2)
+#define APCI1564_EEPROM_CS			(1 << 1)
+#define APCI1564_EEPROM_CLK			(1 << 0)
+#define APCI1564_REV1_TIMER_IOBASE		0x04
+#define APCI1564_REV2_MAIN_IOBASE		0x04
+#define APCI1564_REV2_TIMER_IOBASE		0x48
+
+/*
+ * PCI BAR 1
+ *
+ * PLD Revision 1.0 I/O Mapping
+ *   0x00 - 0x10  Digital Input
+ *   0x14 - 0x20  Digital Output
+ *   0x24 - 0x3c  Watchdog 8-Bit
+ *
+ * PLD Revision 2.x I/O Mapping
+ *   0x00         Counter_0
+ *   0x20         Counter_1
+ *   0x30         Counter_3
+ */
+#define APCI1564_REV1_MAIN_IOBASE		0x00
+
+/*
+ * dev->iobase Register Map
+ *   PLD Revision 1.0 - PCI BAR 1 + 0x00
+ *   PLD Revision 2.x - PCI BAR 0 + 0x04
+ */
+#define APCI1564_DI_REG				0x00
+#define APCI1564_DI_INT_MODE1_REG		0x04
+#define APCI1564_DI_INT_MODE2_REG		0x08
+#define APCI1564_DI_INT_STATUS_REG		0x0c
+#define APCI1564_DI_IRQ_REG			0x10
+#define APCI1564_DO_REG				0x14
+#define APCI1564_DO_INT_CTRL_REG		0x18
+#define APCI1564_DO_INT_STATUS_REG		0x1c
+#define APCI1564_DO_IRQ_REG			0x20
+#define APCI1564_WDOG_REG			0x24
+#define APCI1564_WDOG_RELOAD_REG		0x28
+#define APCI1564_WDOG_TIMEBASE_REG		0x2c
+#define APCI1564_WDOG_CTRL_REG			0x30
+#define APCI1564_WDOG_STATUS_REG		0x34
+#define APCI1564_WDOG_IRQ_REG			0x38
+#define APCI1564_WDOG_WARN_TIMEVAL_REG		0x3c
+#define APCI1564_WDOG_WARN_TIMEBASE_REG		0x40
+
+/*
+ * devpriv->timer Register Map (see addi_tcw.h for register/bit defines)
+ *   PLD Revision 1.0 - PCI BAR 0 + 0x04
+ *   PLD Revision 2.x - PCI BAR 0 + 0x48
+ */
+
+/*
+ * devpriv->counters Register Map (see addi_tcw.h for register/bit defines)
+ *   PLD Revision 2.x - PCI BAR 1 + 0x00
+ */
+#define APCI1564_COUNTER(x)			((x) * 0x20)
+
 struct apci1564_private {
-	unsigned int amcc_iobase;	/* base of AMCC I/O registers */
+	unsigned long eeprom;		/* base address of EEPROM register */
+	unsigned long timer;		/* base address of 12-bit timer */
+	unsigned long counters;		/* base address of 32-bit counters */
 	unsigned int mode1;		/* riding-edge/high level channels */
 	unsigned int mode2;		/* falling-edge/low level channels */
 	unsigned int ctrl;		/* interrupt mode OR (edge) . AND (level) */
-	unsigned char timer_select_mode;
-	unsigned char mode_select_register;
 	struct task_struct *tsk_current;
 };
 
@@ -48,27 +123,30 @@ static int apci1564_reset(struct comedi_device *dev)
 	struct apci1564_private *devpriv = dev->private;
 
 	/* Disable the input interrupts and reset status register */
-	outl(0x0, devpriv->amcc_iobase + APCI1564_DI_IRQ_REG);
-	inl(devpriv->amcc_iobase + APCI1564_DI_INT_STATUS_REG);
-	outl(0x0, devpriv->amcc_iobase + APCI1564_DI_INT_MODE1_REG);
-	outl(0x0, devpriv->amcc_iobase + APCI1564_DI_INT_MODE2_REG);
+	outl(0x0, dev->iobase + APCI1564_DI_IRQ_REG);
+	inl(dev->iobase + APCI1564_DI_INT_STATUS_REG);
+	outl(0x0, dev->iobase + APCI1564_DI_INT_MODE1_REG);
+	outl(0x0, dev->iobase + APCI1564_DI_INT_MODE2_REG);
 
 	/* Reset the output channels and disable interrupts */
-	outl(0x0, devpriv->amcc_iobase + APCI1564_DO_REG);
-	outl(0x0, devpriv->amcc_iobase + APCI1564_DO_INT_CTRL_REG);
+	outl(0x0, dev->iobase + APCI1564_DO_REG);
+	outl(0x0, dev->iobase + APCI1564_DO_INT_CTRL_REG);
 
 	/* Reset the watchdog registers */
-	addi_watchdog_reset(devpriv->amcc_iobase + APCI1564_WDOG_REG);
+	addi_watchdog_reset(dev->iobase + APCI1564_WDOG_REG);
 
 	/* Reset the timer registers */
-	outl(0x0, devpriv->amcc_iobase + APCI1564_TIMER_CTRL_REG);
-	outl(0x0, devpriv->amcc_iobase + APCI1564_TIMER_RELOAD_REG);
+	outl(0x0, devpriv->timer + ADDI_TCW_CTRL_REG);
+	outl(0x0, devpriv->timer + ADDI_TCW_RELOAD_REG);
+
+	if (devpriv->counters) {
+		unsigned long iobase = devpriv->counters + ADDI_TCW_CTRL_REG;
 
-	/* Reset the counter registers */
-	outl(0x0, dev->iobase + APCI1564_COUNTER_CTRL_REG(APCI1564_COUNTER1));
-	outl(0x0, dev->iobase + APCI1564_COUNTER_CTRL_REG(APCI1564_COUNTER2));
-	outl(0x0, dev->iobase + APCI1564_COUNTER_CTRL_REG(APCI1564_COUNTER3));
-	outl(0x0, dev->iobase + APCI1564_COUNTER_CTRL_REG(APCI1564_COUNTER4));
+		/* Reset the counter registers */
+		outl(0x0, iobase + APCI1564_COUNTER(0));
+		outl(0x0, iobase + APCI1564_COUNTER(1));
+		outl(0x0, iobase + APCI1564_COUNTER(2));
+	}
 
 	return 0;
 }
@@ -82,55 +160,52 @@ static irqreturn_t apci1564_interrupt(int irq, void *d)
 	unsigned int ctrl;
 	unsigned int chan;
 
-	/* check interrupt is from this device */
-	if ((inl(devpriv->amcc_iobase + AMCC_OP_REG_INTCSR) &
-	     INTCSR_INTR_ASSERTED) == 0)
-		return IRQ_NONE;
-
-	status = inl(devpriv->amcc_iobase + APCI1564_DI_IRQ_REG);
+	status = inl(dev->iobase + APCI1564_DI_IRQ_REG);
 	if (status & APCI1564_DI_INT_ENABLE) {
 		/* disable the interrupt */
 		outl(status & APCI1564_DI_INT_DISABLE,
-		     devpriv->amcc_iobase + APCI1564_DI_IRQ_REG);
+		     dev->iobase + APCI1564_DI_IRQ_REG);
 
-		s->state = inl(dev->iobase + APCI1564_DI_INT_STATUS_REG)
-			       & 0xffff;
-		comedi_buf_put(s, s->state);
-		s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
-		comedi_event(dev, s);
+		s->state = inl(dev->iobase + APCI1564_DI_INT_STATUS_REG) &
+			   0xffff;
+		comedi_buf_write_samples(s, &s->state, 1);
+		comedi_handle_events(dev, s);
 
 		/* enable the interrupt */
-		outl(status, devpriv->amcc_iobase + APCI1564_DI_IRQ_REG);
+		outl(status, dev->iobase + APCI1564_DI_IRQ_REG);
 	}
 
-	status = inl(devpriv->amcc_iobase + APCI1564_TIMER_IRQ_REG);
+	status = inl(devpriv->timer + ADDI_TCW_IRQ_REG);
 	if (status & 0x01) {
 		/*  Disable Timer Interrupt */
-		ctrl = inl(devpriv->amcc_iobase + APCI1564_TIMER_CTRL_REG);
-		outl(0x0, devpriv->amcc_iobase + APCI1564_TIMER_CTRL_REG);
+		ctrl = inl(devpriv->timer + ADDI_TCW_CTRL_REG);
+		outl(0x0, devpriv->timer + ADDI_TCW_CTRL_REG);
 
 		/* Send a signal to from kernel to user space */
 		send_sig(SIGIO, devpriv->tsk_current, 0);
 
 		/*  Enable Timer Interrupt */
-		outl(ctrl, devpriv->amcc_iobase + APCI1564_TIMER_CTRL_REG);
+		outl(ctrl, devpriv->timer + ADDI_TCW_CTRL_REG);
 	}
 
-	for (chan = 0; chan < 4; chan++) {
-		status = inl(dev->iobase + APCI1564_COUNTER_IRQ_REG(chan));
-		if (status & 0x01) {
-			/*  Disable Counter Interrupt */
-			ctrl = inl(dev->iobase +
-				  APCI1564_COUNTER_CTRL_REG(chan));
-			outl(0x0, dev->iobase +
-			    APCI1564_COUNTER_CTRL_REG(chan));
-
-			/* Send a signal to from kernel to user space */
-			send_sig(SIGIO, devpriv->tsk_current, 0);
-
-			/*  Enable Counter Interrupt */
-			outl(ctrl, dev->iobase +
-			    APCI1564_COUNTER_CTRL_REG(chan));
+	if (devpriv->counters) {
+		for (chan = 0; chan < 4; chan++) {
+			unsigned long iobase;
+
+			iobase = devpriv->counters + APCI1564_COUNTER(chan);
+
+			status = inl(iobase + ADDI_TCW_IRQ_REG);
+			if (status & 0x01) {
+				/*  Disable Counter Interrupt */
+				ctrl = inl(iobase + ADDI_TCW_CTRL_REG);
+				outl(0x0, iobase + ADDI_TCW_CTRL_REG);
+
+				/* Send a signal to from kernel to user space */
+				send_sig(SIGIO, devpriv->tsk_current, 0);
+
+				/*  Enable Counter Interrupt */
+				outl(ctrl, iobase + ADDI_TCW_CTRL_REG);
+			}
 		}
 	}
 
@@ -142,9 +217,7 @@ static int apci1564_di_insn_bits(struct comedi_device *dev,
 				 struct comedi_insn *insn,
 				 unsigned int *data)
 {
-	struct apci1564_private *devpriv = dev->private;
-
-	data[1] = inl(devpriv->amcc_iobase + APCI1564_DI_REG);
+	data[1] = inl(dev->iobase + APCI1564_DI_REG);
 
 	return insn->n;
 }
@@ -154,12 +227,10 @@ static int apci1564_do_insn_bits(struct comedi_device *dev,
 				 struct comedi_insn *insn,
 				 unsigned int *data)
 {
-	struct apci1564_private *devpriv = dev->private;
-
-	s->state = inl(devpriv->amcc_iobase + APCI1564_DO_REG);
+	s->state = inl(dev->iobase + APCI1564_DO_REG);
 
 	if (comedi_dio_update_state(s, data))
-		outl(s->state, devpriv->amcc_iobase + APCI1564_DO_REG);
+		outl(s->state, dev->iobase + APCI1564_DO_REG);
 
 	data[1] = s->state;
 
@@ -171,9 +242,7 @@ static int apci1564_diag_insn_bits(struct comedi_device *dev,
 				   struct comedi_insn *insn,
 				   unsigned int *data)
 {
-	struct apci1564_private *devpriv = dev->private;
-
-	data[1] = inl(devpriv->amcc_iobase + APCI1564_DO_INT_STATUS_REG) & 3;
+	data[1] = inl(dev->iobase + APCI1564_DO_INT_STATUS_REG) & 3;
 
 	return insn->n;
 }
@@ -227,10 +296,10 @@ static int apci1564_cos_insn_config(struct comedi_device *dev,
 			devpriv->ctrl = 0;
 			devpriv->mode1 = 0;
 			devpriv->mode2 = 0;
-			outl(0x0, devpriv->amcc_iobase + APCI1564_DI_IRQ_REG);
-			inl(devpriv->amcc_iobase + APCI1564_DI_INT_STATUS_REG);
-			outl(0x0, devpriv->amcc_iobase + APCI1564_DI_INT_MODE1_REG);
-			outl(0x0, devpriv->amcc_iobase + APCI1564_DI_INT_MODE2_REG);
+			outl(0x0, dev->iobase + APCI1564_DI_IRQ_REG);
+			inl(dev->iobase + APCI1564_DI_INT_STATUS_REG);
+			outl(0x0, dev->iobase + APCI1564_DI_INT_MODE1_REG);
+			outl(0x0, dev->iobase + APCI1564_DI_INT_MODE2_REG);
 			break;
 		case COMEDI_DIGITAL_TRIG_ENABLE_EDGES:
 			if (devpriv->ctrl != (APCI1564_DI_INT_ENABLE |
@@ -342,9 +411,9 @@ static int apci1564_cos_cmd(struct comedi_device *dev,
 		return -EINVAL;
 	}
 
-	outl(devpriv->mode1, devpriv->amcc_iobase + APCI1564_DI_INT_MODE1_REG);
-	outl(devpriv->mode2, devpriv->amcc_iobase + APCI1564_DI_INT_MODE2_REG);
-	outl(devpriv->ctrl, devpriv->amcc_iobase + APCI1564_DI_IRQ_REG);
+	outl(devpriv->mode1, dev->iobase + APCI1564_DI_INT_MODE1_REG);
+	outl(devpriv->mode2, dev->iobase + APCI1564_DI_INT_MODE2_REG);
+	outl(devpriv->ctrl, dev->iobase + APCI1564_DI_IRQ_REG);
 
 	return 0;
 }
@@ -352,12 +421,10 @@ static int apci1564_cos_cmd(struct comedi_device *dev,
 static int apci1564_cos_cancel(struct comedi_device *dev,
 			       struct comedi_subdevice *s)
 {
-	struct apci1564_private *devpriv = dev->private;
-
-	outl(0x0, devpriv->amcc_iobase + APCI1564_DI_IRQ_REG);
-	inl(devpriv->amcc_iobase + APCI1564_DI_INT_STATUS_REG);
-	outl(0x0, devpriv->amcc_iobase + APCI1564_DI_INT_MODE1_REG);
-	outl(0x0, devpriv->amcc_iobase + APCI1564_DI_INT_MODE2_REG);
+	outl(0x0, dev->iobase + APCI1564_DI_IRQ_REG);
+	inl(dev->iobase + APCI1564_DI_INT_STATUS_REG);
+	outl(0x0, dev->iobase + APCI1564_DI_INT_MODE1_REG);
+	outl(0x0, dev->iobase + APCI1564_DI_INT_MODE2_REG);
 
 	return 0;
 }
@@ -368,6 +435,7 @@ static int apci1564_auto_attach(struct comedi_device *dev,
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct apci1564_private *devpriv;
 	struct comedi_subdevice *s;
+	unsigned int val;
 	int ret;
 
 	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
@@ -378,8 +446,20 @@ static int apci1564_auto_attach(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	dev->iobase = pci_resource_start(pcidev, 1);
-	devpriv->amcc_iobase = pci_resource_start(pcidev, 0);
+	/* read the EEPROM register and check the I/O map revision */
+	devpriv->eeprom = pci_resource_start(pcidev, 0);
+	val = inl(devpriv->eeprom + APCI1564_EEPROM_REG);
+	if (APCI1564_EEPROM_TO_REV(val) == 0) {
+		/* PLD Revision 1.0 I/O Mapping */
+		dev->iobase = pci_resource_start(pcidev, 1) +
+			      APCI1564_REV1_MAIN_IOBASE;
+		devpriv->timer = devpriv->eeprom + APCI1564_REV1_TIMER_IOBASE;
+	} else {
+		/* PLD Revision 2.x I/O Mapping */
+		dev->iobase = devpriv->eeprom + APCI1564_REV2_MAIN_IOBASE;
+		devpriv->timer = devpriv->eeprom + APCI1564_REV2_TIMER_IOBASE;
+		devpriv->counters = pci_resource_start(pcidev, 1);
+	}
 
 	apci1564_reset(dev);
 
@@ -390,7 +470,7 @@ static int apci1564_auto_attach(struct comedi_device *dev,
 			dev->irq = pcidev->irq;
 	}
 
-	ret = comedi_alloc_subdevices(dev, 6);
+	ret = comedi_alloc_subdevices(dev, 7);
 	if (ret)
 		return ret;
 
@@ -406,7 +486,7 @@ static int apci1564_auto_attach(struct comedi_device *dev,
 	/*  Allocate and Initialise DO Subdevice Structures */
 	s = &dev->subdevices[1];
 	s->type		= COMEDI_SUBD_DO;
-	s->subdev_flags	= SDF_WRITEABLE;
+	s->subdev_flags	= SDF_WRITABLE;
 	s->n_chan	= 32;
 	s->maxdata	= 1;
 	s->range_table	= &range_digital;
@@ -431,26 +511,40 @@ static int apci1564_auto_attach(struct comedi_device *dev,
 		s->type		= COMEDI_SUBD_UNUSED;
 	}
 
-	/*  Allocate and Initialise Timer Subdevice Structures */
+	/* Timer subdevice */
 	s = &dev->subdevices[3];
 	s->type		= COMEDI_SUBD_TIMER;
-	s->subdev_flags	= SDF_WRITEABLE;
+	s->subdev_flags	= SDF_WRITABLE | SDF_READABLE;
 	s->n_chan	= 1;
-	s->maxdata	= 0;
-	s->len_chanlist	= 1;
+	s->maxdata	= 0x0fff;
 	s->range_table	= &range_digital;
-	s->insn_write	= apci1564_timer_write;
-	s->insn_read	= apci1564_timer_read;
-	s->insn_config	= apci1564_timer_config;
+	s->insn_config	= apci1564_timer_insn_config;
+	s->insn_write	= apci1564_timer_insn_write;
+	s->insn_read	= apci1564_timer_insn_read;
 
-	/* Initialize the watchdog subdevice */
+	/* Counter subdevice */
 	s = &dev->subdevices[4];
-	ret = addi_watchdog_init(s, devpriv->amcc_iobase + APCI1564_WDOG_REG);
+	if (devpriv->counters) {
+		s->type		= COMEDI_SUBD_COUNTER;
+		s->subdev_flags	= SDF_WRITABLE | SDF_READABLE | SDF_LSAMPL;
+		s->n_chan	= 3;
+		s->maxdata	= 0xffffffff;
+		s->range_table	= &range_digital;
+		s->insn_config	= apci1564_counter_insn_config;
+		s->insn_write	= apci1564_counter_insn_write;
+		s->insn_read	= apci1564_counter_insn_read;
+	} else {
+		s->type		= COMEDI_SUBD_UNUSED;
+	}
+
+	/* Initialize the watchdog subdevice */
+	s = &dev->subdevices[5];
+	ret = addi_watchdog_init(s, dev->iobase + APCI1564_WDOG_REG);
 	if (ret)
 		return ret;
 
 	/* Initialize the diagnostic status subdevice */
-	s = &dev->subdevices[5];
+	s = &dev->subdevices[6];
 	s->type		= COMEDI_SUBD_DI;
 	s->subdev_flags	= SDF_READABLE;
 	s->n_chan	= 2;
diff --git a/drivers/staging/comedi/drivers/addi_apci_16xx.c b/drivers/staging/comedi/drivers/addi_apci_16xx.c
index 4162e2dc2860..a1248dab369f 100644
--- a/drivers/staging/comedi/drivers/addi_apci_16xx.c
+++ b/drivers/staging/comedi/drivers/addi_apci_16xx.c
@@ -140,7 +140,7 @@ static int apci16xx_auto_attach(struct comedi_device *dev,
 	for (i = 0; i < n_subdevs; i++) {
 		s = &dev->subdevices[i];
 		s->type		= COMEDI_SUBD_DIO;
-		s->subdev_flags	= SDF_WRITEABLE | SDF_READABLE;
+		s->subdev_flags	= SDF_WRITABLE | SDF_READABLE;
 		s->n_chan	= ((i * 32) < board->n_chan) ? 32 : last;
 		s->maxdata	= 1;
 		s->range_table	= &range_digital;
diff --git a/drivers/staging/comedi/drivers/addi_apci_2032.c b/drivers/staging/comedi/drivers/addi_apci_2032.c
index aea3da325359..eebf4f151b39 100644
--- a/drivers/staging/comedi/drivers/addi_apci_2032.c
+++ b/drivers/staging/comedi/drivers/addi_apci_2032.c
@@ -47,7 +47,6 @@
 
 struct apci2032_int_private {
 	spinlock_t spinlock;
-	unsigned int stop_count;
 	bool active;
 	unsigned char enabled_isns;
 };
@@ -148,7 +147,6 @@ static int apci2032_int_cmd(struct comedi_device *dev,
 	spin_lock_irqsave(&subpriv->spinlock, flags);
 
 	subpriv->enabled_isns = enabled_isns;
-	subpriv->stop_count = cmd->stop_arg;
 	subpriv->active = true;
 	outl(enabled_isns, dev->iobase + APCI2032_INT_CTRL_REG);
 
@@ -178,7 +176,6 @@ static irqreturn_t apci2032_interrupt(int irq, void *d)
 	struct comedi_cmd *cmd = &s->async->cmd;
 	struct apci2032_int_private *subpriv;
 	unsigned int val;
-	bool do_event = false;
 
 	if (!dev->attached)
 		return IRQ_NONE;
@@ -212,27 +209,16 @@ static irqreturn_t apci2032_interrupt(int irq, void *d)
 				bits |= (1 << i);
 		}
 
-		if (comedi_buf_put(s, bits)) {
-			s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
-			if (cmd->stop_src == TRIG_COUNT &&
-			    subpriv->stop_count > 0) {
-				subpriv->stop_count--;
-				if (subpriv->stop_count == 0) {
-					/* end of acquisition */
-					s->async->events |= COMEDI_CB_EOA;
-					apci2032_int_stop(dev, s);
-				}
-			}
-		} else {
-			apci2032_int_stop(dev, s);
-			s->async->events |= COMEDI_CB_OVERFLOW;
-		}
-		do_event = true;
+		comedi_buf_write_samples(s, &bits, 1);
+
+		if (cmd->stop_src == TRIG_COUNT &&
+		    s->async->scans_done >= cmd->stop_arg)
+			s->async->events |= COMEDI_CB_EOA;
 	}
 
 	spin_unlock(&subpriv->spinlock);
-	if (do_event)
-		comedi_event(dev, s);
+
+	comedi_handle_events(dev, s);
 
 	return IRQ_HANDLED;
 }
@@ -274,7 +260,7 @@ static int apci2032_auto_attach(struct comedi_device *dev,
 	/* Initialize the digital output subdevice */
 	s = &dev->subdevices[0];
 	s->type		= COMEDI_SUBD_DO;
-	s->subdev_flags	= SDF_WRITEABLE;
+	s->subdev_flags	= SDF_WRITABLE;
 	s->n_chan	= 32;
 	s->maxdata	= 1;
 	s->range_table	= &range_digital;
@@ -303,7 +289,7 @@ static int apci2032_auto_attach(struct comedi_device *dev,
 			return -ENOMEM;
 		spin_lock_init(&subpriv->spinlock);
 		s->private	= subpriv;
-		s->subdev_flags	= SDF_READABLE | SDF_CMD_READ;
+		s->subdev_flags	= SDF_READABLE | SDF_CMD_READ | SDF_PACKED;
 		s->len_chanlist = 2;
 		s->do_cmdtest	= apci2032_int_cmdtest;
 		s->do_cmd	= apci2032_int_cmd;
diff --git a/drivers/staging/comedi/drivers/addi_apci_2200.c b/drivers/staging/comedi/drivers/addi_apci_2200.c
index 51ab1f937bae..1f9d13661ac9 100644
--- a/drivers/staging/comedi/drivers/addi_apci_2200.c
+++ b/drivers/staging/comedi/drivers/addi_apci_2200.c
@@ -98,7 +98,7 @@ static int apci2200_auto_attach(struct comedi_device *dev,
 	/* Initialize the digital output subdevice */
 	s = &dev->subdevices[1];
 	s->type		= COMEDI_SUBD_DO;
-	s->subdev_flags	= SDF_WRITEABLE;
+	s->subdev_flags	= SDF_WRITABLE;
 	s->n_chan	= 16;
 	s->maxdata	= 1;
 	s->range_table	= &range_digital;
diff --git a/drivers/staging/comedi/drivers/addi_apci_3120.c b/drivers/staging/comedi/drivers/addi_apci_3120.c
index ba71e24a56fd..c65f9407fd06 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3120.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3120.c
@@ -1,70 +1,993 @@
+/*
+ * addi_apci_3120.c
+ * Copyright (C) 2004,2005  ADDI-DATA GmbH for the source code of this module.
+ *
+ *	ADDI-DATA GmbH
+ *	Dieselstrasse 3
+ *	D-77833 Ottersweier
+ *	Tel: +19(0)7223/9493-0
+ *	Fax: +49(0)7223/9493-92
+ *	http://www.addi-data.com
+ *	info@addi-data.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.
+ */
+
 #include <linux/module.h>
 #include <linux/pci.h>
+#include <linux/interrupt.h>
 
 #include "../comedidev.h"
 #include "comedi_fc.h"
 #include "amcc_s5933.h"
 
-#include "addi-data/addi_common.h"
+/*
+ * PCI BAR 0 register map (devpriv->amcc)
+ * see amcc_s5933.h for register and bit defines
+ */
+#define APCI3120_FIFO_ADVANCE_ON_BYTE_2		(1 << 29)
+
+/*
+ * PCI BAR 1 register map (dev->iobase)
+ */
+#define APCI3120_AI_FIFO_REG			0x00
+#define APCI3120_CTRL_REG			0x00
+#define APCI3120_CTRL_EXT_TRIG			(1 << 15)
+#define APCI3120_CTRL_GATE(x)			(1 << (12 + (x)))
+#define APCI3120_CTRL_PR(x)			(((x) & 0xf) << 8)
+#define APCI3120_CTRL_PA(x)			(((x) & 0xf) << 0)
+#define APCI3120_AI_SOFTTRIG_REG		0x02
+#define APCI3120_STATUS_REG			0x02
+#define APCI3120_STATUS_EOC_INT			(1 << 15)
+#define APCI3120_STATUS_AMCC_INT		(1 << 14)
+#define APCI3120_STATUS_EOS_INT			(1 << 13)
+#define APCI3120_STATUS_TIMER2_INT		(1 << 12)
+#define APCI3120_STATUS_INT_MASK		(0xf << 12)
+#define APCI3120_STATUS_TO_DI_BITS(x)		(((x) >> 8) & 0xf)
+#define APCI3120_STATUS_TO_VERSION(x)		(((x) >> 4) & 0xf)
+#define APCI3120_STATUS_FIFO_FULL		(1 << 2)
+#define APCI3120_STATUS_FIFO_EMPTY		(1 << 1)
+#define APCI3120_STATUS_DA_READY		(1 << 0)
+#define APCI3120_TIMER_REG			0x04
+#define APCI3120_CHANLIST_REG			0x06
+#define APCI3120_CHANLIST_INDEX(x)		(((x) & 0xf) << 8)
+#define APCI3120_CHANLIST_UNIPOLAR		(1 << 7)
+#define APCI3120_CHANLIST_GAIN(x)		(((x) & 0x3) << 4)
+#define APCI3120_CHANLIST_MUX(x)		(((x) & 0xf) << 0)
+#define APCI3120_AO_REG(x)			(0x08 + (((x) / 4) * 2))
+#define APCI3120_AO_MUX(x)			(((x) & 0x3) << 14)
+#define APCI3120_AO_DATA(x)			((x) << 0)
+#define APCI3120_TIMER_MODE_REG			0x0c
+#define APCI3120_TIMER_MODE(_t, _m)		((_m) << ((_t) * 2))
+#define APCI3120_TIMER_MODE0			0  /* I8254_MODE0 */
+#define APCI3120_TIMER_MODE2			1  /* I8254_MODE2 */
+#define APCI3120_TIMER_MODE4			2  /* I8254_MODE4 */
+#define APCI3120_TIMER_MODE5			3  /* I8254_MODE5 */
+#define APCI3120_TIMER_MODE_MASK(_t)		(3 << ((_t) * 2))
+#define APCI3120_CTR0_REG			0x0d
+#define APCI3120_CTR0_DO_BITS(x)		((x) << 4)
+#define APCI3120_CTR0_TIMER_SEL(x)		((x) << 0)
+#define APCI3120_MODE_REG			0x0e
+#define APCI3120_MODE_TIMER2_CLK_OSC		(0 << 6)
+#define APCI3120_MODE_TIMER2_CLK_OUT1		(1 << 6)
+#define APCI3120_MODE_TIMER2_CLK_EOC		(2 << 6)
+#define APCI3120_MODE_TIMER2_CLK_EOS		(3 << 6)
+#define APCI3120_MODE_TIMER2_CLK_MASK		(3 << 6)
+#define APCI3120_MODE_TIMER2_AS_TIMER		(0 << 4)
+#define APCI3120_MODE_TIMER2_AS_COUNTER		(1 << 4)
+#define APCI3120_MODE_TIMER2_AS_WDOG		(2 << 4)
+#define APCI3120_MODE_TIMER2_AS_MASK		(3 << 4)  /* sets AS_TIMER */
+#define APCI3120_MODE_SCAN_ENA			(1 << 3)
+#define APCI3120_MODE_TIMER2_IRQ_ENA		(1 << 2)
+#define APCI3120_MODE_EOS_IRQ_ENA		(1 << 1)
+#define APCI3120_MODE_EOC_IRQ_ENA		(1 << 0)
+
+/*
+ * PCI BAR 2 register map (devpriv->addon)
+ */
+#define APCI3120_ADDON_ADDR_REG			0x00
+#define APCI3120_ADDON_DATA_REG			0x02
+#define APCI3120_ADDON_CTRL_REG			0x04
+#define APCI3120_ADDON_CTRL_AMWEN_ENA		(1 << 1)
+#define APCI3120_ADDON_CTRL_A2P_FIFO_ENA	(1 << 0)
 
-#include "addi-data/hwdrv_apci3120.c"
+/*
+ * Board revisions
+ */
+#define APCI3120_REVA				0xa
+#define APCI3120_REVB				0xb
+#define APCI3120_REVA_OSC_BASE			70	/* 70ns = 14.29MHz */
+#define APCI3120_REVB_OSC_BASE			50	/* 50ns = 20MHz */
+
+static const struct comedi_lrange apci3120_ai_range = {
+	8, {
+		BIP_RANGE(10),
+		BIP_RANGE(5),
+		BIP_RANGE(2),
+		BIP_RANGE(1),
+		UNI_RANGE(10),
+		UNI_RANGE(5),
+		UNI_RANGE(2),
+		UNI_RANGE(1)
+	}
+};
 
 enum apci3120_boardid {
 	BOARD_APCI3120,
 	BOARD_APCI3001,
 };
 
-static const struct addi_board apci3120_boardtypes[] = {
+struct apci3120_board {
+	const char *name;
+	unsigned int ai_is_16bit:1;
+	unsigned int has_ao:1;
+};
+
+static const struct apci3120_board apci3120_boardtypes[] = {
 	[BOARD_APCI3120] = {
-		.pc_DriverName		= "apci3120",
-		.i_NbrAiChannel		= 16,
-		.i_NbrAiChannelDiff	= 8,
-		.i_AiChannelList	= 16,
-		.i_NbrAoChannel		= 8,
-		.i_AiMaxdata		= 0xffff,
-		.i_AoMaxdata		= 0x3fff,
-		.i_NbrDiChannel		= 4,
-		.i_NbrDoChannel		= 4,
-		.i_DoMaxdata		= 0x0f,
-		.interrupt		= apci3120_interrupt,
+		.name		= "apci3120",
+		.ai_is_16bit	= 1,
+		.has_ao		= 1,
 	},
 	[BOARD_APCI3001] = {
-		.pc_DriverName		= "apci3001",
-		.i_NbrAiChannel		= 16,
-		.i_NbrAiChannelDiff	= 8,
-		.i_AiChannelList	= 16,
-		.i_AiMaxdata		= 0xfff,
-		.i_NbrDiChannel		= 4,
-		.i_NbrDoChannel		= 4,
-		.i_DoMaxdata		= 0x0f,
-		.interrupt		= apci3120_interrupt,
+		.name		= "apci3001",
 	},
 };
 
-static irqreturn_t v_ADDI_Interrupt(int irq, void *d)
+struct apci3120_dmabuf {
+	unsigned short *virt;
+	dma_addr_t hw;
+	unsigned int size;
+	unsigned int use_size;
+};
+
+struct apci3120_private {
+	unsigned long amcc;
+	unsigned long addon;
+	unsigned int osc_base;
+	unsigned int use_dma:1;
+	unsigned int use_double_buffer:1;
+	unsigned int cur_dmabuf:1;
+	struct apci3120_dmabuf dmabuf[2];
+	unsigned char do_bits;
+	unsigned char timer_mode;
+	unsigned char mode;
+	unsigned short ctrl;
+};
+
+static void apci3120_addon_write(struct comedi_device *dev,
+				 unsigned int val, unsigned int reg)
+{
+	struct apci3120_private *devpriv = dev->private;
+
+	/* 16-bit interface for AMCC add-on registers */
+
+	outw(reg, devpriv->addon + APCI3120_ADDON_ADDR_REG);
+	outw(val & 0xffff, devpriv->addon + APCI3120_ADDON_DATA_REG);
+
+	outw(reg + 2, devpriv->addon + APCI3120_ADDON_ADDR_REG);
+	outw((val >> 16) & 0xffff, devpriv->addon + APCI3120_ADDON_DATA_REG);
+}
+
+static void apci3120_init_dma(struct comedi_device *dev,
+			      struct apci3120_dmabuf *dmabuf)
+{
+	struct apci3120_private *devpriv = dev->private;
+
+	/* AMCC - enable transfer count and reset A2P FIFO */
+	outl(AGCSTS_TC_ENABLE | AGCSTS_RESET_A2P_FIFO,
+	     devpriv->amcc + AMCC_OP_REG_AGCSTS);
+
+	/* Add-On - enable transfer count and reset A2P FIFO */
+	apci3120_addon_write(dev, AGCSTS_TC_ENABLE | AGCSTS_RESET_A2P_FIFO,
+			     AMCC_OP_REG_AGCSTS);
+
+	/* AMCC - enable transfers and reset A2P flags */
+	outl(RESET_A2P_FLAGS | EN_A2P_TRANSFERS,
+	     devpriv->amcc + AMCC_OP_REG_MCSR);
+
+	/* Add-On - DMA start address */
+	apci3120_addon_write(dev, dmabuf->hw, AMCC_OP_REG_AMWAR);
+
+	/* Add-On - Number of acquisitions */
+	apci3120_addon_write(dev, dmabuf->use_size, AMCC_OP_REG_AMWTC);
+
+	/* AMCC - enable write complete (DMA) and set FIFO advance */
+	outl(APCI3120_FIFO_ADVANCE_ON_BYTE_2 | AINT_WRITE_COMPL,
+	     devpriv->amcc + AMCC_OP_REG_INTCSR);
+
+	/* Add-On - enable DMA */
+	outw(APCI3120_ADDON_CTRL_AMWEN_ENA | APCI3120_ADDON_CTRL_A2P_FIFO_ENA,
+	     devpriv->addon + APCI3120_ADDON_CTRL_REG);
+}
+
+static void apci3120_setup_dma(struct comedi_device *dev,
+			       struct comedi_subdevice *s)
+{
+	struct apci3120_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
+	struct apci3120_dmabuf *dmabuf0 = &devpriv->dmabuf[0];
+	struct apci3120_dmabuf *dmabuf1 = &devpriv->dmabuf[1];
+	unsigned int dmalen0 = dmabuf0->size;
+	unsigned int dmalen1 = dmabuf1->size;
+	unsigned int scan_bytes;
+
+	scan_bytes = comedi_samples_to_bytes(s, cmd->scan_end_arg);
+
+	if (cmd->stop_src == TRIG_COUNT) {
+		/*
+		 * Must we fill full first buffer? And must we fill
+		 * full second buffer when first is once filled?
+		 */
+		if (dmalen0 > (cmd->stop_arg * scan_bytes))
+			dmalen0 = cmd->stop_arg * scan_bytes;
+		else if (dmalen1 > (cmd->stop_arg * scan_bytes - dmalen0))
+			dmalen1 = cmd->stop_arg * scan_bytes - dmalen0;
+	}
+
+	if (cmd->flags & CMDF_WAKE_EOS) {
+		/* don't we want wake up every scan? */
+		if (dmalen0 > scan_bytes) {
+			dmalen0 = scan_bytes;
+			if (cmd->scan_end_arg & 1)
+				dmalen0 += 2;
+		}
+		if (dmalen1 > scan_bytes) {
+			dmalen1 = scan_bytes;
+			if (cmd->scan_end_arg & 1)
+				dmalen1 -= 2;
+			if (dmalen1 < 4)
+				dmalen1 = 4;
+		}
+	} else {
+		/* isn't output buff smaller that our DMA buff? */
+		if (dmalen0 > s->async->prealloc_bufsz)
+			dmalen0 = s->async->prealloc_bufsz;
+		if (dmalen1 > s->async->prealloc_bufsz)
+			dmalen1 = s->async->prealloc_bufsz;
+	}
+	dmabuf0->use_size = dmalen0;
+	dmabuf1->use_size = dmalen1;
+
+	apci3120_init_dma(dev, dmabuf0);
+}
+
+/*
+ * There are three timers on the board. They all use the same base
+ * clock with a fixed prescaler for each timer. The base clock used
+ * depends on the board version and type.
+ *
+ * APCI-3120 Rev A boards OSC = 14.29MHz base clock (~70ns)
+ * APCI-3120 Rev B boards OSC = 20MHz base clock (50ns)
+ * APCI-3001 boards OSC = 20MHz base clock (50ns)
+ *
+ * The prescalers for each timer are:
+ * Timer 0 CLK = OSC/10
+ * Timer 1 CLK = OSC/1000
+ * Timer 2 CLK = OSC/1000
+ */
+static unsigned int apci3120_ns_to_timer(struct comedi_device *dev,
+					 unsigned int timer,
+					 unsigned int ns,
+					 unsigned int flags)
+{
+	struct apci3120_private *devpriv = dev->private;
+	unsigned int prescale = (timer == 0) ? 10 : 1000;
+	unsigned int timer_base = devpriv->osc_base * prescale;
+	unsigned int divisor;
+
+	switch (flags & CMDF_ROUND_MASK) {
+	case CMDF_ROUND_UP:
+		divisor = DIV_ROUND_UP(ns, timer_base);
+		break;
+	case CMDF_ROUND_DOWN:
+		divisor = ns / timer_base;
+		break;
+	case CMDF_ROUND_NEAREST:
+	default:
+		divisor = DIV_ROUND_CLOSEST(ns, timer_base);
+		break;
+	}
+
+	if (timer == 2) {
+		/* timer 2 is 24-bits */
+		if (divisor > 0x00ffffff)
+			divisor = 0x00ffffff;
+	} else {
+		/* timers 0 and 1 are 16-bits */
+		if (divisor > 0xffff)
+			divisor = 0xffff;
+	}
+	/* the timers require a minimum divisor of 2 */
+	if (divisor < 2)
+		divisor = 2;
+
+	return divisor;
+}
+
+static void apci3120_clr_timer2_interrupt(struct comedi_device *dev)
+{
+	/* a dummy read of APCI3120_CTR0_REG clears the timer 2 interrupt */
+	inb(dev->iobase + APCI3120_CTR0_REG);
+}
+
+static void apci3120_timer_write(struct comedi_device *dev,
+				 unsigned int timer, unsigned int val)
+{
+	struct apci3120_private *devpriv = dev->private;
+
+	/* write 16-bit value to timer (lower 16-bits of timer 2) */
+	outb(APCI3120_CTR0_DO_BITS(devpriv->do_bits) |
+	     APCI3120_CTR0_TIMER_SEL(timer),
+	     dev->iobase + APCI3120_CTR0_REG);
+	outw(val & 0xffff, dev->iobase + APCI3120_TIMER_REG);
+
+	if (timer == 2) {
+		/* write upper 16-bits to timer 2 */
+		outb(APCI3120_CTR0_DO_BITS(devpriv->do_bits) |
+		     APCI3120_CTR0_TIMER_SEL(timer + 1),
+		     dev->iobase + APCI3120_CTR0_REG);
+		outw((val >> 16) & 0xffff, dev->iobase + APCI3120_TIMER_REG);
+	}
+}
+
+static unsigned int apci3120_timer_read(struct comedi_device *dev,
+					unsigned int timer)
+{
+	struct apci3120_private *devpriv = dev->private;
+	unsigned int val;
+
+	/* read 16-bit value from timer (lower 16-bits of timer 2) */
+	outb(APCI3120_CTR0_DO_BITS(devpriv->do_bits) |
+	     APCI3120_CTR0_TIMER_SEL(timer),
+	     dev->iobase + APCI3120_CTR0_REG);
+	val = inw(dev->iobase + APCI3120_TIMER_REG);
+
+	if (timer == 2) {
+		/* read upper 16-bits from timer 2 */
+		outb(APCI3120_CTR0_DO_BITS(devpriv->do_bits) |
+		     APCI3120_CTR0_TIMER_SEL(timer + 1),
+		     dev->iobase + APCI3120_CTR0_REG);
+		val |= (inw(dev->iobase + APCI3120_TIMER_REG) << 16);
+	}
+
+	return val;
+}
+
+static void apci3120_timer_set_mode(struct comedi_device *dev,
+				    unsigned int timer, unsigned int mode)
+{
+	struct apci3120_private *devpriv = dev->private;
+
+	devpriv->timer_mode &= ~APCI3120_TIMER_MODE_MASK(timer);
+	devpriv->timer_mode |= APCI3120_TIMER_MODE(timer, mode);
+	outb(devpriv->timer_mode, dev->iobase + APCI3120_TIMER_MODE_REG);
+}
+
+static void apci3120_timer_enable(struct comedi_device *dev,
+				  unsigned int timer, bool enable)
+{
+	struct apci3120_private *devpriv = dev->private;
+
+	if (enable)
+		devpriv->ctrl |= APCI3120_CTRL_GATE(timer);
+	else
+		devpriv->ctrl &= ~APCI3120_CTRL_GATE(timer);
+	outw(devpriv->ctrl, dev->iobase + APCI3120_CTRL_REG);
+}
+
+static void apci3120_exttrig_enable(struct comedi_device *dev, bool enable)
+{
+	struct apci3120_private *devpriv = dev->private;
+
+	if (enable)
+		devpriv->ctrl |= APCI3120_CTRL_EXT_TRIG;
+	else
+		devpriv->ctrl &= ~APCI3120_CTRL_EXT_TRIG;
+	outw(devpriv->ctrl, dev->iobase + APCI3120_CTRL_REG);
+}
+
+static void apci3120_set_chanlist(struct comedi_device *dev,
+				  struct comedi_subdevice *s,
+				  int n_chan, unsigned int *chanlist)
+{
+	struct apci3120_private *devpriv = dev->private;
+	int i;
+
+	/* set chanlist for scan */
+	for (i = 0; i < n_chan; i++) {
+		unsigned int chan = CR_CHAN(chanlist[i]);
+		unsigned int range = CR_RANGE(chanlist[i]);
+		unsigned int val;
+
+		val = APCI3120_CHANLIST_MUX(chan) |
+		      APCI3120_CHANLIST_GAIN(range) |
+		      APCI3120_CHANLIST_INDEX(i);
+
+		if (comedi_range_is_unipolar(s, range))
+			val |= APCI3120_CHANLIST_UNIPOLAR;
+
+		outw(val, dev->iobase + APCI3120_CHANLIST_REG);
+	}
+
+	/* a dummy read of APCI3120_TIMER_MODE_REG resets the ai FIFO */
+	inw(dev->iobase + APCI3120_TIMER_MODE_REG);
+
+	/* set scan length (PR) and scan start (PA) */
+	devpriv->ctrl = APCI3120_CTRL_PR(n_chan - 1) | APCI3120_CTRL_PA(0);
+	outw(devpriv->ctrl, dev->iobase + APCI3120_CTRL_REG);
+
+	/* enable chanlist scanning if necessary */
+	if (n_chan > 1)
+		devpriv->mode |= APCI3120_MODE_SCAN_ENA;
+}
+
+static void apci3120_interrupt_dma(struct comedi_device *dev,
+				   struct comedi_subdevice *s)
+{
+	struct apci3120_private *devpriv = dev->private;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
+	struct apci3120_dmabuf *dmabuf;
+	unsigned int nbytes;
+	unsigned int nsamples;
+
+	dmabuf = &devpriv->dmabuf[devpriv->cur_dmabuf];
+
+	nbytes = dmabuf->use_size - inl(devpriv->amcc + AMCC_OP_REG_MWTC);
+
+	if (nbytes < dmabuf->use_size)
+		dev_err(dev->class_dev, "Interrupted DMA transfer!\n");
+	if (nbytes & 1) {
+		dev_err(dev->class_dev, "Odd count of bytes in DMA ring!\n");
+		async->events |= COMEDI_CB_ERROR;
+		return;
+	}
+
+	nsamples = comedi_bytes_to_samples(s, nbytes);
+	if (nsamples) {
+		comedi_buf_write_samples(s, dmabuf->virt, nsamples);
+
+		if (!(cmd->flags & CMDF_WAKE_EOS))
+			async->events |= COMEDI_CB_EOS;
+	}
+
+	if ((async->events & COMEDI_CB_CANCEL_MASK) ||
+	    (cmd->stop_src == TRIG_COUNT && async->scans_done >= cmd->stop_arg))
+		return;
+
+	if (devpriv->use_double_buffer) {
+		/* switch DMA buffers for next interrupt */
+		devpriv->cur_dmabuf = !devpriv->cur_dmabuf;
+		dmabuf = &devpriv->dmabuf[devpriv->cur_dmabuf];
+		apci3120_init_dma(dev, dmabuf);
+	} else {
+		/* restart DMA if not using double buffering */
+		apci3120_init_dma(dev, dmabuf);
+	}
+}
+
+static irqreturn_t apci3120_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	const struct addi_board *this_board = dev->board_ptr;
+	struct apci3120_private *devpriv = dev->private;
+	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
+	unsigned int status;
+	unsigned int int_amcc;
+
+	status = inw(dev->iobase + APCI3120_STATUS_REG);
+	int_amcc = inl(devpriv->amcc + AMCC_OP_REG_INTCSR);
+
+	if (!(status & APCI3120_STATUS_INT_MASK) &&
+	    !(int_amcc & ANY_S593X_INT)) {
+		dev_err(dev->class_dev, "IRQ from unknown source\n");
+		return IRQ_NONE;
+	}
+
+	outl(int_amcc | AINT_INT_MASK, devpriv->amcc + AMCC_OP_REG_INTCSR);
 
-	this_board->interrupt(irq, d);
-	return IRQ_RETVAL(1);
+	if (devpriv->ctrl & APCI3120_CTRL_EXT_TRIG)
+		apci3120_exttrig_enable(dev, false);
+
+	if (int_amcc & MASTER_ABORT_INT)
+		dev_err(dev->class_dev, "AMCC IRQ - MASTER DMA ABORT!\n");
+	if (int_amcc & TARGET_ABORT_INT)
+		dev_err(dev->class_dev, "AMCC IRQ - TARGET DMA ABORT!\n");
+
+	if ((status & APCI3120_STATUS_EOC_INT) == 0 &&
+	    (devpriv->mode & APCI3120_MODE_EOC_IRQ_ENA)) {
+		/* nothing to do... EOC mode is not currently used */
+	}
+
+	if ((status & APCI3120_STATUS_EOS_INT) &&
+	    (devpriv->mode & APCI3120_MODE_EOS_IRQ_ENA)) {
+		unsigned short val;
+		int i;
+
+		for (i = 0; i < cmd->chanlist_len; i++) {
+			val = inw(dev->iobase + APCI3120_AI_FIFO_REG);
+			comedi_buf_write_samples(s, &val, 1);
+		}
+
+		devpriv->mode |= APCI3120_MODE_EOS_IRQ_ENA;
+		outb(devpriv->mode, dev->iobase + APCI3120_MODE_REG);
+	}
+
+	if (status & APCI3120_STATUS_TIMER2_INT) {
+		/*
+		 * for safety...
+		 * timer2 interrupts are not enabled in the driver
+		 */
+		apci3120_clr_timer2_interrupt(dev);
+	}
+
+	if (status & APCI3120_STATUS_AMCC_INT) {
+		/* AMCC- Clear write complete interrupt (DMA) */
+		outl(AINT_WT_COMPLETE, devpriv->amcc + AMCC_OP_REG_INTCSR);
+
+		/* do some data transfer */
+		apci3120_interrupt_dma(dev, s);
+	}
+
+	if (cmd->stop_src == TRIG_COUNT && async->scans_done >= cmd->stop_arg)
+		async->events |= COMEDI_CB_EOA;
+
+	comedi_handle_events(dev, s);
+
+	return IRQ_HANDLED;
+}
+
+static int apci3120_ai_cmd(struct comedi_device *dev,
+			   struct comedi_subdevice *s)
+{
+	struct apci3120_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
+	unsigned int divisor;
+
+	/* set default mode bits */
+	devpriv->mode = APCI3120_MODE_TIMER2_CLK_OSC |
+			APCI3120_MODE_TIMER2_AS_TIMER;
+
+	/* AMCC- Clear write complete interrupt (DMA) */
+	outl(AINT_WT_COMPLETE, devpriv->amcc + AMCC_OP_REG_INTCSR);
+
+	devpriv->cur_dmabuf = 0;
+
+	/* load chanlist for command scan */
+	apci3120_set_chanlist(dev, s, cmd->chanlist_len, cmd->chanlist);
+
+	if (cmd->start_src == TRIG_EXT)
+		apci3120_exttrig_enable(dev, true);
+
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		/*
+		 * Timer 1 is used in MODE2 (rate generator) to set the
+		 * start time for each scan.
+		 */
+		divisor = apci3120_ns_to_timer(dev, 1, cmd->scan_begin_arg,
+					       cmd->flags);
+		apci3120_timer_set_mode(dev, 1, APCI3120_TIMER_MODE2);
+		apci3120_timer_write(dev, 1, divisor);
+	}
+
+	/*
+	 * Timer 0 is used in MODE2 (rate generator) to set the conversion
+	 * time for each acquisition.
+	 */
+	divisor = apci3120_ns_to_timer(dev, 0, cmd->convert_arg, cmd->flags);
+	apci3120_timer_set_mode(dev, 0, APCI3120_TIMER_MODE2);
+	apci3120_timer_write(dev, 0, divisor);
+
+	if (devpriv->use_dma)
+		apci3120_setup_dma(dev, s);
+	else
+		devpriv->mode |= APCI3120_MODE_EOS_IRQ_ENA;
+
+	/* set mode to enable acquisition */
+	outb(devpriv->mode, dev->iobase + APCI3120_MODE_REG);
+
+	if (cmd->scan_begin_src == TRIG_TIMER)
+		apci3120_timer_enable(dev, 1, true);
+	apci3120_timer_enable(dev, 0, true);
+
+	return 0;
+}
+
+static int apci3120_ai_cmdtest(struct comedi_device *dev,
+			       struct comedi_subdevice *s,
+			       struct comedi_cmd *cmd)
+{
+	unsigned int arg;
+	int err = 0;
+
+	/* Step 1 : check if triggers are trivially valid */
+
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+					TRIG_TIMER | TRIG_FOLLOW);
+	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER);
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+
+	if (err)
+		return 1;
+
+	/* Step 2a : make sure trigger sources are unique */
+
+	err |= cfc_check_trigger_is_unique(cmd->start_src);
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
+
+	if (err)
+		return 2;
+
+	/* Step 3: check if arguments are trivially valid */
+
+	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+
+	if (cmd->scan_begin_src == TRIG_TIMER)	/* Test Delay timing */
+		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg, 100000);
+
+	/* minimum conversion time per sample is 10us */
+	err |= cfc_check_trigger_arg_min(&cmd->convert_arg, 10000);
+
+	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/*  TRIG_NONE */
+		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+
+	if (err)
+		return 3;
+
+	/* Step 4: fix up any arguments */
+
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		/* scan begin must be larger than the scan time */
+		arg = cmd->convert_arg * cmd->scan_end_arg;
+		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg, arg);
+	}
+
+	if (err)
+		return 4;
+
+	/* Step 5: check channel list if it exists */
+
+	return 0;
+}
+
+static int apci3120_cancel(struct comedi_device *dev,
+			   struct comedi_subdevice *s)
+{
+	struct apci3120_private *devpriv = dev->private;
+
+	/* Add-On - disable DMA */
+	outw(0, devpriv->addon + 4);
+
+	/* Add-On - disable bus master */
+	apci3120_addon_write(dev, 0, AMCC_OP_REG_AGCSTS);
+
+	/* AMCC - disable bus master */
+	outl(0, devpriv->amcc + AMCC_OP_REG_MCSR);
+
+	/* disable all counters, ext trigger, and reset scan */
+	devpriv->ctrl = 0;
+	outw(devpriv->ctrl, dev->iobase + APCI3120_CTRL_REG);
+
+	/* DISABLE_ALL_INTERRUPT */
+	devpriv->mode = 0;
+	outb(devpriv->mode, dev->iobase + APCI3120_MODE_REG);
+
+	inw(dev->iobase + APCI3120_STATUS_REG);
+	devpriv->cur_dmabuf = 0;
+
+	return 0;
+}
+
+static int apci3120_ai_eoc(struct comedi_device *dev,
+			   struct comedi_subdevice *s,
+			   struct comedi_insn *insn,
+			   unsigned long context)
+{
+	unsigned int status;
+
+	status = inw(dev->iobase + APCI3120_STATUS_REG);
+	if ((status & APCI3120_STATUS_EOC_INT) == 0)
+		return 0;
+	return -EBUSY;
+}
+
+static int apci3120_ai_insn_read(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
+{
+	struct apci3120_private *devpriv = dev->private;
+	unsigned int divisor;
+	int ret;
+	int i;
+
+	/* set mode for A/D conversions by software trigger with timer 0 */
+	devpriv->mode = APCI3120_MODE_TIMER2_CLK_OSC |
+			APCI3120_MODE_TIMER2_AS_TIMER;
+	outb(devpriv->mode, dev->iobase + APCI3120_MODE_REG);
+
+	/* load chanlist for single channel scan */
+	apci3120_set_chanlist(dev, s, 1, &insn->chanspec);
+
+	/*
+	 * Timer 0 is used in MODE4 (software triggered strobe) to set the
+	 * conversion time for each acquisition. Each conversion is triggered
+	 * when the divisor is written to the timer, The conversion is done
+	 * when the EOC bit in the status register is '0'.
+	 */
+	apci3120_timer_set_mode(dev, 0, APCI3120_TIMER_MODE4);
+	apci3120_timer_enable(dev, 0, true);
+
+	/* fixed conversion time of 10 us */
+	divisor = apci3120_ns_to_timer(dev, 0, 10000, CMDF_ROUND_NEAREST);
+
+	for (i = 0; i < insn->n; i++) {
+		/* trigger conversion */
+		apci3120_timer_write(dev, 0, divisor);
+
+		ret = comedi_timeout(dev, s, insn, apci3120_ai_eoc, 0);
+		if (ret)
+			return ret;
+
+		data[i] = inw(dev->iobase + APCI3120_AI_FIFO_REG);
+	}
+
+	return insn->n;
+}
+
+static int apci3120_ao_ready(struct comedi_device *dev,
+			     struct comedi_subdevice *s,
+			     struct comedi_insn *insn,
+			     unsigned long context)
+{
+	unsigned int status;
+
+	status = inw(dev->iobase + APCI3120_STATUS_REG);
+	if (status & APCI3120_STATUS_DA_READY)
+		return 0;
+	return -EBUSY;
+}
+
+static int apci3120_ao_insn_write(struct comedi_device *dev,
+				  struct comedi_subdevice *s,
+				  struct comedi_insn *insn,
+				  unsigned int *data)
+{
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	int i;
+
+	for (i = 0; i < insn->n; i++) {
+		unsigned int val = data[i];
+		int ret;
+
+		ret = comedi_timeout(dev, s, insn, apci3120_ao_ready, 0);
+		if (ret)
+			return ret;
+
+		outw(APCI3120_AO_MUX(chan) | APCI3120_AO_DATA(val),
+		     dev->iobase + APCI3120_AO_REG(chan));
+
+		s->readback[chan] = val;
+	}
+
+	return insn->n;
+}
+
+static int apci3120_di_insn_bits(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
+{
+	unsigned int status;
+
+	status = inw(dev->iobase + APCI3120_STATUS_REG);
+	data[1] = APCI3120_STATUS_TO_DI_BITS(status);
+
+	return insn->n;
+}
+
+static int apci3120_do_insn_bits(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
+{
+	struct apci3120_private *devpriv = dev->private;
+
+	if (comedi_dio_update_state(s, data)) {
+		devpriv->do_bits = s->state;
+		outb(APCI3120_CTR0_DO_BITS(devpriv->do_bits),
+		     dev->iobase + APCI3120_CTR0_REG);
+	}
+
+	data[1] = s->state;
+
+	return insn->n;
+}
+
+static int apci3120_timer_insn_config(struct comedi_device *dev,
+				      struct comedi_subdevice *s,
+				      struct comedi_insn *insn,
+				      unsigned int *data)
+{
+	struct apci3120_private *devpriv = dev->private;
+	unsigned int divisor;
+	unsigned int status;
+	unsigned int mode;
+	unsigned int timer_mode;
+
+	switch (data[0]) {
+	case INSN_CONFIG_ARM:
+		apci3120_clr_timer2_interrupt(dev);
+		divisor = apci3120_ns_to_timer(dev, 2, data[1],
+					       CMDF_ROUND_DOWN);
+		apci3120_timer_write(dev, 2, divisor);
+		apci3120_timer_enable(dev, 2, true);
+		break;
+
+	case INSN_CONFIG_DISARM:
+		apci3120_timer_enable(dev, 2, false);
+		apci3120_clr_timer2_interrupt(dev);
+		break;
+
+	case INSN_CONFIG_GET_COUNTER_STATUS:
+		data[1] = 0;
+		data[2] = COMEDI_COUNTER_ARMED | COMEDI_COUNTER_COUNTING |
+			  COMEDI_COUNTER_TERMINAL_COUNT;
+
+		if (devpriv->ctrl & APCI3120_CTRL_GATE(2)) {
+			data[1] |= COMEDI_COUNTER_ARMED;
+			data[1] |= COMEDI_COUNTER_COUNTING;
+		}
+		status = inw(dev->iobase + APCI3120_STATUS_REG);
+		if (status & APCI3120_STATUS_TIMER2_INT) {
+			data[1] &= ~COMEDI_COUNTER_COUNTING;
+			data[1] |= COMEDI_COUNTER_TERMINAL_COUNT;
+		}
+		break;
+
+	case INSN_CONFIG_SET_COUNTER_MODE:
+		switch (data[1]) {
+		case I8254_MODE0:
+			mode = APCI3120_MODE_TIMER2_AS_COUNTER;
+			timer_mode = APCI3120_TIMER_MODE0;
+			break;
+		case I8254_MODE2:
+			mode = APCI3120_MODE_TIMER2_AS_TIMER;
+			timer_mode = APCI3120_TIMER_MODE2;
+			break;
+		case I8254_MODE4:
+			mode = APCI3120_MODE_TIMER2_AS_TIMER;
+			timer_mode = APCI3120_TIMER_MODE4;
+			break;
+		case I8254_MODE5:
+			mode = APCI3120_MODE_TIMER2_AS_WDOG;
+			timer_mode = APCI3120_TIMER_MODE5;
+			break;
+		default:
+			return -EINVAL;
+		}
+		apci3120_timer_enable(dev, 2, false);
+		apci3120_clr_timer2_interrupt(dev);
+		apci3120_timer_set_mode(dev, 2, timer_mode);
+		devpriv->mode &= ~APCI3120_MODE_TIMER2_AS_MASK;
+		devpriv->mode |= mode;
+		outb(devpriv->mode, dev->iobase + APCI3120_MODE_REG);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return insn->n;
+}
+
+static int apci3120_timer_insn_read(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_insn *insn,
+				    unsigned int *data)
+{
+	int i;
+
+	for (i = 0; i < insn->n; i++)
+		data[i] = apci3120_timer_read(dev, 2);
+
+	return insn->n;
+}
+
+static void apci3120_dma_alloc(struct comedi_device *dev)
+{
+	struct apci3120_private *devpriv = dev->private;
+	struct apci3120_dmabuf *dmabuf;
+	int order;
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		dmabuf = &devpriv->dmabuf[i];
+		for (order = 2; order >= 0; order--) {
+			dmabuf->virt = dma_alloc_coherent(dev->hw_dev,
+							  PAGE_SIZE << order,
+							  &dmabuf->hw,
+							  GFP_KERNEL);
+			if (dmabuf->virt)
+				break;
+		}
+		if (!dmabuf->virt)
+			break;
+		dmabuf->size = PAGE_SIZE << order;
+
+		if (i == 0)
+			devpriv->use_dma = 1;
+		if (i == 1)
+			devpriv->use_double_buffer = 1;
+	}
+}
+
+static void apci3120_dma_free(struct comedi_device *dev)
+{
+	struct apci3120_private *devpriv = dev->private;
+	struct apci3120_dmabuf *dmabuf;
+	int i;
+
+	if (!devpriv)
+		return;
+
+	for (i = 0; i < 2; i++) {
+		dmabuf = &devpriv->dmabuf[i];
+		if (dmabuf->virt) {
+			dma_free_coherent(dev->hw_dev, dmabuf->size,
+					  dmabuf->virt, dmabuf->hw);
+		}
+	}
+}
+
+static void apci3120_reset(struct comedi_device *dev)
+{
+	/* disable all interrupt sources */
+	outb(0, dev->iobase + APCI3120_MODE_REG);
+
+	/* disable all counters, ext trigger, and reset scan */
+	outw(0, dev->iobase + APCI3120_CTRL_REG);
+
+	/* clear interrupt status */
+	inw(dev->iobase + APCI3120_STATUS_REG);
 }
 
 static int apci3120_auto_attach(struct comedi_device *dev,
 				unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct addi_board *this_board = NULL;
-	struct addi_private *devpriv;
+	const struct apci3120_board *this_board = NULL;
+	struct apci3120_private *devpriv;
 	struct comedi_subdevice *s;
-	int ret, order, i;
+	unsigned int status;
+	int ret;
 
 	if (context < ARRAY_SIZE(apci3120_boardtypes))
 		this_board = &apci3120_boardtypes[context];
 	if (!this_board)
 		return -ENODEV;
 	dev->board_ptr = this_board;
-	dev->board_name = this_board->pc_DriverName;
+	dev->board_name = this_board->name;
 
 	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
 	if (!devpriv)
@@ -76,136 +999,100 @@ static int apci3120_auto_attach(struct comedi_device *dev,
 	pci_set_master(pcidev);
 
 	dev->iobase = pci_resource_start(pcidev, 1);
-	devpriv->iobase = dev->iobase;
-	devpriv->i_IobaseAmcc = pci_resource_start(pcidev, 0);
-	devpriv->i_IobaseAddon = pci_resource_start(pcidev, 2);
-	devpriv->i_IobaseReserved = pci_resource_start(pcidev, 3);
+	devpriv->amcc = pci_resource_start(pcidev, 0);
+	devpriv->addon = pci_resource_start(pcidev, 2);
+
+	apci3120_reset(dev);
 
 	if (pcidev->irq > 0) {
-		ret = request_irq(pcidev->irq, v_ADDI_Interrupt, IRQF_SHARED,
+		ret = request_irq(pcidev->irq, apci3120_interrupt, IRQF_SHARED,
 				  dev->board_name, dev);
-		if (ret == 0)
+		if (ret == 0) {
 			dev->irq = pcidev->irq;
-	}
 
-	/* Allocate DMA buffers */
-	for (i = 0; i < 2; i++) {
-		for (order = 2; order >= 0; order--) {
-			devpriv->ul_DmaBufferVirtual[i] =
-			    dma_alloc_coherent(dev->hw_dev, PAGE_SIZE << order,
-					       &devpriv->ul_DmaBufferHw[i],
-					       GFP_KERNEL);
-
-			if (devpriv->ul_DmaBufferVirtual[i])
-				break;
+			apci3120_dma_alloc(dev);
 		}
-		if (!devpriv->ul_DmaBufferVirtual[i])
-			break;
-		devpriv->ui_DmaBufferSize[i] = PAGE_SIZE << order;
 	}
-	if (devpriv->ul_DmaBufferVirtual[0])
-		devpriv->us_UseDma = 1;
 
-	if (devpriv->ul_DmaBufferVirtual[1])
-		devpriv->b_DmaDoubleBuffer = 1;
+	status = inw(dev->iobase + APCI3120_STATUS_REG);
+	if (APCI3120_STATUS_TO_VERSION(status) == APCI3120_REVB ||
+	    context == BOARD_APCI3001)
+		devpriv->osc_base = APCI3120_REVB_OSC_BASE;
+	else
+		devpriv->osc_base = APCI3120_REVA_OSC_BASE;
 
 	ret = comedi_alloc_subdevices(dev, 5);
 	if (ret)
 		return ret;
 
-	/*  Allocate and Initialise AI Subdevice Structures */
+	/* Analog Input subdevice */
 	s = &dev->subdevices[0];
-	dev->read_subdev = s;
-	s->type = COMEDI_SUBD_AI;
-	s->subdev_flags =
-		SDF_READABLE | SDF_COMMON | SDF_GROUND
-		| SDF_DIFF;
-	if (this_board->i_NbrAiChannel)
-		s->n_chan = this_board->i_NbrAiChannel;
-	else
-		s->n_chan = this_board->i_NbrAiChannelDiff;
-	s->maxdata = this_board->i_AiMaxdata;
-	s->len_chanlist = this_board->i_AiChannelList;
-	s->range_table = &range_apci3120_ai;
-
-	s->insn_config = apci3120_ai_insn_config;
-	s->insn_read = apci3120_ai_insn_read;
-	s->do_cmdtest = apci3120_ai_cmdtest;
-	s->do_cmd = apci3120_ai_cmd;
-	s->cancel = apci3120_cancel;
-
-	/*  Allocate and Initialise AO Subdevice Structures */
+	s->type		= COMEDI_SUBD_AI;
+	s->subdev_flags	= SDF_READABLE | SDF_COMMON | SDF_GROUND | SDF_DIFF;
+	s->n_chan	= 16;
+	s->maxdata	= this_board->ai_is_16bit ? 0xffff : 0x0fff;
+	s->range_table	= &apci3120_ai_range;
+	s->insn_read	= apci3120_ai_insn_read;
+	if (dev->irq) {
+		dev->read_subdev = s;
+		s->subdev_flags	|= SDF_CMD_READ;
+		s->len_chanlist	= s->n_chan;
+		s->do_cmdtest	= apci3120_ai_cmdtest;
+		s->do_cmd	= apci3120_ai_cmd;
+		s->cancel	= apci3120_cancel;
+	}
+
+	/* Analog Output subdevice */
 	s = &dev->subdevices[1];
-	if (this_board->i_NbrAoChannel) {
-		s->type = COMEDI_SUBD_AO;
-		s->subdev_flags = SDF_WRITEABLE | SDF_GROUND | SDF_COMMON;
-		s->n_chan = this_board->i_NbrAoChannel;
-		s->maxdata = this_board->i_AoMaxdata;
-		s->len_chanlist = this_board->i_NbrAoChannel;
-		s->range_table = &range_apci3120_ao;
-		s->insn_write = apci3120_ao_insn_write;
+	if (this_board->has_ao) {
+		s->type		= COMEDI_SUBD_AO;
+		s->subdev_flags	= SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
+		s->n_chan	= 8;
+		s->maxdata	= 0x3fff;
+		s->range_table	= &range_bipolar10;
+		s->insn_write	= apci3120_ao_insn_write;
+
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
 	} else {
-		s->type = COMEDI_SUBD_UNUSED;
+		s->type		= COMEDI_SUBD_UNUSED;
 	}
 
-	/*  Allocate and Initialise DI Subdevice Structures */
+	/* Digital Input subdevice */
 	s = &dev->subdevices[2];
-	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_COMMON;
-	s->n_chan = this_board->i_NbrDiChannel;
-	s->maxdata = 1;
-	s->len_chanlist = this_board->i_NbrDiChannel;
-	s->range_table = &range_digital;
-	s->insn_bits = apci3120_di_insn_bits;
-
-	/*  Allocate and Initialise DO Subdevice Structures */
+	s->type		= COMEDI_SUBD_DI;
+	s->subdev_flags	= SDF_READABLE;
+	s->n_chan	= 4;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= apci3120_di_insn_bits;
+
+	/* Digital Output subdevice */
 	s = &dev->subdevices[3];
-	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags =
-		SDF_READABLE | SDF_WRITEABLE | SDF_GROUND | SDF_COMMON;
-	s->n_chan = this_board->i_NbrDoChannel;
-	s->maxdata = this_board->i_DoMaxdata;
-	s->len_chanlist = this_board->i_NbrDoChannel;
-	s->range_table = &range_digital;
-	s->insn_bits = apci3120_do_insn_bits;
-
-	/*  Allocate and Initialise Timer Subdevice Structures */
-	s = &dev->subdevices[4];
-	s->type = COMEDI_SUBD_TIMER;
-	s->subdev_flags = SDF_WRITEABLE | SDF_GROUND | SDF_COMMON;
-	s->n_chan = 1;
-	s->maxdata = 0;
-	s->len_chanlist = 1;
-	s->range_table = &range_digital;
+	s->type		= COMEDI_SUBD_DO;
+	s->subdev_flags	= SDF_WRITABLE;
+	s->n_chan	= 4;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= apci3120_do_insn_bits;
 
-	s->insn_write = apci3120_write_insn_timer;
-	s->insn_read = apci3120_read_insn_timer;
-	s->insn_config = apci3120_config_insn_timer;
+	/* Timer subdevice */
+	s = &dev->subdevices[4];
+	s->type		= COMEDI_SUBD_TIMER;
+	s->subdev_flags	= SDF_READABLE;
+	s->n_chan	= 1;
+	s->maxdata	= 0x00ffffff;
+	s->insn_config	= apci3120_timer_insn_config;
+	s->insn_read	= apci3120_timer_insn_read;
 
-	apci3120_reset(dev);
 	return 0;
 }
 
 static void apci3120_detach(struct comedi_device *dev)
 {
-	struct addi_private *devpriv = dev->private;
-
-	if (dev->iobase)
-		apci3120_reset(dev);
 	comedi_pci_detach(dev);
-	if (devpriv) {
-		unsigned int i;
-
-		for (i = 0; i < 2; i++) {
-			if (devpriv->ul_DmaBufferVirtual[i]) {
-				dma_free_coherent(dev->hw_dev,
-						  devpriv->ui_DmaBufferSize[i],
-						  devpriv->
-						  ul_DmaBufferVirtual[i],
-						  devpriv->ul_DmaBufferHw[i]);
-			}
-		}
-	}
+	apci3120_dma_free(dev);
 }
 
 static struct comedi_driver apci3120_driver = {
diff --git a/drivers/staging/comedi/drivers/addi_apci_3200.c b/drivers/staging/comedi/drivers/addi_apci_3200.c
deleted file mode 100644
index fe6897eff3db..000000000000
--- a/drivers/staging/comedi/drivers/addi_apci_3200.c
+++ /dev/null
@@ -1,125 +0,0 @@
-#include <linux/module.h>
-#include <linux/pci.h>
-
-#include <asm/i387.h>
-
-#include "../comedidev.h"
-#include "comedi_fc.h"
-#include "amcc_s5933.h"
-
-#include "addi-data/addi_common.h"
-
-static void fpu_begin(void)
-{
-	kernel_fpu_begin();
-}
-
-static void fpu_end(void)
-{
-	kernel_fpu_end();
-}
-
-#include "addi-data/addi_eeprom.c"
-#include "addi-data/hwdrv_apci3200.c"
-#include "addi-data/addi_common.c"
-
-enum apci3200_boardid {
-	BOARD_APCI3200,
-	BOARD_APCI3300,
-};
-
-static const struct addi_board apci3200_boardtypes[] = {
-	[BOARD_APCI3200] = {
-		.pc_DriverName		= "apci3200",
-		.i_IorangeBase1		= 256,
-		.i_PCIEeprom		= 1,
-		.pc_EepromChip		= "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		= apci3200_interrupt,
-		.reset			= apci3200_reset,
-		.ai_config		= apci3200_ai_config,
-		.ai_read		= apci3200_ai_read,
-		.ai_write		= apci3200_ai_write,
-		.ai_bits		= apci3200_ai_bits_test,
-		.ai_cmdtest		= apci3200_ai_cmdtest,
-		.ai_cmd			= apci3200_ai_cmd,
-		.ai_cancel		= apci3200_cancel,
-		.di_bits		= apci3200_di_insn_bits,
-		.do_bits		= apci3200_do_insn_bits,
-	},
-	[BOARD_APCI3300] = {
-		.pc_DriverName		= "apci3300",
-		.i_IorangeBase1		= 256,
-		.i_PCIEeprom		= 1,
-		.pc_EepromChip		= "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		= apci3200_interrupt,
-		.reset			= apci3200_reset,
-		.ai_config		= apci3200_ai_config,
-		.ai_read		= apci3200_ai_read,
-		.ai_write		= apci3200_ai_write,
-		.ai_bits		= apci3200_ai_bits_test,
-		.ai_cmdtest		= apci3200_ai_cmdtest,
-		.ai_cmd			= apci3200_ai_cmd,
-		.ai_cancel		= apci3200_cancel,
-		.di_bits		= apci3200_di_insn_bits,
-		.do_bits		= apci3200_do_insn_bits,
-	},
-};
-
-static int apci3200_auto_attach(struct comedi_device *dev,
-				unsigned long context)
-{
-	const struct addi_board *board = NULL;
-
-	if (context < ARRAY_SIZE(apci3200_boardtypes))
-		board = &apci3200_boardtypes[context];
-	if (!board)
-		return -ENODEV;
-	dev->board_ptr = board;
-
-	return addi_auto_attach(dev, context);
-}
-
-static struct comedi_driver apci3200_driver = {
-	.driver_name	= "addi_apci_3200",
-	.module		= THIS_MODULE,
-	.auto_attach	= apci3200_auto_attach,
-	.detach		= i_ADDI_Detach,
-};
-
-static int apci3200_pci_probe(struct pci_dev *dev,
-			      const struct pci_device_id *id)
-{
-	return comedi_pci_auto_config(dev, &apci3200_driver, id->driver_data);
-}
-
-static const struct pci_device_id apci3200_pci_table[] = {
-	{ PCI_VDEVICE(ADDIDATA, 0x3000), BOARD_APCI3200 },
-	{ PCI_VDEVICE(ADDIDATA, 0x3007), BOARD_APCI3300 },
-	{ 0 }
-};
-MODULE_DEVICE_TABLE(pci, apci3200_pci_table);
-
-static struct pci_driver apci3200_pci_driver = {
-	.name		= "addi_apci_3200",
-	.id_table	= apci3200_pci_table,
-	.probe		= apci3200_pci_probe,
-	.remove		= comedi_pci_auto_unconfig,
-};
-module_comedi_pci_driver(apci3200_driver, apci3200_pci_driver);
diff --git a/drivers/staging/comedi/drivers/addi_apci_3501.c b/drivers/staging/comedi/drivers/addi_apci_3501.c
index 010efa3fed6c..a726efcea6a5 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3501.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3501.c
@@ -357,12 +357,11 @@ static int apci3501_auto_attach(struct comedi_device *dev,
 	s = &dev->subdevices[0];
 	if (ao_n_chan) {
 		s->type		= COMEDI_SUBD_AO;
-		s->subdev_flags	= SDF_WRITEABLE | SDF_GROUND | SDF_COMMON;
+		s->subdev_flags	= SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
 		s->n_chan	= ao_n_chan;
 		s->maxdata	= 0x3fff;
 		s->range_table	= &apci3501_ao_range;
 		s->insn_write	= apci3501_ao_insn_write;
-		s->insn_read	= comedi_readback_insn_read;
 
 		ret = comedi_alloc_subdev_readback(s);
 		if (ret)
@@ -383,7 +382,7 @@ static int apci3501_auto_attach(struct comedi_device *dev,
 	/* Initialize the digital output subdevice */
 	s = &dev->subdevices[2];
 	s->type		= COMEDI_SUBD_DO;
-	s->subdev_flags	= SDF_WRITEABLE;
+	s->subdev_flags	= SDF_WRITABLE;
 	s->n_chan	= 2;
 	s->maxdata	= 1;
 	s->range_table	= &range_digital;
@@ -392,7 +391,7 @@ static int apci3501_auto_attach(struct comedi_device *dev,
 	/* Initialize the timer/watchdog subdevice */
 	s = &dev->subdevices[3];
 	s->type = COMEDI_SUBD_TIMER;
-	s->subdev_flags = SDF_WRITEABLE | SDF_GROUND | SDF_COMMON;
+	s->subdev_flags = SDF_WRITABLE;
 	s->n_chan = 1;
 	s->maxdata = 0;
 	s->len_chanlist = 1;
diff --git a/drivers/staging/comedi/drivers/addi_apci_3xxx.c b/drivers/staging/comedi/drivers/addi_apci_3xxx.c
index a296bd5b2c0c..c173810a3b5b 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3xxx.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3xxx.c
@@ -371,10 +371,10 @@ static irqreturn_t apci3xxx_irq_handler(int irq, void *d)
 		writel(status, dev->mmio + 16);
 
 		val = readl(dev->mmio + 28);
-		comedi_buf_put(s, val);
+		comedi_buf_write_samples(s, &val, 1);
 
 		s->async->events |= COMEDI_CB_EOA;
-		comedi_event(dev, s);
+		comedi_handle_events(dev, s);
 
 		return IRQ_HANDLED;
 	}
@@ -849,12 +849,11 @@ static int apci3xxx_auto_attach(struct comedi_device *dev,
 	if (board->has_ao) {
 		s = &dev->subdevices[subdev];
 		s->type		= COMEDI_SUBD_AO;
-		s->subdev_flags	= SDF_WRITEABLE | SDF_GROUND | SDF_COMMON;
+		s->subdev_flags	= SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
 		s->n_chan	= 4;
 		s->maxdata	= 0x0fff;
 		s->range_table	= &apci3xxx_ao_range;
 		s->insn_write	= apci3xxx_ao_insn_write;
-		s->insn_read	= comedi_readback_insn_read;
 
 		ret = comedi_alloc_subdev_readback(s);
 		if (ret)
@@ -880,7 +879,7 @@ static int apci3xxx_auto_attach(struct comedi_device *dev,
 	if (board->has_dig_out) {
 		s = &dev->subdevices[subdev];
 		s->type		= COMEDI_SUBD_DO;
-		s->subdev_flags	= SDF_WRITEABLE;
+		s->subdev_flags	= SDF_WRITABLE;
 		s->n_chan	= 4;
 		s->maxdata	= 1;
 		s->range_table	= &range_digital;
@@ -893,7 +892,7 @@ static int apci3xxx_auto_attach(struct comedi_device *dev,
 	if (board->has_ttl_io) {
 		s = &dev->subdevices[subdev];
 		s->type		= COMEDI_SUBD_DIO;
-		s->subdev_flags	= SDF_READABLE | SDF_WRITEABLE;
+		s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
 		s->n_chan	= 24;
 		s->maxdata	= 1;
 		s->io_bits	= 0xff;	/* channels 0-7 are always outputs */
diff --git a/drivers/staging/comedi/drivers/addi_tcw.h b/drivers/staging/comedi/drivers/addi_tcw.h
new file mode 100644
index 000000000000..8794d4cbbfb0
--- /dev/null
+++ b/drivers/staging/comedi/drivers/addi_tcw.h
@@ -0,0 +1,56 @@
+#ifndef _ADDI_TCW_H
+#define _ADDI_TCW_H
+
+/*
+ * Following are the generic definitions for the ADDI-DATA timer/counter/
+ * watchdog (TCW) registers and bits. Some of the registers are not used
+ * depending on the use of the TCW.
+ */
+
+#define ADDI_TCW_VAL_REG		0x00
+
+#define ADDI_TCW_SYNC_REG		0x00
+#define ADDI_TCW_SYNC_CTR_TRIG		(1 << 8)
+#define ADDI_TCW_SYNC_CTR_DIS		(1 << 7)
+#define ADDI_TCW_SYNC_CTR_ENA		(1 << 6)
+#define ADDI_TCW_SYNC_TIMER_TRIG	(1 << 5)
+#define ADDI_TCW_SYNC_TIMER_DIS		(1 << 4)
+#define ADDI_TCW_SYNC_TIMER_ENA		(1 << 3)
+#define ADDI_TCW_SYNC_WDOG_TRIG		(1 << 2)
+#define ADDI_TCW_SYNC_WDOG_DIS		(1 << 1)
+#define ADDI_TCW_SYNC_WDOG_ENA		(1 << 0)
+
+#define ADDI_TCW_RELOAD_REG		0x04
+
+#define ADDI_TCW_TIMEBASE_REG		0x08
+
+#define ADDI_TCW_CTRL_REG		0x0c
+#define ADDI_TCW_CTRL_EXT_CLK_STATUS	(1 << 21)
+#define ADDI_TCW_CTRL_CASCADE		(1 << 20)
+#define ADDI_TCW_CTRL_CNTR_ENA		(1 << 19)
+#define ADDI_TCW_CTRL_CNT_UP		(1 << 18)
+#define ADDI_TCW_CTRL_EXT_CLK(x)	((x) << 16)
+#define ADDI_TCW_CTRL_OUT(x)		((x) << 11)
+#define ADDI_TCW_CTRL_GATE		(1 << 10)
+#define ADDI_TCW_CTRL_TRIG		(1 << 9)
+#define ADDI_TCW_CTRL_EXT_GATE(x)	((x) << 7)
+#define ADDI_TCW_CTRL_EXT_TRIG(x)	((x) << 5)
+#define ADDI_TCW_CTRL_TIMER_ENA		(1 << 4)
+#define ADDI_TCW_CTRL_RESET_ENA		(1 << 3)
+#define ADDI_TCW_CTRL_WARN_ENA		(1 << 2)
+#define ADDI_TCW_CTRL_IRQ_ENA		(1 << 1)
+#define ADDI_TCW_CTRL_ENA		(1 << 0)
+
+#define ADDI_TCW_STATUS_REG		0x10
+#define ADDI_TCW_STATUS_SOFT_CLR	(1 << 3)
+#define ADDI_TCW_STATUS_SOFT_TRIG	(1 << 1)
+#define ADDI_TCW_STATUS_OVERFLOW	(1 << 0)
+
+#define ADDI_TCW_IRQ_REG		0x14
+#define ADDI_TCW_IRQ			(1 << 0)
+
+#define ADDI_TCW_WARN_TIMEVAL_REG	0x18
+
+#define ADDI_TCW_WARN_TIMEBASE_REG	0x1c
+
+#endif
diff --git a/drivers/staging/comedi/drivers/addi_watchdog.c b/drivers/staging/comedi/drivers/addi_watchdog.c
index 23031feaa095..c5b082d4e51e 100644
--- a/drivers/staging/comedi/drivers/addi_watchdog.c
+++ b/drivers/staging/comedi/drivers/addi_watchdog.c
@@ -20,21 +20,9 @@
 
 #include <linux/module.h>
 #include "../comedidev.h"
+#include "addi_tcw.h"
 #include "addi_watchdog.h"
 
-/*
- * Register offsets/defines for the addi-data watchdog
- */
-#define ADDI_WDOG_REG			0x00
-#define ADDI_WDOG_RELOAD_REG		0x04
-#define ADDI_WDOG_TIMEBASE		0x08
-#define ADDI_WDOG_CTRL_REG		0x0c
-#define ADDI_WDOG_CTRL_ENABLE		(1 << 0)
-#define ADDI_WDOG_CTRL_SW_TRIG		(1 << 9)
-#define ADDI_WDOG_STATUS_REG		0x10
-#define ADDI_WDOG_STATUS_ENABLED	(1 << 0)
-#define ADDI_WDOG_STATUS_SW_TRIG	(1 << 1)
-
 struct addi_watchdog_private {
 	unsigned long iobase;
 	unsigned int wdog_ctrl;
@@ -60,9 +48,9 @@ static int addi_watchdog_insn_config(struct comedi_device *dev,
 
 	switch (data[0]) {
 	case INSN_CONFIG_ARM:
-		spriv->wdog_ctrl = ADDI_WDOG_CTRL_ENABLE;
+		spriv->wdog_ctrl = ADDI_TCW_CTRL_ENA;
 		reload = data[1] & s->maxdata;
-		outl(reload, spriv->iobase + ADDI_WDOG_RELOAD_REG);
+		outl(reload, spriv->iobase + ADDI_TCW_RELOAD_REG);
 
 		/* Time base is 20ms, let the user know the timeout */
 		dev_info(dev->class_dev, "watchdog enabled, timeout:%dms\n",
@@ -75,7 +63,7 @@ static int addi_watchdog_insn_config(struct comedi_device *dev,
 		return -EINVAL;
 	}
 
-	outl(spriv->wdog_ctrl, spriv->iobase + ADDI_WDOG_CTRL_REG);
+	outl(spriv->wdog_ctrl, spriv->iobase + ADDI_TCW_CTRL_REG);
 
 	return insn->n;
 }
@@ -89,7 +77,7 @@ static int addi_watchdog_insn_read(struct comedi_device *dev,
 	int i;
 
 	for (i = 0; i < insn->n; i++)
-		data[i] = inl(spriv->iobase + ADDI_WDOG_STATUS_REG);
+		data[i] = inl(spriv->iobase + ADDI_TCW_STATUS_REG);
 
 	return insn->n;
 }
@@ -109,8 +97,8 @@ static int addi_watchdog_insn_write(struct comedi_device *dev,
 
 	/* "ping" the watchdog */
 	for (i = 0; i < insn->n; i++) {
-		outl(spriv->wdog_ctrl | ADDI_WDOG_CTRL_SW_TRIG,
-		     spriv->iobase + ADDI_WDOG_CTRL_REG);
+		outl(spriv->wdog_ctrl | ADDI_TCW_CTRL_TRIG,
+		     spriv->iobase + ADDI_TCW_CTRL_REG);
 	}
 
 	return insn->n;
@@ -118,8 +106,8 @@ static int addi_watchdog_insn_write(struct comedi_device *dev,
 
 void addi_watchdog_reset(unsigned long iobase)
 {
-	outl(0x0, iobase + ADDI_WDOG_CTRL_REG);
-	outl(0x0, iobase + ADDI_WDOG_RELOAD_REG);
+	outl(0x0, iobase + ADDI_TCW_CTRL_REG);
+	outl(0x0, iobase + ADDI_TCW_RELOAD_REG);
 }
 EXPORT_SYMBOL_GPL(addi_watchdog_reset);
 
@@ -134,7 +122,7 @@ int addi_watchdog_init(struct comedi_subdevice *s, unsigned long iobase)
 	spriv->iobase = iobase;
 
 	s->type		= COMEDI_SUBD_TIMER;
-	s->subdev_flags	= SDF_WRITEABLE;
+	s->subdev_flags	= SDF_WRITABLE;
 	s->n_chan	= 1;
 	s->maxdata	= 0xff;
 	s->insn_config	= addi_watchdog_insn_config;
diff --git a/drivers/staging/comedi/drivers/adl_pci6208.c b/drivers/staging/comedi/drivers/adl_pci6208.c
index 0ad46fe492c9..528f15c25dae 100644
--- a/drivers/staging/comedi/drivers/adl_pci6208.c
+++ b/drivers/staging/comedi/drivers/adl_pci6208.c
@@ -169,7 +169,6 @@ static int pci6208_auto_attach(struct comedi_device *dev,
 	s->maxdata	= 0xffff;
 	s->range_table	= &range_bipolar10;
 	s->insn_write	= pci6208_ao_insn_write;
-	s->insn_read	= comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
diff --git a/drivers/staging/comedi/drivers/adl_pci9111.c b/drivers/staging/comedi/drivers/adl_pci9111.c
index d18d8f21af23..47f6c0e9f014 100644
--- a/drivers/staging/comedi/drivers/adl_pci9111.c
+++ b/drivers/staging/comedi/drivers/adl_pci9111.c
@@ -133,8 +133,6 @@ static const struct comedi_lrange pci9111_ai_range = {
 struct pci9111_private_data {
 	unsigned long lcr_io_base;
 
-	int stop_counter;
-
 	unsigned int scan_delay;
 	unsigned int chunk_counter;
 	unsigned int chunk_num_samples;
@@ -404,12 +402,6 @@ static int pci9111_ai_do_cmd(struct comedi_device *dev,
 	outb(CR_RANGE(cmd->chanlist[0]) & PCI9111_AI_RANGE_MASK,
 		dev->iobase + PCI9111_AI_RANGE_STAT_REG);
 
-	/* Set counter */
-	if (cmd->stop_src == TRIG_COUNT)
-		dev_private->stop_counter = cmd->stop_arg * cmd->chanlist_len;
-	else	/* TRIG_NONE */
-		dev_private->stop_counter = 0;
-
 	/*  Set timer pacer */
 	dev_private->scan_delay = 0;
 	if (cmd->convert_src == TRIG_TIMER) {
@@ -435,7 +427,6 @@ static int pci9111_ai_do_cmd(struct comedi_device *dev,
 	}
 	outb(trig, dev->iobase + PCI9111_AI_TRIG_CTRL_REG);
 
-	dev_private->stop_counter *= (1 + dev_private->scan_delay);
 	dev_private->chunk_counter = 0;
 	dev_private->chunk_num_samples = cmd->chanlist_len *
 					 (1 + dev_private->scan_delay);
@@ -452,7 +443,7 @@ static void pci9111_ai_munge(struct comedi_device *dev,
 	unsigned int maxdata = s->maxdata;
 	unsigned int invert = (maxdata + 1) >> 1;
 	unsigned int shift = (maxdata == 0xffff) ? 0 : 4;
-	unsigned int num_samples = num_bytes / sizeof(short);
+	unsigned int num_samples = comedi_bytes_to_samples(s, num_bytes);
 	unsigned int i;
 
 	for (i = 0; i < num_samples; i++)
@@ -464,22 +455,14 @@ static void pci9111_handle_fifo_half_full(struct comedi_device *dev,
 {
 	struct pci9111_private_data *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
-	unsigned int total = 0;
 	unsigned int samples;
 
-	if (cmd->stop_src == TRIG_COUNT &&
-	    PCI9111_FIFO_HALF_SIZE > devpriv->stop_counter)
-		samples = devpriv->stop_counter;
-	else
-		samples = PCI9111_FIFO_HALF_SIZE;
-
+	samples = comedi_nsamples_left(s, PCI9111_FIFO_HALF_SIZE);
 	insw(dev->iobase + PCI9111_AI_FIFO_REG,
 	     devpriv->ai_bounce_buffer, samples);
 
 	if (devpriv->scan_delay < 1) {
-		total = cfc_write_array_to_buffer(s,
-						  devpriv->ai_bounce_buffer,
-						  samples * sizeof(short));
+		comedi_buf_write_samples(s, devpriv->ai_bounce_buffer, samples);
 	} else {
 		unsigned int pos = 0;
 		unsigned int to_read;
@@ -492,17 +475,15 @@ static void pci9111_handle_fifo_half_full(struct comedi_device *dev,
 				if (to_read > samples - pos)
 					to_read = samples - pos;
 
-				total += cfc_write_array_to_buffer(s,
+				comedi_buf_write_samples(s,
 						devpriv->ai_bounce_buffer + pos,
-						to_read * sizeof(short));
+						to_read);
 			} else {
 				to_read = devpriv->chunk_num_samples -
 					  devpriv->chunk_counter;
 
 				if (to_read > samples - pos)
 					to_read = samples - pos;
-
-				total += to_read * sizeof(short);
 			}
 
 			pos += to_read;
@@ -513,8 +494,6 @@ static void pci9111_handle_fifo_half_full(struct comedi_device *dev,
 				devpriv->chunk_counter = 0;
 		}
 	}
-
-	devpriv->stop_counter -= total / sizeof(short);
 }
 
 static irqreturn_t pci9111_interrupt(int irq, void *p_device)
@@ -561,7 +540,7 @@ static irqreturn_t pci9111_interrupt(int irq, void *p_device)
 			dev_dbg(dev->class_dev, "fifo overflow\n");
 			outb(0, dev->iobase + PCI9111_INT_CLR_REG);
 			async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
-			cfc_handle_events(dev, s);
+			comedi_handle_events(dev, s);
 
 			return IRQ_HANDLED;
 		}
@@ -571,14 +550,14 @@ static irqreturn_t pci9111_interrupt(int irq, void *p_device)
 			pci9111_handle_fifo_half_full(dev, s);
 	}
 
-	if (cmd->stop_src == TRIG_COUNT && dev_private->stop_counter == 0)
+	if (cmd->stop_src == TRIG_COUNT && async->scans_done >= cmd->stop_arg)
 		async->events |= COMEDI_CB_EOA;
 
 	outb(0, dev->iobase + PCI9111_INT_CLR_REG);
 
 	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
 
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 
 	return IRQ_HANDLED;
 }
@@ -752,7 +731,6 @@ static int pci9111_auto_attach(struct comedi_device *dev,
 	s->len_chanlist	= 1;
 	s->range_table	= &range_bipolar10;
 	s->insn_write	= pci9111_ao_insn_write;
-	s->insn_read	= comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
@@ -768,7 +746,7 @@ static int pci9111_auto_attach(struct comedi_device *dev,
 
 	s = &dev->subdevices[3];
 	s->type		= COMEDI_SUBD_DO;
-	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
+	s->subdev_flags	= SDF_WRITABLE;
 	s->n_chan	= 16;
 	s->maxdata	= 1;
 	s->range_table	= &range_digital;
diff --git a/drivers/staging/comedi/drivers/adl_pci9118.c b/drivers/staging/comedi/drivers/adl_pci9118.c
index e18fd9569a2b..26603582e71a 100644
--- a/drivers/staging/comedi/drivers/adl_pci9118.c
+++ b/drivers/staging/comedi/drivers/adl_pci9118.c
@@ -221,7 +221,6 @@ struct pci9118_private {
 	unsigned char int_ctrl;
 	unsigned char ai_cfg;
 	unsigned int ai_do;		/* what do AI? 0=nothing, 1 to 4 mode */
-	unsigned int ai_act_scan;	/* how many scans we finished */
 	unsigned int ai_n_realscanlen;	/*
 					 * what we must transfer for one
 					 * outgoing scan include front/back adds
@@ -447,51 +446,112 @@ static void interrupt_pci9118_ai_mode4_switch(struct comedi_device *dev,
 	outl(devpriv->ai_cfg, dev->iobase + PCI9118_AI_CFG_REG);
 }
 
-static unsigned int defragment_dma_buffer(struct comedi_device *dev,
-					  struct comedi_subdevice *s,
-					  unsigned short *dma_buffer,
-					  unsigned int num_samples)
+static unsigned int valid_samples_in_act_dma_buf(struct comedi_device *dev,
+						 struct comedi_subdevice *s,
+						 unsigned int n_raw_samples)
 {
 	struct pci9118_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
-	unsigned int i = 0, j = 0;
-	unsigned int start_pos = devpriv->ai_add_front,
-	    stop_pos = devpriv->ai_add_front + cmd->chanlist_len;
-	unsigned int raw_scanlen = devpriv->ai_add_front + cmd->chanlist_len +
-	    devpriv->ai_add_back;
+	unsigned int start_pos = devpriv->ai_add_front;
+	unsigned int stop_pos = start_pos + cmd->chanlist_len;
+	unsigned int span_len = stop_pos + devpriv->ai_add_back;
+	unsigned int dma_pos = devpriv->ai_act_dmapos;
+	unsigned int whole_spans, n_samples, x;
 
-	for (i = 0; i < num_samples; i++) {
-		if (devpriv->ai_act_dmapos >= start_pos &&
-		    devpriv->ai_act_dmapos < stop_pos) {
-			dma_buffer[j++] = dma_buffer[i];
+	if (span_len == cmd->chanlist_len)
+		return n_raw_samples;	/* use all samples */
+
+	/*
+	 * Not all samples are to be used.  Buffer contents consist of a
+	 * possibly non-whole number of spans and a region of each span
+	 * is to be used.
+	 *
+	 * Account for samples in whole number of spans.
+	 */
+	whole_spans = n_raw_samples / span_len;
+	n_samples = whole_spans * cmd->chanlist_len;
+	n_raw_samples -= whole_spans * span_len;
+
+	/*
+	 * Deal with remaining samples which could overlap up to two spans.
+	 */
+	while (n_raw_samples) {
+		if (dma_pos < start_pos) {
+			/* Skip samples before start position. */
+			x = start_pos - dma_pos;
+			if (x > n_raw_samples)
+				x = n_raw_samples;
+			dma_pos += x;
+			n_raw_samples -= x;
+			if (!n_raw_samples)
+				break;
 		}
-		devpriv->ai_act_dmapos++;
-		devpriv->ai_act_dmapos %= raw_scanlen;
+		if (dma_pos < stop_pos) {
+			/* Include samples before stop position. */
+			x = stop_pos - dma_pos;
+			if (x > n_raw_samples)
+				x = n_raw_samples;
+			n_samples += x;
+			dma_pos += x;
+			n_raw_samples -= x;
+		}
+		/* Advance to next span. */
+		start_pos += span_len;
+		stop_pos += span_len;
 	}
-
-	return j;
+	return n_samples;
 }
 
-static int move_block_from_dma(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       unsigned short *dma_buffer,
-			       unsigned int num_samples)
+static void move_block_from_dma(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				unsigned short *dma_buffer,
+				unsigned int n_raw_samples)
 {
 	struct pci9118_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
-	unsigned int num_bytes;
-
-	num_samples = defragment_dma_buffer(dev, s, dma_buffer, num_samples);
-	devpriv->ai_act_scan +=
-	    (s->async->cur_chan + num_samples) / cmd->scan_end_arg;
-	s->async->cur_chan += num_samples;
-	s->async->cur_chan %= cmd->scan_end_arg;
-	num_bytes =
-	    cfc_write_array_to_buffer(s, dma_buffer,
-				      num_samples * sizeof(short));
-	if (num_bytes < num_samples * sizeof(short))
-		return -1;
-	return 0;
+	unsigned int start_pos = devpriv->ai_add_front;
+	unsigned int stop_pos = start_pos + cmd->chanlist_len;
+	unsigned int span_len = stop_pos + devpriv->ai_add_back;
+	unsigned int dma_pos = devpriv->ai_act_dmapos;
+	unsigned int x;
+
+	if (span_len == cmd->chanlist_len) {
+		/* All samples are to be copied. */
+		comedi_buf_write_samples(s, dma_buffer, n_raw_samples);
+		dma_pos += n_raw_samples;
+	} else {
+		/*
+		 * Not all samples are to be copied.  Buffer contents consist
+		 * of a possibly non-whole number of spans and a region of
+		 * each span is to be copied.
+		 */
+		while (n_raw_samples) {
+			if (dma_pos < start_pos) {
+				/* Skip samples before start position. */
+				x = start_pos - dma_pos;
+				if (x > n_raw_samples)
+					x = n_raw_samples;
+				dma_pos += x;
+				n_raw_samples -= x;
+				if (!n_raw_samples)
+					break;
+			}
+			if (dma_pos < stop_pos) {
+				/* Copy samples before stop position. */
+				x = stop_pos - dma_pos;
+				if (x > n_raw_samples)
+					x = n_raw_samples;
+				comedi_buf_write_samples(s, dma_buffer, x);
+				dma_pos += x;
+				n_raw_samples -= x;
+			}
+			/* Advance to next span. */
+			start_pos += span_len;
+			stop_pos += span_len;
+		}
+	}
+	/* Update position in span for next time. */
+	devpriv->ai_act_dmapos = dma_pos % span_len;
 }
 
 static void pci9118_exttrg_enable(struct comedi_device *dev, bool enable)
@@ -578,9 +638,7 @@ static int pci9118_ai_cancel(struct comedi_device *dev,
 	devpriv->ai_do = 0;
 	devpriv->usedma = 0;
 
-	devpriv->ai_act_scan = 0;
 	devpriv->ai_act_dmapos = 0;
-	s->async->cur_chan = 0;
 	s->async->inttrig = NULL;
 	devpriv->ai_neverending = 0;
 	devpriv->dma_actbuf = 0;
@@ -594,8 +652,9 @@ static void pci9118_ai_munge(struct comedi_device *dev,
 			     unsigned int start_chan_index)
 {
 	struct pci9118_private *devpriv = dev->private;
-	unsigned int i, num_samples = num_bytes / sizeof(short);
 	unsigned short *array = data;
+	unsigned int num_samples = comedi_bytes_to_samples(s, num_bytes);
+	unsigned int i;
 
 	for (i = 0; i < num_samples; i++) {
 		if (devpriv->usedma)
@@ -617,17 +676,11 @@ static void interrupt_pci9118_ai_onesample(struct comedi_device *dev,
 
 	sampl = inl(dev->iobase + PCI9118_AI_FIFO_REG);
 
-	cfc_write_to_buffer(s, sampl);
-	s->async->cur_chan++;
-	if (s->async->cur_chan >= cmd->scan_end_arg) {
-							/* one scan done */
-		s->async->cur_chan %= cmd->scan_end_arg;
-		devpriv->ai_act_scan++;
-		if (!devpriv->ai_neverending) {
-			/* all data sampled? */
-			if (devpriv->ai_act_scan >= cmd->stop_arg)
-				s->async->events |= COMEDI_CB_EOA;
-		}
+	comedi_buf_write_samples(s, &sampl, 1);
+
+	if (!devpriv->ai_neverending) {
+		if (s->async->scans_done >= cmd->stop_arg)
+			s->async->events |= COMEDI_CB_EOA;
 	}
 }
 
@@ -637,39 +690,37 @@ static void interrupt_pci9118_ai_dma(struct comedi_device *dev,
 	struct pci9118_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	struct pci9118_dmabuf *dmabuf = &devpriv->dmabuf[devpriv->dma_actbuf];
-	unsigned int next_dma_buf, samplesinbuf, sampls, m;
+	unsigned int n_all = comedi_bytes_to_samples(s, dmabuf->use_size);
+	unsigned int n_valid;
+	bool more_dma;
 
-	samplesinbuf = dmabuf->use_size >> 1;	/* number of received samples */
+	/* determine whether more DMA buffers to do after this one */
+	n_valid = valid_samples_in_act_dma_buf(dev, s, n_all);
+	more_dma = n_valid < comedi_nsamples_left(s, n_valid + 1);
 
-	if (devpriv->dma_doublebuf) {	/*
-					 * switch DMA buffers if is used
-					 * double buffering
-					 */
-		next_dma_buf = 1 - devpriv->dma_actbuf;
-		pci9118_amcc_setup_dma(dev, next_dma_buf);
-		if (devpriv->ai_do == 4)
-			interrupt_pci9118_ai_mode4_switch(dev, next_dma_buf);
+	/* switch DMA buffers and restart DMA if double buffering */
+	if (more_dma && devpriv->dma_doublebuf) {
+		devpriv->dma_actbuf = 1 - devpriv->dma_actbuf;
+		pci9118_amcc_setup_dma(dev, devpriv->dma_actbuf);
+		if (devpriv->ai_do == 4) {
+			interrupt_pci9118_ai_mode4_switch(dev,
+							  devpriv->dma_actbuf);
+		}
 	}
 
-	if (samplesinbuf) {
-		/* how many samples is to end of buffer */
-		m = s->async->prealloc_bufsz >> 1;
-		sampls = m;
-		move_block_from_dma(dev, s, dmabuf->virt, samplesinbuf);
-		m = m - sampls;		/* m=how many samples was transferred */
-	}
+	if (n_all)
+		move_block_from_dma(dev, s, dmabuf->virt, n_all);
 
 	if (!devpriv->ai_neverending) {
-		/* all data sampled? */
-		if (devpriv->ai_act_scan >= cmd->stop_arg)
+		if (s->async->scans_done >= cmd->stop_arg)
 			s->async->events |= COMEDI_CB_EOA;
 	}
 
-	if (devpriv->dma_doublebuf) {
-		/* switch dma buffers */
-		devpriv->dma_actbuf = 1 - devpriv->dma_actbuf;
-	} else {
-		/* restart DMA if is not used double buffering */
+	if (s->async->events & COMEDI_CB_CANCEL_MASK)
+		more_dma = false;
+
+	/* restart DMA if not double buffering */
+	if (more_dma && !devpriv->dma_doublebuf) {
 		pci9118_amcc_setup_dma(dev, 0);
 		if (devpriv->ai_do == 4)
 			interrupt_pci9118_ai_mode4_switch(dev, 0);
@@ -766,7 +817,7 @@ static irqreturn_t pci9118_interrupt(int irq, void *d)
 		interrupt_pci9118_ai_onesample(dev, s);
 
 interrupt_exit:
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 	return IRQ_HANDLED;
 }
 
@@ -1123,9 +1174,7 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	inl(dev->iobase + PCI9118_AI_STATUS_REG);
 	inl(dev->iobase + PCI9118_INT_CTRL_REG);
 
-	devpriv->ai_act_scan = 0;
 	devpriv->ai_act_dmapos = 0;
-	s->async->cur_chan = 0;
 
 	if (devpriv->usedma) {
 		Compute_and_setup_dma(dev, s);
@@ -1624,7 +1673,6 @@ static int pci9118_common_attach(struct comedi_device *dev,
 	s->maxdata	= 0x0fff;
 	s->range_table	= &range_bipolar10;
 	s->insn_write	= pci9118_ao_insn_write;
-	s->insn_read	= comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
diff --git a/drivers/staging/comedi/drivers/adv_pci1710.c b/drivers/staging/comedi/drivers/adv_pci1710.c
index 5539bd294862..d02df7d0c629 100644
--- a/drivers/staging/comedi/drivers/adv_pci1710.c
+++ b/drivers/staging/comedi/drivers/adv_pci1710.c
@@ -298,7 +298,6 @@ static const struct boardtype boardtypes[] = {
 
 struct pci1710_private {
 	unsigned int CntrlReg;	/*  Control register */
-	unsigned int ai_act_scan;	/*  how many scans we finished */
 	unsigned char ai_et;
 	unsigned int ai_et_CntrlReg;
 	unsigned int ai_et_MuxVal;
@@ -730,16 +729,12 @@ static int pci171x_ai_cancel(struct comedi_device *dev,
 		break;
 	}
 
-	devpriv->ai_act_scan = 0;
-	s->async->cur_chan = 0;
-
 	return 0;
 }
 
 static void pci1710_handle_every_sample(struct comedi_device *dev,
 					struct comedi_subdevice *s)
 {
-	struct pci1710_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int status;
 	unsigned int val;
@@ -749,14 +744,14 @@ static void pci1710_handle_every_sample(struct comedi_device *dev,
 	if (status & Status_FE) {
 		dev_dbg(dev->class_dev, "A/D FIFO empty (%4x)\n", status);
 		s->async->events |= COMEDI_CB_EOA | COMEDI_CB_ERROR;
-		cfc_handle_events(dev, s);
+		comedi_handle_events(dev, s);
 		return;
 	}
 	if (status & Status_FF) {
 		dev_dbg(dev->class_dev,
 			"A/D FIFO Full status (Fatal Error!) (%4x)\n", status);
 		s->async->events |= COMEDI_CB_EOA | COMEDI_CB_ERROR;
-		cfc_handle_events(dev, s);
+		comedi_handle_events(dev, s);
 		return;
 	}
 
@@ -770,27 +765,19 @@ static void pci1710_handle_every_sample(struct comedi_device *dev,
 			break;
 		}
 
-		comedi_buf_put(s, val & s->maxdata);
-
-		s->async->cur_chan++;
-		if (s->async->cur_chan >= cmd->chanlist_len)
-			s->async->cur_chan = 0;
+		val &= s->maxdata;
+		comedi_buf_write_samples(s, &val, 1);
 
-
-		if (s->async->cur_chan == 0) {	/*  one scan done */
-			devpriv->ai_act_scan++;
-			if (cmd->stop_src == TRIG_COUNT &&
-			    devpriv->ai_act_scan >= cmd->stop_arg) {
-				/*  all data sampled */
-				s->async->events |= COMEDI_CB_EOA;
-				break;
-			}
+		if (cmd->stop_src == TRIG_COUNT &&
+		    s->async->scans_done >= cmd->stop_arg) {
+			s->async->events |= COMEDI_CB_EOA;
+			break;
 		}
 	}
 
 	outb(0, dev->iobase + PCI171x_CLRINT);	/*  clear our INT request */
 
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 }
 
 /*
@@ -799,8 +786,6 @@ static void pci1710_handle_every_sample(struct comedi_device *dev,
 static int move_block_from_fifo(struct comedi_device *dev,
 				struct comedi_subdevice *s, int n, int turn)
 {
-	struct pci1710_private *devpriv = dev->private;
-	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int val;
 	int ret;
 	int i;
@@ -814,13 +799,8 @@ static int move_block_from_fifo(struct comedi_device *dev,
 			return ret;
 		}
 
-		comedi_buf_put(s, val & s->maxdata);
-
-		s->async->cur_chan++;
-		if (s->async->cur_chan >= cmd->chanlist_len) {
-			s->async->cur_chan = 0;
-			devpriv->ai_act_scan++;
-		}
+		val &= s->maxdata;
+		comedi_buf_write_samples(s, &val, 1);
 	}
 	return 0;
 }
@@ -829,48 +809,47 @@ static void pci1710_handle_fifo(struct comedi_device *dev,
 				struct comedi_subdevice *s)
 {
 	const struct boardtype *this_board = dev->board_ptr;
-	struct pci1710_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
-	int m, samplesinbuf;
+	unsigned int nsamples;
+	unsigned int m;
 
 	m = inw(dev->iobase + PCI171x_STATUS);
 	if (!(m & Status_FH)) {
 		dev_dbg(dev->class_dev, "A/D FIFO not half full! (%4x)\n", m);
 		s->async->events |= COMEDI_CB_EOA | COMEDI_CB_ERROR;
-		cfc_handle_events(dev, s);
+		comedi_handle_events(dev, s);
 		return;
 	}
 	if (m & Status_FF) {
 		dev_dbg(dev->class_dev,
 			"A/D FIFO Full status (Fatal Error!) (%4x)\n", m);
 		s->async->events |= COMEDI_CB_EOA | COMEDI_CB_ERROR;
-		cfc_handle_events(dev, s);
+		comedi_handle_events(dev, s);
 		return;
 	}
 
-	samplesinbuf = this_board->fifo_half_size;
-	if (samplesinbuf * sizeof(short) >= s->async->prealloc_bufsz) {
-		m = s->async->prealloc_bufsz / sizeof(short);
+	nsamples = this_board->fifo_half_size;
+	if (comedi_samples_to_bytes(s, nsamples) >= s->async->prealloc_bufsz) {
+		m = comedi_bytes_to_samples(s, s->async->prealloc_bufsz);
 		if (move_block_from_fifo(dev, s, m, 0))
 			return;
-		samplesinbuf -= m;
+		nsamples -= m;
 	}
 
-	if (samplesinbuf) {
-		if (move_block_from_fifo(dev, s, samplesinbuf, 1))
+	if (nsamples) {
+		if (move_block_from_fifo(dev, s, nsamples, 1))
 			return;
 	}
 
 	if (cmd->stop_src == TRIG_COUNT &&
-	    devpriv->ai_act_scan >= cmd->stop_arg) {
-		/* all data sampled */
+	    s->async->scans_done >= cmd->stop_arg) {
 		s->async->events |= COMEDI_CB_EOA;
-		cfc_handle_events(dev, s);
+		comedi_handle_events(dev, s);
 		return;
 	}
 	outb(0, dev->iobase + PCI171x_CLRINT);	/*  clear our INT request */
 
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 }
 
 /*
@@ -928,9 +907,6 @@ static int pci171x_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	outb(0, dev->iobase + PCI171x_CLRFIFO);
 	outb(0, dev->iobase + PCI171x_CLRINT);
 
-	devpriv->ai_act_scan = 0;
-	s->async->cur_chan = 0;
-
 	devpriv->CntrlReg &= Control_CNT0;
 	if ((cmd->flags & CMDF_WAKE_EOS) == 0)
 		devpriv->CntrlReg |= Control_ONEFH;
@@ -1208,7 +1184,7 @@ static int pci1710_auto_attach(struct comedi_device *dev,
 	if (this_board->n_dichan) {
 		s = &dev->subdevices[subdev];
 		s->type = COMEDI_SUBD_DI;
-		s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_COMMON;
+		s->subdev_flags = SDF_READABLE;
 		s->n_chan = this_board->n_dichan;
 		s->maxdata = 1;
 		s->len_chanlist = this_board->n_dichan;
@@ -1220,7 +1196,7 @@ static int pci1710_auto_attach(struct comedi_device *dev,
 	if (this_board->n_dochan) {
 		s = &dev->subdevices[subdev];
 		s->type = COMEDI_SUBD_DO;
-		s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
+		s->subdev_flags = SDF_WRITABLE;
 		s->n_chan = this_board->n_dochan;
 		s->maxdata = 1;
 		s->len_chanlist = this_board->n_dochan;
diff --git a/drivers/staging/comedi/drivers/adv_pci1723.c b/drivers/staging/comedi/drivers/adv_pci1723.c
index 1610e2b406f3..65f854e1eb66 100644
--- a/drivers/staging/comedi/drivers/adv_pci1723.c
+++ b/drivers/staging/comedi/drivers/adv_pci1723.c
@@ -1,205 +1,124 @@
 /*
-   comedi/drivers/pci1723.c
-
-   COMEDI - Linux Control and Measurement Device Interface
-   Copyright (C) 2000 David A. Schleef <ds@schleef.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.
+ * adv_pci1723.c
+ * Comedi driver for the Advantech PCI-1723 card.
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 2000 David A. Schleef <ds@schleef.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.
+ */
 
-   This program is distributed in the hope that it will be useful,
-   but WITHOUT ANY WARRANTY; without even the implied warranty of
-   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-   GNU General Public License for more details.
-*/
 /*
-Driver: adv_pci1723
-Description: Advantech PCI-1723
-Author: yonggang <rsmgnu@gmail.com>, Ian Abbott <abbotti@mev.co.uk>
-Devices: [Advantech] PCI-1723 (adv_pci1723)
-Updated: Mon, 14 Apr 2008 15:12:56 +0100
-Status: works
-
-Configuration Options:
-  [0] - PCI bus of device (optional)
-  [1] - PCI slot of device (optional)
-
-  If bus/slot is not specified, the first supported
-  PCI device found will be used.
-
-Subdevice 0 is 8-channel AO, 16-bit, range +/- 10 V.
-
-Subdevice 1 is 16-channel DIO.  The channels are configurable as input or
-output in 2 groups (0 to 7, 8 to 15).  Configuring any channel implicitly
-configures all channels in the same group.
-
-TODO:
-
-1. Add the two milliamp ranges to the AO subdevice (0 to 20 mA, 4 to 20 mA).
-2. Read the initial ranges and values of the AO subdevice at start-up instead
-   of reinitializing them.
-3. Implement calibration.
-*/
+ * Driver: adv_pci1723
+ * Description: Advantech PCI-1723
+ * Author: yonggang <rsmgnu@gmail.com>, Ian Abbott <abbotti@mev.co.uk>
+ * Devices: (Advantech) PCI-1723 [adv_pci1723]
+ * Updated: Mon, 14 Apr 2008 15:12:56 +0100
+ * Status: works
+ *
+ * Configuration Options: not applicable, uses comedi PCI auto config
+ *
+ * Subdevice 0 is 8-channel AO, 16-bit, range +/- 10 V.
+ *
+ * Subdevice 1 is 16-channel DIO.  The channels are configurable as
+ * input or output in 2 groups (0 to 7, 8 to 15). Configuring any
+ * channel implicitly configures all channels in the same group.
+ *
+ * TODO:
+ * 1. Add the two milliamp ranges to the AO subdevice (0 to 20 mA,
+ *    4 to 20 mA).
+ * 2. Read the initial ranges and values of the AO subdevice at
+ *    start-up instead of reinitializing them.
+ * 3. Implement calibration.
+ */
 
 #include <linux/module.h>
 #include <linux/pci.h>
 
 #include "../comedidev.h"
 
-/* all the registers for the pci1723 board */
-#define PCI1723_DA(N)   ((N)<<1)	/* W: D/A register N (0 to 7) */
-
-#define PCI1723_SYN_SET  0x12		/* synchronized set register */
-#define PCI1723_ALL_CHNNELE_SYN_STROBE  0x12
-					/* synchronized status register */
-
-#define PCI1723_RANGE_CALIBRATION_MODE 0x14
-					/* range and calibration mode */
-#define PCI1723_RANGE_CALIBRATION_STATUS 0x14
-					/* range and calibration status */
-
-#define PCI1723_CONTROL_CMD_CALIBRATION_FUN 0x16
-						/*
-						 * SADC control command for
-						 * calibration function
-						 */
-#define PCI1723_STATUS_CMD_CALIBRATION_FUN 0x16
-						/*
-						 * SADC control status for
-						 * calibration function
-						 */
-
-#define PCI1723_CALIBRATION_PARA_STROBE 0x18
-					/* Calibration parameter strobe */
-
-#define PCI1723_DIGITAL_IO_PORT_SET 0x1A	/* Digital I/O port setting */
-#define PCI1723_DIGITAL_IO_PORT_MODE 0x1A	/* Digital I/O port mode */
-
-#define PCI1723_WRITE_DIGITAL_OUTPUT_CMD 0x1C
-					/* Write digital output command */
-#define PCI1723_READ_DIGITAL_INPUT_DATA 0x1C	/* Read digital input data */
-
-#define PCI1723_WRITE_CAL_CMD 0x1E		/* Write calibration command */
-#define PCI1723_READ_CAL_STATUS 0x1E		/* Read calibration status */
-
-#define PCI1723_SYN_STROBE 0x20			/* Synchronized strobe */
-
-#define PCI1723_RESET_ALL_CHN_STROBE 0x22
-					/* Reset all D/A channels strobe */
-
-#define PCI1723_RESET_CAL_CONTROL_STROBE 0x24
-						/*
-						 * Reset the calibration
-						 * controller strobe
-						 */
-
-#define PCI1723_CHANGE_CHA_OUTPUT_TYPE_STROBE 0x26
-						/*
-						 * Change D/A channels output
-						 * type strobe
-						 */
-
-#define PCI1723_SELECT_CALIBRATION 0x28	/* Select the calibration Ref_V */
-
-struct pci1723_private {
-	unsigned char da_range[8];	/* D/A output range for each channel */
-	unsigned short ao_data[8];	/* data output buffer */
-};
-
 /*
- * The pci1723 card reset;
+ * PCI Bar 2 I/O Register map (dev->iobase)
  */
-static int pci1723_reset(struct comedi_device *dev)
+#define PCI1723_AO_REG(x)		(0x00 + ((x) * 2))
+#define PCI1723_BOARD_ID_REG		0x10
+#define PCI1723_BOARD_ID_MASK		(0xf << 0)
+#define PCI1723_SYNC_CTRL_REG		0x12
+#define PCI1723_SYNC_CTRL_ASYNC		(0 << 0)
+#define PCI1723_SYNC_CTRL_SYNC		(1 << 0)
+#define PCI1723_CTRL_REG		0x14
+#define PCI1723_CTRL_BUSY		(1 << 15)
+#define PCI1723_CTRL_INIT		(1 << 14)
+#define PCI1723_CTRL_SELF		(1 << 8)
+#define PCI1723_CTRL_IDX(x)		(((x) & 0x3) << 6)
+#define PCI1723_CTRL_RANGE(x)		(((x) & 0x3) << 4)
+#define PCI1723_CTRL_GAIN		(0 << 3)
+#define PCI1723_CTRL_OFFSET		(1 << 3)
+#define PCI1723_CTRL_CHAN(x)		(((x) & 0x7) << 0)
+#define PCI1723_CALIB_CTRL_REG		0x16
+#define PCI1723_CALIB_CTRL_CS		(1 << 2)
+#define PCI1723_CALIB_CTRL_DAT		(1 << 1)
+#define PCI1723_CALIB_CTRL_CLK		(1 << 0)
+#define PCI1723_CALIB_STROBE_REG	0x18
+#define PCI1723_DIO_CTRL_REG		0x1a
+#define PCI1723_DIO_CTRL_HDIO		(1 << 1)
+#define PCI1723_DIO_CTRL_LDIO		(1 << 0)
+#define PCI1723_DIO_DATA_REG		0x1c
+#define PCI1723_CALIB_DATA_REG		0x1e
+#define PCI1723_SYNC_STROBE_REG		0x20
+#define PCI1723_RESET_AO_STROBE_REG	0x22
+#define PCI1723_RESET_CALIB_STROBE_REG	0x24
+#define PCI1723_RANGE_STROBE_REG	0x26
+#define PCI1723_VREF_REG		0x28
+#define PCI1723_VREF_NEG10V		(0 << 0)
+#define PCI1723_VREF_0V			(1 << 0)
+#define PCI1723_VREF_POS10V		(3 << 0)
+
+static int pci1723_ao_insn_write(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
 {
-	struct pci1723_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
 	int i;
 
-	outw(0x01, dev->iobase + PCI1723_SYN_SET);
-					       /* set synchronous output mode */
-
-	for (i = 0; i < 8; i++) {
-		/* set all outputs to 0V */
-		devpriv->ao_data[i] = 0x8000;
-		outw(devpriv->ao_data[i], dev->iobase + PCI1723_DA(i));
-		/* set all ranges to +/- 10V */
-		devpriv->da_range[i] = 0;
-		outw(((devpriv->da_range[i] << 4) | i),
-		     PCI1723_RANGE_CALIBRATION_MODE);
-	}
-
-	outw(0, dev->iobase + PCI1723_CHANGE_CHA_OUTPUT_TYPE_STROBE);
-							    /* update ranges */
-	outw(0, dev->iobase + PCI1723_SYN_STROBE);	    /* update outputs */
-
-	/* set asynchronous output mode */
-	outw(0, dev->iobase + PCI1723_SYN_SET);
-
-	return 0;
-}
-
-static int pci1723_insn_read_ao(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
-{
-	struct pci1723_private *devpriv = dev->private;
-	int n, chan;
-
-	chan = CR_CHAN(insn->chanspec);
-	for (n = 0; n < insn->n; n++)
-		data[n] = devpriv->ao_data[chan];
+	for (i = 0; i < insn->n; i++) {
+		unsigned int val = data[i];
 
-	return n;
-}
-
-/*
-  analog data output;
-*/
-static int pci1723_ao_write_winsn(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data)
-{
-	struct pci1723_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int n;
-
-	for (n = 0; n < insn->n; n++) {
-		devpriv->ao_data[chan] = data[n];
-		outw(data[n], dev->iobase + PCI1723_DA(chan));
+		outw(val, dev->iobase + PCI1723_AO_REG(chan));
+		s->readback[chan] = val;
 	}
 
-	return n;
+	return insn->n;
 }
 
-/*
-  digital i/o config/query
-*/
 static int pci1723_dio_insn_config(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn, unsigned int *data)
+				   struct comedi_insn *insn,
+				   unsigned int *data)
 {
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int mask;
-	unsigned short mode;
+	unsigned int mask = (chan < 8) ? 0x00ff : 0xff00;
+	unsigned short mode = 0x0000;		/* assume output */
 	int ret;
 
-	if (chan < 8)
-		mask = 0x00ff;
-	else
-		mask = 0xff00;
-
 	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
 	if (ret)
 		return ret;
 
-	/* update hardware DIO mode */
-	mode = 0x0000;			/* assume output */
 	if (!(s->io_bits & 0x00ff))
-		mode |= 0x0001;		/* low byte input */
+		mode |= PCI1723_DIO_CTRL_LDIO;	/* low byte input */
 	if (!(s->io_bits & 0xff00))
-		mode |= 0x0002;		/* high byte input */
-	outw(mode, dev->iobase + PCI1723_DIGITAL_IO_PORT_SET);
+		mode |= PCI1723_DIO_CTRL_HDIO;	/* high byte input */
+	outw(mode, dev->iobase + PCI1723_DIO_CTRL_REG);
 
 	return insn->n;
 }
@@ -210,24 +129,21 @@ static int pci1723_dio_insn_bits(struct comedi_device *dev,
 				 unsigned int *data)
 {
 	if (comedi_dio_update_state(s, data))
-		outw(s->state, dev->iobase + PCI1723_WRITE_DIGITAL_OUTPUT_CMD);
+		outw(s->state, dev->iobase + PCI1723_DIO_DATA_REG);
 
-	data[1] = inw(dev->iobase + PCI1723_READ_DIGITAL_INPUT_DATA);
+	data[1] = inw(dev->iobase + PCI1723_DIO_DATA_REG);
 
 	return insn->n;
 }
 
 static int pci1723_auto_attach(struct comedi_device *dev,
-					 unsigned long context_unused)
+			       unsigned long context_unused)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	struct pci1723_private *devpriv;
 	struct comedi_subdevice *s;
+	unsigned int val;
 	int ret;
-
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
+	int i;
 
 	ret = comedi_pci_enable(dev);
 	if (ret)
@@ -239,61 +155,57 @@ static int pci1723_auto_attach(struct comedi_device *dev,
 		return ret;
 
 	s = &dev->subdevices[0];
-	dev->write_subdev = s;
 	s->type		= COMEDI_SUBD_AO;
-	s->subdev_flags	= SDF_WRITEABLE | SDF_GROUND | SDF_COMMON;
+	s->subdev_flags	= SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
 	s->n_chan	= 8;
 	s->maxdata	= 0xffff;
-	s->len_chanlist	= 8;
 	s->range_table	= &range_bipolar10;
-	s->insn_write	= pci1723_ao_write_winsn;
-	s->insn_read	= pci1723_insn_read_ao;
+	s->insn_write	= pci1723_ao_insn_write;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
+
+	/* synchronously reset all analog outputs to 0V, +/-10V range */
+	outw(PCI1723_SYNC_CTRL_SYNC, dev->iobase + PCI1723_SYNC_CTRL_REG);
+	for (i = 0; i < s->n_chan; i++) {
+		outw(PCI1723_CTRL_RANGE(0) | PCI1723_CTRL_CHAN(i),
+		     PCI1723_CTRL_REG);
+		outw(0, dev->iobase + PCI1723_RANGE_STROBE_REG);
+
+		outw(0x8000, dev->iobase + PCI1723_AO_REG(i));
+		s->readback[i] = 0x8000;
+	}
+	outw(0, dev->iobase + PCI1723_SYNC_STROBE_REG);
+
+	/* disable syncronous control */
+	outw(PCI1723_SYNC_CTRL_ASYNC, dev->iobase + PCI1723_SYNC_CTRL_REG);
 
 	s = &dev->subdevices[1];
 	s->type		= COMEDI_SUBD_DIO;
 	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
 	s->n_chan	= 16;
 	s->maxdata	= 1;
-	s->len_chanlist	= 16;
 	s->range_table	= &range_digital;
 	s->insn_config	= pci1723_dio_insn_config;
 	s->insn_bits	= pci1723_dio_insn_bits;
 
-	/* read DIO config */
-	switch (inw(dev->iobase + PCI1723_DIGITAL_IO_PORT_MODE) & 0x03) {
-	case 0x00:	/* low byte output, high byte output */
-		s->io_bits = 0xFFFF;
-		break;
-	case 0x01:	/* low byte input, high byte output */
-		s->io_bits = 0xFF00;
-		break;
-	case 0x02:	/* low byte output, high byte input */
-		s->io_bits = 0x00FF;
-		break;
-	case 0x03:	/* low byte input, high byte input */
-		s->io_bits = 0x0000;
-		break;
-	}
-	/* read DIO port state */
-	s->state = inw(dev->iobase + PCI1723_READ_DIGITAL_INPUT_DATA);
-
-	pci1723_reset(dev);
+	/* get initial DIO direction and state */
+	val = inw(dev->iobase + PCI1723_DIO_CTRL_REG);
+	if (!(val & PCI1723_DIO_CTRL_LDIO))
+		s->io_bits |= 0x00ff;	/* low byte output */
+	if (!(val & PCI1723_DIO_CTRL_HDIO))
+		s->io_bits |= 0xff00;	/* high byte output */
+	s->state = inw(dev->iobase + PCI1723_DIO_DATA_REG);
 
 	return 0;
 }
 
-static void pci1723_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		pci1723_reset(dev);
-	comedi_pci_detach(dev);
-}
-
 static struct comedi_driver adv_pci1723_driver = {
 	.driver_name	= "adv_pci1723",
 	.module		= THIS_MODULE,
 	.auto_attach	= pci1723_auto_attach,
-	.detach		= pci1723_detach,
+	.detach		= comedi_pci_detach,
 };
 
 static int adv_pci1723_pci_probe(struct pci_dev *dev,
@@ -318,5 +230,5 @@ static struct pci_driver adv_pci1723_pci_driver = {
 module_comedi_pci_driver(adv_pci1723_driver, adv_pci1723_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("Advantech PCI-1723 Comedi driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/adv_pci1724.c b/drivers/staging/comedi/drivers/adv_pci1724.c
index 2697758b1ed9..a8d28403262e 100644
--- a/drivers/staging/comedi/drivers/adv_pci1724.c
+++ b/drivers/staging/comedi/drivers/adv_pci1724.c
@@ -1,122 +1,76 @@
 /*
-    comedi/drivers/adv_pci1724.c
-    This is a driver for the Advantech PCI-1724U card.
-
-    Author:  Frank Mori Hess <fmh6jj@gmail.com>
-    Copyright (C) 2013 GnuBIO Inc
-
-    COMEDI - Linux Control and Measurement Device Interface
-    Copyright (C) 1997-8 David A. Schleef <ds@schleef.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.
-*/
+ * adv_pci1724.c
+ * Comedi driver for the Advantech PCI-1724U card.
+ *
+ * Author:  Frank Mori Hess <fmh6jj@gmail.com>
+ * Copyright (C) 2013 GnuBIO Inc
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 1997-8 David A. Schleef <ds@schleef.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.
+ */
 
 /*
-
-Driver: adv_1724
-Description: Advantech PCI-1724U
-Author: Frank Mori Hess <fmh6jj@gmail.com>
-Status: works
-Updated: 2013-02-09
-Devices: [Advantech] PCI-1724U (adv_pci1724)
-
-Subdevice 0 is the analog output.
-Subdevice 1 is the offset calibration for the analog output.
-Subdevice 2 is the gain calibration for the analog output.
-
-The calibration offset and gains have quite a large effect
-on the analog output, so it is possible to adjust the analog output to
-have an output range significantly different from the board's
-nominal output ranges.  For a calibrated +/- 10V range, the analog
-output's offset will be set somewhere near mid-range (0x2000) and its
-gain will be near maximum (0x3fff).
-
-There is really no difference between the board's documented 0-20mA
-versus 4-20mA output ranges.  To pick one or the other is simply a matter
-of adjusting the offset and gain calibration until the board outputs in
-the desired range.
-
-Configuration options:
-   None
-
-Manual configuration of comedi devices is not supported by this driver;
-supported PCI devices are configured as comedi devices automatically.
-
-*/
+ * Driver: adv_pci1724
+ * Description: Advantech PCI-1724U
+ * Devices: (Advantech) PCI-1724U [adv_pci1724]
+ * Author: Frank Mori Hess <fmh6jj@gmail.com>
+ * Updated: 2013-02-09
+ * Status: works
+ *
+ * Configuration Options: not applicable, uses comedi PCI auto config
+ *
+ * Subdevice 0 is the analog output.
+ * Subdevice 1 is the offset calibration for the analog output.
+ * Subdevice 2 is the gain calibration for the analog output.
+ *
+ * The calibration offset and gains have quite a large effect on the
+ * analog output, so it is possible to adjust the analog output to
+ * have an output range significantly different from the board's
+ * nominal output ranges. For a calibrated +/-10V range, the analog
+ * output's offset will be set somewhere near mid-range (0x2000) and
+ * its gain will be near maximum (0x3fff).
+ *
+ * There is really no difference between the board's documented 0-20mA
+ * versus 4-20mA output ranges. To pick one or the other is simply a
+ * matter of adjusting the offset and gain calibration until the board
+ * outputs in the desired range.
+ */
 
 #include <linux/module.h>
-#include <linux/delay.h>
 #include <linux/pci.h>
 
 #include "../comedidev.h"
 
-#define PCI_VENDOR_ID_ADVANTECH	0x13fe
-
-#define NUM_AO_CHANNELS 32
-
-/* register offsets */
-enum board_registers {
-	DAC_CONTROL_REG = 0x0,
-	SYNC_OUTPUT_REG = 0x4,
-	EEPROM_CONTROL_REG = 0x8,
-	SYNC_OUTPUT_TRIGGER_REG = 0xc,
-	BOARD_ID_REG = 0x10
-};
-
-/* bit definitions for registers */
-enum dac_control_contents {
-	DAC_DATA_MASK = 0x3fff,
-	DAC_DESTINATION_MASK = 0xc000,
-	DAC_NORMAL_MODE = 0xc000,
-	DAC_OFFSET_MODE = 0x8000,
-	DAC_GAIN_MODE = 0x4000,
-	DAC_CHANNEL_SELECT_MASK = 0xf0000,
-	DAC_GROUP_SELECT_MASK = 0xf00000
-};
-
-static uint32_t dac_data_bits(uint16_t dac_data)
-{
-	return dac_data & DAC_DATA_MASK;
-}
-
-static uint32_t dac_channel_select_bits(unsigned channel)
-{
-	return (channel << 16) & DAC_CHANNEL_SELECT_MASK;
-}
-
-static uint32_t dac_group_select_bits(unsigned group)
-{
-	return (1 << (20 + group)) & DAC_GROUP_SELECT_MASK;
-}
-
-static uint32_t dac_channel_and_group_select_bits(unsigned comedi_channel)
-{
-	return dac_channel_select_bits(comedi_channel % 8) |
-		dac_group_select_bits(comedi_channel / 8);
-}
-
-enum sync_output_contents {
-	SYNC_MODE = 0x1,
-	DAC_BUSY = 0x2, /* dac state machine is not ready */
-};
-
-enum sync_output_trigger_contents {
-	SYNC_TRIGGER_BITS = 0x0 /* any value works */
-};
-
-enum board_id_contents {
-	BOARD_ID_MASK = 0xf
-};
-
-static const struct comedi_lrange ao_ranges_1724 = {
+/*
+ * PCI bar 2 Register I/O map (dev->iobase)
+ */
+#define PCI1724_DAC_CTRL_REG		0x00
+#define PCI1724_DAC_CTRL_GX(x)		(1 << (20 + ((x) / 8)))
+#define PCI1724_DAC_CTRL_CX(x)		(((x) % 8) << 16)
+#define PCI1724_DAC_CTRL_MODE_GAIN	(1 << 14)
+#define PCI1724_DAC_CTRL_MODE_OFFSET	(2 << 14)
+#define PCI1724_DAC_CTRL_MODE_NORMAL	(3 << 14)
+#define PCI1724_DAC_CTRL_MODE_MASK	(3 << 14)
+#define PCI1724_DAC_CTRL_DATA(x)	(((x) & 0x3fff) << 0)
+#define PCI1724_SYNC_CTRL_REG		0x04
+#define PCI1724_SYNC_CTRL_DACSTAT	(1 << 1)
+#define PCI1724_SYNC_CTRL_SYN		(1 << 0)
+#define PCI1724_EEPROM_CTRL_REG		0x08
+#define PCI1724_SYNC_TRIG_REG		0x0c  /* any value works */
+#define PCI1724_BOARD_ID_REG		0x10
+#define PCI1724_BOARD_ID_MASK		(0xf << 0)
+
+static const struct comedi_lrange adv_pci1724_ao_ranges = {
 	4, {
 		BIP_RANGE(10),
 		RANGE_mA(0, 20),
@@ -125,254 +79,120 @@ static const struct comedi_lrange ao_ranges_1724 = {
 	}
 };
 
-/* this structure is for data unique to this hardware driver. */
-struct adv_pci1724_private {
-	int ao_value[NUM_AO_CHANNELS];
-	int offset_value[NUM_AO_CHANNELS];
-	int gain_value[NUM_AO_CHANNELS];
-};
-
-static int wait_for_dac_idle(struct comedi_device *dev)
-{
-	static const int timeout = 10000;
-	int i;
-
-	for (i = 0; i < timeout; ++i) {
-		if ((inl(dev->iobase + SYNC_OUTPUT_REG) & DAC_BUSY) == 0)
-			break;
-		udelay(1);
-	}
-	if (i == timeout) {
-		dev_err(dev->class_dev,
-			"Timed out waiting for dac to become idle\n");
-		return -EIO;
-	}
-	return 0;
-}
-
-static int set_dac(struct comedi_device *dev, unsigned mode, unsigned channel,
-		   unsigned data)
-{
-	int retval;
-	unsigned control_bits;
-
-	retval = wait_for_dac_idle(dev);
-	if (retval < 0)
-		return retval;
-
-	control_bits = mode;
-	control_bits |= dac_channel_and_group_select_bits(channel);
-	control_bits |= dac_data_bits(data);
-	outl(control_bits, dev->iobase + DAC_CONTROL_REG);
-	return 0;
-}
-
-static int ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
-		    struct comedi_insn *insn, unsigned int *data)
+static int adv_pci1724_dac_idle(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn,
+				unsigned long context)
 {
-	struct adv_pci1724_private *devpriv = dev->private;
-	int channel = CR_CHAN(insn->chanspec);
-	int retval;
-	int i;
+	unsigned int status;
 
-	/* turn off synchronous mode */
-	outl(0, dev->iobase + SYNC_OUTPUT_REG);
-
-	for (i = 0; i < insn->n; ++i) {
-		retval = set_dac(dev, DAC_NORMAL_MODE, channel, data[i]);
-		if (retval < 0)
-			return retval;
-		devpriv->ao_value[channel] = data[i];
-	}
-	return insn->n;
+	status = inl(dev->iobase + PCI1724_SYNC_CTRL_REG);
+	if ((status & PCI1724_SYNC_CTRL_DACSTAT) == 0)
+		return 0;
+	return -EBUSY;
 }
 
-static int ao_readback_insn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
+static int adv_pci1724_insn_write(struct comedi_device *dev,
+				  struct comedi_subdevice *s,
+				  struct comedi_insn *insn,
+				  unsigned int *data)
 {
-	struct adv_pci1724_private *devpriv = dev->private;
-	int channel = CR_CHAN(insn->chanspec);
-	int i;
-
-	if (devpriv->ao_value[channel] < 0) {
-		dev_err(dev->class_dev,
-			"Cannot read back channels which have not yet been written to\n");
-		return -EIO;
-	}
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_value[channel];
-
-	return insn->n;
-}
-
-static int offset_write_insn(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_insn *insn, unsigned int *data)
-{
-	struct adv_pci1724_private *devpriv = dev->private;
-	int channel = CR_CHAN(insn->chanspec);
-	int retval;
-	int i;
-
-	/* turn off synchronous mode */
-	outl(0, dev->iobase + SYNC_OUTPUT_REG);
-
-	for (i = 0; i < insn->n; ++i) {
-		retval = set_dac(dev, DAC_OFFSET_MODE, channel, data[i]);
-		if (retval < 0)
-			return retval;
-		devpriv->offset_value[channel] = data[i];
-	}
-
-	return insn->n;
-}
-
-static int offset_read_insn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
-{
-	struct adv_pci1724_private *devpriv = dev->private;
-	unsigned int channel = CR_CHAN(insn->chanspec);
+	unsigned long mode = (unsigned long)s->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int ctrl;
+	int ret;
 	int i;
 
-	if (devpriv->offset_value[channel] < 0) {
-		dev_err(dev->class_dev,
-			"Cannot read back channels which have not yet been written to\n");
-		return -EIO;
-	}
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->offset_value[channel];
-
-	return insn->n;
-}
-
-static int gain_write_insn(struct comedi_device *dev,
-			   struct comedi_subdevice *s,
-			   struct comedi_insn *insn, unsigned int *data)
-{
-	struct adv_pci1724_private *devpriv = dev->private;
-	int channel = CR_CHAN(insn->chanspec);
-	int retval;
-	int i;
+	ctrl = PCI1724_DAC_CTRL_GX(chan) | PCI1724_DAC_CTRL_CX(chan) | mode;
 
 	/* turn off synchronous mode */
-	outl(0, dev->iobase + SYNC_OUTPUT_REG);
+	outl(0, dev->iobase + PCI1724_SYNC_CTRL_REG);
 
 	for (i = 0; i < insn->n; ++i) {
-		retval = set_dac(dev, DAC_GAIN_MODE, channel, data[i]);
-		if (retval < 0)
-			return retval;
-		devpriv->gain_value[channel] = data[i];
-	}
+		unsigned int val = data[i];
 
-	return insn->n;
-}
+		ret = comedi_timeout(dev, s, insn, adv_pci1724_dac_idle, 0);
+		if (ret)
+			return ret;
 
-static int gain_read_insn(struct comedi_device *dev,
-			  struct comedi_subdevice *s, struct comedi_insn *insn,
-			  unsigned int *data)
-{
-	struct adv_pci1724_private *devpriv = dev->private;
-	unsigned int channel = CR_CHAN(insn->chanspec);
-	int i;
+		outl(ctrl | PCI1724_DAC_CTRL_DATA(val),
+		     dev->iobase + PCI1724_DAC_CTRL_REG);
 
-	if (devpriv->gain_value[channel] < 0) {
-		dev_err(dev->class_dev,
-			"Cannot read back channels which have not yet been written to\n");
-		return -EIO;
+		s->readback[chan] = val;
 	}
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->gain_value[channel];
 
 	return insn->n;
 }
 
-/* Allocate and initialize the subdevice structures.
- */
-static int setup_subdevices(struct comedi_device *dev)
+static int adv_pci1724_auto_attach(struct comedi_device *dev,
+				   unsigned long context_unused)
 {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct comedi_subdevice *s;
+	unsigned int board_id;
 	int ret;
 
+	ret = comedi_pci_enable(dev);
+	if (ret)
+		return ret;
+
+	dev->iobase = pci_resource_start(pcidev, 2);
+	board_id = inl(dev->iobase + PCI1724_BOARD_ID_REG);
+	dev_info(dev->class_dev, "board id: %d\n",
+		 board_id & PCI1724_BOARD_ID_MASK);
+
 	ret = comedi_alloc_subdevices(dev, 3);
 	if (ret)
 		return ret;
 
-	/* analog output subdevice */
+	/* Analog Output subdevice */
 	s = &dev->subdevices[0];
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_GROUND;
-	s->n_chan = NUM_AO_CHANNELS;
-	s->maxdata = 0x3fff;
-	s->range_table = &ao_ranges_1724;
-	s->insn_read = ao_readback_insn;
-	s->insn_write = ao_winsn;
+	s->type		= COMEDI_SUBD_AO;
+	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE | SDF_GROUND;
+	s->n_chan	= 32;
+	s->maxdata	= 0x3fff;
+	s->range_table	= &adv_pci1724_ao_ranges;
+	s->insn_write	= adv_pci1724_insn_write;
+	s->private	= (void *)PCI1724_DAC_CTRL_MODE_NORMAL;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
-	/* offset calibration */
+	/* Offset Calibration subdevice */
 	s = &dev->subdevices[1];
-	s->type = COMEDI_SUBD_CALIB;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
-	s->n_chan = NUM_AO_CHANNELS;
-	s->insn_read = offset_read_insn;
-	s->insn_write = offset_write_insn;
-	s->maxdata = 0x3fff;
+	s->type		= COMEDI_SUBD_CALIB;
+	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
+	s->n_chan	= 32;
+	s->maxdata	= 0x3fff;
+	s->insn_write	= adv_pci1724_insn_write;
+	s->private	= (void *)PCI1724_DAC_CTRL_MODE_OFFSET;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
-	/* gain calibration */
+	/* Gain Calibration subdevice */
 	s = &dev->subdevices[2];
-	s->type = COMEDI_SUBD_CALIB;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
-	s->n_chan = NUM_AO_CHANNELS;
-	s->insn_read = gain_read_insn;
-	s->insn_write = gain_write_insn;
-	s->maxdata = 0x3fff;
-
-	return 0;
-}
-
-static int adv_pci1724_auto_attach(struct comedi_device *dev,
-				   unsigned long context_unused)
-{
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	struct adv_pci1724_private *devpriv;
-	int i;
-	int retval;
-	unsigned int board_id;
-
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
-
-	/* init software copies of output values to indicate we don't know
-	 * what the output value is since it has never been written. */
-	for (i = 0; i < NUM_AO_CHANNELS; ++i) {
-		devpriv->ao_value[i] = -1;
-		devpriv->offset_value[i] = -1;
-		devpriv->gain_value[i] = -1;
-	}
-
-	retval = comedi_pci_enable(dev);
-	if (retval)
-		return retval;
-
-	dev->iobase = pci_resource_start(pcidev, 2);
-	board_id = inl(dev->iobase + BOARD_ID_REG) & BOARD_ID_MASK;
-	dev_info(dev->class_dev, "board id: %d\n", board_id);
-
-	retval = setup_subdevices(dev);
-	if (retval < 0)
-		return retval;
+	s->type		= COMEDI_SUBD_CALIB;
+	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
+	s->n_chan	= 32;
+	s->maxdata	= 0x3fff;
+	s->insn_write	= adv_pci1724_insn_write;
+	s->private	= (void *)PCI1724_DAC_CTRL_MODE_GAIN;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
-	dev_info(dev->class_dev, "%s (pci %s) attached, board id: %u\n",
-		 dev->board_name, pci_name(pcidev), board_id);
 	return 0;
 }
 
 static struct comedi_driver adv_pci1724_driver = {
-	.driver_name = "adv_pci1724",
-	.module = THIS_MODULE,
-	.auto_attach = adv_pci1724_auto_attach,
-	.detach = comedi_pci_detach,
+	.driver_name	= "adv_pci1724",
+	.module		= THIS_MODULE,
+	.auto_attach	= adv_pci1724_auto_attach,
+	.detach		= comedi_pci_detach,
 };
 
 static int adv_pci1724_pci_probe(struct pci_dev *dev,
@@ -389,12 +209,11 @@ static const struct pci_device_id adv_pci1724_pci_table[] = {
 MODULE_DEVICE_TABLE(pci, adv_pci1724_pci_table);
 
 static struct pci_driver adv_pci1724_pci_driver = {
-	.name = "adv_pci1724",
-	.id_table = adv_pci1724_pci_table,
-	.probe = adv_pci1724_pci_probe,
-	.remove = comedi_pci_auto_unconfig,
+	.name		= "adv_pci1724",
+	.id_table	= adv_pci1724_pci_table,
+	.probe		= adv_pci1724_pci_probe,
+	.remove		= comedi_pci_auto_unconfig,
 };
-
 module_comedi_pci_driver(adv_pci1724_driver, adv_pci1724_pci_driver);
 
 MODULE_AUTHOR("Frank Mori Hess <fmh6jj@gmail.com>");
diff --git a/drivers/staging/comedi/drivers/adv_pci_dio.c b/drivers/staging/comedi/drivers/adv_pci_dio.c
index f2e2d7e163bf..09609d6d02da 100644
--- a/drivers/staging/comedi/drivers/adv_pci_dio.c
+++ b/drivers/staging/comedi/drivers/adv_pci_dio.c
@@ -930,7 +930,7 @@ static int pci1760_attach(struct comedi_device *dev)
 
 	s = &dev->subdevices[0];
 	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_COMMON;
+	s->subdev_flags = SDF_READABLE;
 	s->n_chan = 8;
 	s->maxdata = 1;
 	s->len_chanlist = 8;
@@ -939,7 +939,7 @@ static int pci1760_attach(struct comedi_device *dev)
 
 	s = &dev->subdevices[1];
 	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
+	s->subdev_flags = SDF_WRITABLE;
 	s->n_chan = 8;
 	s->maxdata = 1;
 	s->len_chanlist = 8;
@@ -978,7 +978,7 @@ static int pci_dio_add_di(struct comedi_device *dev,
 	const struct dio_boardtype *this_board = dev->board_ptr;
 
 	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_COMMON | d->specflags;
+	s->subdev_flags = SDF_READABLE | d->specflags;
 	if (d->chans > 16)
 		s->subdev_flags |= SDF_LSAMPL;
 	s->n_chan = d->chans;
@@ -1008,7 +1008,7 @@ static int pci_dio_add_do(struct comedi_device *dev,
 	const struct dio_boardtype *this_board = dev->board_ptr;
 
 	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
+	s->subdev_flags = SDF_WRITABLE;
 	if (d->chans > 16)
 		s->subdev_flags |= SDF_LSAMPL;
 	s->n_chan = d->chans;
diff --git a/drivers/staging/comedi/drivers/aio_aio12_8.c b/drivers/staging/comedi/drivers/aio_aio12_8.c
index 538277a691b2..fbc3e5aa94cb 100644
--- a/drivers/staging/comedi/drivers/aio_aio12_8.c
+++ b/drivers/staging/comedi/drivers/aio_aio12_8.c
@@ -212,7 +212,6 @@ static int aio_aio12_8_attach(struct comedi_device *dev,
 		s->maxdata	= 0x0fff;
 		s->range_table	= &range_aio_aio12_8;
 		s->insn_write	= aio_aio12_8_ao_insn_write;
-		s->insn_read	= comedi_readback_insn_read;
 
 		ret = comedi_alloc_subdev_readback(s);
 		if (ret)
diff --git a/drivers/staging/comedi/drivers/amcc_s5933.h b/drivers/staging/comedi/drivers/amcc_s5933.h
index cf6a497b092c..d4b8c0195bd3 100644
--- a/drivers/staging/comedi/drivers/amcc_s5933.h
+++ b/drivers/staging/comedi/drivers/amcc_s5933.h
@@ -110,6 +110,8 @@
 #define  AGCSTS_TCZERO_MASK	0x000000c0
 #define  AGCSTS_FIFO_ST_MASK	0x0000003f
 
+#define AGCSTS_TC_ENABLE	0x10000000
+
 #define AGCSTS_RESET_MBFLAGS	0x08000000
 #define AGCSTS_RESET_P2A_FIFO	0x04000000
 #define AGCSTS_RESET_A2P_FIFO	0x02000000
diff --git a/drivers/staging/comedi/drivers/amplc_dio200_common.c b/drivers/staging/comedi/drivers/amplc_dio200_common.c
index 2c1bfb09601d..26aad705aad3 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200_common.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200_common.c
@@ -120,7 +120,6 @@ struct dio200_subdev_intr {
 	unsigned int ofs;
 	unsigned int valid_isns;
 	unsigned int enabled_isns;
-	unsigned int stopcount;
 	bool active:1;
 };
 
@@ -256,7 +255,6 @@ static void dio200_read_scan_intr(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
 				  unsigned int triggered)
 {
-	struct dio200_subdev_intr *subpriv = s->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned short val;
 	unsigned int n, ch;
@@ -267,26 +265,12 @@ static void dio200_read_scan_intr(struct comedi_device *dev,
 		if (triggered & (1U << ch))
 			val |= (1U << n);
 	}
-	/* Write the scan to the buffer. */
-	if (comedi_buf_put(s, val)) {
-		s->async->events |= (COMEDI_CB_BLOCK | COMEDI_CB_EOS);
-	} else {
-		/* Error!  Stop acquisition.  */
-		dio200_stop_intr(dev, s);
-		s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW;
-		dev_err(dev->class_dev, "buffer overflow\n");
-	}
 
-	/* Check for end of acquisition. */
-	if (cmd->stop_src == TRIG_COUNT) {
-		if (subpriv->stopcount > 0) {
-			subpriv->stopcount--;
-			if (subpriv->stopcount == 0) {
-				s->async->events |= COMEDI_CB_EOA;
-				dio200_stop_intr(dev, s);
-			}
-		}
-	}
+	comedi_buf_write_samples(s, &val, 1);
+
+	if (cmd->stop_src == TRIG_COUNT &&
+	    s->async->scans_done >= cmd->stop_arg)
+		s->async->events |= COMEDI_CB_EOA;
 }
 
 static int dio200_handle_read_intr(struct comedi_device *dev,
@@ -297,13 +281,11 @@ static int dio200_handle_read_intr(struct comedi_device *dev,
 	unsigned triggered;
 	unsigned intstat;
 	unsigned cur_enabled;
-	unsigned int oldevents;
 	unsigned long flags;
 
 	triggered = 0;
 
 	spin_lock_irqsave(&subpriv->spinlock, flags);
-	oldevents = s->async->events;
 	if (board->has_int_sce) {
 		/*
 		 * Collect interrupt sources that have triggered and disable
@@ -356,8 +338,7 @@ static int dio200_handle_read_intr(struct comedi_device *dev,
 	}
 	spin_unlock_irqrestore(&subpriv->spinlock, flags);
 
-	if (oldevents != s->async->events)
-		comedi_event(dev, s);
+	comedi_handle_events(dev, s);
 
 	return (triggered != 0);
 }
@@ -436,7 +417,6 @@ static int dio200_subdev_intr_cmd(struct comedi_device *dev,
 	spin_lock_irqsave(&subpriv->spinlock, flags);
 
 	subpriv->active = true;
-	subpriv->stopcount = cmd->stop_arg;
 
 	if (cmd->start_src == TRIG_INT)
 		s->async->inttrig = dio200_inttrig_start_intr;
@@ -469,7 +449,7 @@ static int dio200_subdev_intr_init(struct comedi_device *dev,
 		dio200_write8(dev, subpriv->ofs, 0);
 
 	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE | SDF_CMD_READ;
+	s->subdev_flags = SDF_READABLE | SDF_CMD_READ | SDF_PACKED;
 	if (board->has_int_sce) {
 		s->n_chan = DIO200_MAX_ISNS;
 		s->len_chanlist = DIO200_MAX_ISNS;
diff --git a/drivers/staging/comedi/drivers/amplc_pc236_common.c b/drivers/staging/comedi/drivers/amplc_pc236_common.c
index 963c5d868b81..be87172d1f3f 100644
--- a/drivers/staging/comedi/drivers/amplc_pc236_common.c
+++ b/drivers/staging/comedi/drivers/amplc_pc236_common.c
@@ -135,9 +135,8 @@ static irqreturn_t pc236_interrupt(int irq, void *d)
 
 	handled = pc236_intr_check(dev);
 	if (dev->attached && handled) {
-		comedi_buf_put(s, 0);
-		s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
-		comedi_event(dev, s);
+		comedi_buf_write_samples(s, &s->state, 1);
+		comedi_handle_events(dev, s);
 	}
 	return IRQ_RETVAL(handled);
 }
diff --git a/drivers/staging/comedi/drivers/amplc_pc263.c b/drivers/staging/comedi/drivers/amplc_pc263.c
index f8e551d8fd9e..b1946ce6ecc1 100644
--- a/drivers/staging/comedi/drivers/amplc_pc263.c
+++ b/drivers/staging/comedi/drivers/amplc_pc263.c
@@ -83,7 +83,7 @@ static int pc263_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s = &dev->subdevices[0];
 	/* digital output subdevice */
 	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->subdev_flags = SDF_WRITABLE;
 	s->n_chan = 16;
 	s->maxdata = 1;
 	s->range_table = &range_digital;
diff --git a/drivers/staging/comedi/drivers/amplc_pci224.c b/drivers/staging/comedi/drivers/amplc_pci224.c
index 3bbbb57f19d6..924c8298c7a0 100644
--- a/drivers/staging/comedi/drivers/amplc_pci224.c
+++ b/drivers/staging/comedi/drivers/amplc_pci224.c
@@ -381,7 +381,6 @@ struct pci224_private {
 	unsigned short daccon;
 	unsigned int cached_div1;
 	unsigned int cached_div2;
-	unsigned int ao_stop_count;
 	unsigned short ao_enab;	/* max 16 channels so 'short' will do */
 	unsigned char intsce;
 };
@@ -514,30 +513,21 @@ static void pci224_ao_handle_fifo(struct comedi_device *dev,
 {
 	struct pci224_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
-	unsigned int bytes_per_scan = cfc_bytes_per_scan(s);
-	unsigned int num_scans;
+	unsigned int num_scans = comedi_nscans_left(s, 0);
 	unsigned int room;
 	unsigned short dacstat;
 	unsigned int i, n;
 
-	/* Determine number of scans available in buffer. */
-	num_scans = comedi_buf_read_n_available(s) / bytes_per_scan;
-	if (cmd->stop_src == TRIG_COUNT) {
-		/* Fixed number of scans. */
-		if (num_scans > devpriv->ao_stop_count)
-			num_scans = devpriv->ao_stop_count;
-	}
-
 	/* Determine how much room is in the FIFO (in samples). */
 	dacstat = inw(dev->iobase + PCI224_DACCON);
 	switch (dacstat & PCI224_DACCON_FIFOFL_MASK) {
 	case PCI224_DACCON_FIFOFL_EMPTY:
 		room = PCI224_FIFO_ROOM_EMPTY;
 		if (cmd->stop_src == TRIG_COUNT &&
-		    devpriv->ao_stop_count == 0) {
+		    s->async->scans_done >= cmd->stop_arg) {
 			/* FIFO empty at end of counted acquisition. */
 			s->async->events |= COMEDI_CB_EOA;
-			cfc_handle_events(dev, s);
+			comedi_handle_events(dev, s);
 			return;
 		}
 		break;
@@ -568,25 +558,23 @@ static void pci224_ao_handle_fifo(struct comedi_device *dev,
 
 	/* Process scans. */
 	for (n = 0; n < num_scans; n++) {
-		cfc_read_array_from_buffer(s, &devpriv->ao_scan_vals[0],
-					   bytes_per_scan);
+		comedi_buf_read_samples(s, &devpriv->ao_scan_vals[0],
+					cmd->chanlist_len);
 		for (i = 0; i < cmd->chanlist_len; i++) {
 			outw(devpriv->ao_scan_vals[devpriv->ao_scan_order[i]],
 			     dev->iobase + PCI224_DACDATA);
 		}
 	}
-	if (cmd->stop_src == TRIG_COUNT) {
-		devpriv->ao_stop_count -= num_scans;
-		if (devpriv->ao_stop_count == 0) {
-			/*
-			 * Change FIFO interrupt trigger level to wait
-			 * until FIFO is empty.
-			 */
-			devpriv->daccon = COMBINE(devpriv->daccon,
-						  PCI224_DACCON_FIFOINTR_EMPTY,
-						  PCI224_DACCON_FIFOINTR_MASK);
-			outw(devpriv->daccon, dev->iobase + PCI224_DACCON);
-		}
+	if (cmd->stop_src == TRIG_COUNT &&
+	    s->async->scans_done >= cmd->stop_arg) {
+		/*
+		 * Change FIFO interrupt trigger level to wait
+		 * until FIFO is empty.
+		 */
+		devpriv->daccon = COMBINE(devpriv->daccon,
+					  PCI224_DACCON_FIFOINTR_EMPTY,
+					  PCI224_DACCON_FIFOINTR_MASK);
+		outw(devpriv->daccon, dev->iobase + PCI224_DACCON);
 	}
 	if ((devpriv->daccon & PCI224_DACCON_TRIG_MASK) ==
 	    PCI224_DACCON_TRIG_NONE) {
@@ -618,7 +606,7 @@ static void pci224_ao_handle_fifo(struct comedi_device *dev,
 		outw(devpriv->daccon, dev->iobase + PCI224_DACCON);
 	}
 
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 }
 
 static int pci224_ao_inttrig_start(struct comedi_device *dev,
@@ -908,14 +896,6 @@ static int pci224_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	if (cmd->scan_begin_src == TRIG_TIMER)
 		pci224_ao_start_pacer(dev, s);
 
-	/*
-	 * Sort out end of acquisition.
-	 */
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->ao_stop_count = cmd->stop_arg;
-	else	/* TRIG_EXT | TRIG_NONE */
-		devpriv->ao_stop_count = 0;
-
 	spin_lock_irqsave(&devpriv->ao_spinlock, flags);
 	if (cmd->start_src == TRIG_INT) {
 		s->async->inttrig = pci224_ao_inttrig_start;
@@ -1095,7 +1075,6 @@ pci224_auto_attach(struct comedi_device *dev, unsigned long context_model)
 	s->maxdata = (1 << thisboard->ao_bits) - 1;
 	s->range_table = thisboard->ao_range;
 	s->insn_write = pci224_ao_insn_write;
-	s->insn_read = comedi_readback_insn_read;
 	s->len_chanlist = s->n_chan;
 	dev->write_subdev = s;
 	s->do_cmd = pci224_ao_cmd;
diff --git a/drivers/staging/comedi/drivers/amplc_pci230.c b/drivers/staging/comedi/drivers/amplc_pci230.c
index 01796cd28e5b..49806a5e514c 100644
--- a/drivers/staging/comedi/drivers/amplc_pci230.c
+++ b/drivers/staging/comedi/drivers/amplc_pci230.c
@@ -35,7 +35,7 @@
  * automatically.
  *
  * The PCI230+ and PCI260+ have the same PCI device IDs as the PCI230 and
- * PCI260, but can be distinguished by the the size of the PCI regions.  A
+ * PCI260, but can be distinguished by the size of the PCI regions.  A
  * card will be configured as a "+" model if detected as such.
  *
  * Subdevices:
@@ -490,9 +490,6 @@ struct pci230_private {
 	spinlock_t ai_stop_spinlock;	/* Spin lock for stopping AI command */
 	spinlock_t ao_stop_spinlock;	/* Spin lock for stopping AO command */
 	unsigned long daqio;		/* PCI230's DAQ I/O space */
-	unsigned int ai_scan_count;	/* Number of AI scans remaining */
-	unsigned int ai_scan_pos;	/* Current position within AI scan */
-	unsigned int ao_scan_count;	/* Number of AO scans remaining.  */
 	int intr_cpuid;			/* ID of CPU running ISR */
 	unsigned short hwver;		/* Hardware version (for '+' models) */
 	unsigned short adccon;		/* ADCCON register value */
@@ -1074,37 +1071,27 @@ static void pci230_ao_stop(struct comedi_device *dev,
 static void pci230_handle_ao_nofifo(struct comedi_device *dev,
 				    struct comedi_subdevice *s)
 {
-	struct pci230_private *devpriv = dev->private;
-	unsigned short data;
-	int i, ret;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
+	unsigned short data;
+	int i;
 
-	if (cmd->stop_src == TRIG_COUNT && devpriv->ao_scan_count == 0)
+	if (cmd->stop_src == TRIG_COUNT && async->scans_done >= cmd->stop_arg)
 		return;
+
 	for (i = 0; i < cmd->chanlist_len; i++) {
 		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
 
-		/* Read sample from Comedi's circular buffer. */
-		ret = comedi_buf_get(s, &data);
-		if (ret == 0) {
-			s->async->events |= COMEDI_CB_OVERFLOW;
-			pci230_ao_stop(dev, s);
-			dev_err(dev->class_dev, "AO buffer underrun\n");
+		if (!comedi_buf_read_samples(s, &data, 1)) {
+			async->events |= COMEDI_CB_OVERFLOW;
 			return;
 		}
 		pci230_ao_write_nofifo(dev, data, chan);
 		s->readback[chan] = data;
 	}
-	async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
-	if (cmd->stop_src == TRIG_COUNT) {
-		devpriv->ao_scan_count--;
-		if (devpriv->ao_scan_count == 0) {
-			/* End of acquisition. */
-			async->events |= COMEDI_CB_EOA;
-			pci230_ao_stop(dev, s);
-		}
-	}
+
+	if (cmd->stop_src == TRIG_COUNT && async->scans_done >= cmd->stop_arg)
+		async->events |= COMEDI_CB_EOA;
 }
 
 /*
@@ -1117,26 +1104,18 @@ static bool pci230_handle_ao_fifo(struct comedi_device *dev,
 	struct pci230_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
-	unsigned int num_scans;
+	unsigned int num_scans = comedi_nscans_left(s, 0);
 	unsigned int room;
 	unsigned short dacstat;
 	unsigned int i, n;
 	unsigned int events = 0;
-	bool running;
 
 	/* Get DAC FIFO status. */
 	dacstat = inw(devpriv->daqio + PCI230_DACCON);
-	/* Determine number of scans available in buffer. */
-	num_scans = comedi_buf_read_n_available(s) / cfc_bytes_per_scan(s);
-	if (cmd->stop_src == TRIG_COUNT) {
-		/* Fixed number of scans. */
-		if (num_scans > devpriv->ao_scan_count)
-			num_scans = devpriv->ao_scan_count;
-		if (devpriv->ao_scan_count == 0) {
-			/* End of acquisition. */
-			events |= COMEDI_CB_EOA;
-		}
-	}
+
+	if (cmd->stop_src == TRIG_COUNT && num_scans == 0)
+		events |= COMEDI_CB_EOA;
+
 	if (events == 0) {
 		/* Check for FIFO underrun. */
 		if (dacstat & PCI230P2_DAC_FIFO_UNDERRUN_LATCHED) {
@@ -1175,27 +1154,22 @@ static bool pci230_handle_ao_fifo(struct comedi_device *dev,
 				unsigned int chan = CR_CHAN(cmd->chanlist[i]);
 				unsigned short datum;
 
-				comedi_buf_get(s, &datum);
+				comedi_buf_read_samples(s, &datum, 1);
 				pci230_ao_write_fifo(dev, datum, chan);
 				s->readback[chan] = datum;
 			}
 		}
-		events |= COMEDI_CB_EOS | COMEDI_CB_BLOCK;
-		if (cmd->stop_src == TRIG_COUNT) {
-			devpriv->ao_scan_count -= num_scans;
-			if (devpriv->ao_scan_count == 0) {
-				/*
-				 * All data for the command has been written
-				 * to FIFO.  Set FIFO interrupt trigger level
-				 * to 'empty'.
-				 */
-				devpriv->daccon =
-				    (devpriv->daccon &
-				     ~PCI230P2_DAC_INT_FIFO_MASK) |
-				    PCI230P2_DAC_INT_FIFO_EMPTY;
-				outw(devpriv->daccon,
-				     devpriv->daqio + PCI230_DACCON);
-			}
+
+		if (cmd->stop_src == TRIG_COUNT &&
+		    async->scans_done >= cmd->stop_arg) {
+			/*
+			 * All data for the command has been written
+			 * to FIFO.  Set FIFO interrupt trigger level
+			 * to 'empty'.
+			 */
+			devpriv->daccon &= ~PCI230P2_DAC_INT_FIFO_MASK;
+			devpriv->daccon |= PCI230P2_DAC_INT_FIFO_EMPTY;
+			outw(devpriv->daccon, devpriv->daqio + PCI230_DACCON);
 		}
 		/* Check if FIFO underrun occurred while writing to FIFO. */
 		dacstat = inw(devpriv->daqio + PCI230_DACCON);
@@ -1204,15 +1178,8 @@ static bool pci230_handle_ao_fifo(struct comedi_device *dev,
 			events |= COMEDI_CB_OVERFLOW | COMEDI_CB_ERROR;
 		}
 	}
-	if (events & (COMEDI_CB_EOA | COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW)) {
-		/* Stopping AO due to completion or error. */
-		pci230_ao_stop(dev, s);
-		running = false;
-	} else {
-		running = true;
-	}
 	async->events |= events;
-	return running;
+	return !(async->events & COMEDI_CB_CANCEL_MASK);
 }
 
 static int pci230_ao_inttrig_scan_begin(struct comedi_device *dev,
@@ -1235,7 +1202,7 @@ static int pci230_ao_inttrig_scan_begin(struct comedi_device *dev,
 		/* Not using DAC FIFO. */
 		spin_unlock_irqrestore(&devpriv->ao_stop_spinlock, irqflags);
 		pci230_handle_ao_nofifo(dev, s);
-		comedi_event(dev, s);
+		comedi_handle_events(dev, s);
 	} else {
 		/* Using DAC FIFO. */
 		/* Read DACSWTRIG register to trigger conversion. */
@@ -1265,7 +1232,7 @@ static void pci230_ao_start(struct comedi_device *dev,
 
 		/* Preload FIFO data. */
 		run = pci230_handle_ao_fifo(dev, s);
-		comedi_event(dev, s);
+		comedi_handle_events(dev, s);
 		if (!run) {
 			/* Stopped. */
 			return;
@@ -1354,8 +1321,6 @@ static int pci230_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			return -EBUSY;
 	}
 
-	devpriv->ao_scan_count = cmd->stop_arg;
-
 	/*
 	 * Set range - see analogue output range table; 0 => unipolar 10V,
 	 * 1 => bipolar +/-10V range scale
@@ -1754,19 +1719,15 @@ static void pci230_ai_update_fifo_trigger_level(struct comedi_device *dev,
 {
 	struct pci230_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
-	unsigned int scanlen = cmd->scan_end_arg;
 	unsigned int wake;
 	unsigned short triglev;
 	unsigned short adccon;
 
 	if (cmd->flags & CMDF_WAKE_EOS)
-		wake = scanlen - devpriv->ai_scan_pos;
-	else if (cmd->stop_src != TRIG_COUNT ||
-		 devpriv->ai_scan_count >= PCI230_ADC_FIFOLEVEL_HALFFULL ||
-		 scanlen >= PCI230_ADC_FIFOLEVEL_HALFFULL)
-		wake = PCI230_ADC_FIFOLEVEL_HALFFULL;
+		wake = cmd->scan_end_arg - s->async->cur_chan;
 	else
-		wake = devpriv->ai_scan_count * scanlen - devpriv->ai_scan_pos;
+		wake = comedi_nsamples_left(s, PCI230_ADC_FIFOLEVEL_HALFFULL);
+
 	if (wake >= PCI230_ADC_FIFOLEVEL_HALFFULL) {
 		triglev = PCI230_ADC_INT_FIFO_HALF;
 	} else if (wake > 1 && devpriv->hwver > 0) {
@@ -2059,28 +2020,17 @@ static void pci230_handle_ai(struct comedi_device *dev,
 	struct pci230_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
-	unsigned int scanlen = cmd->scan_end_arg;
-	unsigned int events = 0;
 	unsigned int status_fifo;
 	unsigned int i;
 	unsigned int todo;
 	unsigned int fifoamount;
+	unsigned short val;
 
 	/* Determine number of samples to read. */
-	if (cmd->stop_src != TRIG_COUNT) {
-		todo = PCI230_ADC_FIFOLEVEL_HALFFULL;
-	} else if (devpriv->ai_scan_count == 0) {
-		todo = 0;
-	} else if (devpriv->ai_scan_count > PCI230_ADC_FIFOLEVEL_HALFFULL ||
-		   scanlen > PCI230_ADC_FIFOLEVEL_HALFFULL) {
-		todo = PCI230_ADC_FIFOLEVEL_HALFFULL;
-	} else {
-		todo = devpriv->ai_scan_count * scanlen - devpriv->ai_scan_pos;
-		if (todo > PCI230_ADC_FIFOLEVEL_HALFFULL)
-			todo = PCI230_ADC_FIFOLEVEL_HALFFULL;
-	}
+	todo = comedi_nsamples_left(s, PCI230_ADC_FIFOLEVEL_HALFFULL);
 	if (todo == 0)
 		return;
+
 	fifoamount = 0;
 	for (i = 0; i < todo; i++) {
 		if (fifoamount == 0) {
@@ -2092,7 +2042,7 @@ static void pci230_handle_ai(struct comedi_device *dev,
 				 * unnoticed by the caller.
 				 */
 				dev_err(dev->class_dev, "AI FIFO overrun\n");
-				events |= COMEDI_CB_OVERFLOW | COMEDI_CB_ERROR;
+				async->events |= COMEDI_CB_ERROR;
 				break;
 			} else if (status_fifo & PCI230_ADC_FIFO_EMPTY) {
 				/* FIFO empty. */
@@ -2111,37 +2061,23 @@ static void pci230_handle_ai(struct comedi_device *dev,
 				fifoamount = 1;
 			}
 		}
-		/* Read sample and store in Comedi's circular buffer. */
-		if (comedi_buf_put(s, pci230_ai_read(dev)) == 0) {
-			events |= COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW;
-			dev_err(dev->class_dev, "AI buffer overflow\n");
+
+		val = pci230_ai_read(dev);
+		if (!comedi_buf_write_samples(s, &val, 1))
 			break;
-		}
+
 		fifoamount--;
-		devpriv->ai_scan_pos++;
-		if (devpriv->ai_scan_pos == scanlen) {
-			/* End of scan. */
-			devpriv->ai_scan_pos = 0;
-			devpriv->ai_scan_count--;
-			async->events |= COMEDI_CB_EOS;
+
+		if (cmd->stop_src == TRIG_COUNT &&
+		    async->scans_done >= cmd->stop_arg) {
+			async->events |= COMEDI_CB_EOA;
+			break;
 		}
 	}
-	if (cmd->stop_src == TRIG_COUNT && devpriv->ai_scan_count == 0) {
-		/* End of acquisition. */
-		events |= COMEDI_CB_EOA;
-	} else {
-		/* More samples required, tell Comedi to block. */
-		events |= COMEDI_CB_BLOCK;
-	}
-	async->events |= events;
-	if (async->events & (COMEDI_CB_EOA | COMEDI_CB_ERROR |
-			     COMEDI_CB_OVERFLOW)) {
-		/* disable hardware conversions */
-		pci230_ai_stop(dev, s);
-	} else {
-		/* update FIFO interrupt trigger level */
+
+	/* update FIFO interrupt trigger level if still running */
+	if (!(async->events & COMEDI_CB_CANCEL_MASK))
 		pci230_ai_update_fifo_trigger_level(dev, s);
-	}
 }
 
 static int pci230_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
@@ -2177,9 +2113,6 @@ static int pci230_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	if (!pci230_claim_shared(dev, res_mask, OWNER_AICMD))
 		return -EBUSY;
 
-	devpriv->ai_scan_count = cmd->stop_arg;
-	devpriv->ai_scan_pos = 0;	/* Position within scan. */
-
 	/*
 	 * Steps:
 	 * - Set channel scan list.
@@ -2355,7 +2288,8 @@ static irqreturn_t pci230_interrupt(int irq, void *d)
 	unsigned char status_int, valid_status_int, temp_ier;
 	struct comedi_device *dev = (struct comedi_device *)d;
 	struct pci230_private *devpriv = dev->private;
-	struct comedi_subdevice *s;
+	struct comedi_subdevice *s_ao = dev->write_subdev;
+	struct comedi_subdevice *s_ai = dev->read_subdev;
 	unsigned long irqflags;
 
 	/* Read interrupt status/enable register. */
@@ -2385,23 +2319,14 @@ static irqreturn_t pci230_interrupt(int irq, void *d)
 	 * two.
 	 */
 
-	if (valid_status_int & PCI230_INT_ZCLK_CT1) {
-		s = dev->write_subdev;
-		pci230_handle_ao_nofifo(dev, s);
-		comedi_event(dev, s);
-	}
+	if (valid_status_int & PCI230_INT_ZCLK_CT1)
+		pci230_handle_ao_nofifo(dev, s_ao);
 
-	if (valid_status_int & PCI230P2_INT_DAC) {
-		s = dev->write_subdev;
-		pci230_handle_ao_fifo(dev, s);
-		comedi_event(dev, s);
-	}
+	if (valid_status_int & PCI230P2_INT_DAC)
+		pci230_handle_ao_fifo(dev, s_ao);
 
-	if (valid_status_int & PCI230_INT_ADC) {
-		s = dev->read_subdev;
-		pci230_handle_ai(dev, s);
-		comedi_event(dev, s);
-	}
+	if (valid_status_int & PCI230_INT_ADC)
+		pci230_handle_ai(dev, s_ai);
 
 	/* Reenable interrupts. */
 	spin_lock_irqsave(&devpriv->isr_spinlock, irqflags);
@@ -2410,6 +2335,9 @@ static irqreturn_t pci230_interrupt(int irq, void *d)
 	devpriv->intr_running = false;
 	spin_unlock_irqrestore(&devpriv->isr_spinlock, irqflags);
 
+	comedi_handle_events(dev, s_ao);
+	comedi_handle_events(dev, s_ai);
+
 	return IRQ_HANDLED;
 }
 
@@ -2583,7 +2511,6 @@ static int pci230_auto_attach(struct comedi_device *dev,
 		s->maxdata = (1 << thisboard->ao_bits) - 1;
 		s->range_table = &pci230_ao_range;
 		s->insn_write = pci230_ao_insn_write;
-		s->insn_read = comedi_readback_insn_read;
 		s->len_chanlist = 2;
 		if (dev->irq) {
 			dev->write_subdev = s;
diff --git a/drivers/staging/comedi/drivers/amplc_pci263.c b/drivers/staging/comedi/drivers/amplc_pci263.c
index 2259bee98d48..0d2224b832ac 100644
--- a/drivers/staging/comedi/drivers/amplc_pci263.c
+++ b/drivers/staging/comedi/drivers/amplc_pci263.c
@@ -71,7 +71,7 @@ static int pci263_auto_attach(struct comedi_device *dev,
 	s = &dev->subdevices[0];
 	/* digital output subdevice */
 	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->subdev_flags = SDF_WRITABLE;
 	s->n_chan = 16;
 	s->maxdata = 1;
 	s->range_table = &range_digital;
diff --git a/drivers/staging/comedi/drivers/c6xdigio.c b/drivers/staging/comedi/drivers/c6xdigio.c
index e03dd6e71415..e7cb7032a910 100644
--- a/drivers/staging/comedi/drivers/c6xdigio.c
+++ b/drivers/staging/comedi/drivers/c6xdigio.c
@@ -265,7 +265,7 @@ static int c6xdigio_attach(struct comedi_device *dev,
 	s = &dev->subdevices[0];
 	/* pwm output subdevice */
 	s->type		= COMEDI_SUBD_PWM;
-	s->subdev_flags	= SDF_WRITEABLE;
+	s->subdev_flags	= SDF_WRITABLE;
 	s->n_chan	= 2;
 	s->maxdata	= 500;
 	s->range_table	= &range_unknown;
diff --git a/drivers/staging/comedi/drivers/cb_das16_cs.c b/drivers/staging/comedi/drivers/cb_das16_cs.c
index f88880aea6da..0a48d2a961d5 100644
--- a/drivers/staging/comedi/drivers/cb_das16_cs.c
+++ b/drivers/staging/comedi/drivers/cb_das16_cs.c
@@ -305,7 +305,6 @@ static int das16cs_auto_attach(struct comedi_device *dev,
 		s->maxdata	= 0xffff;
 		s->range_table	= &range_bipolar10;
 		s->insn_write	= &das16cs_ao_insn_write;
-		s->insn_read	= comedi_readback_insn_read;
 
 		ret = comedi_alloc_subdev_readback(s);
 		if (ret)
diff --git a/drivers/staging/comedi/drivers/cb_pcidas.c b/drivers/staging/comedi/drivers/cb_pcidas.c
index 1ec363b7505c..669b1703eb99 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas.c
@@ -346,8 +346,6 @@ struct cb_pcidas_private {
 	/* divisors of master clock for analog input pacing */
 	unsigned int divisor1;
 	unsigned int divisor2;
-	/* number of analog input samples remaining */
-	unsigned int count;
 	/* bits to write to registers */
 	unsigned int adc_fifo_bits;
 	unsigned int s5933_intcsr_bits;
@@ -358,11 +356,6 @@ struct cb_pcidas_private {
 	/* divisors of master clock for analog output pacing */
 	unsigned int ao_divisor1;
 	unsigned int ao_divisor2;
-	/* number of analog output samples remaining */
-	unsigned int ao_count;
-	unsigned int caldac_value[NUM_CHANNELS_8800];
-	unsigned int trimpot_value[NUM_CHANNELS_8402];
-	unsigned int dac08_value;
 	unsigned int calibration_source;
 };
 
@@ -596,25 +589,14 @@ static void write_calibration_bitstream(struct comedi_device *dev,
 	}
 }
 
-static int caldac_8800_write(struct comedi_device *dev, unsigned int address,
-			     uint8_t value)
+static void caldac_8800_write(struct comedi_device *dev,
+			      unsigned int chan, uint8_t val)
 {
 	struct cb_pcidas_private *devpriv = dev->private;
-	static const int num_caldac_channels = 8;
 	static const int bitstream_length = 11;
-	unsigned int bitstream = ((address & 0x7) << 8) | value;
+	unsigned int bitstream = ((chan & 0x7) << 8) | val;
 	static const int caldac_8800_udelay = 1;
 
-	if (address >= num_caldac_channels) {
-		dev_err(dev->class_dev, "illegal caldac channel\n");
-		return -1;
-	}
-
-	if (value == devpriv->caldac_value[address])
-		return 1;
-
-	devpriv->caldac_value[address] = value;
-
 	write_calibration_bitstream(dev, cal_enable_bits(dev), bitstream,
 				    bitstream_length);
 
@@ -623,75 +605,62 @@ static int caldac_8800_write(struct comedi_device *dev, unsigned int address,
 	     devpriv->control_status + CALIBRATION_REG);
 	udelay(caldac_8800_udelay);
 	outw(cal_enable_bits(dev), devpriv->control_status + CALIBRATION_REG);
-
-	return 1;
 }
 
-static int caldac_write_insn(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_insn *insn, unsigned int *data)
+static int cb_pcidas_caldac_insn_write(struct comedi_device *dev,
+				       struct comedi_subdevice *s,
+				       struct comedi_insn *insn,
+				       unsigned int *data)
 {
-	const unsigned int channel = CR_CHAN(insn->chanspec);
+	unsigned int chan = CR_CHAN(insn->chanspec);
 
-	return caldac_8800_write(dev, channel, data[0]);
-}
+	if (insn->n) {
+		unsigned int val = data[insn->n - 1];
 
-static int caldac_read_insn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
-{
-	struct cb_pcidas_private *devpriv = dev->private;
-
-	data[0] = devpriv->caldac_value[CR_CHAN(insn->chanspec)];
+		if (s->readback[chan] != val) {
+			caldac_8800_write(dev, chan, val);
+			s->readback[chan] = val;
+		}
+	}
 
-	return 1;
+	return insn->n;
 }
 
 /* 1602/16 pregain offset */
 static void dac08_write(struct comedi_device *dev, unsigned int value)
 {
 	struct cb_pcidas_private *devpriv = dev->private;
-	unsigned long cal_reg;
-
-	if (devpriv->dac08_value != value) {
-		devpriv->dac08_value = value;
-
-		cal_reg = devpriv->control_status + CALIBRATION_REG;
 
-		value &= 0xff;
-		value |= cal_enable_bits(dev);
+	value &= 0xff;
+	value |= cal_enable_bits(dev);
 
-		/* latch the new value into the caldac */
-		outw(value, cal_reg);
-		udelay(1);
-		outw(value | SELECT_DAC08_BIT, cal_reg);
-		udelay(1);
-		outw(value, cal_reg);
-		udelay(1);
-	}
+	/* latch the new value into the caldac */
+	outw(value, devpriv->control_status + CALIBRATION_REG);
+	udelay(1);
+	outw(value | SELECT_DAC08_BIT,
+	     devpriv->control_status + CALIBRATION_REG);
+	udelay(1);
+	outw(value, devpriv->control_status + CALIBRATION_REG);
+	udelay(1);
 }
 
-static int dac08_write_insn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
+static int cb_pcidas_dac08_insn_write(struct comedi_device *dev,
+				      struct comedi_subdevice *s,
+				      struct comedi_insn *insn,
+				      unsigned int *data)
 {
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		dac08_write(dev, data[i]);
-
-	return insn->n;
-}
+	unsigned int chan = CR_CHAN(insn->chanspec);
 
-static int dac08_read_insn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data)
-{
-	struct cb_pcidas_private *devpriv = dev->private;
+	if (insn->n) {
+		unsigned int val = data[insn->n - 1];
 
-	data[0] = devpriv->dac08_value;
+		if (s->readback[chan] != val) {
+			dac08_write(dev, val);
+			s->readback[chan] = val;
+		}
+	}
 
-	return 1;
+	return insn->n;
 }
 
 static int trimpot_7376_write(struct comedi_device *dev, uint8_t value)
@@ -740,50 +709,41 @@ static int trimpot_8402_write(struct comedi_device *dev, unsigned int channel,
 	return 0;
 }
 
-static int cb_pcidas_trimpot_write(struct comedi_device *dev,
-				   unsigned int channel, unsigned int value)
+static void cb_pcidas_trimpot_write(struct comedi_device *dev,
+				    unsigned int chan, unsigned int val)
 {
 	const struct cb_pcidas_board *thisboard = dev->board_ptr;
-	struct cb_pcidas_private *devpriv = dev->private;
-
-	if (devpriv->trimpot_value[channel] == value)
-		return 1;
 
-	devpriv->trimpot_value[channel] = value;
 	switch (thisboard->trimpot) {
 	case AD7376:
-		trimpot_7376_write(dev, value);
+		trimpot_7376_write(dev, val);
 		break;
 	case AD8402:
-		trimpot_8402_write(dev, channel, value);
+		trimpot_8402_write(dev, chan, val);
 		break;
 	default:
 		dev_err(dev->class_dev, "driver bug?\n");
-		return -1;
+		break;
 	}
-
-	return 1;
 }
 
-static int trimpot_write_insn(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn, unsigned int *data)
+static int cb_pcidas_trimpot_insn_write(struct comedi_device *dev,
+					struct comedi_subdevice *s,
+					struct comedi_insn *insn,
+					unsigned int *data)
 {
-	unsigned int channel = CR_CHAN(insn->chanspec);
+	unsigned int chan = CR_CHAN(insn->chanspec);
 
-	return cb_pcidas_trimpot_write(dev, channel, data[0]);
-}
+	if (insn->n) {
+		unsigned int val = data[insn->n - 1];
 
-static int trimpot_read_insn(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_insn *insn, unsigned int *data)
-{
-	struct cb_pcidas_private *devpriv = dev->private;
-	unsigned int channel = CR_CHAN(insn->chanspec);
-
-	data[0] = devpriv->trimpot_value[channel];
+		if (s->readback[chan] != val) {
+			cb_pcidas_trimpot_write(dev, chan, val);
+			s->readback[chan] = val;
+		}
+	}
 
-	return 1;
+	return insn->n;
 }
 
 static int cb_pcidas_ai_check_chanlist(struct comedi_device *dev,
@@ -976,9 +936,6 @@ static int cb_pcidas_ai_cmd(struct comedi_device *dev,
 	if (cmd->scan_begin_src == TRIG_TIMER || cmd->convert_src == TRIG_TIMER)
 		cb_pcidas_ai_load_counters(dev);
 
-	/*  set number of conversions */
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->count = cmd->chanlist_len * cmd->stop_arg;
 	/*  enable interrupts */
 	spin_lock_irqsave(&dev->spinlock, flags);
 	devpriv->adc_fifo_bits |= INTE;
@@ -1134,32 +1091,34 @@ static int cb_pcidas_cancel(struct comedi_device *dev,
 	return 0;
 }
 
+static void cb_pcidas_ao_load_fifo(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   unsigned int nsamples)
+{
+	struct cb_pcidas_private *devpriv = dev->private;
+	unsigned int nbytes;
+
+	nsamples = comedi_nsamples_left(s, nsamples);
+	nbytes = comedi_buf_read_samples(s, devpriv->ao_buffer, nsamples);
+
+	nsamples = comedi_bytes_to_samples(s, nbytes);
+	outsw(devpriv->ao_registers + DACDATA, devpriv->ao_buffer, nsamples);
+}
+
 static int cb_pcidas_ao_inttrig(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				unsigned int trig_num)
 {
 	const struct cb_pcidas_board *thisboard = dev->board_ptr;
 	struct cb_pcidas_private *devpriv = dev->private;
-	unsigned int num_bytes, num_points = thisboard->fifo_size;
 	struct comedi_async *async = s->async;
-	struct comedi_cmd *cmd = &s->async->cmd;
+	struct comedi_cmd *cmd = &async->cmd;
 	unsigned long flags;
 
 	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
-	/*  load up fifo */
-	if (cmd->stop_src == TRIG_COUNT && devpriv->ao_count < num_points)
-		num_points = devpriv->ao_count;
-
-	num_bytes = cfc_read_array_from_buffer(s, devpriv->ao_buffer,
-					       num_points * sizeof(short));
-	num_points = num_bytes / sizeof(short);
-
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->ao_count -= num_points;
-	/*  write data to board's fifo */
-	outsw(devpriv->ao_registers + DACDATA, devpriv->ao_buffer, num_bytes);
+	cb_pcidas_ao_load_fifo(dev, s, thisboard->fifo_size);
 
 	/*  enable dac half-full and empty interrupts */
 	spin_lock_irqsave(&dev->spinlock, flags);
@@ -1224,9 +1183,6 @@ static int cb_pcidas_ao_cmd(struct comedi_device *dev,
 	if (cmd->scan_begin_src == TRIG_TIMER)
 		cb_pcidas_ao_load_counters(dev);
 
-	/*  set number of conversions */
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->ao_count = cmd->chanlist_len * cmd->stop_arg;
 	/*  set pacer source */
 	spin_lock_irqsave(&dev->spinlock, flags);
 	switch (cmd->scan_begin_src) {
@@ -1275,8 +1231,6 @@ static void handle_ao_interrupt(struct comedi_device *dev, unsigned int status)
 	struct comedi_subdevice *s = dev->write_subdev;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
-	unsigned int half_fifo = thisboard->fifo_size / 2;
-	unsigned int num_points;
 	unsigned long flags;
 
 	if (status & DAEMI) {
@@ -1286,32 +1240,17 @@ static void handle_ao_interrupt(struct comedi_device *dev, unsigned int status)
 		     devpriv->control_status + INT_ADCFIFO);
 		spin_unlock_irqrestore(&dev->spinlock, flags);
 		if (inw(devpriv->ao_registers + DAC_CSR) & DAC_EMPTY) {
-			if (cmd->stop_src == TRIG_NONE ||
-			    (cmd->stop_src == TRIG_COUNT
-			     && devpriv->ao_count)) {
+			if (cmd->stop_src == TRIG_COUNT &&
+			    async->scans_done >= cmd->stop_arg) {
+				async->events |= COMEDI_CB_EOA;
+			} else {
 				dev_err(dev->class_dev, "dac fifo underflow\n");
 				async->events |= COMEDI_CB_ERROR;
 			}
-			async->events |= COMEDI_CB_EOA;
 		}
 	} else if (status & DAHFI) {
-		unsigned int num_bytes;
+		cb_pcidas_ao_load_fifo(dev, s, thisboard->fifo_size / 2);
 
-		/*  figure out how many points we are writing to fifo */
-		num_points = half_fifo;
-		if (cmd->stop_src == TRIG_COUNT &&
-		    devpriv->ao_count < num_points)
-			num_points = devpriv->ao_count;
-		num_bytes =
-		    cfc_read_array_from_buffer(s, devpriv->ao_buffer,
-					       num_points * sizeof(short));
-		num_points = num_bytes / sizeof(short);
-
-		if (cmd->stop_src == TRIG_COUNT)
-			devpriv->ao_count -= num_points;
-		/*  write data to board's fifo */
-		outsw(devpriv->ao_registers + DACDATA, devpriv->ao_buffer,
-		      num_points);
 		/*  clear half-full interrupt latch */
 		spin_lock_irqsave(&dev->spinlock, flags);
 		outw(devpriv->adc_fifo_bits | DAHFI,
@@ -1319,7 +1258,7 @@ static void handle_ao_interrupt(struct comedi_device *dev, unsigned int status)
 		spin_unlock_irqrestore(&dev->spinlock, flags);
 	}
 
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 }
 
 static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
@@ -1362,18 +1301,15 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 	/*  if fifo half-full */
 	if (status & ADHFI) {
 		/*  read data */
-		num_samples = half_fifo;
-		if (cmd->stop_src == TRIG_COUNT &&
-		    num_samples > devpriv->count) {
-			num_samples = devpriv->count;
-		}
+		num_samples = comedi_nsamples_left(s, half_fifo);
 		insw(devpriv->adc_fifo + ADCDATA, devpriv->ai_buffer,
 		     num_samples);
-		cfc_write_array_to_buffer(s, devpriv->ai_buffer,
-					  num_samples * sizeof(short));
-		devpriv->count -= num_samples;
-		if (cmd->stop_src == TRIG_COUNT && devpriv->count == 0)
+		comedi_buf_write_samples(s, devpriv->ai_buffer, num_samples);
+
+		if (cmd->stop_src == TRIG_COUNT &&
+		    async->scans_done >= cmd->stop_arg)
 			async->events |= COMEDI_CB_EOA;
+
 		/*  clear half-full interrupt latch */
 		spin_lock_irqsave(&dev->spinlock, flags);
 		outw(devpriv->adc_fifo_bits | INT,
@@ -1382,14 +1318,17 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 		/*  else if fifo not empty */
 	} else if (status & (ADNEI | EOBI)) {
 		for (i = 0; i < timeout; i++) {
+			unsigned short val;
+
 			/*  break if fifo is empty */
 			if ((ADNE & inw(devpriv->control_status +
 					INT_ADCFIFO)) == 0)
 				break;
-			cfc_write_to_buffer(s, inw(devpriv->adc_fifo));
+			val = inw(devpriv->adc_fifo);
+			comedi_buf_write_samples(s, &val, 1);
+
 			if (cmd->stop_src == TRIG_COUNT &&
-			    --devpriv->count == 0) {
-				/* end of acquisition */
+			    async->scans_done >= cmd->stop_arg) {
 				async->events |= COMEDI_CB_EOA;
 				break;
 			}
@@ -1419,7 +1358,7 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 		async->events |= COMEDI_CB_EOA | COMEDI_CB_ERROR;
 	}
 
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 
 	return IRQ_HANDLED;
 }
@@ -1503,7 +1442,6 @@ static int cb_pcidas_auto_attach(struct comedi_device *dev,
 		s->range_table = &cb_pcidas_ao_ranges;
 		/* default to no fifo (*insn_write) */
 		s->insn_write = cb_pcidas_ao_nofifo_winsn;
-		s->insn_read = comedi_readback_insn_read;
 
 		ret = comedi_alloc_subdev_readback(s);
 		if (ret)
@@ -1542,10 +1480,16 @@ static int cb_pcidas_auto_attach(struct comedi_device *dev,
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
 	s->n_chan = NUM_CHANNELS_8800;
 	s->maxdata = 0xff;
-	s->insn_read = caldac_read_insn;
-	s->insn_write = caldac_write_insn;
-	for (i = 0; i < s->n_chan; i++)
+	s->insn_write = cb_pcidas_caldac_insn_write;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < s->n_chan; i++) {
 		caldac_8800_write(dev, i, s->maxdata / 2);
+		s->readback[i] = s->maxdata / 2;
+	}
 
 	/*  trim potentiometer */
 	s = &dev->subdevices[5];
@@ -1558,10 +1502,16 @@ static int cb_pcidas_auto_attach(struct comedi_device *dev,
 		s->n_chan = NUM_CHANNELS_8402;
 		s->maxdata = 0xff;
 	}
-	s->insn_read = trimpot_read_insn;
-	s->insn_write = trimpot_write_insn;
-	for (i = 0; i < s->n_chan; i++)
+	s->insn_write = cb_pcidas_trimpot_insn_write;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < s->n_chan; i++) {
 		cb_pcidas_trimpot_write(dev, i, s->maxdata / 2);
+		s->readback[i] = s->maxdata / 2;
+	}
 
 	/*  dac08 caldac */
 	s = &dev->subdevices[6];
@@ -1569,10 +1519,17 @@ static int cb_pcidas_auto_attach(struct comedi_device *dev,
 		s->type = COMEDI_SUBD_CALIB;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
 		s->n_chan = NUM_CHANNELS_DAC08;
-		s->insn_read = dac08_read_insn;
-		s->insn_write = dac08_write_insn;
 		s->maxdata = 0xff;
-		dac08_write(dev, s->maxdata / 2);
+		s->insn_write = cb_pcidas_dac08_insn_write;
+
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
+
+		for (i = 0; i < s->n_chan; i++) {
+			dac08_write(dev, s->maxdata / 2);
+			s->readback[i] = s->maxdata / 2;
+		}
 	} else
 		s->type = COMEDI_SUBD_UNUSED;
 
diff --git a/drivers/staging/comedi/drivers/cb_pcidas64.c b/drivers/staging/comedi/drivers/cb_pcidas64.c
index 3b6bffc66918..eddb7ace43df 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas64.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas64.c
@@ -1065,8 +1065,6 @@ struct pcidas64_private {
 	/*  local address (used by dma controller) */
 	uint32_t local0_iobase;
 	uint32_t local1_iobase;
-	/*  number of analog input samples remaining */
-	unsigned int ai_count;
 	/*  dma buffers for analog input */
 	uint16_t *ai_buffer[MAX_AI_DMA_RING_COUNT];
 	/*  physical addresses of ai dma buffers */
@@ -1087,8 +1085,6 @@ struct pcidas64_private {
 	dma_addr_t ao_dma_desc_bus_addr;
 	/*  keeps track of buffer where the next ao sample should go */
 	unsigned int ao_dma_index;
-	/*  number of analog output samples remaining */
-	unsigned long ao_count;
 	unsigned int hw_revision;	/*  stc chip hardware revision number */
 	/*  last bits sent to INTR_ENABLE_REG register */
 	unsigned int intr_enable_bits;
@@ -1109,9 +1105,6 @@ struct pcidas64_private {
 	uint8_t i2c_cal_range_bits;
 	/*  configure digital triggers to trigger on falling edge */
 	unsigned int ext_trig_falling;
-	/*  states of various devices stored to enable read-back */
-	unsigned int ad8402_state[2];
-	unsigned int caldac_state[8];
 	short ai_cmd_running;
 	unsigned int ai_fifo_segment_length;
 	struct ext_clock_info ext_clock;
@@ -2199,10 +2192,6 @@ static void setup_sample_counters(struct comedi_device *dev,
 {
 	struct pcidas64_private *devpriv = dev->private;
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		/*  set software count */
-		devpriv->ai_count = cmd->stop_arg * cmd->chanlist_len;
-	}
 	/*  load hardware conversion counter */
 	if (use_hw_sample_counter(cmd)) {
 		writew(cmd->stop_arg & 0xffff,
@@ -2642,8 +2631,6 @@ static void pio_drain_ai_fifo_16(struct comedi_device *dev)
 {
 	struct pcidas64_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
-	struct comedi_async *async = s->async;
-	struct comedi_cmd *cmd = &async->cmd;
 	unsigned int i;
 	uint16_t prepost_bits;
 	int read_segment, read_index, write_segment, write_index;
@@ -2672,26 +2659,21 @@ static void pio_drain_ai_fifo_16(struct comedi_device *dev)
 				devpriv->ai_fifo_segment_length - read_index;
 		else
 			num_samples = write_index - read_index;
-
-		if (cmd->stop_src == TRIG_COUNT) {
-			if (devpriv->ai_count == 0)
-				break;
-			if (num_samples > devpriv->ai_count)
-				num_samples = devpriv->ai_count;
-
-			devpriv->ai_count -= num_samples;
-		}
-
 		if (num_samples < 0) {
 			dev_err(dev->class_dev,
 				"cb_pcidas64: bug! num_samples < 0\n");
 			break;
 		}
 
+		num_samples = comedi_nsamples_left(s, num_samples);
+		if (num_samples == 0)
+			break;
+
 		for (i = 0; i < num_samples; i++) {
-			cfc_write_to_buffer(s,
-					    readw(devpriv->main_iobase +
-						  ADC_FIFO_REG));
+			unsigned short val;
+
+			val = readw(devpriv->main_iobase + ADC_FIFO_REG);
+			comedi_buf_write_samples(s, &val, 1);
 		}
 
 	} while (read_segment != write_segment);
@@ -2706,33 +2688,30 @@ static void pio_drain_ai_fifo_32(struct comedi_device *dev)
 {
 	struct pcidas64_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
-	struct comedi_async *async = s->async;
-	struct comedi_cmd *cmd = &async->cmd;
+	unsigned int nsamples;
 	unsigned int i;
-	unsigned int max_transfer = 100000;
 	uint32_t fifo_data;
 	int write_code =
 		readw(devpriv->main_iobase + ADC_WRITE_PNTR_REG) & 0x7fff;
 	int read_code =
 		readw(devpriv->main_iobase + ADC_READ_PNTR_REG) & 0x7fff;
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		if (max_transfer > devpriv->ai_count)
-			max_transfer = devpriv->ai_count;
+	nsamples = comedi_nsamples_left(s, 100000);
+	for (i = 0; read_code != write_code && i < nsamples;) {
+		unsigned short val;
 
-	}
-	for (i = 0; read_code != write_code && i < max_transfer;) {
 		fifo_data = readl(dev->mmio + ADC_FIFO_REG);
-		cfc_write_to_buffer(s, fifo_data & 0xffff);
+		val = fifo_data & 0xffff;
+		comedi_buf_write_samples(s, &val, 1);
 		i++;
-		if (i < max_transfer) {
-			cfc_write_to_buffer(s, (fifo_data >> 16) & 0xffff);
+		if (i < nsamples) {
+			val = (fifo_data >> 16) & 0xffff;
+			comedi_buf_write_samples(s, &val, 1);
 			i++;
 		}
 		read_code = readw(devpriv->main_iobase + ADC_READ_PNTR_REG) &
 			    0x7fff;
 	}
-	devpriv->ai_count -= i;
 }
 
 /* empty fifo */
@@ -2750,8 +2729,7 @@ static void drain_dma_buffers(struct comedi_device *dev, unsigned int channel)
 {
 	const struct pcidas64_board *thisboard = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
-	struct comedi_async *async = dev->read_subdev->async;
-	struct comedi_cmd *cmd = &async->cmd;
+	struct comedi_subdevice *s = dev->read_subdev;
 	uint32_t next_transfer_addr;
 	int j;
 	int num_samples = 0;
@@ -2772,16 +2750,10 @@ static void drain_dma_buffers(struct comedi_device *dev, unsigned int channel)
 	      devpriv->ai_buffer_bus_addr[devpriv->ai_dma_index] +
 	      DMA_BUFFER_SIZE) && j < ai_dma_ring_count(thisboard); j++) {
 		/*  transfer data from dma buffer to comedi buffer */
-		num_samples = dma_transfer_size(dev);
-		if (cmd->stop_src == TRIG_COUNT) {
-			if (num_samples > devpriv->ai_count)
-				num_samples = devpriv->ai_count;
-			devpriv->ai_count -= num_samples;
-		}
-		cfc_write_array_to_buffer(dev->read_subdev,
-					  devpriv->ai_buffer[devpriv->
-							     ai_dma_index],
-					  num_samples * sizeof(uint16_t));
+		num_samples = comedi_nsamples_left(s, dma_transfer_size(dev));
+		comedi_buf_write_samples(s,
+				devpriv->ai_buffer[devpriv->ai_dma_index],
+				num_samples);
 		devpriv->ai_dma_index = (devpriv->ai_dma_index + 1) %
 					ai_dma_ring_count(thisboard);
 	}
@@ -2831,12 +2803,12 @@ static void handle_ai_interrupt(struct comedi_device *dev,
 			spin_unlock_irqrestore(&dev->spinlock, flags);
 	}
 	/*  if we are have all the data, then quit */
-	if ((cmd->stop_src == TRIG_COUNT && (int)devpriv->ai_count <= 0) ||
-	    (cmd->stop_src == TRIG_EXT && (status & ADC_STOP_BIT))) {
+	if ((cmd->stop_src == TRIG_COUNT &&
+	     async->scans_done >= cmd->stop_arg) ||
+	    (cmd->stop_src == TRIG_EXT && (status & ADC_STOP_BIT)))
 		async->events |= COMEDI_CB_EOA;
-	}
 
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 }
 
 static inline unsigned int prev_ao_dma_index(struct comedi_device *dev)
@@ -2871,22 +2843,6 @@ static int last_ao_dma_load_completed(struct comedi_device *dev)
 	return 1;
 }
 
-static int ao_stopped_by_error(struct comedi_device *dev,
-			       const struct comedi_cmd *cmd)
-{
-	struct pcidas64_private *devpriv = dev->private;
-
-	if (cmd->stop_src == TRIG_NONE)
-		return 1;
-	if (cmd->stop_src == TRIG_COUNT) {
-		if (devpriv->ao_count)
-			return 1;
-		if (last_ao_dma_load_completed(dev) == 0)
-			return 1;
-	}
-	return 0;
-}
-
 static inline int ao_dma_needs_restart(struct comedi_device *dev,
 				       unsigned short dma_status)
 {
@@ -2912,32 +2868,39 @@ static void restart_ao_dma(struct comedi_device *dev)
 	dma_start_sync(dev, 0);
 }
 
+static unsigned int cb_pcidas64_ao_fill_buffer(struct comedi_device *dev,
+					       struct comedi_subdevice *s,
+					       unsigned short *dest,
+					       unsigned int max_bytes)
+{
+	unsigned int nsamples = comedi_bytes_to_samples(s, max_bytes);
+	unsigned int actual_bytes;
+
+	nsamples = comedi_nsamples_left(s, nsamples);
+	actual_bytes = comedi_buf_read_samples(s, dest, nsamples);
+
+	return comedi_bytes_to_samples(s, actual_bytes);
+}
+
 static unsigned int load_ao_dma_buffer(struct comedi_device *dev,
 				       const struct comedi_cmd *cmd)
 {
 	struct pcidas64_private *devpriv = dev->private;
-	unsigned int num_bytes, buffer_index, prev_buffer_index;
+	struct comedi_subdevice *s = dev->write_subdev;
+	unsigned int buffer_index = devpriv->ao_dma_index;
+	unsigned int prev_buffer_index = prev_ao_dma_index(dev);
+	unsigned int nsamples;
+	unsigned int nbytes;
 	unsigned int next_bits;
 
-	buffer_index = devpriv->ao_dma_index;
-	prev_buffer_index = prev_ao_dma_index(dev);
-
-	num_bytes = comedi_buf_read_n_available(dev->write_subdev);
-	if (num_bytes > DMA_BUFFER_SIZE)
-		num_bytes = DMA_BUFFER_SIZE;
-	if (cmd->stop_src == TRIG_COUNT && num_bytes > devpriv->ao_count)
-		num_bytes = devpriv->ao_count;
-	num_bytes -= num_bytes % bytes_in_sample;
-
-	if (num_bytes == 0)
+	nsamples = cb_pcidas64_ao_fill_buffer(dev, s,
+					      devpriv->ao_buffer[buffer_index],
+					      DMA_BUFFER_SIZE);
+	if (nsamples == 0)
 		return 0;
 
-	num_bytes = cfc_read_array_from_buffer(dev->write_subdev,
-					       devpriv->
-					       ao_buffer[buffer_index],
-					       num_bytes);
-	devpriv->ao_dma_desc[buffer_index].transfer_size =
-		cpu_to_le32(num_bytes);
+	nbytes = comedi_samples_to_bytes(s, nsamples);
+	devpriv->ao_dma_desc[buffer_index].transfer_size = cpu_to_le32(nbytes);
 	/* set end of chain bit so we catch underruns */
 	next_bits = le32_to_cpu(devpriv->ao_dma_desc[buffer_index].next);
 	next_bits |= PLX_END_OF_CHAIN_BIT;
@@ -2949,9 +2912,8 @@ static unsigned int load_ao_dma_buffer(struct comedi_device *dev,
 	devpriv->ao_dma_desc[prev_buffer_index].next = cpu_to_le32(next_bits);
 
 	devpriv->ao_dma_index = (buffer_index + 1) % AO_DMA_RING_COUNT;
-	devpriv->ao_count -= num_bytes;
 
-	return num_bytes;
+	return nbytes;
 }
 
 static void load_ao_dma(struct comedi_device *dev, const struct comedi_cmd *cmd)
@@ -3016,11 +2978,14 @@ static void handle_ao_interrupt(struct comedi_device *dev,
 	}
 
 	if ((status & DAC_DONE_BIT)) {
-		async->events |= COMEDI_CB_EOA;
-		if (ao_stopped_by_error(dev, cmd))
+		if ((cmd->stop_src == TRIG_COUNT &&
+		     async->scans_done >= cmd->stop_arg) ||
+		    last_ao_dma_load_completed(dev))
+			async->events |= COMEDI_CB_EOA;
+		else
 			async->events |= COMEDI_CB_ERROR;
 	}
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 }
 
 static irqreturn_t handle_interrupt(int irq, void *d)
@@ -3191,7 +3156,9 @@ static void set_dac_interval_regs(struct comedi_device *dev,
 static int prep_ao_dma(struct comedi_device *dev, const struct comedi_cmd *cmd)
 {
 	struct pcidas64_private *devpriv = dev->private;
-	unsigned int num_bytes;
+	struct comedi_subdevice *s = dev->write_subdev;
+	unsigned int nsamples;
+	unsigned int nbytes;
 	int i;
 
 	/* clear queue pointer too, since external queue has
@@ -3199,22 +3166,23 @@ static int prep_ao_dma(struct comedi_device *dev, const struct comedi_cmd *cmd)
 	writew(0, devpriv->main_iobase + ADC_QUEUE_CLEAR_REG);
 	writew(0, devpriv->main_iobase + DAC_BUFFER_CLEAR_REG);
 
-	num_bytes = (DAC_FIFO_SIZE / 2) * bytes_in_sample;
-	if (cmd->stop_src == TRIG_COUNT &&
-	    num_bytes / bytes_in_sample > devpriv->ao_count)
-		num_bytes = devpriv->ao_count * bytes_in_sample;
-	num_bytes = cfc_read_array_from_buffer(dev->write_subdev,
-					       devpriv->ao_bounce_buffer,
-					       num_bytes);
-	for (i = 0; i < num_bytes / bytes_in_sample; i++) {
+	nsamples = cb_pcidas64_ao_fill_buffer(dev, s,
+					      devpriv->ao_bounce_buffer,
+					      DAC_FIFO_SIZE);
+	if (nsamples == 0)
+		return -1;
+
+	for (i = 0; i < nsamples; i++) {
 		writew(devpriv->ao_bounce_buffer[i],
 		       devpriv->main_iobase + DAC_FIFO_REG);
 	}
-	devpriv->ao_count -= num_bytes / bytes_in_sample;
-	if (cmd->stop_src == TRIG_COUNT && devpriv->ao_count == 0)
+
+	if (cmd->stop_src == TRIG_COUNT &&
+	    s->async->scans_done >= cmd->stop_arg)
 		return 0;
-	num_bytes = load_ao_dma_buffer(dev, cmd);
-	if (num_bytes == 0)
+
+	nbytes = load_ao_dma_buffer(dev, cmd);
+	if (nbytes == 0)
 		return -1;
 	load_ao_dma(dev, cmd);
 
@@ -3275,7 +3243,6 @@ static int ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	writew(0x0, devpriv->main_iobase + DAC_CONTROL0_REG);
 
 	devpriv->ao_dma_index = 0;
-	devpriv->ao_count = cmd->stop_arg * cmd->chanlist_len;
 
 	set_dac_select_reg(dev, cmd);
 	set_dac_interval_regs(dev, cmd);
@@ -3580,9 +3547,6 @@ static void caldac_write(struct comedi_device *dev, unsigned int channel,
 			 unsigned int value)
 {
 	const struct pcidas64_board *thisboard = dev->board_ptr;
-	struct pcidas64_private *devpriv = dev->private;
-
-	devpriv->caldac_state[channel] = value;
 
 	switch (thisboard->layout) {
 	case LAYOUT_60XX:
@@ -3597,33 +3561,27 @@ static void caldac_write(struct comedi_device *dev, unsigned int channel,
 	}
 }
 
-static int calib_write_insn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
+static int cb_pcidas64_calib_insn_write(struct comedi_device *dev,
+					struct comedi_subdevice *s,
+					struct comedi_insn *insn,
+					unsigned int *data)
 {
-	struct pcidas64_private *devpriv = dev->private;
-	int channel = CR_CHAN(insn->chanspec);
-
-	/* return immediately if setting hasn't changed, since
-	 * programming these things is slow */
-	if (devpriv->caldac_state[channel] == data[0])
-		return 1;
-
-	caldac_write(dev, channel, data[0]);
-
-	return 1;
-}
+	unsigned int chan = CR_CHAN(insn->chanspec);
 
-static int calib_read_insn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data)
-{
-	struct pcidas64_private *devpriv = dev->private;
-	unsigned int channel = CR_CHAN(insn->chanspec);
+	/*
+	 * Programming the calib device is slow. Only write the
+	 * last data value if the value has changed.
+	 */
+	if (insn->n) {
+		unsigned int val = data[insn->n - 1];
 
-	data[0] = devpriv->caldac_state[channel];
+		if (s->readback[chan] != val) {
+			caldac_write(dev, chan, val);
+			s->readback[chan] = val;
+		}
+	}
 
-	return 1;
+	return insn->n;
 }
 
 static void ad8402_write(struct comedi_device *dev, unsigned int channel,
@@ -3635,8 +3593,6 @@ static void ad8402_write(struct comedi_device *dev, unsigned int channel,
 	unsigned int bitstream = ((channel & 0x3) << 8) | (value & 0xff);
 	static const int ad8402_udelay = 1;
 
-	devpriv->ad8402_state[channel] = value;
-
 	register_bits = SELECT_8402_64XX_BIT;
 	udelay(ad8402_udelay);
 	writew(register_bits, devpriv->main_iobase + CALIBRATION_REG);
@@ -3658,35 +3614,27 @@ static void ad8402_write(struct comedi_device *dev, unsigned int channel,
 }
 
 /* for pci-das6402/16, channel 0 is analog input gain and channel 1 is offset */
-static int ad8402_write_insn(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_insn *insn, unsigned int *data)
+static int cb_pcidas64_ad8402_insn_write(struct comedi_device *dev,
+					 struct comedi_subdevice *s,
+					 struct comedi_insn *insn,
+					 unsigned int *data)
 {
-	struct pcidas64_private *devpriv = dev->private;
-	int channel = CR_CHAN(insn->chanspec);
-
-	/* return immediately if setting hasn't changed, since
-	 * programming these things is slow */
-	if (devpriv->ad8402_state[channel] == data[0])
-		return 1;
-
-	devpriv->ad8402_state[channel] = data[0];
-
-	ad8402_write(dev, channel, data[0]);
-
-	return 1;
-}
+	unsigned int chan = CR_CHAN(insn->chanspec);
 
-static int ad8402_read_insn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
-{
-	struct pcidas64_private *devpriv = dev->private;
-	unsigned int channel = CR_CHAN(insn->chanspec);
+	/*
+	 * Programming the calib device is slow. Only write the
+	 * last data value if the value has changed.
+	 */
+	if (insn->n) {
+		unsigned int val = data[insn->n - 1];
 
-	data[0] = devpriv->ad8402_state[channel];
+		if (s->readback[chan] != val) {
+			ad8402_write(dev, chan, val);
+			s->readback[chan] = val;
+		}
+	}
 
-	return 1;
+	return insn->n;
 }
 
 static uint16_t read_eeprom(struct comedi_device *dev, uint8_t address)
@@ -3816,7 +3764,6 @@ static int setup_subdevices(struct comedi_device *dev)
 		s->maxdata = (1 << thisboard->ao_bits) - 1;
 		s->range_table = thisboard->ao_range_table;
 		s->insn_write = ao_winsn;
-		s->insn_read = comedi_readback_insn_read;
 
 		ret = comedi_alloc_subdev_readback(s);
 		if (ret)
@@ -3849,7 +3796,7 @@ static int setup_subdevices(struct comedi_device *dev)
 	if (thisboard->layout == LAYOUT_64XX) {
 		s = &dev->subdevices[3];
 		s->type = COMEDI_SUBD_DO;
-		s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+		s->subdev_flags = SDF_WRITABLE;
 		s->n_chan = 4;
 		s->maxdata = 1;
 		s->range_table = &range_digital;
@@ -3895,10 +3842,16 @@ static int setup_subdevices(struct comedi_device *dev)
 		s->maxdata = 0xfff;
 	else
 		s->maxdata = 0xff;
-	s->insn_read = calib_read_insn;
-	s->insn_write = calib_write_insn;
-	for (i = 0; i < s->n_chan; i++)
+	s->insn_write = cb_pcidas64_calib_insn_write;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < s->n_chan; i++) {
 		caldac_write(dev, i, s->maxdata / 2);
+		s->readback[i] = s->maxdata / 2;
+	}
 
 	/*  2 channel ad8402 potentiometer */
 	s = &dev->subdevices[7];
@@ -3906,11 +3859,17 @@ static int setup_subdevices(struct comedi_device *dev)
 		s->type = COMEDI_SUBD_CALIB;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
 		s->n_chan = 2;
-		s->insn_read = ad8402_read_insn;
-		s->insn_write = ad8402_write_insn;
 		s->maxdata = 0xff;
-		for (i = 0; i < s->n_chan; i++)
+		s->insn_write = cb_pcidas64_ad8402_insn_write;
+
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
+
+		for (i = 0; i < s->n_chan; i++) {
 			ad8402_write(dev, i, s->maxdata / 2);
+			s->readback[i] = s->maxdata / 2;
+		}
 	} else
 		s->type = COMEDI_SUBD_UNUSED;
 
diff --git a/drivers/staging/comedi/drivers/cb_pcimdas.c b/drivers/staging/comedi/drivers/cb_pcimdas.c
index fe4d2544f3dc..70dd2c9eecdb 100644
--- a/drivers/staging/comedi/drivers/cb_pcimdas.c
+++ b/drivers/staging/comedi/drivers/cb_pcimdas.c
@@ -1,40 +1,45 @@
 /*
-    comedi/drivers/cb_pcimdas.c
-    Comedi driver for Computer Boards PCIM-DAS1602/16
-
-    COMEDI - Linux Control and Measurement Device Interface
-    Copyright (C) 2000 David A. Schleef <ds@schleef.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.
-*/
+ * comedi/drivers/cb_pcimdas.c
+ * Comedi driver for Computer Boards PCIM-DAS1602/16 and PCIe-DAS1602/16
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 2000 David A. Schleef <ds@schleef.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.
+ */
+
 /*
-Driver: cb_pcimdas
-Description: Measurement Computing PCI Migration series boards
-Devices: [ComputerBoards] PCIM-DAS1602/16 (cb_pcimdas)
-Author: Richard Bytheway
-Updated: Wed, 13 Nov 2002 12:34:56 +0000
-Status: experimental
-
-Written to support the PCIM-DAS1602/16 on a 2.4 series kernel.
-
-Configuration Options:
-    [0] - PCI bus number
-    [1] - PCI slot number
-
-Developed from cb_pcidas and skel by Richard Bytheway (mocelet@sucs.org).
-Only supports DIO, AO and simple AI in it's present form.
-No interrupts, multi channel or FIFO AI,
-although the card looks like it could support this.
-See http://www.mccdaq.com/PDFs/Manuals/pcim-das1602-16.pdf for more details.
-*/
+ * Driver: cb_pcimdas
+ * Description: Measurement Computing PCI Migration series boards
+ * Devices: [ComputerBoards] PCIM-DAS1602/16 (cb_pcimdas), PCIe-DAS1602/16
+ * Author: Richard Bytheway
+ * Updated: Mon, 13 Oct 2014 11:57:39 +0000
+ * Status: experimental
+ *
+ * Written to support the PCIM-DAS1602/16 and PCIe-DAS1602/16.
+ *
+ * Configuration Options:
+ *   none
+ *
+ * Manual configuration of PCI(e) cards is not supported; they are configured
+ * automatically.
+ *
+ * Developed from cb_pcidas and skel by Richard Bytheway (mocelet@sucs.org).
+ * Only supports DIO, AO and simple AI in it's present form.
+ * No interrupts, multi channel or FIFO AI,
+ * although the card looks like it could support this.
+ *
+ * http://www.mccdaq.com/PDFs/Manuals/pcim-das1602-16.pdf
+ * http://www.mccdaq.com/PDFs/Manuals/pcie-das1602-16.pdf
+ */
 
 #include <linux/module.h>
 #include <linux/pci.h>
@@ -45,7 +50,7 @@ See http://www.mccdaq.com/PDFs/Manuals/pcim-das1602-16.pdf for more details.
 #include "plx9052.h"
 #include "8255.h"
 
-/* Registers for the PCIM-DAS1602/16 */
+/* Registers for the PCIM-DAS1602/16 and PCIe-DAS1602/16 */
 
 /* DAC Offsets */
 #define ADC_TRIG 0
@@ -221,7 +226,6 @@ static int cb_pcimdas_auto_attach(struct comedi_device *dev,
 	/* ranges are hardware settable, but not software readable. */
 	s->range_table = &range_unknown;
 	s->insn_write = cb_pcimdas_ao_insn_write;
-	s->insn_read = comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
@@ -251,7 +255,8 @@ static int cb_pcimdas_pci_probe(struct pci_dev *dev,
 }
 
 static const struct pci_device_id cb_pcimdas_pci_table[] = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0056) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0056) },	/* PCIM-DAS1602/16 */
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0115) },	/* PCIe-DAS1602/16 */
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, cb_pcimdas_pci_table);
@@ -265,5 +270,5 @@ static struct pci_driver cb_pcimdas_pci_driver = {
 module_comedi_pci_driver(cb_pcimdas_driver, cb_pcimdas_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("Comedi driver for PCIM-DAS1602/16 and PCIe-DAS1602/16");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/comedi_bond.c b/drivers/staging/comedi/drivers/comedi_bond.c
index 8450c99af8b0..85b2f4ab1ba4 100644
--- a/drivers/staging/comedi/drivers/comedi_bond.c
+++ b/drivers/staging/comedi/drivers/comedi_bond.c
@@ -61,8 +61,7 @@ struct bonded_device {
 };
 
 struct comedi_bond_private {
-# define MAX_BOARD_NAME 256
-	char name[MAX_BOARD_NAME];
+	char name[256];
 	struct bonded_device **devs;
 	unsigned ndevs;
 	unsigned nchans;
@@ -262,12 +261,10 @@ static int do_dev_config(struct comedi_device *dev, struct comedi_devconfig *it)
 			{
 				/* Append dev:subdev to devpriv->name */
 				char buf[20];
-				int left =
-				    MAX_BOARD_NAME - strlen(devpriv->name) - 1;
 				snprintf(buf, sizeof(buf), "%u:%u ",
 					 bdev->minor, bdev->subdev);
-				buf[sizeof(buf) - 1] = 0;
-				strncat(devpriv->name, buf, left);
+				strlcat(devpriv->name, buf,
+					sizeof(devpriv->name));
 			}
 
 		}
diff --git a/drivers/staging/comedi/drivers/comedi_fc.h b/drivers/staging/comedi/drivers/comedi_fc.h
index ce2835972507..756be931c1a4 100644
--- a/drivers/staging/comedi/drivers/comedi_fc.h
+++ b/drivers/staging/comedi/drivers/comedi_fc.h
@@ -23,49 +23,6 @@
 
 #include "../comedidev.h"
 
-static inline unsigned int cfc_bytes_per_scan(struct comedi_subdevice *s)
-{
-	return comedi_bytes_per_scan(s);
-}
-
-static inline void cfc_inc_scan_progress(struct comedi_subdevice *s,
-					 unsigned int num_bytes)
-{
-	comedi_inc_scan_progress(s, num_bytes);
-}
-
-static inline unsigned int cfc_write_array_to_buffer(struct comedi_subdevice *s,
-						     const void *data,
-						     unsigned int num_bytes)
-{
-	return comedi_write_array_to_buffer(s, data, num_bytes);
-}
-
-static inline unsigned int cfc_write_to_buffer(struct comedi_subdevice *s,
-					       unsigned short data)
-{
-	return comedi_write_array_to_buffer(s, &data, sizeof(data));
-};
-
-static inline unsigned int cfc_write_long_to_buffer(struct comedi_subdevice *s,
-						    unsigned int data)
-{
-	return comedi_write_array_to_buffer(s, &data, sizeof(data));
-};
-
-static inline unsigned int
-cfc_read_array_from_buffer(struct comedi_subdevice *s, void *data,
-			   unsigned int num_bytes)
-{
-	return comedi_read_array_from_buffer(s, data, num_bytes);
-}
-
-static inline unsigned int cfc_handle_events(struct comedi_device *dev,
-					     struct comedi_subdevice *s)
-{
-	return comedi_handle_events(dev, s);
-}
-
 /**
  * cfc_check_trigger_src() - trivially validate a comedi_cmd trigger source
  * @src: pointer to the trigger source to validate
diff --git a/drivers/staging/comedi/drivers/comedi_parport.c b/drivers/staging/comedi/drivers/comedi_parport.c
index bf002988192d..3bac903c8627 100644
--- a/drivers/staging/comedi/drivers/comedi_parport.c
+++ b/drivers/staging/comedi/drivers/comedi_parport.c
@@ -225,10 +225,9 @@ static irqreturn_t parport_interrupt(int irq, void *d)
 	if (!(ctrl & PARPORT_CTRL_IRQ_ENA))
 		return IRQ_NONE;
 
-	comedi_buf_put(s, 0);
-	s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
+	comedi_buf_write_samples(s, &s->state, 1);
+	comedi_handle_events(dev, s);
 
-	comedi_event(dev, s);
 	return IRQ_HANDLED;
 }
 
diff --git a/drivers/staging/comedi/drivers/comedi_test.c b/drivers/staging/comedi/drivers/comedi_test.c
index 00c03df72523..e56525a1c8f3 100644
--- a/drivers/staging/comedi/drivers/comedi_test.c
+++ b/drivers/staging/comedi/drivers/comedi_test.c
@@ -52,18 +52,23 @@ zero volts).
 
 #include "comedi_fc.h"
 #include <linux/timer.h>
+#include <linux/ktime.h>
 
 #define N_CHANS 8
 
+enum waveform_state_bits {
+	WAVEFORM_AI_RUNNING = 0
+};
+
 /* Data unique to this driver */
 struct waveform_private {
 	struct timer_list timer;
-	struct timeval last;		/* time last timer interrupt occurred */
+	ktime_t last;	/* time last timer interrupt occurred */
 	unsigned int uvolt_amplitude;	/* waveform amplitude in microvolts */
 	unsigned long usec_period;	/* waveform period in microseconds */
 	unsigned long usec_current;	/* current time (mod waveform period) */
 	unsigned long usec_remainder;	/* usec since last scan */
-	unsigned long ai_count;		/* number of conversions remaining */
+	unsigned long state_bits;
 	unsigned int scan_period;	/* scan period in usec */
 	unsigned int convert_period;	/* conversion period in usec */
 	unsigned int ao_loopbacks[N_CHANS];
@@ -164,36 +169,29 @@ static void waveform_ai_interrupt(unsigned long arg)
 {
 	struct comedi_device *dev = (struct comedi_device *)arg;
 	struct waveform_private *devpriv = dev->private;
-	struct comedi_async *async = dev->read_subdev->async;
+	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
 	unsigned int i, j;
 	/* all times in microsec */
 	unsigned long elapsed_time;
 	unsigned int num_scans;
-	struct timeval now;
-	bool stopping = false;
+	ktime_t now;
+
+	/* check command is still active */
+	if (!test_bit(WAVEFORM_AI_RUNNING, &devpriv->state_bits))
+		return;
 
-	do_gettimeofday(&now);
+	now = ktime_get();
 
-	elapsed_time =
-	    1000000 * (now.tv_sec - devpriv->last.tv_sec) + now.tv_usec -
-	    devpriv->last.tv_usec;
+	elapsed_time = ktime_to_us(ktime_sub(now, devpriv->last));
 	devpriv->last = now;
 	num_scans =
 	    (devpriv->usec_remainder + elapsed_time) / devpriv->scan_period;
 	devpriv->usec_remainder =
 	    (devpriv->usec_remainder + elapsed_time) % devpriv->scan_period;
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		unsigned int remaining = cmd->stop_arg - devpriv->ai_count;
-
-		if (num_scans >= remaining) {
-			/* about to finish */
-			num_scans = remaining;
-			stopping = true;
-		}
-	}
-
+	num_scans = comedi_nscans_left(s, num_scans);
 	for (i = 0; i < num_scans; i++) {
 		for (j = 0; j < cmd->chanlist_len; j++) {
 			unsigned short sample;
@@ -203,20 +201,19 @@ static void waveform_ai_interrupt(unsigned long arg)
 					       devpriv->usec_current +
 						   i * devpriv->scan_period +
 						   j * devpriv->convert_period);
-			cfc_write_to_buffer(dev->read_subdev, sample);
+			comedi_buf_write_samples(s, &sample, 1);
 		}
 	}
 
-	devpriv->ai_count += i;
 	devpriv->usec_current += elapsed_time;
 	devpriv->usec_current %= devpriv->usec_period;
 
-	if (stopping)
+	if (cmd->stop_src == TRIG_COUNT && async->scans_done >= cmd->stop_arg)
 		async->events |= COMEDI_CB_EOA;
 	else
 		mod_timer(&devpriv->timer, jiffies + 1);
 
-	comedi_event(dev, dev->read_subdev);
+	comedi_handle_events(dev, s);
 }
 
 static int waveform_ai_cmdtest(struct comedi_device *dev,
@@ -308,7 +305,6 @@ static int waveform_ai_cmd(struct comedi_device *dev,
 		return -1;
 	}
 
-	devpriv->ai_count = 0;
 	devpriv->scan_period = cmd->scan_begin_arg / nano_per_micro;
 
 	if (cmd->convert_src == TRIG_NOW)
@@ -316,11 +312,16 @@ static int waveform_ai_cmd(struct comedi_device *dev,
 	else	/* TRIG_TIMER */
 		devpriv->convert_period = cmd->convert_arg / nano_per_micro;
 
-	do_gettimeofday(&devpriv->last);
-	devpriv->usec_current = devpriv->last.tv_usec % devpriv->usec_period;
+	devpriv->last = ktime_get();
+	devpriv->usec_current =
+		((u32)ktime_to_us(devpriv->last)) % devpriv->usec_period;
 	devpriv->usec_remainder = 0;
 
 	devpriv->timer.expires = jiffies + 1;
+	/* mark command as active */
+	smp_mb__before_atomic();
+	set_bit(WAVEFORM_AI_RUNNING, &devpriv->state_bits);
+	smp_mb__after_atomic();
 	add_timer(&devpriv->timer);
 	return 0;
 }
@@ -330,7 +331,11 @@ static int waveform_ai_cancel(struct comedi_device *dev,
 {
 	struct waveform_private *devpriv = dev->private;
 
-	del_timer_sync(&devpriv->timer);
+	/* mark command as no longer active */
+	clear_bit(WAVEFORM_AI_RUNNING, &devpriv->state_bits);
+	smp_mb__after_atomic();
+	/* cannot call del_timer_sync() as may be called from timer routine */
+	del_timer(&devpriv->timer);
 	return 0;
 }
 
@@ -405,7 +410,7 @@ static int waveform_attach(struct comedi_device *dev,
 	dev->write_subdev = s;
 	/* analog output subdevice (loopback) */
 	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITEABLE | SDF_GROUND;
+	s->subdev_flags = SDF_WRITABLE | SDF_GROUND;
 	s->n_chan = N_CHANS;
 	s->maxdata = 0xffff;
 	s->range_table = &waveform_ai_ranges;
@@ -432,7 +437,7 @@ static void waveform_detach(struct comedi_device *dev)
 	struct waveform_private *devpriv = dev->private;
 
 	if (devpriv)
-		waveform_ai_cancel(dev, dev->read_subdev);
+		del_timer_sync(&devpriv->timer);
 }
 
 static struct comedi_driver waveform_driver = {
diff --git a/drivers/staging/comedi/drivers/dac02.c b/drivers/staging/comedi/drivers/dac02.c
index 34cbe83f0ce7..beb36c8dd00a 100644
--- a/drivers/staging/comedi/drivers/dac02.c
+++ b/drivers/staging/comedi/drivers/dac02.c
@@ -129,7 +129,6 @@ static int dac02_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->maxdata	= 0x0fff;
 	s->range_table	= &das02_ao_ranges;
 	s->insn_write	= dac02_ao_insn_write;
-	s->insn_read	= comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
diff --git a/drivers/staging/comedi/drivers/daqboard2000.c b/drivers/staging/comedi/drivers/daqboard2000.c
index e5b5a8133b34..96697fbb5239 100644
--- a/drivers/staging/comedi/drivers/daqboard2000.c
+++ b/drivers/staging/comedi/drivers/daqboard2000.c
@@ -707,7 +707,6 @@ static int daqboard2000_auto_attach(struct comedi_device *dev,
 	s->n_chan = 2;
 	s->maxdata = 0xffff;
 	s->insn_write = daqboard2000_ao_insn_write;
-	s->insn_read = comedi_readback_insn_read;
 	s->range_table = &range_bipolar10;
 
 	result = comedi_alloc_subdev_readback(s);
diff --git a/drivers/staging/comedi/drivers/das08.c b/drivers/staging/comedi/drivers/das08.c
index bdb671a66e22..20a9f0eb72b5 100644
--- a/drivers/staging/comedi/drivers/das08.c
+++ b/drivers/staging/comedi/drivers/das08.c
@@ -474,7 +474,6 @@ int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 		s->maxdata = (1 << thisboard->ao_nbits) - 1;
 		s->range_table = &range_bipolar5;
 		s->insn_write = das08_ao_insn_write;
-		s->insn_read = comedi_readback_insn_read;
 
 		ret = comedi_alloc_subdev_readback(s);
 		if (ret)
@@ -507,7 +506,7 @@ int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 	/* do */
 	if (thisboard->do_nchan) {
 		s->type = COMEDI_SUBD_DO;
-		s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+		s->subdev_flags = SDF_WRITABLE;
 		s->n_chan = thisboard->do_nchan;
 		s->maxdata = 1;
 		s->range_table = &range_digital;
diff --git a/drivers/staging/comedi/drivers/das16.c b/drivers/staging/comedi/drivers/das16.c
index 2d8e86cec47a..2436057304a3 100644
--- a/drivers/staging/comedi/drivers/das16.c
+++ b/drivers/staging/comedi/drivers/das16.c
@@ -541,6 +541,7 @@ static void das16_interrupt(struct comedi_device *dev)
 	struct comedi_cmd *cmd = &async->cmd;
 	unsigned long spin_flags;
 	unsigned long dma_flags;
+	unsigned int nsamples;
 	int num_bytes, residue;
 	int buffer_index;
 
@@ -583,21 +584,25 @@ static void das16_interrupt(struct comedi_device *dev)
 
 	spin_unlock_irqrestore(&dev->spinlock, spin_flags);
 
-	cfc_write_array_to_buffer(s,
-				  devpriv->dma_buffer[buffer_index], num_bytes);
+	nsamples = comedi_bytes_to_samples(s, num_bytes);
+	comedi_buf_write_samples(s, devpriv->dma_buffer[buffer_index],
+				 nsamples);
 
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 }
 
 static void das16_timer_interrupt(unsigned long arg)
 {
 	struct comedi_device *dev = (struct comedi_device *)arg;
 	struct das16_private_struct *devpriv = dev->private;
+	unsigned long flags;
 
 	das16_interrupt(dev);
 
+	spin_lock_irqsave(&dev->spinlock, flags);
 	if (devpriv->timer_running)
 		mod_timer(&devpriv->timer, jiffies + timer_period());
+	spin_unlock_irqrestore(&dev->spinlock, flags);
 }
 
 static int das16_ai_check_chanlist(struct comedi_device *dev,
@@ -764,7 +769,7 @@ static int das16_cmd_exec(struct comedi_device *dev, struct comedi_subdevice *s)
 		return -1;
 	}
 
-	devpriv->adc_byte_count = cmd->stop_arg * cfc_bytes_per_scan(s);
+	devpriv->adc_byte_count = cmd->stop_arg * comedi_bytes_per_scan(s);
 
 	if (devpriv->can_burst)
 		outb(DAS1600_CONV_DISABLE, dev->iobase + DAS1600_CONV_REG);
@@ -814,7 +819,8 @@ static int das16_cmd_exec(struct comedi_device *dev, struct comedi_subdevice *s)
 	enable_dma(devpriv->dma_chan);
 	release_dma_lock(flags);
 
-	/*  set up interrupt */
+	/*  set up timer */
+	spin_lock_irqsave(&dev->spinlock, flags);
 	devpriv->timer_running = 1;
 	devpriv->timer.expires = jiffies + timer_period();
 	add_timer(&devpriv->timer);
@@ -823,6 +829,7 @@ static int das16_cmd_exec(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	if (devpriv->can_burst)
 		outb(0, dev->iobase + DAS1600_CONV_REG);
+	spin_unlock_irqrestore(&dev->spinlock, flags);
 
 	return 0;
 }
@@ -856,8 +863,9 @@ static void das16_ai_munge(struct comedi_device *dev,
 			   unsigned int num_bytes,
 			   unsigned int start_chan_index)
 {
-	unsigned int i, num_samples = num_bytes / sizeof(short);
 	unsigned short *data = array;
+	unsigned int num_samples = comedi_bytes_to_samples(s, num_bytes);
+	unsigned int i;
 
 	for (i = 0; i < num_samples; i++) {
 		data[i] = le16_to_cpu(data[i]);
@@ -1167,7 +1175,6 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		s->maxdata	= 0x0fff;
 		s->range_table	= devpriv->user_ao_range_table;
 		s->insn_write	= das16_ao_insn_write;
-		s->insn_read	= comedi_readback_insn_read;
 
 		ret = comedi_alloc_subdev_readback(s);
 		if (ret)
@@ -1226,6 +1233,8 @@ static void das16_detach(struct comedi_device *dev)
 	int i;
 
 	if (devpriv) {
+		if (devpriv->timer.data)
+			del_timer_sync(&devpriv->timer);
 		if (dev->iobase)
 			das16_reset(dev);
 
diff --git a/drivers/staging/comedi/drivers/das16m1.c b/drivers/staging/comedi/drivers/das16m1.c
index 24b63c452f51..80f41b7e8273 100644
--- a/drivers/staging/comedi/drivers/das16m1.c
+++ b/drivers/staging/comedi/drivers/das16m1.c
@@ -442,8 +442,7 @@ static void das16m1_handler(struct comedi_device *dev, unsigned int status)
 		num_samples = FIFO_SIZE;
 	insw(dev->iobase, devpriv->ai_buffer, num_samples);
 	munge_sample_array(devpriv->ai_buffer, num_samples);
-	cfc_write_array_to_buffer(s, devpriv->ai_buffer,
-				  num_samples * sizeof(short));
+	comedi_buf_write_samples(s, devpriv->ai_buffer, num_samples);
 	devpriv->adc_count += num_samples;
 
 	if (cmd->stop_src == TRIG_COUNT) {
@@ -460,7 +459,7 @@ static void das16m1_handler(struct comedi_device *dev, unsigned int status)
 		dev_err(dev->class_dev, "fifo overflow\n");
 	}
 
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 }
 
 static int das16m1_poll(struct comedi_device *dev, struct comedi_subdevice *s)
@@ -598,7 +597,7 @@ static int das16m1_attach(struct comedi_device *dev,
 	s = &dev->subdevices[2];
 	/* do */
 	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->subdev_flags = SDF_WRITABLE;
 	s->n_chan = 4;
 	s->maxdata = 1;
 	s->range_table = &range_digital;
diff --git a/drivers/staging/comedi/drivers/das1800.c b/drivers/staging/comedi/drivers/das1800.c
index a53d87ce9b14..be825d21a185 100644
--- a/drivers/staging/comedi/drivers/das1800.c
+++ b/drivers/staging/comedi/drivers/das1800.c
@@ -421,7 +421,6 @@ static const struct das1800_board das1800_boards[] = {
 };
 
 struct das1800_private {
-	unsigned int count;	/* number of data points left to be taken */
 	unsigned int divisor1;	/* value to load into board's counter 1 for timed conversions */
 	unsigned int divisor2;	/* value to load into board's counter 2 for timed conversions */
 	int irq_dma_bits;	/* bits for control register b */
@@ -479,42 +478,33 @@ static void das1800_handle_fifo_half_full(struct comedi_device *dev,
 					  struct comedi_subdevice *s)
 {
 	struct das1800_private *devpriv = dev->private;
-	int numPoints = 0;	/* number of points to read */
-	struct comedi_cmd *cmd = &s->async->cmd;
+	unsigned int nsamples = comedi_nsamples_left(s, FIFO_SIZE / 2);
 
-	numPoints = FIFO_SIZE / 2;
-	/* if we only need some of the points */
-	if (cmd->stop_src == TRIG_COUNT && devpriv->count < numPoints)
-		numPoints = devpriv->count;
-	insw(dev->iobase + DAS1800_FIFO, devpriv->ai_buf0, numPoints);
-	munge_data(dev, devpriv->ai_buf0, numPoints);
-	cfc_write_array_to_buffer(s, devpriv->ai_buf0,
-				  numPoints * sizeof(devpriv->ai_buf0[0]));
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->count -= numPoints;
+	insw(dev->iobase + DAS1800_FIFO, devpriv->ai_buf0, nsamples);
+	munge_data(dev, devpriv->ai_buf0, nsamples);
+	comedi_buf_write_samples(s, devpriv->ai_buf0, nsamples);
 }
 
 static void das1800_handle_fifo_not_empty(struct comedi_device *dev,
 					  struct comedi_subdevice *s)
 {
-	struct das1800_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned short dpnt;
 	int unipolar;
-	struct comedi_cmd *cmd = &s->async->cmd;
 
 	unipolar = inb(dev->iobase + DAS1800_CONTROL_C) & UB;
 
 	while (inb(dev->iobase + DAS1800_STATUS) & FNE) {
-		if (cmd->stop_src == TRIG_COUNT && devpriv->count == 0)
-			break;
 		dpnt = inw(dev->iobase + DAS1800_FIFO);
 		/* convert to unsigned type if we are in a bipolar mode */
 		if (!unipolar)
 			;
 		dpnt = munge_bipolar_sample(dev, dpnt);
-		cfc_write_to_buffer(s, dpnt);
-		if (cmd->stop_src == TRIG_COUNT)
-			devpriv->count--;
+		comedi_buf_write_samples(s, &dpnt, 1);
+
+		if (cmd->stop_src == TRIG_COUNT &&
+		    s->async->scans_done >= cmd->stop_arg)
+			break;
 	}
 }
 
@@ -525,8 +515,8 @@ static void das1800_flush_dma_channel(struct comedi_device *dev,
 				      unsigned int channel, uint16_t *buffer)
 {
 	struct das1800_private *devpriv = dev->private;
-	unsigned int num_bytes, num_samples;
-	struct comedi_cmd *cmd = &s->async->cmd;
+	unsigned int nbytes;
+	unsigned int nsamples;
 
 	disable_dma(channel);
 
@@ -535,17 +525,12 @@ static void das1800_flush_dma_channel(struct comedi_device *dev,
 	clear_dma_ff(channel);
 
 	/*  figure out how many points to read */
-	num_bytes = devpriv->dma_transfer_size - get_dma_residue(channel);
-	num_samples = num_bytes / sizeof(short);
-
-	/* if we only need some of the points */
-	if (cmd->stop_src == TRIG_COUNT && devpriv->count < num_samples)
-		num_samples = devpriv->count;
+	nbytes = devpriv->dma_transfer_size - get_dma_residue(channel);
+	nsamples = comedi_bytes_to_samples(s, nbytes);
+	nsamples = comedi_nsamples_left(s, nsamples);
 
-	munge_data(dev, buffer, num_samples);
-	cfc_write_array_to_buffer(s, buffer, num_bytes);
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->count -= num_samples;
+	munge_data(dev, buffer, nsamples);
+	comedi_buf_write_samples(s, buffer, nsamples);
 }
 
 /* flushes remaining data from board when external trigger has stopped acquisition
@@ -649,14 +634,13 @@ static void das1800_ai_handler(struct comedi_device *dev)
 		das1800_handle_fifo_not_empty(dev, s);
 	}
 
-	async->events |= COMEDI_CB_BLOCK;
 	/* if the card's fifo has overflowed */
 	if (status & OVF) {
 		/*  clear OVF interrupt bit */
 		outb(CLEAR_INTR_MASK & ~OVF, dev->iobase + DAS1800_STATUS);
 		dev_err(dev->class_dev, "FIFO overflow\n");
 		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
-		cfc_handle_events(dev, s);
+		comedi_handle_events(dev, s);
 		return;
 	}
 	/*  stop taking data if appropriate */
@@ -670,11 +654,12 @@ static void das1800_ai_handler(struct comedi_device *dev)
 		else
 			das1800_handle_fifo_not_empty(dev, s);
 		async->events |= COMEDI_CB_EOA;
-	} else if (cmd->stop_src == TRIG_COUNT && devpriv->count == 0) {	/*  stop_src TRIG_COUNT */
+	} else if (cmd->stop_src == TRIG_COUNT &&
+		   async->scans_done >= cmd->stop_arg) {
 		async->events |= COMEDI_CB_EOA;
 	}
 
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 }
 
 static int das1800_ai_poll(struct comedi_device *dev,
@@ -1102,9 +1087,6 @@ static int das1800_ai_do_cmd(struct comedi_device *dev,
 		/*  interrupt fifo half full */
 		devpriv->irq_dma_bits |= FIMD;
 	}
-	/*  determine how many conversions we need */
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->count = cmd->stop_arg * cmd->chanlist_len;
 
 	das1800_cancel(dev, s);
 
@@ -1515,7 +1497,7 @@ static int das1800_attach(struct comedi_device *dev,
 	/* do */
 	s = &dev->subdevices[3];
 	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->subdev_flags = SDF_WRITABLE;
 	s->n_chan = thisboard->do_n_chan;
 	s->maxdata = 1;
 	s->range_table = &range_digital;
diff --git a/drivers/staging/comedi/drivers/das6402.c b/drivers/staging/comedi/drivers/das6402.c
index ab6e40608885..780f4f646ea0 100644
--- a/drivers/staging/comedi/drivers/das6402.c
+++ b/drivers/staging/comedi/drivers/das6402.c
@@ -35,6 +35,7 @@
 #include <linux/interrupt.h>
 
 #include "../comedidev.h"
+#include "comedi_fc.h"
 #include "8253.h"
 
 /*
@@ -192,26 +193,197 @@ static void das6402_enable_counter(struct comedi_device *dev, bool load)
 	}
 }
 
+static unsigned int das6402_ai_read_sample(struct comedi_device *dev,
+					   struct comedi_subdevice *s)
+{
+	unsigned int val;
+
+	val = inw(dev->iobase + DAS6402_AI_DATA_REG);
+	if (s->maxdata == 0x0fff)
+		val >>= 4;
+	return val;
+}
+
 static irqreturn_t das6402_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
+	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
+	unsigned int status;
+
+	status = inb(dev->iobase + DAS6402_STATUS_REG);
+	if ((status & DAS6402_STATUS_INT) == 0)
+		return IRQ_NONE;
+
+	if (status & DAS6402_STATUS_FFULL) {
+		async->events |= COMEDI_CB_OVERFLOW;
+	} else if (status & DAS6402_STATUS_FFNE) {
+		unsigned int val;
+
+		val = das6402_ai_read_sample(dev, s);
+		comedi_buf_write_samples(s, &val, 1);
+
+		if (cmd->stop_src == TRIG_COUNT &&
+		    async->scans_done >= cmd->stop_arg)
+			async->events |= COMEDI_CB_EOA;
+	}
 
 	das6402_clear_all_interrupts(dev);
 
+	comedi_handle_events(dev, s);
+
 	return IRQ_HANDLED;
 }
 
+static void das6402_ai_set_mode(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				unsigned int chanspec,
+				unsigned int mode)
+{
+	unsigned int range = CR_RANGE(chanspec);
+	unsigned int aref = CR_AREF(chanspec);
+
+	mode |= DAS6402_MODE_RANGE(range);
+	if (aref == AREF_GROUND)
+		mode |= DAS6402_MODE_SE;
+	if (comedi_range_is_unipolar(s, range))
+		mode |= DAS6402_MODE_UNI;
+
+	das6402_set_mode(dev, mode);
+}
+
 static int das6402_ai_cmd(struct comedi_device *dev,
 			  struct comedi_subdevice *s)
 {
-	return -EINVAL;
+	struct das6402_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
+	unsigned int chan_lo = CR_CHAN(cmd->chanlist[0]);
+	unsigned int chan_hi = CR_CHAN(cmd->chanlist[cmd->chanlist_len - 1]);
+
+	das6402_ai_set_mode(dev, s, cmd->chanlist[0], DAS6402_MODE_FIFONEPTY);
+
+	/* load the mux for chanlist conversion */
+	outw(DAS6402_AI_MUX_HI(chan_hi) | DAS6402_AI_MUX_LO(chan_lo),
+	     dev->iobase + DAS6402_AI_MUX_REG);
+
+	das6402_enable_counter(dev, true);
+
+	/* enable interrupt and pacer trigger */
+	outb(DAS6402_CTRL_INTE |
+	     DAS6402_CTRL_IRQ(devpriv->irq) |
+	     DAS6402_CTRL_PACER_TRIG, dev->iobase + DAS6402_CTRL_REG);
+
+	return 0;
+}
+
+static int das6402_ai_check_chanlist(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     struct comedi_cmd *cmd)
+{
+	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+	unsigned int range0 = CR_RANGE(cmd->chanlist[0]);
+	unsigned int aref0 = CR_AREF(cmd->chanlist[0]);
+	int i;
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int range = CR_RANGE(cmd->chanlist[i]);
+		unsigned int aref = CR_AREF(cmd->chanlist[i]);
+
+		if (chan != chan0 + i) {
+			dev_dbg(dev->class_dev,
+				"chanlist must be consecutive\n");
+			return -EINVAL;
+		}
+
+		if (range != range0) {
+			dev_dbg(dev->class_dev,
+				"chanlist must have the same range\n");
+			return -EINVAL;
+		}
+
+		if (aref != aref0) {
+			dev_dbg(dev->class_dev,
+				"chanlist must have the same reference\n");
+			return -EINVAL;
+		}
+
+		if (aref0 == AREF_DIFF && chan > (s->n_chan / 2)) {
+			dev_dbg(dev->class_dev,
+				"chanlist differential channel to large\n");
+			return -EINVAL;
+		}
+	}
+	return 0;
 }
 
 static int das6402_ai_cmdtest(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_cmd *cmd)
 {
-	return -EINVAL;
+	struct das6402_private *devpriv = dev->private;
+	int err = 0;
+	unsigned int arg;
+
+	/* Step 1 : check if triggers are trivially valid */
+
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_FOLLOW);
+	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER);
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
+
+	if (err)
+		return 1;
+
+	/* Step 2a : make sure trigger sources are unique */
+
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
+
+	if (err)
+		return 2;
+
+	/* Step 3: check if arguments are trivially valid */
+
+	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+	err |= cfc_check_trigger_arg_min(&cmd->convert_arg, 10000);
+	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
+		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+
+	if (err)
+		return 3;
+
+	/* step 4: fix up any arguments */
+
+	if (cmd->convert_src == TRIG_TIMER) {
+		arg = cmd->convert_arg;
+		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
+					  &devpriv->divider1,
+					  &devpriv->divider2,
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+	}
+
+	if (err)
+		return 4;
+
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= das6402_ai_check_chanlist(dev, s, cmd);
+
+	if (err)
+		return 5;
+
+	return 0;
 }
 
 static int das6402_ai_cancel(struct comedi_device *dev,
@@ -246,26 +418,17 @@ static int das6402_ai_insn_read(struct comedi_device *dev,
 				unsigned int *data)
 {
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int range = CR_RANGE(insn->chanspec);
 	unsigned int aref = CR_AREF(insn->chanspec);
-	unsigned int val;
 	int ret;
 	int i;
 
-	val = DAS6402_MODE_RANGE(range) | DAS6402_MODE_POLLED;
-	if (aref == AREF_DIFF) {
-		if (chan > s->n_chan / 2)
-			return -EINVAL;
-	} else {
-		val |= DAS6402_MODE_SE;
-	}
-	if (comedi_range_is_unipolar(s, range))
-		val |= DAS6402_MODE_UNI;
+	if (aref == AREF_DIFF && chan > (s->n_chan / 2))
+		return -EINVAL;
 
 	/* enable software conversion trigger */
 	outb(DAS6402_CTRL_SOFT_TRIG, dev->iobase + DAS6402_CTRL_REG);
 
-	das6402_set_mode(dev, val);
+	das6402_ai_set_mode(dev, s, insn->chanspec, DAS6402_MODE_POLLED);
 
 	/* load the mux for single channel conversion */
 	outw(DAS6402_AI_MUX_HI(chan) | DAS6402_AI_MUX_LO(chan),
@@ -279,12 +442,7 @@ static int das6402_ai_insn_read(struct comedi_device *dev,
 		if (ret)
 			break;
 
-		val = inw(dev->iobase + DAS6402_AI_DATA_REG);
-
-		if (s->maxdata == 0x0fff)
-			val >>= 4;
-
-		data[i] = val;
+		data[i] = das6402_ai_read_sample(dev, s);
 	}
 
 	das6402_ai_clear_eoc(dev);
@@ -497,7 +655,7 @@ static int das6402_attach(struct comedi_device *dev,
 	/* Analog Output subdevice */
 	s = &dev->subdevices[1];
 	s->type		= COMEDI_SUBD_AO;
-	s->subdev_flags	= SDF_WRITEABLE;
+	s->subdev_flags	= SDF_WRITABLE;
 	s->n_chan	= 2;
 	s->maxdata	= board->maxdata;
 	s->range_table	= &das6402_ao_ranges;
@@ -520,7 +678,7 @@ static int das6402_attach(struct comedi_device *dev,
 	/* Digital Input subdevice */
 	s = &dev->subdevices[3];
 	s->type		= COMEDI_SUBD_DO;
-	s->subdev_flags	= SDF_WRITEABLE;
+	s->subdev_flags	= SDF_WRITABLE;
 	s->n_chan	= 8;
 	s->maxdata	= 1;
 	s->range_table	= &range_digital;
diff --git a/drivers/staging/comedi/drivers/das800.c b/drivers/staging/comedi/drivers/das800.c
index d75e5528258c..e5bdc2423445 100644
--- a/drivers/staging/comedi/drivers/das800.c
+++ b/drivers/staging/comedi/drivers/das800.c
@@ -219,7 +219,6 @@ static const struct das800_board das800_boards[] = {
 };
 
 struct das800_private {
-	unsigned int count;	/* number of data points left to be taken */
 	unsigned int divisor1;	/* counter 1 value for timed conversions */
 	unsigned int divisor2;	/* counter 2 value for timed conversions */
 	unsigned int do_bits;	/* digital output bits */
@@ -286,9 +285,6 @@ static void das800_set_frequency(struct comedi_device *dev)
 
 static int das800_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	struct das800_private *devpriv = dev->private;
-
-	devpriv->count = 0;
 	das800_disable(dev);
 	return 0;
 }
@@ -399,7 +395,6 @@ static int das800_ai_do_cmd(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
 	const struct das800_board *thisboard = dev->board_ptr;
-	struct das800_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
 	unsigned int gain = CR_RANGE(cmd->chanlist[0]);
@@ -422,11 +417,6 @@ static int das800_ai_do_cmd(struct comedi_device *dev,
 	gain &= 0xf;
 	outb(gain, dev->iobase + DAS800_GAIN);
 
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->count = cmd->stop_arg * cmd->chanlist_len;
-	else	/* TRIG_NONE */
-		devpriv->count = 0;
-
 	/* enable auto channel scan, send interrupts on end of conversion
 	 * and set clock source to internal or external
 	 */
@@ -509,25 +499,28 @@ static irqreturn_t das800_interrupt(int irq, void *d)
 		if (s->maxdata == 0x0fff)
 			val >>= 4;	/* 12-bit sample */
 
-		/* if there are more data points to collect */
-		if (cmd->stop_src == TRIG_NONE || devpriv->count > 0) {
-			/* write data point to buffer */
-			cfc_write_to_buffer(s, val & s->maxdata);
-			devpriv->count--;
+		val &= s->maxdata;
+		comedi_buf_write_samples(s, &val, 1);
+
+		if (cmd->stop_src == TRIG_COUNT &&
+		    async->scans_done >= cmd->stop_arg) {
+			async->events |= COMEDI_CB_EOA;
+			break;
 		}
 	}
-	async->events |= COMEDI_CB_BLOCK;
 
 	if (fifo_overflow) {
 		spin_unlock_irqrestore(&dev->spinlock, irq_flags);
 		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
-		cfc_handle_events(dev, s);
+		comedi_handle_events(dev, s);
 		return IRQ_HANDLED;
 	}
 
-	if (cmd->stop_src == TRIG_NONE || devpriv->count > 0) {
-		/* Re-enable card's interrupt.
-		 * We already have spinlock, so indirect addressing is safe */
+	if (!(async->events & COMEDI_CB_CANCEL_MASK)) {
+		/*
+		 * Re-enable card's interrupt.
+		 * We already have spinlock, so indirect addressing is safe
+		 */
 		das800_ind_write(dev, CONTROL1_INTE | devpriv->do_bits,
 				 CONTROL1);
 		spin_unlock_irqrestore(&dev->spinlock, irq_flags);
@@ -535,9 +528,8 @@ static irqreturn_t das800_interrupt(int irq, void *d)
 		/* otherwise, stop taking data */
 		spin_unlock_irqrestore(&dev->spinlock, irq_flags);
 		das800_disable(dev);
-		async->events |= COMEDI_CB_EOA;
 	}
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 	return IRQ_HANDLED;
 }
 
@@ -738,7 +730,7 @@ static int das800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	/* Digital Output subdevice */
 	s = &dev->subdevices[2];
 	s->type		= COMEDI_SUBD_DO;
-	s->subdev_flags	= SDF_WRITABLE | SDF_READABLE;
+	s->subdev_flags	= SDF_WRITABLE;
 	s->n_chan	= 4;
 	s->maxdata	= 1;
 	s->range_table	= &range_digital;
diff --git a/drivers/staging/comedi/drivers/dmm32at.c b/drivers/staging/comedi/drivers/dmm32at.c
index 7215e09305cf..6df298a99cc6 100644
--- a/drivers/staging/comedi/drivers/dmm32at.c
+++ b/drivers/staging/comedi/drivers/dmm32at.c
@@ -1,125 +1,139 @@
 /*
-    comedi/drivers/dmm32at.c
-    Diamond Systems mm32at code for a Comedi driver
-
-    COMEDI - Linux Control and Measurement Device Interface
-    Copyright (C) 2000 David A. Schleef <ds@schleef.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.
-*/
+ * dmm32at.c
+ * Diamond Systems Diamond-MM-32-AT Comedi driver
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 2000 David A. Schleef <ds@schleef.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.
+ */
+
 /*
-Driver: dmm32at
-Description: Diamond Systems mm32at driver.
-Devices:
-Author: Perry J. Piplani <perry.j.piplani@nasa.gov>
-Updated: Fri Jun  4 09:13:24 CDT 2004
-Status: experimental
-
-This driver is for the Diamond Systems MM-32-AT board
-http://www.diamondsystems.com/products/diamondmm32at It is being used
-on serveral projects inside NASA, without problems so far. For analog
-input commands, TRIG_EXT is not yet supported at all..
-
-Configuration Options:
-  comedi_config /dev/comedi0 dmm32at baseaddr,irq
-*/
+ * Driver: dmm32at
+ * Description: Diamond Systems Diamond-MM-32-AT
+ * Devices: (Diamond Systems) Diamond-MM-32-AT [dmm32at]
+ * Author: Perry J. Piplani <perry.j.piplani@nasa.gov>
+ * Updated: Fri Jun  4 09:13:24 CDT 2004
+ * Status: experimental
+ *
+ * Configuration Options:
+ *	comedi_config /dev/comedi0 dmm32at baseaddr,irq
+ *
+ * This driver is for the Diamond Systems MM-32-AT board
+ *	http://www.diamondsystems.com/products/diamondmm32at
+ *
+ * It is being used on serveral projects inside NASA, without
+ * problems so far. For analog input commands, TRIG_EXT is not
+ * yet supported.
+ */
 
 #include <linux/module.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 #include "../comedidev.h"
 
+#include "8255.h"
 #include "comedi_fc.h"
 
 /* Board register addresses */
-#define DMM32AT_CONV 0x00
-#define DMM32AT_AILSB 0x00
-#define DMM32AT_AUXDOUT 0x01
-#define DMM32AT_AIMSB 0x01
-#define DMM32AT_AILOW 0x02
-#define DMM32AT_AIHIGH 0x03
-
-#define DMM32AT_DACSTAT 0x04
-#define DMM32AT_DACLSB_REG	0x04
-#define DMM32AT_DACMSB_REG	0x05
-#define DMM32AT_DACMSB_CHAN(x)	((x) << 6)
-
-#define DMM32AT_FIFOCNTRL 0x07
-#define DMM32AT_FIFOSTAT 0x07
-
-#define DMM32AT_CNTRL 0x08
-#define DMM32AT_AISTAT 0x08
-
-#define DMM32AT_INTCLOCK 0x09
-
-#define DMM32AT_CNTRDIO 0x0a
-
-#define DMM32AT_AICONF 0x0b
-#define DMM32AT_AIRBACK 0x0b
+#define DMM32AT_AI_START_CONV_REG	0x00
+#define DMM32AT_AI_LSB_REG		0x00
+#define DMM32AT_AUX_DOUT_REG		0x01
+#define DMM32AT_AUX_DOUT2		(1 << 2)  /* J3.42 - OUT2 (OUT2EN) */
+#define DMM32AT_AUX_DOUT1		(1 << 1)  /* J3.43 */
+#define DMM32AT_AUX_DOUT0		(1 << 0)  /* J3.44 - OUT0 (OUT0EN) */
+#define DMM32AT_AI_MSB_REG		0x01
+#define DMM32AT_AI_LO_CHAN_REG		0x02
+#define DMM32AT_AI_HI_CHAN_REG		0x03
+#define DMM32AT_AUX_DI_REG		0x04
+#define DMM32AT_AUX_DI_DACBUSY		(1 << 7)
+#define DMM32AT_AUX_DI_CALBUSY		(1 << 6)
+#define DMM32AT_AUX_DI3			(1 << 3)  /* J3.45 - ADCLK (CLKSEL) */
+#define DMM32AT_AUX_DI2			(1 << 2)  /* J3.46 - GATE12 (GT12EN) */
+#define DMM32AT_AUX_DI1			(1 << 1)  /* J3.47 - GATE0 (GT0EN) */
+#define DMM32AT_AUX_DI0			(1 << 0)  /* J3.48 - CLK0 (SRC0) */
+#define DMM32AT_AO_LSB_REG		0x04
+#define DMM32AT_AO_MSB_REG		0x05
+#define DMM32AT_AO_MSB_DACH(x)		((x) << 6)
+#define DMM32AT_FIFO_DEPTH_REG		0x06
+#define DMM32AT_FIFO_CTRL_REG		0x07
+#define DMM32AT_FIFO_CTRL_FIFOEN	(1 << 3)
+#define DMM32AT_FIFO_CTRL_SCANEN	(1 << 2)
+#define DMM32AT_FIFO_CTRL_FIFORST	(1 << 1)
+#define DMM32AT_FIFO_STATUS_REG		0x07
+#define DMM32AT_FIFO_STATUS_EF		(1 << 7)
+#define DMM32AT_FIFO_STATUS_HF		(1 << 6)
+#define DMM32AT_FIFO_STATUS_FF		(1 << 5)
+#define DMM32AT_FIFO_STATUS_OVF		(1 << 4)
+#define DMM32AT_FIFO_STATUS_FIFOEN	(1 << 3)
+#define DMM32AT_FIFO_STATUS_SCANEN	(1 << 2)
+#define DMM32AT_FIFO_STATUS_PAGE_MASK	(3 << 0)
+#define DMM32AT_CTRL_REG		0x08
+#define DMM32AT_CTRL_RESETA		(1 << 5)
+#define DMM32AT_CTRL_RESETD		(1 << 4)
+#define DMM32AT_CTRL_INTRST		(1 << 3)
+#define DMM32AT_CTRL_PAGE_8254		(0 << 0)
+#define DMM32AT_CTRL_PAGE_8255		(1 << 0)
+#define DMM32AT_CTRL_PAGE_CALIB		(3 << 0)
+#define DMM32AT_AI_STATUS_REG		0x08
+#define DMM32AT_AI_STATUS_STS		(1 << 7)
+#define DMM32AT_AI_STATUS_SD1		(1 << 6)
+#define DMM32AT_AI_STATUS_SD0		(1 << 5)
+#define DMM32AT_AI_STATUS_ADCH_MASK	(0x1f << 0)
+#define DMM32AT_INTCLK_REG		0x09
+#define DMM32AT_INTCLK_ADINT		(1 << 7)
+#define DMM32AT_INTCLK_DINT		(1 << 6)
+#define DMM32AT_INTCLK_TINT		(1 << 5)
+#define DMM32AT_INTCLK_CLKEN		(1 << 1)  /* 1=see below  0=software */
+#define DMM32AT_INTCLK_CLKSEL		(1 << 0)  /* 1=OUT2  0=EXTCLK */
+#define DMM32AT_CTRDIO_CFG_REG		0x0a
+#define DMM32AT_CTRDIO_CFG_FREQ12	(1 << 7)  /* CLK12 1=100KHz 0=10MHz */
+#define DMM32AT_CTRDIO_CFG_FREQ0	(1 << 6)  /* CLK0  1=10KHz  0=10MHz */
+#define DMM32AT_CTRDIO_CFG_OUT2EN	(1 << 5)  /* J3.42 1=OUT2 is DOUT2 */
+#define DMM32AT_CTRDIO_CFG_OUT0EN	(1 << 4)  /* J3,44 1=OUT0 is DOUT0 */
+#define DMM32AT_CTRDIO_CFG_GT0EN	(1 << 2)  /* J3.47 1=DIN1 is GATE0 */
+#define DMM32AT_CTRDIO_CFG_SRC0		(1 << 1)  /* CLK0 is 0=FREQ0 1=J3.48 */
+#define DMM32AT_CTRDIO_CFG_GT12EN	(1 << 0)  /* J3.46 1=DIN2 is GATE12 */
+#define DMM32AT_AI_CFG_REG		0x0b
+#define DMM32AT_AI_CFG_SCINT_20US	(0 << 4)
+#define DMM32AT_AI_CFG_SCINT_15US	(1 << 4)
+#define DMM32AT_AI_CFG_SCINT_10US	(2 << 4)
+#define DMM32AT_AI_CFG_SCINT_5US	(3 << 4)
+#define DMM32AT_AI_CFG_RANGE		(1 << 3)  /* 0=5V  1=10V */
+#define DMM32AT_AI_CFG_ADBU		(1 << 2)  /* 0=bipolar  1=unipolar */
+#define DMM32AT_AI_CFG_GAIN(x)		((x) << 0)
+#define DMM32AT_AI_READBACK_REG		0x0b
+#define DMM32AT_AI_READBACK_WAIT	(1 << 7)  /* DMM32AT_AI_STATUS_STS */
+#define DMM32AT_AI_READBACK_RANGE	(1 << 3)
+#define DMM32AT_AI_READBACK_ADBU	(1 << 2)
+#define DMM32AT_AI_READBACK_GAIN_MASK	(3 << 0)
 
 #define DMM32AT_CLK1 0x0d
 #define DMM32AT_CLK2 0x0e
 #define DMM32AT_CLKCT 0x0f
 
-#define DMM32AT_DIOA 0x0c
-#define DMM32AT_DIOB 0x0d
-#define DMM32AT_DIOC 0x0e
-#define DMM32AT_DIOCONF 0x0f
+#define DMM32AT_8255_IOBASE		0x0c  /* Page 1 registers */
 
 /* Board register values. */
 
-/* DMM32AT_DACSTAT 0x04 */
-#define DMM32AT_DACBUSY 0x80
-
-/* DMM32AT_FIFOCNTRL 0x07 */
-#define DMM32AT_FIFORESET 0x02
-#define DMM32AT_SCANENABLE 0x04
-
-/* DMM32AT_CNTRL 0x08 */
-#define DMM32AT_RESET 0x20
-#define DMM32AT_INTRESET 0x08
-#define DMM32AT_CLKACC 0x00
-#define DMM32AT_DIOACC 0x01
-
-/* DMM32AT_AISTAT 0x08 */
-#define DMM32AT_STATUS 0x80
-
-/* DMM32AT_INTCLOCK 0x09 */
-#define DMM32AT_ADINT 0x80
-#define DMM32AT_CLKSEL 0x03
-
-/* DMM32AT_CNTRDIO 0x0a */
-#define DMM32AT_FREQ12 0x80
-
-/* DMM32AT_AICONF 0x0b */
+/* DMM32AT_AI_CFG_REG 0x0b */
 #define DMM32AT_RANGE_U10 0x0c
 #define DMM32AT_RANGE_U5 0x0d
 #define DMM32AT_RANGE_B10 0x08
 #define DMM32AT_RANGE_B5 0x00
-#define DMM32AT_SCINT_20 0x00
-#define DMM32AT_SCINT_15 0x10
-#define DMM32AT_SCINT_10 0x20
-#define DMM32AT_SCINT_5 0x30
 
 /* DMM32AT_CLKCT 0x0f */
 #define DMM32AT_CLKCT1 0x56	/* mode3 counter 1 - write low byte only */
 #define DMM32AT_CLKCT2 0xb6	/*  mode3 counter 2 - write high and low byte */
 
-/* DMM32AT_DIOCONF 0x0f */
-#define DMM32AT_DIENABLE 0x80
-#define DMM32AT_DIRA 0x10
-#define DMM32AT_DIRB 0x02
-#define DMM32AT_DIRCL 0x01
-#define DMM32AT_DIRCH 0x08
-
 /* board AI ranges in comedi structure */
 static const struct comedi_lrange dmm32at_airanges = {
 	4, {
@@ -150,12 +164,36 @@ static const struct comedi_lrange dmm32at_aoranges = {
 	}
 };
 
-struct dmm32at_private {
-	int data;
-	int ai_inuse;
-	unsigned int ai_scans_left;
-	unsigned char dio_config;
-};
+static void dmm32at_ai_set_chanspec(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    unsigned int chanspec, int nchan)
+{
+	unsigned int chan = CR_CHAN(chanspec);
+	unsigned int range = CR_RANGE(chanspec);
+	unsigned int last_chan = (chan + nchan - 1) % s->n_chan;
+
+	outb(DMM32AT_FIFO_CTRL_FIFORST, dev->iobase + DMM32AT_FIFO_CTRL_REG);
+
+	if (nchan > 1)
+		outb(DMM32AT_FIFO_CTRL_SCANEN,
+		     dev->iobase + DMM32AT_FIFO_CTRL_REG);
+
+	outb(chan, dev->iobase + DMM32AT_AI_LO_CHAN_REG);
+	outb(last_chan, dev->iobase + DMM32AT_AI_HI_CHAN_REG);
+	outb(dmm32at_rangebits[range], dev->iobase + DMM32AT_AI_CFG_REG);
+}
+
+static unsigned int dmm32at_ai_get_sample(struct comedi_device *dev,
+					  struct comedi_subdevice *s)
+{
+	unsigned int val;
+
+	val = inb(dev->iobase + DMM32AT_AI_LSB_REG);
+	val |= (inb(dev->iobase + DMM32AT_AI_MSB_REG) << 8);
+
+	/* munge two's complement value to offset binary */
+	return comedi_offset_munge(s, val);
+}
 
 static int dmm32at_ai_status(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
@@ -165,75 +203,39 @@ static int dmm32at_ai_status(struct comedi_device *dev,
 	unsigned char status;
 
 	status = inb(dev->iobase + context);
-	if ((status & DMM32AT_STATUS) == 0)
+	if ((status & DMM32AT_AI_STATUS_STS) == 0)
 		return 0;
 	return -EBUSY;
 }
 
-static int dmm32at_ai_rinsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
+static int dmm32at_ai_insn_read(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn,
+				unsigned int *data)
 {
-	int n;
-	unsigned int d;
-	unsigned short msb, lsb;
-	unsigned char chan;
-	int range;
 	int ret;
+	int i;
 
-	/* get the channel and range number */
-
-	chan = CR_CHAN(insn->chanspec) & (s->n_chan - 1);
-	range = CR_RANGE(insn->chanspec);
-
-	/* zero scan and fifo control and reset fifo */
-	outb(DMM32AT_FIFORESET, dev->iobase + DMM32AT_FIFOCNTRL);
-
-	/* write the ai channel range regs */
-	outb(chan, dev->iobase + DMM32AT_AILOW);
-	outb(chan, dev->iobase + DMM32AT_AIHIGH);
-	/* set the range bits */
-	outb(dmm32at_rangebits[range], dev->iobase + DMM32AT_AICONF);
+	dmm32at_ai_set_chanspec(dev, s, insn->chanspec, 1);
 
 	/* wait for circuit to settle */
-	ret = comedi_timeout(dev, s, insn, dmm32at_ai_status, DMM32AT_AIRBACK);
+	ret = comedi_timeout(dev, s, insn, dmm32at_ai_status,
+			     DMM32AT_AI_READBACK_REG);
 	if (ret)
 		return ret;
 
-	/* convert n samples */
-	for (n = 0; n < insn->n; n++) {
-		/* trigger conversion */
-		outb(0xff, dev->iobase + DMM32AT_CONV);
+	for (i = 0; i < insn->n; i++) {
+		outb(0xff, dev->iobase + DMM32AT_AI_START_CONV_REG);
 
-		/* wait for conversion to end */
 		ret = comedi_timeout(dev, s, insn, dmm32at_ai_status,
-				     DMM32AT_AISTAT);
+				     DMM32AT_AI_STATUS_REG);
 		if (ret)
 			return ret;
 
-		/* read data */
-		lsb = inb(dev->iobase + DMM32AT_AILSB);
-		msb = inb(dev->iobase + DMM32AT_AIMSB);
-
-		/* invert sign bit to make range unsigned, this is an
-		   idiosyncrasy of the diamond board, it return
-		   conversions as a signed value, i.e. -32768 to
-		   32767, flipping the bit and interpreting it as
-		   signed gives you a range of 0 to 65535 which is
-		   used by comedi */
-		d = ((msb ^ 0x0080) << 8) + lsb;
-
-		data[n] = d;
+		data[i] = dmm32at_ai_get_sample(dev, s);
 	}
 
-	/* return the number of samples read/written */
-	return n;
-}
-
-static int dmm32at_ns_to_timer(unsigned int *ns, unsigned int flags)
-{
-	/* trivial timer */
-	return *ns;
+	return insn->n;
 }
 
 static int dmm32at_ai_check_chanlist(struct comedi_device *dev,
@@ -273,10 +275,8 @@ static int dmm32at_ai_cmdtest(struct comedi_device *dev,
 	/* Step 1 : check if triggers are trivially valid */
 
 	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
-					TRIG_TIMER /*| TRIG_EXT */);
-	err |= cfc_check_trigger_src(&cmd->convert_src,
-					TRIG_TIMER /*| TRIG_EXT */);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_TIMER);
+	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER);
 	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
 	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
 
@@ -285,8 +285,6 @@ static int dmm32at_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
 	err |= cfc_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
@@ -298,67 +296,32 @@ static int dmm32at_ai_cmdtest(struct comedi_device *dev,
 
 	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
 
-#define MAX_SCAN_SPEED	1000000	/* in nanoseconds */
-#define MIN_SCAN_SPEED	1000000000	/* in nanoseconds */
-
-	if (cmd->scan_begin_src == TRIG_TIMER) {
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						 MAX_SCAN_SPEED);
-		err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg,
-						 MIN_SCAN_SPEED);
-	} else {
-		/* external trigger */
-		/* should be level/edge, hi/lo specification here */
-		/* should specify multiple external triggers */
-		err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg, 9);
-	}
+	err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg, 1000000);
+	err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg, 1000000000);
 
-	if (cmd->convert_src == TRIG_TIMER) {
-		if (cmd->convert_arg >= 17500)
-			cmd->convert_arg = 20000;
-		else if (cmd->convert_arg >= 12500)
-			cmd->convert_arg = 15000;
-		else if (cmd->convert_arg >= 7500)
-			cmd->convert_arg = 10000;
-		else
-			cmd->convert_arg = 5000;
-	} else {
-		/* external trigger */
-		/* see above */
-		err |= cfc_check_trigger_arg_max(&cmd->convert_arg, 9);
-	}
+	if (cmd->convert_arg >= 17500)
+		cmd->convert_arg = 20000;
+	else if (cmd->convert_arg >= 12500)
+		cmd->convert_arg = 15000;
+	else if (cmd->convert_arg >= 7500)
+		cmd->convert_arg = 10000;
+	else
+		cmd->convert_arg = 5000;
 
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		err |= cfc_check_trigger_arg_max(&cmd->stop_arg, 0xfffffff0);
+	if (cmd->stop_src == TRIG_COUNT)
 		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
-	} else {
-		/* TRIG_NONE */
+	else /* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-	}
 
 	if (err)
 		return 3;
 
-	/* step 4: fix up any arguments */
+	/* Step 4: fix up any arguments */
 
-	if (cmd->scan_begin_src == TRIG_TIMER) {
-		arg = cmd->scan_begin_arg;
-		dmm32at_ns_to_timer(&arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
-	}
-	if (cmd->convert_src == TRIG_TIMER) {
-		arg = cmd->convert_arg;
-		dmm32at_ns_to_timer(&arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
-
-		if (cmd->scan_begin_src == TRIG_TIMER) {
-			arg = cmd->convert_arg * cmd->scan_end_arg;
-			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-							 arg);
-		}
-	}
+	arg = cmd->convert_arg * cmd->scan_end_arg;
+	err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg, arg);
 
 	if (err)
 		return 4;
@@ -384,11 +347,11 @@ static void dmm32at_setaitimer(struct comedi_device *dev, unsigned int nansec)
 	hi2 = (both2 & 0xff00) >> 8;
 	lo2 = both2 & 0x00ff;
 
-	/* set the counter frequency to 10mhz */
-	outb(0, dev->iobase + DMM32AT_CNTRDIO);
+	/* set counter clocks to 10MHz, disable all aux dio */
+	outb(0, dev->iobase + DMM32AT_CTRDIO_CFG_REG);
 
 	/* get access to the clock regs */
-	outb(DMM32AT_CLKACC, dev->iobase + DMM32AT_CNTRL);
+	outb(DMM32AT_CTRL_PAGE_8254, dev->iobase + DMM32AT_CTRL_REG);
 
 	/* write the counter 1 control word and low byte to counter */
 	outb(DMM32AT_CLKCT1, dev->iobase + DMM32AT_CLKCT);
@@ -400,65 +363,37 @@ static void dmm32at_setaitimer(struct comedi_device *dev, unsigned int nansec)
 	outb(hi2, dev->iobase + DMM32AT_CLK2);
 
 	/* enable the ai conversion interrupt and the clock to start scans */
-	outb(DMM32AT_ADINT | DMM32AT_CLKSEL, dev->iobase + DMM32AT_INTCLOCK);
+	outb(DMM32AT_INTCLK_ADINT |
+	     DMM32AT_INTCLK_CLKEN | DMM32AT_INTCLK_CLKSEL,
+             dev->iobase + DMM32AT_INTCLK_REG);
 }
 
 static int dmm32at_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	struct dmm32at_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
-	int range;
-	unsigned char chanlo, chanhi;
 	int ret;
 
-	if (!cmd->chanlist)
-		return -EINVAL;
-
-	/* get the channel list and range */
-	chanlo = CR_CHAN(cmd->chanlist[0]) & (s->n_chan - 1);
-	chanhi = chanlo + cmd->chanlist_len - 1;
-	if (chanhi >= s->n_chan)
-		return -EINVAL;
-	range = CR_RANGE(cmd->chanlist[0]);
-
-	/* reset fifo */
-	outb(DMM32AT_FIFORESET, dev->iobase + DMM32AT_FIFOCNTRL);
-
-	/* set scan enable */
-	outb(DMM32AT_SCANENABLE, dev->iobase + DMM32AT_FIFOCNTRL);
-
-	/* write the ai channel range regs */
-	outb(chanlo, dev->iobase + DMM32AT_AILOW);
-	outb(chanhi, dev->iobase + DMM32AT_AIHIGH);
-
-	/* set the range bits */
-	outb(dmm32at_rangebits[range], dev->iobase + DMM32AT_AICONF);
+	dmm32at_ai_set_chanspec(dev, s, cmd->chanlist[0], cmd->chanlist_len);
 
 	/* reset the interrupt just in case */
-	outb(DMM32AT_INTRESET, dev->iobase + DMM32AT_CNTRL);
-
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->ai_scans_left = cmd->stop_arg;
-	else {			/* TRIG_NONE */
-		devpriv->ai_scans_left = 0xffffffff; /* indicates TRIG_NONE to
-						      * isr */
-	}
+	outb(DMM32AT_CTRL_INTRST, dev->iobase + DMM32AT_CTRL_REG);
 
 	/*
 	 * wait for circuit to settle
 	 * we don't have the 'insn' here but it's not needed
 	 */
-	ret = comedi_timeout(dev, s, NULL, dmm32at_ai_status, DMM32AT_AIRBACK);
+	ret = comedi_timeout(dev, s, NULL, dmm32at_ai_status,
+			     DMM32AT_AI_READBACK_REG);
 	if (ret)
 		return ret;
 
-	if (devpriv->ai_scans_left > 1) {
+	if (cmd->stop_src == TRIG_NONE || cmd->stop_arg > 1) {
 		/* start the clock and enable the interrupts */
 		dmm32at_setaitimer(dev, cmd->scan_begin_arg);
 	} else {
 		/* start the interrups and initiate a single scan */
-		outb(DMM32AT_ADINT, dev->iobase + DMM32AT_INTCLOCK);
-		outb(0xff, dev->iobase + DMM32AT_CONV);
+		outb(DMM32AT_INTCLK_ADINT, dev->iobase + DMM32AT_INTCLK_REG);
+		outb(0xff, dev->iobase + DMM32AT_AI_START_CONV_REG);
 	}
 
 	return 0;
@@ -468,19 +403,16 @@ static int dmm32at_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 static int dmm32at_ai_cancel(struct comedi_device *dev,
 			     struct comedi_subdevice *s)
 {
-	struct dmm32at_private *devpriv = dev->private;
-
-	devpriv->ai_scans_left = 1;
+	/* disable further interrupts and clocks */
+	outb(0x0, dev->iobase + DMM32AT_INTCLK_REG);
 	return 0;
 }
 
 static irqreturn_t dmm32at_isr(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	struct dmm32at_private *devpriv = dev->private;
 	unsigned char intstat;
-	unsigned int samp;
-	unsigned short msb, lsb;
+	unsigned int val;
 	int i;
 
 	if (!dev->attached) {
@@ -488,38 +420,26 @@ static irqreturn_t dmm32at_isr(int irq, void *d)
 		return IRQ_HANDLED;
 	}
 
-	intstat = inb(dev->iobase + DMM32AT_INTCLOCK);
+	intstat = inb(dev->iobase + DMM32AT_INTCLK_REG);
 
-	if (intstat & DMM32AT_ADINT) {
+	if (intstat & DMM32AT_INTCLK_ADINT) {
 		struct comedi_subdevice *s = dev->read_subdev;
 		struct comedi_cmd *cmd = &s->async->cmd;
 
 		for (i = 0; i < cmd->chanlist_len; i++) {
-			/* read data */
-			lsb = inb(dev->iobase + DMM32AT_AILSB);
-			msb = inb(dev->iobase + DMM32AT_AIMSB);
-
-			/* invert sign bit to make range unsigned */
-			samp = ((msb ^ 0x0080) << 8) + lsb;
-			comedi_buf_put(s, samp);
+			val = dmm32at_ai_get_sample(dev, s);
+			comedi_buf_write_samples(s, &val, 1);
 		}
 
-		if (devpriv->ai_scans_left != 0xffffffff) {	/* TRIG_COUNT */
-			devpriv->ai_scans_left--;
-			if (devpriv->ai_scans_left == 0) {
-				/* disable further interrupts and clocks */
-				outb(0x0, dev->iobase + DMM32AT_INTCLOCK);
-				/* set the buffer to be flushed with an EOF */
-				s->async->events |= COMEDI_CB_EOA;
-			}
+		if (cmd->stop_src == TRIG_COUNT &&
+		    s->async->scans_done >= cmd->stop_arg)
+			s->async->events |= COMEDI_CB_EOA;
 
-		}
-		/* flush the buffer */
-		comedi_event(dev, s);
+		comedi_handle_events(dev, s);
 	}
 
 	/* reset the interrupt */
-	outb(DMM32AT_INTRESET, dev->iobase + DMM32AT_CNTRL);
+	outb(DMM32AT_CTRL_INTRST, dev->iobase + DMM32AT_CTRL_REG);
 	return IRQ_HANDLED;
 }
 
@@ -530,8 +450,8 @@ static int dmm32at_ao_eoc(struct comedi_device *dev,
 {
 	unsigned char status;
 
-	status = inb(dev->iobase + DMM32AT_DACSTAT);
-	if ((status & DMM32AT_DACBUSY) == 0)
+	status = inb(dev->iobase + DMM32AT_AUX_DI_REG);
+	if ((status & DMM32AT_AUX_DI_DACBUSY) == 0)
 		return 0;
 	return -EBUSY;
 }
@@ -549,9 +469,9 @@ static int dmm32at_ao_insn_write(struct comedi_device *dev,
 		int ret;
 
 		/* write LSB then MSB + chan to load DAC */
-		outb(val & 0xff, dev->iobase + DMM32AT_DACLSB_REG);
-		outb((val >> 8) | DMM32AT_DACMSB_CHAN(chan),
-		     dev->iobase + DMM32AT_DACMSB_REG);
+		outb(val & 0xff, dev->iobase + DMM32AT_AO_LSB_REG);
+		outb((val >> 8) | DMM32AT_AO_MSB_DACH(chan),
+		     dev->iobase + DMM32AT_AO_MSB_REG);
 
 		/* wait for circuit to settle */
 		ret = comedi_timeout(dev, s, insn, dmm32at_ao_eoc, 0);
@@ -559,7 +479,7 @@ static int dmm32at_ao_insn_write(struct comedi_device *dev,
 			return ret;
 
 		/* dummy read to update DAC */
-		inb(dev->iobase + DMM32AT_DACMSB_REG);
+		inb(dev->iobase + DMM32AT_AO_MSB_REG);
 
 		s->readback[chan] = val;
 	}
@@ -567,136 +487,82 @@ static int dmm32at_ao_insn_write(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int dmm32at_dio_insn_bits(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn,
-				 unsigned int *data)
-{
-	struct dmm32at_private *devpriv = dev->private;
-	unsigned int mask;
-	unsigned int val;
-
-	mask = comedi_dio_update_state(s, data);
-	if (mask) {
-		/* get access to the DIO regs */
-		outb(DMM32AT_DIOACC, dev->iobase + DMM32AT_CNTRL);
-
-		/* if either part of dio is set for output */
-		if (((devpriv->dio_config & DMM32AT_DIRCL) == 0) ||
-		    ((devpriv->dio_config & DMM32AT_DIRCH) == 0)) {
-			val = (s->state & 0x00ff0000) >> 16;
-			outb(val, dev->iobase + DMM32AT_DIOC);
-		}
-		if ((devpriv->dio_config & DMM32AT_DIRB) == 0) {
-			val = (s->state & 0x0000ff00) >> 8;
-			outb(val, dev->iobase + DMM32AT_DIOB);
-		}
-		if ((devpriv->dio_config & DMM32AT_DIRA) == 0) {
-			val = (s->state & 0x000000ff);
-			outb(val, dev->iobase + DMM32AT_DIOA);
-		}
-	}
-
-	val = inb(dev->iobase + DMM32AT_DIOA);
-	val |= inb(dev->iobase + DMM32AT_DIOB) << 8;
-	val |= inb(dev->iobase + DMM32AT_DIOC) << 16;
-	s->state = val;
-
-	data[1] = val;
-
-	return insn->n;
-}
-
-static int dmm32at_dio_insn_config(struct comedi_device *dev,
-				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn,
-				   unsigned int *data)
+static int dmm32at_8255_io(struct comedi_device *dev,
+			   int dir, int port, int data, unsigned long regbase)
 {
-	struct dmm32at_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int mask;
-	unsigned char chanbit;
-	int ret;
-
-	if (chan < 8) {
-		mask = 0x0000ff;
-		chanbit = DMM32AT_DIRA;
-	} else if (chan < 16) {
-		mask = 0x00ff00;
-		chanbit = DMM32AT_DIRB;
-	} else if (chan < 20) {
-		mask = 0x0f0000;
-		chanbit = DMM32AT_DIRCL;
-	} else {
-		mask = 0xf00000;
-		chanbit = DMM32AT_DIRCH;
-	}
-
-	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
-	if (ret)
-		return ret;
-
-	if (data[0] == INSN_CONFIG_DIO_OUTPUT)
-		devpriv->dio_config &= ~chanbit;
-	else
-		devpriv->dio_config |= chanbit;
 	/* get access to the DIO regs */
-	outb(DMM32AT_DIOACC, dev->iobase + DMM32AT_CNTRL);
-	/* set the DIO's to the new configuration setting */
-	outb(devpriv->dio_config, dev->iobase + DMM32AT_DIOCONF);
+	outb(DMM32AT_CTRL_PAGE_8255, dev->iobase + DMM32AT_CTRL_REG);
 
-	return insn->n;
+	if (dir) {
+		outb(data, dev->iobase + regbase + port);
+		return 0;
+	}
+	return inb(dev->iobase + regbase + port);
 }
 
-static int dmm32at_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
+/* Make sure the board is there and put it to a known state */
+static int dmm32at_reset(struct comedi_device *dev)
 {
-	struct dmm32at_private *devpriv;
-	int ret;
-	struct comedi_subdevice *s;
 	unsigned char aihi, ailo, fifostat, aistat, intstat, airback;
 
-	ret = comedi_request_region(dev, it->options[0], 0x10);
-	if (ret)
-		return ret;
-
-	/* the following just makes sure the board is there and gets
-	   it to a known state */
-
 	/* reset the board */
-	outb(DMM32AT_RESET, dev->iobase + DMM32AT_CNTRL);
+	outb(DMM32AT_CTRL_RESETA, dev->iobase + DMM32AT_CTRL_REG);
 
 	/* allow a millisecond to reset */
 	udelay(1000);
 
 	/* zero scan and fifo control */
-	outb(0x0, dev->iobase + DMM32AT_FIFOCNTRL);
+	outb(0x0, dev->iobase + DMM32AT_FIFO_CTRL_REG);
 
 	/* zero interrupt and clock control */
-	outb(0x0, dev->iobase + DMM32AT_INTCLOCK);
+	outb(0x0, dev->iobase + DMM32AT_INTCLK_REG);
 
 	/* write a test channel range, the high 3 bits should drop */
-	outb(0x80, dev->iobase + DMM32AT_AILOW);
-	outb(0xff, dev->iobase + DMM32AT_AIHIGH);
+	outb(0x80, dev->iobase + DMM32AT_AI_LO_CHAN_REG);
+	outb(0xff, dev->iobase + DMM32AT_AI_HI_CHAN_REG);
 
 	/* set the range at 10v unipolar */
-	outb(DMM32AT_RANGE_U10, dev->iobase + DMM32AT_AICONF);
+	outb(DMM32AT_RANGE_U10, dev->iobase + DMM32AT_AI_CFG_REG);
 
 	/* should take 10 us to settle, here's a hundred */
 	udelay(100);
 
 	/* read back the values */
-	ailo = inb(dev->iobase + DMM32AT_AILOW);
-	aihi = inb(dev->iobase + DMM32AT_AIHIGH);
-	fifostat = inb(dev->iobase + DMM32AT_FIFOSTAT);
-	aistat = inb(dev->iobase + DMM32AT_AISTAT);
-	intstat = inb(dev->iobase + DMM32AT_INTCLOCK);
-	airback = inb(dev->iobase + DMM32AT_AIRBACK);
-
-	if ((ailo != 0x00) || (aihi != 0x1f) || (fifostat != 0x80) ||
-	    (aistat != 0x60 || (intstat != 0x00) || airback != 0x0c)) {
-		dev_err(dev->class_dev, "board detection failed\n");
+	ailo = inb(dev->iobase + DMM32AT_AI_LO_CHAN_REG);
+	aihi = inb(dev->iobase + DMM32AT_AI_HI_CHAN_REG);
+	fifostat = inb(dev->iobase + DMM32AT_FIFO_STATUS_REG);
+	aistat = inb(dev->iobase + DMM32AT_AI_STATUS_REG);
+	intstat = inb(dev->iobase + DMM32AT_INTCLK_REG);
+	airback = inb(dev->iobase + DMM32AT_AI_READBACK_REG);
+
+	/*
+	 * NOTE: The (DMM32AT_AI_STATUS_SD1 | DMM32AT_AI_STATUS_SD0)
+	 * test makes this driver only work if the board is configured
+	 * with all A/D channels set for single-ended operation.
+	 */
+	if (ailo != 0x00 || aihi != 0x1f ||
+	    fifostat != DMM32AT_FIFO_STATUS_EF ||
+	    aistat != (DMM32AT_AI_STATUS_SD1 | DMM32AT_AI_STATUS_SD0) ||
+	    intstat != 0x00 || airback != 0x0c)
 		return -EIO;
+
+	return 0;
+}
+
+static int dmm32at_attach(struct comedi_device *dev,
+			  struct comedi_devconfig *it)
+{
+	struct comedi_subdevice *s;
+	int ret;
+
+	ret = comedi_request_region(dev, it->options[0], 0x10);
+	if (ret)
+		return ret;
+
+	ret = dmm32at_reset(dev);
+	if (ret) {
+		dev_err(dev->class_dev, "board detection failed\n");
+		return ret;
 	}
 
 	if (it->options[1]) {
@@ -706,65 +572,45 @@ static int dmm32at_attach(struct comedi_device *dev,
 			dev->irq = it->options[1];
 	}
 
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
-
 	ret = comedi_alloc_subdevices(dev, 3);
 	if (ret)
 		return ret;
 
+	/* Analog Input subdevice */
 	s = &dev->subdevices[0];
-	/* analog input subdevice */
-	s->type = COMEDI_SUBD_AI;
-	/* we support single-ended (ground) and differential */
-	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_DIFF;
-	s->n_chan = 32;
-	s->maxdata = 0xffff;
-	s->range_table = &dmm32at_airanges;
-	s->insn_read = dmm32at_ai_rinsn;
+	s->type		= COMEDI_SUBD_AI;
+	s->subdev_flags	= SDF_READABLE | SDF_GROUND | SDF_DIFF;
+	s->n_chan	= 32;
+	s->maxdata	= 0xffff;
+	s->range_table	= &dmm32at_airanges;
+	s->insn_read	= dmm32at_ai_insn_read;
 	if (dev->irq) {
 		dev->read_subdev = s;
-		s->subdev_flags |= SDF_CMD_READ;
-		s->len_chanlist = 32;
-		s->do_cmd = dmm32at_ai_cmd;
-		s->do_cmdtest = dmm32at_ai_cmdtest;
-		s->cancel = dmm32at_ai_cancel;
+		s->subdev_flags	|= SDF_CMD_READ;
+		s->len_chanlist	= s->n_chan;
+		s->do_cmd	= dmm32at_ai_cmd;
+		s->do_cmdtest	= dmm32at_ai_cmdtest;
+		s->cancel	= dmm32at_ai_cancel;
 	}
 
+	/* Analog Output subdevice */
 	s = &dev->subdevices[1];
-	/* analog output subdevice */
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = 4;
-	s->maxdata = 0x0fff;
-	s->range_table = &dmm32at_aoranges;
-	s->insn_write = dmm32at_ao_insn_write;
-	s->insn_read = comedi_readback_insn_read;
+	s->type		= COMEDI_SUBD_AO;
+	s->subdev_flags	= SDF_WRITABLE;
+	s->n_chan	= 4;
+	s->maxdata	= 0x0fff;
+	s->range_table	= &dmm32at_aoranges;
+	s->insn_write	= dmm32at_ao_insn_write;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
 		return ret;
 
+	/* Digital I/O subdevice */
 	s = &dev->subdevices[2];
-	/* digital i/o subdevice */
-
-	/* get access to the DIO regs */
-	outb(DMM32AT_DIOACC, dev->iobase + DMM32AT_CNTRL);
-	/* set the DIO's to the defualt input setting */
-	devpriv->dio_config = DMM32AT_DIRA | DMM32AT_DIRB |
-		DMM32AT_DIRCL | DMM32AT_DIRCH | DMM32AT_DIENABLE;
-	outb(devpriv->dio_config, dev->iobase + DMM32AT_DIOCONF);
-
-	/* set up the subdevice */
-	s->type = COMEDI_SUBD_DIO;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	s->n_chan = 24;
-	s->maxdata = 1;
-	s->state = 0;
-	s->range_table = &range_digital;
-	s->insn_bits = dmm32at_dio_insn_bits;
-	s->insn_config = dmm32at_dio_insn_config;
+	ret = subdev_8255_init(dev, s, dmm32at_8255_io, DMM32AT_8255_IOBASE);
+	if (ret)
+		return ret;
 
 	return 0;
 }
@@ -778,5 +624,5 @@ static struct comedi_driver dmm32at_driver = {
 module_comedi_driver(dmm32at_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("Comedi: Diamond Systems Diamond-MM-32-AT");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/dt2801.c b/drivers/staging/comedi/drivers/dt2801.c
index e97386343a0e..b96e60ffad73 100644
--- a/drivers/staging/comedi/drivers/dt2801.c
+++ b/drivers/staging/comedi/drivers/dt2801.c
@@ -597,7 +597,6 @@ havetype:
 	devpriv->dac_range_types[0] = dac_range_lkup(it->options[4]);
 	devpriv->dac_range_types[1] = dac_range_lkup(it->options[5]);
 	s->insn_write = dt2801_ao_insn_write;
-	s->insn_read = comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
diff --git a/drivers/staging/comedi/drivers/dt2811.c b/drivers/staging/comedi/drivers/dt2811.c
index 1736e397ad2c..d660f277487e 100644
--- a/drivers/staging/comedi/drivers/dt2811.c
+++ b/drivers/staging/comedi/drivers/dt2811.c
@@ -418,7 +418,6 @@ static int dt2811_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	devpriv->range_type_list[0] = dac_range_types[devpriv->dac_range[0]];
 	devpriv->range_type_list[1] = dac_range_types[devpriv->dac_range[1]];
 	s->insn_write = dt2811_ao_insn_write;
-	s->insn_read = comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
diff --git a/drivers/staging/comedi/drivers/dt2814.c b/drivers/staging/comedi/drivers/dt2814.c
index 9216c35c414e..9805be13005a 100644
--- a/drivers/staging/comedi/drivers/dt2814.c
+++ b/drivers/staging/comedi/drivers/dt2814.c
@@ -230,7 +230,7 @@ static irqreturn_t dt2814_interrupt(int irq, void *d)
 
 		s->async->events |= COMEDI_CB_EOA;
 	}
-	comedi_event(dev, s);
+	comedi_handle_events(dev, s);
 	return IRQ_HANDLED;
 }
 
diff --git a/drivers/staging/comedi/drivers/dt282x.c b/drivers/staging/comedi/drivers/dt282x.c
index cc974a5e5cf6..2be98bb9a809 100644
--- a/drivers/staging/comedi/drivers/dt282x.c
+++ b/drivers/staging/comedi/drivers/dt282x.c
@@ -449,13 +449,29 @@ static void dt282x_munge(struct comedi_device *dev,
 	}
 }
 
+static unsigned int dt282x_ao_setup_dma(struct comedi_device *dev,
+					struct comedi_subdevice *s,
+					int cur_dma)
+{
+	struct dt282x_private *devpriv = dev->private;
+	void *ptr = devpriv->dma[cur_dma].buf;
+	unsigned int nsamples = comedi_bytes_to_samples(s, devpriv->dma_maxsize);
+	unsigned int nbytes;
+
+	nbytes = comedi_buf_read_samples(s, ptr, nsamples);
+	if (nbytes)
+		dt282x_prep_ao_dma(dev, cur_dma, nbytes);
+	else
+		dev_err(dev->class_dev, "AO underrun\n");
+
+	return nbytes;
+}
+
 static void dt282x_ao_dma_interrupt(struct comedi_device *dev,
 				    struct comedi_subdevice *s)
 {
 	struct dt282x_private *devpriv = dev->private;
 	int cur_dma = devpriv->current_dma_index;
-	void *ptr = devpriv->dma[cur_dma].buf;
-	int size;
 
 	outw(devpriv->supcsr | DT2821_SUPCSR_CLRDMADNE,
 	     dev->iobase + DT2821_SUPCSR_REG);
@@ -464,13 +480,8 @@ static void dt282x_ao_dma_interrupt(struct comedi_device *dev,
 
 	devpriv->current_dma_index = 1 - cur_dma;
 
-	size = cfc_read_array_from_buffer(s, ptr, devpriv->dma_maxsize);
-	if (size == 0) {
-		dev_err(dev->class_dev, "AO underrun\n");
+	if (!dt282x_ao_setup_dma(dev, s, cur_dma))
 		s->async->events |= COMEDI_CB_OVERFLOW;
-	} else {
-		dt282x_prep_ao_dma(dev, cur_dma, size);
-	}
 }
 
 static void dt282x_ai_dma_interrupt(struct comedi_device *dev,
@@ -480,6 +491,7 @@ static void dt282x_ai_dma_interrupt(struct comedi_device *dev,
 	int cur_dma = devpriv->current_dma_index;
 	void *ptr = devpriv->dma[cur_dma].buf;
 	int size = devpriv->dma[cur_dma].size;
+	unsigned int nsamples = comedi_bytes_to_samples(s, size);
 	int ret;
 
 	outw(devpriv->supcsr | DT2821_SUPCSR_CLRDMADNE,
@@ -490,13 +502,11 @@ static void dt282x_ai_dma_interrupt(struct comedi_device *dev,
 	devpriv->current_dma_index = 1 - cur_dma;
 
 	dt282x_munge(dev, s, ptr, size);
-	ret = cfc_write_array_to_buffer(s, ptr, size);
-	if (ret != size) {
-		s->async->events |= COMEDI_CB_OVERFLOW;
+	ret = comedi_buf_write_samples(s, ptr, nsamples);
+	if (ret != size)
 		return;
-	}
 
-	devpriv->nread -= size / 2;
+	devpriv->nread -= nsamples;
 	if (devpriv->nread < 0) {
 		dev_info(dev->class_dev, "nread off by one\n");
 		devpriv->nread = 0;
@@ -555,7 +565,6 @@ static irqreturn_t dt282x_interrupt(int irq, void *d)
 	}
 #if 0
 	if (adcsr & DT2821_ADCSR_ADDONE) {
-		int ret;
 		unsigned short data;
 
 		data = inw(dev->iobase + DT2821_ADDAT_REG);
@@ -563,10 +572,7 @@ static irqreturn_t dt282x_interrupt(int irq, void *d)
 		if (devpriv->ad_2scomp)
 			data = comedi_offset_munge(s, data);
 
-		ret = comedi_buf_put(s, data);
-
-		if (ret == 0)
-			s->async->events |= COMEDI_CB_OVERFLOW;
+		comedi_buf_write_samples(s, &data, 1);
 
 		devpriv->nread--;
 		if (!devpriv->nread) {
@@ -579,8 +585,8 @@ static irqreturn_t dt282x_interrupt(int irq, void *d)
 		handled = 1;
 	}
 #endif
-	cfc_handle_events(dev, s);
-	cfc_handle_events(dev, s_ao);
+	comedi_handle_events(dev, s);
+	comedi_handle_events(dev, s_ao);
 
 	return IRQ_RETVAL(handled);
 }
@@ -916,26 +922,15 @@ static int dt282x_ao_inttrig(struct comedi_device *dev,
 {
 	struct dt282x_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
-	int size;
 
 	if (trig_num != cmd->start_src)
 		return -EINVAL;
 
-	size = cfc_read_array_from_buffer(s, devpriv->dma[0].buf,
-					  devpriv->dma_maxsize);
-	if (size == 0) {
-		dev_err(dev->class_dev, "AO underrun\n");
+	if (!dt282x_ao_setup_dma(dev, s, 0))
 		return -EPIPE;
-	}
-	dt282x_prep_ao_dma(dev, 0, size);
 
-	size = cfc_read_array_from_buffer(s, devpriv->dma[1].buf,
-					  devpriv->dma_maxsize);
-	if (size == 0) {
-		dev_err(dev->class_dev, "AO underrun\n");
+	if (!dt282x_ao_setup_dma(dev, s, 1))
 		return -EPIPE;
-	}
-	dt282x_prep_ao_dma(dev, 1, size);
 
 	outw(devpriv->supcsr | DT2821_SUPCSR_STRIG,
 	     dev->iobase + DT2821_SUPCSR_REG);
@@ -1236,7 +1231,6 @@ static int dt282x_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		/* ranges are per-channel, set by jumpers on the board */
 		s->range_table	= &dt282x_ao_range;
 		s->insn_write	= dt282x_ao_insn_write;
-		s->insn_read	= comedi_readback_insn_read;
 		if (dev->irq) {
 			dev->write_subdev = s;
 			s->subdev_flags	|= SDF_CMD_WRITE;
diff --git a/drivers/staging/comedi/drivers/dt3000.c b/drivers/staging/comedi/drivers/dt3000.c
index 825561046b6f..1d9a7a63e06f 100644
--- a/drivers/staging/comedi/drivers/dt3000.c
+++ b/drivers/staging/comedi/drivers/dt3000.c
@@ -315,7 +315,7 @@ static void dt3k_ai_empty_fifo(struct comedi_device *dev,
 
 	for (i = 0; i < count; i++) {
 		data = readw(dev->mmio + DPR_ADC_buffer + rear);
-		comedi_buf_put(s, data);
+		comedi_buf_write_samples(s, &data, 1);
 		rear++;
 		if (rear >= AI_FIFO_DEPTH)
 			rear = 0;
@@ -351,10 +351,8 @@ static irqreturn_t dt3k_interrupt(int irq, void *d)
 
 	status = readw(dev->mmio + DPR_Intr_Flag);
 
-	if (status & DT3000_ADFULL) {
+	if (status & DT3000_ADFULL)
 		dt3k_ai_empty_fifo(dev, s);
-		s->async->events |= COMEDI_CB_BLOCK;
-	}
 
 	if (status & (DT3000_ADSWERR | DT3000_ADHWERR))
 		s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
@@ -363,7 +361,7 @@ static irqreturn_t dt3k_interrupt(int irq, void *d)
 	if (debug_n_ints >= 10)
 		s->async->events |= COMEDI_CB_EOA;
 
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 	return IRQ_HANDLED;
 }
 
@@ -699,7 +697,6 @@ static int dt3000_auto_attach(struct comedi_device *dev,
 	s->len_chanlist	= 1;
 	s->range_table	= &range_bipolar10;
 	s->insn_write	= dt3k_ao_insn_write;
-	s->insn_read	= comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
diff --git a/drivers/staging/comedi/drivers/dt9812.c b/drivers/staging/comedi/drivers/dt9812.c
index 77bb89fee327..06c601d8fdff 100644
--- a/drivers/staging/comedi/drivers/dt9812.c
+++ b/drivers/staging/comedi/drivers/dt9812.c
@@ -804,7 +804,7 @@ static int dt9812_auto_attach(struct comedi_device *dev,
 	/* Digital Output subdevice */
 	s = &dev->subdevices[1];
 	s->type		= COMEDI_SUBD_DO;
-	s->subdev_flags	= SDF_WRITEABLE;
+	s->subdev_flags	= SDF_WRITABLE;
 	s->n_chan	= 8;
 	s->maxdata	= 1;
 	s->range_table	= &range_digital;
@@ -822,7 +822,7 @@ static int dt9812_auto_attach(struct comedi_device *dev,
 	/* Analog Output subdevice */
 	s = &dev->subdevices[3];
 	s->type		= COMEDI_SUBD_AO;
-	s->subdev_flags	= SDF_WRITEABLE;
+	s->subdev_flags	= SDF_WRITABLE;
 	s->n_chan	= 2;
 	s->maxdata	= 0x0fff;
 	s->range_table	= is_unipolar ? &range_unipolar2_5 : &range_bipolar10;
diff --git a/drivers/staging/comedi/drivers/dyna_pci10xx.c b/drivers/staging/comedi/drivers/dyna_pci10xx.c
index 608aee0c3a15..1b6324c6eb29 100644
--- a/drivers/staging/comedi/drivers/dyna_pci10xx.c
+++ b/drivers/staging/comedi/drivers/dyna_pci10xx.c
@@ -218,7 +218,7 @@ static int dyna_pci10xx_auto_attach(struct comedi_device *dev,
 	/* digital input */
 	s = &dev->subdevices[2];
 	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE | SDF_GROUND;
+	s->subdev_flags = SDF_READABLE;
 	s->n_chan = 16;
 	s->maxdata = 1;
 	s->range_table = &range_digital;
@@ -228,7 +228,7 @@ static int dyna_pci10xx_auto_attach(struct comedi_device *dev,
 	/* digital output */
 	s = &dev->subdevices[3];
 	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_WRITABLE | SDF_GROUND;
+	s->subdev_flags = SDF_WRITABLE;
 	s->n_chan = 16;
 	s->maxdata = 1;
 	s->range_table = &range_digital;
diff --git a/drivers/staging/comedi/drivers/fl512.c b/drivers/staging/comedi/drivers/fl512.c
index 5a1e3c8fc01c..e1f493241cd6 100644
--- a/drivers/staging/comedi/drivers/fl512.c
+++ b/drivers/staging/comedi/drivers/fl512.c
@@ -135,7 +135,6 @@ static int fl512_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->maxdata	= 0x0fff;
 	s->range_table	= &range_fl512;
 	s->insn_write	= fl512_ao_insn_write;
-	s->insn_read	= comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
diff --git a/drivers/staging/comedi/drivers/gsc_hpdi.c b/drivers/staging/comedi/drivers/gsc_hpdi.c
index b8975a4606ea..0979f536ed39 100644
--- a/drivers/staging/comedi/drivers/gsc_hpdi.c
+++ b/drivers/staging/comedi/drivers/gsc_hpdi.c
@@ -196,8 +196,8 @@ static void gsc_hpdi_drain_dma(struct comedi_device *dev, unsigned int channel)
 				size = devpriv->dio_count;
 			devpriv->dio_count -= size;
 		}
-		cfc_write_array_to_buffer(s, devpriv->desc_dio_buffer[idx],
-					  size * sizeof(uint32_t));
+		comedi_buf_write_samples(s, devpriv->desc_dio_buffer[idx],
+					 size);
 		idx++;
 		idx %= devpriv->num_dma_descriptors;
 		start = le32_to_cpu(devpriv->dma_desc[idx].pci_start_addr);
@@ -272,7 +272,7 @@ static irqreturn_t gsc_hpdi_interrupt(int irq, void *d)
 	if (devpriv->dio_count == 0)
 		async->events |= COMEDI_CB_EOA;
 
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 
 	return IRQ_HANDLED;
 }
@@ -689,7 +689,7 @@ static int gsc_hpdi_auto_attach(struct comedi_device *dev,
 	s = &dev->subdevices[0];
 	dev->read_subdev = s;
 	s->type		= COMEDI_SUBD_DIO;
-	s->subdev_flags	= SDF_READABLE | SDF_WRITEABLE | SDF_LSAMPL |
+	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE | SDF_LSAMPL |
 			  SDF_CMD_READ;
 	s->n_chan	= 32;
 	s->len_chanlist	= 32;
diff --git a/drivers/staging/comedi/drivers/icp_multi.c b/drivers/staging/comedi/drivers/icp_multi.c
index f4e1c1cf4178..1ea168620103 100644
--- a/drivers/staging/comedi/drivers/icp_multi.c
+++ b/drivers/staging/comedi/drivers/icp_multi.c
@@ -53,7 +53,7 @@ Configuration options: not applicable, uses PCI auto config
 #define ICP_MULTI_AI		2	/* R:   Analogue input data */
 #define ICP_MULTI_DAC_CSR	4	/* R/W: DAC command/status register */
 #define ICP_MULTI_AO		6	/* R/W: Analogue output data */
-#define ICP_MULTI_DI		8	/* R/W: Digital inouts */
+#define ICP_MULTI_DI		8	/* R/W: Digital inputs */
 #define ICP_MULTI_DO		0x0A	/* R/W: Digital outputs */
 #define ICP_MULTI_INT_EN	0x0C	/* R/W: Interrupt enable register */
 #define ICP_MULTI_INT_STAT	0x0E	/* R/W: Interrupt status register */
@@ -319,7 +319,7 @@ static int icp_multi_insn_bits_do(struct comedi_device *dev,
 	if (comedi_dio_update_state(s, data))
 		writew(s->state, dev->mmio + ICP_MULTI_DO);
 
-	data[1] = readw(dev->mmio + ICP_MULTI_DI);
+	data[1] = s->state;
 
 	return insn->n;
 }
@@ -495,7 +495,6 @@ static int icp_multi_auto_attach(struct comedi_device *dev,
 	s->len_chanlist = 4;
 	s->range_table = &range_analog;
 	s->insn_write = icp_multi_ao_insn_write;
-	s->insn_read = comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
@@ -512,7 +511,7 @@ static int icp_multi_auto_attach(struct comedi_device *dev,
 
 	s = &dev->subdevices[3];
 	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->subdev_flags = SDF_WRITABLE;
 	s->n_chan = 8;
 	s->maxdata = 1;
 	s->len_chanlist = 8;
@@ -521,7 +520,7 @@ static int icp_multi_auto_attach(struct comedi_device *dev,
 
 	s = &dev->subdevices[4];
 	s->type = COMEDI_SUBD_COUNTER;
-	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
+	s->subdev_flags = SDF_WRITABLE;
 	s->n_chan = 4;
 	s->maxdata = 0xffff;
 	s->len_chanlist = 4;
diff --git a/drivers/staging/comedi/drivers/ii_pci20kc.c b/drivers/staging/comedi/drivers/ii_pci20kc.c
index cc5fd75b8bc0..1085d66935fe 100644
--- a/drivers/staging/comedi/drivers/ii_pci20kc.c
+++ b/drivers/staging/comedi/drivers/ii_pci20kc.c
@@ -392,7 +392,6 @@ static int ii20k_init_module(struct comedi_device *dev,
 		s->maxdata	= 0xffff;
 		s->range_table	= &ii20k_ao_ranges;
 		s->insn_write	= ii20k_ao_insn_write;
-		s->insn_read	= comedi_readback_insn_read;
 
 		ret = comedi_alloc_subdev_readback(s);
 		if (ret)
diff --git a/drivers/staging/comedi/drivers/me4000.c b/drivers/staging/comedi/drivers/me4000.c
index 6561b00bea59..915685c1c85c 100644
--- a/drivers/staging/comedi/drivers/me4000.c
+++ b/drivers/staging/comedi/drivers/me4000.c
@@ -44,8 +44,6 @@ broken.
 #include <linux/pci.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
-#include <linux/list.h>
-#include <linux/spinlock.h>
 
 #include "../comedidev.h"
 
@@ -53,10 +51,7 @@ broken.
 #include "8253.h"
 #include "plx9052.h"
 
-#if 0
-/* file removed due to GPL incompatibility */
-#include "me4000_fw.h"
-#endif
+#define ME4000_FIRMWARE		"me4000_firmware.bin"
 
 /*
  * ME4000 Register map and bit defines
@@ -333,27 +328,20 @@ static const struct comedi_lrange me4000_ai_range = {
 	}
 };
 
-#define FIRMWARE_NOT_AVAILABLE 1
-#if FIRMWARE_NOT_AVAILABLE
-extern unsigned char *xilinx_firm;
-#endif
-
-static int xilinx_download(struct comedi_device *dev)
+static int me4000_xilinx_download(struct comedi_device *dev,
+				  const u8 *data, size_t size,
+				  unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct me4000_info *info = dev->private;
 	unsigned long xilinx_iobase = pci_resource_start(pcidev, 5);
-	u32 value = 0;
-	wait_queue_head_t queue;
-	int idx = 0;
-	int size = 0;
-	unsigned int intcsr;
+	unsigned int file_length;
+	unsigned int val;
+	unsigned int i;
 
 	if (!xilinx_iobase)
 		return -ENODEV;
 
-	init_waitqueue_head(&queue);
-
 	/*
 	 * Set PLX local interrupt 2 polarity to high.
 	 * Interrupt is thrown by init pin of xilinx.
@@ -361,61 +349,58 @@ static int xilinx_download(struct comedi_device *dev)
 	outl(PLX9052_INTCSR_LI2POL, info->plx_regbase + PLX9052_INTCSR);
 
 	/* Set /CS and /WRITE of the Xilinx */
-	value = inl(info->plx_regbase + PLX9052_CNTRL);
-	value |= PLX9052_CNTRL_UIO2_DATA;
-	outl(value, info->plx_regbase + PLX9052_CNTRL);
+	val = inl(info->plx_regbase + PLX9052_CNTRL);
+	val |= PLX9052_CNTRL_UIO2_DATA;
+	outl(val, info->plx_regbase + PLX9052_CNTRL);
 
 	/* Init Xilinx with CS1 */
 	inb(xilinx_iobase + 0xC8);
 
 	/* Wait until /INIT pin is set */
 	udelay(20);
-	intcsr = inl(info->plx_regbase + PLX9052_INTCSR);
-	if (!(intcsr & PLX9052_INTCSR_LI2STAT)) {
+	val = inl(info->plx_regbase + PLX9052_INTCSR);
+	if (!(val & PLX9052_INTCSR_LI2STAT)) {
 		dev_err(dev->class_dev, "Can't init Xilinx\n");
 		return -EIO;
 	}
 
 	/* Reset /CS and /WRITE of the Xilinx */
-	value = inl(info->plx_regbase + PLX9052_CNTRL);
-	value &= ~PLX9052_CNTRL_UIO2_DATA;
-	outl(value, info->plx_regbase + PLX9052_CNTRL);
-	if (FIRMWARE_NOT_AVAILABLE) {
-		dev_err(dev->class_dev,
-			"xilinx firmware unavailable due to licensing, aborting");
-		return -EIO;
-	} else {
-		/* Download Xilinx firmware */
-		size = (xilinx_firm[0] << 24) + (xilinx_firm[1] << 16) +
-		    (xilinx_firm[2] << 8) + xilinx_firm[3];
-		udelay(10);
+	val = inl(info->plx_regbase + PLX9052_CNTRL);
+	val &= ~PLX9052_CNTRL_UIO2_DATA;
+	outl(val, info->plx_regbase + PLX9052_CNTRL);
 
-		for (idx = 0; idx < size; idx++) {
-			outb(xilinx_firm[16 + idx], xilinx_iobase);
-			udelay(10);
+	/* Download Xilinx firmware */
+	file_length = (((unsigned int)data[0] & 0xff) << 24) +
+		      (((unsigned int)data[1] & 0xff) << 16) +
+		      (((unsigned int)data[2] & 0xff) << 8) +
+		      ((unsigned int)data[3] & 0xff);
+	udelay(10);
 
-			/* Check if BUSY flag is low */
-			if (inl(info->plx_regbase + PLX9052_CNTRL) & PLX9052_CNTRL_UIO1_DATA) {
-				dev_err(dev->class_dev,
-					"Xilinx is still busy (idx = %d)\n",
-					idx);
-				return -EIO;
-			}
+	for (i = 0; i < file_length; i++) {
+		outb(data[16 + i], xilinx_iobase);
+		udelay(10);
+
+		/* Check if BUSY flag is low */
+		val = inl(info->plx_regbase + PLX9052_CNTRL);
+		if (val & PLX9052_CNTRL_UIO1_DATA) {
+			dev_err(dev->class_dev,
+				"Xilinx is still busy (i = %d)\n", i);
+			return -EIO;
 		}
 	}
 
 	/* If done flag is high download was successful */
-	if (inl(info->plx_regbase + PLX9052_CNTRL) & PLX9052_CNTRL_UIO0_DATA) {
-	} else {
+	val = inl(info->plx_regbase + PLX9052_CNTRL);
+	if (!(val & PLX9052_CNTRL_UIO0_DATA)) {
 		dev_err(dev->class_dev, "DONE flag is not set\n");
 		dev_err(dev->class_dev, "Download not successful\n");
 		return -EIO;
 	}
 
 	/* Set /CS and /WRITE */
-	value = inl(info->plx_regbase + PLX9052_CNTRL);
-	value |= PLX9052_CNTRL_UIO2_DATA;
-	outl(value, info->plx_regbase + PLX9052_CNTRL);
+	val = inl(info->plx_regbase + PLX9052_CNTRL);
+	val |= PLX9052_CNTRL_UIO2_DATA;
+	outl(val, info->plx_regbase + PLX9052_CNTRL);
 
 	return 0;
 }
@@ -431,7 +416,7 @@ static void me4000_reset(struct comedi_device *dev)
 	val |= PLX9052_CNTRL_PCI_RESET;
 	outl(val, info->plx_regbase + PLX9052_CNTRL);
 	val &= ~PLX9052_CNTRL_PCI_RESET;
-	outl(val , info->plx_regbase + PLX9052_CNTRL);
+	outl(val, info->plx_regbase + PLX9052_CNTRL);
 
 	/* 0x8000 to the DACs means an output voltage of 0V */
 	for (chan = 0; chan < 4; chan++)
@@ -848,9 +833,6 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 	unsigned int scan_ticks;
 	int err = 0;
 
-	/* Only rounding flags are implemented */
-	cmd->flags &= CMDF_ROUND_NEAREST | CMDF_ROUND_UP | CMDF_ROUND_DOWN;
-
 	/* Round the timer arguments */
 	ai_round_cmd_args(dev, s, cmd, &init_ticks, &scan_ticks, &chan_ticks);
 
@@ -1092,8 +1074,6 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 		} else if ((tmp & ME4000_AI_STATUS_BIT_FF_DATA)
 			   && !(tmp & ME4000_AI_STATUS_BIT_HF_DATA)
 			   && (tmp & ME4000_AI_STATUS_BIT_EF_DATA)) {
-			s->async->events |= COMEDI_CB_BLOCK;
-
 			c = ME4000_AI_FIFO_COUNT / 2;
 		} else {
 			dev_err(dev->class_dev,
@@ -1119,7 +1099,7 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 			lval = inl(dev->iobase + ME4000_AI_DATA_REG) & 0xFFFF;
 			lval ^= 0x8000;
 
-			if (!comedi_buf_put(s, lval)) {
+			if (!comedi_buf_write_samples(s, &lval, 1)) {
 				/*
 				 * Buffer overflow, so stop conversion
 				 * and disable all interrupts
@@ -1128,11 +1108,6 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 				tmp &= ~(ME4000_AI_CTRL_BIT_HF_IRQ |
 					 ME4000_AI_CTRL_BIT_SC_IRQ);
 				outl(tmp, dev->iobase + ME4000_AI_CTRL_REG);
-
-				s->async->events |= COMEDI_CB_OVERFLOW;
-
-				dev_err(dev->class_dev, "Buffer overflow\n");
-
 				break;
 			}
 		}
@@ -1146,7 +1121,7 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 
 	if (inl(dev->iobase + ME4000_IRQ_STATUS_REG) &
 	    ME4000_IRQ_STATUS_BIT_SC) {
-		s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOA;
+		s->async->events |= COMEDI_CB_EOA;
 
 		/*
 		 * Acquisition is complete, so stop
@@ -1164,11 +1139,8 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 			lval = inl(dev->iobase + ME4000_AI_DATA_REG) & 0xFFFF;
 			lval ^= 0x8000;
 
-			if (!comedi_buf_put(s, lval)) {
-				dev_err(dev->class_dev, "Buffer overflow\n");
-				s->async->events |= COMEDI_CB_OVERFLOW;
+			if (!comedi_buf_write_samples(s, &lval, 1))
 				break;
-			}
 		}
 
 		/* Work is done, so reset the interrupt */
@@ -1178,8 +1150,7 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 		outl(tmp, dev->iobase + ME4000_AI_CTRL_REG);
 	}
 
-	if (s->async->events)
-		comedi_event(dev, s);
+	comedi_handle_events(dev, s);
 
 	return IRQ_HANDLED;
 }
@@ -1397,8 +1368,9 @@ static int me4000_auto_attach(struct comedi_device *dev,
 	if (!info->plx_regbase || !dev->iobase || !info->timer_regbase)
 		return -ENODEV;
 
-	result = xilinx_download(dev);
-	if (result)
+	result = comedi_load_firmware(dev, &pcidev->dev, ME4000_FIRMWARE,
+				      me4000_xilinx_download, 0);
+	if (result < 0)
 		return result;
 
 	me4000_reset(dev);
@@ -1449,12 +1421,11 @@ static int me4000_auto_attach(struct comedi_device *dev,
 
 	if (thisboard->ao_nchan) {
 		s->type = COMEDI_SUBD_AO;
-		s->subdev_flags = SDF_WRITEABLE | SDF_COMMON | SDF_GROUND;
+		s->subdev_flags = SDF_WRITABLE | SDF_COMMON | SDF_GROUND;
 		s->n_chan = thisboard->ao_nchan;
 		s->maxdata = 0xFFFF;	/*  16 bit DAC */
 		s->range_table = &range_bipolar10;
 		s->insn_write = me4000_ao_insn_write;
-		s->insn_read = comedi_readback_insn_read;
 
 		result = comedi_alloc_subdev_readback(s);
 		if (result)
@@ -1561,3 +1532,4 @@ module_comedi_pci_driver(me4000_driver, me4000_pci_driver);
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
+MODULE_FIRMWARE(ME4000_FIRMWARE);
diff --git a/drivers/staging/comedi/drivers/me_daq.c b/drivers/staging/comedi/drivers/me_daq.c
index 00eaaf8ac148..b5278c11e622 100644
--- a/drivers/staging/comedi/drivers/me_daq.c
+++ b/drivers/staging/comedi/drivers/me_daq.c
@@ -511,13 +511,12 @@ static int me_auto_attach(struct comedi_device *dev,
 	s = &dev->subdevices[1];
 	if (board->has_ao) {
 		s->type		= COMEDI_SUBD_AO;
-		s->subdev_flags	= SDF_WRITEABLE | SDF_COMMON;
+		s->subdev_flags	= SDF_WRITABLE | SDF_COMMON;
 		s->n_chan	= 4;
 		s->maxdata	= 0x0fff;
 		s->len_chanlist	= 4;
 		s->range_table	= &me_ao_range;
 		s->insn_write	= me_ao_insn_write;
-		s->insn_read	= comedi_readback_insn_read;
 
 		ret = comedi_alloc_subdev_readback(s);
 		if (ret)
@@ -528,7 +527,7 @@ static int me_auto_attach(struct comedi_device *dev,
 
 	s = &dev->subdevices[2];
 	s->type		= COMEDI_SUBD_DIO;
-	s->subdev_flags	= SDF_READABLE | SDF_WRITEABLE;
+	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
 	s->n_chan	= 32;
 	s->maxdata	= 1;
 	s->len_chanlist	= 32;
diff --git a/drivers/staging/comedi/drivers/mf6x4.c b/drivers/staging/comedi/drivers/mf6x4.c
index c8d3a22c5896..af21bc180c46 100644
--- a/drivers/staging/comedi/drivers/mf6x4.c
+++ b/drivers/staging/comedi/drivers/mf6x4.c
@@ -259,7 +259,6 @@ static int mf6x4_auto_attach(struct comedi_device *dev, unsigned long context)
 	s->maxdata = 0x3fff; /* 14 bits DAC */
 	s->range_table = &range_bipolar10;
 	s->insn_write = mf6x4_ao_insn_write;
-	s->insn_read = comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
diff --git a/drivers/staging/comedi/drivers/mite.c b/drivers/staging/comedi/drivers/mite.c
index 4f7829010a99..ffc9e61d6cdd 100644
--- a/drivers/staging/comedi/drivers/mite.c
+++ b/drivers/staging/comedi/drivers/mite.c
@@ -494,13 +494,10 @@ EXPORT_SYMBOL_GPL(mite_bytes_read_from_memory_ub);
 unsigned mite_dma_tcr(struct mite_channel *mite_chan)
 {
 	struct mite_struct *mite = mite_chan->mite;
-	int tcr;
 	int lkar;
 
 	lkar = readl(mite->mite_io_addr + MITE_LKAR(mite_chan->channel));
-	tcr = readl(mite->mite_io_addr + MITE_TCR(mite_chan->channel));
-
-	return tcr;
+	return readl(mite->mite_io_addr + MITE_TCR(mite_chan->channel));
 }
 EXPORT_SYMBOL_GPL(mite_dma_tcr);
 
@@ -542,7 +539,7 @@ int mite_sync_input_dma(struct mite_channel *mite_chan,
 		return 0;
 
 	comedi_buf_write_free(s, count);
-	cfc_inc_scan_progress(s, count);
+	comedi_inc_scan_progress(s, count);
 	async->events |= COMEDI_CB_BLOCK;
 	return 0;
 }
@@ -553,7 +550,7 @@ int mite_sync_output_dma(struct mite_channel *mite_chan,
 {
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
-	u32 stop_count = cmd->stop_arg * cfc_bytes_per_scan(s);
+	u32 stop_count = cmd->stop_arg * comedi_bytes_per_scan(s);
 	unsigned int old_alloc_count = async->buf_read_alloc_count;
 	u32 nbytes_ub, nbytes_lb;
 	int count;
diff --git a/drivers/staging/comedi/drivers/multiq3.c b/drivers/staging/comedi/drivers/multiq3.c
index f710c8e81320..8471219210b6 100644
--- a/drivers/staging/comedi/drivers/multiq3.c
+++ b/drivers/staging/comedi/drivers/multiq3.c
@@ -238,7 +238,6 @@ static int multiq3_attach(struct comedi_device *dev,
 	s->maxdata = 0xfff;
 	s->range_table = &range_bipolar5;
 	s->insn_write = multiq3_ao_insn_write;
-	s->insn_read = comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
diff --git a/drivers/staging/comedi/drivers/ni_6527.c b/drivers/staging/comedi/drivers/ni_6527.c
index 45fb601e4080..f99847f3999f 100644
--- a/drivers/staging/comedi/drivers/ni_6527.c
+++ b/drivers/staging/comedi/drivers/ni_6527.c
@@ -208,9 +208,8 @@ static irqreturn_t ni6527_interrupt(int irq, void *d)
 		return IRQ_NONE;
 
 	if (status & NI6527_STATUS_EDGE) {
-		comedi_buf_put(s, 0);
-		s->async->events |= COMEDI_CB_EOS;
-		comedi_event(dev, s);
+		comedi_buf_write_samples(s, &s->state, 1);
+		comedi_handle_events(dev, s);
 	}
 
 	writeb(NI6527_CLR_IRQS, dev->mmio + NI6527_CLR_REG);
@@ -238,9 +237,6 @@ static int ni6527_intr_cmdtest(struct comedi_device *dev,
 	/* Step 2a : make sure trigger sources are unique */
 	/* Step 2b : and mutually compatible */
 
-	if (err)
-		return 2;
-
 	/* Step 3: check if arguments are trivially valid */
 
 	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
diff --git a/drivers/staging/comedi/drivers/ni_65xx.c b/drivers/staging/comedi/drivers/ni_65xx.c
index 3b642861eb36..bcb326e31562 100644
--- a/drivers/staging/comedi/drivers/ni_65xx.c
+++ b/drivers/staging/comedi/drivers/ni_65xx.c
@@ -508,9 +508,9 @@ static irqreturn_t ni_65xx_interrupt(int irq, void *d)
 	writeb(NI_65XX_CLR_EDGE_INT | NI_65XX_CLR_OVERFLOW_INT,
 	       dev->mmio + NI_65XX_CLR_REG);
 
-	comedi_buf_put(s, 0);
-	s->async->events |= COMEDI_CB_EOS;
-	comedi_event(dev, s);
+	comedi_buf_write_samples(s, &s->state, 1);
+	comedi_handle_events(dev, s);
+
 	return IRQ_HANDLED;
 }
 
@@ -534,9 +534,6 @@ static int ni_65xx_intr_cmdtest(struct comedi_device *dev,
 	/* Step 2a : make sure trigger sources are unique */
 	/* Step 2b : and mutually compatible */
 
-	if (err)
-		return 2;
-
 	/* Step 3: check if arguments are trivially valid */
 
 	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
diff --git a/drivers/staging/comedi/drivers/ni_660x.c b/drivers/staging/comedi/drivers/ni_660x.c
index 5b6794c8232e..1e4dd82b12ea 100644
--- a/drivers/staging/comedi/drivers/ni_660x.c
+++ b/drivers/staging/comedi/drivers/ni_660x.c
@@ -780,7 +780,7 @@ static void ni_660x_handle_gpct_interrupt(struct comedi_device *dev,
 	struct ni_gpct *counter = s->private;
 
 	ni_tio_handle_interrupt(counter, s);
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 }
 
 static irqreturn_t ni_660x_interrupt(int irq, void *d)
diff --git a/drivers/staging/comedi/drivers/ni_670x.c b/drivers/staging/comedi/drivers/ni_670x.c
index 54721deb80cc..c42a81c0bfa1 100644
--- a/drivers/staging/comedi/drivers/ni_670x.c
+++ b/drivers/staging/comedi/drivers/ni_670x.c
@@ -228,7 +228,6 @@ static int ni_670x_auto_attach(struct comedi_device *dev,
 		s->range_table = &range_bipolar10;
 	}
 	s->insn_write = ni_670x_ao_insn_write;
-	s->insn_read = comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
diff --git a/drivers/staging/comedi/drivers/ni_at_a2150.c b/drivers/staging/comedi/drivers/ni_at_a2150.c
index 72ec857d073e..69e543a0bf22 100644
--- a/drivers/staging/comedi/drivers/ni_at_a2150.c
+++ b/drivers/staging/comedi/drivers/ni_at_a2150.c
@@ -168,7 +168,6 @@ static irqreturn_t a2150_interrupt(int irq, void *d)
 	struct comedi_cmd *cmd;
 	unsigned int max_points, num_points, residue, leftover;
 	unsigned short dpnt;
-	static const int sample_size = sizeof(devpriv->dma_buffer[0]);
 
 	if (!dev->attached) {
 		dev_err(dev->class_dev, "premature interrupt\n");
@@ -188,14 +187,14 @@ static irqreturn_t a2150_interrupt(int irq, void *d)
 	if (status & OVFL_BIT) {
 		dev_err(dev->class_dev, "fifo overflow\n");
 		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
-		cfc_handle_events(dev, s);
+		comedi_handle_events(dev, s);
 	}
 
 	if ((status & DMA_TC_BIT) == 0) {
 		dev_err(dev->class_dev,
 			"caught non-dma interrupt?  Aborting.\n");
 		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
-		cfc_handle_events(dev, s);
+		comedi_handle_events(dev, s);
 		return IRQ_HANDLED;
 	}
 
@@ -206,12 +205,12 @@ static irqreturn_t a2150_interrupt(int irq, void *d)
 	clear_dma_ff(devpriv->dma);
 
 	/*  figure out how many points to read */
-	max_points = devpriv->dma_transfer_size / sample_size;
+	max_points = comedi_bytes_to_samples(s, devpriv->dma_transfer_size);
 	/* residue is the number of points left to be done on the dma
 	 * transfer.  It should always be zero at this point unless
 	 * the stop_src is set to external triggering.
 	 */
-	residue = get_dma_residue(devpriv->dma) / sample_size;
+	residue = comedi_bytes_to_samples(s, get_dma_residue(devpriv->dma));
 	num_points = max_points - residue;
 	if (devpriv->count < num_points && cmd->stop_src == TRIG_COUNT)
 		num_points = devpriv->count;
@@ -219,7 +218,8 @@ static irqreturn_t a2150_interrupt(int irq, void *d)
 	/*  figure out how many points will be stored next time */
 	leftover = 0;
 	if (cmd->stop_src == TRIG_NONE) {
-		leftover = devpriv->dma_transfer_size / sample_size;
+		leftover = comedi_bytes_to_samples(s,
+						   devpriv->dma_transfer_size);
 	} else if (devpriv->count > max_points) {
 		leftover = devpriv->count - max_points;
 		if (leftover > max_points)
@@ -237,7 +237,7 @@ static irqreturn_t a2150_interrupt(int irq, void *d)
 		dpnt = devpriv->dma_buffer[i];
 		/*  convert from 2's complement to unsigned coding */
 		dpnt ^= 0x8000;
-		cfc_write_to_buffer(s, dpnt);
+		comedi_buf_write_samples(s, &dpnt, 1);
 		if (cmd->stop_src == TRIG_COUNT) {
 			if (--devpriv->count == 0) {	/* end of acquisition */
 				async->events |= COMEDI_CB_EOA;
@@ -248,14 +248,13 @@ static irqreturn_t a2150_interrupt(int irq, void *d)
 	/*  re-enable  dma */
 	if (leftover) {
 		set_dma_addr(devpriv->dma, virt_to_bus(devpriv->dma_buffer));
-		set_dma_count(devpriv->dma, leftover * sample_size);
+		set_dma_count(devpriv->dma,
+			      comedi_samples_to_bytes(s, leftover));
 		enable_dma(devpriv->dma);
 	}
 	release_dma_lock(flags);
 
-	async->events |= COMEDI_CB_BLOCK;
-
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 
 	/* clear interrupt */
 	outw(0x00, dev->iobase + DMA_TC_CLEAR_REG);
diff --git a/drivers/staging/comedi/drivers/ni_at_ao.c b/drivers/staging/comedi/drivers/ni_at_ao.c
index 3e1ce5866147..05370a4a74a5 100644
--- a/drivers/staging/comedi/drivers/ni_at_ao.c
+++ b/drivers/staging/comedi/drivers/ni_at_ao.c
@@ -244,47 +244,31 @@ static int atao_calib_insn_write(struct comedi_device *dev,
 				 struct comedi_insn *insn,
 				 unsigned int *data)
 {
-	struct atao_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int bitstring;
-	unsigned int val;
-	int bit;
 
-	if (insn->n == 0)
-		return 0;
+	if (insn->n) {
+		unsigned int val = data[insn->n - 1];
+		unsigned int bitstring = ((chan & 0x7) << 8) | val;
+		unsigned int bits;
+		int bit;
 
-	devpriv->caldac[chan] = data[insn->n - 1] & s->maxdata;
+		/* write the channel and last data value to the caldac */
+		/* clock the bitstring to the caldac; MSB -> LSB */
+		for (bit = 1 << 10; bit; bit >>= 1) {
+			bits = (bit & bitstring) ? ATAO_CFG2_SDATA : 0;
 
-	/* write the channel and last data value to the caldac */
-	bitstring = ((chan & 0x7) << 8) | devpriv->caldac[chan];
+			outw(bits, dev->iobase + ATAO_CFG2_REG);
+			outw(bits | ATAO_CFG2_SCLK,
+			     dev->iobase + ATAO_CFG2_REG);
+		}
 
-	/* clock the bitstring to the caldac; MSB -> LSB */
-	for (bit = 1 << 10; bit; bit >>= 1) {
-		val = (bit & bitstring) ? ATAO_CFG2_SDATA : 0;
+		/* strobe the caldac to load the value */
+		outw(ATAO_CFG2_CALLD(chan), dev->iobase + ATAO_CFG2_REG);
+		outw(ATAO_CFG2_CALLD_NOP, dev->iobase + ATAO_CFG2_REG);
 
-		outw(val, dev->iobase + ATAO_CFG2_REG);
-		outw(val | ATAO_CFG2_SCLK, dev->iobase + ATAO_CFG2_REG);
+		s->readback[chan] = val;
 	}
 
-	/* strobe the caldac to load the value */
-	outw(ATAO_CFG2_CALLD(chan), dev->iobase + ATAO_CFG2_REG);
-	outw(ATAO_CFG2_CALLD_NOP, dev->iobase + ATAO_CFG2_REG);
-
-	return insn->n;
-}
-
-static int atao_calib_insn_read(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn,
-				unsigned int *data)
-{
-	struct atao_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->caldac[chan];
-
 	return insn->n;
 }
 
@@ -344,7 +328,6 @@ static int atao_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->maxdata	= 0x0fff;
 	s->range_table	= it->options[3] ? &range_unipolar10 : &range_bipolar10;
 	s->insn_write	= atao_ao_insn_write;
-	s->insn_read	= comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
@@ -366,9 +349,12 @@ static int atao_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->subdev_flags	= SDF_WRITABLE | SDF_INTERNAL;
 	s->n_chan	= (board->n_ao_chans * 2) + 1;
 	s->maxdata	= 0xff;
-	s->insn_read	= atao_calib_insn_read;
 	s->insn_write	= atao_calib_insn_write;
 
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
+
 	/* EEPROM subdevice */
 	s = &dev->subdevices[3];
 	s->type		= COMEDI_SUBD_UNUSED;
diff --git a/drivers/staging/comedi/drivers/ni_atmio16d.c b/drivers/staging/comedi/drivers/ni_atmio16d.c
index fc3c19de7005..c484c89c94b5 100644
--- a/drivers/staging/comedi/drivers/ni_atmio16d.c
+++ b/drivers/staging/comedi/drivers/ni_atmio16d.c
@@ -217,10 +217,12 @@ static irqreturn_t atmio16d_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
 	struct comedi_subdevice *s = dev->read_subdev;
+	unsigned short val;
 
-	comedi_buf_put(s, inw(dev->iobase + AD_FIFO_REG));
+	val = inw(dev->iobase + AD_FIFO_REG);
+	comedi_buf_write_samples(s, &val, 1);
+	comedi_handle_events(dev, s);
 
-	comedi_event(dev, s);
 	return IRQ_HANDLED;
 }
 
@@ -298,7 +300,6 @@ static int atmio16d_ai_cmd(struct comedi_device *dev,
 	 * It is still uber-experimental */
 
 	reset_counters(dev);
-	s->async->cur_chan = 0;
 
 	/* check if scanning multiple channels */
 	if (cmd->chanlist_len < 2) {
@@ -691,7 +692,6 @@ static int atmio16d_attach(struct comedi_device *dev,
 		break;
 	}
 	s->insn_write = atmio16d_ao_insn_write;
-	s->insn_read = comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
diff --git a/drivers/staging/comedi/drivers/ni_labpc.h b/drivers/staging/comedi/drivers/ni_labpc.h
index f6e5cd15a409..ac2c01f9dfdc 100644
--- a/drivers/staging/comedi/drivers/ni_labpc.h
+++ b/drivers/staging/comedi/drivers/ni_labpc.h
@@ -37,8 +37,6 @@ struct labpc_boardinfo {
 struct labpc_private {
 	/*  number of data points left to be taken */
 	unsigned long long count;
-	/*  software copy of analog output values */
-	unsigned int ao_value[NUM_AO_CHAN];
 	/*  software copys of bits written to command registers */
 	unsigned int cmd1;
 	unsigned int cmd2;
@@ -70,10 +68,6 @@ struct labpc_private {
 	unsigned int dma_transfer_size;
 	/* we are using dma/fifo-half-full/etc. */
 	enum transfer_type current_transfer;
-	/* stores contents of board's eeprom */
-	unsigned int eeprom_data[EEPROM_SIZE];
-	/* stores settings of calibration dacs */
-	unsigned int caldac[16];
 	/*
 	 * function pointers so we can use inb/outb or readb/writeb as
 	 * appropriate
diff --git a/drivers/staging/comedi/drivers/ni_labpc_common.c b/drivers/staging/comedi/drivers/ni_labpc_common.c
index 35bc2c25ddfb..d89d5852aeea 100644
--- a/drivers/staging/comedi/drivers/ni_labpc_common.c
+++ b/drivers/staging/comedi/drivers/ni_labpc_common.c
@@ -818,7 +818,7 @@ static int labpc_drain_fifo(struct comedi_device *dev)
 			devpriv->count--;
 		}
 		data = labpc_read_adc_fifo(dev);
-		cfc_write_to_buffer(dev->read_subdev, data);
+		comedi_buf_write_samples(dev->read_subdev, &data, 1);
 		devpriv->stat1 = devpriv->read_byte(dev, STAT1_REG);
 	}
 	if (i == timeout) {
@@ -876,7 +876,7 @@ static irqreturn_t labpc_interrupt(int irq, void *d)
 		/* clear error interrupt */
 		devpriv->write_byte(dev, 0x1, ADC_FIFO_CLEAR_REG);
 		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
-		cfc_handle_events(dev, s);
+		comedi_handle_events(dev, s);
 		dev_err(dev->class_dev, "overrun\n");
 		return IRQ_HANDLED;
 	}
@@ -896,7 +896,7 @@ static irqreturn_t labpc_interrupt(int irq, void *d)
 		/*  clear error interrupt */
 		devpriv->write_byte(dev, 0x1, ADC_FIFO_CLEAR_REG);
 		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
-		cfc_handle_events(dev, s);
+		comedi_handle_events(dev, s);
 		dev_err(dev->class_dev, "overflow\n");
 		return IRQ_HANDLED;
 	}
@@ -914,10 +914,22 @@ static irqreturn_t labpc_interrupt(int irq, void *d)
 			async->events |= COMEDI_CB_EOA;
 	}
 
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 	return IRQ_HANDLED;
 }
 
+static void labpc_ao_write(struct comedi_device *dev,
+			   struct comedi_subdevice *s,
+			   unsigned int chan, unsigned int val)
+{
+	struct labpc_private *devpriv = dev->private;
+
+	devpriv->write_byte(dev, val & 0xff, DAC_LSB_REG(chan));
+	devpriv->write_byte(dev, (val >> 8) & 0xff, DAC_MSB_REG(chan));
+
+	s->readback[chan] = val;
+}
+
 static int labpc_ao_insn_write(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn,
@@ -927,7 +939,6 @@ static int labpc_ao_insn_write(struct comedi_device *dev,
 	struct labpc_private *devpriv = dev->private;
 	int channel, range;
 	unsigned long flags;
-	int lsb, msb;
 
 	channel = CR_CHAN(insn->chanspec);
 
@@ -950,25 +961,7 @@ static int labpc_ao_insn_write(struct comedi_device *dev,
 		devpriv->write_byte(dev, devpriv->cmd6, CMD6_REG);
 	}
 	/* send data */
-	lsb = data[0] & 0xff;
-	msb = (data[0] >> 8) & 0xff;
-	devpriv->write_byte(dev, lsb, DAC_LSB_REG(channel));
-	devpriv->write_byte(dev, msb, DAC_MSB_REG(channel));
-
-	/* remember value for readback */
-	devpriv->ao_value[channel] = data[0];
-
-	return 1;
-}
-
-static int labpc_ao_insn_read(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn,
-			      unsigned int *data)
-{
-	struct labpc_private *devpriv = dev->private;
-
-	data[0] = devpriv->ao_value[CR_CHAN(insn->chanspec)];
+	labpc_ao_write(dev, s, channel, data[0]);
 
 	return 1;
 }
@@ -1085,29 +1078,13 @@ static unsigned int labpc_eeprom_read_status(struct comedi_device *dev)
 	return value;
 }
 
-static int labpc_eeprom_write(struct comedi_device *dev,
-			      unsigned int address, unsigned int value)
+static void labpc_eeprom_write(struct comedi_device *dev,
+			       unsigned int address, unsigned int value)
 {
 	struct labpc_private *devpriv = dev->private;
 	const int write_enable_instruction = 0x6;
 	const int write_instruction = 0x2;
 	const int write_length = 8;	/*  8 bit write lengths to eeprom */
-	const int write_in_progress_bit = 0x1;
-	const int timeout = 10000;
-	int i;
-
-	/*  make sure there isn't already a write in progress */
-	for (i = 0; i < timeout; i++) {
-		if ((labpc_eeprom_read_status(dev) & write_in_progress_bit) ==
-		    0)
-			break;
-	}
-	if (i == timeout) {
-		dev_err(dev->class_dev, "eeprom write timed out\n");
-		return -ETIME;
-	}
-	/*  update software copy of eeprom */
-	devpriv->eeprom_data[address] = value;
 
 	/*  enable read/write to eeprom */
 	devpriv->cmd5 &= ~CMD5_EEPROMCS;
@@ -1140,8 +1117,6 @@ static int labpc_eeprom_write(struct comedi_device *dev,
 	devpriv->cmd5 &= ~(CMD5_EEPROMCS | CMD5_WRTPRT);
 	udelay(1);
 	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-
-	return 0;
 }
 
 /* writes to 8 bit calibration dacs */
@@ -1150,10 +1125,6 @@ static void write_caldac(struct comedi_device *dev, unsigned int channel,
 {
 	struct labpc_private *devpriv = dev->private;
 
-	if (value == devpriv->caldac[channel])
-		return;
-	devpriv->caldac[channel] = value;
-
 	/*  clear caldac load bit and make sure we don't write to eeprom */
 	devpriv->cmd5 &= ~(CMD5_CALDACLD | CMD5_EEPROMCS | CMD5_WRTPRT);
 	udelay(1);
@@ -1184,25 +1155,30 @@ static int labpc_calib_insn_write(struct comedi_device *dev,
 	 * Only write the last data value to the caldac. Preceding
 	 * data would be overwritten anyway.
 	 */
-	if (insn->n > 0)
-		write_caldac(dev, chan, data[insn->n - 1]);
+	if (insn->n > 0) {
+		unsigned int val = data[insn->n - 1];
+
+		if (s->readback[chan] != val) {
+			write_caldac(dev, chan, val);
+			s->readback[chan] = val;
+		}
+	}
 
 	return insn->n;
 }
 
-static int labpc_calib_insn_read(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn,
-				 unsigned int *data)
+static int labpc_eeprom_ready(struct comedi_device *dev,
+			      struct comedi_subdevice *s,
+			      struct comedi_insn *insn,
+			      unsigned long context)
 {
-	struct labpc_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->caldac[chan];
+	unsigned int status;
 
-	return insn->n;
+	/* make sure there isn't already a write in progress */
+	status = labpc_eeprom_read_status(dev);
+	if ((status & 0x1) == 0)
+		return 0;
+	return -EBUSY;
 }
 
 static int labpc_eeprom_insn_write(struct comedi_device *dev,
@@ -1222,25 +1198,15 @@ static int labpc_eeprom_insn_write(struct comedi_device *dev,
 	 * data would be overwritten anyway.
 	 */
 	if (insn->n > 0) {
-		ret = labpc_eeprom_write(dev, chan, data[insn->n - 1]);
+		unsigned int val = data[insn->n - 1];
+
+		ret = comedi_timeout(dev, s, insn, labpc_eeprom_ready, 0);
 		if (ret)
 			return ret;
-	}
-
-	return insn->n;
-}
-
-static int labpc_eeprom_insn_read(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn,
-				  unsigned int *data)
-{
-	struct labpc_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
 
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->eeprom_data[chan];
+		labpc_eeprom_write(dev, chan, val);
+		s->readback[chan] = val;
+	}
 
 	return insn->n;
 }
@@ -1309,19 +1275,15 @@ int labpc_common_attach(struct comedi_device *dev,
 		s->n_chan	= NUM_AO_CHAN;
 		s->maxdata	= 0x0fff;
 		s->range_table	= &range_labpc_ao;
-		s->insn_read	= labpc_ao_insn_read;
 		s->insn_write	= labpc_ao_insn_write;
 
-		/* initialize analog outputs to a known value */
-		for (i = 0; i < s->n_chan; i++) {
-			short lsb, msb;
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
 
-			devpriv->ao_value[i] = s->maxdata / 2;
-			lsb = devpriv->ao_value[i] & 0xff;
-			msb = (devpriv->ao_value[i] >> 8) & 0xff;
-			devpriv->write_byte(dev, lsb, DAC_LSB_REG(i));
-			devpriv->write_byte(dev, msb, DAC_MSB_REG(i));
-		}
+		/* initialize analog outputs to a known value */
+		for (i = 0; i < s->n_chan; i++)
+			labpc_ao_write(dev, s, i, s->maxdata / 2);
 	} else {
 		s->type		= COMEDI_SUBD_UNUSED;
 	}
@@ -1342,11 +1304,16 @@ int labpc_common_attach(struct comedi_device *dev,
 		s->subdev_flags	= SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
 		s->n_chan	= 16;
 		s->maxdata	= 0xff;
-		s->insn_read	= labpc_calib_insn_read;
 		s->insn_write	= labpc_calib_insn_write;
 
-		for (i = 0; i < s->n_chan; i++)
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
+
+		for (i = 0; i < s->n_chan; i++) {
 			write_caldac(dev, i, s->maxdata / 2);
+			s->readback[i] = s->maxdata / 2;
+		}
 	} else {
 		s->type		= COMEDI_SUBD_UNUSED;
 	}
@@ -1358,11 +1325,14 @@ int labpc_common_attach(struct comedi_device *dev,
 		s->subdev_flags	= SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
 		s->n_chan	= EEPROM_SIZE;
 		s->maxdata	= 0xff;
-		s->insn_read	= labpc_eeprom_insn_read;
 		s->insn_write	= labpc_eeprom_insn_write;
 
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
+
 		for (i = 0; i < s->n_chan; i++)
-			devpriv->eeprom_data[i] = labpc_eeprom_read(dev, i);
+			s->readback[i] = labpc_eeprom_read(dev, i);
 	} else {
 		s->type		= COMEDI_SUBD_UNUSED;
 	}
diff --git a/drivers/staging/comedi/drivers/ni_labpc_isadma.c b/drivers/staging/comedi/drivers/ni_labpc_isadma.c
index 967202e0635e..6d386050e59d 100644
--- a/drivers/staging/comedi/drivers/ni_labpc_isadma.c
+++ b/drivers/staging/comedi/drivers/ni_labpc_isadma.c
@@ -91,7 +91,6 @@ void labpc_drain_dma(struct comedi_device *dev)
 	int status;
 	unsigned long flags;
 	unsigned int max_points, num_points, residue, leftover;
-	int i;
 
 	status = devpriv->stat1;
 
@@ -122,9 +121,7 @@ void labpc_drain_dma(struct comedi_device *dev)
 			leftover = max_points;
 	}
 
-	/* write data to comedi buffer */
-	for (i = 0; i < num_points; i++)
-		cfc_write_to_buffer(s, devpriv->dma_buffer[i]);
+	comedi_buf_write_samples(s, devpriv->dma_buffer, num_points);
 
 	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->count -= num_points;
@@ -133,8 +130,6 @@ void labpc_drain_dma(struct comedi_device *dev)
 	set_dma_addr(devpriv->dma_chan, devpriv->dma_addr);
 	set_dma_count(devpriv->dma_chan, leftover * sample_size);
 	release_dma_lock(flags);
-
-	async->events |= COMEDI_CB_BLOCK;
 }
 EXPORT_SYMBOL_GPL(labpc_drain_dma);
 
diff --git a/drivers/staging/comedi/drivers/ni_mio_common.c b/drivers/staging/comedi/drivers/ni_mio_common.c
index 320b080149b6..11e70173712d 100644
--- a/drivers/staging/comedi/drivers/ni_mio_common.c
+++ b/drivers/staging/comedi/drivers/ni_mio_common.c
@@ -686,13 +686,12 @@ static inline void ni_set_ai_dma_channel(struct comedi_device *dev, int channel)
 {
 	unsigned bitfield;
 
-	if (channel >= 0) {
+	if (channel >= 0)
 		bitfield =
 		    (ni_stc_dma_channel_select_bitfield(channel) <<
 		     AI_DMA_Select_Shift) & AI_DMA_Select_Mask;
-	} else {
+	else
 		bitfield = 0;
-	}
 	ni_set_bitfield(dev, AI_AO_Select, AI_DMA_Select_Mask, bitfield);
 }
 
@@ -701,13 +700,12 @@ static inline void ni_set_ao_dma_channel(struct comedi_device *dev, int channel)
 {
 	unsigned bitfield;
 
-	if (channel >= 0) {
+	if (channel >= 0)
 		bitfield =
 		    (ni_stc_dma_channel_select_bitfield(channel) <<
 		     AO_DMA_Select_Shift) & AO_DMA_Select_Mask;
-	} else {
+	else
 		bitfield = 0;
-	}
 	ni_set_bitfield(dev, AI_AO_Select, AO_DMA_Select_Mask, bitfield);
 }
 
@@ -1127,31 +1125,18 @@ static void ni_ao_fifo_load(struct comedi_device *dev,
 			    struct comedi_subdevice *s, int n)
 {
 	struct ni_private *devpriv = dev->private;
-	struct comedi_async *async = s->async;
-	struct comedi_cmd *cmd = &async->cmd;
-	int chan;
 	int i;
 	unsigned short d;
 	u32 packed_data;
-	int range;
-	int err = 1;
 
-	chan = async->cur_chan;
 	for (i = 0; i < n; i++) {
-		err &= comedi_buf_get(s, &d);
-		if (err == 0)
-			break;
-
-		range = CR_RANGE(cmd->chanlist[chan]);
+		comedi_buf_read_samples(s, &d, 1);
 
 		if (devpriv->is_6xxx) {
 			packed_data = d & 0xffff;
 			/* 6711 only has 16 bit wide ao fifo */
 			if (!devpriv->is_6711) {
-				err &= comedi_buf_get(s, &d);
-				if (err == 0)
-					break;
-				chan++;
+				comedi_buf_read_samples(s, &d, 1);
 				i++;
 				packed_data |= (d << 16) & 0xffff0000;
 			}
@@ -1159,12 +1144,7 @@ static void ni_ao_fifo_load(struct comedi_device *dev,
 		} else {
 			ni_writew(dev, d, DAC_FIFO_Data);
 		}
-		chan++;
-		chan %= cmd->chanlist_len;
 	}
-	async->cur_chan = chan;
-	if (err == 0)
-		async->events |= COMEDI_CB_OVERFLOW;
 }
 
 /*
@@ -1187,21 +1167,20 @@ static int ni_ao_fifo_half_empty(struct comedi_device *dev,
 				 struct comedi_subdevice *s)
 {
 	const struct ni_board_struct *board = dev->board_ptr;
-	int n;
+	unsigned int nbytes;
+	unsigned int nsamples;
 
-	n = comedi_buf_read_n_available(s);
-	if (n == 0) {
+	nbytes = comedi_buf_read_n_available(s);
+	if (nbytes == 0) {
 		s->async->events |= COMEDI_CB_OVERFLOW;
 		return 0;
 	}
 
-	n /= sizeof(short);
-	if (n > board->ao_fifo_depth / 2)
-		n = board->ao_fifo_depth / 2;
-
-	ni_ao_fifo_load(dev, s, n);
+	nsamples = comedi_bytes_to_samples(s, nbytes);
+	if (nsamples > board->ao_fifo_depth / 2)
+		nsamples = board->ao_fifo_depth / 2;
 
-	s->async->events |= COMEDI_CB_BLOCK;
+	ni_ao_fifo_load(dev, s, nsamples);
 
 	return 1;
 }
@@ -1211,7 +1190,8 @@ static int ni_ao_prep_fifo(struct comedi_device *dev,
 {
 	const struct ni_board_struct *board = dev->board_ptr;
 	struct ni_private *devpriv = dev->private;
-	int n;
+	unsigned int nbytes;
+	unsigned int nsamples;
 
 	/* reset fifo */
 	ni_stc_writew(dev, 1, DAC_FIFO_Clear);
@@ -1219,17 +1199,17 @@ static int ni_ao_prep_fifo(struct comedi_device *dev,
 		ni_ao_win_outl(dev, 0x6, AO_FIFO_Offset_Load_611x);
 
 	/* load some data */
-	n = comedi_buf_read_n_available(s);
-	if (n == 0)
+	nbytes = comedi_buf_read_n_available(s);
+	if (nbytes == 0)
 		return 0;
 
-	n /= sizeof(short);
-	if (n > board->ao_fifo_depth)
-		n = board->ao_fifo_depth;
+	nsamples = comedi_bytes_to_samples(s, nbytes);
+	if (nsamples > board->ao_fifo_depth)
+		nsamples = board->ao_fifo_depth;
 
-	ni_ao_fifo_load(dev, s, n);
+	ni_ao_fifo_load(dev, s, nsamples);
 
-	return n;
+	return nsamples;
 }
 
 static void ni_ai_fifo_read(struct comedi_device *dev,
@@ -1237,44 +1217,42 @@ static void ni_ai_fifo_read(struct comedi_device *dev,
 {
 	struct ni_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
+	u32 dl;
+	unsigned short data;
 	int i;
 
 	if (devpriv->is_611x) {
-		unsigned short data[2];
-		u32 dl;
-
 		for (i = 0; i < n / 2; i++) {
 			dl = ni_readl(dev, ADC_FIFO_Data_611x);
 			/* This may get the hi/lo data in the wrong order */
-			data[0] = (dl >> 16) & 0xffff;
-			data[1] = dl & 0xffff;
-			cfc_write_array_to_buffer(s, data, sizeof(data));
+			data = (dl >> 16) & 0xffff;
+			comedi_buf_write_samples(s, &data, 1);
+			data = dl & 0xffff;
+			comedi_buf_write_samples(s, &data, 1);
 		}
 		/* Check if there's a single sample stuck in the FIFO */
 		if (n % 2) {
 			dl = ni_readl(dev, ADC_FIFO_Data_611x);
-			data[0] = dl & 0xffff;
-			cfc_write_to_buffer(s, data[0]);
+			data = dl & 0xffff;
+			comedi_buf_write_samples(s, &data, 1);
 		}
 	} else if (devpriv->is_6143) {
-		unsigned short data[2];
-		u32 dl;
-
 		/*  This just reads the FIFO assuming the data is present, no checks on the FIFO status are performed */
 		for (i = 0; i < n / 2; i++) {
 			dl = ni_readl(dev, AIFIFO_Data_6143);
 
-			data[0] = (dl >> 16) & 0xffff;
-			data[1] = dl & 0xffff;
-			cfc_write_array_to_buffer(s, data, sizeof(data));
+			data = (dl >> 16) & 0xffff;
+			comedi_buf_write_samples(s, &data, 1);
+			data = dl & 0xffff;
+			comedi_buf_write_samples(s, &data, 1);
 		}
 		if (n % 2) {
 			/* Assume there is a single sample stuck in the FIFO */
 			/* Get stranded sample into FIFO */
 			ni_writel(dev, 0x01, AIFIFO_Control_6143);
 			dl = ni_readl(dev, AIFIFO_Data_6143);
-			data[0] = (dl >> 16) & 0xffff;
-			cfc_write_to_buffer(s, data[0]);
+			data = (dl >> 16) & 0xffff;
+			comedi_buf_write_samples(s, &data, 1);
 		}
 	} else {
 		if (n > sizeof(devpriv->ai_fifo_buffer) /
@@ -1288,9 +1266,7 @@ static void ni_ai_fifo_read(struct comedi_device *dev,
 			devpriv->ai_fifo_buffer[i] =
 			    ni_readw(dev, ADC_FIFO_Data_Register);
 		}
-		cfc_write_array_to_buffer(s, devpriv->ai_fifo_buffer,
-					  n *
-					  sizeof(devpriv->ai_fifo_buffer[0]));
+		comedi_buf_write_samples(s, devpriv->ai_fifo_buffer, n);
 	}
 }
 
@@ -1313,8 +1289,8 @@ static void ni_handle_fifo_dregs(struct comedi_device *dev)
 {
 	struct ni_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
-	unsigned short data[2];
 	u32 dl;
+	unsigned short data;
 	unsigned short fifo_empty;
 	int i;
 
@@ -1324,9 +1300,10 @@ static void ni_handle_fifo_dregs(struct comedi_device *dev)
 			dl = ni_readl(dev, ADC_FIFO_Data_611x);
 
 			/* This may get the hi/lo data in the wrong order */
-			data[0] = (dl >> 16);
-			data[1] = (dl & 0xffff);
-			cfc_write_array_to_buffer(s, data, sizeof(data));
+			data = dl >> 16;
+			comedi_buf_write_samples(s, &data, 1);
+			data = dl & 0xffff;
+			comedi_buf_write_samples(s, &data, 1);
 		}
 	} else if (devpriv->is_6143) {
 		i = 0;
@@ -1334,9 +1311,10 @@ static void ni_handle_fifo_dregs(struct comedi_device *dev)
 			dl = ni_readl(dev, AIFIFO_Data_6143);
 
 			/* This may get the hi/lo data in the wrong order */
-			data[0] = (dl >> 16);
-			data[1] = (dl & 0xffff);
-			cfc_write_array_to_buffer(s, data, sizeof(data));
+			data = dl >> 16;
+			comedi_buf_write_samples(s, &data, 1);
+			data = dl & 0xffff;
+			comedi_buf_write_samples(s, &data, 1);
 			i += 2;
 		}
 		/*  Check if stranded sample is present */
@@ -1344,8 +1322,8 @@ static void ni_handle_fifo_dregs(struct comedi_device *dev)
 			/* Get stranded sample into FIFO */
 			ni_writel(dev, 0x01, AIFIFO_Control_6143);
 			dl = ni_readl(dev, AIFIFO_Data_6143);
-			data[0] = (dl >> 16) & 0xffff;
-			cfc_write_to_buffer(s, data[0]);
+			data = (dl >> 16) & 0xffff;
+			comedi_buf_write_samples(s, &data, 1);
 		}
 
 	} else {
@@ -1364,10 +1342,7 @@ static void ni_handle_fifo_dregs(struct comedi_device *dev)
 				devpriv->ai_fifo_buffer[i] =
 				    ni_readw(dev, ADC_FIFO_Data_Register);
 			}
-			cfc_write_array_to_buffer(s, devpriv->ai_fifo_buffer,
-						  i *
-						  sizeof(devpriv->
-							 ai_fifo_buffer[0]));
+			comedi_buf_write_samples(s, devpriv->ai_fifo_buffer, i);
 		}
 	}
 }
@@ -1386,7 +1361,7 @@ static void get_last_sample_611x(struct comedi_device *dev)
 	if (ni_readb(dev, XXX_Status) & 0x80) {
 		dl = ni_readl(dev, ADC_FIFO_Data_611x);
 		data = (dl & 0xffff);
-		cfc_write_to_buffer(s, data);
+		comedi_buf_write_samples(s, &data, 1);
 	}
 }
 
@@ -1408,7 +1383,7 @@ static void get_last_sample_6143(struct comedi_device *dev)
 
 		/* This may get the hi/lo data in the wrong order */
 		data = (dl >> 16) & 0xffff;
-		cfc_write_to_buffer(s, data);
+		comedi_buf_write_samples(s, &data, 1);
 	}
 }
 
@@ -1462,7 +1437,7 @@ static void handle_gpct_interrupt(struct comedi_device *dev,
 
 	ni_tio_handle_interrupt(&devpriv->counter_dev->counters[counter_index],
 				s);
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 #endif
 }
 
@@ -1518,7 +1493,7 @@ static void handle_a_interrupt(struct comedi_device *dev, unsigned short status,
 			if (comedi_is_subdevice_running(s)) {
 				s->async->events |=
 				    COMEDI_CB_ERROR | COMEDI_CB_EOA;
-				cfc_handle_events(dev, s);
+				comedi_handle_events(dev, s);
 			}
 			return;
 		}
@@ -1533,7 +1508,7 @@ static void handle_a_interrupt(struct comedi_device *dev, unsigned short status,
 			if (status & (AI_Overrun_St | AI_Overflow_St))
 				s->async->events |= COMEDI_CB_OVERFLOW;
 
-			cfc_handle_events(dev, s);
+			comedi_handle_events(dev, s);
 			return;
 		}
 		if (status & AI_SC_TC_St) {
@@ -1559,7 +1534,7 @@ static void handle_a_interrupt(struct comedi_device *dev, unsigned short status,
 	if ((status & AI_STOP_St))
 		ni_handle_eos(dev, s);
 
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 }
 
 static void ack_b_interrupt(struct comedi_device *dev, unsigned short b_status)
@@ -1635,7 +1610,7 @@ static void handle_b_interrupt(struct comedi_device *dev,
 	}
 #endif
 
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 }
 
 static void ni_ai_munge(struct comedi_device *dev, struct comedi_subdevice *s,
@@ -1645,12 +1620,12 @@ static void ni_ai_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 	struct ni_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
-	unsigned int length = num_bytes / bytes_per_sample(s);
+	unsigned int nsamples = comedi_bytes_to_samples(s, num_bytes);
 	unsigned short *array = data;
 	unsigned int *larray = data;
 	unsigned int i;
 
-	for (i = 0; i < length; i++) {
+	for (i = 0; i < nsamples; i++) {
 #ifdef PCIDMA
 		if (s->subdev_flags & SDF_LSAMPL)
 			larray[i] = le32_to_cpu(larray[i]);
@@ -2253,9 +2228,6 @@ static int ni_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	if ((cmd->flags & CMDF_WRITE))
-		cmd->flags &= ~CMDF_WRITE;
-
 	err |= cfc_check_trigger_src(&cmd->start_src,
 					TRIG_NOW | TRIG_INT | TRIG_EXT);
 	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
@@ -2762,11 +2734,11 @@ static void ni_ao_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 			unsigned int chan_index)
 {
 	struct comedi_cmd *cmd = &s->async->cmd;
-	unsigned int length = num_bytes / bytes_per_sample(s);
+	unsigned int nsamples = comedi_bytes_to_samples(s, num_bytes);
 	unsigned short *array = data;
 	unsigned int i;
 
-	for (i = 0; i < length; i++) {
+	for (i = 0; i < nsamples; i++) {
 		unsigned int range = CR_RANGE(cmd->chanlist[chan_index]);
 		unsigned short val = array[i];
 
@@ -2981,12 +2953,15 @@ static int ni_ao_insn_config(struct comedi_device *dev,
 {
 	const struct ni_board_struct *board = dev->board_ptr;
 	struct ni_private *devpriv = dev->private;
+	unsigned int nbytes;
 
 	switch (data[0]) {
 	case INSN_CONFIG_GET_HARDWARE_BUFFER_SIZE:
 		switch (data[1]) {
 		case COMEDI_OUTPUT:
-			data[2] = 1 + board->ao_fifo_depth * sizeof(short);
+			nbytes = comedi_samples_to_bytes(s,
+							 board->ao_fifo_depth);
+			data[2] = 1 + nbytes;
 			if (devpriv->mite)
 				data[2] += devpriv->mite->fifo_size;
 			break;
@@ -3288,9 +3263,6 @@ static int ni_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	if ((cmd->flags & CMDF_WRITE) == 0)
-		cmd->flags |= CMDF_WRITE;
-
 	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_INT | TRIG_EXT);
 	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_TIMER | TRIG_EXT);
@@ -3515,9 +3487,6 @@ static int ni_cdio_cmdtest(struct comedi_device *dev,
 	/* Step 2a : make sure trigger sources are unique */
 	/* Step 2b : and mutually compatible */
 
-	if (err)
-		return 2;
-
 	/* Step 3: check if arguments are trivially valid */
 
 	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
@@ -3693,7 +3662,7 @@ static void handle_cdio_interrupt(struct comedi_device *dev)
 			  M_Offset_CDIO_Command);
 		/* s->async->events |= COMEDI_CB_EOA; */
 	}
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 }
 
 static int ni_serial_hw_readwrite8(struct comedi_device *dev,
@@ -3976,7 +3945,7 @@ static unsigned ni_gpct_to_stc_register(enum ni_gpct_register reg)
 		stc_register = Interrupt_B_Enable_Register;
 		break;
 	default:
-		printk("%s: unhandled register 0x%x in switch.\n",
+		pr_err("%s: unhandled register 0x%x in switch.\n",
 		       __func__, reg);
 		BUG();
 		return 0;
@@ -5472,7 +5441,6 @@ static int ni_E_init(struct comedi_device *dev,
 		s->range_table	= board->ao_range_table;
 		s->insn_config	= ni_ao_insn_config;
 		s->insn_write	= ni_ao_insn_write;
-		s->insn_read	= comedi_readback_insn_read;
 
 		ret = comedi_alloc_subdev_readback(s);
 		if (ret)
diff --git a/drivers/staging/comedi/drivers/ni_pcidio.c b/drivers/staging/comedi/drivers/ni_pcidio.c
index 5252cba82e5e..db7e8aac67b5 100644
--- a/drivers/staging/comedi/drivers/ni_pcidio.c
+++ b/drivers/staging/comedi/drivers/ni_pcidio.c
@@ -384,11 +384,7 @@ static irqreturn_t nidio_interrupt(int irq, void *d)
 	struct comedi_subdevice *s = dev->read_subdev;
 	struct comedi_async *async = s->async;
 	struct mite_struct *mite = devpriv->mite;
-
-	/* int i, j; */
-	unsigned int auxdata = 0;
-	unsigned short data1 = 0;
-	unsigned short data2 = 0;
+	unsigned int auxdata;
 	int flags;
 	int status;
 	int work = 0;
@@ -451,13 +447,9 @@ static irqreturn_t nidio_interrupt(int irq, void *d)
 					goto out;
 				}
 				auxdata = readl(dev->mmio + Group_1_FIFO);
-				data1 = auxdata & 0xffff;
-				data2 = (auxdata & 0xffff0000) >> 16;
-				comedi_buf_put(s, data1);
-				comedi_buf_put(s, data2);
+				comedi_buf_write_samples(s, &auxdata, 1);
 				flags = readb(dev->mmio + Group_1_Flags);
 			}
-			async->events |= COMEDI_CB_BLOCK;
 		}
 
 		if (flags & CountExpired) {
@@ -485,7 +477,7 @@ static irqreturn_t nidio_interrupt(int irq, void *d)
 	}
 
 out:
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 #if 0
 	if (!tag)
 		writeb(0x03, dev->mmio + Master_DMA_And_Interrupt_Control);
diff --git a/drivers/staging/comedi/drivers/ni_stc.h b/drivers/staging/comedi/drivers/ni_stc.h
index 29efce30eb7f..bd69c3f0acdc 100644
--- a/drivers/staging/comedi/drivers/ni_stc.h
+++ b/drivers/staging/comedi/drivers/ni_stc.h
@@ -334,7 +334,7 @@ static inline unsigned RTSI_Output_Bit(unsigned channel, int is_mseries)
 		max_channel = 6;
 	}
 	if (channel > max_channel) {
-		printk("%s: bug, invalid RTSI_channel=%i\n", __func__, channel);
+		pr_err("%s: bug, invalid RTSI_channel=%i\n", __func__, channel);
 		return 0;
 	}
 	return 1 << (base_bit_shift + channel);
@@ -1090,7 +1090,7 @@ static inline int M_Offset_Static_AI_Control(int i)
 		0x263,
 	};
 	if (((unsigned)i) >= ARRAY_SIZE(offset)) {
-		printk("%s: invalid channel=%i\n", __func__, i);
+		pr_err("%s: invalid channel=%i\n", __func__, i);
 		return offset[0];
 	}
 	return offset[i];
@@ -1105,7 +1105,7 @@ static inline int M_Offset_AO_Reference_Attenuation(int channel)
 		0x267
 	};
 	if (((unsigned)channel) >= ARRAY_SIZE(offset)) {
-		printk("%s: invalid channel=%i\n", __func__, channel);
+		pr_err("%s: invalid channel=%i\n", __func__, channel);
 		return offset[0];
 	}
 	return offset[channel];
@@ -1114,7 +1114,7 @@ static inline int M_Offset_AO_Reference_Attenuation(int channel)
 static inline unsigned M_Offset_PFI_Output_Select(unsigned n)
 {
 	if (n < 1 || n > NUM_PFI_OUTPUT_SELECT_REGS) {
-		printk("%s: invalid pfi output select register=%i\n",
+		pr_err("%s: invalid pfi output select register=%i\n",
 		       __func__, n);
 		return M_Offset_PFI_Output_Select_1;
 	}
@@ -1171,7 +1171,7 @@ static inline unsigned MSeries_PLL_In_Source_Select_RTSI_Bits(unsigned
 							      RTSI_channel)
 {
 	if (RTSI_channel > 7) {
-		printk("%s: bug, invalid RTSI_channel=%i\n", __func__,
+		pr_err("%s: bug, invalid RTSI_channel=%i\n", __func__,
 		       RTSI_channel);
 		return 0;
 	}
@@ -1192,7 +1192,7 @@ static inline unsigned MSeries_PLL_Divisor_Bits(unsigned divisor)
 {
 	static const unsigned max_divisor = 0x10;
 	if (divisor < 1 || divisor > max_divisor) {
-		printk("%s: bug, invalid divisor=%i\n", __func__, divisor);
+		pr_err("%s: bug, invalid divisor=%i\n", __func__, divisor);
 		return 0;
 	}
 	return (divisor & 0xf) << 8;
@@ -1202,7 +1202,7 @@ static inline unsigned MSeries_PLL_Multiplier_Bits(unsigned multiplier)
 {
 	static const unsigned max_multiplier = 0x100;
 	if (multiplier < 1 || multiplier > max_multiplier) {
-		printk("%s: bug, invalid multiplier=%i\n", __func__,
+		pr_err("%s: bug, invalid multiplier=%i\n", __func__,
 		       multiplier);
 		return 0;
 	}
@@ -1464,7 +1464,7 @@ struct ni_private {
 
 	unsigned short ai_fifo_buffer[0x2000];
 	uint8_t eeprom_buffer[M_SERIES_EEPROM_SIZE];
-	uint32_t serial_number;
+	__be32 serial_number;
 
 	struct mite_struct *mite;
 	struct mite_channel *ai_mite_chan;
diff --git a/drivers/staging/comedi/drivers/ni_tiocmd.c b/drivers/staging/comedi/drivers/ni_tiocmd.c
index 26e7291c4a51..6037bec77ef1 100644
--- a/drivers/staging/comedi/drivers/ni_tiocmd.c
+++ b/drivers/staging/comedi/drivers/ni_tiocmd.c
@@ -371,9 +371,8 @@ static void ni_tio_acknowledge_and_confirm(struct ni_gpct *counter,
 			  of gate interrupt via dma read/write
 			   and report bogus gate errors */
 			if (counter->counter_dev->variant !=
-			    ni_gpct_variant_660x) {
+			    ni_gpct_variant_660x)
 				*gate_error = 1;
-			}
 		}
 	}
 	if (gxx_status & GI_TC_ERROR(cidx)) {
@@ -450,11 +449,10 @@ void ni_tio_handle_interrupt(struct ni_gpct *counter,
 		return;
 	}
 	gpct_mite_status = mite_get_status(counter->mite_chan);
-	if (gpct_mite_status & CHSR_LINKC) {
+	if (gpct_mite_status & CHSR_LINKC)
 		writel(CHOR_CLRLC,
 		       counter->mite_chan->mite->mite_io_addr +
 		       MITE_CHOR(counter->mite_chan->channel));
-	}
 	mite_sync_input_dma(counter->mite_chan, s);
 	spin_unlock_irqrestore(&counter->lock, flags);
 }
diff --git a/drivers/staging/comedi/drivers/ni_usb6501.c b/drivers/staging/comedi/drivers/ni_usb6501.c
index df7ada8611f4..3b5a1b90366d 100644
--- a/drivers/staging/comedi/drivers/ni_usb6501.c
+++ b/drivers/staging/comedi/drivers/ni_usb6501.c
@@ -561,7 +561,7 @@ static int ni6501_auto_attach(struct comedi_device *dev,
 	/* Counter subdevice */
 	s = &dev->subdevices[1];
 	s->type		= COMEDI_SUBD_COUNTER;
-	s->subdev_flags	= SDF_READABLE | SDF_WRITEABLE | SDF_LSAMPL;
+	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE | SDF_LSAMPL;
 	s->n_chan	= 1;
 	s->maxdata	= 0xffffffff;
 	s->insn_read	= ni6501_cnt_insn_read;
diff --git a/drivers/staging/comedi/drivers/pcl711.c b/drivers/staging/comedi/drivers/pcl711.c
index 47f4887108a7..938aebc8e0ea 100644
--- a/drivers/staging/comedi/drivers/pcl711.c
+++ b/drivers/staging/comedi/drivers/pcl711.c
@@ -156,7 +156,6 @@ static const struct pcl711_board boardtypes[] = {
 };
 
 struct pcl711_private {
-	unsigned int ntrig;
 	unsigned int divisor1;
 	unsigned int divisor2;
 };
@@ -199,7 +198,6 @@ static int pcl711_ai_cancel(struct comedi_device *dev,
 static irqreturn_t pcl711_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	struct pcl711_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int data;
@@ -213,16 +211,14 @@ static irqreturn_t pcl711_interrupt(int irq, void *d)
 
 	outb(PCL711_INT_STAT_CLR, dev->iobase + PCL711_INT_STAT_REG);
 
-	if (comedi_buf_put(s, data) == 0) {
-		s->async->events |= COMEDI_CB_OVERFLOW | COMEDI_CB_ERROR;
-	} else {
-		s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
-		if (cmd->stop_src == TRIG_COUNT && !(--devpriv->ntrig)) {
-			pcl711_ai_set_mode(dev, PCL711_MODE_SOFTTRIG);
-			s->async->events |= COMEDI_CB_EOA;
-		}
-	}
-	comedi_event(dev, s);
+	comedi_buf_write_samples(s, &data, 1);
+
+	if (cmd->stop_src == TRIG_COUNT &&
+	    s->async->scans_done >= cmd->stop_arg)
+		s->async->events |= COMEDI_CB_EOA;
+
+	comedi_handle_events(dev, s);
+
 	return IRQ_HANDLED;
 }
 
@@ -373,14 +369,10 @@ static void pcl711_ai_load_counters(struct comedi_device *dev)
 
 static int pcl711_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	struct pcl711_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 
 	pcl711_set_changain(dev, s, cmd->chanlist[0]);
 
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->ntrig = cmd->stop_arg;
-
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		pcl711_ai_load_counters(dev);
 		outb(PCL711_INT_STAT_CLR, dev->iobase + PCL711_INT_STAT_REG);
@@ -505,7 +497,6 @@ static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->maxdata	= 0xfff;
 	s->range_table	= &range_bipolar5;
 	s->insn_write	= pcl711_ao_insn_write;
-	s->insn_read	= comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
diff --git a/drivers/staging/comedi/drivers/pcl726.c b/drivers/staging/comedi/drivers/pcl726.c
index dc179bd02dfd..86f713fdf1d0 100644
--- a/drivers/staging/comedi/drivers/pcl726.c
+++ b/drivers/staging/comedi/drivers/pcl726.c
@@ -235,9 +235,8 @@ static irqreturn_t pcl726_interrupt(int irq, void *d)
 	if (devpriv->cmd_running) {
 		pcl726_intr_cancel(dev, s);
 
-		comedi_buf_put(s, 0);
-		s->async->events |= (COMEDI_CB_BLOCK | COMEDI_CB_EOS);
-		comedi_event(dev, s);
+		comedi_buf_write_samples(s, &s->state, 1);
+		comedi_handle_events(dev, s);
 	}
 
 	return IRQ_HANDLED;
@@ -377,7 +376,6 @@ static int pcl726_attach(struct comedi_device *dev,
 	s->maxdata	= 0x0fff;
 	s->range_table_list = devpriv->rangelist;
 	s->insn_write	= pcl726_ao_insn_write;
-	s->insn_read	= comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
diff --git a/drivers/staging/comedi/drivers/pcl812.c b/drivers/staging/comedi/drivers/pcl812.c
index fd5ea6e01619..ac243ca5e0f8 100644
--- a/drivers/staging/comedi/drivers/pcl812.c
+++ b/drivers/staging/comedi/drivers/pcl812.c
@@ -512,7 +512,6 @@ struct pcl812_private {
 	unsigned int last_ai_chanspec;
 	unsigned char mode_reg_int;	/*  there is stored INT number for some card */
 	unsigned int ai_poll_ptr;	/*  how many sampes transfer poll */
-	unsigned int ai_act_scan;	/*  how many scans we finished */
 	unsigned int dmapages;
 	unsigned int hwdmasize;
 	unsigned long dmabuf[2];	/*  PTR to DMA buf */
@@ -556,8 +555,8 @@ static void pcl812_ai_setup_dma(struct comedi_device *dev,
 
 	/*  we use EOS, so adapt DMA buffer to one scan */
 	if (devpriv->ai_eos) {
-		devpriv->dmabytestomove[0] = cfc_bytes_per_scan(s);
-		devpriv->dmabytestomove[1] = cfc_bytes_per_scan(s);
+		devpriv->dmabytestomove[0] = comedi_bytes_per_scan(s);
+		devpriv->dmabytestomove[1] = comedi_bytes_per_scan(s);
 		devpriv->dma_runs_to_end = 1;
 	} else {
 		devpriv->dmabytestomove[0] = devpriv->hwdmasize;
@@ -572,7 +571,7 @@ static void pcl812_ai_setup_dma(struct comedi_device *dev,
 			devpriv->dma_runs_to_end = 1;
 		} else {
 			/*  how many samples we must transfer? */
-			bytes = cmd->stop_arg * cfc_bytes_per_scan(s);
+			bytes = cmd->stop_arg * comedi_bytes_per_scan(s);
 
 			/*  how many DMA pages we must fill */
 			devpriv->dma_runs_to_end =
@@ -807,9 +806,7 @@ static int pcl812_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		devpriv->ai_dma = 0;
 	}
 
-	devpriv->ai_act_scan = 0;
 	devpriv->ai_poll_ptr = 0;
-	s->async->cur_chan = 0;
 
 	/*  don't we want wake up every scan? */
 	if (cmd->flags & CMDF_WAKE_EOS) {
@@ -841,21 +838,10 @@ static int pcl812_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 static bool pcl812_ai_next_chan(struct comedi_device *dev,
 				struct comedi_subdevice *s)
 {
-	struct pcl812_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 
-	s->async->events |= COMEDI_CB_BLOCK;
-
-	s->async->cur_chan++;
-	if (s->async->cur_chan >= cmd->chanlist_len) {
-		s->async->cur_chan = 0;
-		devpriv->ai_act_scan++;
-		s->async->events |= COMEDI_CB_EOS;
-	}
-
 	if (cmd->stop_src == TRIG_COUNT &&
-	    devpriv->ai_act_scan >= cmd->stop_arg) {
-		/* all data sampled */
+	    s->async->scans_done >= cmd->stop_arg) {
 		s->async->events |= COMEDI_CB_EOA;
 		return false;
 	}
@@ -867,7 +853,9 @@ static void pcl812_handle_eoc(struct comedi_device *dev,
 			      struct comedi_subdevice *s)
 {
 	struct comedi_cmd *cmd = &s->async->cmd;
+	unsigned int chan = s->async->cur_chan;
 	unsigned int next_chan;
+	unsigned short val;
 
 	if (pcl812_ai_eoc(dev, s, NULL, 0)) {
 		dev_dbg(dev->class_dev, "A/D cmd IRQ without DRDY!\n");
@@ -875,13 +863,12 @@ static void pcl812_handle_eoc(struct comedi_device *dev,
 		return;
 	}
 
-	comedi_buf_put(s, pcl812_ai_get_sample(dev, s));
+	val = pcl812_ai_get_sample(dev, s);
+	comedi_buf_write_samples(s, &val, 1);
 
 	/* Set up next channel. Added by abbotti 2010-01-20, but untested. */
-	next_chan = s->async->cur_chan + 1;
-	if (next_chan >= cmd->chanlist_len)
-		next_chan = 0;
-	if (cmd->chanlist[s->async->cur_chan] != cmd->chanlist[next_chan])
+	next_chan = s->async->cur_chan;
+	if (cmd->chanlist[chan] != cmd->chanlist[next_chan])
 		pcl812_ai_set_chan_range(dev, cmd->chanlist[next_chan], 0);
 
 	pcl812_ai_next_chan(dev, s);
@@ -893,9 +880,11 @@ static void transfer_from_dma_buf(struct comedi_device *dev,
 				  unsigned int bufptr, unsigned int len)
 {
 	unsigned int i;
+	unsigned short val;
 
 	for (i = len; i; i--) {
-		comedi_buf_put(s, ptr[bufptr++]);
+		val = ptr[bufptr++];
+		comedi_buf_write_samples(s, &val, 1);
 
 		if (!pcl812_ai_next_chan(dev, s))
 			break;
@@ -939,7 +928,7 @@ static irqreturn_t pcl812_interrupt(int irq, void *d)
 
 	pcl812_ai_clear_eoc(dev);
 
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 	return IRQ_HANDLED;
 }
 
@@ -1335,7 +1324,6 @@ static int pcl812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			break;
 		}
 		s->insn_write	= pcl812_ao_insn_write;
-		s->insn_read	= comedi_readback_insn_read;
 
 		ret = comedi_alloc_subdev_readback(s);
 		if (ret)
diff --git a/drivers/staging/comedi/drivers/pcl816.c b/drivers/staging/comedi/drivers/pcl816.c
index aa6487132017..73deb4bd5c93 100644
--- a/drivers/staging/comedi/drivers/pcl816.c
+++ b/drivers/staging/comedi/drivers/pcl816.c
@@ -122,7 +122,6 @@ struct pcl816_private {
 	int next_dma_buf;	/*  which DMA buffer will be used next round */
 	long dma_runs_to_end;	/*  how many we must permorm DMA transfer to end of record */
 	unsigned long last_dma_run;	/*  how many bytes we must transfer on last DMA page */
-	int ai_act_scan;	/*  how many scans we finished */
 	unsigned int ai_poll_ptr;	/*  how many sampes transfer poll */
 	unsigned int divisor1;
 	unsigned int divisor2;
@@ -160,7 +159,7 @@ static void pcl816_ai_setup_dma(struct comedi_device *dev,
 	bytes = devpriv->hwdmasize;
 	if (cmd->stop_src == TRIG_COUNT) {
 		/*  how many */
-		bytes = cmd->stop_arg * cfc_bytes_per_scan(s);
+		bytes = cmd->stop_arg * comedi_bytes_per_scan(s);
 
 		/*  how many DMA pages we must fill */
 		devpriv->dma_runs_to_end = bytes / devpriv->hwdmasize;
@@ -286,21 +285,10 @@ static int pcl816_ai_eoc(struct comedi_device *dev,
 static bool pcl816_ai_next_chan(struct comedi_device *dev,
 				struct comedi_subdevice *s)
 {
-	struct pcl816_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 
-	s->async->events |= COMEDI_CB_BLOCK;
-
-	s->async->cur_chan++;
-	if (s->async->cur_chan >= cmd->chanlist_len) {
-		s->async->cur_chan = 0;
-		devpriv->ai_act_scan++;
-		s->async->events |= COMEDI_CB_EOS;
-	}
-
 	if (cmd->stop_src == TRIG_COUNT &&
-	    devpriv->ai_act_scan >= cmd->stop_arg) {
-		/* all data sampled */
+	    s->async->scans_done >= cmd->stop_arg) {
 		s->async->events |= COMEDI_CB_EOA;
 		return false;
 	}
@@ -313,10 +301,12 @@ static void transfer_from_dma_buf(struct comedi_device *dev,
 				  unsigned short *ptr,
 				  unsigned int bufptr, unsigned int len)
 {
+	unsigned short val;
 	int i;
 
 	for (i = 0; i < len; i++) {
-		comedi_buf_put(s, ptr[bufptr++]);
+		val = ptr[bufptr++];
+		comedi_buf_write_samples(s, &val, 1);
 
 		if (!pcl816_ai_next_chan(dev, s))
 			return;
@@ -355,7 +345,7 @@ static irqreturn_t pcl816_interrupt(int irq, void *d)
 
 	pcl816_ai_clear_eoc(dev);
 
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 	return IRQ_HANDLED;
 }
 
@@ -508,8 +498,6 @@ static int pcl816_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	pcl816_ai_setup_chanlist(dev, cmd->chanlist, seglen);
 	udelay(1);
 
-	devpriv->ai_act_scan = 0;
-	s->async->cur_chan = 0;
 	devpriv->ai_cmd_running = 1;
 	devpriv->ai_poll_ptr = 0;
 	devpriv->ai_cmd_canceled = 0;
@@ -566,7 +554,7 @@ static int pcl816_ai_poll(struct comedi_device *dev, struct comedi_subdevice *s)
 	devpriv->ai_poll_ptr = top1;	/*  new buffer position */
 	spin_unlock_irqrestore(&dev->spinlock, flags);
 
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 
 	return comedi_buf_n_bytes_ready(s);
 }
diff --git a/drivers/staging/comedi/drivers/pcl818.c b/drivers/staging/comedi/drivers/pcl818.c
index ac19e83ce62a..8edea35532a9 100644
--- a/drivers/staging/comedi/drivers/pcl818.c
+++ b/drivers/staging/comedi/drivers/pcl818.c
@@ -313,8 +313,6 @@ struct pcl818_private {
 	unsigned long last_dma_run;	/*  how many bytes we must transfer on last DMA page */
 	unsigned int ns_min;	/*  manimal allowed delay between samples (in us) for actual card */
 	int i8253_osc_base;	/*  1/frequency of on board oscilator in ns */
-	int ai_act_scan;	/*  how many scans we finished */
-	int ai_act_chan;	/*  actual position in actual scan */
 	unsigned int act_chanlist[16];	/*  MUX setting for actual AI operations */
 	unsigned int act_chanlist_len;	/*  how long is actual MUX list */
 	unsigned int act_chanlist_pos;	/*  actual position in MUX list */
@@ -352,7 +350,7 @@ static void pcl818_ai_setup_dma(struct comedi_device *dev,
 	disable_dma(devpriv->dma);	/*  disable dma */
 	bytes = devpriv->hwdmasize;
 	if (cmd->stop_src == TRIG_COUNT) {
-		bytes = cmd->stop_arg * cfc_bytes_per_scan(s);
+		bytes = cmd->stop_arg * comedi_bytes_per_scan(s);
 		devpriv->dma_runs_to_end = bytes / devpriv->hwdmasize;
 		devpriv->last_dma_run = bytes % devpriv->hwdmasize;
 		devpriv->dma_runs_to_end--;
@@ -521,21 +519,12 @@ static bool pcl818_ai_next_chan(struct comedi_device *dev,
 	struct pcl818_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 
-	s->async->events |= COMEDI_CB_BLOCK;
-
 	devpriv->act_chanlist_pos++;
 	if (devpriv->act_chanlist_pos >= devpriv->act_chanlist_len)
 		devpriv->act_chanlist_pos = 0;
 
-	s->async->cur_chan++;
-	if (s->async->cur_chan >= cmd->chanlist_len) {
-		s->async->cur_chan = 0;
-		devpriv->ai_act_scan--;
-		s->async->events |= COMEDI_CB_EOS;
-	}
-
-	if (cmd->stop_src == TRIG_COUNT && devpriv->ai_act_scan == 0) {
-		/* all data sampled */
+	if (cmd->stop_src == TRIG_COUNT &&
+	    s->async->scans_done >= cmd->stop_arg) {
 		s->async->events |= COMEDI_CB_EOA;
 		return false;
 	}
@@ -560,7 +549,7 @@ static void pcl818_handle_eoc(struct comedi_device *dev,
 	if (pcl818_ai_dropout(dev, s, chan))
 		return;
 
-	comedi_buf_put(s, val);
+	comedi_buf_write_samples(s, &val, 1);
 
 	pcl818_ai_next_chan(dev, s);
 }
@@ -589,7 +578,7 @@ static void pcl818_handle_dma(struct comedi_device *dev,
 		if (pcl818_ai_dropout(dev, s, chan))
 			break;
 
-		comedi_buf_put(s, val);
+		comedi_buf_write_samples(s, &val, 1);
 
 		if (!pcl818_ai_next_chan(dev, s))
 			break;
@@ -630,7 +619,7 @@ static void pcl818_handle_fifo(struct comedi_device *dev,
 		if (pcl818_ai_dropout(dev, s, chan))
 			break;
 
-		comedi_buf_put(s, val);
+		comedi_buf_write_samples(s, &val, 1);
 
 		if (!pcl818_ai_next_chan(dev, s))
 			break;
@@ -642,6 +631,7 @@ static irqreturn_t pcl818_interrupt(int irq, void *d)
 	struct comedi_device *dev = d;
 	struct pcl818_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_cmd *cmd = &s->async->cmd;
 
 	if (!dev->attached || !devpriv->ai_cmd_running) {
 		pcl818_ai_clear_eoc(dev);
@@ -655,7 +645,7 @@ static irqreturn_t pcl818_interrupt(int irq, void *d)
 		 * being reprogrammed while a DMA transfer is in
 		 * progress.
 		 */
-		devpriv->ai_act_scan = 0;
+		s->async->scans_done = cmd->stop_arg;
 		s->cancel(dev, s);
 		return IRQ_HANDLED;
 	}
@@ -669,7 +659,7 @@ static irqreturn_t pcl818_interrupt(int irq, void *d)
 
 	pcl818_ai_clear_eoc(dev);
 
-	cfc_handle_events(dev, s);
+	comedi_handle_events(dev, s);
 	return IRQ_HANDLED;
 }
 
@@ -829,8 +819,6 @@ static int pcl818_ai_cmd(struct comedi_device *dev,
 	pcl818_ai_setup_chanlist(dev, cmd->chanlist, seglen);
 
 	devpriv->ai_data_len = s->async->prealloc_bufsz;
-	devpriv->ai_act_scan = cmd->stop_arg;
-	devpriv->ai_act_chan = 0;
 	devpriv->ai_cmd_running = 1;
 	devpriv->ai_cmd_canceled = 0;
 	devpriv->act_chanlist_pos = 0;
@@ -873,7 +861,8 @@ static int pcl818_ai_cancel(struct comedi_device *dev,
 
 	if (devpriv->dma) {
 		if (cmd->stop_src == TRIG_NONE ||
-		    (cmd->stop_src == TRIG_COUNT && devpriv->ai_act_scan > 0)) {
+		    (cmd->stop_src == TRIG_COUNT &&
+		     s->async->scans_done < cmd->stop_arg)) {
 			if (!devpriv->ai_cmd_canceled) {
 				/*
 				* Wait for running dma transfer to end,
@@ -1169,7 +1158,6 @@ static int pcl818_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 				s->range_table = &range_unknown;
 		}
 		s->insn_write	= pcl818_ao_insn_write;
-		s->insn_read	= comedi_readback_insn_read;
 
 		ret = comedi_alloc_subdev_readback(s);
 		if (ret)
diff --git a/drivers/staging/comedi/drivers/pcmmio.c b/drivers/staging/comedi/drivers/pcmmio.c
index fc40ee2b34e9..f0059e935da0 100644
--- a/drivers/staging/comedi/drivers/pcmmio.c
+++ b/drivers/staging/comedi/drivers/pcmmio.c
@@ -190,7 +190,6 @@ struct pcmmio_private {
 	spinlock_t pagelock;	/* protects the page registers */
 	spinlock_t spinlock;	/* protects the member variables */
 	unsigned int enabled_mask;
-	unsigned int stop_count;
 	unsigned int active:1;
 };
 
@@ -337,7 +336,6 @@ static void pcmmio_handle_dio_intr(struct comedi_device *dev,
 {
 	struct pcmmio_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
-	unsigned int oldevents = s->async->events;
 	unsigned int val = 0;
 	unsigned long flags;
 	int i;
@@ -357,31 +355,16 @@ static void pcmmio_handle_dio_intr(struct comedi_device *dev,
 			val |= (1 << i);
 	}
 
-	/* Write the scan to the buffer. */
-	if (comedi_buf_put(s, val) &&
-	    comedi_buf_put(s, val >> 16)) {
-		s->async->events |= (COMEDI_CB_BLOCK | COMEDI_CB_EOS);
-	} else {
-		/* Overflow! Stop acquisition!! */
-		/* TODO: STOP_ACQUISITION_CALL_HERE!! */
-		pcmmio_stop_intr(dev, s);
-	}
+	comedi_buf_write_samples(s, &val, 1);
 
-	/* Check for end of acquisition. */
-	if (cmd->stop_src == TRIG_COUNT && devpriv->stop_count > 0) {
-		devpriv->stop_count--;
-		if (devpriv->stop_count == 0) {
-			s->async->events |= COMEDI_CB_EOA;
-			/* TODO: STOP_ACQUISITION_CALL_HERE!! */
-			pcmmio_stop_intr(dev, s);
-		}
-	}
+	if (cmd->stop_src == TRIG_COUNT &&
+	    s->async->scans_done >= cmd->stop_arg)
+		s->async->events |= COMEDI_CB_EOA;
 
 done:
 	spin_unlock_irqrestore(&devpriv->spinlock, flags);
 
-	if (oldevents != s->async->events)
-		comedi_event(dev, s);
+	comedi_handle_events(dev, s);
 }
 
 static irqreturn_t interrupt_pcmmio(int irq, void *d)
@@ -481,8 +464,6 @@ static int pcmmio_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	spin_lock_irqsave(&devpriv->spinlock, flags);
 	devpriv->active = 1;
 
-	devpriv->stop_count = cmd->stop_arg;
-
 	/* Set up start of acquisition. */
 	if (cmd->start_src == TRIG_INT)
 		s->async->inttrig = pcmmio_inttrig_start_intr;
@@ -751,7 +732,6 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->maxdata	= 0xffff;
 	s->range_table	= &pcmmio_ao_ranges;
 	s->insn_write	= pcmmio_ao_insn_write;
-	s->insn_read	= comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
@@ -774,7 +754,7 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->insn_config	= pcmmio_dio_insn_config;
 	if (dev->irq) {
 		dev->read_subdev = s;
-		s->subdev_flags	|= SDF_CMD_READ;
+		s->subdev_flags	|= SDF_CMD_READ | SDF_LSAMPL | SDF_PACKED;
 		s->len_chanlist	= s->n_chan;
 		s->cancel	= pcmmio_cancel;
 		s->do_cmd	= pcmmio_cmd;
diff --git a/drivers/staging/comedi/drivers/pcmuio.c b/drivers/staging/comedi/drivers/pcmuio.c
index d4fe2ec25ecf..0f5483b6147f 100644
--- a/drivers/staging/comedi/drivers/pcmuio.c
+++ b/drivers/staging/comedi/drivers/pcmuio.c
@@ -130,7 +130,6 @@ struct pcmuio_asic {
 	spinlock_t pagelock;	/* protects the page registers */
 	spinlock_t spinlock;	/* protects member variables */
 	unsigned int enabled_mask;
-	unsigned int stop_count;
 	unsigned int active:1;
 };
 
@@ -317,7 +316,6 @@ static void pcmuio_handle_intr_subdev(struct comedi_device *dev,
 	int asic = pcmuio_subdevice_to_asic(s);
 	struct pcmuio_asic *chip = &devpriv->asics[asic];
 	struct comedi_cmd *cmd = &s->async->cmd;
-	unsigned oldevents = s->async->events;
 	unsigned int val = 0;
 	unsigned long flags;
 	unsigned int i;
@@ -337,33 +335,16 @@ static void pcmuio_handle_intr_subdev(struct comedi_device *dev,
 			val |= (1 << i);
 	}
 
-	/* Write the scan to the buffer. */
-	if (comedi_buf_put(s, val) &&
-	    comedi_buf_put(s, val >> 16)) {
-		s->async->events |= (COMEDI_CB_BLOCK | COMEDI_CB_EOS);
-	} else {
-		/* Overflow! Stop acquisition!! */
-		/* TODO: STOP_ACQUISITION_CALL_HERE!! */
-		pcmuio_stop_intr(dev, s);
-	}
+	comedi_buf_write_samples(s, &val, 1);
 
-	/* Check for end of acquisition. */
-	if (cmd->stop_src == TRIG_COUNT) {
-		if (chip->stop_count > 0) {
-			chip->stop_count--;
-			if (chip->stop_count == 0) {
-				s->async->events |= COMEDI_CB_EOA;
-				/* TODO: STOP_ACQUISITION_CALL_HERE!! */
-				pcmuio_stop_intr(dev, s);
-			}
-		}
-	}
+	if (cmd->stop_src == TRIG_COUNT &&
+	    s->async->scans_done >= cmd->stop_arg)
+		s->async->events |= COMEDI_CB_EOA;
 
 done:
 	spin_unlock_irqrestore(&chip->spinlock, flags);
 
-	if (oldevents != s->async->events)
-		comedi_event(dev, s);
+	comedi_handle_events(dev, s);
 }
 
 static int pcmuio_handle_asic_interrupt(struct comedi_device *dev, int asic)
@@ -487,8 +468,6 @@ static int pcmuio_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	spin_lock_irqsave(&chip->spinlock, flags);
 	chip->active = 1;
 
-	chip->stop_count = cmd->stop_arg;
-
 	/* Set up start of acquisition. */
 	if (cmd->start_src == TRIG_INT)
 		s->async->inttrig = pcmuio_inttrig_start_intr;
@@ -614,7 +593,8 @@ static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		if ((i == 0 && dev->irq) || (i == 2 && devpriv->irq2)) {
 			/* setup the interrupt subdevice */
 			dev->read_subdev = s;
-			s->subdev_flags	|= SDF_CMD_READ;
+			s->subdev_flags	|= SDF_CMD_READ | SDF_LSAMPL |
+					   SDF_PACKED;
 			s->len_chanlist	= s->n_chan;
 			s->cancel	= pcmuio_cancel;
 			s->do_cmd	= pcmuio_cmd;
diff --git a/drivers/staging/comedi/drivers/quatech_daqp_cs.c b/drivers/staging/comedi/drivers/quatech_daqp_cs.c
index 6407df0404f0..96098110b0b3 100644
--- a/drivers/staging/comedi/drivers/quatech_daqp_cs.c
+++ b/drivers/staging/comedi/drivers/quatech_daqp_cs.c
@@ -65,8 +65,6 @@ struct daqp_private {
 	enum { semaphore, buffer } interrupt_mode;
 
 	struct completion eos;
-
-	int count;
 };
 
 /* The DAQP communicates with the system through a 16 byte I/O window. */
@@ -194,6 +192,7 @@ static enum irqreturn daqp_interrupt(int irq, void *dev_id)
 	struct comedi_device *dev = dev_id;
 	struct daqp_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	int loop_limit = 10000;
 	int status;
 
@@ -221,18 +220,16 @@ static enum irqreturn daqp_interrupt(int irq, void *dev_id)
 			data |= inb(dev->iobase + DAQP_FIFO) << 8;
 			data ^= 0x8000;
 
-			comedi_buf_put(s, data);
+			comedi_buf_write_samples(s, &data, 1);
 
 			/* If there's a limit, decrement it
 			 * and stop conversion if zero
 			 */
 
-			if (devpriv->count > 0) {
-				devpriv->count--;
-				if (devpriv->count == 0) {
-					s->async->events |= COMEDI_CB_EOA;
-					break;
-				}
+			if (cmd->stop_src == TRIG_COUNT &&
+			    s->async->scans_done >= cmd->stop_arg) {
+				s->async->events |= COMEDI_CB_EOA;
+				break;
 			}
 
 			if ((loop_limit--) <= 0)
@@ -245,9 +242,7 @@ static enum irqreturn daqp_interrupt(int irq, void *dev_id)
 			s->async->events |= COMEDI_CB_EOA | COMEDI_CB_ERROR;
 		}
 
-		s->async->events |= COMEDI_CB_BLOCK;
-
-		cfc_handle_events(dev, s);
+		comedi_handle_events(dev, s);
 	}
 	return IRQ_HANDLED;
 }
@@ -575,12 +570,16 @@ static int daqp_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	 */
 
 	if (cmd->stop_src == TRIG_COUNT) {
-		devpriv->count = cmd->stop_arg * cmd->scan_end_arg;
-		threshold = 2 * devpriv->count;
-		while (threshold > DAQP_FIFO_SIZE * 3 / 4)
-			threshold /= 2;
+		unsigned long long nsamples;
+		unsigned long long nbytes;
+
+		nsamples = (unsigned long long)cmd->stop_arg *
+			   cmd->scan_end_arg;
+		nbytes = nsamples * comedi_bytes_per_sample(s);
+		while (nbytes > DAQP_FIFO_SIZE * 3 / 4)
+			nbytes /= 2;
+		threshold = nbytes;
 	} else {
-		devpriv->count = -1;
 		threshold = DAQP_FIFO_SIZE / 2;
 	}
 
@@ -736,12 +735,11 @@ static int daqp_auto_attach(struct comedi_device *dev,
 
 	s = &dev->subdevices[1];
 	s->type		= COMEDI_SUBD_AO;
-	s->subdev_flags	= SDF_WRITEABLE;
+	s->subdev_flags	= SDF_WRITABLE;
 	s->n_chan	= 2;
 	s->maxdata	= 0x0fff;
 	s->range_table	= &range_bipolar5;
 	s->insn_write	= daqp_ao_insn_write;
-	s->insn_read	= comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
@@ -756,7 +754,7 @@ static int daqp_auto_attach(struct comedi_device *dev,
 
 	s = &dev->subdevices[3];
 	s->type		= COMEDI_SUBD_DO;
-	s->subdev_flags	= SDF_WRITEABLE;
+	s->subdev_flags	= SDF_WRITABLE;
 	s->n_chan	= 1;
 	s->maxdata	= 1;
 	s->insn_bits	= daqp_do_insn_bits;
diff --git a/drivers/staging/comedi/drivers/rtd520.c b/drivers/staging/comedi/drivers/rtd520.c
index 7d4cb140959c..581aa58d9c0a 100644
--- a/drivers/staging/comedi/drivers/rtd520.c
+++ b/drivers/staging/comedi/drivers/rtd520.c
@@ -379,7 +379,6 @@ struct rtd_private {
 	long ai_count;		/* total transfer size (samples) */
 	int xfer_count;		/* # to transfer data. 0->1/2FIFO */
 	int flags;		/* flag event modes */
-	DECLARE_BITMAP(chan_is_bipolar, RTD_MAX_CHANLIST);
 	unsigned fifosz;
 };
 
@@ -438,7 +437,6 @@ static unsigned short rtd_convert_chan_gain(struct comedi_device *dev,
 					    unsigned int chanspec, int index)
 {
 	const struct rtd_boardinfo *board = dev->board_ptr;
-	struct rtd_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(chanspec);
 	unsigned int range = CR_RANGE(chanspec);
 	unsigned int aref = CR_AREF(chanspec);
@@ -451,17 +449,14 @@ static unsigned short rtd_convert_chan_gain(struct comedi_device *dev,
 		/* +-5 range */
 		r |= 0x000;
 		r |= (range & 0x7) << 4;
-		__set_bit(index, devpriv->chan_is_bipolar);
 	} else if (range < board->range_uni10) {
 		/* +-10 range */
 		r |= 0x100;
 		r |= ((range - board->range_bip10) & 0x7) << 4;
-		__set_bit(index, devpriv->chan_is_bipolar);
 	} else {
 		/* +10 range */
 		r |= 0x200;
 		r |= ((range - board->range_uni10) & 0x7) << 4;
-		__clear_bit(index, devpriv->chan_is_bipolar);
 	}
 
 	switch (aref) {
@@ -561,6 +556,7 @@ static int rtd_ai_rinsn(struct comedi_device *dev,
 			unsigned int *data)
 {
 	struct rtd_private *devpriv = dev->private;
+	unsigned int range = CR_RANGE(insn->chanspec);
 	int ret;
 	int n;
 
@@ -586,9 +582,11 @@ static int rtd_ai_rinsn(struct comedi_device *dev,
 		/* read data */
 		d = readw(devpriv->las1 + LAS1_ADC_FIFO);
 		d = d >> 3;	/* low 3 bits are marker lines */
-		if (test_bit(0, devpriv->chan_is_bipolar))
-			/* convert to comedi unsigned data */
+
+		/* convert bipolar data to comedi unsigned data */
+		if (comedi_range_is_bipolar(s, range))
 			d = comedi_offset_munge(s, d);
+
 		data[n] = d & s->maxdata;
 	}
 
@@ -606,9 +604,12 @@ static int ai_read_n(struct comedi_device *dev, struct comedi_subdevice *s,
 		     int count)
 {
 	struct rtd_private *devpriv = dev->private;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
 	int ii;
 
 	for (ii = 0; ii < count; ii++) {
+		unsigned int range = CR_RANGE(cmd->chanlist[async->cur_chan]);
 		unsigned short d;
 
 		if (0 == devpriv->ai_count) {	/* done */
@@ -618,41 +619,13 @@ static int ai_read_n(struct comedi_device *dev, struct comedi_subdevice *s,
 
 		d = readw(devpriv->las1 + LAS1_ADC_FIFO);
 		d = d >> 3;	/* low 3 bits are marker lines */
-		if (test_bit(s->async->cur_chan, devpriv->chan_is_bipolar))
-			/* convert to comedi unsigned data */
-			d = comedi_offset_munge(s, d);
-		d &= s->maxdata;
-
-		if (!comedi_buf_put(s, d))
-			return -1;
-
-		if (devpriv->ai_count > 0)	/* < 0, means read forever */
-			devpriv->ai_count--;
-	}
-	return 0;
-}
-
-/*
-  unknown amout of data is waiting in fifo.
-*/
-static int ai_read_dregs(struct comedi_device *dev, struct comedi_subdevice *s)
-{
-	struct rtd_private *devpriv = dev->private;
-
-	while (readl(dev->mmio + LAS0_ADC) & FS_ADC_NOT_EMPTY) {
-		unsigned short d = readw(devpriv->las1 + LAS1_ADC_FIFO);
-
-		if (0 == devpriv->ai_count) {	/* done */
-			continue;	/* read rest */
-		}
 
-		d = d >> 3;	/* low 3 bits are marker lines */
-		if (test_bit(s->async->cur_chan, devpriv->chan_is_bipolar))
-			/* convert to comedi unsigned data */
+		/* convert bipolar data to comedi unsigned data */
+		if (comedi_range_is_bipolar(s, range))
 			d = comedi_offset_munge(s, d);
 		d &= s->maxdata;
 
-		if (!comedi_buf_put(s, d))
+		if (!comedi_buf_write_samples(s, &d, 1))
 			return -1;
 
 		if (devpriv->ai_count > 0)	/* < 0, means read forever */
@@ -703,8 +676,6 @@ static irqreturn_t rtd_interrupt(int irq, void *d)
 
 			if (0 == devpriv->ai_count)
 				goto xfer_done;
-
-			comedi_event(dev, s);
 		} else if (devpriv->xfer_count > 0) {
 			if (fifo_status & FS_ADC_NOT_EMPTY) {
 				/* FIFO not empty */
@@ -713,8 +684,6 @@ static irqreturn_t rtd_interrupt(int irq, void *d)
 
 				if (0 == devpriv->ai_count)
 					goto xfer_done;
-
-				comedi_event(dev, s);
 			}
 		}
 	}
@@ -726,28 +695,16 @@ static irqreturn_t rtd_interrupt(int irq, void *d)
 	/* clear the interrupt */
 	writew(status, dev->mmio + LAS0_CLEAR);
 	readw(dev->mmio + LAS0_CLEAR);
+
+	comedi_handle_events(dev, s);
+
 	return IRQ_HANDLED;
 
 xfer_abort:
-	writel(0, dev->mmio + LAS0_ADC_FIFO_CLEAR);
 	s->async->events |= COMEDI_CB_ERROR;
-	devpriv->ai_count = 0;	/* stop and don't transfer any more */
-	/* fall into xfer_done */
 
 xfer_done:
-	/* pacer stop source: SOFTWARE */
-	writel(0, dev->mmio + LAS0_PACER_STOP);
-	writel(0, dev->mmio + LAS0_PACER);	/* stop pacer */
-	writel(0, dev->mmio + LAS0_ADC_CONVERSION);
-	writew(0, dev->mmio + LAS0_IT);
-
-	if (devpriv->ai_count > 0) {	/* there shouldn't be anything left */
-		fifo_status = readl(dev->mmio + LAS0_ADC);
-		ai_read_dregs(dev, s);	/* read anything left in FIFO */
-	}
-
-	s->async->events |= COMEDI_CB_EOA;	/* signal end to comedi */
-	comedi_event(dev, s);
+	s->async->events |= COMEDI_CB_EOA;
 
 	/* clear the interrupt */
 	status = readw(dev->mmio + LAS0_IT);
@@ -757,6 +714,8 @@ xfer_done:
 	fifo_status = readl(dev->mmio + LAS0_ADC);
 	overrun = readl(dev->mmio + LAS0_OVERRUN) & 0xffff;
 
+	comedi_handle_events(dev, s);
+
 	return IRQ_HANDLED;
 }
 
@@ -1083,6 +1042,7 @@ static int rtd_ai_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 	devpriv->ai_count = 0;	/* stop and don't transfer any more */
 	status = readw(dev->mmio + LAS0_IT);
 	overrun = readl(dev->mmio + LAS0_OVERRUN) & 0xffff;
+	writel(0, dev->mmio + LAS0_ADC_FIFO_CLEAR);
 	return 0;
 }
 
@@ -1303,7 +1263,6 @@ static int rtd_auto_attach(struct comedi_device *dev,
 	s->maxdata	= 0x0fff;
 	s->range_table	= &rtd_ao_range;
 	s->insn_write	= rtd_ao_winsn;
-	s->insn_read	= comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
diff --git a/drivers/staging/comedi/drivers/rti800.c b/drivers/staging/comedi/drivers/rti800.c
index e3d9f44cefb9..67b4b378bd01 100644
--- a/drivers/staging/comedi/drivers/rti800.c
+++ b/drivers/staging/comedi/drivers/rti800.c
@@ -313,7 +313,6 @@ static int rti800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 				? rti800_ao_ranges[it->options[7]]
 				: &range_unknown;
 		s->insn_write	= rti800_ao_insn_write;
-		s->insn_read	= comedi_readback_insn_read;
 
 		ret = comedi_alloc_subdev_readback(s);
 		if (ret)
diff --git a/drivers/staging/comedi/drivers/rti802.c b/drivers/staging/comedi/drivers/rti802.c
index c81b01c40f12..96c3974207ae 100644
--- a/drivers/staging/comedi/drivers/rti802.c
+++ b/drivers/staging/comedi/drivers/rti802.c
@@ -100,7 +100,6 @@ static int rti802_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->maxdata	= 0xfff;
 	s->n_chan	= 8;
 	s->insn_write	= rti802_ao_insn_write;
-	s->insn_read	= comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
diff --git a/drivers/staging/comedi/drivers/s526.c b/drivers/staging/comedi/drivers/s526.c
index 75872c6aec2a..6f3e8a08e75c 100644
--- a/drivers/staging/comedi/drivers/s526.c
+++ b/drivers/staging/comedi/drivers/s526.c
@@ -583,7 +583,6 @@ static int s526_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->maxdata = 0xffff;
 	s->range_table = &range_bipolar10;
 	s->insn_write = s526_ao_insn_write;
-	s->insn_read = comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
diff --git a/drivers/staging/comedi/drivers/s626.c b/drivers/staging/comedi/drivers/s626.c
index 0e7621e890c3..14932c5f3798 100644
--- a/drivers/staging/comedi/drivers/s626.c
+++ b/drivers/staging/comedi/drivers/s626.c
@@ -78,7 +78,6 @@ struct s626_buffer_dma {
 
 struct s626_private {
 	uint8_t ai_cmd_running;		/* ai_cmd is running */
-	int ai_sample_count;		/* number of samples to acquire */
 	unsigned int ai_sample_timer;	/* time between samples in
 					 * units of the timer */
 	int ai_convert_count;		/* conversion counter */
@@ -1480,7 +1479,6 @@ static bool s626_handle_eos_interrupt(struct comedi_device *dev)
 	 * from the final ADC of the previous poll list scan.
 	 */
 	uint32_t *readaddr = (uint32_t *)devpriv->ana_buf.logical_base + 1;
-	bool finished = false;
 	int i;
 
 	/* get the data and hand it over to comedi */
@@ -1494,36 +1492,21 @@ static bool s626_handle_eos_interrupt(struct comedi_device *dev)
 		tempdata = s626_ai_reg_to_uint(*readaddr);
 		readaddr++;
 
-		/* put data into read buffer */
-		cfc_write_to_buffer(s, tempdata);
+		comedi_buf_write_samples(s, &tempdata, 1);
 	}
 
-	/* end of scan occurs */
-	async->events |= COMEDI_CB_EOS;
+	if (cmd->stop_src == TRIG_COUNT && async->scans_done >= cmd->stop_arg)
+		async->events |= COMEDI_CB_EOA;
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		devpriv->ai_sample_count--;
-		if (devpriv->ai_sample_count <= 0) {
-			devpriv->ai_cmd_running = 0;
-
-			/* Stop RPS program */
-			s626_mc_disable(dev, S626_MC1_ERPS1, S626_P_MC1);
-
-			/* send end of acquisition */
-			async->events |= COMEDI_CB_EOA;
-
-			/* disable master interrupt */
-			finished = true;
-		}
-	}
+	if (async->events & COMEDI_CB_CANCEL_MASK)
+		devpriv->ai_cmd_running = 0;
 
 	if (devpriv->ai_cmd_running && cmd->scan_begin_src == TRIG_EXT)
 		s626_dio_set_irq(dev, cmd->scan_begin_arg);
 
-	/* tell comedi that data is there */
-	comedi_event(dev, s);
+	comedi_handle_events(dev, s);
 
-	return finished;
+	return !devpriv->ai_cmd_running;
 }
 
 static irqreturn_t s626_irq_handler(int irq, void *d)
@@ -1970,13 +1953,13 @@ static int s626_ns_to_timer(unsigned int *nanosec, unsigned int flags)
 	switch (flags & CMDF_ROUND_MASK) {
 	case CMDF_ROUND_NEAREST:
 	default:
-		divider = (*nanosec + base / 2) / base;
+		divider = DIV_ROUND_CLOSEST(*nanosec, base);
 		break;
 	case CMDF_ROUND_DOWN:
 		divider = (*nanosec) / base;
 		break;
 	case CMDF_ROUND_UP:
-		divider = (*nanosec + base - 1) / base;
+		divider = DIV_ROUND_UP(*nanosec, base);
 		break;
 	}
 
@@ -2102,8 +2085,6 @@ static int s626_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		break;
 	}
 
-	devpriv->ai_sample_count = cmd->stop_arg;
-
 	s626_reset_adc(dev, ppl);
 
 	switch (cmd->start_src) {
@@ -2820,7 +2801,6 @@ static int s626_auto_attach(struct comedi_device *dev,
 	s->maxdata	= 0x3fff;
 	s->range_table	= &range_bipolar10;
 	s->insn_write	= s626_ao_insn_write;
-	s->insn_read	= comedi_readback_insn_read;
 
 	ret = comedi_alloc_subdev_readback(s);
 	if (ret)
diff --git a/drivers/staging/comedi/drivers/serial2002.c b/drivers/staging/comedi/drivers/serial2002.c
index 167f82418cb4..71226ee9064e 100644
--- a/drivers/staging/comedi/drivers/serial2002.c
+++ b/drivers/staging/comedi/drivers/serial2002.c
@@ -742,7 +742,7 @@ static int serial2002_attach(struct comedi_device *dev,
 	/* digital output subdevice */
 	s = &dev->subdevices[1];
 	s->type		= COMEDI_SUBD_DO;
-	s->subdev_flags	= SDF_WRITEABLE;
+	s->subdev_flags	= SDF_WRITABLE;
 	s->n_chan	= 0;
 	s->maxdata	= 1;
 	s->range_table	= &range_digital;
@@ -760,7 +760,7 @@ static int serial2002_attach(struct comedi_device *dev,
 	/* analog output subdevice */
 	s = &dev->subdevices[3];
 	s->type		= COMEDI_SUBD_AO;
-	s->subdev_flags	= SDF_WRITEABLE;
+	s->subdev_flags	= SDF_WRITABLE;
 	s->n_chan	= 0;
 	s->maxdata	= 1;
 	s->range_table	= NULL;
diff --git a/drivers/staging/comedi/drivers/usbdux.c b/drivers/staging/comedi/drivers/usbdux.c
index 5adbfedf780f..4737dbf8e01d 100644
--- a/drivers/staging/comedi/drivers/usbdux.c
+++ b/drivers/staging/comedi/drivers/usbdux.c
@@ -1,37 +1,34 @@
 /*
-   comedi/drivers/usbdux.c
-   Copyright (C) 2003-2007 Bernd Porr, Bernd.Porr@f2s.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.
+ * usbdux.c
+ * Copyright (C) 2003-2014 Bernd Porr, mail@berndporr.me.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.
+
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
  */
+
 /*
-Driver: usbdux
-Description: University of Stirling USB DAQ & INCITE Technology Limited
-Devices: [ITL] USB-DUX (usbdux.o)
-Author: Bernd Porr <BerndPorr@f2s.com>
-Updated: 8 Dec 2008
-Status: Stable
-Configuration options:
-  You have to upload firmware with the -i option. The
-  firmware is usually installed under /usr/share/usb or
-  /usr/local/share/usb or /lib/firmware.
-
-Connection scheme for the counter at the digital port:
-  0=/CLK0, 1=UP/DOWN0, 2=RESET0, 4=/CLK1, 5=UP/DOWN1, 6=RESET1.
-  The sampling rate of the counter is approximately 500Hz.
-
-Please note that under USB2.0 the length of the channel list determines
-the max sampling rate. If you sample only one channel you get 8kHz
-sampling rate. If you sample two channels you get 4kHz and so on.
-*/
+ * Driver: usbdux
+ * Description: University of Stirling USB DAQ & INCITE Technology Limited
+ * Devices: (ITL) USB-DUX [usbdux]
+ * Author: Bernd Porr <mail@berndporr.me.uk>
+ * Updated: 10 Oct 2014
+ * Status: Stable
+ * Connection scheme for the counter at the digital port:
+ * 0=/CLK0, 1=UP/DOWN0, 2=RESET0, 4=/CLK1, 5=UP/DOWN1, 6=RESET1.
+ * The sampling rate of the counter is approximately 500Hz.
+ *
+ * Note that under USB2.0 the length of the channel list determines
+ * the max sampling rate. If you sample only one channel you get 8kHz
+ * sampling rate. If you sample two channels you get 4kHz and so on.
+ */
+
 /*
  * I must give credit here to Chris Baugher who
  * wrote the driver for AT-MIO-16d. I used some parts of this
@@ -205,9 +202,6 @@ struct usbdux_private {
 	unsigned int ao_cmd_running:1;
 	unsigned int pwm_cmd_running:1;
 
-	/* number of samples to acquire */
-	int ai_sample_count;
-	int ao_sample_count;
 	/* time between samples in units of the timer */
 	unsigned int ai_timer;
 	unsigned int ao_timer;
@@ -253,128 +247,107 @@ static int usbdux_ai_cancel(struct comedi_device *dev,
 	return 0;
 }
 
-/* analogue IN - interrupt service routine */
+static void usbduxsub_ai_handle_urb(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct urb *urb)
+{
+	struct usbdux_private *devpriv = dev->private;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
+	int ret;
+	int i;
+
+	devpriv->ai_counter--;
+	if (devpriv->ai_counter == 0) {
+		devpriv->ai_counter = devpriv->ai_timer;
+
+		/* get the data from the USB bus and hand it over to comedi */
+		for (i = 0; i < cmd->chanlist_len; i++) {
+			unsigned int range = CR_RANGE(cmd->chanlist[i]);
+			uint16_t val = le16_to_cpu(devpriv->in_buf[i]);
+
+			/* bipolar data is two's-complement */
+			if (comedi_range_is_bipolar(s, range))
+				val ^= ((s->maxdata + 1) >> 1);
+
+			/* transfer data */
+			if (!comedi_buf_write_samples(s, &val, 1))
+				return;
+		}
+
+		if (cmd->stop_src == TRIG_COUNT &&
+		    async->scans_done >= cmd->stop_arg)
+			async->events |= COMEDI_CB_EOA;
+	}
+
+	/* if command is still running, resubmit urb */
+	if (!(async->events & COMEDI_CB_CANCEL_MASK)) {
+		urb->dev = comedi_to_usb_dev(dev);
+		ret = usb_submit_urb(urb, GFP_ATOMIC);
+		if (ret < 0) {
+			dev_err(dev->class_dev,
+				"urb resubmit failed in int-context! err=%d\n",
+				ret);
+			if (ret == -EL2NSYNC)
+				dev_err(dev->class_dev,
+					"buggy USB host controller or bug in IRQ handler!\n");
+			async->events |= COMEDI_CB_ERROR;
+		}
+	}
+}
+
 static void usbduxsub_ai_isoc_irq(struct urb *urb)
 {
 	struct comedi_device *dev = urb->context;
 	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_async *async = s->async;
 	struct usbdux_private *devpriv = dev->private;
-	struct comedi_cmd *cmd = &s->async->cmd;
-	int i, err;
 
-	/* first we test if something unusual has just happened */
+	/* exit if not running a command, do not resubmit urb */
+	if (!devpriv->ai_cmd_running)
+		return;
+
 	switch (urb->status) {
 	case 0:
 		/* copy the result in the transfer buffer */
 		memcpy(devpriv->in_buf, urb->transfer_buffer, SIZEINBUF);
+		usbduxsub_ai_handle_urb(dev, s, urb);
 		break;
+
 	case -EILSEQ:
-		/* error in the ISOchronous data */
-		/* we don't copy the data into the transfer buffer */
-		/* and recycle the last data byte */
+		/*
+		 * error in the ISOchronous data
+		 * we don't copy the data into the transfer buffer
+		 * and recycle the last data byte
+		 */
 		dev_dbg(dev->class_dev, "CRC error in ISO IN stream\n");
+		usbduxsub_ai_handle_urb(dev, s, urb);
 		break;
 
 	case -ECONNRESET:
 	case -ENOENT:
 	case -ESHUTDOWN:
 	case -ECONNABORTED:
-		/* happens after an unlink command */
-		if (devpriv->ai_cmd_running) {
-			s->async->events |= COMEDI_CB_EOA;
-			s->async->events |= COMEDI_CB_ERROR;
-			comedi_event(dev, s);
-			/* stop the transfer w/o unlink */
-			usbdux_ai_stop(dev, 0);
-		}
-		return;
+		/* after an unlink command, unplug, ... etc */
+		async->events |= COMEDI_CB_ERROR;
+		break;
 
 	default:
-		/* a real error on the bus */
-		/* pass error to comedi if we are really running a command */
-		if (devpriv->ai_cmd_running) {
-			dev_err(dev->class_dev,
-				"Non-zero urb status received in ai intr context: %d\n",
-				urb->status);
-			s->async->events |= COMEDI_CB_EOA;
-			s->async->events |= COMEDI_CB_ERROR;
-			comedi_event(dev, s);
-			/* don't do an unlink here */
-			usbdux_ai_stop(dev, 0);
-		}
-		return;
+		/* a real error */
+		dev_err(dev->class_dev,
+			"Non-zero urb status received in ai intr context: %d\n",
+			urb->status);
+		async->events |= COMEDI_CB_ERROR;
+		break;
 	}
 
 	/*
-	 * at this point we are reasonably sure that nothing dodgy has happened
-	 * are we running a command?
+	 * comedi_handle_events() cannot be used in this driver. The (*cancel)
+	 * operation would unlink the urb.
 	 */
-	if (unlikely(!devpriv->ai_cmd_running)) {
-		/*
-		 * not running a command, do not continue execution if no
-		 * asynchronous command is running in particular not resubmit
-		 */
-		return;
-	}
-
-	urb->dev = comedi_to_usb_dev(dev);
-
-	/* resubmit the urb */
-	err = usb_submit_urb(urb, GFP_ATOMIC);
-	if (unlikely(err < 0)) {
-		dev_err(dev->class_dev,
-			"urb resubmit failed in int-context! err=%d\n", err);
-		if (err == -EL2NSYNC)
-			dev_err(dev->class_dev,
-				"buggy USB host controller or bug in IRQ handler!\n");
-		s->async->events |= COMEDI_CB_EOA;
-		s->async->events |= COMEDI_CB_ERROR;
-		comedi_event(dev, s);
-		/* don't do an unlink here */
+	if (async->events & COMEDI_CB_CANCEL_MASK)
 		usbdux_ai_stop(dev, 0);
-		return;
-	}
-
-	devpriv->ai_counter--;
-	if (likely(devpriv->ai_counter > 0))
-		return;
-
-	/* timer zero, transfer measurements to comedi */
-	devpriv->ai_counter = devpriv->ai_timer;
-
-	/* test, if we transmit only a fixed number of samples */
-	if (cmd->stop_src == TRIG_COUNT) {
-		/* not continuous, fixed number of samples */
-		devpriv->ai_sample_count--;
-		/* all samples received? */
-		if (devpriv->ai_sample_count < 0) {
-			/* prevent a resubmit next time */
-			usbdux_ai_stop(dev, 0);
-			/* say comedi that the acquistion is over */
-			s->async->events |= COMEDI_CB_EOA;
-			comedi_event(dev, s);
-			return;
-		}
-	}
-	/* get the data from the USB bus and hand it over to comedi */
-	for (i = 0; i < cmd->chanlist_len; i++) {
-		unsigned int range = CR_RANGE(cmd->chanlist[i]);
-		uint16_t val = le16_to_cpu(devpriv->in_buf[i]);
-
-		/* bipolar data is two's-complement */
-		if (comedi_range_is_bipolar(s, range))
-			val ^= ((s->maxdata + 1) >> 1);
 
-		/* transfer data */
-		err = comedi_buf_put(s, val);
-		if (unlikely(err == 0)) {
-			/* buffer overflow */
-			usbdux_ai_stop(dev, 0);
-			return;
-		}
-	}
-	/* tell comedi that data is there */
-	s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
 	comedi_event(dev, s);
 }
 
@@ -402,71 +375,25 @@ static int usbdux_ao_cancel(struct comedi_device *dev,
 	return 0;
 }
 
-static void usbduxsub_ao_isoc_irq(struct urb *urb)
+static void usbduxsub_ao_handle_urb(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct urb *urb)
 {
-	struct comedi_device *dev = urb->context;
-	struct comedi_subdevice *s = dev->write_subdev;
 	struct usbdux_private *devpriv = dev->private;
-	struct comedi_cmd *cmd = &s->async->cmd;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
 	uint8_t *datap;
 	int ret;
 	int i;
 
-	switch (urb->status) {
-	case 0:
-		/* success */
-		break;
-
-	case -ECONNRESET:
-	case -ENOENT:
-	case -ESHUTDOWN:
-	case -ECONNABORTED:
-		/* after an unlink command, unplug, ... etc */
-		/* no unlink needed here. Already shutting down. */
-		if (devpriv->ao_cmd_running) {
-			s->async->events |= COMEDI_CB_EOA;
-			comedi_event(dev, s);
-			usbdux_ao_stop(dev, 0);
-		}
-		return;
-
-	default:
-		/* a real error */
-		if (devpriv->ao_cmd_running) {
-			dev_err(dev->class_dev,
-				"Non-zero urb status received in ao intr context: %d\n",
-				urb->status);
-			s->async->events |= COMEDI_CB_ERROR;
-			s->async->events |= COMEDI_CB_EOA;
-			comedi_event(dev, s);
-			/* we do an unlink if we are in the high speed mode */
-			usbdux_ao_stop(dev, 0);
-		}
-		return;
-	}
-
-	/* are we actually running? */
-	if (!devpriv->ao_cmd_running)
-		return;
-
-	/* normal operation: executing a command in this subdevice */
 	devpriv->ao_counter--;
-	if ((int)devpriv->ao_counter <= 0) {
-		/* timer zero */
+	if (devpriv->ao_counter == 0) {
 		devpriv->ao_counter = devpriv->ao_timer;
 
-		/* handle non continous acquisition */
-		if (cmd->stop_src == TRIG_COUNT) {
-			/* fixed number of samples */
-			devpriv->ao_sample_count--;
-			if (devpriv->ao_sample_count < 0) {
-				/* all samples transmitted */
-				usbdux_ao_stop(dev, 0);
-				s->async->events |= COMEDI_CB_EOA;
-				comedi_event(dev, s);
-				/* no resubmit of the urb */
-				return;
-			}
+		if (cmd->stop_src == TRIG_COUNT &&
+		    async->scans_done >= cmd->stop_arg) {
+			async->events |= COMEDI_CB_EOA;
+			return;
 		}
 
 		/* transmit data to the USB bus */
@@ -476,26 +403,25 @@ static void usbduxsub_ao_isoc_irq(struct urb *urb)
 			unsigned int chan = CR_CHAN(cmd->chanlist[i]);
 			unsigned short val;
 
-			ret = comedi_buf_get(s, &val);
-			if (ret < 0) {
+			if (!comedi_buf_read_samples(s, &val, 1)) {
 				dev_err(dev->class_dev, "buffer underflow\n");
-				s->async->events |= (COMEDI_CB_EOA |
-						     COMEDI_CB_OVERFLOW);
+				async->events |= COMEDI_CB_OVERFLOW;
+				return;
 			}
+
 			/* pointer to the DA */
 			*datap++ = val & 0xff;
 			*datap++ = (val >> 8) & 0xff;
 			*datap++ = chan << 6;
 			s->readback[chan] = val;
-
-			s->async->events |= COMEDI_CB_BLOCK;
-			comedi_event(dev, s);
 		}
 	}
-	urb->transfer_buffer_length = SIZEOUTBUF;
-	urb->dev = comedi_to_usb_dev(dev);
-	urb->status = 0;
-	if (devpriv->ao_cmd_running) {
+
+	/* if command is still running, resubmit urb for BULK transfer */
+	if (!(async->events & COMEDI_CB_CANCEL_MASK)) {
+		urb->transfer_buffer_length = SIZEOUTBUF;
+		urb->dev = comedi_to_usb_dev(dev);
+		urb->status = 0;
 		if (devpriv->high_speed)
 			urb->interval = 8;	/* uframes */
 		else
@@ -512,16 +438,54 @@ static void usbduxsub_ao_isoc_irq(struct urb *urb)
 			if (ret == -EL2NSYNC)
 				dev_err(dev->class_dev,
 					"buggy USB host controller or bug in IRQ handling!\n");
-
-			s->async->events |= COMEDI_CB_EOA;
-			s->async->events |= COMEDI_CB_ERROR;
-			comedi_event(dev, s);
-			/* don't do an unlink here */
-			usbdux_ao_stop(dev, 0);
+			async->events |= COMEDI_CB_ERROR;
 		}
 	}
 }
 
+static void usbduxsub_ao_isoc_irq(struct urb *urb)
+{
+	struct comedi_device *dev = urb->context;
+	struct comedi_subdevice *s = dev->write_subdev;
+	struct comedi_async *async = s->async;
+	struct usbdux_private *devpriv = dev->private;
+
+	/* exit if not running a command, do not resubmit urb */
+	if (!devpriv->ao_cmd_running)
+		return;
+
+	switch (urb->status) {
+	case 0:
+		usbduxsub_ao_handle_urb(dev, s, urb);
+		break;
+
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+	case -ECONNABORTED:
+		/* after an unlink command, unplug, ... etc */
+		async->events |= COMEDI_CB_ERROR;
+		break;
+
+	default:
+		/* a real error */
+		dev_err(dev->class_dev,
+			"Non-zero urb status received in ao intr context: %d\n",
+			urb->status);
+		async->events |= COMEDI_CB_ERROR;
+		break;
+	}
+
+	/*
+	 * comedi_handle_events() cannot be used in this driver. The (*cancel)
+	 * operation would unlink the urb.
+	 */
+	if (async->events & COMEDI_CB_CANCEL_MASK)
+		usbdux_ao_stop(dev, 0);
+
+	comedi_event(dev, s);
+}
+
 static int usbdux_submit_urbs(struct comedi_device *dev,
 			      struct urb **urbs, int num_urbs,
 			      int input_urb)
@@ -725,9 +689,6 @@ static int usbdux_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	if (devpriv->ai_cmd_running)
 		goto ai_cmd_exit;
 
-	/* set current channel of the running acquisition to zero */
-	s->async->cur_chan = 0;
-
 	devpriv->dux_commands[1] = len;
 	for (i = 0; i < len; ++i) {
 		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
@@ -765,14 +726,6 @@ static int usbdux_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	devpriv->ai_counter = devpriv->ai_timer;
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		/* data arrives as one packet */
-		devpriv->ai_sample_count = cmd->stop_arg;
-	} else {
-		/* continous acquisition */
-		devpriv->ai_sample_count = 0;
-	}
-
 	if (cmd->start_src == TRIG_NOW) {
 		/* enable this acquisition operation */
 		devpriv->ai_cmd_running = 1;
@@ -1023,9 +976,6 @@ static int usbdux_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	if (devpriv->ao_cmd_running)
 		goto ao_cmd_exit;
 
-	/* set current channel of the running acquisition to zero */
-	s->async->cur_chan = 0;
-
 	/* we count in steps of 1ms (125us) */
 	/* 125us mode not used yet */
 	if (0) {		/* (devpriv->high_speed) */
@@ -1044,24 +994,6 @@ static int usbdux_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	devpriv->ao_counter = devpriv->ao_timer;
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		/* not continuous */
-		/* counter */
-		/* high speed also scans everything at once */
-		if (0) {	/* (devpriv->high_speed) */
-			devpriv->ao_sample_count = cmd->stop_arg *
-						   cmd->scan_end_arg;
-		} else {
-			/* there's no scan as the scan has been */
-			/* perf inside the FX2 */
-			/* data arrives as one packet */
-			devpriv->ao_sample_count = cmd->stop_arg;
-		}
-	} else {
-		/* continous acquisition */
-		devpriv->ao_sample_count = 0;
-	}
-
 	if (cmd->start_src == TRIG_NOW) {
 		/* enable this acquisition operation */
 		devpriv->ao_cmd_running = 1;
diff --git a/drivers/staging/comedi/drivers/usbduxfast.c b/drivers/staging/comedi/drivers/usbduxfast.c
index f85818dd5e11..ddc4cb9d5ed4 100644
--- a/drivers/staging/comedi/drivers/usbduxfast.c
+++ b/drivers/staging/comedi/drivers/usbduxfast.c
@@ -1,5 +1,5 @@
 /*
- *  Copyright (C) 2004 Bernd Porr, Bernd.Porr@f2s.com
+ *  Copyright (C) 2004-2014 Bernd Porr, mail@berndporr.me.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
@@ -13,6 +13,15 @@
  */
 
 /*
+ * Driver: usbduxfast
+ * Description: University of Stirling USB DAQ & INCITE Technology Limited
+ * Devices: (ITL) USB-DUX [usbduxfast]
+ * Author: Bernd Porr <mail@berndporr.me.uk>
+ * Updated: 10 Oct 2014
+ * Status: stable
+ */
+
+/*
  * I must give credit here to Chris Baugher who
  * wrote the driver for AT-MIO-16d. I used some parts of this
  * driver. I also must give credits to David Brownell
@@ -152,7 +161,6 @@ struct usbduxfast_private {
 	uint8_t *duxbuf;
 	int8_t *inbuf;
 	short int ai_cmd_running;	/* asynchronous command is running */
-	long int ai_sample_count;	/* number of samples to acquire */
 	int ignore;		/* counter which ignores the first
 				   buffers */
 	struct semaphore sem;
@@ -227,114 +235,82 @@ static int usbduxfast_ai_cancel(struct comedi_device *dev,
 	return ret;
 }
 
-/*
- * analogue IN
- * interrupt service routine
- */
+static void usbduxfast_ai_handle_urb(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     struct urb *urb)
+{
+	struct usbduxfast_private *devpriv = dev->private;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
+	int ret;
+
+	if (devpriv->ignore) {
+		devpriv->ignore--;
+	} else {
+		unsigned int nsamples;
+
+		nsamples = comedi_bytes_to_samples(s, urb->actual_length);
+		nsamples = comedi_nsamples_left(s, nsamples);
+		comedi_buf_write_samples(s, urb->transfer_buffer, nsamples);
+
+		if (cmd->stop_src == TRIG_COUNT &&
+		    async->scans_done >= cmd->stop_arg)
+			async->events |= COMEDI_CB_EOA;
+	}
+
+	/* if command is still running, resubmit urb for BULK transfer */
+	if (!(async->events & COMEDI_CB_CANCEL_MASK)) {
+		urb->dev = comedi_to_usb_dev(dev);
+		urb->status = 0;
+		ret = usb_submit_urb(urb, GFP_ATOMIC);
+		if (ret < 0) {
+			dev_err(dev->class_dev, "urb resubm failed: %d", ret);
+			async->events |= COMEDI_CB_ERROR;
+		}
+	}
+}
+
 static void usbduxfast_ai_interrupt(struct urb *urb)
 {
 	struct comedi_device *dev = urb->context;
 	struct comedi_subdevice *s = dev->read_subdev;
 	struct comedi_async *async = s->async;
-	struct comedi_cmd *cmd = &async->cmd;
-	struct usb_device *usb = comedi_to_usb_dev(dev);
 	struct usbduxfast_private *devpriv = dev->private;
-	int n, err;
 
-	/* are we running a command? */
-	if (unlikely(!devpriv->ai_cmd_running)) {
-		/*
-		 * not running a command
-		 * do not continue execution if no asynchronous command
-		 * is running in particular not resubmit
-		 */
+	/* exit if not running a command, do not resubmit urb */
+	if (!devpriv->ai_cmd_running)
 		return;
-	}
 
-	/* first we test if something unusual has just happened */
 	switch (urb->status) {
 	case 0:
+		usbduxfast_ai_handle_urb(dev, s, urb);
 		break;
 
-		/*
-		 * happens after an unlink command or when the device
-		 * is plugged out
-		 */
 	case -ECONNRESET:
 	case -ENOENT:
 	case -ESHUTDOWN:
 	case -ECONNABORTED:
-		/* tell this comedi */
-		async->events |= COMEDI_CB_EOA;
+		/* after an unlink command, unplug, ... etc */
 		async->events |= COMEDI_CB_ERROR;
-		comedi_event(dev, s);
-		/* stop the transfer w/o unlink */
-		usbduxfast_ai_stop(dev, 0);
-		return;
+		break;
 
 	default:
+		/* a real error */
 		dev_err(dev->class_dev,
 			"non-zero urb status received in ai intr context: %d\n",
 			urb->status);
-		async->events |= COMEDI_CB_EOA;
 		async->events |= COMEDI_CB_ERROR;
-		comedi_event(dev, s);
-		usbduxfast_ai_stop(dev, 0);
-		return;
-	}
-
-	if (!devpriv->ignore) {
-		if (cmd->stop_src == TRIG_COUNT) {
-			/* not continuous, fixed number of samples */
-			n = urb->actual_length / sizeof(uint16_t);
-			if (unlikely(devpriv->ai_sample_count < n)) {
-				unsigned int num_bytes;
-
-				/* partial sample received */
-				num_bytes = devpriv->ai_sample_count *
-					    sizeof(uint16_t);
-				cfc_write_array_to_buffer(s,
-							  urb->transfer_buffer,
-							  num_bytes);
-				usbduxfast_ai_stop(dev, 0);
-				/* tell comedi that the acquistion is over */
-				async->events |= COMEDI_CB_EOA;
-				comedi_event(dev, s);
-				return;
-			}
-			devpriv->ai_sample_count -= n;
-		}
-		/* write the full buffer to comedi */
-		err = cfc_write_array_to_buffer(s, urb->transfer_buffer,
-						urb->actual_length);
-		if (unlikely(err == 0)) {
-			/* buffer overflow */
-			usbduxfast_ai_stop(dev, 0);
-			return;
-		}
-
-		/* tell comedi that data is there */
-		comedi_event(dev, s);
-	} else {
-		/* ignore this packet */
-		devpriv->ignore--;
+		break;
 	}
 
 	/*
-	 * command is still running
-	 * resubmit urb for BULK transfer
+	 * comedi_handle_events() cannot be used in this driver. The (*cancel)
+	 * operation would unlink the urb.
 	 */
-	urb->dev = usb;
-	urb->status = 0;
-	err = usb_submit_urb(urb, GFP_ATOMIC);
-	if (err < 0) {
-		dev_err(dev->class_dev,
-			"urb resubm failed: %d", err);
-		async->events |= COMEDI_CB_EOA;
-		async->events |= COMEDI_CB_ERROR;
-		comedi_event(dev, s);
+	if (async->events & COMEDI_CB_CANCEL_MASK)
 		usbduxfast_ai_stop(dev, 0);
-	}
+
+	comedi_event(dev, s);
 }
 
 static int usbduxfast_submit_urb(struct comedi_device *dev)
@@ -499,8 +475,6 @@ static int usbduxfast_ai_cmd(struct comedi_device *dev,
 		up(&devpriv->sem);
 		return -EBUSY;
 	}
-	/* set current channel of the running acquisition to zero */
-	s->async->cur_chan = 0;
 
 	/*
 	 * ignore the first buffers from the device if there
@@ -810,11 +784,6 @@ static int usbduxfast_ai_cmd(struct comedi_device *dev,
 		return result;
 	}
 
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->ai_sample_count = cmd->stop_arg * cmd->scan_end_arg;
-	else	/* TRIG_NONE */
-		devpriv->ai_sample_count = 0;
-
 	if ((cmd->start_src == TRIG_NOW) || (cmd->start_src == TRIG_EXT)) {
 		/* enable this acquisition operation */
 		devpriv->ai_cmd_running = 1;
diff --git a/drivers/staging/comedi/drivers/usbduxsigma.c b/drivers/staging/comedi/drivers/usbduxsigma.c
index ebd68e365bac..dc19435b6520 100644
--- a/drivers/staging/comedi/drivers/usbduxsigma.c
+++ b/drivers/staging/comedi/drivers/usbduxsigma.c
@@ -1,6 +1,6 @@
 /*
  * usbduxsigma.c
- * Copyright (C) 2011 Bernd Porr, Bernd.Porr@f2s.com
+ * Copyright (C) 2011-2014 Bernd Porr, mail@berndporr.me.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
@@ -17,9 +17,9 @@
  * Driver: usbduxsigma
  * Description: University of Stirling USB DAQ & INCITE Technology Limited
  * Devices: (ITL) USB-DUX [usbduxsigma]
- * Author: Bernd Porr <BerndPorr@f2s.com>
- * Updated: 8 Nov 2011
- * Status: testing
+ * Author: Bernd Porr <mail@berndporr.me.uk>
+ * Updated: 10 Oct 2014
+ * Status: stable
  */
 
 /*
@@ -164,9 +164,6 @@ struct usbduxsigma_private {
 	unsigned ao_cmd_running:1;
 	unsigned pwm_cmd_running:1;
 
-	/* number of samples to acquire */
-	int ai_sample_count;
-	int ao_sample_count;
 	/* time between samples in units of the timer */
 	unsigned int ai_timer;
 	unsigned int ao_timer;
@@ -211,23 +208,75 @@ static int usbduxsigma_ai_cancel(struct comedi_device *dev,
 	return 0;
 }
 
-static void usbduxsigma_ai_urb_complete(struct urb *urb)
+static void usbduxsigma_ai_handle_urb(struct comedi_device *dev,
+				      struct comedi_subdevice *s,
+				      struct urb *urb)
 {
-	struct comedi_device *dev = urb->context;
 	struct usbduxsigma_private *devpriv = dev->private;
-	struct comedi_subdevice *s = dev->read_subdev;
-	struct comedi_cmd *cmd = &s->async->cmd;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
 	unsigned int dio_state;
 	uint32_t val;
 	int ret;
 	int i;
 
-	/* first we test if something unusual has just happened */
+	devpriv->ai_counter--;
+	if (devpriv->ai_counter == 0) {
+		devpriv->ai_counter = devpriv->ai_timer;
+
+		/* get the state of the dio pins to allow external trigger */
+		dio_state = be32_to_cpu(devpriv->in_buf[0]);
+
+		/* get the data from the USB bus and hand it over to comedi */
+		for (i = 0; i < cmd->chanlist_len; i++) {
+			/* transfer data, note first byte is the DIO state */
+			val = be32_to_cpu(devpriv->in_buf[i+1]);
+			val &= 0x00ffffff;	/* strip status byte */
+			val ^= 0x00800000;	/* convert to unsigned */
+
+			if (!comedi_buf_write_samples(s, &val, 1))
+				return;
+		}
+
+		if (cmd->stop_src == TRIG_COUNT &&
+		    async->scans_done >= cmd->stop_arg)
+			async->events |= COMEDI_CB_EOA;
+	}
+
+	/* if command is still running, resubmit urb */
+	if (!(async->events & COMEDI_CB_CANCEL_MASK)) {
+		urb->dev = comedi_to_usb_dev(dev);
+		ret = usb_submit_urb(urb, GFP_ATOMIC);
+		if (ret < 0) {
+			dev_err(dev->class_dev,
+				"%s: urb resubmit failed (%d)\n",
+				__func__, ret);
+			if (ret == -EL2NSYNC)
+				dev_err(dev->class_dev,
+					"buggy USB host controller or bug in IRQ handler\n");
+			async->events |= COMEDI_CB_ERROR;
+		}
+	}
+}
+
+static void usbduxsigma_ai_urb_complete(struct urb *urb)
+{
+	struct comedi_device *dev = urb->context;
+	struct usbduxsigma_private *devpriv = dev->private;
+	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_async *async = s->async;
+
+	/* exit if not running a command, do not resubmit urb */
+	if (!devpriv->ai_cmd_running)
+		return;
+
 	switch (urb->status) {
 	case 0:
 		/* copy the result in the transfer buffer */
 		memcpy(devpriv->in_buf, urb->transfer_buffer, SIZEINBUF);
+		usbduxsigma_ai_handle_urb(dev, s, urb);
 		break;
+
 	case -EILSEQ:
 		/*
 		 * error in the ISOchronous data
@@ -235,7 +284,7 @@ static void usbduxsigma_ai_urb_complete(struct urb *urb)
 		 * and recycle the last data byte
 		 */
 		dev_dbg(dev->class_dev, "CRC error in ISO IN stream\n");
-
+		usbduxsigma_ai_handle_urb(dev, s, urb);
 		break;
 
 	case -ECONNRESET:
@@ -243,86 +292,24 @@ static void usbduxsigma_ai_urb_complete(struct urb *urb)
 	case -ESHUTDOWN:
 	case -ECONNABORTED:
 		/* happens after an unlink command */
-		if (devpriv->ai_cmd_running) {
-			usbduxsigma_ai_stop(dev, 0);	/* w/o unlink */
-			/* we are still running a command, tell comedi */
-			s->async->events |= (COMEDI_CB_EOA | COMEDI_CB_ERROR);
-			comedi_event(dev, s);
-		}
-		return;
+		async->events |= COMEDI_CB_ERROR;
+		break;
 
 	default:
-		/*
-		 * a real error on the bus
-		 * pass error to comedi if we are really running a command
-		 */
-		if (devpriv->ai_cmd_running) {
-			dev_err(dev->class_dev,
-				"%s: non-zero urb status (%d)\n",
-				__func__, urb->status);
-			usbduxsigma_ai_stop(dev, 0);	/* w/o unlink */
-			s->async->events |= (COMEDI_CB_EOA | COMEDI_CB_ERROR);
-			comedi_event(dev, s);
-		}
-		return;
-	}
-
-	if (unlikely(!devpriv->ai_cmd_running))
-		return;
-
-	urb->dev = comedi_to_usb_dev(dev);
-
-	ret = usb_submit_urb(urb, GFP_ATOMIC);
-	if (unlikely(ret < 0)) {
-		dev_err(dev->class_dev, "%s: urb resubmit failed (%d)\n",
-			__func__, ret);
-		if (ret == -EL2NSYNC)
-			dev_err(dev->class_dev,
-				"buggy USB host controller or bug in IRQ handler\n");
-		usbduxsigma_ai_stop(dev, 0);	/* w/o unlink */
-		s->async->events |= (COMEDI_CB_EOA | COMEDI_CB_ERROR);
-		comedi_event(dev, s);
-		return;
-	}
-
-	/* get the state of the dio pins to allow external trigger */
-	dio_state = be32_to_cpu(devpriv->in_buf[0]);
-
-	devpriv->ai_counter--;
-	if (likely(devpriv->ai_counter > 0))
-		return;
-
-	/* timer zero, transfer measurements to comedi */
-	devpriv->ai_counter = devpriv->ai_timer;
-
-	if (cmd->stop_src == TRIG_COUNT) {
-		/* not continuous, fixed number of samples */
-		devpriv->ai_sample_count--;
-		if (devpriv->ai_sample_count < 0) {
-			usbduxsigma_ai_stop(dev, 0);	/* w/o unlink */
-			/* acquistion is over, tell comedi */
-			s->async->events |= COMEDI_CB_EOA;
-			comedi_event(dev, s);
-			return;
-		}
+		/* a real error */
+		dev_err(dev->class_dev, "%s: non-zero urb status (%d)\n",
+			__func__, urb->status);
+		async->events |= COMEDI_CB_ERROR;
+		break;
 	}
 
-	/* get the data from the USB bus and hand it over to comedi */
-	for (i = 0; i < cmd->chanlist_len; i++) {
-		/* transfer data, note first byte is the DIO state */
-		val = be32_to_cpu(devpriv->in_buf[i+1]);
-		val &= 0x00ffffff;	/* strip status byte */
-		val ^= 0x00800000;	/* convert to unsigned */
+	/*
+	 * comedi_handle_events() cannot be used in this driver. The (*cancel)
+	 * operation would unlink the urb.
+	 */
+	if (async->events & COMEDI_CB_CANCEL_MASK)
+		usbduxsigma_ai_stop(dev, 0);
 
-		ret = cfc_write_array_to_buffer(s, &val, sizeof(uint32_t));
-		if (unlikely(ret == 0)) {
-			/* buffer overflow */
-			usbduxsigma_ai_stop(dev, 0);	/* w/o unlink */
-			return;
-		}
-	}
-	/* tell comedi that data is there */
-	s->async->events |= (COMEDI_CB_BLOCK | COMEDI_CB_EOS);
 	comedi_event(dev, s);
 }
 
@@ -349,64 +336,25 @@ static int usbduxsigma_ao_cancel(struct comedi_device *dev,
 	return 0;
 }
 
-static void usbduxsigma_ao_urb_complete(struct urb *urb)
+static void usbduxsigma_ao_handle_urb(struct comedi_device *dev,
+				      struct comedi_subdevice *s,
+				      struct urb *urb)
 {
-	struct comedi_device *dev = urb->context;
 	struct usbduxsigma_private *devpriv = dev->private;
-	struct comedi_subdevice *s = dev->write_subdev;
-	struct comedi_cmd *cmd = &s->async->cmd;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
 	uint8_t *datap;
 	int ret;
 	int i;
 
-	switch (urb->status) {
-	case 0:
-		/* success */
-		break;
-
-	case -ECONNRESET:
-	case -ENOENT:
-	case -ESHUTDOWN:
-	case -ECONNABORTED:
-		/* happens after an unlink command */
-		if (devpriv->ao_cmd_running) {
-			usbduxsigma_ao_stop(dev, 0);	/* w/o unlink */
-			s->async->events |= COMEDI_CB_EOA;
-			comedi_event(dev, s);
-		}
-		return;
-
-	default:
-		/* a real error */
-		if (devpriv->ao_cmd_running) {
-			dev_err(dev->class_dev,
-				"%s: non-zero urb status (%d)\n",
-				__func__, urb->status);
-			usbduxsigma_ao_stop(dev, 0);	/* w/o unlink */
-			s->async->events |= (COMEDI_CB_ERROR | COMEDI_CB_EOA);
-			comedi_event(dev, s);
-		}
-		return;
-	}
-
-	if (!devpriv->ao_cmd_running)
-		return;
-
 	devpriv->ao_counter--;
-	if ((int)devpriv->ao_counter <= 0) {
-		/* timer zero, transfer from comedi */
+	if (devpriv->ao_counter == 0) {
 		devpriv->ao_counter = devpriv->ao_timer;
 
-		if (cmd->stop_src == TRIG_COUNT) {
-			/* not continuous, fixed number of samples */
-			devpriv->ao_sample_count--;
-			if (devpriv->ao_sample_count < 0) {
-				usbduxsigma_ao_stop(dev, 0);	/* w/o unlink */
-				/* acquistion is over, tell comedi */
-				s->async->events |= COMEDI_CB_EOA;
-				comedi_event(dev, s);
-				return;
-			}
+		if (cmd->stop_src == TRIG_COUNT &&
+		    async->scans_done >= cmd->stop_arg) {
+			async->events |= COMEDI_CB_EOA;
+			return;
 		}
 
 		/* transmit data to the USB bus */
@@ -416,46 +364,86 @@ static void usbduxsigma_ao_urb_complete(struct urb *urb)
 			unsigned int chan = CR_CHAN(cmd->chanlist[i]);
 			unsigned short val;
 
-			ret = comedi_buf_get(s, &val);
-			if (ret < 0) {
+			if (!comedi_buf_read_samples(s, &val, 1)) {
 				dev_err(dev->class_dev, "buffer underflow\n");
-				s->async->events |= (COMEDI_CB_EOA |
-						     COMEDI_CB_OVERFLOW);
+				async->events |= COMEDI_CB_OVERFLOW;
+				return;
 			}
+
 			*datap++ = val;
 			*datap++ = chan;
 			s->readback[chan] = val;
-
-			s->async->events |= COMEDI_CB_BLOCK;
-			comedi_event(dev, s);
 		}
 	}
 
-	urb->transfer_buffer_length = SIZEOUTBUF;
-	urb->dev = comedi_to_usb_dev(dev);
-	urb->status = 0;
-	if (devpriv->high_speed)
-		urb->interval = 8;	/* uframes */
-	else
-		urb->interval = 1;	/* frames */
-	urb->number_of_packets = 1;
-	urb->iso_frame_desc[0].offset = 0;
-	urb->iso_frame_desc[0].length = SIZEOUTBUF;
-	urb->iso_frame_desc[0].status = 0;
-	ret = usb_submit_urb(urb, GFP_ATOMIC);
-	if (ret < 0) {
-		dev_err(dev->class_dev,
-			"%s: urb resubmit failed (%d)\n",
-			__func__, ret);
-		if (ret == -EL2NSYNC)
+	/* if command is still running, resubmit urb */
+	if (!(async->events & COMEDI_CB_CANCEL_MASK)) {
+		urb->transfer_buffer_length = SIZEOUTBUF;
+		urb->dev = comedi_to_usb_dev(dev);
+		urb->status = 0;
+		if (devpriv->high_speed)
+			urb->interval = 8;	/* uframes */
+		else
+			urb->interval = 1;	/* frames */
+		urb->number_of_packets = 1;
+		urb->iso_frame_desc[0].offset = 0;
+		urb->iso_frame_desc[0].length = SIZEOUTBUF;
+		urb->iso_frame_desc[0].status = 0;
+		ret = usb_submit_urb(urb, GFP_ATOMIC);
+		if (ret < 0) {
 			dev_err(dev->class_dev,
-				"buggy USB host controller or bug in IRQ handler\n");
-		usbduxsigma_ao_stop(dev, 0);	/* w/o unlink */
-		s->async->events |= (COMEDI_CB_EOA | COMEDI_CB_ERROR);
-		comedi_event(dev, s);
+				"%s: urb resubmit failed (%d)\n",
+				__func__, ret);
+			if (ret == -EL2NSYNC)
+				dev_err(dev->class_dev,
+					"buggy USB host controller or bug in IRQ handler\n");
+			async->events |= COMEDI_CB_ERROR;
+		}
 	}
 }
 
+static void usbduxsigma_ao_urb_complete(struct urb *urb)
+{
+	struct comedi_device *dev = urb->context;
+	struct usbduxsigma_private *devpriv = dev->private;
+	struct comedi_subdevice *s = dev->write_subdev;
+	struct comedi_async *async = s->async;
+
+	/* exit if not running a command, do not resubmit urb */
+	if (!devpriv->ao_cmd_running)
+		return;
+
+	switch (urb->status) {
+	case 0:
+		usbduxsigma_ao_handle_urb(dev, s, urb);
+		break;
+
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+	case -ECONNABORTED:
+		/* happens after an unlink command */
+		async->events |= COMEDI_CB_ERROR;
+		break;
+
+	default:
+		/* a real error */
+		dev_err(dev->class_dev, "%s: non-zero urb status (%d)\n",
+			__func__, urb->status);
+		async->events |= COMEDI_CB_ERROR;
+		break;
+	}
+
+	/*
+	 * comedi_handle_events() cannot be used in this driver. The (*cancel)
+	 * operation would unlink the urb.
+	 */
+	if (async->events & COMEDI_CB_CANCEL_MASK)
+		usbduxsigma_ao_stop(dev, 0);
+
+	comedi_event(dev, s);
+}
+
 static int usbduxsigma_submit_urbs(struct comedi_device *dev,
 				   struct urb **urbs, int num_urbs,
 				   int input_urb)
@@ -584,14 +572,6 @@ static int usbduxsigma_ai_cmdtest(struct comedi_device *dev,
 	if (devpriv->ai_timer < 1)
 		err |= -EINVAL;
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		/* data arrives as one packet */
-		devpriv->ai_sample_count = cmd->stop_arg;
-	} else {
-		/* continuous acquisition */
-		devpriv->ai_sample_count = 0;
-	}
-
 	if (err)
 		return 4;
 
@@ -692,8 +672,6 @@ static int usbduxsigma_ai_cmd(struct comedi_device *dev,
 
 	down(&devpriv->sem);
 
-	/* set current channel of the running acquisition to zero */
-	s->async->cur_chan = 0;
 	for (i = 0; i < len; i++) {
 		unsigned int chan  = CR_CHAN(cmd->chanlist[i]);
 
@@ -957,25 +935,6 @@ static int usbduxsigma_ao_cmdtest(struct comedi_device *dev,
 	if (devpriv->ao_timer < 1)
 		err |= -EINVAL;
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		/* not continuous, use counter */
-		if (high_speed) {
-			/* high speed also scans everything at once */
-			devpriv->ao_sample_count = cmd->stop_arg *
-						   cmd->scan_end_arg;
-		} else {
-			/*
-			 * There's no scan as the scan has been
-			 * handled inside the FX2. Data arrives as
-			 * one packet.
-			 */
-			devpriv->ao_sample_count = cmd->stop_arg;
-		}
-	} else {
-		/* continuous acquisition */
-		devpriv->ao_sample_count = 0;
-	}
-
 	if (err)
 		return 4;
 
@@ -991,9 +950,6 @@ static int usbduxsigma_ao_cmd(struct comedi_device *dev,
 
 	down(&devpriv->sem);
 
-	/* set current channel of the running acquisition to zero */
-	s->async->cur_chan = 0;
-
 	devpriv->ao_counter = devpriv->ao_timer;
 
 	if (cmd->start_src == TRIG_NOW) {
diff --git a/drivers/staging/comedi/drivers/vmk80xx.c b/drivers/staging/comedi/drivers/vmk80xx.c
index 71003416edcf..a19a56ee0eef 100644
--- a/drivers/staging/comedi/drivers/vmk80xx.c
+++ b/drivers/staging/comedi/drivers/vmk80xx.c
@@ -797,7 +797,7 @@ static int vmk80xx_init_subdevices(struct comedi_device *dev)
 	/* Analog output subdevice */
 	s = &dev->subdevices[1];
 	s->type		= COMEDI_SUBD_AO;
-	s->subdev_flags	= SDF_WRITEABLE | SDF_GROUND;
+	s->subdev_flags	= SDF_WRITABLE | SDF_GROUND;
 	s->n_chan	= boardinfo->ao_nchans;
 	s->maxdata	= 0x00ff;
 	s->range_table	= boardinfo->range;
@@ -819,7 +819,7 @@ static int vmk80xx_init_subdevices(struct comedi_device *dev)
 	/* Digital output subdevice */
 	s = &dev->subdevices[3];
 	s->type		= COMEDI_SUBD_DO;
-	s->subdev_flags	= SDF_WRITEABLE;
+	s->subdev_flags	= SDF_WRITABLE;
 	s->n_chan	= 8;
 	s->maxdata	= 1;
 	s->range_table	= &range_digital;
@@ -834,7 +834,7 @@ static int vmk80xx_init_subdevices(struct comedi_device *dev)
 	s->insn_read	= vmk80xx_cnt_insn_read;
 	s->insn_config	= vmk80xx_cnt_insn_config;
 	if (devpriv->model == VMK8055_MODEL) {
-		s->subdev_flags	|= SDF_WRITEABLE;
+		s->subdev_flags	|= SDF_WRITABLE;
 		s->insn_write	= vmk80xx_cnt_insn_write;
 	}
 
@@ -842,7 +842,7 @@ static int vmk80xx_init_subdevices(struct comedi_device *dev)
 	if (devpriv->model == VMK8061_MODEL) {
 		s = &dev->subdevices[5];
 		s->type		= COMEDI_SUBD_PWM;
-		s->subdev_flags	= SDF_READABLE | SDF_WRITEABLE;
+		s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
 		s->n_chan	= boardinfo->pwm_nchans;
 		s->maxdata	= boardinfo->pwm_maxdata;
 		s->insn_read	= vmk80xx_pwm_insn_read;
diff --git a/drivers/staging/comedi/range.c b/drivers/staging/comedi/range.c
index b6849545b810..9a1dc56f21d1 100644
--- a/drivers/staging/comedi/range.c
+++ b/drivers/staging/comedi/range.c
@@ -97,39 +97,6 @@ int do_rangeinfo_ioctl(struct comedi_device *dev,
 	return 0;
 }
 
-static int aref_invalid(struct comedi_subdevice *s, unsigned int chanspec)
-{
-	unsigned int aref;
-
-	/*  disable reporting invalid arefs... maybe someday */
-	return 0;
-
-	aref = CR_AREF(chanspec);
-	switch (aref) {
-	case AREF_DIFF:
-		if (s->subdev_flags & SDF_DIFF)
-			return 0;
-		break;
-	case AREF_COMMON:
-		if (s->subdev_flags & SDF_COMMON)
-			return 0;
-		break;
-	case AREF_GROUND:
-		if (s->subdev_flags & SDF_GROUND)
-			return 0;
-		break;
-	case AREF_OTHER:
-		if (s->subdev_flags & SDF_OTHER)
-			return 0;
-		break;
-	default:
-		break;
-	}
-	dev_dbg(s->device->class_dev, "subdevice does not support aref %i",
-		aref);
-	return 1;
-}
-
 /**
  * comedi_check_chanlist() - Validate each element in a chanlist.
  * @s: comedi_subdevice struct
@@ -153,8 +120,7 @@ int comedi_check_chanlist(struct comedi_subdevice *s, int n,
 		else
 			range_len = 0;
 		if (chan >= s->n_chan ||
-		    CR_RANGE(chanspec) >= range_len ||
-		    aref_invalid(s, chanspec)) {
+		    CR_RANGE(chanspec) >= range_len) {
 			dev_warn(dev->class_dev,
 				 "bad chanlist[%d]=0x%08x chan=%d range length=%d\n",
 				 i, chanspec, chan, range_len);
diff --git a/drivers/staging/cptm1217/clearpad_tm1217.c b/drivers/staging/cptm1217/clearpad_tm1217.c
index edf9ff2ea25b..7f265ce0dd13 100644
--- a/drivers/staging/cptm1217/clearpad_tm1217.c
+++ b/drivers/staging/cptm1217/clearpad_tm1217.c
@@ -278,7 +278,7 @@ static void cp_tm1217_get_data(struct cp_tm1217_device *ts)
 
 static irqreturn_t cp_tm1217_sample_thread(int irq, void *handle)
 {
-	struct cp_tm1217_device *ts = (struct cp_tm1217_device *) handle;
+	struct cp_tm1217_device *ts = handle;
 	u8 req[2];
 	int retval;
 
diff --git a/drivers/staging/dgap/dgap.c b/drivers/staging/dgap/dgap.c
index d0be1cebf5a9..bdb5317e3d9d 100644
--- a/drivers/staging/dgap/dgap.c
+++ b/drivers/staging/dgap/dgap.c
@@ -65,144 +65,6 @@
 
 #include "dgap.h"
 
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Digi International, http://www.digi.com");
-MODULE_DESCRIPTION("Driver for the Digi International EPCA PCI based product line");
-MODULE_SUPPORTED_DEVICE("dgap");
-
-static int dgap_start(void);
-static void dgap_init_globals(void);
-static struct board_t *dgap_found_board(struct pci_dev *pdev, int id,
-					int boardnum);
-static void dgap_cleanup_board(struct board_t *brd);
-static void dgap_poll_handler(ulong dummy);
-static int dgap_init_one(struct pci_dev *pdev, const struct pci_device_id *ent);
-static void dgap_remove_one(struct pci_dev *dev);
-static int dgap_do_remap(struct board_t *brd);
-static void dgap_release_remap(struct board_t *brd);
-static irqreturn_t dgap_intr(int irq, void *voidbrd);
-
-static int dgap_tty_open(struct tty_struct *tty, struct file *file);
-static void dgap_tty_close(struct tty_struct *tty, struct file *file);
-static int dgap_block_til_ready(struct tty_struct *tty, struct file *file,
-				struct channel_t *ch);
-static int dgap_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
-				unsigned long arg);
-static int dgap_tty_digigeta(struct channel_t *ch,
-			     struct digi_t __user *retinfo);
-static int dgap_tty_digiseta(struct channel_t *ch, struct board_t *bd,
-			     struct un_t *un, struct digi_t __user *new_info);
-static int dgap_tty_digigetedelay(struct tty_struct *tty, int __user *retinfo);
-static int dgap_tty_digisetedelay(struct channel_t *ch, struct board_t *bd,
-				  struct un_t *un, int __user *new_info);
-static int dgap_tty_write_room(struct tty_struct *tty);
-static int dgap_tty_chars_in_buffer(struct tty_struct *tty);
-static void dgap_tty_start(struct tty_struct *tty);
-static void dgap_tty_stop(struct tty_struct *tty);
-static void dgap_tty_throttle(struct tty_struct *tty);
-static void dgap_tty_unthrottle(struct tty_struct *tty);
-static void dgap_tty_flush_chars(struct tty_struct *tty);
-static void dgap_tty_flush_buffer(struct tty_struct *tty);
-static void dgap_tty_hangup(struct tty_struct *tty);
-static int dgap_wait_for_drain(struct tty_struct *tty);
-static int dgap_set_modem_info(struct channel_t *ch, struct board_t *bd,
-			       struct un_t *un, unsigned int command,
-			       unsigned int __user *value);
-static int dgap_get_modem_info(struct channel_t *ch,
-				unsigned int __user *value);
-static int dgap_tty_digisetcustombaud(struct channel_t *ch, struct board_t *bd,
-				      struct un_t *un, int __user *new_info);
-static int dgap_tty_digigetcustombaud(struct channel_t *ch, struct un_t *un,
-				      int __user *retinfo);
-static int dgap_tty_tiocmget(struct tty_struct *tty);
-static int dgap_tty_tiocmset(struct tty_struct *tty, unsigned int set,
-				unsigned int clear);
-static int dgap_tty_send_break(struct tty_struct *tty, int msec);
-static void dgap_tty_wait_until_sent(struct tty_struct *tty, int timeout);
-static int dgap_tty_write(struct tty_struct *tty, const unsigned char *buf,
-				int count);
-static void dgap_tty_set_termios(struct tty_struct *tty,
-				struct ktermios *old_termios);
-static int dgap_tty_put_char(struct tty_struct *tty, unsigned char c);
-static void dgap_tty_send_xchar(struct tty_struct *tty, char ch);
-
-static int dgap_tty_register(struct board_t *brd);
-static void dgap_tty_unregister(struct board_t *brd);
-static int dgap_tty_init(struct board_t *);
-static void dgap_tty_free(struct board_t *);
-static void dgap_cleanup_tty(struct board_t *);
-static void dgap_carrier(struct channel_t *ch);
-static void dgap_input(struct channel_t *ch);
-
-/*
- * Our function prototypes from dgap_fep5
- */
-static void dgap_cmdw_ext(struct channel_t *ch, u16 cmd, u16 word, uint ncmds);
-static int dgap_event(struct board_t *bd);
-
-static void dgap_poll_tasklet(unsigned long data);
-static void dgap_cmdb(struct channel_t *ch, u8 cmd, u8 byte1,
-			u8 byte2, uint ncmds);
-static void dgap_cmdw(struct channel_t *ch, u8 cmd, u16 word, uint ncmds);
-static void dgap_wmove(struct channel_t *ch, char *buf, uint cnt);
-static int dgap_param(struct channel_t *ch, struct board_t *bd, u32 un_type);
-static void dgap_parity_scan(struct channel_t *ch, unsigned char *cbuf,
-				unsigned char *fbuf, int *len);
-static uint dgap_get_custom_baud(struct channel_t *ch);
-static void dgap_firmware_reset_port(struct channel_t *ch);
-
-/*
- * Function prototypes from dgap_parse.c.
- */
-static int dgap_gettok(char **in);
-static char *dgap_getword(char **in);
-static int dgap_checknode(struct cnode *p);
-
-/*
- * Function prototypes from dgap_sysfs.h
- */
-static void dgap_create_ports_sysfiles(struct board_t *bd);
-static void dgap_remove_ports_sysfiles(struct board_t *bd);
-
-static int dgap_create_driver_sysfiles(struct pci_driver *);
-static void dgap_remove_driver_sysfiles(struct pci_driver *);
-
-static void dgap_create_tty_sysfs(struct un_t *un, struct device *c);
-static void dgap_remove_tty_sysfs(struct device *c);
-
-/*
- * Function prototypes from dgap_parse.h
- */
-static int dgap_parsefile(char **in);
-static struct cnode *dgap_find_config(int type, int bus, int slot);
-static uint dgap_config_get_num_prts(struct board_t *bd);
-static char *dgap_create_config_string(struct board_t *bd, char *string);
-static uint dgap_config_get_useintr(struct board_t *bd);
-static uint dgap_config_get_altpin(struct board_t *bd);
-
-static void dgap_do_bios_load(struct board_t *brd, const u8 *ubios, int len);
-static void dgap_do_fep_load(struct board_t *brd, const u8 *ufep, int len);
-#ifdef DIGI_CONCENTRATORS_SUPPORTED
-static void dgap_do_conc_load(struct board_t *brd, u8 *uaddr, int len);
-#endif
-static int dgap_alloc_flipbuf(struct board_t *brd);
-static void dgap_free_flipbuf(struct board_t *brd);
-static int dgap_request_irq(struct board_t *brd);
-static void dgap_free_irq(struct board_t *brd);
-
-static void dgap_get_vpd(struct board_t *brd);
-static void dgap_do_reset_board(struct board_t *brd);
-static int dgap_test_bios(struct board_t *brd);
-static int dgap_test_fep(struct board_t *brd);
-static int dgap_tty_register_ports(struct board_t *brd);
-static int dgap_firmware_load(struct pci_dev *pdev, int card_type,
-			      struct board_t *brd);
-static void dgap_cleanup_nodes(void);
-
-static void dgap_cleanup_module(void);
-
-module_exit(dgap_cleanup_module);
-
 /*
  * File operations permitted on Control/Management major.
  */
@@ -218,7 +80,6 @@ static int dgap_poll_tick = 20;	/* Poll interval - 20 ms */
 
 static struct class *dgap_class;
 
-static struct board_t *dgap_boards_by_major[256];
 static uint dgap_count = 500;
 
 /*
@@ -297,13 +158,6 @@ static struct board_id dgap_ids[] = {
 	{0,}						/* 0 terminated list. */
 };
 
-static struct pci_driver dgap_driver = {
-	.name		= "dgap",
-	.probe		= dgap_init_one,
-	.id_table	= dgap_pci_tbl,
-	.remove		= dgap_remove_one,
-};
-
 struct firmware_info {
 	u8 *conf_name;  /* dgap.conf */
 	u8 *bios_name;	/* BIOS filename */
@@ -366,29 +220,6 @@ static struct ktermios dgap_default_termios = {
 	.c_line =	0,
 };
 
-static const struct tty_operations dgap_tty_ops = {
-	.open = dgap_tty_open,
-	.close = dgap_tty_close,
-	.write = dgap_tty_write,
-	.write_room = dgap_tty_write_room,
-	.flush_buffer = dgap_tty_flush_buffer,
-	.chars_in_buffer = dgap_tty_chars_in_buffer,
-	.flush_chars = dgap_tty_flush_chars,
-	.ioctl = dgap_tty_ioctl,
-	.set_termios = dgap_tty_set_termios,
-	.stop = dgap_tty_stop,
-	.start = dgap_tty_start,
-	.throttle = dgap_tty_throttle,
-	.unthrottle = dgap_tty_unthrottle,
-	.hangup = dgap_tty_hangup,
-	.put_char = dgap_tty_put_char,
-	.tiocmget = dgap_tty_tiocmget,
-	.tiocmset = dgap_tty_tiocmset,
-	.break_ctl = dgap_tty_send_break,
-	.wait_until_sent = dgap_tty_wait_until_sent,
-	.send_xchar = dgap_tty_send_xchar
-};
-
 /*
  * Our needed internal static variables from dgap_parse.c
  */
@@ -456,1078 +287,1226 @@ static struct toklist dgap_tlist[] = {
 	{ 0,		NULL }
 };
 
-/************************************************************************
- *
- * Driver load/unload functions
- *
- ************************************************************************/
 
 /*
- * init_module()
- *
- * Module load.  This is where it all starts.
+ * dgap_sindex: much like index(), but it looks for a match of any character in
+ * the group, and returns that position.  If the first character is a ^, then
+ * this will match the first occurrence not in that group.
  */
-static int dgap_init_module(void)
+static char *dgap_sindex(char *string, char *group)
 {
-	int rc;
+	char *ptr;
 
-	pr_info("%s, Digi International Part Number %s\n", DG_NAME, DG_PART);
+	if (!string || !group)
+		return NULL;
 
-	rc = dgap_start();
-	if (rc)
-		return rc;
+	if (*group == '^') {
+		group++;
+		for (; *string; string++) {
+			for (ptr = group; *ptr; ptr++) {
+				if (*ptr == *string)
+					break;
+			}
+			if (*ptr == '\0')
+				return string;
+		}
+	} else {
+		for (; *string; string++) {
+			for (ptr = group; *ptr; ptr++) {
+				if (*ptr == *string)
+					return string;
+			}
+		}
+	}
 
-	rc = pci_register_driver(&dgap_driver);
-	if (rc)
-		goto err_cleanup;
+	return NULL;
+}
 
-	rc = dgap_create_driver_sysfiles(&dgap_driver);
-	if (rc)
-		goto err_cleanup;
+/*
+ * get a word from the input stream, also keep track of current line number.
+ * words are separated by whitespace.
+ */
+static char *dgap_getword(char **in)
+{
+	char *ret_ptr = *in;
 
-	dgap_driver_state = DRIVER_READY;
+	char *ptr = dgap_sindex(*in, " \t\n");
 
-	return 0;
+	/* If no word found, return null */
+	if (!ptr)
+		return NULL;
 
-err_cleanup:
+	/* Mark new location for our buffer */
+	*ptr = '\0';
+	*in = ptr + 1;
 
-	dgap_cleanup_module();
+	/* Eat any extra spaces/tabs/newlines that might be present */
+	while (*in && **in && ((**in == ' ') ||
+			       (**in == '\t') ||
+			       (**in == '\n'))) {
+		**in = '\0';
+		*in = *in + 1;
+	}
 
-	return rc;
+	return ret_ptr;
 }
-module_init(dgap_init_module);
+
 
 /*
- * Start of driver.
+ * Get a token from the input file; return 0 if end of file is reached
  */
-static int dgap_start(void)
+static int dgap_gettok(char **in)
 {
-	int rc;
-	unsigned long flags;
-	struct device *device;
-
-	/*
-	 * make sure that the globals are
-	 * init'd before we do anything else
-	 */
-	dgap_init_globals();
-
-	dgap_numboards = 0;
+	char *w;
+	struct toklist *t;
 
-	pr_info("For the tools package please visit http://www.digi.com\n");
+	if (strstr(dgap_cword, "board")) {
+		w = dgap_getword(in);
+		snprintf(dgap_cword, MAXCWORD, "%s", w);
+		for (t = dgap_brdtype; t->token != 0; t++) {
+			if (!strcmp(w, t->string))
+				return t->token;
+		}
+	} else {
+		while ((w = dgap_getword(in))) {
+			snprintf(dgap_cword, MAXCWORD, "%s", w);
+			for (t = dgap_tlist; t->token != 0; t++) {
+				if (!strcmp(w, t->string))
+					return t->token;
+			}
+		}
+	}
 
-	/*
-	 * Register our base character device into the kernel.
-	 */
+	return 0;
+}
 
-	/*
-	 * Register management/dpa devices
-	 */
-	rc = register_chrdev(DIGI_DGAP_MAJOR, "dgap", &dgap_board_fops);
-	if (rc < 0)
-		return rc;
+/*
+ * dgap_checknode: see if all the necessary info has been supplied for a node
+ * before creating the next node.
+ */
+static int dgap_checknode(struct cnode *p)
+{
+	switch (p->type) {
+	case LNODE:
+		if (p->u.line.v_speed == 0) {
+			pr_err("line speed not specified");
+			return 1;
+		}
+		return 0;
 
-	dgap_class = class_create(THIS_MODULE, "dgap_mgmt");
-	if (IS_ERR(dgap_class)) {
-		rc = PTR_ERR(dgap_class);
-		goto failed_class;
-	}
+	case CNODE:
+		if (p->u.conc.v_speed == 0) {
+			pr_err("concentrator line speed not specified");
+			return 1;
+		}
+		if (p->u.conc.v_nport == 0) {
+			pr_err("number of ports on concentrator not specified");
+			return 1;
+		}
+		if (p->u.conc.v_id == 0) {
+			pr_err("concentrator id letter not specified");
+			return 1;
+		}
+		return 0;
 
-	device = device_create(dgap_class, NULL,
-		MKDEV(DIGI_DGAP_MAJOR, 0),
-		NULL, "dgap_mgmt");
-	if (IS_ERR(device)) {
-		rc = PTR_ERR(device);
-		goto failed_device;
+	case MNODE:
+		if (p->u.module.v_nport == 0) {
+			pr_err("number of ports on EBI module not specified");
+			return 1;
+		}
+		if (p->u.module.v_id == 0) {
+			pr_err("EBI module id letter not specified");
+			return 1;
+		}
+		return 0;
 	}
+	return 0;
+}
 
-	/* Start the poller */
-	spin_lock_irqsave(&dgap_poll_lock, flags);
-	init_timer(&dgap_poll_timer);
-	dgap_poll_timer.function = dgap_poll_handler;
-	dgap_poll_timer.data = 0;
-	dgap_poll_time = jiffies + dgap_jiffies_from_ms(dgap_poll_tick);
-	dgap_poll_timer.expires = dgap_poll_time;
-	spin_unlock_irqrestore(&dgap_poll_lock, flags);
+/*
+ * Given a board pointer, returns whether we should use interrupts or not.
+ */
+static uint dgap_config_get_useintr(struct board_t *bd)
+{
+	struct cnode *p;
 
-	add_timer(&dgap_poll_timer);
+	if (!bd)
+		return 0;
 
-	return rc;
+	for (p = bd->bd_config; p; p = p->next) {
+		if (p->type == INTRNODE) {
+			/*
+			 * check for pcxr types.
+			 */
+			return p->u.useintr;
+		}
+	}
 
-failed_device:
-	class_destroy(dgap_class);
-failed_class:
-	unregister_chrdev(DIGI_DGAP_MAJOR, "dgap");
-	return rc;
+	/* If not found, then don't turn on interrupts. */
+	return 0;
 }
 
-static int dgap_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
+/*
+ * Given a board pointer, returns whether we turn on altpin or not.
+ */
+static uint dgap_config_get_altpin(struct board_t *bd)
 {
-	int rc;
-	struct board_t *brd;
+	struct cnode *p;
 
-	if (dgap_numboards >= MAXBOARDS)
-		return -EPERM;
+	if (!bd)
+		return 0;
 
-	rc = pci_enable_device(pdev);
-	if (rc)
-		return -EIO;
+	for (p = bd->bd_config; p; p = p->next) {
+		if (p->type == ANODE) {
+			/*
+			 * check for pcxr types.
+			 */
+			return p->u.altpin;
+		}
+	}
 
-	brd = dgap_found_board(pdev, ent->driver_data, dgap_numboards);
-	if (IS_ERR(brd))
-		return PTR_ERR(brd);
+	/* If not found, then don't turn on interrupts. */
+	return 0;
+}
 
-	rc = dgap_firmware_load(pdev, ent->driver_data, brd);
-	if (rc)
-		goto cleanup_brd;
+/*
+ * Given a specific type of board, if found, detached link and
+ * returns the first occurrence in the list.
+ */
+static struct cnode *dgap_find_config(int type, int bus, int slot)
+{
+	struct cnode *p, *prev, *prev2, *found;
 
-	rc = dgap_alloc_flipbuf(brd);
-	if (rc)
-		goto cleanup_brd;
+	p = &dgap_head;
 
-	rc = dgap_tty_register(brd);
-	if (rc)
-		goto free_flipbuf;
+	while (p->next) {
+		prev = p;
+		p = p->next;
 
-	rc = dgap_request_irq(brd);
-	if (rc)
-		goto unregister_tty;
+		if (p->type != BNODE)
+			continue;
 
-	/*
-	 * Do tty device initialization.
-	 */
-	rc = dgap_tty_init(brd);
-	if (rc < 0)
-		goto free_irq;
+		if (p->u.board.type != type)
+			continue;
 
-	rc = dgap_tty_register_ports(brd);
-	if (rc)
-		goto tty_free;
+		if (p->u.board.v_pcibus &&
+		    p->u.board.pcibus != bus)
+			continue;
 
-	brd->state = BOARD_READY;
-	brd->dpastatus = BD_RUNNING;
+		if (p->u.board.v_pcislot &&
+		    p->u.board.pcislot != slot)
+			continue;
 
-	dgap_board[dgap_numboards++] = brd;
+		found = p;
+		/*
+		 * Keep walking thru the list till we
+		 * find the next board.
+		 */
+		while (p->next) {
+			prev2 = p;
+			p = p->next;
 
-	return 0;
+			if (p->type != BNODE)
+				continue;
 
-tty_free:
-	dgap_tty_free(brd);
-free_irq:
-	dgap_free_irq(brd);
-unregister_tty:
-	dgap_tty_unregister(brd);
-free_flipbuf:
-	dgap_free_flipbuf(brd);
-cleanup_brd:
-	dgap_cleanup_nodes();
-	dgap_release_remap(brd);
-	kfree(brd);
+			/*
+			 * Mark the end of our 1 board
+			 * chain of configs.
+			 */
+			prev2->next = NULL;
 
-	return rc;
-}
+			/*
+			 * Link the "next" board to the
+			 * previous board, effectively
+			 * "unlinking" our board from
+			 * the main config.
+			 */
+			prev->next = p;
 
-static void dgap_remove_one(struct pci_dev *dev)
-{
-	/* Do Nothing */
+			return found;
+		}
+		/*
+		 * It must be the last board in the list.
+		 */
+		prev->next = NULL;
+		return found;
+	}
+	return NULL;
 }
 
 /*
- * dgap_cleanup_module()
- *
- * Module unload.  This is where it all ends.
+ * Given a board pointer, walks the config link, counting up
+ * all ports user specified should be on the board.
+ * (This does NOT mean they are all actually present right now tho)
  */
-static void dgap_cleanup_module(void)
+static uint dgap_config_get_num_prts(struct board_t *bd)
 {
-	int i;
-	ulong lock_flags;
-
-	spin_lock_irqsave(&dgap_poll_lock, lock_flags);
-	dgap_poll_stop = 1;
-	spin_unlock_irqrestore(&dgap_poll_lock, lock_flags);
-
-	/* Turn off poller right away. */
-	del_timer_sync(&dgap_poll_timer);
+	int count = 0;
+	struct cnode *p;
 
-	dgap_remove_driver_sysfiles(&dgap_driver);
+	if (!bd)
+		return 0;
 
-	device_destroy(dgap_class, MKDEV(DIGI_DGAP_MAJOR, 0));
-	class_destroy(dgap_class);
-	unregister_chrdev(DIGI_DGAP_MAJOR, "dgap");
+	for (p = bd->bd_config; p; p = p->next) {
 
-	for (i = 0; i < dgap_numboards; ++i) {
-		dgap_remove_ports_sysfiles(dgap_board[i]);
-		dgap_cleanup_tty(dgap_board[i]);
-		dgap_cleanup_board(dgap_board[i]);
+		switch (p->type) {
+		case BNODE:
+			/*
+			 * check for pcxr types.
+			 */
+			if (p->u.board.type > EPCFE)
+				count += p->u.board.nport;
+			break;
+		case CNODE:
+			count += p->u.conc.nport;
+			break;
+		case MNODE:
+			count += p->u.module.nport;
+			break;
+		}
 	}
-
-	dgap_cleanup_nodes();
-
-	if (dgap_numboards)
-		pci_unregister_driver(&dgap_driver);
+	return count;
 }
 
-/*
- * dgap_cleanup_board()
- *
- * Free all the memory associated with a board
- */
-static void dgap_cleanup_board(struct board_t *brd)
+static char *dgap_create_config_string(struct board_t *bd, char *string)
 {
-	int i;
-
-	if (!brd || brd->magic != DGAP_BOARD_MAGIC)
-		return;
-
-	dgap_free_irq(brd);
-
-	tasklet_kill(&brd->helper_tasklet);
+	char *ptr = string;
+	struct cnode *p;
+	struct cnode *q;
+	int speed;
 
-	dgap_release_remap(brd);
+	if (!bd) {
+		*ptr = 0xff;
+		return string;
+	}
 
-	/* Free all allocated channels structs */
-	for (i = 0; i < MAXPORTS ; i++)
-		kfree(brd->channels[i]);
+	for (p = bd->bd_config; p; p = p->next) {
 
-	kfree(brd->flipbuf);
-	kfree(brd->flipflagbuf);
+		switch (p->type) {
+		case LNODE:
+			*ptr = '\0';
+			ptr++;
+			*ptr = p->u.line.speed;
+			ptr++;
+			break;
+		case CNODE:
+			/*
+			 * Because the EPC/con concentrators can have EM modules
+			 * hanging off of them, we have to walk ahead in the
+			 * list and keep adding the number of ports on each EM
+			 * to the config. UGH!
+			 */
+			speed = p->u.conc.speed;
+			q = p->next;
+			if (q && (q->type == MNODE)) {
+				*ptr = (p->u.conc.nport + 0x80);
+				ptr++;
+				p = q;
+				while (q->next && (q->next->type) == MNODE) {
+					*ptr = (q->u.module.nport + 0x80);
+					ptr++;
+					p = q;
+					q = q->next;
+				}
+				*ptr = q->u.module.nport;
+				ptr++;
+			} else {
+				*ptr = p->u.conc.nport;
+				ptr++;
+			}
 
-	dgap_board[brd->boardnum] = NULL;
+			*ptr = speed;
+			ptr++;
+			break;
+		}
+	}
 
-	kfree(brd);
+	*ptr = 0xff;
+	return string;
 }
 
 /*
- * dgap_found_board()
- *
- * A board has been found, init it.
+ * Parse a configuration file read into memory as a string.
  */
-static struct board_t *dgap_found_board(struct pci_dev *pdev, int id,
-					int boardnum)
+static int dgap_parsefile(char **in)
 {
-	struct board_t *brd;
-	unsigned int pci_irq;
-	int i;
-	int ret;
-
-	/* get the board structure and prep it */
-	brd = kzalloc(sizeof(struct board_t), GFP_KERNEL);
-	if (!brd)
-		return ERR_PTR(-ENOMEM);
+	struct cnode *p, *brd, *line, *conc;
+	int rc;
+	char *s;
+	int linecnt = 0;
 
-	/* store the info for the board we've found */
-	brd->magic = DGAP_BOARD_MAGIC;
-	brd->boardnum = boardnum;
-	brd->vendor = dgap_pci_tbl[id].vendor;
-	brd->device = dgap_pci_tbl[id].device;
-	brd->pdev = pdev;
-	brd->pci_bus = pdev->bus->number;
-	brd->pci_slot = PCI_SLOT(pdev->devfn);
-	brd->name = dgap_ids[id].name;
-	brd->maxports = dgap_ids[id].maxports;
-	brd->type = dgap_ids[id].config_type;
-	brd->dpatype = dgap_ids[id].dpatype;
-	brd->dpastatus = BD_NOFEP;
-	init_waitqueue_head(&brd->state_wait);
+	p = &dgap_head;
+	brd = line = conc = NULL;
 
-	spin_lock_init(&brd->bd_lock);
+	/* perhaps we are adding to an existing list? */
+	while (p->next)
+		p = p->next;
 
-	brd->inhibit_poller	= FALSE;
-	brd->wait_for_bios	= 0;
-	brd->wait_for_fep	= 0;
+	/* file must start with a BEGIN */
+	while ((rc = dgap_gettok(in)) != BEGIN) {
+		if (rc == 0) {
+			pr_err("unexpected EOF");
+			return -1;
+		}
+	}
 
-	for (i = 0; i < MAXPORTS; i++)
-		brd->channels[i] = NULL;
+	for (; ;) {
+		int board_type = 0;
+		int conc_type = 0;
+		int module_type = 0;
 
-	/* store which card & revision we have */
-	pci_read_config_word(pdev, PCI_SUBSYSTEM_VENDOR_ID, &brd->subvendor);
-	pci_read_config_word(pdev, PCI_SUBSYSTEM_ID, &brd->subdevice);
-	pci_read_config_byte(pdev, PCI_REVISION_ID, &brd->rev);
+		rc = dgap_gettok(in);
+		if (rc == 0) {
+			pr_err("unexpected EOF");
+			return -1;
+		}
 
-	pci_irq = pdev->irq;
-	brd->irq = pci_irq;
+		switch (rc) {
+		case BEGIN:	/* should only be 1 begin */
+			pr_err("unexpected config_begin\n");
+			return -1;
 
-	/* get the PCI Base Address Registers */
+		case END:
+			return 0;
 
-	/* Xr Jupiter and EPC use BAR 2 */
-	if (brd->device == PCI_DEV_XRJ_DID || brd->device == PCI_DEV_EPCJ_DID) {
-		brd->membase     = pci_resource_start(pdev, 2);
-		brd->membase_end = pci_resource_end(pdev, 2);
-	}
-	/* Everyone else uses BAR 0 */
-	else {
-		brd->membase     = pci_resource_start(pdev, 0);
-		brd->membase_end = pci_resource_end(pdev, 0);
-	}
+		case BOARD:	/* board info */
+			if (dgap_checknode(p))
+				return -1;
 
-	if (!brd->membase) {
-		ret = -ENODEV;
-		goto free_brd;
-	}
+			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
+			if (!p->next)
+				return -1;
 
-	if (brd->membase & 1)
-		brd->membase &= ~3;
-	else
-		brd->membase &= ~15;
+			p = p->next;
 
-	/*
-	 * On the PCI boards, there is no IO space allocated
-	 * The I/O registers will be in the first 3 bytes of the
-	 * upper 2MB of the 4MB memory space.  The board memory
-	 * will be mapped into the low 2MB of the 4MB memory space
-	 */
-	brd->port = brd->membase + PCI_IO_OFFSET;
-	brd->port_end = brd->port + PCI_IO_SIZE;
+			p->type = BNODE;
+			p->u.board.status = kstrdup("No", GFP_KERNEL);
+			line = conc = NULL;
+			brd = p;
+			linecnt = -1;
 
-	/*
-	 * Special initialization for non-PLX boards
-	 */
-	if (brd->device != PCI_DEV_XRJ_DID && brd->device != PCI_DEV_EPCJ_DID) {
-		unsigned short cmd;
+			board_type = dgap_gettok(in);
+			if (board_type == 0) {
+				pr_err("board !!type not specified");
+				return -1;
+			}
 
-		pci_write_config_byte(pdev, 0x40, 0);
-		pci_write_config_byte(pdev, 0x46, 0);
+			p->u.board.type = board_type;
 
-		/* Limit burst length to 2 doubleword transactions */
-		pci_write_config_byte(pdev, 0x42, 1);
+			break;
 
-		/*
-		 * Enable IO and mem if not already done.
-		 * This was needed for support on Itanium.
-		 */
-		pci_read_config_word(pdev, PCI_COMMAND, &cmd);
-		cmd |= (PCI_COMMAND_IO | PCI_COMMAND_MEMORY);
-		pci_write_config_word(pdev, PCI_COMMAND, cmd);
-	}
+		case IO:	/* i/o port */
+			if (p->type != BNODE) {
+				pr_err("IO port only valid for boards");
+				return -1;
+			}
+			s = dgap_getword(in);
+			if (!s) {
+				pr_err("unexpected end of file");
+				return -1;
+			}
+			p->u.board.portstr = kstrdup(s, GFP_KERNEL);
+			if (kstrtol(s, 0, &p->u.board.port)) {
+				pr_err("bad number for IO port");
+				return -1;
+			}
+			p->u.board.v_port = 1;
+			break;
 
-	/* init our poll helper tasklet */
-	tasklet_init(&brd->helper_tasklet, dgap_poll_tasklet,
-			(unsigned long) brd);
+		case MEM:	/* memory address */
+			if (p->type != BNODE) {
+				pr_err("memory address only valid for boards");
+				return -1;
+			}
+			s = dgap_getword(in);
+			if (!s) {
+				pr_err("unexpected end of file");
+				return -1;
+			}
+			p->u.board.addrstr = kstrdup(s, GFP_KERNEL);
+			if (kstrtoul(s, 0, &p->u.board.addr)) {
+				pr_err("bad number for memory address");
+				return -1;
+			}
+			p->u.board.v_addr = 1;
+			break;
 
-	ret = dgap_do_remap(brd);
-	if (ret)
-		goto free_brd;
+		case PCIINFO:	/* pci information */
+			if (p->type != BNODE) {
+				pr_err("memory address only valid for boards");
+				return -1;
+			}
+			s = dgap_getword(in);
+			if (!s) {
+				pr_err("unexpected end of file");
+				return -1;
+			}
+			p->u.board.pcibusstr = kstrdup(s, GFP_KERNEL);
+			if (kstrtoul(s, 0, &p->u.board.pcibus)) {
+				pr_err("bad number for pci bus");
+				return -1;
+			}
+			p->u.board.v_pcibus = 1;
+			s = dgap_getword(in);
+			if (!s) {
+				pr_err("unexpected end of file");
+				return -1;
+			}
+			p->u.board.pcislotstr = kstrdup(s, GFP_KERNEL);
+			if (kstrtoul(s, 0, &p->u.board.pcislot)) {
+				pr_err("bad number for pci slot");
+				return -1;
+			}
+			p->u.board.v_pcislot = 1;
+			break;
 
-	pr_info("dgap: board %d: %s (rev %d), irq %ld\n",
-		boardnum, brd->name, brd->rev, brd->irq);
+		case METHOD:
+			if (p->type != BNODE) {
+				pr_err("install method only valid for boards");
+				return -1;
+			}
+			s = dgap_getword(in);
+			if (!s) {
+				pr_err("unexpected end of file");
+				return -1;
+			}
+			p->u.board.method = kstrdup(s, GFP_KERNEL);
+			p->u.board.v_method = 1;
+			break;
 
-	return brd;
+		case STATUS:
+			if (p->type != BNODE) {
+				pr_err("config status only valid for boards");
+				return -1;
+			}
+			s = dgap_getword(in);
+			if (!s) {
+				pr_err("unexpected end of file");
+				return -1;
+			}
+			p->u.board.status = kstrdup(s, GFP_KERNEL);
+			break;
 
-free_brd:
-	kfree(brd);
+		case NPORTS:	/* number of ports */
+			if (p->type == BNODE) {
+				s = dgap_getword(in);
+				if (!s) {
+					pr_err("unexpected end of file");
+					return -1;
+				}
+				if (kstrtol(s, 0, &p->u.board.nport)) {
+					pr_err("bad number for number of ports");
+					return -1;
+				}
+				p->u.board.v_nport = 1;
+			} else if (p->type == CNODE) {
+				s = dgap_getword(in);
+				if (!s) {
+					pr_err("unexpected end of file");
+					return -1;
+				}
+				if (kstrtol(s, 0, &p->u.conc.nport)) {
+					pr_err("bad number for number of ports");
+					return -1;
+				}
+				p->u.conc.v_nport = 1;
+			} else if (p->type == MNODE) {
+				s = dgap_getword(in);
+				if (!s) {
+					pr_err("unexpected end of file");
+					return -1;
+				}
+				if (kstrtol(s, 0, &p->u.module.nport)) {
+					pr_err("bad number for number of ports");
+					return -1;
+				}
+				p->u.module.v_nport = 1;
+			} else {
+				pr_err("nports only valid for concentrators or modules");
+				return -1;
+			}
+			break;
 
-	return ERR_PTR(ret);
-}
+		case ID:	/* letter ID used in tty name */
+			s = dgap_getword(in);
+			if (!s) {
+				pr_err("unexpected end of file");
+				return -1;
+			}
 
+			p->u.board.status = kstrdup(s, GFP_KERNEL);
 
-static int dgap_request_irq(struct board_t *brd)
-{
-	int rc;
+			if (p->type == CNODE) {
+				p->u.conc.id = kstrdup(s, GFP_KERNEL);
+				p->u.conc.v_id = 1;
+			} else if (p->type == MNODE) {
+				p->u.module.id = kstrdup(s, GFP_KERNEL);
+				p->u.module.v_id = 1;
+			} else {
+				pr_err("id only valid for concentrators or modules");
+				return -1;
+			}
+			break;
 
-	if (!brd || brd->magic != DGAP_BOARD_MAGIC)
-		return -ENODEV;
+		case STARTO:	/* start offset of ID */
+			if (p->type == BNODE) {
+				s = dgap_getword(in);
+				if (!s) {
+					pr_err("unexpected end of file");
+					return -1;
+				}
+				if (kstrtol(s, 0, &p->u.board.start)) {
+					pr_err("bad number for start of tty count");
+					return -1;
+				}
+				p->u.board.v_start = 1;
+			} else if (p->type == CNODE) {
+				s = dgap_getword(in);
+				if (!s) {
+					pr_err("unexpected end of file");
+					return -1;
+				}
+				if (kstrtol(s, 0, &p->u.conc.start)) {
+					pr_err("bad number for start of tty count");
+					return -1;
+				}
+				p->u.conc.v_start = 1;
+			} else if (p->type == MNODE) {
+				s = dgap_getword(in);
+				if (!s) {
+					pr_err("unexpected end of file");
+					return -1;
+				}
+				if (kstrtol(s, 0, &p->u.module.start)) {
+					pr_err("bad number for start of tty count");
+					return -1;
+				}
+				p->u.module.v_start = 1;
+			} else {
+				pr_err("start only valid for concentrators or modules");
+				return -1;
+			}
+			break;
 
-	/*
-	 * Set up our interrupt handler if we are set to do interrupts.
-	 */
-	if (dgap_config_get_useintr(brd) && brd->irq) {
+		case TTYN:	/* tty name prefix */
+			if (dgap_checknode(p))
+				return -1;
 
-		rc = request_irq(brd->irq, dgap_intr, IRQF_SHARED, "DGAP", brd);
+			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
+			if (!p->next)
+				return -1;
 
-		if (!rc)
-			brd->intr_used = 1;
-	}
-	return 0;
-}
+			p = p->next;
+			p->type = TNODE;
 
-static void dgap_free_irq(struct board_t *brd)
-{
-	if (brd->intr_used && brd->irq)
-		free_irq(brd->irq, brd);
-}
+			s = dgap_getword(in);
+			if (!s) {
+				pr_err("unexpeced end of file");
+				return -1;
+			}
+			p->u.ttyname = kstrdup(s, GFP_KERNEL);
+			if (!p->u.ttyname)
+				return -1;
 
-static int dgap_firmware_load(struct pci_dev *pdev, int card_type,
-			      struct board_t *brd)
-{
-	const struct firmware *fw;
-	char *tmp_ptr;
-	int ret;
-	char *dgap_config_buf;
+			break;
 
-	dgap_get_vpd(brd);
-	dgap_do_reset_board(brd);
+		case CU:	/* cu name prefix */
+			if (dgap_checknode(p))
+				return -1;
 
-	if (fw_info[card_type].conf_name) {
-		ret = request_firmware(&fw, fw_info[card_type].conf_name,
-					 &pdev->dev);
-		if (ret) {
-			dev_err(&pdev->dev, "config file %s not found\n",
-				fw_info[card_type].conf_name);
-			return ret;
-		}
+			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
+			if (!p->next)
+				return -1;
 
-		dgap_config_buf = kzalloc(fw->size + 1, GFP_KERNEL);
-		if (!dgap_config_buf) {
-			release_firmware(fw);
-			return -ENOMEM;
-		}
+			p = p->next;
+			p->type = CUNODE;
 
-		memcpy(dgap_config_buf, fw->data, fw->size);
-		release_firmware(fw);
+			s = dgap_getword(in);
+			if (!s) {
+				pr_err("unexpeced end of file");
+				return -1;
+			}
+			p->u.cuname = kstrdup(s, GFP_KERNEL);
+			if (!p->u.cuname)
+				return -1;
 
-		/*
-		 * preserve dgap_config_buf
-		 * as dgap_parsefile would
-		 * otherwise alter it.
-		 */
-		tmp_ptr = dgap_config_buf;
+			break;
 
-		if (dgap_parsefile(&tmp_ptr) != 0) {
-			kfree(dgap_config_buf);
-			return -EINVAL;
-		}
-		kfree(dgap_config_buf);
-	}
+		case LINE:	/* line information */
+			if (dgap_checknode(p))
+				return -1;
+			if (!brd) {
+				pr_err("must specify board before line info");
+				return -1;
+			}
+			switch (brd->u.board.type) {
+			case PPCM:
+				pr_err("line not valid for PC/em");
+				return -1;
+			}
 
-	/*
-	 * Match this board to a config the user created for us.
-	 */
-	brd->bd_config =
-		dgap_find_config(brd->type, brd->pci_bus, brd->pci_slot);
+			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
+			if (!p->next)
+				return -1;
 
-	/*
-	 * Because the 4 port Xr products share the same PCI ID
-	 * as the 8 port Xr products, if we receive a NULL config
-	 * back, and this is a PAPORT8 board, retry with a
-	 * PAPORT4 attempt as well.
-	 */
-	if (brd->type == PAPORT8 && !brd->bd_config)
-		brd->bd_config =
-			dgap_find_config(PAPORT4, brd->pci_bus, brd->pci_slot);
+			p = p->next;
+			p->type = LNODE;
+			conc = NULL;
+			line = p;
+			linecnt++;
+			break;
 
-	if (!brd->bd_config) {
-		dev_err(&pdev->dev, "No valid configuration found\n");
-		return -EINVAL;
-	}
+		case CONC:	/* concentrator information */
+			if (dgap_checknode(p))
+				return -1;
+			if (!line) {
+				pr_err("must specify line info before concentrator");
+				return -1;
+			}
 
-	if (fw_info[card_type].bios_name) {
-		ret = request_firmware(&fw, fw_info[card_type].bios_name,
-					&pdev->dev);
-		if (ret) {
-			dev_err(&pdev->dev, "bios file %s not found\n",
-				fw_info[card_type].bios_name);
-			return ret;
-		}
-		dgap_do_bios_load(brd, fw->data, fw->size);
-		release_firmware(fw);
+			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
+			if (!p->next)
+				return -1;
 
-		/* Wait for BIOS to test board... */
-		ret = dgap_test_bios(brd);
-		if (ret)
-			return ret;
-	}
+			p = p->next;
+			p->type = CNODE;
+			conc = p;
 
-	if (fw_info[card_type].fep_name) {
-		ret = request_firmware(&fw, fw_info[card_type].fep_name,
-					&pdev->dev);
-		if (ret) {
-			dev_err(&pdev->dev, "dgap: fep file %s not found\n",
-				fw_info[card_type].fep_name);
-			return ret;
-		}
-		dgap_do_fep_load(brd, fw->data, fw->size);
-		release_firmware(fw);
+			if (linecnt)
+				brd->u.board.conc2++;
+			else
+				brd->u.board.conc1++;
 
-		/* Wait for FEP to load on board... */
-		ret = dgap_test_fep(brd);
-		if (ret)
-			return ret;
-	}
+			conc_type = dgap_gettok(in);
+			if (conc_type == 0 || conc_type != CX ||
+			    conc_type != EPC) {
+				pr_err("failed to set a type of concentratros");
+				return -1;
+			}
 
-#ifdef DIGI_CONCENTRATORS_SUPPORTED
-	/*
-	 * If this is a CX or EPCX, we need to see if the firmware
-	 * is requesting a concentrator image from us.
-	 */
-	if ((bd->type == PCX) || (bd->type == PEPC)) {
-		chk_addr = (u16 *) (vaddr + DOWNREQ);
-		/* Nonzero if FEP is requesting concentrator image. */
-		check = readw(chk_addr);
-		vaddr = brd->re_map_membase;
-	}
+			p->u.conc.type = conc_type;
 
-	if (fw_info[card_type].con_name && check && vaddr) {
-		ret = request_firmware(&fw, fw_info[card_type].con_name,
-					&pdev->dev);
-		if (ret) {
-			dev_err(&pdev->dev, "conc file %s not found\n",
-				fw_info[card_type].con_name);
-			return ret;
-		}
-		/* Put concentrator firmware loading code here */
-		offset = readw((u16 *) (vaddr + DOWNREQ));
-		memcpy_toio(offset, fw->data, fw->size);
+			break;
 
-		dgap_do_conc_load(brd, (char *)fw->data, fw->size)
-		release_firmware(fw);
-	}
-#endif
+		case MOD:	/* EBI module */
+			if (dgap_checknode(p))
+				return -1;
+			if (!brd) {
+				pr_err("must specify board info before EBI modules");
+				return -1;
+			}
+			switch (brd->u.board.type) {
+			case PPCM:
+				linecnt = 0;
+				break;
+			default:
+				if (!conc) {
+					pr_err("must specify concentrator info before EBI module");
+					return -1;
+				}
+			}
 
-	return 0;
-}
+			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
+			if (!p->next)
+				return -1;
 
-/*
- * Remap PCI memory.
- */
-static int dgap_do_remap(struct board_t *brd)
-{
-	if (!brd || brd->magic != DGAP_BOARD_MAGIC)
-		return -EIO;
+			p = p->next;
+			p->type = MNODE;
 
-	if (!request_mem_region(brd->membase, 0x200000, "dgap"))
-		return -ENOMEM;
+			if (linecnt)
+				brd->u.board.module2++;
+			else
+				brd->u.board.module1++;
 
-	if (!request_mem_region(brd->membase + PCI_IO_OFFSET, 0x200000,
-					"dgap")) {
-		release_mem_region(brd->membase, 0x200000);
-		return -ENOMEM;
-	}
+			module_type = dgap_gettok(in);
+			if (module_type == 0 || module_type != PORTS ||
+			    module_type != MODEM) {
+				pr_err("failed to set a type of module");
+				return -1;
+			}
 
-	brd->re_map_membase = ioremap(brd->membase, 0x200000);
-	if (!brd->re_map_membase) {
-		release_mem_region(brd->membase, 0x200000);
-		release_mem_region(brd->membase + PCI_IO_OFFSET, 0x200000);
-		return -ENOMEM;
-	}
+			p->u.module.type = module_type;
 
-	brd->re_map_port = ioremap((brd->membase + PCI_IO_OFFSET), 0x200000);
-	if (!brd->re_map_port) {
-		release_mem_region(brd->membase, 0x200000);
-		release_mem_region(brd->membase + PCI_IO_OFFSET, 0x200000);
-		iounmap(brd->re_map_membase);
-		return -ENOMEM;
-	}
+			break;
 
-	return 0;
-}
+		case CABLE:
+			if (p->type == LNODE) {
+				s = dgap_getword(in);
+				if (!s) {
+					pr_err("unexpected end of file");
+					return -1;
+				}
+				p->u.line.cable = kstrdup(s, GFP_KERNEL);
+				p->u.line.v_cable = 1;
+			}
+			break;
 
-static void dgap_release_remap(struct board_t *brd)
-{
-	if (brd->re_map_membase) {
-		release_mem_region(brd->membase, 0x200000);
-		iounmap(brd->re_map_membase);
-	}
+		case SPEED:	/* sync line speed indication */
+			if (p->type == LNODE) {
+				s = dgap_getword(in);
+				if (!s) {
+					pr_err("unexpected end of file");
+					return -1;
+				}
+				if (kstrtol(s, 0, &p->u.line.speed)) {
+					pr_err("bad number for line speed");
+					return -1;
+				}
+				p->u.line.v_speed = 1;
+			} else if (p->type == CNODE) {
+				s = dgap_getword(in);
+				if (!s) {
+					pr_err("unexpected end of file");
+					return -1;
+				}
+				if (kstrtol(s, 0, &p->u.conc.speed)) {
+					pr_err("bad number for line speed");
+					return -1;
+				}
+				p->u.conc.v_speed = 1;
+			} else {
+				pr_err("speed valid only for lines or concentrators.");
+				return -1;
+			}
+			break;
 
-	if (brd->re_map_port) {
-		release_mem_region(brd->membase + PCI_IO_OFFSET, 0x200000);
-		iounmap(brd->re_map_port);
-	}
-}
-/*****************************************************************************
-*
-* Function:
-*
-*    dgap_poll_handler
-*
-* Author:
-*
-*    Scott H Kilau
-*
-* Parameters:
-*
-*    dummy -- ignored
-*
-* Return Values:
-*
-*    none
-*
-* Description:
-*
-*    As each timer expires, it determines (a) whether the "transmit"
-*    waiter needs to be woken up, and (b) whether the poller needs to
-*    be rescheduled.
-*
-******************************************************************************/
+		case CONNECT:
+			if (p->type == CNODE) {
+				s = dgap_getword(in);
+				if (!s) {
+					pr_err("unexpected end of file");
+					return -1;
+				}
+				p->u.conc.connect = kstrdup(s, GFP_KERNEL);
+				p->u.conc.v_connect = 1;
+			}
+			break;
+		case PRINT:	/* transparent print name prefix */
+			if (dgap_checknode(p))
+				return -1;
 
-static void dgap_poll_handler(ulong dummy)
-{
-	int i;
-	struct board_t *brd;
-	unsigned long lock_flags;
-	ulong new_time;
+			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
+			if (!p->next)
+				return -1;
 
-	dgap_poll_counter++;
+			p = p->next;
+			p->type = PNODE;
 
-	/*
-	 * Do not start the board state machine until
-	 * driver tells us its up and running, and has
-	 * everything it needs.
-	 */
-	if (dgap_driver_state != DRIVER_READY)
-		goto schedule_poller;
+			s = dgap_getword(in);
+			if (!s) {
+				pr_err("unexpeced end of file");
+				return -1;
+			}
+			p->u.printname = kstrdup(s, GFP_KERNEL);
+			if (!p->u.printname)
+				return -1;
 
-	/*
-	 * If we have just 1 board, or the system is not SMP,
-	 * then use the typical old style poller.
-	 * Otherwise, use our new tasklet based poller, which should
-	 * speed things up for multiple boards.
-	 */
-	if ((dgap_numboards == 1) || (num_online_cpus() <= 1)) {
-		for (i = 0; i < dgap_numboards; i++) {
+			break;
 
-			brd = dgap_board[i];
+		case CMAJOR:	/* major number */
+			if (dgap_checknode(p))
+				return -1;
 
-			if (brd->state == BOARD_FAILED)
-				continue;
-			if (!brd->intr_running)
-				/* Call the real board poller directly */
-				dgap_poll_tasklet((unsigned long) brd);
-		}
-	} else {
-		/*
-		 * Go thru each board, kicking off a
-		 * tasklet for each if needed
-		 */
-		for (i = 0; i < dgap_numboards; i++) {
-			brd = dgap_board[i];
+			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
+			if (!p->next)
+				return -1;
 
-			/*
-			 * Attempt to grab the board lock.
-			 *
-			 * If we can't get it, no big deal, the next poll
-			 * will get it. Basically, I just really don't want
-			 * to spin in here, because I want to kick off my
-			 * tasklets as fast as I can, and then get out the
-			 * poller.
-			 */
-			if (!spin_trylock(&brd->bd_lock))
-				continue;
+			p = p->next;
+			p->type = JNODE;
 
-			/*
-			 * If board is in a failed state, don't bother
-			 *  scheduling a tasklet
-			 */
-			if (brd->state == BOARD_FAILED) {
-				spin_unlock(&brd->bd_lock);
-				continue;
+			s = dgap_getword(in);
+			if (!s) {
+				pr_err("unexpected end of file");
+				return -1;
 			}
+			if (kstrtol(s, 0, &p->u.majornumber)) {
+				pr_err("bad number for major number");
+				return -1;
+			}
+			break;
 
-			/* Schedule a poll helper task */
-			if (!brd->intr_running)
-				tasklet_schedule(&brd->helper_tasklet);
-
-			/*
-			 * Can't do DGAP_UNLOCK here, as we don't have
-			 * lock_flags because we did a trylock above.
-			 */
-			spin_unlock(&brd->bd_lock);
-		}
-	}
+		case ALTPIN:	/* altpin setting */
+			if (dgap_checknode(p))
+				return -1;
 
-schedule_poller:
+			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
+			if (!p->next)
+				return -1;
 
-	/*
-	 * Schedule ourself back at the nominal wakeup interval.
-	 */
-	spin_lock_irqsave(&dgap_poll_lock, lock_flags);
-	dgap_poll_time +=  dgap_jiffies_from_ms(dgap_poll_tick);
+			p = p->next;
+			p->type = ANODE;
 
-	new_time = dgap_poll_time - jiffies;
+			s = dgap_getword(in);
+			if (!s) {
+				pr_err("unexpected end of file");
+				return -1;
+			}
+			if (kstrtol(s, 0, &p->u.altpin)) {
+				pr_err("bad number for altpin");
+				return -1;
+			}
+			break;
 
-	if ((ulong) new_time >= 2 * dgap_poll_tick) {
-		dgap_poll_time =
-			jiffies +  dgap_jiffies_from_ms(dgap_poll_tick);
-	}
+		case USEINTR:		/* enable interrupt setting */
+			if (dgap_checknode(p))
+				return -1;
 
-	dgap_poll_timer.function = dgap_poll_handler;
-	dgap_poll_timer.data = 0;
-	dgap_poll_timer.expires = dgap_poll_time;
-	spin_unlock_irqrestore(&dgap_poll_lock, lock_flags);
+			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
+			if (!p->next)
+				return -1;
 
-	if (!dgap_poll_stop)
-		add_timer(&dgap_poll_timer);
-}
+			p = p->next;
+			p->type = INTRNODE;
+			s = dgap_getword(in);
+			if (!s) {
+				pr_err("unexpected end of file");
+				return -1;
+			}
+			if (kstrtol(s, 0, &p->u.useintr)) {
+				pr_err("bad number for useintr");
+				return -1;
+			}
+			break;
 
-/*
- * dgap_intr()
- *
- * Driver interrupt handler.
- */
-static irqreturn_t dgap_intr(int irq, void *voidbrd)
-{
-	struct board_t *brd = (struct board_t *) voidbrd;
+		case TTSIZ:	/* size of tty structure */
+			if (dgap_checknode(p))
+				return -1;
 
-	if (!brd)
-		return IRQ_NONE;
+			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
+			if (!p->next)
+				return -1;
 
-	/*
-	 * Check to make sure its for us.
-	 */
-	if (brd->magic != DGAP_BOARD_MAGIC)
-		return IRQ_NONE;
+			p = p->next;
+			p->type = TSNODE;
 
-	brd->intr_count++;
+			s = dgap_getword(in);
+			if (!s) {
+				pr_err("unexpected end of file");
+				return -1;
+			}
+			if (kstrtol(s, 0, &p->u.ttysize)) {
+				pr_err("bad number for ttysize");
+				return -1;
+			}
+			break;
 
-	/*
-	 * Schedule tasklet to run at a better time.
-	 */
-	tasklet_schedule(&brd->helper_tasklet);
-	return IRQ_HANDLED;
-}
+		case CHSIZ:	/* channel structure size */
+			if (dgap_checknode(p))
+				return -1;
 
-/*
- * dgap_init_globals()
- *
- * This is where we initialize the globals from the static insmod
- * configuration variables.  These are declared near the head of
- * this file.
- */
-static void dgap_init_globals(void)
-{
-	int i;
+			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
+			if (!p->next)
+				return -1;
 
-	for (i = 0; i < MAXBOARDS; i++)
-		dgap_board[i] = NULL;
+			p = p->next;
+			p->type = CSNODE;
 
-	init_timer(&dgap_poll_timer);
-}
+			s = dgap_getword(in);
+			if (!s) {
+				pr_err("unexpected end of file");
+				return -1;
+			}
+			if (kstrtol(s, 0, &p->u.chsize)) {
+				pr_err("bad number for chsize");
+				return -1;
+			}
+			break;
 
-/************************************************************************
- *
- * TTY Initialization/Cleanup Functions
- *
- ************************************************************************/
+		case BSSIZ:	/* board structure size */
+			if (dgap_checknode(p))
+				return -1;
 
-/*
- * dgap_tty_register()
- *
- * Init the tty subsystem for this board.
- */
-static int dgap_tty_register(struct board_t *brd)
-{
-	int rc;
+			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
+			if (!p->next)
+				return -1;
 
-	brd->serial_driver = tty_alloc_driver(MAXPORTS, 0);
-	if (IS_ERR(brd->serial_driver))
-		return PTR_ERR(brd->serial_driver);
+			p = p->next;
+			p->type = BSNODE;
 
-	snprintf(brd->serial_name, MAXTTYNAMELEN, "tty_dgap_%d_",
-		 brd->boardnum);
-	brd->serial_driver->name = brd->serial_name;
-	brd->serial_driver->name_base = 0;
-	brd->serial_driver->major = 0;
-	brd->serial_driver->minor_start = 0;
-	brd->serial_driver->type = TTY_DRIVER_TYPE_SERIAL;
-	brd->serial_driver->subtype = SERIAL_TYPE_NORMAL;
-	brd->serial_driver->init_termios = dgap_default_termios;
-	brd->serial_driver->driver_name = DRVSTR;
-	brd->serial_driver->flags = (TTY_DRIVER_REAL_RAW |
-				    TTY_DRIVER_DYNAMIC_DEV |
-				    TTY_DRIVER_HARDWARE_BREAK);
-
-	/* The kernel wants space to store pointers to tty_structs */
-	brd->serial_driver->ttys =
-		kzalloc(MAXPORTS * sizeof(struct tty_struct *), GFP_KERNEL);
-	if (!brd->serial_driver->ttys) {
-		rc = -ENOMEM;
-		goto free_serial_drv;
-	}
+			s = dgap_getword(in);
+			if (!s) {
+				pr_err("unexpected end of file");
+				return -1;
+			}
+			if (kstrtol(s, 0, &p->u.bssize)) {
+				pr_err("bad number for bssize");
+				return -1;
+			}
+			break;
 
-	/*
-	 * Entry points for driver.  Called by the kernel from
-	 * tty_io.c and n_tty.c.
-	 */
-	tty_set_operations(brd->serial_driver, &dgap_tty_ops);
+		case UNTSIZ:	/* sched structure size */
+			if (dgap_checknode(p))
+				return -1;
 
-	/*
-	 * If we're doing transparent print, we have to do all of the above
-	 * again, separately so we don't get the LD confused about what major
-	 * we are when we get into the dgap_tty_open() routine.
-	 */
-	brd->print_driver = tty_alloc_driver(MAXPORTS, 0);
-	if (IS_ERR(brd->print_driver)) {
-		rc = PTR_ERR(brd->print_driver);
-		goto free_serial_drv;
-	}
+			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
+			if (!p->next)
+				return -1;
 
-	snprintf(brd->print_name, MAXTTYNAMELEN, "pr_dgap_%d_",
-		 brd->boardnum);
-	brd->print_driver->name = brd->print_name;
-	brd->print_driver->name_base = 0;
-	brd->print_driver->major = 0;
-	brd->print_driver->minor_start = 0;
-	brd->print_driver->type = TTY_DRIVER_TYPE_SERIAL;
-	brd->print_driver->subtype = SERIAL_TYPE_NORMAL;
-	brd->print_driver->init_termios = dgap_default_termios;
-	brd->print_driver->driver_name = DRVSTR;
-	brd->print_driver->flags = (TTY_DRIVER_REAL_RAW |
-				   TTY_DRIVER_DYNAMIC_DEV |
-				   TTY_DRIVER_HARDWARE_BREAK);
-
-	/* The kernel wants space to store pointers to tty_structs */
-	brd->print_driver->ttys =
-		kzalloc(MAXPORTS * sizeof(struct tty_struct *), GFP_KERNEL);
-	if (!brd->print_driver->ttys) {
-		rc = -ENOMEM;
-		goto free_print_drv;
-	}
+			p = p->next;
+			p->type = USNODE;
 
-	/*
-	 * Entry points for driver.  Called by the kernel from
-	 * tty_io.c and n_tty.c.
-	 */
-	tty_set_operations(brd->print_driver, &dgap_tty_ops);
+			s = dgap_getword(in);
+			if (!s) {
+				pr_err("unexpected end of file");
+				return -1;
+			}
+			if (kstrtol(s, 0, &p->u.unsize)) {
+				pr_err("bad number for schedsize");
+				return -1;
+			}
+			break;
 
-	/* Register tty devices */
-	rc = tty_register_driver(brd->serial_driver);
-	if (rc < 0)
-		goto free_print_drv;
+		case F2SIZ:	/* f2200 structure size */
+			if (dgap_checknode(p))
+				return -1;
 
-	/* Register Transparent Print devices */
-	rc = tty_register_driver(brd->print_driver);
-	if (rc < 0)
-		goto unregister_serial_drv;
+			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
+			if (!p->next)
+				return -1;
 
-	dgap_boards_by_major[brd->serial_driver->major] = brd;
-	brd->dgap_serial_major = brd->serial_driver->major;
+			p = p->next;
+			p->type = FSNODE;
 
-	dgap_boards_by_major[brd->print_driver->major] = brd;
-	brd->dgap_transparent_print_major = brd->print_driver->major;
+			s = dgap_getword(in);
+			if (!s) {
+				pr_err("unexpected end of file");
+				return -1;
+			}
+			if (kstrtol(s, 0, &p->u.f2size)) {
+				pr_err("bad number for f2200size");
+				return -1;
+			}
+			break;
 
-	return 0;
+		case VPSIZ:	/* vpix structure size */
+			if (dgap_checknode(p))
+				return -1;
 
-unregister_serial_drv:
-	tty_unregister_driver(brd->serial_driver);
-free_print_drv:
-	put_tty_driver(brd->print_driver);
-free_serial_drv:
-	put_tty_driver(brd->serial_driver);
+			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
+			if (!p->next)
+				return -1;
 
-	return rc;
-}
+			p = p->next;
+			p->type = VSNODE;
 
-static void dgap_tty_unregister(struct board_t *brd)
-{
-	tty_unregister_driver(brd->print_driver);
-	tty_unregister_driver(brd->serial_driver);
-	put_tty_driver(brd->print_driver);
-	put_tty_driver(brd->serial_driver);
+			s = dgap_getword(in);
+			if (!s) {
+				pr_err("unexpected end of file");
+				return -1;
+			}
+			if (kstrtol(s, 0, &p->u.vpixsize)) {
+				pr_err("bad number for vpixsize");
+				return -1;
+			}
+			break;
+		}
+	}
 }
 
-/*
- * dgap_tty_init()
- *
- * Init the tty subsystem.  Called once per board after board has been
- * downloaded and init'ed.
- */
-static int dgap_tty_init(struct board_t *brd)
+static void dgap_cleanup_nodes(void)
 {
-	int i;
-	int tlw;
-	uint true_count;
-	u8 __iomem *vaddr;
-	u8 modem;
-	struct channel_t *ch;
-	struct bs_t __iomem *bs;
-	struct cm_t __iomem *cm;
-	int ret;
-
-	/*
-	 * Initialize board structure elements.
-	 */
-
-	vaddr = brd->re_map_membase;
-	true_count = readw((vaddr + NCHAN));
-
-	brd->nasync = dgap_config_get_num_prts(brd);
-
-	if (!brd->nasync)
-		brd->nasync = brd->maxports;
+	struct cnode *p;
 
-	if (brd->nasync > brd->maxports)
-		brd->nasync = brd->maxports;
+	p = &dgap_head;
 
-	if (true_count != brd->nasync) {
-		dev_warn(&brd->pdev->dev,
-			 "%s configured for %d ports, has %d ports.\n",
-			 brd->name, brd->nasync, true_count);
+	while (p) {
+		struct cnode *tmp = p->next;
 
-		if ((brd->type == PPCM) &&
-		    (true_count == 64 || true_count == 0)) {
-			dev_warn(&brd->pdev->dev,
-				 "Please make SURE the EBI cable running from the card\n");
-			dev_warn(&brd->pdev->dev,
-				 "to each EM module is plugged into EBI IN!\n");
+		if (p->type == NULLNODE) {
+			p = tmp;
+			continue;
 		}
 
-		brd->nasync = true_count;
-
-		/* If no ports, don't bother going any further */
-		if (!brd->nasync) {
-			brd->state = BOARD_FAILED;
-			brd->dpastatus = BD_NOFEP;
-			return -EIO;
+		switch (p->type) {
+		case BNODE:
+			kfree(p->u.board.portstr);
+			kfree(p->u.board.addrstr);
+			kfree(p->u.board.pcibusstr);
+			kfree(p->u.board.pcislotstr);
+			kfree(p->u.board.method);
+			break;
+		case CNODE:
+			kfree(p->u.conc.id);
+			kfree(p->u.conc.connect);
+			break;
+		case MNODE:
+			kfree(p->u.module.id);
+			break;
+		case TNODE:
+			kfree(p->u.ttyname);
+			break;
+		case CUNODE:
+			kfree(p->u.cuname);
+			break;
+		case LNODE:
+			kfree(p->u.line.cable);
+			break;
+		case PNODE:
+			kfree(p->u.printname);
+			break;
 		}
-	}
 
-	/*
-	 * Allocate channel memory that might not have been allocated
-	 * when the driver was first loaded.
-	 */
-	for (i = 0; i < brd->nasync; i++) {
-		brd->channels[i] =
-			kzalloc(sizeof(struct channel_t), GFP_KERNEL);
-		if (!brd->channels[i]) {
-			ret = -ENOMEM;
-			goto free_chan;
-		}
+		kfree(p->u.board.status);
+		kfree(p);
+		p = tmp;
 	}
+}
 
-	ch = brd->channels[0];
-	vaddr = brd->re_map_membase;
-
-	bs = (struct bs_t __iomem *) ((ulong) vaddr + CHANBUF);
-	cm = (struct cm_t __iomem *) ((ulong) vaddr + CMDBUF);
-
-	brd->bd_bs = bs;
-
-	/* Set up channel variables */
-	for (i = 0; i < brd->nasync; i++, ch = brd->channels[i], bs++) {
-
-		spin_lock_init(&ch->ch_lock);
-
-		/* Store all our magic numbers */
-		ch->magic = DGAP_CHANNEL_MAGIC;
-		ch->ch_tun.magic = DGAP_UNIT_MAGIC;
-		ch->ch_tun.un_type = DGAP_SERIAL;
-		ch->ch_tun.un_ch = ch;
-		ch->ch_tun.un_dev = i;
-
-		ch->ch_pun.magic = DGAP_UNIT_MAGIC;
-		ch->ch_pun.un_type = DGAP_PRINT;
-		ch->ch_pun.un_ch = ch;
-		ch->ch_pun.un_dev = i;
+/*
+ * Retrives the current custom baud rate from FEP memory,
+ * and returns it back to the user.
+ * Returns 0 on error.
+ */
+static uint dgap_get_custom_baud(struct channel_t *ch)
+{
+	u8 __iomem *vaddr;
+	ulong offset;
+	uint value;
 
-		ch->ch_vaddr = vaddr;
-		ch->ch_bs = bs;
-		ch->ch_cm = cm;
-		ch->ch_bd = brd;
-		ch->ch_portnum = i;
-		ch->ch_digi = dgap_digi_init;
+	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
+		return 0;
 
-		/*
-		 * Set up digi dsr and dcd bits based on altpin flag.
-		 */
-		if (dgap_config_get_altpin(brd)) {
-			ch->ch_dsr	= DM_CD;
-			ch->ch_cd	= DM_DSR;
-			ch->ch_digi.digi_flags |= DIGI_ALTPIN;
-		} else {
-			ch->ch_cd	= DM_CD;
-			ch->ch_dsr	= DM_DSR;
-		}
+	if (!ch->ch_bd || ch->ch_bd->magic != DGAP_BOARD_MAGIC)
+		return 0;
 
-		ch->ch_taddr = vaddr + (ioread16(&(ch->ch_bs->tx_seg)) << 4);
-		ch->ch_raddr = vaddr + (ioread16(&(ch->ch_bs->rx_seg)) << 4);
-		ch->ch_tx_win = 0;
-		ch->ch_rx_win = 0;
-		ch->ch_tsize = readw(&(ch->ch_bs->tx_max)) + 1;
-		ch->ch_rsize = readw(&(ch->ch_bs->rx_max)) + 1;
-		ch->ch_tstart = 0;
-		ch->ch_rstart = 0;
+	if (!(ch->ch_bd->bd_flags & BD_FEP5PLUS))
+		return 0;
 
-		/*
-		 * Set queue water marks, interrupt mask,
-		 * and general tty parameters.
-		 */
-		tlw = ch->ch_tsize >= 2000 ? ((ch->ch_tsize * 5) / 8) :
-						ch->ch_tsize / 2;
-		ch->ch_tlw = tlw;
+	vaddr = ch->ch_bd->re_map_membase;
 
-		dgap_cmdw(ch, STLOW, tlw, 0);
+	if (!vaddr)
+		return 0;
 
-		dgap_cmdw(ch, SRLOW, ch->ch_rsize / 2, 0);
+	/*
+	 * Go get from fep mem, what the fep
+	 * believes the custom baud rate is.
+	 */
+	offset = (ioread16(vaddr + ECS_SEG) << 4) + (ch->ch_portnum * 0x28)
+	       + LINE_SPEED;
 
-		dgap_cmdw(ch, SRHIGH, 7 * ch->ch_rsize / 8, 0);
+	value = readw(vaddr + offset);
+	return value;
+}
 
-		ch->ch_mistat = readb(&(ch->ch_bs->m_stat));
+/*
+ * Remap PCI memory.
+ */
+static int dgap_remap(struct board_t *brd)
+{
+	if (!brd || brd->magic != DGAP_BOARD_MAGIC)
+		return -EIO;
 
-		init_waitqueue_head(&ch->ch_flags_wait);
-		init_waitqueue_head(&ch->ch_tun.un_flags_wait);
-		init_waitqueue_head(&ch->ch_pun.un_flags_wait);
+	if (!request_mem_region(brd->membase, 0x200000, "dgap"))
+		return -ENOMEM;
 
-		/* Turn on all modem interrupts for now */
-		modem = (DM_CD | DM_DSR | DM_CTS | DM_RI);
-		writeb(modem, &(ch->ch_bs->m_int));
+	if (!request_mem_region(brd->membase + PCI_IO_OFFSET, 0x200000,
+					"dgap")) {
+		release_mem_region(brd->membase, 0x200000);
+		return -ENOMEM;
+	}
 
-		/*
-		 * Set edelay to 0 if interrupts are turned on,
-		 * otherwise set edelay to the usual 100.
-		 */
-		if (brd->intr_used)
-			writew(0, &(ch->ch_bs->edelay));
-		else
-			writew(100, &(ch->ch_bs->edelay));
+	brd->re_map_membase = ioremap(brd->membase, 0x200000);
+	if (!brd->re_map_membase) {
+		release_mem_region(brd->membase, 0x200000);
+		release_mem_region(brd->membase + PCI_IO_OFFSET, 0x200000);
+		return -ENOMEM;
+	}
 
-		writeb(1, &(ch->ch_bs->idata));
+	brd->re_map_port = ioremap((brd->membase + PCI_IO_OFFSET), 0x200000);
+	if (!brd->re_map_port) {
+		release_mem_region(brd->membase, 0x200000);
+		release_mem_region(brd->membase + PCI_IO_OFFSET, 0x200000);
+		iounmap(brd->re_map_membase);
+		return -ENOMEM;
 	}
 
 	return 0;
-
-free_chan:
-	while (--i >= 0) {
-		kfree(brd->channels[i]);
-		brd->channels[i] = NULL;
-	}
-	return ret;
 }
 
-/*
- * dgap_tty_free()
- *
- * Free the channles which are allocated in dgap_tty_init().
- */
-static void dgap_tty_free(struct board_t *brd)
+static void dgap_unmap(struct board_t *brd)
 {
-	int i;
-
-	for (i = 0; i < brd->nasync; i++)
-		kfree(brd->channels[i]);
+	iounmap(brd->re_map_port);
+	iounmap(brd->re_map_membase);
+	release_mem_region(brd->membase + PCI_IO_OFFSET, 0x200000);
+	release_mem_region(brd->membase, 0x200000);
 }
+
 /*
- * dgap_cleanup_tty()
+ * dgap_parity_scan()
  *
- * Uninitialize the TTY portion of this driver.  Free all memory and
- * resources.
+ * Convert the FEP5 way of reporting parity errors and breaks into
+ * the Linux line discipline way.
  */
-static void dgap_cleanup_tty(struct board_t *brd)
+static void dgap_parity_scan(struct channel_t *ch, unsigned char *cbuf,
+				unsigned char *fbuf, int *len)
 {
-	struct device *dev;
-	int i;
+	int l = *len;
+	int count = 0;
+	unsigned char *in, *cout, *fout;
+	unsigned char c;
 
-	dgap_boards_by_major[brd->serial_driver->major] = NULL;
-	brd->dgap_serial_major = 0;
-	for (i = 0; i < brd->nasync; i++) {
-		tty_port_destroy(&brd->serial_ports[i]);
-		dev = brd->channels[i]->ch_tun.un_sysfs;
-		dgap_remove_tty_sysfs(dev);
-		tty_unregister_device(brd->serial_driver, i);
-	}
-	tty_unregister_driver(brd->serial_driver);
-	put_tty_driver(brd->serial_driver);
-	kfree(brd->serial_ports);
+	in = cbuf;
+	cout = cbuf;
+	fout = fbuf;
 
-	dgap_boards_by_major[brd->print_driver->major] = NULL;
-	brd->dgap_transparent_print_major = 0;
-	for (i = 0; i < brd->nasync; i++) {
-		tty_port_destroy(&brd->printer_ports[i]);
-		dev = brd->channels[i]->ch_pun.un_sysfs;
-		dgap_remove_tty_sysfs(dev);
-		tty_unregister_device(brd->print_driver, i);
+	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
+		return;
+
+	while (l--) {
+		c = *in++;
+		switch (ch->pscan_state) {
+		default:
+			/* reset to sanity and fall through */
+			ch->pscan_state = 0;
+
+		case 0:
+			/* No FF seen yet */
+			if (c == (unsigned char) '\377')
+				/* delete this character from stream */
+				ch->pscan_state = 1;
+			else {
+				*cout++ = c;
+				*fout++ = TTY_NORMAL;
+				count += 1;
+			}
+			break;
+
+		case 1:
+			/* first FF seen */
+			if (c == (unsigned char) '\377') {
+				/* doubled ff, transform to single ff */
+				*cout++ = c;
+				*fout++ = TTY_NORMAL;
+				count += 1;
+				ch->pscan_state = 0;
+			} else {
+				/* save value examination in next state */
+				ch->pscan_savechar = c;
+				ch->pscan_state = 2;
+			}
+			break;
+
+		case 2:
+			/* third character of ff sequence */
+
+			*cout++ = c;
+
+			if (ch->pscan_savechar == 0x0) {
+
+				if (c == 0x0) {
+					ch->ch_err_break++;
+					*fout++ = TTY_BREAK;
+				} else {
+					ch->ch_err_parity++;
+					*fout++ = TTY_PARITY;
+				}
+			}
+
+			count += 1;
+			ch->pscan_state = 0;
+		}
 	}
-	tty_unregister_driver(brd->print_driver);
-	put_tty_driver(brd->print_driver);
-	kfree(brd->printer_ports);
+	*len = count;
 }
 
 /*=======================================================================
@@ -1738,6 +1717,33 @@ static void dgap_input(struct channel_t *ch)
 
 }
 
+static void dgap_write_wakeup(struct board_t *bd, struct channel_t *ch,
+			      struct un_t *un, u32 mask,
+			      unsigned long *irq_flags1,
+			      unsigned long *irq_flags2)
+{
+	if (!(un->un_flags & mask))
+		return;
+
+	un->un_flags &= ~mask;
+
+	if (!(un->un_flags & UN_ISOPEN))
+		return;
+
+	if ((un->un_tty->flags & (1 << TTY_DO_WRITE_WAKEUP)) &&
+	    un->un_tty->ldisc->ops->write_wakeup) {
+		spin_unlock_irqrestore(&ch->ch_lock, *irq_flags2);
+		spin_unlock_irqrestore(&bd->bd_lock, *irq_flags1);
+
+		(un->un_tty->ldisc->ops->write_wakeup)(un->un_tty);
+
+		spin_lock_irqsave(&bd->bd_lock, *irq_flags1);
+		spin_lock_irqsave(&ch->ch_lock, *irq_flags2);
+	}
+	wake_up_interruptible(&un->un_tty->write_wait);
+	wake_up_interruptible(&un->un_flags_wait);
+}
+
 /************************************************************************
  * Determines when CARRIER changes state and takes appropriate
  * action.
@@ -1852,163 +1858,1207 @@ static void dgap_carrier(struct channel_t *ch)
 		ch->ch_flags &= ~CH_CD;
 }
 
-/************************************************************************
+/*=======================================================================
  *
- * TTY Entry points and helper functions
+ *      dgap_event - FEP to host event processing routine.
  *
- ************************************************************************/
-
-/*
- * dgap_tty_open()
+ *              bd     - Board of current event.
  *
- */
-static int dgap_tty_open(struct tty_struct *tty, struct file *file)
+ *=======================================================================*/
+static int dgap_event(struct board_t *bd)
 {
-	struct board_t *brd;
 	struct channel_t *ch;
-	struct un_t *un;
-	struct bs_t __iomem *bs;
-	uint major;
-	uint minor;
-	int rc;
 	ulong lock_flags;
 	ulong lock_flags2;
-	u16 head;
+	struct bs_t __iomem *bs;
+	u8 __iomem *event;
+	u8 __iomem *vaddr;
+	struct ev_t __iomem *eaddr;
+	uint head;
+	uint tail;
+	int port;
+	int reason;
+	int modem;
+	int b1;
 
-	major = MAJOR(tty_devnum(tty));
-	minor = MINOR(tty_devnum(tty));
+	if (!bd || bd->magic != DGAP_BOARD_MAGIC)
+		return -EIO;
+
+	spin_lock_irqsave(&bd->bd_lock, lock_flags);
+
+	vaddr = bd->re_map_membase;
 
-	if (major > 255)
+	if (!vaddr) {
+		spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
 		return -EIO;
+	}
 
-	/* Get board pointer from our array of majors we have allocated */
-	brd = dgap_boards_by_major[major];
-	if (!brd)
+	eaddr = (struct ev_t __iomem *) (vaddr + EVBUF);
+
+	/* Get our head and tail */
+	head = readw(&(eaddr->ev_head));
+	tail = readw(&(eaddr->ev_tail));
+
+	/*
+	 * Forget it if pointers out of range.
+	 */
+
+	if (head >= EVMAX - EVSTART || tail >= EVMAX - EVSTART ||
+	    (head | tail) & 03) {
+		/* Let go of board lock */
+		spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
 		return -EIO;
+	}
 
 	/*
-	 * If board is not yet up to a state of READY, go to
-	 * sleep waiting for it to happen or they cancel the open.
+	 * Loop to process all the events in the buffer.
 	 */
-	rc = wait_event_interruptible(brd->state_wait,
-		(brd->state & BOARD_READY));
+	while (tail != head) {
 
-	if (rc)
-		return rc;
+		/*
+		 * Get interrupt information.
+		 */
 
-	spin_lock_irqsave(&brd->bd_lock, lock_flags);
+		event = bd->re_map_membase + tail + EVSTART;
 
-	/* The wait above should guarantee this cannot happen */
-	if (brd->state != BOARD_READY) {
-		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
-		return -EIO;
+		port   = ioread8(event);
+		reason = ioread8(event + 1);
+		modem  = ioread8(event + 2);
+		b1     = ioread8(event + 3);
+
+		/*
+		 * Make sure the interrupt is valid.
+		 */
+		if (port >= bd->nasync)
+			goto next;
+
+		if (!(reason & (IFMODEM | IFBREAK | IFTLW | IFTEM | IFDATA)))
+			goto next;
+
+		ch = bd->channels[port];
+
+		if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
+			goto next;
+
+		/*
+		 * If we have made it here, the event was valid.
+		 * Lock down the channel.
+		 */
+		spin_lock_irqsave(&ch->ch_lock, lock_flags2);
+
+		bs = ch->ch_bs;
+
+		if (!bs) {
+			spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
+			goto next;
+		}
+
+		/*
+		 * Process received data.
+		 */
+		if (reason & IFDATA) {
+
+			/*
+			 * ALL LOCKS *MUST* BE DROPPED BEFORE CALLING INPUT!
+			 * input could send some data to ld, which in turn
+			 * could do a callback to one of our other functions.
+			 */
+			spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
+			spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
+
+			dgap_input(ch);
+
+			spin_lock_irqsave(&bd->bd_lock, lock_flags);
+			spin_lock_irqsave(&ch->ch_lock, lock_flags2);
+
+			if (ch->ch_flags & CH_RACTIVE)
+				ch->ch_flags |= CH_RENABLE;
+			else
+				writeb(1, &(bs->idata));
+
+			if (ch->ch_flags & CH_RWAIT) {
+				ch->ch_flags &= ~CH_RWAIT;
+
+				wake_up_interruptible
+					(&ch->ch_tun.un_flags_wait);
+			}
+		}
+
+		/*
+		 * Process Modem change signals.
+		 */
+		if (reason & IFMODEM) {
+			ch->ch_mistat = modem;
+			dgap_carrier(ch);
+		}
+
+		/*
+		 * Process break.
+		 */
+		if (reason & IFBREAK) {
+
+			if (ch->ch_tun.un_tty) {
+				/* A break has been indicated */
+				ch->ch_err_break++;
+				tty_buffer_request_room
+					(ch->ch_tun.un_tty->port, 1);
+				tty_insert_flip_char(ch->ch_tun.un_tty->port,
+						     0, TTY_BREAK);
+				tty_flip_buffer_push(ch->ch_tun.un_tty->port);
+			}
+		}
+
+		/*
+		 * Process Transmit low.
+		 */
+		if (reason & IFTLW) {
+			dgap_write_wakeup(bd, ch, &ch->ch_tun, UN_LOW,
+					  &lock_flags, &lock_flags2);
+			dgap_write_wakeup(bd, ch, &ch->ch_pun, UN_LOW,
+					  &lock_flags, &lock_flags2);
+			if (ch->ch_flags & CH_WLOW) {
+				ch->ch_flags &= ~CH_WLOW;
+				wake_up_interruptible(&ch->ch_flags_wait);
+			}
+		}
+
+		/*
+		 * Process Transmit empty.
+		 */
+		if (reason & IFTEM) {
+			dgap_write_wakeup(bd, ch, &ch->ch_tun, UN_EMPTY,
+					  &lock_flags, &lock_flags2);
+			dgap_write_wakeup(bd, ch, &ch->ch_pun, UN_EMPTY,
+					  &lock_flags, &lock_flags2);
+			if (ch->ch_flags & CH_WEMPTY) {
+				ch->ch_flags &= ~CH_WEMPTY;
+				wake_up_interruptible(&ch->ch_flags_wait);
+			}
+		}
+
+		spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
+
+next:
+		tail = (tail + 4) & (EVMAX - EVSTART - 4);
 	}
 
-	/* If opened device is greater than our number of ports, bail. */
-	if (MINOR(tty_devnum(tty)) > brd->nasync) {
-		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
-		return -EIO;
+	writew(tail, &(eaddr->ev_tail));
+	spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
+
+	return 0;
+}
+
+/*
+ * Our board poller function.
+ */
+static void dgap_poll_tasklet(unsigned long data)
+{
+	struct board_t *bd = (struct board_t *) data;
+	ulong lock_flags;
+	char __iomem *vaddr;
+	u16 head, tail;
+
+	if (!bd || (bd->magic != DGAP_BOARD_MAGIC))
+		return;
+
+	if (bd->inhibit_poller)
+		return;
+
+	spin_lock_irqsave(&bd->bd_lock, lock_flags);
+
+	vaddr = bd->re_map_membase;
+
+	/*
+	 * If board is ready, parse deeper to see if there is anything to do.
+	 */
+	if (bd->state == BOARD_READY) {
+
+		struct ev_t __iomem *eaddr;
+
+		if (!bd->re_map_membase) {
+			spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
+			return;
+		}
+		if (!bd->re_map_port) {
+			spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
+			return;
+		}
+
+		if (!bd->nasync)
+			goto out;
+
+		eaddr = (struct ev_t __iomem *) (vaddr + EVBUF);
+
+		/* Get our head and tail */
+		head = readw(&(eaddr->ev_head));
+		tail = readw(&(eaddr->ev_tail));
+
+		/*
+		 * If there is an event pending. Go service it.
+		 */
+		if (head != tail) {
+			spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
+			dgap_event(bd);
+			spin_lock_irqsave(&bd->bd_lock, lock_flags);
+		}
+
+out:
+		/*
+		 * If board is doing interrupts, ACK the interrupt.
+		 */
+		if (bd && bd->intr_running)
+			readb(bd->re_map_port + 2);
+
+		spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
+		return;
 	}
 
-	ch = brd->channels[minor];
-	if (!ch) {
-		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
-		return -EIO;
+	spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
+}
+
+/*
+ * dgap_found_board()
+ *
+ * A board has been found, init it.
+ */
+static struct board_t *dgap_found_board(struct pci_dev *pdev, int id,
+					int boardnum)
+{
+	struct board_t *brd;
+	unsigned int pci_irq;
+	int i;
+	int ret;
+
+	/* get the board structure and prep it */
+	brd = kzalloc(sizeof(struct board_t), GFP_KERNEL);
+	if (!brd)
+		return ERR_PTR(-ENOMEM);
+
+	/* store the info for the board we've found */
+	brd->magic = DGAP_BOARD_MAGIC;
+	brd->boardnum = boardnum;
+	brd->vendor = dgap_pci_tbl[id].vendor;
+	brd->device = dgap_pci_tbl[id].device;
+	brd->pdev = pdev;
+	brd->pci_bus = pdev->bus->number;
+	brd->pci_slot = PCI_SLOT(pdev->devfn);
+	brd->name = dgap_ids[id].name;
+	brd->maxports = dgap_ids[id].maxports;
+	brd->type = dgap_ids[id].config_type;
+	brd->dpatype = dgap_ids[id].dpatype;
+	brd->dpastatus = BD_NOFEP;
+	init_waitqueue_head(&brd->state_wait);
+
+	spin_lock_init(&brd->bd_lock);
+
+	brd->inhibit_poller	= FALSE;
+	brd->wait_for_bios	= 0;
+	brd->wait_for_fep	= 0;
+
+	for (i = 0; i < MAXPORTS; i++)
+		brd->channels[i] = NULL;
+
+	/* store which card & revision we have */
+	pci_read_config_word(pdev, PCI_SUBSYSTEM_VENDOR_ID, &brd->subvendor);
+	pci_read_config_word(pdev, PCI_SUBSYSTEM_ID, &brd->subdevice);
+	pci_read_config_byte(pdev, PCI_REVISION_ID, &brd->rev);
+
+	pci_irq = pdev->irq;
+	brd->irq = pci_irq;
+
+	/* get the PCI Base Address Registers */
+
+	/* Xr Jupiter and EPC use BAR 2 */
+	if (brd->device == PCI_DEV_XRJ_DID || brd->device == PCI_DEV_EPCJ_DID) {
+		brd->membase     = pci_resource_start(pdev, 2);
+		brd->membase_end = pci_resource_end(pdev, 2);
+	}
+	/* Everyone else uses BAR 0 */
+	else {
+		brd->membase     = pci_resource_start(pdev, 0);
+		brd->membase_end = pci_resource_end(pdev, 0);
 	}
 
-	/* Grab channel lock */
-	spin_lock_irqsave(&ch->ch_lock, lock_flags2);
+	if (!brd->membase) {
+		ret = -ENODEV;
+		goto free_brd;
+	}
 
-	/* Figure out our type */
-	if (major == brd->dgap_serial_major) {
-		un = &brd->channels[minor]->ch_tun;
-		un->un_type = DGAP_SERIAL;
-	} else if (major == brd->dgap_transparent_print_major) {
-		un = &brd->channels[minor]->ch_pun;
-		un->un_type = DGAP_PRINT;
+	if (brd->membase & 1)
+		brd->membase &= ~3;
+	else
+		brd->membase &= ~15;
+
+	/*
+	 * On the PCI boards, there is no IO space allocated
+	 * The I/O registers will be in the first 3 bytes of the
+	 * upper 2MB of the 4MB memory space.  The board memory
+	 * will be mapped into the low 2MB of the 4MB memory space
+	 */
+	brd->port = brd->membase + PCI_IO_OFFSET;
+	brd->port_end = brd->port + PCI_IO_SIZE;
+
+	/*
+	 * Special initialization for non-PLX boards
+	 */
+	if (brd->device != PCI_DEV_XRJ_DID && brd->device != PCI_DEV_EPCJ_DID) {
+		unsigned short cmd;
+
+		pci_write_config_byte(pdev, 0x40, 0);
+		pci_write_config_byte(pdev, 0x46, 0);
+
+		/* Limit burst length to 2 doubleword transactions */
+		pci_write_config_byte(pdev, 0x42, 1);
+
+		/*
+		 * Enable IO and mem if not already done.
+		 * This was needed for support on Itanium.
+		 */
+		pci_read_config_word(pdev, PCI_COMMAND, &cmd);
+		cmd |= (PCI_COMMAND_IO | PCI_COMMAND_MEMORY);
+		pci_write_config_word(pdev, PCI_COMMAND, cmd);
+	}
+
+	/* init our poll helper tasklet */
+	tasklet_init(&brd->helper_tasklet, dgap_poll_tasklet,
+			(unsigned long) brd);
+
+	ret = dgap_remap(brd);
+	if (ret)
+		goto free_brd;
+
+	pr_info("dgap: board %d: %s (rev %d), irq %ld\n",
+		boardnum, brd->name, brd->rev, brd->irq);
+
+	return brd;
+
+free_brd:
+	kfree(brd);
+
+	return ERR_PTR(ret);
+}
+
+/*
+ * dgap_intr()
+ *
+ * Driver interrupt handler.
+ */
+static irqreturn_t dgap_intr(int irq, void *voidbrd)
+{
+	struct board_t *brd = voidbrd;
+
+	if (!brd)
+		return IRQ_NONE;
+
+	/*
+	 * Check to make sure its for us.
+	 */
+	if (brd->magic != DGAP_BOARD_MAGIC)
+		return IRQ_NONE;
+
+	brd->intr_count++;
+
+	/*
+	 * Schedule tasklet to run at a better time.
+	 */
+	tasklet_schedule(&brd->helper_tasklet);
+	return IRQ_HANDLED;
+}
+
+/*****************************************************************************
+*
+* Function:
+*
+*    dgap_poll_handler
+*
+* Author:
+*
+*    Scott H Kilau
+*
+* Parameters:
+*
+*    dummy -- ignored
+*
+* Return Values:
+*
+*    none
+*
+* Description:
+*
+*    As each timer expires, it determines (a) whether the "transmit"
+*    waiter needs to be woken up, and (b) whether the poller needs to
+*    be rescheduled.
+*
+******************************************************************************/
+
+static void dgap_poll_handler(ulong dummy)
+{
+	unsigned int i;
+	struct board_t *brd;
+	unsigned long lock_flags;
+	ulong new_time;
+
+	dgap_poll_counter++;
+
+	/*
+	 * Do not start the board state machine until
+	 * driver tells us its up and running, and has
+	 * everything it needs.
+	 */
+	if (dgap_driver_state != DRIVER_READY)
+		goto schedule_poller;
+
+	/*
+	 * If we have just 1 board, or the system is not SMP,
+	 * then use the typical old style poller.
+	 * Otherwise, use our new tasklet based poller, which should
+	 * speed things up for multiple boards.
+	 */
+	if ((dgap_numboards == 1) || (num_online_cpus() <= 1)) {
+		for (i = 0; i < dgap_numboards; i++) {
+
+			brd = dgap_board[i];
+
+			if (brd->state == BOARD_FAILED)
+				continue;
+			if (!brd->intr_running)
+				/* Call the real board poller directly */
+				dgap_poll_tasklet((unsigned long) brd);
+		}
 	} else {
-		spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
-		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
-		return -EIO;
+		/*
+		 * Go thru each board, kicking off a
+		 * tasklet for each if needed
+		 */
+		for (i = 0; i < dgap_numboards; i++) {
+			brd = dgap_board[i];
+
+			/*
+			 * Attempt to grab the board lock.
+			 *
+			 * If we can't get it, no big deal, the next poll
+			 * will get it. Basically, I just really don't want
+			 * to spin in here, because I want to kick off my
+			 * tasklets as fast as I can, and then get out the
+			 * poller.
+			 */
+			if (!spin_trylock(&brd->bd_lock))
+				continue;
+
+			/*
+			 * If board is in a failed state, don't bother
+			 *  scheduling a tasklet
+			 */
+			if (brd->state == BOARD_FAILED) {
+				spin_unlock(&brd->bd_lock);
+				continue;
+			}
+
+			/* Schedule a poll helper task */
+			if (!brd->intr_running)
+				tasklet_schedule(&brd->helper_tasklet);
+
+			/*
+			 * Can't do DGAP_UNLOCK here, as we don't have
+			 * lock_flags because we did a trylock above.
+			 */
+			spin_unlock(&brd->bd_lock);
+		}
 	}
 
-	/* Store our unit into driver_data, so we always have it available. */
-	tty->driver_data = un;
+schedule_poller:
 
 	/*
-	 * Error if channel info pointer is NULL.
+	 * Schedule ourself back at the nominal wakeup interval.
 	 */
-	bs = ch->ch_bs;
-	if (!bs) {
-		spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
-		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
-		return -EIO;
+	spin_lock_irqsave(&dgap_poll_lock, lock_flags);
+	dgap_poll_time +=  dgap_jiffies_from_ms(dgap_poll_tick);
+
+	new_time = dgap_poll_time - jiffies;
+
+	if ((ulong) new_time >= 2 * dgap_poll_tick) {
+		dgap_poll_time =
+			jiffies +  dgap_jiffies_from_ms(dgap_poll_tick);
+	}
+
+	dgap_poll_timer.function = dgap_poll_handler;
+	dgap_poll_timer.data = 0;
+	dgap_poll_timer.expires = dgap_poll_time;
+	spin_unlock_irqrestore(&dgap_poll_lock, lock_flags);
+
+	if (!dgap_poll_stop)
+		add_timer(&dgap_poll_timer);
+}
+
+/*=======================================================================
+ *
+ *      dgap_cmdb - Sends a 2 byte command to the FEP.
+ *
+ *              ch      - Pointer to channel structure.
+ *              cmd     - Command to be sent.
+ *              byte1   - Integer containing first byte to be sent.
+ *              byte2   - Integer containing second byte to be sent.
+ *              ncmds   - Wait until ncmds or fewer cmds are left
+ *                        in the cmd buffer before returning.
+ *
+ *=======================================================================*/
+static void dgap_cmdb(struct channel_t *ch, u8 cmd, u8 byte1,
+			u8 byte2, uint ncmds)
+{
+	char __iomem *vaddr;
+	struct __iomem cm_t *cm_addr;
+	uint count;
+	uint n;
+	u16 head;
+	u16 tail;
+
+	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
+		return;
+
+	/*
+	 * Check if board is still alive.
+	 */
+	if (ch->ch_bd->state == BOARD_FAILED)
+		return;
+
+	/*
+	 * Make sure the pointers are in range before
+	 * writing to the FEP memory.
+	 */
+	vaddr = ch->ch_bd->re_map_membase;
+
+	if (!vaddr)
+		return;
+
+	cm_addr = (struct cm_t __iomem *) (vaddr + CMDBUF);
+	head = readw(&(cm_addr->cm_head));
+
+	/*
+	 * Forget it if pointers out of range.
+	 */
+	if (head >= (CMDMAX - CMDSTART) || (head & 03)) {
+		ch->ch_bd->state = BOARD_FAILED;
+		return;
 	}
 
 	/*
-	 * Initialize tty's
+	 * Put the data in the circular command buffer.
 	 */
-	if (!(un->un_flags & UN_ISOPEN)) {
-		/* Store important variables. */
-		un->un_tty     = tty;
+	writeb(cmd, (vaddr + head + CMDSTART + 0));
+	writeb((u8) ch->ch_portnum, (vaddr + head + CMDSTART + 1));
+	writeb(byte1, (vaddr + head + CMDSTART + 2));
+	writeb(byte2, (vaddr + head + CMDSTART + 3));
 
-		/* Maybe do something here to the TTY struct as well? */
+	head = (head + 4) & (CMDMAX - CMDSTART - 4);
+
+	writew(head, &(cm_addr->cm_head));
+
+	/*
+	 * Wait if necessary before updating the head
+	 * pointer to limit the number of outstanding
+	 * commands to the FEP.   If the time spent waiting
+	 * is outlandish, declare the FEP dead.
+	 */
+	for (count = dgap_count ;;) {
+
+		head = readw(&(cm_addr->cm_head));
+		tail = readw(&(cm_addr->cm_tail));
+
+		n = (head - tail) & (CMDMAX - CMDSTART - 4);
+
+		if (n <= ncmds * sizeof(struct cm_t))
+			break;
+
+		if (--count == 0) {
+			ch->ch_bd->state = BOARD_FAILED;
+			return;
+		}
+		udelay(10);
 	}
+}
+
+/*=======================================================================
+ *
+ *      dgap_cmdw - Sends a 1 word command to the FEP.
+ *
+ *              ch      - Pointer to channel structure.
+ *              cmd     - Command to be sent.
+ *              word    - Integer containing word to be sent.
+ *              ncmds   - Wait until ncmds or fewer cmds are left
+ *                        in the cmd buffer before returning.
+ *
+ *=======================================================================*/
+static void dgap_cmdw(struct channel_t *ch, u8 cmd, u16 word, uint ncmds)
+{
+	char __iomem *vaddr;
+	struct __iomem cm_t *cm_addr;
+	uint count;
+	uint n;
+	u16 head;
+	u16 tail;
+
+	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
+		return;
 
 	/*
-	 * Initialize if neither terminal or printer is open.
+	 * Check if board is still alive.
 	 */
-	if (!((ch->ch_tun.un_flags | ch->ch_pun.un_flags) & UN_ISOPEN)) {
+	if (ch->ch_bd->state == BOARD_FAILED)
+		return;
 
-		ch->ch_mforce = 0;
-		ch->ch_mval = 0;
+	/*
+	 * Make sure the pointers are in range before
+	 * writing to the FEP memory.
+	 */
+	vaddr = ch->ch_bd->re_map_membase;
+	if (!vaddr)
+		return;
+
+	cm_addr = (struct cm_t __iomem *) (vaddr + CMDBUF);
+	head = readw(&(cm_addr->cm_head));
+
+	/*
+	 * Forget it if pointers out of range.
+	 */
+	if (head >= (CMDMAX - CMDSTART) || (head & 03)) {
+		ch->ch_bd->state = BOARD_FAILED;
+		return;
+	}
+
+	/*
+	 * Put the data in the circular command buffer.
+	 */
+	writeb(cmd, (vaddr + head + CMDSTART + 0));
+	writeb((u8) ch->ch_portnum, (vaddr + head + CMDSTART + 1));
+	writew((u16) word, (vaddr + head + CMDSTART + 2));
+
+	head = (head + 4) & (CMDMAX - CMDSTART - 4);
+
+	writew(head, &(cm_addr->cm_head));
+
+	/*
+	 * Wait if necessary before updating the head
+	 * pointer to limit the number of outstanding
+	 * commands to the FEP.   If the time spent waiting
+	 * is outlandish, declare the FEP dead.
+	 */
+	for (count = dgap_count ;;) {
+
+		head = readw(&(cm_addr->cm_head));
+		tail = readw(&(cm_addr->cm_tail));
+
+		n = (head - tail) & (CMDMAX - CMDSTART - 4);
+
+		if (n <= ncmds * sizeof(struct cm_t))
+			break;
+
+		if (--count == 0) {
+			ch->ch_bd->state = BOARD_FAILED;
+			return;
+		}
+		udelay(10);
+	}
+}
+
+/*=======================================================================
+ *
+ *      dgap_cmdw_ext - Sends a extended word command to the FEP.
+ *
+ *              ch      - Pointer to channel structure.
+ *              cmd     - Command to be sent.
+ *              word    - Integer containing word to be sent.
+ *              ncmds   - Wait until ncmds or fewer cmds are left
+ *                        in the cmd buffer before returning.
+ *
+ *=======================================================================*/
+static void dgap_cmdw_ext(struct channel_t *ch, u16 cmd, u16 word, uint ncmds)
+{
+	char __iomem *vaddr;
+	struct __iomem cm_t *cm_addr;
+	uint count;
+	uint n;
+	u16 head;
+	u16 tail;
+
+	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
+		return;
+
+	/*
+	 * Check if board is still alive.
+	 */
+	if (ch->ch_bd->state == BOARD_FAILED)
+		return;
+
+	/*
+	 * Make sure the pointers are in range before
+	 * writing to the FEP memory.
+	 */
+	vaddr = ch->ch_bd->re_map_membase;
+	if (!vaddr)
+		return;
+
+	cm_addr = (struct cm_t __iomem *) (vaddr + CMDBUF);
+	head = readw(&(cm_addr->cm_head));
+
+	/*
+	 * Forget it if pointers out of range.
+	 */
+	if (head >= (CMDMAX - CMDSTART) || (head & 03)) {
+		ch->ch_bd->state = BOARD_FAILED;
+		return;
+	}
+
+	/*
+	 * Put the data in the circular command buffer.
+	 */
+
+	/* Write an FF to tell the FEP that we want an extended command */
+	writeb((u8) 0xff, (vaddr + head + CMDSTART + 0));
+
+	writeb((u8) ch->ch_portnum, (vaddr + head + CMDSTART + 1));
+	writew((u16) cmd, (vaddr + head + CMDSTART + 2));
+
+	/*
+	 * If the second part of the command won't fit,
+	 * put it at the beginning of the circular buffer.
+	 */
+	if (((head + 4) >= ((CMDMAX - CMDSTART)) || (head & 03)))
+		writew((u16) word, (vaddr + CMDSTART));
+	else
+		writew((u16) word, (vaddr + head + CMDSTART + 4));
+
+	head = (head + 8) & (CMDMAX - CMDSTART - 4);
+
+	writew(head, &(cm_addr->cm_head));
+
+	/*
+	 * Wait if necessary before updating the head
+	 * pointer to limit the number of outstanding
+	 * commands to the FEP.   If the time spent waiting
+	 * is outlandish, declare the FEP dead.
+	 */
+	for (count = dgap_count ;;) {
+
+		head = readw(&(cm_addr->cm_head));
+		tail = readw(&(cm_addr->cm_tail));
+
+		n = (head - tail) & (CMDMAX - CMDSTART - 4);
+
+		if (n <= ncmds * sizeof(struct cm_t))
+			break;
+
+		if (--count == 0) {
+			ch->ch_bd->state = BOARD_FAILED;
+			return;
+		}
+		udelay(10);
+	}
+}
+
+/*=======================================================================
+ *
+ *      dgap_wmove - Write data to FEP buffer.
+ *
+ *              ch      - Pointer to channel structure.
+ *              buf     - Poiter to characters to be moved.
+ *              cnt     - Number of characters to move.
+ *
+ *=======================================================================*/
+static void dgap_wmove(struct channel_t *ch, char *buf, uint cnt)
+{
+	int n;
+	char __iomem *taddr;
+	struct bs_t __iomem *bs;
+	u16 head;
+
+	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
+		return;
+
+	/*
+	 * Check parameters.
+	 */
+	bs   = ch->ch_bs;
+	head = readw(&(bs->tx_head));
+
+	/*
+	 * If pointers are out of range, just return.
+	 */
+	if ((cnt > ch->ch_tsize) ||
+	    (unsigned)(head - ch->ch_tstart) >= ch->ch_tsize)
+		return;
+
+	/*
+	 * If the write wraps over the top of the circular buffer,
+	 * move the portion up to the wrap point, and reset the
+	 * pointers to the bottom.
+	 */
+	n = ch->ch_tstart + ch->ch_tsize - head;
+
+	if (cnt >= n) {
+		cnt -= n;
+		taddr = ch->ch_taddr + head;
+		memcpy_toio(taddr, buf, n);
+		head = ch->ch_tstart;
+		buf += n;
+	}
+
+	/*
+	 * Move rest of data.
+	 */
+	taddr = ch->ch_taddr + head;
+	n = cnt;
+	memcpy_toio(taddr, buf, n);
+	head += cnt;
+
+	writew(head, &(bs->tx_head));
+}
+
+/*
+ * Calls the firmware to reset this channel.
+ */
+static void dgap_firmware_reset_port(struct channel_t *ch)
+{
+	dgap_cmdb(ch, CHRESET, 0, 0, 0);
+
+	/*
+	 * Now that the channel is reset, we need to make sure
+	 * all the current settings get reapplied to the port
+	 * in the firmware.
+	 *
+	 * So we will set the driver's cache of firmware
+	 * settings all to 0, and then call param.
+	 */
+	ch->ch_fepiflag = 0;
+	ch->ch_fepcflag = 0;
+	ch->ch_fepoflag = 0;
+	ch->ch_fepstartc = 0;
+	ch->ch_fepstopc = 0;
+	ch->ch_fepastartc = 0;
+	ch->ch_fepastopc = 0;
+	ch->ch_mostat = 0;
+	ch->ch_hflow = 0;
+}
+
+/*=======================================================================
+ *
+ *      dgap_param - Set Digi parameters.
+ *
+ *              struct tty_struct *     - TTY for port.
+ *
+ *=======================================================================*/
+static int dgap_param(struct channel_t *ch, struct board_t *bd, u32 un_type)
+{
+	u16 head;
+	u16 cflag;
+	u16 iflag;
+	u8 mval;
+	u8 hflow;
+
+	/*
+	 * If baud rate is zero, flush queues, and set mval to drop DTR.
+	 */
+	if ((ch->ch_c_cflag & (CBAUD)) == 0) {
+
+		/* flush rx */
+		head = readw(&(ch->ch_bs->rx_head));
+		writew(head, &(ch->ch_bs->rx_tail));
+
+		/* flush tx */
+		head = readw(&(ch->ch_bs->tx_head));
+		writew(head, &(ch->ch_bs->tx_tail));
 
+		ch->ch_flags |= (CH_BAUD0);
+
+		/* Drop RTS and DTR */
+		ch->ch_mval &= ~(D_RTS(ch)|D_DTR(ch));
+		mval = D_DTR(ch) | D_RTS(ch);
+		ch->ch_baud_info = 0;
+
+	} else if (ch->ch_custom_speed && (bd->bd_flags & BD_FEP5PLUS)) {
 		/*
-		 * Flush input queue.
+		 * Tell the fep to do the command
 		 */
-		head = readw(&(bs->rx_head));
-		writew(head, &(bs->rx_tail));
 
-		ch->ch_flags = 0;
-		ch->pscan_state = 0;
-		ch->pscan_savechar = 0;
+		dgap_cmdw_ext(ch, 0xff01, ch->ch_custom_speed, 0);
 
-		ch->ch_c_cflag   = tty->termios.c_cflag;
-		ch->ch_c_iflag   = tty->termios.c_iflag;
-		ch->ch_c_oflag   = tty->termios.c_oflag;
-		ch->ch_c_lflag   = tty->termios.c_lflag;
-		ch->ch_startc = tty->termios.c_cc[VSTART];
-		ch->ch_stopc  = tty->termios.c_cc[VSTOP];
+		/*
+		 * Now go get from fep mem, what the fep
+		 * believes the custom baud rate is.
+		 */
+		ch->ch_custom_speed = dgap_get_custom_baud(ch);
+		ch->ch_baud_info = ch->ch_custom_speed;
 
-		/* TODO: flush our TTY struct here? */
+		/* Handle transition from B0 */
+		if (ch->ch_flags & CH_BAUD0) {
+			ch->ch_flags &= ~(CH_BAUD0);
+			ch->ch_mval |= (D_RTS(ch)|D_DTR(ch));
+		}
+		mval = D_DTR(ch) | D_RTS(ch);
+
+	} else {
+		/*
+		 * Set baud rate, character size, and parity.
+		 */
+
+
+		int iindex = 0;
+		int jindex = 0;
+		int baud = 0;
+
+		ulong bauds[4][16] = {
+			{ /* slowbaud */
+				0,	50,	75,	110,
+				134,	150,	200,	300,
+				600,	1200,	1800,	2400,
+				4800,	9600,	19200,	38400 },
+			{ /* slowbaud & CBAUDEX */
+				0,	57600,	115200,	230400,
+				460800,	150,	200,	921600,
+				600,	1200,	1800,	2400,
+				4800,	9600,	19200,	38400 },
+			{ /* fastbaud */
+				0,	57600,	76800,	115200,
+				14400,	57600,	230400,	76800,
+				115200,	230400,	28800,	460800,
+				921600,	9600,	19200,	38400 },
+			{ /* fastbaud & CBAUDEX */
+				0,	57600,	115200,	230400,
+				460800,	150,	200,	921600,
+				600,	1200,	1800,	2400,
+				4800,	9600,	19200,	38400 }
+		};
+
+		/*
+		 * Only use the TXPrint baud rate if the
+		 * terminal unit is NOT open
+		 */
+		if (!(ch->ch_tun.un_flags & UN_ISOPEN) &&
+		    un_type == DGAP_PRINT)
+			baud = C_BAUD(ch->ch_pun.un_tty) & 0xff;
+		else
+			baud = C_BAUD(ch->ch_tun.un_tty) & 0xff;
+
+		if (ch->ch_c_cflag & CBAUDEX)
+			iindex = 1;
+
+		if (ch->ch_digi.digi_flags & DIGI_FAST)
+			iindex += 2;
+
+		jindex = baud;
+
+		if ((iindex >= 0) && (iindex < 4) &&
+		    (jindex >= 0) && (jindex < 16))
+			baud = bauds[iindex][jindex];
+		else
+			baud = 0;
+
+		if (baud == 0)
+			baud = 9600;
+
+		ch->ch_baud_info = baud;
+
+		/*
+		 * CBAUD has bit position 0x1000 set these days to
+		 * indicate Linux baud rate remap.
+		 * We use a different bit assignment for high speed.
+		 * Clear this bit out while grabbing the parts of
+		 * "cflag" we want.
+		 */
+		cflag = ch->ch_c_cflag & ((CBAUD ^ CBAUDEX) | PARODD | PARENB |
+						   CSTOPB | CSIZE);
+
+		/*
+		 * HUPCL bit is used by FEP to indicate fast baud
+		 * table is to be used.
+		 */
+		if ((ch->ch_digi.digi_flags & DIGI_FAST) ||
+		    (ch->ch_c_cflag & CBAUDEX))
+			cflag |= HUPCL;
+
+		if ((ch->ch_c_cflag & CBAUDEX) &&
+		    !(ch->ch_digi.digi_flags & DIGI_FAST)) {
+			/*
+			 * The below code is trying to guarantee that only
+			 * baud rates 115200, 230400, 460800, 921600 are
+			 * remapped. We use exclusive or  because the various
+			 * baud rates share common bit positions and therefore
+			 * can't be tested for easily.
+			 */
+			tcflag_t tcflag = (ch->ch_c_cflag & CBAUD) | CBAUDEX;
+			int baudpart = 0;
+
+			/*
+			 * Map high speed requests to index
+			 * into FEP's baud table
+			 */
+			switch (tcflag) {
+			case B57600:
+				baudpart = 1;
+				break;
+#ifdef B76800
+			case B76800:
+				baudpart = 2;
+				break;
+#endif
+			case B115200:
+				baudpart = 3;
+				break;
+			case B230400:
+				baudpart = 9;
+				break;
+			case B460800:
+				baudpart = 11;
+				break;
+#ifdef B921600
+			case B921600:
+				baudpart = 12;
+				break;
+#endif
+			default:
+				baudpart = 0;
+			}
+
+			if (baudpart)
+				cflag = (cflag & ~(CBAUD | CBAUDEX)) | baudpart;
+		}
+
+		cflag &= 0xffff;
+
+		if (cflag != ch->ch_fepcflag) {
+			ch->ch_fepcflag = (u16) (cflag & 0xffff);
+
+			/*
+			 * Okay to have channel and board
+			 * locks held calling this
+			 */
+			dgap_cmdw(ch, SCFLAG, (u16) cflag, 0);
+		}
+
+		/* Handle transition from B0 */
+		if (ch->ch_flags & CH_BAUD0) {
+			ch->ch_flags &= ~(CH_BAUD0);
+			ch->ch_mval |= (D_RTS(ch)|D_DTR(ch));
+		}
+		mval = D_DTR(ch) | D_RTS(ch);
 	}
 
-	dgap_carrier(ch);
 	/*
-	 * Run param in case we changed anything
+	 * Get input flags.
 	 */
-	dgap_param(ch, brd, un->un_type);
+	iflag = ch->ch_c_iflag & (IGNBRK | BRKINT | IGNPAR | PARMRK |
+				  INPCK | ISTRIP | IXON | IXANY | IXOFF);
+
+	if ((ch->ch_startc == _POSIX_VDISABLE) ||
+	    (ch->ch_stopc == _POSIX_VDISABLE)) {
+		iflag &= ~(IXON | IXOFF);
+		ch->ch_c_iflag &= ~(IXON | IXOFF);
+	}
 
 	/*
-	 * follow protocol for opening port
+	 * Only the IBM Xr card can switch between
+	 * 232 and 422 modes on the fly
+	 */
+	if (bd->device == PCI_DEV_XR_IBM_DID) {
+		if (ch->ch_digi.digi_flags & DIGI_422)
+			dgap_cmdb(ch, SCOMMODE, MODE_422, 0, 0);
+		else
+			dgap_cmdb(ch, SCOMMODE, MODE_232, 0, 0);
+	}
+
+	if (ch->ch_digi.digi_flags & DIGI_ALTPIN)
+		iflag |= IALTPIN;
+
+	if (iflag != ch->ch_fepiflag) {
+		ch->ch_fepiflag = iflag;
+
+		/* Okay to have channel and board locks held calling this */
+		dgap_cmdw(ch, SIFLAG, (u16) ch->ch_fepiflag, 0);
+	}
+
+	/*
+	 * Select hardware handshaking.
 	 */
+	hflow = 0;
 
-	spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
-	spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
+	if (ch->ch_c_cflag & CRTSCTS)
+		hflow |= (D_RTS(ch) | D_CTS(ch));
+	if (ch->ch_digi.digi_flags & RTSPACE)
+		hflow |= D_RTS(ch);
+	if (ch->ch_digi.digi_flags & DTRPACE)
+		hflow |= D_DTR(ch);
+	if (ch->ch_digi.digi_flags & CTSPACE)
+		hflow |= D_CTS(ch);
+	if (ch->ch_digi.digi_flags & DSRPACE)
+		hflow |= D_DSR(ch);
+	if (ch->ch_digi.digi_flags & DCDPACE)
+		hflow |= D_CD(ch);
 
-	rc = dgap_block_til_ready(tty, file, ch);
+	if (hflow != ch->ch_hflow) {
+		ch->ch_hflow = hflow;
 
-	if (!un->un_tty)
-		return -ENODEV;
+		/* Okay to have channel and board locks held calling this */
+		dgap_cmdb(ch, SHFLOW, (u8) hflow, 0xff, 0);
+	}
 
-	/* No going back now, increment our unit and channel counters */
-	spin_lock_irqsave(&ch->ch_lock, lock_flags);
-	ch->ch_open_count++;
-	un->un_open_count++;
-	un->un_flags |= (UN_ISOPEN);
-	spin_unlock_irqrestore(&ch->ch_lock, lock_flags);
+	/*
+	 * Set RTS and/or DTR Toggle if needed,
+	 * but only if product is FEP5+ based.
+	 */
+	if (bd->bd_flags & BD_FEP5PLUS) {
+		u16 hflow2 = 0;
 
-	return rc;
+		if (ch->ch_digi.digi_flags & DIGI_RTS_TOGGLE)
+			hflow2 |= (D_RTS(ch));
+		if (ch->ch_digi.digi_flags & DIGI_DTR_TOGGLE)
+			hflow2 |= (D_DTR(ch));
+
+		dgap_cmdw_ext(ch, 0xff03, hflow2, 0);
+	}
+
+	/*
+	 * Set modem control lines.
+	 */
+
+	mval ^= ch->ch_mforce & (mval ^ ch->ch_mval);
+
+	if (ch->ch_mostat ^ mval) {
+		ch->ch_mostat = mval;
+
+		/* Okay to have channel and board locks held calling this */
+		dgap_cmdb(ch, SMODEM, (u8) mval, D_RTS(ch)|D_DTR(ch), 0);
+	}
+
+	/*
+	 * Read modem signals, and then call carrier function.
+	 */
+	ch->ch_mistat = readb(&(ch->ch_bs->m_stat));
+	dgap_carrier(ch);
+
+	/*
+	 * Set the start and stop characters.
+	 */
+	if (ch->ch_startc != ch->ch_fepstartc ||
+	    ch->ch_stopc != ch->ch_fepstopc) {
+		ch->ch_fepstartc = ch->ch_startc;
+		ch->ch_fepstopc =  ch->ch_stopc;
+
+		/* Okay to have channel and board locks held calling this */
+		dgap_cmdb(ch, SFLOWC, ch->ch_fepstartc, ch->ch_fepstopc, 0);
+	}
+
+	/*
+	 * Set the Auxiliary start and stop characters.
+	 */
+	if (ch->ch_astartc != ch->ch_fepastartc ||
+	    ch->ch_astopc != ch->ch_fepastopc) {
+		ch->ch_fepastartc = ch->ch_astartc;
+		ch->ch_fepastopc = ch->ch_astopc;
+
+		/* Okay to have channel and board locks held calling this */
+		dgap_cmdb(ch, SAFLOWC, ch->ch_fepastartc, ch->ch_fepastopc, 0);
+	}
+
+	return 0;
 }
 
 /*
@@ -2143,15 +3193,18 @@ static int dgap_block_til_ready(struct tty_struct *tty, struct file *file,
 }
 
 /*
- * dgap_tty_hangup()
+ * dgap_tty_flush_buffer()
  *
- * Hangup the port.  Like a close, but don't wait for output to drain.
+ * Flush Tx buffer (make in == out)
  */
-static void dgap_tty_hangup(struct tty_struct *tty)
+static void dgap_tty_flush_buffer(struct tty_struct *tty)
 {
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
+	ulong lock_flags;
+	ulong lock_flags2;
+	u16 head;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -2168,21 +3221,39 @@ static void dgap_tty_hangup(struct tty_struct *tty)
 	if (!bd || bd->magic != DGAP_BOARD_MAGIC)
 		return;
 
-	/* flush the transmit queues */
-	dgap_tty_flush_buffer(tty);
+	spin_lock_irqsave(&bd->bd_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, lock_flags2);
+
+	ch->ch_flags &= ~CH_STOP;
+	head = readw(&(ch->ch_bs->tx_head));
+	dgap_cmdw(ch, FLUSHTX, (u16) head, 0);
+	dgap_cmdw(ch, RESUMETX, 0, 0);
+	if (ch->ch_tun.un_flags & (UN_LOW|UN_EMPTY)) {
+		ch->ch_tun.un_flags &= ~(UN_LOW|UN_EMPTY);
+		wake_up_interruptible(&ch->ch_tun.un_flags_wait);
+	}
+	if (ch->ch_pun.un_flags & (UN_LOW|UN_EMPTY)) {
+		ch->ch_pun.un_flags &= ~(UN_LOW|UN_EMPTY);
+		wake_up_interruptible(&ch->ch_pun.un_flags_wait);
+	}
+
+	spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
+	spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
+	if (waitqueue_active(&tty->write_wait))
+		wake_up_interruptible(&tty->write_wait);
+	tty_wakeup(tty);
 }
 
 /*
- * dgap_tty_close()
+ * dgap_tty_hangup()
  *
+ * Hangup the port.  Like a close, but don't wait for output to drain.
  */
-static void dgap_tty_close(struct tty_struct *tty, struct file *file)
+static void dgap_tty_hangup(struct tty_struct *tty)
 {
-	struct ktermios *ts;
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong lock_flags;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -2199,107 +3270,8 @@ static void dgap_tty_close(struct tty_struct *tty, struct file *file)
 	if (!bd || bd->magic != DGAP_BOARD_MAGIC)
 		return;
 
-	ts = &tty->termios;
-
-	spin_lock_irqsave(&ch->ch_lock, lock_flags);
-
-	/*
-	 * Determine if this is the last close or not - and if we agree about
-	 * which type of close it is with the Line Discipline
-	 */
-	if ((tty->count == 1) && (un->un_open_count != 1)) {
-		/*
-		 * Uh, oh.  tty->count is 1, which means that the tty
-		 * structure will be freed.  un_open_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.
-		 */
-		un->un_open_count = 1;
-	}
-
-	if (--un->un_open_count < 0)
-		un->un_open_count = 0;
-
-	ch->ch_open_count--;
-
-	if (ch->ch_open_count && un->un_open_count) {
-		spin_unlock_irqrestore(&ch->ch_lock, lock_flags);
-		return;
-	}
-
-	/* OK, its the last close on the unit */
-
-	un->un_flags |= UN_CLOSING;
-
-	tty->closing = 1;
-
-	/*
-	 * Only officially close channel if count is 0 and
-	 * DIGI_PRINTER bit is not set.
-	 */
-	if ((ch->ch_open_count == 0) &&
-	    !(ch->ch_digi.digi_flags & DIGI_PRINTER)) {
-
-		ch->ch_flags &= ~(CH_RXBLOCK);
-
-		spin_unlock_irqrestore(&ch->ch_lock, lock_flags);
-
-		/* wait for output to drain */
-		/* This will also return if we take an interrupt */
-
-		dgap_wait_for_drain(tty);
-
-		dgap_tty_flush_buffer(tty);
-		tty_ldisc_flush(tty);
-
-		spin_lock_irqsave(&ch->ch_lock, lock_flags);
-
-		tty->closing = 0;
-
-		/*
-		 * If we have HUPCL set, lower DTR and RTS
-		 */
-		if (ch->ch_c_cflag & HUPCL) {
-			ch->ch_mostat &= ~(D_RTS(ch)|D_DTR(ch));
-			dgap_cmdb(ch, SMODEM, 0, D_DTR(ch)|D_RTS(ch), 0);
-
-			/*
-			 * Go to sleep to ensure RTS/DTR
-			 * have been dropped for modems to see it.
-			 */
-			spin_unlock_irqrestore(&ch->ch_lock,
-					lock_flags);
-
-			/* .25 second delay for dropping RTS/DTR */
-			schedule_timeout_interruptible(msecs_to_jiffies(250));
-
-			spin_lock_irqsave(&ch->ch_lock, lock_flags);
-		}
-
-		ch->pscan_state = 0;
-		ch->pscan_savechar = 0;
-		ch->ch_baud_info = 0;
-
-	}
-
-	/*
-	 * turn off print device when closing print device.
-	 */
-	if ((un->un_type == DGAP_PRINT)  && (ch->ch_flags & CH_PRON)) {
-		dgap_wmove(ch, ch->ch_digi.digi_offstr,
-			(int) ch->ch_digi.digi_offlen);
-		ch->ch_flags &= ~CH_PRON;
-	}
-
-	un->un_tty = NULL;
-	un->un_flags &= ~(UN_ISOPEN | UN_CLOSING);
-	tty->driver_data = NULL;
-
-	wake_up_interruptible(&ch->ch_flags_wait);
-	wake_up_interruptible(&un->un_flags_wait);
-
-	spin_unlock_irqrestore(&ch->ch_lock, lock_flags);
+	/* flush the transmit queues */
+	dgap_tty_flush_buffer(tty);
 }
 
 /*
@@ -2599,22 +3571,6 @@ static int dgap_tty_write_room(struct tty_struct *tty)
 }
 
 /*
- * dgap_tty_put_char()
- *
- * Put a character into ch->ch_buf
- *
- *      - used by the line discipline for OPOST processing
- */
-static int dgap_tty_put_char(struct tty_struct *tty, unsigned char c)
-{
-	/*
-	 * Simply call tty_write.
-	 */
-	dgap_tty_write(tty, &c, 1);
-	return 1;
-}
-
-/*
  * dgap_tty_write()
  *
  * Take data from the user or kernel and send it out to the FEP.
@@ -2629,7 +3585,6 @@ static int dgap_tty_write(struct tty_struct *tty, const unsigned char *buf,
 	char __iomem *vaddr;
 	u16 head, tail, tmask, remain;
 	int bufcount, n;
-	int orig_count;
 	ulong lock_flags;
 
 	if (!tty)
@@ -2650,13 +3605,6 @@ static int dgap_tty_write(struct tty_struct *tty, const unsigned char *buf,
 	if (!count)
 		return 0;
 
-	/*
-	 * Store original amount of characters passed in.
-	 * This helps to figure out if we should ask the FEP
-	 * to send us an event when it has more space available.
-	 */
-	orig_count = count;
-
 	spin_lock_irqsave(&ch->ch_lock, lock_flags);
 
 	/* Get our space available for the channel from the board */
@@ -2784,6 +3732,22 @@ static int dgap_tty_write(struct tty_struct *tty, const unsigned char *buf,
 }
 
 /*
+ * dgap_tty_put_char()
+ *
+ * Put a character into ch->ch_buf
+ *
+ *      - used by the line discipline for OPOST processing
+ */
+static int dgap_tty_put_char(struct tty_struct *tty, unsigned char c)
+{
+	/*
+	 * Simply call tty_write.
+	 */
+	dgap_tty_write(tty, &c, 1);
+	return 1;
+}
+
+/*
  * Return modem signals to ld.
  */
 static int dgap_tty_tiocmget(struct tty_struct *tty)
@@ -3444,13 +4408,189 @@ static void dgap_tty_unthrottle(struct tty_struct *tty)
 	spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
 }
 
-static void dgap_tty_start(struct tty_struct *tty)
+static struct board_t *find_board_by_major(unsigned int major)
 {
-	struct board_t *bd;
+	unsigned int i;
+
+	for (i = 0; i < MAXBOARDS; i++) {
+		struct board_t *brd = dgap_board[i];
+
+		if (!brd)
+			return NULL;
+		if (major == brd->serial_driver->major ||
+		    major == brd->print_driver->major)
+			return brd;
+	}
+
+	return NULL;
+}
+
+/************************************************************************
+ *
+ * TTY Entry points and helper functions
+ *
+ ************************************************************************/
+
+/*
+ * dgap_tty_open()
+ *
+ */
+static int dgap_tty_open(struct tty_struct *tty, struct file *file)
+{
+	struct board_t *brd;
 	struct channel_t *ch;
 	struct un_t *un;
+	struct bs_t __iomem *bs;
+	uint major;
+	uint minor;
+	int rc;
 	ulong lock_flags;
 	ulong lock_flags2;
+	u16 head;
+
+	major = MAJOR(tty_devnum(tty));
+	minor = MINOR(tty_devnum(tty));
+
+	brd = find_board_by_major(major);
+	if (!brd)
+		return -EIO;
+
+	/*
+	 * If board is not yet up to a state of READY, go to
+	 * sleep waiting for it to happen or they cancel the open.
+	 */
+	rc = wait_event_interruptible(brd->state_wait,
+		(brd->state & BOARD_READY));
+
+	if (rc)
+		return rc;
+
+	spin_lock_irqsave(&brd->bd_lock, lock_flags);
+
+	/* The wait above should guarantee this cannot happen */
+	if (brd->state != BOARD_READY) {
+		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
+		return -EIO;
+	}
+
+	/* If opened device is greater than our number of ports, bail. */
+	if (MINOR(tty_devnum(tty)) > brd->nasync) {
+		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
+		return -EIO;
+	}
+
+	ch = brd->channels[minor];
+	if (!ch) {
+		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
+		return -EIO;
+	}
+
+	/* Grab channel lock */
+	spin_lock_irqsave(&ch->ch_lock, lock_flags2);
+
+	/* Figure out our type */
+	if (major == brd->serial_driver->major) {
+		un = &brd->channels[minor]->ch_tun;
+		un->un_type = DGAP_SERIAL;
+	} else if (major == brd->print_driver->major) {
+		un = &brd->channels[minor]->ch_pun;
+		un->un_type = DGAP_PRINT;
+	} else {
+		spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
+		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
+		return -EIO;
+	}
+
+	/* Store our unit into driver_data, so we always have it available. */
+	tty->driver_data = un;
+
+	/*
+	 * Error if channel info pointer is NULL.
+	 */
+	bs = ch->ch_bs;
+	if (!bs) {
+		spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
+		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
+		return -EIO;
+	}
+
+	/*
+	 * Initialize tty's
+	 */
+	if (!(un->un_flags & UN_ISOPEN)) {
+		/* Store important variables. */
+		un->un_tty     = tty;
+
+		/* Maybe do something here to the TTY struct as well? */
+	}
+
+	/*
+	 * Initialize if neither terminal or printer is open.
+	 */
+	if (!((ch->ch_tun.un_flags | ch->ch_pun.un_flags) & UN_ISOPEN)) {
+
+		ch->ch_mforce = 0;
+		ch->ch_mval = 0;
+
+		/*
+		 * Flush input queue.
+		 */
+		head = readw(&(bs->rx_head));
+		writew(head, &(bs->rx_tail));
+
+		ch->ch_flags = 0;
+		ch->pscan_state = 0;
+		ch->pscan_savechar = 0;
+
+		ch->ch_c_cflag   = tty->termios.c_cflag;
+		ch->ch_c_iflag   = tty->termios.c_iflag;
+		ch->ch_c_oflag   = tty->termios.c_oflag;
+		ch->ch_c_lflag   = tty->termios.c_lflag;
+		ch->ch_startc = tty->termios.c_cc[VSTART];
+		ch->ch_stopc  = tty->termios.c_cc[VSTOP];
+
+		/* TODO: flush our TTY struct here? */
+	}
+
+	dgap_carrier(ch);
+	/*
+	 * Run param in case we changed anything
+	 */
+	dgap_param(ch, brd, un->un_type);
+
+	/*
+	 * follow protocol for opening port
+	 */
+
+	spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
+	spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
+
+	rc = dgap_block_til_ready(tty, file, ch);
+
+	if (!un->un_tty)
+		return -ENODEV;
+
+	/* No going back now, increment our unit and channel counters */
+	spin_lock_irqsave(&ch->ch_lock, lock_flags);
+	ch->ch_open_count++;
+	un->un_open_count++;
+	un->un_flags |= (UN_ISOPEN);
+	spin_unlock_irqrestore(&ch->ch_lock, lock_flags);
+
+	return rc;
+}
+
+/*
+ * dgap_tty_close()
+ *
+ */
+static void dgap_tty_close(struct tty_struct *tty, struct file *file)
+{
+	struct ktermios *ts;
+	struct board_t *bd;
+	struct channel_t *ch;
+	struct un_t *un;
+	ulong lock_flags;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -3467,16 +4607,110 @@ static void dgap_tty_start(struct tty_struct *tty)
 	if (!bd || bd->magic != DGAP_BOARD_MAGIC)
 		return;
 
-	spin_lock_irqsave(&bd->bd_lock, lock_flags);
-	spin_lock_irqsave(&ch->ch_lock, lock_flags2);
+	ts = &tty->termios;
 
-	dgap_cmdw(ch, RESUMETX, 0, 0);
+	spin_lock_irqsave(&ch->ch_lock, lock_flags);
 
-	spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
-	spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
+	/*
+	 * Determine if this is the last close or not - and if we agree about
+	 * which type of close it is with the Line Discipline
+	 */
+	if ((tty->count == 1) && (un->un_open_count != 1)) {
+		/*
+		 * Uh, oh.  tty->count is 1, which means that the tty
+		 * structure will be freed.  un_open_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.
+		 */
+		un->un_open_count = 1;
+	}
+
+	if (--un->un_open_count < 0)
+		un->un_open_count = 0;
+
+	ch->ch_open_count--;
+
+	if (ch->ch_open_count && un->un_open_count) {
+		spin_unlock_irqrestore(&ch->ch_lock, lock_flags);
+		return;
+	}
+
+	/* OK, its the last close on the unit */
+
+	un->un_flags |= UN_CLOSING;
+
+	tty->closing = 1;
+
+	/*
+	 * Only officially close channel if count is 0 and
+	 * DIGI_PRINTER bit is not set.
+	 */
+	if ((ch->ch_open_count == 0) &&
+	    !(ch->ch_digi.digi_flags & DIGI_PRINTER)) {
+
+		ch->ch_flags &= ~(CH_RXBLOCK);
+
+		spin_unlock_irqrestore(&ch->ch_lock, lock_flags);
+
+		/* wait for output to drain */
+		/* This will also return if we take an interrupt */
+
+		dgap_wait_for_drain(tty);
+
+		dgap_tty_flush_buffer(tty);
+		tty_ldisc_flush(tty);
+
+		spin_lock_irqsave(&ch->ch_lock, lock_flags);
+
+		tty->closing = 0;
+
+		/*
+		 * If we have HUPCL set, lower DTR and RTS
+		 */
+		if (ch->ch_c_cflag & HUPCL) {
+			ch->ch_mostat &= ~(D_RTS(ch)|D_DTR(ch));
+			dgap_cmdb(ch, SMODEM, 0, D_DTR(ch)|D_RTS(ch), 0);
+
+			/*
+			 * Go to sleep to ensure RTS/DTR
+			 * have been dropped for modems to see it.
+			 */
+			spin_unlock_irqrestore(&ch->ch_lock,
+					lock_flags);
+
+			/* .25 second delay for dropping RTS/DTR */
+			schedule_timeout_interruptible(msecs_to_jiffies(250));
+
+			spin_lock_irqsave(&ch->ch_lock, lock_flags);
+		}
+
+		ch->pscan_state = 0;
+		ch->pscan_savechar = 0;
+		ch->ch_baud_info = 0;
+
+	}
+
+	/*
+	 * turn off print device when closing print device.
+	 */
+	if ((un->un_type == DGAP_PRINT)  && (ch->ch_flags & CH_PRON)) {
+		dgap_wmove(ch, ch->ch_digi.digi_offstr,
+			(int) ch->ch_digi.digi_offlen);
+		ch->ch_flags &= ~CH_PRON;
+	}
+
+	un->un_tty = NULL;
+	un->un_flags &= ~(UN_ISOPEN | UN_CLOSING);
+	tty->driver_data = NULL;
+
+	wake_up_interruptible(&ch->ch_flags_wait);
+	wake_up_interruptible(&un->un_flags_wait);
+
+	spin_unlock_irqrestore(&ch->ch_lock, lock_flags);
 }
 
-static void dgap_tty_stop(struct tty_struct *tty)
+static void dgap_tty_start(struct tty_struct *tty)
 {
 	struct board_t *bd;
 	struct channel_t *ch;
@@ -3502,26 +4736,13 @@ static void dgap_tty_stop(struct tty_struct *tty)
 	spin_lock_irqsave(&bd->bd_lock, lock_flags);
 	spin_lock_irqsave(&ch->ch_lock, lock_flags2);
 
-	dgap_cmdw(ch, PAUSETX, 0, 0);
+	dgap_cmdw(ch, RESUMETX, 0, 0);
 
 	spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
 	spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
 }
 
-/*
- * dgap_tty_flush_chars()
- *
- * Flush the cook buffer
- *
- * Note to self, and any other poor souls who venture here:
- *
- * flush in this case DOES NOT mean dispose of the data.
- * instead, it means "stop buffering and send it if you
- * haven't already."  Just guess how I figured that out...   SRW 2-Jun-98
- *
- * It is also always called in interrupt context - JAR 8-Sept-99
- */
-static void dgap_tty_flush_chars(struct tty_struct *tty)
+static void dgap_tty_stop(struct tty_struct *tty)
 {
 	struct board_t *bd;
 	struct channel_t *ch;
@@ -3547,25 +4768,32 @@ static void dgap_tty_flush_chars(struct tty_struct *tty)
 	spin_lock_irqsave(&bd->bd_lock, lock_flags);
 	spin_lock_irqsave(&ch->ch_lock, lock_flags2);
 
-	/* TODO: Do something here */
+	dgap_cmdw(ch, PAUSETX, 0, 0);
 
 	spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
 	spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
 }
 
 /*
- * dgap_tty_flush_buffer()
+ * dgap_tty_flush_chars()
  *
- * Flush Tx buffer (make in == out)
+ * Flush the cook buffer
+ *
+ * Note to self, and any other poor souls who venture here:
+ *
+ * flush in this case DOES NOT mean dispose of the data.
+ * instead, it means "stop buffering and send it if you
+ * haven't already."  Just guess how I figured that out...   SRW 2-Jun-98
+ *
+ * It is also always called in interrupt context - JAR 8-Sept-99
  */
-static void dgap_tty_flush_buffer(struct tty_struct *tty)
+static void dgap_tty_flush_chars(struct tty_struct *tty)
 {
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
 	ulong lock_flags;
 	ulong lock_flags2;
-	u16 head;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -3585,24 +4813,10 @@ static void dgap_tty_flush_buffer(struct tty_struct *tty)
 	spin_lock_irqsave(&bd->bd_lock, lock_flags);
 	spin_lock_irqsave(&ch->ch_lock, lock_flags2);
 
-	ch->ch_flags &= ~CH_STOP;
-	head = readw(&(ch->ch_bs->tx_head));
-	dgap_cmdw(ch, FLUSHTX, (u16) head, 0);
-	dgap_cmdw(ch, RESUMETX, 0, 0);
-	if (ch->ch_tun.un_flags & (UN_LOW|UN_EMPTY)) {
-		ch->ch_tun.un_flags &= ~(UN_LOW|UN_EMPTY);
-		wake_up_interruptible(&ch->ch_tun.un_flags_wait);
-	}
-	if (ch->ch_pun.un_flags & (UN_LOW|UN_EMPTY)) {
-		ch->ch_pun.un_flags &= ~(UN_LOW|UN_EMPTY);
-		wake_up_interruptible(&ch->ch_pun.un_flags_wait);
-	}
+	/* TODO: Do something here */
 
 	spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
 	spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
-	if (waitqueue_active(&tty->write_wait))
-		wake_up_interruptible(&tty->write_wait);
-	tty_wakeup(tty);
 }
 
 /*****************************************************************************
@@ -3996,1614 +5210,151 @@ static int dgap_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 	}
 }
 
-static int dgap_alloc_flipbuf(struct board_t *brd)
-{
-	/*
-	 * allocate flip buffer for board.
-	 */
-	brd->flipbuf = kmalloc(MYFLIPLEN, GFP_KERNEL);
-	if (!brd->flipbuf)
-		return -ENOMEM;
-
-	brd->flipflagbuf = kmalloc(MYFLIPLEN, GFP_KERNEL);
-	if (!brd->flipflagbuf) {
-		kfree(brd->flipbuf);
-		return -ENOMEM;
-	}
-
-	return 0;
-}
-
-static void dgap_free_flipbuf(struct board_t *brd)
-{
-	kfree(brd->flipbuf);
-	kfree(brd->flipflagbuf);
-}
-
-/*
- * Create pr and tty device entries
- */
-static int dgap_tty_register_ports(struct board_t *brd)
-{
-	struct channel_t *ch;
-	int i;
-	int ret;
-
-	brd->serial_ports = kcalloc(brd->nasync, sizeof(*brd->serial_ports),
-					GFP_KERNEL);
-	if (!brd->serial_ports)
-		return -ENOMEM;
-
-	brd->printer_ports = kcalloc(brd->nasync, sizeof(*brd->printer_ports),
-					GFP_KERNEL);
-	if (!brd->printer_ports) {
-		ret = -ENOMEM;
-		goto free_serial_ports;
-	}
-
-	for (i = 0; i < brd->nasync; i++) {
-		tty_port_init(&brd->serial_ports[i]);
-		tty_port_init(&brd->printer_ports[i]);
-	}
-
-	ch = brd->channels[0];
-	for (i = 0; i < brd->nasync; i++, ch = brd->channels[i]) {
-
-		struct device *classp;
-
-		classp = tty_port_register_device(&brd->serial_ports[i],
-						  brd->serial_driver,
-						  i, NULL);
-
-		if (IS_ERR(classp)) {
-			ret = PTR_ERR(classp);
-			goto unregister_ttys;
-		}
-
-		dgap_create_tty_sysfs(&ch->ch_tun, classp);
-		ch->ch_tun.un_sysfs = classp;
-
-		classp = tty_port_register_device(&brd->printer_ports[i],
-						  brd->print_driver,
-						  i, NULL);
-
-		if (IS_ERR(classp)) {
-			ret = PTR_ERR(classp);
-			goto unregister_ttys;
-		}
-
-		dgap_create_tty_sysfs(&ch->ch_pun, classp);
-		ch->ch_pun.un_sysfs = classp;
-	}
-	dgap_create_ports_sysfiles(brd);
-
-	return 0;
-
-unregister_ttys:
-	while (i >= 0) {
-		ch = brd->channels[i];
-		if (ch->ch_tun.un_sysfs) {
-			dgap_remove_tty_sysfs(ch->ch_tun.un_sysfs);
-			tty_unregister_device(brd->serial_driver, i);
-		}
-
-		if (ch->ch_pun.un_sysfs) {
-			dgap_remove_tty_sysfs(ch->ch_pun.un_sysfs);
-			tty_unregister_device(brd->print_driver, i);
-		}
-		i--;
-	}
-
-	for (i = 0; i < brd->nasync; i++) {
-		tty_port_destroy(&brd->serial_ports[i]);
-		tty_port_destroy(&brd->printer_ports[i]);
-	}
-
-	kfree(brd->printer_ports);
-	brd->printer_ports = NULL;
-
-free_serial_ports:
-	kfree(brd->serial_ports);
-	brd->serial_ports = NULL;
-
-	return ret;
-}
-
-/*
- * Copies the BIOS code from the user to the board,
- * and starts the BIOS running.
- */
-static void dgap_do_bios_load(struct board_t *brd, const u8 *ubios, int len)
-{
-	u8 __iomem *addr;
-	uint offset;
-	int i;
-
-	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) || !brd->re_map_membase)
-		return;
-
-	addr = brd->re_map_membase;
-
-	/*
-	 * clear POST area
-	 */
-	for (i = 0; i < 16; i++)
-		writeb(0, addr + POSTAREA + i);
-
-	/*
-	 * Download bios
-	 */
-	offset = 0x1000;
-	memcpy_toio(addr + offset, ubios, len);
-
-	writel(0x0bf00401, addr);
-	writel(0, (addr + 4));
-
-	/* Clear the reset, and change states. */
-	writeb(FEPCLR, brd->re_map_port);
-}
-
-/*
- * Checks to see if the BIOS completed running on the card.
- */
-static int dgap_test_bios(struct board_t *brd)
-{
-	u8 __iomem *addr;
-	u16 word;
-	u16 err1;
-	u16 err2;
-
-	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) || !brd->re_map_membase)
-		return -EINVAL;
-
-	addr = brd->re_map_membase;
-	word = readw(addr + POSTAREA);
-
-	/*
-	 * It can take 5-6 seconds for a board to
-	 * pass the bios self test and post results.
-	 * Give it 10 seconds.
-	 */
-	brd->wait_for_bios = 0;
-	while (brd->wait_for_bios < 1000) {
-		/* Check to see if BIOS thinks board is good. (GD). */
-		if (word == *(u16 *) "GD")
-			return 0;
-		msleep_interruptible(10);
-		brd->wait_for_bios++;
-		word = readw(addr + POSTAREA);
-	}
-
-	/* Gave up on board after too long of time taken */
-	err1 = readw(addr + SEQUENCE);
-	err2 = readw(addr + ERROR);
-	dev_warn(&brd->pdev->dev, "%s failed diagnostics.  Error #(%x,%x).\n",
-		brd->name, err1, err2);
-	brd->state = BOARD_FAILED;
-	brd->dpastatus = BD_NOBIOS;
-
-	return -EIO;
-}
-
-/*
- * Copies the FEP code from the user to the board,
- * and starts the FEP running.
- */
-static void dgap_do_fep_load(struct board_t *brd, const u8 *ufep, int len)
-{
-	u8 __iomem *addr;
-	uint offset;
-
-	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) || !brd->re_map_membase)
-		return;
-
-	addr = brd->re_map_membase;
-
-	/*
-	 * Download FEP
-	 */
-	offset = 0x1000;
-	memcpy_toio(addr + offset, ufep, len);
-
-	/*
-	 * If board is a concentrator product, we need to give
-	 * it its config string describing how the concentrators look.
-	 */
-	if ((brd->type == PCX) || (brd->type == PEPC)) {
-		u8 string[100];
-		u8 __iomem *config;
-		u8 *xconfig;
-		int i = 0;
-
-		xconfig = dgap_create_config_string(brd, string);
-
-		/* Write string to board memory */
-		config = addr + CONFIG;
-		for (; i < CONFIGSIZE; i++, config++, xconfig++) {
-			writeb(*xconfig, config);
-			if ((*xconfig & 0xff) == 0xff)
-				break;
-		}
-	}
-
-	writel(0xbfc01004, (addr + 0xc34));
-	writel(0x3, (addr + 0xc30));
-
-}
-
-/*
- * Waits for the FEP to report thats its ready for us to use.
- */
-static int dgap_test_fep(struct board_t *brd)
-{
-	u8 __iomem *addr;
-	u16 word;
-	u16 err1;
-	u16 err2;
-
-	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) || !brd->re_map_membase)
-		return -EINVAL;
-
-	addr = brd->re_map_membase;
-	word = readw(addr + FEPSTAT);
-
-	/*
-	 * It can take 2-3 seconds for the FEP to
-	 * be up and running. Give it 5 secs.
-	 */
-	brd->wait_for_fep = 0;
-	while (brd->wait_for_fep < 500) {
-		/* Check to see if FEP is up and running now. */
-		if (word == *(u16 *) "OS") {
-			/*
-			 * Check to see if the board can support FEP5+ commands.
-			*/
-			word = readw(addr + FEP5_PLUS);
-			if (word == *(u16 *) "5A")
-				brd->bd_flags |= BD_FEP5PLUS;
-
-			return 0;
-		}
-		msleep_interruptible(10);
-		brd->wait_for_fep++;
-		word = readw(addr + FEPSTAT);
-	}
-
-	/* Gave up on board after too long of time taken */
-	err1 = readw(addr + SEQUENCE);
-	err2 = readw(addr + ERROR);
-	dev_warn(&brd->pdev->dev,
-		 "FEPOS for %s not functioning.  Error #(%x,%x).\n",
-		 brd->name, err1, err2);
-	brd->state = BOARD_FAILED;
-	brd->dpastatus = BD_NOFEP;
-
-	return -EIO;
-}
-
-/*
- * Physically forces the FEP5 card to reset itself.
- */
-static void dgap_do_reset_board(struct board_t *brd)
-{
-	u8 check;
-	u32 check1;
-	u32 check2;
-	int i;
-
-	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) ||
-	    !brd->re_map_membase || !brd->re_map_port)
-		return;
-
-	/* FEPRST does not vary among supported boards */
-	writeb(FEPRST, brd->re_map_port);
-
-	for (i = 0; i <= 1000; i++) {
-		check = readb(brd->re_map_port) & 0xe;
-		if (check == FEPRST)
-			break;
-		udelay(10);
-
-	}
-	if (i > 1000) {
-		dev_warn(&brd->pdev->dev,
-			 "dgap: Board not resetting...  Failing board.\n");
-		brd->state = BOARD_FAILED;
-		brd->dpastatus = BD_NOFEP;
-		return;
-	}
-
-	/*
-	 * Make sure there really is memory out there.
-	 */
-	writel(0xa55a3cc3, (brd->re_map_membase + LOWMEM));
-	writel(0x5aa5c33c, (brd->re_map_membase + HIGHMEM));
-	check1 = readl(brd->re_map_membase + LOWMEM);
-	check2 = readl(brd->re_map_membase + HIGHMEM);
-
-	if ((check1 != 0xa55a3cc3) || (check2 != 0x5aa5c33c)) {
-		dev_warn(&brd->pdev->dev,
-			 "No memory at %p for board.\n",
-			 brd->re_map_membase);
-		brd->state = BOARD_FAILED;
-		brd->dpastatus = BD_NOFEP;
-		return;
-	}
-}
-
-#ifdef DIGI_CONCENTRATORS_SUPPORTED
-/*
- * Sends a concentrator image into the FEP5 board.
- */
-static void dgap_do_conc_load(struct board_t *brd, u8 *uaddr, int len)
-{
-	char __iomem *vaddr;
-	u16 offset;
-	struct downld_t *to_dp;
-
-	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) || !brd->re_map_membase)
-		return;
-
-	vaddr = brd->re_map_membase;
-
-	offset = readw((u16 *) (vaddr + DOWNREQ));
-	to_dp = (struct downld_t *) (vaddr + (int) offset);
-	memcpy_toio(to_dp, uaddr, len);
-
-	/* Tell card we have data for it */
-	writew(0, vaddr + (DOWNREQ));
-
-	brd->conc_dl_status = NO_PENDING_CONCENTRATOR_REQUESTS;
-}
-#endif
-
-#define EXPANSION_ROM_SIZE	(64 * 1024)
-#define FEP5_ROM_MAGIC		(0xFEFFFFFF)
-
-static void dgap_get_vpd(struct board_t *brd)
-{
-	u32 magic;
-	u32 base_offset;
-	u16 rom_offset;
-	u16 vpd_offset;
-	u16 image_length;
-	u16 i;
-	u8 byte1;
-	u8 byte2;
-
-	/*
-	 * Poke the magic number at the PCI Rom Address location.
-	 * If VPD is supported, the value read from that address
-	 * will be non-zero.
-	 */
-	magic = FEP5_ROM_MAGIC;
-	pci_write_config_dword(brd->pdev, PCI_ROM_ADDRESS, magic);
-	pci_read_config_dword(brd->pdev, PCI_ROM_ADDRESS, &magic);
-
-	/* VPD not supported, bail */
-	if (!magic)
-		return;
-
-	/*
-	 * To get to the OTPROM memory, we have to send the boards base
-	 * address or'ed with 1 into the PCI Rom Address location.
-	 */
-	magic = brd->membase | 0x01;
-	pci_write_config_dword(brd->pdev, PCI_ROM_ADDRESS, magic);
-	pci_read_config_dword(brd->pdev, PCI_ROM_ADDRESS, &magic);
-
-	byte1 = readb(brd->re_map_membase);
-	byte2 = readb(brd->re_map_membase + 1);
-
-	/*
-	 * If the board correctly swapped to the OTPROM memory,
-	 * the first 2 bytes (header) should be 0x55, 0xAA
-	 */
-	if (byte1 == 0x55 && byte2 == 0xAA) {
-
-		base_offset = 0;
-
-		/*
-		 * We have to run through all the OTPROM memory looking
-		 * for the VPD offset.
-		 */
-		while (base_offset <= EXPANSION_ROM_SIZE) {
-
-			/*
-			 * Lots of magic numbers here.
-			 *
-			 * The VPD offset is located inside the ROM Data
-			 * Structure.
-			 *
-			 * We also have to remember the length of each
-			 * ROM Data Structure, so we can "hop" to the next
-			 * entry if the VPD isn't in the current
-			 * ROM Data Structure.
-			 */
-			rom_offset = readw(brd->re_map_membase +
-						base_offset + 0x18);
-			image_length = readw(brd->re_map_membase +
-						rom_offset + 0x10) * 512;
-			vpd_offset = readw(brd->re_map_membase +
-						rom_offset + 0x08);
-
-			/* Found the VPD entry */
-			if (vpd_offset)
-				break;
-
-			/* We didn't find a VPD entry, go to next ROM entry. */
-			base_offset += image_length;
-
-			byte1 = readb(brd->re_map_membase + base_offset);
-			byte2 = readb(brd->re_map_membase + base_offset + 1);
-
-			/*
-			 * If the new ROM offset doesn't have 0x55, 0xAA
-			 * as its header, we have run out of ROM.
-			 */
-			if (byte1 != 0x55 || byte2 != 0xAA)
-				break;
-		}
-
-		/*
-		 * If we have a VPD offset, then mark the board
-		 * as having a valid VPD, and copy VPDSIZE (512) bytes of
-		 * that VPD to the buffer we have in our board structure.
-		 */
-		if (vpd_offset) {
-			brd->bd_flags |= BD_HAS_VPD;
-			for (i = 0; i < VPDSIZE; i++) {
-				brd->vpd[i] = readb(brd->re_map_membase +
-							vpd_offset + i);
-			}
-		}
-	}
-
-	/*
-	 * We MUST poke the magic number at the PCI Rom Address location again.
-	 * This makes the card report the regular board memory back to us,
-	 * rather than the OTPROM memory.
-	 */
-	magic = FEP5_ROM_MAGIC;
-	pci_write_config_dword(brd->pdev, PCI_ROM_ADDRESS, magic);
-}
-
-/*
- * Our board poller function.
- */
-static void dgap_poll_tasklet(unsigned long data)
-{
-	struct board_t *bd = (struct board_t *) data;
-	ulong lock_flags;
-	char __iomem *vaddr;
-	u16 head, tail;
-
-	if (!bd || (bd->magic != DGAP_BOARD_MAGIC))
-		return;
-
-	if (bd->inhibit_poller)
-		return;
-
-	spin_lock_irqsave(&bd->bd_lock, lock_flags);
-
-	vaddr = bd->re_map_membase;
-
-	/*
-	 * If board is ready, parse deeper to see if there is anything to do.
-	 */
-	if (bd->state == BOARD_READY) {
-
-		struct ev_t __iomem *eaddr;
-
-		if (!bd->re_map_membase) {
-			spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
-			return;
-		}
-		if (!bd->re_map_port) {
-			spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
-			return;
-		}
-
-		if (!bd->nasync)
-			goto out;
-
-		eaddr = (struct ev_t __iomem *) (vaddr + EVBUF);
-
-		/* Get our head and tail */
-		head = readw(&(eaddr->ev_head));
-		tail = readw(&(eaddr->ev_tail));
-
-		/*
-		 * If there is an event pending. Go service it.
-		 */
-		if (head != tail) {
-			spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
-			dgap_event(bd);
-			spin_lock_irqsave(&bd->bd_lock, lock_flags);
-		}
-
-out:
-		/*
-		 * If board is doing interrupts, ACK the interrupt.
-		 */
-		if (bd && bd->intr_running)
-			readb(bd->re_map_port + 2);
-
-		spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
-		return;
-	}
-
-	spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
-}
-
-/*=======================================================================
- *
- *      dgap_cmdb - Sends a 2 byte command to the FEP.
- *
- *              ch      - Pointer to channel structure.
- *              cmd     - Command to be sent.
- *              byte1   - Integer containing first byte to be sent.
- *              byte2   - Integer containing second byte to be sent.
- *              ncmds   - Wait until ncmds or fewer cmds are left
- *                        in the cmd buffer before returning.
- *
- *=======================================================================*/
-static void dgap_cmdb(struct channel_t *ch, u8 cmd, u8 byte1,
-			u8 byte2, uint ncmds)
-{
-	char __iomem *vaddr;
-	struct __iomem cm_t *cm_addr;
-	uint count;
-	uint n;
-	u16 head;
-	u16 tail;
-
-	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return;
-
-	/*
-	 * Check if board is still alive.
-	 */
-	if (ch->ch_bd->state == BOARD_FAILED)
-		return;
-
-	/*
-	 * Make sure the pointers are in range before
-	 * writing to the FEP memory.
-	 */
-	vaddr = ch->ch_bd->re_map_membase;
-
-	if (!vaddr)
-		return;
-
-	cm_addr = (struct cm_t __iomem *) (vaddr + CMDBUF);
-	head = readw(&(cm_addr->cm_head));
-
-	/*
-	 * Forget it if pointers out of range.
-	 */
-	if (head >= (CMDMAX - CMDSTART) || (head & 03)) {
-		ch->ch_bd->state = BOARD_FAILED;
-		return;
-	}
-
-	/*
-	 * Put the data in the circular command buffer.
-	 */
-	writeb(cmd, (vaddr + head + CMDSTART + 0));
-	writeb((u8) ch->ch_portnum, (vaddr + head + CMDSTART + 1));
-	writeb(byte1, (vaddr + head + CMDSTART + 2));
-	writeb(byte2, (vaddr + head + CMDSTART + 3));
-
-	head = (head + 4) & (CMDMAX - CMDSTART - 4);
-
-	writew(head, &(cm_addr->cm_head));
-
-	/*
-	 * Wait if necessary before updating the head
-	 * pointer to limit the number of outstanding
-	 * commands to the FEP.   If the time spent waiting
-	 * is outlandish, declare the FEP dead.
-	 */
-	for (count = dgap_count ;;) {
-
-		head = readw(&(cm_addr->cm_head));
-		tail = readw(&(cm_addr->cm_tail));
-
-		n = (head - tail) & (CMDMAX - CMDSTART - 4);
-
-		if (n <= ncmds * sizeof(struct cm_t))
-			break;
-
-		if (--count == 0) {
-			ch->ch_bd->state = BOARD_FAILED;
-			return;
-		}
-		udelay(10);
-	}
-}
-
-/*=======================================================================
- *
- *      dgap_cmdw - Sends a 1 word command to the FEP.
- *
- *              ch      - Pointer to channel structure.
- *              cmd     - Command to be sent.
- *              word    - Integer containing word to be sent.
- *              ncmds   - Wait until ncmds or fewer cmds are left
- *                        in the cmd buffer before returning.
- *
- *=======================================================================*/
-static void dgap_cmdw(struct channel_t *ch, u8 cmd, u16 word, uint ncmds)
-{
-	char __iomem *vaddr;
-	struct __iomem cm_t *cm_addr;
-	uint count;
-	uint n;
-	u16 head;
-	u16 tail;
-
-	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return;
-
-	/*
-	 * Check if board is still alive.
-	 */
-	if (ch->ch_bd->state == BOARD_FAILED)
-		return;
-
-	/*
-	 * Make sure the pointers are in range before
-	 * writing to the FEP memory.
-	 */
-	vaddr = ch->ch_bd->re_map_membase;
-	if (!vaddr)
-		return;
-
-	cm_addr = (struct cm_t __iomem *) (vaddr + CMDBUF);
-	head = readw(&(cm_addr->cm_head));
-
-	/*
-	 * Forget it if pointers out of range.
-	 */
-	if (head >= (CMDMAX - CMDSTART) || (head & 03)) {
-		ch->ch_bd->state = BOARD_FAILED;
-		return;
-	}
-
-	/*
-	 * Put the data in the circular command buffer.
-	 */
-	writeb(cmd, (vaddr + head + CMDSTART + 0));
-	writeb((u8) ch->ch_portnum, (vaddr + head + CMDSTART + 1));
-	writew((u16) word, (vaddr + head + CMDSTART + 2));
-
-	head = (head + 4) & (CMDMAX - CMDSTART - 4);
-
-	writew(head, &(cm_addr->cm_head));
-
-	/*
-	 * Wait if necessary before updating the head
-	 * pointer to limit the number of outstanding
-	 * commands to the FEP.   If the time spent waiting
-	 * is outlandish, declare the FEP dead.
-	 */
-	for (count = dgap_count ;;) {
-
-		head = readw(&(cm_addr->cm_head));
-		tail = readw(&(cm_addr->cm_tail));
-
-		n = (head - tail) & (CMDMAX - CMDSTART - 4);
-
-		if (n <= ncmds * sizeof(struct cm_t))
-			break;
-
-		if (--count == 0) {
-			ch->ch_bd->state = BOARD_FAILED;
-			return;
-		}
-		udelay(10);
-	}
-}
-
-/*=======================================================================
- *
- *      dgap_cmdw_ext - Sends a extended word command to the FEP.
- *
- *              ch      - Pointer to channel structure.
- *              cmd     - Command to be sent.
- *              word    - Integer containing word to be sent.
- *              ncmds   - Wait until ncmds or fewer cmds are left
- *                        in the cmd buffer before returning.
- *
- *=======================================================================*/
-static void dgap_cmdw_ext(struct channel_t *ch, u16 cmd, u16 word, uint ncmds)
-{
-	char __iomem *vaddr;
-	struct __iomem cm_t *cm_addr;
-	uint count;
-	uint n;
-	u16 head;
-	u16 tail;
-
-	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return;
-
-	/*
-	 * Check if board is still alive.
-	 */
-	if (ch->ch_bd->state == BOARD_FAILED)
-		return;
-
-	/*
-	 * Make sure the pointers are in range before
-	 * writing to the FEP memory.
-	 */
-	vaddr = ch->ch_bd->re_map_membase;
-	if (!vaddr)
-		return;
-
-	cm_addr = (struct cm_t __iomem *) (vaddr + CMDBUF);
-	head = readw(&(cm_addr->cm_head));
-
-	/*
-	 * Forget it if pointers out of range.
-	 */
-	if (head >= (CMDMAX - CMDSTART) || (head & 03)) {
-		ch->ch_bd->state = BOARD_FAILED;
-		return;
-	}
-
-	/*
-	 * Put the data in the circular command buffer.
-	 */
-
-	/* Write an FF to tell the FEP that we want an extended command */
-	writeb((u8) 0xff, (vaddr + head + CMDSTART + 0));
-
-	writeb((u8) ch->ch_portnum, (vaddr + head + CMDSTART + 1));
-	writew((u16) cmd, (vaddr + head + CMDSTART + 2));
-
-	/*
-	 * If the second part of the command won't fit,
-	 * put it at the beginning of the circular buffer.
-	 */
-	if (((head + 4) >= ((CMDMAX - CMDSTART)) || (head & 03)))
-		writew((u16) word, (vaddr + CMDSTART));
-	else
-		writew((u16) word, (vaddr + head + CMDSTART + 4));
-
-	head = (head + 8) & (CMDMAX - CMDSTART - 4);
-
-	writew(head, &(cm_addr->cm_head));
-
-	/*
-	 * Wait if necessary before updating the head
-	 * pointer to limit the number of outstanding
-	 * commands to the FEP.   If the time spent waiting
-	 * is outlandish, declare the FEP dead.
-	 */
-	for (count = dgap_count ;;) {
-
-		head = readw(&(cm_addr->cm_head));
-		tail = readw(&(cm_addr->cm_tail));
-
-		n = (head - tail) & (CMDMAX - CMDSTART - 4);
-
-		if (n <= ncmds * sizeof(struct cm_t))
-			break;
-
-		if (--count == 0) {
-			ch->ch_bd->state = BOARD_FAILED;
-			return;
-		}
-		udelay(10);
-	}
-}
+static const struct tty_operations dgap_tty_ops = {
+	.open = dgap_tty_open,
+	.close = dgap_tty_close,
+	.write = dgap_tty_write,
+	.write_room = dgap_tty_write_room,
+	.flush_buffer = dgap_tty_flush_buffer,
+	.chars_in_buffer = dgap_tty_chars_in_buffer,
+	.flush_chars = dgap_tty_flush_chars,
+	.ioctl = dgap_tty_ioctl,
+	.set_termios = dgap_tty_set_termios,
+	.stop = dgap_tty_stop,
+	.start = dgap_tty_start,
+	.throttle = dgap_tty_throttle,
+	.unthrottle = dgap_tty_unthrottle,
+	.hangup = dgap_tty_hangup,
+	.put_char = dgap_tty_put_char,
+	.tiocmget = dgap_tty_tiocmget,
+	.tiocmset = dgap_tty_tiocmset,
+	.break_ctl = dgap_tty_send_break,
+	.wait_until_sent = dgap_tty_wait_until_sent,
+	.send_xchar = dgap_tty_send_xchar
+};
 
-/*=======================================================================
- *
- *      dgap_wmove - Write data to FEP buffer.
+/************************************************************************
  *
- *              ch      - Pointer to channel structure.
- *              buf     - Poiter to characters to be moved.
- *              cnt     - Number of characters to move.
+ * TTY Initialization/Cleanup Functions
  *
- *=======================================================================*/
-static void dgap_wmove(struct channel_t *ch, char *buf, uint cnt)
-{
-	int n;
-	char __iomem *taddr;
-	struct bs_t __iomem *bs;
-	u16 head;
-
-	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return;
-
-	/*
-	 * Check parameters.
-	 */
-	bs   = ch->ch_bs;
-	head = readw(&(bs->tx_head));
-
-	/*
-	 * If pointers are out of range, just return.
-	 */
-	if ((cnt > ch->ch_tsize) ||
-	    (unsigned)(head - ch->ch_tstart) >= ch->ch_tsize)
-		return;
-
-	/*
-	 * If the write wraps over the top of the circular buffer,
-	 * move the portion up to the wrap point, and reset the
-	 * pointers to the bottom.
-	 */
-	n = ch->ch_tstart + ch->ch_tsize - head;
-
-	if (cnt >= n) {
-		cnt -= n;
-		taddr = ch->ch_taddr + head;
-		memcpy_toio(taddr, buf, n);
-		head = ch->ch_tstart;
-		buf += n;
-	}
-
-	/*
-	 * Move rest of data.
-	 */
-	taddr = ch->ch_taddr + head;
-	n = cnt;
-	memcpy_toio(taddr, buf, n);
-	head += cnt;
-
-	writew(head, &(bs->tx_head));
-}
-
-/*
- * Retrives the current custom baud rate from FEP memory,
- * and returns it back to the user.
- * Returns 0 on error.
- */
-static uint dgap_get_custom_baud(struct channel_t *ch)
-{
-	u8 __iomem *vaddr;
-	ulong offset;
-	uint value;
-
-	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return 0;
-
-	if (!ch->ch_bd || ch->ch_bd->magic != DGAP_BOARD_MAGIC)
-		return 0;
-
-	if (!(ch->ch_bd->bd_flags & BD_FEP5PLUS))
-		return 0;
-
-	vaddr = ch->ch_bd->re_map_membase;
-
-	if (!vaddr)
-		return 0;
-
-	/*
-	 * Go get from fep mem, what the fep
-	 * believes the custom baud rate is.
-	 */
-	offset = (ioread16(vaddr + ECS_SEG) << 4) + (ch->ch_portnum * 0x28)
-	       + LINE_SPEED;
-
-	value = readw(vaddr + offset);
-	return value;
-}
+ ************************************************************************/
 
 /*
- * Calls the firmware to reset this channel.
- */
-static void dgap_firmware_reset_port(struct channel_t *ch)
-{
-	dgap_cmdb(ch, CHRESET, 0, 0, 0);
-
-	/*
-	 * Now that the channel is reset, we need to make sure
-	 * all the current settings get reapplied to the port
-	 * in the firmware.
-	 *
-	 * So we will set the driver's cache of firmware
-	 * settings all to 0, and then call param.
-	 */
-	ch->ch_fepiflag = 0;
-	ch->ch_fepcflag = 0;
-	ch->ch_fepoflag = 0;
-	ch->ch_fepstartc = 0;
-	ch->ch_fepstopc = 0;
-	ch->ch_fepastartc = 0;
-	ch->ch_fepastopc = 0;
-	ch->ch_mostat = 0;
-	ch->ch_hflow = 0;
-}
-
-/*=======================================================================
- *
- *      dgap_param - Set Digi parameters.
- *
- *              struct tty_struct *     - TTY for port.
+ * dgap_tty_register()
  *
- *=======================================================================*/
-static int dgap_param(struct channel_t *ch, struct board_t *bd, u32 un_type)
+ * Init the tty subsystem for this board.
+ */
+static int dgap_tty_register(struct board_t *brd)
 {
-	u16 head;
-	u16 cflag;
-	u16 iflag;
-	u8 mval;
-	u8 hflow;
-
-	/*
-	 * If baud rate is zero, flush queues, and set mval to drop DTR.
-	 */
-	if ((ch->ch_c_cflag & (CBAUD)) == 0) {
-
-		/* flush rx */
-		head = readw(&(ch->ch_bs->rx_head));
-		writew(head, &(ch->ch_bs->rx_tail));
-
-		/* flush tx */
-		head = readw(&(ch->ch_bs->tx_head));
-		writew(head, &(ch->ch_bs->tx_tail));
-
-		ch->ch_flags |= (CH_BAUD0);
-
-		/* Drop RTS and DTR */
-		ch->ch_mval &= ~(D_RTS(ch)|D_DTR(ch));
-		mval = D_DTR(ch) | D_RTS(ch);
-		ch->ch_baud_info = 0;
-
-	} else if (ch->ch_custom_speed && (bd->bd_flags & BD_FEP5PLUS)) {
-		/*
-		 * Tell the fep to do the command
-		 */
-
-		dgap_cmdw_ext(ch, 0xff01, ch->ch_custom_speed, 0);
-
-		/*
-		 * Now go get from fep mem, what the fep
-		 * believes the custom baud rate is.
-		 */
-		ch->ch_custom_speed = dgap_get_custom_baud(ch);
-		ch->ch_baud_info = ch->ch_custom_speed;
-
-		/* Handle transition from B0 */
-		if (ch->ch_flags & CH_BAUD0) {
-			ch->ch_flags &= ~(CH_BAUD0);
-			ch->ch_mval |= (D_RTS(ch)|D_DTR(ch));
-		}
-		mval = D_DTR(ch) | D_RTS(ch);
-
-	} else {
-		/*
-		 * Set baud rate, character size, and parity.
-		 */
-
-
-		int iindex = 0;
-		int jindex = 0;
-		int baud = 0;
-
-		ulong bauds[4][16] = {
-			{ /* slowbaud */
-				0,	50,	75,	110,
-				134,	150,	200,	300,
-				600,	1200,	1800,	2400,
-				4800,	9600,	19200,	38400 },
-			{ /* slowbaud & CBAUDEX */
-				0,	57600,	115200,	230400,
-				460800,	150,	200,	921600,
-				600,	1200,	1800,	2400,
-				4800,	9600,	19200,	38400 },
-			{ /* fastbaud */
-				0,	57600,	76800,	115200,
-				14400,	57600,	230400,	76800,
-				115200,	230400,	28800,	460800,
-				921600,	9600,	19200,	38400 },
-			{ /* fastbaud & CBAUDEX */
-				0,	57600,	115200,	230400,
-				460800,	150,	200,	921600,
-				600,	1200,	1800,	2400,
-				4800,	9600,	19200,	38400 }
-		};
-
-		/*
-		 * Only use the TXPrint baud rate if the
-		 * terminal unit is NOT open
-		 */
-		if (!(ch->ch_tun.un_flags & UN_ISOPEN) &&
-		    un_type == DGAP_PRINT)
-			baud = C_BAUD(ch->ch_pun.un_tty) & 0xff;
-		else
-			baud = C_BAUD(ch->ch_tun.un_tty) & 0xff;
-
-		if (ch->ch_c_cflag & CBAUDEX)
-			iindex = 1;
-
-		if (ch->ch_digi.digi_flags & DIGI_FAST)
-			iindex += 2;
-
-		jindex = baud;
-
-		if ((iindex >= 0) && (iindex < 4) &&
-		    (jindex >= 0) && (jindex < 16))
-			baud = bauds[iindex][jindex];
-		else
-			baud = 0;
-
-		if (baud == 0)
-			baud = 9600;
-
-		ch->ch_baud_info = baud;
-
-		/*
-		 * CBAUD has bit position 0x1000 set these days to
-		 * indicate Linux baud rate remap.
-		 * We use a different bit assignment for high speed.
-		 * Clear this bit out while grabbing the parts of
-		 * "cflag" we want.
-		 */
-		cflag = ch->ch_c_cflag & ((CBAUD ^ CBAUDEX) | PARODD | PARENB |
-						   CSTOPB | CSIZE);
-
-		/*
-		 * HUPCL bit is used by FEP to indicate fast baud
-		 * table is to be used.
-		 */
-		if ((ch->ch_digi.digi_flags & DIGI_FAST) ||
-		    (ch->ch_c_cflag & CBAUDEX))
-			cflag |= HUPCL;
-
-		if ((ch->ch_c_cflag & CBAUDEX) &&
-		    !(ch->ch_digi.digi_flags & DIGI_FAST)) {
-			/*
-			 * The below code is trying to guarantee that only
-			 * baud rates 115200, 230400, 460800, 921600 are
-			 * remapped. We use exclusive or  because the various
-			 * baud rates share common bit positions and therefore
-			 * can't be tested for easily.
-			 */
-			tcflag_t tcflag = (ch->ch_c_cflag & CBAUD) | CBAUDEX;
-			int baudpart = 0;
-
-			/*
-			 * Map high speed requests to index
-			 * into FEP's baud table
-			 */
-			switch (tcflag) {
-			case B57600:
-				baudpart = 1;
-				break;
-#ifdef B76800
-			case B76800:
-				baudpart = 2;
-				break;
-#endif
-			case B115200:
-				baudpart = 3;
-				break;
-			case B230400:
-				baudpart = 9;
-				break;
-			case B460800:
-				baudpart = 11;
-				break;
-#ifdef B921600
-			case B921600:
-				baudpart = 12;
-				break;
-#endif
-			default:
-				baudpart = 0;
-			}
-
-			if (baudpart)
-				cflag = (cflag & ~(CBAUD | CBAUDEX)) | baudpart;
-		}
-
-		cflag &= 0xffff;
-
-		if (cflag != ch->ch_fepcflag) {
-			ch->ch_fepcflag = (u16) (cflag & 0xffff);
-
-			/*
-			 * Okay to have channel and board
-			 * locks held calling this
-			 */
-			dgap_cmdw(ch, SCFLAG, (u16) cflag, 0);
-		}
-
-		/* Handle transition from B0 */
-		if (ch->ch_flags & CH_BAUD0) {
-			ch->ch_flags &= ~(CH_BAUD0);
-			ch->ch_mval |= (D_RTS(ch)|D_DTR(ch));
-		}
-		mval = D_DTR(ch) | D_RTS(ch);
-	}
-
-	/*
-	 * Get input flags.
-	 */
-	iflag = ch->ch_c_iflag & (IGNBRK | BRKINT | IGNPAR | PARMRK |
-				  INPCK | ISTRIP | IXON | IXANY | IXOFF);
-
-	if ((ch->ch_startc == _POSIX_VDISABLE) ||
-	    (ch->ch_stopc == _POSIX_VDISABLE)) {
-		iflag &= ~(IXON | IXOFF);
-		ch->ch_c_iflag &= ~(IXON | IXOFF);
-	}
-
-	/*
-	 * Only the IBM Xr card can switch between
-	 * 232 and 422 modes on the fly
-	 */
-	if (bd->device == PCI_DEV_XR_IBM_DID) {
-		if (ch->ch_digi.digi_flags & DIGI_422)
-			dgap_cmdb(ch, SCOMMODE, MODE_422, 0, 0);
-		else
-			dgap_cmdb(ch, SCOMMODE, MODE_232, 0, 0);
-	}
-
-	if (ch->ch_digi.digi_flags & DIGI_ALTPIN)
-		iflag |= IALTPIN;
-
-	if (iflag != ch->ch_fepiflag) {
-		ch->ch_fepiflag = iflag;
-
-		/* Okay to have channel and board locks held calling this */
-		dgap_cmdw(ch, SIFLAG, (u16) ch->ch_fepiflag, 0);
-	}
-
-	/*
-	 * Select hardware handshaking.
-	 */
-	hflow = 0;
-
-	if (ch->ch_c_cflag & CRTSCTS)
-		hflow |= (D_RTS(ch) | D_CTS(ch));
-	if (ch->ch_digi.digi_flags & RTSPACE)
-		hflow |= D_RTS(ch);
-	if (ch->ch_digi.digi_flags & DTRPACE)
-		hflow |= D_DTR(ch);
-	if (ch->ch_digi.digi_flags & CTSPACE)
-		hflow |= D_CTS(ch);
-	if (ch->ch_digi.digi_flags & DSRPACE)
-		hflow |= D_DSR(ch);
-	if (ch->ch_digi.digi_flags & DCDPACE)
-		hflow |= D_CD(ch);
+	int rc;
 
-	if (hflow != ch->ch_hflow) {
-		ch->ch_hflow = hflow;
+	brd->serial_driver = tty_alloc_driver(MAXPORTS,
+					      TTY_DRIVER_REAL_RAW |
+					      TTY_DRIVER_DYNAMIC_DEV |
+					      TTY_DRIVER_HARDWARE_BREAK);
+	if (IS_ERR(brd->serial_driver))
+		return PTR_ERR(brd->serial_driver);
 
-		/* Okay to have channel and board locks held calling this */
-		dgap_cmdb(ch, SHFLOW, (u8) hflow, 0xff, 0);
-	}
+	snprintf(brd->serial_name, MAXTTYNAMELEN, "tty_dgap_%d_",
+		 brd->boardnum);
+	brd->serial_driver->name = brd->serial_name;
+	brd->serial_driver->name_base = 0;
+	brd->serial_driver->major = 0;
+	brd->serial_driver->minor_start = 0;
+	brd->serial_driver->type = TTY_DRIVER_TYPE_SERIAL;
+	brd->serial_driver->subtype = SERIAL_TYPE_NORMAL;
+	brd->serial_driver->init_termios = dgap_default_termios;
+	brd->serial_driver->driver_name = DRVSTR;
 
 	/*
-	 * Set RTS and/or DTR Toggle if needed,
-	 * but only if product is FEP5+ based.
+	 * Entry points for driver.  Called by the kernel from
+	 * tty_io.c and n_tty.c.
 	 */
-	if (bd->bd_flags & BD_FEP5PLUS) {
-		u16 hflow2 = 0;
-
-		if (ch->ch_digi.digi_flags & DIGI_RTS_TOGGLE)
-			hflow2 |= (D_RTS(ch));
-		if (ch->ch_digi.digi_flags & DIGI_DTR_TOGGLE)
-			hflow2 |= (D_DTR(ch));
-
-		dgap_cmdw_ext(ch, 0xff03, hflow2, 0);
-	}
+	tty_set_operations(brd->serial_driver, &dgap_tty_ops);
 
 	/*
-	 * Set modem control lines.
+	 * If we're doing transparent print, we have to do all of the above
+	 * again, separately so we don't get the LD confused about what major
+	 * we are when we get into the dgap_tty_open() routine.
 	 */
-
-	mval ^= ch->ch_mforce & (mval ^ ch->ch_mval);
-
-	if (ch->ch_mostat ^ mval) {
-		ch->ch_mostat = mval;
-
-		/* Okay to have channel and board locks held calling this */
-		dgap_cmdb(ch, SMODEM, (u8) mval, D_RTS(ch)|D_DTR(ch), 0);
+	brd->print_driver = tty_alloc_driver(MAXPORTS,
+					     TTY_DRIVER_REAL_RAW |
+					     TTY_DRIVER_DYNAMIC_DEV |
+					     TTY_DRIVER_HARDWARE_BREAK);
+	if (IS_ERR(brd->print_driver)) {
+		rc = PTR_ERR(brd->print_driver);
+		goto free_serial_drv;
 	}
 
-	/*
-	 * Read modem signals, and then call carrier function.
-	 */
-	ch->ch_mistat = readb(&(ch->ch_bs->m_stat));
-	dgap_carrier(ch);
+	snprintf(brd->print_name, MAXTTYNAMELEN, "pr_dgap_%d_",
+		 brd->boardnum);
+	brd->print_driver->name = brd->print_name;
+	brd->print_driver->name_base = 0;
+	brd->print_driver->major = 0;
+	brd->print_driver->minor_start = 0;
+	brd->print_driver->type = TTY_DRIVER_TYPE_SERIAL;
+	brd->print_driver->subtype = SERIAL_TYPE_NORMAL;
+	brd->print_driver->init_termios = dgap_default_termios;
+	brd->print_driver->driver_name = DRVSTR;
 
 	/*
-	 * Set the start and stop characters.
+	 * Entry points for driver.  Called by the kernel from
+	 * tty_io.c and n_tty.c.
 	 */
-	if (ch->ch_startc != ch->ch_fepstartc ||
-	    ch->ch_stopc != ch->ch_fepstopc) {
-		ch->ch_fepstartc = ch->ch_startc;
-		ch->ch_fepstopc =  ch->ch_stopc;
-
-		/* Okay to have channel and board locks held calling this */
-		dgap_cmdb(ch, SFLOWC, ch->ch_fepstartc, ch->ch_fepstopc, 0);
-	}
+	tty_set_operations(brd->print_driver, &dgap_tty_ops);
 
-	/*
-	 * Set the Auxiliary start and stop characters.
-	 */
-	if (ch->ch_astartc != ch->ch_fepastartc ||
-	    ch->ch_astopc != ch->ch_fepastopc) {
-		ch->ch_fepastartc = ch->ch_astartc;
-		ch->ch_fepastopc = ch->ch_astopc;
+	/* Register tty devices */
+	rc = tty_register_driver(brd->serial_driver);
+	if (rc < 0)
+		goto free_print_drv;
 
-		/* Okay to have channel and board locks held calling this */
-		dgap_cmdb(ch, SAFLOWC, ch->ch_fepastartc, ch->ch_fepastopc, 0);
-	}
+	/* Register Transparent Print devices */
+	rc = tty_register_driver(brd->print_driver);
+	if (rc < 0)
+		goto unregister_serial_drv;
 
 	return 0;
-}
-
-/*
- * dgap_parity_scan()
- *
- * Convert the FEP5 way of reporting parity errors and breaks into
- * the Linux line discipline way.
- */
-static void dgap_parity_scan(struct channel_t *ch, unsigned char *cbuf,
-				unsigned char *fbuf, int *len)
-{
-	int l = *len;
-	int count = 0;
-	unsigned char *in, *cout, *fout;
-	unsigned char c;
-
-	in = cbuf;
-	cout = cbuf;
-	fout = fbuf;
-
-	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return;
 
-	while (l--) {
-		c = *in++;
-		switch (ch->pscan_state) {
-		default:
-			/* reset to sanity and fall through */
-			ch->pscan_state = 0;
-
-		case 0:
-			/* No FF seen yet */
-			if (c == (unsigned char) '\377')
-				/* delete this character from stream */
-				ch->pscan_state = 1;
-			else {
-				*cout++ = c;
-				*fout++ = TTY_NORMAL;
-				count += 1;
-			}
-			break;
-
-		case 1:
-			/* first FF seen */
-			if (c == (unsigned char) '\377') {
-				/* doubled ff, transform to single ff */
-				*cout++ = c;
-				*fout++ = TTY_NORMAL;
-				count += 1;
-				ch->pscan_state = 0;
-			} else {
-				/* save value examination in next state */
-				ch->pscan_savechar = c;
-				ch->pscan_state = 2;
-			}
-			break;
-
-		case 2:
-			/* third character of ff sequence */
-
-			*cout++ = c;
-
-			if (ch->pscan_savechar == 0x0) {
-
-				if (c == 0x0) {
-					ch->ch_err_break++;
-					*fout++ = TTY_BREAK;
-				} else {
-					ch->ch_err_parity++;
-					*fout++ = TTY_PARITY;
-				}
-			}
+unregister_serial_drv:
+	tty_unregister_driver(brd->serial_driver);
+free_print_drv:
+	put_tty_driver(brd->print_driver);
+free_serial_drv:
+	put_tty_driver(brd->serial_driver);
 
-			count += 1;
-			ch->pscan_state = 0;
-		}
-	}
-	*len = count;
+	return rc;
 }
 
-static void dgap_write_wakeup(struct board_t *bd, struct channel_t *ch,
-			      struct un_t *un, u32 mask,
-			      unsigned long *irq_flags1,
-			      unsigned long *irq_flags2)
+static void dgap_tty_unregister(struct board_t *brd)
 {
-	if (!(un->un_flags & mask))
-		return;
-
-	un->un_flags &= ~mask;
-
-	if (!(un->un_flags & UN_ISOPEN))
-		return;
-
-	if ((un->un_tty->flags & (1 << TTY_DO_WRITE_WAKEUP)) &&
-	    un->un_tty->ldisc->ops->write_wakeup) {
-		spin_unlock_irqrestore(&ch->ch_lock, *irq_flags2);
-		spin_unlock_irqrestore(&bd->bd_lock, *irq_flags1);
-
-		(un->un_tty->ldisc->ops->write_wakeup)(un->un_tty);
-
-		spin_lock_irqsave(&bd->bd_lock, *irq_flags1);
-		spin_lock_irqsave(&ch->ch_lock, *irq_flags2);
-	}
-	wake_up_interruptible(&un->un_tty->write_wait);
-	wake_up_interruptible(&un->un_flags_wait);
+	tty_unregister_driver(brd->print_driver);
+	tty_unregister_driver(brd->serial_driver);
+	put_tty_driver(brd->print_driver);
+	put_tty_driver(brd->serial_driver);
 }
 
-/*=======================================================================
- *
- *      dgap_event - FEP to host event processing routine.
- *
- *              bd     - Board of current event.
- *
- *=======================================================================*/
-static int dgap_event(struct board_t *bd)
+static int dgap_alloc_flipbuf(struct board_t *brd)
 {
-	struct channel_t *ch;
-	ulong lock_flags;
-	ulong lock_flags2;
-	struct bs_t __iomem *bs;
-	u8 __iomem *event;
-	u8 __iomem *vaddr;
-	struct ev_t __iomem *eaddr;
-	uint head;
-	uint tail;
-	int port;
-	int reason;
-	int modem;
-	int b1;
-
-	if (!bd || bd->magic != DGAP_BOARD_MAGIC)
-		return -EIO;
-
-	spin_lock_irqsave(&bd->bd_lock, lock_flags);
-
-	vaddr = bd->re_map_membase;
-
-	if (!vaddr) {
-		spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
-		return -EIO;
-	}
-
-	eaddr = (struct ev_t __iomem *) (vaddr + EVBUF);
-
-	/* Get our head and tail */
-	head = readw(&(eaddr->ev_head));
-	tail = readw(&(eaddr->ev_tail));
-
-	/*
-	 * Forget it if pointers out of range.
-	 */
-
-	if (head >= EVMAX - EVSTART || tail >= EVMAX - EVSTART ||
-	    (head | tail) & 03) {
-		/* Let go of board lock */
-		spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
-		return -EIO;
-	}
-
 	/*
-	 * Loop to process all the events in the buffer.
+	 * allocate flip buffer for board.
 	 */
-	while (tail != head) {
-
-		/*
-		 * Get interrupt information.
-		 */
-
-		event = bd->re_map_membase + tail + EVSTART;
-
-		port   = ioread8(event);
-		reason = ioread8(event + 1);
-		modem  = ioread8(event + 2);
-		b1     = ioread8(event + 3);
-
-		/*
-		 * Make sure the interrupt is valid.
-		 */
-		if (port >= bd->nasync)
-			goto next;
-
-		if (!(reason & (IFMODEM | IFBREAK | IFTLW | IFTEM | IFDATA)))
-			goto next;
-
-		ch = bd->channels[port];
-
-		if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-			goto next;
-
-		/*
-		 * If we have made it here, the event was valid.
-		 * Lock down the channel.
-		 */
-		spin_lock_irqsave(&ch->ch_lock, lock_flags2);
-
-		bs = ch->ch_bs;
-
-		if (!bs) {
-			spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
-			goto next;
-		}
-
-		/*
-		 * Process received data.
-		 */
-		if (reason & IFDATA) {
-
-			/*
-			 * ALL LOCKS *MUST* BE DROPPED BEFORE CALLING INPUT!
-			 * input could send some data to ld, which in turn
-			 * could do a callback to one of our other functions.
-			 */
-			spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
-			spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
-
-			dgap_input(ch);
-
-			spin_lock_irqsave(&bd->bd_lock, lock_flags);
-			spin_lock_irqsave(&ch->ch_lock, lock_flags2);
-
-			if (ch->ch_flags & CH_RACTIVE)
-				ch->ch_flags |= CH_RENABLE;
-			else
-				writeb(1, &(bs->idata));
-
-			if (ch->ch_flags & CH_RWAIT) {
-				ch->ch_flags &= ~CH_RWAIT;
-
-				wake_up_interruptible
-					(&ch->ch_tun.un_flags_wait);
-			}
-		}
-
-		/*
-		 * Process Modem change signals.
-		 */
-		if (reason & IFMODEM) {
-			ch->ch_mistat = modem;
-			dgap_carrier(ch);
-		}
-
-		/*
-		 * Process break.
-		 */
-		if (reason & IFBREAK) {
-
-			if (ch->ch_tun.un_tty) {
-				/* A break has been indicated */
-				ch->ch_err_break++;
-				tty_buffer_request_room
-					(ch->ch_tun.un_tty->port, 1);
-				tty_insert_flip_char(ch->ch_tun.un_tty->port,
-						     0, TTY_BREAK);
-				tty_flip_buffer_push(ch->ch_tun.un_tty->port);
-			}
-		}
-
-		/*
-		 * Process Transmit low.
-		 */
-		if (reason & IFTLW) {
-			dgap_write_wakeup(bd, ch, &ch->ch_tun, UN_LOW,
-					  &lock_flags, &lock_flags2);
-			dgap_write_wakeup(bd, ch, &ch->ch_pun, UN_LOW,
-					  &lock_flags, &lock_flags2);
-			if (ch->ch_flags & CH_WLOW) {
-				ch->ch_flags &= ~CH_WLOW;
-				wake_up_interruptible(&ch->ch_flags_wait);
-			}
-		}
-
-		/*
-		 * Process Transmit empty.
-		 */
-		if (reason & IFTEM) {
-			dgap_write_wakeup(bd, ch, &ch->ch_tun, UN_EMPTY,
-					  &lock_flags, &lock_flags2);
-			dgap_write_wakeup(bd, ch, &ch->ch_pun, UN_EMPTY,
-					  &lock_flags, &lock_flags2);
-			if (ch->ch_flags & CH_WEMPTY) {
-				ch->ch_flags &= ~CH_WEMPTY;
-				wake_up_interruptible(&ch->ch_flags_wait);
-			}
-		}
-
-		spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
+	brd->flipbuf = kmalloc(MYFLIPLEN, GFP_KERNEL);
+	if (!brd->flipbuf)
+		return -ENOMEM;
 
-next:
-		tail = (tail + 4) & (EVMAX - EVSTART - 4);
+	brd->flipflagbuf = kmalloc(MYFLIPLEN, GFP_KERNEL);
+	if (!brd->flipflagbuf) {
+		kfree(brd->flipbuf);
+		return -ENOMEM;
 	}
 
-	writew(tail, &(eaddr->ev_tail));
-	spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
-
 	return 0;
 }
 
-static ssize_t dgap_driver_version_show(struct device_driver *ddp, char *buf)
-{
-	return snprintf(buf, PAGE_SIZE, "%s\n", DG_PART);
-}
-static DRIVER_ATTR(version, S_IRUSR, dgap_driver_version_show, NULL);
-
-
-static ssize_t dgap_driver_boards_show(struct device_driver *ddp, char *buf)
-{
-	return snprintf(buf, PAGE_SIZE, "%d\n", dgap_numboards);
-}
-static DRIVER_ATTR(boards, S_IRUSR, dgap_driver_boards_show, NULL);
-
-
-static ssize_t dgap_driver_maxboards_show(struct device_driver *ddp, char *buf)
-{
-	return snprintf(buf, PAGE_SIZE, "%d\n", MAXBOARDS);
-}
-static DRIVER_ATTR(maxboards, S_IRUSR, dgap_driver_maxboards_show, NULL);
-
-
-static ssize_t dgap_driver_pollcounter_show(struct device_driver *ddp,
-					    char *buf)
-{
-	return snprintf(buf, PAGE_SIZE, "%ld\n", dgap_poll_counter);
-}
-static DRIVER_ATTR(pollcounter, S_IRUSR, dgap_driver_pollcounter_show, NULL);
-
-static ssize_t dgap_driver_pollrate_show(struct device_driver *ddp, char *buf)
-{
-	return snprintf(buf, PAGE_SIZE, "%dms\n", dgap_poll_tick);
-}
-
-static ssize_t dgap_driver_pollrate_store(struct device_driver *ddp,
-					  const char *buf, size_t count)
-{
-	if (sscanf(buf, "%d\n", &dgap_poll_tick) != 1)
-		return -EINVAL;
-	return count;
-}
-static DRIVER_ATTR(pollrate, (S_IRUSR | S_IWUSR), dgap_driver_pollrate_show,
-		   dgap_driver_pollrate_store);
-
-static int dgap_create_driver_sysfiles(struct pci_driver *dgap_driver)
-{
-	int rc = 0;
-	struct device_driver *driverfs = &dgap_driver->driver;
-
-	rc |= driver_create_file(driverfs, &driver_attr_version);
-	rc |= driver_create_file(driverfs, &driver_attr_boards);
-	rc |= driver_create_file(driverfs, &driver_attr_maxboards);
-	rc |= driver_create_file(driverfs, &driver_attr_pollrate);
-	rc |= driver_create_file(driverfs, &driver_attr_pollcounter);
-
-	return rc;
-}
-
-static void dgap_remove_driver_sysfiles(struct pci_driver *dgap_driver)
+static void dgap_free_flipbuf(struct board_t *brd)
 {
-	struct device_driver *driverfs = &dgap_driver->driver;
-
-	driver_remove_file(driverfs, &driver_attr_version);
-	driver_remove_file(driverfs, &driver_attr_boards);
-	driver_remove_file(driverfs, &driver_attr_maxboards);
-	driver_remove_file(driverfs, &driver_attr_pollrate);
-	driver_remove_file(driverfs, &driver_attr_pollcounter);
+	kfree(brd->flipbuf);
+	kfree(brd->flipflagbuf);
 }
 
 static struct board_t *dgap_verify_board(struct device *p)
@@ -5626,7 +5377,7 @@ static ssize_t dgap_ports_state_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i;
+	unsigned int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5647,7 +5398,7 @@ static ssize_t dgap_ports_baud_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i;
+	unsigned int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5668,7 +5419,7 @@ static ssize_t dgap_ports_msignals_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i;
+	unsigned int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5705,7 +5456,7 @@ static ssize_t dgap_ports_iflag_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i;
+	unsigned int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5725,7 +5476,7 @@ static ssize_t dgap_ports_cflag_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i;
+	unsigned int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5745,7 +5496,7 @@ static ssize_t dgap_ports_oflag_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i;
+	unsigned int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5765,7 +5516,7 @@ static ssize_t dgap_ports_lflag_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i;
+	unsigned int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5785,7 +5536,7 @@ static ssize_t dgap_ports_digi_flag_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i;
+	unsigned int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5805,7 +5556,7 @@ static ssize_t dgap_ports_rxcount_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i;
+	unsigned int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5825,7 +5576,7 @@ static ssize_t dgap_ports_txcount_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i;
+	unsigned int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5839,39 +5590,6 @@ static ssize_t dgap_ports_txcount_show(struct device *p,
 }
 static DEVICE_ATTR(ports_txcount, S_IRUSR, dgap_ports_txcount_show, NULL);
 
-/* this function creates the sys files that will export each signal status
- * to sysfs each value will be put in a separate filename
- */
-static void dgap_create_ports_sysfiles(struct board_t *bd)
-{
-	dev_set_drvdata(&bd->pdev->dev, bd);
-	device_create_file(&(bd->pdev->dev), &dev_attr_ports_state);
-	device_create_file(&(bd->pdev->dev), &dev_attr_ports_baud);
-	device_create_file(&(bd->pdev->dev), &dev_attr_ports_msignals);
-	device_create_file(&(bd->pdev->dev), &dev_attr_ports_iflag);
-	device_create_file(&(bd->pdev->dev), &dev_attr_ports_cflag);
-	device_create_file(&(bd->pdev->dev), &dev_attr_ports_oflag);
-	device_create_file(&(bd->pdev->dev), &dev_attr_ports_lflag);
-	device_create_file(&(bd->pdev->dev), &dev_attr_ports_digi_flag);
-	device_create_file(&(bd->pdev->dev), &dev_attr_ports_rxcount);
-	device_create_file(&(bd->pdev->dev), &dev_attr_ports_txcount);
-}
-
-/* removes all the sys files created for that port */
-static void dgap_remove_ports_sysfiles(struct board_t *bd)
-{
-	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_state);
-	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_baud);
-	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_msignals);
-	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_iflag);
-	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_cflag);
-	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_oflag);
-	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_lflag);
-	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_digi_flag);
-	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_rxcount);
-	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_txcount);
-}
-
 static ssize_t dgap_tty_state_show(struct device *d,
 				   struct device_attribute *attr,
 				   char *buf)
@@ -6242,7 +5960,6 @@ static ssize_t dgap_tty_name_show(struct device *d,
 			}
 
 			ncount += cptr->u.module.nport;
-
 		}
 	}
 
@@ -6266,1086 +5983,1211 @@ static struct attribute *dgap_sysfs_tty_entries[] = {
 	NULL
 };
 
-static struct attribute_group dgap_tty_attribute_group = {
-	.name = NULL,
-	.attrs = dgap_sysfs_tty_entries,
-};
 
-static void dgap_create_tty_sysfs(struct un_t *un, struct device *c)
+/* this function creates the sys files that will export each signal status
+ * to sysfs each value will be put in a separate filename
+ */
+static void dgap_create_ports_sysfiles(struct board_t *bd)
 {
-	int ret;
-
-	ret = sysfs_create_group(&c->kobj, &dgap_tty_attribute_group);
-	if (ret)
-		return;
-
-	dev_set_drvdata(c, un);
-
+	dev_set_drvdata(&bd->pdev->dev, bd);
+	device_create_file(&(bd->pdev->dev), &dev_attr_ports_state);
+	device_create_file(&(bd->pdev->dev), &dev_attr_ports_baud);
+	device_create_file(&(bd->pdev->dev), &dev_attr_ports_msignals);
+	device_create_file(&(bd->pdev->dev), &dev_attr_ports_iflag);
+	device_create_file(&(bd->pdev->dev), &dev_attr_ports_cflag);
+	device_create_file(&(bd->pdev->dev), &dev_attr_ports_oflag);
+	device_create_file(&(bd->pdev->dev), &dev_attr_ports_lflag);
+	device_create_file(&(bd->pdev->dev), &dev_attr_ports_digi_flag);
+	device_create_file(&(bd->pdev->dev), &dev_attr_ports_rxcount);
+	device_create_file(&(bd->pdev->dev), &dev_attr_ports_txcount);
 }
 
-static void dgap_remove_tty_sysfs(struct device *c)
+/* removes all the sys files created for that port */
+static void dgap_remove_ports_sysfiles(struct board_t *bd)
 {
-	sysfs_remove_group(&c->kobj, &dgap_tty_attribute_group);
+	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_state);
+	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_baud);
+	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_msignals);
+	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_iflag);
+	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_cflag);
+	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_oflag);
+	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_lflag);
+	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_digi_flag);
+	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_rxcount);
+	device_remove_file(&(bd->pdev->dev), &dev_attr_ports_txcount);
 }
 
-static void dgap_cleanup_nodes(void)
+/*
+ * Copies the BIOS code from the user to the board,
+ * and starts the BIOS running.
+ */
+static void dgap_do_bios_load(struct board_t *brd, const u8 *ubios, int len)
 {
-	struct cnode *p;
+	u8 __iomem *addr;
+	uint offset;
+	unsigned int i;
 
-	p = &dgap_head;
+	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) || !brd->re_map_membase)
+		return;
 
-	while (p) {
-		struct cnode *tmp = p->next;
+	addr = brd->re_map_membase;
 
-		if (p->type == NULLNODE) {
-			p = tmp;
-			continue;
-		}
+	/*
+	 * clear POST area
+	 */
+	for (i = 0; i < 16; i++)
+		writeb(0, addr + POSTAREA + i);
 
-		switch (p->type) {
-		case BNODE:
-			kfree(p->u.board.portstr);
-			kfree(p->u.board.addrstr);
-			kfree(p->u.board.pcibusstr);
-			kfree(p->u.board.pcislotstr);
-			kfree(p->u.board.method);
-			break;
-		case CNODE:
-			kfree(p->u.conc.id);
-			kfree(p->u.conc.connect);
-			break;
-		case MNODE:
-			kfree(p->u.module.id);
-			break;
-		case TNODE:
-			kfree(p->u.ttyname);
-			break;
-		case CUNODE:
-			kfree(p->u.cuname);
-			break;
-		case LNODE:
-			kfree(p->u.line.cable);
-			break;
-		case PNODE:
-			kfree(p->u.printname);
-			break;
-		}
+	/*
+	 * Download bios
+	 */
+	offset = 0x1000;
+	memcpy_toio(addr + offset, ubios, len);
 
-		kfree(p->u.board.status);
-		kfree(p);
-		p = tmp;
-	}
+	writel(0x0bf00401, addr);
+	writel(0, (addr + 4));
+
+	/* Clear the reset, and change states. */
+	writeb(FEPCLR, brd->re_map_port);
 }
+
 /*
- * Parse a configuration file read into memory as a string.
+ * Checks to see if the BIOS completed running on the card.
  */
-static int dgap_parsefile(char **in)
+static int dgap_test_bios(struct board_t *brd)
 {
-	struct cnode *p, *brd, *line, *conc;
-	int rc;
-	char *s;
-	int linecnt = 0;
+	u8 __iomem *addr;
+	u16 word;
+	u16 err1;
+	u16 err2;
 
-	p = &dgap_head;
-	brd = line = conc = NULL;
+	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) || !brd->re_map_membase)
+		return -EINVAL;
 
-	/* perhaps we are adding to an existing list? */
-	while (p->next)
-		p = p->next;
+	addr = brd->re_map_membase;
+	word = readw(addr + POSTAREA);
 
-	/* file must start with a BEGIN */
-	while ((rc = dgap_gettok(in)) != BEGIN) {
-		if (rc == 0) {
-			pr_err("unexpected EOF");
-			return -1;
-		}
+	/*
+	 * It can take 5-6 seconds for a board to
+	 * pass the bios self test and post results.
+	 * Give it 10 seconds.
+	 */
+	brd->wait_for_bios = 0;
+	while (brd->wait_for_bios < 1000) {
+		/* Check to see if BIOS thinks board is good. (GD). */
+		if (word == *(u16 *) "GD")
+			return 0;
+		msleep_interruptible(10);
+		brd->wait_for_bios++;
+		word = readw(addr + POSTAREA);
 	}
 
-	for (; ;) {
-		int board_type = 0;
-		int conc_type = 0;
-		int module_type = 0;
+	/* Gave up on board after too long of time taken */
+	err1 = readw(addr + SEQUENCE);
+	err2 = readw(addr + ERROR);
+	dev_warn(&brd->pdev->dev, "%s failed diagnostics.  Error #(%x,%x).\n",
+		brd->name, err1, err2);
+	brd->state = BOARD_FAILED;
+	brd->dpastatus = BD_NOBIOS;
 
-		rc = dgap_gettok(in);
-		if (rc == 0) {
-			pr_err("unexpected EOF");
-			return -1;
-		}
+	return -EIO;
+}
 
-		switch (rc) {
-		case BEGIN:	/* should only be 1 begin */
-			pr_err("unexpected config_begin\n");
-			return -1;
+/*
+ * Copies the FEP code from the user to the board,
+ * and starts the FEP running.
+ */
+static void dgap_do_fep_load(struct board_t *brd, const u8 *ufep, int len)
+{
+	u8 __iomem *addr;
+	uint offset;
 
-		case END:
-			return 0;
+	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) || !brd->re_map_membase)
+		return;
 
-		case BOARD:	/* board info */
-			if (dgap_checknode(p))
-				return -1;
+	addr = brd->re_map_membase;
 
-			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next)
-				return -1;
+	/*
+	 * Download FEP
+	 */
+	offset = 0x1000;
+	memcpy_toio(addr + offset, ufep, len);
 
-			p = p->next;
+	/*
+	 * If board is a concentrator product, we need to give
+	 * it its config string describing how the concentrators look.
+	 */
+	if ((brd->type == PCX) || (brd->type == PEPC)) {
+		u8 string[100];
+		u8 __iomem *config;
+		u8 *xconfig;
+		unsigned int i = 0;
 
-			p->type = BNODE;
-			p->u.board.status = kstrdup("No", GFP_KERNEL);
-			line = conc = NULL;
-			brd = p;
-			linecnt = -1;
+		xconfig = dgap_create_config_string(brd, string);
 
-			board_type = dgap_gettok(in);
-			if (board_type == 0) {
-				pr_err("board !!type not specified");
-				return -1;
-			}
+		/* Write string to board memory */
+		config = addr + CONFIG;
+		for (; i < CONFIGSIZE; i++, config++, xconfig++) {
+			writeb(*xconfig, config);
+			if ((*xconfig & 0xff) == 0xff)
+				break;
+		}
+	}
 
-			p->u.board.type = board_type;
+	writel(0xbfc01004, (addr + 0xc34));
+	writel(0x3, (addr + 0xc30));
 
-			break;
+}
 
-		case IO:	/* i/o port */
-			if (p->type != BNODE) {
-				pr_err("IO port only vaild for boards");
-				return -1;
-			}
-			s = dgap_getword(in);
-			if (!s) {
-				pr_err("unexpected end of file");
-				return -1;
-			}
-			p->u.board.portstr = kstrdup(s, GFP_KERNEL);
-			if (kstrtol(s, 0, &p->u.board.port)) {
-				pr_err("bad number for IO port");
-				return -1;
-			}
-			p->u.board.v_port = 1;
-			break;
+/*
+ * Waits for the FEP to report thats its ready for us to use.
+ */
+static int dgap_test_fep(struct board_t *brd)
+{
+	u8 __iomem *addr;
+	u16 word;
+	u16 err1;
+	u16 err2;
 
-		case MEM:	/* memory address */
-			if (p->type != BNODE) {
-				pr_err("memory address only vaild for boards");
-				return -1;
-			}
-			s = dgap_getword(in);
-			if (!s) {
-				pr_err("unexpected end of file");
-				return -1;
-			}
-			p->u.board.addrstr = kstrdup(s, GFP_KERNEL);
-			if (kstrtoul(s, 0, &p->u.board.addr)) {
-				pr_err("bad number for memory address");
-				return -1;
-			}
-			p->u.board.v_addr = 1;
-			break;
+	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) || !brd->re_map_membase)
+		return -EINVAL;
 
-		case PCIINFO:	/* pci information */
-			if (p->type != BNODE) {
-				pr_err("memory address only vaild for boards");
-				return -1;
-			}
-			s = dgap_getword(in);
-			if (!s) {
-				pr_err("unexpected end of file");
-				return -1;
-			}
-			p->u.board.pcibusstr = kstrdup(s, GFP_KERNEL);
-			if (kstrtoul(s, 0, &p->u.board.pcibus)) {
-				pr_err("bad number for pci bus");
-				return -1;
-			}
-			p->u.board.v_pcibus = 1;
-			s = dgap_getword(in);
-			if (!s) {
-				pr_err("unexpected end of file");
-				return -1;
-			}
-			p->u.board.pcislotstr = kstrdup(s, GFP_KERNEL);
-			if (kstrtoul(s, 0, &p->u.board.pcislot)) {
-				pr_err("bad number for pci slot");
-				return -1;
-			}
-			p->u.board.v_pcislot = 1;
-			break;
+	addr = brd->re_map_membase;
+	word = readw(addr + FEPSTAT);
 
-		case METHOD:
-			if (p->type != BNODE) {
-				pr_err("install method only vaild for boards");
-				return -1;
-			}
-			s = dgap_getword(in);
-			if (!s) {
-				pr_err("unexpected end of file");
-				return -1;
-			}
-			p->u.board.method = kstrdup(s, GFP_KERNEL);
-			p->u.board.v_method = 1;
-			break;
+	/*
+	 * It can take 2-3 seconds for the FEP to
+	 * be up and running. Give it 5 secs.
+	 */
+	brd->wait_for_fep = 0;
+	while (brd->wait_for_fep < 500) {
+		/* Check to see if FEP is up and running now. */
+		if (word == *(u16 *) "OS") {
+			/*
+			 * Check to see if the board can support FEP5+ commands.
+			*/
+			word = readw(addr + FEP5_PLUS);
+			if (word == *(u16 *) "5A")
+				brd->bd_flags |= BD_FEP5PLUS;
 
-		case STATUS:
-			if (p->type != BNODE) {
-				pr_err("config status only vaild for boards");
-				return -1;
-			}
-			s = dgap_getword(in);
-			if (!s) {
-				pr_err("unexpected end of file");
-				return -1;
-			}
-			p->u.board.status = kstrdup(s, GFP_KERNEL);
-			break;
+			return 0;
+		}
+		msleep_interruptible(10);
+		brd->wait_for_fep++;
+		word = readw(addr + FEPSTAT);
+	}
 
-		case NPORTS:	/* number of ports */
-			if (p->type == BNODE) {
-				s = dgap_getword(in);
-				if (!s) {
-					pr_err("unexpected end of file");
-					return -1;
-				}
-				if (kstrtol(s, 0, &p->u.board.nport)) {
-					pr_err("bad number for number of ports");
-					return -1;
-				}
-				p->u.board.v_nport = 1;
-			} else if (p->type == CNODE) {
-				s = dgap_getword(in);
-				if (!s) {
-					pr_err("unexpected end of file");
-					return -1;
-				}
-				if (kstrtol(s, 0, &p->u.conc.nport)) {
-					pr_err("bad number for number of ports");
-					return -1;
-				}
-				p->u.conc.v_nport = 1;
-			} else if (p->type == MNODE) {
-				s = dgap_getword(in);
-				if (!s) {
-					pr_err("unexpected end of file");
-					return -1;
-				}
-				if (kstrtol(s, 0, &p->u.module.nport)) {
-					pr_err("bad number for number of ports");
-					return -1;
-				}
-				p->u.module.v_nport = 1;
-			} else {
-				pr_err("nports only valid for concentrators or modules");
-				return -1;
-			}
-			break;
+	/* Gave up on board after too long of time taken */
+	err1 = readw(addr + SEQUENCE);
+	err2 = readw(addr + ERROR);
+	dev_warn(&brd->pdev->dev,
+		 "FEPOS for %s not functioning.  Error #(%x,%x).\n",
+		 brd->name, err1, err2);
+	brd->state = BOARD_FAILED;
+	brd->dpastatus = BD_NOFEP;
 
-		case ID:	/* letter ID used in tty name */
-			s = dgap_getword(in);
-			if (!s) {
-				pr_err("unexpected end of file");
-				return -1;
-			}
+	return -EIO;
+}
 
-			p->u.board.status = kstrdup(s, GFP_KERNEL);
+/*
+ * Physically forces the FEP5 card to reset itself.
+ */
+static void dgap_do_reset_board(struct board_t *brd)
+{
+	u8 check;
+	u32 check1;
+	u32 check2;
+	unsigned int i;
 
-			if (p->type == CNODE) {
-				p->u.conc.id = kstrdup(s, GFP_KERNEL);
-				p->u.conc.v_id = 1;
-			} else if (p->type == MNODE) {
-				p->u.module.id = kstrdup(s, GFP_KERNEL);
-				p->u.module.v_id = 1;
-			} else {
-				pr_err("id only valid for concentrators or modules");
-				return -1;
-			}
-			break;
+	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) ||
+	    !brd->re_map_membase || !brd->re_map_port)
+		return;
 
-		case STARTO:	/* start offset of ID */
-			if (p->type == BNODE) {
-				s = dgap_getword(in);
-				if (!s) {
-					pr_err("unexpected end of file");
-					return -1;
-				}
-				if (kstrtol(s, 0, &p->u.board.start)) {
-					pr_err("bad number for start of tty count");
-					return -1;
-				}
-				p->u.board.v_start = 1;
-			} else if (p->type == CNODE) {
-				s = dgap_getword(in);
-				if (!s) {
-					pr_err("unexpected end of file");
-					return -1;
-				}
-				if (kstrtol(s, 0, &p->u.conc.start)) {
-					pr_err("bad number for start of tty count");
-					return -1;
-				}
-				p->u.conc.v_start = 1;
-			} else if (p->type == MNODE) {
-				s = dgap_getword(in);
-				if (!s) {
-					pr_err("unexpected end of file");
-					return -1;
-				}
-				if (kstrtol(s, 0, &p->u.module.start)) {
-					pr_err("bad number for start of tty count");
-					return -1;
-				}
-				p->u.module.v_start = 1;
-			} else {
-				pr_err("start only valid for concentrators or modules");
-				return -1;
-			}
+	/* FEPRST does not vary among supported boards */
+	writeb(FEPRST, brd->re_map_port);
+
+	for (i = 0; i <= 1000; i++) {
+		check = readb(brd->re_map_port) & 0xe;
+		if (check == FEPRST)
 			break;
+		udelay(10);
 
-		case TTYN:	/* tty name prefix */
-			if (dgap_checknode(p))
-				return -1;
+	}
+	if (i > 1000) {
+		dev_warn(&brd->pdev->dev,
+			 "dgap: Board not resetting...  Failing board.\n");
+		brd->state = BOARD_FAILED;
+		brd->dpastatus = BD_NOFEP;
+		return;
+	}
 
-			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next)
-				return -1;
+	/*
+	 * Make sure there really is memory out there.
+	 */
+	writel(0xa55a3cc3, (brd->re_map_membase + LOWMEM));
+	writel(0x5aa5c33c, (brd->re_map_membase + HIGHMEM));
+	check1 = readl(brd->re_map_membase + LOWMEM);
+	check2 = readl(brd->re_map_membase + HIGHMEM);
 
-			p = p->next;
-			p->type = TNODE;
+	if ((check1 != 0xa55a3cc3) || (check2 != 0x5aa5c33c)) {
+		dev_warn(&brd->pdev->dev,
+			 "No memory at %p for board.\n",
+			 brd->re_map_membase);
+		brd->state = BOARD_FAILED;
+		brd->dpastatus = BD_NOFEP;
+		return;
+	}
+}
 
-			s = dgap_getword(in);
-			if (!s) {
-				pr_err("unexpeced end of file");
-				return -1;
-			}
-			p->u.ttyname = kstrdup(s, GFP_KERNEL);
-			if (!p->u.ttyname)
-				return -1;
+#ifdef DIGI_CONCENTRATORS_SUPPORTED
+/*
+ * Sends a concentrator image into the FEP5 board.
+ */
+static void dgap_do_conc_load(struct board_t *brd, u8 *uaddr, int len)
+{
+	char __iomem *vaddr;
+	u16 offset;
+	struct downld_t *to_dp;
 
-			break;
+	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) || !brd->re_map_membase)
+		return;
 
-		case CU:	/* cu name prefix */
-			if (dgap_checknode(p))
-				return -1;
+	vaddr = brd->re_map_membase;
 
-			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next)
-				return -1;
+	offset = readw((u16 *) (vaddr + DOWNREQ));
+	to_dp = (struct downld_t *) (vaddr + (int) offset);
+	memcpy_toio(to_dp, uaddr, len);
 
-			p = p->next;
-			p->type = CUNODE;
+	/* Tell card we have data for it */
+	writew(0, vaddr + (DOWNREQ));
 
-			s = dgap_getword(in);
-			if (!s) {
-				pr_err("unexpeced end of file");
-				return -1;
-			}
-			p->u.cuname = kstrdup(s, GFP_KERNEL);
-			if (!p->u.cuname)
-				return -1;
+	brd->conc_dl_status = NO_PENDING_CONCENTRATOR_REQUESTS;
+}
+#endif
 
-			break;
+#define EXPANSION_ROM_SIZE	(64 * 1024)
+#define FEP5_ROM_MAGIC		(0xFEFFFFFF)
 
-		case LINE:	/* line information */
-			if (dgap_checknode(p))
-				return -1;
-			if (!brd) {
-				pr_err("must specify board before line info");
-				return -1;
-			}
-			switch (brd->u.board.type) {
-			case PPCM:
-				pr_err("line not vaild for PC/em");
-				return -1;
-			}
+static void dgap_get_vpd(struct board_t *brd)
+{
+	u32 magic;
+	u32 base_offset;
+	u16 rom_offset;
+	u16 vpd_offset;
+	u16 image_length;
+	u16 i;
+	u8 byte1;
+	u8 byte2;
 
-			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next)
-				return -1;
+	/*
+	 * Poke the magic number at the PCI Rom Address location.
+	 * If VPD is supported, the value read from that address
+	 * will be non-zero.
+	 */
+	magic = FEP5_ROM_MAGIC;
+	pci_write_config_dword(brd->pdev, PCI_ROM_ADDRESS, magic);
+	pci_read_config_dword(brd->pdev, PCI_ROM_ADDRESS, &magic);
 
-			p = p->next;
-			p->type = LNODE;
-			conc = NULL;
-			line = p;
-			linecnt++;
-			break;
+	/* VPD not supported, bail */
+	if (!magic)
+		return;
 
-		case CONC:	/* concentrator information */
-			if (dgap_checknode(p))
-				return -1;
-			if (!line) {
-				pr_err("must specify line info before concentrator");
-				return -1;
-			}
+	/*
+	 * To get to the OTPROM memory, we have to send the boards base
+	 * address or'ed with 1 into the PCI Rom Address location.
+	 */
+	magic = brd->membase | 0x01;
+	pci_write_config_dword(brd->pdev, PCI_ROM_ADDRESS, magic);
+	pci_read_config_dword(brd->pdev, PCI_ROM_ADDRESS, &magic);
 
-			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next)
-				return -1;
+	byte1 = readb(brd->re_map_membase);
+	byte2 = readb(brd->re_map_membase + 1);
 
-			p = p->next;
-			p->type = CNODE;
-			conc = p;
+	/*
+	 * If the board correctly swapped to the OTPROM memory,
+	 * the first 2 bytes (header) should be 0x55, 0xAA
+	 */
+	if (byte1 == 0x55 && byte2 == 0xAA) {
 
-			if (linecnt)
-				brd->u.board.conc2++;
-			else
-				brd->u.board.conc1++;
+		base_offset = 0;
 
-			conc_type = dgap_gettok(in);
-			if (conc_type == 0 || conc_type != CX ||
-			    conc_type != EPC) {
-				pr_err("failed to set a type of concentratros");
-				return -1;
-			}
+		/*
+		 * We have to run through all the OTPROM memory looking
+		 * for the VPD offset.
+		 */
+		while (base_offset <= EXPANSION_ROM_SIZE) {
 
-			p->u.conc.type = conc_type;
+			/*
+			 * Lots of magic numbers here.
+			 *
+			 * The VPD offset is located inside the ROM Data
+			 * Structure.
+			 *
+			 * We also have to remember the length of each
+			 * ROM Data Structure, so we can "hop" to the next
+			 * entry if the VPD isn't in the current
+			 * ROM Data Structure.
+			 */
+			rom_offset = readw(brd->re_map_membase +
+						base_offset + 0x18);
+			image_length = readw(brd->re_map_membase +
+						rom_offset + 0x10) * 512;
+			vpd_offset = readw(brd->re_map_membase +
+						rom_offset + 0x08);
 
-			break;
+			/* Found the VPD entry */
+			if (vpd_offset)
+				break;
 
-		case MOD:	/* EBI module */
-			if (dgap_checknode(p))
-				return -1;
-			if (!brd) {
-				pr_err("must specify board info before EBI modules");
-				return -1;
-			}
-			switch (brd->u.board.type) {
-			case PPCM:
-				linecnt = 0;
+			/* We didn't find a VPD entry, go to next ROM entry. */
+			base_offset += image_length;
+
+			byte1 = readb(brd->re_map_membase + base_offset);
+			byte2 = readb(brd->re_map_membase + base_offset + 1);
+
+			/*
+			 * If the new ROM offset doesn't have 0x55, 0xAA
+			 * as its header, we have run out of ROM.
+			 */
+			if (byte1 != 0x55 || byte2 != 0xAA)
 				break;
-			default:
-				if (!conc) {
-					pr_err("must specify concentrator info before EBI module");
-					return -1;
-				}
+		}
+
+		/*
+		 * If we have a VPD offset, then mark the board
+		 * as having a valid VPD, and copy VPDSIZE (512) bytes of
+		 * that VPD to the buffer we have in our board structure.
+		 */
+		if (vpd_offset) {
+			brd->bd_flags |= BD_HAS_VPD;
+			for (i = 0; i < VPDSIZE; i++) {
+				brd->vpd[i] = readb(brd->re_map_membase +
+							vpd_offset + i);
 			}
+		}
+	}
 
-			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next)
-				return -1;
+	/*
+	 * We MUST poke the magic number at the PCI Rom Address location again.
+	 * This makes the card report the regular board memory back to us,
+	 * rather than the OTPROM memory.
+	 */
+	magic = FEP5_ROM_MAGIC;
+	pci_write_config_dword(brd->pdev, PCI_ROM_ADDRESS, magic);
+}
 
-			p = p->next;
-			p->type = MNODE;
 
-			if (linecnt)
-				brd->u.board.module2++;
-			else
-				brd->u.board.module1++;
+static ssize_t dgap_driver_version_show(struct device_driver *ddp, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%s\n", DG_PART);
+}
+static DRIVER_ATTR(version, S_IRUSR, dgap_driver_version_show, NULL);
 
-			module_type = dgap_gettok(in);
-			if (module_type == 0 || module_type != PORTS ||
-			    module_type != MODEM) {
-				pr_err("failed to set a type of module");
-				return -1;
-			}
 
-			p->u.module.type = module_type;
+static ssize_t dgap_driver_boards_show(struct device_driver *ddp, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%d\n", dgap_numboards);
+}
+static DRIVER_ATTR(boards, S_IRUSR, dgap_driver_boards_show, NULL);
 
-			break;
 
-		case CABLE:
-			if (p->type == LNODE) {
-				s = dgap_getword(in);
-				if (!s) {
-					pr_err("unexpected end of file");
-					return -1;
-				}
-				p->u.line.cable = kstrdup(s, GFP_KERNEL);
-				p->u.line.v_cable = 1;
-			}
-			break;
+static ssize_t dgap_driver_maxboards_show(struct device_driver *ddp, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%d\n", MAXBOARDS);
+}
+static DRIVER_ATTR(maxboards, S_IRUSR, dgap_driver_maxboards_show, NULL);
 
-		case SPEED:	/* sync line speed indication */
-			if (p->type == LNODE) {
-				s = dgap_getword(in);
-				if (!s) {
-					pr_err("unexpected end of file");
-					return -1;
-				}
-				if (kstrtol(s, 0, &p->u.line.speed)) {
-					pr_err("bad number for line speed");
-					return -1;
-				}
-				p->u.line.v_speed = 1;
-			} else if (p->type == CNODE) {
-				s = dgap_getword(in);
-				if (!s) {
-					pr_err("unexpected end of file");
-					return -1;
-				}
-				if (kstrtol(s, 0, &p->u.conc.speed)) {
-					pr_err("bad number for line speed");
-					return -1;
-				}
-				p->u.conc.v_speed = 1;
-			} else {
-				pr_err("speed valid only for lines or concentrators.");
-				return -1;
-			}
-			break;
 
-		case CONNECT:
-			if (p->type == CNODE) {
-				s = dgap_getword(in);
-				if (!s) {
-					pr_err("unexpected end of file");
-					return -1;
-				}
-				p->u.conc.connect = kstrdup(s, GFP_KERNEL);
-				p->u.conc.v_connect = 1;
-			}
-			break;
-		case PRINT:	/* transparent print name prefix */
-			if (dgap_checknode(p))
-				return -1;
+static ssize_t dgap_driver_pollcounter_show(struct device_driver *ddp,
+					    char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%ld\n", dgap_poll_counter);
+}
+static DRIVER_ATTR(pollcounter, S_IRUSR, dgap_driver_pollcounter_show, NULL);
 
-			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next)
-				return -1;
+static ssize_t dgap_driver_pollrate_show(struct device_driver *ddp, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%dms\n", dgap_poll_tick);
+}
 
-			p = p->next;
-			p->type = PNODE;
+static ssize_t dgap_driver_pollrate_store(struct device_driver *ddp,
+					  const char *buf, size_t count)
+{
+	if (sscanf(buf, "%d\n", &dgap_poll_tick) != 1)
+		return -EINVAL;
+	return count;
+}
+static DRIVER_ATTR(pollrate, (S_IRUSR | S_IWUSR), dgap_driver_pollrate_show,
+		   dgap_driver_pollrate_store);
 
-			s = dgap_getword(in);
-			if (!s) {
-				pr_err("unexpeced end of file");
-				return -1;
-			}
-			p->u.printname = kstrdup(s, GFP_KERNEL);
-			if (!p->u.printname)
-				return -1;
 
-			break;
+static int dgap_create_driver_sysfiles(struct pci_driver *dgap_driver)
+{
+	int rc = 0;
+	struct device_driver *driverfs = &dgap_driver->driver;
 
-		case CMAJOR:	/* major number */
-			if (dgap_checknode(p))
-				return -1;
+	rc |= driver_create_file(driverfs, &driver_attr_version);
+	rc |= driver_create_file(driverfs, &driver_attr_boards);
+	rc |= driver_create_file(driverfs, &driver_attr_maxboards);
+	rc |= driver_create_file(driverfs, &driver_attr_pollrate);
+	rc |= driver_create_file(driverfs, &driver_attr_pollcounter);
 
-			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next)
-				return -1;
+	return rc;
+}
 
-			p = p->next;
-			p->type = JNODE;
+static void dgap_remove_driver_sysfiles(struct pci_driver *dgap_driver)
+{
+	struct device_driver *driverfs = &dgap_driver->driver;
 
-			s = dgap_getword(in);
-			if (!s) {
-				pr_err("unexpected end of file");
-				return -1;
-			}
-			if (kstrtol(s, 0, &p->u.majornumber)) {
-				pr_err("bad number for major number");
-				return -1;
-			}
-			break;
+	driver_remove_file(driverfs, &driver_attr_version);
+	driver_remove_file(driverfs, &driver_attr_boards);
+	driver_remove_file(driverfs, &driver_attr_maxboards);
+	driver_remove_file(driverfs, &driver_attr_pollrate);
+	driver_remove_file(driverfs, &driver_attr_pollcounter);
+}
 
-		case ALTPIN:	/* altpin setting */
-			if (dgap_checknode(p))
-				return -1;
+static struct attribute_group dgap_tty_attribute_group = {
+	.name = NULL,
+	.attrs = dgap_sysfs_tty_entries,
+};
 
-			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next)
-				return -1;
+static void dgap_create_tty_sysfs(struct un_t *un, struct device *c)
+{
+	int ret;
 
-			p = p->next;
-			p->type = ANODE;
+	ret = sysfs_create_group(&c->kobj, &dgap_tty_attribute_group);
+	if (ret)
+		return;
 
-			s = dgap_getword(in);
-			if (!s) {
-				pr_err("unexpected end of file");
-				return -1;
-			}
-			if (kstrtol(s, 0, &p->u.altpin)) {
-				pr_err("bad number for altpin");
-				return -1;
-			}
-			break;
+	dev_set_drvdata(c, un);
 
-		case USEINTR:		/* enable interrupt setting */
-			if (dgap_checknode(p))
-				return -1;
+}
 
-			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next)
-				return -1;
+static void dgap_remove_tty_sysfs(struct device *c)
+{
+	sysfs_remove_group(&c->kobj, &dgap_tty_attribute_group);
+}
 
-			p = p->next;
-			p->type = INTRNODE;
-			s = dgap_getword(in);
-			if (!s) {
-				pr_err("unexpected end of file");
-				return -1;
-			}
-			if (kstrtol(s, 0, &p->u.useintr)) {
-				pr_err("bad number for useintr");
-				return -1;
-			}
-			break;
+/*
+ * Create pr and tty device entries
+ */
+static int dgap_tty_register_ports(struct board_t *brd)
+{
+	struct channel_t *ch;
+	int i;
+	int ret;
 
-		case TTSIZ:	/* size of tty structure */
-			if (dgap_checknode(p))
-				return -1;
+	brd->serial_ports = kcalloc(brd->nasync, sizeof(*brd->serial_ports),
+					GFP_KERNEL);
+	if (!brd->serial_ports)
+		return -ENOMEM;
 
-			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next)
-				return -1;
+	brd->printer_ports = kcalloc(brd->nasync, sizeof(*brd->printer_ports),
+					GFP_KERNEL);
+	if (!brd->printer_ports) {
+		ret = -ENOMEM;
+		goto free_serial_ports;
+	}
 
-			p = p->next;
-			p->type = TSNODE;
+	for (i = 0; i < brd->nasync; i++) {
+		tty_port_init(&brd->serial_ports[i]);
+		tty_port_init(&brd->printer_ports[i]);
+	}
 
-			s = dgap_getword(in);
-			if (!s) {
-				pr_err("unexpected end of file");
-				return -1;
-			}
-			if (kstrtol(s, 0, &p->u.ttysize)) {
-				pr_err("bad number for ttysize");
-				return -1;
-			}
-			break;
+	ch = brd->channels[0];
+	for (i = 0; i < brd->nasync; i++, ch = brd->channels[i]) {
 
-		case CHSIZ:	/* channel structure size */
-			if (dgap_checknode(p))
-				return -1;
+		struct device *classp;
 
-			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next)
-				return -1;
+		classp = tty_port_register_device(&brd->serial_ports[i],
+						  brd->serial_driver,
+						  i, NULL);
 
-			p = p->next;
-			p->type = CSNODE;
+		if (IS_ERR(classp)) {
+			ret = PTR_ERR(classp);
+			goto unregister_ttys;
+		}
 
-			s = dgap_getword(in);
-			if (!s) {
-				pr_err("unexpected end of file");
-				return -1;
-			}
-			if (kstrtol(s, 0, &p->u.chsize)) {
-				pr_err("bad number for chsize");
-				return -1;
-			}
-			break;
+		dgap_create_tty_sysfs(&ch->ch_tun, classp);
+		ch->ch_tun.un_sysfs = classp;
 
-		case BSSIZ:	/* board structure size */
-			if (dgap_checknode(p))
-				return -1;
+		classp = tty_port_register_device(&brd->printer_ports[i],
+						  brd->print_driver,
+						  i, NULL);
 
-			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next)
-				return -1;
+		if (IS_ERR(classp)) {
+			ret = PTR_ERR(classp);
+			goto unregister_ttys;
+		}
 
-			p = p->next;
-			p->type = BSNODE;
+		dgap_create_tty_sysfs(&ch->ch_pun, classp);
+		ch->ch_pun.un_sysfs = classp;
+	}
+	dgap_create_ports_sysfiles(brd);
 
-			s = dgap_getword(in);
-			if (!s) {
-				pr_err("unexpected end of file");
-				return -1;
-			}
-			if (kstrtol(s, 0, &p->u.bssize)) {
-				pr_err("bad number for bssize");
-				return -1;
-			}
-			break;
+	return 0;
 
-		case UNTSIZ:	/* sched structure size */
-			if (dgap_checknode(p))
-				return -1;
+unregister_ttys:
+	while (i >= 0) {
+		ch = brd->channels[i];
+		if (ch->ch_tun.un_sysfs) {
+			dgap_remove_tty_sysfs(ch->ch_tun.un_sysfs);
+			tty_unregister_device(brd->serial_driver, i);
+		}
 
-			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next)
-				return -1;
+		if (ch->ch_pun.un_sysfs) {
+			dgap_remove_tty_sysfs(ch->ch_pun.un_sysfs);
+			tty_unregister_device(brd->print_driver, i);
+		}
+		i--;
+	}
 
-			p = p->next;
-			p->type = USNODE;
+	for (i = 0; i < brd->nasync; i++) {
+		tty_port_destroy(&brd->serial_ports[i]);
+		tty_port_destroy(&brd->printer_ports[i]);
+	}
 
-			s = dgap_getword(in);
-			if (!s) {
-				pr_err("unexpected end of file");
-				return -1;
-			}
-			if (kstrtol(s, 0, &p->u.unsize)) {
-				pr_err("bad number for schedsize");
-				return -1;
-			}
-			break;
+	kfree(brd->printer_ports);
+	brd->printer_ports = NULL;
 
-		case F2SIZ:	/* f2200 structure size */
-			if (dgap_checknode(p))
-				return -1;
+free_serial_ports:
+	kfree(brd->serial_ports);
+	brd->serial_ports = NULL;
 
-			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next)
-				return -1;
+	return ret;
+}
 
-			p = p->next;
-			p->type = FSNODE;
+/*
+ * dgap_cleanup_tty()
+ *
+ * Uninitialize the TTY portion of this driver.  Free all memory and
+ * resources.
+ */
+static void dgap_cleanup_tty(struct board_t *brd)
+{
+	struct device *dev;
+	unsigned int i;
 
-			s = dgap_getword(in);
-			if (!s) {
-				pr_err("unexpected end of file");
-				return -1;
-			}
-			if (kstrtol(s, 0, &p->u.f2size)) {
-				pr_err("bad number for f2200size");
-				return -1;
-			}
-			break;
+	for (i = 0; i < brd->nasync; i++) {
+		tty_port_destroy(&brd->serial_ports[i]);
+		dev = brd->channels[i]->ch_tun.un_sysfs;
+		dgap_remove_tty_sysfs(dev);
+		tty_unregister_device(brd->serial_driver, i);
+	}
+	tty_unregister_driver(brd->serial_driver);
+	put_tty_driver(brd->serial_driver);
+	kfree(brd->serial_ports);
 
-		case VPSIZ:	/* vpix structure size */
-			if (dgap_checknode(p))
-				return -1;
+	for (i = 0; i < brd->nasync; i++) {
+		tty_port_destroy(&brd->printer_ports[i]);
+		dev = brd->channels[i]->ch_pun.un_sysfs;
+		dgap_remove_tty_sysfs(dev);
+		tty_unregister_device(brd->print_driver, i);
+	}
+	tty_unregister_driver(brd->print_driver);
+	put_tty_driver(brd->print_driver);
+	kfree(brd->printer_ports);
+}
 
-			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next)
-				return -1;
+static int dgap_request_irq(struct board_t *brd)
+{
+	int rc;
 
-			p = p->next;
-			p->type = VSNODE;
+	if (!brd || brd->magic != DGAP_BOARD_MAGIC)
+		return -ENODEV;
 
-			s = dgap_getword(in);
-			if (!s) {
-				pr_err("unexpected end of file");
-				return -1;
-			}
-			if (kstrtol(s, 0, &p->u.vpixsize)) {
-				pr_err("bad number for vpixsize");
-				return -1;
-			}
-			break;
-		}
+	/*
+	 * Set up our interrupt handler if we are set to do interrupts.
+	 */
+	if (dgap_config_get_useintr(brd) && brd->irq) {
+
+		rc = request_irq(brd->irq, dgap_intr, IRQF_SHARED, "DGAP", brd);
+
+		if (!rc)
+			brd->intr_used = 1;
 	}
+	return 0;
 }
 
-/*
- * dgap_sindex: much like index(), but it looks for a match of any character in
- * the group, and returns that position.  If the first character is a ^, then
- * this will match the first occurrence not in that group.
- */
-static char *dgap_sindex(char *string, char *group)
+static void dgap_free_irq(struct board_t *brd)
 {
-	char *ptr;
+	if (brd->intr_used && brd->irq)
+		free_irq(brd->irq, brd);
+}
 
-	if (!string || !group)
-		return (char *) NULL;
+static int dgap_firmware_load(struct pci_dev *pdev, int card_type,
+			      struct board_t *brd)
+{
+	const struct firmware *fw;
+	char *tmp_ptr;
+	int ret;
+	char *dgap_config_buf;
 
-	if (*group == '^') {
-		group++;
-		for (; *string; string++) {
-			for (ptr = group; *ptr; ptr++) {
-				if (*ptr == *string)
-					break;
-			}
-			if (*ptr == '\0')
-				return string;
+	dgap_get_vpd(brd);
+	dgap_do_reset_board(brd);
+
+	if (fw_info[card_type].conf_name) {
+		ret = request_firmware(&fw, fw_info[card_type].conf_name,
+					 &pdev->dev);
+		if (ret) {
+			dev_err(&pdev->dev, "config file %s not found\n",
+				fw_info[card_type].conf_name);
+			return ret;
 		}
-	} else {
-		for (; *string; string++) {
-			for (ptr = group; *ptr; ptr++) {
-				if (*ptr == *string)
-					return string;
-			}
+
+		dgap_config_buf = kzalloc(fw->size + 1, GFP_KERNEL);
+		if (!dgap_config_buf) {
+			release_firmware(fw);
+			return -ENOMEM;
+		}
+
+		memcpy(dgap_config_buf, fw->data, fw->size);
+		release_firmware(fw);
+
+		/*
+		 * preserve dgap_config_buf
+		 * as dgap_parsefile would
+		 * otherwise alter it.
+		 */
+		tmp_ptr = dgap_config_buf;
+
+		if (dgap_parsefile(&tmp_ptr) != 0) {
+			kfree(dgap_config_buf);
+			return -EINVAL;
 		}
+		kfree(dgap_config_buf);
 	}
 
-	return (char *) NULL;
-}
+	/*
+	 * Match this board to a config the user created for us.
+	 */
+	brd->bd_config =
+		dgap_find_config(brd->type, brd->pci_bus, brd->pci_slot);
 
-/*
- * Get a token from the input file; return 0 if end of file is reached
- */
-static int dgap_gettok(char **in)
-{
-	char *w;
-	struct toklist *t;
+	/*
+	 * Because the 4 port Xr products share the same PCI ID
+	 * as the 8 port Xr products, if we receive a NULL config
+	 * back, and this is a PAPORT8 board, retry with a
+	 * PAPORT4 attempt as well.
+	 */
+	if (brd->type == PAPORT8 && !brd->bd_config)
+		brd->bd_config =
+			dgap_find_config(PAPORT4, brd->pci_bus, brd->pci_slot);
 
-	if (strstr(dgap_cword, "board")) {
-		w = dgap_getword(in);
-		snprintf(dgap_cword, MAXCWORD, "%s", w);
-		for (t = dgap_brdtype; t->token != 0; t++) {
-			if (!strcmp(w, t->string))
-				return t->token;
+	if (!brd->bd_config) {
+		dev_err(&pdev->dev, "No valid configuration found\n");
+		return -EINVAL;
+	}
+
+	if (fw_info[card_type].bios_name) {
+		ret = request_firmware(&fw, fw_info[card_type].bios_name,
+					&pdev->dev);
+		if (ret) {
+			dev_err(&pdev->dev, "bios file %s not found\n",
+				fw_info[card_type].bios_name);
+			return ret;
 		}
-	} else {
-		while ((w = dgap_getword(in))) {
-			snprintf(dgap_cword, MAXCWORD, "%s", w);
-			for (t = dgap_tlist; t->token != 0; t++) {
-				if (!strcmp(w, t->string))
-					return t->token;
-			}
+		dgap_do_bios_load(brd, fw->data, fw->size);
+		release_firmware(fw);
+
+		/* Wait for BIOS to test board... */
+		ret = dgap_test_bios(brd);
+		if (ret)
+			return ret;
+	}
+
+	if (fw_info[card_type].fep_name) {
+		ret = request_firmware(&fw, fw_info[card_type].fep_name,
+					&pdev->dev);
+		if (ret) {
+			dev_err(&pdev->dev, "dgap: fep file %s not found\n",
+				fw_info[card_type].fep_name);
+			return ret;
+		}
+		dgap_do_fep_load(brd, fw->data, fw->size);
+		release_firmware(fw);
+
+		/* Wait for FEP to load on board... */
+		ret = dgap_test_fep(brd);
+		if (ret)
+			return ret;
+	}
+
+#ifdef DIGI_CONCENTRATORS_SUPPORTED
+	/*
+	 * If this is a CX or EPCX, we need to see if the firmware
+	 * is requesting a concentrator image from us.
+	 */
+	if ((bd->type == PCX) || (bd->type == PEPC)) {
+		chk_addr = (u16 *) (vaddr + DOWNREQ);
+		/* Nonzero if FEP is requesting concentrator image. */
+		check = readw(chk_addr);
+		vaddr = brd->re_map_membase;
+	}
+
+	if (fw_info[card_type].con_name && check && vaddr) {
+		ret = request_firmware(&fw, fw_info[card_type].con_name,
+					&pdev->dev);
+		if (ret) {
+			dev_err(&pdev->dev, "conc file %s not found\n",
+				fw_info[card_type].con_name);
+			return ret;
 		}
+		/* Put concentrator firmware loading code here */
+		offset = readw((u16 *) (vaddr + DOWNREQ));
+		memcpy_toio(offset, fw->data, fw->size);
+
+		dgap_do_conc_load(brd, (char *)fw->data, fw->size)
+		release_firmware(fw);
 	}
+#endif
 
 	return 0;
 }
 
 /*
- * get a word from the input stream, also keep track of current line number.
- * words are separated by whitespace.
+ * dgap_tty_init()
+ *
+ * Init the tty subsystem.  Called once per board after board has been
+ * downloaded and init'ed.
  */
-static char *dgap_getword(char **in)
+static int dgap_tty_init(struct board_t *brd)
 {
-	char *ret_ptr = *in;
+	int i;
+	int tlw;
+	uint true_count;
+	u8 __iomem *vaddr;
+	u8 modem;
+	struct channel_t *ch;
+	struct bs_t __iomem *bs;
+	struct cm_t __iomem *cm;
+	int ret;
 
-	char *ptr = dgap_sindex(*in, " \t\n");
+	/*
+	 * Initialize board structure elements.
+	 */
 
-	/* If no word found, return null */
-	if (!ptr)
-		return NULL;
+	vaddr = brd->re_map_membase;
+	true_count = readw((vaddr + NCHAN));
 
-	/* Mark new location for our buffer */
-	*ptr = '\0';
-	*in = ptr + 1;
+	brd->nasync = dgap_config_get_num_prts(brd);
 
-	/* Eat any extra spaces/tabs/newlines that might be present */
-	while (*in && **in && ((**in == ' ') ||
-			       (**in == '\t') ||
-			       (**in == '\n'))) {
-		**in = '\0';
-		*in = *in + 1;
-	}
+	if (!brd->nasync)
+		brd->nasync = brd->maxports;
 
-	return ret_ptr;
-}
+	if (brd->nasync > brd->maxports)
+		brd->nasync = brd->maxports;
 
-/*
- * dgap_checknode: see if all the necessary info has been supplied for a node
- * before creating the next node.
- */
-static int dgap_checknode(struct cnode *p)
-{
-	switch (p->type) {
-	case LNODE:
-		if (p->u.line.v_speed == 0) {
-			pr_err("line speed not specified");
-			return 1;
-		}
-		return 0;
+	if (true_count != brd->nasync) {
+		dev_warn(&brd->pdev->dev,
+			 "%s configured for %d ports, has %d ports.\n",
+			 brd->name, brd->nasync, true_count);
 
-	case CNODE:
-		if (p->u.conc.v_speed == 0) {
-			pr_err("concentrator line speed not specified");
-			return 1;
-		}
-		if (p->u.conc.v_nport == 0) {
-			pr_err("number of ports on concentrator not specified");
-			return 1;
+		if ((brd->type == PPCM) &&
+		    (true_count == 64 || true_count == 0)) {
+			dev_warn(&brd->pdev->dev,
+				 "Please make SURE the EBI cable running from the card\n");
+			dev_warn(&brd->pdev->dev,
+				 "to each EM module is plugged into EBI IN!\n");
 		}
-		if (p->u.conc.v_id == 0) {
-			pr_err("concentrator id letter not specified");
-			return 1;
+
+		brd->nasync = true_count;
+
+		/* If no ports, don't bother going any further */
+		if (!brd->nasync) {
+			brd->state = BOARD_FAILED;
+			brd->dpastatus = BD_NOFEP;
+			return -EIO;
 		}
-		return 0;
+	}
 
-	case MNODE:
-		if (p->u.module.v_nport == 0) {
-			pr_err("number of ports on EBI module not specified");
-			return 1;
+	/*
+	 * Allocate channel memory that might not have been allocated
+	 * when the driver was first loaded.
+	 */
+	for (i = 0; i < brd->nasync; i++) {
+		brd->channels[i] =
+			kzalloc(sizeof(struct channel_t), GFP_KERNEL);
+		if (!brd->channels[i]) {
+			ret = -ENOMEM;
+			goto free_chan;
 		}
-		if (p->u.module.v_id == 0) {
-			pr_err("EBI module id letter not specified");
-			return 1;
+	}
+
+	ch = brd->channels[0];
+	vaddr = brd->re_map_membase;
+
+	bs = (struct bs_t __iomem *) ((ulong) vaddr + CHANBUF);
+	cm = (struct cm_t __iomem *) ((ulong) vaddr + CMDBUF);
+
+	brd->bd_bs = bs;
+
+	/* Set up channel variables */
+	for (i = 0; i < brd->nasync; i++, ch = brd->channels[i], bs++) {
+
+		spin_lock_init(&ch->ch_lock);
+
+		/* Store all our magic numbers */
+		ch->magic = DGAP_CHANNEL_MAGIC;
+		ch->ch_tun.magic = DGAP_UNIT_MAGIC;
+		ch->ch_tun.un_type = DGAP_SERIAL;
+		ch->ch_tun.un_ch = ch;
+		ch->ch_tun.un_dev = i;
+
+		ch->ch_pun.magic = DGAP_UNIT_MAGIC;
+		ch->ch_pun.un_type = DGAP_PRINT;
+		ch->ch_pun.un_ch = ch;
+		ch->ch_pun.un_dev = i;
+
+		ch->ch_vaddr = vaddr;
+		ch->ch_bs = bs;
+		ch->ch_cm = cm;
+		ch->ch_bd = brd;
+		ch->ch_portnum = i;
+		ch->ch_digi = dgap_digi_init;
+
+		/*
+		 * Set up digi dsr and dcd bits based on altpin flag.
+		 */
+		if (dgap_config_get_altpin(brd)) {
+			ch->ch_dsr	= DM_CD;
+			ch->ch_cd	= DM_DSR;
+			ch->ch_digi.digi_flags |= DIGI_ALTPIN;
+		} else {
+			ch->ch_cd	= DM_CD;
+			ch->ch_dsr	= DM_DSR;
 		}
-		return 0;
+
+		ch->ch_taddr = vaddr + (ioread16(&(ch->ch_bs->tx_seg)) << 4);
+		ch->ch_raddr = vaddr + (ioread16(&(ch->ch_bs->rx_seg)) << 4);
+		ch->ch_tx_win = 0;
+		ch->ch_rx_win = 0;
+		ch->ch_tsize = readw(&(ch->ch_bs->tx_max)) + 1;
+		ch->ch_rsize = readw(&(ch->ch_bs->rx_max)) + 1;
+		ch->ch_tstart = 0;
+		ch->ch_rstart = 0;
+
+		/*
+		 * Set queue water marks, interrupt mask,
+		 * and general tty parameters.
+		 */
+		tlw = ch->ch_tsize >= 2000 ? ((ch->ch_tsize * 5) / 8) :
+						ch->ch_tsize / 2;
+		ch->ch_tlw = tlw;
+
+		dgap_cmdw(ch, STLOW, tlw, 0);
+
+		dgap_cmdw(ch, SRLOW, ch->ch_rsize / 2, 0);
+
+		dgap_cmdw(ch, SRHIGH, 7 * ch->ch_rsize / 8, 0);
+
+		ch->ch_mistat = readb(&(ch->ch_bs->m_stat));
+
+		init_waitqueue_head(&ch->ch_flags_wait);
+		init_waitqueue_head(&ch->ch_tun.un_flags_wait);
+		init_waitqueue_head(&ch->ch_pun.un_flags_wait);
+
+		/* Turn on all modem interrupts for now */
+		modem = (DM_CD | DM_DSR | DM_CTS | DM_RI);
+		writeb(modem, &(ch->ch_bs->m_int));
+
+		/*
+		 * Set edelay to 0 if interrupts are turned on,
+		 * otherwise set edelay to the usual 100.
+		 */
+		if (brd->intr_used)
+			writew(0, &(ch->ch_bs->edelay));
+		else
+			writew(100, &(ch->ch_bs->edelay));
+
+		writeb(1, &(ch->ch_bs->idata));
 	}
+
 	return 0;
+
+free_chan:
+	while (--i >= 0) {
+		kfree(brd->channels[i]);
+		brd->channels[i] = NULL;
+	}
+	return ret;
 }
 
 /*
- * Given a board pointer, returns whether we should use interrupts or not.
+ * dgap_tty_free()
+ *
+ * Free the channles which are allocated in dgap_tty_init().
  */
-static uint dgap_config_get_useintr(struct board_t *bd)
+static void dgap_tty_free(struct board_t *brd)
 {
-	struct cnode *p;
+	int i;
 
-	if (!bd)
-		return 0;
+	for (i = 0; i < brd->nasync; i++)
+		kfree(brd->channels[i]);
+}
 
-	for (p = bd->bd_config; p; p = p->next) {
-		if (p->type == INTRNODE) {
-			/*
-			 * check for pcxr types.
-			 */
-			return p->u.useintr;
-		}
-	}
+static int dgap_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	int rc;
+	struct board_t *brd;
+
+	if (dgap_numboards >= MAXBOARDS)
+		return -EPERM;
+
+	rc = pci_enable_device(pdev);
+	if (rc)
+		return -EIO;
+
+	brd = dgap_found_board(pdev, ent->driver_data, dgap_numboards);
+	if (IS_ERR(brd))
+		return PTR_ERR(brd);
+
+	rc = dgap_firmware_load(pdev, ent->driver_data, brd);
+	if (rc)
+		goto cleanup_brd;
+
+	rc = dgap_alloc_flipbuf(brd);
+	if (rc)
+		goto cleanup_brd;
+
+	rc = dgap_tty_register(brd);
+	if (rc)
+		goto free_flipbuf;
+
+	rc = dgap_request_irq(brd);
+	if (rc)
+		goto unregister_tty;
+
+	/*
+	 * Do tty device initialization.
+	 */
+	rc = dgap_tty_init(brd);
+	if (rc < 0)
+		goto free_irq;
+
+	rc = dgap_tty_register_ports(brd);
+	if (rc)
+		goto tty_free;
+
+	brd->state = BOARD_READY;
+	brd->dpastatus = BD_RUNNING;
+
+	dgap_board[dgap_numboards++] = brd;
 
-	/* If not found, then don't turn on interrupts. */
 	return 0;
+
+tty_free:
+	dgap_tty_free(brd);
+free_irq:
+	dgap_free_irq(brd);
+unregister_tty:
+	dgap_tty_unregister(brd);
+free_flipbuf:
+	dgap_free_flipbuf(brd);
+cleanup_brd:
+	dgap_cleanup_nodes();
+	dgap_unmap(brd);
+	kfree(brd);
+
+	return rc;
 }
 
+static void dgap_remove_one(struct pci_dev *dev)
+{
+	/* Do Nothing */
+}
+
+static struct pci_driver dgap_driver = {
+	.name		= "dgap",
+	.probe		= dgap_init_one,
+	.id_table	= dgap_pci_tbl,
+	.remove		= dgap_remove_one,
+};
+
 /*
- * Given a board pointer, returns whether we turn on altpin or not.
+ * Start of driver.
  */
-static uint dgap_config_get_altpin(struct board_t *bd)
+static int dgap_start(void)
 {
-	struct cnode *p;
+	int rc;
+	unsigned long flags;
+	struct device *device;
 
-	if (!bd)
-		return 0;
+	dgap_numboards = 0;
 
-	for (p = bd->bd_config; p; p = p->next) {
-		if (p->type == ANODE) {
-			/*
-			 * check for pcxr types.
-			 */
-			return p->u.altpin;
-		}
+	pr_info("For the tools package please visit http://www.digi.com\n");
+
+	/*
+	 * Register our base character device into the kernel.
+	 */
+
+	/*
+	 * Register management/dpa devices
+	 */
+	rc = register_chrdev(DIGI_DGAP_MAJOR, "dgap", &dgap_board_fops);
+	if (rc < 0)
+		return rc;
+
+	dgap_class = class_create(THIS_MODULE, "dgap_mgmt");
+	if (IS_ERR(dgap_class)) {
+		rc = PTR_ERR(dgap_class);
+		goto failed_class;
 	}
 
-	/* If not found, then don't turn on interrupts. */
-	return 0;
+	device = device_create(dgap_class, NULL,
+		MKDEV(DIGI_DGAP_MAJOR, 0),
+		NULL, "dgap_mgmt");
+	if (IS_ERR(device)) {
+		rc = PTR_ERR(device);
+		goto failed_device;
+	}
+
+	/* Start the poller */
+	spin_lock_irqsave(&dgap_poll_lock, flags);
+	init_timer(&dgap_poll_timer);
+	dgap_poll_timer.function = dgap_poll_handler;
+	dgap_poll_timer.data = 0;
+	dgap_poll_time = jiffies + dgap_jiffies_from_ms(dgap_poll_tick);
+	dgap_poll_timer.expires = dgap_poll_time;
+	spin_unlock_irqrestore(&dgap_poll_lock, flags);
+
+	add_timer(&dgap_poll_timer);
+
+	return rc;
+
+failed_device:
+	class_destroy(dgap_class);
+failed_class:
+	unregister_chrdev(DIGI_DGAP_MAJOR, "dgap");
+	return rc;
 }
 
-/*
- * Given a specific type of board, if found, detached link and
- * returns the first occurrence in the list.
- */
-static struct cnode *dgap_find_config(int type, int bus, int slot)
+static void dgap_stop(void)
 {
-	struct cnode *p, *prev, *prev2, *found;
+	unsigned long lock_flags;
 
-	p = &dgap_head;
+	spin_lock_irqsave(&dgap_poll_lock, lock_flags);
+	dgap_poll_stop = 1;
+	spin_unlock_irqrestore(&dgap_poll_lock, lock_flags);
 
-	while (p->next) {
-		prev = p;
-		p = p->next;
+	del_timer_sync(&dgap_poll_timer);
 
-		if (p->type != BNODE)
-			continue;
+	device_destroy(dgap_class, MKDEV(DIGI_DGAP_MAJOR, 0));
+	class_destroy(dgap_class);
+	unregister_chrdev(DIGI_DGAP_MAJOR, "dgap");
+}
 
-		if (p->u.board.type != type)
-			continue;
+/*
+ * dgap_cleanup_board()
+ *
+ * Free all the memory associated with a board
+ */
+static void dgap_cleanup_board(struct board_t *brd)
+{
+	unsigned int i;
 
-		if (p->u.board.v_pcibus &&
-		    p->u.board.pcibus != bus)
-			continue;
+	if (!brd || brd->magic != DGAP_BOARD_MAGIC)
+		return;
 
-		if (p->u.board.v_pcislot &&
-		    p->u.board.pcislot != slot)
-			continue;
+	dgap_free_irq(brd);
 
-		found = p;
-		/*
-		 * Keep walking thru the list till we
-		 * find the next board.
-		 */
-		while (p->next) {
-			prev2 = p;
-			p = p->next;
+	tasklet_kill(&brd->helper_tasklet);
 
-			if (p->type != BNODE)
-				continue;
+	dgap_unmap(brd);
 
-			/*
-			 * Mark the end of our 1 board
-			 * chain of configs.
-			 */
-			prev2->next = NULL;
+	/* Free all allocated channels structs */
+	for (i = 0; i < MAXPORTS ; i++)
+		kfree(brd->channels[i]);
 
-			/*
-			 * Link the "next" board to the
-			 * previous board, effectively
-			 * "unlinking" our board from
-			 * the main config.
-			 */
-			prev->next = p;
+	kfree(brd->flipbuf);
+	kfree(brd->flipflagbuf);
 
-			return found;
-		}
-		/*
-		 * It must be the last board in the list.
-		 */
-		prev->next = NULL;
-		return found;
-	}
-	return NULL;
+	dgap_board[brd->boardnum] = NULL;
+
+	kfree(brd);
 }
 
+
+/************************************************************************
+ *
+ * Driver load/unload functions
+ *
+ ************************************************************************/
+
 /*
- * Given a board pointer, walks the config link, counting up
- * all ports user specified should be on the board.
- * (This does NOT mean they are all actually present right now tho)
+ * init_module()
+ *
+ * Module load.  This is where it all starts.
  */
-static uint dgap_config_get_num_prts(struct board_t *bd)
+static int dgap_init_module(void)
 {
-	int count = 0;
-	struct cnode *p;
+	int rc;
 
-	if (!bd)
-		return 0;
+	pr_info("%s, Digi International Part Number %s\n", DG_NAME, DG_PART);
 
-	for (p = bd->bd_config; p; p = p->next) {
+	rc = dgap_start();
+	if (rc)
+		return rc;
 
-		switch (p->type) {
-		case BNODE:
-			/*
-			 * check for pcxr types.
-			 */
-			if (p->u.board.type > EPCFE)
-				count += p->u.board.nport;
-			break;
-		case CNODE:
-			count += p->u.conc.nport;
-			break;
-		case MNODE:
-			count += p->u.module.nport;
-			break;
-		}
-	}
-	return count;
+	rc = pci_register_driver(&dgap_driver);
+	if (rc)
+		goto err_stop;
+
+	rc = dgap_create_driver_sysfiles(&dgap_driver);
+	if (rc)
+		goto err_unregister;
+
+	dgap_driver_state = DRIVER_READY;
+
+	return 0;
+
+err_unregister:
+	pci_unregister_driver(&dgap_driver);
+err_stop:
+	dgap_stop();
+
+	return rc;
 }
 
-static char *dgap_create_config_string(struct board_t *bd, char *string)
+/*
+ * dgap_cleanup_module()
+ *
+ * Module unload.  This is where it all ends.
+ */
+static void dgap_cleanup_module(void)
 {
-	char *ptr = string;
-	struct cnode *p;
-	struct cnode *q;
-	int speed;
+	unsigned int i;
+	ulong lock_flags;
 
-	if (!bd) {
-		*ptr = 0xff;
-		return string;
-	}
+	spin_lock_irqsave(&dgap_poll_lock, lock_flags);
+	dgap_poll_stop = 1;
+	spin_unlock_irqrestore(&dgap_poll_lock, lock_flags);
 
-	for (p = bd->bd_config; p; p = p->next) {
+	/* Turn off poller right away. */
+	del_timer_sync(&dgap_poll_timer);
 
-		switch (p->type) {
-		case LNODE:
-			*ptr = '\0';
-			ptr++;
-			*ptr = p->u.line.speed;
-			ptr++;
-			break;
-		case CNODE:
-			/*
-			 * Because the EPC/con concentrators can have EM modules
-			 * hanging off of them, we have to walk ahead in the
-			 * list and keep adding the number of ports on each EM
-			 * to the config. UGH!
-			 */
-			speed = p->u.conc.speed;
-			q = p->next;
-			if (q && (q->type == MNODE)) {
-				*ptr = (p->u.conc.nport + 0x80);
-				ptr++;
-				p = q;
-				while (q->next && (q->next->type) == MNODE) {
-					*ptr = (q->u.module.nport + 0x80);
-					ptr++;
-					p = q;
-					q = q->next;
-				}
-				*ptr = q->u.module.nport;
-				ptr++;
-			} else {
-				*ptr = p->u.conc.nport;
-				ptr++;
-			}
+	dgap_remove_driver_sysfiles(&dgap_driver);
 
-			*ptr = speed;
-			ptr++;
-			break;
-		}
+	device_destroy(dgap_class, MKDEV(DIGI_DGAP_MAJOR, 0));
+	class_destroy(dgap_class);
+	unregister_chrdev(DIGI_DGAP_MAJOR, "dgap");
+
+	for (i = 0; i < dgap_numboards; ++i) {
+		dgap_remove_ports_sysfiles(dgap_board[i]);
+		dgap_cleanup_tty(dgap_board[i]);
+		dgap_cleanup_board(dgap_board[i]);
 	}
 
-	*ptr = 0xff;
-	return string;
+	dgap_cleanup_nodes();
+
+	if (dgap_numboards)
+		pci_unregister_driver(&dgap_driver);
 }
+
+module_init(dgap_init_module);
+module_exit(dgap_cleanup_module);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Digi International, http://www.digi.com");
+MODULE_DESCRIPTION("Driver for the Digi International EPCA PCI based product line");
+MODULE_SUPPORTED_DEVICE("dgap");
diff --git a/drivers/staging/dgap/dgap.h b/drivers/staging/dgap/dgap.h
index 14e2ed0fe39b..684033156e8c 100644
--- a/drivers/staging/dgap/dgap.h
+++ b/drivers/staging/dgap/dgap.h
@@ -584,9 +584,6 @@ struct board_t {
 	struct tty_port *printer_ports;
 	char		print_name[200];
 
-	u32		dgap_serial_major;
-	u32		dgap_transparent_print_major;
-
 	struct bs_t __iomem *bd_bs;	/* Base structure pointer         */
 
 	char	*flipbuf;		/* Our flip buffer, alloced if    */
diff --git a/drivers/staging/dgnc/dgnc_cls.c b/drivers/staging/dgnc/dgnc_cls.c
index a17f4f6095c8..bedc5221b6fc 100644
--- a/drivers/staging/dgnc/dgnc_cls.c
+++ b/drivers/staging/dgnc/dgnc_cls.c
@@ -724,10 +724,8 @@ static void cls_tasklet(unsigned long data)
 	int state = 0;
 	int ports = 0;
 
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC) {
-		APR(("poll_tasklet() - NULL or bad bd.\n"));
+	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
 		return;
-	}
 
 	/* Cache a couple board values */
 	spin_lock_irqsave(&bd->bd_lock, flags);
@@ -794,25 +792,17 @@ static void cls_tasklet(unsigned long data)
  */
 static irqreturn_t cls_intr(int irq, void *voidbrd)
 {
-	struct dgnc_board *brd = (struct dgnc_board *) voidbrd;
+	struct dgnc_board *brd = voidbrd;
 	uint i = 0;
 	unsigned char poll_reg;
 	unsigned long flags;
 
-	if (!brd) {
-		APR(("Received interrupt (%d) with null board associated\n",
-									 irq));
-		return IRQ_NONE;
-	}
-
 	/*
-	 * Check to make sure its for us.
+	 * Check to make sure it didn't receive interrupt with a null board
+	 * associated or a board pointer that wasn't ours.
 	 */
-	if (brd->magic != DGNC_BOARD_MAGIC) {
-		APR(("Received interrupt (%d) with a board pointer that wasn't ours!\n",
-			  irq));
+	if (!brd || brd->magic != DGNC_BOARD_MAGIC)
 		return IRQ_NONE;
-	}
 
 	spin_lock_irqsave(&brd->bd_intr_lock, flags);
 
@@ -928,8 +918,6 @@ static void cls_copy_data_from_uart_to_queue(struct channel_t *ch)
 		ch->ch_equeue[head] = linestatus & (UART_LSR_BI | UART_LSR_PE
 								 | UART_LSR_FE);
 		ch->ch_rqueue[head] = readb(&ch->ch_cls_uart->txrx);
-		dgnc_sniff_nowait_nolock(ch, "UART READ",
-						 ch->ch_rqueue + head, 1);
 
 		qleft--;
 
@@ -964,7 +952,6 @@ static int cls_drain(struct tty_struct *tty, uint seconds)
 	unsigned long flags;
 	struct channel_t *ch;
 	struct un_t *un;
-	int rc = 0;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return -ENXIO;
@@ -984,12 +971,11 @@ static int cls_drain(struct tty_struct *tty, uint seconds)
 	/*
 	 * NOTE: Do something with time passed in.
 	 */
-	rc = wait_event_interruptible(un->un_flags_wait,
-					 ((un->un_flags & UN_EMPTY) == 0));
 
 	/* If ret is non-zero, user ctrl-c'ed us */
 
-	return rc;
+	return wait_event_interruptible(un->un_flags_wait,
+					 ((un->un_flags & UN_EMPTY) == 0));
 }
 
 
@@ -1098,8 +1084,6 @@ static void cls_copy_data_from_queue_to_uart(struct channel_t *ch)
 			ch->ch_tun.un_flags |= (UN_EMPTY);
 		}
 		writeb(ch->ch_wqueue[ch->ch_w_tail], &ch->ch_cls_uart->txrx);
-		dgnc_sniff_nowait_nolock(ch, "UART WRITE",
-					    ch->ch_wqueue + ch->ch_w_tail, 1);
 		ch->ch_w_tail++;
 		ch->ch_w_tail &= WQUEUEMASK;
 		ch->ch_txcount++;
diff --git a/drivers/staging/dgnc/dgnc_driver.c b/drivers/staging/dgnc/dgnc_driver.c
index 21546659ff07..ba98ff348112 100644
--- a/drivers/staging/dgnc/dgnc_driver.c
+++ b/drivers/staging/dgnc/dgnc_driver.c
@@ -49,16 +49,6 @@ MODULE_AUTHOR("Digi International, http://www.digi.com");
 MODULE_DESCRIPTION("Driver for the Digi International Neo and Classic PCI based product line");
 MODULE_SUPPORTED_DEVICE("dgnc");
 
-/*
- * insmod command line overrideable parameters
- *
- * NOTE: we use a set of macros to create the variables, which allows
- * us to specify the variable type, name, initial value, and description.
- */
-PARM_INT(debug,		0x00,		0644,	"Driver debugging level");
-PARM_INT(rawreadok,	1,		0644,	"Bypass flip buffers on input");
-PARM_INT(trcbuf_size,	0x100000,	0644,	"Debugging trace buffer size.");
-
 /**************************************************************************
  *
  * protos for this file
@@ -207,8 +197,6 @@ static int __init dgnc_init_module(void)
 {
 	int rc = 0;
 
-	APR(("%s, Digi International Part Number %s\n", DG_NAME, DG_PART));
-
 	/*
 	 * Initialize global stuff
 	 */
@@ -254,8 +242,6 @@ static int dgnc_start(void)
 	/* make sure that the globals are init'd before we do anything else */
 	dgnc_init_globals();
 
-	APR(("For the tools package or updated drivers please visit http://www.digi.com\n"));
-
 	/*
 	 * Register our base character device into the kernel.
 	 * This allows the download daemon to connect to the downld device
@@ -265,7 +251,7 @@ static int dgnc_start(void)
 	 */
 	rc = register_chrdev(0, "dgnc", &dgnc_BoardFops);
 	if (rc <= 0) {
-		APR(("Can't register dgnc driver device (%d)\n", rc));
+		pr_err(DRVSTR ": Can't register dgnc driver device (%d)\n", rc);
 		return -ENXIO;
 	}
 	dgnc_Major = rc;
@@ -281,7 +267,7 @@ static int dgnc_start(void)
 	rc = dgnc_tty_preinit();
 
 	if (rc < 0) {
-		APR(("tty preinit - not enough memory (%d)\n", rc));
+		pr_err(DRVSTR ": tty preinit - not enough memory (%d)\n", rc);
 		return rc;
 	}
 
@@ -468,7 +454,8 @@ static int dgnc_found_board(struct pci_dev *pdev, int id)
 		brd->membase = pci_resource_start(pdev, 4);
 
 		if (!brd->membase) {
-			APR(("card has no PCI IO resources, failing board.\n"));
+			dev_err(&brd->pdev->dev,
+				"Card has no PCI IO resources, failing.\n");
 			return -ENODEV;
 		}
 
@@ -555,7 +542,8 @@ static int dgnc_found_board(struct pci_dev *pdev, int id)
 		break;
 
 	default:
-		APR(("Did not find any compatible Neo or Classic PCI boards in system.\n"));
+		dev_err(&brd->pdev->dev,
+			"Didn't find any compatible Neo/Classic PCI boards.\n");
 		return -ENXIO;
 
 	}
@@ -567,7 +555,7 @@ static int dgnc_found_board(struct pci_dev *pdev, int id)
 	rc = dgnc_tty_register(brd);
 	if (rc < 0) {
 		dgnc_tty_uninit(brd);
-		APR(("Can't register tty devices (%d)\n", rc));
+		pr_err(DRVSTR ": Can't register tty devices (%d)\n", rc);
 		brd->state = BOARD_FAILED;
 		brd->dpastatus = BD_NOFEP;
 		goto failed;
@@ -575,7 +563,7 @@ static int dgnc_found_board(struct pci_dev *pdev, int id)
 
 	rc = dgnc_finalize_board_init(brd);
 	if (rc < 0) {
-		APR(("Can't finalize board init (%d)\n", rc));
+		pr_err(DRVSTR ": Can't finalize board init (%d)\n", rc);
 		brd->state = BOARD_FAILED;
 		brd->dpastatus = BD_NOFEP;
 
@@ -585,7 +573,7 @@ static int dgnc_found_board(struct pci_dev *pdev, int id)
 	rc = dgnc_tty_init(brd);
 	if (rc < 0) {
 		dgnc_tty_uninit(brd);
-		APR(("Can't init tty devices (%d)\n", rc));
+		pr_err(DRVSTR ": Can't init tty devices (%d)\n", rc);
 		brd->state = BOARD_FAILED;
 		brd->dpastatus = BD_NOFEP;
 
@@ -744,9 +732,6 @@ static void dgnc_init_globals(void)
 {
 	int i = 0;
 
-	dgnc_rawreadok		= rawreadok;
-	dgnc_trcbuf_size	= trcbuf_size;
-	dgnc_debug		= debug;
 	dgnc_NumBoards		= 0;
 
 	for (i = 0; i < MAXBOARDS; i++)
diff --git a/drivers/staging/dgnc/dgnc_driver.h b/drivers/staging/dgnc/dgnc_driver.h
index f901957c757f..a8157eba28da 100644
--- a/drivers/staging/dgnc/dgnc_driver.h
+++ b/drivers/staging/dgnc/dgnc_driver.h
@@ -42,53 +42,13 @@
  *
  *************************************************************************/
 
-/*
- * Driver identification, error and debugging statments
- *
- * In theory, you can change all occurrences of "digi" in the next
- * three lines, and the driver printk's will all automagically change.
- *
- * APR((fmt, args, ...));	Always prints message
- */
+/* Driver identification and error statments */
 #define	PROCSTR		"dgnc"			/* /proc entries	 */
 #define	DEVSTR		"/dev/dg/dgnc"		/* /dev entries		 */
-#define	DRVSTR		"dgnc"			/* Driver name string
-						 * displayed by APR	 */
-#define	APR(args)	do { printk(DRVSTR": "); printk args; \
-			   } while (0)
-#define	RAPR(args)	do { printk args; } while (0)
+#define	DRVSTR		"dgnc"			/* Driver name string	 */
 
 #define TRC_TO_CONSOLE 1
 
-/*
- * Debugging levels can be set using debug insmod variable
- * They can also be compiled out completely.
- */
-
-#define	DBG_INIT		(dgnc_debug & 0x01)
-#define	DBG_BASIC		(dgnc_debug & 0x02)
-#define	DBG_CORE		(dgnc_debug & 0x04)
-
-#define	DBG_OPEN		(dgnc_debug & 0x08)
-#define	DBG_CLOSE		(dgnc_debug & 0x10)
-#define	DBG_READ		(dgnc_debug & 0x20)
-#define	DBG_WRITE		(dgnc_debug & 0x40)
-
-#define	DBG_IOCTL		(dgnc_debug & 0x80)
-
-#define	DBG_PROC		(dgnc_debug & 0x100)
-#define	DBG_PARAM		(dgnc_debug & 0x200)
-#define	DBG_PSCAN		(dgnc_debug & 0x400)
-#define	DBG_EVENT		(dgnc_debug & 0x800)
-
-#define	DBG_DRAIN		(dgnc_debug & 0x1000)
-#define	DBG_MSIGS		(dgnc_debug & 0x2000)
-
-#define	DBG_MGMT		(dgnc_debug & 0x4000)
-#define	DBG_INTR		(dgnc_debug & 0x8000)
-
-#define	DBG_CARR		(dgnc_debug & 0x10000)
-
 /* Number of boards we support at once. */
 #define	MAXBOARDS	20
 #define	MAXPORTS	8
@@ -134,8 +94,6 @@
 #define   _POSIX_VDISABLE '\0'
 #endif
 
-#define SNIFF_MAX	65536		/* Sniff buffer size (2^n) */
-#define SNIFF_MASK	(SNIFF_MAX - 1)	/* Sniff wrap mask */
 
 /*
  * All the possible states the driver can be while being loaded.
@@ -342,13 +300,6 @@ struct un_t {
 #define CH_FORCED_STOP  0x20000		/* Output is forcibly stopped	*/
 #define CH_FORCED_STOPI 0x40000		/* Input is forcibly stopped	*/
 
-/*
- * Definitions for ch_sniff_flags
- */
-#define SNIFF_OPEN	0x1
-#define SNIFF_WAIT_DATA	0x2
-#define SNIFF_WAIT_SPACE 0x4
-
 
 /* Our Read/Error/Write queue sizes */
 #define RQUEUEMASK	0x1FFF		/* 8 K - 1 */
@@ -442,21 +393,13 @@ struct channel_t {
 	struct proc_dir_entry *proc_entry_pointer;
 	struct dgnc_proc_entry *dgnc_channel_table;
 
-	uint ch_sniff_in;
-	uint ch_sniff_out;
-	char *ch_sniff_buf;		/* Sniff buffer for proc */
-	ulong ch_sniff_flags;		/* Channel flags		*/
-	wait_queue_head_t ch_sniff_wait;
 };
 
 /*
  * Our Global Variables.
  */
 extern uint		dgnc_Major;		/* Our driver/mgmt major	*/
-extern int		dgnc_debug;		/* Debug variable		*/
-extern int		dgnc_rawreadok;		/* Set if user wants rawreads	*/
 extern int		dgnc_poll_tick;		/* Poll interval - 20 ms	*/
-extern int		dgnc_trcbuf_size;	/* Size of the ringbuffer	*/
 extern spinlock_t	dgnc_global_lock;	/* Driver global spinlock	*/
 extern uint		dgnc_NumBoards;		/* Total number of boards	*/
 extern struct dgnc_board	*dgnc_Board[MAXBOARDS];	/* Array of board structs	*/
diff --git a/drivers/staging/dgnc/dgnc_kcompat.h b/drivers/staging/dgnc/dgnc_kcompat.h
index eaec7e6a28e1..566cad0d33e7 100644
--- a/drivers/staging/dgnc/dgnc_kcompat.h
+++ b/drivers/staging/dgnc/dgnc_kcompat.h
@@ -43,22 +43,4 @@
 # endif
 
 
-#  define PARM_STR(VAR, INIT, PERM, DESC) \
-		static char *VAR = INIT; \
-		char *dgnc_##VAR; \
-		module_param(VAR, charp, PERM); \
-		MODULE_PARM_DESC(VAR, DESC);
-
-#  define PARM_INT(VAR, INIT, PERM, DESC) \
-		static int VAR = INIT; \
-		int dgnc_##VAR; \
-		module_param(VAR, int, PERM); \
-		MODULE_PARM_DESC(VAR, DESC);
-
-#  define PARM_ULONG(VAR, INIT, PERM, DESC) \
-		static ulong VAR = INIT; \
-		ulong dgnc_##VAR; \
-		module_param(VAR, long, PERM); \
-		MODULE_PARM_DESC(VAR, DESC);
-
 #endif /* ! __DGNC_KCOMPAT_H */
diff --git a/drivers/staging/dgnc/dgnc_neo.c b/drivers/staging/dgnc/dgnc_neo.c
index a5bd08fef270..c9a8a9825cfb 100644
--- a/drivers/staging/dgnc/dgnc_neo.c
+++ b/drivers/staging/dgnc/dgnc_neo.c
@@ -530,10 +530,11 @@ static inline void neo_parse_lsr(struct dgnc_board *brd, uint port)
 	int linestatus;
 	unsigned long flags;
 
-	if (!brd)
-		return;
-
-	if (brd->magic != DGNC_BOARD_MAGIC)
+	/*
+	 * Check to make sure it didn't receive interrupt with a null board
+	 * associated or a board pointer that wasn't ours.
+	 */
+	if (!brd || brd->magic != DGNC_BOARD_MAGIC)
 		return;
 
 	if (port > brd->maxports)
@@ -869,10 +870,8 @@ static void neo_tasklet(unsigned long data)
 	int state = 0;
 	int ports = 0;
 
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC) {
-		APR(("poll_tasklet() - NULL or bad bd.\n"));
+	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
 		return;
-	}
 
 	/* Cache a couple board values */
 	spin_lock_irqsave(&bd->bd_lock, flags);
@@ -945,7 +944,7 @@ static void neo_tasklet(unsigned long data)
  */
 static irqreturn_t neo_intr(int irq, void *voidbrd)
 {
-	struct dgnc_board *brd = (struct dgnc_board *) voidbrd;
+	struct dgnc_board *brd = voidbrd;
 	struct channel_t *ch;
 	int port = 0;
 	int type = 0;
@@ -955,18 +954,12 @@ static irqreturn_t neo_intr(int irq, void *voidbrd)
 	unsigned long flags;
 	unsigned long flags2;
 
-	if (!brd) {
-		APR(("Received interrupt (%d) with null board associated\n", irq));
-		return IRQ_NONE;
-	}
-
 	/*
-	 * Check to make sure its for us.
+	 * Check to make sure it didn't receive interrupt with a null board
+	 * associated or a board pointer that wasn't ours.
 	 */
-	if (brd->magic != DGNC_BOARD_MAGIC) {
-		APR(("Received interrupt (%d) with a board pointer that wasn't ours!\n", irq));
+	if (!brd || brd->magic != DGNC_BOARD_MAGIC)
 		return IRQ_NONE;
-	}
 
 	brd->intr_count++;
 
@@ -1224,7 +1217,6 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 
 		/* Copy data from uart to the queue */
 		memcpy_fromio(ch->ch_rqueue + head, &ch->ch_neo_uart->txrxburst, n);
-		dgnc_sniff_nowait_nolock(ch, "UART READ", ch->ch_rqueue + head, n);
 
 		/*
 		 * Since RX_FIFO_DATA_ERROR was 0, we are guarenteed
@@ -1310,7 +1302,6 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 
 		memcpy_fromio(ch->ch_rqueue + head, &ch->ch_neo_uart->txrxburst, 1);
 		ch->ch_equeue[head] = (unsigned char) linestatus;
-		dgnc_sniff_nowait_nolock(ch, "UART READ", ch->ch_rqueue + head, 1);
 
 		/* Ditch any remaining linestatus value. */
 		linestatus = 0;
@@ -1563,7 +1554,6 @@ static void neo_copy_data_from_queue_to_uart(struct channel_t *ch)
 		}
 
 		memcpy_toio(&ch->ch_neo_uart->txrxburst, ch->ch_wqueue + tail, s);
-		dgnc_sniff_nowait_nolock(ch, "UART WRITE", ch->ch_wqueue + tail, s);
 
 		/* Add and flip queue if needed */
 		tail = (tail + s) & WQUEUEMASK;
diff --git a/drivers/staging/dgnc/dgnc_sysfs.c b/drivers/staging/dgnc/dgnc_sysfs.c
index 6c3b387622e9..2fd34ca70c59 100644
--- a/drivers/staging/dgnc/dgnc_sysfs.c
+++ b/drivers/staging/dgnc/dgnc_sysfs.c
@@ -63,39 +63,6 @@ static ssize_t dgnc_driver_maxboards_show(struct device_driver *ddp, char *buf)
 }
 static DRIVER_ATTR(maxboards, S_IRUSR, dgnc_driver_maxboards_show, NULL);
 
-static ssize_t dgnc_driver_debug_show(struct device_driver *ddp, char *buf)
-{
-	return snprintf(buf, PAGE_SIZE, "0x%x\n", dgnc_debug);
-}
-
-static ssize_t dgnc_driver_debug_store(struct device_driver *ddp, const char *buf, size_t count)
-{
-	int ret;
-
-	ret = sscanf(buf, "0x%x\n", &dgnc_debug);
-	if (ret != 1)
-		return -EINVAL;
-	return count;
-}
-static DRIVER_ATTR(debug, (S_IRUSR | S_IWUSR), dgnc_driver_debug_show, dgnc_driver_debug_store);
-
-
-static ssize_t dgnc_driver_rawreadok_show(struct device_driver *ddp, char *buf)
-{
-	return snprintf(buf, PAGE_SIZE, "0x%x\n", dgnc_rawreadok);
-}
-
-static ssize_t dgnc_driver_rawreadok_store(struct device_driver *ddp, const char *buf, size_t count)
-{
-	int ret;
-
-	ret = sscanf(buf, "0x%x\n", &dgnc_rawreadok);
-	if (ret != 1)
-		return -EINVAL;
-	return count;
-}
-static DRIVER_ATTR(rawreadok, (S_IRUSR | S_IWUSR), dgnc_driver_rawreadok_show, dgnc_driver_rawreadok_store);
-
 
 static ssize_t dgnc_driver_pollrate_show(struct device_driver *ddp, char *buf)
 {
@@ -122,8 +89,6 @@ void dgnc_create_driver_sysfiles(struct pci_driver *dgnc_driver)
 	rc |= driver_create_file(driverfs, &driver_attr_version);
 	rc |= driver_create_file(driverfs, &driver_attr_boards);
 	rc |= driver_create_file(driverfs, &driver_attr_maxboards);
-	rc |= driver_create_file(driverfs, &driver_attr_debug);
-	rc |= driver_create_file(driverfs, &driver_attr_rawreadok);
 	rc |= driver_create_file(driverfs, &driver_attr_pollrate);
 	if (rc)
 		printk(KERN_ERR "DGNC: sysfs driver_create_file failed!\n");
@@ -137,8 +102,6 @@ void dgnc_remove_driver_sysfiles(struct pci_driver *dgnc_driver)
 	driver_remove_file(driverfs, &driver_attr_version);
 	driver_remove_file(driverfs, &driver_attr_boards);
 	driver_remove_file(driverfs, &driver_attr_maxboards);
-	driver_remove_file(driverfs, &driver_attr_debug);
-	driver_remove_file(driverfs, &driver_attr_rawreadok);
 	driver_remove_file(driverfs, &driver_attr_pollrate);
 }
 
diff --git a/drivers/staging/dgnc/dgnc_tty.c b/drivers/staging/dgnc/dgnc_tty.c
index 03c15069731f..f81a375f8bc1 100644
--- a/drivers/staging/dgnc/dgnc_tty.c
+++ b/drivers/staging/dgnc/dgnc_tty.c
@@ -49,7 +49,6 @@
 #include <linux/delay.h>	/* For udelay */
 #include <linux/uaccess.h>	/* For copy_from_user/copy_to_user */
 #include <linux/pci.h>
-
 #include "dgnc_driver.h"
 #include "dgnc_tty.h"
 #include "dgnc_types.h"
@@ -233,7 +232,8 @@ int dgnc_tty_register(struct dgnc_board *brd)
 		/* Register tty devices */
 		rc = tty_register_driver(&brd->SerialDriver);
 		if (rc < 0) {
-			APR(("Can't register tty device (%d)\n", rc));
+			dev_dbg(&brd->pdev->dev,
+				"Can't register tty device (%d)\n", rc);
 			return rc;
 		}
 		brd->dgnc_Major_Serial_Registered = TRUE;
@@ -281,7 +281,9 @@ int dgnc_tty_register(struct dgnc_board *brd)
 		/* Register Transparent Print devices */
 		rc = tty_register_driver(&brd->PrintDriver);
 		if (rc < 0) {
-			APR(("Can't register Transparent Print device (%d)\n", rc));
+			dev_dbg(&brd->pdev->dev,
+				"Can't register Transparent Print device(%d)\n",
+				rc);
 			return rc;
 		}
 		brd->dgnc_Major_TransparentPrint_Registered = TRUE;
@@ -371,7 +373,6 @@ int dgnc_tty_init(struct dgnc_board *brd)
 		init_waitqueue_head(&ch->ch_flags_wait);
 		init_waitqueue_head(&ch->ch_tun.un_flags_wait);
 		init_waitqueue_head(&ch->ch_pun.un_flags_wait);
-		init_waitqueue_head(&ch->ch_sniff_wait);
 
 		{
 			struct device *classp;
@@ -446,127 +447,6 @@ void dgnc_tty_uninit(struct dgnc_board *brd)
 
 #define TMPBUFLEN (1024)
 
-/*
- * dgnc_sniff - Dump data out to the "sniff" buffer if the
- * proc sniff file is opened...
- */
-void dgnc_sniff_nowait_nolock(struct channel_t *ch, unsigned char *text, unsigned char *buf, int len)
-{
-	struct timeval tv;
-	int n;
-	int r;
-	int nbuf;
-	int i;
-	int tmpbuflen;
-	char *tmpbuf;
-	char *p;
-	int too_much_data;
-
-	tmpbuf = kzalloc(TMPBUFLEN, GFP_ATOMIC);
-	if (!tmpbuf)
-		return;
-	p = tmpbuf;
-
-	/* Leave if sniff not open */
-	if (!(ch->ch_sniff_flags & SNIFF_OPEN))
-		goto exit;
-
-	do_gettimeofday(&tv);
-
-	/* Create our header for data dump */
-	p += sprintf(p, "<%ld %ld><%s><", tv.tv_sec, tv.tv_usec, text);
-	tmpbuflen = p - tmpbuf;
-
-	do {
-		too_much_data = 0;
-
-		for (i = 0; i < len && tmpbuflen < (TMPBUFLEN - 4); i++) {
-			p += sprintf(p, "%02x ", *buf);
-			buf++;
-			tmpbuflen = p - tmpbuf;
-		}
-
-		if (tmpbuflen < (TMPBUFLEN - 4)) {
-			if (i > 0)
-				p += sprintf(p - 1, "%s\n", ">");
-			else
-				p += sprintf(p, "%s\n", ">");
-		} else {
-			too_much_data = 1;
-			len -= i;
-		}
-
-		nbuf = strlen(tmpbuf);
-		p = tmpbuf;
-
-		/*
-		 *  Loop while data remains.
-		 */
-		while (nbuf > 0 && ch->ch_sniff_buf) {
-			/*
-			 *  Determine the amount of available space left in the
-			 *  buffer.  If there's none, wait until some appears.
-			 */
-			n = (ch->ch_sniff_out - ch->ch_sniff_in - 1) & SNIFF_MASK;
-
-			/*
-			 * If there is no space left to write to in our sniff buffer,
-			 * we have no choice but to drop the data.
-			 * We *cannot* sleep here waiting for space, because this
-			 * function was probably called by the interrupt/timer routines!
-			 */
-			if (n == 0)
-				goto exit;
-
-			/*
-			 * Copy as much data as will fit.
-			 */
-
-			if (n > nbuf)
-				n = nbuf;
-
-			r = SNIFF_MAX - ch->ch_sniff_in;
-
-			if (r <= n) {
-				memcpy(ch->ch_sniff_buf + ch->ch_sniff_in, p, r);
-
-				n -= r;
-				ch->ch_sniff_in = 0;
-				p += r;
-				nbuf -= r;
-			}
-
-			memcpy(ch->ch_sniff_buf + ch->ch_sniff_in, p, n);
-
-			ch->ch_sniff_in += n;
-			p += n;
-			nbuf -= n;
-
-			/*
-			 *  Wakeup any thread waiting for data
-			 */
-			if (ch->ch_sniff_flags & SNIFF_WAIT_DATA) {
-				ch->ch_sniff_flags &= ~SNIFF_WAIT_DATA;
-				wake_up_interruptible(&ch->ch_sniff_wait);
-			}
-		}
-
-		/*
-		 * If the user sent us too much data to push into our tmpbuf,
-		 * we need to keep looping around on all the data.
-		 */
-		if (too_much_data) {
-			p = tmpbuf;
-			tmpbuflen = 0;
-		}
-
-	} while (too_much_data);
-
-exit:
-	kfree(tmpbuf);
-}
-
-
 /*=======================================================================
  *
  *	dgnc_wmove - Write data to transmit queue.
@@ -781,8 +661,6 @@ void dgnc_input(struct channel_t *ch)
 			tty_insert_flip_string(tp->port, ch->ch_rqueue + tail, s);
 		}
 
-		dgnc_sniff_nowait_nolock(ch, "USER READ", ch->ch_rqueue + tail, s);
-
 		tail += s;
 		n -= s;
 		/* Flip queue if needed */
@@ -1546,14 +1424,18 @@ static void dgnc_tty_close(struct tty_struct *tty, struct file *file)
 		 * one, we've got real problems, since it means the
 		 * serial port won't be shutdown.
 		 */
-		APR(("tty->count is 1, un open count is %d\n", un->un_open_count));
+		dev_dbg(tty->dev,
+			"tty->count is 1, un open count is %d\n",
+			un->un_open_count);
 		un->un_open_count = 1;
 	}
 
 	if (un->un_open_count)
 		un->un_open_count--;
 	else
-		APR(("bad serial port open count of %d\n", un->un_open_count));
+		dev_dbg(tty->dev,
+			"bad serial port open count of %d\n",
+			un->un_open_count);
 
 	ch->ch_open_count--;
 
@@ -1974,7 +1856,6 @@ static int dgnc_tty_write(struct tty_struct *tty,
 	if (n >= remain) {
 		n -= remain;
 		memcpy(ch->ch_wqueue + head, buf, remain);
-		dgnc_sniff_nowait_nolock(ch, "USER WRITE", ch->ch_wqueue + head, remain);
 		head = 0;
 		buf += remain;
 	}
@@ -1985,7 +1866,6 @@ static int dgnc_tty_write(struct tty_struct *tty,
 		 */
 		remain = n;
 		memcpy(ch->ch_wqueue + head, buf, remain);
-		dgnc_sniff_nowait_nolock(ch, "USER WRITE", ch->ch_wqueue + head, remain);
 		head += remain;
 	}
 
@@ -2325,8 +2205,6 @@ static int dgnc_set_modem_info(struct tty_struct *tty, unsigned int command, uns
 	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
 		return ret;
 
-	ret = 0;
-
 	ret = get_user(arg, value);
 	if (ret)
 		return ret;
@@ -3089,7 +2967,7 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		struct digi_getcounter buf;
 
 		buf.norun = ch->ch_err_overrun;
-		buf.noflow = 0;  	/* The driver doesn't keep this stat */
+		buf.noflow = 0;		/* The driver doesn't keep this stat */
 		buf.nframe = ch->ch_err_frame;
 		buf.nparity = ch->ch_err_parity;
 		buf.nbreak = ch->ch_err_break;
diff --git a/drivers/staging/dgnc/dgnc_tty.h b/drivers/staging/dgnc/dgnc_tty.h
index 58eef257c2ec..3975f0407143 100644
--- a/drivers/staging/dgnc/dgnc_tty.h
+++ b/drivers/staging/dgnc/dgnc_tty.h
@@ -37,6 +37,4 @@ void	dgnc_carrier(struct channel_t *ch);
 void	dgnc_wakeup_writes(struct channel_t *ch);
 void	dgnc_check_queue_flow_control(struct channel_t *ch);
 
-void	dgnc_sniff_nowait_nolock(struct channel_t *ch, unsigned char *text, unsigned char *buf, int nbuf);
-
 #endif
diff --git a/drivers/staging/emxx_udc/emxx_udc.c b/drivers/staging/emxx_udc/emxx_udc.c
index 1ae0013fb118..eb178fcb7954 100644
--- a/drivers/staging/emxx_udc/emxx_udc.c
+++ b/drivers/staging/emxx_udc/emxx_udc.c
@@ -2622,7 +2622,7 @@ static int nbu2ss_ep_enable(
 		return -EINVAL;
 	}
 
-	ep_type = desc->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK;
+	ep_type = usb_endpoint_type(desc);
 	if ((ep_type == USB_ENDPOINT_XFER_CONTROL)
 		|| (ep_type == USB_ENDPOINT_XFER_ISOC)) {
 
@@ -2644,7 +2644,7 @@ static int nbu2ss_ep_enable(
 	spin_lock_irqsave(&udc->lock, flags);
 
 	ep->desc = desc;
-	ep->epnum = desc->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK;
+	ep->epnum = usb_endpoint_num(desc);
 	ep->direct = desc->bEndpointAddress & USB_ENDPOINT_DIR_MASK;
 	ep->ep_type = ep_type;
 	ep->wedged = 0;
@@ -2722,8 +2722,7 @@ static void nbu2ss_ep_free_request(
 	if (_req != NULL) {
 		req = container_of(_req, struct nbu2ss_req, req);
 
-		if (req != NULL)
-			kfree(req);
+		kfree(req);
 	}
 }
 
@@ -3491,7 +3490,6 @@ static struct platform_driver udc_driver = {
 	.suspend	= nbu2ss_drv_suspend,
 	.resume		= nbu2ss_drv_resume,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= driver_name,
 	},
 };
diff --git a/drivers/staging/ft1000/ft1000-pcmcia/boot.h b/drivers/staging/ft1000/ft1000-pcmcia/boot.h
index 60c015c1c28a..e4a698528520 100644
--- a/drivers/staging/ft1000/ft1000-pcmcia/boot.h
+++ b/drivers/staging/ft1000/ft1000-pcmcia/boot.h
@@ -1,28 +1,28 @@
 /*---------------------------------------------------------------------------
-   FT1000 driver for Flarion Flash OFDM NIC Device
+  FT1000 driver for Flarion Flash OFDM NIC Device
 
-   Copyright (C) 2002 Flarion Technologies, All rights reserved.
+  Copyright (C) 2002 Flarion Technologies, All rights reserved.
 
-   This program is free software; you can redistribute it and/or modify it
-   under the terms of the GNU General Public License as published by the Free
-   Software Foundation; either version 2 of the License, or (at your option) any
-   later version. This program is distributed in the hope that it will be useful,
-   but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
-   or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
-   more details. You should have received a copy of the GNU General Public
-   License along with this program; if not, write to the
-   Free Software Foundation, Inc., 59 Temple Place -
-   Suite 330, Boston, MA 02111-1307, USA.
+  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.
   ---------------------------------------------------------------------------
 
-   File:         boot.h
+  File:         boot.h
 
-   Description:    boatloader
+  Description:    boatloader
 
-   History:
-   1/11/05    Whc                Ported to Linux.
+  History:
+  1/11/05    Whc                Ported to Linux.
 
----------------------------------------------------------------------------*/
+  ---------------------------------------------------------------------------*/
 #ifndef _BOOTH_
 #define _BOOTH_
 
diff --git a/drivers/staging/ft1000/ft1000-pcmcia/ft1000.h b/drivers/staging/ft1000/ft1000-pcmcia/ft1000.h
index 1d52738fff49..5992670f7747 100644
--- a/drivers/staging/ft1000/ft1000-pcmcia/ft1000.h
+++ b/drivers/staging/ft1000/ft1000-pcmcia/ft1000.h
@@ -1,21 +1,21 @@
 /*---------------------------------------------------------------------------
-   FT1000 driver for Flarion Flash OFDM NIC Device
+  FT1000 driver for Flarion Flash OFDM NIC Device
 
-   Copyright (C) 2002 Flarion Technologies, All rights reserved.
+  Copyright (C) 2002 Flarion Technologies, All rights reserved.
 
-   This program is free software; you can redistribute it and/or modify it
-   under the terms of the GNU General Public License as published by the Free
-   Software Foundation; either version 2 of the License, or (at your option) any
-   later version. This program is distributed in the hope that it will be useful,
-   but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
-   or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
-   more details. You should have received a copy of the GNU General Public
-   License along with this program; if not, write to the
-   Free Software Foundation, Inc., 59 Temple Place -
-   Suite 330, Boston, MA 02111-1307, USA.
----------------------------------------------------------------------------
-   Description:    Common structures and defines
----------------------------------------------------------------------------*/
+  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.
+  ---------------------------------------------------------------------------
+  Description:    Common structures and defines
+  ---------------------------------------------------------------------------*/
 #ifndef _FT1000H_
 #define _FT1000H_
 
diff --git a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_cs.c b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_cs.c
index 1f8b3ca35c69..922478e1cb57 100644
--- a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_cs.c
+++ b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_cs.c
@@ -1,30 +1,30 @@
 /*---------------------------------------------------------------------------
-   FT1000 driver for Flarion Flash OFDM NIC Device
+  FT1000 driver for Flarion Flash OFDM NIC Device
 
-   Copyright (C) 1999 David A. Hinds.  All Rights Reserved.
-   Copyright (C) 2002 Flarion Technologies, All rights reserved.
-   Copyright (C) 2006 Patrik Ostrihon, All rights reserved.
-   Copyright (C) 2006 ProWeb Consulting, a.s, All rights reserved.
+  Copyright (C) 1999 David A. Hinds.  All Rights Reserved.
+  Copyright (C) 2002 Flarion Technologies, All rights reserved.
+  Copyright (C) 2006 Patrik Ostrihon, All rights reserved.
+  Copyright (C) 2006 ProWeb Consulting, a.s, All rights reserved.
 
-   The initial developer of the original code is David A. Hinds
-   <dahinds@users.sourceforge.net>.  Portions created by David A. Hinds.
+  The initial developer of the original code is David A. Hinds
+  <dahinds@users.sourceforge.net>.  Portions created by David A. Hinds.
 
-   This file was modified to support the Flarion Flash OFDM NIC Device
-   by Wai Chan (w.chan@flarion.com).
+  This file was modified to support the Flarion Flash OFDM NIC Device
+  by Wai Chan (w.chan@flarion.com).
 
-   Port for kernel 2.6 created by Patrik Ostrihon (patrik.ostrihon@pwc.sk)
+  Port for kernel 2.6 created by Patrik Ostrihon (patrik.ostrihon@pwc.sk)
 
-   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 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>
@@ -80,11 +80,11 @@ static int ft1000_confcheck(struct pcmcia_device *link, void *priv_data)
 
 /*======================================================================
 
-    ft1000_config() is scheduled to run after a CARD_INSERTION event
-    is received, to configure the PCMCIA socket, and to make the
-    device available to the system.
+  ft1000_config() is scheduled to run after a CARD_INSERTION event
+  is received, to configure the PCMCIA socket, and to make the
+  device available to the system.
 
-======================================================================*/
+  ======================================================================*/
 
 static int ft1000_config(struct pcmcia_device *link)
 {
diff --git a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_dnld.c b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_dnld.c
index c1856f7d1e26..06b0e9cfb9b1 100644
--- a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_dnld.c
+++ b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_dnld.c
@@ -1,24 +1,26 @@
 /*---------------------------------------------------------------------------
-   FT1000 driver for Flarion Flash OFDM NIC Device
-
-   Copyright (C) 2002 Flarion Technologies, All rights reserved.
-
-   This program is free software; you can redistribute it and/or modify it
-   under the terms of the GNU General Public License as published by the Free
-   Software Foundation; either version 2 of the License, or (at your option) any
-   later version. This program is distributed in the hope that it will be useful,
-   but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
-   or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
-   more details. You should have received a copy of the GNU General Public
-   License along with this program; if not, write to the
-   Free Software Foundation, Inc., 59 Temple Place -
-   Suite 330, Boston, MA 02111-1307, USA.
+  FT1000 driver for Flarion Flash OFDM NIC Device
+
+  Copyright (C) 2002 Flarion Technologies, All rights reserved.
+
+  This program is free software; you can redistribute it and/or modify it
+  under the terms of the GNU General Public License as published by the Free
+  Software Foundation; either version 2 of the License, or (at your option) any
+  later version. This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+  or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details. You should have received a copy of the GNU General Public
+  License along with this program; if not, write to the
+  Free Software Foundation, Inc., 59 Temple Place -
+  Suite 330, Boston, MA 02111-1307, USA.
   --------------------------------------------------------------------------
 
-   Description: This module will handshake with the DSP bootloader to
-		download the DSP runtime image.
+  Description: This module will handshake with the DSP bootloader to
+  download the DSP runtime image.
 
----------------------------------------------------------------------------*/
+  ---------------------------------------------------------------------------*/
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #define __KERNEL_SYSCALLS__
 
@@ -99,7 +101,7 @@ struct dsp_file_hdr {
 	u32  version_data_offset;	/* Offset were scrambled version data begins. */
 	u32  version_data_size;	/* Size, in words, of scrambled version data. */
 	u32  nDspImages;	/* Number of DSP images in file. */
-} __attribute__ ((packed));
+} __packed;
 
 struct dsp_image_info {
 	u32  coff_date;		/* Date/time when DSP Coff image was built. */
@@ -110,11 +112,11 @@ struct dsp_image_info {
 	u32  version;		/* Embedded version # of DSP code. */
 	unsigned short checksum;	/* Dsp File checksum */
 	unsigned short pad1;
-} __attribute__ ((packed));
+} __packed;
 
 void card_bootload(struct net_device *dev)
 {
-	struct ft1000_info *info = (struct ft1000_info *) netdev_priv(dev);
+	struct ft1000_info *info = (struct ft1000_info *)netdev_priv(dev);
 	unsigned long flags;
 	u32 *pdata;
 	u32 size;
@@ -123,7 +125,7 @@ void card_bootload(struct net_device *dev)
 
 	netdev_dbg(dev, "card_bootload is called\n");
 
-	pdata = (u32 *) bootimage;
+	pdata = (u32 *)bootimage;
 	size = sizeof(bootimage);
 
 	/* check for odd word */
@@ -146,7 +148,7 @@ void card_bootload(struct net_device *dev)
 
 u16 get_handshake(struct net_device *dev, u16 expected_value)
 {
-	struct ft1000_info *info = (struct ft1000_info *) netdev_priv(dev);
+	struct ft1000_info *info = (struct ft1000_info *)netdev_priv(dev);
 	u16 handshake;
 	u32 tempx;
 	int loopcnt;
@@ -161,12 +163,12 @@ u16 get_handshake(struct net_device *dev, u16 expected_value)
 		} else {
 			tempx =
 				ntohl(ft1000_read_dpram_mag_32
-				  (dev, DWNLD_MAG_HANDSHAKE_LOC));
-			handshake = (u16) tempx;
+				      (dev, DWNLD_MAG_HANDSHAKE_LOC));
+			handshake = (u16)tempx;
 		}
 
 		if ((handshake == expected_value)
-			|| (handshake == HANDSHAKE_RESET_VALUE)) {
+		    || (handshake == HANDSHAKE_RESET_VALUE)) {
 			return handshake;
 		}
 		loopcnt++;
@@ -180,7 +182,7 @@ u16 get_handshake(struct net_device *dev, u16 expected_value)
 
 void put_handshake(struct net_device *dev, u16 handshake_value)
 {
-	struct ft1000_info *info = (struct ft1000_info *) netdev_priv(dev);
+	struct ft1000_info *info = (struct ft1000_info *)netdev_priv(dev);
 	u32 tempx;
 
 	if (info->AsicID == ELECTRABUZZ_ID) {
@@ -188,7 +190,7 @@ void put_handshake(struct net_device *dev, u16 handshake_value)
 				 DWNLD_HANDSHAKE_LOC);
 		ft1000_write_reg(dev, FT1000_REG_DPRAM_DATA, handshake_value);	/* Handshake */
 	} else {
-		tempx = (u32) handshake_value;
+		tempx = (u32)handshake_value;
 		tempx = ntohl(tempx);
 		ft1000_write_dpram_mag_32(dev, DWNLD_MAG_HANDSHAKE_LOC, tempx);	/* Handshake */
 	}
@@ -196,7 +198,7 @@ void put_handshake(struct net_device *dev, u16 handshake_value)
 
 u16 get_request_type(struct net_device *dev)
 {
-	struct ft1000_info *info = (struct ft1000_info *) netdev_priv(dev);
+	struct ft1000_info *info = (struct ft1000_info *)netdev_priv(dev);
 	u16 request_type;
 	u32 tempx;
 
@@ -206,7 +208,7 @@ u16 get_request_type(struct net_device *dev)
 	} else {
 		tempx = ft1000_read_dpram_mag_32(dev, DWNLD_MAG_TYPE_LOC);
 		tempx = ntohl(tempx);
-		request_type = (u16) tempx;
+		request_type = (u16)tempx;
 	}
 
 	return request_type;
@@ -215,7 +217,7 @@ u16 get_request_type(struct net_device *dev)
 
 long get_request_value(struct net_device *dev)
 {
-	struct ft1000_info *info = (struct ft1000_info *) netdev_priv(dev);
+	struct ft1000_info *info = (struct ft1000_info *)netdev_priv(dev);
 	long value;
 	u16 w_val;
 
@@ -244,7 +246,7 @@ long get_request_value(struct net_device *dev)
 
 void put_request_value(struct net_device *dev, long lvalue)
 {
-	struct ft1000_info *info = (struct ft1000_info *) netdev_priv(dev);
+	struct ft1000_info *info = (struct ft1000_info *)netdev_priv(dev);
 	u16 size;
 	u32 tempx;
 
@@ -271,11 +273,11 @@ void put_request_value(struct net_device *dev, long lvalue)
 
 u16 hdr_checksum(struct pseudo_hdr *pHdr)
 {
-	u16 *usPtr = (u16 *) pHdr;
+	u16 *usPtr = (u16 *)pHdr;
 	u16 chksum;
 
 	chksum = ((((((usPtr[0] ^ usPtr[1]) ^ usPtr[2]) ^ usPtr[3]) ^
-			usPtr[4]) ^ usPtr[5]) ^ usPtr[6]);
+		    usPtr[4]) ^ usPtr[5]) ^ usPtr[6]);
 
 	return chksum;
 }
@@ -283,7 +285,7 @@ u16 hdr_checksum(struct pseudo_hdr *pHdr)
 int card_download(struct net_device *dev, const u8 *pFileStart,
 		  size_t FileLength)
 {
-	struct ft1000_info *info = (struct ft1000_info *) netdev_priv(dev);
+	struct ft1000_info *info = (struct ft1000_info *)netdev_priv(dev);
 	int Status = SUCCESS;
 	u32 uiState;
 	u16 handshake;
@@ -316,13 +318,13 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 
 	file_version = *(long *)pFileStart;
 	if (file_version != 6) {
-		printk(KERN_ERR "ft1000: unsupported firmware version %ld\n", file_version);
+		pr_err("unsupported firmware version %ld\n", file_version);
 		Status = FAILURE;
 	}
 
 	uiState = STATE_START_DWNLD;
 
-	pFileHdr5 = (struct dsp_file_hdr *) pFileStart;
+	pFileHdr5 = (struct dsp_file_hdr *)pFileStart;
 
 	pUsFile = (u16 *) ((long)pFileStart + pFileHdr5->loader_offset);
 	pUcFile = (u8 *) ((long)pFileStart + pFileHdr5->loader_offset);
@@ -376,7 +378,7 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 						break;
 					}
 					if ((word_length * 2 + (long)pUcFile) >
-						(long)pBootEnd) {
+					    (long)pBootEnd) {
 						/*
 						 * Error, beyond boot code range.
 						 */
@@ -390,8 +392,8 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 					 * Position ASIC DPRAM auto-increment pointer.
 					 */
 					outw(DWNLD_MAG_PS_HDR_LOC,
-						 dev->base_addr +
-						 FT1000_REG_DPRAM_ADDR);
+					     dev->base_addr +
+					     FT1000_REG_DPRAM_ADDR);
 					if (word_length & 0x01)
 						word_length++;
 					word_length = word_length / 2;
@@ -402,12 +404,12 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 							(*pUsFile++ << 16);
 						pUcFile += 4;
 						outl(templong,
-							 dev->base_addr +
-							 FT1000_REG_MAG_DPDATAL);
+						     dev->base_addr +
+						     FT1000_REG_MAG_DPDATAL);
 					}
 					spin_unlock_irqrestore(&info->
-								   dpram_lock,
-								   flags);
+							       dpram_lock,
+							       flags);
 					break;
 				default:
 					Status = FAILURE;
@@ -430,7 +432,7 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 				switch (request) {
 				case REQUEST_FILE_CHECKSUM:
 					netdev_dbg(dev,
-						  "ft1000_dnld: REQUEST_FOR_CHECKSUM\n");
+						   "ft1000_dnld: REQUEST_FOR_CHECKSUM\n");
 					put_request_value(dev, image_chksum);
 					break;
 				case REQUEST_RUN_ADDRESS:
@@ -468,7 +470,7 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 						break;
 					}
 					if ((word_length * 2 + (long)pUcFile) >
-						(long)pCodeEnd) {
+					    (long)pCodeEnd) {
 						/*
 						 * Error, beyond boot code range.
 						 */
@@ -479,8 +481,8 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 					 * Position ASIC DPRAM auto-increment pointer.
 					 */
 					outw(DWNLD_MAG_PS_HDR_LOC,
-						 dev->base_addr +
-						 FT1000_REG_DPRAM_ADDR);
+					     dev->base_addr +
+					     FT1000_REG_DPRAM_ADDR);
 					if (word_length & 0x01)
 						word_length++;
 					word_length = word_length / 2;
@@ -491,8 +493,8 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 							(*pUsFile++ << 16);
 						pUcFile += 4;
 						outl(templong,
-							 dev->base_addr +
-							 FT1000_REG_MAG_DPDATAL);
+						     dev->base_addr +
+						     FT1000_REG_MAG_DPDATAL);
 					}
 					break;
 
@@ -502,9 +504,9 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 						(long)(info->DSPInfoBlklen + 1) / 2;
 					put_request_value(dev, word_length);
 					pMailBoxData =
-						(struct drv_msg *) &info->DSPInfoBlk[0];
+						(struct drv_msg *)&info->DSPInfoBlk[0];
 					pUsData =
-						(u16 *) &pMailBoxData->data[0];
+						(u16 *)&pMailBoxData->data[0];
 					/* Provide mutual exclusive access while reading ASIC registers. */
 					spin_lock_irqsave(&info->dpram_lock,
 							  flags);
@@ -528,8 +530,8 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 						 * Position ASIC DPRAM auto-increment pointer.
 						 */
 						outw(DWNLD_MAG_PS_HDR_LOC,
-							 dev->base_addr +
-							 FT1000_REG_DPRAM_ADDR);
+						     dev->base_addr +
+						     FT1000_REG_DPRAM_ADDR);
 						if (word_length & 0x01)
 							word_length++;
 
@@ -540,13 +542,13 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 							templong |=
 								(*pUsData++ << 16);
 							outl(templong,
-								 dev->base_addr +
-								 FT1000_REG_MAG_DPDATAL);
+							     dev->base_addr +
+							     FT1000_REG_MAG_DPDATAL);
 						}
 					}
 					spin_unlock_irqrestore(&info->
-								   dpram_lock,
-								   flags);
+							       dpram_lock,
+							       flags);
 					break;
 
 				case REQUEST_VERSION_INFO:
@@ -555,8 +557,8 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 					put_request_value(dev, word_length);
 					pUsFile =
 						(u16 *) ((long)pFileStart +
-							pFileHdr5->
-							version_data_offset);
+							 pFileHdr5->
+							 version_data_offset);
 					/* Provide mutual exclusive access while reading ASIC registers. */
 					spin_lock_irqsave(&info->dpram_lock,
 							  flags);
@@ -564,8 +566,8 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 					 * Position ASIC DPRAM auto-increment pointer.
 					 */
 					outw(DWNLD_MAG_PS_HDR_LOC,
-						 dev->base_addr +
-						 FT1000_REG_DPRAM_ADDR);
+					     dev->base_addr +
+					     FT1000_REG_DPRAM_ADDR);
 					if (word_length & 0x01)
 						word_length++;
 					word_length = word_length / 2;
@@ -578,12 +580,12 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 						templong |=
 							(temp << 16);
 						outl(templong,
-							 dev->base_addr +
-							 FT1000_REG_MAG_DPDATAL);
+						     dev->base_addr +
+						     FT1000_REG_MAG_DPDATAL);
 					}
 					spin_unlock_irqrestore(&info->
-								   dpram_lock,
-								   flags);
+							       dpram_lock,
+							       flags);
 					break;
 
 				case REQUEST_CODE_BY_VERSION:
@@ -592,14 +594,14 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 						get_request_value(dev);
 					pDspImageInfoV6 =
 						(struct dsp_image_info *) ((long)
-								  pFileStart
-								  +
-								  sizeof
-								  (struct dsp_file_hdr));
+									   pFileStart
+									   +
+									   sizeof
+									   (struct dsp_file_hdr));
 					for (imageN = 0;
-						 imageN <
-						 pFileHdr5->nDspImages;
-						 imageN++) {
+					     imageN <
+						     pFileHdr5->nDspImages;
+					     imageN++) {
 						temp = (u16)
 							(pDspImageInfoV6->
 							 version);
@@ -610,30 +612,30 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 						templong |=
 							(temp << 16);
 						if (templong ==
-							requested_version) {
+						    requested_version) {
 							bGoodVersion =
 								true;
 							pUsFile =
 								(u16
 								 *) ((long)
-								 pFileStart
-								 +
-								 pDspImageInfoV6->
-								 begin_offset);
+								     pFileStart
+								     +
+								     pDspImageInfoV6->
+								     begin_offset);
 							pUcFile =
 								(u8
 								 *) ((long)
-								 pFileStart
-								 +
-								 pDspImageInfoV6->
-								 begin_offset);
+								     pFileStart
+								     +
+								     pDspImageInfoV6->
+								     begin_offset);
 							pCodeEnd =
 								(u8
 								 *) ((long)
-								 pFileStart
-								 +
-								 pDspImageInfoV6->
-								 end_offset);
+								     pFileStart
+								     +
+								     pDspImageInfoV6->
+								     end_offset);
 							run_address =
 								pDspImageInfoV6->
 								run_address;
@@ -645,10 +647,10 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 								pDspImageInfoV6->
 								checksum;
 							netdev_dbg(dev,
-								  "ft1000_dnld: image_chksum = 0x%8x\n",
-								  (unsigned
-								   int)
-								  image_chksum);
+								   "ft1000_dnld: image_chksum = 0x%8x\n",
+								   (unsigned
+								    int)
+								   image_chksum);
 							break;
 						}
 						pDspImageInfoV6++;
@@ -674,25 +676,25 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 			break;
 
 		case STATE_DONE_DWNLD:
-			if (((unsigned long) (pUcFile) - (unsigned long) pFileStart) >=
-				(unsigned long) FileLength) {
+			if (((unsigned long)(pUcFile) - (unsigned long) pFileStart) >=
+			    (unsigned long)FileLength) {
 				uiState = STATE_DONE_FILE;
 				break;
 			}
 
-			pHdr = (struct pseudo_hdr *) pUsFile;
+			pHdr = (struct pseudo_hdr *)pUsFile;
 
 			if (pHdr->portdest == 0x80	/* DspOAM */
-				&& (pHdr->portsrc == 0x00	/* Driver */
+			    && (pHdr->portsrc == 0x00	/* Driver */
 				|| pHdr->portsrc == 0x10 /* FMM */)) {
 				uiState = STATE_SECTION_PROV;
 			} else {
 				netdev_dbg(dev,
-					  "FT1000:download:Download error: Bad Port IDs in Pseudo Record\n");
+					   "Download error: Bad Port IDs in Pseudo Record\n");
 				netdev_dbg(dev, "\t Port Source = 0x%2.2x\n",
-					  pHdr->portsrc);
+					   pHdr->portsrc);
 				netdev_dbg(dev, "\t Port Destination = 0x%2.2x\n",
-					  pHdr->portdest);
+					   pHdr->portdest);
 				Status = FAILURE;
 			}
 
@@ -700,7 +702,7 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 
 		case STATE_SECTION_PROV:
 
-			pHdr = (struct pseudo_hdr *) pUcFile;
+			pHdr = (struct pseudo_hdr *)pUcFile;
 
 			if (pHdr->checksum == hdr_checksum(pHdr)) {
 				if (pHdr->portdest != 0x80 /* Dsp OAM */) {
@@ -715,8 +717,8 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 						GFP_ATOMIC);
 				if (pbuffer) {
 					memcpy(pbuffer, (void *)pUcFile,
-						   (u32) (usHdrLength +
-							   sizeof(struct pseudo_hdr)));
+					       (u32) (usHdrLength +
+						      sizeof(struct pseudo_hdr)));
 					/* link provisioning data */
 					pprov_record =
 						kmalloc(sizeof(struct prov_record),
@@ -725,15 +727,15 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 						pprov_record->pprov_data =
 							pbuffer;
 						list_add_tail(&pprov_record->
-								  list,
-								  &info->prov_list);
+							      list,
+							      &info->prov_list);
 						/* Move to next entry if available */
 						pUcFile =
-							(u8 *) ((unsigned long) pUcFile +
-								   (unsigned long) ((usHdrLength + 1) & 0xFFFFFFFE) + sizeof(struct pseudo_hdr));
+							(u8 *)((unsigned long) pUcFile +
+							       (unsigned long) ((usHdrLength + 1) & 0xFFFFFFFE) + sizeof(struct pseudo_hdr));
 						if ((unsigned long) (pUcFile) -
-							(unsigned long) (pFileStart) >=
-							(unsigned long) FileLength) {
+						    (unsigned long) (pFileStart) >=
+						    (unsigned long)FileLength) {
 							uiState =
 								STATE_DONE_FILE;
 						}
diff --git a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_hw.c b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_hw.c
index 44575c78cf0c..d5475b7270a8 100644
--- a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_hw.c
+++ b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_hw.c
@@ -1,22 +1,24 @@
 /*---------------------------------------------------------------------------
-   FT1000 driver for Flarion Flash OFDM NIC Device
-
-   Copyright (C) 2002 Flarion Technologies, All rights reserved.
-   Copyright (C) 2006 Patrik Ostrihon, All rights reserved.
-   Copyright (C) 2006 ProWeb Consulting, a.s, All rights reserved.
-
-   This program is free software; you can redistribute it and/or modify it
-   under the terms of the GNU General Public License as published by the Free
-   Software Foundation; either version 2 of the License, or (at your option) any
-   later version. This program is distributed in the hope that it will be useful,
-   but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
-   or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
-   more details. You should have received a copy of the GNU General Public
-   License along with this program; if not, write to the
-   Free Software Foundation, Inc., 59 Temple Place -
-   Suite 330, Boston, MA 02111-1307, USA.
+  FT1000 driver for Flarion Flash OFDM NIC Device
+
+  Copyright (C) 2002 Flarion Technologies, All rights reserved.
+  Copyright (C) 2006 Patrik Ostrihon, All rights reserved.
+  Copyright (C) 2006 ProWeb Consulting, a.s, All rights reserved.
+
+  This program is free software; you can redistribute it and/or modify it
+  under the terms of the GNU General Public License as published by the Free
+  Software Foundation; either version 2 of the License, or (at your option) any
+  later version. This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+  or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details. You should have received a copy of the GNU General Public
+  License along with this program; if not, write to the
+  Free Software Foundation, Inc., 59 Temple Place -
+  Suite 330, Boston, MA 02111-1307, USA.
   -------------------------------------------------------------------------*/
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/sched.h>
@@ -44,12 +46,6 @@
 #include <pcmcia/cisreg.h>
 #include <pcmcia/ds.h>
 
-#ifdef FT_DEBUG
-#define DEBUG(n, args...) printk(KERN_DEBUG args);
-#else
-#define DEBUG(n, args...)
-#endif
-
 #include <linux/delay.h>
 #include "ft1000.h"
 
@@ -57,7 +53,7 @@ static const struct firmware *fw_entry;
 
 static void ft1000_hbchk(u_long data);
 static struct timer_list poll_timer = {
-      .function = ft1000_hbchk
+	.function = ft1000_hbchk
 };
 
 static u16 cmdbuffer[1024];
@@ -72,7 +68,7 @@ static void ft1000_disable_interrupts(struct net_device *dev);
 /* new kernel */
 MODULE_AUTHOR("");
 MODULE_DESCRIPTION
-    ("Support for Flarion Flash OFDM NIC Device. Support for PCMCIA when used with ft1000_cs.");
+("Support for Flarion Flash OFDM NIC Device. Support for PCMCIA when used with ft1000_cs.");
 MODULE_LICENSE("GPL");
 MODULE_SUPPORTED_DEVICE("FT1000");
 
@@ -80,15 +76,15 @@ MODULE_SUPPORTED_DEVICE("FT1000");
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_read_fifo_len
-   Description: This function will read the ASIC Uplink FIFO status register
-               which will return the number of bytes remaining in the Uplink FIFO.
-               Sixteen bytes are subtracted to make sure that the ASIC does not
-               reach its threshold.
-   Input:
-       dev    - network device structure
-   Output:
-       value  - number of bytes available in the ASIC Uplink FIFO.
+  Function:   ft1000_read_fifo_len
+  Description: This function will read the ASIC Uplink FIFO status register
+  which will return the number of bytes remaining in the Uplink FIFO.
+  Sixteen bytes are subtracted to make sure that the ASIC does not
+  reach its threshold.
+  Input:
+  dev    - network device structure
+  Output:
+  value  - number of bytes available in the ASIC Uplink FIFO.
 
   -------------------------------------------------------------------------*/
 static inline u16 ft1000_read_fifo_len(struct net_device *dev)
@@ -103,14 +99,14 @@ static inline u16 ft1000_read_fifo_len(struct net_device *dev)
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_read_dpram
-   Description: This function will read the specific area of dpram
-               (Electrabuzz ASIC only)
-   Input:
-       dev    - device structure
-       offset - index of dpram
-   Output:
-       value  - value of dpram
+  Function:   ft1000_read_dpram
+  Description: This function will read the specific area of dpram
+  (Electrabuzz ASIC only)
+  Input:
+  dev    - device structure
+  offset - index of dpram
+  Output:
+  value  - value of dpram
 
   -------------------------------------------------------------------------*/
 u16 ft1000_read_dpram(struct net_device *dev, int offset)
@@ -130,19 +126,19 @@ u16 ft1000_read_dpram(struct net_device *dev, int offset)
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_write_dpram
-   Description: This function will write to a specific area of dpram
-               (Electrabuzz ASIC only)
-   Input:
-       dev    - device structure
-       offset - index of dpram
-       value  - value to write
-   Output:
-       none.
+  Function:   ft1000_write_dpram
+  Description: This function will write to a specific area of dpram
+  (Electrabuzz ASIC only)
+  Input:
+  dev    - device structure
+  offset - index of dpram
+  value  - value to write
+  Output:
+  none.
 
   -------------------------------------------------------------------------*/
 static inline void ft1000_write_dpram(struct net_device *dev,
-					  int offset, u16 value)
+				      int offset, u16 value)
 {
 	struct ft1000_info *info = netdev_priv(dev);
 	unsigned long flags;
@@ -156,14 +152,14 @@ static inline void ft1000_write_dpram(struct net_device *dev,
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_read_dpram_mag_16
-   Description: This function will read the specific area of dpram
-               (Magnemite ASIC only)
-   Input:
-       dev    - device structure
-       offset - index of dpram
-   Output:
-       value  - value of dpram
+  Function:   ft1000_read_dpram_mag_16
+  Description: This function will read the specific area of dpram
+  (Magnemite ASIC only)
+  Input:
+  dev    - device structure
+  offset - index of dpram
+  Output:
+  value  - value of dpram
 
   -------------------------------------------------------------------------*/
 u16 ft1000_read_dpram_mag_16(struct net_device *dev, int offset, int Index)
@@ -188,19 +184,19 @@ u16 ft1000_read_dpram_mag_16(struct net_device *dev, int offset, int Index)
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_write_dpram_mag_16
-   Description: This function will write to a specific area of dpram
-               (Magnemite ASIC only)
-   Input:
-       dev    - device structure
-       offset - index of dpram
-       value  - value to write
-   Output:
-       none.
+  Function:   ft1000_write_dpram_mag_16
+  Description: This function will write to a specific area of dpram
+  (Magnemite ASIC only)
+  Input:
+  dev    - device structure
+  offset - index of dpram
+  value  - value to write
+  Output:
+  none.
 
   -------------------------------------------------------------------------*/
 static inline void ft1000_write_dpram_mag_16(struct net_device *dev,
-						 int offset, u16 value, int Index)
+					     int offset, u16 value, int Index)
 {
 	struct ft1000_info *info = netdev_priv(dev);
 	unsigned long flags;
@@ -218,14 +214,14 @@ static inline void ft1000_write_dpram_mag_16(struct net_device *dev,
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_read_dpram_mag_32
-   Description: This function will read the specific area of dpram
-               (Magnemite ASIC only)
-   Input:
-       dev    - device structure
-       offset - index of dpram
-   Output:
-       value  - value of dpram
+  Function:   ft1000_read_dpram_mag_32
+  Description: This function will read the specific area of dpram
+  (Magnemite ASIC only)
+  Input:
+  dev    - device structure
+  offset - index of dpram
+  Output:
+  value  - value of dpram
 
   -------------------------------------------------------------------------*/
 u32 ft1000_read_dpram_mag_32(struct net_device *dev, int offset)
@@ -245,15 +241,15 @@ u32 ft1000_read_dpram_mag_32(struct net_device *dev, int offset)
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_write_dpram_mag_32
-   Description: This function will write to a specific area of dpram
-               (Magnemite ASIC only)
-   Input:
-       dev    - device structure
-       offset - index of dpram
-       value  - value to write
-   Output:
-       none.
+  Function:   ft1000_write_dpram_mag_32
+  Description: This function will write to a specific area of dpram
+  (Magnemite ASIC only)
+  Input:
+  dev    - device structure
+  offset - index of dpram
+  value  - value to write
+  Output:
+  none.
 
   -------------------------------------------------------------------------*/
 void ft1000_write_dpram_mag_32(struct net_device *dev, int offset, u32 value)
@@ -270,57 +266,51 @@ void ft1000_write_dpram_mag_32(struct net_device *dev, int offset, u32 value)
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_enable_interrupts
-   Description: This function will enable interrupts base on the current interrupt mask.
-   Input:
-       dev    - device structure
-   Output:
-       None.
+  Function:   ft1000_enable_interrupts
+  Description: This function will enable interrupts base on the current interrupt mask.
+  Input:
+  dev    - device structure
+  Output:
+  None.
 
   -------------------------------------------------------------------------*/
 static void ft1000_enable_interrupts(struct net_device *dev)
 {
 	u16 tempword;
 
-	DEBUG(1, "ft1000_hw:ft1000_enable_interrupts()\n");
 	ft1000_write_reg(dev, FT1000_REG_SUP_IMASK, ISR_DEFAULT_MASK);
 	tempword = ft1000_read_reg(dev, FT1000_REG_SUP_IMASK);
-	DEBUG(1,
-		  "ft1000_hw:ft1000_enable_interrupts:current interrupt enable mask = 0x%x\n",
-		  tempword);
+	pr_debug("current interrupt enable mask = 0x%x\n", tempword);
 }
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_disable_interrupts
-   Description: This function will disable all interrupts.
-   Input:
-       dev    - device structure
-   Output:
-       None.
+  Function:   ft1000_disable_interrupts
+  Description: This function will disable all interrupts.
+  Input:
+  dev    - device structure
+  Output:
+  None.
 
   -------------------------------------------------------------------------*/
 static void ft1000_disable_interrupts(struct net_device *dev)
 {
 	u16 tempword;
 
-	DEBUG(1, "ft1000_hw: ft1000_disable_interrupts()\n");
 	ft1000_write_reg(dev, FT1000_REG_SUP_IMASK, ISR_MASK_ALL);
 	tempword = ft1000_read_reg(dev, FT1000_REG_SUP_IMASK);
-	DEBUG(1,
-		  "ft1000_hw:ft1000_disable_interrupts:current interrupt enable mask = 0x%x\n",
-		  tempword);
+	pr_debug("current interrupt enable mask = 0x%x\n", tempword);
 }
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_reset_asic
-   Description: This function will call the Card Service function to reset the
-               ASIC.
-   Input:
-       dev    - device structure
-   Output:
-       none
+  Function:   ft1000_reset_asic
+  Description: This function will call the Card Service function to reset the
+  ASIC.
+  Input:
+  dev    - device structure
+  Output:
+  none
 
   -------------------------------------------------------------------------*/
 static void ft1000_reset_asic(struct net_device *dev)
@@ -329,8 +319,6 @@ static void ft1000_reset_asic(struct net_device *dev)
 	struct ft1000_pcmcia *pcmcia = info->priv;
 	u16 tempword;
 
-	DEBUG(1, "ft1000_hw:ft1000_reset_asic called\n");
-
 	(*info->ft1000_reset) (pcmcia->link);
 
 	/*
@@ -351,22 +339,22 @@ static void ft1000_reset_asic(struct net_device *dev)
 	}
 	/* clear interrupts */
 	tempword = ft1000_read_reg(dev, FT1000_REG_SUP_ISR);
-	DEBUG(1, "ft1000_hw: interrupt status register = 0x%x\n", tempword);
+	pr_debug("interrupt status register = 0x%x\n", tempword);
 	ft1000_write_reg(dev, FT1000_REG_SUP_ISR, tempword);
 	tempword = ft1000_read_reg(dev, FT1000_REG_SUP_ISR);
-	DEBUG(1, "ft1000_hw: interrupt status register = 0x%x\n", tempword);
+	pr_debug("interrupt status register = 0x%x\n", tempword);
 
 }
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_reset_card
-   Description: This function will reset the card
-   Input:
-       dev    - device structure
-   Output:
-       status - false (card reset fail)
-                true  (card reset successful)
+  Function:   ft1000_reset_card
+  Description: This function will reset the card
+  Input:
+  dev    - device structure
+  Output:
+  status - false (card reset fail)
+  true  (card reset successful)
 
   -------------------------------------------------------------------------*/
 static int ft1000_reset_card(struct net_device *dev)
@@ -377,8 +365,6 @@ static int ft1000_reset_card(struct net_device *dev)
 	unsigned long flags;
 	struct prov_record *ptr;
 
-	DEBUG(1, "ft1000_hw:ft1000_reset_card called.....\n");
-
 	info->CardReady = 0;
 	info->ProgConStat = 0;
 	info->squeseqnum = 0;
@@ -388,8 +374,7 @@ static int ft1000_reset_card(struct net_device *dev)
 
 	/* Make sure we free any memory reserve for provisioning */
 	while (list_empty(&info->prov_list) == 0) {
-		DEBUG(0,
-			  "ft1000_hw:ft1000_reset_card:deleting provisioning record\n");
+		pr_debug("deleting provisioning record\n");
 		ptr = list_entry(info->prov_list.next, struct prov_record, list);
 		list_del(&ptr->list);
 		kfree(ptr->pprov_data);
@@ -397,11 +382,10 @@ static int ft1000_reset_card(struct net_device *dev)
 	}
 
 	if (info->AsicID == ELECTRABUZZ_ID) {
-		DEBUG(1, "ft1000_hw:ft1000_reset_card:resetting DSP\n");
+		pr_debug("resetting DSP\n");
 		ft1000_write_reg(dev, FT1000_REG_RESET, DSP_RESET_BIT);
 	} else {
-		DEBUG(1,
-			  "ft1000_hw:ft1000_reset_card:resetting ASIC and DSP\n");
+		pr_debug("resetting ASIC and DSP\n");
 		ft1000_write_reg(dev, FT1000_REG_RESET,
 				 (DSP_RESET_BIT | ASIC_RESET_BIT));
 	}
@@ -428,17 +412,16 @@ static int ft1000_reset_card(struct net_device *dev)
 		spin_unlock_irqrestore(&info->dpram_lock, flags);
 	}
 
-	DEBUG(1, "ft1000_hw:ft1000_reset_card:resetting ASIC\n");
+	pr_debug("resetting ASIC\n");
 	mdelay(10);
 	/* reset ASIC */
 	ft1000_reset_asic(dev);
 
-	DEBUG(1, "ft1000_hw:ft1000_reset_card:downloading dsp image\n");
+	pr_debug("downloading dsp image\n");
 
 	if (info->AsicID == MAGNEMITE_ID) {
 		/* Put dsp in reset and take ASIC out of reset */
-		DEBUG(0,
-			  "ft1000_hw:ft1000_reset_card:Put DSP in reset and take ASIC out of reset\n");
+		pr_debug("Put DSP in reset and take ASIC out of reset\n");
 		ft1000_write_reg(dev, FT1000_REG_RESET, DSP_RESET_BIT);
 
 		/* Setting MAGNEMITE ASIC to big endian mode */
@@ -450,7 +433,7 @@ static int ft1000_reset_card(struct net_device *dev)
 		ft1000_write_reg(dev, FT1000_REG_RESET, 0);
 		/* FLARION_DSP_ACTIVE; */
 		mdelay(10);
-		DEBUG(0, "ft1000_hw:ft1000_reset_card:Take DSP out of reset\n");
+		pr_debug("Take DSP out of reset\n");
 
 		/* Wait for 0xfefe indicating dsp ready before starting download */
 		for (i = 0; i < 50; i++) {
@@ -464,8 +447,7 @@ static int ft1000_reset_card(struct net_device *dev)
 		}
 
 		if (i == 50) {
-			DEBUG(0,
-				  "ft1000_hw:ft1000_reset_card:No FEFE detected from DSP\n");
+			pr_debug("No FEFE detected from DSP\n");
 			return false;
 		}
 
@@ -476,10 +458,10 @@ static int ft1000_reset_card(struct net_device *dev)
 	}
 
 	if (card_download(dev, fw_entry->data, fw_entry->size)) {
-		DEBUG(1, "card download unsuccessful\n");
+		pr_debug("card download unsuccessful\n");
 		return false;
 	} else {
-		DEBUG(1, "card download successful\n");
+		pr_debug("card download successful\n");
 	}
 
 	mdelay(10);
@@ -494,8 +476,7 @@ static int ft1000_reset_card(struct net_device *dev)
 		/* Initialize DSP heartbeat area to ho */
 		ft1000_write_dpram(dev, FT1000_HI_HO, ho);
 		tempword = ft1000_read_dpram(dev, FT1000_HI_HO);
-		DEBUG(1, "ft1000_hw:ft1000_reset_asic:hi_ho value = 0x%x\n",
-			  tempword);
+		pr_debug("hi_ho value = 0x%x\n", tempword);
 	} else {
 		/* Initialize DSP heartbeat area to ho */
 		ft1000_write_dpram_mag_16(dev, FT1000_MAG_HI_HO, ho_mag,
@@ -503,8 +484,7 @@ static int ft1000_reset_card(struct net_device *dev)
 		tempword =
 			ft1000_read_dpram_mag_16(dev, FT1000_MAG_HI_HO,
 						 FT1000_MAG_HI_HO_INDX);
-		DEBUG(1, "ft1000_hw:ft1000_reset_card:hi_ho value = 0x%x\n",
-			  tempword);
+		pr_debug("hi_ho value = 0x%x\n", tempword);
 	}
 
 	info->CardReady = 1;
@@ -521,14 +501,14 @@ static int ft1000_reset_card(struct net_device *dev)
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_chkcard
-   Description: This function will check if the device is presently available on
-               the system.
-   Input:
-       dev    - device structure
-   Output:
-       status - false (device is not present)
-                true  (device is present)
+  Function:   ft1000_chkcard
+  Description: This function will check if the device is presently available on
+  the system.
+  Input:
+  dev    - device structure
+  Output:
+  status - false (device is not present)
+  true  (device is present)
 
   -------------------------------------------------------------------------*/
 static int ft1000_chkcard(struct net_device *dev)
@@ -541,8 +521,7 @@ static int ft1000_chkcard(struct net_device *dev)
 	 */
 	tempword = ft1000_read_reg(dev, FT1000_REG_SUP_IMASK);
 	if (tempword == 0) {
-		DEBUG(1,
-			  "ft1000_hw:ft1000_chkcard: IMASK = 0 Card not detected\n");
+		pr_debug("IMASK = 0 Card not detected\n");
 		return false;
 	}
 	/*
@@ -551,8 +530,7 @@ static int ft1000_chkcard(struct net_device *dev)
 	 */
 	tempword = ft1000_read_reg(dev, FT1000_REG_ASIC_ID);
 	if (tempword == 0xffff) {
-		DEBUG(1,
-			  "ft1000_hw:ft1000_chkcard: Version = 0xffff Card not detected\n");
+		pr_debug("Version = 0xffff Card not detected\n");
 		return false;
 	}
 	return true;
@@ -561,13 +539,13 @@ static int ft1000_chkcard(struct net_device *dev)
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_hbchk
-   Description: This function will perform the heart beat check of the DSP as
-               well as the ASIC.
-   Input:
-       dev    - device structure
-   Output:
-       none
+  Function:   ft1000_hbchk
+  Description: This function will perform the heart beat check of the DSP as
+  well as the ASIC.
+  Input:
+  dev    - device structure
+  Output:
+  none
 
   -------------------------------------------------------------------------*/
 static void ft1000_hbchk(u_long data)
@@ -586,11 +564,10 @@ static void ft1000_hbchk(u_long data)
 		} else {
 			tempword =
 				ntohs(ft1000_read_dpram_mag_16
-				  (dev, FT1000_MAG_HI_HO,
-				   FT1000_MAG_HI_HO_INDX));
+				      (dev, FT1000_MAG_HI_HO,
+				       FT1000_MAG_HI_HO_INDX));
 		}
-		DEBUG(1, "ft1000_hw:ft1000_hbchk:hi_ho value = 0x%x\n",
-			  tempword);
+		pr_debug("hi_ho value = 0x%x\n", tempword);
 		/* Let's perform another check if ho is not detected */
 		if (tempword != ho) {
 			if (info->AsicID == ELECTRABUZZ_ID) {
@@ -601,8 +578,7 @@ static void ft1000_hbchk(u_long data)
 			}
 		}
 		if (tempword != ho) {
-			printk(KERN_INFO
-				   "ft1000: heartbeat failed - no ho detected\n");
+			pr_info("heartbeat failed - no ho detected\n");
 			if (info->AsicID == ELECTRABUZZ_ID) {
 				info->DSP_TIME[0] =
 					ft1000_read_dpram(dev, FT1000_DSP_TIMER0);
@@ -632,8 +608,7 @@ static void ft1000_hbchk(u_long data)
 			}
 			info->DrvErrNum = DSP_HB_INFO;
 			if (ft1000_reset_card(dev) == 0) {
-				printk(KERN_INFO
-					   "ft1000: Hardware Failure Detected - PC Card disabled\n");
+				pr_info("Hardware Failure Detected - PC Card disabled\n");
 				info->ProgConStat = 0xff;
 				return;
 			}
@@ -650,8 +625,7 @@ static void ft1000_hbchk(u_long data)
 			tempword = ft1000_read_reg(dev, FT1000_REG_DOORBELL);
 		}
 		if (tempword & FT1000_DB_HB) {
-			printk(KERN_INFO
-				   "ft1000: heartbeat doorbell not clear by firmware\n");
+			pr_info("heartbeat doorbell not clear by firmware\n");
 			if (info->AsicID == ELECTRABUZZ_ID) {
 				info->DSP_TIME[0] =
 					ft1000_read_dpram(dev, FT1000_DSP_TIMER0);
@@ -681,8 +655,7 @@ static void ft1000_hbchk(u_long data)
 			}
 			info->DrvErrNum = DSP_HB_INFO;
 			if (ft1000_reset_card(dev) == 0) {
-				printk(KERN_INFO
-					   "ft1000: Hardware Failure Detected - PC Card disabled\n");
+				pr_info("Hardware Failure Detected - PC Card disabled\n");
 				info->ProgConStat = 0xff;
 				return;
 			}
@@ -708,8 +681,8 @@ static void ft1000_hbchk(u_long data)
 		} else {
 			tempword =
 				ntohs(ft1000_read_dpram_mag_16
-				  (dev, FT1000_MAG_HI_HO,
-				   FT1000_MAG_HI_HO_INDX));
+				      (dev, FT1000_MAG_HI_HO,
+				       FT1000_MAG_HI_HO_INDX));
 		}
 		/* Let's write hi again if fail */
 		if (tempword != hi) {
@@ -730,8 +703,7 @@ static void ft1000_hbchk(u_long data)
 		}
 
 		if (tempword != hi) {
-			printk(KERN_INFO
-				   "ft1000: heartbeat failed - cannot write hi into DPRAM\n");
+			pr_info("heartbeat failed - cannot write hi into DPRAM\n");
 			if (info->AsicID == ELECTRABUZZ_ID) {
 				info->DSP_TIME[0] =
 					ft1000_read_dpram(dev, FT1000_DSP_TIMER0);
@@ -761,8 +733,7 @@ static void ft1000_hbchk(u_long data)
 			}
 			info->DrvErrNum = DSP_HB_INFO;
 			if (ft1000_reset_card(dev) == 0) {
-				printk(KERN_INFO
-					   "ft1000: Hardware Failure Detected - PC Card disabled\n");
+				pr_info("Hardware Failure Detected - PC Card disabled\n");
 				info->ProgConStat = 0xff;
 				return;
 			}
@@ -778,19 +749,19 @@ static void ft1000_hbchk(u_long data)
 
 	/* Schedule this module to run every 2 seconds */
 	poll_timer.expires = jiffies + (2 * HZ);
-	poll_timer.data = (u_long) dev;
+	poll_timer.data = (u_long)dev;
 	add_timer(&poll_timer);
 }
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_send_cmd
-   Description:
-   Input:
-   Output:
+  Function:   ft1000_send_cmd
+  Description:
+  Input:
+  Output:
 
   -------------------------------------------------------------------------*/
-static void ft1000_send_cmd (struct net_device *dev, u16 *ptempbuffer, int size, u16 qtype)
+static void ft1000_send_cmd(struct net_device *dev, u16 *ptempbuffer, int size, u16 qtype)
 {
 	struct ft1000_info *info = netdev_priv(dev);
 	int i;
@@ -802,26 +773,26 @@ static void ft1000_send_cmd (struct net_device *dev, u16 *ptempbuffer, int size,
 	if ((size & 0x0001)) {
 		size++;
 	}
-	DEBUG(1, "FT1000:ft1000_send_cmd:total length = %d\n", size);
-	DEBUG(1, "FT1000:ft1000_send_cmd:length = %d\n", ntohs(*ptempbuffer));
+	pr_debug("total length = %d\n", size);
+	pr_debug("length = %d\n", ntohs(*ptempbuffer));
 	/*
 	 * put message into slow queue area
 	 * All messages are in the form total_len + pseudo header + message body
 	 */
 	spin_lock_irqsave(&info->dpram_lock, flags);
 
-    /* Make sure SLOWQ doorbell is clear */
-    tempword = ft1000_read_reg(dev, FT1000_REG_DOORBELL);
-    i=0;
-    while (tempword & FT1000_DB_DPRAM_TX) {
-        mdelay(10);
-        i++;
-        if (i==10) {
-            spin_unlock_irqrestore(&info->dpram_lock, flags);
-            return;
-        }
-        tempword = ft1000_read_reg(dev, FT1000_REG_DOORBELL);
-    }
+	/* Make sure SLOWQ doorbell is clear */
+	tempword = ft1000_read_reg(dev, FT1000_REG_DOORBELL);
+	i = 0;
+	while (tempword & FT1000_DB_DPRAM_TX) {
+		mdelay(10);
+		i++;
+		if (i == 10) {
+			spin_unlock_irqrestore(&info->dpram_lock, flags);
+			return;
+		}
+		tempword = ft1000_read_reg(dev, FT1000_REG_DOORBELL);
+	}
 
 	if (info->AsicID == ELECTRABUZZ_ID) {
 		ft1000_write_reg(dev, FT1000_REG_DPRAM_ADDR,
@@ -830,8 +801,7 @@ static void ft1000_send_cmd (struct net_device *dev, u16 *ptempbuffer, int size,
 		ft1000_write_reg(dev, FT1000_REG_DPRAM_DATA, size);
 		/* Write pseudo header and messgae body */
 		for (i = 0; i < (size >> 1); i++) {
-			DEBUG(1, "FT1000:ft1000_send_cmd:data %d = 0x%x\n", i,
-				  *ptempbuffer);
+			pr_debug("data %d = 0x%x\n", i, *ptempbuffer);
 			tempword = htons(*ptempbuffer++);
 			ft1000_write_reg(dev, FT1000_REG_DPRAM_DATA, tempword);
 		}
@@ -844,18 +814,16 @@ static void ft1000_send_cmd (struct net_device *dev, u16 *ptempbuffer, int size,
 		ft1000_write_reg(dev, FT1000_REG_DPRAM_ADDR,
 				 FT1000_DPRAM_MAG_TX_BASE + 1);
 		for (i = 0; i < (size >> 2); i++) {
-			DEBUG(1, "FT1000:ft1000_send_cmd:data = 0x%x\n",
-				  *ptempbuffer);
+			pr_debug("data = 0x%x\n", *ptempbuffer);
 			outw(*ptempbuffer++,
-				 dev->base_addr + FT1000_REG_MAG_DPDATAL);
-			DEBUG(1, "FT1000:ft1000_send_cmd:data = 0x%x\n",
-				  *ptempbuffer);
+			     dev->base_addr + FT1000_REG_MAG_DPDATAL);
+			pr_debug("data = 0x%x\n", *ptempbuffer);
 			outw(*ptempbuffer++,
-				 dev->base_addr + FT1000_REG_MAG_DPDATAH);
+			     dev->base_addr + FT1000_REG_MAG_DPDATAH);
 		}
-		DEBUG(1, "FT1000:ft1000_send_cmd:data = 0x%x\n", *ptempbuffer);
+		pr_debug("data = 0x%x\n", *ptempbuffer);
 		outw(*ptempbuffer++, dev->base_addr + FT1000_REG_MAG_DPDATAL);
-		DEBUG(1, "FT1000:ft1000_send_cmd:data = 0x%x\n", *ptempbuffer);
+		pr_debug("data = 0x%x\n", *ptempbuffer);
 		outw(*ptempbuffer++, dev->base_addr + FT1000_REG_MAG_DPDATAH);
 	}
 	spin_unlock_irqrestore(&info->dpram_lock, flags);
@@ -866,19 +834,19 @@ static void ft1000_send_cmd (struct net_device *dev, u16 *ptempbuffer, int size,
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_receive_cmd
-   Description: This function will read a message from the dpram area.
-   Input:
-      dev - network device structure
-      pbuffer - caller supply address to buffer
-      pnxtph - pointer to next pseudo header
-   Output:
-     Status = 0 (unsuccessful)
-            = 1 (successful)
+  Function:   ft1000_receive_cmd
+  Description: This function will read a message from the dpram area.
+  Input:
+  dev - network device structure
+  pbuffer - caller supply address to buffer
+  pnxtph - pointer to next pseudo header
+  Output:
+  Status = 0 (unsuccessful)
+  = 1 (successful)
 
   -------------------------------------------------------------------------*/
 static bool ft1000_receive_cmd(struct net_device *dev, u16 *pbuffer,
-				int maxsz, u16 *pnxtph)
+			       int maxsz, u16 *pnxtph)
 {
 	struct ft1000_info *info = netdev_priv(dev);
 	u16 size;
@@ -888,20 +856,18 @@ static bool ft1000_receive_cmd(struct net_device *dev, u16 *pbuffer,
 	unsigned long flags;
 
 	if (info->AsicID == ELECTRABUZZ_ID) {
-		size = ( ft1000_read_dpram(dev, *pnxtph) ) + sizeof(struct pseudo_hdr);
+		size = (ft1000_read_dpram(dev, *pnxtph)) + sizeof(struct pseudo_hdr);
 	} else {
 		size =
 			ntohs(ft1000_read_dpram_mag_16
-			  (dev, FT1000_MAG_PH_LEN,
-			   FT1000_MAG_PH_LEN_INDX)) + sizeof(struct pseudo_hdr);
+			      (dev, FT1000_MAG_PH_LEN,
+			       FT1000_MAG_PH_LEN_INDX)) + sizeof(struct pseudo_hdr);
 	}
 	if (size > maxsz) {
-		DEBUG(1,
-			  "FT1000:ft1000_receive_cmd:Invalid command length = %d\n",
-			  size);
+		pr_debug("Invalid command length = %d\n", size);
 		return false;
 	} else {
-		ppseudohdr = (u16 *) pbuffer;
+		ppseudohdr = (u16 *)pbuffer;
 		spin_lock_irqsave(&info->dpram_lock, flags);
 		if (info->AsicID == ELECTRABUZZ_ID) {
 			ft1000_write_reg(dev, FT1000_REG_DPRAM_ADDR,
@@ -915,26 +881,26 @@ static bool ft1000_receive_cmd(struct net_device *dev, u16 *pbuffer,
 			ft1000_write_reg(dev, FT1000_REG_DPRAM_ADDR,
 					 FT1000_DPRAM_MAG_RX_BASE);
 			*pbuffer = inw(dev->base_addr + FT1000_REG_MAG_DPDATAH);
-			DEBUG(1, "ft1000_hw:received data = 0x%x\n", *pbuffer);
+			pr_debug("received data = 0x%x\n", *pbuffer);
 			pbuffer++;
 			ft1000_write_reg(dev, FT1000_REG_DPRAM_ADDR,
 					 FT1000_DPRAM_MAG_RX_BASE + 1);
 			for (i = 0; i <= (size >> 2); i++) {
 				*pbuffer =
 					inw(dev->base_addr +
-					FT1000_REG_MAG_DPDATAL);
+					    FT1000_REG_MAG_DPDATAL);
 				pbuffer++;
 				*pbuffer =
 					inw(dev->base_addr +
-					FT1000_REG_MAG_DPDATAH);
+					    FT1000_REG_MAG_DPDATAH);
 				pbuffer++;
 			}
 			/* copy odd aligned word */
 			*pbuffer = inw(dev->base_addr + FT1000_REG_MAG_DPDATAL);
-			DEBUG(1, "ft1000_hw:received data = 0x%x\n", *pbuffer);
+			pr_debug("received data = 0x%x\n", *pbuffer);
 			pbuffer++;
 			*pbuffer = inw(dev->base_addr + FT1000_REG_MAG_DPDATAH);
-			DEBUG(1, "ft1000_hw:received data = 0x%x\n", *pbuffer);
+			pr_debug("received data = 0x%x\n", *pbuffer);
 			pbuffer++;
 		}
 		if (size & 0x0001) {
@@ -953,8 +919,7 @@ static bool ft1000_receive_cmd(struct net_device *dev, u16 *pbuffer,
 			tempword ^= *ppseudohdr++;
 		}
 		if ((tempword != *ppseudohdr)) {
-			DEBUG(1,
-				  "FT1000:ft1000_receive_cmd:Pseudo header checksum mismatch\n");
+			pr_debug("Pseudo header checksum mismatch\n");
 			/* Drop this message */
 			return false;
 		}
@@ -964,13 +929,13 @@ static bool ft1000_receive_cmd(struct net_device *dev, u16 *pbuffer,
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_proc_drvmsg
-   Description: This function will process the various driver messages.
-   Input:
-       dev    - device structure
-       pnxtph - pointer to next pseudo header
-   Output:
-       none
+  Function:   ft1000_proc_drvmsg
+  Description: This function will process the various driver messages.
+  Input:
+  dev    - device structure
+  pnxtph - pointer to next pseudo header
+  Output:
+  none
 
   -------------------------------------------------------------------------*/
 static void ft1000_proc_drvmsg(struct net_device *dev)
@@ -992,25 +957,24 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 		u16 wrd;
 	} convert;
 
-    if (info->AsicID == ELECTRABUZZ_ID) {
-        tempword = FT1000_DPRAM_RX_BASE+2;
-    }
-    else {
-        tempword = FT1000_DPRAM_MAG_RX_BASE;
-    }
-    if ( ft1000_receive_cmd(dev, &cmdbuffer[0], MAX_CMD_SQSIZE, &tempword) ) {
+	if (info->AsicID == ELECTRABUZZ_ID) {
+		tempword = FT1000_DPRAM_RX_BASE+2;
+	}
+	else {
+		tempword = FT1000_DPRAM_MAG_RX_BASE;
+	}
+	if (ft1000_receive_cmd(dev, &cmdbuffer[0], MAX_CMD_SQSIZE, &tempword)) {
 
 		/* Get the message type which is total_len + PSEUDO header + msgtype + message body */
-		pdrvmsg = (struct drv_msg *) & cmdbuffer[0];
+		pdrvmsg = (struct drv_msg *)&cmdbuffer[0];
 		msgtype = ntohs(pdrvmsg->type);
-		DEBUG(1, "Command message type = 0x%x\n", msgtype);
+		pr_debug("Command message type = 0x%x\n", msgtype);
 		switch (msgtype) {
 		case DSP_PROVISION:
-			DEBUG(0,
-				  "Got a provisioning request message from DSP\n");
+			pr_debug("Got a provisioning request message from DSP\n");
 			mdelay(25);
 			while (list_empty(&info->prov_list) == 0) {
-				DEBUG(0, "Sending a provisioning message\n");
+				pr_debug("Sending a provisioning message\n");
 				/* Make sure SLOWQ doorbell is clear */
 				tempword =
 					ft1000_read_reg(dev, FT1000_REG_DOORBELL);
@@ -1025,25 +989,25 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 				ptr =
 					list_entry(info->prov_list.next,
 						   struct prov_record, list);
-				len = *(u16 *) ptr->pprov_data;
+				len = *(u16 *)ptr->pprov_data;
 				len = htons(len);
 
-				pmsg = (u16 *) ptr->pprov_data;
-				ppseudo_hdr = (struct pseudo_hdr *) pmsg;
+				pmsg = (u16 *)ptr->pprov_data;
+				ppseudo_hdr = (struct pseudo_hdr *)pmsg;
 				/* Insert slow queue sequence number */
 				ppseudo_hdr->seq_num = info->squeseqnum++;
 				ppseudo_hdr->portsrc = 0;
 				/* Calculate new checksum */
 				ppseudo_hdr->checksum = *pmsg++;
-				DEBUG(1, "checksum = 0x%x\n",
-					  ppseudo_hdr->checksum);
+				pr_debug("checksum = 0x%x\n",
+					 ppseudo_hdr->checksum);
 				for (i = 1; i < 7; i++) {
 					ppseudo_hdr->checksum ^= *pmsg++;
-					DEBUG(1, "checksum = 0x%x\n",
-						  ppseudo_hdr->checksum);
+					pr_debug("checksum = 0x%x\n",
+						 ppseudo_hdr->checksum);
 				}
 
-				ft1000_send_cmd (dev, (u16 *)ptr->pprov_data, len, SLOWQ_TYPE);
+				ft1000_send_cmd(dev, (u16 *)ptr->pprov_data, len, SLOWQ_TYPE);
 				list_del(&ptr->list);
 				kfree(ptr->pprov_data);
 				kfree(ptr);
@@ -1055,19 +1019,29 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 			info->CardReady = 1;
 			break;
 		case MEDIA_STATE:
-			pmediamsg = (struct media_msg *) & cmdbuffer[0];
+			pmediamsg = (struct media_msg *)&cmdbuffer[0];
 			if (info->ProgConStat != 0xFF) {
-			if (pmediamsg->state) {
-				DEBUG(1, "Media is up\n");
-				if (info->mediastate == 0) {
-					netif_carrier_on(dev);
-					netif_wake_queue(dev);
-					info->mediastate = 1;
-					do_gettimeofday(&tv);
-					info->ConTm = tv.tv_sec;
+				if (pmediamsg->state) {
+					pr_debug("Media is up\n");
+					if (info->mediastate == 0) {
+						netif_carrier_on(dev);
+						netif_wake_queue(dev);
+						info->mediastate = 1;
+						do_gettimeofday(&tv);
+						info->ConTm = tv.tv_sec;
+					}
+				} else {
+					pr_debug("Media is down\n");
+					if (info->mediastate == 1) {
+						info->mediastate = 0;
+						netif_carrier_off(dev);
+						netif_stop_queue(dev);
+						info->ConTm = 0;
+					}
 				}
-			} else {
-				DEBUG(1, "Media is down\n");
+			}
+			else {
+				pr_debug("Media is down\n");
 				if (info->mediastate == 1) {
 					info->mediastate = 0;
 					netif_carrier_off(dev);
@@ -1075,25 +1049,15 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 					info->ConTm = 0;
 				}
 			}
-            }
-            else {
-                DEBUG(1, "Media is down\n");
-                if (info->mediastate == 1) {
-                    info->mediastate = 0;
-                    netif_carrier_off(dev);
-                    netif_stop_queue(dev);
-                    info->ConTm = 0;
-                }
-            }
 			break;
 		case DSP_INIT_MSG:
-			pdspinitmsg = (struct dsp_init_msg *) & cmdbuffer[0];
+			pdspinitmsg = (struct dsp_init_msg *)&cmdbuffer[0];
 			memcpy(info->DspVer, pdspinitmsg->DspVer, DSPVERSZ);
-			DEBUG(1, "DSPVER = 0x%2x 0x%2x 0x%2x 0x%2x\n",
-				  info->DspVer[0], info->DspVer[1], info->DspVer[2],
-				   info->DspVer[3]);
+			pr_debug("DSPVER = 0x%2x 0x%2x 0x%2x 0x%2x\n",
+				 info->DspVer[0], info->DspVer[1],
+				 info->DspVer[2], info->DspVer[3]);
 			memcpy(info->HwSerNum, pdspinitmsg->HwSerNum,
-				   HWSERNUMSZ);
+			       HWSERNUMSZ);
 			memcpy(info->Sku, pdspinitmsg->Sku, SKUSZ);
 			memcpy(info->eui64, pdspinitmsg->eui64, EUISZ);
 			dev->dev_addr[0] = info->eui64[0];
@@ -1104,34 +1068,33 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 			dev->dev_addr[5] = info->eui64[7];
 
 			if (ntohs(pdspinitmsg->length) ==
-				(sizeof(struct dsp_init_msg) - 20)) {
+			    (sizeof(struct dsp_init_msg) - 20)) {
 				memcpy(info->ProductMode,
-					   pdspinitmsg->ProductMode, MODESZ);
+				       pdspinitmsg->ProductMode, MODESZ);
 				memcpy(info->RfCalVer, pdspinitmsg->RfCalVer,
-					   CALVERSZ);
+				       CALVERSZ);
 				memcpy(info->RfCalDate, pdspinitmsg->RfCalDate,
-					   CALDATESZ);
-				DEBUG(1, "RFCalVer = 0x%2x 0x%2x\n",
-					  info->RfCalVer[0], info->RfCalVer[1]);
+				       CALDATESZ);
+				pr_debug("RFCalVer = 0x%2x 0x%2x\n",
+					 info->RfCalVer[0], info->RfCalVer[1]);
 			}
 
-			break ;
+			break;
 		case DSP_STORE_INFO:
-			DEBUG(1, "FT1000:drivermsg:Got DSP_STORE_INFO\n");
+			pr_debug("Got DSP_STORE_INFO\n");
 			tempword = ntohs(pdrvmsg->length);
 			info->DSPInfoBlklen = tempword;
 			if (tempword < (MAX_DSP_SESS_REC - 4)) {
-				pmsg = (u16 *) & pdrvmsg->data[0];
+				pmsg = (u16 *)&pdrvmsg->data[0];
 				for (i = 0; i < ((tempword + 1) / 2); i++) {
-					DEBUG(1,
-						  "FT1000:drivermsg:dsp info data = 0x%x\n",
-						  *pmsg);
+					pr_debug("dsp info data = 0x%x\n",
+						 *pmsg);
 					info->DSPInfoBlk[i + 10] = *pmsg++;
 				}
 			}
 			break;
 		case DSP_GET_INFO:
-			DEBUG(1, "FT1000:drivermsg:Got DSP_GET_INFO\n");
+			pr_debug("Got DSP_GET_INFO\n");
 			/*
 			 * copy dsp info block to dsp
 			 * allow any outstanding ioctl to finish
@@ -1152,8 +1115,8 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 				 * Put message into Slow Queue
 				 * Form Pseudo header
 				 */
-				pmsg = (u16 *) info->DSPInfoBlk;
-				ppseudo_hdr = (struct pseudo_hdr *) pmsg;
+				pmsg = (u16 *)info->DSPInfoBlk;
+				ppseudo_hdr = (struct pseudo_hdr *)pmsg;
 				ppseudo_hdr->length =
 					htons(info->DSPInfoBlklen + 4);
 				ppseudo_hdr->source = 0x10;
@@ -1177,12 +1140,12 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 				info->DSPInfoBlk[8] = 0x7200;
 				info->DSPInfoBlk[9] =
 					htons(info->DSPInfoBlklen);
-				ft1000_send_cmd (dev, (u16 *)info->DSPInfoBlk, (u16)(info->DSPInfoBlklen+4), 0);
+				ft1000_send_cmd(dev, (u16 *)info->DSPInfoBlk, (u16)(info->DSPInfoBlklen+4), 0);
 			}
 
 			break;
 		case GET_DRV_ERR_RPT_MSG:
-			DEBUG(1, "FT1000:drivermsg:Got GET_DRV_ERR_RPT_MSG\n");
+			pr_debug("Got GET_DRV_ERR_RPT_MSG\n");
 			/*
 			 * copy driver error message to dsp
 			 * allow any outstanding ioctl to finish
@@ -1203,8 +1166,8 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 				 * Put message into Slow Queue
 				 * Form Pseudo header
 				 */
-				pmsg = (u16 *) & tempbuffer[0];
-				ppseudo_hdr = (struct pseudo_hdr *) pmsg;
+				pmsg = (u16 *)&tempbuffer[0];
+				ppseudo_hdr = (struct pseudo_hdr *)pmsg;
 				ppseudo_hdr->length = htons(0x0012);
 				ppseudo_hdr->source = 0x10;
 				ppseudo_hdr->destination = 0x20;
@@ -1220,11 +1183,11 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 				/* Insert application id */
 				ppseudo_hdr->portsrc = 0;
 				/* Calculate new checksum */
-                ppseudo_hdr->checksum = *pmsg++;
-                for (i=1; i<7; i++) {
-                    ppseudo_hdr->checksum ^= *pmsg++;
-                }
-				pmsg = (u16 *) & tempbuffer[16];
+				ppseudo_hdr->checksum = *pmsg++;
+				for (i = 1; i < 7; i++) {
+					ppseudo_hdr->checksum ^= *pmsg++;
+				}
+				pmsg = (u16 *)&tempbuffer[16];
 				*pmsg++ = htons(RSP_DRV_ERR_RPT_MSG);
 				*pmsg++ = htons(0x000e);
 				*pmsg++ = htons(info->DSP_TIME[0]);
@@ -1239,7 +1202,7 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 				*pmsg++ = convert.wrd;
 				*pmsg++ = htons(info->DrvErrNum);
 
-				ft1000_send_cmd (dev, (u16 *)&tempbuffer[0], (u16)(0x0012), 0);
+				ft1000_send_cmd(dev, (u16 *)&tempbuffer[0], (u16)(0x0012), 0);
 				info->DrvErrNum = 0;
 			}
 
@@ -1252,14 +1215,14 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_parse_dpram_msg
-   Description: This function will parse the message received from the DSP
-               via the DPRAM interface.
-   Input:
-       dev    - device structure
-   Output:
-       status - FAILURE
-                SUCCESS
+  Function:   ft1000_parse_dpram_msg
+  Description: This function will parse the message received from the DSP
+  via the DPRAM interface.
+  Input:
+  dev    - device structure
+  Output:
+  status - FAILURE
+  SUCCESS
 
   -------------------------------------------------------------------------*/
 static int ft1000_parse_dpram_msg(struct net_device *dev)
@@ -1270,11 +1233,10 @@ static int ft1000_parse_dpram_msg(struct net_device *dev)
 	u16 nxtph;
 	u16 total_len;
 	int i = 0;
-	int cnt;
 	unsigned long flags;
 
 	doorbell = ft1000_read_reg(dev, FT1000_REG_DOORBELL);
-	DEBUG(1, "Doorbell = 0x%x\n", doorbell);
+	pr_debug("Doorbell = 0x%x\n", doorbell);
 
 	if (doorbell & FT1000_ASIC_RESET_REQ) {
 		/* Copy DSP session record from info block */
@@ -1291,7 +1253,7 @@ static int ft1000_parse_dpram_msg(struct net_device *dev)
 					 FT1000_DPRAM_MAG_RX_BASE);
 			for (i = 0; i < MAX_DSP_SESS_REC / 2; i++) {
 				outl(info->DSPSess.MagRec[i],
-					 dev->base_addr + FT1000_REG_MAG_DPDATA);
+				     dev->base_addr + FT1000_REG_MAG_DPDATA);
 			}
 		}
 		spin_unlock_irqrestore(&info->dpram_lock, flags);
@@ -1299,7 +1261,7 @@ static int ft1000_parse_dpram_msg(struct net_device *dev)
 		/* clear ASIC RESET request */
 		ft1000_write_reg(dev, FT1000_REG_DOORBELL,
 				 FT1000_ASIC_RESET_REQ);
-		DEBUG(1, "Got an ASIC RESET Request\n");
+		pr_debug("Got an ASIC RESET Request\n");
 		ft1000_write_reg(dev, FT1000_REG_DOORBELL,
 				 FT1000_ASIC_RESET_DSP);
 
@@ -1311,8 +1273,7 @@ static int ft1000_parse_dpram_msg(struct net_device *dev)
 	}
 
 	if (doorbell & FT1000_DSP_ASIC_RESET) {
-		DEBUG(0,
-			  "FT1000:ft1000_parse_dpram_msg: Got a dsp ASIC reset message\n");
+		pr_debug("Got a dsp ASIC reset message\n");
 		ft1000_write_reg(dev, FT1000_REG_DOORBELL,
 				 FT1000_DSP_ASIC_RESET);
 		udelay(200);
@@ -1320,8 +1281,7 @@ static int ft1000_parse_dpram_msg(struct net_device *dev)
 	}
 
 	if (doorbell & FT1000_DB_DPRAM_RX) {
-		DEBUG(1,
-			  "FT1000:ft1000_parse_dpram_msg: Got a slow queue message\n");
+		pr_debug("Got a slow queue message\n");
 		nxtph = FT1000_DPRAM_RX_BASE + 2;
 		if (info->AsicID == ELECTRABUZZ_ID) {
 			total_len =
@@ -1329,14 +1289,12 @@ static int ft1000_parse_dpram_msg(struct net_device *dev)
 		} else {
 			total_len =
 				ntohs(ft1000_read_dpram_mag_16
-				  (dev, FT1000_MAG_TOTAL_LEN,
-				   FT1000_MAG_TOTAL_LEN_INDX));
+				      (dev, FT1000_MAG_TOTAL_LEN,
+				       FT1000_MAG_TOTAL_LEN_INDX));
 		}
-		DEBUG(1, "FT1000:ft1000_parse_dpram_msg:total length = %d\n",
-			  total_len);
+		pr_debug("total length = %d\n", total_len);
 		if ((total_len < MAX_CMD_SQSIZE) && (total_len > sizeof(struct pseudo_hdr))) {
-            total_len += nxtph;
-            cnt = 0;
+			total_len += nxtph;
 			/*
 			 * ft1000_read_reg will return a value that needs to be byteswap
 			 * in order to get DSP_QID_OFFSET.
@@ -1353,7 +1311,7 @@ static int ft1000_parse_dpram_msg(struct net_device *dev)
 					 (dev, FT1000_MAG_PORT_ID,
 					  FT1000_MAG_PORT_ID_INDX) & 0xff);
 			}
-			DEBUG(1, "DSP_QID = 0x%x\n", portid);
+			pr_debug("DSP_QID = 0x%x\n", portid);
 
 			if (portid == DRIVERID) {
 				/* We are assumming one driver message from the DSP at a time. */
@@ -1389,7 +1347,7 @@ static int ft1000_parse_dpram_msg(struct net_device *dev)
 							 FT1000_MAG_DSP_TIMER3_INDX);
 		}
 		info->DrvErrNum = DSP_CONDRESET_INFO;
-		DEBUG(1, "ft1000_hw:DSP conditional reset requested\n");
+		pr_debug("DSP conditional reset requested\n");
 		ft1000_reset_card(dev);
 		ft1000_write_reg(dev, FT1000_REG_DOORBELL,
 				 FT1000_DB_COND_RESET);
@@ -1397,9 +1355,9 @@ static int ft1000_parse_dpram_msg(struct net_device *dev)
 	/* let's clear any unexpected doorbells from DSP */
 	doorbell =
 		doorbell & ~(FT1000_DB_DPRAM_RX | FT1000_ASIC_RESET_REQ |
-			 FT1000_DB_COND_RESET | 0xff00);
+			     FT1000_DB_COND_RESET | 0xff00);
 	if (doorbell) {
-		DEBUG(1, "Clearing unexpected doorbell = 0x%x\n", doorbell);
+		pr_debug("Clearing unexpected doorbell = 0x%x\n", doorbell);
 		ft1000_write_reg(dev, FT1000_REG_DOORBELL, doorbell);
 	}
 
@@ -1409,14 +1367,14 @@ static int ft1000_parse_dpram_msg(struct net_device *dev)
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_flush_fifo
-   Description: This function will flush one packet from the downlink
-               FIFO.
-   Input:
-       dev      - device structure
-       drv_err  - driver error causing the flush fifo
-   Output:
-       None.
+  Function:   ft1000_flush_fifo
+  Description: This function will flush one packet from the downlink
+  FIFO.
+  Input:
+  dev      - device structure
+  drv_err  - driver error causing the flush fifo
+  Output:
+  None.
 
   -------------------------------------------------------------------------*/
 static void ft1000_flush_fifo(struct net_device *dev, u16 DrvErrNum)
@@ -1427,7 +1385,6 @@ static void ft1000_flush_fifo(struct net_device *dev, u16 DrvErrNum)
 	u32 templong;
 	u16 tempword;
 
-	DEBUG(1, "ft1000:ft1000_hw:ft1000_flush_fifo called\n");
 	if (pcmcia->PktIntfErr > MAX_PH_ERR) {
 		if (info->AsicID == ELECTRABUZZ_ID) {
 			info->DSP_TIME[0] =
@@ -1514,7 +1471,7 @@ static void ft1000_flush_fifo(struct net_device *dev, u16 DrvErrNum)
 					 */
 					tempword =
 						inw(dev->base_addr +
-						FT1000_REG_SUP_IMASK);
+						    FT1000_REG_SUP_IMASK);
 					if (tempword == 0) {
 						/* This indicates that we can not communicate with the ASIC */
 						info->DrvErrNum =
@@ -1533,23 +1490,23 @@ static void ft1000_flush_fifo(struct net_device *dev, u16 DrvErrNum)
 		} while ((tempword & 0x03) != 0x03);
 		if (info->AsicID == ELECTRABUZZ_ID) {
 			i++;
-			DEBUG(0, "Flushing FIFO complete = %x\n", tempword);
+			pr_debug("Flushing FIFO complete = %x\n", tempword);
 			/* Flush last word in FIFO. */
 			tempword = ft1000_read_reg(dev, FT1000_REG_DFIFO);
 			/* Update FIFO counter for DSP */
 			i = i * 2;
-			DEBUG(0, "Flush Data byte count to dsp = %d\n", i);
+			pr_debug("Flush Data byte count to dsp = %d\n", i);
 			info->fifo_cnt += i;
 			ft1000_write_dpram(dev, FT1000_FIFO_LEN,
 					   info->fifo_cnt);
 		} else {
-			DEBUG(0, "Flushing FIFO complete = %x\n", tempword);
+			pr_debug("Flushing FIFO complete = %x\n", tempword);
 			/* Flush last word in FIFO */
 			templong = inl(dev->base_addr + FT1000_REG_MAG_DFR);
 			tempword = inw(dev->base_addr + FT1000_REG_SUP_STAT);
-			DEBUG(0, "FT1000_REG_SUP_STAT = 0x%x\n", tempword);
+			pr_debug("FT1000_REG_SUP_STAT = 0x%x\n", tempword);
 			tempword = inw(dev->base_addr + FT1000_REG_MAG_DFSR);
-			DEBUG(0, "FT1000_REG_MAG_DFSR = 0x%x\n", tempword);
+			pr_debug("FT1000_REG_MAG_DFSR = 0x%x\n", tempword);
 		}
 		if (DrvErrNum) {
 			pcmcia->PktIntfErr++;
@@ -1559,15 +1516,15 @@ static void ft1000_flush_fifo(struct net_device *dev, u16 DrvErrNum)
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_copy_up_pkt
-   Description: This function will pull Flarion packets out of the Downlink
-               FIFO and convert it to an ethernet packet.  The ethernet packet will
-               then be deliver to the TCP/IP stack.
-   Input:
-       dev    - device structure
-   Output:
-       status - FAILURE
-                SUCCESS
+  Function:   ft1000_copy_up_pkt
+  Description: This function will pull Flarion packets out of the Downlink
+  FIFO and convert it to an ethernet packet.  The ethernet packet will
+  then be deliver to the TCP/IP stack.
+  Input:
+  dev    - device structure
+  Output:
+  status - FAILURE
+  SUCCESS
 
   -------------------------------------------------------------------------*/
 static int ft1000_copy_up_pkt(struct net_device *dev)
@@ -1583,7 +1540,6 @@ static int ft1000_copy_up_pkt(struct net_device *dev)
 	u32 *ptemplong;
 	u32 templong;
 
-	DEBUG(1, "ft1000_copy_up_pkt\n");
 	/* Read length */
 	if (info->AsicID == ELECTRABUZZ_ID) {
 		tempword = ft1000_read_reg(dev, FT1000_REG_DFIFO);
@@ -1593,10 +1549,10 @@ static int ft1000_copy_up_pkt(struct net_device *dev)
 		len = ntohs(tempword);
 	}
 	chksum = tempword;
-	DEBUG(1, "Number of Bytes in FIFO = %d\n", len);
+	pr_debug("Number of Bytes in FIFO = %d\n", len);
 
 	if (len > ENET_MAX_SIZE) {
-		DEBUG(0, "size of ethernet packet invalid\n");
+		pr_debug("size of ethernet packet invalid\n");
 		if (info->AsicID == MAGNEMITE_ID) {
 			/* Read High word to complete 32 bit access */
 			tempword = ft1000_read_reg(dev, FT1000_REG_MAG_DFRH);
@@ -1609,7 +1565,7 @@ static int ft1000_copy_up_pkt(struct net_device *dev)
 	skb = dev_alloc_skb(len + 12 + 2);
 
 	if (skb == NULL) {
-		DEBUG(0, "No Network buffers available\n");
+		pr_debug("No Network buffers available\n");
 		/* Read High word to complete 32 bit access */
 		if (info->AsicID == MAGNEMITE_ID) {
 			tempword = ft1000_read_reg(dev, FT1000_REG_MAG_DFRH);
@@ -1618,7 +1574,7 @@ static int ft1000_copy_up_pkt(struct net_device *dev)
 		info->stats.rx_errors++;
 		return FAILURE;
 	}
-	pbuffer = (u8 *) skb_put(skb, len + 12);
+	pbuffer = (u8 *)skb_put(skb, len + 12);
 
 	/* Pseudo header */
 	if (info->AsicID == ELECTRABUZZ_ID) {
@@ -1630,37 +1586,37 @@ static int ft1000_copy_up_pkt(struct net_device *dev)
 		tempword = ft1000_read_reg(dev, FT1000_REG_DFIFO);
 	} else {
 		tempword = ft1000_read_reg(dev, FT1000_REG_MAG_DFRH);
-		DEBUG(1, "Pseudo = 0x%x\n", tempword);
+		pr_debug("Pseudo = 0x%x\n", tempword);
 		chksum ^= tempword;
 
 		tempword = ft1000_read_reg(dev, FT1000_REG_MAG_DFRL);
-		DEBUG(1, "Pseudo = 0x%x\n", tempword);
+		pr_debug("Pseudo = 0x%x\n", tempword);
 		chksum ^= tempword;
 
 		tempword = ft1000_read_reg(dev, FT1000_REG_MAG_DFRH);
-		DEBUG(1, "Pseudo = 0x%x\n", tempword);
+		pr_debug("Pseudo = 0x%x\n", tempword);
 		chksum ^= tempword;
 
 		tempword = ft1000_read_reg(dev, FT1000_REG_MAG_DFRL);
-		DEBUG(1, "Pseudo = 0x%x\n", tempword);
+		pr_debug("Pseudo = 0x%x\n", tempword);
 		chksum ^= tempword;
 
 		tempword = ft1000_read_reg(dev, FT1000_REG_MAG_DFRH);
-		DEBUG(1, "Pseudo = 0x%x\n", tempword);
+		pr_debug("Pseudo = 0x%x\n", tempword);
 		chksum ^= tempword;
 
 		tempword = ft1000_read_reg(dev, FT1000_REG_MAG_DFRL);
-		DEBUG(1, "Pseudo = 0x%x\n", tempword);
+		pr_debug("Pseudo = 0x%x\n", tempword);
 		chksum ^= tempword;
 
 		/* read checksum value */
 		tempword = ft1000_read_reg(dev, FT1000_REG_MAG_DFRH);
-		DEBUG(1, "Pseudo = 0x%x\n", tempword);
+		pr_debug("Pseudo = 0x%x\n", tempword);
 	}
 
 	if (chksum != tempword) {
-		DEBUG(0, "Packet checksum mismatch 0x%x 0x%x\n", chksum,
-			  tempword);
+		pr_debug("Packet checksum mismatch 0x%x 0x%x\n",
+			 chksum, tempword);
 		ft1000_flush_fifo(dev, DSP_PKTPHCKSUM_INFO);
 		info->stats.rx_errors++;
 		kfree_skb(skb);
@@ -1687,7 +1643,7 @@ static int ft1000_copy_up_pkt(struct net_device *dev)
 		for (i = 0; i < len / 2; i++) {
 			tempword = ft1000_read_reg(dev, FT1000_REG_DFIFO);
 			*pbuffer++ = (u8) (tempword >> 8);
-			*pbuffer++ = (u8) tempword;
+			*pbuffer++ = (u8)tempword;
 			if (ft1000_chkcard(dev) == false) {
 				kfree_skb(skb);
 				return FAILURE;
@@ -1700,25 +1656,25 @@ static int ft1000_copy_up_pkt(struct net_device *dev)
 			*pbuffer++ = (u8) (tempword >> 8);
 		}
 	} else {
-		ptemplong = (u32 *) pbuffer;
+		ptemplong = (u32 *)pbuffer;
 		for (i = 0; i < len / 4; i++) {
 			templong = inl(dev->base_addr + FT1000_REG_MAG_DFR);
-			DEBUG(1, "Data = 0x%8x\n", templong);
+			pr_debug("Data = 0x%8x\n", templong);
 			*ptemplong++ = templong;
 		}
 
 		/* Need to read one more word if odd align. */
 		if (len & 0x0003) {
 			templong = inl(dev->base_addr + FT1000_REG_MAG_DFR);
-			DEBUG(1, "Data = 0x%8x\n", templong);
+			pr_debug("Data = 0x%8x\n", templong);
 			*ptemplong++ = templong;
 		}
 
 	}
 
-	DEBUG(1, "Data passed to Protocol layer:\n");
+	pr_debug("Data passed to Protocol layer:\n");
 	for (i = 0; i < len + 12; i++) {
-		DEBUG(1, "Protocol Data: 0x%x\n ", *ptemp++);
+		pr_debug("Protocol Data: 0x%x\n", *ptemp++);
 	}
 
 	skb->dev = dev;
@@ -1745,20 +1701,20 @@ static int ft1000_copy_up_pkt(struct net_device *dev)
 
 /*---------------------------------------------------------------------------
 
-   Function:   ft1000_copy_down_pkt
-   Description: This function will take an ethernet packet and convert it to
-               a Flarion packet prior to sending it to the ASIC Downlink
-               FIFO.
-   Input:
-       dev    - device structure
-       packet - address of ethernet packet
-       len    - length of IP packet
-   Output:
-       status - FAILURE
-                SUCCESS
+  Function:   ft1000_copy_down_pkt
+  Description: This function will take an ethernet packet and convert it to
+  a Flarion packet prior to sending it to the ASIC Downlink
+  FIFO.
+  Input:
+  dev    - device structure
+  packet - address of ethernet packet
+  len    - length of IP packet
+  Output:
+  status - FAILURE
+  SUCCESS
 
   -------------------------------------------------------------------------*/
-static int ft1000_copy_down_pkt(struct net_device *dev, u16 * packet, u16 len)
+static int ft1000_copy_down_pkt(struct net_device *dev, u16 *packet, u16 len)
 {
 	struct ft1000_info *info = netdev_priv(dev);
 	struct ft1000_pcmcia *pcmcia = info->priv;
@@ -1770,8 +1726,6 @@ static int ft1000_copy_down_pkt(struct net_device *dev, u16 * packet, u16 len)
 	int i;
 	u32 *plong;
 
-	DEBUG(1, "ft1000_hw: copy_down_pkt()\n");
-
 	/* Check if there is room on the FIFO */
 	if (len > ft1000_read_fifo_len(dev)) {
 		udelay(10);
@@ -1791,8 +1745,7 @@ static int ft1000_copy_down_pkt(struct net_device *dev, u16 * packet, u16 len)
 			udelay(20);
 		}
 		if (len > ft1000_read_fifo_len(dev)) {
-			DEBUG(1,
-				  "ft1000_hw:ft1000_copy_down_pkt:Transmit FIFO is fulli - pkt drop\n");
+			pr_debug("Transmit FIFO is full - pkt drop\n");
 			info->stats.tx_errors++;
 			return SUCCESS;
 		}
@@ -1823,39 +1776,30 @@ static int ft1000_copy_down_pkt(struct net_device *dev, u16 * packet, u16 len)
 	if (info->AsicID == ELECTRABUZZ_ID) {
 		/* copy first word to UFIFO_BEG reg */
 		ft1000_write_reg(dev, FT1000_REG_UFIFO_BEG, pseudo.buff[0]);
-		DEBUG(1, "ft1000_hw:ft1000_copy_down_pkt:data 0 BEG = 0x%04x\n",
-			  pseudo.buff[0]);
+		pr_debug("data 0 BEG = 0x%04x\n", pseudo.buff[0]);
 
 		/* copy subsequent words to UFIFO_MID reg */
 		ft1000_write_reg(dev, FT1000_REG_UFIFO_MID, pseudo.buff[1]);
-		DEBUG(1, "ft1000_hw:ft1000_copy_down_pkt:data 1 MID = 0x%04x\n",
-			  pseudo.buff[1]);
+		pr_debug("data 1 MID = 0x%04x\n", pseudo.buff[1]);
 		ft1000_write_reg(dev, FT1000_REG_UFIFO_MID, pseudo.buff[2]);
-		DEBUG(1, "ft1000_hw:ft1000_copy_down_pkt:data 2 MID = 0x%04x\n",
-			  pseudo.buff[2]);
+		pr_debug("data 2 MID = 0x%04x\n", pseudo.buff[2]);
 		ft1000_write_reg(dev, FT1000_REG_UFIFO_MID, pseudo.buff[3]);
-		DEBUG(1, "ft1000_hw:ft1000_copy_down_pkt:data 3 MID = 0x%04x\n",
-			  pseudo.buff[3]);
+		pr_debug("data 3 MID = 0x%04x\n", pseudo.buff[3]);
 		ft1000_write_reg(dev, FT1000_REG_UFIFO_MID, pseudo.buff[4]);
-		DEBUG(1, "ft1000_hw:ft1000_copy_down_pkt:data 4 MID = 0x%04x\n",
-			  pseudo.buff[4]);
+		pr_debug("data 4 MID = 0x%04x\n", pseudo.buff[4]);
 		ft1000_write_reg(dev, FT1000_REG_UFIFO_MID, pseudo.buff[5]);
-		DEBUG(1, "ft1000_hw:ft1000_copy_down_pkt:data 5 MID = 0x%04x\n",
-			  pseudo.buff[5]);
+		pr_debug("data 5 MID = 0x%04x\n", pseudo.buff[5]);
 		ft1000_write_reg(dev, FT1000_REG_UFIFO_MID, pseudo.buff[6]);
-		DEBUG(1, "ft1000_hw:ft1000_copy_down_pkt:data 6 MID = 0x%04x\n",
-			  pseudo.buff[6]);
+		pr_debug("data 6 MID = 0x%04x\n", pseudo.buff[6]);
 		ft1000_write_reg(dev, FT1000_REG_UFIFO_MID, pseudo.buff[7]);
-		DEBUG(1, "ft1000_hw:ft1000_copy_down_pkt:data 7 MID = 0x%04x\n",
-			  pseudo.buff[7]);
+		pr_debug("data 7 MID = 0x%04x\n", pseudo.buff[7]);
 
 		/* Write PPP type + IP Packet into Downlink FIFO */
 		for (i = 0; i < (len >> 1) - 1; i++) {
 			ft1000_write_reg(dev, FT1000_REG_UFIFO_MID,
 					 htons(*packet));
-			DEBUG(1,
-				  "ft1000_hw:ft1000_copy_down_pkt:data %d MID = 0x%04x\n",
-				  i + 8, htons(*packet));
+			pr_debug("data %d MID = 0x%04x\n",
+				 i + 8, htons(*packet));
 			packet++;
 		}
 
@@ -1863,41 +1807,33 @@ static int ft1000_copy_down_pkt(struct net_device *dev, u16 * packet, u16 len)
 		if (len & 0x0001) {
 			ft1000_write_reg(dev, FT1000_REG_UFIFO_MID,
 					 htons(*packet));
-			DEBUG(1,
-				  "ft1000_hw:ft1000_copy_down_pkt:data MID = 0x%04x\n",
-				  htons(*packet));
+			pr_debug("data MID = 0x%04x\n", htons(*packet));
 			packet++;
 			ft1000_write_reg(dev, FT1000_REG_UFIFO_END,
 					 htons(*packet));
-			DEBUG(1,
-				  "ft1000_hw:ft1000_copy_down_pkt:data %d MID = 0x%04x\n",
-				  i + 8, htons(*packet));
+			pr_debug("data %d MID = 0x%04x\n",
+				 i + 8, htons(*packet));
 		} else {
 			ft1000_write_reg(dev, FT1000_REG_UFIFO_END,
 					 htons(*packet));
-			DEBUG(1,
-				  "ft1000_hw:ft1000_copy_down_pkt:data %d MID = 0x%04x\n",
-				  i + 8, htons(*packet));
+			pr_debug("data %d MID = 0x%04x\n",
+				 i + 8, htons(*packet));
 		}
 	} else {
-		outl(*(u32 *) & pseudo.buff[0],
-			 dev->base_addr + FT1000_REG_MAG_UFDR);
-		DEBUG(1, "ft1000_copy_down_pkt: Pseudo = 0x%8x\n",
-			  *(u32 *) & pseudo.buff[0]);
-		outl(*(u32 *) & pseudo.buff[2],
-			 dev->base_addr + FT1000_REG_MAG_UFDR);
-		DEBUG(1, "ft1000_copy_down_pkt: Pseudo = 0x%8x\n",
-			  *(u32 *) & pseudo.buff[2]);
-		outl(*(u32 *) & pseudo.buff[4],
-			 dev->base_addr + FT1000_REG_MAG_UFDR);
-		DEBUG(1, "ft1000_copy_down_pkt: Pseudo = 0x%8x\n",
-			  *(u32 *) & pseudo.buff[4]);
-		outl(*(u32 *) & pseudo.buff[6],
-			 dev->base_addr + FT1000_REG_MAG_UFDR);
-		DEBUG(1, "ft1000_copy_down_pkt: Pseudo = 0x%8x\n",
-			  *(u32 *) & pseudo.buff[6]);
-
-		plong = (u32 *) packet;
+		outl(*(u32 *)&pseudo.buff[0],
+		     dev->base_addr + FT1000_REG_MAG_UFDR);
+		pr_debug("Pseudo = 0x%8x\n", *(u32 *)&pseudo.buff[0]);
+		outl(*(u32 *)&pseudo.buff[2],
+		     dev->base_addr + FT1000_REG_MAG_UFDR);
+		pr_debug("Pseudo = 0x%8x\n", *(u32 *)&pseudo.buff[2]);
+		outl(*(u32 *)&pseudo.buff[4],
+		     dev->base_addr + FT1000_REG_MAG_UFDR);
+		pr_debug("Pseudo = 0x%8x\n", *(u32 *)&pseudo.buff[4]);
+		outl(*(u32 *)&pseudo.buff[6],
+		     dev->base_addr + FT1000_REG_MAG_UFDR);
+		pr_debug("Pseudo = 0x%8x\n", *(u32 *)&pseudo.buff[6]);
+
+		plong = (u32 *)packet;
 		/* Write PPP type + IP Packet into Downlink FIFO */
 		for (i = 0; i < (len >> 2); i++) {
 			outl(*plong++, dev->base_addr + FT1000_REG_MAG_UFDR);
@@ -1905,9 +1841,7 @@ static int ft1000_copy_down_pkt(struct net_device *dev, u16 * packet, u16 len)
 
 		/* Check for odd alignment */
 		if (len & 0x0003) {
-			DEBUG(1,
-				  "ft1000_hw:ft1000_copy_down_pkt:data = 0x%8x\n",
-				  *plong);
+			pr_debug("data = 0x%8x\n", *plong);
 			outl(*plong++, dev->base_addr + FT1000_REG_MAG_UFDR);
 		}
 		outl(1, dev->base_addr + FT1000_REG_MAG_UFER);
@@ -1928,19 +1862,14 @@ static struct net_device_stats *ft1000_stats(struct net_device *dev)
 
 static int ft1000_open(struct net_device *dev)
 {
-
-	DEBUG(0, "ft1000_hw: ft1000_open is called\n");
-
 	ft1000_reset_card(dev);
-	DEBUG(0, "ft1000_hw: ft1000_open is ended\n");
 
 	/* schedule ft1000_hbchk to perform periodic heartbeat checks on DSP and ASIC */
 	init_timer(&poll_timer);
 	poll_timer.expires = jiffies + (2 * HZ);
-	poll_timer.data = (u_long) dev;
+	poll_timer.data = (u_long)dev;
 	add_timer(&poll_timer);
 
-	DEBUG(0, "ft1000_hw: ft1000_open is ended2\n");
 	return 0;
 }
 
@@ -1948,13 +1877,11 @@ static int ft1000_close(struct net_device *dev)
 {
 	struct ft1000_info *info = netdev_priv(dev);
 
-	DEBUG(0, "ft1000_hw: ft1000_close()\n");
-
 	info->CardReady = 0;
 	del_timer(&poll_timer);
 
 	if (ft1000_card_present == 1) {
-		DEBUG(0, "Media is down\n");
+		pr_debug("Media is down\n");
 		netif_stop_queue(dev);
 
 		ft1000_disable_interrupts(dev);
@@ -1971,31 +1898,28 @@ static int ft1000_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	struct ft1000_info *info = netdev_priv(dev);
 	u8 *pdata;
 
-	DEBUG(1, "ft1000_hw: ft1000_start_xmit()\n");
 	if (skb == NULL) {
-		DEBUG(1, "ft1000_hw: ft1000_start_xmit:skb == NULL!!!\n");
+		pr_debug("skb == NULL!!!\n");
 		return 0;
 	}
 
-	DEBUG(1, "ft1000_hw: ft1000_start_xmit:length of packet = %d\n",
-		  skb->len);
+	pr_debug("length of packet = %d\n", skb->len);
 
-	pdata = (u8 *) skb->data;
+	pdata = (u8 *)skb->data;
 
 	if (info->mediastate == 0) {
 		/* Drop packet is mediastate is down */
-		DEBUG(1, "ft1000_hw:ft1000_copy_down_pkt:mediastate is down\n");
+		pr_debug("mediastate is down\n");
 		return SUCCESS;
 	}
 
 	if ((skb->len < ENET_HEADER_SIZE) || (skb->len > ENET_MAX_SIZE)) {
 		/* Drop packet which has invalid size */
-		DEBUG(1,
-			  "ft1000_hw:ft1000_copy_down_pkt:invalid ethernet length\n");
+		pr_debug("invalid ethernet length\n");
 		return SUCCESS;
 	}
 	ft1000_copy_down_pkt(dev, (u16 *) (pdata + ENET_HEADER_SIZE - 2),
-				 skb->len - ENET_HEADER_SIZE + 2);
+			     skb->len - ENET_HEADER_SIZE + 2);
 
 	dev_kfree_skb(skb);
 
@@ -2004,14 +1928,12 @@ static int ft1000_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 static irqreturn_t ft1000_interrupt(int irq, void *dev_id)
 {
-	struct net_device *dev = (struct net_device *)dev_id;
+	struct net_device *dev = dev_id;
 	struct ft1000_info *info = netdev_priv(dev);
 	u16 tempword;
 	u16 inttype;
 	int cnt;
 
-	DEBUG(1, "ft1000_hw: ft1000_interrupt()\n");
-
 	if (info->CardReady == 0) {
 		ft1000_disable_interrupts(dev);
 		return IRQ_HANDLED;
@@ -2033,19 +1955,19 @@ static irqreturn_t ft1000_interrupt(int irq, void *dev_id)
 			ft1000_parse_dpram_msg(dev);
 
 		if (inttype & ISR_RCV) {
-			DEBUG(1, "Data in FIFO\n");
+			pr_debug("Data in FIFO\n");
 
 			cnt = 0;
 			do {
 				/* Check if we have packets in the Downlink FIFO */
 				if (info->AsicID == ELECTRABUZZ_ID) {
 					tempword =
-					ft1000_read_reg(dev,
-							FT1000_REG_DFIFO_STAT);
+						ft1000_read_reg(dev,
+								FT1000_REG_DFIFO_STAT);
 				} else {
 					tempword =
-					ft1000_read_reg(dev,
-							FT1000_REG_MAG_DFSR);
+						ft1000_read_reg(dev,
+								FT1000_REG_MAG_DFSR);
 				}
 				if (tempword & 0x1f) {
 					ft1000_copy_up_pkt(dev);
@@ -2058,12 +1980,13 @@ static irqreturn_t ft1000_interrupt(int irq, void *dev_id)
 		}
 		/* clear interrupts */
 		tempword = ft1000_read_reg(dev, FT1000_REG_SUP_ISR);
-		DEBUG(1, "ft1000_hw: interrupt status register = 0x%x\n", tempword);
+		pr_debug("interrupt status register = 0x%x\n", tempword);
 		ft1000_write_reg(dev, FT1000_REG_SUP_ISR, tempword);
 
 		/* Read interrupt type */
-		inttype = ft1000_read_reg (dev, FT1000_REG_SUP_ISR);
-		DEBUG(1, "ft1000_hw: interrupt status register after clear = 0x%x\n", inttype);
+		inttype = ft1000_read_reg(dev, FT1000_REG_SUP_ISR);
+		pr_debug("interrupt status register after clear = 0x%x\n",
+			 inttype);
 	}
 	ft1000_enable_interrupts(dev);
 	return IRQ_HANDLED;
@@ -2075,8 +1998,6 @@ void stop_ft1000_card(struct net_device *dev)
 	struct prov_record *ptr;
 	/* int cnt; */
 
-	DEBUG(0, "ft1000_hw: stop_ft1000_card()\n");
-
 	info->CardReady = 0;
 	ft1000_card_present = 0;
 	netif_stop_queue(dev);
@@ -2105,7 +2026,7 @@ void stop_ft1000_card(struct net_device *dev)
 }
 
 static void ft1000_get_drvinfo(struct net_device *dev,
-				   struct ethtool_drvinfo *info)
+			       struct ethtool_drvinfo *info)
 {
 	struct ft1000_info *ft_info;
 	ft_info = netdev_priv(dev);
@@ -2121,6 +2042,7 @@ static void ft1000_get_drvinfo(struct net_device *dev,
 static u32 ft1000_get_link(struct net_device *dev)
 {
 	struct ft1000_info *info;
+
 	info = netdev_priv(dev);
 	return info->mediastate;
 }
@@ -2131,37 +2053,36 @@ static const struct ethtool_ops ops = {
 };
 
 struct net_device *init_ft1000_card(struct pcmcia_device *link,
-						void *ft1000_reset)
+				    void *ft1000_reset)
 {
 	struct ft1000_info *info;
 	struct ft1000_pcmcia *pcmcia;
 	struct net_device *dev;
 
 	static const struct net_device_ops ft1000ops =		/* Slavius 21.10.2009 due to kernel changes */
-	{
-		.ndo_open = &ft1000_open,
-		.ndo_stop = &ft1000_close,
-		.ndo_start_xmit = &ft1000_start_xmit,
-		.ndo_get_stats = &ft1000_stats,
-	};
+		{
+			.ndo_open = &ft1000_open,
+			.ndo_stop = &ft1000_close,
+			.ndo_start_xmit = &ft1000_start_xmit,
+			.ndo_get_stats = &ft1000_stats,
+		};
 
-	DEBUG(1, "ft1000_hw: init_ft1000_card()\n");
-	DEBUG(1, "ft1000_hw: irq = %d\n", link->irq);
-	DEBUG(1, "ft1000_hw: port = 0x%04x\n", link->resource[0]->start);
+	pr_debug("irq = %d, port = 0x%04llx\n",
+		 link->irq, (unsigned long long)link->resource[0]->start);
 
 	flarion_ft1000_cnt++;
 
 	if (flarion_ft1000_cnt > 1) {
 		flarion_ft1000_cnt--;
 
-		printk(KERN_INFO
-			   "ft1000: This driver can not support more than one instance\n");
+		dev_info(&link->dev,
+			 "This driver can not support more than one instance\n");
 		return NULL;
 	}
 
 	dev = alloc_etherdev(sizeof(struct ft1000_info));
 	if (!dev) {
-		printk(KERN_ERR "ft1000: failed to allocate etherdev\n");
+		dev_err(&link->dev, "Failed to allocate etherdev\n");
 		return NULL;
 	}
 
@@ -2170,9 +2091,9 @@ struct net_device *init_ft1000_card(struct pcmcia_device *link,
 
 	memset(info, 0, sizeof(struct ft1000_info));
 
-	DEBUG(1, "address of dev = 0x%8x\n", (u32) dev);
-	DEBUG(1, "address of dev info = 0x%8x\n", (u32) info);
-	DEBUG(0, "device name = %s\n", dev->name);
+	pr_debug("address of dev = 0x%p\n", dev);
+	pr_debug("address of dev info = 0x%p\n", info);
+	pr_debug("device name = %s\n", dev->name);
 
 	memset(&info->stats, 0, sizeof(struct net_device_stats));
 
@@ -2204,41 +2125,41 @@ struct net_device *init_ft1000_card(struct pcmcia_device *link,
 
 	dev->netdev_ops = &ft1000ops;		/* Slavius 21.10.2009 due to kernel changes */
 
-	DEBUG(0, "device name = %s\n", dev->name);
+	pr_debug("device name = %s\n", dev->name);
 
 	dev->irq = link->irq;
 	dev->base_addr = link->resource[0]->start;
 	if (pcmcia_get_mac_from_cis(link, dev)) {
-		printk(KERN_ERR "ft1000: Could not read mac address\n");
+		netdev_err(dev, "Could not read mac address\n");
 		goto err_dev;
 	}
 
 	if (request_irq(dev->irq, ft1000_interrupt, IRQF_SHARED, dev->name, dev)) {
-		printk(KERN_ERR "ft1000: Could not request_irq\n");
+		netdev_err(dev, "Could not request_irq\n");
 		goto err_dev;
 	}
 
 	if (request_region(dev->base_addr, 256, dev->name) == NULL) {
-		printk(KERN_ERR "ft1000: Could not request_region\n");
+		netdev_err(dev, "Could not request_region\n");
 		goto err_irq;
 	}
 
 	if (register_netdev(dev) != 0) {
-		DEBUG(0, "ft1000: Could not register netdev");
+		pr_debug("Could not register netdev\n");
 		goto err_reg;
 	}
 
 	info->AsicID = ft1000_read_reg(dev, FT1000_REG_ASIC_ID);
 	if (info->AsicID == ELECTRABUZZ_ID) {
-		DEBUG(0, "ft1000_hw: ELECTRABUZZ ASIC\n");
+		pr_debug("ELECTRABUZZ ASIC\n");
 		if (request_firmware(&fw_entry, "ft1000.img", &link->dev) != 0) {
-			printk(KERN_INFO "ft1000: Could not open ft1000.img\n");
+			pr_info("Could not open ft1000.img\n");
 			goto err_unreg;
 		}
 	} else {
-		DEBUG(0, "ft1000_hw: MAGNEMITE ASIC\n");
+		pr_debug("MAGNEMITE ASIC\n");
 		if (request_firmware(&fw_entry, "ft2000.img", &link->dev) != 0) {
-			printk(KERN_INFO "ft1000: Could not open ft2000.img\n");
+			pr_info("Could not open ft2000.img\n");
 			goto err_unreg;
 		}
 	}
@@ -2247,8 +2168,8 @@ struct net_device *init_ft1000_card(struct pcmcia_device *link,
 
 	ft1000_card_present = 1;
 	dev->ethtool_ops = &ops;
-	printk(KERN_INFO "ft1000: %s: addr 0x%04lx irq %d, MAC addr %pM\n",
-			dev->name, dev->base_addr, dev->irq, dev->dev_addr);
+	pr_info("%s: addr 0x%04lx irq %d, MAC addr %pM\n",
+		dev->name, dev->base_addr, dev->irq, dev->dev_addr);
 	return dev;
 
 err_unreg:
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c b/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
index 0f347ab0d300..c8d278229940 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
@@ -1,31 +1,34 @@
 /*
-*---------------------------------------------------------------------------
-* FT1000 driver for Flarion Flash OFDM NIC Device
-*
-* Copyright (C) 2006 Flarion Technologies, All rights reserved.
-*
-* This program is free software; you can redistribute it and/or modify it
-* under the terms of the GNU General Public License as published by the Free
-* Software Foundation; either version 2 of the License, or (at your option) any
-* later version. This program is distributed in the hope that it will be useful,
-* but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
-* or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
-* more details. You should have received a copy of the GNU General Public
-* License along with this program; if not, write to the
-* Free Software Foundation, Inc., 59 Temple Place -
-* Suite 330, Boston, MA 02111-1307, USA.
-*---------------------------------------------------------------------------
-*
-* File:         ft1000_chdev.c
-*
-* Description:  Custom character device dispatch routines.
-*
-* History:
-* 8/29/02    Whc                Ported to Linux.
-* 6/05/06    Whc                Porting to Linux 2.6.9
-*
-*---------------------------------------------------------------------------
-*/
+ *---------------------------------------------------------------------------
+ * FT1000 driver for Flarion Flash OFDM NIC Device
+ *
+ * Copyright (C) 2006 Flarion Technologies, All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option) any
+ * later version. This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details. You should have received a copy of the GNU General Public
+ * License along with this program; if not, write to the
+ * Free Software Foundation, Inc., 59 Temple Place -
+ * Suite 330, Boston, MA 02111-1307, USA.
+ *---------------------------------------------------------------------------
+ *
+ * File:         ft1000_chdev.c
+ *
+ * Description:  Custom character device dispatch routines.
+ *
+ * History:
+ * 8/29/02    Whc                Ported to Linux.
+ * 6/05/06    Whc                Porting to Linux 2.6.9
+ *
+ *---------------------------------------------------------------------------
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/sched.h>
@@ -38,12 +41,12 @@
 #include <linux/debugfs.h>
 #include "ft1000_usb.h"
 
-static int ft1000_flarion_cnt = 0;
+static int ft1000_flarion_cnt;
 
 static int ft1000_open(struct inode *inode, struct file *file);
 static unsigned int ft1000_poll_dev(struct file *file, poll_table *wait);
 static long ft1000_ioctl(struct file *file, unsigned int command,
-                           unsigned long argument);
+			 unsigned long argument);
 static int ft1000_release(struct inode *inode, struct file *file);
 
 /* List to free receive command buffer pool */
@@ -55,8 +58,8 @@ spinlock_t free_buff_lock;
 int numofmsgbuf = 0;
 
 /*
-* Table of entry-point routines for char device
-*/
+ * Table of entry-point routines for char device
+ */
 static const struct file_operations ft1000fops = {
 	.unlocked_ioctl	= ft1000_ioctl,
 	.poll		= ft1000_poll_dev,
@@ -66,104 +69,104 @@ static const struct file_operations ft1000fops = {
 };
 
 /*
----------------------------------------------------------------------------
-* Function:    ft1000_get_buffer
-*
-* Parameters:
-*
-* Returns:
-*
-* Description:
-*
-* Notes:
-*
-*---------------------------------------------------------------------------
-*/
+  ---------------------------------------------------------------------------
+  * Function:    ft1000_get_buffer
+  *
+  * Parameters:
+  *
+  * Returns:
+  *
+  * Description:
+  *
+  * Notes:
+  *
+  *---------------------------------------------------------------------------
+  */
 struct dpram_blk *ft1000_get_buffer(struct list_head *bufflist)
 {
-    unsigned long flags;
+	unsigned long flags;
 	struct dpram_blk *ptr;
 
-    spin_lock_irqsave(&free_buff_lock, flags);
-    /* Check if buffer is available */
-    if (list_empty(bufflist)) {
-        DEBUG("ft1000_get_buffer:  No more buffer - %d\n", numofmsgbuf);
-        ptr = NULL;
-    } else {
-        numofmsgbuf--;
-	ptr = list_entry(bufflist->next, struct dpram_blk, list);
-        list_del(&ptr->list);
-        /* DEBUG("ft1000_get_buffer: number of free msg buffers = %d\n", numofmsgbuf); */
-    }
-    spin_unlock_irqrestore(&free_buff_lock, flags);
-
-    return ptr;
+	spin_lock_irqsave(&free_buff_lock, flags);
+	/* Check if buffer is available */
+	if (list_empty(bufflist)) {
+		pr_debug("No more buffer - %d\n", numofmsgbuf);
+		ptr = NULL;
+	} else {
+		numofmsgbuf--;
+		ptr = list_entry(bufflist->next, struct dpram_blk, list);
+		list_del(&ptr->list);
+		/* pr_debug("number of free msg buffers = %d\n", numofmsgbuf); */
+	}
+	spin_unlock_irqrestore(&free_buff_lock, flags);
+
+	return ptr;
 }
 
 
 
 
 /*
-*---------------------------------------------------------------------------
-* Function:    ft1000_free_buffer
-*
-* Parameters:
-*
-* Returns:
-*
-* Description:
-*
-* Notes:
-*
-*---------------------------------------------------------------------------
-*/
+ *---------------------------------------------------------------------------
+ * Function:    ft1000_free_buffer
+ *
+ * Parameters:
+ *
+ * Returns:
+ *
+ * Description:
+ *
+ * Notes:
+ *
+ *---------------------------------------------------------------------------
+ */
 void ft1000_free_buffer(struct dpram_blk *pdpram_blk, struct list_head *plist)
 {
-    unsigned long flags;
-
-    spin_lock_irqsave(&free_buff_lock, flags);
-    /* Put memory back to list */
-    list_add_tail(&pdpram_blk->list, plist);
-    numofmsgbuf++;
-    /*DEBUG("ft1000_free_buffer: number of free msg buffers = %d\n", numofmsgbuf); */
-    spin_unlock_irqrestore(&free_buff_lock, flags);
+	unsigned long flags;
+
+	spin_lock_irqsave(&free_buff_lock, flags);
+	/* Put memory back to list */
+	list_add_tail(&pdpram_blk->list, plist);
+	numofmsgbuf++;
+	/*pr_debug("number of free msg buffers = %d\n", numofmsgbuf); */
+	spin_unlock_irqrestore(&free_buff_lock, flags);
 }
 
 /*
-*---------------------------------------------------------------------------
-* Function:    ft1000_CreateDevice
-*
-* Parameters:  dev - pointer to adapter object
-*
-* Returns:     0 if successful
-*
-* Description: Creates a private char device.
-*
-* Notes:       Only called by init_module().
-*
-*---------------------------------------------------------------------------
-*/
+ *---------------------------------------------------------------------------
+ * Function:    ft1000_CreateDevice
+ *
+ * Parameters:  dev - pointer to adapter object
+ *
+ * Returns:     0 if successful
+ *
+ * Description: Creates a private char device.
+ *
+ * Notes:       Only called by init_module().
+ *
+ *---------------------------------------------------------------------------
+ */
 int ft1000_create_dev(struct ft1000_usb *dev)
 {
-    int result;
-    int i;
+	int result;
+	int i;
 	struct dentry *dir, *file;
 	struct ft1000_debug_dirs *tmp;
 
-    /* make a new device name */
-    sprintf(dev->DeviceName, "%s%d", "FT1000_", dev->CardNumber);
+	/* make a new device name */
+	sprintf(dev->DeviceName, "%s%d", "FT1000_", dev->CardNumber);
 
-    DEBUG("%s: number of instance = %d\n", __func__, ft1000_flarion_cnt);
-    DEBUG("DeviceCreated = %x\n", dev->DeviceCreated);
+	pr_debug("number of instance = %d\n", ft1000_flarion_cnt);
+	pr_debug("DeviceCreated = %x\n", dev->DeviceCreated);
 
-    if (dev->DeviceCreated) {
-	DEBUG("%s: \"%s\" already registered\n", __func__, dev->DeviceName);
-	return -EIO;
-    }
+	if (dev->DeviceCreated) {
+		pr_debug("\"%s\" already registered\n", dev->DeviceName);
+		return -EIO;
+	}
 
 
-    /* register the device */
-    DEBUG("%s: \"%s\" debugfs device registration\n", __func__, dev->DeviceName);
+	/* register the device */
+	pr_debug("\"%s\" debugfs device registration\n", dev->DeviceName);
 
 	tmp = kmalloc(sizeof(struct ft1000_debug_dirs), GFP_KERNEL);
 	if (tmp == NULL) {
@@ -178,7 +181,7 @@ int ft1000_create_dev(struct ft1000_usb *dev)
 	}
 
 	file = debugfs_create_file("device", S_IRUGO | S_IWUSR, dir,
-					dev, &ft1000fops);
+				   dev, &ft1000fops);
 	if (IS_ERR(file)) {
 		result = PTR_ERR(file);
 		goto debug_file_fail;
@@ -189,25 +192,25 @@ int ft1000_create_dev(struct ft1000_usb *dev)
 	tmp->int_number = dev->CardNumber;
 	list_add(&(tmp->list), &(dev->nodes.list));
 
-    DEBUG("%s: registered debugfs directory \"%s\"\n", __func__, dev->DeviceName);
-
-    /* initialize application information */
-    dev->appcnt = 0;
-    for (i=0; i<MAX_NUM_APP; i++) {
-        dev->app_info[i].nTxMsg = 0;
-        dev->app_info[i].nRxMsg = 0;
-        dev->app_info[i].nTxMsgReject = 0;
-        dev->app_info[i].nRxMsgMiss = 0;
-        dev->app_info[i].fileobject = NULL;
-        dev->app_info[i].app_id = i+1;
-        dev->app_info[i].DspBCMsgFlag = 0;
-        dev->app_info[i].NumOfMsg = 0;
-        init_waitqueue_head(&dev->app_info[i].wait_dpram_msg);
-        INIT_LIST_HEAD(&dev->app_info[i].app_sqlist);
-    }
-
-    dev->DeviceCreated = TRUE;
-    ft1000_flarion_cnt++;
+	pr_debug("registered debugfs directory \"%s\"\n", dev->DeviceName);
+
+	/* initialize application information */
+	dev->appcnt = 0;
+	for (i = 0; i < MAX_NUM_APP; i++) {
+		dev->app_info[i].nTxMsg = 0;
+		dev->app_info[i].nRxMsg = 0;
+		dev->app_info[i].nTxMsgReject = 0;
+		dev->app_info[i].nRxMsgMiss = 0;
+		dev->app_info[i].fileobject = NULL;
+		dev->app_info[i].app_id = i+1;
+		dev->app_info[i].DspBCMsgFlag = 0;
+		dev->app_info[i].NumOfMsg = 0;
+		init_waitqueue_head(&dev->app_info[i].wait_dpram_msg);
+		INIT_LIST_HEAD(&dev->app_info[i].app_sqlist);
+	}
+
+	dev->DeviceCreated = TRUE;
+	ft1000_flarion_cnt++;
 
 	return 0;
 
@@ -220,33 +223,29 @@ fail:
 }
 
 /*
-*---------------------------------------------------------------------------
-* Function:    ft1000_DestroyDeviceDEBUG
-*
-* Parameters:  dev - pointer to adapter object
-*
-* Description: Destroys a private char device.
-*
-* Notes:       Only called by cleanup_module().
-*
-*---------------------------------------------------------------------------
-*/
+ *---------------------------------------------------------------------------
+ * Function:    ft1000_DestroyDeviceDEBUG
+ *
+ * Parameters:  dev - pointer to adapter object
+ *
+ * Description: Destroys a private char device.
+ *
+ * Notes:       Only called by cleanup_module().
+ *
+ *---------------------------------------------------------------------------
+ */
 void ft1000_destroy_dev(struct net_device *netdev)
 {
 	struct ft1000_info *info = netdev_priv(netdev);
 	struct ft1000_usb *dev = info->priv;
-		int i;
+	int i;
 	struct dpram_blk *pdpram_blk;
 	struct dpram_blk *ptr;
 	struct list_head *pos, *q;
 	struct ft1000_debug_dirs *dir;
 
-    DEBUG("%s called\n", __func__);
-
-
-
-    if (dev->DeviceCreated) {
-        ft1000_flarion_cnt--;
+	if (dev->DeviceCreated) {
+		ft1000_flarion_cnt--;
 		list_for_each_safe(pos, q, &dev->nodes.list) {
 			dir = list_entry(pos, struct ft1000_debug_dirs, list);
 			if (dir->int_number == dev->CardNumber) {
@@ -256,29 +255,28 @@ void ft1000_destroy_dev(struct net_device *netdev)
 				kfree(dir);
 			}
 		}
-		DEBUG("%s: unregistered device \"%s\"\n", __func__,
-					   dev->DeviceName);
-
-        /* Make sure we free any memory reserve for slow Queue */
-        for (i=0; i<MAX_NUM_APP; i++) {
-            while (list_empty(&dev->app_info[i].app_sqlist) == 0) {
-                pdpram_blk = list_entry(dev->app_info[i].app_sqlist.next, struct dpram_blk, list);
-                list_del(&pdpram_blk->list);
-                ft1000_free_buffer(pdpram_blk, &freercvpool);
-
-            }
-            wake_up_interruptible(&dev->app_info[i].wait_dpram_msg);
-        }
-
-        /* Remove buffer allocated for receive command data */
-        if (ft1000_flarion_cnt == 0) {
-            while (list_empty(&freercvpool) == 0) {
-		ptr = list_entry(freercvpool.next, struct dpram_blk, list);
-                list_del(&ptr->list);
-                kfree(ptr->pbuffer);
-                kfree(ptr);
-            }
-        }
+		pr_debug("unregistered device \"%s\"\n", dev->DeviceName);
+
+		/* Make sure we free any memory reserve for slow Queue */
+		for (i = 0; i < MAX_NUM_APP; i++) {
+			while (list_empty(&dev->app_info[i].app_sqlist) == 0) {
+				pdpram_blk = list_entry(dev->app_info[i].app_sqlist.next, struct dpram_blk, list);
+				list_del(&pdpram_blk->list);
+				ft1000_free_buffer(pdpram_blk, &freercvpool);
+
+			}
+			wake_up_interruptible(&dev->app_info[i].wait_dpram_msg);
+		}
+
+		/* Remove buffer allocated for receive command data */
+		if (ft1000_flarion_cnt == 0) {
+			while (list_empty(&freercvpool) == 0) {
+				ptr = list_entry(freercvpool.next, struct dpram_blk, list);
+				list_del(&ptr->list);
+				kfree(ptr->pbuffer);
+				kfree(ptr);
+			}
+		}
 		dev->DeviceCreated = FALSE;
 	}
 
@@ -286,507 +284,499 @@ void ft1000_destroy_dev(struct net_device *netdev)
 }
 
 /*
-*---------------------------------------------------------------------------
-* Function:    ft1000_open
-*
-* Parameters:
-*
-* Description:
-*
-* Notes:
-*
-*---------------------------------------------------------------------------
-*/
+ *---------------------------------------------------------------------------
+ * Function:    ft1000_open
+ *
+ * Parameters:
+ *
+ * Description:
+ *
+ * Notes:
+ *
+ *---------------------------------------------------------------------------
+ */
 static int ft1000_open(struct inode *inode, struct file *file)
 {
 	struct ft1000_info *info;
 	struct ft1000_usb *dev = (struct ft1000_usb *)inode->i_private;
-    int i,num;
+	int i, num;
 
-    DEBUG("%s called\n", __func__);
-    num = (MINOR(inode->i_rdev) & 0xf);
-    DEBUG("ft1000_open: minor number=%d\n", num);
+	num = (MINOR(inode->i_rdev) & 0xf);
+	pr_debug("minor number=%d\n", num);
 
 	info = file->private_data = netdev_priv(dev->net);
 
-    DEBUG("f_owner = %p number of application = %d\n", (&file->f_owner), dev->appcnt);
-
-    /* Check if maximum number of application exceeded */
-    if (dev->appcnt > MAX_NUM_APP) {
-        DEBUG("Maximum number of application exceeded\n");
-        return -EACCES;
-    }
-
-    /* Search for available application info block */
-    for (i=0; i<MAX_NUM_APP; i++) {
-        if ((dev->app_info[i].fileobject == NULL)) {
-            break;
-        }
-    }
-
-    /* Fail due to lack of application info block */
-    if (i == MAX_NUM_APP) {
-        DEBUG("Could not find an application info block\n");
-        return -EACCES;
-    }
-
-    dev->appcnt++;
-    dev->app_info[i].fileobject = &file->f_owner;
-    dev->app_info[i].nTxMsg = 0;
-    dev->app_info[i].nRxMsg = 0;
-    dev->app_info[i].nTxMsgReject = 0;
-    dev->app_info[i].nRxMsgMiss = 0;
+	pr_debug("f_owner = %p number of application = %d\n",
+		 &file->f_owner, dev->appcnt);
+
+	/* Check if maximum number of application exceeded */
+	if (dev->appcnt > MAX_NUM_APP) {
+		pr_debug("Maximum number of application exceeded\n");
+		return -EACCES;
+	}
+
+	/* Search for available application info block */
+	for (i = 0; i < MAX_NUM_APP; i++) {
+		if ((dev->app_info[i].fileobject == NULL)) {
+			break;
+		}
+	}
+
+	/* Fail due to lack of application info block */
+	if (i == MAX_NUM_APP) {
+		pr_debug("Could not find an application info block\n");
+		return -EACCES;
+	}
+
+	dev->appcnt++;
+	dev->app_info[i].fileobject = &file->f_owner;
+	dev->app_info[i].nTxMsg = 0;
+	dev->app_info[i].nRxMsg = 0;
+	dev->app_info[i].nTxMsgReject = 0;
+	dev->app_info[i].nRxMsgMiss = 0;
 
 	nonseekable_open(inode, file);
-    return 0;
+	return 0;
 }
 
 
 /*
-*---------------------------------------------------------------------------
-* Function:    ft1000_poll_dev
-*
-* Parameters:
-*
-* Description:
-*
-* Notes:
-*
-*---------------------------------------------------------------------------
-*/
+ *---------------------------------------------------------------------------
+ * Function:    ft1000_poll_dev
+ *
+ * Parameters:
+ *
+ * Description:
+ *
+ * Notes:
+ *
+ *---------------------------------------------------------------------------
+ */
 
 static unsigned int ft1000_poll_dev(struct file *file, poll_table *wait)
 {
-    struct net_device *netdev = file->private_data;
+	struct net_device *netdev = file->private_data;
 	struct ft1000_info *info = netdev_priv(netdev);
 	struct ft1000_usb *dev = info->priv;
-    int i;
-
-    /* DEBUG("ft1000_poll_dev called\n"); */
-    if (ft1000_flarion_cnt == 0) {
-        DEBUG("FT1000:ft1000_poll_dev called when ft1000_flarion_cnt is zero\n");
-        return (-EBADF);
-    }
-
-    /* Search for matching file object */
-    for (i=0; i<MAX_NUM_APP; i++) {
-        if (dev->app_info[i].fileobject == &file->f_owner) {
-            /* DEBUG("FT1000:ft1000_ioctl: Message is for AppId = %d\n", dev->app_info[i].app_id); */
-            break;
-        }
-    }
-
-    /* Could not find application info block */
-    if (i == MAX_NUM_APP) {
-        DEBUG("FT1000:ft1000_ioctl:Could not find application info block\n");
-        return (-EACCES);
-    }
-
-    if (list_empty(&dev->app_info[i].app_sqlist) == 0) {
-        DEBUG("FT1000:ft1000_poll_dev:Message detected in slow queue\n");
-        return(POLLIN | POLLRDNORM | POLLPRI);
-    }
-
-    poll_wait(file, &dev->app_info[i].wait_dpram_msg, wait);
-    /* DEBUG("FT1000:ft1000_poll_dev:Polling for data from DSP\n"); */
+	int i;
+
+	if (ft1000_flarion_cnt == 0) {
+		pr_debug("called with ft1000_flarion_cnt value zero\n");
+		return -EBADF;
+	}
+
+	/* Search for matching file object */
+	for (i = 0; i < MAX_NUM_APP; i++) {
+		if (dev->app_info[i].fileobject == &file->f_owner) {
+			/* pr_debug("Message is for AppId = %d\n", dev->app_info[i].app_id); */
+			break;
+		}
+	}
+
+	/* Could not find application info block */
+	if (i == MAX_NUM_APP) {
+		pr_debug("Could not find application info block\n");
+		return -EACCES;
+	}
+
+	if (list_empty(&dev->app_info[i].app_sqlist) == 0) {
+		pr_debug("Message detected in slow queue\n");
+		return(POLLIN | POLLRDNORM | POLLPRI);
+	}
+
+	poll_wait(file, &dev->app_info[i].wait_dpram_msg, wait);
+	/* pr_debug("Polling for data from DSP\n"); */
 
 	return 0;
 }
 
 /*
-*---------------------------------------------------------------------------
-* Function:    ft1000_ioctl
-*
-* Parameters:
-*
-* Description:
-*
-* Notes:
-*
-*---------------------------------------------------------------------------
-*/
+ *---------------------------------------------------------------------------
+ * Function:    ft1000_ioctl
+ *
+ * Parameters:
+ *
+ * Description:
+ *
+ * Notes:
+ *
+ *---------------------------------------------------------------------------
+ */
 static long ft1000_ioctl(struct file *file, unsigned int command,
-                           unsigned long argument)
+			 unsigned long argument)
 {
-    void __user *argp = (void __user *)argument;
+	void __user *argp = (void __user *)argument;
 	struct ft1000_info *info;
-    struct ft1000_usb *ft1000dev;
-    int result=0;
-    int cmd;
-    int i;
-    u16 tempword;
-    unsigned long flags;
-    struct timeval tv;
+	struct ft1000_usb *ft1000dev;
+	int result = 0;
+	int cmd;
+	int i;
+	u16 tempword;
+	unsigned long flags;
+	struct timeval tv;
 	struct IOCTL_GET_VER get_ver_data;
 	struct IOCTL_GET_DSP_STAT get_stat_data;
-    u8 ConnectionMsg[] = {0x00,0x44,0x10,0x20,0x80,0x00,0x00,0x00,0x00,0x00,0x03,0x00,0x00,0x00,0x93,0x64,
-                          0x00,0x00,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x00,0x00,0x00,0x0a,
-                          0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-                          0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x12,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-                          0x00,0x00,0x02,0x37,0x00,0x00,0x00,0x08,0x00,0x00,0x00,0x01,0x00,0x01,0x7f,0x00,
-                          0x00,0x01,0x00,0x00};
+	u8 ConnectionMsg[] = {0x00, 0x44, 0x10, 0x20, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x93, 0x64,
+			      0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x0a,
+			      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x12, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			      0x00, 0x00, 0x02, 0x37, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7f, 0x00,
+			      0x00, 0x01, 0x00, 0x00};
+
+	unsigned short ledStat = 0;
+	unsigned short conStat = 0;
+
+	if (ft1000_flarion_cnt == 0) {
+		pr_debug("called with ft1000_flarion_cnt of zero\n");
+		return -EBADF;
+	}
 
-    unsigned short ledStat=0;
-    unsigned short conStat=0;
+	/* pr_debug("command = 0x%x argument = 0x%8x\n", command, (u32)argument); */
 
-    /* DEBUG("ft1000_ioctl called\n"); */
+	info = file->private_data;
+	ft1000dev = info->priv;
+	cmd = _IOC_NR(command);
+	/* pr_debug("cmd = 0x%x\n", cmd); */
+
+	/* process the command */
+	switch (cmd) {
+	case IOCTL_REGISTER_CMD:
+		pr_debug("IOCTL_FT1000_REGISTER called\n");
+		result = get_user(tempword, (__u16 __user *)argp);
+		if (result) {
+			pr_debug("result = %d failed to get_user\n", result);
+			break;
+		}
+		if (tempword == DSPBCMSGID) {
+			/* Search for matching file object */
+			for (i = 0; i < MAX_NUM_APP; i++) {
+				if (ft1000dev->app_info[i].fileobject == &file->f_owner) {
+					ft1000dev->app_info[i].DspBCMsgFlag = 1;
+					pr_debug("Registered for broadcast messages\n");
+					break;
+				}
+			}
+		}
+		break;
 
-    if (ft1000_flarion_cnt == 0) {
-        DEBUG("FT1000:ft1000_ioctl called when ft1000_flarion_cnt is zero\n");
-        return (-EBADF);
-    }
+	case IOCTL_GET_VER_CMD:
+		pr_debug("IOCTL_FT1000_GET_VER called\n");
 
-    /* DEBUG("FT1000:ft1000_ioctl:command = 0x%x argument = 0x%8x\n", command, (u32)argument); */
+		get_ver_data.drv_ver = FT1000_DRV_VER;
 
-	info = file->private_data;
-	ft1000dev = info->priv;
-    cmd = _IOC_NR(command);
-    /* DEBUG("FT1000:ft1000_ioctl:cmd = 0x%x\n", cmd); */
-
-    /* process the command */
-    switch (cmd) {
-    case IOCTL_REGISTER_CMD:
-            DEBUG("FT1000:ft1000_ioctl: IOCTL_FT1000_REGISTER called\n");
-            result = get_user(tempword, (__u16 __user*)argp);
-            if (result) {
-                DEBUG("result = %d failed to get_user\n", result);
-                break;
-            }
-            if (tempword == DSPBCMSGID) {
-                /* Search for matching file object */
-                for (i=0; i<MAX_NUM_APP; i++) {
-                    if (ft1000dev->app_info[i].fileobject == &file->f_owner) {
-                        ft1000dev->app_info[i].DspBCMsgFlag = 1;
-                        DEBUG("FT1000:ft1000_ioctl:Registered for broadcast messages\n");
-                        break;
-                    }
-                }
-            }
-            break;
-
-    case IOCTL_GET_VER_CMD:
-        DEBUG("FT1000:ft1000_ioctl: IOCTL_FT1000_GET_VER called\n");
-
-        get_ver_data.drv_ver = FT1000_DRV_VER;
-
-        if (copy_to_user(argp, &get_ver_data, sizeof(get_ver_data))) {
-            DEBUG("FT1000:ft1000_ioctl: copy fault occurred\n");
-            result = -EFAULT;
-            break;
-        }
-
-        DEBUG("FT1000:ft1000_ioctl:driver version = 0x%x\n",(unsigned int)get_ver_data.drv_ver);
-
-        break;
-    case IOCTL_CONNECT:
-        /* Connect Message */
-        DEBUG("FT1000:ft1000_ioctl: IOCTL_FT1000_CONNECT\n");
-        ConnectionMsg[79] = 0xfc;
-			   result = card_send_command(ft1000dev, (unsigned short *)ConnectionMsg, 0x4c);
-
-        break;
-    case IOCTL_DISCONNECT:
-        /* Disconnect Message */
-        DEBUG("FT1000:ft1000_ioctl: IOCTL_FT1000_DISCONNECT\n");
-        ConnectionMsg[79] = 0xfd;
-			   result = card_send_command(ft1000dev, (unsigned short *)ConnectionMsg, 0x4c);
-        break;
-    case IOCTL_GET_DSP_STAT_CMD:
-        /* DEBUG("FT1000:ft1000_ioctl: IOCTL_FT1000_GET_DSP_STAT called\n"); */
-	memset(&get_stat_data, 0, sizeof(get_stat_data));
-        memcpy(get_stat_data.DspVer, info->DspVer, DSPVERSZ);
-        memcpy(get_stat_data.HwSerNum, info->HwSerNum, HWSERNUMSZ);
-        memcpy(get_stat_data.Sku, info->Sku, SKUSZ);
-        memcpy(get_stat_data.eui64, info->eui64, EUISZ);
-
-            if (info->ProgConStat != 0xFF) {
-                ft1000_read_dpram16(ft1000dev, FT1000_MAG_DSP_LED, (u8 *)&ledStat, FT1000_MAG_DSP_LED_INDX);
-                get_stat_data.LedStat = ntohs(ledStat);
-                DEBUG("FT1000:ft1000_ioctl: LedStat = 0x%x\n", get_stat_data.LedStat);
-                ft1000_read_dpram16(ft1000dev, FT1000_MAG_DSP_CON_STATE, (u8 *)&conStat, FT1000_MAG_DSP_CON_STATE_INDX);
-                get_stat_data.ConStat = ntohs(conStat);
-                DEBUG("FT1000:ft1000_ioctl: ConStat = 0x%x\n", get_stat_data.ConStat);
-            } else {
-                get_stat_data.ConStat = 0x0f;
-            }
-
-
-        get_stat_data.nTxPkts = info->stats.tx_packets;
-        get_stat_data.nRxPkts = info->stats.rx_packets;
-        get_stat_data.nTxBytes = info->stats.tx_bytes;
-        get_stat_data.nRxBytes = info->stats.rx_bytes;
-        do_gettimeofday(&tv);
-        get_stat_data.ConTm = (u32)(tv.tv_sec - info->ConTm);
-        DEBUG("Connection Time = %d\n", (int)get_stat_data.ConTm);
-        if (copy_to_user(argp, &get_stat_data, sizeof(get_stat_data))) {
-            DEBUG("FT1000:ft1000_ioctl: copy fault occurred\n");
-            result = -EFAULT;
-            break;
-        }
-        DEBUG("ft1000_chioctl: GET_DSP_STAT succeed\n");
-        break;
-    case IOCTL_SET_DPRAM_CMD:
-        {
+		if (copy_to_user(argp, &get_ver_data, sizeof(get_ver_data))) {
+			pr_debug("copy fault occurred\n");
+			result = -EFAULT;
+			break;
+		}
+
+		pr_debug("driver version = 0x%x\n",
+			 (unsigned int)get_ver_data.drv_ver);
+
+		break;
+	case IOCTL_CONNECT:
+		/* Connect Message */
+		pr_debug("IOCTL_FT1000_CONNECT\n");
+		ConnectionMsg[79] = 0xfc;
+		result = card_send_command(ft1000dev, (unsigned short *)ConnectionMsg, 0x4c);
+
+		break;
+	case IOCTL_DISCONNECT:
+		/* Disconnect Message */
+		pr_debug("IOCTL_FT1000_DISCONNECT\n");
+		ConnectionMsg[79] = 0xfd;
+		result = card_send_command(ft1000dev, (unsigned short *)ConnectionMsg, 0x4c);
+		break;
+	case IOCTL_GET_DSP_STAT_CMD:
+		/* pr_debug("IOCTL_FT1000_GET_DSP_STAT\n"); */
+		memset(&get_stat_data, 0, sizeof(get_stat_data));
+		memcpy(get_stat_data.DspVer, info->DspVer, DSPVERSZ);
+		memcpy(get_stat_data.HwSerNum, info->HwSerNum, HWSERNUMSZ);
+		memcpy(get_stat_data.Sku, info->Sku, SKUSZ);
+		memcpy(get_stat_data.eui64, info->eui64, EUISZ);
+
+		if (info->ProgConStat != 0xFF) {
+			ft1000_read_dpram16(ft1000dev, FT1000_MAG_DSP_LED, (u8 *)&ledStat, FT1000_MAG_DSP_LED_INDX);
+			get_stat_data.LedStat = ntohs(ledStat);
+			pr_debug("LedStat = 0x%x\n", get_stat_data.LedStat);
+			ft1000_read_dpram16(ft1000dev, FT1000_MAG_DSP_CON_STATE, (u8 *)&conStat, FT1000_MAG_DSP_CON_STATE_INDX);
+			get_stat_data.ConStat = ntohs(conStat);
+			pr_debug("ConStat = 0x%x\n", get_stat_data.ConStat);
+		} else {
+			get_stat_data.ConStat = 0x0f;
+		}
+
+
+		get_stat_data.nTxPkts = info->stats.tx_packets;
+		get_stat_data.nRxPkts = info->stats.rx_packets;
+		get_stat_data.nTxBytes = info->stats.tx_bytes;
+		get_stat_data.nRxBytes = info->stats.rx_bytes;
+		do_gettimeofday(&tv);
+		get_stat_data.ConTm = (u32)(tv.tv_sec - info->ConTm);
+		pr_debug("Connection Time = %d\n", (int)get_stat_data.ConTm);
+		if (copy_to_user(argp, &get_stat_data, sizeof(get_stat_data))) {
+			pr_debug("copy fault occurred\n");
+			result = -EFAULT;
+			break;
+		}
+		pr_debug("GET_DSP_STAT succeed\n");
+		break;
+	case IOCTL_SET_DPRAM_CMD:
+	{
 		struct IOCTL_DPRAM_BLK *dpram_data = NULL;
 		/* struct IOCTL_DPRAM_COMMAND dpram_command; */
-            u16 qtype;
-            u16 msgsz;
+		u16 qtype;
+		u16 msgsz;
 		struct pseudo_hdr *ppseudo_hdr;
-            u16 *pmsg;
-            u16 total_len;
-            u16 app_index;
-            u16 status;
+		u16 *pmsg;
+		u16 total_len;
+		u16 app_index;
+		u16 status;
 
-            /* DEBUG("FT1000:ft1000_ioctl: IOCTL_FT1000_SET_DPRAM called\n");*/
+		/* pr_debug("IOCTL_FT1000_SET_DPRAM called\n");*/
 
 
-            if (ft1000_flarion_cnt == 0) {
-                return (-EBADF);
-            }
+		if (ft1000_flarion_cnt == 0)
+			return -EBADF;
 
-            if (ft1000dev->DrvMsgPend) {
-                return (-ENOTTY);
-            }
+		if (ft1000dev->DrvMsgPend)
+			return -ENOTTY;
 
-            if (ft1000dev->fProvComplete == 0) {
-                return (-EACCES);
-            }
+		if (ft1000dev->fProvComplete == 0)
+			return -EACCES;
 
-            ft1000dev->fAppMsgPend = 1;
+		ft1000dev->fAppMsgPend = 1;
 
-            if (info->CardReady) {
+		if (info->CardReady) {
 
-               /* DEBUG("FT1000:ft1000_ioctl: try to SET_DPRAM \n"); */
+			/* pr_debug("try to SET_DPRAM\n"); */
 
-                /* Get the length field to see how many bytes to copy */
-                result = get_user(msgsz, (__u16 __user *)argp);
-		if (result)
-			break;
-                msgsz = ntohs(msgsz);
-                /* DEBUG("FT1000:ft1000_ioctl: length of message = %d\n", msgsz); */
-
-                if (msgsz > MAX_CMD_SQSIZE) {
-                    DEBUG("FT1000:ft1000_ioctl: bad message length = %d\n", msgsz);
-                    result = -EINVAL;
-                    break;
-                }
-
-		result = -ENOMEM;
-		dpram_data = kmalloc(msgsz + 2, GFP_KERNEL);
-		if (!dpram_data)
-			break;
+			/* Get the length field to see how many bytes to copy */
+			result = get_user(msgsz, (__u16 __user *)argp);
+			if (result)
+				break;
+			msgsz = ntohs(msgsz);
+			/* pr_debug("length of message = %d\n", msgsz); */
+
+			if (msgsz > MAX_CMD_SQSIZE) {
+				pr_debug("bad message length = %d\n", msgsz);
+				result = -EINVAL;
+				break;
+			}
 
-                if (copy_from_user(dpram_data, argp, msgsz+2)) {
-                    DEBUG("FT1000:ft1000_ChIoctl: copy fault occurred\n");
-                    result = -EFAULT;
-                } else {
-                    /* Check if this message came from a registered application */
-                    for (i=0; i<MAX_NUM_APP; i++) {
-                        if (ft1000dev->app_info[i].fileobject == &file->f_owner) {
-                            break;
-                        }
-                    }
-                    if (i==MAX_NUM_APP) {
-                        DEBUG("FT1000:No matching application fileobject\n");
-                        result = -EINVAL;
-			kfree(dpram_data);
-                        break;
-                    }
-                    app_index = i;
-
-                    /* Check message qtype type which is the lower byte within qos_class */
-                    qtype = ntohs(dpram_data->pseudohdr.qos_class) & 0xff;
-                    /* DEBUG("FT1000_ft1000_ioctl: qtype = %d\n", qtype); */
-                    if (qtype) {
-                    } else {
-                        /* Put message into Slow Queue */
-                        /* Only put a message into the DPRAM if msg doorbell is available */
-                        status = ft1000_read_register(ft1000dev, &tempword, FT1000_REG_DOORBELL);
-                        /* DEBUG("FT1000_ft1000_ioctl: READ REGISTER tempword=%x\n", tempword); */
-                        if (tempword & FT1000_DB_DPRAM_TX) {
-                            /* Suspend for 2ms and try again due to DSP doorbell busy */
-                            mdelay(2);
-                            status = ft1000_read_register(ft1000dev, &tempword, FT1000_REG_DOORBELL);
-                            if (tempword & FT1000_DB_DPRAM_TX) {
-                                /* Suspend for 1ms and try again due to DSP doorbell busy */
-                                mdelay(1);
-                                status = ft1000_read_register(ft1000dev, &tempword, FT1000_REG_DOORBELL);
-                                if (tempword & FT1000_DB_DPRAM_TX) {
-                                    status = ft1000_read_register(ft1000dev, &tempword, FT1000_REG_DOORBELL);
-                                    if (tempword & FT1000_DB_DPRAM_TX) {
-                                        /* Suspend for 3ms and try again due to DSP doorbell busy */
-                                        mdelay(3);
-                                        status = ft1000_read_register(ft1000dev, &tempword, FT1000_REG_DOORBELL);
-                                        if (tempword & FT1000_DB_DPRAM_TX) {
-                                            DEBUG("FT1000:ft1000_ioctl:Doorbell not available\n");
-                                            result = -ENOTTY;
-						kfree(dpram_data);
-                                            break;
-                                        }
-                                    }
-                                }
-                            }
-                        }
-
-                        /*DEBUG("FT1000_ft1000_ioctl: finished reading register\n"); */
-
-                        /* Make sure we are within the limits of the slow queue memory limitation */
-                        if ((msgsz < MAX_CMD_SQSIZE) && (msgsz > PSEUDOSZ)) {
-                            /* Need to put sequence number plus new checksum for message */
-                            pmsg = (u16 *)&dpram_data->pseudohdr;
-				ppseudo_hdr = (struct pseudo_hdr *)pmsg;
-                            total_len = msgsz+2;
-                            if (total_len & 0x1) {
-                                total_len++;
-                            }
-
-                            /* Insert slow queue sequence number */
-                            ppseudo_hdr->seq_num = info->squeseqnum++;
-                            ppseudo_hdr->portsrc = ft1000dev->app_info[app_index].app_id;
-                            /* Calculate new checksum */
-                            ppseudo_hdr->checksum = *pmsg++;
-                            /* DEBUG("checksum = 0x%x\n", ppseudo_hdr->checksum); */
-                            for (i=1; i<7; i++) {
-                                ppseudo_hdr->checksum ^= *pmsg++;
-                                /* DEBUG("checksum = 0x%x\n", ppseudo_hdr->checksum); */
-                            }
-                            pmsg++;
-				ppseudo_hdr = (struct pseudo_hdr *)pmsg;
-                           result = card_send_command(ft1000dev,(unsigned short*)dpram_data,total_len+2);
-
-
-                            ft1000dev->app_info[app_index].nTxMsg++;
-                        } else {
-                            result = -EINVAL;
-                        }
-                    }
-                }
-            } else {
-                DEBUG("FT1000:ft1000_ioctl: Card not ready take messages\n");
-                result = -EACCES;
-            }
-	    kfree(dpram_data);
-
-        }
-        break;
-    case IOCTL_GET_DPRAM_CMD:
-        {
+			result = -ENOMEM;
+			dpram_data = kmalloc(msgsz + 2, GFP_KERNEL);
+			if (!dpram_data)
+				break;
+
+			if (copy_from_user(dpram_data, argp, msgsz+2)) {
+				pr_debug("copy fault occurred\n");
+				result = -EFAULT;
+			} else {
+				/* Check if this message came from a registered application */
+				for (i = 0; i < MAX_NUM_APP; i++) {
+					if (ft1000dev->app_info[i].fileobject == &file->f_owner) {
+						break;
+					}
+				}
+				if (i == MAX_NUM_APP) {
+					pr_debug("No matching application fileobject\n");
+					result = -EINVAL;
+					kfree(dpram_data);
+					break;
+				}
+				app_index = i;
+
+				/* Check message qtype type which is the lower byte within qos_class */
+				qtype = ntohs(dpram_data->pseudohdr.qos_class) & 0xff;
+				/* pr_debug("qtype = %d\n", qtype); */
+				if (qtype) {
+				} else {
+					/* Put message into Slow Queue */
+					/* Only put a message into the DPRAM if msg doorbell is available */
+					status = ft1000_read_register(ft1000dev, &tempword, FT1000_REG_DOORBELL);
+					/* pr_debug("READ REGISTER tempword=%x\n", tempword); */
+					if (tempword & FT1000_DB_DPRAM_TX) {
+						/* Suspend for 2ms and try again due to DSP doorbell busy */
+						mdelay(2);
+						status = ft1000_read_register(ft1000dev, &tempword, FT1000_REG_DOORBELL);
+						if (tempword & FT1000_DB_DPRAM_TX) {
+							/* Suspend for 1ms and try again due to DSP doorbell busy */
+							mdelay(1);
+							status = ft1000_read_register(ft1000dev, &tempword, FT1000_REG_DOORBELL);
+							if (tempword & FT1000_DB_DPRAM_TX) {
+								status = ft1000_read_register(ft1000dev, &tempword, FT1000_REG_DOORBELL);
+								if (tempword & FT1000_DB_DPRAM_TX) {
+									/* Suspend for 3ms and try again due to DSP doorbell busy */
+									mdelay(3);
+									status = ft1000_read_register(ft1000dev, &tempword, FT1000_REG_DOORBELL);
+									if (tempword & FT1000_DB_DPRAM_TX) {
+										pr_debug("Doorbell not available\n");
+										result = -ENOTTY;
+										kfree(dpram_data);
+										break;
+									}
+								}
+							}
+						}
+					}
+
+					/*pr_debug("finished reading register\n"); */
+
+					/* Make sure we are within the limits of the slow queue memory limitation */
+					if ((msgsz < MAX_CMD_SQSIZE) && (msgsz > PSEUDOSZ)) {
+						/* Need to put sequence number plus new checksum for message */
+						pmsg = (u16 *)&dpram_data->pseudohdr;
+						ppseudo_hdr = (struct pseudo_hdr *)pmsg;
+						total_len = msgsz+2;
+						if (total_len & 0x1) {
+							total_len++;
+						}
+
+						/* Insert slow queue sequence number */
+						ppseudo_hdr->seq_num = info->squeseqnum++;
+						ppseudo_hdr->portsrc = ft1000dev->app_info[app_index].app_id;
+						/* Calculate new checksum */
+						ppseudo_hdr->checksum = *pmsg++;
+						/* pr_debug("checksum = 0x%x\n", ppseudo_hdr->checksum); */
+						for (i = 1; i < 7; i++) {
+							ppseudo_hdr->checksum ^= *pmsg++;
+							/* pr_debug("checksum = 0x%x\n", ppseudo_hdr->checksum); */
+						}
+						pmsg++;
+						ppseudo_hdr = (struct pseudo_hdr *)pmsg;
+						result = card_send_command(ft1000dev, (unsigned short *)dpram_data, total_len+2);
+
+
+						ft1000dev->app_info[app_index].nTxMsg++;
+					} else {
+						result = -EINVAL;
+					}
+				}
+			}
+		} else {
+			pr_debug("Card not ready take messages\n");
+			result = -EACCES;
+		}
+		kfree(dpram_data);
+
+	}
+	break;
+	case IOCTL_GET_DPRAM_CMD:
+	{
 		struct dpram_blk *pdpram_blk;
 		struct IOCTL_DPRAM_BLK __user *pioctl_dpram;
-            int msglen;
-
-            /* DEBUG("FT1000:ft1000_ioctl: IOCTL_FT1000_GET_DPRAM called\n"); */
-
-            if (ft1000_flarion_cnt == 0) {
-                return (-EBADF);
-            }
-
-            /* Search for matching file object */
-            for (i=0; i<MAX_NUM_APP; i++) {
-                if (ft1000dev->app_info[i].fileobject == &file->f_owner) {
-                    /*DEBUG("FT1000:ft1000_ioctl: Message is for AppId = %d\n", ft1000dev->app_info[i].app_id); */
-                    break;
-                }
-            }
-
-            /* Could not find application info block */
-            if (i == MAX_NUM_APP) {
-                DEBUG("FT1000:ft1000_ioctl:Could not find application info block\n");
-                result = -EBADF;
-                break;
-            }
-
-            result = 0;
-            pioctl_dpram = argp;
-            if (list_empty(&ft1000dev->app_info[i].app_sqlist) == 0) {
-                /* DEBUG("FT1000:ft1000_ioctl:Message detected in slow queue\n"); */
-                spin_lock_irqsave(&free_buff_lock, flags);
-                pdpram_blk = list_entry(ft1000dev->app_info[i].app_sqlist.next, struct dpram_blk, list);
-                list_del(&pdpram_blk->list);
-                ft1000dev->app_info[i].NumOfMsg--;
-                /* DEBUG("FT1000:ft1000_ioctl:NumOfMsg for app %d = %d\n", i, ft1000dev->app_info[i].NumOfMsg); */
-                spin_unlock_irqrestore(&free_buff_lock, flags);
-                msglen = ntohs(*(u16 *)pdpram_blk->pbuffer) + PSEUDOSZ;
-                result = get_user(msglen, &pioctl_dpram->total_len);
-		if (result)
-			break;
-		msglen = htons(msglen);
-                /* DEBUG("FT1000:ft1000_ioctl:msg length = %x\n", msglen); */
-                if (copy_to_user (&pioctl_dpram->pseudohdr, pdpram_blk->pbuffer, msglen)) {
-					DEBUG("FT1000:ft1000_ioctl: copy fault occurred\n");
-			result = -EFAULT;
+		int msglen;
+
+		/* pr_debug("IOCTL_FT1000_GET_DPRAM called\n"); */
+
+		if (ft1000_flarion_cnt == 0)
+			return -EBADF;
+
+		/* Search for matching file object */
+		for (i = 0; i < MAX_NUM_APP; i++) {
+			if (ft1000dev->app_info[i].fileobject == &file->f_owner) {
+				/*pr_debug("Message is for AppId = %d\n", ft1000dev->app_info[i].app_id); */
+				break;
+			}
+		}
+
+		/* Could not find application info block */
+		if (i == MAX_NUM_APP) {
+			pr_debug("Could not find application info block\n");
+			result = -EBADF;
 			break;
-				}
+		}
+
+		result = 0;
+		pioctl_dpram = argp;
+		if (list_empty(&ft1000dev->app_info[i].app_sqlist) == 0) {
+			/* pr_debug("Message detected in slow queue\n"); */
+			spin_lock_irqsave(&free_buff_lock, flags);
+			pdpram_blk = list_entry(ft1000dev->app_info[i].app_sqlist.next, struct dpram_blk, list);
+			list_del(&pdpram_blk->list);
+			ft1000dev->app_info[i].NumOfMsg--;
+			/* pr_debug("NumOfMsg for app %d = %d\n", i, ft1000dev->app_info[i].NumOfMsg); */
+			spin_unlock_irqrestore(&free_buff_lock, flags);
+			msglen = ntohs(*(u16 *)pdpram_blk->pbuffer) + PSEUDOSZ;
+			result = get_user(msglen, &pioctl_dpram->total_len);
+			if (result)
+				break;
+			msglen = htons(msglen);
+			/* pr_debug("msg length = %x\n", msglen); */
+			if (copy_to_user(&pioctl_dpram->pseudohdr, pdpram_blk->pbuffer, msglen)) {
+				pr_debug("copy fault occurred\n");
+				result = -EFAULT;
+				break;
+			}
+
+			ft1000_free_buffer(pdpram_blk, &freercvpool);
+			result = msglen;
+		}
+		/* pr_debug("IOCTL_FT1000_GET_DPRAM no message\n"); */
+	}
+	break;
 
-                ft1000_free_buffer(pdpram_blk, &freercvpool);
-                result = msglen;
-            }
-            /* DEBUG("FT1000:ft1000_ioctl: IOCTL_FT1000_GET_DPRAM no message\n"); */
-        }
-        break;
-
-    default:
-        DEBUG("FT1000:ft1000_ioctl:unknown command: 0x%x\n", command);
-        result = -ENOTTY;
-        break;
-    }
-    ft1000dev->fAppMsgPend = 0;
-    return result;
+	default:
+		pr_debug("unknown command: 0x%x\n", command);
+		result = -ENOTTY;
+		break;
+	}
+	ft1000dev->fAppMsgPend = 0;
+	return result;
 }
 
 /*
-*---------------------------------------------------------------------------
-* Function:    ft1000_release
-*
-* Parameters:
-*
-* Description:
-*
-* Notes:
-*
-*---------------------------------------------------------------------------
-*/
+ *---------------------------------------------------------------------------
+ * Function:    ft1000_release
+ *
+ * Parameters:
+ *
+ * Description:
+ *
+ * Notes:
+ *
+ *---------------------------------------------------------------------------
+ */
 static int ft1000_release(struct inode *inode, struct file *file)
 {
 	struct ft1000_info *info;
-    struct net_device *dev;
-    struct ft1000_usb *ft1000dev;
-    int i;
+	struct net_device *dev;
+	struct ft1000_usb *ft1000dev;
+	int i;
 	struct dpram_blk *pdpram_blk;
 
-    DEBUG("ft1000_release called\n");
-
-    dev = file->private_data;
+	dev = file->private_data;
 	info = netdev_priv(dev);
 	ft1000dev = info->priv;
 
-    if (ft1000_flarion_cnt == 0) {
-        ft1000dev->appcnt--;
-        return (-EBADF);
-    }
-
-    /* Search for matching file object */
-    for (i=0; i<MAX_NUM_APP; i++) {
-        if (ft1000dev->app_info[i].fileobject == &file->f_owner) {
-            /* DEBUG("FT1000:ft1000_ioctl: Message is for AppId = %d\n", ft1000dev->app_info[i].app_id); */
-            break;
-        }
-    }
-
-    if (i==MAX_NUM_APP)
-	    return 0;
-
-    while (list_empty(&ft1000dev->app_info[i].app_sqlist) == 0) {
-        DEBUG("Remove and free memory queue up on slow queue\n");
-        pdpram_blk = list_entry(ft1000dev->app_info[i].app_sqlist.next, struct dpram_blk, list);
-        list_del(&pdpram_blk->list);
-        ft1000_free_buffer(pdpram_blk, &freercvpool);
-    }
-
-    /* initialize application information */
-    ft1000dev->appcnt--;
-    DEBUG("ft1000_chdev:%s:appcnt = %d\n", __func__, ft1000dev->appcnt);
-    ft1000dev->app_info[i].fileobject = NULL;
-
-    return 0;
+	if (ft1000_flarion_cnt == 0) {
+		ft1000dev->appcnt--;
+		return -EBADF;
+	}
+
+	/* Search for matching file object */
+	for (i = 0; i < MAX_NUM_APP; i++) {
+		if (ft1000dev->app_info[i].fileobject == &file->f_owner) {
+			/* pr_debug("Message is for AppId = %d\n", ft1000dev->app_info[i].app_id); */
+			break;
+		}
+	}
+
+	if (i == MAX_NUM_APP)
+		return 0;
+
+	while (list_empty(&ft1000dev->app_info[i].app_sqlist) == 0) {
+		pr_debug("Remove and free memory queue up on slow queue\n");
+		pdpram_blk = list_entry(ft1000dev->app_info[i].app_sqlist.next, struct dpram_blk, list);
+		list_del(&pdpram_blk->list);
+		ft1000_free_buffer(pdpram_blk, &freercvpool);
+	}
+
+	/* initialize application information */
+	ft1000dev->appcnt--;
+	pr_debug("appcnt = %d\n", ft1000dev->appcnt);
+	ft1000dev->app_info[i].fileobject = NULL;
+
+	return 0;
 }
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_download.c b/drivers/staging/ft1000/ft1000-usb/ft1000_download.c
index 37707da09e9c..e8126325877b 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_download.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_download.c
@@ -1,8 +1,10 @@
 /*
-* CopyRight (C) 2007 Qualcomm Inc. All Rights Reserved.
-*
-* This file is part of Express Card USB Driver
-*/
+ * CopyRight (C) 2007 Qualcomm Inc. All Rights Reserved.
+ *
+ * This file is part of Express Card USB Driver
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/kernel.h>
 #include <linux/module.h>
@@ -117,17 +119,16 @@ static int check_usb_db(struct ft1000_usb *ft1000dev)
 
 	while (loopcnt < 10) {
 		status = ft1000_read_register(ft1000dev, &temp,
-					       FT1000_REG_DOORBELL);
-		DEBUG("check_usb_db: read FT1000_REG_DOORBELL value is %x\n",
-		       temp);
+					      FT1000_REG_DOORBELL);
+		pr_debug("read FT1000_REG_DOORBELL value is %x\n", temp);
 		if (temp & 0x0080) {
-			DEBUG("FT1000:Got checkusb doorbell\n");
+			pr_debug("Got checkusb doorbell\n");
 			status = ft1000_write_register(ft1000dev, 0x0080,
-						FT1000_REG_DOORBELL);
+						       FT1000_REG_DOORBELL);
 			status = ft1000_write_register(ft1000dev, 0x0100,
-						FT1000_REG_DOORBELL);
+						       FT1000_REG_DOORBELL);
 			status = ft1000_write_register(ft1000dev,  0x8000,
-						FT1000_REG_DOORBELL);
+						       FT1000_REG_DOORBELL);
 			break;
 		}
 		loopcnt++;
@@ -138,13 +139,13 @@ static int check_usb_db(struct ft1000_usb *ft1000dev)
 	loopcnt = 0;
 	while (loopcnt < 20) {
 		status = ft1000_read_register(ft1000dev, &temp,
-					       FT1000_REG_DOORBELL);
-		DEBUG("FT1000:check_usb_db:Doorbell = 0x%x\n", temp);
+					      FT1000_REG_DOORBELL);
+		pr_debug("Doorbell = 0x%x\n", temp);
 		if (temp & 0x8000) {
 			loopcnt++;
 			msleep(10);
 		} else	{
-			DEBUG("check_usb_db: door bell is cleared, return 0\n");
+			pr_debug("door bell is cleared, return 0\n");
 			return 0;
 		}
 	}
@@ -164,23 +165,22 @@ static u16 get_handshake(struct ft1000_usb *ft1000dev, u16 expected_value)
 	while (loopcnt < 100) {
 		/* Need to clear downloader doorbell if Hartley ASIC */
 		status = ft1000_write_register(ft1000dev,  FT1000_DB_DNLD_RX,
-						FT1000_REG_DOORBELL);
+					       FT1000_REG_DOORBELL);
 		if (ft1000dev->fcodeldr) {
-			DEBUG(" get_handshake: fcodeldr is %d\n",
-				ft1000dev->fcodeldr);
+			pr_debug("fcodeldr is %d\n", ft1000dev->fcodeldr);
 			ft1000dev->fcodeldr = 0;
 			status = check_usb_db(ft1000dev);
 			if (status != 0) {
-				DEBUG("get_handshake: check_usb_db failed\n");
+				pr_debug("check_usb_db failed\n");
 				break;
 			}
 			status = ft1000_write_register(ft1000dev,
-					FT1000_DB_DNLD_RX,
-					FT1000_REG_DOORBELL);
+						       FT1000_DB_DNLD_RX,
+						       FT1000_REG_DOORBELL);
 		}
 
 		status = ft1000_read_dpram16(ft1000dev,
-				DWNLD_MAG1_HANDSHAKE_LOC, (u8 *)&handshake, 1);
+					     DWNLD_MAG1_HANDSHAKE_LOC, (u8 *)&handshake, 1);
 		handshake = ntohs(handshake);
 
 		if (status)
@@ -209,12 +209,12 @@ static void put_handshake(struct ft1000_usb *ft1000dev, u16 handshake_value)
 
 	tempword = (u16)(tempx & 0xffff);
 	status = ft1000_write_dpram16(ft1000dev, DWNLD_MAG1_HANDSHAKE_LOC,
-					tempword, 0);
+				      tempword, 0);
 	tempword = (u16)(tempx >> 16);
 	status = ft1000_write_dpram16(ft1000dev, DWNLD_MAG1_HANDSHAKE_LOC,
-					tempword, 1);
+				      tempword, 1);
 	status = ft1000_write_register(ft1000dev, FT1000_DB_DNLD_TX,
-					FT1000_REG_DOORBELL);
+				       FT1000_REG_DOORBELL);
 }
 
 static u16 get_handshake_usb(struct ft1000_usb *ft1000dev, u16 expected_value)
@@ -230,27 +230,27 @@ static u16 get_handshake_usb(struct ft1000_usb *ft1000dev, u16 expected_value)
 	while (loopcnt < 100) {
 		if (ft1000dev->usbboot == 2) {
 			status = ft1000_read_dpram32(ft1000dev, 0,
-					(u8 *)&(ft1000dev->tempbuf[0]), 64);
+						     (u8 *)&(ft1000dev->tempbuf[0]), 64);
 			for (temp = 0; temp < 16; temp++) {
-				DEBUG("tempbuf %d = 0x%x\n", temp,
-					ft1000dev->tempbuf[temp]);
+				pr_debug("tempbuf %d = 0x%x\n",
+					 temp, ft1000dev->tempbuf[temp]);
 			}
 			status = ft1000_read_dpram16(ft1000dev,
-						DWNLD_MAG1_HANDSHAKE_LOC,
-						(u8 *)&handshake, 1);
-			DEBUG("handshake from read_dpram16 = 0x%x\n",
-				handshake);
+						     DWNLD_MAG1_HANDSHAKE_LOC,
+						     (u8 *)&handshake, 1);
+			pr_debug("handshake from read_dpram16 = 0x%x\n",
+				 handshake);
 			if (ft1000dev->dspalive == ft1000dev->tempbuf[6]) {
 				handshake = 0;
 			} else {
 				handshake = ft1000dev->tempbuf[1];
 				ft1000dev->dspalive =
-						ft1000dev->tempbuf[6];
+					ft1000dev->tempbuf[6];
 			}
 		} else {
 			status = ft1000_read_dpram16(ft1000dev,
-						DWNLD_MAG1_HANDSHAKE_LOC,
-						(u8 *)&handshake, 1);
+						     DWNLD_MAG1_HANDSHAKE_LOC,
+						     (u8 *)&handshake, 1);
 		}
 
 		loopcnt++;
@@ -281,12 +281,12 @@ static u16 get_request_type(struct ft1000_usb *ft1000dev)
 
 	if (ft1000dev->bootmode == 1) {
 		status = fix_ft1000_read_dpram32(ft1000dev,
-				DWNLD_MAG1_TYPE_LOC, (u8 *)&tempx);
+						 DWNLD_MAG1_TYPE_LOC, (u8 *)&tempx);
 		tempx = ntohl(tempx);
 	} else {
 		tempx = 0;
 		status = ft1000_read_dpram16(ft1000dev,
-				DWNLD_MAG1_TYPE_LOC, (u8 *)&tempword, 1);
+					     DWNLD_MAG1_TYPE_LOC, (u8 *)&tempword, 1);
 		tempx |= (tempword << 16);
 		tempx = ntohl(tempx);
 	}
@@ -304,7 +304,7 @@ static u16 get_request_type_usb(struct ft1000_usb *ft1000dev)
 
 	if (ft1000dev->bootmode == 1) {
 		status = fix_ft1000_read_dpram32(ft1000dev,
-				DWNLD_MAG1_TYPE_LOC, (u8 *)&tempx);
+						 DWNLD_MAG1_TYPE_LOC, (u8 *)&tempx);
 		tempx = ntohl(tempx);
 	} else {
 		if (ft1000dev->usbboot == 2) {
@@ -313,8 +313,8 @@ static u16 get_request_type_usb(struct ft1000_usb *ft1000dev)
 		} else {
 			tempx = 0;
 			status = ft1000_read_dpram16(ft1000dev,
-					DWNLD_MAG1_TYPE_LOC,
-					(u8 *)&tempword, 1);
+						     DWNLD_MAG1_TYPE_LOC,
+						     (u8 *)&tempword, 1);
 		}
 		tempx |= (tempword << 16);
 		tempx = ntohl(tempx);
@@ -332,14 +332,14 @@ static long get_request_value(struct ft1000_usb *ft1000dev)
 
 	if (ft1000dev->bootmode == 1) {
 		status = fix_ft1000_read_dpram32(ft1000dev,
-				DWNLD_MAG1_SIZE_LOC, (u8 *)&value);
+						 DWNLD_MAG1_SIZE_LOC, (u8 *)&value);
 		value = ntohl(value);
 	} else	{
 		status = ft1000_read_dpram16(ft1000dev,
-				DWNLD_MAG1_SIZE_LOC, (u8 *)&tempword, 0);
+					     DWNLD_MAG1_SIZE_LOC, (u8 *)&tempword, 0);
 		value = tempword;
 		status = ft1000_read_dpram16(ft1000dev,
-				DWNLD_MAG1_SIZE_LOC, (u8 *)&tempword, 1);
+					     DWNLD_MAG1_SIZE_LOC, (u8 *)&tempword, 1);
 		value |= (tempword << 16);
 		value = ntohl(value);
 	}
@@ -369,7 +369,7 @@ static u16 hdr_checksum(struct pseudo_hdr *pHdr)
 
 
 	chksum = ((((((usPtr[0] ^ usPtr[1]) ^ usPtr[2]) ^ usPtr[3]) ^
-	usPtr[4]) ^ usPtr[5]) ^ usPtr[6]);
+		    usPtr[4]) ^ usPtr[5]) ^ usPtr[6]);
 
 	return chksum;
 }
@@ -387,7 +387,7 @@ static int check_buffers(u16 *buff_w, u16 *buff_r, int len, int offset)
 }
 
 static int write_dpram32_and_check(struct ft1000_usb *ft1000dev,
-		u16 tempbuffer[], u16 dpram)
+				   u16 tempbuffer[], u16 dpram)
 {
 	int status;
 	u16 resultbuffer[64];
@@ -395,38 +395,38 @@ static int write_dpram32_and_check(struct ft1000_usb *ft1000dev,
 
 	for (i = 0; i < 10; i++) {
 		status = ft1000_write_dpram32(ft1000dev, dpram,
-				(u8 *)&tempbuffer[0], 64);
+					      (u8 *)&tempbuffer[0], 64);
 		if (status == 0) {
 			/* Work around for ASIC bit stuffing problem. */
 			if ((tempbuffer[31] & 0xfe00) == 0xfe00) {
 				status = ft1000_write_dpram32(ft1000dev,
-						dpram+12, (u8 *)&tempbuffer[24],
-						64);
+							      dpram+12, (u8 *)&tempbuffer[24],
+							      64);
 			}
 			/* Let's check the data written */
 			status = ft1000_read_dpram32(ft1000dev, dpram,
-					(u8 *)&resultbuffer[0], 64);
+						     (u8 *)&resultbuffer[0], 64);
 			if ((tempbuffer[31] & 0xfe00) == 0xfe00) {
 				if (check_buffers(tempbuffer, resultbuffer, 28,
-							0)) {
-					DEBUG("FT1000:download:DPRAM write failed 1 during bootloading\n");
+						  0)) {
+					pr_debug("DPRAM write failed 1 during bootloading\n");
 					usleep_range(9000, 11000);
 					break;
 				}
 				status = ft1000_read_dpram32(ft1000dev,
-						dpram+12,
-						(u8 *)&resultbuffer[0], 64);
+							     dpram+12,
+							     (u8 *)&resultbuffer[0], 64);
 
 				if (check_buffers(tempbuffer, resultbuffer, 16,
-							24)) {
-					DEBUG("FT1000:download:DPRAM write failed 2 during bootloading\n");
+						  24)) {
+					pr_debug("DPRAM write failed 2 during bootloading\n");
 					usleep_range(9000, 11000);
 					break;
 				}
 			} else {
 				if (check_buffers(tempbuffer, resultbuffer, 32,
-							0)) {
-					DEBUG("FT1000:download:DPRAM write failed 3 during bootloading\n");
+						  0)) {
+					pr_debug("DPRAM write failed 3 during bootloading\n");
 					usleep_range(9000, 11000);
 					break;
 				}
@@ -445,7 +445,7 @@ static int write_dpram32_and_check(struct ft1000_usb *ft1000dev,
  *              long word_length - length of the buffer to be written to DPRAM
  */
 static int write_blk(struct ft1000_usb *ft1000dev, u16 **pUsFile, u8 **pUcFile,
-		long word_length)
+		     long word_length)
 {
 	int status = 0;
 	u16 dpram;
@@ -453,7 +453,7 @@ static int write_blk(struct ft1000_usb *ft1000dev, u16 **pUsFile, u8 **pUcFile,
 	u16 tempword;
 	u16 tempbuffer[64];
 
-	/*DEBUG("FT1000:download:start word_length = %d\n",(int)word_length); */
+	/*pr_debug("start word_length = %d\n",(int)word_length); */
 	dpram = (u16)DWNLD_MAG1_PS_HDR_LOC;
 	tempword = *(*pUsFile);
 	(*pUsFile)++;
@@ -483,21 +483,22 @@ static int write_blk(struct ft1000_usb *ft1000dev, u16 **pUsFile, u8 **pUcFile,
 			}
 		}
 
-		/*DEBUG("write_blk: loopcnt is %d\n", loopcnt); */
-		/*DEBUG("write_blk: bootmode = %d\n", bootmode); */
-		/*DEBUG("write_blk: dpram = %x\n", dpram); */
+		/*pr_debug("loopcnt is %d\n", loopcnt); */
+		/*pr_debug("write_blk: bootmode = %d\n", bootmode); */
+		/*pr_debug("write_blk: dpram = %x\n", dpram); */
 		if (ft1000dev->bootmode == 0) {
 			if (dpram >= 0x3F4)
 				status = ft1000_write_dpram32(ft1000dev, dpram,
-						(u8 *)&tempbuffer[0], 8);
+							      (u8 *)&tempbuffer[0], 8);
 			else
 				status = ft1000_write_dpram32(ft1000dev, dpram,
-						(u8 *)&tempbuffer[0], 64);
+							      (u8 *)&tempbuffer[0], 64);
 		} else {
 			status = write_dpram32_and_check(ft1000dev, tempbuffer,
-					dpram);
+							 dpram);
 			if (status != 0) {
-				DEBUG("FT1000:download:Write failed tempbuffer[31] = 0x%x\n", tempbuffer[31]);
+				pr_debug("Write failed tempbuffer[31] = 0x%x\n",
+					 tempbuffer[31]);
 				break;
 			}
 		}
@@ -508,7 +509,7 @@ static int write_blk(struct ft1000_usb *ft1000dev, u16 **pUsFile, u8 **pUcFile,
 
 static void usb_dnld_complete(struct urb *urb)
 {
-	/* DEBUG("****** usb_dnld_complete\n"); */
+	/* pr_debug("****** usb_dnld_complete\n"); */
 }
 
 /* writes a block of DSP image to DPRAM
@@ -548,22 +549,21 @@ static int write_blk_fifo(struct ft1000_usb *ft1000dev, u16 **pUsFile,
 }
 
 static int scram_start_dwnld(struct ft1000_usb *ft1000dev, u16 *hshake,
-		u32 *state)
+			     u32 *state)
 {
 	int status = 0;
 
-	DEBUG("FT1000:STATE_START_DWNLD\n");
 	if (ft1000dev->usbboot)
 		*hshake = get_handshake_usb(ft1000dev, HANDSHAKE_DSP_BL_READY);
 	else
 		*hshake = get_handshake(ft1000dev, HANDSHAKE_DSP_BL_READY);
 	if (*hshake == HANDSHAKE_DSP_BL_READY) {
-		DEBUG("scram_dnldr: handshake is HANDSHAKE_DSP_BL_READY, call put_handshake(HANDSHAKE_DRIVER_READY)\n");
+		pr_debug("handshake is HANDSHAKE_DSP_BL_READY, call put_handshake(HANDSHAKE_DRIVER_READY)\n");
 		put_handshake(ft1000dev, HANDSHAKE_DRIVER_READY);
 	} else if (*hshake == HANDSHAKE_TIMEOUT_VALUE) {
 		status = -ETIMEDOUT;
 	} else {
-		DEBUG("FT1000:download:Download error: Handshake failed\n");
+		pr_debug("Download error: Handshake failed\n");
 		status = -ENETRESET;
 	}
 	*state = STATE_BOOT_DWNLD;
@@ -571,22 +571,22 @@ static int scram_start_dwnld(struct ft1000_usb *ft1000dev, u16 *hshake,
 }
 
 static int request_code_segment(struct ft1000_usb *ft1000dev, u16 **s_file,
-		 u8 **c_file, const u8 *endpoint, bool boot_case)
+				u8 **c_file, const u8 *endpoint, bool boot_case)
 {
 	long word_length;
 	int status = 0;
 
-	/*DEBUG("FT1000:REQUEST_CODE_SEGMENT\n");i*/
 	word_length = get_request_value(ft1000dev);
-	/*DEBUG("FT1000:word_length = 0x%x\n", (int)word_length); */
+	/*pr_debug("word_length = 0x%x\n", (int)word_length); */
 	/*NdisMSleep (100); */
 	if (word_length > MAX_LENGTH) {
-		DEBUG("FT1000:download:Download error: Max length exceeded\n");
+		pr_debug("Download error: Max length exceeded\n");
 		return -1;
 	}
 	if ((word_length * 2 + (long)c_file) > (long)endpoint) {
 		/* Error, beyond boot code range.*/
-		DEBUG("FT1000:download:Download error: Requested len=%d exceeds BOOT code boundary.\n", (int)word_length);
+		pr_debug("Download error: Requested len=%d exceeds BOOT code boundary\n",
+			 (int)word_length);
 		return -1;
 	}
 	if (word_length & 0x1)
@@ -595,14 +595,14 @@ static int request_code_segment(struct ft1000_usb *ft1000dev, u16 **s_file,
 
 	if (boot_case) {
 		status = write_blk(ft1000dev, s_file, c_file, word_length);
-		/*DEBUG("write_blk returned %d\n", status); */
+		/*pr_debug("write_blk returned %d\n", status); */
 	} else {
 		status = write_blk_fifo(ft1000dev, s_file, c_file, word_length);
 		if (ft1000dev->usbboot == 0)
 			ft1000dev->usbboot++;
 		if (ft1000dev->usbboot == 1)
 			status |= ft1000_write_dpram16(ft1000dev,
-					DWNLD_MAG1_PS_HDR_LOC, 0, 0);
+						       DWNLD_MAG1_PS_HDR_LOC, 0, 0);
 	}
 	return status;
 }
@@ -641,8 +641,6 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 	struct prov_record *pprov_record;
 	struct ft1000_info *pft1000info = netdev_priv(ft1000dev->net);
 
-	DEBUG("Entered   scram_dnldr...\n");
-
 	ft1000dev->fcodeldr = 0;
 	ft1000dev->usbboot = 0;
 	ft1000dev->dspalive = 0xffff;
@@ -653,7 +651,7 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 
 	state = STATE_START_DWNLD;
 
-	file_hdr = (struct dsp_file_hdr *)pFileStart;
+	file_hdr = pFileStart;
 
 	ft1000_write_register(ft1000dev, 0x800, FT1000_REG_MAG_WATERMARK);
 
@@ -674,7 +672,7 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 			break;
 
 		case STATE_BOOT_DWNLD:
-			DEBUG("FT1000:STATE_BOOT_DWNLD\n");
+			pr_debug("STATE_BOOT_DWNLD\n");
 			ft1000dev->bootmode = 1;
 			handshake = get_handshake(ft1000dev, HANDSHAKE_REQUEST);
 			if (handshake == HANDSHAKE_REQUEST) {
@@ -684,35 +682,34 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 				request = get_request_type(ft1000dev);
 				switch (request) {
 				case REQUEST_RUN_ADDRESS:
-					DEBUG("FT1000:REQUEST_RUN_ADDRESS\n");
+					pr_debug("REQUEST_RUN_ADDRESS\n");
 					put_request_value(ft1000dev,
 							  loader_code_address);
 					break;
 				case REQUEST_CODE_LENGTH:
-					DEBUG("FT1000:REQUEST_CODE_LENGTH\n");
+					pr_debug("REQUEST_CODE_LENGTH\n");
 					put_request_value(ft1000dev,
 							  loader_code_size);
 					break;
 				case REQUEST_DONE_BL:
-					DEBUG("FT1000:REQUEST_DONE_BL\n");
+					pr_debug("REQUEST_DONE_BL\n");
 					/* Reposition ptrs to beginning of code section */
 					s_file = (u16 *) (boot_end);
 					c_file = (u8 *) (boot_end);
-					/* DEBUG("FT1000:download:s_file = 0x%8x\n", (int)s_file); */
-					/* DEBUG("FT1000:download:c_file = 0x%8x\n", (int)c_file); */
+					/* pr_debug("download:s_file = 0x%8x\n", (int)s_file); */
+					/* pr_debug("FT1000:download:c_file = 0x%8x\n", (int)c_file); */
 					state = STATE_CODE_DWNLD;
 					ft1000dev->fcodeldr = 1;
 					break;
 				case REQUEST_CODE_SEGMENT:
 					status = request_code_segment(ft1000dev,
-							&s_file, &c_file,
-							(const u8 *)boot_end,
-							true);
-				break;
+								      &s_file, &c_file,
+								      (const u8 *)boot_end,
+								      true);
+					break;
 				default:
-					DEBUG
-					    ("FT1000:download:Download error: Bad request type=%d in BOOT download state.\n",
-					     request);
+					pr_debug("Download error: Bad request type=%d in BOOT download state\n",
+						 request);
 					status = -1;
 					break;
 				}
@@ -723,68 +720,60 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 					put_handshake(ft1000dev,
 						      HANDSHAKE_RESPONSE);
 			} else {
-				DEBUG
-				    ("FT1000:download:Download error: Handshake failed\n");
+				pr_debug("Download error: Handshake failed\n");
 				status = -1;
 			}
 
 			break;
 
 		case STATE_CODE_DWNLD:
-			/* DEBUG("FT1000:STATE_CODE_DWNLD\n"); */
+			/* pr_debug("STATE_CODE_DWNLD\n"); */
 			ft1000dev->bootmode = 0;
 			if (ft1000dev->usbboot)
 				handshake =
-				    get_handshake_usb(ft1000dev,
-						      HANDSHAKE_REQUEST);
+					get_handshake_usb(ft1000dev,
+							  HANDSHAKE_REQUEST);
 			else
 				handshake =
-				    get_handshake(ft1000dev, HANDSHAKE_REQUEST);
+					get_handshake(ft1000dev, HANDSHAKE_REQUEST);
 			if (handshake == HANDSHAKE_REQUEST) {
 				/*
 				 * Get type associated with the request.
 				 */
 				if (ft1000dev->usbboot)
 					request =
-					    get_request_type_usb(ft1000dev);
+						get_request_type_usb(ft1000dev);
 				else
 					request = get_request_type(ft1000dev);
 				switch (request) {
 				case REQUEST_FILE_CHECKSUM:
-					DEBUG
-					    ("FT1000:download:image_chksum = 0x%8x\n",
-					     image_chksum);
+					pr_debug("image_chksum = 0x%8x\n",
+						 image_chksum);
 					put_request_value(ft1000dev,
 							  image_chksum);
 					break;
 				case REQUEST_RUN_ADDRESS:
-					DEBUG
-					    ("FT1000:download:  REQUEST_RUN_ADDRESS\n");
+					pr_debug("REQUEST_RUN_ADDRESS\n");
 					if (correct_version) {
-						DEBUG
-						    ("FT1000:download:run_address = 0x%8x\n",
-						     (int)run_address);
+						pr_debug("run_address = 0x%8x\n",
+							 (int)run_address);
 						put_request_value(ft1000dev,
 								  run_address);
 					} else {
-						DEBUG
-						    ("FT1000:download:Download error: Got Run address request before image offset request.\n");
+						pr_debug("Download error: Got Run address request before image offset request\n");
 						status = -1;
 						break;
 					}
 					break;
 				case REQUEST_CODE_LENGTH:
-					DEBUG
-					    ("FT1000:download:REQUEST_CODE_LENGTH\n");
+					pr_debug("REQUEST_CODE_LENGTH\n");
 					if (correct_version) {
-						DEBUG
-						    ("FT1000:download:run_size = 0x%8x\n",
-						     (int)run_size);
+						pr_debug("run_size = 0x%8x\n",
+							 (int)run_size);
 						put_request_value(ft1000dev,
 								  run_size);
 					} else {
-						DEBUG
-						    ("FT1000:download:Download error: Got Size request before image offset request.\n");
+						pr_debug("Download error: Got Size request before image offset request\n");
 						status = -1;
 						break;
 					}
@@ -793,69 +782,66 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 					ft1000dev->usbboot = 3;
 					/* Reposition ptrs to beginning of provisioning section */
 					s_file =
-					    (u16 *) (pFileStart +
-						     file_hdr->commands_offset);
+						(u16 *) (pFileStart +
+							 file_hdr->commands_offset);
 					c_file =
-					    (u8 *) (pFileStart +
-						    file_hdr->commands_offset);
+						(u8 *) (pFileStart +
+							file_hdr->commands_offset);
 					state = STATE_DONE_DWNLD;
 					break;
 				case REQUEST_CODE_SEGMENT:
-					/* DEBUG("FT1000:download: REQUEST_CODE_SEGMENT - CODELOADER\n"); */
+					/* pr_debug("REQUEST_CODE_SEGMENT - CODELOADER\n"); */
 					if (!correct_version) {
-						DEBUG
-						    ("FT1000:download:Download error: Got Code Segment request before image offset request.\n");
+						pr_debug("Download error: Got Code Segment request before image offset request\n");
 						status = -1;
 						break;
 					}
 
 					status = request_code_segment(ft1000dev,
-							&s_file, &c_file,
-							(const u8 *)code_end,
-							false);
+								      &s_file, &c_file,
+								      (const u8 *)code_end,
+								      false);
 
 					break;
 
 				case REQUEST_MAILBOX_DATA:
-					DEBUG
-					    ("FT1000:download: REQUEST_MAILBOX_DATA\n");
+					pr_debug("REQUEST_MAILBOX_DATA\n");
 					/* Convert length from byte count to word count. Make sure we round up. */
 					word_length =
-					    (long)(pft1000info->DSPInfoBlklen +
-						   1) / 2;
+						(long)(pft1000info->DSPInfoBlklen +
+						       1) / 2;
 					put_request_value(ft1000dev,
 							  word_length);
 					mailbox_data =
-					    (struct drv_msg *)&(pft1000info->
-								DSPInfoBlk[0]);
+						(struct drv_msg *)&(pft1000info->
+								    DSPInfoBlk[0]);
 					/*
 					 * Position ASIC DPRAM auto-increment pointer.
 					 */
 
-					data = (u16 *) &mailbox_data->data[0];
-					dpram = (u16) DWNLD_MAG1_PS_HDR_LOC;
+					data = (u16 *)&mailbox_data->data[0];
+					dpram = (u16)DWNLD_MAG1_PS_HDR_LOC;
 					if (word_length & 0x1)
 						word_length++;
 
-					word_length = (word_length / 2);
+					word_length = word_length / 2;
 
 					for (; word_length > 0; word_length--) {	/* In words */
 
 						templong = *data++;
 						templong |= (*data++ << 16);
 						status =
-						    fix_ft1000_write_dpram32
-						    (ft1000dev, dpram++,
-						     (u8 *) &templong);
+							fix_ft1000_write_dpram32
+							(ft1000dev, dpram++,
+							 (u8 *)&templong);
 
 					}
 					break;
 
 				case REQUEST_VERSION_INFO:
-					DEBUG
-					    ("FT1000:download:REQUEST_VERSION_INFO\n");
+					pr_debug("REQUEST_VERSION_INFO\n");
 					word_length =
-					    file_hdr->version_data_size;
+						file_hdr->version_data_size;
 					put_request_value(ft1000dev,
 							  word_length);
 					/*
@@ -863,15 +849,15 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 					 */
 
 					s_file =
-					    (u16 *) (pFileStart +
-						     file_hdr->
-						     version_data_offset);
+						(u16 *) (pFileStart +
+							 file_hdr->
+							 version_data_offset);
 
-					dpram = (u16) DWNLD_MAG1_PS_HDR_LOC;
+					dpram = (u16)DWNLD_MAG1_PS_HDR_LOC;
 					if (word_length & 0x1)
 						word_length++;
 
-					word_length = (word_length / 2);
+					word_length = word_length / 2;
 
 					for (; word_length > 0; word_length--) {	/* In words */
 
@@ -879,26 +865,25 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 						temp = ntohs(*s_file++);
 						templong |= (temp << 16);
 						status =
-						    fix_ft1000_write_dpram32
-						    (ft1000dev, dpram++,
-						     (u8 *) &templong);
+							fix_ft1000_write_dpram32
+							(ft1000dev, dpram++,
+							 (u8 *)&templong);
 
 					}
 					break;
 
 				case REQUEST_CODE_BY_VERSION:
-					DEBUG
-					    ("FT1000:download:REQUEST_CODE_BY_VERSION\n");
+					pr_debug("REQUEST_CODE_BY_VERSION\n");
 					correct_version = false;
 					requested_version =
-					    get_request_value(ft1000dev);
+						get_request_value(ft1000dev);
 
 					dsp_img_info =
-					    (struct dsp_image_info *)(pFileStart
-								      +
-								      sizeof
-								      (struct
-								       dsp_file_hdr));
+						(struct dsp_image_info *)(pFileStart
+									  +
+									  sizeof
+									  (struct
+									   dsp_file_hdr));
 
 					for (image = 0;
 					     image < file_hdr->nDspImages;
@@ -907,30 +892,29 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 						if (dsp_img_info->version ==
 						    requested_version) {
 							correct_version = true;
-							DEBUG
-							    ("FT1000:download: correct_version is TRUE\n");
+							pr_debug("correct_version is TRUE\n");
 							s_file =
-							    (u16 *) (pFileStart
-								     +
-								     dsp_img_info->
-								     begin_offset);
+								(u16 *) (pFileStart
+									 +
+									 dsp_img_info->
+									 begin_offset);
 							c_file =
-							    (u8 *) (pFileStart +
-								    dsp_img_info->
-								    begin_offset);
+								(u8 *) (pFileStart +
+									dsp_img_info->
+									begin_offset);
 							code_end =
-							    (u8 *) (pFileStart +
-								    dsp_img_info->
-								    end_offset);
+								(u8 *) (pFileStart +
+									dsp_img_info->
+									end_offset);
 							run_address =
-							    dsp_img_info->
-							    run_address;
+								dsp_img_info->
+								run_address;
 							run_size =
-							    dsp_img_info->
-							    image_size;
+								dsp_img_info->
+								image_size;
 							image_chksum =
-							    (u32) dsp_img_info->
-							    checksum;
+								(u32)dsp_img_info->
+								checksum;
 							break;
 						}
 						dsp_img_info++;
@@ -941,18 +925,16 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 						/*
 						 * Error, beyond boot code range.
 						 */
-						DEBUG
-						    ("FT1000:download:Download error: Bad Version Request = 0x%x.\n",
-						     (int)requested_version);
+						pr_debug("Download error: Bad Version Request = 0x%x.\n",
+							 (int)requested_version);
 						status = -1;
 						break;
 					}
 					break;
 
 				default:
-					DEBUG
-					    ("FT1000:download:Download error: Bad request type=%d in CODE download state.\n",
-					     request);
+					pr_debug("Download error: Bad request type=%d in CODE download state.\n",
+						 request);
 					status = -1;
 					break;
 				}
@@ -963,20 +945,19 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 					put_handshake(ft1000dev,
 						      HANDSHAKE_RESPONSE);
 			} else {
-				DEBUG
-				    ("FT1000:download:Download error: Handshake failed\n");
+				pr_debug("Download error: Handshake failed\n");
 				status = -1;
 			}
 
 			break;
 
 		case STATE_DONE_DWNLD:
-			DEBUG("FT1000:download:Code loader is done...\n");
+			pr_debug("Code loader is done...\n");
 			state = STATE_SECTION_PROV;
 			break;
 
 		case STATE_SECTION_PROV:
-			DEBUG("FT1000:download:STATE_SECTION_PROV\n");
+			pr_debug("STATE_SECTION_PROV\n");
 			pseudo_header = (struct pseudo_hdr *)c_file;
 
 			if (pseudo_header->checksum ==
@@ -990,9 +971,9 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 
 				/* Get buffer for provisioning data */
 				pbuffer =
-				    kmalloc((pseudo_header_len +
-					     sizeof(struct pseudo_hdr)),
-					    GFP_ATOMIC);
+					kmalloc((pseudo_header_len +
+						 sizeof(struct pseudo_hdr)),
+						GFP_ATOMIC);
 				if (pbuffer) {
 					memcpy(pbuffer, (void *)c_file,
 					       (u32) (pseudo_header_len +
@@ -1000,20 +981,20 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 							     pseudo_hdr)));
 					/* link provisioning data */
 					pprov_record =
-					    kmalloc(sizeof(struct prov_record),
-						    GFP_ATOMIC);
+						kmalloc(sizeof(struct prov_record),
+							GFP_ATOMIC);
 					if (pprov_record) {
 						pprov_record->pprov_data =
-						    pbuffer;
+							pbuffer;
 						list_add_tail(&pprov_record->
 							      list,
 							      &pft1000info->
 							      prov_list);
 						/* Move to next entry if available */
 						c_file =
-						    (u8 *) ((unsigned long)
-							    c_file +
-							    (u32) ((pseudo_header_len + 1) & 0xFFFFFFFE) + sizeof(struct pseudo_hdr));
+							(u8 *) ((unsigned long)
+								c_file +
+								(u32) ((pseudo_header_len + 1) & 0xFFFFFFFE) + sizeof(struct pseudo_hdr));
 						if ((unsigned long)(c_file) -
 						    (unsigned long)(pFileStart)
 						    >=
@@ -1031,13 +1012,12 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 				/* Checksum did not compute */
 				status = -1;
 			}
-			DEBUG
-			    ("ft1000:download: after STATE_SECTION_PROV, state = %d, status= %d\n",
-			     state, status);
+			pr_debug("after STATE_SECTION_PROV, state = %d, status= %d\n",
+				 state, status);
 			break;
 
 		case STATE_DONE_PROV:
-			DEBUG("FT1000:download:STATE_DONE_PROV\n");
+			pr_debug("STATE_DONE_PROV\n");
 			state = STATE_DONE_FILE;
 			break;
 
@@ -1050,21 +1030,21 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 			break;
 
 /****
-      // Check if Card is present
-      status = Harley_Read_Register(&temp, FT1000_REG_SUP_IMASK);
-      if ( (status != NDIS_STATUS_SUCCESS) || (temp == 0x0000) ) {
-	break;
-      }
-
-      status = Harley_Read_Register(&temp, FT1000_REG_ASIC_ID);
-      if ( (status != NDIS_STATUS_SUCCESS) || (temp == 0xffff) ) {
-	break;
-      }
+ // Check if Card is present
+ status = Harley_Read_Register(&temp, FT1000_REG_SUP_IMASK);
+ if ( (status != NDIS_STATUS_SUCCESS) || (temp == 0x0000) ) {
+ break;
+ }
+
+ status = Harley_Read_Register(&temp, FT1000_REG_ASIC_ID);
+ if ( (status != NDIS_STATUS_SUCCESS) || (temp == 0xffff) ) {
+ break;
+ }
 ****/
 
 	}			/* End while */
 
-	DEBUG("Download exiting with status = 0x%8x\n", status);
+	pr_debug("Download exiting with status = 0x%8x\n", status);
 	ft1000_write_register(ft1000dev, FT1000_DB_DNLD_TX,
 			      FT1000_REG_DOORBELL);
 
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c b/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c
index 2e13e7b7ec10..d12cfc9aa32a 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c
@@ -1,8 +1,10 @@
 /* CopyRight (C) 2007 Qualcomm Inc. All Rights Reserved.
-*
-*
-* This file is part of Express Card USB Driver
-*/
+ *
+ *
+ * This file is part of Express Card USB Driver
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/kernel.h>
 #include <linux/module.h>
@@ -35,16 +37,16 @@ static u8 tempbuffer[1600];
 #define MAX_RCV_LOOP   100
 
 /* send a control message via USB interface synchronously
-*  Parameters:  ft1000_usb  - device structure
-*               pipe - usb control message pipe
-*               request - control request
-*               requesttype - control message request type
-*               value - value to be written or 0
-*               index - register index
-*               data - data buffer to hold the read/write values
-*               size - data size
-*               timeout - control message time out value
-*/
+ *  Parameters:  ft1000_usb  - device structure
+ *               pipe - usb control message pipe
+ *               request - control request
+ *               requesttype - control message request type
+ *               value - value to be written or 0
+ *               index - register index
+ *               data - data buffer to hold the read/write values
+ *               size - data size
+ *               timeout - control message time out value
+ */
 static int ft1000_control(struct ft1000_usb *ft1000dev, unsigned int pipe,
 			  u8 request, u8 requesttype, u16 value, u16 index,
 			  void *data, u16 size, int timeout)
@@ -52,7 +54,7 @@ static int ft1000_control(struct ft1000_usb *ft1000dev, unsigned int pipe,
 	int ret;
 
 	if ((ft1000dev == NULL) || (ft1000dev->dev == NULL)) {
-		DEBUG("ft1000dev or ft1000dev->dev == NULL, failure\n");
+		pr_debug("ft1000dev or ft1000dev->dev == NULL, failure\n");
 		return -ENODEV;
 	}
 
@@ -171,7 +173,7 @@ int ft1000_read_dpram16(struct ft1000_usb *ft1000dev, u16 indx, u8 *buffer,
 
 /* write into DPRAM a number of bytes */
 int ft1000_write_dpram16(struct ft1000_usb *ft1000dev, u16 indx, u16 value,
-		u8 highlow)
+			 u8 highlow)
 {
 	int ret = 0;
 	u8 request;
@@ -212,7 +214,7 @@ int fix_ft1000_read_dpram32(struct ft1000_usb *ft1000dev, u16 indx,
 		*buffer++ = buf[pos++];
 		*buffer++ = buf[pos++];
 	} else {
-		DEBUG("fix_ft1000_read_dpram32: DPRAM32 Read failed\n");
+		pr_debug("DPRAM32 Read failed\n");
 		*buffer++ = 0;
 		*buffer++ = 0;
 		*buffer++ = 0;
@@ -246,7 +248,7 @@ int fix_ft1000_write_dpram32(struct ft1000_usb *ft1000dev, u16 indx, u8 *buffer)
 		buf[pos2++] = *buffer++;
 		ret = ft1000_write_dpram32(ft1000dev, pos1, buf, 16);
 	} else {
-		DEBUG("fix_ft1000_write_dpram32: DPRAM32 Read failed\n");
+		pr_debug("DPRAM32 Read failed\n");
 		return ret;
 	}
 
@@ -270,8 +272,7 @@ int fix_ft1000_write_dpram32(struct ft1000_usb *ft1000dev, u16 indx, u8 *buffer)
 			for (i = 0; i < 16; i++) {
 				if (tempbuffer[i] != resultbuffer[i]) {
 					ret = -1;
-					DEBUG("%s Failed to write\n",
-					      __func__);
+					pr_debug("Failed to write\n");
 				}
 			}
 		}
@@ -287,19 +288,19 @@ static void card_reset_dsp(struct ft1000_usb *ft1000dev, bool value)
 	u16 tempword;
 
 	status = ft1000_write_register(ft1000dev, HOST_INTF_BE,
-					FT1000_REG_SUP_CTRL);
+				       FT1000_REG_SUP_CTRL);
 	status = ft1000_read_register(ft1000dev, &tempword,
 				      FT1000_REG_SUP_CTRL);
 
 	if (value) {
-		DEBUG("Reset DSP\n");
+		pr_debug("Reset DSP\n");
 		status = ft1000_read_register(ft1000dev, &tempword,
 					      FT1000_REG_RESET);
 		tempword |= DSP_RESET_BIT;
 		status = ft1000_write_register(ft1000dev, tempword,
 					       FT1000_REG_RESET);
 	} else {
-		DEBUG("Activate DSP\n");
+		pr_debug("Activate DSP\n");
 		status = ft1000_read_register(ft1000dev, &tempword,
 					      FT1000_REG_RESET);
 		tempword |= DSP_ENCRYPTED;
@@ -318,18 +319,18 @@ static void card_reset_dsp(struct ft1000_usb *ft1000dev, bool value)
 }
 
 /* send a command to ASIC
-*  Parameters:  ft1000_usb  - device structure
-*               ptempbuffer - command buffer
-*               size - command buffer size
-*/
+ *  Parameters:  ft1000_usb  - device structure
+ *               ptempbuffer - command buffer
+ *               size - command buffer size
+ */
 int card_send_command(struct ft1000_usb *ft1000dev, void *ptempbuffer,
-		       int size)
+		      int size)
 {
 	int ret;
 	unsigned short temp;
 	unsigned char *commandbuf;
 
-	DEBUG("card_send_command: enter card_send_command... size=%d\n", size);
+	pr_debug("enter card_send_command... size=%d\n", size);
 
 	commandbuf = kmalloc(size + 2, GFP_KERNEL);
 	if (!commandbuf)
@@ -355,7 +356,7 @@ int card_send_command(struct ft1000_usb *ft1000dev, void *ptempbuffer,
 		return ret;
 	usleep_range(900, 1100);
 	ret = ft1000_write_register(ft1000dev, FT1000_DB_DPRAM_TX,
-			      FT1000_REG_DOORBELL);
+				    FT1000_REG_DOORBELL);
 	if (ret)
 		return ret;
 	usleep_range(900, 1100);
@@ -364,7 +365,7 @@ int card_send_command(struct ft1000_usb *ft1000dev, void *ptempbuffer,
 
 #if 0
 	if ((temp & 0x0100) == 0)
-		DEBUG("card_send_command: Message sent\n");
+		pr_debug("Message sent\n");
 #endif
 	return ret;
 }
@@ -390,7 +391,7 @@ int dsp_reload(struct ft1000_usb *ft1000dev)
 	status = ft1000_write_register(ft1000dev, tempword, FT1000_REG_RESET);
 	msleep(1000);
 	status = ft1000_read_register(ft1000dev, &tempword, FT1000_REG_RESET);
-	DEBUG("Reset Register = 0x%x\n", tempword);
+	pr_debug("Reset Register = 0x%x\n", tempword);
 
 	/* Toggle DSP reset */
 	card_reset_dsp(ft1000dev, 1);
@@ -399,13 +400,13 @@ int dsp_reload(struct ft1000_usb *ft1000dev)
 	msleep(1000);
 
 	status =
-	    ft1000_write_register(ft1000dev, HOST_INTF_BE, FT1000_REG_SUP_CTRL);
+		ft1000_write_register(ft1000dev, HOST_INTF_BE, FT1000_REG_SUP_CTRL);
 
 	/* Let's check for FEFE */
 	status =
-	    ft1000_read_dpram32(ft1000dev, FT1000_MAG_DPRAM_FEFE_INDX,
-				(u8 *) &templong, 4);
-	DEBUG("templong (fefe) = 0x%8x\n", templong);
+		ft1000_read_dpram32(ft1000dev, FT1000_MAG_DPRAM_FEFE_INDX,
+				    (u8 *)&templong, 4);
+	pr_debug("templong (fefe) = 0x%8x\n", templong);
 
 	/* call codeloader */
 	status = scram_dnldr(ft1000dev, pFileStart, FileLength);
@@ -415,8 +416,6 @@ int dsp_reload(struct ft1000_usb *ft1000dev)
 
 	msleep(1000);
 
-	DEBUG("dsp_reload returned\n");
-
 	return 0;
 }
 
@@ -427,8 +426,6 @@ static void ft1000_reset_asic(struct net_device *dev)
 	struct ft1000_usb *ft1000dev = info->priv;
 	u16 tempword;
 
-	DEBUG("ft1000_hw:ft1000_reset_asic called\n");
-
 	/* Let's use the register provided by the Magnemite ASIC to reset the
 	 * ASIC and DSP.
 	 */
@@ -442,10 +439,10 @@ static void ft1000_reset_asic(struct net_device *dev)
 
 	/* clear interrupts */
 	ft1000_read_register(ft1000dev, &tempword, FT1000_REG_SUP_ISR);
-	DEBUG("ft1000_hw: interrupt status register = 0x%x\n", tempword);
+	pr_debug("interrupt status register = 0x%x\n", tempword);
 	ft1000_write_register(ft1000dev, tempword, FT1000_REG_SUP_ISR);
 	ft1000_read_register(ft1000dev, &tempword, FT1000_REG_SUP_ISR);
-	DEBUG("ft1000_hw: interrupt status register = 0x%x\n", tempword);
+	pr_debug("interrupt status register = 0x%x\n", tempword);
 }
 
 static int ft1000_reset_card(struct net_device *dev)
@@ -455,38 +452,36 @@ static int ft1000_reset_card(struct net_device *dev)
 	u16 tempword;
 	struct prov_record *ptr;
 
-	DEBUG("ft1000_hw:ft1000_reset_card called.....\n");
-
 	ft1000dev->fCondResetPend = true;
 	info->CardReady = 0;
 	ft1000dev->fProvComplete = false;
 
 	/* Make sure we free any memory reserve for provisioning */
 	while (list_empty(&info->prov_list) == 0) {
-		DEBUG("ft1000_reset_card:deleting provisioning record\n");
+		pr_debug("deleting provisioning record\n");
 		ptr =
-		    list_entry(info->prov_list.next, struct prov_record, list);
+			list_entry(info->prov_list.next, struct prov_record, list);
 		list_del(&ptr->list);
 		kfree(ptr->pprov_data);
 		kfree(ptr);
 	}
 
-	DEBUG("ft1000_hw:ft1000_reset_card: reset asic\n");
+	pr_debug("reset asic\n");
 	ft1000_reset_asic(dev);
 
-	DEBUG("ft1000_hw:ft1000_reset_card: call dsp_reload\n");
+	pr_debug("call dsp_reload\n");
 	dsp_reload(ft1000dev);
 
-	DEBUG("dsp reload successful\n");
+	pr_debug("dsp reload successful\n");
 
 	mdelay(10);
 
 	/* Initialize DSP heartbeat area */
 	ft1000_write_dpram16(ft1000dev, FT1000_MAG_HI_HO, ho_mag,
 			     FT1000_MAG_HI_HO_INDX);
-	ft1000_read_dpram16(ft1000dev, FT1000_MAG_HI_HO, (u8 *) &tempword,
+	ft1000_read_dpram16(ft1000dev, FT1000_MAG_HI_HO, (u8 *)&tempword,
 			    FT1000_MAG_HI_HO_INDX);
-	DEBUG("ft1000_hw:ft1000_reset_card:hi_ho value = 0x%x\n", tempword);
+	pr_debug("hi_ho value = 0x%x\n", tempword);
 
 	info->CardReady = 1;
 
@@ -508,8 +503,8 @@ static void ft1000_usb_transmit_complete(struct urb *urb)
 }
 
 /* take an ethernet packet and convert it to a Flarion
-*  packet prior to sending it to the ASIC Downlink FIFO.
-*/
+ *  packet prior to sending it to the ASIC Downlink FIFO.
+ */
 static int ft1000_copy_down_pkt(struct net_device *netdev, u8 *packet, u16 len)
 {
 	struct ft1000_info *pInfo = netdev_priv(netdev);
@@ -520,14 +515,13 @@ static int ft1000_copy_down_pkt(struct net_device *netdev, u8 *packet, u16 len)
 	struct pseudo_hdr hdr;
 
 	if (!pInfo->CardReady) {
-		DEBUG("ft1000_copy_down_pkt::Card Not Ready\n");
+		pr_debug("Card Not Ready\n");
 		return -ENODEV;
 	}
 
 	count = sizeof(struct pseudo_hdr) + len;
 	if (count > MAX_BUF_SIZE) {
-		DEBUG("Error:ft1000_copy_down_pkt:Message Size Overflow!\n");
-		DEBUG("size = %d\n", count);
+		pr_debug("Message Size Overflow! size = %d\n", count);
 		return -EINVAL;
 	}
 
@@ -545,7 +539,7 @@ static int ft1000_copy_down_pkt(struct net_device *netdev, u8 *packet, u16 len)
 	hdr.control = 0x00;
 
 	hdr.checksum = hdr.length ^ hdr.source ^ hdr.destination ^
-	    hdr.portdest ^ hdr.portsrc ^ hdr.sh_str_id ^ hdr.control;
+		hdr.portdest ^ hdr.portsrc ^ hdr.sh_str_id ^ hdr.control;
 
 	memcpy(&pFt1000Dev->tx_buf[0], &hdr, sizeof(hdr));
 	memcpy(&(pFt1000Dev->tx_buf[sizeof(struct pseudo_hdr)]), packet, len);
@@ -559,12 +553,12 @@ static int ft1000_copy_down_pkt(struct net_device *netdev, u8 *packet, u16 len)
 			  pFt1000Dev->tx_buf, count,
 			  ft1000_usb_transmit_complete, (void *)pFt1000Dev);
 
-	t = (u8 *) pFt1000Dev->tx_urb->transfer_buffer;
+	t = (u8 *)pFt1000Dev->tx_urb->transfer_buffer;
 
 	ret = usb_submit_urb(pFt1000Dev->tx_urb, GFP_ATOMIC);
 
 	if (ret) {
-		DEBUG("ft1000 failed tx_urb %d\n", ret);
+		pr_debug("failed tx_urb %d\n", ret);
 		return ret;
 	}
 	pInfo->stats.tx_packets++;
@@ -574,9 +568,9 @@ static int ft1000_copy_down_pkt(struct net_device *netdev, u8 *packet, u16 len)
 }
 
 /* transmit an ethernet packet
-*  Parameters:  skb - socket buffer to be sent
-*               dev - network device
-*/
+ *  Parameters:  skb - socket buffer to be sent
+ *               dev - network device
+ */
 static int ft1000_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct ft1000_info *pInfo = netdev_priv(dev);
@@ -585,30 +579,30 @@ static int ft1000_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	int maxlen, pipe;
 
 	if (skb == NULL) {
-		DEBUG("ft1000_hw: ft1000_start_xmit:skb == NULL!!!\n");
+		pr_debug("skb == NULL!!!\n");
 		return NETDEV_TX_OK;
 	}
 
 	if (pFt1000Dev->status & FT1000_STATUS_CLOSING) {
-		DEBUG("network driver is closed, return\n");
+		pr_debug("network driver is closed, return\n");
 		goto err;
 	}
 
 	pipe =
-	    usb_sndbulkpipe(pFt1000Dev->dev, pFt1000Dev->bulk_out_endpointAddr);
+		usb_sndbulkpipe(pFt1000Dev->dev, pFt1000Dev->bulk_out_endpointAddr);
 	maxlen = usb_maxpacket(pFt1000Dev->dev, pipe, usb_pipeout(pipe));
 
-	pdata = (u8 *) skb->data;
+	pdata = (u8 *)skb->data;
 
 	if (pInfo->mediastate == 0) {
 		/* Drop packet is mediastate is down */
-		DEBUG("ft1000_hw:ft1000_start_xmit:mediastate is down\n");
+		pr_debug("mediastate is down\n");
 		goto err;
 	}
 
 	if ((skb->len < ENET_HEADER_SIZE) || (skb->len > ENET_MAX_SIZE)) {
 		/* Drop packet which has invalid size */
-		DEBUG("ft1000_hw:ft1000_start_xmit:invalid ethernet length\n");
+		pr_debug("invalid ethernet length\n");
 		goto err;
 	}
 
@@ -628,7 +622,7 @@ static int ft1000_open(struct net_device *dev)
 	struct ft1000_usb *pFt1000Dev = pInfo->priv;
 	struct timeval tv;
 
-	DEBUG("ft1000_open is called for card %d\n", pFt1000Dev->CardNumber);
+	pr_debug("ft1000_open is called for card %d\n", pFt1000Dev->CardNumber);
 
 	pInfo->stats.rx_bytes = 0;
 	pInfo->stats.tx_bytes = 0;
@@ -676,11 +670,9 @@ int init_ft1000_netdev(struct ft1000_usb *ft1000dev)
 	char card_nr[2];
 	u8 gCardIndex = 0;
 
-	DEBUG("Enter init_ft1000_netdev...\n");
-
 	netdev = alloc_etherdev(sizeof(struct ft1000_info));
 	if (!netdev) {
-		DEBUG("init_ft1000_netdev: can not allocate network device\n");
+		pr_debug("can not allocate network device\n");
 		return -ENOMEM;
 	}
 
@@ -690,7 +682,7 @@ int init_ft1000_netdev(struct ft1000_usb *ft1000dev)
 
 	dev_alloc_name(netdev, netdev->name);
 
-	DEBUG("init_ft1000_netdev: network device name is %s\n", netdev->name);
+	pr_debug("network device name is %s\n", netdev->name);
 
 	if (strncmp(netdev->name, "eth", 3) == 0) {
 		card_nr[0] = netdev->name[3];
@@ -702,7 +694,7 @@ int init_ft1000_netdev(struct ft1000_usb *ft1000dev)
 		}
 
 		ft1000dev->CardNumber = gCardIndex;
-		DEBUG("card number = %d\n", ft1000dev->CardNumber);
+		pr_debug("card number = %d\n", ft1000dev->CardNumber);
 	} else {
 		netdev_err(ft1000dev->net, "ft1000: Invalid device name\n");
 		ret_val = -ENXIO;
@@ -738,7 +730,7 @@ int init_ft1000_netdev(struct ft1000_usb *ft1000dev)
 
 	ft1000dev->net = netdev;
 
-	DEBUG("Initialize free_buff_lock and freercvpool\n");
+	pr_debug("Initialize free_buff_lock and freercvpool\n");
 	spin_lock_init(&free_buff_lock);
 
 	/* initialize a list of buffers to be use for queuing
@@ -790,7 +782,6 @@ int reg_ft1000_netdev(struct ft1000_usb *ft1000dev,
 
 	netdev = ft1000dev->net;
 	pInfo = netdev_priv(ft1000dev->net);
-	DEBUG("Enter reg_ft1000_netdev...\n");
 
 	ft1000_read_register(ft1000dev, &pInfo->AsicID, FT1000_REG_ASIC_ID);
 
@@ -799,23 +790,21 @@ int reg_ft1000_netdev(struct ft1000_usb *ft1000dev,
 
 	rc = register_netdev(netdev);
 	if (rc) {
-		DEBUG("reg_ft1000_netdev: could not register network device\n");
+		pr_debug("could not register network device\n");
 		free_netdev(netdev);
 		return rc;
 	}
 
 	ft1000_create_dev(ft1000dev);
 
-	DEBUG("reg_ft1000_netdev returned\n");
-
 	pInfo->CardReady = 1;
 
 	return 0;
 }
 
 /* take a packet from the FIFO up link and
-*  convert it into an ethernet packet and deliver it to the IP stack
-*/
+ *  convert it into an ethernet packet and deliver it to the IP stack
+ */
 static int ft1000_copy_up_pkt(struct urb *urb)
 {
 	struct ft1000_info *info = urb->context;
@@ -832,14 +821,14 @@ static int ft1000_copy_up_pkt(struct urb *urb)
 	u16 *chksum;
 
 	if (ft1000dev->status & FT1000_STATUS_CLOSING) {
-		DEBUG("network driver is closed, return\n");
+		pr_debug("network driver is closed, return\n");
 		return 0;
 	}
 	/* Read length */
 	len = urb->transfer_buffer_length;
 	lena = urb->actual_length;
 
-	chksum = (u16 *) ft1000dev->rx_buf;
+	chksum = (u16 *)ft1000dev->rx_buf;
 
 	tempword = *chksum++;
 	for (i = 1; i < 7; i++)
@@ -854,13 +843,13 @@ static int ft1000_copy_up_pkt(struct urb *urb)
 	skb = dev_alloc_skb(len + 12 + 2);
 
 	if (skb == NULL) {
-		DEBUG("ft1000_copy_up_pkt: No Network buffers available\n");
+		pr_debug("No Network buffers available\n");
 		info->stats.rx_errors++;
 		ft1000_submit_rx_urb(info);
 		return -1;
 	}
 
-	pbuffer = (u8 *) skb_put(skb, len + 12);
+	pbuffer = (u8 *)skb_put(skb, len + 12);
 
 	/* subtract the number of bytes read already */
 	ptemp = pbuffer;
@@ -905,7 +894,7 @@ static int ft1000_submit_rx_urb(struct ft1000_info *info)
 	struct ft1000_usb *pFt1000Dev = info->priv;
 
 	if (pFt1000Dev->status & FT1000_STATUS_CLOSING) {
-		DEBUG("network driver is closed, return\n");
+		pr_debug("network driver is closed, return\n");
 		return -ENODEV;
 	}
 
@@ -914,13 +903,12 @@ static int ft1000_submit_rx_urb(struct ft1000_info *info)
 			  usb_rcvbulkpipe(pFt1000Dev->dev,
 					  pFt1000Dev->bulk_in_endpointAddr),
 			  pFt1000Dev->rx_buf, MAX_BUF_SIZE,
-			  (usb_complete_t) ft1000_copy_up_pkt, info);
+			  (usb_complete_t)ft1000_copy_up_pkt, info);
 
 	result = usb_submit_urb(pFt1000Dev->rx_urb, GFP_ATOMIC);
 
 	if (result) {
-		pr_err("ft1000_submit_rx_urb: submitting rx_urb %d failed\n",
-		       result);
+		pr_err("submitting rx_urb %d failed\n", result);
 		return result;
 	}
 
@@ -935,7 +923,7 @@ int ft1000_close(struct net_device *net)
 
 	ft1000dev->status |= FT1000_STATUS_CLOSING;
 
-	DEBUG("ft1000_close: pInfo=%p, ft1000dev=%p\n", pInfo, ft1000dev);
+	pr_debug("pInfo=%p, ft1000dev=%p\n", pInfo, ft1000dev);
 	netif_carrier_off(net);
 	netif_stop_queue(net);
 	ft1000dev->status &= ~FT1000_STATUS_CLOSING;
@@ -952,7 +940,7 @@ static int ft1000_chkcard(struct ft1000_usb *dev)
 	int status;
 
 	if (dev->fCondResetPend) {
-		DEBUG("ft1000_hw:ft1000_chkcard:Card is being reset, return FALSE\n");
+		pr_debug("Card is being reset, return FALSE\n");
 		return TRUE;
 	}
 	/* Mask register is used to check for device presence since it is never
@@ -960,7 +948,7 @@ static int ft1000_chkcard(struct ft1000_usb *dev)
 	 */
 	status = ft1000_read_register(dev, &tempword, FT1000_REG_SUP_IMASK);
 	if (tempword == 0) {
-		DEBUG("ft1000_hw:ft1000_chkcard: IMASK = 0 Card not detected\n");
+		pr_debug("IMASK = 0 Card not detected\n");
 		return FALSE;
 	}
 	/* The system will return the value of 0xffff for the version register
@@ -969,17 +957,17 @@ static int ft1000_chkcard(struct ft1000_usb *dev)
 	status = ft1000_read_register(dev, &tempword, FT1000_REG_ASIC_ID);
 	if (tempword != 0x1b01) {
 		dev->status |= FT1000_STATUS_CLOSING;
-		DEBUG("ft1000_hw:ft1000_chkcard: Version = 0xffff Card not detected\n");
+		pr_debug("Version = 0xffff Card not detected\n");
 		return FALSE;
 	}
 	return TRUE;
 }
 
 /* read a message from the dpram area.
-*  Input:
-*    dev - network device structure
-*    pbuffer - caller supply address to buffer
-*/
+ *  Input:
+ *    dev - network device structure
+ *    pbuffer - caller supply address to buffer
+ */
 static bool ft1000_receive_cmd(struct ft1000_usb *dev, u16 *pbuffer,
 			       int maxsz)
 {
@@ -990,46 +978,45 @@ static bool ft1000_receive_cmd(struct ft1000_usb *dev, u16 *pbuffer,
 	u16 tempword;
 
 	ret =
-	    ft1000_read_dpram16(dev, FT1000_MAG_PH_LEN, (u8 *) &size,
-				FT1000_MAG_PH_LEN_INDX);
+		ft1000_read_dpram16(dev, FT1000_MAG_PH_LEN, (u8 *)&size,
+				    FT1000_MAG_PH_LEN_INDX);
 	size = ntohs(size) + PSEUDOSZ;
 	if (size > maxsz) {
-		DEBUG("FT1000:ft1000_receive_cmd:Invalid command length = %d\n",
-		      size);
+		pr_debug("Invalid command length = %d\n", size);
 		return FALSE;
 	}
-	ppseudohdr = (u16 *) pbuffer;
+	ppseudohdr = (u16 *)pbuffer;
 	ft1000_write_register(dev, FT1000_DPRAM_MAG_RX_BASE,
 			      FT1000_REG_DPRAM_ADDR);
 	ret =
-	    ft1000_read_register(dev, pbuffer, FT1000_REG_MAG_DPDATAH);
+		ft1000_read_register(dev, pbuffer, FT1000_REG_MAG_DPDATAH);
 	pbuffer++;
 	ft1000_write_register(dev, FT1000_DPRAM_MAG_RX_BASE + 1,
 			      FT1000_REG_DPRAM_ADDR);
 	for (i = 0; i <= (size >> 2); i++) {
 		ret =
-		    ft1000_read_register(dev, pbuffer,
-					 FT1000_REG_MAG_DPDATAL);
+			ft1000_read_register(dev, pbuffer,
+					     FT1000_REG_MAG_DPDATAL);
 		pbuffer++;
 		ret =
-		    ft1000_read_register(dev, pbuffer,
-					 FT1000_REG_MAG_DPDATAH);
+			ft1000_read_register(dev, pbuffer,
+					     FT1000_REG_MAG_DPDATAH);
 		pbuffer++;
 	}
 	/* copy odd aligned word */
 	ret =
-	    ft1000_read_register(dev, pbuffer, FT1000_REG_MAG_DPDATAL);
+		ft1000_read_register(dev, pbuffer, FT1000_REG_MAG_DPDATAL);
 
 	pbuffer++;
 	ret =
-	    ft1000_read_register(dev, pbuffer, FT1000_REG_MAG_DPDATAH);
+		ft1000_read_register(dev, pbuffer, FT1000_REG_MAG_DPDATAH);
 
 	pbuffer++;
 	if (size & 0x0001) {
 		/* copy odd byte from fifo */
 		ret =
-		    ft1000_read_register(dev, &tempword,
-					 FT1000_REG_DPRAM_DATA);
+			ft1000_read_register(dev, &tempword,
+					     FT1000_REG_DPRAM_DATA);
 		*pbuffer = ntohs(tempword);
 	}
 	/* Check if pseudo header checksum is good
@@ -1058,17 +1045,15 @@ static int ft1000_dsp_prov(void *arg)
 	int status;
 	u16 TempShortBuf[256];
 
-	DEBUG("*** DspProv Entered\n");
-
 	while (list_empty(&info->prov_list) == 0) {
-		DEBUG("DSP Provisioning List Entry\n");
+		pr_debug("DSP Provisioning List Entry\n");
 
 		/* Check if doorbell is available */
-		DEBUG("check if doorbell is cleared\n");
+		pr_debug("check if doorbell is cleared\n");
 		status =
-		    ft1000_read_register(dev, &tempword, FT1000_REG_DOORBELL);
+			ft1000_read_register(dev, &tempword, FT1000_REG_DOORBELL);
 		if (status) {
-			DEBUG("ft1000_dsp_prov::ft1000_read_register error\n");
+			pr_debug("ft1000_read_register error\n");
 			break;
 		}
 
@@ -1076,7 +1061,7 @@ static int ft1000_dsp_prov(void *arg)
 			mdelay(10);
 			i++;
 			if (i == 10) {
-				DEBUG("FT1000:ft1000_dsp_prov:message drop\n");
+				pr_debug("message drop\n");
 				return -1;
 			}
 			ft1000_read_register(dev, &tempword,
@@ -1084,17 +1069,17 @@ static int ft1000_dsp_prov(void *arg)
 		}
 
 		if (!(tempword & FT1000_DB_DPRAM_TX)) {
-			DEBUG("*** Provision Data Sent to DSP\n");
+			pr_debug("*** Provision Data Sent to DSP\n");
 
 			/* Send provisioning data */
 			ptr =
-			    list_entry(info->prov_list.next, struct prov_record,
-				       list);
-			len = *(u16 *) ptr->pprov_data;
+				list_entry(info->prov_list.next, struct prov_record,
+					   list);
+			len = *(u16 *)ptr->pprov_data;
 			len = htons(len);
 			len += PSEUDOSZ;
 
-			pmsg = (u16 *) ptr->pprov_data;
+			pmsg = (u16 *)ptr->pprov_data;
 			ppseudo_hdr = (struct pseudo_hdr *)pmsg;
 			/* Insert slow queue sequence number */
 			ppseudo_hdr->seq_num = info->squeseqnum++;
@@ -1109,12 +1094,12 @@ static int ft1000_dsp_prov(void *arg)
 			memcpy(&TempShortBuf[2], ppseudo_hdr, len);
 
 			status =
-			    ft1000_write_dpram32(dev, 0,
-						 (u8 *) &TempShortBuf[0],
-						 (unsigned short)(len + 2));
+				ft1000_write_dpram32(dev, 0,
+						     (u8 *)&TempShortBuf[0],
+						     (unsigned short)(len + 2));
 			status =
-			    ft1000_write_register(dev, FT1000_DB_DPRAM_TX,
-						  FT1000_REG_DOORBELL);
+				ft1000_write_register(dev, FT1000_DB_DPRAM_TX,
+						      FT1000_REG_DOORBELL);
 
 			list_del(&ptr->list);
 			kfree(ptr->pprov_data);
@@ -1123,7 +1108,7 @@ static int ft1000_dsp_prov(void *arg)
 		usleep_range(9000, 11000);
 	}
 
-	DEBUG("DSP Provisioning List Entry finished\n");
+	pr_debug("DSP Provisioning List Entry finished\n");
 
 	msleep(100);
 
@@ -1158,37 +1143,26 @@ static int ft1000_proc_drvmsg(struct ft1000_usb *dev, u16 size)
 	status = ft1000_read_dpram32(dev, 0x200, cmdbuffer, size);
 
 #ifdef JDEBUG
-	DEBUG("ft1000_proc_drvmsg:cmdbuffer\n");
-	for (i = 0; i < size; i += 5) {
-		if ((i + 5) < size)
-			DEBUG("0x%x, 0x%x, 0x%x, 0x%x, 0x%x\n", cmdbuffer[i],
-			      cmdbuffer[i + 1], cmdbuffer[i + 2],
-			      cmdbuffer[i + 3], cmdbuffer[i + 4]);
-		else {
-			for (j = i; j < size; j++)
-				DEBUG("0x%x ", cmdbuffer[j]);
-			DEBUG("\n");
-			break;
-		}
-	}
+	print_hex_dump_debug("cmdbuffer: ", HEX_DUMP_OFFSET, 16, 1,
+			     cmdbuffer, size, true);
 #endif
 	pdrvmsg = (struct drv_msg *)&cmdbuffer[2];
 	msgtype = ntohs(pdrvmsg->type);
-	DEBUG("ft1000_proc_drvmsg:Command message type = 0x%x\n", msgtype);
+	pr_debug("Command message type = 0x%x\n", msgtype);
 	switch (msgtype) {
 	case MEDIA_STATE:{
-		DEBUG("ft1000_proc_drvmsg:Command message type = MEDIA_STATE");
+		pr_debug("Command message type = MEDIA_STATE\n");
 		pmediamsg = (struct media_msg *)&cmdbuffer[0];
 		if (info->ProgConStat != 0xFF) {
 			if (pmediamsg->state) {
-				DEBUG("Media is up\n");
+				pr_debug("Media is up\n");
 				if (info->mediastate == 0) {
 					if (dev->NetDevRegDone)
 						netif_wake_queue(dev->net);
 					info->mediastate = 1;
 				}
 			} else {
-				DEBUG("Media is down\n");
+				pr_debug("Media is down\n");
 				if (info->mediastate == 1) {
 					info->mediastate = 0;
 					if (dev->NetDevRegDone)
@@ -1196,7 +1170,7 @@ static int ft1000_proc_drvmsg(struct ft1000_usb *dev, u16 size)
 				}
 			}
 		} else {
-			DEBUG("Media is down\n");
+			pr_debug("Media is down\n");
 			if (info->mediastate == 1) {
 				info->mediastate = 0;
 				info->ConTm = 0;
@@ -1205,20 +1179,20 @@ static int ft1000_proc_drvmsg(struct ft1000_usb *dev, u16 size)
 		break;
 	}
 	case DSP_INIT_MSG:{
-		DEBUG("ft1000_proc_drvmsg:Command message type = DSP_INIT_MSG");
+		pr_debug("Command message type = DSP_INIT_MSG\n");
 		pdspinitmsg = (struct dsp_init_msg *)&cmdbuffer[2];
 		memcpy(info->DspVer, pdspinitmsg->DspVer, DSPVERSZ);
-		DEBUG("DSPVER = 0x%2x 0x%2x 0x%2x 0x%2x\n",
-		      info->DspVer[0], info->DspVer[1], info->DspVer[2],
-		      info->DspVer[3]);
+		pr_debug("DSPVER = 0x%2x 0x%2x 0x%2x 0x%2x\n",
+			 info->DspVer[0], info->DspVer[1], info->DspVer[2],
+			 info->DspVer[3]);
 		memcpy(info->HwSerNum, pdspinitmsg->HwSerNum,
 		       HWSERNUMSZ);
 		memcpy(info->Sku, pdspinitmsg->Sku, SKUSZ);
 		memcpy(info->eui64, pdspinitmsg->eui64, EUISZ);
-		DEBUG("EUI64=%2x.%2x.%2x.%2x.%2x.%2x.%2x.%2x\n",
-		      info->eui64[0], info->eui64[1], info->eui64[2],
-		      info->eui64[3], info->eui64[4], info->eui64[5],
-		      info->eui64[6], info->eui64[7]);
+		pr_debug("EUI64=%2x.%2x.%2x.%2x.%2x.%2x.%2x.%2x\n",
+			 info->eui64[0], info->eui64[1], info->eui64[2],
+			 info->eui64[3], info->eui64[4], info->eui64[5],
+			 info->eui64[6], info->eui64[7]);
 		dev->net->dev_addr[0] = info->eui64[0];
 		dev->net->dev_addr[1] = info->eui64[1];
 		dev->net->dev_addr[2] = info->eui64[2];
@@ -1229,17 +1203,17 @@ static int ft1000_proc_drvmsg(struct ft1000_usb *dev, u16 size)
 		if (ntohs(pdspinitmsg->length) ==
 		    (sizeof(struct dsp_init_msg) - 20)) {
 			memcpy(info->ProductMode, pdspinitmsg->ProductMode,
-					MODESZ);
+			       MODESZ);
 			memcpy(info->RfCalVer, pdspinitmsg->RfCalVer, CALVERSZ);
 			memcpy(info->RfCalDate, pdspinitmsg->RfCalDate,
 			       CALDATESZ);
-			DEBUG("RFCalVer = 0x%2x 0x%2x\n", info->RfCalVer[0],
-					info->RfCalVer[1]);
+			pr_debug("RFCalVer = 0x%2x 0x%2x\n",
+				 info->RfCalVer[0], info->RfCalVer[1]);
 		}
 		break;
 	}
 	case DSP_PROVISION:{
-		DEBUG("ft1000_proc_drvmsg:Command message type = DSP_PROVISION\n");
+		pr_debug("Command message type = DSP_PROVISION\n");
 
 		/* kick off dspprov routine to start provisioning
 		 * Send provisioning data to DSP
@@ -1252,21 +1226,20 @@ static int ft1000_proc_drvmsg(struct ft1000_usb *dev, u16 size)
 		} else {
 			dev->fProvComplete = true;
 			status = ft1000_write_register(dev, FT1000_DB_HB,
-					FT1000_REG_DOORBELL);
-			DEBUG("FT1000:drivermsg:No more DSP provisioning data in dsp image\n");
+						       FT1000_REG_DOORBELL);
+			pr_debug("No more DSP provisioning data in dsp image\n");
 		}
-		DEBUG("ft1000_proc_drvmsg:DSP PROVISION is done\n");
+		pr_debug("DSP PROVISION is done\n");
 		break;
 	}
 	case DSP_STORE_INFO:{
-		DEBUG("ft1000_proc_drvmsg:Command message type = DSP_STORE_INFO");
-		DEBUG("FT1000:drivermsg:Got DSP_STORE_INFO\n");
+		pr_debug("Command message type = DSP_STORE_INFO");
 		tempword = ntohs(pdrvmsg->length);
 		info->DSPInfoBlklen = tempword;
 		if (tempword < (MAX_DSP_SESS_REC - 4)) {
-			pmsg = (u16 *) &pdrvmsg->data[0];
+			pmsg = (u16 *)&pdrvmsg->data[0];
 			for (i = 0; i < ((tempword + 1) / 2); i++) {
-				DEBUG("FT1000:drivermsg:dsp info data = 0x%x\n", *pmsg);
+				pr_debug("dsp info data = 0x%x\n", *pmsg);
 				info->DSPInfoBlk[i + 10] = *pmsg++;
 			}
 		} else {
@@ -1275,33 +1248,33 @@ static int ft1000_proc_drvmsg(struct ft1000_usb *dev, u16 size)
 		break;
 	}
 	case DSP_GET_INFO:{
-		DEBUG("FT1000:drivermsg:Got DSP_GET_INFO\n");
+		pr_debug("Got DSP_GET_INFO\n");
 		/* copy dsp info block to dsp */
 		dev->DrvMsgPend = 1;
 		/* allow any outstanding ioctl to finish */
 		mdelay(10);
 		status = ft1000_read_register(dev, &tempword,
-				FT1000_REG_DOORBELL);
+					      FT1000_REG_DOORBELL);
 		if (tempword & FT1000_DB_DPRAM_TX) {
 			mdelay(10);
 			status = ft1000_read_register(dev, &tempword,
-					FT1000_REG_DOORBELL);
+						      FT1000_REG_DOORBELL);
 			if (tempword & FT1000_DB_DPRAM_TX) {
 				mdelay(10);
 				status = ft1000_read_register(dev, &tempword,
-						FT1000_REG_DOORBELL);
+							      FT1000_REG_DOORBELL);
 				if (tempword & FT1000_DB_DPRAM_TX)
 					break;
 			}
 		}
 		/* Put message into Slow Queue Form Pseudo header */
-		pmsg = (u16 *) info->DSPInfoBlk;
+		pmsg = (u16 *)info->DSPInfoBlk;
 		*pmsg++ = 0;
 		*pmsg++ = htons(info->DSPInfoBlklen + 20 + info->DSPInfoBlklen);
 		ppseudo_hdr =
-		    (struct pseudo_hdr *)(u16 *) &info->DSPInfoBlk[2];
+			(struct pseudo_hdr *)(u16 *)&info->DSPInfoBlk[2];
 		ppseudo_hdr->length = htons(info->DSPInfoBlklen + 4
-				+ info->DSPInfoBlklen);
+					    + info->DSPInfoBlklen);
 		ppseudo_hdr->source = 0x10;
 		ppseudo_hdr->destination = 0x20;
 		ppseudo_hdr->portdest = 0;
@@ -1323,31 +1296,31 @@ static int ft1000_proc_drvmsg(struct ft1000_usb *dev, u16 size)
 		info->DSPInfoBlk[10] = 0x7200;
 		info->DSPInfoBlk[11] = htons(info->DSPInfoBlklen);
 		status = ft1000_write_dpram32(dev, 0,
-				(u8 *)&info->DSPInfoBlk[0],
-				(unsigned short)(info->DSPInfoBlklen + 22));
+					      (u8 *)&info->DSPInfoBlk[0],
+					      (unsigned short)(info->DSPInfoBlklen + 22));
 		status = ft1000_write_register(dev, FT1000_DB_DPRAM_TX,
-				FT1000_REG_DOORBELL);
+					       FT1000_REG_DOORBELL);
 		dev->DrvMsgPend = 0;
 		break;
 	}
 	case GET_DRV_ERR_RPT_MSG:{
-		DEBUG("FT1000:drivermsg:Got GET_DRV_ERR_RPT_MSG\n");
+		pr_debug("Got GET_DRV_ERR_RPT_MSG\n");
 		/* copy driver error message to dsp */
 		dev->DrvMsgPend = 1;
 		/* allow any outstanding ioctl to finish */
 		mdelay(10);
 		status = ft1000_read_register(dev, &tempword,
-				FT1000_REG_DOORBELL);
+					      FT1000_REG_DOORBELL);
 		if (tempword & FT1000_DB_DPRAM_TX) {
 			mdelay(10);
 			status = ft1000_read_register(dev, &tempword,
-					FT1000_REG_DOORBELL);
+						      FT1000_REG_DOORBELL);
 			if (tempword & FT1000_DB_DPRAM_TX)
 				mdelay(10);
 		}
 		if ((tempword & FT1000_DB_DPRAM_TX) == 0) {
 			/* Put message into Slow Queue Form Pseudo header */
-			pmsg = (u16 *) &tempbuffer[0];
+			pmsg = (u16 *)&tempbuffer[0];
 			ppseudo_hdr = (struct pseudo_hdr *)pmsg;
 			ppseudo_hdr->length = htons(0x0012);
 			ppseudo_hdr->source = 0x10;
@@ -1368,7 +1341,7 @@ static int ft1000_proc_drvmsg(struct ft1000_usb *dev, u16 size)
 			for (i = 1; i < 7; i++)
 				ppseudo_hdr->checksum ^= *pmsg++;
 
-			pmsg = (u16 *) &tempbuffer[16];
+			pmsg = (u16 *)&tempbuffer[16];
 			*pmsg++ = htons(RSP_DRV_ERR_RPT_MSG);
 			*pmsg++ = htons(0x000e);
 			*pmsg++ = htons(info->DSP_TIME[0]);
@@ -1384,7 +1357,7 @@ static int ft1000_proc_drvmsg(struct ft1000_usb *dev, u16 size)
 			*pmsg++ = htons(info->DrvErrNum);
 
 			status = card_send_command(dev, (unsigned char *)&tempbuffer[0],
-					(u16)(0x0012 + PSEUDOSZ));
+						   (u16)(0x0012 + PSEUDOSZ));
 			if (status)
 				goto out;
 			info->DrvErrNum = 0;
@@ -1399,7 +1372,6 @@ static int ft1000_proc_drvmsg(struct ft1000_usb *dev, u16 size)
 	status = 0;
 out:
 	kfree(cmdbuffer);
-	DEBUG("return from ft1000_proc_drvmsg\n");
 	return status;
 }
 
@@ -1412,32 +1384,32 @@ static int dsp_broadcast_msg_id(struct ft1000_usb *dev)
 
 	for (i = 0; i < MAX_NUM_APP; i++) {
 		if ((dev->app_info[i].DspBCMsgFlag)
-				&& (dev->app_info[i].fileobject)
-				&& (dev->app_info[i].NumOfMsg
-					< MAX_MSG_LIMIT)) {
+		    && (dev->app_info[i].fileobject)
+		    && (dev->app_info[i].NumOfMsg
+			< MAX_MSG_LIMIT)) {
 			pdpram_blk = ft1000_get_buffer(&freercvpool);
 			if (pdpram_blk == NULL) {
-				DEBUG("Out of memory in free receive command pool\n");
+				pr_debug("Out of memory in free receive command pool\n");
 				dev->app_info[i].nRxMsgMiss++;
 				return -1;
 			}
 			if (ft1000_receive_cmd(dev, pdpram_blk->pbuffer,
-						MAX_CMD_SQSIZE)) {
+					       MAX_CMD_SQSIZE)) {
 				/* Put message into the
 				 * appropriate application block
 				 */
 				dev->app_info[i].nRxMsg++;
 				spin_lock_irqsave(&free_buff_lock, flags);
 				list_add_tail(&pdpram_blk->list,
-						&dev->app_info[i] .app_sqlist);
+					      &dev->app_info[i] .app_sqlist);
 				dev->app_info[i].NumOfMsg++;
 				spin_unlock_irqrestore(&free_buff_lock, flags);
 				wake_up_interruptible(&dev->app_info[i]
-						.wait_dpram_msg);
+						      .wait_dpram_msg);
 			} else {
 				dev->app_info[i].nRxMsgMiss++;
 				ft1000_free_buffer(pdpram_blk, &freercvpool);
-				DEBUG("pdpram_blk::ft1000_get_buffer NULL\n");
+				pr_debug("ft1000_get_buffer NULL\n");
 				return -1;
 			}
 		}
@@ -1452,7 +1424,7 @@ static int handle_misc_portid(struct ft1000_usb *dev)
 
 	pdpram_blk = ft1000_get_buffer(&freercvpool);
 	if (pdpram_blk == NULL) {
-		DEBUG("Out of memory in free receive command pool\n");
+		pr_debug("Out of memory in free receive command pool\n");
 		return -1;
 	}
 	if (!ft1000_receive_cmd(dev, pdpram_blk->pbuffer, MAX_CMD_SQSIZE))
@@ -1461,11 +1433,12 @@ static int handle_misc_portid(struct ft1000_usb *dev)
 	/* Search for correct application block */
 	for (i = 0; i < MAX_NUM_APP; i++) {
 		if (dev->app_info[i].app_id == ((struct pseudo_hdr *)
-					pdpram_blk->pbuffer)->portdest)
+						pdpram_blk->pbuffer)->portdest)
 			break;
 	}
 	if (i == MAX_NUM_APP) {
-		DEBUG("FT1000:ft1000_parse_dpram_msg: No application matching id = %d\n", ((struct pseudo_hdr *)pdpram_blk->pbuffer)->portdest);
+		pr_debug("No application matching id = %d\n",
+			 ((struct pseudo_hdr *)pdpram_blk->pbuffer)->portdest);
 		goto exit_failure;
 	} else if (dev->app_info[i].NumOfMsg > MAX_MSG_LIMIT) {
 		goto exit_failure;
@@ -1495,26 +1468,26 @@ int ft1000_poll(void *dev_id)
 	u16 portid;
 
 	if (ft1000_chkcard(dev) == FALSE) {
-		DEBUG("ft1000_poll::ft1000_chkcard: failed\n");
+		pr_debug("failed\n");
 		return -1;
 	}
 	status = ft1000_read_register(dev, &tempword, FT1000_REG_DOORBELL);
 	if (!status) {
 		if (tempword & FT1000_DB_DPRAM_RX) {
 			status = ft1000_read_dpram16(dev,
-					0x200, (u8 *)&data, 0);
+						     0x200, (u8 *)&data, 0);
 			size = ntohs(data) + 16 + 2;
 			if (size % 4) {
 				modulo = 4 - (size % 4);
 				size = size + modulo;
 			}
 			status = ft1000_read_dpram16(dev, 0x201,
-					(u8 *)&portid, 1);
+						     (u8 *)&portid, 1);
 			portid &= 0xff;
 			if (size < MAX_CMD_SQSIZE) {
 				switch (portid) {
 				case DRIVERID:
-					DEBUG("ft1000_poll: FT1000_REG_DOORBELL message type: FT1000_DB_DPRAM_RX : portid DRIVERID\n");
+					pr_debug("FT1000_REG_DOORBELL message type: FT1000_DB_DPRAM_RX : portid DRIVERID\n");
 					status = ft1000_proc_drvmsg(dev, size);
 					if (status != 0)
 						return status;
@@ -1527,87 +1500,88 @@ int ft1000_poll(void *dev_id)
 					break;
 				}
 			} else
-				DEBUG("FT1000:dpc:Invalid total length for SlowQ = %d\n", size);
+				pr_debug("Invalid total length for SlowQ = %d\n",
+					 size);
 			status = ft1000_write_register(dev,
-					FT1000_DB_DPRAM_RX,
-					FT1000_REG_DOORBELL);
+						       FT1000_DB_DPRAM_RX,
+						       FT1000_REG_DOORBELL);
 		} else if (tempword & FT1000_DSP_ASIC_RESET) {
 			/* Let's reset the ASIC from the Host side as well */
 			status = ft1000_write_register(dev, ASIC_RESET_BIT,
-					FT1000_REG_RESET);
+						       FT1000_REG_RESET);
 			status = ft1000_read_register(dev, &tempword,
-					FT1000_REG_RESET);
+						      FT1000_REG_RESET);
 			i = 0;
 			while (tempword & ASIC_RESET_BIT) {
 				status = ft1000_read_register(dev, &tempword,
-						FT1000_REG_RESET);
+							      FT1000_REG_RESET);
 				usleep_range(9000, 11000);
 				i++;
 				if (i == 100)
 					break;
 			}
 			if (i == 100) {
-				DEBUG("Unable to reset ASIC\n");
+				pr_debug("Unable to reset ASIC\n");
 				return 0;
 			}
 			usleep_range(9000, 11000);
 			/* Program WMARK register */
 			status = ft1000_write_register(dev, 0x600,
-					FT1000_REG_MAG_WATERMARK);
+						       FT1000_REG_MAG_WATERMARK);
 			/* clear ASIC reset doorbell */
 			status = ft1000_write_register(dev,
-					FT1000_DSP_ASIC_RESET,
-					FT1000_REG_DOORBELL);
+						       FT1000_DSP_ASIC_RESET,
+						       FT1000_REG_DOORBELL);
 			usleep_range(9000, 11000);
 		} else if (tempword & FT1000_ASIC_RESET_REQ) {
-			DEBUG("ft1000_poll: FT1000_REG_DOORBELL message type:  FT1000_ASIC_RESET_REQ\n");
+			pr_debug("FT1000_REG_DOORBELL message type: FT1000_ASIC_RESET_REQ\n");
 			/* clear ASIC reset request from DSP */
 			status = ft1000_write_register(dev,
-					FT1000_ASIC_RESET_REQ,
-					FT1000_REG_DOORBELL);
+						       FT1000_ASIC_RESET_REQ,
+						       FT1000_REG_DOORBELL);
 			status = ft1000_write_register(dev, HOST_INTF_BE,
-					FT1000_REG_SUP_CTRL);
+						       FT1000_REG_SUP_CTRL);
 			/* copy dsp session record from Adapter block */
 			status = ft1000_write_dpram32(dev, 0,
-					(u8 *)&info->DSPSess.Rec[0], 1024);
+						      (u8 *)&info->DSPSess.Rec[0], 1024);
 			status = ft1000_write_register(dev, 0x600,
-					FT1000_REG_MAG_WATERMARK);
+						       FT1000_REG_MAG_WATERMARK);
 			/* ring doorbell to tell DSP that
 			 * ASIC is out of reset
 			 * */
 			status = ft1000_write_register(dev,
-					FT1000_ASIC_RESET_DSP,
-					FT1000_REG_DOORBELL);
+						       FT1000_ASIC_RESET_DSP,
+						       FT1000_REG_DOORBELL);
 		} else if (tempword & FT1000_DB_COND_RESET) {
-			DEBUG("ft1000_poll: FT1000_REG_DOORBELL message type:  FT1000_DB_COND_RESET\n");
+			pr_debug("FT1000_REG_DOORBELL message type: FT1000_DB_COND_RESET\n");
 			if (!dev->fAppMsgPend) {
 				/* Reset ASIC and DSP */
 				status = ft1000_read_dpram16(dev,
-						FT1000_MAG_DSP_TIMER0,
-						(u8 *)&(info->DSP_TIME[0]),
-						FT1000_MAG_DSP_TIMER0_INDX);
+							     FT1000_MAG_DSP_TIMER0,
+							     (u8 *)&(info->DSP_TIME[0]),
+							     FT1000_MAG_DSP_TIMER0_INDX);
 				status = ft1000_read_dpram16(dev,
-						FT1000_MAG_DSP_TIMER1,
-						(u8 *)&(info->DSP_TIME[1]),
-						FT1000_MAG_DSP_TIMER1_INDX);
+							     FT1000_MAG_DSP_TIMER1,
+							     (u8 *)&(info->DSP_TIME[1]),
+							     FT1000_MAG_DSP_TIMER1_INDX);
 				status = ft1000_read_dpram16(dev,
-						FT1000_MAG_DSP_TIMER2,
-						(u8 *)&(info->DSP_TIME[2]),
-						FT1000_MAG_DSP_TIMER2_INDX);
+							     FT1000_MAG_DSP_TIMER2,
+							     (u8 *)&(info->DSP_TIME[2]),
+							     FT1000_MAG_DSP_TIMER2_INDX);
 				status = ft1000_read_dpram16(dev,
-						FT1000_MAG_DSP_TIMER3,
-						(u8 *)&(info->DSP_TIME[3]),
-						FT1000_MAG_DSP_TIMER3_INDX);
+							     FT1000_MAG_DSP_TIMER3,
+							     (u8 *)&(info->DSP_TIME[3]),
+							     FT1000_MAG_DSP_TIMER3_INDX);
 				info->CardReady = 0;
 				info->DrvErrNum = DSP_CONDRESET_INFO;
-				DEBUG("ft1000_hw:DSP conditional reset requested\n");
+				pr_debug("DSP conditional reset requested\n");
 				info->ft1000_reset(dev->net);
 			} else {
 				dev->fProvComplete = false;
 				dev->fCondResetPend = true;
 			}
 			ft1000_write_register(dev, FT1000_DB_COND_RESET,
-					FT1000_REG_DOORBELL);
+					      FT1000_REG_DOORBELL);
 		}
 	}
 	return 0;
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_ioctl.h b/drivers/staging/ft1000/ft1000-usb/ft1000_ioctl.h
index cb644a58d9f3..e9472bebda0b 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_ioctl.h
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_ioctl.h
@@ -1,30 +1,30 @@
 /*
-*---------------------------------------------------------------------------
-* FT1000 driver for Flarion Flash OFDM NIC Device
-*
-* Copyright (C) 2002 Flarion Technologies, All rights reserved.
-*
-* This program is free software; you can redistribute it and/or modify it
-* under the terms of the GNU General Public License as published by the Free
-* Software Foundation; either version 2 of the License, or (at your option) any
-* later version. This program is distributed in the hope that it will be useful,
-* but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
-* or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
-* more details. You should have received a copy of the GNU General Public
-* License along with this program; if not, write to the
-* Free Software Foundation, Inc., 59 Temple Place -
-* Suite 330, Boston, MA 02111-1307, USA.
-*---------------------------------------------------------------------------
-*
-* File:         ft1000_ioctl.h
-*
-* Description:    Common structures and defines relating to IOCTL
-*
-* History:
-* 11/5/02    Whc                Created.
-*
-*---------------------------------------------------------------------------
-*/
+ *---------------------------------------------------------------------------
+ * FT1000 driver for Flarion Flash OFDM NIC Device
+ *
+ * Copyright (C) 2002 Flarion Technologies, All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option) any
+ * later version. This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details. You should have received a copy of the GNU General Public
+ * License along with this program; if not, write to the
+ * Free Software Foundation, Inc., 59 Temple Place -
+ * Suite 330, Boston, MA 02111-1307, USA.
+ *---------------------------------------------------------------------------
+ *
+ * File:         ft1000_ioctl.h
+ *
+ * Description:    Common structures and defines relating to IOCTL
+ *
+ * History:
+ * 11/5/02    Whc                Created.
+ *
+ *---------------------------------------------------------------------------
+ */
 #ifndef _FT1000IOCTLH_
 #define _FT1000IOCTLH_
 
@@ -94,8 +94,8 @@ struct IOCTL_DPRAM_COMMAND {
 } __packed;
 
 /*
-* Custom IOCTL command codes
-*/
+ * Custom IOCTL command codes
+ */
 #define FT1000_MAGIC_CODE      'F'
 
 #define IOCTL_REGISTER_CMD	0
@@ -106,8 +106,8 @@ struct IOCTL_DPRAM_COMMAND {
 #define IOCTL_CONNECT		10
 #define IOCTL_DISCONNECT	11
 
-#define IOCTL_FT1000_GET_DSP_STAT _IOR(FT1000_MAGIC_CODE,      \
-				       IOCTL_GET_DSP_STAT_CMD, \
+#define IOCTL_FT1000_GET_DSP_STAT _IOR(FT1000_MAGIC_CODE,		\
+				       IOCTL_GET_DSP_STAT_CMD,		\
 				       struct IOCTL_GET_DSP_STAT)
 #define IOCTL_FT1000_GET_VER _IOR(FT1000_MAGIC_CODE, IOCTL_GET_VER_CMD, \
 				  struct IOCTL_GET_VER)
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
index 39be30c0eedf..a6b55f42c07c 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
@@ -7,6 +7,9 @@
  * $Id:
  *====================================================
  */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/usb.h>
@@ -45,7 +48,7 @@ static int ft1000_poll_thread(void *arg)
 		if (!gPollingfailed) {
 			ret = ft1000_poll(arg);
 			if (ret != 0) {
-				DEBUG("ft1000_poll_thread: polling failed\n");
+				pr_debug("polling failed\n");
 				gPollingfailed = true;
 			}
 		}
@@ -71,9 +74,8 @@ static int ft1000_probe(struct usb_interface *interface,
 		return -ENOMEM;
 
 	dev = interface_to_usbdev(interface);
-	DEBUG("ft1000_probe: usb device descriptor info:\n");
-	DEBUG("ft1000_probe: number of configuration is %d\n",
-	      dev->descriptor.bNumConfigurations);
+	pr_debug("usb device descriptor info - number of configuration is %d\n",
+		 dev->descriptor.bNumConfigurations);
 
 	ft1000dev->dev = dev;
 	ft1000dev->status = 0;
@@ -85,60 +87,56 @@ static int ft1000_probe(struct usb_interface *interface,
 		goto err_fw;
 	}
 
-	DEBUG("ft1000_probe is called\n");
 	numaltsetting = interface->num_altsetting;
-	DEBUG("ft1000_probe: number of alt settings is :%d\n", numaltsetting);
+	pr_debug("number of alt settings is: %d\n", numaltsetting);
 	iface_desc = interface->cur_altsetting;
-	DEBUG("ft1000_probe: number of endpoints is %d\n",
-	      iface_desc->desc.bNumEndpoints);
-	DEBUG("ft1000_probe: descriptor type is %d\n",
-	      iface_desc->desc.bDescriptorType);
-	DEBUG("ft1000_probe: interface number is %d\n",
-	      iface_desc->desc.bInterfaceNumber);
-	DEBUG("ft1000_probe: alternatesetting is %d\n",
-	      iface_desc->desc.bAlternateSetting);
-	DEBUG("ft1000_probe: interface class is %d\n",
-	      iface_desc->desc.bInterfaceClass);
-	DEBUG("ft1000_probe: control endpoint info:\n");
-	DEBUG("ft1000_probe: descriptor0 type -- %d\n",
-	      iface_desc->endpoint[0].desc.bmAttributes);
-	DEBUG("ft1000_probe: descriptor1 type -- %d\n",
-	      iface_desc->endpoint[1].desc.bmAttributes);
-	DEBUG("ft1000_probe: descriptor2 type -- %d\n",
-	      iface_desc->endpoint[2].desc.bmAttributes);
+	pr_debug("number of endpoints is: %d\n",
+		 iface_desc->desc.bNumEndpoints);
+	pr_debug("descriptor type is: %d\n", iface_desc->desc.bDescriptorType);
+	pr_debug("interface number is: %d\n",
+		 iface_desc->desc.bInterfaceNumber);
+	pr_debug("alternatesetting is: %d\n",
+		 iface_desc->desc.bAlternateSetting);
+	pr_debug("interface class is: %d\n", iface_desc->desc.bInterfaceClass);
+	pr_debug("control endpoint info:\n");
+	pr_debug("descriptor0 type -- %d\n",
+		 iface_desc->endpoint[0].desc.bmAttributes);
+	pr_debug("descriptor1 type -- %d\n",
+		 iface_desc->endpoint[1].desc.bmAttributes);
+	pr_debug("descriptor2 type -- %d\n",
+		 iface_desc->endpoint[2].desc.bmAttributes);
 
 	for (i = 0; i < iface_desc->desc.bNumEndpoints; i++) {
 		endpoint =
-		    (struct usb_endpoint_descriptor *)&iface_desc->
-		    endpoint[i].desc;
-		DEBUG("endpoint %d\n", i);
-		DEBUG("bEndpointAddress=%x, bmAttributes=%x\n",
-		      endpoint->bEndpointAddress, endpoint->bmAttributes);
+			(struct usb_endpoint_descriptor *)&iface_desc->
+			endpoint[i].desc;
+		pr_debug("endpoint %d\n", i);
+		pr_debug("bEndpointAddress=%x, bmAttributes=%x\n",
+			 endpoint->bEndpointAddress, endpoint->bmAttributes);
 		if ((endpoint->bEndpointAddress & USB_DIR_IN)
 		    && ((endpoint->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) ==
 			USB_ENDPOINT_XFER_BULK)) {
 			ft1000dev->bulk_in_endpointAddr =
-			    endpoint->bEndpointAddress;
-			DEBUG("ft1000_probe: in: %d\n",
-			      endpoint->bEndpointAddress);
+				endpoint->bEndpointAddress;
+			pr_debug("in: %d\n", endpoint->bEndpointAddress);
 		}
 
 		if (!(endpoint->bEndpointAddress & USB_DIR_IN)
 		    && ((endpoint->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) ==
 			USB_ENDPOINT_XFER_BULK)) {
 			ft1000dev->bulk_out_endpointAddr =
-			    endpoint->bEndpointAddress;
-			DEBUG("ft1000_probe: out: %d\n",
-			      endpoint->bEndpointAddress);
+				endpoint->bEndpointAddress;
+			pr_debug("out: %d\n", endpoint->bEndpointAddress);
 		}
 	}
 
-	DEBUG("bulk_in=%d, bulk_out=%d\n", ft1000dev->bulk_in_endpointAddr,
-	      ft1000dev->bulk_out_endpointAddr);
+	pr_debug("bulk_in=%d, bulk_out=%d\n",
+		 ft1000dev->bulk_in_endpointAddr,
+		 ft1000dev->bulk_out_endpointAddr);
 
 	ret = request_firmware(&dsp_fw, "ft3000.img", &dev->dev);
 	if (ret < 0) {
-		pr_err("Error request_firmware().\n");
+		pr_err("Error request_firmware()\n");
 		goto err_fw;
 	}
 
@@ -155,7 +153,7 @@ static int ft1000_probe(struct usb_interface *interface,
 	FileLength = dsp_fw->size;
 	release_firmware(dsp_fw);
 
-	DEBUG("ft1000_probe: start downloading dsp image...\n");
+	pr_debug("start downloading dsp image...\n");
 
 	ret = init_ft1000_netdev(ft1000dev);
 	if (ret)
@@ -163,7 +161,7 @@ static int ft1000_probe(struct usb_interface *interface,
 
 	pft1000info = netdev_priv(ft1000dev->net);
 
-	DEBUG("In probe: pft1000info=%p\n", pft1000info);
+	pr_debug("pft1000info=%p\n", pft1000info);
 	ret = dsp_reload(ft1000dev);
 	if (ret) {
 		pr_err("Problem with DSP image loading\n");
@@ -172,7 +170,7 @@ static int ft1000_probe(struct usb_interface *interface,
 
 	gPollingfailed = false;
 	ft1000dev->pPollThread =
-	    kthread_run(ft1000_poll_thread, ft1000dev, "ft1000_poll");
+		kthread_run(ft1000_poll_thread, ft1000dev, "ft1000_poll");
 
 	if (IS_ERR(ft1000dev->pPollThread)) {
 		ret = PTR_ERR(ft1000dev->pPollThread);
@@ -187,10 +185,10 @@ static int ft1000_probe(struct usb_interface *interface,
 			goto err_thread;
 		}
 		msleep(100);
-		DEBUG("ft1000_probe::Waiting for Card Ready\n");
+		pr_debug("Waiting for Card Ready\n");
 	}
 
-	DEBUG("ft1000_probe::Card Ready!!!! Registering network device\n");
+	pr_debug("Card Ready!!!! Registering network device\n");
 
 	ret = reg_ft1000_netdev(ft1000dev, interface);
 	if (ret)
@@ -216,24 +214,21 @@ static void ft1000_disconnect(struct usb_interface *interface)
 	struct ft1000_info *pft1000info;
 	struct ft1000_usb *ft1000dev;
 
-	DEBUG("ft1000_disconnect is called\n");
-
-	pft1000info = (struct ft1000_info *) usb_get_intfdata(interface);
-	DEBUG("In disconnect pft1000info=%p\n", pft1000info);
+	pft1000info = (struct ft1000_info *)usb_get_intfdata(interface);
+	pr_debug("In disconnect pft1000info=%p\n", pft1000info);
 
 	if (pft1000info) {
 		ft1000dev = pft1000info->priv;
 		if (ft1000dev->pPollThread)
 			kthread_stop(ft1000dev->pPollThread);
 
-		DEBUG("ft1000_disconnect: threads are terminated\n");
+		pr_debug("threads are terminated\n");
 
 		if (ft1000dev->net) {
-			DEBUG("ft1000_disconnect: destroy char driver\n");
+			pr_debug("destroy char driver\n");
 			ft1000_destroy_dev(ft1000dev->net);
 			unregister_netdev(ft1000dev->net);
-			DEBUG
-			    ("ft1000_disconnect: network device unregistered\n");
+			pr_debug("network device unregistered\n");
 			free_netdev(ft1000dev->net);
 
 		}
@@ -241,7 +236,7 @@ static void ft1000_disconnect(struct usb_interface *interface)
 		usb_free_urb(ft1000dev->rx_urb);
 		usb_free_urb(ft1000dev->tx_urb);
 
-		DEBUG("ft1000_disconnect: urb freed\n");
+		pr_debug("urb freed\n");
 
 		kfree(ft1000dev);
 	}
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h
index 8f7ccae57f31..fea60d5651a7 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h
@@ -28,8 +28,6 @@ struct app_info_block {
 					*/
 } __packed;
 
-#define DEBUG(args...) pr_info(args)
-
 #define FALSE           0
 #define TRUE            1
 
@@ -137,7 +135,7 @@ extern spinlock_t free_buff_lock;
 int ft1000_create_dev(struct ft1000_usb *dev);
 void ft1000_destroy_dev(struct net_device *dev);
 extern int card_send_command(struct ft1000_usb *ft1000dev,
-			      void *ptempbuffer, int size);
+			     void *ptempbuffer, int size);
 
 struct dpram_blk *ft1000_get_buffer(struct list_head *bufflist);
 void ft1000_free_buffer(struct dpram_blk *pdpram_blk, struct list_head *plist);
diff --git a/drivers/staging/fwserial/fwserial.c b/drivers/staging/fwserial/fwserial.c
index af0c3878358c..73deae3cd9eb 100644
--- a/drivers/staging/fwserial/fwserial.c
+++ b/drivers/staging/fwserial/fwserial.c
@@ -278,7 +278,6 @@ static void fwtty_send_txn_async(struct fwtty_peer *peer,
 			len, fwtty_common_callback, txn);
 }
 
-
 static void __fwtty_restart_tx(struct fwtty_port *port)
 {
 	int len, avail;
@@ -508,7 +507,6 @@ static void fwtty_do_hangup(struct work_struct *work)
 	tty_kref_put(tty);
 }
 
-
 static void fwtty_emit_breaks(struct work_struct *work)
 {
 	struct fwtty_port *port = to_port(to_delayed_work(work), emit_breaks);
@@ -1622,7 +1620,6 @@ static inline int mgmt_pkt_expected_len(__be16 code)
 	case FWSC_VIRT_CABLE_PLUG_RSP:  /* | FWSC_RSP_OK */
 		return sizeof(pkt.hdr) + sizeof(pkt.plug_rsp);
 
-
 	case FWSC_VIRT_CABLE_UNPLUG:
 	case FWSC_VIRT_CABLE_UNPLUG_RSP:
 	case FWSC_VIRT_CABLE_PLUG_RSP | FWSC_RSP_NACK:
diff --git a/drivers/staging/gdm724x/gdm_lte.c b/drivers/staging/gdm724x/gdm_lte.c
index c657639f884b..73eede163820 100644
--- a/drivers/staging/gdm724x/gdm_lte.c
+++ b/drivers/staging/gdm724x/gdm_lte.c
@@ -626,7 +626,7 @@ static void gdm_lte_netif_rx(struct net_device *dev, char *buf,
 			void *addr = buf + sizeof(struct iphdr) +
 				sizeof(struct udphdr) +
 				offsetof(struct dhcp_packet, chaddr);
-			memcpy(nic->dest_mac_addr, addr, ETH_ALEN);
+			ether_addr_copy(nic->dest_mac_addr, addr);
 		}
 	}
 
@@ -639,7 +639,7 @@ static void gdm_lte_netif_rx(struct net_device *dev, char *buf,
 	}
 
 	/* Format the data so that it can be put to skb */
-	memcpy(mac_header_data, nic->dest_mac_addr, ETH_ALEN);
+	ether_addr_copy(mac_header_data, nic->dest_mac_addr);
 	memcpy(mac_header_data + ETH_ALEN, nic->src_mac_addr, ETH_ALEN);
 
 	vlan_eth.h_vlan_TCI = htons(nic->vlan_id);
@@ -842,9 +842,9 @@ static void form_mac_address(u8 *dev_addr, u8 *nic_src, u8 *nic_dest,
 {
 	/* Form the dev_addr */
 	if (!mac_address)
-		memcpy(dev_addr, gdm_lte_macaddr, ETH_ALEN);
+		ether_addr_copy(dev_addr, gdm_lte_macaddr);
 	else
-		memcpy(dev_addr, mac_address, ETH_ALEN);
+		ether_addr_copy(dev_addr, mac_address);
 
 	/* The last byte of the mac address
 	 * should be less than or equal to 0xFC
@@ -858,7 +858,7 @@ static void form_mac_address(u8 *dev_addr, u8 *nic_src, u8 *nic_dest,
 	memcpy(nic_src, dev_addr, 3);
 
 	/* Copy the nic_dest from dev_addr*/
-	memcpy(nic_dest, dev_addr, ETH_ALEN);
+	ether_addr_copy(nic_dest, dev_addr);
 }
 
 static void validate_mac_address(u8 *mac_address)
diff --git a/drivers/staging/gdm724x/gdm_mux.h b/drivers/staging/gdm724x/gdm_mux.h
index 0163b243d3e0..3d50383c6ced 100644
--- a/drivers/staging/gdm724x/gdm_mux.h
+++ b/drivers/staging/gdm724x/gdm_mux.h
@@ -35,10 +35,10 @@
 #define RETRY_TIMER 30 /* msec */
 
 struct mux_pkt_header {
-	unsigned int start_flag;
-	unsigned int seq_num;
-	unsigned int payload_size;
-	unsigned short packet_type;
+	__le32 start_flag;
+	__le32 seq_num;
+	__le32 payload_size;
+	__le16 packet_type;
 	unsigned char data[0];
 };
 
diff --git a/drivers/staging/gdm72xx/Kconfig b/drivers/staging/gdm72xx/Kconfig
index 5836503caa7b..bf11a7fbfc51 100644
--- a/drivers/staging/gdm72xx/Kconfig
+++ b/drivers/staging/gdm72xx/Kconfig
@@ -53,7 +53,7 @@ if WIMAX_GDM72XX_USB
 
 config WIMAX_GDM72XX_USB_PM
 	bool "Enable power management support"
-	depends on PM_RUNTIME
+	depends on PM
 	help
 	  Enable USB power management in order to reduce power consumption
 	  while the interface is not in use.
diff --git a/drivers/staging/gdm72xx/gdm_wimax.c b/drivers/staging/gdm72xx/gdm_wimax.c
index f5a3378c3951..9cab54bfa6f4 100644
--- a/drivers/staging/gdm72xx/gdm_wimax.c
+++ b/drivers/staging/gdm72xx/gdm_wimax.c
@@ -115,7 +115,7 @@ static void gdm_wimax_event_rcv(struct net_device *dev, u16 type, void *msg,
 {
 	struct nic *nic = netdev_priv(dev);
 
-	u8 *buf = (u8 *)msg;
+	u8 *buf = msg;
 	u16 hci_cmd =  (buf[0]<<8) | buf[1];
 	u16 hci_len = (buf[2]<<8) | buf[3];
 
@@ -605,10 +605,8 @@ static void gdm_wimax_netif_rx(struct net_device *dev, char *buf, int len)
 	int ret;
 
 	skb = dev_alloc_skb(len + 2);
-	if (!skb) {
-		netdev_err(dev, "%s: dev_alloc_skb failed!\n", __func__);
+	if (!skb)
 		return;
-	}
 	skb_reserve(skb, 2);
 
 	dev->stats.rx_packets++;
diff --git a/drivers/staging/gs_fpgaboot/gs_fpgaboot.c b/drivers/staging/gs_fpgaboot/gs_fpgaboot.c
index 6aa9d7c30139..6da72858d28c 100644
--- a/drivers/staging/gs_fpgaboot/gs_fpgaboot.c
+++ b/drivers/staging/gs_fpgaboot/gs_fpgaboot.c
@@ -46,27 +46,6 @@ static char	*file = "xlinx_fpga_firmware.bit";
 module_param(file, charp, S_IRUGO);
 MODULE_PARM_DESC(file, "Xilinx FPGA firmware file.");
 
-#ifdef DEBUG_FPGA
-static void datadump(char *msg, void *m, int n)
-{
-	int i;
-	unsigned char *c;
-
-	pr_info("=== %s ===\n", msg);
-
-	c = m;
-
-	for (i = 0; i < n; i++) {
-		if ((i&0xf) == 0)
-			pr_info(KERN_INFO "\n  0x%4x: ", i);
-
-		pr_info("%02X ", c[i]);
-	}
-
-	pr_info("\n");
-}
-#endif /* DEBUG_FPGA */
-
 static void read_bitstream(char *bitdata, char *buf, int *offset, int rdsize)
 {
 	memcpy(buf, bitdata + *offset, rdsize);
@@ -157,12 +136,10 @@ static void gs_print_header(struct fpgaimage *fimage)
 static void gs_read_bitstream(struct fpgaimage *fimage)
 {
 	char *bitdata;
-	int size;
 	int offset;
 
 	offset = 0;
 	bitdata = (char *)fimage->fw_entry->data;
-	size = fimage->fw_entry->size;
 
 	readmagic_bitstream(bitdata, &offset);
 	readinfo_bitstream(bitdata, fimage->filename, &offset);
@@ -195,15 +172,15 @@ static int gs_read_image(struct fpgaimage *fimage)
 	return 0;
 }
 
-static int gs_load_image(struct fpgaimage *fimage, char *file)
+static int gs_load_image(struct fpgaimage *fimage, char *fw_file)
 {
 	int err;
 
-	pr_info("load fpgaimage %s\n", file);
+	pr_info("load fpgaimage %s\n", fw_file);
 
-	err = request_firmware(&fimage->fw_entry, file, &firmware_pdev->dev);
+	err = request_firmware(&fimage->fw_entry, fw_file, &firmware_pdev->dev);
 	if (err != 0) {
-		pr_err("firmware %s is missing, cannot continue.\n", file);
+		pr_err("firmware %s is missing, cannot continue.\n", fw_file);
 		return err;
 	}
 
@@ -220,9 +197,9 @@ static int gs_download_image(struct fpgaimage *fimage, enum wbus bus_bytes)
 	size = fimage->lendata;
 
 #ifdef DEBUG_FPGA
-	datadump("bitfile sample", bitdata, 0x100);
+	print_hex_dump_bytes("bitfile sample: ", DUMP_PREFIX_OFFSET,
+			     bitdata, 0x100);
 #endif /* DEBUG_FPGA */
-
 	if (!xl_supported_prog_bus_width(bus_bytes)) {
 		pr_err("unsupported program bus width %d\n",
 				bus_bytes);
@@ -316,10 +293,8 @@ static int gs_fpgaboot(void)
 	struct fpgaimage	*fimage;
 
 	fimage = kmalloc(sizeof(struct fpgaimage), GFP_KERNEL);
-	if (fimage == NULL) {
-		pr_err("No memory is available\n");
-		goto err_out;
-	}
+	if (!fimage)
+		return -ENOMEM;
 
 	err = gs_load_image(fimage, file);
 	if (err) {
@@ -361,50 +336,44 @@ err_out2:
 err_out1:
 	kfree(fimage);
 
-err_out:
 	return -1;
 
 }
 
 static int __init gs_fpgaboot_init(void)
 {
-	int err, r;
-
-	r = -1;
+	int err;
 
 	pr_info("FPGA DOWNLOAD --->\n");
 
 	pr_info("FPGA image file name: %s\n", file);
 
 	err = init_driver();
-	if (err != 0) {
+	if (err) {
 		pr_err("FPGA DRIVER INIT FAIL!!\n");
-		return r;
+		return err;
 	}
 
 	err = xl_init_io();
 	if (err) {
 		pr_err("GPIO INIT FAIL!!\n");
-		r = -1;
 		goto errout;
 	}
 
 	err = gs_fpgaboot();
 	if (err) {
 		pr_err("FPGA DOWNLOAD FAIL!!\n");
-		r = -1;
 		goto errout;
 	}
 
 	pr_info("FPGA DOWNLOAD DONE <---\n");
 
-	r = 0;
-	return r;
+	return 0;
 
 errout:
 	finish_driver();
 
-	return r;
+	return err;
 }
 
 static void __exit gs_fpgaboot_exit(void)
diff --git a/drivers/staging/iio/Documentation/generic_buffer.c b/drivers/staging/iio/Documentation/generic_buffer.c
index 044ea196aa6f..de4647e2495e 100644
--- a/drivers/staging/iio/Documentation/generic_buffer.c
+++ b/drivers/staging/iio/Documentation/generic_buffer.c
@@ -47,6 +47,7 @@ int size_from_channelarray(struct iio_channel_info *channels, int num_channels)
 {
 	int bytes = 0;
 	int i = 0;
+
 	while (i < num_channels) {
 		if (bytes % channels[i].bytes == 0)
 			channels[i].location = bytes;
@@ -74,12 +75,14 @@ void print2byte(int input, struct iio_channel_info *info)
 	input = input >> info->shift;
 	if (info->is_signed) {
 		int16_t val = input;
+
 		val &= (1 << info->bits_used) - 1;
 		val = (int16_t)(val << (16 - info->bits_used)) >>
 			(16 - info->bits_used);
 		printf("%05f ", ((float)val + info->offset)*info->scale);
 	} else {
 		uint16_t val = input;
+
 		val &= (1 << info->bits_used) - 1;
 		printf("%05f ", ((float)val + info->offset)*info->scale);
 	}
@@ -97,6 +100,7 @@ void process_scan(char *data,
 		  int num_channels)
 {
 	int k;
+
 	for (k = 0; k < num_channels; k++)
 		switch (channels[k].bytes) {
 			/* only a few cases implemented so far */
@@ -158,11 +162,12 @@ int main(int argc, char **argv)
 	char *buffer_access;
 	int scan_size;
 	int noevents = 0;
+	int notrigger = 0;
 	char *dummy;
 
 	struct iio_channel_info *channels;
 
-	while ((c = getopt(argc, argv, "l:w:c:et:n:")) != -1) {
+	while ((c = getopt(argc, argv, "l:w:c:et:n:g")) != -1) {
 		switch (c) {
 		case 'n':
 			device_name = optarg;
@@ -183,6 +188,9 @@ int main(int argc, char **argv)
 		case 'l':
 			buf_len = strtoul(optarg, &dummy, 10);
 			break;
+		case 'g':
+			notrigger = 1;
+			break;
 		case '?':
 			return -1;
 		}
@@ -201,28 +209,32 @@ int main(int argc, char **argv)
 	printf("iio device number being used is %d\n", dev_num);
 
 	asprintf(&dev_dir_name, "%siio:device%d", iio_dir, dev_num);
-	if (trigger_name == NULL) {
-		/*
-		 * Build the trigger name. If it is device associated its
-		 * name is <device_name>_dev[n] where n matches the device
-		 * number found above
-		 */
-		ret = asprintf(&trigger_name,
-			       "%s-dev%d", device_name, dev_num);
-		if (ret < 0) {
-			ret = -ENOMEM;
-			goto error_ret;
+
+	if (!notrigger) {
+		if (trigger_name == NULL) {
+			/*
+			 * Build the trigger name. If it is device associated
+			 * its name is <device_name>_dev[n] where n matches
+			 * the device number found above.
+			 */
+			ret = asprintf(&trigger_name,
+				       "%s-dev%d", device_name, dev_num);
+			if (ret < 0) {
+				ret = -ENOMEM;
+				goto error_ret;
+			}
 		}
-	}
 
-	/* Verify the trigger exists */
-	trig_num = find_type_by_name(trigger_name, "trigger");
-	if (trig_num < 0) {
-		printf("Failed to find the trigger %s\n", trigger_name);
-		ret = -ENODEV;
-		goto error_free_triggername;
-	}
-	printf("iio trigger number being used is %d\n", trig_num);
+		/* Verify the trigger exists */
+		trig_num = find_type_by_name(trigger_name, "trigger");
+		if (trig_num < 0) {
+			printf("Failed to find the trigger %s\n", trigger_name);
+			ret = -ENODEV;
+			goto error_free_triggername;
+		}
+		printf("iio trigger number being used is %d\n", trig_num);
+	} else
+		printf("trigger-less mode selected\n");
 
 	/*
 	 * Parse the files in scan_elements to identify what channels are
@@ -246,14 +258,18 @@ int main(int argc, char **argv)
 		ret = -ENOMEM;
 		goto error_free_triggername;
 	}
-	printf("%s %s\n", dev_dir_name, trigger_name);
-	/* Set the device trigger to be the data ready trigger found above */
-	ret = write_sysfs_string_and_verify("trigger/current_trigger",
-					dev_dir_name,
-					trigger_name);
-	if (ret < 0) {
-		printf("Failed to write current_trigger file\n");
-		goto error_free_buf_dir_name;
+
+	if (!notrigger) {
+		printf("%s %s\n", dev_dir_name, trigger_name);
+		/* Set the device trigger to be the data ready trigger found
+		 * above */
+		ret = write_sysfs_string_and_verify("trigger/current_trigger",
+						    dev_dir_name,
+						    trigger_name);
+		if (ret < 0) {
+			printf("Failed to write current_trigger file\n");
+			goto error_free_buf_dir_name;
+		}
 	}
 
 	/* Setup ring buffer parameters */
@@ -323,9 +339,10 @@ int main(int argc, char **argv)
 	if (ret < 0)
 		goto error_close_buffer_access;
 
-	/* Disconnect the trigger - just write a dummy name. */
-	write_sysfs_string("trigger/current_trigger",
-			dev_dir_name, "NULL");
+	if (!notrigger)
+		/* Disconnect the trigger - just write a dummy name. */
+		write_sysfs_string("trigger/current_trigger",
+				   dev_dir_name, "NULL");
 
 error_close_buffer_access:
 	close(fp);
diff --git a/drivers/staging/iio/Documentation/iio_event_monitor.c b/drivers/staging/iio/Documentation/iio_event_monitor.c
index 569d6f8face5..940ed2399e73 100644
--- a/drivers/staging/iio/Documentation/iio_event_monitor.c
+++ b/drivers/staging/iio/Documentation/iio_event_monitor.c
@@ -69,16 +69,29 @@ static const char * const iio_modifier_names[] = {
 	[IIO_MOD_X] = "x",
 	[IIO_MOD_Y] = "y",
 	[IIO_MOD_Z] = "z",
+	[IIO_MOD_X_AND_Y] = "x&y",
+	[IIO_MOD_X_AND_Z] = "x&z",
+	[IIO_MOD_Y_AND_Z] = "y&z",
+	[IIO_MOD_X_AND_Y_AND_Z] = "x&y&z",
+	[IIO_MOD_X_OR_Y] = "x|y",
+	[IIO_MOD_X_OR_Z] = "x|z",
+	[IIO_MOD_Y_OR_Z] = "y|z",
+	[IIO_MOD_X_OR_Y_OR_Z] = "x|y|z",
 	[IIO_MOD_LIGHT_BOTH] = "both",
 	[IIO_MOD_LIGHT_IR] = "ir",
 	[IIO_MOD_ROOT_SUM_SQUARED_X_Y] = "sqrt(x^2+y^2)",
 	[IIO_MOD_SUM_SQUARED_X_Y_Z] = "x^2+y^2+z^2",
-	[IIO_MOD_LIGHT_BOTH] = "both",
-	[IIO_MOD_LIGHT_IR] = "ir",
 	[IIO_MOD_LIGHT_CLEAR] = "clear",
 	[IIO_MOD_LIGHT_RED] = "red",
 	[IIO_MOD_LIGHT_GREEN] = "green",
 	[IIO_MOD_LIGHT_BLUE] = "blue",
+	[IIO_MOD_QUATERNION] = "quaternion",
+	[IIO_MOD_TEMP_AMBIENT] = "ambient",
+	[IIO_MOD_TEMP_OBJECT] = "object",
+	[IIO_MOD_NORTH_MAGN] = "from_north_magnetic",
+	[IIO_MOD_NORTH_TRUE] = "from_north_true",
+	[IIO_MOD_NORTH_MAGN_TILT_COMP] = "from_north_magnetic_tilt_comp",
+	[IIO_MOD_NORTH_TRUE_TILT_COMP] = "from_north_true_tilt_comp",
 };
 
 static bool event_is_known(struct iio_event_data *event)
@@ -118,6 +131,14 @@ static bool event_is_known(struct iio_event_data *event)
 	case IIO_MOD_X:
 	case IIO_MOD_Y:
 	case IIO_MOD_Z:
+	case IIO_MOD_X_AND_Y:
+	case IIO_MOD_X_AND_Z:
+	case IIO_MOD_Y_AND_Z:
+	case IIO_MOD_X_AND_Y_AND_Z:
+	case IIO_MOD_X_OR_Y:
+	case IIO_MOD_X_OR_Z:
+	case IIO_MOD_Y_OR_Z:
+	case IIO_MOD_X_OR_Y_OR_Z:
 	case IIO_MOD_LIGHT_BOTH:
 	case IIO_MOD_LIGHT_IR:
 	case IIO_MOD_ROOT_SUM_SQUARED_X_Y:
@@ -126,6 +147,13 @@ static bool event_is_known(struct iio_event_data *event)
 	case IIO_MOD_LIGHT_RED:
 	case IIO_MOD_LIGHT_GREEN:
 	case IIO_MOD_LIGHT_BLUE:
+	case IIO_MOD_QUATERNION:
+	case IIO_MOD_TEMP_AMBIENT:
+	case IIO_MOD_TEMP_OBJECT:
+	case IIO_MOD_NORTH_MAGN:
+	case IIO_MOD_NORTH_TRUE:
+	case IIO_MOD_NORTH_MAGN_TILT_COMP:
+	case IIO_MOD_NORTH_TRUE_TILT_COMP:
 		break;
 	default:
 		return false;
diff --git a/drivers/staging/iio/Documentation/iio_utils.h b/drivers/staging/iio/Documentation/iio_utils.h
index 0973a092224a..568eff06f803 100644
--- a/drivers/staging/iio/Documentation/iio_utils.h
+++ b/drivers/staging/iio/Documentation/iio_utils.h
@@ -34,6 +34,7 @@ inline int iioutils_break_up_name(const char *full_name,
 	char *current;
 	char *w, *r;
 	char *working;
+
 	current = strdup(full_name);
 	working = strtok(current, "_\0");
 	w = working;
@@ -335,6 +336,7 @@ inline int build_channel_array(const char *device_dir,
 		if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
 			   "_en") == 0) {
 			int current_enabled = 0;
+
 			current = &(*ci_array)[count++];
 			ret = asprintf(&filename,
 				       "%s/%s", scan_el_dir, ent->d_name);
@@ -506,6 +508,7 @@ inline int _write_sysfs_int(char *filename, char *basedir, int val, int verify)
 	FILE *sysfsfp;
 	int test;
 	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+
 	if (temp == NULL)
 		return -ENOMEM;
 	sprintf(temp, "%s/%s", basedir, filename);
@@ -554,6 +557,7 @@ int _write_sysfs_string(char *filename, char *basedir, char *val, int verify)
 	int ret = 0;
 	FILE  *sysfsfp;
 	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+
 	if (temp == NULL) {
 		printf("Memory allocation failed\n");
 		return -ENOMEM;
@@ -614,6 +618,7 @@ int read_sysfs_posint(char *filename, char *basedir)
 	int ret;
 	FILE  *sysfsfp;
 	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+
 	if (temp == NULL) {
 		printf("Memory allocation failed");
 		return -ENOMEM;
@@ -636,6 +641,7 @@ int read_sysfs_float(char *filename, char *basedir, float *val)
 	int ret = 0;
 	FILE  *sysfsfp;
 	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+
 	if (temp == NULL) {
 		printf("Memory allocation failed");
 		return -ENOMEM;
@@ -658,6 +664,7 @@ int read_sysfs_string(const char *filename, const char *basedir, char *str)
 	int ret = 0;
 	FILE  *sysfsfp;
 	char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
+
 	if (temp == NULL) {
 		printf("Memory allocation failed");
 		return -ENOMEM;
diff --git a/drivers/staging/iio/Documentation/lsiio.c b/drivers/staging/iio/Documentation/lsiio.c
index 24ae9694eb41..98a0de098130 100644
--- a/drivers/staging/iio/Documentation/lsiio.c
+++ b/drivers/staging/iio/Documentation/lsiio.c
@@ -46,6 +46,7 @@ static int dump_channels(const char *dev_dir_name)
 {
 	DIR *dp;
 	const struct dirent *ent;
+
 	dp = opendir(dev_dir_name);
 	if (dp == NULL)
 		return -errno;
@@ -62,17 +63,17 @@ static int dump_one_device(const char *dev_dir_name)
 {
 	char name[IIO_MAX_NAME_LENGTH];
 	int dev_idx;
+	int retval;
 
-	sscanf(dev_dir_name + strlen(iio_dir) + strlen(type_device),
+	retval = sscanf(dev_dir_name + strlen(iio_dir) + strlen(type_device),
 			"%i", &dev_idx);
+	if (retval != 1)
+		return -EINVAL;
 	read_sysfs_string("name", dev_dir_name, name);
 	printf("Device %03d: %s\n", dev_idx, name);
 
-	if (verblevel >= VERBLEVEL_SENSORS) {
-		int ret = dump_channels(dev_dir_name);
-		if (ret)
-			return ret;
-	}
+	if (verblevel >= VERBLEVEL_SENSORS)
+		return dump_channels(dev_dir_name);
 	return 0;
 }
 
@@ -80,9 +81,12 @@ static int dump_one_trigger(const char *dev_dir_name)
 {
 	char name[IIO_MAX_NAME_LENGTH];
 	int dev_idx;
+	int retval;
 
-	sscanf(dev_dir_name + strlen(iio_dir) + strlen(type_trigger),
+	retval = sscanf(dev_dir_name + strlen(iio_dir) + strlen(type_trigger),
 			"%i", &dev_idx);
+	if (retval != 1)
+		return -EINVAL;
 	read_sysfs_string("name", dev_dir_name, name);
 	printf("Trigger %03d: %s\n", dev_idx, name);
 	return 0;
@@ -107,6 +111,7 @@ static void dump_devices(void)
 	while (ent = readdir(dp), ent != NULL) {
 		if (check_prefix(ent->d_name, type_device)) {
 			char *dev_dir_name;
+
 			asprintf(&dev_dir_name, "%s%s", iio_dir, ent->d_name);
 			dump_one_device(dev_dir_name);
 			free(dev_dir_name);
@@ -118,6 +123,7 @@ static void dump_devices(void)
 	while (ent = readdir(dp), ent != NULL) {
 		if (check_prefix(ent->d_name, type_trigger)) {
 			char *dev_dir_name;
+
 			asprintf(&dev_dir_name, "%s%s", iio_dir, ent->d_name);
 			dump_one_trigger(dev_dir_name);
 			free(dev_dir_name);
diff --git a/drivers/staging/iio/accel/Kconfig b/drivers/staging/iio/accel/Kconfig
index ad45dfbdf417..07b7ffa00ab5 100644
--- a/drivers/staging/iio/accel/Kconfig
+++ b/drivers/staging/iio/accel/Kconfig
@@ -9,53 +9,71 @@ config ADIS16201
 	select IIO_ADIS_LIB
 	select IIO_ADIS_LIB_BUFFER if IIO_BUFFER
 	help
-	  Say yes here to build support for Analog Devices adis16201 dual-axis
+	  Say Y here to build support for Analog Devices adis16201 dual-axis
 	  digital inclinometer and accelerometer.
 
+	  To compile this driver as a module, say M here: the module will
+	  be called adis16201.
+
 config ADIS16203
 	tristate "Analog Devices ADIS16203 Programmable 360 Degrees Inclinometer"
 	depends on SPI
 	select IIO_ADIS_LIB
 	select IIO_ADIS_LIB_BUFFER if IIO_BUFFER
 	help
-	  Say yes here to build support for Analog Devices adis16203 Programmable
+	  Say Y here to build support for Analog Devices adis16203 Programmable
 	  360 Degrees Inclinometer.
 
+	  To compile this driver as a module, say M here: the module will be
+	  called adis16203.
+
 config ADIS16204
 	tristate "Analog Devices ADIS16204 Programmable High-g Digital Impact Sensor and Recorder"
 	depends on SPI
 	select IIO_ADIS_LIB
 	select IIO_ADIS_LIB_BUFFER if IIO_BUFFER
 	help
-	  Say yes here to build support for Analog Devices adis16204 Programmable
+	  Say Y here to build support for Analog Devices adis16204 Programmable
 	  High-g Digital Impact Sensor and Recorder.
 
+	  To compile this driver as a module, say M here: the module will be
+	  called adis16204.
+
 config ADIS16209
 	tristate "Analog Devices ADIS16209 Dual-Axis Digital Inclinometer and Accelerometer"
 	depends on SPI
 	select IIO_ADIS_LIB
 	select IIO_ADIS_LIB_BUFFER if IIO_BUFFER
 	help
-	  Say yes here to build support for Analog Devices adis16209 dual-axis digital inclinometer
+	  Say Y here to build support for Analog Devices adis16209 dual-axis digital inclinometer
 	  and accelerometer.
 
+	  To compile this driver as a module, say M here: the module will be
+	  called adis16209.
+
 config ADIS16220
 	tristate "Analog Devices ADIS16220 Programmable Digital Vibration Sensor"
 	depends on SPI
 	select IIO_ADIS_LIB
 	help
-	  Say yes here to build support for Analog Devices adis16220 programmable
+	  Say Y here to build support for Analog Devices adis16220 programmable
 	  digital vibration sensor.
 
+	  To compile this driver as a module, say M here: the module will be
+	  called adis16220.
+
 config ADIS16240
 	tristate "Analog Devices ADIS16240 Programmable Impact Sensor and Recorder"
 	depends on SPI
 	select IIO_ADIS_LIB
 	select IIO_ADIS_LIB_BUFFER if IIO_BUFFER
 	help
-	  Say yes here to build support for Analog Devices adis16240 programmable
+	  Say Y here to build support for Analog Devices adis16240 programmable
 	  impact Sensor and recorder.
 
+	  To compile this driver as a module, say M here: the module will be
+	  called adis16240.
+
 config LIS3L02DQ
 	tristate "ST Microelectronics LIS3L02DQ Accelerometer Driver"
 	depends on SPI
@@ -63,16 +81,21 @@ config LIS3L02DQ
 	depends on !IIO_BUFFER || IIO_KFIFO_BUF
 	depends on GPIOLIB
 	help
-	  Say yes here to build SPI support for the ST microelectronics
+	  Say Y here to build SPI support for the ST microelectronics
 	  accelerometer. The driver supplies direct access via sysfs files
 	  and an event interface via a character device.
 
+	  To compile this driver as a module, say M here: the module will be
+	  called lis3l02dq.
+
 config SCA3000
 	depends on IIO_BUFFER
 	depends on SPI
 	tristate "VTI SCA3000 series accelerometers"
 	help
-	  Say yes here to build support for the VTI SCA3000 series of SPI
+	  Say Y here to build support for the VTI SCA3000 series of SPI
 	  accelerometers. These devices use a hardware ring buffer.
 
+	  To compile this driver as a module, say M here: the module will be
+	  called sca3000.
 endmenu
diff --git a/drivers/staging/iio/accel/lis3l02dq_ring.c b/drivers/staging/iio/accel/lis3l02dq_ring.c
index 61f94221b8b7..9efc77b0ebdd 100644
--- a/drivers/staging/iio/accel/lis3l02dq_ring.c
+++ b/drivers/staging/iio/accel/lis3l02dq_ring.c
@@ -35,6 +35,7 @@ irqreturn_t lis3l02dq_data_rdy_trig_poll(int irq, void *private)
 		iio_trigger_poll(st->trig);
 		return IRQ_HANDLED;
 	}
+
 	return IRQ_WAKE_THREAD;
 }
 
diff --git a/drivers/staging/iio/adc/ad7192.c b/drivers/staging/iio/adc/ad7192.c
index c110a255d4e8..f6526aa22e8a 100644
--- a/drivers/staging/iio/adc/ad7192.c
+++ b/drivers/staging/iio/adc/ad7192.c
@@ -223,7 +223,8 @@ static int ad7192_setup(struct ad7192_state *st,
 	id &= AD7192_ID_MASK;
 
 	if (id != st->devid)
-		dev_warn(&st->sd.spi->dev, "device ID query failed (0x%X)\n", id);
+		dev_warn(&st->sd.spi->dev, "device ID query failed (0x%X)\n",
+			id);
 
 	switch (pdata->clock_source_sel) {
 	case AD7192_CLK_EXT_MCLK1_2:
diff --git a/drivers/staging/iio/adc/ad7280a.c b/drivers/staging/iio/adc/ad7280a.c
index d215edf66af2..4d4870787eed 100644
--- a/drivers/staging/iio/adc/ad7280a.c
+++ b/drivers/staging/iio/adc/ad7280a.c
@@ -188,7 +188,7 @@ static void ad7280_delay(struct ad7280_state *st)
 	if (st->readback_delay_us < 50)
 		udelay(st->readback_delay_us);
 	else
-		msleep(1);
+		usleep_range(250, 500);
 }
 
 static int __ad7280_read32(struct ad7280_state *st, unsigned *val)
diff --git a/drivers/staging/iio/adc/ad7606_par.c b/drivers/staging/iio/adc/ad7606_par.c
index 7511839ba94e..9e24b4d4455f 100644
--- a/drivers/staging/iio/adc/ad7606_par.c
+++ b/drivers/staging/iio/adc/ad7606_par.c
@@ -141,7 +141,6 @@ static struct platform_driver ad7606_driver = {
 	.id_table = ad7606_driver_ids,
 	.driver = {
 		.name	 = "ad7606",
-		.owner	= THIS_MODULE,
 		.pm    = AD7606_PAR_PM_OPS,
 	},
 };
diff --git a/drivers/staging/iio/adc/ad7606_spi.c b/drivers/staging/iio/adc/ad7606_spi.c
index 6a8ecd73a1a7..7303983e64a7 100644
--- a/drivers/staging/iio/adc/ad7606_spi.c
+++ b/drivers/staging/iio/adc/ad7606_spi.c
@@ -23,7 +23,7 @@ static int ad7606_spi_read_block(struct device *dev,
 	int i, ret;
 	unsigned short *data = buf;
 
-	ret = spi_read(spi, (u8 *)buf, count * 2);
+	ret = spi_read(spi, buf, count * 2);
 	if (ret < 0) {
 		dev_err(&spi->dev, "SPI read error\n");
 		return ret;
diff --git a/drivers/staging/iio/adc/ad7816.c b/drivers/staging/iio/adc/ad7816.c
index 734a7e4886a0..48b1c3740030 100644
--- a/drivers/staging/iio/adc/ad7816.c
+++ b/drivers/staging/iio/adc/ad7816.c
@@ -152,7 +152,8 @@ static ssize_t ad7816_show_available_modes(struct device *dev,
 	return sprintf(buf, "full\npower-save\n");
 }
 
-static IIO_DEVICE_ATTR(available_modes, S_IRUGO, ad7816_show_available_modes, NULL, 0);
+static IIO_DEVICE_ATTR(available_modes, S_IRUGO, ad7816_show_available_modes,
+			NULL, 0);
 
 static ssize_t ad7816_show_channel(struct device *dev,
 		struct device_attribute *attr,
diff --git a/drivers/staging/iio/adc/lpc32xx_adc.c b/drivers/staging/iio/adc/lpc32xx_adc.c
index a876ce755351..5331c442fcfc 100644
--- a/drivers/staging/iio/adc/lpc32xx_adc.c
+++ b/drivers/staging/iio/adc/lpc32xx_adc.c
@@ -116,7 +116,7 @@ static const struct iio_chan_spec lpc32xx_adc_iio_channels[] = {
 
 static irqreturn_t lpc32xx_adc_isr(int irq, void *dev_id)
 {
-	struct lpc32xx_adc_info *info = (struct lpc32xx_adc_info *) dev_id;
+	struct lpc32xx_adc_info *info = dev_id;
 
 	/* Read value and clear irq */
 	info->value = __raw_readl(LPC32XX_ADC_VALUE(info->adc_base)) &
@@ -204,7 +204,6 @@ static struct platform_driver lpc32xx_adc_driver = {
 	.probe		= lpc32xx_adc_probe,
 	.driver		= {
 		.name	= MOD_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(lpc32xx_adc_match),
 	},
 };
diff --git a/drivers/staging/iio/adc/mxs-lradc.c b/drivers/staging/iio/adc/mxs-lradc.c
index 2a29b9baec0d..f053535385bf 100644
--- a/drivers/staging/iio/adc/mxs-lradc.c
+++ b/drivers/staging/iio/adc/mxs-lradc.c
@@ -455,7 +455,8 @@ static void mxs_lradc_setup_ts_channel(struct mxs_lradc *lradc, unsigned ch)
 	 * SoC's delay unit and start the conversion later
 	 * and automatically.
 	 */
-	mxs_lradc_reg_wrt(lradc, LRADC_DELAY_TRIGGER(0) | /* don't trigger ADC */
+	mxs_lradc_reg_wrt(lradc,
+		LRADC_DELAY_TRIGGER(0) | /* don't trigger ADC */
 		LRADC_DELAY_TRIGGER_DELAYS(1 << 3) | /* trigger DELAY unit#3 */
 		LRADC_DELAY_KICK |
 		LRADC_DELAY_DELAY(lradc->settling_delay),
@@ -513,7 +514,8 @@ static void mxs_lradc_setup_ts_pressure(struct mxs_lradc *lradc, unsigned ch1,
 	 * SoC's delay unit and start the conversion later
 	 * and automatically.
 	 */
-	mxs_lradc_reg_wrt(lradc, LRADC_DELAY_TRIGGER(0) | /* don't trigger ADC */
+	mxs_lradc_reg_wrt(lradc,
+		LRADC_DELAY_TRIGGER(0) | /* don't trigger ADC */
 		LRADC_DELAY_TRIGGER_DELAYS(1 << 3) | /* trigger DELAY unit#3 */
 		LRADC_DELAY_KICK |
 		LRADC_DELAY_DELAY(lradc->settling_delay), LRADC_DELAY(2));
@@ -1668,7 +1670,6 @@ static int mxs_lradc_remove(struct platform_device *pdev)
 static struct platform_driver mxs_lradc_driver = {
 	.driver	= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = mxs_lradc_dt_ids,
 	},
 	.probe	= mxs_lradc_probe,
diff --git a/drivers/staging/iio/adc/spear_adc.c b/drivers/staging/iio/adc/spear_adc.c
index c5492ba50751..8ad71691fc09 100644
--- a/drivers/staging/iio/adc/spear_adc.c
+++ b/drivers/staging/iio/adc/spear_adc.c
@@ -98,7 +98,7 @@ static void spear_adc_set_clk(struct spear_adc_state *st, u32 val)
 	u32 clk_high, clk_low, count;
 	u32 apb_clk = clk_get_rate(st->clk);
 
-	count = (apb_clk + val - 1) / val;
+	count = DIV_ROUND_UP(apb_clk, val);
 	clk_low = count / 2;
 	clk_high = count - clk_low;
 	st->current_clk = apb_clk / count;
@@ -226,7 +226,7 @@ static const struct iio_chan_spec spear_adc_iio_channels[] = {
 
 static irqreturn_t spear_adc_isr(int irq, void *dev_id)
 {
-	struct spear_adc_state *st = (struct spear_adc_state *)dev_id;
+	struct spear_adc_state *st = dev_id;
 
 	/* Read value to clear IRQ */
 	st->value = spear_adc_get_average(st);
@@ -389,7 +389,6 @@ static struct platform_driver spear_adc_driver = {
 	.remove		= spear_adc_remove,
 	.driver		= {
 		.name	= SPEAR_ADC_MOD_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(spear_adc_dt_ids),
 	},
 };
diff --git a/drivers/staging/iio/addac/Kconfig b/drivers/staging/iio/addac/Kconfig
index e6795e0bed1d..0ed7e13e2283 100644
--- a/drivers/staging/iio/addac/Kconfig
+++ b/drivers/staging/iio/addac/Kconfig
@@ -10,6 +10,9 @@ config ADT7316
 	  Say yes here to build support for Analog Devices ADT7316, ADT7317, ADT7318
 	  and ADT7516, ADT7517, ADT7519 temperature sensors, ADC and DAC.
 
+	  To compile this driver as a module, choose M here: the module will
+	  be called adt7316.
+
 config ADT7316_SPI
 	tristate "support SPI bus connection"
 	depends on SPI && ADT7316
@@ -18,6 +21,9 @@ config ADT7316_SPI
 	  Say yes here to build SPI bus support for Analog Devices ADT7316/7/8
 	  and ADT7516/7/9.
 
+	  To compile this driver as a module, choose M here: the module will
+	  be called adt7316_spi.
+
 config ADT7316_I2C
 	tristate "support I2C bus connection"
 	depends on I2C && ADT7316
@@ -25,4 +31,7 @@ config ADT7316_I2C
 	  Say yes here to build I2C bus support for Analog Devices ADT7316/7/8
 	  and ADT7516/7/9.
 
+	  To compile this driver as a module, choose M here: the module will
+	  be called adt7316_i2c.
+
 endmenu
diff --git a/drivers/staging/iio/addac/adt7316.h b/drivers/staging/iio/addac/adt7316.h
index ec50bf34628d..ec40fbb698a6 100644
--- a/drivers/staging/iio/addac/adt7316.h
+++ b/drivers/staging/iio/addac/adt7316.h
@@ -30,6 +30,7 @@ extern const struct dev_pm_ops adt7316_pm_ops;
 #else
 #define ADT7316_PM_OPS NULL
 #endif
-int adt7316_probe(struct device *dev, struct adt7316_bus *bus, const char *name);
+int adt7316_probe(struct device *dev, struct adt7316_bus *bus,
+		   const char *name);
 
 #endif
diff --git a/drivers/staging/iio/gyro/Kconfig b/drivers/staging/iio/gyro/Kconfig
index 88b199bb2926..f62f68fd6f3f 100644
--- a/drivers/staging/iio/gyro/Kconfig
+++ b/drivers/staging/iio/gyro/Kconfig
@@ -7,7 +7,10 @@ config ADIS16060
 	tristate "Analog Devices ADIS16060 Yaw Rate Gyroscope with SPI driver"
 	depends on SPI
 	help
-	  Say yes here to build support for Analog Devices adis16060 wide bandwidth
+	  Say Y (yes) here to build support for Analog Devices adis16060 wide bandwidth
 	  yaw rate gyroscope with SPI.
 
+	  To compile this driver as a module, say M here: the module will be
+	  called adis16060. If unsure, say N.
+
 endmenu
diff --git a/drivers/staging/iio/light/tsl2x7x_core.c b/drivers/staging/iio/light/tsl2x7x_core.c
index e0d88fa2a5b5..423f96bdf595 100644
--- a/drivers/staging/iio/light/tsl2x7x_core.c
+++ b/drivers/staging/iio/light/tsl2x7x_core.c
@@ -1040,8 +1040,8 @@ static ssize_t tsl2x7x_als_persistence_show(struct device *dev,
 	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);
+	y = filter_delay / 1000;
+	z = filter_delay % 1000;
 
 	return snprintf(buf, PAGE_SIZE, "%d.%03d\n", y, z);
 }
@@ -1086,8 +1086,8 @@ static ssize_t tsl2x7x_prox_persistence_show(struct device *dev,
 	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);
+	y = filter_delay / 1000;
+	z = filter_delay % 1000;
 
 	return snprintf(buf, PAGE_SIZE, "%d.%03d\n", y, z);
 }
@@ -1573,8 +1573,7 @@ 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,
+	&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,
@@ -1591,8 +1590,7 @@ 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,
+	&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,
@@ -1611,8 +1609,7 @@ 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,
+	&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,
diff --git a/drivers/staging/iio/meter/Kconfig b/drivers/staging/iio/meter/Kconfig
index e53274b64ae1..64cd3704ec6e 100644
--- a/drivers/staging/iio/meter/Kconfig
+++ b/drivers/staging/iio/meter/Kconfig
@@ -10,6 +10,9 @@ config ADE7753
 	  Say yes here to build support for Analog Devices ADE7753 Single-Phase Multifunction
 	  Metering IC with di/dt Sensor Interface.
 
+	  To compile this driver as a module, choose M here: the
+	  module will be called ade7753.
+
 config ADE7754
 	tristate "Analog Devices ADE7754 Polyphase Multifunction Energy Metering IC Driver"
 	depends on SPI
@@ -17,6 +20,9 @@ config ADE7754
 	  Say yes here to build support for Analog Devices ADE7754 Polyphase
 	  Multifunction Energy Metering IC Driver.
 
+	  To compile this driver as a module, choose M here: the
+	  module will be called ade7754.
+
 config ADE7758
 	tristate "Analog Devices ADE7758 Poly Phase Multifunction Energy Metering IC Driver"
 	depends on SPI
@@ -26,6 +32,9 @@ config ADE7758
 	  Say yes here to build support for Analog Devices ADE7758 Polyphase
 	  Multifunction Energy Metering IC with Per Phase Information Driver.
 
+	  To compile this driver as a module, choose M here: the
+	  module will be called ade7758.
+
 config ADE7759
 	tristate "Analog Devices ADE7759 Active Energy Metering IC Driver"
 	depends on SPI
@@ -33,6 +42,9 @@ config ADE7759
 	  Say yes here to build support for Analog Devices ADE7758 Active Energy
 	  Metering IC with di/dt Sensor Interface.
 
+	  To compile this driver as a module, choose M here: the
+	  module will be called ade7759.
+
 config ADE7854
 	tristate "Analog Devices ADE7854/58/68/78 Polyphase Multifunction Energy Metering IC Driver"
 	depends on SPI || I2C
@@ -40,6 +52,9 @@ config ADE7854
 	  Say yes here to build support for Analog Devices ADE7854/58/68/78 Polyphase
 	  Multifunction Energy Metering IC Driver.
 
+	  To compile this driver as a module, choose M here: the
+	  module will be called ade7854.
+
 config ADE7854_I2C
 	tristate "support I2C bus connection"
 	depends on ADE7854 && I2C
diff --git a/drivers/staging/iio/trigger/Kconfig b/drivers/staging/iio/trigger/Kconfig
index 2fd18c60323d..710a2f3e787e 100644
--- a/drivers/staging/iio/trigger/Kconfig
+++ b/drivers/staging/iio/trigger/Kconfig
@@ -1,4 +1,4 @@
-#
+  #
 # Industrial I/O standalone triggers
 #
 comment "Triggers - standalone"
@@ -12,6 +12,9 @@ config IIO_PERIODIC_RTC_TRIGGER
 	  Provides support for using periodic capable real time
 	  clocks as IIO triggers.
 
+	  To compile this driver as a module, choose M here: the
+	  module will be called iio-trig-periodic-rtc.
+
 config IIO_BFIN_TMR_TRIGGER
 	tristate "Blackfin TIMER trigger"
 	depends on BLACKFIN
diff --git a/drivers/staging/iio/trigger/iio-trig-bfin-timer.c b/drivers/staging/iio/trigger/iio-trig-bfin-timer.c
index a21b7c514776..2af8d677d4ed 100644
--- a/drivers/staging/iio/trigger/iio-trig-bfin-timer.c
+++ b/drivers/staging/iio/trigger/iio-trig-bfin-timer.c
@@ -279,7 +279,6 @@ static int iio_bfin_tmr_trigger_remove(struct platform_device *pdev)
 static struct platform_driver iio_bfin_tmr_trigger_driver = {
 	.driver = {
 		.name = "iio_bfin_tmr_trigger",
-		.owner = THIS_MODULE,
 	},
 	.probe = iio_bfin_tmr_trigger_probe,
 	.remove = iio_bfin_tmr_trigger_remove,
diff --git a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
index 8f0a2ffa7150..a24caf73ae0b 100644
--- a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
+++ b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
@@ -72,7 +72,8 @@ static ssize_t iio_trig_periodic_write_freq(struct device *dev,
 	if (val > 0) {
 		ret = rtc_irq_set_freq(trig_info->rtc, &trig_info->task, val);
 		if (ret == 0 && trig_info->state && trig_info->frequency == 0)
-			ret = rtc_irq_set_state(trig_info->rtc, &trig_info->task, 1);
+			ret = rtc_irq_set_state(trig_info->rtc,
+						&trig_info->task, 1);
 	} else if (val == 0) {
 		ret = rtc_irq_set_state(trig_info->rtc, &trig_info->task, 0);
 	} else
@@ -206,7 +207,6 @@ static struct platform_driver iio_trig_periodic_rtc_driver = {
 	.remove = iio_trig_periodic_rtc_remove,
 	.driver = {
 		.name = "iio_prtc_trigger",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/staging/imx-drm/TODO b/drivers/staging/imx-drm/TODO
deleted file mode 100644
index 29636fb13959..000000000000
--- a/drivers/staging/imx-drm/TODO
+++ /dev/null
@@ -1,17 +0,0 @@
-TODO:
-- get DRM Maintainer review for this code
-- decide where to put the base driver. It is not specific to a subsystem
-  and would be used by DRM/KMS and media/V4L2
-
-Missing features (not necessarily for moving out of staging):
-
-- Add support for IC (Image converter)
-- Add support for CSI (CMOS Sensor interface)
-- Add support for VDIC (Video Deinterlacer)
-
-Many work-in-progress patches for the above features exist. Contact
-Sascha Hauer <kernel@pengutronix.de> if you are interested in working
-on a specific feature.
-
-Please send any patches to Greg Kroah-Hartman <gregkh@linuxfoundation.org> and
-Sascha Hauer <kernel@pengutronix.de>
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h
index 8888b2756174..2e5a9e5965b1 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h
@@ -242,18 +242,6 @@ do {									\
 
 #define LCONSOLE_EMERG(format, ...) CDEBUG(D_CONSOLE | D_EMERG, format, ## __VA_ARGS__)
 
-void libcfs_log_goto(struct libcfs_debug_msg_data *, const char *, long_ptr_t);
-#define GOTO(label, rc)							\
-do {									\
-	if (cfs_cdebug_show(D_TRACE, DEBUG_SUBSYSTEM)) {		\
-		LIBCFS_DEBUG_MSG_DATA_DECL(msgdata, D_TRACE, NULL);	\
-		libcfs_log_goto(&msgdata, #label, (long_ptr_t)(rc));	\
-	} else {							\
-		(void)(rc);						\
-	}								\
-	goto label;							\
-} while (0)
-
 int libcfs_debug_msg(struct libcfs_debug_msg_data *msgdata,
 			    const char *format1, ...)
 	__attribute__ ((format (printf, 2, 3)));
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
index 8f5cdd584f85..62b575deac3a 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
@@ -930,8 +930,7 @@ kiblnd_close_peer_conns_locked (kib_peer_t *peer, int why)
 	list_for_each_safe (ctmp, cnxt, &peer->ibp_conns) {
 		conn = list_entry(ctmp, kib_conn_t, ibc_list);
 
-		CDEBUG(D_NET, "Closing conn -> %s, "
-			      "version: %x, reason: %d\n",
+		CDEBUG(D_NET, "Closing conn -> %s, version: %x, reason: %d\n",
 		       libcfs_nid2str(peer->ibp_nid),
 		       conn->ibc_version, why);
 
@@ -958,8 +957,7 @@ kiblnd_close_stale_conns_locked (kib_peer_t *peer,
 		    conn->ibc_incarnation == incarnation)
 			continue;
 
-		CDEBUG(D_NET, "Closing stale conn -> %s version: %x, "
-			      "incarnation:%#llx(%x, %#llx)\n",
+		CDEBUG(D_NET, "Closing stale conn -> %s version: %x, incarnation:%#llx(%x, %#llx)\n",
 		       libcfs_nid2str(peer->ibp_nid),
 		       conn->ibc_version, conn->ibc_incarnation,
 		       version, incarnation);
@@ -1599,8 +1597,7 @@ kiblnd_fmr_pool_map(kib_fmr_poolset_t *fps, __u64 *pages, int npages,
 
 	if (fps->fps_increasing) {
 		spin_unlock(&fps->fps_lock);
-		CDEBUG(D_NET, "Another thread is allocating new "
-		       "FMR pool, waiting for her to complete\n");
+		CDEBUG(D_NET, "Another thread is allocating new FMR pool, waiting for her to complete\n");
 		schedule();
 		goto again;
 
@@ -1801,8 +1798,7 @@ kiblnd_pool_alloc_node(kib_poolset_t *ps)
 	if (ps->ps_increasing) {
 		/* another thread is allocating a new pool */
 		spin_unlock(&ps->ps_lock);
-		CDEBUG(D_NET, "Another thread is allocating new "
-		       "%s pool, waiting for her to complete\n",
+		CDEBUG(D_NET, "Another thread is allocating new %s pool, waiting for her to complete\n",
 		       ps->ps_name);
 		schedule();
 		goto again;
@@ -2411,7 +2407,7 @@ kiblnd_hdev_setup_mrs(kib_hca_dev_t *hdev)
 		goto out;
 	}
 
-	mr_size = (1ULL << hdev->ibh_mr_shift);
+	mr_size = 1ULL << hdev->ibh_mr_shift;
 	mm_size = (unsigned long)high_memory - PAGE_OFFSET;
 
 	hdev->ibh_nmrs = (int)((mm_size + mr_size - 1) >> hdev->ibh_mr_shift);
@@ -3081,7 +3077,7 @@ kiblnd_startup (lnet_ni_t *ni)
 	LIBCFS_ALLOC(net, sizeof(*net));
 	ni->ni_data = net;
 	if (net == NULL)
-		goto failed;
+		goto net_failed;
 
 	do_gettimeofday(&tv);
 	net->ibn_incarnation = (((__u64)tv.tv_sec) * 1000000) + tv.tv_usec;
@@ -3147,6 +3143,7 @@ failed:
 	if (net->ibn_dev == NULL && ibdev != NULL)
 		kiblnd_destroy_dev(ibdev);
 
+net_failed:
 	kiblnd_shutdown(ni);
 
 	CDEBUG(D_NET, "kiblnd_startup failed\n");
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
index 14c9c8d18d02..b48d7edf5669 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
@@ -1116,8 +1116,7 @@ kiblnd_init_rdma (kib_conn_t *conn, kib_tx_t *tx, int type,
 		}
 
 		if (tx->tx_nwrq == IBLND_RDMA_FRAGS(conn->ibc_version)) {
-			CERROR("RDMA too fragmented for %s (%d): "
-			       "%d/%d src %d/%d dst frags\n",
+			CERROR("RDMA too fragmented for %s (%d): %d/%d src %d/%d dst frags\n",
 			       libcfs_nid2str(conn->ibc_peer->ibp_nid),
 			       IBLND_RDMA_FRAGS(conn->ibc_version),
 			       srcidx, srcrd->rd_nfrags,
@@ -2254,8 +2253,8 @@ kiblnd_passive_connect (struct rdma_cm_id *cmid, void *priv, int priv_nob)
 	if (ni == NULL ||			 /* no matching net */
 	    ni->ni_nid != reqmsg->ibm_dstnid ||   /* right NET, wrong NID! */
 	    net->ibn_dev != ibdev) {	      /* wrong device */
-		CERROR("Can't accept %s on %s (%s:%d:%pI4h): "
-		       "bad dst nid %s\n", libcfs_nid2str(nid),
+		CERROR("Can't accept %s on %s (%s:%d:%pI4h): bad dst nid %s\n",
+		       libcfs_nid2str(nid),
 		       ni == NULL ? "NA" : libcfs_nid2str(ni->ni_nid),
 		       ibdev->ibd_ifname, ibdev->ibd_nnets,
 		       &ibdev->ibd_ifip,
@@ -2295,8 +2294,7 @@ kiblnd_passive_connect (struct rdma_cm_id *cmid, void *priv, int priv_nob)
 
 	if (reqmsg->ibm_u.connparams.ibcp_max_frags !=
 	    IBLND_RDMA_FRAGS(version)) {
-		CERROR("Can't accept %s(version %x): "
-		       "incompatible max_frags %d (%d wanted)\n",
+		CERROR("Can't accept %s(version %x): incompatible max_frags %d (%d wanted)\n",
 		       libcfs_nid2str(nid), version,
 		       reqmsg->ibm_u.connparams.ibcp_max_frags,
 		       IBLND_RDMA_FRAGS(version));
@@ -2502,8 +2500,7 @@ kiblnd_reconnect (kib_conn_t *conn, int version,
 		break;
 	}
 
-	CNETERR("%s: retrying (%s), %x, %x, "
-		"queue_dep: %d, max_frag: %d, msg_size: %d\n",
+	CNETERR("%s: retrying (%s), %x, %x, queue_dep: %d, max_frag: %d, msg_size: %d\n",
 		libcfs_nid2str(peer->ibp_nid),
 		reason, IBLND_MSG_VERSION, version,
 		cp != NULL? cp->ibcp_queue_depth :IBLND_MSG_QUEUE_SIZE(version),
@@ -2679,8 +2676,7 @@ kiblnd_check_connreply (kib_conn_t *conn, void *priv, int priv_nob)
 	}
 
 	if (ver != msg->ibm_version) {
-		CERROR("%s replied version %x is different with "
-		       "requested version %x\n",
+		CERROR("%s replied version %x is different with requested version %x\n",
 		       libcfs_nid2str(peer->ibp_nid), msg->ibm_version, ver);
 		rc = -EPROTO;
 		goto failed;
@@ -2724,8 +2720,7 @@ kiblnd_check_connreply (kib_conn_t *conn, void *priv, int priv_nob)
 	read_unlock_irqrestore(&kiblnd_data.kib_global_lock, flags);
 
 	if (rc != 0) {
-		CERROR("Bad connection reply from %s, rc = %d, "
-		       "version: %x max_frags: %d\n",
+		CERROR("Bad connection reply from %s, rc = %d, version: %x max_frags: %d\n",
 		       libcfs_nid2str(peer->ibp_nid), rc,
 		       msg->ibm_version, msg->ibm_u.connparams.ibcp_max_frags);
 		goto failed;
@@ -3060,8 +3055,7 @@ kiblnd_check_conns (int idx)
 			}
 
 			if (timedout) {
-				CERROR("Timed out RDMA with %s (%lu): "
-				       "c: %u, oc: %u, rc: %u\n",
+				CERROR("Timed out RDMA with %s (%lu): c: %u, oc: %u, rc: %u\n",
 				       libcfs_nid2str(peer->ibp_nid),
 				       cfs_duration_sec(cfs_time_current() -
 							peer->ibp_last_alive),
@@ -3334,10 +3328,8 @@ kiblnd_scheduler(void *arg)
 
 	rc = cfs_cpt_bind(lnet_cpt_table(), sched->ibs_cpt);
 	if (rc != 0) {
-		CWARN("Failed to bind on CPT %d, please verify whether "
-		      "all CPUs are healthy and reload modules if necessary, "
-		      "otherwise your system might under risk of low "
-		      "performance\n", sched->ibs_cpt);
+		CWARN("Failed to bind on CPT %d, please verify whether all CPUs are healthy and reload modules if necessary, otherwise your system might under risk of low performance\n",
+		      sched->ibs_cpt);
 	}
 
 	spin_lock_irqsave(&sched->ibs_lock, flags);
@@ -3369,8 +3361,7 @@ kiblnd_scheduler(void *arg)
 				rc = ib_req_notify_cq(conn->ibc_cq,
 						      IB_CQ_NEXT_COMP);
 				if (rc < 0) {
-					CWARN("%s: ib_req_notify_cq failed: %d, "
-					      "closing connection\n",
+					CWARN("%s: ib_req_notify_cq failed: %d, closing connection\n",
 					      libcfs_nid2str(conn->ibc_peer->ibp_nid), rc);
 					kiblnd_close_conn(conn, -EIO);
 					kiblnd_conn_decref(conn);
@@ -3383,8 +3374,7 @@ kiblnd_scheduler(void *arg)
 			}
 
 			if (rc < 0) {
-				CWARN("%s: ib_poll_cq failed: %d, "
-				      "closing connection\n",
+				CWARN("%s: ib_poll_cq failed: %d, closing connection\n",
 				      libcfs_nid2str(conn->ibc_peer->ibp_nid),
 				      rc);
 				kiblnd_close_conn(conn, -EIO);
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_modparams.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_modparams.c
index cefdfb6b1bec..8b4a8e9a29b4 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_modparams.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_modparams.c
@@ -222,8 +222,7 @@ kiblnd_tunables_init (void)
 		*kiblnd_tunables.kib_concurrent_sends = *kiblnd_tunables.kib_peertxcredits / 2;
 
 	if (*kiblnd_tunables.kib_concurrent_sends < *kiblnd_tunables.kib_peertxcredits) {
-		CWARN("Concurrent sends %d is lower than message queue size: %d, "
-		      "performance may drop slightly.\n",
+		CWARN("Concurrent sends %d is lower than message queue size: %d, performance may drop slightly.\n",
 		      *kiblnd_tunables.kib_concurrent_sends, *kiblnd_tunables.kib_peertxcredits);
 	}
 
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
index 038854e8302f..9188b34e6948 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
@@ -337,8 +337,7 @@ ksocknal_associate_route_conn_locked(ksock_route_t *route, ksock_conn_t *conn)
 			       &route->ksnr_ipaddr,
 			       &conn->ksnc_myipaddr);
 		} else {
-			CDEBUG(D_NET, "Rebinding %s %pI4h from "
-			       "%pI4h to %pI4h\n",
+			CDEBUG(D_NET, "Rebinding %s %pI4h from %pI4h to %pI4h\n",
 			       libcfs_id2str(peer->ksnp_id),
 			       &route->ksnr_ipaddr,
 			       &route->ksnr_myipaddr,
@@ -974,8 +973,7 @@ ksocknal_accept (lnet_ni_t *ni, struct socket *sock)
 
 	LIBCFS_ALLOC(cr, sizeof(*cr));
 	if (cr == NULL) {
-		LCONSOLE_ERROR_MSG(0x12f, "Dropping connection request from "
-				   "%pI4h: memory exhausted\n",
+		LCONSOLE_ERROR_MSG(0x12f, "Dropping connection request from %pI4h: memory exhausted\n",
 				   &peer_ip);
 		return -ENOMEM;
 	}
@@ -1288,8 +1286,7 @@ ksocknal_create_conn (lnet_ni_t *ni, ksock_route_t *route,
 	 *	socket callbacks.
 	 */
 
-	CDEBUG(D_NET, "New conn %s p %d.x %pI4h -> %pI4h/%d"
-	       " incarnation:%lld sched[%d:%d]\n",
+	CDEBUG(D_NET, "New conn %s p %d.x %pI4h -> %pI4h/%d incarnation:%lld sched[%d:%d]\n",
 	       libcfs_id2str(peerid), conn->ksnc_proto->pro_version,
 	       &conn->ksnc_myipaddr, &conn->ksnc_ipaddr,
 	       conn->ksnc_port, incarnation, cpt,
@@ -1638,37 +1635,32 @@ ksocknal_destroy_conn (ksock_conn_t *conn)
 	case SOCKNAL_RX_LNET_PAYLOAD:
 		last_rcv = conn->ksnc_rx_deadline -
 			   cfs_time_seconds(*ksocknal_tunables.ksnd_timeout);
-		CERROR("Completing partial receive from %s[%d]"
-		       ", ip %pI4h:%d, with error, wanted: %d, left: %d, "
-		       "last alive is %ld secs ago\n",
+		CERROR("Completing partial receive from %s[%d], ip %pI4h:%d, with error, wanted: %d, left: %d, last alive is %ld secs ago\n",
 		       libcfs_id2str(conn->ksnc_peer->ksnp_id), conn->ksnc_type,
 		       &conn->ksnc_ipaddr, conn->ksnc_port,
 		       conn->ksnc_rx_nob_wanted, conn->ksnc_rx_nob_left,
 		       cfs_duration_sec(cfs_time_sub(cfs_time_current(),
-					last_rcv)));
+						     last_rcv)));
 		lnet_finalize (conn->ksnc_peer->ksnp_ni,
 			       conn->ksnc_cookie, -EIO);
 		break;
 	case SOCKNAL_RX_LNET_HEADER:
 		if (conn->ksnc_rx_started)
-			CERROR("Incomplete receive of lnet header from %s"
-			       ", ip %pI4h:%d, with error, protocol: %d.x.\n",
+			CERROR("Incomplete receive of lnet header from %s, ip %pI4h:%d, with error, protocol: %d.x.\n",
 			       libcfs_id2str(conn->ksnc_peer->ksnp_id),
 			       &conn->ksnc_ipaddr, conn->ksnc_port,
 			       conn->ksnc_proto->pro_version);
 		break;
 	case SOCKNAL_RX_KSM_HEADER:
 		if (conn->ksnc_rx_started)
-			CERROR("Incomplete receive of ksock message from %s"
-			       ", ip %pI4h:%d, with error, protocol: %d.x.\n",
+			CERROR("Incomplete receive of ksock message from %s, ip %pI4h:%d, with error, protocol: %d.x.\n",
 			       libcfs_id2str(conn->ksnc_peer->ksnp_id),
 			       &conn->ksnc_ipaddr, conn->ksnc_port,
 			       conn->ksnc_proto->pro_version);
 		break;
 	case SOCKNAL_RX_SLOP:
 		if (conn->ksnc_rx_started)
-			CERROR("Incomplete receive of slops from %s"
-			       ", ip %pI4h:%d, with error\n",
+			CERROR("Incomplete receive of slops from %s, ip %pI4h:%d, with error\n",
 			       libcfs_id2str(conn->ksnc_peer->ksnp_id),
 			       &conn->ksnc_ipaddr, conn->ksnc_port);
 	       break;
@@ -2348,16 +2340,11 @@ ksocknal_base_shutdown(void)
 static __u64
 ksocknal_new_incarnation (void)
 {
-	struct timeval tv;
 
 	/* The incarnation number is the time this module loaded and it
-	 * identifies this particular instance of the socknal.  Hopefully
-	 * we won't be able to reboot more frequently than 1MHz for the
-	 * foreseeable future :) */
-
-	do_gettimeofday(&tv);
-
-	return (((__u64)tv.tv_sec) * 1000000) + tv.tv_usec;
+	 * identifies this particular instance of the socknal.
+	 */
+	return ktime_get_ns();
 }
 
 static int
@@ -2516,22 +2503,21 @@ ksocknal_debug_peerhash (lnet_ni_t *ni)
 		ksock_route_t *route;
 		ksock_conn_t  *conn;
 
-		CWARN ("Active peer on shutdown: %s, ref %d, scnt %d, "
-		       "closing %d, accepting %d, err %d, zcookie %llu, "
-		       "txq %d, zc_req %d\n", libcfs_id2str(peer->ksnp_id),
-		       atomic_read(&peer->ksnp_refcount),
-		       peer->ksnp_sharecount, peer->ksnp_closing,
-		       peer->ksnp_accepting, peer->ksnp_error,
-		       peer->ksnp_zc_next_cookie,
-		       !list_empty(&peer->ksnp_tx_queue),
-		       !list_empty(&peer->ksnp_zc_req_list));
+		CWARN("Active peer on shutdown: %s, ref %d, scnt %d, closing %d, accepting %d, err %d, zcookie %llu, txq %d, zc_req %d\n",
+		      libcfs_id2str(peer->ksnp_id),
+		      atomic_read(&peer->ksnp_refcount),
+		      peer->ksnp_sharecount, peer->ksnp_closing,
+		      peer->ksnp_accepting, peer->ksnp_error,
+		      peer->ksnp_zc_next_cookie,
+		      !list_empty(&peer->ksnp_tx_queue),
+		      !list_empty(&peer->ksnp_zc_req_list));
 
 		list_for_each (tmp, &peer->ksnp_routes) {
 			route = list_entry(tmp, ksock_route_t, ksnr_list);
-			CWARN ("Route: ref %d, schd %d, conn %d, cnted %d, "
-			       "del %d\n", atomic_read(&route->ksnr_refcount),
-			       route->ksnr_scheduled, route->ksnr_connecting,
-			       route->ksnr_connected, route->ksnr_deleted);
+			CWARN("Route: ref %d, schd %d, conn %d, cnted %d, del %d\n",
+			      atomic_read(&route->ksnr_refcount),
+			      route->ksnr_scheduled, route->ksnr_connecting,
+			      route->ksnr_connected, route->ksnr_deleted);
 		}
 
 		list_for_each (tmp, &peer->ksnp_conns) {
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
index d29f5f134b89..e6c1d3647952 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
@@ -551,19 +551,16 @@ ksocknal_process_transmit (ksock_conn_t *conn, ksock_tx_t *tx)
 	if (!conn->ksnc_closing) {
 		switch (rc) {
 		case -ECONNRESET:
-			LCONSOLE_WARN("Host %pI4h reset our connection "
-				      "while we were sending data; it may have "
-				      "rebooted.\n",
+			LCONSOLE_WARN("Host %pI4h reset our connection while we were sending data; it may have rebooted.\n",
 				      &conn->ksnc_ipaddr);
 			break;
 		default:
-			LCONSOLE_WARN("There was an unexpected network error "
-				      "while writing to %pI4h: %d.\n",
+			LCONSOLE_WARN("There was an unexpected network error while writing to %pI4h: %d.\n",
 				      &conn->ksnc_ipaddr, rc);
 			break;
 		}
-		CDEBUG(D_NET, "[%p] Error %d on write to %s"
-		       " ip %pI4h:%d\n", conn, rc,
+		CDEBUG(D_NET, "[%p] Error %d on write to %s ip %pI4h:%d\n",
+		       conn, rc,
 		       libcfs_id2str(conn->ksnc_peer->ksnp_id),
 		       &conn->ksnc_ipaddr,
 		       conn->ksnc_port);
@@ -799,8 +796,7 @@ ksocknal_find_connectable_route_locked (ksock_peer_t *peer)
 		if (!(route->ksnr_retry_interval == 0 || /* first attempt */
 		      cfs_time_aftereq(now, route->ksnr_timeout))) {
 			CDEBUG(D_NET,
-			       "Too soon to retry route %pI4h "
-			       "(cnted %d, interval %ld, %ld secs later)\n",
+			       "Too soon to retry route %pI4h (cnted %d, interval %ld, %ld secs later)\n",
 			       &route->ksnr_ipaddr,
 			       route->ksnr_connected,
 			       route->ksnr_retry_interval,
@@ -874,8 +870,8 @@ ksocknal_launch_packet (lnet_ni_t *ni, ksock_tx_t *tx, lnet_process_id_t id)
 		write_unlock_bh(g_lock);
 
 		if ((id.pid & LNET_PID_USERFLAG) != 0) {
-			CERROR("Refusing to create a connection to "
-			       "userspace process %s\n", libcfs_id2str(id));
+			CERROR("Refusing to create a connection to userspace process %s\n",
+			       libcfs_id2str(id));
 			return -EHOSTUNREACH;
 		}
 
@@ -1132,18 +1128,17 @@ ksocknal_process_receive (ksock_conn_t *conn)
 			LASSERT (rc != -EAGAIN);
 
 			if (rc == 0)
-				CDEBUG(D_NET, "[%p] EOF from %s"
-					" ip %pI4h:%d\n", conn,
-					libcfs_id2str(conn->ksnc_peer->ksnp_id),
-					&conn->ksnc_ipaddr,
-					conn->ksnc_port);
+				CDEBUG(D_NET, "[%p] EOF from %s ip %pI4h:%d\n",
+				       conn,
+				       libcfs_id2str(conn->ksnc_peer->ksnp_id),
+				       &conn->ksnc_ipaddr,
+				       conn->ksnc_port);
 			else if (!conn->ksnc_closing)
-				CERROR("[%p] Error %d on read from %s"
-					" ip %pI4h:%d\n",
-					conn, rc,
-					libcfs_id2str(conn->ksnc_peer->ksnp_id),
-					&conn->ksnc_ipaddr,
-					conn->ksnc_port);
+				CERROR("[%p] Error %d on read from %s ip %pI4h:%d\n",
+				       conn, rc,
+				       libcfs_id2str(conn->ksnc_peer->ksnp_id),
+				       &conn->ksnc_ipaddr,
+				       conn->ksnc_port);
 
 			/* it's not an error if conn is being closed */
 			ksocknal_close_conn_and_siblings (conn,
@@ -1724,10 +1719,10 @@ ksocknal_recv_hello (lnet_ni_t *ni, ksock_conn_t *conn,
 	    hello->kshm_magic != __swab32(LNET_PROTO_MAGIC) &&
 	    hello->kshm_magic != le32_to_cpu (LNET_PROTO_TCP_MAGIC)) {
 		/* Unexpected magic! */
-		CERROR("Bad magic(1) %#08x (%#08x expected) from "
-			"%pI4h\n", __cpu_to_le32 (hello->kshm_magic),
-			LNET_PROTO_TCP_MAGIC,
-			&conn->ksnc_ipaddr);
+		CERROR("Bad magic(1) %#08x (%#08x expected) from %pI4h\n",
+		       __cpu_to_le32 (hello->kshm_magic),
+		       LNET_PROTO_TCP_MAGIC,
+		       &conn->ksnc_ipaddr);
 		return -EPROTO;
 	}
 
@@ -1755,10 +1750,9 @@ ksocknal_recv_hello (lnet_ni_t *ni, ksock_conn_t *conn,
 			ksocknal_send_hello(ni, conn, ni->ni_nid, hello);
 		}
 
-		CERROR("Unknown protocol version (%d.x expected)"
-			" from %pI4h\n",
-			conn->ksnc_proto->pro_version,
-			&conn->ksnc_ipaddr);
+		CERROR("Unknown protocol version (%d.x expected) from %pI4h\n",
+		       conn->ksnc_proto->pro_version,
+		       &conn->ksnc_ipaddr);
 
 		return -EPROTO;
 	}
@@ -1778,8 +1772,8 @@ ksocknal_recv_hello (lnet_ni_t *ni, ksock_conn_t *conn,
 	*incarnation = hello->kshm_src_incarnation;
 
 	if (hello->kshm_src_nid == LNET_NID_ANY) {
-		CERROR("Expecting a HELLO hdr with a NID, but got LNET_NID_ANY"
-		       "from %pI4h\n", &conn->ksnc_ipaddr);
+		CERROR("Expecting a HELLO hdr with a NID, but got LNET_NID_ANY from %pI4h\n",
+		       &conn->ksnc_ipaddr);
 		return -EPROTO;
 	}
 
@@ -1810,10 +1804,7 @@ ksocknal_recv_hello (lnet_ni_t *ni, ksock_conn_t *conn,
 
 	if (peerid->pid != recv_id.pid ||
 	    peerid->nid != recv_id.nid) {
-		LCONSOLE_ERROR_MSG(0x130, "Connected successfully to %s on host"
-				   " %pI4h, but they claimed they were "
-				   "%s; please check your Lustre "
-				   "configuration.\n",
+		LCONSOLE_ERROR_MSG(0x130, "Connected successfully to %s on host %pI4h, but they claimed they were %s; please check your Lustre configuration.\n",
 				   libcfs_id2str(*peerid),
 				   &conn->ksnc_ipaddr,
 				   libcfs_id2str(recv_id));
@@ -2199,8 +2190,7 @@ ksocknal_connd (void *arg)
 			if (ksocknal_connect(route)) {
 				/* consecutive retry */
 				if (cons_retry++ > SOCKNAL_INSANITY_RECONN) {
-					CWARN("massive consecutive "
-					      "re-connecting to %pI4h\n",
+					CWARN("massive consecutive re-connecting to %pI4h\n",
 					      &route->ksnr_ipaddr);
 					cons_retry = 0;
 				}
@@ -2264,25 +2254,20 @@ ksocknal_find_timed_out_conn (ksock_peer_t *peer)
 
 			switch (error) {
 			case ECONNRESET:
-				CNETERR("A connection with %s "
-					"(%pI4h:%d) was reset; "
-					"it may have rebooted.\n",
+				CNETERR("A connection with %s (%pI4h:%d) was reset; it may have rebooted.\n",
 					libcfs_id2str(peer->ksnp_id),
 					&conn->ksnc_ipaddr,
 					conn->ksnc_port);
 				break;
 			case ETIMEDOUT:
-				CNETERR("A connection with %s "
-					"(%pI4h:%d) timed out; the "
-					"network or node may be down.\n",
+				CNETERR("A connection with %s (%pI4h:%d) timed out; the network or node may be down.\n",
 					libcfs_id2str(peer->ksnp_id),
 					&conn->ksnc_ipaddr,
 					conn->ksnc_port);
 				break;
 			default:
-				CNETERR("An unexpected network error %d "
-					"occurred with %s "
-					"(%pI4h:%d\n", error,
+				CNETERR("An unexpected network error %d occurred with %s (%pI4h:%d\n",
+					error,
 					libcfs_id2str(peer->ksnp_id),
 					&conn->ksnc_ipaddr,
 					conn->ksnc_port);
@@ -2297,8 +2282,7 @@ ksocknal_find_timed_out_conn (ksock_peer_t *peer)
 				     conn->ksnc_rx_deadline)) {
 			/* Timed out incomplete incoming message */
 			ksocknal_conn_addref(conn);
-			CNETERR("Timeout receiving from %s (%pI4h:%d), "
-				"state %d wanted %d left %d\n",
+			CNETERR("Timeout receiving from %s (%pI4h:%d), state %d wanted %d left %d\n",
 				libcfs_id2str(peer->ksnp_id),
 				&conn->ksnc_ipaddr,
 				conn->ksnc_port,
@@ -2315,8 +2299,7 @@ ksocknal_find_timed_out_conn (ksock_peer_t *peer)
 			/* Timed out messages queued for sending or
 			 * buffered in the socket's send buffer */
 			ksocknal_conn_addref(conn);
-			CNETERR("Timeout sending data to %s (%pI4h:%d) "
-				"the network or that node may be down.\n",
+			CNETERR("Timeout sending data to %s (%pI4h:%d) the network or that node may be down.\n",
 				libcfs_id2str(peer->ksnp_id),
 				&conn->ksnc_ipaddr,
 				conn->ksnc_port);
@@ -2500,9 +2483,7 @@ ksocknal_check_peer_timeouts (int idx)
 		spin_unlock(&peer->ksnp_lock);
 		read_unlock(&ksocknal_data.ksnd_global_lock);
 
-		CERROR("Total %d stale ZC_REQs for peer %s detected; the "
-		       "oldest(%p) timed out %ld secs ago, "
-		       "resid: %d, wmem: %d\n",
+		CERROR("Total %d stale ZC_REQs for peer %s detected; the oldest(%p) timed out %ld secs ago, resid: %d, wmem: %d\n",
 		       n, libcfs_nid2str(peer->ksnp_id.nid), tx,
 		       cfs_duration_sec(cfs_time_current() - deadline),
 		       resid, conn->ksnc_sock->sk->sk_wmem_queued);
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c
index 9dde548070af..ea9d80f40cab 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c
@@ -515,8 +515,8 @@ ksocknal_send_hello_v1 (ksock_conn_t *conn, ksock_hello_msg_t *hello)
 			       hello->kshm_nips * sizeof(__u32),
 			       lnet_acceptor_timeout());
 	if (rc != 0) {
-		CNETERR("Error %d sending HELLO payload (%d)"
-			" to %pI4h/%d\n", rc, hello->kshm_nips,
+		CNETERR("Error %d sending HELLO payload (%d) to %pI4h/%d\n",
+			rc, hello->kshm_nips,
 			&conn->ksnc_ipaddr, conn->ksnc_port);
 	}
 out:
@@ -560,8 +560,8 @@ ksocknal_send_hello_v2 (ksock_conn_t *conn, ksock_hello_msg_t *hello)
 			       hello->kshm_nips * sizeof(__u32),
 			       lnet_acceptor_timeout());
 	if (rc != 0) {
-		CNETERR("Error %d sending HELLO payload (%d)"
-			" to %pI4h/%d\n", rc, hello->kshm_nips,
+		CNETERR("Error %d sending HELLO payload (%d) to %pI4h/%d\n",
+			rc, hello->kshm_nips,
 			&conn->ksnc_ipaddr, conn->ksnc_port);
 	}
 
@@ -595,10 +595,9 @@ ksocknal_recv_hello_v1(ksock_conn_t *conn, ksock_hello_msg_t *hello,
 
 	/* ...and check we got what we expected */
 	if (hdr->type != cpu_to_le32 (LNET_MSG_HELLO)) {
-		CERROR("Expecting a HELLO hdr,"
-			" but got type %d from %pI4h\n",
-			le32_to_cpu (hdr->type),
-			&conn->ksnc_ipaddr);
+		CERROR("Expecting a HELLO hdr, but got type %d from %pI4h\n",
+		       le32_to_cpu(hdr->type),
+		       &conn->ksnc_ipaddr);
 		rc = -EPROTO;
 		goto out;
 	}
diff --git a/drivers/staging/lustre/lnet/lnet/api-ni.c b/drivers/staging/lustre/lnet/lnet/api-ni.c
index 60bc2ae4fdf1..faceb9505d84 100644
--- a/drivers/staging/lustre/lnet/lnet/api-ni.c
+++ b/drivers/staging/lustre/lnet/lnet/api-ni.c
@@ -37,6 +37,7 @@
 #define DEBUG_SUBSYSTEM S_LNET
 #include "../../include/linux/lnet/lib-lnet.h"
 #include <linux/log2.h>
+#include <linux/ktime.h>
 
 #define D_LNI D_CONSOLE
 
@@ -276,7 +277,7 @@ lnet_find_lnd_by_type(int type)
 	struct list_head	 *tmp;
 
 	/* holding lnd mutex */
-	list_for_each (tmp, &the_lnet.ln_lnds) {
+	list_for_each(tmp, &the_lnet.ln_lnds) {
 		lnd = list_entry(tmp, lnd_t, lnd_list);
 
 		if ((int)lnd->lnd_type == type)
@@ -417,17 +418,9 @@ static __u64
 lnet_create_interface_cookie(void)
 {
 	/* NB the interface cookie in wire handles guards against delayed
-	 * replies and ACKs appearing valid after reboot. Initialisation time,
-	 * even if it's only implemented to millisecond resolution is probably
-	 * easily good enough. */
-	struct timeval tv;
-	__u64	  cookie;
-
-	do_gettimeofday(&tv);
-	cookie = tv.tv_sec;
-	cookie *= 1000000;
-	cookie += tv.tv_usec;
-	return cookie;
+	 * replies and ACKs appearing valid after reboot.
+	 */
+	return ktime_get_ns();
 }
 
 static char *
@@ -1652,7 +1645,6 @@ lnet_destroy_ping_info(void)
 		    offsetof(lnet_ping_info_t,
 			     pi_ni[the_lnet.ln_ping_info->pi_nnis]));
 	the_lnet.ln_ping_info = NULL;
-	return;
 }
 
 int
diff --git a/drivers/staging/lustre/lnet/lnet/lib-md.c b/drivers/staging/lustre/lnet/lnet/lib-md.c
index e4d906a65635..3225c069637d 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-md.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-md.c
@@ -218,7 +218,7 @@ lnet_md_deconstruct(lnet_libmd_t *lmd, lnet_md_t *umd)
 	lnet_eq2handle(&umd->eq_handle, lmd->md_eq);
 }
 
-int
+static int
 lnet_md_validate(lnet_md_t *umd)
 {
 	if (umd->start == NULL && umd->length != 0) {
diff --git a/drivers/staging/lustre/lnet/lnet/lib-move.c b/drivers/staging/lustre/lnet/lnet/lib-move.c
index 4b9567d67f33..c8c1ed84fe5c 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-move.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-move.c
@@ -561,7 +561,7 @@ lnet_extract_kiov(int dst_niov, lnet_kiov_t *dst,
 }
 EXPORT_SYMBOL(lnet_extract_kiov);
 
-void
+static void
 lnet_ni_recv(lnet_ni_t *ni, void *private, lnet_msg_t *msg, int delayed,
 	     unsigned int offset, unsigned int mlen, unsigned int rlen)
 {
@@ -599,7 +599,7 @@ lnet_ni_recv(lnet_ni_t *ni, void *private, lnet_msg_t *msg, int delayed,
 		lnet_finalize(ni, msg, rc);
 }
 
-void
+static void
 lnet_setpayloadbuffer(lnet_msg_t *msg)
 {
 	lnet_libmd_t *md = msg->msg_md;
@@ -639,7 +639,7 @@ lnet_prep_send(lnet_msg_t *msg, int type, lnet_process_id_t target,
 	msg->msg_hdr.payload_length = cpu_to_le32(len);
 }
 
-void
+static void
 lnet_ni_send(lnet_ni_t *ni, lnet_msg_t *msg)
 {
 	void   *priv = msg->msg_private;
@@ -654,7 +654,7 @@ lnet_ni_send(lnet_ni_t *ni, lnet_msg_t *msg)
 		lnet_finalize(ni, msg, rc);
 }
 
-int
+static int
 lnet_ni_eager_recv(lnet_ni_t *ni, lnet_msg_t *msg)
 {
 	int	rc;
@@ -668,8 +668,7 @@ lnet_ni_eager_recv(lnet_ni_t *ni, lnet_msg_t *msg)
 	rc = (ni->ni_lnd->lnd_eager_recv)(ni, msg->msg_private, msg,
 					  &msg->msg_private);
 	if (rc != 0) {
-		CERROR("recv from %s / send to %s aborted: "
-		       "eager_recv failed %d\n",
+		CERROR("recv from %s / send to %s aborted: eager_recv failed %d\n",
 		       libcfs_nid2str(msg->msg_rxpeer->lp_nid),
 		       libcfs_id2str(msg->msg_target), rc);
 		LASSERT(rc < 0); /* required by my callers */
@@ -679,7 +678,7 @@ lnet_ni_eager_recv(lnet_ni_t *ni, lnet_msg_t *msg)
 }
 
 /* NB: caller shall hold a ref on 'lp' as I'd drop lnet_net_lock */
-void
+static void
 lnet_ni_query_locked(lnet_ni_t *ni, lnet_peer_t *lp)
 {
 	unsigned long last_alive = 0;
@@ -731,7 +730,7 @@ lnet_peer_is_alive(lnet_peer_t *lp, unsigned long now)
 
 /* NB: returns 1 when alive, 0 when dead, negative when error;
  *     may drop the lnet_net_lock */
-int
+static int
 lnet_peer_alive_locked(lnet_peer_t *lp)
 {
 	unsigned long now = cfs_time_current();
@@ -753,8 +752,7 @@ lnet_peer_alive_locked(lnet_peer_t *lp)
 
 		if (time_before(now, next_query)) {
 			if (lp->lp_alive)
-				CWARN("Unexpected aliveness of peer %s: "
-				      "%d < %d (%d/%d)\n",
+				CWARN("Unexpected aliveness of peer %s: %d < %d (%d/%d)\n",
 				      libcfs_nid2str(lp->lp_nid),
 				      (int)now, (int)next_query,
 				      lnet_queryinterval,
@@ -817,8 +815,7 @@ lnet_post_send_locked(lnet_msg_t *msg, int do_send)
 	    (msg->msg_md->md_flags & LNET_MD_FLAG_ABORTED) != 0) {
 		lnet_net_unlock(cpt);
 
-		CNETERR("Aborting message for %s: LNetM[DE]Unlink() already "
-			"called on the MD/ME.\n",
+		CNETERR("Aborting message for %s: LNetM[DE]Unlink() already called on the MD/ME.\n",
 			libcfs_id2str(msg->msg_target));
 		if (do_send)
 			lnet_finalize(ni, msg, -ECANCELED);
@@ -871,7 +868,7 @@ lnet_post_send_locked(lnet_msg_t *msg, int do_send)
 }
 
 
-lnet_rtrbufpool_t *
+static lnet_rtrbufpool_t *
 lnet_msg2bufpool(lnet_msg_t *msg)
 {
 	lnet_rtrbufpool_t	*rbp;
@@ -891,7 +888,7 @@ lnet_msg2bufpool(lnet_msg_t *msg)
 	return rbp;
 }
 
-int
+static int
 lnet_post_routed_recv_locked(lnet_msg_t *msg, int do_recv)
 {
 	/* lnet_parse is going to lnet_net_unlock immediately after this, so it
@@ -1220,8 +1217,8 @@ lnet_send(lnet_nid_t src_nid, lnet_msg_t *msg, lnet_nid_t rtr_nid)
 		src_ni = lnet_nid2ni_locked(src_nid, cpt);
 		if (src_ni == NULL) {
 			lnet_net_unlock(cpt);
-			LCONSOLE_WARN("Can't send to %s: src %s is not a "
-				      "local nid\n", libcfs_nid2str(dst_nid),
+			LCONSOLE_WARN("Can't send to %s: src %s is not a local nid\n",
+				      libcfs_nid2str(dst_nid),
 				      libcfs_nid2str(src_nid));
 			return -EINVAL;
 		}
@@ -1283,8 +1280,7 @@ lnet_send(lnet_nid_t src_nid, lnet_msg_t *msg, lnet_nid_t rtr_nid)
 				lnet_ni_decref_locked(src_ni, cpt);
 			lnet_net_unlock(cpt);
 
-			LCONSOLE_WARN("No route to %s via %s "
-				      "(all routers down)\n",
+			LCONSOLE_WARN("No route to %s via %s (all routers down)\n",
 				      libcfs_id2str(msg->msg_target),
 				      libcfs_nid2str(src_nid));
 			return -EHOSTUNREACH;
@@ -1676,8 +1672,7 @@ lnet_print_hdr(lnet_hdr_t *hdr)
 		break;
 
 	case LNET_MSG_PUT:
-		CWARN("    Ptl index %d, ack md %#llx.%#llx, "
-		      "match bits %llu\n",
+		CWARN("    Ptl index %d, ack md %#llx.%#llx, match bits %llu\n",
 		      hdr->msg.put.ptl_index,
 		      hdr->msg.put.ack_wmd.wh_interface_cookie,
 		      hdr->msg.put.ack_wmd.wh_object_cookie,
@@ -1688,8 +1683,8 @@ lnet_print_hdr(lnet_hdr_t *hdr)
 		break;
 
 	case LNET_MSG_GET:
-		CWARN("    Ptl index %d, return md %#llx.%#llx, "
-		      "match bits %llu\n", hdr->msg.get.ptl_index,
+		CWARN("    Ptl index %d, return md %#llx.%#llx, match bits %llu\n",
+		      hdr->msg.get.ptl_index,
 		      hdr->msg.get.return_wmd.wh_interface_cookie,
 		      hdr->msg.get.return_wmd.wh_object_cookie,
 		      hdr->msg.get.match_bits);
@@ -1699,16 +1694,14 @@ lnet_print_hdr(lnet_hdr_t *hdr)
 		break;
 
 	case LNET_MSG_ACK:
-		CWARN("    dst md %#llx.%#llx, "
-		      "manipulated length %d\n",
+		CWARN("    dst md %#llx.%#llx, manipulated length %d\n",
 		      hdr->msg.ack.dst_wmd.wh_interface_cookie,
 		      hdr->msg.ack.dst_wmd.wh_object_cookie,
 		      hdr->msg.ack.mlength);
 		break;
 
 	case LNET_MSG_REPLY:
-		CWARN("    dst md %#llx.%#llx, "
-		      "length %d\n",
+		CWARN("    dst md %#llx.%#llx, length %d\n",
 		      hdr->msg.reply.dst_wmd.wh_interface_cookie,
 		      hdr->msg.reply.dst_wmd.wh_object_cookie,
 		      hdr->payload_length);
@@ -1757,8 +1750,7 @@ lnet_parse(lnet_ni_t *ni, lnet_hdr_t *hdr, lnet_nid_t from_nid,
 	case LNET_MSG_REPLY:
 		if (payload_length >
 		   (__u32)(for_me ? LNET_MAX_PAYLOAD : LNET_MTU)) {
-			CERROR("%s, src %s: bad %s payload %d "
-			       "(%d max expected)\n",
+			CERROR("%s, src %s: bad %s payload %d (%d max expected)\n",
 			       libcfs_nid2str(from_nid),
 			       libcfs_nid2str(src_nid),
 			       lnet_msgtyp2str(type),
@@ -1794,40 +1786,36 @@ lnet_parse(lnet_ni_t *ni, lnet_hdr_t *hdr, lnet_nid_t from_nid,
 	if (!for_me) {
 		if (LNET_NIDNET(dest_nid) == LNET_NIDNET(ni->ni_nid)) {
 			/* should have gone direct */
-			CERROR("%s, src %s: Bad dest nid %s "
-				"(should have been sent direct)\n",
-				libcfs_nid2str(from_nid),
-				libcfs_nid2str(src_nid),
-				libcfs_nid2str(dest_nid));
+			CERROR("%s, src %s: Bad dest nid %s (should have been sent direct)\n",
+			       libcfs_nid2str(from_nid),
+			       libcfs_nid2str(src_nid),
+			       libcfs_nid2str(dest_nid));
 			return -EPROTO;
 		}
 
 		if (lnet_islocalnid(dest_nid)) {
 			/* dest is another local NI; sender should have used
 			 * this node's NID on its own network */
-			CERROR("%s, src %s: Bad dest nid %s "
-				"(it's my nid but on a different network)\n",
-				libcfs_nid2str(from_nid),
-				libcfs_nid2str(src_nid),
-				libcfs_nid2str(dest_nid));
+			CERROR("%s, src %s: Bad dest nid %s (it's my nid but on a different network)\n",
+			       libcfs_nid2str(from_nid),
+			       libcfs_nid2str(src_nid),
+			       libcfs_nid2str(dest_nid));
 			return -EPROTO;
 		}
 
 		if (rdma_req && type == LNET_MSG_GET) {
-			CERROR("%s, src %s: Bad optimized GET for %s "
-				"(final destination must be me)\n",
-				libcfs_nid2str(from_nid),
-				libcfs_nid2str(src_nid),
-				libcfs_nid2str(dest_nid));
+			CERROR("%s, src %s: Bad optimized GET for %s (final destination must be me)\n",
+			       libcfs_nid2str(from_nid),
+			       libcfs_nid2str(src_nid),
+			       libcfs_nid2str(dest_nid));
 			return -EPROTO;
 		}
 
 		if (!the_lnet.ln_routing) {
-			CERROR("%s, src %s: Dropping message for %s "
-				"(routing not enabled)\n",
-				libcfs_nid2str(from_nid),
-				libcfs_nid2str(src_nid),
-				libcfs_nid2str(dest_nid));
+			CERROR("%s, src %s: Dropping message for %s (routing not enabled)\n",
+			       libcfs_nid2str(from_nid),
+			       libcfs_nid2str(src_nid),
+			       libcfs_nid2str(dest_nid));
 			goto drop;
 		}
 	}
@@ -1882,8 +1870,7 @@ lnet_parse(lnet_ni_t *ni, lnet_hdr_t *hdr, lnet_nid_t from_nid,
 	rc = lnet_nid2peer_locked(&msg->msg_rxpeer, from_nid, cpt);
 	if (rc != 0) {
 		lnet_net_unlock(cpt);
-		CERROR("%s, src %s: Dropping %s "
-		       "(error %d looking up sender)\n",
+		CERROR("%s, src %s: Dropping %s (error %d looking up sender)\n",
 		       libcfs_nid2str(from_nid), libcfs_nid2str(src_nid),
 		       lnet_msgtyp2str(type), rc);
 		lnet_msg_free(msg);
@@ -2003,12 +1990,11 @@ lnet_recv_delayed_msg_list(struct list_head *head)
 		LASSERT(msg->msg_rxpeer != NULL);
 		LASSERT(msg->msg_hdr.type == LNET_MSG_PUT);
 
-		CDEBUG(D_NET, "Resuming delayed PUT from %s portal %d "
-		       "match %llu offset %d length %d.\n",
-			libcfs_id2str(id), msg->msg_hdr.msg.put.ptl_index,
-			msg->msg_hdr.msg.put.match_bits,
-			msg->msg_hdr.msg.put.offset,
-			msg->msg_hdr.payload_length);
+		CDEBUG(D_NET, "Resuming delayed PUT from %s portal %d match %llu offset %d length %d.\n",
+		       libcfs_id2str(id), msg->msg_hdr.msg.put.ptl_index,
+		       msg->msg_hdr.msg.put.match_bits,
+		       msg->msg_hdr.msg.put.offset,
+		       msg->msg_hdr.payload_length);
 
 		lnet_recv_put(msg->msg_rxpeer->lp_ni, msg);
 	}
diff --git a/drivers/staging/lustre/lnet/lnet/lib-ptl.c b/drivers/staging/lustre/lnet/lnet/lib-ptl.c
index 720c73be4d3c..19ed696344fe 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-ptl.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-ptl.c
@@ -192,8 +192,7 @@ lnet_try_match_md(lnet_libmd_t *md,
 	}
 
 	/* Commit to this ME/MD */
-	CDEBUG(D_NET, "Incoming %s index %x from %s of "
-	       "length %d/%d into md %#llx [%d] + %d\n",
+	CDEBUG(D_NET, "Incoming %s index %x from %s of length %d/%d into md %#llx [%d] + %d\n",
 	       (info->mi_opc == LNET_MD_OP_PUT) ? "put" : "get",
 	       info->mi_portal, libcfs_id2str(info->mi_id), mlength,
 	       info->mi_rlength, md->md_lh.lh_cookie, md->md_niov, offset);
diff --git a/drivers/staging/lustre/lnet/lnet/lo.c b/drivers/staging/lustre/lnet/lnet/lo.c
index be31dfc5fa4b..17e1643fd675 100644
--- a/drivers/staging/lustre/lnet/lnet/lo.c
+++ b/drivers/staging/lustre/lnet/lnet/lo.c
@@ -35,7 +35,7 @@
 #define DEBUG_SUBSYSTEM S_LNET
 #include "../../include/linux/lnet/lib-lnet.h"
 
-int
+static int
 lolnd_send(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
 {
 	LASSERT(!lntmsg->msg_routing);
@@ -44,7 +44,7 @@ lolnd_send(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
 	return lnet_parse(ni, &lntmsg->msg_hdr, ni->ni_nid, lntmsg, 0);
 }
 
-int
+static int
 lolnd_recv(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg,
 	    int delayed, unsigned int niov,
 	    struct iovec *iov, lnet_kiov_t *kiov,
@@ -86,7 +86,7 @@ lolnd_recv(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg,
 
 static int lolnd_instanced;
 
-void
+static void
 lolnd_shutdown(lnet_ni_t *ni)
 {
 	CDEBUG(D_NET, "shutdown\n");
@@ -95,7 +95,7 @@ lolnd_shutdown(lnet_ni_t *ni)
 	lolnd_instanced = 0;
 }
 
-int
+static int
 lolnd_startup(lnet_ni_t *ni)
 {
 	LASSERT(ni->ni_lnd == &the_lolnd);
diff --git a/drivers/staging/lustre/lnet/lnet/module.c b/drivers/staging/lustre/lnet/lnet/module.c
index e84d59d23ae0..3c23677bc280 100644
--- a/drivers/staging/lustre/lnet/lnet/module.c
+++ b/drivers/staging/lustre/lnet/lnet/module.c
@@ -43,7 +43,7 @@ MODULE_PARM_DESC(config_on_load, "configure network at module load");
 
 static struct mutex lnet_config_mutex;
 
-int
+static int
 lnet_configure(void *arg)
 {
 	/* 'arg' only there so I can be passed to cfs_create_thread() */
@@ -63,7 +63,7 @@ lnet_configure(void *arg)
 	return rc;
 }
 
-int
+static int
 lnet_unconfigure(void)
 {
 	int   refcount;
@@ -83,7 +83,7 @@ lnet_unconfigure(void)
 	return (refcount == 0) ? 0 : -EBUSY;
 }
 
-int
+static int
 lnet_ioctl(unsigned int cmd, struct libcfs_ioctl_data *data)
 {
 	int   rc;
@@ -110,7 +110,7 @@ lnet_ioctl(unsigned int cmd, struct libcfs_ioctl_data *data)
 
 DECLARE_IOCTL_HANDLER(lnet_ioctl_handler, lnet_ioctl);
 
-int
+static int __init
 init_lnet(void)
 {
 	int		  rc;
@@ -135,7 +135,7 @@ init_lnet(void)
 	return 0;
 }
 
-void
+static void __exit
 fini_lnet(void)
 {
 	int rc;
diff --git a/drivers/staging/lustre/lnet/lnet/router.c b/drivers/staging/lustre/lnet/lnet/router.c
index b5b8fb576bfb..c667b5b76761 100644
--- a/drivers/staging/lustre/lnet/lnet/router.c
+++ b/drivers/staging/lustre/lnet/lnet/router.c
@@ -457,8 +457,7 @@ lnet_check_routes(void)
 
 				lnet_net_unlock(cpt);
 
-				CERROR("Routes to %s via %s and %s not "
-				       "supported\n",
+				CERROR("Routes to %s via %s and %s not supported\n",
 				       libcfs_net2str(net),
 				       libcfs_nid2str(nid1),
 				       libcfs_nid2str(nid2));
@@ -752,7 +751,7 @@ lnet_router_checker_event(lnet_event_t *event)
 	lnet_net_unlock(lp->lp_cpt);
 }
 
-void
+static void
 lnet_wait_known_routerstate(void)
 {
 	lnet_peer_t	 *rtr;
@@ -784,7 +783,7 @@ lnet_wait_known_routerstate(void)
 	}
 }
 
-void
+static void
 lnet_update_ni_status_locked(void)
 {
 	lnet_ni_t	*ni;
@@ -824,7 +823,7 @@ lnet_update_ni_status_locked(void)
 	}
 }
 
-void
+static void
 lnet_destroy_rc_data(lnet_rc_data_t *rcd)
 {
 	LASSERT(list_empty(&rcd->rcd_list));
@@ -845,7 +844,7 @@ lnet_destroy_rc_data(lnet_rc_data_t *rcd)
 	LIBCFS_FREE(rcd, sizeof(*rcd));
 }
 
-lnet_rc_data_t *
+static lnet_rc_data_t *
 lnet_create_rc_data_locked(lnet_peer_t *gateway)
 {
 	lnet_rc_data_t		*rcd = NULL;
@@ -959,8 +958,7 @@ lnet_ping_router_locked (lnet_peer_t *rtr)
 	secs = lnet_router_check_interval(rtr);
 
 	CDEBUG(D_NET,
-	       "rtr %s %d: deadline %lu ping_notsent %d alive %d "
-	       "alive_count %d lp_ping_timestamp %lu\n",
+	       "rtr %s %d: deadline %lu ping_notsent %d alive %d alive_count %d lp_ping_timestamp %lu\n",
 	       libcfs_nid2str(rtr->lp_nid), secs,
 	       rtr->lp_ping_deadline, rtr->lp_ping_notsent,
 	       rtr->lp_alive, rtr->lp_alive_count, rtr->lp_ping_timestamp);
@@ -1010,9 +1008,7 @@ lnet_router_checker_start(void)
 
 	if (check_routers_before_use &&
 	    dead_router_check_interval <= 0) {
-		LCONSOLE_ERROR_MSG(0x10a, "'dead_router_check_interval' must be"
-				   " set if 'check_routers_before_use' is set"
-				   "\n");
+		LCONSOLE_ERROR_MSG(0x10a, "'dead_router_check_interval' must be set if 'check_routers_before_use' is set\n");
 		return -EINVAL;
 	}
 
@@ -1224,7 +1220,7 @@ rescan:
 	return 0;
 }
 
-void
+static void
 lnet_destroy_rtrbuf(lnet_rtrbuf_t *rb, int npages)
 {
 	int sz = offsetof(lnet_rtrbuf_t, rb_kiov[npages]);
@@ -1235,7 +1231,7 @@ lnet_destroy_rtrbuf(lnet_rtrbuf_t *rb, int npages)
 	LIBCFS_FREE(rb, sz);
 }
 
-lnet_rtrbuf_t *
+static lnet_rtrbuf_t *
 lnet_new_rtrbuf(lnet_rtrbufpool_t *rbp, int cpt)
 {
 	int	    npages = rbp->rbp_npages;
@@ -1270,7 +1266,7 @@ lnet_new_rtrbuf(lnet_rtrbufpool_t *rbp, int cpt)
 	return rb;
 }
 
-void
+static void
 lnet_rtrpool_free_bufs(lnet_rtrbufpool_t *rbp)
 {
 	int		npages = rbp->rbp_npages;
@@ -1299,7 +1295,7 @@ lnet_rtrpool_free_bufs(lnet_rtrbufpool_t *rbp)
 	rbp->rbp_nbuffers = rbp->rbp_credits = 0;
 }
 
-int
+static int
 lnet_rtrpool_alloc_bufs(lnet_rtrbufpool_t *rbp, int nbufs, int cpt)
 {
 	lnet_rtrbuf_t *rb;
@@ -1333,7 +1329,7 @@ lnet_rtrpool_alloc_bufs(lnet_rtrbufpool_t *rbp, int nbufs, int cpt)
 	return 0;
 }
 
-void
+static void
 lnet_rtrpool_init(lnet_rtrbufpool_t *rbp, int npages)
 {
 	INIT_LIST_HEAD(&rbp->rbp_msgs);
@@ -1370,8 +1366,8 @@ lnet_nrb_tiny_calculate(int npages)
 
 	if (tiny_router_buffers < 0) {
 		LCONSOLE_ERROR_MSG(0x10c,
-				   "tiny_router_buffers=%d invalid when "
-				   "routing enabled\n", tiny_router_buffers);
+				   "tiny_router_buffers=%d invalid when routing enabled\n",
+				   tiny_router_buffers);
 		return -1;
 	}
 
@@ -1389,8 +1385,8 @@ lnet_nrb_small_calculate(int npages)
 
 	if (small_router_buffers < 0) {
 		LCONSOLE_ERROR_MSG(0x10c,
-				   "small_router_buffers=%d invalid when "
-				   "routing enabled\n", small_router_buffers);
+				   "small_router_buffers=%d invalid when routing enabled\n",
+				   small_router_buffers);
 		return -1;
 	}
 
@@ -1408,8 +1404,8 @@ lnet_nrb_large_calculate(int npages)
 
 	if (large_router_buffers < 0) {
 		LCONSOLE_ERROR_MSG(0x10c,
-				   "large_router_buffers=%d invalid when "
-				   "routing enabled\n", large_router_buffers);
+				   "large_router_buffers=%d invalid when routing enabled\n",
+				   large_router_buffers);
 		return -1;
 	}
 
@@ -1442,8 +1438,7 @@ lnet_rtrpools_alloc(int im_a_router)
 	} else if (!strcmp(forwarding, "enabled")) {
 		/* explicitly enabled */
 	} else {
-		LCONSOLE_ERROR_MSG(0x10b, "'forwarding' not set to either "
-				   "'enabled' or 'disabled'\n");
+		LCONSOLE_ERROR_MSG(0x10b, "'forwarding' not set to either 'enabled' or 'disabled'\n");
 		return -EINVAL;
 	}
 
@@ -1520,11 +1515,10 @@ lnet_notify(lnet_ni_t *ni, lnet_nid_t nid, int alive, unsigned long when)
 
 	/* can't do predictions... */
 	if (cfs_time_after(when, now)) {
-		CWARN ("Ignoring prediction from %s of %s %s "
-		       "%ld seconds in the future\n",
-		       (ni == NULL) ? "userspace" : libcfs_nid2str(ni->ni_nid),
-		       libcfs_nid2str(nid), alive ? "up" : "down",
-		       cfs_duration_sec(cfs_time_sub(when, now)));
+		CWARN("Ignoring prediction from %s of %s %s %ld seconds in the future\n",
+		      (ni == NULL) ? "userspace" : libcfs_nid2str(ni->ni_nid),
+		      libcfs_nid2str(nid), alive ? "up" : "down",
+		      cfs_duration_sec(cfs_time_sub(when, now)));
 		return -EINVAL;
 	}
 
diff --git a/drivers/staging/lustre/lnet/lnet/router_proc.c b/drivers/staging/lustre/lnet/lnet/router_proc.c
index 6e8f7e2bbcfc..46cde7036f1d 100644
--- a/drivers/staging/lustre/lnet/lnet/router_proc.c
+++ b/drivers/staging/lustre/lnet/lnet/router_proc.c
@@ -164,8 +164,8 @@ static int proc_lnet_stats(struct ctl_table *table, int write,
 				 __proc_lnet_stats);
 }
 
-int proc_lnet_routes(struct ctl_table *table, int write, void __user *buffer,
-		     size_t *lenp, loff_t *ppos)
+static int proc_lnet_routes(struct ctl_table *table, int write,
+			    void __user *buffer, size_t *lenp, loff_t *ppos)
 {
 	const int	tmpsiz = 256;
 	char		*tmpstr;
@@ -290,8 +290,8 @@ int proc_lnet_routes(struct ctl_table *table, int write, void __user *buffer,
 	return rc;
 }
 
-int proc_lnet_routers(struct ctl_table *table, int write, void __user *buffer,
-		      size_t *lenp, loff_t *ppos)
+static int proc_lnet_routers(struct ctl_table *table, int write,
+			     void __user *buffer, size_t *lenp, loff_t *ppos)
 {
 	int	rc = 0;
 	char      *tmpstr;
@@ -425,8 +425,8 @@ int proc_lnet_routers(struct ctl_table *table, int write, void __user *buffer,
 	return rc;
 }
 
-int proc_lnet_peers(struct ctl_table *table, int write, void __user *buffer,
-		    size_t *lenp, loff_t *ppos)
+static int proc_lnet_peers(struct ctl_table *table, int write,
+			   void __user *buffer, size_t *lenp, loff_t *ppos)
 {
 	const int		tmpsiz  = 256;
 	struct lnet_peer_table	*ptable;
@@ -657,8 +657,8 @@ static int proc_lnet_buffers(struct ctl_table *table, int write,
 				 __proc_lnet_buffers);
 }
 
-int proc_lnet_nis(struct ctl_table *table, int write, void __user *buffer,
-		  size_t *lenp, loff_t *ppos)
+static int proc_lnet_nis(struct ctl_table *table, int write,
+			 void __user *buffer, size_t *lenp, loff_t *ppos)
 {
 	int	tmpsiz = 128 * LNET_CPT_NUMBER;
 	int	rc = 0;
@@ -791,20 +791,17 @@ static struct lnet_portal_rotors	portal_rotors[] = {
 	{
 		.pr_value = LNET_PTL_ROTOR_ON,
 		.pr_name  = "ON",
-		.pr_desc  = "round-robin dispatch all PUT messages for "
-			    "wildcard portals"
+		.pr_desc  = "round-robin dispatch all PUT messages for wildcard portals"
 	},
 	{
 		.pr_value = LNET_PTL_ROTOR_RR_RT,
 		.pr_name  = "RR_RT",
-		.pr_desc  = "round-robin dispatch routed PUT message for "
-			    "wildcard portals"
+		.pr_desc  = "round-robin dispatch routed PUT message for wildcard portals"
 	},
 	{
 		.pr_value = LNET_PTL_ROTOR_HASH_RT,
 		.pr_name  = "HASH_RT",
-		.pr_desc  = "dispatch routed PUT message by hashing source "
-			    "NID for wildcard portals"
+		.pr_desc  = "dispatch routed PUT message by hashing source NID for wildcard portals"
 	},
 	{
 		.pr_value = -1,
diff --git a/drivers/staging/lustre/lnet/selftest/brw_test.c b/drivers/staging/lustre/lnet/selftest/brw_test.c
index a94f336d578c..463da076fa70 100644
--- a/drivers/staging/lustre/lnet/selftest/brw_test.c
+++ b/drivers/staging/lustre/lnet/selftest/brw_test.c
@@ -233,7 +233,7 @@ brw_fill_bulk(srpc_bulk_t *bk, int pattern, __u64 magic)
 	}
 }
 
-int
+static int
 brw_check_bulk(srpc_bulk_t *bk, int pattern, __u64 magic)
 {
 	int	 i;
@@ -358,7 +358,7 @@ out:
 	return;
 }
 
-void
+static void
 brw_server_rpc_done(srpc_server_rpc_t *rpc)
 {
 	srpc_bulk_t *blk = rpc->srpc_bulk;
@@ -378,7 +378,7 @@ brw_server_rpc_done(srpc_server_rpc_t *rpc)
 	sfw_free_pages(rpc);
 }
 
-int
+static int
 brw_bulk_ready(srpc_server_rpc_t *rpc, int status)
 {
 	__u64	     magic = BRW_MAGIC;
@@ -414,7 +414,7 @@ brw_bulk_ready(srpc_server_rpc_t *rpc, int status)
 	return 0;
 }
 
-int
+static int
 brw_server_handle(struct srpc_server_rpc *rpc)
 {
 	struct srpc_service	*sv = rpc->srpc_scd->scd_svc;
diff --git a/drivers/staging/lustre/lnet/selftest/conctl.c b/drivers/staging/lustre/lnet/selftest/conctl.c
index ae7b0fcd818d..5bc615309e72 100644
--- a/drivers/staging/lustre/lnet/selftest/conctl.c
+++ b/drivers/staging/lustre/lnet/selftest/conctl.c
@@ -45,7 +45,7 @@
 #include "../../include/linux/lnet/lnetst.h"
 #include "console.h"
 
-int
+static int
 lst_session_new_ioctl(lstio_session_new_args_t *args)
 {
 	char      *name;
@@ -82,7 +82,7 @@ lst_session_new_ioctl(lstio_session_new_args_t *args)
 	return rc;
 }
 
-int
+static int
 lst_session_end_ioctl(lstio_session_end_args_t *args)
 {
 	if (args->lstio_ses_key != console_session.ses_key)
@@ -91,7 +91,7 @@ lst_session_end_ioctl(lstio_session_end_args_t *args)
 	return lstcon_session_end();
 }
 
-int
+static int
 lst_session_info_ioctl(lstio_session_info_args_t *args)
 {
 	/* no checking of key */
@@ -113,7 +113,7 @@ lst_session_info_ioctl(lstio_session_info_args_t *args)
 				   args->lstio_ses_nmlen);
 }
 
-int
+static int
 lst_debug_ioctl(lstio_debug_args_t *args)
 {
 	char   *name   = NULL;
@@ -194,7 +194,7 @@ out:
 	return rc;
 }
 
-int
+static int
 lst_group_add_ioctl(lstio_group_add_args_t *args)
 {
 	char	   *name;
@@ -228,7 +228,7 @@ lst_group_add_ioctl(lstio_group_add_args_t *args)
 	return rc;
 }
 
-int
+static int
 lst_group_del_ioctl(lstio_group_del_args_t *args)
 {
 	int     rc;
@@ -262,7 +262,7 @@ lst_group_del_ioctl(lstio_group_del_args_t *args)
 	return rc;
 }
 
-int
+static int
 lst_group_update_ioctl(lstio_group_update_args_t *args)
 {
 	int     rc;
@@ -320,7 +320,7 @@ lst_group_update_ioctl(lstio_group_update_args_t *args)
 	return rc;
 }
 
-int
+static int
 lst_nodes_add_ioctl(lstio_group_nodes_args_t *args)
 {
 	unsigned feats;
@@ -365,7 +365,7 @@ lst_nodes_add_ioctl(lstio_group_nodes_args_t *args)
 	return rc;
 }
 
-int
+static int
 lst_group_list_ioctl(lstio_group_list_args_t *args)
 {
 	if (args->lstio_grp_key != console_session.ses_key)
@@ -382,7 +382,7 @@ lst_group_list_ioctl(lstio_group_list_args_t *args)
 			      args->lstio_grp_namep);
 }
 
-int
+static int
 lst_group_info_ioctl(lstio_group_info_args_t *args)
 {
 	char	   *name;
@@ -446,7 +446,7 @@ lst_group_info_ioctl(lstio_group_info_args_t *args)
 	return 0;
 }
 
-int
+static int
 lst_batch_add_ioctl(lstio_batch_add_args_t *args)
 {
 	int	     rc;
@@ -480,7 +480,7 @@ lst_batch_add_ioctl(lstio_batch_add_args_t *args)
 	return rc;
 }
 
-int
+static int
 lst_batch_run_ioctl(lstio_batch_run_args_t *args)
 {
 	int	     rc;
@@ -515,7 +515,7 @@ lst_batch_run_ioctl(lstio_batch_run_args_t *args)
 	return rc;
 }
 
-int
+static int
 lst_batch_stop_ioctl(lstio_batch_stop_args_t *args)
 {
 	int	     rc;
@@ -551,7 +551,7 @@ lst_batch_stop_ioctl(lstio_batch_stop_args_t *args)
 	return rc;
 }
 
-int
+static int
 lst_batch_query_ioctl(lstio_batch_query_args_t *args)
 {
 	char   *name;
@@ -593,7 +593,7 @@ lst_batch_query_ioctl(lstio_batch_query_args_t *args)
 	return rc;
 }
 
-int
+static int
 lst_batch_list_ioctl(lstio_batch_list_args_t *args)
 {
 	if (args->lstio_bat_key != console_session.ses_key)
@@ -610,7 +610,7 @@ lst_batch_list_ioctl(lstio_batch_list_args_t *args)
 			      args->lstio_bat_namep);
 }
 
-int
+static int
 lst_batch_info_ioctl(lstio_batch_info_args_t *args)
 {
 	char	   *name;
@@ -675,7 +675,7 @@ lst_batch_info_ioctl(lstio_batch_info_args_t *args)
 	return rc;
 }
 
-int
+static int
 lst_stat_query_ioctl(lstio_stat_args_t *args)
 {
 	int	     rc;
diff --git a/drivers/staging/lustre/lnet/selftest/conrpc.c b/drivers/staging/lustre/lnet/selftest/conrpc.c
index a3a60d6e9081..9999b0dc03e4 100644
--- a/drivers/staging/lustre/lnet/selftest/conrpc.c
+++ b/drivers/staging/lustre/lnet/selftest/conrpc.c
@@ -88,7 +88,7 @@ lstcon_rpc_done(srpc_client_rpc_t *rpc)
 	spin_unlock(&rpc->crpc_lock);
 }
 
-int
+static int
 lstcon_rpc_init(lstcon_node_t *nd, int service, unsigned feats,
 		int bulk_npg, int bulk_len, int embedded, lstcon_rpc_t *crpc)
 {
@@ -113,7 +113,7 @@ lstcon_rpc_init(lstcon_node_t *nd, int service, unsigned feats,
 	return 0;
 }
 
-int
+static int
 lstcon_rpc_prep(lstcon_node_t *nd, int service, unsigned feats,
 		int bulk_npg, int bulk_len, lstcon_rpc_t **crpcpp)
 {
@@ -182,7 +182,7 @@ lstcon_rpc_put(lstcon_rpc_t *crpc)
 	atomic_dec(&console_session.ses_rpc_counter);
 }
 
-void
+static void
 lstcon_rpc_post(lstcon_rpc_t *crpc)
 {
 	lstcon_rpc_trans_t *trans = crpc->crp_trans;
@@ -383,7 +383,7 @@ lstcon_rpc_trans_postwait(lstcon_rpc_trans_t *trans, int timeout)
 	return rc;
 }
 
-int
+static int
 lstcon_rpc_get_reply(lstcon_rpc_t *crpc, srpc_msg_t **msgpp)
 {
 	lstcon_node_t	*nd  = crpc->crp_node;
@@ -718,7 +718,7 @@ lstcon_next_id(int idx, int nkiov, lnet_kiov_t *kiov)
 	return &pid[idx % SFW_ID_PER_PAGE];
 }
 
-int
+static int
 lstcon_dstnodes_prep(lstcon_group_t *grp, int idx,
 		     int dist, int span, int nkiov, lnet_kiov_t *kiov)
 {
@@ -772,7 +772,7 @@ lstcon_dstnodes_prep(lstcon_group_t *grp, int idx,
 	return 0;
 }
 
-int
+static int
 lstcon_pingrpc_prep(lst_test_ping_param_t *param, srpc_test_reqst_t *req)
 {
 	test_ping_req_t *prq = &req->tsr_u.ping;
@@ -783,7 +783,7 @@ lstcon_pingrpc_prep(lst_test_ping_param_t *param, srpc_test_reqst_t *req)
 	return 0;
 }
 
-int
+static int
 lstcon_bulkrpc_v0_prep(lst_test_bulk_param_t *param, srpc_test_reqst_t *req)
 {
 	test_bulk_req_t *brq = &req->tsr_u.bulk_v0;
@@ -795,7 +795,7 @@ lstcon_bulkrpc_v0_prep(lst_test_bulk_param_t *param, srpc_test_reqst_t *req)
 	return 0;
 }
 
-int
+static int
 lstcon_bulkrpc_v1_prep(lst_test_bulk_param_t *param, srpc_test_reqst_t *req)
 {
 	test_bulk_req_v1_t *brq = &req->tsr_u.bulk_v1;
@@ -915,7 +915,7 @@ lstcon_testrpc_prep(lstcon_node_t *nd, int transop, unsigned feats,
 	return rc;
 }
 
-int
+static int
 lstcon_sesnew_stat_reply(lstcon_rpc_trans_t *trans,
 			 lstcon_node_t *nd, srpc_msg_t *reply)
 {
@@ -1162,7 +1162,7 @@ lstcon_rpc_trans_ndlist(struct list_head *ndlist,
 	return rc;
 }
 
-void
+static void
 lstcon_rpc_pinger(void *arg)
 {
 	stt_timer_t	*ptimer = (stt_timer_t *)arg;
diff --git a/drivers/staging/lustre/lnet/selftest/console.c b/drivers/staging/lustre/lnet/selftest/console.c
index 5dad9f1f9462..49cb6543d538 100644
--- a/drivers/staging/lustre/lnet/selftest/console.c
+++ b/drivers/staging/lustre/lnet/selftest/console.c
@@ -1208,8 +1208,7 @@ again:
 
 		lstcon_rpc_trans_destroy(trans);
 		/* return if any error */
-		CDEBUG(D_NET, "Failed to add test %s, "
-			      "RPC error %d, framework error %d\n",
+		CDEBUG(D_NET, "Failed to add test %s, RPC error %d, framework error %d\n",
 		       transop == LST_TRANS_TSBCLIADD ? "client" : "server",
 		       lstcon_trans_stat()->trs_rpc_errno,
 		       lstcon_trans_stat()->trs_fwk_errno);
@@ -1885,8 +1884,7 @@ lstcon_session_feats_check(unsigned feats)
 	spin_unlock(&console_session.ses_rpc_lock);
 
 	if (rc != 0) {
-		CERROR("remote features %x do not match with "
-		       "session features %x of console\n",
+		CERROR("remote features %x do not match with session features %x of console\n",
 		       feats, console_session.ses_features);
 	}
 
diff --git a/drivers/staging/lustre/lnet/selftest/framework.c b/drivers/staging/lustre/lnet/selftest/framework.c
index df04ab7de835..cc9d1826ae66 100644
--- a/drivers/staging/lustre/lnet/selftest/framework.c
+++ b/drivers/staging/lustre/lnet/selftest/framework.c
@@ -156,7 +156,7 @@ sfw_register_test (srpc_service_t *service, sfw_test_client_ops_t *cliops)
 	return 0;
 }
 
-void
+static void
 sfw_add_session_timer (void)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
@@ -176,7 +176,7 @@ sfw_add_session_timer (void)
 	return;
 }
 
-int
+static int
 sfw_del_session_timer (void)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
@@ -238,7 +238,7 @@ sfw_deactivate_session (void)
 }
 
 
-void
+static void
 sfw_session_expired (void *data)
 {
 	sfw_session_t *sn = data;
@@ -284,15 +284,14 @@ sfw_init_session(sfw_session_t *sn, lst_sid_t sid,
 }
 
 /* completion handler for incoming framework RPCs */
-void
+static void
 sfw_server_rpc_done(struct srpc_server_rpc *rpc)
 {
 	struct srpc_service	*sv	= rpc->srpc_scd->scd_svc;
 	int			status	= rpc->srpc_status;
 
 	CDEBUG (D_NET,
-		"Incoming framework RPC done: "
-		"service %s, peer %s, status %s:%d\n",
+		"Incoming framework RPC done: service %s, peer %s, status %s:%d\n",
 		sv->sv_name, libcfs_id2str(rpc->srpc_peer),
 		swi_state2str(rpc->srpc_wi.swi_state),
 		status);
@@ -302,7 +301,7 @@ sfw_server_rpc_done(struct srpc_server_rpc *rpc)
 	return;
 }
 
-void
+static void
 sfw_client_rpc_fini (srpc_client_rpc_t *rpc)
 {
 	LASSERT (rpc->crpc_bulk.bk_niov == 0);
@@ -310,8 +309,7 @@ sfw_client_rpc_fini (srpc_client_rpc_t *rpc)
 	LASSERT (atomic_read(&rpc->crpc_refcount) == 0);
 
 	CDEBUG (D_NET,
-		"Outgoing framework RPC done: "
-		"service %d, peer %s, status %s:%d:%d\n",
+		"Outgoing framework RPC done: service %d, peer %s, status %s:%d:%d\n",
 		rpc->crpc_service, libcfs_id2str(rpc->crpc_dest),
 		swi_state2str(rpc->crpc_wi.swi_state),
 		rpc->crpc_aborted, rpc->crpc_status);
@@ -325,7 +323,7 @@ sfw_client_rpc_fini (srpc_client_rpc_t *rpc)
 	spin_unlock(&sfw_data.fw_lock);
 }
 
-sfw_batch_t *
+static sfw_batch_t *
 sfw_find_batch (lst_bid_t bid)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
@@ -341,7 +339,7 @@ sfw_find_batch (lst_bid_t bid)
 	return NULL;
 }
 
-sfw_batch_t *
+static sfw_batch_t *
 sfw_bid2batch (lst_bid_t bid)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
@@ -367,7 +365,7 @@ sfw_bid2batch (lst_bid_t bid)
 	return bat;
 }
 
-int
+static int
 sfw_get_stats (srpc_stat_reqst_t *request, srpc_stat_reply_t *reply)
 {
 	sfw_session_t  *sn = sfw_data.fw_session;
@@ -479,7 +477,7 @@ sfw_make_session(srpc_mksn_reqst_t *request, srpc_mksn_reply_t *reply)
 	return 0;
 }
 
-int
+static int
 sfw_remove_session (srpc_rmsn_reqst_t *request, srpc_rmsn_reply_t *reply)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
@@ -511,7 +509,7 @@ sfw_remove_session (srpc_rmsn_reqst_t *request, srpc_rmsn_reply_t *reply)
 	return 0;
 }
 
-int
+static int
 sfw_debug_session (srpc_debug_reqst_t *request, srpc_debug_reply_t *reply)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
@@ -532,7 +530,7 @@ sfw_debug_session (srpc_debug_reqst_t *request, srpc_debug_reply_t *reply)
 	return 0;
 }
 
-void
+static void
 sfw_test_rpc_fini (srpc_client_rpc_t *rpc)
 {
 	sfw_test_unit_t     *tsu = rpc->crpc_priv;
@@ -554,7 +552,7 @@ sfw_test_buffers(sfw_test_instance_t *tsi)
 	return max(SFW_TEST_WI_MIN, nbuf + SFW_TEST_WI_EXTRA);
 }
 
-int
+static int
 sfw_load_test(struct sfw_test_instance *tsi)
 {
 	struct sfw_test_case	*tsc;
@@ -575,8 +573,8 @@ sfw_load_test(struct sfw_test_instance *tsi)
 
 	rc = srpc_service_add_buffers(svc, nbuf);
 	if (rc != 0) {
-		CWARN("Failed to reserve enough buffers: "
-		      "service %s, %d needed: %d\n", svc->sv_name, nbuf, rc);
+		CWARN("Failed to reserve enough buffers: service %s, %d needed: %d\n",
+		      svc->sv_name, nbuf, rc);
 		/* NB: this error handler is not strictly correct, because
 		 * it may release more buffers than already allocated,
 		 * but it doesn't matter because request portal should
@@ -591,7 +589,7 @@ sfw_load_test(struct sfw_test_instance *tsi)
 	return 0;
 }
 
-void
+static void
 sfw_unload_test(struct sfw_test_instance *tsi)
 {
 	struct sfw_test_case *tsc = sfw_find_test_case(tsi->tsi_service);
@@ -609,7 +607,7 @@ sfw_unload_test(struct sfw_test_instance *tsi)
 	return;
 }
 
-void
+static void
 sfw_destroy_test_instance (sfw_test_instance_t *tsi)
 {
 	srpc_client_rpc_t *rpc;
@@ -643,7 +641,7 @@ clean:
 	return;
 }
 
-void
+static void
 sfw_destroy_batch (sfw_batch_t *tsb)
 {
 	sfw_test_instance_t *tsi;
@@ -682,7 +680,7 @@ sfw_destroy_session (sfw_session_t *sn)
 	return;
 }
 
-void
+static void
 sfw_unpack_addtest_req(srpc_msg_t *msg)
 {
 	srpc_test_reqst_t *req = &msg->msg_body.tes_reqst;
@@ -727,7 +725,7 @@ sfw_unpack_addtest_req(srpc_msg_t *msg)
 	return;
 }
 
-int
+static int
 sfw_add_test_instance (sfw_batch_t *tsb, srpc_server_rpc_t *rpc)
 {
 	srpc_msg_t	  *msg = &rpc->srpc_reqstbuf->buf_msg;
@@ -865,7 +863,7 @@ sfw_test_unit_done (sfw_test_unit_t *tsu)
 	return;
 }
 
-void
+static void
 sfw_test_rpc_done (srpc_client_rpc_t *rpc)
 {
 	sfw_test_unit_t     *tsu = rpc->crpc_priv;
@@ -944,7 +942,7 @@ sfw_create_test_rpc(sfw_test_unit_t *tsu, lnet_process_id_t peer,
 	return 0;
 }
 
-int
+static int
 sfw_run_test (swi_workitem_t *wi)
 {
 	sfw_test_unit_t     *tsu = wi->swi_workitem.wi_data;
@@ -994,7 +992,7 @@ test_done:
 	return 1;
 }
 
-int
+static int
 sfw_run_batch (sfw_batch_t *tsb)
 {
 	swi_workitem_t      *wi;
@@ -1072,7 +1070,7 @@ sfw_stop_batch (sfw_batch_t *tsb, int force)
 	return 0;
 }
 
-int
+static int
 sfw_query_batch (sfw_batch_t *tsb, int testidx, srpc_batch_reply_t *reply)
 {
 	sfw_test_instance_t *tsi;
@@ -1117,7 +1115,7 @@ sfw_alloc_pages(struct srpc_server_rpc *rpc, int cpt, int npages, int len,
 	return 0;
 }
 
-int
+static int
 sfw_add_test (srpc_server_rpc_t *rpc)
 {
 	sfw_session_t     *sn = sfw_data.fw_session;
@@ -1187,7 +1185,7 @@ sfw_add_test (srpc_server_rpc_t *rpc)
 	return 0;
 }
 
-int
+static int
 sfw_control_batch (srpc_batch_reqst_t *request, srpc_batch_reply_t *reply)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
@@ -1228,7 +1226,7 @@ sfw_control_batch (srpc_batch_reqst_t *request, srpc_batch_reply_t *reply)
 	return 0;
 }
 
-int
+static int
 sfw_handle_server_rpc(struct srpc_server_rpc *rpc)
 {
 	struct srpc_service	*sv = rpc->srpc_scd->scd_svc;
@@ -1270,8 +1268,7 @@ sfw_handle_server_rpc(struct srpc_server_rpc *rpc)
 
 		if (sn != NULL &&
 		    sn->sn_features != request->msg_ses_feats) {
-			CNETERR("Features of framework RPC don't match "
-				"features of current session: %x/%x\n",
+			CNETERR("Features of framework RPC don't match features of current session: %x/%x\n",
 				request->msg_ses_feats, sn->sn_features);
 			reply->msg_body.reply.status = EPROTO;
 			reply->msg_body.reply.sid    = sn->sn_id;
@@ -1334,7 +1331,7 @@ sfw_handle_server_rpc(struct srpc_server_rpc *rpc)
 	return rc;
 }
 
-int
+static int
 sfw_bulk_ready(struct srpc_server_rpc *rpc, int status)
 {
 	struct srpc_service	*sv = rpc->srpc_scd->scd_svc;
@@ -1348,8 +1345,7 @@ sfw_bulk_ready(struct srpc_server_rpc *rpc, int status)
 	spin_lock(&sfw_data.fw_lock);
 
 	if (status != 0) {
-		CERROR("Bulk transfer failed for RPC: "
-		       "service %s, peer %s, status %d\n",
+		CERROR("Bulk transfer failed for RPC: service %s, peer %s, status %d\n",
 		       sv->sv_name, libcfs_id2str(rpc->srpc_peer), status);
 		spin_unlock(&sfw_data.fw_lock);
 		return -EIO;
@@ -1664,12 +1660,10 @@ sfw_startup (void)
 	}
 
 	if (session_timeout == 0)
-		CWARN ("Zero session_timeout specified "
-		       "- test sessions never expire.\n");
+		CWARN("Zero session_timeout specified - test sessions never expire.\n");
 
 	if (rpc_timeout == 0)
-		CWARN ("Zero rpc_timeout specified "
-		       "- test RPC never expire.\n");
+		CWARN("Zero rpc_timeout specified - test RPC never expire.\n");
 
 	memset(&sfw_data, 0, sizeof(struct smoketest_framework));
 
@@ -1727,8 +1721,7 @@ sfw_startup (void)
 
 		rc = srpc_service_add_buffers(sv, sv->sv_wi_total);
 		if (rc != 0) {
-			CWARN("Failed to reserve enough buffers: "
-			      "service %s, %d needed: %d\n",
+			CWARN("Failed to reserve enough buffers: service %s, %d needed: %d\n",
 			      sv->sv_name, sv->sv_wi_total, rc);
 			error = -ENOMEM;
 		}
diff --git a/drivers/staging/lustre/lnet/selftest/module.c b/drivers/staging/lustre/lnet/selftest/module.c
index 6dd4309dc5ea..c6ef5b0d8de1 100644
--- a/drivers/staging/lustre/lnet/selftest/module.c
+++ b/drivers/staging/lustre/lnet/selftest/module.c
@@ -55,7 +55,7 @@ static int lst_init_step = LST_INIT_NONE;
 struct cfs_wi_sched *lst_sched_serial;
 struct cfs_wi_sched **lst_sched_test;
 
-void
+static void
 lnet_selftest_fini(void)
 {
 	int	i;
@@ -90,18 +90,7 @@ lnet_selftest_fini(void)
 	return;
 }
 
-void
-lnet_selftest_structure_assertion(void)
-{
-	CLASSERT(sizeof(srpc_msg_t) == 160);
-	CLASSERT(sizeof(srpc_test_reqst_t) == 70);
-	CLASSERT(offsetof(srpc_msg_t, msg_body.tes_reqst.tsr_concur) == 72);
-	CLASSERT(offsetof(srpc_msg_t, msg_body.tes_reqst.tsr_ndest) == 78);
-	CLASSERT(sizeof(srpc_stat_reply_t) == 136);
-	CLASSERT(sizeof(srpc_stat_reqst_t) == 28);
-}
-
-int
+static int
 lnet_selftest_init(void)
 {
 	int	nscheds;
@@ -130,8 +119,8 @@ lnet_selftest_init(void)
 		rc = cfs_wi_sched_create("lst_t", lnet_cpt_table(), i,
 					 nthrs, &lst_sched_test[i]);
 		if (rc != 0) {
-			CERROR("Failed to create CPT affinity WI scheduler "
-			       "%d for LST\n", i);
+			CERROR("Failed to create CPT affinity WI scheduler %d for LST\n",
+			       i);
 			goto error;
 		}
 	}
diff --git a/drivers/staging/lustre/lnet/selftest/ping_test.c b/drivers/staging/lustre/lnet/selftest/ping_test.c
index 750cac4afbb2..d8c0df6e6852 100644
--- a/drivers/staging/lustre/lnet/selftest/ping_test.c
+++ b/drivers/staging/lustre/lnet/selftest/ping_test.c
@@ -44,7 +44,7 @@
 
 #define LST_PING_TEST_MAGIC     0xbabeface
 
-int ping_srv_workitems = SFW_TEST_WI_MAX;
+static int ping_srv_workitems = SFW_TEST_WI_MAX;
 module_param(ping_srv_workitems, int, 0644);
 MODULE_PARM_DESC(ping_srv_workitems, "# PING server workitems");
 
diff --git a/drivers/staging/lustre/lnet/selftest/rpc.c b/drivers/staging/lustre/lnet/selftest/rpc.c
index a9f29d8833a9..f753add7bfb3 100644
--- a/drivers/staging/lustre/lnet/selftest/rpc.c
+++ b/drivers/staging/lustre/lnet/selftest/rpc.c
@@ -87,7 +87,7 @@ void srpc_set_counters (const srpc_counters_t *cnt)
 	spin_unlock(&srpc_data.rpc_glock);
 }
 
-int
+static int
 srpc_add_bulk_page(srpc_bulk_t *bk, struct page *pg, int i, int nob)
 {
 	nob = min(nob, (int)PAGE_CACHE_SIZE);
@@ -170,7 +170,7 @@ srpc_next_id (void)
 	return id;
 }
 
-void
+static void
 srpc_init_server_rpc(struct srpc_server_rpc *rpc,
 		     struct srpc_service_cd *scd,
 		     struct srpc_buffer *buffer)
@@ -351,7 +351,7 @@ srpc_remove_service (srpc_service_t *sv)
 	return 0;
 }
 
-int
+static int
 srpc_post_passive_rdma(int portal, int local, __u64 matchbits, void *buf,
 		       int len, int options, lnet_process_id_t peer,
 		       lnet_handle_md_t *mdh, srpc_event_t *ev)
@@ -391,7 +391,7 @@ srpc_post_passive_rdma(int portal, int local, __u64 matchbits, void *buf,
 	return 0;
 }
 
-int
+static int
 srpc_post_active_rdma(int portal, __u64 matchbits, void *buf, int len,
 		      int options, lnet_process_id_t peer, lnet_nid_t self,
 		      lnet_handle_md_t *mdh, srpc_event_t *ev)
@@ -443,7 +443,7 @@ srpc_post_active_rdma(int portal, __u64 matchbits, void *buf, int len,
 	return 0;
 }
 
-int
+static int
 srpc_post_active_rqtbuf(lnet_process_id_t peer, int service, void *buf,
 			int len, lnet_handle_md_t *mdh, srpc_event_t *ev)
 {
@@ -452,7 +452,7 @@ srpc_post_active_rqtbuf(lnet_process_id_t peer, int service, void *buf,
 				     LNET_NID_ANY, mdh, ev);
 }
 
-int
+static int
 srpc_post_passive_rqtbuf(int service, int local, void *buf, int len,
 			 lnet_handle_md_t *mdh, srpc_event_t *ev)
 {
@@ -466,7 +466,7 @@ srpc_post_passive_rqtbuf(int service, int local, void *buf, int len,
 				      LNET_MD_OP_PUT, any, mdh, ev);
 }
 
-int
+static int
 srpc_service_post_buffer(struct srpc_service_cd *scd, struct srpc_buffer *buf)
 {
 	struct srpc_service	*sv = scd->scd_svc;
@@ -678,9 +678,7 @@ srpc_finish_service(struct srpc_service *sv)
 
 		rpc = list_entry(scd->scd_rpc_active.next,
 				     struct srpc_server_rpc, srpc_list);
-		CNETERR("Active RPC %p on shutdown: sv %s, peer %s, "
-			"wi %s scheduled %d running %d, "
-			"ev fired %d type %d status %d lnet %d\n",
+		CNETERR("Active RPC %p on shutdown: sv %s, peer %s, wi %s scheduled %d running %d, ev fired %d type %d status %d lnet %d\n",
 			rpc, sv->sv_name, libcfs_id2str(rpc->srpc_peer),
 			swi_state2str(rpc->srpc_wi.swi_state),
 			rpc->srpc_wi.swi_workitem.wi_scheduled,
@@ -697,7 +695,7 @@ srpc_finish_service(struct srpc_service *sv)
 }
 
 /* called with sv->sv_lock held */
-void
+static void
 srpc_service_recycle_buffer(struct srpc_service_cd *scd, srpc_buffer_t *buf)
 {
 	if (!scd->scd_svc->sv_shuttingdown && scd->scd_buf_adjust >= 0) {
@@ -787,7 +785,7 @@ srpc_shutdown_service(srpc_service_t *sv)
 	}
 }
 
-int
+static int
 srpc_send_request (srpc_client_rpc_t *rpc)
 {
 	srpc_event_t *ev = &rpc->crpc_reqstev;
@@ -807,7 +805,7 @@ srpc_send_request (srpc_client_rpc_t *rpc)
 	return rc;
 }
 
-int
+static int
 srpc_prepare_reply (srpc_client_rpc_t *rpc)
 {
 	srpc_event_t *ev = &rpc->crpc_replyev;
@@ -831,7 +829,7 @@ srpc_prepare_reply (srpc_client_rpc_t *rpc)
 	return rc;
 }
 
-int
+static int
 srpc_prepare_bulk (srpc_client_rpc_t *rpc)
 {
 	srpc_bulk_t  *bk = &rpc->crpc_bulk;
@@ -863,7 +861,7 @@ srpc_prepare_bulk (srpc_client_rpc_t *rpc)
 	return rc;
 }
 
-int
+static int
 srpc_do_bulk (srpc_server_rpc_t *rpc)
 {
 	srpc_event_t  *ev = &rpc->srpc_ev;
@@ -891,7 +889,7 @@ srpc_do_bulk (srpc_server_rpc_t *rpc)
 }
 
 /* only called from srpc_handle_rpc */
-void
+static void
 srpc_server_rpc_done(srpc_server_rpc_t *rpc, int status)
 {
 	struct srpc_service_cd	*scd = rpc->srpc_scd;
@@ -1066,7 +1064,7 @@ srpc_handle_rpc(swi_workitem_t *wi)
 	return 0;
 }
 
-void
+static void
 srpc_client_rpc_expired (void *data)
 {
 	srpc_client_rpc_t *rpc = data;
@@ -1108,7 +1106,7 @@ srpc_add_client_rpc_timer (srpc_client_rpc_t *rpc)
  *
  * Upon exit the RPC expiry timer is not queued and the handler is not
  * running on any CPU. */
-void
+static void
 srpc_del_client_rpc_timer (srpc_client_rpc_t *rpc)
 {
 	/* timer not planted or already exploded */
@@ -1129,7 +1127,7 @@ srpc_del_client_rpc_timer (srpc_client_rpc_t *rpc)
 	}
 }
 
-void
+static void
 srpc_client_rpc_done (srpc_client_rpc_t *rpc, int status)
 {
 	swi_workitem_t *wi = &rpc->crpc_wi;
@@ -1236,20 +1234,18 @@ srpc_send_rpc (swi_workitem_t *wi)
 		if (reply->msg_type != type ||
 		    (reply->msg_magic != SRPC_MSG_MAGIC &&
 		     reply->msg_magic != __swab32(SRPC_MSG_MAGIC))) {
-			CWARN ("Bad message from %s: type %u (%d expected),"
-			       " magic %u (%d expected).\n",
-			       libcfs_id2str(rpc->crpc_dest),
-			       reply->msg_type, type,
-			       reply->msg_magic, SRPC_MSG_MAGIC);
+			CWARN("Bad message from %s: type %u (%d expected), magic %u (%d expected).\n",
+			      libcfs_id2str(rpc->crpc_dest),
+			      reply->msg_type, type,
+			      reply->msg_magic, SRPC_MSG_MAGIC);
 			rc = -EBADMSG;
 			break;
 		}
 
 		if (do_bulk && reply->msg_body.reply.status != 0) {
-			CWARN ("Remote error %d at %s, unlink bulk buffer in "
-			       "case peer didn't initiate bulk transfer\n",
-			       reply->msg_body.reply.status,
-			       libcfs_id2str(rpc->crpc_dest));
+			CWARN("Remote error %d at %s, unlink bulk buffer in case peer didn't initiate bulk transfer\n",
+			      reply->msg_body.reply.status,
+			      libcfs_id2str(rpc->crpc_dest));
 			LNetMDUnlink(rpc->crpc_bulk.bk_mdh);
 		}
 
@@ -1393,7 +1389,7 @@ srpc_send_reply(struct srpc_server_rpc *rpc)
 }
 
 /* when in kernel always called with LNET_LOCK() held, and in thread context */
-void
+static void
 srpc_lnet_ev_handler(lnet_event_t *ev)
 {
 	struct srpc_service_cd	*scd;
@@ -1504,11 +1500,10 @@ srpc_lnet_ev_handler(lnet_event_t *ev)
 		     msg->msg_type != __swab32(type)) ||
 		    (msg->msg_magic != SRPC_MSG_MAGIC &&
 		     msg->msg_magic != __swab32(SRPC_MSG_MAGIC))) {
-			CERROR ("Dropping RPC (%s) from %s: "
-				"status %d mlength %d type %u magic %u.\n",
-				sv->sv_name, libcfs_id2str(ev->initiator),
-				ev->status, ev->mlength,
-				msg->msg_type, msg->msg_magic);
+			CERROR("Dropping RPC (%s) from %s: status %d mlength %d type %u magic %u.\n",
+			       sv->sv_name, libcfs_id2str(ev->initiator),
+			       ev->status, ev->mlength,
+			       msg->msg_type, msg->msg_magic);
 
 			/* NB can't call srpc_service_recycle_buffer here since
 			 * it may call LNetM[DE]Attach. The invalid magic tells
diff --git a/drivers/staging/lustre/lnet/selftest/timer.c b/drivers/staging/lustre/lnet/selftest/timer.c
index 91d4caa4edb0..f8352c2a7d37 100644
--- a/drivers/staging/lustre/lnet/selftest/timer.c
+++ b/drivers/staging/lustre/lnet/selftest/timer.c
@@ -121,7 +121,7 @@ stt_del_timer(stt_timer_t *timer)
 }
 
 /* called with stt_data.stt_lock held */
-int
+static int
 stt_expire_list(struct list_head *slot, unsigned long now)
 {
 	int	  expired = 0;
@@ -145,7 +145,7 @@ stt_expire_list(struct list_head *slot, unsigned long now)
 	return expired;
 }
 
-int
+static int
 stt_check_timers(unsigned long *last)
 {
 	int	expired = 0;
@@ -168,7 +168,7 @@ stt_check_timers(unsigned long *last)
 }
 
 
-int
+static int
 stt_timer_main(void *arg)
 {
 	cfs_block_allsigs();
@@ -187,7 +187,7 @@ stt_timer_main(void *arg)
 	return 0;
 }
 
-int
+static int
 stt_start_timer_thread(void)
 {
 	struct task_struct *task;
diff --git a/drivers/staging/lustre/lustre/Kconfig b/drivers/staging/lustre/lustre/Kconfig
index 4f65ba1158bf..6725467ef4d0 100644
--- a/drivers/staging/lustre/lustre/Kconfig
+++ b/drivers/staging/lustre/lustre/Kconfig
@@ -57,5 +57,5 @@ config LUSTRE_TRANSLATE_ERRNOS
 config LUSTRE_LLITE_LLOOP
 	tristate "Lustre virtual block device"
 	depends on LUSTRE_FS && BLOCK
-	depends on !PPC_64K_PAGES && !ARM64_64K_PAGES
+	depends on !PPC_64K_PAGES && !ARM64_64K_PAGES && !MICROBLAZE_64K_PAGES && !PAGE_SIZE_64KB && !IA64_PAGE_SIZE_64KB && !PARISC_PAGE_SIZE_64KB
 	default m
diff --git a/drivers/staging/lustre/lustre/include/dt_object.h b/drivers/staging/lustre/lustre/include/dt_object.h
index 212ebaea8555..be4c7d95e788 100644
--- a/drivers/staging/lustre/lustre/include/dt_object.h
+++ b/drivers/staging/lustre/lustre/include/dt_object.h
@@ -617,7 +617,7 @@ struct dt_index_operations {
 		int	   (*load)(const struct lu_env *env,
 				      const struct dt_it *di, __u64 hash);
 		int	(*key_rec)(const struct lu_env *env,
-				      const struct dt_it *di, void* key_rec);
+				      const struct dt_it *di, void *key_rec);
 	} dio_it;
 };
 
@@ -667,7 +667,7 @@ static inline int lu_device_is_dt(const struct lu_device *d)
 	return ergo(d != NULL, d->ld_type->ldt_tags & LU_DEVICE_DT);
 }
 
-static inline struct dt_device * lu2dt_dev(struct lu_device *l)
+static inline struct dt_device *lu2dt_dev(struct lu_device *l)
 {
 	LASSERT(lu_device_is_dt(l));
 	return container_of0(l, struct dt_device, dd_lu_dev);
diff --git a/drivers/staging/lustre/lustre/include/linux/lustre_compat25.h b/drivers/staging/lustre/lustre/include/linux/lustre_compat25.h
index e94ab343ab25..3925db160650 100644
--- a/drivers/staging/lustre/lustre/include/linux/lustre_compat25.h
+++ b/drivers/staging/lustre/lustre/include/linux/lustre_compat25.h
@@ -42,28 +42,6 @@
 
 #include "lustre_patchless_compat.h"
 
-# define LOCK_FS_STRUCT(fs)	spin_lock(&(fs)->lock)
-# define UNLOCK_FS_STRUCT(fs)	spin_unlock(&(fs)->lock)
-
-static inline void ll_set_fs_pwd(struct fs_struct *fs, struct vfsmount *mnt,
-				 struct dentry *dentry)
-{
-	struct path path;
-	struct path old_pwd;
-
-	path.mnt = mnt;
-	path.dentry = dentry;
-	LOCK_FS_STRUCT(fs);
-	old_pwd = fs->pwd;
-	path_get(&path);
-	fs->pwd = path;
-	UNLOCK_FS_STRUCT(fs);
-
-	if (old_pwd.dentry)
-		path_put(&old_pwd);
-}
-
-
 /*
  * set ATTR_BLOCKS to a high value to avoid any risk of collision with other
  * ATTR_* attributes (see bug 13828)
@@ -91,8 +69,6 @@ static inline void ll_set_fs_pwd(struct fs_struct *fs, struct vfsmount *mnt,
 # define inode_dio_read(i)		atomic_inc(&(i)->i_dio_count)
 /* inode_dio_done(i) use as-is for read unlock */
 
-#define TREE_READ_LOCK_IRQ(mapping)	spin_lock_irq(&(mapping)->tree_lock)
-#define TREE_READ_UNLOCK_IRQ(mapping)	spin_unlock_irq(&(mapping)->tree_lock)
 
 #ifndef FS_HAS_FIEMAP
 #define FS_HAS_FIEMAP			(0)
@@ -112,8 +88,6 @@ static inline void ll_set_fs_pwd(struct fs_struct *fs, struct vfsmount *mnt,
 #define cfs_bio_io_error(a, b)   bio_io_error((a))
 #define cfs_bio_endio(a, b, c)    bio_endio((a), (c))
 
-#define cfs_fs_pwd(fs)       ((fs)->pwd.dentry)
-#define cfs_fs_mnt(fs)       ((fs)->pwd.mnt)
 #define cfs_path_put(nd)     path_put(&(nd)->path)
 
 
@@ -139,8 +113,7 @@ ll_quota_on(struct super_block *sb, int off, int ver, char *name, int remount)
 					   );
 		path_put(&path);
 		return rc;
-	}
-	else
+	} else
 		return -ENOSYS;
 }
 
@@ -149,8 +122,7 @@ static inline int ll_quota_off(struct super_block *sb, int off, int remount)
 	if (sb->s_qcop->quota_off) {
 		return sb->s_qcop->quota_off(sb, off
 					    );
-	}
-	else
+	} else
 		return -ENOSYS;
 }
 
diff --git a/drivers/staging/lustre/lustre/include/linux/obd.h b/drivers/staging/lustre/lustre/include/linux/obd.h
index 9d7e28ace42d..9cd8683573ce 100644
--- a/drivers/staging/lustre/lustre/include/linux/obd.h
+++ b/drivers/staging/lustre/lustre/include/linux/obd.h
@@ -87,8 +87,7 @@ static inline void __client_obd_list_lock(client_obd_lock_t *lock,
 			if (task == NULL)
 				continue;
 
-			LCONSOLE_WARN("%s:%d: lock %p was acquired"
-				      " by <%s:%d:%s:%d> for %lu seconds.\n",
+			LCONSOLE_WARN("%s:%d: lock %p was acquired by <%s:%d:%s:%d> for %lu seconds.\n",
 				      current->comm, current->pid,
 				      lock, task->comm, task->pid,
 				      lock->func, lock->line,
diff --git a/drivers/staging/lustre/lustre/include/lprocfs_status.h b/drivers/staging/lustre/lustre/include/lprocfs_status.h
index ccb6cd42a67d..cfe503b7df62 100644
--- a/drivers/staging/lustre/lustre/include/lprocfs_status.h
+++ b/drivers/staging/lustre/lustre/include/lprocfs_status.h
@@ -374,8 +374,8 @@ static inline void s2dhms(struct dhms *ts, time_t secs)
 #define JOBSTATS_PROCNAME_UID		"procname_uid"
 #define JOBSTATS_NODELOCAL		"nodelocal"
 
-extern int lprocfs_write_frac_helper(const char *buffer, unsigned long count,
-				     int *val, int mult);
+extern int lprocfs_write_frac_helper(const char __user *buffer,
+				     unsigned long count, int *val, int mult);
 extern int lprocfs_read_frac_helper(char *buffer, unsigned long count,
 				    long val, int mult);
 #if defined (CONFIG_PROC_FS)
@@ -557,7 +557,7 @@ extern void lprocfs_free_obd_stats(struct obd_device *obddev);
 extern void lprocfs_free_md_stats(struct obd_device *obddev);
 struct obd_export;
 struct nid_stat;
-extern int lprocfs_add_clear_entry(struct obd_device * obd,
+extern int lprocfs_add_clear_entry(struct obd_device *obd,
 				   struct proc_dir_entry *entry);
 extern int lprocfs_exp_setup(struct obd_export *exp,
 			     lnet_nid_t *peer_nid, int *newnid);
@@ -647,7 +647,7 @@ extern int lprocfs_rd_kbytesavail(struct seq_file *m, void *data);
 extern int lprocfs_rd_filestotal(struct seq_file *m, void *data);
 extern int lprocfs_rd_filesfree(struct seq_file *m, void *data);
 
-extern int lprocfs_write_helper(const char *buffer, unsigned long count,
+extern int lprocfs_write_helper(const char __user *buffer, unsigned long count,
 				int *val);
 extern int lprocfs_seq_read_frac_helper(struct seq_file *m, long val, int mult);
 extern int lprocfs_write_u64_helper(const char *buffer, unsigned long count,
diff --git a/drivers/staging/lustre/lustre/include/lu_object.h b/drivers/staging/lustre/lustre/include/lu_object.h
index 6015ee5c4b64..2ddb2b054d8d 100644
--- a/drivers/staging/lustre/lustre/include/lu_object.h
+++ b/drivers/staging/lustre/lustre/include/lu_object.h
@@ -1120,7 +1120,7 @@ struct lu_context_key {
 };
 
 #define LU_KEY_INIT(mod, type)				    \
-	static void* mod##_key_init(const struct lu_context *ctx, \
+	static void *mod##_key_init(const struct lu_context *ctx, \
 				    struct lu_context_key *key)   \
 	{							 \
 		type *value;				      \
@@ -1137,7 +1137,7 @@ struct lu_context_key {
 
 #define LU_KEY_FINI(mod, type)					      \
 	static void mod##_key_fini(const struct lu_context *ctx,	    \
-				    struct lu_context_key *key, void* data) \
+				    struct lu_context_key *key, void *data) \
 	{								   \
 		type *info = data;					  \
 									    \
diff --git a/drivers/staging/lustre/lustre/include/lustre_capa.h b/drivers/staging/lustre/lustre/include/lustre_capa.h
index ab6b9ea98a70..fe19534ebd8f 100644
--- a/drivers/staging/lustre/lustre/include/lustre_capa.h
+++ b/drivers/staging/lustre/lustre/include/lustre_capa.h
@@ -154,7 +154,7 @@ static inline __u32 capa_expiry(struct lustre_capa *capa)
 }
 
 void _debug_capa(struct lustre_capa *, struct libcfs_debug_msg_data *,
-		 const char *fmt, ... );
+		 const char *fmt, ...);
 #define DEBUG_CAPA(level, capa, fmt, args...)				  \
 do {									   \
 	if (((level) & D_CANTMASK) != 0 ||				     \
diff --git a/drivers/staging/lustre/lustre/include/lustre_disk.h b/drivers/staging/lustre/lustre/include/lustre_disk.h
index 515b835ce14d..9b2833131744 100644
--- a/drivers/staging/lustre/lustre/include/lustre_disk.h
+++ b/drivers/staging/lustre/lustre/include/lustre_disk.h
@@ -368,8 +368,7 @@ static inline void check_lcd(char *obd_name, int index,
 	if (strnlen((char*)lcd->lcd_uuid, length) == length) {
 		lcd->lcd_uuid[length - 1] = '\0';
 
-		LCONSOLE_ERROR("the client UUID (%s) on %s for exports"
-			       "stored in last_rcvd(index = %d) is bad!\n",
+		LCONSOLE_ERROR("the client UUID (%s) on %s for exports stored in last_rcvd(index = %d) is bad!\n",
 			       lcd->lcd_uuid, obd_name, index);
 	}
 }
diff --git a/drivers/staging/lustre/lustre/include/lustre_dlm.h b/drivers/staging/lustre/lustre/include/lustre_dlm.h
index 14ac46f45fd1..83bc0a9d7d4c 100644
--- a/drivers/staging/lustre/lustre/include/lustre_dlm.h
+++ b/drivers/staging/lustre/lustre/include/lustre_dlm.h
@@ -1446,7 +1446,7 @@ static inline void check_res_locked(struct ldlm_resource *res)
 	assert_spin_locked(&res->lr_lock);
 }
 
-struct ldlm_resource * lock_res_and_lock(struct ldlm_lock *lock);
+struct ldlm_resource *lock_res_and_lock(struct ldlm_lock *lock);
 void unlock_res_and_lock(struct ldlm_lock *lock);
 
 /* ldlm_pool.c */
diff --git a/drivers/staging/lustre/lustre/include/lustre_eacl.h b/drivers/staging/lustre/lustre/include/lustre_eacl.h
index b94f76a3301b..0f8f76c43ee1 100644
--- a/drivers/staging/lustre/lustre/include/lustre_eacl.h
+++ b/drivers/staging/lustre/lustre/include/lustre_eacl.h
@@ -74,7 +74,7 @@ typedef struct {
 extern ext_acl_xattr_header *
 lustre_posix_acl_xattr_2ext(posix_acl_xattr_header *header, int size);
 extern int
-lustre_posix_acl_xattr_filter(posix_acl_xattr_header *header, int size,
+lustre_posix_acl_xattr_filter(posix_acl_xattr_header *header, size_t size,
 			      posix_acl_xattr_header **out);
 extern void
 lustre_posix_acl_xattr_free(posix_acl_xattr_header *header, int size);
diff --git a/drivers/staging/lustre/lustre/include/lustre_lib.h b/drivers/staging/lustre/lustre/include/lustre_lib.h
index 12c7590e61fa..bf135630c39a 100644
--- a/drivers/staging/lustre/lustre/include/lustre_lib.h
+++ b/drivers/staging/lustre/lustre/include/lustre_lib.h
@@ -85,7 +85,7 @@ void target_send_reply(struct ptlrpc_request *req, int rc, int fail_id);
 
 /* client.c */
 
-int client_sanobd_setup(struct obd_device *obddev, struct lustre_cfg* lcfg);
+int client_sanobd_setup(struct obd_device *obddev, struct lustre_cfg *lcfg);
 struct client_obd *client_conn2cli(struct lustre_handle *conn);
 
 struct md_open_data;
diff --git a/drivers/staging/lustre/lustre/include/lustre_net.h b/drivers/staging/lustre/lustre/include/lustre_net.h
index 0a024d3cfeb7..36396d1c94dc 100644
--- a/drivers/staging/lustre/lustre/include/lustre_net.h
+++ b/drivers/staging/lustre/lustre/include/lustre_net.h
@@ -2627,12 +2627,7 @@ __u32 lustre_msg_get_timeout(struct lustre_msg *msg);
 __u32 lustre_msg_get_service_time(struct lustre_msg *msg);
 char *lustre_msg_get_jobid(struct lustre_msg *msg);
 __u32 lustre_msg_get_cksum(struct lustre_msg *msg);
-#if LUSTRE_VERSION_CODE < OBD_OCD_VERSION(2, 7, 50, 0)
-__u32 lustre_msg_calc_cksum(struct lustre_msg *msg, int compat18);
-#else
-# warning "remove checksum compatibility support for b1_8"
 __u32 lustre_msg_calc_cksum(struct lustre_msg *msg);
-#endif
 void lustre_msg_set_handle(struct lustre_msg *msg,
 			   struct lustre_handle *handle);
 void lustre_msg_set_type(struct lustre_msg *msg, __u32 type);
@@ -2951,7 +2946,7 @@ void ptlrpcd_decref(void);
  * procfs output related functions
  * @{
  */
-const char* ll_opcode2str(__u32 opcode);
+const char *ll_opcode2str(__u32 opcode);
 #if defined (CONFIG_PROC_FS)
 void ptlrpc_lprocfs_register_obd(struct obd_device *obd);
 void ptlrpc_lprocfs_unregister_obd(struct obd_device *obd);
diff --git a/drivers/staging/lustre/lustre/include/obd_class.h b/drivers/staging/lustre/lustre/include/obd_class.h
index 882e40bd584c..4a29261c514d 100644
--- a/drivers/staging/lustre/lustre/include/obd_class.h
+++ b/drivers/staging/lustre/lustre/include/obd_class.h
@@ -414,7 +414,7 @@ do {							    \
 #define EXP_MD_COUNTER_INCREMENT(exp, op)
 #endif
 
-static inline int lprocfs_nid_ldlm_stats_init(struct nid_stat* tmp)
+static inline int lprocfs_nid_ldlm_stats_init(struct nid_stat *tmp)
 {
 	/* Always add in ldlm_stats */
 	tmp->nid_ldlm_stats = lprocfs_alloc_stats(LDLM_LAST_OPC - LDLM_FIRST_OPC
diff --git a/drivers/staging/lustre/lustre/ldlm/interval_tree.c b/drivers/staging/lustre/lustre/ldlm/interval_tree.c
index a3d7a7292417..eab2bd60241b 100644
--- a/drivers/staging/lustre/lustre/ldlm/interval_tree.c
+++ b/drivers/staging/lustre/lustre/ldlm/interval_tree.c
@@ -73,6 +73,7 @@ static inline int extent_compare(struct interval_node_extent *e1,
 				 struct interval_node_extent *e2)
 {
 	int rc;
+
 	if (e1->start == e2->start) {
 		if (e1->end < e2->end)
 			rc = -1;
@@ -321,6 +322,7 @@ static void interval_insert_color(struct interval_node *node,
 		/* Parent is RED, so gparent must not be NULL */
 		if (node_is_left_child(parent)) {
 			struct interval_node *uncle;
+
 			uncle = gparent->in_right;
 			if (uncle && node_is_red(uncle)) {
 				uncle->in_color = INTERVAL_BLACK;
@@ -340,6 +342,7 @@ static void interval_insert_color(struct interval_node *node,
 			__rotate_right(gparent, root);
 		} else {
 			struct interval_node *uncle;
+
 			uncle = gparent->in_left;
 			if (uncle && node_is_red(uncle)) {
 				uncle->in_color = INTERVAL_BLACK;
@@ -427,6 +430,7 @@ static void interval_erase_color(struct interval_node *node,
 			} else {
 				if (node_is_black_or_0(tmp->in_right)) {
 					struct interval_node *o_left;
+
 					o_left = tmp->in_left;
 					if (o_left)
 						o_left->in_color = INTERVAL_BLACK;
@@ -458,6 +462,7 @@ static void interval_erase_color(struct interval_node *node,
 			} else {
 				if (node_is_black_or_0(tmp->in_left)) {
 					struct interval_node *o_right;
+
 					o_right = tmp->in_right;
 					if (o_right)
 						o_right->in_color = INTERVAL_BLACK;
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c b/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c
index 0c09b611f4a6..a89eebaedabf 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c
@@ -182,7 +182,9 @@ void ldlm_extent_add_lock(struct ldlm_resource *res,
 	root = &res->lr_itree[idx].lit_root;
 	found = interval_insert(&node->li_node, root);
 	if (found) { /* The policy group found. */
-		struct ldlm_interval *tmp = ldlm_interval_detach(lock);
+		struct ldlm_interval *tmp;
+
+		tmp = ldlm_interval_detach(lock);
 		LASSERT(tmp != NULL);
 		ldlm_interval_free(tmp);
 		ldlm_interval_attach(to_ldlm_interval(found), lock);
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_flock.c b/drivers/staging/lustre/lustre/ldlm/ldlm_flock.c
index b798daa094bc..a4c252febfe4 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_flock.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_flock.c
@@ -260,7 +260,8 @@ ldlm_process_flock_lock(struct ldlm_lock *req, __u64 *flags, int first_enq,
 	int splitted = 0;
 	const struct ldlm_callback_suite null_cbs = { NULL };
 
-	CDEBUG(D_DLMTRACE, "flags %#llx owner %llu pid %u mode %u start %llu end %llu\n",
+	CDEBUG(D_DLMTRACE,
+	       "flags %#llx owner %llu pid %u mode %u start %llu end %llu\n",
 	       *flags, new->l_policy_data.l_flock.owner,
 	       new->l_policy_data.l_flock.pid, mode,
 	       req->l_policy_data.l_flock.start,
@@ -291,6 +292,7 @@ reprocess:
 		}
 	} else {
 		int reprocess_failed = 0;
+
 		lockmode_verify(mode);
 
 		/* This loop determines if there are existing locks
@@ -496,7 +498,8 @@ reprocess:
 			new->l_policy_data.l_flock.end + 1;
 		new2->l_conn_export = lock->l_conn_export;
 		if (lock->l_export != NULL) {
-			new2->l_export = class_export_lock_get(lock->l_export, new2);
+			new2->l_export = class_export_lock_get(lock->l_export,
+							       new2);
 			if (new2->l_export->exp_lock_hash &&
 			    hlist_unhashed(&new2->l_exp_hash))
 				cfs_hash_add(new2->l_export->exp_lock_hash,
@@ -619,8 +622,7 @@ ldlm_flock_completion_ast(struct ldlm_lock *lock, __u64 flags, void *data)
 		return 0;
 	}
 
-	LDLM_DEBUG(lock, "client-side enqueue returned a blocked lock, "
-		   "sleeping");
+	LDLM_DEBUG(lock, "client-side enqueue returned a blocked lock, sleeping");
 	fwd.fwd_lock = lock;
 	obd = class_exp2obd(lock->l_conn_export);
 
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h b/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
index f997566ef11e..6c6c57ca91de 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
@@ -91,7 +91,8 @@ static inline int ldlm_ns_empty(struct ldlm_namespace *ns)
 }
 
 void ldlm_namespace_move_to_active_locked(struct ldlm_namespace *, ldlm_side_t);
-void ldlm_namespace_move_to_inactive_locked(struct ldlm_namespace *, ldlm_side_t);
+void ldlm_namespace_move_to_inactive_locked(struct ldlm_namespace *,
+					    ldlm_side_t);
 struct ldlm_namespace *ldlm_namespace_first_locked(ldlm_side_t);
 
 /* ldlm_request.c */
@@ -214,6 +215,7 @@ static inline struct ldlm_extent *
 ldlm_interval_extent(struct ldlm_interval *node)
 {
 	struct ldlm_lock *lock;
+
 	LASSERT(!list_empty(&node->li_group));
 
 	lock = list_entry(node->li_group.next, struct ldlm_lock,
@@ -244,7 +246,7 @@ typedef enum ldlm_policy_res ldlm_policy_res_t;
 									    \
 		return lprocfs_rd_uint(m, &tmp);			    \
 	}								    \
-	struct __##var##__dummy_read {;} /* semicolon catcher */
+	struct __##var##__dummy_read {; } /* semicolon catcher */
 
 #define LDLM_POOL_PROC_WRITER(var, type)				    \
 	static int lprocfs_wr_##var(struct file *file, const char *buffer,  \
@@ -266,7 +268,7 @@ typedef enum ldlm_policy_res ldlm_policy_res_t;
 									    \
 		return rc;						    \
 	}								    \
-	struct __##var##__dummy_write {;} /* semicolon catcher */
+	struct __##var##__dummy_write {; } /* semicolon catcher */
 
 static inline int is_granted_or_cancelled(struct ldlm_lock *lock)
 {
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
index c21e30a074b9..c5c86e73ca52 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
@@ -603,7 +603,8 @@ int client_disconnect_export(struct obd_export *exp)
 		/* obd_force == local only */
 		ldlm_cli_cancel_unused(obd->obd_namespace, NULL,
 				       obd->obd_force ? LCF_LOCAL : 0, NULL);
-		ldlm_namespace_free_prior(obd->obd_namespace, imp, obd->obd_force);
+		ldlm_namespace_free_prior(obd->obd_namespace, imp,
+					  obd->obd_force);
 	}
 
 	/* There's no need to hold sem while disconnecting an import,
@@ -858,8 +859,8 @@ void ldlm_dump_export_locks(struct obd_export *exp)
 	if (!list_empty(&exp->exp_locks_list)) {
 		struct ldlm_lock *lock;
 
-		CERROR("dumping locks for export %p,"
-		       "ignore if the unmount doesn't hang\n", exp);
+		CERROR("dumping locks for export %p,ignore if the unmount doesn't hang\n",
+		       exp);
 		list_for_each_entry(lock, &exp->exp_locks_list,
 					l_exp_refs_link)
 			LDLM_ERROR(lock, "lock:");
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
index 6140130b6056..8191005464b1 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
@@ -226,6 +226,7 @@ EXPORT_SYMBOL(ldlm_lock_put);
 int ldlm_lock_remove_from_lru_nolock(struct ldlm_lock *lock)
 {
 	int rc = 0;
+
 	if (!list_empty(&lock->l_lru)) {
 		struct ldlm_namespace *ns = ldlm_lock_to_ns(lock);
 
@@ -575,7 +576,7 @@ struct ldlm_lock *__ldlm_handle2lock(const struct lustre_handle *handle,
 
 	/* It's unlikely but possible that someone marked the lock as
 	 * destroyed after we did handle2object on it */
-	if (flags == 0 && ((lock->l_flags & LDLM_FL_DESTROYED)== 0)) {
+	if (flags == 0 && ((lock->l_flags & LDLM_FL_DESTROYED) == 0)) {
 		lu_ref_add(&lock->l_reference, "handle", current);
 		return lock;
 	}
@@ -811,8 +812,7 @@ void ldlm_lock_decref_internal(struct ldlm_lock *lock, __u32 mode)
 		/* If we received a blocked AST and this was the last reference,
 		 * run the callback. */
 		if ((lock->l_flags & LDLM_FL_NS_SRV) && lock->l_export)
-			CERROR("FL_CBPENDING set on non-local lock--just a "
-			       "warning\n");
+			CERROR("FL_CBPENDING set on non-local lock--just a warning\n");
 
 		LDLM_DEBUG(lock, "final decref done on cbpending lock");
 
@@ -859,6 +859,7 @@ void ldlm_lock_decref_internal(struct ldlm_lock *lock, __u32 mode)
 void ldlm_lock_decref(struct lustre_handle *lockh, __u32 mode)
 {
 	struct ldlm_lock *lock = __ldlm_handle2lock(lockh, 0);
+
 	LASSERTF(lock != NULL, "Non-existing lock: %#llx\n", lockh->cookie);
 	ldlm_lock_decref_internal(lock, mode);
 	LDLM_LOCK_PUT(lock);
@@ -981,7 +982,6 @@ static void search_granted_lock(struct list_head *queue,
 	prev->res_link = queue->prev;
 	prev->mode_link = &req->l_sl_mode;
 	prev->policy_link = &req->l_sl_policy;
-	return;
 }
 
 /**
@@ -1287,6 +1287,7 @@ ldlm_mode_t ldlm_lock_match(struct ldlm_namespace *ns, __u64 flags,
 			__u64 wait_flags = LDLM_FL_LVB_READY |
 				LDLM_FL_DESTROYED | LDLM_FL_FAIL_NOTIFIED;
 			struct l_wait_info lwi;
+
 			if (lock->l_completion_ast) {
 				int err = lock->l_completion_ast(lock,
 							  LDLM_FL_WAIT_NOREPROC,
@@ -1340,9 +1341,10 @@ ldlm_mode_t ldlm_lock_match(struct ldlm_namespace *ns, __u64 flags,
 
 	} else if (!(flags & LDLM_FL_TEST_LOCK)) {/*less verbose for test-only*/
 		LDLM_DEBUG_NOLOCK("not matched ns %p type %u mode %u res %llu/%llu (%llu %llu)",
-				  ns, type, mode, res_id->name[0], res_id->name[1],
+				  ns, type, mode, res_id->name[0],
+				  res_id->name[1],
 				  (type == LDLM_PLAIN || type == LDLM_IBITS) ?
-					res_id->name[2] :policy->l_extent.start,
+					res_id->name[2] : policy->l_extent.start,
 				  (type == LDLM_PLAIN || type == LDLM_IBITS) ?
 					res_id->name[3] : policy->l_extent.end);
 	}
@@ -1453,7 +1455,8 @@ int ldlm_fill_lvb(struct ldlm_lock *lock, struct req_capsule *pill,
 
 			memcpy(data, lvb, size);
 		} else {
-			LDLM_ERROR(lock, "Replied unexpected lquota LVB size %d",
+			LDLM_ERROR(lock,
+				   "Replied unexpected lquota LVB size %d",
 				   size);
 			return -EINVAL;
 		}
@@ -1641,8 +1644,7 @@ ldlm_error_t ldlm_lock_enqueue(struct ldlm_namespace *ns,
 			ldlm_grant_lock(lock, NULL);
 		goto out;
 	} else {
-		CERROR("This is client-side-only module, cannot handle "
-		       "LDLM_NAMESPACE_SERVER resource type lock.\n");
+		CERROR("This is client-side-only module, cannot handle LDLM_NAMESPACE_SERVER resource type lock.\n");
 		LBUG();
 	}
 
@@ -1820,24 +1822,24 @@ int ldlm_run_ast_work(struct ldlm_namespace *ns, struct list_head *rpc_list,
 	arg->list = rpc_list;
 
 	switch (ast_type) {
-		case LDLM_WORK_BL_AST:
-			arg->type = LDLM_BL_CALLBACK;
-			work_ast_lock = ldlm_work_bl_ast_lock;
-			break;
-		case LDLM_WORK_CP_AST:
-			arg->type = LDLM_CP_CALLBACK;
-			work_ast_lock = ldlm_work_cp_ast_lock;
-			break;
-		case LDLM_WORK_REVOKE_AST:
-			arg->type = LDLM_BL_CALLBACK;
-			work_ast_lock = ldlm_work_revoke_ast_lock;
-			break;
-		case LDLM_WORK_GL_AST:
-			arg->type = LDLM_GL_CALLBACK;
-			work_ast_lock = ldlm_work_gl_ast_lock;
-			break;
-		default:
-			LBUG();
+	case LDLM_WORK_BL_AST:
+		arg->type = LDLM_BL_CALLBACK;
+		work_ast_lock = ldlm_work_bl_ast_lock;
+		break;
+	case LDLM_WORK_CP_AST:
+		arg->type = LDLM_CP_CALLBACK;
+		work_ast_lock = ldlm_work_cp_ast_lock;
+		break;
+	case LDLM_WORK_REVOKE_AST:
+		arg->type = LDLM_BL_CALLBACK;
+		work_ast_lock = ldlm_work_revoke_ast_lock;
+		break;
+	case LDLM_WORK_GL_AST:
+		arg->type = LDLM_GL_CALLBACK;
+		work_ast_lock = ldlm_work_gl_ast_lock;
+		break;
+	default:
+		LBUG();
 	}
 
 	/* We create a ptlrpc request set with flow control extension.
@@ -1904,8 +1906,7 @@ void ldlm_reprocess_all(struct ldlm_resource *res)
 	LIST_HEAD(rpc_list);
 
 	if (!ns_is_client(ldlm_res_to_ns(res))) {
-		CERROR("This is client-side-only module, cannot handle "
-		       "LDLM_NAMESPACE_SERVER resource type lock.\n");
+		CERROR("This is client-side-only module, cannot handle LDLM_NAMESPACE_SERVER resource type lock.\n");
 		LBUG();
 	}
 }
@@ -2038,8 +2039,7 @@ int ldlm_cancel_locks_for_export_cb(struct cfs_hash *hs, struct cfs_hash_bd *bd,
 	ecl->ecl_loop++;
 	if ((ecl->ecl_loop & -ecl->ecl_loop) == ecl->ecl_loop) {
 		CDEBUG(D_INFO,
-		       "Cancel lock %p for export %p (loop %d), still have "
-		       "%d locks left on hash table.\n",
+		       "Cancel lock %p for export %p (loop %d), still have %d locks left on hash table.\n",
 		       lock, exp, ecl->ecl_loop,
 		       atomic_read(&hs->hs_count));
 	}
@@ -2169,8 +2169,7 @@ struct ldlm_resource *ldlm_lock_convert(struct ldlm_lock *lock, int new_mode,
 				lock->l_completion_ast(lock, 0, NULL);
 		}
 	} else {
-		CERROR("This is client-side-only module, cannot handle "
-		       "LDLM_NAMESPACE_SERVER resource type lock.\n");
+		CERROR("This is client-side-only module, cannot handle LDLM_NAMESPACE_SERVER resource type lock.\n");
 		LBUG();
 	}
 	unlock_res_and_lock(lock);
@@ -2224,23 +2223,21 @@ void _ldlm_lock_debug(struct ldlm_lock *lock,
 		nid = libcfs_nid2str(exp->exp_connection->c_peer.nid);
 	} else if (exp && exp->exp_obd != NULL) {
 		struct obd_import *imp = exp->exp_obd->u.cli.cl_import;
+
 		nid = libcfs_nid2str(imp->imp_connection->c_peer.nid);
 	}
 
 	if (resource == NULL) {
 		libcfs_debug_vmsg2(msgdata, fmt, args,
-		       " ns: \?\? lock: %p/%#llx lrc: %d/%d,%d mode: %s/%s "
-		       "res: \?\? rrc=\?\? type: \?\?\? flags: %#llx nid: %s "
-		       "remote: %#llx expref: %d pid: %u timeout: %lu "
-		       "lvb_type: %d\n",
-		       lock,
-		       lock->l_handle.h_cookie, atomic_read(&lock->l_refc),
-		       lock->l_readers, lock->l_writers,
-		       ldlm_lockname[lock->l_granted_mode],
-		       ldlm_lockname[lock->l_req_mode],
-		       lock->l_flags, nid, lock->l_remote_handle.cookie,
-		       exp ? atomic_read(&exp->exp_refcount) : -99,
-		       lock->l_pid, lock->l_callback_timeout, lock->l_lvb_type);
+				   " ns: \?\? lock: %p/%#llx lrc: %d/%d,%d mode: %s/%s res: \?\? rrc=\?\? type: \?\?\? flags: %#llx nid: %s remote: %#llx expref: %d pid: %u timeout: %lu lvb_type: %d\n",
+				   lock,
+				   lock->l_handle.h_cookie, atomic_read(&lock->l_refc),
+				   lock->l_readers, lock->l_writers,
+				   ldlm_lockname[lock->l_granted_mode],
+				   ldlm_lockname[lock->l_req_mode],
+				   lock->l_flags, nid, lock->l_remote_handle.cookie,
+				   exp ? atomic_read(&exp->exp_refcount) : -99,
+				   lock->l_pid, lock->l_callback_timeout, lock->l_lvb_type);
 		va_end(args);
 		return;
 	}
@@ -2248,90 +2245,78 @@ void _ldlm_lock_debug(struct ldlm_lock *lock,
 	switch (resource->lr_type) {
 	case LDLM_EXTENT:
 		libcfs_debug_vmsg2(msgdata, fmt, args,
-			" ns: %s lock: %p/%#llx lrc: %d/%d,%d mode: %s/%s "
-			"res: "DLDLMRES" rrc: %d type: %s [%llu->%llu] "
-			"(req %llu->%llu) flags: %#llx nid: %s remote: "
-			"%#llx expref: %d pid: %u timeout: %lu lvb_type: %d\n",
-			ldlm_lock_to_ns_name(lock), lock,
-			lock->l_handle.h_cookie, atomic_read(&lock->l_refc),
-			lock->l_readers, lock->l_writers,
-			ldlm_lockname[lock->l_granted_mode],
-			ldlm_lockname[lock->l_req_mode],
-			PLDLMRES(resource),
-			atomic_read(&resource->lr_refcount),
-			ldlm_typename[resource->lr_type],
-			lock->l_policy_data.l_extent.start,
-			lock->l_policy_data.l_extent.end,
-			lock->l_req_extent.start, lock->l_req_extent.end,
-			lock->l_flags, nid, lock->l_remote_handle.cookie,
-			exp ? atomic_read(&exp->exp_refcount) : -99,
-			lock->l_pid, lock->l_callback_timeout,
-			lock->l_lvb_type);
+				   " ns: %s lock: %p/%#llx lrc: %d/%d,%d mode: %s/%s res: " DLDLMRES " rrc: %d type: %s [%llu->%llu] (req %llu->%llu) flags: %#llx nid: %s remote: %#llx expref: %d pid: %u timeout: %lu lvb_type: %d\n",
+				   ldlm_lock_to_ns_name(lock), lock,
+				   lock->l_handle.h_cookie, atomic_read(&lock->l_refc),
+				   lock->l_readers, lock->l_writers,
+				   ldlm_lockname[lock->l_granted_mode],
+				   ldlm_lockname[lock->l_req_mode],
+				   PLDLMRES(resource),
+				   atomic_read(&resource->lr_refcount),
+				   ldlm_typename[resource->lr_type],
+				   lock->l_policy_data.l_extent.start,
+				   lock->l_policy_data.l_extent.end,
+				   lock->l_req_extent.start, lock->l_req_extent.end,
+				   lock->l_flags, nid, lock->l_remote_handle.cookie,
+				   exp ? atomic_read(&exp->exp_refcount) : -99,
+				   lock->l_pid, lock->l_callback_timeout,
+				   lock->l_lvb_type);
 		break;
 
 	case LDLM_FLOCK:
 		libcfs_debug_vmsg2(msgdata, fmt, args,
-			" ns: %s lock: %p/%#llx lrc: %d/%d,%d mode: %s/%s "
-			"res: "DLDLMRES" rrc: %d type: %s pid: %d "
-			"[%llu->%llu] flags: %#llx nid: %s "
-			"remote: %#llx expref: %d pid: %u timeout: %lu\n",
-			ldlm_lock_to_ns_name(lock), lock,
-			lock->l_handle.h_cookie, atomic_read(&lock->l_refc),
-			lock->l_readers, lock->l_writers,
-			ldlm_lockname[lock->l_granted_mode],
-			ldlm_lockname[lock->l_req_mode],
-			PLDLMRES(resource),
-			atomic_read(&resource->lr_refcount),
-			ldlm_typename[resource->lr_type],
-			lock->l_policy_data.l_flock.pid,
-			lock->l_policy_data.l_flock.start,
-			lock->l_policy_data.l_flock.end,
-			lock->l_flags, nid, lock->l_remote_handle.cookie,
-			exp ? atomic_read(&exp->exp_refcount) : -99,
-			lock->l_pid, lock->l_callback_timeout);
+				   " ns: %s lock: %p/%#llx lrc: %d/%d,%d mode: %s/%s res: " DLDLMRES " rrc: %d type: %s pid: %d [%llu->%llu] flags: %#llx nid: %s remote: %#llx expref: %d pid: %u timeout: %lu\n",
+				   ldlm_lock_to_ns_name(lock), lock,
+				   lock->l_handle.h_cookie, atomic_read(&lock->l_refc),
+				   lock->l_readers, lock->l_writers,
+				   ldlm_lockname[lock->l_granted_mode],
+				   ldlm_lockname[lock->l_req_mode],
+				   PLDLMRES(resource),
+				   atomic_read(&resource->lr_refcount),
+				   ldlm_typename[resource->lr_type],
+				   lock->l_policy_data.l_flock.pid,
+				   lock->l_policy_data.l_flock.start,
+				   lock->l_policy_data.l_flock.end,
+				   lock->l_flags, nid, lock->l_remote_handle.cookie,
+				   exp ? atomic_read(&exp->exp_refcount) : -99,
+				   lock->l_pid, lock->l_callback_timeout);
 		break;
 
 	case LDLM_IBITS:
 		libcfs_debug_vmsg2(msgdata, fmt, args,
-			" ns: %s lock: %p/%#llx lrc: %d/%d,%d mode: %s/%s "
-			"res: "DLDLMRES" bits %#llx rrc: %d type: %s "
-			"flags: %#llx nid: %s remote: %#llx expref: %d "
-			"pid: %u timeout: %lu lvb_type: %d\n",
-			ldlm_lock_to_ns_name(lock),
-			lock, lock->l_handle.h_cookie,
-			atomic_read(&lock->l_refc),
-			lock->l_readers, lock->l_writers,
-			ldlm_lockname[lock->l_granted_mode],
-			ldlm_lockname[lock->l_req_mode],
-			PLDLMRES(resource),
-			lock->l_policy_data.l_inodebits.bits,
-			atomic_read(&resource->lr_refcount),
-			ldlm_typename[resource->lr_type],
-			lock->l_flags, nid, lock->l_remote_handle.cookie,
-			exp ? atomic_read(&exp->exp_refcount) : -99,
-			lock->l_pid, lock->l_callback_timeout,
-			lock->l_lvb_type);
+				   " ns: %s lock: %p/%#llx lrc: %d/%d,%d mode: %s/%s res: " DLDLMRES " bits %#llx rrc: %d type: %s flags: %#llx nid: %s remote: %#llx expref: %d pid: %u timeout: %lu lvb_type: %d\n",
+				   ldlm_lock_to_ns_name(lock),
+				   lock, lock->l_handle.h_cookie,
+				   atomic_read(&lock->l_refc),
+				   lock->l_readers, lock->l_writers,
+				   ldlm_lockname[lock->l_granted_mode],
+				   ldlm_lockname[lock->l_req_mode],
+				   PLDLMRES(resource),
+				   lock->l_policy_data.l_inodebits.bits,
+				   atomic_read(&resource->lr_refcount),
+				   ldlm_typename[resource->lr_type],
+				   lock->l_flags, nid, lock->l_remote_handle.cookie,
+				   exp ? atomic_read(&exp->exp_refcount) : -99,
+				   lock->l_pid, lock->l_callback_timeout,
+				   lock->l_lvb_type);
 		break;
 
 	default:
 		libcfs_debug_vmsg2(msgdata, fmt, args,
-			" ns: %s lock: %p/%#llx lrc: %d/%d,%d mode: %s/%s "
-			"res: "DLDLMRES" rrc: %d type: %s flags: %#llx "
-			"nid: %s remote: %#llx expref: %d pid: %u "
-			"timeout: %lu lvb_type: %d\n",
-			ldlm_lock_to_ns_name(lock),
-			lock, lock->l_handle.h_cookie,
-			atomic_read(&lock->l_refc),
-			lock->l_readers, lock->l_writers,
-			ldlm_lockname[lock->l_granted_mode],
-			ldlm_lockname[lock->l_req_mode],
-			PLDLMRES(resource),
-			atomic_read(&resource->lr_refcount),
-			ldlm_typename[resource->lr_type],
-			lock->l_flags, nid, lock->l_remote_handle.cookie,
-			exp ? atomic_read(&exp->exp_refcount) : -99,
-			lock->l_pid, lock->l_callback_timeout,
-			lock->l_lvb_type);
+				   " ns: %s lock: %p/%#llx lrc: %d/%d,%d mode: %s/%s res: " DLDLMRES " rrc: %d type: %s flags: %#llx nid: %s remote: %#llx expref: %d pid: %u timeout: %lu lvb_type: %d\n",
+				   ldlm_lock_to_ns_name(lock),
+				   lock, lock->l_handle.h_cookie,
+				   atomic_read(&lock->l_refc),
+				   lock->l_readers, lock->l_writers,
+				   ldlm_lockname[lock->l_granted_mode],
+				   ldlm_lockname[lock->l_req_mode],
+				   PLDLMRES(resource),
+				   atomic_read(&resource->lr_refcount),
+				   ldlm_typename[resource->lr_type],
+				   lock->l_flags, nid, lock->l_remote_handle.cookie,
+				   exp ? atomic_read(&exp->exp_refcount) : -99,
+				   lock->l_pid, lock->l_callback_timeout,
+				   lock->l_lvb_type);
 		break;
 	}
 	va_end(args);
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c
index 91cf7ebae114..98fbd3f7e751 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c
@@ -158,13 +158,15 @@ void ldlm_handle_bl_callback(struct ldlm_namespace *ns,
 	unlock_res_and_lock(lock);
 
 	if (do_ast) {
-		CDEBUG(D_DLMTRACE, "Lock %p already unused, calling callback (%p)\n",
-		       lock, lock->l_blocking_ast);
+		CDEBUG(D_DLMTRACE,
+		       "Lock %p already unused, calling callback (%p)\n", lock,
+		       lock->l_blocking_ast);
 		if (lock->l_blocking_ast != NULL)
 			lock->l_blocking_ast(lock, ld, lock->l_ast_data,
 					     LDLM_CB_BLOCKING);
 	} else {
-		CDEBUG(D_DLMTRACE, "Lock %p is referenced, will be cancelled later\n",
+		CDEBUG(D_DLMTRACE,
+		       "Lock %p is referenced, will be cancelled later\n",
 		       lock);
 	}
 
@@ -190,6 +192,7 @@ static void ldlm_handle_cp_callback(struct ptlrpc_request *req,
 
 	if (OBD_FAIL_CHECK(OBD_FAIL_LDLM_CANCEL_BL_CB_RACE)) {
 		int to = cfs_time_seconds(1);
+
 		while (to > 0) {
 			set_current_state(TASK_INTERRUPTIBLE);
 			schedule_timeout(to);
@@ -210,9 +213,7 @@ static void ldlm_handle_cp_callback(struct ptlrpc_request *req,
 			LASSERT(lock->l_lvb_data != NULL);
 
 			if (unlikely(lock->l_lvb_len < lvb_len)) {
-				LDLM_ERROR(lock, "Replied LVB is larger than "
-					   "expectation, expected = %d, "
-					   "replied = %d",
+				LDLM_ERROR(lock, "Replied LVB is larger than expectation, expected = %d, replied = %d",
 					   lock->l_lvb_len, lvb_len);
 				rc = -EINVAL;
 				goto out;
@@ -639,8 +640,8 @@ static int ldlm_callback_handler(struct ptlrpc_request *req)
 
 	lock = ldlm_handle2lock_long(&dlm_req->lock_handle[0], 0);
 	if (!lock) {
-		CDEBUG(D_DLMTRACE, "callback on lock %#llx - lock "
-		       "disappeared\n", dlm_req->lock_handle[0].cookie);
+		CDEBUG(D_DLMTRACE, "callback on lock %#llx - lock disappeared\n",
+		       dlm_req->lock_handle[0].cookie);
 		rc = ldlm_callback_reply(req, -EINVAL);
 		ldlm_callback_errmsg(req, "Operate with invalid parameter", rc,
 				     &dlm_req->lock_handle[0]);
@@ -663,8 +664,7 @@ static int ldlm_callback_handler(struct ptlrpc_request *req)
 		if (((lock->l_flags & LDLM_FL_CANCELING) &&
 		    (lock->l_flags & LDLM_FL_BL_DONE)) ||
 		    (lock->l_flags & LDLM_FL_FAILED)) {
-			LDLM_DEBUG(lock, "callback on lock "
-				   "%#llx - lock disappeared\n",
+			LDLM_DEBUG(lock, "callback on lock %#llx - lock disappeared\n",
 				   dlm_req->lock_handle[0].cookie);
 			unlock_res_and_lock(lock);
 			LDLM_LOCK_RELEASE(lock);
@@ -724,7 +724,7 @@ static int ldlm_callback_handler(struct ptlrpc_request *req)
 static struct ldlm_bl_work_item *ldlm_bl_get_work(struct ldlm_bl_pool *blp)
 {
 	struct ldlm_bl_work_item *blwi = NULL;
-	static unsigned int num_bl = 0;
+	static unsigned int num_bl;
 
 	spin_lock(&blp->blp_lock);
 	/* process a request from the blp_list at least every blp_num_threads */
@@ -887,6 +887,7 @@ void ldlm_put_ref(void)
 	mutex_lock(&ldlm_ref_mutex);
 	if (ldlm_refcount == 1) {
 		int rc = ldlm_cleanup();
+
 		if (rc)
 			CERROR("ldlm_cleanup failed: %d\n", rc);
 		else
@@ -969,6 +970,7 @@ static cfs_hash_ops_t ldlm_export_lock_ops = {
 int ldlm_init_export(struct obd_export *exp)
 {
 	int rc;
+
 	exp->exp_lock_hash =
 		cfs_hash_create(obd_uuid2str(&exp->exp_client_uuid),
 				HASH_EXP_LOCK_CUR_BITS,
@@ -1049,7 +1051,7 @@ static int ldlm_setup(void)
 			.so_req_handler		= ldlm_callback_handler,
 		},
 	};
-	ldlm_state->ldlm_cb_service = \
+	ldlm_state->ldlm_cb_service =
 			ptlrpc_register_service(&conf, ldlm_svc_proc_dir);
 	if (IS_ERR(ldlm_state->ldlm_cb_service)) {
 		CERROR("failed to start service\n");
@@ -1077,7 +1079,7 @@ static int ldlm_setup(void)
 		blp->blp_min_threads = LDLM_NTHRS_INIT;
 		blp->blp_max_threads = LDLM_NTHRS_MAX;
 	} else {
-		blp->blp_min_threads = blp->blp_max_threads = \
+		blp->blp_min_threads = blp->blp_max_threads =
 			min_t(int, LDLM_NTHRS_MAX, max_t(int, LDLM_NTHRS_INIT,
 							 ldlm_num_threads));
 	}
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c b/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
index 6054eee848d3..4c838f615a64 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
@@ -54,10 +54,10 @@
  * calculated as the number of locks in LRU * lock live time in seconds. If
  * CLV > SLV - lock is canceled.
  *
- * Client has LVF, that is, lock volume factor which regulates how much sensitive
- * client should be about last SLV from server. The higher LVF is the more locks
- * will be canceled on client. Default value for it is 1. Setting LVF to 2 means
- * that client will cancel locks 2 times faster.
+ * Client has LVF, that is, lock volume factor which regulates how much
+ * sensitive client should be about last SLV from server. The higher LVF is the
+ * more locks will be canceled on client. Default value for it is 1. Setting LVF
+ * to 2 means that client will cancel locks 2 times faster.
  *
  * Locks on a client will be canceled more intensively in these cases:
  * (1) if SLV is smaller, that is, load is higher on the server;
@@ -70,11 +70,12 @@
  * if flow is getting thinner, more and more particles become outside of it and
  * as particles are locks, they should be canceled.
  *
- * General idea of this belongs to Vitaly Fertman (vitaly@clusterfs.com). Andreas
- * Dilger (adilger@clusterfs.com) proposed few nice ideas like using LVF and many
- * cleanups. Flow definition to allow more easy understanding of the logic belongs
- * to Nikita Danilov (nikita@clusterfs.com) as well as many cleanups and fixes.
- * And design and implementation are done by Yury Umanets (umka@clusterfs.com).
+ * General idea of this belongs to Vitaly Fertman (vitaly@clusterfs.com).
+ * Andreas Dilger (adilger@clusterfs.com) proposed few nice ideas like using
+ * LVF and many cleanups. Flow definition to allow more easy understanding of
+ * the logic belongs to Nikita Danilov (nikita@clusterfs.com) as well as many
+ * cleanups and fixes. And design and implementation are done by Yury Umanets
+ * (umka@clusterfs.com).
  *
  * Glossary for terms used:
  *
@@ -265,16 +266,15 @@ static void ldlm_pool_recalc_slv(struct ldlm_pool *pl)
 	 * SLV. And the opposite, the more grant plan is over-consumed
 	 * (load time) the faster drops SLV.
 	 */
-	slv_factor = (grant_usage << LDLM_POOL_SLV_SHIFT);
+	slv_factor = grant_usage << LDLM_POOL_SLV_SHIFT;
 	do_div(slv_factor, limit);
 	slv = slv * slv_factor;
 	slv = dru(slv, LDLM_POOL_SLV_SHIFT, round_up);
 
-	if (slv > ldlm_pool_slv_max(limit)) {
+	if (slv > ldlm_pool_slv_max(limit))
 		slv = ldlm_pool_slv_max(limit);
-	} else if (slv < ldlm_pool_slv_min(limit)) {
+	else if (slv < ldlm_pool_slv_min(limit))
 		slv = ldlm_pool_slv_min(limit);
-	}
 
 	pl->pl_server_lock_volume = slv;
 }
@@ -614,8 +614,8 @@ int ldlm_pool_shrink(struct ldlm_pool *pl, int nr,
 			lprocfs_counter_add(pl->pl_stats,
 					    LDLM_POOL_SHRINK_FREED_STAT,
 					    cancel);
-			CDEBUG(D_DLMTRACE, "%s: request to shrink %d locks, "
-			       "shrunk %d\n", pl->pl_name, nr, cancel);
+			CDEBUG(D_DLMTRACE, "%s: request to shrink %d locks, shrunk %d\n",
+			       pl->pl_name, nr, cancel);
 		}
 	}
 	return cancel;
@@ -636,7 +636,7 @@ int ldlm_pool_setup(struct ldlm_pool *pl, int limit)
 }
 EXPORT_SYMBOL(ldlm_pool_setup);
 
-#if defined (CONFIG_PROC_FS)
+#if defined(CONFIG_PROC_FS)
 static int lprocfs_pool_state_seq_show(struct seq_file *m, void *unused)
 {
 	int granted, grant_rate, cancel_rate, grant_step;
@@ -696,8 +696,9 @@ LPROC_SEQ_FOPS_RO(lprocfs_grant_plan);
 
 LDLM_POOL_PROC_READER_SEQ_SHOW(recalc_period, int);
 LDLM_POOL_PROC_WRITER(recalc_period, int);
-static ssize_t lprocfs_recalc_period_seq_write(struct file *file, const char *buf,
-					   size_t len, loff_t *off)
+static ssize_t lprocfs_recalc_period_seq_write(struct file *file,
+					       const char *buf, size_t len,
+					       loff_t *off)
 {
 	struct seq_file *seq = file->private_data;
 
@@ -943,6 +944,7 @@ EXPORT_SYMBOL(ldlm_pool_del);
 __u64 ldlm_pool_get_slv(struct ldlm_pool *pl)
 {
 	__u64 slv;
+
 	spin_lock(&pl->pl_lock);
 	slv = pl->pl_server_lock_volume;
 	spin_unlock(&pl->pl_lock);
@@ -971,6 +973,7 @@ EXPORT_SYMBOL(ldlm_pool_set_slv);
 __u64 ldlm_pool_get_clv(struct ldlm_pool *pl)
 {
 	__u64 slv;
+
 	spin_lock(&pl->pl_lock);
 	slv = pl->pl_client_lock_volume;
 	spin_unlock(&pl->pl_lock);
@@ -1132,23 +1135,27 @@ static unsigned long ldlm_pools_scan(ldlm_side_t client, int nr, gfp_t gfp_mask)
 	return (client == LDLM_NAMESPACE_SERVER) ? SHRINK_STOP : freed;
 }
 
-static unsigned long ldlm_pools_srv_count(struct shrinker *s, struct shrink_control *sc)
+static unsigned long ldlm_pools_srv_count(struct shrinker *s,
+					  struct shrink_control *sc)
 {
 	return ldlm_pools_count(LDLM_NAMESPACE_SERVER, sc->gfp_mask);
 }
 
-static unsigned long ldlm_pools_srv_scan(struct shrinker *s, struct shrink_control *sc)
+static unsigned long ldlm_pools_srv_scan(struct shrinker *s,
+					 struct shrink_control *sc)
 {
 	return ldlm_pools_scan(LDLM_NAMESPACE_SERVER, sc->nr_to_scan,
 			       sc->gfp_mask);
 }
 
-static unsigned long ldlm_pools_cli_count(struct shrinker *s, struct shrink_control *sc)
+static unsigned long ldlm_pools_cli_count(struct shrinker *s,
+					  struct shrink_control *sc)
 {
 	return ldlm_pools_count(LDLM_NAMESPACE_CLIENT, sc->gfp_mask);
 }
 
-static unsigned long ldlm_pools_cli_scan(struct shrinker *s, struct shrink_control *sc)
+static unsigned long ldlm_pools_cli_scan(struct shrinker *s,
+					 struct shrink_control *sc)
 {
 	return ldlm_pools_scan(LDLM_NAMESPACE_CLIENT, sc->nr_to_scan,
 			       sc->gfp_mask);
@@ -1194,10 +1201,8 @@ int ldlm_pools_recalc(ldlm_side_t client)
 		 * of limit.
 		 */
 		if (nr_l >= 2 * (LDLM_POOL_HOST_L / 3)) {
-			CWARN("\"Modest\" pools eat out 2/3 of server locks "
-			      "limit (%d of %lu). This means that you have too "
-			      "many clients for this amount of server RAM. "
-			      "Upgrade server!\n", nr_l, LDLM_POOL_HOST_L);
+			CWARN("\"Modest\" pools eat out 2/3 of server locks limit (%d of %lu). This means that you have too many clients for this amount of server RAM. Upgrade server!\n",
+			      nr_l, LDLM_POOL_HOST_L);
 			equal = 1;
 		}
 
@@ -1205,8 +1210,7 @@ int ldlm_pools_recalc(ldlm_side_t client)
 		 * The rest is given to greedy namespaces.
 		 */
 		list_for_each_entry(ns, ldlm_namespace_list(client),
-					ns_list_chain)
-		{
+				    ns_list_chain) {
 			if (!equal && ns->ns_appetite != LDLM_NAMESPACE_GREEDY)
 				continue;
 
@@ -1383,9 +1387,8 @@ static int ldlm_pools_thread_start(void)
 
 static void ldlm_pools_thread_stop(void)
 {
-	if (ldlm_pools_thread == NULL) {
+	if (ldlm_pools_thread == NULL)
 		return;
-	}
 
 	thread_set_flags(ldlm_pools_thread, SVC_STOPPING);
 	wake_up(&ldlm_pools_thread->t_ctl_waitq);
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_request.c b/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
index 9ce437b18793..287da325d928 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
@@ -95,16 +95,14 @@ int ldlm_expired_completion_wait(void *data)
 	struct obd_device *obd;
 
 	if (lock->l_conn_export == NULL) {
-		static unsigned long next_dump = 0, last_dump = 0;
+		static unsigned long next_dump, last_dump;
 
 		LCONSOLE_WARN("lock timed out (enqueued at "CFS_TIME_T", "
 			      CFS_DURATION_T"s ago)\n",
 			      lock->l_last_activity,
 			      cfs_time_sub(get_seconds(),
 					   lock->l_last_activity));
-		LDLM_DEBUG(lock, "lock timed out (enqueued at "CFS_TIME_T", "
-			   CFS_DURATION_T"s ago); not entering recovery in "
-			   "server code, just going back to sleep",
+		LDLM_DEBUG(lock, "lock timed out (enqueued at " CFS_TIME_T ", " CFS_DURATION_T "s ago); not entering recovery in server code, just going back to sleep",
 			   lock->l_last_activity,
 			   cfs_time_sub(get_seconds(),
 					lock->l_last_activity));
@@ -137,6 +135,7 @@ EXPORT_SYMBOL(ldlm_expired_completion_wait);
 int ldlm_get_enq_timeout(struct ldlm_lock *lock)
 {
 	int timeout = at_get(ldlm_lock_to_ns_at(lock));
+
 	if (AT_OFF)
 		return obd_timeout / 2;
 	/* Since these are non-updating timeouts, we should be conservative.
@@ -191,8 +190,7 @@ int ldlm_completion_ast_async(struct ldlm_lock *lock, __u64 flags, void *data)
 		return ldlm_completion_tail(lock);
 	}
 
-	LDLM_DEBUG(lock, "client-side enqueue returned a blocked lock, "
-		   "going forward");
+	LDLM_DEBUG(lock, "client-side enqueue returned a blocked lock, going forward");
 	ldlm_reprocess_all(lock->l_resource);
 	return 0;
 }
@@ -240,17 +238,15 @@ int ldlm_completion_ast(struct ldlm_lock *lock, __u64 flags, void *data)
 		return 0;
 	}
 
-	LDLM_DEBUG(lock, "client-side enqueue returned a blocked lock, "
-		   "sleeping");
+	LDLM_DEBUG(lock, "client-side enqueue returned a blocked lock, sleeping");
 
 noreproc:
 
 	obd = class_exp2obd(lock->l_conn_export);
 
 	/* if this is a local lock, then there is no import */
-	if (obd != NULL) {
+	if (obd != NULL)
 		imp = obd->u.cli.cl_import;
-	}
 
 	/* Wait a long time for enqueue - server may have to callback a
 	   lock from another client.  Server will evict the other client if it
@@ -324,8 +320,7 @@ int ldlm_blocking_ast_nocheck(struct ldlm_lock *lock)
 		if (rc < 0)
 			CERROR("ldlm_cli_cancel: %d\n", rc);
 	} else {
-		LDLM_DEBUG(lock, "Lock still has references, will be "
-			   "cancelled later");
+		LDLM_DEBUG(lock, "Lock still has references, will be cancelled later");
 	}
 	return 0;
 }
@@ -483,8 +478,7 @@ static void failed_lock_cleanup(struct ldlm_namespace *ns,
 
 	if (need_cancel)
 		LDLM_DEBUG(lock,
-			   "setting FL_LOCAL_ONLY | LDLM_FL_FAILED | "
-			   "LDLM_FL_ATOMIC_CB | LDLM_FL_CBPENDING");
+			   "setting FL_LOCAL_ONLY | LDLM_FL_FAILED | LDLM_FL_ATOMIC_CB | LDLM_FL_CBPENDING");
 	else
 		LDLM_DEBUG(lock, "lock was granted or failed in race");
 
@@ -557,8 +551,7 @@ int ldlm_cli_enqueue_fini(struct obd_export *exp, struct ptlrpc_request *req,
 			rc = size;
 			goto cleanup;
 		} else if (unlikely(size > lvb_len)) {
-			LDLM_ERROR(lock, "Replied LVB is larger than "
-				   "expectation, expected = %d, replied = %d",
+			LDLM_ERROR(lock, "Replied LVB is larger than expectation, expected = %d, replied = %d",
 				   lvb_len, size);
 			rc = -EINVAL;
 			goto cleanup;
@@ -608,6 +601,7 @@ int ldlm_cli_enqueue_fini(struct obd_export *exp, struct ptlrpc_request *req,
 	 * again. */
 	if ((*flags) & LDLM_FL_LOCK_CHANGED) {
 		int newmode = reply->lock_desc.l_req_mode;
+
 		LASSERT(!is_replay);
 		if (newmode && newmode != lock->l_req_mode) {
 			LDLM_DEBUG(lock, "server returned different mode %s",
@@ -676,6 +670,7 @@ int ldlm_cli_enqueue_fini(struct obd_export *exp, struct ptlrpc_request *req,
 		rc = ldlm_lock_enqueue(ns, &lock, NULL, flags);
 		if (lock->l_completion_ast != NULL) {
 			int err = lock->l_completion_ast(lock, *flags, NULL);
+
 			if (!rc)
 				rc = err;
 			if (rc)
@@ -725,6 +720,7 @@ static inline int ldlm_capsule_handles_avail(struct req_capsule *pill,
 					     int off)
 {
 	int size = req_capsule_msg_size(pill, loc);
+
 	return ldlm_req_handles_avail(size, off);
 }
 
@@ -733,6 +729,7 @@ static inline int ldlm_format_handles_avail(struct obd_import *imp,
 					    enum req_location loc, int off)
 {
 	int size = req_capsule_fmt_size(imp->imp_msg_magic, fmt, loc);
+
 	return ldlm_req_handles_avail(size, off);
 }
 
@@ -1107,8 +1104,7 @@ static __u64 ldlm_cli_cancel_local(struct ldlm_lock *lock)
 		unlock_res_and_lock(lock);
 
 		if (local_only) {
-			CDEBUG(D_DLMTRACE, "not sending request (at caller's "
-			       "instruction)\n");
+			CDEBUG(D_DLMTRACE, "not sending request (at caller's instruction)\n");
 			rc = LDLM_FL_LOCAL_ONLY;
 		}
 		ldlm_lock_cancel(lock);
@@ -1223,8 +1219,7 @@ int ldlm_cli_cancel_req(struct obd_export *exp, struct list_head *cancels,
 			rc = ptlrpc_queue_wait(req);
 		}
 		if (rc == LUSTRE_ESTALE) {
-			CDEBUG(D_DLMTRACE, "client/server (nid %s) "
-			       "out of sync -- not fatal\n",
+			CDEBUG(D_DLMTRACE, "client/server (nid %s) out of sync -- not fatal\n",
 			       libcfs_nid2str(req->rq_import->
 					      imp_connection->c_peer.nid));
 			rc = 0;
@@ -1235,8 +1230,8 @@ int ldlm_cli_cancel_req(struct obd_export *exp, struct list_head *cancels,
 		} else if (rc != ELDLM_OK) {
 			/* -ESHUTDOWN is common on umount */
 			CDEBUG_LIMIT(rc == -ESHUTDOWN ? D_DLMTRACE : D_ERROR,
-				     "Got rc %d from cancel RPC: "
-				     "canceling anyway\n", rc);
+				     "Got rc %d from cancel RPC: canceling anyway\n",
+				     rc);
 			break;
 		}
 		sent = count;
@@ -1279,7 +1274,8 @@ int ldlm_cli_update_pool(struct ptlrpc_request *req)
 	 * server-side namespace is not possible. */
 	if (lustre_msg_get_slv(req->rq_repmsg) == 0 ||
 	    lustre_msg_get_limit(req->rq_repmsg) == 0) {
-		DEBUG_REQ(D_HA, req, "Zero SLV or Limit found (SLV: %llu, Limit: %u)",
+		DEBUG_REQ(D_HA, req,
+			  "Zero SLV or Limit found (SLV: %llu, Limit: %u)",
 			  lustre_msg_get_slv(req->rq_repmsg),
 			  lustre_msg_get_limit(req->rq_repmsg));
 		return 0;
@@ -1416,19 +1412,20 @@ static ldlm_policy_res_t ldlm_cancel_no_wait_policy(struct ldlm_namespace *ns,
 {
 	ldlm_policy_res_t result = LDLM_POLICY_CANCEL_LOCK;
 	ldlm_cancel_for_recovery cb = ns->ns_cancel_for_recovery;
+
 	lock_res_and_lock(lock);
 
 	/* don't check added & count since we want to process all locks
 	 * from unused list */
 	switch (lock->l_resource->lr_type) {
-		case LDLM_EXTENT:
-		case LDLM_IBITS:
-			if (cb && cb(lock))
-				break;
-		default:
-			result = LDLM_POLICY_SKIP_LOCK;
-			lock->l_flags |= LDLM_FL_SKIPPED;
+	case LDLM_EXTENT:
+	case LDLM_IBITS:
+		if (cb && cb(lock))
 			break;
+	default:
+		result = LDLM_POLICY_SKIP_LOCK;
+		lock->l_flags |= LDLM_FL_SKIPPED;
+		break;
 	}
 
 	unlock_res_and_lock(lock);
@@ -1594,8 +1591,9 @@ ldlm_cancel_lru_policy(struct ldlm_namespace *ns, int flags)
  *			       sending any RPCs or waiting for any
  *			       outstanding RPC to complete.
  */
-static int ldlm_prepare_lru_list(struct ldlm_namespace *ns, struct list_head *cancels,
-				 int count, int max, int flags)
+static int ldlm_prepare_lru_list(struct ldlm_namespace *ns,
+				 struct list_head *cancels, int count, int max,
+				 int flags)
 {
 	ldlm_cancel_lru_policy_t pf;
 	struct ldlm_lock *lock, *next;
@@ -1730,6 +1728,7 @@ int ldlm_cancel_lru_local(struct ldlm_namespace *ns, struct list_head *cancels,
 			  int flags)
 {
 	int added;
+
 	added = ldlm_prepare_lru_list(ns, cancels, count, max, flags);
 	if (added <= 0)
 		return added;
@@ -1918,7 +1917,8 @@ struct ldlm_cli_cancel_arg {
 	void   *lc_opaque;
 };
 
-static int ldlm_cli_hash_cancel_unused(struct cfs_hash *hs, struct cfs_hash_bd *bd,
+static int ldlm_cli_hash_cancel_unused(struct cfs_hash *hs,
+				       struct cfs_hash_bd *bd,
 				       struct hlist_node *hnode, void *arg)
 {
 	struct ldlm_resource	   *res = cfs_hash_object(hs, hnode);
@@ -2014,6 +2014,7 @@ struct iter_helper_data {
 static int ldlm_iter_helper(struct ldlm_lock *lock, void *closure)
 {
 	struct iter_helper_data *helper = closure;
+
 	return helper->iter(lock, helper->closure);
 }
 
@@ -2080,7 +2081,8 @@ static int ldlm_chain_lock_for_replay(struct ldlm_lock *lock, void *closure)
 	/* we use l_pending_chain here, because it's unused on clients. */
 	LASSERTF(list_empty(&lock->l_pending_chain),
 		 "lock %p next %p prev %p\n",
-		 lock, &lock->l_pending_chain.next,&lock->l_pending_chain.prev);
+		 lock, &lock->l_pending_chain.next,
+		 &lock->l_pending_chain.prev);
 	/* bug 9573: don't replay locks left after eviction, or
 	 * bug 17614: locks being actively cancelled. Get a reference
 	 * on a lock so that it does not disappear under us (e.g. due to cancel)
@@ -2114,8 +2116,7 @@ static int replay_lock_interpret(const struct lu_env *env,
 
 	lock = ldlm_handle2lock(&aa->lock_handle);
 	if (!lock) {
-		CERROR("received replay ack for unknown local cookie %#llx"
-		       " remote cookie %#llx from server %s id %s\n",
+		CERROR("received replay ack for unknown local cookie %#llx remote cookie %#llx from server %s id %s\n",
 		       aa->lock_handle.cookie, reply->lock_handle.cookie,
 		       req->rq_export->exp_client_uuid.uuid,
 		       libcfs_id2str(req->rq_peer));
@@ -2243,9 +2244,8 @@ static void ldlm_cancel_unused_locks_for_replay(struct ldlm_namespace *ns)
 	int canceled;
 	LIST_HEAD(cancels);
 
-	CDEBUG(D_DLMTRACE, "Dropping as many unused locks as possible before"
-			   "replay for namespace %s (%d)\n",
-			   ldlm_ns_name(ns), ns->ns_nr_unused);
+	CDEBUG(D_DLMTRACE, "Dropping as many unused locks as possible before replay for namespace %s (%d)\n",
+	       ldlm_ns_name(ns), ns->ns_nr_unused);
 
 	/* We don't need to care whether or not LRU resize is enabled
 	 * because the LDLM_CANCEL_NO_WAIT policy doesn't use the
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c b/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
index a785b7a7d1b3..1f150e46f50e 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
@@ -71,7 +71,7 @@ extern unsigned int ldlm_cancel_unused_locks_before_replay;
  * DDOS. */
 unsigned int ldlm_dump_granted_max = 256;
 
-#if defined (CONFIG_PROC_FS)
+#if defined(CONFIG_PROC_FS)
 static ssize_t lprocfs_wr_dump_ns(struct file *file, const char *buffer,
 				  size_t count, loff_t *off)
 {
@@ -93,7 +93,7 @@ int ldlm_proc_setup(void)
 		  &ldlm_dump_granted_max },
 		{ "cancel_unused_locks_before_replay", &ldlm_rw_uint_fops,
 		  &ldlm_cancel_unused_locks_before_replay },
-		{ NULL }};
+		{ NULL } };
 	LASSERT(ldlm_ns_proc_dir == NULL);
 
 	ldlm_type_proc_dir = lprocfs_register(OBD_LDLM_DEVICENAME,
@@ -215,8 +215,8 @@ static ssize_t lprocfs_lru_size_seq_write(struct file *file,
 						   LDLM_CANCEL_PASSED);
 			if (canceled < unused) {
 				CDEBUG(D_DLMTRACE,
-				       "not all requested locks are canceled, "
-				       "requested: %d, canceled: %d\n", unused,
+				       "not all requested locks are canceled, requested: %d, canceled: %d\n",
+				       unused,
 				       canceled);
 				return -EINVAL;
 			}
@@ -385,8 +385,8 @@ int ldlm_namespace_proc_register(struct ldlm_namespace *ns)
 #undef MAX_STRING_SIZE
 #else /* CONFIG_PROC_FS */
 
-#define ldlm_namespace_proc_unregister(ns)      ({;})
-#define ldlm_namespace_proc_register(ns)	({0;})
+#define ldlm_namespace_proc_unregister(ns)      ({; })
+#define ldlm_namespace_proc_register(ns)	({0; })
 
 #endif /* CONFIG_PROC_FS */
 
@@ -454,7 +454,8 @@ static void *ldlm_res_hop_object(struct hlist_node *hnode)
 	return hlist_entry(hnode, struct ldlm_resource, lr_hash);
 }
 
-static void ldlm_res_hop_get_locked(struct cfs_hash *hs, struct hlist_node *hnode)
+static void ldlm_res_hop_get_locked(struct cfs_hash *hs,
+				    struct hlist_node *hnode)
 {
 	struct ldlm_resource *res;
 
@@ -462,7 +463,8 @@ static void ldlm_res_hop_get_locked(struct cfs_hash *hs, struct hlist_node *hnod
 	ldlm_resource_getref(res);
 }
 
-static void ldlm_res_hop_put_locked(struct cfs_hash *hs, struct hlist_node *hnode)
+static void ldlm_res_hop_put_locked(struct cfs_hash *hs,
+				    struct hlist_node *hnode)
 {
 	struct ldlm_resource *res;
 
@@ -501,7 +503,7 @@ cfs_hash_ops_t ldlm_ns_fid_hash_ops = {
 	.hs_put	 = ldlm_res_hop_put
 };
 
-typedef struct {
+struct ldlm_ns_hash_def {
 	ldlm_ns_type_t  nsd_type;
 	/** hash bucket bits */
 	unsigned	nsd_bkt_bits;
@@ -509,9 +511,9 @@ typedef struct {
 	unsigned	nsd_all_bits;
 	/** hash operations */
 	cfs_hash_ops_t *nsd_hops;
-} ldlm_ns_hash_def_t;
+};
 
-ldlm_ns_hash_def_t ldlm_ns_hash_defs[] = {
+struct ldlm_ns_hash_def ldlm_ns_hash_defs[] = {
 	{
 		.nsd_type       = LDLM_NS_TYPE_MDC,
 		.nsd_bkt_bits   = 11,
@@ -563,7 +565,7 @@ struct ldlm_namespace *ldlm_namespace_new(struct obd_device *obd, char *name,
 {
 	struct ldlm_namespace *ns = NULL;
 	struct ldlm_ns_bucket *nsb;
-	ldlm_ns_hash_def_t    *nsd;
+	struct ldlm_ns_hash_def    *nsd;
 	struct cfs_hash_bd	  bd;
 	int		    idx;
 	int		    rc;
@@ -576,7 +578,7 @@ struct ldlm_namespace *ldlm_namespace_new(struct obd_device *obd, char *name,
 		return NULL;
 	}
 
-	for (idx = 0;;idx++) {
+	for (idx = 0;; idx++) {
 		nsd = &ldlm_ns_hash_defs[idx];
 		if (nsd->nsd_type == LDLM_NS_TYPE_UNKNOWN) {
 			CERROR("Unknown type %d for ns %s\n", ns_type, name);
@@ -735,8 +737,7 @@ static void cleanup_resource(struct ldlm_resource *res, struct list_head *q,
 		} else {
 			ldlm_resource_unlink_lock(lock);
 			unlock_res(res);
-			LDLM_DEBUG(lock, "Freeing a lock still held by a "
-				   "client node");
+			LDLM_DEBUG(lock, "Freeing a lock still held by a client node");
 			ldlm_lock_destroy(lock);
 		}
 		LDLM_LOCK_RELEASE(lock);
@@ -805,6 +806,7 @@ static int __ldlm_namespace_free(struct ldlm_namespace *ns, int force)
 	if (atomic_read(&ns->ns_bref) > 0) {
 		struct l_wait_info lwi = LWI_INTR(LWI_ON_SIGNAL_NOOP, NULL);
 		int rc;
+
 		CDEBUG(D_DLMTRACE,
 		       "dlm namespace %s free waiting on refcount %d\n",
 		       ldlm_ns_name(ns), atomic_read(&ns->ns_bref));
@@ -818,16 +820,14 @@ force_wait:
 		/* Forced cleanups should be able to reclaim all references,
 		 * so it's safe to wait forever... we can't leak locks... */
 		if (force && rc == -ETIMEDOUT) {
-			LCONSOLE_ERROR("Forced cleanup waiting for %s "
-				       "namespace with %d resources in use, "
-				       "(rc=%d)\n", ldlm_ns_name(ns),
+			LCONSOLE_ERROR("Forced cleanup waiting for %s namespace with %d resources in use, (rc=%d)\n",
+				       ldlm_ns_name(ns),
 				       atomic_read(&ns->ns_bref), rc);
 			goto force_wait;
 		}
 
 		if (atomic_read(&ns->ns_bref)) {
-			LCONSOLE_ERROR("Cleanup waiting for %s namespace "
-				       "with %d resources in use, (rc=%d)\n",
+			LCONSOLE_ERROR("Cleanup waiting for %s namespace with %d resources in use, (rc=%d)\n",
 				       ldlm_ns_name(ns),
 				       atomic_read(&ns->ns_bref), rc);
 			return ELDLM_NAMESPACE_EXISTS;
@@ -1335,6 +1335,7 @@ void ldlm_dump_all_namespaces(ldlm_side_t client, int level)
 
 	list_for_each(tmp, ldlm_namespace_list(client)) {
 		struct ldlm_namespace *ns;
+
 		ns = list_entry(tmp, struct ldlm_namespace, ns_list_chain);
 		ldlm_namespace_dump(level, ns);
 	}
@@ -1404,8 +1405,8 @@ void ldlm_resource_dump(int level, struct ldlm_resource *res)
 			LDLM_DEBUG_LIMIT(level, lock, "###");
 			if (!(level & D_CANTMASK) &&
 			    ++granted > ldlm_dump_granted_max) {
-				CDEBUG(level, "only dump %d granted locks to "
-				       "avoid DDOS.\n", granted);
+				CDEBUG(level, "only dump %d granted locks to avoid DDOS.\n",
+				       granted);
 				break;
 			}
 		}
diff --git a/drivers/staging/lustre/lustre/libcfs/debug.c b/drivers/staging/lustre/lustre/libcfs/debug.c
index ba43b3067fa3..a7a7ac626aaf 100644
--- a/drivers/staging/lustre/lustre/libcfs/debug.c
+++ b/drivers/staging/lustre/lustre/libcfs/debug.c
@@ -318,9 +318,7 @@ libcfs_debug_str2mask(int *mask, const char *str, int is_subsys)
 	if (t >= 1 && matched == n) {
 		/* don't print warning for lctl set_param debug=0 or -1 */
 		if (m != 0 && m != -1)
-			CWARN("You are trying to use a numerical value for the "
-			      "mask - this will be deprecated in a future "
-			      "release.\n");
+			CWARN("You are trying to use a numerical value for the mask - this will be deprecated in a future release.\n");
 		*mask = m;
 		return 0;
 	}
@@ -375,8 +373,8 @@ void libcfs_debug_dumplog(void)
 			     (void *)(long)current_pid(),
 			     "libcfs_debug_dumper");
 	if (IS_ERR(dumper))
-		pr_err("LustreError: cannot start log dump thread:"
-		       " %ld\n", PTR_ERR(dumper));
+		pr_err("LustreError: cannot start log dump thread: %ld\n",
+		       PTR_ERR(dumper));
 	else
 		schedule();
 
@@ -412,7 +410,7 @@ int libcfs_debug_init(unsigned long bufsize)
 		max = TCD_MAX_PAGES;
 	} else {
 		max = (max / num_possible_cpus());
-		max = (max << (20 - PAGE_CACHE_SHIFT));
+		max = max << (20 - PAGE_CACHE_SHIFT);
 	}
 	rc = cfs_tracefile_init(max);
 
@@ -460,11 +458,3 @@ void libcfs_debug_set_level(unsigned int debug_level)
 }
 
 EXPORT_SYMBOL(libcfs_debug_set_level);
-
-void libcfs_log_goto(struct libcfs_debug_msg_data *msgdata, const char *label,
-		     long_ptr_t rc)
-{
-	libcfs_debug_msg(msgdata, "Process leaving via %s (rc=%lu : %ld : %#lx)\n",
-			 label, (ulong_ptr_t)rc, rc, rc);
-}
-EXPORT_SYMBOL(libcfs_log_goto);
diff --git a/drivers/staging/lustre/lustre/libcfs/fail.c b/drivers/staging/lustre/lustre/libcfs/fail.c
index e73ca3df9734..92444b0fe2a3 100644
--- a/drivers/staging/lustre/lustre/libcfs/fail.c
+++ b/drivers/staging/lustre/lustre/libcfs/fail.c
@@ -103,18 +103,18 @@ int __cfs_fail_check_set(__u32 id, __u32 value, int set)
 	}
 
 	switch (set) {
-		case CFS_FAIL_LOC_NOSET:
-		case CFS_FAIL_LOC_VALUE:
-			break;
-		case CFS_FAIL_LOC_ORSET:
-			cfs_fail_loc |= value & ~(CFS_FAILED | CFS_FAIL_ONCE);
-			break;
-		case CFS_FAIL_LOC_RESET:
-			cfs_fail_loc = value;
-			break;
-		default:
-			LASSERTF(0, "called with bad set %u\n", set);
-			break;
+	case CFS_FAIL_LOC_NOSET:
+	case CFS_FAIL_LOC_VALUE:
+		break;
+	case CFS_FAIL_LOC_ORSET:
+		cfs_fail_loc |= value & ~(CFS_FAILED | CFS_FAIL_ONCE);
+		break;
+	case CFS_FAIL_LOC_RESET:
+		cfs_fail_loc = value;
+		break;
+	default:
+		LASSERTF(0, "called with bad set %u\n", set);
+		break;
 	}
 
 	return 1;
diff --git a/drivers/staging/lustre/lustre/libcfs/hash.c b/drivers/staging/lustre/lustre/libcfs/hash.c
index 3b67b7b6428c..2d1e6729e996 100644
--- a/drivers/staging/lustre/lustre/libcfs/hash.c
+++ b/drivers/staging/lustre/lustre/libcfs/hash.c
@@ -1121,8 +1121,7 @@ cfs_hash_destroy(struct cfs_hash *hs)
 		cfs_hash_bd_for_each_hlist(hs, &bd, hhead) {
 			hlist_for_each_safe(hnode, pos, hhead) {
 				LASSERTF(!cfs_hash_with_assert_empty(hs),
-					 "hash %s bucket %u(%u) is not "
-					 " empty: %u items left\n",
+					 "hash %s bucket %u(%u) is not empty: %u items left\n",
 					 hs->hs_name, bd.bd_bucket->hsb_index,
 					 bd.bd_offset, bd.bd_bucket->hsb_count);
 				/* can't assert key valicate, because we
@@ -1371,8 +1370,7 @@ cfs_hash_lookup(struct cfs_hash *hs, const void *key)
 EXPORT_SYMBOL(cfs_hash_lookup);
 
 static void
-cfs_hash_for_each_enter(struct cfs_hash *hs)
-{
+cfs_hash_for_each_enter(struct cfs_hash *hs) {
 	LASSERT(!cfs_hash_is_exiting(hs));
 
 	if (!cfs_hash_with_rehash(hs))
@@ -1397,8 +1395,7 @@ cfs_hash_for_each_enter(struct cfs_hash *hs)
 }
 
 static void
-cfs_hash_for_each_exit(struct cfs_hash *hs)
-{
+cfs_hash_for_each_exit(struct cfs_hash *hs) {
 	int remained;
 	int bits;
 
@@ -1429,8 +1426,7 @@ cfs_hash_for_each_exit(struct cfs_hash *hs)
  */
 static __u64
 cfs_hash_for_each_tight(struct cfs_hash *hs, cfs_hash_for_each_cb_t func,
-			void *data, int remove_safe)
-{
+			void *data, int remove_safe) {
 	struct hlist_node     *hnode;
 	struct hlist_node     *pos;
 	struct cfs_hash_bd	 bd;
@@ -1523,8 +1519,7 @@ EXPORT_SYMBOL(cfs_hash_for_each);
 
 void
 cfs_hash_for_each_safe(struct cfs_hash *hs,
-		       cfs_hash_for_each_cb_t func, void *data)
-{
+		       cfs_hash_for_each_cb_t func, void *data) {
 	cfs_hash_for_each_tight(hs, func, data, 1);
 }
 EXPORT_SYMBOL(cfs_hash_for_each_safe);
@@ -1572,8 +1567,8 @@ EXPORT_SYMBOL(cfs_hash_size_get);
  * two cases, so iteration has to be stopped on change.
  */
 static int
-cfs_hash_for_each_relax(struct cfs_hash *hs, cfs_hash_for_each_cb_t func, void *data)
-{
+cfs_hash_for_each_relax(struct cfs_hash *hs, cfs_hash_for_each_cb_t func,
+			void *data) {
 	struct hlist_node *hnode;
 	struct hlist_node *tmp;
 	struct cfs_hash_bd     bd;
@@ -1634,8 +1629,7 @@ cfs_hash_for_each_relax(struct cfs_hash *hs, cfs_hash_for_each_cb_t func, void *
 
 int
 cfs_hash_for_each_nolock(struct cfs_hash *hs,
-			 cfs_hash_for_each_cb_t func, void *data)
-{
+			 cfs_hash_for_each_cb_t func, void *data) {
 	if (cfs_hash_with_no_lock(hs) ||
 	    cfs_hash_with_rehash_key(hs) ||
 	    !cfs_hash_with_no_itemref(hs))
@@ -1667,8 +1661,7 @@ EXPORT_SYMBOL(cfs_hash_for_each_nolock);
  */
 int
 cfs_hash_for_each_empty(struct cfs_hash *hs,
-			cfs_hash_for_each_cb_t func, void *data)
-{
+			cfs_hash_for_each_cb_t func, void *data) {
 	unsigned  i = 0;
 
 	if (cfs_hash_with_no_lock(hs))
@@ -1726,8 +1719,7 @@ EXPORT_SYMBOL(cfs_hash_hlist_for_each);
    */
 void
 cfs_hash_for_each_key(struct cfs_hash *hs, const void *key,
-		      cfs_hash_for_each_cb_t func, void *data)
-{
+		      cfs_hash_for_each_cb_t func, void *data) {
 	struct hlist_node   *hnode;
 	struct cfs_hash_bd       bds[2];
 	unsigned	    i;
diff --git a/drivers/staging/lustre/lustre/libcfs/libcfs_cpu.c b/drivers/staging/lustre/lustre/libcfs/libcfs_cpu.c
index dbb81b6cc200..31a558115a96 100644
--- a/drivers/staging/lustre/lustre/libcfs/libcfs_cpu.c
+++ b/drivers/staging/lustre/lustre/libcfs/libcfs_cpu.c
@@ -38,7 +38,7 @@
 #include "../../include/linux/libcfs/libcfs.h"
 
 /** Global CPU partition table */
-struct cfs_cpt_table   *cfs_cpt_table __read_mostly = NULL;
+struct cfs_cpt_table   *cfs_cpt_table __read_mostly;
 EXPORT_SYMBOL(cfs_cpt_table);
 
 #ifndef HAVE_LIBCFS_CPT
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c
index 224c65b5ce4e..05f7595f18aa 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c
@@ -333,8 +333,8 @@ cfs_cpt_unset_cpu(struct cfs_cpt_table *cptab, int cpt, int cpu)
 		/* caller doesn't know the partition ID */
 		cpt = cptab->ctb_cpu2cpt[cpu];
 		if (cpt < 0) { /* not set in this CPT-table */
-			CDEBUG(D_INFO, "Try to unset cpu %d which is "
-				       "not in CPT-table %p\n", cpt, cptab);
+			CDEBUG(D_INFO, "Try to unset cpu %d which is not in CPT-table %p\n",
+			       cpt, cptab);
 			return;
 		}
 
@@ -384,8 +384,8 @@ cfs_cpt_set_cpumask(struct cfs_cpt_table *cptab, int cpt, cpumask_t *mask)
 	int	i;
 
 	if (cpus_weight(*mask) == 0 || any_online_cpu(*mask) == NR_CPUS) {
-		CDEBUG(D_INFO, "No online CPU is found in the CPU mask "
-			       "for CPU partition %d\n", cpt);
+		CDEBUG(D_INFO, "No online CPU is found in the CPU mask for CPU partition %d\n",
+		       cpt);
 		return 0;
 	}
 
@@ -579,9 +579,8 @@ cfs_cpt_bind(struct cfs_cpt_table *cptab, int cpt)
 	}
 
 	if (any_online_cpu(*cpumask) == NR_CPUS) {
-		CERROR("No online CPU found in CPU partition %d, did someone "
-		       "do CPU hotplug on system? You might need to reload "
-		       "Lustre modules to keep system working well.\n", cpt);
+		CERROR("No online CPU found in CPU partition %d, did someone do CPU hotplug on system? You might need to reload Lustre modules to keep system working well.\n",
+		       cpt);
 		return -EINVAL;
 	}
 
@@ -737,16 +736,12 @@ cfs_cpt_table_create(int ncpt)
 		ncpt = rc;
 
 	if (ncpt > num_online_cpus() || ncpt > 4 * rc) {
-		CWARN("CPU partition number %d is larger than suggested "
-		      "value (%d), your system may have performance"
-		      "issue or run out of memory while under pressure\n",
+		CWARN("CPU partition number %d is larger than suggested value (%d), your system may have performance issue or run out of memory while under pressure\n",
 		      ncpt, rc);
 	}
 
 	if (num_online_cpus() % ncpt != 0) {
-		CERROR("CPU number %d is not multiple of cpu_npartition %d, "
-		       "please try different cpu_npartitions value or"
-		       "set pattern string by cpu_pattern=STRING\n",
+		CERROR("CPU number %d is not multiple of cpu_npartition %d, please try different cpu_npartitions value or set pattern string by cpu_pattern=STRING\n",
 		       (int)num_online_cpus(), ncpt);
 		goto failed;
 	}
@@ -796,8 +791,7 @@ cfs_cpt_table_create(int ncpt)
 
 	if (cpt != ncpt ||
 	    num != cpus_weight(*cptab->ctb_parts[ncpt - 1].cpt_cpumask)) {
-		CERROR("Expect %d(%d) CPU partitions but got %d(%d), "
-		       "CPU hotplug/unplug while setting?\n",
+		CERROR("Expect %d(%d) CPU partitions but got %d(%d), CPU hotplug/unplug while setting?\n",
 		       cptab->ctb_nparts, num, cpt,
 		       cpus_weight(*cptab->ctb_parts[ncpt - 1].cpt_cpumask));
 		goto failed;
@@ -808,8 +802,7 @@ cfs_cpt_table_create(int ncpt)
 	return cptab;
 
  failed:
-	CERROR("Failed to setup CPU-partition-table with %d "
-	       "CPU-partitions, online HW nodes: %d, HW cpus: %d.\n",
+	CERROR("Failed to setup CPU-partition-table with %d CPU-partitions, online HW nodes: %d, HW cpus: %d.\n",
 	       ncpt, num_online_nodes(), num_online_cpus());
 
 	if (mask != NULL)
@@ -975,9 +968,8 @@ cfs_cpu_notify(struct notifier_block *self, unsigned long action, void *hcpu)
 		warn = any_online_cpu(*cpt_data.cpt_cpumask) >= nr_cpu_ids;
 		mutex_unlock(&cpt_data.cpt_mutex);
 		CDEBUG(warn ? D_WARNING : D_INFO,
-		       "Lustre: can't support CPU plug-out well now, "
-		       "performance and stability could be impacted "
-		       "[CPU %u action: %lx]\n", cpu, action);
+		       "Lustre: can't support CPU plug-out well now, performance and stability could be impacted [CPU %u action: %lx]\n",
+		       cpu, action);
 	}
 
 	return NOTIFY_OK;
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-debug.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-debug.c
index 3298ddf6d3be..12005a71aa73 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-debug.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-debug.c
@@ -87,8 +87,7 @@ void libcfs_run_debug_log_upcall(char *file)
 
 	rc = call_usermodehelper(argv[0], argv, envp, 1);
 	if (rc < 0 && rc != -ENOENT) {
-		CERROR("Error %d invoking LNET debug log upcall %s %s; "
-		       "check /proc/sys/lnet/debug_log_upcall\n",
+		CERROR("Error %d invoking LNET debug log upcall %s %s; check /proc/sys/lnet/debug_log_upcall\n",
 		       rc, argv[0], argv[1]);
 	} else {
 		CDEBUG(D_HA, "Invoked LNET debug log upcall %s %s\n",
@@ -114,8 +113,7 @@ void libcfs_run_upcall(char **argv)
 
 	rc = call_usermodehelper(argv[0], argv, envp, 1);
 	if (rc < 0 && rc != -ENOENT) {
-		CERROR("Error %d invoking LNET upcall %s %s%s%s%s%s%s%s%s; "
-		       "check /proc/sys/lnet/upcall\n",
+		CERROR("Error %d invoking LNET upcall %s %s%s%s%s%s%s%s%s; check /proc/sys/lnet/upcall\n",
 		       rc, argv[0], argv[1],
 		       argc < 3 ? "" : ",", argc < 3 ? "" : argv[2],
 		       argc < 4 ? "" : ",", argc < 4 ? "" : argv[3],
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-proc.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-proc.c
index bbe2c68c18a6..83d3f08a37b2 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-proc.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-proc.c
@@ -365,8 +365,8 @@ static int __proc_cpt_table(void *data, int write,
 		if (rc >= 0)
 			break;
 
-		LIBCFS_FREE(buf, len);
 		if (rc == -EFBIG) {
+			LIBCFS_FREE(buf, len);
 			len <<= 1;
 			continue;
 		}
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c
index 939b33dd6520..b91a1f95bbd0 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c
@@ -279,8 +279,7 @@ libcfs_sock_write (struct socket *sock, void *buffer, int nob, int timeout)
 			rc = kernel_setsockopt(sock, SOL_SOCKET, SO_SNDTIMEO,
 					     (char *)&tv, sizeof(tv));
 			if (rc != 0) {
-				CERROR("Can't set socket send timeout "
-				       "%ld.%06d: %d\n",
+				CERROR("Can't set socket send timeout %ld.%06d: %d\n",
 				       (long)tv.tv_sec, (int)tv.tv_usec, rc);
 				return rc;
 			}
diff --git a/drivers/staging/lustre/lustre/libcfs/tracefile.c b/drivers/staging/lustre/lustre/libcfs/tracefile.c
index 7561030c96e6..5917c31c7ed6 100644
--- a/drivers/staging/lustre/lustre/libcfs/tracefile.c
+++ b/drivers/staging/lustre/lustre/libcfs/tracefile.c
@@ -196,8 +196,7 @@ static void cfs_tcd_shrink(struct cfs_trace_cpu_data *tcd)
 	 */
 
 	if (printk_ratelimit())
-		printk(KERN_WARNING "debug daemon buffer overflowed; "
-		       "discarding 10%% of pages (%d of %ld)\n",
+		printk(KERN_WARNING "debug daemon buffer overflowed; discarding 10%% of pages (%d of %ld)\n",
 		       pgcount + 1, tcd->tcd_cur_pages);
 
 	INIT_LIST_HEAD(&pc.pc_pages);
@@ -357,8 +356,8 @@ int libcfs_debug_vmsg2(struct libcfs_debug_msg_data *msgdata,
 	}
 
 	if (*(string_buf+needed-1) != '\n')
-		printk(KERN_INFO "format at %s:%d:%s doesn't end in "
-		       "newline\n", file, msgdata->msg_line, msgdata->msg_fn);
+		printk(KERN_INFO "format at %s:%d:%s doesn't end in newline\n",
+		       file, msgdata->msg_line, msgdata->msg_fn);
 
 	header.ph_len = known_size + needed;
 	debug_buf = (char *)page_address(tage->page) + tage->used;
@@ -715,8 +714,8 @@ int cfs_tracefile_dump_all_pages(char *filename)
 		kunmap(tage->page);
 
 		if (rc != (int)tage->used) {
-			printk(KERN_WARNING "wanted to write %u but wrote "
-			       "%d\n", tage->used, rc);
+			printk(KERN_WARNING "wanted to write %u but wrote %d\n",
+			       tage->used, rc);
 			put_pages_back(&pc);
 			__LASSERT(list_empty(&pc.pc_pages));
 			break;
@@ -875,8 +874,8 @@ int cfs_trace_daemon_command(char *str)
 		strcpy(cfs_tracefile, str);
 
 		printk(KERN_INFO
-		       "Lustre: debug daemon will attempt to start writing "
-		       "to %s (%lukB max)\n", cfs_tracefile,
+		       "Lustre: debug daemon will attempt to start writing to %s (%lukB max)\n",
+		       cfs_tracefile,
 		       (long)(cfs_tracefile_size >> 10));
 
 		cfs_trace_start_thread();
@@ -914,15 +913,15 @@ int cfs_trace_set_debug_mb(int mb)
 
 	if (mb < num_possible_cpus()) {
 		printk(KERN_WARNING
-		       "Lustre: %d MB is too small for debug buffer size, "
-		       "setting it to %d MB.\n", mb, num_possible_cpus());
+		       "Lustre: %d MB is too small for debug buffer size, setting it to %d MB.\n",
+		       mb, num_possible_cpus());
 		mb = num_possible_cpus();
 	}
 
 	if (mb > limit) {
 		printk(KERN_WARNING
-		       "Lustre: %d MB is too large for debug buffer size, "
-		       "setting it to %d MB.\n", mb, limit);
+		       "Lustre: %d MB is too large for debug buffer size, setting it to %d MB.\n",
+		       mb, limit);
 		mb = limit;
 	}
 
@@ -1004,8 +1003,8 @@ static int tracefiled(void *arg)
 			if (IS_ERR(filp)) {
 				rc = PTR_ERR(filp);
 				filp = NULL;
-				printk(KERN_WARNING "couldn't open %s: "
-				       "%d\n", cfs_tracefile, rc);
+				printk(KERN_WARNING "couldn't open %s: %d\n",
+				       cfs_tracefile, rc);
 			}
 		}
 		cfs_tracefile_read_unlock();
@@ -1034,8 +1033,8 @@ static int tracefiled(void *arg)
 			kunmap(tage->page);
 
 			if (rc != (int)tage->used) {
-				printk(KERN_WARNING "wanted to write %u "
-				       "but wrote %d\n", tage->used, rc);
+				printk(KERN_WARNING "wanted to write %u but wrote %d\n",
+				       tage->used, rc);
 				put_pages_back(&pc);
 				__LASSERT(list_empty(&pc.pc_pages));
 			}
@@ -1047,8 +1046,7 @@ static int tracefiled(void *arg)
 		if (!list_empty(&pc.pc_pages)) {
 			int i;
 
-			printk(KERN_ALERT "Lustre: trace pages aren't "
-			       " empty\n");
+			printk(KERN_ALERT "Lustre: trace pages aren't empty\n");
 			printk(KERN_ERR "total cpus(%d): ",
 			       num_possible_cpus());
 			for (i = 0; i < num_possible_cpus(); i++)
@@ -1061,8 +1059,8 @@ static int tracefiled(void *arg)
 			i = 0;
 			list_for_each_entry_safe(tage, tmp, &pc.pc_pages,
 						     linkage)
-				printk(KERN_ERR "page %d belongs to cpu "
-				       "%d\n", ++i, tage->cpu);
+				printk(KERN_ERR "page %d belongs to cpu %d\n",
+				       ++i, tage->cpu);
 			printk(KERN_ERR "There are %d pages unwritten\n",
 			       i);
 		}
diff --git a/drivers/staging/lustre/lustre/llite/dcache.c b/drivers/staging/lustre/lustre/llite/dcache.c
index f692261e9b5c..5bb9c85cec81 100644
--- a/drivers/staging/lustre/lustre/llite/dcache.c
+++ b/drivers/staging/lustre/lustre/llite/dcache.c
@@ -259,8 +259,8 @@ void ll_invalidate_aliases(struct inode *inode)
 
 	ll_lock_dcache(inode);
 	ll_d_hlist_for_each_entry(dentry, p, &inode->i_dentry, d_u.d_alias) {
-		CDEBUG(D_DENTRY, "dentry in drop %pd (%p) parent %p "
-		       "inode %p flags %d\n", dentry, dentry, dentry->d_parent,
+		CDEBUG(D_DENTRY, "dentry in drop %pd (%p) parent %p inode %p flags %d\n",
+		       dentry, dentry, dentry->d_parent,
 		       dentry->d_inode, dentry->d_flags);
 
 		if (unlikely(dentry == dentry->d_sb->s_root)) {
diff --git a/drivers/staging/lustre/lustre/llite/dir.c b/drivers/staging/lustre/lustre/llite/dir.c
index a79fd65ec4c6..1ac7a702ce26 100644
--- a/drivers/staging/lustre/lustre/llite/dir.c
+++ b/drivers/staging/lustre/lustre/llite/dir.c
@@ -163,7 +163,7 @@ static int ll_dir_filler(void *_hash, struct page *page0)
 
 	LASSERT(max_pages > 0 && max_pages <= MD_MAX_BRW_PAGES);
 
-	page_pool = kzalloc(sizeof(page) * max_pages, GFP_NOFS);
+	page_pool = kcalloc(max_pages, sizeof(page), GFP_NOFS);
 	if (page_pool) {
 		page_pool[0] = page0;
 	} else {
@@ -228,8 +228,8 @@ static int ll_dir_filler(void *_hash, struct page *page0)
 			if (ll_pagevec_add(&lru_pvec, page) == 0)
 				ll_pagevec_lru_add_file(&lru_pvec);
 		} else {
-			CDEBUG(D_VFSTRACE, "page %lu add to page cache failed:"
-			       " %d\n", offset, ret);
+			CDEBUG(D_VFSTRACE, "page %lu add to page cache failed: %d\n",
+			       offset, ret);
 		}
 		page_cache_release(page);
 	}
@@ -275,14 +275,14 @@ static struct page *ll_dir_page_locate(struct inode *dir, __u64 *hash,
 	struct page *page;
 	int found;
 
-	TREE_READ_LOCK_IRQ(mapping);
+	spin_lock_irq(&mapping->tree_lock);
 	found = radix_tree_gang_lookup(&mapping->page_tree,
 				       (void **)&page, offset, 1);
 	if (found > 0) {
 		struct lu_dirpage *dp;
 
 		page_cache_get(page);
-		TREE_READ_UNLOCK_IRQ(mapping);
+		spin_unlock_irq(&mapping->tree_lock);
 		/*
 		 * In contrast to find_lock_page() we are sure that directory
 		 * page cannot be truncated (while DLM lock is held) and,
@@ -326,7 +326,7 @@ static struct page *ll_dir_page_locate(struct inode *dir, __u64 *hash,
 		}
 
 	} else {
-		TREE_READ_UNLOCK_IRQ(mapping);
+		spin_unlock_irq(&mapping->tree_lock);
 		page = NULL;
 	}
 	return page;
@@ -600,8 +600,8 @@ static int ll_readdir(struct file *filp, struct dir_context *ctx)
 	int			api32	= ll_need_32bit_api(sbi);
 	int			rc;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:inode=%lu/%u(%p) pos %lu/%llu "
-	       " 32bit_api %d\n", inode->i_ino, inode->i_generation,
+	CDEBUG(D_VFSTRACE, "VFS Op:inode=%lu/%u(%p) pos %lu/%llu 32bit_api %d\n",
+	       inode->i_ino, inode->i_generation,
 	       inode, (unsigned long)lfd->lfd_pos, i_size_read(inode), api32);
 
 	if (lfd->lfd_pos == MDS_DIR_END_OFF) {
@@ -661,7 +661,7 @@ int ll_dir_setdirstripe(struct inode *dir, struct lmv_user_md *lump,
 	int mode;
 	int err;
 
-	mode = (0755 & (S_IRWXUGO|S_ISVTX) & ~current->fs->umask) | S_IFDIR;
+	mode = (0755 & ~current_umask()) | S_IFDIR;
 	op_data = ll_prep_md_op_data(NULL, dir, NULL, filename,
 				     strlen(filename), mode, LUSTRE_OPC_MKDIR,
 				     lump);
@@ -715,10 +715,9 @@ int ll_dir_setstripe(struct inode *inode, struct lov_user_md *lump,
 			break;
 		}
 		default: {
-			CDEBUG(D_IOCTL, "bad userland LOV MAGIC:"
-					" %#08x != %#08x nor %#08x\n",
-					lump->lmm_magic, LOV_USER_MAGIC_V1,
-					LOV_USER_MAGIC_V3);
+			CDEBUG(D_IOCTL, "bad userland LOV MAGIC: %#08x != %#08x nor %#08x\n",
+			       lump->lmm_magic, LOV_USER_MAGIC_V1,
+			       LOV_USER_MAGIC_V3);
 			return -EINVAL;
 		}
 		}
@@ -814,8 +813,8 @@ int ll_dir_getstripe(struct inode *inode, struct lov_mds_md **lmmp,
 	rc = md_getattr(sbi->ll_md_exp, op_data, &req);
 	ll_finish_md_op_data(op_data);
 	if (rc < 0) {
-		CDEBUG(D_INFO, "md_getattr failed on inode "
-		       "%lu/%u: rc %d\n", inode->i_ino,
+		CDEBUG(D_INFO, "md_getattr failed on inode %lu/%u: rc %d\n",
+		       inode->i_ino,
 		       inode->i_generation, rc);
 		goto out;
 	}
@@ -1013,8 +1012,7 @@ static int ll_ioc_copy_end(struct super_block *sb, struct hsm_copy *copy)
 				     copy->hc_hai.hai_action == HSMA_ARCHIVE);
 		iput(inode);
 		if (rc) {
-			CDEBUG(D_HSM, "Could not read file data version. "
-				      "Request could not be confirmed.\n");
+			CDEBUG(D_HSM, "Could not read file data version. Request could not be confirmed.\n");
 			if (hpk.hpk_errval == 0)
 				hpk.hpk_errval = -rc;
 			goto progress;
@@ -1028,8 +1026,7 @@ static int ll_ioc_copy_end(struct super_block *sb, struct hsm_copy *copy)
 		 * to check anyway. */
 		if ((copy->hc_hai.hai_action == HSMA_ARCHIVE) &&
 		    (copy->hc_data_version != data_version)) {
-			CDEBUG(D_HSM, "File data version mismatched. "
-			      "File content was changed during archiving. "
+			CDEBUG(D_HSM, "File data version mismatched. File content was changed during archiving. "
 			       DFID", start:%#llx current:%#llx\n",
 			       PFID(&copy->hc_hai.hai_fid),
 			       copy->hc_data_version, data_version);
@@ -1384,7 +1381,7 @@ lmv_out_free:
 		if (copy_from_user(lumv1, lumv1p, sizeof(*lumv1)))
 			return -EFAULT;
 
-		if ((lumv1->lmm_magic == LOV_USER_MAGIC_V3) ) {
+		if (lumv1->lmm_magic == LOV_USER_MAGIC_V3) {
 			if (copy_from_user(&lumv3, lumv3p, sizeof(lumv3)))
 				return -EFAULT;
 		}
@@ -1509,8 +1506,7 @@ out_rmdir:
 					       cmd == LL_IOC_MDC_GETINFO)) {
 				rc = 0;
 				goto skip_lmm;
-			}
-			else
+			} else
 				goto out_req;
 		}
 
@@ -1694,64 +1690,6 @@ out_poll:
 		OBD_FREE_PTR(check);
 		return rc;
 	}
-#if LUSTRE_VERSION_CODE < OBD_OCD_VERSION(2, 7, 50, 0)
-	case LL_IOC_QUOTACTL_18: {
-		/* copy the old 1.x quota struct for internal use, then copy
-		 * back into old format struct.  For 1.8 compatibility. */
-		struct if_quotactl_18 *qctl_18;
-		struct if_quotactl *qctl_20;
-
-		qctl_18 = kzalloc(sizeof(*qctl_18), GFP_NOFS);
-		if (!qctl_18)
-			return -ENOMEM;
-
-		qctl_20 = kzalloc(sizeof(*qctl_20), GFP_NOFS);
-		if (!qctl_20) {
-			rc = -ENOMEM;
-			goto out_quotactl_18;
-		}
-
-		if (copy_from_user(qctl_18, (void *)arg, sizeof(*qctl_18))) {
-			rc = -ENOMEM;
-			goto out_quotactl_20;
-		}
-
-		QCTL_COPY(qctl_20, qctl_18);
-		qctl_20->qc_idx = 0;
-
-		/* XXX: dqb_valid was borrowed as a flag to mark that
-		 *      only mds quota is wanted */
-		if (qctl_18->qc_cmd == Q_GETQUOTA &&
-		    qctl_18->qc_dqblk.dqb_valid) {
-			qctl_20->qc_valid = QC_MDTIDX;
-			qctl_20->qc_dqblk.dqb_valid = 0;
-		} else if (qctl_18->obd_uuid.uuid[0] != '\0') {
-			qctl_20->qc_valid = QC_UUID;
-			qctl_20->obd_uuid = qctl_18->obd_uuid;
-		} else {
-			qctl_20->qc_valid = QC_GENERAL;
-		}
-
-		rc = quotactl_ioctl(sbi, qctl_20);
-
-		if (rc == 0) {
-			QCTL_COPY(qctl_18, qctl_20);
-			qctl_18->obd_uuid = qctl_20->obd_uuid;
-
-			if (copy_to_user((void *)arg, qctl_18,
-					     sizeof(*qctl_18)))
-				rc = -EFAULT;
-		}
-
-out_quotactl_20:
-		OBD_FREE_PTR(qctl_20);
-out_quotactl_18:
-		OBD_FREE_PTR(qctl_18);
-		return rc;
-	}
-#else
-#warning "remove old LL_IOC_QUOTACTL_18 compatibility code"
-#endif /* LUSTRE_VERSION_CODE < OBD_OCD_VERSION(2, 7, 50, 0) */
 	case LL_IOC_QUOTACTL: {
 		struct if_quotactl *qctl;
 
diff --git a/drivers/staging/lustre/lustre/llite/file.c b/drivers/staging/lustre/lustre/llite/file.c
index a2ae9a68a9a0..35a2df01528c 100644
--- a/drivers/staging/lustre/lustre/llite/file.c
+++ b/drivers/staging/lustre/lustre/llite/file.c
@@ -170,8 +170,8 @@ static int ll_close_inode_openhandle(struct obd_export *md_exp,
 		 * OSTs and send setattr to back to MDS. */
 		rc = ll_som_update(inode, op_data);
 		if (rc) {
-			CERROR("inode %lu mdc Size-on-MDS update failed: "
-			       "rc = %d\n", inode->i_ino, rc);
+			CERROR("inode %lu mdc Size-on-MDS update failed: rc = %d\n",
+			       inode->i_ino, rc);
 			rc = 0;
 		}
 	} else if (rc) {
@@ -247,7 +247,7 @@ int ll_md_real_close(struct inode *inode, fmode_t fmode)
 		return 0;
 	}
 
-	och=*och_p;
+	och = *och_p;
 	*och_p = NULL;
 	mutex_unlock(&lli->lli_och_mutex);
 
@@ -358,7 +358,7 @@ int ll_file_release(struct inode *inode, struct file *file)
 	fd = LUSTRE_FPRIVATE(file);
 	LASSERT(fd != NULL);
 
-	/* The last ref on @file, maybe not the the owner pid of statahead.
+	/* The last ref on @file, maybe not the owner pid of statahead.
 	 * Different processes can open the same dir, "ll_opendir_key" means:
 	 * it is me that should stop the statahead thread. */
 	if (S_ISDIR(inode->i_mode) && lli->lli_opendir_key == fd &&
@@ -975,8 +975,8 @@ int ll_inode_getattr(struct inode *inode, struct obdo *obdo,
 		struct ost_id *oi = lsm ? &lsm->lsm_oi : &obdo->o_oi;
 
 		obdo_refresh_inode(inode, obdo, obdo->o_valid);
-		CDEBUG(D_INODE, "objid "DOSTID" size %llu, blocks %llu,"
-		       " blksize %lu\n", POSTID(oi), i_size_read(inode),
+		CDEBUG(D_INODE, "objid " DOSTID " size %llu, blocks %llu, blksize %lu\n",
+		       POSTID(oi), i_size_read(inode),
 		       (unsigned long long)inode->i_blocks,
 		       1UL << inode->i_blkbits);
 	}
@@ -1403,8 +1403,8 @@ int ll_lov_getstripe_ea_info(struct inode *inode, const char *filename,
 	rc = md_getattr_name(sbi->ll_md_exp, op_data, &req);
 	ll_finish_md_op_data(op_data);
 	if (rc < 0) {
-		CDEBUG(D_INFO, "md_getattr_name failed "
-		       "on %s: rc %d\n", filename, rc);
+		CDEBUG(D_INFO, "md_getattr_name failed on %s: rc %d\n",
+		       filename, rc);
 		goto out;
 	}
 
@@ -2221,8 +2221,8 @@ ll_file_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		if (cmd == LL_IOC_SETFLAGS) {
 			if ((flags & LL_FILE_IGNORE_LOCK) &&
 			    !(file->f_flags & O_DIRECT)) {
-				CERROR("%s: unable to disable locking on "
-				       "non-O_DIRECT file\n", current->comm);
+				CERROR("%s: unable to disable locking on non-O_DIRECT file\n",
+				       current->comm);
 				return -EINVAL;
 			}
 
@@ -2848,7 +2848,7 @@ ldlm_mode_t ll_take_md_lock(struct inode *inode, __u64 bits,
 			    struct lustre_handle *lockh, __u64 flags,
 			    ldlm_mode_t mode)
 {
-	ldlm_policy_data_t policy = { .l_inodebits = {bits}};
+	ldlm_policy_data_t policy = { .l_inodebits = {bits} };
 	struct lu_fid *fid;
 	ldlm_mode_t rc;
 
diff --git a/drivers/staging/lustre/lustre/llite/llite_capa.c b/drivers/staging/lustre/lustre/llite/llite_capa.c
index b1e39ee412cd..aec9a44120c0 100644
--- a/drivers/staging/lustre/lustre/llite/llite_capa.c
+++ b/drivers/staging/lustre/lustre/llite/llite_capa.c
@@ -540,8 +540,7 @@ static int ll_update_capa(struct obd_capa *ocapa, struct lustre_capa *capa)
 			if (rc == -EIO && !capa_is_expired(ocapa)) {
 				delay_capa_renew(ocapa, 120);
 				DEBUG_CAPA(D_ERROR, &ocapa->c_capa,
-					   "renewal failed: -EIO, "
-					   "retry in 2 mins");
+					   "renewal failed: -EIO, retry in 2 mins");
 				ll_capa_renewal_retries++;
 				goto retry;
 			} else {
diff --git a/drivers/staging/lustre/lustre/llite/llite_close.c b/drivers/staging/lustre/lustre/llite/llite_close.c
index 84e0003f2daf..21b4a5026776 100644
--- a/drivers/staging/lustre/lustre/llite/llite_close.c
+++ b/drivers/staging/lustre/lustre/llite/llite_close.c
@@ -90,8 +90,7 @@ void ll_queue_done_writing(struct inode *inode, unsigned long flags)
 		struct ll_close_queue *lcq = ll_i2sbi(inode)->ll_lcq;
 
 		if (lli->lli_flags & LLIF_MDS_SIZE_LOCK)
-			CWARN("ino %lu/%u(flags %u) som valid it just after "
-			      "recovery\n",
+			CWARN("ino %lu/%u(flags %u) som valid it just after recovery\n",
 			      inode->i_ino, inode->i_generation,
 			      lli->lli_flags);
 		/* DONE_WRITING is allowed and inode has no dirty page. */
@@ -124,8 +123,8 @@ void ll_done_writing_attr(struct inode *inode, struct md_op_data *op_data)
 	op_data->op_flags |= MF_SOM_CHANGE;
 	/* Check if Size-on-MDS attributes are valid. */
 	if (lli->lli_flags & LLIF_MDS_SIZE_LOCK)
-		CERROR("ino %lu/%u(flags %u) som valid it just after "
-		       "recovery\n", inode->i_ino, inode->i_generation,
+		CERROR("ino %lu/%u(flags %u) som valid it just after recovery\n",
+		       inode->i_ino, inode->i_generation,
 		       lli->lli_flags);
 
 	if (!cl_local_size(inode)) {
@@ -218,8 +217,8 @@ int ll_som_update(struct inode *inode, struct md_op_data *op_data)
 
 	LASSERT(op_data != NULL);
 	if (lli->lli_flags & LLIF_MDS_SIZE_LOCK)
-		CERROR("ino %lu/%u(flags %u) som valid it just after "
-		       "recovery\n", inode->i_ino, inode->i_generation,
+		CERROR("ino %lu/%u(flags %u) som valid it just after recovery\n",
+		       inode->i_ino, inode->i_generation,
 		       lli->lli_flags);
 
 	OBDO_ALLOC(oa);
@@ -238,9 +237,8 @@ int ll_som_update(struct inode *inode, struct md_op_data *op_data)
 		if (rc) {
 			oa->o_valid = 0;
 			if (rc != -ENOENT)
-				CERROR("inode_getattr failed (%d): unable to "
-				       "send a Size-on-MDS attribute update "
-				       "for inode %lu/%u\n", rc, inode->i_ino,
+				CERROR("inode_getattr failed (%d): unable to send a Size-on-MDS attribute update for inode %lu/%u\n",
+				       rc, inode->i_ino,
 				       inode->i_generation);
 		} else {
 			CDEBUG(D_INODE, "Size-on-MDS update on "DFID"\n",
diff --git a/drivers/staging/lustre/lustre/llite/llite_internal.h b/drivers/staging/lustre/lustre/llite/llite_internal.h
index 77d1c12704b4..37306e0c7aad 100644
--- a/drivers/staging/lustre/lustre/llite/llite_internal.h
+++ b/drivers/staging/lustre/lustre/llite/llite_internal.h
@@ -1435,8 +1435,8 @@ static inline void ll_set_lock_data(struct obd_export *exp, struct inode *inode,
 		 * case the dcache being cleared */
 		if (it->d.lustre.it_remote_lock_mode) {
 			handle.cookie = it->d.lustre.it_remote_lock_handle;
-			CDEBUG(D_DLMTRACE, "setting l_data to inode %p"
-			       "(%lu/%u) for remote lock %#llx\n", inode,
+			CDEBUG(D_DLMTRACE, "setting l_data to inode %p(%lu/%u) for remote lock %#llx\n",
+			       inode,
 			       inode->i_ino, inode->i_generation,
 			       handle.cookie);
 			md_set_lock_data(exp, &handle.cookie, inode, NULL);
@@ -1444,8 +1444,8 @@ static inline void ll_set_lock_data(struct obd_export *exp, struct inode *inode,
 
 		handle.cookie = it->d.lustre.it_lock_handle;
 
-		CDEBUG(D_DLMTRACE, "setting l_data to inode %p (%lu/%u)"
-		       " for lock %#llx\n", inode, inode->i_ino,
+		CDEBUG(D_DLMTRACE, "setting l_data to inode %p (%lu/%u) for lock %#llx\n",
+		       inode, inode->i_ino,
 		       inode->i_generation, handle.cookie);
 
 		md_set_lock_data(exp, &handle.cookie, inode,
@@ -1489,8 +1489,8 @@ static inline void __d_lustre_invalidate(struct dentry *dentry)
  */
 static inline void d_lustre_invalidate(struct dentry *dentry, int nested)
 {
-	CDEBUG(D_DENTRY, "invalidate dentry %pd (%p) parent %p inode %p "
-	       "refc %d\n", dentry, dentry,
+	CDEBUG(D_DENTRY, "invalidate dentry %pd (%p) parent %p inode %p refc %d\n",
+	       dentry, dentry,
 	       dentry->d_parent, dentry->d_inode, d_count(dentry));
 
 	spin_lock_nested(&dentry->d_lock,
@@ -1509,24 +1509,6 @@ static inline void d_lustre_revalidate(struct dentry *dentry)
 	spin_unlock(&dentry->d_lock);
 }
 
-#if LUSTRE_VERSION_CODE < OBD_OCD_VERSION(2, 7, 50, 0)
-/* Compatibility for old (1.8) compiled userspace quota code */
-struct if_quotactl_18 {
-	__u32		   qc_cmd;
-	__u32		   qc_type;
-	__u32		   qc_id;
-	__u32		   qc_stat;
-	struct obd_dqinfo       qc_dqinfo;
-	struct obd_dqblk	qc_dqblk;
-	char		    obd_type[16];
-	struct obd_uuid	 obd_uuid;
-};
-#define LL_IOC_QUOTACTL_18	      _IOWR('f', 162, struct if_quotactl_18 *)
-/* End compatibility for old (1.8) compiled userspace quota code */
-#else
-#warning "remove old LL_IOC_QUOTACTL_18 compatibility code"
-#endif /* LUSTRE_VERSION_CODE < OBD_OCD_VERSION(2, 7, 50, 0) */
-
 enum {
 	LL_LAYOUT_GEN_NONE  = ((__u32)-2),	/* layout lock was cancelled */
 	LL_LAYOUT_GEN_EMPTY = ((__u32)-1)	/* for empty layout */
diff --git a/drivers/staging/lustre/lustre/llite/llite_lib.c b/drivers/staging/lustre/lustre/llite/llite_lib.c
index 7b6b9e2e0102..a3367bfb1456 100644
--- a/drivers/staging/lustre/lustre/llite/llite_lib.c
+++ b/drivers/staging/lustre/lustre/llite/llite_lib.c
@@ -250,12 +250,11 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 
 	data->ocd_brw_size = MD_MAX_BRW_SIZE;
 
-	err = obd_connect(NULL, &sbi->ll_md_exp, obd, &sbi->ll_sb_uuid, data, NULL);
+	err = obd_connect(NULL, &sbi->ll_md_exp, obd, &sbi->ll_sb_uuid,
+			  data, NULL);
 	if (err == -EBUSY) {
-		LCONSOLE_ERROR_MSG(0x14f, "An MDT (md %s) is performing "
-				   "recovery, of which this client is not a "
-				   "part. Please wait for recovery to complete,"
-				   " abort, or time out.\n", md);
+		LCONSOLE_ERROR_MSG(0x14f, "An MDT (md %s) is performing recovery, of which this client is not a part. Please wait for recovery to complete, abort, or time out.\n",
+				   md);
 		goto out;
 	} else if (err) {
 		CERROR("cannot connect to %s: rc = %d\n", md, err);
@@ -267,8 +266,8 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 	err = obd_fid_init(sbi->ll_md_exp->exp_obd, sbi->ll_md_exp,
 			   LUSTRE_SEQ_METADATA);
 	if (err) {
-		CERROR("%s: Can't init metadata layer FID infrastructure, "
-		       "rc = %d\n", sbi->ll_md_exp->exp_obd->obd_name, err);
+		CERROR("%s: Can't init metadata layer FID infrastructure, rc = %d\n",
+		       sbi->ll_md_exp->exp_obd->obd_name, err);
 		goto out_md;
 	}
 
@@ -296,10 +295,7 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 		buf = kzalloc(PAGE_CACHE_SIZE, GFP_KERNEL);
 		obd_connect_flags2str(buf, PAGE_CACHE_SIZE,
 				      valid ^ CLIENT_CONNECT_MDT_REQD, ",");
-		LCONSOLE_ERROR_MSG(0x170, "Server %s does not support "
-				   "feature(s) needed for correct operation "
-				   "of this client (%s). Please upgrade "
-				   "server or downgrade client.\n",
+		LCONSOLE_ERROR_MSG(0x170, "Server %s does not support feature(s) needed for correct operation of this client (%s). Please upgrade server or downgrade client.\n",
 				   sbi->ll_md_exp->exp_obd->obd_name, buf);
 		OBD_FREE(buf, PAGE_CACHE_SIZE);
 		err = -EPROTO;
@@ -325,8 +321,7 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 
 	if ((sbi->ll_flags & LL_SBI_USER_XATTR) &&
 	    !(data->ocd_connect_flags & OBD_CONNECT_XATTR)) {
-		LCONSOLE_INFO("Disabling user_xattr feature because "
-			      "it is not supported on the server\n");
+		LCONSOLE_INFO("Disabling user_xattr feature because it is not supported on the server\n");
 		sbi->ll_flags &= ~LL_SBI_USER_XATTR;
 	}
 
@@ -351,8 +346,7 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 	} else {
 		if (sbi->ll_flags & LL_SBI_RMT_CLIENT) {
 			sbi->ll_flags &= ~LL_SBI_RMT_CLIENT;
-			LCONSOLE_INFO("client claims to be remote, but server "
-				      "rejected, forced to be local.\n");
+			LCONSOLE_INFO("client claims to be remote, but server rejected, forced to be local.\n");
 		}
 	}
 
@@ -429,8 +423,8 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 	if (sbi->ll_flags & LL_SBI_RMT_CLIENT)
 		data->ocd_connect_flags |= OBD_CONNECT_RMT_CLIENT_FORCE;
 
-	CDEBUG(D_RPCTRACE, "ocd_connect_flags: %#llx ocd_version: %d "
-	       "ocd_grant: %d\n", data->ocd_connect_flags,
+	CDEBUG(D_RPCTRACE, "ocd_connect_flags: %#llx ocd_version: %d ocd_grant: %d\n",
+	       data->ocd_connect_flags,
 	       data->ocd_version, data->ocd_grant);
 
 	obd->obd_upcall.onu_owner = &sbi->ll_lco;
@@ -441,10 +435,8 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 	err = obd_connect(NULL, &sbi->ll_dt_exp, obd, &sbi->ll_sb_uuid, data,
 			  NULL);
 	if (err == -EBUSY) {
-		LCONSOLE_ERROR_MSG(0x150, "An OST (dt %s) is performing "
-				   "recovery, of which this client is not a "
-				   "part.  Please wait for recovery to "
-				   "complete, abort, or time out.\n", dt);
+		LCONSOLE_ERROR_MSG(0x150, "An OST (dt %s) is performing recovery, of which this client is not a part.  Please wait for recovery to complete, abort, or time out.\n",
+				   dt);
 		goto out_md;
 	} else if (err) {
 		CERROR("%s: Cannot connect to %s: rc = %d\n",
@@ -457,8 +449,8 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 	err = obd_fid_init(sbi->ll_dt_exp->exp_obd, sbi->ll_dt_exp,
 			   LUSTRE_SEQ_METADATA);
 	if (err) {
-		CERROR("%s: Can't init data layer FID infrastructure, "
-		       "rc = %d\n", sbi->ll_dt_exp->exp_obd->obd_name, err);
+		CERROR("%s: Can't init data layer FID infrastructure, rc = %d\n",
+		       sbi->ll_dt_exp->exp_obd->obd_name, err);
 		goto out_dt;
 	}
 
@@ -698,9 +690,9 @@ void lustre_dump_dentry(struct dentry *dentry, int recur)
 	list_for_each(tmp, &dentry->d_subdirs)
 		subdirs++;
 
-	CERROR("dentry %p dump: name=%pd parent=%p, inode=%p, count=%u,"
-	       " flags=0x%x, fsdata=%p, %d subdirs\n", dentry, dentry,
-	       dentry->d_parent, dentry->d_inode, d_count(dentry),
+	CERROR("dentry %p dump: name=%pd parent=%pd (%p), inode=%p, count=%u, flags=0x%x, fsdata=%p, %d subdirs\n",
+	       dentry, dentry, dentry->d_parent, dentry->d_parent,
+	       dentry->d_inode, d_count(dentry),
 	       dentry->d_flags, dentry->d_fsdata, subdirs);
 	if (dentry->d_inode != NULL)
 		ll_dump_inode(dentry->d_inode);
@@ -710,6 +702,7 @@ void lustre_dump_dentry(struct dentry *dentry, int recur)
 
 	list_for_each(tmp, &dentry->d_subdirs) {
 		struct dentry *d = list_entry(tmp, struct dentry, d_child);
+
 		lustre_dump_dentry(d, recur - 1);
 	}
 }
@@ -754,9 +747,9 @@ void ll_kill_super(struct super_block *sb)
 		return;
 
 	sbi = ll_s2sbi(sb);
-	/* we need to restore s_dev from changed for clustered NFS before put_super
-	 * because new kernels have cached s_dev and change sb->s_dev in
-	 * put_super not affected real removing devices */
+	/* we need to restore s_dev from changed for clustered NFS before
+	 * put_super because new kernels have cached s_dev and change sb->s_dev
+	 * in put_super not affected real removing devices */
 	if (sbi) {
 		sb->s_dev = sbi->ll_sdev_orig;
 		sbi->ll_umounting = 1;
@@ -814,25 +807,6 @@ static int ll_options(char *options, int *flags)
 			*flags &= ~tmp;
 			goto next;
 		}
-#if LUSTRE_VERSION_CODE < OBD_OCD_VERSION(2, 5, 50, 0)
-		tmp = ll_set_opt("acl", s1, LL_SBI_ACL);
-		if (tmp) {
-			/* Ignore deprecated mount option.  The client will
-			 * always try to mount with ACL support, whether this
-			 * is used depends on whether server supports it. */
-			LCONSOLE_ERROR_MSG(0x152, "Ignoring deprecated "
-						  "mount option 'acl'.\n");
-			goto next;
-		}
-		tmp = ll_set_opt("noacl", s1, LL_SBI_ACL);
-		if (tmp) {
-			LCONSOLE_ERROR_MSG(0x152, "Ignoring deprecated "
-						  "mount option 'noacl'.\n");
-			goto next;
-		}
-#else
-#warning "{no}acl options have been deprecated since 1.8, please remove them"
-#endif
 		tmp = ll_set_opt("remote_client", s1, LL_SBI_RMT_CLIENT);
 		if (tmp) {
 			*flags |= tmp;
@@ -1038,9 +1012,8 @@ int ll_fill_super(struct super_block *sb, struct vfsmount *mnt)
 	/* Profile set with LCFG_MOUNTOPT so we can find our mdc and osc obds */
 	lprof = class_get_profile(profilenm);
 	if (lprof == NULL) {
-		LCONSOLE_ERROR_MSG(0x156, "The client profile '%s' could not be"
-				   " read from the MGS.  Does that filesystem "
-				   "exist?\n", profilenm);
+		LCONSOLE_ERROR_MSG(0x156, "The client profile '%s' could not be read from the MGS.  Does that filesystem exist?\n",
+				   profilenm);
 		err = -EINVAL;
 		goto out_free;
 	}
@@ -1119,9 +1092,8 @@ void ll_put_super(struct super_block *sb)
 	}
 
 	next = 0;
-	while ((obd = class_devices_in_group(&sbi->ll_sb_uuid, &next)) !=NULL) {
+	while ((obd = class_devices_in_group(&sbi->ll_sb_uuid, &next)))
 		class_manual_cleanup(obd);
-	}
 
 	if (sbi->ll_flags & LL_SBI_VERBOSE)
 		LCONSOLE_WARN("Unmounted %s\n", profilenm ? profilenm : "");
@@ -1150,14 +1122,14 @@ struct inode *ll_inode_from_resource_lock(struct ldlm_lock *lock)
 	lock_res_and_lock(lock);
 	if (lock->l_resource->lr_lvb_inode) {
 		struct ll_inode_info *lli;
+
 		lli = ll_i2info(lock->l_resource->lr_lvb_inode);
 		if (lli->lli_inode_magic == LLI_INODE_MAGIC) {
 			inode = igrab(lock->l_resource->lr_lvb_inode);
 		} else {
 			inode = lock->l_resource->lr_lvb_inode;
 			LDLM_DEBUG_LIMIT(inode->i_state & I_FREEING ?  D_INFO :
-					 D_WARNING, lock, "lr_lvb_inode %p is "
-					 "bogus: magic %08x",
+					 D_WARNING, lock, "lr_lvb_inode %p is bogus: magic %08x",
 					 lock->l_resource->lr_lvb_inode,
 					 lli->lli_inode_magic);
 			inode = NULL;
@@ -1730,11 +1702,11 @@ void ll_update_inode(struct inode *inode, struct lustre_md *md)
 	if (body->valid & OBD_MD_FLTYPE)
 		inode->i_mode = (inode->i_mode & ~S_IFMT)|(body->mode & S_IFMT);
 	LASSERT(inode->i_mode != 0);
-	if (S_ISREG(inode->i_mode)) {
-		inode->i_blkbits = min(PTLRPC_MAX_BRW_BITS + 1, LL_MAX_BLKSIZE_BITS);
-	} else {
+	if (S_ISREG(inode->i_mode))
+		inode->i_blkbits = min(PTLRPC_MAX_BRW_BITS + 1,
+				       LL_MAX_BLKSIZE_BITS);
+	else
 		inode->i_blkbits = inode->i_sb->s_blocksize_bits;
-	}
 	if (body->valid & OBD_MD_FLUID)
 		inode->i_uid = make_kuid(&init_user_ns, body->uid);
 	if (body->valid & OBD_MD_FLGID)
@@ -1778,9 +1750,7 @@ void ll_update_inode(struct inode *inode, struct lustre_md *md)
 				if (lli->lli_flags & (LLIF_DONE_WRITING |
 						      LLIF_EPOCH_PENDING |
 						      LLIF_SOM_DIRTY)) {
-					CERROR("ino %lu flags %u still has "
-					       "size authority! do not trust "
-					       "the size got from MDS\n",
+					CERROR("ino %lu flags %u still has size authority! do not trust the size got from MDS\n",
 					       inode->i_ino, lli->lli_flags);
 				} else {
 					/* Use old size assignment to avoid
@@ -1848,6 +1818,7 @@ void ll_read_inode2(struct inode *inode, void *opaque)
 
 	if (S_ISREG(inode->i_mode)) {
 		struct ll_sb_info *sbi = ll_i2sbi(inode);
+
 		inode->i_op = &ll_file_inode_operations;
 		inode->i_fop = sbi->ll_fop;
 		inode->i_mapping->a_ops = (struct address_space_operations *)&ll_aops;
@@ -1878,11 +1849,10 @@ void ll_delete_inode(struct inode *inode)
 
 	/* Workaround for LU-118 */
 	if (inode->i_data.nrpages) {
-		TREE_READ_LOCK_IRQ(&inode->i_data);
-		TREE_READ_UNLOCK_IRQ(&inode->i_data);
+		spin_lock_irq(&inode->i_data.tree_lock);
+		spin_unlock_irq(&inode->i_data.tree_lock);
 		LASSERTF(inode->i_data.nrpages == 0,
-			 "inode=%lu/%u(%p) nrpages=%lu, see "
-			 "http://jira.whamcloud.com/browse/LU-118\n",
+			 "inode=%lu/%u(%p) nrpages=%lu, see http://jira.whamcloud.com/browse/LU-118\n",
 			 inode->i_ino, inode->i_generation, inode,
 			 inode->i_data.nrpages);
 	}
@@ -2164,7 +2134,13 @@ int ll_obd_statfs(struct inode *inode, void *arg)
 	__u32 flags;
 	int len = 0, rc;
 
-	if (!inode || !(sbi = ll_i2sbi(inode))) {
+	if (!inode) {
+		rc = -EINVAL;
+		goto out_statfs;
+	}
+
+	sbi = ll_i2sbi(inode);
+	if (!sbi) {
 		rc = -EINVAL;
 		goto out_statfs;
 	}
@@ -2396,21 +2372,6 @@ char *ll_get_fsname(struct super_block *sb, char *buf, int buflen)
 	return buf;
 }
 
-static char* ll_d_path(struct dentry *dentry, char *buf, int bufsize)
-{
-	char *path = NULL;
-
-	struct path p;
-
-	p.dentry = dentry;
-	p.mnt = current->fs->root.mnt;
-	path_get(&p);
-	path = d_path(&p, buf, bufsize);
-	path_put(&p);
-
-	return path;
-}
-
 void ll_dirty_page_discard_warn(struct page *page, int ioret)
 {
 	char *buf, *path = NULL;
@@ -2422,12 +2383,12 @@ void ll_dirty_page_discard_warn(struct page *page, int ioret)
 	if (buf != NULL) {
 		dentry = d_find_alias(page->mapping->host);
 		if (dentry != NULL)
-			path = ll_d_path(dentry, buf, PAGE_SIZE);
+			path = dentry_path_raw(dentry, buf, PAGE_SIZE);
 	}
 
 	CDEBUG(D_WARNING,
-	       "%s: dirty page discard: %s/fid: "DFID"/%s may get corrupted "
-	       "(rc %d)\n", ll_get_fsname(page->mapping->host->i_sb, NULL, 0),
+	       "%s: dirty page discard: %s/fid: " DFID "/%s may get corrupted (rc %d)\n",
+	       ll_get_fsname(page->mapping->host->i_sb, NULL, 0),
 	       s2lsi(page->mapping->host->i_sb)->lsi_lmd->lmd_dev,
 	       PFID(&obj->cob_header.coh_lu.loh_fid),
 	       (path && !IS_ERR(path)) ? path : "", ioret);
diff --git a/drivers/staging/lustre/lustre/llite/llite_mmap.c b/drivers/staging/lustre/lustre/llite/llite_mmap.c
index ba1c047ae927..479bf428780c 100644
--- a/drivers/staging/lustre/lustre/llite/llite_mmap.c
+++ b/drivers/staging/lustre/lustre/llite/llite_mmap.c
@@ -234,8 +234,7 @@ static int ll_page_mkwrite0(struct vm_area_struct *vma, struct page *vmpage,
 			 */
 			unlock_page(vmpage);
 
-			CDEBUG(D_MMAP, "Race on page_mkwrite %p/%lu, page has "
-			       "been written out, retry.\n",
+			CDEBUG(D_MMAP, "Race on page_mkwrite %p/%lu, page has been written out, retry.\n",
 			       vmpage, vmpage->index);
 
 			*retry = true;
@@ -366,8 +365,7 @@ restart:
 			vmf->page = NULL;
 
 			if (!printed && ++count > 16) {
-				CWARN("the page is under heavy contention,"
-				      "maybe your app(%s) needs revising :-)\n",
+				CWARN("the page is under heavy contention, maybe your app(%s) needs revising :-)\n",
 				      current->comm);
 				printed = true;
 			}
@@ -393,8 +391,7 @@ static int ll_page_mkwrite(struct vm_area_struct *vma, struct vm_fault *vmf)
 		result = ll_page_mkwrite0(vma, vmf->page, &retry);
 
 		if (!printed && ++count > 16) {
-			CWARN("app(%s): the page %lu of file %lu is under heavy"
-			      " contention.\n",
+			CWARN("app(%s): the page %lu of file %lu is under heavy contention.\n",
 			      current->comm, vmf->pgoff,
 			      file_inode(vma->vm_file)->i_ino);
 			printed = true;
diff --git a/drivers/staging/lustre/lustre/llite/llite_rmtacl.c b/drivers/staging/lustre/lustre/llite/llite_rmtacl.c
index 586f49a374ec..f4da156f3874 100644
--- a/drivers/staging/lustre/lustre/llite/llite_rmtacl.c
+++ b/drivers/staging/lustre/lustre/llite/llite_rmtacl.c
@@ -131,8 +131,8 @@ int rct_add(struct rmtacl_ctl_table *rct, pid_t key, int ops)
 	spin_lock(&rct->rct_lock);
 	e = __rct_search(rct, key);
 	if (unlikely(e != NULL)) {
-		CWARN("Unexpected stale rmtacl_entry found: "
-		      "[key: %d] [ops: %d]\n", (int)key, ops);
+		CWARN("Unexpected stale rmtacl_entry found: [key: %d] [ops: %d]\n",
+		      (int)key, ops);
 		rce_free(e);
 	}
 	list_add_tail(&rce->rce_list, &rct->rct_entries[rce_hashfunc(key)]);
@@ -263,8 +263,7 @@ int ee_add(struct eacl_table *et, pid_t key, struct lu_fid *fid, int type,
 	spin_lock(&et->et_lock);
 	e = __et_search_del(et, key, fid, type);
 	if (unlikely(e != NULL)) {
-		CWARN("Unexpected stale eacl_entry found: "
-		      "[key: %d] [fid: "DFID"] [type: %d]\n",
+		CWARN("Unexpected stale eacl_entry found: [key: %d] [fid: " DFID "] [type: %d]\n",
 		      (int)key, PFID(fid), type);
 		ee_free(e);
 	}
diff --git a/drivers/staging/lustre/lustre/llite/lloop.c b/drivers/staging/lustre/lustre/llite/lloop.c
index 9e31b789b790..031248840642 100644
--- a/drivers/staging/lustre/lustre/llite/lloop.c
+++ b/drivers/staging/lustre/lustre/llite/lloop.c
@@ -777,8 +777,8 @@ static int __init lloop_init(void)
 
 	if (max_loop < 1 || max_loop > 256) {
 		max_loop = MAX_LOOP_DEFAULT;
-		CWARN("lloop: invalid max_loop (must be between"
-		      " 1 and 256), using default (%u)\n", max_loop);
+		CWARN("lloop: invalid max_loop (must be between 1 and 256), using default (%u)\n",
+		      max_loop);
 	}
 
 	lloop_major = register_blkdev(0, "lloop");
@@ -792,11 +792,11 @@ static int __init lloop_init(void)
 	if (ll_iocontrol_magic == NULL)
 		goto out_mem1;
 
-	loop_dev = kzalloc(max_loop * sizeof(*loop_dev), GFP_KERNEL);
+	loop_dev = kcalloc(max_loop, sizeof(*loop_dev), GFP_KERNEL);
 	if (!loop_dev)
 		goto out_mem1;
 
-	disks = kzalloc(max_loop * sizeof(*disks), GFP_KERNEL);
+	disks = kcalloc(max_loop, sizeof(*disks), GFP_KERNEL);
 	if (!disks)
 		goto out_mem2;
 
diff --git a/drivers/staging/lustre/lustre/llite/lproc_llite.c b/drivers/staging/lustre/lustre/llite/lproc_llite.c
index 3b3df9f03422..e6a909e6faf0 100644
--- a/drivers/staging/lustre/lustre/llite/lproc_llite.c
+++ b/drivers/staging/lustre/lustre/llite/lproc_llite.c
@@ -227,8 +227,9 @@ static int ll_max_readahead_mb_seq_show(struct seq_file *m, void *v)
 	return lprocfs_seq_read_frac_helper(m, pages_number, mult);
 }
 
-static ssize_t ll_max_readahead_mb_seq_write(struct file *file, const char *buffer,
-					 size_t count, loff_t *off)
+static ssize_t ll_max_readahead_mb_seq_write(struct file *file,
+					     const char __user *buffer,
+					     size_t count, loff_t *off)
 {
 	struct super_block *sb = ((struct seq_file *)file->private_data)->private;
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
@@ -269,7 +270,7 @@ static int ll_max_readahead_per_file_mb_seq_show(struct seq_file *m, void *v)
 }
 
 static ssize_t ll_max_readahead_per_file_mb_seq_write(struct file *file,
-						  const char *buffer,
+						  const char __user *buffer,
 						  size_t count, loff_t *off)
 {
 	struct super_block *sb = ((struct seq_file *)file->private_data)->private;
@@ -283,8 +284,7 @@ static ssize_t ll_max_readahead_per_file_mb_seq_write(struct file *file,
 
 	if (pages_number < 0 ||
 		pages_number > sbi->ll_ra_info.ra_max_pages) {
-		CERROR("can't set file readahead more than"
-		       "max_read_ahead_mb %lu MB\n",
+		CERROR("can't set file readahead more than max_read_ahead_mb %lu MB\n",
 		       sbi->ll_ra_info.ra_max_pages);
 		return -ERANGE;
 	}
@@ -313,7 +313,7 @@ static int ll_max_read_ahead_whole_mb_seq_show(struct seq_file *m, void *unused)
 }
 
 static ssize_t ll_max_read_ahead_whole_mb_seq_write(struct file *file,
-						const char *buffer,
+						const char __user *buffer,
 						size_t count, loff_t *off)
 {
 	struct super_block *sb = ((struct seq_file *)file->private_data)->private;
@@ -329,9 +329,8 @@ static ssize_t ll_max_read_ahead_whole_mb_seq_write(struct file *file,
 	 * algorithm does this anyway so it's pointless to set it larger. */
 	if (pages_number < 0 ||
 	    pages_number > sbi->ll_ra_info.ra_max_pages_per_file) {
-		CERROR("can't set max_read_ahead_whole_mb more than "
-		       "max_read_ahead_per_file_mb: %lu\n",
-			sbi->ll_ra_info.ra_max_pages_per_file >> (20 - PAGE_CACHE_SHIFT));
+		CERROR("can't set max_read_ahead_whole_mb more than max_read_ahead_per_file_mb: %lu\n",
+		       sbi->ll_ra_info.ra_max_pages_per_file >> (20 - PAGE_CACHE_SHIFT));
 		return -ERANGE;
 	}
 
@@ -469,8 +468,9 @@ static int ll_checksum_seq_show(struct seq_file *m, void *v)
 	return seq_printf(m, "%u\n", (sbi->ll_flags & LL_SBI_CHECKSUM) ? 1 : 0);
 }
 
-static ssize_t ll_checksum_seq_write(struct file *file, const char *buffer,
-				 size_t count, loff_t *off)
+static ssize_t ll_checksum_seq_write(struct file *file,
+				     const char __user *buffer,
+				     size_t count, loff_t *off)
 {
 	struct super_block *sb = ((struct seq_file *)file->private_data)->private;
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
@@ -504,8 +504,9 @@ static int ll_max_rw_chunk_seq_show(struct seq_file *m, void *v)
 	return seq_printf(m, "%lu\n", ll_s2sbi(sb)->ll_max_rw_chunk);
 }
 
-static ssize_t ll_max_rw_chunk_seq_write(struct file *file, const char *buffer,
-				     size_t count, loff_t *off)
+static ssize_t ll_max_rw_chunk_seq_write(struct file *file,
+					 const char __user *buffer,
+					 size_t count, loff_t *off)
 {
 	struct super_block *sb = ((struct seq_file *)file->private_data)->private;
 	int rc, val;
@@ -533,8 +534,8 @@ static int ll_rd_track_id(struct seq_file *m, enum stats_track_type type)
 	}
 }
 
-static int ll_wr_track_id(const char *buffer, unsigned long count, void *data,
-			  enum stats_track_type type)
+static int ll_wr_track_id(const char __user *buffer, unsigned long count,
+			  void *data, enum stats_track_type type)
 {
 	struct super_block *sb = data;
 	int rc, pid;
@@ -556,8 +557,9 @@ static int ll_track_pid_seq_show(struct seq_file *m, void *v)
 	return ll_rd_track_id(m, STATS_TRACK_PID);
 }
 
-static ssize_t ll_track_pid_seq_write(struct file *file, const char *buffer,
-				  size_t count, loff_t *off)
+static ssize_t ll_track_pid_seq_write(struct file *file,
+				      const char __user *buffer,
+				      size_t count, loff_t *off)
 {
 	struct seq_file *seq = file->private_data;
 	return ll_wr_track_id(buffer, count, seq->private, STATS_TRACK_PID);
@@ -569,8 +571,9 @@ static int ll_track_ppid_seq_show(struct seq_file *m, void *v)
 	return ll_rd_track_id(m, STATS_TRACK_PPID);
 }
 
-static ssize_t ll_track_ppid_seq_write(struct file *file, const char *buffer,
-				   size_t count, loff_t *off)
+static ssize_t ll_track_ppid_seq_write(struct file *file,
+				       const char __user *buffer,
+				       size_t count, loff_t *off)
 {
 	struct seq_file *seq = file->private_data;
 	return ll_wr_track_id(buffer, count, seq->private, STATS_TRACK_PPID);
@@ -582,8 +585,9 @@ static int ll_track_gid_seq_show(struct seq_file *m, void *v)
 	return ll_rd_track_id(m, STATS_TRACK_GID);
 }
 
-static ssize_t ll_track_gid_seq_write(struct file *file, const char *buffer,
-				  size_t count, loff_t *off)
+static ssize_t ll_track_gid_seq_write(struct file *file,
+				      const char __user *buffer,
+				      size_t count, loff_t *off)
 {
 	struct seq_file *seq = file->private_data;
 	return ll_wr_track_id(buffer, count, seq->private, STATS_TRACK_GID);
@@ -598,8 +602,9 @@ static int ll_statahead_max_seq_show(struct seq_file *m, void *v)
 	return seq_printf(m, "%u\n", sbi->ll_sa_max);
 }
 
-static ssize_t ll_statahead_max_seq_write(struct file *file, const char *buffer,
-				      size_t count, loff_t *off)
+static ssize_t ll_statahead_max_seq_write(struct file *file,
+					  const char __user *buffer,
+					  size_t count, loff_t *off)
 {
 	struct super_block *sb = ((struct seq_file *)file->private_data)->private;
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
@@ -612,8 +617,8 @@ static ssize_t ll_statahead_max_seq_write(struct file *file, const char *buffer,
 	if (val >= 0 && val <= LL_SA_RPC_MAX)
 		sbi->ll_sa_max = val;
 	else
-		CERROR("Bad statahead_max value %d. Valid values are in the "
-		       "range [0, %d]\n", val, LL_SA_RPC_MAX);
+		CERROR("Bad statahead_max value %d. Valid values are in the range [0, %d]\n",
+		       val, LL_SA_RPC_MAX);
 
 	return count;
 }
@@ -628,8 +633,9 @@ static int ll_statahead_agl_seq_show(struct seq_file *m, void *v)
 			sbi->ll_flags & LL_SBI_AGL_ENABLED ? 1 : 0);
 }
 
-static ssize_t ll_statahead_agl_seq_write(struct file *file, const char *buffer,
-				      size_t count, loff_t *off)
+static ssize_t ll_statahead_agl_seq_write(struct file *file,
+					  const char __user *buffer,
+					  size_t count, loff_t *off)
 {
 	struct super_block *sb = ((struct seq_file *)file->private_data)->private;
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
@@ -672,8 +678,9 @@ static int ll_lazystatfs_seq_show(struct seq_file *m, void *v)
 			(sbi->ll_flags & LL_SBI_LAZYSTATFS) ? 1 : 0);
 }
 
-static ssize_t ll_lazystatfs_seq_write(struct file *file, const char *buffer,
-				   size_t count, loff_t *off)
+static ssize_t ll_lazystatfs_seq_write(struct file *file,
+				       const char __user *buffer,
+				       size_t count, loff_t *off)
 {
 	struct super_block *sb = ((struct seq_file *)file->private_data)->private;
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
@@ -761,8 +768,8 @@ static int ll_sbi_flags_seq_show(struct seq_file *m, void *v)
 
 	while (flags != 0) {
 		if (ARRAY_SIZE(str) <= i) {
-			CERROR("%s: Revise array LL_SBI_FLAGS to match sbi "
-				"flags please.\n", ll_get_fsname(sb, NULL, 0));
+			CERROR("%s: Revise array LL_SBI_FLAGS to match sbi flags please.\n",
+			       ll_get_fsname(sb, NULL, 0));
 			return -EINVAL;
 		}
 
@@ -787,7 +794,8 @@ static int ll_xattr_cache_seq_show(struct seq_file *m, void *v)
 	return rc;
 }
 
-static ssize_t ll_xattr_cache_seq_write(struct file *file, const char *buffer,
+static ssize_t ll_xattr_cache_seq_write(struct file *file,
+					const char __user *buffer,
 					size_t count, loff_t *off)
 {
 	struct seq_file *seq = file->private_data;
@@ -813,7 +821,7 @@ LPROC_SEQ_FOPS(ll_xattr_cache);
 
 static struct lprocfs_vars lprocfs_llite_obd_vars[] = {
 	{ "uuid",	  &ll_sb_uuid_fops,	  NULL, 0 },
-	//{ "mntpt_path",   ll_rd_path,	     0, 0 },
+	/* { "mntpt_path",   ll_rd_path,	     0, 0 }, */
 	{ "fstype",       &ll_fstype_fops,	  NULL, 0 },
 	{ "site",	  &ll_site_stats_fops,    NULL, 0 },
 	{ "blocksize",    &ll_blksize_fops,	  NULL, 0 },
@@ -823,7 +831,7 @@ static struct lprocfs_vars lprocfs_llite_obd_vars[] = {
 	{ "filestotal",   &ll_filestotal_fops,    NULL, 0 },
 	{ "filesfree",    &ll_filesfree_fops,	  NULL, 0 },
 	{ "client_type",  &ll_client_type_fops,   NULL, 0 },
-	//{ "filegroups",   lprocfs_rd_filegroups,  0, 0 },
+	/* { "filegroups",   lprocfs_rd_filegroups,  0, 0 }, */
 	{ "max_read_ahead_mb", &ll_max_readahead_mb_fops, NULL },
 	{ "max_read_ahead_per_file_mb", &ll_max_readahead_per_file_mb_fops,
 		NULL },
@@ -1133,8 +1141,8 @@ static void ll_display_extents_info(struct ll_rw_extents_info *io_extents,
 		read_cum += r;
 		write_cum += w;
 		end = 1 << (i + LL_HIST_START - units);
-		seq_printf(seq, "%4lu%c - %4lu%c%c: %14lu %4lu %4lu  | "
-			   "%14lu %4lu %4lu\n", start, *unitp, end, *unitp,
+		seq_printf(seq, "%4lu%c - %4lu%c%c: %14lu %4lu %4lu  | %14lu %4lu %4lu\n",
+			   start, *unitp, end, *unitp,
 			   (i == LL_HIST_MAX - 1) ? '+' : ' ',
 			   r, pct(r, read_tot), pct(read_cum, read_tot),
 			   w, pct(w, write_tot), pct(write_cum, write_tot));
@@ -1160,8 +1168,7 @@ static int ll_rw_extents_stats_pp_seq_show(struct seq_file *seq, void *v)
 
 	if (!sbi->ll_rw_stats_on) {
 		seq_printf(seq, "disabled\n"
-				"write anything in this file to activate, "
-				"then 0 or \"[D/d]isabled\" to deactivate\n");
+			   "write anything in this file to activate, then 0 or \"[D/d]isabled\" to deactivate\n");
 		return 0;
 	}
 	seq_printf(seq, "snapshot_time:	 %lu.%lu (secs.usecs)\n",
@@ -1239,8 +1246,7 @@ static int ll_rw_extents_stats_seq_show(struct seq_file *seq, void *v)
 
 	if (!sbi->ll_rw_stats_on) {
 		seq_printf(seq, "disabled\n"
-				"write anything in this file to activate, "
-				"then 0 or \"[D/d]isabled\" to deactivate\n");
+			   "write anything in this file to activate, then 0 or \"[D/d]isabled\" to deactivate\n");
 		return 0;
 	}
 	seq_printf(seq, "snapshot_time:	 %lu.%lu (secs.usecs)\n",
@@ -1418,8 +1424,7 @@ static int ll_rw_offset_stats_seq_show(struct seq_file *seq, void *v)
 
 	if (!sbi->ll_rw_stats_on) {
 		seq_printf(seq, "disabled\n"
-				"write anything in this file to activate, "
-				"then 0 or \"[D/d]isabled\" to deactivate\n");
+			   "write anything in this file to activate, then 0 or \"[D/d]isabled\" to deactivate\n");
 		return 0;
 	}
 	spin_lock(&sbi->ll_process_lock);
diff --git a/drivers/staging/lustre/lustre/llite/namei.c b/drivers/staging/lustre/lustre/llite/namei.c
index 8e926b385a60..4f361b77c749 100644
--- a/drivers/staging/lustre/lustre/llite/namei.c
+++ b/drivers/staging/lustre/lustre/llite/namei.c
@@ -83,8 +83,8 @@ static int ll_set_inode(struct inode *inode, void *opaque)
 
 	lli->lli_fid = body->fid1;
 	if (unlikely(!(body->valid & OBD_MD_FLTYPE))) {
-		CERROR("Can not initialize inode "DFID" without object type: "
-		       "valid = %#llx\n", PFID(&lli->lli_fid), body->valid);
+		CERROR("Can not initialize inode " DFID " without object type: valid = %#llx\n",
+		       PFID(&lli->lli_fid), body->valid);
 		return -EINVAL;
 	}
 
@@ -264,7 +264,7 @@ int ll_md_blocking_ast(struct ldlm_lock *lock, struct ldlm_lock_desc *desc,
 
 		if ((bits & (MDS_INODELOCK_LOOKUP | MDS_INODELOCK_PERM)) &&
 		    inode->i_sb->s_root != NULL &&
-		    is_root_inode(inode))
+		    !is_root_inode(inode))
 			ll_invalidate_aliases(inode);
 
 		iput(inode);
@@ -598,8 +598,7 @@ static int ll_atomic_open(struct inode *dir, struct dentry *dentry,
 	long long lookup_flags = LOOKUP_OPEN;
 	int rc = 0;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd,dir=%lu/%u(%p),file %p,"
-			   "open_flags %x,mode %x opened %d\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd,dir=%lu/%u(%p),file %p,open_flags %x,mode %x opened %d\n",
 	       dentry, dir->i_ino,
 	       dir->i_generation, dir, file, open_flags, mode, *opened);
 
@@ -843,8 +842,7 @@ static int ll_create_nd(struct inode *dir, struct dentry *dentry,
 {
 	int rc;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd,dir=%lu/%u(%p),"
-			   "flags=%u, excl=%d\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd,dir=%lu/%u(%p),flags=%u, excl=%d\n",
 	       dentry, dir->i_ino,
 	       dir->i_generation, dir, mode, want_excl);
 
diff --git a/drivers/staging/lustre/lustre/llite/remote_perm.c b/drivers/staging/lustre/lustre/llite/remote_perm.c
index c05a9126cfe3..a58182600dae 100644
--- a/drivers/staging/lustre/lustre/llite/remote_perm.c
+++ b/drivers/staging/lustre/lustre/llite/remote_perm.c
@@ -194,7 +194,7 @@ int ll_update_remote_perm(struct inode *inode, struct mdt_remote_perm *perm)
 
 	if (!lli->lli_remote_perms)
 		lli->lli_remote_perms = perm_hash;
-	else if (perm_hash)
+	else
 		free_rmtperm_hash(perm_hash);
 
 	head = lli->lli_remote_perms + remote_perm_hashfunc(perm->rp_uid);
@@ -209,8 +209,7 @@ again:
 			continue;
 		if (tmp->lrp_fsgid != perm->rp_fsgid)
 			continue;
-		if (lrp)
-			free_ll_remote_perm(lrp);
+		free_ll_remote_perm(lrp);
 		lrp = tmp;
 		break;
 	}
diff --git a/drivers/staging/lustre/lustre/llite/rw.c b/drivers/staging/lustre/lustre/llite/rw.c
index 1f53b9863385..10a0421366d0 100644
--- a/drivers/staging/lustre/lustre/llite/rw.c
+++ b/drivers/staging/lustre/lustre/llite/rw.c
@@ -121,8 +121,8 @@ static struct ll_cl_context *ll_cl_init(struct file *file,
 			/* this is too bad. Someone is trying to write the
 			 * page w/o holding inode mutex. This means we can
 			 * add dirty pages into cache during truncate */
-			CERROR("Proc %s is dirting page w/o inode lock, this"
-			       "will break truncate.\n", current->comm);
+			CERROR("Proc %s is dirtying page w/o inode lock, this will break truncate\n",
+			       current->comm);
 			dump_stack();
 			LBUG();
 			return ERR_PTR(-EIO);
@@ -145,7 +145,7 @@ static struct ll_cl_context *ll_cl_init(struct file *file,
 		 */
 		io->ci_lockreq = CILR_NEVER;
 
-		pos = (vmpage->index << PAGE_CACHE_SHIFT);
+		pos = vmpage->index << PAGE_CACHE_SHIFT;
 
 		/* Create a temp IO to serve write. */
 		result = cl_io_rw_init(env, io, CIT_WRITE, pos, PAGE_CACHE_SIZE);
@@ -606,8 +606,8 @@ stride_pg_count(pgoff_t st_off, unsigned long st_len, unsigned long st_pgs,
 	else
 		pg_count = start_left + st_pgs * (end - start - 1) + end_left;
 
-	CDEBUG(D_READA, "st_off %lu, st_len %lu st_pgs %lu off %lu length %lu"
-	       "pgcount %lu\n", st_off, st_len, st_pgs, off, length, pg_count);
+	CDEBUG(D_READA, "st_off %lu, st_len %lu st_pgs %lu off %lu length %lu pgcount %lu\n",
+	       st_off, st_len, st_pgs, off, length, pg_count);
 
 	return pg_count;
 }
@@ -667,10 +667,10 @@ static int ll_read_ahead_pages(const struct lu_env *env,
 			/* FIXME: This assertion only is valid when it is for
 			 * forward read-ahead, it will be fixed when backward
 			 * read-ahead is implemented */
-			LASSERTF(page_idx > ria->ria_stoff, "Invalid page_idx %lu"
-				"rs %lu re %lu ro %lu rl %lu rp %lu\n", page_idx,
-				ria->ria_start, ria->ria_end, ria->ria_stoff,
-				ria->ria_length, ria->ria_pages);
+			LASSERTF(page_idx > ria->ria_stoff, "Invalid page_idx %lu rs %lu re %lu ro %lu rl %lu rp %lu\n",
+				 page_idx,
+				 ria->ria_start, ria->ria_end, ria->ria_stoff,
+				 ria->ria_length, ria->ria_pages);
 			offset = page_idx - ria->ria_stoff;
 			offset = offset % (ria->ria_length);
 			if (offset > ria->ria_pages) {
@@ -927,8 +927,8 @@ static void ras_stride_increase_window(struct ll_readahead_state *ras,
 
 	LASSERT(ras->ras_stride_length > 0);
 	LASSERTF(ras->ras_window_start + ras->ras_window_len
-		 >= ras->ras_stride_offset, "window_start %lu, window_len %lu"
-		 " stride_offset %lu\n", ras->ras_window_start,
+		 >= ras->ras_stride_offset, "window_start %lu, window_len %lu stride_offset %lu\n",
+		 ras->ras_window_start,
 		 ras->ras_window_len, ras->ras_stride_offset);
 
 	stride_len = ras->ras_window_start + ras->ras_window_len -
diff --git a/drivers/staging/lustre/lustre/llite/rw26.c b/drivers/staging/lustre/lustre/llite/rw26.c
index 4c77ae8b9350..2f21304046aa 100644
--- a/drivers/staging/lustre/lustre/llite/rw26.c
+++ b/drivers/staging/lustre/lustre/llite/rw26.c
@@ -183,7 +183,7 @@ static int ll_set_page_dirty(struct page *vmpage)
 	return __set_page_dirty_nobuffers(vmpage);
 }
 
-#define MAX_DIRECTIO_SIZE 2*1024*1024*1024UL
+#define MAX_DIRECTIO_SIZE (2*1024*1024*1024UL)
 
 static inline int ll_get_user_pages(int rw, unsigned long user_addr,
 				    size_t size, struct page ***pages,
@@ -417,7 +417,7 @@ static ssize_t ll_direct_IO_26(int rw, struct kiocb *iocb,
 
 		result = iov_iter_get_pages_alloc(iter, &pages, count, &offs);
 		if (likely(result > 0)) {
-			int n = (result + offs + PAGE_SIZE - 1) / PAGE_SIZE;
+			int n = DIV_ROUND_UP(result + offs, PAGE_SIZE);
 			result = ll_direct_IO_26_seg(env, io, rw, inode,
 						     file->f_mapping,
 						     result, file_offset,
@@ -535,7 +535,7 @@ const struct address_space_operations ll_aops = {
 #else
 const struct address_space_operations_ext ll_aops = {
 	.orig_aops.readpage       = ll_readpage,
-//	.orig_aops.readpages      = ll_readpages,
+/*	.orig_aops.readpages      = ll_readpages, */
 	.orig_aops.direct_IO      = ll_direct_IO_26,
 	.orig_aops.writepage      = ll_writepage,
 	.orig_aops.writepages     = ll_writepages,
diff --git a/drivers/staging/lustre/lustre/llite/statahead.c b/drivers/staging/lustre/lustre/llite/statahead.c
index 09d965e76842..6ad9dd0fe2b3 100644
--- a/drivers/staging/lustre/lustre/llite/statahead.c
+++ b/drivers/staging/lustre/lustre/llite/statahead.c
@@ -334,8 +334,7 @@ static void ll_sa_entry_put(struct ll_statahead_info *sai,
 		LASSERT(ll_sa_entry_unhashed(entry));
 
 		ll_sa_entry_cleanup(sai, entry);
-		if (entry->se_inode)
-			iput(entry->se_inode);
+		iput(entry->se_inode);
 
 		OBD_FREE(entry, entry->se_size);
 		atomic_dec(&sai->sai_cache_count);
@@ -915,7 +914,7 @@ static int do_sa_revalidate(struct inode *dir, struct ll_sa_entry *entry,
 	return rc;
 }
 
-static void ll_statahead_one(struct dentry *parent, const char* entry_name,
+static void ll_statahead_one(struct dentry *parent, const char *entry_name,
 			     int entry_name_len)
 {
 	struct inode	     *dir    = parent->d_inode;
@@ -1491,10 +1490,7 @@ ll_sai_unplug(struct ll_statahead_info *sai, struct ll_sa_entry *entry)
 		sai->sai_consecutive_miss++;
 		if (sa_low_hit(sai) && thread_is_running(thread)) {
 			atomic_inc(&sbi->ll_sa_wrong);
-			CDEBUG(D_READA, "Statahead for dir "DFID" hit "
-			       "ratio too low: hit/miss %llu/%llu"
-			       ", sent/replied %llu/%llu, stopping "
-			       "statahead thread\n",
+			CDEBUG(D_READA, "Statahead for dir " DFID " hit ratio too low: hit/miss %llu/%llu, sent/replied %llu/%llu, stopping statahead thread\n",
 			       PFID(&lli->lli_fid), sai->sai_hit,
 			       sai->sai_miss, sai->sai_sent,
 			       sai->sai_replied);
@@ -1612,8 +1608,7 @@ int do_statahead_enter(struct inode *dir, struct dentry **dentryp,
 				} else if ((*dentryp)->d_inode != inode) {
 					/* revalidate, but inode is recreated */
 					CDEBUG(D_READA,
-					      "stale dentry %pd inode %lu/%u, "
-					      "statahead inode %lu/%u\n",
+					      "stale dentry %pd inode %lu/%u, statahead inode %lu/%u\n",
 					      *dentryp,
 					      (*dentryp)->d_inode->i_ino,
 					      (*dentryp)->d_inode->i_generation,
@@ -1665,8 +1660,7 @@ int do_statahead_enter(struct inode *dir, struct dentry **dentryp,
 	if (unlikely(sai->sai_inode != parent->d_inode)) {
 		struct ll_inode_info *nlli = ll_i2info(parent->d_inode);
 
-		CWARN("Race condition, someone changed %pd just now: "
-		      "old parent "DFID", new parent "DFID"\n",
+		CWARN("Race condition, someone changed %pd just now: old parent "DFID", new parent "DFID"\n",
 		      *dentryp,
 		      PFID(&lli->lli_fid), PFID(&nlli->lli_fid));
 		dput(parent);
diff --git a/drivers/staging/lustre/lustre/llite/super25.c b/drivers/staging/lustre/lustre/llite/super25.c
index e61dbed120a3..6aff155651cc 100644
--- a/drivers/staging/lustre/lustre/llite/super25.c
+++ b/drivers/staging/lustre/lustre/llite/super25.c
@@ -162,7 +162,7 @@ static int __init init_lustre_lite(void)
 
 	/* Nodes with small feet have little entropy
 	 * the NID for this node gives the most entropy in the low bits */
-	for (i=0; ; i++) {
+	for (i = 0; ; i++) {
 		if (LNetGetId(i, &lnet_id) == -ENOENT) {
 			break;
 		}
diff --git a/drivers/staging/lustre/lustre/llite/symlink.c b/drivers/staging/lustre/lustre/llite/symlink.c
index eccd3a717a4d..686b6a574cc5 100644
--- a/drivers/staging/lustre/lustre/llite/symlink.c
+++ b/drivers/staging/lustre/lustre/llite/symlink.c
@@ -100,8 +100,8 @@ static int ll_readlink_internal(struct inode *inode,
 	if (*symname == NULL ||
 	    strnlen(*symname, symlen) != symlen - 1) {
 		/* not full/NULL terminated */
-		CERROR("inode %lu: symlink not NULL terminated string"
-			"of length %d\n", inode->i_ino, symlen - 1);
+		CERROR("inode %lu: symlink not NULL terminated string of length %d\n",
+		       inode->i_ino, symlen - 1);
 		rc = -EPROTO;
 		goto failed;
 	}
diff --git a/drivers/staging/lustre/lustre/llite/vvp_io.c b/drivers/staging/lustre/lustre/llite/vvp_io.c
index e540a6d286f8..930f6010203e 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_io.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_io.c
@@ -709,7 +709,7 @@ static int vvp_io_fault_start(const struct lu_env *env,
 	}
 
 
-	if (fio->ft_mkwrite ) {
+	if (fio->ft_mkwrite) {
 		pgoff_t last_index;
 		/*
 		 * Capture the size while holding the lli_trunc_sem from above
@@ -720,9 +720,8 @@ static int vvp_io_fault_start(const struct lu_env *env,
 		last_index = cl_index(obj, size - 1);
 		if (last_index < fio->ft_index) {
 			CDEBUG(D_PAGE,
-				"llite: mkwrite and truncate race happened: "
-				"%p: 0x%lx 0x%lx\n",
-				vmpage->mapping, fio->ft_index, last_index);
+			       "llite: mkwrite and truncate race happened: %p: 0x%lx 0x%lx\n",
+			       vmpage->mapping, fio->ft_index, last_index);
 			/*
 			 * We need to return if we are
 			 * passed the end of the file. This will propagate
diff --git a/drivers/staging/lustre/lustre/llite/vvp_page.c b/drivers/staging/lustre/lustre/llite/vvp_page.c
index 4626346f6ee1..954ed08c6af2 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_page.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_page.c
@@ -376,8 +376,7 @@ static int vvp_page_print(const struct lu_env *env,
 	struct ccc_page *vp = cl2ccc_page(slice);
 	struct page      *vmpage = vp->cpg_page;
 
-	(*printer)(env, cookie, LUSTRE_VVP_NAME"-page@%p(%d:%d:%d) "
-		   "vm@%p ",
+	(*printer)(env, cookie, LUSTRE_VVP_NAME "-page@%p(%d:%d:%d) vm@%p ",
 		   vp, vp->cpg_defer_uptodate, vp->cpg_ra_used,
 		   vp->cpg_write_queued, vmpage);
 	if (vmpage != NULL) {
diff --git a/drivers/staging/lustre/lustre/llite/xattr.c b/drivers/staging/lustre/lustre/llite/xattr.c
index 3151baf5585c..b439936b4524 100644
--- a/drivers/staging/lustre/lustre/llite/xattr.c
+++ b/drivers/staging/lustre/lustre/llite/xattr.c
@@ -201,8 +201,7 @@ int ll_setxattr_common(struct inode *inode, const char *name,
 #endif
 	if (rc) {
 		if (rc == -EOPNOTSUPP && xattr_type == XATTR_USER_T) {
-			LCONSOLE_INFO("Disabling user_xattr feature because "
-				      "it is not supported on the server\n");
+			LCONSOLE_INFO("Disabling user_xattr feature because it is not supported on the server\n");
 			sbi->ll_flags &= ~LL_SBI_USER_XATTR;
 		}
 		return rc;
@@ -234,6 +233,9 @@ int ll_setxattr(struct dentry *dentry, const char *name,
 		struct lov_user_md *lump = (struct lov_user_md *)value;
 		int rc = 0;
 
+		if (size != 0 && size < sizeof(struct lov_user_md))
+			return -EINVAL;
+
 		/* Attributes that are saved via getxattr will always have
 		 * the stripe_offset as 0.  Instead, the MDS should be
 		 * allowed to pick the starting OST index.   b=17846 */
diff --git a/drivers/staging/lustre/lustre/llite/xattr_cache.c b/drivers/staging/lustre/lustre/llite/xattr_cache.c
index 627cbe242f22..e2badf17d95e 100644
--- a/drivers/staging/lustre/lustre/llite/xattr_cache.c
+++ b/drivers/staging/lustre/lustre/llite/xattr_cache.c
@@ -126,9 +126,7 @@ static int ll_xattr_cache_add(struct list_head *cache,
 		return -ENOMEM;
 	}
 
-	xattr->xe_namelen = strlen(xattr_name) + 1;
-
-	xattr->xe_name = kzalloc(xattr->xe_namelen, GFP_NOFS);
+	xattr->xe_name = kstrdup(xattr_name, GFP_NOFS);
 	if (!xattr->xe_name) {
 		CDEBUG(D_CACHE, "failed to alloc xattr name %u\n",
 		       xattr->xe_namelen);
@@ -141,7 +139,6 @@ static int ll_xattr_cache_add(struct list_head *cache,
 		goto err_value;
 	}
 
-	memcpy(xattr->xe_name, xattr_name, xattr->xe_namelen);
 	memcpy(xattr->xe_value, xattr_val, xattr_val_len);
 	xattr->xe_vallen = xattr_val_len;
 	list_add(&xattr->xe_list, cache);
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_fld.c b/drivers/staging/lustre/lustre/lmv/lmv_fld.c
index e8421f04beda..ee235926f52b 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_fld.c
+++ b/drivers/staging/lustre/lustre/lmv/lmv_fld.c
@@ -75,8 +75,7 @@ int lmv_fld_lookup(struct lmv_obd *lmv,
 	       *mds, PFID(fid));
 
 	if (*mds >= lmv->desc.ld_tgt_count) {
-		CERROR("FLD lookup got invalid mds #%x (max: %x) "
-		       "for fid="DFID"\n", *mds, lmv->desc.ld_tgt_count,
+		CERROR("FLD lookup got invalid mds #%x (max: %x) for fid=" DFID "\n", *mds, lmv->desc.ld_tgt_count,
 		       PFID(fid));
 		rc = -EINVAL;
 	}
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_intent.c b/drivers/staging/lustre/lustre/lmv/lmv_intent.c
index 5106124b7d92..d22d57b4ff38 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_intent.c
+++ b/drivers/staging/lustre/lustre/lmv/lmv_intent.c
@@ -186,8 +186,8 @@ int lmv_intent_open(struct obd_export *exp, struct md_op_data *op_data,
 			return rc;
 	}
 
-	CDEBUG(D_INODE, "OPEN_INTENT with fid1="DFID", fid2="DFID","
-	       " name='%s' -> mds #%d\n", PFID(&op_data->op_fid1),
+	CDEBUG(D_INODE, "OPEN_INTENT with fid1=" DFID ", fid2=" DFID ", name='%s' -> mds #%d\n",
+	       PFID(&op_data->op_fid1),
 	       PFID(&op_data->op_fid2), op_data->op_name, tgt->ltd_idx);
 
 	rc = md_intent_lock(tgt->ltd_exp, op_data, lmm, lmmsize, it, flags,
@@ -226,8 +226,8 @@ int lmv_intent_open(struct obd_export *exp, struct md_op_data *op_data,
 		 * this is normal situation, we should not print error here,
 		 * only debug info.
 		 */
-		CDEBUG(D_INODE, "Can't handle remote %s: dir "DFID"("DFID"):"
-		       "%*s: %d\n", LL_IT2STR(it), PFID(&op_data->op_fid2),
+		CDEBUG(D_INODE, "Can't handle remote %s: dir " DFID "(" DFID "):%*s: %d\n",
+		       LL_IT2STR(it), PFID(&op_data->op_fid2),
 		       PFID(&op_data->op_fid1), op_data->op_namelen,
 		       op_data->op_name, rc);
 		return rc;
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_internal.h b/drivers/staging/lustre/lustre/lmv/lmv_internal.h
index b911e7643874..852d78721ca9 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_internal.h
+++ b/drivers/staging/lustre/lustre/lmv/lmv_internal.h
@@ -42,8 +42,8 @@
 
 #define LMV_MAX_TGT_COUNT 128
 
-#define lmv_init_lock(lmv)   mutex_lock(&lmv->init_mutex);
-#define lmv_init_unlock(lmv) mutex_unlock(&lmv->init_mutex);
+#define lmv_init_lock(lmv)   mutex_lock(&lmv->init_mutex)
+#define lmv_init_unlock(lmv) mutex_unlock(&lmv->init_mutex)
 
 #define LL_IT2STR(it)					\
 	((it) ? ldlm_it2str((it)->it_op) : "0")
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_obd.c b/drivers/staging/lustre/lustre/lmv/lmv_obd.c
index 1a5821289c39..9f3837412cdf 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_obd.c
+++ b/drivers/staging/lustre/lustre/lmv/lmv_obd.c
@@ -325,8 +325,8 @@ static int lmv_init_ea_size(struct obd_export *exp, int easize,
 		rc = md_init_ea_size(lmv->tgts[i]->ltd_exp, easize, def_easize,
 				     cookiesize, def_cookiesize);
 		if (rc) {
-			CERROR("%s: obd_init_ea_size() failed on MDT target %d:"
-			       " rc = %d.\n", obd->obd_name, i, rc);
+			CERROR("%s: obd_init_ea_size() failed on MDT target %d: rc = %d.\n",
+			       obd->obd_name, i, rc);
 			break;
 		}
 	}
@@ -427,8 +427,7 @@ int lmv_connect_mdc(struct obd_device *obd, struct lmv_tgt_desc *tgt)
 						  mdc_obd->obd_type->typ_name,
 						  mdc_obd->obd_name);
 		if (mdc_symlink == NULL) {
-			CERROR("Could not register LMV target "
-			       "/proc/fs/lustre/%s/%s/target_obds/%s.",
+			CERROR("Could not register LMV target /proc/fs/lustre/%s/%s/target_obds/%s.",
 			       obd->obd_type->typ_name, obd->obd_name,
 			       mdc_obd->obd_name);
 			lprocfs_remove(&lmv_proc_dir);
@@ -474,8 +473,8 @@ static int lmv_add_target(struct obd_device *obd, struct obd_uuid *uuidp,
 
 	if ((index < lmv->tgts_size) && (lmv->tgts[index] != NULL)) {
 		tgt = lmv->tgts[index];
-		CERROR("%s: UUID %s already assigned at LOV target index %d:"
-		       " rc = %d\n", obd->obd_name,
+		CERROR("%s: UUID %s already assigned at LOV target index %d: rc = %d\n",
+		       obd->obd_name,
 		       obd_uuid2str(&tgt->ltd_uuid), index, -EEXIST);
 		lmv_init_unlock(lmv);
 		return -EEXIST;
@@ -600,8 +599,7 @@ int lmv_check_connect(struct obd_device *obd)
 			--lmv->desc.ld_active_tgt_count;
 			rc2 = obd_disconnect(tgt->ltd_exp);
 			if (rc2) {
-				CERROR("LMV target %s disconnect on "
-				       "MDC idx %d: error %d\n",
+				CERROR("LMV target %s disconnect on MDC idx %d: error %d\n",
 				       tgt->ltd_uuid.uuid, i, rc2);
 			}
 		}
@@ -865,10 +863,9 @@ static int lmv_hsm_ct_register(struct lmv_obd *lmv, unsigned int cmd, int len,
 		if (err) {
 			if (lmv->tgts[i]->ltd_active) {
 				/* permanent error */
-				CERROR("error: iocontrol MDC %s on MDT"
-				       "idx %d cmd %x: err = %d\n",
-					lmv->tgts[i]->ltd_uuid.uuid,
-					i, cmd, err);
+				CERROR("error: iocontrol MDC %s on MDTidx %d cmd %x: err = %d\n",
+				       lmv->tgts[i]->ltd_uuid.uuid,
+				       i, cmd, err);
 				rc = err;
 				lk->lk_flags |= LK_FLG_STOP;
 				/* unregister from previous MDS */
@@ -925,7 +922,7 @@ static int lmv_iocontrol(unsigned int cmd, struct obd_export *exp,
 		__u32 index;
 
 		memcpy(&index, data->ioc_inlbuf2, sizeof(__u32));
-		if ((index >= count))
+		if (index >= count)
 			return -ENODEV;
 
 		if (lmv->tgts[index] == NULL ||
@@ -1147,10 +1144,9 @@ static int lmv_iocontrol(unsigned int cmd, struct obd_export *exp,
 				return err;
 			} else if (err) {
 				if (lmv->tgts[i]->ltd_active) {
-					CERROR("error: iocontrol MDC %s on MDT"
-					       "idx %d cmd %x: err = %d\n",
-						lmv->tgts[i]->ltd_uuid.uuid,
-						i, cmd, err);
+					CERROR("error: iocontrol MDC %s on MDTidx %d cmd %x: err = %d\n",
+					       lmv->tgts[i]->ltd_uuid.uuid,
+					       i, cmd, err);
 					if (!rc)
 						rc = err;
 				}
@@ -1234,8 +1230,8 @@ static int lmv_placement_policy(struct obd_device *obd,
 		if (lum->lum_type == LMV_STRIPE_TYPE &&
 		    lum->lum_stripe_offset != -1) {
 			if (lum->lum_stripe_offset >= lmv->desc.ld_tgt_count) {
-				CERROR("%s: Stripe_offset %d > MDT count %d:"
-				       " rc = %d\n", obd->obd_name,
+				CERROR("%s: Stripe_offset %d > MDT count %d: rc = %d\n",
+				       obd->obd_name,
 				       lum->lum_stripe_offset,
 				       lmv->desc.ld_tgt_count, -ERANGE);
 				return -ERANGE;
@@ -1298,8 +1294,8 @@ int lmv_fid_alloc(struct obd_export *exp, struct lu_fid *fid,
 
 	rc = lmv_placement_policy(obd, op_data, &mds);
 	if (rc) {
-		CERROR("Can't get target for allocating fid, "
-		       "rc %d\n", rc);
+		CERROR("Can't get target for allocating fid, rc %d\n",
+		       rc);
 		return rc;
 	}
 
@@ -2310,7 +2306,6 @@ retry:
 static int lmv_precleanup(struct obd_device *obd, enum obd_cleanup_stage stage)
 {
 	struct lmv_obd *lmv = &obd->u.lmv;
-	int rc = 0;
 
 	switch (stage) {
 	case OBD_CLEANUP_EARLY:
@@ -2324,7 +2319,7 @@ static int lmv_precleanup(struct obd_device *obd, enum obd_cleanup_stage stage)
 	default:
 		break;
 	}
-	return rc;
+	return 0;
 }
 
 static int lmv_get_info(const struct lu_env *env, struct obd_export *exp,
diff --git a/drivers/staging/lustre/lustre/lov/lov_ea.c b/drivers/staging/lustre/lustre/lov/lov_ea.c
index 9e21e5efcdb6..e9ec39c5a6c2 100644
--- a/drivers/staging/lustre/lustre/lov/lov_ea.c
+++ b/drivers/staging/lustre/lustre/lov/lov_ea.c
@@ -348,9 +348,8 @@ const struct lsm_operations lsm_v3_ops = {
 
 void dump_lsm(unsigned int level, const struct lov_stripe_md *lsm)
 {
-	CDEBUG(level, "lsm %p, objid "DOSTID", maxbytes %#llx, magic 0x%08X,"
-	       " stripe_size %u, stripe_count %u, refc: %d,"
-	       " layout_gen %u, pool ["LOV_POOLNAMEF"]\n", lsm,
+	CDEBUG(level, "lsm %p, objid " DOSTID ", maxbytes %#llx, magic 0x%08X, stripe_size %u, stripe_count %u, refc: %d, layout_gen %u, pool [" LOV_POOLNAMEF "]\n",
+	       lsm,
 	       POSTID(&lsm->lsm_oi), lsm->lsm_maxbytes, lsm->lsm_magic,
 	       lsm->lsm_stripe_size, lsm->lsm_stripe_count,
 	       atomic_read(&lsm->lsm_refc), lsm->lsm_layout_gen,
diff --git a/drivers/staging/lustre/lustre/lov/lov_obd.c b/drivers/staging/lustre/lustre/lov/lov_obd.c
index 94dfd64bd283..ea503d2a19f8 100644
--- a/drivers/staging/lustre/lustre/lov/lov_obd.c
+++ b/drivers/staging/lustre/lustre/lov/lov_obd.c
@@ -168,8 +168,8 @@ int lov_connect_obd(struct obd_device *obd, __u32 index, int activate,
 
 
 	if (imp->imp_invalid) {
-		CDEBUG(D_CONFIG, "not connecting OSC %s; administratively "
-		       "disabled\n", obd_uuid2str(tgt_uuid));
+		CDEBUG(D_CONFIG, "not connecting OSC %s; administratively disabled\n",
+		       obd_uuid2str(tgt_uuid));
 		return 0;
 	}
 
@@ -201,10 +201,9 @@ int lov_connect_obd(struct obd_device *obd, __u32 index, int activate,
 						  osc_obd->obd_type->typ_name,
 						  osc_obd->obd_name);
 		if (osc_symlink == NULL) {
-			CERROR("could not register LOV target "
-				"/proc/fs/lustre/%s/%s/target_obds/%s.",
-				obd->obd_type->typ_name, obd->obd_name,
-				osc_obd->obd_name);
+			CERROR("could not register LOV target /proc/fs/lustre/%s/%s/target_obds/%s.",
+			       obd->obd_type->typ_name, obd->obd_name,
+			       osc_obd->obd_name);
 			lprocfs_remove(&lov_proc_dir);
 			obd->obd_proc_private = NULL;
 		}
@@ -726,8 +725,7 @@ void lov_fix_desc_stripe_size(__u64 *val)
 {
 	if (*val < LOV_MIN_STRIPE_SIZE) {
 		if (*val != 0)
-			LCONSOLE_INFO("Increasing default stripe size to "
-				      "minimum %u\n",
+			LCONSOLE_INFO("Increasing default stripe size to minimum %u\n",
 				      LOV_DESC_STRIPE_SIZE_DEFAULT);
 		*val = LOV_DESC_STRIPE_SIZE_DEFAULT;
 	} else if (*val & (LOV_MIN_STRIPE_SIZE - 1)) {
@@ -847,7 +845,6 @@ out:
 
 static int lov_precleanup(struct obd_device *obd, enum obd_cleanup_stage stage)
 {
-	int rc = 0;
 	struct lov_obd *lov = &obd->u.lov;
 
 	switch (stage) {
@@ -865,7 +862,7 @@ static int lov_precleanup(struct obd_device *obd, enum obd_cleanup_stage stage)
 		break;
 	}
 
-	return rc;
+	return 0;
 }
 
 static int lov_cleanup(struct obd_device *obd)
@@ -900,8 +897,7 @@ static int lov_cleanup(struct obd_device *obd)
 			    /* We should never get here - these
 			       should have been removed in the
 			     disconnect. */
-				CERROR("lov tgt %d not cleaned!"
-				       " deathrow=%d, lovrc=%d\n",
+				CERROR("lov tgt %d not cleaned! deathrow=%d, lovrc=%d\n",
 				       i, lov->lov_death_row,
 				       atomic_read(&lov->lov_refcount));
 			lov_del_target(obd, i, NULL, 0);
@@ -1176,8 +1172,8 @@ static int lov_getattr_async(struct obd_export *exp, struct obd_info *oinfo,
 	list_for_each(pos, &lovset->set_list) {
 		req = list_entry(pos, struct lov_request, rq_link);
 
-		CDEBUG(D_INFO, "objid "DOSTID"[%d] has subobj "DOSTID" at idx"
-		       "%u\n", POSTID(&oinfo->oi_oa->o_oi), req->rq_stripe,
+		CDEBUG(D_INFO, "objid " DOSTID "[%d] has subobj " DOSTID " at idx%u\n",
+		       POSTID(&oinfo->oi_oa->o_oi), req->rq_stripe,
 		       POSTID(&req->rq_oi.oi_oa->o_oi), req->rq_idx);
 		rc = obd_getattr_async(lov->lov_tgts[req->rq_idx]->ltd_exp,
 				       &req->rq_oi, rqset);
@@ -1256,8 +1252,8 @@ static int lov_setattr_async(struct obd_export *exp, struct obd_info *oinfo,
 		if (oinfo->oi_oa->o_valid & OBD_MD_FLCOOKIE)
 			oti->oti_logcookies = set->set_cookies + req->rq_stripe;
 
-		CDEBUG(D_INFO, "objid "DOSTID"[%d] has subobj "DOSTID" at idx"
-		       "%u\n", POSTID(&oinfo->oi_oa->o_oi), req->rq_stripe,
+		CDEBUG(D_INFO, "objid " DOSTID "[%d] has subobj " DOSTID " at idx%u\n",
+		       POSTID(&oinfo->oi_oa->o_oi), req->rq_stripe,
 		       POSTID(&req->rq_oi.oi_oa->o_oi), req->rq_idx);
 
 		rc = obd_setattr_async(lov->lov_tgts[req->rq_idx]->ltd_exp,
@@ -1568,8 +1564,7 @@ static int lov_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 				if (lov->lov_tgts[i]->ltd_active) {
 					CDEBUG(err == -ENOTTY ?
 					       D_IOCTL : D_WARNING,
-					       "iocontrol OSC %s on OST "
-					       "idx %d cmd %x: err = %d\n",
+					       "iocontrol OSC %s on OST idx %d cmd %x: err = %d\n",
 					       lov_uuid2str(lov, i),
 					       i, cmd, err);
 					if (!rc)
@@ -2266,8 +2261,8 @@ static int lov_quotacheck(struct obd_device *obd, struct obd_export *exp,
 
 		/* Skip quota check on the administratively disabled OSTs. */
 		if (!lov->lov_tgts[i]->ltd_activate) {
-			CWARN("lov idx %d was administratively disabled, "
-			      "skip quotacheck on it.\n", i);
+			CWARN("lov idx %d was administratively disabled, skip quotacheck on it.\n",
+			      i);
 			continue;
 		}
 
diff --git a/drivers/staging/lustre/lustre/lov/lov_pack.c b/drivers/staging/lustre/lustre/lov/lov_pack.c
index 5edd6a3a9c54..5356d5324176 100644
--- a/drivers/staging/lustre/lustre/lov/lov_pack.c
+++ b/drivers/staging/lustre/lustre/lov/lov_pack.c
@@ -438,8 +438,7 @@ int lov_getstripe(struct obd_export *exp, struct lov_stripe_md *lsm,
 	if (copy_from_user(&lum, lump, lum_size)) {
 		rc = -EFAULT;
 		goto out_set;
-	}
-	else if ((lum.lmm_magic != LOV_USER_MAGIC) &&
+	} else if ((lum.lmm_magic != LOV_USER_MAGIC) &&
 		 (lum.lmm_magic != LOV_USER_MAGIC_V3)) {
 		rc = -EINVAL;
 		goto out_set;
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_lib.c b/drivers/staging/lustre/lustre/mdc/mdc_lib.c
index e8732cc30ce2..4e59995e0042 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_lib.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_lib.c
@@ -194,10 +194,8 @@ static __u64 mds_pack_open_flags(__u64 flags, __u32 mode)
 		cr_flags |= MDS_OPEN_SYNC;
 	if (flags & O_DIRECTORY)
 		cr_flags |= MDS_OPEN_DIRECTORY;
-#ifdef FMODE_EXEC
-	if (flags & FMODE_EXEC)
+	if (flags & __FMODE_EXEC)
 		cr_flags |= MDS_FMODE_EXEC;
-#endif
 	if (cl_is_lov_delay_create(flags))
 		cr_flags |= MDS_OPEN_DELAY_CREATE;
 
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_locks.c b/drivers/staging/lustre/lustre/mdc/mdc_locks.c
index b58147ee62b6..8c9b4f5494e9 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_locks.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_locks.c
@@ -296,10 +296,8 @@ static struct ptlrpc_request *mdc_intent_open_pack(struct obd_export *exp,
 		} else {
 			if (it->it_flags & (FMODE_WRITE|MDS_OPEN_TRUNC))
 				mode = LCK_CW;
-#ifdef FMODE_EXEC
-			else if (it->it_flags & FMODE_EXEC)
+			else if (it->it_flags & __FMODE_EXEC)
 				mode = LCK_PR;
-#endif
 			else
 				mode = LCK_CR;
 		}
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_request.c b/drivers/staging/lustre/lustre/mdc/mdc_request.c
index 14e1ba1675f6..3b0f245a8780 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_request.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_request.c
@@ -60,7 +60,7 @@ struct mdc_renew_capa_args {
 
 static int mdc_cleanup(struct obd_device *obd);
 
-int mdc_unpack_capa(struct obd_export *exp, struct ptlrpc_request *req,
+static int mdc_unpack_capa(struct obd_export *exp, struct ptlrpc_request *req,
 		    const struct req_msg_field *field, struct obd_capa **oc)
 {
 	struct lustre_capa *capa;
@@ -147,7 +147,7 @@ out:
 }
 
 /* This should be mdc_get_info("rootfid") */
-int mdc_getstatus(struct obd_export *exp, struct lu_fid *rootfid,
+static int mdc_getstatus(struct obd_export *exp, struct lu_fid *rootfid,
 		  struct obd_capa **pc)
 {
 	return send_getstatus(class_exp2cliimp(exp), rootfid, pc,
@@ -214,7 +214,7 @@ static int mdc_getattr_common(struct obd_export *exp,
 	return 0;
 }
 
-int mdc_getattr(struct obd_export *exp, struct md_op_data *op_data,
+static int mdc_getattr(struct obd_export *exp, struct md_op_data *op_data,
 		struct ptlrpc_request **request)
 {
 	struct ptlrpc_request *req;
@@ -258,7 +258,7 @@ int mdc_getattr(struct obd_export *exp, struct md_op_data *op_data,
 	return rc;
 }
 
-int mdc_getattr_name(struct obd_export *exp, struct md_op_data *op_data,
+static int mdc_getattr_name(struct obd_export *exp, struct md_op_data *op_data,
 		     struct ptlrpc_request **request)
 {
 	struct ptlrpc_request *req;
@@ -441,7 +441,7 @@ static int mdc_xattr_common(struct obd_export *exp,
 	return rc;
 }
 
-int mdc_setxattr(struct obd_export *exp, const struct lu_fid *fid,
+static int mdc_setxattr(struct obd_export *exp, const struct lu_fid *fid,
 		 struct obd_capa *oc, u64 valid, const char *xattr_name,
 		 const char *input, int input_size, int output_size,
 		 int flags, __u32 suppgid, struct ptlrpc_request **request)
@@ -452,7 +452,7 @@ int mdc_setxattr(struct obd_export *exp, const struct lu_fid *fid,
 				suppgid, request);
 }
 
-int mdc_getxattr(struct obd_export *exp, const struct lu_fid *fid,
+static int mdc_getxattr(struct obd_export *exp, const struct lu_fid *fid,
 		 struct obd_capa *oc, u64 valid, const char *xattr_name,
 		 const char *input, int input_size, int output_size,
 		 int flags, struct ptlrpc_request **request)
diff --git a/drivers/staging/lustre/lustre/mgc/mgc_request.c b/drivers/staging/lustre/lustre/mgc/mgc_request.c
index bc263adf09d4..60d2b0f12693 100644
--- a/drivers/staging/lustre/lustre/mgc/mgc_request.c
+++ b/drivers/staging/lustre/lustre/mgc/mgc_request.c
@@ -510,8 +510,6 @@ static void do_requeue(struct config_llog_data *cld)
 
 static int mgc_requeue_thread(void *data)
 {
-	int rc = 0;
-
 	CDEBUG(D_MGC, "Starting requeue thread\n");
 
 	/* Keep trying failed locks periodically */
@@ -592,7 +590,7 @@ static int mgc_requeue_thread(void *data)
 	complete(&rq_exit);
 
 	CDEBUG(D_MGC, "Ending requeue thread\n");
-	return rc;
+	return 0;
 }
 
 /* Add a cld to the list to requeue.  Start the requeue thread if needed.
@@ -736,8 +734,7 @@ static int mgc_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 		rc = PTR_ERR(kthread_run(mgc_requeue_thread, NULL,
 					     "ll_cfg_requeue"));
 		if (IS_ERR_VALUE(rc)) {
-			CERROR("%s: Cannot start requeue thread (%d),"
-			       "no more log updates!\n",
+			CERROR("%s: Cannot start requeue thread (%d),no more log updates!\n",
 			       obd->obd_name, rc);
 			goto err_cleanup;
 		}
@@ -1021,8 +1018,7 @@ int mgc_set_info_async(const struct lu_env *env, struct obd_export *exp,
 
 			sptlrpc_flavor2name(&cli->cl_flvr_mgc,
 					    str, sizeof(str));
-			LCONSOLE_ERROR("asking sptlrpc flavor %s to MGS but "
-				       "currently %s is in use\n",
+			LCONSOLE_ERROR("asking sptlrpc flavor %s to MGS but currently %s is in use\n",
 				       (char *) val, str);
 			rc = -EPERM;
 		}
@@ -1055,8 +1051,6 @@ static int mgc_import_event(struct obd_device *obd,
 			    struct obd_import *imp,
 			    enum obd_import_event event)
 {
-	int rc = 0;
-
 	LASSERT(imp->imp_obd == obd);
 	CDEBUG(D_MGC, "import event %#x\n", event);
 
@@ -1090,7 +1084,7 @@ static int mgc_import_event(struct obd_device *obd,
 		CERROR("Unknown import event %#x\n", event);
 		LBUG();
 	}
-	return rc;
+	return 0;
 }
 
 enum {
diff --git a/drivers/staging/lustre/lustre/obdclass/acl.c b/drivers/staging/lustre/lustre/obdclass/acl.c
index 2619bfeceb8b..9a69f6b35a0e 100644
--- a/drivers/staging/lustre/lustre/obdclass/acl.c
+++ b/drivers/staging/lustre/lustre/obdclass/acl.c
@@ -171,17 +171,17 @@ EXPORT_SYMBOL(lustre_posix_acl_xattr_2ext);
 /*
  * Filter out the "nobody" entries in the posix ACL.
  */
-int lustre_posix_acl_xattr_filter(posix_acl_xattr_header *header, int size,
+int lustre_posix_acl_xattr_filter(posix_acl_xattr_header *header, size_t size,
 				  posix_acl_xattr_header **out)
 {
 	int count, i, j, rc = 0;
 	__u32 id;
 	posix_acl_xattr_header *new;
 
-	if (unlikely(size < 0))
-		return -EINVAL;
-	else if (!size)
+	if (!size)
 		return 0;
+	if (size < sizeof(*new))
+		return -EINVAL;
 
 	OBD_ALLOC(new, size);
 	if (unlikely(new == NULL))
diff --git a/drivers/staging/lustre/lustre/obdclass/capa.c b/drivers/staging/lustre/lustre/obdclass/capa.c
index cd1abce378ea..d206b1046a18 100644
--- a/drivers/staging/lustre/lustre/obdclass/capa.c
+++ b/drivers/staging/lustre/lustre/obdclass/capa.c
@@ -272,8 +272,7 @@ int capa_hmac(__u8 *hmac, struct lustre_capa *capa, __u8 *key)
 
 	tfm = crypto_alloc_hash(alg->ha_name, 0, 0);
 	if (IS_ERR(tfm)) {
-		CERROR("crypto_alloc_tfm failed, check whether your kernel"
-		       "has crypto support!\n");
+		CERROR("crypto_alloc_tfm failed, check whether your kernel has crypto support!\n");
 		return PTR_ERR(tfm);
 	}
 	keylen = alg->ha_keylen;
@@ -302,7 +301,7 @@ int capa_encrypt_id(__u32 *d, __u32 *s, __u8 *key, int keylen)
 
 	/* passing "aes" in a variable instead of a constant string keeps gcc
 	 * 4.3.2 happy */
-	tfm = crypto_alloc_blkcipher(alg, 0, 0 );
+	tfm = crypto_alloc_blkcipher(alg, 0, 0);
 	if (IS_ERR(tfm)) {
 		CERROR("failed to load transform for aes\n");
 		return PTR_ERR(tfm);
@@ -355,7 +354,7 @@ int capa_decrypt_id(__u32 *d, __u32 *s, __u8 *key, int keylen)
 
 	/* passing "aes" in a variable instead of a constant string keeps gcc
 	 * 4.3.2 happy */
-	tfm = crypto_alloc_blkcipher(alg, 0, 0 );
+	tfm = crypto_alloc_blkcipher(alg, 0, 0);
 	if (IS_ERR(tfm)) {
 		CERROR("failed to load transform for aes\n");
 		return PTR_ERR(tfm);
@@ -407,14 +406,13 @@ EXPORT_SYMBOL(capa_cpy);
 
 void _debug_capa(struct lustre_capa *c,
 		 struct libcfs_debug_msg_data *msgdata,
-		 const char *fmt, ... )
+		 const char *fmt, ...)
 {
 	va_list args;
 	va_start(args, fmt);
 	libcfs_debug_vmsg2(msgdata, fmt, args,
-			   " capability@%p fid "DFID" opc %#llx uid %llu"
-			   " gid %llu flags %u alg %d keyid %u timeout %u "
-			   "expiry %u\n", c, PFID(capa_fid(c)), capa_opc(c),
+			   " capability@%p fid " DFID " opc %#llx uid %llu gid %llu flags %u alg %d keyid %u timeout %u expiry %u\n",
+			   c, PFID(capa_fid(c)), capa_opc(c),
 			   capa_uid(c), capa_gid(c), capa_flags(c),
 			   capa_alg(c), capa_keyid(c), capa_timeout(c),
 			   capa_expiry(c));
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_io.c b/drivers/staging/lustre/lustre/obdclass/cl_io.c
index f2383a497cbe..3141b6043708 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_io.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_io.c
@@ -1622,8 +1622,7 @@ int cl_sync_io_wait(const struct lu_env *env, struct cl_io *io,
 			  atomic_read(&anchor->csi_sync_nr) == 0,
 			  &lwi);
 	if (rc < 0) {
-		CERROR("SYNC IO failed with error: %d, try to cancel "
-		       "%d remaining pages\n",
+		CERROR("SYNC IO failed with error: %d, try to cancel %d remaining pages\n",
 		       rc, atomic_read(&anchor->csi_sync_nr));
 
 		(void)cl_io_cancel(env, io, queue);
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_lock.c b/drivers/staging/lustre/lustre/obdclass/cl_lock.c
index b204531ef710..b081167f9767 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_lock.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_lock.c
@@ -129,8 +129,7 @@ static void cl_lock_trace0(int level, const struct lu_env *env,
 			   const char *func, const int line)
 {
 	struct cl_object_header *h = cl_object_header(lock->cll_descr.cld_obj);
-	CDEBUG(level, "%s: %p@(%d %p %d %d %d %d %d %lx)"
-		      "(%p/%d/%d) at %s():%d\n",
+	CDEBUG(level, "%s: %p@(%d %p %d %d %d %d %d %lx)(%p/%d/%d) at %s():%d\n",
 	       prefix, lock, atomic_read(&lock->cll_ref),
 	       lock->cll_guarder, lock->cll_depth,
 	       lock->cll_state, lock->cll_error, lock->cll_holds,
diff --git a/drivers/staging/lustre/lustre/obdclass/class_obd.c b/drivers/staging/lustre/lustre/obdclass/class_obd.c
index 7265ecbc6f9d..89a3fb2e56b2 100644
--- a/drivers/staging/lustre/lustre/obdclass/class_obd.c
+++ b/drivers/staging/lustre/lustre/obdclass/class_obd.c
@@ -144,13 +144,11 @@ int obd_alloc_fail(const void *ptr, const char *name, const char *type,
 		CERROR("%s%salloc of %s (%llu bytes) failed at %s:%d\n",
 		       ptr ? "force " :"", type, name, (__u64)size, file,
 		       line);
-		CERROR("%llu total bytes and %llu total pages "
-		       "(%llu bytes) allocated by Lustre, "
-		       "%d total bytes by LNET\n",
+		CERROR("%llu total bytes and %llu total pages (%llu bytes) allocated by Lustre, %d total bytes by LNET\n",
 		       obd_memory_sum(),
 		       obd_pages_sum() << PAGE_CACHE_SHIFT,
 		       obd_pages_sum(),
-			atomic_read(&libcfs_kmemory));
+		       atomic_read(&libcfs_kmemory));
 		return 1;
 	}
 	return 0;
diff --git a/drivers/staging/lustre/lustre/obdclass/debug.c b/drivers/staging/lustre/lustre/obdclass/debug.c
index d0f8f875ddd6..9c934e6d2ea1 100644
--- a/drivers/staging/lustre/lustre/obdclass/debug.c
+++ b/drivers/staging/lustre/lustre/obdclass/debug.c
@@ -40,6 +40,7 @@
 
 #define DEBUG_SUBSYSTEM D_OTHER
 
+#include <linux/unaligned/access_ok.h>
 
 #include "../include/obd_support.h"
 #include "../include/lustre_debug.h"
@@ -60,14 +61,11 @@ int block_debug_setup(void *addr, int len, __u64 off, __u64 id)
 {
 	LASSERT(addr);
 
-	off = cpu_to_le64 (off);
-	id = cpu_to_le64 (id);
-	memcpy(addr, (char *)&off, LPDS);
-	memcpy(addr + LPDS, (char *)&id, LPDS);
-
+	put_unaligned_le64(off, addr);
+	put_unaligned_le64(id, addr+LPDS);
 	addr += len - LPDS - LPDS;
-	memcpy(addr, (char *)&off, LPDS);
-	memcpy(addr + LPDS, (char *)&id, LPDS);
+	put_unaligned_le64(off, addr);
+	put_unaligned_le64(id, addr+LPDS);
 
 	return 0;
 }
diff --git a/drivers/staging/lustre/lustre/obdclass/dt_object.c b/drivers/staging/lustre/lustre/obdclass/dt_object.c
index 52256c26bf07..e7be26ec7521 100644
--- a/drivers/staging/lustre/lustre/obdclass/dt_object.c
+++ b/drivers/staging/lustre/lustre/obdclass/dt_object.c
@@ -332,8 +332,7 @@ static struct dt_object *dt_reg_open(const struct lu_env *env,
 	result = dt_lookup_dir(env, p, name, fid);
 	if (result == 0){
 		o = dt_locate(env, dt, fid);
-	}
-	else
+	} else
 		o = ERR_PTR(result);
 
 	return o;
@@ -950,8 +949,8 @@ int lprocfs_dt_rd_blksize(char *page, char **start, off_t off,
 {
 	struct dt_device *dt = data;
 	struct obd_statfs osfs;
-
 	int rc = dt_statfs(NULL, dt, &osfs);
+
 	if (rc == 0) {
 		*eof = 1;
 		rc = snprintf(page, count, "%u\n",
@@ -967,8 +966,8 @@ int lprocfs_dt_rd_kbytestotal(char *page, char **start, off_t off,
 {
 	struct dt_device *dt = data;
 	struct obd_statfs osfs;
-
 	int rc = dt_statfs(NULL, dt, &osfs);
+
 	if (rc == 0) {
 		__u32 blk_size = osfs.os_bsize >> 10;
 		__u64 result = osfs.os_blocks;
@@ -989,8 +988,8 @@ int lprocfs_dt_rd_kbytesfree(char *page, char **start, off_t off,
 {
 	struct dt_device *dt = data;
 	struct obd_statfs osfs;
-
 	int rc = dt_statfs(NULL, dt, &osfs);
+
 	if (rc == 0) {
 		__u32 blk_size = osfs.os_bsize >> 10;
 		__u64 result = osfs.os_bfree;
@@ -1011,8 +1010,8 @@ int lprocfs_dt_rd_kbytesavail(char *page, char **start, off_t off,
 {
 	struct dt_device *dt = data;
 	struct obd_statfs osfs;
-
 	int rc = dt_statfs(NULL, dt, &osfs);
+
 	if (rc == 0) {
 		__u32 blk_size = osfs.os_bsize >> 10;
 		__u64 result = osfs.os_bavail;
@@ -1033,8 +1032,8 @@ int lprocfs_dt_rd_filestotal(char *page, char **start, off_t off,
 {
 	struct dt_device *dt = data;
 	struct obd_statfs osfs;
-
 	int rc = dt_statfs(NULL, dt, &osfs);
+
 	if (rc == 0) {
 		*eof = 1;
 		rc = snprintf(page, count, "%llu\n", osfs.os_files);
@@ -1049,8 +1048,8 @@ int lprocfs_dt_rd_filesfree(char *page, char **start, off_t off,
 {
 	struct dt_device *dt = data;
 	struct obd_statfs osfs;
-
 	int rc = dt_statfs(NULL, dt, &osfs);
+
 	if (rc == 0) {
 		*eof = 1;
 		rc = snprintf(page, count, "%llu\n", osfs.os_ffree);
diff --git a/drivers/staging/lustre/lustre/obdclass/genops.c b/drivers/staging/lustre/lustre/obdclass/genops.c
index c314e9c2343e..736ca410aca3 100644
--- a/drivers/staging/lustre/lustre/obdclass/genops.c
+++ b/drivers/staging/lustre/lustre/obdclass/genops.c
@@ -317,7 +317,7 @@ struct obd_device *class_newdev(const char *type_name, const char *name)
 					 result->obd_minor, new_obd_minor);
 
 				obd_devs[result->obd_minor] = NULL;
-				result->obd_name[0]='\0';
+				result->obd_name[0] = '\0';
 			 }
 			result = ERR_PTR(-EEXIST);
 			break;
@@ -524,8 +524,8 @@ void class_obd_list(void)
 /* Search for a client OBD connected to tgt_uuid.  If grp_uuid is
    specified, then only the client with that uuid is returned,
    otherwise any client connected to the tgt is returned. */
-struct obd_device * class_find_client_obd(struct obd_uuid *tgt_uuid,
-					  const char * typ_name,
+struct obd_device *class_find_client_obd(struct obd_uuid *tgt_uuid,
+					  const char *typ_name,
 					  struct obd_uuid *grp_uuid)
 {
 	int i;
@@ -557,7 +557,7 @@ EXPORT_SYMBOL(class_find_client_obd);
    searching at *next, and if a device is found, the next index to look
    at is saved in *next. If next is NULL, then the first matching device
    will always be returned. */
-struct obd_device * class_devices_in_group(struct obd_uuid *grp_uuid, int *next)
+struct obd_device *class_devices_in_group(struct obd_uuid *grp_uuid, int *next)
 {
 	int i;
 
@@ -1087,8 +1087,7 @@ void __class_export_del_lock_ref(struct obd_export *exp, struct ldlm_lock *lock)
 	spin_lock(&exp->exp_locks_list_guard);
 	LASSERT(lock->l_exp_refs_nr > 0);
 	if (lock->l_exp_refs_target != exp) {
-		LCONSOLE_WARN("lock %p, "
-			      "mismatching export pointers: %p, %p\n",
+		LCONSOLE_WARN("lock %p, mismatching export pointers: %p, %p\n",
 			      lock, lock->l_exp_refs_target, exp);
 	}
 	if (-- lock->l_exp_refs_nr == 0) {
@@ -1259,8 +1258,7 @@ static void class_disconnect_export_list(struct list_head *list,
 		}
 
 		class_export_get(exp);
-		CDEBUG(D_HA, "%s: disconnecting export at %s (%p), "
-		       "last request at "CFS_TIME_T"\n",
+		CDEBUG(D_HA, "%s: disconnecting export at %s (%p), last request at " CFS_TIME_T "\n",
 		       exp->exp_obd->obd_name, obd_export_nid2str(exp),
 		       exp, exp->exp_last_request_time);
 		/* release one export reference anyway */
@@ -1284,8 +1282,8 @@ void class_disconnect_exports(struct obd_device *obd)
 	spin_unlock(&obd->obd_dev_lock);
 
 	if (!list_empty(&work_list)) {
-		CDEBUG(D_HA, "OBD device %d (%p) has exports, "
-		       "disconnecting them\n", obd->obd_minor, obd);
+		CDEBUG(D_HA, "OBD device %d (%p) has exports, disconnecting them\n",
+		       obd->obd_minor, obd);
 		class_disconnect_export_list(&work_list,
 					     exp_flags_from_obd(obd));
 	} else
@@ -1422,8 +1420,8 @@ int obd_export_evict_by_nid(struct obd_device *obd, const char *nid)
 		LASSERTF(doomed_exp != obd->obd_self_export,
 			 "self-export is hashed by NID?\n");
 		exports_evicted++;
-		LCONSOLE_WARN("%s: evicting %s (at %s) by administrative "
-			      "request\n", obd->obd_name,
+		LCONSOLE_WARN("%s: evicting %s (at %s) by administrative request\n",
+			      obd->obd_name,
 			      obd_uuid2str(&doomed_exp->exp_client_uuid),
 			      obd_export_nid2str(doomed_exp));
 		class_fail_export(doomed_exp);
@@ -1546,9 +1544,7 @@ void obd_exports_barrier(struct obd_device *obd)
 		set_current_state(TASK_UNINTERRUPTIBLE);
 		schedule_timeout(cfs_time_seconds(waited));
 		if (waited > 5 && IS_PO2(waited)) {
-			LCONSOLE_WARN("%s is waiting for obd_unlinked_exports "
-				      "more than %d seconds. "
-				      "The obd refcount = %d. Is it stuck?\n",
+			LCONSOLE_WARN("%s is waiting for obd_unlinked_exports more than %d seconds. The obd refcount = %d. Is it stuck?\n",
 				      obd->obd_name, waited,
 				      atomic_read(&obd->obd_refcount));
 			dump_exports(obd, 1);
@@ -1783,7 +1779,7 @@ EXPORT_SYMBOL(kuc_len);
  * @param p Pointer to payload area
  * @returns Pointer to kuc header
  */
-struct kuc_hdr * kuc_ptr(void *p)
+struct kuc_hdr *kuc_ptr(void *p)
 {
 	struct kuc_hdr *lh = ((struct kuc_hdr *)p) - 1;
 	LASSERT(lh->kuc_magic == KUC_MAGIC);
diff --git a/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c b/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c
index 7eaaaa648dfb..66ceab20c743 100644
--- a/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c
+++ b/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c
@@ -166,14 +166,14 @@ int obd_ioctl_popdata(void *arg, void *data, int len)
 EXPORT_SYMBOL(obd_ioctl_popdata);
 
 /*  opening /dev/obd */
-static int obd_class_open(struct inode * inode, struct file * file)
+static int obd_class_open(struct inode *inode, struct file *file)
 {
 	try_module_get(THIS_MODULE);
 	return 0;
 }
 
 /*  closing /dev/obd */
-static int obd_class_release(struct inode * inode, struct file * file)
+static int obd_class_release(struct inode *inode, struct file *file)
 {
 	module_put(THIS_MODULE);
 	return 0;
diff --git a/drivers/staging/lustre/lustre/obdclass/linux/linux-sysctl.c b/drivers/staging/lustre/lustre/obdclass/linux/linux-sysctl.c
index 38a9b319355e..dd46e7358160 100644
--- a/drivers/staging/lustre/lustre/obdclass/linux/linux-sysctl.c
+++ b/drivers/staging/lustre/lustre/obdclass/linux/linux-sysctl.c
@@ -202,9 +202,8 @@ static int proc_max_dirty_pages_in_mb(struct ctl_table *table, int write,
 		/* Don't allow them to let dirty pages exceed 90% of system
 		 * memory and set a hard minimum of 4MB. */
 		if (obd_max_dirty_pages > ((totalram_pages / 10) * 9)) {
-			CERROR("Refusing to set max dirty pages to %u, which "
-			       "is more than 90%% of available RAM; setting "
-			       "to %lu\n", obd_max_dirty_pages,
+			CERROR("Refusing to set max dirty pages to %u, which is more than 90%% of available RAM; setting to %lu\n",
+			       obd_max_dirty_pages,
 			       ((totalram_pages / 10) * 9));
 			obd_max_dirty_pages = ((totalram_pages / 10) * 9);
 		} else if (obd_max_dirty_pages < 4 << (20 - PAGE_CACHE_SHIFT)) {
diff --git a/drivers/staging/lustre/lustre/obdclass/llog.c b/drivers/staging/lustre/lustre/obdclass/llog.c
index 3ab05292152c..114be4a78ccf 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog.c
@@ -346,8 +346,8 @@ repeat:
 			}
 			if (rec->lrh_len == 0 ||
 			    rec->lrh_len > LLOG_CHUNK_SIZE) {
-				CWARN("invalid length %d in llog record for "
-				      "index %d/%d\n", rec->lrh_len,
+				CWARN("invalid length %d in llog record for index %d/%d\n",
+				      rec->lrh_len,
 				      rec->lrh_index, index);
 				rc = -EINVAL;
 				goto out;
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_cat.c b/drivers/staging/lustre/lustre/obdclass/llog_cat.c
index 6e139cf372c4..4b850fc5f5d9 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog_cat.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog_cat.c
@@ -228,8 +228,7 @@ int llog_cat_close(const struct lu_env *env, struct llog_handle *cathandle)
 		    (llh->llh_count == 1)) {
 			rc = llog_destroy(env, loghandle);
 			if (rc)
-				CERROR("%s: failure destroying log during "
-				       "cleanup: rc = %d\n",
+				CERROR("%s: failure destroying log during cleanup: rc = %d\n",
 				       loghandle->lgh_ctxt->loc_obd->obd_name,
 				       rc);
 
@@ -746,8 +745,7 @@ int llog_cat_cleanup(const struct lu_env *env, struct llog_handle *cathandle,
 	llog_cat_set_first_idx(cathandle, index);
 	rc = llog_cancel_rec(env, cathandle, index);
 	if (rc == 0)
-		CDEBUG(D_HA, "cancel plain log at index"
-		       " %u of catalog "DOSTID"\n",
+		CDEBUG(D_HA, "cancel plain log at index %u of catalog " DOSTID "\n",
 		       index, POSTID(&cathandle->lgh_id.lgl_oi));
 	return rc;
 }
@@ -810,8 +808,8 @@ int llog_cat_init_and_process(const struct lu_env *env,
 
 	rc = llog_process_or_fork(env, llh, cat_cancel_cb, NULL, NULL, false);
 	if (rc)
-		CERROR("%s: llog_process() with cat_cancel_cb failed: rc = "
-		       "%d\n", llh->lgh_ctxt->loc_obd->obd_name, rc);
+		CERROR("%s: llog_process() with cat_cancel_cb failed: rc = %d\n",
+		       llh->lgh_ctxt->loc_obd->obd_name, rc);
 	return 0;
 }
 EXPORT_SYMBOL(llog_cat_init_and_process);
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_obd.c b/drivers/staging/lustre/lustre/obdclass/llog_obd.c
index da769db0af77..978d886a1103 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog_obd.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog_obd.c
@@ -42,7 +42,7 @@
 #include "llog_internal.h"
 
 /* helper functions for calling the llog obd methods */
-static struct llog_ctxt* llog_new_ctxt(struct obd_device *obd)
+static struct llog_ctxt *llog_new_ctxt(struct obd_device *obd)
 {
 	struct llog_ctxt *ctxt;
 
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_swab.c b/drivers/staging/lustre/lustre/obdclass/llog_swab.c
index bfac8387021e..d3ec90e85eb9 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog_swab.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog_swab.c
@@ -400,8 +400,7 @@ void lustre_swab_cfg_marker(struct cfg_marker *marker, int swab, int size)
 		}
 		marker->cm_createtime = createtime;
 		marker->cm_canceltime = canceltime;
-		CDEBUG(D_CONFIG, "Find old cfg_marker(Srv32b,Clt64b) "
-		       "for target %s, converting\n",
+		CDEBUG(D_CONFIG, "Find old cfg_marker(Srv32b,Clt64b) for target %s, converting\n",
 		       marker->cm_tgtname);
 	} else if (swab) {
 		__swab64s(&marker->cm_createtime);
diff --git a/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c b/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
index 61e04af2464f..3b7dfc367722 100644
--- a/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
+++ b/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
@@ -177,7 +177,7 @@ int lprocfs_read_frac_helper(char *buffer, unsigned long count, long val,
 }
 EXPORT_SYMBOL(lprocfs_read_frac_helper);
 
-int lprocfs_write_frac_helper(const char *buffer, unsigned long count,
+int lprocfs_write_frac_helper(const char __user *buffer, unsigned long count,
 			      int *val, int mult)
 {
 	char kernbuf[20], *end, *pbuf;
@@ -1400,8 +1400,8 @@ int lprocfs_alloc_obd_stats(struct obd_device *obd, unsigned num_private_stats)
 		 * LPROCFS_OBD_OP_INIT(.., .., opname)
 		 * is missing from the list above. */
 		LASSERTF(stats->ls_cnt_header[i].lc_name != NULL,
-			 "Missing obd_stat initializer obd_op "
-			 "operation at offset %d.\n", i - num_private_stats);
+			 "Missing obd_stat initializer obd_op operation at offset %d.\n",
+			 i - num_private_stats);
 	}
 	rc = lprocfs_register_stats(obd->obd_proc_entry, "stats", stats);
 	if (rc < 0) {
@@ -1486,8 +1486,7 @@ int lprocfs_alloc_md_stats(struct obd_device *obd,
 
 	for (i = num_private_stats; i < num_stats; i++) {
 		if (stats->ls_cnt_header[i].lc_name == NULL) {
-			CERROR("Missing md_stat initializer md_op "
-			       "operation at offset %d. Aborting.\n",
+			CERROR("Missing md_stat initializer md_op operation at offset %d. Aborting.\n",
 			       i - num_private_stats);
 			LBUG();
 		}
@@ -1607,8 +1606,7 @@ LPROC_SEQ_FOPS_RO(lproc_exp_hash);
 int lprocfs_nid_stats_clear_read(struct seq_file *m, void *data)
 {
 	return seq_printf(m, "%s\n",
-			"Write into this file to clear all nid stats and "
-			"stale nid entries");
+			  "Write into this file to clear all nid stats and stale nid entries");
 }
 EXPORT_SYMBOL(lprocfs_nid_stats_clear_read);
 
@@ -1819,7 +1817,7 @@ __s64 lprocfs_read_helper(struct lprocfs_counter *lc,
 }
 EXPORT_SYMBOL(lprocfs_read_helper);
 
-int lprocfs_write_helper(const char *buffer, unsigned long count,
+int lprocfs_write_helper(const char __user *buffer, unsigned long count,
 			 int *val)
 {
 	return lprocfs_write_frac_helper(buffer, count, val, 1);
diff --git a/drivers/staging/lustre/lustre/obdclass/lu_object.c b/drivers/staging/lustre/lustre/obdclass/lu_object.c
index 2fc037cfb62f..83bf168c2939 100644
--- a/drivers/staging/lustre/lustre/obdclass/lu_object.c
+++ b/drivers/staging/lustre/lustre/obdclass/lu_object.c
@@ -866,8 +866,7 @@ static int lu_htable_order(void)
 
 	/* clear off unreasonable cache setting. */
 	if (lu_cache_percent == 0 || lu_cache_percent > LU_CACHE_PERCENT_MAX) {
-		CWARN("obdclass: invalid lu_cache_percent: %u, it must be in"
-		      " the range of (0, %u]. Will use default value: %u.\n",
+		CWARN("obdclass: invalid lu_cache_percent: %u, it must be in the range of (0, %u]. Will use default value: %u.\n",
 		      lu_cache_percent, LU_CACHE_PERCENT_MAX,
 		      LU_CACHE_PERCENT_DEFAULT);
 
diff --git a/drivers/staging/lustre/lustre/obdclass/obd_config.c b/drivers/staging/lustre/lustre/obdclass/obd_config.c
index 5e7b3d7cc984..6ce9adc2f11c 100644
--- a/drivers/staging/lustre/lustre/obdclass/obd_config.c
+++ b/drivers/staging/lustre/lustre/obdclass/obd_config.c
@@ -835,7 +835,7 @@ int class_del_conn(struct obd_device *obd, struct lustre_cfg *lcfg)
 
 LIST_HEAD(lustre_profile_list);
 
-struct lustre_profile *class_get_profile(const char * prof)
+struct lustre_profile *class_get_profile(const char *prof)
 {
 	struct lustre_profile *lprof;
 
@@ -1443,8 +1443,7 @@ int class_config_llog_handler(const struct lu_env *env,
 		if (!(clli->cfg_flags & CFG_F_COMPAT146) &&
 		    !(clli->cfg_flags & CFG_F_MARKER) &&
 		    (lcfg->lcfg_command != LCFG_MARKER)) {
-			CWARN("Config not inside markers, ignoring! "
-			      "(inst: %p, uuid: %s, flags: %#x)\n",
+			CWARN("Config not inside markers, ignoring! (inst: %p, uuid: %s, flags: %#x)\n",
 			      clli->cfg_instance,
 			      clli->cfg_uuid.uuid, clli->cfg_flags);
 			clli->cfg_flags |= CFG_F_SKIP;
@@ -1467,14 +1466,12 @@ int class_config_llog_handler(const struct lu_env *env,
 
 			if ((lcfg->lcfg_command == LCFG_ATTACH && typename &&
 			     strcmp(typename, "mds") == 0)) {
-				CWARN("For 1.8 interoperability, rename obd "
-				       "type from mds to mdt\n");
+				CWARN("For 1.8 interoperability, rename obd type from mds to mdt\n");
 				typename[2] = 't';
 			}
 			if ((lcfg->lcfg_command == LCFG_SETUP && index &&
 			     strcmp(index, "type") == 0)) {
-				CDEBUG(D_INFO, "For 1.8 interoperability, "
-				       "set this index to '0'\n");
+				CDEBUG(D_INFO, "For 1.8 interoperability, set this index to '0'\n");
 				index[0] = '0';
 				index[1] = 0;
 			}
diff --git a/drivers/staging/lustre/lustre/obdclass/obd_mount.c b/drivers/staging/lustre/lustre/obdclass/obd_mount.c
index 1260c8713bc6..4f39cdee1b5c 100644
--- a/drivers/staging/lustre/lustre/obdclass/obd_mount.c
+++ b/drivers/staging/lustre/lustre/obdclass/obd_mount.c
@@ -100,19 +100,12 @@ int lustre_process_log(struct super_block *sb, char *logname,
 	OBD_FREE_PTR(bufs);
 
 	if (rc == -EINVAL)
-		LCONSOLE_ERROR_MSG(0x15b, "%s: The configuration from log '%s'"
-				   "failed from the MGS (%d).  Make sure this "
-				   "client and the MGS are running compatible "
-				   "versions of Lustre.\n",
+		LCONSOLE_ERROR_MSG(0x15b, "%s: The configuration from log '%s' failed from the MGS (%d).  Make sure this client and the MGS are running compatible versions of Lustre.\n",
 				   mgc->obd_name, logname, rc);
 
 	if (rc)
-		LCONSOLE_ERROR_MSG(0x15c, "%s: The configuration from log '%s' "
-				   "failed (%d). This may be the result of "
-				   "communication errors between this node and "
-				   "the MGS, a bad configuration, or other "
-				   "errors. See the syslog for more "
-				   "information.\n", mgc->obd_name, logname,
+		LCONSOLE_ERROR_MSG(0x15c, "%s: The configuration from log '%s' failed (%d). This may be the result of communication errors between this node and the MGS, a bad configuration, or other errors. See the syslog for more information.\n",
+				   mgc->obd_name, logname,
 				   rc);
 
 	/* class_obd_list(); */
@@ -297,11 +290,8 @@ int lustre_start_mgc(struct super_block *sb)
 			if (has_ir ^ !(*flags & LMD_FLG_NOIR)) {
 				/* LMD_FLG_NOIR is for test purpose only */
 				LCONSOLE_WARN(
-				    "Trying to mount a client with IR setting "
-				    "not compatible with current mgc. "
-				    "Force to use current mgc setting that is "
-				    "IR %s.\n",
-				    has_ir ? "enabled" : "disabled");
+					"Trying to mount a client with IR setting not compatible with current mgc. Force to use current mgc setting that is IR %s.\n",
+					has_ir ? "enabled" : "disabled");
 				if (has_ir)
 					*flags &= ~LMD_FLG_NOIR;
 				else
@@ -998,16 +988,14 @@ static int lmd_parse(char *options, struct lustre_mount_data *lmd)
 
 	LASSERT(lmd);
 	if (!options) {
-		LCONSOLE_ERROR_MSG(0x162, "Missing mount data: check that "
-				   "/sbin/mount.lustre is installed.\n");
+		LCONSOLE_ERROR_MSG(0x162, "Missing mount data: check that /sbin/mount.lustre is installed.\n");
 		return -EINVAL;
 	}
 
 	/* Options should be a string - try to detect old lmd data */
 	if ((raw->lmd_magic & 0xffffff00) == (LMD_MAGIC & 0xffffff00)) {
-		LCONSOLE_ERROR_MSG(0x163, "You're using an old version of "
-				   "/sbin/mount.lustre.  Please install "
-				   "version %s\n", LUSTRE_VERSION_STRING);
+		LCONSOLE_ERROR_MSG(0x163, "You're using an old version of /sbin/mount.lustre.  Please install version %s\n",
+				   LUSTRE_VERSION_STRING);
 		return -EINVAL;
 	}
 	lmd->lmd_magic = LMD_MAGIC;
@@ -1139,8 +1127,7 @@ static int lmd_parse(char *options, struct lustre_mount_data *lmd)
 	}
 
 	if (!devname) {
-		LCONSOLE_ERROR_MSG(0x164, "Can't find the device name "
-				   "(need mount option 'device=...')\n");
+		LCONSOLE_ERROR_MSG(0x164, "Can't find the device name (need mount option 'device=...')\n");
 		goto invalid;
 	}
 
@@ -1232,9 +1219,7 @@ int lustre_fill_super(struct super_block *sb, void *data, int silent)
 		if (client_fill_super == NULL)
 			request_module("lustre");
 		if (client_fill_super == NULL) {
-			LCONSOLE_ERROR_MSG(0x165, "Nothing registered for "
-					   "client mount! Is the 'lustre' "
-					   "module loaded?\n");
+			LCONSOLE_ERROR_MSG(0x165, "Nothing registered for client mount! Is the 'lustre' module loaded?\n");
 			lustre_put_lsi(sb);
 			rc = -ENODEV;
 		} else {
@@ -1249,8 +1234,7 @@ int lustre_fill_super(struct super_block *sb, void *data, int silent)
 			/* c_f_s will call lustre_common_put_super on failure */
 		}
 	} else {
-		CERROR("This is client-side-only module, "
-		       "cannot handle server mount.\n");
+		CERROR("This is client-side-only module, cannot handle server mount.\n");
 		rc = -EINVAL;
 	}
 
diff --git a/drivers/staging/lustre/lustre/obdecho/echo_client.c b/drivers/staging/lustre/lustre/obdecho/echo_client.c
index 98e4290919d0..5f6d9441bc44 100644
--- a/drivers/staging/lustre/lustre/obdecho/echo_client.c
+++ b/drivers/staging/lustre/lustre/obdecho/echo_client.c
@@ -698,14 +698,16 @@ static struct lu_device *echo_device_alloc(const struct lu_env *env,
 	int cleanup = 0;
 
 	OBD_ALLOC_PTR(ed);
-	if (ed == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (ed == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	cleanup = 1;
 	cd = &ed->ed_cl;
 	rc = cl_device_init(cd, t);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	cd->cd_lu_dev.ld_ops = &echo_device_lu_ops;
 	cd->cd_ops = &echo_device_cl_ops;
@@ -719,24 +721,26 @@ static struct lu_device *echo_device_alloc(const struct lu_env *env,
 	if (tgt == NULL) {
 		CERROR("Can not find tgt device %s\n",
 			lustre_cfg_string(cfg, 1));
-		GOTO(out, rc = -ENODEV);
+		rc = -ENODEV;
+		goto out;
 	}
 
 	next = tgt->obd_lu_dev;
 	if (!strcmp(tgt->obd_type->typ_name, LUSTRE_MDT_NAME)) {
 		CERROR("echo MDT client must be run on server\n");
-		GOTO(out, rc = -EOPNOTSUPP);
+		rc = -EOPNOTSUPP;
+		goto out;
 	}
 
 	rc = echo_site_init(env, ed);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	cleanup = 3;
 
 	rc = echo_client_setup(env, obd, cfg);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	ed->ed_ec = &obd->u.echo_client;
 	cleanup = 4;
@@ -749,15 +753,17 @@ static struct lu_device *echo_device_alloc(const struct lu_env *env,
 	tgt_type_name = tgt->obd_type->typ_name;
 	if (next != NULL) {
 		LASSERT(next != NULL);
-		if (next->ld_site != NULL)
-			GOTO(out, rc = -EBUSY);
+		if (next->ld_site != NULL) {
+			rc = -EBUSY;
+			goto out;
+		}
 
 		next->ld_site = &ed->ed_site->cs_lu;
 		rc = next->ld_type->ldt_ops->ldto_device_init(env, next,
 						next->ld_type->ldt_name,
 							      NULL);
 		if (rc)
-			GOTO(out, rc);
+			goto out;
 
 		/* Tricky case, I have to determine the obd type since
 		 * CLIO uses the different parameters to initialize
@@ -865,8 +871,7 @@ static struct lu_device *echo_device_free(const struct lu_env *env,
 	spin_lock(&ec->ec_lock);
 	while (!list_empty(&ec->ec_objects)) {
 		spin_unlock(&ec->ec_lock);
-		CERROR("echo_client still has objects at cleanup time, "
-		       "wait for 1 second\n");
+		CERROR("echo_client still has objects at cleanup time, wait for 1 second\n");
 		set_current_state(TASK_UNINTERRUPTIBLE);
 		schedule_timeout(cfs_time_seconds(1));
 		lu_site_purge(env, &ed->ed_site->cs_lu, -1);
@@ -968,15 +973,19 @@ static struct echo_object *cl_echo_object_find(struct echo_device *d,
 
 	fid  = &info->eti_fid;
 	rc = ostid_to_fid(fid, &lsm->lsm_oi, 0);
-	if (rc != 0)
-		GOTO(out, eco = ERR_PTR(rc));
+	if (rc != 0) {
+		eco = ERR_PTR(rc);
+		goto out;
+	}
 
 	/* In the function below, .hs_keycmp resolves to
 	 * lu_obj_hop_keycmp() */
 	/* coverity[overrun-buffer-val] */
 	obj = cl_object_find(env, echo_dev2cl(d), fid, &conf->eoc_cl);
-	if (IS_ERR(obj))
-		GOTO(out, eco = (void *)obj);
+	if (IS_ERR(obj)) {
+		eco = (void *)obj;
+		goto out;
+	}
 
 	eco = cl2echo_obj(obj);
 	if (eco->eo_deleted) {
@@ -1076,7 +1085,7 @@ static int cl_echo_enqueue(struct echo_object *eco, u64 start, u64 end,
 	io->ci_ignore_layout = 1;
 	result = cl_io_init(env, io, CIT_MISC, echo_obj2cl(eco));
 	if (result < 0)
-		GOTO(out, result);
+		goto out;
 	LASSERT(result == 0);
 
 	result = cl_echo_enqueue0(env, eco, start, end, mode, cookie, 0);
@@ -1182,7 +1191,7 @@ static int cl_echo_object_brw(struct echo_object *eco, int rw, u64 offset,
 	io->ci_ignore_layout = 1;
 	rc = cl_io_init(env, io, CIT_MISC, obj);
 	if (rc < 0)
-		GOTO(out, rc);
+		goto out;
 	LASSERT(rc == 0);
 
 
@@ -1191,7 +1200,7 @@ static int cl_echo_object_brw(struct echo_object *eco, int rw, u64 offset,
 			      rw == READ ? LCK_PR : LCK_PW, &lh.cookie,
 			      CEF_NEVER);
 	if (rc < 0)
-		GOTO(error_lock, rc);
+		goto error_lock;
 
 	for (i = 0; i < npages; i++) {
 		LASSERT(pages[i]);
@@ -1318,7 +1327,7 @@ static int echo_create_object(const struct lu_env *env, struct echo_device *ed,
 	rc = echo_alloc_memmd(ed, &lsm);
 	if (rc < 0) {
 		CERROR("Cannot allocate md: rc = %d\n", rc);
-		GOTO(failed, rc);
+		goto failed;
 	}
 
 	if (ulsm != NULL) {
@@ -1326,7 +1335,7 @@ static int echo_create_object(const struct lu_env *env, struct echo_device *ed,
 
 		rc = echo_copyin_lsm (ed, lsm, ulsm, ulsm_nob);
 		if (rc != 0)
-			GOTO(failed, rc);
+			goto failed;
 
 		if (lsm->lsm_stripe_count == 0)
 			lsm->lsm_stripe_count = ec->ec_nstripes;
@@ -1363,7 +1372,7 @@ static int echo_create_object(const struct lu_env *env, struct echo_device *ed,
 		rc = obd_create(env, ec->ec_exp, oa, &lsm, oti);
 		if (rc != 0) {
 			CERROR("Cannot create objects: rc = %d\n", rc);
-			GOTO(failed, rc);
+			goto failed;
 		}
 		created = 1;
 	}
@@ -1373,8 +1382,10 @@ static int echo_create_object(const struct lu_env *env, struct echo_device *ed,
 	oa->o_valid |= OBD_MD_FLID;
 
 	eco = cl_echo_object_find(ed, &lsm);
-	if (IS_ERR(eco))
-		GOTO(failed, rc = PTR_ERR(eco));
+	if (IS_ERR(eco)) {
+		rc = PTR_ERR(eco);
+		goto failed;
+	}
 	cl_echo_object_put(eco);
 
 	CDEBUG(D_INFO, "oa oid "DOSTID"\n", POSTID(&oa->o_oi));
@@ -1642,8 +1653,10 @@ static int echo_client_prep_commit(const struct lu_env *env,
 	OBD_ALLOC(lnb, npages * sizeof(struct niobuf_local));
 	OBD_ALLOC(rnb, npages * sizeof(struct niobuf_remote));
 
-	if (lnb == NULL || rnb == NULL)
-		GOTO(out, ret = -ENOMEM);
+	if (lnb == NULL || rnb == NULL) {
+		ret = -ENOMEM;
+		goto out;
+	}
 
 	if (rw == OBD_BRW_WRITE && async)
 		brw_flags |= OBD_BRW_ASYNC;
@@ -1671,7 +1684,7 @@ static int echo_client_prep_commit(const struct lu_env *env,
 		ret = obd_preprw(env, rw, exp, oa, 1, &ioo, rnb, &lpages,
 				 lnb, oti, NULL);
 		if (ret != 0)
-			GOTO(out, ret);
+			goto out;
 		LASSERT(lpages == npages);
 
 		for (i = 0; i < lpages; i++) {
@@ -1704,7 +1717,7 @@ static int echo_client_prep_commit(const struct lu_env *env,
 		ret = obd_commitrw(env, rw, exp, oa, 1, &ioo,
 				   rnb, npages, lnb, oti, ret);
 		if (ret != 0)
-			GOTO(out, ret);
+			goto out;
 
 		/* Reset oti otherwise it would confuse ldiskfs. */
 		memset(oti, 0, sizeof(*oti));
@@ -1862,21 +1875,27 @@ echo_client_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 		return -ENOMEM;
 
 	rc = lu_env_init(env, LCT_DT_THREAD);
-	if (rc)
-		GOTO(out, rc = -ENOMEM);
+	if (rc) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	switch (cmd) {
 	case OBD_IOC_CREATE:		    /* may create echo object */
-		if (!capable(CFS_CAP_SYS_ADMIN))
-			GOTO (out, rc = -EPERM);
+		if (!capable(CFS_CAP_SYS_ADMIN)) {
+			rc = -EPERM;
+			goto out;
+		}
 
 		rc = echo_create_object(env, ed, 1, oa, data->ioc_pbuf1,
 					data->ioc_plen1, &dummy_oti);
-		GOTO(out, rc);
+		goto out;
 
 	case OBD_IOC_DESTROY:
-		if (!capable(CFS_CAP_SYS_ADMIN))
-			GOTO (out, rc = -EPERM);
+		if (!capable(CFS_CAP_SYS_ADMIN)) {
+			rc = -EPERM;
+			goto out;
+		}
 
 		rc = echo_get_object(&eco, ed, oa);
 		if (rc == 0) {
@@ -1886,7 +1905,7 @@ echo_client_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 				eco->eo_deleted = 1;
 			echo_put_object(eco);
 		}
-		GOTO(out, rc);
+		goto out;
 
 	case OBD_IOC_GETATTR:
 		rc = echo_get_object(&eco, ed, oa);
@@ -1897,11 +1916,13 @@ echo_client_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 			rc = obd_getattr(env, ec->ec_exp, &oinfo);
 			echo_put_object(eco);
 		}
-		GOTO(out, rc);
+		goto out;
 
 	case OBD_IOC_SETATTR:
-		if (!capable(CFS_CAP_SYS_ADMIN))
-			GOTO (out, rc = -EPERM);
+		if (!capable(CFS_CAP_SYS_ADMIN)) {
+			rc = -EPERM;
+			goto out;
+		}
 
 		rc = echo_get_object(&eco, ed, oa);
 		if (rc == 0) {
@@ -1912,17 +1933,19 @@ echo_client_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 			rc = obd_setattr(env, ec->ec_exp, &oinfo, NULL);
 			echo_put_object(eco);
 		}
-		GOTO(out, rc);
+		goto out;
 
 	case OBD_IOC_BRW_WRITE:
-		if (!capable(CFS_CAP_SYS_ADMIN))
-			GOTO (out, rc = -EPERM);
+		if (!capable(CFS_CAP_SYS_ADMIN)) {
+			rc = -EPERM;
+			goto out;
+		}
 
 		rw = OBD_BRW_WRITE;
 		/* fall through */
 	case OBD_IOC_BRW_READ:
 		rc = echo_client_brw_ioctl(env, rw, exp, data, &dummy_oti);
-		GOTO(out, rc);
+		goto out;
 
 	case ECHO_IOC_GET_STRIPE:
 		rc = echo_get_object(&eco, ed, oa);
@@ -1931,11 +1954,13 @@ echo_client_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 					      data->ioc_plen1);
 			echo_put_object(eco);
 		}
-		GOTO(out, rc);
+		goto out;
 
 	case ECHO_IOC_SET_STRIPE:
-		if (!capable(CFS_CAP_SYS_ADMIN))
-			GOTO (out, rc = -EPERM);
+		if (!capable(CFS_CAP_SYS_ADMIN)) {
+			rc = -EPERM;
+			goto out;
+		}
 
 		if (data->ioc_pbuf1 == NULL) {  /* unset */
 			rc = echo_get_object(&eco, ed, oa);
@@ -1948,25 +1973,28 @@ echo_client_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 						data->ioc_pbuf1,
 						data->ioc_plen1, &dummy_oti);
 		}
-		GOTO (out, rc);
+		goto out;
 
 	case ECHO_IOC_ENQUEUE:
-		if (!capable(CFS_CAP_SYS_ADMIN))
-			GOTO (out, rc = -EPERM);
+		if (!capable(CFS_CAP_SYS_ADMIN)) {
+			rc = -EPERM;
+			goto out;
+		}
 
 		rc = echo_client_enqueue(exp, oa,
 					 data->ioc_conn1, /* lock mode */
 					 data->ioc_offset,
 					 data->ioc_count);/*extent*/
-		GOTO (out, rc);
+		goto out;
 
 	case ECHO_IOC_CANCEL:
 		rc = echo_client_cancel(exp, oa);
-		GOTO (out, rc);
+		goto out;
 
 	default:
 		CERROR ("echo_ioctl(): unrecognised ioctl %#x\n", cmd);
-		GOTO (out, rc = -ENOTTY);
+		rc = -ENOTTY;
+		goto out;
 	}
 
 out:
@@ -2084,11 +2112,13 @@ static int echo_client_disconnect(struct obd_export *exp)
 {
 	int		     rc;
 
-	if (exp == NULL)
-		GOTO(out, rc = -EINVAL);
+	if (exp == NULL) {
+		rc = -EINVAL;
+		goto out;
+	}
 
 	rc = class_disconnect(exp);
-	GOTO(out, rc);
+	goto out;
  out:
 	return rc;
 }
diff --git a/drivers/staging/lustre/lustre/osc/osc_cache.c b/drivers/staging/lustre/lustre/osc/osc_cache.c
index 7734d666b7a1..370e6d4896c6 100644
--- a/drivers/staging/lustre/lustre/osc/osc_cache.c
+++ b/drivers/staging/lustre/lustre/osc/osc_cache.c
@@ -178,76 +178,113 @@ static int osc_extent_sanity_check0(struct osc_extent *ext,
 	int page_count;
 	int rc = 0;
 
-	if (!osc_object_is_locked(obj))
-		GOTO(out, rc = 9);
+	if (!osc_object_is_locked(obj)) {
+		rc = 9;
+		goto out;
+	}
 
-	if (ext->oe_state >= OES_STATE_MAX)
-		GOTO(out, rc = 10);
+	if (ext->oe_state >= OES_STATE_MAX) {
+		rc = 10;
+		goto out;
+	}
 
-	if (atomic_read(&ext->oe_refc) <= 0)
-		GOTO(out, rc = 20);
+	if (atomic_read(&ext->oe_refc) <= 0) {
+		rc = 20;
+		goto out;
+	}
 
-	if (atomic_read(&ext->oe_refc) < atomic_read(&ext->oe_users))
-		GOTO(out, rc = 30);
+	if (atomic_read(&ext->oe_refc) < atomic_read(&ext->oe_users)) {
+		rc = 30;
+		goto out;
+	}
 
 	switch (ext->oe_state) {
 	case OES_INV:
 		if (ext->oe_nr_pages > 0 || !list_empty(&ext->oe_pages))
-			GOTO(out, rc = 35);
-		GOTO(out, rc = 0);
-		break;
+			rc = 35;
+		else
+			rc = 0;
+		goto out;
 	case OES_ACTIVE:
-		if (atomic_read(&ext->oe_users) == 0)
-			GOTO(out, rc = 40);
-		if (ext->oe_hp)
-			GOTO(out, rc = 50);
-		if (ext->oe_fsync_wait && !ext->oe_urgent)
-			GOTO(out, rc = 55);
+		if (atomic_read(&ext->oe_users) == 0) {
+			rc = 40;
+			goto out;
+		}
+		if (ext->oe_hp) {
+			rc = 50;
+			goto out;
+		}
+		if (ext->oe_fsync_wait && !ext->oe_urgent) {
+			rc = 55;
+			goto out;
+		}
 		break;
 	case OES_CACHE:
-		if (ext->oe_grants == 0)
-			GOTO(out, rc = 60);
-		if (ext->oe_fsync_wait && !ext->oe_urgent && !ext->oe_hp)
-			GOTO(out, rc = 65);
+		if (ext->oe_grants == 0) {
+			rc = 60;
+			goto out;
+		}
+		if (ext->oe_fsync_wait && !ext->oe_urgent && !ext->oe_hp) {
+			rc = 65;
+			goto out;
+		}
 	default:
-		if (atomic_read(&ext->oe_users) > 0)
-			GOTO(out, rc = 70);
+		if (atomic_read(&ext->oe_users) > 0) {
+			rc = 70;
+			goto out;
+		}
 	}
 
-	if (ext->oe_max_end < ext->oe_end || ext->oe_end < ext->oe_start)
-		GOTO(out, rc = 80);
+	if (ext->oe_max_end < ext->oe_end || ext->oe_end < ext->oe_start) {
+		rc = 80;
+		goto out;
+	}
 
-	if (ext->oe_osclock == NULL && ext->oe_grants > 0)
-		GOTO(out, rc = 90);
+	if (ext->oe_osclock == NULL && ext->oe_grants > 0) {
+		rc = 90;
+		goto out;
+	}
 
 	if (ext->oe_osclock) {
 		struct cl_lock_descr *descr;
 		descr = &ext->oe_osclock->cll_descr;
 		if (!(descr->cld_start <= ext->oe_start &&
-		      descr->cld_end >= ext->oe_max_end))
-			GOTO(out, rc = 100);
+		      descr->cld_end >= ext->oe_max_end)) {
+			rc = 100;
+			goto out;
+		}
 	}
 
-	if (ext->oe_nr_pages > ext->oe_mppr)
-		GOTO(out, rc = 105);
+	if (ext->oe_nr_pages > ext->oe_mppr) {
+		rc = 105;
+		goto out;
+	}
 
 	/* Do not verify page list if extent is in RPC. This is because an
 	 * in-RPC extent is supposed to be exclusively accessible w/o lock. */
-	if (ext->oe_state > OES_CACHE)
-		GOTO(out, rc = 0);
+	if (ext->oe_state > OES_CACHE) {
+		rc = 0;
+		goto out;
+	}
 
-	if (!extent_debug)
-		GOTO(out, rc = 0);
+	if (!extent_debug) {
+		rc = 0;
+		goto out;
+	}
 
 	page_count = 0;
 	list_for_each_entry(oap, &ext->oe_pages, oap_pending_item) {
 		pgoff_t index = oap2cl_page(oap)->cp_index;
 		++page_count;
-		if (index > ext->oe_end || index < ext->oe_start)
-			GOTO(out, rc = 110);
+		if (index > ext->oe_end || index < ext->oe_start) {
+			rc = 110;
+			goto out;
+		}
+	}
+	if (page_count != ext->oe_nr_pages) {
+		rc = 120;
+		goto out;
 	}
-	if (page_count != ext->oe_nr_pages)
-		GOTO(out, rc = 120);
 
 out:
 	if (rc != 0)
@@ -536,10 +573,9 @@ static int osc_extent_merge(const struct lu_env *env, struct osc_extent *cur,
 /**
  * Drop user count of osc_extent, and unplug IO asynchronously.
  */
-int osc_extent_release(const struct lu_env *env, struct osc_extent *ext)
+void osc_extent_release(const struct lu_env *env, struct osc_extent *ext)
 {
 	struct osc_object *obj = ext->oe_obj;
-	int rc = 0;
 
 	LASSERT(atomic_read(&ext->oe_users) > 0);
 	LASSERT(sanity_check(ext) == 0);
@@ -571,7 +607,6 @@ int osc_extent_release(const struct lu_env *env, struct osc_extent *ext)
 		osc_io_unplug_async(env, osc_cli(obj), obj);
 	}
 	osc_extent_put(env, ext);
-	return rc;
 }
 
 static inline int overlapped(struct osc_extent *ex1, struct osc_extent *ex2)
@@ -776,8 +811,10 @@ restart:
 		rc = osc_extent_wait(env, conflict, OES_INV);
 		osc_extent_put(env, conflict);
 		conflict = NULL;
-		if (rc < 0)
-			GOTO(out, found = ERR_PTR(rc));
+		if (rc < 0) {
+			found = ERR_PTR(rc);
+			goto out;
+		}
 
 		goto restart;
 	}
@@ -934,7 +971,7 @@ static int osc_extent_truncate(struct osc_extent *ext, pgoff_t trunc_index,
 	io->ci_obj = cl_object_top(osc2cl(obj));
 	rc = cl_io_init(env, io, CIT_MISC, io->ci_obj);
 	if (rc < 0)
-		GOTO(out, rc);
+		goto out;
 
 	/* discard all pages with index greater then trunc_index */
 	list_for_each_entry_safe(oap, tmp, &ext->oe_pages,
@@ -1114,21 +1151,27 @@ static int osc_extent_expand(struct osc_extent *ext, pgoff_t index, int *grants)
 	osc_object_lock(obj);
 	LASSERT(sanity_check_nolock(ext) == 0);
 	end_chunk = ext->oe_end >> ppc_bits;
-	if (chunk > end_chunk + 1)
-		GOTO(out, rc = -ERANGE);
+	if (chunk > end_chunk + 1) {
+		rc = -ERANGE;
+		goto out;
+	}
 
-	if (end_chunk >= chunk)
-		GOTO(out, rc = 0);
+	if (end_chunk >= chunk) {
+		rc = 0;
+		goto out;
+	}
 
 	LASSERT(end_chunk + 1 == chunk);
 	/* try to expand this extent to cover @index */
 	end_index = min(ext->oe_max_end, ((chunk + 1) << ppc_bits) - 1);
 
 	next = next_extent(ext);
-	if (next != NULL && next->oe_start <= end_index)
+	if (next != NULL && next->oe_start <= end_index) {
 		/* complex mode - overlapped with the next extent,
 		 * this case will be handled by osc_extent_find() */
-		GOTO(out, rc = -EAGAIN);
+		rc = -EAGAIN;
+		goto out;
+	}
 
 	ext->oe_end = end_index;
 	ext->oe_grants += chunksize;
@@ -1497,12 +1540,16 @@ static int osc_enter_cache(const struct lu_env *env, struct client_obd *cli,
 	 * of queued writes and create a discontiguous rpc stream */
 	if (OBD_FAIL_CHECK(OBD_FAIL_OSC_NO_GRANT) ||
 	    cli->cl_dirty_max < PAGE_CACHE_SIZE     ||
-	    cli->cl_ar.ar_force_sync || loi->loi_ar.ar_force_sync)
-		GOTO(out, rc = -EDQUOT);
+	    cli->cl_ar.ar_force_sync || loi->loi_ar.ar_force_sync) {
+		rc = -EDQUOT;
+		goto out;
+	}
 
 	/* Hopefully normal case - cache space and write credits available */
-	if (osc_enter_cache_try(cli, oap, bytes, 0))
-		GOTO(out, rc = 0);
+	if (osc_enter_cache_try(cli, oap, bytes, 0)) {
+		rc = 0;
+		goto out;
+	}
 
 	/* We can get here for two reasons: too many dirty pages in cache, or
 	 * run out of grants. In both cases we should write dirty pages out.
@@ -1530,16 +1577,18 @@ static int osc_enter_cache(const struct lu_env *env, struct client_obd *cli,
 		/* l_wait_event is interrupted by signal */
 		if (rc < 0) {
 			list_del_init(&ocw.ocw_entry);
-			GOTO(out, rc);
+			goto out;
 		}
 
 		LASSERT(list_empty(&ocw.ocw_entry));
 		rc = ocw.ocw_rc;
 
 		if (rc != -EDQUOT)
-			GOTO(out, rc);
-		if (osc_enter_cache_try(cli, oap, bytes, 0))
-			GOTO(out, rc = 0);
+			goto out;
+		if (osc_enter_cache_try(cli, oap, bytes, 0)) {
+			rc = 0;
+			goto out;
+		}
 	}
 out:
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
@@ -1562,8 +1611,8 @@ void osc_wake_cache_waiters(struct client_obd *cli)
 		if ((cli->cl_dirty + PAGE_CACHE_SIZE > cli->cl_dirty_max) ||
 		    (atomic_read(&obd_dirty_pages) + 1 >
 		     obd_max_dirty_pages)) {
-			CDEBUG(D_CACHE, "no dirty room: dirty: %ld "
-			       "osc max %ld, sys max %d\n", cli->cl_dirty,
+			CDEBUG(D_CACHE, "no dirty room: dirty: %ld osc max %ld, sys max %d\n",
+			       cli->cl_dirty,
 			       cli->cl_dirty_max, obd_max_dirty_pages);
 			goto wakeup;
 		}
@@ -2401,14 +2450,15 @@ int osc_flush_async_page(const struct lu_env *env, struct cl_io *io,
 		 * one making the extent active, we could deadlock waiting for
 		 * the page writeback to clear but it won't because the extent
 		 * is active and won't be written out. */
-		GOTO(out, rc = -EAGAIN);
+		rc = -EAGAIN;
+		goto out;
 	default:
 		break;
 	}
 
 	rc = cl_page_prep(env, io, cl_page_top(cp), CRT_WRITE);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	spin_lock(&oap->oap_lock);
 	oap->oap_async_flags |= ASYNC_READY|ASYNC_URGENT;
diff --git a/drivers/staging/lustre/lustre/osc/osc_cl_internal.h b/drivers/staging/lustre/lustre/osc/osc_cl_internal.h
index ebbd95c0cea8..365b2787b3c8 100644
--- a/drivers/staging/lustre/lustre/osc/osc_cl_internal.h
+++ b/drivers/staging/lustre/lustre/osc/osc_cl_internal.h
@@ -678,7 +678,7 @@ struct osc_extent {
 
 int osc_extent_finish(const struct lu_env *env, struct osc_extent *ext,
 		      int sent, int rc);
-int osc_extent_release(const struct lu_env *env, struct osc_extent *ext);
+void osc_extent_release(const struct lu_env *env, struct osc_extent *ext);
 
 /** @} osc */
 
diff --git a/drivers/staging/lustre/lustre/osc/osc_lock.c b/drivers/staging/lustre/lustre/osc/osc_lock.c
index 8138856fda8c..a7f08bc48166 100644
--- a/drivers/staging/lustre/lustre/osc/osc_lock.c
+++ b/drivers/staging/lustre/lustre/osc/osc_lock.c
@@ -1067,14 +1067,12 @@ static int osc_lock_enqueue_wait(const struct lu_env *env,
 			 * conflicts, we do not wait but return 0 so the
 			 * request is send to the server
 			 */
-			CDEBUG(D_DLMTRACE, "group lock %p is conflicted "
-					   "with %p, no wait, send to server\n",
+			CDEBUG(D_DLMTRACE, "group lock %p is conflicted with %p, no wait, send to server\n",
 			       lock, conflict);
 			cl_lock_put(env, conflict);
 			rc = 0;
 		} else {
-			CDEBUG(D_DLMTRACE, "lock %p is conflicted with %p, "
-					   "will wait\n",
+			CDEBUG(D_DLMTRACE, "lock %p is conflicted with %p, will wait\n",
 			       lock, conflict);
 			LASSERT(lock->cll_conflict == NULL);
 			lu_ref_add(&conflict->cll_reference, "cancel-wait",
diff --git a/drivers/staging/lustre/lustre/osc/osc_object.c b/drivers/staging/lustre/lustre/osc/osc_object.c
index 69000584619d..92c202f70395 100644
--- a/drivers/staging/lustre/lustre/osc/osc_object.c
+++ b/drivers/staging/lustre/lustre/osc/osc_object.c
@@ -140,8 +140,7 @@ static int osc_object_print(const struct lu_env *env, void *cookie,
 	struct lov_oinfo    *oinfo = osc->oo_oinfo;
 	struct osc_async_rc *ar    = &oinfo->loi_ar;
 
-	(*p)(env, cookie, "id: "DOSTID" "
-	     "idx: %d gen: %d kms_valid: %u kms %llu rc: %d force_sync: %d min_xid: %llu ",
+	(*p)(env, cookie, "id: " DOSTID " idx: %d gen: %d kms_valid: %u kms %llu rc: %d force_sync: %d min_xid: %llu ",
 	     POSTID(&oinfo->loi_oi), oinfo->loi_ost_idx,
 	     oinfo->loi_ost_gen, oinfo->loi_kms_valid, oinfo->loi_kms,
 	     ar->ar_rc, ar->ar_force_sync, ar->ar_min_xid);
diff --git a/drivers/staging/lustre/lustre/osc/osc_page.c b/drivers/staging/lustre/lustre/osc/osc_page.c
index fcd079b1af01..76ba58b09c5d 100644
--- a/drivers/staging/lustre/lustre/osc/osc_page.c
+++ b/drivers/staging/lustre/lustre/osc/osc_page.c
@@ -369,12 +369,7 @@ static int osc_page_print(const struct lu_env *env,
 	struct osc_object     *obj = cl2osc(slice->cpl_obj);
 	struct client_obd     *cli = &osc_export(obj)->exp_obd->u.cli;
 
-	return (*printer)(env, cookie, LUSTRE_OSC_NAME"-page@%p: "
-			  "1< %#x %d %u %s %s > "
-			  "2< %llu %u %u %#x %#x | %p %p %p > "
-			  "3< %s %p %d %lu %d > "
-			  "4< %d %d %d %lu %s | %s %s %s %s > "
-			  "5< %s %s %s %s | %d %s | %d %s %s>\n",
+	return (*printer)(env, cookie, LUSTRE_OSC_NAME "-page@%p: 1< %#x %d %u %s %s > 2< %llu %u %u %#x %#x | %p %p %p > 3< %s %p %d %lu %d > 4< %d %d %d %lu %s | %s %s %s %s > 5< %s %s %s %s | %d %s | %d %s %s>\n",
 			  opg,
 			  /* 1 */
 			  oap->oap_magic, oap->oap_cmd,
@@ -550,8 +545,8 @@ void osc_page_submit(const struct lu_env *env, struct osc_page *opg,
 	LINVRNT(osc_page_protected(env, opg,
 				   crt == CRT_WRITE ? CLM_WRITE : CLM_READ, 1));
 
-	LASSERTF(oap->oap_magic == OAP_MAGIC, "Bad oap magic: oap %p, "
-		 "magic 0x%x\n", oap, oap->oap_magic);
+	LASSERTF(oap->oap_magic == OAP_MAGIC, "Bad oap magic: oap %p, magic 0x%x\n",
+		 oap, oap->oap_magic);
 	LASSERT(oap->oap_async_flags & ASYNC_READY);
 	LASSERT(oap->oap_async_flags & ASYNC_COUNT_STABLE);
 
diff --git a/drivers/staging/lustre/lustre/osc/osc_request.c b/drivers/staging/lustre/lustre/osc/osc_request.c
index 44657a06b8a5..b9450b95f1c5 100644
--- a/drivers/staging/lustre/lustre/osc/osc_request.c
+++ b/drivers/staging/lustre/lustre/osc/osc_request.c
@@ -1078,9 +1078,9 @@ static void osc_init_grant(struct client_obd *cli, struct obd_connect_data *ocd)
 	cli->cl_chunkbits = max_t(int, PAGE_CACHE_SHIFT, ocd->ocd_blocksize);
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
 
-	CDEBUG(D_CACHE, "%s, setting cl_avail_grant: %ld cl_lost_grant: %ld."
-		"chunk bits: %d.\n", cli->cl_import->imp_obd->obd_name,
-		cli->cl_avail_grant, cli->cl_lost_grant, cli->cl_chunkbits);
+	CDEBUG(D_CACHE, "%s, setting cl_avail_grant: %ld cl_lost_grant: %ld chunk bits: %d\n",
+	       cli->cl_import->imp_obd->obd_name,
+	       cli->cl_avail_grant, cli->cl_lost_grant, cli->cl_chunkbits);
 
 	if (ocd->ocd_connect_flags & OBD_CONNECT_GRANT_SHRINK &&
 	    list_empty(&cli->cl_grant_shrink_list))
@@ -1171,8 +1171,7 @@ static inline int can_merge_pages(struct brw_page *p1, struct brw_page *p2)
 		/* warn if we try to combine flags that we don't know to be
 		 * safe to combine */
 		if (unlikely((p1->flag & mask) != (p2->flag & mask))) {
-			CWARN("Saw flags 0x%x and 0x%x in the same brw, please "
-			      "report this at http://bugs.whamcloud.com/\n",
+			CWARN("Saw flags 0x%x and 0x%x in the same brw, please report this at http://bugs.whamcloud.com/\n",
 			      p1->flag, p2->flag);
 		}
 		return 0;
@@ -1343,8 +1342,7 @@ static int osc_brw_prep_request(int cmd, struct client_obd *cli,
 			 "i: %d/%d pg: %p off: %llu, count: %u\n",
 			 i, page_count, pg, pg->off, pg->count);
 		LASSERTF(i == 0 || pg->off > pg_prev->off,
-			 "i %d p_c %u pg %p [pri %lu ind %lu] off %llu"
-			 " prev_pg %p [pri %lu ind %lu] off %llu\n",
+			 "i %d p_c %u pg %p [pri %lu ind %lu] off %llu prev_pg %p [pri %lu ind %lu] off %llu\n",
 			 i, page_count,
 			 pg->pg, page_private(pg->pg), pg->pg->index, pg->off,
 			 pg_prev->pg, page_private(pg_prev->pg),
@@ -1467,16 +1465,16 @@ static int check_write_checksum(struct obdo *oa, const lnet_process_id_t *peer,
 				      cksum_type);
 
 	if (cksum_type != client_cksum_type)
-		msg = "the server did not use the checksum type specified in "
-		      "the original request - likely a protocol problem";
+		msg = "the server did not use the checksum type specified in the original request - likely a protocol problem"
+			;
 	else if (new_cksum == server_cksum)
-		msg = "changed on the client after we checksummed it - "
-		      "likely false positive due to mmap IO (bug 11742)";
+		msg = "changed on the client after we checksummed it - likely false positive due to mmap IO (bug 11742)"
+			;
 	else if (new_cksum == client_cksum)
 		msg = "changed in transit before arrival at OST";
 	else
-		msg = "changed in transit AND doesn't match the original - "
-		      "likely false positive due to mmap IO (bug 11742)";
+		msg = "changed in transit AND doesn't match the original - likely false positive due to mmap IO (bug 11742)"
+			;
 
 	LCONSOLE_ERROR_MSG(0x132, "BAD WRITE CHECKSUM: %s: from %s inode "DFID
 			   " object "DOSTID" extent [%llu-%llu]\n",
@@ -1486,8 +1484,8 @@ static int check_write_checksum(struct obdo *oa, const lnet_process_id_t *peer,
 			   oa->o_valid & OBD_MD_FLFID ? oa->o_parent_ver : 0,
 			   POSTID(&oa->o_oi), pga[0]->off,
 			   pga[page_count-1]->off + pga[page_count-1]->count - 1);
-	CERROR("original client csum %x (type %x), server csum %x (type %x), "
-	       "client csum now %x\n", client_cksum, client_cksum_type,
+	CERROR("original client csum %x (type %x), server csum %x (type %x), client csum now %x\n",
+	       client_cksum, client_cksum_type,
 	       server_cksum, cksum_type, new_cksum);
 	return 1;
 }
@@ -1601,23 +1599,21 @@ static int osc_brw_fini_request(struct ptlrpc_request *req, int rc)
 		}
 
 		if (server_cksum != client_cksum) {
-			LCONSOLE_ERROR_MSG(0x133, "%s: BAD READ CHECKSUM: from "
-					   "%s%s%s inode "DFID" object "DOSTID
-					   " extent [%llu-%llu]\n",
+			LCONSOLE_ERROR_MSG(0x133, "%s: BAD READ CHECKSUM: from %s%s%s inode " DFID " object " DOSTID " extent [%llu-%llu]\n",
 					   req->rq_import->imp_obd->obd_name,
 					   libcfs_nid2str(peer->nid),
 					   via, router,
 					   body->oa.o_valid & OBD_MD_FLFID ?
-						body->oa.o_parent_seq : (__u64)0,
+					   body->oa.o_parent_seq : (__u64)0,
 					   body->oa.o_valid & OBD_MD_FLFID ?
-						body->oa.o_parent_oid : 0,
+					   body->oa.o_parent_oid : 0,
 					   body->oa.o_valid & OBD_MD_FLFID ?
-						body->oa.o_parent_ver : 0,
+					   body->oa.o_parent_ver : 0,
 					   POSTID(&body->oa.o_oi),
 					   aa->aa_ppga[0]->off,
 					   aa->aa_ppga[aa->aa_page_count-1]->off +
 					   aa->aa_ppga[aa->aa_page_count-1]->count -
-									1);
+					   1);
 			CERROR("client %x, server %x, cksum_type %x\n",
 			       client_cksum, server_cksum, cksum_type);
 			cksum_counter = 0;
@@ -1771,8 +1767,7 @@ static int brw_interpret(const struct lu_env *env,
 	if (osc_recoverable_error(rc)) {
 		if (req->rq_import_generation !=
 		    req->rq_import->imp_generation) {
-			CDEBUG(D_HA, "%s: resend cross eviction for object: "
-			       ""DOSTID", rc = %d.\n",
+			CDEBUG(D_HA, "%s: resend cross eviction for object: " DOSTID ", rc = %d.\n",
 			       req->rq_import->imp_obd->obd_name,
 			       POSTID(&aa->aa_oa->o_oi), rc);
 		} else if (rc == -EINPROGRESS ||
@@ -3013,8 +3008,8 @@ static int osc_reconnect(const struct lu_env *env,
 		cli->cl_lost_grant = 0;
 		client_obd_list_unlock(&cli->cl_loi_list_lock);
 
-		CDEBUG(D_RPCTRACE, "ocd_connect_flags: %#llx ocd_version: %d"
-		       " ocd_grant: %d, lost: %ld.\n", data->ocd_connect_flags,
+		CDEBUG(D_RPCTRACE, "ocd_connect_flags: %#llx ocd_version: %d ocd_grant: %d, lost: %ld.\n",
+		       data->ocd_connect_flags,
 		       data->ocd_version, data->ocd_grant, lost_grant);
 	}
 
@@ -3217,8 +3212,6 @@ out_ptlrpcd:
 
 static int osc_precleanup(struct obd_device *obd, enum obd_cleanup_stage stage)
 {
-	int rc = 0;
-
 	switch (stage) {
 	case OBD_CLEANUP_EARLY: {
 		struct obd_import *imp;
@@ -3253,7 +3246,7 @@ static int osc_precleanup(struct obd_device *obd, enum obd_cleanup_stage stage)
 		break;
 		}
 	}
-	return rc;
+	return 0;
 }
 
 int osc_cleanup(struct obd_device *obd)
diff --git a/drivers/staging/lustre/lustre/ptlrpc/client.c b/drivers/staging/lustre/lustre/ptlrpc/client.c
index 38cc931a189c..dc9e406f3212 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/client.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/client.c
@@ -265,8 +265,7 @@ static void ptlrpc_at_adj_service(struct ptlrpc_request *req,
 	   so just keep minimal history here */
 	oldse = at_measured(&at->iat_service_estimate[idx], serv_est);
 	if (oldse != 0)
-		CDEBUG(D_ADAPTTO, "The RPC service estimate for %s ptl %d "
-		       "has changed from %d to %d\n",
+		CDEBUG(D_ADAPTTO, "The RPC service estimate for %s ptl %d has changed from %d to %d\n",
 		       req->rq_import->imp_obd->obd_name, req->rq_request_portal,
 		       oldse, at_get(&at->iat_service_estimate[idx]));
 }
@@ -297,8 +296,7 @@ static void ptlrpc_at_adj_net_latency(struct ptlrpc_request *req,
 
 	oldnl = at_measured(&at->iat_net_latency, nl);
 	if (oldnl != 0)
-		CDEBUG(D_ADAPTTO, "The network latency for %s (nid %s) "
-		       "has changed from %d to %d\n",
+		CDEBUG(D_ADAPTTO, "The network latency for %s (nid %s) has changed from %d to %d\n",
 		       req->rq_import->imp_obd->obd_name,
 		       obd_uuid2str(
 			       &req->rq_import->imp_connection->c_remote_uuid),
@@ -371,8 +369,8 @@ static int ptlrpc_at_recv_early_reply(struct ptlrpc_request *req)
 			   ptlrpc_at_get_net_latency(req);
 
 	DEBUG_REQ(D_ADAPTTO, req,
-		  "Early reply #%d, new deadline in "CFS_DURATION_T"s "
-		  "("CFS_DURATION_T"s)", req->rq_early_count,
+		  "Early reply #%d, new deadline in " CFS_DURATION_T "s (" CFS_DURATION_T "s)",
+		  req->rq_early_count,
 		  cfs_time_sub(req->rq_deadline, get_seconds()),
 		  cfs_time_sub(req->rq_deadline, olddl));
 
@@ -445,8 +443,8 @@ void ptlrpc_add_rqs_to_pool(struct ptlrpc_request_pool *pool, int num_rq)
 
 	LASSERTF(list_empty(&pool->prp_req_list) ||
 		 size == pool->prp_rq_size,
-		 "Trying to change pool size with nonempty pool "
-		 "from %d to %d bytes\n", pool->prp_rq_size, size);
+		 "Trying to change pool size with nonempty pool from %d to %d bytes\n",
+		 pool->prp_rq_size, size);
 
 	spin_lock(&pool->prp_lock);
 	pool->prp_rq_size = size;
@@ -1146,11 +1144,10 @@ static int ptlrpc_check_status(struct ptlrpc_request *req)
 		struct obd_import *imp = req->rq_import;
 		__u32 opc = lustre_msg_get_opc(req->rq_reqmsg);
 		if (ptlrpc_console_allow(req))
-			LCONSOLE_ERROR_MSG(0x011, "%s: Communicating with %s,"
-					   " operation %s failed with %d.\n",
+			LCONSOLE_ERROR_MSG(0x011, "%s: Communicating with %s, operation %s failed with %d.\n",
 					   imp->imp_obd->obd_name,
 					   libcfs_nid2str(
-					   imp->imp_connection->c_peer.nid),
+						   imp->imp_connection->c_peer.nid),
 					   ll_opcode2str(opc), err);
 		return err < 0 ? err : -EINVAL;
 	}
@@ -1206,8 +1203,7 @@ static int after_reply(struct ptlrpc_request *req)
 
 	if (req->rq_reply_truncate) {
 		if (ptlrpc_no_resend(req)) {
-			DEBUG_REQ(D_ERROR, req, "reply buffer overflow,"
-				  " expected: %d, actual size: %d",
+			DEBUG_REQ(D_ERROR, req, "reply buffer overflow, expected: %d, actual size: %d",
 				  req->rq_nob_received, req->rq_repbuf_len);
 			return -EOVERFLOW;
 		}
@@ -1259,8 +1255,7 @@ static int after_reply(struct ptlrpc_request *req)
 			/* new xid is already allocated for bulk in
 			 * ptlrpc_check_set() */
 			req->rq_xid = ptlrpc_next_xid();
-			DEBUG_REQ(D_RPCTRACE, req, "Allocating new xid for "
-				  "resend on EINPROGRESS");
+			DEBUG_REQ(D_RPCTRACE, req, "Allocating new xid for resend on EINPROGRESS");
 		}
 
 		/* Readjust the timeout for current conditions */
@@ -1412,8 +1407,8 @@ static int ptlrpc_send_new_req(struct ptlrpc_request *req)
 		req->rq_waiting = 1;
 		spin_unlock(&req->rq_lock);
 
-		DEBUG_REQ(D_HA, req, "req from PID %d waiting for recovery: "
-			  "(%s != %s)", lustre_msg_get_status(req->rq_reqmsg),
+		DEBUG_REQ(D_HA, req, "req from PID %d waiting for recovery: (%s != %s)",
+			  lustre_msg_get_status(req->rq_reqmsg),
 			  ptlrpc_import_state_name(req->rq_send_state),
 			  ptlrpc_import_state_name(imp->imp_state));
 		LASSERT(list_empty(&req->rq_list));
@@ -1450,8 +1445,8 @@ static int ptlrpc_send_new_req(struct ptlrpc_request *req)
 		}
 	}
 
-	CDEBUG(D_RPCTRACE, "Sending RPC pname:cluuid:pid:xid:nid:opc"
-	       " %s:%s:%d:%llu:%s:%d\n", current_comm(),
+	CDEBUG(D_RPCTRACE, "Sending RPC pname:cluuid:pid:xid:nid:opc %s:%s:%d:%llu:%s:%d\n",
+	       current_comm(),
 	       imp->imp_obd->obd_uuid.uuid,
 	       lustre_msg_get_status(req->rq_reqmsg), req->rq_xid,
 	       libcfs_nid2str(imp->imp_connection->c_peer.nid),
@@ -1828,12 +1823,11 @@ interpret:
 		ptlrpc_rqphase_move(req, RQ_PHASE_COMPLETE);
 
 		CDEBUG(req->rq_reqmsg != NULL ? D_RPCTRACE : 0,
-			"Completed RPC pname:cluuid:pid:xid:nid:"
-			"opc %s:%s:%d:%llu:%s:%d\n",
-			current_comm(), imp->imp_obd->obd_uuid.uuid,
-			lustre_msg_get_status(req->rq_reqmsg), req->rq_xid,
-			libcfs_nid2str(imp->imp_connection->c_peer.nid),
-			lustre_msg_get_opc(req->rq_reqmsg));
+		       "Completed RPC pname:cluuid:pid:xid:nid:opc %s:%s:%d:%llu:%s:%d\n",
+		       current_comm(), imp->imp_obd->obd_uuid.uuid,
+		       lustre_msg_get_status(req->rq_reqmsg), req->rq_xid,
+		       libcfs_nid2str(imp->imp_connection->c_peer.nid),
+		       lustre_msg_get_opc(req->rq_reqmsg));
 
 		spin_lock(&imp->imp_lock);
 		/* Request already may be not on sending or delaying list. This
@@ -2215,10 +2209,8 @@ EXPORT_SYMBOL(ptlrpc_set_wait);
  */
 static void __ptlrpc_free_req(struct ptlrpc_request *request, int locked)
 {
-	if (request == NULL) {
+	if (request == NULL)
 		return;
-	}
-
 	LASSERTF(!request->rq_receiving_reply, "req %p\n", request);
 	LASSERTF(request->rq_rqbd == NULL, "req %p\n", request);/* client-side */
 	LASSERTF(list_empty(&request->rq_list), "req %p\n", request);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/events.c b/drivers/staging/lustre/lustre/ptlrpc/events.c
index 32dfffa76a5e..7f8644e01112 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/events.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/events.c
@@ -128,8 +128,8 @@ void reply_in_callback(lnet_event_t *ev)
 	    ((lustre_msghdr_get_flags(req->rq_reqmsg) & MSGHDR_AT_SUPPORT))) {
 		/* Early reply */
 		DEBUG_REQ(D_ADAPTTO, req,
-			  "Early reply received: mlen=%u offset=%d replen=%d "
-			  "replied=%d unlinked=%d", ev->mlength, ev->offset,
+			  "Early reply received: mlen=%u offset=%d replen=%d replied=%d unlinked=%d",
+			  ev->mlength, ev->offset,
 			  req->rq_replen, req->rq_replied, ev->unlinked);
 
 		req->rq_early_count++; /* number received, client side */
@@ -313,8 +313,7 @@ void request_in_callback(lnet_event_t *ev)
 		}
 		req = ptlrpc_request_cache_alloc(GFP_ATOMIC);
 		if (req == NULL) {
-			CERROR("Can't allocate incoming request descriptor: "
-			       "Dropping %s RPC from %s\n",
+			CERROR("Can't allocate incoming request descriptor: Dropping %s RPC from %s\n",
 			       service->srv_name,
 			       libcfs_id2str(ev->initiator));
 			return;
diff --git a/drivers/staging/lustre/lustre/ptlrpc/import.c b/drivers/staging/lustre/lustre/ptlrpc/import.c
index 2e7e7171ca63..4ceb90db02a3 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/import.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/import.c
@@ -157,18 +157,14 @@ int ptlrpc_set_import_discon(struct obd_import *imp, __u32 conn_cnt)
 			  &target_start, &target_len);
 
 		if (imp->imp_replayable) {
-			LCONSOLE_WARN("%s: Connection to %.*s (at %s) was "
-			       "lost; in progress operations using this "
-			       "service will wait for recovery to complete\n",
-			       imp->imp_obd->obd_name, target_len, target_start,
-			       libcfs_nid2str(imp->imp_connection->c_peer.nid));
+			LCONSOLE_WARN("%s: Connection to %.*s (at %s) was lost; in progress operations using this service will wait for recovery to complete\n",
+				      imp->imp_obd->obd_name, target_len, target_start,
+				      libcfs_nid2str(imp->imp_connection->c_peer.nid));
 		} else {
-			LCONSOLE_ERROR_MSG(0x166, "%s: Connection to "
-			       "%.*s (at %s) was lost; in progress "
-			       "operations using this service will fail\n",
-			       imp->imp_obd->obd_name,
-			       target_len, target_start,
-			       libcfs_nid2str(imp->imp_connection->c_peer.nid));
+			LCONSOLE_ERROR_MSG(0x166, "%s: Connection to %.*s (at %s) was lost; in progress operations using this service will fail\n",
+					   imp->imp_obd->obd_name,
+					   target_len, target_start,
+					   libcfs_nid2str(imp->imp_connection->c_peer.nid));
 		}
 		IMPORT_SET_STATE_NOLOCK(imp, LUSTRE_IMP_DISCON);
 		spin_unlock(&imp->imp_lock);
@@ -328,8 +324,8 @@ void ptlrpc_invalidate_import(struct obd_import *imp)
 				 * sluggish nets). Let's check this. If there
 				 * is no inflight and unregistering != 0, this
 				 * is bug. */
-				LASSERTF(count == 0, "Some RPCs are still "
-					 "unregistering: %d\n", count);
+				LASSERTF(count == 0, "Some RPCs are still unregistering: %d\n",
+					 count);
 
 				/* Let's save one loop as soon as inflight have
 				 * dropped to zero. No new inflights possible at
@@ -353,12 +349,11 @@ void ptlrpc_invalidate_import(struct obd_import *imp)
 						  "still on delayed list");
 				}
 
-				CERROR("%s: RPCs in \"%s\" phase found (%d). "
-				       "Network is sluggish? Waiting them "
-				       "to error out.\n", cli_tgt,
+				CERROR("%s: RPCs in \"%s\" phase found (%d). Network is sluggish? Waiting them to error out.\n",
+				       cli_tgt,
 				       ptlrpc_phase2str(RQ_PHASE_UNREGISTERING),
 				       atomic_read(&imp->
-						       imp_unregistering));
+						   imp_unregistering));
 			}
 			spin_unlock(&imp->imp_lock);
 		  }
@@ -413,8 +408,7 @@ void ptlrpc_fail_import(struct obd_import *imp, __u32 conn_cnt)
 
 	if (ptlrpc_set_import_discon(imp, conn_cnt)) {
 		if (!imp->imp_replayable) {
-			CDEBUG(D_HA, "import %s@%s for %s not replayable, "
-			       "auto-deactivating\n",
+			CDEBUG(D_HA, "import %s@%s for %s not replayable, auto-deactivating\n",
 			       obd2cli_tgt(imp->imp_obd),
 			       imp->imp_connection->c_remote_uuid.uuid,
 			       imp->imp_obd->obd_name);
@@ -541,8 +535,8 @@ static int import_select_connection(struct obd_import *imp)
 				at_reset(at, CONNECTION_SWITCH_MAX);
 		}
 		LASSERT(imp_conn->oic_last_attempt);
-		CDEBUG(D_HA, "%s: tried all connections, increasing latency "
-			"to %ds\n", imp->imp_obd->obd_name, at_get(at));
+		CDEBUG(D_HA, "%s: tried all connections, increasing latency to %ds\n",
+		       imp->imp_obd->obd_name, at_get(at));
 	}
 
 	imp_conn->oic_last_attempt = cfs_time_current_64();
@@ -564,8 +558,7 @@ static int import_select_connection(struct obd_import *imp)
 			deuuidify(obd2cli_tgt(imp->imp_obd), NULL,
 				  &target_start, &target_len);
 
-			CDEBUG(D_HA, "%s: Connection changing to"
-			       " %.*s (at %s)\n",
+			CDEBUG(D_HA, "%s: Connection changing to %.*s (at %s)\n",
 			       imp->imp_obd->obd_name,
 			       target_len, target_start,
 			       libcfs_nid2str(imp_conn->oic_conn->c_peer.nid));
@@ -935,14 +928,13 @@ static int ptlrpc_connect_interpret(const struct lu_env *env,
 				       lustre_msg_get_handle(
 				       request->rq_repmsg)->cookie);
 			} else {
-				LCONSOLE_WARN("Evicted from %s (at %s) "
-					      "after server handle changed from %#llx to %#llx\n",
+				LCONSOLE_WARN("Evicted from %s (at %s) after server handle changed from %#llx to %#llx\n",
 					      obd2cli_tgt(imp->imp_obd),
 					      imp->imp_connection-> \
 					      c_remote_uuid.uuid,
 					      imp->imp_remote_handle.cookie,
 					      lustre_msg_get_handle(
-					      request->rq_repmsg)->cookie);
+						      request->rq_repmsg)->cookie);
 			}
 
 
@@ -962,8 +954,8 @@ static int ptlrpc_connect_interpret(const struct lu_env *env,
 		}
 
 		if (imp->imp_invalid) {
-			CDEBUG(D_HA, "%s: reconnected but import is invalid; "
-			       "marking evicted\n", imp->imp_obd->obd_name);
+			CDEBUG(D_HA, "%s: reconnected but import is invalid; marking evicted\n",
+			       imp->imp_obd->obd_name);
 			IMPORT_SET_STATE(imp, LUSTRE_IMP_EVICTED);
 		} else if (MSG_CONNECT_RECOVERING & msg_flags) {
 			CDEBUG(D_HA, "%s: reconnected to %s during replay\n",
@@ -985,8 +977,8 @@ static int ptlrpc_connect_interpret(const struct lu_env *env,
 		imp->imp_last_replay_transno = 0;
 		IMPORT_SET_STATE(imp, LUSTRE_IMP_REPLAY);
 	} else {
-		DEBUG_REQ(D_HA, request, "%s: evicting (reconnect/recover flags"
-			  " not set: %x)", imp->imp_obd->obd_name, msg_flags);
+		DEBUG_REQ(D_HA, request, "%s: evicting (reconnect/recover flags not set: %x)",
+			  imp->imp_obd->obd_name, msg_flags);
 		imp->imp_remote_handle =
 				*lustre_msg_get_handle(request->rq_repmsg);
 		IMPORT_SET_STATE(imp, LUSTRE_IMP_EVICTED);
@@ -994,17 +986,13 @@ static int ptlrpc_connect_interpret(const struct lu_env *env,
 
 	/* Sanity checks for a reconnected import. */
 	if (!(imp->imp_replayable) != !(msg_flags & MSG_CONNECT_REPLAYABLE)) {
-		CERROR("imp_replayable flag does not match server "
-		       "after reconnect. We should LBUG right here.\n");
+		CERROR("imp_replayable flag does not match server after reconnect. We should LBUG right here.\n");
 	}
 
 	if (lustre_msg_get_last_committed(request->rq_repmsg) > 0 &&
 	    lustre_msg_get_last_committed(request->rq_repmsg) <
 	    aa->pcaa_peer_committed) {
-		CERROR("%s went back in time (transno %lld"
-		       " was previously committed, server now claims %lld"
-		       ")!  See https://bugzilla.lustre.org/show_bug.cgi?"
-		       "id=9646\n",
+		CERROR("%s went back in time (transno %lld was previously committed, server now claims %lld)!  See https://bugzilla.lustre.org/show_bug.cgi?id=9646\n",
 		       obd2cli_tgt(imp->imp_obd), aa->pcaa_peer_committed,
 		       lustre_msg_get_last_committed(request->rq_repmsg));
 	}
@@ -1013,8 +1001,7 @@ finish:
 	rc = ptlrpc_import_recovery_state_machine(imp);
 	if (rc != 0) {
 		if (rc == -ENOTCONN) {
-			CDEBUG(D_HA, "evicted/aborted by %s@%s during recovery;"
-			       "invalidating and reconnecting\n",
+			CDEBUG(D_HA, "evicted/aborted by %s@%s during recovery; invalidating and reconnecting\n",
 			       obd2cli_tgt(imp->imp_obd),
 			       imp->imp_connection->c_remote_uuid.uuid);
 			ptlrpc_connect_import(imp);
@@ -1034,9 +1021,7 @@ finish:
 
 		if ((imp->imp_connect_flags_orig & OBD_CONNECT_IBITS) &&
 		    !(ocd->ocd_connect_flags & OBD_CONNECT_IBITS)) {
-			LCONSOLE_WARN("%s: MDS %s does not support ibits "
-				      "lock, either very old or invalid: "
-				      "requested %llx, replied %llx\n",
+			LCONSOLE_WARN("%s: MDS %s does not support ibits lock, either very old or invalid: requested %llx, replied %llx\n",
 				      imp->imp_obd->obd_name,
 				      imp->imp_connection->c_remote_uuid.uuid,
 				      imp->imp_connect_flags_orig,
@@ -1052,13 +1037,12 @@ finish:
 					LUSTRE_VERSION_OFFSET_WARN)) {
 			/* Sigh, some compilers do not like #ifdef in the middle
 			   of macro arguments */
-			const char *older = "older. Consider upgrading server "
-					    "or downgrading client";
-			const char *newer = "newer than client version. "
-					    "Consider upgrading client";
+			const char *older = "older. Consider upgrading server or downgrading client"
+				;
+			const char *newer = "newer than client version. Consider upgrading client"
+					    ;
 
-			LCONSOLE_WARN("Server %s version (%d.%d.%d.%d) "
-				      "is much %s (%s)\n",
+			LCONSOLE_WARN("Server %s version (%d.%d.%d.%d) is much %s (%s)\n",
 				      obd2cli_tgt(imp->imp_obd),
 				      OBD_OCD_VERSION_MAJOR(ocd->ocd_version),
 				      OBD_OCD_VERSION_MINOR(ocd->ocd_version),
@@ -1095,10 +1079,7 @@ finish:
 			 * the checksum types it doesn't support */
 			if ((ocd->ocd_cksum_types &
 			     cksum_types_supported_client()) == 0) {
-				LCONSOLE_WARN("The negotiation of the checksum "
-					      "algorithm to use with server %s "
-					      "failed (%x/%x), disabling "
-					      "checksums\n",
+				LCONSOLE_WARN("The negotiation of the checksum algorithm to use with server %s failed (%x/%x), disabling checksums\n",
 					      obd2cli_tgt(imp->imp_obd),
 					      ocd->ocd_cksum_types,
 					      cksum_types_supported_client());
@@ -1191,17 +1172,13 @@ out:
 				 * connection from liblustre clients, so we
 				 * should never see this from VFS context
 				 */
-				LCONSOLE_ERROR_MSG(0x16a, "Server %s version "
-					"(%d.%d.%d.%d)"
-					" refused connection from this client "
-					"with an incompatible version (%s).  "
-					"Client must be recompiled\n",
-					obd2cli_tgt(imp->imp_obd),
-					OBD_OCD_VERSION_MAJOR(ocd->ocd_version),
-					OBD_OCD_VERSION_MINOR(ocd->ocd_version),
-					OBD_OCD_VERSION_PATCH(ocd->ocd_version),
-					OBD_OCD_VERSION_FIX(ocd->ocd_version),
-					LUSTRE_VERSION_STRING);
+				LCONSOLE_ERROR_MSG(0x16a, "Server %s version (%d.%d.%d.%d) refused connection from this client with an incompatible version (%s).  Client must be recompiled\n",
+						   obd2cli_tgt(imp->imp_obd),
+						   OBD_OCD_VERSION_MAJOR(ocd->ocd_version),
+						   OBD_OCD_VERSION_MINOR(ocd->ocd_version),
+						   OBD_OCD_VERSION_PATCH(ocd->ocd_version),
+						   OBD_OCD_VERSION_FIX(ocd->ocd_version),
+						   LUSTRE_VERSION_STRING);
 				ptlrpc_deactivate_import(imp);
 				IMPORT_SET_STATE(imp, LUSTRE_IMP_CLOSED);
 			}
@@ -1237,8 +1214,7 @@ static int completed_replay_interpret(const struct lu_env *env,
 			       "%s: version recovery fails, reconnecting\n",
 			       req->rq_import->imp_obd->obd_name);
 		} else {
-			CDEBUG(D_HA, "%s: LAST_REPLAY message error: %d, "
-				     "reconnecting\n",
+			CDEBUG(D_HA, "%s: LAST_REPLAY message error: %d, reconnecting\n",
 			       req->rq_import->imp_obd->obd_name,
 			       req->rq_status);
 		}
@@ -1343,9 +1319,7 @@ int ptlrpc_import_recovery_state_machine(struct obd_import *imp)
 		/* Don't care about MGC eviction */
 		if (strcmp(imp->imp_obd->obd_type->typ_name,
 			   LUSTRE_MGC_NAME) != 0) {
-			LCONSOLE_ERROR_MSG(0x167, "%s: This client was evicted "
-					   "by %.*s; in progress operations "
-					   "using this service will fail.\n",
+			LCONSOLE_ERROR_MSG(0x167, "%s: This client was evicted by %.*s; in progress operations using this service will fail.\n",
 					   imp->imp_obd->obd_name, target_len,
 					   target_start);
 		}
@@ -1455,8 +1429,7 @@ int ptlrpc_disconnect_import(struct obd_import *imp, int noclose)
 		break;
 	default:
 		rc = -EINVAL;
-		CERROR("%s: don't know how to disconnect from %s "
-		       "(connect_op %d): rc = %d\n",
+		CERROR("%s: don't know how to disconnect from %s (connect_op %d): rc = %d\n",
 		       imp->imp_obd->obd_name, obd2cli_tgt(imp->imp_obd),
 		       imp->imp_connect_op, rc);
 		return rc;
@@ -1607,8 +1580,8 @@ int at_measured(struct adaptive_timeout *at, unsigned int val)
 	at->at_current =  max(at->at_current, at_min);
 
 	if (at->at_current != old)
-		CDEBUG(D_OTHER, "AT %p change: old=%u new=%u delta=%d "
-		       "(val=%u) hist %u %u %u %u\n", at,
+		CDEBUG(D_OTHER, "AT %p change: old=%u new=%u delta=%d (val=%u) hist %u %u %u %u\n",
+		       at,
 		       old, at->at_current, at->at_current - old, val,
 		       at->at_hist[0], at->at_hist[1], at->at_hist[2],
 		       at->at_hist[3]);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/layout.c b/drivers/staging/lustre/lustre/ptlrpc/layout.c
index 5b8337187b59..dc5ceb55d001 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/layout.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/layout.c
@@ -980,18 +980,7 @@ EXPORT_SYMBOL(RMF_CONN);
 struct req_msg_field RMF_CONNECT_DATA =
 	DEFINE_MSGF("cdata",
 		    RMF_F_NO_SIZE_CHECK /* we allow extra space for interop */,
-#if LUSTRE_VERSION_CODE > OBD_OCD_VERSION(2, 7, 50, 0)
 		    sizeof(struct obd_connect_data),
-#else
-/* For interoperability with 1.8 and 2.0 clients/servers.
- * The RPC verification code allows larger RPC buffers, but not
- * smaller buffers.  Until we no longer need to keep compatibility
- * with older servers/clients we can only check that the buffer
- * size is at least as large as obd_connect_data_v1.  That is not
- * not in itself harmful, since the chance of just corrupting this
- * field is low.  See JIRA LU-16 for details. */
-		    sizeof(struct obd_connect_data_v1),
-#endif
 		    lustre_swab_connect, NULL);
 EXPORT_SYMBOL(RMF_CONNECT_DATA);
 
@@ -1897,8 +1886,8 @@ swabber_dumper_helper(struct req_capsule *pill,
 		swabber(value);
 		ptlrpc_buf_set_swabbed(pill->rc_req, inout, offset);
 		if (dump) {
-			CDEBUG(D_RPCTRACE, "Dump of swabbed field %s "
-			       "follows\n", field->rmf_name);
+			CDEBUG(D_RPCTRACE, "Dump of swabbed field %s follows\n",
+			       field->rmf_name);
 			field->rmf_dumper(value);
 		}
 
@@ -1914,8 +1903,7 @@ swabber_dumper_helper(struct req_capsule *pill,
 	     i < n;
 	     i++, p += field->rmf_size) {
 		if (dump) {
-			CDEBUG(D_RPCTRACE, "Dump of %sarray field %s, "
-			       "element %d follows\n",
+			CDEBUG(D_RPCTRACE, "Dump of %sarray field %s, element %d follows\n",
 			       do_swab ? "unswabbed " : "", field->rmf_name, i);
 			field->rmf_dumper(p);
 		}
@@ -1923,8 +1911,8 @@ swabber_dumper_helper(struct req_capsule *pill,
 			continue;
 		swabber(p);
 		if (dump) {
-			CDEBUG(D_RPCTRACE, "Dump of swabbed array field %s, "
-			       "element %d follows\n", field->rmf_name, i);
+			CDEBUG(D_RPCTRACE, "Dump of swabbed array field %s, element %d follows\n",
+			       field->rmf_name, i);
 			field->rmf_dumper(value);
 		}
 	}
@@ -1983,8 +1971,7 @@ static void *__req_capsule_get(struct req_capsule *pill,
 		 */
 		len = lustre_msg_buflen(msg, offset);
 		if ((len % field->rmf_size) != 0) {
-			CERROR("%s: array field size mismatch "
-			       "%d modulo %d != 0 (%d)\n",
+			CERROR("%s: array field size mismatch %d modulo %d != 0 (%d)\n",
 			       field->rmf_name, len, field->rmf_size, loc);
 			return NULL;
 		}
@@ -1997,8 +1984,7 @@ static void *__req_capsule_get(struct req_capsule *pill,
 
 	if (value == NULL) {
 		DEBUG_REQ(D_ERROR, pill->rc_req,
-			  "Wrong buffer for field `%s' (%d of %d) "
-			  "in format `%s': %d vs. %d (%s)\n",
+			  "Wrong buffer for field `%s' (%d of %d) in format `%s': %d vs. %d (%s)\n",
 			  field->rmf_name, offset, lustre_msg_bufcount(msg),
 			  fmt->rf_name, lustre_msg_buflen(msg, offset), len,
 			  rcl_names[loc]);
@@ -2013,7 +1999,7 @@ static void *__req_capsule_get(struct req_capsule *pill,
 /**
  * Dump a request and/or reply
  */
-void __req_capsule_dump(struct req_capsule *pill, enum req_location loc)
+static void __req_capsule_dump(struct req_capsule *pill, enum req_location loc)
 {
 	const struct    req_format *fmt;
 	const struct    req_msg_field *field;
@@ -2031,8 +2017,8 @@ void __req_capsule_dump(struct req_capsule *pill, enum req_location loc)
 			 * have a specific dumper
 			 */
 			len = req_capsule_get_size(pill, field, loc);
-			CDEBUG(D_RPCTRACE, "Field %s has no dumper function;"
-			       "field size is %d\n", field->rmf_name, len);
+			CDEBUG(D_RPCTRACE, "Field %s has no dumper function; field size is %d\n",
+			       field->rmf_name, len);
 		} else {
 			/* It's the dumping side-effect that we're interested in */
 			(void) __req_capsule_get(pill, field, loc, NULL, 1);
@@ -2184,8 +2170,7 @@ void req_capsule_set_size(struct req_capsule *pill,
 	    (size > 0)) {
 		if ((field->rmf_flags & RMF_F_STRUCT_ARRAY) &&
 		    (size % field->rmf_size != 0)) {
-			CERROR("%s: array field size mismatch "
-			       "%d %% %d != 0 (%d)\n",
+			CERROR("%s: array field size mismatch %d %% %d != 0 (%d)\n",
 			       field->rmf_name, size, field->rmf_size, loc);
 			LBUG();
 		} else if (!(field->rmf_flags & RMF_F_STRUCT_ARRAY) &&
diff --git a/drivers/staging/lustre/lustre/ptlrpc/llog_client.c b/drivers/staging/lustre/lustre/ptlrpc/llog_client.c
index 56f825fbb17c..e9baf5bbee3a 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/llog_client.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/llog_client.c
@@ -334,8 +334,7 @@ static int llog_client_read_header(const struct lu_env *env,
 		       llh_hdr->lrh_type, LLOG_HDR_MAGIC);
 		rc = -EIO;
 	} else if (llh_hdr->lrh_len != LLOG_CHUNK_SIZE) {
-		CERROR("incorrectly sized log header: %#x "
-		       "(expecting %#x)\n",
+		CERROR("incorrectly sized log header: %#x (expecting %#x)\n",
 		       llh_hdr->lrh_len, LLOG_CHUNK_SIZE);
 		CERROR("you may need to re-run lconf --write_conf.\n");
 		rc = -EIO;
diff --git a/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c b/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
index 87b9764a4f19..4011e0050fcb 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
@@ -175,7 +175,7 @@ const char *ll_opcode2str(__u32 opcode)
 	return ll_rpc_opcode_table[offset].opname;
 }
 
-const char* ll_eopcode2str(__u32 opcode)
+const char *ll_eopcode2str(__u32 opcode)
 {
 	LASSERT(ll_eopcode_table[opcode].opcode == opcode);
 	return ll_eopcode_table[opcode].opname;
@@ -694,8 +694,7 @@ default_queue:
 	if (queue == PTLRPC_NRS_QUEUE_HP && !nrs_svc_has_hp(svc)) {
 		rc = -ENODEV;
 		goto out;
-	}
-	else if (queue == PTLRPC_NRS_QUEUE_BOTH && !nrs_svc_has_hp(svc))
+	} else if (queue == PTLRPC_NRS_QUEUE_BOTH && !nrs_svc_has_hp(svc))
 		queue = PTLRPC_NRS_QUEUE_REG;
 
 	/**
@@ -746,8 +745,7 @@ ptlrpc_lprocfs_svc_req_history_seek(struct ptlrpc_service_part *svcpt,
 			 svcpt->scp_service->srv_name, svcpt->scp_cpt,
 			 srhi->srhi_seq, srhi->srhi_req->rq_history_seq);
 		LASSERTF(!list_empty(&svcpt->scp_hist_reqs),
-			 "%s:%d: seek offset %llu, request seq %llu, "
-			 "last culled %llu\n",
+			 "%s:%d: seek offset %llu, request seq %llu, last culled %llu\n",
 			 svcpt->scp_service->srv_name, svcpt->scp_cpt,
 			 seq, srhi->srhi_seq, svcpt->scp_hist_seq_culled);
 		e = &srhi->srhi_req->rq_history_list;
@@ -814,8 +812,8 @@ ptlrpc_lprocfs_svc_req_history_start(struct seq_file *s, loff_t *pos)
 	int				i;
 
 	if (sizeof(loff_t) != sizeof(__u64)) { /* can't support */
-		CWARN("Failed to read request history because size of loff_t "
-		      "%d can't match size of u64\n", (int)sizeof(loff_t));
+		CWARN("Failed to read request history because size of loff_t %d can't match size of u64\n",
+		      (int)sizeof(loff_t));
 		return NULL;
 	}
 
@@ -1298,14 +1296,12 @@ int lprocfs_wr_import(struct file *file, const char *buffer,
 		if (*endptr) {
 			CERROR("config: wrong instance # %s\n", ptr);
 		} else if (inst != imp->imp_connect_data.ocd_instance) {
-			CDEBUG(D_INFO, "IR: %s is connecting to an obsoleted "
-			       "target(%u/%u), reconnecting...\n",
+			CDEBUG(D_INFO, "IR: %s is connecting to an obsoleted target(%u/%u), reconnecting...\n",
 			       imp->imp_obd->obd_name,
 			       imp->imp_connect_data.ocd_instance, inst);
 			do_reconn = 1;
 		} else {
-			CDEBUG(D_INFO, "IR: %s has already been connecting to "
-			       "new target(%u)\n",
+			CDEBUG(D_INFO, "IR: %s has already been connecting to new target(%u)\n",
 			       imp->imp_obd->obd_name, inst);
 		}
 	}
diff --git a/drivers/staging/lustre/lustre/ptlrpc/niobuf.c b/drivers/staging/lustre/lustre/ptlrpc/niobuf.c
index c1e8aa4d5ec4..f715e9a8b996 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/niobuf.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/niobuf.c
@@ -224,8 +224,8 @@ int ptlrpc_register_bulk(struct ptlrpc_request *req)
 		      total_md - desc->bd_md_count);
 	spin_unlock(&desc->bd_lock);
 
-	CDEBUG(D_NET, "Setup %u bulk %s buffers: %u pages %u bytes, "
-	       "xid x%#llx-%#llx, portal %u\n", desc->bd_md_count,
+	CDEBUG(D_NET, "Setup %u bulk %s buffers: %u pages %u bytes, xid x%#llx-%#llx, portal %u\n",
+	       desc->bd_md_count,
 	       desc->bd_type == BULK_GET_SOURCE ? "get-source" : "put-sink",
 	       desc->bd_iov_count, desc->bd_nob,
 	       desc->bd_last_xid, req->rq_xid, desc->bd_portal);
@@ -337,8 +337,7 @@ static void ptlrpc_at_set_reply(struct ptlrpc_request *req, int flags)
 
 	if (req->rq_reqmsg &&
 	    !(lustre_msghdr_get_flags(req->rq_reqmsg) & MSGHDR_AT_SUPPORT)) {
-		CDEBUG(D_ADAPTTO, "No early reply support: flags=%#x "
-		       "req_flags=%#x magic=%d:%x/%x len=%d\n",
+		CDEBUG(D_ADAPTTO, "No early reply support: flags=%#x req_flags=%#x magic=%d:%x/%x len=%d\n",
 		       flags, lustre_msg_get_flags(req->rq_reqmsg),
 		       lustre_msg_is_v1(req->rq_reqmsg),
 		       lustre_msg_get_magic(req->rq_reqmsg),
diff --git a/drivers/staging/lustre/lustre/ptlrpc/nrs.c b/drivers/staging/lustre/lustre/ptlrpc/nrs.c
index 181301bd2083..d5fd7215c72f 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/nrs.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/nrs.c
@@ -770,8 +770,8 @@ static int nrs_policy_register(struct ptlrpc_nrs *nrs,
 
 	tmp = nrs_policy_find_locked(nrs, policy->pol_desc->pd_name);
 	if (tmp != NULL) {
-		CERROR("NRS policy %s has been registered, can't register it "
-		       "for %s\n", policy->pol_desc->pd_name,
+		CERROR("NRS policy %s has been registered, can't register it for %s\n",
+		       policy->pol_desc->pd_name,
 		       svcpt->scp_service->srv_name);
 		nrs_policy_put_locked(tmp);
 
@@ -882,8 +882,7 @@ static int nrs_register_policies_locked(struct ptlrpc_nrs *nrs)
 		if (nrs_policy_compatible(svc, desc)) {
 			rc = nrs_policy_register(nrs, desc);
 			if (rc != 0) {
-				CERROR("Failed to register NRS policy %s for "
-				       "partition %d of service %s: %d\n",
+				CERROR("Failed to register NRS policy %s for partition %d of service %s: %d\n",
 				       desc->pd_name, svcpt->scp_cpt,
 				       svc->srv_name, rc);
 				/**
@@ -1082,8 +1081,7 @@ again:
 			if (rc == -ENOENT) {
 				rc = 0;
 			} else if (rc != 0) {
-				CERROR("Failed to unregister NRS policy %s for "
-				       "partition %d of service %s: %d\n",
+				CERROR("Failed to unregister NRS policy %s for partition %d of service %s: %d\n",
 				       desc->pd_name, svcpt->scp_cpt,
 				       svcpt->scp_service->srv_name, rc);
 				return rc;
@@ -1145,18 +1143,15 @@ int ptlrpc_nrs_policy_register(struct ptlrpc_nrs_pol_conf *conf)
 	if ((conf->nc_flags & PTLRPC_NRS_FL_REG_EXTERN) &&
 	    (conf->nc_flags & (PTLRPC_NRS_FL_FALLBACK |
 			       PTLRPC_NRS_FL_REG_START))) {
-		CERROR("NRS: failing to register policy %s. Please check "
-		       "policy flags; external policies cannot act as fallback "
-		       "policies, or be started immediately upon registration "
-		       "without interaction with lprocfs\n", conf->nc_name);
+		CERROR("NRS: failing to register policy %s. Please check policy flags; external policies cannot act as fallback policies, or be started immediately upon registration without interaction with lprocfs\n",
+		       conf->nc_name);
 		return -EINVAL;
 	}
 
 	mutex_lock(&nrs_core.nrs_mutex);
 
 	if (nrs_policy_find_desc_locked(conf->nc_name) != NULL) {
-		CERROR("NRS: failing to register policy %s which has already "
-		       "been registered with NRS core!\n",
+		CERROR("NRS: failing to register policy %s which has already been registered with NRS core!\n",
 		       conf->nc_name);
 		rc = -EEXIST;
 		goto fail;
@@ -1209,8 +1204,7 @@ again:
 			nrs = nrs_svcpt2nrs(svcpt, hp);
 			rc = nrs_policy_register(nrs, desc);
 			if (rc != 0) {
-				CERROR("Failed to register NRS policy %s for "
-				       "partition %d of service %s: %d\n",
+				CERROR("Failed to register NRS policy %s for partition %d of service %s: %d\n",
 				       desc->pd_name, svcpt->scp_cpt,
 				       svcpt->scp_service->srv_name, rc);
 
@@ -1281,8 +1275,7 @@ int ptlrpc_nrs_policy_unregister(struct ptlrpc_nrs_pol_conf *conf)
 	LASSERT(conf != NULL);
 
 	if (conf->nc_flags & PTLRPC_NRS_FL_FALLBACK) {
-		CERROR("Unable to unregister a fallback policy, unless the "
-		       "PTLRPC service is stopping.\n");
+		CERROR("Unable to unregister a fallback policy, unless the PTLRPC service is stopping.\n");
 		return -EPERM;
 	}
 
@@ -1292,8 +1285,7 @@ int ptlrpc_nrs_policy_unregister(struct ptlrpc_nrs_pol_conf *conf)
 
 	desc = nrs_policy_find_desc_locked(conf->nc_name);
 	if (desc == NULL) {
-		CERROR("Failing to unregister NRS policy %s which has "
-		       "not been registered with NRS core!\n",
+		CERROR("Failing to unregister NRS policy %s which has not been registered with NRS core!\n",
 		       conf->nc_name);
 		rc = -ENOENT;
 		goto not_exist;
@@ -1304,9 +1296,8 @@ int ptlrpc_nrs_policy_unregister(struct ptlrpc_nrs_pol_conf *conf)
 	rc = nrs_policy_unregister_locked(desc);
 	if (rc < 0) {
 		if (rc == -EBUSY)
-			CERROR("Please first stop policy %s on all service "
-			       "partitions and then retry to unregister the "
-			       "policy.\n", conf->nc_name);
+			CERROR("Please first stop policy %s on all service partitions and then retry to unregister the policy.\n",
+			       conf->nc_name);
 		goto fail;
 	}
 
diff --git a/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c b/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
index 50556db15f79..2f45f7657830 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
@@ -412,8 +412,8 @@ void *lustre_msg_buf_v2(struct lustre_msg_v2 *m, int n, int min_size)
 
 	buflen = m->lm_buflens[n];
 	if (unlikely(buflen < min_size)) {
-		CERROR("msg %p buffer[%d] size %d too small "
-		       "(required %d, opc=%d)\n", m, n, buflen, min_size,
+		CERROR("msg %p buffer[%d] size %d too small (required %d, opc=%d)\n",
+		       m, n, buflen, min_size,
 		       n == MSG_PTLRPC_BODY_OFF ? -1 : lustre_msg_get_opc(m));
 		return NULL;
 	}
@@ -749,21 +749,19 @@ char *lustre_msg_string(struct lustre_msg *m, int index, int max_len)
 	slen = strnlen(str, blen);
 
 	if (slen == blen) {		     /* not NULL terminated */
-		CERROR("can't unpack non-NULL terminated string in "
-			"msg %p buffer[%d] len %d\n", m, index, blen);
+		CERROR("can't unpack non-NULL terminated string in msg %p buffer[%d] len %d\n",
+		       m, index, blen);
 		return NULL;
 	}
 
 	if (max_len == 0) {
 		if (slen != blen - 1) {
-			CERROR("can't unpack short string in msg %p "
-			       "buffer[%d] len %d: strlen %d\n",
+			CERROR("can't unpack short string in msg %p buffer[%d] len %d: strlen %d\n",
 			       m, index, blen, slen);
 			return NULL;
 		}
 	} else if (slen > max_len) {
-		CERROR("can't unpack oversized string in msg %p "
-		       "buffer[%d] len %d strlen %d: max %d expected\n",
+		CERROR("can't unpack oversized string in msg %p buffer[%d] len %d strlen %d: max %d expected\n",
 		       m, index, blen, slen, max_len);
 		return NULL;
 	}
@@ -1313,43 +1311,17 @@ __u32 lustre_msg_get_cksum(struct lustre_msg *msg)
 	}
 }
 
-#if LUSTRE_VERSION_CODE < OBD_OCD_VERSION(2, 7, 50, 0)
-/*
- * In 1.6 and 1.8 the checksum was computed only on struct ptlrpc_body as
- * it was in 1.6 (88 bytes, smaller than the full size in 1.8).  It makes
- * more sense to compute the checksum on the full ptlrpc_body, regardless
- * of what size it is, but in order to keep interoperability with 1.8 we
- * can optionally also checksum only the first 88 bytes (caller decides). */
-# define ptlrpc_body_cksum_size_compat18	 88
-
-__u32 lustre_msg_calc_cksum(struct lustre_msg *msg, int compat18)
-#else
-# warning "remove checksum compatibility support for b1_8"
 __u32 lustre_msg_calc_cksum(struct lustre_msg *msg)
-#endif
 {
 	switch (msg->lm_magic) {
 	case LUSTRE_MSG_MAGIC_V2: {
 		struct ptlrpc_body *pb = lustre_msg_ptlrpc_body(msg);
-#if LUSTRE_VERSION_CODE < OBD_OCD_VERSION(2, 7, 50, 0)
-		__u32 crc;
-		unsigned int hsize = 4;
-		__u32 len = compat18 ? ptlrpc_body_cksum_size_compat18 :
-			    lustre_msg_buflen(msg, MSG_PTLRPC_BODY_OFF);
-		LASSERTF(pb, "invalid msg %p: no ptlrpc body!\n", msg);
-		cfs_crypto_hash_digest(CFS_HASH_ALG_CRC32, (unsigned char *)pb,
-				       len, NULL, 0, (unsigned char *)&crc,
-				       &hsize);
-		return crc;
-#else
-# warning "remove checksum compatibility support for b1_8"
 		__u32 crc;
 		unsigned int hsize = 4;
 		cfs_crypto_hash_digest(CFS_HASH_ALG_CRC32, (unsigned char *)pb,
 				   lustre_msg_buflen(msg, MSG_PTLRPC_BODY_OFF),
 				   NULL, 0, (unsigned char *)&crc, &hsize);
 		return crc;
-#endif
 	}
 	default:
 		CERROR("incorrect message magic: %08x\n", msg->lm_magic);
@@ -2284,8 +2256,8 @@ void lustre_swab_quota_body(struct quota_body *b)
 void dump_ioo(struct obd_ioobj *ioo)
 {
 	CDEBUG(D_RPCTRACE,
-	       "obd_ioobj: ioo_oid="DOSTID", ioo_max_brw=%#x, "
-	       "ioo_bufct=%d\n", POSTID(&ioo->ioo_oid), ioo->ioo_max_brw,
+	       "obd_ioobj: ioo_oid=" DOSTID ", ioo_max_brw=%#x, ioo_bufct=%d\n",
+	       POSTID(&ioo->ioo_oid), ioo->ioo_max_brw,
 	       ioo->ioo_bufcnt);
 }
 EXPORT_SYMBOL(dump_ioo);
@@ -2356,8 +2328,7 @@ void dump_obdo(struct obdo *oa)
 		CDEBUG(D_RPCTRACE, "obdo: o_handle = %lld\n",
 		       oa->o_handle.cookie);
 	if (valid & OBD_MD_FLCOOKIE)
-		CDEBUG(D_RPCTRACE, "obdo: o_lcookie = "
-		       "(llog_cookie dumping not yet implemented)\n");
+		CDEBUG(D_RPCTRACE, "obdo: o_lcookie = (llog_cookie dumping not yet implemented)\n");
 }
 EXPORT_SYMBOL(dump_obdo);
 
@@ -2421,17 +2392,15 @@ void _debug_req(struct ptlrpc_request *req,
 
 	va_start(args, fmt);
 	libcfs_debug_vmsg2(msgdata, fmt, args,
-			   " req@%p x%llu/t%lld(%lld) o%d->%s@%s:%d/%d"
-			   " lens %d/%d e %d to %d dl "CFS_TIME_T" ref %d "
-			   "fl "REQ_FLAGS_FMT"/%x/%x rc %d/%d\n",
+			   " req@%p x%llu/t%lld(%lld) o%d->%s@%s:%d/%d lens %d/%d e %d to %d dl " CFS_TIME_T " ref %d fl " REQ_FLAGS_FMT "/%x/%x rc %d/%d\n",
 			   req, req->rq_xid, req->rq_transno,
 			   req_ok ? lustre_msg_get_transno(req->rq_reqmsg) : 0,
 			   req_ok ? lustre_msg_get_opc(req->rq_reqmsg) : -1,
 			   req->rq_import ?
-				req->rq_import->imp_obd->obd_name :
-				req->rq_export ?
-				     req->rq_export->exp_client_uuid.uuid :
-				     "<?>",
+			   req->rq_import->imp_obd->obd_name :
+			   req->rq_export ?
+			   req->rq_export->exp_client_uuid.uuid :
+			   "<?>",
 			   libcfs_nid2str(nid),
 			   req->rq_request_portal, req->rq_reply_portal,
 			   req->rq_reqlen, req->rq_replen,
diff --git a/drivers/staging/lustre/lustre/ptlrpc/pinger.c b/drivers/staging/lustre/lustre/ptlrpc/pinger.c
index 20341b27a06a..340d98a64137 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/pinger.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/pinger.c
@@ -206,8 +206,7 @@ static void ptlrpc_pinger_process_import(struct obd_import *imp,
 
 	spin_unlock(&imp->imp_lock);
 
-	CDEBUG(level == LUSTRE_IMP_FULL ? D_INFO : D_HA, "%s->%s: level %s/%u "
-	       "force %u force_next %u deactive %u pingable %u suppress %u\n",
+	CDEBUG(level == LUSTRE_IMP_FULL ? D_INFO : D_HA, "%s->%s: level %s/%u force %u force_next %u deactive %u pingable %u suppress %u\n",
 	       imp->imp_obd->obd_uuid.uuid, obd2cli_tgt(imp->imp_obd),
 	       ptlrpc_import_state_name(level), level, force, force_next,
 	       imp->imp_deactive, imp->imp_pingable, suppress);
@@ -220,8 +219,7 @@ static void ptlrpc_pinger_process_import(struct obd_import *imp,
 	} else if (level != LUSTRE_IMP_FULL ||
 		   imp->imp_obd->obd_no_recov ||
 		   imp_is_deactive(imp)) {
-		CDEBUG(D_HA, "%s->%s: not pinging (in recovery "
-		       "or recovery disabled: %s)\n",
+		CDEBUG(D_HA, "%s->%s: not pinging (in recovery or recovery disabled: %s)\n",
 		       imp->imp_obd->obd_uuid.uuid, obd2cli_tgt(imp->imp_obd),
 		       ptlrpc_import_state_name(level));
 		if (force) {
@@ -334,11 +332,7 @@ int ptlrpc_start_pinger(void)
 		     thread_is_running(&pinger_thread), &lwi);
 
 	if (suppress_pings)
-		CWARN("Pings will be suppressed at the request of the "
-		      "administrator.  The configuration shall meet the "
-		      "additional requirements described in the manual.  "
-		      "(Search for the \"suppress_pings\" kernel module "
-		      "parameter.)\n");
+		CWARN("Pings will be suppressed at the request of the administrator.  The configuration shall meet the additional requirements described in the manual.  (Search for the \"suppress_pings\" kernel module parameter.)\n");
 
 	return 0;
 }
@@ -428,7 +422,7 @@ EXPORT_SYMBOL(ptlrpc_pinger_del_import);
  * Register a timeout callback to the pinger list, and the callback will
  * be called when timeout happens.
  */
-struct timeout_item* ptlrpc_new_timeout(int time, enum timeout_event event,
+struct timeout_item *ptlrpc_new_timeout(int time, enum timeout_event event,
 					timeout_cb_t cb, void *data)
 {
 	struct timeout_item *ti;
@@ -448,7 +442,7 @@ struct timeout_item* ptlrpc_new_timeout(int time, enum timeout_event event,
 }
 
 /**
- * Register timeout event on the the pinger thread.
+ * Register timeout event on the pinger thread.
  * Note: the timeout list is an sorted list with increased timeout value.
  */
 static struct timeout_item*
@@ -623,11 +617,7 @@ static int ping_evictor_main(void *arg)
 			if (expire_time > exp->exp_last_request_time) {
 				class_export_get(exp);
 				spin_unlock(&obd->obd_dev_lock);
-				LCONSOLE_WARN("%s: haven't heard from client %s"
-					      " (at %s) in %ld seconds. I think"
-					      " it's dead, and I am evicting"
-					      " it. exp %p, cur %ld expire %ld"
-					      " last %ld\n",
+				LCONSOLE_WARN("%s: haven't heard from client %s (at %s) in %ld seconds. I think it's dead, and I am evicting it. exp %p, cur %ld expire %ld last %ld\n",
 					      obd->obd_name,
 					      obd_uuid2str(&exp->exp_client_uuid),
 					      obd_export_nid2str(exp),
diff --git a/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c b/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c
index 357ea9f8bd57..cbcc541cac43 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c
@@ -356,10 +356,9 @@ static int ptlrpcd_check(struct lu_env *env, struct ptlrpcd_ctl *pc)
 				if (atomic_read(&ps->set_new_count)) {
 					rc = ptlrpcd_steal_rqset(set, ps);
 					if (rc > 0)
-						CDEBUG(D_RPCTRACE, "transfer %d"
-						       " async RPCs [%d->%d]\n",
-							rc, partner->pc_index,
-							pc->pc_index);
+						CDEBUG(D_RPCTRACE, "transfer %d async RPCs [%d->%d]\n",
+						       rc, partner->pc_index,
+						       pc->pc_index);
 				}
 				ptlrpc_reqset_put(ps);
 			} while (rc == 0 && pc->pc_cursor != first);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/recover.c b/drivers/staging/lustre/lustre/ptlrpc/recover.c
index e1bc77b83ffb..7b1d72947330 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/recover.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/recover.c
@@ -237,8 +237,7 @@ void ptlrpc_request_handle_notconn(struct ptlrpc_request *failed_req)
 	if (ptlrpc_set_import_discon(imp,
 			      lustre_msg_get_conn_cnt(failed_req->rq_reqmsg))) {
 		if (!imp->imp_replayable) {
-			CDEBUG(D_HA, "import %s@%s for %s not replayable, "
-			       "auto-deactivating\n",
+			CDEBUG(D_HA, "import %s@%s for %s not replayable, auto-deactivating\n",
 			       obd2cli_tgt(imp->imp_obd),
 			       imp->imp_connection->c_remote_uuid.uuid,
 			       imp->imp_obd->obd_name);
@@ -274,8 +273,8 @@ int ptlrpc_set_import_active(struct obd_import *imp, int active)
 	/* When deactivating, mark import invalid, and abort in-flight
 	 * requests. */
 	if (!active) {
-		LCONSOLE_WARN("setting import %s INACTIVE by administrator "
-			      "request\n", obd2cli_tgt(imp->imp_obd));
+		LCONSOLE_WARN("setting import %s INACTIVE by administrator request\n",
+			      obd2cli_tgt(imp->imp_obd));
 
 		/* set before invalidate to avoid messages about imp_inval
 		 * set without imp_deactive in ptlrpc_import_delay_req */
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec.c b/drivers/staging/lustre/lustre/ptlrpc/sec.c
index 4ce76852e6ee..21e9dc9d5580 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec.c
@@ -209,7 +209,7 @@ EXPORT_SYMBOL(sptlrpc_flavor2name_bulk);
 
 char *sptlrpc_flavor2name(struct sptlrpc_flavor *sf, char *buf, int bufsize)
 {
-	snprintf(buf, bufsize, "%s", sptlrpc_flavor2name_base(sf->sf_rpc));
+	strlcpy(buf, sptlrpc_flavor2name_base(sf->sf_rpc), bufsize);
 
 	/*
 	 * currently we don't support customized bulk specification for
@@ -220,10 +220,9 @@ char *sptlrpc_flavor2name(struct sptlrpc_flavor *sf, char *buf, int bufsize)
 
 		bspec[0] = '-';
 		sptlrpc_flavor2name_bulk(sf, &bspec[1], sizeof(bspec) - 1);
-		strncat(buf, bspec, bufsize);
+		strlcat(buf, bspec, bufsize);
 	}
 
-	buf[bufsize - 1] = '\0';
 	return buf;
 }
 EXPORT_SYMBOL(sptlrpc_flavor2name);
@@ -457,8 +456,8 @@ int sptlrpc_req_ctx_switch(struct ptlrpc_request *req,
 	LASSERT(req->rq_reqlen);
 	LASSERT(req->rq_replen);
 
-	CDEBUG(D_SEC, "req %p: switch ctx %p(%u->%s) -> %p(%u->%s), "
-	       "switch sec %p(%s) -> %p(%s)\n", req,
+	CDEBUG(D_SEC, "req %p: switch ctx %p(%u->%s) -> %p(%u->%s), switch sec %p(%s) -> %p(%s)\n",
+	       req,
 	       oldctx, oldctx->cc_vcred.vc_uid, sec2target_str(oldctx->cc_sec),
 	       newctx, newctx->cc_vcred.vc_uid, sec2target_str(newctx->cc_sec),
 	       oldctx->cc_sec, oldctx->cc_sec->ps_policy->sp_name,
@@ -1842,8 +1841,8 @@ int sptlrpc_target_export_check(struct obd_export *exp,
 							req->rq_svc_ctx,
 							&flavor);
 		} else {
-			CDEBUG(D_SEC, "exp %p (%x|%x|%x): is current flavor, "
-			       "install rvs ctx\n", exp, exp->exp_flvr.sf_rpc,
+			CDEBUG(D_SEC, "exp %p (%x|%x|%x): is current flavor, install rvs ctx\n",
+			       exp, exp->exp_flvr.sf_rpc,
 			       exp->exp_flvr_old[0].sf_rpc,
 			       exp->exp_flvr_old[1].sf_rpc);
 			spin_unlock(&exp->exp_lock);
@@ -1856,13 +1855,12 @@ int sptlrpc_target_export_check(struct obd_export *exp,
 	if (exp->exp_flvr_expire[0]) {
 		if (exp->exp_flvr_expire[0] >= get_seconds()) {
 			if (flavor_allowed(&exp->exp_flvr_old[0], req)) {
-				CDEBUG(D_SEC, "exp %p (%x|%x|%x): match the "
-				       "middle one ("CFS_DURATION_T")\n", exp,
+				CDEBUG(D_SEC, "exp %p (%x|%x|%x): match the middle one (" CFS_DURATION_T ")\n", exp,
 				       exp->exp_flvr.sf_rpc,
 				       exp->exp_flvr_old[0].sf_rpc,
 				       exp->exp_flvr_old[1].sf_rpc,
 				       exp->exp_flvr_expire[0] -
-						get_seconds());
+				       get_seconds());
 				spin_unlock(&exp->exp_lock);
 				return 0;
 			}
@@ -1881,13 +1879,13 @@ int sptlrpc_target_export_check(struct obd_export *exp,
 	if (exp->exp_flvr_changed == 0 && exp->exp_flvr_expire[1]) {
 		if (exp->exp_flvr_expire[1] >= get_seconds()) {
 			if (flavor_allowed(&exp->exp_flvr_old[1], req)) {
-				CDEBUG(D_SEC, "exp %p (%x|%x|%x): match the "
-				       "oldest one ("CFS_DURATION_T")\n", exp,
+				CDEBUG(D_SEC, "exp %p (%x|%x|%x): match the oldest one (" CFS_DURATION_T ")\n",
+				       exp,
 				       exp->exp_flvr.sf_rpc,
 				       exp->exp_flvr_old[0].sf_rpc,
 				       exp->exp_flvr_old[1].sf_rpc,
 				       exp->exp_flvr_expire[1] -
-						get_seconds());
+				       get_seconds());
 				spin_unlock(&exp->exp_lock);
 				return 0;
 			}
@@ -1907,8 +1905,7 @@ int sptlrpc_target_export_check(struct obd_export *exp,
 
 	spin_unlock(&exp->exp_lock);
 
-	CWARN("exp %p(%s): req %p (%u|%u|%u|%u|%u|%u) with "
-	      "unauthorized flavor %x, expect %x|%x(%+ld)|%x(%+ld)\n",
+	CWARN("exp %p(%s): req %p (%u|%u|%u|%u|%u|%u) with unauthorized flavor %x, expect %x|%x(%+ld)|%x(%+ld)\n",
 	      exp, exp->exp_obd->obd_name,
 	      req, req->rq_auth_gss, req->rq_ctx_init, req->rq_ctx_fini,
 	      req->rq_auth_usr_root, req->rq_auth_usr_mdt, req->rq_auth_usr_ost,
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c b/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
index cc68a1cf24e3..0dabd83fd46f 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
@@ -772,8 +772,7 @@ void sptlrpc_enc_pool_fini(void)
 
 	if (page_pools.epp_st_access > 0) {
 		CDEBUG(D_SEC,
-		       "max pages %lu, grows %u, grow fails %u, shrinks %u, "
-		       "access %lu, missing %lu, max qlen %u, max wait "
+		       "max pages %lu, grows %u, grow fails %u, shrinks %u, access %lu, missing %lu, max qlen %u, max wait "
 		       CFS_TIME_T"/%d\n",
 		       page_pools.epp_st_max_pages, page_pools.epp_st_grows,
 		       page_pools.epp_st_grow_fails,
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_null.c b/drivers/staging/lustre/lustre/ptlrpc/sec_null.c
index 099cec3b669f..4e132435b450 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_null.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_null.c
@@ -101,16 +101,7 @@ int null_ctx_verify(struct ptlrpc_cli_ctx *ctx, struct ptlrpc_request *req)
 
 	if (req->rq_early) {
 		cksums = lustre_msg_get_cksum(req->rq_repdata);
-#if LUSTRE_VERSION_CODE < OBD_OCD_VERSION(2, 7, 50, 0)
-		if (lustre_msghdr_get_flags(req->rq_reqmsg) &
-		    MSGHDR_CKSUM_INCOMPAT18)
-			cksumc = lustre_msg_calc_cksum(req->rq_repmsg, 0);
-		else
-			cksumc = lustre_msg_calc_cksum(req->rq_repmsg, 1);
-#else
-# warning "remove checksum compatibility support for b1_8"
 		cksumc = lustre_msg_calc_cksum(req->rq_repmsg);
-#endif
 		if (cksumc != cksums) {
 			CDEBUG(D_SEC,
 			       "early reply checksum mismatch: %08x != %08x\n",
@@ -371,16 +362,7 @@ int null_authorize(struct ptlrpc_request *req)
 	} else {
 		__u32 cksum;
 
-#if LUSTRE_VERSION_CODE < OBD_OCD_VERSION(2, 7, 50, 0)
-		if (lustre_msghdr_get_flags(req->rq_reqmsg) &
-		    MSGHDR_CKSUM_INCOMPAT18)
-			cksum = lustre_msg_calc_cksum(rs->rs_repbuf, 0);
-		else
-			cksum = lustre_msg_calc_cksum(rs->rs_repbuf, 1);
-#else
-# warning "remove checksum compatibility support for b1_8"
 		cksum = lustre_msg_calc_cksum(rs->rs_repbuf);
-#endif
 		lustre_msg_set_cksum(rs->rs_repbuf, cksum);
 		req->rq_reply_off = 0;
 	}
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_plain.c b/drivers/staging/lustre/lustre/ptlrpc/sec_plain.c
index 3d72b810c45c..a79cd53010a4 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_plain.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_plain.c
@@ -938,8 +938,8 @@ int plain_svc_wrap_bulk(struct ptlrpc_request *req,
 	rc = plain_generate_bulk_csum(desc, req->rq_flvr.u_bulk.hash.hash_alg,
 				      tokenv);
 	if (rc) {
-		CERROR("bulk read: server failed to compute "
-		       "checksum: %d\n", rc);
+		CERROR("bulk read: server failed to compute checksum: %d\n",
+		       rc);
 	} else {
 		if (OBD_FAIL_CHECK(OBD_FAIL_OSC_CHECKSUM_RECEIVE))
 			corrupt_bulk_data(desc);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/service.c b/drivers/staging/lustre/lustre/ptlrpc/service.c
index a8df8a792333..635b12b22cef 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/service.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/service.c
@@ -580,8 +580,7 @@ ptlrpc_server_nthreads_check(struct ptlrpc_service *svc,
 	svc->srv_nthrs_cpt_init = init;
 
 	if (nthrs * svc->srv_ncpts > tc->tc_nthrs_max) {
-		CDEBUG(D_OTHER, "%s: This service may have more threads (%d) "
-		       "than the given soft limit (%d)\n",
+		CDEBUG(D_OTHER, "%s: This service may have more threads (%d) than the given soft limit (%d)\n",
 		       svc->srv_name, nthrs * svc->srv_ncpts,
 		       tc->tc_nthrs_max);
 	}
@@ -1251,8 +1250,8 @@ static int ptlrpc_at_send_early_reply(struct ptlrpc_request *req)
 	/* deadline is when the client expects us to reply, margin is the
 	   difference between clients' and servers' expectations */
 	DEBUG_REQ(D_ADAPTTO, req,
-		  "%ssending early reply (deadline %+lds, margin %+lds) for "
-		  "%d+%d", AT_OFF ? "AT off - not " : "",
+		  "%ssending early reply (deadline %+lds, margin %+lds) for %d+%d",
+		  AT_OFF ? "AT off - not " : "",
 		  olddl, olddl - at_get(&svcpt->scp_at_estimate),
 		  at_get(&svcpt->scp_at_estimate), at_extra);
 
@@ -1260,17 +1259,15 @@ static int ptlrpc_at_send_early_reply(struct ptlrpc_request *req)
 		return 0;
 
 	if (olddl < 0) {
-		DEBUG_REQ(D_WARNING, req, "Already past deadline (%+lds), "
-			  "not sending early reply. Consider increasing "
-			  "at_early_margin (%d)?", olddl, at_early_margin);
+		DEBUG_REQ(D_WARNING, req, "Already past deadline (%+lds), not sending early reply. Consider increasing at_early_margin (%d)?",
+			  olddl, at_early_margin);
 
 		/* Return an error so we're not re-added to the timed list. */
 		return -ETIMEDOUT;
 	}
 
 	if (!(lustre_msghdr_get_flags(req->rq_reqmsg) & MSGHDR_AT_SUPPORT)) {
-		DEBUG_REQ(D_INFO, req, "Wanted to ask client for more time, "
-			  "but no AT support");
+		DEBUG_REQ(D_INFO, req, "Wanted to ask client for more time, but no AT support");
 		return -ENOSYS;
 	}
 
@@ -1296,8 +1293,7 @@ static int ptlrpc_at_send_early_reply(struct ptlrpc_request *req)
 		 * we may be past adaptive_max */
 		if (req->rq_deadline >= req->rq_arrival_time.tv_sec +
 		    at_get(&svcpt->scp_at_estimate)) {
-			DEBUG_REQ(D_WARNING, req, "Couldn't add any time "
-				  "(%ld/%ld), not sending early reply\n",
+			DEBUG_REQ(D_WARNING, req, "Couldn't add any time (%ld/%ld), not sending early reply\n",
 				  olddl, req->rq_arrival_time.tv_sec +
 				  at_get(&svcpt->scp_at_estimate) -
 				  get_seconds());
@@ -1329,8 +1325,7 @@ static int ptlrpc_at_send_early_reply(struct ptlrpc_request *req)
 	LASSERT(atomic_read(&req->rq_refcount));
 	/** if it is last refcount then early reply isn't needed */
 	if (atomic_read(&req->rq_refcount) == 1) {
-		DEBUG_REQ(D_ADAPTTO, reqcopy, "Normal reply already sent out, "
-			  "abort sending early reply\n");
+		DEBUG_REQ(D_ADAPTTO, reqcopy, "Normal reply already sent out, abort sending early reply\n");
 		rc = -EINVAL;
 		goto out;
 	}
@@ -1454,16 +1449,14 @@ static int ptlrpc_at_check_timed(struct ptlrpc_service_part *svcpt)
 
 	spin_unlock(&svcpt->scp_at_lock);
 
-	CDEBUG(D_ADAPTTO, "timeout in %+ds, asking for %d secs on %d early "
-	       "replies\n", first, at_extra, counter);
+	CDEBUG(D_ADAPTTO, "timeout in %+ds, asking for %d secs on %d early replies\n",
+	       first, at_extra, counter);
 	if (first < 0) {
 		/* We're already past request deadlines before we even get a
 		   chance to send early replies */
-		LCONSOLE_WARN("%s: This server is not able to keep up with "
-			      "request traffic (cpu-bound).\n",
+		LCONSOLE_WARN("%s: This server is not able to keep up with request traffic (cpu-bound).\n",
 			      svcpt->scp_service->srv_name);
-		CWARN("earlyQ=%d reqQ=%d recA=%d, svcEst=%d, "
-		      "delay="CFS_DURATION_T"(jiff)\n",
+		CWARN("earlyQ=%d reqQ=%d recA=%d, svcEst=%d, delay=" CFS_DURATION_T "(jiff)\n",
 		      counter, svcpt->scp_nreqs_incoming,
 		      svcpt->scp_nreqs_active,
 		      at_get(&svcpt->scp_at_estimate), delay);
@@ -1825,8 +1818,7 @@ ptlrpc_server_handle_req_in(struct ptlrpc_service_part *svcpt,
 		if (rc == 0) {
 			rc = sptlrpc_target_export_check(req->rq_export, req);
 			if (rc)
-				DEBUG_REQ(D_ERROR, req, "DROPPING req with "
-					  "illegal security flavor,");
+				DEBUG_REQ(D_ERROR, req, "DROPPING req with illegal security flavor,");
 		}
 
 		if (rc)
@@ -1942,18 +1934,17 @@ ptlrpc_server_handle_request(struct ptlrpc_service_part *svcpt,
 	/* Discard requests queued for longer than the deadline.
 	   The deadline is increased if we send an early reply. */
 	if (get_seconds() > request->rq_deadline) {
-		DEBUG_REQ(D_ERROR, request, "Dropping timed-out request from %s"
-			  ": deadline "CFS_DURATION_T":"CFS_DURATION_T"s ago\n",
+		DEBUG_REQ(D_ERROR, request, "Dropping timed-out request from %s: deadline " CFS_DURATION_T ":" CFS_DURATION_T "s ago\n",
 			  libcfs_id2str(request->rq_peer),
 			  cfs_time_sub(request->rq_deadline,
-			  request->rq_arrival_time.tv_sec),
+				       request->rq_arrival_time.tv_sec),
 			  cfs_time_sub(get_seconds(),
-			  request->rq_deadline));
+				       request->rq_deadline));
 		goto put_conn;
 	}
 
-	CDEBUG(D_RPCTRACE, "Handling RPC pname:cluuid+ref:pid:xid:nid:opc "
-	       "%s:%s+%d:%d:x%llu:%s:%d\n", current_comm(),
+	CDEBUG(D_RPCTRACE, "Handling RPC pname:cluuid+ref:pid:xid:nid:opc %s:%s+%d:%d:x%llu:%s:%d\n",
+	       current_comm(),
 	       (request->rq_export ?
 		(char *)request->rq_export->exp_client_uuid.uuid : "0"),
 	       (request->rq_export ?
@@ -1986,26 +1977,24 @@ put_conn:
 
 	do_gettimeofday(&work_end);
 	timediff = cfs_timeval_sub(&work_end, &work_start, NULL);
-	CDEBUG(D_RPCTRACE, "Handled RPC pname:cluuid+ref:pid:xid:nid:opc "
-	       "%s:%s+%d:%d:x%llu:%s:%d Request processed in "
-	       "%ldus (%ldus total) trans %llu rc %d/%d\n",
-		current_comm(),
-		(request->rq_export ?
-		 (char *)request->rq_export->exp_client_uuid.uuid : "0"),
-		(request->rq_export ?
-		 atomic_read(&request->rq_export->exp_refcount) : -99),
-		lustre_msg_get_status(request->rq_reqmsg),
-		request->rq_xid,
-		libcfs_id2str(request->rq_peer),
-		lustre_msg_get_opc(request->rq_reqmsg),
-		timediff,
-		cfs_timeval_sub(&work_end, &request->rq_arrival_time, NULL),
-		(request->rq_repmsg ?
-		 lustre_msg_get_transno(request->rq_repmsg) :
-		 request->rq_transno),
-		request->rq_status,
-		(request->rq_repmsg ?
-		 lustre_msg_get_status(request->rq_repmsg) : -999));
+	CDEBUG(D_RPCTRACE, "Handled RPC pname:cluuid+ref:pid:xid:nid:opc %s:%s+%d:%d:x%llu:%s:%d Request processed in %ldus (%ldus total) trans %llu rc %d/%d\n",
+	       current_comm(),
+	       (request->rq_export ?
+		(char *)request->rq_export->exp_client_uuid.uuid : "0"),
+	       (request->rq_export ?
+		atomic_read(&request->rq_export->exp_refcount) : -99),
+	       lustre_msg_get_status(request->rq_reqmsg),
+	       request->rq_xid,
+	       libcfs_id2str(request->rq_peer),
+	       lustre_msg_get_opc(request->rq_reqmsg),
+	       timediff,
+	       cfs_timeval_sub(&work_end, &request->rq_arrival_time, NULL),
+	       (request->rq_repmsg ?
+		lustre_msg_get_transno(request->rq_repmsg) :
+		request->rq_transno),
+	       request->rq_status,
+	       (request->rq_repmsg ?
+		lustre_msg_get_status(request->rq_repmsg) : -999));
 	if (likely(svc->srv_stats != NULL && request->rq_reqmsg != NULL)) {
 		__u32 op = lustre_msg_get_opc(request->rq_reqmsg);
 		int opc = opcode_offset(op);
@@ -2557,8 +2546,8 @@ static int ptlrpc_start_hr_threads(void)
 		if (!IS_ERR_VALUE(rc))
 			continue;
 
-		CERROR("Reply handling thread %d:%d Failed on starting: "
-		       "rc = %d\n", i, j, rc);
+		CERROR("Reply handling thread %d:%d Failed on starting: rc = %d\n",
+		       i, j, rc);
 		ptlrpc_stop_hr_threads();
 		return rc;
 	}
@@ -2920,8 +2909,7 @@ ptlrpc_service_unlink_rqbd(struct ptlrpc_service *svc)
 			rc = l_wait_event(svcpt->scp_waitq,
 					  svcpt->scp_nrqbds_posted == 0, &lwi);
 			if (rc == -ETIMEDOUT) {
-				CWARN("Service %s waiting for "
-				      "request buffers\n",
+				CWARN("Service %s waiting for request buffers\n",
 				      svcpt->scp_service->srv_name);
 			}
 			spin_lock(&svcpt->scp_lock);
diff --git a/drivers/staging/media/Kconfig b/drivers/staging/media/Kconfig
index 96498b7fc20e..2a054a99d433 100644
--- a/drivers/staging/media/Kconfig
+++ b/drivers/staging/media/Kconfig
@@ -27,12 +27,18 @@ source "drivers/staging/media/davinci_vpfe/Kconfig"
 
 source "drivers/staging/media/dt3155v4l/Kconfig"
 
+source "drivers/staging/media/tlg2300/Kconfig"
+
 source "drivers/staging/media/mn88472/Kconfig"
 
 source "drivers/staging/media/mn88473/Kconfig"
 
 source "drivers/staging/media/omap4iss/Kconfig"
 
+source "drivers/staging/media/parport/Kconfig"
+
+source "drivers/staging/media/vino/Kconfig"
+
 # Keep LIRC at the end, as it has sub-menus
 source "drivers/staging/media/lirc/Kconfig"
 
diff --git a/drivers/staging/media/Makefile b/drivers/staging/media/Makefile
index 30fb352fc4a9..412b28408398 100644
--- a/drivers/staging/media/Makefile
+++ b/drivers/staging/media/Makefile
@@ -6,4 +6,7 @@ obj-$(CONFIG_VIDEO_DM365_VPFE)	+= davinci_vpfe/
 obj-$(CONFIG_VIDEO_OMAP4)	+= omap4iss/
 obj-$(CONFIG_DVB_MN88472)       += mn88472/
 obj-$(CONFIG_DVB_MN88473)       += mn88473/
+obj-y				+= parport/
+obj-$(CONFIG_VIDEO_TLG2300)	+= tlg2300/
+obj-y                           += vino/
 
diff --git a/drivers/staging/media/cxd2099/cxd2099.c b/drivers/staging/media/cxd2099/cxd2099.c
index 73e7b2c9e4a7..657ea480c6e7 100644
--- a/drivers/staging/media/cxd2099/cxd2099.c
+++ b/drivers/staging/media/cxd2099/cxd2099.c
@@ -527,7 +527,7 @@ static int slot_reset(struct dvb_ca_en50221 *ca, int slot)
 		u8 val;
 #endif
 		for (i = 0; i < 100; i++) {
-			msleep(10);
+			usleep_range(10000, 11000);
 #if 0
 			read_reg(ci, 0x06, &val);
 			dev_info(&ci->i2c->dev, "%d:%02x\n", i, val);
diff --git a/drivers/staging/media/davinci_vpfe/dm365_ipipe.h b/drivers/staging/media/davinci_vpfe/dm365_ipipe.h
index cf4204603eb8..d81b29e19309 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_ipipe.h
+++ b/drivers/staging/media/davinci_vpfe/dm365_ipipe.h
@@ -120,8 +120,8 @@ struct vpfe_ipipe_device {
 	enum ipipe_input_entity input;
 	unsigned int output;
 	struct v4l2_ctrl_handler ctrls;
-	void *__iomem base_addr;
-	void *__iomem isp5_base_addr;
+	void __iomem *base_addr;
+	void __iomem *isp5_base_addr;
 	struct ipipe_module_params config;
 };
 
diff --git a/drivers/staging/media/davinci_vpfe/dm365_ipipe_hw.c b/drivers/staging/media/davinci_vpfe/dm365_ipipe_hw.c
index 6461de1a61fd..2a3a56b88de1 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_ipipe_hw.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_ipipe_hw.c
@@ -24,7 +24,7 @@
 #define IPIPE_MODE_CONTINUOUS		0
 #define IPIPE_MODE_SINGLE_SHOT		1
 
-static void ipipe_clock_enable(void *__iomem base_addr)
+static void ipipe_clock_enable(void __iomem *base_addr)
 {
 	/* enable IPIPE MMR for register write access */
 	regw_ip(base_addr, IPIPE_GCK_MMR_DEFAULT, IPIPE_GCK_MMR);
@@ -34,7 +34,7 @@ static void ipipe_clock_enable(void *__iomem base_addr)
 }
 
 static void
-rsz_set_common_params(void *__iomem rsz_base, struct resizer_params *params)
+rsz_set_common_params(void __iomem *rsz_base, struct resizer_params *params)
 {
 	struct rsz_common_params *rsz_common = &params->rsz_common;
 	u32 val;
@@ -66,7 +66,7 @@ rsz_set_common_params(void *__iomem rsz_base, struct resizer_params *params)
 }
 
 static void
-rsz_set_rsz_regs(void *__iomem rsz_base, unsigned int rsz_id,
+rsz_set_rsz_regs(void __iomem *rsz_base, unsigned int rsz_id,
 		 struct resizer_params *params)
 {
 	struct resizer_scale_param *rsc_params;
@@ -171,7 +171,7 @@ rsz_set_rsz_regs(void *__iomem rsz_base, unsigned int rsz_id,
 
 /*set the registers of either RSZ0 or RSZ1 */
 static void
-ipipe_setup_resizer(void *__iomem rsz_base, struct resizer_params *params)
+ipipe_setup_resizer(void __iomem *rsz_base, struct resizer_params *params)
 {
 	/* enable MMR gate to write to Resizer */
 	regw_rsz(rsz_base, 1, RSZ_GCK_MMR);
@@ -302,8 +302,8 @@ int config_rsz_hw(struct vpfe_resizer_device *resizer,
 		  struct resizer_params *config)
 {
 	struct vpfe_device *vpfe_dev = to_vpfe_device(resizer);
-	void *__iomem ipipe_base = vpfe_dev->vpfe_ipipe.base_addr;
-	void *__iomem rsz_base = vpfe_dev->vpfe_resizer.base_addr;
+	void __iomem *ipipe_base = vpfe_dev->vpfe_ipipe.base_addr;
+	void __iomem *rsz_base = vpfe_dev->vpfe_resizer.base_addr;
 
 	/* enable VPSS clock */
 	vpss_enable_clock(VPSS_IPIPE_CLOCK, 1);
@@ -315,7 +315,7 @@ int config_rsz_hw(struct vpfe_resizer_device *resizer,
 }
 
 static void
-rsz_set_y_address(void *__iomem rsz_base, unsigned int address,
+rsz_set_y_address(void __iomem *rsz_base, unsigned int address,
 		  unsigned int offset)
 {
 	u32 val;
@@ -330,7 +330,7 @@ rsz_set_y_address(void *__iomem rsz_base, unsigned int address,
 }
 
 static void
-rsz_set_c_address(void *__iomem rsz_base, unsigned int address,
+rsz_set_c_address(void __iomem *rsz_base, unsigned int address,
 		  unsigned int offset)
 {
 	u32 val;
@@ -352,7 +352,7 @@ rsz_set_c_address(void *__iomem rsz_base, unsigned int address,
  * @address: the address to set
  */
 int
-resizer_set_outaddr(void *__iomem rsz_base, struct resizer_params *params,
+resizer_set_outaddr(void __iomem *rsz_base, struct resizer_params *params,
 		    int resize_no, unsigned int address)
 {
 	struct resizer_scale_param *rsc_param;
@@ -411,7 +411,7 @@ resizer_set_outaddr(void *__iomem rsz_base, struct resizer_params *params,
 }
 
 void
-ipipe_set_lutdpc_regs(void *__iomem base_addr, void *__iomem isp5_base_addr,
+ipipe_set_lutdpc_regs(void __iomem *base_addr, void __iomem *isp5_base_addr,
 		      struct vpfe_ipipe_lutdpc *dpc)
 {
 	u32 max_tbl_size = LUT_DPC_MAX_SIZE >> 1;
@@ -446,7 +446,7 @@ ipipe_set_lutdpc_regs(void *__iomem base_addr, void *__iomem isp5_base_addr,
 }
 
 static void
-set_dpc_thresholds(void *__iomem base_addr,
+set_dpc_thresholds(void __iomem *base_addr,
 		   struct vpfe_ipipe_otfdpc_2_0_cfg *dpc_thr)
 {
 	regw_ip(base_addr, dpc_thr->corr_thr.r & OTFDPC_DPC2_THR_MASK,
@@ -467,7 +467,7 @@ set_dpc_thresholds(void *__iomem base_addr,
 		DPC_OTF_2D_THR_B);
 }
 
-void ipipe_set_otfdpc_regs(void *__iomem base_addr,
+void ipipe_set_otfdpc_regs(void __iomem *base_addr,
 			   struct vpfe_ipipe_otfdpc *otfdpc)
 {
 	struct vpfe_ipipe_otfdpc_2_0_cfg *dpc_2_0 = &otfdpc->alg_cfg.dpc_2_0;
@@ -523,7 +523,7 @@ void ipipe_set_otfdpc_regs(void *__iomem base_addr,
 
 /* 2D Noise filter */
 void
-ipipe_set_d2f_regs(void *__iomem base_addr, unsigned int id,
+ipipe_set_d2f_regs(void __iomem *base_addr, unsigned int id,
 		   struct vpfe_ipipe_nf *noise_filter)
 {
 
@@ -571,7 +571,7 @@ ipipe_set_d2f_regs(void *__iomem base_addr, unsigned int id,
 	(((decimal & 0x1f) | ((integer & 0x7) << 5)))
 
 /* Green Imbalance Correction */
-void ipipe_set_gic_regs(void *__iomem base_addr, struct vpfe_ipipe_gic *gic)
+void ipipe_set_gic_regs(void __iomem *base_addr, struct vpfe_ipipe_gic *gic)
 {
 	u32 val;
 
@@ -609,7 +609,7 @@ void ipipe_set_gic_regs(void *__iomem base_addr, struct vpfe_ipipe_gic *gic)
 #define IPIPE_U13Q9(decimal, integer) \
 	(((decimal & 0x1ff) | ((integer & 0xf) << 9)))
 /* White balance */
-void ipipe_set_wb_regs(void *__iomem base_addr, struct vpfe_ipipe_wb *wb)
+void ipipe_set_wb_regs(void __iomem *base_addr, struct vpfe_ipipe_wb *wb)
 {
 	u32 val;
 
@@ -635,7 +635,7 @@ void ipipe_set_wb_regs(void *__iomem base_addr, struct vpfe_ipipe_wb *wb)
 }
 
 /* CFA */
-void ipipe_set_cfa_regs(void *__iomem base_addr, struct vpfe_ipipe_cfa *cfa)
+void ipipe_set_cfa_regs(void __iomem *base_addr, struct vpfe_ipipe_cfa *cfa)
 {
 	ipipe_clock_enable(base_addr);
 
@@ -671,7 +671,7 @@ void ipipe_set_cfa_regs(void *__iomem base_addr, struct vpfe_ipipe_cfa *cfa)
 }
 
 void
-ipipe_set_rgb2rgb_regs(void *__iomem base_addr, unsigned int id,
+ipipe_set_rgb2rgb_regs(void __iomem *base_addr, unsigned int id,
 		       struct vpfe_ipipe_rgb2rgb *rgb)
 {
 	u32 offset_mask = RGB2RGB_1_OFST_MASK;
@@ -724,7 +724,7 @@ ipipe_set_rgb2rgb_regs(void *__iomem base_addr, unsigned int id,
 }
 
 static void
-ipipe_update_gamma_tbl(void *__iomem isp5_base_addr,
+ipipe_update_gamma_tbl(void __iomem *isp5_base_addr,
 	struct vpfe_ipipe_gamma_entry *table, int size, u32 addr)
 {
 	int count;
@@ -738,7 +738,7 @@ ipipe_update_gamma_tbl(void *__iomem isp5_base_addr,
 }
 
 void
-ipipe_set_gamma_regs(void *__iomem base_addr, void *__iomem isp5_base_addr,
+ipipe_set_gamma_regs(void __iomem *base_addr, void __iomem *isp5_base_addr,
 			  struct vpfe_ipipe_gamma *gamma)
 {
 	int table_size;
@@ -770,7 +770,7 @@ ipipe_set_gamma_regs(void *__iomem base_addr, void *__iomem isp5_base_addr,
 }
 
 void
-ipipe_set_3d_lut_regs(void *__iomem base_addr, void *__iomem isp5_base_addr,
+ipipe_set_3d_lut_regs(void __iomem *base_addr, void __iomem *isp5_base_addr,
 			   struct vpfe_ipipe_3d_lut *lut_3d)
 {
 	struct vpfe_ipipe_3d_lut_entry *tbl;
@@ -819,7 +819,7 @@ ipipe_set_3d_lut_regs(void *__iomem base_addr, void *__iomem isp5_base_addr,
 
 /* Lumina adjustments */
 void
-ipipe_set_lum_adj_regs(void *__iomem base_addr, struct ipipe_lum_adj *lum_adj)
+ipipe_set_lum_adj_regs(void __iomem *base_addr, struct ipipe_lum_adj *lum_adj)
 {
 	u32 val;
 
@@ -834,7 +834,7 @@ ipipe_set_lum_adj_regs(void *__iomem base_addr, struct ipipe_lum_adj *lum_adj)
 #define IPIPE_S12Q8(decimal, integer) \
 	(((decimal & 0xff) | ((integer & 0xf) << 8)))
 
-void ipipe_set_rgb2ycbcr_regs(void *__iomem base_addr,
+void ipipe_set_rgb2ycbcr_regs(void __iomem *base_addr,
 			      struct vpfe_ipipe_rgb2yuv *yuv)
 {
 	u32 val;
@@ -866,7 +866,7 @@ void ipipe_set_rgb2ycbcr_regs(void *__iomem base_addr,
 
 /* YUV 422 conversion */
 void
-ipipe_set_yuv422_conv_regs(void *__iomem base_addr,
+ipipe_set_yuv422_conv_regs(void __iomem *base_addr,
 			   struct vpfe_ipipe_yuv422_conv *conv)
 {
 	u32 val;
@@ -879,7 +879,7 @@ ipipe_set_yuv422_conv_regs(void *__iomem base_addr,
 }
 
 void
-ipipe_set_gbce_regs(void *__iomem base_addr, void *__iomem isp5_base_addr,
+ipipe_set_gbce_regs(void __iomem *base_addr, void __iomem *isp5_base_addr,
 		    struct vpfe_ipipe_gbce *gbce)
 {
 	unsigned int count;
@@ -906,7 +906,7 @@ ipipe_set_gbce_regs(void *__iomem base_addr, void *__iomem isp5_base_addr,
 }
 
 void
-ipipe_set_ee_regs(void *__iomem base_addr, void *__iomem isp5_base_addr,
+ipipe_set_ee_regs(void __iomem *base_addr, void __iomem *isp5_base_addr,
 		  struct vpfe_ipipe_yee *ee)
 {
 	unsigned int count;
@@ -950,7 +950,7 @@ ipipe_set_ee_regs(void *__iomem base_addr, void *__iomem isp5_base_addr,
 }
 
 /* Chromatic Artifact Correction. CAR */
-static void ipipe_set_mf(void *__iomem base_addr)
+static void ipipe_set_mf(void __iomem *base_addr)
 {
 	/* typ to dynamic switch */
 	regw_ip(base_addr, VPFE_IPIPE_CAR_DYN_SWITCH, CAR_TYP);
@@ -959,7 +959,7 @@ static void ipipe_set_mf(void *__iomem base_addr)
 }
 
 static void
-ipipe_set_gain_ctrl(void *__iomem base_addr, struct vpfe_ipipe_car *car)
+ipipe_set_gain_ctrl(void __iomem *base_addr, struct vpfe_ipipe_car *car)
 {
 	regw_ip(base_addr, VPFE_IPIPE_CAR_CHR_GAIN_CTRL, CAR_TYP);
 	regw_ip(base_addr, car->hpf, CAR_HPF_TYP);
@@ -975,7 +975,7 @@ ipipe_set_gain_ctrl(void *__iomem base_addr, struct vpfe_ipipe_car *car)
 		CAR_GN2_MIN);
 }
 
-void ipipe_set_car_regs(void *__iomem base_addr, struct vpfe_ipipe_car *car)
+void ipipe_set_car_regs(void __iomem *base_addr, struct vpfe_ipipe_car *car)
 {
 	u32 val;
 
@@ -1010,7 +1010,7 @@ void ipipe_set_car_regs(void *__iomem base_addr, struct vpfe_ipipe_car *car)
 }
 
 /* Chromatic Gain Suppression */
-void ipipe_set_cgs_regs(void *__iomem base_addr, struct vpfe_ipipe_cgs *cgs)
+void ipipe_set_cgs_regs(void __iomem *base_addr, struct vpfe_ipipe_cgs *cgs)
 {
 	ipipe_clock_enable(base_addr);
 	regw_ip(base_addr, cgs->en, CGS_EN);
@@ -1025,12 +1025,12 @@ void ipipe_set_cgs_regs(void *__iomem base_addr, struct vpfe_ipipe_cgs *cgs)
 	regw_ip(base_addr, cgs->h_min, CGS_GN1_H_MIN);
 }
 
-void rsz_src_enable(void *__iomem rsz_base, int enable)
+void rsz_src_enable(void __iomem *rsz_base, int enable)
 {
 	regw_rsz(rsz_base, enable, RSZ_SRC_EN);
 }
 
-int rsz_enable(void *__iomem rsz_base, int rsz_id, int enable)
+int rsz_enable(void __iomem *rsz_base, int rsz_id, int enable)
 {
 	if (rsz_id == RSZ_A) {
 		regw_rsz(rsz_base, enable, RSZ_EN_A);
diff --git a/drivers/staging/media/davinci_vpfe/dm365_ipipe_hw.h b/drivers/staging/media/davinci_vpfe/dm365_ipipe_hw.h
index 81176fb9d164..2bf2f7a69173 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_ipipe_hw.h
+++ b/drivers/staging/media/davinci_vpfe/dm365_ipipe_hw.h
@@ -490,29 +490,29 @@
 #define RSZ_RGB_TYP_SHIFT		0
 #define RSZ_RGB_ALPHA_MASK		0xff
 
-static inline u32 regr_ip(void *__iomem addr, u32 offset)
+static inline u32 regr_ip(void __iomem *addr, u32 offset)
 {
 	return readl(addr + offset);
 }
 
-static inline void regw_ip(void *__iomem addr, u32 val, u32 offset)
+static inline void regw_ip(void __iomem *addr, u32 val, u32 offset)
 {
 	writel(val, addr + offset);
 }
 
-static inline u32 w_ip_table(void *__iomem addr, u32 val, u32 offset)
+static inline u32 w_ip_table(void __iomem *addr, u32 val, u32 offset)
 {
 	writel(val, addr + offset);
 
 	return val;
 }
 
-static inline u32 regr_rsz(void *__iomem addr, u32 offset)
+static inline u32 regr_rsz(void __iomem *addr, u32 offset)
 {
 	return readl(addr + offset);
 }
 
-static inline u32 regw_rsz(void *__iomem addr, u32 val, u32 offset)
+static inline u32 regw_rsz(void __iomem *addr, u32 val, u32 offset)
 {
 	writel(val, addr + offset);
 
@@ -520,39 +520,39 @@ static inline u32 regw_rsz(void *__iomem addr, u32 val, u32 offset)
 }
 
 int config_ipipe_hw(struct vpfe_ipipe_device *ipipe);
-int resizer_set_outaddr(void *__iomem rsz_base, struct resizer_params *params,
+int resizer_set_outaddr(void __iomem *rsz_base, struct resizer_params *params,
 			int resize_no, unsigned int address);
-int rsz_enable(void *__iomem rsz_base, int rsz_id, int enable);
-void rsz_src_enable(void *__iomem rsz_base, int enable);
+int rsz_enable(void __iomem *rsz_base, int rsz_id, int enable);
+void rsz_src_enable(void __iomem *rsz_base, int enable);
 void rsz_set_in_pix_format(unsigned char y_c);
 int config_rsz_hw(struct vpfe_resizer_device *resizer,
 		  struct resizer_params *config);
-void ipipe_set_d2f_regs(void *__iomem base_addr, unsigned int id,
+void ipipe_set_d2f_regs(void __iomem *base_addr, unsigned int id,
 	struct vpfe_ipipe_nf *noise_filter);
-void ipipe_set_rgb2rgb_regs(void *__iomem base_addr, unsigned int id,
+void ipipe_set_rgb2rgb_regs(void __iomem *base_addr, unsigned int id,
 	struct vpfe_ipipe_rgb2rgb *rgb);
-void ipipe_set_yuv422_conv_regs(void *__iomem base_addr,
+void ipipe_set_yuv422_conv_regs(void __iomem *base_addr,
 	struct vpfe_ipipe_yuv422_conv *conv);
-void ipipe_set_lum_adj_regs(void *__iomem base_addr,
+void ipipe_set_lum_adj_regs(void __iomem *base_addr,
 	struct ipipe_lum_adj *lum_adj);
-void ipipe_set_rgb2ycbcr_regs(void *__iomem base_addr,
+void ipipe_set_rgb2ycbcr_regs(void __iomem *base_addr,
 	struct vpfe_ipipe_rgb2yuv *yuv);
-void ipipe_set_lutdpc_regs(void *__iomem base_addr,
-	void *__iomem isp5_base_addr, struct vpfe_ipipe_lutdpc *lutdpc);
-void ipipe_set_otfdpc_regs(void *__iomem base_addr,
+void ipipe_set_lutdpc_regs(void __iomem *base_addr,
+	void __iomem *isp5_base_addr, struct vpfe_ipipe_lutdpc *lutdpc);
+void ipipe_set_otfdpc_regs(void __iomem *base_addr,
 	struct vpfe_ipipe_otfdpc *otfdpc);
-void ipipe_set_3d_lut_regs(void *__iomem base_addr,
-	void *__iomem isp5_base_addr, struct vpfe_ipipe_3d_lut *lut_3d);
-void ipipe_set_gamma_regs(void *__iomem base_addr,
-	void *__iomem isp5_base_addr, struct vpfe_ipipe_gamma *gamma);
-void ipipe_set_ee_regs(void *__iomem base_addr,
-	void *__iomem isp5_base_addr, struct vpfe_ipipe_yee *ee);
-void ipipe_set_gbce_regs(void *__iomem base_addr,
-	void *__iomem isp5_base_addr, struct vpfe_ipipe_gbce *gbce);
-void ipipe_set_gic_regs(void *__iomem base_addr, struct vpfe_ipipe_gic *gic);
-void ipipe_set_cfa_regs(void *__iomem base_addr, struct vpfe_ipipe_cfa *cfa);
-void ipipe_set_car_regs(void *__iomem base_addr, struct vpfe_ipipe_car *car);
-void ipipe_set_cgs_regs(void *__iomem base_addr, struct vpfe_ipipe_cgs *cgs);
-void ipipe_set_wb_regs(void *__iomem base_addr, struct vpfe_ipipe_wb *wb);
+void ipipe_set_3d_lut_regs(void __iomem *base_addr,
+	void __iomem *isp5_base_addr, struct vpfe_ipipe_3d_lut *lut_3d);
+void ipipe_set_gamma_regs(void __iomem *base_addr,
+	void __iomem *isp5_base_addr, struct vpfe_ipipe_gamma *gamma);
+void ipipe_set_ee_regs(void __iomem *base_addr,
+	void __iomem *isp5_base_addr, struct vpfe_ipipe_yee *ee);
+void ipipe_set_gbce_regs(void __iomem *base_addr,
+	void __iomem *isp5_base_addr, struct vpfe_ipipe_gbce *gbce);
+void ipipe_set_gic_regs(void __iomem *base_addr, struct vpfe_ipipe_gic *gic);
+void ipipe_set_cfa_regs(void __iomem *base_addr, struct vpfe_ipipe_cfa *cfa);
+void ipipe_set_car_regs(void __iomem *base_addr, struct vpfe_ipipe_car *car);
+void ipipe_set_cgs_regs(void __iomem *base_addr, struct vpfe_ipipe_cgs *cgs);
+void ipipe_set_wb_regs(void __iomem *base_addr, struct vpfe_ipipe_wb *wb);
 
 #endif		/* _DAVINCI_VPFE_DM365_IPIPE_HW_H */
diff --git a/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c b/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c
index a86f16ff5818..87d42e18377d 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c
@@ -421,7 +421,7 @@ static int
 ipipeif_get_config(struct v4l2_subdev *sd, void __user *arg)
 {
 	struct vpfe_ipipeif_device *ipipeif = v4l2_get_subdevdata(sd);
-	struct ipipeif_params *config = (struct ipipeif_params *)arg;
+	struct ipipeif_params *config = arg;
 	struct device *dev = ipipeif->subdev.v4l2_dev->dev;
 
 	if (!arg) {
@@ -462,7 +462,7 @@ ipipeif_get_config(struct v4l2_subdev *sd, void __user *arg)
 static long ipipeif_ioctl(struct v4l2_subdev *sd,
 			  unsigned int cmd, void *arg)
 {
-	struct ipipeif_params *config = (struct ipipeif_params *)arg;
+	struct ipipeif_params *config = arg;
 	int ret = -ENOIOCTLCMD;
 
 	switch (cmd) {
diff --git a/drivers/staging/media/davinci_vpfe/dm365_ipipeif.h b/drivers/staging/media/davinci_vpfe/dm365_ipipeif.h
index 608701fc5fed..cea3d61335af 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_ipipeif.h
+++ b/drivers/staging/media/davinci_vpfe/dm365_ipipeif.h
@@ -134,7 +134,7 @@ struct vpfe_ipipeif_device {
 	unsigned int output;
 	struct vpfe_video_device video_in;
 	struct v4l2_ctrl_handler ctrls;
-	void *__iomem ipipeif_base_addr;
+	void __iomem *ipipeif_base_addr;
 	struct ipipeif_params config;
 	int dpcm_predictor;
 	int gain;
diff --git a/drivers/staging/media/davinci_vpfe/dm365_isif.c b/drivers/staging/media/davinci_vpfe/dm365_isif.c
index fa26f63831b7..0ba0bf2c1cff 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_isif.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_isif.c
@@ -70,17 +70,17 @@ static const u32 isif_srggb_pattern =
 	ISIF_COLPTN_Gb_G  << ISIF_CCOLP_CP15_4 |
 	ISIF_COLPTN_B_Mg  << ISIF_CCOLP_CP17_6;
 
-static inline u32 isif_read(void *__iomem base_addr, u32 offset)
+static inline u32 isif_read(void __iomem *base_addr, u32 offset)
 {
 	return readl(base_addr + offset);
 }
 
-static inline void isif_write(void *__iomem base_addr, u32 val, u32 offset)
+static inline void isif_write(void __iomem *base_addr, u32 val, u32 offset)
 {
 	writel(val, base_addr + offset);
 }
 
-static inline u32 isif_merge(void *__iomem base_addr, u32 mask, u32 val,
+static inline u32 isif_merge(void __iomem *base_addr, u32 mask, u32 val,
 			     u32 offset)
 {
 	u32 new_val = (isif_read(base_addr, offset) & ~mask) | (val & mask);
@@ -646,7 +646,7 @@ static void isif_config_gain_offset(struct vpfe_isif_device *isif)
 {
 	struct vpfe_isif_gain_offsets_adj *gain_off_ptr =
 		&isif->isif_cfg.bayer.config_params.gain_offset;
-	void *__iomem base = isif->isif_cfg.base_addr;
+	void __iomem *base = isif->isif_cfg.base_addr;
 	u32 val;
 
 	val = ((gain_off_ptr->gain_sdram_en & 1) << GAIN_SDRAM_EN_SHIFT) |
@@ -1602,6 +1602,7 @@ isif_pad_get_crop(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 
 	if (crop->which == V4L2_SUBDEV_FORMAT_TRY) {
 		struct v4l2_rect *rect;
+
 		rect = v4l2_subdev_get_try_crop(fh, ISIF_PAD_SINK);
 		memcpy(&crop->rect, rect, sizeof(*rect));
 	} else {
@@ -1991,7 +1992,7 @@ int vpfe_isif_init(struct vpfe_isif_device *isif, struct platform_device *pdev)
 	struct media_entity *me = &sd->entity;
 	static resource_size_t res_len;
 	struct resource *res;
-	void *__iomem addr;
+	void __iomem *addr;
 	int status;
 	int i = 0;
 
diff --git a/drivers/staging/media/davinci_vpfe/dm365_isif.h b/drivers/staging/media/davinci_vpfe/dm365_isif.h
index 473fd2cfe350..89e814e9c0d7 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_isif.h
+++ b/drivers/staging/media/davinci_vpfe/dm365_isif.h
@@ -159,9 +159,9 @@ struct isif_oper_config {
 	struct isif_params_raw bayer;
 	enum isif_data_pack data_pack;
 	struct isif_gain_values isif_gain_params;
-	void *__iomem base_addr;
-	void *__iomem linear_tbl0_addr;
-	void *__iomem linear_tbl1_addr;
+	void __iomem *base_addr;
+	void __iomem *linear_tbl0_addr;
+	void __iomem *linear_tbl1_addr;
 };
 
 #define ISIF_PAD_SINK      0
diff --git a/drivers/staging/media/davinci_vpfe/dm365_resizer.c b/drivers/staging/media/davinci_vpfe/dm365_resizer.c
index e0b29c8ca221..75e70e14b724 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_resizer.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_resizer.c
@@ -149,7 +149,7 @@ configure_resizer_out_params(struct vpfe_resizer_device *resizer, int index,
 		param->rsz_en[index] = DISABLE;
 		return;
 	}
-	output = (struct vpfe_rsz_output_spec *)output_spec;
+	output = output_spec;
 	param->rsz_en[index] = ENABLE;
 	if (partial) {
 		param->rsz_rsc_param[index].h_flip = output->h_flip;
@@ -633,7 +633,7 @@ resizer_calculate_normal_f_div_param(struct device *dev, int input_width,
 	if (!(val % 2)) {
 		h1 = val;
 	} else {
-		val = (input_width << 7);
+		val = input_width << 7;
 		val -= rsz >> 1;
 		val /= rsz << 1;
 		val <<= 1;
@@ -1218,12 +1218,12 @@ static long resizer_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
 
 	switch (cmd) {
 	case VIDIOC_VPFE_RSZ_S_CONFIG:
-		user_config = (struct vpfe_rsz_config *)arg;
+		user_config = arg;
 		ret = resizer_set_configuration(resizer, user_config);
 		break;
 
 	case VIDIOC_VPFE_RSZ_G_CONFIG:
-		user_config = (struct vpfe_rsz_config *)arg;
+		user_config = arg;
 		if (!user_config->config) {
 			dev_err(dev, "error in VIDIOC_VPFE_RSZ_G_CONFIG\n");
 			return -EINVAL;
diff --git a/drivers/staging/media/davinci_vpfe/dm365_resizer.h b/drivers/staging/media/davinci_vpfe/dm365_resizer.h
index 59a79422b914..93b0f44030aa 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_resizer.h
+++ b/drivers/staging/media/davinci_vpfe/dm365_resizer.h
@@ -228,7 +228,7 @@ struct vpfe_resizer_device {
 	struct dm365_resizer_device		resizer_a;
 	struct dm365_resizer_device		resizer_b;
 	struct resizer_params			config;
-	void *__iomem base_addr;
+	void __iomem *base_addr;
 };
 
 int vpfe_resizer_init(struct vpfe_resizer_device *vpfe_rsz,
diff --git a/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c b/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
index bf45d2cc5965..a350a20955f1 100644
--- a/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
+++ b/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
@@ -707,7 +707,6 @@ static int vpfe_remove(struct platform_device *pdev)
 static struct platform_driver vpfe_driver = {
 	.driver = {
 		.name = CAPTURE_DRV_NAME,
-		.owner = THIS_MODULE,
 	},
 	.probe = vpfe_probe,
 	.remove = vpfe_remove,
diff --git a/drivers/staging/media/lirc/lirc_bt829.c b/drivers/staging/media/lirc/lirc_bt829.c
index 4c806ba41323..44f565547179 100644
--- a/drivers/staging/media/lirc/lirc_bt829.c
+++ b/drivers/staging/media/lirc/lirc_bt829.c
@@ -56,11 +56,6 @@ static unsigned char do_get_bits(void);
 #define DRIVER_NAME "lirc_bt829"
 
 static bool debug;
-#define dprintk(fmt, args...)						 \
-	do {								 \
-		if (debug)						 \
-			printk(KERN_DEBUG DRIVER_NAME ": "fmt, ## args); \
-	} while (0)
 
 static int atir_minor;
 static phys_addr_t pci_addr_phys;
@@ -101,7 +96,7 @@ static int atir_add_to_buf(void *data, struct lirc_buffer *buf)
 	status = poll_main();
 	key = (status >> 8) & 0xFF;
 	if (status & 0xFF) {
-		dprintk("reading key %02X\n", key);
+		dev_dbg(atir_driver.dev, "reading key %02X\n", key);
 		lirc_buffer_write(buf, &key);
 		return 0;
 	}
@@ -110,13 +105,13 @@ static int atir_add_to_buf(void *data, struct lirc_buffer *buf)
 
 static int atir_set_use_inc(void *data)
 {
-	dprintk("driver is opened\n");
+	dev_dbg(atir_driver.dev, "driver is opened\n");
 	return 0;
 }
 
 static void atir_set_use_dec(void *data)
 {
-	dprintk("driver is closed\n");
+	dev_dbg(atir_driver.dev, "driver is closed\n");
 }
 
 int init_module(void)
@@ -154,7 +149,8 @@ int init_module(void)
 		rc = atir_minor;
 		goto err_unmap;
 	}
-	dprintk("driver is registered on minor %d\n", atir_minor);
+	dev_dbg(atir_driver.dev, "driver is registered on minor %d\n",
+				atir_minor);
 
 	return 0;
 
diff --git a/drivers/staging/media/lirc/lirc_imon.c b/drivers/staging/media/lirc/lirc_imon.c
index 232edd5b1742..9ce7d9990e3e 100644
--- a/drivers/staging/media/lirc/lirc_imon.c
+++ b/drivers/staging/media/lirc/lirc_imon.c
@@ -495,7 +495,7 @@ static int ir_open(void *data)
 	/* prevent races with disconnect */
 	mutex_lock(&driver_lock);
 
-	context = (struct imon_context *)data;
+	context = data;
 
 	/* initial IR protocol decode variables */
 	context->rx.count = 0;
@@ -516,7 +516,7 @@ static void ir_close(void *data)
 {
 	struct imon_context *context;
 
-	context = (struct imon_context *)data;
+	context = data;
 	if (!context) {
 		pr_err("%s: no context for device\n", __func__);
 		return;
@@ -572,29 +572,6 @@ static void submit_data(struct imon_context *context)
 	wake_up(&context->driver->rbuf->wait_poll);
 }
 
-static inline int tv2int(const struct timeval *a, const struct timeval *b)
-{
-	int usecs = 0;
-	int sec   = 0;
-
-	if (b->tv_usec > a->tv_usec) {
-		usecs = 1000000;
-		sec--;
-	}
-
-	usecs += a->tv_usec - b->tv_usec;
-
-	sec += a->tv_sec - b->tv_sec;
-	sec *= 1000;
-	usecs /= 1000;
-	sec += usecs;
-
-	if (sec < 0)
-		sec = 1000;
-
-	return sec;
-}
-
 /**
  * Process the incoming packet
  */
diff --git a/drivers/staging/media/lirc/lirc_parallel.c b/drivers/staging/media/lirc/lirc_parallel.c
index 5156c2181016..19c5c21babf5 100644
--- a/drivers/staging/media/lirc/lirc_parallel.c
+++ b/drivers/staging/media/lirc/lirc_parallel.c
@@ -605,7 +605,6 @@ static struct platform_driver lirc_parallel_driver = {
 	.resume	= lirc_parallel_resume,
 	.driver	= {
 		.name	= LIRC_DRIVER_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/staging/media/lirc/lirc_sasem.c b/drivers/staging/media/lirc/lirc_sasem.c
index 2f0463eb9887..4a268200cbf5 100644
--- a/drivers/staging/media/lirc/lirc_sasem.c
+++ b/drivers/staging/media/lirc/lirc_sasem.c
@@ -488,7 +488,7 @@ static int ir_open(void *data)
 	/* prevent races with disconnect */
 	mutex_lock(&disconnect_lock);
 
-	context = (struct sasem_context *) data;
+	context = data;
 
 	mutex_lock(&context->ctx_lock);
 
@@ -530,7 +530,7 @@ static void ir_close(void *data)
 {
 	struct sasem_context *context;
 
-	context = (struct sasem_context *)data;
+	context = data;
 	if (!context) {
 		pr_err("%s: no context for device\n", __func__);
 		return;
diff --git a/drivers/staging/media/lirc/lirc_serial.c b/drivers/staging/media/lirc/lirc_serial.c
index 181b92bfe25d..eb4ccb8d2a93 100644
--- a/drivers/staging/media/lirc/lirc_serial.c
+++ b/drivers/staging/media/lirc/lirc_serial.c
@@ -1060,7 +1060,6 @@ static struct platform_driver lirc_serial_driver = {
 	.resume		= lirc_serial_resume,
 	.driver		= {
 		.name	= "lirc_serial",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/staging/media/lirc/lirc_sir.c b/drivers/staging/media/lirc/lirc_sir.c
index 2ee55eaf2a53..39f4733fb1ee 100644
--- a/drivers/staging/media/lirc/lirc_sir.c
+++ b/drivers/staging/media/lirc/lirc_sir.c
@@ -140,12 +140,6 @@ static int rx_buf[RBUF_LEN];
 static unsigned int rx_tail, rx_head;
 
 static bool debug;
-#define dprintk(fmt, args...)						\
-	do {								\
-		if (debug)						\
-			printk(KERN_DEBUG LIRC_DRIVER_NAME ": "		\
-				fmt, ## args);				\
-	} while (0)
 
 /* SECTION: Prototypes */
 
@@ -322,7 +316,7 @@ static void add_read_queue(int flag, unsigned long val)
 	unsigned int new_rx_tail;
 	int newval;
 
-	dprintk("add flag %d with val %lu\n", flag, val);
+	pr_debug("add flag %d with val %lu\n", flag, val);
 
 	newval = val & PULSE_MASK;
 
@@ -342,7 +336,7 @@ static void add_read_queue(int flag, unsigned long val)
 	}
 	new_rx_tail = (rx_tail + 1) & (RBUF_LEN - 1);
 	if (new_rx_tail == rx_head) {
-		dprintk("Buffer overrun.\n");
+		pr_debug("Buffer overrun.\n");
 		return;
 	}
 	rx_buf[rx_tail] = newval;
@@ -439,7 +433,8 @@ static void sir_timeout(unsigned long data)
 		outb(UART_FCR_CLEAR_RCVR, io + UART_FCR);
 		/* determine 'virtual' pulse end: */
 		pulse_end = delta(&last_tv, &last_intr_tv);
-		dprintk("timeout add %d for %lu usec\n", last_value, pulse_end);
+		dev_dbg(driver.dev, "timeout add %d for %lu usec\n",
+				    last_value, pulse_end);
 		add_read_queue(last_value, pulse_end);
 		last_value = 0;
 		last_tv = last_intr_tv;
@@ -479,14 +474,15 @@ static irqreturn_t sir_interrupt(int irq, void *dev_id)
 				do_gettimeofday(&curr_tv);
 				deltv = delta(&last_tv, &curr_tv);
 				deltintrtv = delta(&last_intr_tv, &curr_tv);
-				dprintk("t %lu, d %d\n", deltintrtv, (int)data);
+				dev_dbg(driver.dev, "t %lu, d %d\n",
+						    deltintrtv, (int)data);
 				/*
 				 * if nothing came in last X cycles,
 				 * it was gap
 				 */
 				if (deltintrtv > TIME_CONST * threshold) {
 					if (last_value) {
-						dprintk("GAP\n");
+						dev_dbg(driver.dev, "GAP\n");
 						/* simulate signal change */
 						add_read_queue(last_value,
 							       deltv -
@@ -931,7 +927,6 @@ static struct platform_driver lirc_sir_driver = {
 	.remove		= lirc_sir_remove,
 	.driver		= {
 		.name	= "lirc_sir",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/staging/media/lirc/lirc_zilog.c b/drivers/staging/media/lirc/lirc_zilog.c
index 1e15d2cab727..cc872fb4ca68 100644
--- a/drivers/staging/media/lirc/lirc_zilog.c
+++ b/drivers/staging/media/lirc/lirc_zilog.c
@@ -152,23 +152,12 @@ struct tx_data_struct {
 static struct tx_data_struct *tx_data;
 static struct mutex tx_data_lock;
 
-#define zilog_notify(s, args...) printk(KERN_NOTICE KBUILD_MODNAME ": " s, \
-					## args)
-#define zilog_error(s, args...) printk(KERN_ERR KBUILD_MODNAME ": " s, ## args)
-#define zilog_info(s, args...) printk(KERN_INFO KBUILD_MODNAME ": " s, ## args)
 
 /* module parameters */
 static bool debug;	/* debug output */
 static bool tx_only;	/* only handle the IR Tx function */
 static int minor = -1;	/* minor number */
 
-#define dprintk(fmt, args...)						\
-	do {								\
-		if (debug)						\
-			printk(KERN_DEBUG KBUILD_MODNAME ": " fmt,	\
-				 ## args);				\
-	} while (0)
-
 
 /* struct IR reference counting */
 static struct IR *get_ir_device(struct IR *ir, bool ir_devices_lock_held)
@@ -333,7 +322,7 @@ static int add_to_buf(struct IR *ir)
 	struct IR_tx *tx;
 
 	if (lirc_buffer_full(rbuf)) {
-		dprintk("buffer overflow\n");
+		dev_dbg(ir->l.dev, "buffer overflow\n");
 		return -EOVERFLOW;
 	}
 
@@ -379,16 +368,17 @@ static int add_to_buf(struct IR *ir)
 		 */
 		ret = i2c_master_send(rx->c, sendbuf, 1);
 		if (ret != 1) {
-			zilog_error("i2c_master_send failed with %d\n",	ret);
+			dev_err(ir->l.dev, "i2c_master_send failed with %d\n",
+					   ret);
 			if (failures >= 3) {
 				mutex_unlock(&ir->ir_lock);
-				zilog_error("unable to read from the IR chip "
+				dev_err(ir->l.dev, "unable to read from the IR chip "
 					    "after 3 resets, giving up\n");
 				break;
 			}
 
 			/* Looks like the chip crashed, reset it */
-			zilog_error("polling the IR receiver chip failed, "
+			dev_err(ir->l.dev, "polling the IR receiver chip failed, "
 				    "trying reset\n");
 
 			set_current_state(TASK_UNINTERRUPTIBLE);
@@ -415,13 +405,14 @@ static int add_to_buf(struct IR *ir)
 		ret = i2c_master_recv(rx->c, keybuf, sizeof(keybuf));
 		mutex_unlock(&ir->ir_lock);
 		if (ret != sizeof(keybuf)) {
-			zilog_error("i2c_master_recv failed with %d -- "
+			dev_err(ir->l.dev, "i2c_master_recv failed with %d -- "
 				    "keeping last read buffer\n", ret);
 		} else {
 			rx->b[0] = keybuf[3];
 			rx->b[1] = keybuf[4];
 			rx->b[2] = keybuf[5];
-			dprintk("key (0x%02x/0x%02x)\n", rx->b[0], rx->b[1]);
+			dev_dbg(ir->l.dev, "key (0x%02x/0x%02x)\n",
+					   rx->b[0], rx->b[1]);
 		}
 
 		/* key pressed ? */
@@ -472,7 +463,7 @@ static int lirc_thread(void *arg)
 	struct IR *ir = arg;
 	struct lirc_buffer *rbuf = ir->l.rbuf;
 
-	dprintk("poll thread started\n");
+	dev_dbg(ir->l.dev, "poll thread started\n");
 
 	while (!kthread_should_stop()) {
 		set_current_state(TASK_INTERRUPTIBLE);
@@ -500,7 +491,7 @@ static int lirc_thread(void *arg)
 			wake_up_interruptible(&rbuf->wait_poll);
 	}
 
-	dprintk("poll thread ended\n");
+	dev_dbg(ir->l.dev, "poll thread ended\n");
 	return 0;
 }
 
@@ -644,7 +635,7 @@ static int get_key_data(unsigned char *buf,
 	return -EPROTO;
 
 corrupt:
-	zilog_error("firmware is corrupt\n");
+	pr_err("firmware is corrupt\n");
 	return -EFAULT;
 }
 
@@ -662,10 +653,11 @@ static int send_data_block(struct IR_tx *tx, unsigned char *data_block)
 		buf[0] = (unsigned char)(i + 1);
 		for (j = 0; j < tosend; ++j)
 			buf[1 + j] = data_block[i + j];
-		dprintk("%*ph", 5, buf);
+		dev_dbg(tx->ir->l.dev, "%*ph", 5, buf);
 		ret = i2c_master_send(tx->c, buf, tosend + 1);
 		if (ret != tosend + 1) {
-			zilog_error("i2c_master_send failed with %d\n", ret);
+			dev_err(tx->ir->l.dev, "i2c_master_send failed with %d\n",
+					       ret);
 			return ret < 0 ? ret : -EFAULT;
 		}
 		i += tosend;
@@ -689,7 +681,7 @@ static int send_boot_data(struct IR_tx *tx)
 	buf[1] = 0x20;
 	ret = i2c_master_send(tx->c, buf, 2);
 	if (ret != 2) {
-		zilog_error("i2c_master_send failed with %d\n", ret);
+		dev_err(tx->ir->l.dev, "i2c_master_send failed with %d\n", ret);
 		return ret < 0 ? ret : -EFAULT;
 	}
 
@@ -706,21 +698,22 @@ static int send_boot_data(struct IR_tx *tx)
 	}
 
 	if (ret != 1) {
-		zilog_error("i2c_master_send failed with %d\n", ret);
+		dev_err(tx->ir->l.dev, "i2c_master_send failed with %d\n", ret);
 		return ret < 0 ? ret : -EFAULT;
 	}
 
 	/* Here comes the firmware version... (hopefully) */
 	ret = i2c_master_recv(tx->c, buf, 4);
 	if (ret != 4) {
-		zilog_error("i2c_master_recv failed with %d\n", ret);
+		dev_err(tx->ir->l.dev, "i2c_master_recv failed with %d\n", ret);
 		return 0;
 	}
 	if ((buf[0] != 0x80) && (buf[0] != 0xa0)) {
-		zilog_error("unexpected IR TX init response: %02x\n", buf[0]);
+		dev_err(tx->ir->l.dev, "unexpected IR TX init response: %02x\n",
+				       buf[0]);
 		return 0;
 	}
-	zilog_notify("Zilog/Hauppauge IR blaster firmware version "
+	dev_notice(tx->ir->l.dev, "Zilog/Hauppauge IR blaster firmware version "
 		     "%d.%d.%d loaded\n", buf[1], buf[2], buf[3]);
 
 	return 0;
@@ -736,7 +729,7 @@ static void fw_unload_locked(void)
 
 		vfree(tx_data);
 		tx_data = NULL;
-		dprintk("successfully unloaded IR blaster firmware\n");
+		pr_debug("successfully unloaded IR blaster firmware\n");
 	}
 }
 
@@ -766,17 +759,16 @@ static int fw_load(struct IR_tx *tx)
 	/* Request codeset data file */
 	ret = request_firmware(&fw_entry, "haup-ir-blaster.bin", tx->ir->l.dev);
 	if (ret != 0) {
-		zilog_error("firmware haup-ir-blaster.bin not available (%d)\n",
+		dev_err(tx->ir->l.dev, "firmware haup-ir-blaster.bin not available (%d)\n",
 			    ret);
 		ret = ret < 0 ? ret : -EFAULT;
 		goto out;
 	}
-	dprintk("firmware of size %zu loaded\n", fw_entry->size);
+	dev_dbg(tx->ir->l.dev, "firmware of size %zu loaded\n", fw_entry->size);
 
 	/* Parse the file */
 	tx_data = vmalloc(sizeof(*tx_data));
 	if (tx_data == NULL) {
-		zilog_error("out of memory\n");
 		release_firmware(fw_entry);
 		ret = -ENOMEM;
 		goto out;
@@ -786,7 +778,6 @@ static int fw_load(struct IR_tx *tx)
 	/* Copy the data so hotplug doesn't get confused and timeout */
 	tx_data->datap = vmalloc(fw_entry->size);
 	if (tx_data->datap == NULL) {
-		zilog_error("out of memory\n");
 		release_firmware(fw_entry);
 		vfree(tx_data);
 		ret = -ENOMEM;
@@ -801,7 +792,7 @@ static int fw_load(struct IR_tx *tx)
 	if (!read_uint8(&data, tx_data->endp, &version))
 		goto corrupt;
 	if (version != 1) {
-		zilog_error("unsupported code set file version (%u, expected"
+		dev_err(tx->ir->l.dev, "unsupported code set file version (%u, expected"
 			    "1) -- please upgrade to a newer driver",
 			    version);
 		fw_unload_locked();
@@ -818,7 +809,8 @@ static int fw_load(struct IR_tx *tx)
 			      &tx_data->num_code_sets))
 		goto corrupt;
 
-	dprintk("%u IR blaster codesets loaded\n", tx_data->num_code_sets);
+	dev_dbg(tx->ir->l.dev, "%u IR blaster codesets loaded\n",
+			       tx_data->num_code_sets);
 
 	tx_data->code_sets = vmalloc(
 		tx_data->num_code_sets * sizeof(char *));
@@ -882,7 +874,7 @@ static int fw_load(struct IR_tx *tx)
 	goto out;
 
 corrupt:
-	zilog_error("firmware is corrupt\n");
+	dev_err(tx->ir->l.dev, "firmware is corrupt\n");
 	fw_unload_locked();
 	ret = -EFAULT;
 
@@ -902,9 +894,9 @@ static ssize_t read(struct file *filep, char __user *outbuf, size_t n,
 	unsigned int m;
 	DECLARE_WAITQUEUE(wait, current);
 
-	dprintk("read called\n");
+	dev_dbg(ir->l.dev, "read called\n");
 	if (n % rbuf->chunk_size) {
-		dprintk("read result = -EINVAL\n");
+		dev_dbg(ir->l.dev, "read result = -EINVAL\n");
 		return -EINVAL;
 	}
 
@@ -948,7 +940,7 @@ static ssize_t read(struct file *filep, char __user *outbuf, size_t n,
 			unsigned char buf[MAX_XFER_SIZE];
 
 			if (rbuf->chunk_size > sizeof(buf)) {
-				zilog_error("chunk_size is too big (%d)!\n",
+				dev_err(ir->l.dev, "chunk_size is too big (%d)!\n",
 					    rbuf->chunk_size);
 				ret = -EINVAL;
 				break;
@@ -962,7 +954,7 @@ static ssize_t read(struct file *filep, char __user *outbuf, size_t n,
 				retries++;
 			}
 			if (retries >= 5) {
-				zilog_error("Buffer read failed!\n");
+				dev_err(ir->l.dev, "Buffer read failed!\n");
 				ret = -EIO;
 			}
 		}
@@ -972,7 +964,8 @@ static ssize_t read(struct file *filep, char __user *outbuf, size_t n,
 	put_ir_rx(rx, false);
 	set_current_state(TASK_RUNNING);
 
-	dprintk("read result = %d (%s)\n", ret, ret ? "Error" : "OK");
+	dev_dbg(ir->l.dev, "read result = %d (%s)\n",
+			   ret, ret ? "Error" : "OK");
 
 	return ret ? ret : written;
 }
@@ -988,7 +981,7 @@ static int send_code(struct IR_tx *tx, unsigned int code, unsigned int key)
 	ret = get_key_data(data_block, code, key);
 
 	if (ret == -EPROTO) {
-		zilog_error("failed to get data for code %u, key %u -- check "
+		dev_err(tx->ir->l.dev, "failed to get data for code %u, key %u -- check "
 			    "lircd.conf entries\n", code, key);
 		return ret;
 	} else if (ret != 0)
@@ -1004,7 +997,7 @@ static int send_code(struct IR_tx *tx, unsigned int code, unsigned int key)
 	buf[1] = 0x40;
 	ret = i2c_master_send(tx->c, buf, 2);
 	if (ret != 2) {
-		zilog_error("i2c_master_send failed with %d\n", ret);
+		dev_err(tx->ir->l.dev, "i2c_master_send failed with %d\n", ret);
 		return ret < 0 ? ret : -EFAULT;
 	}
 
@@ -1017,18 +1010,18 @@ static int send_code(struct IR_tx *tx, unsigned int code, unsigned int key)
 	}
 
 	if (ret != 1) {
-		zilog_error("i2c_master_send failed with %d\n", ret);
+		dev_err(tx->ir->l.dev, "i2c_master_send failed with %d\n", ret);
 		return ret < 0 ? ret : -EFAULT;
 	}
 
 	/* Send finished download? */
 	ret = i2c_master_recv(tx->c, buf, 1);
 	if (ret != 1) {
-		zilog_error("i2c_master_recv failed with %d\n", ret);
+		dev_err(tx->ir->l.dev, "i2c_master_recv failed with %d\n", ret);
 		return ret < 0 ? ret : -EFAULT;
 	}
 	if (buf[0] != 0xA0) {
-		zilog_error("unexpected IR TX response #1: %02x\n",
+		dev_err(tx->ir->l.dev, "unexpected IR TX response #1: %02x\n",
 			buf[0]);
 		return -EFAULT;
 	}
@@ -1038,7 +1031,7 @@ static int send_code(struct IR_tx *tx, unsigned int code, unsigned int key)
 	buf[1] = 0x80;
 	ret = i2c_master_send(tx->c, buf, 2);
 	if (ret != 2) {
-		zilog_error("i2c_master_send failed with %d\n", ret);
+		dev_err(tx->ir->l.dev, "i2c_master_send failed with %d\n", ret);
 		return ret < 0 ? ret : -EFAULT;
 	}
 
@@ -1048,7 +1041,7 @@ static int send_code(struct IR_tx *tx, unsigned int code, unsigned int key)
 	 * going to skip this whole mess and say we're done on the HD PVR
 	 */
 	if (!tx->post_tx_ready_poll) {
-		dprintk("sent code %u, key %u\n", code, key);
+		dev_dbg(tx->ir->l.dev, "sent code %u, key %u\n", code, key);
 		return 0;
 	}
 
@@ -1064,11 +1057,11 @@ static int send_code(struct IR_tx *tx, unsigned int code, unsigned int key)
 		ret = i2c_master_send(tx->c, buf, 1);
 		if (ret == 1)
 			break;
-		dprintk("NAK expected: i2c_master_send "
+		dev_dbg(tx->ir->l.dev, "NAK expected: i2c_master_send "
 			"failed with %d (try %d)\n", ret, i+1);
 	}
 	if (ret != 1) {
-		zilog_error("IR TX chip never got ready: last i2c_master_send "
+		dev_err(tx->ir->l.dev, "IR TX chip never got ready: last i2c_master_send "
 			    "failed with %d\n", ret);
 		return ret < 0 ? ret : -EFAULT;
 	}
@@ -1076,16 +1069,17 @@ static int send_code(struct IR_tx *tx, unsigned int code, unsigned int key)
 	/* Seems to be an 'ok' response */
 	i = i2c_master_recv(tx->c, buf, 1);
 	if (i != 1) {
-		zilog_error("i2c_master_recv failed with %d\n", ret);
+		dev_err(tx->ir->l.dev, "i2c_master_recv failed with %d\n", ret);
 		return -EFAULT;
 	}
 	if (buf[0] != 0x80) {
-		zilog_error("unexpected IR TX response #2: %02x\n", buf[0]);
+		dev_err(tx->ir->l.dev, "unexpected IR TX response #2: %02x\n",
+				       buf[0]);
 		return -EFAULT;
 	}
 
 	/* Oh good, it worked */
-	dprintk("sent code %u, key %u\n", code, key);
+	dev_dbg(tx->ir->l.dev, "sent code %u, key %u\n", code, key);
 	return 0;
 }
 
@@ -1171,11 +1165,11 @@ static ssize_t write(struct file *filep, const char __user *buf, size_t n,
 		 */
 		if (ret != 0) {
 			/* Looks like the chip crashed, reset it */
-			zilog_error("sending to the IR transmitter chip "
+			dev_err(tx->ir->l.dev, "sending to the IR transmitter chip "
 				    "failed, trying reset\n");
 
 			if (failures >= 3) {
-				zilog_error("unable to send to the IR chip "
+				dev_err(tx->ir->l.dev, "unable to send to the IR chip "
 					    "after 3 resets, giving up\n");
 				mutex_unlock(&ir->ir_lock);
 				mutex_unlock(&tx->client_lock);
@@ -1210,7 +1204,7 @@ static unsigned int poll(struct file *filep, poll_table *wait)
 	struct lirc_buffer *rbuf = ir->l.rbuf;
 	unsigned int ret;
 
-	dprintk("poll called\n");
+	dev_dbg(ir->l.dev, "poll called\n");
 
 	rx = get_ir_rx(ir);
 	if (rx == NULL) {
@@ -1218,7 +1212,7 @@ static unsigned int poll(struct file *filep, poll_table *wait)
 		 * Revisit this, if our poll function ever reports writeable
 		 * status for Tx
 		 */
-		dprintk("poll result = POLLERR\n");
+		dev_dbg(ir->l.dev, "poll result = POLLERR\n");
 		return POLLERR;
 	}
 
@@ -1231,7 +1225,8 @@ static unsigned int poll(struct file *filep, poll_table *wait)
 	/* Indicate what ops could happen immediately without blocking */
 	ret = lirc_buffer_empty(rbuf) ? 0 : (POLLIN|POLLRDNORM);
 
-	dprintk("poll result = %s\n", ret ? "POLLIN|POLLRDNORM" : "none");
+	dev_dbg(ir->l.dev, "poll result = %s\n",
+			   ret ? "POLLIN|POLLRDNORM" : "none");
 	return ret;
 }
 
@@ -1338,7 +1333,7 @@ static int close(struct inode *node, struct file *filep)
 	struct IR *ir = filep->private_data;
 
 	if (ir == NULL) {
-		zilog_error("close: no private_data attached to the file!\n");
+		dev_err(ir->l.dev, "close: no private_data attached to the file!\n");
 		return -ENODEV;
 	}
 
@@ -1450,7 +1445,7 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	int ret;
 	bool tx_probe = false;
 
-	dprintk("%s: %s on i2c-%d (%s), client addr=0x%02x\n",
+	dev_dbg(&client->dev, "%s: %s on i2c-%d (%s), client addr=0x%02x\n",
 		__func__, id->name, adap->nr, adap->name, client->addr);
 
 	/*
@@ -1463,7 +1458,7 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	else if (tx_only) /* module option */
 		return -ENXIO;
 
-	zilog_info("probing IR %s on %s (i2c-%d)\n",
+	pr_info("probing IR %s on %s (i2c-%d)\n",
 		   tx_probe ? "Tx" : "Rx", adap->name, adap->nr);
 
 	mutex_lock(&ir_devices_lock);
@@ -1545,7 +1540,7 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 
 		/* Proceed only if the Rx client is also ready or not needed */
 		if (rx == NULL && !tx_only) {
-			zilog_info("probe of IR Tx on %s (i2c-%d) done. Waiting"
+			dev_info(tx->ir->l.dev, "probe of IR Tx on %s (i2c-%d) done. Waiting"
 				   " on IR Rx.\n", adap->name, adap->nr);
 			goto out_ok;
 		}
@@ -1584,7 +1579,7 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 				       "zilog-rx-i2c-%d", adap->nr);
 		if (IS_ERR(rx->task)) {
 			ret = PTR_ERR(rx->task);
-			zilog_error("%s: could not start IR Rx polling thread"
+			dev_err(tx->ir->l.dev, "%s: could not start IR Rx polling thread"
 				    "\n", __func__);
 			/* Failed kthread, so put back the ir ref */
 			put_ir_device(ir, true);
@@ -1597,7 +1592,7 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 
 		/* Proceed only if the Tx client is also ready */
 		if (tx == NULL) {
-			zilog_info("probe of IR Rx on %s (i2c-%d) done. Waiting"
+			pr_info("probe of IR Rx on %s (i2c-%d) done. Waiting"
 				   " on IR Tx.\n", adap->name, adap->nr);
 			goto out_ok;
 		}
@@ -1607,12 +1602,12 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	ir->l.minor = minor; /* module option: user requested minor number */
 	ir->l.minor = lirc_register_driver(&ir->l);
 	if (ir->l.minor < 0 || ir->l.minor >= MAX_IRCTL_DEVICES) {
-		zilog_error("%s: \"minor\" must be between 0 and %d (%d)!\n",
+		dev_err(tx->ir->l.dev, "%s: \"minor\" must be between 0 and %d (%d)!\n",
 			    __func__, MAX_IRCTL_DEVICES-1, ir->l.minor);
 		ret = -EBADRQC;
 		goto out_put_xx;
 	}
-	zilog_info("IR unit on %s (i2c-%d) registered as lirc%d and ready\n",
+	dev_info(ir->l.dev, "IR unit on %s (i2c-%d) registered as lirc%d and ready\n",
 		   adap->name, adap->nr, ir->l.minor);
 
 out_ok:
@@ -1621,7 +1616,7 @@ out_ok:
 	if (tx != NULL)
 		put_ir_tx(tx, true);
 	put_ir_device(ir, true);
-	zilog_info("probe of IR %s on %s (i2c-%d) done\n",
+	dev_info(ir->l.dev, "probe of IR %s on %s (i2c-%d) done\n",
 		   tx_probe ? "Tx" : "Rx", adap->name, adap->nr);
 	mutex_unlock(&ir_devices_lock);
 	return 0;
@@ -1634,7 +1629,7 @@ out_put_xx:
 out_put_ir:
 	put_ir_device(ir, true);
 out_no_ir:
-	zilog_error("%s: probing IR %s on %s (i2c-%d) failed with %d\n",
+	dev_err(&client->dev, "%s: probing IR %s on %s (i2c-%d) failed with %d\n",
 		    __func__, tx_probe ? "Tx" : "Rx", adap->name, adap->nr,
 		   ret);
 	mutex_unlock(&ir_devices_lock);
@@ -1645,7 +1640,7 @@ static int __init zilog_init(void)
 {
 	int ret;
 
-	zilog_notify("Zilog/Hauppauge IR driver initializing\n");
+	pr_notice("Zilog/Hauppauge IR driver initializing\n");
 
 	mutex_init(&tx_data_lock);
 
@@ -1653,9 +1648,9 @@ static int __init zilog_init(void)
 
 	ret = i2c_add_driver(&driver);
 	if (ret)
-		zilog_error("initialization failed\n");
+		pr_err("initialization failed\n");
 	else
-		zilog_notify("initialization complete\n");
+		pr_notice("initialization complete\n");
 
 	return ret;
 }
@@ -1665,7 +1660,7 @@ static void __exit zilog_exit(void)
 	i2c_del_driver(&driver);
 	/* if loaded */
 	fw_unload();
-	zilog_notify("Zilog/Hauppauge IR driver unloaded\n");
+	pr_notice("Zilog/Hauppauge IR driver unloaded\n");
 }
 
 module_init(zilog_init);
diff --git a/drivers/staging/media/omap4iss/iss.c b/drivers/staging/media/omap4iss/iss.c
index d548371db65a..cc1dfadd91eb 100644
--- a/drivers/staging/media/omap4iss/iss.c
+++ b/drivers/staging/media/omap4iss/iss.c
@@ -1357,10 +1357,8 @@ static int iss_probe(struct platform_device *pdev)
 		return -EINVAL;
 
 	iss = devm_kzalloc(&pdev->dev, sizeof(*iss), GFP_KERNEL);
-	if (!iss) {
-		dev_err(&pdev->dev, "Could not allocate memory\n");
+	if (!iss)
 		return -ENOMEM;
-	}
 
 	mutex_init(&iss->iss_mutex);
 
@@ -1476,7 +1474,6 @@ static struct platform_driver iss_driver = {
 	.remove		= iss_remove,
 	.id_table	= omap4iss_id_table,
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= "omap4iss",
 	},
 };
diff --git a/drivers/staging/media/omap4iss/iss_csi2.c b/drivers/staging/media/omap4iss/iss_csi2.c
index 7dbf68cd3566..21971c675b8c 100644
--- a/drivers/staging/media/omap4iss/iss_csi2.c
+++ b/drivers/staging/media/omap4iss/iss_csi2.c
@@ -1231,7 +1231,7 @@ static int csi2_init_entities(struct iss_csi2_device *csi2, const char *subname)
 
 	v4l2_subdev_init(sd, &csi2_ops);
 	sd->internal_ops = &csi2_internal_ops;
-	sprintf(name, "CSI2%s", subname);
+	snprintf(name, sizeof(name), "CSI2%s", subname);
 	snprintf(sd->name, sizeof(sd->name), "OMAP4 ISS %s", name);
 
 	sd->grp_id = 1 << 16;	/* group ID for iss subdevs */
diff --git a/drivers/media/parport/Kconfig b/drivers/staging/media/parport/Kconfig
index 948c981d9f05..15974efdba1d 100644
--- a/drivers/media/parport/Kconfig
+++ b/drivers/staging/media/parport/Kconfig
@@ -7,18 +7,22 @@ menuconfig MEDIA_PARPORT_SUPPORT
 
 if MEDIA_PARPORT_SUPPORT
 config VIDEO_BWQCAM
-	tristate "Quickcam BW Video For Linux"
+	tristate "Quickcam BW Video For Linux (Deprecated)"
 	depends on PARPORT && VIDEO_V4L2
 	select VIDEOBUF2_VMALLOC
 	help
 	  Say Y have if you the black and white version of the QuickCam
 	  camera. See the next option for the color version.
 
+	  This driver is deprecated and will be removed soon. If you have
+	  hardware for this and you want to work on this driver, then contact
+	  the linux-media mailinglist.
+
 	  To compile this driver as a module, choose M here: the
 	  module will be called bw-qcam.
 
 config VIDEO_CQCAM
-	tristate "QuickCam Colour Video For Linux"
+	tristate "QuickCam Colour Video For Linux (Deprecated)"
 	depends on PARPORT && VIDEO_V4L2
 	help
 	  This is the video4linux driver for the colour version of the
@@ -28,18 +32,26 @@ config VIDEO_CQCAM
 	  as a module (c-qcam).
 	  Read <file:Documentation/video4linux/CQcam.txt> for more information.
 
+	  This driver is deprecated and will be removed soon. If you have
+	  hardware for this and you want to work on this driver, then contact
+	  the linux-media mailinglist.
+
 config VIDEO_PMS
-	tristate "Mediavision Pro Movie Studio Video For Linux"
+	tristate "Mediavision Pro Movie Studio Video For Linux (Deprecated)"
 	depends on ISA && VIDEO_V4L2
 	help
 	  Say Y if you have the ISA Mediavision Pro Movie Studio
 	  capture card.
 
+	  This driver is deprecated and will be removed soon. If you have
+	  hardware for this and you want to work on this driver, then contact
+	  the linux-media mailinglist.
+
 	  To compile this driver as a module, choose M here: the
 	  module will be called pms.
 
 config VIDEO_W9966
-	tristate "W9966CF Webcam (FlyCam Supra and others) Video For Linux"
+	tristate "W9966CF Webcam (FlyCam Supra and others) Video For Linux (Deprecated)"
 	depends on PARPORT_1284 && PARPORT && VIDEO_V4L2
 	help
 	  Video4linux driver for Winbond's w9966 based Webcams.
@@ -50,4 +62,8 @@ config VIDEO_W9966
 
 	  Check out <file:Documentation/video4linux/w9966.txt> for more
 	  information.
+
+	  This driver is deprecated and will be removed soon. If you have
+	  hardware for this and you want to work on this driver, then contact
+	  the linux-media mailinglist.
 endif
diff --git a/drivers/media/parport/Makefile b/drivers/staging/media/parport/Makefile
index 4eea06d7af5b..4eea06d7af5b 100644
--- a/drivers/media/parport/Makefile
+++ b/drivers/staging/media/parport/Makefile
diff --git a/drivers/media/parport/bw-qcam.c b/drivers/staging/media/parport/bw-qcam.c
index 67b9da1dc43f..67b9da1dc43f 100644
--- a/drivers/media/parport/bw-qcam.c
+++ b/drivers/staging/media/parport/bw-qcam.c
diff --git a/drivers/media/parport/c-qcam.c b/drivers/staging/media/parport/c-qcam.c
index b9010bd3ed3e..b9010bd3ed3e 100644
--- a/drivers/media/parport/c-qcam.c
+++ b/drivers/staging/media/parport/c-qcam.c
diff --git a/drivers/media/parport/pms.c b/drivers/staging/media/parport/pms.c
index e6b497528cea..e6b497528cea 100644
--- a/drivers/media/parport/pms.c
+++ b/drivers/staging/media/parport/pms.c
diff --git a/drivers/media/parport/w9966.c b/drivers/staging/media/parport/w9966.c
index f7502f3a6a3c..f7502f3a6a3c 100644
--- a/drivers/media/parport/w9966.c
+++ b/drivers/staging/media/parport/w9966.c
diff --git a/drivers/media/usb/tlg2300/Kconfig b/drivers/staging/media/tlg2300/Kconfig
index 645d915267e6..81784c6f7b88 100644
--- a/drivers/media/usb/tlg2300/Kconfig
+++ b/drivers/staging/media/tlg2300/Kconfig
@@ -1,5 +1,5 @@
 config VIDEO_TLG2300
-	tristate "Telegent TLG2300 USB video capture support"
+	tristate "Telegent TLG2300 USB video capture support (Deprecated)"
 	depends on VIDEO_DEV && I2C && SND && DVB_CORE
 	select VIDEO_TUNER
 	select VIDEO_TVEEPROM
@@ -12,5 +12,9 @@ config VIDEO_TLG2300
 	  This is a video4linux driver for Telegent tlg2300 based TV cards.
 	  The driver supports V4L2, DVB-T and radio.
 
+	  This driver is deprecated and will be removed soon. If you have
+	  hardware for this and you want to work on this driver, then contact
+	  the linux-media mailinglist.
+
 	  To compile this driver as a module, choose M here: the
 	  module will be called poseidon
diff --git a/drivers/media/usb/tlg2300/Makefile b/drivers/staging/media/tlg2300/Makefile
index 137f8e38cdec..137f8e38cdec 100644
--- a/drivers/media/usb/tlg2300/Makefile
+++ b/drivers/staging/media/tlg2300/Makefile
diff --git a/drivers/media/usb/tlg2300/pd-alsa.c b/drivers/staging/media/tlg2300/pd-alsa.c
index dd8fe100590f..dd8fe100590f 100644
--- a/drivers/media/usb/tlg2300/pd-alsa.c
+++ b/drivers/staging/media/tlg2300/pd-alsa.c
diff --git a/drivers/media/usb/tlg2300/pd-common.h b/drivers/staging/media/tlg2300/pd-common.h
index 9e23ad32d2fe..9e23ad32d2fe 100644
--- a/drivers/media/usb/tlg2300/pd-common.h
+++ b/drivers/staging/media/tlg2300/pd-common.h
diff --git a/drivers/media/usb/tlg2300/pd-dvb.c b/drivers/staging/media/tlg2300/pd-dvb.c
index ca4994a5190c..ca4994a5190c 100644
--- a/drivers/media/usb/tlg2300/pd-dvb.c
+++ b/drivers/staging/media/tlg2300/pd-dvb.c
diff --git a/drivers/media/usb/tlg2300/pd-main.c b/drivers/staging/media/tlg2300/pd-main.c
index b31f4791b8ff..b31f4791b8ff 100644
--- a/drivers/media/usb/tlg2300/pd-main.c
+++ b/drivers/staging/media/tlg2300/pd-main.c
diff --git a/drivers/media/usb/tlg2300/pd-radio.c b/drivers/staging/media/tlg2300/pd-radio.c
index b391194a840c..b391194a840c 100644
--- a/drivers/media/usb/tlg2300/pd-radio.c
+++ b/drivers/staging/media/tlg2300/pd-radio.c
diff --git a/drivers/media/usb/tlg2300/pd-video.c b/drivers/staging/media/tlg2300/pd-video.c
index 8cd7f02fcf9f..8cd7f02fcf9f 100644
--- a/drivers/media/usb/tlg2300/pd-video.c
+++ b/drivers/staging/media/tlg2300/pd-video.c
diff --git a/drivers/media/usb/tlg2300/vendorcmds.h b/drivers/staging/media/tlg2300/vendorcmds.h
index ba6f4ae3b2c2..ba6f4ae3b2c2 100644
--- a/drivers/media/usb/tlg2300/vendorcmds.h
+++ b/drivers/staging/media/tlg2300/vendorcmds.h
diff --git a/drivers/staging/media/vino/Kconfig b/drivers/staging/media/vino/Kconfig
new file mode 100644
index 000000000000..03700dadafd8
--- /dev/null
+++ b/drivers/staging/media/vino/Kconfig
@@ -0,0 +1,24 @@
+config VIDEO_VINO
+	tristate "SGI Vino Video For Linux (Deprecated)"
+	depends on I2C && SGI_IP22 && VIDEO_V4L2
+	select VIDEO_SAA7191 if MEDIA_SUBDRV_AUTOSELECT
+	help
+	  Say Y here to build in support for the Vino video input system found
+	  on SGI Indy machines.
+
+	  This driver is deprecated and will be removed soon. If you have
+	  hardware for this and you want to work on this driver, then contact
+	  the linux-media mailinglist.
+
+config VIDEO_SAA7191
+	tristate "Philips SAA7191 video decoder (Deprecated)"
+	depends on VIDEO_V4L2 && I2C
+	---help---
+	  Support for the Philips SAA7191 video decoder.
+
+	  This driver is deprecated and will be removed soon. If you have
+	  hardware for this and you want to work on this driver, then contact
+	  the linux-media mailinglist.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called saa7191.
diff --git a/drivers/staging/media/vino/Makefile b/drivers/staging/media/vino/Makefile
new file mode 100644
index 000000000000..914c2513687c
--- /dev/null
+++ b/drivers/staging/media/vino/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_VIDEO_VINO) += indycam.o
+obj-$(CONFIG_VIDEO_VINO) += vino.o
+obj-$(CONFIG_VIDEO_SAA7191) += saa7191.o
diff --git a/drivers/media/platform/indycam.c b/drivers/staging/media/vino/indycam.c
index f1d192bbcb4c..f1d192bbcb4c 100644
--- a/drivers/media/platform/indycam.c
+++ b/drivers/staging/media/vino/indycam.c
diff --git a/drivers/media/platform/indycam.h b/drivers/staging/media/vino/indycam.h
index 881f21c474c4..881f21c474c4 100644
--- a/drivers/media/platform/indycam.h
+++ b/drivers/staging/media/vino/indycam.h
diff --git a/drivers/media/i2c/saa7191.c b/drivers/staging/media/vino/saa7191.c
index 8e9699268a63..8e9699268a63 100644
--- a/drivers/media/i2c/saa7191.c
+++ b/drivers/staging/media/vino/saa7191.c
diff --git a/drivers/media/i2c/saa7191.h b/drivers/staging/media/vino/saa7191.h
index 803c74d6066f..803c74d6066f 100644
--- a/drivers/media/i2c/saa7191.h
+++ b/drivers/staging/media/vino/saa7191.h
diff --git a/drivers/media/platform/vino.c b/drivers/staging/media/vino/vino.c
index 2c85357f774d..2c85357f774d 100644
--- a/drivers/media/platform/vino.c
+++ b/drivers/staging/media/vino/vino.c
diff --git a/drivers/media/platform/vino.h b/drivers/staging/media/vino/vino.h
index de2d615ae7c9..de2d615ae7c9 100644
--- a/drivers/media/platform/vino.h
+++ b/drivers/staging/media/vino/vino.h
diff --git a/drivers/staging/netlogic/xlr_net.c b/drivers/staging/netlogic/xlr_net.c
index ad19ba9df3c7..5ecb3e6a5bb3 100644
--- a/drivers/staging/netlogic/xlr_net.c
+++ b/drivers/staging/netlogic/xlr_net.c
@@ -1133,7 +1133,6 @@ static struct platform_driver xlr_net_driver = {
 	.remove		= xlr_net_remove,
 	.driver		= {
 		.name	= "xlr-net",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/staging/nvec/nvec.c b/drivers/staging/nvec/nvec.c
index a93208adbfcf..093535c6217b 100644
--- a/drivers/staging/nvec/nvec.c
+++ b/drivers/staging/nvec/nvec.c
@@ -973,7 +973,6 @@ static struct platform_driver nvec_device_driver = {
 	.remove  = tegra_nvec_remove,
 	.driver  = {
 		.name = "nvec",
-		.owner = THIS_MODULE,
 		.pm = &nvec_pm_ops,
 		.of_match_table = nvidia_nvec_of_match,
 	}
diff --git a/drivers/staging/nvec/nvec_kbd.c b/drivers/staging/nvec/nvec_kbd.c
index c17a1c3eb3ca..e881e6b26a4c 100644
--- a/drivers/staging/nvec/nvec_kbd.c
+++ b/drivers/staging/nvec/nvec_kbd.c
@@ -181,7 +181,6 @@ static struct platform_driver nvec_kbd_driver = {
 	.remove = nvec_kbd_remove,
 	.driver = {
 		.name = "nvec-kbd",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/staging/nvec/nvec_paz00.c b/drivers/staging/nvec/nvec_paz00.c
index e2e675a6e95a..f0cea0e43c96 100644
--- a/drivers/staging/nvec/nvec_paz00.c
+++ b/drivers/staging/nvec/nvec_paz00.c
@@ -87,7 +87,6 @@ static struct platform_driver nvec_paz00_driver = {
 	.remove = nvec_paz00_remove,
 	.driver = {
 		.name  = "nvec-paz00",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/staging/nvec/nvec_power.c b/drivers/staging/nvec/nvec_power.c
index 6446e151866f..6a1459d4f8fb 100644
--- a/drivers/staging/nvec/nvec_power.c
+++ b/drivers/staging/nvec/nvec_power.c
@@ -432,7 +432,6 @@ static struct platform_driver nvec_power_driver = {
 	.remove = nvec_power_remove,
 	.driver = {
 		   .name = "nvec-power",
-		   .owner = THIS_MODULE,
 		   }
 };
 
diff --git a/drivers/staging/nvec/nvec_ps2.c b/drivers/staging/nvec/nvec_ps2.c
index f56f1db15bad..4fd63c239454 100644
--- a/drivers/staging/nvec/nvec_ps2.c
+++ b/drivers/staging/nvec/nvec_ps2.c
@@ -177,7 +177,6 @@ static struct platform_driver nvec_mouse_driver = {
 	.remove = nvec_mouse_remove,
 	.driver = {
 		.name = "nvec-mouse",
-		.owner = THIS_MODULE,
 		.pm = &nvec_mouse_pm_ops,
 	},
 };
diff --git a/drivers/staging/octeon-usb/octeon-hcd.c b/drivers/staging/octeon-usb/octeon-hcd.c
index 5f9db4cbb381..6b8b108c4e6d 100644
--- a/drivers/staging/octeon-usb/octeon-hcd.c
+++ b/drivers/staging/octeon-usb/octeon-hcd.c
@@ -456,6 +456,16 @@ struct octeon_temp_buffer {
 	u8 data[0];
 };
 
+static inline struct octeon_hcd *cvmx_usb_to_octeon(struct cvmx_usb_state *p)
+{
+	return container_of(p, struct octeon_hcd, usb);
+}
+
+static inline struct usb_hcd *octeon_to_hcd(struct octeon_hcd *p)
+{
+	return container_of((void *)p, struct usb_hcd, hcd_priv);
+}
+
 /**
  * octeon_alloc_temp_buffer - allocate a temporary buffer for USB transfer
  *                            (if needed)
@@ -640,8 +650,7 @@ static inline int __cvmx_usb_get_data_pid(struct cvmx_usb_pipe *pipe)
 {
 	if (pipe->pid_toggle)
 		return 2; /* Data1 */
-	else
-		return 0; /* Data0 */
+	return 0; /* Data0 */
 }
 
 /**
@@ -744,7 +753,7 @@ static int cvmx_usb_initialize(struct cvmx_usb_state *usb,
 	 *     such that USB is as close as possible to 125Mhz
 	 */
 	{
-		int divisor = (octeon_get_clock_rate()+125000000-1)/125000000;
+		int divisor = DIV_ROUND_UP(octeon_get_clock_rate(), 125000000);
 		/* Lower than 4 doesn't seem to work properly */
 		if (divisor < 4)
 			divisor = 4;
@@ -1328,7 +1337,8 @@ static void __cvmx_usb_poll_rx_fifo(struct cvmx_usb_state *usb)
 
 	/* Loop writing the FIFO data for this packet into memory */
 	while (bytes > 0) {
-		*ptr++ = __cvmx_usb_read_csr32(usb, USB_FIFO_ADDRESS(channel, usb->index));
+		*ptr++ = __cvmx_usb_read_csr32(usb,
+				USB_FIFO_ADDRESS(channel, usb->index));
 		bytes -= 4;
 	}
 	CVMX_SYNCW;
@@ -1479,7 +1489,8 @@ static void __cvmx_usb_fill_tx_fifo(struct cvmx_usb_state *usb, int channel)
 		fifo = &usb->nonperiodic;
 
 	fifo->entry[fifo->head].channel = channel;
-	fifo->entry[fifo->head].address = __cvmx_usb_read_csr64(usb, CVMX_USBNX_DMA0_OUTB_CHN0(usb->index) + channel*8);
+	fifo->entry[fifo->head].address = __cvmx_usb_read_csr64(usb,
+			CVMX_USBNX_DMA0_OUTB_CHN0(usb->index) + channel*8);
 	fifo->entry[fifo->head].size = (usbc_hctsiz.s.xfersize+3)>>2;
 	fifo->head++;
 	if (fifo->head > MAX_CHANNELS)
@@ -1501,6 +1512,9 @@ static void __cvmx_usb_start_channel_control(struct cvmx_usb_state *usb,
 					     int channel,
 					     struct cvmx_usb_pipe *pipe)
 {
+	struct octeon_hcd *priv = cvmx_usb_to_octeon(usb);
+	struct usb_hcd *hcd = octeon_to_hcd(priv);
+	struct device *dev = hcd->self.controller;
 	struct cvmx_usb_transaction *transaction =
 		list_first_entry(&pipe->transactions, typeof(*transaction),
 				 node);
@@ -1517,7 +1531,7 @@ static void __cvmx_usb_start_channel_control(struct cvmx_usb_state *usb,
 	switch (transaction->stage) {
 	case CVMX_USB_STAGE_NON_CONTROL:
 	case CVMX_USB_STAGE_NON_CONTROL_SPLIT_COMPLETE:
-		cvmx_dprintf("%s: ERROR - Non control stage\n", __func__);
+		dev_err(dev, "%s: ERROR - Non control stage\n", __func__);
 		break;
 	case CVMX_USB_STAGE_SETUP:
 		usbc_hctsiz.s.pid = 3; /* Setup */
@@ -1607,8 +1621,8 @@ static void __cvmx_usb_start_channel_control(struct cvmx_usb_state *usb,
 	 * Calculate the number of packets to transfer. If the length is zero
 	 * we still need to transfer one packet
 	 */
-	packets_to_transfer = (bytes_to_transfer + pipe->max_packet - 1) /
-		pipe->max_packet;
+	packets_to_transfer = DIV_ROUND_UP(bytes_to_transfer,
+					   pipe->max_packet);
 	if (packets_to_transfer == 0)
 		packets_to_transfer = 1;
 	else if ((packets_to_transfer > 1) &&
@@ -1700,7 +1714,9 @@ static void __cvmx_usb_start_channel(struct cvmx_usb_state *usb,
 			usbc_hcintmsk.s.stallmsk = 1;
 			usbc_hcintmsk.s.xfercomplmsk = 1;
 		}
-		__cvmx_usb_write_csr32(usb, CVMX_USBCX_HCINTMSKX(channel, usb->index), usbc_hcintmsk.u32);
+		__cvmx_usb_write_csr32(usb,
+				CVMX_USBCX_HCINTMSKX(channel, usb->index),
+				usbc_hcintmsk.u32);
 
 		/* Enable the channel interrupt to propagate */
 		usbc_haintmsk.u32 = __cvmx_usb_read_csr32(usb,
@@ -1853,8 +1869,7 @@ static void __cvmx_usb_start_channel(struct cvmx_usb_state *usb,
 		 * zero we still need to transfer one packet
 		 */
 		packets_to_transfer =
-			(bytes_to_transfer + pipe->max_packet - 1) /
-			pipe->max_packet;
+			DIV_ROUND_UP(bytes_to_transfer, pipe->max_packet);
 		if (packets_to_transfer == 0)
 			packets_to_transfer = 1;
 		else if ((packets_to_transfer > 1) &&
@@ -2110,16 +2125,6 @@ done:
 			union cvmx_usbcx_gintmsk, sofmsk, need_sof);
 }
 
-static inline struct octeon_hcd *cvmx_usb_to_octeon(struct cvmx_usb_state *p)
-{
-	return container_of(p, struct octeon_hcd, usb);
-}
-
-static inline struct usb_hcd *octeon_to_hcd(struct octeon_hcd *p)
-{
-	return container_of((void *)p, struct usb_hcd, hcd_priv);
-}
-
 static void octeon_usb_urb_complete_callback(struct cvmx_usb_state *usb,
 					     enum cvmx_usb_complete status,
 					     struct cvmx_usb_pipe *pipe,
@@ -2583,6 +2588,9 @@ static int cvmx_usb_get_frame_number(struct cvmx_usb_state *usb)
  */
 static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 {
+	struct octeon_hcd *priv = cvmx_usb_to_octeon(usb);
+	struct usb_hcd *hcd = octeon_to_hcd(priv);
+	struct device *dev = hcd->self.controller;
 	union cvmx_usbcx_hcintx usbc_hcint;
 	union cvmx_usbcx_hctsizx usbc_hctsiz;
 	union cvmx_usbcx_hccharx usbc_hcchar;
@@ -2640,8 +2648,8 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 				 * Channel halt isn't needed.
 				 */
 			} else {
-				cvmx_dprintf("USB%d: Channel %d interrupt without halt\n",
-						usb->index, channel);
+				dev_err(dev, "USB%d: Channel %d interrupt without halt\n",
+					usb->index, channel);
 				return 0;
 			}
 		}
@@ -2881,9 +2889,11 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 					struct usb_ctrlrequest *header =
 						cvmx_phys_to_ptr(transaction->control_header);
 					if (header->wLength)
-						transaction->stage = CVMX_USB_STAGE_DATA;
+						transaction->stage =
+							CVMX_USB_STAGE_DATA;
 					else
-						transaction->stage = CVMX_USB_STAGE_STATUS;
+						transaction->stage =
+							CVMX_USB_STAGE_STATUS;
 				}
 				break;
 			case CVMX_USB_STAGE_SETUP_SPLIT_COMPLETE:
@@ -2891,9 +2901,11 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 					struct usb_ctrlrequest *header =
 						cvmx_phys_to_ptr(transaction->control_header);
 					if (header->wLength)
-						transaction->stage = CVMX_USB_STAGE_DATA;
+						transaction->stage =
+							CVMX_USB_STAGE_DATA;
 					else
-						transaction->stage = CVMX_USB_STAGE_STATUS;
+						transaction->stage =
+							CVMX_USB_STAGE_STATUS;
 				}
 				break;
 			case CVMX_USB_STAGE_DATA:
@@ -3015,7 +3027,8 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 				 * is complete, the pipe sleeps until the next
 				 * schedule interval
 				 */
-				if (pipe->transfer_dir == CVMX_USB_DIRECTION_OUT) {
+				if (pipe->transfer_dir ==
+					CVMX_USB_DIRECTION_OUT) {
 					/*
 					 * If no space left or this wasn't a max
 					 * size packet then this transfer is
@@ -3882,7 +3895,6 @@ static struct of_device_id octeon_usb_match[] = {
 static struct platform_driver octeon_usb_driver = {
 	.driver = {
 		.name       = "OcteonUSB",
-		.owner		= THIS_MODULE,
 		.of_match_table = octeon_usb_match,
 	},
 	.probe      = octeon_usb_probe,
diff --git a/drivers/staging/octeon/ethernet-rx.c b/drivers/staging/octeon/ethernet-rx.c
index b2b6c3cd2bed..fcbe836aa997 100644
--- a/drivers/staging/octeon/ethernet-rx.c
+++ b/drivers/staging/octeon/ethernet-rx.c
@@ -61,66 +61,7 @@
 
 #include <asm/octeon/cvmx-gmxx-defs.h>
 
-struct cvm_napi_wrapper {
-	struct napi_struct napi;
-} ____cacheline_aligned_in_smp;
-
-static struct cvm_napi_wrapper cvm_oct_napi[NR_CPUS] __cacheline_aligned_in_smp;
-
-struct cvm_oct_core_state {
-	int baseline_cores;
-	/*
-	 * The number of additional cores that could be processing
-	 * input packets.
-	 */
-	atomic_t available_cores;
-	cpumask_t cpu_state;
-} ____cacheline_aligned_in_smp;
-
-static struct cvm_oct_core_state core_state __cacheline_aligned_in_smp;
-
-static int cvm_irq_cpu;
-
-static void cvm_oct_enable_napi(void *_)
-{
-	int cpu = smp_processor_id();
-	napi_schedule(&cvm_oct_napi[cpu].napi);
-}
-
-static void cvm_oct_enable_one_cpu(void)
-{
-	int v;
-	int cpu;
-
-	/* Check to see if more CPUs are available for receive processing... */
-	v = atomic_sub_if_positive(1, &core_state.available_cores);
-	if (v < 0)
-		return;
-
-	/* ... if a CPU is available, Turn on NAPI polling for that CPU.  */
-	for_each_online_cpu(cpu) {
-		if (!cpu_test_and_set(cpu, core_state.cpu_state)) {
-			v = smp_call_function_single(cpu, cvm_oct_enable_napi,
-						     NULL, 0);
-			if (v)
-				panic("Can't enable NAPI.");
-			break;
-		}
-	}
-}
-
-static void cvm_oct_no_more_work(void)
-{
-	int cpu = smp_processor_id();
-
-	if (cpu == cvm_irq_cpu) {
-		enable_irq(OCTEON_IRQ_WORKQ0 + pow_receive_group);
-		return;
-	}
-
-	cpu_clear(cpu, core_state.cpu_state);
-	atomic_add(1, &core_state.available_cores);
-}
+static struct napi_struct cvm_oct_napi;
 
 /**
  * cvm_oct_do_interrupt - interrupt handler.
@@ -132,8 +73,7 @@ static irqreturn_t cvm_oct_do_interrupt(int cpl, void *dev_id)
 {
 	/* Disable the IRQ and start napi_poll. */
 	disable_irq_nosync(OCTEON_IRQ_WORKQ0 + pow_receive_group);
-	cvm_irq_cpu = smp_processor_id();
-	cvm_oct_enable_napi(NULL);
+	napi_schedule(&cvm_oct_napi);
 
 	return IRQ_HANDLED;
 }
@@ -186,13 +126,15 @@ static inline int cvm_oct_check_rcv_error(cvmx_wqe_t *work)
 
 			if (*ptr == 0xd5) {
 				/*
-				  printk_ratelimited("Port %d received 0xd5 preamble\n", work->ipprt);
+				  printk_ratelimited("Port %d received 0xd5 preamble\n",
+					  work->ipprt);
 				 */
 				work->packet_ptr.s.addr += i + 1;
 				work->len -= i + 5;
 			} else if ((*ptr & 0xf) == 0xd) {
 				/*
-				  printk_ratelimited("Port %d received 0x?d preamble\n", work->ipprt);
+				  printk_ratelimited("Port %d received 0x?d preamble\n",
+					  work->ipprt);
 				 */
 				work->packet_ptr.s.addr += i;
 				work->len -= i + 4;
@@ -278,28 +220,15 @@ static int cvm_oct_napi_poll(struct napi_struct *napi, int budget)
 			cvmx_write_csr(CVMX_POW_WQ_INT, wq_int.u64);
 			break;
 		}
-		pskb = (struct sk_buff **)(cvm_oct_get_buffer_ptr(work->packet_ptr) - sizeof(void *));
+		pskb = (struct sk_buff **)(cvm_oct_get_buffer_ptr(work->packet_ptr) -
+			sizeof(void *));
 		prefetch(pskb);
 
 		if (USE_ASYNC_IOBDMA && rx_count < (budget - 1)) {
-			cvmx_pow_work_request_async_nocheck(CVMX_SCR_SCRATCH, CVMX_POW_NO_WAIT);
+			cvmx_pow_work_request_async_nocheck(CVMX_SCR_SCRATCH,
+							    CVMX_POW_NO_WAIT);
 			did_work_request = 1;
 		}
-
-		if (rx_count == 0) {
-			/*
-			 * First time through, see if there is enough
-			 * work waiting to merit waking another
-			 * CPU.
-			 */
-			union cvmx_pow_wq_int_cntx counts;
-			int backlog;
-			int cores_in_use = core_state.baseline_cores - atomic_read(&core_state.available_cores);
-			counts.u64 = cvmx_read_csr(CVMX_POW_WQ_INT_CNTX(pow_receive_group));
-			backlog = counts.s.iq_cnt + counts.s.ds_cnt;
-			if (backlog > budget * cores_in_use && napi != NULL)
-				cvm_oct_enable_one_cpu();
-		}
 		rx_count++;
 
 		skb_in_hw = USE_SKBUFFS_IN_HW && work->word2.s.bufs == 1;
@@ -322,7 +251,8 @@ static int cvm_oct_napi_poll(struct napi_struct *napi, int budget)
 		 * buffer.
 		 */
 		if (likely(skb_in_hw)) {
-			skb->data = skb->head + work->packet_ptr.s.addr - cvmx_ptr_to_phys(skb->head);
+			skb->data = skb->head + work->packet_ptr.s.addr -
+				cvmx_ptr_to_phys(skb->head);
 			prefetch(skb->data);
 			skb->len = work->len;
 			skb_set_tail_pointer(skb, skb->len);
@@ -359,7 +289,8 @@ static int cvm_oct_napi_poll(struct napi_struct *napi, int budget)
 				/* No packet buffers to free */
 			} else {
 				int segments = work->word2.s.bufs;
-				union cvmx_buf_ptr segment_ptr = work->packet_ptr;
+				union cvmx_buf_ptr segment_ptr =
+				    work->packet_ptr;
 				int len = work->len;
 
 				while (segments--) {
@@ -375,8 +306,11 @@ static int cvm_oct_napi_poll(struct napi_struct *napi, int budget)
 			 * one: int segment_size =
 			 * segment_ptr.s.size;
 			 */
-					int segment_size = CVMX_FPA_PACKET_POOL_SIZE -
-						(segment_ptr.s.addr - (((segment_ptr.s.addr >> 7) - segment_ptr.s.back) << 7));
+					int segment_size =
+					    CVMX_FPA_PACKET_POOL_SIZE -
+					    (segment_ptr.s.addr -
+					     (((segment_ptr.s.addr >> 7) -
+					       segment_ptr.s.back) << 7));
 					/*
 					 * Don't copy more than what
 					 * is left in the packet.
@@ -407,8 +341,10 @@ static int cvm_oct_napi_poll(struct napi_struct *napi, int budget)
 				skb->protocol = eth_type_trans(skb, dev);
 				skb->dev = dev;
 
-				if (unlikely(work->word2.s.not_IP || work->word2.s.IP_exc ||
-					work->word2.s.L4_error || !work->word2.s.tcp_or_udp))
+				if (unlikely(work->word2.s.not_IP ||
+					     work->word2.s.IP_exc ||
+					     work->word2.s.L4_error ||
+					     !work->word2.s.tcp_or_udp))
 					skb->ip_summed = CHECKSUM_NONE;
 				else
 					skb->ip_summed = CHECKSUM_UNNECESSARY;
@@ -416,11 +352,15 @@ static int cvm_oct_napi_poll(struct napi_struct *napi, int budget)
 				/* Increment RX stats for virtual ports */
 				if (work->ipprt >= CVMX_PIP_NUM_INPUT_PORTS) {
 #ifdef CONFIG_64BIT
-					atomic64_add(1, (atomic64_t *)&priv->stats.rx_packets);
-					atomic64_add(skb->len, (atomic64_t *)&priv->stats.rx_bytes);
+					atomic64_add(1,
+						     (atomic64_t *)&priv->stats.rx_packets);
+					atomic64_add(skb->len,
+						     (atomic64_t *)&priv->stats.rx_bytes);
 #else
-					atomic_add(1, (atomic_t *)&priv->stats.rx_packets);
-					atomic_add(skb->len, (atomic_t *)&priv->stats.rx_bytes);
+					atomic_add(1,
+						   (atomic_t *)&priv->stats.rx_packets);
+					atomic_add(skb->len,
+						   (atomic_t *)&priv->stats.rx_bytes);
 #endif
 				}
 				netif_receive_skb(skb);
@@ -431,9 +371,11 @@ static int cvm_oct_napi_poll(struct napi_struct *napi, int budget)
 					   dev->name);
 				*/
 #ifdef CONFIG_64BIT
-				atomic64_add(1, (atomic64_t *)&priv->stats.rx_dropped);
+				atomic64_add(1,
+					     (atomic64_t *)&priv->stats.rx_dropped);
 #else
-				atomic_add(1, (atomic_t *)&priv->stats.rx_dropped);
+				atomic_add(1,
+					   (atomic_t *)&priv->stats.rx_dropped);
 #endif
 				dev_kfree_skb_irq(skb);
 			}
@@ -476,7 +418,7 @@ static int cvm_oct_napi_poll(struct napi_struct *napi, int budget)
 	if (rx_count < budget && napi != NULL) {
 		/* No more work */
 		napi_complete(napi);
-		cvm_oct_no_more_work();
+		enable_irq(OCTEON_IRQ_WORKQ0 + pow_receive_group);
 	}
 	return rx_count;
 }
@@ -511,18 +453,10 @@ void cvm_oct_rx_initialize(void)
 	if (NULL == dev_for_napi)
 		panic("No net_devices were allocated.");
 
-	if (max_rx_cpus >= 1 && max_rx_cpus < num_online_cpus())
-		atomic_set(&core_state.available_cores, max_rx_cpus);
-	else
-		atomic_set(&core_state.available_cores, num_online_cpus());
-	core_state.baseline_cores = atomic_read(&core_state.available_cores);
-
-	core_state.cpu_state = CPU_MASK_NONE;
-	for_each_possible_cpu(i) {
-		netif_napi_add(dev_for_napi, &cvm_oct_napi[i].napi,
-			       cvm_oct_napi_poll, rx_napi_weight);
-		napi_enable(&cvm_oct_napi[i].napi);
-	}
+	netif_napi_add(dev_for_napi, &cvm_oct_napi, cvm_oct_napi_poll,
+		       rx_napi_weight);
+	napi_enable(&cvm_oct_napi);
+
 	/* Register an IRQ handler to receive POW interrupts */
 	i = request_irq(OCTEON_IRQ_WORKQ0 + pow_receive_group,
 			cvm_oct_do_interrupt, 0, "Ethernet", cvm_oct_device);
@@ -543,15 +477,11 @@ void cvm_oct_rx_initialize(void)
 	int_pc.s.pc_thr = 5;
 	cvmx_write_csr(CVMX_POW_WQ_INT_PC, int_pc.u64);
 
-
-	/* Scheduld NAPI now.  This will indirectly enable interrupts. */
-	cvm_oct_enable_one_cpu();
+	/* Schedule NAPI now. This will indirectly enable the interrupt. */
+	napi_schedule(&cvm_oct_napi);
 }
 
 void cvm_oct_rx_shutdown(void)
 {
-	int i;
-	/* Shutdown all of the NAPIs */
-	for_each_possible_cpu(i)
-		netif_napi_del(&cvm_oct_napi[i].napi);
+	netif_napi_del(&cvm_oct_napi);
 }
diff --git a/drivers/staging/octeon/ethernet-tx.c b/drivers/staging/octeon/ethernet-tx.c
index 4e54d8540219..b7a7854d3f7e 100644
--- a/drivers/staging/octeon/ethernet-tx.c
+++ b/drivers/staging/octeon/ethernet-tx.c
@@ -77,6 +77,7 @@ static DECLARE_TASKLET(cvm_oct_tx_cleanup_tasklet, cvm_oct_tx_do_cleanup, 0);
 static inline int32_t cvm_oct_adjust_skb_to_free(int32_t skb_to_free, int fau)
 {
 	int32_t undo;
+
 	undo = skb_to_free > 0 ? MAX_SKB_TO_FREE : skb_to_free +
 						   MAX_SKB_TO_FREE;
 	if (undo > 0)
@@ -89,6 +90,7 @@ static inline int32_t cvm_oct_adjust_skb_to_free(int32_t skb_to_free, int fau)
 static void cvm_oct_kick_tx_poll_watchdog(void)
 {
 	union cvmx_ciu_timx ciu_timx;
+
 	ciu_timx.u64 = 0;
 	ciu_timx.s.one_shot = 1;
 	ciu_timx.s.len = cvm_oct_tx_poll_interval;
@@ -118,9 +120,11 @@ static void cvm_oct_free_tx_skbs(struct net_device *dev)
 		total_freed += skb_to_free;
 		if (skb_to_free > 0) {
 			struct sk_buff *to_free_list = NULL;
+
 			spin_lock_irqsave(&priv->tx_free_list[qos].lock, flags);
 			while (skb_to_free > 0) {
 				struct sk_buff *t;
+
 				t = __skb_dequeue(&priv->tx_free_list[qos]);
 				t->next = to_free_list;
 				to_free_list = t;
@@ -131,6 +135,7 @@ static void cvm_oct_free_tx_skbs(struct net_device *dev)
 			/* Do the actual freeing outside of the lock. */
 			while (to_free_list) {
 				struct sk_buff *t = to_free_list;
+
 				to_free_list = to_free_list->next;
 				dev_kfree_skb_any(t);
 			}
@@ -258,6 +263,7 @@ int cvm_oct_xmit(struct sk_buff *skb, struct net_device *dev)
 			    cvmx_read_csr(CVMX_GMXX_PRTX_CFG(index, interface));
 			if (gmx_prt_cfg.s.duplex == 0) {
 				int add_bytes = 64 - skb->len;
+
 				if ((skb_tail_pointer(skb) + add_bytes) <=
 				    skb_end_pointer(skb))
 					memset(__skb_put(skb, add_bytes), 0,
@@ -289,6 +295,7 @@ int cvm_oct_xmit(struct sk_buff *skb, struct net_device *dev)
 		CVM_OCT_SKB_CB(skb)[0] = hw_buffer.u64;
 		for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
 			struct skb_frag_struct *fs = skb_shinfo(skb)->frags + i;
+
 			hw_buffer.s.addr = XKPHYS_TO_PHYS(
 				(u64)(page_address(fs->page.p) +
 				fs->page_offset));
@@ -495,6 +502,7 @@ skip_xmit:
 
 	while (skb_to_free > 0) {
 		struct sk_buff *t = __skb_dequeue(&priv->tx_free_list[qos]);
+
 		t->next = to_free_list;
 		to_free_list = t;
 		skb_to_free--;
@@ -505,6 +513,7 @@ skip_xmit:
 	/* Do the actual freeing outside of the lock. */
 	while (to_free_list) {
 		struct sk_buff *t = to_free_list;
+
 		to_free_list = to_free_list->next;
 		dev_kfree_skb_any(t);
 	}
@@ -550,6 +559,7 @@ int cvm_oct_xmit_pow(struct sk_buff *skb, struct net_device *dev)
 
 	/* Get a work queue entry */
 	cvmx_wqe_t *work = cvmx_fpa_alloc(CVMX_FPA_WQE_POOL);
+
 	if (unlikely(work == NULL)) {
 		printk_ratelimited("%s: Failed to allocate a work queue entry\n",
 				   dev->name);
@@ -713,6 +723,7 @@ static void cvm_oct_tx_do_cleanup(unsigned long arg)
 	for (port = 0; port < TOTAL_NUMBER_OF_PORTS; port++) {
 		if (cvm_oct_device[port]) {
 			struct net_device *dev = cvm_oct_device[port];
+
 			cvm_oct_free_tx_skbs(dev);
 		}
 	}
diff --git a/drivers/staging/octeon/ethernet.c b/drivers/staging/octeon/ethernet.c
index 8f9e3fb4871d..ee321496dcdd 100644
--- a/drivers/staging/octeon/ethernet.c
+++ b/drivers/staging/octeon/ethernet.c
@@ -98,12 +98,6 @@ MODULE_PARM_DESC(pow_send_list, "\n"
 	"\t\"eth2,spi3,spi7\" would cause these three devices to transmit\n"
 	"\tusing the pow_send_group.");
 
-int max_rx_cpus = -1;
-module_param(max_rx_cpus, int, 0444);
-MODULE_PARM_DESC(max_rx_cpus, "\n"
-	"\t\tThe maximum number of CPUs to use for packet reception.\n"
-	"\t\tUse -1 to use all available CPUs.");
-
 int rx_napi_weight = 32;
 module_param(rx_napi_weight, int, 0444);
 MODULE_PARM_DESC(rx_napi_weight, "The NAPI WEIGHT parameter.");
@@ -452,7 +446,7 @@ int cvm_oct_common_init(struct net_device *dev)
 		mac = of_get_mac_address(priv->of_node);
 
 	if (mac)
-		memcpy(dev->dev_addr, mac, ETH_ALEN);
+		ether_addr_copy(dev->dev_addr, mac);
 	else
 		eth_hw_addr_random(dev);
 
@@ -877,7 +871,6 @@ static struct platform_driver cvm_oct_driver = {
 	.probe		= cvm_oct_probe,
 	.remove		= cvm_oct_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= KBUILD_MODNAME,
 		.of_match_table = cvm_oct_match,
 	},
diff --git a/drivers/staging/octeon/octeon-ethernet.h b/drivers/staging/octeon/octeon-ethernet.h
index d0e321119914..f48dc766fada 100644
--- a/drivers/staging/octeon/octeon-ethernet.h
+++ b/drivers/staging/octeon/octeon-ethernet.h
@@ -99,7 +99,6 @@ extern struct workqueue_struct *cvm_oct_poll_queue;
 extern atomic_t cvm_oct_poll_queue_stopping;
 extern u64 cvm_oct_tx_poll_interval;
 
-extern int max_rx_cpus;
 extern int rx_napi_weight;
 
 #endif
diff --git a/drivers/staging/olpc_dcon/olpc_dcon.c b/drivers/staging/olpc_dcon/olpc_dcon.c
index eb83b28b8cd1..6a9a8815477c 100644
--- a/drivers/staging/olpc_dcon/olpc_dcon.c
+++ b/drivers/staging/olpc_dcon/olpc_dcon.c
@@ -682,8 +682,7 @@ static int dcon_remove(struct i2c_client *client)
 
 	free_irq(DCON_IRQ, dcon);
 
-	if (dcon->bl_dev)
-		backlight_device_unregister(dcon->bl_dev);
+	backlight_device_unregister(dcon->bl_dev);
 
 	if (dcon_device != NULL)
 		platform_device_unregister(dcon_device);
diff --git a/drivers/staging/ozwpan/ozhcd.c b/drivers/staging/ozwpan/ozhcd.c
index e88045228607..8543bb29a138 100644
--- a/drivers/staging/ozwpan/ozhcd.c
+++ b/drivers/staging/ozwpan/ozhcd.c
@@ -224,7 +224,6 @@ static struct platform_driver g_oz_plat_drv = {
 	.resume = oz_plat_resume,
 	.driver = {
 		.name = OZ_PLAT_DEV_NAME,
-		.owner = THIS_MODULE,
 	},
 };
 
@@ -354,8 +353,7 @@ static void oz_complete_urb(struct usb_hcd *hcd, struct urb *urb,
 	}
 	spin_lock(&g_tasklet_lock);
 	spin_unlock_irqrestore(&g_tasklet_lock, irq_state);
-	if (cancel_urbl)
-		oz_free_urb_link(cancel_urbl);
+	oz_free_urb_link(cancel_urbl);
 }
 
 /*
@@ -523,8 +521,7 @@ static int oz_dequeue_ep_urb(struct oz_port *port, u8 ep_addr, int in_dir,
 		}
 	}
 	spin_unlock_bh(&port->ozhcd->hcd_lock);
-	if (urbl)
-		oz_free_urb_link(urbl);
+	oz_free_urb_link(urbl);
 	return urbl ? 0 : -EIDRM;
 }
 
@@ -730,7 +727,7 @@ void oz_hcd_pd_reset(void *hpd, void *hport)
 {
 	/* Cleanup the current configuration and report reset to the core.
 	 */
-	struct oz_port *port = (struct oz_port *)hport;
+	struct oz_port *port = hport;
 	struct oz_hcd *ozhcd = port->ozhcd;
 
 	oz_dbg(ON, "PD Reset\n");
@@ -749,7 +746,7 @@ void oz_hcd_pd_reset(void *hpd, void *hport)
 void oz_hcd_get_desc_cnf(void *hport, u8 req_id, int status, const u8 *desc,
 			int length, int offset, int total_size)
 {
-	struct oz_port *port = (struct oz_port *)hport;
+	struct oz_port *port = hport;
 	struct urb *urb;
 	int err = 0;
 
@@ -889,7 +886,7 @@ static void oz_hcd_complete_set_interface(struct oz_port *port, struct urb *urb,
 void oz_hcd_control_cnf(void *hport, u8 req_id, u8 rcode, const u8 *data,
 	int data_len)
 {
-	struct oz_port *port = (struct oz_port *)hport;
+	struct oz_port *port = hport;
 	struct urb *urb;
 	struct usb_ctrlrequest *setup;
 	struct usb_hcd *hcd = port->ozhcd->hcd;
@@ -1036,7 +1033,7 @@ static inline int oz_usb_get_frame_number(void)
 int oz_hcd_heartbeat(void *hport)
 {
 	int rc = 0;
-	struct oz_port *port = (struct oz_port *)hport;
+	struct oz_port *port = hport;
 	struct oz_hcd *ozhcd = port->ozhcd;
 	struct oz_urb_link *urbl, *n;
 	LIST_HEAD(xfr_list);
@@ -1914,7 +1911,7 @@ static void oz_get_hub_descriptor(struct usb_hcd *hcd,
 	memset(desc, 0, sizeof(*desc));
 	desc->bDescriptorType = 0x29;
 	desc->bDescLength = 9;
-	desc->wHubCharacteristics = (__force __u16)cpu_to_le16(0x0001);
+	desc->wHubCharacteristics = cpu_to_le16(0x0001);
 	desc->bNbrPorts = OZ_NB_PORTS;
 }
 
@@ -2032,11 +2029,11 @@ static int oz_clear_port_feature(struct usb_hcd *hcd, u16 wvalue, u16 windex)
 		break;
 	case USB_PORT_FEAT_C_CONNECTION:
 		oz_dbg(HUB, "USB_PORT_FEAT_C_CONNECTION\n");
-		clear_bits = (USB_PORT_STAT_C_CONNECTION << 16);
+		clear_bits = USB_PORT_STAT_C_CONNECTION << 16;
 		break;
 	case USB_PORT_FEAT_C_ENABLE:
 		oz_dbg(HUB, "USB_PORT_FEAT_C_ENABLE\n");
-		clear_bits = (USB_PORT_STAT_C_ENABLE << 16);
+		clear_bits = USB_PORT_STAT_C_ENABLE << 16;
 		break;
 	case USB_PORT_FEAT_C_SUSPEND:
 		oz_dbg(HUB, "USB_PORT_FEAT_C_SUSPEND\n");
@@ -2046,7 +2043,7 @@ static int oz_clear_port_feature(struct usb_hcd *hcd, u16 wvalue, u16 windex)
 		break;
 	case USB_PORT_FEAT_C_RESET:
 		oz_dbg(HUB, "USB_PORT_FEAT_C_RESET\n");
-		clear_bits = (USB_PORT_FEAT_C_RESET << 16);
+		clear_bits = USB_PORT_FEAT_C_RESET << 16;
 		break;
 	case USB_PORT_FEAT_TEST:
 		oz_dbg(HUB, "USB_PORT_FEAT_TEST\n");
diff --git a/drivers/staging/ozwpan/ozusbsvc1.c b/drivers/staging/ozwpan/ozusbsvc1.c
index be7ee01c50ab..d434d8c6fff6 100644
--- a/drivers/staging/ozwpan/ozusbsvc1.c
+++ b/drivers/staging/ozwpan/ozusbsvc1.c
@@ -56,7 +56,7 @@ static int oz_usb_submit_elt(struct oz_elt_buf *eb, struct oz_elt_info *ei,
 int oz_usb_get_desc_req(void *hpd, u8 req_id, u8 req_type, u8 desc_type,
 	u8 index, __le16 windex, int offset, int len)
 {
-	struct oz_usb_ctx *usb_ctx = (struct oz_usb_ctx *)hpd;
+	struct oz_usb_ctx *usb_ctx = hpd;
 	struct oz_pd *pd = usb_ctx->pd;
 	struct oz_elt *elt;
 	struct oz_get_desc_req *body;
@@ -92,7 +92,7 @@ int oz_usb_get_desc_req(void *hpd, u8 req_id, u8 req_type, u8 desc_type,
  */
 static int oz_usb_set_config_req(void *hpd, u8 req_id, u8 index)
 {
-	struct oz_usb_ctx *usb_ctx = (struct oz_usb_ctx *)hpd;
+	struct oz_usb_ctx *usb_ctx = hpd;
 	struct oz_pd *pd = usb_ctx->pd;
 	struct oz_elt *elt;
 	struct oz_elt_buf *eb = &pd->elt_buff;
@@ -115,7 +115,7 @@ static int oz_usb_set_config_req(void *hpd, u8 req_id, u8 index)
  */
 static int oz_usb_set_interface_req(void *hpd, u8 req_id, u8 index, u8 alt)
 {
-	struct oz_usb_ctx *usb_ctx = (struct oz_usb_ctx *)hpd;
+	struct oz_usb_ctx *usb_ctx = hpd;
 	struct oz_pd *pd = usb_ctx->pd;
 	struct oz_elt *elt;
 	struct oz_elt_buf *eb = &pd->elt_buff;
@@ -140,7 +140,7 @@ static int oz_usb_set_interface_req(void *hpd, u8 req_id, u8 index, u8 alt)
 static int oz_usb_set_clear_feature_req(void *hpd, u8 req_id, u8 type,
 			u8 recipient, u8 index, __le16 feature)
 {
-	struct oz_usb_ctx *usb_ctx = (struct oz_usb_ctx *)hpd;
+	struct oz_usb_ctx *usb_ctx = hpd;
 	struct oz_pd *pd = usb_ctx->pd;
 	struct oz_elt *elt;
 	struct oz_elt_buf *eb = &pd->elt_buff;
@@ -166,7 +166,7 @@ static int oz_usb_set_clear_feature_req(void *hpd, u8 req_id, u8 type,
 static int oz_usb_vendor_class_req(void *hpd, u8 req_id, u8 req_type,
 	u8 request, __le16 value, __le16 index, const u8 *data, int data_len)
 {
-	struct oz_usb_ctx *usb_ctx = (struct oz_usb_ctx *)hpd;
+	struct oz_usb_ctx *usb_ctx = hpd;
 	struct oz_pd *pd = usb_ctx->pd;
 	struct oz_elt *elt;
 	struct oz_elt_buf *eb = &pd->elt_buff;
@@ -244,7 +244,7 @@ int oz_usb_control_req(void *hpd, u8 req_id, struct usb_ctrlrequest *setup,
  */
 int oz_usb_send_isoc(void *hpd, u8 ep_num, struct urb *urb)
 {
-	struct oz_usb_ctx *usb_ctx = (struct oz_usb_ctx *)hpd;
+	struct oz_usb_ctx *usb_ctx = hpd;
 	struct oz_pd *pd = usb_ctx->pd;
 	struct oz_elt_buf *eb;
 	int i;
diff --git a/drivers/staging/panel/TODO b/drivers/staging/panel/TODO
index a4be749bcdfc..2db3f994b632 100644
--- a/drivers/staging/panel/TODO
+++ b/drivers/staging/panel/TODO
@@ -1,6 +1,5 @@
 TODO:
 	- checkpatch.pl cleanups
-	- Lindent
 	- review major/minor usages
 	- review userspace api
 	- see if all of this could be easier done in userspace instead.
diff --git a/drivers/staging/panel/panel.c b/drivers/staging/panel/panel.c
index 6d1a32097d3c..98325b7b4462 100644
--- a/drivers/staging/panel/panel.c
+++ b/drivers/staging/panel/panel.c
@@ -133,6 +133,8 @@
 #define LCD_ESCAPE_LEN		24	/* max chars for LCD escape command */
 #define LCD_ESCAPE_CHAR	27	/* use char 27 for escape command */
 
+#define NOT_SET			-1
+
 /* macros to simplify use of the parallel port */
 #define r_ctr(x)        (parport_read_control((x)->port))
 #define r_dtr(x)        (parport_read_data((x)->port))
@@ -210,6 +212,10 @@ static pmask_t phys_prev;
 static char inputs_stable;
 
 /* these variables are specific to the keypad */
+static struct {
+	bool enabled;
+} keypad;
+
 static char keypad_buffer[KEYPAD_BUFFER];
 static int keypad_buflen;
 static int keypad_start;
@@ -217,17 +223,50 @@ static char keypressed;
 static wait_queue_head_t keypad_read_wait;
 
 /* lcd-specific variables */
-
-/* contains the LCD config state */
-static unsigned long int lcd_flags;
-/* contains the LCD X offset */
-static unsigned long int lcd_addr_x;
-/* contains the LCD Y offset */
-static unsigned long int lcd_addr_y;
-/* current escape sequence, 0 terminated */
-static char lcd_escape[LCD_ESCAPE_LEN + 1];
-/* not in escape state. >=0 = escape cmd len */
-static int lcd_escape_len = -1;
+static struct {
+	bool enabled;
+	bool initialized;
+	bool must_clear;
+
+	/* TODO: use bool here? */
+	char left_shift;
+
+	int height;
+	int width;
+	int bwidth;
+	int hwidth;
+	int charset;
+	int proto;
+	int light_tempo;
+
+	/* TODO: use union here? */
+	struct {
+		int e;
+		int rs;
+		int rw;
+		int cl;
+		int da;
+		int bl;
+	} pins;
+
+	/* contains the LCD config state */
+	unsigned long int flags;
+
+	/* Contains the LCD X and Y offset */
+	struct {
+		unsigned long int x;
+		unsigned long int y;
+	} addr;
+
+	/* Current escape sequence and it's length or -1 if outside */
+	struct {
+		char buf[LCD_ESCAPE_LEN + 1];
+		int len;
+	} esc_seq;
+} lcd;
+
+/* Needed only for init */
+static int selected_lcd_type = NOT_SET;
 
 /*
  * Bit masks to convert LCD signals to parallel port outputs.
@@ -302,14 +341,15 @@ static unsigned char lcd_bits[LCD_PORTS][LCD_BITS][BIT_STATES];
 /*
  * Construct custom config from the kernel's configuration
  */
-#define DEFAULT_PROFILE         PANEL_PROFILE_LARGE
 #define DEFAULT_PARPORT         0
-#define DEFAULT_LCD             LCD_TYPE_OLD
-#define DEFAULT_KEYPAD          KEYPAD_TYPE_OLD
+#define DEFAULT_PROFILE         PANEL_PROFILE_LARGE
+#define DEFAULT_KEYPAD_TYPE     KEYPAD_TYPE_OLD
+#define DEFAULT_LCD_TYPE        LCD_TYPE_OLD
+#define DEFAULT_LCD_HEIGHT      2
 #define DEFAULT_LCD_WIDTH       40
 #define DEFAULT_LCD_BWIDTH      40
 #define DEFAULT_LCD_HWIDTH      64
-#define DEFAULT_LCD_HEIGHT      2
+#define DEFAULT_LCD_CHARSET     LCD_CHARSET_NORMAL
 #define DEFAULT_LCD_PROTO       LCD_PROTO_PARALLEL
 
 #define DEFAULT_LCD_PIN_E       PIN_AUTOLF
@@ -318,27 +358,31 @@ static unsigned char lcd_bits[LCD_PORTS][LCD_BITS][BIT_STATES];
 #define DEFAULT_LCD_PIN_SCL     PIN_STROBE
 #define DEFAULT_LCD_PIN_SDA     PIN_D0
 #define DEFAULT_LCD_PIN_BL      PIN_NOT_SET
-#define DEFAULT_LCD_CHARSET     LCD_CHARSET_NORMAL
-
-#ifdef CONFIG_PANEL_PROFILE
-#undef DEFAULT_PROFILE
-#define DEFAULT_PROFILE CONFIG_PANEL_PROFILE
-#endif
 
 #ifdef CONFIG_PANEL_PARPORT
 #undef DEFAULT_PARPORT
 #define DEFAULT_PARPORT CONFIG_PANEL_PARPORT
 #endif
 
+#ifdef CONFIG_PANEL_PROFILE
+#undef DEFAULT_PROFILE
+#define DEFAULT_PROFILE CONFIG_PANEL_PROFILE
+#endif
+
 #if DEFAULT_PROFILE == 0	/* custom */
 #ifdef CONFIG_PANEL_KEYPAD
-#undef DEFAULT_KEYPAD
-#define DEFAULT_KEYPAD CONFIG_PANEL_KEYPAD
+#undef DEFAULT_KEYPAD_TYPE
+#define DEFAULT_KEYPAD_TYPE CONFIG_PANEL_KEYPAD
 #endif
 
 #ifdef CONFIG_PANEL_LCD
-#undef DEFAULT_LCD
-#define DEFAULT_LCD CONFIG_PANEL_LCD
+#undef DEFAULT_LCD_TYPE
+#define DEFAULT_LCD_TYPE CONFIG_PANEL_LCD
+#endif
+
+#ifdef CONFIG_PANEL_LCD_HEIGHT
+#undef DEFAULT_LCD_HEIGHT
+#define DEFAULT_LCD_HEIGHT CONFIG_PANEL_LCD_HEIGHT
 #endif
 
 #ifdef CONFIG_PANEL_LCD_WIDTH
@@ -356,9 +400,9 @@ static unsigned char lcd_bits[LCD_PORTS][LCD_BITS][BIT_STATES];
 #define DEFAULT_LCD_HWIDTH CONFIG_PANEL_LCD_HWIDTH
 #endif
 
-#ifdef CONFIG_PANEL_LCD_HEIGHT
-#undef DEFAULT_LCD_HEIGHT
-#define DEFAULT_LCD_HEIGHT CONFIG_PANEL_LCD_HEIGHT
+#ifdef CONFIG_PANEL_LCD_CHARSET
+#undef DEFAULT_LCD_CHARSET
+#define DEFAULT_LCD_CHARSET CONFIG_PANEL_LCD_CHARSET
 #endif
 
 #ifdef CONFIG_PANEL_LCD_PROTO
@@ -396,25 +440,18 @@ static unsigned char lcd_bits[LCD_PORTS][LCD_BITS][BIT_STATES];
 #define DEFAULT_LCD_PIN_BL CONFIG_PANEL_LCD_PIN_BL
 #endif
 
-#ifdef CONFIG_PANEL_LCD_CHARSET
-#undef DEFAULT_LCD_CHARSET
-#define DEFAULT_LCD_CHARSET CONFIG_PANEL_LCD_CHARSET
-#endif
-
 #endif /* DEFAULT_PROFILE == 0 */
 
 /* global variables */
-static int keypad_open_cnt;	/* #times opened */
-static int lcd_open_cnt;	/* #times opened */
+
+/* Device single-open policy control */
+static atomic_t lcd_available = ATOMIC_INIT(1);
+static atomic_t keypad_available = ATOMIC_INIT(1);
+
 static struct pardevice *pprt;
 
-static int lcd_initialized;
 static int keypad_initialized;
 
-static int light_tempo;
-
-static char lcd_must_clear;
-static char lcd_left_shift;
 static char init_in_progress;
 
 static void (*lcd_write_cmd)(int);
@@ -426,59 +463,51 @@ static struct timer_list scan_timer;
 
 MODULE_DESCRIPTION("Generic parallel port LCD/Keypad driver");
 
-static int parport = -1;
+static int parport = DEFAULT_PARPORT;
 module_param(parport, int, 0000);
 MODULE_PARM_DESC(parport, "Parallel port index (0=lpt1, 1=lpt2, ...)");
 
-static int lcd_height = -1;
+static int profile = DEFAULT_PROFILE;
+module_param(profile, int, 0000);
+MODULE_PARM_DESC(profile,
+		 "1=16x2 old kp; 2=serial 16x2, new kp; 3=16x2 hantronix; "
+		 "4=16x2 nexcom; default=40x2, old kp");
+
+static int keypad_type = NOT_SET;
+module_param(keypad_type, int, 0000);
+MODULE_PARM_DESC(keypad_type,
+		 "Keypad type: 0=none, 1=old 6 keys, 2=new 6+1 keys, 3=nexcom 4 keys");
+
+static int lcd_type = NOT_SET;
+module_param(lcd_type, int, 0000);
+MODULE_PARM_DESC(lcd_type,
+		 "LCD type: 0=none, 1=old //, 2=serial ks0074, 3=hantronix //, 4=nexcom //, 5=compiled-in");
+
+static int lcd_height = NOT_SET;
 module_param(lcd_height, int, 0000);
 MODULE_PARM_DESC(lcd_height, "Number of lines on the LCD");
 
-static int lcd_width = -1;
+static int lcd_width = NOT_SET;
 module_param(lcd_width, int, 0000);
 MODULE_PARM_DESC(lcd_width, "Number of columns on the LCD");
 
-static int lcd_bwidth = -1;	/* internal buffer width (usually 40) */
+static int lcd_bwidth = NOT_SET;	/* internal buffer width (usually 40) */
 module_param(lcd_bwidth, int, 0000);
 MODULE_PARM_DESC(lcd_bwidth, "Internal LCD line width (40)");
 
-static int lcd_hwidth = -1;	/* hardware buffer width (usually 64) */
+static int lcd_hwidth = NOT_SET;	/* hardware buffer width (usually 64) */
 module_param(lcd_hwidth, int, 0000);
 MODULE_PARM_DESC(lcd_hwidth, "LCD line hardware address (64)");
 
-static int lcd_enabled = -1;
-module_param(lcd_enabled, int, 0000);
-MODULE_PARM_DESC(lcd_enabled, "Deprecated option, use lcd_type instead");
-
-static int keypad_enabled = -1;
-module_param(keypad_enabled, int, 0000);
-MODULE_PARM_DESC(keypad_enabled, "Deprecated option, use keypad_type instead");
-
-static int lcd_type = -1;
-module_param(lcd_type, int, 0000);
-MODULE_PARM_DESC(lcd_type,
-		 "LCD type: 0=none, 1=old //, 2=serial ks0074, 3=hantronix //, 4=nexcom //, 5=compiled-in");
+static int lcd_charset = NOT_SET;
+module_param(lcd_charset, int, 0000);
+MODULE_PARM_DESC(lcd_charset, "LCD character set: 0=standard, 1=KS0074");
 
-static int lcd_proto = -1;
+static int lcd_proto = NOT_SET;
 module_param(lcd_proto, int, 0000);
 MODULE_PARM_DESC(lcd_proto,
 		 "LCD communication: 0=parallel (//), 1=serial, 2=TI LCD Interface");
 
-static int lcd_charset = -1;
-module_param(lcd_charset, int, 0000);
-MODULE_PARM_DESC(lcd_charset, "LCD character set: 0=standard, 1=KS0074");
-
-static int keypad_type = -1;
-module_param(keypad_type, int, 0000);
-MODULE_PARM_DESC(keypad_type,
-		 "Keypad type: 0=none, 1=old 6 keys, 2=new 6+1 keys, 3=nexcom 4 keys");
-
-static int profile = DEFAULT_PROFILE;
-module_param(profile, int, 0000);
-MODULE_PARM_DESC(profile,
-		 "1=16x2 old kp; 2=serial 16x2, new kp; 3=16x2 hantronix; "
-		 "4=16x2 nexcom; default=40x2, old kp");
-
 /*
  * These are the parallel port pins the LCD control signals are connected to.
  * Set this to 0 if the signal is not used. Set it to its opposite value
@@ -503,20 +532,31 @@ module_param(lcd_rw_pin, int, 0000);
 MODULE_PARM_DESC(lcd_rw_pin,
 		 "# of the // port pin connected to LCD 'RW' signal, with polarity (-17..17)");
 
-static int lcd_bl_pin = PIN_NOT_SET;
-module_param(lcd_bl_pin, int, 0000);
-MODULE_PARM_DESC(lcd_bl_pin,
-		 "# of the // port pin connected to LCD backlight, with polarity (-17..17)");
+static int lcd_cl_pin = PIN_NOT_SET;
+module_param(lcd_cl_pin, int, 0000);
+MODULE_PARM_DESC(lcd_cl_pin,
+		 "# of the // port pin connected to serial LCD 'SCL' signal, with polarity (-17..17)");
 
 static int lcd_da_pin = PIN_NOT_SET;
 module_param(lcd_da_pin, int, 0000);
 MODULE_PARM_DESC(lcd_da_pin,
 		 "# of the // port pin connected to serial LCD 'SDA' signal, with polarity (-17..17)");
 
-static int lcd_cl_pin = PIN_NOT_SET;
-module_param(lcd_cl_pin, int, 0000);
-MODULE_PARM_DESC(lcd_cl_pin,
-		 "# of the // port pin connected to serial LCD 'SCL' signal, with polarity (-17..17)");
+static int lcd_bl_pin = PIN_NOT_SET;
+module_param(lcd_bl_pin, int, 0000);
+MODULE_PARM_DESC(lcd_bl_pin,
+		 "# of the // port pin connected to LCD backlight, with polarity (-17..17)");
+
+/* Deprecated module parameters - consider not using them anymore */
+
+static int lcd_enabled = NOT_SET;
+module_param(lcd_enabled, int, 0000);
+MODULE_PARM_DESC(lcd_enabled, "Deprecated option, use lcd_type instead");
+
+static int keypad_enabled = NOT_SET;
+module_param(keypad_enabled, int, 0000);
+MODULE_PARM_DESC(keypad_enabled, "Deprecated option, use keypad_type instead");
+
 
 static const unsigned char *lcd_char_conv;
 
@@ -748,7 +788,7 @@ static void lcd_send_serial(int byte)
 /* turn the backlight on or off */
 static void lcd_backlight(int on)
 {
-	if (lcd_bl_pin == PIN_NONE)
+	if (lcd.pins.bl == PIN_NONE)
 		return;
 
 	/* The backlight is activated by setting the AUTOFEED line to +5V  */
@@ -847,23 +887,23 @@ static void lcd_write_data_tilcd(int data)
 static void lcd_gotoxy(void)
 {
 	lcd_write_cmd(0x80	/* set DDRAM address */
-		      | (lcd_addr_y ? lcd_hwidth : 0)
+		      | (lcd.addr.y ? lcd.hwidth : 0)
 		      /* we force the cursor to stay at the end of the
 			 line if it wants to go farther */
-		      | ((lcd_addr_x < lcd_bwidth) ? lcd_addr_x &
-			 (lcd_hwidth - 1) : lcd_bwidth - 1));
+		      | ((lcd.addr.x < lcd.bwidth) ? lcd.addr.x &
+			 (lcd.hwidth - 1) : lcd.bwidth - 1));
 }
 
 static void lcd_print(char c)
 {
-	if (lcd_addr_x < lcd_bwidth) {
+	if (lcd.addr.x < lcd.bwidth) {
 		if (lcd_char_conv != NULL)
 			c = lcd_char_conv[(unsigned char)c];
 		lcd_write_data(c);
-		lcd_addr_x++;
+		lcd.addr.x++;
 	}
 	/* prevents the cursor from wrapping onto the next line */
-	if (lcd_addr_x == lcd_bwidth)
+	if (lcd.addr.x == lcd.bwidth)
 		lcd_gotoxy();
 }
 
@@ -872,12 +912,12 @@ static void lcd_clear_fast_s(void)
 {
 	int pos;
 
-	lcd_addr_x = 0;
-	lcd_addr_y = 0;
+	lcd.addr.x = 0;
+	lcd.addr.y = 0;
 	lcd_gotoxy();
 
 	spin_lock_irq(&pprt_lock);
-	for (pos = 0; pos < lcd_height * lcd_hwidth; pos++) {
+	for (pos = 0; pos < lcd.height * lcd.hwidth; pos++) {
 		lcd_send_serial(0x5F);	/* R/W=W, RS=1 */
 		lcd_send_serial(' ' & 0x0F);
 		lcd_send_serial((' ' >> 4) & 0x0F);
@@ -885,8 +925,8 @@ static void lcd_clear_fast_s(void)
 	}
 	spin_unlock_irq(&pprt_lock);
 
-	lcd_addr_x = 0;
-	lcd_addr_y = 0;
+	lcd.addr.x = 0;
+	lcd.addr.y = 0;
 	lcd_gotoxy();
 }
 
@@ -895,12 +935,12 @@ static void lcd_clear_fast_p8(void)
 {
 	int pos;
 
-	lcd_addr_x = 0;
-	lcd_addr_y = 0;
+	lcd.addr.x = 0;
+	lcd.addr.y = 0;
 	lcd_gotoxy();
 
 	spin_lock_irq(&pprt_lock);
-	for (pos = 0; pos < lcd_height * lcd_hwidth; pos++) {
+	for (pos = 0; pos < lcd.height * lcd.hwidth; pos++) {
 		/* present the data to the data port */
 		w_dtr(pprt, ' ');
 
@@ -923,8 +963,8 @@ static void lcd_clear_fast_p8(void)
 	}
 	spin_unlock_irq(&pprt_lock);
 
-	lcd_addr_x = 0;
-	lcd_addr_y = 0;
+	lcd.addr.x = 0;
+	lcd.addr.y = 0;
 	lcd_gotoxy();
 }
 
@@ -933,12 +973,12 @@ static void lcd_clear_fast_tilcd(void)
 {
 	int pos;
 
-	lcd_addr_x = 0;
-	lcd_addr_y = 0;
+	lcd.addr.x = 0;
+	lcd.addr.y = 0;
 	lcd_gotoxy();
 
 	spin_lock_irq(&pprt_lock);
-	for (pos = 0; pos < lcd_height * lcd_hwidth; pos++) {
+	for (pos = 0; pos < lcd.height * lcd.hwidth; pos++) {
 		/* present the data to the data port */
 		w_dtr(pprt, ' ');
 		udelay(60);
@@ -946,8 +986,8 @@ static void lcd_clear_fast_tilcd(void)
 
 	spin_unlock_irq(&pprt_lock);
 
-	lcd_addr_x = 0;
-	lcd_addr_y = 0;
+	lcd.addr.x = 0;
+	lcd.addr.y = 0;
 	lcd_gotoxy();
 }
 
@@ -955,15 +995,15 @@ static void lcd_clear_fast_tilcd(void)
 static void lcd_clear_display(void)
 {
 	lcd_write_cmd(0x01);	/* clear display */
-	lcd_addr_x = 0;
-	lcd_addr_y = 0;
+	lcd.addr.x = 0;
+	lcd.addr.y = 0;
 	/* we must wait a few milliseconds (15) */
 	long_sleep(15);
 }
 
 static void lcd_init_display(void)
 {
-	lcd_flags = ((lcd_height > 1) ? LCD_FLAG_N : 0)
+	lcd.flags = ((lcd.height > 1) ? LCD_FLAG_N : 0)
 	    | LCD_FLAG_D | LCD_FLAG_C | LCD_FLAG_B;
 
 	long_sleep(20);		/* wait 20 ms after power-up for the paranoid */
@@ -976,8 +1016,8 @@ static void lcd_init_display(void)
 	long_sleep(10);
 
 	lcd_write_cmd(0x30	/* set font height and lines number */
-		      | ((lcd_flags & LCD_FLAG_F) ? 4 : 0)
-		      | ((lcd_flags & LCD_FLAG_N) ? 8 : 0)
+		      | ((lcd.flags & LCD_FLAG_F) ? 4 : 0)
+		      | ((lcd.flags & LCD_FLAG_N) ? 8 : 0)
 	    );
 	long_sleep(10);
 
@@ -985,12 +1025,12 @@ static void lcd_init_display(void)
 	long_sleep(10);
 
 	lcd_write_cmd(0x08	/* set display mode */
-		      | ((lcd_flags & LCD_FLAG_D) ? 4 : 0)
-		      | ((lcd_flags & LCD_FLAG_C) ? 2 : 0)
-		      | ((lcd_flags & LCD_FLAG_B) ? 1 : 0)
+		      | ((lcd.flags & LCD_FLAG_D) ? 4 : 0)
+		      | ((lcd.flags & LCD_FLAG_C) ? 2 : 0)
+		      | ((lcd.flags & LCD_FLAG_B) ? 1 : 0)
 	    );
 
-	lcd_backlight((lcd_flags & LCD_FLAG_L) ? 1 : 0);
+	lcd_backlight((lcd.flags & LCD_FLAG_L) ? 1 : 0);
 
 	long_sleep(10);
 
@@ -1013,100 +1053,101 @@ static inline int handle_lcd_special_code(void)
 
 	int processed = 0;
 
-	char *esc = lcd_escape + 2;
-	int oldflags = lcd_flags;
+	char *esc = lcd.esc_seq.buf + 2;
+	int oldflags = lcd.flags;
 
 	/* check for display mode flags */
 	switch (*esc) {
 	case 'D':	/* Display ON */
-		lcd_flags |= LCD_FLAG_D;
+		lcd.flags |= LCD_FLAG_D;
 		processed = 1;
 		break;
 	case 'd':	/* Display OFF */
-		lcd_flags &= ~LCD_FLAG_D;
+		lcd.flags &= ~LCD_FLAG_D;
 		processed = 1;
 		break;
 	case 'C':	/* Cursor ON */
-		lcd_flags |= LCD_FLAG_C;
+		lcd.flags |= LCD_FLAG_C;
 		processed = 1;
 		break;
 	case 'c':	/* Cursor OFF */
-		lcd_flags &= ~LCD_FLAG_C;
+		lcd.flags &= ~LCD_FLAG_C;
 		processed = 1;
 		break;
 	case 'B':	/* Blink ON */
-		lcd_flags |= LCD_FLAG_B;
+		lcd.flags |= LCD_FLAG_B;
 		processed = 1;
 		break;
 	case 'b':	/* Blink OFF */
-		lcd_flags &= ~LCD_FLAG_B;
+		lcd.flags &= ~LCD_FLAG_B;
 		processed = 1;
 		break;
 	case '+':	/* Back light ON */
-		lcd_flags |= LCD_FLAG_L;
+		lcd.flags |= LCD_FLAG_L;
 		processed = 1;
 		break;
 	case '-':	/* Back light OFF */
-		lcd_flags &= ~LCD_FLAG_L;
+		lcd.flags &= ~LCD_FLAG_L;
 		processed = 1;
 		break;
 	case '*':
 		/* flash back light using the keypad timer */
 		if (scan_timer.function != NULL) {
-			if (light_tempo == 0 && ((lcd_flags & LCD_FLAG_L) == 0))
+			if (lcd.light_tempo == 0
+					&& ((lcd.flags & LCD_FLAG_L) == 0))
 				lcd_backlight(1);
-			light_tempo = FLASH_LIGHT_TEMPO;
+			lcd.light_tempo = FLASH_LIGHT_TEMPO;
 		}
 		processed = 1;
 		break;
 	case 'f':	/* Small Font */
-		lcd_flags &= ~LCD_FLAG_F;
+		lcd.flags &= ~LCD_FLAG_F;
 		processed = 1;
 		break;
 	case 'F':	/* Large Font */
-		lcd_flags |= LCD_FLAG_F;
+		lcd.flags |= LCD_FLAG_F;
 		processed = 1;
 		break;
 	case 'n':	/* One Line */
-		lcd_flags &= ~LCD_FLAG_N;
+		lcd.flags &= ~LCD_FLAG_N;
 		processed = 1;
 		break;
 	case 'N':	/* Two Lines */
-		lcd_flags |= LCD_FLAG_N;
+		lcd.flags |= LCD_FLAG_N;
 		break;
 	case 'l':	/* Shift Cursor Left */
-		if (lcd_addr_x > 0) {
+		if (lcd.addr.x > 0) {
 			/* back one char if not at end of line */
-			if (lcd_addr_x < lcd_bwidth)
+			if (lcd.addr.x < lcd.bwidth)
 				lcd_write_cmd(0x10);
-			lcd_addr_x--;
+			lcd.addr.x--;
 		}
 		processed = 1;
 		break;
 	case 'r':	/* shift cursor right */
-		if (lcd_addr_x < lcd_width) {
+		if (lcd.addr.x < lcd.width) {
 			/* allow the cursor to pass the end of the line */
-			if (lcd_addr_x <
-			    (lcd_bwidth - 1))
+			if (lcd.addr.x <
+			    (lcd.bwidth - 1))
 				lcd_write_cmd(0x14);
-			lcd_addr_x++;
+			lcd.addr.x++;
 		}
 		processed = 1;
 		break;
 	case 'L':	/* shift display left */
-		lcd_left_shift++;
+		lcd.left_shift++;
 		lcd_write_cmd(0x18);
 		processed = 1;
 		break;
 	case 'R':	/* shift display right */
-		lcd_left_shift--;
+		lcd.left_shift--;
 		lcd_write_cmd(0x1C);
 		processed = 1;
 		break;
 	case 'k': {	/* kill end of line */
 		int x;
 
-		for (x = lcd_addr_x; x < lcd_bwidth; x++)
+		for (x = lcd.addr.x; x < lcd.bwidth; x++)
 			lcd_write_data(' ');
 
 		/* restore cursor position */
@@ -1116,7 +1157,7 @@ static inline int handle_lcd_special_code(void)
 	}
 	case 'I':	/* reinitialize display */
 		lcd_init_display();
-		lcd_left_shift = 0;
+		lcd.left_shift = 0;
 		processed = 1;
 		break;
 	case 'G': {
@@ -1187,11 +1228,11 @@ static inline int handle_lcd_special_code(void)
 		while (*esc) {
 			if (*esc == 'x') {
 				esc++;
-				if (kstrtoul(esc, 10, &lcd_addr_x) < 0)
+				if (kstrtoul(esc, 10, &lcd.addr.x) < 0)
 					break;
 			} else if (*esc == 'y') {
 				esc++;
-				if (kstrtoul(esc, 10, &lcd_addr_y) < 0)
+				if (kstrtoul(esc, 10, &lcd.addr.y) < 0)
 					break;
 			} else {
 				break;
@@ -1204,25 +1245,25 @@ static inline int handle_lcd_special_code(void)
 	}
 
 	/* Check whether one flag was changed */
-	if (oldflags != lcd_flags) {
+	if (oldflags != lcd.flags) {
 		/* check whether one of B,C,D flags were changed */
-		if ((oldflags ^ lcd_flags) &
+		if ((oldflags ^ lcd.flags) &
 		    (LCD_FLAG_B | LCD_FLAG_C | LCD_FLAG_D))
 			/* set display mode */
 			lcd_write_cmd(0x08
-				      | ((lcd_flags & LCD_FLAG_D) ? 4 : 0)
-				      | ((lcd_flags & LCD_FLAG_C) ? 2 : 0)
-				      | ((lcd_flags & LCD_FLAG_B) ? 1 : 0));
+				      | ((lcd.flags & LCD_FLAG_D) ? 4 : 0)
+				      | ((lcd.flags & LCD_FLAG_C) ? 2 : 0)
+				      | ((lcd.flags & LCD_FLAG_B) ? 1 : 0));
 		/* check whether one of F,N flags was changed */
-		else if ((oldflags ^ lcd_flags) & (LCD_FLAG_F | LCD_FLAG_N))
+		else if ((oldflags ^ lcd.flags) & (LCD_FLAG_F | LCD_FLAG_N))
 			lcd_write_cmd(0x30
-				      | ((lcd_flags & LCD_FLAG_F) ? 4 : 0)
-				      | ((lcd_flags & LCD_FLAG_N) ? 8 : 0));
+				      | ((lcd.flags & LCD_FLAG_F) ? 4 : 0)
+				      | ((lcd.flags & LCD_FLAG_N) ? 8 : 0));
 		/* check whether L flag was changed */
-		else if ((oldflags ^ lcd_flags) & (LCD_FLAG_L)) {
-			if (lcd_flags & (LCD_FLAG_L))
+		else if ((oldflags ^ lcd.flags) & (LCD_FLAG_L)) {
+			if (lcd.flags & (LCD_FLAG_L))
 				lcd_backlight(1);
-			else if (light_tempo == 0)
+			else if (lcd.light_tempo == 0)
 				/* switch off the light only when the tempo
 				   lighting is gone */
 				lcd_backlight(0);
@@ -1235,29 +1276,29 @@ static inline int handle_lcd_special_code(void)
 static void lcd_write_char(char c)
 {
 	/* first, we'll test if we're in escape mode */
-	if ((c != '\n') && lcd_escape_len >= 0) {
+	if ((c != '\n') && lcd.esc_seq.len >= 0) {
 		/* yes, let's add this char to the buffer */
-		lcd_escape[lcd_escape_len++] = c;
-		lcd_escape[lcd_escape_len] = 0;
+		lcd.esc_seq.buf[lcd.esc_seq.len++] = c;
+		lcd.esc_seq.buf[lcd.esc_seq.len] = 0;
 	} else {
 		/* aborts any previous escape sequence */
-		lcd_escape_len = -1;
+		lcd.esc_seq.len = -1;
 
 		switch (c) {
 		case LCD_ESCAPE_CHAR:
 			/* start of an escape sequence */
-			lcd_escape_len = 0;
-			lcd_escape[lcd_escape_len] = 0;
+			lcd.esc_seq.len = 0;
+			lcd.esc_seq.buf[lcd.esc_seq.len] = 0;
 			break;
 		case '\b':
 			/* go back one char and clear it */
-			if (lcd_addr_x > 0) {
+			if (lcd.addr.x > 0) {
 				/* check if we're not at the
 				   end of the line */
-				if (lcd_addr_x < lcd_bwidth)
+				if (lcd.addr.x < lcd.bwidth)
 					/* back one char */
 					lcd_write_cmd(0x10);
-				lcd_addr_x--;
+				lcd.addr.x--;
 			}
 			/* replace with a space */
 			lcd_write_data(' ');
@@ -1271,15 +1312,15 @@ static void lcd_write_char(char c)
 		case '\n':
 			/* flush the remainder of the current line and
 			   go to the beginning of the next line */
-			for (; lcd_addr_x < lcd_bwidth; lcd_addr_x++)
+			for (; lcd.addr.x < lcd.bwidth; lcd.addr.x++)
 				lcd_write_data(' ');
-			lcd_addr_x = 0;
-			lcd_addr_y = (lcd_addr_y + 1) % lcd_height;
+			lcd.addr.x = 0;
+			lcd.addr.y = (lcd.addr.y + 1) % lcd.height;
 			lcd_gotoxy();
 			break;
 		case '\r':
 			/* go to the beginning of the same line */
-			lcd_addr_x = 0;
+			lcd.addr.x = 0;
 			lcd_gotoxy();
 			break;
 		case '\t':
@@ -1295,32 +1336,32 @@ static void lcd_write_char(char c)
 
 	/* now we'll see if we're in an escape mode and if the current
 	   escape sequence can be understood. */
-	if (lcd_escape_len >= 2) {
+	if (lcd.esc_seq.len >= 2) {
 		int processed = 0;
 
-		if (!strcmp(lcd_escape, "[2J")) {
+		if (!strcmp(lcd.esc_seq.buf, "[2J")) {
 			/* clear the display */
 			lcd_clear_fast();
 			processed = 1;
-		} else if (!strcmp(lcd_escape, "[H")) {
+		} else if (!strcmp(lcd.esc_seq.buf, "[H")) {
 			/* cursor to home */
-			lcd_addr_x = 0;
-			lcd_addr_y = 0;
+			lcd.addr.x = 0;
+			lcd.addr.y = 0;
 			lcd_gotoxy();
 			processed = 1;
 		}
 		/* codes starting with ^[[L */
-		else if ((lcd_escape_len >= 3) &&
-			 (lcd_escape[0] == '[') &&
-			 (lcd_escape[1] == 'L')) {
+		else if ((lcd.esc_seq.len >= 3) &&
+			 (lcd.esc_seq.buf[0] == '[') &&
+			 (lcd.esc_seq.buf[1] == 'L')) {
 			processed = handle_lcd_special_code();
 		}
 
 		/* LCD special escape codes */
 		/* flush the escape sequence if it's been processed
 		   or if it is getting too long. */
-		if (processed || (lcd_escape_len >= LCD_ESCAPE_LEN))
-			lcd_escape_len = -1;
+		if (processed || (lcd.esc_seq.len >= LCD_ESCAPE_LEN))
+			lcd.esc_seq.len = -1;
 	} /* escape codes */
 }
 
@@ -1347,23 +1388,22 @@ static ssize_t lcd_write(struct file *file,
 
 static int lcd_open(struct inode *inode, struct file *file)
 {
-	if (lcd_open_cnt)
+	if (!atomic_dec_and_test(&lcd_available))
 		return -EBUSY;	/* open only once at a time */
 
 	if (file->f_mode & FMODE_READ)	/* device is write-only */
 		return -EPERM;
 
-	if (lcd_must_clear) {
+	if (lcd.must_clear) {
 		lcd_clear_display();
-		lcd_must_clear = 0;
+		lcd.must_clear = false;
 	}
-	lcd_open_cnt++;
 	return nonseekable_open(inode, file);
 }
 
 static int lcd_release(struct inode *inode, struct file *file)
 {
-	lcd_open_cnt--;
+	atomic_inc(&lcd_available);
 	return 0;
 }
 
@@ -1375,9 +1415,9 @@ static const struct file_operations lcd_fops = {
 };
 
 static struct miscdevice lcd_dev = {
-	LCD_MINOR,
-	"lcd",
-	&lcd_fops
+	.minor	= LCD_MINOR,
+	.name	= "lcd",
+	.fops	= &lcd_fops,
 };
 
 /* public function usable from the kernel for any purpose */
@@ -1386,7 +1426,7 @@ static void panel_lcd_print(const char *s)
 	const char *tmp = s;
 	int count = strlen(s);
 
-	if (lcd_enabled && lcd_initialized) {
+	if (lcd.enabled && lcd.initialized) {
 		for (; count-- > 0; tmp++) {
 			if (!in_interrupt() && (((count + 1) & 0x1f) == 0))
 				/* let's be a little nice with other processes
@@ -1401,183 +1441,173 @@ static void panel_lcd_print(const char *s)
 /* initialize the LCD driver */
 static void lcd_init(void)
 {
-	switch (lcd_type) {
+	switch (selected_lcd_type) {
 	case LCD_TYPE_OLD:
 		/* parallel mode, 8 bits */
-		if (lcd_proto < 0)
-			lcd_proto = LCD_PROTO_PARALLEL;
-		if (lcd_charset < 0)
-			lcd_charset = LCD_CHARSET_NORMAL;
-		if (lcd_e_pin == PIN_NOT_SET)
-			lcd_e_pin = PIN_STROBE;
-		if (lcd_rs_pin == PIN_NOT_SET)
-			lcd_rs_pin = PIN_AUTOLF;
-
-		if (lcd_width < 0)
-			lcd_width = 40;
-		if (lcd_bwidth < 0)
-			lcd_bwidth = 40;
-		if (lcd_hwidth < 0)
-			lcd_hwidth = 64;
-		if (lcd_height < 0)
-			lcd_height = 2;
+		lcd.proto = LCD_PROTO_PARALLEL;
+		lcd.charset = LCD_CHARSET_NORMAL;
+		lcd.pins.e = PIN_STROBE;
+		lcd.pins.rs = PIN_AUTOLF;
+
+		lcd.width = 40;
+		lcd.bwidth = 40;
+		lcd.hwidth = 64;
+		lcd.height = 2;
 		break;
 	case LCD_TYPE_KS0074:
 		/* serial mode, ks0074 */
-		if (lcd_proto < 0)
-			lcd_proto = LCD_PROTO_SERIAL;
-		if (lcd_charset < 0)
-			lcd_charset = LCD_CHARSET_KS0074;
-		if (lcd_bl_pin == PIN_NOT_SET)
-			lcd_bl_pin = PIN_AUTOLF;
-		if (lcd_cl_pin == PIN_NOT_SET)
-			lcd_cl_pin = PIN_STROBE;
-		if (lcd_da_pin == PIN_NOT_SET)
-			lcd_da_pin = PIN_D0;
-
-		if (lcd_width < 0)
-			lcd_width = 16;
-		if (lcd_bwidth < 0)
-			lcd_bwidth = 40;
-		if (lcd_hwidth < 0)
-			lcd_hwidth = 16;
-		if (lcd_height < 0)
-			lcd_height = 2;
+		lcd.proto = LCD_PROTO_SERIAL;
+		lcd.charset = LCD_CHARSET_KS0074;
+		lcd.pins.bl = PIN_AUTOLF;
+		lcd.pins.cl = PIN_STROBE;
+		lcd.pins.da = PIN_D0;
+
+		lcd.width = 16;
+		lcd.bwidth = 40;
+		lcd.hwidth = 16;
+		lcd.height = 2;
 		break;
 	case LCD_TYPE_NEXCOM:
 		/* parallel mode, 8 bits, generic */
-		if (lcd_proto < 0)
-			lcd_proto = LCD_PROTO_PARALLEL;
-		if (lcd_charset < 0)
-			lcd_charset = LCD_CHARSET_NORMAL;
-		if (lcd_e_pin == PIN_NOT_SET)
-			lcd_e_pin = PIN_AUTOLF;
-		if (lcd_rs_pin == PIN_NOT_SET)
-			lcd_rs_pin = PIN_SELECP;
-		if (lcd_rw_pin == PIN_NOT_SET)
-			lcd_rw_pin = PIN_INITP;
-
-		if (lcd_width < 0)
-			lcd_width = 16;
-		if (lcd_bwidth < 0)
-			lcd_bwidth = 40;
-		if (lcd_hwidth < 0)
-			lcd_hwidth = 64;
-		if (lcd_height < 0)
-			lcd_height = 2;
+		lcd.proto = LCD_PROTO_PARALLEL;
+		lcd.charset = LCD_CHARSET_NORMAL;
+		lcd.pins.e = PIN_AUTOLF;
+		lcd.pins.rs = PIN_SELECP;
+		lcd.pins.rw = PIN_INITP;
+
+		lcd.width = 16;
+		lcd.bwidth = 40;
+		lcd.hwidth = 64;
+		lcd.height = 2;
 		break;
 	case LCD_TYPE_CUSTOM:
 		/* customer-defined */
-		if (lcd_proto < 0)
-			lcd_proto = DEFAULT_LCD_PROTO;
-		if (lcd_charset < 0)
-			lcd_charset = DEFAULT_LCD_CHARSET;
+		lcd.proto = DEFAULT_LCD_PROTO;
+		lcd.charset = DEFAULT_LCD_CHARSET;
 		/* default geometry will be set later */
 		break;
 	case LCD_TYPE_HANTRONIX:
 		/* parallel mode, 8 bits, hantronix-like */
 	default:
-		if (lcd_proto < 0)
-			lcd_proto = LCD_PROTO_PARALLEL;
-		if (lcd_charset < 0)
-			lcd_charset = LCD_CHARSET_NORMAL;
-		if (lcd_e_pin == PIN_NOT_SET)
-			lcd_e_pin = PIN_STROBE;
-		if (lcd_rs_pin == PIN_NOT_SET)
-			lcd_rs_pin = PIN_SELECP;
-
-		if (lcd_width < 0)
-			lcd_width = 16;
-		if (lcd_bwidth < 0)
-			lcd_bwidth = 40;
-		if (lcd_hwidth < 0)
-			lcd_hwidth = 64;
-		if (lcd_height < 0)
-			lcd_height = 2;
+		lcd.proto = LCD_PROTO_PARALLEL;
+		lcd.charset = LCD_CHARSET_NORMAL;
+		lcd.pins.e = PIN_STROBE;
+		lcd.pins.rs = PIN_SELECP;
+
+		lcd.width = 16;
+		lcd.bwidth = 40;
+		lcd.hwidth = 64;
+		lcd.height = 2;
 		break;
 	}
 
+	/* Overwrite with module params set on loading */
+	if (lcd_height != NOT_SET)
+		lcd.height = lcd_height;
+	if (lcd_width != NOT_SET)
+		lcd.width = lcd_width;
+	if (lcd_bwidth != NOT_SET)
+		lcd.bwidth = lcd_bwidth;
+	if (lcd_hwidth != NOT_SET)
+		lcd.hwidth = lcd_hwidth;
+	if (lcd_charset != NOT_SET)
+		lcd.charset = lcd_charset;
+	if (lcd_proto != NOT_SET)
+		lcd.proto = lcd_proto;
+	if (lcd_e_pin != PIN_NOT_SET)
+		lcd.pins.e = lcd_e_pin;
+	if (lcd_rs_pin != PIN_NOT_SET)
+		lcd.pins.rs = lcd_rs_pin;
+	if (lcd_rw_pin != PIN_NOT_SET)
+		lcd.pins.rw = lcd_rw_pin;
+	if (lcd_cl_pin != PIN_NOT_SET)
+		lcd.pins.cl = lcd_cl_pin;
+	if (lcd_da_pin != PIN_NOT_SET)
+		lcd.pins.da = lcd_da_pin;
+	if (lcd_bl_pin != PIN_NOT_SET)
+		lcd.pins.bl = lcd_bl_pin;
+
 	/* this is used to catch wrong and default values */
-	if (lcd_width <= 0)
-		lcd_width = DEFAULT_LCD_WIDTH;
-	if (lcd_bwidth <= 0)
-		lcd_bwidth = DEFAULT_LCD_BWIDTH;
-	if (lcd_hwidth <= 0)
-		lcd_hwidth = DEFAULT_LCD_HWIDTH;
-	if (lcd_height <= 0)
-		lcd_height = DEFAULT_LCD_HEIGHT;
-
-	if (lcd_proto == LCD_PROTO_SERIAL) {	/* SERIAL */
+	if (lcd.width <= 0)
+		lcd.width = DEFAULT_LCD_WIDTH;
+	if (lcd.bwidth <= 0)
+		lcd.bwidth = DEFAULT_LCD_BWIDTH;
+	if (lcd.hwidth <= 0)
+		lcd.hwidth = DEFAULT_LCD_HWIDTH;
+	if (lcd.height <= 0)
+		lcd.height = DEFAULT_LCD_HEIGHT;
+
+	if (lcd.proto == LCD_PROTO_SERIAL) {	/* SERIAL */
 		lcd_write_cmd = lcd_write_cmd_s;
 		lcd_write_data = lcd_write_data_s;
 		lcd_clear_fast = lcd_clear_fast_s;
 
-		if (lcd_cl_pin == PIN_NOT_SET)
-			lcd_cl_pin = DEFAULT_LCD_PIN_SCL;
-		if (lcd_da_pin == PIN_NOT_SET)
-			lcd_da_pin = DEFAULT_LCD_PIN_SDA;
+		if (lcd.pins.cl == PIN_NOT_SET)
+			lcd.pins.cl = DEFAULT_LCD_PIN_SCL;
+		if (lcd.pins.da == PIN_NOT_SET)
+			lcd.pins.da = DEFAULT_LCD_PIN_SDA;
 
-	} else if (lcd_proto == LCD_PROTO_PARALLEL) {	/* PARALLEL */
+	} else if (lcd.proto == LCD_PROTO_PARALLEL) {	/* PARALLEL */
 		lcd_write_cmd = lcd_write_cmd_p8;
 		lcd_write_data = lcd_write_data_p8;
 		lcd_clear_fast = lcd_clear_fast_p8;
 
-		if (lcd_e_pin == PIN_NOT_SET)
-			lcd_e_pin = DEFAULT_LCD_PIN_E;
-		if (lcd_rs_pin == PIN_NOT_SET)
-			lcd_rs_pin = DEFAULT_LCD_PIN_RS;
-		if (lcd_rw_pin == PIN_NOT_SET)
-			lcd_rw_pin = DEFAULT_LCD_PIN_RW;
+		if (lcd.pins.e == PIN_NOT_SET)
+			lcd.pins.e = DEFAULT_LCD_PIN_E;
+		if (lcd.pins.rs == PIN_NOT_SET)
+			lcd.pins.rs = DEFAULT_LCD_PIN_RS;
+		if (lcd.pins.rw == PIN_NOT_SET)
+			lcd.pins.rw = DEFAULT_LCD_PIN_RW;
 	} else {
 		lcd_write_cmd = lcd_write_cmd_tilcd;
 		lcd_write_data = lcd_write_data_tilcd;
 		lcd_clear_fast = lcd_clear_fast_tilcd;
 	}
 
-	if (lcd_bl_pin == PIN_NOT_SET)
-		lcd_bl_pin = DEFAULT_LCD_PIN_BL;
-
-	if (lcd_e_pin == PIN_NOT_SET)
-		lcd_e_pin = PIN_NONE;
-	if (lcd_rs_pin == PIN_NOT_SET)
-		lcd_rs_pin = PIN_NONE;
-	if (lcd_rw_pin == PIN_NOT_SET)
-		lcd_rw_pin = PIN_NONE;
-	if (lcd_bl_pin == PIN_NOT_SET)
-		lcd_bl_pin = PIN_NONE;
-	if (lcd_cl_pin == PIN_NOT_SET)
-		lcd_cl_pin = PIN_NONE;
-	if (lcd_da_pin == PIN_NOT_SET)
-		lcd_da_pin = PIN_NONE;
-
-	if (lcd_charset < 0)
-		lcd_charset = DEFAULT_LCD_CHARSET;
-
-	if (lcd_charset == LCD_CHARSET_KS0074)
+	if (lcd.pins.bl == PIN_NOT_SET)
+		lcd.pins.bl = DEFAULT_LCD_PIN_BL;
+
+	if (lcd.pins.e == PIN_NOT_SET)
+		lcd.pins.e = PIN_NONE;
+	if (lcd.pins.rs == PIN_NOT_SET)
+		lcd.pins.rs = PIN_NONE;
+	if (lcd.pins.rw == PIN_NOT_SET)
+		lcd.pins.rw = PIN_NONE;
+	if (lcd.pins.bl == PIN_NOT_SET)
+		lcd.pins.bl = PIN_NONE;
+	if (lcd.pins.cl == PIN_NOT_SET)
+		lcd.pins.cl = PIN_NONE;
+	if (lcd.pins.da == PIN_NOT_SET)
+		lcd.pins.da = PIN_NONE;
+
+	if (lcd.charset == NOT_SET)
+		lcd.charset = DEFAULT_LCD_CHARSET;
+
+	if (lcd.charset == LCD_CHARSET_KS0074)
 		lcd_char_conv = lcd_char_conv_ks0074;
 	else
 		lcd_char_conv = NULL;
 
-	if (lcd_bl_pin != PIN_NONE)
+	if (lcd.pins.bl != PIN_NONE)
 		init_scan_timer();
 
-	pin_to_bits(lcd_e_pin, lcd_bits[LCD_PORT_D][LCD_BIT_E],
+	pin_to_bits(lcd.pins.e, lcd_bits[LCD_PORT_D][LCD_BIT_E],
 		    lcd_bits[LCD_PORT_C][LCD_BIT_E]);
-	pin_to_bits(lcd_rs_pin, lcd_bits[LCD_PORT_D][LCD_BIT_RS],
+	pin_to_bits(lcd.pins.rs, lcd_bits[LCD_PORT_D][LCD_BIT_RS],
 		    lcd_bits[LCD_PORT_C][LCD_BIT_RS]);
-	pin_to_bits(lcd_rw_pin, lcd_bits[LCD_PORT_D][LCD_BIT_RW],
+	pin_to_bits(lcd.pins.rw, lcd_bits[LCD_PORT_D][LCD_BIT_RW],
 		    lcd_bits[LCD_PORT_C][LCD_BIT_RW]);
-	pin_to_bits(lcd_bl_pin, lcd_bits[LCD_PORT_D][LCD_BIT_BL],
+	pin_to_bits(lcd.pins.bl, lcd_bits[LCD_PORT_D][LCD_BIT_BL],
 		    lcd_bits[LCD_PORT_C][LCD_BIT_BL]);
-	pin_to_bits(lcd_cl_pin, lcd_bits[LCD_PORT_D][LCD_BIT_CL],
+	pin_to_bits(lcd.pins.cl, lcd_bits[LCD_PORT_D][LCD_BIT_CL],
 		    lcd_bits[LCD_PORT_C][LCD_BIT_CL]);
-	pin_to_bits(lcd_da_pin, lcd_bits[LCD_PORT_D][LCD_BIT_DA],
+	pin_to_bits(lcd.pins.da, lcd_bits[LCD_PORT_D][LCD_BIT_DA],
 		    lcd_bits[LCD_PORT_C][LCD_BIT_DA]);
 
 	/* before this line, we must NOT send anything to the display.
 	 * Since lcd_init_display() needs to write data, we have to
 	 * enable mark the LCD initialized just before. */
-	lcd_initialized = 1;
+	lcd.initialized = true;
 	lcd_init_display();
 
 	/* display a short message */
@@ -1589,10 +1619,10 @@ static void lcd_init(void)
 	panel_lcd_print("\x1b[Lc\x1b[Lb\x1b[L*Linux-" UTS_RELEASE "\nPanel-"
 			PANEL_VERSION);
 #endif
-	lcd_addr_x = 0;
-	lcd_addr_y = 0;
+	lcd.addr.x = 0;
+	lcd.addr.y = 0;
 	/* clear the display on the next device opening */
-	lcd_must_clear = 1;
+	lcd.must_clear = true;
 	lcd_gotoxy();
 }
 
@@ -1627,20 +1657,19 @@ static ssize_t keypad_read(struct file *file,
 
 static int keypad_open(struct inode *inode, struct file *file)
 {
-	if (keypad_open_cnt)
+	if (!atomic_dec_and_test(&keypad_available))
 		return -EBUSY;	/* open only once at a time */
 
 	if (file->f_mode & FMODE_WRITE)	/* device is read-only */
 		return -EPERM;
 
 	keypad_buflen = 0;	/* flush the buffer on opening */
-	keypad_open_cnt++;
 	return 0;
 }
 
 static int keypad_release(struct inode *inode, struct file *file)
 {
-	keypad_open_cnt--;
+	atomic_inc(&keypad_available);
 	return 0;
 }
 
@@ -1652,9 +1681,9 @@ static const struct file_operations keypad_fops = {
 };
 
 static struct miscdevice keypad_dev = {
-	KEYPAD_MINOR,
-	"keypad",
-	&keypad_fops
+	.minor	= KEYPAD_MINOR,
+	.name	= "keypad",
+	.fops	= &keypad_fops,
 };
 
 static void keypad_send_key(const char *string, int max_len)
@@ -1663,7 +1692,7 @@ static void keypad_send_key(const char *string, int max_len)
 		return;
 
 	/* send the key to the device only if a process is attached to it. */
-	if (keypad_open_cnt > 0) {
+	if (!atomic_read(&keypad_available)) {
 		while (max_len-- && keypad_buflen < KEYPAD_BUFFER && *string) {
 			keypad_buffer[(keypad_start + keypad_buflen++) %
 				      KEYPAD_BUFFER] = *string++;
@@ -1917,7 +1946,7 @@ static void panel_process_inputs(void)
 
 static void panel_scan_timer(void)
 {
-	if (keypad_enabled && keypad_initialized) {
+	if (keypad.enabled && keypad_initialized) {
 		if (spin_trylock_irq(&pprt_lock)) {
 			phys_scan_contacts();
 
@@ -1929,14 +1958,16 @@ static void panel_scan_timer(void)
 			panel_process_inputs();
 	}
 
-	if (lcd_enabled && lcd_initialized) {
+	if (lcd.enabled && lcd.initialized) {
 		if (keypressed) {
-			if (light_tempo == 0 && ((lcd_flags & LCD_FLAG_L) == 0))
+			if (lcd.light_tempo == 0
+					&& ((lcd.flags & LCD_FLAG_L) == 0))
 				lcd_backlight(1);
-			light_tempo = FLASH_LIGHT_TEMPO;
-		} else if (light_tempo > 0) {
-			light_tempo--;
-			if (light_tempo == 0 && ((lcd_flags & LCD_FLAG_L) == 0))
+			lcd.light_tempo = FLASH_LIGHT_TEMPO;
+		} else if (lcd.light_tempo > 0) {
+			lcd.light_tempo--;
+			if (lcd.light_tempo == 0
+					&& ((lcd.flags & LCD_FLAG_L) == 0))
 				lcd_backlight(0);
 		}
 	}
@@ -2108,7 +2139,7 @@ static void keypad_init(void)
 static int panel_notify_sys(struct notifier_block *this, unsigned long code,
 			    void *unused)
 {
-	if (lcd_enabled && lcd_initialized) {
+	if (lcd.enabled && lcd.initialized) {
 		switch (code) {
 		case SYS_DOWN:
 			panel_lcd_print
@@ -2164,13 +2195,13 @@ static void panel_attach(struct parport *port)
 	/* must init LCD first, just in case an IRQ from the keypad is
 	 * generated at keypad init
 	 */
-	if (lcd_enabled) {
+	if (lcd.enabled) {
 		lcd_init();
 		if (misc_register(&lcd_dev))
 			goto err_unreg_device;
 	}
 
-	if (keypad_enabled) {
+	if (keypad.enabled) {
 		keypad_init();
 		if (misc_register(&keypad_dev))
 			goto err_lcd_unreg;
@@ -2178,7 +2209,7 @@ static void panel_attach(struct parport *port)
 	return;
 
 err_lcd_unreg:
-	if (lcd_enabled)
+	if (lcd.enabled)
 		misc_deregister(&lcd_dev);
 err_unreg_device:
 	parport_unregister_device(pprt);
@@ -2196,14 +2227,14 @@ static void panel_detach(struct parport *port)
 		return;
 	}
 
-	if (keypad_enabled && keypad_initialized) {
+	if (keypad.enabled && keypad_initialized) {
 		misc_deregister(&keypad_dev);
 		keypad_initialized = 0;
 	}
 
-	if (lcd_enabled && lcd_initialized) {
+	if (lcd.enabled && lcd.initialized) {
 		misc_deregister(&lcd_dev);
-		lcd_initialized = 0;
+		lcd.initialized = false;
 	}
 
 	parport_release(pprt);
@@ -2218,72 +2249,89 @@ static struct parport_driver panel_driver = {
 };
 
 /* init function */
-static int panel_init(void)
+static int __init panel_init_module(void)
 {
-	/* for backwards compatibility */
-	if (keypad_type < 0)
-		keypad_type = keypad_enabled;
-
-	if (lcd_type < 0)
-		lcd_type = lcd_enabled;
-
-	if (parport < 0)
-		parport = DEFAULT_PARPORT;
+	int selected_keypad_type = NOT_SET;
 
 	/* take care of an eventual profile */
 	switch (profile) {
 	case PANEL_PROFILE_CUSTOM:
 		/* custom profile */
-		if (keypad_type < 0)
-			keypad_type = DEFAULT_KEYPAD;
-		if (lcd_type < 0)
-			lcd_type = DEFAULT_LCD;
+		selected_keypad_type = DEFAULT_KEYPAD_TYPE;
+		selected_lcd_type = DEFAULT_LCD_TYPE;
 		break;
 	case PANEL_PROFILE_OLD:
 		/* 8 bits, 2*16, old keypad */
-		if (keypad_type < 0)
-			keypad_type = KEYPAD_TYPE_OLD;
-		if (lcd_type < 0)
-			lcd_type = LCD_TYPE_OLD;
-		if (lcd_width < 0)
+		selected_keypad_type = KEYPAD_TYPE_OLD;
+		selected_lcd_type = LCD_TYPE_OLD;
+
+		/* TODO: This two are a little hacky, sort it out later */
+		if (lcd_width == NOT_SET)
 			lcd_width = 16;
-		if (lcd_hwidth < 0)
+		if (lcd_hwidth == NOT_SET)
 			lcd_hwidth = 16;
 		break;
 	case PANEL_PROFILE_NEW:
 		/* serial, 2*16, new keypad */
-		if (keypad_type < 0)
-			keypad_type = KEYPAD_TYPE_NEW;
-		if (lcd_type < 0)
-			lcd_type = LCD_TYPE_KS0074;
+		selected_keypad_type = KEYPAD_TYPE_NEW;
+		selected_lcd_type = LCD_TYPE_KS0074;
 		break;
 	case PANEL_PROFILE_HANTRONIX:
 		/* 8 bits, 2*16 hantronix-like, no keypad */
-		if (keypad_type < 0)
-			keypad_type = KEYPAD_TYPE_NONE;
-		if (lcd_type < 0)
-			lcd_type = LCD_TYPE_HANTRONIX;
+		selected_keypad_type = KEYPAD_TYPE_NONE;
+		selected_lcd_type = LCD_TYPE_HANTRONIX;
 		break;
 	case PANEL_PROFILE_NEXCOM:
 		/* generic 8 bits, 2*16, nexcom keypad, eg. Nexcom. */
-		if (keypad_type < 0)
-			keypad_type = KEYPAD_TYPE_NEXCOM;
-		if (lcd_type < 0)
-			lcd_type = LCD_TYPE_NEXCOM;
+		selected_keypad_type = KEYPAD_TYPE_NEXCOM;
+		selected_lcd_type = LCD_TYPE_NEXCOM;
 		break;
 	case PANEL_PROFILE_LARGE:
 		/* 8 bits, 2*40, old keypad */
-		if (keypad_type < 0)
-			keypad_type = KEYPAD_TYPE_OLD;
-		if (lcd_type < 0)
-			lcd_type = LCD_TYPE_OLD;
+		selected_keypad_type = KEYPAD_TYPE_OLD;
+		selected_lcd_type = LCD_TYPE_OLD;
 		break;
 	}
 
-	lcd_enabled = (lcd_type > 0);
-	keypad_enabled = (keypad_type > 0);
+	/*
+	 * Init lcd struct with load-time values to preserve exact current
+	 * functionality (at least for now).
+	 */
+	lcd.height = lcd_height;
+	lcd.width = lcd_width;
+	lcd.bwidth = lcd_bwidth;
+	lcd.hwidth = lcd_hwidth;
+	lcd.charset = lcd_charset;
+	lcd.proto = lcd_proto;
+	lcd.pins.e = lcd_e_pin;
+	lcd.pins.rs = lcd_rs_pin;
+	lcd.pins.rw = lcd_rw_pin;
+	lcd.pins.cl = lcd_cl_pin;
+	lcd.pins.da = lcd_da_pin;
+	lcd.pins.bl = lcd_bl_pin;
+
+	/* Leave it for now, just in case */
+	lcd.esc_seq.len = -1;
+
+	/*
+	 * Overwrite selection with module param values (both keypad and lcd),
+	 * where the deprecated params have lower prio.
+	 */
+	if (keypad_enabled != NOT_SET)
+		selected_keypad_type = keypad_enabled;
+	if (keypad_type != NOT_SET)
+		selected_keypad_type = keypad_type;
 
-	switch (keypad_type) {
+	keypad.enabled = (selected_keypad_type > 0);
+
+	if (lcd_enabled != NOT_SET)
+		selected_lcd_type = lcd_enabled;
+	if (lcd_type != NOT_SET)
+		selected_lcd_type = lcd_type;
+
+	lcd.enabled = (selected_lcd_type > 0);
+
+	switch (selected_keypad_type) {
 	case KEYPAD_TYPE_OLD:
 		keypad_profile = old_keypad_profile;
 		break;
@@ -2306,7 +2354,7 @@ static int panel_init(void)
 		return -EIO;
 	}
 
-	if (!lcd_enabled && !keypad_enabled) {
+	if (!lcd.enabled && !keypad.enabled) {
 		/* no device enabled, let's release the parport */
 		if (pprt) {
 			parport_release(pprt);
@@ -2333,11 +2381,6 @@ static int panel_init(void)
 	return 0;
 }
 
-static int __init panel_init_module(void)
-{
-	return panel_init();
-}
-
 static void __exit panel_cleanup_module(void)
 {
 	unregister_reboot_notifier(&panel_notifier);
@@ -2346,16 +2389,16 @@ static void __exit panel_cleanup_module(void)
 		del_timer_sync(&scan_timer);
 
 	if (pprt != NULL) {
-		if (keypad_enabled) {
+		if (keypad.enabled) {
 			misc_deregister(&keypad_dev);
 			keypad_initialized = 0;
 		}
 
-		if (lcd_enabled) {
+		if (lcd.enabled) {
 			panel_lcd_print("\x0cLCD driver " PANEL_VERSION
 					"\nunloaded.\x1b[Lc\x1b[Lb\x1b[L-");
 			misc_deregister(&lcd_dev);
-			lcd_initialized = 0;
+			lcd.initialized = false;
 		}
 
 		/* TODO: free all input signals */
diff --git a/drivers/staging/rtl8188eu/core/rtw_ap.c b/drivers/staging/rtl8188eu/core/rtw_ap.c
index 9224e029ef2b..d61842ed673e 100644
--- a/drivers/staging/rtl8188eu/core/rtw_ap.c
+++ b/drivers/staging/rtl8188eu/core/rtw_ap.c
@@ -888,7 +888,7 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 
 	pbss_network->Rssi = 0;
 
-	memcpy(pbss_network->MacAddress, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	ether_addr_copy(pbss_network->MacAddress, myid(&(padapter->eeprompriv)));
 
 	/* beacon interval */
 	p = rtw_get_beacon_interval_from_ie(ie);/* 8: TimeStamp, 2: Beacon Interval 2:Capability */
@@ -1164,7 +1164,7 @@ int rtw_acl_add_sta(struct adapter *padapter, u8 *addr)
 		if (!paclnode->valid) {
 			INIT_LIST_HEAD(&paclnode->list);
 
-			memcpy(paclnode->addr, addr, ETH_ALEN);
+			ether_addr_copy(paclnode->addr, addr);
 
 			paclnode->valid = true;
 
@@ -1186,7 +1186,6 @@ int rtw_acl_add_sta(struct adapter *padapter, u8 *addr)
 int rtw_acl_remove_sta(struct adapter *padapter, u8 *addr)
 {
 	struct list_head *plist, *phead;
-	int ret = 0;
 	struct rtw_wlan_acl_node *paclnode;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
@@ -1217,7 +1216,7 @@ int rtw_acl_remove_sta(struct adapter *padapter, u8 *addr)
 	spin_unlock_bh(&(pacl_node_q->lock));
 
 	DBG_88E("%s, acl_num =%d\n", __func__, pacl_list->num);
-	return ret;
+	return 0;
 }
 
 static void update_bcn_fixed_ie(struct adapter *padapter)
@@ -1753,7 +1752,6 @@ u8 ap_free_sta(struct adapter *padapter, struct sta_info *psta,
 int rtw_ap_inform_ch_switch(struct adapter *padapter, u8 new_ch, u8 ch_offset)
 {
 	struct list_head *phead, *plist;
-	int ret = 0;
 	struct sta_info *psta = NULL;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
@@ -1761,7 +1759,7 @@ int rtw_ap_inform_ch_switch(struct adapter *padapter, u8 new_ch, u8 ch_offset)
 	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 
 	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
-		return ret;
+		return 0;
 
 	DBG_88E(FUNC_NDEV_FMT" with ch:%u, offset:%u\n",
 		FUNC_NDEV_ARG(padapter->pnetdev), new_ch, ch_offset);
@@ -1782,13 +1780,12 @@ int rtw_ap_inform_ch_switch(struct adapter *padapter, u8 new_ch, u8 ch_offset)
 
 	issue_action_spct_ch_switch(padapter, bc_addr, new_ch, ch_offset);
 
-	return ret;
+	return 0;
 }
 
 int rtw_sta_flush(struct adapter *padapter)
 {
 	struct list_head *phead, *plist;
-	int ret = 0;
 	struct sta_info *psta = NULL;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
@@ -1798,7 +1795,7 @@ int rtw_sta_flush(struct adapter *padapter)
 	DBG_88E(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));
 
 	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
-		return ret;
+		return 0;
 
 	spin_lock_bh(&pstapriv->asoc_list_lock);
 	phead = &pstapriv->asoc_list;
@@ -1822,7 +1819,7 @@ int rtw_sta_flush(struct adapter *padapter)
 
 	associated_clients_update(padapter, true);
 
-	return ret;
+	return 0;
 }
 
 /* called > TSR LEVEL for USB or SDIO Interface*/
diff --git a/drivers/staging/rtl8188eu/core/rtw_cmd.c b/drivers/staging/rtl8188eu/core/rtw_cmd.c
index eddef9cd2e16..4b4346244953 100644
--- a/drivers/staging/rtl8188eu/core/rtw_cmd.c
+++ b/drivers/staging/rtl8188eu/core/rtw_cmd.c
@@ -167,7 +167,7 @@ int rtw_cmd_thread(void *context)
 	struct cmd_obj *pcmd;
 	u8 (*cmd_hdl)(struct adapter *padapter, u8 *pbuf);
 	void (*pcmd_callback)(struct adapter *dev, struct cmd_obj *pcmd);
-	struct adapter *padapter = (struct adapter *)context;
+	struct adapter *padapter = context;
 	struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
 
 	allow_signal(SIGTERM);
@@ -433,8 +433,7 @@ u8 rtw_joinbss_cmd(struct adapter  *padapter, struct wlan_network *pnetwork)
 
 	psecnetwork = (struct wlan_bssid_ex *)&psecuritypriv->sec_bss;
 	if (psecnetwork == NULL) {
-		if (pcmd != NULL)
-			kfree(pcmd);
+		kfree(pcmd);
 
 		res = _FAIL;
 
@@ -456,7 +455,7 @@ u8 rtw_joinbss_cmd(struct adapter  *padapter, struct wlan_network *pnetwork)
 
 	psecnetwork->IELength = 0;
 	/*  Added by Albert 2009/02/18 */
-	/*  If the the driver wants to use the bssid to create the connection. */
+	/*  If the driver wants to use the bssid to create the connection. */
 	/*  If not,  we have to copy the connecting AP's MAC address to it so that */
 	/*  the driver just has the bssid information for PMKIDList searching. */
 
@@ -638,7 +637,7 @@ u8 rtw_setstakey_cmd(struct adapter *padapter, u8 *psta, u8 unicast_key)
 	ether_addr_copy(psetstakey_para->addr, sta->hwaddr);
 
 	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
-		psetstakey_para->algorithm = (unsigned char) psecuritypriv->dot11PrivacyAlgrthm;
+		psetstakey_para->algorithm = (unsigned char)psecuritypriv->dot11PrivacyAlgrthm;
 	else
 		GET_ENCRY_ALGO(psecuritypriv, sta, psetstakey_para->algorithm, false);
 
diff --git a/drivers/staging/rtl8188eu/core/rtw_debug.c b/drivers/staging/rtl8188eu/core/rtw_debug.c
index 1f72f7d8097e..bc3fe10ff247 100644
--- a/drivers/staging/rtl8188eu/core/rtw_debug.c
+++ b/drivers/staging/rtl8188eu/core/rtw_debug.c
@@ -45,7 +45,7 @@ int proc_get_write_reg(char *page, char **start,
 int proc_set_write_reg(struct file *file, const char __user *buffer,
 		unsigned long count, void *data)
 {
-	struct net_device *dev = (struct net_device *)data;
+	struct net_device *dev = data;
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
 	char tmp[32];
 	u32 addr, val, len;
@@ -577,7 +577,7 @@ int proc_get_rx_signal(char *page, char **start,
 int proc_set_rx_signal(struct file *file, const char __user *buffer,
 		unsigned long count, void *data)
 {
-	struct net_device *dev = (struct net_device *)data;
+	struct net_device *dev = data;
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
 	char tmp[32];
 	u32 is_signal_dbg;
@@ -627,7 +627,7 @@ int proc_get_ht_enable(char *page, char **start,
 int proc_set_ht_enable(struct file *file, const char __user *buffer,
 		unsigned long count, void *data)
 {
-	struct net_device *dev = (struct net_device *)data;
+	struct net_device *dev = data;
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
 	struct registry_priv	*pregpriv = &padapter->registrypriv;
 	char tmp[32];
@@ -669,7 +669,7 @@ int proc_get_cbw40_enable(char *page, char **start,
 int proc_set_cbw40_enable(struct file *file, const char __user *buffer,
 		unsigned long count, void *data)
 {
-	struct net_device *dev = (struct net_device *)data;
+	struct net_device *dev = data;
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
 	struct registry_priv	*pregpriv = &padapter->registrypriv;
 	char tmp[32];
@@ -710,7 +710,7 @@ int proc_get_ampdu_enable(char *page, char **start,
 int proc_set_ampdu_enable(struct file *file, const char __user *buffer,
 		unsigned long count, void *data)
 {
-	struct net_device *dev = (struct net_device *)data;
+	struct net_device *dev = data;
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
 	struct registry_priv	*pregpriv = &padapter->registrypriv;
 	char tmp[32];
@@ -771,7 +771,7 @@ int proc_get_rx_stbc(char *page, char **start,
 int proc_set_rx_stbc(struct file *file, const char __user *buffer,
 		unsigned long count, void *data)
 {
-	struct net_device *dev = (struct net_device *)data;
+	struct net_device *dev = data;
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
 	struct registry_priv	*pregpriv = &padapter->registrypriv;
 	char tmp[32];
@@ -800,7 +800,7 @@ int proc_get_rssi_disp(char *page, char **start,
 int proc_set_rssi_disp(struct file *file, const char __user *buffer,
 		unsigned long count, void *data)
 {
-	struct net_device *dev = (struct net_device *)data;
+	struct net_device *dev = data;
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
 	char tmp[32];
 	u32 enable = 0;
diff --git a/drivers/staging/rtl8188eu/core/rtw_efuse.c b/drivers/staging/rtl8188eu/core/rtw_efuse.c
index 7006088d1ad0..8816d116a8b8 100644
--- a/drivers/staging/rtl8188eu/core/rtw_efuse.c
+++ b/drivers/staging/rtl8188eu/core/rtw_efuse.c
@@ -106,13 +106,13 @@ efuse_phymap_to_logical(u8 *phymap, u16 _offset, u16 _size_byte, u8  *pbuf)
 	efuseTbl = kzalloc(EFUSE_MAP_LEN_88E, GFP_KERNEL);
 	if (efuseTbl == NULL) {
 		DBG_88E("%s: alloc efuseTbl fail!\n", __func__);
-		goto exit;
+		return;
 	}
 
 	eFuseWord = (u16 **)rtw_malloc2d(EFUSE_MAX_SECTION_88E, EFUSE_MAX_WORD_UNIT, sizeof(u16));
 	if (eFuseWord == NULL) {
 		DBG_88E("%s: alloc eFuseWord fail!\n", __func__);
-		goto exit;
+		goto eFuseWord_failed;
 	}
 
 	/*  0. Refresh efuse init map as all oxFF. */
@@ -210,10 +210,10 @@ efuse_phymap_to_logical(u8 *phymap, u16 _offset, u16 _size_byte, u8  *pbuf)
 	/*  */
 
 exit:
-	kfree(efuseTbl);
+	kfree(eFuseWord);
 
-	if (eFuseWord)
-		kfree(eFuseWord);
+eFuseWord_failed:
+	kfree(efuseTbl);
 }
 
 static void efuse_read_phymap_from_txpktbuf(
@@ -250,7 +250,7 @@ static void efuse_read_phymap_from_txpktbuf(
 		while (!(reg_0x143 = usb_read8(adapter, REG_TXPKTBUF_DBG)) &&
 		       (passing_time = rtw_get_passing_time_ms(start)) < 1000) {
 			DBG_88E("%s polling reg_0x143:0x%02x, reg_0x106:0x%02x\n", __func__, reg_0x143, usb_read8(adapter, 0x106));
-			msleep(1);
+			usleep_range(1000, 2000);
 		}
 
 		lo32 = usb_read32(adapter, REG_PKTBUF_DBG_DATA_L);
@@ -322,7 +322,6 @@ void efuse_ReadEFuse(struct adapter *Adapter, u8 efuseType, u16 _offset, u16 _si
 		iol_read_efuse(Adapter, 0, _offset, _size_byte, pbuf);
 		iol_mode_enable(Adapter, 0);
 	}
-	return;
 }
 
 /* Do not support BT */
@@ -332,56 +331,56 @@ void EFUSE_GetEfuseDefinition(struct adapter *pAdapter, u8 efuseType, u8 type, v
 	case TYPE_EFUSE_MAX_SECTION:
 		{
 			u8 *pMax_section;
-			pMax_section = (u8 *)pOut;
+			pMax_section = pOut;
 			*pMax_section = EFUSE_MAX_SECTION_88E;
 		}
 		break;
 	case TYPE_EFUSE_REAL_CONTENT_LEN:
 		{
 			u16 *pu2Tmp;
-			pu2Tmp = (u16 *)pOut;
+			pu2Tmp = pOut;
 			*pu2Tmp = EFUSE_REAL_CONTENT_LEN_88E;
 		}
 		break;
 	case TYPE_EFUSE_CONTENT_LEN_BANK:
 		{
 			u16 *pu2Tmp;
-			pu2Tmp = (u16 *)pOut;
+			pu2Tmp = pOut;
 			*pu2Tmp = EFUSE_REAL_CONTENT_LEN_88E;
 		}
 		break;
 	case TYPE_AVAILABLE_EFUSE_BYTES_BANK:
 		{
 			u16 *pu2Tmp;
-			pu2Tmp = (u16 *)pOut;
+			pu2Tmp = pOut;
 			*pu2Tmp = (u16)(EFUSE_REAL_CONTENT_LEN_88E-EFUSE_OOB_PROTECT_BYTES_88E);
 		}
 		break;
 	case TYPE_AVAILABLE_EFUSE_BYTES_TOTAL:
 		{
 			u16 *pu2Tmp;
-			pu2Tmp = (u16 *)pOut;
+			pu2Tmp = pOut;
 			*pu2Tmp = (u16)(EFUSE_REAL_CONTENT_LEN_88E-EFUSE_OOB_PROTECT_BYTES_88E);
 		}
 		break;
 	case TYPE_EFUSE_MAP_LEN:
 		{
 			u16 *pu2Tmp;
-			pu2Tmp = (u16 *)pOut;
+			pu2Tmp = pOut;
 			*pu2Tmp = (u16)EFUSE_MAP_LEN_88E;
 		}
 		break;
 	case TYPE_EFUSE_PROTECT_BYTES_BANK:
 		{
 			u8 *pu1Tmp;
-			pu1Tmp = (u8 *)pOut;
+			pu1Tmp = pOut;
 			*pu1Tmp = (u8)(EFUSE_OOB_PROTECT_BYTES_88E);
 		}
 		break;
 	default:
 		{
 			u8 *pu1Tmp;
-			pu1Tmp = (u8 *)pOut;
+			pu1Tmp = pOut;
 			*pu1Tmp = 0;
 		}
 		break;
@@ -638,10 +637,9 @@ static bool hal_EfusePgPacketWrite2ByteHeader(struct adapter *pAdapter, u8 efuse
 			if ((tmp_header & 0x0F) == 0x0F) {	/* word_en PG fail */
 				if (repeatcnt++ > EFUSE_REPEAT_THRESHOLD_) {
 					return false;
-				} else {
-					efuse_addr++;
-					continue;
 				}
+				efuse_addr++;
+				continue;
 			} else if (pg_header != tmp_header) {	/* offset PG fail */
 				struct pgpkt	fixPkt;
 				fixPkt.offset = ((pg_header_temp & 0xE0) >> 5) | ((tmp_header & 0xF0) >> 1);
@@ -708,14 +706,13 @@ static bool hal_EfusePgPacketWriteData(struct adapter *pAdapter, u8 efuseType, u
 	if (badworden == 0x0F) {
 		/*  write ok */
 		return true;
-	} else {
-		/* reorganize other pg packet */
-		PgWriteSuccess = Efuse_PgPacketWrite(pAdapter, pTargetPkt->offset, badworden, pTargetPkt->data);
-		if (!PgWriteSuccess)
-			return false;
-		else
-			return true;
 	}
+	/* reorganize other pg packet */
+	PgWriteSuccess = Efuse_PgPacketWrite(pAdapter, pTargetPkt->offset, badworden, pTargetPkt->data);
+	if (!PgWriteSuccess)
+		return false;
+	else
+		return true;
 }
 
 static bool
diff --git a/drivers/staging/rtl8188eu/core/rtw_ieee80211.c b/drivers/staging/rtl8188eu/core/rtw_ieee80211.c
index 755d3effd0a7..f2c3ca79c0c9 100644
--- a/drivers/staging/rtl8188eu/core/rtw_ieee80211.c
+++ b/drivers/staging/rtl8188eu/core/rtw_ieee80211.c
@@ -159,7 +159,7 @@ u8 *rtw_set_ie
 	return pbuf + len + 2;
 }
 
-inline u8 *rtw_set_ie_ch_switch (u8 *buf, u32 *buf_len, u8 ch_switch_mode,
+inline u8 *rtw_set_ie_ch_switch(u8 *buf, u32 *buf_len, u8 ch_switch_mode,
 	u8 new_ch, u8 ch_switch_cnt)
 {
 	u8 ie_data[3];
@@ -870,7 +870,7 @@ static int rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen,
 	if (elen < 4) {
 		if (show_errors) {
 			DBG_88E("short vendor specific information element ignored (len=%lu)\n",
-				(unsigned long) elen);
+				(unsigned long)elen);
 		}
 		return -1;
 	}
@@ -890,7 +890,7 @@ static int rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen,
 		case WME_OUI_TYPE: /* this is a Wi-Fi WME info. element */
 			if (elen < 5) {
 				DBG_88E("short WME information element ignored (len=%lu)\n",
-					(unsigned long) elen);
+					(unsigned long)elen);
 				return -1;
 			}
 			switch (pos[4]) {
@@ -905,7 +905,7 @@ static int rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen,
 				break;
 			default:
 				DBG_88E("unknown WME information element ignored (subtype=%d len=%lu)\n",
-					pos[4], (unsigned long) elen);
+					pos[4], (unsigned long)elen);
 				return -1;
 			}
 			break;
@@ -916,7 +916,7 @@ static int rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen,
 			break;
 		default:
 			DBG_88E("Unknown Microsoft information element ignored (type=%d len=%lu)\n",
-				pos[3], (unsigned long) elen);
+				pos[3], (unsigned long)elen);
 			return -1;
 		}
 		break;
@@ -929,13 +929,13 @@ static int rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen,
 			break;
 		default:
 			DBG_88E("Unknown Broadcom information element ignored (type=%d len=%lu)\n",
-				pos[3], (unsigned long) elen);
+				pos[3], (unsigned long)elen);
 			return -1;
 		}
 		break;
 	default:
 		DBG_88E("unknown vendor specific information element ignored (vendor OUI %02x:%02x:%02x len=%lu)\n",
-			pos[0], pos[1], pos[2], (unsigned long) elen);
+			pos[0], pos[1], pos[2], (unsigned long)elen);
 		return -1;
 	}
 	return 0;
@@ -969,7 +969,7 @@ enum parse_res rtw_ieee802_11_parse_elems(u8 *start, uint len,
 		if (elen > left) {
 			if (show_errors) {
 				DBG_88E("IEEE 802.11 element parse failed (id=%d elen=%d left=%lu)\n",
-					id, elen, (unsigned long) left);
+					id, elen, (unsigned long)left);
 			}
 			return ParseFailed;
 		}
diff --git a/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c b/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c
index fc280ce57d2c..2faf6b2e8129 100644
--- a/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c
+++ b/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c
@@ -98,7 +98,6 @@ u8 rtw_do_join(struct adapter *padapter)
 
 				pibss = padapter->registrypriv.dev_network.MacAddress;
 
-				memset(&pdev_network->Ssid, 0, sizeof(struct ndis_802_11_ssid));
 				memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct ndis_802_11_ssid));
 
 				rtw_update_registrypriv_dev_network(padapter);
diff --git a/drivers/staging/rtl8188eu/core/rtw_led.c b/drivers/staging/rtl8188eu/core/rtw_led.c
index 384be22052e5..1b8264b978da 100644
--- a/drivers/staging/rtl8188eu/core/rtw_led.c
+++ b/drivers/staging/rtl8188eu/core/rtw_led.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU 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
- *
  *
  ******************************************************************************/
 
@@ -28,7 +24,7 @@
 /*  */
 void BlinkTimerCallback(void *data)
 {
-	struct LED_871x *pLed = (struct LED_871x *)data;
+	struct LED_871x *pLed = data;
 	struct adapter *padapter = pLed->padapter;
 
 	if ((padapter->bSurpriseRemoved) || (padapter->bDriverStopped))
@@ -228,7 +224,8 @@ static void SwLedBlink1(struct LED_871x *pLed)
 			pLed->bLedWPSBlinkInProgress = false;
 		} else {
 			pLed->BlinkingLedState = RTW_LED_OFF;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA);
+			_set_timer(&(pLed->BlinkTimer),
+				   LED_BLINK_WPS_SUCCESS_INTERVAL_ALPHA);
 		}
 		break;
 	default:
@@ -392,7 +389,8 @@ static void SwLedControlMode1(struct adapter *padapter, enum LED_CTL_MODE LedAct
 		pLed->CurrLedState = LED_BLINK_WPS_STOP;
 		if (pLed->bLedOn) {
 			pLed->BlinkingLedState = RTW_LED_OFF;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA);
+			_set_timer(&(pLed->BlinkTimer),
+				   LED_BLINK_WPS_SUCCESS_INTERVAL_ALPHA);
 		} else {
 			pLed->BlinkingLedState = RTW_LED_ON;
 			_set_timer(&(pLed->BlinkTimer), 0);
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme.c b/drivers/staging/rtl8188eu/core/rtw_mlme.c
index 149c271e966d..d4632da50c1d 100644
--- a/drivers/staging/rtl8188eu/core/rtw_mlme.c
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme.c
@@ -674,7 +674,6 @@ void rtw_surveydone_event_callback(struct adapter	*adapter, u8 *pbuf)
 
 					RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("switching to adhoc master\n"));
 
-					memset(&pdev_network->Ssid, 0, sizeof(struct ndis_802_11_ssid));
 					memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct ndis_802_11_ssid));
 
 					rtw_update_registrypriv_dev_network(adapter);
@@ -1334,7 +1333,6 @@ void rtw_stadel_event_callback(struct adapter *adapter, u8 *pbuf)
 
 			memcpy(pdev_network, &tgt_network->network, get_wlan_bssid_ex_sz(&tgt_network->network));
 
-			memset(&pdev_network->Ssid, 0, sizeof(struct ndis_802_11_ssid));
 			memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct ndis_802_11_ssid));
 
 			rtw_update_registrypriv_dev_network(adapter);
@@ -1364,7 +1362,7 @@ void rtw_cpwm_event_callback(struct adapter *padapter, u8 *pbuf)
 */
 void _rtw_join_timeout_handler (void *function_context)
 {
-	struct adapter *adapter = (struct adapter *)function_context;
+	struct adapter *adapter = function_context;
 	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
 	int do_join_r;
 
@@ -1406,7 +1404,7 @@ void _rtw_join_timeout_handler (void *function_context)
 */
 void rtw_scan_timeout_handler (void *function_context)
 {
-	struct adapter *adapter = (struct adapter *)function_context;
+	struct adapter *adapter = function_context;
 	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
 
 	DBG_88E(FUNC_ADPT_FMT" fw_state=%x\n", FUNC_ADPT_ARG(adapter), get_fwstate(pmlmepriv));
@@ -1437,7 +1435,7 @@ void rtw_dynamic_check_timer_handlder(void *function_context)
 	struct registry_priv *pregistrypriv = &adapter->registrypriv;
 
 	if (!adapter)
-		goto exit;
+		return;
 	if (!adapter->hw_init_completed)
 		goto exit;
 	if ((adapter->bDriverStopped) || (adapter->bSurpriseRemoved))
@@ -2117,7 +2115,7 @@ void rtw_issue_addbareq_cmd(struct adapter *padapter, struct xmit_frame *pxmitfr
 		if (0 == issued) {
 			DBG_88E("rtw_issue_addbareq_cmd, p=%d\n", priority);
 			psta->htpriv.candidate_tid_bitmap |= BIT((u8)priority);
-			rtw_addbareq_cmd(padapter, (u8) priority, pattrib->ra);
+			rtw_addbareq_cmd(padapter, (u8)priority, pattrib->ra);
 		}
 	}
 }
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
index 70b1bc3e0e63..e4b7ee4c99d5 100644
--- a/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
@@ -227,7 +227,7 @@ static void init_mlme_ext_priv_value(struct adapter *padapter)
 	pmlmeext->cur_channel = padapter->registrypriv.channel;
 	pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
 	pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
-	pmlmeext->oper_channel = pmlmeext->cur_channel ;
+	pmlmeext->oper_channel = pmlmeext->cur_channel;
 	pmlmeext->oper_bwmode = pmlmeext->cur_bwmode;
 	pmlmeext->oper_ch_offset = pmlmeext->cur_ch_offset;
 	pmlmeext->retry = 0;
@@ -371,7 +371,6 @@ static u8 init_channel_set(struct adapter *padapter, u8 ChannelPlan, struct rt_c
 
 int	init_mlme_ext_priv(struct adapter *padapter)
 {
-	int	res = _SUCCESS;
 	struct registry_priv *pregistrypriv = &padapter->registrypriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
@@ -397,7 +396,7 @@ int	init_mlme_ext_priv(struct adapter *padapter)
 
 	pmlmeext->active_keep_alive_check = true;
 
-	return res;
+	return _SUCCESS;
 }
 
 void free_mlme_ext_priv(struct mlme_ext_priv *pmlmeext)
@@ -945,7 +944,7 @@ unsigned int OnAssocReq(struct adapter *padapter, struct recv_frame *precv_frame
 	}
 
 	pstat = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
-	if (pstat == (struct sta_info *)NULL) {
+	if (pstat == NULL) {
 		status = _RSON_CLS2_;
 		goto asoc_class2_error;
 	}
@@ -1554,7 +1553,6 @@ unsigned int OnAtim(struct adapter *padapter, struct recv_frame *precv_frame)
 
 unsigned int on_action_spct(struct adapter *padapter, struct recv_frame *precv_frame)
 {
-	unsigned int ret = _FAIL;
 	struct sta_info *psta = NULL;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	u8 *pframe = precv_frame->rx_data;
@@ -1587,7 +1585,7 @@ unsigned int on_action_spct(struct adapter *padapter, struct recv_frame *precv_f
 	}
 
 exit:
-	return ret;
+	return _FAIL;
 }
 
 unsigned int OnAction_qos(struct adapter *padapter, struct recv_frame *precv_frame)
@@ -2000,7 +1998,7 @@ void issue_beacon(struct adapter *padapter, int timeout_ms)
 		DBG_88E("%s, alloc mgnt frame fail\n", __func__);
 		return;
 	}
-#if defined (CONFIG_88EU_AP_MODE)
+#if defined(CONFIG_88EU_AP_MODE)
 	spin_lock_bh(&pmlmepriv->bcn_update_lock);
 #endif /* if defined (CONFIG_88EU_AP_MODE) */
 
@@ -2027,7 +2025,7 @@ void issue_beacon(struct adapter *padapter, int timeout_ms)
 	SetFrameSubType(pframe, WIFI_BEACON);
 
 	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
 
 	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) {
 		int len_diff;
@@ -2042,8 +2040,8 @@ void issue_beacon(struct adapter *padapter, int timeout_ms)
 			);
 		pframe += (cur_network->IELength+len_diff);
 		pattrib->pktlen += (cur_network->IELength+len_diff);
-		wps_ie = rtw_get_wps_ie(pmgntframe->buf_addr+TXDESC_OFFSET+sizeof (struct rtw_ieee80211_hdr_3addr)+_BEACON_IE_OFFSET_,
-			pattrib->pktlen-sizeof (struct rtw_ieee80211_hdr_3addr)-_BEACON_IE_OFFSET_, NULL, &wps_ielen);
+		wps_ie = rtw_get_wps_ie(pmgntframe->buf_addr+TXDESC_OFFSET+sizeof(struct rtw_ieee80211_hdr_3addr)+_BEACON_IE_OFFSET_,
+			pattrib->pktlen-sizeof(struct rtw_ieee80211_hdr_3addr)-_BEACON_IE_OFFSET_, NULL, &wps_ielen);
 		if (wps_ie && wps_ielen > 0)
 			rtw_get_wps_attr_content(wps_ie,  wps_ielen, WPS_ATTR_SELECTED_REGISTRAR, (u8 *)(&sr), NULL);
 		if (sr != 0)
@@ -2101,7 +2099,7 @@ void issue_beacon(struct adapter *padapter, int timeout_ms)
 	/* todo:HT for adhoc */
 _issue_bcn:
 
-#if defined (CONFIG_88EU_AP_MODE)
+#if defined(CONFIG_88EU_AP_MODE)
 	pmlmepriv->update_bcn = false;
 
 	spin_unlock_bh(&pmlmepriv->bcn_update_lock);
@@ -2130,7 +2128,7 @@ void issue_probersp(struct adapter *padapter, unsigned char *da, u8 is_valid_p2p
 	__le16 *fctrl;
 	unsigned char					*mac, *bssid;
 	struct xmit_priv	*pxmitpriv = &(padapter->xmitpriv);
-#if defined (CONFIG_88EU_AP_MODE)
+#if defined(CONFIG_88EU_AP_MODE)
 	u8 *pwps_ie;
 	uint wps_ielen;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
@@ -2323,8 +2321,8 @@ static int _issue_probereq(struct adapter *padapter, struct ndis_802_11_ssid *ps
 	pmlmeext->mgnt_seq++;
 	SetFrameSubType(pframe, WIFI_PROBEREQ);
 
-	pframe += sizeof (struct rtw_ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
 
 	if (pssid)
 		pframe = rtw_set_ie(pframe, _SSID_IE_, pssid->SsidLength, pssid->Ssid, &(pattrib->pktlen));
@@ -3209,7 +3207,7 @@ exit:
 	return ret;
 }
 
-void issue_action_spct_ch_switch (struct adapter *padapter, u8 *ra, u8 new_ch, u8 ch_offset)
+void issue_action_spct_ch_switch(struct adapter *padapter, u8 *ra, u8 new_ch, u8 ch_offset)
 {
 	struct xmit_frame			*pmgntframe;
 	struct pkt_attrib			*pattrib;
@@ -3260,7 +3258,7 @@ void issue_action_spct_ch_switch (struct adapter *padapter, u8 *ra, u8 new_ch, u
 		pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
 	}
 
-	pframe = rtw_set_ie_ch_switch (pframe, &(pattrib->pktlen), 0, new_ch, 0);
+	pframe = rtw_set_ie_ch_switch(pframe, &(pattrib->pktlen), 0, new_ch, 0);
 	pframe = rtw_set_ie_secondary_ch_offset(pframe, &(pattrib->pktlen),
 		hal_ch_offset_to_secondary_ch_offset(ch_offset));
 
@@ -4835,7 +4833,7 @@ void linked_status_chk(struct adapter *padapter)
 
 void survey_timer_hdl(void *function_context)
 {
-	struct adapter *padapter = (struct adapter *)function_context;
+	struct adapter *padapter = function_context;
 	struct cmd_obj	*ph2c;
 	struct sitesurvey_parm	*psurveyPara;
 	struct cmd_priv					*pcmdpriv = &padapter->cmdpriv;
@@ -4912,7 +4910,7 @@ void link_timer_hdl(void *function_context)
 
 void addba_timer_hdl(void *function_context)
 {
-	struct sta_info *psta = (struct sta_info *)function_context;
+	struct sta_info *psta = function_context;
 	struct ht_priv	*phtpriv;
 
 	if (!psta)
diff --git a/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c b/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
index 27ed83cca193..df463a29b641 100644
--- a/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
+++ b/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
@@ -279,12 +279,11 @@ void rtw_ps_processor(struct adapter *padapter)
 exit:
 	rtw_set_pwr_state_check_timer(&padapter->pwrctrlpriv);
 	pwrpriv->ps_processing = false;
-	return;
 }
 
 static void pwr_state_check_handler(void *FunctionContext)
 {
-	struct adapter *padapter = (struct adapter *)FunctionContext;
+	struct adapter *padapter = FunctionContext;
 	rtw_ps_cmd(padapter);
 }
 
@@ -527,7 +526,7 @@ void rtw_init_pwrctrl_priv(struct adapter *padapter)
 
 	pwrctrlpriv->LpsIdleCount = 0;
 	if (padapter->registrypriv.mp_mode == 1)
-		pwrctrlpriv->power_mgnt = PS_MODE_ACTIVE ;
+		pwrctrlpriv->power_mgnt = PS_MODE_ACTIVE;
 	else
 		pwrctrlpriv->power_mgnt = padapter->registrypriv.power_mgnt;/*  PS_MODE_MIN; */
 	pwrctrlpriv->bLeisurePs = (PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt) ? true : false;
@@ -577,7 +576,7 @@ int _rtw_pwr_wakeup(struct adapter *padapter, u32 ips_deffer_ms, const char *cal
 	if (pwrpriv->ps_processing) {
 		DBG_88E("%s wait ps_processing...\n", __func__);
 		while (pwrpriv->ps_processing && rtw_get_passing_time_ms(start) <= 3000)
-			msleep(10);
+			usleep_range(1000, 3000);
 		if (pwrpriv->ps_processing)
 			DBG_88E("%s wait ps_processing timeout\n", __func__);
 		else
diff --git a/drivers/staging/rtl8188eu/core/rtw_recv.c b/drivers/staging/rtl8188eu/core/rtw_recv.c
index 4d56dbad2a7d..bd79e9e7105a 100644
--- a/drivers/staging/rtl8188eu/core/rtw_recv.c
+++ b/drivers/staging/rtl8188eu/core/rtw_recv.c
@@ -46,7 +46,7 @@ void rtw_signal_stat_timer_hdl(RTW_TIMER_HDL_ARGS);
 void _rtw_init_sta_recv_priv(struct sta_recv_priv *psta_recvpriv)
 {
 
-	memset((u8 *)psta_recvpriv, 0, sizeof (struct sta_recv_priv));
+	memset((u8 *)psta_recvpriv, 0, sizeof(struct sta_recv_priv));
 
 	spin_lock_init(&psta_recvpriv->lock);
 
@@ -109,7 +109,7 @@ exit:
 	return res;
 }
 
-void _rtw_free_recv_priv (struct recv_priv *precvpriv)
+void _rtw_free_recv_priv(struct recv_priv *precvpriv)
 {
 	struct adapter	*padapter = precvpriv->adapter;
 
@@ -124,7 +124,7 @@ void _rtw_free_recv_priv (struct recv_priv *precvpriv)
 
 }
 
-struct recv_frame *_rtw_alloc_recvframe (struct __queue *pfree_recv_queue)
+struct recv_frame *_rtw_alloc_recvframe(struct __queue *pfree_recv_queue)
 {
 	struct recv_frame *hdr;
 	struct list_head *plist, *phead;
@@ -797,7 +797,7 @@ exit:
 	return ret;
 }
 
-static int ap2sta_data_frame (
+static int ap2sta_data_frame(
 	struct adapter *adapter,
 	struct recv_frame *precv_frame,
 	struct sta_info **psta)
@@ -1266,7 +1266,7 @@ static int validate_recv_frame(struct adapter *adapter,
 	u8 bDumpRxPkt;
 	struct rx_pkt_attrib *pattrib = &precv_frame->attrib;
 	u8 *ptr = precv_frame->rx_data;
-	u8  ver = (unsigned char) (*ptr)&0x3;
+	u8  ver = (unsigned char)(*ptr)&0x3;
 	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
 
 
@@ -1373,7 +1373,6 @@ static int wlanhdr_to_ethhdr(struct recv_frame *precvframe)
 	u8	*psnap_type;
 	struct ieee80211_snap_hdr	*psnap;
 
-	int ret = _SUCCESS;
 	struct adapter		*adapter = precvframe->adapter;
 	struct mlme_priv	*pmlmepriv = &adapter->mlmepriv;
 	u8 *ptr = precvframe->rx_data;
@@ -1428,7 +1427,7 @@ static int wlanhdr_to_ethhdr(struct recv_frame *precvframe)
 		memcpy(ptr+12, &be_tmp, 2);
 	}
 
-	return ret;
+	return _SUCCESS;
 }
 
 /* perform defrag */
@@ -1624,7 +1623,6 @@ static int amsdu_to_msdu(struct adapter *padapter, struct recv_frame *prframe)
 	struct sk_buff *sub_skb, *subframes[MAX_SUBFRAME_COUNT];
 	struct recv_priv *precvpriv = &padapter->recvpriv;
 	struct __queue *pfree_recv_queue = &(precvpriv->free_recv_queue);
-	int	ret = _SUCCESS;
 	nr_subframes = 0;
 
 	pattrib = &prframe->attrib;
@@ -1728,7 +1726,7 @@ exit:
 	prframe->len = 0;
 	rtw_free_recvframe(prframe, pfree_recv_queue);/* free this recv_frame */
 
-	return ret;
+	return _SUCCESS;
 }
 
 static int check_indicate_seq(struct recv_reorder_ctrl *preorder_ctrl, u16 seq_num)
@@ -1949,7 +1947,7 @@ _err_exit:
 
 void rtw_reordering_ctrl_timeout_handler(void *pcontext)
 {
-	struct recv_reorder_ctrl *preorder_ctrl = (struct recv_reorder_ctrl *)pcontext;
+	struct recv_reorder_ctrl *preorder_ctrl = pcontext;
 	struct adapter *padapter = preorder_ctrl->padapter;
 	struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
 
@@ -1981,7 +1979,7 @@ static int process_recv_indicatepkts(struct adapter *padapter,
 			}
 		}
 	} else { /* B/G mode */
-		retval = wlanhdr_to_ethhdr (prframe);
+		retval = wlanhdr_to_ethhdr(prframe);
 		if (retval != _SUCCESS) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("wlanhdr_to_ethhdr: drop pkt\n"));
 			return retval;
diff --git a/drivers/staging/rtl8188eu/core/rtw_security.c b/drivers/staging/rtl8188eu/core/rtw_security.c
index f9096a512da5..bd8d60a230e9 100644
--- a/drivers/staging/rtl8188eu/core/rtw_security.c
+++ b/drivers/staging/rtl8188eu/core/rtw_security.c
@@ -189,7 +189,7 @@ void rtw_wep_encrypt(struct adapter *padapter, u8 *pxmitframe)
 				arcfour_encrypt(&mycontext, payload+length, crc, 4);
 
 				pframe += pxmitpriv->frag_len;
-				pframe = (u8 *) round_up((size_t)(pframe), 4);
+				pframe = (u8 *)round_up((size_t)(pframe), 4);
 			}
 		}
 	}
@@ -258,7 +258,7 @@ static void secmicputuint32(u8 *p, u32 val)
 {
 	long i;
 	for (i = 0; i < 4; i++) {
-		*p++ = (u8) (val & 0xff);
+		*p++ = (u8)(val & 0xff);
 		val >>= 8;
 	}
 }
@@ -621,14 +621,14 @@ u32	rtw_tkip_encrypt(struct adapter *padapter, u8 *pxmitframe)
 					arcfour_encrypt(&mycontext, payload, payload, length);
 					arcfour_encrypt(&mycontext, payload+length, crc, 4);
 				} else {
-					length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len ;
+					length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
 					*((__le32 *)crc) = getcrc32(payload, length);/* modified by Amy*/
 					arcfour_init(&mycontext, rc4key, 16);
 					arcfour_encrypt(&mycontext, payload, payload, length);
 					arcfour_encrypt(&mycontext, payload+length, crc, 4);
 
 					pframe += pxmitpriv->frag_len;
-					pframe = (u8 *) round_up((size_t)(pframe), 4);
+					pframe = (u8 *)round_up((size_t)(pframe), 4);
 				}
 			}
 		} else {
@@ -953,8 +953,8 @@ static void construct_mic_iv(u8 *mic_iv, int qc_exists, int a4_exists, u8 *mpdu,
 		mic_iv[i] = mpdu[i + 8];	/* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
 	for (i = 8; i < 14; i++)
 		mic_iv[i] = pn_vector[13 - i];	/* mic_iv[8:13] = PN[5:0] */
-	mic_iv[14] = (unsigned char) (payload_length / 256);
-	mic_iv[15] = (unsigned char) (payload_length % 256);
+	mic_iv[14] = (unsigned char)(payload_length / 256);
+	mic_iv[15] = (unsigned char)(payload_length % 256);
 }
 
 /************************************************/
@@ -1045,8 +1045,8 @@ static void construct_ctr_preload(u8 *ctr_preload, int a4_exists, int qc_exists,
 		ctr_preload[i] = mpdu[i + 8];		       /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
 	for (i = 8; i < 14; i++)
 		ctr_preload[i] =    pn_vector[13 - i];	  /* ctr_preload[8:13] = PN[5:0] */
-	ctr_preload[14] =  (unsigned char) (c / 256); /* Ctr */
-	ctr_preload[15] =  (unsigned char) (c % 256);
+	ctr_preload[14] =  (unsigned char)(c / 256); /* Ctr */
+	ctr_preload[15] =  (unsigned char)(c % 256);
 }
 
 /************************************/
@@ -1219,7 +1219,7 @@ u32	rtw_aes_encrypt(struct adapter *padapter, u8 *pxmitframe)
 	pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset;
 
 	/* 4 start to encrypt each fragment */
-	if ((pattrib->encrypt == _AES_)) {
+	if (pattrib->encrypt == _AES_) {
 		if (pattrib->psta)
 			stainfo = pattrib->psta;
 		else
@@ -1238,11 +1238,11 @@ u32	rtw_aes_encrypt(struct adapter *padapter, u8 *pxmitframe)
 
 					aes_cipher(prwskey, pattrib->hdrlen, pframe, length);
 				} else{
-					length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len ;
+					length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
 
 					aes_cipher(prwskey, pattrib->hdrlen, pframe, length);
 					pframe += pxmitpriv->frag_len;
-					pframe = (u8 *) round_up((size_t)(pframe), 8);
+					pframe = (u8 *)round_up((size_t)(pframe), 8);
 				}
 			}
 		} else{
@@ -1460,7 +1460,7 @@ u32	rtw_aes_decrypt(struct adapter *padapter, u8 *precvframe)
 	u32	res = _SUCCESS;
 	pframe = (unsigned char *)((struct recv_frame *)precvframe)->rx_data;
 	/* 4 start to encrypt each fragment */
-	if ((prxattrib->encrypt == _AES_)) {
+	if (prxattrib->encrypt == _AES_) {
 		stainfo = rtw_get_stainfo(&padapter->stapriv, &prxattrib->ta[0]);
 		if (stainfo != NULL) {
 			RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_aes_decrypt: stainfo!= NULL!!!\n"));
diff --git a/drivers/staging/rtl8188eu/core/rtw_sta_mgt.c b/drivers/staging/rtl8188eu/core/rtw_sta_mgt.c
index e1dc8fa82d38..dc9d0ddf6b3a 100644
--- a/drivers/staging/rtl8188eu/core/rtw_sta_mgt.c
+++ b/drivers/staging/rtl8188eu/core/rtw_sta_mgt.c
@@ -29,7 +29,7 @@
 
 static void _rtw_init_stainfo(struct sta_info *psta)
 {
-	memset((u8 *)psta, 0, sizeof (struct sta_info));
+	memset((u8 *)psta, 0, sizeof(struct sta_info));
 
 	 spin_lock_init(&psta->lock);
 	INIT_LIST_HEAD(&psta->list);
diff --git a/drivers/staging/rtl8188eu/core/rtw_wlan_util.c b/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
index d300369977fa..324c1a7fd0bc 100644
--- a/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
+++ b/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
@@ -1394,7 +1394,6 @@ unsigned char check_assoc_AP(u8 *pframe, uint len)
 				DBG_88E("link to Artheros AP\n");
 				return HT_IOT_PEER_ATHEROS;
 			} else if ((!memcmp(pIE->data, BROADCOM_OUI1, 3)) ||
-				   (!memcmp(pIE->data, BROADCOM_OUI2, 3)) ||
 				   (!memcmp(pIE->data, BROADCOM_OUI2, 3))) {
 				DBG_88E("link to Broadcom AP\n");
 				return HT_IOT_PEER_BROADCOM;
diff --git a/drivers/staging/rtl8188eu/core/rtw_xmit.c b/drivers/staging/rtl8188eu/core/rtw_xmit.c
index 639ace06a3d6..7a71df167464 100644
--- a/drivers/staging/rtl8188eu/core/rtw_xmit.c
+++ b/drivers/staging/rtl8188eu/core/rtw_xmit.c
@@ -37,7 +37,7 @@ static void _init_txservq(struct tx_servq *ptxservq)
 
 void	_rtw_init_sta_xmit_priv(struct sta_xmit_priv *psta_xmitpriv)
 {
-	memset((unsigned char *)psta_xmitpriv, 0, sizeof (struct sta_xmit_priv));
+	memset((unsigned char *)psta_xmitpriv, 0, sizeof(struct sta_xmit_priv));
 	spin_lock_init(&psta_xmitpriv->lock);
 	_init_txservq(&psta_xmitpriv->be_q);
 	_init_txservq(&psta_xmitpriv->bk_q);
@@ -223,7 +223,7 @@ exit:
 	return res;
 }
 
-void _rtw_free_xmit_priv (struct xmit_priv *pxmitpriv)
+void _rtw_free_xmit_priv(struct xmit_priv *pxmitpriv)
 {
 	int i;
 	struct adapter *padapter = pxmitpriv->adapter;
@@ -691,7 +691,7 @@ static s32 xmitframe_addmic(struct adapter *padapter, struct xmit_frame *pxmitfr
 			payload = pframe;
 
 			for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
-				payload = (u8 *) round_up((size_t)(payload), 4);
+				payload = (u8 *)round_up((size_t)(payload), 4);
 				RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
 					 ("=== curfragnum=%d, pframe = 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x,!!!\n",
 					 curfragnum, *payload, *(payload+1),
@@ -772,7 +772,7 @@ static s32 xmitframe_swencrypt(struct adapter *padapter, struct xmit_frame *pxmi
 	return _SUCCESS;
 }
 
-s32 rtw_make_wlanhdr (struct adapter *padapter , u8 *hdr, struct pkt_attrib *pattrib)
+s32 rtw_make_wlanhdr(struct adapter *padapter, u8 *hdr, struct pkt_attrib *pattrib)
 {
 	u16 *qc;
 
@@ -1025,8 +1025,7 @@ s32 rtw_xmitframe_coalesce(struct adapter *padapter, struct sk_buff *pkt, struct
 
 		/* adding icv, if necessary... */
 		if (pattrib->iv_len) {
-			if (psta != NULL) {
-				switch (pattrib->encrypt) {
+			switch (pattrib->encrypt) {
 				case _WEP40_:
 				case _WEP104_:
 					WEP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
@@ -1043,7 +1042,6 @@ s32 rtw_xmitframe_coalesce(struct adapter *padapter, struct sk_buff *pkt, struct
 					else
 						AES_IV(pattrib->iv, psta->dot11txpn, 0);
 					break;
-				}
 			}
 
 			memcpy(pframe, pattrib->iv, pattrib->iv_len);
@@ -1098,7 +1096,7 @@ s32 rtw_xmitframe_coalesce(struct adapter *padapter, struct sk_buff *pkt, struct
 
 		addr = (size_t)(pframe);
 
-		mem_start = (unsigned char *) round_up(addr, 4) + hw_hdr_offset;
+		mem_start = (unsigned char *)round_up(addr, 4) + hw_hdr_offset;
 		memcpy(mem_start, pbuf_start + hw_hdr_offset, pattrib->hdrlen);
 	}
 
diff --git a/drivers/staging/rtl8188eu/hal/bb_cfg.c b/drivers/staging/rtl8188eu/hal/bb_cfg.c
index 80e8cc92c10a..1e963bf9e48b 100644
--- a/drivers/staging/rtl8188eu/hal/bb_cfg.c
+++ b/drivers/staging/rtl8188eu/hal/bb_cfg.c
@@ -173,7 +173,7 @@ static bool set_baseband_agc_config(struct adapter *adapt)
 		u32 v1 = array[i];
 		u32 v2 = array[i+1];
 
-		if (v1 < 0xCDCDCDCD){
+		if (v1 < 0xCDCDCDCD) {
 			phy_set_bb_reg(adapt, v1, bMaskDWord, v2);
 			udelay(1);
 		}
@@ -552,7 +552,7 @@ static void store_pwrindex_offset(struct adapter *Adapter, u32 regaddr, u32 bitm
 	}
 }
 
-static void rtl_addr_delay(struct adapter *adapt, u32 addr, u32 bit_mask ,u32 data)
+static void rtl_addr_delay(struct adapter *adapt, u32 addr, u32 bit_mask, u32 data)
 {
 	if (addr == 0xfe) {
 		msleep(50);
diff --git a/drivers/staging/rtl8188eu/hal/fw.c b/drivers/staging/rtl8188eu/hal/fw.c
index 17b7f3750547..3b2875481fc5 100644
--- a/drivers/staging/rtl8188eu/hal/fw.c
+++ b/drivers/staging/rtl8188eu/hal/fw.c
@@ -84,7 +84,7 @@ static void _rtl88e_fw_block_write(struct adapter *adapt,
 static void _rtl88e_fill_dummy(u8 *pfwbuf, u32 *pfwlen)
 {
 	u32 fwlen = *pfwlen;
-	u8 remain = (u8) (fwlen % 4);
+	u8 remain = (u8)(fwlen % 4);
 
 	remain = (remain == 0) ? 0 : (4 - remain);
 
@@ -101,7 +101,7 @@ static void _rtl88e_fw_page_write(struct adapter *adapt,
 				  u32 page, const u8 *buffer, u32 size)
 {
 	u8 value8;
-	u8 u8page = (u8) (page & 0x07);
+	u8 u8page = (u8)(page & 0x07);
 
 	value8 = (usb_read8(adapt, REG_MCUFWDL + 2) & 0xF8) | u8page;
 
@@ -193,13 +193,13 @@ int rtl88eu_download_fw(struct adapter *adapt)
 	u32 fwsize;
 	int err;
 
-	if (request_firmware(&fw, fw_name, device)){
+	if (request_firmware(&fw, fw_name, device)) {
 		dev_err(device, "Firmware %s not available\n", fw_name);
 		return -ENOENT;
 	}
 
 	if (fw->size > FW_8188E_SIZE) {
-		dev_err(device,"Firmware size exceed 0x%X. Check it.\n",
+		dev_err(device, "Firmware size exceed 0x%X. Check it.\n",
 			 FW_8188E_SIZE);
 		return -1;
 	}
diff --git a/drivers/staging/rtl8188eu/hal/hal_intf.c b/drivers/staging/rtl8188eu/hal/hal_intf.c
index 538a0f65d09c..4bdbed28774e 100644
--- a/drivers/staging/rtl8188eu/hal/hal_intf.c
+++ b/drivers/staging/rtl8188eu/hal/hal_intf.c
@@ -275,13 +275,6 @@ void rtw_hal_write_rfreg(struct adapter *adapt, enum rf_radio_path rfpath,
 					      bitmask, data);
 }
 
-s32 rtw_hal_interrupt_handler(struct adapter *adapt)
-{
-	if (adapt->HalFunc.interrupt_handler)
-		return adapt->HalFunc.interrupt_handler(adapt);
-	return _FAIL;
-}
-
 void rtw_hal_set_bwmode(struct adapter *adapt,
 			enum ht_channel_width bandwidth, u8 offset)
 {
@@ -329,15 +322,6 @@ void rtw_hal_sreset_init(struct adapter *adapt)
 		adapt->HalFunc.sreset_init_value(adapt);
 }
 
-u8   rtw_hal_sreset_get_wifi_status(struct adapter *adapt)
-{
-	u8 status = 0;
-
-	if (adapt->HalFunc.sreset_get_wifi_status)
-		status = adapt->HalFunc.sreset_get_wifi_status(adapt);
-	return status;
-}
-
 void rtw_hal_notch_filter(struct adapter *adapter, bool enable)
 {
 	if (adapter->HalFunc.hal_notch_filter)
diff --git a/drivers/staging/rtl8188eu/hal/mac_cfg.c b/drivers/staging/rtl8188eu/hal/mac_cfg.c
index c0e7fa938059..febc83a5adb8 100644
--- a/drivers/staging/rtl8188eu/hal/mac_cfg.c
+++ b/drivers/staging/rtl8188eu/hal/mac_cfg.c
@@ -127,7 +127,7 @@ bool rtl88eu_phy_mac_config(struct adapter *adapt)
 	ptrarray = array_MAC_REG_8188E;
 
 	for (i = 0; i < arraylength; i = i + 2)
-		usb_write8(adapt, ptrarray[i], (u8) ptrarray[i + 1]);
+		usb_write8(adapt, ptrarray[i], (u8)ptrarray[i + 1]);
 
 	usb_write8(adapt, REG_MAX_AGGR_NUM, MAX_AGGR_NUM);
 	return true;
diff --git a/drivers/staging/rtl8188eu/hal/odm.c b/drivers/staging/rtl8188eu/hal/odm.c
index e4df83710ca6..9873998011d2 100644
--- a/drivers/staging/rtl8188eu/hal/odm.c
+++ b/drivers/staging/rtl8188eu/hal/odm.c
@@ -437,8 +437,8 @@ void odm_CommonInfoSelfInit(struct odm_dm_struct *pDM_Odm)
 {
 	struct adapter *adapter = pDM_Odm->Adapter;
 
-	pDM_Odm->bCckHighPower = (bool) phy_query_bb_reg(adapter, 0x824, BIT9);
-	pDM_Odm->RFPathRxEnable = (u8) phy_query_bb_reg(adapter, 0xc04, 0x0F);
+	pDM_Odm->bCckHighPower = (bool)phy_query_bb_reg(adapter, 0x824, BIT9);
+	pDM_Odm->RFPathRxEnable = (u8)phy_query_bb_reg(adapter, 0xc04, 0x0F);
 
 	ODM_InitDebugSetting(pDM_Odm);
 }
@@ -529,7 +529,7 @@ void odm_DIGInit(struct odm_dm_struct *pDM_Odm)
 	struct adapter *adapter = pDM_Odm->Adapter;
 	struct rtw_dig *pDM_DigTable = &pDM_Odm->DM_DigTable;
 
-	pDM_DigTable->CurIGValue = (u8) phy_query_bb_reg(adapter, ODM_REG_IGI_A_11N, ODM_BIT_IGI_11N);
+	pDM_DigTable->CurIGValue = (u8)phy_query_bb_reg(adapter, ODM_REG_IGI_A_11N, ODM_BIT_IGI_11N);
 	pDM_DigTable->RssiLowThresh	= DM_DIG_THRESH_LOW;
 	pDM_DigTable->RssiHighThresh	= DM_DIG_THRESH_HIGH;
 	pDM_DigTable->FALowThresh	= DM_false_ALARM_THRESH_LOW;
@@ -620,7 +620,7 @@ void odm_DIG(struct odm_dm_struct *pDM_Odm)
 		} else if (pDM_Odm->SupportAbility & ODM_BB_ANT_DIV) {
 			/* 1 Lower Bound for 88E AntDiv */
 			if (pDM_Odm->AntDivType == CG_TRX_HW_ANTDIV) {
-				DIG_Dynamic_MIN = (u8) pDM_DigTable->AntDiv_RSSI_max;
+				DIG_Dynamic_MIN = (u8)pDM_DigTable->AntDiv_RSSI_max;
 				ODM_RT_TRACE(pDM_Odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD,
 					     ("odm_DIG(): pDM_DigTable->AntDiv_RSSI_max=%d\n",
 					     pDM_DigTable->AntDiv_RSSI_max));
diff --git a/drivers/staging/rtl8188eu/hal/odm_HWConfig.c b/drivers/staging/rtl8188eu/hal/odm_HWConfig.c
index 4e4e21936e7c..29f87dffbad3 100644
--- a/drivers/staging/rtl8188eu/hal/odm_HWConfig.c
+++ b/drivers/staging/rtl8188eu/hal/odm_HWConfig.c
@@ -118,7 +118,7 @@ static void odm_RxPhyStatus92CSeries_Parsing(struct odm_dm_struct *dm_odm,
 
 		cck_highpwr = dm_odm->bCckHighPower;
 
-		cck_agc_rpt =  pPhyStaRpt->cck_agc_rpt_ofdm_cfosho_a ;
+		cck_agc_rpt =  pPhyStaRpt->cck_agc_rpt_ofdm_cfosho_a;
 
 		/* 2011.11.28 LukeLee: 88E use different LNA & VGA gain table */
 		/* The RSSI formula should be modified according to the gain table */
diff --git a/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c b/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c
index 5342af778eb0..d3c6873925ba 100644
--- a/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c
+++ b/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c
@@ -278,7 +278,7 @@ static void rtl88eu_dm_hw_ant_div(struct odm_dm_struct *dm_odm)
 	struct rtw_dig *dig_table = &dm_odm->DM_DigTable;
 	struct sta_info *entry;
 	u32 i, min_rssi = 0xFF, ant_div_max_rssi = 0, max_rssi = 0;
-	u32 local_min_rssi,local_max_rssi;
+	u32 local_min_rssi, local_max_rssi;
 	u32 main_rssi, aux_rssi;
 	u8 RxIdleAnt = 0, target_ant = 7;
 
diff --git a/drivers/staging/rtl8188eu/hal/phy.c b/drivers/staging/rtl8188eu/hal/phy.c
index c4f7f358a81c..3f663fe151ba 100644
--- a/drivers/staging/rtl8188eu/hal/phy.c
+++ b/drivers/staging/rtl8188eu/hal/phy.c
@@ -478,7 +478,7 @@ void rtl88eu_dm_txpower_tracking_callback_thermalmeter(struct adapter *adapt)
 		/* 2.4G, decrease power */
 		{0, 0, 2, 3, 4, 4, 5, 6, 7, 7, 8, 9, 10, 10, 11},
 		/* 2.4G, increase power */
-		{0, 0, -1, -2, -3, -4,-4, -4, -4, -5, -7, -8,-9, -9, -10},
+		{0, 0, -1, -2, -3, -4, -4, -4, -4, -5, -7, -8, -9, -9, -10},
 	};
 	u8 thermal_mapping[2][index_mapping_NUM_88E] = {
 		/* 2.4G, decrease power */
diff --git a/drivers/staging/rtl8188eu/hal/rf.c b/drivers/staging/rtl8188eu/hal/rf.c
index c2fac34c8132..eea4c8a6022b 100644
--- a/drivers/staging/rtl8188eu/hal/rf.c
+++ b/drivers/staging/rtl8188eu/hal/rf.c
@@ -131,7 +131,7 @@ void rtl88eu_phy_rf6052_set_cck_txpower(struct adapter *adapt, u8 *powerlevel)
 /*  powerbase1 for HT MCS rates */
 static void getpowerbase88e(struct adapter *adapt, u8 *pwr_level_ofdm,
 			    u8 *pwr_level_bw20, u8 *pwr_level_bw40,
-			    u8 channel,u32 *ofdmbase, u32 *mcs_base)
+			    u8 channel, u32 *ofdmbase, u32 *mcs_base)
 {
 	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
 	u32 powerbase0, powerbase1;
diff --git a/drivers/staging/rtl8188eu/hal/rf_cfg.c b/drivers/staging/rtl8188eu/hal/rf_cfg.c
index ddc2f55fe13f..5dc11cae2ef9 100644
--- a/drivers/staging/rtl8188eu/hal/rf_cfg.c
+++ b/drivers/staging/rtl8188eu/hal/rf_cfg.c
@@ -164,7 +164,7 @@ do {								\
 #define B3WIREDATALENGTH 0x800
 #define BRFSI_RFENV 0x10
 
-static void rtl_rfreg_delay(struct adapter *adapt, enum rf_radio_path rfpath,u32 addr, u32 mask, u32 data)
+static void rtl_rfreg_delay(struct adapter *adapt, enum rf_radio_path rfpath, u32 addr, u32 mask, u32 data)
 {
 	if (addr == 0xfe) {
 		mdelay(50);
@@ -190,7 +190,7 @@ static void rtl8188e_config_rf_reg(struct adapter *adapt,
 	u32 content = 0x1000; /*RF Content: radio_a_txt*/
 	u32 maskforphyset = (u32)(content & 0xE000);
 
-	rtl_rfreg_delay(adapt, RF90_PATH_A, addr| maskforphyset,
+	rtl_rfreg_delay(adapt, RF90_PATH_A, addr | maskforphyset,
 			RFREG_OFFSET_MASK,
 			data);
 }
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_cmd.c b/drivers/staging/rtl8188eu/hal/rtl8188e_cmd.c
index 023a3d84ee8b..7f30dea1b53b 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_cmd.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_cmd.c
@@ -150,11 +150,9 @@ u8 rtl8188e_set_raid_cmd(struct adapter *adapt, u32 mask)
 	struct hal_data_8188e *haldata = GET_HAL_DATA(adapt);
 
 	if (haldata->fw_ractrl) {
-		__le32 lmask;
 
 		memset(buf, 0, 3);
-		lmask = cpu_to_le32(mask);
-		memcpy(buf, &lmask, 3);
+		put_unaligned_le32(mask, buf);
 
 		FillH2CCmd_88E(adapt, H2C_DM_MACID_CFG, 3, buf);
 	} else {
@@ -254,7 +252,7 @@ void rtl8188e_set_FwMediaStatus_cmd(struct adapter *adapt, __le16 mstatus_rpt)
 {
 	u8 opmode, macid;
 	u16 mst_rpt = le16_to_cpu(mstatus_rpt);
-	opmode = (u8) mst_rpt;
+	opmode = (u8)mst_rpt;
 	macid = (u8)(mst_rpt >> 8);
 
 	DBG_88E("### %s: MStatus=%x MACID=%d\n", __func__, opmode, macid);
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c b/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c
index dab4c337a863..01566210bbd2 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c
@@ -155,6 +155,8 @@ void rtl8188e_HalDmWatchDog(struct adapter *Adapter)
 	bool fw_ps_awake = true;
 	u8 hw_init_completed = false;
 	struct hal_data_8188e *hal_data = GET_HAL_DATA(Adapter);
+	struct mlme_priv *pmlmepriv = NULL;
+	u8 bLinked = false;
 
 	hw_init_completed = Adapter->hw_init_completed;
 
@@ -170,22 +172,20 @@ void rtl8188e_HalDmWatchDog(struct adapter *Adapter)
 		fw_ps_awake = false;
 
 	/* ODM */
-	if (hw_init_completed) {
-		struct mlme_priv *pmlmepriv = &Adapter->mlmepriv;
-		u8 bLinked = false;
-
-		if ((check_fwstate(pmlmepriv, WIFI_AP_STATE)) ||
-		    (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE))) {
-			if (Adapter->stapriv.asoc_sta_count > 2)
-				bLinked = true;
-		} else {/* Station mode */
-			if (check_fwstate(pmlmepriv, _FW_LINKED))
-				bLinked = true;
-		}
-
-		ODM_CmnInfoUpdate(&hal_data->odmpriv, ODM_CMNINFO_LINK, bLinked);
-		ODM_DMWatchdog(&hal_data->odmpriv);
+	pmlmepriv = &Adapter->mlmepriv;
+
+	if ((check_fwstate(pmlmepriv, WIFI_AP_STATE)) ||
+	    (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE |
+			   WIFI_ADHOC_MASTER_STATE))) {
+		if (Adapter->stapriv.asoc_sta_count > 2)
+			bLinked = true;
+	} else {/* Station mode */
+		if (check_fwstate(pmlmepriv, _FW_LINKED))
+			bLinked = true;
 	}
+
+	ODM_CmnInfoUpdate(&hal_data->odmpriv, ODM_CMNINFO_LINK, bLinked);
+	ODM_DMWatchdog(&hal_data->odmpriv);
 skip_dm:
 	/*  Check GPIO to determine current RF on/off and Pbc status. */
 	/*  Check Hardware Radio ON/OFF or not */
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c b/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
index d6fe5e6aa4f4..7d460eaafa35 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
@@ -181,7 +181,8 @@ static void rtl8188e_SetHalODMVar(struct adapter *Adapter, enum hal_odm_variable
 	switch (eVariable) {
 	case HAL_ODM_STA_INFO:
 		{
-			struct sta_info *psta = (struct sta_info *)pValue1;
+			struct sta_info *psta = pValue1;
+
 			if (bSet) {
 				DBG_88E("### Set STA_(%d) info\n", psta->mac_id);
 				ODM_CmnInfoPtrArrayHook(podmpriv, ODM_CMNINFO_STA_STATUS, psta->mac_id, psta);
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_xmit.c b/drivers/staging/rtl8188eu/hal/rtl8188e_xmit.c
index 7a4f754d86df..a6ba53b488e3 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_xmit.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_xmit.c
@@ -25,7 +25,7 @@
 
 void dump_txrpt_ccx_88e(void *buf)
 {
-	struct txrpt_ccx_88e *txrpt_ccx = (struct txrpt_ccx_88e *)buf;
+	struct txrpt_ccx_88e *txrpt_ccx = buf;
 
 	DBG_88E("%s:\n"
 		"tag1:%u, pkt_num:%u, txdma_underflow:%u, int_bt:%u, int_tri:%u, int_ccx:%u\n"
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188eu_xmit.c b/drivers/staging/rtl8188eu/hal/rtl8188eu_xmit.c
index be9eede6931d..594c1da9db23 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188eu_xmit.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188eu_xmit.c
@@ -399,7 +399,7 @@ static s32 rtw_dump_xframe(struct adapter *adapt, struct xmit_frame *pxmitframe)
 
 		mem_addr += w_sz;
 
-		mem_addr = (u8 *) round_up((size_t)mem_addr, 4);
+		mem_addr = (u8 *)round_up((size_t)mem_addr, 4);
 	}
 
 	rtw_free_xmitframe(pxmitpriv, pxmitframe);
diff --git a/drivers/staging/rtl8188eu/hal/usb_halinit.c b/drivers/staging/rtl8188eu/hal/usb_halinit.c
index caf2ca3a47e7..14650e91c78a 100644
--- a/drivers/staging/rtl8188eu/hal/usb_halinit.c
+++ b/drivers/staging/rtl8188eu/hal/usb_halinit.c
@@ -1673,7 +1673,7 @@ static void SetHwReg8188EU(struct adapter *Adapter, u8 variable, u8 *val)
 			pRegToSet = RegToSet_Normal; /*  0xb972a841; */
 			FactorToSet = *((u8 *)val);
 			if (FactorToSet <= 3) {
-				FactorToSet = (1<<(FactorToSet + 2));
+				FactorToSet = 1 << (FactorToSet + 2);
 				if (FactorToSet > 0xf)
 					FactorToSet = 0xf;
 
@@ -2012,7 +2012,7 @@ static u8 SetHalDefVar8188EUsb(struct adapter *Adapter, enum hal_def_variable eV
 			u8 bRSSIDump = *((u8 *)pValue);
 			struct odm_dm_struct *dm_ocm = &(haldata->odmpriv);
 			if (bRSSIDump)
-				dm_ocm->DebugComponents	=	ODM_COMP_DIG|ODM_COMP_FA_CNT	;
+				dm_ocm->DebugComponents	=	ODM_COMP_DIG|ODM_COMP_FA_CNT;
 			else
 				dm_ocm->DebugComponents	= 0;
 		}
diff --git a/drivers/staging/rtl8188eu/include/hal_intf.h b/drivers/staging/rtl8188eu/include/hal_intf.h
index 9191993dd3f5..3b476d80f64d 100644
--- a/drivers/staging/rtl8188eu/include/hal_intf.h
+++ b/drivers/staging/rtl8188eu/include/hal_intf.h
@@ -304,8 +304,6 @@ void	rtw_hal_write_rfreg(struct adapter *padapter,
 			    enum rf_radio_path eRFPath, u32 RegAddr,
 			    u32 BitMask, u32 Data);
 
-s32	rtw_hal_interrupt_handler(struct adapter *padapter);
-
 void	rtw_hal_set_bwmode(struct adapter *padapter,
 			   enum ht_channel_width Bandwidth, u8 Offset);
 void	rtw_hal_set_chan(struct adapter *padapter, u8 channel);
@@ -317,7 +315,6 @@ void	rtw_hal_antdiv_rssi_compared(struct adapter *padapter,
 				     struct wlan_bssid_ex *src);
 
 void rtw_hal_sreset_init(struct adapter *padapter);
-u8   rtw_hal_sreset_get_wifi_status(struct adapter *padapter);
 
 void rtw_hal_notch_filter(struct adapter *adapter, bool enable);
 void rtw_hal_reset_security_engine(struct adapter *adapter);
diff --git a/drivers/staging/rtl8188eu/include/ieee80211_ext.h b/drivers/staging/rtl8188eu/include/ieee80211_ext.h
index 1052d1817a97..15e53d380ad0 100644
--- a/drivers/staging/rtl8188eu/include/ieee80211_ext.h
+++ b/drivers/staging/rtl8188eu/include/ieee80211_ext.h
@@ -103,24 +103,24 @@ struct wme_parameter_element {
 
 #define WPA_PUT_LE16(a, val)			\
 	do {					\
-		(a)[1] = ((u16) (val)) >> 8;	\
-		(a)[0] = ((u16) (val)) & 0xff;	\
+		(a)[1] = ((u16)(val)) >> 8;	\
+		(a)[0] = ((u16)(val)) & 0xff;	\
 	} while (0)
 
 #define WPA_PUT_BE32(a, val)					\
 	do {							\
-		(a)[0] = (u8) ((((u32) (val)) >> 24) & 0xff);	\
-		(a)[1] = (u8) ((((u32) (val)) >> 16) & 0xff);	\
-		(a)[2] = (u8) ((((u32) (val)) >> 8) & 0xff);	\
-		(a)[3] = (u8) (((u32) (val)) & 0xff);		\
+		(a)[0] = (u8)((((u32) (val)) >> 24) & 0xff);	\
+		(a)[1] = (u8)((((u32) (val)) >> 16) & 0xff);	\
+		(a)[2] = (u8)((((u32) (val)) >> 8) & 0xff);	\
+		(a)[3] = (u8)(((u32) (val)) & 0xff);		\
 	} while (0)
 
 #define WPA_PUT_LE32(a, val)					\
 	do {							\
-		(a)[3] = (u8) ((((u32) (val)) >> 24) & 0xff);	\
-		(a)[2] = (u8) ((((u32) (val)) >> 16) & 0xff);	\
-		(a)[1] = (u8) ((((u32) (val)) >> 8) & 0xff);	\
-		(a)[0] = (u8) (((u32) (val)) & 0xff);		\
+		(a)[3] = (u8)((((u32) (val)) >> 24) & 0xff);	\
+		(a)[2] = (u8)((((u32) (val)) >> 16) & 0xff);	\
+		(a)[1] = (u8)((((u32) (val)) >> 8) & 0xff);	\
+		(a)[0] = (u8)(((u32) (val)) & 0xff);		\
 	} while (0)
 
 #define RSN_SELECTOR_PUT(a, val) WPA_PUT_BE32((u8 *)(a), (val))
diff --git a/drivers/staging/rtl8188eu/include/odm_debug.h b/drivers/staging/rtl8188eu/include/odm_debug.h
index db7b44e16c48..914f831a5b77 100644
--- a/drivers/staging/rtl8188eu/include/odm_debug.h
+++ b/drivers/staging/rtl8188eu/include/odm_debug.h
@@ -83,9 +83,8 @@
 #define ODM_COMP_INIT					BIT31
 
 /*------------------------Export Marco Definition---------------------------*/
-#define DbgPrint	pr_info
 #define RT_PRINTK(fmt, args...)				\
-	DbgPrint("%s(): " fmt, __func__, ## args);
+	pr_info("%s(): " fmt, __func__, ## args);
 
 #ifndef ASSERT
 	#define ASSERT(expr)
@@ -94,40 +93,18 @@
 #define ODM_RT_TRACE(pDM_Odm, comp, level, fmt)				\
 	if (((comp) & pDM_Odm->DebugComponents) &&			\
 	    (level <= pDM_Odm->DebugLevel)) {				\
-		DbgPrint("[ODM-8188E] ");				\
-		RT_PRINTK fmt;						\
-	}
-
-#define ODM_RT_TRACE_F(pDM_Odm, comp, level, fmt)			\
-	if (((comp) & pDM_Odm->DebugComponents) &&			\
-	    (level <= pDM_Odm->DebugLevel)) {				\
+		pr_info("[ODM-8188E] ");				\
 		RT_PRINTK fmt;						\
 	}
 
 #define ODM_RT_ASSERT(pDM_Odm, expr, fmt)				\
 	if (!(expr)) {							\
-		DbgPrint("Assertion failed! %s at ......\n", #expr);	\
-		DbgPrint("      ......%s,%s,line=%d\n", __FILE__,	\
+		pr_info("Assertion failed! %s at ......\n", #expr);	\
+		pr_info("      ......%s,%s,line=%d\n", __FILE__,	\
 			__func__, __LINE__);				\
 		RT_PRINTK fmt;						\
 		ASSERT(false);						\
 	}
-#define ODM_dbg_enter() { DbgPrint("==> %s\n", __func__); }
-#define ODM_dbg_exit() { DbgPrint("<== %s\n", __func__); }
-#define ODM_dbg_trace(str) { DbgPrint("%s:%s\n", __func__, str); }
-
-#define ODM_PRINT_ADDR(pDM_Odm, comp, level, title_str, ptr)		\
-	if (((comp) & pDM_Odm->DebugComponents) &&			\
-	    (level <= pDM_Odm->DebugLevel)) {				\
-		int __i;						\
-		u8 *__ptr = (u8 *)ptr;					\
-		DbgPrint("[ODM] ");					\
-		DbgPrint(title_str);					\
-		DbgPrint(" ");						\
-		for (__i = 0; __i < 6; __i++)				\
-			DbgPrint("%02X%s", __ptr[__i], (__i == 5)?"":"-");\
-		DbgPrint("\n");						\
-	}
 
 void ODM_InitDebugSetting(struct odm_dm_struct *pDM_Odm);
 
diff --git a/drivers/staging/rtl8188eu/include/osdep_service.h b/drivers/staging/rtl8188eu/include/osdep_service.h
index fed9c86890b4..82f58f87656a 100644
--- a/drivers/staging/rtl8188eu/include/osdep_service.h
+++ b/drivers/staging/rtl8188eu/include/osdep_service.h
@@ -182,8 +182,8 @@ u64 rtw_modular64(u64 x, u64 y);
 
 /* Macros for handling unaligned memory accesses */
 
-#define RTW_GET_BE24(a) ((((u32) (a)[0]) << 16) | (((u32) (a)[1]) << 8) | \
-			 ((u32) (a)[2]))
+#define RTW_GET_BE24(a) ((((u32)(a)[0]) << 16) | (((u32) (a)[1]) << 8) | \
+			 ((u32)(a)[2]))
 
 void rtw_buf_free(u8 **buf, u32 *buf_len);
 void rtw_buf_update(u8 **buf, u32 *buf_len, u8 *src, u32 src_len);
diff --git a/drivers/staging/rtl8188eu/include/rtw_debug.h b/drivers/staging/rtl8188eu/include/rtw_debug.h
index a38616e3cad2..971bf457f32d 100644
--- a/drivers/staging/rtl8188eu/include/rtw_debug.h
+++ b/drivers/staging/rtl8188eu/include/rtw_debug.h
@@ -106,7 +106,7 @@ extern u32 GlobalDebugLevel;
 			u8	*ptr = (u8 *)_hexdata;			\
 			pr_info("%s", DRIVER_PREFIX);			\
 			pr_info(_titlestring);				\
-			for (__i = 0; __i < (int)_hexdatalen; __i++ ) {	\
+			for (__i = 0; __i < (int)_hexdatalen; __i++) {	\
 				pr_info("%02X%s", ptr[__i],		\
 					 (((__i + 1) % 4) == 0) ?	\
 					 "  " : " ");	\
diff --git a/drivers/staging/rtl8188eu/include/rtw_led.h b/drivers/staging/rtl8188eu/include/rtw_led.h
index c5194b620da4..23f0cfe312f3 100644
--- a/drivers/staging/rtl8188eu/include/rtw_led.h
+++ b/drivers/staging/rtl8188eu/include/rtw_led.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU 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 __RTW_LED_H_
@@ -27,7 +23,7 @@
 #define LED_BLINK_LINK_INTERVAL_ALPHA		500	/* 500 */
 #define LED_BLINK_SCAN_INTERVAL_ALPHA		180	/* 150 */
 #define LED_BLINK_FASTER_INTERVAL_ALPHA		50
-#define LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA	5000
+#define LED_BLINK_WPS_SUCCESS_INTERVAL_ALPHA	5000
 
 enum LED_CTL_MODE {
 	LED_CTL_POWER_ON,
@@ -92,7 +88,7 @@ struct LED_871x {
 
 void LedControl8188eu(struct adapter *padapter, enum LED_CTL_MODE	LedAction);
 
-struct led_priv{
+struct led_priv {
 	/* add for led control */
 	struct LED_871x			SwLed0;
 	u8	bRegUseLed;
diff --git a/drivers/staging/rtl8188eu/include/rtw_mlme_ext.h b/drivers/staging/rtl8188eu/include/rtw_mlme_ext.h
index d699ca19ef16..8d72ccf5f2a0 100644
--- a/drivers/staging/rtl8188eu/include/rtw_mlme_ext.h
+++ b/drivers/staging/rtl8188eu/include/rtw_mlme_ext.h
@@ -448,7 +448,7 @@ struct mlme_ext_priv {
 
 int init_mlme_ext_priv(struct adapter *adapter);
 int init_hw_mlme_ext(struct adapter *padapter);
-void free_mlme_ext_priv (struct mlme_ext_priv *pmlmeext);
+void free_mlme_ext_priv(struct mlme_ext_priv *pmlmeext);
 extern void init_mlme_ext_timer(struct adapter *padapter);
 extern void init_addba_retry_timer(struct adapter *adapt, struct sta_info *sta);
 extern struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv);
@@ -646,8 +646,8 @@ void mlmeext_sta_add_event_callback(struct adapter *padapter,
 
 void linked_status_chk(struct adapter *padapter);
 
-void survey_timer_hdl (void *function_context);
-void link_timer_hdl (void *funtion_context);
+void survey_timer_hdl(void *function_context);
+void link_timer_hdl(void *funtion_context);
 void addba_timer_hdl(void *function_context);
 
 #define set_survey_timer(mlmeext, ms) \
@@ -708,15 +708,15 @@ u8 tdls_hdl(struct adapter *padapter, unsigned char *pbuf);
 #ifdef _RTW_CMD_C_
 
 static struct cmd_hdl wlancmds[] = {
-	GEN_MLME_EXT_HANDLER(sizeof (struct wlan_bssid_ex), join_cmd_hdl)
-	GEN_MLME_EXT_HANDLER(sizeof (struct disconnect_parm), disconnect_hdl)
-	GEN_MLME_EXT_HANDLER(sizeof (struct wlan_bssid_ex), createbss_hdl)
-	GEN_MLME_EXT_HANDLER(sizeof (struct setopmode_parm), setopmode_hdl)
-	GEN_MLME_EXT_HANDLER(sizeof (struct sitesurvey_parm), sitesurvey_cmd_hdl)
-	GEN_MLME_EXT_HANDLER(sizeof (struct setauth_parm), setauth_hdl)
-	GEN_MLME_EXT_HANDLER(sizeof (struct setkey_parm), setkey_hdl)
-	GEN_MLME_EXT_HANDLER(sizeof (struct set_stakey_parm), set_stakey_hdl)
-	GEN_MLME_EXT_HANDLER(sizeof (struct set_assocsta_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct wlan_bssid_ex), join_cmd_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct disconnect_parm), disconnect_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct wlan_bssid_ex), createbss_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setopmode_parm), setopmode_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct sitesurvey_parm), sitesurvey_cmd_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setauth_parm), setauth_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setkey_parm), setkey_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct set_stakey_parm), set_stakey_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct set_assocsta_parm), NULL)
 	GEN_MLME_EXT_HANDLER(sizeof(struct addBaReq_parm), add_ba_hdl)
 	GEN_MLME_EXT_HANDLER(sizeof(struct set_ch_parm), set_ch_hdl)
 	GEN_MLME_EXT_HANDLER(sizeof(struct wlan_bssid_ex), tx_beacon_hdl)
@@ -787,7 +787,7 @@ static struct fwevent wlanevents[] = {
 	{0, NULL},
 	{0, NULL},
 	{0, &rtw_survey_event_callback},		/*8*/
-	{sizeof (struct surveydone_event), &rtw_surveydone_event_callback},/*9*/
+	{sizeof(struct surveydone_event), &rtw_surveydone_event_callback},/*9*/
 	{0, &rtw_joinbss_event_callback},		/*10*/
 	{sizeof(struct stassoc_event), &rtw_stassoc_event_callback},
 	{sizeof(struct stadel_event), &rtw_stadel_event_callback},
diff --git a/drivers/staging/rtl8188eu/include/wifi.h b/drivers/staging/rtl8188eu/include/wifi.h
index a88ebf41bba1..8dbdfafd52b5 100644
--- a/drivers/staging/rtl8188eu/include/wifi.h
+++ b/drivers/staging/rtl8188eu/include/wifi.h
@@ -766,27 +766,27 @@ enum ht_cap_ampdu_factor {
 #define OP_MODE_20MHZ_HT_STA_ASSOCED    2
 #define OP_MODE_MIXED                   3
 
-#define HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK	((u8) BIT(0) | BIT(1))
-#define HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE		((u8) BIT(0))
-#define HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW		((u8) BIT(0) | BIT(1))
-#define HT_INFO_HT_PARAM_REC_TRANS_CHNL_WIDTH		((u8) BIT(2))
-#define HT_INFO_HT_PARAM_RIFS_MODE			((u8) BIT(3))
-#define HT_INFO_HT_PARAM_CTRL_ACCESS_ONLY		((u8) BIT(4))
-#define HT_INFO_HT_PARAM_SRV_INTERVAL_GRANULARITY	((u8) BIT(5))
+#define HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK	((u8)BIT(0) | BIT(1))
+#define HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE		((u8)BIT(0))
+#define HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW		((u8)BIT(0) | BIT(1))
+#define HT_INFO_HT_PARAM_REC_TRANS_CHNL_WIDTH		((u8)BIT(2))
+#define HT_INFO_HT_PARAM_RIFS_MODE			((u8)BIT(3))
+#define HT_INFO_HT_PARAM_CTRL_ACCESS_ONLY		((u8)BIT(4))
+#define HT_INFO_HT_PARAM_SRV_INTERVAL_GRANULARITY	((u8)BIT(5))
 
 #define HT_INFO_OPERATION_MODE_OP_MODE_MASK	\
-		((u16) (0x0001 | 0x0002))
+		((u16)(0x0001 | 0x0002))
 #define HT_INFO_OPERATION_MODE_OP_MODE_OFFSET		0
-#define HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT	((u8) BIT(2))
-#define HT_INFO_OPERATION_MODE_TRANSMIT_BURST_LIMIT	((u8) BIT(3))
-#define HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT	((u8) BIT(4))
-
-#define HT_INFO_STBC_PARAM_DUAL_BEACON		((u16) BIT(6))
-#define HT_INFO_STBC_PARAM_DUAL_STBC_PROTECT	((u16) BIT(7))
-#define HT_INFO_STBC_PARAM_SECONDARY_BC		((u16) BIT(8))
-#define HT_INFO_STBC_PARAM_LSIG_TXOP_PROTECT_ALLOWED	((u16) BIT(9))
-#define HT_INFO_STBC_PARAM_PCO_ACTIVE		((u16) BIT(10))
-#define HT_INFO_STBC_PARAM_PCO_PHASE		((u16) BIT(11))
+#define HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT	((u8)BIT(2))
+#define HT_INFO_OPERATION_MODE_TRANSMIT_BURST_LIMIT	((u8)BIT(3))
+#define HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT	((u8)BIT(4))
+
+#define HT_INFO_STBC_PARAM_DUAL_BEACON		((u16)BIT(6))
+#define HT_INFO_STBC_PARAM_DUAL_STBC_PROTECT	((u16)BIT(7))
+#define HT_INFO_STBC_PARAM_SECONDARY_BC		((u16)BIT(8))
+#define HT_INFO_STBC_PARAM_LSIG_TXOP_PROTECT_ALLOWED	((u16)BIT(9))
+#define HT_INFO_STBC_PARAM_PCO_ACTIVE		((u16)BIT(10))
+#define HT_INFO_STBC_PARAM_PCO_PHASE		((u16)BIT(11))
 
 /*	===============WPS Section=============== */
 /*	For WPSv1.0 */
diff --git a/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c b/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c
index d598fec4abbf..24a8f5ac96e5 100644
--- a/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c
+++ b/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c
@@ -361,7 +361,7 @@ static int wpa_set_encryption(struct net_device *dev, struct ieee_param *param,
 	param->u.crypt.err = 0;
 	param->u.crypt.alg[IEEE_CRYPT_ALG_NAME_LEN - 1] = '\0';
 
-	if (param_len < (u32) ((u8 *)param->u.crypt.key - (u8 *)param) + param->u.crypt.key_len) {
+	if (param_len < (u32)((u8 *)param->u.crypt.key - (u8 *)param) + param->u.crypt.key_len) {
 		ret =  -EINVAL;
 		goto exit;
 	}
@@ -512,14 +512,12 @@ static int rtw_set_wpa_ie(struct adapter *padapter, char *pie, unsigned short ie
 	}
 
 	if (ielen) {
-		buf = kzalloc(ielen, GFP_KERNEL);
+		buf = kmemdup(pie, ielen, GFP_KERNEL);
 		if (buf == NULL) {
 			ret =  -ENOMEM;
 			goto exit;
 		}
 
-		memcpy(buf, pie, ielen);
-
 		/* dump */
 		{
 			int i;
@@ -1136,7 +1134,8 @@ static int rtw_wx_set_scan(struct net_device *dev, struct iw_request_info *a,
 		struct iw_scan_req *req = (struct iw_scan_req *)extra;
 
 		if (wrqu->data.flags & IW_SCAN_THIS_ESSID) {
-			int len = min((int)req->essid_len, IW_ESSID_MAX_SIZE);
+			int len = min_t(int, req->essid_len,
+					IW_ESSID_MAX_SIZE);
 
 			memcpy(ssid[0].Ssid, req->essid, len);
 			ssid[0].SsidLength = len;
@@ -1417,7 +1416,7 @@ static int rtw_wx_set_rate(struct net_device *dev,
 			      struct iw_request_info *a,
 			      union iwreq_data *wrqu, char *extra)
 {
-	int i, ret = 0;
+	int i;
 	u8 datarates[NumRates];
 	u32	target_rate = wrqu->bitrate.value;
 	u32	fixed = wrqu->bitrate.fixed;
@@ -1490,7 +1489,7 @@ set_rate:
 		RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_info_, ("datarate_inx =%d\n", datarates[i]));
 	}
 
-	return ret;
+	return 0;
 }
 
 static int rtw_wx_get_rate(struct net_device *dev,
@@ -2699,10 +2698,8 @@ static int rtw_set_wps_beacon(struct net_device *dev, struct ieee_param *param,
 
 	ie_len = len-12-2;/*  12 = param header, 2:no packed */
 
-	if (pmlmepriv->wps_beacon_ie) {
-		kfree(pmlmepriv->wps_beacon_ie);
-		pmlmepriv->wps_beacon_ie = NULL;
-	}
+	kfree(pmlmepriv->wps_beacon_ie);
+	pmlmepriv->wps_beacon_ie = NULL;
 
 	if (ie_len > 0) {
 		pmlmepriv->wps_beacon_ie = rtw_malloc(ie_len);
@@ -2736,10 +2733,8 @@ static int rtw_set_wps_probe_resp(struct net_device *dev, struct ieee_param *par
 
 	ie_len = len-12-2;/*  12 = param header, 2:no packed */
 
-	if (pmlmepriv->wps_probe_resp_ie) {
-		kfree(pmlmepriv->wps_probe_resp_ie);
-		pmlmepriv->wps_probe_resp_ie = NULL;
-	}
+	kfree(pmlmepriv->wps_probe_resp_ie);
+	pmlmepriv->wps_probe_resp_ie = NULL;
 
 	if (ie_len > 0) {
 		pmlmepriv->wps_probe_resp_ie = rtw_malloc(ie_len);
@@ -2768,10 +2763,8 @@ static int rtw_set_wps_assoc_resp(struct net_device *dev, struct ieee_param *par
 
 	ie_len = len-12-2;/*  12 = param header, 2:no packed */
 
-	if (pmlmepriv->wps_assoc_resp_ie) {
-		kfree(pmlmepriv->wps_assoc_resp_ie);
-		pmlmepriv->wps_assoc_resp_ie = NULL;
-	}
+	kfree(pmlmepriv->wps_assoc_resp_ie);
+	pmlmepriv->wps_assoc_resp_ie = NULL;
 
 	if (ie_len > 0) {
 		pmlmepriv->wps_assoc_resp_ie = rtw_malloc(ie_len);
diff --git a/drivers/staging/rtl8188eu/os_dep/os_intfs.c b/drivers/staging/rtl8188eu/os_dep/os_intfs.c
index 08a80f759b8d..88a909c9e457 100644
--- a/drivers/staging/rtl8188eu/os_dep/os_intfs.c
+++ b/drivers/staging/rtl8188eu/os_dep/os_intfs.c
@@ -512,7 +512,6 @@ void rtw_proc_remove_one(struct net_device *dev)
 
 static uint loadparam(struct adapter *padapter,  struct  net_device *pnetdev)
 {
-	uint status = _SUCCESS;
 	struct registry_priv  *registry_par = &padapter->registrypriv;
 
 
@@ -527,7 +526,7 @@ static uint loadparam(struct adapter *padapter,  struct  net_device *pnetdev)
 
 	registry_par->channel = (u8)rtw_channel;
 	registry_par->wireless_mode = (u8)rtw_wireless_mode;
-	registry_par->vrtl_carrier_sense = (u8)rtw_vrtl_carrier_sense ;
+	registry_par->vrtl_carrier_sense = (u8)rtw_vrtl_carrier_sense;
 	registry_par->vcs_type = (u8)rtw_vcs_type;
 	registry_par->rts_thresh = (u16)rtw_rts_thresh;
 	registry_par->frag_thresh = (u16)rtw_frag_thresh;
@@ -582,7 +581,7 @@ static uint loadparam(struct adapter *padapter,  struct  net_device *pnetdev)
 	snprintf(registry_par->ifname, 16, "%s", ifname);
 	snprintf(registry_par->if2name, 16, "%s", if2name);
 	registry_par->notch_filter = (u8)rtw_notch_filter;
-	return status;
+	return _SUCCESS;
 }
 
 static int rtw_net_set_mac_address(struct net_device *pnetdev, void *p)
@@ -760,7 +759,6 @@ void rtw_stop_drv_threads(struct adapter *padapter)
 
 static u8 rtw_init_default_value(struct adapter *padapter)
 {
-	u8 ret  = _SUCCESS;
 	struct registry_priv *pregistrypriv = &padapter->registrypriv;
 	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
@@ -803,12 +801,11 @@ static u8 rtw_init_default_value(struct adapter *padapter)
 	padapter->bWritePortCancel = false;
 	padapter->bRxRSSIDisplay = 0;
 	padapter->bNotifyChannelChange = 0;
-	return ret;
+	return _SUCCESS;
 }
 
 u8 rtw_reset_drv_sw(struct adapter *padapter)
 {
-	u8	ret8 = _SUCCESS;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;
 
@@ -833,7 +830,7 @@ u8 rtw_reset_drv_sw(struct adapter *padapter)
 
 	rtw_set_signal_stat_timer(&padapter->recvpriv);
 
-	return ret8;
+	return _SUCCESS;
 }
 
 u8 rtw_init_drv_sw(struct adapter *padapter)
diff --git a/drivers/staging/rtl8188eu/os_dep/osdep_service.c b/drivers/staging/rtl8188eu/os_dep/osdep_service.c
index 8af4a8d24cce..abcb3a8589ef 100644
--- a/drivers/staging/rtl8188eu/os_dep/osdep_service.c
+++ b/drivers/staging/rtl8188eu/os_dep/osdep_service.c
@@ -51,7 +51,7 @@ void *rtw_malloc2d(int h, int w, int size)
 {
 	int j;
 
-	void **a = (void **)kzalloc(h*sizeof(void *) + h*w*size, GFP_KERNEL);
+	void **a = kzalloc(h*sizeof(void *) + h*w*size, GFP_KERNEL);
 	if (a == NULL) {
 		pr_info("%s: alloc memory fail!\n", __func__);
 		return NULL;
diff --git a/drivers/staging/rtl8188eu/os_dep/rtw_android.c b/drivers/staging/rtl8188eu/os_dep/rtw_android.c
index d9d55d12fd5f..99ce077007f4 100644
--- a/drivers/staging/rtl8188eu/os_dep/rtw_android.c
+++ b/drivers/staging/rtl8188eu/os_dep/rtw_android.c
@@ -148,36 +148,21 @@ static int rtw_android_set_block(struct net_device *net, char *command,
 int rtw_android_priv_cmd(struct net_device *net, struct ifreq *ifr, int cmd)
 {
 	int ret = 0;
-	char *command = NULL;
+	char *command;
 	int cmd_num;
 	int bytes_written = 0;
 	struct android_wifi_priv_cmd priv_cmd;
 
-	if (!ifr->ifr_data) {
-		ret = -EINVAL;
-		goto exit;
-	}
-	if (copy_from_user(&priv_cmd, ifr->ifr_data,
-			   sizeof(struct android_wifi_priv_cmd))) {
-		ret = -EFAULT;
-		goto exit;
-	}
-	command = kmalloc(priv_cmd.total_len, GFP_KERNEL);
-	if (!command) {
-		DBG_88E("%s: failed to allocate memory\n", __func__);
-		ret = -ENOMEM;
-		goto exit;
-	}
-	if (!access_ok(VERIFY_READ, priv_cmd.buf, priv_cmd.total_len)) {
-		DBG_88E("%s: failed to access memory\n", __func__);
-		ret = -EFAULT;
-		goto exit;
-	}
-	if (copy_from_user(command, (char __user *)priv_cmd.buf,
-			   priv_cmd.total_len)) {
-		ret = -EFAULT;
-		goto exit;
-	}
+	if (!ifr->ifr_data)
+		return -EINVAL;
+	if (copy_from_user(&priv_cmd, ifr->ifr_data, sizeof(priv_cmd)))
+		return -EFAULT;
+	if (priv_cmd.total_len < 1)
+		return -EINVAL;
+	command = memdup_user(priv_cmd.buf, priv_cmd.total_len);
+	if (IS_ERR(command))
+		return PTR_ERR(command);
+	command[priv_cmd.total_len - 1] = 0;
 	DBG_88E("%s: Android private cmd \"%s\" on %s\n",
 		__func__, command, ifr->ifr_name);
 	cmd_num = rtw_android_cmdstr_to_num(command);
@@ -191,7 +176,7 @@ int rtw_android_priv_cmd(struct net_device *net, struct ifreq *ifr, int cmd)
 		DBG_88E("%s: Ignore private cmd \"%s\" - iface %s is down\n",
 			__func__, command, ifr->ifr_name);
 		ret = 0;
-		goto exit;
+		goto free;
 	}
 	switch (cmd_num) {
 	case ANDROID_WIFI_CMD_STOP:
@@ -279,7 +264,7 @@ response:
 	} else {
 		ret = bytes_written;
 	}
-exit:
+free:
 	kfree(command);
 	return ret;
 }
diff --git a/drivers/staging/rtl8188eu/os_dep/usb_intf.c b/drivers/staging/rtl8188eu/os_dep/usb_intf.c
index 2f87150a21b7..bee39c2278f1 100644
--- a/drivers/staging/rtl8188eu/os_dep/usb_intf.c
+++ b/drivers/staging/rtl8188eu/os_dep/usb_intf.c
@@ -63,7 +63,6 @@ static struct dvobj_priv *usb_dvobj_init(struct usb_interface *usb_intf)
 	struct usb_config_descriptor	*pconf_desc;
 	struct usb_host_interface	*phost_iface;
 	struct usb_interface_descriptor	*piface_desc;
-	struct usb_host_endpoint	*phost_endp;
 	struct usb_endpoint_descriptor	*pendp_desc;
 	struct usb_device	*pusbd;
 
@@ -92,24 +91,22 @@ static struct dvobj_priv *usb_dvobj_init(struct usb_interface *usb_intf)
 
 	for (i = 0; i < pdvobjpriv->nr_endpoint; i++) {
 		int ep_num;
-		phost_endp = phost_iface->endpoint + i;
-
-		if (phost_endp) {
-			pendp_desc = &phost_endp->desc;
-			ep_num = usb_endpoint_num(pendp_desc);
-
-			if (usb_endpoint_is_bulk_in(pendp_desc)) {
-				pdvobjpriv->RtInPipe[pdvobjpriv->RtNumInPipes] = ep_num;
-				pdvobjpriv->RtNumInPipes++;
-			} else if (usb_endpoint_is_int_in(pendp_desc)) {
-				pdvobjpriv->RtInPipe[pdvobjpriv->RtNumInPipes] = ep_num;
-				pdvobjpriv->RtNumInPipes++;
-			} else if (usb_endpoint_is_bulk_out(pendp_desc)) {
-				pdvobjpriv->RtOutPipe[pdvobjpriv->RtNumOutPipes] = ep_num;
-				pdvobjpriv->RtNumOutPipes++;
-			}
-			pdvobjpriv->ep_num[i] = ep_num;
+		pendp_desc = &phost_iface->endpoint[i].desc;
+
+		ep_num = usb_endpoint_num(pendp_desc);
+
+		if (usb_endpoint_is_bulk_in(pendp_desc)) {
+			pdvobjpriv->RtInPipe[pdvobjpriv->RtNumInPipes] = ep_num;
+			pdvobjpriv->RtNumInPipes++;
+		} else if (usb_endpoint_is_int_in(pendp_desc)) {
+			pdvobjpriv->RtInPipe[pdvobjpriv->RtNumInPipes] = ep_num;
+			pdvobjpriv->RtNumInPipes++;
+		} else if (usb_endpoint_is_bulk_out(pendp_desc)) {
+			pdvobjpriv->RtOutPipe[pdvobjpriv->RtNumOutPipes] =
+				ep_num;
+			pdvobjpriv->RtNumOutPipes++;
 		}
+		pdvobjpriv->ep_num[i] = ep_num;
 	}
 
 	if (pusbd->speed == USB_SPEED_HIGH)
@@ -557,8 +554,6 @@ static void rtw_dev_remove(struct usb_interface *pusb_intf)
 
 	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("-dev_remove()\n"));
 	DBG_88E("-r871xu_dev_remove, done\n");
-
-	return;
 }
 
 static struct usb_driver rtl8188e_usb_drv = {
diff --git a/drivers/staging/rtl8188eu/os_dep/usb_ops_linux.c b/drivers/staging/rtl8188eu/os_dep/usb_ops_linux.c
index ba1e178fb510..d2efa9dfc8c0 100644
--- a/drivers/staging/rtl8188eu/os_dep/usb_ops_linux.c
+++ b/drivers/staging/rtl8188eu/os_dep/usb_ops_linux.c
@@ -160,10 +160,10 @@ static int recvbuf2recvframe(struct adapter *adapt, struct sk_buff *pskb)
 		switch (haldata->UsbRxAggMode) {
 		case USB_RX_AGG_DMA:
 		case USB_RX_AGG_MIX:
-			pkt_offset = (u16) round_up(pkt_offset, 128);
+			pkt_offset = (u16)round_up(pkt_offset, 128);
 			break;
 		case USB_RX_AGG_USB:
-			pkt_offset = (u16) round_up(pkt_offset, 4);
+			pkt_offset = (u16)round_up(pkt_offset, 4);
 			break;
 		case USB_RX_AGG_DISABLE:
 		default:
@@ -843,7 +843,7 @@ void usb_write_port_cancel(struct adapter *padapter)
 void rtl8188eu_recv_tasklet(void *priv)
 {
 	struct sk_buff *pskb;
-	struct adapter *adapt = (struct adapter *)priv;
+	struct adapter *adapt = priv;
 	struct recv_priv *precvpriv = &adapt->recvpriv;
 
 	while (NULL != (pskb = skb_dequeue(&precvpriv->rx_skb_queue))) {
@@ -862,7 +862,7 @@ void rtl8188eu_recv_tasklet(void *priv)
 void rtl8188eu_xmit_tasklet(void *priv)
 {
 	int ret = false;
-	struct adapter *adapt = (struct adapter *)priv;
+	struct adapter *adapt = priv;
 	struct xmit_priv *pxmitpriv = &adapt->xmitpriv;
 
 	if (check_fwstate(&adapt->mlmepriv, _FW_UNDER_SURVEY))
diff --git a/drivers/staging/rtl8188eu/os_dep/xmit_linux.c b/drivers/staging/rtl8188eu/os_dep/xmit_linux.c
index 0ce47b07ef86..5acf9a9dddeb 100644
--- a/drivers/staging/rtl8188eu/os_dep/xmit_linux.c
+++ b/drivers/staging/rtl8188eu/os_dep/xmit_linux.c
@@ -46,7 +46,7 @@ void _rtw_open_pktfile(struct sk_buff *pktptr, struct pkt_file *pfile)
 
 }
 
-uint _rtw_pktfile_read (struct pkt_file *pfile, u8 *rmem, uint rlen)
+uint _rtw_pktfile_read(struct pkt_file *pfile, u8 *rmem, uint rlen)
 {
 	uint	len = 0;
 
@@ -66,13 +66,7 @@ uint _rtw_pktfile_read (struct pkt_file *pfile, u8 *rmem, uint rlen)
 
 int rtw_endofpktfile(struct pkt_file *pfile)
 {
-
-	if (pfile->pkt_len == 0) {
-		return true;
-	}
-
-
-	return false;
+	return pfile->pkt_len == 0;
 }
 
 int rtw_os_xmit_resource_alloc(struct adapter *padapter, struct xmit_buf *pxmitbuf, u32 alloc_sz)
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
index 0ffed2d06b58..552d943b1761 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
@@ -1287,7 +1287,7 @@ void  rtl8192_tx_fill_desc(struct net_device *dev, struct tx_desc *pdesc,
 
 void  rtl8192_tx_fill_cmd_desc(struct net_device *dev,
 			       struct tx_desc_cmd *entry,
-			       struct cb_desc *cb_desc, struct sk_buff* skb)
+			       struct cb_desc *cb_desc, struct sk_buff *skb)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
 	dma_addr_t mapping = pci_map_single(priv->pdev, skb->data, skb->len,
@@ -1302,7 +1302,7 @@ void  rtl8192_tx_fill_cmd_desc(struct net_device *dev,
 	if (cb_desc->bCmdOrInit == DESC_PACKET_TYPE_INIT) {
 		entry->CmdInit = DESC_PACKET_TYPE_INIT;
 	} else {
-		struct tx_desc * entry_tmp = (struct tx_desc *)entry;
+		struct tx_desc *entry_tmp = (struct tx_desc *)entry;
 
 		entry_tmp->CmdInit = DESC_PACKET_TYPE_NORMAL;
 		entry_tmp->Offset = sizeof(struct tx_fwinfo_8190pci) + 8;
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
index b6ce8c3b2f8c..885315cac3a4 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
@@ -1449,13 +1449,11 @@ static void dm_CCKTxPowerAdjust_TSSI(struct net_device *dev, bool  bInCH14)
 			  (priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[1]<<8)) ;
 
 		rtl8192_setBBreg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
-		TempVal = 0;
 		TempVal = (u32)(priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[2] +
 			  (priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[3]<<8) +
 			  (priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[4]<<16)+
 			  (priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[5]<<24));
 		rtl8192_setBBreg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
-		TempVal = 0;
 		TempVal = (u32)(priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[6] +
 			  (priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[7]<<8)) ;
 
@@ -1465,13 +1463,11 @@ static void dm_CCKTxPowerAdjust_TSSI(struct net_device *dev, bool  bInCH14)
 			  (priv->cck_txbbgain_ch14_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[1]<<8)) ;
 
 		rtl8192_setBBreg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
-		TempVal = 0;
 		TempVal = (u32)(priv->cck_txbbgain_ch14_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[2] +
 			  (priv->cck_txbbgain_ch14_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[3]<<8) +
 			  (priv->cck_txbbgain_ch14_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[4]<<16)+
 			  (priv->cck_txbbgain_ch14_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[5]<<24));
 		rtl8192_setBBreg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
-		TempVal = 0;
 		TempVal = (u32)(priv->cck_txbbgain_ch14_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[6] +
 			  (priv->cck_txbbgain_ch14_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[7]<<8)) ;
 
@@ -1493,7 +1489,6 @@ static void dm_CCKTxPowerAdjust_ThermalMeter(struct net_device *dev,	bool  bInCH
 		rtl8192_setBBreg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
 		RT_TRACE(COMP_POWER_TRACKING, "CCK not chnl 14, reg 0x%x = 0x%x\n",
 			rCCK0_TxFilter1, TempVal);
-		TempVal = 0;
 		TempVal =	CCKSwingTable_Ch1_Ch13[priv->CCK_index][2] +
 					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][3]<<8) +
 					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][4]<<16)+
@@ -1501,7 +1496,6 @@ static void dm_CCKTxPowerAdjust_ThermalMeter(struct net_device *dev,	bool  bInCH
 		rtl8192_setBBreg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
 		RT_TRACE(COMP_POWER_TRACKING, "CCK not chnl 14, reg 0x%x = 0x%x\n",
 			rCCK0_TxFilter2, TempVal);
-		TempVal = 0;
 		TempVal =	CCKSwingTable_Ch1_Ch13[priv->CCK_index][6] +
 					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][7]<<8) ;
 
@@ -1515,7 +1509,6 @@ static void dm_CCKTxPowerAdjust_ThermalMeter(struct net_device *dev,	bool  bInCH
 		rtl8192_setBBreg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
 		RT_TRACE(COMP_POWER_TRACKING, "CCK chnl 14, reg 0x%x = 0x%x\n",
 			rCCK0_TxFilter1, TempVal);
-		TempVal = 0;
 		TempVal =	CCKSwingTable_Ch14[priv->CCK_index][2] +
 					(CCKSwingTable_Ch14[priv->CCK_index][3]<<8) +
 					(CCKSwingTable_Ch14[priv->CCK_index][4]<<16)+
@@ -1523,7 +1516,6 @@ static void dm_CCKTxPowerAdjust_ThermalMeter(struct net_device *dev,	bool  bInCH
 		rtl8192_setBBreg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
 		RT_TRACE(COMP_POWER_TRACKING, "CCK chnl 14, reg 0x%x = 0x%x\n",
 			rCCK0_TxFilter2, TempVal);
-		TempVal = 0;
 		TempVal =	CCKSwingTable_Ch14[priv->CCK_index][6] +
 					(CCKSwingTable_Ch14[priv->CCK_index][7]<<8) ;
 
diff --git a/drivers/staging/rtl8192e/rtl819x_BAProc.c b/drivers/staging/rtl8192e/rtl819x_BAProc.c
index 6da57847a533..0415e02b4eff 100644
--- a/drivers/staging/rtl8192e/rtl819x_BAProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_BAProc.c
@@ -16,6 +16,8 @@
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
 ******************************************************************************/
+#include <asm/byteorder.h>
+#include <asm/unaligned.h>
 #include "rtllib.h"
 #include "rtl819x_BA.h"
 
@@ -79,7 +81,6 @@ static struct sk_buff *rtllib_ADDBA(struct rtllib_device *ieee, u8 *Dst,
 	struct sk_buff *skb = NULL;
 	 struct rtllib_hdr_3addr *BAReq = NULL;
 	u8 *tag = NULL;
-	u16 tmp = 0;
 	u16 len = ieee->tx_headroom + 9;
 
 	RTLLIB_DEBUG(RTLLIB_DL_TRACE | RTLLIB_DL_BA, "========>%s(), frame(%d)"
@@ -115,15 +116,15 @@ static struct sk_buff *rtllib_ADDBA(struct rtllib_device *ieee, u8 *Dst,
 
 	if (ACT_ADDBARSP == type) {
 		RT_TRACE(COMP_DBG, "====>to send ADDBARSP\n");
-		tmp = StatusCode;
-		memcpy(tag, (u8 *)&tmp, 2);
+
+		put_unaligned_le16(StatusCode, tag);
 		tag += 2;
 	}
-	tmp = pBA->BaParamSet.shortData;
-	memcpy(tag, (u8 *)&tmp, 2);
+
+	put_unaligned_le16(pBA->BaParamSet.shortData, tag);
 	tag += 2;
-	tmp = pBA->BaTimeoutValue;
-	memcpy(tag, (u8 *)&tmp, 2);
+
+	put_unaligned_le16(pBA->BaTimeoutValue, tag);
 	tag += 2;
 
 	if (ACT_ADDBAREQ == type) {
@@ -143,7 +144,6 @@ static struct sk_buff *rtllib_DELBA(struct rtllib_device *ieee, u8 *dst,
 	struct sk_buff *skb = NULL;
 	 struct rtllib_hdr_3addr *Delba = NULL;
 	u8 *tag = NULL;
-	u16 tmp = 0;
 	u16 len = 6 + ieee->tx_headroom;
 
 	if (net_ratelimit())
@@ -178,11 +178,11 @@ static struct sk_buff *rtllib_DELBA(struct rtllib_device *ieee, u8 *dst,
 	*tag++ = ACT_CAT_BA;
 	*tag++ = ACT_DELBA;
 
-	tmp = DelbaParamSet.shortData;
-	memcpy(tag, (u8 *)&tmp, 2);
+
+	put_unaligned_le16(DelbaParamSet.shortData, tag);
 	tag += 2;
-	tmp = ReasonCode;
-	memcpy(tag, (u8 *)&tmp, 2);
+
+	put_unaligned_le16(ReasonCode, tag);
 	tag += 2;
 
 	RTLLIB_DEBUG_DATA(RTLLIB_DL_DATA|RTLLIB_DL_BA, skb->data, skb->len);
diff --git a/drivers/staging/rtl8192e/rtllib.h b/drivers/staging/rtl8192e/rtllib.h
index 2d82f8993ea1..cef2dc27103f 100644
--- a/drivers/staging/rtl8192e/rtllib.h
+++ b/drivers/staging/rtl8192e/rtllib.h
@@ -2762,8 +2762,6 @@ extern void rtllib_stop_scan(struct rtllib_device *ieee);
 extern bool rtllib_act_scanning(struct rtllib_device *ieee, bool sync_scan);
 extern void rtllib_stop_scan_syncro(struct rtllib_device *ieee);
 extern void rtllib_start_scan_syncro(struct rtllib_device *ieee, u8 is_mesh);
-extern inline struct sk_buff *rtllib_probe_req(struct rtllib_device *ieee);
-extern u8 MgntQuery_MgntFrameTxRate(struct rtllib_device *ieee);
 extern void rtllib_sta_ps_send_null_frame(struct rtllib_device *ieee,
 					  short pwr);
 extern void rtllib_sta_wakeup(struct rtllib_device *ieee, short nl);
@@ -2944,12 +2942,12 @@ void rtllib_softmac_scan_syncro(struct rtllib_device *ieee, u8 is_mesh);
 
 extern const long rtllib_wlan_frequencies[];
 
-extern inline void rtllib_increment_scans(struct rtllib_device *ieee)
+static inline void rtllib_increment_scans(struct rtllib_device *ieee)
 {
 	ieee->scans++;
 }
 
-extern inline int rtllib_get_scans(struct rtllib_device *ieee)
+static inline int rtllib_get_scans(struct rtllib_device *ieee)
 {
 	return ieee->scans;
 }
diff --git a/drivers/staging/rtl8192e/rtllib_rx.c b/drivers/staging/rtl8192e/rtllib_rx.c
index 1c2014fd8d81..cf11b042b93a 100644
--- a/drivers/staging/rtl8192e/rtllib_rx.c
+++ b/drivers/staging/rtl8192e/rtllib_rx.c
@@ -1415,10 +1415,6 @@ static int rtllib_rx_InfraAdhoc(struct rtllib_device *ieee, struct sk_buff *skb,
 	return 1;
 
  rx_dropped:
-	if (rxb != NULL) {
-		kfree(rxb);
-		rxb = NULL;
-	}
 	ieee->stats.rx_dropped++;
 
 	/* Returning 0 indicates to caller that we have not handled the SKB--
@@ -2691,7 +2687,7 @@ void rtllib_rx_mgt(struct rtllib_device *ieee,
 		      struct sk_buff *skb,
 		      struct rtllib_rx_stats *stats)
 {
-	struct rtllib_hdr_4addr *header = (struct rtllib_hdr_4addr *)skb->data ;
+	struct rtllib_hdr_4addr *header = (struct rtllib_hdr_4addr *)skb->data;
 
 	if ((WLAN_FC_GET_STYPE(le16_to_cpu(header->frame_ctl)) !=
 	    RTLLIB_STYPE_PROBE_RESP) &&
diff --git a/drivers/staging/rtl8192e/rtllib_softmac.c b/drivers/staging/rtl8192e/rtllib_softmac.c
index abb6729ae279..d992a754e72d 100644
--- a/drivers/staging/rtl8192e/rtllib_softmac.c
+++ b/drivers/staging/rtl8192e/rtllib_softmac.c
@@ -193,7 +193,7 @@ MgntQuery_TxRateExcludeCCKRates(struct rtllib_device *ieee)
 	return QueryRate;
 }
 
-u8 MgntQuery_MgntFrameTxRate(struct rtllib_device *ieee)
+static u8 MgntQuery_MgntFrameTxRate(struct rtllib_device *ieee)
 {
 	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
 	u8 rate;
@@ -343,7 +343,7 @@ inline void softmac_ps_mgmt_xmit(struct sk_buff *skb,
 	}
 }
 
-inline struct sk_buff *rtllib_probe_req(struct rtllib_device *ieee)
+static inline struct sk_buff *rtllib_probe_req(struct rtllib_device *ieee)
 {
 	unsigned int len, rate_len;
 	u8 *tag;
@@ -1311,7 +1311,7 @@ inline struct sk_buff *rtllib_association_req(struct rtllib_network *beacon,
 	}
 
 	if (beacon->bCkipSupported) {
-		static u8	AironetIeOui[] = {0x00, 0x01, 0x66};
+		static const u8 AironetIeOui[] = {0x00, 0x01, 0x66};
 		u8	CcxAironetBuf[30];
 		struct octet_string osCcxAironetIE;
 
@@ -1331,10 +1331,11 @@ inline struct sk_buff *rtllib_association_req(struct rtllib_network *beacon,
 	}
 
 	if (beacon->bCcxRmEnable) {
-		static u8 CcxRmCapBuf[] = {0x00, 0x40, 0x96, 0x01, 0x01, 0x00};
+		static const u8 CcxRmCapBuf[] = {0x00, 0x40, 0x96, 0x01, 0x01,
+			0x00};
 		struct octet_string osCcxRmCap;
 
-		osCcxRmCap.Octet = CcxRmCapBuf;
+		osCcxRmCap.Octet = (u8 *) CcxRmCapBuf;
 		osCcxRmCap.Length = sizeof(CcxRmCapBuf);
 		tag = skb_put(skb, ccxrm_ie_len);
 		*tag++ = MFIE_TYPE_GENERIC;
@@ -3167,6 +3168,7 @@ void rtllib_softmac_free(struct rtllib_device *ieee)
 	cancel_delayed_work(&ieee->associate_retry_wq);
 	destroy_workqueue(ieee->wq);
 	up(&ieee->wx_sem);
+	tasklet_kill(&ieee->ps_task);
 }
 
 /********************************************************
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
index 73de9e9669f6..d401dbf4c7c6 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
@@ -713,8 +713,8 @@ static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
 	while(!list_empty(&pTS->RxPendingPktList)) {
 		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): start RREORDER indicate\n",__func__);
 		pReorderEntry = (PRX_REORDER_ENTRY)list_entry(pTS->RxPendingPktList.prev,RX_REORDER_ENTRY,List);
-		if( SN_LESS(pReorderEntry->SeqNum, pTS->RxIndicateSeq) ||
-				SN_EQUAL(pReorderEntry->SeqNum, pTS->RxIndicateSeq))
+		if (SN_LESS(pReorderEntry->SeqNum, pTS->RxIndicateSeq) ||
+		    SN_EQUAL(pReorderEntry->SeqNum, pTS->RxIndicateSeq))
 		{
 			/* This protect buffer from overflow. */
 			if(index >= REORDER_WIN_SIZE) {
@@ -800,9 +800,8 @@ static u8 parse_subframe(struct sk_buff *skb,
 	// Null packet, don't indicate it to upper layer
 	ChkLength = LLCOffset;/* + (Frame_WEP(frame)!=0 ?Adapter->MgntInfo.SecurityInfo.EncryptionHeadOverhead:0);*/
 
-	if( skb->len <= ChkLength ) {
+	if (skb->len <= ChkLength)
 		return 0;
-	}
 
 	skb_pull(skb, LLCOffset);
 
@@ -1035,10 +1034,9 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 		{
 
 		//	IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): pRxTS->RxLastFragNum is %d,frag is %d,pRxTS->RxLastSeqNum is %d,seq is %d\n",__func__,pRxTS->RxLastFragNum,frag,pRxTS->RxLastSeqNum,WLAN_GET_SEQ_SEQ(sc));
-			if(	(fc & (1<<11))  &&
-					(frag == pRxTS->RxLastFragNum) &&
-					(WLAN_GET_SEQ_SEQ(sc) == pRxTS->RxLastSeqNum)	)
-			{
+			if ((fc & (1<<11)) &&
+			    (frag == pRxTS->RxLastFragNum) &&
+			    (WLAN_GET_SEQ_SEQ(sc) == pRxTS->RxLastSeqNum)) {
 				goto rx_dropped;
 			}
 			else
@@ -2456,7 +2454,7 @@ static inline void ieee80211_process_probe_response(
 	//       then wireless adapter should do active scan from ch1~11 and
 	//       passive scan from ch12~14
 
-	if( !IsLegalChannel(ieee, network.channel) )
+	if (!IsLegalChannel(ieee, network.channel))
 		return;
 	if(ieee->bGlobalDomain)
 	{
@@ -2465,8 +2463,7 @@ static inline void ieee80211_process_probe_response(
 			// Case 1: Country code
 			if(IS_COUNTRY_IE_VALID(ieee) )
 			{
-				if( !IsLegalChannel(ieee, network.channel) )
-				{
+				if (!IsLegalChannel(ieee, network.channel)) {
 					printk("GetScanInfo(): For Country code, filter probe response at channel(%d).\n", network.channel);
 					return;
 				}
@@ -2487,8 +2484,7 @@ static inline void ieee80211_process_probe_response(
 			// Case 1: Country code
 			if(IS_COUNTRY_IE_VALID(ieee) )
 			{
-				if( !IsLegalChannel(ieee, network.channel) )
-				{
+				if (!IsLegalChannel(ieee, network.channel)) {
 					printk("GetScanInfo(): For Country code, filter beacon at channel(%d).\n",network.channel);
 					return;
 				}
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
index a85bb232be97..d1471877e19d 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
@@ -411,7 +411,7 @@ static void ieee80211_send_probe(struct ieee80211_device *ieee)
 	}
 }
 
-void ieee80211_send_probe_requests(struct ieee80211_device *ieee)
+static void ieee80211_send_probe_requests(struct ieee80211_device *ieee)
 {
 	if (ieee->active_scan && (ieee->softmac_features & IEEE_SOFTMAC_PROBERQ)){
 		ieee80211_send_probe(ieee);
@@ -517,7 +517,7 @@ static void ieee80211_softmac_scan_wq(struct work_struct *work)
 		goto out;
 	ieee->set_chan(ieee->dev, ieee->current_network.channel);
 	if(channel_map[ieee->current_network.channel] == 1)
-	ieee80211_send_probe_requests(ieee);
+		ieee80211_send_probe_requests(ieee);
 
 
 	queue_delayed_work(ieee->wq, &ieee->softmac_scan_wq, IEEE80211_SOFTMAC_SCAN_TIME);
@@ -804,12 +804,11 @@ static struct sk_buff *ieee80211_probe_resp(struct ieee80211_device *ieee, u8 *d
 	*(tag++) = ieee->current_network.channel;
 
 	if(atim_len){
-	u16 val16;
 		*(tag++) = MFIE_TYPE_IBSS_SET;
 		*(tag++) = 2;
-		//*((u16*)(tag)) = cpu_to_le16(ieee->current_network.atim_window);
-		 val16 = cpu_to_le16(ieee->current_network.atim_window);
-		memcpy((u8 *)tag, (u8 *)&val16, 2);
+
+		put_unaligned_le16(ieee->current_network.atim_window,
+				   (u8 *)tag);
 		tag+=2;
 	}
 
@@ -1043,10 +1042,9 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 	{
 		ccxrm_ie_len = 6+2;
 	}
-	if( beacon->BssCcxVerNumber >= 2 )
-	{
+	if (beacon->BssCcxVerNumber >= 2)
 		cxvernum_ie_len = 5+2;
-	}
+
 #ifdef THOMAS_TURBO
 	len = sizeof(struct ieee80211_assoc_request_frame)+ 2
 		+ beacon->ssid_len//essid tagged val
@@ -1103,7 +1101,7 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 	if(ieee->short_slot)
 		hdr->capability |= cpu_to_le16(WLAN_CAPABILITY_SHORT_SLOT);
 	if (wmm_info_len) //QOS
-	hdr->capability |= cpu_to_le16(WLAN_CAPABILITY_QOS);
+		hdr->capability |= cpu_to_le16(WLAN_CAPABILITY_QOS);
 
 	hdr->listen_interval = 0xa; //FIXME
 
@@ -1118,8 +1116,7 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 	ieee80211_MFIE_Brate(ieee, &tag);
 	ieee80211_MFIE_Grate(ieee, &tag);
 	// For CCX 1 S13, CKIP. Added by Annie, 2006-08-14.
-	if( beacon->bCkipSupported )
-	{
+	if (beacon->bCkipSupported) {
 		static u8	AironetIeOui[] = {0x00, 0x01, 0x66}; // "4500-client"
 		u8	CcxAironetBuf[30];
 		OCTET_STRING	osCcxAironetIE;
@@ -1158,8 +1155,7 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 		tag += osCcxRmCap.Length;
 	}
 
-	if( beacon->BssCcxVerNumber >= 2 )
-	{
+	if (beacon->BssCcxVerNumber >= 2) {
 		u8			CcxVerNumBuf[] = {0x00, 0x40, 0x96, 0x03, 0x00};
 		OCTET_STRING	osCcxVerNum;
 		CcxVerNumBuf[4] = beacon->BssCcxVerNumber;
@@ -1533,7 +1529,7 @@ void ieee80211_softmac_check_all_nets(struct ieee80211_device *ieee)
 			break;
 
 		if (ieee->scan_age == 0 || time_after(target->last_scanned + ieee->scan_age, jiffies))
-		ieee80211_softmac_new_net(ieee, target);
+			ieee80211_softmac_new_net(ieee, target);
 	}
 
 	spin_unlock_irqrestore(&ieee->lock, flags);
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
index 82ea533a0cf4..644368df6342 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
@@ -374,7 +374,7 @@ int ieee80211_wx_set_scan(struct ieee80211_device *ieee, struct iw_request_info
 		goto out;
 	}
 
-	if ( ieee->state == IEEE80211_LINKED){
+	if (ieee->state == IEEE80211_LINKED) {
 		queue_work(ieee->wq, &ieee->wx_sync_scan_wq);
 		/* intentionally forget to up sem */
 		return 0;
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
index 57bef219687b..fca73c7c9fbe 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
@@ -529,8 +529,7 @@ static void ieee80211_query_protectionmode(struct ieee80211_device *ieee,
 		}
 		}
 	// For test , CTS replace with RTS
-	if( 0 )
-	{
+	if (0) {
 		tcb_desc->bCTSEnable	= true;
 		tcb_desc->rts_rate = MGN_24M;
 		tcb_desc->bRTSEnable	= true;
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c
index 68f5ede86633..ae1b3cf2866c 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c
@@ -172,7 +172,7 @@ static inline char *rtl819x_translate_scan(struct ieee80211_device *ieee,
 	iwe.cmd = IWEVCUSTOM;
 	iwe.u.data.length = p - custom;
 	if (iwe.u.data.length)
-	start = iwe_stream_add_point(info, start, stop, &iwe, custom);
+		start = iwe_stream_add_point(info, start, stop, &iwe, custom);
 	/* Add quality statistics */
 	/* TODO: Fix these values... */
 	iwe.cmd = IWEVQUAL;
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
index 51552d42d66c..cd196cec0dd9 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
@@ -110,7 +110,7 @@ static struct sk_buff *ieee80211_ADDBA(struct ieee80211_device *ieee, u8 *Dst, P
 	struct sk_buff *skb = NULL;
 	 struct ieee80211_hdr_3addr *BAReq = NULL;
 	u8 *tag = NULL;
-	u16 tmp = 0;
+	__le16 tmp = 0;
 	u16 len = ieee->tx_headroom + 9;
 	//category(1) + action field(1) + Dialog Token(1) + BA Parameter Set(2) +  BA Timeout Value(2) +  BA Start SeqCtrl(2)(or StatusCode(2))
 	IEEE80211_DEBUG(IEEE80211_DL_TRACE | IEEE80211_DL_BA, "========>%s(), frame(%d) sentd to:%pM, ieee->dev:%p\n", __func__, type, Dst, ieee->dev);
@@ -196,7 +196,7 @@ static struct sk_buff *ieee80211_DELBA(
 	struct sk_buff *skb = NULL;
 	 struct ieee80211_hdr_3addr *Delba = NULL;
 	u8 *tag = NULL;
-	u16 tmp = 0;
+	__le16 tmp = 0;
 	//len = head len + DELBA Parameter Set(2) + Reason Code(2)
 	u16 len = 6 + ieee->tx_headroom;
 
@@ -342,8 +342,7 @@ int ieee80211_rx_ADDBAReq(struct ieee80211_device *ieee, struct sk_buff *skb)
 	PSEQUENCE_CONTROL pBaStartSeqCtrl = NULL;
 	PRX_TS_RECORD	pTS = NULL;
 
-	if (skb->len < sizeof( struct ieee80211_hdr_3addr) + 9)
-	{
+	if (skb->len < sizeof(struct ieee80211_hdr_3addr) + 9) {
 		IEEE80211_DEBUG(IEEE80211_DL_ERR,
 				" Invalid skb len in BAREQ(%d / %zu)\n",
 				skb->len,
@@ -444,8 +443,7 @@ int ieee80211_rx_ADDBARsp(struct ieee80211_device *ieee, struct sk_buff *skb)
 	PBA_PARAM_SET		pBaParamSet = NULL;
 	u16			ReasonCode;
 
-	if (skb->len < sizeof( struct ieee80211_hdr_3addr) + 9)
-	{
+	if (skb->len < sizeof(struct ieee80211_hdr_3addr) + 9) {
 		IEEE80211_DEBUG(IEEE80211_DL_ERR,
 				" Invalid skb len in BARSP(%d / %zu)\n",
 				skb->len,
@@ -463,10 +461,9 @@ int ieee80211_rx_ADDBARsp(struct ieee80211_device *ieee, struct sk_buff *skb)
 
 	// Check the capability
 	// Since we can always receive A-MPDU, we just check if it is under HT mode.
-	if(     ieee->current_network.qos_data.active == 0  ||
-		ieee->pHTInfo->bCurrentHTSupport == false ||
-		ieee->pHTInfo->bCurrentAMPDUEnable == false )
-	{
+	if (ieee->current_network.qos_data.active == 0  ||
+	    ieee->pHTInfo->bCurrentHTSupport == false ||
+	    ieee->pHTInfo->bCurrentAMPDUEnable == false) {
 		IEEE80211_DEBUG(IEEE80211_DL_ERR, "reject to ADDBA_RSP as some capability is not ready(%d, %d, %d)\n",ieee->current_network.qos_data.active, ieee->pHTInfo->bCurrentHTSupport, ieee->pHTInfo->bCurrentAMPDUEnable);
 		ReasonCode = DELBA_REASON_UNKNOWN_BA;
 		goto OnADDBARsp_Reject;
@@ -577,8 +574,7 @@ int ieee80211_rx_DELBA(struct ieee80211_device *ieee, struct sk_buff *skb)
 	u16			*pReasonCode = NULL;
 	u8			*dst = NULL;
 
-	if (skb->len < sizeof( struct ieee80211_hdr_3addr) + 6)
-	{
+	if (skb->len < sizeof(struct ieee80211_hdr_3addr) + 6) {
 		IEEE80211_DEBUG(IEEE80211_DL_ERR,
 				" Invalid skb len in DELBA(%d / %zu)\n",
 				skb->len,
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
index 1ea2cd392670..e60d926a3973 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
@@ -602,8 +602,7 @@ void HTConstructCapabilityElement(struct ieee80211_device *ieee, u8 *posHTCap, u
 	// TODO: Nedd to take care of this part
 	IEEE80211_DEBUG(IEEE80211_DL_HT, "TX HT cap/info ele BW=%d MaxAMSDUSize:%d DssCCk:%d\n", pCapELE->ChlWidth, pCapELE->MaxAMSDUSize, pCapELE->DssCCk);
 
-	if( IsEncrypt)
-	{
+	if (IsEncrypt) {
 		pCapELE->MPDUDensity	= 7; // 8us
 		pCapELE->MaxRxAMPDUFactor	= 2; // 2 is for 32 K and 3 is 64K
 	}
@@ -951,8 +950,7 @@ void HTOnAssocRsp(struct ieee80211_device *ieee)
 	static u8				EWC11NHTCap[] = {0x00, 0x90, 0x4c, 0x33};		// For 11n EWC definition, 2007.07.17, by Emily
 	static u8				EWC11NHTInfo[] = {0x00, 0x90, 0x4c, 0x34};	// For 11n EWC definition, 2007.07.17, by Emily
 
-	if( pHTInfo->bCurrentHTSupport == false )
-	{
+	if (pHTInfo->bCurrentHTSupport == false) {
 		IEEE80211_DEBUG(IEEE80211_DL_ERR, "<=== HTOnAssocRsp(): HT_DISABLE\n");
 		return;
 	}
@@ -1043,7 +1041,7 @@ void HTOnAssocRsp(struct ieee80211_device *ieee)
 		// Replace MPDU factor declared in original association response frame format. 2007.08.20 by Emily
 		if (ieee->current_network.bssht.bdRT2RTAggregation)
 		{
-			if( ieee->pairwise_key_type != KEY_TYPE_NA)
+			if (ieee->pairwise_key_type != KEY_TYPE_NA)
 				// Realtek may set 32k in security mode and 64k for others
 				pHTInfo->CurrentAMPDUFactor = pPeerHTCap->MaxRxAMPDUFactor;
 			else
@@ -1332,8 +1330,7 @@ u8 HTCCheck(struct ieee80211_device *ieee, u8 *pFrame)
 {
 	if(ieee->pHTInfo->bCurrentHTSupport)
 	{
-		if( (IsQoSDataFrame(pFrame) && Frame_Order(pFrame)) == 1)
-		{
+		if ((IsQoSDataFrame(pFrame) && Frame_Order(pFrame)) == 1) {
 			IEEE80211_DEBUG(IEEE80211_DL_HT, "HT CONTROL FILED EXIST!!\n");
 			return true;
 		}
diff --git a/drivers/staging/rtl8192u/r8192U_core.c b/drivers/staging/rtl8192u/r8192U_core.c
index 929ac29197cc..e031a253e2ae 100644
--- a/drivers/staging/rtl8192u/r8192U_core.c
+++ b/drivers/staging/rtl8192u/r8192U_core.c
@@ -1791,8 +1791,8 @@ static void rtl8192_link_change(struct net_device *dev)
 }
 
 static struct ieee80211_qos_parameters def_qos_parameters = {
-	{3, 3, 3, 3},/* cw_min */
-	{7, 7, 7, 7},/* cw_max */
+	{cpu_to_le16(3), cpu_to_le16(3), cpu_to_le16(3), cpu_to_le16(3)},
+	{cpu_to_le16(7), cpu_to_le16(7), cpu_to_le16(7), cpu_to_le16(7)},
 	{2, 2, 2, 2},/* aifs */
 	{0, 0, 0, 0},/* flags */
 	{0, 0, 0, 0} /* tx_op_limit */
@@ -1821,8 +1821,11 @@ static void rtl8192_qos_activate(struct work_struct *work)
 	struct net_device *dev = priv->ieee80211->dev;
 	struct ieee80211_qos_parameters *qos_parameters = &priv->ieee80211->current_network.qos_data.parameters;
 	u8 mode = priv->ieee80211->current_network.mode;
-	u8  u1bAIFS;
+	u32  u1bAIFS;
 	u32 u4bAcParam;
+	u32 op_limit;
+	u32 cw_max;
+	u32 cw_min;
 	int i;
 
 	mutex_lock(&priv->mutex);
@@ -1835,11 +1838,14 @@ static void rtl8192_qos_activate(struct work_struct *work)
 	for (i = 0; i <  QOS_QUEUE_NUM; i++) {
 		//Mode G/A: slotTimeTimer = 9; Mode B: 20
 		u1bAIFS = qos_parameters->aifs[i] * ((mode&(IEEE_G|IEEE_N_24G)) ? 9 : 20) + aSifsTime;
-		u4bAcParam = ((((u32)(qos_parameters->tx_op_limit[i]))<< AC_PARAM_TXOP_LIMIT_OFFSET)|
-			      (((u32)(qos_parameters->cw_max[i]))<< AC_PARAM_ECW_MAX_OFFSET)|
-			      (((u32)(qos_parameters->cw_min[i]))<< AC_PARAM_ECW_MIN_OFFSET)|
-			      ((u32)u1bAIFS << AC_PARAM_AIFS_OFFSET));
-
+		u1bAIFS <<= AC_PARAM_AIFS_OFFSET;
+		op_limit = (u32)le16_to_cpu(qos_parameters->tx_op_limit[i]);
+		op_limit <<= AC_PARAM_TXOP_LIMIT_OFFSET;
+		cw_max = (u32)le16_to_cpu(qos_parameters->cw_max[i]);
+		cw_max <<= AC_PARAM_ECW_MAX_OFFSET;
+		cw_min = (u32)le16_to_cpu(qos_parameters->cw_min[i]);
+		cw_min <<= AC_PARAM_ECW_MIN_OFFSET;
+		u4bAcParam = op_limit | cw_max | cw_min | u1bAIFS;
 		write_nic_dword(dev, WDCAPARA_ADD[i], u4bAcParam);
 	}
 
diff --git a/drivers/staging/rtl8192u/r8192U_dm.c b/drivers/staging/rtl8192u/r8192U_dm.c
index b0b66fba563b..936565d46014 100644
--- a/drivers/staging/rtl8192u/r8192U_dm.c
+++ b/drivers/staging/rtl8192u/r8192U_dm.c
@@ -1476,14 +1476,12 @@ static void dm_CCKTxPowerAdjust_TSSI(struct net_device *dev, bool  bInCH14)
 
 		rtl8192_setBBreg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
 		//Write 0xa24 ~ 0xa27
-		TempVal = 0;
 		TempVal =	priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[2] +
 					(priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[3]<<8) +
 					(priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[4]<<16)+
 					(priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[5]<<24);
 		rtl8192_setBBreg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
 		//Write 0xa28  0xa29
-		TempVal = 0;
 		TempVal =	priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[6] +
 					(priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[7]<<8) ;
 
@@ -1496,14 +1494,12 @@ static void dm_CCKTxPowerAdjust_TSSI(struct net_device *dev, bool  bInCH14)
 
 		rtl8192_setBBreg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
 		//Write 0xa24 ~ 0xa27
-		TempVal = 0;
 		TempVal =	priv->cck_txbbgain_ch14_table[priv->cck_present_attentuation].ccktxbb_valuearray[2] +
 					(priv->cck_txbbgain_ch14_table[priv->cck_present_attentuation].ccktxbb_valuearray[3]<<8) +
 					(priv->cck_txbbgain_ch14_table[priv->cck_present_attentuation].ccktxbb_valuearray[4]<<16)+
 					(priv->cck_txbbgain_ch14_table[priv->cck_present_attentuation].ccktxbb_valuearray[5]<<24);
 		rtl8192_setBBreg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
 		//Write 0xa28  0xa29
-		TempVal = 0;
 		TempVal =	priv->cck_txbbgain_ch14_table[priv->cck_present_attentuation].ccktxbb_valuearray[6] +
 					(priv->cck_txbbgain_ch14_table[priv->cck_present_attentuation].ccktxbb_valuearray[7]<<8) ;
 
@@ -1528,7 +1524,6 @@ static void dm_CCKTxPowerAdjust_ThermalMeter(struct net_device *dev,	bool  bInCH
 		RT_TRACE(COMP_POWER_TRACKING, "CCK not chnl 14, reg 0x%x = 0x%x\n",
 			rCCK0_TxFilter1, TempVal);
 		//Write 0xa24 ~ 0xa27
-		TempVal = 0;
 		TempVal =	CCKSwingTable_Ch1_Ch13[priv->CCK_index][2] +
 					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][3]<<8) +
 					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][4]<<16)+
@@ -1537,7 +1532,6 @@ static void dm_CCKTxPowerAdjust_ThermalMeter(struct net_device *dev,	bool  bInCH
 		RT_TRACE(COMP_POWER_TRACKING, "CCK not chnl 14, reg 0x%x = 0x%x\n",
 			rCCK0_TxFilter2, TempVal);
 		//Write 0xa28  0xa29
-		TempVal = 0;
 		TempVal =	CCKSwingTable_Ch1_Ch13[priv->CCK_index][6] +
 					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][7]<<8) ;
 
@@ -1556,7 +1550,6 @@ static void dm_CCKTxPowerAdjust_ThermalMeter(struct net_device *dev,	bool  bInCH
 		RT_TRACE(COMP_POWER_TRACKING, "CCK chnl 14, reg 0x%x = 0x%x\n",
 			rCCK0_TxFilter1, TempVal);
 		//Write 0xa24 ~ 0xa27
-		TempVal = 0;
 		TempVal =	CCKSwingTable_Ch14[priv->CCK_index][2] +
 					(CCKSwingTable_Ch14[priv->CCK_index][3]<<8) +
 					(CCKSwingTable_Ch14[priv->CCK_index][4]<<16)+
@@ -1565,7 +1558,6 @@ static void dm_CCKTxPowerAdjust_ThermalMeter(struct net_device *dev,	bool  bInCH
 		RT_TRACE(COMP_POWER_TRACKING, "CCK chnl 14, reg 0x%x = 0x%x\n",
 			rCCK0_TxFilter2, TempVal);
 		//Write 0xa28  0xa29
-		TempVal = 0;
 		TempVal =	CCKSwingTable_Ch14[priv->CCK_index][6] +
 					(CCKSwingTable_Ch14[priv->CCK_index][7]<<8) ;
 
@@ -1810,85 +1802,6 @@ void dm_change_dynamic_initgain_thresh(struct net_device *dev, u32 dm_type,
 	}
 }	/* DM_ChangeDynamicInitGainThresh */
 
-void
-dm_change_rxpath_selection_setting(
-	struct net_device *dev,
-	s32		DM_Type,
-	s32		DM_Value)
-{
-	struct r8192_priv *priv = ieee80211_priv(dev);
-	prate_adaptive	pRA = (prate_adaptive)&(priv->rate_adaptive);
-
-
-	if(DM_Type == 0)
-	{
-		if(DM_Value > 1)
-			DM_Value = 1;
-		DM_RxPathSelTable.Enable = (u8)DM_Value;
-	}
-	else if(DM_Type == 1)
-	{
-		if(DM_Value > 1)
-			DM_Value = 1;
-		DM_RxPathSelTable.DbgMode = (u8)DM_Value;
-	}
-	else if(DM_Type == 2)
-	{
-		if(DM_Value > 40)
-			DM_Value = 40;
-		DM_RxPathSelTable.SS_TH_low = (u8)DM_Value;
-	}
-	else if(DM_Type == 3)
-	{
-		if(DM_Value > 25)
-			DM_Value = 25;
-		DM_RxPathSelTable.diff_TH = (u8)DM_Value;
-	}
-	else if(DM_Type == 4)
-	{
-		if(DM_Value >= CCK_Rx_Version_MAX)
-			DM_Value = CCK_Rx_Version_1;
-		DM_RxPathSelTable.cck_method= (u8)DM_Value;
-	}
-	else if(DM_Type == 10)
-	{
-		if(DM_Value > 100)
-			DM_Value = 50;
-		DM_RxPathSelTable.rf_rssi[0] = (u8)DM_Value;
-	}
-	else if(DM_Type == 11)
-	{
-		if(DM_Value > 100)
-			DM_Value = 50;
-		DM_RxPathSelTable.rf_rssi[1] = (u8)DM_Value;
-	}
-	else if(DM_Type == 12)
-	{
-		if(DM_Value > 100)
-			DM_Value = 50;
-		DM_RxPathSelTable.rf_rssi[2] = (u8)DM_Value;
-	}
-	else if(DM_Type == 13)
-	{
-		if(DM_Value > 100)
-			DM_Value = 50;
-		DM_RxPathSelTable.rf_rssi[3] = (u8)DM_Value;
-	}
-	else if(DM_Type == 20)
-	{
-		if(DM_Value > 1)
-			DM_Value = 1;
-		pRA->ping_rssi_enable = (u8)DM_Value;
-	}
-	else if(DM_Type == 21)
-	{
-		if(DM_Value > 30)
-			DM_Value = 30;
-		pRA->ping_rssi_thresh_for_ra = DM_Value;
-	}
-}
-
-
 /*-----------------------------------------------------------------------------
  * Function:	dm_dig_init()
  *
@@ -3554,7 +3467,6 @@ static void dm_check_txrateandretrycount(struct net_device *dev)
 
 static void dm_send_rssi_tofw(struct net_device *dev)
 {
-	DCMD_TXCMD_T			tx_cmd;
 	struct r8192_priv *priv = ieee80211_priv(dev);
 
 	// If we test chariot, we should stop the TX command ?
@@ -3562,9 +3474,6 @@ static void dm_send_rssi_tofw(struct net_device *dev)
 	// 0x1e0(byte) to notify driver.
 	write_nic_byte(dev, DRIVER_RSSI, (u8)priv->undecorated_smoothed_pwdb);
 	return;
-	tx_cmd.Op		= TXCMD_SET_RX_RSSI;
-	tx_cmd.Length	= 4;
-	tx_cmd.Value		= priv->undecorated_smoothed_pwdb;
 }
 
 /*---------------------------Define function prototype------------------------*/
diff --git a/drivers/staging/rtl8192u/r8192U_wx.c b/drivers/staging/rtl8192u/r8192U_wx.c
index 28f60d2dbe5b..361d2d0c3df1 100644
--- a/drivers/staging/rtl8192u/r8192U_wx.c
+++ b/drivers/staging/rtl8192u/r8192U_wx.c
@@ -171,7 +171,6 @@ static int r8192_wx_set_crcmon(struct net_device *dev,
 	struct r8192_priv *priv = ieee80211_priv(dev);
 	int *parms = (int *)extra;
 	int enable = (parms[0] > 0);
-	short prev = priv->crcmon;
 
 	down(&priv->wx_sem);
 
@@ -183,11 +182,6 @@ static int r8192_wx_set_crcmon(struct net_device *dev,
 	DMESG("bad CRC in monitor mode are %s",
 	      priv->crcmon ? "accepted" : "rejected");
 
-	if (prev != priv->crcmon && priv->up) {
-		/* rtl8180_down(dev); */
-		/* rtl8180_up(dev); */
-	}
-
 	up(&priv->wx_sem);
 
 	return 0;
diff --git a/drivers/staging/rtl8192u/r819xU_firmware.c b/drivers/staging/rtl8192u/r819xU_firmware.c
index f66ad8a0dfe0..c230be290ab6 100644
--- a/drivers/staging/rtl8192u/r819xU_firmware.c
+++ b/drivers/staging/rtl8192u/r819xU_firmware.c
@@ -45,6 +45,7 @@ static bool fw_download_code(struct net_device *dev, u8 *code_virtual_address,
 	unsigned char	    *seg_ptr;
 	cb_desc		    *tcb_desc;
 	u8                  bLastIniPkt;
+	u8		    index;
 
 	firmware_init_param(dev);
 	//Fragmentation might be required
@@ -78,18 +79,19 @@ static bool fw_download_code(struct net_device *dev, u8 *code_virtual_address,
 		 * Transform from little endian to big endian
 		 * and pending  zero
 		 */
-		for (i=0; i < frag_length; i+=4) {
-			*seg_ptr++ = ((i+0)<frag_length)?code_virtual_address[i+3]:0;
-			*seg_ptr++ = ((i+1)<frag_length)?code_virtual_address[i+2]:0;
-			*seg_ptr++ = ((i+2)<frag_length)?code_virtual_address[i+1]:0;
-			*seg_ptr++ = ((i+3)<frag_length)?code_virtual_address[i+0]:0;
+		for (i = 0; i < frag_length; i += 4) {
+			*seg_ptr++ = ((i+0) < frag_length)?code_virtual_address[i+3] : 0;
+			*seg_ptr++ = ((i+1) < frag_length)?code_virtual_address[i+2] : 0;
+			*seg_ptr++ = ((i+2) < frag_length)?code_virtual_address[i+1] : 0;
+			*seg_ptr++ = ((i+3) < frag_length)?code_virtual_address[i+0] : 0;
 		}
-		tcb_desc->txbuf_size= (u16)i;
+		tcb_desc->txbuf_size = (u16)i;
 		skb_put(skb, i);
 
-		if (!priv->ieee80211->check_nic_enough_desc(dev,tcb_desc->queue_index)||
-			(!skb_queue_empty(&priv->ieee80211->skb_waitQ[tcb_desc->queue_index]))||\
-			(priv->ieee80211->queue_stop)) {
+		index = tcb_desc->queue_index;
+		if (!priv->ieee80211->check_nic_enough_desc(dev, index) ||
+		       (!skb_queue_empty(&priv->ieee80211->skb_waitQ[index])) ||
+		       (priv->ieee80211->queue_stop)) {
 			RT_TRACE(COMP_FIRMWARE,"=====================================================> tx full!\n");
 			skb_queue_tail(&priv->ieee80211->skb_waitQ[tcb_desc->queue_index], skb);
 		} else {
diff --git a/drivers/staging/rtl8192u/r819xU_firmware.h b/drivers/staging/rtl8192u/r819xU_firmware.h
index c48c884aa1af..cfa222350a9a 100644
--- a/drivers/staging/rtl8192u/r819xU_firmware.h
+++ b/drivers/staging/rtl8192u/r819xU_firmware.h
@@ -12,15 +12,15 @@
 #define GET_COMMAND_PACKET_FRAG_THRESHOLD(v)	(4*(v/4) - 8 - USB_HWDESC_HEADER_LEN)
 //#endif
 
-typedef enum _firmware_init_step{
+typedef enum _firmware_init_step {
 	FW_INIT_STEP0_BOOT = 0,
 	FW_INIT_STEP1_MAIN = 1,
 	FW_INIT_STEP2_DATA = 2,
-}firmware_init_step_e;
+} firmware_init_step_e;
 
-typedef enum _opt_rst_type{
+typedef enum _opt_rst_type {
 	OPT_SYSTEM_RESET = 0,
 	OPT_FIRMWARE_RESET = 1,
-}opt_rst_type_e;
+} opt_rst_type_e;
 
 #endif
diff --git a/drivers/staging/rtl8192u/r819xU_phy.c b/drivers/staging/rtl8192u/r819xU_phy.c
index e9c15fe8ded5..058960251bac 100644
--- a/drivers/staging/rtl8192u/r819xU_phy.c
+++ b/drivers/staging/rtl8192u/r819xU_phy.c
@@ -1463,6 +1463,7 @@ void rtl8192_SwChnl_WorkItem(struct net_device *dev)
 u8 rtl8192_phy_SwChnl(struct net_device *dev, u8 channel)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
+
 	RT_TRACE(COMP_CH, "%s(), SwChnlInProgress: %d\n", __func__,
 		 priv->SwChnlInProgress);
 	if (!priv->up)
diff --git a/drivers/staging/rtl8712/hal_init.c b/drivers/staging/rtl8712/hal_init.c
index 1d6ade05fa18..324da34383ea 100644
--- a/drivers/staging/rtl8712/hal_init.c
+++ b/drivers/staging/rtl8712/hal_init.c
@@ -86,7 +86,7 @@ static u32 rtl871x_open_fw(struct _adapter *padapter, const u8 **ppmappedfw)
 			(int)padapter->fw->size);
 		return 0;
 	}
-	*ppmappedfw = (u8 *)((*praw)->data);
+	*ppmappedfw = (*praw)->data;
 	return (*praw)->size;
 }
 
@@ -136,15 +136,10 @@ static void update_fwhdr(struct fw_hdr	*pfwhdr, const u8 *pmappedfw)
 static u8 chk_fwhdr(struct fw_hdr *pfwhdr, u32 ulfilelength)
 {
 	u32	fwhdrsz, fw_sz;
-	u8 intf, rfconf;
 
 	/* check signature */
 	if ((pfwhdr->signature != 0x8712) && (pfwhdr->signature != 0x8192))
 		return _FAIL;
-	/* check interface */
-	intf = (u8)((pfwhdr->version&0x3000) >> 12);
-	/* check rf_conf */
-	rfconf = (u8)((pfwhdr->version&0xC000) >> 14);
 	/* check fw_priv_sze & sizeof(struct fw_priv) */
 	if (pfwhdr->fw_priv_sz != sizeof(struct fw_priv))
 		return _FAIL;
@@ -162,7 +157,7 @@ static u8 rtl8712_dl_fw(struct _adapter *padapter)
 	sint i;
 	u8 tmp8, tmp8_a;
 	u16 tmp16;
-	u32 maxlen = 0, tmp32; /* for compare usage */
+	u32 maxlen = 0; /* for compare usage */
 	uint dump_imem_sz, imem_sz, dump_emem_sz, emem_sz; /* max = 49152; */
 	struct fw_hdr fwhdr;
 	u32 ulfilelength;	/* FW file size */
@@ -262,7 +257,7 @@ static u8 rtl8712_dl_fw(struct _adapter *padapter)
 		if (tmp8_a != (tmp8|BIT(2)))
 			goto exit_fail;
 
-		tmp32 = r8712_read32(padapter, TCR);
+		r8712_read32(padapter, TCR);
 
 		/* 4.polling IMEM Ready */
 		i = 100;
diff --git a/drivers/staging/rtl8712/ieee80211.c b/drivers/staging/rtl8712/ieee80211.c
index fe9459e483c5..57868085ce58 100644
--- a/drivers/staging/rtl8712/ieee80211.c
+++ b/drivers/staging/rtl8712/ieee80211.c
@@ -124,11 +124,10 @@ u8 *r8712_get_ie(u8 *pbuf, sint index, sint *len, sint limit)
 		if (*p == index) {
 			*len = *(p + 1);
 			return p;
-		} else {
-			tmp = *(p + 1);
-			p += (tmp + 2);
-			i += (tmp + 2);
 		}
+		tmp = *(p + 1);
+		p += (tmp + 2);
+		i += (tmp + 2);
 		if (i >= limit)
 			break;
 	}
@@ -237,10 +236,9 @@ unsigned char *r8712_get_wpa_ie(unsigned char *pie, int *wpa_ie_len, int limit)
 				goto check_next_ie;
 			*wpa_ie_len = *(pbuf + 1);
 			return pbuf;
-		} else {
-			*wpa_ie_len = 0;
-			return NULL;
 		}
+		*wpa_ie_len = 0;
+		return NULL;
 check_next_ie:
 		limit = limit - (pbuf - pie) - 2 - len;
 		if (limit <= 0)
@@ -370,13 +368,12 @@ int r8712_parse_wpa2_ie(u8 *rsn_ie, int rsn_ie_len, int *group_cipher,
 int r8712_get_sec_ie(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len,
 	       u8 *wpa_ie, u16 *wpa_len)
 {
-	u8 authmode, sec_idx;
+	u8 authmode;
 	u8 wpa_oui[4] = {0x0, 0x50, 0xf2, 0x01};
 	uint cnt;
 
 	/*Search required WPA or WPA2 IE and copy to sec_ie[ ]*/
 	cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_);
-	sec_idx = 0;
 	while (cnt < in_len) {
 		authmode = in_ie[cnt];
 		if ((authmode == _WPA_IE_ID_) &&
@@ -414,7 +411,7 @@ int r8712_get_wps_ie(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen)
 			cnt += in_ie[cnt+1]+2;
 			match = true;
 			break;
-		} else
+		}
 			cnt += in_ie[cnt+1]+2; /* goto next */
 	}
 	return match;
diff --git a/drivers/staging/rtl8712/osdep_service.h b/drivers/staging/rtl8712/osdep_service.h
index d7a357b8d2d1..5153ad9c2c75 100644
--- a/drivers/staging/rtl8712/osdep_service.h
+++ b/drivers/staging/rtl8712/osdep_service.h
@@ -88,17 +88,13 @@ static inline u32 _down_sema(struct semaphore *sema)
 {
 	if (down_interruptible(sema))
 		return _FAIL;
-	else
-		return _SUCCESS;
+	return _SUCCESS;
 }
 
 static inline u32 end_of_queue_search(struct list_head *head,
 		struct list_head *plist)
 {
-	if (head == plist)
-		return true;
-	else
-		return false;
+	return (head == plist);
 }
 
 static inline void sleep_schedulable(int ms)
diff --git a/drivers/staging/rtl8712/recv_linux.c b/drivers/staging/rtl8712/recv_linux.c
index 495ee1205e02..0631f3638257 100644
--- a/drivers/staging/rtl8712/recv_linux.c
+++ b/drivers/staging/rtl8712/recv_linux.c
@@ -146,7 +146,7 @@ void r8712_os_read_port(struct _adapter *padapter, struct recv_buf *precvbuf)
 	dev_kfree_skb_any(precvbuf->pskb);
 	precvbuf->pskb = NULL;
 	precvbuf->reuse = false;
-	if (precvbuf->irp_pending == false)
+	if (!precvbuf->irp_pending)
 		r8712_read_port(padapter, precvpriv->ff_hwaddr, 0,
 			 (unsigned char *)precvbuf);
 }
diff --git a/drivers/staging/rtl8712/rtl8712_cmd.c b/drivers/staging/rtl8712/rtl8712_cmd.c
index 720e8a15db96..62e53cc1d8b9 100644
--- a/drivers/staging/rtl8712/rtl8712_cmd.c
+++ b/drivers/staging/rtl8712/rtl8712_cmd.c
@@ -157,10 +157,8 @@ static u8 read_bbreg_hdl(struct _adapter *padapter, u8 *pbuf)
 {
 	u32 val;
 	void (*pcmd_callback)(struct _adapter *dev, struct cmd_obj	*pcmd);
-	struct readBB_parm *prdbbparm;
 	struct cmd_obj *pcmd  = (struct cmd_obj *)pbuf;
 
-	prdbbparm = (struct readBB_parm *)pcmd->parmbuf;
 	if (pcmd->rsp && pcmd->rspsz > 0)
 		memcpy(pcmd->rsp, (u8 *)&val, pcmd->rspsz);
 	pcmd_callback = cmd_callback[pcmd->cmdcode].callback;
@@ -174,10 +172,8 @@ static u8 read_bbreg_hdl(struct _adapter *padapter, u8 *pbuf)
 static u8 write_bbreg_hdl(struct _adapter *padapter, u8 *pbuf)
 {
 	void (*pcmd_callback)(struct _adapter *dev, struct cmd_obj *pcmd);
-	struct writeBB_parm *pwritebbparm;
 	struct cmd_obj *pcmd  = (struct cmd_obj *)pbuf;
 
-	pwritebbparm = (struct writeBB_parm *)pcmd->parmbuf;
 	pcmd_callback = cmd_callback[pcmd->cmdcode].callback;
 	if (pcmd_callback == NULL)
 		r8712_free_cmd_obj(pcmd);
@@ -190,10 +186,8 @@ static u8 read_rfreg_hdl(struct _adapter *padapter, u8 *pbuf)
 {
 	u32 val;
 	void (*pcmd_callback)(struct _adapter *dev, struct cmd_obj *pcmd);
-	struct readRF_parm *prdrfparm;
 	struct cmd_obj *pcmd  = (struct cmd_obj *)pbuf;
 
-	prdrfparm = (struct readRF_parm *)pcmd->parmbuf;
 	if (pcmd->rsp && pcmd->rspsz > 0)
 		memcpy(pcmd->rsp, (u8 *)&val, pcmd->rspsz);
 	pcmd_callback = cmd_callback[pcmd->cmdcode].callback;
@@ -207,10 +201,8 @@ static u8 read_rfreg_hdl(struct _adapter *padapter, u8 *pbuf)
 static u8 write_rfreg_hdl(struct _adapter *padapter, u8 *pbuf)
 {
 	void (*pcmd_callback)(struct _adapter *dev, struct cmd_obj *pcmd);
-	struct writeRF_parm *pwriterfparm;
 	struct cmd_obj *pcmd  = (struct cmd_obj *)pbuf;
 
-	pwriterfparm = (struct writeRF_parm *)pcmd->parmbuf;
 	pcmd_callback = cmd_callback[pcmd->cmdcode].callback;
 	if (pcmd_callback == NULL)
 		r8712_free_cmd_obj(pcmd);
@@ -222,9 +214,7 @@ static u8 write_rfreg_hdl(struct _adapter *padapter, u8 *pbuf)
 static u8 sys_suspend_hdl(struct _adapter *padapter, u8 *pbuf)
 {
 	struct cmd_obj *pcmd  = (struct cmd_obj *)pbuf;
-	struct usb_suspend_parm *psetusbsuspend;
 
-	psetusbsuspend = (struct usb_suspend_parm *)pcmd->parmbuf;
 	r8712_free_cmd_obj(pcmd);
 	return H2C_SUCCESS;
 }
@@ -320,7 +310,7 @@ void r8712_fw_cmd_data(struct _adapter *pAdapter, u32 *value, u8 flag)
 int r8712_cmd_thread(void *context)
 {
 	struct cmd_obj *pcmd;
-	unsigned int cmdsz, wr_sz, *pcmdbuf, *prspbuf;
+	unsigned int cmdsz, wr_sz, *pcmdbuf;
 	struct tx_desc *pdesc;
 	void (*pcmd_callback)(struct _adapter *dev, struct cmd_obj *pcmd);
 	struct _adapter *padapter = (struct _adapter *)context;
@@ -342,7 +332,6 @@ _next:
 			continue;
 		}
 		pcmdbuf = (unsigned int *)pcmdpriv->cmd_buf;
-		prspbuf = (unsigned int *)pcmdpriv->rsp_buf;
 		pdesc = (struct tx_desc *)pcmdbuf;
 		memset(pdesc, 0, TXDESC_SIZE);
 		pcmd = cmd_hdl_filter(padapter, pcmd);
diff --git a/drivers/staging/rtl8712/rtl8712_efuse.c b/drivers/staging/rtl8712/rtl8712_efuse.c
index c9eeb4270ab9..d95716999498 100644
--- a/drivers/staging/rtl8712/rtl8712_efuse.c
+++ b/drivers/staging/rtl8712/rtl8712_efuse.c
@@ -219,13 +219,12 @@ u16 r8712_efuse_get_current_size(struct _adapter *padapter)
 {
 	int bContinual = true;
 	u16 efuse_addr = 0;
-	u8 hoffset = 0, hworden = 0;
+	u8 hworden = 0;
 	u8 efuse_data, word_cnts = 0;
 
 	while (bContinual && efuse_one_byte_read(padapter, efuse_addr,
 	       &efuse_data) && (efuse_addr < efuse_available_max_size)) {
 		if (efuse_data != 0xFF) {
-			hoffset = (efuse_data >> 4) & 0x0F;
 			hworden =  efuse_data & 0x0F;
 			word_cnts = calculate_word_cnts(hworden);
 			/* read next header */
@@ -414,19 +413,18 @@ u8 r8712_efuse_pg_packet_write(struct _adapter *padapter, const u8 offset,
 					bResult = false;
 			}
 			break;
-		} else { /* write header fail */
-			bResult = false;
-			if (0xFF == efuse_data)
-				return bResult; /* nothing damaged. */
-			/* call rescue procedure */
-			if (fix_header(padapter, efuse_data, efuse_addr) ==
-			    false)
-				return false; /* rescue fail */
-
-			if (++repeat_times > _REPEAT_THRESHOLD_) /* fail */
-				break;
-			/* otherwise, take another risk... */
 		}
+		/* write header fail */
+		bResult = false;
+		if (0xFF == efuse_data)
+			return bResult; /* nothing damaged. */
+		/* call rescue procedure */
+		if (!fix_header(padapter, efuse_data, efuse_addr))
+			return false; /* rescue fail */
+
+		if (++repeat_times > _REPEAT_THRESHOLD_) /* fail */
+			break;
+		/* otherwise, take another risk... */
 	}
 	return bResult;
 }
@@ -541,15 +539,16 @@ u8 r8712_efuse_map_write(struct _adapter *padapter, u16 addr, u16 cnts,
 				}
 				idx++;
 				break;
-			} else {
-				if ((data[idx] != pktdata[i]) || (data[idx+1] !=
-				     pktdata[i+1])) {
-					word_en &= ~BIT(i >> 1);
-					newdata[j++] = data[idx];
-					newdata[j++] = data[idx + 1];
-				}
-				idx += 2;
 			}
+
+			if ((data[idx] != pktdata[i]) || (data[idx+1] !=
+			     pktdata[i+1])) {
+				word_en &= ~BIT(i >> 1);
+				newdata[j++] = data[idx];
+				newdata[j++] = data[idx + 1];
+			}
+			idx += 2;
+
 			if (idx == cnts)
 				break;
 		}
diff --git a/drivers/staging/rtl8712/rtl8712_recv.c b/drivers/staging/rtl8712/rtl8712_recv.c
index b27806209268..cd8b444b255b 100644
--- a/drivers/staging/rtl8712/rtl8712_recv.c
+++ b/drivers/staging/rtl8712/rtl8712_recv.c
@@ -158,8 +158,6 @@ int r8712_free_recvframe(union recv_frame *precvframe,
 static void update_recvframe_attrib_from_recvstat(struct rx_pkt_attrib *pattrib,
 					   struct recv_stat *prxstat)
 {
-	u32 *pphy_info;
-	struct phy_stat *pphy_stat;
 	u16 drvinfo_sz = 0;
 
 	drvinfo_sz = (le32_to_cpu(prxstat->rxdw0)&0x000f0000)>>16;
@@ -189,10 +187,6 @@ static void update_recvframe_attrib_from_recvstat(struct rx_pkt_attrib *pattrib,
 	/*Offset 16*/
 	/*Offset 20*/
 	/*phy_info*/
-	if (drvinfo_sz) {
-		pphy_stat = (struct phy_stat *)(prxstat+1);
-		pphy_info = (u32 *)prxstat+1;
-	}
 }
 
 /*perform defrag*/
@@ -200,7 +194,7 @@ static union recv_frame *recvframe_defrag(struct _adapter *adapter,
 				   struct  __queue *defrag_q)
 {
 	struct list_head *plist, *phead;
-	u8	*data, wlanhdr_offset;
+	u8 wlanhdr_offset;
 	u8	curfragnum;
 	struct recv_frame_hdr *pfhdr, *pnfhdr;
 	union recv_frame *prframe, *pnextrframe;
@@ -223,7 +217,6 @@ static union recv_frame *recvframe_defrag(struct _adapter *adapter,
 	curfragnum++;
 	plist = &defrag_q->queue;
 	plist = plist->next;
-	data = get_recvframe_data(prframe);
 	while (end_of_queue_search(phead, plist) == false) {
 		pnextrframe = LIST_CONTAINOR(plist, union recv_frame, u);
 		pnfhdr = &pnextrframe->u.hdr;
@@ -436,13 +429,11 @@ void r8712_rxcmd_event_hdl(struct _adapter *padapter, void *prxcmdbuf)
 {
 	uint voffset;
 	u8 *poffset;
-	u16 pkt_len, cmd_len, drvinfo_sz;
-	u8 eid, cmd_seq;
+	u16 cmd_len, drvinfo_sz;
 	struct recv_stat *prxstat;
 
 	poffset = (u8 *)prxcmdbuf;
 	voffset = *(uint *)poffset;
-	pkt_len = le32_to_cpu(voffset) & 0x00003fff;
 	prxstat = (struct recv_stat *)prxcmdbuf;
 	drvinfo_sz = ((le32_to_cpu(prxstat->rxdw0) & 0x000f0000) >> 16);
 	drvinfo_sz = drvinfo_sz << 3;
@@ -450,8 +441,6 @@ void r8712_rxcmd_event_hdl(struct _adapter *padapter, void *prxcmdbuf)
 	do {
 		voffset  = *(uint *)poffset;
 		cmd_len = (u16)(le32_to_cpu(voffset) & 0xffff);
-		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 alignment*/
 	} while (le32_to_cpu(voffset) & BIT(31));
@@ -533,11 +522,10 @@ int r8712_recv_indicatepkts_in_order(struct _adapter *padapter,
 	if (bforced == true) {
 		if (list_empty(phead))
 			return true;
-		else {
-			prframe = LIST_CONTAINOR(plist, union recv_frame, u);
-			pattrib = &prframe->u.hdr.attrib;
-			preorder_ctrl->indicate_seq = pattrib->seq_num;
-		}
+
+		prframe = LIST_CONTAINOR(plist, union recv_frame, u);
+		pattrib = &prframe->u.hdr.attrib;
+		preorder_ctrl->indicate_seq = pattrib->seq_num;
 	}
 	/* Prepare indication list and indication.
 	 * Check if there is any packet need indicate. */
diff --git a/drivers/staging/rtl8712/rtl871x_cmd.c b/drivers/staging/rtl8712/rtl871x_cmd.c
index d9c1561e3272..fe5e315319f7 100644
--- a/drivers/staging/rtl8712/rtl871x_cmd.c
+++ b/drivers/staging/rtl8712/rtl871x_cmd.c
@@ -205,12 +205,12 @@ void r8712_free_cmd_obj(struct cmd_obj *pcmd)
 {
 	if ((pcmd->cmdcode != _JoinBss_CMD_) &&
 	    (pcmd->cmdcode != _CreateBss_CMD_))
-		kfree((unsigned char *)pcmd->parmbuf);
+		kfree(pcmd->parmbuf);
 	if (pcmd->rsp != NULL) {
 		if (pcmd->rspsz != 0)
-			kfree((unsigned char *)pcmd->rsp);
+			kfree(pcmd->rsp);
 	}
-	kfree((unsigned char *)pcmd);
+	kfree(pcmd);
 }
 
 /*
@@ -232,7 +232,7 @@ u8 r8712_sitesurvey_cmd(struct _adapter *padapter,
 		return _FAIL;
 	psurveyPara = kmalloc(sizeof(*psurveyPara), GFP_ATOMIC);
 	if (psurveyPara == NULL) {
-		kfree((unsigned char *) ph2c);
+		kfree(ph2c);
 		return _FAIL;
 	}
 	init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara,
@@ -264,7 +264,7 @@ u8 r8712_setdatarate_cmd(struct _adapter *padapter, u8 *rateset)
 		return _FAIL;
 	pbsetdataratepara = kmalloc(sizeof(*pbsetdataratepara), GFP_ATOMIC);
 	if (pbsetdataratepara == NULL) {
-		kfree((u8 *) ph2c);
+		kfree(ph2c);
 		return _FAIL;
 	}
 	init_h2fwcmd_w_parm_no_rsp(ph2c, pbsetdataratepara,
@@ -286,7 +286,7 @@ u8 r8712_set_chplan_cmd(struct _adapter *padapter, int chplan)
 		return _FAIL;
 	psetchplanpara = kmalloc(sizeof(*psetchplanpara), GFP_ATOMIC);
 	if (psetchplanpara == NULL) {
-		kfree((u8 *) ph2c);
+		kfree(ph2c);
 		return _FAIL;
 	}
 	init_h2fwcmd_w_parm_no_rsp(ph2c, psetchplanpara,
@@ -307,7 +307,7 @@ u8 r8712_setbasicrate_cmd(struct _adapter *padapter, u8 *rateset)
 		return _FAIL;
 	pssetbasicratepara = kmalloc(sizeof(*pssetbasicratepara), GFP_ATOMIC);
 	if (pssetbasicratepara == NULL) {
-		kfree((u8 *) ph2c);
+		kfree(ph2c);
 		return _FAIL;
 	}
 	init_h2fwcmd_w_parm_no_rsp(ph2c, pssetbasicratepara,
@@ -329,7 +329,7 @@ u8 r8712_setptm_cmd(struct _adapter *padapter, u8 type)
 		return _FAIL;
 	pwriteptmparm = kmalloc(sizeof(*pwriteptmparm), GFP_ATOMIC);
 	if (pwriteptmparm == NULL) {
-		kfree((u8 *) ph2c);
+		kfree(ph2c);
 		return _FAIL;
 	}
 	init_h2fwcmd_w_parm_no_rsp(ph2c, pwriteptmparm, GEN_CMD_CODE(_SetPT));
@@ -349,7 +349,7 @@ u8 r8712_setfwdig_cmd(struct _adapter *padapter, u8 type)
 		return _FAIL;
 	pwriteptmparm = kmalloc(sizeof(*pwriteptmparm), GFP_ATOMIC);
 	if (pwriteptmparm == NULL) {
-		kfree((u8 *) ph2c);
+		kfree(ph2c);
 		return _FAIL;
 	}
 	init_h2fwcmd_w_parm_no_rsp(ph2c, pwriteptmparm, GEN_CMD_CODE(_SetDIG));
@@ -369,7 +369,7 @@ u8 r8712_setfwra_cmd(struct _adapter *padapter, u8 type)
 		return _FAIL;
 	pwriteptmparm = kmalloc(sizeof(*pwriteptmparm), GFP_ATOMIC);
 	if (pwriteptmparm == NULL) {
-		kfree((u8 *) ph2c);
+		kfree(ph2c);
 		return _FAIL;
 	}
 	init_h2fwcmd_w_parm_no_rsp(ph2c, pwriteptmparm, GEN_CMD_CODE(_SetRA));
@@ -389,7 +389,7 @@ u8 r8712_setrfreg_cmd(struct _adapter  *padapter, u8 offset, u32 val)
 		return _FAIL;
 	pwriterfparm = kmalloc(sizeof(*pwriterfparm), GFP_ATOMIC);
 	if (pwriterfparm == NULL) {
-		kfree((u8 *) ph2c);
+		kfree(ph2c);
 		return _FAIL;
 	}
 	init_h2fwcmd_w_parm_no_rsp(ph2c, pwriterfparm, GEN_CMD_CODE(_SetRFReg));
@@ -410,7 +410,7 @@ u8 r8712_getrfreg_cmd(struct _adapter *padapter, u8 offset, u8 *pval)
 		return _FAIL;
 	prdrfparm = kmalloc(sizeof(*prdrfparm), GFP_ATOMIC);
 	if (prdrfparm == NULL) {
-		kfree((u8 *) ph2c);
+		kfree(ph2c);
 		return _FAIL;
 	}
 	INIT_LIST_HEAD(&ph2c->list);
@@ -470,7 +470,6 @@ u8 r8712_createbss_cmd(struct _adapter *padapter)
 
 u8 r8712_joinbss_cmd(struct _adapter  *padapter, struct wlan_network *pnetwork)
 {
-	u8 *auth;
 	uint t_len = 0;
 	struct ndis_wlan_bssid_ex *psecnetwork;
 	struct cmd_obj		*pcmd;
@@ -517,7 +516,6 @@ u8 r8712_joinbss_cmd(struct _adapter  *padapter, struct wlan_network *pnetwork)
 		return _FAIL;
 	}
 	memcpy(psecnetwork, &pnetwork->network, t_len);
-	auth = &psecuritypriv->authenticator_ie[0];
 	psecuritypriv->authenticator_ie[0] = (unsigned char)
 					     psecnetwork->IELength;
 	if ((psecnetwork->IELength-12) < (256 - 1))
@@ -527,7 +525,7 @@ u8 r8712_joinbss_cmd(struct _adapter  *padapter, struct wlan_network *pnetwork)
 		memcpy(&psecuritypriv->authenticator_ie[1],
 			&psecnetwork->IEs[12], (256-1));
 	psecnetwork->IELength = 0;
-	/* If the the driver wants to use the bssid to create the connection.
+	/* If the driver wants to use the bssid to create the connection.
 	 * If not,  we copy the connecting AP's MAC address to it so that
 	 * the driver just has the bssid information for PMKIDList searching.
 	 */
@@ -626,7 +624,7 @@ u8 r8712_disassoc_cmd(struct _adapter *padapter) /* for sta_mode */
 		return _FAIL;
 	pdisconnect = kmalloc(sizeof(*pdisconnect), GFP_ATOMIC);
 	if (pdisconnect == NULL) {
-		kfree((u8 *)pdisconnect_cmd);
+		kfree(pdisconnect_cmd);
 		return _FAIL;
 	}
 	init_h2fwcmd_w_parm_no_rsp(pdisconnect_cmd, pdisconnect,
@@ -648,7 +646,7 @@ u8 r8712_setopmode_cmd(struct _adapter *padapter,
 		return _FAIL;
 	psetop = kmalloc(sizeof(*psetop), GFP_ATOMIC);
 	if (psetop == NULL) {
-		kfree((u8 *) ph2c);
+		kfree(ph2c);
 		return _FAIL;
 	}
 	init_h2fwcmd_w_parm_no_rsp(ph2c, psetop, _SetOpMode_CMD_);
@@ -672,13 +670,13 @@ u8 r8712_setstakey_cmd(struct _adapter *padapter, u8 *psta, u8 unicast_key)
 		return _FAIL;
 	psetstakey_para = kmalloc(sizeof(*psetstakey_para), GFP_ATOMIC);
 	if (psetstakey_para == NULL) {
-		kfree((u8 *) ph2c);
+		kfree(ph2c);
 		return _FAIL;
 	}
 	psetstakey_rsp = kmalloc(sizeof(*psetstakey_rsp), GFP_ATOMIC);
 	if (psetstakey_rsp == NULL) {
-		kfree((u8 *) ph2c);
-		kfree((u8 *) psetstakey_para);
+		kfree(ph2c);
+		kfree(psetstakey_para);
 		return _FAIL;
 	}
 	init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
@@ -712,7 +710,7 @@ u8 r8712_setrfintfs_cmd(struct _adapter *padapter, u8 mode)
 		return _FAIL;
 	psetrfintfsparm = kmalloc(sizeof(*psetrfintfsparm), GFP_ATOMIC);
 	if (psetrfintfsparm == NULL) {
-		kfree((unsigned char *) ph2c);
+		kfree(ph2c);
 		return _FAIL;
 	}
 	init_h2fwcmd_w_parm_no_rsp(ph2c, psetrfintfsparm,
@@ -734,7 +732,7 @@ u8 r8712_setrttbl_cmd(struct _adapter *padapter,
 		return _FAIL;
 	psetrttblparm = kmalloc(sizeof(*psetrttblparm), GFP_ATOMIC);
 	if (psetrttblparm == NULL) {
-		kfree((unsigned char *)ph2c);
+		kfree(ph2c);
 		return _FAIL;
 	}
 	init_h2fwcmd_w_parm_no_rsp(ph2c, psetrttblparm,
@@ -755,7 +753,7 @@ u8 r8712_gettssi_cmd(struct _adapter *padapter, u8 offset, u8 *pval)
 		return _FAIL;
 	prdtssiparm = kmalloc(sizeof(*prdtssiparm), GFP_ATOMIC);
 	if (prdtssiparm == NULL) {
-		kfree((unsigned char *) ph2c);
+		kfree(ph2c);
 		return _FAIL;
 	}
 	INIT_LIST_HEAD(&ph2c->list);
@@ -781,7 +779,7 @@ u8 r8712_setMacAddr_cmd(struct _adapter *padapter, u8 *mac_addr)
 		return _FAIL;
 	psetMacAddr_para = kmalloc(sizeof(*psetMacAddr_para), GFP_ATOMIC);
 	if (psetMacAddr_para == NULL) {
-		kfree((u8 *) ph2c);
+		kfree(ph2c);
 		return _FAIL;
 	}
 	init_h2fwcmd_w_parm_no_rsp(ph2c, psetMacAddr_para,
@@ -803,13 +801,13 @@ u8 r8712_setassocsta_cmd(struct _adapter *padapter, u8 *mac_addr)
 		return _FAIL;
 	psetassocsta_para = kmalloc(sizeof(*psetassocsta_para), GFP_ATOMIC);
 	if (psetassocsta_para == NULL) {
-		kfree((u8 *) ph2c);
+		kfree(ph2c);
 		return _FAIL;
 	}
 	psetassocsta_rsp = kmalloc(sizeof(*psetassocsta_rsp), GFP_ATOMIC);
 	if (psetassocsta_rsp == NULL) {
-		kfree((u8 *)ph2c);
-		kfree((u8 *)psetassocsta_para);
+		kfree(ph2c);
+		kfree(psetassocsta_para);
 		return _FAIL;
 	}
 	init_h2fwcmd_w_parm_no_rsp(ph2c, psetassocsta_para, _SetAssocSta_CMD_);
@@ -831,7 +829,7 @@ u8 r8712_addbareq_cmd(struct _adapter *padapter, u8 tid)
 		return _FAIL;
 	paddbareq_parm = kmalloc(sizeof(*paddbareq_parm), GFP_ATOMIC);
 	if (paddbareq_parm == NULL) {
-		kfree((unsigned char *)ph2c);
+		kfree(ph2c);
 		return _FAIL;
 	}
 	paddbareq_parm->tid = tid;
@@ -852,7 +850,7 @@ u8 r8712_wdg_wk_cmd(struct _adapter *padapter)
 		return _FAIL;
 	pdrvintcmd_param = kmalloc(sizeof(*pdrvintcmd_param), GFP_ATOMIC);
 	if (pdrvintcmd_param == NULL) {
-		kfree((unsigned char *)ph2c);
+		kfree(ph2c);
 		return _FAIL;
 	}
 	pdrvintcmd_param->i_cid = WDG_WK_CID;
@@ -1026,7 +1024,7 @@ u8 r8712_disconnectCtrlEx_cmd(struct _adapter *adapter, u32 enableDrvCtrl,
 		return _FAIL;
 	param = kzalloc(sizeof(*param), GFP_ATOMIC);
 	if (param == NULL) {
-		kfree((unsigned char *) ph2c);
+		kfree(ph2c);
 		return _FAIL;
 	}
 
diff --git a/drivers/staging/rtl8712/rtl871x_io.c b/drivers/staging/rtl8712/rtl871x_io.c
index d7b63aedead7..e4e5b13cb927 100644
--- a/drivers/staging/rtl8712/rtl871x_io.c
+++ b/drivers/staging/rtl8712/rtl871x_io.c
@@ -93,7 +93,7 @@ static uint register_intf_hdl(u8 *dev, struct intf_hdl *pintfhdl)
 	pintfhdl->intf_option = 0;
 	pintfhdl->adapter = dev;
 	pintfhdl->intf_dev = (u8 *)&(adapter->dvobjpriv);
-	if (_init_intf_hdl(adapter, pintfhdl) == false)
+	if (!_init_intf_hdl(adapter, pintfhdl))
 		goto register_intf_hdl_fail;
 	return _SUCCESS;
 register_intf_hdl_fail:
@@ -142,7 +142,7 @@ uint r8712_alloc_io_queue(struct _adapter *adapter)
 alloc_io_queue_fail:
 	if (pio_queue) {
 		kfree(pio_queue->pallocated_free_ioreqs_buf);
-		kfree((u8 *)pio_queue);
+		kfree(pio_queue);
 	}
 	adapter->pio_queue = NULL;
 	return _FAIL;
@@ -156,6 +156,6 @@ void r8712_free_io_queue(struct _adapter *adapter)
 		kfree(pio_queue->pallocated_free_ioreqs_buf);
 		adapter->pio_queue = NULL;
 		unregister_intf_hdl(&pio_queue->intf);
-		kfree((u8 *)pio_queue);
+		kfree(pio_queue);
 	}
 }
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl_linux.c b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
index 8e42ce06e5d7..73b7d864ccbd 100644
--- a/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
+++ b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
@@ -170,7 +170,7 @@ static inline char *translate_scan(struct _adapter *padapter,
 	s8 *p;
 	u32 i = 0, ht_ielen = 0;
 	u16	cap, ht_cap = false, mcs_rate;
-	u8	rssi, bw_40MHz = 0, short_GI = 0;
+	u8 rssi;
 
 	if ((pnetwork->network.Configuration.DSConfig < 1) ||
 	    (pnetwork->network.Configuration.DSConfig > 14)) {
@@ -197,10 +197,6 @@ static inline char *translate_scan(struct _adapter *padapter,
 		ht_cap = true;
 		pht_capie = (struct ieee80211_ht_cap *)(p + 2);
 		memcpy(&mcs_rate , pht_capie->supp_mcs_set, 2);
-		bw_40MHz = (pht_capie->cap_info&IEEE80211_HT_CAP_SUP_WIDTH)
-			   ? 1 : 0;
-		short_GI = (pht_capie->cap_info&(IEEE80211_HT_CAP_SGI_20 |
-			    IEEE80211_HT_CAP_SGI_40)) ? 1 : 0;
 	}
 	/* Add the protocol name */
 	iwe.cmd = SIOCGIWNAME;
@@ -287,12 +283,10 @@ static inline char *translate_scan(struct _adapter *padapter,
 		u8 wpa_ie[255], rsn_ie[255];
 		u16 wpa_len = 0, rsn_len = 0;
 		int n;
-		sint out_len = 0;
 
-		out_len = r8712_get_sec_ie(pnetwork->network.IEs,
-					   pnetwork->network.
-					   IELength, rsn_ie, &rsn_len,
-					   wpa_ie, &wpa_len);
+		r8712_get_sec_ie(pnetwork->network.IEs,
+				 pnetwork->network.IELength, rsn_ie, &rsn_len,
+				 wpa_ie, &wpa_len);
 		if (wpa_len > 0) {
 			memset(buf, 0, MAX_WPA_IE_LEN);
 			n = sprintf(buf, "wpa_ie=");
@@ -505,14 +499,14 @@ static int wpa_set_encryption(struct net_device *dev, struct ieee_param *param,
 		}
 	}
 exit:
-	kfree((u8 *)pwep);
+	kfree(pwep);
 	return ret;
 }
 
 static int r871x_set_wpa_ie(struct _adapter *padapter, char *pie,
 			    unsigned short ielen)
 {
-	u8 *buf = NULL, *pos = NULL;
+	u8 *buf = NULL;
 	int group_cipher = 0, pairwise_cipher = 0;
 	int ret = 0;
 
@@ -522,7 +516,6 @@ static int r871x_set_wpa_ie(struct _adapter *padapter, char *pie,
 		buf = kmemdup(pie, ielen, GFP_ATOMIC);
 		if (buf == NULL)
 			return -ENOMEM;
-		pos = buf;
 		if (ielen < RSN_HEADER_LEN) {
 			ret  = -EINVAL;
 			goto exit;
@@ -1133,13 +1126,11 @@ static int r871x_wx_set_mlme(struct net_device *dev,
 			     union iwreq_data *wrqu, char *extra)
 {
 	int ret = 0;
-	u16 reason;
 	struct _adapter *padapter = (struct _adapter *)netdev_priv(dev);
 	struct iw_mlme *mlme = (struct iw_mlme *) extra;
 
 	if (mlme == NULL)
 		return -1;
-	reason = cpu_to_le16(mlme->reason_code);
 	switch (mlme->cmd) {
 	case IW_MLME_DEAUTH:
 		if (!r8712_set_802_11_disassociate(padapter))
@@ -2216,7 +2207,7 @@ static int wpa_supplicant_ioctl(struct net_device *dev, struct iw_point *p)
 	}
 	if (ret == 0 && copy_to_user(p->pointer, param, p->length))
 		ret = -EFAULT;
-	kfree((u8 *)param);
+	kfree(param);
 	return ret;
 }
 
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl_set.c b/drivers/staging/rtl8712/rtl871x_ioctl_set.c
index 9d47eb472837..6318a0e65e6e 100644
--- a/drivers/staging/rtl8712/rtl871x_ioctl_set.c
+++ b/drivers/staging/rtl8712/rtl871x_ioctl_set.c
@@ -77,7 +77,7 @@ static u8 do_join(struct _adapter *padapter)
 		/* when set_ssid/set_bssid for do_join(), but scanning queue
 		 * is empty we try to issue sitesurvey firstly
 		 */
-		if (pmlmepriv->sitesurveyctrl.traffic_busy == false)
+		if (!pmlmepriv->sitesurveyctrl.traffic_busy)
 			r8712_sitesurvey_cmd(padapter, &pmlmepriv->assoc_ssid);
 		return true;
 	} else {
@@ -143,8 +143,7 @@ u8 r8712_set_802_11_bssid(struct _adapter *padapter, u8 *bssid)
 	    _FW_LINKED|WIFI_ADHOC_MASTER_STATE) == true) {
 		if (!memcmp(&pmlmepriv->cur_network.network.MacAddress, bssid,
 		    ETH_ALEN)) {
-			if (check_fwstate(pmlmepriv,
-			    WIFI_STATION_STATE) == false)
+			if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE))
 				goto _Abort_Set_BSSID; /* driver is in
 						* WIFI_ADHOC_MASTER_STATE */
 		} else {
@@ -177,7 +176,7 @@ void r8712_set_802_11_ssid(struct _adapter *padapter,
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct wlan_network *pnetwork = &pmlmepriv->cur_network;
 
-	if (padapter->hw_init_completed == false)
+	if (!padapter->hw_init_completed)
 		return;
 	spin_lock_irqsave(&pmlmepriv->lock, irqL);
 	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY|_FW_UNDER_LINKING)) {
@@ -188,10 +187,9 @@ void r8712_set_802_11_ssid(struct _adapter *padapter,
 		if ((pmlmepriv->assoc_ssid.SsidLength == ssid->SsidLength) &&
 		    (!memcmp(&pmlmepriv->assoc_ssid.Ssid, ssid->Ssid,
 		    ssid->SsidLength))) {
-			if ((check_fwstate(pmlmepriv,
-			     WIFI_STATION_STATE) == false)) {
-				if (r8712_is_same_ibss(padapter,
-				     pnetwork) == false) {
+			if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+				if (!r8712_is_same_ibss(padapter,
+				     pnetwork)) {
 					/* if in WIFI_ADHOC_MASTER_STATE or
 					 *  WIFI_ADHOC_STATE, create bss or
 					 * rejoin again
@@ -227,7 +225,7 @@ void r8712_set_802_11_ssid(struct _adapter *padapter,
 	}
 	if (padapter->securitypriv.btkip_countermeasure == true)
 		goto _Abort_Set_SSID;
-	if (validate_ssid(ssid) == false)
+	if (!validate_ssid(ssid))
 		goto _Abort_Set_SSID;
 	memcpy(&pmlmepriv->assoc_ssid, ssid, sizeof(struct ndis_802_11_ssid));
 	pmlmepriv->assoc_by_bssid = false;
@@ -308,10 +306,10 @@ u8 r8712_set_802_11_bssid_list_scan(struct _adapter *padapter)
 	unsigned long irqL;
 	u8 ret = true;
 
-	if (padapter == NULL)
+	if (!padapter)
 		return false;
 	pmlmepriv = &padapter->mlmepriv;
-	if (padapter->hw_init_completed == false)
+	if (!padapter->hw_init_completed)
 		return false;
 	spin_lock_irqsave(&pmlmepriv->lock, irqL);
 	if ((check_fwstate(pmlmepriv, _FW_UNDER_SURVEY|_FW_UNDER_LINKING)) ||
@@ -345,13 +343,9 @@ u8 r8712_set_802_11_authentication_mode(struct _adapter *padapter,
 u8 r8712_set_802_11_add_wep(struct _adapter *padapter,
 			    struct NDIS_802_11_WEP *wep)
 {
-	u8	bdefaultkey;
-	u8	btransmitkey;
 	sint	keyid;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 
-	bdefaultkey = (wep->KeyIndex & 0x40000000) > 0 ? false : true;
-	btransmitkey = (wep->KeyIndex & 0x80000000) > 0 ? true : false;
 	keyid = wep->KeyIndex & 0x3fffffff;
 	if (keyid >= WEP_KEYS)
 		return false;
diff --git a/drivers/staging/rtl8712/rtl871x_mlme.c b/drivers/staging/rtl8712/rtl871x_mlme.c
index 00f2e0fc4ac5..b7462e8145d6 100644
--- a/drivers/staging/rtl8712/rtl871x_mlme.c
+++ b/drivers/staging/rtl8712/rtl871x_mlme.c
@@ -923,7 +923,7 @@ void r8712_joinbss_event_callback(struct _adapter *adapter, u8 *pbuf)
 ignore_joinbss_callback:
 	spin_unlock_irqrestore(&pmlmepriv->lock, irqL);
 	if (sizeof(struct list_head) == 4 * sizeof(u32))
-		kfree((u8 *)pnetwork);
+		kfree(pnetwork);
 }
 
 void r8712_stassoc_event_callback(struct _adapter *adapter, u8 *pbuf)
@@ -1218,7 +1218,7 @@ sint r8712_set_auth(struct _adapter *adapter,
 
 	psetauthparm = kzalloc(sizeof(*psetauthparm), GFP_ATOMIC);
 	if (psetauthparm == NULL) {
-		kfree((unsigned char *)pcmd);
+		kfree(pcmd);
 		return _FAIL;
 	}
 	psetauthparm->mode = (u8)psecuritypriv->AuthAlgrthm;
@@ -1372,7 +1372,7 @@ static int SecIsInPMKIDList(struct _adapter *Adapter, u8 *bssid)
 sint r8712_restruct_sec_ie(struct _adapter *adapter, u8 *in_ie,
 		     u8 *out_ie, uint in_len)
 {
-	u8 authmode = 0, securitytype, match;
+	u8 authmode = 0, match;
 	u8 sec_ie[255], uncst_oui[4], bkup_ie[255];
 	u8 wpa_oui[4] = {0x0, 0x50, 0xf2, 0x01};
 	uint ielength, cnt, remove_cnt;
@@ -1399,21 +1399,17 @@ sint r8712_restruct_sec_ie(struct _adapter *adapter, u8 *in_ie,
 	switch (ndissecuritytype) {
 	case Ndis802_11Encryption1Enabled:
 	case Ndis802_11Encryption1KeyAbsent:
-		securitytype = _WEP40_;
 		uncst_oui[3] = 0x1;
 		break;
 	case Ndis802_11Encryption2Enabled:
 	case Ndis802_11Encryption2KeyAbsent:
-		securitytype = _TKIP_;
 		uncst_oui[3] = 0x2;
 		break;
 	case Ndis802_11Encryption3Enabled:
 	case Ndis802_11Encryption3KeyAbsent:
-		securitytype = _AES_;
 		uncst_oui[3] = 0x4;
 		break;
 	default:
-		securitytype = _NO_PRIVACY_;
 		break;
 	}
 	/*Search required WPA or WPA2 IE and copy to sec_ie[] */
@@ -1705,7 +1701,7 @@ unsigned int r8712_restructure_ht_ie(struct _adapter *padapter, u8 *in_ie,
 				     u8 *out_ie, uint in_len, uint *pout_len)
 {
 	u32 ielen, out_len;
-	unsigned char *p, *pframe;
+	unsigned char *p;
 	struct ieee80211_ht_cap ht_capie;
 	unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01, 0x00};
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
@@ -1717,10 +1713,8 @@ unsigned int r8712_restructure_ht_ie(struct _adapter *padapter, u8 *in_ie,
 	if (p && (ielen > 0)) {
 		if (pqospriv->qos_option == 0) {
 			out_len = *pout_len;
-			pframe = r8712_set_ie(out_ie+out_len,
-					      _VENDOR_SPECIFIC_IE_,
-					      _WMM_IE_Length_,
-					       WMM_IE, pout_len);
+			r8712_set_ie(out_ie+out_len, _VENDOR_SPECIFIC_IE_,
+				     _WMM_IE_Length_, WMM_IE, pout_len);
 			pqospriv->qos_option = 1;
 		}
 		out_len = *pout_len;
@@ -1733,9 +1727,9 @@ unsigned int r8712_restructure_ht_ie(struct _adapter *padapter, u8 *in_ie,
 				    IEEE80211_HT_CAP_DSSSCCK40;
 		ht_capie.ampdu_params_info = (IEEE80211_HT_CAP_AMPDU_FACTOR &
 				0x03) | (IEEE80211_HT_CAP_AMPDU_DENSITY & 0x00);
-		pframe = r8712_set_ie(out_ie+out_len, _HT_CAPABILITY_IE_,
-				sizeof(struct ieee80211_ht_cap),
-				(unsigned char *)&ht_capie, pout_len);
+		r8712_set_ie(out_ie+out_len, _HT_CAPABILITY_IE_,
+			     sizeof(struct ieee80211_ht_cap),
+			     (unsigned char *)&ht_capie, pout_len);
 		phtpriv->ht_option = 1;
 	}
 	return phtpriv->ht_option;
@@ -1748,7 +1742,6 @@ static void update_ht_cap(struct _adapter *padapter, u8 *pie, uint ie_len)
 	int i, len;
 	struct sta_info *bmc_sta, *psta;
 	struct ieee80211_ht_cap *pht_capie;
-	struct ieee80211_ht_addt_info *pht_addtinfo;
 	struct recv_reorder_ctrl *preorder_ctrl;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
@@ -1801,8 +1794,6 @@ static void update_ht_cap(struct _adapter *padapter, u8 *pie, uint ie_len)
 	p = r8712_get_ie(pie + sizeof(struct NDIS_802_11_FIXED_IEs),
 		   _HT_ADD_INFO_IE_, &len,
 		   ie_len-sizeof(struct NDIS_802_11_FIXED_IEs));
-	if (p && len > 0)
-		pht_addtinfo = (struct ieee80211_ht_addt_info *)(p + 2);
 }
 
 void r8712_issue_addbareq_cmd(struct _adapter *padapter, int priority)
diff --git a/drivers/staging/rtl8712/rtl871x_mp.c b/drivers/staging/rtl8712/rtl871x_mp.c
index 2ec660ad78af..3d913b9701bb 100644
--- a/drivers/staging/rtl8712/rtl871x_mp.c
+++ b/drivers/staging/rtl8712/rtl871x_mp.c
@@ -56,7 +56,7 @@ static int init_mp_priv(struct mp_priv *pmp_priv)
 	pmp_priv->pallocated_mp_xmitframe_buf = kmalloc(NR_MP_XMITFRAME *
 				sizeof(struct mp_xmit_frame) + 4,
 				GFP_ATOMIC);
-	if (pmp_priv->pallocated_mp_xmitframe_buf == NULL) {
+	if (!pmp_priv->pallocated_mp_xmitframe_buf) {
 		res = _FAIL;
 		goto _exit_init_mp_priv;
 	}
@@ -173,7 +173,7 @@ u8 r8712_bb_reg_write(struct _adapter *pAdapter, u16 offset, u32 value)
 		oldValue = r8712_bb_reg_read(pAdapter, iocmd.value);
 		oldValue &= (0xFFFFFFFF >> ((4 - shift) * 8));
 		value = oldValue | (newValue << (shift * 8));
-		if (fw_iocmd_write(pAdapter, iocmd, value) == false)
+		if (!fw_iocmd_write(pAdapter, iocmd, value))
 			return false;
 		iocmd.value += 4;
 		oldValue = r8712_bb_reg_read(pAdapter, iocmd.value);
diff --git a/drivers/staging/rtl8712/rtl871x_mp_ioctl.c b/drivers/staging/rtl8712/rtl871x_mp_ioctl.c
index 9827ff8143b2..a16f15e91992 100644
--- a/drivers/staging/rtl8712/rtl871x_mp_ioctl.c
+++ b/drivers/staging/rtl8712/rtl871x_mp_ioctl.c
@@ -1431,11 +1431,8 @@ unsigned int mp_ioctl_xmit_packet_hdl(struct oid_par_priv *poid_par_priv)
 /*-------------------------------------------------------------------------*/
 uint oid_rt_set_power_down_hdl(struct oid_par_priv *poid_par_priv)
 {
-	u8	bpwrup;
-
 	if (poid_par_priv->type_of_oid != SET_OID)
 		return RNDIS_STATUS_NOT_ACCEPTED;
-	bpwrup = *(u8 *)poid_par_priv->information_buf;
 	/*CALL  the power_down function*/
 	return RNDIS_STATUS_SUCCESS;
 }
diff --git a/drivers/staging/rtl8712/rtl871x_pwrctrl.c b/drivers/staging/rtl8712/rtl871x_pwrctrl.c
index 51dcf5555b62..ed2844d2b02a 100644
--- a/drivers/staging/rtl8712/rtl871x_pwrctrl.c
+++ b/drivers/staging/rtl8712/rtl871x_pwrctrl.c
@@ -156,11 +156,9 @@ static void rpwm_workitem_callback(struct work_struct *work)
 				       struct pwrctrl_priv, rpwm_workitem);
 	struct _adapter *padapter = container_of(pwrpriv,
 				    struct _adapter, pwrctrlpriv);
-	u8 cpwm = pwrpriv->cpwm;
-
 	if (pwrpriv->cpwm != pwrpriv->rpwm) {
 		_enter_pwrlock(&pwrpriv->lock);
-		cpwm = r8712_read8(padapter, SDIO_HCPWM);
+		r8712_read8(padapter, SDIO_HCPWM);
 		pwrpriv->rpwm_retry = 1;
 		r8712_set_rpwm(padapter, pwrpriv->rpwm);
 		up(&pwrpriv->lock);
diff --git a/drivers/staging/rtl8712/rtl871x_recv.c b/drivers/staging/rtl8712/rtl871x_recv.c
index 9b99a71670f3..06f15f81c4d8 100644
--- a/drivers/staging/rtl8712/rtl871x_recv.c
+++ b/drivers/staging/rtl8712/rtl871x_recv.c
@@ -601,7 +601,7 @@ sint r8712_wlanhdr_to_ethhdr(union recv_frame *precvframe)
 {
 	/*remove the wlanhdr and add the eth_hdr*/
 	sint	rmv_len;
-	u16	eth_type, len;
+	u16	len;
 	u8	bsnaphdr;
 	u8	*psnap_type;
 	struct ieee80211_snap_hdr *psnap;
@@ -635,7 +635,6 @@ sint r8712_wlanhdr_to_ethhdr(union recv_frame *precvframe)
 		ptr += rmv_len;
 		*ptr = 0x87;
 		*(ptr+1) = 0x12;
-		eth_type = 0x8712;
 		/* append rx status for mp test packets */
 		ptr = recvframe_pull(precvframe, (rmv_len -
 		      sizeof(struct ethhdr) + 2) - 24);
@@ -658,27 +657,11 @@ s32 r8712_recv_entry(union recv_frame *precvframe)
 {
 	struct _adapter *padapter;
 	struct recv_priv *precvpriv;
-	struct	mlme_priv *pmlmepriv;
-	struct recv_stat *prxstat;
-	struct dvobj_priv *pdev;
-	u8 *phead, *pdata, *ptail, *pend;
 
-	struct  __queue *pfree_recv_queue, *ppending_recv_queue;
 	s32 ret = _SUCCESS;
-	struct intf_hdl *pintfhdl;
 
 	padapter = precvframe->u.hdr.adapter;
-	pintfhdl = &padapter->pio_queue->intf;
-	pmlmepriv = &padapter->mlmepriv;
 	precvpriv = &(padapter->recvpriv);
-	pdev = &padapter->dvobjpriv;
-	pfree_recv_queue = &(precvpriv->free_recv_queue);
-	ppending_recv_queue = &(precvpriv->recv_pending_queue);
-	phead = precvframe->u.hdr.rx_head;
-	pdata = precvframe->u.hdr.rx_data;
-	ptail = precvframe->u.hdr.rx_tail;
-	pend = precvframe->u.hdr.rx_end;
-	prxstat = (struct recv_stat *)phead;
 
 	padapter->ledpriv.LedControlHandler(padapter, LED_CTL_RX);
 
diff --git a/drivers/staging/rtl8712/rtl871x_recv.h b/drivers/staging/rtl8712/rtl871x_recv.h
index 92ca8997e5bc..77487bb9d3c0 100644
--- a/drivers/staging/rtl8712/rtl871x_recv.h
+++ b/drivers/staging/rtl8712/rtl871x_recv.h
@@ -170,11 +170,8 @@ static inline u8 *recvframe_put(union recv_frame *precvframe, sint sz)
 	/* used for append sz bytes from ptr to rx_tail, update rx_tail and
 	 * return the updated rx_tail to the caller
 	 * after putting, rx_tail must be still larger than rx_end. */
-	unsigned char *prev_rx_tail;
-
 	if (precvframe == NULL)
 		return NULL;
-	prev_rx_tail = precvframe->u.hdr.rx_tail;
 	precvframe->u.hdr.rx_tail += sz;
 	if (precvframe->u.hdr.rx_tail > precvframe->u.hdr.rx_end) {
 		precvframe->u.hdr.rx_tail -= sz;
diff --git a/drivers/staging/rtl8712/rtl871x_security.c b/drivers/staging/rtl8712/rtl871x_security.c
index 8faf22bb7c90..c653ad6854b4 100644
--- a/drivers/staging/rtl8712/rtl871x_security.c
+++ b/drivers/staging/rtl8712/rtl871x_security.c
@@ -578,7 +578,7 @@ u32 r8712_tkip_encrypt(struct _adapter *padapter, u8 *pxmitframe)
 	u8 ttkey[16];
 	u8 crc[4];
 	struct arc4context mycontext;
-	u32 curfragnum, length, prwskeylen;
+	u32 curfragnum, length;
 
 	u8 *pframe, *payload, *iv, *prwskey;
 	union pn48 txpn;
@@ -600,7 +600,6 @@ u32 r8712_tkip_encrypt(struct _adapter *padapter, u8 *pxmitframe)
 				  &pattrib->ra[0]);
 		if (stainfo != NULL) {
 			prwskey = &stainfo->x_UncstKey.skey[0];
-			prwskeylen = 16;
 			for (curfragnum = 0; curfragnum < pattrib->nr_frags;
 			     curfragnum++) {
 				iv = pframe + pattrib->hdrlen;
@@ -655,7 +654,7 @@ u32 r8712_tkip_decrypt(struct _adapter *padapter, u8 *precvframe)
 	u8 ttkey[16];
 	u8 crc[4];
 	struct arc4context mycontext;
-	u32 length, prwskeylen;
+	u32 length;
 	u8 *pframe, *payload, *iv, *prwskey, idx = 0;
 	union pn48 txpn;
 	struct	sta_info *stainfo;
@@ -683,7 +682,6 @@ u32 r8712_tkip_decrypt(struct _adapter *padapter, u8 *precvframe)
 					return _FAIL;
 			} else
 				prwskey = &stainfo->x_UncstKey.skey[0];
-			prwskeylen = 16;
 			GET_TKIP_PN(iv, txpn);
 			pnl = (u16)(txpn.val);
 			pnh = (u32)(txpn.val >> 16);
@@ -1154,7 +1152,6 @@ u32 r8712_aes_encrypt(struct _adapter *padapter, u8 *pxmitframe)
 {	/* exclude ICV */
 	/* Intermediate Buffers */
 	sint	curfragnum, length;
-	u32	prwskeylen;
 	u8	*pframe, *prwskey;
 	struct	sta_info *stainfo;
 	struct	pkt_attrib  *pattrib = &((struct xmit_frame *)
@@ -1174,7 +1171,6 @@ u32 r8712_aes_encrypt(struct _adapter *padapter, u8 *pxmitframe)
 				  &pattrib->ra[0]);
 		if (stainfo != NULL) {
 			prwskey = &stainfo->x_UncstKey.skey[0];
-			prwskeylen = 16;
 			for (curfragnum = 0; curfragnum < pattrib->nr_frags;
 			     curfragnum++) {
 				if ((curfragnum + 1) == pattrib->nr_frags) {
@@ -1363,7 +1359,6 @@ u32 r8712_aes_decrypt(struct _adapter *padapter, u8 *precvframe)
 {	/* exclude ICV */
 	/* Intermediate Buffers */
 	sint		length;
-	u32	prwskeylen;
 	u8	*pframe, *prwskey, *iv, idx;
 	struct	sta_info *stainfo;
 	struct	rx_pkt_attrib *prxattrib = &((union recv_frame *)
@@ -1387,7 +1382,6 @@ u32 r8712_aes_decrypt(struct _adapter *padapter, u8 *precvframe)
 
 			} else
 				prwskey = &stainfo->x_UncstKey.skey[0];
-			prwskeylen = 16;
 			length = ((union recv_frame *)precvframe)->
 				 u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len;
 			aes_decipher(prwskey, prxattrib->hdrlen, pframe,
diff --git a/drivers/staging/rtl8712/rtl871x_sta_mgt.c b/drivers/staging/rtl8712/rtl871x_sta_mgt.c
index e769bb5c5fb8..4c9b98e8210e 100644
--- a/drivers/staging/rtl8712/rtl871x_sta_mgt.c
+++ b/drivers/staging/rtl8712/rtl871x_sta_mgt.c
@@ -79,13 +79,11 @@ static void mfree_all_stainfo(struct sta_priv *pstapriv)
 {
 	unsigned long irqL;
 	struct list_head *plist, *phead;
-	struct sta_info *psta = NULL;
 
 	spin_lock_irqsave(&pstapriv->sta_hash_lock, irqL);
 	phead = &pstapriv->free_sta_queue.queue;
 	plist = phead->next;
 	while ((end_of_queue_search(phead, plist)) == false) {
-		psta = LIST_CONTAINOR(plist, struct sta_info, list);
 		plist = plist->next;
 	}
 
@@ -109,7 +107,6 @@ u32 _r8712_free_sta_priv(struct sta_priv *pstapriv)
 
 struct sta_info *r8712_alloc_stainfo(struct sta_priv *pstapriv, u8 *hwaddr)
 {
-	uint tmp_aid;
 	s32	index;
 	struct list_head *phash_list;
 	struct sta_info	*psta;
@@ -127,7 +124,6 @@ struct sta_info *r8712_alloc_stainfo(struct sta_priv *pstapriv, u8 *hwaddr)
 		psta = LIST_CONTAINOR(pfree_sta_queue->queue.next,
 				      struct sta_info, list);
 		list_del_init(&(psta->list));
-		tmp_aid = psta->aid;
 		_init_stainfo(psta);
 		memcpy(psta->hwaddr, hwaddr, ETH_ALEN);
 		index = wifi_mac_hash(hwaddr);
@@ -267,15 +263,10 @@ struct sta_info *r8712_get_stainfo(struct sta_priv *pstapriv, u8 *hwaddr)
 
 void r8712_init_bcmc_stainfo(struct _adapter *padapter)
 {
-	struct sta_info	*psta;
-	struct tx_servq	*ptxservq;
 	unsigned char bcast_addr[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 	struct	sta_priv *pstapriv = &padapter->stapriv;
 
-	psta = r8712_alloc_stainfo(pstapriv, bcast_addr);
-	if (psta == NULL)
-		return;
-	ptxservq = &(psta->sta_xmitpriv.be_q);
+	r8712_alloc_stainfo(pstapriv, bcast_addr);
 }
 
 struct sta_info *r8712_get_bcmc_stainfo(struct _adapter *padapter)
diff --git a/drivers/staging/rtl8712/rtl871x_xmit.c b/drivers/staging/rtl8712/rtl871x_xmit.c
index f49acaf04076..62a377e7fdc7 100644
--- a/drivers/staging/rtl8712/rtl871x_xmit.c
+++ b/drivers/staging/rtl8712/rtl871x_xmit.c
@@ -184,7 +184,6 @@ void _free_xmit_priv(struct xmit_priv *pxmitpriv)
 sint r8712_update_attrib(struct _adapter *padapter, _pkt *pkt,
 		   struct pkt_attrib *pattrib)
 {
-	uint i;
 	struct pkt_file pktfile;
 	struct sta_info *psta = NULL;
 	struct ethhdr etherhdr;
@@ -199,7 +198,7 @@ sint r8712_update_attrib(struct _adapter *padapter, _pkt *pkt,
 
 	_r8712_open_pktfile(pkt, &pktfile);
 
-	i = _r8712_pktfile_read(&pktfile, (unsigned char *)&etherhdr, ETH_HLEN);
+	_r8712_pktfile_read(&pktfile, (unsigned char *)&etherhdr, ETH_HLEN);
 
 	pattrib->ether_type = ntohs(etherhdr.h_proto);
 
@@ -236,7 +235,7 @@ sint r8712_update_attrib(struct _adapter *padapter, _pkt *pkt,
 		/* for mp storing the txcmd per packet,
 		 * according to the info of txcmd to update pattrib */
 		/*get MP_TXDESC_SIZE bytes txcmd per packet*/
-		i = _r8712_pktfile_read(&pktfile, (u8 *)&txdesc, TXDESC_SIZE);
+		_r8712_pktfile_read(&pktfile, (u8 *)&txdesc, TXDESC_SIZE);
 		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
 		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
 		pattrib->pctrl = 1;
@@ -347,7 +346,7 @@ sint r8712_update_attrib(struct _adapter *padapter, _pkt *pkt,
 static sint xmitframe_addmic(struct _adapter *padapter,
 			     struct xmit_frame *pxmitframe)
 {
-	u32	curfragnum, length, datalen;
+	u32	curfragnum, length;
 	u8	*pframe, *payload, mic[8];
 	struct	mic_data micdata;
 	struct	sta_info *stainfo;
@@ -369,7 +368,6 @@ static sint xmitframe_addmic(struct _adapter *padapter,
 			u8 null_key[16] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
 					   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
 					   0x0, 0x0};
-			datalen = pattrib->pktlen - pattrib->hdrlen;
 			pframe = pxmitframe->buf_addr + TXDESC_OFFSET;
 			if (bmcst) {
 				if (!memcmp(psecuritypriv->XGrptxmickey
@@ -486,7 +484,7 @@ static sint make_wlanhdr(struct _adapter *padapter , u8 *hdr,
 	memset(hdr, 0, WLANHDR_OFFSET);
 	SetFrameSubType(fctrl, pattrib->subtype);
 	if (pattrib->subtype & WIFI_DATA_TYPE) {
-		if ((check_fwstate(pmlmepriv,  WIFI_STATION_STATE) == true)) {
+		if (check_fwstate(pmlmepriv,  WIFI_STATION_STATE) == true) {
 			/* to_ds = 1, fr_ds = 0; */
 			SetToDs(fctrl);
 			memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv),
@@ -825,16 +823,13 @@ void r8712_free_xmitframe(struct xmit_priv *pxmitpriv,
 	unsigned long irqL;
 	struct  __queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue;
 	struct _adapter *padapter = pxmitpriv->adapter;
-	struct sk_buff *pndis_pkt = NULL;
 
 	if (pxmitframe == NULL)
 		return;
 	spin_lock_irqsave(&pfree_xmit_queue->lock, irqL);
 	list_del_init(&pxmitframe->list);
-	if (pxmitframe->pkt) {
-		pndis_pkt = pxmitframe->pkt;
+	if (pxmitframe->pkt)
 		pxmitframe->pkt = NULL;
-	}
 	list_add_tail(&pxmitframe->list, &pfree_xmit_queue->queue);
 	pxmitpriv->free_xmitframe_cnt++;
 	spin_unlock_irqrestore(&pfree_xmit_queue->lock, irqL);
diff --git a/drivers/staging/rtl8712/usb_intf.c b/drivers/staging/rtl8712/usb_intf.c
index a3d733b145eb..7d0d1719b136 100644
--- a/drivers/staging/rtl8712/usb_intf.c
+++ b/drivers/staging/rtl8712/usb_intf.c
@@ -255,9 +255,6 @@ static struct drv_priv drvpriv = {
 static uint r8712_usb_dvobj_init(struct _adapter *padapter)
 {
 	uint	status = _SUCCESS;
-	struct	usb_device_descriptor		*pdev_desc;
-	struct	usb_host_config			*phost_conf;
-	struct	usb_config_descriptor		*pconf_desc;
 	struct	usb_host_interface		*phost_iface;
 	struct	usb_interface_descriptor	*piface_desc;
 	struct dvobj_priv *pdvobjpriv = &padapter->dvobjpriv;
@@ -265,9 +262,6 @@ static uint r8712_usb_dvobj_init(struct _adapter *padapter)
 
 	pdvobjpriv->padapter = padapter;
 	padapter->EepromAddressSize = 6;
-	pdev_desc = &pusbd->descriptor;
-	phost_conf = pusbd->actconfig;
-	pconf_desc = &phost_conf->desc;
 	phost_iface = &pintf->altsetting[0];
 	piface_desc = &phost_iface->desc;
 	pdvobjpriv->nr_endpoint = piface_desc->bNumEndpoints;
@@ -292,13 +286,13 @@ static void r8712_usb_dvobj_deinit(struct _adapter *padapter)
 void rtl871x_intf_stop(struct _adapter *padapter)
 {
 	/*disable_hw_interrupt*/
-	if (padapter->bSurpriseRemoved == false) {
+	if (!padapter->bSurpriseRemoved) {
 		/*device still exists, so driver can do i/o operation
 		 * TODO: */
 	}
 
 	/* cancel in irp */
-	if (padapter->dvobjpriv.inirp_deinit != NULL)
+	if (padapter->dvobjpriv.inirp_deinit)
 		padapter->dvobjpriv.inirp_deinit(padapter);
 	/* cancel out irp */
 	r8712_usb_write_port_cancel(padapter);
@@ -318,7 +312,7 @@ void r871x_dev_unload(struct _adapter *padapter)
 		r8712_stop_drv_threads(padapter);
 
 		/*s5.*/
-		if (padapter->bSurpriseRemoved == false) {
+		if (!padapter->bSurpriseRemoved) {
 			padapter->hw_init_completed = false;
 			rtl8712_hal_deinit(padapter);
 		}
@@ -402,7 +396,7 @@ static int r871xu_drv_init(struct usb_interface *pusb_intf,
 	/* step 3.
 	 * initialize the dvobj_priv
 	 */
-	if (padapter->dvobj_init == NULL)
+	if (!padapter->dvobj_init)
 			goto error;
 	else {
 		status = padapter->dvobj_init(padapter);
@@ -568,7 +562,7 @@ static int r871xu_drv_init(struct usb_interface *pusb_intf,
 		    ((mac[0] == 0x00) && (mac[1] == 0x00) &&
 		     (mac[2] == 0x00) && (mac[3] == 0x00) &&
 		     (mac[4] == 0x00) && (mac[5] == 0x00)) ||
-		     (AutoloadFail == false)) {
+		     (!AutoloadFail)) {
 			mac[0] = 0x00;
 			mac[1] = 0xe0;
 			mac[2] = 0x4c;
diff --git a/drivers/staging/rtl8712/usb_ops_linux.c b/drivers/staging/rtl8712/usb_ops_linux.c
index e89d2b07fcb9..c3a4e3f26b40 100644
--- a/drivers/staging/rtl8712/usb_ops_linux.c
+++ b/drivers/staging/rtl8712/usb_ops_linux.c
@@ -168,7 +168,6 @@ static void usb_write_mem_complete(struct urb *purb)
 void r8712_usb_write_mem(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *wmem)
 {
 	unsigned int pipe;
-	int status;
 	struct _adapter *padapter = (struct _adapter *)pintfhdl->adapter;
 	struct intf_priv *pintfpriv = pintfhdl->pintfpriv;
 	struct io_queue *pio_queue = (struct io_queue *)padapter->pio_queue;
@@ -186,7 +185,7 @@ void r8712_usb_write_mem(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *wmem)
 	usb_fill_bulk_urb(piorw_urb, pusbd, pipe,
 			  wmem, cnt, usb_write_mem_complete,
 			  pio_queue);
-	status = usb_submit_urb(piorw_urb, GFP_ATOMIC);
+	usb_submit_urb(piorw_urb, GFP_ATOMIC);
 	_down_sema(&pintfpriv->io_retevt);
 }
 
@@ -267,7 +266,7 @@ u32 r8712_usb_read_port(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *rmem)
 	if (adapter->bDriverStopped || adapter->bSurpriseRemoved ||
 	    adapter->pwrctrlpriv.pnp_bstop_trx)
 		return _FAIL;
-	if ((precvbuf->reuse == false) || (precvbuf->pskb == NULL)) {
+	if (!precvbuf->reuse == false || !precvbuf->pskb) {
 		precvbuf->pskb = skb_dequeue(&precvpriv->free_recv_skb_queue);
 		if (NULL != precvbuf->pskb)
 			precvbuf->reuse = true;
@@ -275,10 +274,10 @@ u32 r8712_usb_read_port(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *rmem)
 	if (precvbuf != NULL) {
 		r8712_init_recvbuf(adapter, precvbuf);
 		/* re-assign for linux based on skb */
-		if ((precvbuf->reuse == false) || (precvbuf->pskb == NULL)) {
+		if (!precvbuf->reuse || !precvbuf->pskb) {
 			precvbuf->pskb = netdev_alloc_skb(adapter->pnetdev,
 					 MAX_RECVBUF_SZ + RECVBUFF_ALIGN_SZ);
-			if (precvbuf->pskb == NULL)
+			if (!precvbuf->pskb)
 				return _FAIL;
 			tmpaddr = (addr_t)precvbuf->pskb->data;
 			alignment = tmpaddr & (RECVBUFF_ALIGN_SZ-1);
diff --git a/drivers/staging/rtl8712/xmit_linux.c b/drivers/staging/rtl8712/xmit_linux.c
index 0ac9130faf6c..039b598152bc 100644
--- a/drivers/staging/rtl8712/xmit_linux.c
+++ b/drivers/staging/rtl8712/xmit_linux.c
@@ -79,7 +79,6 @@ sint r8712_endofpktfile(struct pkt_file *pfile)
 
 void r8712_set_qos(struct pkt_file *ppktfile, struct pkt_attrib *pattrib)
 {
-	int i;
 	struct ethhdr etherhdr;
 	struct iphdr ip_hdr;
 	u16 UserPriority = 0;
@@ -89,8 +88,7 @@ void r8712_set_qos(struct pkt_file *ppktfile, struct pkt_attrib *pattrib)
 
 	/* get UserPriority from IP hdr*/
 	if (pattrib->ether_type == 0x0800) {
-		i = _r8712_pktfile_read(ppktfile, (u8 *)&ip_hdr,
-					sizeof(ip_hdr));
+		_r8712_pktfile_read(ppktfile, (u8 *)&ip_hdr, sizeof(ip_hdr));
 		/*UserPriority = (ntohs(ip_hdr.tos) >> 5) & 0x3 ;*/
 		UserPriority = ip_hdr.tos >> 5;
 	} else {
diff --git a/drivers/staging/rtl8723au/Makefile b/drivers/staging/rtl8723au/Makefile
index a9aae2163639..3e8989018a88 100644
--- a/drivers/staging/rtl8723au/Makefile
+++ b/drivers/staging/rtl8723au/Makefile
@@ -2,7 +2,6 @@ r8723au-y :=				\
 		core/rtw_cmd.o		\
 		core/rtw_efuse.o	\
 		core/rtw_ieee80211.o	\
-		core/rtw_led.o		\
 		core/rtw_mlme.o		\
 		core/rtw_mlme_ext.o	\
 		core/rtw_pwrctrl.o	\
@@ -33,8 +32,6 @@ r8723au-y :=				\
 		hal/rtl8723a_rf6052.o	\
 		hal/rtl8723a_rxdesc.o	\
 		hal/rtl8723a_sreset.o	\
-		hal/rtl8723a_xmit.o	\
-		hal/rtl8723au_led.o	\
 		hal/rtl8723au_recv.o	\
 		hal/rtl8723au_xmit.o	\
 		hal/usb_halinit.o	\
diff --git a/drivers/staging/rtl8723au/core/rtw_ap.c b/drivers/staging/rtl8723au/core/rtw_ap.c
index 6b4092f05da5..e394d12c36b0 100644
--- a/drivers/staging/rtl8723au/core/rtw_ap.c
+++ b/drivers/staging/rtl8723au/core/rtw_ap.c
@@ -231,12 +231,10 @@ void	expire_timeout_chk23a(struct rtw_adapter *padapter)
 			psta->expire_to--;
 		}
 
-		if (psta->expire_to <= 0)
-		{
+		if (psta->expire_to <= 0) {
 			struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 
-			if (padapter->registrypriv.wifi_spec == 1)
-			{
+			if (padapter->registrypriv.wifi_spec == 1) {
 				psta->expire_to = pstapriv->expire_to;
 				continue;
 			}
@@ -308,15 +306,12 @@ void	expire_timeout_chk23a(struct rtw_adapter *padapter)
 			ret = issue_nulldata23a(padapter, psta->hwaddr, 0, 3, 50);
 
 		psta->keep_alive_trycnt++;
-		if (ret == _SUCCESS)
-		{
+		if (ret == _SUCCESS) {
 			DBG_8723A("asoc check, sta(" MAC_FMT ") is alive\n", MAC_ARG(psta->hwaddr));
 			psta->expire_to = pstapriv->expire_to;
 			psta->keep_alive_trycnt = 0;
 			continue;
-		}
-		else if (psta->keep_alive_trycnt <= 3)
-		{
+		} else if (psta->keep_alive_trycnt <= 3) {
 			DBG_8723A("ack check for asoc expire, keep_alive_trycnt =%d\n", psta->keep_alive_trycnt);
 			psta->expire_to = 1;
 			continue;
@@ -363,8 +358,7 @@ void add_RATid23a(struct rtw_adapter *padapter, struct sta_info *psta, u8 rssi_l
 		return;
 
 	/* b/g mode ra_bitmap */
-	for (i = 0; i < sizeof(psta->bssrateset); i++)
-	{
+	for (i = 0; i < sizeof(psta->bssrateset); i++) {
 		if (psta->bssrateset[i])
 			tx_ra_bitmap |= rtw_get_bit_value_from_ieee_value23a(psta->bssrateset[i]&0x7f);
 	}
@@ -406,8 +400,7 @@ void add_RATid23a(struct rtw_adapter *padapter, struct sta_info *psta, u8 rssi_l
 	raid = networktype_to_raid23a(sta_band);
 	init_rate = get_highest_rate_idx23a(tx_ra_bitmap&0x0fffffff)&0x3f;
 
-	if (psta->aid < NUM_STA)
-	{
+	if (psta->aid < NUM_STA) {
 		u8 arg = 0;
 
 		arg = psta->mac_id&0x1f;
@@ -436,11 +429,8 @@ void add_RATid23a(struct rtw_adapter *padapter, struct sta_info *psta, u8 rssi_l
 		psta->raid = raid;
 		psta->init_rate = init_rate;
 
-	}
-	else
-	{
+	} else
 		DBG_8723A("station aid %d exceed the max number\n", psta->aid);
-	}
 }
 
 static void update_bmc_sta(struct rtw_adapter *padapter)
@@ -453,8 +443,7 @@ static void update_bmc_sta(struct rtw_adapter *padapter)
 	struct wlan_bssid_ex *pcur_network = &pmlmepriv->cur_network.network;
 	struct sta_info *psta = rtw_get_bcmc_stainfo23a(padapter);
 
-	if (psta)
-	{
+	if (psta) {
 		psta->aid = 0;/* default set to 0 */
 		psta->mac_id = psta->aid + 1;
 
@@ -474,8 +463,7 @@ static void update_bmc_sta(struct rtw_adapter *padapter)
 		psta->bssratelen = supportRateNum;
 
 		/* b/g mode ra_bitmap */
-		for (i = 0; i < supportRateNum; i++)
-		{
+		for (i = 0; i < supportRateNum; i++) {
 			if (psta->bssrateset[i])
 				tx_ra_bitmap |= rtw_get_bit_value_from_ieee_value23a(psta->bssrateset[i]&0x7f);
 		}
@@ -522,11 +510,8 @@ static void update_bmc_sta(struct rtw_adapter *padapter)
 		psta->state = _FW_LINKED;
 		spin_unlock_bh(&psta->lock);
 
-	}
-	else
-	{
+	} else
 		DBG_8723A("add_RATid23a_bmc_sta error!\n");
-	}
 }
 
 /* notes: */
@@ -561,8 +546,7 @@ void update_sta_info23a_apmode23a(struct rtw_adapter *padapter, struct sta_info
 	/* ERP */
 	VCS_update23a(padapter, psta);
 	/* HT related cap */
-	if (phtpriv_sta->ht_option)
-	{
+	if (phtpriv_sta->ht_option) {
 		/* check if sta supports rx ampdu */
 		phtpriv_sta->ampdu_enable = phtpriv_ap->ampdu_enable;
 
@@ -580,9 +564,7 @@ void update_sta_info23a_apmode23a(struct rtw_adapter *padapter, struct sta_info
 
 		psta->qos_option = true;
 
-	}
-	else
-	{
+	} else {
 		phtpriv_sta->ampdu_enable = false;
 
 		phtpriv_sta->sgi = false;
@@ -654,7 +636,7 @@ static void start_bss_network(struct rtw_adapter *padapter, u8 *pbuf)
 
 	bcn_interval = (u16)pnetwork->beacon_interval;
 	cur_channel = pnetwork->DSConfig;
-	cur_bwmode = HT_CHANNEL_WIDTH_20;;
+	cur_bwmode = HT_CHANNEL_WIDTH_20;
 	cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 
 	/* check if there is wps ie, */
@@ -1122,7 +1104,6 @@ int rtw_acl_remove_sta23a(struct rtw_adapter *padapter, u8 *addr)
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
 	struct rtw_queue *pacl_node_q = &pacl_list->acl_node_q;
-	int ret = 0;
 
 	DBG_8723A("%s(acl_num =%d) = %pM\n", __func__, pacl_list->num, addr);
 
@@ -1148,7 +1129,7 @@ int rtw_acl_remove_sta23a(struct rtw_adapter *padapter, u8 *addr)
 
 	DBG_8723A("%s, acl_num =%d\n", __func__, pacl_list->num);
 
-	return ret;
+	return 0;
 }
 
 static void update_bcn_fixed_ie(struct rtw_adapter *padapter)
@@ -1217,8 +1198,6 @@ static void update_bcn_wmm_ie(struct rtw_adapter *padapter)
 static void update_bcn_wps_ie(struct rtw_adapter *padapter)
 {
 	DBG_8723A("%s\n", __func__);
-
-	return;
 }
 
 static void update_bcn_p2p_ie(struct rtw_adapter *padapter)
@@ -1261,8 +1240,7 @@ void update_beacon23a(struct rtw_adapter *padapter, u8 ie_id, u8 *oui, u8 tx)
 
 	spin_lock_bh(&pmlmepriv->bcn_update_lock);
 
-	switch (ie_id)
-	{
+	switch (ie_id) {
 	case 0xFF:
 		/* 8: TimeStamp, 2: Beacon Interval 2:Capability */
 		update_bcn_fixed_ie(padapter);
@@ -1389,8 +1367,7 @@ static int rtw_ht_operation_update(struct rtw_adapter *padapter)
 void associated_clients_update23a(struct rtw_adapter *padapter, u8 updated)
 {
 	/* update associated stations cap. */
-	if (updated == true)
-	{
+	if (updated == true) {
 		struct list_head *phead, *plist, *ptmp;
 		struct sta_info *psta;
 		struct sta_priv *pstapriv = &padapter->stapriv;
@@ -1416,34 +1393,27 @@ void bss_cap_update_on_sta_join23a(struct rtw_adapter *padapter, struct sta_info
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 
-	if (!(psta->flags & WLAN_STA_SHORT_PREAMBLE))
-	{
-		if (!psta->no_short_preamble_set)
-		{
+	if (!(psta->flags & WLAN_STA_SHORT_PREAMBLE)) {
+		if (!psta->no_short_preamble_set) {
 			psta->no_short_preamble_set = 1;
 
 			pmlmepriv->num_sta_no_short_preamble++;
 
 			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
-				(pmlmepriv->num_sta_no_short_preamble == 1))
-			{
+				(pmlmepriv->num_sta_no_short_preamble == 1)) {
 				beacon_updated = true;
 				update_beacon23a(padapter, 0xFF, NULL, true);
 			}
 
 		}
-	}
-	else
-	{
-		if (psta->no_short_preamble_set)
-		{
+	} else {
+		if (psta->no_short_preamble_set) {
 			psta->no_short_preamble_set = 0;
 
 			pmlmepriv->num_sta_no_short_preamble--;
 
 			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
-				(pmlmepriv->num_sta_no_short_preamble == 0))
-			{
+				(pmlmepriv->num_sta_no_short_preamble == 0)) {
 				beacon_updated = true;
 				update_beacon23a(padapter, 0xFF, NULL, true);
 			}
@@ -1451,32 +1421,25 @@ void bss_cap_update_on_sta_join23a(struct rtw_adapter *padapter, struct sta_info
 		}
 	}
 
-	if (psta->flags & WLAN_STA_NONERP)
-	{
-		if (!psta->nonerp_set)
-		{
+	if (psta->flags & WLAN_STA_NONERP) {
+		if (!psta->nonerp_set) {
 			psta->nonerp_set = 1;
 
 			pmlmepriv->num_sta_non_erp++;
 
-			if (pmlmepriv->num_sta_non_erp == 1)
-			{
+			if (pmlmepriv->num_sta_non_erp == 1) {
 				beacon_updated = true;
 				update_beacon23a(padapter, WLAN_EID_ERP_INFO, NULL, true);
 			}
 		}
 
-	}
-	else
-	{
-		if (psta->nonerp_set)
-		{
+	} else {
+		if (psta->nonerp_set) {
 			psta->nonerp_set = 0;
 
 			pmlmepriv->num_sta_non_erp--;
 
-			if (pmlmepriv->num_sta_non_erp == 0)
-			{
+			if (pmlmepriv->num_sta_non_erp == 0) {
 				beacon_updated = true;
 				update_beacon23a(padapter, WLAN_EID_ERP_INFO, NULL, true);
 			}
@@ -1484,42 +1447,34 @@ void bss_cap_update_on_sta_join23a(struct rtw_adapter *padapter, struct sta_info
 
 	}
 
-	if (!(psta->capability & WLAN_CAPABILITY_SHORT_SLOT_TIME))
-	{
-		if (!psta->no_short_slot_time_set)
-		{
+	if (!(psta->capability & WLAN_CAPABILITY_SHORT_SLOT_TIME)) {
+		if (!psta->no_short_slot_time_set) {
 			psta->no_short_slot_time_set = 1;
 
 			pmlmepriv->num_sta_no_short_slot_time++;
 
 			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
-				 (pmlmepriv->num_sta_no_short_slot_time == 1))
-			{
+				 (pmlmepriv->num_sta_no_short_slot_time == 1)) {
 				beacon_updated = true;
 				update_beacon23a(padapter, 0xFF, NULL, true);
 			}
 
 		}
-	}
-	else
-	{
-		if (psta->no_short_slot_time_set)
-		{
+	} else {
+		if (psta->no_short_slot_time_set) {
 			psta->no_short_slot_time_set = 0;
 
 			pmlmepriv->num_sta_no_short_slot_time--;
 
 			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
-				 (pmlmepriv->num_sta_no_short_slot_time == 0))
-			{
+				 (pmlmepriv->num_sta_no_short_slot_time == 0)) {
 				beacon_updated = true;
 				update_beacon23a(padapter, 0xFF, NULL, true);
 			}
 		}
 	}
 
-	if (psta->flags & WLAN_STA_HT)
-	{
+	if (psta->flags & WLAN_STA_HT) {
 		u16 ht_capab = le16_to_cpu(psta->htpriv.ht_cap.cap_info);
 
 		DBG_8723A("HT: STA " MAC_FMT " HT Capabilities "
@@ -1552,9 +1507,7 @@ void bss_cap_update_on_sta_join23a(struct rtw_adapter *padapter, struct sta_info
 				   pmlmepriv->num_sta_ht_20mhz);
 		}
 
-	}
-	else
-	{
+	} else {
 		if (!psta->no_ht_set) {
 			psta->no_ht_set = 1;
 			pmlmepriv->num_sta_no_ht++;
@@ -1567,8 +1520,7 @@ void bss_cap_update_on_sta_join23a(struct rtw_adapter *padapter, struct sta_info
 		}
 	}
 
-	if (rtw_ht_operation_update(padapter) > 0)
-	{
+	if (rtw_ht_operation_update(padapter) > 0) {
 		update_beacon23a(padapter, WLAN_EID_HT_CAPABILITY, NULL, false);
 		update_beacon23a(padapter, WLAN_EID_HT_OPERATION, NULL, true);
 	}
@@ -1592,8 +1544,7 @@ u8 bss_cap_update_on_sta_leave23a(struct rtw_adapter *padapter, struct sta_info
 		psta->no_short_preamble_set = 0;
 		pmlmepriv->num_sta_no_short_preamble--;
 		if (pmlmeext->cur_wireless_mode > WIRELESS_11B
-		    && pmlmepriv->num_sta_no_short_preamble == 0)
-		{
+		    && pmlmepriv->num_sta_no_short_preamble == 0) {
 			beacon_updated = true;
 			update_beacon23a(padapter, 0xFF, NULL, true);
 		}
@@ -1602,8 +1553,7 @@ u8 bss_cap_update_on_sta_leave23a(struct rtw_adapter *padapter, struct sta_info
 	if (psta->nonerp_set) {
 		psta->nonerp_set = 0;
 		pmlmepriv->num_sta_non_erp--;
-		if (pmlmepriv->num_sta_non_erp == 0)
-		{
+		if (pmlmepriv->num_sta_non_erp == 0) {
 			beacon_updated = true;
 			update_beacon23a(padapter, WLAN_EID_ERP_INFO,
 					 NULL, true);
@@ -1614,8 +1564,7 @@ u8 bss_cap_update_on_sta_leave23a(struct rtw_adapter *padapter, struct sta_info
 		psta->no_short_slot_time_set = 0;
 		pmlmepriv->num_sta_no_short_slot_time--;
 		if (pmlmeext->cur_wireless_mode > WIRELESS_11B
-		    && pmlmepriv->num_sta_no_short_slot_time == 0)
-		{
+		    && pmlmepriv->num_sta_no_short_slot_time == 0) {
 			beacon_updated = true;
 			update_beacon23a(padapter, 0xFF, NULL, true);
 		}
@@ -1636,8 +1585,7 @@ u8 bss_cap_update_on_sta_leave23a(struct rtw_adapter *padapter, struct sta_info
 		pmlmepriv->num_sta_ht_20mhz--;
 	}
 
-	if (rtw_ht_operation_update(padapter) > 0)
-	{
+	if (rtw_ht_operation_update(padapter) > 0) {
 		update_beacon23a(padapter, WLAN_EID_HT_CAPABILITY, NULL, false);
 		update_beacon23a(padapter, WLAN_EID_HT_OPERATION, NULL, true);
 	}
@@ -1657,8 +1605,7 @@ u8 ap_free_sta23a(struct rtw_adapter *padapter, struct sta_info *psta, bool acti
 	if (!psta)
 		return beacon_updated;
 
-	if (active == true)
-	{
+	if (active) {
 		/* tear down Rx AMPDU */
 		send_delba23a(padapter, 0, psta->hwaddr);/*  recipient */
 
@@ -1698,7 +1645,6 @@ u8 ap_free_sta23a(struct rtw_adapter *padapter, struct sta_info *psta, bool acti
 int rtw_ap_inform_ch_switch23a (struct rtw_adapter *padapter, u8 new_ch, u8 ch_offset)
 {
 	struct list_head *phead, *plist;
-	int ret = 0;
 	struct sta_info *psta = NULL;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
@@ -1706,7 +1652,7 @@ int rtw_ap_inform_ch_switch23a (struct rtw_adapter *padapter, u8 new_ch, u8 ch_o
 	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 
 	if ((pmlmeinfo->state&0x03) != MSR_AP)
-		return ret;
+		return 0;
 
 	DBG_8723A("%s(%s): with ch:%u, offset:%u\n", __func__,
 		  padapter->pnetdev->name, new_ch, ch_offset);
@@ -1724,13 +1670,12 @@ int rtw_ap_inform_ch_switch23a (struct rtw_adapter *padapter, u8 new_ch, u8 ch_o
 
 	issue_action_spct_ch_switch23a (padapter, bc_addr, new_ch, ch_offset);
 
-	return ret;
+	return 0;
 }
 
 int rtw_sta_flush23a(struct rtw_adapter *padapter)
 {
 	struct list_head *phead, *plist, *ptmp;
-	int ret = 0;
 	struct sta_info *psta;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
@@ -1743,7 +1688,7 @@ int rtw_sta_flush23a(struct rtw_adapter *padapter)
 	DBG_8723A("%s(%s)\n", __func__, padapter->pnetdev->name);
 
 	if ((pmlmeinfo->state&0x03) != MSR_AP)
-		return ret;
+		return 0;
 
 	spin_lock_bh(&pstapriv->asoc_list_lock);
 	phead = &pstapriv->asoc_list;
@@ -1769,7 +1714,7 @@ int rtw_sta_flush23a(struct rtw_adapter *padapter)
 
 	associated_clients_update23a(padapter, true);
 
-	return ret;
+	return 0;
 }
 
 /* called > TSR LEVEL for USB or SDIO Interface*/
@@ -1788,13 +1733,10 @@ void sta_info_update23a(struct rtw_adapter *padapter, struct sta_info *psta)
 		psta->qos_option = 0;
 
 	/* update 802.11n ht cap. */
-	if (WLAN_STA_HT&flags)
-	{
+	if (WLAN_STA_HT&flags) {
 		psta->htpriv.ht_option = true;
 		psta->qos_option = 1;
-	}
-	else
-	{
+	} else {
 		psta->htpriv.ht_option = false;
 	}
 
@@ -1807,8 +1749,7 @@ void sta_info_update23a(struct rtw_adapter *padapter, struct sta_info *psta)
 /* called >= TSR LEVEL for USB or SDIO Interface*/
 void ap_sta_info_defer_update23a(struct rtw_adapter *padapter, struct sta_info *psta)
 {
-	if (psta->state & _FW_LINKED)
-	{
+	if (psta->state & _FW_LINKED) {
 		/* add ratid */
 		add_RATid23a(padapter, psta, 0);/* DM_RATR_STA_INIT */
 	}
@@ -1819,7 +1760,7 @@ void rtw_ap_restore_network(struct rtw_adapter *padapter)
 {
 	struct mlme_priv *mlmepriv = &padapter->mlmepriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct sta_priv * pstapriv = &padapter->stapriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct sta_info *psta;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct list_head *phead, *plist, *ptmp;
diff --git a/drivers/staging/rtl8723au/core/rtw_cmd.c b/drivers/staging/rtl8723au/core/rtw_cmd.c
index 4eaa50297b95..60e0ded8ae02 100644
--- a/drivers/staging/rtl8723au/core/rtw_cmd.c
+++ b/drivers/staging/rtl8723au/core/rtw_cmd.c
@@ -36,25 +36,25 @@ static struct cmd_hdl wlancmds[] = {
 	GEN_MLME_EXT_HANDLER(0, NULL)
 	GEN_MLME_EXT_HANDLER(0, NULL)
 	GEN_MLME_EXT_HANDLER(0, NULL)
-	GEN_MLME_EXT_HANDLER(sizeof (struct wlan_bssid_ex), join_cmd_hdl23a) /*14*/
-	GEN_MLME_EXT_HANDLER(sizeof (struct disconnect_parm), disconnect_hdl23a)
-	GEN_MLME_EXT_HANDLER(sizeof (struct wlan_bssid_ex), createbss_hdl23a)
-	GEN_MLME_EXT_HANDLER(sizeof (struct setopmode_parm), setopmode_hdl23a)
-	GEN_MLME_EXT_HANDLER(sizeof (struct sitesurvey_parm), sitesurvey_cmd_hdl23a) /*18*/
-	GEN_MLME_EXT_HANDLER(sizeof (struct setauth_parm), setauth_hdl23a)
-	GEN_MLME_EXT_HANDLER(sizeof (struct setkey_parm), setkey_hdl23a) /*20*/
-	GEN_MLME_EXT_HANDLER(sizeof (struct set_stakey_parm), set_stakey_hdl23a)
-	GEN_MLME_EXT_HANDLER(sizeof (struct set_assocsta_parm), NULL)
-	GEN_MLME_EXT_HANDLER(sizeof (struct del_assocsta_parm), NULL)
-	GEN_MLME_EXT_HANDLER(sizeof (struct setstapwrstate_parm), NULL)
-	GEN_MLME_EXT_HANDLER(sizeof (struct setbasicrate_parm), NULL)
-	GEN_MLME_EXT_HANDLER(sizeof (struct getbasicrate_parm), NULL)
-	GEN_MLME_EXT_HANDLER(sizeof (struct setdatarate_parm), NULL)
-	GEN_MLME_EXT_HANDLER(sizeof (struct getdatarate_parm), NULL)
-	GEN_MLME_EXT_HANDLER(sizeof (struct setphyinfo_parm), NULL)
-	GEN_MLME_EXT_HANDLER(sizeof (struct getphyinfo_parm), NULL)  /*30*/
-	GEN_MLME_EXT_HANDLER(sizeof (struct setphy_parm), NULL)
-	GEN_MLME_EXT_HANDLER(sizeof (struct getphy_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct wlan_bssid_ex), join_cmd_hdl23a) /*14*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct disconnect_parm), disconnect_hdl23a)
+	GEN_MLME_EXT_HANDLER(sizeof(struct wlan_bssid_ex), createbss_hdl23a)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setopmode_parm), setopmode_hdl23a)
+	GEN_MLME_EXT_HANDLER(sizeof(struct sitesurvey_parm), sitesurvey_cmd_hdl23a) /*18*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct setauth_parm), setauth_hdl23a)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setkey_parm), setkey_hdl23a) /*20*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct set_stakey_parm), set_stakey_hdl23a)
+	GEN_MLME_EXT_HANDLER(sizeof(struct set_assocsta_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct del_assocsta_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setstapwrstate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setbasicrate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct getbasicrate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setdatarate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct getdatarate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setphyinfo_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct getphyinfo_parm), NULL)  /*30*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct setphy_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct getphy_parm), NULL)
 	GEN_MLME_EXT_HANDLER(0, NULL)
 	GEN_MLME_EXT_HANDLER(0, NULL)
 	GEN_MLME_EXT_HANDLER(0, NULL)
@@ -359,6 +359,7 @@ int rtw_sitesurvey_cmd23a(struct rtw_adapter *padapter,
 	/* prepare ssid list */
 	if (ssid) {
 		int i;
+
 		for (i = 0; i < ssid_num && i < RTW_SSID_SCAN_AMOUNT; i++) {
 			if (ssid[i].ssid_len) {
 				memcpy(&psurveyPara->ssid[i], &ssid[i],
@@ -371,6 +372,7 @@ int rtw_sitesurvey_cmd23a(struct rtw_adapter *padapter,
 	/* prepare channel list */
 	if (ch) {
 		int i;
+
 		for (i = 0; i < ch_num && i < RTW_CHANNEL_SCAN_AMOUNT; i++) {
 			if (ch[i].hw_value &&
 			    !(ch[i].flags & IEEE80211_CHAN_DISABLED)) {
@@ -389,8 +391,6 @@ int rtw_sitesurvey_cmd23a(struct rtw_adapter *padapter,
 		mod_timer(&pmlmepriv->scan_to_timer, jiffies +
 			  msecs_to_jiffies(SCANNING_TIMEOUT));
 
-		rtw_led_control(padapter, LED_CTL_SITE_SURVEY);
-
 		pmlmepriv->scan_interval = SCAN_INTERVAL;/*  30*2 sec = 60sec */
 	} else
 		_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
@@ -415,8 +415,6 @@ int rtw_createbss_cmd23a(struct rtw_adapter  *padapter)
 
 	pdev_network = &padapter->registrypriv.dev_network;
 
-	rtw_led_control(padapter, LED_CTL_START_TO_LINK);
-
 	if (pmlmepriv->assoc_ssid.ssid_len == 0) {
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
 			 (" createbss for Any SSid:%s\n",
@@ -465,8 +463,6 @@ int rtw_joinbss_cmd23a(struct rtw_adapter *padapter,
 
 	ifmode = pnetwork->network.ifmode;
 
-	rtw_led_control(padapter, LED_CTL_START_TO_LINK);
-
 	if (pmlmepriv->assoc_ssid.ssid_len == 0) {
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
 			 ("+Join cmd: Any SSid\n"));
@@ -599,7 +595,7 @@ exit:
 	return res;
 }
 
-int rtw_disassoc_cmd23a(struct rtw_adapter*padapter, u32 deauth_timeout_ms,
+int rtw_disassoc_cmd23a(struct rtw_adapter *padapter, u32 deauth_timeout_ms,
 			bool enqueue)
 {
 	struct cmd_obj *cmdobj = NULL;
@@ -719,6 +715,7 @@ int rtw_setstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 unicast_key)
 		memcpy(&psetstakey_para->key, &sta->dot118021x_UncstKey, 16);
 	} else {
 		int idx = psecuritypriv->dot118021XGrpKeyid;
+
 		memcpy(&psetstakey_para->key,
 		       &psecuritypriv->dot118021XGrpKey[idx].skey, 16);
 	}
@@ -786,7 +783,7 @@ exit:
 	return res;
 }
 
-int rtw_addbareq_cmd23a(struct rtw_adapter*padapter, u8 tid, u8 *addr)
+int rtw_addbareq_cmd23a(struct rtw_adapter *padapter, u8 tid, u8 *addr)
 {
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 	struct cmd_obj *ph2c;
@@ -822,7 +819,7 @@ exit:
 	return res;
 }
 
-int rtw_dynamic_chk_wk_cmd23a(struct rtw_adapter*padapter)
+int rtw_dynamic_chk_wk_cmd23a(struct rtw_adapter *padapter)
 {
 	struct cmd_obj *ph2c;
 	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
@@ -859,7 +856,7 @@ exit:
  * This is only ever called from on_action_spct23a_ch_switch () which isn't
  * called from anywhere itself
  */
-int rtw_set_ch_cmd23a(struct rtw_adapter*padapter, u8 ch, u8 bw, u8 ch_offset,
+int rtw_set_ch_cmd23a(struct rtw_adapter *padapter, u8 ch, u8 bw, u8 ch_offset,
 		      u8 enqueue)
 {
 	struct cmd_obj *pcmdobj;
@@ -919,34 +916,34 @@ static void traffic_status_watchdog(struct rtw_adapter *padapter)
 	u8 bHigherBusyTxTraffic = false;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	int BusyThreshold = 100;
+	struct rt_link_detect *ldi = &pmlmepriv->LinkDetectInfo;
+
 	/*  */
 	/*  Determine if our traffic is busy now */
 	/*  */
 	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 		if (rtl8723a_BT_coexist(padapter))
 			BusyThreshold = 50;
-		else if (pmlmepriv->LinkDetectInfo.bBusyTraffic)
+		else if (ldi->bBusyTraffic)
 			BusyThreshold = 75;
 		/*  if we raise bBusyTraffic in last watchdog, using
 		    lower threshold. */
-		if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > BusyThreshold ||
-		    pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > BusyThreshold) {
+		if (ldi->NumRxOkInPeriod > BusyThreshold ||
+		    ldi->NumTxOkInPeriod > BusyThreshold) {
 			bBusyTraffic = true;
 
-			if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod >
-			    pmlmepriv->LinkDetectInfo.NumTxOkInPeriod)
+			if (ldi->NumRxOkInPeriod > ldi->NumTxOkInPeriod)
 				bRxBusyTraffic = true;
 			else
 				bTxBusyTraffic = true;
 		}
 
 		/*  Higher Tx/Rx data. */
-		if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > 4000 ||
-		    pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > 4000) {
+		if (ldi->NumRxOkInPeriod > 4000 ||
+		    ldi->NumTxOkInPeriod > 4000) {
 			bHigherBusyTraffic = true;
 
-			if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod >
-			    pmlmepriv->LinkDetectInfo.NumTxOkInPeriod)
+			if (ldi->NumRxOkInPeriod > ldi->NumTxOkInPeriod)
 				bHigherBusyRxTraffic = true;
 			else
 				bHigherBusyTxTraffic = true;
@@ -955,9 +952,9 @@ static void traffic_status_watchdog(struct rtw_adapter *padapter)
 		if (!rtl8723a_BT_coexist(padapter) ||
 		    !rtl8723a_BT_using_antenna_1(padapter)) {
 		/*  check traffic for  powersaving. */
-			if (((pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod +
-			      pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) > 8) ||
-			    pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod >2)
+			if (((ldi->NumRxUnicastOkInPeriod +
+			      ldi->NumTxOkInPeriod) > 8) ||
+			    ldi->NumRxUnicastOkInPeriod > 2)
 				bEnterPS = false;
 			else
 				bEnterPS = true;
@@ -971,15 +968,15 @@ static void traffic_status_watchdog(struct rtw_adapter *padapter)
 	} else
 		LPS_Leave23a(padapter);
 
-	pmlmepriv->LinkDetectInfo.NumRxOkInPeriod = 0;
-	pmlmepriv->LinkDetectInfo.NumTxOkInPeriod = 0;
-	pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod = 0;
-	pmlmepriv->LinkDetectInfo.bBusyTraffic = bBusyTraffic;
-	pmlmepriv->LinkDetectInfo.bTxBusyTraffic = bTxBusyTraffic;
-	pmlmepriv->LinkDetectInfo.bRxBusyTraffic = bRxBusyTraffic;
-	pmlmepriv->LinkDetectInfo.bHigherBusyTraffic = bHigherBusyTraffic;
-	pmlmepriv->LinkDetectInfo.bHigherBusyRxTraffic = bHigherBusyRxTraffic;
-	pmlmepriv->LinkDetectInfo.bHigherBusyTxTraffic = bHigherBusyTxTraffic;
+	ldi->NumRxOkInPeriod = 0;
+	ldi->NumTxOkInPeriod = 0;
+	ldi->NumRxUnicastOkInPeriod = 0;
+	ldi->bBusyTraffic = bBusyTraffic;
+	ldi->bTxBusyTraffic = bTxBusyTraffic;
+	ldi->bRxBusyTraffic = bRxBusyTraffic;
+	ldi->bHigherBusyTraffic = bHigherBusyTraffic;
+	ldi->bHigherBusyRxTraffic = bHigherBusyRxTraffic;
+	ldi->bHigherBusyTxTraffic = bHigherBusyTxTraffic;
 }
 
 static void dynamic_chk_wk_hdl(struct rtw_adapter *padapter, u8 *pbuf, int sz)
@@ -1017,46 +1014,45 @@ static void lps_ctrl_wk_hdl(struct rtw_adapter *padapter, u8 lps_ctrl_type)
 	    check_fwstate(pmlmepriv, WIFI_ADHOC_STATE))
 		return;
 
-	switch (lps_ctrl_type)
-	{
-		case LPS_CTRL_SCAN:
-			rtl8723a_BT_wifiscan_notify(padapter, true);
-			if (!rtl8723a_BT_using_antenna_1(padapter)) {
-				if (check_fwstate(pmlmepriv, _FW_LINKED))
-					LPS_Leave23a(padapter);
+	switch (lps_ctrl_type) {
+	case LPS_CTRL_SCAN:
+		rtl8723a_BT_wifiscan_notify(padapter, true);
+		if (!rtl8723a_BT_using_antenna_1(padapter)) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED))
+				LPS_Leave23a(padapter);
 			}
-			break;
-		case LPS_CTRL_JOINBSS:
+		break;
+	case LPS_CTRL_JOINBSS:
+		LPS_Leave23a(padapter);
+		break;
+	case LPS_CTRL_CONNECT:
+		mstatus = 1;/* connect */
+		/*  Reset LPS Setting */
+		padapter->pwrctrlpriv.LpsIdleCount = 0;
+		rtl8723a_set_FwJoinBssReport_cmd(padapter, 1);
+		rtl8723a_BT_mediastatus_notify(padapter, mstatus);
+		break;
+	case LPS_CTRL_DISCONNECT:
+		mstatus = 0;/* disconnect */
+		rtl8723a_BT_mediastatus_notify(padapter, mstatus);
+		if (!rtl8723a_BT_using_antenna_1(padapter))
 			LPS_Leave23a(padapter);
-			break;
-		case LPS_CTRL_CONNECT:
-			mstatus = 1;/* connect */
-			/*  Reset LPS Setting */
-			padapter->pwrctrlpriv.LpsIdleCount = 0;
-			rtl8723a_set_FwJoinBssReport_cmd(padapter, 1);
-			rtl8723a_BT_mediastatus_notify(padapter, mstatus);
-			break;
-		case LPS_CTRL_DISCONNECT:
-			mstatus = 0;/* disconnect */
-			rtl8723a_BT_mediastatus_notify(padapter, mstatus);
-			if (!rtl8723a_BT_using_antenna_1(padapter))
-				LPS_Leave23a(padapter);
-			rtl8723a_set_FwJoinBssReport_cmd(padapter, 0);
-			break;
-		case LPS_CTRL_SPECIAL_PACKET:
-			pwrpriv->DelayLPSLastTimeStamp = jiffies;
-			rtl8723a_BT_specialpacket_notify(padapter);
-			if (!rtl8723a_BT_using_antenna_1(padapter))
-				LPS_Leave23a(padapter);
-			break;
-		case LPS_CTRL_LEAVE:
-			rtl8723a_BT_lps_leave(padapter);
-			if (!rtl8723a_BT_using_antenna_1(padapter))
-				LPS_Leave23a(padapter);
-			break;
+		rtl8723a_set_FwJoinBssReport_cmd(padapter, 0);
+		break;
+	case LPS_CTRL_SPECIAL_PACKET:
+		pwrpriv->DelayLPSLastTimeStamp = jiffies;
+		rtl8723a_BT_specialpacket_notify(padapter);
+		if (!rtl8723a_BT_using_antenna_1(padapter))
+			LPS_Leave23a(padapter);
+		break;
+	case LPS_CTRL_LEAVE:
+		rtl8723a_BT_lps_leave(padapter);
+		if (!rtl8723a_BT_using_antenna_1(padapter))
+			LPS_Leave23a(padapter);
+		break;
 
-		default:
-			break;
+	default:
+		break;
 	}
 }
 
@@ -1098,7 +1094,7 @@ exit:
 	return res;
 }
 
-int rtw_ps_cmd23a(struct rtw_adapter*padapter)
+int rtw_ps_cmd23a(struct rtw_adapter *padapter)
 {
 	struct cmd_obj *ppscmd;
 	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
@@ -1147,12 +1143,12 @@ static void rtw_chk_hi_queue_hdl(struct rtw_adapter *padapter)
 
 		val = rtl8723a_chk_hi_queue_empty(padapter);
 
-		while (val == false) {
+		while (!val) {
 			msleep(100);
 
 			cnt++;
 
-			if (cnt>10)
+			if (cnt > 10)
 				break;
 
 			val = rtl8723a_chk_hi_queue_empty(padapter);
@@ -1168,7 +1164,7 @@ static void rtw_chk_hi_queue_hdl(struct rtw_adapter *padapter)
 	}
 }
 
-int rtw_chk_hi_queue_cmd23a(struct rtw_adapter*padapter)
+int rtw_chk_hi_queue_cmd23a(struct rtw_adapter *padapter)
 {
 	struct cmd_obj *ph2c;
 	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
@@ -1305,8 +1301,7 @@ int rtw_drvextra_cmd_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 
 	pdrvextra_cmd = (struct drvextra_cmd_parm *)pbuf;
 
-	switch (pdrvextra_cmd->ec_id)
-	{
+	switch (pdrvextra_cmd->ec_id) {
 	case DYNAMIC_CHK_WK_CID:
 		dynamic_chk_wk_hdl(padapter, pdrvextra_cmd->pbuf,
 				   pdrvextra_cmd->type_size);
diff --git a/drivers/staging/rtl8723au/core/rtw_efuse.c b/drivers/staging/rtl8723au/core/rtw_efuse.c
index 9f6ce7d071cd..81960e788f89 100644
--- a/drivers/staging/rtl8723au/core/rtw_efuse.c
+++ b/drivers/staging/rtl8723au/core/rtw_efuse.c
@@ -117,12 +117,7 @@ Efuse_GetCurrentSize23a(struct rtw_adapter *pAdapter, u8 efuseType)
 u8
 Efuse_CalculateWordCnts23a(u8 word_en)
 {
-	u8 word_cnts = 0;
-	if (!(word_en & BIT(0)))	word_cnts++; /*  0 : write enable */
-	if (!(word_en & BIT(1)))	word_cnts++;
-	if (!(word_en & BIT(2)))	word_cnts++;
-	if (!(word_en & BIT(3)))	word_cnts++;
-	return word_cnts;
+	return hweight8((~word_en) & 0xf);
 }
 
 /*  */
@@ -181,7 +176,7 @@ EFUSE_GetEfuseDefinition23a(struct rtw_adapter *pAdapter, u8 efuseType,
 
 	switch (type) {
 	case TYPE_EFUSE_MAX_SECTION:
-		pMax_section = (u8 *) pOut;
+		pMax_section = pOut;
 
 		if (efuseType == EFUSE_WIFI)
 			*pMax_section = EFUSE_MAX_SECTION_8723A;
@@ -190,7 +185,7 @@ EFUSE_GetEfuseDefinition23a(struct rtw_adapter *pAdapter, u8 efuseType,
 		break;
 
 	case TYPE_EFUSE_REAL_CONTENT_LEN:
-		pu2Tmp = (u16 *) pOut;
+		pu2Tmp = pOut;
 
 		if (efuseType == EFUSE_WIFI)
 			*pu2Tmp = EFUSE_REAL_CONTENT_LEN_8723A;
@@ -199,7 +194,7 @@ EFUSE_GetEfuseDefinition23a(struct rtw_adapter *pAdapter, u8 efuseType,
 		break;
 
 	case TYPE_AVAILABLE_EFUSE_BYTES_BANK:
-		pu2Tmp = (u16 *) pOut;
+		pu2Tmp = pOut;
 
 		if (efuseType == EFUSE_WIFI)
 			*pu2Tmp = (EFUSE_REAL_CONTENT_LEN_8723A -
@@ -210,7 +205,7 @@ EFUSE_GetEfuseDefinition23a(struct rtw_adapter *pAdapter, u8 efuseType,
 		break;
 
 	case TYPE_AVAILABLE_EFUSE_BYTES_TOTAL:
-		pu2Tmp = (u16 *) pOut;
+		pu2Tmp = pOut;
 
 		if (efuseType == EFUSE_WIFI)
 			*pu2Tmp = (EFUSE_REAL_CONTENT_LEN_8723A -
@@ -221,7 +216,7 @@ EFUSE_GetEfuseDefinition23a(struct rtw_adapter *pAdapter, u8 efuseType,
 		break;
 
 	case TYPE_EFUSE_MAP_LEN:
-		pu2Tmp = (u16 *) pOut;
+		pu2Tmp = pOut;
 
 		if (efuseType == EFUSE_WIFI)
 			*pu2Tmp = EFUSE_MAP_LEN_8723A;
@@ -230,7 +225,7 @@ EFUSE_GetEfuseDefinition23a(struct rtw_adapter *pAdapter, u8 efuseType,
 		break;
 
 	case TYPE_EFUSE_PROTECT_BYTES_BANK:
-		pu1Tmp = (u8 *) pOut;
+		pu1Tmp = pOut;
 
 		if (efuseType == EFUSE_WIFI)
 			*pu1Tmp = EFUSE_OOB_PROTECT_BYTES;
@@ -239,7 +234,7 @@ EFUSE_GetEfuseDefinition23a(struct rtw_adapter *pAdapter, u8 efuseType,
 		break;
 
 	case TYPE_EFUSE_CONTENT_LEN_BANK:
-		pu2Tmp = (u16 *) pOut;
+		pu2Tmp = pOut;
 
 		if (efuseType == EFUSE_WIFI)
 			*pu2Tmp = EFUSE_REAL_CONTENT_LEN_8723A;
@@ -248,7 +243,7 @@ EFUSE_GetEfuseDefinition23a(struct rtw_adapter *pAdapter, u8 efuseType,
 		break;
 
 	default:
-		pu1Tmp = (u8 *) pOut;
+		pu1Tmp = pOut;
 		*pu1Tmp = 0;
 		break;
 	}
diff --git a/drivers/staging/rtl8723au/core/rtw_ieee80211.c b/drivers/staging/rtl8723au/core/rtw_ieee80211.c
index 6274cb397c92..bbbcfc8257da 100644
--- a/drivers/staging/rtl8723au/core/rtw_ieee80211.c
+++ b/drivers/staging/rtl8723au/core/rtw_ieee80211.c
@@ -69,6 +69,7 @@ int rtw_get_bit_value_from_ieee_value23a(u8 val)
 		{2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108, 0};
 
 	int i = 0;
+
 	while (dot11_rate_table[i] != 0) {
 		if (dot11_rate_table[i] == val)
 			return BIT(i);
@@ -301,8 +302,7 @@ void rtw_set_supported_rate23a(u8 *SupportedRates, uint mode)
 
 	memset(SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
 
-	switch (mode)
-	{
+	switch (mode) {
 	case WIRELESS_11B:
 		memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
 		break;
diff --git a/drivers/staging/rtl8723au/core/rtw_led.c b/drivers/staging/rtl8723au/core/rtw_led.c
deleted file mode 100644
index 989cda29a57e..000000000000
--- a/drivers/staging/rtl8723au/core/rtw_led.c
+++ /dev/null
@@ -1,1893 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2012 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.
- *
- ******************************************************************************/
-
-#include <drv_types.h>
-#include <rtl8723a_led.h>
-
-/*  */
-/*	Description: */
-/*		Callback function of LED BlinkTimer, */
-/*		it just schedules to corresponding BlinkWorkItem/led_blink_hdl23a */
-/*  */
-static void BlinkTimerCallback(unsigned long data)
-{
-	struct led_8723a *pLed = (struct led_8723a *)data;
-	struct rtw_adapter *padapter = pLed->padapter;
-
-	/* DBG_8723A("%s\n", __func__); */
-
-	if ((padapter->bSurpriseRemoved == true) || (padapter->bDriverStopped == true))
-	{
-		/* DBG_8723A("%s bSurpriseRemoved:%d, bDriverStopped:%d\n", __func__, padapter->bSurpriseRemoved, padapter->bDriverStopped); */
-		return;
-	}
-	schedule_work(&pLed->BlinkWorkItem);
-}
-
-/*  */
-/*	Description: */
-/*		Callback function of LED BlinkWorkItem. */
-/*		We dispatch acture LED blink action according to LedStrategy. */
-/*  */
-void BlinkWorkItemCallback23a(struct work_struct *work)
-{
-	struct led_8723a *pLed = container_of(work, struct led_8723a, BlinkWorkItem);
-	BlinkHandler23a(pLed);
-}
-
-/*  */
-/*	Description: */
-/*		Reset status of led_8723a object. */
-/*  */
-void ResetLedStatus23a(struct led_8723a * pLed) {
-
-	pLed->CurrLedState = RTW_LED_OFF; /*  Current LED state. */
-	pLed->bLedOn = false; /*  true if LED is ON, false if LED is OFF. */
-
-	pLed->bLedBlinkInProgress = false; /*  true if it is blinking, false o.w.. */
-	pLed->bLedWPSBlinkInProgress = false;
-
-	pLed->BlinkTimes = 0; /*  Number of times to toggle led state for blinking. */
-	pLed->BlinkingLedState = LED_UNKNOWN; /*  Next state for blinking, either RTW_LED_ON or RTW_LED_OFF are. */
-
-	pLed->bLedNoLinkBlinkInProgress = false;
-	pLed->bLedLinkBlinkInProgress = false;
-	pLed->bLedStartToLinkBlinkInProgress = false;
-	pLed->bLedScanBlinkInProgress = false;
-}
-
- /*  */
-/*	Description: */
-/*		Initialize an led_8723a object. */
-/*  */
-void
-InitLed871x23a(struct rtw_adapter *padapter, struct led_8723a *pLed, enum led_pin_8723a LedPin)
-{
-	pLed->padapter = padapter;
-	pLed->LedPin = LedPin;
-
-	ResetLedStatus23a(pLed);
-
-	setup_timer(&pLed->BlinkTimer, BlinkTimerCallback, (unsigned long)pLed);
-
-	INIT_WORK(&pLed->BlinkWorkItem, BlinkWorkItemCallback23a);
-}
-
-/*  */
-/*	Description: */
-/*		DeInitialize an led_8723a object. */
-/*  */
-void
-DeInitLed871x23a(struct led_8723a *pLed)
-{
-	cancel_work_sync(&pLed->BlinkWorkItem);
-	del_timer_sync(&pLed->BlinkTimer);
-	ResetLedStatus23a(pLed);
-}
-
-/*	Description: */
-/*		Implementation of LED blinking behavior. */
-/*		It toggle off LED and schedule corresponding timer if necessary. */
-
-static void SwLedBlink(struct led_8723a *pLed)
-{
-	struct rtw_adapter *padapter = pLed->padapter;
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	u8 bStopBlinking = false;
-
-	/*  Change LED according to BlinkingLedState specified. */
-	if (pLed->BlinkingLedState == RTW_LED_ON) {
-		SwLedOn23a(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
-	} else {
-		SwLedOff23a(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
-	}
-
-	/*  Determine if we shall change LED state again. */
-	pLed->BlinkTimes--;
-	switch (pLed->CurrLedState) {
-
-	case LED_BLINK_NORMAL:
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		break;
-	case LED_BLINK_StartToBlink:
-		if (check_fwstate(pmlmepriv, _FW_LINKED) &&
-		    check_fwstate(pmlmepriv, WIFI_STATION_STATE))
-			bStopBlinking = true;
-		if (check_fwstate(pmlmepriv, _FW_LINKED) &&
-		    (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
-		    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)))
-			bStopBlinking = true;
-		else if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		break;
-	case LED_BLINK_WPS:
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		break;
-	default:
-		bStopBlinking = true;
-		break;
-	}
-
-	if (bStopBlinking) {
-		if ((check_fwstate(pmlmepriv, _FW_LINKED)) && !pLed->bLedOn)
-			SwLedOn23a(padapter, pLed);
-		else if ((check_fwstate(pmlmepriv, _FW_LINKED)) &&  pLed->bLedOn)
-			SwLedOff23a(padapter, pLed);
-
-		pLed->BlinkTimes = 0;
-		pLed->bLedBlinkInProgress = false;
-	} else {
-		/*  Assign LED state to toggle. */
-		if (pLed->BlinkingLedState == RTW_LED_ON)
-			pLed->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed->BlinkingLedState = RTW_LED_ON;
-
-		/*  Schedule a timer to toggle LED state. */
-		switch (pLed->CurrLedState) {
-		case LED_BLINK_NORMAL:
-			mod_timer(&pLed->BlinkTimer, jiffies +
-				  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
-			break;
-		case LED_BLINK_SLOWLY:
-		case LED_BLINK_StartToBlink:
-			mod_timer(&pLed->BlinkTimer, jiffies +
-				  msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL));
-			break;
-		case LED_BLINK_WPS:
-			mod_timer(&pLed->BlinkTimer, jiffies +
-				  msecs_to_jiffies(LED_BLINK_LONG_INTERVAL));
-			break;
-		default:
-			mod_timer(&pLed->BlinkTimer, jiffies +
-				  msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL));
-			break;
-		}
-	}
-}
-
-static void SwLedBlink1(struct led_8723a *pLed)
-{
-	struct rtw_adapter *padapter = pLed->padapter;
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	unsigned long delay = 0;
-	u8 bStopBlinking = false;
-
-	/*  Change LED according to BlinkingLedState specified. */
-	if (pLed->BlinkingLedState == RTW_LED_ON) {
-		SwLedOn23a(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_,
-			 ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
-	} else {
-		SwLedOff23a(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_,
-			 ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
-	}
-
-	if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-		SwLedOff23a(padapter, pLed);
-		ResetLedStatus23a(pLed);
-		return;
-	}
-	switch (pLed->CurrLedState) {
-	case LED_BLINK_SLOWLY:
-		if (pLed->bLedOn)
-			pLed->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed->BlinkingLedState = RTW_LED_ON;
-		delay = LED_BLINK_NO_LINK_INTERVAL_ALPHA;
-		break;
-	case LED_BLINK_NORMAL:
-		if (pLed->bLedOn)
-			pLed->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed->BlinkingLedState = RTW_LED_ON;
-		delay = LED_BLINK_LINK_INTERVAL_ALPHA;
-		break;
-	case LED_BLINK_SCAN:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		if (bStopBlinking) {
-			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->bLedLinkBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_NORMAL;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				delay = LED_BLINK_LINK_INTERVAL_ALPHA;
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-			} else {
-				pLed->bLedNoLinkBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_SLOWLY;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				delay = LED_BLINK_NO_LINK_INTERVAL_ALPHA;
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-			}
-			pLed->bLedScanBlinkInProgress = false;
-		} else {
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			delay = LED_BLINK_SCAN_INTERVAL_ALPHA;
-		}
-		break;
-	case LED_BLINK_TXRX:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		if (bStopBlinking) {
-			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->bLedLinkBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_NORMAL;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				delay = LED_BLINK_LINK_INTERVAL_ALPHA;
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-			} else {
-				pLed->bLedNoLinkBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_SLOWLY;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				delay = LED_BLINK_NO_LINK_INTERVAL_ALPHA;
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-			}
-			pLed->BlinkTimes = 0;
-			pLed->bLedBlinkInProgress = false;
-		} else {
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			delay = LED_BLINK_FASTER_INTERVAL_ALPHA;
-		}
-		break;
-	case LED_BLINK_WPS:
-		if (pLed->bLedOn)
-			pLed->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed->BlinkingLedState = RTW_LED_ON;
-		delay = LED_BLINK_SCAN_INTERVAL_ALPHA;
-		break;
-	case LED_BLINK_WPS_STOP:	/* WPS success */
-		if (pLed->BlinkingLedState == RTW_LED_ON)
-			bStopBlinking = false;
-		else
-			bStopBlinking = true;
-		if (bStopBlinking) {
-			pLed->bLedLinkBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_NORMAL;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			delay = LED_BLINK_LINK_INTERVAL_ALPHA;
-			RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-
-			pLed->bLedWPSBlinkInProgress = false;
-		} else {
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			delay = LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA;
-		}
-		break;
-	default:
-		break;
-	}
-	if (delay)
-		mod_timer(&pLed->BlinkTimer, jiffies + msecs_to_jiffies(delay));
-}
-
-static void SwLedBlink2(struct led_8723a *pLed)
-{
-	struct rtw_adapter *padapter = pLed->padapter;
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	u8 bStopBlinking = false;
-
-	/*  Change LED according to BlinkingLedState specified. */
-	if (pLed->BlinkingLedState == RTW_LED_ON) {
-		SwLedOn23a(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_,
-			 ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
-	} else {
-		SwLedOff23a(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_,
-			 ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
-	}
-	switch (pLed->CurrLedState) {
-	case LED_BLINK_SCAN:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff23a(padapter, pLed);
-			} else if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->CurrLedState = RTW_LED_ON;
-				pLed->BlinkingLedState = RTW_LED_ON;
-				SwLedOn23a(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_,
-					 ("stop scan blink CurrLedState %d\n",
-					 pLed->CurrLedState));
-			} else {
-				pLed->CurrLedState = RTW_LED_OFF;
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				SwLedOff23a(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_,
-					 ("stop scan blink CurrLedState %d\n",
-					 pLed->CurrLedState));
-			}
-			pLed->bLedScanBlinkInProgress = false;
-		} else {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff23a(padapter, pLed);
-			} else {
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				mod_timer(&pLed->BlinkTimer,
-					  jiffies + msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
-			}
-		}
-		break;
-	case LED_BLINK_TXRX:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff23a(padapter, pLed);
-			} else if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->CurrLedState = RTW_LED_ON;
-				pLed->BlinkingLedState = RTW_LED_ON;
-				SwLedOn23a(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_,
-					 ("stop CurrLedState %d\n", pLed->CurrLedState));
-
-			} else {
-				pLed->CurrLedState = RTW_LED_OFF;
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				SwLedOff23a(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_,
-					 ("stop CurrLedState %d\n", pLed->CurrLedState));
-			}
-			pLed->bLedBlinkInProgress = false;
-		} else {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff23a(padapter, pLed);
-			} else {
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				mod_timer(&pLed->BlinkTimer,
-					  jiffies + msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
-			}
-		}
-		break;
-	default:
-		break;
-	}
-}
-
-static void SwLedBlink3(struct led_8723a *pLed)
-{
-	struct rtw_adapter *padapter = pLed->padapter;
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	u8 bStopBlinking = false;
-
-	/*  Change LED according to BlinkingLedState specified. */
-	if (pLed->BlinkingLedState == RTW_LED_ON)
-	{
-		SwLedOn23a(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
-	}
-	else
-	{
-		if (pLed->CurrLedState != LED_BLINK_WPS_STOP)
-			SwLedOff23a(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
-	}
-
-	switch (pLed->CurrLedState)
-	{
-		case LED_BLINK_SCAN:
-			pLed->BlinkTimes--;
-			if (pLed->BlinkTimes == 0)
-			{
-				bStopBlinking = true;
-			}
-
-			if (bStopBlinking)
-			{
-				if (padapter->pwrctrlpriv.rf_pwrstate != rf_on)
-				{
-					SwLedOff23a(padapter, pLed);
-				}
-				else if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-					pLed->CurrLedState = RTW_LED_ON;
-					pLed->BlinkingLedState = RTW_LED_ON;
-					if (!pLed->bLedOn)
-						SwLedOn23a(padapter, pLed);
-
-					RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-				} else {
-					pLed->CurrLedState = RTW_LED_OFF;
-					pLed->BlinkingLedState = RTW_LED_OFF;
-					if (pLed->bLedOn)
-						SwLedOff23a(padapter, pLed);
-
-					RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-				}
-				pLed->bLedScanBlinkInProgress = false;
-			}
-			else
-			{
-				if (padapter->pwrctrlpriv.rf_pwrstate != rf_on)
-				{
-					SwLedOff23a(padapter, pLed);
-				}
-				else
-				{
-					if (pLed->bLedOn)
-						pLed->BlinkingLedState = RTW_LED_OFF;
-					else
-						pLed->BlinkingLedState = RTW_LED_ON;
-					mod_timer(&pLed->BlinkTimer,
-						  jiffies + msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
-				}
-			}
-			break;
-
-		case LED_BLINK_TXRX:
-			pLed->BlinkTimes--;
-			if (pLed->BlinkTimes == 0)
-			{
-				bStopBlinking = true;
-			}
-			if (bStopBlinking)
-			{
-				if (padapter->pwrctrlpriv.rf_pwrstate != rf_on)
-				{
-					SwLedOff23a(padapter, pLed);
-				} else if (check_fwstate(pmlmepriv,
-							 _FW_LINKED)) {
-					pLed->CurrLedState = RTW_LED_ON;
-					pLed->BlinkingLedState = RTW_LED_ON;
-
-					if (!pLed->bLedOn)
-						SwLedOn23a(padapter, pLed);
-
-					RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-				} else {
-					pLed->CurrLedState = RTW_LED_OFF;
-					pLed->BlinkingLedState = RTW_LED_OFF;
-
-					if (pLed->bLedOn)
-						SwLedOff23a(padapter, pLed);
-
-					RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-				}
-				pLed->bLedBlinkInProgress = false;
-			}
-			else
-			{
-				if (padapter->pwrctrlpriv.rf_pwrstate != rf_on)
-				{
-					SwLedOff23a(padapter, pLed);
-				}
-				else
-				{
-					if (pLed->bLedOn)
-						pLed->BlinkingLedState = RTW_LED_OFF;
-					else
-						pLed->BlinkingLedState = RTW_LED_ON;
-					mod_timer(&pLed->BlinkTimer,
-						  jiffies + msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
-				}
-			}
-			break;
-
-		case LED_BLINK_WPS:
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			mod_timer(&pLed->BlinkTimer, jiffies +
-				  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
-			break;
-
-		case LED_BLINK_WPS_STOP:	/* WPS success */
-			if (pLed->BlinkingLedState == RTW_LED_ON)
-			{
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				mod_timer(&pLed->BlinkTimer, jiffies +
-					  msecs_to_jiffies(LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA));
-				bStopBlinking = false;
-			} else {
-				bStopBlinking = true;
-			}
-
-			if (bStopBlinking)
-			{
-				if (padapter->pwrctrlpriv.rf_pwrstate != rf_on)
-				{
-					SwLedOff23a(padapter, pLed);
-				}
-				else
-				{
-					pLed->CurrLedState = RTW_LED_ON;
-					pLed->BlinkingLedState = RTW_LED_ON;
-					SwLedOn23a(padapter, pLed);
-					RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-				}
-				pLed->bLedWPSBlinkInProgress = false;
-			}
-			break;
-
-		default:
-			break;
-	}
-}
-
-static void SwLedBlink4(struct led_8723a *pLed)
-{
-	struct rtw_adapter *padapter = pLed->padapter;
-	struct led_priv *ledpriv = &padapter->ledpriv;
-	struct led_8723a *pLed1 = &ledpriv->SwLed1;
-	u8 bStopBlinking = false;
-	unsigned long delay = 0;
-
-	/*  Change LED according to BlinkingLedState specified. */
-	if (pLed->BlinkingLedState == RTW_LED_ON)
-	{
-		SwLedOn23a(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
-	} else {
-		SwLedOff23a(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
-	}
-
-	if (!pLed1->bLedWPSBlinkInProgress && pLed1->BlinkingLedState == LED_UNKNOWN)
-	{
-		pLed1->BlinkingLedState = RTW_LED_OFF;
-		pLed1->CurrLedState = RTW_LED_OFF;
-		SwLedOff23a(padapter, pLed1);
-	}
-
-	switch (pLed->CurrLedState)
-	{
-		case LED_BLINK_SLOWLY:
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			delay = LED_BLINK_NO_LINK_INTERVAL_ALPHA;
-			break;
-
-		case LED_BLINK_StartToBlink:
-			if (pLed->bLedOn) {
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				delay = LED_BLINK_SLOWLY_INTERVAL;
-			} else {
-				pLed->BlinkingLedState = RTW_LED_ON;
-				delay = LED_BLINK_NORMAL_INTERVAL;
-			}
-			break;
-
-		case LED_BLINK_SCAN:
-			pLed->BlinkTimes--;
-			if (pLed->BlinkTimes == 0) {
-				bStopBlinking = false;
-			}
-
-			if (bStopBlinking) {
-				if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-					SwLedOff23a(padapter, pLed);
-				} else {
-					pLed->bLedNoLinkBlinkInProgress = false;
-					pLed->CurrLedState = LED_BLINK_SLOWLY;
-					if (pLed->bLedOn)
-						pLed->BlinkingLedState = RTW_LED_OFF;
-					else
-						pLed->BlinkingLedState = RTW_LED_ON;
-					delay = LED_BLINK_NO_LINK_INTERVAL_ALPHA;
-				}
-				pLed->bLedScanBlinkInProgress = false;
-			} else {
-				if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-					SwLedOff23a(padapter, pLed);
-				} else {
-					if (pLed->bLedOn)
-						pLed->BlinkingLedState = RTW_LED_OFF;
-					else
-						pLed->BlinkingLedState = RTW_LED_ON;
-					delay = LED_BLINK_SCAN_INTERVAL_ALPHA;
-				}
-			}
-			break;
-
-		case LED_BLINK_TXRX:
-			pLed->BlinkTimes--;
-			if (pLed->BlinkTimes == 0) {
-				bStopBlinking = true;
-			}
-			if (bStopBlinking) {
-				if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-					SwLedOff23a(padapter, pLed);
-				} else {
-					pLed->bLedNoLinkBlinkInProgress = true;
-					pLed->CurrLedState = LED_BLINK_SLOWLY;
-					if (pLed->bLedOn)
-						pLed->BlinkingLedState = RTW_LED_OFF;
-					else
-						pLed->BlinkingLedState = RTW_LED_ON;
-					delay = LED_BLINK_NO_LINK_INTERVAL_ALPHA;
-				}
-				pLed->bLedBlinkInProgress = false;
-			} else {
-				if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-					SwLedOff23a(padapter, pLed);
-				} else {
-					if (pLed->bLedOn)
-						pLed->BlinkingLedState = RTW_LED_OFF;
-					else
-						pLed->BlinkingLedState = RTW_LED_ON;
-					delay = LED_BLINK_FASTER_INTERVAL_ALPHA;
-				}
-			}
-			break;
-
-		case LED_BLINK_WPS:
-			if (pLed->bLedOn) {
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				delay = LED_BLINK_SLOWLY_INTERVAL;
-			} else {
-				pLed->BlinkingLedState = RTW_LED_ON;
-				delay = LED_BLINK_NORMAL_INTERVAL;
-			}
-			break;
-
-		case LED_BLINK_WPS_STOP:	/* WPS authentication fail */
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-
-			delay = LED_BLINK_NORMAL_INTERVAL;
-			break;
-
-		case LED_BLINK_WPS_STOP_OVERLAP:	/* WPS session overlap */
-			pLed->BlinkTimes--;
-			if (pLed->BlinkTimes == 0) {
-				if (pLed->bLedOn) {
-					pLed->BlinkTimes = 1;
-				} else {
-					bStopBlinking = true;
-				}
-			}
-
-			if (bStopBlinking) {
-				pLed->BlinkTimes = 10;
-				pLed->BlinkingLedState = RTW_LED_ON;
-				delay = LED_BLINK_LINK_INTERVAL_ALPHA;
-			} else {
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-
-				delay = LED_BLINK_NORMAL_INTERVAL;
-			}
-			break;
-
-		default:
-			break;
-	}
-	if (delay)
-		mod_timer(&pLed->BlinkTimer, jiffies + msecs_to_jiffies(delay));
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("SwLedBlink4 CurrLedState %d\n", pLed->CurrLedState));
-}
-
-static void SwLedBlink5(struct led_8723a *pLed)
-{
-	struct rtw_adapter *padapter = pLed->padapter;
-	u8 bStopBlinking = false;
-	unsigned long delay = 0;
-
-	/*  Change LED according to BlinkingLedState specified. */
-	if (pLed->BlinkingLedState == RTW_LED_ON) {
-		SwLedOn23a(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
-	} else {
-		SwLedOff23a(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
-	}
-
-	switch (pLed->CurrLedState)
-	{
-		case LED_BLINK_SCAN:
-			pLed->BlinkTimes--;
-			if (pLed->BlinkTimes == 0) {
-				bStopBlinking = true;
-			}
-
-			if (bStopBlinking) {
-				if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-					pLed->CurrLedState = RTW_LED_OFF;
-					pLed->BlinkingLedState = RTW_LED_OFF;
-					if (pLed->bLedOn)
-						SwLedOff23a(padapter, pLed);
-				} else {
-					pLed->CurrLedState = RTW_LED_ON;
-					pLed->BlinkingLedState = RTW_LED_ON;
-					if (!pLed->bLedOn)
-						delay = LED_BLINK_FASTER_INTERVAL_ALPHA;
-				}
-
-				pLed->bLedScanBlinkInProgress = false;
-			} else {
-				if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-					SwLedOff23a(padapter, pLed);
-				} else {
-					if (pLed->bLedOn)
-						pLed->BlinkingLedState = RTW_LED_OFF;
-					else
-						pLed->BlinkingLedState = RTW_LED_ON;
-					delay = LED_BLINK_SCAN_INTERVAL_ALPHA;
-				}
-			}
-			break;
-
-		case LED_BLINK_TXRX:
-			pLed->BlinkTimes--;
-			if (pLed->BlinkTimes == 0) {
-				bStopBlinking = true;
-			}
-
-			if (bStopBlinking) {
-				if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-					pLed->CurrLedState = RTW_LED_OFF;
-					pLed->BlinkingLedState = RTW_LED_OFF;
-					if (pLed->bLedOn)
-						SwLedOff23a(padapter, pLed);
-				} else {
-					pLed->CurrLedState = RTW_LED_ON;
-					pLed->BlinkingLedState = RTW_LED_ON;
-					if (!pLed->bLedOn)
-						delay = LED_BLINK_FASTER_INTERVAL_ALPHA;
-				}
-
-				pLed->bLedBlinkInProgress = false;
-			} else {
-				if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-					SwLedOff23a(padapter, pLed);
-				} else {
-					if (pLed->bLedOn)
-						pLed->BlinkingLedState = RTW_LED_OFF;
-					else
-						pLed->BlinkingLedState = RTW_LED_ON;
-					delay = LED_BLINK_FASTER_INTERVAL_ALPHA;
-				}
-			}
-			break;
-
-		default:
-			break;
-	}
-
-	if (delay)
-		mod_timer(&pLed->BlinkTimer, jiffies + msecs_to_jiffies(delay));
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("SwLedBlink5 CurrLedState %d\n", pLed->CurrLedState));
-}
-
-static void SwLedBlink6(struct led_8723a *pLed)
-{
-	struct rtw_adapter *padapter = pLed->padapter;
-
-	/*  Change LED according to BlinkingLedState specified. */
-	if (pLed->BlinkingLedState == RTW_LED_ON) {
-		SwLedOn23a(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
-	} else {
-		SwLedOff23a(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
-	}
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("<==== blink6\n"));
-}
-
-/* ALPHA, added by chiyoko, 20090106 */
-static void
-SwLedControlMode1(struct rtw_adapter *padapter, enum led_ctl_mode LedAction)
-{
-	struct led_priv *ledpriv = &padapter->ledpriv;
-	struct led_8723a *pLed = &ledpriv->SwLed0;
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-
-	long delay = -1;
-
-	switch (LedAction)
-	{
-		case LED_CTL_POWER_ON:
-		case LED_CTL_START_TO_LINK:
-		case LED_CTL_NO_LINK:
-			if (pLed->bLedNoLinkBlinkInProgress == false) {
-				if (pLed->CurrLedState == LED_BLINK_SCAN ||
-				    IS_LED_WPS_BLINKING(pLed)) {
-					return;
-				}
-				if (pLed->bLedLinkBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedLinkBlinkInProgress = false;
-				}
-				if (pLed->bLedBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedBlinkInProgress = false;
-				}
-
-				pLed->bLedNoLinkBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_SLOWLY;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				delay = LED_BLINK_NO_LINK_INTERVAL_ALPHA;
-			}
-			break;
-
-		case LED_CTL_LINK:
-			if (pLed->bLedLinkBlinkInProgress == false) {
-				if (pLed->CurrLedState == LED_BLINK_SCAN ||
-				    IS_LED_WPS_BLINKING(pLed)) {
-					return;
-				}
-				if (pLed->bLedNoLinkBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedNoLinkBlinkInProgress = false;
-				}
-				if (pLed->bLedBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedBlinkInProgress = false;
-				}
-				pLed->bLedLinkBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_NORMAL;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				delay = LED_BLINK_LINK_INTERVAL_ALPHA;
-			}
-			break;
-
-		case LED_CTL_SITE_SURVEY:
-			 if (pmlmepriv->LinkDetectInfo.bBusyTraffic &&
-			     check_fwstate(pmlmepriv, _FW_LINKED))
-				;
-			 else if (pLed->bLedScanBlinkInProgress == false) {
-				if (IS_LED_WPS_BLINKING(pLed))
-					return;
-
-				if (pLed->bLedNoLinkBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedNoLinkBlinkInProgress = false;
-				}
-				if (pLed->bLedLinkBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedLinkBlinkInProgress = false;
-				}
-				if (pLed->bLedBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedBlinkInProgress = false;
-				}
-				pLed->bLedScanBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_SCAN;
-				pLed->BlinkTimes = 24;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				delay = LED_BLINK_SCAN_INTERVAL_ALPHA;
-			 }
-			break;
-
-		case LED_CTL_TX:
-		case LED_CTL_RX:
-			if (pLed->bLedBlinkInProgress == false) {
-				if (pLed->CurrLedState == LED_BLINK_SCAN ||
-				    IS_LED_WPS_BLINKING(pLed)) {
-					return;
-				}
-				if (pLed->bLedNoLinkBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedNoLinkBlinkInProgress = false;
-				}
-				if (pLed->bLedLinkBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedLinkBlinkInProgress = false;
-				}
-				pLed->bLedBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_TXRX;
-				pLed->BlinkTimes = 2;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				delay = LED_BLINK_FASTER_INTERVAL_ALPHA;
-			}
-			break;
-
-		case LED_CTL_START_WPS: /* wait until xinpin finish */
-		case LED_CTL_START_WPS_BOTTON:
-			if (pLed->bLedWPSBlinkInProgress == false) {
-				if (pLed->bLedNoLinkBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedNoLinkBlinkInProgress = false;
-				}
-				if (pLed->bLedLinkBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedLinkBlinkInProgress = false;
-				}
-				if (pLed->bLedBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedBlinkInProgress = false;
-				}
-				if (pLed->bLedScanBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedScanBlinkInProgress = false;
-				}
-				pLed->bLedWPSBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_WPS;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				delay = LED_BLINK_SCAN_INTERVAL_ALPHA;
-			 }
-			break;
-
-		case LED_CTL_STOP_WPS:
-			if (pLed->bLedNoLinkBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedNoLinkBlinkInProgress = false;
-			}
-			if (pLed->bLedLinkBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedLinkBlinkInProgress = false;
-			}
-			if (pLed->bLedBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedBlinkInProgress = false;
-			}
-			if (pLed->bLedScanBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedScanBlinkInProgress = false;
-			}
-			if (pLed->bLedWPSBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-			} else {
-				pLed->bLedWPSBlinkInProgress = true;
-			}
-
-			pLed->CurrLedState = LED_BLINK_WPS_STOP;
-			if (pLed->bLedOn) {
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				delay = LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA;
-			} else {
-				pLed->BlinkingLedState = RTW_LED_ON;
-				delay = 0;
-			}
-			break;
-
-		case LED_CTL_STOP_WPS_FAIL:
-			if (pLed->bLedWPSBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedWPSBlinkInProgress = false;
-			}
-
-			pLed->bLedNoLinkBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_SLOWLY;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			delay = LED_BLINK_NO_LINK_INTERVAL_ALPHA;
-			break;
-
-		case LED_CTL_POWER_OFF:
-			pLed->CurrLedState = RTW_LED_OFF;
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			if (pLed->bLedNoLinkBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedNoLinkBlinkInProgress = false;
-			}
-			if (pLed->bLedLinkBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedLinkBlinkInProgress = false;
-			}
-			if (pLed->bLedBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedBlinkInProgress = false;
-			}
-			if (pLed->bLedWPSBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedWPSBlinkInProgress = false;
-			}
-			if (pLed->bLedScanBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedScanBlinkInProgress = false;
-			}
-
-			SwLedOff23a(padapter, pLed);
-			break;
-
-		default:
-			break;
-
-	}
-
-	if (delay != -1)
-		mod_timer(&pLed->BlinkTimer, jiffies + msecs_to_jiffies(delay));
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Led %d\n", pLed->CurrLedState));
-}
-
- /* Arcadyan/Sitecom , added by chiyoko, 20090216 */
-static void
-SwLedControlMode2(struct rtw_adapter *padapter, enum led_ctl_mode LedAction)
-{
-	struct led_priv *ledpriv = &padapter->ledpriv;
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct led_8723a *pLed = &ledpriv->SwLed0;
-	long delay = -1;
-
-	switch (LedAction) {
-	case LED_CTL_SITE_SURVEY:
-		 if (pmlmepriv->LinkDetectInfo.bBusyTraffic)
-			;
-		 else if (pLed->bLedScanBlinkInProgress == false) {
-			if (IS_LED_WPS_BLINKING(pLed))
-				return;
-
-			if (pLed->bLedBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedBlinkInProgress = false;
-			}
-			pLed->bLedScanBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_SCAN;
-			pLed->BlinkTimes = 24;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			delay = LED_BLINK_SCAN_INTERVAL_ALPHA;
-		 }
-		 break;
-	case LED_CTL_TX:
-	case LED_CTL_RX:
-		if (pLed->bLedBlinkInProgress == false &&
-		    check_fwstate(pmlmepriv, _FW_LINKED)) {
-			if (pLed->CurrLedState == LED_BLINK_SCAN ||
-			    IS_LED_WPS_BLINKING(pLed)) {
-				return;
-			}
-
-			pLed->bLedBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_TXRX;
-			pLed->BlinkTimes = 2;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			delay = LED_BLINK_FASTER_INTERVAL_ALPHA;
-		}
-		break;
-	case LED_CTL_LINK:
-		pLed->CurrLedState = RTW_LED_ON;
-		pLed->BlinkingLedState = RTW_LED_ON;
-		if (pLed->bLedBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
-			pLed->bLedBlinkInProgress = false;
-		}
-		if (pLed->bLedScanBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
-			pLed->bLedScanBlinkInProgress = false;
-		}
-
-		delay = 0;
-		break;
-	case LED_CTL_START_WPS: /* wait until xinpin finish */
-	case LED_CTL_START_WPS_BOTTON:
-		if (pLed->bLedWPSBlinkInProgress == false) {
-			if (pLed->bLedBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedBlinkInProgress = false;
-			}
-			if (pLed->bLedScanBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedScanBlinkInProgress = false;
-			}
-			pLed->bLedWPSBlinkInProgress = true;
-			pLed->CurrLedState = RTW_LED_ON;
-			pLed->BlinkingLedState = RTW_LED_ON;
-			delay = 0;
-		 }
-		break;
-	case LED_CTL_STOP_WPS:
-		pLed->bLedWPSBlinkInProgress = false;
-		if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-			SwLedOff23a(padapter, pLed);
-		} else {
-			pLed->CurrLedState = RTW_LED_ON;
-			pLed->BlinkingLedState = RTW_LED_ON;
-			delay = 0;
-			RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-		}
-		break;
-	case LED_CTL_STOP_WPS_FAIL:
-		pLed->bLedWPSBlinkInProgress = false;
-		if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-			SwLedOff23a(padapter, pLed);
-		} else {
-			pLed->CurrLedState = RTW_LED_OFF;
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			delay = 0;
-			RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-		}
-		break;
-	case LED_CTL_START_TO_LINK:
-	case LED_CTL_NO_LINK:
-		if (!IS_LED_BLINKING(pLed))
-		{
-			pLed->CurrLedState = RTW_LED_OFF;
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			delay = 0;
-		}
-		break;
-	case LED_CTL_POWER_OFF:
-		pLed->CurrLedState = RTW_LED_OFF;
-		pLed->BlinkingLedState = RTW_LED_OFF;
-		if (pLed->bLedBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
-			pLed->bLedBlinkInProgress = false;
-		}
-		if (pLed->bLedScanBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
-			pLed->bLedScanBlinkInProgress = false;
-		}
-		if (pLed->bLedWPSBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
-			pLed->bLedWPSBlinkInProgress = false;
-		}
-
-		delay = 0;
-		break;
-	default:
-		break;
-
-	}
-
-	if (delay != -1)
-		mod_timer(&pLed->BlinkTimer, jiffies + msecs_to_jiffies(delay));
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-}
-
-  /* COREGA, added by chiyoko, 20090316 */
-static void
-SwLedControlMode3(struct rtw_adapter *padapter, enum led_ctl_mode LedAction)
-{
-	struct led_priv *ledpriv = &padapter->ledpriv;
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct led_8723a *pLed = &ledpriv->SwLed0;
-	long delay = -1;
-
-	switch (LedAction)
-	{
-		case LED_CTL_SITE_SURVEY:
-			if (pmlmepriv->LinkDetectInfo.bBusyTraffic)
-				;
-			else if (pLed->bLedScanBlinkInProgress == false) {
-				if (IS_LED_WPS_BLINKING(pLed))
-					return;
-
-				if (pLed->bLedBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedBlinkInProgress = false;
-				}
-				pLed->bLedScanBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_SCAN;
-				pLed->BlinkTimes = 24;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				delay = LED_BLINK_SCAN_INTERVAL_ALPHA;
-			}
-			break;
-
-		case LED_CTL_TX:
-		case LED_CTL_RX:
-			if (pLed->bLedBlinkInProgress == false &&
-			    check_fwstate(pmlmepriv, _FW_LINKED)) {
-				if (pLed->CurrLedState == LED_BLINK_SCAN ||
-				    IS_LED_WPS_BLINKING(pLed)) {
-					return;
-				}
-
-				pLed->bLedBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_TXRX;
-				pLed->BlinkTimes = 2;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				delay =  LED_BLINK_FASTER_INTERVAL_ALPHA;
-			}
-			break;
-
-		case LED_CTL_LINK:
-			if (IS_LED_WPS_BLINKING(pLed))
-				return;
-
-			pLed->CurrLedState = RTW_LED_ON;
-			pLed->BlinkingLedState = RTW_LED_ON;
-			if (pLed->bLedBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedBlinkInProgress = false;
-			}
-			if (pLed->bLedScanBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedScanBlinkInProgress = false;
-			}
-
-			delay = 0;
-			break;
-
-		case LED_CTL_START_WPS: /* wait until xinpin finish */
-		case LED_CTL_START_WPS_BOTTON:
-			if (pLed->bLedWPSBlinkInProgress == false) {
-				if (pLed->bLedBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedBlinkInProgress = false;
-				}
-				if (pLed->bLedScanBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedScanBlinkInProgress = false;
-				}
-				pLed->bLedWPSBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_WPS;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				delay = LED_BLINK_SCAN_INTERVAL_ALPHA;
-			}
-			break;
-
-		case LED_CTL_STOP_WPS:
-			if (pLed->bLedWPSBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedWPSBlinkInProgress = false;
-			} else {
-				pLed->bLedWPSBlinkInProgress = true;
-			}
-
-			pLed->CurrLedState = LED_BLINK_WPS_STOP;
-			if (pLed->bLedOn) {
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				delay = LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA;
-			} else {
-				pLed->BlinkingLedState = RTW_LED_ON;
-				delay = 0;
-			}
-
-			break;
-
-		case LED_CTL_STOP_WPS_FAIL:
-			if (pLed->bLedWPSBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedWPSBlinkInProgress = false;
-			}
-
-			pLed->CurrLedState = RTW_LED_OFF;
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			delay = 0;
-			break;
-
-		case LED_CTL_START_TO_LINK:
-		case LED_CTL_NO_LINK:
-			if (!IS_LED_BLINKING(pLed))
-			{
-				pLed->CurrLedState = RTW_LED_OFF;
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				delay = 0;
-			}
-			break;
-
-		case LED_CTL_POWER_OFF:
-			pLed->CurrLedState = RTW_LED_OFF;
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			if (pLed->bLedBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedBlinkInProgress = false;
-			}
-			if (pLed->bLedScanBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedScanBlinkInProgress = false;
-			}
-			if (pLed->bLedWPSBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedWPSBlinkInProgress = false;
-			}
-
-			delay = 0;
-			break;
-
-		default:
-			break;
-
-	}
-
-	if (delay != -1)
-		mod_timer(&pLed->BlinkTimer, jiffies + msecs_to_jiffies(delay));
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-}
-
- /* Edimax-Belkin, added by chiyoko, 20090413 */
-static void
-SwLedControlMode4(struct rtw_adapter *padapter, enum led_ctl_mode LedAction)
-{
-	struct led_priv *ledpriv = &padapter->ledpriv;
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct led_8723a *pLed = &ledpriv->SwLed0;
-	struct led_8723a *pLed1 = &ledpriv->SwLed1;
-
-	switch (LedAction)
-	{
-		case LED_CTL_START_TO_LINK:
-			if (pLed1->bLedWPSBlinkInProgress) {
-				pLed1->bLedWPSBlinkInProgress = false;
-				del_timer_sync(&pLed1->BlinkTimer);
-
-				pLed1->BlinkingLedState = RTW_LED_OFF;
-				pLed1->CurrLedState = RTW_LED_OFF;
-
-				if (pLed1->bLedOn)
-					mod_timer(&pLed->BlinkTimer, jiffies);
-			}
-
-			if (pLed->bLedStartToLinkBlinkInProgress == false) {
-				if (pLed->CurrLedState == LED_BLINK_SCAN ||
-				    IS_LED_WPS_BLINKING(pLed)) {
-					return;
-				}
-				if (pLed->bLedBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedBlinkInProgress = false;
-				}
-				if (pLed->bLedNoLinkBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedNoLinkBlinkInProgress = false;
-				}
-
-				pLed->bLedStartToLinkBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_StartToBlink;
-				if (pLed->bLedOn) {
-					pLed->BlinkingLedState = RTW_LED_OFF;
-					mod_timer(&pLed->BlinkTimer,
-						  jiffies + msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL));
-				} else {
-					pLed->BlinkingLedState = RTW_LED_ON;
-					mod_timer(&pLed->BlinkTimer,
-						  jiffies + msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
-				}
-			}
-			break;
-
-		case LED_CTL_LINK:
-		case LED_CTL_NO_LINK:
-			/* LED1 settings */
-			if (LedAction == LED_CTL_LINK) {
-				if (pLed1->bLedWPSBlinkInProgress) {
-					pLed1->bLedWPSBlinkInProgress = false;
-					del_timer_sync(&pLed1->BlinkTimer);
-
-					pLed1->BlinkingLedState = RTW_LED_OFF;
-					pLed1->CurrLedState = RTW_LED_OFF;
-
-					if (pLed1->bLedOn)
-						mod_timer(&pLed->BlinkTimer, jiffies);
-				}
-			}
-
-			if (pLed->bLedNoLinkBlinkInProgress == false) {
-				if (pLed->CurrLedState == LED_BLINK_SCAN ||
-				    IS_LED_WPS_BLINKING(pLed)) {
-					return;
-				}
-				if (pLed->bLedBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedBlinkInProgress = false;
-				}
-
-				pLed->bLedNoLinkBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_SLOWLY;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				mod_timer(&pLed->BlinkTimer, jiffies +
-					  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
-			}
-			break;
-
-		case LED_CTL_SITE_SURVEY:
-			if (pmlmepriv->LinkDetectInfo.bBusyTraffic &&
-			    check_fwstate(pmlmepriv, _FW_LINKED))
-				;
-			else if (pLed->bLedScanBlinkInProgress == false) {
-				if (IS_LED_WPS_BLINKING(pLed))
-					return;
-
-				if (pLed->bLedNoLinkBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedNoLinkBlinkInProgress = false;
-				}
-				if (pLed->bLedBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedBlinkInProgress = false;
-				}
-				pLed->bLedScanBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_SCAN;
-				pLed->BlinkTimes = 24;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				mod_timer(&pLed->BlinkTimer, jiffies +
-					  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
-			}
-			break;
-
-		case LED_CTL_TX:
-		case LED_CTL_RX:
-			if (pLed->bLedBlinkInProgress == false) {
-				if (pLed->CurrLedState == LED_BLINK_SCAN ||
-				    IS_LED_WPS_BLINKING(pLed)) {
-					return;
-				}
-				if (pLed->bLedNoLinkBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedNoLinkBlinkInProgress = false;
-				}
-				pLed->bLedBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_TXRX;
-				pLed->BlinkTimes = 2;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				mod_timer(&pLed->BlinkTimer, jiffies +
-					  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
-			}
-			break;
-
-		case LED_CTL_START_WPS: /* wait until xinpin finish */
-		case LED_CTL_START_WPS_BOTTON:
-			if (pLed1->bLedWPSBlinkInProgress) {
-				pLed1->bLedWPSBlinkInProgress = false;
-				del_timer_sync(&pLed1->BlinkTimer);
-
-				pLed1->BlinkingLedState = RTW_LED_OFF;
-				pLed1->CurrLedState = RTW_LED_OFF;
-
-				if (pLed1->bLedOn)
-					mod_timer(&pLed->BlinkTimer, jiffies);
-			}
-
-			if (pLed->bLedWPSBlinkInProgress == false) {
-				if (pLed->bLedNoLinkBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedNoLinkBlinkInProgress = false;
-				}
-				if (pLed->bLedBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedBlinkInProgress = false;
-				}
-				if (pLed->bLedScanBlinkInProgress == true) {
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedScanBlinkInProgress = false;
-				}
-				pLed->bLedWPSBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_WPS;
-				if (pLed->bLedOn)
-				{
-					pLed->BlinkingLedState = RTW_LED_OFF;
-					mod_timer(&pLed->BlinkTimer, jiffies +
-						  msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL));
-				} else {
-					pLed->BlinkingLedState = RTW_LED_ON;
-					mod_timer(&pLed->BlinkTimer, jiffies +
-						  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
-				}
-			}
-			break;
-
-		case LED_CTL_STOP_WPS:	/* WPS connect success */
-			if (pLed->bLedWPSBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedWPSBlinkInProgress = false;
-			}
-
-			pLed->bLedNoLinkBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_SLOWLY;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			mod_timer(&pLed->BlinkTimer, jiffies +
-				  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
-			break;
-
-		case LED_CTL_STOP_WPS_FAIL:		/* WPS authentication fail */
-			if (pLed->bLedWPSBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedWPSBlinkInProgress = false;
-			}
-
-			pLed->bLedNoLinkBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_SLOWLY;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			mod_timer(&pLed->BlinkTimer, jiffies +
-				  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
-
-			/* LED1 settings */
-			if (pLed1->bLedWPSBlinkInProgress)
-				del_timer_sync(&pLed1->BlinkTimer);
-			else
-				pLed1->bLedWPSBlinkInProgress = true;
-
-			pLed1->CurrLedState = LED_BLINK_WPS_STOP;
-			if (pLed1->bLedOn)
-				pLed1->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed1->BlinkingLedState = RTW_LED_ON;
-			mod_timer(&pLed->BlinkTimer, jiffies +
-				  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
-
-			break;
-
-		case LED_CTL_STOP_WPS_FAIL_OVERLAP:	/* WPS session overlap */
-			if (pLed->bLedWPSBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedWPSBlinkInProgress = false;
-			}
-
-			pLed->bLedNoLinkBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_SLOWLY;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			mod_timer(&pLed->BlinkTimer, jiffies +
-				  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
-
-			/* LED1 settings */
-			if (pLed1->bLedWPSBlinkInProgress)
-				del_timer_sync(&pLed1->BlinkTimer);
-			else
-				pLed1->bLedWPSBlinkInProgress = true;
-
-			pLed1->CurrLedState = LED_BLINK_WPS_STOP_OVERLAP;
-			pLed1->BlinkTimes = 10;
-			if (pLed1->bLedOn)
-				pLed1->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed1->BlinkingLedState = RTW_LED_ON;
-			mod_timer(&pLed->BlinkTimer, jiffies +
-				  msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL));
-
-			break;
-
-		case LED_CTL_POWER_OFF:
-			pLed->CurrLedState = RTW_LED_OFF;
-			pLed->BlinkingLedState = RTW_LED_OFF;
-
-			if (pLed->bLedNoLinkBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedNoLinkBlinkInProgress = false;
-			}
-			if (pLed->bLedLinkBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedLinkBlinkInProgress = false;
-			}
-			if (pLed->bLedBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedBlinkInProgress = false;
-			}
-			if (pLed->bLedWPSBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedWPSBlinkInProgress = false;
-			}
-			if (pLed->bLedScanBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedScanBlinkInProgress = false;
-			}
-			if (pLed->bLedStartToLinkBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedStartToLinkBlinkInProgress = false;
-			}
-
-			if (pLed1->bLedWPSBlinkInProgress) {
-				del_timer_sync(&pLed1->BlinkTimer);
-				pLed1->bLedWPSBlinkInProgress = false;
-			}
-
-			pLed1->BlinkingLedState = LED_UNKNOWN;
-			SwLedOff23a(padapter, pLed);
-			SwLedOff23a(padapter, pLed1);
-			break;
-
-		default:
-			break;
-
-	}
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Led %d\n", pLed->CurrLedState));
-}
-
- /* Sercomm-Belkin, added by chiyoko, 20090415 */
-static void
-SwLedControlMode5(struct rtw_adapter *padapter, enum led_ctl_mode LedAction)
-{
-	struct led_priv *ledpriv = &padapter->ledpriv;
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct led_8723a *pLed = &ledpriv->SwLed0;
-
-	switch (LedAction)
-	{
-		case LED_CTL_POWER_ON:
-		case LED_CTL_NO_LINK:
-		case LED_CTL_LINK:	/* solid blue */
-			pLed->CurrLedState = RTW_LED_ON;
-			pLed->BlinkingLedState = RTW_LED_ON;
-
-			mod_timer(&pLed->BlinkTimer, jiffies);
-			break;
-
-		case LED_CTL_SITE_SURVEY:
-			if (pmlmepriv->LinkDetectInfo.bBusyTraffic &&
-			    check_fwstate(pmlmepriv, _FW_LINKED))
-				;
-			else if (pLed->bLedScanBlinkInProgress == false)
-			{
-				if (pLed->bLedBlinkInProgress == true)
-				{
-					del_timer_sync(&pLed->BlinkTimer);
-					pLed->bLedBlinkInProgress = false;
-				}
-				pLed->bLedScanBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_SCAN;
-				pLed->BlinkTimes = 24;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				mod_timer(&pLed->BlinkTimer, jiffies +
-					  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
-			}
-			break;
-
-		case LED_CTL_TX:
-		case LED_CTL_RX:
-			if (pLed->bLedBlinkInProgress == false) {
-				if (pLed->CurrLedState == LED_BLINK_SCAN) {
-					return;
-				}
-				pLed->bLedBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_TXRX;
-				pLed->BlinkTimes = 2;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				mod_timer(&pLed->BlinkTimer, jiffies +
-					  msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA));
-			}
-			break;
-
-		case LED_CTL_POWER_OFF:
-			pLed->CurrLedState = RTW_LED_OFF;
-			pLed->BlinkingLedState = RTW_LED_OFF;
-
-			if (pLed->bLedBlinkInProgress) {
-				del_timer_sync(&pLed->BlinkTimer);
-				pLed->bLedBlinkInProgress = false;
-			}
-
-			SwLedOff23a(padapter, pLed);
-			break;
-
-		default:
-			break;
-
-	}
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Led %d\n", pLed->CurrLedState));
-}
-
- /* WNC-Corega, added by chiyoko, 20090902 */
-static void SwLedControlMode6(struct rtw_adapter *padapter,
-			      enum led_ctl_mode LedAction)
-{
-	struct led_priv *ledpriv = &padapter->ledpriv;
-	struct led_8723a *pLed0 = &ledpriv->SwLed0;
-
-	switch (LedAction) {
-	case LED_CTL_POWER_ON:
-	case LED_CTL_LINK:
-	case LED_CTL_NO_LINK:
-		del_timer_sync(&pLed0->BlinkTimer);
-		pLed0->CurrLedState = RTW_LED_ON;
-		pLed0->BlinkingLedState = RTW_LED_ON;
-		mod_timer(&pLed0->BlinkTimer, jiffies);
-		break;
-	case LED_CTL_POWER_OFF:
-		SwLedOff23a(padapter, pLed0);
-		break;
-	default:
-		break;
-	}
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("ledcontrol 6 Led %d\n", pLed0->CurrLedState));
-}
-
-/*  */
-/*	Description: */
-/*		Handler function of LED Blinking. */
-/*		We dispatch acture LED blink action according to LedStrategy. */
-/*  */
-void BlinkHandler23a(struct led_8723a *pLed)
-{
-	struct rtw_adapter *padapter = pLed->padapter;
-	struct led_priv *ledpriv = &padapter->ledpriv;
-
-	/* DBG_8723A("%s (%s:%d)\n", __func__, current->comm, current->pid); */
-
-	if ((padapter->bSurpriseRemoved) || (padapter->bDriverStopped))
-		return;
-
-	switch (ledpriv->LedStrategy) {
-	case SW_LED_MODE0:
-		SwLedBlink(pLed);
-		break;
-	case SW_LED_MODE1:
-		SwLedBlink1(pLed);
-		break;
-	case SW_LED_MODE2:
-		SwLedBlink2(pLed);
-		break;
-	case SW_LED_MODE3:
-		SwLedBlink3(pLed);
-		break;
-	case SW_LED_MODE4:
-		SwLedBlink4(pLed);
-		break;
-	case SW_LED_MODE5:
-		SwLedBlink5(pLed);
-		break;
-	case SW_LED_MODE6:
-		SwLedBlink6(pLed);
-		break;
-	default:
-		break;
-	}
-}
-
-void
-LedControl871x23a(struct rtw_adapter *padapter, enum led_ctl_mode LedAction) {
-	struct led_priv *ledpriv = &padapter->ledpriv;
-
-	if ((padapter->bSurpriseRemoved == true) ||
-	    (padapter->bDriverStopped == true) ||
-	    (padapter->hw_init_completed == false)) {
-		return;
-	}
-
-	if (ledpriv->bRegUseLed == false)
-		return;
-
-	/* if (!priv->up) */
-	/*	return; */
-
-	/* if (priv->bInHctTest) */
-	/*	return; */
-
-	if ((padapter->pwrctrlpriv.rf_pwrstate != rf_on &&
-	     padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) &&
-	    (LedAction == LED_CTL_TX || LedAction == LED_CTL_RX ||
-	     LedAction == LED_CTL_SITE_SURVEY ||
-	     LedAction == LED_CTL_LINK ||
-	     LedAction == LED_CTL_NO_LINK ||
-	     LedAction == LED_CTL_POWER_ON)) {
-		return;
-	}
-
-	switch (ledpriv->LedStrategy) {
-	case SW_LED_MODE0:
-		break;
-	case SW_LED_MODE1:
-		SwLedControlMode1(padapter, LedAction);
-		break;
-	case SW_LED_MODE2:
-		SwLedControlMode2(padapter, LedAction);
-		break;
-	case SW_LED_MODE3:
-		SwLedControlMode3(padapter, LedAction);
-		break;
-	case SW_LED_MODE4:
-		SwLedControlMode4(padapter, LedAction);
-		break;
-	case SW_LED_MODE5:
-		SwLedControlMode5(padapter, LedAction);
-		break;
-	case SW_LED_MODE6:
-		SwLedControlMode6(padapter, LedAction);
-		break;
-	default:
-		break;
-	}
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("LedStrategy:%d, LedAction %d\n", ledpriv->LedStrategy, LedAction));
-}
diff --git a/drivers/staging/rtl8723au/core/rtw_mlme.c b/drivers/staging/rtl8723au/core/rtw_mlme.c
index 1f6006439bbb..7299ef0a2e54 100644
--- a/drivers/staging/rtl8723au/core/rtw_mlme.c
+++ b/drivers/staging/rtl8723au/core/rtw_mlme.c
@@ -50,7 +50,6 @@ static void rtw_init_mlme_timer(struct rtw_adapter *padapter)
 int rtw_init_mlme_priv23a(struct rtw_adapter *padapter)
 {
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	int res = _SUCCESS;
 
 	pmlmepriv->nic_hdl = padapter;
 
@@ -68,7 +67,7 @@ int rtw_init_mlme_priv23a(struct rtw_adapter *padapter)
 	rtw_clear_scan_deny(padapter);
 
 	rtw_init_mlme_timer(padapter);
-	return res;
+	return _SUCCESS;
 }
 
 #ifdef CONFIG_8723AU_AP_MODE
@@ -110,7 +109,6 @@ struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv, gfp_t gfp)
 		pnetwork->network_type = 0;
 		pnetwork->fixed = false;
 		pnetwork->last_scanned = jiffies;
-		pnetwork->aid = 0;
 		pnetwork->join_res = 0;
 	}
 
@@ -218,8 +216,6 @@ void rtw_generate_random_ibss23a(u8 *pibss)
 	pibss[3] = curtime & 0xff;/* p[0]; */
 	pibss[4] = (curtime >> 8) & 0xff;/* p[1]; */
 	pibss[5] = (curtime >> 16) & 0xff;/* p[2]; */
-
-	return;
 }
 
 void rtw_set_roaming(struct rtw_adapter *adapter, u8 to_roaming)
@@ -356,12 +352,12 @@ rtw_get_oldest_wlan_network23a(struct rtw_queue *scanned_queue)
 void update_network23a(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
 		       struct rtw_adapter *padapter, bool update_ie)
 {
-	u8 ss_ori = dst->PhyInfo.SignalStrength;
-	u8 sq_ori = dst->PhyInfo.SignalQuality;
+	u8 ss_ori = dst->SignalStrength;
+	u8 sq_ori = dst->SignalQuality;
 	long rssi_ori = dst->Rssi;
 
-	u8 ss_smp = src->PhyInfo.SignalStrength;
-	u8 sq_smp = src->PhyInfo.SignalQuality;
+	u8 ss_smp = src->SignalStrength;
+	u8 sq_smp = src->SignalQuality;
 	long rssi_smp = src->Rssi;
 
 	u8 ss_final;
@@ -389,16 +385,16 @@ void update_network23a(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
 			rssi_final = rssi_ori;
 	} else {
 		if (sq_smp != 101) { /* from the right channel */
-			ss_final = ((u32)src->PhyInfo.SignalStrength +
-				    (u32)dst->PhyInfo.SignalStrength * 4) / 5;
-			sq_final = ((u32)src->PhyInfo.SignalQuality +
-				    (u32)dst->PhyInfo.SignalQuality * 4) / 5;
+			ss_final = ((u32)src->SignalStrength +
+				    (u32)dst->SignalStrength * 4) / 5;
+			sq_final = ((u32)src->SignalQuality +
+				    (u32)dst->SignalQuality * 4) / 5;
 			rssi_final = src->Rssi+dst->Rssi * 4 / 5;
 		} else {
 			/* bss info not receiving from the right channel, use
 			   the original RX signal infos */
-			ss_final = dst->PhyInfo.SignalStrength;
-			sq_final = dst->PhyInfo.SignalQuality;
+			ss_final = dst->SignalStrength;
+			sq_final = dst->SignalQuality;
 			rssi_final = dst->Rssi;
 		}
 
@@ -407,14 +403,13 @@ void update_network23a(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
 	if (update_ie)
 		memcpy(dst, src, get_wlan_bssid_ex_sz(src));
 
-	dst->PhyInfo.SignalStrength = ss_final;
-	dst->PhyInfo.SignalQuality = sq_final;
+	dst->SignalStrength = ss_final;
+	dst->SignalQuality = sq_final;
 	dst->Rssi = rssi_final;
 
 	DBG_8723A("%s %s(%pM), SignalStrength:%u, SignalQuality:%u, "
 		  "RawRSSI:%ld\n",  __func__, dst->Ssid.ssid, dst->MacAddress,
-		  dst->PhyInfo.SignalStrength,
-		  dst->PhyInfo.SignalQuality, dst->Rssi);
+		  dst->SignalStrength, dst->SignalQuality, dst->Rssi);
 }
 
 static void update_current_network(struct rtw_adapter *adapter,
@@ -487,12 +482,11 @@ static void rtw_update_scanned_network(struct rtw_adapter *adapter,
 		pnetwork->last_scanned = jiffies;
 
 		pnetwork->network_type = 0;
-		pnetwork->aid = 0;
 		pnetwork->join_res = 0;
 
 		/* bss info not receiving from the right channel */
-		if (pnetwork->network.PhyInfo.SignalQuality == 101)
-			pnetwork->network.PhyInfo.SignalQuality = 0;
+		if (pnetwork->network.SignalQuality == 101)
+			pnetwork->network.SignalQuality = 0;
 	} else {
 		/*
 		 * we have an entry and we are going to update it. But
@@ -579,8 +573,6 @@ void rtw_atimdone_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 {
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
 		 ("receive atimdone_evet\n"));
-
-	return;
 }
 
 void rtw_survey_event_cb23a(struct rtw_adapter *adapter, const u8 *pbuf)
@@ -650,8 +642,6 @@ exit:
 
 	kfree(survey->bss);
 	survey->bss = NULL;
-
-	return;
 }
 
 void
@@ -825,8 +815,6 @@ void rtw_indicate_connect23a(struct rtw_adapter *padapter)
 	if (!check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
 		set_fwstate(pmlmepriv, _FW_LINKED);
 
-		rtw_led_control(padapter, LED_CTL_LINK);
-
 		rtw_cfg80211_indicate_connect(padapter);
 
 		netif_carrier_on(padapter->pnetdev);
@@ -871,10 +859,7 @@ void rtw_indicate_disconnect23a(struct rtw_adapter *padapter)
 
 		_clr_fwstate_(pmlmepriv, _FW_LINKED);
 
-		rtw_led_control(padapter, LED_CTL_NO_LINK);
-
 		rtw_clear_scan_deny(padapter);
-
 	}
 
 	rtw_lps_ctrl_wk_cmd23a(padapter, LPS_CTRL_DISCONNECT, 1);
@@ -1028,22 +1013,18 @@ rtw_joinbss_update_network23a(struct rtw_adapter *padapter,
 	cur_network->network.beacon_interval =
 		ptarget_wlan->network.beacon_interval;
 	cur_network->network.tsf = ptarget_wlan->network.tsf;
-	cur_network->aid = pnetwork->join_res;
 
 	rtw_set_signal_stat_timer(&padapter->recvpriv);
 	padapter->recvpriv.signal_strength =
-		ptarget_wlan->network.PhyInfo.SignalStrength;
-	padapter->recvpriv.signal_qual =
-		ptarget_wlan->network.PhyInfo.SignalQuality;
+		ptarget_wlan->network.SignalStrength;
+	padapter->recvpriv.signal_qual = ptarget_wlan->network.SignalQuality;
 	/*
 	 * the ptarget_wlan->network.Rssi is raw data, we use
-	 * ptarget_wlan->network.PhyInfo.SignalStrength instead (has scaled)
+	 * ptarget_wlan->network.SignalStrength instead (has scaled)
 	 */
-	padapter->recvpriv.rssi = translate_percentage_to_dbm(
-		ptarget_wlan->network.PhyInfo.SignalStrength);
-	DBG_8723A("%s signal_strength:%3u, rssi:%3d, signal_qual:%3u\n",
+	DBG_8723A("%s signal_strength:%3u, signal_qual:%3u\n",
 		  __func__, padapter->recvpriv.signal_strength,
-		  padapter->recvpriv.rssi, padapter->recvpriv.signal_qual);
+		  padapter->recvpriv.signal_qual);
 	rtw_set_signal_stat_timer(&padapter->recvpriv);
 
 	/* update fw_state will clr _FW_UNDER_LINKING here indirectly */
@@ -1132,7 +1113,7 @@ void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf)
 		if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
 			/* s1. find ptarget_wlan */
 			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-				if (the_same_macaddr == true) {
+				if (the_same_macaddr) {
 					ptarget_wlan = rtw_find_network23a(&pmlmepriv->scanned_queue, cur_network->network.MacAddress);
 				} else {
 					pcur_wlan = rtw_find_network23a(&pmlmepriv->scanned_queue, cur_network->network.MacAddress);
@@ -1515,18 +1496,21 @@ out:
 inline bool rtw_is_scan_deny(struct rtw_adapter *adapter)
 {
 	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
+
 	return (atomic_read(&mlmepriv->set_scan_deny) != 0) ? true : false;
 }
 
 void rtw_clear_scan_deny(struct rtw_adapter *adapter)
 {
 	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
+
 	atomic_set(&mlmepriv->set_scan_deny, 0);
 }
 
 void rtw_set_scan_deny_timer_hdl(unsigned long data)
 {
 	struct rtw_adapter *adapter = (struct rtw_adapter *)data;
+
 	rtw_clear_scan_deny(adapter);
 }
 
@@ -1540,7 +1524,8 @@ void rtw_set_scan_deny(struct rtw_adapter *adapter, u32 ms)
 }
 
 #if defined(IEEE80211_SCAN_RESULT_EXPIRE)
-#define RTW_SCAN_RESULT_EXPIRE IEEE80211_SCAN_RESULT_EXPIRE/HZ*1000 -1000 /* 3000 -1000 */
+#define RTW_SCAN_RESULT_EXPIRE  \
+	((IEEE80211_SCAN_RESULT_EXPIRE / (HZ*1000)) - 1000) /* 3000 -1000 */
 #else
 #define RTW_SCAN_RESULT_EXPIRE 2000
 #endif
@@ -1766,7 +1751,7 @@ exit:
 	return ret;
 }
 
-int rtw_set_auth23a(struct rtw_adapter * adapter,
+int rtw_set_auth23a(struct rtw_adapter *adapter,
 		    struct security_priv *psecuritypriv)
 {
 	struct cmd_obj *pcmd;
@@ -2151,6 +2136,7 @@ bool rtw_restructure_ht_ie23a(struct rtw_adapter *padapter, u8 *in_ie,
 
 	if (p && p[1] > 0) {
 		u32 rx_packet_offset, max_recvbuf_sz;
+
 		if (pmlmepriv->qos_option == 0) {
 			out_len = *pout_len;
 			pframe = rtw_set_ie23a(out_ie + out_len,
@@ -2165,9 +2151,9 @@ bool rtw_restructure_ht_ie23a(struct rtw_adapter *padapter, u8 *in_ie,
 
 		memset(&ht_capie, 0, sizeof(struct ieee80211_ht_cap));
 
-		ht_capie.cap_info = IEEE80211_HT_CAP_SUP_WIDTH_20_40 |
+		ht_capie.cap_info = cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH_20_40 |
 			IEEE80211_HT_CAP_SGI_20 | IEEE80211_HT_CAP_SGI_40 |
-			IEEE80211_HT_CAP_TX_STBC | IEEE80211_HT_CAP_DSSSCCK40;
+			IEEE80211_HT_CAP_TX_STBC | IEEE80211_HT_CAP_DSSSCCK40);
 
 		GetHalDefVar8192CUsb(padapter, HAL_DEF_RX_PACKET_OFFSET,
 				     &rx_packet_offset);
diff --git a/drivers/staging/rtl8723au/core/rtw_mlme_ext.c b/drivers/staging/rtl8723au/core/rtw_mlme_ext.c
index 3eb77de17e3a..0e0f73c86e53 100644
--- a/drivers/staging/rtl8723au/core/rtw_mlme_ext.c
+++ b/drivers/staging/rtl8723au/core/rtw_mlme_ext.c
@@ -331,6 +331,7 @@ rtw_update_TSF(struct mlme_ext_priv *pmlmeext, struct ieee80211_mgmt *mgmt)
 int rtw_ch_set_search_ch23a(struct rt_channel_info *ch_set, const u32 ch)
 {
 	int i;
+
 	for (i = 0; ch_set[i]. ChannelNum != 0; i++) {
 		if (ch == ch_set[i].ChannelNum)
 			break;
@@ -566,7 +567,6 @@ static u8 init_channel_set(struct rtw_adapter *padapter, u8 cplan,
 
 int init_mlme_ext_priv23a(struct rtw_adapter *padapter)
 {
-	int res = _SUCCESS;
 	struct registry_priv *pregistrypriv = &padapter->registrypriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
@@ -593,7 +593,7 @@ int init_mlme_ext_priv23a(struct rtw_adapter *padapter)
 	pmlmeext->mlmeext_init = true;
 
 	pmlmeext->active_keep_alive_check = true;
-	return res;
+	return _SUCCESS;
 }
 
 void free_mlme_ext_priv23a (struct mlme_ext_priv *pmlmeext)
@@ -680,8 +680,7 @@ void mgt_dispatcher23a(struct rtw_adapter *padapter,
 	}
 
 #ifdef CONFIG_8723AU_AP_MODE
-	switch (stype)
-	{
+	switch (stype) {
 	case IEEE80211_STYPE_AUTH:
 		if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
 			ptable->func = &OnAuth23a;
@@ -1572,6 +1571,7 @@ OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 	pstat->uapsd_bk = 0;
 	if (pmlmepriv->qos_option) {
 		const u8 *end = pos + left;
+
 		p = pos;
 
 		for (;;) {
@@ -2335,12 +2335,9 @@ static int update_hidden_ssid(u8 *ies, u32 ies_len, u8 hidden_ssid_mode)
 	   __func__, hidden_ssid_mode, ssid_ie, ssid_len_ori); */
 
 	if (ssid_ie && ssid_len_ori > 0) {
-		switch (hidden_ssid_mode)
-		{
+		switch (hidden_ssid_mode) {
 		case 1:
 			next_ie = ssid_ie + 2 + ssid_len_ori;
-			remain_len = 0;
-
 			remain_len = ies_len -(next_ie-ies);
 
 			ssid_ie[1] = 0;
@@ -2599,7 +2596,9 @@ static void issue_probersp(struct rtw_adapter *padapter, unsigned char *da,
 		if (ssid_ie && cur_network->Ssid.ssid_len) {
 			uint remainder_ielen;
 			u8 *remainder_ie;
+
 			remainder_ie = ssid_ie + 2;
+
 			remainder_ielen = pframe - remainder_ie;
 
 			DBG_8723A_LEVEL(_drv_warning_, "%s(%s): "
@@ -2862,6 +2861,7 @@ static void issue_auth(struct rtw_adapter *padapter, struct sta_info *psta,
 	if (psta) { /*  for AP mode */
 #ifdef CONFIG_8723AU_AP_MODE
 		unsigned short val16;
+
 		ether_addr_copy(mgmt->da, psta->hwaddr);
 		ether_addr_copy(mgmt->sa, myid(&padapter->eeprompriv));
 		ether_addr_copy(mgmt->bssid, myid(&padapter->eeprompriv));
@@ -3306,6 +3306,7 @@ static void issue_assocreq(struct rtw_adapter *padapter)
 			    !memcmp(p + 2, WMM_OUI23A, 4) ||
 			    !memcmp(p + 2, WPS_OUI23A, 4)) {
 				u8 plen = p[1];
+
 				if (!padapter->registrypriv.wifi_spec) {
 					/* Commented by Kurt 20110629 */
 					/* In some older APs, WPS handshake */
@@ -3997,7 +3998,7 @@ int send_beacon23a(struct rtw_adapter *padapter)
 			yield();
 			bxmitok = rtl8723a_get_bcn_valid(padapter);
 			poll++;
-		} while ((poll % 10) != 0 && bxmitok == false &&
+		} while ((poll % 10) != 0 && !bxmitok &&
 			 !padapter->bSurpriseRemoved &&
 			 !padapter->bDriverStopped);
 
@@ -4070,6 +4071,7 @@ static void rtw_site_survey(struct rtw_adapter *padapter)
 		if (ScanType == SCAN_ACTIVE) /* obey the channel plan setting... */
 		{
 			int i;
+
 			for (i = 0;i<RTW_SSID_SCAN_AMOUNT;i++) {
 				if (pmlmeext->sitesurvey_res.ssid[i].ssid_len) {
 					/* todo: to issue two probe req??? */
@@ -4197,9 +4199,9 @@ static struct wlan_bssid_ex *collect_bss_info(struct rtw_adapter *padapter,
 	/* get the signal strength */
 	/*  in dBM.raw data */
 	bssid->Rssi = precv_frame->attrib.phy_info.RecvSignalPower;
-	bssid->PhyInfo.SignalQuality =
+	bssid->SignalQuality =
 		precv_frame->attrib.phy_info.SignalQuality;/* in percentage */
-	bssid->PhyInfo.SignalStrength =
+	bssid->SignalStrength =
 		precv_frame->attrib.phy_info.SignalStrength;/* in percentage */
 
 	/*  checking SSID */
@@ -4293,6 +4295,7 @@ static struct wlan_bssid_ex *collect_bss_info(struct rtw_adapter *padapter,
 				     bssid->IELength);
 		if (p && p[1] > 0) {
 			struct ieee80211_ht_cap *pHT_caps;
+
 			pHT_caps = (struct ieee80211_ht_cap *)(p + 2);
 
 			if (pHT_caps->cap_info &
@@ -4305,7 +4308,7 @@ static struct wlan_bssid_ex *collect_bss_info(struct rtw_adapter *padapter,
 
 	/*  mark bss info receiving from nearby channel as SignalQuality 101 */
 	if (bssid->DSConfig != rtw_get_oper_ch23a(padapter))
-		bssid->PhyInfo.SignalQuality = 101;
+		bssid->SignalQuality = 101;
 
 	return bssid;
 fail:
@@ -4319,6 +4322,7 @@ static void start_create_ibss(struct rtw_adapter *padapter)
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
+
 	pmlmeext->cur_channel = (u8)pnetwork->DSConfig;
 	pmlmeinfo->bcn_interval = pnetwork->beacon_interval;
 
@@ -4354,9 +4358,7 @@ static void start_create_ibss(struct rtw_adapter *padapter)
 			report_join_res23a(padapter, 1);
 			pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
 		}
-	}
-	else
-	{
+	} else {
 		DBG_8723A("%s: invalid cap:%x\n", __func__, caps);
 		return;
 	}
@@ -4414,9 +4416,7 @@ static void start_clnt_join(struct rtw_adapter *padapter)
 		pmlmeinfo->state = MSR_ADHOC;
 
 		report_join_res23a(padapter, 1);
-	}
-	else
-	{
+	} else {
 		/* DBG_8723A("marc: invalid cap:%x\n", caps); */
 		return;
 	}
@@ -4480,16 +4480,12 @@ int receive_disconnect23a(struct rtw_adapter *padapter,
 
 	DBG_8723A("%s\n", __func__);
 
-	if ((pmlmeinfo->state&0x03) == MSR_INFRA)
-	{
-		if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
-		{
+	if ((pmlmeinfo->state&0x03) == MSR_INFRA) {
+		if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
 			pmlmeinfo->state = MSR_NOLINK;
 			report_del_sta_event23a(padapter, MacAddr, reason);
 
-		}
-		else if (pmlmeinfo->state & WIFI_FW_LINKING_STATE)
-		{
+		} else if (pmlmeinfo->state & WIFI_FW_LINKING_STATE) {
 			pmlmeinfo->state = MSR_NOLINK;
 			report_join_res23a(padapter, -2);
 		}
@@ -4866,7 +4862,7 @@ void report_join_res23a(struct rtw_adapter *padapter, int res)
 	pjoinbss_evt = (struct joinbss_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
 	memcpy((unsigned char *)&pjoinbss_evt->network.network,
 	       &pmlmeinfo->network, sizeof(struct wlan_bssid_ex));
-	pjoinbss_evt->network.join_res	= pjoinbss_evt->network.aid = res;
+	pjoinbss_evt->network.join_res = res;
 
 	DBG_8723A("report_join_res23a(%d)\n", res);
 
@@ -4995,8 +4991,7 @@ void update_sta_info23a(struct rtw_adapter *padapter, struct sta_info *psta)
 	VCS_update23a(padapter, psta);
 
 	/* HT */
-	if (pmlmepriv->htpriv.ht_option)
-	{
+	if (pmlmepriv->htpriv.ht_option) {
 		psta->htpriv.ht_option = true;
 
 		psta->htpriv.ampdu_enable = pmlmepriv->htpriv.ampdu_enable;
@@ -5006,9 +5001,7 @@ void update_sta_info23a(struct rtw_adapter *padapter, struct sta_info *psta)
 
 		psta->qos_option = true;
 
-	}
-	else
-	{
+	} else {
 		psta->htpriv.ht_option = false;
 
 		psta->htpriv.ampdu_enable = false;
@@ -5050,12 +5043,10 @@ void mlmeext_joinbss_event_callback23a(struct rtw_adapter *padapter,
 		goto exit_mlmeext_joinbss_event_callback23a;
 	}
 
-	if ((pmlmeinfo->state&0x03) == MSR_ADHOC)
-	{
+	if ((pmlmeinfo->state&0x03) == MSR_ADHOC) {
 		/* for bc/mc */
 		psta_bmc = rtw_get_bcmc_stainfo23a(padapter);
-		if (psta_bmc)
-		{
+		if (psta_bmc) {
 			pmlmeinfo->FW_sta_info[psta_bmc->mac_id].psta = psta_bmc;
 			update_bmc_sta_support_rate23a(padapter, psta_bmc->mac_id);
 			Update_RA_Entry23a(padapter, psta_bmc);
@@ -5086,8 +5077,7 @@ void mlmeext_joinbss_event_callback23a(struct rtw_adapter *padapter,
 	set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
 
 	psta = rtw_get_stainfo23a(pstapriv, cur_network->MacAddress);
-	if (psta) /* only for infra. mode */
-	{
+	if (psta) { /* only for infra. mode */
 		pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
 
 		/* DBG_8723A("set_sta_rate23a\n"); */
@@ -5123,8 +5113,7 @@ void mlmeext_sta_add_event_callback23a(struct rtw_adapter *padapter,
 
 	if ((pmlmeinfo->state & 0x03) == MSR_ADHOC) {
 	/* adhoc master or sta_count>1 */
-		if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
-		{
+		if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
 			/* nothing to do */
 		} else { /* adhoc client */
 			/*  correcting TSF */
diff --git a/drivers/staging/rtl8723au/core/rtw_pwrctrl.c b/drivers/staging/rtl8723au/core/rtw_pwrctrl.c
index 1b2af7381d82..e2d51afe522c 100644
--- a/drivers/staging/rtl8723au/core/rtw_pwrctrl.c
+++ b/drivers/staging/rtl8723au/core/rtw_pwrctrl.c
@@ -159,7 +159,7 @@ void rtw_ps_processor23a(struct rtw_adapter *padapter)
 	if (pwrpriv->ips_mode_req == IPS_NONE)
 		goto exit;
 
-	if (rtw_pwr_unassociated_idle(padapter) == false)
+	if (!rtw_pwr_unassociated_idle(padapter))
 		goto exit;
 
 	if (pwrpriv->rf_pwrstate == rf_on &&
@@ -172,12 +172,12 @@ void rtw_ps_processor23a(struct rtw_adapter *padapter)
 exit:
 	rtw_set_pwr_state_check_timer(&padapter->pwrctrlpriv);
 	pwrpriv->ps_processing = false;
-	return;
 }
 
 static void pwr_state_check_handler(unsigned long data)
 {
 	struct rtw_adapter *padapter = (struct rtw_adapter *)data;
+
 	rtw_ps_cmd23a(padapter);
 }
 
@@ -338,8 +338,7 @@ s32 LPS_RF_ON_check23a(struct rtw_adapter *padapter, u32 delay_ms)
 	start_time = jiffies;
 	end_time = start_time + msecs_to_jiffies(delay_ms);
 
-	while (1)
-	{
+	while (1) {
 		bAwake = rtl8723a_get_fwlps_rf_on(padapter);
 		if (bAwake == true)
 			break;
@@ -470,6 +469,7 @@ void rtw_free_pwrctrl_priv(struct rtw_adapter *adapter)
 inline void rtw_set_ips_deny23a(struct rtw_adapter *padapter, u32 ms)
 {
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+
 	pwrpriv->ips_deny_time = jiffies + msecs_to_jiffies(ms);
 }
 
diff --git a/drivers/staging/rtl8723au/core/rtw_recv.c b/drivers/staging/rtl8723au/core/rtw_recv.c
index 5bc7734d9a72..559dddee2648 100644
--- a/drivers/staging/rtl8723au/core/rtw_recv.c
+++ b/drivers/staging/rtl8723au/core/rtw_recv.c
@@ -215,6 +215,7 @@ u32 rtw_free_uc_swdec_pending_queue23a(struct rtw_adapter *adapter)
 {
 	u32 cnt = 0;
 	struct recv_frame *pending_frame;
+
 	while ((pending_frame = rtw_alloc_recvframe23a(&adapter->recvpriv.uc_swdec_pending_queue))) {
 		rtw_free_recvframe23a(pending_frame);
 		DBG_8723A("%s: dequeue uc_swdec_pending_queue\n", __func__);
@@ -239,6 +240,7 @@ int rtw_enqueue_recvbuf23a_to_head(struct recv_buf *precvbuf, struct rtw_queue *
 int rtw_enqueue_recvbuf23a(struct recv_buf *precvbuf, struct rtw_queue *queue)
 {
 	unsigned long irqL;
+
 	spin_lock_irqsave(&queue->lock, irqL);
 
 	list_del_init(&precvbuf->list);
@@ -364,6 +366,7 @@ int recvframe_chkmic(struct rtw_adapter *adapter,
 
 			if (bmic_err == true) {
 				int i;
+
 				RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
 					 ("\n *(pframemic-8)-*(pframemic-1) ="
 					  "0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:"
@@ -483,6 +486,7 @@ struct recv_frame *decryptor(struct rtw_adapter *padapter,
 
 	if (prxattrib->encrypt > 0) {
 		u8 *iv = precv_frame->pkt->data + prxattrib->hdrlen;
+
 		prxattrib->key_index = (((iv[3]) >> 6) & 0x3);
 
 		if (prxattrib->key_index > WEP_KEYS) {
@@ -564,59 +568,27 @@ static struct recv_frame *portctrl(struct rtw_adapter *adapter,
 		 ("########portctrl:adapter->securitypriv.dot11AuthAlgrthm ="
 		  "%d\n", adapter->securitypriv.dot11AuthAlgrthm));
 
+	prtnframe = precv_frame;
+
 	if (auth_alg == dot11AuthAlgrthm_8021X) {
 		/* get ether_type */
 		ptr = pfhdr->pkt->data + pfhdr->attrib.hdrlen;
 
 		ether_type = (ptr[6] << 8) | ptr[7];
 
-		if ((psta != NULL) && (psta->ieee8021x_blocked)) {
+		if (psta && psta->ieee8021x_blocked) {
 			/* blocked */
 			/* only accept EAPOL frame */
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
 				 ("########portctrl:psta->ieee8021x_blocked =="
 				  "1\n"));
 
-			if (ether_type == eapol_type) {
-				prtnframe = precv_frame;
-			} else {
+			if (ether_type != eapol_type) {
 				/* free this frame */
 				rtw_free_recvframe23a(precv_frame);
 				prtnframe = NULL;
 			}
-		} else {
-			/* allowed */
-			/* check decryption status, and decrypt the frame if needed */
-			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-				 ("########portctrl:psta->ieee8021x_blocked =="
-				  "0\n"));
-			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-				 ("portctrl:precv_frame->hdr.attrib.privacy ="
-				  "%x\n", precv_frame->attrib.privacy));
-
-			if (pattrib->bdecrypted == 0) {
-				RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-					 ("portctrl:prxstat->decrypted =%x\n",
-					  pattrib->bdecrypted));
-			}
-
-			prtnframe = precv_frame;
-			/* check is the EAPOL frame or not (Rekey) */
-			if (ether_type == eapol_type) {
-				RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,
-					 ("########portctrl:ether_type == "
-					  "0x888e\n"));
-				/* check Rekey */
-
-				prtnframe = precv_frame;
-			} else {
-				RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-					 ("########portctrl:ether_type = 0x%04x"
-					  "\n", ether_type));
-			}
 		}
-	} else {
-		prtnframe = precv_frame;
 	}
 
 	return prtnframe;
@@ -1066,6 +1038,7 @@ int sta2ap_data_frame(struct rtw_adapter *adapter,
 		}
 	} else {
 		u8 *myhwaddr = myid(&adapter->eeprompriv);
+
 		if (!ether_addr_equal(pattrib->ra, myhwaddr)) {
 			ret = RTW_RX_HANDLED;
 			goto exit;
@@ -1405,8 +1378,7 @@ static int validate_recv_data_frame(struct rtw_adapter *adapter,
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
 			 ("\n pattrib->encrypt =%d\n", pattrib->encrypt));
 
-		switch (pattrib->encrypt)
-		{
+		switch (pattrib->encrypt) {
 		case WLAN_CIPHER_SUITE_WEP40:
 		case WLAN_CIPHER_SUITE_WEP104:
 			pattrib->iv_len = IEEE80211_WEP_IV_LEN;
@@ -1505,8 +1477,7 @@ static int validate_recv_frame(struct rtw_adapter *adapter,
 	if (unlikely(bDumpRxPkt == 1))
 		dump_rx_pkt(skb, type, bDumpRxPkt);
 
-	switch (type)
-	{
+	switch (type) {
 	case IEEE80211_FTYPE_MGMT:
 		retval = validate_recv_mgnt_frame(adapter, precv_frame);
 		if (retval == _FAIL) {
@@ -1524,7 +1495,6 @@ static int validate_recv_frame(struct rtw_adapter *adapter,
 		retval = _FAIL; /*  only data frame return _SUCCESS */
 		break;
 	case IEEE80211_FTYPE_DATA:
-		rtw_led_control(adapter, LED_CTL_RX);
 		pattrib->qos = (subtype & IEEE80211_STYPE_QOS_DATA) ? 1 : 0;
 		retval = validate_recv_data_frame(adapter, precv_frame);
 		if (retval == _FAIL) {
@@ -1551,8 +1521,6 @@ static int wlanhdr_to_ethhdr (struct recv_frame *precvframe)
 	u16	eth_type, len, hdrlen;
 	u8	bsnaphdr;
 	u8	*psnap;
-
-	int ret = _SUCCESS;
 	struct rtw_adapter *adapter = precvframe->adapter;
 	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
 
@@ -1613,7 +1581,7 @@ static int wlanhdr_to_ethhdr (struct recv_frame *precvframe)
 	}
 
 
-	return ret;
+	return _SUCCESS;
 }
 
 /* perform defrag */
@@ -1691,7 +1659,7 @@ struct recv_frame *recvframe_defrag(struct rtw_adapter *adapter,
 		skb_put(skb, pnfhdr->pkt->len);
 
 		prframe->attrib.icv_len = pnfhdr->attrib.icv_len;
-	};
+	}
 
 	/* free the defrag_q queue and return the prframe */
 	rtw_free_recvframe23a_queue(defrag_q);
@@ -2177,8 +2145,7 @@ int process_recv_indicatepkts(struct rtw_adapter *padapter,
 				return retval;
 			}
 		}
-	} else /* B/G mode */
-	{
+	} else { /* B/G mode */
 		retval = wlanhdr_to_ethhdr(prframe);
 		if (retval != _SUCCESS) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
@@ -2238,8 +2205,6 @@ static int recv_func_posthandle(struct rtw_adapter *padapter,
 	struct recv_priv *precvpriv = &padapter->recvpriv;
 
 	/*  DATA FRAME */
-	rtw_led_control(padapter, LED_CTL_RX);
-
 	prframe = decryptor(padapter, prframe);
 	if (prframe == NULL) {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
@@ -2349,66 +2314,52 @@ void rtw_signal_stat_timer_hdl23a(unsigned long data)
 	u8 _alpha = 3;	/* this value is based on converging_constant = 5000 */
 			/* and sampling_interval = 1000 */
 
-	if (adapter->recvpriv.is_signal_dbg) {
-		/* update the user specific value, signal_strength_dbg, */
-		/* to signal_strength, rssi */
-		adapter->recvpriv.signal_strength =
-			adapter->recvpriv.signal_strength_dbg;
-		adapter->recvpriv.rssi =
-			(s8)translate_percentage_to_dbm((u8)adapter->recvpriv.signal_strength_dbg);
-	} else {
-		if (recvpriv->signal_strength_data.update_req == 0) {
-			/*  update_req is clear, means we got rx */
-			avg_signal_strength =
-				recvpriv->signal_strength_data.avg_val;
-			num_signal_strength =
-				recvpriv->signal_strength_data.total_num;
-			/*  after avg_vals are acquired, we can re-stat */
-			/* the signal values */
-			recvpriv->signal_strength_data.update_req = 1;
-		}
+	if (recvpriv->signal_strength_data.update_req == 0) {
+		/*  update_req is clear, means we got rx */
+		avg_signal_strength = recvpriv->signal_strength_data.avg_val;
+		num_signal_strength = recvpriv->signal_strength_data.total_num;
+		/*  after avg_vals are acquired, we can re-stat */
+		/* the signal values */
+		recvpriv->signal_strength_data.update_req = 1;
+	}
+
+	if (recvpriv->signal_qual_data.update_req == 0) {
+		/*  update_req is clear, means we got rx */
+		avg_signal_qual = recvpriv->signal_qual_data.avg_val;
+		num_signal_qual = recvpriv->signal_qual_data.total_num;
+		/*  after avg_vals are acquired, we can re-stat */
+		/*the signal values */
+		recvpriv->signal_qual_data.update_req = 1;
+	}
+
+	/* update value of signal_strength, rssi, signal_qual */
+	if (!check_fwstate(&adapter->mlmepriv, _FW_UNDER_SURVEY)) {
+		tmp_s = (avg_signal_strength + (_alpha - 1) *
+			 recvpriv->signal_strength);
+		if (tmp_s %_alpha)
+			tmp_s = tmp_s / _alpha + 1;
+		else
+			tmp_s = tmp_s / _alpha;
+		if (tmp_s > 100)
+			tmp_s = 100;
 
-		if (recvpriv->signal_qual_data.update_req == 0) {
-			/*  update_req is clear, means we got rx */
-			avg_signal_qual = recvpriv->signal_qual_data.avg_val;
-			num_signal_qual = recvpriv->signal_qual_data.total_num;
-			/*  after avg_vals are acquired, we can re-stat */
-			/*the signal values */
-			recvpriv->signal_qual_data.update_req = 1;
-		}
+		tmp_q = avg_signal_qual + (_alpha - 1) * recvpriv->signal_qual;
+		if (tmp_q %_alpha)
+			tmp_q = tmp_q / _alpha + 1;
+		else
+			tmp_q = tmp_q / _alpha;
+		if (tmp_q > 100)
+			tmp_q = 100;
 
-		/* update value of signal_strength, rssi, signal_qual */
-		if (!check_fwstate(&adapter->mlmepriv, _FW_UNDER_SURVEY)) {
-			tmp_s = (avg_signal_strength + (_alpha - 1) *
-				 recvpriv->signal_strength);
-			if (tmp_s %_alpha)
-				tmp_s = tmp_s / _alpha + 1;
-			else
-				tmp_s = tmp_s / _alpha;
-			if (tmp_s > 100)
-				tmp_s = 100;
-
-			tmp_q = (avg_signal_qual + (_alpha - 1) *
-				 recvpriv->signal_qual);
-			if (tmp_q %_alpha)
-				tmp_q = tmp_q / _alpha + 1;
-			else
-				tmp_q = tmp_q / _alpha;
-			if (tmp_q > 100)
-				tmp_q = 100;
-
-			recvpriv->signal_strength = tmp_s;
-			recvpriv->rssi = (s8)translate_percentage_to_dbm(tmp_s);
-			recvpriv->signal_qual = tmp_q;
-
-			DBG_8723A("%s signal_strength:%3u, rssi:%3d, "
-				  "signal_qual:%3u, num_signal_strength:%u, "
-				  "num_signal_qual:%u\n",
-				  __func__, recvpriv->signal_strength,
-				  recvpriv->rssi, recvpriv->signal_qual,
-				  num_signal_strength, num_signal_qual
-			);
-		}
+		recvpriv->signal_strength = tmp_s;
+		recvpriv->signal_qual = tmp_q;
+
+		DBG_8723A("%s signal_strength:%3u, signal_qual:%3u, "
+			  "num_signal_strength:%u, num_signal_qual:%u\n",
+			  __func__, recvpriv->signal_strength,
+			  recvpriv->signal_qual, num_signal_strength,
+			  num_signal_qual);
 	}
+
 	rtw_set_signal_stat_timer(recvpriv);
 }
diff --git a/drivers/staging/rtl8723au/core/rtw_security.c b/drivers/staging/rtl8723au/core/rtw_security.c
index 76371ae69377..715a47414bdd 100644
--- a/drivers/staging/rtl8723au/core/rtw_security.c
+++ b/drivers/staging/rtl8723au/core/rtw_security.c
@@ -23,19 +23,18 @@
 
 #define CRC32_POLY 0x04c11db7
 
-struct arc4context
-{
+struct arc4context {
 	u32 x;
 	u32 y;
 	u8 state[256];
 };
 
-static void arcfour_init(struct arc4context	*parc4ctx, u8 * key, u32	key_len)
+static void arcfour_init(struct arc4context *parc4ctx, u8 *key, u32 key_len)
 {
 	u32	t, u;
 	u32	keyindex;
 	u32	stateindex;
-	u8 * state;
+	u8 *state;
 	u32	counter;
 
 	state = parc4ctx->state;
@@ -45,8 +44,7 @@ static void arcfour_init(struct arc4context	*parc4ctx, u8 * key, u32	key_len)
 		state[counter] = (u8)counter;
 	keyindex = 0;
 	stateindex = 0;
-	for (counter = 0; counter < 256; counter++)
-	{
+	for (counter = 0; counter < 256; counter++) {
 		t = state[counter];
 		stateindex = (stateindex + key[keyindex] + t) & 0xff;
 		u = state[stateindex];
@@ -62,7 +60,7 @@ static u32 arcfour_byte(	struct arc4context	*parc4ctx)
 	u32 x;
 	u32 y;
 	u32 sx, sy;
-	u8 * state;
+	u8 *state;
 
 	state = parc4ctx->state;
 	x = (parc4ctx->x + 1) & 0xff;
@@ -78,8 +76,8 @@ static u32 arcfour_byte(	struct arc4context	*parc4ctx)
 }
 
 static void arcfour_encrypt(	struct arc4context	*parc4ctx,
-	u8 * dest,
-	u8 * src,
+	u8 *dest,
+	u8 *src,
 	u32 len)
 {
 	u32	i;
@@ -114,8 +112,7 @@ static void crc32_init(void)
 
 		c = 0x12340000;
 
-		for (i = 0; i < 256; ++i)
-		{
+		for (i = 0; i < 256; ++i) {
 			k = crc32_reverseBit((u8)i);
 			for (c = ((u32)k) << 24, j = 8; j > 0; --j) {
 				c = c & 0x80000000 ? (c << 1) ^ CRC32_POLY : (c << 1);
@@ -221,7 +218,7 @@ void rtw_wep_decrypt23a(struct rtw_adapter *padapter,
 	u8 keyindex;
 	struct rx_pkt_attrib *prxattrib = &precvframe->attrib;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
-	struct sk_buff * skb = precvframe->pkt;
+	struct sk_buff *skb = precvframe->pkt;
 
 	pframe = skb->data;
 
@@ -260,33 +257,29 @@ void rtw_wep_decrypt23a(struct rtw_adapter *padapter,
 			  crc[1], payload[length - 3],
 			  crc[0], payload[length - 4]));
 	}
-
-	return;
 }
 
 /* 3		===== TKIP related ===== */
 
-static u32 secmicgetuint32(u8 * p)
+static u32 secmicgetuint32(u8 *p)
 /*  Convert from Byte[] to u32 in a portable way */
 {
 	s32 i;
 	u32 res = 0;
 
-	for (i = 0; i<4; i++)
-	{
+	for (i = 0; i<4; i++) {
 		res |= ((u32)(*p++)) << (8*i);
 	}
 
 	return res;
 }
 
-static void secmicputuint32(u8 * p, u32 val)
+static void secmicputuint32(u8 *p, u32 val)
 /*  Convert from long to Byte[] in a portable way */
 {
 	long i;
 
-	for (i = 0; i<4; i++)
-	{
+	for (i = 0; i<4; i++) {
 		*p++ = (u8) (val & 0xff);
 		val >>= 8;
 	}
@@ -304,7 +297,7 @@ static void secmicclear(struct mic_data *pmicdata)
 
 }
 
-void rtw_secmicsetkey23a(struct mic_data *pmicdata, u8 * key)
+void rtw_secmicsetkey23a(struct mic_data *pmicdata, u8 *key)
 {
 	/*  Set the key */
 
@@ -322,8 +315,7 @@ void rtw_secmicappend23abyte23a(struct mic_data *pmicdata, u8 b)
 	pmicdata->M |= ((unsigned long)b) << (8*pmicdata->nBytesInM);
 	pmicdata->nBytesInM++;
 	/*  Process the word if it is full. */
-	if (pmicdata->nBytesInM >= 4)
-	{
+	if (pmicdata->nBytesInM >= 4) {
 		pmicdata->L ^= pmicdata->M;
 		pmicdata->R ^= ROL32(pmicdata->L, 17);
 		pmicdata->L += pmicdata->R;
@@ -340,19 +332,18 @@ void rtw_secmicappend23abyte23a(struct mic_data *pmicdata, u8 b)
 
 }
 
-void rtw_secmicappend23a(struct mic_data *pmicdata, u8 * src, u32 nbytes)
+void rtw_secmicappend23a(struct mic_data *pmicdata, u8 *src, u32 nbytes)
 {
 
 	/*  This is simple */
-	while(nbytes > 0)
-	{
+	while(nbytes > 0) {
 		rtw_secmicappend23abyte23a(pmicdata, *src++);
 		nbytes--;
 	}
 
 }
 
-void rtw_secgetmic23a(struct mic_data *pmicdata, u8 * dst)
+void rtw_secgetmic23a(struct mic_data *pmicdata, u8 *dst)
 {
 
 	/*  Append the minimum padding */
@@ -362,8 +353,7 @@ void rtw_secgetmic23a(struct mic_data *pmicdata, u8 * dst)
 	rtw_secmicappend23abyte23a(pmicdata, 0);
 	rtw_secmicappend23abyte23a(pmicdata, 0);
 	/*  and then zeroes until the length is a multiple of 4 */
-	while(pmicdata->nBytesInM != 0)
-	{
+	while(pmicdata->nBytesInM != 0) {
 		rtw_secmicappend23abyte23a(pmicdata, 0);
 	}
 	/*  The appendByte function has already computed the result. */
@@ -374,7 +364,8 @@ void rtw_secgetmic23a(struct mic_data *pmicdata, u8 * dst)
 
 }
 
-void rtw_seccalctkipmic23a(u8 * key, u8 *header, u8 *data, u32 data_len, u8 *mic_code, u8 pri)
+void rtw_seccalctkipmic23a(u8 *key, u8 *header, u8 *data, u32 data_len,
+			   u8 *mic_code, u8 pri)
 {
 
 	struct mic_data	micdata;
@@ -531,8 +522,8 @@ static void phase1(u16 *p1k, const u8 *tk, const u8 *ta, u32 iv32)
 
 	/* Now compute an unbalanced Feistel cipher with 80-bit block */
 	/* size on the 80-bit block P1K[], using the 128-bit key TK[] */
-	for (i = 0; i < PHASE1_LOOP_CNT ;i++)
-	{                 /* Each add operation here is mod 2**16 */
+	for (i = 0; i < PHASE1_LOOP_CNT ;i++) {
+		/* Each add operation here is mod 2**16 */
 		p1k[0] += _S_(p1k[4] ^ TK16((i&1)+0));
 		p1k[1] += _S_(p1k[0] ^ TK16((i&1)+2));
 		p1k[2] += _S_(p1k[1] ^ TK16((i&1)+4));
@@ -602,8 +593,7 @@ static void phase2(u8 *rc4key, const u8 *tk, const u16 *p1k, u16 iv16)
 	rc4key[3] = Lo8((PPK[5] ^ TK16(0)) >> 1);
 
 	/* Copy 96 bits of PPK[0..5] to RC4KEY[4..15]  (little-endian)       */
-	for (i = 0;i<6;i++)
-	{
+	for (i = 0;i<6;i++) {
 		rc4key[4+2*i] = Lo8(PPK[i]);
 		rc4key[5+2*i] = Hi8(PPK[i]);
 	}
@@ -649,8 +639,7 @@ int rtw_tkip_encrypt23a(struct rtw_adapter *padapter,
 
 		if (stainfo!= NULL) {
 
-			if (!(stainfo->state &_FW_LINKED))
-			{
+			if (!(stainfo->state &_FW_LINKED)) {
 				DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state);
 				return _FAIL;
 			}
@@ -728,7 +717,7 @@ int rtw_tkip_decrypt23a(struct rtw_adapter *padapter,
 	struct	sta_info		*stainfo;
 	struct	rx_pkt_attrib *prxattrib = &precvframe->attrib;
 	struct	security_priv *psecuritypriv = &padapter->securitypriv;
-	struct sk_buff * skb = precvframe->pkt;
+	struct sk_buff *skb = precvframe->pkt;
 	int res = _SUCCESS;
 
 	pframe = skb->data;
@@ -887,8 +876,7 @@ static void byte_sub(u8 *in, u8 *out)
 {
 	int i;
 
-	for (i = 0; i< 16; i++)
-	{
+	for (i = 0; i< 16; i++) {
 		out[i] = sbox(in[i]);
 	}
 
@@ -928,8 +916,7 @@ static void mix_column(u8 *in, u8 *out)
 	u8 temp[4];
 	u8 tempb[4];
 
-	for (i = 0 ; i<4; i++)
-	{
+	for (i = 0 ; i<4; i++) {
 		if ((in[i] & 0x80) == 0x80)
 		    add1b[i] = 0x1b;
 		else
@@ -951,11 +938,9 @@ static void mix_column(u8 *in, u8 *out)
 	andf7[2] = in[2] & 0x7f;
 	andf7[3] = in[3] & 0x7f;
 
-	for (i = 3; i>0; i--)    /* logical shift left 1 bit */
-	{
+	for (i = 3; i>0; i--) { /* logical shift left 1 bit */
 		andf7[i] = andf7[i] << 1;
-		if ((andf7[i-1] & 0x80) == 0x80)
-		{
+		if ((andf7[i-1] & 0x80) == 0x80) {
 		    andf7[i] = (andf7[i] | 0x01);
 		}
 	}
@@ -988,21 +973,15 @@ static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext)
 
 	for (i = 0; i<16; i++) round_key[i] = key[i];
 
-	for (round = 0; round < 11; round++)
-	{
-		if (round == 0)
-		{
+	for (round = 0; round < 11; round++) {
+		if (round == 0) {
 		    xor_128(round_key, data, ciphertext);
 		    next_key(round_key, round);
-		}
-		else if (round == 10)
-		{
+		} else if (round == 10) {
 		    byte_sub(ciphertext, intermediatea);
 		    shift_row(intermediatea, intermediateb);
 		    xor_128(intermediateb, round_key, ciphertext);
-		}
-		else    /* 1 - 9 */
-		{
+		} else { /* 1 - 9 */
 		    byte_sub(ciphertext, intermediatea);
 		    shift_row(intermediatea, intermediateb);
 		    mix_column(&intermediateb[0], &intermediatea[0]);
@@ -1088,20 +1067,17 @@ static void construct_mic_header2(u8 *mic_header2, u8 *mpdu, int a4_exists,
 	mic_header2[6] = 0x00;
 	mic_header2[7] = 0x00; /* mpdu[23]; */
 
-	if (!qc_exists && a4_exists)
-	{
+	if (!qc_exists && a4_exists) {
 		for (i = 0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
 
 	}
 
-	if (qc_exists && !a4_exists)
-	{
+	if (qc_exists && !a4_exists) {
 		mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
 		mic_header2[9] = mpdu[25] & 0x00;
 	}
 
-	if (qc_exists && a4_exists)
-	{
+	if (qc_exists && a4_exists) {
 		for (i = 0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
 
 		mic_header2[14] = mpdu[30] & 0x0f;
diff --git a/drivers/staging/rtl8723au/core/rtw_sreset.c b/drivers/staging/rtl8723au/core/rtw_sreset.c
index 58ed980795a6..29a29d92a6ac 100644
--- a/drivers/staging/rtl8723au/core/rtw_sreset.c
+++ b/drivers/staging/rtl8723au/core/rtw_sreset.c
@@ -107,7 +107,7 @@ static void sreset_restore_network_station(struct rtw_adapter *padapter)
 
 	mlmeext_joinbss_event_callback23a(padapter, 1);
 	/* restore Sequence No. */
-	rtl8723au_write8(padapter, 0x4dc, padapter->xmitpriv.nqos_ssn);
+	rtl8723au_write8(padapter, REG_NQOS_SEQ, padapter->xmitpriv.nqos_ssn);
 
 	sreset_restore_security_station(padapter);
 }
diff --git a/drivers/staging/rtl8723au/core/rtw_wlan_util.c b/drivers/staging/rtl8723au/core/rtw_wlan_util.c
index 09c44a55d4a6..69d9e0f17fd8 100644
--- a/drivers/staging/rtl8723au/core/rtw_wlan_util.c
+++ b/drivers/staging/rtl8723au/core/rtw_wlan_util.c
@@ -608,8 +608,6 @@ void WMMOnAssocRsp23a(struct rtw_adapter *padapter)
 		DBG_8723A("wmm_para_seq(%d): %d\n", i,
 			  pxmitpriv->wmm_para_seq[i]);
 	}
-
-	return;
 }
 
 static void bwmode_update_check(struct rtw_adapter *padapter, const u8 *p)
@@ -750,7 +748,6 @@ void HT_caps_handler23a(struct rtw_adapter *padapter, const u8 *p)
 		else
 			cap->mcs.rx_mask[i] &= MCS_rate_2R23A[i];
 	}
-	return;
 }
 
 void HT_info_handler23a(struct rtw_adapter *padapter, const u8 *p)
@@ -771,7 +768,6 @@ void HT_info_handler23a(struct rtw_adapter *padapter, const u8 *p)
 
 	pmlmeinfo->HT_info_enable = 1;
 	memcpy(&pmlmeinfo->HT_info, p + 2, p[1]);
-	return;
 }
 
 void HTOnAssocRsp23a(struct rtw_adapter *padapter)
@@ -833,7 +829,7 @@ void VCS_update23a(struct rtw_adapter *padapter, struct sta_info *psta)
 		psta->cts2self = 0;
 		break;
 	case 1: /* on */
-		if (pregpriv->vcs_type == 1) { /* 1:RTS/CTS 2:CTS to self */
+		if (pregpriv->vcs_type == RTS_CTS) {
 			psta->rtsen = 1;
 			psta->cts2self = 0;
 		} else {
@@ -844,7 +840,7 @@ void VCS_update23a(struct rtw_adapter *padapter, struct sta_info *psta)
 	case 2: /* auto */
 	default:
 		if (pmlmeinfo->ERP_enable && pmlmeinfo->ERP_IE & BIT(1)) {
-			if (pregpriv->vcs_type == 1) {
+			if (pregpriv->vcs_type == RTS_CTS) {
 				psta->rtsen = 1;
 				psta->cts2self = 0;
 			} else {
@@ -870,7 +866,7 @@ int rtw_check_bcn_info23a(struct rtw_adapter *Adapter,
 	int pie_len, ssid_len, privacy;
 	const u8 *p, *ssid;
 
-	if (is_client_associated_to_ap23a(Adapter) == false)
+	if (!is_client_associated_to_ap23a(Adapter))
 		return _SUCCESS;
 
 	if (unlikely(!ieee80211_is_beacon(mgmt->frame_control))) {
@@ -1080,7 +1076,7 @@ bool is_ap_in_tkip23a(struct rtw_adapter *padapter)
 		return false;
 }
 
-bool should_forbid_n_rate23a(struct rtw_adapter * padapter)
+bool should_forbid_n_rate23a(struct rtw_adapter *padapter)
 {
 	u32 i;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
@@ -1153,6 +1149,7 @@ bool is_ap_in_wep23a(struct rtw_adapter *padapter)
 static int wifirate2_ratetbl_inx23a(unsigned char rate)
 {
 	int inx = 0;
+
 	rate = rate & 0x7f;
 
 	switch (rate) {
@@ -1311,6 +1308,7 @@ unsigned char check_assoc_AP23a(u8 *pframe, uint len)
 	u8 epigram_vendor_flag;
 	u8 ralink_vendor_flag;
 	const u8 *p;
+
 	epigram_vendor_flag = 0;
 	ralink_vendor_flag = 0;
 
@@ -1324,7 +1322,6 @@ unsigned char check_assoc_AP23a(u8 *pframe, uint len)
 				DBG_8723A("link to Artheros AP\n");
 				return HT_IOT_PEER_ATHEROS;
 			} else if (!memcmp(p + 2, BROADCOM_OUI1, 3) ||
-				   !memcmp(p + 2, BROADCOM_OUI2, 3) ||
 				   !memcmp(p + 2, BROADCOM_OUI2, 3)) {
 				DBG_8723A("link to Broadcom AP\n");
 				return HT_IOT_PEER_BROADCOM;
diff --git a/drivers/staging/rtl8723au/core/rtw_xmit.c b/drivers/staging/rtl8723au/core/rtw_xmit.c
index 7a8038156cea..7a5e6bf0d1ae 100644
--- a/drivers/staging/rtl8723au/core/rtw_xmit.c
+++ b/drivers/staging/rtl8723au/core/rtw_xmit.c
@@ -22,9 +22,6 @@
 #include <usb_ops.h>
 #include <rtl8723a_xmit.h>
 
-static u8 P802_1H_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0xf8 };
-static u8 RFC1042_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 };
-
 static void _init_txservq(struct tx_servq *ptxservq)
 {
 
@@ -180,16 +177,8 @@ int _rtw_init_xmit_priv23a(struct xmit_priv *pxmitpriv,
 	for (i = 0; i < 4; i ++)
 		pxmitpriv->wmm_para_seq[i] = i;
 
-	pxmitpriv->txirp_cnt = 1;
-
 	sema_init(&pxmitpriv->tx_retevt, 0);
 
-	/* per AC pending irp */
-	pxmitpriv->beq_cnt = 0;
-	pxmitpriv->bkq_cnt = 0;
-	pxmitpriv->viq_cnt = 0;
-	pxmitpriv->voq_cnt = 0;
-
 	pxmitpriv->ack_tx = false;
 	mutex_init(&pxmitpriv->ack_tx_mutex);
 	rtw_sctx_init23a(&pxmitpriv->ack_tx_ops, 0);
@@ -315,6 +304,7 @@ static void update_attrib_vcs_info(struct rtw_adapter *padapter, struct xmit_fra
 			/* check HT op mode */
 			if (pattrib->ht_en) {
 				u8 HTOpMode = pmlmeinfo->HT_protection;
+
 				if ((pmlmeext->cur_bwmode && (HTOpMode == 2 || HTOpMode == 3)) ||
 				    (!pmlmeext->cur_bwmode && HTOpMode == 3)) {
 					pattrib->vcs_mode = RTS_CTS;
@@ -464,6 +454,7 @@ static int update_attrib(struct rtw_adapter *padapter,
 		if (pattrib->pktlen > 282 + 24) {
 			if (pattrib->ether_type == ETH_P_IP) {/*  IP header */
 				u8 *pframe = skb->data;
+
 				pframe += ETH_HLEN;
 
 				if ((pframe[21] == 68 && pframe[23] == 67) ||
@@ -1048,21 +1039,23 @@ s32 rtw_txframes_sta_ac_pending23a(struct rtw_adapter *padapter,
 	return ptxservq->qcnt;
 }
 
-/*
- * Calculate wlan 802.11 packet MAX size from pkt_attrib
- * This function doesn't consider fragment case
+/* Logical Link Control(LLC) SubNetwork Attachment Point(SNAP) header
+ * IEEE LLC/SNAP header contains 8 octets
+ * First 3 octets comprise the LLC portion
+ * SNAP portion, 5 octets, is divided into two fields:
+ *	Organizationally Unique Identifier(OUI), 3 octets,
+ *	type, defined by that organization, 2 octets.
  */
-u32 rtw_calculate_wlan_pkt_size_by_attribue23a(struct pkt_attrib *pattrib)
+static int rtw_put_snap(u8 *data, u16 h_proto)
 {
-	u32	len = 0;
-
-	len = pattrib->hdrlen + pattrib->iv_len; /*  WLAN Header and IV */
-	len += SNAP_SIZE + sizeof(u16); /*  LLC */
-	len += pattrib->pktlen;
-	if (pattrib->encrypt == WLAN_CIPHER_SUITE_TKIP) len += 8; /*  MIC */
-	len += ((pattrib->bswenc) ? pattrib->icv_len : 0); /*  ICV */
+	if (h_proto == ETH_P_IPX || h_proto == ETH_P_AARP)
+		ether_addr_copy(data, bridge_tunnel_header);
+	else
+		ether_addr_copy(data, rfc1042_header);
 
-	return len;
+	data += ETH_ALEN;
+	put_unaligned_be16(h_proto, data);
+	return ETH_ALEN + sizeof(u16);
 }
 
 /*
@@ -1188,7 +1181,7 @@ int rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *skb,
 			mpdu_len -= pattrib->iv_len;
 		}
 		if (frg_inx == 0) {
-			llc_sz = rtw_put_snap23a(pframe, pattrib->ether_type);
+			llc_sz = rtw_put_snap(pframe, pattrib->ether_type);
 			pframe += llc_sz;
 			mpdu_len -= llc_sz;
 		}
@@ -1258,34 +1251,6 @@ exit:
 	return res;
 }
 
-/* Logical Link Control(LLC) SubNetwork Attachment Point(SNAP) header
- * IEEE LLC/SNAP header contains 8 octets
- * First 3 octets comprise the LLC portion
- * SNAP portion, 5 octets, is divided into two fields:
- *	Organizationally Unique Identifier(OUI), 3 octets,
- *	type, defined by that organization, 2 octets.
- */
-s32 rtw_put_snap23a(u8 *data, u16 h_proto)
-{
-	struct ieee80211_snap_hdr *snap;
-	u8 *oui;
-
-	snap = (struct ieee80211_snap_hdr *)data;
-	snap->dsap = 0xaa;
-	snap->ssap = 0xaa;
-	snap->ctrl = 0x03;
-
-	if (h_proto == 0x8137 || h_proto == 0x80f3)
-		oui = P802_1H_OUI;
-	else
-		oui = RFC1042_OUI;
-	snap->oui[0] = oui[0];
-	snap->oui[1] = oui[1];
-	snap->oui[2] = oui[2];
-	*(u16 *)(data + SNAP_SIZE) = htons(h_proto);
-	return SNAP_SIZE + sizeof(u16);
-}
-
 void rtw_update_protection23a(struct rtw_adapter *padapter, u8 *ie, uint ie_len)
 {
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
@@ -1293,7 +1258,7 @@ void rtw_update_protection23a(struct rtw_adapter *padapter, u8 *ie, uint ie_len)
 	uint protection;
 	const u8 *p;
 
-	switch (pxmitpriv->vcs_setting) {
+	switch (pregistrypriv->vrtl_carrier_sense) {
 	case DISABLE_VCS:
 		pxmitpriv->vcs = NONE_VCS;
 		break;
@@ -1326,7 +1291,7 @@ void rtw_count_tx_stats23a(struct rtw_adapter *padapter, struct xmit_frame *pxmi
 	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
 	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
 
-	if ((pxmitframe->frame_tag&0x0f) == DATA_FRAMETAG) {
+	if (pxmitframe->frame_tag == DATA_FRAMETAG) {
 		pxmitpriv->tx_bytes += sz;
 		pmlmepriv->LinkDetectInfo.NumTxOkInPeriod++;
 
@@ -1893,18 +1858,6 @@ u32 rtw_get_ff_hwaddr23a(struct xmit_frame *pxmitframe)
 	return addr;
 }
 
-static void do_queue_select(struct rtw_adapter	*padapter, struct pkt_attrib *pattrib)
-{
-	u8 qsel;
-
-	qsel = pattrib->priority;
-	RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
-		 ("### do_queue_select priority =%d , qsel = %d\n",
-		  pattrib->priority, qsel));
-
-	pattrib->qsel = qsel;
-}
-
 /*
  * The main transmit(tx) entry
  *
@@ -1936,9 +1889,7 @@ int rtw_xmit23a(struct rtw_adapter *padapter, struct sk_buff *skb)
 	}
 	pxmitframe->pkt = skb;
 
-	rtw_led_control(padapter, LED_CTL_TX);
-
-	do_queue_select(padapter, &pxmitframe->attrib);
+	pxmitframe->attrib.qsel = pxmitframe->attrib.priority;
 
 #ifdef CONFIG_8723AU_AP_MODE
 	spin_lock_bh(&pxmitpriv->lock);
@@ -2411,11 +2362,6 @@ void rtw23a_sctx_done_err(struct submit_ctx **sctx, int status)
 	}
 }
 
-void rtw_sctx_done23a(struct submit_ctx **sctx)
-{
-	rtw23a_sctx_done_err(sctx, RTW_SCTX_DONE_SUCCESS);
-}
-
 int rtw_ack_tx_wait23a(struct xmit_priv *pxmitpriv, u32 timeout_ms)
 {
 	struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
diff --git a/drivers/staging/rtl8723au/hal/HalDMOutSrc8723A_CE.c b/drivers/staging/rtl8723au/hal/HalDMOutSrc8723A_CE.c
index 4b41bc4ce1e6..179a1ba03029 100644
--- a/drivers/staging/rtl8723au/hal/HalDMOutSrc8723A_CE.c
+++ b/drivers/staging/rtl8723au/hal/HalDMOutSrc8723A_CE.c
@@ -612,7 +612,7 @@ static void _PHY_PathADDAOn(struct rtw_adapter *pAdapter, u32 *ADDAReg, bool isP
 	u32 i;
 
 	pathOn = isPathAOn ? 0x04db25a4 : 0x0b1b25a4;
-	if (false == is2T) {
+	if (!is2T) {
 		pathOn = 0x0bdb25a0;
 		PHY_SetBBReg(pAdapter, ADDAReg[0], bMaskDWord, 0x0b1b25a0);
 	} else {
diff --git a/drivers/staging/rtl8723au/hal/hal_com.c b/drivers/staging/rtl8723au/hal/hal_com.c
index bf919f6e4128..bf4cae20bd12 100644
--- a/drivers/staging/rtl8723au/hal/hal_com.c
+++ b/drivers/staging/rtl8723au/hal/hal_com.c
@@ -463,7 +463,7 @@ void rtl8723a_set_ampdu_factor(struct rtw_adapter *padapter, u8 FactorToSet)
 		MaxAggNum = 0xF;
 
 	if (FactorToSet <= 3) {
-		FactorToSet = (1 << (FactorToSet + 2));
+		FactorToSet = 1 << (FactorToSet + 2);
 		if (FactorToSet > MaxAggNum)
 			FactorToSet = MaxAggNum;
 
@@ -727,7 +727,7 @@ void rtl8723a_fifo_cleanup(struct rtw_adapter *padapter)
 	rtl8723au_write8(padapter, REG_TXPAUSE, 0xff);
 
 	/*  keep sn */
-	padapter->xmitpriv.nqos_ssn = rtl8723au_read16(padapter, REG_NQOS_SEQ);
+	padapter->xmitpriv.nqos_ssn = rtl8723au_read8(padapter, REG_NQOS_SEQ);
 
 	if (pwrpriv->bkeepfwalive != true) {
 		u32 v32;
diff --git a/drivers/staging/rtl8723au/hal/odm_HWConfig.c b/drivers/staging/rtl8723au/hal/odm_HWConfig.c
index 29d844d66cae..fb3cc872f205 100644
--- a/drivers/staging/rtl8723au/hal/odm_HWConfig.c
+++ b/drivers/staging/rtl8723au/hal/odm_HWConfig.c
@@ -391,20 +391,11 @@ static void odm_Process_RSSIForDM(struct dm_odm_t *pDM_Odm,
 	}
 }
 
-/*  Endianness before calling this API */
-static void ODM_PhyStatusQuery23a_92CSeries(struct dm_odm_t *pDM_Odm,
-					 struct phy_info *pPhyInfo,
-					 u8 *pPhyStatus,
-					 struct odm_packet_info *pPktinfo)
+void ODM_PhyStatusQuery23a(struct dm_odm_t *pDM_Odm, struct phy_info *pPhyInfo,
+			   u8 *pPhyStatus, struct odm_packet_info *pPktinfo)
 {
 	odm_RxPhyStatus92CSeries_Parsing(pDM_Odm, pPhyInfo,
 					 pPhyStatus, pPktinfo);
 
 	odm_Process_RSSIForDM(pDM_Odm, pPhyInfo, pPktinfo);
 }
-
-void ODM_PhyStatusQuery23a(struct dm_odm_t *pDM_Odm, struct phy_info *pPhyInfo,
-			   u8 *pPhyStatus, struct odm_packet_info *pPktinfo)
-{
-	ODM_PhyStatusQuery23a_92CSeries(pDM_Odm, pPhyInfo, pPhyStatus, pPktinfo);
-}
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_bt-coexist.c b/drivers/staging/rtl8723au/hal/rtl8723a_bt-coexist.c
index 9054a987f06b..86a83975f4f0 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_bt-coexist.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_bt-coexist.c
@@ -340,7 +340,7 @@ static u8 bthci_GetAssocInfo(struct rtw_adapter *padapter, u8 EntryNum)
 			tempBuf, TotalLen-BaseMemoryShift);
 
 		pAmpAsoc = (struct amp_assoc_structure *)tempBuf;
-		pAmpAsoc->Length = le16_to_cpu(pAmpAsoc->Length);
+		le16_to_cpus(&pAmpAsoc->Length);
 		BaseMemoryShift += 3 + pAmpAsoc->Length;
 
 		RTPRINT(FIOCTL, IOCTL_BT_HCICMD, ("TypeID = 0x%x, ", pAmpAsoc->TypeID));
@@ -1759,18 +1759,6 @@ static enum hci_status bthci_CmdReadConnectionAcceptTimeout(struct rtw_adapter *
 	return status;
 }
 
-/* 7.3.3 */
-static enum hci_status
-bthci_CmdSetEventFilter(
-	struct rtw_adapter *padapter,
-	struct packet_irp_hcicmd_data *pHciCmd
-	)
-{
-	enum hci_status status = HCI_STATUS_SUCCESS;
-
-	return status;
-}
-
 /* 7.3.14 */
 static enum hci_status
 bthci_CmdWriteConnectionAcceptTimeout(
@@ -2982,19 +2970,12 @@ bthci_CmdReadLinkQuality(
 	return status;
 }
 
-static enum hci_status bthci_CmdReadRSSI(struct rtw_adapter *padapter)
-{
-	enum hci_status status = HCI_STATUS_SUCCESS;
-	return status;
-}
-
 static enum hci_status
 bthci_CmdCreateLogicalLink(
 	struct rtw_adapter *padapter,
 	struct packet_irp_hcicmd_data *pHciCmd
 	)
 {
-	enum hci_status status = HCI_STATUS_SUCCESS;
 	struct bt_30info *pBTInfo = GET_BT_INFO(padapter);
 	struct bt_dgb *pBtDbg = &pBTInfo->BtDbg;
 
@@ -3003,7 +2984,7 @@ bthci_CmdCreateLogicalLink(
 	bthci_BuildLogicalLink(padapter, pHciCmd,
 		HCI_CREATE_LOGICAL_LINK);
 
-	return status;
+	return HCI_STATUS_SUCCESS;
 }
 
 static enum hci_status
@@ -3012,7 +2993,6 @@ bthci_CmdAcceptLogicalLink(
 	struct packet_irp_hcicmd_data *pHciCmd
 	)
 {
-	enum hci_status status = HCI_STATUS_SUCCESS;
 	struct bt_30info *pBTInfo = GET_BT_INFO(padapter);
 	struct bt_dgb *pBtDbg = &pBTInfo->BtDbg;
 
@@ -3021,7 +3001,7 @@ bthci_CmdAcceptLogicalLink(
 	bthci_BuildLogicalLink(padapter, pHciCmd,
 		HCI_ACCEPT_LOGICAL_LINK);
 
-	return status;
+	return HCI_STATUS_SUCCESS;
 }
 
 static enum hci_status
@@ -4138,15 +4118,6 @@ bthci_CmdHostBufferSize(struct rtw_adapter *padapter,
 }
 
 static enum hci_status
-bthci_CmdHostNumberOfCompletedPackets(struct rtw_adapter *padapter,
-				      struct packet_irp_hcicmd_data *pHciCmd)
-{
-	enum hci_status status = HCI_STATUS_SUCCESS;
-
-	return status;
-}
-
-static enum hci_status
 bthci_UnknownCMD(struct rtw_adapter *padapter, struct packet_irp_hcicmd_data *pHciCmd)
 {
 	enum hci_status status = HCI_STATUS_UNKNOW_HCI_CMD;
@@ -4219,7 +4190,6 @@ bthci_HandleOGFSetEventMaskCMD(struct rtw_adapter *padapter,
 		break;
 	case HCI_SET_EVENT_FILTER:
 		RTPRINT(FIOCTL, IOCTL_BT_HCICMD, ("HCI_SET_EVENT_FILTER\n"));
-		status = bthci_CmdSetEventFilter(padapter, pHciCmd);
 		break;
 	case HCI_WRITE_CONNECTION_ACCEPT_TIMEOUT:
 		RTPRINT(FIOCTL, IOCTL_BT_HCICMD, ("HCI_WRITE_CONNECTION_ACCEPT_TIMEOUT\n"));
@@ -4235,7 +4205,6 @@ bthci_HandleOGFSetEventMaskCMD(struct rtw_adapter *padapter,
 		break;
 	case HCI_HOST_NUMBER_OF_COMPLETED_PACKETS:
 		RTPRINT(FIOCTL, IOCTL_BT_HCICMD, ("HCI_HOST_NUMBER_OF_COMPLETED_PACKETS\n"));
-		status = bthci_CmdHostNumberOfCompletedPackets(padapter, pHciCmd);
 		break;
 	case HCI_READ_LINK_SUPERVISION_TIMEOUT:
 		RTPRINT(FIOCTL, IOCTL_BT_HCICMD, ("HCI_READ_LINK_SUPERVISION_TIMEOUT\n"));
@@ -4323,7 +4292,6 @@ bthci_HandleOGFStatusParameters(struct rtw_adapter *padapter,
 		break;
 	case HCI_READ_RSSI:
 		RTPRINT(FIOCTL, IOCTL_BT_HCICMD, ("HCI_READ_RSSI\n"));
-		status = bthci_CmdReadRSSI(padapter);
 		break;
 	case HCI_READ_LOCAL_AMP_INFO:
 		RTPRINT(FIOCTL, IOCTL_BT_HCICMD, ("HCI_READ_LOCAL_AMP_INFO\n"));
@@ -10671,7 +10639,7 @@ void BTDM_BBBackOffLevel(struct rtw_adapter *padapter, u8 type)
 
 void BTDM_FWCoexAllOff(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);;
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
 	RTPRINT(FBT, BT_TRACE, ("BTDM_FWCoexAllOff()\n"));
 	if (pHalData->bt_coexist.bFWCoexistAllOff)
@@ -10685,7 +10653,7 @@ void BTDM_FWCoexAllOff(struct rtw_adapter *padapter)
 
 void BTDM_SWCoexAllOff(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);;
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
 	RTPRINT(FBT, BT_TRACE, ("BTDM_SWCoexAllOff()\n"));
 	if (pHalData->bt_coexist.bSWCoexistAllOff)
@@ -10698,7 +10666,7 @@ void BTDM_SWCoexAllOff(struct rtw_adapter *padapter)
 
 void BTDM_HWCoexAllOff(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);;
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
 	RTPRINT(FBT, BT_TRACE, ("BTDM_HWCoexAllOff()\n"));
 	if (pHalData->bt_coexist.bHWCoexistAllOff)
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c b/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c
index 271c33d6ca5a..7b56411cc3c8 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c
@@ -115,19 +115,16 @@ exit:
 
 int rtl8723a_set_rssi_cmd(struct rtw_adapter *padapter, u8 *param)
 {
-	int res = _SUCCESS;
-
 	*((u32 *)param) = cpu_to_le32(*((u32 *)param));
 
 	FillH2CCmd(padapter, RSSI_SETTING_EID, 3, param);
 
-	return res;
+	return _SUCCESS;
 }
 
 int rtl8723a_set_raid_cmd(struct rtw_adapter *padapter, u32 mask, u8 arg)
 {
 	u8 buf[5];
-	int res = _SUCCESS;
 
 	memset(buf, 0, 5);
 	mask = cpu_to_le32(mask);
@@ -136,7 +133,7 @@ int rtl8723a_set_raid_cmd(struct rtw_adapter *padapter, u32 mask, u8 arg)
 
 	FillH2CCmd(padapter, MACID_CONFIG_EID, 5, buf);
 
-	return res;
+	return _SUCCESS;
 }
 
 /* bitmap[0:27] = tx_rate_bitmap */
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c b/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c
index 8523908d5e5f..a5eadd4e2580 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c
@@ -47,29 +47,19 @@ static void _FWDownloadEnable(struct rtw_adapter *padapter, bool enable)
 	}
 }
 
-static int _BlockWrite(struct rtw_adapter *padapter, void *buffer, u32 buffSize)
-{
-	int ret;
-
-	if (buffSize > MAX_PAGE_SIZE)
-		return _FAIL;
-
-	ret = rtl8723au_writeN(padapter, FW_8723A_START_ADDRESS,
-			       buffSize, buffer);
-
-	return ret;
-}
-
 static int
 _PageWrite(struct rtw_adapter *padapter, u32 page, void *buffer, u32 size)
 {
 	u8 value8;
 	u8 u8Page = (u8) (page & 0x07);
 
+	if (size > MAX_PAGE_SIZE)
+		return _FAIL;
+
 	value8 = (rtl8723au_read8(padapter, REG_MCUFWDL + 2) & 0xF8) | u8Page;
 	rtl8723au_write8(padapter, REG_MCUFWDL + 2, value8);
 
-	return _BlockWrite(padapter, buffer, size);
+	return rtl8723au_writeN(padapter, FW_8723A_START_ADDRESS, size, buffer);
 }
 
 static int _WriteFW(struct rtw_adapter *padapter, void *buffer, u32 size)
@@ -743,84 +733,6 @@ u16 rtl8723a_EfuseGetCurrentSize_BT(struct rtw_adapter *padapter)
 	return retU2;
 }
 
-bool
-rtl8723a_EfusePgPacketRead(struct rtw_adapter *padapter, u8 offset, u8 *data)
-{
-	u8 efuse_data, word_cnts = 0;
-	u16 efuse_addr = 0;
-	u8 hoffset = 0, hworden = 0;
-	u8 i;
-	u8 max_section = 0;
-	s32 ret;
-
-	if (data == NULL)
-		return false;
-
-	EFUSE_GetEfuseDefinition23a(padapter, EFUSE_WIFI, TYPE_EFUSE_MAX_SECTION,
-				 &max_section);
-	if (offset > max_section) {
-		DBG_8723A("%s: Packet offset(%d) is illegal(>%d)!\n",
-			  __func__, offset, max_section);
-		return false;
-	}
-
-	memset(data, 0xFF, PGPKT_DATA_SIZE);
-	ret = true;
-
-	/*  */
-	/*  <Roger_TODO> Efuse has been pre-programmed dummy 5Bytes at the
-	    end of Efuse by CP. */
-	/*  Skip dummy parts to prevent unexpected data read from Efuse. */
-	/*  By pass right now. 2009.02.19. */
-	/*  */
-	while (AVAILABLE_EFUSE_ADDR(efuse_addr)) {
-		if (efuse_OneByteRead23a(padapter, efuse_addr++, &efuse_data) ==
-		    _FAIL) {
-			ret = false;
-			break;
-		}
-
-		if (efuse_data == 0xFF)
-			break;
-
-		if (EXT_HEADER(efuse_data)) {
-			hoffset = GET_HDR_OFFSET_2_0(efuse_data);
-			efuse_OneByteRead23a(padapter, efuse_addr++, &efuse_data);
-			if (ALL_WORDS_DISABLED(efuse_data)) {
-				DBG_8723A("%s: Error!! All words disabled!\n",
-					  __func__);
-				continue;
-			}
-
-			hoffset |= ((efuse_data & 0xF0) >> 1);
-			hworden = efuse_data & 0x0F;
-		} else {
-			hoffset = (efuse_data >> 4) & 0x0F;
-			hworden = efuse_data & 0x0F;
-		}
-
-		if (hoffset == offset) {
-			for (i = 0; i < EFUSE_MAX_WORD_UNIT; i++) {
-				/* Check word enable condition in the section */
-				if (!(hworden & (0x01 << i))) {
-					ReadEFuseByte23a(padapter, efuse_addr++,
-						      &efuse_data);
-					data[i * 2] = efuse_data;
-
-					ReadEFuseByte23a(padapter, efuse_addr++,
-						      &efuse_data);
-					data[(i * 2) + 1] = efuse_data;
-				}
-			}
-		} else {
-			word_cnts = Efuse_CalculateWordCnts23a(hworden);
-			efuse_addr += word_cnts * 2;
-		}
-	}
-
-	return ret;
-}
-
 void rtl8723a_read_chip_version(struct rtw_adapter *padapter)
 {
 	u32 value32;
@@ -1126,6 +1038,21 @@ exit:
 	return ret;
 }
 
+void handle_txrpt_ccx_8723a(struct rtw_adapter *adapter, void *buf)
+{
+	struct txrpt_ccx_8723a *txrpt_ccx = buf;
+	struct submit_ctx *pack_tx_ops = &adapter->xmitpriv.ack_tx_ops;
+
+	if (txrpt_ccx->int_ccx && adapter->xmitpriv.ack_tx) {
+		if (txrpt_ccx->pkt_ok)
+			rtw23a_sctx_done_err(&pack_tx_ops,
+					     RTW_SCTX_DONE_SUCCESS);
+		else
+			rtw23a_sctx_done_err(&pack_tx_ops,
+					     RTW_SCTX_DONE_CCX_PKT_FAIL);
+	}
+}
+
 void rtl8723a_InitAntenna_Selection(struct rtw_adapter *padapter)
 {
 	u8 val;
@@ -1326,18 +1253,17 @@ c.	APSD_CTRL 0x600[7:0] = 0x40
 d.	SYS_FUNC_EN 0x02[7:0] = 0x16		reset BB state machine
 e.	SYS_FUNC_EN 0x02[7:0] = 0x14		reset BB state machine
 ***************************************/
-	u8 eRFPath = 0, value8 = 0;
+	u8 value8;
 
 	rtl8723au_write8(padapter, REG_TXPAUSE, 0xFF);
 
-	PHY_SetRFReg(padapter, (enum RF_RADIO_PATH) eRFPath, 0x0, bMaskByte0, 0x0);
+	PHY_SetRFReg(padapter, RF_PATH_A, 0x0, bMaskByte0, 0x0);
 
-	value8 |= APSDOFF;
+	value8 = APSDOFF;
 	rtl8723au_write8(padapter, REG_APSD_CTRL, value8);	/* 0x40 */
 
 	/*  Set BB reset at first */
-	value8 = 0;
-	value8 |= (FEN_USBD | FEN_USBA | FEN_BB_GLB_RSTn);
+	value8 = FEN_USBD | FEN_USBA | FEN_BB_GLB_RSTn;
 	rtl8723au_write8(padapter, REG_SYS_FUNC_EN, value8);	/* 0x16 */
 
 	/*  Set global reset. */
@@ -1350,11 +1276,6 @@ e.	SYS_FUNC_EN 0x02[7:0] = 0x14		reset BB state machine
 /*	RT_TRACE(COMP_INIT, DBG_LOUD, ("======> RF off and reset BB.\n")); */
 }
 
-static void _DisableRFAFEAndResetBB(struct rtw_adapter *padapter)
-{
-	_DisableRFAFEAndResetBB8192C(padapter);
-}
-
 static void _ResetDigitalProcedure1_92C(struct rtw_adapter *padapter,
 					bool bWithoutHWSM)
 {
@@ -1368,18 +1289,18 @@ static void _ResetDigitalProcedure1_92C(struct rtw_adapter *padapter,
 	i.     SYS_FUNC_EN 0x02[10]= 1		enable MCU register,
 						(8051 enable)
 	******************************/
-		u16 valu16 = 0;
+		u16 valu16;
 		rtl8723au_write8(padapter, REG_MCUFWDL, 0);
 
 		valu16 = rtl8723au_read16(padapter, REG_SYS_FUNC_EN);
 		/* reset MCU , 8051 */
 		rtl8723au_write16(padapter, REG_SYS_FUNC_EN,
-				  valu16 & (~FEN_CPUEN));
+				  valu16 & ~FEN_CPUEN);
 
 		valu16 = rtl8723au_read16(padapter, REG_SYS_FUNC_EN) & 0x0FFF;
 		/* reset MAC */
 		rtl8723au_write16(padapter, REG_SYS_FUNC_EN,
-				  valu16 | (FEN_HWPDN | FEN_ELDR));
+				  valu16 | FEN_HWPDN | FEN_ELDR);
 
 		valu16 = rtl8723au_read16(padapter, REG_SYS_FUNC_EN);
 		/* enable MCU , 8051 */
@@ -1387,43 +1308,41 @@ static void _ResetDigitalProcedure1_92C(struct rtw_adapter *padapter,
 				  valu16 | FEN_CPUEN);
 	} else {
 		u8 retry_cnts = 0;
+		u8 val8;
+
+		val8 = rtl8723au_read8(padapter, REG_MCUFWDL);
 
 		/*  2010/08/12 MH For USB SS, we can not stop 8051 when we
 		    are trying to enter IPS/HW&SW radio off. For
 		    S3/S4/S5/Disable, we can stop 8051 because */
 		/*  we will init FW when power on again. */
 		/*  If we want to SS mode, we can not reset 8051. */
-		if (rtl8723au_read8(padapter, REG_MCUFWDL) & BIT(1)) {
+		if ((val8 & BIT(1)) && padapter->bFWReady) {
 			/* IF fw in RAM code, do reset */
-			if (padapter->bFWReady) {
-				/*  2010/08/25 MH Accordign to RD alfred's
-				    suggestion, we need to disable other */
-				/*  HRCV INT to influence 8051 reset. */
-				rtl8723au_write8(padapter, REG_FWIMR, 0x20);
-				/*  2011/02/15 MH According to Alex's
-				    suggestion, close mask to prevent
-				    incorrect FW write operation. */
-				rtl8723au_write8(padapter, REG_FTIMR, 0x00);
-				rtl8723au_write8(padapter, REG_FSIMR, 0x00);
-
-				/* 8051 reset by self */
-				rtl8723au_write8(padapter, REG_HMETFR + 3,
-						 0x20);
-
-				while ((retry_cnts++ < 100) &&
-				       (FEN_CPUEN &
-					rtl8723au_read16(padapter,
-							 REG_SYS_FUNC_EN))) {
-					udelay(50);	/* us */
-				}
+			/*  2010/08/25 MH Accordign to RD alfred's
+			    suggestion, we need to disable other */
+			/*  HRCV INT to influence 8051 reset. */
+			rtl8723au_write8(padapter, REG_FWIMR, 0x20);
+			/*  2011/02/15 MH According to Alex's
+			    suggestion, close mask to prevent
+			    incorrect FW write operation. */
+			rtl8723au_write8(padapter, REG_FTIMR, 0x00);
+			rtl8723au_write8(padapter, REG_FSIMR, 0x00);
+
+			/* 8051 reset by self */
+			rtl8723au_write8(padapter, REG_HMETFR + 3, 0x20);
+
+			while ((retry_cnts++ < 100) &&
+			       (rtl8723au_read16(padapter, REG_SYS_FUNC_EN) &
+				FEN_CPUEN)) {
+				udelay(50);	/* us */
+			}
 
-				if (retry_cnts >= 100) {
-					/* Reset MAC and Enable 8051 */
-					rtl8723au_write8(padapter,
-							 REG_SYS_FUNC_EN + 1,
-							 0x50);
-					mdelay(10);
-				}
+			if (retry_cnts >= 100) {
+				/* Reset MAC and Enable 8051 */
+				rtl8723au_write8(padapter,
+						 REG_SYS_FUNC_EN + 1, 0x50);
+				mdelay(10);
 			}
 		}
 		/* Reset MAC and Enable 8051 */
@@ -1450,12 +1369,6 @@ static void _ResetDigitalProcedure1_92C(struct rtw_adapter *padapter,
 	}
 }
 
-static void _ResetDigitalProcedure1(struct rtw_adapter *padapter,
-				    bool bWithoutHWSM)
-{
-	_ResetDigitalProcedure1_92C(padapter, bWithoutHWSM);
-}
-
 static void _ResetDigitalProcedure2(struct rtw_adapter *padapter)
 {
 /*****************************
@@ -1472,8 +1385,8 @@ m.	SYS_ISO_CTRL 0x01[7:0] = 0x83		isolated ELDR to PON
 static void _DisableAnalog(struct rtw_adapter *padapter, bool bWithoutHWSM)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-	u16 value16 = 0;
-	u8 value8 = 0;
+	u16 value16;
+	u8 value8;
 
 	if (bWithoutHWSM) {
 	/*****************************
@@ -1487,7 +1400,7 @@ static void _DisableAnalog(struct rtw_adapter *padapter, bool bWithoutHWSM)
 		/* rtl8723au_write8(padapter, REG_LDOV12D_CTRL, 0x54); */
 
 		value8 = rtl8723au_read8(padapter, REG_LDOV12D_CTRL);
-		value8 &= (~LDV12_EN);
+		value8 &= ~LDV12_EN;
 		rtl8723au_write8(padapter, REG_LDOV12D_CTRL, value8);
 /*		RT_TRACE(COMP_INIT, DBG_LOUD,
 		(" REG_LDOV12D_CTRL Reg0x21:0x%02x.\n", value8)); */
@@ -1509,9 +1422,9 @@ static void _DisableAnalog(struct rtw_adapter *padapter, bool bWithoutHWSM)
 		    use HW to shut down 8051 automatically. */
 		/*  Becasue suspend operatione need the asistance of 8051
 		    to wait for 3ms. */
-		value16 |= (APDM_HOST | AFSM_HSUS | PFM_ALDN);
+		value16 = APDM_HOST | AFSM_HSUS | PFM_ALDN;
 	} else {
-		value16 |= (APDM_HOST | AFSM_HSUS | PFM_ALDN);
+		value16 = APDM_HOST | AFSM_HSUS | PFM_ALDN;
 	}
 
 	rtl8723au_write16(padapter, REG_APS_FSMCO, value16);	/* 0x4802 */
@@ -1522,16 +1435,14 @@ static void _DisableAnalog(struct rtw_adapter *padapter, bool bWithoutHWSM)
 /*  HW Auto state machine */
 int CardDisableHWSM(struct rtw_adapter *padapter, u8 resetMCU)
 {
-	int rtStatus = _SUCCESS;
-
 	if (padapter->bSurpriseRemoved) {
-		return rtStatus;
+		return _SUCCESS;
 	}
 	/*  RF Off Sequence ==== */
-	_DisableRFAFEAndResetBB(padapter);
+	_DisableRFAFEAndResetBB8192C(padapter);
 
 	/*   ==== Reset digital sequence   ====== */
-	_ResetDigitalProcedure1(padapter, false);
+	_ResetDigitalProcedure1_92C(padapter, false);
 
 	/*   ==== Pull GPIO PIN to balance level and LED control ====== */
 	_DisableGPIO(padapter);
@@ -1542,25 +1453,21 @@ int CardDisableHWSM(struct rtw_adapter *padapter, u8 resetMCU)
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
 		 ("======> Card disable finished.\n"));
 
-	return rtStatus;
+	return _SUCCESS;
 }
 
 /*  without HW Auto state machine */
 int CardDisableWithoutHWSM(struct rtw_adapter *padapter)
 {
-	int rtStatus = _SUCCESS;
-
-	/* RT_TRACE(COMP_INIT, DBG_LOUD,
-	   ("======> Card Disable Without HWSM .\n")); */
 	if (padapter->bSurpriseRemoved) {
-		return rtStatus;
+		return _SUCCESS;
 	}
 
 	/*  RF Off Sequence ==== */
-	_DisableRFAFEAndResetBB(padapter);
+	_DisableRFAFEAndResetBB8192C(padapter);
 
 	/*   ==== Reset digital sequence   ====== */
-	_ResetDigitalProcedure1(padapter, true);
+	_ResetDigitalProcedure1_92C(padapter, true);
 
 	/*   ==== Pull GPIO PIN to balance level and LED control ====== */
 	_DisableGPIO(padapter);
@@ -1573,29 +1480,27 @@ int CardDisableWithoutHWSM(struct rtw_adapter *padapter)
 
 	/* RT_TRACE(COMP_INIT, DBG_LOUD,
 	   ("<====== Card Disable Without HWSM .\n")); */
-	return rtStatus;
+	return _SUCCESS;
 }
 
 void Hal_InitPGData(struct rtw_adapter *padapter, u8 *PROMContent)
 {
 	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
 
-	if (false == pEEPROM->bautoload_fail_flag) {	/*  autoload OK. */
+	if (!pEEPROM->bautoload_fail_flag) {	/*  autoload OK. */
 		if (!pEEPROM->EepromOrEfuse) {
 			/*  Read EFUSE real map to shadow. */
 			EFUSE_ShadowMapUpdate23a(padapter, EFUSE_WIFI);
-			memcpy((void *)PROMContent,
-			       (void *)pEEPROM->efuse_eeprom_data,
+			memcpy(PROMContent, pEEPROM->efuse_eeprom_data,
 			       HWSET_MAX_SIZE);
 		}
-	} else {		/* autoload fail */
+	} else {
 		RT_TRACE(_module_hci_hal_init_c_, _drv_notice_,
 			 ("AutoLoad Fail reported from CR9346!!\n"));
-/*		pHalData->AutoloadFailFlag = true; */
 		/* update to default value 0xFF */
-		if (false == pEEPROM->EepromOrEfuse)
+		if (!pEEPROM->EepromOrEfuse)
 			EFUSE_ShadowMapUpdate23a(padapter, EFUSE_WIFI);
-		memcpy((void *)PROMContent, (void *)pEEPROM->efuse_eeprom_data,
+		memcpy(PROMContent, pEEPROM->efuse_eeprom_data,
 		       HWSET_MAX_SIZE);
 	}
 }
@@ -1945,13 +1850,13 @@ Hal_EfuseParseThermalMeter_8723A(struct rtw_adapter *padapter,
 	/*  */
 	/*  ThermalMeter from EEPROM */
 	/*  */
-	if (AutoloadFail == false)
+	if (!AutoloadFail)
 		pHalData->EEPROMThermalMeter =
 		    PROMContent[EEPROM_THERMAL_METER_8723A];
 	else
 		pHalData->EEPROMThermalMeter = EEPROM_Default_ThermalMeter;
 
-	if ((pHalData->EEPROMThermalMeter == 0xff) || (AutoloadFail == true)) {
+	if ((pHalData->EEPROMThermalMeter == 0xff) || AutoloadFail) {
 		pHalData->bAPKThermalMeterIgnore = true;
 		pHalData->EEPROMThermalMeter = EEPROM_Default_ThermalMeter;
 	}
@@ -1960,10 +1865,6 @@ Hal_EfuseParseThermalMeter_8723A(struct rtw_adapter *padapter,
 		  pHalData->EEPROMThermalMeter);
 }
 
-void Hal_InitChannelPlan23a(struct rtw_adapter *padapter)
-{
-}
-
 static void rtl8723a_cal_txdesc_chksum(struct tx_desc *ptxdesc)
 {
 	u16 *usPtr = (u16 *) ptxdesc;
@@ -1981,254 +1882,6 @@ static void rtl8723a_cal_txdesc_chksum(struct tx_desc *ptxdesc)
 	ptxdesc->txdw7 |= cpu_to_le32(checksum & 0x0000ffff);
 }
 
-static void fill_txdesc_sectype(struct pkt_attrib *pattrib,
-				struct txdesc_8723a *ptxdesc)
-{
-	if ((pattrib->encrypt > 0) && !pattrib->bswenc) {
-		switch (pattrib->encrypt) {
-			/*  SEC_TYPE */
-		case WLAN_CIPHER_SUITE_WEP40:
-		case WLAN_CIPHER_SUITE_WEP104:
-		case WLAN_CIPHER_SUITE_TKIP:
-			ptxdesc->sectype = 1;
-			break;
-
-		case WLAN_CIPHER_SUITE_CCMP:
-			ptxdesc->sectype = 3;
-			break;
-
-		case 0:
-		default:
-			break;
-		}
-	}
-}
-
-static void fill_txdesc_vcs(struct pkt_attrib *pattrib,
-			    struct txdesc_8723a *ptxdesc)
-{
-	/* DBG_8723A("cvs_mode =%d\n", pattrib->vcs_mode); */
-
-	switch (pattrib->vcs_mode) {
-	case RTS_CTS:
-		ptxdesc->rtsen = 1;
-		break;
-
-	case CTS_TO_SELF:
-		ptxdesc->cts2self = 1;
-		break;
-
-	case NONE_VCS:
-	default:
-		break;
-	}
-
-	if (pattrib->vcs_mode) {
-		ptxdesc->hw_rts_en = 1;	/*  ENABLE HW RTS */
-
-		/*  Set RTS BW */
-		if (pattrib->ht_en) {
-			if (pattrib->bwmode & HT_CHANNEL_WIDTH_40)
-				ptxdesc->rts_bw = 1;
-
-			switch (pattrib->ch_offset) {
-			case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
-				ptxdesc->rts_sc = 0;
-				break;
-
-			case HAL_PRIME_CHNL_OFFSET_LOWER:
-				ptxdesc->rts_sc = 1;
-				break;
-
-			case HAL_PRIME_CHNL_OFFSET_UPPER:
-				ptxdesc->rts_sc = 2;
-				break;
-
-			default:
-				ptxdesc->rts_sc = 3;	/*  Duplicate */
-				break;
-			}
-		}
-	}
-}
-
-static void fill_txdesc_phy(struct pkt_attrib *pattrib,
-			    struct txdesc_8723a *ptxdesc)
-{
-	if (pattrib->ht_en) {
-		if (pattrib->bwmode & HT_CHANNEL_WIDTH_40)
-			ptxdesc->data_bw = 1;
-
-		switch (pattrib->ch_offset) {
-		case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
-			ptxdesc->data_sc = 0;
-			break;
-
-		case HAL_PRIME_CHNL_OFFSET_LOWER:
-			ptxdesc->data_sc = 1;
-			break;
-
-		case HAL_PRIME_CHNL_OFFSET_UPPER:
-			ptxdesc->data_sc = 2;
-			break;
-
-		default:
-			ptxdesc->data_sc = 3;	/*  Duplicate */
-			break;
-		}
-	}
-}
-
-static void rtl8723a_fill_default_txdesc(struct xmit_frame *pxmitframe,
-					 u8 *pbuf)
-{
-	struct rtw_adapter *padapter;
-	struct hal_data_8723a *pHalData;
-	struct dm_priv *pdmpriv;
-	struct mlme_ext_priv *pmlmeext;
-	struct mlme_ext_info *pmlmeinfo;
-	struct pkt_attrib *pattrib;
-	struct txdesc_8723a *ptxdesc;
-	s32 bmcst;
-
-	padapter = pxmitframe->padapter;
-	pHalData = GET_HAL_DATA(padapter);
-	pdmpriv = &pHalData->dmpriv;
-	pmlmeext = &padapter->mlmeextpriv;
-	pmlmeinfo = &pmlmeext->mlmext_info;
-
-	pattrib = &pxmitframe->attrib;
-	bmcst = is_multicast_ether_addr(pattrib->ra);
-
-	ptxdesc = (struct txdesc_8723a *)pbuf;
-
-	if (pxmitframe->frame_tag == DATA_FRAMETAG) {
-		ptxdesc->macid = pattrib->mac_id;	/*  CAM_ID(MAC_ID) */
-
-		if (pattrib->ampdu_en == true)
-			ptxdesc->agg_en = 1;	/*  AGG EN */
-		else
-			ptxdesc->bk = 1;	/*  AGG BK */
-
-		ptxdesc->qsel = pattrib->qsel;
-		ptxdesc->rate_id = pattrib->raid;
-
-		fill_txdesc_sectype(pattrib, ptxdesc);
-
-		ptxdesc->seq = pattrib->seqnum;
-
-		if ((pattrib->ether_type != 0x888e) &&
-		    (pattrib->ether_type != 0x0806) &&
-		    (pattrib->dhcp_pkt != 1)) {
-			/*  Non EAP & ARP & DHCP type data packet */
-
-			fill_txdesc_vcs(pattrib, ptxdesc);
-			fill_txdesc_phy(pattrib, ptxdesc);
-
-			ptxdesc->rtsrate = 8;	/*  RTS Rate = 24M */
-			ptxdesc->data_ratefb_lmt = 0x1F;
-			ptxdesc->rts_ratefb_lmt = 0xF;
-
-			/*  use REG_INIDATA_RATE_SEL value */
-			ptxdesc->datarate =
-				pdmpriv->INIDATA_RATE[pattrib->mac_id];
-
-		} else {
-			/*  EAP data packet and ARP packet. */
-			/*  Use the 1M data rate to send the EAP/ARP packet. */
-			/*  This will maybe make the handshake smooth. */
-
-			ptxdesc->bk = 1;	/*  AGG BK */
-			ptxdesc->userate = 1;	/*  driver uses rate */
-			if (pmlmeinfo->preamble_mode == PREAMBLE_SHORT)
-				ptxdesc->data_short = 1;
-			ptxdesc->datarate = MRateToHwRate23a(pmlmeext->tx_rate);
-		}
-	} else if (pxmitframe->frame_tag == MGNT_FRAMETAG) {
-/*		RT_TRACE(_module_hal_xmit_c_, _drv_notice_,
-		("%s: MGNT_FRAMETAG\n", __func__)); */
-
-		ptxdesc->macid = pattrib->mac_id;	/*  CAM_ID(MAC_ID) */
-		ptxdesc->qsel = pattrib->qsel;
-		ptxdesc->rate_id = pattrib->raid;	/*  Rate ID */
-		ptxdesc->seq = pattrib->seqnum;
-		ptxdesc->userate = 1;	/*  driver uses rate, 1M */
-		ptxdesc->rty_lmt_en = 1;	/*  retry limit enable */
-		ptxdesc->data_rt_lmt = 6;	/*  retry limit = 6 */
-
-		/* CCX-TXRPT ack for xmit mgmt frames. */
-		if (pxmitframe->ack_report)
-			ptxdesc->ccx = 1;
-
-		ptxdesc->datarate = MRateToHwRate23a(pmlmeext->tx_rate);
-	} else if (pxmitframe->frame_tag == TXAGG_FRAMETAG) {
-		RT_TRACE(_module_hal_xmit_c_, _drv_warning_,
-			 ("%s: TXAGG_FRAMETAG\n", __func__));
-	} else {
-		RT_TRACE(_module_hal_xmit_c_, _drv_warning_,
-			 ("%s: frame_tag = 0x%x\n", __func__,
-			  pxmitframe->frame_tag));
-
-		ptxdesc->macid = 4;	/*  CAM_ID(MAC_ID) */
-		ptxdesc->rate_id = 6;	/*  Rate ID */
-		ptxdesc->seq = pattrib->seqnum;
-		ptxdesc->userate = 1;	/*  driver uses rate */
-		ptxdesc->datarate = MRateToHwRate23a(pmlmeext->tx_rate);
-	}
-
-	ptxdesc->pktlen = pattrib->last_txcmdsz;
-	ptxdesc->offset = TXDESC_SIZE + OFFSET_SZ;
-	if (bmcst)
-		ptxdesc->bmc = 1;
-	ptxdesc->ls = 1;
-	ptxdesc->fs = 1;
-	ptxdesc->own = 1;
-
-	/*  2009.11.05. tynli_test. Suggested by SD4 Filen for FW LPS. */
-	/*  (1) The sequence number of each non-Qos frame / broadcast /
-	 *   multicast / mgnt frame should be controled by Hw because Fw
-	 * will also send null data which we cannot control when Fw LPS enable.
-	 *  --> default enable non-Qos data sequense number.
-	 2010.06.23. by tynli. */
-	/*  (2) Enable HW SEQ control for beacon packet,
-	 * because we use Hw beacon. */
-	/*  (3) Use HW Qos SEQ to control the seq num of Ext port
-	 * non-Qos packets. */
-	/*  2010.06.23. Added by tynli. */
-	if (!pattrib->qos_en) {
-		/*  Hw set sequence number */
-		ptxdesc->hwseq_en = 1;	/*  HWSEQ_EN */
-		ptxdesc->hwseq_sel = 0;	/*  HWSEQ_SEL */
-	}
-}
-
-/*
- *	Description:
- *
- *	Parameters:
- *		pxmitframe	xmitframe
- *		pbuf		where to fill tx desc
- */
-void rtl8723a_update_txdesc(struct xmit_frame *pxmitframe, u8 *pbuf)
-{
-	struct tx_desc *pdesc;
-
-	pdesc = (struct tx_desc *)pbuf;
-	memset(pdesc, 0, sizeof(struct tx_desc));
-
-	rtl8723a_fill_default_txdesc(pxmitframe, pbuf);
-
-	pdesc->txdw0 = cpu_to_le32(pdesc->txdw0);
-	pdesc->txdw1 = cpu_to_le32(pdesc->txdw1);
-	pdesc->txdw2 = cpu_to_le32(pdesc->txdw2);
-	pdesc->txdw3 = cpu_to_le32(pdesc->txdw3);
-	pdesc->txdw4 = cpu_to_le32(pdesc->txdw4);
-	pdesc->txdw5 = cpu_to_le32(pdesc->txdw5);
-	pdesc->txdw6 = cpu_to_le32(pdesc->txdw6);
-	pdesc->txdw7 = cpu_to_le32(pdesc->txdw7);
-	rtl8723a_cal_txdesc_chksum(pdesc);
-}
-
 /*
  *  Description: In normal chip, we should send some packet to Hw which
  *  will be used by Fw in FW LPS mode. The function is to fill the Tx
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c b/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c
index 3d4d7ec27509..88e91cd8ebb9 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c
@@ -418,7 +418,6 @@ PHY_SetRFReg(struct rtw_adapter *Adapter, enum RF_RADIO_PATH eRFPath,
  *---------------------------------------------------------------------------*/
 int PHY_MACConfig8723A(struct rtw_adapter *Adapter)
 {
-	int rtStatus = _SUCCESS;
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 	bool is92C = IS_92C_SERIAL(pHalData->VersionID);
 
@@ -433,7 +432,7 @@ int PHY_MACConfig8723A(struct rtw_adapter *Adapter)
 	if (is92C && (BOARD_USB_DONGLE == pHalData->BoardType))
 		rtl8723au_write8(Adapter, 0x40, 0x04);
 
-	return rtStatus;
+	return _SUCCESS;
 }
 
 /**
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c b/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c
index 2dc0886e5f90..1aad4384471c 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c
@@ -42,21 +42,6 @@
 #include <rtl8723a_hal.h>
 #include <usb_ops_linux.h>
 
-/*---------------------------Define Local Constant---------------------------*/
-/*  Define local structure for debug!!!!! */
-struct rf_shadow_compare_map {
-	/*  Shadow register value */
-	u32		Value;
-	/*  Compare or not flag */
-	u8		Compare;
-	/*  Record If it had ever modified unpredicted */
-	u8		ErrorOrNot;
-	/*  Recorver Flag */
-	u8		Recorver;
-	/*  */
-	u8		Driver_Write;
-};
-
 /*-----------------------------------------------------------------------------
  * Function:    PHY_RF6052SetBandwidth()
  *
@@ -71,20 +56,23 @@ struct rf_shadow_compare_map {
  *
  * Note:		For RF type 0222D
  *---------------------------------------------------------------------------*/
-void rtl8723a_phy_rf6052set_bw(
-	struct rtw_adapter *Adapter,
-	enum ht_channel_width Bandwidth)	/* 20M or 40M */
+void rtl8723a_phy_rf6052set_bw(struct rtw_adapter *Adapter,
+			       enum ht_channel_width Bandwidth)	/* 20M or 40M */
 {
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(Adapter);
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 
 	switch (Bandwidth) {
 	case HT_CHANNEL_WIDTH_20:
-		pHalData->RfRegChnlVal[0] = ((pHalData->RfRegChnlVal[0] & 0xfffff3ff) | 0x0400);
-		PHY_SetRFReg(Adapter, RF_PATH_A, RF_CHNLBW, bRFRegOffsetMask, pHalData->RfRegChnlVal[0]);
+		pHalData->RfRegChnlVal[0] =
+			(pHalData->RfRegChnlVal[0] & 0xfffff3ff) | 0x0400;
+		PHY_SetRFReg(Adapter, RF_PATH_A, RF_CHNLBW, bRFRegOffsetMask,
+			     pHalData->RfRegChnlVal[0]);
 		break;
 	case HT_CHANNEL_WIDTH_40:
-		pHalData->RfRegChnlVal[0] = ((pHalData->RfRegChnlVal[0] & 0xfffff3ff));
-		PHY_SetRFReg(Adapter, RF_PATH_A, RF_CHNLBW, bRFRegOffsetMask, pHalData->RfRegChnlVal[0]);
+		pHalData->RfRegChnlVal[0] =
+			(pHalData->RfRegChnlVal[0] & 0xfffff3ff);
+		PHY_SetRFReg(Adapter, RF_PATH_A, RF_CHNLBW, bRFRegOffsetMask,
+			     pHalData->RfRegChnlVal[0]);
 		break;
 	default:
 		break;
@@ -108,7 +96,8 @@ void rtl8723a_phy_rf6052set_bw(
  *
  *---------------------------------------------------------------------------*/
 
-void rtl823a_phy_rf6052setccktxpower(struct rtw_adapter *Adapter, u8 *pPowerlevel)
+void rtl823a_phy_rf6052setccktxpower(struct rtw_adapter *Adapter,
+				     u8 *pPowerlevel)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 	struct dm_priv *pdmpriv = &pHalData->dmpriv;
@@ -118,7 +107,8 @@ void rtl823a_phy_rf6052setccktxpower(struct rtw_adapter *Adapter, u8 *pPowerleve
 	u8 idx1, idx2;
 	u8 *ptr;
 
-	/*  According to SD3 eechou's suggestion, we need to disable turbo scan for RU. */
+	/*  According to SD3 eechou's suggestion, we need to disable
+	    turbo scan for RU. */
 	/*  Otherwise, external PA will be broken if power index > 0x20. */
 	if (pHalData->EEPROMRegulatory != 0 || pHalData->ExternalPA)
 		TurboScanOff = true;
@@ -131,29 +121,37 @@ void rtl823a_phy_rf6052setccktxpower(struct rtw_adapter *Adapter, u8 *pPowerleve
 
 		if (TurboScanOff) {
 			for (idx1 = RF_PATH_A; idx1 <= RF_PATH_B; idx1++) {
-				TxAGC[idx1] =
-					pPowerlevel[idx1] | (pPowerlevel[idx1]<<8) |
-					(pPowerlevel[idx1]<<16) | (pPowerlevel[idx1]<<24);
-				/*  2010/10/18 MH For external PA module. We need to limit power index to be less than 0x20. */
+				TxAGC[idx1] = pPowerlevel[idx1] |
+					(pPowerlevel[idx1] << 8) |
+					(pPowerlevel[idx1] << 16) |
+					(pPowerlevel[idx1] << 24);
+				/*  2010/10/18 MH For external PA module.
+				    We need to limit power index to be less
+				    than 0x20. */
 				if (TxAGC[idx1] > 0x20 && pHalData->ExternalPA)
 					TxAGC[idx1] = 0x20;
 			}
 		}
 	} else {
-/*  20100427 Joseph: Driver dynamic Tx power shall not affect Tx power. It shall be determined by power training mechanism. */
-/*  Currently, we cannot fully disable driver dynamic tx power mechanism because it is referenced by BT coexist mechanism. */
-/*  In the future, two mechanism shall be separated from each other and maintained independantly. Thanks for Lanhsin's reminder. */
+/*  20100427 Joseph: Driver dynamic Tx power shall not affect Tx
+ *  power. It shall be determined by power training mechanism. */
+/*  Currently, we cannot fully disable driver dynamic tx power
+ *  mechanism because it is referenced by BT coexist mechanism. */
+/*  In the future, two mechanism shall be separated from each other
+ *  and maintained independantly. Thanks for Lanhsin's reminder. */
 		if (pdmpriv->DynamicTxHighPowerLvl == TxHighPwrLevel_Level1) {
 			TxAGC[RF_PATH_A] = 0x10101010;
 			TxAGC[RF_PATH_B] = 0x10101010;
-		} else if (pdmpriv->DynamicTxHighPowerLvl == TxHighPwrLevel_Level2) {
+		} else if (pdmpriv->DynamicTxHighPowerLvl ==
+			   TxHighPwrLevel_Level2) {
 			TxAGC[RF_PATH_A] = 0x00000000;
 			TxAGC[RF_PATH_B] = 0x00000000;
 		} else {
 			for (idx1 = RF_PATH_A; idx1 <= RF_PATH_B; idx1++) {
-				TxAGC[idx1] =
-					pPowerlevel[idx1] | (pPowerlevel[idx1]<<8) |
-					(pPowerlevel[idx1]<<16) | (pPowerlevel[idx1]<<24);
+				TxAGC[idx1] = pPowerlevel[idx1] |
+					(pPowerlevel[idx1] << 8) |
+					(pPowerlevel[idx1] << 16) |
+					(pPowerlevel[idx1] << 24);
 			}
 
 			if (pHalData->EEPROMRegulatory == 0) {
@@ -178,29 +176,24 @@ void rtl823a_phy_rf6052setccktxpower(struct rtw_adapter *Adapter, u8 *pPowerleve
 	}
 
 	/*  rf-A cck tx power */
-	tmpval = TxAGC[RF_PATH_A]&0xff;
+	tmpval = TxAGC[RF_PATH_A] & 0xff;
 	PHY_SetBBReg(Adapter, rTxAGC_A_CCK1_Mcs32, bMaskByte1, tmpval);
-	tmpval = TxAGC[RF_PATH_A]>>8;
+	tmpval = TxAGC[RF_PATH_A] >> 8;
 	PHY_SetBBReg(Adapter, rTxAGC_B_CCK11_A_CCK2_11, 0xffffff00, tmpval);
 
 	/*  rf-B cck tx power */
-	tmpval = TxAGC[RF_PATH_B]>>24;
+	tmpval = TxAGC[RF_PATH_B] >> 24;
 	PHY_SetBBReg(Adapter, rTxAGC_B_CCK11_A_CCK2_11, bMaskByte0, tmpval);
-	tmpval = TxAGC[RF_PATH_B]&0x00ffffff;
+	tmpval = TxAGC[RF_PATH_B] & 0x00ffffff;
 	PHY_SetBBReg(Adapter, rTxAGC_B_CCK1_55_Mcs32, 0xffffff00, tmpval);
 }	/* PHY_RF6052SetCckTxPower */
 
 /*  powerbase0 for OFDM rates */
 /*  powerbase1 for HT MCS rates */
-static void getPowerBase(
-	struct rtw_adapter *Adapter,
-	u8 *pPowerLevel,
-	u8 Channel,
-	u32 *OfdmBase,
-	u32 *MCSBase
-	)
+static void getPowerBase(struct rtw_adapter *Adapter, u8 *pPowerLevel,
+			 u8 Channel, u32 *OfdmBase, u32 *MCSBase)
 {
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(Adapter);
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 	u32 powerBase0, powerBase1;
 	u8 Legacy_pwrdiff = 0;
 	s8 HT20_pwrdiff = 0;
@@ -211,8 +204,9 @@ static void getPowerBase(
 		Legacy_pwrdiff = pHalData->TxPwrLegacyHtDiff[i][Channel-1];
 		powerBase0 = powerlevel[i] + Legacy_pwrdiff;
 
-		powerBase0 = (powerBase0<<24) | (powerBase0<<16) | (powerBase0<<8) | powerBase0;
-		*(OfdmBase+i) = powerBase0;
+		powerBase0 = powerBase0 << 24 | powerBase0 << 16 |
+			powerBase0 << 8 | powerBase0;
+		*(OfdmBase + i) = powerBase0;
 	}
 
 	for (i = 0; i < 2; i++) {
@@ -222,36 +216,35 @@ static void getPowerBase(
 			powerlevel[i] += HT20_pwrdiff;
 		}
 		powerBase1 = powerlevel[i];
-		powerBase1 = (powerBase1<<24) | (powerBase1<<16) | (powerBase1<<8) | powerBase1;
-		*(MCSBase+i) = powerBase1;
+		powerBase1 = powerBase1 << 24 | powerBase1 << 16 |
+			powerBase1 << 8 | powerBase1;
+		*(MCSBase + i) = powerBase1;
 	}
 }
 
-static void getTxPowerWriteValByRegulatory(
-		struct rtw_adapter *Adapter,
-		u8 Channel,
-		u8 index,
-		u32 *powerBase0,
-		u32 *powerBase1,
-		u32 *pOutWriteVal
-	)
+static void
+getTxPowerWriteValByRegulatory(struct rtw_adapter *Adapter, u8 Channel,
+			       u8 index, u32 *powerBase0, u32 *powerBase1,
+			       u32 *pOutWriteVal)
 {
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(Adapter);
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 	struct dm_priv	*pdmpriv = &pHalData->dmpriv;
-	u8	i, chnlGroup = 0, pwr_diff_limit[4];
-	u32	writeVal, customer_limit, rf;
+	u8 i, chnlGroup = 0, pwr_diff_limit[4];
+	u32 writeVal, customer_limit, rf;
 
 	/*  Index 0 & 1 = legacy OFDM, 2-5 = HT_MCS rate */
 	for (rf = 0; rf < 2; rf++) {
 		switch (pHalData->EEPROMRegulatory) {
 		case 0:	/*  Realtek better performance */
-			/*  increase power diff defined by Realtek for large power */
+			/*  increase power diff defined by Realtek for
+			 *  large power */
 			chnlGroup = 0;
 			writeVal = pHalData->MCSTxPowerLevelOriginalOffset[chnlGroup][index+(rf?8:0)] +
 				((index < 2) ? powerBase0[rf] : powerBase1[rf]);
 			break;
 		case 1:	/*  Realtek regulatory */
-			/*  increase power diff defined by Realtek for regulatory */
+			/*  increase power diff defined by Realtek for
+			 *  regulatory */
 			if (pHalData->pwrGroupCnt == 1)
 				chnlGroup = 0;
 			if (pHalData->pwrGroupCnt >= 3) {
@@ -262,17 +255,20 @@ static void getTxPowerWriteValByRegulatory(
 				else if (Channel > 9)
 					chnlGroup = 2;
 
-				if (pHalData->CurrentChannelBW == HT_CHANNEL_WIDTH_20)
+				if (pHalData->CurrentChannelBW ==
+				    HT_CHANNEL_WIDTH_20)
 					chnlGroup++;
 				else
 					chnlGroup += 4;
 			}
 			writeVal = pHalData->MCSTxPowerLevelOriginalOffset[chnlGroup][index+(rf?8:0)] +
-				   ((index < 2) ? powerBase0[rf] : powerBase1[rf]);
+				   ((index < 2) ? powerBase0[rf] :
+				    powerBase1[rf]);
 			break;
 		case 2:	/*  Better regulatory */
-				/*  don't increase any power diff */
-			writeVal = ((index < 2) ? powerBase0[rf] : powerBase1[rf]);
+			/*  don't increase any power diff */
+			writeVal = ((index < 2) ? powerBase0[rf] :
+				    powerBase1[rf]);
 			break;
 		case 3:	/*  Customer defined power diff. */
 			chnlGroup = 0;
@@ -299,28 +295,34 @@ static void getTxPowerWriteValByRegulatory(
 			break;
 		}
 
-/*  20100427 Joseph: Driver dynamic Tx power shall not affect Tx power. It shall be determined by power training mechanism. */
-/*  Currently, we cannot fully disable driver dynamic tx power mechanism because it is referenced by BT coexist mechanism. */
-/*  In the future, two mechanism shall be separated from each other and maintained independantly. Thanks for Lanhsin's reminder. */
+/*  20100427 Joseph: Driver dynamic Tx power shall not affect Tx power.
+    It shall be determined by power training mechanism. */
+/*  Currently, we cannot fully disable driver dynamic tx power mechanism
+    because it is referenced by BT coexist mechanism. */
+/*  In the future, two mechanism shall be separated from each other and
+    maintained independantly. Thanks for Lanhsin's reminder. */
 
 		if (pdmpriv->DynamicTxHighPowerLvl == TxHighPwrLevel_Level1)
 			writeVal = 0x14141414;
-		else if (pdmpriv->DynamicTxHighPowerLvl == TxHighPwrLevel_Level2)
+		else if (pdmpriv->DynamicTxHighPowerLvl ==
+			 TxHighPwrLevel_Level2)
 			writeVal = 0x00000000;
 
-		/*  20100628 Joseph: High power mode for BT-Coexist mechanism. */
-		/*  This mechanism is only applied when Driver-Highpower-Mechanism is OFF. */
+		/* 20100628 Joseph: High power mode for BT-Coexist mechanism. */
+		/* This mechanism is only applied when
+		   Driver-Highpower-Mechanism is OFF. */
 		if (pdmpriv->DynamicTxHighPowerLvl == TxHighPwrLevel_BT1)
 			writeVal = writeVal - 0x06060606;
 		else if (pdmpriv->DynamicTxHighPowerLvl == TxHighPwrLevel_BT2)
 			writeVal = writeVal;
-		*(pOutWriteVal+rf) = writeVal;
+		*(pOutWriteVal + rf) = writeVal;
 	}
 }
 
-static void writeOFDMPowerReg(struct rtw_adapter *Adapter, u8 index, u32 *pValue)
+static void writeOFDMPowerReg(struct rtw_adapter *Adapter, u8 index,
+			      u32 *pValue)
 {
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(Adapter);
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 	u16 RegOffset_A[6] = {
 		rTxAGC_A_Rate18_06, rTxAGC_A_Rate54_24,
 		rTxAGC_A_Mcs03_Mcs00, rTxAGC_A_Mcs07_Mcs04,
@@ -338,12 +340,13 @@ static void writeOFDMPowerReg(struct rtw_adapter *Adapter, u8 index, u32 *pValue
 	for (rf = 0; rf < 2; rf++) {
 		writeVal = pValue[rf];
 		for (i = 0; i < 4; i++) {
-			pwr_val[i] = (u8)((writeVal & (0x7f<<(i*8)))>>(i*8));
-			if (pwr_val[i]  > RF6052_MAX_TX_PWR)
+			pwr_val[i] = (u8)((writeVal &
+					   (0x7f << (i * 8))) >> (i * 8));
+			if (pwr_val[i] > RF6052_MAX_TX_PWR)
 				pwr_val[i]  = RF6052_MAX_TX_PWR;
 		}
-		writeVal = (pwr_val[3]<<24) | (pwr_val[2]<<16) |
-			   (pwr_val[1]<<8) | pwr_val[0];
+		writeVal = pwr_val[3] << 24 | pwr_val[2] << 16 |
+			pwr_val[1] << 8 | pwr_val[0];
 
 		if (rf == 0)
 			RegOffset = RegOffset_A[index];
@@ -352,7 +355,8 @@ static void writeOFDMPowerReg(struct rtw_adapter *Adapter, u8 index, u32 *pValue
 
 		PHY_SetBBReg(Adapter, RegOffset, bMaskDWord, writeVal);
 
-		/*  201005115 Joseph: Set Tx Power diff for Tx power training mechanism. */
+		/*  201005115 Joseph: Set Tx Power diff for Tx power
+		    training mechanism. */
 		if (((pHalData->rf_type == RF_2T2R) &&
 		    (RegOffset == rTxAGC_A_Mcs15_Mcs12 ||
 		     RegOffset == rTxAGC_B_Mcs15_Mcs12)) ||
@@ -360,15 +364,19 @@ static void writeOFDMPowerReg(struct rtw_adapter *Adapter, u8 index, u32 *pValue
 		     (RegOffset == rTxAGC_A_Mcs07_Mcs04 ||
 		      RegOffset == rTxAGC_B_Mcs07_Mcs04))) {
 			writeVal = pwr_val[3];
-			if (RegOffset == rTxAGC_A_Mcs15_Mcs12 || RegOffset == rTxAGC_A_Mcs07_Mcs04)
+			if (RegOffset == rTxAGC_A_Mcs15_Mcs12 ||
+			    RegOffset == rTxAGC_A_Mcs07_Mcs04)
 				RegOffset = 0xc90;
-			if (RegOffset == rTxAGC_B_Mcs15_Mcs12 || RegOffset == rTxAGC_B_Mcs07_Mcs04)
+			if (RegOffset == rTxAGC_B_Mcs15_Mcs12 ||
+			    RegOffset == rTxAGC_B_Mcs07_Mcs04)
 				RegOffset = 0xc98;
 			for (i = 0; i < 3; i++) {
 				if (i != 2)
-					writeVal = (writeVal > 8) ? (writeVal-8) : 0;
+					writeVal = (writeVal > 8) ?
+						(writeVal - 8) : 0;
 				else
-					writeVal = (writeVal > 6) ? (writeVal-6) : 0;
+					writeVal = (writeVal > 6) ?
+						(writeVal - 6) : 0;
 				rtl8723au_write8(Adapter, RegOffset + i,
 						 (u8)writeVal);
 			}
@@ -379,8 +387,9 @@ static void writeOFDMPowerReg(struct rtw_adapter *Adapter, u8 index, u32 *pValue
  * Function:	PHY_RF6052SetOFDMTxPower
  *
  * Overview:	For legacy and HY OFDM, we must read EEPROM TX power index for
- *			different channel and read original value in TX power register area from
- *			0xe00. We increase offset and original value to be correct tx pwr.
+ *		different channel and read original value in TX power
+ *		register area from 0xe00. We increase offset and
+ *		original value to be correct tx pwr.
  *
  * Input:       NONE
  *
@@ -389,20 +398,23 @@ static void writeOFDMPowerReg(struct rtw_adapter *Adapter, u8 index, u32 *pValue
  * Return:      NONE
  *
  * Revised History:
- * When			Who		Remark
- * 11/05/2008	MHC		Simulate 8192 series method.
- * 01/06/2009	MHC		1. Prevent Path B tx power overflow or underflow dure to
- *						A/B pwr difference or legacy/HT pwr diff.
- *						2. We concern with path B legacy/HT OFDM difference.
- * 01/22/2009	MHC		Support new EPRO format from SD3.
+ * When			Remark
+ * 11/05/2008	MHC	Simulate 8192 series method.
+ * 01/06/2009	MHC	1. Prevent Path B tx power overflow or
+ *			underflow dure to A/B pwr difference or
+ *			legacy/HT pwr diff.
+ *			2. We concern with path B legacy/HT OFDM difference.
+ * 01/22/2009	MHC	Support new EPRO format from SD3.
  *
  *---------------------------------------------------------------------------*/
-void rtl8723a_PHY_RF6052SetOFDMTxPower(struct rtw_adapter *Adapter, u8 *pPowerLevel, u8 Channel)
+void rtl8723a_PHY_RF6052SetOFDMTxPower(struct rtw_adapter *Adapter,
+				       u8 *pPowerLevel, u8 Channel)
 {
 	u32 writeVal[2], powerBase0[2], powerBase1[2];
 	u8 index = 0;
 
-	getPowerBase(Adapter, pPowerLevel, Channel, &powerBase0[0], &powerBase1[0]);
+	getPowerBase(Adapter, pPowerLevel, Channel,
+		     &powerBase0[0], &powerBase1[0]);
 
 	for (index = 0; index < 6; index++) {
 		getTxPowerWriteValByRegulatory(Adapter, Channel, index,
@@ -416,7 +428,7 @@ static int phy_RF6052_Config_ParaFile(struct rtw_adapter *Adapter)
 {
 	u32 u4RegValue = 0;
 	u8 eRFPath;
-	struct bb_reg_define	*pPhyReg;
+	struct bb_reg_define *pPhyReg;
 	int rtStatus = _SUCCESS;
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 
@@ -430,15 +442,17 @@ static int phy_RF6052_Config_ParaFile(struct rtw_adapter *Adapter)
 		/*----Store original RFENV control type----*/
 		switch (eRFPath) {
 		case RF_PATH_A:
-			u4RegValue = PHY_QueryBBReg(Adapter, pPhyReg->rfintfs, bRFSI_RFENV);
+			u4RegValue = PHY_QueryBBReg(Adapter, pPhyReg->rfintfs,
+						    bRFSI_RFENV);
 			break;
 		case RF_PATH_B:
-			u4RegValue = PHY_QueryBBReg(Adapter, pPhyReg->rfintfs, bRFSI_RFENV<<16);
+			u4RegValue = PHY_QueryBBReg(Adapter, pPhyReg->rfintfs,
+						    bRFSI_RFENV << 16);
 			break;
 		}
 
 		/*----Set RF_ENV enable----*/
-		PHY_SetBBReg(Adapter, pPhyReg->rfintfe, bRFSI_RFENV<<16, 0x1);
+		PHY_SetBBReg(Adapter, pPhyReg->rfintfe, bRFSI_RFENV << 16, 0x1);
 		udelay(1);/* PlatformStallExecution(1); */
 
 		/*----Set RF_ENV output high----*/
@@ -446,10 +460,12 @@ static int phy_RF6052_Config_ParaFile(struct rtw_adapter *Adapter)
 		udelay(1);/* PlatformStallExecution(1); */
 
 		/* Set bit number of Address and Data for RF register */
-		PHY_SetBBReg(Adapter, pPhyReg->rfHSSIPara2, b3WireAddressLength, 0x0);	/*  Set 1 to 4 bits for 8255 */
+		PHY_SetBBReg(Adapter, pPhyReg->rfHSSIPara2, b3WireAddressLength,
+			     0x0);	/*  Set 1 to 4 bits for 8255 */
 		udelay(1);/* PlatformStallExecution(1); */
 
-		PHY_SetBBReg(Adapter, pPhyReg->rfHSSIPara2, b3WireDataLength, 0x0);	/*  Set 0 to 12  bits for 8255 */
+		PHY_SetBBReg(Adapter, pPhyReg->rfHSSIPara2, b3WireDataLength,
+			     0x0);	/*  Set 0 to 12  bits for 8255 */
 		udelay(1);/* PlatformStallExecution(1); */
 
 		/*----Initialize RF fom connfiguration file----*/
@@ -464,15 +480,16 @@ static int phy_RF6052_Config_ParaFile(struct rtw_adapter *Adapter)
 		/*----Restore RFENV control type----*/;
 		switch (eRFPath) {
 		case RF_PATH_A:
-			PHY_SetBBReg(Adapter, pPhyReg->rfintfs, bRFSI_RFENV, u4RegValue);
+			PHY_SetBBReg(Adapter, pPhyReg->rfintfs,
+				     bRFSI_RFENV, u4RegValue);
 			break;
 		case RF_PATH_B:
-			PHY_SetBBReg(Adapter, pPhyReg->rfintfs, bRFSI_RFENV<<16, u4RegValue);
+			PHY_SetBBReg(Adapter, pPhyReg->rfintfs,
+				     bRFSI_RFENV << 16, u4RegValue);
 			break;
 		}
 
 		if (rtStatus != _SUCCESS) {
-			/* RT_TRACE(COMP_FPGA, DBG_LOUD, ("phy_RF6052_Config_ParaFile():Radio[%d] Fail!!", eRFPath)); */
 			goto phy_RF6052_Config_ParaFile_Fail;
 		}
 	}
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_xmit.c b/drivers/staging/rtl8723au/hal/rtl8723a_xmit.c
deleted file mode 100644
index 6ea2f9efef64..000000000000
--- a/drivers/staging/rtl8723au/hal/rtl8723a_xmit.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2012 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.
- *
- ******************************************************************************/
-#define _RTL8723A_XMIT_C_
-
-#include <osdep_service.h>
-#include <drv_types.h>
-#include <rtl8723a_hal.h>
-
-void handle_txrpt_ccx_8723a(struct rtw_adapter *adapter, void *buf)
-{
-	struct txrpt_ccx_8723a *txrpt_ccx = buf;
-
-	if (txrpt_ccx->int_ccx) {
-		if (txrpt_ccx->pkt_ok)
-			rtw_ack_tx_done23a(&adapter->xmitpriv, RTW_SCTX_DONE_SUCCESS);
-		else
-			rtw_ack_tx_done23a(&adapter->xmitpriv, RTW_SCTX_DONE_CCX_PKT_FAIL);
-	}
-}
diff --git a/drivers/staging/rtl8723au/hal/rtl8723au_led.c b/drivers/staging/rtl8723au/hal/rtl8723au_led.c
deleted file mode 100644
index b946636af9b3..000000000000
--- a/drivers/staging/rtl8723au/hal/rtl8723au_led.c
+++ /dev/null
@@ -1,124 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 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.
- *
- ******************************************************************************/
-
-#include "drv_types.h"
-#include "rtl8723a_hal.h"
-#include "rtl8723a_led.h"
-#include "usb_ops_linux.h"
-
-/*  */
-/*  LED object. */
-/*  */
-
-/*  */
-/*	Prototype of protected function. */
-/*  */
-
-/*  */
-/*  LED_819xUsb routines. */
-/*  */
-
-/*	Description: */
-/*		Turn on LED according to LedPin specified. */
-void SwLedOn23a(struct rtw_adapter *padapter, struct led_8723a *pLed)
-{
-	u8	LedCfg = 0;
-
-	if ((padapter->bSurpriseRemoved == true) || (padapter->bDriverStopped == true))
-		return;
-	switch (pLed->LedPin) {
-	case LED_PIN_GPIO0:
-		break;
-	case LED_PIN_LED0:
-		/*  SW control led0 on. */
-		rtl8723au_write8(padapter, REG_LEDCFG0,
-				 (LedCfg&0xf0)|BIT(5)|BIT(6));
-		break;
-	case LED_PIN_LED1:
-		 /*  SW control led1 on. */
-		rtl8723au_write8(padapter, REG_LEDCFG1, (LedCfg&0x00)|BIT(6));
-		break;
-	case LED_PIN_LED2:
-		LedCfg = rtl8723au_read8(padapter, REG_LEDCFG2);
-		 /*  SW control led1 on. */
-		rtl8723au_write8(padapter, REG_LEDCFG2, (LedCfg&0x80)|BIT(5));
-		break;
-	default:
-		break;
-	}
-	pLed->bLedOn = true;
-}
-
-/*	Description: */
-/*		Turn off LED according to LedPin specified. */
-void SwLedOff23a(struct rtw_adapter *padapter, struct led_8723a *pLed)
-{
-	u8	LedCfg = 0;
-	/* struct hal_data_8723a	*pHalData = GET_HAL_DATA(padapter); */
-
-	if ((padapter->bSurpriseRemoved) || (padapter->bDriverStopped))
-		goto exit;
-
-	switch (pLed->LedPin) {
-	case LED_PIN_GPIO0:
-		break;
-	case LED_PIN_LED0:
-		/*  SW control led0 on. */
-		rtl8723au_write8(padapter, REG_LEDCFG0,
-				 (LedCfg&0xf0)|BIT(5)|BIT(6));
-		break;
-	case LED_PIN_LED1:
-		/*  SW control led1 on. */
-		rtl8723au_write8(padapter, REG_LEDCFG1,
-				 (LedCfg&0x00)|BIT(5)|BIT(6));
-		break;
-	case LED_PIN_LED2:
-		LedCfg = rtl8723au_read8(padapter, REG_LEDCFG2);
-		/*  SW control led1 on. */
-		rtl8723au_write8(padapter, REG_LEDCFG2,
-				 (LedCfg&0x80)|BIT(3)|BIT(5));
-		break;
-	default:
-		break;
-	}
-exit:
-	pLed->bLedOn = false;
-}
-
-/*  Interface to manipulate LED objects. */
-
-/*	Description: */
-/*		Initialize all LED_871x objects. */
-void
-rtl8723au_InitSwLeds(struct rtw_adapter	*padapter)
-{
-	struct led_priv *pledpriv = &padapter->ledpriv;
-
-	pledpriv->LedControlHandler = LedControl871x23a;
-	/* 8723as-vau wifi used led2 */
-	InitLed871x23a(padapter, &pledpriv->SwLed0, LED_PIN_LED2);
-
-/*	InitLed871x23a(padapter,&pledpriv->SwLed1, LED_PIN_LED2); */
-}
-
-/*	Description: */
-/*		DeInitialize all LED_819xUsb objects. */
-void
-rtl8723au_DeInitSwLeds(struct rtw_adapter *padapter)
-{
-	struct led_priv	*ledpriv = &padapter->ledpriv;
-
-	DeInitLed871x23a(&ledpriv->SwLed0);
-}
diff --git a/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c b/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c
index a67850fe6e5d..6070510bb470 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c
@@ -21,18 +21,6 @@
 /* include <rtl8192c_hal.h> */
 #include <rtl8723a_hal.h>
 
-static void do_queue_select(struct rtw_adapter	*padapter, struct pkt_attrib *pattrib)
-{
-	u8 qsel;
-
-	qsel = pattrib->priority;
-	RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
-		 ("### do_queue_select priority =%d , qsel = %d\n",
-		  pattrib->priority, qsel));
-
-	pattrib->qsel = qsel;
-}
-
 static int urb_zero_packet_chk(struct rtw_adapter *padapter, int sz)
 {
 	int blnSetTxDescOffset;
@@ -163,7 +151,7 @@ static s32 update_txdesc(struct xmit_frame *pxmitframe, u8 *pmem, s32 sz, u8 bag
 
 	memset(ptxdesc, 0, sizeof(struct tx_desc));
 
-	if ((pxmitframe->frame_tag&0x0f) == DATA_FRAMETAG) {
+	if (pxmitframe->frame_tag == DATA_FRAMETAG) {
 		/* offset 4 */
 		ptxdesc->txdw1 |= cpu_to_le32(pattrib->mac_id&0x1f);
 
@@ -215,7 +203,7 @@ static s32 update_txdesc(struct xmit_frame *pxmitframe, u8 *pmem, s32 sz, u8 bag
 
 			ptxdesc->txdw5 |= cpu_to_le32(MRateToHwRate23a(pmlmeext->tx_rate));
 		}
-	} else if ((pxmitframe->frame_tag&0x0f) == MGNT_FRAMETAG) {
+	} else if (pxmitframe->frame_tag == MGNT_FRAMETAG) {
 		/* offset 4 */
 		ptxdesc->txdw1 |= cpu_to_le32(pattrib->mac_id&0x1f);
 
@@ -240,10 +228,11 @@ static s32 update_txdesc(struct xmit_frame *pxmitframe, u8 *pmem, s32 sz, u8 bag
 		ptxdesc->txdw5 |= cpu_to_le32(0x00180000);/* retry limit = 6 */
 
 		ptxdesc->txdw5 |= cpu_to_le32(MRateToHwRate23a(pmlmeext->tx_rate));
-	} else if ((pxmitframe->frame_tag&0x0f) == TXAGG_FRAMETAG) {
+	} else if (pxmitframe->frame_tag == TXAGG_FRAMETAG) {
 		DBG_8723A("pxmitframe->frame_tag == TXAGG_FRAMETAG\n");
 	} else {
-		DBG_8723A("pxmitframe->frame_tag = %d\n", pxmitframe->frame_tag);
+		DBG_8723A("pxmitframe->frame_tag = %d\n",
+			  pxmitframe->frame_tag);
 
 		/* offset 4 */
 		ptxdesc->txdw1 |= cpu_to_le32((4)&0x1f);/* CAM_ID(MAC_ID) */
@@ -306,10 +295,10 @@ static int rtw_dump_xframe(struct rtw_adapter *padapter,
 	struct pkt_attrib *pattrib = &pxmitframe->attrib;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 
-	if ((pxmitframe->frame_tag == DATA_FRAMETAG) &&
-	    (pxmitframe->attrib.ether_type != 0x0806) &&
-	    (pxmitframe->attrib.ether_type != 0x888e) &&
-	    (pxmitframe->attrib.dhcp_pkt != 1))
+	if (pxmitframe->frame_tag == DATA_FRAMETAG &&
+	    pxmitframe->attrib.ether_type != ETH_P_ARP &&
+	    pxmitframe->attrib.ether_type != ETH_P_PAE &&
+	    pxmitframe->attrib.dhcp_pkt != 1)
 		rtw_issue_addbareq_cmd23a(padapter, pxmitframe);
 
 	mem_addr = pxmitframe->buf_addr;
@@ -392,7 +381,7 @@ bool rtl8723au_xmitframe_complete(struct rtw_adapter *padapter,
 
 		pxmitbuf->priv_data = pxmitframe;
 
-		if ((pxmitframe->frame_tag&0x0f) == DATA_FRAMETAG) {
+		if (pxmitframe->frame_tag == DATA_FRAMETAG) {
 			if (pxmitframe->attrib.priority <= 15)/* TID0~15 */
 				res = rtw_xmitframe_coalesce23a(padapter, pxmitframe->pkt, pxmitframe);
 
@@ -440,7 +429,7 @@ bool rtl8723au_hal_xmit(struct rtw_adapter *padapter,
 	struct pkt_attrib *pattrib = &pxmitframe->attrib;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-	do_queue_select(padapter, pattrib);
+	pattrib->qsel = pattrib->priority;
 	spin_lock_bh(&pxmitpriv->lock);
 
 #ifdef CONFIG_8723AU_AP_MODE
diff --git a/drivers/staging/rtl8723au/hal/usb_halinit.c b/drivers/staging/rtl8723au/hal/usb_halinit.c
index a5de03d45aa9..febe5cedef8f 100644
--- a/drivers/staging/rtl8723au/hal/usb_halinit.c
+++ b/drivers/staging/rtl8723au/hal/usb_halinit.c
@@ -21,11 +21,13 @@
 #include <HalPwrSeqCmd.h>
 #include <Hal8723PwrSeq.h>
 #include <rtl8723a_hal.h>
-#include <rtl8723a_led.h>
 #include <linux/ieee80211.h>
 
 #include <usb_ops.h>
 
+static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
+				enum rt_rf_power_state eRFPowerState);
+
 static void
 _ConfigChipOutEP(struct rtw_adapter *pAdapter, u8 NumOutPipe)
 {
@@ -61,42 +63,28 @@ _ConfigChipOutEP(struct rtw_adapter *pAdapter, u8 NumOutPipe)
 	   (u32)NumOutPipe, (u32)pHalData->OutEpNumber)); */
 }
 
-static bool rtl8723au_set_queue_pipe_mapping(struct rtw_adapter *pAdapter,
-					     u8 NumInPipe, u8 NumOutPipe)
+bool rtl8723au_chip_configure(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(pAdapter);
-	bool result = false;
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
+	u8 NumInPipe = pdvobjpriv->RtNumInPipes;
+	u8 NumOutPipe = pdvobjpriv->RtNumOutPipes;
 
-	_ConfigChipOutEP(pAdapter, NumOutPipe);
+	_ConfigChipOutEP(padapter, NumOutPipe);
 
 	/*  Normal chip with one IN and one OUT doesn't have interrupt IN EP. */
 	if (pHalData->OutEpNumber == 1) {
 		if (NumInPipe != 1)
-			return result;
+			return false;
 	}
 
-	result = Hal_MappingOutPipe23a(pAdapter, NumOutPipe);
-
-	return result;
-}
-
-void rtl8723au_chip_configure(struct rtw_adapter *padapter)
-{
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
-
-	pHalData->interfaceIndex = pdvobjpriv->InterfaceNumber;
-
-	rtl8723au_set_queue_pipe_mapping(padapter,
-					 pdvobjpriv->RtNumInPipes,
-					 pdvobjpriv->RtNumOutPipes);
+	return Hal_MappingOutPipe23a(padapter, NumOutPipe);
 }
 
 static int _InitPowerOn(struct rtw_adapter *padapter)
 {
-	int status = _SUCCESS;
-	u16 value16 = 0;
-	u8 value8 = 0;
+	u16 value16;
+	u8 value8;
 
 	/*  RSV_CTRL 0x1C[7:0] = 0x00
 	    unlock ISO/CLK/Power control register */
@@ -123,7 +111,7 @@ static int _InitPowerOn(struct rtw_adapter *padapter)
 	/* for Efuse PG, suggest by Jackie 2011.11.23 */
 	PHY_SetBBReg(padapter, REG_EFUSE_CTRL, BIT(28)|BIT(29)|BIT(30), 0x06);
 
-	return status;
+	return _SUCCESS;
 }
 
 /*  Shall USB interface init this? */
@@ -313,7 +301,7 @@ static void _InitNormalChipThreeOutEpPriority(struct rtw_adapter *Adapter)
 	_InitNormalChipRegPriority(Adapter, beQ, bkQ, viQ, voQ, mgtQ, hiQ);
 }
 
-static void _InitNormalChipQueuePriority(struct rtw_adapter *Adapter)
+static void _InitQueuePriority(struct rtw_adapter *Adapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 
@@ -333,11 +321,6 @@ static void _InitNormalChipQueuePriority(struct rtw_adapter *Adapter)
 	}
 }
 
-static void _InitQueuePriority(struct rtw_adapter *Adapter)
-{
-	_InitNormalChipQueuePriority(Adapter);
-}
-
 static void _InitTransferPageSize(struct rtw_adapter *Adapter)
 {
 	/*  Tx page size is always 128. */
@@ -442,18 +425,6 @@ static void _InitEDCA(struct rtw_adapter *Adapter)
 	rtl8723au_write32(Adapter, REG_EDCA_VO_PARAM, 0x002FA226);
 }
 
-static void _InitHWLed(struct rtw_adapter *Adapter)
-{
-	struct led_priv *pledpriv = &Adapter->ledpriv;
-
-	if (pledpriv->LedStrategy != HW_LED)
-		return;
-
-/*  HW led control */
-/*  to do .... */
-/* must consider cases of antenna diversity/ commbo card/solo card/mini card */
-}
-
 static void _InitRDGSetting(struct rtw_adapter *Adapter)
 {
 	rtl8723au_write8(Adapter, REG_RD_CTRL, 0xFF);
@@ -480,7 +451,7 @@ static void _InitRFType(struct rtw_adapter *Adapter)
 
 	pHalData->rf_chip = RF_6052;
 
-	if (is92CU == false) {
+	if (!is92CU) {
 		pHalData->rf_type = RF_1T1R;
 		DBG_8723A("Set RF Chip ID to RF_6052 and RF type to 1T1R.\n");
 		return;
@@ -527,23 +498,22 @@ enum rt_rf_power_state RfOnOffDetect23a(struct rtw_adapter *pAdapter)
 	return rfpowerstate;
 }
 
-void _ps_open_RF23a(struct rtw_adapter *padapter);
-
 int rtl8723au_hal_init(struct rtw_adapter *Adapter)
 {
-	u8 val8 = 0;
-	u32 boundary;
-	int status = _SUCCESS;
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 	struct pwrctrl_priv *pwrctrlpriv = &Adapter->pwrctrlpriv;
 	struct registry_priv *pregistrypriv = &Adapter->registrypriv;
+	u8 val8 = 0;
+	u32 boundary;
+	int status = _SUCCESS;
+	bool mac_on;
 
 	unsigned long init_start_time = jiffies;
 
 	Adapter->hw_init_completed = false;
 
 	if (Adapter->pwrctrlpriv.bkeepfwalive) {
-		_ps_open_RF23a(Adapter);
+		phy_SsPwrSwitch92CU(Adapter, rf_on);
 
 		if (pHalData->bIQKInitialized) {
 			rtl8723a_phy_iq_calibrate(Adapter, true);
@@ -566,9 +536,9 @@ int rtl8723au_hal_init(struct rtw_adapter *Adapter)
 	/* 0x100 value of first mac is 0xEA while 0x100 value of secondary
 	   is 0x00 */
 	if (val8 == 0xEA) {
-		pHalData->bMACFuncEnable = false;
+		mac_on = false;
 	} else {
-		pHalData->bMACFuncEnable = true;
+		mac_on = true;
 		RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
 			 ("%s: MAC has already power on\n", __func__));
 	}
@@ -587,7 +557,7 @@ int rtl8723au_hal_init(struct rtw_adapter *Adapter)
 		boundary = WMM_NORMAL_TX_PAGE_BOUNDARY;
 	}
 
-	if (!pHalData->bMACFuncEnable) {
+	if (!mac_on) {
 		status =  InitLLTTable23a(Adapter, boundary);
 		if (status == _FAIL) {
 			RT_TRACE(_module_hci_hal_init_c_, _drv_err_,
@@ -673,7 +643,7 @@ int rtl8723au_hal_init(struct rtw_adapter *Adapter)
 	pHalData->RfRegChnlVal[0] = PHY_QueryRFReg(Adapter, (enum RF_RADIO_PATH)0, RF_CHNLBW, bRFRegOffsetMask);
 	pHalData->RfRegChnlVal[1] = PHY_QueryRFReg(Adapter, (enum RF_RADIO_PATH)1, RF_CHNLBW, bRFRegOffsetMask);
 
-	if (!pHalData->bMACFuncEnable) {
+	if (!mac_on) {
 		_InitQueueReservedPage(Adapter);
 		_InitTxBufferBoundary(Adapter);
 	}
@@ -694,8 +664,6 @@ int rtl8723au_hal_init(struct rtw_adapter *Adapter)
 	_InitRetryFunction(Adapter);
 	rtl8723a_InitBeaconParameters(Adapter);
 
-	_InitHWLed(Adapter);
-
 	_BBTurnOnBlock(Adapter);
 	/* NicIFSetMacAddress(padapter, padapter->PermanentAddress); */
 
@@ -806,266 +774,108 @@ exit:
 }
 
 static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
-				enum rt_rf_power_state eRFPowerState,
-				int bRegSSPwrLvl)
+				enum rt_rf_power_state eRFPowerState)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
-	u8 value8;
-	u8 bytetmp;
+	u8 sps0;
+
+	sps0 = rtl8723au_read8(Adapter, REG_SPS0_CTRL);
 
 	switch (eRFPowerState) {
 	case rf_on:
-		if (bRegSSPwrLvl == 1) {
-			/*  1. Enable MAC Clock. Can not be enabled now. */
-			/* WriteXBYTE(REG_SYS_CLKR+1,
-			   ReadXBYTE(REG_SYS_CLKR+1) | BIT(3)); */
-
-			/*  2. Force PWM, Enable SPS18_LDO_Marco_Block */
-			rtl8723au_write8(Adapter, REG_SPS0_CTRL,
-					 rtl8723au_read8(Adapter, REG_SPS0_CTRL) |
-					 BIT(0) | BIT(3));
-
-			/*  3. restore BB, AFE control register. */
-			/* RF */
-			if (pHalData->rf_type ==  RF_2T2R)
-				PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter,
-					     0x380038, 1);
-			else
-				PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter,
-					     0x38, 1);
-			PHY_SetBBReg(Adapter, rOFDM0_TRxPathEnable, 0xf0, 1);
-			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT(1), 0);
-
-			/* AFE */
-			if (pHalData->rf_type ==  RF_2T2R)
-				PHY_SetBBReg(Adapter, rRx_Wait_CCA, bMaskDWord,
-					     0x63DB25A0);
-			else if (pHalData->rf_type ==  RF_1T1R)
-				PHY_SetBBReg(Adapter, rRx_Wait_CCA, bMaskDWord,
-					     0x631B25A0);
-
-			/*  4. issue 3-wire command that RF set to Rx idle
-			    mode. This is used to re-write the RX idle mode. */
-			/*  We can only prvide a usual value instead and then
-			    HW will modify the value by itself. */
-			PHY_SetRFReg(Adapter, RF_PATH_A, 0,
-				     bRFRegOffsetMask, 0x32D95);
-			if (pHalData->rf_type ==  RF_2T2R) {
-				PHY_SetRFReg(Adapter, RF_PATH_B, 0,
-					     bRFRegOffsetMask, 0x32D95);
-			}
-		} else {		/*  Level 2 or others. */
-			/* h.	AFE_PLL_CTRL 0x28[7:0] = 0x80
-			   disable AFE PLL */
-			rtl8723au_write8(Adapter, REG_AFE_PLL_CTRL, 0x81);
-
-			/*  i.	AFE_XTAL_CTRL 0x24[15:0] = 0x880F
-			    gated AFE DIG_CLOCK */
-			rtl8723au_write16(Adapter, REG_AFE_XTAL_CTRL, 0x800F);
-			mdelay(1);
-
-			/*  2. Force PWM, Enable SPS18_LDO_Marco_Block */
-			rtl8723au_write8(Adapter, REG_SPS0_CTRL,
-					 rtl8723au_read8(Adapter, REG_SPS0_CTRL) |
-					 BIT(0) | BIT(3));
-
-			/*  3. restore BB, AFE control register. */
-			/* RF */
-			if (pHalData->rf_type ==  RF_2T2R)
-				PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter,
-					     0x380038, 1);
-			else
-				PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter,
-					     0x38, 1);
-			PHY_SetBBReg(Adapter, rOFDM0_TRxPathEnable, 0xf0, 1);
-			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT(1), 0);
-
-			/* AFE */
-			if (pHalData->rf_type ==  RF_2T2R)
-				PHY_SetBBReg(Adapter, rRx_Wait_CCA,
-					     bMaskDWord, 0x63DB25A0);
-			else if (pHalData->rf_type ==  RF_1T1R)
-				PHY_SetBBReg(Adapter, rRx_Wait_CCA,
-					     bMaskDWord, 0x631B25A0);
-
-			/*  4. issue 3-wire command that RF set to Rx idle
-			    mode. This is used to re-write the RX idle mode. */
-			/*  We can only prvide a usual value instead and
-			    then HW will modify the value by itself. */
-			PHY_SetRFReg(Adapter, RF_PATH_A, 0,
+		/*  1. Enable MAC Clock. Can not be enabled now. */
+		/* WriteXBYTE(REG_SYS_CLKR+1,
+		   ReadXBYTE(REG_SYS_CLKR+1) | BIT(3)); */
+
+		/*  2. Force PWM, Enable SPS18_LDO_Marco_Block */
+		rtl8723au_write8(Adapter, REG_SPS0_CTRL,
+				 sps0 | BIT(0) | BIT(3));
+
+		/*  3. restore BB, AFE control register. */
+		/* RF */
+		if (pHalData->rf_type ==  RF_2T2R)
+			PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter,
+				     0x380038, 1);
+		else
+			PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter,
+				     0x38, 1);
+		PHY_SetBBReg(Adapter, rOFDM0_TRxPathEnable, 0xf0, 1);
+		PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT(1), 0);
+
+		/* AFE */
+		if (pHalData->rf_type ==  RF_2T2R)
+			PHY_SetBBReg(Adapter, rRx_Wait_CCA, bMaskDWord,
+				     0x63DB25A0);
+		else if (pHalData->rf_type ==  RF_1T1R)
+			PHY_SetBBReg(Adapter, rRx_Wait_CCA, bMaskDWord,
+				     0x631B25A0);
+
+		/*  4. issue 3-wire command that RF set to Rx idle
+		    mode. This is used to re-write the RX idle mode. */
+		/*  We can only prvide a usual value instead and then
+		    HW will modify the value by itself. */
+		PHY_SetRFReg(Adapter, RF_PATH_A, 0, bRFRegOffsetMask, 0x32D95);
+		if (pHalData->rf_type ==  RF_2T2R) {
+			PHY_SetRFReg(Adapter, RF_PATH_B, 0,
 				     bRFRegOffsetMask, 0x32D95);
-			if (pHalData->rf_type ==  RF_2T2R) {
-				PHY_SetRFReg(Adapter, RF_PATH_B, 0,
-					     bRFRegOffsetMask, 0x32D95);
-			}
-
-			/*  5. gated MAC Clock */
-			bytetmp = rtl8723au_read8(Adapter, REG_APSD_CTRL);
-			rtl8723au_write8(Adapter, REG_APSD_CTRL,
-					 bytetmp & ~BIT(6));
-
-			mdelay(10);
-
-			/*  Set BB reset at first */
-			/* 0x16 */
-			rtl8723au_write8(Adapter, REG_SYS_FUNC_EN, 0x17);
-
-			/*  Enable TX */
-			rtl8723au_write8(Adapter, REG_TXPAUSE, 0x0);
 		}
 		break;
 	case rf_sleep:
 	case rf_off:
-		value8 = rtl8723au_read8(Adapter, REG_SPS0_CTRL);
 		if (IS_81xxC_VENDOR_UMC_B_CUT(pHalData->VersionID))
-			value8 &= ~BIT(0);
+			sps0 &= ~BIT(0);
 		else
-			value8 &= ~(BIT(0) | BIT(3));
-		if (bRegSSPwrLvl == 1) {
-			RT_TRACE(_module_hal_init_c_, _drv_err_, ("SS LVL1\n"));
-			/*  Disable RF and BB only for SelectSuspend. */
-
-			/*  1. Set BB/RF to shutdown. */
-			/*	(1) Reg878[5:3]= 0	 RF rx_code for
-							preamble power saving */
-			/*	(2)Reg878[21:19]= 0	Turn off RF-B */
-			/*	(3) RegC04[7:4]= 0	Turn off all paths
-							for packet detection */
-			/*	(4) Reg800[1] = 1	enable preamble power
-							saving */
-			Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_RF0] =
-				PHY_QueryBBReg(Adapter, rFPGA0_XAB_RFParameter,
-					       bMaskDWord);
-			Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_RF1] =
-				PHY_QueryBBReg(Adapter, rOFDM0_TRxPathEnable,
-					       bMaskDWord);
-			Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_RF2] =
-				PHY_QueryBBReg(Adapter, rFPGA0_RFMOD,
-					       bMaskDWord);
-			if (pHalData->rf_type ==  RF_2T2R) {
-				PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter,
-					     0x380038, 0);
-			} else if (pHalData->rf_type ==  RF_1T1R) {
-				PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter,
-					     0x38, 0);
-			}
-			PHY_SetBBReg(Adapter, rOFDM0_TRxPathEnable, 0xf0, 0);
-			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT(1), 1);
-
-			/*  2 .AFE control register to power down. bit[30:22] */
-			Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_AFE0] =
-				PHY_QueryBBReg(Adapter, rRx_Wait_CCA,
-					       bMaskDWord);
-			if (pHalData->rf_type ==  RF_2T2R)
-				PHY_SetBBReg(Adapter, rRx_Wait_CCA, bMaskDWord,
-					     0x00DB25A0);
-			else if (pHalData->rf_type ==  RF_1T1R)
-				PHY_SetBBReg(Adapter, rRx_Wait_CCA, bMaskDWord,
-					     0x001B25A0);
-
-			/*  3. issue 3-wire command that RF set to power down.*/
-			PHY_SetRFReg(Adapter, RF_PATH_A, 0, bRFRegOffsetMask, 0);
-			if (pHalData->rf_type ==  RF_2T2R)
-				PHY_SetRFReg(Adapter, RF_PATH_B, 0,
-					     bRFRegOffsetMask, 0);
-
-			/*  4. Force PFM , disable SPS18_LDO_Marco_Block */
-			rtl8723au_write8(Adapter, REG_SPS0_CTRL, value8);
-		} else {	/*  Level 2 or others. */
-			RT_TRACE(_module_hal_init_c_, _drv_err_, ("SS LVL2\n"));
-			{
-				u8 eRFPath = RF_PATH_A, value8 = 0;
-				rtl8723au_write8(Adapter, REG_TXPAUSE, 0xFF);
-				PHY_SetRFReg(Adapter,
-					     (enum RF_RADIO_PATH)eRFPath,
-					     0x0, bMaskByte0, 0x0);
-				value8 |= APSDOFF;
-				/* 0x40 */
-				rtl8723au_write8(Adapter, REG_APSD_CTRL,
-						 value8);
-
-				/*  After switch APSD, we need to delay
-				    for stability */
-				mdelay(10);
-
-				/*  Set BB reset at first */
-				value8 = 0;
-				value8 |= (FEN_USBD | FEN_USBA |
-					   FEN_BB_GLB_RSTn);
-				/* 0x16 */
-				rtl8723au_write8(Adapter, REG_SYS_FUNC_EN,
-						 value8);
-			}
-
-			/*  Disable RF and BB only for SelectSuspend. */
-
-			/*  1. Set BB/RF to shutdown. */
-			/*	(1) Reg878[5:3]= 0	RF rx_code for
-							preamble power saving */
-			/*	(2)Reg878[21:19]= 0	Turn off RF-B */
-			/*	(3) RegC04[7:4]= 0	Turn off all paths for
-							packet detection */
-			/*	(4) Reg800[1] = 1	enable preamble power
-							saving */
-			Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_RF0] =
-				PHY_QueryBBReg(Adapter, rFPGA0_XAB_RFParameter,
-					       bMaskDWord);
-			Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_RF1] =
-				PHY_QueryBBReg(Adapter, rOFDM0_TRxPathEnable,
-					       bMaskDWord);
-			Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_RF2] =
-				PHY_QueryBBReg(Adapter, rFPGA0_RFMOD,
-					       bMaskDWord);
-			if (pHalData->rf_type ==  RF_2T2R)
-				PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter,
-					     0x380038, 0);
-			else if (pHalData->rf_type ==  RF_1T1R)
-				PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter,
-					     0x38, 0);
-			PHY_SetBBReg(Adapter, rOFDM0_TRxPathEnable, 0xf0, 0);
-			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT(1), 1);
-
-			/*  2 .AFE control register to power down. bit[30:22] */
-			Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_AFE0] =
-				PHY_QueryBBReg(Adapter, rRx_Wait_CCA,
-					       bMaskDWord);
-			if (pHalData->rf_type ==  RF_2T2R)
-				PHY_SetBBReg(Adapter, rRx_Wait_CCA, bMaskDWord,
-					     0x00DB25A0);
-			else if (pHalData->rf_type ==  RF_1T1R)
-				PHY_SetBBReg(Adapter, rRx_Wait_CCA, bMaskDWord,
-					     0x001B25A0);
-
-			/* 3. issue 3-wire command that RF set to power down. */
-			PHY_SetRFReg(Adapter, RF_PATH_A, 0, bRFRegOffsetMask, 0);
-			if (pHalData->rf_type ==  RF_2T2R)
-				PHY_SetRFReg(Adapter, RF_PATH_B, 0,
-					     bRFRegOffsetMask, 0);
-
-			/*  4. Force PFM , disable SPS18_LDO_Marco_Block */
-			rtl8723au_write8(Adapter, REG_SPS0_CTRL, value8);
-
-			/*  2010/10/13 MH/Isaachsu exchange sequence. */
-			/* h.	AFE_PLL_CTRL 0x28[7:0] = 0x80
-				disable AFE PLL */
-			rtl8723au_write8(Adapter, REG_AFE_PLL_CTRL, 0x80);
-			mdelay(1);
-
-			/*  i.	AFE_XTAL_CTRL 0x24[15:0] = 0x880F
-				gated AFE DIG_CLOCK */
-			rtl8723au_write16(Adapter, REG_AFE_XTAL_CTRL, 0xA80F);
+			sps0 &= ~(BIT(0) | BIT(3));
+
+		RT_TRACE(_module_hal_init_c_, _drv_err_, ("SS LVL1\n"));
+		/*  Disable RF and BB only for SelectSuspend. */
+
+		/*  1. Set BB/RF to shutdown. */
+		/*	(1) Reg878[5:3]= 0	RF rx_code for
+						preamble power saving */
+		/*	(2)Reg878[21:19]= 0	Turn off RF-B */
+		/*	(3) RegC04[7:4]= 0	Turn off all paths
+						for packet detection */
+		/*	(4) Reg800[1] = 1	enable preamble power saving */
+		Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_RF0] =
+			PHY_QueryBBReg(Adapter, rFPGA0_XAB_RFParameter,
+				       bMaskDWord);
+		Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_RF1] =
+			PHY_QueryBBReg(Adapter, rOFDM0_TRxPathEnable,
+				       bMaskDWord);
+		Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_RF2] =
+			PHY_QueryBBReg(Adapter, rFPGA0_RFMOD, bMaskDWord);
+		if (pHalData->rf_type ==  RF_2T2R) {
+			PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter,
+				     0x380038, 0);
+		} else if (pHalData->rf_type ==  RF_1T1R) {
+			PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter, 0x38, 0);
 		}
+		PHY_SetBBReg(Adapter, rOFDM0_TRxPathEnable, 0xf0, 0);
+		PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT(1), 1);
+
+		/*  2 .AFE control register to power down. bit[30:22] */
+		Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_AFE0] =
+			PHY_QueryBBReg(Adapter, rRx_Wait_CCA, bMaskDWord);
+		if (pHalData->rf_type ==  RF_2T2R)
+			PHY_SetBBReg(Adapter, rRx_Wait_CCA, bMaskDWord,
+				     0x00DB25A0);
+		else if (pHalData->rf_type ==  RF_1T1R)
+			PHY_SetBBReg(Adapter, rRx_Wait_CCA, bMaskDWord,
+				     0x001B25A0);
+
+		/*  3. issue 3-wire command that RF set to power down.*/
+		PHY_SetRFReg(Adapter, RF_PATH_A, 0, bRFRegOffsetMask, 0);
+		if (pHalData->rf_type ==  RF_2T2R)
+			PHY_SetRFReg(Adapter, RF_PATH_B, 0,
+				     bRFRegOffsetMask, 0);
+
+		/*  4. Force PFM , disable SPS18_LDO_Marco_Block */
+		rtl8723au_write8(Adapter, REG_SPS0_CTRL, sps0);
 		break;
 	default:
 		break;
 	}
-
-}	/*  phy_PowerSwitch92CU */
-
-void _ps_open_RF23a(struct rtw_adapter *padapter)
-{
-	/* here call with bRegSSPwrLvl 1, bRegSSPwrLvl 2 needs to be verified */
-	phy_SsPwrSwitch92CU(padapter, rf_on, 1);
 }
 
 static void CardDisableRTL8723U(struct rtw_adapter *Adapter)
@@ -1142,8 +952,7 @@ int rtl8723au_inirp_init(struct rtw_adapter *Adapter)
 	/* issue Rx irp to receive data */
 	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
 	for (i = 0; i < NR_RECVBUFF; i++) {
-		if (rtl8723au_read_port(Adapter, RECV_BULK_IN_ADDR, 0,
-					precvbuf) == _FAIL) {
+		if (rtl8723au_read_port(Adapter, 0, precvbuf) == _FAIL) {
 			RT_TRACE(_module_hci_hal_init_c_, _drv_err_,
 				 ("usb_rx_init: usb_read_port error\n"));
 			status = _FAIL;
@@ -1151,9 +960,9 @@ int rtl8723au_inirp_init(struct rtw_adapter *Adapter)
 		}
 		precvbuf++;
 	}
-	if (rtl8723au_read_interrupt(Adapter, RECV_INT_IN_ADDR) == _FAIL) {
+	if (rtl8723au_read_interrupt(Adapter) == _FAIL) {
 		RT_TRACE(_module_hci_hal_init_c_, _drv_err_,
-			 ("usb_rx_init: usb_read_interrupt error\n"));
+			 ("%s: usb_read_interrupt error\n", __func__));
 		status = _FAIL;
 	}
 	pHalData->IntrMask[0] = rtl8723au_read32(Adapter, REG_USB_HIMR);
@@ -1209,14 +1018,6 @@ static void _ReadBoardType(struct rtw_adapter *Adapter, u8 *PROMContent,
 		pHalData->ExternalPA = 1;
 }
 
-static void _ReadLEDSetting(struct rtw_adapter *Adapter, u8 *PROMContent,
-			    bool AutoloadFail)
-{
-	struct led_priv *pledpriv = &Adapter->ledpriv;
-
-	pledpriv->LedStrategy = HW_LED;
-}
-
 static void Hal_EfuseParseMACAddr_8723AU(struct rtw_adapter *padapter,
 					 u8 *hwinfo, bool AutoLoadFail)
 {
@@ -1263,7 +1064,6 @@ static void readAdapterInfo(struct rtw_adapter *padapter)
 				    pEEPROM->bautoload_fail_flag);
 	Hal_EfuseParseThermalMeter_8723A(padapter, hwinfo,
 					 pEEPROM->bautoload_fail_flag);
-	_ReadLEDSetting(padapter, hwinfo, pEEPROM->bautoload_fail_flag);
 /*	_ReadRFSetting(Adapter, PROMContent, pEEPROM->bautoload_fail_flag); */
 /*	_ReadPSSetting(Adapter, PROMContent, pEEPROM->bautoload_fail_flag); */
 	Hal_EfuseParseAntennaDiversity(padapter, hwinfo,
@@ -1276,10 +1076,6 @@ static void readAdapterInfo(struct rtw_adapter *padapter)
 					   pEEPROM->bautoload_fail_flag);
 	Hal_EfuseParseXtal_8723A(padapter, hwinfo,
 				 pEEPROM->bautoload_fail_flag);
-	/*  */
-	/*  The following part initialize some vars by PG info. */
-	/*  */
-	Hal_InitChannelPlan23a(padapter);
 
 	/* hal_CustomizedBehavior_8723U(Adapter); */
 
@@ -1311,13 +1107,6 @@ static void _ReadRFType(struct rtw_adapter *Adapter)
 	pHalData->rf_chip = RF_6052;
 }
 
-static void _ReadSilmComboMode(struct rtw_adapter *Adapter)
-{
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
-
-	pHalData->SlimComboDbg = false;	/*  Default is not debug mode. */
-}
-
 /*  */
 /*	Description: */
 /*		We should set Efuse cell selection to WiFi cell in default. */
@@ -1350,10 +1139,6 @@ void rtl8723a_read_adapter_info(struct rtw_adapter *Adapter)
 	_ReadRFType(Adapter);/* rf_chip -> _InitRFType() */
 	_ReadPROMContent(Adapter);
 
-	/*  2010/10/25 MH THe function must be called after
-	    borad_type & IC-Version recognize. */
-	_ReadSilmComboMode(Adapter);
-
 	/* MSG_8723A("%s()(done), rf_chip = 0x%x, rf_type = 0x%x\n",
 	   __func__, pHalData->rf_chip, pHalData->rf_type); */
 
@@ -1417,17 +1202,17 @@ int GetHalDefVar8192CUsb(struct rtw_adapter *Adapter,
 void rtl8723a_update_ramask(struct rtw_adapter *padapter,
 			    u32 mac_id, u8 rssi_level)
 {
-	u8	init_rate = 0;
-	u8	networkType, raid;
-	u32	mask, rate_bitmap;
-	u8	shortGIrate = false;
-	int	supportRateNum = 0;
 	struct sta_info	*psta;
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(padapter);
-	struct dm_priv	*pdmpriv = &pHalData->dmpriv;
+	struct FW_Sta_Info *fw_sta;
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
+	u8 init_rate, networkType, raid;
+	u32 mask, rate_bitmap;
+	u8 shortGIrate = false;
+	int supportRateNum;
 
 	if (mac_id >= NUM_STA) /* CAM_SIZE */
 		return;
@@ -1456,8 +1241,8 @@ void rtl8723a_update_ramask(struct rtw_adapter *padapter,
 		break;
 
 	case 1:/* for broadcast/multicast */
-		supportRateNum = rtw_get_rateset_len23a(
-			pmlmeinfo->FW_sta_info[mac_id].SupportedRates);
+		fw_sta = &pmlmeinfo->FW_sta_info[mac_id]; 
+		supportRateNum = rtw_get_rateset_len23a(fw_sta->SupportedRates);
 		if (pmlmeext->cur_wireless_mode & WIRELESS_11B)
 			networkType = WIRELESS_11B;
 		else
@@ -1469,23 +1254,22 @@ void rtl8723a_update_ramask(struct rtw_adapter *padapter,
 		break;
 
 	default: /* for each sta in IBSS */
-		supportRateNum = rtw_get_rateset_len23a(
-			pmlmeinfo->FW_sta_info[mac_id].SupportedRates);
+		fw_sta = &pmlmeinfo->FW_sta_info[mac_id]; 
+		supportRateNum = rtw_get_rateset_len23a(fw_sta->SupportedRates);
 		networkType = judge_network_type23a(padapter,
-						 pmlmeinfo->FW_sta_info[mac_id].SupportedRates,
-						 supportRateNum) & 0xf;
+						    fw_sta->SupportedRates,
+						    supportRateNum) & 0xf;
 		/* pmlmeext->cur_wireless_mode = networkType; */
 		raid = networktype_to_raid23a(networkType);
 
 		mask = update_supported_rate23a(cur_network->SupportedRates,
-					     supportRateNum);
+						supportRateNum);
 
 		/* todo: support HT in IBSS */
 		break;
 	}
 
 	/* mask &= 0x0fffffff; */
-	rate_bitmap = 0x0fffffff;
 	rate_bitmap = ODM_Get_Rate_Bitmap23a(pHalData, mac_id, mask,
 					     rssi_level);
 	DBG_8723A("%s => mac_id:%d, networkType:0x%02x, "
@@ -1493,15 +1277,14 @@ void rtl8723a_update_ramask(struct rtw_adapter *padapter,
 		  __func__, mac_id, networkType, mask, rssi_level, rate_bitmap);
 
 	mask &= rate_bitmap;
-	mask |= ((raid<<28)&0xf0000000);
+	mask |= ((raid << 28) & 0xf0000000);
 
-	init_rate = get_highest_rate_idx23a(mask)&0x3f;
+	init_rate = get_highest_rate_idx23a(mask) & 0x3f;
 
 	if (pHalData->fw_ractrl == true) {
 		u8 arg = 0;
 
-		/* arg = (cam_idx-4)&0x1f;MACID */
-		arg = mac_id&0x1f;/* MACID */
+		arg = mac_id & 0x1f;/* MACID */
 
 		arg |= BIT(7);
 
@@ -1516,7 +1299,7 @@ void rtl8723a_update_ramask(struct rtw_adapter *padapter,
 		if (shortGIrate == true)
 			init_rate |= BIT(6);
 
-		rtl8723au_write8(padapter, (REG_INIDATA_RATE_SEL+mac_id),
+		rtl8723au_write8(padapter, (REG_INIDATA_RATE_SEL + mac_id),
 				 init_rate);
 	}
 
diff --git a/drivers/staging/rtl8723au/hal/usb_ops_linux.c b/drivers/staging/rtl8723au/hal/usb_ops_linux.c
index c1b04c13c392..a6d16adce107 100644
--- a/drivers/staging/rtl8723au/hal/usb_ops_linux.c
+++ b/drivers/staging/rtl8723au/hal/usb_ops_linux.c
@@ -317,7 +317,7 @@ urb_submit:
 	}
 }
 
-int rtl8723au_read_interrupt(struct rtw_adapter *adapter, u32 addr)
+int rtl8723au_read_interrupt(struct rtw_adapter *adapter)
 {
 	int err;
 	unsigned int pipe;
@@ -545,8 +545,7 @@ static void usb_read_port_complete(struct urb *purb)
 				 ("usb_read_port_complete: (purb->actual_"
 				  "length > MAX_RECVBUF_SZ) || (purb->actual_"
 				  "length < RXDESC_SIZE)\n"));
-			rtl8723au_read_port(padapter, RECV_BULK_IN_ADDR, 0,
-					    precvbuf);
+			rtl8723au_read_port(padapter, 0, precvbuf);
 			DBG_8723A("%s()-%d: RX Warning!\n",
 				  __func__, __LINE__);
 		} else {
@@ -561,8 +560,7 @@ static void usb_read_port_complete(struct urb *purb)
 				tasklet_schedule(&precvpriv->recv_tasklet);
 
 			precvbuf->pskb = NULL;
-			rtl8723au_read_port(padapter, RECV_BULK_IN_ADDR, 0,
-					    precvbuf);
+			rtl8723au_read_port(padapter, 0, precvbuf);
 		}
 	} else {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
@@ -596,8 +594,7 @@ static void usb_read_port_complete(struct urb *purb)
 			break;
 		case -EPROTO:
 		case -EOVERFLOW:
-			rtl8723au_read_port(padapter, RECV_BULK_IN_ADDR, 0,
-					    precvbuf);
+			rtl8723au_read_port(padapter, 0, precvbuf);
 			break;
 		case -EINPROGRESS:
 			DBG_8723A("ERROR: URB IS IN PROGRESS!\n");
@@ -608,18 +605,18 @@ static void usb_read_port_complete(struct urb *purb)
 	}
 }
 
-int rtl8723au_read_port(struct rtw_adapter *adapter, u32 addr, u32 cnt,
+int rtl8723au_read_port(struct rtw_adapter *adapter, u32 cnt,
 			struct recv_buf *precvbuf)
 {
+	struct urb *purb;
+	struct dvobj_priv *pdvobj = adapter_to_dvobj(adapter);
+	struct recv_priv *precvpriv = &adapter->recvpriv;
+	struct usb_device *pusbd = pdvobj->pusbdev;
 	int err;
 	unsigned int pipe;
 	unsigned long tmpaddr;
 	unsigned long alignment;
 	int ret = _SUCCESS;
-	struct urb *purb;
-	struct dvobj_priv *pdvobj = adapter_to_dvobj(adapter);
-	struct recv_priv *precvpriv = &adapter->recvpriv;
-	struct usb_device *pusbd = pdvobj->pusbdev;
 
 	if (adapter->bDriverStopped || adapter->bSurpriseRemoved) {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
diff --git a/drivers/staging/rtl8723au/include/Hal8723UHWImg_CE.h b/drivers/staging/rtl8723au/include/Hal8723UHWImg_CE.h
index bbeaab48057a..c834b3a738d7 100644
--- a/drivers/staging/rtl8723au/include/Hal8723UHWImg_CE.h
+++ b/drivers/staging/rtl8723au/include/Hal8723UHWImg_CE.h
@@ -19,7 +19,7 @@ extern u8 Rtl8723UFwUMCBCutImgArrayWithoutBT[Rtl8723UUMCBCutImgArrayWithoutBTLen
 extern const u8 Rtl8723SFwUMCBCutMPImgArray[Rtl8723SUMCBCutMPImgArrayLength];
 
 #define Rtl8723EBTImgArrayLength 15276
-extern u8 Rtl8723EFwBTImgArray[Rtl8723EBTImgArrayLength] ;
+extern u8 Rtl8723EFwBTImgArray[Rtl8723EBTImgArrayLength];
 
 #define Rtl8723UPHY_REG_Array_PGLength 336
 extern u32 Rtl8723UPHY_REG_Array_PG[Rtl8723UPHY_REG_Array_PGLength];
diff --git a/drivers/staging/rtl8723au/include/drv_types.h b/drivers/staging/rtl8723au/include/drv_types.h
index 9870f87bdc70..e83463aeb9b1 100644
--- a/drivers/staging/rtl8723au/include/drv_types.h
+++ b/drivers/staging/rtl8723au/include/drv_types.h
@@ -51,7 +51,6 @@ enum _NIC_VERSION {
 #include <rtw_debug.h>
 #include <rtw_rf.h>
 #include <rtw_event.h>
-#include <rtw_led.h>
 #include <rtw_mlme_ext.h>
 #include <rtw_ap.h>
 
@@ -228,7 +227,6 @@ struct rtw_adapter {
 	struct	registry_priv	registrypriv;
 	struct	pwrctrl_priv	pwrctrlpriv;
 	struct	eeprom_priv eeprompriv;
-	struct	led_priv	ledpriv;
 
 	u32	setband;
 
diff --git a/drivers/staging/rtl8723au/include/odm_debug.h b/drivers/staging/rtl8723au/include/odm_debug.h
index 4d935a33ccb3..83be5bab9e09 100644
--- a/drivers/staging/rtl8723au/include/odm_debug.h
+++ b/drivers/staging/rtl8723au/include/odm_debug.h
@@ -91,8 +91,7 @@
 #define ODM_COMP_INIT				BIT(31)
 
 /*------------------------Export Macro Definition---------------------------*/
-	#define DbgPrint	printk
-	#define RT_PRINTK(fmt, args...)	DbgPrint("%s(): " fmt, __func__, ## args);
+	#define RT_PRINTK(fmt, args...)	printk("%s(): " fmt, __func__, ## args);
 
 #ifndef ASSERT
 	#define ASSERT(expr)
@@ -101,38 +100,17 @@
 #define ODM_RT_TRACE(pDM_Odm, comp, level, fmt)							\
 		if(((comp) & pDM_Odm->DebugComponents) && (level <= pDM_Odm->DebugLevel))	\
 		{										\
-			DbgPrint("[ODM-8723A] ");						\
-			RT_PRINTK fmt;								\
-		}
-
-#define ODM_RT_TRACE_F(pDM_Odm, comp, level, fmt)						\
-		if(((comp) & pDM_Odm->DebugComponents) && (level <= pDM_Odm->DebugLevel))	\
-		{										\
+			printk("[ODM-8723A] ");						\
 			RT_PRINTK fmt;								\
 		}
 
 #define ODM_RT_ASSERT(pDM_Odm, expr, fmt)							\
 		if(!(expr)) {									\
-			DbgPrint("Assertion failed! %s at ......\n", #expr);			\
-			DbgPrint("      ......%s,%s,line=%d\n", __FILE__, __func__, __LINE__);\
+			printk("Assertion failed! %s at ......\n", #expr);			\
+			printk("      ......%s,%s,line=%d\n", __FILE__, __func__, __LINE__);\
 			RT_PRINTK fmt;								\
 			ASSERT(false);								\
 		}
-#define ODM_dbg_enter() { DbgPrint("==> %s\n", __func__); }
-#define ODM_dbg_exit() { DbgPrint("<== %s\n", __func__); }
-#define ODM_dbg_trace(str) { DbgPrint("%s:%s\n", __func__, str); }
-
-#define ODM_PRINT_ADDR(pDM_Odm, comp, level, title_str, ptr)						\
-			if(((comp) & pDM_Odm->DebugComponents) && (level <= pDM_Odm->DebugLevel){	\
-				int __i;			\
-				u8 *	__ptr = (u8 *)ptr;	\
-				DbgPrint("[ODM] ");		\
-				DbgPrint(title_str);		\
-				DbgPrint(" ");			\
-				for (__i=0; __i < 6; __i++)	\
-					DbgPrint("%02X%s", __ptr[__i], (__i == 5) ? "" : "-");		\
-				DbgPrint("\n");			\
-			}
 
 void ODM_InitDebugSetting23a(struct dm_odm_t *pDM_Odm);
 
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_dm.h b/drivers/staging/rtl8723au/include/rtl8723a_dm.h
index 18112225e53f..bf236e8e47a2 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_dm.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_dm.h
@@ -37,8 +37,7 @@ enum{
 #define IQK_BB_REG_NUM			9
 #define HP_THERMAL_NUM		8
 /*  duplicate code,will move to ODM ######### */
-struct dm_priv
-{
+struct dm_priv {
 	u32	InitODMFlag;
 
 	/*  Upper and Lower Signal threshold for Rate Adaptive*/
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_hal.h b/drivers/staging/rtl8723au/include/rtl8723a_hal.h
index ee203a572cb7..e14633678b52 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_hal.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_hal.h
@@ -372,16 +372,9 @@ struct hal_data_8723a {
 	/*  2010/08/09 MH Add CU power down mode. */
 	u8	pwrdown;
 
-	/*  Add for dual MAC  0--Mac0 1--Mac1 */
-	u32	interfaceIndex;
-
 	u8	OutEpQueueSel;
 	u8	OutEpNumber;
 
-	/*  2010/11/22 MH Add for slim combo debug mode selective. */
-	/*  This is used for fix the drawback of CU TSMC-A/UMC-A cut. HW auto suspend ability. Close BT clock. */
-	bool		SlimComboDbg;
-
 	/*  */
 	/*  Add For EEPROM Efuse switch and  Efuse Shadow map Setting */
 	/*  */
@@ -405,8 +398,6 @@ struct hal_data_8723a {
 	 *  2011/02/23 MH Add for 8723 mylti function definition. The define should be moved to an */
 	/*  independent file in the future. */
 
-	bool				bMACFuncEnable;
-
 	/*  Interrupt related register information. */
 	u32	IntArray[2];
 	u32	IntrMask[2];
@@ -518,8 +509,6 @@ void Hal_EfuseParseRateIndicationOption(struct rtw_adapter *padapter, u8 *hwinfo
 void Hal_EfuseParseXtal_8723A(struct rtw_adapter *pAdapter, u8 *hwinfo, u8 AutoLoadFail);
 void Hal_EfuseParseThermalMeter_8723A(struct rtw_adapter *padapter, u8 *hwinfo, bool AutoLoadFail);
 
-void Hal_InitChannelPlan23a(struct rtw_adapter *padapter);
-
 /*  register */
 void SetBcnCtrlReg23a(struct rtw_adapter *padapter, u8 SetBits, u8 ClearBits);
 void rtl8723a_InitBeaconParameters(struct rtw_adapter *padapter);
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_led.h b/drivers/staging/rtl8723au/include/rtl8723a_led.h
deleted file mode 100644
index 1623d186feb4..000000000000
--- a/drivers/staging/rtl8723au/include/rtl8723a_led.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 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.
- *
- ******************************************************************************/
-#ifndef __RTL8723A_LED_H__
-#define __RTL8723A_LED_H__
-
-#include <osdep_service.h>
-#include <drv_types.h>
-
-
-/*  */
-/*  Interface to manipulate LED objects. */
-/*  */
-void rtl8723au_InitSwLeds(struct rtw_adapter *padapter);
-void rtl8723au_DeInitSwLeds(struct rtw_adapter *padapter);
-void SwLedOn23a(struct rtw_adapter *padapter, struct led_8723a * pLed);
-void SwLedOff23a(struct rtw_adapter *padapter, struct led_8723a * pLed);
-
-#endif
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_recv.h b/drivers/staging/rtl8723au/include/rtl8723a_recv.h
index 885d4d3859f8..0177bbc1c1cf 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_recv.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_recv.h
@@ -28,15 +28,11 @@
 
 #define MAX_RECVBUF_SZ			15360 /*  15k < 16k */
 
-#define RECV_BULK_IN_ADDR		0x80
-#define RECV_INT_IN_ADDR		0x81
-
 #define PHY_RSSI_SLID_WIN_MAX		100
 #define PHY_LINKQUALITY_SLID_WIN_MAX	20
 
 
-struct phy_stat
-{
+struct phy_stat {
 	unsigned int phydw0;
 	unsigned int phydw1;
 	unsigned int phydw2;
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_xmit.h b/drivers/staging/rtl8723au/include/rtl8723a_xmit.h
index 815560c6e1d7..7db29f40ab70 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_xmit.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_xmit.h
@@ -212,7 +212,6 @@ struct txrpt_ccx_8723a {
 #define txrpt_ccx_qtime_8723a(txrpt_ccx) ((txrpt_ccx)->ccx_qtime0+((txrpt_ccx)->ccx_qtime1<<8))
 
 void handle_txrpt_ccx_8723a(struct rtw_adapter *adapter, void *buf);
-void rtl8723a_update_txdesc(struct xmit_frame *pxmitframe, u8 *pmem);
 void rtl8723a_fill_fake_txdesc(struct rtw_adapter *padapter, u8 *pDesc, u32 BufferLen, u8 IsPsPoll, u8 IsBTQosNull);
 
 int rtl8723au_hal_xmitframe_enqueue(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe);
diff --git a/drivers/staging/rtl8723au/include/rtw_cmd.h b/drivers/staging/rtl8723au/include/rtw_cmd.h
index ef67068a5fe3..71044107d13b 100644
--- a/drivers/staging/rtl8723au/include/rtw_cmd.h
+++ b/drivers/staging/rtl8723au/include/rtw_cmd.h
@@ -17,7 +17,6 @@
 
 #include <wlan_bssdef.h>
 #include <rtw_rf.h>
-#include <rtw_led.h>
 
 #define C2H_MEM_SZ (16*1024)
 
@@ -450,8 +449,7 @@ struct getrfintfs_parm {
 	u8	rfintfs;
 };
 
-struct Tx_Beacon_param
-{
+struct Tx_Beacon_param {
 	struct wlan_bssid_ex network;
 };
 
diff --git a/drivers/staging/rtl8723au/include/rtw_ht.h b/drivers/staging/rtl8723au/include/rtw_ht.h
index cfc947daf08b..780eb8944118 100644
--- a/drivers/staging/rtl8723au/include/rtw_ht.h
+++ b/drivers/staging/rtl8723au/include/rtw_ht.h
@@ -19,8 +19,7 @@
 #include "linux/ieee80211.h"
 #include "wifi.h"
 
-struct ht_priv
-{
+struct ht_priv {
 	bool	ht_option;
 	bool	ampdu_enable;/* for enable Tx A-MPDU */
 	/* u8	baddbareq_issued[16]; */
diff --git a/drivers/staging/rtl8723au/include/rtw_led.h b/drivers/staging/rtl8723au/include/rtw_led.h
deleted file mode 100644
index c071da587efd..000000000000
--- a/drivers/staging/rtl8723au/include/rtw_led.h
+++ /dev/null
@@ -1,181 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 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.
- *
- ******************************************************************************/
-#ifndef __RTW_LED_H_
-#define __RTW_LED_H_
-
-#include <osdep_service.h>
-#include <drv_types.h>
-
-#define MSECS(t)        (HZ * ((t) / 1000) + (HZ * ((t) % 1000)) / 1000)
-
-#define LED_BLINK_NORMAL_INTERVAL	100
-#define LED_BLINK_SLOWLY_INTERVAL	200
-#define LED_BLINK_LONG_INTERVAL	400
-
-#define LED_BLINK_NO_LINK_INTERVAL_ALPHA		1000
-#define LED_BLINK_LINK_INTERVAL_ALPHA			500		/* 500 */
-#define LED_BLINK_SCAN_INTERVAL_ALPHA		180	/* 150 */
-#define LED_BLINK_FASTER_INTERVAL_ALPHA		50
-#define LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA	5000
-
-#define LED_BLINK_NORMAL_INTERVAL_NETTRONIX  100
-#define LED_BLINK_SLOWLY_INTERVAL_NETTRONIX  2000
-
-#define LED_BLINK_SLOWLY_INTERVAL_PORNET 1000
-#define LED_BLINK_NORMAL_INTERVAL_PORNET 100
-
-#define LED_BLINK_FAST_INTERVAL_BITLAND 30
-
-/*  060403, rcnjko: Customized for AzWave. */
-#define LED_CM2_BLINK_ON_INTERVAL			250
-#define LED_CM2_BLINK_OFF_INTERVAL		4750
-
-#define LED_CM8_BLINK_INTERVAL			500		/* for QMI */
-#define LED_CM8_BLINK_OFF_INTERVAL	3750	/* for QMI */
-
-/*  080124, lanhsin: Customized for RunTop */
-#define LED_RunTop_BLINK_INTERVAL			300
-
-/*  060421, rcnjko: Customized for Sercomm Printer Server case. */
-#define LED_CM3_BLINK_INTERVAL				1500
-
-enum led_ctl_mode {
-	LED_CTL_POWER_ON = 1,
-	LED_CTL_LINK = 2,
-	LED_CTL_NO_LINK = 3,
-	LED_CTL_TX = 4,
-	LED_CTL_RX = 5,
-	LED_CTL_SITE_SURVEY = 6,
-	LED_CTL_POWER_OFF = 7,
-	LED_CTL_START_TO_LINK = 8,
-	LED_CTL_START_WPS = 9,
-	LED_CTL_STOP_WPS = 10,
-	LED_CTL_START_WPS_BOTTON = 11, /* added for runtop */
-	LED_CTL_STOP_WPS_FAIL = 12, /* added for ALPHA */
-	LED_CTL_STOP_WPS_FAIL_OVERLAP = 13, /* added for BELKIN */
-	LED_CTL_CONNECTION_NO_TRANSFER = 14,
-};
-
-enum led_state_872x {
-	LED_UNKNOWN = 0,
-	RTW_LED_ON = 1,
-	RTW_LED_OFF = 2,
-	LED_BLINK_NORMAL = 3,
-	LED_BLINK_SLOWLY = 4,
-	LED_BLINK_POWER_ON = 5,
-	LED_BLINK_SCAN = 6, /*  LED is blinking during scanning period, the # of times to blink is depend on time for scanning. */
-	LED_BLINK_NO_LINK = 7, /*  LED is blinking during no link state. */
-	LED_BLINK_StartToBlink = 8,/*  Customzied for Sercomm Printer Server case */
-	LED_BLINK_TXRX = 9,
-	LED_BLINK_WPS = 10,	/*  LED is blinkg during WPS communication */
-	LED_BLINK_WPS_STOP = 11,	/* for ALPHA */
-	LED_BLINK_WPS_STOP_OVERLAP = 12,	/* for BELKIN */
-	LED_BLINK_RUNTOP = 13, /*  Customized for RunTop */
-	LED_BLINK_CAMEO = 14,
-	LED_BLINK_XAVI = 15,
-	LED_BLINK_ALWAYS_ON = 16,
-};
-
-enum led_pin_8723a {
-	LED_PIN_NULL = 0,
-	LED_PIN_LED0 = 1,
-	LED_PIN_LED1 = 2,
-	LED_PIN_LED2 = 3,
-	LED_PIN_GPIO0 = 4,
-};
-
-struct led_8723a {
-	struct rtw_adapter				*padapter;
-
-	enum led_pin_8723a		LedPin;	/*  Identify how to implement this SW led. */
-	enum led_state_872x		CurrLedState; /*  Current LED state. */
-	enum led_state_872x		BlinkingLedState; /*  Next state for blinking, either RTW_LED_ON or RTW_LED_OFF are. */
-
-	u8					bLedOn; /*  true if LED is ON, false if LED is OFF. */
-
-	u8					bLedBlinkInProgress; /*  true if it is blinking, false o.w.. */
-
-	u8					bLedWPSBlinkInProgress;
-
-	u32					BlinkTimes; /*  Number of times to toggle led state for blinking. */
-
-	struct timer_list			BlinkTimer; /*  Timer object for led blinking. */
-
-	u8					bSWLedCtrl;
-
-	/*  ALPHA, added by chiyoko, 20090106 */
-	u8					bLedNoLinkBlinkInProgress;
-	u8					bLedLinkBlinkInProgress;
-	u8					bLedStartToLinkBlinkInProgress;
-	u8					bLedScanBlinkInProgress;
-
-	struct work_struct			BlinkWorkItem; /*  Workitem used by BlinkTimer to manipulate H/W to blink LED. */
-};
-
-#define IS_LED_WPS_BLINKING(_LED_871x)	(((struct led_8723a *)_LED_871x)->CurrLedState==LED_BLINK_WPS \
-					|| ((struct led_8723a *)_LED_871x)->CurrLedState==LED_BLINK_WPS_STOP \
-					|| ((struct led_8723a *)_LED_871x)->bLedWPSBlinkInProgress)
-
-#define IS_LED_BLINKING(_LED_871x)	(((struct led_8723a *)_LED_871x)->bLedWPSBlinkInProgress \
-					||((struct led_8723a *)_LED_871x)->bLedScanBlinkInProgress)
-
-/*  */
-/*  LED customization. */
-/*  */
-
-enum led_strategy_8723a {
-	SW_LED_MODE0 = 0, /*  SW control 1 LED via GPIO0. It is default option. */
-	SW_LED_MODE1= 1, /*  2 LEDs, through LED0 and LED1. For ALPHA. */
-	SW_LED_MODE2 = 2, /*  SW control 1 LED via GPIO0, customized for AzWave 8187 minicard. */
-	SW_LED_MODE3 = 3, /*  SW control 1 LED via GPIO0, customized for Sercomm Printer Server case. */
-	SW_LED_MODE4 = 4, /* for Edimax / Belkin */
-	SW_LED_MODE5 = 5, /* for Sercomm / Belkin */
-	SW_LED_MODE6 = 6, /* for 88CU minicard, porting from ce SW_LED_MODE7 */
-	HW_LED = 50, /*  HW control 2 LEDs, LED0 and LED1 (there are 4 different control modes, see MAC.CONFIG1 for details.) */
-	LED_ST_NONE = 99,
-};
-
-void LedControl871x23a(struct rtw_adapter *padapter, enum led_ctl_mode LedAction);
-
-struct led_priv{
-	/* add for led controll */
-	struct led_8723a			SwLed0;
-	struct led_8723a			SwLed1;
-	enum led_strategy_8723a	LedStrategy;
-	u8					bRegUseLed;
-	void (*LedControlHandler)(struct rtw_adapter *padapter, enum led_ctl_mode LedAction);
-	/* add for led controll */
-};
-
-#define rtw_led_control(adapter, LedAction)
-
-void BlinkWorkItemCallback23a(struct work_struct *work);
-
-void ResetLedStatus23a(struct led_8723a *pLed);
-
-void
-InitLed871x23a(
-	struct rtw_adapter *padapter,
-	struct led_8723a *pLed,
-	enum led_pin_8723a	LedPin
-);
-
-void
-DeInitLed871x23a(struct led_8723a *pLed);
-
-/* hal... */
-void BlinkHandler23a(struct led_8723a *pLed);
-
-#endif /* __RTW_LED_H_ */
diff --git a/drivers/staging/rtl8723au/include/rtw_mlme.h b/drivers/staging/rtl8723au/include/rtw_mlme.h
index 2ff01eb8fc0c..a6751f138336 100644
--- a/drivers/staging/rtl8723au/include/rtw_mlme.h
+++ b/drivers/staging/rtl8723au/include/rtw_mlme.h
@@ -270,7 +270,7 @@ static inline void _clr_fwstate_(struct mlme_priv *pmlmepriv, int state)
 static inline void clr_fwstate(struct mlme_priv *pmlmepriv, int state)
 {
 	spin_lock_bh(&pmlmepriv->lock);
-	if (check_fwstate(pmlmepriv, state) == true)
+	if (check_fwstate(pmlmepriv, state))
 		pmlmepriv->fw_state ^= state;
 	spin_unlock_bh(&pmlmepriv->lock);
 }
diff --git a/drivers/staging/rtl8723au/include/rtw_mlme_ext.h b/drivers/staging/rtl8723au/include/rtw_mlme_ext.h
index 97c3c4478f29..51dba1fa4c5d 100644
--- a/drivers/staging/rtl8723au/include/rtw_mlme_ext.h
+++ b/drivers/staging/rtl8723au/include/rtw_mlme_ext.h
@@ -270,8 +270,7 @@ struct action_handler {
 	int (*func)(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
 };
 
-struct	ss_res
-{
+struct	ss_res {
 	int	state;
 	int	bss_cnt;
 	int	channel_idx;
@@ -318,8 +317,7 @@ struct FW_Sta_Info {
  * 5. ... and so on, till survey done.
  */
 
-struct mlme_ext_info
-{
+struct mlme_ext_info {
 	u32	state;
 	u32	reauth_count;
 	u32	reassoc_count;
diff --git a/drivers/staging/rtl8723au/include/rtw_recv.h b/drivers/staging/rtl8723au/include/rtw_recv.h
index f846bb5e7ab7..dc784be3ddd9 100644
--- a/drivers/staging/rtl8723au/include/rtw_recv.h
+++ b/drivers/staging/rtl8723au/include/rtw_recv.h
@@ -200,9 +200,6 @@ struct recv_priv {
 	u8 *precv_buf;
 
 	/* For display the phy informatiom */
-	u8 is_signal_dbg;	/*  for debug */
-	u8 signal_strength_dbg;	/*  for debug */
-	s8 rssi;
 	s8 rxpwdb;
 	u8 signal_strength;
 	u8 signal_qual;
diff --git a/drivers/staging/rtl8723au/include/rtw_xmit.h b/drivers/staging/rtl8723au/include/rtw_xmit.h
index 32a844170327..2b7d6d08238b 100644
--- a/drivers/staging/rtl8723au/include/rtw_xmit.h
+++ b/drivers/staging/rtl8723au/include/rtw_xmit.h
@@ -196,7 +196,6 @@ enum {
 void rtw_sctx_init23a(struct submit_ctx *sctx, int timeout_ms);
 int rtw_sctx_wait23a(struct submit_ctx *sctx);
 void rtw23a_sctx_done_err(struct submit_ctx **sctx, int status);
-void rtw_sctx_done23a(struct submit_ctx **sctx);
 
 struct xmit_buf {
 	struct list_head list, list2;
@@ -295,10 +294,6 @@ struct	xmit_priv {
 
 	struct rtw_adapter	*adapter;
 
-	u8   vcs_setting;
-	u8	vcs;
-	u8	vcs_type;
-
 	u64	tx_bytes;
 	u64	tx_pkts;
 	u64	tx_drop;
@@ -307,6 +302,8 @@ struct	xmit_priv {
 
 	struct hw_xmit *hwxmits;
 	u8	hwxmit_entry;
+	u8	vcs;
+	u8	nqos_ssn;
 
 	u8	wmm_para_seq[4];/* sequence for wmm ac parameter strength from
 				 * large to small. it's value is 0->vo, 1->vi,
@@ -314,14 +311,8 @@ struct	xmit_priv {
 				 */
 
 	struct semaphore	tx_retevt;/* all tx return event; */
-	u8		txirp_cnt;/*  */
 
 	struct tasklet_struct xmit_tasklet;
-	/* per AC pending irp */
-	int beq_cnt;
-	int bkq_cnt;
-	int viq_cnt;
-	int voq_cnt;
 
 	struct rtw_queue free_xmitbuf_queue;
 	struct list_head xmitbuf_list;		/* track buffers for cleanup */
@@ -332,7 +323,6 @@ struct	xmit_priv {
 	struct list_head xmitextbuf_list;	/* track buffers for cleanup */
 	uint free_xmit_extbuf_cnt;
 
-	u16	nqos_ssn;
 	int	ack_tx;
 	struct mutex ack_tx_mutex;
 	struct submit_ctx ack_tx_ops;
@@ -349,7 +339,6 @@ s32 rtw_free_xmitbuf23a(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf);
 void rtw_count_tx_stats23a(struct rtw_adapter *padapter,
 			struct xmit_frame *pxmitframe, int sz);
 void rtw_update_protection23a(struct rtw_adapter *padapter, u8 *ie, uint ie_len);
-s32 rtw_put_snap23a(u8 *data, u16 h_proto);
 struct xmit_frame *rtw_alloc_xmitframe23a_ext(struct xmit_priv *pxmitpriv);
 struct xmit_frame *rtw_alloc_xmitframe23a_once(struct xmit_priv *pxmitpriv);
 s32 rtw_free_xmitframe23a(struct xmit_priv *pxmitpriv,
@@ -363,8 +352,6 @@ struct xmit_frame *rtw_dequeue_xframe23a(struct xmit_priv *pxmitpriv,
 				      struct hw_xmit *phwxmit_i, int entry);
 s32 rtw_xmit23a_classifier(struct rtw_adapter *padapter,
 			struct xmit_frame *pxmitframe);
-u32 rtw_calculate_wlan_pkt_size_by_attribue23a(struct pkt_attrib *pattrib);
-#define rtw_wlan_pkt_size(f) rtw_calculate_wlan_pkt_size_by_attribue23a(&f->attrib)
 s32 rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *pkt,
 			      struct xmit_frame *pxmitframe);
 s32 _rtw_init_hw_txqueue(struct hw_txqueue *phw_txqueue, u8 ac_tag);
@@ -391,7 +378,6 @@ void xmit_delivery_enabled_frames23a(struct rtw_adapter *padapter,
 u8	qos_acm23a(u8 acm_mask, u8 priority);
 u32	rtw_get_ff_hwaddr23a(struct xmit_frame	*pxmitframe);
 int rtw_ack_tx_wait23a(struct xmit_priv *pxmitpriv, u32 timeout_ms);
-void rtw_ack_tx_done23a(struct xmit_priv *pxmitpriv, int status);
 
 /* include after declaring struct xmit_buf, in order to avoid warning */
 #include <xmit_osdep.h>
diff --git a/drivers/staging/rtl8723au/include/usb_ops.h b/drivers/staging/rtl8723au/include/usb_ops.h
index ade8bc71572a..ff11e13b24a8 100644
--- a/drivers/staging/rtl8723au/include/usb_ops.h
+++ b/drivers/staging/rtl8723au/include/usb_ops.h
@@ -63,6 +63,6 @@ static inline void rtw_reset_continual_urb_error(struct dvobj_priv *dvobj)
 	atomic_set(&dvobj->continual_urb_error, 0);
 }
 
-void rtl8723au_chip_configure(struct rtw_adapter *padapter);
+bool rtl8723au_chip_configure(struct rtw_adapter *padapter);
 
 #endif /* __USB_OPS_H_ */
diff --git a/drivers/staging/rtl8723au/include/usb_ops_linux.h b/drivers/staging/rtl8723au/include/usb_ops_linux.h
index bf68bbb41f9c..af2f14b8b360 100644
--- a/drivers/staging/rtl8723au/include/usb_ops_linux.h
+++ b/drivers/staging/rtl8723au/include/usb_ops_linux.h
@@ -21,13 +21,13 @@
 
 #define MAX_USBCTRL_VENDORREQ_TIMES	10
 
-int rtl8723au_read_port(struct rtw_adapter *adapter, u32 addr, u32 cnt,
+int rtl8723au_read_port(struct rtw_adapter *adapter, u32 cnt,
 			struct recv_buf *precvbuf);
 void rtl8723au_read_port_cancel(struct rtw_adapter *padapter);
 int rtl8723au_write_port(struct rtw_adapter *padapter, u32 addr, u32 cnt,
 			 struct xmit_buf *pxmitbuf);
 void rtl8723au_write_port_cancel(struct rtw_adapter *padapter);
-int rtl8723au_read_interrupt(struct rtw_adapter *adapter, u32 addr);
+int rtl8723au_read_interrupt(struct rtw_adapter *adapter);
 
 u8 rtl8723au_read8(struct rtw_adapter *padapter, u16 addr);
 u16 rtl8723au_read16(struct rtw_adapter *padapter, u16 addr);
diff --git a/drivers/staging/rtl8723au/include/wlan_bssdef.h b/drivers/staging/rtl8723au/include/wlan_bssdef.h
index 96e8074a7c18..95b32e15a4d0 100644
--- a/drivers/staging/rtl8723au/include/wlan_bssdef.h
+++ b/drivers/staging/rtl8723au/include/wlan_bssdef.h
@@ -57,23 +57,6 @@ enum  {
 	Ndis802_11Encryption3KeyAbsent,
 };
 
-/*  Key mapping keys require a BSSID */
-struct ndis_802_11_key {
-	u32 Length;             /*  Length of this structure */
-	u32 KeyIndex;
-	u32 KeyLength;          /*  length of key in bytes */
-	unsigned char BSSID[6];
-	unsigned long long KeyRSC;
-	u8 KeyMaterial[32]; /*  variable length depending on above field */
-};
-
-struct wlan_phy_info {
-	u8	SignalStrength;/* in percentage) */
-	u8	SignalQuality;/* in percentage) */
-	u8	Optimum_antenna;  /* for Antenna diversity */
-	u8	Reserved_0;
-};
-
 struct wlan_bcn_info {
 	/* these infor get from rtw_get_encrypt_info when
 	 *	 * translate scan to UI */
@@ -99,7 +82,8 @@ struct wlan_bssid_ex {
 	u32 DSConfig;           /*  Frequency, units are kHz */
 	enum nl80211_iftype ifmode;
 	unsigned char SupportedRates[NDIS_802_11_LENGTH_RATES_EX];
-	struct wlan_phy_info	PhyInfo;
+	u8 SignalStrength;/* in percentage */
+	u8 SignalQuality;/* in percentage */
 	u32  IELength;
 	u8  IEs[MAX_IE_SZ]; /* timestamp, beacon interval, and capability info*/
 } __packed;
@@ -115,7 +99,6 @@ struct	wlan_network {
 	/*  set to fixed when not to be removed as site-surveying */
 	int	fixed;
 	unsigned long	last_scanned; /* timestamp for the network */
-	int	aid;		/* will only be valid when a BSS is joined. */
 	int	join_res;
 	struct wlan_bssid_ex	network; /* must be the last item */
 	struct wlan_bcn_info	BcnInfo;
diff --git a/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c b/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c
index c76874d72a22..c5800ae71fcf 100644
--- a/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c
+++ b/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c
@@ -275,7 +275,8 @@ static int rtw_cfg80211_inform_bss(struct rtw_adapter *padapter,
 			    &pnetwork->network)) {
 		notify_signal = 100 * translate_percentage_to_dbm(padapter->recvpriv.signal_strength);	/* dbm */
 	} else {
-		notify_signal = 100 * translate_percentage_to_dbm(pnetwork->network.PhyInfo.SignalStrength);	/* dbm */
+		notify_signal = 100 * translate_percentage_to_dbm(
+			pnetwork->network.SignalStrength);	/* dbm */
 	}
 
 	bss = cfg80211_inform_bss(wiphy, notify_channel,
@@ -471,7 +472,6 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev, u8 key_index,
 					  int set_tx, const u8 *sta_addr,
 					  struct key_params *keyparms)
 {
-	int ret = 0;
 	int key_len;
 	struct sta_info *psta = NULL, *pbcmc_sta = NULL;
 	struct rtw_adapter *padapter = netdev_priv(dev);
@@ -708,7 +708,7 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev, u8 key_index,
 
 exit:
 
-	return ret;
+	return 0;
 }
 #endif
 
@@ -850,7 +850,6 @@ static int rtw_cfg80211_set_encryption(struct net_device *dev, u8 key_index,
 					    dot11PrivacyAlgrthm;
 				}
 			}
-		} else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {	/* adhoc mode */
 		}
 	}
 
@@ -2431,20 +2430,16 @@ void rtw_cfg80211_indicate_sta_disassoc(struct rtw_adapter *padapter,
 
 static int rtw_cfg80211_monitor_if_open(struct net_device *ndev)
 {
-	int ret = 0;
-
 	DBG_8723A("%s\n", __func__);
 
-	return ret;
+	return 0;
 }
 
 static int rtw_cfg80211_monitor_if_close(struct net_device *ndev)
 {
-	int ret = 0;
-
 	DBG_8723A("%s\n", __func__);
 
-	return ret;
+	return 0;
 }
 
 static int rtw_cfg80211_monitor_if_xmit_entry(struct sk_buff *skb,
@@ -2573,11 +2568,9 @@ fail:
 static int
 rtw_cfg80211_monitor_if_set_mac_address(struct net_device *ndev, void *addr)
 {
-	int ret = 0;
-
 	DBG_8723A("%s\n", __func__);
 
-	return ret;
+	return 0;
 }
 
 static const struct net_device_ops rtw_cfg80211_monitor_if_ops = {
diff --git a/drivers/staging/rtl8723au/os_dep/os_intfs.c b/drivers/staging/rtl8723au/os_dep/os_intfs.c
index b34eaec9dd48..9966d16342b3 100644
--- a/drivers/staging/rtl8723au/os_dep/os_intfs.c
+++ b/drivers/staging/rtl8723au/os_dep/os_intfs.c
@@ -175,7 +175,6 @@ static int netdev_close(struct net_device *pnetdev);
 static int loadparam(struct rtw_adapter *padapter,  struct net_device *pnetdev)
 {
 	struct registry_priv  *registry_par = &padapter->registrypriv;
-	int status = _SUCCESS;
 
 	GlobalDebugLevel23A = rtw_debug;
 	registry_par->chip_version = (u8)rtw_chip_version;
@@ -234,7 +233,7 @@ static int loadparam(struct rtw_adapter *padapter,  struct net_device *pnetdev)
 	snprintf(registry_par->if2name, 16, "%s", if2name);
 	registry_par->notch_filter = (u8)rtw_notch_filter;
 	registry_par->regulatory_tid = (u8)rtw_regulatory_id;
-	return status;
+	return _SUCCESS;
 }
 
 static int rtw_net_set_mac_address(struct net_device *pnetdev, void *p)
@@ -384,12 +383,9 @@ static int rtw_init_default_value(struct rtw_adapter *padapter)
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
-	int ret = _SUCCESS;
 
 	/* xmit_priv */
-	pxmitpriv->vcs_setting = pregistrypriv->vrtl_carrier_sense;
 	pxmitpriv->vcs = pregistrypriv->vcs_type;
-	pxmitpriv->vcs_type = pregistrypriv->vcs_type;
 	/* pxmitpriv->rts_thresh = pregistrypriv->rts_thresh; */
 	pxmitpriv->frag_len = pregistrypriv->frag_thresh;
 
@@ -425,7 +421,7 @@ static int rtw_init_default_value(struct rtw_adapter *padapter)
 	/* misc. */
 	padapter->bReadPortCancel = false;
 	padapter->bWritePortCancel = false;
-	return ret;
+	return _SUCCESS;
 }
 
 int rtw_reset_drv_sw23a(struct rtw_adapter *padapter)
@@ -546,9 +542,6 @@ void rtw_cancel_all_timer23a(struct rtw_adapter *padapter)
 	RT_TRACE(_module_os_intfs_c_, _drv_info_,
 		 ("%s:cancel dynamic_chk_timer!\n", __func__));
 
-	RT_TRACE(_module_os_intfs_c_, _drv_info_,
-		 ("%s:cancel DeInitSwLeds!\n", __func__));
-
 	del_timer_sync(&padapter->pwrctrlpriv.pwr_state_check_timer);
 
 	del_timer_sync(&padapter->mlmepriv.set_scan_deny_timer);
@@ -685,8 +678,6 @@ int netdev_open23a(struct net_device *pnetdev)
 
 		rtw_cfg80211_init_wiphy(padapter);
 
-		rtw_led_control(padapter, LED_CTL_NO_LINK);
-
 		padapter->bup = true;
 	}
 	padapter->net_closed = false;
@@ -768,8 +759,6 @@ int rtw_ips_pwr_up23a(struct rtw_adapter *padapter)
 
 	result = ips_netdrv_open(padapter);
 
-	rtw_led_control(padapter, LED_CTL_NO_LINK);
-
 	DBG_8723A("<===  rtw_ips_pwr_up23a.............. in %dms\n",
 		  jiffies_to_msecs(jiffies - start_time));
 	return result;
@@ -784,8 +773,6 @@ void rtw_ips_pwr_down23a(struct rtw_adapter *padapter)
 	padapter->bCardDisableWOHSM = true;
 	padapter->net_closed = true;
 
-	rtw_led_control(padapter, LED_CTL_POWER_OFF);
-
 	rtw_ips_dev_unload23a(padapter);
 	padapter->bCardDisableWOHSM = false;
 	DBG_8723A("<=== rtw_ips_pwr_down23a..................... in %dms\n",
@@ -844,8 +831,6 @@ static int netdev_close(struct net_device *pnetdev)
 		rtw_free_assoc_resources23a(padapter, 1);
 		/* s2-4. */
 		rtw_free_network_queue23a(padapter);
-		/*  Close LED */
-		rtw_led_control(padapter, LED_CTL_POWER_OFF);
 	}
 
 	rtw_scan_abort23a(padapter);
diff --git a/drivers/staging/rtl8723au/os_dep/usb_intf.c b/drivers/staging/rtl8723au/os_dep/usb_intf.c
index 865743ecd855..373a617ace54 100644
--- a/drivers/staging/rtl8723au/os_dep/usb_intf.c
+++ b/drivers/staging/rtl8723au/os_dep/usb_intf.c
@@ -59,21 +59,6 @@ static struct usb_driver rtl8723a_usb_drv = {
 
 static struct usb_driver *usb_drv = &rtl8723a_usb_drv;
 
-static inline int RT_usb_endpoint_is_bulk_in(const struct usb_endpoint_descriptor *epd)
-{
-	return usb_endpoint_xfer_bulk(epd) && usb_endpoint_dir_in(epd);
-}
-
-static inline int RT_usb_endpoint_is_bulk_out(const struct usb_endpoint_descriptor *epd)
-{
-	return usb_endpoint_xfer_bulk(epd) && usb_endpoint_dir_out(epd);
-}
-
-static inline int RT_usb_endpoint_is_int_in(const struct usb_endpoint_descriptor *epd)
-{
-	return usb_endpoint_xfer_int(epd) && usb_endpoint_dir_in(epd);
-}
-
 static int rtw_init_intf_priv(struct dvobj_priv *dvobj)
 {
 	mutex_init(&dvobj->usb_vendor_req_mutex);
@@ -143,21 +128,21 @@ static struct dvobj_priv *usb_dvobj_init(struct usb_interface *usb_intf)
 				  le16_to_cpu(pendp_desc->wMaxPacketSize));
 			DBG_8723A("bInterval =%x\n", pendp_desc->bInterval);
 
-			if (RT_usb_endpoint_is_bulk_in(pendp_desc)) {
-				DBG_8723A("RT_usb_endpoint_is_bulk_in = %x\n",
+			if (usb_endpoint_is_bulk_in(pendp_desc)) {
+				DBG_8723A("usb_endpoint_is_bulk_in = %x\n",
 					  usb_endpoint_num(pendp_desc));
 				pdvobjpriv->RtInPipe[pdvobjpriv->RtNumInPipes] =
 					usb_endpoint_num(pendp_desc);
 				pdvobjpriv->RtNumInPipes++;
-			} else if (RT_usb_endpoint_is_int_in(pendp_desc)) {
-				DBG_8723A("RT_usb_endpoint_is_int_in = %x, Interval = %x\n",
+			} else if (usb_endpoint_is_int_in(pendp_desc)) {
+				DBG_8723A("usb_endpoint_is_int_in = %x, Interval = %x\n",
 					  usb_endpoint_num(pendp_desc),
 					  pendp_desc->bInterval);
 				pdvobjpriv->RtInPipe[pdvobjpriv->RtNumInPipes] =
 					usb_endpoint_num(pendp_desc);
 				pdvobjpriv->RtNumInPipes++;
-			} else if (RT_usb_endpoint_is_bulk_out(pendp_desc)) {
-				DBG_8723A("RT_usb_endpoint_is_bulk_out = %x\n",
+			} else if (usb_endpoint_is_bulk_out(pendp_desc)) {
+				DBG_8723A("usb_endpoint_is_bulk_out = %x\n",
 					  usb_endpoint_num(pendp_desc));
 				pdvobjpriv->RtOutPipe[pdvobjpriv->RtNumOutPipes] =
 					usb_endpoint_num(pendp_desc);
@@ -257,6 +242,7 @@ void rtl8723a_usb_intf_stop(struct rtw_adapter *padapter)
 
 static void rtw_dev_unload(struct rtw_adapter *padapter)
 {
+	struct submit_ctx *pack_tx_ops = &padapter->xmitpriv.ack_tx_ops;
 	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("+rtw_dev_unload\n"));
 
 	if (padapter->bup) {
@@ -264,8 +250,8 @@ static void rtw_dev_unload(struct rtw_adapter *padapter)
 
 		padapter->bDriverStopped = true;
 		if (padapter->xmitpriv.ack_tx)
-			rtw_ack_tx_done23a(&padapter->xmitpriv,
-					RTW_SCTX_DONE_DRV_STOP);
+			rtw23a_sctx_done_err(&pack_tx_ops,
+					     RTW_SCTX_DONE_DRV_STOP);
 
 		/* s3. */
 		rtl8723a_usb_intf_stop(padapter);
@@ -322,8 +308,6 @@ int rtw_hw_suspend23a(struct rtw_adapter *padapter)
 		if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 			_clr_fwstate_(pmlmepriv, _FW_LINKED);
 
-			rtw_led_control(padapter, LED_CTL_NO_LINK);
-
 			rtw_os_indicate_disconnect23a(padapter);
 
 			/* donnot enqueue cmd */
@@ -546,7 +530,8 @@ static struct rtw_adapter *rtw_usb_if1_init(struct dvobj_priv *dvobj,
 	rtl8723a_read_chip_version(padapter);
 
 	/* step usb endpoint mapping */
-	rtl8723au_chip_configure(padapter);
+	if (!rtl8723au_chip_configure(padapter))
+		goto free_hal_data;
 
 	/* step read efuse/eeprom data and get mac_addr */
 	rtl8723a_read_adapter_info(padapter);
diff --git a/drivers/staging/rtl8723au/os_dep/usb_ops_linux.c b/drivers/staging/rtl8723au/os_dep/usb_ops_linux.c
index a3349ac57bae..3e19b3b2c1c2 100644
--- a/drivers/staging/rtl8723au/os_dep/usb_ops_linux.c
+++ b/drivers/staging/rtl8723au/os_dep/usb_ops_linux.c
@@ -18,13 +18,6 @@
 #include <usb_ops_linux.h>
 #include <rtw_sreset.h>
 
-struct zero_bulkout_context {
-	void *pbuf;
-	void *purb;
-	void *pirp;
-	void *padapter;
-};
-
 void rtl8723au_read_port_cancel(struct rtw_adapter *padapter)
 {
 	struct recv_buf *precvbuf;
@@ -53,18 +46,6 @@ static void usb_write_port23a_complete(struct urb *purb)
 	unsigned long irqL;
 
 	switch (pxmitbuf->flags) {
-	case VO_QUEUE_INX:
-		pxmitpriv->voq_cnt--;
-		break;
-	case VI_QUEUE_INX:
-		pxmitpriv->viq_cnt--;
-		break;
-	case BE_QUEUE_INX:
-		pxmitpriv->beq_cnt--;
-		break;
-	case BK_QUEUE_INX:
-		pxmitpriv->bkq_cnt--;
-		break;
 	case HIGH_QUEUE_INX:
 #ifdef CONFIG_8723AU_AP_MODE
 		rtw_chk_hi_queue_cmd23a(padapter);
@@ -166,19 +147,15 @@ int rtl8723au_write_port(struct rtw_adapter *padapter, u32 addr, u32 cnt,
 
 	switch (addr) {
 	case VO_QUEUE_INX:
-		pxmitpriv->voq_cnt++;
 		pxmitbuf->flags = VO_QUEUE_INX;
 		break;
 	case VI_QUEUE_INX:
-		pxmitpriv->viq_cnt++;
 		pxmitbuf->flags = VI_QUEUE_INX;
 		break;
 	case BE_QUEUE_INX:
-		pxmitpriv->beq_cnt++;
 		pxmitbuf->flags = BE_QUEUE_INX;
 		break;
 	case BK_QUEUE_INX:
-		pxmitpriv->bkq_cnt++;
 		pxmitbuf->flags = BK_QUEUE_INX;
 		break;
 	case HIGH_QUEUE_INX:
diff --git a/drivers/staging/rts5208/ms.c b/drivers/staging/rts5208/ms.c
index 228e48339b9e..b4612fb615f6 100644
--- a/drivers/staging/rts5208/ms.c
+++ b/drivers/staging/rts5208/ms.c
@@ -2599,9 +2599,9 @@ static int mspro_rw_multi_sector(struct scsi_cmnd *srb,
 
 			if (count > sector_cnt) {
 				if (mode_2k)
-					ms_card->seq_mode |= MODE_2K_SEQ;
+					ms_card->seq_mode = MODE_2K_SEQ;
 				else
-					ms_card->seq_mode |= MODE_512_SEQ;
+					ms_card->seq_mode = MODE_512_SEQ;
 			}
 		} else {
 			count = sector_cnt;
diff --git a/drivers/staging/rts5208/rtsx.c b/drivers/staging/rts5208/rtsx.c
index 2d2527c3aea2..c74f1b8108f6 100644
--- a/drivers/staging/rts5208/rtsx.c
+++ b/drivers/staging/rts5208/rtsx.c
@@ -418,7 +418,7 @@ static void rtsx_shutdown(struct pci_dev *pci)
 
 static int rtsx_control_thread(void *__dev)
 {
-	struct rtsx_dev *dev = (struct rtsx_dev *)__dev;
+	struct rtsx_dev *dev = __dev;
 	struct rtsx_chip *chip = dev->chip;
 	struct Scsi_Host *host = rtsx_to_host(dev);
 
@@ -527,7 +527,7 @@ SkipForAbort:
 
 static int rtsx_polling_thread(void *__dev)
 {
-	struct rtsx_dev *dev = (struct rtsx_dev *)__dev;
+	struct rtsx_dev *dev = __dev;
 	struct rtsx_chip *chip = dev->chip;
 	struct sd_info *sd_card = &(chip->sd_card);
 	struct xd_info *xd_card = &(chip->xd_card);
diff --git a/drivers/staging/rts5208/rtsx_chip.c b/drivers/staging/rts5208/rtsx_chip.c
index a7ade8b4e7f2..9593d8132938 100644
--- a/drivers/staging/rts5208/rtsx_chip.c
+++ b/drivers/staging/rts5208/rtsx_chip.c
@@ -126,10 +126,11 @@ static int rtsx_pre_handle_sdio_old(struct rtsx_chip *chip)
 	if (chip->ignore_sd && CHK_SDIO_EXIST(chip)) {
 		if (chip->asic_code) {
 			RTSX_WRITE_REG(chip, CARD_PULL_CTL5, 0xFF,
-				MS_INS_PU | SD_WP_PU | SD_CD_PU | SD_CMD_PU);
+				       MS_INS_PU | SD_WP_PU |
+				       SD_CD_PU | SD_CMD_PU);
 		} else {
 			RTSX_WRITE_REG(chip, FPGA_PULL_CTL, 0xFF,
-				FPGA_SD_PULL_CTL_EN);
+				       FPGA_SD_PULL_CTL_EN);
 		}
 		RTSX_WRITE_REG(chip, CARD_SHARE_MODE, 0xFF, CARD_SHARE_48_SD);
 
@@ -137,7 +138,7 @@ static int rtsx_pre_handle_sdio_old(struct rtsx_chip *chip)
 		RTSX_WRITE_REG(chip, 0xFF2C, 0x01, 0x01);
 
 		RTSX_WRITE_REG(chip, SDIO_CTRL, 0xFF,
-			SDIO_BUS_CTRL | SDIO_CD_CTRL);
+			       SDIO_BUS_CTRL | SDIO_CD_CTRL);
 
 		chip->sd_int = 1;
 		chip->sd_io = 1;
@@ -201,7 +202,7 @@ static int rtsx_pre_handle_sdio_new(struct rtsx_chip *chip)
 					TRACE_RET(chip, STATUS_FAIL);
 			} else {
 				RTSX_WRITE_REG(chip, FPGA_PULL_CTL,
-					FPGA_SD_PULL_CTL_BIT | 0x20, 0);
+					       FPGA_SD_PULL_CTL_BIT | 0x20, 0);
 			}
 			retval = card_share_mode(chip, SD_CARD);
 			if (retval != STATUS_SUCCESS)
@@ -226,6 +227,87 @@ static int rtsx_pre_handle_sdio_new(struct rtsx_chip *chip)
 }
 #endif
 
+static int rtsx_reset_aspm(struct rtsx_chip *chip)
+{
+	int ret;
+
+	if (chip->dynamic_aspm) {
+		if (!CHK_SDIO_EXIST(chip) || !CHECK_PID(chip, 0x5288))
+			return STATUS_SUCCESS;
+
+		ret = rtsx_write_cfg_dw(chip, 2, 0xC0, 0xFF,
+					chip->aspm_l0s_l1_en);
+		if (ret != STATUS_SUCCESS)
+			TRACE_RET(chip, STATUS_FAIL);
+
+		return STATUS_SUCCESS;
+	}
+
+	if (CHECK_PID(chip, 0x5208))
+		RTSX_WRITE_REG(chip, ASPM_FORCE_CTL, 0xFF, 0x3F);
+	ret = rtsx_write_config_byte(chip, LCTLR, chip->aspm_l0s_l1_en);
+	if (ret != STATUS_SUCCESS)
+		TRACE_RET(chip, STATUS_FAIL);
+
+	chip->aspm_level[0] = chip->aspm_l0s_l1_en;
+	if (CHK_SDIO_EXIST(chip)) {
+		chip->aspm_level[1] = chip->aspm_l0s_l1_en;
+		ret = rtsx_write_cfg_dw(chip, CHECK_PID(chip, 0x5288) ? 2 : 1,
+					0xC0, 0xFF, chip->aspm_l0s_l1_en);
+		if (ret != STATUS_SUCCESS)
+			TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	chip->aspm_enabled = 1;
+
+	return STATUS_SUCCESS;
+}
+
+static int rtsx_enable_pcie_intr(struct rtsx_chip *chip)
+{
+	int ret;
+
+	if (!chip->asic_code || !CHECK_PID(chip, 0x5208)) {
+		rtsx_enable_bus_int(chip);
+		return STATUS_SUCCESS;
+	}
+
+	if (chip->phy_debug_mode) {
+		RTSX_WRITE_REG(chip, CDRESUMECTL, 0x77, 0);
+		rtsx_disable_bus_int(chip);
+	} else {
+		rtsx_enable_bus_int(chip);
+	}
+
+	if (chip->ic_version >= IC_VER_D) {
+		u16 reg;
+
+		ret = rtsx_read_phy_register(chip, 0x00, &reg);
+		if (ret != STATUS_SUCCESS)
+			TRACE_RET(chip, STATUS_FAIL);
+
+		reg &= 0xFE7F;
+		reg |= 0x80;
+		ret = rtsx_write_phy_register(chip, 0x00, reg);
+		if (ret != STATUS_SUCCESS)
+			TRACE_RET(chip, STATUS_FAIL);
+
+		ret = rtsx_read_phy_register(chip, 0x1C, &reg);
+		if (ret != STATUS_SUCCESS)
+			TRACE_RET(chip, STATUS_FAIL);
+
+		reg &= 0xFFF7;
+		ret = rtsx_write_phy_register(chip, 0x1C, reg);
+		if (ret != STATUS_SUCCESS)
+			TRACE_RET(chip, STATUS_FAIL);
+	}
+
+	if (chip->driver_first_load && (chip->ic_version < IC_VER_C))
+		rtsx_calibration(chip);
+
+	return STATUS_SUCCESS;
+}
+
 int rtsx_reset_chip(struct rtsx_chip *chip)
 {
 	int retval;
@@ -268,7 +350,7 @@ int rtsx_reset_chip(struct rtsx_chip *chip)
 
 #ifdef LED_AUTO_BLINK
 	RTSX_WRITE_REG(chip, CARD_AUTO_BLINK, 0xFF,
-			LED_BLINK_SPEED | BLINK_EN | LED_GPIO0);
+		       LED_BLINK_SPEED | BLINK_EN | LED_GPIO0);
 #endif
 
 	if (chip->asic_code) {
@@ -288,39 +370,9 @@ int rtsx_reset_chip(struct rtsx_chip *chip)
 
 	/* Enable ASPM */
 	if (chip->aspm_l0s_l1_en) {
-		if (chip->dynamic_aspm) {
-			if (CHK_SDIO_EXIST(chip)) {
-				if (CHECK_PID(chip, 0x5288)) {
-					retval = rtsx_write_cfg_dw(chip, 2, 0xC0, 0xFF, chip->aspm_l0s_l1_en);
-					if (retval != STATUS_SUCCESS)
-						TRACE_RET(chip, STATUS_FAIL);
-				}
-			}
-		} else {
-			if (CHECK_PID(chip, 0x5208))
-				RTSX_WRITE_REG(chip, ASPM_FORCE_CTL,
-					0xFF, 0x3F);
-
-			retval = rtsx_write_config_byte(chip, LCTLR,
-							chip->aspm_l0s_l1_en);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
-
-			chip->aspm_level[0] = chip->aspm_l0s_l1_en;
-			if (CHK_SDIO_EXIST(chip)) {
-				chip->aspm_level[1] = chip->aspm_l0s_l1_en;
-				if (CHECK_PID(chip, 0x5288))
-					retval = rtsx_write_cfg_dw(chip, 2, 0xC0, 0xFF, chip->aspm_l0s_l1_en);
-				else
-					retval = rtsx_write_cfg_dw(chip, 1, 0xC0, 0xFF, chip->aspm_l0s_l1_en);
-
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, STATUS_FAIL);
-
-			}
-
-			chip->aspm_enabled = 1;
-		}
+		retval = rtsx_reset_aspm(chip);
+		if (retval != STATUS_SUCCESS)
+			TRACE_RET(chip, STATUS_FAIL);
 	} else {
 		if (chip->asic_code && CHECK_PID(chip, 0x5208)) {
 			retval = rtsx_write_phy_register(chip, 0x07, 0x0129);
@@ -338,91 +390,38 @@ int rtsx_reset_chip(struct rtsx_chip *chip)
 		TRACE_RET(chip, STATUS_FAIL);
 
 	if (CHK_SDIO_EXIST(chip)) {
-		if (CHECK_PID(chip, 0x5288))
-			retval = rtsx_write_cfg_dw(chip, 2, 0xC0,
-						0xFF00, 0x0100);
-		else
-			retval = rtsx_write_cfg_dw(chip, 1, 0xC0,
-						0xFF00, 0x0100);
+		retval = rtsx_write_cfg_dw(chip,
+					   CHECK_PID(chip, 0x5288) ? 2 : 1,
+					   0xC0, 0xFF00, 0x0100);
 
 		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-
 	}
 
-	if (CHECK_PID(chip, 0x5288)) {
-		if (!CHK_SDIO_EXIST(chip)) {
-			retval = rtsx_write_cfg_dw(chip, 2, 0xC0, 0xFFFF,
-						0x0103);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
-
-			retval = rtsx_write_cfg_dw(chip, 2, 0x84, 0xFF, 0x03);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
+	if (CHECK_PID(chip, 0x5288) && !CHK_SDIO_EXIST(chip)) {
+		retval = rtsx_write_cfg_dw(chip, 2, 0xC0, 0xFFFF, 0x0103);
+		if (retval != STATUS_SUCCESS)
+			TRACE_RET(chip, STATUS_FAIL);
 
-		}
+		retval = rtsx_write_cfg_dw(chip, 2, 0x84, 0xFF, 0x03);
+		if (retval != STATUS_SUCCESS)
+			TRACE_RET(chip, STATUS_FAIL);
 	}
 
 	RTSX_WRITE_REG(chip, IRQSTAT0, LINK_RDY_INT, LINK_RDY_INT);
 
 	RTSX_WRITE_REG(chip, PERST_GLITCH_WIDTH, 0xFF, 0x80);
 
-	/* Enable PCIE interrupt */
-	if (chip->asic_code) {
-		if (CHECK_PID(chip, 0x5208)) {
-			if (chip->phy_debug_mode) {
-				RTSX_WRITE_REG(chip, CDRESUMECTL, 0x77, 0);
-				rtsx_disable_bus_int(chip);
-			} else {
-				rtsx_enable_bus_int(chip);
-			}
-
-			if (chip->ic_version >= IC_VER_D) {
-				u16 reg;
-
-				retval = rtsx_read_phy_register(chip, 0x00,
-								&reg);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, STATUS_FAIL);
-
-				reg &= 0xFE7F;
-				reg |= 0x80;
-				retval = rtsx_write_phy_register(chip, 0x00,
-								reg);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, STATUS_FAIL);
-
-				retval = rtsx_read_phy_register(chip, 0x1C,
-								&reg);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, STATUS_FAIL);
-
-				reg &= 0xFFF7;
-				retval = rtsx_write_phy_register(chip, 0x1C,
-								reg);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, STATUS_FAIL);
-
-			}
-
-			if (chip->driver_first_load &&
-				(chip->ic_version < IC_VER_C))
-				rtsx_calibration(chip);
-
-		} else {
-			rtsx_enable_bus_int(chip);
-		}
-	} else {
-		rtsx_enable_bus_int(chip);
-	}
+	retval = rtsx_enable_pcie_intr(chip);
+	if (retval != STATUS_SUCCESS)
+		TRACE_RET(chip, STATUS_FAIL);
 
 	chip->need_reset = 0;
 
 	chip->int_reg = rtsx_readl(chip, RTSX_BIPR);
 
 	if (chip->hw_bypass_sd)
-		goto NextCard;
+		goto nextcard;
 	dev_dbg(rtsx_dev(chip), "In %s, chip->int_reg = 0x%x\n", __func__,
 		chip->int_reg);
 	if (chip->int_reg & SD_EXIST) {
@@ -443,10 +442,10 @@ int rtsx_reset_chip(struct rtsx_chip *chip)
 	} else {
 		chip->sd_io = 0;
 		RTSX_WRITE_REG(chip, SDIO_CTRL, SDIO_BUS_CTRL | SDIO_CD_CTRL,
-			0);
+			       0);
 	}
 
-NextCard:
+nextcard:
 	if (chip->int_reg & XD_EXIST)
 		chip->need_reset |= XD_CARD;
 	if (chip->int_reg & MS_EXIST)
@@ -484,10 +483,10 @@ NextCard:
 
 	if (chip->ft2_fast_mode) {
 		RTSX_WRITE_REG(chip, CARD_PWR_CTL, 0xFF,
-			MS_PARTIAL_POWER_ON | SD_PARTIAL_POWER_ON);
+			       MS_PARTIAL_POWER_ON | SD_PARTIAL_POWER_ON);
 		udelay(chip->pmos_pwr_on_interval);
 		RTSX_WRITE_REG(chip, CARD_PWR_CTL, 0xFF,
-			MS_POWER_ON | SD_POWER_ON);
+			       MS_POWER_ON | SD_POWER_ON);
 
 		wait_timeout(200);
 	}
@@ -540,10 +539,7 @@ static int rts5208_init(struct rtsx_chip *chip)
 
 	RTSX_WRITE_REG(chip, CLK_SEL, 0x03, 0x03);
 	RTSX_READ_REG(chip, CLK_SEL, &val);
-	if (val == 0)
-		chip->asic_code = 1;
-	else
-		chip->asic_code = 0;
+	chip->asic_code = val == 0 ? 1 : 0;
 
 	if (chip->asic_code) {
 		retval = rtsx_read_phy_register(chip, 0x1C, &reg);
@@ -553,10 +549,7 @@ static int rts5208_init(struct rtsx_chip *chip)
 		dev_dbg(rtsx_dev(chip), "Value of phy register 0x1C is 0x%x\n",
 			reg);
 		chip->ic_version = (reg >> 4) & 0x07;
-		if (reg & PHY_DEBUG_MODE)
-			chip->phy_debug_mode = 1;
-		else
-			chip->phy_debug_mode = 0;
+		chip->phy_debug_mode = reg & PHY_DEBUG_MODE ? 1 : 0;
 
 	} else {
 		RTSX_READ_REG(chip, 0xFE80, &val);
@@ -566,16 +559,10 @@ static int rts5208_init(struct rtsx_chip *chip)
 
 	RTSX_READ_REG(chip, PDINFO, &val);
 	dev_dbg(rtsx_dev(chip), "PDINFO: 0x%x\n", val);
-	if (val & AUX_PWR_DETECTED)
-		chip->aux_pwr_exist = 1;
-	else
-		chip->aux_pwr_exist = 0;
+	chip->aux_pwr_exist = val & AUX_PWR_DETECTED ? 1 : 0;
 
 	RTSX_READ_REG(chip, 0xFE50, &val);
-	if (val & 0x01)
-		chip->hw_bypass_sd = 1;
-	else
-		chip->hw_bypass_sd = 0;
+	chip->hw_bypass_sd = val & 0x01 ? 1 : 0;
 
 	rtsx_read_config_byte(chip, 0x0E, &val);
 	if (val & 0x80)
@@ -585,10 +572,7 @@ static int rts5208_init(struct rtsx_chip *chip)
 
 	if (chip->use_hw_setting) {
 		RTSX_READ_REG(chip, CHANGE_LINK_STATE, &val);
-		if (val & 0x80)
-			chip->auto_delink_en = 1;
-		else
-			chip->auto_delink_en = 0;
+		chip->auto_delink_en = val & 0x80 ? 1 : 0;
 	}
 
 	return STATUS_SUCCESS;
@@ -602,33 +586,21 @@ static int rts5288_init(struct rtsx_chip *chip)
 
 	RTSX_WRITE_REG(chip, CLK_SEL, 0x03, 0x03);
 	RTSX_READ_REG(chip, CLK_SEL, &val);
-	if (val == 0)
-		chip->asic_code = 1;
-	else
-		chip->asic_code = 0;
+	chip->asic_code = val == 0 ? 1 : 0;
 
 	chip->ic_version = 0;
 	chip->phy_debug_mode = 0;
 
 	RTSX_READ_REG(chip, PDINFO, &val);
 	dev_dbg(rtsx_dev(chip), "PDINFO: 0x%x\n", val);
-	if (val & AUX_PWR_DETECTED)
-		chip->aux_pwr_exist = 1;
-	else
-		chip->aux_pwr_exist = 0;
+	chip->aux_pwr_exist = val & AUX_PWR_DETECTED ? 1 : 0;
 
 	RTSX_READ_REG(chip, CARD_SHARE_MODE, &val);
 	dev_dbg(rtsx_dev(chip), "CARD_SHARE_MODE: 0x%x\n", val);
-	if (val & 0x04)
-		chip->baro_pkg = QFN;
-	else
-		chip->baro_pkg = LQFP;
+	chip->baro_pkg = val & 0x04 ? QFN : LQFP;
 
 	RTSX_READ_REG(chip, 0xFE5A, &val);
-	if (val & 0x10)
-		chip->hw_bypass_sd = 1;
-	else
-		chip->hw_bypass_sd = 0;
+	chip->hw_bypass_sd = val & 0x10 ? 1 : 0;
 
 	retval = rtsx_read_cfg_dw(chip, 0, 0x718, &lval);
 	if (retval != STATUS_SUCCESS)
@@ -643,16 +615,12 @@ static int rts5288_init(struct rtsx_chip *chip)
 
 	if (chip->use_hw_setting) {
 		RTSX_READ_REG(chip, CHANGE_LINK_STATE, &val);
-		if (val & 0x80)
-			chip->auto_delink_en = 1;
-		else
-			chip->auto_delink_en = 0;
+		chip->auto_delink_en = val & 0x80 ? 1 : 0;
 
 		if (CHECK_BARO_PKG(chip, LQFP))
 			chip->lun_mode = SD_MS_1LUN;
 		else
 			chip->lun_mode = DEFAULT_SINGLE;
-
 	}
 
 	return STATUS_SUCCESS;
@@ -660,9 +628,9 @@ static int rts5288_init(struct rtsx_chip *chip)
 
 int rtsx_init_chip(struct rtsx_chip *chip)
 {
-	struct sd_info *sd_card = &(chip->sd_card);
-	struct xd_info *xd_card = &(chip->xd_card);
-	struct ms_info *ms_card = &(chip->ms_card);
+	struct sd_info *sd_card = &chip->sd_card;
+	struct xd_info *xd_card = &chip->xd_card;
+	struct ms_info *ms_card = &chip->ms_card;
 	int retval;
 	unsigned int i;
 
@@ -740,7 +708,6 @@ int rtsx_init_chip(struct rtsx_chip *chip)
 		retval = rts5288_init(chip);
 		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, STATUS_FAIL);
-
 	}
 
 	if (chip->ss_en == 2)
@@ -842,7 +809,6 @@ static void rtsx_monitor_aspm_config(struct rtsx_chip *chip)
 	} else {
 		if (reg0 & 0x03)
 			maybe_support_aspm = 1;
-
 	}
 
 	if (reg_changed) {
@@ -859,15 +825,15 @@ static void rtsx_monitor_aspm_config(struct rtsx_chip *chip)
 			chip->sdio_aspm = 0;
 		}
 		rtsx_write_register(chip, ASPM_FORCE_CTL, 0xFF,
-				0x30 | chip->aspm_level[0] |
-				(chip->aspm_level[1] << 2));
+				    0x30 | chip->aspm_level[0] |
+				    (chip->aspm_level[1] << 2));
 	}
 }
 
 void rtsx_polling_func(struct rtsx_chip *chip)
 {
 #ifdef SUPPORT_SD_LOCK
-	struct sd_info *sd_card = &(chip->sd_card);
+	struct sd_info *sd_card = &chip->sd_card;
 #endif
 	int ss_allowed;
 
@@ -875,7 +841,7 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 		return;
 
 	if (rtsx_chk_stat(chip, RTSX_STAT_DELINK))
-		goto Delink_Stage;
+		goto delink_stage;
 
 	if (chip->polling_config) {
 		u8 val;
@@ -888,7 +854,7 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 
 #ifdef SUPPORT_OCP
 	if (chip->ocp_int) {
-		rtsx_read_register(chip, OCPSTAT, &(chip->ocp_stat));
+		rtsx_read_register(chip, OCPSTAT, &chip->ocp_stat);
 
 		if (chip->card_exist & SD_CARD)
 			sd_power_off_card3v3(chip);
@@ -932,7 +898,6 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 				rtsx_read_cfg_dw(chip, 1, 0x04, &val);
 				if (val & 0x07)
 					ss_allowed = 0;
-
 			}
 		}
 	} else {
@@ -958,7 +923,7 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 
 #ifdef SUPPORT_SDIO_ASPM
 		if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip) &&
-				chip->aspm_l0s_l1_en && chip->dynamic_aspm) {
+		    chip->aspm_l0s_l1_en && chip->dynamic_aspm) {
 			if (chip->sd_io) {
 				dynamic_configure_sdio_aspm(chip);
 			} else {
@@ -966,7 +931,8 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 					dev_dbg(rtsx_dev(chip), "SDIO enter ASPM!\n");
 					rtsx_write_register(chip,
 						ASPM_FORCE_CTL, 0xFC,
-						0x30 | (chip->aspm_level[1] << 2));
+						0x30 |
+						(chip->aspm_level[1] << 2));
 					chip->sdio_aspm = 1;
 				}
 			}
@@ -988,9 +954,10 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 
 			turn_off_led(chip, LED_GPIO);
 
-			if (chip->auto_power_down && !chip->card_ready && !chip->sd_io)
-				rtsx_force_power_down(chip, SSC_PDCTL | OC_PDCTL);
-
+			if (chip->auto_power_down && !chip->card_ready &&
+			    !chip->sd_io)
+				rtsx_force_power_down(chip,
+						      SSC_PDCTL | OC_PDCTL);
 		}
 	}
 
@@ -1013,7 +980,6 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 		break;
 	}
 
-
 #ifdef SUPPORT_OCP
 	if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
 		if (chip->ocp_stat &
@@ -1024,7 +990,7 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 		if (chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER)) {
 			if (chip->card_exist & SD_CARD) {
 				rtsx_write_register(chip, CARD_OE, SD_OUTPUT_EN,
-						0);
+						    0);
 				card_power_off(chip, SD_CARD);
 				chip->card_fail |= SD_CARD;
 			}
@@ -1032,7 +998,7 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 		if (chip->ocp_stat & (MS_OC_NOW | MS_OC_EVER)) {
 			if (chip->card_exist & MS_CARD) {
 				rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN,
-						0);
+						    0);
 				card_power_off(chip, MS_CARD);
 				chip->card_fail |= MS_CARD;
 			}
@@ -1043,15 +1009,15 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 				chip->ocp_stat);
 			if (chip->card_exist & SD_CARD) {
 				rtsx_write_register(chip, CARD_OE, SD_OUTPUT_EN,
-						0);
+						    0);
 				chip->card_fail |= SD_CARD;
 			} else if (chip->card_exist & MS_CARD) {
 				rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN,
-						0);
+						    0);
 				chip->card_fail |= MS_CARD;
 			} else if (chip->card_exist & XD_CARD) {
 				rtsx_write_register(chip, CARD_OE, XD_OUTPUT_EN,
-						0);
+						    0);
 				chip->card_fail |= XD_CARD;
 			}
 			card_power_off(chip, SD_CARD);
@@ -1059,9 +1025,9 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 	}
 #endif
 
-Delink_Stage:
+delink_stage:
 	if (chip->auto_delink_en && chip->auto_delink_allowed &&
-		!chip->card_ready && !chip->card_ejected && !chip->sd_io) {
+	    !chip->card_ready && !chip->card_ejected && !chip->sd_io) {
 		int enter_L1 = chip->auto_delink_in_L1 && (
 			chip->aspm_l0s_l1_en || chip->ss_en);
 		int delink_stage1_cnt = chip->delink_stage1_step;
@@ -1081,27 +1047,33 @@ Delink_Stage:
 					dev_dbg(rtsx_dev(chip), "False card inserted, do force delink\n");
 
 					if (enter_L1)
-						rtsx_write_register(chip, HOST_SLEEP_STATE, 0x03, 1);
+						rtsx_write_register(chip,
+							      HOST_SLEEP_STATE,
+							      0x03, 1);
 
 					rtsx_write_register(chip,
-							CHANGE_LINK_STATE, 0x0A,
-							0x0A);
+							    CHANGE_LINK_STATE,
+							    0x0A, 0x0A);
 
 					if (enter_L1)
 						rtsx_enter_L1(chip);
 
-					chip->auto_delink_cnt = delink_stage3_cnt + 1;
+					chip->auto_delink_cnt =
+						delink_stage3_cnt + 1;
 				} else {
 					dev_dbg(rtsx_dev(chip), "No card inserted, do delink\n");
 
 					if (enter_L1)
-						rtsx_write_register(chip, HOST_SLEEP_STATE, 0x03, 1);
+						rtsx_write_register(chip,
+							      HOST_SLEEP_STATE,
+							      0x03, 1);
 
-					rtsx_write_register(chip, CHANGE_LINK_STATE, 0x02, 0x02);
+					rtsx_write_register(chip,
+							    CHANGE_LINK_STATE,
+							    0x02, 0x02);
 
 					if (enter_L1)
 						rtsx_enter_L1(chip);
-
 				}
 			}
 
@@ -1115,7 +1087,7 @@ Delink_Stage:
 					rtsx_set_phy_reg_bit(chip, 0x1C, 2);
 
 				rtsx_write_register(chip, CHANGE_LINK_STATE,
-						0x0A, 0x0A);
+						    0x0A, 0x0A);
 			}
 
 			chip->auto_delink_cnt++;
@@ -1219,7 +1191,7 @@ int rtsx_read_register(struct rtsx_chip *chip, u16 addr, u8 *data)
 }
 
 int rtsx_write_cfg_dw(struct rtsx_chip *chip, u8 func_no, u16 addr, u32 mask,
-		u32 val)
+		      u32 val)
 {
 	u8 mode = 0, tmp;
 	int i;
@@ -1279,7 +1251,7 @@ int rtsx_read_cfg_dw(struct rtsx_chip *chip, u8 func_no, u16 addr, u32 *val)
 }
 
 int rtsx_write_cfg_seq(struct rtsx_chip *chip, u8 func, u16 addr, u8 *buf,
-		int len)
+		       int len)
 {
 	u32 *data, *mask;
 	u16 offset = addr % 4;
@@ -1324,7 +1296,7 @@ int rtsx_write_cfg_seq(struct rtsx_chip *chip, u8 func, u16 addr, u8 *buf,
 
 	for (i = 0; i < dw_len; i++) {
 		retval = rtsx_write_cfg_dw(chip, func, aligned_addr + i * 4,
-					mask[i], data[i]);
+					   mask[i], data[i]);
 		if (retval != STATUS_SUCCESS) {
 			vfree(data);
 			vfree(mask);
@@ -1339,7 +1311,7 @@ int rtsx_write_cfg_seq(struct rtsx_chip *chip, u8 func, u16 addr, u8 *buf,
 }
 
 int rtsx_read_cfg_seq(struct rtsx_chip *chip, u8 func, u16 addr, u8 *buf,
-		int len)
+		      int len)
 {
 	u32 *data;
 	u16 offset = addr % 4;
@@ -1360,7 +1332,7 @@ int rtsx_read_cfg_seq(struct rtsx_chip *chip, u8 func, u16 addr, u8 *buf,
 
 	for (i = 0; i < dw_len; i++) {
 		retval = rtsx_read_cfg_dw(chip, func, aligned_addr + i * 4,
-					data + i);
+					  data + i);
 		if (retval != STATUS_SUCCESS) {
 			vfree(data);
 			TRACE_RET(chip, STATUS_FAIL);
@@ -1522,7 +1494,7 @@ int rtsx_set_phy_reg_bit(struct rtsx_chip *chip, u8 reg, u8 bit)
 	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
 
-	if (0 == (value & (1 << bit))) {
+	if ((value & (1 << bit)) == 0) {
 		value |= (1 << bit);
 		retval = rtsx_write_phy_register(chip, reg, value);
 		if (retval != STATUS_SUCCESS)
@@ -1595,12 +1567,9 @@ void rtsx_enter_ss(struct rtsx_chip *chip)
 		rtsx_force_power_down(chip, SSC_PDCTL | OC_PDCTL);
 	}
 
-	if (CHK_SDIO_EXIST(chip)) {
-		if (CHECK_PID(chip, 0x5288))
-			rtsx_write_cfg_dw(chip, 2, 0xC0, 0xFF00, 0x0100);
-		else
-			rtsx_write_cfg_dw(chip, 1, 0xC0, 0xFF00, 0x0100);
-	}
+	if (CHK_SDIO_EXIST(chip))
+		rtsx_write_cfg_dw(chip, CHECK_PID(chip, 0x5288) ? 2 : 1,
+				  0xC0, 0xFF00, 0x0100);
 
 	if (chip->auto_delink_en) {
 		rtsx_write_register(chip, HOST_SLEEP_STATE, 0x01, 0x01);
@@ -1666,7 +1635,7 @@ int rtsx_pre_handle_interrupt(struct rtsx_chip *chip)
 	chip->int_reg = rtsx_readl(chip, RTSX_BIPR);
 
 	if (((chip->int_reg & int_enable) == 0) ||
-		(chip->int_reg == 0xFFFFFFFF))
+	    (chip->int_reg == 0xFFFFFFFF))
 		return STATUS_FAIL;
 
 	status = chip->int_reg &= (int_enable | 0x7FFFFF);
@@ -1676,12 +1645,12 @@ int rtsx_pre_handle_interrupt(struct rtsx_chip *chip)
 
 		if (status & SD_INT) {
 			if (status & SD_EXIST) {
-				set_bit(SD_NR, &(chip->need_reset));
+				set_bit(SD_NR, &chip->need_reset);
 			} else {
-				set_bit(SD_NR, &(chip->need_release));
+				set_bit(SD_NR, &chip->need_release);
 				chip->sd_reset_counter = 0;
 				chip->sd_show_cnt = 0;
-				clear_bit(SD_NR, &(chip->need_reset));
+				clear_bit(SD_NR, &chip->need_reset);
 			}
 		} else {
 			/* If multi-luns, it's possible that
@@ -1691,35 +1660,35 @@ int rtsx_pre_handle_interrupt(struct rtsx_chip *chip)
 			   all existed cards should be reset.
 			*/
 			if (exit_ss && (status & SD_EXIST))
-				set_bit(SD_NR, &(chip->need_reinit));
+				set_bit(SD_NR, &chip->need_reinit);
 		}
 		if (!CHECK_PID(chip, 0x5288) || CHECK_BARO_PKG(chip, QFN)) {
 			if (status & XD_INT) {
 				if (status & XD_EXIST) {
-					set_bit(XD_NR, &(chip->need_reset));
+					set_bit(XD_NR, &chip->need_reset);
 				} else {
-					set_bit(XD_NR, &(chip->need_release));
+					set_bit(XD_NR, &chip->need_release);
 					chip->xd_reset_counter = 0;
 					chip->xd_show_cnt = 0;
-					clear_bit(XD_NR, &(chip->need_reset));
+					clear_bit(XD_NR, &chip->need_reset);
 				}
 			} else {
 				if (exit_ss && (status & XD_EXIST))
-					set_bit(XD_NR, &(chip->need_reinit));
+					set_bit(XD_NR, &chip->need_reinit);
 			}
 		}
 		if (status & MS_INT) {
 			if (status & MS_EXIST) {
-				set_bit(MS_NR, &(chip->need_reset));
+				set_bit(MS_NR, &chip->need_reset);
 			} else {
-				set_bit(MS_NR, &(chip->need_release));
+				set_bit(MS_NR, &chip->need_release);
 				chip->ms_reset_counter = 0;
 				chip->ms_show_cnt = 0;
-				clear_bit(MS_NR, &(chip->need_reset));
+				clear_bit(MS_NR, &chip->need_reset);
 			}
 		} else {
 			if (exit_ss && (status & MS_EXIST))
-				set_bit(MS_NR, &(chip->need_reinit));
+				set_bit(MS_NR, &chip->need_reinit);
 		}
 	}
 
@@ -1727,10 +1696,8 @@ int rtsx_pre_handle_interrupt(struct rtsx_chip *chip)
 	chip->ocp_int = ocp_int & status;
 #endif
 
-	if (chip->sd_io) {
-		if (chip->int_reg & DATA_DONE_INT)
-			chip->int_reg &= ~(u32)DATA_DONE_INT;
-	}
+	if (chip->sd_io && (chip->int_reg & DATA_DONE_INT))
+		chip->int_reg &= ~(u32)DATA_DONE_INT;
 
 	return STATUS_SUCCESS;
 }
@@ -1774,14 +1741,14 @@ void rtsx_do_before_power_down(struct rtsx_chip *chip, int pm_stat)
 	if (pm_stat == PM_S1) {
 		dev_dbg(rtsx_dev(chip), "Host enter S1\n");
 		rtsx_write_register(chip, HOST_SLEEP_STATE, 0x03,
-				HOST_ENTER_S1);
+				    HOST_ENTER_S1);
 	} else if (pm_stat == PM_S3) {
 		if (chip->s3_pwr_off_delay > 0)
 			wait_timeout(chip->s3_pwr_off_delay);
 
 		dev_dbg(rtsx_dev(chip), "Host enter S3\n");
 		rtsx_write_register(chip, HOST_SLEEP_STATE, 0x03,
-				HOST_ENTER_S3);
+				    HOST_ENTER_S3);
 	}
 
 	if (chip->do_delink_before_power_down && chip->auto_delink_en)
@@ -1796,31 +1763,25 @@ void rtsx_do_before_power_down(struct rtsx_chip *chip, int pm_stat)
 
 void rtsx_enable_aspm(struct rtsx_chip *chip)
 {
-	if (chip->aspm_l0s_l1_en && chip->dynamic_aspm) {
-		if (!chip->aspm_enabled) {
-			dev_dbg(rtsx_dev(chip), "Try to enable ASPM\n");
-			chip->aspm_enabled = 1;
+	if (chip->aspm_l0s_l1_en && chip->dynamic_aspm && !chip->aspm_enabled) {
+		dev_dbg(rtsx_dev(chip), "Try to enable ASPM\n");
+		chip->aspm_enabled = 1;
 
-			if (chip->asic_code && CHECK_PID(chip, 0x5208))
-				rtsx_write_phy_register(chip, 0x07, 0);
-			if (CHECK_PID(chip, 0x5208)) {
-				rtsx_write_register(chip, ASPM_FORCE_CTL, 0xF3,
-					0x30 | chip->aspm_level[0]);
-			} else {
-				rtsx_write_config_byte(chip, LCTLR,
-						chip->aspm_l0s_l1_en);
-			}
+		if (chip->asic_code && CHECK_PID(chip, 0x5208))
+			rtsx_write_phy_register(chip, 0x07, 0);
+		if (CHECK_PID(chip, 0x5208)) {
+			rtsx_write_register(chip, ASPM_FORCE_CTL, 0xF3,
+					    0x30 | chip->aspm_level[0]);
+		} else {
+			rtsx_write_config_byte(chip, LCTLR,
+					       chip->aspm_l0s_l1_en);
+		}
 
-			if (CHK_SDIO_EXIST(chip)) {
-				u16 val = chip->aspm_l0s_l1_en | 0x0100;
+		if (CHK_SDIO_EXIST(chip)) {
+			u16 val = chip->aspm_l0s_l1_en | 0x0100;
 
-				if (CHECK_PID(chip, 0x5288))
-					rtsx_write_cfg_dw(chip, 2, 0xC0,
-							0xFFFF, val);
-				else
-					rtsx_write_cfg_dw(chip, 1, 0xC0,
-							0xFFFF, val);
-			}
+			rtsx_write_cfg_dw(chip, CHECK_PID(chip, 0x5288) ? 2 : 1,
+					  0xC0, 0xFFF, val);
 		}
 	}
 }
@@ -1830,21 +1791,19 @@ void rtsx_disable_aspm(struct rtsx_chip *chip)
 	if (CHECK_PID(chip, 0x5208))
 		rtsx_monitor_aspm_config(chip);
 
-	if (chip->aspm_l0s_l1_en && chip->dynamic_aspm) {
-		if (chip->aspm_enabled) {
-			dev_dbg(rtsx_dev(chip), "Try to disable ASPM\n");
-			chip->aspm_enabled = 0;
+	if (chip->aspm_l0s_l1_en && chip->dynamic_aspm && chip->aspm_enabled) {
+		dev_dbg(rtsx_dev(chip), "Try to disable ASPM\n");
+		chip->aspm_enabled = 0;
 
-			if (chip->asic_code && CHECK_PID(chip, 0x5208))
-				rtsx_write_phy_register(chip, 0x07, 0x0129);
-			if (CHECK_PID(chip, 0x5208))
-				rtsx_write_register(chip, ASPM_FORCE_CTL,
-						0xF3, 0x30);
-			else
-				rtsx_write_config_byte(chip, LCTLR, 0x00);
+		if (chip->asic_code && CHECK_PID(chip, 0x5208))
+			rtsx_write_phy_register(chip, 0x07, 0x0129);
+		if (CHECK_PID(chip, 0x5208))
+			rtsx_write_register(chip, ASPM_FORCE_CTL,
+					    0xF3, 0x30);
+		else
+			rtsx_write_config_byte(chip, LCTLR, 0x00);
 
-			wait_timeout(1);
-		}
+		wait_timeout(1);
 	}
 }
 
@@ -1907,7 +1866,7 @@ int rtsx_write_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len)
 
 		for (j = 0; j < 256; j++) {
 			rtsx_add_cmd(chip, WRITE_REG_CMD, reg_addr++, 0xFF,
-				*ptr);
+				     *ptr);
 			ptr++;
 		}
 
@@ -1921,7 +1880,7 @@ int rtsx_write_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len)
 
 		for (j = 0; j < buf_len%256; j++) {
 			rtsx_add_cmd(chip, WRITE_REG_CMD, reg_addr++, 0xFF,
-				*ptr);
+				     *ptr);
 			ptr++;
 		}
 
diff --git a/drivers/staging/rts5208/rtsx_scsi.c b/drivers/staging/rts5208/rtsx_scsi.c
index bbbf7968a0b6..11610826acf1 100644
--- a/drivers/staging/rts5208/rtsx_scsi.c
+++ b/drivers/staging/rts5208/rtsx_scsi.c
@@ -584,9 +584,8 @@ static int start_stop_unit(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	case MAKE_MEDIUM_READY:
 	case LOAD_MEDIUM:
-		if (check_card_ready(chip, lun)) {
+		if (check_card_ready(chip, lun))
 			return TRANSPORT_GOOD;
-		}
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
 		TRACE_RET(chip, TRANSPORT_FAILED);
 
diff --git a/drivers/staging/rts5208/rtsx_transport.c b/drivers/staging/rts5208/rtsx_transport.c
index 0a67dca72dff..756a9687c293 100644
--- a/drivers/staging/rts5208/rtsx_transport.c
+++ b/drivers/staging/rts5208/rtsx_transport.c
@@ -539,7 +539,7 @@ static int rtsx_transfer_sglist_adma(struct rtsx_chip *chip, u8 card,
 		if (i == buf_cnt / (HOST_SG_TBL_BUF_LEN / 8))
 			sg_cnt = buf_cnt % (HOST_SG_TBL_BUF_LEN / 8);
 		else
-			sg_cnt = (HOST_SG_TBL_BUF_LEN / 8);
+			sg_cnt = HOST_SG_TBL_BUF_LEN / 8;
 
 		chip->sgi = 0;
 		for (j = 0; j < sg_cnt; j++) {
@@ -728,15 +728,13 @@ int rtsx_transfer_data_partial(struct rtsx_chip *chip, u8 card,
 	if (rtsx_chk_stat(chip, RTSX_STAT_ABORT))
 		return -EIO;
 
-	if (use_sg) {
+	if (use_sg)
 		err = rtsx_transfer_sglist_adma_partial(chip, card,
 				(struct scatterlist *)buf, use_sg,
 				index, offset, (int)len, dma_dir, timeout);
-	} else {
+	else
 		err = rtsx_transfer_buf(chip, card,
 					buf, len, dma_dir, timeout);
-	}
-
 	if (err < 0) {
 		if (RTSX_TST_DELINK(chip)) {
 			RTSX_CLR_DELINK(chip);
diff --git a/drivers/staging/rts5208/rtsx_transport.h b/drivers/staging/rts5208/rtsx_transport.h
index b4b112372776..899bc2079dbe 100644
--- a/drivers/staging/rts5208/rtsx_transport.h
+++ b/drivers/staging/rts5208/rtsx_transport.h
@@ -46,7 +46,7 @@ void rtsx_add_cmd(struct rtsx_chip *chip,
 void rtsx_send_cmd_no_wait(struct rtsx_chip *chip);
 int rtsx_send_cmd(struct rtsx_chip *chip, u8 card, int timeout);
 
-extern inline u8 *rtsx_get_cmd_data(struct rtsx_chip *chip)
+static inline u8 *rtsx_get_cmd_data(struct rtsx_chip *chip)
 {
 #ifdef CMD_USING_SG
 	return (u8 *)(chip->host_sg_tbl_ptr);
diff --git a/drivers/staging/skein/Kconfig b/drivers/staging/skein/Kconfig
index b9172bfcdc1b..012a8233376e 100644
--- a/drivers/staging/skein/Kconfig
+++ b/drivers/staging/skein/Kconfig
@@ -1,8 +1,8 @@
 config CRYPTO_SKEIN
-	bool "Skein digest algorithm"
+	tristate "Skein digest algorithm"
 	depends on (X86 || UML_X86) && 64BIT && CRYPTO
-	select CRYPTO_THREEFISH
 	select CRYPTO_HASH
+	select CRYPTO_ALGAPI
 	help
 	  Skein secure hash algorithm is one of 5 finalists from the NIST SHA3
 	  competition.
@@ -12,21 +12,5 @@ config CRYPTO_SKEIN
 
 	  http://www.skein-hash.info/sites/default/files/skein1.3.pdf
 
-	  for more information.  This module depends on the threefish block
-	  cipher module.
-
-config CRYPTO_THREEFISH
-	bool "Threefish tweakable block cipher"
-	depends on (X86 || UML_X86) && 64BIT && CRYPTO
-	select CRYPTO_ALGAPI
-	help
-	  Threefish cipher algorithm is the tweakable block cipher underneath
-	  the Skein family of secure hash algorithms.  Skein is one of 5
-	  finalists from the NIST SHA3 competition.
-
-	  Skein is optimized for modern, 64bit processors and is highly
-	  customizable.  See:
-
-	  http://www.skein-hash.info/sites/default/files/skein1.3.pdf
-
-	  for more information.
+	  for more information. This module also contains the threefish block
+	  cipher algorithm.
diff --git a/drivers/staging/skein/Makefile b/drivers/staging/skein/Makefile
index a14aaddd829c..b7f947fb98f0 100644
--- a/drivers/staging/skein/Makefile
+++ b/drivers/staging/skein/Makefile
@@ -1,9 +1,10 @@
 #
 # Makefile for the skein secure hash algorithm
 #
-obj-$(CONFIG_CRYPTO_SKEIN) +=   skein.o \
-				skein_api.o \
-				skein_block.o
-
-obj-$(CONFIG_CRYPTO_THREEFISH) += threefish_block.o \
-				  threefish_api.o
+obj-$(CONFIG_CRYPTO_SKEIN) += skein.o
+skein-y := skein_base.o \
+	   skein_api.o \
+	   skein_block.o \
+	   threefish_block.o \
+	   threefish_api.o \
+	   skein_generic.o
diff --git a/drivers/staging/skein/skein_api.c b/drivers/staging/skein/skein_api.c
index 6e700eefc00c..5bfce076f7c8 100644
--- a/drivers/staging/skein/skein_api.c
+++ b/drivers/staging/skein/skein_api.c
@@ -31,7 +31,7 @@ int skein_ctx_prepare(struct skein_ctx *ctx, enum skein_size size)
 {
 	skein_assert_ret(ctx && size, SKEIN_FAIL);
 
-	memset(ctx , 0, sizeof(struct skein_ctx));
+	memset(ctx, 0, sizeof(struct skein_ctx));
 	ctx->skein_size = size;
 
 	return SKEIN_SUCCESS;
diff --git a/drivers/staging/skein/skein_api.h b/drivers/staging/skein/skein_api.h
index e02fa19d9458..171b87549548 100644
--- a/drivers/staging/skein/skein_api.h
+++ b/drivers/staging/skein/skein_api.h
@@ -79,7 +79,7 @@ OTHER DEALINGS IN THE SOFTWARE.
  */
 
 #include <linux/types.h>
-#include "skein.h"
+#include "skein_base.h"
 
 /**
  * Which Skein size to use
diff --git a/drivers/staging/skein/skein.c b/drivers/staging/skein/skein_base.c
index 8cc83587b1f1..7e700a6b5788 100644
--- a/drivers/staging/skein/skein.c
+++ b/drivers/staging/skein/skein_base.c
@@ -8,10 +8,9 @@
 **
 ************************************************************************/
 
-#define  SKEIN_PORT_CODE /* instantiate any code in skein_port.h */
-
 #include <linux/string.h>       /* get the memcpy/memset functions */
-#include "skein.h" /* get the Skein API definitions   */
+#include <linux/export.h>
+#include "skein_base.h" /* get the Skein API definitions   */
 #include "skein_iv.h"    /* get precomputed IVs */
 #include "skein_block.h"
 
@@ -125,8 +124,6 @@ int skein_256_init_ext(struct skein_256_ctx *ctx, size_t hash_bit_len,
 	/* tree hash config info (or SKEIN_CFG_TREE_INFO_SEQUENTIAL) */
 	cfg.w[2] = skein_swap64(tree_info);
 
-	skein_show_key(256, &ctx->h, key, key_bytes);
-
 	/* compute the initial chaining values from config block */
 	skein_256_process_block(ctx, cfg.b, 1, SKEIN_CFG_STR_LEN);
 
@@ -233,8 +230,6 @@ int skein_256_final(struct skein_256_ctx *ctx, u8 *hash_val)
 		/* "output" the ctr mode bytes */
 		skein_put64_lsb_first(hash_val+i*SKEIN_256_BLOCK_BYTES, ctx->x,
 				      n);
-		skein_show_final(256, &ctx->h, n,
-				 hash_val+i*SKEIN_256_BLOCK_BYTES);
 		/* restore the counter mode key for next time */
 		memcpy(ctx->x, x, sizeof(x));
 	}
@@ -354,8 +349,6 @@ int skein_512_init_ext(struct skein_512_ctx *ctx, size_t hash_bit_len,
 	/* tree hash config info (or SKEIN_CFG_TREE_INFO_SEQUENTIAL) */
 	cfg.w[2] = skein_swap64(tree_info);
 
-	skein_show_key(512, &ctx->h, key, key_bytes);
-
 	/* compute the initial chaining values from config block */
 	skein_512_process_block(ctx, cfg.b, 1, SKEIN_CFG_STR_LEN);
 
@@ -462,8 +455,6 @@ int skein_512_final(struct skein_512_ctx *ctx, u8 *hash_val)
 		/* "output" the ctr mode bytes */
 		skein_put64_lsb_first(hash_val+i*SKEIN_512_BLOCK_BYTES, ctx->x,
 				      n);
-		skein_show_final(512, &ctx->h, n,
-				 hash_val+i*SKEIN_512_BLOCK_BYTES);
 		/* restore the counter mode key for next time */
 		memcpy(ctx->x, x, sizeof(x));
 	}
@@ -578,8 +569,6 @@ int skein_1024_init_ext(struct skein_1024_ctx *ctx, size_t hash_bit_len,
 	/* tree hash config info (or SKEIN_CFG_TREE_INFO_SEQUENTIAL) */
 	cfg.w[2] = skein_swap64(tree_info);
 
-	skein_show_key(1024, &ctx->h, key, key_bytes);
-
 	/* compute the initial chaining values from config block */
 	skein_1024_process_block(ctx, cfg.b, 1, SKEIN_CFG_STR_LEN);
 
@@ -686,8 +675,6 @@ int skein_1024_final(struct skein_1024_ctx *ctx, u8 *hash_val)
 		/* "output" the ctr mode bytes */
 		skein_put64_lsb_first(hash_val+i*SKEIN_1024_BLOCK_BYTES, ctx->x,
 				      n);
-		skein_show_final(1024, &ctx->h, n,
-				 hash_val+i*SKEIN_1024_BLOCK_BYTES);
 		/* restore the counter mode key for next time */
 		memcpy(ctx->x, x, sizeof(x));
 	}
@@ -795,8 +782,6 @@ int skein_256_output(struct skein_256_ctx *ctx, u8 *hash_val)
 		/* "output" the ctr mode bytes */
 		skein_put64_lsb_first(hash_val+i*SKEIN_256_BLOCK_BYTES, ctx->x,
 				      n);
-		skein_show_final(256, &ctx->h, n,
-				 hash_val+i*SKEIN_256_BLOCK_BYTES);
 		/* restore the counter mode key for next time */
 		memcpy(ctx->x, x, sizeof(x));
 	}
@@ -834,8 +819,6 @@ int skein_512_output(struct skein_512_ctx *ctx, u8 *hash_val)
 		/* "output" the ctr mode bytes */
 		skein_put64_lsb_first(hash_val+i*SKEIN_512_BLOCK_BYTES, ctx->x,
 				      n);
-		skein_show_final(256, &ctx->h, n,
-				 hash_val+i*SKEIN_512_BLOCK_BYTES);
 		/* restore the counter mode key for next time */
 		memcpy(ctx->x, x, sizeof(x));
 	}
@@ -873,8 +856,6 @@ int skein_1024_output(struct skein_1024_ctx *ctx, u8 *hash_val)
 		/* "output" the ctr mode bytes */
 		skein_put64_lsb_first(hash_val+i*SKEIN_1024_BLOCK_BYTES, ctx->x,
 				      n);
-		skein_show_final(256, &ctx->h, n,
-				 hash_val+i*SKEIN_1024_BLOCK_BYTES);
 		/* restore the counter mode key for next time */
 		memcpy(ctx->x, x, sizeof(x));
 	}
diff --git a/drivers/staging/skein/skein.h b/drivers/staging/skein/skein_base.h
index e6669f196e5d..3c7f8ad3627d 100644
--- a/drivers/staging/skein/skein.h
+++ b/drivers/staging/skein/skein_base.h
@@ -15,10 +15,6 @@
 **
 ** The "default" note explains what happens when the switch is not defined.
 **
-**  SKEIN_DEBUG            -- make callouts from inside Skein code
-**                            to examine/display intermediate values.
-**                            [default: no callouts (no overhead)]
-**
 **  SKEIN_ERR_CHECK        -- how error checking is handled inside Skein
 **                            code. If not defined, most error checking
 **                            is disabled (for performance). Otherwise,
@@ -28,9 +24,10 @@
 **
 ***************************************************************************/
 
-#ifndef rotl_64
-#define rotl_64(x, N)    (((x) << (N)) | ((x) >> (64-(N))))
-#endif
+/*Skein digest sizes for crypto api*/
+#define SKEIN256_DIGEST_BIT_SIZE 256
+#define SKEIN512_DIGEST_BIT_SIZE 512
+#define SKEIN1024_DIGEST_BIT_SIZE 1024
 
 /* below two prototype assume we are handed aligned data */
 #define skein_put64_lsb_first(dst08, src64, b_cnt) memcpy(dst08, src64, b_cnt)
@@ -44,12 +41,12 @@ enum {
 	SKEIN_BAD_HASHLEN     =      2
 };
 
-#define  SKEIN_MODIFIER_WORDS   (2) /* number of modifier (tweak) words */
+#define  SKEIN_MODIFIER_WORDS   2 /* number of modifier (tweak) words */
 
-#define  SKEIN_256_STATE_WORDS  (4)
-#define  SKEIN_512_STATE_WORDS  (8)
-#define  SKEIN_1024_STATE_WORDS (16)
-#define  SKEIN_MAX_STATE_WORDS (16)
+#define  SKEIN_256_STATE_WORDS  4
+#define  SKEIN_512_STATE_WORDS  8
+#define  SKEIN_1024_STATE_WORDS 16
+#define  SKEIN_MAX_STATE_WORDS	16
 
 #define  SKEIN_256_STATE_BYTES  (8*SKEIN_256_STATE_WORDS)
 #define  SKEIN_512_STATE_BYTES  (8*SKEIN_512_STATE_WORDS)
@@ -87,6 +84,11 @@ struct skein_1024_ctx { /* 1024-bit Skein hash context structure */
 	u8 b[SKEIN_1024_BLOCK_BYTES];	/* partial block buf (8-byte aligned) */
 };
 
+static inline u64 rotl_64(u64 x, u8 N)
+{
+	return (x << N) | (x >> (64 - N));
+}
+
 /* Skein APIs for (incremental) "straight hashing" */
 int skein_256_init(struct skein_256_ctx *ctx, size_t hash_bit_len);
 int skein_512_init(struct skein_512_ctx *ctx, size_t hash_bit_len);
@@ -273,19 +275,6 @@ int skein_1024_output(struct skein_1024_ctx *ctx, u8 *hash_val);
 		(hdr).tweak[1] |= SKEIN_T1_TREE_LEVEL(height); \
 	}
 
-/*****************************************************************
-** "Internal" Skein definitions for debugging and error checking
-******************************************************************/
-#ifdef SKEIN_DEBUG             /* examine/display intermediate values? */
-#include "skein_debug.h"
-#else                           /* default is no callouts */
-#define skein_show_block(bits, ctx, x, blk_ptr, w_ptr, ks_event_ptr, ks_odd_ptr)
-#define skein_show_round(bits, ctx, r, x)
-#define skein_show_r_ptr(bits, ctx, r, x_ptr)
-#define skein_show_final(bits, ctx, cnt, out_ptr)
-#define skein_show_key(bits, ctx, key, key_bytes)
-#endif
-
 /* ignore all asserts, for performance */
 #define skein_assert_ret(x, ret_code)
 #define skein_assert(x)
diff --git a/drivers/staging/skein/skein_block.c b/drivers/staging/skein/skein_block.c
index 616364faf92e..66261ab25c88 100644
--- a/drivers/staging/skein/skein_block.c
+++ b/drivers/staging/skein/skein_block.c
@@ -15,7 +15,7 @@
 ************************************************************************/
 
 #include <linux/string.h>
-#include "skein.h"
+#include "skein_base.h"
 #include "skein_block.h"
 
 #ifndef SKEIN_USE_ASM
@@ -26,32 +26,27 @@
 #define SKEIN_LOOP 001 /* default: unroll 256 and 512, but not 1024 */
 #endif
 
-#define BLK_BITS        (WCNT*64) /* some useful definitions for code here */
+#define BLK_BITS        (WCNT * 64) /* some useful definitions for code here */
 #define KW_TWK_BASE     (0)
 #define KW_KEY_BASE     (3)
 #define ks              (kw + KW_KEY_BASE)
 #define ts              (kw + KW_TWK_BASE)
 
 #ifdef SKEIN_DEBUG
-#define debug_save_tweak(ctx) { \
-			ctx->h.tweak[0] = ts[0]; ctx->h.tweak[1] = ts[1]; }
+#define debug_save_tweak(ctx)       \
+{                                   \
+	ctx->h.tweak[0] = ts[0];    \
+	ctx->h.tweak[1] = ts[1];    \
+}
 #else
 #define debug_save_tweak(ctx)
 #endif
 
-/*****************************  SKEIN_256 ******************************/
 #if !(SKEIN_USE_ASM & 256)
-void skein_256_process_block(struct skein_256_ctx *ctx, const u8 *blk_ptr,
-			     size_t blk_cnt, size_t byte_cnt_add)
-	{ /* do it in C */
-	enum {
-		WCNT = SKEIN_256_STATE_WORDS
-	};
 #undef  RCNT
-#define RCNT  (SKEIN_256_ROUNDS_TOTAL/8)
-
+#define RCNT (SKEIN_256_ROUNDS_TOTAL / 8)
 #ifdef SKEIN_LOOP /* configure how much to unroll the loop */
-#define SKEIN_UNROLL_256 (((SKEIN_LOOP)/100)%10)
+#define SKEIN_UNROLL_256 (((SKEIN_LOOP) / 100) % 10)
 #else
 #define SKEIN_UNROLL_256 (0)
 #endif
@@ -60,17 +55,329 @@ void skein_256_process_block(struct skein_256_ctx *ctx, const u8 *blk_ptr,
 #if (RCNT % SKEIN_UNROLL_256)
 #error "Invalid SKEIN_UNROLL_256" /* sanity check on unroll count */
 #endif
-	size_t  r;
-	u64  kw[WCNT+4+RCNT*2]; /* key schedule: chaining vars + tweak + "rot"*/
+#endif
+#define ROUND256(p0, p1, p2, p3, ROT, r_num) \
+do {                                         \
+	X##p0 += X##p1;                      \
+	X##p1 = rotl_64(X##p1, ROT##_0);     \
+	X##p1 ^= X##p0;                      \
+	X##p2 += X##p3;                      \
+	X##p3 = rotl_64(X##p3, ROT##_1);     \
+	X##p3 ^= X##p2;                      \
+} while (0)
+
+#if SKEIN_UNROLL_256 == 0
+#define R256(p0, p1, p2, p3, ROT, r_num) /* fully unrolled */ \
+do {                                                          \
+	ROUND256(p0, p1, p2, p3, ROT, r_num);                 \
+} while (0)
+
+#define I256(R)                                                           \
+do {                                                                      \
+	/* inject the key schedule value */                               \
+	X0   += ks[((R) + 1) % 5];                                        \
+	X1   += ks[((R) + 2) % 5] + ts[((R) + 1) % 3];                    \
+	X2   += ks[((R) + 3) % 5] + ts[((R) + 2) % 3];                    \
+	X3   += ks[((R) + 4) % 5] + (R) + 1;                              \
+} while (0)
 #else
-	u64  kw[WCNT+4]; /* key schedule words : chaining vars + tweak */
+/* looping version */
+#define R256(p0, p1, p2, p3, ROT, r_num) \
+do { \
+	ROUND256(p0, p1, p2, p3, ROT, r_num); \
+} while (0)
+
+#define I256(R) \
+do { \
+	/* inject the key schedule value */ \
+	X0 += ks[r + (R) + 0]; \
+	X1 += ks[r + (R) + 1] + ts[r + (R) + 0];                          \
+	X2 += ks[r + (R) + 2] + ts[r + (R) + 1];                          \
+	X3 += ks[r + (R) + 3] + r + (R);                                  \
+	/* rotate key schedule */                                         \
+	ks[r + (R) + 4] = ks[r + (R) - 1];                                \
+	ts[r + (R) + 2] = ts[r + (R) - 1];                                \
+} while (0)
+#endif
+#define R256_8_ROUNDS(R)                                 \
+do {                                                     \
+		R256(0, 1, 2, 3, R_256_0, 8 * (R) + 1);  \
+		R256(0, 3, 2, 1, R_256_1, 8 * (R) + 2);  \
+		R256(0, 1, 2, 3, R_256_2, 8 * (R) + 3);  \
+		R256(0, 3, 2, 1, R_256_3, 8 * (R) + 4);  \
+		I256(2 * (R));                           \
+		R256(0, 1, 2, 3, R_256_4, 8 * (R) + 5);  \
+		R256(0, 3, 2, 1, R_256_5, 8 * (R) + 6);  \
+		R256(0, 1, 2, 3, R_256_6, 8 * (R) + 7);  \
+		R256(0, 3, 2, 1, R_256_7, 8 * (R) + 8);  \
+		I256(2 * (R) + 1);                       \
+} while (0)
+
+#define R256_UNROLL_R(NN)                     \
+	((SKEIN_UNROLL_256 == 0 &&            \
+	SKEIN_256_ROUNDS_TOTAL / 8 > (NN)) || \
+	(SKEIN_UNROLL_256 > (NN)))
+
+#if  (SKEIN_UNROLL_256 > 14)
+#error  "need more unrolling in skein_256_process_block"
+#endif
+#endif
+
+#if !(SKEIN_USE_ASM & 512)
+#undef  RCNT
+#define RCNT  (SKEIN_512_ROUNDS_TOTAL/8)
+
+#ifdef SKEIN_LOOP /* configure how much to unroll the loop */
+#define SKEIN_UNROLL_512 (((SKEIN_LOOP)/10)%10)
+#else
+#define SKEIN_UNROLL_512 (0)
+#endif
+
+#if SKEIN_UNROLL_512
+#if (RCNT % SKEIN_UNROLL_512)
+#error "Invalid SKEIN_UNROLL_512" /* sanity check on unroll count */
+#endif
+#endif
+#define ROUND512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num) \
+do {                                                         \
+	X##p0 += X##p1;                                      \
+	X##p1 = rotl_64(X##p1, ROT##_0);                     \
+	X##p1 ^= X##p0;                                      \
+	X##p2 += X##p3;                                      \
+	X##p3 = rotl_64(X##p3, ROT##_1);                     \
+	X##p3 ^= X##p2;                                      \
+	X##p4 += X##p5;					     \
+	X##p5 = rotl_64(X##p5, ROT##_2);                     \
+	X##p5 ^= X##p4;                                      \
+	X##p6 += X##p7; X##p7 = rotl_64(X##p7, ROT##_3);     \
+	X##p7 ^= X##p6;                                      \
+} while (0)
+
+#if SKEIN_UNROLL_512 == 0
+#define R512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num) /* unrolled */ \
+do {                                                                    \
+	ROUND512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num);           \
+} while (0)
+
+#define I512(R)                                                           \
+do {                                                                      \
+	/* inject the key schedule value */                               \
+	X0   += ks[((R) + 1) % 9];                                        \
+	X1   += ks[((R) + 2) % 9];                                        \
+	X2   += ks[((R) + 3) % 9];                                        \
+	X3   += ks[((R) + 4) % 9];                                        \
+	X4   += ks[((R) + 5) % 9];                                        \
+	X5   += ks[((R) + 6) % 9] + ts[((R) + 1) % 3];                    \
+	X6   += ks[((R) + 7) % 9] + ts[((R) + 2) % 3];                    \
+	X7   += ks[((R) + 8) % 9] + (R) + 1;                              \
+} while (0)
+
+#else /* looping version */
+#define R512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num)                 \
+do {                                                                     \
+	ROUND512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num);            \
+} while (0)
+
+#define I512(R)                                                           \
+do {                                                                      \
+	/* inject the key schedule value */                               \
+	X0   += ks[r + (R) + 0];                                          \
+	X1   += ks[r + (R) + 1];                                          \
+	X2   += ks[r + (R) + 2];                                          \
+	X3   += ks[r + (R) + 3];                                          \
+	X4   += ks[r + (R) + 4];                                          \
+	X5   += ks[r + (R) + 5] + ts[r + (R) + 0];                        \
+	X6   += ks[r + (R) + 6] + ts[r + (R) + 1];                        \
+	X7   += ks[r + (R) + 7] + r + (R);                                \
+	/* rotate key schedule */                                         \
+	ks[r + (R) + 8] = ks[r + (R) - 1];                                \
+	ts[r + (R) + 2] = ts[r + (R) - 1];                                \
+} while (0)
+#endif /* end of looped code definitions */
+#define R512_8_ROUNDS(R)  /* do 8 full rounds */                      \
+do {                                                                  \
+		R512(0, 1, 2, 3, 4, 5, 6, 7, R_512_0, 8 * (R) + 1);   \
+		R512(2, 1, 4, 7, 6, 5, 0, 3, R_512_1, 8 * (R) + 2);   \
+		R512(4, 1, 6, 3, 0, 5, 2, 7, R_512_2, 8 * (R) + 3);   \
+		R512(6, 1, 0, 7, 2, 5, 4, 3, R_512_3, 8 * (R) + 4);   \
+		I512(2 * (R));                              \
+		R512(0, 1, 2, 3, 4, 5, 6, 7, R_512_4, 8 * (R) + 5);   \
+		R512(2, 1, 4, 7, 6, 5, 0, 3, R_512_5, 8 * (R) + 6);   \
+		R512(4, 1, 6, 3, 0, 5, 2, 7, R_512_6, 8 * (R) + 7);   \
+		R512(6, 1, 0, 7, 2, 5, 4, 3, R_512_7, 8 * (R) + 8);   \
+		I512(2 * (R) + 1);        /* and key injection */     \
+} while (0)
+#define R512_UNROLL_R(NN)                             \
+		((SKEIN_UNROLL_512 == 0 &&            \
+		SKEIN_512_ROUNDS_TOTAL/8 > (NN)) ||   \
+		(SKEIN_UNROLL_512 > (NN)))
+
+#if  (SKEIN_UNROLL_512 > 14)
+#error  "need more unrolling in skein_512_process_block"
+#endif
+#endif
+
+#if !(SKEIN_USE_ASM & 1024)
+#undef  RCNT
+#define RCNT  (SKEIN_1024_ROUNDS_TOTAL/8)
+#ifdef SKEIN_LOOP /* configure how much to unroll the loop */
+#define SKEIN_UNROLL_1024 ((SKEIN_LOOP)%10)
+#else
+#define SKEIN_UNROLL_1024 (0)
+#endif
+
+#if (SKEIN_UNROLL_1024 != 0)
+#if (RCNT % SKEIN_UNROLL_1024)
+#error "Invalid SKEIN_UNROLL_1024" /* sanity check on unroll count */
+#endif
+#endif
+#define ROUND1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, \
+		  pF, ROT, r_num)                                             \
+do {                                                                          \
+	X##p0 += X##p1;                                                       \
+	X##p1 = rotl_64(X##p1, ROT##_0);                                      \
+	X##p1 ^= X##p0;                                                       \
+	X##p2 += X##p3;                                                       \
+	X##p3 = rotl_64(X##p3, ROT##_1);                                      \
+	X##p3 ^= X##p2;                                                       \
+	X##p4 += X##p5;                                                       \
+	X##p5 = rotl_64(X##p5, ROT##_2);                                      \
+	X##p5 ^= X##p4;                                                       \
+	X##p6 += X##p7;                                                       \
+	X##p7 = rotl_64(X##p7, ROT##_3);                                      \
+	X##p7 ^= X##p6;                                                       \
+	X##p8 += X##p9;                                                       \
+	X##p9 = rotl_64(X##p9, ROT##_4);                                      \
+	X##p9 ^= X##p8;                                                       \
+	X##pA += X##pB;                                                       \
+	X##pB = rotl_64(X##pB, ROT##_5);                                      \
+	X##pB ^= X##pA;                                                       \
+	X##pC += X##pD;                                                       \
+	X##pD = rotl_64(X##pD, ROT##_6);                                      \
+	X##pD ^= X##pC;                                                       \
+	X##pE += X##pF;                                                       \
+	X##pF = rotl_64(X##pF, ROT##_7);                                      \
+	X##pF ^= X##pE;                                                       \
+} while (0)
+
+#if SKEIN_UNROLL_1024 == 0
+#define R1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, pF, \
+	      ROT, rn)                                                        \
+do {                                                                          \
+	ROUND1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, \
+		  pF, ROT, rn);                                               \
+} while (0)
+
+#define I1024(R)                                                          \
+do {                                                                      \
+	/* inject the key schedule value */                               \
+	X00 += ks[((R) + 1) % 17];                                        \
+	X01 += ks[((R) + 2) % 17];                                        \
+	X02 += ks[((R) + 3) % 17];                                        \
+	X03 += ks[((R) + 4) % 17];                                        \
+	X04 += ks[((R) + 5) % 17];                                        \
+	X05 += ks[((R) + 6) % 17];                                        \
+	X06 += ks[((R) + 7) % 17];                                        \
+	X07 += ks[((R) + 8) % 17];                                        \
+	X08 += ks[((R) + 9) % 17];                                        \
+	X09 += ks[((R) + 10) % 17];                                       \
+	X10 += ks[((R) + 11) % 17];                                       \
+	X11 += ks[((R) + 12) % 17];                                       \
+	X12 += ks[((R) + 13) % 17];                                       \
+	X13 += ks[((R) + 14) % 17] + ts[((R) + 1) % 3];                   \
+	X14 += ks[((R) + 15) % 17] + ts[((R) + 2) % 3];                   \
+	X15 += ks[((R) + 16) % 17] + (R) + 1;                             \
+} while (0)
+#else /* looping version */
+#define R1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, pF, \
+	      ROT, rn)                                                        \
+do {                                                                          \
+	ROUND1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, \
+		  pF, ROT, rn);                                               \
+} while (0)
+
+#define I1024(R)                                                           \
+do {                                                                       \
+	/* inject the key schedule value */                                \
+	X00 += ks[r + (R) + 0];                                            \
+	X01 += ks[r + (R) + 1];                                            \
+	X02 += ks[r + (R) + 2];                                            \
+	X03 += ks[r + (R) + 3];                                            \
+	X04 += ks[r + (R) + 4];                                            \
+	X05 += ks[r + (R) + 5];                                            \
+	X06 += ks[r + (R) + 6];                                            \
+	X07 += ks[r + (R) + 7];                                            \
+	X08 += ks[r + (R) + 8];                                            \
+	X09 += ks[r + (R) + 9];                                            \
+	X10 += ks[r + (R) + 10];                                           \
+	X11 += ks[r + (R) + 11];                                           \
+	X12 += ks[r + (R) + 12];                                           \
+	X13 += ks[r + (R) + 13] + ts[r + (R) + 0];                         \
+	X14 += ks[r + (R) + 14] + ts[r + (R) + 1];                         \
+	X15 += ks[r + (R) + 15] + r + (R);                                 \
+	/* rotate key schedule */                                          \
+	ks[r + (R) + 16] = ks[r + (R) - 1];                                \
+	ts[r + (R) + 2] = ts[r + (R) - 1];                                 \
+} while (0)
+
+#endif
+#define R1024_8_ROUNDS(R)                                                     \
+do {                                                                          \
+	R1024(00, 01, 02, 03, 04, 05, 06, 07, 08, 09, 10, 11, 12, 13, 14, 15, \
+	      R1024_0, 8*(R) + 1);                                            \
+	R1024(00, 09, 02, 13, 06, 11, 04, 15, 10, 07, 12, 03, 14, 05, 08, 01, \
+	      R1024_1, 8*(R) + 2);                                            \
+	R1024(00, 07, 02, 05, 04, 03, 06, 01, 12, 15, 14, 13, 08, 11, 10, 09, \
+	      R1024_2, 8*(R) + 3);                                            \
+	R1024(00, 15, 02, 11, 06, 13, 04, 09, 14, 01, 08, 05, 10, 03, 12, 07, \
+	      R1024_3, 8*(R) + 4);                                            \
+	I1024(2*(R));                                                         \
+	R1024(00, 01, 02, 03, 04, 05, 06, 07, 08, 09, 10, 11, 12, 13, 14, 15, \
+	      R1024_4, 8*(R) + 5);                                            \
+	R1024(00, 09, 02, 13, 06, 11, 04, 15, 10, 07, 12, 03, 14, 05, 08, 01, \
+	      R1024_5, 8*(R) + 6);                                            \
+	R1024(00, 07, 02, 05, 04, 03, 06, 01, 12, 15, 14, 13, 08, 11, 10, 09, \
+	      R1024_6, 8*(R) + 7);                                            \
+	R1024(00, 15, 02, 11, 06, 13, 04, 09, 14, 01, 08, 05, 10, 03, 12, 07, \
+	      R1024_7, 8*(R) + 8);                                            \
+	I1024(2*(R)+1);                                                       \
+} while (0)
+
+#define R1024_UNROLL_R(NN)                              \
+		((SKEIN_UNROLL_1024 == 0 &&             \
+		SKEIN_1024_ROUNDS_TOTAL/8 > (NN)) ||  \
+		(SKEIN_UNROLL_1024 > (NN)))
+
+#if  (SKEIN_UNROLL_1024 > 14)
+#error  "need more unrolling in Skein_1024_Process_Block"
+#endif
+#endif
+
+/*****************************  SKEIN_256 ******************************/
+#if !(SKEIN_USE_ASM & 256)
+void skein_256_process_block(struct skein_256_ctx *ctx, const u8 *blk_ptr,
+			     size_t blk_cnt, size_t byte_cnt_add)
+{ /* do it in C */
+	enum {
+		WCNT = SKEIN_256_STATE_WORDS
+	};
+	size_t r;
+#if SKEIN_UNROLL_256
+	/* key schedule: chaining vars + tweak + "rot"*/
+	u64  kw[WCNT+4+RCNT*2];
+#else
+	/* key schedule words : chaining vars + tweak */
+	u64  kw[WCNT+4];
 #endif
 	u64  X0, X1, X2, X3; /* local copy of context vars, for speed */
 	u64  w[WCNT]; /* local copy of input block */
 #ifdef SKEIN_DEBUG
 	const u64 *X_ptr[4]; /* use for debugging (help cc put Xn in regs) */
 
-	X_ptr[0] = &X0;  X_ptr[1] = &X1;  X_ptr[2] = &X2;  X_ptr[3] = &X3;
+	X_ptr[0] = &X0;
+	X_ptr[1] = &X1;
+	X_ptr[2] = &X2;
+	X_ptr[3] = &X3;
 #endif
 	skein_assert(blk_cnt != 0); /* never call with blk_cnt == 0! */
 	ts[0] = ctx->h.tweak[0];
@@ -94,132 +401,62 @@ void skein_256_process_block(struct skein_256_ctx *ctx, const u8 *blk_ptr,
 		/* get input block in little-endian format */
 		skein_get64_lsb_first(w, blk_ptr, WCNT);
 		debug_save_tweak(ctx);
-		skein_show_block(BLK_BITS, &ctx->h, ctx->x, blk_ptr, w, ks, ts);
 
-		X0 = w[0] + ks[0]; /* do the first full key injection */
+		/* do the first full key injection */
+		X0 = w[0] + ks[0];
 		X1 = w[1] + ks[1] + ts[0];
 		X2 = w[2] + ks[2] + ts[1];
 		X3 = w[3] + ks[3];
 
-		/* show starting state values */
-		skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INITIAL,
-				 x_ptr);
-
 		blk_ptr += SKEIN_256_BLOCK_BYTES;
 
 		/* run the rounds */
-
-#define ROUND256(p0, p1, p2, p3, ROT, r_num)                              \
-do { \
-	X##p0 += X##p1; X##p1 = rotl_64(X##p1, ROT##_0); X##p1 ^= X##p0; \
-	X##p2 += X##p3; X##p3 = rotl_64(X##p3, ROT##_1); X##p3 ^= X##p2; \
-} while (0)
-
-#if SKEIN_UNROLL_256 == 0
-#define R256(p0, p1, p2, p3, ROT, r_num) /* fully unrolled */ \
-do { \
-	ROUND256(p0, p1, p2, p3, ROT, r_num); \
-	skein_show_r_ptr(BLK_BITS, &ctx->h, r_num, X_ptr); \
-} while (0)
-
-#define I256(R) \
-do { \
-	/* inject the key schedule value */ \
-	X0   += ks[((R)+1) % 5]; \
-	X1   += ks[((R)+2) % 5] + ts[((R)+1) % 3]; \
-	X2   += ks[((R)+3) % 5] + ts[((R)+2) % 3]; \
-	X3   += ks[((R)+4) % 5] +     (R)+1;       \
-	skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INJECT, X_ptr); \
-} while (0)
-#else /* looping version */
-#define R256(p0, p1, p2, p3, ROT, r_num) \
-do { \
-	ROUND256(p0, p1, p2, p3, ROT, r_num); \
-	skein_show_r_ptr(BLK_BITS, &ctx->h, 4 * (r - 1) + r_num, X_ptr); \
-} while (0)
-
-#define I256(R) \
-do { \
-	/* inject the key schedule value */ \
-	X0   += ks[r+(R)+0]; \
-	X1   += ks[r+(R)+1] + ts[r+(R)+0]; \
-	X2   += ks[r+(R)+2] + ts[r+(R)+1]; \
-	X3   += ks[r+(R)+3] +    r+(R);    \
-	/* rotate key schedule */ \
-	ks[r + (R) + 4]   = ks[r + (R) - 1]; \
-	ts[r + (R) + 2]   = ts[r + (R) - 1]; \
-	skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INJECT, X_ptr); \
-} while (0)
-
-	for (r = 1; r < 2 * RCNT; r += 2 * SKEIN_UNROLL_256)
+		for (r = 1;
+			r < (SKEIN_UNROLL_256 ? 2 * RCNT : 2);
+			r += (SKEIN_UNROLL_256 ? 2 * SKEIN_UNROLL_256 : 1)) {
+			R256_8_ROUNDS(0);
+#if   R256_UNROLL_R(1)
+			R256_8_ROUNDS(1);
+#endif
+#if   R256_UNROLL_R(2)
+			R256_8_ROUNDS(2);
+#endif
+#if   R256_UNROLL_R(3)
+			R256_8_ROUNDS(3);
+#endif
+#if   R256_UNROLL_R(4)
+			R256_8_ROUNDS(4);
+#endif
+#if   R256_UNROLL_R(5)
+			R256_8_ROUNDS(5);
+#endif
+#if   R256_UNROLL_R(6)
+			R256_8_ROUNDS(6);
+#endif
+#if   R256_UNROLL_R(7)
+			R256_8_ROUNDS(7);
+#endif
+#if   R256_UNROLL_R(8)
+			R256_8_ROUNDS(8);
+#endif
+#if   R256_UNROLL_R(9)
+			R256_8_ROUNDS(9);
+#endif
+#if   R256_UNROLL_R(10)
+			R256_8_ROUNDS(10);
+#endif
+#if   R256_UNROLL_R(11)
+			R256_8_ROUNDS(11);
+#endif
+#if   R256_UNROLL_R(12)
+			R256_8_ROUNDS(12);
+#endif
+#if   R256_UNROLL_R(13)
+			R256_8_ROUNDS(13);
+#endif
+#if   R256_UNROLL_R(14)
+			R256_8_ROUNDS(14);
 #endif
-		{
-#define R256_8_ROUNDS(R)                  \
-do { \
-		R256(0, 1, 2, 3, R_256_0, 8 * (R) + 1);  \
-		R256(0, 3, 2, 1, R_256_1, 8 * (R) + 2);  \
-		R256(0, 1, 2, 3, R_256_2, 8 * (R) + 3);  \
-		R256(0, 3, 2, 1, R_256_3, 8 * (R) + 4);  \
-		I256(2 * (R));                      \
-		R256(0, 1, 2, 3, R_256_4, 8 * (R) + 5);  \
-		R256(0, 3, 2, 1, R_256_5, 8 * (R) + 6);  \
-		R256(0, 1, 2, 3, R_256_6, 8 * (R) + 7);  \
-		R256(0, 3, 2, 1, R_256_7, 8 * (R) + 8);  \
-		I256(2 * (R) + 1); \
-} while (0)
-
-		R256_8_ROUNDS(0);
-
-#define R256_UNROLL_R(NN) \
-	((SKEIN_UNROLL_256 == 0 && \
-	  SKEIN_256_ROUNDS_TOTAL/8 > (NN)) || \
-	 (SKEIN_UNROLL_256 > (NN)))
-
-	#if   R256_UNROLL_R(1)
-		R256_8_ROUNDS(1);
-	#endif
-	#if   R256_UNROLL_R(2)
-		R256_8_ROUNDS(2);
-	#endif
-	#if   R256_UNROLL_R(3)
-		R256_8_ROUNDS(3);
-	#endif
-	#if   R256_UNROLL_R(4)
-		R256_8_ROUNDS(4);
-	#endif
-	#if   R256_UNROLL_R(5)
-		R256_8_ROUNDS(5);
-	#endif
-	#if   R256_UNROLL_R(6)
-		R256_8_ROUNDS(6);
-	#endif
-	#if   R256_UNROLL_R(7)
-		R256_8_ROUNDS(7);
-	#endif
-	#if   R256_UNROLL_R(8)
-		R256_8_ROUNDS(8);
-	#endif
-	#if   R256_UNROLL_R(9)
-		R256_8_ROUNDS(9);
-	#endif
-	#if   R256_UNROLL_R(10)
-		R256_8_ROUNDS(10);
-	#endif
-	#if   R256_UNROLL_R(11)
-		R256_8_ROUNDS(11);
-	#endif
-	#if   R256_UNROLL_R(12)
-		R256_8_ROUNDS(12);
-	#endif
-	#if   R256_UNROLL_R(13)
-		R256_8_ROUNDS(13);
-	#endif
-	#if   R256_UNROLL_R(14)
-		R256_8_ROUNDS(14);
-	#endif
-	#if  (SKEIN_UNROLL_256 > 14)
-#error  "need more unrolling in skein_256_process_block"
-	#endif
 		}
 		/* do the final "feedforward" xor, update context chaining */
 		ctx->x[0] = X0 ^ w[0];
@@ -227,8 +464,6 @@ do { \
 		ctx->x[2] = X2 ^ w[2];
 		ctx->x[3] = X3 ^ w[3];
 
-		skein_show_round(BLK_BITS, &ctx->h, SKEIN_RND_FEED_FWD, ctx->x);
-
 		ts[1] &= ~SKEIN_T1_FLAG_FIRST;
 	} while (--blk_cnt);
 	ctx->h.tweak[0] = ts[0];
@@ -256,20 +491,8 @@ void skein_512_process_block(struct skein_512_ctx *ctx, const u8 *blk_ptr,
 	enum {
 		WCNT = SKEIN_512_STATE_WORDS
 	};
-#undef  RCNT
-#define RCNT  (SKEIN_512_ROUNDS_TOTAL/8)
-
-#ifdef SKEIN_LOOP /* configure how much to unroll the loop */
-#define SKEIN_UNROLL_512 (((SKEIN_LOOP)/10)%10)
-#else
-#define SKEIN_UNROLL_512 (0)
-#endif
-
-#if SKEIN_UNROLL_512
-#if (RCNT % SKEIN_UNROLL_512)
-#error "Invalid SKEIN_UNROLL_512" /* sanity check on unroll count */
-#endif
 	size_t  r;
+#if SKEIN_UNROLL_512
 	u64  kw[WCNT+4+RCNT*2]; /* key sched: chaining vars + tweak + "rot"*/
 #else
 	u64  kw[WCNT+4]; /* key schedule words : chaining vars + tweak */
@@ -279,8 +502,14 @@ void skein_512_process_block(struct skein_512_ctx *ctx, const u8 *blk_ptr,
 #ifdef SKEIN_DEBUG
 	const u64 *X_ptr[8]; /* use for debugging (help cc put Xn in regs) */
 
-	X_ptr[0] = &X0;  X_ptr[1] = &X1;  X_ptr[2] = &X2;  X_ptr[3] = &X3;
-	X_ptr[4] = &X4;  X_ptr[5] = &X5;  X_ptr[6] = &X6;  X_ptr[7] = &X7;
+	X_ptr[0] = &X0;
+	X_ptr[1] = &X1;
+	X_ptr[2] = &X2;
+	X_ptr[3] = &X3;
+	X_ptr[4] = &X4;
+	X_ptr[5] = &X5;
+	X_ptr[6] = &X6;
+	X_ptr[7] = &X7;
 #endif
 
 	skein_assert(blk_cnt != 0); /* never call with blk_cnt == 0! */
@@ -310,143 +539,68 @@ void skein_512_process_block(struct skein_512_ctx *ctx, const u8 *blk_ptr,
 		/* get input block in little-endian format */
 		skein_get64_lsb_first(w, blk_ptr, WCNT);
 		debug_save_tweak(ctx);
-		skein_show_block(BLK_BITS, &ctx->h, ctx->x, blk_ptr, w, ks, ts);
 
-		X0   = w[0] + ks[0]; /* do the first full key injection */
-		X1   = w[1] + ks[1];
-		X2   = w[2] + ks[2];
-		X3   = w[3] + ks[3];
-		X4   = w[4] + ks[4];
-		X5   = w[5] + ks[5] + ts[0];
-		X6   = w[6] + ks[6] + ts[1];
-		X7   = w[7] + ks[7];
+		/* do the first full key injection */
+		X0 = w[0] + ks[0];
+		X1 = w[1] + ks[1];
+		X2 = w[2] + ks[2];
+		X3 = w[3] + ks[3];
+		X4 = w[4] + ks[4];
+		X5 = w[5] + ks[5] + ts[0];
+		X6 = w[6] + ks[6] + ts[1];
+		X7 = w[7] + ks[7];
 
 		blk_ptr += SKEIN_512_BLOCK_BYTES;
 
-		skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INITIAL,
-				 X_ptr);
 		/* run the rounds */
-#define ROUND512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num) \
-do { \
-	X##p0 += X##p1; X##p1 = rotl_64(X##p1, ROT##_0); X##p1 ^= X##p0; \
-	X##p2 += X##p3; X##p3 = rotl_64(X##p3, ROT##_1); X##p3 ^= X##p2; \
-	X##p4 += X##p5; X##p5 = rotl_64(X##p5, ROT##_2); X##p5 ^= X##p4; \
-	X##p6 += X##p7; X##p7 = rotl_64(X##p7, ROT##_3); X##p7 ^= X##p6; \
-} while (0)
-
-#if SKEIN_UNROLL_512 == 0
-#define R512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num) /* unrolled */ \
-do { \
-	ROUND512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num) \
-	skein_show_r_ptr(BLK_BITS, &ctx->h, r_num, X_ptr); \
-} while (0)
-
-#define I512(R) \
-do { \
-	/* inject the key schedule value */ \
-	X0   += ks[((R) + 1) % 9]; \
-	X1   += ks[((R) + 2) % 9]; \
-	X2   += ks[((R) + 3) % 9]; \
-	X3   += ks[((R) + 4) % 9]; \
-	X4   += ks[((R) + 5) % 9]; \
-	X5   += ks[((R) + 6) % 9] + ts[((R) + 1) % 3]; \
-	X6   += ks[((R) + 7) % 9] + ts[((R) + 2) % 3]; \
-	X7   += ks[((R) + 8) % 9] +     (R) + 1;       \
-	skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INJECT, X_ptr); \
-} while (0)
-#else /* looping version */
-#define R512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num) \
-do { \
-	ROUND512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num); \
-	skein_show_r_ptr(BLK_BITS, &ctx->h, 4 * (r - 1) + r_num, X_ptr); \
-} while (0)
-
-#define I512(R) \
-do { \
-	/* inject the key schedule value */ \
-	X0   += ks[r + (R) + 0]; \
-	X1   += ks[r + (R) + 1]; \
-	X2   += ks[r + (R) + 2]; \
-	X3   += ks[r + (R) + 3]; \
-	X4   += ks[r + (R) + 4]; \
-	X5   += ks[r + (R) + 5] + ts[r + (R) + 0]; \
-	X6   += ks[r + (R) + 6] + ts[r + (R) + 1]; \
-	X7   += ks[r + (R) + 7] +         r + (R); \
-	/* rotate key schedule */ \
-	ks[r +         (R) + 8] = ks[r + (R) - 1]; \
-	ts[r +         (R) + 2] = ts[r + (R) - 1]; \
-	skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INJECT, X_ptr); \
-} while (0)
-
-		for (r = 1; r < 2 * RCNT; r += 2 * SKEIN_UNROLL_512)
-#endif /* end of looped code definitions */
-		{
-#define R512_8_ROUNDS(R)  /* do 8 full rounds */  \
-do { \
-		R512(0, 1, 2, 3, 4, 5, 6, 7, R_512_0, 8 * (R) + 1);   \
-		R512(2, 1, 4, 7, 6, 5, 0, 3, R_512_1, 8 * (R) + 2);   \
-		R512(4, 1, 6, 3, 0, 5, 2, 7, R_512_2, 8 * (R) + 3);   \
-		R512(6, 1, 0, 7, 2, 5, 4, 3, R_512_3, 8 * (R) + 4);   \
-		I512(2 * (R));                              \
-		R512(0, 1, 2, 3, 4, 5, 6, 7, R_512_4, 8 * (R) + 5);   \
-		R512(2, 1, 4, 7, 6, 5, 0, 3, R_512_5, 8 * (R) + 6);   \
-		R512(4, 1, 6, 3, 0, 5, 2, 7, R_512_6, 8 * (R) + 7);   \
-		R512(6, 1, 0, 7, 2, 5, 4, 3, R_512_7, 8 * (R) + 8);   \
-		I512(2 * (R) + 1);        /* and key injection */ \
-} while (0)
+		for (r = 1;
+			r < (SKEIN_UNROLL_512 ? 2 * RCNT : 2);
+			r += (SKEIN_UNROLL_512 ? 2 * SKEIN_UNROLL_512 : 1)) {
 
 			R512_8_ROUNDS(0);
 
-#define R512_UNROLL_R(NN) \
-		((SKEIN_UNROLL_512 == 0 && \
-		  SKEIN_512_ROUNDS_TOTAL/8 > (NN)) || \
-		 (SKEIN_UNROLL_512 > (NN)))
-
-	#if   R512_UNROLL_R(1)
+#if   R512_UNROLL_R(1)
 			R512_8_ROUNDS(1);
-	#endif
-	#if   R512_UNROLL_R(2)
+#endif
+#if   R512_UNROLL_R(2)
 			R512_8_ROUNDS(2);
-	#endif
-	#if   R512_UNROLL_R(3)
+#endif
+#if   R512_UNROLL_R(3)
 			R512_8_ROUNDS(3);
-	#endif
-	#if   R512_UNROLL_R(4)
+#endif
+#if   R512_UNROLL_R(4)
 			R512_8_ROUNDS(4);
-	#endif
-	#if   R512_UNROLL_R(5)
+#endif
+#if   R512_UNROLL_R(5)
 			R512_8_ROUNDS(5);
-	#endif
-	#if   R512_UNROLL_R(6)
+#endif
+#if   R512_UNROLL_R(6)
 			R512_8_ROUNDS(6);
-	#endif
-	#if   R512_UNROLL_R(7)
+#endif
+#if   R512_UNROLL_R(7)
 			R512_8_ROUNDS(7);
-	#endif
-	#if   R512_UNROLL_R(8)
+#endif
+#if   R512_UNROLL_R(8)
 			R512_8_ROUNDS(8);
-	#endif
-	#if   R512_UNROLL_R(9)
+#endif
+#if   R512_UNROLL_R(9)
 			R512_8_ROUNDS(9);
-	#endif
-	#if   R512_UNROLL_R(10)
+#endif
+#if   R512_UNROLL_R(10)
 			R512_8_ROUNDS(10);
-	#endif
-	#if   R512_UNROLL_R(11)
+#endif
+#if   R512_UNROLL_R(11)
 			R512_8_ROUNDS(11);
-	#endif
-	#if   R512_UNROLL_R(12)
+#endif
+#if   R512_UNROLL_R(12)
 			R512_8_ROUNDS(12);
-	#endif
-	#if   R512_UNROLL_R(13)
+#endif
+#if   R512_UNROLL_R(13)
 			R512_8_ROUNDS(13);
-	#endif
-	#if   R512_UNROLL_R(14)
+#endif
+#if   R512_UNROLL_R(14)
 			R512_8_ROUNDS(14);
-	#endif
-	#if  (SKEIN_UNROLL_512 > 14)
-#error  "need more unrolling in skein_512_process_block"
-	#endif
+#endif
 		}
 
 		/* do the final "feedforward" xor, update context chaining */
@@ -458,7 +612,6 @@ do { \
 		ctx->x[5] = X5 ^ w[5];
 		ctx->x[6] = X6 ^ w[6];
 		ctx->x[7] = X7 ^ w[7];
-		skein_show_round(BLK_BITS, &ctx->h, SKEIN_RND_FEED_FWD, ctx->x);
 
 		ts[1] &= ~SKEIN_T1_FLAG_FIRST;
 	} while (--blk_cnt);
@@ -487,20 +640,8 @@ void skein_1024_process_block(struct skein_1024_ctx *ctx, const u8 *blk_ptr,
 	enum {
 		WCNT = SKEIN_1024_STATE_WORDS
 	};
-#undef  RCNT
-#define RCNT  (SKEIN_1024_ROUNDS_TOTAL/8)
-
-#ifdef SKEIN_LOOP /* configure how much to unroll the loop */
-#define SKEIN_UNROLL_1024 ((SKEIN_LOOP)%10)
-#else
-#define SKEIN_UNROLL_1024 (0)
-#endif
-
-#if (SKEIN_UNROLL_1024 != 0)
-#if (RCNT % SKEIN_UNROLL_1024)
-#error "Invalid SKEIN_UNROLL_1024" /* sanity check on unroll count */
-#endif
 	size_t  r;
+#if (SKEIN_UNROLL_1024 != 0)
 	u64  kw[WCNT+4+RCNT*2]; /* key sched: chaining vars + tweak + "rot" */
 #else
 	u64  kw[WCNT+4]; /* key schedule words : chaining vars + tweak */
@@ -510,16 +651,6 @@ void skein_1024_process_block(struct skein_1024_ctx *ctx, const u8 *blk_ptr,
 	u64  X00, X01, X02, X03, X04, X05, X06, X07,
 	     X08, X09, X10, X11, X12, X13, X14, X15;
 	u64  w[WCNT]; /* local copy of input block */
-#ifdef SKEIN_DEBUG
-	const u64 *X_ptr[16]; /* use for debugging (help cc put Xn in regs) */
-
-	X_ptr[0]  = &X00;  X_ptr[1]  = &X01;  X_ptr[2]  = &X02;
-	X_ptr[3]  = &X03;  X_ptr[4]  = &X04;  X_ptr[5]  = &X05;
-	X_ptr[6]  = &X06;  X_ptr[7]  = &X07;  X_ptr[8]  = &X08;
-	X_ptr[9]  = &X09;  X_ptr[10] = &X10;  X_ptr[11] = &X11;
-	X_ptr[12] = &X12;  X_ptr[13] = &X13;  X_ptr[14] = &X14;
-	X_ptr[15] = &X15;
-#endif
 
 	skein_assert(blk_cnt != 0); /* never call with blk_cnt == 0! */
 	ts[0] = ctx->h.tweak[0];
@@ -548,192 +679,81 @@ void skein_1024_process_block(struct skein_1024_ctx *ctx, const u8 *blk_ptr,
 		ks[13] = ctx->x[13];
 		ks[14] = ctx->x[14];
 		ks[15] = ctx->x[15];
-		ks[16] =  ks[0] ^  ks[1] ^  ks[2] ^  ks[3] ^
-			  ks[4] ^  ks[5] ^  ks[6] ^  ks[7] ^
-			  ks[8] ^  ks[9] ^ ks[10] ^ ks[11] ^
+		ks[16] =  ks[0] ^ ks[1] ^ ks[2] ^ ks[3] ^
+			  ks[4] ^ ks[5] ^ ks[6] ^ ks[7] ^
+			  ks[8] ^ ks[9] ^ ks[10] ^ ks[11] ^
 			  ks[12] ^ ks[13] ^ ks[14] ^ ks[15] ^ SKEIN_KS_PARITY;
 
-		ts[2]  = ts[0] ^ ts[1];
+		ts[2] = ts[0] ^ ts[1];
 
 		/* get input block in little-endian format */
 		skein_get64_lsb_first(w, blk_ptr, WCNT);
 		debug_save_tweak(ctx);
-		skein_show_block(BLK_BITS, &ctx->h, ctx->x, blk_ptr, w, ks, ts);
-
-		X00    =  w[0] +  ks[0]; /* do the first full key injection */
-		X01    =  w[1] +  ks[1];
-		X02    =  w[2] +  ks[2];
-		X03    =  w[3] +  ks[3];
-		X04    =  w[4] +  ks[4];
-		X05    =  w[5] +  ks[5];
-		X06    =  w[6] +  ks[6];
-		X07    =  w[7] +  ks[7];
-		X08    =  w[8] +  ks[8];
-		X09    =  w[9] +  ks[9];
-		X10    = w[10] + ks[10];
-		X11    = w[11] + ks[11];
-		X12    = w[12] + ks[12];
-		X13    = w[13] + ks[13] + ts[0];
-		X14    = w[14] + ks[14] + ts[1];
-		X15    = w[15] + ks[15];
-
-		skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INITIAL,
-				 X_ptr);
-
-#define ROUND1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, \
-			pF, ROT, r_num) \
-do { \
-	X##p0 += X##p1; X##p1 = rotl_64(X##p1, ROT##_0); X##p1 ^= X##p0;   \
-	X##p2 += X##p3; X##p3 = rotl_64(X##p3, ROT##_1); X##p3 ^= X##p2;   \
-	X##p4 += X##p5; X##p5 = rotl_64(X##p5, ROT##_2); X##p5 ^= X##p4;   \
-	X##p6 += X##p7; X##p7 = rotl_64(X##p7, ROT##_3); X##p7 ^= X##p6;   \
-	X##p8 += X##p9; X##p9 = rotl_64(X##p9, ROT##_4); X##p9 ^= X##p8;   \
-	X##pA += X##pB; X##pB = rotl_64(X##pB, ROT##_5); X##pB ^= X##pA;   \
-	X##pC += X##pD; X##pD = rotl_64(X##pD, ROT##_6); X##pD ^= X##pC;   \
-	X##pE += X##pF; X##pF = rotl_64(X##pF, ROT##_7); X##pF ^= X##pE;   \
-} while (0)
-
-#if SKEIN_UNROLL_1024 == 0
-#define R1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, pF, \
-		ROT, rn) \
-do { \
-	ROUND1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, \
-			pF, ROT, rn); \
-	skein_show_r_ptr(BLK_BITS, &ctx->h, rn, X_ptr); \
-} while (0)
-
-#define I1024(R) \
-do { \
-	/* inject the key schedule value */ \
-	X00   += ks[((R) +  1) % 17]; \
-	X01   += ks[((R) +  2) % 17]; \
-	X02   += ks[((R) +  3) % 17]; \
-	X03   += ks[((R) +  4) % 17]; \
-	X04   += ks[((R) +  5) % 17]; \
-	X05   += ks[((R) +  6) % 17]; \
-	X06   += ks[((R) +  7) % 17]; \
-	X07   += ks[((R) +  8) % 17]; \
-	X08   += ks[((R) +  9) % 17]; \
-	X09   += ks[((R) + 10) % 17]; \
-	X10   += ks[((R) + 11) % 17]; \
-	X11   += ks[((R) + 12) % 17]; \
-	X12   += ks[((R) + 13) % 17]; \
-	X13   += ks[((R) + 14) % 17] + ts[((R) + 1) % 3]; \
-	X14   += ks[((R) + 15) % 17] + ts[((R) + 2) % 3]; \
-	X15   += ks[((R) + 16) % 17] +     (R) + 1;       \
-	skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INJECT, X_ptr); \
-} while (0)
-#else /* looping version */
-#define R1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, pF, \
-		ROT, rn) \
-do { \
-	ROUND1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, \
-			pF, ROT, rn); \
-	skein_show_r_ptr(BLK_BITS, &ctx->h, 4 * (r - 1) + rn, X_ptr); \
-} while (0)
-
-#define I1024(R) \
-do { \
-	/* inject the key schedule value */ \
-	X00   += ks[r + (R) +  0]; \
-	X01   += ks[r + (R) +  1]; \
-	X02   += ks[r + (R) +  2]; \
-	X03   += ks[r + (R) +  3]; \
-	X04   += ks[r + (R) +  4]; \
-	X05   += ks[r + (R) +  5]; \
-	X06   += ks[r + (R) +  6]; \
-	X07   += ks[r + (R) +  7]; \
-	X08   += ks[r + (R) +  8]; \
-	X09   += ks[r + (R) +  9]; \
-	X10   += ks[r + (R) + 10]; \
-	X11   += ks[r + (R) + 11]; \
-	X12   += ks[r + (R) + 12]; \
-	X13   += ks[r + (R) + 13] + ts[r + (R) + 0]; \
-	X14   += ks[r + (R) + 14] + ts[r + (R) + 1]; \
-	X15   += ks[r + (R) + 15] +         r + (R); \
-	/* rotate key schedule */ \
-	ks[r  +         (R) + 16] = ks[r + (R) - 1]; \
-	ts[r  +         (R) +  2] = ts[r + (R) - 1]; \
-	skein_show_r_ptr(BLK_BITSi, &ctx->h, SKEIN_RND_KEY_INJECT, X_ptr); \
-} while (0)
-
-		for (r = 1; r <= 2 * RCNT; r += 2 * SKEIN_UNROLL_1024)
-#endif
-		{
-#define R1024_8_ROUNDS(R) \
-do { \
-	R1024(00, 01, 02, 03, 04, 05, 06, 07, 08, 09, 10, 11, 12, 13, 14, 15, \
-		R1024_0, 8*(R) + 1); \
-	R1024(00, 09, 02, 13, 06, 11, 04, 15, 10, 07, 12, 03, 14, 05, 08, 01, \
-		R1024_1, 8*(R) + 2); \
-	R1024(00, 07, 02, 05, 04, 03, 06, 01, 12, 15, 14, 13, 08, 11, 10, 09, \
-		R1024_2, 8*(R) + 3); \
-	R1024(00, 15, 02, 11, 06, 13, 04, 09, 14, 01, 08, 05, 10, 03, 12, 07, \
-		R1024_3, 8*(R) + 4); \
-	I1024(2*(R)); \
-	R1024(00, 01, 02, 03, 04, 05, 06, 07, 08, 09, 10, 11, 12, 13, 14, 15, \
-		R1024_4, 8*(R) + 5); \
-	R1024(00, 09, 02, 13, 06, 11, 04, 15, 10, 07, 12, 03, 14, 05, 08, 01, \
-		R1024_5, 8*(R) + 6); \
-	R1024(00, 07, 02, 05, 04, 03, 06, 01, 12, 15, 14, 13, 08, 11, 10, 09, \
-		R1024_6, 8*(R) + 7); \
-	R1024(00, 15, 02, 11, 06, 13, 04, 09, 14, 01, 08, 05, 10, 03, 12, 07, \
-		R1024_7, 8*(R) + 8); \
-	I1024(2*(R)+1); \
-} while (0)
 
+		/* do the first full key injection */
+		X00 = w[0] + ks[0];
+		X01 = w[1] + ks[1];
+		X02 = w[2] + ks[2];
+		X03 = w[3] + ks[3];
+		X04 = w[4] + ks[4];
+		X05 = w[5] + ks[5];
+		X06 = w[6] + ks[6];
+		X07 = w[7] + ks[7];
+		X08 = w[8] + ks[8];
+		X09 = w[9] + ks[9];
+		X10 = w[10] + ks[10];
+		X11 = w[11] + ks[11];
+		X12 = w[12] + ks[12];
+		X13 = w[13] + ks[13] + ts[0];
+		X14 = w[14] + ks[14] + ts[1];
+		X15 = w[15] + ks[15];
+
+		for (r = 1;
+			r < (SKEIN_UNROLL_1024 ? 2 * RCNT : 2);
+			r += (SKEIN_UNROLL_1024 ? 2 * SKEIN_UNROLL_1024 : 1)) {
 			R1024_8_ROUNDS(0);
-
-#define R1024_UNROLL_R(NN) \
-		((SKEIN_UNROLL_1024 == 0 && \
-		  SKEIN_1024_ROUNDS_TOTAL/8 > (NN)) || \
-		 (SKEIN_UNROLL_1024 > (NN)))
-
-	#if   R1024_UNROLL_R(1)
+#if   R1024_UNROLL_R(1)
 			R1024_8_ROUNDS(1);
-	#endif
-	#if   R1024_UNROLL_R(2)
+#endif
+#if   R1024_UNROLL_R(2)
 			R1024_8_ROUNDS(2);
-	#endif
-	#if   R1024_UNROLL_R(3)
+#endif
+#if   R1024_UNROLL_R(3)
 			R1024_8_ROUNDS(3);
-	#endif
-	#if   R1024_UNROLL_R(4)
+#endif
+#if   R1024_UNROLL_R(4)
 			R1024_8_ROUNDS(4);
-	#endif
-	#if   R1024_UNROLL_R(5)
+#endif
+#if   R1024_UNROLL_R(5)
 			R1024_8_ROUNDS(5);
-	#endif
-	#if   R1024_UNROLL_R(6)
+#endif
+#if   R1024_UNROLL_R(6)
 			R1024_8_ROUNDS(6);
-	#endif
-	#if   R1024_UNROLL_R(7)
+#endif
+#if   R1024_UNROLL_R(7)
 			R1024_8_ROUNDS(7);
-	#endif
-	#if   R1024_UNROLL_R(8)
+#endif
+#if   R1024_UNROLL_R(8)
 			R1024_8_ROUNDS(8);
-	#endif
-	#if   R1024_UNROLL_R(9)
+#endif
+#if   R1024_UNROLL_R(9)
 			R1024_8_ROUNDS(9);
-	#endif
-	#if   R1024_UNROLL_R(10)
+#endif
+#if   R1024_UNROLL_R(10)
 			R1024_8_ROUNDS(10);
-	#endif
-	#if   R1024_UNROLL_R(11)
+#endif
+#if   R1024_UNROLL_R(11)
 			R1024_8_ROUNDS(11);
-	#endif
-	#if   R1024_UNROLL_R(12)
+#endif
+#if   R1024_UNROLL_R(12)
 			R1024_8_ROUNDS(12);
-	#endif
-	#if   R1024_UNROLL_R(13)
+#endif
+#if   R1024_UNROLL_R(13)
 			R1024_8_ROUNDS(13);
-	#endif
-	#if   R1024_UNROLL_R(14)
+#endif
+#if   R1024_UNROLL_R(14)
 			R1024_8_ROUNDS(14);
-	#endif
-#if  (SKEIN_UNROLL_1024 > 14)
-#error  "need more unrolling in Skein_1024_Process_Block"
-  #endif
+#endif
 		}
 		/* do the final "feedforward" xor, update context chaining */
 
@@ -754,8 +774,6 @@ do { \
 		ctx->x[14] = X14 ^ w[14];
 		ctx->x[15] = X15 ^ w[15];
 
-		skein_show_round(BLK_BITS, &ctx->h, SKEIN_RND_FEED_FWD, ctx->x);
-
 		ts[1] &= ~SKEIN_T1_FLAG_FIRST;
 		blk_ptr += SKEIN_1024_BLOCK_BYTES;
 	} while (--blk_cnt);
diff --git a/drivers/staging/skein/skein_block.h b/drivers/staging/skein/skein_block.h
index bd7bdc35df29..9d40f4a5267b 100644
--- a/drivers/staging/skein/skein_block.h
+++ b/drivers/staging/skein/skein_block.h
@@ -10,7 +10,7 @@
 #ifndef _SKEIN_BLOCK_H_
 #define _SKEIN_BLOCK_H_
 
-#include "skein.h" /* get the Skein API definitions   */
+#include "skein_base.h" /* get the Skein API definitions   */
 
 void skein_256_process_block(struct skein_256_ctx *ctx, const u8 *blk_ptr,
 			     size_t blk_cnt, size_t byte_cnt_add);
diff --git a/drivers/staging/skein/skein_generic.c b/drivers/staging/skein/skein_generic.c
new file mode 100644
index 000000000000..85bd7d0168b0
--- /dev/null
+++ b/drivers/staging/skein/skein_generic.c
@@ -0,0 +1,216 @@
+/*
+ * Cryptographic API.
+ *
+ * Skein256 Hash Algorithm.
+ *
+ * Derived from cryptoapi implementation, adapted for in-place
+ * scatterlist interface.
+ *
+ * Copyright (c) Eric Rost <eric.rost@mybabylon.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.
+ *
+ */
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <crypto/internal/hash.h>
+#include "skein_base.h"
+
+
+static int skein256_init(struct shash_desc *desc)
+{
+	return skein_256_init((struct skein_256_ctx *) shash_desc_ctx(desc),
+			SKEIN256_DIGEST_BIT_SIZE);
+}
+
+static int skein256_update(struct shash_desc *desc, const u8 *data,
+			unsigned int len)
+{
+	return skein_256_update((struct skein_256_ctx *)shash_desc_ctx(desc),
+				data, len);
+}
+
+static int skein256_final(struct shash_desc *desc, u8 *out)
+{
+	return skein_256_final((struct skein_256_ctx *)shash_desc_ctx(desc),
+				out);
+}
+
+static int skein256_export(struct shash_desc *desc, void *out)
+{
+	struct skein_256_ctx *sctx = shash_desc_ctx(desc);
+
+	memcpy(out, sctx, sizeof(*sctx));
+	return 0;
+}
+
+static int skein256_import(struct shash_desc *desc, const void *in)
+{
+	struct skein_256_ctx *sctx = shash_desc_ctx(desc);
+
+	memcpy(sctx, in, sizeof(*sctx));
+	return 0;
+}
+
+static int skein512_init(struct shash_desc *desc)
+{
+	return skein_512_init((struct skein_512_ctx *)shash_desc_ctx(desc),
+				SKEIN512_DIGEST_BIT_SIZE);
+}
+
+static int skein512_update(struct shash_desc *desc, const u8 *data,
+			unsigned int len)
+{
+	return skein_512_update((struct skein_512_ctx *)shash_desc_ctx(desc),
+				data, len);
+}
+
+static int skein512_final(struct shash_desc *desc, u8 *out)
+{
+	return skein_512_final((struct skein_512_ctx *)shash_desc_ctx(desc),
+				out);
+}
+
+static int skein512_export(struct shash_desc *desc, void *out)
+{
+	struct skein_512_ctx *sctx = shash_desc_ctx(desc);
+
+	memcpy(out, sctx, sizeof(*sctx));
+	return 0;
+}
+
+static int skein512_import(struct shash_desc *desc, const void *in)
+{
+	struct skein_512_ctx *sctx = shash_desc_ctx(desc);
+
+	memcpy(sctx, in, sizeof(*sctx));
+	return 0;
+}
+
+static int skein1024_init(struct shash_desc *desc)
+{
+	return skein_1024_init((struct skein_1024_ctx *)shash_desc_ctx(desc),
+				SKEIN1024_DIGEST_BIT_SIZE);
+}
+
+static int skein1024_update(struct shash_desc *desc, const u8 *data,
+			unsigned int len)
+{
+	return skein_1024_update((struct skein_1024_ctx *)shash_desc_ctx(desc),
+				data, len);
+}
+
+static int skein1024_final(struct shash_desc *desc, u8 *out)
+{
+	return skein_1024_final((struct skein_1024_ctx *)shash_desc_ctx(desc),
+			out);
+}
+
+static int skein1024_export(struct shash_desc *desc, void *out)
+{
+	struct skein_1024_ctx *sctx = shash_desc_ctx(desc);
+
+	memcpy(out, sctx, sizeof(*sctx));
+	return 0;
+}
+
+static int skein1024_import(struct shash_desc *desc, const void *in)
+{
+	struct skein_1024_ctx *sctx = shash_desc_ctx(desc);
+
+	memcpy(sctx, in, sizeof(*sctx));
+	return 0;
+}
+
+static struct shash_alg alg256 = {
+	.digestsize	=	(SKEIN256_DIGEST_BIT_SIZE / 8),
+	.init		=	skein256_init,
+	.update		=	skein256_update,
+	.final		=	skein256_final,
+	.export		=	skein256_export,
+	.import		=	skein256_import,
+	.descsize	=	sizeof(struct skein_256_ctx),
+	.statesize	=	sizeof(struct skein_256_ctx),
+	.base		=	{
+		.cra_name		=	"skein256",
+		.cra_driver_name	=	"skein",
+		.cra_flags		=	CRYPTO_ALG_TYPE_SHASH,
+		.cra_blocksize		=	SKEIN_256_BLOCK_BYTES,
+		.cra_module		=	THIS_MODULE,
+	}
+};
+
+static struct shash_alg alg512 = {
+	.digestsize	=	(SKEIN512_DIGEST_BIT_SIZE / 8),
+	.init		=	skein512_init,
+	.update		=	skein512_update,
+	.final		=	skein512_final,
+	.export		=	skein512_export,
+	.import		=	skein512_import,
+	.descsize	=	sizeof(struct skein_512_ctx),
+	.statesize	=	sizeof(struct skein_512_ctx),
+	.base		=	{
+		.cra_name		=	"skein512",
+		.cra_driver_name	=	"skein",
+		.cra_flags		=	CRYPTO_ALG_TYPE_SHASH,
+		.cra_blocksize		=	SKEIN_512_BLOCK_BYTES,
+		.cra_module		=	THIS_MODULE,
+	}
+};
+
+static struct shash_alg alg1024 = {
+	.digestsize	=	(SKEIN1024_DIGEST_BIT_SIZE / 8),
+	.init		=	skein1024_init,
+	.update		=	skein1024_update,
+	.final		=	skein1024_final,
+	.export		=	skein1024_export,
+	.import		=	skein1024_import,
+	.descsize	=	sizeof(struct skein_1024_ctx),
+	.statesize	=	sizeof(struct skein_1024_ctx),
+	.base		=	{
+		.cra_name		=	"skein1024",
+		.cra_driver_name	=	"skein",
+		.cra_flags		=	CRYPTO_ALG_TYPE_SHASH,
+		.cra_blocksize		=	SKEIN_1024_BLOCK_BYTES,
+		.cra_module		=	THIS_MODULE,
+	}
+};
+
+static int __init skein_generic_init(void)
+{
+	if (crypto_register_shash(&alg256))
+		goto out;
+	if (crypto_register_shash(&alg512))
+		goto unreg256;
+	if (crypto_register_shash(&alg1024))
+		goto unreg512;
+
+	return 0;
+
+		
+unreg512:
+	crypto_unregister_shash(&alg512);
+unreg256:
+	crypto_unregister_shash(&alg256);
+out:
+	return -1;
+}
+
+static void __exit skein_generic_fini(void)
+{
+	crypto_unregister_shash(&alg256);
+	crypto_unregister_shash(&alg512);
+	crypto_unregister_shash(&alg1024);
+}
+
+module_init(skein_generic_init);
+module_exit(skein_generic_fini);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Skein Hash Algorithm");
+
+MODULE_ALIAS("skein");
diff --git a/drivers/staging/skein/skein_iv.h b/drivers/staging/skein/skein_iv.h
index d9dc1d5ed551..8a06314d0ed4 100644
--- a/drivers/staging/skein/skein_iv.h
+++ b/drivers/staging/skein/skein_iv.h
@@ -1,7 +1,7 @@
 #ifndef _SKEIN_IV_H_
 #define _SKEIN_IV_H_
 
-#include "skein.h"    /* get Skein macros and types */
+#include "skein_base.h"    /* get Skein macros and types */
 
 /*
 ***************** Pre-computed Skein IVs *******************
diff --git a/drivers/staging/skein/threefish_api.h b/drivers/staging/skein/threefish_api.h
index 8d5ddf8b3a9b..8e0a0b77ecce 100644
--- a/drivers/staging/skein/threefish_api.h
+++ b/drivers/staging/skein/threefish_api.h
@@ -29,7 +29,7 @@
  */
 
 #include <linux/types.h>
-#include "skein.h"
+#include "skein_base.h"
 
 #define KEY_SCHEDULE_CONST 0x1BD11BDAA9FC1A22L
 
diff --git a/drivers/staging/slicoss/slicoss.c b/drivers/staging/slicoss/slicoss.c
index 56ca3b6c1444..42d62ef56cb8 100644
--- a/drivers/staging/slicoss/slicoss.c
+++ b/drivers/staging/slicoss/slicoss.c
@@ -498,12 +498,14 @@ static int slic_card_download(struct adapter *adapter)
 			slic_reg32_write(&slic_regs->slic_wcs,
 					 baseaddress + codeaddr, FLUSH);
 			/* Write out instruction to low addr */
-			slic_reg32_write(&slic_regs->slic_wcs, instruction, FLUSH);
+			slic_reg32_write(&slic_regs->slic_wcs,
+					instruction, FLUSH);
 			instruction = *(u32 *)(fw->data + index);
 			index += 4;
 
 			/* Write out instruction to high addr */
-			slic_reg32_write(&slic_regs->slic_wcs, instruction, FLUSH);
+			slic_reg32_write(&slic_regs->slic_wcs,
+					instruction, FLUSH);
 			instruction = *(u32 *)(fw->data + index);
 			index += 4;
 		}
@@ -596,8 +598,7 @@ static void slic_mac_address_config(struct adapter *adapter)
 	u32 value2;
 	__iomem struct slic_regs *slic_regs = adapter->slic_regs;
 
-	value = *(u32 *) &adapter->currmacaddr[2];
-	value = ntohl(value);
+	value = ntohl(*(__be32 *) &adapter->currmacaddr[2]);
 	slic_reg32_write(&slic_regs->slic_wraddral, value, FLUSH);
 	slic_reg32_write(&slic_regs->slic_wraddrbl, value, FLUSH);
 
@@ -1533,14 +1534,18 @@ retry_rcvqfill:
 				dev_err(dev, "%s: LOW 32bits PHYSICAL ADDRESS == 0\n",
 					__func__);
 				dev_err(dev, "skb[%p] PROBLEM\n", skb);
-				dev_err(dev, "         skbdata[%p]\n", skb->data);
+				dev_err(dev, "         skbdata[%p]\n",
+						skb->data);
 				dev_err(dev, "         skblen[%x]\n", skb->len);
 				dev_err(dev, "         paddr[%p]\n", paddr);
 				dev_err(dev, "         paddrl[%x]\n", paddrl);
 				dev_err(dev, "         paddrh[%x]\n", paddrh);
-				dev_err(dev, "         rcvq->head[%p]\n", rcvq->head);
-				dev_err(dev, "         rcvq->tail[%p]\n", rcvq->tail);
-				dev_err(dev, "         rcvq->count[%x]\n", rcvq->count);
+				dev_err(dev, "         rcvq->head[%p]\n",
+						rcvq->head);
+				dev_err(dev, "         rcvq->tail[%p]\n",
+						rcvq->tail);
+				dev_err(dev, "         rcvq->count[%x]\n",
+						rcvq->count);
 				dev_err(dev, "SKIP THIS SKB!!!!!!!!\n");
 				goto retry_rcvqfill;
 			}
@@ -1549,14 +1554,18 @@ retry_rcvqfill:
 				dev_err(dev, "%s: LOW 32bits PHYSICAL ADDRESS == 0\n",
 					__func__);
 				dev_err(dev, "skb[%p] PROBLEM\n", skb);
-				dev_err(dev, "         skbdata[%p]\n", skb->data);
+				dev_err(dev, "         skbdata[%p]\n",
+						skb->data);
 				dev_err(dev, "         skblen[%x]\n", skb->len);
 				dev_err(dev, "         paddr[%p]\n", paddr);
 				dev_err(dev, "         paddrl[%x]\n", paddrl);
 				dev_err(dev, "         paddrh[%x]\n", paddrh);
-				dev_err(dev, "         rcvq->head[%p]\n", rcvq->head);
-				dev_err(dev, "         rcvq->tail[%p]\n", rcvq->tail);
-				dev_err(dev, "         rcvq->count[%x]\n", rcvq->count);
+				dev_err(dev, "         rcvq->head[%p]\n",
+						rcvq->head);
+				dev_err(dev, "         rcvq->tail[%p]\n",
+						rcvq->tail);
+				dev_err(dev, "         rcvq->count[%x]\n",
+						rcvq->count);
 				dev_err(dev, "GIVE TO CARD ANYWAY\n");
 			}
 #endif
@@ -1612,7 +1621,7 @@ static int slic_rcvqueue_init(struct adapter *adapter)
 	rcvq->size = SLIC_RCVQ_ENTRIES;
 	rcvq->errors = 0;
 	rcvq->count = 0;
-	i = (SLIC_RCVQ_ENTRIES / SLIC_RCVQ_FILLENTRIES);
+	i = SLIC_RCVQ_ENTRIES / SLIC_RCVQ_FILLENTRIES;
 	count = 0;
 	while (i) {
 		count += slic_rcvqueue_fill(adapter);
@@ -1788,7 +1797,7 @@ static int slic_mcast_add_list(struct adapter *adapter, char *address)
 	if (mcaddr == NULL)
 		return 1;
 
-	memcpy(mcaddr->address, address, ETH_ALEN);
+	ether_addr_copy(mcaddr->address, address);
 
 	mcaddr->next = adapter->mcastaddrs;
 	adapter->mcastaddrs = mcaddr;
@@ -1885,7 +1894,8 @@ static void slic_xmit_fail(struct adapter *adapter,
 			break;
 		case XMIT_FAIL_HOSTCMD_FAIL:
 			dev_err(&adapter->netdev->dev,
-				"xmit_start skb[%p] type[%x] No host commands available\n", skb, skb->pkt_type);
+				"xmit_start skb[%p] type[%x] No host commands available\n",
+				skb, skb->pkt_type);
 			break;
 		}
 	}
@@ -2097,7 +2107,8 @@ static void slic_interrupt_card_up(u32 isr, struct adapter *adapter,
 				}
 			} else if (isr & ISR_XDROP) {
 				dev_err(&dev->dev,
-						"isr & ISR_ERR [%x] ISR_XDROP\n", isr);
+						"isr & ISR_ERR [%x] ISR_XDROP\n",
+						isr);
 			} else {
 				dev_err(&dev->dev,
 						"isr & ISR_ERR [%x]\n",
@@ -2341,7 +2352,8 @@ static int slic_if_init(struct adapter *adapter)
 				 SLIC_GET_ADDR_LOW(&pshmem->isr), FLUSH);
 #else
 		slic_reg32_write(&slic_regs->slic_addr_upper, 0, DONT_FLUSH);
-		slic_reg32_write(&slic_regs->slic_isp, (u32)&pshmem->isr, FLUSH);
+		slic_reg32_write(&slic_regs->slic_isp, (u32)&pshmem->isr,
+				FLUSH);
 #endif
 		spin_unlock_irqrestore(&adapter->bit64reglock.lock,
 					adapter->bit64reglock.flags);
diff --git a/drivers/staging/speakup/kobjects.c b/drivers/staging/speakup/kobjects.c
index bcc7f62654f4..b12c76de60b0 100644
--- a/drivers/staging/speakup/kobjects.c
+++ b/drivers/staging/speakup/kobjects.c
@@ -81,7 +81,7 @@ static ssize_t chars_chartab_show(struct kobject *kobj,
 static void report_char_chartab_status(int reset, int received, int used,
 	int rejected, int do_characters)
 {
-	char *object_type[] = {
+	static char const *object_type[] = {
 		"character class entries",
 		"character descriptions",
 	};
@@ -809,10 +809,10 @@ static ssize_t message_store_helper(const char *buf, size_t count,
 			if (msg_stored == -ENOMEM)
 				reset = 1;
 			break;
-		} else {
-			used++;
 		}
 
+		used++;
+
 		cp = linefeed + 1;
 	}
 
diff --git a/drivers/staging/speakup/main.c b/drivers/staging/speakup/main.c
index 3f30a1b6e72c..e9f0c150d246 100644
--- a/drivers/staging/speakup/main.c
+++ b/drivers/staging/speakup/main.c
@@ -695,7 +695,7 @@ static void say_next_word(struct vc_data *vc)
 
 static void spell_word(struct vc_data *vc)
 {
-	static char *delay_str[] = { "", ",", ".", ". .", ". . ." };
+	static char const *delay_str[] = { "", ",", ".", ". .", ". . ." };
 	char *cp = buf, *str_cap = spk_str_caps_stop;
 	char *cp1, *last_cap = spk_str_caps_stop;
 	u_char ch;
diff --git a/drivers/staging/speakup/speakup_dtlk.c b/drivers/staging/speakup/speakup_dtlk.c
index d7d515273896..4e059ea78d4c 100644
--- a/drivers/staging/speakup/speakup_dtlk.c
+++ b/drivers/staging/speakup/speakup_dtlk.c
@@ -231,7 +231,7 @@ static void do_catch_up(struct spk_synth *synth)
 		if (ch == '\n')
 			ch = PROCSPEECH;
 		spk_out(ch);
-		if ((jiffies >= jiff_max) && (ch == SPACE)) {
+		if (time_after_eq(jiffies, jiff_max) && (ch == SPACE)) {
 			spk_out(PROCSPEECH);
 			spin_lock_irqsave(&speakup_info.spinlock, flags);
 			delay_time_val = delay_time->u.n.value;
diff --git a/drivers/staging/speakup/speakup_keypc.c b/drivers/staging/speakup/speakup_keypc.c
index 4ed38898a17a..cef20fdda646 100644
--- a/drivers/staging/speakup/speakup_keypc.c
+++ b/drivers/staging/speakup/speakup_keypc.c
@@ -229,7 +229,7 @@ spin_lock_irqsave(&speakup_info.spinlock, flags);
 			ch = PROCSPEECH;
 		outb_p(ch, synth_port);
 		SWAIT;
-		if ((jiffies >= jiff_max) && (ch == SPACE)) {
+		if (time_after_eq(jiffies, jiff_max) && (ch == SPACE)) {
 			timeout = 1000;
 			while (synth_writable())
 				if (--timeout <= 0)
diff --git a/drivers/staging/unisys/channels/channel.c b/drivers/staging/unisys/channels/channel.c
index b4bdee4b575b..74cc4d6b515f 100644
--- a/drivers/staging/unisys/channels/channel.c
+++ b/drivers/staging/unisys/channels/channel.c
@@ -43,45 +43,45 @@
  * Return value:
  * 1 if the insertion succeeds, 0 if the queue was full.
  */
-unsigned char
-visor_signal_insert(CHANNEL_HEADER __iomem *pChannel, u32 Queue, void *pSignal)
+unsigned char spar_signal_insert(struct channel_header __iomem *ch, u32 queue,
+				 void *sig)
 {
 	void __iomem *psignal;
 	unsigned int head, tail, nof;
 
-	SIGNAL_QUEUE_HEADER __iomem *pqhdr =
-	    (SIGNAL_QUEUE_HEADER __iomem *)
-		((char __iomem *) pChannel + readq(&pChannel->oChannelSpace))
-		+ Queue;
+	struct signal_queue_header __iomem *pqhdr =
+	    (struct signal_queue_header __iomem *)
+		((char __iomem *)ch + readq(&ch->ch_space_offset))
+		+ queue;
 
 	/* capture current head and tail */
-	head = readl(&pqhdr->Head);
-	tail = readl(&pqhdr->Tail);
+	head = readl(&pqhdr->head);
+	tail = readl(&pqhdr->tail);
 
 	/* queue is full if (head + 1) % n equals tail */
-	if (((head + 1) % readl(&pqhdr->MaxSignalSlots)) == tail) {
-		nof = readq(&pqhdr->NumOverflows) + 1;
-		writeq(nof, &pqhdr->NumOverflows);
+	if (((head + 1) % readl(&pqhdr->max_slots)) == tail) {
+		nof = readq(&pqhdr->num_overflows) + 1;
+		writeq(nof, &pqhdr->num_overflows);
 		return 0;
 	}
 
 	/* increment the head index */
-	head = (head + 1) % readl(&pqhdr->MaxSignalSlots);
+	head = (head + 1) % readl(&pqhdr->max_slots);
 
 	/* copy signal to the head location from the area pointed to
 	 * by pSignal
 	 */
-	psignal = (char __iomem *)pqhdr + readq(&pqhdr->oSignalBase) +
-		(head * readl(&pqhdr->SignalSize));
-	memcpy_toio(psignal, pSignal, readl(&pqhdr->SignalSize));
+	psignal = (char __iomem *)pqhdr + readq(&pqhdr->sig_base_offset) +
+		(head * readl(&pqhdr->signal_size));
+	memcpy_toio(psignal, sig, readl(&pqhdr->signal_size));
 
 	mb(); /* channel synch */
-	writel(head, &pqhdr->Head);
+	writel(head, &pqhdr->head);
 
-	writeq(readq(&pqhdr->NumSignalsSent) + 1, &pqhdr->NumSignalsSent);
+	writeq(readq(&pqhdr->num_sent) + 1, &pqhdr->num_sent);
 	return 1;
 }
-EXPORT_SYMBOL_GPL(visor_signal_insert);
+EXPORT_SYMBOL_GPL(spar_signal_insert);
 
 /*
  * Routine Description:
@@ -102,40 +102,40 @@ EXPORT_SYMBOL_GPL(visor_signal_insert);
  * 1 if the removal succeeds, 0 if the queue was empty.
  */
 unsigned char
-visor_signal_remove(CHANNEL_HEADER __iomem *pChannel, u32 Queue, void *pSignal)
+spar_signal_remove(struct channel_header __iomem *ch, u32 queue, void *sig)
 {
 	void __iomem *psource;
 	unsigned int head, tail;
-	SIGNAL_QUEUE_HEADER __iomem *pqhdr =
-	    (SIGNAL_QUEUE_HEADER __iomem *) ((char __iomem *) pChannel +
-				    readq(&pChannel->oChannelSpace)) + Queue;
+	struct signal_queue_header __iomem *pqhdr =
+	    (struct signal_queue_header __iomem *)((char __iomem *)ch +
+				    readq(&ch->ch_space_offset)) + queue;
 
 	/* capture current head and tail */
-	head = readl(&pqhdr->Head);
-	tail = readl(&pqhdr->Tail);
+	head = readl(&pqhdr->head);
+	tail = readl(&pqhdr->tail);
 
 	/* queue is empty if the head index equals the tail index */
 	if (head == tail) {
-		writeq(readq(&pqhdr->NumEmptyCnt) + 1, &pqhdr->NumEmptyCnt);
+		writeq(readq(&pqhdr->num_empty) + 1, &pqhdr->num_empty);
 		return 0;
 	}
 
 	/* advance past the 'empty' front slot */
-	tail = (tail + 1) % readl(&pqhdr->MaxSignalSlots);
+	tail = (tail + 1) % readl(&pqhdr->max_slots);
 
 	/* copy signal from tail location to the area pointed to by pSignal */
-	psource = (char __iomem *) pqhdr + readq(&pqhdr->oSignalBase) +
-		(tail * readl(&pqhdr->SignalSize));
-	memcpy_fromio(pSignal, psource, readl(&pqhdr->SignalSize));
+	psource = (char __iomem *)pqhdr + readq(&pqhdr->sig_base_offset) +
+		(tail * readl(&pqhdr->signal_size));
+	memcpy_fromio(sig, psource, readl(&pqhdr->signal_size));
 
 	mb(); /* channel synch */
-	writel(tail, &pqhdr->Tail);
+	writel(tail, &pqhdr->tail);
 
-	writeq(readq(&pqhdr->NumSignalsReceived) + 1,
-	       &pqhdr->NumSignalsReceived);
+	writeq(readq(&pqhdr->num_received) + 1,
+	       &pqhdr->num_received);
 	return 1;
 }
-EXPORT_SYMBOL_GPL(visor_signal_remove);
+EXPORT_SYMBOL_GPL(spar_signal_remove);
 
 /*
  * Routine Description:
@@ -156,18 +156,18 @@ EXPORT_SYMBOL_GPL(visor_signal_remove);
  * Return value:
  * # of signals copied.
  */
-unsigned int
-SignalRemoveAll(pCHANNEL_HEADER pChannel, u32 Queue, void *pSignal)
+unsigned int spar_signal_remove_all(struct channel_header *ch, u32 queue,
+				    void *sig)
 {
 	void *psource;
-	unsigned int head, tail, signalCount = 0;
-	pSIGNAL_QUEUE_HEADER pqhdr =
-	    (pSIGNAL_QUEUE_HEADER) ((char *) pChannel +
-				    pChannel->oChannelSpace) + Queue;
+	unsigned int head, tail, count = 0;
+	struct signal_queue_header *pqhdr =
+	    (struct signal_queue_header *)((char *)ch +
+				    ch->ch_space_offset) + queue;
 
 	/* capture current head and tail */
-	head = pqhdr->Head;
-	tail = pqhdr->Tail;
+	head = pqhdr->head;
+	tail = pqhdr->tail;
 
 	/* queue is empty if the head index equals the tail index */
 	if (head == tail)
@@ -175,25 +175,25 @@ SignalRemoveAll(pCHANNEL_HEADER pChannel, u32 Queue, void *pSignal)
 
 	while (head != tail) {
 		/* advance past the 'empty' front slot */
-		tail = (tail + 1) % pqhdr->MaxSignalSlots;
+		tail = (tail + 1) % pqhdr->max_slots;
 
 		/* copy signal from tail location to the area pointed
 		 * to by pSignal
 		 */
 		psource =
-		    (char *) pqhdr + pqhdr->oSignalBase +
-		    (tail * pqhdr->SignalSize);
-		memcpy((char *) pSignal + (pqhdr->SignalSize * signalCount),
-		       psource, pqhdr->SignalSize);
+		    (char *)pqhdr + pqhdr->sig_base_offset +
+		    (tail * pqhdr->signal_size);
+		memcpy((char *)sig + (pqhdr->signal_size * count),
+		       psource, pqhdr->signal_size);
 
 		mb(); /* channel synch */
-		pqhdr->Tail = tail;
+		pqhdr->tail = tail;
 
-		signalCount++;
-		pqhdr->NumSignalsReceived++;
+		count++;
+		pqhdr->num_received++;
 	}
 
-	return signalCount;
+	return count;
 }
 
 /*
@@ -207,13 +207,13 @@ SignalRemoveAll(pCHANNEL_HEADER pChannel, u32 Queue, void *pSignal)
  * Return value:
  * 1 if the signal queue is empty, 0 otherwise.
  */
-unsigned char
-visor_signalqueue_empty(CHANNEL_HEADER __iomem *pChannel, u32 Queue)
+unsigned char spar_signalqueue_empty(struct channel_header __iomem *ch,
+				     u32 queue)
 {
-	SIGNAL_QUEUE_HEADER __iomem *pqhdr =
-	    (SIGNAL_QUEUE_HEADER __iomem *) ((char __iomem *) pChannel +
-				    readq(&pChannel->oChannelSpace)) + Queue;
-	return readl(&pqhdr->Head) == readl(&pqhdr->Tail);
+	struct signal_queue_header __iomem *pqhdr =
+	    (struct signal_queue_header __iomem *)((char __iomem *)ch +
+				    readq(&ch->ch_space_offset)) + queue;
+	return readl(&pqhdr->head) == readl(&pqhdr->tail);
 }
-EXPORT_SYMBOL_GPL(visor_signalqueue_empty);
+EXPORT_SYMBOL_GPL(spar_signalqueue_empty);
 
diff --git a/drivers/staging/unisys/channels/chanstub.c b/drivers/staging/unisys/channels/chanstub.c
index d54c5d635a94..b6fd126f16f1 100644
--- a/drivers/staging/unisys/channels/chanstub.c
+++ b/drivers/staging/unisys/channels/chanstub.c
@@ -42,26 +42,26 @@ channel_mod_exit(void)
 }
 
 unsigned char
-SignalInsert_withLock(CHANNEL_HEADER __iomem *pChannel, u32 Queue,
+SignalInsert_withLock(struct channel_header __iomem *pChannel, u32 Queue,
 		      void *pSignal, spinlock_t *lock)
 {
 	unsigned char result;
 	unsigned long flags;
 
 	spin_lock_irqsave(lock, flags);
-	result = visor_signal_insert(pChannel, Queue, pSignal);
+	result = spar_signal_insert(pChannel, Queue, pSignal);
 	spin_unlock_irqrestore(lock, flags);
 	return result;
 }
 
 unsigned char
-SignalRemove_withLock(CHANNEL_HEADER __iomem *pChannel, u32 Queue,
+SignalRemove_withLock(struct channel_header __iomem *pChannel, u32 Queue,
 		      void *pSignal, spinlock_t *lock)
 {
 	unsigned char result;
 
 	spin_lock(lock);
-	result = visor_signal_remove(pChannel, Queue, pSignal);
+	result = spar_signal_remove(pChannel, Queue, pSignal);
 	spin_unlock(lock);
 	return result;
 }
diff --git a/drivers/staging/unisys/channels/chanstub.h b/drivers/staging/unisys/channels/chanstub.h
index d08e2c69d2ad..1531759a1b31 100644
--- a/drivers/staging/unisys/channels/chanstub.h
+++ b/drivers/staging/unisys/channels/chanstub.h
@@ -15,9 +15,9 @@
 
 #ifndef __CHANSTUB_H__
 #define __CHANSTUB_H__
-unsigned char SignalInsert_withLock(CHANNEL_HEADER __iomem *pChannel, u32 Queue,
-				     void *pSignal, spinlock_t *lock);
-unsigned char SignalRemove_withLock(CHANNEL_HEADER __iomem *pChannel, u32 Queue,
-				     void *pSignal, spinlock_t *lock);
+unsigned char SignalInsert_withLock(struct channel_header __iomem *pChannel,
+				    u32 Queue, void *pSignal, spinlock_t *lock);
+unsigned char SignalRemove_withLock(struct channel_header __iomem *pChannel,
+				    u32 Queue, void *pSignal, spinlock_t *lock);
 
 #endif
diff --git a/drivers/staging/unisys/common-spar/include/channels/channel.h b/drivers/staging/unisys/common-spar/include/channels/channel.h
index c25dfbf7f6bc..6fb6e5b3ddaf 100644
--- a/drivers/staging/unisys/common-spar/include/channels/channel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/channel.h
@@ -50,43 +50,12 @@
 
 #define ULTRA_CHANNEL_PROTOCOL_SIGNATURE  SIGNATURE_32('E', 'C', 'N', 'L')
 
-#define CHANNEL_GUID_MISMATCH(chType, chName, field, expected, actual, fil, \
-			      lin, logCtx)				\
-	do {								\
-		pr_err("Channel mismatch on channel=%s(%pUL) field=%s expected=%pUL actual=%pUL @%s:%d\n", \
-		       chName, &chType, field,	\
-		       &expected, &actual, \
-		       fil, lin);					\
-	} while (0)
-#define CHANNEL_U32_MISMATCH(chType, chName, field, expected, actual, fil, \
-			     lin, logCtx)				\
-	do {								\
-		pr_err("Channel mismatch on channel=%s(%pUL) field=%s expected=0x%-8.8lx actual=0x%-8.8lx @%s:%d\n", \
-		       chName, &chType, field,	\
-		       (unsigned long)expected, (unsigned long)actual,	\
-		       fil, lin);					\
-	} while (0)
-
-#define CHANNEL_U64_MISMATCH(chType, chName, field, expected, actual, fil, \
-			     lin, logCtx)				\
-	do {								\
-		pr_err("Channel mismatch on channel=%s(%pUL) field=%s expected=0x%-8.8Lx actual=0x%-8.8Lx @%s:%d\n", \
-		       chName, &chType, field,	\
-		       (unsigned long long)expected,			\
-		       (unsigned long long)actual,			\
-		       fil, lin);					\
-	} while (0)
-
-#define UltraLogEvent(logCtx, EventId, Severity, SubsystemMask, pFunctionName, \
-		      LineNumber, Str, args...)				\
-	pr_info(Str, ## args)
-
-typedef enum {
+enum channel_serverstate {
 	CHANNELSRV_UNINITIALIZED = 0,	/* channel is in an undefined state */
 	CHANNELSRV_READY = 1	/* channel has been initialized by server */
-} CHANNEL_SERVERSTATE;
+};
 
-typedef enum {
+enum channel_clientstate {
 	CHANNELCLI_DETACHED = 0,
 	CHANNELCLI_DISABLED = 1,	/* client can see channel but is NOT
 					 * allowed to use it unless given TBD
@@ -100,32 +69,32 @@ typedef enum {
 				 * using channel */
 	CHANNELCLI_OWNED = 5	/* "no worries" state - client can
 				 * access channel anytime */
-} CHANNEL_CLIENTSTATE;
+};
+
 static inline const u8 *
 ULTRA_CHANNELCLI_STRING(u32 v)
 {
 	switch (v) {
 	case CHANNELCLI_DETACHED:
-		return (const u8 *) ("DETACHED");
+		return (const u8 *)("DETACHED");
 	case CHANNELCLI_DISABLED:
-		return (const u8 *) ("DISABLED");
+		return (const u8 *)("DISABLED");
 	case CHANNELCLI_ATTACHING:
-		return (const u8 *) ("ATTACHING");
+		return (const u8 *)("ATTACHING");
 	case CHANNELCLI_ATTACHED:
-		return (const u8 *) ("ATTACHED");
+		return (const u8 *)("ATTACHED");
 	case CHANNELCLI_BUSY:
-		return (const u8 *) ("BUSY");
+		return (const u8 *)("BUSY");
 	case CHANNELCLI_OWNED:
-		return (const u8 *) ("OWNED");
+		return (const u8 *)("OWNED");
 	default:
 		break;
 	}
-	return (const u8 *) ("?");
+	return (const u8 *)("?");
 }
 
-#define ULTRA_CHANNELSRV_IS_READY(x)     ((x) == CHANNELSRV_READY)
-#define ULTRA_CHANNEL_SERVER_READY(pChannel) \
-	(ULTRA_CHANNELSRV_IS_READY(readl(&(pChannel)->SrvState)))
+#define SPAR_CHANNEL_SERVER_READY(ch) \
+	(readl(&(ch)->srv_state) == CHANNELSRV_READY)
 
 #define ULTRA_VALID_CHANNELCLI_TRANSITION(o, n)				\
 	(((((o) == CHANNELCLI_DETACHED) && ((n) == CHANNELCLI_DISABLED)) || \
@@ -145,60 +114,41 @@ ULTRA_CHANNELCLI_STRING(u32 v)
 	  (((o) == CHANNELCLI_BUSY) && ((n) == CHANNELCLI_OWNED)) || (0)) \
 	 ? (1) : (0))
 
-#define ULTRA_CHANNEL_CLIENT_CHK_TRANSITION(old, new, chanId, logCtx,	\
+#define SPAR_CHANNEL_CLIENT_CHK_TRANSITION(old, new, id, log,	\
 					    file, line)			\
 	do {								\
 		if (!ULTRA_VALID_CHANNELCLI_TRANSITION(old, new))	\
-			UltraLogEvent(logCtx,				\
-				      CHANNELSTATE_DIAG_EVENTID_TRANSITERR, \
-				      CHANNELSTATE_DIAG_SEVERITY, \
-				      CHANNELSTATE_DIAG_SUBSYS,		\
-				      __func__, __LINE__,		\
-				      "%s Channel StateTransition INVALID! (%s) %s(%d)-->%s(%d) @%s:%d\n", \
-				      chanId, "CliState<x>",		\
-				      ULTRA_CHANNELCLI_STRING(old),	\
-				      old,				\
-				      ULTRA_CHANNELCLI_STRING(new),	\
-				      new,				\
-				      PathName_Last_N_Nodes((u8 *)file, 4), \
-				      line);				\
+			pr_info("%s Channel StateTransition INVALID! (%s) %s(%d)-->%s(%d) @%s:%d\n", \
+				id, "CliState<x>",		\
+				ULTRA_CHANNELCLI_STRING(old),	\
+				old,				\
+				ULTRA_CHANNELCLI_STRING(new),	\
+				new,				\
+				pathname_last_n_nodes((u8 *)file, 4), \
+				line);				\
 	} while (0)
 
-#define ULTRA_CHANNEL_CLIENT_TRANSITION(pChan, chanId,			\
-					newstate, logCtx)		\
+#define SPAR_CHANNEL_CLIENT_TRANSITION(ch, id, newstate, log)		\
 	do {								\
-		ULTRA_CHANNEL_CLIENT_CHK_TRANSITION(			\
-			readl(&(((CHANNEL_HEADER __iomem *) \
-				 (pChan))->CliStateOS)),		\
-			newstate,					\
-			chanId, logCtx, __FILE__, __LINE__);		\
-		UltraLogEvent(logCtx, CHANNELSTATE_DIAG_EVENTID_TRANSITOK, \
-			CHANNELSTATE_DIAG_SEVERITY, \
-			      CHANNELSTATE_DIAG_SUBSYS,			\
-			      __func__, __LINE__,			\
-			      "%s Channel StateTransition (%s) %s(%d)-->%s(%d) @%s:%d\n", \
-			      chanId, "CliStateOS",			\
-			      ULTRA_CHANNELCLI_STRING( \
-				      readl(&((CHANNEL_HEADER __iomem *) \
-					      (pChan))->CliStateOS)),	\
-			      readl(&((CHANNEL_HEADER __iomem *) \
-				      (pChan))->CliStateOS),		\
-			      ULTRA_CHANNELCLI_STRING(newstate),	\
-			      newstate,					\
-			      PathName_Last_N_Nodes(__FILE__, 4), __LINE__); \
-		writel(newstate, &((CHANNEL_HEADER __iomem *) \
-				   (pChan))->CliStateOS);		\
+		SPAR_CHANNEL_CLIENT_CHK_TRANSITION(			\
+			readl(&(((struct channel_header __iomem *)\
+				 (ch))->cli_state_os)),		\
+			newstate, id, log, __FILE__, __LINE__);		\
+			pr_info("%s Channel StateTransition (%s) %s(%d)-->%s(%d) @%s:%d\n", \
+				id, "CliStateOS",			\
+				ULTRA_CHANNELCLI_STRING( \
+				      readl(&((struct channel_header __iomem *)\
+					      (ch))->cli_state_os)),	\
+				readl(&((struct channel_header __iomem *)\
+				      (ch))->cli_state_os),		\
+				ULTRA_CHANNELCLI_STRING(newstate),	\
+				newstate,				\
+				pathname_last_n_nodes(__FILE__, 4), __LINE__); \
+		writel(newstate, &((struct channel_header __iomem *)\
+				   (ch))->cli_state_os);		\
 		mb(); /* required for channel synch */			\
 	} while (0)
 
-#define ULTRA_CHANNEL_CLIENT_ACQUIRE_OS(pChan, chanId, logCtx)	\
-	ULTRA_channel_client_acquire_os(pChan, chanId, logCtx,		\
-					(char *)__FILE__, __LINE__,	\
-					(char *)__func__)
-#define ULTRA_CHANNEL_CLIENT_RELEASE_OS(pChan, chanId, logCtx)	\
-	ULTRA_channel_client_release_os(pChan, chanId, logCtx,	\
-		(char *)__FILE__, __LINE__, (char *)__func__)
-
 /* Values for ULTRA_CHANNEL_PROTOCOL.CliErrorBoot: */
 /* throttling invalid boot channel statetransition error due to client
  * disabled */
@@ -239,98 +189,98 @@ ULTRA_CHANNELCLI_STRING(u32 v)
 
 #pragma pack(push, 1)		/* both GCC and VC now allow this pragma */
 /* Common Channel Header */
-typedef struct _CHANNEL_HEADER {
-	u64 Signature;		/* Signature */
-	u32 LegacyState;	/* DEPRECATED - being replaced by */
+struct channel_header {
+	u64 signature;		/* Signature */
+	u32 legacy_state;	/* DEPRECATED - being replaced by */
 	/* /              SrvState, CliStateBoot, and CliStateOS below */
-	u32 HeaderSize;		/* sizeof(CHANNEL_HEADER) */
-	u64 Size;		/* Total size of this channel in bytes */
-	u64 Features;		/* Flags to modify behavior */
-	uuid_le Type;		/* Channel type: data, bus, control, etc. */
-	u64 PartitionHandle;	/* ID of guest partition */
-	u64 Handle;		/* Device number of this channel in client */
-	u64 oChannelSpace;	/* Offset in bytes to channel specific area */
-	u32 VersionId;		/* CHANNEL_HEADER Version ID */
-	u32 PartitionIndex;	/* Index of guest partition */
-	uuid_le ZoneGuid;		/* Guid of Channel's zone */
-	u32 oClientString;	/* offset from channel header to
+	u32 header_size;	/* sizeof(struct channel_header) */
+	u64 size;		/* Total size of this channel in bytes */
+	u64 features;		/* Flags to modify behavior */
+	uuid_le chtype;		/* Channel type: data, bus, control, etc. */
+	u64 partition_handle;	/* ID of guest partition */
+	u64 handle;		/* Device number of this channel in client */
+	u64 ch_space_offset;	/* Offset in bytes to channel specific area */
+	u32 version_id;		/* struct channel_header Version ID */
+	u32 partition_index;	/* Index of guest partition */
+	uuid_le zone_uuid;	/* Guid of Channel's zone */
+	u32 cli_str_offset;	/* offset from channel header to
 				 * nul-terminated ClientString (0 if
 				 * ClientString not present) */
-	u32 CliStateBoot;	/* CHANNEL_CLIENTSTATE of pre-boot
+	u32 cli_state_boot;	/* CHANNEL_CLIENTSTATE of pre-boot
 				 * EFI client of this channel */
-	u32 CmdStateCli;	/* CHANNEL_COMMANDSTATE (overloaded in
+	u32 cmd_state_cli;	/* CHANNEL_COMMANDSTATE (overloaded in
 				 * Windows drivers, see ServerStateUp,
 				 * ServerStateDown, etc) */
-	u32 CliStateOS;		/* CHANNEL_CLIENTSTATE of Guest OS
+	u32 cli_state_os;	/* CHANNEL_CLIENTSTATE of Guest OS
 				 * client of this channel */
-	u32 ChannelCharacteristics;	/* CHANNEL_CHARACTERISTIC_<xxx> */
-	u32 CmdStateSrv;	/* CHANNEL_COMMANDSTATE (overloaded in
+	u32 ch_characteristic;	/* CHANNEL_CHARACTERISTIC_<xxx> */
+	u32 cmd_state_srv;	/* CHANNEL_COMMANDSTATE (overloaded in
 				 * Windows drivers, see ServerStateUp,
 				 * ServerStateDown, etc) */
-	u32 SrvState;		/* CHANNEL_SERVERSTATE */
-	u8 CliErrorBoot;	/* bits to indicate err states for
+	u32 srv_state;		/* CHANNEL_SERVERSTATE */
+	u8 cli_error_boot;	/* bits to indicate err states for
 				 * boot clients, so err messages can
 				 * be throttled */
-	u8 CliErrorOS;		/* bits to indicate err states for OS
+	u8 cli_error_os;	/* bits to indicate err states for OS
 				 * clients, so err messages can be
 				 * throttled */
-	u8 Filler[1];		/* Pad out to 128 byte cacheline */
+	u8 filler[1];		/* Pad out to 128 byte cacheline */
 	/* Please add all new single-byte values below here */
-	u8 RecoverChannel;
-} CHANNEL_HEADER, *pCHANNEL_HEADER, ULTRA_CHANNEL_PROTOCOL;
+	u8 recover_channel;
+};
 
 #define ULTRA_CHANNEL_ENABLE_INTS (0x1ULL << 0)
 
 /* Subheader for the Signal Type variation of the Common Channel */
-typedef struct _SIGNAL_QUEUE_HEADER {
+struct signal_queue_header {
 	/* 1st cache line */
-	u32 VersionId;		/* SIGNAL_QUEUE_HEADER Version ID */
-	u32 Type;		/* Queue type: storage, network */
-	u64 Size;		/* Total size of this queue in bytes */
-	u64 oSignalBase;	/* Offset to signal queue area */
-	u64 FeatureFlags;	/* Flags to modify behavior */
-	u64 NumSignalsSent;	/* Total # of signals placed in this queue */
-	u64 NumOverflows;	/* Total # of inserts failed due to
+	u32 version;		/* SIGNAL_QUEUE_HEADER Version ID */
+	u32 chtype;		/* Queue type: storage, network */
+	u64 size;		/* Total size of this queue in bytes */
+	u64 sig_base_offset;	/* Offset to signal queue area */
+	u64 features;		/* Flags to modify behavior */
+	u64 num_sent;		/* Total # of signals placed in this queue */
+	u64 num_overflows;	/* Total # of inserts failed due to
 				 * full queue */
-	u32 SignalSize;		/* Total size of a signal for this queue */
-	u32 MaxSignalSlots;	/* Max # of slots in queue, 1 slot is
+	u32 signal_size;	/* Total size of a signal for this queue */
+	u32 max_slots;		/* Max # of slots in queue, 1 slot is
 				 * always empty */
-	u32 MaxSignals;		/* Max # of signals in queue
+	u32 max_signals;	/* Max # of signals in queue
 				 * (MaxSignalSlots-1) */
-	u32 Head;		/* Queue head signal # */
+	u32 head;		/* Queue head signal # */
 	/* 2nd cache line */
-	u64 NumSignalsReceived;	/* Total # of signals removed from this queue */
-	u32 Tail;		/* Queue tail signal # (on separate
+	u64 num_received;	/* Total # of signals removed from this queue */
+	u32 tail;		/* Queue tail signal # (on separate
 				 * cache line) */
-	u32 Reserved1;		/* Reserved field */
-	u64 Reserved2;		/* Resrved field */
-	u64 ClientQueue;
-	u64 NumInterruptsReceived;	/* Total # of Interrupts received.  This
+	u32 reserved1;		/* Reserved field */
+	u64 reserved2;		/* Reserved field */
+	u64 client_queue;
+	u64 num_irq_received;	/* Total # of Interrupts received.  This
 					 * is incremented by the ISR in the
 					 * guest windows driver */
-	u64 NumEmptyCnt;	/* Number of times that visor_signal_remove
+	u64 num_empty;		/* Number of times that visor_signal_remove
 				 * is called and returned Empty
 				 * Status. */
-	u32 ErrorFlags;		/* Error bits set during SignalReinit
+	u32 errorflags;		/* Error bits set during SignalReinit
 				 * to denote trouble with client's
 				 * fields */
-	u8 Filler[12];		/* Pad out to 64 byte cacheline */
-} SIGNAL_QUEUE_HEADER, *pSIGNAL_QUEUE_HEADER;
+	u8 filler[12];		/* Pad out to 64 byte cacheline */
+};
 
 #pragma pack(pop)
 
-#define SignalInit(chan, QHDRFLD, QDATAFLD, QDATATYPE, ver, typ)	\
+#define spar_signal_init(chan, QHDRFLD, QDATAFLD, QDATATYPE, ver, typ)	\
 	do {								\
 		memset(&chan->QHDRFLD, 0, sizeof(chan->QHDRFLD));	\
-		chan->QHDRFLD.VersionId = ver;				\
-		chan->QHDRFLD.Type = typ;				\
-		chan->QHDRFLD.Size = sizeof(chan->QDATAFLD);		\
-		chan->QHDRFLD.SignalSize = sizeof(QDATATYPE);		\
-		chan->QHDRFLD.oSignalBase = (u64)(chan->QDATAFLD)-	\
+		chan->QHDRFLD.version = ver;				\
+		chan->QHDRFLD.chtype = typ;				\
+		chan->QHDRFLD.size = sizeof(chan->QDATAFLD);		\
+		chan->QHDRFLD.signal_size = sizeof(QDATATYPE);		\
+		chan->QHDRFLD.sig_base_offset = (u64)(chan->QDATAFLD)-	\
 			(u64)(&chan->QHDRFLD);				\
-		chan->QHDRFLD.MaxSignalSlots =				\
+		chan->QHDRFLD.max_slots =				\
 			sizeof(chan->QDATAFLD)/sizeof(QDATATYPE);	\
-		chan->QHDRFLD.MaxSignals = chan->QHDRFLD.MaxSignalSlots-1; \
+		chan->QHDRFLD.max_signals = chan->QHDRFLD.max_slots-1;	\
 	} while (0)
 
 /* Generic function useful for validating any type of channel when it is
@@ -339,64 +289,62 @@ typedef struct _SIGNAL_QUEUE_HEADER {
  * is used to pass the EFI_DIAG_CAPTURE_PROTOCOL needed to log messages.
  */
 static inline int
-ULTRA_check_channel_client(void __iomem *pChannel,
-			   uuid_le expectedTypeGuid,
-			   char *channelName,
-			   u64 expectedMinBytes,
-			   u32 expectedVersionId,
-			   u64 expectedSignature,
-			   char *fileName, int lineNumber, void *logCtx)
+spar_check_channel_client(void __iomem *ch,
+			  uuid_le expected_uuid,
+			  char *chname,
+			  u64 expected_min_bytes,
+			  u32 expected_version,
+			  u64 expected_signature)
 {
-	if (uuid_le_cmp(expectedTypeGuid, NULL_UUID_LE) != 0) {
+	if (uuid_le_cmp(expected_uuid, NULL_UUID_LE) != 0) {
 		uuid_le guid;
 
 		memcpy_fromio(&guid,
-			      &((CHANNEL_HEADER __iomem *)(pChannel))->Type,
+			      &((struct channel_header __iomem *)(ch))->chtype,
 			      sizeof(guid));
 		/* caller wants us to verify type GUID */
-		if (uuid_le_cmp(guid, expectedTypeGuid) != 0) {
-			CHANNEL_GUID_MISMATCH(expectedTypeGuid, channelName,
-					      "type", expectedTypeGuid,
-					      guid, fileName,
-					      lineNumber, logCtx);
+		if (uuid_le_cmp(guid, expected_uuid) != 0) {
+			pr_err("Channel mismatch on channel=%s(%pUL) field=type expected=%pUL actual=%pUL\n",
+			       chname, &expected_uuid,
+			       &expected_uuid, &guid);
 			return 0;
 		}
 	}
-	if (expectedMinBytes > 0)	/* caller wants us to verify
+	if (expected_min_bytes > 0) {	/* caller wants us to verify
 					 * channel size */
-		if (readq(&((CHANNEL_HEADER __iomem *)
-			   (pChannel))->Size) < expectedMinBytes) {
-			CHANNEL_U64_MISMATCH(expectedTypeGuid, channelName,
-					     "size", expectedMinBytes,
-					     readq(&((CHANNEL_HEADER __iomem *)
-						     (pChannel))->Size),
-					     fileName,
-					     lineNumber, logCtx);
+		unsigned long long bytes =
+				readq(&((struct channel_header __iomem *)
+					(ch))->size);
+		if (bytes < expected_min_bytes) {
+			pr_err("Channel mismatch on channel=%s(%pUL) field=size expected=0x%-8.8Lx actual=0x%-8.8Lx\n",
+			       chname, &expected_uuid,
+			       (unsigned long long)expected_min_bytes, bytes);
 			return 0;
 		}
-	if (expectedVersionId > 0)	/* caller wants us to verify
+	}
+	if (expected_version > 0) {	/* caller wants us to verify
 					 * channel version */
-		if (readl(&((CHANNEL_HEADER __iomem *) (pChannel))->VersionId)
-		    != expectedVersionId) {
-			CHANNEL_U32_MISMATCH(expectedTypeGuid, channelName,
-					     "version", expectedVersionId,
-					     readl(&((CHANNEL_HEADER __iomem *)
-						     (pChannel))->VersionId),
-					     fileName, lineNumber, logCtx);
+		unsigned long ver = readl(&((struct channel_header __iomem *)
+				    (ch))->version_id);
+		if (ver != expected_version) {
+			pr_err("Channel mismatch on channel=%s(%pUL) field=version expected=0x%-8.8lx actual=0x%-8.8lx\n",
+			       chname, &expected_uuid,
+			       (unsigned long)expected_version, ver);
 			return 0;
 		}
-	if (expectedSignature > 0)	/* caller wants us to verify
+	}
+	if (expected_signature > 0) {	/* caller wants us to verify
 					 * channel signature */
-		if (readq(&((CHANNEL_HEADER __iomem *) (pChannel))->Signature)
-		    != expectedSignature) {
-			CHANNEL_U64_MISMATCH(expectedTypeGuid, channelName,
-					     "signature", expectedSignature,
-					     readq(&((CHANNEL_HEADER __iomem *)
-						     (pChannel))->Signature),
-					     fileName,
-					     lineNumber, logCtx);
+		unsigned long long sig =
+				readq(&((struct channel_header __iomem *)
+					(ch))->signature);
+		if (sig != expected_signature) {
+			pr_err("Channel mismatch on channel=%s(%pUL) field=signature expected=0x%-8.8llx actual=0x%-8.8llx\n",
+			       chname, &expected_uuid,
+			       expected_signature, sig);
 			return 0;
 		}
+	}
 	return 1;
 }
 
@@ -405,19 +353,16 @@ ULTRA_check_channel_client(void __iomem *pChannel,
  * Note that <logCtx> is only needed for callers in the EFI environment, and
  * is used to pass the EFI_DIAG_CAPTURE_PROTOCOL needed to log messages.
  */
-static inline int
-ULTRA_check_channel_server(uuid_le typeGuid,
-			   char *channelName,
-			   u64 expectedMinBytes,
-			   u64 actualBytes,
-			   char *fileName, int lineNumber, void *logCtx)
+static inline int spar_check_channel_server(uuid_le typeuuid, char *name,
+					    u64 expected_min_bytes,
+					    u64 actual_bytes)
 {
-	if (expectedMinBytes > 0)	/* caller wants us to verify
+	if (expected_min_bytes > 0)	/* caller wants us to verify
 					 * channel size */
-		if (actualBytes < expectedMinBytes) {
-			CHANNEL_U64_MISMATCH(typeGuid, channelName, "size",
-					     expectedMinBytes, actualBytes,
-					     fileName, lineNumber, logCtx);
+		if (actual_bytes < expected_min_bytes) {
+			pr_err("Channel mismatch on channel=%s(%pUL) field=size expected=0x%-8.8llx actual=0x%-8.8llx\n",
+			       name, &typeuuid, expected_min_bytes,
+			       actual_bytes);
 			return 0;
 		}
 	return 1;
@@ -430,7 +375,7 @@ ULTRA_check_channel_server(uuid_le typeGuid,
  * in it, the return pointer will be to the beginning of the string.
  */
 static inline u8 *
-PathName_Last_N_Nodes(u8 *s, unsigned int n)
+pathname_last_n_nodes(u8 *s, unsigned int n)
 {
 	u8 *p = s;
 	unsigned int node_count = 0;
@@ -455,59 +400,43 @@ PathName_Last_N_Nodes(u8 *s, unsigned int n)
 }
 
 static inline int
-ULTRA_channel_client_acquire_os(void __iomem *pChannel, u8 *chanId,
-				void *logCtx, char *file, int line, char *func)
+spar_channel_client_acquire_os(void __iomem *ch, u8 *id)
 {
-	CHANNEL_HEADER __iomem *pChan = pChannel;
+	struct channel_header __iomem *hdr = ch;
 
-	if (readl(&pChan->CliStateOS) == CHANNELCLI_DISABLED) {
-		if ((readb(&pChan->CliErrorOS)
+	if (readl(&hdr->cli_state_os) == CHANNELCLI_DISABLED) {
+		if ((readb(&hdr->cli_error_os)
 		     & ULTRA_CLIERROROS_THROTTLEMSG_DISABLED) == 0) {
 			/* we are NOT throttling this message */
-			writeb(readb(&pChan->CliErrorOS) |
+			writeb(readb(&hdr->cli_error_os) |
 			       ULTRA_CLIERROROS_THROTTLEMSG_DISABLED,
-			       &pChan->CliErrorOS);
+			       &hdr->cli_error_os);
 			/* throttle until acquire successful */
 
-			UltraLogEvent(logCtx,
-				      CHANNELSTATE_DIAG_EVENTID_TRANSITERR,
-				      CHANNELSTATE_DIAG_SEVERITY,
-				      CHANNELSTATE_DIAG_SUBSYS, func, line,
-				      "%s Channel StateTransition INVALID! - acquire failed because OS client DISABLED @%s:%d\n",
-				      chanId, PathName_Last_N_Nodes(
-					      (u8 *) file, 4), line);
+			pr_info("%s Channel StateTransition INVALID! - acquire failed because OS client DISABLED\n",
+				id);
 		}
 		return 0;
 	}
-	if ((readl(&pChan->CliStateOS) != CHANNELCLI_OWNED)
-	    && (readl(&pChan->CliStateBoot) == CHANNELCLI_DISABLED)) {
+	if ((readl(&hdr->cli_state_os) != CHANNELCLI_OWNED) &&
+	    (readl(&hdr->cli_state_boot) == CHANNELCLI_DISABLED)) {
 		/* Our competitor is DISABLED, so we can transition to OWNED */
-		UltraLogEvent(logCtx, CHANNELSTATE_DIAG_EVENTID_TRANSITOK,
-			      CHANNELSTATE_DIAG_SEVERITY,
-			      CHANNELSTATE_DIAG_SUBSYS, func, line,
-			      "%s Channel StateTransition (%s) %s(%d)-->%s(%d) @%s:%d\n",
-			      chanId, "CliStateOS",
-			      ULTRA_CHANNELCLI_STRING(
-				      readl(&pChan->CliStateOS)),
-			      readl(&pChan->CliStateOS),
-			      ULTRA_CHANNELCLI_STRING(CHANNELCLI_OWNED),
-			      CHANNELCLI_OWNED,
-			      PathName_Last_N_Nodes((u8 *) file, 4), line);
-		writel(CHANNELCLI_OWNED, &pChan->CliStateOS);
+		pr_info("%s Channel StateTransition (%s) %s(%d)-->%s(%d)\n",
+			id, "cli_state_os",
+			ULTRA_CHANNELCLI_STRING(readl(&hdr->cli_state_os)),
+			readl(&hdr->cli_state_os),
+			ULTRA_CHANNELCLI_STRING(CHANNELCLI_OWNED),
+			CHANNELCLI_OWNED);
+		writel(CHANNELCLI_OWNED, &hdr->cli_state_os);
 		mb(); /* required for channel synch */
 	}
-	if (readl(&pChan->CliStateOS) == CHANNELCLI_OWNED) {
-		if (readb(&pChan->CliErrorOS) != 0) {
+	if (readl(&hdr->cli_state_os) == CHANNELCLI_OWNED) {
+		if (readb(&hdr->cli_error_os) != 0) {
 			/* we are in an error msg throttling state;
 			 * come out of it */
-			UltraLogEvent(logCtx,
-				      CHANNELSTATE_DIAG_EVENTID_TRANSITOK,
-				      CHANNELSTATE_DIAG_SEVERITY,
-				      CHANNELSTATE_DIAG_SUBSYS, func, line,
-				      "%s Channel OS client acquire now successful @%s:%d\n",
-				      chanId, PathName_Last_N_Nodes((u8 *) file,
-								    4), line);
-			writeb(0, &pChan->CliErrorOS);
+			pr_info("%s Channel OS client acquire now successful\n",
+				id);
+			writeb(0, &hdr->cli_error_os);
 		}
 		return 1;
 	}
@@ -515,95 +444,67 @@ ULTRA_channel_client_acquire_os(void __iomem *pChannel, u8 *chanId,
 	/* We have to do it the "hard way".  We transition to BUSY,
 	* and can use the channel iff our competitor has not also
 	* transitioned to BUSY. */
-	if (readl(&pChan->CliStateOS) != CHANNELCLI_ATTACHED) {
-		if ((readb(&pChan->CliErrorOS)
+	if (readl(&hdr->cli_state_os) != CHANNELCLI_ATTACHED) {
+		if ((readb(&hdr->cli_error_os)
 		     & ULTRA_CLIERROROS_THROTTLEMSG_NOTATTACHED) == 0) {
 			/* we are NOT throttling this message */
-			writeb(readb(&pChan->CliErrorOS) |
+			writeb(readb(&hdr->cli_error_os) |
 			       ULTRA_CLIERROROS_THROTTLEMSG_NOTATTACHED,
-			       &pChan->CliErrorOS);
+			       &hdr->cli_error_os);
 			/* throttle until acquire successful */
-			UltraLogEvent(logCtx,
-				      CHANNELSTATE_DIAG_EVENTID_TRANSITERR,
-				      CHANNELSTATE_DIAG_SEVERITY,
-				      CHANNELSTATE_DIAG_SUBSYS, func, line,
-				      "%s Channel StateTransition INVALID! - acquire failed because OS client NOT ATTACHED (state=%s(%d)) @%s:%d\n",
-				      chanId,
-				      ULTRA_CHANNELCLI_STRING(
-					      readl(&pChan->CliStateOS)),
-				      readl(&pChan->CliStateOS),
-				      PathName_Last_N_Nodes((u8 *) file, 4),
-				      line);
+			pr_info("%s Channel StateTransition INVALID! - acquire failed because OS client NOT ATTACHED (state=%s(%d))\n",
+				id, ULTRA_CHANNELCLI_STRING(
+						readl(&hdr->cli_state_os)),
+				readl(&hdr->cli_state_os));
 		}
 		return 0;
 	}
-	writel(CHANNELCLI_BUSY, &pChan->CliStateOS);
+	writel(CHANNELCLI_BUSY, &hdr->cli_state_os);
 	mb(); /* required for channel synch */
-	if (readl(&pChan->CliStateBoot) == CHANNELCLI_BUSY) {
-		if ((readb(&pChan->CliErrorOS)
+	if (readl(&hdr->cli_state_boot) == CHANNELCLI_BUSY) {
+		if ((readb(&hdr->cli_error_os)
 		     & ULTRA_CLIERROROS_THROTTLEMSG_BUSY) == 0) {
 			/* we are NOT throttling this message */
-			writeb(readb(&pChan->CliErrorOS) |
+			writeb(readb(&hdr->cli_error_os) |
 			       ULTRA_CLIERROROS_THROTTLEMSG_BUSY,
-			       &pChan->CliErrorOS);
+			       &hdr->cli_error_os);
 			/* throttle until acquire successful */
-			UltraLogEvent(logCtx,
-				      CHANNELSTATE_DIAG_EVENTID_TRANSITBUSY,
-				      CHANNELSTATE_DIAG_SEVERITY,
-				      CHANNELSTATE_DIAG_SUBSYS, func, line,
-				      "%s Channel StateTransition failed - host OS acquire failed because boot BUSY @%s:%d\n",
-				      chanId, PathName_Last_N_Nodes((u8 *) file,
-								    4), line);
+			pr_info("%s Channel StateTransition failed - host OS acquire failed because boot BUSY\n",
+				id);
 		}
 		/* reset busy */
-		writel(CHANNELCLI_ATTACHED, &pChan->CliStateOS);
+		writel(CHANNELCLI_ATTACHED, &hdr->cli_state_os);
 		mb(); /* required for channel synch */
 		return 0;
 	}
-	if (readb(&pChan->CliErrorOS) != 0) {
+	if (readb(&hdr->cli_error_os) != 0) {
 		/* we are in an error msg throttling state; come out of it */
-		UltraLogEvent(logCtx, CHANNELSTATE_DIAG_EVENTID_TRANSITOK,
-			      CHANNELSTATE_DIAG_SEVERITY,
-			      CHANNELSTATE_DIAG_SUBSYS, func, line,
-			      "%s Channel OS client acquire now successful @%s:%d\n",
-			      chanId, PathName_Last_N_Nodes((u8 *) file, 4),
-			      line);
-		writeb(0, &pChan->CliErrorOS);
+		pr_info("%s Channel OS client acquire now successful\n", id);
+		writeb(0, &hdr->cli_error_os);
 	}
 	return 1;
 }
 
 static inline void
-ULTRA_channel_client_release_os(void __iomem *pChannel, u8 *chanId,
-				void *logCtx, char *file, int line, char *func)
+spar_channel_client_release_os(void __iomem *ch, u8 *id)
 {
-	CHANNEL_HEADER __iomem *pChan = pChannel;
+	struct channel_header __iomem *hdr = ch;
 
-	if (readb(&pChan->CliErrorOS) != 0) {
+	if (readb(&hdr->cli_error_os) != 0) {
 		/* we are in an error msg throttling state; come out of it */
-		UltraLogEvent(logCtx, CHANNELSTATE_DIAG_EVENTID_TRANSITOK,
-			      CHANNELSTATE_DIAG_SEVERITY,
-			      CHANNELSTATE_DIAG_SUBSYS, func, line,
-			      "%s Channel OS client error state cleared @%s:%d\n",
-			      chanId, PathName_Last_N_Nodes((u8 *) file, 4),
-			      line);
-		writeb(0, &pChan->CliErrorOS);
+		pr_info("%s Channel OS client error state cleared\n", id);
+		writeb(0, &hdr->cli_error_os);
 	}
-	if (readl(&pChan->CliStateOS) == CHANNELCLI_OWNED)
+	if (readl(&hdr->cli_state_os) == CHANNELCLI_OWNED)
 		return;
-	if (readl(&pChan->CliStateOS) != CHANNELCLI_BUSY) {
-		UltraLogEvent(logCtx, CHANNELSTATE_DIAG_EVENTID_TRANSITERR,
-			      CHANNELSTATE_DIAG_SEVERITY,
-			      CHANNELSTATE_DIAG_SUBSYS, func, line,
-			      "%s Channel StateTransition INVALID! - release failed because OS client NOT BUSY (state=%s(%d)) @%s:%d\n",
-			      chanId,
-			      ULTRA_CHANNELCLI_STRING(
-				      readl(&pChan->CliStateOS)),
-			      readl(&pChan->CliStateOS),
-			      PathName_Last_N_Nodes((u8 *) file, 4), line);
+	if (readl(&hdr->cli_state_os) != CHANNELCLI_BUSY) {
+		pr_info("%s Channel StateTransition INVALID! - release failed because OS client NOT BUSY (state=%s(%d))\n",
+			id, ULTRA_CHANNELCLI_STRING(
+					readl(&hdr->cli_state_os)),
+			readl(&hdr->cli_state_os));
 		/* return; */
 	}
-	writel(CHANNELCLI_ATTACHED, &pChan->CliStateOS); /* release busy */
+	writel(CHANNELCLI_ATTACHED, &hdr->cli_state_os); /* release busy */
 }
 
 /*
@@ -625,8 +526,8 @@ ULTRA_channel_client_release_os(void __iomem *pChannel, u8 *chanId,
 * full.
 */
 
-unsigned char visor_signal_insert(CHANNEL_HEADER __iomem *pChannel, u32 Queue,
-				  void *pSignal);
+unsigned char spar_signal_insert(struct channel_header __iomem *ch, u32 queue,
+				 void *sig);
 
 /*
 * Routine Description:
@@ -647,8 +548,8 @@ unsigned char visor_signal_insert(CHANNEL_HEADER __iomem *pChannel, u32 Queue,
 * empty.
 */
 
-unsigned char visor_signal_remove(CHANNEL_HEADER __iomem *pChannel, u32 Queue,
-				  void *pSignal);
+unsigned char spar_signal_remove(struct channel_header __iomem *ch, u32 queue,
+				 void *sig);
 
 /*
 * Routine Description:
@@ -669,8 +570,8 @@ unsigned char visor_signal_remove(CHANNEL_HEADER __iomem *pChannel, u32 Queue,
 * Return value:
 * # of signals copied.
 */
-unsigned int SignalRemoveAll(pCHANNEL_HEADER pChannel, u32 Queue,
-			     void *pSignal);
+unsigned int spar_signal_remove_all(struct channel_header *ch, u32 queue,
+				    void *sig);
 
 /*
 * Routine Description:
@@ -683,7 +584,7 @@ unsigned int SignalRemoveAll(pCHANNEL_HEADER pChannel, u32 Queue,
 * Return value:
 * 1 if the signal queue is empty, 0 otherwise.
 */
-unsigned char visor_signalqueue_empty(CHANNEL_HEADER __iomem *pChannel,
-				      u32 Queue);
+unsigned char spar_signalqueue_empty(struct channel_header __iomem *ch,
+				     u32 queue);
 
 #endif
diff --git a/drivers/staging/unisys/common-spar/include/channels/channel_guid.h b/drivers/staging/unisys/common-spar/include/channels/channel_guid.h
index 63c67ca4c9ec..706363fc3e9a 100644
--- a/drivers/staging/unisys/common-spar/include/channels/channel_guid.h
+++ b/drivers/staging/unisys/common-spar/include/channels/channel_guid.h
@@ -20,45 +20,42 @@
 /* Used in IOChannel
  * {414815ed-c58c-11da-95a9-00e08161165f}
  */
-#define ULTRA_VHBA_CHANNEL_PROTOCOL_GUID \
+#define SPAR_VHBA_CHANNEL_PROTOCOL_UUID \
 		UUID_LE(0x414815ed, 0xc58c, 0x11da, \
 				0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f)
-static const uuid_le UltraVhbaChannelProtocolGuid =
-	ULTRA_VHBA_CHANNEL_PROTOCOL_GUID;
+static const uuid_le spar_vhba_channel_protocol_uuid =
+	SPAR_VHBA_CHANNEL_PROTOCOL_UUID;
 
 /* Used in IOChannel
  * {8cd5994d-c58e-11da-95a9-00e08161165f}
  */
-#define ULTRA_VNIC_CHANNEL_PROTOCOL_GUID \
+#define SPAR_VNIC_CHANNEL_PROTOCOL_UUID \
 		UUID_LE(0x8cd5994d, 0xc58e, 0x11da, \
 				0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f)
-static const uuid_le UltraVnicChannelProtocolGuid =
-	ULTRA_VNIC_CHANNEL_PROTOCOL_GUID;
+static const uuid_le spar_vnic_channel_protocol_uuid =
+	SPAR_VNIC_CHANNEL_PROTOCOL_UUID;
 
 /* Used in IOChannel
  * {72120008-4AAB-11DC-8530-444553544200}
  */
-#define ULTRA_SIOVM_GUID \
+#define SPAR_SIOVM_UUID \
 		UUID_LE(0x72120008, 0x4AAB, 0x11DC, \
 				0x85, 0x30, 0x44, 0x45, 0x53, 0x54, 0x42, 0x00)
-static const uuid_le UltraSIOVMGuid = ULTRA_SIOVM_GUID;
-
+static const uuid_le spar_siovm_uuid = SPAR_SIOVM_UUID;
 
 /* Used in visornoop/visornoop_main.c
  * {5b52c5ac-e5f5-4d42-8dff-429eaecd221f}
  */
-#define ULTRA_CONTROLDIRECTOR_CHANNEL_PROTOCOL_GUID  \
+#define SPAR_CONTROLDIRECTOR_CHANNEL_PROTOCOL_UUID  \
 		UUID_LE(0x5b52c5ac, 0xe5f5, 0x4d42, \
 				0x8d, 0xff, 0x42, 0x9e, 0xae, 0xcd, 0x22, 0x1f)
 
-static const uuid_le UltraControlDirectorChannelProtocolGuid =
-	ULTRA_CONTROLDIRECTOR_CHANNEL_PROTOCOL_GUID;
+static const uuid_le spar_controldirector_channel_protocol_uuid =
+	SPAR_CONTROLDIRECTOR_CHANNEL_PROTOCOL_UUID;
 
 /* Used in visorchipset/visorchipset_main.c
  * {B4E79625-AEDE-4EAA-9E11-D3EDDCD4504C}
  */
-#define ULTRA_DIAG_POOL_CHANNEL_PROTOCOL_GUID				\
+#define SPAR_DIAG_POOL_CHANNEL_PROTOCOL_UUID				\
 		UUID_LE(0xb4e79625, 0xaede, 0x4eaa, \
 				0x9e, 0x11, 0xd3, 0xed, 0xdc, 0xd4, 0x50, 0x4c)
-
-
diff --git a/drivers/staging/unisys/common-spar/include/channels/controlframework.h b/drivers/staging/unisys/common-spar/include/channels/controlframework.h
index fd4726e754ea..33d9caf337c8 100644
--- a/drivers/staging/unisys/common-spar/include/channels/controlframework.h
+++ b/drivers/staging/unisys/common-spar/include/channels/controlframework.h
@@ -28,50 +28,35 @@
 #include <linux/types.h>
 #include "channel.h"
 
-#define ULTRA_MEMORY_COUNT_Ki 1024
-
-/* Scale order 0 is one 32-bit (4-byte) word (in 64 or 128-bit
- * architecture potentially 64 or 128-bit word) */
-#define ULTRA_MEMORY_PAGE_WORD 4
-
-/* Define Ki scale page to be traditional 4KB page */
-#define ULTRA_MEMORY_PAGE_Ki (ULTRA_MEMORY_PAGE_WORD * ULTRA_MEMORY_COUNT_Ki)
-typedef struct _ULTRA_SEGMENT_STATE  {
-	u16 Enabled:1;		/* Bit 0: May enter other states */
-	u16 Active:1;		/* Bit 1: Assigned to active partition */
-	u16 Alive:1;		/* Bit 2: Configure message sent to
+struct spar_segment_state  {
+	u16 enabled:1;		/* Bit 0: May enter other states */
+	u16 active:1;		/* Bit 1: Assigned to active partition */
+	u16 alive:1;		/* Bit 2: Configure message sent to
 				 * service/server */
-	u16 Revoked:1;		/* Bit 3: similar to partition state
+	u16 revoked:1;		/* Bit 3: similar to partition state
 				 * ShuttingDown */
-	u16 Allocated:1;	/* Bit 4: memory (device/port number)
+	u16 allocated:1;	/* Bit 4: memory (device/port number)
 				 * has been selected by Command */
-	u16 Known:1;		/* Bit 5: has been introduced to the
+	u16 known:1;		/* Bit 5: has been introduced to the
 				 * service/guest partition */
-	u16 Ready:1;		/* Bit 6: service/Guest partition has
+	u16 ready:1;		/* Bit 6: service/Guest partition has
 				 * responded to introduction */
-	u16 Operating:1;	/* Bit 7: resource is configured and
+	u16 operating:1;	/* Bit 7: resource is configured and
 				 * operating */
 	/* Note: don't use high bit unless we need to switch to ushort
 	 * which is non-compliant */
-} ULTRA_SEGMENT_STATE;
-static const ULTRA_SEGMENT_STATE SegmentStateRunning = {
+};
+
+static const struct spar_segment_state segment_state_running = {
 	1, 1, 1, 0, 1, 1, 1, 1
 };
-static const ULTRA_SEGMENT_STATE SegmentStatePaused = {
+
+static const struct spar_segment_state segment_state_paused = {
 	1, 1, 1, 0, 1, 1, 1, 0
 };
-static const ULTRA_SEGMENT_STATE SegmentStateStandby = {
+
+static const struct spar_segment_state segment_state_standby = {
 	1, 1, 0, 0, 1, 1, 1, 0
 };
-typedef union {
-	u64 Full;
-	struct {
-		u8 Major;	/* will be 1 for the first release and
-				 * increment thereafter  */
-		u8 Minor;
-		u16 Maintenance;
-		u32 Revision;	/* Subversion revision */
-	} Part;
-} ULTRA_COMPONENT_VERSION;
 
 #endif				/* _CONTROL_FRAMEWORK_H_ not defined */
diff --git a/drivers/staging/unisys/common-spar/include/channels/controlvmchannel.h b/drivers/staging/unisys/common-spar/include/channels/controlvmchannel.h
index d08c198e0de3..a66db7968d6c 100644
--- a/drivers/staging/unisys/common-spar/include/channels/controlvmchannel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/controlvmchannel.h
@@ -27,12 +27,12 @@ enum { INVALID_GUEST_FIRMWARE, SAMPLE_GUEST_FIRMWARE,
 };
 
 /* {2B3C2D10-7EF5-4ad8-B966-3448B7386B3D} */
-#define ULTRA_CONTROLVM_CHANNEL_PROTOCOL_GUID	\
+#define SPAR_CONTROLVM_CHANNEL_PROTOCOL_UUID	\
 		UUID_LE(0x2b3c2d10, 0x7ef5, 0x4ad8, \
 				0xb9, 0x66, 0x34, 0x48, 0xb7, 0x38, 0x6b, 0x3d)
 
-static const uuid_le UltraControlvmChannelProtocolGuid =
-	ULTRA_CONTROLVM_CHANNEL_PROTOCOL_GUID;
+static const uuid_le spar_controlvm_channel_protocol_uuid =
+	SPAR_CONTROLVM_CHANNEL_PROTOCOL_UUID;
 
 #define ULTRA_CONTROLVM_CHANNEL_PROTOCOL_SIGNATURE \
 	ULTRA_CHANNEL_PROTOCOL_SIGNATURE
@@ -45,19 +45,13 @@ static const uuid_le UltraControlvmChannelProtocolGuid =
 * channel struct withOUT needing to increment this. */
 #define ULTRA_CONTROLVM_CHANNEL_PROTOCOL_VERSIONID  1
 
-#define ULTRA_CONTROLVM_CHANNEL_OK_CLIENT(pChannel, logCtx)           \
-	(ULTRA_check_channel_client(pChannel, \
-		UltraControlvmChannelProtocolGuid, \
+#define SPAR_CONTROLVM_CHANNEL_OK_CLIENT(ch)           \
+	spar_check_channel_client(ch, \
+		spar_controlvm_channel_protocol_uuid, \
 		"controlvm", \
-		sizeof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL), \
+		sizeof(struct spar_controlvm_channel_protocol), \
 		ULTRA_CONTROLVM_CHANNEL_PROTOCOL_VERSIONID, \
-		ULTRA_CONTROLVM_CHANNEL_PROTOCOL_SIGNATURE, \
-		__FILE__, __LINE__, logCtx))
-#define ULTRA_CONTROLVM_CHANNEL_OK_SERVER(actualBytes, logCtx)        \
-	(ULTRA_check_channel_server(UltraControlvmChannelProtocolGuid,	\
-				    "controlvm",			\
-				    sizeof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL), \
-				    actualBytes, __FILE__, __LINE__, logCtx))
+		ULTRA_CONTROLVM_CHANNEL_PROTOCOL_SIGNATURE)
 
 #define MY_DEVICE_INDEX 0
 #define MAX_MACDATA_LEN 8 /* number of bytes for MAC address in config packet */
@@ -88,7 +82,7 @@ static const uuid_le UltraControlvmChannelProtocolGuid =
  *  - issued on the EventQueue queue (q #2) in the ControlVm channel
  *  - responded to on the EventAckQueue queue (q #3) in the ControlVm channel
  */
-typedef enum  {
+enum controlvm_id {
 	CONTROLVM_INVALID = 0,
 	/* SWITCH commands required Parameter: SwitchNumber  */
 	/* BUS commands required Parameter: BusNumber  */
@@ -117,9 +111,9 @@ typedef enum  {
 	CONTROLVM_CHIPSET_READY = 0x304,	/* CP --> SP */
 	CONTROLVM_CHIPSET_SELFTEST = 0x305,	/* CP --> SP */
 
-} CONTROLVM_ID;
+};
 
-struct InterruptInfo {
+struct irq_info {
 	 /**< specifies interrupt info. It is used to send interrupts
 	  *   for this channel. The peer at the end of this channel
 	  *   who has registered an interrupt (using recv fields
@@ -128,495 +122,390 @@ struct InterruptInfo {
 	  *   interrupt.  Currently this is used by IOPart-SP to wake
 	  *   up GP when Data Channel transitions from empty to
 	  *   non-empty.*/
-	u64 sendInterruptHandle;
+	u64 send_irq_handle;
 
 	 /**< specifies interrupt handle. It is used to retrieve the
 	  *   corresponding interrupt pin from Monitor; and the
 	  *   interrupt pin is used to connect to the corresponding
-	  *   intrrupt.  Used by IOPart-GP only. */
-	u64 recvInterruptHandle;
+	  *   interrupt.  Used by IOPart-GP only. */
+	u64 recv_irq_handle;
 
 	 /**< specifies interrupt vector. It, interrupt pin, and shared are
 	  *   used to connect to the corresponding interrupt.  Used by
 	  *   IOPart-GP only. */
-	u32 recvInterruptVector;
+	u32 recv_irq_vector;
 
     /**< specifies if the recvInterrupt is shared.  It, interrupt pin
      *   and vector are used to connect to 0 = not shared; 1 = shared.
      *   the corresponding interrupt.  Used by IOPart-GP only. */
-	u8 recvInterruptShared;
+	u8 recv_irq_shared;
 	u8 reserved[3];	/* Natural alignment purposes */
 };
 
-struct PciId {
-	u16 Domain;
-	u8 Bus;
-	u8 Slot;
-	u8 Func;
-	u8 Reserved[3];	/* Natural alignment purposes */
-};
-
-struct PciConfigHdr {
-	u16 VendorId;
-	u16 SubSysVendor;
-	u16 DeviceId;
-	u16 SubSysDevice;
-	u32 ClassCode;
-	u32 Reserved;		/* Natural alignment purposes */
-};
-
-struct ScsiId {
-	u32 Bus;
-	u32 Target;
-	u32 Lun;
-	u32 Host; /* Command should ignore this for *
-		   * DiskArrival/RemovalEvents */
-};
-
-struct WWID {
-	u32 wwid1;
-	u32 wwid2;
-};
-
-struct virtDiskInfo  {
-	u32 switchNo;		/* defined by SWITCH_CREATE */
-	u32 externalPortNo;	/* 0 for SAS RAID provided (external)
-				 * virtual disks, 1 for virtual disk
-				 * images, 2 for gold disk images */
-	u16 VirtualDiskIndex;	/* Index of disk descriptor in the
-				 * VirtualDisk segment associated with
-				 * externalPortNo */
-	u16 Reserved1;
-	u32 Reserved2;
+struct pci_id {
+	u16 domain;
+	u8 bus;
+	u8 slot;
+	u8 func;
+	u8 reserved[3];	/* Natural alignment purposes */
 };
 
-typedef enum {
-	CONTROLVM_ACTION_NONE = 0,
-	CONTROLVM_ACTION_SET_RESTORE = 0x05E7,
-	CONTROLVM_ACTION_CLEAR_RESTORE = 0x0C18,
-	CONTROLVM_ACTION_RESTORING = 0x08E5,
-	CONTROLVM_ACTION_RESTORE_BUSY = 0x0999,
-	CONTROLVM_ACTION_CLEAR_NVRAM = 0xB01
-} CONTROLVM_ACTION;
-
-typedef enum _ULTRA_TOOL_ACTIONS {
-	    /* enumeration that defines intended action  */
-	    ULTRA_TOOL_ACTION_NONE = 0,	/* normal boot of boot disk */
-	ULTRA_TOOL_ACTION_INSTALL = 1,	/* install source disk(s) to boot
-					 * disk */
-	ULTRA_TOOL_ACTION_CAPTURE = 2,	/* capture boot disk to target disk(s)
-					 * as 'gold image' */
-	ULTRA_TOOL_ACTION_REPAIR = 3,	/* use source disk(s) to repair
-					 * installation on boot disk */
-	ULTRA_TOOL_ACTION_CLEAN = 4,	/* 'scrub' virtual disk before
-					 * releasing back to storage pool */
-	ULTRA_TOOL_ACTION_UPGRADE = 5,	/* upgrade to use content of images
-					 * referenced from newer blueprint */
-	ULTRA_TOOL_ACTION_DIAG = 6,	/* use tool to invoke diagnostic script
-					 * provided by blueprint */
-	ULTRA_TOOL_ACTION_FAILED = 7,	/* used when tool fails installation
-					   and cannot continue */
-	ULTRA_TOOL_ACTION_COUNT = 8
-} ULTRA_TOOL_ACTIONS;
-
-typedef struct _ULTRA_EFI_SPAR_INDICATION  {
-	u64 BootToFirmwareUI:1;	/* Bit 0: Stop in uefi ui */
-	u64 ClearNvram:1;	/* Bit 1: Clear NVRAM */
-	u64 ClearCmos:1;	/* Bit 2: Clear CMOS */
-	u64 BootToTool:1;	/* Bit 3: Run install tool */
+struct efi_spar_indication  {
+	u64 boot_to_fw_ui:1;	/* Bit 0: Stop in uefi ui */
+	u64 clear_nvram:1;	/* Bit 1: Clear NVRAM */
+	u64 clear_cmos:1;	/* Bit 2: Clear CMOS */
+	u64 boot_to_tool:1;	/* Bit 3: Run install tool */
 	/* remaining bits are available */
-} ULTRA_EFI_SPAR_INDICATION;
+};
 
-typedef enum {
+enum ultra_chipset_feature {
 	ULTRA_CHIPSET_FEATURE_REPLY = 0x00000001,
 	ULTRA_CHIPSET_FEATURE_PARA_HOTPLUG = 0x00000002,
 	ULTRA_CHIPSET_FEATURE_PCIVBUS = 0x00000004
-} ULTRA_CHIPSET_FEATURE;
+};
 
 /** This is the common structure that is at the beginning of every
  *  ControlVm message (both commands and responses) in any ControlVm
  *  queue.  Commands are easily distinguished from responses by
  *  looking at the flags.response field.
  */
-typedef struct _CONTROLVM_MESSAGE_HEADER  {
-	u32 Id;		/* See CONTROLVM_ID. */
+struct controlvm_message_header  {
+	u32 id;		/* See CONTROLVM_ID. */
 	/* For requests, indicates the message type. */
 	/* For responses, indicates the type of message we are responding to. */
 
-	u32 MessageSize;	/* Includes size of this struct + size
+	u32 message_size;	/* Includes size of this struct + size
 				 * of message */
-	u32 SegmentIndex;	/* Index of segment containing Vm
+	u32 segment_index;	/* Index of segment containing Vm
 				 * message/information */
-	u32 CompletionStatus;	/* Error status code or result of
+	u32 completion_status;	/* Error status code or result of
 				 * message completion */
 	struct  {
 		u32 failed:1;		   /**< =1 in a response to * signify
 					    * failure */
-		u32 responseExpected:1;   /**< =1 in all messages that expect a
+		u32 response_expected:1;   /**< =1 in all messages that expect a
 					   * response (Control ignores this
 					   * bit) */
 		u32 server:1;		   /**< =1 in all bus & device-related
 					    * messages where the message
 					    * receiver is to act as the bus or
 					    * device server */
-		u32 testMessage:1;	   /**< =1 for testing use only
+		u32 test_message:1;	   /**< =1 for testing use only
 					    * (Control and Command ignore this
 					    * bit) */
-		u32 partialCompletion:1;  /**< =1 if there are forthcoming
+		u32 partial_completion:1;  /**< =1 if there are forthcoming
 					   * responses/acks associated
 					   * with this message */
 		u32 preserve:1;	       /**< =1 this is to let us know to
 					* preserve channel contents
 					* (for running guests)*/
-		u32 writerInDiag:1;	/**< =1 the DiagWriter is active in the
+		u32 writer_in_diag:1;	/**< =1 the DiagWriter is active in the
 					 * Diagnostic Partition*/
-
-		    /* remaining bits in this 32-bit word are available */
-	} Flags;
-	u32 Reserved;		/* Natural alignment */
-	u64 MessageHandle;	/* Identifies the particular message instance,
+	} flags;
+	u32 reserved;		/* Natural alignment */
+	u64 message_handle;	/* Identifies the particular message instance,
 				 * and is used to match particular */
 	/* request instances with the corresponding response instance. */
-	u64 PayloadVmOffset;	/* Offset of payload area from start of this
+	u64 payload_vm_offset;	/* Offset of payload area from start of this
 				 * instance of ControlVm segment */
-	u32 PayloadMaxBytes;	/* Maximum bytes allocated in payload
+	u32 payload_max_bytes;	/* Maximum bytes allocated in payload
 				 * area of ControlVm segment */
-	u32 PayloadBytes;	/* Actual number of bytes of payload
+	u32 payload_bytes;	/* Actual number of bytes of payload
 				 * area to copy between IO/Command; */
 	/* if non-zero, there is a payload to copy. */
-} CONTROLVM_MESSAGE_HEADER;
-
-typedef struct _CONTROLVM_PACKET_DEVICE_CREATE  {
-	u32 busNo;	   /**< bus # (0..n-1) from the msg receiver's
-			    * perspective */
+};
 
-	    /* Control uses header SegmentIndex field to access bus number... */
-	u32 devNo;	   /**< bus-relative (0..n-1) device number */
-	u64 channelAddr;  /**< Guest physical address of the channel, which
-			*   can be dereferenced by the receiver
-			*   of this ControlVm command */
-	u64 channelBytes; /**< specifies size of the channel in bytes */
-	uuid_le dataTypeGuid;/**< specifies format of data in channel */
-	uuid_le devInstGuid; /**< instance guid for the device */
-	struct InterruptInfo intr; /**< specifies interrupt information */
-} CONTROLVM_PACKET_DEVICE_CREATE;	/* for CONTROLVM_DEVICE_CREATE */
-
-typedef struct _CONTROLVM_PACKET_DEVICE_CONFIGURE  {
-	u32 busNo;	      /**< bus # (0..n-1) from the msg
+struct controlvm_packet_device_create  {
+	u32 bus_no;	/* bus # (0..n-1) from the msg receiver's end */
+	u32 dev_no;	/* bus-relative (0..n-1) device number */
+	u64 channel_addr;	/* Guest physical address of the channel, which
+				 * can be dereferenced by the receiver of this
+				 * ControlVm command */
+	u64 channel_bytes;	/* specifies size of the channel in bytes */
+	uuid_le data_type_uuid;	/* specifies format of data in channel */
+	uuid_le dev_inst_uuid;	/* instance guid for the device */
+	struct irq_info intr;	/* specifies interrupt information */
+};	/* for CONTROLVM_DEVICE_CREATE */
+
+struct controlvm_packet_device_configure  {
+	u32 bus_no;	      /**< bus # (0..n-1) from the msg
 			       * receiver's perspective */
 
 	    /* Control uses header SegmentIndex field to access bus number... */
-	u32 devNo;	      /**< bus-relative (0..n-1) device number */
-} CONTROLVM_PACKET_DEVICE_CONFIGURE;	/* for CONTROLVM_DEVICE_CONFIGURE */
+	u32 dev_no;	      /**< bus-relative (0..n-1) device number */
+} ;	/* for CONTROLVM_DEVICE_CONFIGURE */
 
-typedef struct _CONTROLVM_MESSAGE_DEVICE_CREATE  {
-	CONTROLVM_MESSAGE_HEADER Header;
-	CONTROLVM_PACKET_DEVICE_CREATE Packet;
-} CONTROLVM_MESSAGE_DEVICE_CREATE;	/* total 128 bytes */
+struct controlvm_message_device_create {
+	struct controlvm_message_header header;
+	struct controlvm_packet_device_create packet;
+};	/* total 128 bytes */
 
-typedef struct _CONTROLVM_MESSAGE_DEVICE_CONFIGURE  {
-	CONTROLVM_MESSAGE_HEADER Header;
-	CONTROLVM_PACKET_DEVICE_CONFIGURE Packet;
-} CONTROLVM_MESSAGE_DEVICE_CONFIGURE;	/* total 56 bytes */
+struct controlvm_message_device_configure  {
+	struct controlvm_message_header header;
+	struct controlvm_packet_device_configure packet;
+};	/* total 56 bytes */
 
 /* This is the format for a message in any ControlVm queue. */
-typedef struct _CONTROLVM_MESSAGE_PACKET  {
+struct controlvm_message_packet  {
 	union  {
-
-		/* BEGIN Request messages */
 		struct  {
-			u32 busNo;	      /*< bus # (0..n-1) from the msg
-					       * receiver's perspective */
-
-	    /* Control uses header SegmentIndex field to access bus number... */
-			u32 deviceCount;      /*< indicates the max number of
-					       * devices on this bus */
-			u64 channelAddr;     /*< Guest physical address of the
-					      *   channel, which can be
-					      *   dereferenced by the receiver
-					      *   of this ControlVm command */
-			u64 channelBytes;    /*< size of the channel in bytes */
-			uuid_le busDataTypeGuid;/*< indicates format of data in
-						    bus channel */
-			uuid_le busInstGuid;    /*< instance guid for the bus */
-		} createBus;	/* for CONTROLVM_BUS_CREATE */
+			u32 bus_no;	/* bus # (0..n-1) from the msg
+					 * receiver's perspective */
+			u32 dev_count;	/* indicates the max number of
+					 * devices on this bus */
+			u64 channel_addr;	/* Guest physical address of
+						 * the channel, which can be
+						 * dereferenced by the receiver
+						 * of this ControlVm command */
+			u64 channel_bytes;	/* size of the channel */
+			uuid_le bus_data_type_uuid;	/* indicates format of
+							 * data in bus channel*/
+			uuid_le bus_inst_uuid;	/* instance uuid for the bus */
+		} create_bus;	/* for CONTROLVM_BUS_CREATE */
 		struct  {
-			u32 busNo;	      /*< bus # (0..n-1) from the msg
-					       * receiver's perspective */
-
-	    /* Control uses header SegmentIndex field to access bus number... */
+			u32 bus_no;	/* bus # (0..n-1) from the msg
+					 * receiver's perspective */
 			u32 reserved;	/* Natural alignment purposes */
-		} destroyBus;	/* for CONTROLVM_BUS_DESTROY */
+		} destroy_bus;	/* for CONTROLVM_BUS_DESTROY */
 		struct  {
-			u32 busNo;		    /*< bus # (0..n-1) from the
-						     * msg receiver's
-						     * perspective */
-
-	    /* Control uses header SegmentIndex field to access bus number... */
-			u32 reserved1;		    /* for alignment purposes */
-			u64 guestHandle;	    /* This is used to convert
-					 *  guest physical address to real
-					 *  physical address for DMA, for ex. */
-			u64 recvBusInterruptHandle;/*< specifies interrupt
-					 *   info. It is used by SP to register
-					 *   to receive interrupts from the CP.
-					 *   This interrupt is used for bus
-					 *   level notifications.  The
-					 *   corresponding
-					 *   sendBusInterruptHandle is kept in
-					 *   CP. */
-		} configureBus;	/* for CONTROLVM_BUS_CONFIGURE */
-
+			u32 bus_no;	/* bus # (0..n-1) from the receiver's
+					 * perspective */
+			u32 reserved1;	/* for alignment purposes */
+			u64 guest_handle;	/* This is used to convert
+						 * guest physical address to
+						 * physical address */
+			u64 recv_bus_irq_handle;
+				/* specifies interrupt info. It is used by SP
+				 * to register to receive interrupts from the
+				 * CP. This interrupt is used for bus level
+				 * notifications.  The corresponding
+				 * sendBusInterruptHandle is kept in CP. */
+		} configure_bus;	/* for CONTROLVM_BUS_CONFIGURE */
 		/* for CONTROLVM_DEVICE_CREATE */
-		CONTROLVM_PACKET_DEVICE_CREATE createDevice;
+		struct controlvm_packet_device_create create_device;
 		struct  {
-			u32 busNo;	      /*< bus # (0..n-1) from the msg
-					       * receiver's perspective */
-
-	    /* Control uses header SegmentIndex field to access bus number... */
-			u32 devNo;	      /*< bus-relative (0..n-1) device
-					       * number */
-		} destroyDevice;	/* for CONTROLVM_DEVICE_DESTROY */
-
+			u32 bus_no;	/* bus # (0..n-1) from the msg
+					 * receiver's perspective */
+			u32 dev_no;	/* bus-relative (0..n-1) device # */
+		} destroy_device;	/* for CONTROLVM_DEVICE_DESTROY */
 		/* for CONTROLVM_DEVICE_CONFIGURE */
-		CONTROLVM_PACKET_DEVICE_CONFIGURE configureDevice;
+		struct controlvm_packet_device_configure configure_device;
 		struct  {
-			u32 busNo;	      /*< bus # (0..n-1) from the msg
-					       * receiver's perspective */
-
-	    /* Control uses header SegmentIndex field to access bus number... */
-			u32 devNo;	      /*< bus-relative (0..n-1) device
-					       * number */
-		} reconfigureDevice;	/* for CONTROLVM_DEVICE_RECONFIGURE */
+			u32 bus_no;	/* bus # (0..n-1) from the msg
+					 * receiver's perspective */
+			u32 dev_no;	/* bus-relative (0..n-1) device # */
+		} reconfigure_device;	/* for CONTROLVM_DEVICE_RECONFIGURE */
 		struct  {
-			u32 busNo;
-			ULTRA_SEGMENT_STATE state;
+			u32 bus_no;
+			struct spar_segment_state state;
 			u8 reserved[2];	/* Natural alignment purposes */
-		} busChangeState;	/* for CONTROLVM_BUS_CHANGESTATE */
+		} bus_change_state;	/* for CONTROLVM_BUS_CHANGESTATE */
 		struct  {
-			u32 busNo;
-			u32 devNo;
-			ULTRA_SEGMENT_STATE state;
+			u32 bus_no;
+			u32 dev_no;
+			struct spar_segment_state state;
 			struct  {
-				u32 physicalDevice:1;	/* =1 if message is for
+				u32 phys_device:1;	/* =1 if message is for
 							 * a physical device */
-			/* remaining bits in this 32-bit word are available */
 			} flags;
 			u8 reserved[2];	/* Natural alignment purposes */
-		} deviceChangeState;	/* for CONTROLVM_DEVICE_CHANGESTATE */
+		} device_change_state;	/* for CONTROLVM_DEVICE_CHANGESTATE */
 		struct  {
-			u32 busNo;
-			u32 devNo;
-			ULTRA_SEGMENT_STATE state;
+			u32 bus_no;
+			u32 dev_no;
+			struct spar_segment_state state;
 			u8 reserved[6];	/* Natural alignment purposes */
-		} deviceChangeStateEvent; /* for CONTROLVM_DEVICE_CHANGESTATE_EVENT */
+		} device_change_state_event;
+			/* for CONTROLVM_DEVICE_CHANGESTATE_EVENT */
 		struct  {
-			u32 busCount; /*< indicates the max number of busses */
-			u32 switchCount; /*< indicates the max number of
-					  *   switches (applicable for service
-					  *   partition only) */
-			ULTRA_CHIPSET_FEATURE features;
-			u32 platformNumber;	/* Platform Number */
-		} initChipset;	/* for CONTROLVM_CHIPSET_INIT */
+			u32 bus_count;	/* indicates the max number of busses */
+			u32 switch_count; /* indicates the max number of
+					   * switches if a service partition */
+			enum ultra_chipset_feature features;
+			u32 platform_number;	/* Platform Number */
+		} init_chipset;	/* for CONTROLVM_CHIPSET_INIT */
 		struct  {
-			u32 Options; /*< reserved */
-			u32 Test;    /*< bit 0 set to run embedded selftest */
-		} chipsetSelftest;	/* for CONTROLVM_CHIPSET_SELFTEST */
-
-		    /* END Request messages */
-
-		    /* BEGIN Response messages */
-
-		    /* END Response messages */
-
-		    /* BEGIN Event messages */
+			u32 options;	/* reserved */
+			u32 test;	/* bit 0 set to run embedded selftest */
+		} chipset_selftest;	/* for CONTROLVM_CHIPSET_SELFTEST */
+		u64 addr;	/* a physical address of something, that can be
+				 * dereferenced by the receiver of this
+				 * ControlVm command (depends on command id) */
+		u64 handle;	/* a handle of something (depends on command
+				 * id) */
+	};
+};
 
-		    /* END Event messages */
+/* All messages in any ControlVm queue have this layout. */
+struct controlvm_message {
+	struct controlvm_message_header hdr;
+	struct controlvm_message_packet cmd;
+};
 
-		    /* BEGIN Ack messages */
+struct device_map {
+	GUEST_PHYSICAL_ADDRESS device_channel_address;
+	u64 device_channel_size;
+	u32 ca_index;
+	u32 reserved;		/* natural alignment */
+	u64 reserved2;		/* Align structure on 32-byte boundary */
+};
 
-		    /* END Ack messages */
-		u64 addr;	    /*< a physical address of something, that
-				     *   can be dereferenced by the receiver of
-				     *   this ControlVm command (depends on
-				     *   command id) */
-		u64 handle;	    /*< a handle of something (depends on
-				     * command id) */
-	};
-} CONTROLVM_MESSAGE_PACKET;
+struct guest_devices  {
+	struct device_map video_channel;
+	struct device_map keyboard_channel;
+	struct device_map network_channel;
+	struct device_map storage_channel;
+	struct device_map console_channel;
+	u32 partition_index;
+	u32 pad;
+};
 
-/* All messages in any ControlVm queue have this layout. */
-typedef struct _CONTROLVM_MESSAGE  {
-	CONTROLVM_MESSAGE_HEADER hdr;
-	CONTROLVM_MESSAGE_PACKET cmd;
-} CONTROLVM_MESSAGE;
-
-typedef struct _DEVICE_MAP  {
-	GUEST_PHYSICAL_ADDRESS DeviceChannelAddress;
-	u64 DeviceChannelSize;
-	u32 CA_Index;
-	u32 Reserved;		/* natural alignment */
-	u64 Reserved2;		/* Align structure on 32-byte boundary */
-} DEVICE_MAP;
-
-typedef struct _GUEST_DEVICES  {
-	DEVICE_MAP VideoChannel;
-	DEVICE_MAP KeyboardChannel;
-	DEVICE_MAP NetworkChannel;
-	DEVICE_MAP StorageChannel;
-	DEVICE_MAP ConsoleChannel;
-	u32 PartitionIndex;
-	u32 Pad;
-} GUEST_DEVICES;
-
-typedef struct _ULTRA_CONTROLVM_CHANNEL_PROTOCOL  {
-	 CHANNEL_HEADER Header;
-	 GUEST_PHYSICAL_ADDRESS gpControlVm;	/* guest physical address of
+struct spar_controlvm_channel_protocol {
+	 struct channel_header header;
+	 GUEST_PHYSICAL_ADDRESS gp_controlvm;	/* guest physical address of
 						 * this channel */
-	 GUEST_PHYSICAL_ADDRESS gpPartitionTables; /* guest physical address of
-						    * partition tables */
-	 GUEST_PHYSICAL_ADDRESS gpDiagGuest;	/* guest physical address of
+	 GUEST_PHYSICAL_ADDRESS gp_partition_tables;/* guest physical address of
+						     * partition tables */
+	 GUEST_PHYSICAL_ADDRESS gp_diag_guest;	/* guest physical address of
 						 * diagnostic channel */
-	 GUEST_PHYSICAL_ADDRESS gpBootRomDisk;	/* guest phys addr of (read
+	 GUEST_PHYSICAL_ADDRESS gp_boot_romdisk;/* guest phys addr of (read
 						 * only) Boot ROM disk */
-	 GUEST_PHYSICAL_ADDRESS gpBootRamDisk;	/* guest phys addr of writable
+	 GUEST_PHYSICAL_ADDRESS gp_boot_ramdisk;/* guest phys addr of writable
 						 * Boot RAM disk */
-	 GUEST_PHYSICAL_ADDRESS gpAcpiTable;	/* guest phys addr of acpi
+	 GUEST_PHYSICAL_ADDRESS gp_acpi_table;	/* guest phys addr of acpi
 						 * table */
-	 GUEST_PHYSICAL_ADDRESS gpControlChannel; /* guest phys addr of control
-						   * channel */
-	 GUEST_PHYSICAL_ADDRESS gpDiagRomDisk;	/* guest phys addr of diagnostic
+	 GUEST_PHYSICAL_ADDRESS gp_control_channel;/* guest phys addr of control
+						    * channel */
+	 GUEST_PHYSICAL_ADDRESS gp_diag_romdisk;/* guest phys addr of diagnostic
 						 * ROM disk */
-	 GUEST_PHYSICAL_ADDRESS gpNvram;	/* guest phys addr of NVRAM
+	 GUEST_PHYSICAL_ADDRESS gp_nvram;	/* guest phys addr of NVRAM
 						 * channel */
-	 u64 RequestPayloadOffset;	/* Offset to request payload area */
-	 u64 EventPayloadOffset;	/* Offset to event payload area */
-	 u32 RequestPayloadBytes;	/* Bytes available in request payload
+	 u64 request_payload_offset;	/* Offset to request payload area */
+	 u64 event_payload_offset;	/* Offset to event payload area */
+	 u32 request_payload_bytes;	/* Bytes available in request payload
 					 * area */
-	 u32 EventPayloadBytes;	/* Bytes available in event payload area */
-	 u32 ControlChannelBytes;
-	 u32 NvramChannelBytes;	/* Bytes in PartitionNvram segment */
-	 u32 MessageBytes;	/* sizeof(CONTROLVM_MESSAGE) */
-	 u32 MessageCount;	/* CONTROLVM_MESSAGE_MAX */
-	 GUEST_PHYSICAL_ADDRESS gpSmbiosTable;	/* guest phys addr of SMBIOS
+	 u32 event_payload_bytes;/* Bytes available in event payload area */
+	 u32 control_channel_bytes;
+	 u32 nvram_channel_bytes;	/* Bytes in PartitionNvram segment */
+	 u32 message_bytes;	/* sizeof(CONTROLVM_MESSAGE) */
+	 u32 message_count;	/* CONTROLVM_MESSAGE_MAX */
+	 GUEST_PHYSICAL_ADDRESS gp_smbios_table;/* guest phys addr of SMBIOS
 						 * tables */
-	 GUEST_PHYSICAL_ADDRESS gpPhysicalSmbiosTable;	/* guest phys addr of
-							 * SMBIOS table  */
+	 GUEST_PHYSICAL_ADDRESS gp_physical_smbios_table;/* guest phys addr of
+							  * SMBIOS table  */
 	 /* ULTRA_MAX_GUESTS_PER_SERVICE */
-	 GUEST_DEVICES gpObsoleteGuestDevices[16];
+	 struct guest_devices gp_obsolete_guest_devices[16];
 
 	 /* guest physical address of EFI firmware image base  */
-	 GUEST_PHYSICAL_ADDRESS VirtualGuestFirmwareImageBase;
+	 GUEST_PHYSICAL_ADDRESS virtual_guest_firmware_image_base;
 
 	 /* guest physical address of EFI firmware entry point  */
-	 GUEST_PHYSICAL_ADDRESS VirtualGuestFirmwareEntryPoint;
+	 GUEST_PHYSICAL_ADDRESS virtual_guest_firmware_entry_point;
 
 	 /* guest EFI firmware image size  */
-	 u64 VirtualGuestFirmwareImageSize;
+	 u64 virtual_guest_firmware_image_size;
 
 	 /* GPA = 1MB where EFI firmware image is copied to  */
-	 GUEST_PHYSICAL_ADDRESS VirtualGuestFirmwareBootBase;
-	 GUEST_PHYSICAL_ADDRESS VirtualGuestImageBase;
-	 GUEST_PHYSICAL_ADDRESS VirtualGuestImageSize;
-	 u64 PrototypeControlChannelOffset;
-	 GUEST_PHYSICAL_ADDRESS VirtualGuestPartitionHandle;
+	 GUEST_PHYSICAL_ADDRESS virtual_guest_firmware_boot_base;
+	 GUEST_PHYSICAL_ADDRESS virtual_guest_image_base;
+	 GUEST_PHYSICAL_ADDRESS virtual_guest_image_size;
+	 u64 prototype_control_channel_offset;
+	 GUEST_PHYSICAL_ADDRESS virtual_guest_partition_handle;
 
-	 u16 RestoreAction;	/* Restore Action field to restore the guest
+	 u16 restore_action;	/* Restore Action field to restore the guest
 				 * partition */
-	u16 DumpAction;		/* For Windows guests it shows if the visordisk
+	u16 dump_action;	/* For Windows guests it shows if the visordisk
 				 * is running in dump mode */
-	u16 NvramFailCount;
-	u16 SavedCrashMsgCount;	/* = CONTROLVM_CRASHMSG_MAX */
-	u32 SavedCrashMsgOffset;	/* Offset to request payload area needed
+	u16 nvram_fail_count;
+	u16 saved_crash_message_count;	/* = CONTROLVM_CRASHMSG_MAX */
+	u32 saved_crash_message_offset;	/* Offset to request payload area needed
 					 * for crash dump */
-	u32 InstallationError;	/* Type of error encountered during
+	u32 installation_error;	/* Type of error encountered during
 				 * installation */
-	u32 InstallationTextId;	/* Id of string to display */
-	u16 InstallationRemainingSteps;	/* Number of remaining installation
-					 * steps (for progress bars) */
-	u8 ToolAction;		/* ULTRA_TOOL_ACTIONS Installation Action
+	u32 installation_text_id;	/* Id of string to display */
+	u16 installation_remaining_steps;/* Number of remaining installation
+					  * steps (for progress bars) */
+	u8 tool_action;		/* ULTRA_TOOL_ACTIONS Installation Action
 				 * field */
-	u8 Reserved;		/* alignment */
-	ULTRA_EFI_SPAR_INDICATION EfiSparIndication;
-	ULTRA_EFI_SPAR_INDICATION EfiSparIndicationSupported;
-	u32 SPReserved;
-	u8 Reserved2[28];	/* Force signals to begin on 128-byte cache
+	u8 reserved;		/* alignment */
+	struct efi_spar_indication efi_spar_ind;
+	struct efi_spar_indication efi_spar_ind_supported;
+	u32 sp_reserved;
+	u8 reserved2[28];	/* Force signals to begin on 128-byte cache
 				 * line */
-	SIGNAL_QUEUE_HEADER RequestQueue;	/* Service or guest partition
-						 * uses this queue to send
-						 * requests to Control */
-	SIGNAL_QUEUE_HEADER ResponseQueue;	/* Control uses this queue to
-						 * respond to service or guest
-						 * partition requests */
-	SIGNAL_QUEUE_HEADER EventQueue;		/* Control uses this queue to
+	struct signal_queue_header request_queue;/* Service or guest partition
+						  * uses this queue to send
+						  * requests to Control */
+	struct signal_queue_header response_queue;/* Control uses this queue to
+						   * respond to service or guest
+						   * partition requests */
+	struct signal_queue_header event_queue;	/* Control uses this queue to
 						 * send events to service or
 						 * guest partition */
-	SIGNAL_QUEUE_HEADER EventAckQueue;	/* Service or guest partition
-						 * uses this queue to ack
-						 * Control events */
+	struct signal_queue_header event_ack_queue;/* Service or guest partition
+						    * uses this queue to ack
+						    * Control events */
 
 	 /* Request fixed-size message pool - does not include payload */
-	 CONTROLVM_MESSAGE RequestMsg[CONTROLVM_MESSAGE_MAX];
+	 struct controlvm_message request_msg[CONTROLVM_MESSAGE_MAX];
 
 	 /* Response fixed-size message pool - does not include payload */
-	 CONTROLVM_MESSAGE ResponseMsg[CONTROLVM_MESSAGE_MAX];
+	 struct controlvm_message response_msg[CONTROLVM_MESSAGE_MAX];
 
 	 /* Event fixed-size message pool - does not include payload */
-	 CONTROLVM_MESSAGE EventMsg[CONTROLVM_MESSAGE_MAX];
+	 struct controlvm_message event_msg[CONTROLVM_MESSAGE_MAX];
 
 	 /* Ack fixed-size message pool - does not include payload */
-	 CONTROLVM_MESSAGE EventAckMsg[CONTROLVM_MESSAGE_MAX];
+	 struct controlvm_message event_ack_msg[CONTROLVM_MESSAGE_MAX];
 
 	 /* Message stored during IOVM creation to be reused after crash */
-	 CONTROLVM_MESSAGE SavedCrashMsg[CONTROLVM_CRASHMSG_MAX];
-} ULTRA_CONTROLVM_CHANNEL_PROTOCOL;
+	 struct controlvm_message saved_crash_msg[CONTROLVM_CRASHMSG_MAX];
+};
 
 /* Offsets for VM channel attributes... */
 #define VM_CH_REQ_QUEUE_OFFSET \
-	offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL, RequestQueue)
+	offsetof(struct spar_controlvm_channel_protocol, request_queue)
 #define VM_CH_RESP_QUEUE_OFFSET \
-	offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL, ResponseQueue)
+	offsetof(struct spar_controlvm_channel_protocol, response_queue)
 #define VM_CH_EVENT_QUEUE_OFFSET \
-	offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL, EventQueue)
+	offsetof(struct spar_controlvm_channel_protocol, event_queue)
 #define VM_CH_ACK_QUEUE_OFFSET \
-	offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL, EventAckQueue)
+	offsetof(struct spar_controlvm_channel_protocol, event_ack_queue)
 #define VM_CH_REQ_MSG_OFFSET \
-	offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL, RequestMsg)
+	offsetof(struct spar_controlvm_channel_protocol, request_msg)
 #define VM_CH_RESP_MSG_OFFSET \
-	offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL, ResponseMsg)
+	offsetof(struct spar_controlvm_channel_protocol, response_msg)
 #define VM_CH_EVENT_MSG_OFFSET \
-	offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL, EventMsg)
+	offsetof(struct spar_controlvm_channel_protocol, event_msg)
 #define VM_CH_ACK_MSG_OFFSET \
-	offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL, EventAckMsg)
+	offsetof(struct spar_controlvm_channel_protocol, event_ack_msg)
 #define VM_CH_CRASH_MSG_OFFSET \
-	offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL, SavedCrashMsg)
+	offsetof(struct spar_controlvm_channel_protocol, saved_crash_msg)
 
 /* The following header will be located at the beginning of PayloadVmOffset for
- *  various ControlVm commands. The receiver of a ControlVm command with a
- *  PayloadVmOffset will dereference this address and then use ConnectionOffset,
- *  InitiatorOffset, and TargetOffset to get the location of UTF-8 formatted
- *  strings that can be parsed to obtain command-specific information. The value
- *  of TotalLength should equal PayloadBytes.  The format of the strings at
- *  PayloadVmOffset will take different forms depending on the message.  See the
- *  following Wiki page for more information:
- *  https://ustr-linux-1.na.uis.unisys.com/spar/index.php/ControlVm_Parameters_Area
+ * various ControlVm commands. The receiver of a ControlVm command with a
+ * PayloadVmOffset will dereference this address and then use connection_offset,
+ * initiator_offset, and target_offset to get the location of UTF-8 formatted
+ * strings that can be parsed to obtain command-specific information. The value
+ * of total_length should equal PayloadBytes. The format of the strings at
+ * PayloadVmOffset will take different forms depending on the message.
  */
-typedef struct _ULTRA_CONTROLVM_PARAMETERS_HEADER  {
-	u32 TotalLength;
-	u32 HeaderLength;
-	u32 ConnectionOffset;
-	u32 ConnectionLength;
-	u32 InitiatorOffset;
-	u32 InitiatorLength;
-	u32 TargetOffset;
-	u32 TargetLength;
-	u32 ClientOffset;
-	u32 ClientLength;
-	u32 NameOffset;
-	u32 NameLength;
-	uuid_le Id;
-	u32 Revision;
-	u32 Reserved;		/* Natural alignment */
-} ULTRA_CONTROLVM_PARAMETERS_HEADER;
+struct spar_controlvm_parameters_header {
+	u32 total_length;
+	u32 header_length;
+	u32 connection_offset;
+	u32 connection_length;
+	u32 initiator_offset;
+	u32 initiator_length;
+	u32 target_offset;
+	u32 target_length;
+	u32 client_offset;
+	u32 client_length;
+	u32 name_offset;
+	u32 name_length;
+	uuid_le id;
+	u32 revision;
+	u32 reserved;		/* Natural alignment */
+};
 
 #endif				/* __CONTROLVMCHANNEL_H__ */
diff --git a/drivers/staging/unisys/common-spar/include/channels/diagchannel.h b/drivers/staging/unisys/common-spar/include/channels/diagchannel.h
index 9912e51b89b5..e8fb8678a8e2 100644
--- a/drivers/staging/unisys/common-spar/include/channels/diagchannel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/diagchannel.h
@@ -37,12 +37,12 @@
 #include "channel.h"
 
 /* {EEA7A573-DB82-447c-8716-EFBEAAAE4858} */
-#define ULTRA_DIAG_CHANNEL_PROTOCOL_GUID \
+#define SPAR_DIAG_CHANNEL_PROTOCOL_UUID \
 		UUID_LE(0xeea7a573, 0xdb82, 0x447c, \
 				0x87, 0x16, 0xef, 0xbe, 0xaa, 0xae, 0x48, 0x58)
 
-static const uuid_le UltraDiagChannelProtocolGuid =
-	ULTRA_DIAG_CHANNEL_PROTOCOL_GUID;
+static const uuid_le spar_diag_channel_protocol_uuid =
+	SPAR_DIAG_CHANNEL_PROTOCOL_UUID;
 
 /* {E850F968-3263-4484-8CA5-2A35D087A5A8} */
 #define ULTRA_DIAG_ROOT_CHANNEL_PROTOCOL_GUID \
@@ -58,19 +58,20 @@ static const uuid_le UltraDiagChannelProtocolGuid =
 * increment this. */
 #define ULTRA_DIAG_CHANNEL_PROTOCOL_VERSIONID 2
 
-#define ULTRA_DIAG_CHANNEL_OK_CLIENT(pChannel, logCtx)			\
-	(ULTRA_check_channel_client(pChannel,				\
-				    UltraDiagChannelProtocolGuid,	\
-				    "diag",				\
-				    sizeof(ULTRA_DIAG_CHANNEL_PROTOCOL), \
-				    ULTRA_DIAG_CHANNEL_PROTOCOL_VERSIONID, \
-				    ULTRA_DIAG_CHANNEL_PROTOCOL_SIGNATURE, \
-				    __FILE__, __LINE__, logCtx))
-#define ULTRA_DIAG_CHANNEL_OK_SERVER(actualBytes, logCtx)		\
-	(ULTRA_check_channel_server(UltraDiagChannelProtocolGuid,	\
-				    "diag",				\
-				    sizeof(ULTRA_DIAG_CHANNEL_PROTOCOL), \
-				    actualBytes, __FILE__, __LINE__, logCtx))
+#define SPAR_DIAG_CHANNEL_OK_CLIENT(ch)\
+	(spar_check_channel_client(ch,\
+				   spar_diag_channel_protocol_uuid,\
+				   "diag",\
+				   sizeof(struct spar_diag_channel_protocol),\
+				   ULTRA_DIAG_CHANNEL_PROTOCOL_VERSIONID,\
+				   ULTRA_DIAG_CHANNEL_PROTOCOL_SIGNATURE))
+
+#define SPAR_DIAG_CHANNEL_OK_SERVER(bytes)\
+	(spar_check_channel_server(spar_diag_channel_protocol_uuid,\
+				   "diag",\
+				   sizeof(struct spar_diag_channel_protocol),\
+				   bytes))
+
 #define MAX_MODULE_NAME_SIZE 128	/* Maximum length of module name... */
 #define MAX_ADDITIONAL_INFO_SIZE 256	/* Maximum length of any additional info
 					 * accompanying event... */
@@ -105,21 +106,21 @@ static const uuid_le UltraDiagChannelProtocolGuid =
 /* Copied from EFI's EFI_TIME struct in efidef.h.  EFI headers are not allowed
 * in some of the Supervisor areas, such as Monitor, so it has been "ported" here
 * for use in diagnostic event timestamps... */
-typedef struct _DIAG_EFI_TIME  {
-	u16 Year;		/* 1998 - 20XX */
-	u8 Month;		/* 1 - 12 */
-	u8 Day;			/* 1 - 31 */
-	u8 Hour;		/* 0 - 23 */
-	u8 Minute;		/* 0 - 59 */
-	u8 Second;		/* 0 - 59 */
-	u8 Pad1;
-	u32 Nanosecond;	/* 0 - 999, 999, 999 */
-	s16 TimeZone;		/* -1440 to 1440 or 2047 */
-	u8 Daylight;
-	u8 Pad2;
-} DIAG_EFI_TIME;
-
-typedef enum  {
+struct diag_efi_time  {
+	u16 year;		/* 1998 - 20XX */
+	u8 month;		/* 1 - 12 */
+	u8 day;			/* 1 - 31 */
+	u8 hour;		/* 0 - 23 */
+	u8 minute;		/* 0 - 59 */
+	u8 second;		/* 0 - 59 */
+	u8 pad1;
+	u32 nanosecond;	/* 0 - 999, 999, 999 */
+	s16 timezone;		/* -1440 to 1440 or 2047 */
+	u8 daylight;
+	u8 pad2;
+};
+
+enum spar_component_types  {
 	 ULTRA_COMPONENT_GUEST = 0,
 	 ULTRA_COMPONENT_MONITOR = 0x01,
 	 ULTRA_COMPONENT_CCM = 0x02,	/* Common Control module */
@@ -144,9 +145,9 @@ typedef enum  {
 	 ULTRA_COMPONENT_PSERVICES = 0x17,
 	 ULTRA_COMPONENT_PDIAG = 0x18
 	 /* RESERVED 0x18 - 0x1F */
-} ULTRA_COMPONENT_TYPES;
+};
 
-/* Structure: DIAG_CHANNEL_EVENT Purpose: Contains attributes that make up an
+/* Structure: diag_channel_event Purpose: Contains attributes that make up an
  * event to be written to the DIAG_CHANNEL memory.  Attributes: EventId: Id of
  * the diagnostic event to write to memory.  Severity: Severity of the event
  * (Error, Info, etc).  ModuleName: Module/file name where event originated.
@@ -155,40 +156,40 @@ typedef enum  {
  * Reserved: Padding to align structure on a 64-byte cache line boundary.
  * AdditionalInfo: Array of characters for additional event info (may be
  * empty).  */
-typedef struct _DIAG_CHANNEL_EVENT  {
-	u32 EventId;
-	u32 Severity;
-	u8 ModuleName[MAX_MODULE_NAME_SIZE];
-	u32 LineNumber;
-	DIAG_EFI_TIME Timestamp;	/* Size = 16 bytes */
-	u32 PartitionNumber;	/* Filled in by Diag Switch as pool blocks are
+struct diag_channel_event {
+	u32 event_id;
+	u32 severity;
+	u8 module_name[MAX_MODULE_NAME_SIZE];
+	u32 line_number;
+	struct diag_efi_time timestamp;	/* Size = 16 bytes */
+	u32 partition_number;	/* Filled in by Diag Switch as pool blocks are
 				 * filled */
-	u16 VirtualProcessorNumber;
-	u16 LogicalProcessorNumber;
-	u8 ComponentType;	/* ULTRA_COMPONENT_TYPES */
-	u8 Subsystem;
-	u16 Reserved0;		/* pad to u64 alignment */
-	u32 BlockNumber;	/* filled in by DiagSwitch as pool blocks are
+	u16 vcpu_number;
+	u16 lcpu_number;
+	u8 component_type;	/* ULTRA_COMPONENT_TYPES */
+	u8 subsystem;
+	u16 reserved0;		/* pad to u64 alignment */
+	u32 block_no;		/* filled in by DiagSwitch as pool blocks are
 				 * filled */
-	u32 BlockNumberHigh;
-	u32 EventNumber;	/* filled in by DiagSwitch as pool blocks are
+	u32 block_no_high;
+	u32 event_no;		/* filled in by DiagSwitch as pool blocks are
 				 * filled */
-	u32 EventNumberHigh;
+	u32 event_no_high;
 
-	/* The BlockNumber and EventNumber fields are set only by DiagSwitch
+	/* The block_no and event_no fields are set only by DiagSwitch
 	 * and referenced only by WinDiagDisplay formatting tool as
 	 * additional diagnostic information.  Other tools including
 	 * WinDiagDisplay currently ignore these 'Reserved' bytes. */
-	u8 Reserved[8];
-	u8 AdditionalInfo[MAX_ADDITIONAL_INFO_SIZE];
+	u8 reserved[8];
+	u8 additional_info[MAX_ADDITIONAL_INFO_SIZE];
 
-	/* NOTE: Changesto DIAG_CHANNEL_EVENT generally need to be reflected in
+	/* NOTE: Changes to diag_channel_event generally need to be reflected in
 	 * existing copies *
 	 * - for AppOS at
 	 * GuestLinux/visordiag_early/supervisor_diagchannel.h *
 	 * - for WinDiagDisplay at
 	 * EFI/Ultra/Tools/WinDiagDisplay/WinDiagDisplay/diagstruct.h */
-} DIAG_CHANNEL_EVENT;
+};
 
 /* Levels of severity for diagnostic events, in order from lowest severity to
 * highest (i.e. fatal errors are the most severe, and should always be logged,
@@ -201,7 +202,8 @@ typedef struct _DIAG_CHANNEL_EVENT  {
 * they are valid for controlling the amount of event data.  This enum is also
 * defined in DotNet\sParFramework\ControlFramework\ControlFramework.cs.  If a
 * change is made to this enum, they should also be reflected in that file.  */
-typedef enum  { DIAG_SEVERITY_ENUM_BEGIN = 0,
+enum diag_severity {
+		DIAG_SEVERITY_ENUM_BEGIN = 0,
 		DIAG_SEVERITY_OVERRIDE = DIAG_SEVERITY_ENUM_BEGIN,
 		DIAG_SEVERITY_VERBOSE = DIAG_SEVERITY_OVERRIDE,	/* 0 */
 		DIAG_SEVERITY_INFO = DIAG_SEVERITY_VERBOSE + 1,	/* 1 */
@@ -212,7 +214,7 @@ typedef enum  { DIAG_SEVERITY_ENUM_BEGIN = 0,
 		DIAG_SEVERITY_ENUM_END = DIAG_SEVERITY_SHUTOFF,	/* 5 */
 		DIAG_SEVERITY_NONFATAL_ERR = DIAG_SEVERITY_ERR,
 		DIAG_SEVERITY_FATAL_ERR = DIAG_SEVERITY_PRINT
-} DIAG_SEVERITY;
+};
 
 /* Event Cause enums
 *
@@ -233,26 +235,24 @@ typedef enum  { DIAG_SEVERITY_ENUM_BEGIN = 0,
 * If a change is made to this enum, they should also be reflected in that
 * file.  */
 
-
-
 /* A cause value "DIAG_CAUSE_FILE_XFER" together with a severity value of
 * "DIAG_SEVERITY_PRINT" (=4), is used for transferring text or binary file to
 * the Diag partition. This cause-severity combination will be used by Logger
 * DiagSwitch to segregate events into block types. The files are transferred in
-* 256 byte chunks maximum, in the AdditionalInfo field of the DIAG_CHANNEL_EVENT
+* 256 byte chunks maximum, in the AdditionalInfo field of the diag_channel_event
 * structure. In the file transfer mode, some event fields will have different
 * meaning: EventId specifies the file offset, severity specifies the block type,
 * ModuleName specifies the filename, LineNumber specifies the number of valid
 * data bytes in an event and AdditionalInfo contains up to 256 bytes of data. */
 
 /* The Diag DiagWriter appends event blocks to events.raw as today, and for data
- * blocks uses DIAG_CHANNEL_EVENT
+ * blocks uses diag_channel_event
  * PartitionNumber to extract and append 'AdditionalInfo' to filename (specified
  * by ModuleName). */
 
 /* The Dell PDiag uses this new mechanism to stash DSET .zip onto the
  * 'diagnostic' virtual disk.  */
-typedef enum  {
+enum diag_cause {
 	DIAG_CAUSE_UNKNOWN = 0,
 	DIAG_CAUSE_UNKNOWN_DEBUG = DIAG_CAUSE_UNKNOWN + 1,	/* 1 */
 	DIAG_CAUSE_DEBUG = DIAG_CAUSE_UNKNOWN_DEBUG + 1,	/* 2 */
@@ -264,7 +264,7 @@ typedef enum  {
 	DIAG_CAUSE_INTERNAL_ERROR = DIAG_CAUSE_INVALID_REQUEST + 1, /* 8 */
 	DIAG_CAUSE_FILE_XFER = DIAG_CAUSE_INTERNAL_ERROR + 1,	/* 9 */
 	DIAG_CAUSE_ENUM_END = DIAG_CAUSE_FILE_XFER	/* 9 */
-} DIAG_CAUSE;
+};
 
 /* Event Cause category defined into the byte 2 of Severity */
 #define CAUSE_DEBUG (DIAG_CAUSE_DEBUG << CAUSE_SHIFT_AMT)
@@ -344,7 +344,7 @@ typedef enum  {
 #define CAUSE_FILE_XFER_SEVERITY_PRINT \
 	(CAUSE_FILE_XFER | DIAG_SEVERITY_PRINT)
 
-/* Structure: DIAG_CHANNEL_PROTOCOL_HEADER
+/* Structure: diag_channel_protocol_header
  *
  * Purpose: Contains attributes that make up the header specific to the
  * DIAG_CHANNEL area.
@@ -362,12 +362,12 @@ typedef enum  {
  *			whether events are logged.  Any event's severity for a
  *			particular subsystem below this level will be discarded.
  */
-typedef struct _DIAG_CHANNEL_PROTOCOL_HEADER  {
-	volatile u32 DiagLock;
-	u8 IsChannelInitialized;
-	u8 Reserved[3];
-	u8 SubsystemSeverityFilter[64];
-} DIAG_CHANNEL_PROTOCOL_HEADER;
+struct diag_channel_protocol_header {
+	u32 diag_lock;
+	u8 channel_initialized;
+	u8 reserved[3];
+	u8 subsystem_severity_filter[64];
+};
 
 /* The Diagram for the Diagnostic Channel: */
 /* ----------------------- */
@@ -375,19 +375,20 @@ typedef struct _DIAG_CHANNEL_PROTOCOL_HEADER  {
 /* ----------------------- */
 /* | Signal Queue Header   |	Defined by SIGNAL_QUEUE_HEADER */
 /* ----------------------- */
-/* | DiagChannel Header    |	Defined by DIAG_CHANNEL_PROTOCOL_HEADER */
+/* | DiagChannel Header    |	Defined by diag_channel_protocol_header */
 /* ----------------------- */
-/* | Channel Event Info    |	Defined by (DIAG_CHANNEL_EVENT * MAX_EVENTS) */
+/* | Channel Event Info    |	Defined by diag_channel_event*MAX_EVENTS */
 /* ----------------------- */
 /* | Reserved              |	Reserved (pad out to 4MB) */
 /* ----------------------- */
 
 /* Offsets/sizes for diagnostic channel attributes... */
-#define DIAG_CH_QUEUE_HEADER_OFFSET (sizeof(ULTRA_CHANNEL_PROTOCOL))
-#define DIAG_CH_QUEUE_HEADER_SIZE (sizeof(SIGNAL_QUEUE_HEADER))
+#define DIAG_CH_QUEUE_HEADER_OFFSET (sizeof(struct channel_header))
+#define DIAG_CH_QUEUE_HEADER_SIZE (sizeof(struct signal_queue_header))
 #define DIAG_CH_PROTOCOL_HEADER_OFFSET \
 	(DIAG_CH_QUEUE_HEADER_OFFSET + DIAG_CH_QUEUE_HEADER_SIZE)
-#define DIAG_CH_PROTOCOL_HEADER_SIZE (sizeof(DIAG_CHANNEL_PROTOCOL_HEADER))
+#define DIAG_CH_PROTOCOL_HEADER_SIZE \
+	(sizeof(struct diag_channel_protocol_header))
 #define DIAG_CH_EVENT_OFFSET \
 	(DIAG_CH_PROTOCOL_HEADER_OFFSET + DIAG_CH_PROTOCOL_HEADER_SIZE)
 #define DIAG_CH_SIZE (4096 * 1024)
@@ -397,7 +398,7 @@ typedef struct _DIAG_CHANNEL_PROTOCOL_HEADER  {
 #define DIAG_CH_LRG_SIZE (2 * DIAG_CH_SIZE)	/* 8 MB */
 
 /*
- * Structure: ULTRA_DIAG_CHANNEL_PROTOCOL
+ * Structure: spar_diag_channel_protocol
  *
  * Purpose: Contains attributes that make up the DIAG_CHANNEL memory.
  *
@@ -409,19 +410,18 @@ typedef struct _DIAG_CHANNEL_PROTOCOL_HEADER  {
  * store event.
  *
  * DiagChannelHeader: Diagnostic channel header info (see
- * DIAG_CHANNEL_PROTOCOL_HEADER comments).
+ * diag_channel_protocol_header comments).
  *
  * Events: Area where diagnostic events (up to MAX_EVENTS) are written.
  *
  *Reserved: Reserved area to allow for correct channel size padding.
 */
-typedef struct _ULTRA_DIAG_CHANNEL_PROTOCOL  {
-	ULTRA_CHANNEL_PROTOCOL CommonChannelHeader;
-	SIGNAL_QUEUE_HEADER QueueHeader;
-	DIAG_CHANNEL_PROTOCOL_HEADER DiagChannelHeader;
-	DIAG_CHANNEL_EVENT Events[(DIAG_CH_SIZE - DIAG_CH_EVENT_OFFSET) /
-				   sizeof(DIAG_CHANNEL_EVENT)];
-}
-ULTRA_DIAG_CHANNEL_PROTOCOL;
+struct spar_diag_channel_protocol  {
+	struct channel_header common_channel_header;
+	struct signal_queue_header queue_header;
+	struct diag_channel_protocol_header diag_channel_header;
+	struct diag_channel_event events[(DIAG_CH_SIZE - DIAG_CH_EVENT_OFFSET) /
+				   sizeof(struct diag_channel_event)];
+};
 
 #endif
diff --git a/drivers/staging/unisys/common-spar/include/channels/iochannel.h b/drivers/staging/unisys/common-spar/include/channels/iochannel.h
index b1dd73d1f42c..eb7efe484f6f 100644
--- a/drivers/staging/unisys/common-spar/include/channels/iochannel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/iochannel.h
@@ -8,7 +8,6 @@
 * this file.  Note: Everything is OS-independent because this file is
 * used by Windows, Linux and possible EFI drivers.  */
 
-
 /*
 * Communication flow between the IOPart and GuestPart uses the channel headers
 * channel state.  The following states are currently being used:
@@ -60,42 +59,22 @@
 #define ULTRA_VNIC_CHANNEL_PROTOCOL_VERSIONID 2
 #define ULTRA_VSWITCH_CHANNEL_PROTOCOL_VERSIONID 1
 
-#define ULTRA_VHBA_CHANNEL_OK_CLIENT(pChannel, logCtx)			\
-	(ULTRA_check_channel_client(pChannel, UltraVhbaChannelProtocolGuid, \
-				    "vhba", MIN_IO_CHANNEL_SIZE,	\
-				    ULTRA_VHBA_CHANNEL_PROTOCOL_VERSIONID, \
-				    ULTRA_VHBA_CHANNEL_PROTOCOL_SIGNATURE, \
-				    __FILE__, __LINE__, logCtx))
-#define ULTRA_VHBA_CHANNEL_OK_SERVER(actualBytes, logCtx)		\
-	(ULTRA_check_channel_server(UltraVhbaChannelProtocolGuid,	\
-				    "vhba", MIN_IO_CHANNEL_SIZE, actualBytes, \
-				    __FILE__, __LINE__, logCtx))
-#define ULTRA_VNIC_CHANNEL_OK_CLIENT(pChannel, logCtx)			\
-	(ULTRA_check_channel_client(pChannel, UltraVnicChannelProtocolGuid, \
-				    "vnic", MIN_IO_CHANNEL_SIZE,	\
-				    ULTRA_VNIC_CHANNEL_PROTOCOL_VERSIONID, \
-				    ULTRA_VNIC_CHANNEL_PROTOCOL_SIGNATURE, \
-				    __FILE__, __LINE__, logCtx))
-#define ULTRA_VNIC_CHANNEL_OK_SERVER(actualBytes, logCtx)		\
-	(ULTRA_check_channel_server(UltraVnicChannelProtocolGuid,	\
-				    "vnic", MIN_IO_CHANNEL_SIZE, actualBytes, \
-				    __FILE__, __LINE__, logCtx))
-#define ULTRA_VSWITCH_CHANNEL_OK_CLIENT(pChannel, logCtx)		\
-	(ULTRA_check_channel_client(pChannel, UltraVswitchChannelProtocolGuid, \
-				    "vswitch", MIN_IO_CHANNEL_SIZE,	\
-				    ULTRA_VSWITCH_CHANNEL_PROTOCOL_VERSIONID, \
-				    ULTRA_VSWITCH_CHANNEL_PROTOCOL_SIGNATURE, \
-				    __FILE__, __LINE__, logCtx))
-#define ULTRA_VSWITCH_CHANNEL_OK_SERVER(actualBytes, logCtx)          \
-	(ULTRA_check_channel_server(UltraVswitchChannelProtocolGuid,	\
-				    "vswitch", MIN_IO_CHANNEL_SIZE,	\
-				    actualBytes,		    \
-				    __FILE__, __LINE__, logCtx))
+#define SPAR_VHBA_CHANNEL_OK_CLIENT(ch)			\
+	(spar_check_channel_client(ch, spar_vhba_channel_protocol_uuid, \
+				   "vhba", MIN_IO_CHANNEL_SIZE,	\
+				   ULTRA_VHBA_CHANNEL_PROTOCOL_VERSIONID, \
+				   ULTRA_VHBA_CHANNEL_PROTOCOL_SIGNATURE))
+
+#define SPAR_VNIC_CHANNEL_OK_CLIENT(ch)			\
+	(spar_check_channel_client(ch, spar_vnic_channel_protocol_uuid, \
+				   "vnic", MIN_IO_CHANNEL_SIZE,	\
+				   ULTRA_VNIC_CHANNEL_PROTOCOL_VERSIONID, \
+				   ULTRA_VNIC_CHANNEL_PROTOCOL_SIGNATURE))
+
 /*
 * Everything necessary to handle SCSI & NIC traffic between Guest Partition and
 * IO Partition is defined below.  */
 
-
 /*
 * Defines and enums.
 */
@@ -172,8 +151,9 @@
 						 * SCSI Host value */
 
 /* various types of network packets that can be sent in cmdrsp */
-typedef enum { NET_RCV_POST = 0,	/* submit buffer to hold receiving
-					 * incoming packet */
+enum net_types {
+	NET_RCV_POST = 0,	/* submit buffer to hold receiving
+				 * incoming packet */
 	/* virtnic -> uisnic */
 	NET_RCV,		/* incoming packet received */
 	/* uisnic -> virtpci */
@@ -195,7 +175,7 @@ typedef enum { NET_RCV_POST = 0,	/* submit buffer to hold receiving
 				 * its MAC addr */
 	NET_MACADDR_ACK,	/* MAC address  */
 
-} NET_TYPES;
+};
 
 #define		ETH_HEADER_SIZE 14	/* size of ethernet header */
 
@@ -211,19 +191,23 @@ typedef enum { NET_RCV_POST = 0,	/* submit buffer to hold receiving
 #define MAX_MACADDR_LEN 6	/* number of bytes in MAC address */
 #endif				/* MAX_MACADDR_LEN */
 
-#define ETH_IS_LOCALLY_ADMINISTERED(Address) \
-	(((u8 *) (Address))[0] & ((u8) 0x02))
+#define ETH_IS_LOCALLY_ADMINISTERED(address) \
+	(((u8 *)(address))[0] & ((u8)0x02))
 #define NIC_VENDOR_ID 0x0008000B
 
 /* various types of scsi task mgmt commands  */
-typedef enum { TASK_MGMT_ABORT_TASK =
-	    1, TASK_MGMT_BUS_RESET, TASK_MGMT_LUN_RESET,
-	    TASK_MGMT_TARGET_RESET,
-} TASK_MGMT_TYPES;
+enum task_mgmt_types {
+	TASK_MGMT_ABORT_TASK = 1,
+	TASK_MGMT_BUS_RESET,
+	TASK_MGMT_LUN_RESET,
+	TASK_MGMT_TARGET_RESET,
+};
 
 /* various types of vdisk mgmt commands  */
-typedef enum { VDISK_MGMT_ACQUIRE = 1, VDISK_MGMT_RELEASE,
-} VDISK_MGMT_TYPES;
+enum vdisk_mgmt_types {
+	VDISK_MGMT_ACQUIRE = 1,
+	VDISK_MGMT_RELEASE,
+};
 
 /* this is used in the vdest field  */
 #define VDEST_ALL 0xFFFF
@@ -242,7 +226,6 @@ typedef enum { VDISK_MGMT_ACQUIRE = 1, VDISK_MGMT_RELEASE,
 /*
  * structs with pragma pack  */
 
-
 /* ///////////// BEGIN PRAGMA PACK PUSH 1 ///////////////////////// */
 /* ///////////// ONLY STRUCT TYPE SHOULD BE BELOW */
 
@@ -377,16 +360,16 @@ struct uiscmdrsp_scsi {
 	do {								\
 		memset(buf, 0,						\
 		       MINNUM(len,					\
-			      (unsigned int) NO_DISK_INQUIRY_RESULT_LEN)); \
-		buf[2] = (u8) SCSI_SPC2_VER;				\
+			      (unsigned int)NO_DISK_INQUIRY_RESULT_LEN)); \
+		buf[2] = (u8)SCSI_SPC2_VER;				\
 		if (lun == 0) {						\
-			buf[0] = (u8) lun0notpresent;			\
-			buf[3] = (u8) DEV_HISUPPORT;			\
+			buf[0] = (u8)lun0notpresent;			\
+			buf[3] = (u8)DEV_HISUPPORT;			\
 		} else							\
-			buf[0] = (u8) notpresent;			\
-		buf[4] = (u8) (						\
+			buf[0] = (u8)notpresent;			\
+		buf[4] = (u8)(						\
 			MINNUM(len,					\
-			       (unsigned int) NO_DISK_INQUIRY_RESULT_LEN) - 5);	\
+			       (unsigned int)NO_DISK_INQUIRY_RESULT_LEN) - 5);\
 		if (len >= NO_DISK_INQUIRY_RESULT_LEN) {		\
 			buf[8] = 'D';					\
 			buf[9] = 'E';					\
@@ -410,12 +393,10 @@ struct uiscmdrsp_scsi {
 		}							\
 	} while (0)
 
-
 /*
 * Struct & Defines to support sense information.
 */
 
-
 /* The following struct is returned in sensebuf field in uiscmdrsp_scsi.  It is
 * initialized in exactly the manner that is recommended in Windows (hence the
 * odd values).
@@ -429,21 +410,21 @@ struct uiscmdrsp_scsi {
 * AdditionalSenseLength		contains will be sizeof(sense_data)-8=10.
 */
 struct sense_data {
-	u8 ErrorCode:7;
-	u8 Valid:1;
-	u8 SegmentNumber;
-	u8 SenseKey:4;
-	u8 Reserved:1;
-	u8 IncorrectLength:1;
-	u8 EndOfMedia:1;
-	u8 FileMark:1;
-	u8 Information[4];
-	u8 AdditionalSenseLength;
-	u8 CommandSpecificInformation[4];
-	u8 AdditionalSenseCode;
-	u8 AdditionalSenseCodeQualifier;
-	u8 FieldReplaceableUnitCode;
-	u8 SenseKeySpecific[3];
+	u8 errorcode:7;
+	u8 valid:1;
+	u8 segment_number;
+	u8 sense_key:4;
+	u8 reserved:1;
+	u8 incorrect_length:1;
+	u8 end_of_media:1;
+	u8 file_mark:1;
+	u8 information[4];
+	u8 additional_sense_length;
+	u8 command_specific_information[4];
+	u8 additional_sense_code;
+	u8 additional_sense_code_qualifier;
+	u8 fru_code;
+	u8 sense_key_specific[3];
 };
 
 /* some SCSI ADSENSE codes */
@@ -484,7 +465,6 @@ struct net_pkt_xmt {
 						 * each fragment */
 	char ethhdr[ETH_HEADER_SIZE];	/* the ethernet header  */
 	struct {
-
 		    /* these are needed for csum at uisnic end */
 		u8 valid;	/* 1 = rest of this struct is valid - else
 				 * ignore */
@@ -528,13 +508,12 @@ struct net_pkt_rcvpost {
 	    * to be describable */
 	    struct phys_info frag;	/* physical page information for the
 					 * single fragment 2K rcv buf */
-	    u64 UniqueNum;		/* This is used to make sure that
+	    u64 unique_num;		/* This is used to make sure that
 					 * receive posts are returned to  */
 	    /* the Adapter which sent them origonally. */
 };
 
 struct net_pkt_rcv {
-
 	/* the number of receive buffers that can be chained  */
 	/* is based on max mtu and size of each rcv buf */
 	u32 rcv_done_len;	/* length of received data */
@@ -544,8 +523,8 @@ struct net_pkt_rcv {
 						 * that must be chained; */
 	/* each entry is a receive buffer provided by NET_RCV_POST. */
 	/* NOTE: first rcvbuf in the chain will also be provided in net.buf. */
-	u64 UniqueNum;
-	u32 RcvsDroppedDelta;
+	u64 unique_num;
+	u32 rcvs_dropped_delta;
 };
 
 struct net_pkt_enbdis {
@@ -560,7 +539,7 @@ struct net_pkt_macaddr {
 
 /* cmd rsp packet used for VNIC network traffic  */
 struct uiscmdrsp_net {
-	NET_TYPES type;
+	enum net_types type;
 	void *buf;
 	union {
 		struct net_pkt_xmt xmt;	/* used for NET_XMIT */
@@ -576,7 +555,7 @@ struct uiscmdrsp_net {
 };
 
 struct uiscmdrsp_scsitaskmgmt {
-	TASK_MGMT_TYPES tasktype;
+	enum task_mgmt_types tasktype;
 
 	    /* the type of task */
 	struct uisscsi_dest vdest;
@@ -594,7 +573,7 @@ struct uiscmdrsp_scsitaskmgmt {
 	    * For windows guests, this is a pointer to a location that a waiting
 	    * thread is testing to see if the taskmgmt command has completed.
 	    * When the rsp is received by guest, the thread receiving the
-	    * response uses this to notify the the thread waiting for taskmgmt
+	    * response uses this to notify the thread waiting for taskmgmt
 	    * command completion.  Its value is preserved by iopart & returned
 	    * as is in the task mgmt rsp. */
 	void *notifyresult;
@@ -615,7 +594,7 @@ struct uiscmdrsp_scsitaskmgmt {
 /* Note that the vHba pointer is not used by the Client/Guest side. */
 struct uiscmdrsp_disknotify {
 	u8 add;		/* 0-remove, 1-add */
-	void *vHba;		/* Pointer to vhba_info for channel info to
+	void *v_hba;		/* Pointer to vhba_info for channel info to
 				 * route msg */
 	u32 channel, id, lun;	/* SCSI Path of Disk to added or removed */
 };
@@ -623,7 +602,7 @@ struct uiscmdrsp_disknotify {
 /* The following is used by virthba/vSCSI to send the Acquire/Release commands
 * to the IOVM.  */
 struct uiscmdrsp_vdiskmgmt {
-	VDISK_MGMT_TYPES vdisktype;
+	enum vdisk_mgmt_types vdisktype;
 
 	    /* the type of task */
 	struct uisscsi_dest vdest;
@@ -641,7 +620,7 @@ struct uiscmdrsp_vdiskmgmt {
 	    * For windows guests, this is a pointer to a location that a waiting
 	    * thread is testing to see if the taskmgmt command has completed.
 	    * When the rsp is received by guest, the thread receiving the
-	    * response uses this to notify the the thread waiting for taskmgmt
+	    * response uses this to notify the thread waiting for taskmgmt
 	    * command completion.  Its value is preserved by iopart & returned
 	    * as is in the task mgmt rsp. */
 	void *notifyresult;
@@ -683,11 +662,11 @@ struct uiscmdrsp {
 
 /* This is just the header of the IO channel.  It is assumed that directly after
 * this header there is a large region of memory which contains the command and
-* response queues as specified in cmdQ and rspQ SIGNAL_QUEUE_HEADERS. */
-typedef struct _ULTRA_IO_CHANNEL_PROTOCOL {
-	CHANNEL_HEADER ChannelHeader;
-	SIGNAL_QUEUE_HEADER cmdQ;
-	SIGNAL_QUEUE_HEADER rspQ;
+* response queues as specified in cmd_q and rsp_q SIGNAL_QUEUE_HEADERS. */
+struct spar_io_channel_protocol {
+	struct channel_header channel_header;
+	struct signal_queue_header cmd_q;
+	struct signal_queue_header rsp_q;
 	union {
 		struct {
 			struct vhba_wwnn wwnn;	/* 8 bytes */
@@ -697,14 +676,14 @@ typedef struct _ULTRA_IO_CHANNEL_PROTOCOL {
 			u8 macaddr[MAX_MACADDR_LEN];	/* 6 bytes */
 			u32 num_rcv_bufs;	/* 4 */
 			u32 mtu;	/* 4 */
-			uuid_le zoneGuid;	/* 16 */
+			uuid_le zone_uuid;	/* 16 */
 		} vnic;		/* total     30 */
 	};
 
 #define MAX_CLIENTSTRING_LEN 1024
-	 u8 clientString[MAX_CLIENTSTRING_LEN];	/* NULL terminated - so holds
+	 u8 client_string[MAX_CLIENTSTRING_LEN];/* NULL terminated - so holds
 						 * max - 1 bytes */
-} ULTRA_IO_CHANNEL_PROTOCOL;
+};
 
 #pragma pack(pop)
 /* ///////////// END PRAGMA PACK PUSH 1 /////////////////////////// */
@@ -733,144 +712,17 @@ typedef struct _ULTRA_IO_CHANNEL_PROTOCOL {
 * INLINE functions for initializing and accessing I/O data channels
 */
 
-
-#define NUMSIGNALS(x, q) (((ULTRA_IO_CHANNEL_PROTOCOL *)(x))->q.MaxSignalSlots)
-#define SIZEOF_PROTOCOL (COVER(sizeof(ULTRA_IO_CHANNEL_PROTOCOL), 64))
+#define SIZEOF_PROTOCOL (COVER(sizeof(struct spar_io_channel_protocol), 64))
 #define SIZEOF_CMDRSP (COVER(sizeof(struct uiscmdrsp), 64))
 
-#define IO_CHANNEL_SIZE(x) COVER(SIZEOF_PROTOCOL + \
-				 (NUMSIGNALS(x, cmdQ) + \
-				  NUMSIGNALS(x, rspQ)) * SIZEOF_CMDRSP, 4096)
 #define MIN_IO_CHANNEL_SIZE COVER(SIZEOF_PROTOCOL + \
 				  2 * MIN_NUMSIGNALS * SIZEOF_CMDRSP, 4096)
-#ifdef __GNUC__
-/* These defines should only ever be used in service partitons */
-/* because they rely on the size of uiscmdrsp */
-#define QSLOTSFROMBYTES(bytes) (((bytes-SIZEOF_PROTOCOL)/2)/SIZEOF_CMDRSP)
-#define QSIZEFROMBYTES(bytes) (QSLOTSFROMBYTES(bytes)*SIZEOF_CMDRSP)
-#define SignalQInit(x)						\
-	do {							\
-		x->cmdQ.Size = QSIZEFROMBYTES(x->ChannelHeader.Size);	\
-		x->cmdQ.oSignalBase = SIZEOF_PROTOCOL -			\
-			offsetof(ULTRA_IO_CHANNEL_PROTOCOL, cmdQ);	\
-		x->cmdQ.SignalSize = SIZEOF_CMDRSP;			\
-		x->cmdQ.MaxSignalSlots =				\
-			QSLOTSFROMBYTES(x->ChannelHeader.Size);		\
-		x->cmdQ.MaxSignals = x->cmdQ.MaxSignalSlots - 1;	\
-		x->rspQ.Size = QSIZEFROMBYTES(x->ChannelHeader.Size);	\
-		x->rspQ.oSignalBase =					\
-			(SIZEOF_PROTOCOL + x->cmdQ.Size) -		\
-			offsetof(ULTRA_IO_CHANNEL_PROTOCOL, rspQ);	\
-		x->rspQ.SignalSize = SIZEOF_CMDRSP;			\
-		x->rspQ.MaxSignalSlots =				\
-			QSLOTSFROMBYTES(x->ChannelHeader.Size);		\
-		x->rspQ.MaxSignals = x->rspQ.MaxSignalSlots - 1;	\
-		x->ChannelHeader.oChannelSpace =			\
-			offsetof(ULTRA_IO_CHANNEL_PROTOCOL, cmdQ);	\
-	} while (0)
-
-#define INIT_CLIENTSTRING(chan, type, clientStr, clientStrLen)	\
-	do {								\
-		if (clientStr) {					\
-			chan->ChannelHeader.oClientString =		\
-				offsetof(type, clientString);		\
-			memcpy(chan->clientString, clientStr,		\
-			       MINNUM(clientStrLen,			\
-				      (u32) (MAX_CLIENTSTRING_LEN - 1))); \
-			chan->clientString[MINNUM(clientStrLen,		\
-						  (u32) (MAX_CLIENTSTRING_LEN \
-							 - 1))]		\
-				= '\0';					\
-		}							\
-		else							\
-			if (clientStrLen > 0)				\
-				return 0;				\
-	} while (0)
-
-
-#define ULTRA_IO_CHANNEL_SERVER_READY(x, chanId, logCtx) \
-	ULTRA_CHANNEL_SERVER_TRANSITION(x, chanId, SrvState, CHANNELSRV_READY, \
-					logCtx)
-
-#define ULTRA_IO_CHANNEL_SERVER_NOTREADY(x, chanId, logCtx)	\
-	ULTRA_CHANNEL_SERVER_TRANSITION(x, chanId, SrvState, \
-					CHANNELSRV_UNINITIALIZED, logCtx)
-
-static inline int ULTRA_VHBA_init_channel(ULTRA_IO_CHANNEL_PROTOCOL *x,
-					      struct vhba_wwnn *wwnn,
-					      struct vhba_config_max *max,
-					      unsigned char *clientStr,
-					      u32 clientStrLen, u64 bytes)  {
-	memset(x, 0, sizeof(ULTRA_IO_CHANNEL_PROTOCOL));
-	x->ChannelHeader.VersionId = ULTRA_VHBA_CHANNEL_PROTOCOL_VERSIONID;
-	x->ChannelHeader.Signature = ULTRA_VHBA_CHANNEL_PROTOCOL_SIGNATURE;
-	x->ChannelHeader.SrvState = CHANNELSRV_UNINITIALIZED;
-	x->ChannelHeader.HeaderSize = sizeof(x->ChannelHeader);
-	x->ChannelHeader.Size = COVER(bytes, 4096);
-	x->ChannelHeader.Type = UltraVhbaChannelProtocolGuid;
-	x->ChannelHeader.ZoneGuid = NULL_UUID_LE;
-	x->vhba.wwnn = *wwnn;
-	x->vhba.max = *max;
-	INIT_CLIENTSTRING(x, ULTRA_IO_CHANNEL_PROTOCOL, clientStr,
-			  clientStrLen);
-	SignalQInit(x);
-	if ((x->cmdQ.MaxSignalSlots > MAX_NUMSIGNALS) ||
-	     (x->rspQ.MaxSignalSlots > MAX_NUMSIGNALS)) {
-		return 0;
-	}
-	if ((x->cmdQ.MaxSignalSlots < MIN_NUMSIGNALS) ||
-	     (x->rspQ.MaxSignalSlots < MIN_NUMSIGNALS)) {
-		return 0;
-	}
-	return 1;
-}
-
-static inline void ULTRA_VHBA_set_max(ULTRA_IO_CHANNEL_PROTOCOL *x,
-				      struct vhba_config_max *max)  {
-	x->vhba.max = *max;
-}
-
-static inline int ULTRA_VNIC_init_channel(ULTRA_IO_CHANNEL_PROTOCOL *x,
-						 unsigned char *macaddr,
-						 u32 num_rcv_bufs, u32 mtu,
-						 uuid_le zoneGuid,
-						 unsigned char *clientStr,
-						 u32 clientStrLen,
-						 u64 bytes)  {
-	memset(x, 0, sizeof(ULTRA_IO_CHANNEL_PROTOCOL));
-	x->ChannelHeader.VersionId = ULTRA_VNIC_CHANNEL_PROTOCOL_VERSIONID;
-	x->ChannelHeader.Signature = ULTRA_VNIC_CHANNEL_PROTOCOL_SIGNATURE;
-	x->ChannelHeader.SrvState = CHANNELSRV_UNINITIALIZED;
-	x->ChannelHeader.HeaderSize = sizeof(x->ChannelHeader);
-	x->ChannelHeader.Size = COVER(bytes, 4096);
-	x->ChannelHeader.Type = UltraVnicChannelProtocolGuid;
-	x->ChannelHeader.ZoneGuid = NULL_UUID_LE;
-	memcpy(x->vnic.macaddr, macaddr, MAX_MACADDR_LEN);
-	x->vnic.num_rcv_bufs = num_rcv_bufs;
-	x->vnic.mtu = mtu;
-	x->vnic.zoneGuid = zoneGuid;
-	INIT_CLIENTSTRING(x, ULTRA_IO_CHANNEL_PROTOCOL, clientStr,
-			   clientStrLen);
-	SignalQInit(x);
-	if ((x->cmdQ.MaxSignalSlots > MAX_NUMSIGNALS) ||
-	     (x->rspQ.MaxSignalSlots > MAX_NUMSIGNALS)) {
-		return 0;
-	}
-	if ((x->cmdQ.MaxSignalSlots < MIN_NUMSIGNALS) ||
-	     (x->rspQ.MaxSignalSlots < MIN_NUMSIGNALS)) {
-		return 0;
-	}
-	return 1;
-}
-
-#endif	/* __GNUC__ */
 
 /*
 * INLINE function for expanding a guest's pfn-off-size into multiple 4K page
 * pfn-off-size entires.
 */
 
-
 /* we deal with 4K page sizes when we it comes to passing page information
  * between */
 /* Guest and IOPartition. */
@@ -900,13 +752,12 @@ add_physinfo_entries(u32 inp_pfn,	/* input - specifies the pfn to be used
 
 	firstlen = PI_PAGE_SIZE - inp_off;
 	if (inp_len <= firstlen) {
-
 		/* the input entry spans only one page - add as is */
 		if (index >= max_pi_arr_entries)
 			return 0;
 		pi_arr[index].pi_pfn = inp_pfn;
-		pi_arr[index].pi_off = (u16) inp_off;
-		pi_arr[index].pi_len = (u16) inp_len;
+		pi_arr[index].pi_off = (u16)inp_off;
+		pi_arr[index].pi_len = (u16)inp_len;
 		    return index + 1;
 	}
 
@@ -924,9 +775,8 @@ add_physinfo_entries(u32 inp_pfn,	/* input - specifies the pfn to be used
 		else {
 			pi_arr[index + i].pi_off = 0;
 			pi_arr[index + i].pi_len =
-			    (u16) MINNUM(len, (u32) PI_PAGE_SIZE);
+			    (u16)MINNUM(len, (u32)PI_PAGE_SIZE);
 		}
-
 	}
 	return index + i;
 }
diff --git a/drivers/staging/unisys/common-spar/include/channels/vbuschannel.h b/drivers/staging/unisys/common-spar/include/channels/vbuschannel.h
index 1231c454176f..2c42ce16e0cf 100644
--- a/drivers/staging/unisys/common-spar/include/channels/vbuschannel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/vbuschannel.h
@@ -28,65 +28,61 @@
 #include "channel.h"
 
 /* {193b331b-c58f-11da-95a9-00e08161165f} */
-#define ULTRA_VBUS_CHANNEL_PROTOCOL_GUID \
+#define SPAR_VBUS_CHANNEL_PROTOCOL_UUID \
 		UUID_LE(0x193b331b, 0xc58f, 0x11da, \
 				0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f)
-static const uuid_le UltraVbusChannelProtocolGuid =
-	ULTRA_VBUS_CHANNEL_PROTOCOL_GUID;
+static const uuid_le spar_vbus_channel_protocol_uuid =
+	SPAR_VBUS_CHANNEL_PROTOCOL_UUID;
 
-#define ULTRA_VBUS_CHANNEL_PROTOCOL_SIGNATURE ULTRA_CHANNEL_PROTOCOL_SIGNATURE
+#define SPAR_VBUS_CHANNEL_PROTOCOL_SIGNATURE ULTRA_CHANNEL_PROTOCOL_SIGNATURE
 
 /* Must increment this whenever you insert or delete fields within this channel
 * struct.  Also increment whenever you change the meaning of fields within this
 * channel struct so as to break pre-existing software.  Note that you can
 * usually add fields to the END of the channel struct withOUT needing to
 * increment this. */
-#define ULTRA_VBUS_CHANNEL_PROTOCOL_VERSIONID 1
+#define SPAR_VBUS_CHANNEL_PROTOCOL_VERSIONID 1
 
-#define ULTRA_VBUS_CHANNEL_OK_CLIENT(pChannel, logCtx)       \
-	(ULTRA_check_channel_client(pChannel,				\
-				    UltraVbusChannelProtocolGuid,	\
-				    "vbus",				\
-				    sizeof(ULTRA_VBUS_CHANNEL_PROTOCOL), \
-				    ULTRA_VBUS_CHANNEL_PROTOCOL_VERSIONID, \
-				    ULTRA_VBUS_CHANNEL_PROTOCOL_SIGNATURE, \
-				    __FILE__, __LINE__, logCtx))
-
-#define ULTRA_VBUS_CHANNEL_OK_SERVER(actualBytes, logCtx)    \
-	(ULTRA_check_channel_server(UltraVbusChannelProtocolGuid,	\
-				    "vbus",				\
-				    sizeof(ULTRA_VBUS_CHANNEL_PROTOCOL), \
-				    actualBytes,			\
-				    __FILE__, __LINE__, logCtx))
+#define SPAR_VBUS_CHANNEL_OK_CLIENT(ch)       \
+	spar_check_channel_client(ch,				\
+				   spar_vbus_channel_protocol_uuid,	\
+				   "vbus",				\
+				   sizeof(struct spar_vbus_channel_protocol),\
+				   SPAR_VBUS_CHANNEL_PROTOCOL_VERSIONID, \
+				   SPAR_VBUS_CHANNEL_PROTOCOL_SIGNATURE)
 
+#define SPAR_VBUS_CHANNEL_OK_SERVER(actual_bytes)    \
+	(spar_check_channel_server(spar_vbus_channel_protocol_uuid,	\
+				   "vbus",				\
+				   sizeof(struct ultra_vbus_channel_protocol),\
+				   actual_bytes))
 
 #pragma pack(push, 1)		/* both GCC and VC now allow this pragma */
-typedef struct _ULTRA_VBUS_HEADERINFO {
-	u32 structBytes;	/* size of this struct in bytes */
-	u32 deviceInfoStructBytes;	/* sizeof(ULTRA_VBUS_DEVICEINFO) */
-	u32 devInfoCount;	/* num of items in DevInfo member */
+struct spar_vbus_headerinfo {
+	u32 struct_bytes;	/* size of this struct in bytes */
+	u32 device_info_struct_bytes;	/* sizeof(ULTRA_VBUS_DEVICEINFO) */
+	u32 dev_info_count;	/* num of items in DevInfo member */
 	/* (this is the allocated size) */
-	u32 chpInfoByteOffset;	/* byte offset from beginning of this struct */
-	/* to the the ChpInfo struct (below) */
-	u32 busInfoByteOffset;	/* byte offset from beginning of this struct */
-	/* to the the BusInfo struct (below) */
-	u32 devInfoByteOffset;	/* byte offset from beginning of this struct */
-	/* to the the DevInfo array (below) */
+	u32 chp_info_offset;	/* byte offset from beginning of this struct */
+	/* to the ChpInfo struct (below) */
+	u32 bus_info_offset;	/* byte offset from beginning of this struct */
+	/* to the BusInfo struct (below) */
+	u32 dev_info_offset;	/* byte offset from beginning of this struct */
+	/* to the DevInfo array (below) */
 	u8 reserved[104];
-} ULTRA_VBUS_HEADERINFO;
+};
 
-typedef struct _ULTRA_VBUS_CHANNEL_PROTOCOL {
-	ULTRA_CHANNEL_PROTOCOL ChannelHeader;	/* initialized by server */
-	ULTRA_VBUS_HEADERINFO HdrInfo;	/* initialized by server */
+struct spar_vbus_channel_protocol {
+	struct channel_header channel_header;	/* initialized by server */
+	struct spar_vbus_headerinfo hdr_info;	/* initialized by server */
 	/* the remainder of this channel is filled in by the client */
-	ULTRA_VBUS_DEVICEINFO ChpInfo;	/* describes client chipset device and
-					 * driver */
-	ULTRA_VBUS_DEVICEINFO BusInfo;	/* describes client bus device and
-					 * driver */
-	ULTRA_VBUS_DEVICEINFO DevInfo[0];	/* describes client device and
-						 * driver for */
-	/* each device on the bus */
-} ULTRA_VBUS_CHANNEL_PROTOCOL;
+	struct ultra_vbus_deviceinfo chp_info;
+	/* describes client chipset device and driver */
+	struct ultra_vbus_deviceinfo bus_info;
+	/* describes client bus device and driver */
+	struct ultra_vbus_deviceinfo dev_info[0];
+	/* describes client device and driver for each device on the bus */
+};
 
 #define VBUS_CH_SIZE_EXACT(MAXDEVICES) \
 	(sizeof(ULTRA_VBUS_CHANNEL_PROTOCOL) + ((MAXDEVICES) * \
diff --git a/drivers/staging/unisys/common-spar/include/vbusdeviceinfo.h b/drivers/staging/unisys/common-spar/include/vbusdeviceinfo.h
index 3bbdc2bb7ebf..9b6d3e69355c 100644
--- a/drivers/staging/unisys/common-spar/include/vbusdeviceinfo.h
+++ b/drivers/staging/unisys/common-spar/include/vbusdeviceinfo.h
@@ -25,13 +25,13 @@
  * It is filled in by the client side to provide info about the device
  * and driver from the client's perspective.
  */
-typedef struct _ULTRA_VBUS_DEVICEINFO {
-	u8 devType[16];		/* short string identifying the device type */
-	u8 drvName[16];		/* driver .sys file name */
-	u8 infoStrings[96];	/* sequence of tab-delimited id strings: */
+struct ultra_vbus_deviceinfo {
+	u8 devtype[16];		/* short string identifying the device type */
+	u8 drvname[16];		/* driver .sys file name */
+	u8 infostrs[96];	/* sequence of tab-delimited id strings: */
 	/* <DRIVER_REV> <DRIVER_VERTAG> <DRIVER_COMPILETIME> */
 	u8 reserved[128];	/* pad size to 256 bytes */
-} ULTRA_VBUS_DEVICEINFO;
+};
 
 #pragma pack(pop)
 
@@ -63,8 +63,9 @@ vbuschannel_sanitize_buffer(char *p, int remain, char *src, int srcmax)
 					p++;
 					remain--;
 					chars++;
-				} else if (p == NULL)
+				} else if (p == NULL) {
 					chars++;
+				}
 				nonprintable_streak = 0;
 			}
 			if (remain > 0) {
@@ -72,10 +73,12 @@ vbuschannel_sanitize_buffer(char *p, int remain, char *src, int srcmax)
 				p++;
 				remain--;
 				chars++;
-			} else if (p == NULL)
+			} else if (p == NULL) {
 				chars++;
-		} else
+			}
+		} else {
 			nonprintable_streak = 1;
+		}
 		src++;
 		srcmax--;
 	}
@@ -115,7 +118,7 @@ vbuschannel_itoa(char *p, int remain, int num)
 	}
 	/* form a backwards decimal ascii string in <s> */
 	while (num > 0) {
-		if (digits >= (int) sizeof(s))
+		if (digits >= (int)sizeof(s))
 			return 0;
 		s[digits++] = (num % 10) + '0';
 		num = num / 10;
@@ -147,15 +150,15 @@ vbuschannel_itoa(char *p, int remain, int num)
  * Returns the number of bytes written to <p>.
  */
 static inline int
-vbuschannel_devinfo_to_string(ULTRA_VBUS_DEVICEINFO *devinfo,
-				  char *p, int remain, int devix)
+vbuschannel_devinfo_to_string(struct ultra_vbus_deviceinfo *devinfo,
+			      char *p, int remain, int devix)
 {
 	char *psrc;
 	int nsrc, x, i, pad;
 	int chars = 0;
 
-	psrc = &(devinfo->devType[0]);
-	nsrc = sizeof(devinfo->devType);
+	psrc = &devinfo->devtype[0];
+	nsrc = sizeof(devinfo->devtype);
 	if (vbuschannel_sanitize_buffer(NULL, 0, psrc, nsrc) <= 0)
 		return 0;
 
@@ -184,8 +187,8 @@ vbuschannel_devinfo_to_string(ULTRA_VBUS_DEVICEINFO *devinfo,
 	VBUSCHANNEL_ADDACHAR(' ', p, remain, chars);
 
 	/* emit driver name */
-	psrc = &(devinfo->drvName[0]);
-	nsrc = sizeof(devinfo->drvName);
+	psrc = &devinfo->drvname[0];
+	nsrc = sizeof(devinfo->drvname);
 	x = vbuschannel_sanitize_buffer(p, remain, psrc, nsrc);
 	p += x;
 	remain -= x;
@@ -196,8 +199,8 @@ vbuschannel_devinfo_to_string(ULTRA_VBUS_DEVICEINFO *devinfo,
 	VBUSCHANNEL_ADDACHAR(' ', p, remain, chars);
 
 	/* emit strings */
-	psrc = &(devinfo->infoStrings[0]);
-	nsrc = sizeof(devinfo->infoStrings);
+	psrc = &devinfo->infostrs[0];
+	nsrc = sizeof(devinfo->infostrs);
 	x = vbuschannel_sanitize_buffer(p, remain, psrc, nsrc);
 	p += x;
 	remain -= x;
diff --git a/drivers/staging/unisys/common-spar/include/vmcallinterface.h b/drivers/staging/unisys/common-spar/include/vmcallinterface.h
index 0b5b5626af5a..78333719c496 100644
--- a/drivers/staging/unisys/common-spar/include/vmcallinterface.h
+++ b/drivers/staging/unisys/common-spar/include/vmcallinterface.h
@@ -33,7 +33,7 @@
 
 /* define subsystem number for AppOS, used in uislib driver  */
 #define MDS_APPOS 0x4000000000000000L	/* subsystem = 62 - AppOS */
-typedef enum {		/* VMCALL identification tuples  */
+enum vmcall_monitor_interface_method_tuple { /* VMCALL identification tuples  */
 	    /* Note: when a new VMCALL is added:
 	     * - the 1st 2 hex digits correspond to one of the
 	     *   VMCALL_MONITOR_INTERFACE types and
@@ -66,7 +66,7 @@ typedef enum {		/* VMCALL identification tuples  */
 						 * ULTRA_SERVICE_CAPABILITY_TIME
 						 * capable guest to make
 						 * VMCALL */
-} VMCALL_MONITOR_INTERFACE_METHOD_TUPLE;
+};
 
 #define VMCALL_SUCCESS 0
 #define VMCALL_SUCCESSFUL(result)	(result == 0)
@@ -76,12 +76,12 @@ typedef enum {		/* VMCALL identification tuples  */
 	__unisys_vmcall_gnuc(tuple, reg_ebx, reg_ecx)
 #define unisys_extended_vmcall(tuple, reg_ebx, reg_ecx, reg_edx) \
 	__unisys_extended_vmcall_gnuc(tuple, reg_ebx, reg_ecx, reg_edx)
-#define ISSUE_IO_VMCALL(InterfaceMethod, param, result) \
-	(result = unisys_vmcall(InterfaceMethod, (param) & 0xFFFFFFFF,	\
+#define ISSUE_IO_VMCALL(method, param, result) \
+	(result = unisys_vmcall(method, (param) & 0xFFFFFFFF,	\
 				(param) >> 32))
-#define ISSUE_IO_EXTENDED_VMCALL(InterfaceMethod, param1, param2,	\
+#define ISSUE_IO_EXTENDED_VMCALL(method, param1, param2,	\
 				 param3, result)			\
-	(result = unisys_extended_vmcall(InterfaceMethod, param1,	\
+	(result = unisys_extended_vmcall(method, param1,	\
 					 param2, param3))
 
     /* The following uses VMCALL_POST_CODE_LOGEVENT interface but is currently
@@ -107,21 +107,20 @@ struct phys_info {
 
 #pragma pack(pop)
 /* ///////////// END PRAGMA PACK PUSH 1 /////////////////////////// */
-typedef struct phys_info IO_DATA_STRUCTURE;
 
 /* ///////////// BEGIN PRAGMA PACK PUSH 1 ///////////////////////// */
 /* ///////////// ONLY STRUCT TYPE SHOULD BE BELOW */
 #pragma pack(push, 1)
 /* Parameters to VMCALL_IO_CONTROLVM_ADDR interface */
-typedef struct _VMCALL_IO_CONTROLVM_ADDR_PARAMS {
+struct vmcall_io_controlvm_addr_params {
 	    /* The Guest-relative physical address of the ControlVm channel.
 	    * This VMCall fills this in with the appropriate address. */
-	u64 ChannelAddress;	/* contents provided by this VMCALL (OUT) */
+	u64 address;	/* contents provided by this VMCALL (OUT) */
 	    /* the size of the ControlVm channel in bytes This VMCall fills this
 	    * in with the appropriate address. */
-	u32 ChannelBytes;	/* contents provided by this VMCALL (OUT) */
-	u8 Unused[4];		/* Unused Bytes in the 64-Bit Aligned Struct */
-} VMCALL_IO_CONTROLVM_ADDR_PARAMS;
+	u32 channel_bytes;	/* contents provided by this VMCALL (OUT) */
+	u8 unused[4];		/* Unused Bytes in the 64-Bit Aligned Struct */
+};
 
 #pragma pack(pop)
 /* ///////////// END PRAGMA PACK PUSH 1 /////////////////////////// */
@@ -130,11 +129,11 @@ typedef struct _VMCALL_IO_CONTROLVM_ADDR_PARAMS {
 /* ///////////// ONLY STRUCT TYPE SHOULD BE BELOW */
 #pragma pack(push, 1)
 /* Parameters to VMCALL_IO_DIAG_ADDR interface */
-typedef struct _VMCALL_IO_DIAG_ADDR_PARAMS {
+struct vmcall_io_diag_addr_params {
 	    /* The Guest-relative physical address of the diagnostic channel.
 	    * This VMCall fills this in with the appropriate address. */
-	u64 ChannelAddress;	/* contents provided by this VMCALL (OUT) */
-} VMCALL_IO_DIAG_ADDR_PARAMS;
+	u64 address;	/* contents provided by this VMCALL (OUT) */
+};
 
 #pragma pack(pop)
 /* ///////////// END PRAGMA PACK PUSH 1 /////////////////////////// */
@@ -143,25 +142,25 @@ typedef struct _VMCALL_IO_DIAG_ADDR_PARAMS {
 /* ///////////// ONLY STRUCT TYPE SHOULD BE BELOW */
 #pragma pack(push, 1)
 /* Parameters to VMCALL_IO_VISORSERIAL_ADDR interface */
-typedef struct _VMCALL_IO_VISORSERIAL_ADDR_PARAMS {
+struct vmcall_io_visorserial_addr_params {
 	    /* The Guest-relative physical address of the serial console
 	    * channel.  This VMCall fills this in with the appropriate
 	    * address. */
-	u64 ChannelAddress;	/* contents provided by this VMCALL (OUT) */
-} VMCALL_IO_VISORSERIAL_ADDR_PARAMS;
+	u64 address;	/* contents provided by this VMCALL (OUT) */
+};
 
 #pragma pack(pop)
 /* ///////////// END PRAGMA PACK PUSH 1 /////////////////////////// */
 
 /* Parameters to VMCALL_CHANNEL_MISMATCH interface */
-typedef struct _VMCALL_CHANNEL_VERSION_MISMATCH_PARAMS {
-	u8 ChannelName[32];	/* Null terminated string giving name of channel
+struct vmcall_channel_version_mismatch_params {
+	u8 chname[32];	/* Null terminated string giving name of channel
 				 * (IN) */
-	u8 ItemName[32];	/* Null terminated string giving name of
+	u8 item_name[32];	/* Null terminated string giving name of
 				 * mismatched item (IN) */
-	u32 SourceLineNumber;	/* line# where invoked. (IN) */
-	u8 SourceFileName[36];	/* source code where invoked - Null terminated
+	u32 line_no;		/* line# where invoked. (IN) */
+	u8 file_name[36];	/* source code where invoked - Null terminated
 				 * string (IN) */
-} VMCALL_CHANNEL_VERSION_MISMATCH_PARAMS;
+};
 
 #endif /* __IOMONINTF_H__ */
diff --git a/drivers/staging/unisys/include/timskmod.h b/drivers/staging/unisys/include/timskmod.h
index b14494ff6c1b..cff7983dab85 100644
--- a/drivers/staging/unisys/include/timskmod.h
+++ b/drivers/staging/unisys/include/timskmod.h
@@ -31,7 +31,6 @@
 #include <linux/slab.h>
 #include <linux/errno.h>
 #include <linux/interrupt.h>
-#include <linux/sched.h>
 #include <linux/wait.h>
 #include <linux/vmalloc.h>
 #include <linux/proc_fs.h>
@@ -71,7 +70,6 @@
 
 /** Try to evaulate the provided expression, and do a RETINT(x) iff
  *  the expression evaluates to < 0.
- *  @param x the expression to try
  */
 #define ASSERT(cond)                                           \
 	do { if (!(cond))                                      \
@@ -89,11 +87,6 @@
 		(void *)(p2) = SWAPPOINTERS_TEMP;	\
 	} while (0)
 
-/**
- *  @addtogroup driverlogging
- *  @{
- */
-
 #define PRINTKDRV(fmt, args...) LOGINF(fmt, ## args)
 #define TBDDRV(fmt, args...)    LOGERR(fmt, ## args)
 #define HUHDRV(fmt, args...)    LOGERR(fmt, ## args)
@@ -114,8 +107,6 @@
 #define INFODEVX(devno, fmt, args...)     LOGINFDEVX(devno, fmt, ## args)
 #define DEBUGDEV(devname, fmt, args...)   DBGINFDEV(devname, fmt, ## args)
 
-/* @} */
-
 /** Verifies the consistency of your PRIVATEDEVICEDATA structure using
  *  conventional "signature" fields:
  *  <p>
@@ -139,7 +130,7 @@
 	 ((fd)->sig2 == fd))
 
 /** Sleep for an indicated number of seconds (for use in kernel mode).
- *  @param x the number of seconds to sleep.
+ *  x - the number of seconds to sleep.
  */
 #define SLEEP(x)					     \
 	do { current->state = TASK_INTERRUPTIBLE;	     \
@@ -147,17 +138,13 @@
 	} while (0)
 
 /** Sleep for an indicated number of jiffies (for use in kernel mode).
- *  @param x the number of jiffies to sleep.
+ *  x - the number of jiffies to sleep.
  */
 #define SLEEPJIFFIES(x)						    \
 	do { current->state = TASK_INTERRUPTIBLE;		    \
 		schedule_timeout(x);				    \
 	} while (0)
 
-#ifndef max
-#define max(a, b) (((a) > (b)) ? (a) : (b))
-#endif
-
 static inline struct cdev *cdev_alloc_init(struct module *owner,
 					   const struct file_operations *fops)
 {
diff --git a/drivers/staging/unisys/include/uisqueue.h b/drivers/staging/unisys/include/uisqueue.h
index 5178270b98d1..25b6181d78af 100644
--- a/drivers/staging/unisys/include/uisqueue.h
+++ b/drivers/staging/unisys/include/uisqueue.h
@@ -34,8 +34,7 @@
 #include "controlvmcompletionstatus.h"
 
 struct uisqueue_info {
-
-	CHANNEL_HEADER __iomem *chan;
+	struct channel_header __iomem *chan;
 	/* channel containing queues in which scsi commands &
 	 * responses are queued
 	 */
@@ -48,8 +47,8 @@ struct uisqueue_info {
 	u64 non_empty_wakeup_cnt;
 
 	struct {
-		SIGNAL_QUEUE_HEADER reserved1;	/*  */
-		SIGNAL_QUEUE_HEADER reserved2;	/*  */
+		struct signal_queue_header reserved1;	/*  */
+		struct signal_queue_header reserved2;	/*  */
 	} safe_uis_queue;
 	unsigned int (*send_int_if_needed)(struct uisqueue_info *info,
 					   unsigned int whichcqueue,
@@ -119,7 +118,7 @@ struct extport_info {
 	*/
 
 	struct switch_info *swtch;
-	struct PciId pci_id;
+	struct pci_id pci_id;
 	char name[MAX_NAME_SIZE_UISQUEUE];
 	union {
 		struct vhba_wwnn wwnn;
@@ -133,7 +132,7 @@ struct device_info {
 	u64 channel_bytes;
 	uuid_le channel_uuid;
 	uuid_le instance_uuid;
-	struct InterruptInfo intr;
+	struct irq_info intr;
 	struct switch_info *swtch;
 	char devid[30];		/* "vbus<busno>:dev<devno>" */
 	u16 polling;
@@ -149,30 +148,27 @@ struct device_info {
 	unsigned long long last_on_list_cnt;
 };
 
-typedef enum {
+enum switch_type {
 	RECOVERY_LAN = 1,
 	IB_LAN = 2
-} SWITCH_TYPE;
+};
 
 struct bus_info {
-	u32 busNo, deviceCount;
+	u32 bus_no, device_count;
 	struct device_info **device;
-	u64 guestHandle, recvBusInterruptHandle;
-	uuid_le busInstGuid;
-	ULTRA_VBUS_CHANNEL_PROTOCOL __iomem *pBusChannel;
-	int busChannelBytes;
+	u64 guest_handle, recv_bus_irq_handle;
+	uuid_le bus_inst_uuid;
+	struct ultra_vbus_channel_protocol __iomem *bus_channel;
+	int bus_channel_bytes;
 	struct proc_dir_entry *proc_dir;	/* proc/uislib/vbus/<x> */
 	struct proc_dir_entry *proc_info;	/* proc/uislib/vbus/<x>/info */
 	char name[25];
-	char partitionName[99];
+	char partition_name[99];
 	struct bus_info *next;
-	u8 localVnic;		/* 1 if local vnic created internally
+	u8 local_vnic;		/* 1 if local vnic created internally
 				 * by IOVM; 0 otherwise... */
 };
 
-#define DEDICATED_SWITCH(s) ((s->extPortCount == 1) &&	\
-			     (s->intPortCount == 1))
-
 struct sn_list_entry {
 	struct uisscsi_dest pdest;	/* scsi bus, target, lun for
 					 * phys disk */
@@ -183,23 +179,12 @@ struct sn_list_entry {
 	struct sn_list_entry *next;
 };
 
-struct network_policy {
-	u32 promiscuous:1;
-	u32 macassign:1;
-	u32 peerforwarding:1;
-	u32 nonotify:1;
-	u32 standby:1;
-	u32 callhome:2;
-	char ip_addr[30];
-};
-
 /*
  * IO messages sent to UisnicControlChanFunc & UissdControlChanFunc by
  * code that processes the ControlVm channel messages.
  */
 
-
-typedef enum {
+enum iopart_msg_type {
 	IOPART_ADD_VNIC,
 	IOPART_DEL_VNIC,
 	IOPART_DEL_ALL_VNICS,
@@ -219,7 +204,7 @@ typedef enum {
 	IOPART_RESUME_VDISK,
 	IOPART_ADD_DEVICE,	/* add generic device */
 	IOPART_DEL_DEVICE,	/* del generic device */
-} IOPART_MSG_TYPE;
+};
 
 struct add_virt_iopart {
 	void *chanptr;		/* pointer to data channel */
@@ -228,7 +213,7 @@ struct add_virt_iopart {
 				 * for DMA, for ex. */
 	u64 recv_bus_irq_handle;	/* used to register to receive
 					 * bus level interrupts. */
-	struct InterruptInfo intr;	/* contains recv & send
+	struct irq_info intr;	/* contains recv & send
 					 * interrupt info */
 	/* recvInterruptHandle is used to register to receive
 	* interrupts on the data channel. Used by GuestLinux/Windows
@@ -259,21 +244,15 @@ struct add_vdisk_iopart {
 	struct uisscsi_dest pdest;    /* scsi bus, target, lun for phys disk */
 	u8 sernum[MAX_SERIAL_NUM];    /* serial num of physical disk */
 	u32 serlen;		      /* length of serial num */
-	u32 bus_no;
-	u32 dev_no;
 };
 
 struct del_vdisk_iopart {
 	void *chanptr;		     /* pointer to data channel */
 	struct uisscsi_dest vdest;   /* scsi bus, target, lun for virt disk */
-	u32 bus_no;
-	u32 dev_no;
 };
 
 struct del_virt_iopart {
 	void *chanptr;		     /* pointer to data channel */
-	u32 bus_no;
-	u32 dev_no;
 };
 
 struct det_virt_iopart {	     /* detach internal port */
@@ -297,8 +276,7 @@ struct del_switch_iopart {	     /* destroy switch */
 };
 
 struct io_msgs {
-
-	IOPART_MSG_TYPE msgtype;
+	enum iopart_msg_type msgtype;
 
 	/* additional params needed by some messages */
 	union {
@@ -329,7 +307,7 @@ struct io_msgs {
 * the ControlVm channel messages.
 */
 
-typedef enum {
+enum guestpart_msg_type {
 	GUEST_ADD_VBUS,
 	GUEST_ADD_VHBA,
 	GUEST_ADD_VNIC,
@@ -344,15 +322,15 @@ typedef enum {
 	GUEST_PAUSE_VNIC,
 	GUEST_RESUME_VHBA,
 	GUEST_RESUME_VNIC
-} GUESTPART_MSG_TYPE;
+};
 
 struct add_vbus_guestpart {
 	void __iomem *chanptr;		/* pointer to data channel for bus -
 					 * NOT YET USED */
-	u32 busNo;		/* bus number to be created/deleted */
-	u32 deviceCount;	/* max num of devices on bus */
-	uuid_le busTypeGuid;	/* indicates type of bus */
-	uuid_le busInstGuid;	/* instance guid for device */
+	u32 bus_no;		/* bus number to be created/deleted */
+	u32 dev_count;	/* max num of devices on bus */
+	uuid_le bus_uuid;	/* indicates type of bus */
+	uuid_le instance_uuid;	/* instance guid for device */
 };
 
 struct del_vbus_guestpart {
@@ -367,7 +345,7 @@ struct add_virt_guestpart {
 	u32 bus_no;		/* bus number for the operation */
 	u32 device_no;		/* number of device on the bus */
 	uuid_le instance_uuid;	/* instance guid for device */
-	struct InterruptInfo intr;	/* recv/send interrupt info */
+	struct irq_info intr;	/* recv/send interrupt info */
 	/* recvInterruptHandle contains info needed in order to
 	 * register to receive interrupts on the data channel.
 	 * sendInterruptHandle contains handle which is provided to
@@ -394,8 +372,7 @@ struct init_chipset_guestpart {
 };
 
 struct guest_msgs {
-
-	GUESTPART_MSG_TYPE msgtype;
+	enum guestpart_msg_type msgtype;
 
 	/* additional params needed by messages */
 	union {
diff --git a/drivers/staging/unisys/include/uisutils.h b/drivers/staging/unisys/include/uisutils.h
index 74e7cf65502c..7414220676d3 100644
--- a/drivers/staging/unisys/include/uisutils.h
+++ b/drivers/staging/unisys/include/uisutils.h
@@ -26,6 +26,7 @@
 #include <linux/sched.h>
 #include <linux/gfp.h>
 #include <linux/uuid.h>
+#include <linux/if_ether.h>
 
 #include "vmcallinterface.h"
 #include "channel.h"
@@ -43,39 +44,38 @@
 /* global function pointers that act as callback functions into
  * uisnicmod, uissdmod, and virtpcimod
  */
-extern int (*UisnicControlChanFunc)(struct io_msgs *);
-extern int (*UissdControlChanFunc)(struct io_msgs *);
-extern int (*VirtControlChanFunc)(struct guest_msgs *);
+extern int (*uisnic_control_chan_func)(struct io_msgs *);
+extern int (*uissd_control_chan_func)(struct io_msgs *);
+extern int (*virt_control_chan_func)(struct guest_msgs *);
 
 /* Return values of above callback functions: */
 #define CCF_ERROR        0	/* completed and failed */
 #define CCF_OK           1	/* completed successfully */
 #define CCF_PENDING      2	/* operation still pending */
-extern atomic_t UisUtils_Registered_Services;
+extern atomic_t uisutils_registered_services;
 
-typedef unsigned int MACARRAY[MAX_MACADDR_LEN];
-typedef struct ReqHandlerInfo_struct {
-	uuid_le switchTypeGuid;
+struct req_handler_info {
+	uuid_le switch_uuid;
 	int (*controlfunc)(struct io_msgs *);
 	unsigned long min_channel_bytes;
-	int (*Server_Channel_Ok)(unsigned long channelBytes);
-	int (*Server_Channel_Init)
-	 (void *x, unsigned char *clientStr, u32 clientStrLen, u64 bytes);
+	int (*server_channel_ok)(unsigned long channel_bytes);
+	int (*server_channel_init)(void *x, unsigned char *client_str,
+				   u32 client_str_len, u64 bytes);
 	char switch_type_name[99];
 	struct list_head list_link;	/* links into ReqHandlerInfo_list */
-} ReqHandlerInfo_t;
+};
 
-ReqHandlerInfo_t *ReqHandlerAdd(uuid_le switchTypeGuid,
+struct req_handler_info *req_handler_add(uuid_le switch_uuid,
 				const char *switch_type_name,
 				int (*controlfunc)(struct io_msgs *),
 				unsigned long min_channel_bytes,
-				int (*Server_Channel_Ok)(unsigned long
-							 channelBytes),
-				int (*Server_Channel_Init)
-				 (void *x, unsigned char *clientStr,
-				  u32 clientStrLen, u64 bytes));
-ReqHandlerInfo_t *ReqHandlerFind(uuid_le switchTypeGuid);
-int ReqHandlerDel(uuid_le switchTypeGuid);
+				int (*svr_channel_ok)(unsigned long
+							 channel_bytes),
+				int (*svr_channel_init)(void *x,
+						unsigned char *client_str,
+						u32 client_str_len, u64 bytes));
+struct req_handler_info *req_handler_find(uuid_le switch_uuid);
+int req_handler_del(uuid_le switch_uuid);
 
 #define uislib_ioremap_cache(addr, size) \
 	dbg_ioremap_cache(addr, size, __FILE__, __LINE__)
@@ -114,52 +114,49 @@ int uisutil_add_proc_line_ex(int *total, char **buffer, int *buffer_remaining,
 			     char *format, ...);
 
 int uisctrl_register_req_handler(int type, void *fptr,
-				 ULTRA_VBUS_DEVICEINFO *chipset_driver_info);
-int uisctrl_register_req_handler_ex(uuid_le switchTypeGuid,
-				    const char *switch_type_name,
-				    int (*fptr)(struct io_msgs *),
-				    unsigned long min_channel_bytes,
-				    int (*Server_Channel_Ok)(unsigned long
-							     channelBytes),
-				    int (*Server_Channel_Init)
-				    (void *x, unsigned char *clientStr,
-				     u32 clientStrLen, u64 bytes),
-				    ULTRA_VBUS_DEVICEINFO *chipset_DriverInfo);
-
-int uisctrl_unregister_req_handler_ex(uuid_le switchTypeGuid);
+			struct ultra_vbus_deviceinfo *chipset_driver_info);
+int uisctrl_register_req_handler_ex(uuid_le switch_guid,
+			const char *switch_type_name,
+			int (*fptr)(struct io_msgs *),
+			unsigned long min_channel_bytes,
+			int (*svr_channel_ok)(unsigned long
+					      channel_bytes),
+			int (*svr_channel_init)(void *x,
+						unsigned char *client_str,
+						u32 client_str_len,
+						u64 bytes),
+			struct ultra_vbus_deviceinfo *chipset_driver_info);
+
+int uisctrl_unregister_req_handler_ex(uuid_le switch_uuid);
 unsigned char *util_map_virt(struct phys_info *sg);
 void util_unmap_virt(struct phys_info *sg);
 unsigned char *util_map_virt_atomic(struct phys_info *sg);
 void util_unmap_virt_atomic(void *buf);
-int uislib_server_inject_add_vnic(u32 switchNo, u32 BusNo, u32 numIntPorts,
-				  u32 numExtPorts, MACARRAY pmac[],
-				  pCHANNEL_HEADER **chan);
-void uislib_server_inject_del_vnic(u32 switchNo, u32 busNo, u32 numIntPorts,
-				   u32 numExtPorts);
-int uislib_client_inject_add_bus(u32 busNo, uuid_le instGuid,
-				 u64 channelAddr, ulong nChannelBytes);
-int  uislib_client_inject_del_bus(u32 busNo);
-
-int uislib_client_inject_add_vhba(u32 busNo, u32 devNo,
+int uislib_client_inject_add_bus(u32 bus_no, uuid_le inst_uuid,
+				 u64 channel_addr, ulong n_channel_bytes);
+int  uislib_client_inject_del_bus(u32 bus_no);
+
+int uislib_client_inject_add_vhba(u32 bus_no, u32 dev_no,
 				  u64 phys_chan_addr, u32 chan_bytes,
-				  int is_test_addr, uuid_le instGuid,
-				  struct InterruptInfo *intr);
-int  uislib_client_inject_pause_vhba(u32 busNo, u32 devNo);
-int  uislib_client_inject_resume_vhba(u32 busNo, u32 devNo);
-int uislib_client_inject_del_vhba(u32 busNo, u32 devNo);
-int uislib_client_inject_add_vnic(u32 busNo, u32 devNo,
+				  int is_test_addr, uuid_le inst_uuid,
+				  struct irq_info *intr);
+int  uislib_client_inject_pause_vhba(u32 bus_no, u32 dev_no);
+int  uislib_client_inject_resume_vhba(u32 bus_no, u32 dev_no);
+int uislib_client_inject_del_vhba(u32 bus_no, u32 dev_no);
+int uislib_client_inject_add_vnic(u32 bus_no, u32 dev_no,
 				  u64 phys_chan_addr, u32 chan_bytes,
-				  int is_test_addr, uuid_le instGuid,
-				  struct InterruptInfo *intr);
-int uislib_client_inject_pause_vnic(u32 busNo, u32 devNo);
-int uislib_client_inject_resume_vnic(u32 busNo, u32 devNo);
-int uislib_client_inject_del_vnic(u32 busNo, u32 devNo);
+				  int is_test_addr, uuid_le inst_uuid,
+				  struct irq_info *intr);
+int uislib_client_inject_pause_vnic(u32 bus_no, u32 dev_no);
+int uislib_client_inject_resume_vnic(u32 bus_no, u32 dev_no);
+int uislib_client_inject_del_vnic(u32 bus_no, u32 dev_no);
 #ifdef STORAGE_CHANNEL
 u64 uislib_storage_channel(int client_id);
 #endif
 int uislib_get_owned_pdest(struct uisscsi_dest *pdest);
 
-int uislib_send_event(CONTROLVM_ID id, CONTROLVM_MESSAGE_PACKET *event);
+int uislib_send_event(enum controlvm_id id,
+		      struct controlvm_message_packet *event);
 
 /* structure used by vhba & vnic to keep track of queue & thread info */
 struct chaninfo {
@@ -182,11 +179,14 @@ struct chaninfo {
 	set_current_state(TASK_INTERRUPTIBLE); \
 	schedule_timeout(msecs_to_jiffies(x)); \
 }
+
 #define UIS_THREAD_WAIT_USEC(x) { \
 	set_current_state(TASK_INTERRUPTIBLE); \
 	schedule_timeout(usecs_to_jiffies(x)); \
 }
+
 #define UIS_THREAD_WAIT UIS_THREAD_WAIT_MSEC(5)
+
 #define UIS_THREAD_WAIT_SEC(x) { \
 	set_current_state(TASK_INTERRUPTIBLE); \
 	schedule_timeout((x)*HZ); \
@@ -224,42 +224,42 @@ unsigned int uisutil_copy_fragsinfo_from_skb(unsigned char *calling_ctx,
 static inline unsigned int
 issue_vmcall_io_controlvm_addr(u64 *control_addr, u32 *control_bytes)
 {
-	VMCALL_IO_CONTROLVM_ADDR_PARAMS params;
+	struct vmcall_io_controlvm_addr_params params;
 	int result = VMCALL_SUCCESS;
 	u64 physaddr;
 
 	physaddr = virt_to_phys(&params);
 	ISSUE_IO_VMCALL(VMCALL_IO_CONTROLVM_ADDR, physaddr, result);
 	if (VMCALL_SUCCESSFUL(result)) {
-		*control_addr = params.ChannelAddress;
-		*control_bytes = params.ChannelBytes;
+		*control_addr = params.address;
+		*control_bytes = params.channel_bytes;
 	}
 	return result;
 }
 
 static inline unsigned int issue_vmcall_io_diag_addr(u64 *diag_channel_addr)
 {
-	VMCALL_IO_DIAG_ADDR_PARAMS params;
+	struct vmcall_io_diag_addr_params params;
 	int result = VMCALL_SUCCESS;
 	u64 physaddr;
 
 	physaddr = virt_to_phys(&params);
 	ISSUE_IO_VMCALL(VMCALL_IO_DIAG_ADDR, physaddr, result);
 	if (VMCALL_SUCCESSFUL(result))
-		*diag_channel_addr = params.ChannelAddress;
+		*diag_channel_addr = params.address;
 	return result;
 }
 
 static inline unsigned int issue_vmcall_io_visorserial_addr(u64 *channel_addr)
 {
-	VMCALL_IO_VISORSERIAL_ADDR_PARAMS params;
+	struct vmcall_io_visorserial_addr_params params;
 	int result = VMCALL_SUCCESS;
 	u64 physaddr;
 
 	physaddr = virt_to_phys(&params);
 	ISSUE_IO_VMCALL(VMCALL_IO_VISORSERIAL_ADDR, physaddr, result);
 	if (VMCALL_SUCCESSFUL(result))
-		*channel_addr = params.ChannelAddress;
+		*channel_addr = params.address;
 	return result;
 }
 
@@ -273,17 +273,8 @@ static inline s64 issue_vmcall_query_guest_virtual_time_offset(void)
 	return result;
 }
 
-static inline s64 issue_vmcall_measurement_do_nothing(void)
-{
-	u64 result = VMCALL_SUCCESS;
-	u64 physaddr = 0;
-
-	ISSUE_IO_VMCALL(VMCALL_MEASUREMENT_DO_NOTHING, physaddr, result);
-	return result;
-}
-
 struct log_info_t {
-	volatile unsigned long long last_cycles;
+	unsigned long long last_cycles;
 	unsigned long long delta_sum[64];
 	unsigned long long delta_cnt[64];
 	unsigned long long max_delta[64];
@@ -302,44 +293,29 @@ static inline unsigned int issue_vmcall_channel_mismatch(const char *chname,
 			      const char *item_name, u32 line_no,
 			      const char *path_n_fn)
 {
-	VMCALL_CHANNEL_VERSION_MISMATCH_PARAMS params;
+	struct vmcall_channel_version_mismatch_params params;
 	int result = VMCALL_SUCCESS;
 	u64 physaddr;
 	char *last_slash = NULL;
 
-	strlcpy(params.ChannelName, chname,
-		lengthof(VMCALL_CHANNEL_VERSION_MISMATCH_PARAMS, ChannelName));
-	strlcpy(params.ItemName, item_name,
-		lengthof(VMCALL_CHANNEL_VERSION_MISMATCH_PARAMS, ItemName));
-	params.SourceLineNumber = line_no;
+	strlcpy(params.chname, chname, sizeof(params.chname));
+	strlcpy(params.item_name, item_name, sizeof(params.item_name));
+	params.line_no = line_no;
 
 	last_slash = strrchr(path_n_fn, '/');
 	if (last_slash != NULL) {
 		last_slash++;
-		strlcpy(params.SourceFileName, last_slash,
-			lengthof(VMCALL_CHANNEL_VERSION_MISMATCH_PARAMS,
-				 SourceFileName));
+		strlcpy(params.file_name, last_slash, sizeof(params.file_name));
 	} else
-		strlcpy(params.SourceFileName,
+		strlcpy(params.file_name,
 			"Cannot determine source filename",
-			lengthof(VMCALL_CHANNEL_VERSION_MISMATCH_PARAMS,
-				 SourceFileName));
+			sizeof(params.file_name));
 
 	physaddr = virt_to_phys(&params);
 	ISSUE_IO_VMCALL(VMCALL_CHANNEL_VERSION_MISMATCH, physaddr, result);
 	return result;
 }
 
-static inline unsigned int issue_vmcall_fatal(void)
-{
-	int result = VMCALL_SUCCESS;
-	u64 physaddr = 0;
-
-	ISSUE_IO_VMCALL(VMCALL_GENERIC_SURRENDER_QUANTUM_FOREVER, physaddr,
-			result);
-	return result;
-}
-
 #define UIS_DAEMONIZE(nam)
 void *uislib_cache_alloc(struct kmem_cache *cur_pool, char *fn, int ln);
 #define UISCACHEALLOC(cur_pool) uislib_cache_alloc(cur_pool, __FILE__, __LINE__)
diff --git a/drivers/staging/unisys/include/vbushelper.h b/drivers/staging/unisys/include/vbushelper.h
index 1bde549ec0df..84abe5f99f54 100644
--- a/drivers/staging/unisys/include/vbushelper.h
+++ b/drivers/staging/unisys/include/vbushelper.h
@@ -26,19 +26,19 @@
 #define TARGET_HOSTNAME "linuxguest"
 
 static inline void bus_device_info_init(
-		ULTRA_VBUS_DEVICEINFO * bus_device_info_ptr,
+		struct ultra_vbus_deviceinfo *bus_device_info_ptr,
 		const char *dev_type, const char *drv_name,
 		const char *ver, const char *ver_tag)
 {
-	memset(bus_device_info_ptr, 0, sizeof(ULTRA_VBUS_DEVICEINFO));
-	snprintf(bus_device_info_ptr->devType,
-		 sizeof(bus_device_info_ptr->devType),
+	memset(bus_device_info_ptr, 0, sizeof(struct ultra_vbus_deviceinfo));
+	snprintf(bus_device_info_ptr->devtype,
+		 sizeof(bus_device_info_ptr->devtype),
 		 "%s", (dev_type) ? dev_type : "unknownType");
-	snprintf(bus_device_info_ptr->drvName,
-		 sizeof(bus_device_info_ptr->drvName),
+	snprintf(bus_device_info_ptr->drvname,
+		 sizeof(bus_device_info_ptr->drvname),
 		 "%s", (drv_name) ? drv_name : "unknownDriver");
-	snprintf(bus_device_info_ptr->infoStrings,
-		 sizeof(bus_device_info_ptr->infoStrings), "%s\t%s\t%s",
+	snprintf(bus_device_info_ptr->infostrs,
+		 sizeof(bus_device_info_ptr->infostrs), "%s\t%s\t%s",
 		 (ver) ? ver : "unknownVer",
 		 (ver_tag) ? ver_tag : "unknownVerTag",
 		 TARGET_HOSTNAME);
diff --git a/drivers/staging/unisys/uislib/uislib.c b/drivers/staging/unisys/uislib/uislib.c
index 706f1c0c2c6e..7c87452a9f14 100644
--- a/drivers/staging/unisys/uislib/uislib.c
+++ b/drivers/staging/unisys/uislib/uislib.c
@@ -57,7 +57,7 @@
 #define __MYFILE__ "uislib.c"
 
 /* global function pointers that act as callback functions into virtpcimod */
-int (*VirtControlChanFunc)(struct guest_msgs *);
+int (*virt_control_chan_func)(struct guest_msgs *);
 
 static int ProcReadBufferValid;
 static char *ProcReadBuffer;	/* Note this MUST be global,
@@ -121,12 +121,12 @@ static const struct file_operations debugfs_info_fops = {
 };
 
 static void
-init_msg_header(CONTROLVM_MESSAGE *msg, u32 id, uint rsp, uint svr)
+init_msg_header(struct controlvm_message *msg, u32 id, uint rsp, uint svr)
 {
-	memset(msg, 0, sizeof(CONTROLVM_MESSAGE));
-	msg->hdr.Id = id;
-	msg->hdr.Flags.responseExpected = rsp;
-	msg->hdr.Flags.server = svr;
+	memset(msg, 0, sizeof(struct controlvm_message));
+	msg->hdr.id = id;
+	msg->hdr.flags.response_expected = rsp;
+	msg->hdr.flags.server = svr;
 }
 
 static __iomem void *
@@ -142,7 +142,7 @@ init_vbus_channel(u64 channelAddr, u32 channelBytes)
 		rc = NULL;
 		goto Away;
 	}
-	if (!ULTRA_VBUS_CHANNEL_OK_CLIENT(pChan, NULL)) {
+	if (!SPAR_VBUS_CHANNEL_OK_CLIENT(pChan)) {
 		ERRDRV("%s channel cannot be used", __func__);
 		uislib_iounmap(pChan);
 		rc = NULL;
@@ -154,7 +154,7 @@ Away:
 }
 
 static int
-create_bus(CONTROLVM_MESSAGE *msg, char *buf)
+create_bus(struct controlvm_message *msg, char *buf)
 {
 	u32 busNo, deviceCount;
 	struct bus_info *tmp, *bus;
@@ -168,8 +168,8 @@ create_bus(CONTROLVM_MESSAGE *msg, char *buf)
 		return CONTROLVM_RESP_ERROR_MAX_BUSES;
 	}
 
-	busNo = msg->cmd.createBus.busNo;
-	deviceCount = msg->cmd.createBus.deviceCount;
+	busNo = msg->cmd.create_bus.bus_no;
+	deviceCount = msg->cmd.create_bus.dev_count;
 
 	POSTCODE_LINUX_4(BUS_CREATE_ENTRY_PC, busNo, deviceCount,
 			 POSTCODE_SEVERITY_INFO);
@@ -188,25 +188,25 @@ create_bus(CONTROLVM_MESSAGE *msg, char *buf)
 	/* Currently by default, the bus Number is the GuestHandle.
 	 * Configure Bus message can override this.
 	 */
-	if (msg->hdr.Flags.testMessage) {
+	if (msg->hdr.flags.test_message) {
 		/* This implies we're the IOVM so set guest handle to 0... */
-		bus->guestHandle = 0;
-		bus->busNo = busNo;
-		bus->localVnic = 1;
+		bus->guest_handle = 0;
+		bus->bus_no = busNo;
+		bus->local_vnic = 1;
 	} else
-		bus->busNo = bus->guestHandle = busNo;
-	sprintf(bus->name, "%d", (int) bus->busNo);
-	bus->deviceCount = deviceCount;
+		bus->bus_no = bus->guest_handle = busNo;
+	sprintf(bus->name, "%d", (int) bus->bus_no);
+	bus->device_count = deviceCount;
 	bus->device =
 	    (struct device_info **) ((char *) bus + sizeof(struct bus_info));
-	bus->busInstGuid = msg->cmd.createBus.busInstGuid;
-	bus->busChannelBytes = 0;
-	bus->pBusChannel = NULL;
+	bus->bus_inst_uuid = msg->cmd.create_bus.bus_inst_uuid;
+	bus->bus_channel_bytes = 0;
+	bus->bus_channel = NULL;
 
 	/* add bus to our bus list - but check for duplicates first */
 	read_lock(&BusListLock);
 	for (tmp = BusListHead; tmp; tmp = tmp->next) {
-		if (tmp->busNo == bus->busNo)
+		if (tmp->bus_no == bus->bus_no)
 			break;
 	}
 	read_unlock(&BusListLock);
@@ -215,39 +215,39 @@ create_bus(CONTROLVM_MESSAGE *msg, char *buf)
 		 * reject add
 		 */
 		LOGERR("CONTROLVM_BUS_CREATE Failed: bus %d already exists.\n",
-		       bus->busNo);
-		POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus->busNo,
+		       bus->bus_no);
+		POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus->bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		kfree(bus);
 		return CONTROLVM_RESP_ERROR_ALREADY_DONE;
 	}
-	if ((msg->cmd.createBus.channelAddr != 0)
-	    && (msg->cmd.createBus.channelBytes != 0)) {
-		bus->busChannelBytes = msg->cmd.createBus.channelBytes;
-		bus->pBusChannel =
-		    init_vbus_channel(msg->cmd.createBus.channelAddr,
-				      msg->cmd.createBus.channelBytes);
+	if ((msg->cmd.create_bus.channel_addr != 0)
+	    && (msg->cmd.create_bus.channel_bytes != 0)) {
+		bus->bus_channel_bytes = msg->cmd.create_bus.channel_bytes;
+		bus->bus_channel =
+		    init_vbus_channel(msg->cmd.create_bus.channel_addr,
+				      msg->cmd.create_bus.channel_bytes);
 	}
 	/* the msg is bound for virtpci; send guest_msgs struct to callback */
-	if (!msg->hdr.Flags.server) {
+	if (!msg->hdr.flags.server) {
 		struct guest_msgs cmd;
 
 		cmd.msgtype = GUEST_ADD_VBUS;
-		cmd.add_vbus.busNo = busNo;
-		cmd.add_vbus.chanptr = bus->pBusChannel;
-		cmd.add_vbus.deviceCount = deviceCount;
-		cmd.add_vbus.busTypeGuid = msg->cmd.createBus.busDataTypeGuid;
-		cmd.add_vbus.busInstGuid = msg->cmd.createBus.busInstGuid;
-		if (!VirtControlChanFunc) {
+		cmd.add_vbus.bus_no = busNo;
+		cmd.add_vbus.chanptr = bus->bus_channel;
+		cmd.add_vbus.dev_count = deviceCount;
+		cmd.add_vbus.bus_uuid = msg->cmd.create_bus.bus_data_type_uuid;
+		cmd.add_vbus.instance_uuid = msg->cmd.create_bus.bus_inst_uuid;
+		if (!virt_control_chan_func) {
 			LOGERR("CONTROLVM_BUS_CREATE Failed: virtpci callback not registered.");
-			POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus->busNo,
+			POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus->bus_no,
 					 POSTCODE_SEVERITY_ERR);
 			kfree(bus);
 			return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
 		}
-		if (!VirtControlChanFunc(&cmd)) {
+		if (!virt_control_chan_func(&cmd)) {
 			LOGERR("CONTROLVM_BUS_CREATE Failed: virtpci GUEST_ADD_VBUS returned error.");
-			POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus->busNo,
+			POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus->bus_no,
 					 POSTCODE_SEVERITY_ERR);
 			kfree(bus);
 			return
@@ -266,26 +266,26 @@ create_bus(CONTROLVM_MESSAGE *msg, char *buf)
 	BusListCount++;
 	write_unlock(&BusListLock);
 
-	POSTCODE_LINUX_3(BUS_CREATE_EXIT_PC, bus->busNo,
+	POSTCODE_LINUX_3(BUS_CREATE_EXIT_PC, bus->bus_no,
 			 POSTCODE_SEVERITY_INFO);
 	return CONTROLVM_RESP_SUCCESS;
 }
 
 static int
-destroy_bus(CONTROLVM_MESSAGE *msg, char *buf)
+destroy_bus(struct controlvm_message *msg, char *buf)
 {
 	int i;
 	struct bus_info *bus, *prev = NULL;
 	struct guest_msgs cmd;
 	u32 busNo;
 
-	busNo = msg->cmd.destroyBus.busNo;
+	busNo = msg->cmd.destroy_bus.bus_no;
 
 	read_lock(&BusListLock);
 
 	bus = BusListHead;
 	while (bus) {
-		if (bus->busNo == busNo)
+		if (bus->bus_no == busNo)
 			break;
 		prev = bus;
 		bus = bus->next;
@@ -299,7 +299,7 @@ destroy_bus(CONTROLVM_MESSAGE *msg, char *buf)
 	}
 
 	/* verify that this bus has no devices. */
-	for (i = 0; i < bus->deviceCount; i++) {
+	for (i = 0; i < bus->device_count; i++) {
 		if (bus->device[i] != NULL) {
 			LOGERR("CONTROLVM_BUS_DESTROY Failed: device %i attached to bus %d.",
 			     i, busNo);
@@ -309,18 +309,18 @@ destroy_bus(CONTROLVM_MESSAGE *msg, char *buf)
 	}
 	read_unlock(&BusListLock);
 
-	if (msg->hdr.Flags.server)
+	if (msg->hdr.flags.server)
 		goto remove;
 
 	/* client messages require us to call the virtpci callback associated
 	   with this bus. */
 	cmd.msgtype = GUEST_DEL_VBUS;
 	cmd.del_vbus.bus_no = busNo;
-	if (!VirtControlChanFunc) {
+	if (!virt_control_chan_func) {
 		LOGERR("CONTROLVM_BUS_DESTROY Failed: virtpci callback not registered.");
 		return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
 	}
-	if (!VirtControlChanFunc(&cmd)) {
+	if (!virt_control_chan_func(&cmd)) {
 		LOGERR("CONTROLVM_BUS_DESTROY Failed: virtpci GUEST_DEL_VBUS returned error.");
 		return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
 	}
@@ -335,9 +335,9 @@ remove:
 	BusListCount--;
 	write_unlock(&BusListLock);
 
-	if (bus->pBusChannel) {
-		uislib_iounmap(bus->pBusChannel);
-		bus->pBusChannel = NULL;
+	if (bus->bus_channel) {
+		uislib_iounmap(bus->bus_channel);
+		bus->bus_channel = NULL;
 	}
 
 	kfree(bus);
@@ -345,17 +345,17 @@ remove:
 }
 
 static int
-create_device(CONTROLVM_MESSAGE *msg, char *buf)
+create_device(struct controlvm_message *msg, char *buf)
 {
 	struct device_info *dev;
 	struct bus_info *bus;
 	u32 busNo, devNo;
 	int result = CONTROLVM_RESP_SUCCESS;
 	u64 minSize = MIN_IO_CHANNEL_SIZE;
-	ReqHandlerInfo_t *pReqHandler;
+	struct req_handler_info *pReqHandler;
 
-	busNo = msg->cmd.createDevice.busNo;
-	devNo = msg->cmd.createDevice.devNo;
+	busNo = msg->cmd.create_device.bus_no;
+	devNo = msg->cmd.create_device.dev_no;
 
 	POSTCODE_LINUX_4(DEVICE_CREATE_ENTRY_PC, devNo, busNo,
 			 POSTCODE_SEVERITY_INFO);
@@ -368,26 +368,26 @@ create_device(CONTROLVM_MESSAGE *msg, char *buf)
 		return CONTROLVM_RESP_ERROR_KMALLOC_FAILED;
 	}
 
-	dev->channel_uuid = msg->cmd.createDevice.dataTypeGuid;
-	dev->intr = msg->cmd.createDevice.intr;
-	dev->channel_addr = msg->cmd.createDevice.channelAddr;
+	dev->channel_uuid = msg->cmd.create_device.data_type_uuid;
+	dev->intr = msg->cmd.create_device.intr;
+	dev->channel_addr = msg->cmd.create_device.channel_addr;
 	dev->bus_no = busNo;
 	dev->dev_no = devNo;
 	sema_init(&dev->interrupt_callback_lock, 1);	/* unlocked */
 	sprintf(dev->devid, "vbus%u:dev%u", (unsigned) busNo, (unsigned) devNo);
 	/* map the channel memory for the device. */
-	if (msg->hdr.Flags.testMessage)
+	if (msg->hdr.flags.test_message)
 		dev->chanptr = (void __iomem *)__va(dev->channel_addr);
 	else {
-		pReqHandler = ReqHandlerFind(dev->channel_uuid);
+		pReqHandler = req_handler_find(dev->channel_uuid);
 		if (pReqHandler)
 			/* generic service handler registered for this
 			 * channel
 			 */
 			minSize = pReqHandler->min_channel_bytes;
-		if (minSize > msg->cmd.createDevice.channelBytes) {
+		if (minSize > msg->cmd.create_device.channel_bytes) {
 			LOGERR("CONTROLVM_DEVICE_CREATE Failed: channel size is too small, channel size:0x%lx, required size:0x%lx",
-			     (ulong) msg->cmd.createDevice.channelBytes,
+			     (ulong) msg->cmd.create_device.channel_bytes,
 			     (ulong) minSize);
 			POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, devNo, busNo,
 					 POSTCODE_SEVERITY_ERR);
@@ -396,27 +396,27 @@ create_device(CONTROLVM_MESSAGE *msg, char *buf)
 		}
 		dev->chanptr =
 		    uislib_ioremap_cache(dev->channel_addr,
-					 msg->cmd.createDevice.channelBytes);
+					 msg->cmd.create_device.channel_bytes);
 		if (!dev->chanptr) {
 			LOGERR("CONTROLVM_DEVICE_CREATE Failed: ioremap_cache of channelAddr:%Lx for channelBytes:%llu failed",
 			     dev->channel_addr,
-			     msg->cmd.createDevice.channelBytes);
+			     msg->cmd.create_device.channel_bytes);
 			result = CONTROLVM_RESP_ERROR_IOREMAP_FAILED;
 			POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, devNo, busNo,
 					 POSTCODE_SEVERITY_ERR);
 			goto Away;
 		}
 	}
-	dev->instance_uuid = msg->cmd.createDevice.devInstGuid;
-	dev->channel_bytes = msg->cmd.createDevice.channelBytes;
+	dev->instance_uuid = msg->cmd.create_device.dev_inst_uuid;
+	dev->channel_bytes = msg->cmd.create_device.channel_bytes;
 
 	read_lock(&BusListLock);
 	for (bus = BusListHead; bus; bus = bus->next) {
-		if (bus->busNo == busNo) {
+		if (bus->bus_no == busNo) {
 			/* make sure the device number is valid */
-			if (devNo >= bus->deviceCount) {
+			if (devNo >= bus->device_count) {
 				LOGERR("CONTROLVM_DEVICE_CREATE Failed: device (%d) >= deviceCount (%d).",
-				     devNo, bus->deviceCount);
+				     devNo, bus->device_count);
 				result = CONTROLVM_RESP_ERROR_MAX_DEVICES;
 				POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC,
 						 devNo, busNo,
@@ -439,17 +439,18 @@ create_device(CONTROLVM_MESSAGE *msg, char *buf)
 			/* the msg is bound for virtpci; send
 			 * guest_msgs struct to callback
 			 */
-			if (!msg->hdr.Flags.server) {
+			if (!msg->hdr.flags.server) {
 				struct guest_msgs cmd;
 
 				if (!uuid_le_cmp(dev->channel_uuid,
-				     UltraVhbaChannelProtocolGuid)) {
-					wait_for_valid_guid(&((CHANNEL_HEADER
-							      __iomem *) (dev->
+				     spar_vhba_channel_protocol_uuid)) {
+					wait_for_valid_guid(&((
+						struct channel_header
+							__iomem *) (dev->
 								  chanptr))->
-							    Type);
-					if (!ULTRA_VHBA_CHANNEL_OK_CLIENT
-					    (dev->chanptr, NULL)) {
+							    chtype);
+					if (!SPAR_VHBA_CHANNEL_OK_CLIENT
+					    (dev->chanptr)) {
 						LOGERR("CONTROLVM_DEVICE_CREATE Failed:[CLIENT]VHBA dev %d chan invalid.",
 						     devNo);
 						POSTCODE_LINUX_4
@@ -468,13 +469,14 @@ create_device(CONTROLVM_MESSAGE *msg, char *buf)
 					cmd.add_vhba.intr = dev->intr;
 				} else
 				    if (!uuid_le_cmp(dev->channel_uuid,
-					 UltraVnicChannelProtocolGuid)) {
-					wait_for_valid_guid(&((CHANNEL_HEADER
-							      __iomem *) (dev->
+					 spar_vnic_channel_protocol_uuid)) {
+					wait_for_valid_guid(&((
+						struct channel_header
+							__iomem *) (dev->
 								  chanptr))->
-							    Type);
-					if (!ULTRA_VNIC_CHANNEL_OK_CLIENT
-					    (dev->chanptr, NULL)) {
+							    chtype);
+					if (!SPAR_VNIC_CHANNEL_OK_CLIENT
+					    (dev->chanptr)) {
 						LOGERR("CONTROLVM_DEVICE_CREATE Failed: VNIC[CLIENT] dev %d chan invalid.",
 						     devNo);
 						POSTCODE_LINUX_4
@@ -500,7 +502,7 @@ create_device(CONTROLVM_MESSAGE *msg, char *buf)
 					goto Away;
 				}
 
-				if (!VirtControlChanFunc) {
+				if (!virt_control_chan_func) {
 					LOGERR("CONTROLVM_DEVICE_CREATE Failed: virtpci callback not registered.");
 					POSTCODE_LINUX_4
 					    (DEVICE_CREATE_FAILURE_PC, devNo,
@@ -509,7 +511,7 @@ create_device(CONTROLVM_MESSAGE *msg, char *buf)
 					goto Away;
 				}
 
-				if (!VirtControlChanFunc(&cmd)) {
+				if (!virt_control_chan_func(&cmd)) {
 					LOGERR("CONTROLVM_DEVICE_CREATE Failed: virtpci GUEST_ADD_[VHBA||VNIC] returned error.");
 					POSTCODE_LINUX_4
 					    (DEVICE_CREATE_FAILURE_PC, devNo,
@@ -532,7 +534,7 @@ create_device(CONTROLVM_MESSAGE *msg, char *buf)
 	result = CONTROLVM_RESP_ERROR_BUS_INVALID;
 
 Away:
-	if (!msg->hdr.Flags.testMessage) {
+	if (!msg->hdr.flags.test_message) {
 		uislib_iounmap(dev->chanptr);
 		dev->chanptr = NULL;
 	}
@@ -542,7 +544,7 @@ Away:
 }
 
 static int
-pause_device(CONTROLVM_MESSAGE *msg)
+pause_device(struct controlvm_message *msg)
 {
 	u32 busNo, devNo;
 	struct bus_info *bus;
@@ -550,16 +552,16 @@ pause_device(CONTROLVM_MESSAGE *msg)
 	struct guest_msgs cmd;
 	int retval = CONTROLVM_RESP_SUCCESS;
 
-	busNo = msg->cmd.deviceChangeState.busNo;
-	devNo = msg->cmd.deviceChangeState.devNo;
+	busNo = msg->cmd.device_change_state.bus_no;
+	devNo = msg->cmd.device_change_state.dev_no;
 
 	read_lock(&BusListLock);
 	for (bus = BusListHead; bus; bus = bus->next) {
-		if (bus->busNo == busNo) {
+		if (bus->bus_no == busNo) {
 			/* make sure the device number is valid */
-			if (devNo >= bus->deviceCount) {
+			if (devNo >= bus->device_count) {
 				LOGERR("CONTROLVM_DEVICE_CHANGESTATE:pause Failed: device(%d) >= deviceCount(%d).",
-				     devNo, bus->deviceCount);
+				     devNo, bus->device_count);
 				retval = CONTROLVM_RESP_ERROR_DEVICE_INVALID;
 			} else {
 				/* make sure this device exists */
@@ -585,22 +587,22 @@ pause_device(CONTROLVM_MESSAGE *msg)
 		 * guest_msgs struct to callback
 		 */
 		if (!uuid_le_cmp(dev->channel_uuid,
-				UltraVhbaChannelProtocolGuid)) {
+				spar_vhba_channel_protocol_uuid)) {
 			cmd.msgtype = GUEST_PAUSE_VHBA;
 			cmd.pause_vhba.chanptr = dev->chanptr;
 		} else if (!uuid_le_cmp(dev->channel_uuid,
-					UltraVnicChannelProtocolGuid)) {
+					spar_vnic_channel_protocol_uuid)) {
 			cmd.msgtype = GUEST_PAUSE_VNIC;
 			cmd.pause_vnic.chanptr = dev->chanptr;
 		} else {
 			LOGERR("CONTROLVM_DEVICE_CHANGESTATE:pause Failed: unknown channelTypeGuid.\n");
 			return CONTROLVM_RESP_ERROR_CHANNEL_TYPE_UNKNOWN;
 		}
-		if (!VirtControlChanFunc) {
+		if (!virt_control_chan_func) {
 			LOGERR("CONTROLVM_DEVICE_CHANGESTATE Failed: virtpci callback not registered.");
 			return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
 		}
-		if (!VirtControlChanFunc(&cmd)) {
+		if (!virt_control_chan_func(&cmd)) {
 			LOGERR("CONTROLVM_DEVICE_CHANGESTATE:pause Failed: virtpci GUEST_PAUSE_[VHBA||VNIC] returned error.");
 			return
 			  CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
@@ -610,7 +612,7 @@ pause_device(CONTROLVM_MESSAGE *msg)
 }
 
 static int
-resume_device(CONTROLVM_MESSAGE *msg)
+resume_device(struct controlvm_message *msg)
 {
 	u32 busNo, devNo;
 	struct bus_info *bus;
@@ -618,16 +620,16 @@ resume_device(CONTROLVM_MESSAGE *msg)
 	struct guest_msgs cmd;
 	int retval = CONTROLVM_RESP_SUCCESS;
 
-	busNo = msg->cmd.deviceChangeState.busNo;
-	devNo = msg->cmd.deviceChangeState.devNo;
+	busNo = msg->cmd.device_change_state.bus_no;
+	devNo = msg->cmd.device_change_state.dev_no;
 
 	read_lock(&BusListLock);
 	for (bus = BusListHead; bus; bus = bus->next) {
-		if (bus->busNo == busNo) {
+		if (bus->bus_no == busNo) {
 			/* make sure the device number is valid */
-			if (devNo >= bus->deviceCount) {
+			if (devNo >= bus->device_count) {
 				LOGERR("CONTROLVM_DEVICE_CHANGESTATE:resume Failed: device(%d) >= deviceCount(%d).",
-				     devNo, bus->deviceCount);
+				     devNo, bus->device_count);
 				retval = CONTROLVM_RESP_ERROR_DEVICE_INVALID;
 			} else {
 				/* make sure this device exists */
@@ -654,22 +656,22 @@ resume_device(CONTROLVM_MESSAGE *msg)
 	 */
 	if (retval == CONTROLVM_RESP_SUCCESS) {
 		if (!uuid_le_cmp(dev->channel_uuid,
-				 UltraVhbaChannelProtocolGuid)) {
+				 spar_vhba_channel_protocol_uuid)) {
 			cmd.msgtype = GUEST_RESUME_VHBA;
 			cmd.resume_vhba.chanptr = dev->chanptr;
 		} else if (!uuid_le_cmp(dev->channel_uuid,
-					UltraVnicChannelProtocolGuid)) {
+					spar_vnic_channel_protocol_uuid)) {
 			cmd.msgtype = GUEST_RESUME_VNIC;
 			cmd.resume_vnic.chanptr = dev->chanptr;
 		} else {
 			LOGERR("CONTROLVM_DEVICE_CHANGESTATE:resume Failed: unknown channelTypeGuid.\n");
 			return CONTROLVM_RESP_ERROR_CHANNEL_TYPE_UNKNOWN;
 		}
-		if (!VirtControlChanFunc) {
+		if (!virt_control_chan_func) {
 			LOGERR("CONTROLVM_DEVICE_CHANGESTATE Failed: virtpci callback not registered.");
 			return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
 		}
-		if (!VirtControlChanFunc(&cmd)) {
+		if (!virt_control_chan_func(&cmd)) {
 			LOGERR("CONTROLVM_DEVICE_CHANGESTATE:resume Failed: virtpci GUEST_RESUME_[VHBA||VNIC] returned error.");
 			return
 			  CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
@@ -679,7 +681,7 @@ resume_device(CONTROLVM_MESSAGE *msg)
 }
 
 static int
-destroy_device(CONTROLVM_MESSAGE *msg, char *buf)
+destroy_device(struct controlvm_message *msg, char *buf)
 {
 	u32 busNo, devNo;
 	struct bus_info *bus;
@@ -687,17 +689,17 @@ destroy_device(CONTROLVM_MESSAGE *msg, char *buf)
 	struct guest_msgs cmd;
 	int retval = CONTROLVM_RESP_SUCCESS;
 
-	busNo = msg->cmd.destroyDevice.busNo;
-	devNo = msg->cmd.destroyDevice.devNo;
+	busNo = msg->cmd.destroy_device.bus_no;
+	devNo = msg->cmd.destroy_device.bus_no;
 
 	read_lock(&BusListLock);
 	LOGINF("destroy_device called for busNo=%u, devNo=%u", busNo, devNo);
 	for (bus = BusListHead; bus; bus = bus->next) {
-		if (bus->busNo == busNo) {
+		if (bus->bus_no == busNo) {
 			/* make sure the device number is valid */
-			if (devNo >= bus->deviceCount) {
+			if (devNo >= bus->device_count) {
 				LOGERR("CONTROLVM_DEVICE_DESTORY Failed: device(%d) >= deviceCount(%d).",
-				       devNo, bus->deviceCount);
+				       devNo, bus->device_count);
 				retval = CONTROLVM_RESP_ERROR_DEVICE_INVALID;
 			} else {
 				/* make sure this device exists */
@@ -724,11 +726,11 @@ destroy_device(CONTROLVM_MESSAGE *msg, char *buf)
 		 * guest_msgs struct to callback
 		 */
 		if (!uuid_le_cmp(dev->channel_uuid,
-				 UltraVhbaChannelProtocolGuid)) {
+				 spar_vhba_channel_protocol_uuid)) {
 			cmd.msgtype = GUEST_DEL_VHBA;
 			cmd.del_vhba.chanptr = dev->chanptr;
 		} else if (!uuid_le_cmp(dev->channel_uuid,
-					UltraVnicChannelProtocolGuid)) {
+					spar_vnic_channel_protocol_uuid)) {
 			cmd.msgtype = GUEST_DEL_VNIC;
 			cmd.del_vnic.chanptr = dev->chanptr;
 		} else {
@@ -736,12 +738,12 @@ destroy_device(CONTROLVM_MESSAGE *msg, char *buf)
 			return
 			    CONTROLVM_RESP_ERROR_CHANNEL_TYPE_UNKNOWN;
 		}
-		if (!VirtControlChanFunc) {
+		if (!virt_control_chan_func) {
 			LOGERR("CONTROLVM_DEVICE_DESTORY Failed: virtpci callback not registered.");
 			return
 			    CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
 		}
-		if (!VirtControlChanFunc(&cmd)) {
+		if (!virt_control_chan_func(&cmd)) {
 			LOGERR("CONTROLVM_DEVICE_DESTROY Failed: virtpci GUEST_DEL_[VHBA||VNIC] returned error.");
 			return
 			    CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
@@ -756,7 +758,7 @@ destroy_device(CONTROLVM_MESSAGE *msg, char *buf)
 			uislib_disable_channel_interrupts(busNo, devNo);
 		}
 		/* unmap the channel memory for the device. */
-		if (!msg->hdr.Flags.testMessage) {
+		if (!msg->hdr.flags.test_message) {
 			LOGINF("destroy_device, doing iounmap");
 			uislib_iounmap(dev->chanptr);
 		}
@@ -767,23 +769,23 @@ destroy_device(CONTROLVM_MESSAGE *msg, char *buf)
 }
 
 static int
-init_chipset(CONTROLVM_MESSAGE *msg, char *buf)
+init_chipset(struct controlvm_message *msg, char *buf)
 {
 	POSTCODE_LINUX_2(CHIPSET_INIT_ENTRY_PC, POSTCODE_SEVERITY_INFO);
 
-	MaxBusCount = msg->cmd.initChipset.busCount;
-	PlatformNumber = msg->cmd.initChipset.platformNumber;
+	MaxBusCount = msg->cmd.init_chipset.bus_count;
+	PlatformNumber = msg->cmd.init_chipset.platform_number;
 	PhysicalDataChan = 0;
 
 	/* We need to make sure we have our functions registered
 	* before processing messages.  If we are a test vehicle the
-	* testMessage for init_chipset will be set.  We can ignore the
+	* test_message for init_chipset will be set.  We can ignore the
 	* waits for the callbacks, since this will be manually entered
-	* from a user.  If no testMessage is set, we will wait for the
+	* from a user.  If no test_message is set, we will wait for the
 	* functions.
 	*/
-	if (!msg->hdr.Flags.testMessage)
-		WAIT_ON_CALLBACK(VirtControlChanFunc);
+	if (!msg->hdr.flags.test_message)
+		WAIT_ON_CALLBACK(virt_control_chan_func);
 
 	chipset_inited = 1;
 	POSTCODE_LINUX_2(CHIPSET_INIT_EXIT_PC, POSTCODE_SEVERITY_INFO);
@@ -794,10 +796,10 @@ init_chipset(CONTROLVM_MESSAGE *msg, char *buf)
 static int
 delete_bus_glue(u32 busNo)
 {
-	CONTROLVM_MESSAGE msg;
+	struct controlvm_message msg;
 
 	init_msg_header(&msg, CONTROLVM_BUS_DESTROY, 0, 0);
-	msg.cmd.destroyBus.busNo = busNo;
+	msg.cmd.destroy_bus.bus_no = busNo;
 	if (destroy_bus(&msg, NULL) != CONTROLVM_RESP_SUCCESS) {
 		LOGERR("destroy_bus failed. busNo=0x%x\n", busNo);
 		return 0;
@@ -808,11 +810,11 @@ delete_bus_glue(u32 busNo)
 static int
 delete_device_glue(u32 busNo, u32 devNo)
 {
-	CONTROLVM_MESSAGE msg;
+	struct controlvm_message msg;
 
 	init_msg_header(&msg, CONTROLVM_DEVICE_DESTROY, 0, 0);
-	msg.cmd.destroyDevice.busNo = busNo;
-	msg.cmd.destroyDevice.devNo = devNo;
+	msg.cmd.destroy_device.bus_no = busNo;
+	msg.cmd.destroy_device.dev_no = devNo;
 	if (destroy_device(&msg, NULL) != CONTROLVM_RESP_SUCCESS) {
 		LOGERR("destroy_device failed. busNo=0x%x devNo=0x%x\n", busNo,
 		       devNo);
@@ -822,14 +824,14 @@ delete_device_glue(u32 busNo, u32 devNo)
 }
 
 int
-uislib_client_inject_add_bus(u32 busNo, uuid_le instGuid,
-			     u64 channelAddr, ulong nChannelBytes)
+uislib_client_inject_add_bus(u32 bus_no, uuid_le inst_uuid,
+			     u64 channel_addr, ulong n_channel_bytes)
 {
-	CONTROLVM_MESSAGE msg;
+	struct controlvm_message msg;
 
-	LOGINF("enter busNo=0x%x\n", busNo);
+	LOGINF("enter busNo=0x%x\n", bus_no);
 	/* step 0: init the chipset */
-	POSTCODE_LINUX_3(CHIPSET_INIT_ENTRY_PC, busNo, POSTCODE_SEVERITY_INFO);
+	POSTCODE_LINUX_3(CHIPSET_INIT_ENTRY_PC, bus_no, POSTCODE_SEVERITY_INFO);
 
 	if (!chipset_inited) {
 		/* step: initialize the chipset */
@@ -841,31 +843,32 @@ uislib_client_inject_add_bus(u32 busNo, uuid_le instGuid,
 		* after number 4, then the add_vnic will fail, and the
 		* ultraboot will fail.
 		*/
-		msg.cmd.initChipset.busCount = 23;
-		msg.cmd.initChipset.switchCount = 0;
+		msg.cmd.init_chipset.bus_count = 23;
+		msg.cmd.init_chipset.switch_count = 0;
 		if (init_chipset(&msg, NULL) != CONTROLVM_RESP_SUCCESS) {
 			LOGERR("init_chipset failed.\n");
 			return 0;
 		}
 		LOGINF("chipset initialized\n");
-		POSTCODE_LINUX_3(CHIPSET_INIT_EXIT_PC, busNo,
+		POSTCODE_LINUX_3(CHIPSET_INIT_EXIT_PC, bus_no,
 				 POSTCODE_SEVERITY_INFO);
 	}
 
 	/* step 1: create a bus */
-	POSTCODE_LINUX_3(BUS_CREATE_ENTRY_PC, busNo, POSTCODE_SEVERITY_WARNING);
+	POSTCODE_LINUX_3(BUS_CREATE_ENTRY_PC, bus_no,
+			 POSTCODE_SEVERITY_WARNING);
 	init_msg_header(&msg, CONTROLVM_BUS_CREATE, 0, 0);
-	msg.cmd.createBus.busNo = busNo;
-	msg.cmd.createBus.deviceCount = 23;	/* devNo+1; */
-	msg.cmd.createBus.channelAddr = channelAddr;
-	msg.cmd.createBus.channelBytes = nChannelBytes;
+	msg.cmd.create_bus.bus_no = bus_no;
+	msg.cmd.create_bus.dev_count = 23;	/* devNo+1; */
+	msg.cmd.create_bus.channel_addr = channel_addr;
+	msg.cmd.create_bus.channel_bytes = n_channel_bytes;
 	if (create_bus(&msg, NULL) != CONTROLVM_RESP_SUCCESS) {
 		LOGERR("create_bus failed.\n");
-		POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, busNo,
+		POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		return 0;
 	}
-	POSTCODE_LINUX_3(BUS_CREATE_EXIT_PC, busNo, POSTCODE_SEVERITY_INFO);
+	POSTCODE_LINUX_3(BUS_CREATE_EXIT_PC, bus_no, POSTCODE_SEVERITY_INFO);
 
 	return 1;
 }
@@ -873,26 +876,26 @@ EXPORT_SYMBOL_GPL(uislib_client_inject_add_bus);
 
 
 int
-uislib_client_inject_del_bus(u32 busNo)
+uislib_client_inject_del_bus(u32 bus_no)
 {
-	return delete_bus_glue(busNo);
+	return delete_bus_glue(bus_no);
 }
 EXPORT_SYMBOL_GPL(uislib_client_inject_del_bus);
 
 int
-uislib_client_inject_pause_vhba(u32 busNo, u32 devNo)
+uislib_client_inject_pause_vhba(u32 bus_no, u32 dev_no)
 {
-	CONTROLVM_MESSAGE msg;
+	struct controlvm_message msg;
 	int rc;
 
 	init_msg_header(&msg, CONTROLVM_DEVICE_CHANGESTATE, 0, 0);
-	msg.cmd.deviceChangeState.busNo = busNo;
-	msg.cmd.deviceChangeState.devNo = devNo;
-	msg.cmd.deviceChangeState.state = SegmentStateStandby;
+	msg.cmd.device_change_state.bus_no = bus_no;
+	msg.cmd.device_change_state.dev_no = dev_no;
+	msg.cmd.device_change_state.state = segment_state_standby;
 	rc = pause_device(&msg);
 	if (rc != CONTROLVM_RESP_SUCCESS) {
 		LOGERR("VHBA pause_device failed. busNo=0x%x devNo=0x%x\n",
-		       busNo, devNo);
+		       bus_no, dev_no);
 		return rc;
 	}
 	return 0;
@@ -900,19 +903,19 @@ uislib_client_inject_pause_vhba(u32 busNo, u32 devNo)
 EXPORT_SYMBOL_GPL(uislib_client_inject_pause_vhba);
 
 int
-uislib_client_inject_resume_vhba(u32 busNo, u32 devNo)
+uislib_client_inject_resume_vhba(u32 bus_no, u32 dev_no)
 {
-	CONTROLVM_MESSAGE msg;
+	struct controlvm_message msg;
 	int rc;
 
 	init_msg_header(&msg, CONTROLVM_DEVICE_CHANGESTATE, 0, 0);
-	msg.cmd.deviceChangeState.busNo = busNo;
-	msg.cmd.deviceChangeState.devNo = devNo;
-	msg.cmd.deviceChangeState.state = SegmentStateRunning;
+	msg.cmd.device_change_state.bus_no = bus_no;
+	msg.cmd.device_change_state.dev_no = dev_no;
+	msg.cmd.device_change_state.state = segment_state_running;
 	rc = resume_device(&msg);
 	if (rc != CONTROLVM_RESP_SUCCESS) {
 		LOGERR("VHBA resume_device failed. busNo=0x%x devNo=0x%x\n",
-		       busNo, devNo);
+		       bus_no, dev_no);
 		return rc;
 	}
 	return 0;
@@ -921,19 +924,19 @@ uislib_client_inject_resume_vhba(u32 busNo, u32 devNo)
 EXPORT_SYMBOL_GPL(uislib_client_inject_resume_vhba);
 
 int
-uislib_client_inject_add_vhba(u32 busNo, u32 devNo,
+uislib_client_inject_add_vhba(u32 bus_no, u32 dev_no,
 			      u64 phys_chan_addr, u32 chan_bytes,
-			      int is_test_addr, uuid_le instGuid,
-			      struct InterruptInfo *intr)
+			      int is_test_addr, uuid_le inst_uuid,
+			      struct irq_info *intr)
 {
-	CONTROLVM_MESSAGE msg;
+	struct controlvm_message msg;
 
-	LOGINF(" enter busNo=0x%x devNo=0x%x\n", busNo, devNo);
+	LOGINF(" enter busNo=0x%x devNo=0x%x\n", bus_no, dev_no);
 	/* chipset init'ed with bus bus has been previously created -
 	* Verify it still exists step 2: create the VHBA device on the
 	* bus
 	*/
-	POSTCODE_LINUX_4(VHBA_CREATE_ENTRY_PC, devNo, busNo,
+	POSTCODE_LINUX_4(VHBA_CREATE_ENTRY_PC, dev_no, bus_no,
 			 POSTCODE_SEVERITY_INFO);
 
 	init_msg_header(&msg, CONTROLVM_DEVICE_CREATE, 0, 0);
@@ -941,16 +944,16 @@ uislib_client_inject_add_vhba(u32 busNo, u32 devNo,
 		/* signify that the physical channel address does NOT
 		 * need to be ioremap()ed
 		 */
-		msg.hdr.Flags.testMessage = 1;
-	msg.cmd.createDevice.busNo = busNo;
-	msg.cmd.createDevice.devNo = devNo;
-	msg.cmd.createDevice.devInstGuid = instGuid;
+		msg.hdr.flags.test_message = 1;
+	msg.cmd.create_device.bus_no = bus_no;
+	msg.cmd.create_device.dev_no = dev_no;
+	msg.cmd.create_device.dev_inst_uuid = inst_uuid;
 	if (intr)
-		msg.cmd.createDevice.intr = *intr;
+		msg.cmd.create_device.intr = *intr;
 	else
-		memset(&msg.cmd.createDevice.intr, 0,
-		       sizeof(struct InterruptInfo));
-	msg.cmd.createDevice.channelAddr = phys_chan_addr;
+		memset(&msg.cmd.create_device.intr, 0,
+		       sizeof(struct irq_info));
+	msg.cmd.create_device.channel_addr = phys_chan_addr;
 	if (chan_bytes < MIN_IO_CHANNEL_SIZE) {
 		LOGERR("wrong channel size.chan_bytes = 0x%x IO_CHANNEL_SIZE= 0x%x\n",
 		     chan_bytes, (unsigned int) MIN_IO_CHANNEL_SIZE);
@@ -958,41 +961,41 @@ uislib_client_inject_add_vhba(u32 busNo, u32 devNo,
 				 MIN_IO_CHANNEL_SIZE, POSTCODE_SEVERITY_ERR);
 		return 0;
 	}
-	msg.cmd.createDevice.channelBytes = chan_bytes;
-	msg.cmd.createDevice.dataTypeGuid = UltraVhbaChannelProtocolGuid;
+	msg.cmd.create_device.channel_bytes = chan_bytes;
+	msg.cmd.create_device.data_type_uuid = spar_vhba_channel_protocol_uuid;
 	if (create_device(&msg, NULL) != CONTROLVM_RESP_SUCCESS) {
 		LOGERR("VHBA create_device failed.\n");
-		POSTCODE_LINUX_4(VHBA_CREATE_FAILURE_PC, devNo, busNo,
+		POSTCODE_LINUX_4(VHBA_CREATE_FAILURE_PC, dev_no, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		return 0;
 	}
-	POSTCODE_LINUX_4(VHBA_CREATE_SUCCESS_PC, devNo, busNo,
+	POSTCODE_LINUX_4(VHBA_CREATE_SUCCESS_PC, dev_no, bus_no,
 			 POSTCODE_SEVERITY_INFO);
 	return 1;
 }
 EXPORT_SYMBOL_GPL(uislib_client_inject_add_vhba);
 
 int
-uislib_client_inject_del_vhba(u32 busNo, u32 devNo)
+uislib_client_inject_del_vhba(u32 bus_no, u32 dev_no)
 {
-	return delete_device_glue(busNo, devNo);
+	return delete_device_glue(bus_no, dev_no);
 }
 EXPORT_SYMBOL_GPL(uislib_client_inject_del_vhba);
 
 int
-uislib_client_inject_add_vnic(u32 busNo, u32 devNo,
+uislib_client_inject_add_vnic(u32 bus_no, u32 dev_no,
 			      u64 phys_chan_addr, u32 chan_bytes,
-			      int is_test_addr, uuid_le instGuid,
-			      struct InterruptInfo *intr)
+			      int is_test_addr, uuid_le inst_uuid,
+			      struct irq_info *intr)
 {
-	CONTROLVM_MESSAGE msg;
+	struct controlvm_message msg;
 
-	LOGINF(" enter busNo=0x%x devNo=0x%x\n", busNo, devNo);
+	LOGINF(" enter busNo=0x%x devNo=0x%x\n", bus_no, dev_no);
 	/* chipset init'ed with bus bus has been previously created -
 	* Verify it still exists step 2: create the VNIC device on the
 	* bus
 	*/
-	POSTCODE_LINUX_4(VNIC_CREATE_ENTRY_PC, devNo, busNo,
+	POSTCODE_LINUX_4(VNIC_CREATE_ENTRY_PC, dev_no, bus_no,
 			 POSTCODE_SEVERITY_INFO);
 
 	init_msg_header(&msg, CONTROLVM_DEVICE_CREATE, 0, 0);
@@ -1000,16 +1003,16 @@ uislib_client_inject_add_vnic(u32 busNo, u32 devNo,
 		/* signify that the physical channel address does NOT
 		 * need to be ioremap()ed
 		 */
-		msg.hdr.Flags.testMessage = 1;
-	msg.cmd.createDevice.busNo = busNo;
-	msg.cmd.createDevice.devNo = devNo;
-	msg.cmd.createDevice.devInstGuid = instGuid;
+		msg.hdr.flags.test_message = 1;
+	msg.cmd.create_device.bus_no = bus_no;
+	msg.cmd.create_device.dev_no = dev_no;
+	msg.cmd.create_device.dev_inst_uuid = inst_uuid;
 	if (intr)
-		msg.cmd.createDevice.intr = *intr;
+		msg.cmd.create_device.intr = *intr;
 	else
-		memset(&msg.cmd.createDevice.intr, 0,
-		       sizeof(struct InterruptInfo));
-	msg.cmd.createDevice.channelAddr = phys_chan_addr;
+		memset(&msg.cmd.create_device.intr, 0,
+		       sizeof(struct irq_info));
+	msg.cmd.create_device.channel_addr = phys_chan_addr;
 	if (chan_bytes < MIN_IO_CHANNEL_SIZE) {
 		LOGERR("wrong channel size.chan_bytes = 0x%x IO_CHANNEL_SIZE= 0x%x\n",
 		     chan_bytes, (unsigned int) MIN_IO_CHANNEL_SIZE);
@@ -1017,35 +1020,35 @@ uislib_client_inject_add_vnic(u32 busNo, u32 devNo,
 				 MIN_IO_CHANNEL_SIZE, POSTCODE_SEVERITY_ERR);
 		return 0;
 	}
-	msg.cmd.createDevice.channelBytes = chan_bytes;
-	msg.cmd.createDevice.dataTypeGuid = UltraVnicChannelProtocolGuid;
+	msg.cmd.create_device.channel_bytes = chan_bytes;
+	msg.cmd.create_device.data_type_uuid = spar_vnic_channel_protocol_uuid;
 	if (create_device(&msg, NULL) != CONTROLVM_RESP_SUCCESS) {
 		LOGERR("VNIC create_device failed.\n");
-		POSTCODE_LINUX_4(VNIC_CREATE_FAILURE_PC, devNo, busNo,
+		POSTCODE_LINUX_4(VNIC_CREATE_FAILURE_PC, dev_no, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		return 0;
 	}
 
-	POSTCODE_LINUX_4(VNIC_CREATE_SUCCESS_PC, devNo, busNo,
+	POSTCODE_LINUX_4(VNIC_CREATE_SUCCESS_PC, dev_no, bus_no,
 			 POSTCODE_SEVERITY_INFO);
 	return 1;
 }
 EXPORT_SYMBOL_GPL(uislib_client_inject_add_vnic);
 
 int
-uislib_client_inject_pause_vnic(u32 busNo, u32 devNo)
+uislib_client_inject_pause_vnic(u32 bus_no, u32 dev_no)
 {
-	CONTROLVM_MESSAGE msg;
+	struct controlvm_message msg;
 	int rc;
 
 	init_msg_header(&msg, CONTROLVM_DEVICE_CHANGESTATE, 0, 0);
-	msg.cmd.deviceChangeState.busNo = busNo;
-	msg.cmd.deviceChangeState.devNo = devNo;
-	msg.cmd.deviceChangeState.state = SegmentStateStandby;
+	msg.cmd.device_change_state.bus_no = bus_no;
+	msg.cmd.device_change_state.dev_no = dev_no;
+	msg.cmd.device_change_state.state = segment_state_standby;
 	rc = pause_device(&msg);
 	if (rc != CONTROLVM_RESP_SUCCESS) {
 		LOGERR("VNIC pause_device failed. busNo=0x%x devNo=0x%x\n",
-		       busNo, devNo);
+		       bus_no, dev_no);
 		return -1;
 	}
 	return 0;
@@ -1053,19 +1056,19 @@ uislib_client_inject_pause_vnic(u32 busNo, u32 devNo)
 EXPORT_SYMBOL_GPL(uislib_client_inject_pause_vnic);
 
 int
-uislib_client_inject_resume_vnic(u32 busNo, u32 devNo)
+uislib_client_inject_resume_vnic(u32 bus_no, u32 dev_no)
 {
-	CONTROLVM_MESSAGE msg;
+	struct controlvm_message msg;
 	int rc;
 
 	init_msg_header(&msg, CONTROLVM_DEVICE_CHANGESTATE, 0, 0);
-	msg.cmd.deviceChangeState.busNo = busNo;
-	msg.cmd.deviceChangeState.devNo = devNo;
-	msg.cmd.deviceChangeState.state = SegmentStateRunning;
+	msg.cmd.device_change_state.bus_no = bus_no;
+	msg.cmd.device_change_state.dev_no = dev_no;
+	msg.cmd.device_change_state.state = segment_state_running;
 	rc = resume_device(&msg);
 	if (rc != CONTROLVM_RESP_SUCCESS) {
 		LOGERR("VNIC resume_device failed. busNo=0x%x devNo=0x%x\n",
-		       busNo, devNo);
+		       bus_no, dev_no);
 		return -1;
 	}
 	return 0;
@@ -1074,88 +1077,12 @@ uislib_client_inject_resume_vnic(u32 busNo, u32 devNo)
 EXPORT_SYMBOL_GPL(uislib_client_inject_resume_vnic);
 
 int
-uislib_client_inject_del_vnic(u32 busNo, u32 devNo)
+uislib_client_inject_del_vnic(u32 bus_no, u32 dev_no)
 {
-	return delete_device_glue(busNo, devNo);
+	return delete_device_glue(bus_no, dev_no);
 }
 EXPORT_SYMBOL_GPL(uislib_client_inject_del_vnic);
 
-static int
-uislib_client_add_vnic(u32 busNo)
-{
-	BOOL busCreated = FALSE;
-	int devNo = 0;		/* Default to 0, since only one device
-				 * will be created for this bus... */
-	CONTROLVM_MESSAGE msg;
-
-	init_msg_header(&msg, CONTROLVM_BUS_CREATE, 0, 0);
-	msg.hdr.Flags.testMessage = 1;
-	msg.cmd.createBus.busNo = busNo;
-	msg.cmd.createBus.deviceCount = 4;
-	msg.cmd.createBus.channelAddr = 0;
-	msg.cmd.createBus.channelBytes = 0;
-	if (create_bus(&msg, NULL) != CONTROLVM_RESP_SUCCESS) {
-		LOGERR("client create_bus failed");
-		return 0;
-	}
-	busCreated = TRUE;
-
-	init_msg_header(&msg, CONTROLVM_DEVICE_CREATE, 0, 0);
-	msg.hdr.Flags.testMessage = 1;
-	msg.cmd.createDevice.busNo = busNo;
-	msg.cmd.createDevice.devNo = devNo;
-	msg.cmd.createDevice.devInstGuid = NULL_UUID_LE;
-	memset(&msg.cmd.createDevice.intr, 0, sizeof(struct InterruptInfo));
-	msg.cmd.createDevice.channelAddr = PhysicalDataChan;
-	msg.cmd.createDevice.channelBytes = MIN_IO_CHANNEL_SIZE;
-	msg.cmd.createDevice.dataTypeGuid = UltraVnicChannelProtocolGuid;
-	if (create_device(&msg, NULL) != CONTROLVM_RESP_SUCCESS) {
-		LOGERR("client create_device failed");
-		goto AwayCleanup;
-	}
-
-	return 1;
-
-AwayCleanup:
-	if (busCreated) {
-		init_msg_header(&msg, CONTROLVM_BUS_DESTROY, 0, 0);
-		msg.hdr.Flags.testMessage = 1;
-		msg.cmd.destroyBus.busNo = busNo;
-		if (destroy_bus(&msg, NULL) != CONTROLVM_RESP_SUCCESS)
-			LOGERR("client destroy_bus failed.\n");
-	}
-
-	return 0;
-}				/* end uislib_client_add_vnic */
-EXPORT_SYMBOL_GPL(uislib_client_add_vnic);
-
-static int
-uislib_client_delete_vnic(u32 busNo)
-{
-	int devNo = 0;		/* Default to 0, since only one device
-				 * will be created for this bus... */
-	CONTROLVM_MESSAGE msg;
-
-	init_msg_header(&msg, CONTROLVM_DEVICE_DESTROY, 0, 0);
-	msg.hdr.Flags.testMessage = 1;
-	msg.cmd.destroyDevice.busNo = busNo;
-	msg.cmd.destroyDevice.devNo = devNo;
-	if (destroy_device(&msg, NULL) != CONTROLVM_RESP_SUCCESS) {
-		/* Don't error exit - try to see if bus can be destroyed... */
-		LOGERR("client destroy_device failed.\n");
-	}
-
-	init_msg_header(&msg, CONTROLVM_BUS_DESTROY, 0, 0);
-	msg.hdr.Flags.testMessage = 1;
-	msg.cmd.destroyBus.busNo = busNo;
-	if (destroy_bus(&msg, NULL) != CONTROLVM_RESP_SUCCESS)
-		LOGERR("client destroy_bus failed.\n");
-
-	return 1;
-}
-EXPORT_SYMBOL_GPL(uislib_client_delete_vnic);
-/* end client_delete_vnic */
-
 void *
 uislib_cache_alloc(struct kmem_cache *cur_pool, char *fn, int ln)
 {
@@ -1206,17 +1133,17 @@ info_debugfs_read_helper(char **buff, int *buff_len)
 	for (bus = BusListHead; bus; bus = bus->next) {
 
 		if (PLINE("    bus=0x%p, busNo=%d, deviceCount=%d\n",
-			  bus, bus->busNo, bus->deviceCount) < 0)
+			  bus, bus->bus_no, bus->device_count) < 0)
 			goto err_done_unlock;
 
 
 		if (PLINE("        Devices:\n") < 0)
 			goto err_done_unlock;
 
-		for (i = 0; i < bus->deviceCount; i++) {
+		for (i = 0; i < bus->device_count; i++) {
 			if (bus->device[i]) {
 				if (PLINE("            busNo %d, device[%i]: 0x%p, chanptr=0x%p, swtch=0x%p\n",
-					  bus->busNo, i, bus->device[i],
+					  bus->bus_no, i, bus->device[i],
 					  bus->device[i]->chanptr,
 					  bus->device[i]->swtch) < 0)
 					goto err_done_unlock;
@@ -1232,7 +1159,7 @@ info_debugfs_read_helper(char **buff, int *buff_len)
 	read_unlock(&BusListLock);
 
 	if (PLINE("UisUtils_Registered_Services: %d\n",
-		  atomic_read(&UisUtils_Registered_Services)) < 0)
+		  atomic_read(&uisutils_registered_services)) < 0)
 		goto err_done;
 	if (PLINE("cycles_before_wait %llu wait_cycles:%llu\n",
 		  cycles_before_wait, wait_cycles) < 0)
@@ -1294,9 +1221,9 @@ find_dev(u32 busNo, u32 devNo)
 
 	read_lock(&BusListLock);
 	for (bus = BusListHead; bus; bus = bus->next) {
-		if (bus->busNo == busNo) {
+		if (bus->bus_no == busNo) {
 			/* make sure the device number is valid */
-			if (devNo >= bus->deviceCount) {
+			if (devNo >= bus->device_count) {
 				LOGERR("%s bad busNo, devNo=%d,%d",
 				       __func__,
 				       (int) (busNo), (int) (devNo));
@@ -1560,13 +1487,13 @@ uislib_mod_init(void)
 	LOGINF("sizeof(uiscmdrsp_net):%lu\n",
 	       (ulong) sizeof(struct uiscmdrsp_net));
 	LOGINF("sizeof(CONTROLVM_MESSAGE):%lu bytes\n",
-	       (ulong) sizeof(CONTROLVM_MESSAGE));
-	LOGINF("sizeof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL):%lu bytes\n",
-	       (ulong) sizeof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL));
+	       (ulong) sizeof(struct controlvm_message));
+	LOGINF("sizeof(struct spar_controlvm_channel_protocol):%lu bytes\n",
+	       (ulong) sizeof(struct spar_controlvm_channel_protocol));
 	LOGINF("sizeof(CHANNEL_HEADER):%lu bytes\n",
-	       (ulong) sizeof(CHANNEL_HEADER));
-	LOGINF("sizeof(ULTRA_IO_CHANNEL_PROTOCOL):%lu bytes\n",
-	       (ulong) sizeof(ULTRA_IO_CHANNEL_PROTOCOL));
+	       (ulong) sizeof(struct channel_header));
+	LOGINF("sizeof(struct spar_io_channel_protocol):%lu bytes\n",
+	       (ulong) sizeof(struct spar_io_channel_protocol));
 	LOGINF("SIZEOF_CMDRSP:%lu bytes\n", SIZEOF_CMDRSP);
 	LOGINF("SIZEOF_PROTOCOL:%lu bytes\n", SIZEOF_PROTOCOL);
 
@@ -1574,7 +1501,7 @@ uislib_mod_init(void)
 	BusListHead = NULL;
 	BusListCount = MaxBusCount = 0;
 	rwlock_init(&BusListLock);
-	VirtControlChanFunc = NULL;
+	virt_control_chan_func = NULL;
 
 	/* Issue VMCALL_GET_CONTROLVM_ADDR to get CtrlChanPhysAddr and
 	 * then map this physical address to a virtual address. */
diff --git a/drivers/staging/unisys/uislib/uisqueue.c b/drivers/staging/unisys/uislib/uisqueue.c
index 44208841bd5a..f9f8442d58c5 100644
--- a/drivers/staging/unisys/uislib/uisqueue.c
+++ b/drivers/staging/unisys/uislib/uisqueue.c
@@ -81,13 +81,13 @@ do_locked_client_insert(struct uisqueue_info *queueinfo,
 	u8 rc = 0;
 
 	spin_lock_irqsave(lock, flags);
-	if (!ULTRA_CHANNEL_CLIENT_ACQUIRE_OS(queueinfo->chan, channelId, NULL))
+	if (!spar_channel_client_acquire_os(queueinfo->chan, channelId))
 		goto unlock;
-	if (visor_signal_insert(queueinfo->chan, whichqueue, pSignal)) {
+	if (spar_signal_insert(queueinfo->chan, whichqueue, pSignal)) {
 		queueinfo->packets_sent++;
 		rc = 1;
 	}
-	ULTRA_CHANNEL_CLIENT_RELEASE_OS(queueinfo->chan, channelId, NULL);
+	spar_channel_client_release_os(queueinfo->chan, channelId);
 unlock:
 	spin_unlock_irqrestore((spinlock_t *)lock, flags);
 	return rc;
@@ -125,7 +125,7 @@ int
 uisqueue_get_cmdrsp(struct uisqueue_info *queueinfo,
 		    void *cmdrsp, unsigned int whichqueue)
 {
-	if (!visor_signal_remove(queueinfo->chan, whichqueue, cmdrsp))
+	if (!spar_signal_remove(queueinfo->chan, whichqueue, cmdrsp))
 		return 0;
 
 	queueinfo->packets_received++;
diff --git a/drivers/staging/unisys/uislib/uisutils.c b/drivers/staging/unisys/uislib/uisutils.c
index 8ff6d26ff00b..4a5b86773927 100644
--- a/drivers/staging/unisys/uislib/uisutils.c
+++ b/drivers/staging/unisys/uislib/uisutils.c
@@ -25,9 +25,7 @@
 #include "uisutils.h"
 #include "version.h"
 #include "vbushelper.h"
-#include <linux/uuid.h>
 #include <linux/skbuff.h>
-#include <linux/uuid.h>
 #ifdef CONFIG_HIGHMEM
 #include <linux/highmem.h>
 #endif
@@ -39,7 +37,7 @@
 #define __MYFILE__ "uisutils.c"
 
 /* exports */
-atomic_t UisUtils_Registered_Services = ATOMIC_INIT(0);
+atomic_t uisutils_registered_services = ATOMIC_INIT(0);
 					/* num registrations via
 					 * uisctrl_register_req_handler() or
 					 * uisctrl_register_req_handler_ex() */
@@ -75,20 +73,20 @@ EXPORT_SYMBOL_GPL(uisutil_add_proc_line_ex);
 
 int
 uisctrl_register_req_handler(int type, void *fptr,
-			     ULTRA_VBUS_DEVICEINFO *chipset_driver_info)
+			     struct ultra_vbus_deviceinfo *chipset_driver_info)
 {
 	LOGINF("type = %d, fptr = 0x%p.\n", type, fptr);
 
 	switch (type) {
 	case 2:
 		if (fptr) {
-			if (!VirtControlChanFunc)
-				atomic_inc(&UisUtils_Registered_Services);
-			VirtControlChanFunc = fptr;
+			if (!virt_control_chan_func)
+				atomic_inc(&uisutils_registered_services);
+			virt_control_chan_func = fptr;
 		} else {
-			if (VirtControlChanFunc)
-				atomic_dec(&UisUtils_Registered_Services);
-			VirtControlChanFunc = NULL;
+			if (virt_control_chan_func)
+				atomic_dec(&uisutils_registered_services);
+			virt_control_chan_func = NULL;
 		}
 		break;
 
@@ -105,76 +103,75 @@ uisctrl_register_req_handler(int type, void *fptr,
 EXPORT_SYMBOL_GPL(uisctrl_register_req_handler);
 
 int
-uisctrl_register_req_handler_ex(uuid_le switchTypeGuid,
-				const char *switch_type_name,
-				int (*controlfunc)(struct io_msgs *),
-				unsigned long min_channel_bytes,
-				int (*Server_Channel_Ok)(unsigned long
-							  channelBytes),
-				int (*Server_Channel_Init)
-				 (void *x, unsigned char *clientStr,
-				  u32 clientStrLen, u64 bytes),
-				ULTRA_VBUS_DEVICEINFO *chipset_DriverInfo)
+uisctrl_register_req_handler_ex(uuid_le switch_uuid,
+			const char *switch_type_name,
+			int (*controlfunc)(struct io_msgs *),
+			unsigned long min_channel_bytes,
+			int (*server_channel_ok)(unsigned long channel_bytes),
+			int (*server_channel_init)(void *x,
+						unsigned char *client_str,
+						u32 client_str_len, u64 bytes),
+			struct ultra_vbus_deviceinfo *chipset_driver_info)
 {
-	ReqHandlerInfo_t *pReqHandlerInfo;
+	struct req_handler_info *pReqHandlerInfo;
 	int rc = 0;		/* assume failure */
 
 	LOGINF("type=%pUL, controlfunc=0x%p.\n",
-	       &switchTypeGuid, controlfunc);
+	       &switch_uuid, controlfunc);
 	if (!controlfunc) {
-		LOGERR("%pUL: controlfunc must be supplied\n", &switchTypeGuid);
+		LOGERR("%pUL: controlfunc must be supplied\n", &switch_uuid);
 		goto Away;
 	}
-	if (!Server_Channel_Ok) {
+	if (!server_channel_ok) {
 		LOGERR("%pUL: Server_Channel_Ok must be supplied\n",
-				&switchTypeGuid);
+				&switch_uuid);
 		goto Away;
 	}
-	if (!Server_Channel_Init) {
+	if (!server_channel_init) {
 		LOGERR("%pUL: Server_Channel_Init must be supplied\n",
-				&switchTypeGuid);
+				&switch_uuid);
 		goto Away;
 	}
-	pReqHandlerInfo = ReqHandlerAdd(switchTypeGuid,
+	pReqHandlerInfo = req_handler_add(switch_uuid,
 					switch_type_name,
 					controlfunc,
 					min_channel_bytes,
-					Server_Channel_Ok, Server_Channel_Init);
+					server_channel_ok, server_channel_init);
 	if (!pReqHandlerInfo) {
-		LOGERR("failed to add %pUL to server list\n", &switchTypeGuid);
+		LOGERR("failed to add %pUL to server list\n", &switch_uuid);
 		goto Away;
 	}
 
-	atomic_inc(&UisUtils_Registered_Services);
+	atomic_inc(&uisutils_registered_services);
 	rc = 1;			/* success */
 Away:
 	if (rc) {
-		if (chipset_DriverInfo)
-			bus_device_info_init(chipset_DriverInfo, "chipset",
+		if (chipset_driver_info)
+			bus_device_info_init(chipset_driver_info, "chipset",
 					   "uislib", VERSION, NULL);
 	} else
-		LOGERR("failed to register type %pUL.\n", &switchTypeGuid);
+		LOGERR("failed to register type %pUL.\n", &switch_uuid);
 
 	return rc;
 }
 EXPORT_SYMBOL_GPL(uisctrl_register_req_handler_ex);
 
 int
-uisctrl_unregister_req_handler_ex(uuid_le switchTypeGuid)
+uisctrl_unregister_req_handler_ex(uuid_le switch_uuid)
 {
 	int rc = 0;		/* assume failure */
 
-	LOGINF("type=%pUL.\n", &switchTypeGuid);
-	if (ReqHandlerDel(switchTypeGuid) < 0) {
+	LOGINF("type=%pUL.\n", &switch_uuid);
+	if (req_handler_del(switch_uuid) < 0) {
 		LOGERR("failed to remove %pUL from server list\n",
-				&switchTypeGuid);
+				&switch_uuid);
 		goto Away;
 	}
-	atomic_dec(&UisUtils_Registered_Services);
+	atomic_dec(&uisutils_registered_services);
 	rc = 1;			/* success */
 Away:
 	if (!rc)
-		LOGERR("failed to unregister type %pUL.\n", &switchTypeGuid);
+		LOGERR("failed to unregister type %pUL.\n", &switch_uuid);
 	return rc;
 }
 EXPORT_SYMBOL_GPL(uisctrl_unregister_req_handler_ex);
@@ -275,11 +272,11 @@ dolist: if (skb_shinfo(skb)->frag_list) {
 }
 EXPORT_SYMBOL_GPL(uisutil_copy_fragsinfo_from_skb);
 
-static LIST_HEAD(ReqHandlerInfo_list);	/* list of ReqHandlerInfo_t */
+static LIST_HEAD(ReqHandlerInfo_list);	/* list of struct req_handler_info */
 static DEFINE_SPINLOCK(ReqHandlerInfo_list_lock);
 
-ReqHandlerInfo_t *
-ReqHandlerAdd(uuid_le switchTypeGuid,
+struct req_handler_info *
+req_handler_add(uuid_le switch_uuid,
 	      const char *switch_type_name,
 	      int (*controlfunc)(struct io_msgs *),
 	      unsigned long min_channel_bytes,
@@ -287,16 +284,16 @@ ReqHandlerAdd(uuid_le switchTypeGuid,
 	      int (*Server_Channel_Init)
 	       (void *x, unsigned char *clientStr, u32 clientStrLen, u64 bytes))
 {
-	ReqHandlerInfo_t *rc = NULL;
+	struct req_handler_info *rc = NULL;
 
 	rc = kzalloc(sizeof(*rc), GFP_ATOMIC);
 	if (!rc)
 		return NULL;
-	rc->switchTypeGuid = switchTypeGuid;
+	rc->switch_uuid = switch_uuid;
 	rc->controlfunc = controlfunc;
 	rc->min_channel_bytes = min_channel_bytes;
-	rc->Server_Channel_Ok = Server_Channel_Ok;
-	rc->Server_Channel_Init = Server_Channel_Init;
+	rc->server_channel_ok = Server_Channel_Ok;
+	rc->server_channel_init = Server_Channel_Init;
 	if (switch_type_name)
 		strncpy(rc->switch_type_name, switch_type_name,
 			sizeof(rc->switch_type_name) - 1);
@@ -307,16 +304,16 @@ ReqHandlerAdd(uuid_le switchTypeGuid,
 	return rc;
 }
 
-ReqHandlerInfo_t *
-ReqHandlerFind(uuid_le switchTypeGuid)
+struct req_handler_info *
+req_handler_find(uuid_le switch_uuid)
 {
 	struct list_head *lelt, *tmp;
-	ReqHandlerInfo_t *entry = NULL;
+	struct req_handler_info *entry = NULL;
 
 	spin_lock(&ReqHandlerInfo_list_lock);
 	list_for_each_safe(lelt, tmp, &ReqHandlerInfo_list) {
-		entry = list_entry(lelt, ReqHandlerInfo_t, list_link);
-		if (uuid_le_cmp(entry->switchTypeGuid, switchTypeGuid) == 0) {
+		entry = list_entry(lelt, struct req_handler_info, list_link);
+		if (uuid_le_cmp(entry->switch_uuid, switch_uuid) == 0) {
 			spin_unlock(&ReqHandlerInfo_list_lock);
 			return entry;
 		}
@@ -326,16 +323,16 @@ ReqHandlerFind(uuid_le switchTypeGuid)
 }
 
 int
-ReqHandlerDel(uuid_le switchTypeGuid)
+req_handler_del(uuid_le switch_uuid)
 {
 	struct list_head *lelt, *tmp;
-	ReqHandlerInfo_t *entry = NULL;
+	struct req_handler_info *entry = NULL;
 	int rc = -1;
 
 	spin_lock(&ReqHandlerInfo_list_lock);
 	list_for_each_safe(lelt, tmp, &ReqHandlerInfo_list) {
-		entry = list_entry(lelt, ReqHandlerInfo_t, list_link);
-		if (uuid_le_cmp(entry->switchTypeGuid, switchTypeGuid) == 0) {
+		entry = list_entry(lelt, struct req_handler_info, list_link);
+		if (uuid_le_cmp(entry->switch_uuid, switch_uuid) == 0) {
 			list_del(lelt);
 			kfree(entry);
 			rc++;
diff --git a/drivers/staging/unisys/virthba/virthba.c b/drivers/staging/unisys/virthba/virthba.c
index 938e2c82c1ab..d7a629b5f111 100644
--- a/drivers/staging/unisys/virthba/virthba.c
+++ b/drivers/staging/unisys/virthba/virthba.c
@@ -101,7 +101,6 @@ static DEF_SCSI_QCMD(virthba_queue_command)
 #define virthba_queue_command virthba_queue_command_lck
 #endif
 
-
 static int virthba_slave_alloc(struct scsi_device *scsidev);
 static int virthba_slave_configure(struct scsi_device *scsidev);
 static void virthba_slave_destroy(struct scsi_device *scsidev);
@@ -172,7 +171,7 @@ struct virthba_info {
 	struct virtpci_dev *virtpcidev;
 	struct list_head dev_info_list;
 	struct chaninfo chinfo;
-	struct InterruptInfo intr;	/* use recvInterrupt info to receive
+	struct irq_info intr;		/* use recvInterrupt info to receive
 					   interrupts when IOs complete */
 	int interrupt_vector;
 	struct scsipending pending[MAX_PENDING_REQUESTS]; /* Tracks the requests
@@ -420,8 +419,8 @@ static irqreturn_t
 virthba_ISR(int irq, void *dev_id)
 {
 	struct virthba_info *virthbainfo = (struct virthba_info *) dev_id;
-	CHANNEL_HEADER __iomem *pChannelHeader;
-	SIGNAL_QUEUE_HEADER __iomem *pqhdr;
+	struct channel_header __iomem *pChannelHeader;
+	struct signal_queue_header __iomem *pqhdr;
 	u64 mask;
 	unsigned long long rc1;
 
@@ -429,24 +428,24 @@ virthba_ISR(int irq, void *dev_id)
 		return IRQ_NONE;
 	virthbainfo->interrupts_rcvd++;
 	pChannelHeader = virthbainfo->chinfo.queueinfo->chan;
-	if (((readq(&pChannelHeader->Features)
+	if (((readq(&pChannelHeader->features)
 	      & ULTRA_IO_IOVM_IS_OK_WITH_DRIVER_DISABLING_INTS) != 0)
-	    && ((readq(&pChannelHeader->Features) &
+	    && ((readq(&pChannelHeader->features) &
 		 ULTRA_IO_DRIVER_DISABLES_INTS) !=
 		0)) {
 		virthbainfo->interrupts_disabled++;
 		mask = ~ULTRA_CHANNEL_ENABLE_INTS;
 		rc1 = uisqueue_interlocked_and(virthbainfo->flags_addr, mask);
 	}
-	if (visor_signalqueue_empty(pChannelHeader, IOCHAN_FROM_IOPART)) {
+	if (spar_signalqueue_empty(pChannelHeader, IOCHAN_FROM_IOPART)) {
 		virthbainfo->interrupts_notme++;
 		return IRQ_NONE;
 	}
-	pqhdr = (SIGNAL_QUEUE_HEADER __iomem *)
+	pqhdr = (struct signal_queue_header __iomem *)
 		((char __iomem *) pChannelHeader +
-		 readq(&pChannelHeader->oChannelSpace)) + IOCHAN_FROM_IOPART;
-	writeq(readq(&pqhdr->NumInterruptsReceived) + 1,
-	       &pqhdr->NumInterruptsReceived);
+		 readq(&pChannelHeader->ch_space_offset)) + IOCHAN_FROM_IOPART;
+	writeq(readq(&pqhdr->num_irq_received) + 1,
+	       &pqhdr->num_irq_received);
 	atomic_set(&virthbainfo->interrupt_rcvd, 1);
 	wake_up_interruptible(&virthbainfo->rsp_queue);
 	return IRQ_HANDLED;
@@ -461,17 +460,17 @@ virthba_probe(struct virtpci_dev *virtpcidev, const struct pci_device_id *id)
 	int rsp;
 	int i;
 	irq_handler_t handler = virthba_ISR;
-	CHANNEL_HEADER __iomem *pChannelHeader;
-	SIGNAL_QUEUE_HEADER __iomem *pqhdr;
+	struct channel_header __iomem *pChannelHeader;
+	struct signal_queue_header __iomem *pqhdr;
 	u64 mask;
 
 	LOGVER("entering virthba_probe...\n");
-	LOGVER("virtpcidev busNo<<%d>>devNo<<%d>>", virtpcidev->busNo,
-	       virtpcidev->deviceNo);
+	LOGVER("virtpcidev bus_no<<%d>>devNo<<%d>>", virtpcidev->bus_no,
+	       virtpcidev->device_no);
 
 	LOGINF("entering virthba_probe...\n");
-	LOGINF("virtpcidev busNo<<%d>>devNo<<%d>>", virtpcidev->busNo,
-	       virtpcidev->deviceNo);
+	LOGINF("virtpcidev bus_no<<%d>>devNo<<%d>>", virtpcidev->bus_no,
+	       virtpcidev->device_no);
 	POSTCODE_LINUX_2(VHBA_PROBE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
 	/* call scsi_host_alloc to register a scsi host adapter
 	 * instance - this virthba that has just been created is an
@@ -578,18 +577,18 @@ virthba_probe(struct virtpci_dev *virtpcidev, const struct pci_device_id *id)
 	INIT_WORK(&virthbainfo->serverdown_completion,
 		  virthba_serverdown_complete);
 
-	writeq(readq(&virthbainfo->chinfo.queueinfo->chan->Features) |
+	writeq(readq(&virthbainfo->chinfo.queueinfo->chan->features) |
 	       ULTRA_IO_CHANNEL_IS_POLLING,
-	       &virthbainfo->chinfo.queueinfo->chan->Features);
+	       &virthbainfo->chinfo.queueinfo->chan->features);
 	/* start thread that will receive scsicmnd responses */
 	DBGINF("starting rsp thread -- queueinfo: 0x%p, threadinfo: 0x%p.\n",
 	       virthbainfo->chinfo.queueinfo, &virthbainfo->chinfo.threadinfo);
 
 	pChannelHeader = virthbainfo->chinfo.queueinfo->chan;
-	pqhdr = (SIGNAL_QUEUE_HEADER __iomem *)
+	pqhdr = (struct signal_queue_header __iomem *)
 		((char __iomem *)pChannelHeader +
-		 readq(&pChannelHeader->oChannelSpace)) + IOCHAN_FROM_IOPART;
-	virthbainfo->flags_addr = &pqhdr->FeatureFlags;
+		 readq(&pChannelHeader->ch_space_offset)) + IOCHAN_FROM_IOPART;
+	virthbainfo->flags_addr = &pqhdr->features;
 
 	if (!uisthread_start(&virthbainfo->chinfo.threadinfo,
 			     process_incoming_rsps,
@@ -603,16 +602,16 @@ virthba_probe(struct virtpci_dev *virtpcidev, const struct pci_device_id *id)
 		return -ENODEV;
 	}
 	LOGINF("sendInterruptHandle=0x%16llX",
-	       virthbainfo->intr.sendInterruptHandle);
+	       virthbainfo->intr.send_irq_handle);
 	LOGINF("recvInterruptHandle=0x%16llX",
-	       virthbainfo->intr.recvInterruptHandle);
+	       virthbainfo->intr.recv_irq_handle);
 	LOGINF("recvInterruptVector=0x%8X",
-	       virthbainfo->intr.recvInterruptVector);
+	       virthbainfo->intr.recv_irq_vector);
 	LOGINF("recvInterruptShared=0x%2X",
-	       virthbainfo->intr.recvInterruptShared);
+	       virthbainfo->intr.recv_irq_shared);
 	LOGINF("scsihost.hostt->name=%s", scsihost->hostt->name);
 	virthbainfo->interrupt_vector =
-	    virthbainfo->intr.recvInterruptHandle & INTERRUPT_VECTOR_MASK;
+	    virthbainfo->intr.recv_irq_handle & INTERRUPT_VECTOR_MASK;
 	rsp = request_irq(virthbainfo->interrupt_vector, handler, IRQF_SHARED,
 			  scsihost->hostt->name, virthbainfo);
 	if (rsp != 0) {
@@ -622,7 +621,7 @@ virthba_probe(struct virtpci_dev *virtpcidev, const struct pci_device_id *id)
 		POSTCODE_LINUX_2(VHBA_PROBE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
 	} else {
 		u64 __iomem *Features_addr =
-		    &virthbainfo->chinfo.queueinfo->chan->Features;
+		    &virthbainfo->chinfo.queueinfo->chan->features;
 		LOGERR("request_irq(%d) uislib_virthba_ISR request succeeded\n",
 		       virthbainfo->interrupt_vector);
 		mask = ~(ULTRA_IO_CHANNEL_IS_POLLING |
@@ -649,8 +648,8 @@ virthba_remove(struct virtpci_dev *virtpcidev)
 	struct Scsi_Host *scsihost =
 	    (struct Scsi_Host *) virtpcidev->scsi.scsihost;
 
-	LOGINF("virtpcidev busNo<<%d>>devNo<<%d>>", virtpcidev->busNo,
-	       virtpcidev->deviceNo);
+	LOGINF("virtpcidev bus_no<<%d>>devNo<<%d>>", virtpcidev->bus_no,
+	       virtpcidev->device_no);
 	virthbainfo = (struct virthba_info *) scsihost->hostdata;
 	if (virthbainfo->interrupt_vector != -1)
 		free_irq(virthbainfo->interrupt_vector, virthbainfo);
@@ -674,7 +673,7 @@ virthba_remove(struct virtpci_dev *virtpcidev)
 }
 
 static int
-forward_vdiskmgmt_command(VDISK_MGMT_TYPES vdiskcmdtype,
+forward_vdiskmgmt_command(enum vdisk_mgmt_types vdiskcmdtype,
 			  struct Scsi_Host *scsihost,
 			  struct uisscsi_dest *vdest)
 {
@@ -738,7 +737,8 @@ forward_vdiskmgmt_command(VDISK_MGMT_TYPES vdiskcmdtype,
 /*****************************************************/
 
 static int
-forward_taskmgmt_command(TASK_MGMT_TYPES tasktype, struct scsi_device *scsidev)
+forward_taskmgmt_command(enum task_mgmt_types tasktype,
+			 struct scsi_device *scsidev)
 {
 	struct uiscmdrsp *cmdrsp;
 	struct virthba_info *virthbainfo =
@@ -1142,9 +1142,9 @@ do_scsi_linuxstat(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
 			       scsicmd, cmdrsp->scsi.cmnd[0],
 			       scsidev->host->host_no, scsidev->id,
 			       scsidev->channel, scsidev->lun,
-			       cmdrsp->scsi.linuxstat, sd->Valid, sd->SenseKey,
-			       sd->AdditionalSenseCode,
-			       sd->AdditionalSenseCodeQualifier);
+			       cmdrsp->scsi.linuxstat, sd->valid, sd->sense_key,
+			       sd->additional_sense_code,
+			       sd->additional_sense_code_qualifier);
 			if (atomic_read(&vdisk->error_count) ==
 			    VIRTHBA_ERROR_COUNT) {
 				LOGERR("Throtling SCSICMD errors disk <%d:%d:%d:%llu>\n",
@@ -1276,8 +1276,8 @@ drain_queue(struct virthba_info *virthbainfo, struct chaninfo *dc,
 
 	while (1) {
 		spin_lock_irqsave(&virthbainfo->chinfo.insertlock, flags);
-		if (!ULTRA_CHANNEL_CLIENT_ACQUIRE_OS(dc->queueinfo->chan,
-						     "vhba", NULL)) {
+		if (!spar_channel_client_acquire_os(dc->queueinfo->chan,
+						     "vhba")) {
 			spin_unlock_irqrestore(&virthbainfo->chinfo.insertlock,
 					       flags);
 			virthbainfo->acquire_failed_cnt++;
@@ -1285,8 +1285,7 @@ drain_queue(struct virthba_info *virthbainfo, struct chaninfo *dc,
 		}
 		qrslt = uisqueue_get_cmdrsp(dc->queueinfo, cmdrsp,
 					    IOCHAN_FROM_IOPART);
-		ULTRA_CHANNEL_CLIENT_RELEASE_OS(dc->queueinfo->chan,
-						"vhba", NULL);
+		spar_channel_client_release_os(dc->queueinfo->chan, "vhba");
 		spin_unlock_irqrestore(&virthbainfo->chinfo.insertlock, flags);
 		if (qrslt == 0)
 			break;
@@ -1310,7 +1309,7 @@ drain_queue(struct virthba_info *virthbainfo, struct chaninfo *dc,
 			 * a Client/Guest Partition. Let's be
 			 * safe and set it to NULL now.  Do
 			 * not use it here! */
-			cmdrsp->disknotify.vHba = NULL;
+			cmdrsp->disknotify.v_hba = NULL;
 			process_disk_notify(shost, cmdrsp);
 		} else if (cmdrsp->cmdtype == CMD_VDISKMGMT_TYPE) {
 			if (!del_scsipending_entry(virthbainfo,
@@ -1323,7 +1322,6 @@ drain_queue(struct virthba_info *virthbainfo, struct chaninfo *dc,
 	}
 }
 
-
 /* main function for the thread that waits for scsi commands to arrive
  * in a specified queue
  */
@@ -1453,7 +1451,7 @@ static ssize_t enable_ints_write(struct file *file,
 		if (VirtHbasOpen[i].virthbainfo != NULL) {
 			virthbainfo = VirtHbasOpen[i].virthbainfo;
 			Features_addr =
-				&virthbainfo->chinfo.queueinfo->chan->Features;
+				&virthbainfo->chinfo.queueinfo->chan->features;
 			if (new_value == 1) {
 				mask = ~(ULTRA_IO_CHANNEL_IS_POLLING |
 					 ULTRA_IO_DRIVER_DISABLES_INTS);
@@ -1482,8 +1480,8 @@ virthba_serverup(struct virtpci_dev *virtpcidev)
 	    (struct virthba_info *) ((struct Scsi_Host *) virtpcidev->scsi.
 				     scsihost)->hostdata;
 
-	DBGINF("virtpcidev busNo<<%d>>devNo<<%d>>", virtpcidev->busNo,
-	       virtpcidev->deviceNo);
+	DBGINF("virtpcidev bus_no<<%d>>devNo<<%d>>", virtpcidev->bus_no,
+	       virtpcidev->device_no);
 
 	if (!virthbainfo->serverdown) {
 		DBGINF("Server up message received while server is already up.\n");
@@ -1498,9 +1496,9 @@ virthba_serverup(struct virtpci_dev *virtpcidev)
 	/* Must transition channel to ATTACHED state BEFORE we
 	 * can start using the device again
 	 */
-	ULTRA_CHANNEL_CLIENT_TRANSITION(virthbainfo->chinfo.queueinfo->chan,
-					dev_name(&virtpcidev->generic_dev),
-					CHANNELCLI_ATTACHED, NULL);
+	SPAR_CHANNEL_CLIENT_TRANSITION(virthbainfo->chinfo.queueinfo->chan,
+				       dev_name(&virtpcidev->generic_dev),
+				       CHANNELCLI_ATTACHED, NULL);
 
 	/* Start Processing the IOVM Response Queue Again */
 	if (!uisthread_start(&virthbainfo->chinfo.threadinfo,
@@ -1573,13 +1571,13 @@ virthba_serverdown_complete(struct work_struct *work)
 
 	virtpcidev = virthbainfo->virtpcidev;
 
-	DBGINF("virtpcidev busNo<<%d>>devNo<<%d>>", virtpcidev->busNo,
-	       virtpcidev->deviceNo);
+	DBGINF("virtpcidev bus_no<<%d>>devNo<<%d>>", virtpcidev->bus_no,
+	       virtpcidev->device_no);
 	virthbainfo->serverdown = true;
 	virthbainfo->serverchangingstate = false;
 	/* Return the ServerDown response to Command */
-	visorchipset_device_pause_response(virtpcidev->busNo,
-					   virtpcidev->deviceNo, 0);
+	visorchipset_device_pause_response(virtpcidev->bus_no,
+					   virtpcidev->device_no, 0);
 }
 
 /* As per VirtpciFunc returns 1 for success and 0 for failure */
@@ -1591,8 +1589,8 @@ virthba_serverdown(struct virtpci_dev *virtpcidev, u32 state)
 				     scsihost)->hostdata;
 
 	DBGINF("virthba_serverdown");
-	DBGINF("virtpcidev busNo<<%d>>devNo<<%d>>", virtpcidev->busNo,
-	       virtpcidev->deviceNo);
+	DBGINF("virtpcidev bus_no<<%d>>devNo<<%d>>", virtpcidev->bus_no,
+	       virtpcidev->device_no);
 
 	if (!virthbainfo->serverdown && !virthbainfo->serverchangingstate) {
 		virthbainfo->serverchangingstate = true;
diff --git a/drivers/staging/unisys/virthba/virthba.h b/drivers/staging/unisys/virthba/virthba.h
index d4b809b0c7bc..59901668d4f4 100644
--- a/drivers/staging/unisys/virthba/virthba.h
+++ b/drivers/staging/unisys/virthba/virthba.h
@@ -19,13 +19,9 @@
  * Unisys Virtual HBA driver header
  */
 
-
-
 #ifndef __VIRTHBA_H__
 #define __VIRTHBA_H__
 
-
 #define VIRTHBA_VERSION "01.00"
 
-
 #endif /* __VIRTHBA_H__ */
diff --git a/drivers/staging/unisys/virtpci/virtpci.c b/drivers/staging/unisys/virtpci/virtpci.c
index ee9f8260cd15..39b828dce503 100644
--- a/drivers/staging/unisys/virtpci/virtpci.c
+++ b/drivers/staging/unisys/virtpci/virtpci.c
@@ -50,6 +50,7 @@ struct driver_private {
 	struct module_kobject *mkobj;
 	struct device_driver *driver;
 };
+
 #define to_driver(obj) container_of(obj, struct driver_private, kobj)
 
 /* bus_id went away in 2.6.30 - the size was 20 bytes, so we'll define
@@ -109,7 +110,7 @@ static int virtpci_device_probe(struct device *dev);
 static int virtpci_device_remove(struct device *dev);
 
 static ssize_t info_debugfs_read(struct file *file, char __user *buf,
-			      size_t len, loff_t *offset);
+				 size_t len, loff_t *offset);
 
 static const struct file_operations debugfs_info_fops = {
 	.read = info_debugfs_read,
@@ -137,7 +138,7 @@ static struct device virtpci_rootbus_device = {
 };
 
 /* filled in with info about parent chipset driver when we register with it */
-static ULTRA_VBUS_DEVICEINFO Chipset_DriverInfo;
+static struct ultra_vbus_deviceinfo chipset_driver_info;
 
 static const struct sysfs_ops virtpci_driver_sysfs_ops = {
 	.show = virtpci_driver_attr_show,
@@ -148,11 +149,11 @@ static struct kobj_type virtpci_driver_kobj_type = {
 	.sysfs_ops = &virtpci_driver_sysfs_ops,
 };
 
-static struct virtpci_dev *VpcidevListHead;
-static DEFINE_RWLOCK(VpcidevListLock);
+static struct virtpci_dev *vpcidev_list_head;
+static DEFINE_RWLOCK(vpcidev_list_lock);
 
 /* filled in with info about this driver, wrt it servicing client busses */
-static ULTRA_VBUS_DEVICEINFO Bus_DriverInfo;
+static struct ultra_vbus_deviceinfo bus_driver_info;
 
 /*****************************************************/
 /* debugfs entries                                   */
@@ -171,13 +172,12 @@ struct virtpci_busdev {
 /*****************************************************/
 
 static inline
-int WAIT_FOR_IO_CHANNEL(ULTRA_IO_CHANNEL_PROTOCOL __iomem  *chanptr)
+int WAIT_FOR_IO_CHANNEL(struct spar_io_channel_protocol __iomem  *chanptr)
 {
 	int count = 120;
 
 	while (count > 0) {
-
-		if (ULTRA_CHANNEL_SERVER_READY(&chanptr->ChannelHeader))
+		if (SPAR_CHANNEL_SERVER_READY(&chanptr->channel_header))
 			return 1;
 		UIS_THREAD_WAIT_SEC(1);
 		count--;
@@ -186,8 +186,8 @@ int WAIT_FOR_IO_CHANNEL(ULTRA_IO_CHANNEL_PROTOCOL __iomem  *chanptr)
 }
 
 /* Write the contents of <info> to the ULTRA_VBUS_CHANNEL_PROTOCOL.ChpInfo. */
-static int write_vbus_chpInfo(ULTRA_VBUS_CHANNEL_PROTOCOL *chan,
-			      ULTRA_VBUS_DEVICEINFO *info)
+static int write_vbus_chp_info(struct spar_vbus_channel_protocol *chan,
+			       struct ultra_vbus_deviceinfo *info)
 {
 	int off;
 
@@ -195,18 +195,18 @@ static int write_vbus_chpInfo(ULTRA_VBUS_CHANNEL_PROTOCOL *chan,
 		LOGERR("vbus channel not present");
 		return -1;
 	}
-	off = sizeof(ULTRA_CHANNEL_PROTOCOL) + chan->HdrInfo.chpInfoByteOffset;
-	if (chan->HdrInfo.chpInfoByteOffset == 0) {
-		LOGERR("vbus channel not used, because chpInfoByteOffset == 0");
+	off = sizeof(struct channel_header) + chan->hdr_info.chp_info_offset;
+	if (chan->hdr_info.chp_info_offset == 0) {
+		LOGERR("vbus channel not used, because chp_info_offset == 0");
 		return -1;
 	}
-	memcpy(((u8 *) (chan)) + off, info, sizeof(*info));
+	memcpy(((u8 *)(chan)) + off, info, sizeof(*info));
 	return 0;
 }
 
 /* Write the contents of <info> to the ULTRA_VBUS_CHANNEL_PROTOCOL.BusInfo. */
-static int write_vbus_busInfo(ULTRA_VBUS_CHANNEL_PROTOCOL *chan,
-			      ULTRA_VBUS_DEVICEINFO *info)
+static int write_vbus_bus_info(struct spar_vbus_channel_protocol *chan,
+			       struct ultra_vbus_deviceinfo *info)
 {
 	int off;
 
@@ -214,12 +214,12 @@ static int write_vbus_busInfo(ULTRA_VBUS_CHANNEL_PROTOCOL *chan,
 		LOGERR("vbus channel not present");
 		return -1;
 	}
-	off = sizeof(ULTRA_CHANNEL_PROTOCOL) + chan->HdrInfo.busInfoByteOffset;
-	if (chan->HdrInfo.busInfoByteOffset == 0) {
-		LOGERR("vbus channel not used, because busInfoByteOffset == 0");
+	off = sizeof(struct channel_header) + chan->hdr_info.bus_info_offset;
+	if (chan->hdr_info.bus_info_offset == 0) {
+		LOGERR("vbus channel not used, because bus_info_offset == 0");
 		return -1;
 	}
-	memcpy(((u8 *) (chan)) + off, info, sizeof(*info));
+	memcpy(((u8 *)(chan)) + off, info, sizeof(*info));
 	return 0;
 }
 
@@ -227,8 +227,8 @@ static int write_vbus_busInfo(ULTRA_VBUS_CHANNEL_PROTOCOL *chan,
  * ULTRA_VBUS_CHANNEL_PROTOCOL.DevInfo[<devix>].
  */
 static int
-write_vbus_devInfo(ULTRA_VBUS_CHANNEL_PROTOCOL *chan,
-		   ULTRA_VBUS_DEVICEINFO *info, int devix)
+write_vbus_dev_info(struct spar_vbus_channel_protocol *chan,
+		    struct ultra_vbus_deviceinfo *info, int devix)
 {
 	int off;
 
@@ -237,14 +237,14 @@ write_vbus_devInfo(ULTRA_VBUS_CHANNEL_PROTOCOL *chan,
 		return -1;
 	}
 	off =
-	    (sizeof(ULTRA_CHANNEL_PROTOCOL) +
-	     chan->HdrInfo.devInfoByteOffset) +
-	    (chan->HdrInfo.deviceInfoStructBytes * devix);
-	if (chan->HdrInfo.devInfoByteOffset == 0) {
-		LOGERR("vbus channel not used, because devInfoByteOffset == 0");
+	    (sizeof(struct channel_header) +
+	     chan->hdr_info.dev_info_offset) +
+	    (chan->hdr_info.device_info_struct_bytes * devix);
+	if (chan->hdr_info.dev_info_offset == 0) {
+		LOGERR("vbus channel not used, because dev_info_offset == 0");
 		return -1;
 	}
-	memcpy(((u8 *) (chan)) + off, info, sizeof(*info));
+	memcpy(((u8 *)(chan)) + off, info, sizeof(*info));
 	return 0;
 }
 
@@ -256,13 +256,13 @@ static int add_vbus(struct add_vbus_guestpart *addparams)
 	int ret;
 	struct device *vbus;
 
-	vbus = kzalloc(sizeof(struct device), GFP_ATOMIC);
+	vbus = kzalloc(sizeof(*vbus), GFP_ATOMIC);
 
 	POSTCODE_LINUX_2(VPCI_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
 	if (!vbus)
 		return 0;
 
-	dev_set_name(vbus, "vbus%d", addparams->busNo);
+	dev_set_name(vbus, "vbus%d", addparams->bus_no);
 	vbus->release = virtpci_bus_release;
 	vbus->parent = &virtpci_rootbus_device;	/* root bus is parent */
 	vbus->bus = &virtpci_bus_type;	/* bus type */
@@ -279,11 +279,12 @@ static int add_vbus(struct add_vbus_guestpart *addparams)
 		POSTCODE_LINUX_2(VPCI_CREATE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
 		return 0;
 	}
-	write_vbus_chpInfo(vbus->platform_data /* chanptr */ ,
-			   &Chipset_DriverInfo);
-	write_vbus_busInfo(vbus->platform_data /* chanptr */ , &Bus_DriverInfo);
+	write_vbus_chp_info(vbus->platform_data /* chanptr */ ,
+			    &chipset_driver_info);
+	write_vbus_bus_info(vbus->platform_data /* chanptr */ ,
+			    &bus_driver_info);
 	LOGINF("Added vbus %d; device %s created successfully\n",
-	       addparams->busNo, BUS_ID(vbus));
+	       addparams->bus_no, BUS_ID(vbus));
 	POSTCODE_LINUX_2(VPCI_CREATE_EXIT_PC, POSTCODE_SEVERITY_INFO);
 	return 1;
 }
@@ -293,26 +294,15 @@ static int add_vbus(struct add_vbus_guestpart *addparams)
  */
 #define GET_SCSIADAPINFO_FROM_CHANPTR(chanptr) {			\
 	memcpy_fromio(&scsi.wwnn,					\
-		      &((ULTRA_IO_CHANNEL_PROTOCOL __iomem *)		\
+		      &((struct spar_io_channel_protocol __iomem *)	\
 			chanptr)->vhba.wwnn,				\
 		      sizeof(struct vhba_wwnn));			\
 	memcpy_fromio(&scsi.max,					\
-		      &((ULTRA_IO_CHANNEL_PROTOCOL __iomem *)		\
+		      &((struct spar_io_channel_protocol __iomem *)	\
 			chanptr)->vhba.max,				\
 		      sizeof(struct vhba_config_max));			\
 	}
 
-/* find bus device with the busid that matches - match_busid matches bus_id */
-#define GET_BUS_DEV(busno) { \
-	sprintf(busid, "vbus%d", busno); \
-	vbus = bus_find_device(&virtpci_bus_type, NULL, \
-			       (void *)busid, match_busid);	\
-	if (!vbus) { \
-		LOGERR("**** FAILED to find vbus %s\n", busid); \
-		return 0; \
-	} \
-}
-
 /* adds a vhba
  * returns 0 failure, 1 success,
  */
@@ -325,7 +315,7 @@ static int add_vhba(struct add_virt_guestpart *addparams)
 
 	POSTCODE_LINUX_2(VPCI_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
 	if (!WAIT_FOR_IO_CHANNEL
-	    ((ULTRA_IO_CHANNEL_PROTOCOL __iomem *) addparams->chanptr)) {
+	    ((struct spar_io_channel_protocol __iomem *)addparams->chanptr)) {
 		LOGERR("Timed out.  Channel not ready\n");
 		POSTCODE_LINUX_2(VPCI_CREATE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
 		return 0;
@@ -333,7 +323,14 @@ static int add_vhba(struct add_virt_guestpart *addparams)
 
 	GET_SCSIADAPINFO_FROM_CHANPTR(addparams->chanptr);
 
-	GET_BUS_DEV(addparams->bus_no);
+	/* find bus device with the busid that matches match_busid */
+	sprintf(busid, "vbus%d", addparams->bus_no);
+	vbus = bus_find_device(&virtpci_bus_type, NULL,
+			       (void *)busid, match_busid);
+	if (!vbus) {
+		LOGERR("**** FAILED to find vbus %s\n", busid);
+		return 0;
+	}
 
 	LOGINF("Adding vhba wwnn:%x:%x config:%d-%d-%d-%d chanptr:%p\n",
 	       scsi.wwnn.wwnn1, scsi.wwnn.wwnn2,
@@ -347,7 +344,6 @@ static int add_vhba(struct add_virt_guestpart *addparams)
 				 POSTCODE_SEVERITY_INFO);
 	}
 	return i;
-
 }
 
 /* for CHANSOCK macaddr is AUTO-GENERATED; for normal channels,
@@ -355,17 +351,17 @@ static int add_vhba(struct add_virt_guestpart *addparams)
  */
 #define GET_NETADAPINFO_FROM_CHANPTR(chanptr) {				\
 		memcpy_fromio(net.mac_addr,				\
-		       ((ULTRA_IO_CHANNEL_PROTOCOL __iomem *)		\
-			chanptr)->vnic.macaddr,				\
+		       ((struct spar_io_channel_protocol __iomem *)	\
+		       chanptr)->vnic.macaddr,				\
 		       MAX_MACADDR_LEN);				\
 		net.num_rcv_bufs =					\
-			readl(&((ULTRA_IO_CHANNEL_PROTOCOL __iomem *)	\
-				chanptr)->vnic.num_rcv_bufs);		\
-		net.mtu = readl(&((ULTRA_IO_CHANNEL_PROTOCOL __iomem *) \
-				  chanptr)->vnic.mtu);			\
-		memcpy_fromio(&net.zoneGuid, \
-			      &((ULTRA_IO_CHANNEL_PROTOCOL __iomem *)	\
-				chanptr)->vnic.zoneGuid,		\
+			readl(&((struct spar_io_channel_protocol __iomem *)\
+			      chanptr)->vnic.num_rcv_bufs);		\
+		net.mtu = readl(&((struct spar_io_channel_protocol __iomem *) \
+				chanptr)->vnic.mtu);			\
+		memcpy_fromio(&net.zone_uuid, \
+			      &((struct spar_io_channel_protocol __iomem *)\
+			      chanptr)->vnic.zone_uuid,		\
 			      sizeof(uuid_le));				\
 }
 
@@ -382,7 +378,7 @@ add_vnic(struct add_virt_guestpart *addparams)
 
 	POSTCODE_LINUX_2(VPCI_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
 	if (!WAIT_FOR_IO_CHANNEL
-	    ((ULTRA_IO_CHANNEL_PROTOCOL __iomem *) addparams->chanptr)) {
+	    ((struct spar_io_channel_protocol __iomem *)addparams->chanptr)) {
 		LOGERR("Timed out, channel not ready\n");
 		POSTCODE_LINUX_2(VPCI_CREATE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
 		return 0;
@@ -390,12 +386,19 @@ add_vnic(struct add_virt_guestpart *addparams)
 
 	GET_NETADAPINFO_FROM_CHANPTR(addparams->chanptr);
 
-	GET_BUS_DEV(addparams->bus_no);
+	/* find bus device with the busid that matches match_busid */
+	sprintf(busid, "vbus%d", addparams->bus_no);
+	vbus = bus_find_device(&virtpci_bus_type, NULL,
+			       (void *)busid, match_busid);
+	if (!vbus) {
+		LOGERR("**** FAILED to find vbus %s\n", busid);
+		return 0;
+	}
 
 	LOGINF("Adding vnic macaddr:%02x:%02x:%02x:%02x:%02x:%02x rcvbufs:%d mtu:%d chanptr:%p%pUL\n",
-	     net.mac_addr[0], net.mac_addr[1], net.mac_addr[2], net.mac_addr[3],
-	     net.mac_addr[4], net.mac_addr[5], net.num_rcv_bufs, net.mtu,
-	     addparams->chanptr, &net.zoneGuid);
+	       net.mac_addr[0], net.mac_addr[1], net.mac_addr[2],
+	       net.mac_addr[3], net.mac_addr[4], net.mac_addr[5],
+	       net.num_rcv_bufs, net.mtu, addparams->chanptr, &net.zone_uuid);
 	i = virtpci_device_add(vbus, VIRTNIC_TYPE, addparams, NULL, &net);
 	if (i) {
 		LOGINF("Added vnic macaddr:%02x:%02x:%02x:%02x:%02x:%02x\n",
@@ -417,7 +420,15 @@ delete_vbus(struct del_vbus_guestpart *delparams)
 	struct device *vbus;
 	unsigned char busid[BUS_ID_SIZE];
 
-	GET_BUS_DEV(delparams->bus_no);
+	/* find bus device with the busid that matches match_busid */
+	sprintf(busid, "vbus%d", delparams->bus_no);
+	vbus = bus_find_device(&virtpci_bus_type, NULL,
+			       (void *)busid, match_busid);
+	if (!vbus) {
+		LOGERR("**** FAILED to find vbus %s\n", busid);
+		return 0;
+	}
+
 	/* ensure that bus has no devices? -- TBD */
 	LOGINF("Deleting %s\n", BUS_ID(vbus));
 	if (delete_vbus_device(vbus, NULL))
@@ -430,9 +441,9 @@ static int
 delete_vbus_device(struct device *vbus, void *data)
 {
 	int checkforroot = (data != NULL);
-	struct device *pDev = &virtpci_rootbus_device;
+	struct device *dev = &virtpci_rootbus_device;
 
-	if ((checkforroot) && match_busid(vbus, (void *) BUS_ID(pDev))) {
+	if ((checkforroot) && match_busid(vbus, (void *)BUS_ID(dev))) {
 		/* skip it - don't delete root bus */
 		LOGINF("skipping root bus\n");
 		return 0;	/* pretend no error */
@@ -590,10 +601,10 @@ static void delete_all(void)
 	struct virtpci_dev *tmpvpcidev, *nextvpcidev;
 
 	/* delete the entire vhba/vnic list in one shot */
-	write_lock_irqsave(&VpcidevListLock, flags);
-	tmpvpcidev = VpcidevListHead;
-	VpcidevListHead = NULL;
-	write_unlock_irqrestore(&VpcidevListLock, flags);
+	write_lock_irqsave(&vpcidev_list_lock, flags);
+	tmpvpcidev = vpcidev_list_head;
+	vpcidev_list_head = NULL;
+	write_unlock_irqrestore(&vpcidev_list_lock, flags);
 
 	/* delete one vhba/vnic at a time */
 	while (tmpvpcidev) {
@@ -607,24 +618,32 @@ static void delete_all(void)
 
 	/* now delete each vbus */
 	if (bus_for_each_dev
-	    (&virtpci_bus_type, NULL, (void *) 1, delete_vbus_device))
+	    (&virtpci_bus_type, NULL, (void *)1, delete_vbus_device))
 		LOGERR("delete of all vbus failed\n");
 }
 
 /* deletes all vnics or vhbas
  * returns 0 failure, 1 success,
  */
-static int delete_all_virt(VIRTPCI_DEV_TYPE devtype, struct del_vbus_guestpart *delparams)
+static int delete_all_virt(enum virtpci_dev_type devtype,
+			   struct del_vbus_guestpart *delparams)
 {
 	int i;
 	unsigned char busid[BUS_ID_SIZE];
 	struct device *vbus;
 
-	GET_BUS_DEV(delparams->bus_no);
+	/* find bus device with the busid that matches match_busid */
+	sprintf(busid, "vbus%d", delparams->bus_no);
+	vbus = bus_find_device(&virtpci_bus_type, NULL,
+			       (void *)busid, match_busid);
+	if (!vbus) {
+		LOGERR("**** FAILED to find vbus %s\n", busid);
+		return 0;
+	}
 
 	if ((devtype != VIRTHBA_TYPE) && (devtype != VIRTNIC_TYPE)) {
 		LOGERR("**** FAILED to delete all devices; devtype:%d not vhba:%d or vnic:%d\n",
-		     devtype, VIRTHBA_TYPE, VIRTNIC_TYPE);
+		       devtype, VIRTHBA_TYPE, VIRTNIC_TYPE);
 		return 0;
 	}
 
@@ -694,10 +713,10 @@ virtpci_match_device(const struct pci_device_id *ids,
 {
 	while (ids->vendor || ids->subvendor || ids->class_mask) {
 		DBGINF("ids->vendor:%x dev->vendor:%x ids->device:%x dev->device:%x\n",
-		     ids->vendor, dev->vendor, ids->device, dev->device);
+		       ids->vendor, dev->vendor, ids->device, dev->device);
 
-		if ((ids->vendor == dev->vendor)
-		    && (ids->device == dev->device))
+		if ((ids->vendor == dev->vendor) &&
+		    (ids->device == dev->device))
 			return ids;
 
 		ids++;
@@ -752,15 +771,15 @@ static int virtpci_device_resume(struct device *dev)
 
 /* For a child device just created on a client bus, fill in
  * information about the driver that is controlling this device into
- * the the appropriate slot within the vbus channel of the bus
+ * the appropriate slot within the vbus channel of the bus
  * instance.
  */
-static void fix_vbus_devInfo(struct device *dev, int devNo, int devType,
-			     struct virtpci_driver *virtpcidrv)
+static void fix_vbus_dev_info(struct device *dev, int dev_no, int dev_type,
+			      struct virtpci_driver *virtpcidrv)
 {
 	struct device *vbus;
-	void *pChan;
-	ULTRA_VBUS_DEVICEINFO devInfo;
+	void *chan;
+	struct ultra_vbus_deviceinfo dev_info;
 	const char *stype;
 
 	if (!dev) {
@@ -776,12 +795,12 @@ static void fix_vbus_devInfo(struct device *dev, int devNo, int devType,
 		LOGERR("%s dev has no parent bus", __func__);
 		return;
 	}
-	pChan = vbus->platform_data;
-	if (!pChan) {
+	chan = vbus->platform_data;
+	if (!chan) {
 		LOGERR("%s dev bus has no channel", __func__);
 		return;
 	}
-	switch (devType) {
+	switch (dev_type) {
 	case PCI_DEVICE_ID_VIRTHBA:
 		stype = "vHBA";
 		break;
@@ -792,17 +811,17 @@ static void fix_vbus_devInfo(struct device *dev, int devNo, int devType,
 		stype = "unknown";
 		break;
 	}
-	bus_device_info_init(&devInfo, stype,
-			   virtpcidrv->name,
-			   virtpcidrv->version,
-			   virtpcidrv->vertag);
-	write_vbus_devInfo(pChan, &devInfo, devNo);
+	bus_device_info_init(&dev_info, stype,
+			     virtpcidrv->name,
+			     virtpcidrv->version,
+			     virtpcidrv->vertag);
+	write_vbus_dev_info(chan, &dev_info, dev_no);
 
 	/* Re-write bus+chipset info, because it is possible that this
 	* was previously written by our good counterpart, visorbus.
 	*/
-	write_vbus_chpInfo(pChan, &Chipset_DriverInfo);
-	write_vbus_busInfo(pChan, &Bus_DriverInfo);
+	write_vbus_chp_info(chan, &chipset_driver_info);
+	write_vbus_bus_info(chan, &bus_driver_info);
 }
 
 /* This function is called to query the existence of a specific device
@@ -842,13 +861,14 @@ static int virtpci_device_probe(struct device *dev)
 		 */
 		error = virtpcidrv->probe(virtpcidev, id);
 		if (!error) {
-			fix_vbus_devInfo(dev, virtpcidev->deviceNo,
-					 virtpcidev->device, virtpcidrv);
+			fix_vbus_dev_info(dev, virtpcidev->device_no,
+					  virtpcidev->device, virtpcidrv);
 			virtpcidev->mydriver = virtpcidrv;
 			POSTCODE_LINUX_2(VPCI_PROBE_EXIT_PC,
 					 POSTCODE_SEVERITY_INFO);
-		} else
+		} else {
 			put_device(dev);
+		}
 	}
 	POSTCODE_LINUX_2(VPCI_PROBE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
 	return error;		/* -ENODEV for probe failure */
@@ -896,17 +916,20 @@ static void virtpci_bus_release(struct device *dev)
 /* Adapter functions                                 */
 /*****************************************************/
 
+/* scsi is expected to be NULL for VNIC add
+ * net is expected to be NULL for VHBA add
+ */
 static int virtpci_device_add(struct device *parentbus, int devtype,
 			      struct add_virt_guestpart *addparams,
-			      struct scsi_adap_info *scsi, /* NULL for VNIC add */
-			      struct net_adap_info *net	/* NULL for VHBA add */)
+			      struct scsi_adap_info *scsi,
+			      struct net_adap_info *net)
 {
 	struct virtpci_dev *virtpcidev = NULL;
 	struct virtpci_dev *tmpvpcidev = NULL, *prev;
 	unsigned long flags;
 	int ret;
-	ULTRA_IO_CHANNEL_PROTOCOL __iomem *pIoChan = NULL;
-	struct device *pDev;
+	struct spar_io_channel_protocol __iomem *io_chan = NULL;
+	struct device *dev;
 
 	LOGINF("virtpci_device_add parentbus:%p chanptr:%p\n", parentbus,
 	       addparams->chanptr);
@@ -915,14 +938,14 @@ static int virtpci_device_add(struct device *parentbus, int devtype,
 
 	if ((devtype != VIRTHBA_TYPE) && (devtype != VIRTNIC_TYPE)) {
 		LOGERR("**** FAILED to add device; devtype:%d not vhba:%d or vnic:%d\n",
-		     devtype, VIRTHBA_TYPE, VIRTNIC_TYPE);
+		       devtype, VIRTHBA_TYPE, VIRTNIC_TYPE);
 		POSTCODE_LINUX_3(VPCI_CREATE_FAILURE_PC, devtype,
 				 POSTCODE_SEVERITY_ERR);
 		return 0;
 	}
 
 	/* add a Virtual Device */
-	virtpcidev = kzalloc(sizeof(struct virtpci_dev), GFP_ATOMIC);
+	virtpcidev = kzalloc(sizeof(*virtpcidev), GFP_ATOMIC);
 	if (virtpcidev == NULL) {
 		LOGERR("can't add device - malloc FALLED\n");
 		POSTCODE_LINUX_2(MALLOC_FAILURE_PC, POSTCODE_SEVERITY_ERR);
@@ -939,14 +962,14 @@ static int virtpci_device_add(struct device *parentbus, int devtype,
 		virtpcidev->net = *net;
 	}
 	virtpcidev->vendor = PCI_VENDOR_ID_UNISYS;
-	virtpcidev->busNo = addparams->bus_no;
-	virtpcidev->deviceNo = addparams->device_no;
+	virtpcidev->bus_no = addparams->bus_no;
+	virtpcidev->device_no = addparams->device_no;
 
 	virtpcidev->queueinfo.chan = addparams->chanptr;
 	virtpcidev->queueinfo.send_int_if_needed = NULL;
 
 	/* Set up safe queue... */
-	pIoChan = (ULTRA_IO_CHANNEL_PROTOCOL __iomem *)
+	io_chan = (struct spar_io_channel_protocol __iomem *)
 		virtpcidev->queueinfo.chan;
 
 	virtpcidev->intr = addparams->intr;
@@ -962,8 +985,8 @@ static int virtpci_device_add(struct device *parentbus, int devtype,
 	/* add the vhba/vnic to virtpci device list - but check for
 	 * duplicate wwnn/macaddr first
 	 */
-	write_lock_irqsave(&VpcidevListLock, flags);
-	for (tmpvpcidev = VpcidevListHead; tmpvpcidev;
+	write_lock_irqsave(&vpcidev_list_lock, flags);
+	for (tmpvpcidev = vpcidev_list_head; tmpvpcidev;
 	     tmpvpcidev = tmpvpcidev->next) {
 		if (devtype == VIRTHBA_TYPE) {
 			if ((tmpvpcidev->scsi.wwnn.wwnn1 == scsi->wwnn.wwnn1) &&
@@ -984,7 +1007,7 @@ static int virtpci_device_add(struct device *parentbus, int devtype,
 		/* found a vhba/vnic already in the list with same
 		 * wwnn or macaddr - reject add
 		 */
-		write_unlock_irqrestore(&VpcidevListLock, flags);
+		write_unlock_irqrestore(&vpcidev_list_lock, flags);
 		kfree(virtpcidev);
 		LOGERR("**** FAILED vhba/vnic already exists in the list\n");
 		POSTCODE_LINUX_2(VPCI_CREATE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
@@ -992,26 +1015,26 @@ static int virtpci_device_add(struct device *parentbus, int devtype,
 	}
 
 	/* add it at the head */
-	if (!VpcidevListHead)
-		VpcidevListHead = virtpcidev;
-	else {
+	if (!vpcidev_list_head) {
+		vpcidev_list_head = virtpcidev;
+	} else {
 		/* insert virtpcidev at the head of our linked list of
 		 * vpcidevs
 		 */
-		virtpcidev->next = VpcidevListHead;
-		VpcidevListHead = virtpcidev;
+		virtpcidev->next = vpcidev_list_head;
+		vpcidev_list_head = virtpcidev;
 	}
 
-	write_unlock_irqrestore(&VpcidevListLock, flags);
+	write_unlock_irqrestore(&vpcidev_list_lock, flags);
 
 	/* Must transition channel to ATTACHED state BEFORE
 	 * registering the device, because polling of the channel
 	 * queues can begin at any time after device_register().
 	 */
-	pDev = &virtpcidev->generic_dev;
-	ULTRA_CHANNEL_CLIENT_TRANSITION(addparams->chanptr,
-					BUS_ID(pDev),
-					CHANNELCLI_ATTACHED, NULL);
+	dev = &virtpcidev->generic_dev;
+	SPAR_CHANNEL_CLIENT_TRANSITION(addparams->chanptr,
+				       BUS_ID(dev),
+				       CHANNELCLI_ATTACHED, NULL);
 
 	/* don't register until device has been added to
 	* list. Otherwise, a device_unregister from this function can
@@ -1031,24 +1054,24 @@ static int virtpci_device_add(struct device *parentbus, int devtype,
 	 */
 	if (ret) {
 		LOGERR("device_register returned %d\n", ret);
-		pDev = &virtpcidev->generic_dev;
-		ULTRA_CHANNEL_CLIENT_TRANSITION(addparams->chanptr,
-						BUS_ID(pDev),
-						CHANNELCLI_DETACHED, NULL);
+		dev = &virtpcidev->generic_dev;
+		SPAR_CHANNEL_CLIENT_TRANSITION(addparams->chanptr,
+					       BUS_ID(dev),
+					       CHANNELCLI_DETACHED, NULL);
 		/* remove virtpcidev, the one we just added, from the list */
-		write_lock_irqsave(&VpcidevListLock, flags);
-		for (tmpvpcidev = VpcidevListHead, prev = NULL;
+		write_lock_irqsave(&vpcidev_list_lock, flags);
+		for (tmpvpcidev = vpcidev_list_head, prev = NULL;
 		     tmpvpcidev;
 		     prev = tmpvpcidev, tmpvpcidev = tmpvpcidev->next) {
 			if (tmpvpcidev == virtpcidev) {
 				if (prev)
 					prev->next = tmpvpcidev->next;
 				else
-					VpcidevListHead = tmpvpcidev->next;
+					vpcidev_list_head = tmpvpcidev->next;
 				break;
 			}
 		}
-		write_unlock_irqrestore(&VpcidevListLock, flags);
+		write_unlock_irqrestore(&vpcidev_list_lock, flags);
 		kfree(virtpcidev);
 		return 0;
 	}
@@ -1080,9 +1103,9 @@ static int virtpci_device_serverdown(struct device *parentbus,
 	}
 
 	/* find the vhba or vnic in virtpci device list */
-	write_lock_irqsave(&VpcidevListLock, flags);
+	write_lock_irqsave(&vpcidev_list_lock, flags);
 
-	for (tmpvpcidev = VpcidevListHead, prevvpcidev = NULL;
+	for (tmpvpcidev = vpcidev_list_head, prevvpcidev = NULL;
 	     (tmpvpcidev && !found);
 	     prevvpcidev = tmpvpcidev, tmpvpcidev = tmpvpcidev->next) {
 		if (tmpvpcidev->devtype != devtype)
@@ -1110,7 +1133,7 @@ static int virtpci_device_serverdown(struct device *parentbus,
 		vpcidriver = tmpvpcidev->mydriver;
 		rc = vpcidriver->suspend(tmpvpcidev, 0);
 	}
-	write_unlock_irqrestore(&VpcidevListLock, flags);
+	write_unlock_irqrestore(&vpcidev_list_lock, flags);
 
 	if (!found) {
 		LOGERR("**** FAILED to find vhba/vnic in the list\n");
@@ -1139,9 +1162,9 @@ static int virtpci_device_serverup(struct device *parentbus,
 	}
 
 	/* find the vhba or vnic in virtpci device list */
-	write_lock_irqsave(&VpcidevListLock, flags);
+	write_lock_irqsave(&vpcidev_list_lock, flags);
 
-	for (tmpvpcidev = VpcidevListHead, prevvpcidev = NULL;
+	for (tmpvpcidev = vpcidev_list_head, prevvpcidev = NULL;
 	     (tmpvpcidev && !found);
 	     prevvpcidev = tmpvpcidev, tmpvpcidev = tmpvpcidev->next) {
 		if (tmpvpcidev->devtype != devtype)
@@ -1172,12 +1195,13 @@ static int virtpci_device_serverup(struct device *parentbus,
 		* ever have a bus that contains NO devices, since we
 		* would never even get here in that case.
 		*/
-		fix_vbus_devInfo(&tmpvpcidev->generic_dev, tmpvpcidev->deviceNo,
-				 tmpvpcidev->device, vpcidriver);
+		fix_vbus_dev_info(&tmpvpcidev->generic_dev,
+				  tmpvpcidev->device_no,
+				  tmpvpcidev->device, vpcidriver);
 		rc = vpcidriver->resume(tmpvpcidev);
 	}
 
-	write_unlock_irqrestore(&VpcidevListLock, flags);
+	write_unlock_irqrestore(&vpcidev_list_lock, flags);
 
 	if (!found) {
 		LOGERR("**** FAILED to find vhba/vnic in the list\n");
@@ -1218,8 +1242,8 @@ static int virtpci_device_del(struct device *parentbus,
 	* device_unregister after we release the lock; otherwise we
 	* encounter "schedule while atomic"
 	*/
-	write_lock_irqsave(&VpcidevListLock, flags);
-	for (tmpvpcidev = VpcidevListHead, prevvpcidev = NULL; tmpvpcidev;) {
+	write_lock_irqsave(&vpcidev_list_lock, flags);
+	for (tmpvpcidev = vpcidev_list_head, prevvpcidev = NULL; tmpvpcidev;) {
 		if (tmpvpcidev->devtype != devtype)
 			DEL_CONTINUE;
 
@@ -1253,7 +1277,7 @@ static int virtpci_device_del(struct device *parentbus,
 			/* not at head */
 			prevvpcidev->next = tmpvpcidev->next;
 		else
-			VpcidevListHead = tmpvpcidev->next;
+			vpcidev_list_head = tmpvpcidev->next;
 
 		/* add it to our deletelist */
 		tmpvpcidev->next = dellist;
@@ -1268,9 +1292,9 @@ static int virtpci_device_del(struct device *parentbus,
 		if (prevvpcidev)
 			tmpvpcidev = prevvpcidev->next;
 		else
-			tmpvpcidev = VpcidevListHead;
+			tmpvpcidev = vpcidev_list_head;
 	}
-	write_unlock_irqrestore(&VpcidevListLock, flags);
+	write_unlock_irqrestore(&vpcidev_list_lock, flags);
 
 	if (!all && (count == 0)) {
 		LOGERR("**** FAILED to find vhba/vnic in the list\n");
@@ -1425,7 +1449,7 @@ static int print_vbus(struct device *vbus, void *data)
 }
 
 static ssize_t info_debugfs_read(struct file *file, char __user *buf,
-			      size_t len, loff_t *offset)
+				 size_t len, loff_t *offset)
 {
 	ssize_t bytes_read = 0;
 	int str_pos = 0;
@@ -1446,18 +1470,19 @@ static ssize_t info_debugfs_read(struct file *file, char __user *buf,
 	printparam.buf = vbuf;
 	printparam.len = &len;
 	if (bus_for_each_dev(&virtpci_bus_type, NULL,
-			     (void *) &printparam, print_vbus))
+			     (void *)&printparam, print_vbus))
 		LOGERR("Failed to find bus\n");
 
 	str_pos += scnprintf(vbuf + str_pos, len - str_pos,
 			"\n Virtual PCI devices\n");
-	read_lock_irqsave(&VpcidevListLock, flags);
-	tmpvpcidev = VpcidevListHead;
+	read_lock_irqsave(&vpcidev_list_lock, flags);
+	tmpvpcidev = vpcidev_list_head;
 	while (tmpvpcidev) {
 		if (tmpvpcidev->devtype == VIRTHBA_TYPE) {
 			str_pos += scnprintf(vbuf + str_pos, len - str_pos,
 					"[%d:%d] VHba:%08x:%08x max-config:%d-%d-%d-%d",
-					tmpvpcidev->busNo, tmpvpcidev->deviceNo,
+					tmpvpcidev->bus_no,
+					tmpvpcidev->device_no,
 					tmpvpcidev->scsi.wwnn.wwnn1,
 					tmpvpcidev->scsi.wwnn.wwnn2,
 					tmpvpcidev->scsi.max.max_channel,
@@ -1467,7 +1492,8 @@ static ssize_t info_debugfs_read(struct file *file, char __user *buf,
 		} else {
 			str_pos += scnprintf(vbuf + str_pos, len - str_pos,
 					"[%d:%d] VNic:%02x:%02x:%02x:%02x:%02x:%02x num_rcv_bufs:%d mtu:%d",
-					tmpvpcidev->busNo, tmpvpcidev->deviceNo,
+					tmpvpcidev->bus_no,
+					tmpvpcidev->device_no,
 					tmpvpcidev->net.mac_addr[0],
 					tmpvpcidev->net.mac_addr[1],
 					tmpvpcidev->net.mac_addr[2],
@@ -1482,7 +1508,7 @@ static ssize_t info_debugfs_read(struct file *file, char __user *buf,
 				tmpvpcidev->queueinfo.chan);
 				tmpvpcidev = tmpvpcidev->next;
 	}
-	read_unlock_irqrestore(&VpcidevListLock, flags);
+	read_unlock_irqrestore(&vpcidev_list_lock, flags);
 
 	str_pos += scnprintf(vbuf + str_pos, len - str_pos, "\n");
 	bytes_read = simple_read_from_buffer(buf, len, offset, vbuf, str_pos);
@@ -1498,7 +1524,6 @@ static int __init virtpci_mod_init(void)
 {
 	int ret;
 
-
 	if (!unisys_spar_platform)
 		return -ENODEV;
 
@@ -1515,8 +1540,8 @@ static int __init virtpci_mod_init(void)
 		return ret;
 	}
 	DBGINF("bus_register successful\n");
-	bus_device_info_init(&Bus_DriverInfo, "clientbus", "virtpci",
-			   VERSION, NULL);
+	bus_device_info_init(&bus_driver_info, "clientbus", "virtpci",
+			     VERSION, NULL);
 
 	/* create a root bus used to parent all the virtpci buses. */
 	ret = device_register(&virtpci_rootbus_device);
@@ -1529,8 +1554,8 @@ static int __init virtpci_mod_init(void)
 	}
 	DBGINF("device_register successful ret:%x\n", ret);
 
-	if (!uisctrl_register_req_handler(2, (void *) &virtpci_ctrlchan_func,
-					  &Chipset_DriverInfo)) {
+	if (!uisctrl_register_req_handler(2, (void *)&virtpci_ctrlchan_func,
+					  &chipset_driver_info)) {
 		LOGERR("uisctrl_register_req_handler ****FAILED.\n");
 		POSTCODE_LINUX_2(VPCI_CREATE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
 		device_unregister(&virtpci_rootbus_device);
@@ -1539,11 +1564,11 @@ static int __init virtpci_mod_init(void)
 	}
 
 	LOGINF("successfully registered virtpci_ctrlchan_func (0x%p) as callback.\n",
-	     (void *) &virtpci_ctrlchan_func);
+	       (void *)&virtpci_ctrlchan_func);
 	/* create debugfs directory and info file inside. */
 	virtpci_debugfs_dir = debugfs_create_dir("virtpci", NULL);
 	debugfs_create_file("info", S_IRUSR, virtpci_debugfs_dir,
-			NULL, &debugfs_info_fops);
+			    NULL, &debugfs_info_fops);
 	LOGINF("Leaving\n");
 	POSTCODE_LINUX_2(VPCI_CREATE_EXIT_PC, POSTCODE_SEVERITY_INFO);
 	return 0;
@@ -1561,7 +1586,6 @@ static void __exit virtpci_mod_exit(void)
 	bus_unregister(&virtpci_bus_type);
 	debugfs_remove_recursive(virtpci_debugfs_dir);
 	LOGINF("Leaving\n");
-
 }
 
 module_init(virtpci_mod_init);
diff --git a/drivers/staging/unisys/virtpci/virtpci.h b/drivers/staging/unisys/virtpci/virtpci.h
index 6e26956c79f4..9d85f55e8169 100644
--- a/drivers/staging/unisys/virtpci/virtpci.h
+++ b/drivers/staging/unisys/virtpci/virtpci.h
@@ -42,25 +42,25 @@ struct net_adap_info {
 	u8 mac_addr[MAX_MACADDR_LEN];
 	int num_rcv_bufs;
 	unsigned mtu;
-	uuid_le zoneGuid;
+	uuid_le zone_uuid;
 };
 
-typedef enum {
+enum virtpci_dev_type {
 	VIRTHBA_TYPE = 0,
 	VIRTNIC_TYPE = 1,
 	VIRTBUS_TYPE = 6,
-} VIRTPCI_DEV_TYPE;
+};
 
 struct virtpci_dev {
-	VIRTPCI_DEV_TYPE devtype;	/* indicates type of the
+	enum virtpci_dev_type devtype;	/* indicates type of the
 					 * virtual pci device */
 	struct virtpci_driver *mydriver;	/* which driver has allocated
 						 * this device */
 	unsigned short vendor;	/* vendor id for device */
 	unsigned short device;	/* device id for device */
-	u32 busNo;		/* number of bus on which device exists */
-	u32 deviceNo;		/* device's number on the bus */
-	struct InterruptInfo intr;	/* interrupt info */
+	u32 bus_no;		/* number of bus on which device exists */
+	u32 device_no;		/* device's number on the bus */
+	struct irq_info intr;	/* interrupt info */
 	struct device generic_dev;	/* generic device */
 	union {
 		struct scsi_adap_info scsi;
@@ -80,15 +80,15 @@ struct virtpci_driver {
 	const struct pci_device_id *id_table;	/* must be non-NULL for probe
 						 * to be called */
 	int (*probe)(struct virtpci_dev *dev,
-		      const struct pci_device_id *id); /* device inserted */
+		     const struct pci_device_id *id); /* device inserted */
 	void (*remove)(struct virtpci_dev *dev); /* Device removed (NULL if
 						    * not a hot-plug capable
 						    * driver) */
 	int (*suspend)(struct virtpci_dev *dev,
-			u32 state);		   /* Device suspended */
+		       u32 state);		   /* Device suspended */
 	int (*resume)(struct virtpci_dev *dev);	/* Device woken up */
 	int (*enable_wake)(struct virtpci_dev *dev,
-			    u32 state, int enable);	/* Enable wake event */
+			   u32 state, int enable);	/* Enable wake event */
 	struct device_driver core_driver;	/* VIRTPCI core fills this in */
 };
 
diff --git a/drivers/staging/unisys/visorchannel/globals.h b/drivers/staging/unisys/visorchannel/globals.h
index 07653b8dea7b..581ed83fe6d0 100644
--- a/drivers/staging/unisys/visorchannel/globals.h
+++ b/drivers/staging/unisys/visorchannel/globals.h
@@ -25,5 +25,4 @@
 
 #define MYDRVNAME "visorchannel"
 
-
 #endif
diff --git a/drivers/staging/unisys/visorchannel/visorchannel.h b/drivers/staging/unisys/visorchannel/visorchannel.h
index 9a4d7b6755d1..5061edff959a 100644
--- a/drivers/staging/unisys/visorchannel/visorchannel.h
+++ b/drivers/staging/unisys/visorchannel/visorchannel.h
@@ -66,7 +66,7 @@ char *visorchannel_id(VISORCHANNEL *channel, char *s);
 char *visorchannel_zoneid(VISORCHANNEL *channel, char *s);
 u64 visorchannel_get_clientpartition(VISORCHANNEL *channel);
 uuid_le visorchannel_get_uuid(VISORCHANNEL *channel);
-MEMREGION *visorchannel_get_memregion(VISORCHANNEL *channel);
+struct memregion *visorchannel_get_memregion(VISORCHANNEL *channel);
 char *visorchannel_uuid_id(uuid_le *guid, char *s);
 void visorchannel_debug(VISORCHANNEL *channel, int nQueues,
 			struct seq_file *seq, u32 off);
diff --git a/drivers/staging/unisys/visorchannel/visorchannel_funcs.c b/drivers/staging/unisys/visorchannel/visorchannel_funcs.c
index 01a44c553500..36559d5fa673 100644
--- a/drivers/staging/unisys/visorchannel/visorchannel_funcs.c
+++ b/drivers/staging/unisys/visorchannel/visorchannel_funcs.c
@@ -29,8 +29,8 @@
 #define MYDRVNAME "visorchannel"
 
 struct VISORCHANNEL_Tag {
-	MEMREGION *memregion;	/* from visor_memregion_create() */
-	CHANNEL_HEADER chan_hdr;
+	struct memregion *memregion;	/* from visor_memregion_create() */
+	struct channel_header chan_hdr;
 	uuid_le guid;
 	ulong size;
 	BOOL needs_lock;
@@ -38,10 +38,10 @@ struct VISORCHANNEL_Tag {
 	spinlock_t remove_lock;
 
 	struct {
-		SIGNAL_QUEUE_HEADER req_queue;
-		SIGNAL_QUEUE_HEADER rsp_queue;
-		SIGNAL_QUEUE_HEADER event_queue;
-		SIGNAL_QUEUE_HEADER ack_queue;
+		struct signal_queue_header req_queue;
+		struct signal_queue_header rsp_queue;
+		struct signal_queue_header event_queue;
+		struct signal_queue_header ack_queue;
 	} safe_uis_queue;
 };
 
@@ -60,7 +60,7 @@ visorchannel_create_guts(HOSTADDRESS physaddr, ulong channelBytes,
 	if (p == NULL) {
 		ERRDRV("allocation failed: (status=0)\n");
 		rc = NULL;
-		goto Away;
+		goto cleanup;
 	}
 	p->memregion = NULL;
 	p->needs_lock = needs_lock;
@@ -70,39 +70,39 @@ visorchannel_create_guts(HOSTADDRESS physaddr, ulong channelBytes,
 	/* prepare chan_hdr (abstraction to read/write channel memory) */
 	if (parent == NULL)
 		p->memregion =
-		    visor_memregion_create(physaddr, sizeof(CHANNEL_HEADER));
+		    visor_memregion_create(physaddr,
+					   sizeof(struct channel_header));
 	else
 		p->memregion =
 		    visor_memregion_create_overlapped(parent->memregion,
-						      off,
-						      sizeof(CHANNEL_HEADER));
+				off, sizeof(struct channel_header));
 	if (p->memregion == NULL) {
 		ERRDRV("visor_memregion_create failed failed: (status=0)\n");
 		rc = NULL;
-		goto Away;
+		goto cleanup;
 	}
 	if (visor_memregion_read(p->memregion, 0, &p->chan_hdr,
-				 sizeof(CHANNEL_HEADER)) < 0) {
+				 sizeof(struct channel_header)) < 0) {
 		ERRDRV("visor_memregion_read failed: (status=0)\n");
 		rc = NULL;
-		goto Away;
+		goto cleanup;
 	}
 	if (channelBytes == 0)
 		/* we had better be a CLIENT of this channel */
-		channelBytes = (ulong) p->chan_hdr.Size;
+		channelBytes = (ulong)p->chan_hdr.size;
 	if (uuid_le_cmp(guid, NULL_UUID_LE) == 0)
 		/* we had better be a CLIENT of this channel */
-		guid = p->chan_hdr.Type;
+		guid = p->chan_hdr.chtype;
 	if (visor_memregion_resize(p->memregion, channelBytes) < 0) {
 		ERRDRV("visor_memregion_resize failed: (status=0)\n");
 		rc = NULL;
-		goto Away;
+		goto cleanup;
 	}
 	p->size = channelBytes;
 	p->guid = guid;
 
 	rc = p;
-Away:
+cleanup:
 
 	if (rc == NULL) {
 		if (p != NULL) {
@@ -194,14 +194,14 @@ EXPORT_SYMBOL_GPL(visorchannel_id);
 char *
 visorchannel_zoneid(VISORCHANNEL *channel, char *s)
 {
-	return visorchannel_uuid_id(&channel->chan_hdr.ZoneGuid, s);
+	return visorchannel_uuid_id(&channel->chan_hdr.zone_uuid, s);
 }
 EXPORT_SYMBOL_GPL(visorchannel_zoneid);
 
 HOSTADDRESS
 visorchannel_get_clientpartition(VISORCHANNEL *channel)
 {
-	return channel->chan_hdr.PartitionHandle;
+	return channel->chan_hdr.partition_handle;
 }
 EXPORT_SYMBOL_GPL(visorchannel_get_clientpartition);
 
@@ -212,7 +212,7 @@ visorchannel_get_uuid(VISORCHANNEL *channel)
 }
 EXPORT_SYMBOL_GPL(visorchannel_get_uuid);
 
-MEMREGION *
+struct memregion *
 visorchannel_get_memregion(VISORCHANNEL *channel)
 {
 	return channel->memregion;
@@ -225,8 +225,11 @@ visorchannel_read(VISORCHANNEL *channel, ulong offset,
 {
 	int rc = visor_memregion_read(channel->memregion, offset,
 				      local, nbytes);
-	if ((rc >= 0) && (offset == 0) && (nbytes >= sizeof(CHANNEL_HEADER)))
-		memcpy(&channel->chan_hdr, local, sizeof(CHANNEL_HEADER));
+	if ((rc >= 0) && (offset == 0) &&
+	    (nbytes >= sizeof(struct channel_header))) {
+		memcpy(&channel->chan_hdr, local,
+		       sizeof(struct channel_header));
+	}
 	return rc;
 }
 EXPORT_SYMBOL_GPL(visorchannel_read);
@@ -235,8 +238,9 @@ int
 visorchannel_write(VISORCHANNEL *channel, ulong offset,
 		   void *local, ulong nbytes)
 {
-	if (offset == 0 && nbytes >= sizeof(CHANNEL_HEADER))
-		memcpy(&channel->chan_hdr, local, sizeof(CHANNEL_HEADER));
+	if (offset == 0 && nbytes >= sizeof(struct channel_header))
+		memcpy(&channel->chan_hdr, local,
+		       sizeof(struct channel_header));
 	return visor_memregion_write(channel->memregion, offset, local, nbytes);
 }
 EXPORT_SYMBOL_GPL(visorchannel_write);
@@ -251,7 +255,7 @@ visorchannel_clear(VISORCHANNEL *channel, ulong offset, u8 ch, ulong nbytes)
 
 	if (buf == NULL) {
 		ERRDRV("%s failed memory allocation", __func__);
-		goto Away;
+		goto cleanup;
 	}
 	memset(buf, ch, bufsize);
 	while (nbytes > 0) {
@@ -264,14 +268,14 @@ visorchannel_clear(VISORCHANNEL *channel, ulong offset, u8 ch, ulong nbytes)
 					  buf, thisbytes);
 		if (x < 0) {
 			rc = x;
-			goto Away;
+			goto cleanup;
 		}
 		written += thisbytes;
 		nbytes -= thisbytes;
 	}
 	rc = 0;
 
-Away:
+cleanup:
 	if (buf != NULL) {
 		vfree(buf);
 		buf = NULL;
@@ -283,7 +287,7 @@ EXPORT_SYMBOL_GPL(visorchannel_clear);
 void __iomem  *
 visorchannel_get_header(VISORCHANNEL *channel)
 {
-	return (void __iomem *) &(channel->chan_hdr);
+	return (void __iomem *)&channel->chan_hdr;
 }
 EXPORT_SYMBOL_GPL(visorchannel_get_header);
 
@@ -291,14 +295,15 @@ EXPORT_SYMBOL_GPL(visorchannel_get_header);
  *  channel header
  */
 #define SIG_QUEUE_OFFSET(chan_hdr, q) \
-	((chan_hdr)->oChannelSpace + ((q) * sizeof(SIGNAL_QUEUE_HEADER)))
+	((chan_hdr)->ch_space_offset + \
+	 ((q) * sizeof(struct signal_queue_header)))
 
 /** Return offset of a specific queue entry (data) from the beginning of a
  *  channel header
  */
 #define SIG_DATA_OFFSET(chan_hdr, q, sig_hdr, slot) \
-	(SIG_QUEUE_OFFSET(chan_hdr, q) + (sig_hdr)->oSignalBase + \
-	    ((slot) * (sig_hdr)->SignalSize))
+	(SIG_QUEUE_OFFSET(chan_hdr, q) + (sig_hdr)->sig_base_offset + \
+	    ((slot) * (sig_hdr)->signal_size))
 
 /** Write the contents of a specific field within a SIGNAL_QUEUE_HEADER back
  *  into host memory
@@ -306,39 +311,42 @@ EXPORT_SYMBOL_GPL(visorchannel_get_header);
 #define SIG_WRITE_FIELD(channel, queue, sig_hdr, FIELD)			\
 	(visor_memregion_write(channel->memregion,			\
 			       SIG_QUEUE_OFFSET(&channel->chan_hdr, queue)+ \
-			       offsetof(SIGNAL_QUEUE_HEADER, FIELD),	\
+			       offsetof(struct signal_queue_header, FIELD),\
 			       &((sig_hdr)->FIELD),			\
 			       sizeof((sig_hdr)->FIELD)) >= 0)
 
 static BOOL
 sig_read_header(VISORCHANNEL *channel, u32 queue,
-		SIGNAL_QUEUE_HEADER *sig_hdr)
+		struct signal_queue_header *sig_hdr)
 {
 	BOOL rc = FALSE;
 
-	if (channel->chan_hdr.oChannelSpace < sizeof(CHANNEL_HEADER)) {
+	if (channel->chan_hdr.ch_space_offset < sizeof(struct channel_header)) {
 		ERRDRV("oChannelSpace too small: (status=%d)\n", rc);
-		goto Away;
+		goto cleanup;
 	}
 
 	/* Read the appropriate SIGNAL_QUEUE_HEADER into local memory. */
 
 	if (visor_memregion_read(channel->memregion,
 				 SIG_QUEUE_OFFSET(&channel->chan_hdr, queue),
-				 sig_hdr, sizeof(SIGNAL_QUEUE_HEADER)) < 0) {
+				 sig_hdr,
+				 sizeof(struct signal_queue_header)) < 0) {
 		ERRDRV("queue=%d SIG_QUEUE_OFFSET=%d",
 		       queue, (int)SIG_QUEUE_OFFSET(&channel->chan_hdr, queue));
-		ERRDRV("visor_memregion_read of signal queue failed: (status=%d)\n", rc);
-		goto Away;
+		ERRDRV("visor_memregion_read of signal queue failed: (status=%d)\n",
+		       rc);
+		goto cleanup;
 	}
 	rc = TRUE;
-Away:
+cleanup:
 	return rc;
 }
 
 static BOOL
 sig_do_data(VISORCHANNEL *channel, u32 queue,
-	    SIGNAL_QUEUE_HEADER *sig_hdr, u32 slot, void *data, BOOL is_write)
+	    struct signal_queue_header *sig_hdr, u32 slot, void *data,
+	    BOOL is_write)
 {
 	BOOL rc = FALSE;
 	int signal_data_offset = SIG_DATA_OFFSET(&channel->chan_hdr, queue,
@@ -346,53 +354,55 @@ sig_do_data(VISORCHANNEL *channel, u32 queue,
 	if (is_write) {
 		if (visor_memregion_write(channel->memregion,
 					  signal_data_offset,
-					  data, sig_hdr->SignalSize) < 0) {
-			ERRDRV("visor_memregion_write of signal data failed: (status=%d)\n", rc);
-			goto Away;
+					  data, sig_hdr->signal_size) < 0) {
+			ERRDRV("visor_memregion_write of signal data failed: (status=%d)\n",
+			       rc);
+			goto cleanup;
 		}
 	} else {
 		if (visor_memregion_read(channel->memregion, signal_data_offset,
-					 data, sig_hdr->SignalSize) < 0) {
-			ERRDRV("visor_memregion_read of signal data failed: (status=%d)\n", rc);
-			goto Away;
+					 data, sig_hdr->signal_size) < 0) {
+			ERRDRV("visor_memregion_read of signal data failed: (status=%d)\n",
+			       rc);
+			goto cleanup;
 		}
 	}
 	rc = TRUE;
-Away:
+cleanup:
 	return rc;
 }
 
 static inline BOOL
 sig_read_data(VISORCHANNEL *channel, u32 queue,
-	      SIGNAL_QUEUE_HEADER *sig_hdr, u32 slot, void *data)
+	      struct signal_queue_header *sig_hdr, u32 slot, void *data)
 {
 	return sig_do_data(channel, queue, sig_hdr, slot, data, FALSE);
 }
 
 static inline BOOL
 sig_write_data(VISORCHANNEL *channel, u32 queue,
-	       SIGNAL_QUEUE_HEADER *sig_hdr, u32 slot, void *data)
+	       struct signal_queue_header *sig_hdr, u32 slot, void *data)
 {
 	return sig_do_data(channel, queue, sig_hdr, slot, data, TRUE);
 }
 
 static inline unsigned char
-safe_sig_queue_validate(pSIGNAL_QUEUE_HEADER psafe_sqh,
-			pSIGNAL_QUEUE_HEADER punsafe_sqh,
+safe_sig_queue_validate(struct signal_queue_header *psafe_sqh,
+			struct signal_queue_header *punsafe_sqh,
 			u32 *phead, u32 *ptail)
 {
-	if ((*phead >= psafe_sqh->MaxSignalSlots)
-	    || (*ptail >= psafe_sqh->MaxSignalSlots)) {
+	if ((*phead >= psafe_sqh->max_slots) ||
+	    (*ptail >= psafe_sqh->max_slots)) {
 		/* Choose 0 or max, maybe based on current tail value */
 		*phead = 0;
 		*ptail = 0;
 
 		/* Sync with client as necessary */
-		punsafe_sqh->Head = *phead;
-		punsafe_sqh->Tail = *ptail;
+		punsafe_sqh->head = *phead;
+		punsafe_sqh->tail = *ptail;
 
 		ERRDRV("safe_sig_queue_validate: head = 0x%x, tail = 0x%x, MaxSlots = 0x%x",
-		     *phead, *ptail, psafe_sqh->MaxSignalSlots);
+		       *phead, *ptail, psafe_sqh->max_slots);
 		return 0;
 	}
 	return 1;
@@ -402,41 +412,42 @@ BOOL
 visorchannel_signalremove(VISORCHANNEL *channel, u32 queue, void *msg)
 {
 	BOOL rc = FALSE;
-	SIGNAL_QUEUE_HEADER sig_hdr;
+	struct signal_queue_header sig_hdr;
 
 	if (channel->needs_lock)
 		spin_lock(&channel->remove_lock);
 
 	if (!sig_read_header(channel, queue, &sig_hdr)) {
 		rc = FALSE;
-		goto Away;
+		goto cleanup;
 	}
-	if (sig_hdr.Head == sig_hdr.Tail) {
+	if (sig_hdr.head == sig_hdr.tail) {
 		rc = FALSE;	/* no signals to remove */
-		goto Away;
+		goto cleanup;
 	}
-	sig_hdr.Tail = (sig_hdr.Tail + 1) % sig_hdr.MaxSignalSlots;
-	if (!sig_read_data(channel, queue, &sig_hdr, sig_hdr.Tail, msg)) {
+	sig_hdr.tail = (sig_hdr.tail + 1) % sig_hdr.max_slots;
+	if (!sig_read_data(channel, queue, &sig_hdr, sig_hdr.tail, msg)) {
 		ERRDRV("sig_read_data failed: (status=%d)\n", rc);
-		goto Away;
+		goto cleanup;
 	}
-	sig_hdr.NumSignalsReceived++;
+	sig_hdr.num_received++;
 
 	/* For each data field in SIGNAL_QUEUE_HEADER that was modified,
 	 * update host memory.
 	 */
 	mb(); /* required for channel synch */
-	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, Tail)) {
+	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, tail)) {
 		ERRDRV("visor_memregion_write of Tail failed: (status=%d)\n",
 		       rc);
-		goto Away;
+		goto cleanup;
 	}
-	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, NumSignalsReceived)) {
-		ERRDRV("visor_memregion_write of NumSignalsReceived failed: (status=%d)\n", rc);
-		goto Away;
+	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, num_received)) {
+		ERRDRV("visor_memregion_write of NumSignalsReceived failed: (status=%d)\n",
+		       rc);
+		goto cleanup;
 	}
 	rc = TRUE;
-Away:
+cleanup:
 	if (channel->needs_lock)
 		spin_unlock(&channel->remove_lock);
 
@@ -448,48 +459,50 @@ BOOL
 visorchannel_signalinsert(VISORCHANNEL *channel, u32 queue, void *msg)
 {
 	BOOL rc = FALSE;
-	SIGNAL_QUEUE_HEADER sig_hdr;
+	struct signal_queue_header sig_hdr;
 
 	if (channel->needs_lock)
 		spin_lock(&channel->insert_lock);
 
 	if (!sig_read_header(channel, queue, &sig_hdr)) {
 		rc = FALSE;
-		goto Away;
+		goto cleanup;
 	}
 
-	sig_hdr.Head = ((sig_hdr.Head + 1) % sig_hdr.MaxSignalSlots);
-	if (sig_hdr.Head == sig_hdr.Tail) {
-		sig_hdr.NumOverflows++;
-		if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, NumOverflows)) {
-			ERRDRV("visor_memregion_write of NumOverflows failed: (status=%d)\n", rc);
-			goto Away;
+	sig_hdr.head = ((sig_hdr.head + 1) % sig_hdr.max_slots);
+	if (sig_hdr.head == sig_hdr.tail) {
+		sig_hdr.num_overflows++;
+		if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, num_overflows)) {
+			ERRDRV("visor_memregion_write of NumOverflows failed: (status=%d)\n",
+			       rc);
+			goto cleanup;
 		}
 		rc = FALSE;
-		goto Away;
+		goto cleanup;
 	}
 
-	if (!sig_write_data(channel, queue, &sig_hdr, sig_hdr.Head, msg)) {
+	if (!sig_write_data(channel, queue, &sig_hdr, sig_hdr.head, msg)) {
 		ERRDRV("sig_write_data failed: (status=%d)\n", rc);
-		goto Away;
+		goto cleanup;
 	}
-	sig_hdr.NumSignalsSent++;
+	sig_hdr.num_sent++;
 
 	/* For each data field in SIGNAL_QUEUE_HEADER that was modified,
 	 * update host memory.
 	 */
 	mb(); /* required for channel synch */
-	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, Head)) {
+	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, head)) {
 		ERRDRV("visor_memregion_write of Head failed: (status=%d)\n",
 		       rc);
-		goto Away;
+		goto cleanup;
 	}
-	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, NumSignalsSent)) {
-		ERRDRV("visor_memregion_write of NumSignalsSent failed: (status=%d)\n", rc);
-		goto Away;
+	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, num_sent)) {
+		ERRDRV("visor_memregion_write of NumSignalsSent failed: (status=%d)\n",
+		       rc);
+		goto cleanup;
 	}
 	rc = TRUE;
-Away:
+cleanup:
 	if (channel->needs_lock)
 		spin_unlock(&channel->insert_lock);
 
@@ -497,59 +510,58 @@ Away:
 }
 EXPORT_SYMBOL_GPL(visorchannel_signalinsert);
 
-
 int
 visorchannel_signalqueue_slots_avail(VISORCHANNEL *channel, u32 queue)
 {
-	SIGNAL_QUEUE_HEADER sig_hdr;
+	struct signal_queue_header sig_hdr;
 	u32 slots_avail, slots_used;
 	u32 head, tail;
 
 	if (!sig_read_header(channel, queue, &sig_hdr))
 		return 0;
-	head = sig_hdr.Head;
-	tail = sig_hdr.Tail;
+	head = sig_hdr.head;
+	tail = sig_hdr.tail;
 	if (head < tail)
-		head = head + sig_hdr.MaxSignalSlots;
+		head = head + sig_hdr.max_slots;
 	slots_used = (head - tail);
-	slots_avail = sig_hdr.MaxSignals - slots_used;
-	return (int) slots_avail;
+	slots_avail = sig_hdr.max_signals - slots_used;
+	return (int)slots_avail;
 }
 EXPORT_SYMBOL_GPL(visorchannel_signalqueue_slots_avail);
 
 int
 visorchannel_signalqueue_max_slots(VISORCHANNEL *channel, u32 queue)
 {
-	SIGNAL_QUEUE_HEADER sig_hdr;
+	struct signal_queue_header sig_hdr;
 
 	if (!sig_read_header(channel, queue, &sig_hdr))
 		return 0;
-	return (int) sig_hdr.MaxSignals;
+	return (int)sig_hdr.max_signals;
 }
 EXPORT_SYMBOL_GPL(visorchannel_signalqueue_max_slots);
 
 static void
-sigqueue_debug(SIGNAL_QUEUE_HEADER *q, int which, struct seq_file *seq)
+sigqueue_debug(struct signal_queue_header *q, int which, struct seq_file *seq)
 {
 	seq_printf(seq, "Signal Queue #%d\n", which);
-	seq_printf(seq, "   VersionId          = %lu\n", (ulong) q->VersionId);
-	seq_printf(seq, "   Type               = %lu\n", (ulong) q->Type);
+	seq_printf(seq, "   VersionId          = %lu\n", (ulong)q->version);
+	seq_printf(seq, "   Type               = %lu\n", (ulong)q->chtype);
 	seq_printf(seq, "   oSignalBase        = %llu\n",
-		   (long long) q->oSignalBase);
-	seq_printf(seq, "   SignalSize         = %lu\n", (ulong) q->SignalSize);
+		   (long long)q->sig_base_offset);
+	seq_printf(seq, "   SignalSize         = %lu\n", (ulong)q->signal_size);
 	seq_printf(seq, "   MaxSignalSlots     = %lu\n",
-		   (ulong) q->MaxSignalSlots);
-	seq_printf(seq, "   MaxSignals         = %lu\n", (ulong) q->MaxSignals);
+		   (ulong)q->max_slots);
+	seq_printf(seq, "   MaxSignals         = %lu\n", (ulong)q->max_signals);
 	seq_printf(seq, "   FeatureFlags       = %-16.16Lx\n",
-		   (long long) q->FeatureFlags);
+		   (long long)q->features);
 	seq_printf(seq, "   NumSignalsSent     = %llu\n",
-		   (long long) q->NumSignalsSent);
+		   (long long)q->num_sent);
 	seq_printf(seq, "   NumSignalsReceived = %llu\n",
-		   (long long) q->NumSignalsReceived);
+		   (long long)q->num_received);
 	seq_printf(seq, "   NumOverflows       = %llu\n",
-		   (long long) q->NumOverflows);
-	seq_printf(seq, "   Head               = %lu\n", (ulong) q->Head);
-	seq_printf(seq, "   Tail               = %lu\n", (ulong) q->Tail);
+		   (long long)q->num_overflows);
+	seq_printf(seq, "   Head               = %lu\n", (ulong)q->head);
+	seq_printf(seq, "   Tail               = %lu\n", (ulong)q->tail);
 }
 
 void
@@ -558,9 +570,9 @@ visorchannel_debug(VISORCHANNEL *channel, int nQueues,
 {
 	HOSTADDRESS addr = 0;
 	ulong nbytes = 0, nbytes_region = 0;
-	MEMREGION *memregion = NULL;
-	CHANNEL_HEADER hdr;
-	CHANNEL_HEADER *phdr = &hdr;
+	struct memregion *memregion = NULL;
+	struct channel_header hdr;
+	struct channel_header *phdr = &hdr;
 	int i = 0;
 	int errcode = 0;
 
@@ -576,7 +588,7 @@ visorchannel_debug(VISORCHANNEL *channel, int nQueues,
 	addr = visor_memregion_get_physaddr(memregion);
 	nbytes_region = visor_memregion_get_nbytes(memregion);
 	errcode = visorchannel_read(channel, off,
-				    phdr, sizeof(CHANNEL_HEADER));
+				    phdr, sizeof(struct channel_header));
 	if (errcode < 0) {
 		seq_printf(seq,
 			   "Read of channel header failed with errcode=%d)\n",
@@ -584,39 +596,41 @@ visorchannel_debug(VISORCHANNEL *channel, int nQueues,
 		if (off == 0) {
 			phdr = &channel->chan_hdr;
 			seq_puts(seq, "(following data may be stale)\n");
-		} else
+		} else {
 			return;
+		}
 	}
-	nbytes = (ulong) (phdr->Size);
+	nbytes = (ulong)(phdr->size);
 	seq_printf(seq, "--- Begin channel @0x%-16.16Lx for 0x%lx bytes (region=0x%lx bytes) ---\n",
 		   addr + off, nbytes, nbytes_region);
-	seq_printf(seq, "Type            = %pUL\n", &phdr->Type);
-	seq_printf(seq, "ZoneGuid        = %pUL\n", &phdr->ZoneGuid);
+	seq_printf(seq, "Type            = %pUL\n", &phdr->chtype);
+	seq_printf(seq, "ZoneGuid        = %pUL\n", &phdr->zone_uuid);
 	seq_printf(seq, "Signature       = 0x%-16.16Lx\n",
-		   (long long) phdr->Signature);
-	seq_printf(seq, "LegacyState     = %lu\n", (ulong) phdr->LegacyState);
-	seq_printf(seq, "SrvState        = %lu\n", (ulong) phdr->SrvState);
-	seq_printf(seq, "CliStateBoot    = %lu\n", (ulong) phdr->CliStateBoot);
-	seq_printf(seq, "CliStateOS      = %lu\n", (ulong) phdr->CliStateOS);
-	seq_printf(seq, "HeaderSize      = %lu\n", (ulong) phdr->HeaderSize);
-	seq_printf(seq, "Size            = %llu\n", (long long) phdr->Size);
+		   (long long)phdr->signature);
+	seq_printf(seq, "LegacyState     = %lu\n", (ulong)phdr->legacy_state);
+	seq_printf(seq, "SrvState        = %lu\n", (ulong)phdr->srv_state);
+	seq_printf(seq, "CliStateBoot    = %lu\n", (ulong)phdr->cli_state_boot);
+	seq_printf(seq, "CliStateOS      = %lu\n", (ulong)phdr->cli_state_os);
+	seq_printf(seq, "HeaderSize      = %lu\n", (ulong)phdr->header_size);
+	seq_printf(seq, "Size            = %llu\n", (long long)phdr->size);
 	seq_printf(seq, "Features        = 0x%-16.16llx\n",
-		   (long long) phdr->Features);
+		   (long long)phdr->features);
 	seq_printf(seq, "PartitionHandle = 0x%-16.16llx\n",
-		   (long long) phdr->PartitionHandle);
+		   (long long)phdr->partition_handle);
 	seq_printf(seq, "Handle          = 0x%-16.16llx\n",
-		   (long long) phdr->Handle);
-	seq_printf(seq, "VersionId       = %lu\n", (ulong) phdr->VersionId);
+		   (long long)phdr->handle);
+	seq_printf(seq, "VersionId       = %lu\n", (ulong)phdr->version_id);
 	seq_printf(seq, "oChannelSpace   = %llu\n",
-		   (long long) phdr->oChannelSpace);
-	if ((phdr->oChannelSpace == 0) || (errcode < 0))
+		   (long long)phdr->ch_space_offset);
+	if ((phdr->ch_space_offset == 0) || (errcode < 0))
 		;
 	else
 		for (i = 0; i < nQueues; i++) {
-			SIGNAL_QUEUE_HEADER q;
+			struct signal_queue_header q;
 
 			errcode = visorchannel_read(channel,
-						    off + phdr->oChannelSpace +
+						    off +
+						    phdr->ch_space_offset +
 						    (i * sizeof(q)),
 						    &q, sizeof(q));
 			if (errcode < 0) {
@@ -643,15 +657,17 @@ visorchannel_dump_section(VISORCHANNEL *chan, char *s,
 
 	fmtbufsize = 100 * COVQ(len, 16);
 	buf = kmalloc(len, GFP_KERNEL|__GFP_NORETRY);
+	if (!buf)
+		return;
 	fmtbuf = kmalloc(fmtbufsize, GFP_KERNEL|__GFP_NORETRY);
-	if (buf == NULL || fmtbuf == NULL)
-		goto Away;
+	if (!fmtbuf)
+		goto fmt_failed;
 
 	errcode = visorchannel_read(chan, off, buf, len);
 	if (errcode < 0) {
 		ERRDRV("%s failed to read %s from channel errcode=%d",
 		       s, __func__, errcode);
-		goto Away;
+		goto read_failed;
 	}
 	seq_printf(seq, "channel %s:\n", s);
 	tbuf = buf;
@@ -663,14 +679,9 @@ visorchannel_dump_section(VISORCHANNEL *chan, char *s,
 		len -= 16;
 	}
 
-Away:
-	if (buf != NULL) {
-		kfree(buf);
-		buf = NULL;
-	}
-	if (fmtbuf != NULL) {
-		kfree(fmtbuf);
-		fmtbuf = NULL;
-	}
+read_failed:
+	kfree(fmtbuf);
+fmt_failed:
+	kfree(buf);
 }
 EXPORT_SYMBOL_GPL(visorchannel_dump_section);
diff --git a/drivers/staging/unisys/visorchipset/file.c b/drivers/staging/unisys/visorchipset/file.c
index 3321764069de..373fa36b7119 100644
--- a/drivers/staging/unisys/visorchipset/file.c
+++ b/drivers/staging/unisys/visorchipset/file.c
@@ -155,9 +155,9 @@ visorchipset_mmap(struct file *file, struct vm_area_struct *vma)
 			return -ENXIO;
 		}
 		visorchannel_read(*PControlVm_channel,
-				  offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL,
-					   gpControlChannel), &addr,
-				  sizeof(addr));
+			offsetof(struct spar_controlvm_channel_protocol,
+				 gp_control_channel),
+			&addr, sizeof(addr));
 		if (addr == 0) {
 			ERRDRV("%s control channel address is 0", __func__);
 			return -ENXIO;
diff --git a/drivers/staging/unisys/visorchipset/parser.c b/drivers/staging/unisys/visorchipset/parser.c
index 661aaae9b154..9edbd3bbd186 100644
--- a/drivers/staging/unisys/visorchipset/parser.c
+++ b/drivers/staging/unisys/visorchipset/parser.c
@@ -47,8 +47,8 @@ parser_init_guts(u64 addr, u32 bytes, BOOL isLocal,
 	int allocbytes = sizeof(PARSER_CONTEXT) + bytes;
 	PARSER_CONTEXT *rc = NULL;
 	PARSER_CONTEXT *ctx = NULL;
-	MEMREGION *rgn = NULL;
-	ULTRA_CONTROLVM_PARAMETERS_HEADER *phdr = NULL;
+	struct memregion *rgn = NULL;
+	struct spar_controlvm_parameters_header *phdr = NULL;
 
 	if (tryAgain)
 		*tryAgain = FALSE;
@@ -110,27 +110,29 @@ parser_init_guts(u64 addr, u32 bytes, BOOL isLocal,
 		rc = ctx;
 		goto Away;
 	}
-	phdr = (ULTRA_CONTROLVM_PARAMETERS_HEADER *) (ctx->data);
-	if (phdr->TotalLength != bytes) {
+	phdr = (struct spar_controlvm_parameters_header *)(ctx->data);
+	if (phdr->total_length != bytes) {
 		ERRDRV("%s - bad total length %lu (should be %lu)",
 		       __func__,
-		       (ulong) (phdr->TotalLength), (ulong) (bytes));
+		       (ulong) (phdr->total_length), (ulong) (bytes));
 		rc = NULL;
 		goto Away;
 	}
-	if (phdr->TotalLength < phdr->HeaderLength) {
+	if (phdr->total_length < phdr->header_length) {
 		ERRDRV("%s - total length < header length (%lu < %lu)",
 		       __func__,
-		       (ulong) (phdr->TotalLength),
-		       (ulong) (phdr->HeaderLength));
+		       (ulong) (phdr->total_length),
+		       (ulong) (phdr->header_length));
 		rc = NULL;
 		goto Away;
 	}
-	if (phdr->HeaderLength < sizeof(ULTRA_CONTROLVM_PARAMETERS_HEADER)) {
+	if (phdr->header_length <
+	    sizeof(struct spar_controlvm_parameters_header)) {
 		ERRDRV("%s - header is too small (%lu < %lu)",
 		       __func__,
-		       (ulong) (phdr->HeaderLength),
-		       (ulong) (sizeof(ULTRA_CONTROLVM_PARAMETERS_HEADER)));
+		       (ulong) (phdr->header_length),
+		       (ulong)(sizeof(
+				struct spar_controlvm_parameters_header)));
 		rc = NULL;
 		goto Away;
 	}
@@ -159,7 +161,7 @@ parser_init(u64 addr, u32 bytes, BOOL isLocal, BOOL *tryAgain)
 }
 
 /* Call this instead of parser_init() if the payload area consists of just
- * a sequence of bytes, rather than a ULTRA_CONTROLVM_PARAMETERS_HEADER
+ * a sequence of bytes, rather than a struct spar_controlvm_parameters_header
  * structures.  Afterwards, you can call parser_simpleString_get() or
  * parser_byteStream_get() to obtain the data.
  */
@@ -196,44 +198,44 @@ parser_byteStream_get(PARSER_CONTEXT *ctx, ulong *nbytes)
 uuid_le
 parser_id_get(PARSER_CONTEXT *ctx)
 {
-	ULTRA_CONTROLVM_PARAMETERS_HEADER *phdr = NULL;
+	struct spar_controlvm_parameters_header *phdr = NULL;
 
 	if (ctx == NULL) {
 		ERRDRV("%s (%s:%d) - no context",
 		       __func__, __FILE__, __LINE__);
 		return NULL_UUID_LE;
 	}
-	phdr = (ULTRA_CONTROLVM_PARAMETERS_HEADER *) (ctx->data);
-	return phdr->Id;
+	phdr = (struct spar_controlvm_parameters_header *)(ctx->data);
+	return phdr->id;
 }
 
 void
 parser_param_start(PARSER_CONTEXT *ctx, PARSER_WHICH_STRING which_string)
 {
-	ULTRA_CONTROLVM_PARAMETERS_HEADER *phdr = NULL;
+	struct spar_controlvm_parameters_header *phdr = NULL;
 
 	if (ctx == NULL) {
 		ERRDRV("%s (%s:%d) - no context",
 		       __func__, __FILE__, __LINE__);
 		goto Away;
 	}
-	phdr = (ULTRA_CONTROLVM_PARAMETERS_HEADER *) (ctx->data);
+	phdr = (struct spar_controlvm_parameters_header *)(ctx->data);
 	switch (which_string) {
 	case PARSERSTRING_INITIATOR:
-		ctx->curr = ctx->data + phdr->InitiatorOffset;
-		ctx->bytes_remaining = phdr->InitiatorLength;
+		ctx->curr = ctx->data + phdr->initiator_offset;
+		ctx->bytes_remaining = phdr->initiator_length;
 		break;
 	case PARSERSTRING_TARGET:
-		ctx->curr = ctx->data + phdr->TargetOffset;
-		ctx->bytes_remaining = phdr->TargetLength;
+		ctx->curr = ctx->data + phdr->target_offset;
+		ctx->bytes_remaining = phdr->target_length;
 		break;
 	case PARSERSTRING_CONNECTION:
-		ctx->curr = ctx->data + phdr->ConnectionOffset;
-		ctx->bytes_remaining = phdr->ConnectionLength;
+		ctx->curr = ctx->data + phdr->connection_offset;
+		ctx->bytes_remaining = phdr->connection_length;
 		break;
 	case PARSERSTRING_NAME:
-		ctx->curr = ctx->data + phdr->NameOffset;
-		ctx->bytes_remaining = phdr->NameLength;
+		ctx->curr = ctx->data + phdr->name_offset;
+		ctx->bytes_remaining = phdr->name_length;
 		break;
 	default:
 		ERRDRV("%s - bad which_string %d", __func__, which_string);
diff --git a/drivers/staging/unisys/visorchipset/testing.h b/drivers/staging/unisys/visorchipset/testing.h
index 015d502cbb16..573aa8b5ba6a 100644
--- a/drivers/staging/unisys/visorchipset/testing.h
+++ b/drivers/staging/unisys/visorchipset/testing.h
@@ -23,8 +23,9 @@
 #include "globals.h"
 #include "controlvmchannel.h"
 
-void test_produce_test_message(CONTROLVM_MESSAGE *msg, int isLocalTestAddr);
-BOOL test_consume_test_message(CONTROLVM_MESSAGE *msg);
+void test_produce_test_message(struct controlvm_message *msg,
+			       int isLocalTestAddr);
+BOOL test_consume_test_message(struct controlvm_message *msg);
 void test_manufacture_vnic_client_add(void *p);
 void test_manufacture_vnic_client_add_phys(HOSTADDRESS addr);
 void test_manufacture_preamble_messages(void);
diff --git a/drivers/staging/unisys/visorchipset/visorchipset.h b/drivers/staging/unisys/visorchipset/visorchipset.h
index 2bf2e2f368ef..46dad63fa2c8 100644
--- a/drivers/staging/unisys/visorchipset/visorchipset.h
+++ b/drivers/staging/unisys/visorchipset/visorchipset.h
@@ -31,85 +31,85 @@
 /** Describes the state from the perspective of which controlvm messages have
  *  been received for a bus or device.
  */
-typedef struct {
+struct visorchipset_state {
 	u32 created:1;
 	u32 attached:1;
 	u32 configured:1;
 	u32 running:1;
 	/* Add new fields above. */
 	/* Remaining bits in this 32-bit word are unused. */
-} VISORCHIPSET_STATE;
+};
 
-typedef enum {
+enum visorchipset_addresstype {
 	/** address is guest physical, but outside of the physical memory
 	 *  region that is controlled by the running OS (this is the normal
 	 *  address type for Supervisor channels)
 	 */
-	ADDRTYPE_localPhysical,
+	ADDRTYPE_LOCALPHYSICAL,
 
 	/** address is guest physical, and withIN the confines of the
 	 *  physical memory controlled by the running OS.
 	 */
-	ADDRTYPE_localTest,
-} VISORCHIPSET_ADDRESSTYPE;
+	ADDRTYPE_LOCALTEST,
+};
 
-typedef enum {
-	CRASH_dev,
-	CRASH_bus,
-} CRASH_OBJ_TYPE;
+enum crash_obj_type {
+	CRASH_DEV,
+	CRASH_BUS,
+};
 
 /** Attributes for a particular Supervisor channel.
  */
-typedef struct {
-	VISORCHIPSET_ADDRESSTYPE addrType;
-	HOSTADDRESS channelAddr;
-	struct InterruptInfo intr;
-	u64 nChannelBytes;
-	uuid_le channelTypeGuid;
-	uuid_le channelInstGuid;
+struct visorchipset_channel_info {
+	enum visorchipset_addresstype addr_type;
+	HOSTADDRESS channel_addr;
+	struct irq_info intr;
+	u64 n_channel_bytes;
+	uuid_le channel_type_uuid;
+	uuid_le channel_inst_uuid;
 
-} VISORCHIPSET_CHANNEL_INFO;
+};
 
 /** Attributes for a particular Supervisor device.
  *  Any visorchipset client can query these attributes using
  *  visorchipset_get_client_device_info() or
  *  visorchipset_get_server_device_info().
  */
-typedef struct {
+struct visorchipset_device_info {
 	struct list_head entry;
-	u32 busNo;
-	u32 devNo;
-	uuid_le devInstGuid;
-	VISORCHIPSET_STATE state;
-	VISORCHIPSET_CHANNEL_INFO chanInfo;
-	u32 Reserved1;		/* CONTROLVM_ID */
-	u64 Reserved2;
-	u32 switchNo;		/* when devState.attached==1 */
-	u32 internalPortNo;	/* when devState.attached==1 */
-	CONTROLVM_MESSAGE_HEADER pendingMsgHdr;	/* CONTROLVM_MESSAGE */
+	u32 bus_no;
+	u32 dev_no;
+	uuid_le dev_inst_uuid;
+	struct visorchipset_state state;
+	struct visorchipset_channel_info chan_info;
+	u32 reserved1;		/* control_vm_id */
+	u64 reserved2;
+	u32 switch_no;		/* when devState.attached==1 */
+	u32 internal_port_no;	/* when devState.attached==1 */
+	struct controlvm_message_header pending_msg_hdr;/* CONTROLVM_MESSAGE */
 	/** For private use by the bus driver */
 	void *bus_driver_context;
 
-} VISORCHIPSET_DEVICE_INFO;
+};
 
-static inline VISORCHIPSET_DEVICE_INFO *
-finddevice(struct list_head *list, u32 busNo, u32 devNo)
+static inline struct visorchipset_device_info *finddevice(
+		struct list_head *list, u32 bus_no, u32 dev_no)
 {
-	VISORCHIPSET_DEVICE_INFO *p;
+	struct visorchipset_device_info *p;
 
 	list_for_each_entry(p, list, entry) {
-		if (p->busNo == busNo && p->devNo == devNo)
+		if (p->bus_no == bus_no && p->dev_no == dev_no)
 			return p;
 	}
 	return NULL;
 }
 
-static inline void delbusdevices(struct list_head *list, u32 busNo)
+static inline void delbusdevices(struct list_head *list, u32 bus_no)
 {
-	VISORCHIPSET_DEVICE_INFO *p, *tmp;
+	struct visorchipset_device_info *p, *tmp;
 
 	list_for_each_entry_safe(p, tmp, list, entry) {
-		if (p->busNo == busNo) {
+		if (p->bus_no == bus_no) {
 			list_del(&p->entry);
 			kfree(p);
 		}
@@ -122,37 +122,37 @@ static inline void delbusdevices(struct list_head *list, u32 busNo)
  *  Any visorchipset client can query these attributes using
  *  visorchipset_get_client_bus_info() or visorchipset_get_bus_info().
  */
-typedef struct {
+struct visorchipset_bus_info {
 	struct list_head entry;
-	u32 busNo;
-	VISORCHIPSET_STATE state;
-	VISORCHIPSET_CHANNEL_INFO chanInfo;
-	uuid_le partitionGuid;
-	u64 partitionHandle;
+	u32 bus_no;
+	struct visorchipset_state state;
+	struct visorchipset_channel_info chan_info;
+	uuid_le partition_uuid;
+	u64 partition_handle;
 	u8 *name;		/* UTF8 */
 	u8 *description;	/* UTF8 */
-	u64 Reserved1;
-	u32 Reserved2;
-	MYPROCOBJECT *procObject;
+	u64 reserved1;
+	u32 reserved2;
+	MYPROCOBJECT *proc_object;
 	struct {
 		u32 server:1;
 		/* Add new fields above. */
 		/* Remaining bits in this 32-bit word are unused. */
 	} flags;
-	CONTROLVM_MESSAGE_HEADER pendingMsgHdr;	/* CONTROLVM MsgHdr */
+	struct controlvm_message_header pending_msg_hdr;/* CONTROLVM MsgHdr */
 	/** For private use by the bus driver */
 	void *bus_driver_context;
-	u64 devNo;
+	u64 dev_no;
 
-} VISORCHIPSET_BUS_INFO;
+};
 
-static inline VISORCHIPSET_BUS_INFO *
-findbus(struct list_head *list, u32 busNo)
+static inline struct visorchipset_bus_info *
+findbus(struct list_head *list, u32 bus_no)
 {
-	VISORCHIPSET_BUS_INFO *p;
+	struct visorchipset_bus_info *p;
 
 	list_for_each_entry(p, list, entry) {
-		if (p->busNo == busNo)
+		if (p->bus_no == bus_no)
 			return p;
 	}
 	return NULL;
@@ -160,75 +160,73 @@ findbus(struct list_head *list, u32 busNo)
 
 /** Attributes for a particular Supervisor switch.
  */
-typedef struct {
-	u32 switchNo;
-	VISORCHIPSET_STATE state;
-	uuid_le switchTypeGuid;
-	u8 *authService1;
-	u8 *authService2;
-	u8 *authService3;
-	u8 *securityContext;
-	u64 Reserved;
-	u32 Reserved2;		/* CONTROLVM_ID */
+struct visorchipset_switch_info {
+	u32 switch_no;
+	struct visorchipset_state state;
+	uuid_le switch_type_uuid;
+	u8 *authservice1;
+	u8 *authservice2;
+	u8 *authservice3;
+	u8 *security_context;
+	u64 reserved;
+	u32 reserved2;		/* control_vm_id */
 	struct device dev;
 	BOOL dev_exists;
-	CONTROLVM_MESSAGE_HEADER pendingMsgHdr;
-
-} VISORCHIPSET_SWITCH_INFO;
+	struct controlvm_message_header pending_msg_hdr;
+};
 
 /** Attributes for a particular Supervisor external port, which is connected
  *  to a specific switch.
  */
-typedef struct {
-	u32 switchNo;
-	u32 externalPortNo;
-	VISORCHIPSET_STATE state;
-	uuid_le networkZoneGuid;
-	int pdPort;
+struct visorchipset_externalport_info {
+	u32 switch_no;
+	u32 external_port_no;
+	struct visorchipset_state state;
+	uuid_le network_zone_uuid;
+	int pd_port;
 	u8 *ip;
-	u8 *ipNetmask;
-	u8 *ipBroadcast;
-	u8 *ipNetwork;
-	u8 *ipGateway;
-	u8 *ipDNS;
-	u64 Reserved1;
-	u32 Reserved2;		/* CONTROLVM_ID */
+	u8 *ip_netmask;
+	u8 *ip_broadcast;
+	u8 *ip_network;
+	u8 *ip_gateway;
+	u8 *ip_dns;
+	u64 reserved1;
+	u32 reserved2;		/* control_vm_id */
 	struct device dev;
 	BOOL dev_exists;
-	CONTROLVM_MESSAGE_HEADER pendingMsgHdr;
-
-} VISORCHIPSET_EXTERNALPORT_INFO;
+	struct controlvm_message_header pending_msg_hdr;
+};
 
 /** Attributes for a particular Supervisor internal port, which is how a
  *  device connects to a particular switch.
  */
-typedef struct {
-	u32 switchNo;
-	u32 internalPortNo;
-	VISORCHIPSET_STATE state;
-	u32 busNo;		/* valid only when state.attached == 1 */
-	u32 devNo;		/* valid only when state.attached == 1 */
-	u64 Reserved1;
-	u32 Reserved2;		/* CONTROLVM_ID */
-	CONTROLVM_MESSAGE_HEADER pendingMsgHdr;
-	MYPROCOBJECT *procObject;
-
-} VISORCHIPSET_INTERNALPORT_INFO;
+struct visorchipset_internalport_info {
+	u32 switch_no;
+	u32 internal_port_no;
+	struct visorchipset_state state;
+	u32 bus_no;		/* valid only when state.attached == 1 */
+	u32 dev_no;		/* valid only when state.attached == 1 */
+	u64 reserved1;
+	u32 reserved2;		/* CONTROLVM_ID */
+	struct controlvm_message_header pending_msg_hdr;
+	MYPROCOBJECT *proc_object;
+
+};
 
 /*  These functions will be called from within visorchipset when certain
  *  events happen.  (The implementation of these functions is outside of
  *  visorchipset.)
  */
-typedef struct {
-	void (*bus_create)(ulong busNo);
-	void (*bus_destroy)(ulong busNo);
-	void (*device_create)(ulong busNo, ulong devNo);
-	void (*device_destroy)(ulong busNo, ulong devNo);
-	void (*device_pause)(ulong busNo, ulong devNo);
-	void (*device_resume)(ulong busNo, ulong devNo);
-	int (*get_channel_info)(uuid_le typeGuid, ulong *minSize,
-				 ulong *maxSize);
-} VISORCHIPSET_BUSDEV_NOTIFIERS;
+struct visorchipset_busdev_notifiers {
+	void (*bus_create)(ulong bus_no);
+	void (*bus_destroy)(ulong bus_no);
+	void (*device_create)(ulong bus_no, ulong dev_no);
+	void (*device_destroy)(ulong bus_no, ulong dev_no);
+	void (*device_pause)(ulong bus_no, ulong dev_no);
+	void (*device_resume)(ulong bus_no, ulong dev_no);
+	int (*get_channel_info)(uuid_le type_uuid, ulong *min_size,
+				ulong *max_size);
+};
 
 /*  These functions live inside visorchipset, and will be called to indicate
  *  responses to specific events (by code outside of visorchipset).
@@ -236,14 +234,14 @@ typedef struct {
  *       0 = it worked
  *      -1 = it failed
  */
-typedef struct {
-	void (*bus_create)(ulong busNo, int response);
-	void (*bus_destroy)(ulong busNo, int response);
-	void (*device_create)(ulong busNo, ulong devNo, int response);
-	void (*device_destroy)(ulong busNo, ulong devNo, int response);
-	void (*device_pause)(ulong busNo, ulong devNo, int response);
-	void (*device_resume)(ulong busNo, ulong devNo, int response);
-} VISORCHIPSET_BUSDEV_RESPONDERS;
+struct visorchipset_busdev_responders {
+	void (*bus_create)(ulong bus_no, int response);
+	void (*bus_destroy)(ulong bus_no, int response);
+	void (*device_create)(ulong bus_no, ulong dev_no, int response);
+	void (*device_destroy)(ulong bus_no, ulong dev_no, int response);
+	void (*device_pause)(ulong bus_no, ulong dev_no, int response);
+	void (*device_resume)(ulong bus_no, ulong dev_no, int response);
+};
 
 /** Register functions (in the bus driver) to get called by visorchipset
  *  whenever a bus or device appears for which this service partition is
@@ -252,9 +250,10 @@ typedef struct {
  *  responses.
  */
 void
-visorchipset_register_busdev_client(VISORCHIPSET_BUSDEV_NOTIFIERS *notifiers,
-				    VISORCHIPSET_BUSDEV_RESPONDERS *responders,
-				    ULTRA_VBUS_DEVICEINFO *driverInfo);
+visorchipset_register_busdev_client(
+			struct visorchipset_busdev_notifiers *notifiers,
+			struct visorchipset_busdev_responders *responders,
+			struct ultra_vbus_deviceinfo *driver_info);
 
 /** Register functions (in the bus driver) to get called by visorchipset
  *  whenever a bus or device appears for which this service partition is
@@ -263,47 +262,31 @@ visorchipset_register_busdev_client(VISORCHIPSET_BUSDEV_NOTIFIERS *notifiers,
  *  responses.
  */
 void
-visorchipset_register_busdev_server(VISORCHIPSET_BUSDEV_NOTIFIERS *notifiers,
-				    VISORCHIPSET_BUSDEV_RESPONDERS *responders,
-				    ULTRA_VBUS_DEVICEINFO *driverInfo);
+visorchipset_register_busdev_server(
+			struct visorchipset_busdev_notifiers *notifiers,
+			struct visorchipset_busdev_responders *responders,
+			struct ultra_vbus_deviceinfo *driver_info);
 
-typedef void (*SPARREPORTEVENT_COMPLETE_FUNC) (CONTROLVM_MESSAGE *msg,
+typedef void (*SPARREPORTEVENT_COMPLETE_FUNC) (struct controlvm_message *msg,
 					       int status);
 
-void visorchipset_device_pause_response(ulong busNo, ulong devNo, int response);
+void visorchipset_device_pause_response(ulong bus_no, ulong dev_no,
+					int response);
 
-BOOL visorchipset_get_bus_info(ulong busNo, VISORCHIPSET_BUS_INFO *busInfo);
-BOOL visorchipset_get_device_info(ulong busNo, ulong devNo,
-				  VISORCHIPSET_DEVICE_INFO *devInfo);
-BOOL visorchipset_get_switch_info(ulong switchNo,
-				  VISORCHIPSET_SWITCH_INFO *switchInfo);
-BOOL visorchipset_get_externalport_info(ulong switchNo, ulong externalPortNo,
-					VISORCHIPSET_EXTERNALPORT_INFO
-					*externalPortInfo);
-BOOL visorchipset_set_bus_context(ulong busNo, void *context);
-BOOL visorchipset_set_device_context(ulong busNo, ulong devNo, void *context);
+BOOL visorchipset_get_bus_info(ulong bus_no,
+			       struct visorchipset_bus_info *bus_info);
+BOOL visorchipset_get_device_info(ulong bus_no, ulong dev_no,
+				  struct visorchipset_device_info *dev_info);
+BOOL visorchipset_set_bus_context(ulong bus_no, void *context);
+BOOL visorchipset_set_device_context(ulong bus_no, ulong dev_no, void *context);
 int visorchipset_chipset_ready(void);
 int visorchipset_chipset_selftest(void);
 int visorchipset_chipset_notready(void);
-void visorchipset_controlvm_respond_reportEvent(CONTROLVM_MESSAGE *msg,
-						void *payload);
-void visorchipset_save_message(CONTROLVM_MESSAGE *msg, CRASH_OBJ_TYPE type);
+void visorchipset_save_message(struct controlvm_message *msg,
+			       enum crash_obj_type type);
 void *visorchipset_cache_alloc(struct kmem_cache *pool,
 			       BOOL ok_to_block, char *fn, int ln);
 void visorchipset_cache_free(struct kmem_cache *pool, void *p,
 			     char *fn, int ln);
 
-#if defined(TRANSMITFILE_DEBUG) || defined(DEBUG)
-#define DBG_GETFILE_PAYLOAD(msg, controlvm_header)      \
-	LOGINF(msg,                                     \
-	       (ulong)controlvm_header.PayloadVmOffset, \
-	       (ulong)controlvm_header.PayloadMaxBytes)
-#define DBG_GETFILE(fmt, ...)  LOGINF(fmt, ##__VA_ARGS__)
-#define DBG_PUTFILE(fmt, ...)  LOGINF(fmt, ##__VA_ARGS__)
-#else
-#define DBG_GETFILE_PAYLOAD(msg, controlvm_header)
-#define DBG_GETFILE(fmt, ...)
-#define DBG_PUTFILE(fmt, ...)
-#endif
-
 #endif
diff --git a/drivers/staging/unisys/visorchipset/visorchipset_main.c b/drivers/staging/unisys/visorchipset/visorchipset_main.c
index e5df39554a1a..7e6be32cf7bb 100644
--- a/drivers/staging/unisys/visorchipset/visorchipset_main.c
+++ b/drivers/staging/unisys/visorchipset/visorchipset_main.c
@@ -72,26 +72,28 @@ static struct workqueue_struct *Periodic_controlvm_workqueue;
 static DEFINE_SEMAPHORE(NotifierLock);
 
 typedef struct {
-	CONTROLVM_MESSAGE message;
+	struct controlvm_message message;
 	unsigned int crc;
 } MESSAGE_ENVELOPE;
 
-static CONTROLVM_MESSAGE_HEADER g_DiagMsgHdr;
-static CONTROLVM_MESSAGE_HEADER g_ChipSetMsgHdr;
-static CONTROLVM_MESSAGE_HEADER g_DelDumpMsgHdr;
+static struct controlvm_message_header g_DiagMsgHdr;
+static struct controlvm_message_header g_ChipSetMsgHdr;
+static struct controlvm_message_header g_DelDumpMsgHdr;
 static const uuid_le UltraDiagPoolChannelProtocolGuid =
-	ULTRA_DIAG_POOL_CHANNEL_PROTOCOL_GUID;
+	SPAR_DIAG_POOL_CHANNEL_PROTOCOL_UUID;
 /* 0xffffff is an invalid Bus/Device number */
 static ulong g_diagpoolBusNo = 0xffffff;
 static ulong g_diagpoolDevNo = 0xffffff;
-static CONTROLVM_MESSAGE_PACKET g_DeviceChangeStatePacket;
+static struct controlvm_message_packet g_DeviceChangeStatePacket;
 
 /* Only VNIC and VHBA channels are sent to visorclientbus (aka
  * "visorhackbus")
  */
 #define FOR_VISORHACKBUS(channel_type_guid) \
-	(((uuid_le_cmp(channel_type_guid, UltraVnicChannelProtocolGuid) == 0)\
-	|| (uuid_le_cmp(channel_type_guid, UltraVhbaChannelProtocolGuid) == 0)))
+	(((uuid_le_cmp(channel_type_guid,\
+		       spar_vnic_channel_protocol_uuid) == 0)\
+	|| (uuid_le_cmp(channel_type_guid,\
+			spar_vhba_channel_protocol_uuid) == 0)))
 #define FOR_VISORBUS(channel_type_guid) (!(FOR_VISORHACKBUS(channel_type_guid)))
 
 #define is_diagpool_channel(channel_type_guid) \
@@ -112,12 +114,12 @@ typedef struct {
 /* Manages the request payload in the controlvm channel */
 static CONTROLVM_PAYLOAD_INFO ControlVm_payload_info;
 
-static pCHANNEL_HEADER Test_Vnic_channel;
+static struct channel_header *Test_Vnic_channel;
 
 typedef struct {
-	CONTROLVM_MESSAGE_HEADER Dumpcapture_header;
-	CONTROLVM_MESSAGE_HEADER Gettextdump_header;
-	CONTROLVM_MESSAGE_HEADER Dumpcomplete_header;
+	struct controlvm_message_header Dumpcapture_header;
+	struct controlvm_message_header Gettextdump_header;
+	struct controlvm_message_header Dumpcomplete_header;
 	BOOL Gettextdump_outstanding;
 	u32 crc32;
 	ulong length;
@@ -134,7 +136,7 @@ static LIVEDUMP_INFO LiveDump_info;
  * this scenario, we simply stash the controlvm message, then attempt to
  * process it again the next time controlvm_periodic_work() runs.
  */
-static CONTROLVM_MESSAGE ControlVm_Pending_Msg;
+static struct controlvm_message ControlVm_Pending_Msg;
 static BOOL ControlVm_Pending_Msg_Valid = FALSE;
 
 /* Pool of struct putfile_buffer_entry, for keeping track of pending (incoming)
@@ -180,7 +182,7 @@ struct putfile_request {
 	u64 sig;		/* PUTFILE_REQUEST_SIG */
 
 	/* header from original TransmitFile request */
-	CONTROLVM_MESSAGE_HEADER controlvm_header;
+	struct controlvm_message_header controlvm_header;
 	u64 file_request_number;	/* from original TransmitFile request */
 
 	/* link to next struct putfile_request */
@@ -218,7 +220,7 @@ struct parahotplug_request {
 	struct list_head list;
 	int id;
 	unsigned long expiration;
-	CONTROLVM_MESSAGE msg;
+	struct controlvm_message msg;
 };
 
 static LIST_HEAD(Parahotplug_request_list);
@@ -228,8 +230,8 @@ static void parahotplug_process_list(void);
 /* Manages the info for a CONTROLVM_DUMP_CAPTURESTATE /
  * CONTROLVM_REPORTEVENT.
  */
-static VISORCHIPSET_BUSDEV_NOTIFIERS BusDev_Server_Notifiers;
-static VISORCHIPSET_BUSDEV_NOTIFIERS BusDev_Client_Notifiers;
+static struct visorchipset_busdev_notifiers BusDev_Server_Notifiers;
+static struct visorchipset_busdev_notifiers BusDev_Client_Notifiers;
 
 static void bus_create_response(ulong busNo, int response);
 static void bus_destroy_response(ulong busNo, int response);
@@ -237,7 +239,7 @@ static void device_create_response(ulong busNo, ulong devNo, int response);
 static void device_destroy_response(ulong busNo, ulong devNo, int response);
 static void device_resume_response(ulong busNo, ulong devNo, int response);
 
-static VISORCHIPSET_BUSDEV_RESPONDERS BusDev_Responders = {
+static struct visorchipset_busdev_responders BusDev_Responders = {
 	.bus_create = bus_create_response,
 	.bus_destroy = bus_destroy_response,
 	.device_create = device_create_response,
@@ -342,13 +344,14 @@ static struct platform_device Visorchipset_platform_device = {
 };
 
 /* Function prototypes */
-static void controlvm_respond(CONTROLVM_MESSAGE_HEADER *msgHdr, int response);
-static void controlvm_respond_chipset_init(CONTROLVM_MESSAGE_HEADER *msgHdr,
-					   int response,
-					   ULTRA_CHIPSET_FEATURE features);
-static void controlvm_respond_physdev_changestate(CONTROLVM_MESSAGE_HEADER *
-						  msgHdr, int response,
-						  ULTRA_SEGMENT_STATE state);
+static void controlvm_respond(struct controlvm_message_header *msgHdr,
+			      int response);
+static void controlvm_respond_chipset_init(
+		struct controlvm_message_header *msgHdr, int response,
+		enum ultra_chipset_feature features);
+static void controlvm_respond_physdev_changestate(
+		struct controlvm_message_header *msgHdr, int response,
+		struct spar_segment_state state);
 
 static ssize_t toolaction_show(struct device *dev,
 			       struct device_attribute *attr,
@@ -357,8 +360,8 @@ static ssize_t toolaction_show(struct device *dev,
 	u8 toolAction;
 
 	visorchannel_read(ControlVm_channel,
-		offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL,
-			   ToolAction), &toolAction, sizeof(u8));
+		offsetof(struct spar_controlvm_channel_protocol,
+			   tool_action), &toolAction, sizeof(u8));
 	return scnprintf(buf, PAGE_SIZE, "%u\n", toolAction);
 }
 
@@ -373,7 +376,7 @@ static ssize_t toolaction_store(struct device *dev,
 		return -EINVAL;
 
 	ret = visorchannel_write(ControlVm_channel,
-		offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL, ToolAction),
+		offsetof(struct spar_controlvm_channel_protocol, tool_action),
 		&toolAction, sizeof(u8));
 
 	if (ret)
@@ -385,14 +388,14 @@ static ssize_t boottotool_show(struct device *dev,
 			       struct device_attribute *attr,
 			       char *buf)
 {
-	ULTRA_EFI_SPAR_INDICATION efiSparIndication;
+	struct efi_spar_indication efiSparIndication;
 
 	visorchannel_read(ControlVm_channel,
-		offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL,
-			EfiSparIndication), &efiSparIndication,
-		sizeof(ULTRA_EFI_SPAR_INDICATION));
+		offsetof(struct spar_controlvm_channel_protocol,
+			efi_spar_ind), &efiSparIndication,
+		sizeof(struct efi_spar_indication));
 	return scnprintf(buf, PAGE_SIZE, "%u\n",
-			efiSparIndication.BootToTool);
+			efiSparIndication.boot_to_tool);
 }
 
 static ssize_t boottotool_store(struct device *dev,
@@ -400,17 +403,17 @@ static ssize_t boottotool_store(struct device *dev,
 				const char *buf, size_t count)
 {
 	int val, ret;
-	ULTRA_EFI_SPAR_INDICATION efiSparIndication;
+	struct efi_spar_indication efiSparIndication;
 
 	if (kstrtoint(buf, 10, &val) != 0)
 		return -EINVAL;
 
-	efiSparIndication.BootToTool = val;
+	efiSparIndication.boot_to_tool = val;
 	ret = visorchannel_write(ControlVm_channel,
-			offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL,
-				EfiSparIndication),
+			offsetof(struct spar_controlvm_channel_protocol,
+				efi_spar_ind),
 			&(efiSparIndication),
-		sizeof(ULTRA_EFI_SPAR_INDICATION));
+		sizeof(struct efi_spar_indication));
 
 	if (ret)
 		return ret;
@@ -423,7 +426,7 @@ static ssize_t error_show(struct device *dev, struct device_attribute *attr,
 	u32 error;
 
 	visorchannel_read(ControlVm_channel, offsetof(
-		ULTRA_CONTROLVM_CHANNEL_PROTOCOL, InstallationError),
+		struct spar_controlvm_channel_protocol, installation_error),
 		&error, sizeof(u32));
 	return scnprintf(buf, PAGE_SIZE, "%i\n", error);
 }
@@ -438,8 +441,8 @@ static ssize_t error_store(struct device *dev, struct device_attribute *attr,
 		return -EINVAL;
 
 	ret = visorchannel_write(ControlVm_channel,
-			offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL,
-				InstallationError),
+			offsetof(struct spar_controlvm_channel_protocol,
+				installation_error),
 			&error, sizeof(u32));
 	if (ret)
 		return ret;
@@ -452,7 +455,7 @@ static ssize_t textid_show(struct device *dev, struct device_attribute *attr,
 	u32 textId;
 
 	visorchannel_read(ControlVm_channel, offsetof(
-		ULTRA_CONTROLVM_CHANNEL_PROTOCOL, InstallationTextId),
+		struct spar_controlvm_channel_protocol, installation_text_id),
 		&textId, sizeof(u32));
 	return scnprintf(buf, PAGE_SIZE, "%i\n", textId);
 }
@@ -467,8 +470,8 @@ static ssize_t textid_store(struct device *dev, struct device_attribute *attr,
 		return -EINVAL;
 
 	ret = visorchannel_write(ControlVm_channel,
-			offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL,
-				InstallationTextId),
+			offsetof(struct spar_controlvm_channel_protocol,
+				installation_text_id),
 			&textId, sizeof(u32));
 	if (ret)
 		return ret;
@@ -482,8 +485,8 @@ static ssize_t remaining_steps_show(struct device *dev,
 	u16 remainingSteps;
 
 	visorchannel_read(ControlVm_channel,
-		offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL,
-			InstallationRemainingSteps),
+		offsetof(struct spar_controlvm_channel_protocol,
+			installation_remaining_steps),
 		&remainingSteps,
 		sizeof(u16));
 	return scnprintf(buf, PAGE_SIZE, "%hu\n", remainingSteps);
@@ -499,8 +502,8 @@ static ssize_t remaining_steps_store(struct device *dev,
 		return -EINVAL;
 
 	ret = visorchannel_write(ControlVm_channel,
-			offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL,
-				InstallationRemainingSteps),
+			offsetof(struct spar_controlvm_channel_protocol,
+				installation_remaining_steps),
 			&remainingSteps, sizeof(u16));
 	if (ret)
 		return ret;
@@ -539,11 +542,11 @@ testUnicode(void)
 static void
 busInfo_clear(void *v)
 {
-	VISORCHIPSET_BUS_INFO *p = (VISORCHIPSET_BUS_INFO *) (v);
+	struct visorchipset_bus_info *p = (struct visorchipset_bus_info *) (v);
 
-	if (p->procObject) {
-		visor_proc_DestroyObject(p->procObject);
-		p->procObject = NULL;
+	if (p->proc_object) {
+		visor_proc_DestroyObject(p->proc_object);
+		p->proc_object = NULL;
 	}
 	kfree(p->name);
 	p->name = NULL;
@@ -552,16 +555,17 @@ busInfo_clear(void *v)
 	p->description = NULL;
 
 	p->state.created = 0;
-	memset(p, 0, sizeof(VISORCHIPSET_BUS_INFO));
+	memset(p, 0, sizeof(struct visorchipset_bus_info));
 }
 
 static void
 devInfo_clear(void *v)
 {
-	VISORCHIPSET_DEVICE_INFO *p = (VISORCHIPSET_DEVICE_INFO *) (v);
+	struct visorchipset_device_info *p =
+			(struct visorchipset_device_info *)(v);
 
 	p->state.created = 0;
-	memset(p, 0, sizeof(VISORCHIPSET_DEVICE_INFO));
+	memset(p, 0, sizeof(struct visorchipset_device_info));
 }
 
 static u8
@@ -585,9 +589,10 @@ clear_chipset_events(void)
 }
 
 void
-visorchipset_register_busdev_server(VISORCHIPSET_BUSDEV_NOTIFIERS *notifiers,
-				    VISORCHIPSET_BUSDEV_RESPONDERS *responders,
-				    ULTRA_VBUS_DEVICEINFO *driverInfo)
+visorchipset_register_busdev_server(
+			struct visorchipset_busdev_notifiers *notifiers,
+			struct visorchipset_busdev_responders *responders,
+			struct ultra_vbus_deviceinfo *driver_info)
 {
 	down(&NotifierLock);
 	if (notifiers == NULL) {
@@ -600,8 +605,8 @@ visorchipset_register_busdev_server(VISORCHIPSET_BUSDEV_NOTIFIERS *notifiers,
 	}
 	if (responders)
 		*responders = BusDev_Responders;
-	if (driverInfo)
-		bus_device_info_init(driverInfo, "chipset", "visorchipset",
+	if (driver_info)
+		bus_device_info_init(driver_info, "chipset", "visorchipset",
 				   VERSION, NULL);
 
 	up(&NotifierLock);
@@ -609,9 +614,10 @@ visorchipset_register_busdev_server(VISORCHIPSET_BUSDEV_NOTIFIERS *notifiers,
 EXPORT_SYMBOL_GPL(visorchipset_register_busdev_server);
 
 void
-visorchipset_register_busdev_client(VISORCHIPSET_BUSDEV_NOTIFIERS *notifiers,
-				    VISORCHIPSET_BUSDEV_RESPONDERS *responders,
-				    ULTRA_VBUS_DEVICEINFO *driverInfo)
+visorchipset_register_busdev_client(
+			struct visorchipset_busdev_notifiers *notifiers,
+			struct visorchipset_busdev_responders *responders,
+			struct ultra_vbus_deviceinfo *driver_info)
 {
 	down(&NotifierLock);
 	if (notifiers == NULL) {
@@ -624,9 +630,9 @@ visorchipset_register_busdev_client(VISORCHIPSET_BUSDEV_NOTIFIERS *notifiers,
 	}
 	if (responders)
 		*responders = BusDev_Responders;
-	if (driverInfo)
-		bus_device_info_init(driverInfo, "chipset(bolts)", "visorchipset",
-				   VERSION, NULL);
+	if (driver_info)
+		bus_device_info_init(driver_info, "chipset(bolts)",
+				     "visorchipset", VERSION, NULL);
 	up(&NotifierLock);
 }
 EXPORT_SYMBOL_GPL(visorchipset_register_busdev_client);
@@ -634,8 +640,8 @@ EXPORT_SYMBOL_GPL(visorchipset_register_busdev_client);
 static void
 cleanup_controlvm_structures(void)
 {
-	VISORCHIPSET_BUS_INFO *bi, *tmp_bi;
-	VISORCHIPSET_DEVICE_INFO *di, *tmp_di;
+	struct visorchipset_bus_info *bi, *tmp_bi;
+	struct visorchipset_device_info *di, *tmp_di;
 
 	list_for_each_entry_safe(bi, tmp_bi, &BusInfoList, entry) {
 		busInfo_clear(bi);
@@ -651,10 +657,10 @@ cleanup_controlvm_structures(void)
 }
 
 static void
-chipset_init(CONTROLVM_MESSAGE *inmsg)
+chipset_init(struct controlvm_message *inmsg)
 {
 	static int chipset_inited;
-	ULTRA_CHIPSET_FEATURE features = 0;
+	enum ultra_chipset_feature features = 0;
 	int rc = CONTROLVM_RESP_SUCCESS;
 
 	POSTCODE_LINUX_2(CHIPSET_INIT_ENTRY_PC, POSTCODE_SEVERITY_INFO);
@@ -669,7 +675,7 @@ chipset_init(CONTROLVM_MESSAGE *inmsg)
 	/* Set features to indicate we support parahotplug (if Command
 	 * also supports it). */
 	features =
-	    inmsg->cmd.initChipset.
+	    inmsg->cmd.init_chipset.
 	    features & ULTRA_CHIPSET_FEATURE_PARA_HOTPLUG;
 
 	/* Set the "reply" bit so Command knows this is a
@@ -679,42 +685,42 @@ chipset_init(CONTROLVM_MESSAGE *inmsg)
 Away:
 	if (rc < 0)
 		cleanup_controlvm_structures();
-	if (inmsg->hdr.Flags.responseExpected)
+	if (inmsg->hdr.flags.response_expected)
 		controlvm_respond_chipset_init(&inmsg->hdr, rc, features);
 }
 
 static void
-controlvm_init_response(CONTROLVM_MESSAGE *msg,
-			CONTROLVM_MESSAGE_HEADER *msgHdr, int response)
-{
-	memset(msg, 0, sizeof(CONTROLVM_MESSAGE));
-	memcpy(&msg->hdr, msgHdr, sizeof(CONTROLVM_MESSAGE_HEADER));
-	msg->hdr.PayloadBytes = 0;
-	msg->hdr.PayloadVmOffset = 0;
-	msg->hdr.PayloadMaxBytes = 0;
+controlvm_init_response(struct controlvm_message *msg,
+			struct controlvm_message_header *msgHdr, int response)
+{
+	memset(msg, 0, sizeof(struct controlvm_message));
+	memcpy(&msg->hdr, msgHdr, sizeof(struct controlvm_message_header));
+	msg->hdr.payload_bytes = 0;
+	msg->hdr.payload_vm_offset = 0;
+	msg->hdr.payload_max_bytes = 0;
 	if (response < 0) {
-		msg->hdr.Flags.failed = 1;
-		msg->hdr.CompletionStatus = (u32) (-response);
+		msg->hdr.flags.failed = 1;
+		msg->hdr.completion_status = (u32) (-response);
 	}
 }
 
 static void
-controlvm_respond(CONTROLVM_MESSAGE_HEADER *msgHdr, int response)
+controlvm_respond(struct controlvm_message_header *msgHdr, int response)
 {
-	CONTROLVM_MESSAGE outmsg;
+	struct controlvm_message outmsg;
 
 	controlvm_init_response(&outmsg, msgHdr, response);
 	/* For DiagPool channel DEVICE_CHANGESTATE, we need to send
 	* back the deviceChangeState structure in the packet. */
-	if (msgHdr->Id == CONTROLVM_DEVICE_CHANGESTATE
-	    && g_DeviceChangeStatePacket.deviceChangeState.busNo ==
+	if (msgHdr->id == CONTROLVM_DEVICE_CHANGESTATE
+	    && g_DeviceChangeStatePacket.device_change_state.bus_no ==
 	    g_diagpoolBusNo
-	    && g_DeviceChangeStatePacket.deviceChangeState.devNo ==
+	    && g_DeviceChangeStatePacket.device_change_state.dev_no ==
 	    g_diagpoolDevNo)
 		outmsg.cmd = g_DeviceChangeStatePacket;
-	if (outmsg.hdr.Flags.testMessage == 1) {
+	if (outmsg.hdr.flags.test_message == 1) {
 		LOGINF("%s controlvm_msg=0x%x response=%d for test message",
-		       __func__, outmsg.hdr.Id, response);
+		       __func__, outmsg.hdr.id, response);
 		return;
 	}
 	if (!visorchannel_signalinsert(ControlVm_channel,
@@ -725,13 +731,14 @@ controlvm_respond(CONTROLVM_MESSAGE_HEADER *msgHdr, int response)
 }
 
 static void
-controlvm_respond_chipset_init(CONTROLVM_MESSAGE_HEADER *msgHdr, int response,
-			       ULTRA_CHIPSET_FEATURE features)
+controlvm_respond_chipset_init(struct controlvm_message_header *msgHdr,
+			       int response,
+			       enum ultra_chipset_feature features)
 {
-	CONTROLVM_MESSAGE outmsg;
+	struct controlvm_message outmsg;
 
 	controlvm_init_response(&outmsg, msgHdr, response);
-	outmsg.cmd.initChipset.features = features;
+	outmsg.cmd.init_chipset.features = features;
 	if (!visorchannel_signalinsert(ControlVm_channel,
 				       CONTROLVM_QUEUE_REQUEST, &outmsg)) {
 		LOGERR("signalinsert failed!");
@@ -739,15 +746,15 @@ controlvm_respond_chipset_init(CONTROLVM_MESSAGE_HEADER *msgHdr, int response,
 	}
 }
 
-static void
-controlvm_respond_physdev_changestate(CONTROLVM_MESSAGE_HEADER *msgHdr,
-				      int response, ULTRA_SEGMENT_STATE state)
+static void controlvm_respond_physdev_changestate(
+		struct controlvm_message_header *msgHdr, int response,
+		struct spar_segment_state state)
 {
-	CONTROLVM_MESSAGE outmsg;
+	struct controlvm_message outmsg;
 
 	controlvm_init_response(&outmsg, msgHdr, response);
-	outmsg.cmd.deviceChangeState.state = state;
-	outmsg.cmd.deviceChangeState.flags.physicalDevice = 1;
+	outmsg.cmd.device_change_state.state = state;
+	outmsg.cmd.device_change_state.flags.phys_device = 1;
 	if (!visorchannel_signalinsert(ControlVm_channel,
 				       CONTROLVM_QUEUE_REQUEST, &outmsg)) {
 		LOGERR("signalinsert failed!");
@@ -756,15 +763,16 @@ controlvm_respond_physdev_changestate(CONTROLVM_MESSAGE_HEADER *msgHdr,
 }
 
 void
-visorchipset_save_message(CONTROLVM_MESSAGE *msg, CRASH_OBJ_TYPE type)
+visorchipset_save_message(struct controlvm_message *msg,
+			  enum crash_obj_type type)
 {
 	u32 localSavedCrashMsgOffset;
 	u16 localSavedCrashMsgCount;
 
 	/* get saved message count */
 	if (visorchannel_read(ControlVm_channel,
-			      offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL,
-				       SavedCrashMsgCount),
+			      offsetof(struct spar_controlvm_channel_protocol,
+				       saved_crash_message_count),
 			      &localSavedCrashMsgCount, sizeof(u16)) < 0) {
 		LOGERR("failed to get Saved Message Count");
 		POSTCODE_LINUX_2(CRASH_DEV_CTRL_RD_FAILURE_PC,
@@ -783,8 +791,8 @@ visorchipset_save_message(CONTROLVM_MESSAGE *msg, CRASH_OBJ_TYPE type)
 
 	/* get saved crash message offset */
 	if (visorchannel_read(ControlVm_channel,
-			      offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL,
-				       SavedCrashMsgOffset),
+			      offsetof(struct spar_controlvm_channel_protocol,
+				       saved_crash_message_offset),
 			      &localSavedCrashMsgOffset, sizeof(u32)) < 0) {
 		LOGERR("failed to get Saved Message Offset");
 		POSTCODE_LINUX_2(CRASH_DEV_CTRL_RD_FAILURE_PC,
@@ -792,10 +800,11 @@ visorchipset_save_message(CONTROLVM_MESSAGE *msg, CRASH_OBJ_TYPE type)
 		return;
 	}
 
-	if (type == CRASH_bus) {
+	if (type == CRASH_BUS) {
 		if (visorchannel_write(ControlVm_channel,
 				       localSavedCrashMsgOffset,
-				       msg, sizeof(CONTROLVM_MESSAGE)) < 0) {
+				       msg,
+				       sizeof(struct controlvm_message)) < 0) {
 			LOGERR("SAVE_MSG_BUS_FAILURE: Failed to write CrashCreateBusMsg!");
 			POSTCODE_LINUX_2(SAVE_MSG_BUS_FAILURE_PC,
 					 POSTCODE_SEVERITY_ERR);
@@ -804,8 +813,8 @@ visorchipset_save_message(CONTROLVM_MESSAGE *msg, CRASH_OBJ_TYPE type)
 	} else {
 		if (visorchannel_write(ControlVm_channel,
 				       localSavedCrashMsgOffset +
-				       sizeof(CONTROLVM_MESSAGE), msg,
-				       sizeof(CONTROLVM_MESSAGE)) < 0) {
+				       sizeof(struct controlvm_message), msg,
+				       sizeof(struct controlvm_message)) < 0) {
 			LOGERR("SAVE_MSG_DEV_FAILURE: Failed to write CrashCreateDevMsg!");
 			POSTCODE_LINUX_2(SAVE_MSG_DEV_FAILURE_PC,
 					 POSTCODE_SEVERITY_ERR);
@@ -816,9 +825,9 @@ visorchipset_save_message(CONTROLVM_MESSAGE *msg, CRASH_OBJ_TYPE type)
 EXPORT_SYMBOL_GPL(visorchipset_save_message);
 
 static void
-bus_responder(CONTROLVM_ID cmdId, ulong busNo, int response)
+bus_responder(enum controlvm_id cmdId, ulong busNo, int response)
 {
-	VISORCHIPSET_BUS_INFO *p = NULL;
+	struct visorchipset_bus_info *p = NULL;
 	BOOL need_clear = FALSE;
 
 	p = findbus(&BusInfoList, busNo);
@@ -838,16 +847,16 @@ bus_responder(CONTROLVM_ID cmdId, ulong busNo, int response)
 			need_clear = TRUE;
 	}
 
-	if (p->pendingMsgHdr.Id == CONTROLVM_INVALID) {
+	if (p->pending_msg_hdr.id == CONTROLVM_INVALID) {
 		LOGERR("bus_responder no pending msg");
 		return;		/* no controlvm response needed */
 	}
-	if (p->pendingMsgHdr.Id != (u32) cmdId) {
-		LOGERR("expected=%d, found=%d", cmdId, p->pendingMsgHdr.Id);
+	if (p->pending_msg_hdr.id != (u32) cmdId) {
+		LOGERR("expected=%d, found=%d", cmdId, p->pending_msg_hdr.id);
 		return;
 	}
-	controlvm_respond(&p->pendingMsgHdr, response);
-	p->pendingMsgHdr.Id = CONTROLVM_INVALID;
+	controlvm_respond(&p->pending_msg_hdr, response);
+	p->pending_msg_hdr.id = CONTROLVM_INVALID;
 	if (need_clear) {
 		busInfo_clear(p);
 		delbusdevices(&DevInfoList, busNo);
@@ -855,32 +864,32 @@ bus_responder(CONTROLVM_ID cmdId, ulong busNo, int response)
 }
 
 static void
-device_changestate_responder(CONTROLVM_ID cmdId,
+device_changestate_responder(enum controlvm_id cmdId,
 			     ulong busNo, ulong devNo, int response,
-			     ULTRA_SEGMENT_STATE responseState)
+			     struct spar_segment_state responseState)
 {
-	VISORCHIPSET_DEVICE_INFO *p = NULL;
-	CONTROLVM_MESSAGE outmsg;
+	struct visorchipset_device_info *p = NULL;
+	struct controlvm_message outmsg;
 
 	p = finddevice(&DevInfoList, busNo, devNo);
 	if (!p) {
 		LOGERR("internal error; busNo=%lu, devNo=%lu", busNo, devNo);
 		return;
 	}
-	if (p->pendingMsgHdr.Id == CONTROLVM_INVALID) {
+	if (p->pending_msg_hdr.id == CONTROLVM_INVALID) {
 		LOGERR("device_responder no pending msg");
 		return;		/* no controlvm response needed */
 	}
-	if (p->pendingMsgHdr.Id != cmdId) {
-		LOGERR("expected=%d, found=%d", cmdId, p->pendingMsgHdr.Id);
+	if (p->pending_msg_hdr.id != cmdId) {
+		LOGERR("expected=%d, found=%d", cmdId, p->pending_msg_hdr.id);
 		return;
 	}
 
-	controlvm_init_response(&outmsg, &p->pendingMsgHdr, response);
+	controlvm_init_response(&outmsg, &p->pending_msg_hdr, response);
 
-	outmsg.cmd.deviceChangeState.busNo = busNo;
-	outmsg.cmd.deviceChangeState.devNo = devNo;
-	outmsg.cmd.deviceChangeState.state = responseState;
+	outmsg.cmd.device_change_state.bus_no = busNo;
+	outmsg.cmd.device_change_state.dev_no = devNo;
+	outmsg.cmd.device_change_state.state = responseState;
 
 	if (!visorchannel_signalinsert(ControlVm_channel,
 				       CONTROLVM_QUEUE_REQUEST, &outmsg)) {
@@ -888,13 +897,14 @@ device_changestate_responder(CONTROLVM_ID cmdId,
 		return;
 	}
 
-	p->pendingMsgHdr.Id = CONTROLVM_INVALID;
+	p->pending_msg_hdr.id = CONTROLVM_INVALID;
 }
 
 static void
-device_responder(CONTROLVM_ID cmdId, ulong busNo, ulong devNo, int response)
+device_responder(enum controlvm_id cmdId, ulong busNo, ulong devNo,
+		 int response)
 {
-	VISORCHIPSET_DEVICE_INFO *p = NULL;
+	struct visorchipset_device_info *p = NULL;
 	BOOL need_clear = FALSE;
 
 	p = finddevice(&DevInfoList, busNo, devNo);
@@ -909,38 +919,38 @@ device_responder(CONTROLVM_ID cmdId, ulong busNo, ulong devNo, int response)
 			need_clear = TRUE;
 	}
 
-	if (p->pendingMsgHdr.Id == CONTROLVM_INVALID) {
+	if (p->pending_msg_hdr.id == CONTROLVM_INVALID) {
 		LOGERR("device_responder no pending msg");
 		return;		/* no controlvm response needed */
 	}
-	if (p->pendingMsgHdr.Id != (u32) cmdId) {
-		LOGERR("expected=%d, found=%d", cmdId, p->pendingMsgHdr.Id);
+	if (p->pending_msg_hdr.id != (u32) cmdId) {
+		LOGERR("expected=%d, found=%d", cmdId, p->pending_msg_hdr.id);
 		return;
 	}
-	controlvm_respond(&p->pendingMsgHdr, response);
-	p->pendingMsgHdr.Id = CONTROLVM_INVALID;
+	controlvm_respond(&p->pending_msg_hdr, response);
+	p->pending_msg_hdr.id = CONTROLVM_INVALID;
 	if (need_clear)
 		devInfo_clear(p);
 }
 
 static void
 bus_epilog(u32 busNo,
-	   u32 cmd, CONTROLVM_MESSAGE_HEADER *msgHdr,
+	   u32 cmd, struct controlvm_message_header *msgHdr,
 	   int response, BOOL needResponse)
 {
 	BOOL notified = FALSE;
 
-	VISORCHIPSET_BUS_INFO *pBusInfo = findbus(&BusInfoList, busNo);
+	struct visorchipset_bus_info *pBusInfo = findbus(&BusInfoList, busNo);
 
 	if (!pBusInfo) {
 		LOGERR("HUH? bad busNo=%d", busNo);
 		return;
 	}
 	if (needResponse) {
-		memcpy(&pBusInfo->pendingMsgHdr, msgHdr,
-		       sizeof(CONTROLVM_MESSAGE_HEADER));
+		memcpy(&pBusInfo->pending_msg_hdr, msgHdr,
+		       sizeof(struct controlvm_message_header));
 	} else
-		pBusInfo->pendingMsgHdr.Id = CONTROLVM_INVALID;
+		pBusInfo->pending_msg_hdr.id = CONTROLVM_INVALID;
 
 	down(&NotifierLock);
 	if (response == CONTROLVM_RESP_SUCCESS) {
@@ -981,7 +991,7 @@ bus_epilog(u32 busNo,
 	}
 	if (notified)
 		/* The callback function just called above is responsible
-		 * for calling the appropriate VISORCHIPSET_BUSDEV_RESPONDERS
+		 * for calling the appropriate visorchipset_busdev_responders
 		 * function, which will call bus_responder()
 		 */
 		;
@@ -991,14 +1001,14 @@ bus_epilog(u32 busNo,
 }
 
 static void
-device_epilog(u32 busNo, u32 devNo, ULTRA_SEGMENT_STATE state, u32 cmd,
-	      CONTROLVM_MESSAGE_HEADER *msgHdr, int response,
+device_epilog(u32 busNo, u32 devNo, struct spar_segment_state state, u32 cmd,
+	      struct controlvm_message_header *msgHdr, int response,
 	      BOOL needResponse, BOOL for_visorbus)
 {
-	VISORCHIPSET_BUSDEV_NOTIFIERS *notifiers = NULL;
+	struct visorchipset_busdev_notifiers *notifiers = NULL;
 	BOOL notified = FALSE;
 
-	VISORCHIPSET_DEVICE_INFO *pDevInfo =
+	struct visorchipset_device_info *pDevInfo =
 		finddevice(&DevInfoList, busNo, devNo);
 	char *envp[] = {
 		"SPARSP_DIAGPOOL_PAUSED_STATE = 1",
@@ -1014,10 +1024,10 @@ device_epilog(u32 busNo, u32 devNo, ULTRA_SEGMENT_STATE state, u32 cmd,
 	else
 		notifiers = &BusDev_Client_Notifiers;
 	if (needResponse) {
-		memcpy(&pDevInfo->pendingMsgHdr, msgHdr,
-		       sizeof(CONTROLVM_MESSAGE_HEADER));
+		memcpy(&pDevInfo->pending_msg_hdr, msgHdr,
+		       sizeof(struct controlvm_message_header));
 	} else
-		pDevInfo->pendingMsgHdr.Id = CONTROLVM_INVALID;
+		pDevInfo->pending_msg_hdr.id = CONTROLVM_INVALID;
 
 	down(&NotifierLock);
 	if (response >= 0) {
@@ -1030,8 +1040,9 @@ device_epilog(u32 busNo, u32 devNo, ULTRA_SEGMENT_STATE state, u32 cmd,
 			break;
 		case CONTROLVM_DEVICE_CHANGESTATE:
 			/* ServerReady / ServerRunning / SegmentStateRunning */
-			if (state.Alive == SegmentStateRunning.Alive &&
-			    state.Operating == SegmentStateRunning.Operating) {
+			if (state.alive == segment_state_running.alive &&
+			    state.operating ==
+				segment_state_running.operating) {
 				if (notifiers->device_resume) {
 					(*notifiers->device_resume) (busNo,
 								     devNo);
@@ -1039,9 +1050,9 @@ device_epilog(u32 busNo, u32 devNo, ULTRA_SEGMENT_STATE state, u32 cmd,
 				}
 			}
 			/* ServerNotReady / ServerLost / SegmentStateStandby */
-			else if (state.Alive == SegmentStateStandby.Alive &&
-				 state.Operating ==
-				 SegmentStateStandby.Operating) {
+			else if (state.alive == segment_state_standby.alive &&
+				 state.operating ==
+				 segment_state_standby.operating) {
 				/* technically this is standby case
 				 * where server is lost
 				 */
@@ -1050,9 +1061,9 @@ device_epilog(u32 busNo, u32 devNo, ULTRA_SEGMENT_STATE state, u32 cmd,
 								    devNo);
 					notified = TRUE;
 				}
-			} else if (state.Alive == SegmentStatePaused.Alive &&
-				   state.Operating ==
-				   SegmentStatePaused.Operating) {
+			} else if (state.alive == segment_state_paused.alive &&
+				   state.operating ==
+				   segment_state_paused.operating) {
 				/* this is lite pause where channel is
 				 * still valid just 'pause' of it
 				 */
@@ -1079,7 +1090,7 @@ device_epilog(u32 busNo, u32 devNo, ULTRA_SEGMENT_STATE state, u32 cmd,
 	}
 	if (notified)
 		/* The callback function just called above is responsible
-		 * for calling the appropriate VISORCHIPSET_BUSDEV_RESPONDERS
+		 * for calling the appropriate visorchipset_busdev_responders
 		 * function, which will call device_responder()
 		 */
 		;
@@ -1089,12 +1100,12 @@ device_epilog(u32 busNo, u32 devNo, ULTRA_SEGMENT_STATE state, u32 cmd,
 }
 
 static void
-bus_create(CONTROLVM_MESSAGE *inmsg)
+bus_create(struct controlvm_message *inmsg)
 {
-	CONTROLVM_MESSAGE_PACKET *cmd = &inmsg->cmd;
-	ulong busNo = cmd->createBus.busNo;
+	struct controlvm_message_packet *cmd = &inmsg->cmd;
+	ulong busNo = cmd->create_bus.bus_no;
 	int rc = CONTROLVM_RESP_SUCCESS;
-	VISORCHIPSET_BUS_INFO *pBusInfo = NULL;
+	struct visorchipset_bus_info *pBusInfo = NULL;
 
 
 	pBusInfo = findbus(&BusInfoList, busNo);
@@ -1106,7 +1117,7 @@ bus_create(CONTROLVM_MESSAGE *inmsg)
 		rc = -CONTROLVM_RESP_ERROR_ALREADY_DONE;
 		goto Away;
 	}
-	pBusInfo = kzalloc(sizeof(VISORCHIPSET_BUS_INFO), GFP_KERNEL);
+	pBusInfo = kzalloc(sizeof(struct visorchipset_bus_info), GFP_KERNEL);
 	if (pBusInfo == NULL) {
 		LOGERR("CONTROLVM_BUS_CREATE Failed: bus %lu kzalloc failed",
 		       busNo);
@@ -1117,21 +1128,22 @@ bus_create(CONTROLVM_MESSAGE *inmsg)
 	}
 
 	INIT_LIST_HEAD(&pBusInfo->entry);
-	pBusInfo->busNo = busNo;
-	pBusInfo->devNo = cmd->createBus.deviceCount;
+	pBusInfo->bus_no = busNo;
+	pBusInfo->dev_no = cmd->create_bus.dev_count;
 
 	POSTCODE_LINUX_3(BUS_CREATE_ENTRY_PC, busNo, POSTCODE_SEVERITY_INFO);
 
-	if (inmsg->hdr.Flags.testMessage == 1)
-		pBusInfo->chanInfo.addrType = ADDRTYPE_localTest;
+	if (inmsg->hdr.flags.test_message == 1)
+		pBusInfo->chan_info.addr_type = ADDRTYPE_LOCALTEST;
 	else
-		pBusInfo->chanInfo.addrType = ADDRTYPE_localPhysical;
+		pBusInfo->chan_info.addr_type = ADDRTYPE_LOCALPHYSICAL;
 
-	pBusInfo->flags.server = inmsg->hdr.Flags.server;
-	pBusInfo->chanInfo.channelAddr = cmd->createBus.channelAddr;
-	pBusInfo->chanInfo.nChannelBytes = cmd->createBus.channelBytes;
-	pBusInfo->chanInfo.channelTypeGuid = cmd->createBus.busDataTypeGuid;
-	pBusInfo->chanInfo.channelInstGuid = cmd->createBus.busInstGuid;
+	pBusInfo->flags.server = inmsg->hdr.flags.server;
+	pBusInfo->chan_info.channel_addr = cmd->create_bus.channel_addr;
+	pBusInfo->chan_info.n_channel_bytes = cmd->create_bus.channel_bytes;
+	pBusInfo->chan_info.channel_type_uuid =
+			cmd->create_bus.bus_data_type_uuid;
+	pBusInfo->chan_info.channel_inst_uuid = cmd->create_bus.bus_inst_uuid;
 
 	list_add(&pBusInfo->entry, &BusInfoList);
 
@@ -1139,15 +1151,15 @@ bus_create(CONTROLVM_MESSAGE *inmsg)
 
 Away:
 	bus_epilog(busNo, CONTROLVM_BUS_CREATE, &inmsg->hdr,
-		   rc, inmsg->hdr.Flags.responseExpected == 1);
+		   rc, inmsg->hdr.flags.response_expected == 1);
 }
 
 static void
-bus_destroy(CONTROLVM_MESSAGE *inmsg)
+bus_destroy(struct controlvm_message *inmsg)
 {
-	CONTROLVM_MESSAGE_PACKET *cmd = &inmsg->cmd;
-	ulong busNo = cmd->destroyBus.busNo;
-	VISORCHIPSET_BUS_INFO *pBusInfo;
+	struct controlvm_message_packet *cmd = &inmsg->cmd;
+	ulong busNo = cmd->destroy_bus.bus_no;
+	struct visorchipset_bus_info *pBusInfo;
 	int rc = CONTROLVM_RESP_SUCCESS;
 
 	pBusInfo = findbus(&BusInfoList, busNo);
@@ -1165,19 +1177,19 @@ bus_destroy(CONTROLVM_MESSAGE *inmsg)
 
 Away:
 	bus_epilog(busNo, CONTROLVM_BUS_DESTROY, &inmsg->hdr,
-		   rc, inmsg->hdr.Flags.responseExpected == 1);
+		   rc, inmsg->hdr.flags.response_expected == 1);
 }
 
 static void
-bus_configure(CONTROLVM_MESSAGE *inmsg, PARSER_CONTEXT *parser_ctx)
+bus_configure(struct controlvm_message *inmsg, PARSER_CONTEXT *parser_ctx)
 {
-	CONTROLVM_MESSAGE_PACKET *cmd = &inmsg->cmd;
-	ulong busNo = cmd->configureBus.busNo;
-	VISORCHIPSET_BUS_INFO *pBusInfo = NULL;
+	struct controlvm_message_packet *cmd = &inmsg->cmd;
+	ulong busNo = cmd->configure_bus.bus_no;
+	struct visorchipset_bus_info *pBusInfo = NULL;
 	int rc = CONTROLVM_RESP_SUCCESS;
 	char s[99];
 
-	busNo = cmd->configureBus.busNo;
+	busNo = cmd->configure_bus.bus_no;
 	POSTCODE_LINUX_3(BUS_CONFIGURE_ENTRY_PC, busNo, POSTCODE_SEVERITY_INFO);
 
 	pBusInfo = findbus(&BusInfoList, busNo);
@@ -1198,35 +1210,35 @@ bus_configure(CONTROLVM_MESSAGE *inmsg, PARSER_CONTEXT *parser_ctx)
 		goto Away;
 	}
 	/* TBD - add this check to other commands also... */
-	if (pBusInfo->pendingMsgHdr.Id != CONTROLVM_INVALID) {
+	if (pBusInfo->pending_msg_hdr.id != CONTROLVM_INVALID) {
 		LOGERR("CONTROLVM_BUS_CONFIGURE Failed: bus %lu MsgId=%u outstanding",
-		     busNo, (uint) pBusInfo->pendingMsgHdr.Id);
+		     busNo, (uint) pBusInfo->pending_msg_hdr.id);
 		POSTCODE_LINUX_3(BUS_CONFIGURE_FAILURE_PC, busNo,
 				 POSTCODE_SEVERITY_ERR);
 		rc = -CONTROLVM_RESP_ERROR_MESSAGE_ID_INVALID_FOR_CLIENT;
 		goto Away;
 	}
 
-	pBusInfo->partitionHandle = cmd->configureBus.guestHandle;
-	pBusInfo->partitionGuid = parser_id_get(parser_ctx);
+	pBusInfo->partition_handle = cmd->configure_bus.guest_handle;
+	pBusInfo->partition_uuid = parser_id_get(parser_ctx);
 	parser_param_start(parser_ctx, PARSERSTRING_NAME);
 	pBusInfo->name = parser_string_get(parser_ctx);
 
-	visorchannel_uuid_id(&pBusInfo->partitionGuid, s);
+	visorchannel_uuid_id(&pBusInfo->partition_uuid, s);
 	POSTCODE_LINUX_3(BUS_CONFIGURE_EXIT_PC, busNo, POSTCODE_SEVERITY_INFO);
 Away:
 	bus_epilog(busNo, CONTROLVM_BUS_CONFIGURE, &inmsg->hdr,
-		   rc, inmsg->hdr.Flags.responseExpected == 1);
+		   rc, inmsg->hdr.flags.response_expected == 1);
 }
 
 static void
-my_device_create(CONTROLVM_MESSAGE *inmsg)
+my_device_create(struct controlvm_message *inmsg)
 {
-	CONTROLVM_MESSAGE_PACKET *cmd = &inmsg->cmd;
-	ulong busNo = cmd->createDevice.busNo;
-	ulong devNo = cmd->createDevice.devNo;
-	VISORCHIPSET_DEVICE_INFO *pDevInfo = NULL;
-	VISORCHIPSET_BUS_INFO *pBusInfo = NULL;
+	struct controlvm_message_packet *cmd = &inmsg->cmd;
+	ulong busNo = cmd->create_device.bus_no;
+	ulong devNo = cmd->create_device.dev_no;
+	struct visorchipset_device_info *pDevInfo = NULL;
+	struct visorchipset_bus_info *pBusInfo = NULL;
 	int rc = CONTROLVM_RESP_SUCCESS;
 
 	pDevInfo = finddevice(&DevInfoList, busNo, devNo);
@@ -1255,7 +1267,7 @@ my_device_create(CONTROLVM_MESSAGE *inmsg)
 		rc = -CONTROLVM_RESP_ERROR_BUS_INVALID;
 		goto Away;
 	}
-	pDevInfo = kzalloc(sizeof(VISORCHIPSET_DEVICE_INFO), GFP_KERNEL);
+	pDevInfo = kzalloc(sizeof(struct visorchipset_device_info), GFP_KERNEL);
 	if (pDevInfo == NULL) {
 		LOGERR("CONTROLVM_DEVICE_CREATE Failed: busNo=%lu, devNo=%lu kmaloc failed",
 		     busNo, devNo);
@@ -1266,45 +1278,47 @@ my_device_create(CONTROLVM_MESSAGE *inmsg)
 	}
 
 	INIT_LIST_HEAD(&pDevInfo->entry);
-	pDevInfo->busNo = busNo;
-	pDevInfo->devNo = devNo;
-	pDevInfo->devInstGuid = cmd->createDevice.devInstGuid;
+	pDevInfo->bus_no = busNo;
+	pDevInfo->dev_no = devNo;
+	pDevInfo->dev_inst_uuid = cmd->create_device.dev_inst_uuid;
 	POSTCODE_LINUX_4(DEVICE_CREATE_ENTRY_PC, devNo, busNo,
 			 POSTCODE_SEVERITY_INFO);
 
-	if (inmsg->hdr.Flags.testMessage == 1)
-		pDevInfo->chanInfo.addrType = ADDRTYPE_localTest;
+	if (inmsg->hdr.flags.test_message == 1)
+		pDevInfo->chan_info.addr_type = ADDRTYPE_LOCALTEST;
 	else
-		pDevInfo->chanInfo.addrType = ADDRTYPE_localPhysical;
-	pDevInfo->chanInfo.channelAddr = cmd->createDevice.channelAddr;
-	pDevInfo->chanInfo.nChannelBytes = cmd->createDevice.channelBytes;
-	pDevInfo->chanInfo.channelTypeGuid = cmd->createDevice.dataTypeGuid;
-	pDevInfo->chanInfo.intr = cmd->createDevice.intr;
+		pDevInfo->chan_info.addr_type = ADDRTYPE_LOCALPHYSICAL;
+	pDevInfo->chan_info.channel_addr = cmd->create_device.channel_addr;
+	pDevInfo->chan_info.n_channel_bytes = cmd->create_device.channel_bytes;
+	pDevInfo->chan_info.channel_type_uuid =
+			cmd->create_device.data_type_uuid;
+	pDevInfo->chan_info.intr = cmd->create_device.intr;
 	list_add(&pDevInfo->entry, &DevInfoList);
 	POSTCODE_LINUX_4(DEVICE_CREATE_EXIT_PC, devNo, busNo,
 			 POSTCODE_SEVERITY_INFO);
 Away:
 	/* get the bus and devNo for DiagPool channel */
-	if (is_diagpool_channel(pDevInfo->chanInfo.channelTypeGuid)) {
+	if (pDevInfo &&
+	    is_diagpool_channel(pDevInfo->chan_info.channel_type_uuid)) {
 		g_diagpoolBusNo = busNo;
 		g_diagpoolDevNo = devNo;
 		LOGINF("CONTROLVM_DEVICE_CREATE for DiagPool channel: busNo=%lu, devNo=%lu",
 		     g_diagpoolBusNo, g_diagpoolDevNo);
 	}
-	device_epilog(busNo, devNo, SegmentStateRunning,
+	device_epilog(busNo, devNo, segment_state_running,
 		      CONTROLVM_DEVICE_CREATE, &inmsg->hdr, rc,
-		      inmsg->hdr.Flags.responseExpected == 1,
-		      FOR_VISORBUS(pDevInfo->chanInfo.channelTypeGuid));
+		      inmsg->hdr.flags.response_expected == 1,
+		      FOR_VISORBUS(pDevInfo->chan_info.channel_type_uuid));
 }
 
 static void
-my_device_changestate(CONTROLVM_MESSAGE *inmsg)
+my_device_changestate(struct controlvm_message *inmsg)
 {
-	CONTROLVM_MESSAGE_PACKET *cmd = &inmsg->cmd;
-	ulong busNo = cmd->deviceChangeState.busNo;
-	ulong devNo = cmd->deviceChangeState.devNo;
-	ULTRA_SEGMENT_STATE state = cmd->deviceChangeState.state;
-	VISORCHIPSET_DEVICE_INFO *pDevInfo = NULL;
+	struct controlvm_message_packet *cmd = &inmsg->cmd;
+	ulong busNo = cmd->device_change_state.bus_no;
+	ulong devNo = cmd->device_change_state.dev_no;
+	struct spar_segment_state state = cmd->device_change_state.state;
+	struct visorchipset_device_info *pDevInfo = NULL;
 	int rc = CONTROLVM_RESP_SUCCESS;
 
 	pDevInfo = finddevice(&DevInfoList, busNo, devNo);
@@ -1327,17 +1341,18 @@ Away:
 	if ((rc >= CONTROLVM_RESP_SUCCESS) && pDevInfo)
 		device_epilog(busNo, devNo, state, CONTROLVM_DEVICE_CHANGESTATE,
 			      &inmsg->hdr, rc,
-			      inmsg->hdr.Flags.responseExpected == 1,
-			      FOR_VISORBUS(pDevInfo->chanInfo.channelTypeGuid));
+			      inmsg->hdr.flags.response_expected == 1,
+			      FOR_VISORBUS(
+					pDevInfo->chan_info.channel_type_uuid));
 }
 
 static void
-my_device_destroy(CONTROLVM_MESSAGE *inmsg)
+my_device_destroy(struct controlvm_message *inmsg)
 {
-	CONTROLVM_MESSAGE_PACKET *cmd = &inmsg->cmd;
-	ulong busNo = cmd->destroyDevice.busNo;
-	ulong devNo = cmd->destroyDevice.devNo;
-	VISORCHIPSET_DEVICE_INFO *pDevInfo = NULL;
+	struct controlvm_message_packet *cmd = &inmsg->cmd;
+	ulong busNo = cmd->destroy_device.bus_no;
+	ulong devNo = cmd->destroy_device.dev_no;
+	struct visorchipset_device_info *pDevInfo = NULL;
 	int rc = CONTROLVM_RESP_SUCCESS;
 
 	pDevInfo = finddevice(&DevInfoList, busNo, devNo);
@@ -1355,10 +1370,11 @@ my_device_destroy(CONTROLVM_MESSAGE *inmsg)
 
 Away:
 	if ((rc >= CONTROLVM_RESP_SUCCESS) && pDevInfo)
-		device_epilog(busNo, devNo, SegmentStateRunning,
+		device_epilog(busNo, devNo, segment_state_running,
 			      CONTROLVM_DEVICE_DESTROY, &inmsg->hdr, rc,
-			      inmsg->hdr.Flags.responseExpected == 1,
-			      FOR_VISORBUS(pDevInfo->chanInfo.channelTypeGuid));
+			      inmsg->hdr.flags.response_expected == 1,
+			      FOR_VISORBUS(
+					pDevInfo->chan_info.channel_type_uuid));
 }
 
 /* When provided with the physical address of the controlvm channel
@@ -1382,7 +1398,7 @@ initialize_controlvm_payload_info(HOSTADDRESS phys_addr, u64 offset, u32 bytes,
 	}
 	memset(info, 0, sizeof(CONTROLVM_PAYLOAD_INFO));
 	if ((offset == 0) || (bytes == 0)) {
-		LOGERR("CONTROLVM_PAYLOAD_INIT Failed: RequestPayloadOffset=%llu RequestPayloadBytes=%llu!",
+		LOGERR("CONTROLVM_PAYLOAD_INIT Failed: request_payload_offset=%llu request_payload_bytes=%llu!",
 		     (u64) offset, (u64) bytes);
 		rc = -CONTROLVM_RESP_ERROR_PAYLOAD_INVALID;
 		goto Away;
@@ -1429,8 +1445,8 @@ initialize_controlvm_payload(void)
 	u32 payloadBytes = 0;
 
 	if (visorchannel_read(ControlVm_channel,
-			      offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL,
-				       RequestPayloadOffset),
+			      offsetof(struct spar_controlvm_channel_protocol,
+				       request_payload_offset),
 			      &payloadOffset, sizeof(payloadOffset)) < 0) {
 		LOGERR("CONTROLVM_PAYLOAD_INIT Failed to read controlvm channel!");
 		POSTCODE_LINUX_2(CONTROLVM_INIT_FAILURE_PC,
@@ -1438,8 +1454,8 @@ initialize_controlvm_payload(void)
 		return;
 	}
 	if (visorchannel_read(ControlVm_channel,
-			      offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL,
-				       RequestPayloadBytes),
+			      offsetof(struct spar_controlvm_channel_protocol,
+				       request_payload_bytes),
 			      &payloadBytes, sizeof(payloadBytes)) < 0) {
 		LOGERR("CONTROLVM_PAYLOAD_INIT Failed to read controlvm channel!");
 		POSTCODE_LINUX_2(CONTROLVM_INIT_FAILURE_PC,
@@ -1487,15 +1503,15 @@ visorchipset_chipset_notready(void)
 EXPORT_SYMBOL_GPL(visorchipset_chipset_notready);
 
 static void
-chipset_ready(CONTROLVM_MESSAGE_HEADER *msgHdr)
+chipset_ready(struct controlvm_message_header *msgHdr)
 {
 	int rc = visorchipset_chipset_ready();
 
 	if (rc != CONTROLVM_RESP_SUCCESS)
 		rc = -rc;
-	if (msgHdr->Flags.responseExpected && !visorchipset_holdchipsetready)
+	if (msgHdr->flags.response_expected && !visorchipset_holdchipsetready)
 		controlvm_respond(msgHdr, rc);
-	if (msgHdr->Flags.responseExpected && visorchipset_holdchipsetready) {
+	if (msgHdr->flags.response_expected && visorchipset_holdchipsetready) {
 		/* Send CHIPSET_READY response when all modules have been loaded
 		 * and disks mounted for the partition
 		 */
@@ -1505,24 +1521,24 @@ chipset_ready(CONTROLVM_MESSAGE_HEADER *msgHdr)
 }
 
 static void
-chipset_selftest(CONTROLVM_MESSAGE_HEADER *msgHdr)
+chipset_selftest(struct controlvm_message_header *msgHdr)
 {
 	int rc = visorchipset_chipset_selftest();
 
 	if (rc != CONTROLVM_RESP_SUCCESS)
 		rc = -rc;
-	if (msgHdr->Flags.responseExpected)
+	if (msgHdr->flags.response_expected)
 		controlvm_respond(msgHdr, rc);
 }
 
 static void
-chipset_notready(CONTROLVM_MESSAGE_HEADER *msgHdr)
+chipset_notready(struct controlvm_message_header *msgHdr)
 {
 	int rc = visorchipset_chipset_notready();
 
 	if (rc != CONTROLVM_RESP_SUCCESS)
 		rc = -rc;
-	if (msgHdr->Flags.responseExpected)
+	if (msgHdr->flags.response_expected)
 		controlvm_respond(msgHdr, rc);
 }
 
@@ -1530,13 +1546,14 @@ chipset_notready(CONTROLVM_MESSAGE_HEADER *msgHdr)
  * CONTROLVM_QUEUE_EVENT queue in the controlvm channel.
  */
 static BOOL
-read_controlvm_event(CONTROLVM_MESSAGE *msg)
+read_controlvm_event(struct controlvm_message *msg)
 {
 	if (visorchannel_signalremove(ControlVm_channel,
 				      CONTROLVM_QUEUE_EVENT, msg)) {
 		/* got a message */
-		if (msg->hdr.Flags.testMessage == 1) {
-			LOGERR("ignoring bad CONTROLVM_QUEUE_EVENT msg with controlvm_msg_id=0x%x because Flags.testMessage is nonsensical (=1)", msg->hdr.Id);
+		if (msg->hdr.flags.test_message == 1) {
+			LOGERR("ignoring bad CONTROLVM_QUEUE_EVENT msg with controlvm_msg_id=0x%x because Flags.testMessage is nonsensical (=1)",
+			       msg->hdr.id);
 			return FALSE;
 		}
 		return TRUE;
@@ -1586,7 +1603,7 @@ parahotplug_next_expiration(void)
  * CONTROLVM_MESSAGE that we can stick on a list
  */
 static struct parahotplug_request *
-parahotplug_request_create(CONTROLVM_MESSAGE *msg)
+parahotplug_request_create(struct controlvm_message *msg)
 {
 	struct parahotplug_request *req =
 	    kmalloc(sizeof(struct parahotplug_request),
@@ -1618,7 +1635,7 @@ parahotplug_request_destroy(struct parahotplug_request *req)
 static void
 parahotplug_request_kickoff(struct parahotplug_request *req)
 {
-	CONTROLVM_MESSAGE_PACKET *cmd = &req->msg.cmd;
+	struct controlvm_message_packet *cmd = &req->msg.cmd;
 	char env_cmd[40], env_id[40], env_state[40], env_bus[40], env_dev[40],
 	    env_func[40];
 	char *envp[] = {
@@ -1628,18 +1645,19 @@ parahotplug_request_kickoff(struct parahotplug_request *req)
 	sprintf(env_cmd, "SPAR_PARAHOTPLUG=1");
 	sprintf(env_id, "SPAR_PARAHOTPLUG_ID=%d", req->id);
 	sprintf(env_state, "SPAR_PARAHOTPLUG_STATE=%d",
-		cmd->deviceChangeState.state.Active);
+		cmd->device_change_state.state.active);
 	sprintf(env_bus, "SPAR_PARAHOTPLUG_BUS=%d",
-		cmd->deviceChangeState.busNo);
+		cmd->device_change_state.bus_no);
 	sprintf(env_dev, "SPAR_PARAHOTPLUG_DEVICE=%d",
-		cmd->deviceChangeState.devNo >> 3);
+		cmd->device_change_state.dev_no >> 3);
 	sprintf(env_func, "SPAR_PARAHOTPLUG_FUNCTION=%d",
-		cmd->deviceChangeState.devNo & 0x7);
+		cmd->device_change_state.dev_no & 0x7);
 
 	LOGINF("parahotplug_request_kickoff: state=%d, bdf=%d/%d/%d, id=%u\n",
-	       cmd->deviceChangeState.state.Active,
-	       cmd->deviceChangeState.busNo, cmd->deviceChangeState.devNo >> 3,
-	       cmd->deviceChangeState.devNo & 7, req->id);
+	       cmd->device_change_state.state.active,
+	       cmd->device_change_state.bus_no,
+	       cmd->device_change_state.dev_no >> 3,
+	       cmd->device_change_state.dev_no & 7, req->id);
 
 	kobject_uevent_env(&Visorchipset_platform_device.dev.kobj, KOBJ_CHANGE,
 			   envp);
@@ -1662,11 +1680,11 @@ parahotplug_process_list(void)
 		    list_entry(pos, struct parahotplug_request, list);
 		if (time_after_eq(jiffies, req->expiration)) {
 			list_del(pos);
-			if (req->msg.hdr.Flags.responseExpected)
+			if (req->msg.hdr.flags.response_expected)
 				controlvm_respond_physdev_changestate(
 					&req->msg.hdr,
 					CONTROLVM_RESP_ERROR_DEVICE_UDEV_TIMEOUT,
-					req->msg.cmd.deviceChangeState.state);
+					req->msg.cmd.device_change_state.state);
 			parahotplug_request_destroy(req);
 		}
 	}
@@ -1697,11 +1715,11 @@ parahotplug_request_complete(int id, u16 active)
 			 */
 			list_del(pos);
 			spin_unlock(&Parahotplug_request_list_lock);
-			req->msg.cmd.deviceChangeState.state.Active = active;
-			if (req->msg.hdr.Flags.responseExpected)
+			req->msg.cmd.device_change_state.state.active = active;
+			if (req->msg.hdr.flags.response_expected)
 				controlvm_respond_physdev_changestate(
 					&req->msg.hdr, CONTROLVM_RESP_SUCCESS,
-					req->msg.cmd.deviceChangeState.state);
+					req->msg.cmd.device_change_state.state);
 			parahotplug_request_destroy(req);
 			return 0;
 		}
@@ -1715,7 +1733,7 @@ parahotplug_request_complete(int id, u16 active)
  * Enables or disables a PCI device by kicking off a udev script
  */
 static void
-parahotplug_process_message(CONTROLVM_MESSAGE *inmsg)
+parahotplug_process_message(struct controlvm_message *inmsg)
 {
 	struct parahotplug_request *req;
 
@@ -1726,7 +1744,7 @@ parahotplug_process_message(CONTROLVM_MESSAGE *inmsg)
 		return;
 	}
 
-	if (inmsg->cmd.deviceChangeState.state.Active) {
+	if (inmsg->cmd.device_change_state.state.active) {
 		/* For enable messages, just respond with success
 		* right away.  This is a bit of a hack, but there are
 		* issues with the early enable messages we get (with
@@ -1738,9 +1756,8 @@ parahotplug_process_message(CONTROLVM_MESSAGE *inmsg)
 		*/
 		parahotplug_request_kickoff(req);
 		controlvm_respond_physdev_changestate(&inmsg->hdr,
-						      CONTROLVM_RESP_SUCCESS,
-						      inmsg->cmd.
-						      deviceChangeState.state);
+				CONTROLVM_RESP_SUCCESS, inmsg->cmd.
+				device_change_state.state);
 		parahotplug_request_destroy(req);
 	} else {
 		/* For disable messages, add the request to the
@@ -1768,23 +1785,23 @@ parahotplug_process_message(CONTROLVM_MESSAGE *inmsg)
  *            either successfully or with an error.
  */
 static BOOL
-handle_command(CONTROLVM_MESSAGE inmsg, HOSTADDRESS channel_addr)
+handle_command(struct controlvm_message inmsg, HOSTADDRESS channel_addr)
 {
-	CONTROLVM_MESSAGE_PACKET *cmd = &inmsg.cmd;
+	struct controlvm_message_packet *cmd = &inmsg.cmd;
 	u64 parametersAddr = 0;
 	u32 parametersBytes = 0;
 	PARSER_CONTEXT *parser_ctx = NULL;
 	BOOL isLocalAddr = FALSE;
-	CONTROLVM_MESSAGE ackmsg;
+	struct controlvm_message ackmsg;
 
 	/* create parsing context if necessary */
-	isLocalAddr = (inmsg.hdr.Flags.testMessage == 1);
+	isLocalAddr = (inmsg.hdr.flags.test_message == 1);
 	if (channel_addr == 0) {
 		LOGERR("HUH? channel_addr is 0!");
 		return TRUE;
 	}
-	parametersAddr = channel_addr + inmsg.hdr.PayloadVmOffset;
-	parametersBytes = inmsg.hdr.PayloadBytes;
+	parametersAddr = channel_addr + inmsg.hdr.payload_vm_offset;
+	parametersBytes = inmsg.hdr.payload_bytes;
 
 	/* Parameter and channel addresses within test messages actually lie
 	 * within our OS-controlled memory.  We need to know that, because it
@@ -1802,7 +1819,7 @@ handle_command(CONTROLVM_MESSAGE inmsg, HOSTADDRESS channel_addr)
 				return FALSE;
 			}
 			LOGWRN("parsing failed");
-			LOGWRN("inmsg.hdr.Id=0x%lx", (ulong) inmsg.hdr.Id);
+			LOGWRN("inmsg.hdr.Id=0x%lx", (ulong) inmsg.hdr.id);
 			LOGWRN("parametersAddr=0x%llx", (u64) parametersAddr);
 			LOGWRN("parametersBytes=%lu", (ulong) parametersBytes);
 			LOGWRN("isLocalAddr=%d", isLocalAddr);
@@ -1818,45 +1835,45 @@ handle_command(CONTROLVM_MESSAGE inmsg, HOSTADDRESS channel_addr)
 		     (ControlVm_channel, CONTROLVM_QUEUE_ACK, &ackmsg)))
 			LOGWRN("failed to send ACK failed");
 	}
-	switch (inmsg.hdr.Id) {
+	switch (inmsg.hdr.id) {
 	case CONTROLVM_CHIPSET_INIT:
 		LOGINF("CHIPSET_INIT(#busses=%lu,#switches=%lu)",
-		       (ulong) inmsg.cmd.initChipset.busCount,
-		       (ulong) inmsg.cmd.initChipset.switchCount);
+		       (ulong) inmsg.cmd.init_chipset.bus_count,
+		       (ulong) inmsg.cmd.init_chipset.switch_count);
 		chipset_init(&inmsg);
 		break;
 	case CONTROLVM_BUS_CREATE:
 		LOGINF("BUS_CREATE(%lu,#devs=%lu)",
-		       (ulong) cmd->createBus.busNo,
-		       (ulong) cmd->createBus.deviceCount);
+		       (ulong) cmd->create_bus.bus_no,
+		       (ulong) cmd->create_bus.dev_count);
 		bus_create(&inmsg);
 		break;
 	case CONTROLVM_BUS_DESTROY:
-		LOGINF("BUS_DESTROY(%lu)", (ulong) cmd->destroyBus.busNo);
+		LOGINF("BUS_DESTROY(%lu)", (ulong) cmd->destroy_bus.bus_no);
 		bus_destroy(&inmsg);
 		break;
 	case CONTROLVM_BUS_CONFIGURE:
-		LOGINF("BUS_CONFIGURE(%lu)", (ulong) cmd->configureBus.busNo);
+		LOGINF("BUS_CONFIGURE(%lu)", (ulong) cmd->configure_bus.bus_no);
 		bus_configure(&inmsg, parser_ctx);
 		break;
 	case CONTROLVM_DEVICE_CREATE:
 		LOGINF("DEVICE_CREATE(%lu,%lu)",
-		       (ulong) cmd->createDevice.busNo,
-		       (ulong) cmd->createDevice.devNo);
+		       (ulong) cmd->create_device.bus_no,
+		       (ulong) cmd->create_device.dev_no);
 		my_device_create(&inmsg);
 		break;
 	case CONTROLVM_DEVICE_CHANGESTATE:
-		if (cmd->deviceChangeState.flags.physicalDevice) {
+		if (cmd->device_change_state.flags.phys_device) {
 			LOGINF("DEVICE_CHANGESTATE for physical device (%lu,%lu, active=%lu)",
-			     (ulong) cmd->deviceChangeState.busNo,
-			     (ulong) cmd->deviceChangeState.devNo,
-			     (ulong) cmd->deviceChangeState.state.Active);
+			     (ulong) cmd->device_change_state.bus_no,
+			     (ulong) cmd->device_change_state.dev_no,
+			     (ulong) cmd->device_change_state.state.active);
 			parahotplug_process_message(&inmsg);
 		} else {
 			LOGINF("DEVICE_CHANGESTATE for virtual device (%lu,%lu, state.Alive=0x%lx)",
-			     (ulong) cmd->deviceChangeState.busNo,
-			     (ulong) cmd->deviceChangeState.devNo,
-			     (ulong) cmd->deviceChangeState.state.Alive);
+			     (ulong) cmd->device_change_state.bus_no,
+			     (ulong) cmd->device_change_state.dev_no,
+			     (ulong) cmd->device_change_state.state.alive);
 			/* save the hdr and cmd structures for later use */
 			/* when sending back the response to Command */
 			my_device_changestate(&inmsg);
@@ -1867,16 +1884,16 @@ handle_command(CONTROLVM_MESSAGE inmsg, HOSTADDRESS channel_addr)
 		break;
 	case CONTROLVM_DEVICE_DESTROY:
 		LOGINF("DEVICE_DESTROY(%lu,%lu)",
-		       (ulong) cmd->destroyDevice.busNo,
-		       (ulong) cmd->destroyDevice.devNo);
+		       (ulong) cmd->destroy_device.bus_no,
+		       (ulong) cmd->destroy_device.dev_no);
 		my_device_destroy(&inmsg);
 		break;
 	case CONTROLVM_DEVICE_CONFIGURE:
 		LOGINF("DEVICE_CONFIGURE(%lu,%lu)",
-		       (ulong) cmd->configureDevice.busNo,
-		       (ulong) cmd->configureDevice.devNo);
+		       (ulong) cmd->configure_device.bus_no,
+		       (ulong) cmd->configure_device.dev_no);
 		/* no op for now, just send a respond that we passed */
-		if (inmsg.hdr.Flags.responseExpected)
+		if (inmsg.hdr.flags.response_expected)
 			controlvm_respond(&inmsg.hdr, CONTROLVM_RESP_SUCCESS);
 		break;
 	case CONTROLVM_CHIPSET_READY:
@@ -1892,8 +1909,8 @@ handle_command(CONTROLVM_MESSAGE inmsg, HOSTADDRESS channel_addr)
 		chipset_notready(&inmsg.hdr);
 		break;
 	default:
-		LOGERR("unrecognized controlvm cmd=%d", (int) inmsg.hdr.Id);
-		if (inmsg.hdr.Flags.responseExpected)
+		LOGERR("unrecognized controlvm cmd=%d", (int) inmsg.hdr.id);
+		if (inmsg.hdr.flags.response_expected)
 			controlvm_respond(&inmsg.hdr,
 					  -CONTROLVM_RESP_ERROR_MESSAGE_ID_UNKNOWN);
 		break;
@@ -1923,8 +1940,7 @@ static HOSTADDRESS controlvm_get_channel_address(void)
 static void
 controlvm_periodic_work(struct work_struct *work)
 {
-	VISORCHIPSET_CHANNEL_INFO chanInfo;
-	CONTROLVM_MESSAGE inmsg;
+	struct controlvm_message inmsg;
 	BOOL gotACommand = FALSE;
 	BOOL handle_command_failed = FALSE;
 	static u64 Poll_Count;
@@ -1938,8 +1954,6 @@ controlvm_periodic_work(struct work_struct *work)
 	if (visorchipset_clientregwait && !clientregistered)
 		goto Away;
 
-	memset(&chanInfo, 0, sizeof(VISORCHIPSET_CHANNEL_INFO));
-
 	Poll_Count++;
 	if (Poll_Count >= 250)
 		;	/* keep going */
@@ -1950,24 +1964,24 @@ controlvm_periodic_work(struct work_struct *work)
 	 * should be sent
 	 */
 	if (visorchipset_holdchipsetready
-	    && (g_ChipSetMsgHdr.Id != CONTROLVM_INVALID)) {
+	    && (g_ChipSetMsgHdr.id != CONTROLVM_INVALID)) {
 		if (check_chipset_events() == 1) {
 			LOGINF("Sending CHIPSET_READY response");
 			controlvm_respond(&g_ChipSetMsgHdr, 0);
 			clear_chipset_events();
 			memset(&g_ChipSetMsgHdr, 0,
-			       sizeof(CONTROLVM_MESSAGE_HEADER));
+			       sizeof(struct controlvm_message_header));
 		}
 	}
 
 	while (visorchannel_signalremove(ControlVm_channel,
 					 CONTROLVM_QUEUE_RESPONSE,
 					 &inmsg)) {
-		if (inmsg.hdr.PayloadMaxBytes != 0) {
+		if (inmsg.hdr.payload_max_bytes != 0) {
 			LOGERR("Payload of size %lu returned @%lu with unexpected message id %d.",
-			     (ulong) inmsg.hdr.PayloadMaxBytes,
-			     (ulong) inmsg.hdr.PayloadVmOffset,
-			     inmsg.hdr.Id);
+			     (ulong) inmsg.hdr.payload_max_bytes,
+			     (ulong) inmsg.hdr.payload_vm_offset,
+			     inmsg.hdr.id);
 		}
 	}
 	if (!gotACommand) {
@@ -2033,9 +2047,9 @@ static void
 setup_crash_devices_work_queue(struct work_struct *work)
 {
 
-	CONTROLVM_MESSAGE localCrashCreateBusMsg;
-	CONTROLVM_MESSAGE localCrashCreateDevMsg;
-	CONTROLVM_MESSAGE msg;
+	struct controlvm_message localCrashCreateBusMsg;
+	struct controlvm_message localCrashCreateDevMsg;
+	struct controlvm_message msg;
 	u32 localSavedCrashMsgOffset;
 	u16 localSavedCrashMsgCount;
 
@@ -2052,16 +2066,16 @@ setup_crash_devices_work_queue(struct work_struct *work)
 	POSTCODE_LINUX_2(CRASH_DEV_ENTRY_PC, POSTCODE_SEVERITY_INFO);
 
 	/* send init chipset msg */
-	msg.hdr.Id = CONTROLVM_CHIPSET_INIT;
-	msg.cmd.initChipset.busCount = 23;
-	msg.cmd.initChipset.switchCount = 0;
+	msg.hdr.id = CONTROLVM_CHIPSET_INIT;
+	msg.cmd.init_chipset.bus_count = 23;
+	msg.cmd.init_chipset.switch_count = 0;
 
 	chipset_init(&msg);
 
 	/* get saved message count */
 	if (visorchannel_read(ControlVm_channel,
-			      offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL,
-				       SavedCrashMsgCount),
+			      offsetof(struct spar_controlvm_channel_protocol,
+				       saved_crash_message_count),
 			      &localSavedCrashMsgCount, sizeof(u16)) < 0) {
 		LOGERR("failed to get Saved Message Count");
 		POSTCODE_LINUX_2(CRASH_DEV_CTRL_RD_FAILURE_PC,
@@ -2080,8 +2094,8 @@ setup_crash_devices_work_queue(struct work_struct *work)
 
 	/* get saved crash message offset */
 	if (visorchannel_read(ControlVm_channel,
-			      offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL,
-				       SavedCrashMsgOffset),
+			      offsetof(struct spar_controlvm_channel_protocol,
+				       saved_crash_message_offset),
 			      &localSavedCrashMsgOffset, sizeof(u32)) < 0) {
 		LOGERR("failed to get Saved Message Offset");
 		POSTCODE_LINUX_2(CRASH_DEV_CTRL_RD_FAILURE_PC,
@@ -2093,7 +2107,7 @@ setup_crash_devices_work_queue(struct work_struct *work)
 	if (visorchannel_read(ControlVm_channel,
 			      localSavedCrashMsgOffset,
 			      &localCrashCreateBusMsg,
-			      sizeof(CONTROLVM_MESSAGE)) < 0) {
+			      sizeof(struct controlvm_message)) < 0) {
 		LOGERR("CRASH_DEV_RD_BUS_FAIULRE: Failed to read CrashCreateBusMsg!");
 		POSTCODE_LINUX_2(CRASH_DEV_RD_BUS_FAIULRE_PC,
 				 POSTCODE_SEVERITY_ERR);
@@ -2103,9 +2117,9 @@ setup_crash_devices_work_queue(struct work_struct *work)
 	/* read create device message for storage device */
 	if (visorchannel_read(ControlVm_channel,
 			      localSavedCrashMsgOffset +
-			      sizeof(CONTROLVM_MESSAGE),
+			      sizeof(struct controlvm_message),
 			      &localCrashCreateDevMsg,
-			      sizeof(CONTROLVM_MESSAGE)) < 0) {
+			      sizeof(struct controlvm_message)) < 0) {
 		LOGERR("CRASH_DEV_RD_DEV_FAIULRE: Failed to read CrashCreateDevMsg!");
 		POSTCODE_LINUX_2(CRASH_DEV_RD_DEV_FAIULRE_PC,
 				 POSTCODE_SEVERITY_ERR);
@@ -2113,7 +2127,7 @@ setup_crash_devices_work_queue(struct work_struct *work)
 	}
 
 	/* reuse IOVM create bus message */
-	if (localCrashCreateBusMsg.cmd.createBus.channelAddr != 0)
+	if (localCrashCreateBusMsg.cmd.create_bus.channel_addr != 0)
 		bus_create(&localCrashCreateBusMsg);
 	else {
 		LOGERR("CrashCreateBusMsg is null, no dump will be taken");
@@ -2123,7 +2137,7 @@ setup_crash_devices_work_queue(struct work_struct *work)
 	}
 
 	/* reuse create device message for storage device */
-	if (localCrashCreateDevMsg.cmd.createDevice.channelAddr != 0)
+	if (localCrashCreateDevMsg.cmd.create_device.channel_addr != 0)
 		my_device_create(&localCrashCreateDevMsg);
 	else {
 		LOGERR("CrashCreateDevMsg is null, no dump will be taken");
@@ -2168,12 +2182,12 @@ device_destroy_response(ulong busNo, ulong devNo, int response)
 }
 
 void
-visorchipset_device_pause_response(ulong busNo, ulong devNo, int response)
+visorchipset_device_pause_response(ulong bus_no, ulong dev_no, int response)
 {
 
 	device_changestate_responder(CONTROLVM_DEVICE_CHANGESTATE,
-				     busNo, devNo, response,
-				     SegmentStateStandby);
+				     bus_no, dev_no, response,
+				     segment_state_standby);
 }
 EXPORT_SYMBOL_GPL(visorchipset_device_pause_response);
 
@@ -2182,30 +2196,30 @@ device_resume_response(ulong busNo, ulong devNo, int response)
 {
 	device_changestate_responder(CONTROLVM_DEVICE_CHANGESTATE,
 				     busNo, devNo, response,
-				     SegmentStateRunning);
+				     segment_state_running);
 }
 
 BOOL
-visorchipset_get_bus_info(ulong busNo, VISORCHIPSET_BUS_INFO *busInfo)
+visorchipset_get_bus_info(ulong bus_no, struct visorchipset_bus_info *bus_info)
 {
-	void *p = findbus(&BusInfoList, busNo);
+	void *p = findbus(&BusInfoList, bus_no);
 
 	if (!p) {
-		LOGERR("(%lu) failed", busNo);
+		LOGERR("(%lu) failed", bus_no);
 		return FALSE;
 	}
-	memcpy(busInfo, p, sizeof(VISORCHIPSET_BUS_INFO));
+	memcpy(bus_info, p, sizeof(struct visorchipset_bus_info));
 	return TRUE;
 }
 EXPORT_SYMBOL_GPL(visorchipset_get_bus_info);
 
 BOOL
-visorchipset_set_bus_context(ulong busNo, void *context)
+visorchipset_set_bus_context(ulong bus_no, void *context)
 {
-	VISORCHIPSET_BUS_INFO *p = findbus(&BusInfoList, busNo);
+	struct visorchipset_bus_info *p = findbus(&BusInfoList, bus_no);
 
 	if (!p) {
-		LOGERR("(%lu) failed", busNo);
+		LOGERR("(%lu) failed", bus_no);
 		return FALSE;
 	}
 	p->bus_driver_context = context;
@@ -2214,27 +2228,28 @@ visorchipset_set_bus_context(ulong busNo, void *context)
 EXPORT_SYMBOL_GPL(visorchipset_set_bus_context);
 
 BOOL
-visorchipset_get_device_info(ulong busNo, ulong devNo,
-			     VISORCHIPSET_DEVICE_INFO *devInfo)
+visorchipset_get_device_info(ulong bus_no, ulong dev_no,
+			     struct visorchipset_device_info *dev_info)
 {
-	void *p = finddevice(&DevInfoList, busNo, devNo);
+	void *p = finddevice(&DevInfoList, bus_no, dev_no);
 
 	if (!p) {
-		LOGERR("(%lu,%lu) failed", busNo, devNo);
+		LOGERR("(%lu,%lu) failed", bus_no, dev_no);
 		return FALSE;
 	}
-	memcpy(devInfo, p, sizeof(VISORCHIPSET_DEVICE_INFO));
+	memcpy(dev_info, p, sizeof(struct visorchipset_device_info));
 	return TRUE;
 }
 EXPORT_SYMBOL_GPL(visorchipset_get_device_info);
 
 BOOL
-visorchipset_set_device_context(ulong busNo, ulong devNo, void *context)
+visorchipset_set_device_context(ulong bus_no, ulong dev_no, void *context)
 {
-	VISORCHIPSET_DEVICE_INFO *p = finddevice(&DevInfoList, busNo, devNo);
+	struct visorchipset_device_info *p =
+			finddevice(&DevInfoList, bus_no, dev_no);
 
 	if (!p) {
-		LOGERR("(%lu,%lu) failed", busNo, devNo);
+		LOGERR("(%lu,%lu) failed", bus_no, dev_no);
 		return FALSE;
 	}
 	p->bus_driver_context = context;
@@ -2377,11 +2392,10 @@ visorchipset_init(void)
 		ControlVm_channel =
 		    visorchannel_create_with_lock
 		    (addr,
-		     sizeof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL),
-		     UltraControlvmChannelProtocolGuid);
-		if (ULTRA_CONTROLVM_CHANNEL_OK_CLIENT
-			 (visorchannel_get_header(ControlVm_channel),
-			  NULL)) {
+		     sizeof(struct spar_controlvm_channel_protocol),
+		     spar_controlvm_channel_protocol_uuid);
+		if (SPAR_CONTROLVM_CHANNEL_OK_CLIENT(
+				visorchannel_get_header(ControlVm_channel))) {
 			LOGINF("Channel %s (ControlVm) discovered",
 			       visorchannel_id(ControlVm_channel, s));
 			initialize_controlvm_payload();
@@ -2404,11 +2418,11 @@ visorchipset_init(void)
 		goto Away;
 	}
 
-	memset(&g_DiagMsgHdr, 0, sizeof(CONTROLVM_MESSAGE_HEADER));
+	memset(&g_DiagMsgHdr, 0, sizeof(struct controlvm_message_header));
 
-	memset(&g_ChipSetMsgHdr, 0, sizeof(CONTROLVM_MESSAGE_HEADER));
+	memset(&g_ChipSetMsgHdr, 0, sizeof(struct controlvm_message_header));
 
-	memset(&g_DelDumpMsgHdr, 0, sizeof(CONTROLVM_MESSAGE_HEADER));
+	memset(&g_DelDumpMsgHdr, 0, sizeof(struct controlvm_message_header));
 
 	Putfile_buffer_list_pool =
 	    kmem_cache_create(Putfile_buffer_list_pool_name,
@@ -2497,11 +2511,11 @@ visorchipset_exit(void)
 
 	cleanup_controlvm_structures();
 
-	memset(&g_DiagMsgHdr, 0, sizeof(CONTROLVM_MESSAGE_HEADER));
+	memset(&g_DiagMsgHdr, 0, sizeof(struct controlvm_message_header));
 
-	memset(&g_ChipSetMsgHdr, 0, sizeof(CONTROLVM_MESSAGE_HEADER));
+	memset(&g_ChipSetMsgHdr, 0, sizeof(struct controlvm_message_header));
 
-	memset(&g_DelDumpMsgHdr, 0, sizeof(CONTROLVM_MESSAGE_HEADER));
+	memset(&g_DelDumpMsgHdr, 0, sizeof(struct controlvm_message_header));
 
 	LOGINF("Channel %s (ControlVm) disconnected",
 	       visorchannel_id(ControlVm_channel, s));
diff --git a/drivers/staging/unisys/visorutil/charqueue.c b/drivers/staging/unisys/visorutil/charqueue.c
index 22241c7b4f7f..1ce7003c3a90 100644
--- a/drivers/staging/unisys/visorutil/charqueue.c
+++ b/drivers/staging/unisys/visorutil/charqueue.c
@@ -25,9 +25,7 @@
 
 #define IS_EMPTY(charqueue) (charqueue->head == charqueue->tail)
 
-
-
-struct CHARQUEUE_Tag {
+struct charqueue {
 	int alloc_size;
 	int nslots;
 	spinlock_t lock;
@@ -35,12 +33,10 @@ struct CHARQUEUE_Tag {
 	unsigned char buf[0];
 };
 
-
-
-CHARQUEUE *visor_charqueue_create(ulong nslots)
+struct charqueue *visor_charqueue_create(ulong nslots)
 {
-	int alloc_size = sizeof(CHARQUEUE) + nslots + 1;
-	CHARQUEUE *cq = kmalloc(alloc_size, GFP_KERNEL|__GFP_NORETRY);
+	int alloc_size = sizeof(struct charqueue) + nslots + 1;
+	struct charqueue *cq = kmalloc(alloc_size, GFP_KERNEL|__GFP_NORETRY);
 
 	if (cq == NULL) {
 		ERRDRV("visor_charqueue_create allocation failed (alloc_size=%d)",
@@ -49,15 +45,14 @@ CHARQUEUE *visor_charqueue_create(ulong nslots)
 	}
 	cq->alloc_size = alloc_size;
 	cq->nslots = nslots;
-	cq->head = cq->tail = 0;
+	cq->head = 0;
+	cq->tail = 0;
 	spin_lock_init(&cq->lock);
 	return cq;
 }
 EXPORT_SYMBOL_GPL(visor_charqueue_create);
 
-
-
-void visor_charqueue_enqueue(CHARQUEUE *charqueue, unsigned char c)
+void visor_charqueue_enqueue(struct charqueue *charqueue, unsigned char c)
 {
 	int alloc_slots = charqueue->nslots+1;  /* 1 slot is always empty */
 
@@ -71,9 +66,7 @@ void visor_charqueue_enqueue(CHARQUEUE *charqueue, unsigned char c)
 }
 EXPORT_SYMBOL_GPL(visor_charqueue_enqueue);
 
-
-
-BOOL visor_charqueue_is_empty(CHARQUEUE *charqueue)
+BOOL visor_charqueue_is_empty(struct charqueue *charqueue)
 {
 	BOOL b;
 
@@ -84,9 +77,7 @@ BOOL visor_charqueue_is_empty(CHARQUEUE *charqueue)
 }
 EXPORT_SYMBOL_GPL(visor_charqueue_is_empty);
 
-
-
-static int charqueue_dequeue_1(CHARQUEUE *charqueue)
+static int charqueue_dequeue_1(struct charqueue *charqueue)
 {
 	int alloc_slots = charqueue->nslots + 1;  /* 1 slot is always empty */
 
@@ -96,9 +87,7 @@ static int charqueue_dequeue_1(CHARQUEUE *charqueue)
 	return charqueue->buf[charqueue->tail];
 }
 
-
-
-int charqueue_dequeue(CHARQUEUE *charqueue)
+int charqueue_dequeue(struct charqueue *charqueue)
 {
 	int rc;
 
@@ -108,9 +97,8 @@ int charqueue_dequeue(CHARQUEUE *charqueue)
 	return rc;
 }
 
-
-
-int visor_charqueue_dequeue_n(CHARQUEUE *charqueue, unsigned char *buf, int n)
+int visor_charqueue_dequeue_n(struct charqueue *charqueue, unsigned char *buf,
+			      int n)
 {
 	int rc, counter = 0, c;
 
@@ -132,9 +120,7 @@ int visor_charqueue_dequeue_n(CHARQUEUE *charqueue, unsigned char *buf, int n)
 }
 EXPORT_SYMBOL_GPL(visor_charqueue_dequeue_n);
 
-
-
-void visor_charqueue_destroy(CHARQUEUE *charqueue)
+void visor_charqueue_destroy(struct charqueue *charqueue)
 {
 	if (charqueue == NULL)
 		return;
diff --git a/drivers/staging/unisys/visorutil/charqueue.h b/drivers/staging/unisys/visorutil/charqueue.h
index d6f16587a364..56c1f79a54b0 100644
--- a/drivers/staging/unisys/visorutil/charqueue.h
+++ b/drivers/staging/unisys/visorutil/charqueue.h
@@ -21,17 +21,18 @@
 #include "uniklog.h"
 #include "timskmod.h"
 
-/* CHARQUEUE is an opaque structure to users.
+/* struct charqueue is an opaque structure to users.
  * Fields are declared only in the implementation .c files.
  */
-typedef struct CHARQUEUE_Tag CHARQUEUE;
+struct charqueue;
 
-CHARQUEUE *visor_charqueue_create(ulong nslots);
-void visor_charqueue_enqueue(CHARQUEUE *charqueue, unsigned char c);
-int charqueue_dequeue(CHARQUEUE *charqueue);
-int visor_charqueue_dequeue_n(CHARQUEUE *charqueue, unsigned char *buf, int n);
-BOOL visor_charqueue_is_empty(CHARQUEUE *charqueue);
-void visor_charqueue_destroy(CHARQUEUE *charqueue);
+struct charqueue *visor_charqueue_create(ulong nslots);
+void visor_charqueue_enqueue(struct charqueue *charqueue, unsigned char c);
+int charqueue_dequeue(struct charqueue *charqueue);
+int visor_charqueue_dequeue_n(struct charqueue *charqueue, unsigned char *buf,
+			      int n);
+BOOL visor_charqueue_is_empty(struct charqueue *charqueue);
+void visor_charqueue_destroy(struct charqueue *charqueue);
 
 #endif
 
diff --git a/drivers/staging/unisys/visorutil/easyproc.c b/drivers/staging/unisys/visorutil/easyproc.c
index 3b388494e2af..40f1ae9a155c 100644
--- a/drivers/staging/unisys/visorutil/easyproc.c
+++ b/drivers/staging/unisys/visorutil/easyproc.c
@@ -254,9 +254,9 @@ void visor_easyproc_CreateDeviceProperty(struct easyproc_device_info *p,
 	}
 	strcpy(px->property_name, property_name);
 	if (px->procEntry == NULL) {
-		ERRDEVX(p->devno, "failed to register /proc/%s/device/%d/%s entry",
-			p->pdriver->ProcId, p->devno, property_name
-		       );
+		ERRDEVX(p->devno,
+			"failed to register /proc/%s/device/%d/%s entry",
+			p->pdriver->ProcId, p->devno, property_name);
 		return;
 	}
 	px->show_device_property_info = show_property_info;
diff --git a/drivers/staging/unisys/visorutil/memregion.h b/drivers/staging/unisys/visorutil/memregion.h
index f4a65d2fcf02..0c3eebcf6d50 100644
--- a/drivers/staging/unisys/visorutil/memregion.h
+++ b/drivers/staging/unisys/visorutil/memregion.h
@@ -20,24 +20,24 @@
 
 #include "timskmod.h"
 
-/* MEMREGION is an opaque structure to users.
+/* struct memregion is an opaque structure to users.
  * Fields are declared only in the implementation .c files.
  */
-typedef struct MEMREGION_Tag MEMREGION;
+struct memregion;
 
-MEMREGION *visor_memregion_create(HOSTADDRESS physaddr, ulong nbytes);
-MEMREGION *visor_memregion_create_overlapped(MEMREGION *parent,
-					     ulong offset, ulong nbytes);
-int visor_memregion_resize(MEMREGION *memregion, ulong newsize);
-int visor_memregion_read(MEMREGION *memregion,
-		   ulong offset, void *dest, ulong nbytes);
-int visor_memregion_write(MEMREGION *memregion,
+struct memregion *visor_memregion_create(HOSTADDRESS physaddr, ulong nbytes);
+struct memregion *visor_memregion_create_overlapped(struct memregion *parent,
+						    ulong offset, ulong nbytes);
+int visor_memregion_resize(struct memregion *memregion, ulong newsize);
+int visor_memregion_read(struct memregion *memregion,
+			 ulong offset, void *dest, ulong nbytes);
+int visor_memregion_write(struct memregion *memregion,
 			  ulong offset, void *src, ulong nbytes);
-void visor_memregion_destroy(MEMREGION *memregion);
-HOSTADDRESS visor_memregion_get_physaddr(MEMREGION *memregion);
-ulong visor_memregion_get_nbytes(MEMREGION *memregion);
-void memregion_dump(MEMREGION *memregion, char *s,
+void visor_memregion_destroy(struct memregion *memregion);
+HOSTADDRESS visor_memregion_get_physaddr(struct memregion *memregion);
+ulong visor_memregion_get_nbytes(struct memregion *memregion);
+void memregion_dump(struct memregion *memregion, char *s,
 		    ulong off, ulong len, struct seq_file *seq);
-void __iomem *visor_memregion_get_pointer(MEMREGION *memregion);
+void __iomem *visor_memregion_get_pointer(struct memregion *memregion);
 
 #endif
diff --git a/drivers/staging/unisys/visorutil/memregion_direct.c b/drivers/staging/unisys/visorutil/memregion_direct.c
index 65bc07b947db..33522cc8c22c 100644
--- a/drivers/staging/unisys/visorutil/memregion_direct.c
+++ b/drivers/staging/unisys/visorutil/memregion_direct.c
@@ -26,7 +26,7 @@
 
 #define MYDRVNAME "memregion"
 
-struct MEMREGION_Tag {
+struct memregion {
 	HOSTADDRESS physaddr;
 	ulong nbytes;
 	void __iomem *mapped;
@@ -34,15 +34,15 @@ struct MEMREGION_Tag {
 	BOOL overlapped;
 };
 
-static BOOL mapit(MEMREGION *memregion);
-static void unmapit(MEMREGION *memregion);
+static BOOL mapit(struct memregion *memregion);
+static void unmapit(struct memregion *memregion);
 
-MEMREGION *
+struct memregion *
 visor_memregion_create(HOSTADDRESS physaddr, ulong nbytes)
 {
-	MEMREGION *rc = NULL;
-	MEMREGION *memregion = kzalloc(sizeof(MEMREGION),
-				       GFP_KERNEL | __GFP_NORETRY);
+	struct memregion *rc = NULL;
+	struct memregion *memregion = kzalloc(sizeof(*memregion),
+					      GFP_KERNEL | __GFP_NORETRY);
 	if (memregion == NULL) {
 		ERRDRV("visor_memregion_create allocation failed");
 		return NULL;
@@ -52,24 +52,23 @@ visor_memregion_create(HOSTADDRESS physaddr, ulong nbytes)
 	memregion->overlapped = FALSE;
 	if (!mapit(memregion)) {
 		rc = NULL;
-		goto Away;
+		goto cleanup;
 	}
 	rc = memregion;
-Away:
+cleanup:
 	if (rc == NULL) {
-		if (memregion != NULL) {
-			visor_memregion_destroy(memregion);
-			memregion = NULL;
-		}
+		visor_memregion_destroy(memregion);
+		memregion = NULL;
 	}
 	return rc;
 }
 EXPORT_SYMBOL_GPL(visor_memregion_create);
 
-MEMREGION *
-visor_memregion_create_overlapped(MEMREGION *parent, ulong offset, ulong nbytes)
+struct memregion *
+visor_memregion_create_overlapped(struct memregion *parent, ulong offset,
+				  ulong nbytes)
 {
-	MEMREGION *memregion = NULL;
+	struct memregion *memregion = NULL;
 
 	if (parent == NULL) {
 		ERRDRV("%s parent is NULL", __func__);
@@ -85,7 +84,7 @@ visor_memregion_create_overlapped(MEMREGION *parent, ulong offset, ulong nbytes)
 		       __func__, offset, nbytes);
 		return NULL;
 	}
-	memregion = kzalloc(sizeof(MEMREGION), GFP_KERNEL|__GFP_NORETRY);
+	memregion = kzalloc(sizeof(*memregion), GFP_KERNEL|__GFP_NORETRY);
 	if (memregion == NULL) {
 		ERRDRV("%s allocation failed", __func__);
 		return NULL;
@@ -93,23 +92,23 @@ visor_memregion_create_overlapped(MEMREGION *parent, ulong offset, ulong nbytes)
 
 	memregion->physaddr = parent->physaddr + offset;
 	memregion->nbytes = nbytes;
-	memregion->mapped = ((u8 __iomem *) (parent->mapped)) + offset;
+	memregion->mapped = ((u8 __iomem *)(parent->mapped)) + offset;
 	memregion->requested = FALSE;
 	memregion->overlapped = TRUE;
 	return memregion;
 }
 EXPORT_SYMBOL_GPL(visor_memregion_create_overlapped);
 
-
 static BOOL
-mapit(MEMREGION *memregion)
+mapit(struct memregion *memregion)
 {
-	ulong physaddr = (ulong) (memregion->physaddr);
+	ulong physaddr = (ulong)(memregion->physaddr);
 	ulong nbytes = memregion->nbytes;
 
 	memregion->requested = FALSE;
 	if (!request_mem_region(physaddr, nbytes, MYDRVNAME))
-		ERRDRV("cannot reserve channel memory @0x%lx for 0x%lx-- no big deal", physaddr, nbytes);
+		ERRDRV("cannot reserve channel memory @0x%lx for 0x%lx-- no big deal",
+		       physaddr, nbytes);
 	else
 		memregion->requested = TRUE;
 	memregion->mapped = ioremap_cache(physaddr, nbytes);
@@ -122,42 +121,42 @@ mapit(MEMREGION *memregion)
 }
 
 static void
-unmapit(MEMREGION *memregion)
+unmapit(struct memregion *memregion)
 {
 	if (memregion->mapped != NULL) {
 		iounmap(memregion->mapped);
 		memregion->mapped = NULL;
 	}
 	if (memregion->requested) {
-		release_mem_region((ulong) (memregion->physaddr),
+		release_mem_region((ulong)(memregion->physaddr),
 				   memregion->nbytes);
 		memregion->requested = FALSE;
 	}
 }
 
 HOSTADDRESS
-visor_memregion_get_physaddr(MEMREGION *memregion)
+visor_memregion_get_physaddr(struct memregion *memregion)
 {
 	return memregion->physaddr;
 }
 EXPORT_SYMBOL_GPL(visor_memregion_get_physaddr);
 
 ulong
-visor_memregion_get_nbytes(MEMREGION *memregion)
+visor_memregion_get_nbytes(struct memregion *memregion)
 {
 	return memregion->nbytes;
 }
 EXPORT_SYMBOL_GPL(visor_memregion_get_nbytes);
 
 void __iomem *
-visor_memregion_get_pointer(MEMREGION *memregion)
+visor_memregion_get_pointer(struct memregion *memregion)
 {
 	return memregion->mapped;
 }
 EXPORT_SYMBOL_GPL(visor_memregion_get_pointer);
 
 int
-visor_memregion_resize(MEMREGION *memregion, ulong newsize)
+visor_memregion_resize(struct memregion *memregion, ulong newsize)
 {
 	if (newsize == memregion->nbytes)
 		return 0;
@@ -176,10 +175,9 @@ visor_memregion_resize(MEMREGION *memregion, ulong newsize)
 }
 EXPORT_SYMBOL_GPL(visor_memregion_resize);
 
-
 static int
 memregion_readwrite(BOOL is_write,
-		    MEMREGION *memregion, ulong offset,
+		    struct memregion *memregion, ulong offset,
 		    void *local, ulong nbytes)
 {
 	if (offset + nbytes > memregion->nbytes) {
@@ -195,7 +193,7 @@ memregion_readwrite(BOOL is_write,
 }
 
 int
-visor_memregion_read(MEMREGION *memregion, ulong offset, void *dest,
+visor_memregion_read(struct memregion *memregion, ulong offset, void *dest,
 		     ulong nbytes)
 {
 	return memregion_readwrite(FALSE, memregion, offset, dest, nbytes);
@@ -203,7 +201,7 @@ visor_memregion_read(MEMREGION *memregion, ulong offset, void *dest,
 EXPORT_SYMBOL_GPL(visor_memregion_read);
 
 int
-visor_memregion_write(MEMREGION *memregion, ulong offset, void *src,
+visor_memregion_write(struct memregion *memregion, ulong offset, void *src,
 		      ulong nbytes)
 {
 	return memregion_readwrite(TRUE, memregion, offset, src, nbytes);
@@ -211,7 +209,7 @@ visor_memregion_write(MEMREGION *memregion, ulong offset, void *src,
 EXPORT_SYMBOL_GPL(visor_memregion_write);
 
 void
-visor_memregion_destroy(MEMREGION *memregion)
+visor_memregion_destroy(struct memregion *memregion)
 {
 	if (memregion == NULL)
 		return;
diff --git a/drivers/staging/unisys/visorutil/periodic_work.c b/drivers/staging/unisys/visorutil/periodic_work.c
index 3dd1c04d0e14..0908bf929401 100644
--- a/drivers/staging/unisys/visorutil/periodic_work.c
+++ b/drivers/staging/unisys/visorutil/periodic_work.c
@@ -25,8 +25,6 @@
 
 #define MYDRVNAME "periodic_work"
 
-
-
 struct periodic_work {
 	rwlock_t lock;
 	struct delayed_work work;
@@ -39,8 +37,6 @@ struct periodic_work {
 	const char *devnam;
 };
 
-
-
 static void periodic_work_func(struct work_struct *work)
 {
 	struct periodic_work *pw;
@@ -49,8 +45,6 @@ static void periodic_work_func(struct work_struct *work)
 	(*pw->workfunc)(pw->workfuncarg);
 }
 
-
-
 struct periodic_work *visor_periodic_work_create(ulong jiffy_interval,
 					struct workqueue_struct *workqueue,
 					void (*workfunc)(void *),
@@ -73,16 +67,12 @@ struct periodic_work *visor_periodic_work_create(ulong jiffy_interval,
 }
 EXPORT_SYMBOL_GPL(visor_periodic_work_create);
 
-
-
 void visor_periodic_work_destroy(struct periodic_work *pw)
 {
 	kfree(pw);
 }
 EXPORT_SYMBOL_GPL(visor_periodic_work_destroy);
 
-
-
 /** Call this from your periodic work worker function to schedule the next
  *  call.
  *  If this function returns FALSE, there was a failure and the
@@ -112,8 +102,6 @@ unlock:
 }
 EXPORT_SYMBOL_GPL(visor_periodic_work_nextperiod);
 
-
-
 /** This function returns TRUE iff new periodic work was actually started.
  *  If this function returns FALSE, then no work was started
  *  (either because it was already started, or because of a failure).
@@ -145,13 +133,9 @@ BOOL visor_periodic_work_start(struct periodic_work *pw)
 unlock:
 	write_unlock(&pw->lock);
 	return rc;
-
 }
 EXPORT_SYMBOL_GPL(visor_periodic_work_start);
 
-
-
-
 /** This function returns TRUE iff your call actually stopped the periodic
  *  work.
  *
@@ -223,8 +207,9 @@ BOOL visor_periodic_work_stop(struct periodic_work *pw)
 			 */
 			SLEEPJIFFIES(10);
 			write_lock(&pw->lock);
-		} else
+		} else {
 			pw->want_to_stop = FALSE;
+		}
 	}
 	write_unlock(&pw->lock);
 	return stopped_something;
diff --git a/drivers/staging/unisys/visorutil/procobjecttree.c b/drivers/staging/unisys/visorutil/procobjecttree.c
index c476036f7382..195772d22c9e 100644
--- a/drivers/staging/unisys/visorutil/procobjecttree.c
+++ b/drivers/staging/unisys/visorutil/procobjecttree.c
@@ -320,19 +320,18 @@ void visor_proc_DestroyObject(MYPROCOBJECT *obj)
 		kfree(obj->procDirProperties);
 		obj->procDirProperties = NULL;
 	}
-	if (obj->procDirPropertyContexts != NULL) {
-		kfree(obj->procDirPropertyContexts);
-		obj->procDirPropertyContexts = NULL;
-	}
+
+	kfree(obj->procDirPropertyContexts);
+	obj->procDirPropertyContexts = NULL;
+
 	if (obj->procDir != NULL) {
 		if (obj->name != NULL)
 			remove_proc_entry(obj->name, type->procDir);
 		obj->procDir = NULL;
 	}
-	if (obj->name != NULL) {
-		kfree(obj->name);
-		obj->name = NULL;
-	}
+
+	kfree(obj->name);
+	obj->name = NULL;
 	kfree(obj);
 }
 EXPORT_SYMBOL_GPL(visor_proc_DestroyObject);
diff --git a/drivers/staging/unisys/visorutil/visorkmodutils.c b/drivers/staging/unisys/visorutil/visorkmodutils.c
index d6815f9e1337..556e2642d2d9 100644
--- a/drivers/staging/unisys/visorutil/visorkmodutils.c
+++ b/drivers/staging/unisys/visorutil/visorkmodutils.c
@@ -36,18 +36,7 @@
 int unisys_spar_platform;
 EXPORT_SYMBOL_GPL(unisys_spar_platform);
 
-/** Callers to interfaces that set __GFP_NORETRY flag below
- *  must check for a NULL (error) result as we are telling the
- *  kernel interface that it is okay to fail.
- */
-
-void *kmalloc_kernel(size_t siz)
-{
-	return kmalloc(siz, GFP_KERNEL | __GFP_NORETRY);
-}
-
-static __init uint32_t
-visorutil_spar_detect(void)
+static __init uint32_t visorutil_spar_detect(void)
 {
 	unsigned int eax, ebx, ecx, edx;
 
@@ -57,22 +46,19 @@ visorutil_spar_detect(void)
 		return  (ebx == UNISYS_SPAR_ID_EBX) &&
 			(ecx == UNISYS_SPAR_ID_ECX) &&
 			(edx == UNISYS_SPAR_ID_EDX);
-	} else
+	} else {
 		return 0;
-
+	}
 }
 
-
-
-
-static __init int
-visorutil_mod_init(void)
+static __init int visorutil_mod_init(void)
 {
 	if (visorutil_spar_detect()) {
 		unisys_spar_platform = TRUE;
 		return 0;
-	} else
+	} else {
 		return -ENODEV;
+	}
 }
 
 static __exit void
diff --git a/drivers/staging/vme/devices/Kconfig b/drivers/staging/vme/devices/Kconfig
index 8e8bbb1dcd9b..1d2ff0cc41f1 100644
--- a/drivers/staging/vme/devices/Kconfig
+++ b/drivers/staging/vme/devices/Kconfig
@@ -8,6 +8,9 @@ config VME_USER
 	  VME windows in a manner at least semi-compatible with the interface
 	  provided with the original driver at <http://www.vmelinux.org/>.
 
+	  To compile this driver as a module, choose M here. The module will
+	  be called vme_user. If unsure, say N.
+
 config VME_PIO2
 	tristate "GE PIO2 VME"
 	depends on STAGING && GPIOLIB
diff --git a/drivers/staging/vme/devices/vme_pio2_gpio.c b/drivers/staging/vme/devices/vme_pio2_gpio.c
index c64776f71809..da34d5529f51 100644
--- a/drivers/staging/vme/devices/vme_pio2_gpio.c
+++ b/drivers/staging/vme/devices/vme_pio2_gpio.c
@@ -191,11 +191,11 @@ int pio2_gpio_init(struct pio2_card *card)
 	int retval = 0;
 	char *label;
 
-	label = kmalloc(PIO2_NUM_CHANNELS, GFP_KERNEL);
+	label = kasprintf(GFP_KERNEL,
+			  "%s@%s", driver_name, dev_name(&card->vdev->dev));
 	if (label == NULL)
 		return -ENOMEM;
 
-	sprintf(label, "%s@%s", driver_name, dev_name(&card->vdev->dev));
 	card->gc.label = label;
 
 	card->gc.ngpio = PIO2_NUM_CHANNELS;
diff --git a/drivers/staging/vt6655/80211hdr.h b/drivers/staging/vt6655/80211hdr.h
deleted file mode 100644
index 36e14ec50564..000000000000
--- a/drivers/staging/vt6655/80211hdr.h
+++ /dev/null
@@ -1,318 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: 80211hdr.h
- *
- * Purpose: 802.11 MAC headers related pre-defines and macros.
- *
- *
- * Author: Lyndon Chen
- *
- * Date: Apr 8, 2002
- *
- */
-
-#ifndef __80211HDR_H__
-#define __80211HDR_H__
-
-#include "ttype.h"
-
-/* bit type */
-#define BIT0	0x00000001
-#define BIT1	0x00000002
-#define BIT2	0x00000004
-#define BIT3	0x00000008
-#define BIT4	0x00000010
-#define BIT5	0x00000020
-#define BIT6	0x00000040
-#define BIT7	0x00000080
-#define BIT8	0x00000100
-#define BIT9	0x00000200
-#define BIT10	0x00000400
-#define BIT11	0x00000800
-#define BIT12	0x00001000
-#define BIT13	0x00002000
-#define BIT14	0x00004000
-#define BIT15	0x00008000
-#define BIT16	0x00010000
-#define BIT17	0x00020000
-#define BIT18	0x00040000
-#define BIT19	0x00080000
-#define BIT20	0x00100000
-#define BIT21	0x00200000
-#define BIT22	0x00400000
-#define BIT23	0x00800000
-#define BIT24	0x01000000
-#define BIT25	0x02000000
-#define BIT26	0x04000000
-#define BIT27	0x08000000
-#define BIT28	0x10000000
-#define BIT29	0x20000000
-#define BIT30	0x40000000
-#define BIT31	0x80000000
-
-/* 802.11 frame related, defined as 802.11 spec */
-#define WLAN_ADDR_LEN               6
-#define WLAN_CRC_LEN                4
-#define WLAN_CRC32_LEN              4
-#define WLAN_FCS_LEN                4
-#define WLAN_BSSID_LEN              6
-#define WLAN_BSS_TS_LEN             8
-#define WLAN_HDR_ADDR2_LEN          16
-#define WLAN_HDR_ADDR3_LEN          24
-#define WLAN_HDR_ADDR4_LEN          30
-#define WLAN_IEHDR_LEN              2
-#define WLAN_SSID_MAXLEN            32
-#define WLAN_RATES_MAXLEN           16
-#define WLAN_RATES_MAXLEN_11B       4
-#define WLAN_RSN_MAXLEN             32
-#define WLAN_DATA_MAXLEN            2312
-#define WLAN_A3FR_MAXLEN            (WLAN_HDR_ADDR3_LEN + WLAN_DATA_MAXLEN + \
-				     WLAN_CRC_LEN)
-
-#define WLAN_BEACON_FR_MAXLEN       WLAN_A3FR_MAXLEN
-#define WLAN_ATIM_FR_MAXLEN         (WLAN_HDR_ADDR3_LEN + 0)
-#define WLAN_NULLDATA_FR_MAXLEN     (WLAN_HDR_ADDR3_LEN + 0)
-#define WLAN_DISASSOC_FR_MAXLEN     (WLAN_HDR_ADDR3_LEN + 2)
-#define WLAN_ASSOCREQ_FR_MAXLEN     WLAN_A3FR_MAXLEN
-#define WLAN_ASSOCRESP_FR_MAXLEN    WLAN_A3FR_MAXLEN
-#define WLAN_REASSOCREQ_FR_MAXLEN   WLAN_A3FR_MAXLEN
-#define WLAN_REASSOCRESP_FR_MAXLEN  WLAN_A3FR_MAXLEN
-#define WLAN_PROBEREQ_FR_MAXLEN     WLAN_A3FR_MAXLEN
-#define WLAN_PROBERESP_FR_MAXLEN    WLAN_A3FR_MAXLEN
-#define WLAN_AUTHEN_FR_MAXLEN       WLAN_A3FR_MAXLEN
-#define WLAN_DEAUTHEN_FR_MAXLEN     (WLAN_HDR_ADDR3_LEN + 2)
-
-#define WLAN_WEP_NKEYS              4
-#define WLAN_WEP40_KEYLEN           5
-#define WLAN_WEP104_KEYLEN          13
-#define WLAN_WEP232_KEYLEN          29
-#define WLAN_WEPMAX_KEYLEN          32
-#define WLAN_CHALLENGE_IE_MAXLEN    255
-#define WLAN_CHALLENGE_IE_LEN       130
-#define WLAN_CHALLENGE_LEN          128
-#define WLAN_WEP_IV_LEN             4
-#define WLAN_WEP_ICV_LEN            4
-#define WLAN_FRAGS_MAX              16
-
-/* Frame Type */
-#define WLAN_TYPE_MGR 0x00
-#define WLAN_TYPE_CTL  0x01
-#define WLAN_TYPE_DATA 0x02
-
-#define WLAN_FTYPE_MGMT 0x00
-#define WLAN_FTYPE_CTL  0x01
-#define WLAN_FTYPE_DATA 0x02
-
-/* Frame Subtypes */
-#define WLAN_FSTYPE_ASSOCREQ        0x00
-#define WLAN_FSTYPE_ASSOCRESP       0x01
-#define WLAN_FSTYPE_REASSOCREQ      0x02
-#define WLAN_FSTYPE_REASSOCRESP     0x03
-#define WLAN_FSTYPE_PROBEREQ        0x04
-#define WLAN_FSTYPE_PROBERESP       0x05
-#define WLAN_FSTYPE_BEACON          0x08
-#define WLAN_FSTYPE_ATIM            0x09
-#define WLAN_FSTYPE_DISASSOC        0x0a
-#define WLAN_FSTYPE_AUTHEN          0x0b
-#define WLAN_FSTYPE_DEAUTHEN        0x0c
-#define WLAN_FSTYPE_ACTION          0x0d
-
-/* Control */
-#define WLAN_FSTYPE_PSPOLL          0x0a
-#define WLAN_FSTYPE_RTS             0x0b
-#define WLAN_FSTYPE_CTS             0x0c
-#define WLAN_FSTYPE_ACK             0x0d
-#define WLAN_FSTYPE_CFEND           0x0e
-#define WLAN_FSTYPE_CFENDCFACK      0x0f
-
-/* Data */
-#define WLAN_FSTYPE_DATAONLY        0x00
-#define WLAN_FSTYPE_DATA_CFACK      0x01
-#define WLAN_FSTYPE_DATA_CFPOLL     0x02
-#define WLAN_FSTYPE_DATA_CFACK_CFPOLL   0x03
-#define WLAN_FSTYPE_NULL            0x04
-#define WLAN_FSTYPE_CFACK           0x05
-#define WLAN_FSTYPE_CFPOLL          0x06
-#define WLAN_FSTYPE_CFACK_CFPOLL    0x07
-
-#ifdef __BIG_ENDIAN
-
-/* GET & SET Frame Control bit */
-#define WLAN_GET_FC_PRVER(n)    (((unsigned short)(n) >> 8) & (BIT0 | BIT1))
-#define WLAN_GET_FC_FTYPE(n)    ((((unsigned short)(n) >> 8) & (BIT2 | BIT3)) >> 2)
-#define WLAN_GET_FC_FSTYPE(n)   ((((unsigned short)(n) >> 8) & (BIT4|BIT5|BIT6|BIT7)) >> 4)
-#define WLAN_GET_FC_TODS(n)     ((((unsigned short)(n) << 8) & (BIT8)) >> 8)
-#define WLAN_GET_FC_FROMDS(n)   ((((unsigned short)(n) << 8) & (BIT9)) >> 9)
-#define WLAN_GET_FC_MOREFRAG(n) ((((unsigned short)(n) << 8) & (BIT10)) >> 10)
-#define WLAN_GET_FC_RETRY(n)    ((((unsigned short)(n) << 8) & (BIT11)) >> 11)
-#define WLAN_GET_FC_PWRMGT(n)   ((((unsigned short)(n) << 8) & (BIT12)) >> 12)
-#define WLAN_GET_FC_MOREDATA(n) ((((unsigned short)(n) << 8) & (BIT13)) >> 13)
-#define WLAN_GET_FC_ISWEP(n)    ((((unsigned short)(n) << 8) & (BIT14)) >> 14)
-#define WLAN_GET_FC_ORDER(n)    ((((unsigned short)(n) << 8) & (BIT15)) >> 15)
-
-/* Sequence Field bit */
-#define WLAN_GET_SEQ_FRGNUM(n) (((unsigned short)(n) >> 8) & (BIT0|BIT1|BIT2|BIT3))
-#define WLAN_GET_SEQ_SEQNUM(n) ((((unsigned short)(n) >> 8) & (~(BIT0|BIT1|BIT2|BIT3))) >> 4)
-
-/* Capability Field bit */
-#define WLAN_GET_CAP_INFO_ESS(n)           (((n) >> 8) & BIT0)
-#define WLAN_GET_CAP_INFO_IBSS(n)          ((((n) >> 8) & BIT1) >> 1)
-#define WLAN_GET_CAP_INFO_CFPOLLABLE(n)    ((((n) >> 8) & BIT2) >> 2)
-#define WLAN_GET_CAP_INFO_CFPOLLREQ(n)     ((((n) >> 8) & BIT3) >> 3)
-#define WLAN_GET_CAP_INFO_PRIVACY(n)       ((((n) >> 8) & BIT4) >> 4)
-#define WLAN_GET_CAP_INFO_SHORTPREAMBLE(n) ((((n) >> 8) & BIT5) >> 5)
-#define WLAN_GET_CAP_INFO_PBCC(n)          ((((n) >> 8) & BIT6) >> 6)
-#define WLAN_GET_CAP_INFO_AGILITY(n)       ((((n) >> 8) & BIT7) >> 7)
-#define WLAN_GET_CAP_INFO_SPECTRUMMNG(n)   ((((n))      & BIT8) >> 10)
-#define WLAN_GET_CAP_INFO_SHORTSLOTTIME(n) ((((n))      & BIT10) >> 10)
-#define WLAN_GET_CAP_INFO_DSSSOFDM(n)      ((((n))      & BIT13) >> 13)
-#define WLAN_GET_CAP_INFO_GRPACK(n)        ((((n))      & BIT14) >> 14)
-
-#else
-
-/* GET & SET Frame Control bit */
-#define WLAN_GET_FC_PRVER(n)    (((unsigned short)(n)) & (BIT0 | BIT1))
-#define WLAN_GET_FC_FTYPE(n)    ((((unsigned short)(n)) & (BIT2 | BIT3)) >> 2)
-#define WLAN_GET_FC_FSTYPE(n)   ((((unsigned short)(n)) & (BIT4|BIT5|BIT6|BIT7)) >> 4)
-#define WLAN_GET_FC_TODS(n)     ((((unsigned short)(n)) & (BIT8)) >> 8)
-#define WLAN_GET_FC_FROMDS(n)   ((((unsigned short)(n)) & (BIT9)) >> 9)
-#define WLAN_GET_FC_MOREFRAG(n) ((((unsigned short)(n)) & (BIT10)) >> 10)
-#define WLAN_GET_FC_RETRY(n)    ((((unsigned short)(n)) & (BIT11)) >> 11)
-#define WLAN_GET_FC_PWRMGT(n)   ((((unsigned short)(n)) & (BIT12)) >> 12)
-#define WLAN_GET_FC_MOREDATA(n) ((((unsigned short)(n)) & (BIT13)) >> 13)
-#define WLAN_GET_FC_ISWEP(n)    ((((unsigned short)(n)) & (BIT14)) >> 14)
-#define WLAN_GET_FC_ORDER(n)    ((((unsigned short)(n)) & (BIT15)) >> 15)
-
-/* Sequence Field bit */
-#define WLAN_GET_SEQ_FRGNUM(n) (((unsigned short)(n)) & (BIT0|BIT1|BIT2|BIT3))
-#define WLAN_GET_SEQ_SEQNUM(n) ((((unsigned short)(n)) & (~(BIT0|BIT1|BIT2|BIT3))) >> 4)
-
-/* Capability Field bit */
-#define WLAN_GET_CAP_INFO_ESS(n)           ((n) & BIT0)
-#define WLAN_GET_CAP_INFO_IBSS(n)          (((n) & BIT1) >> 1)
-#define WLAN_GET_CAP_INFO_CFPOLLABLE(n)    (((n) & BIT2) >> 2)
-#define WLAN_GET_CAP_INFO_CFPOLLREQ(n)     (((n) & BIT3) >> 3)
-#define WLAN_GET_CAP_INFO_PRIVACY(n)       (((n) & BIT4) >> 4)
-#define WLAN_GET_CAP_INFO_SHORTPREAMBLE(n) (((n) & BIT5) >> 5)
-#define WLAN_GET_CAP_INFO_PBCC(n)          (((n) & BIT6) >> 6)
-#define WLAN_GET_CAP_INFO_AGILITY(n)       (((n) & BIT7) >> 7)
-#define WLAN_GET_CAP_INFO_SPECTRUMMNG(n)   (((n) & BIT8) >> 10)
-#define WLAN_GET_CAP_INFO_SHORTSLOTTIME(n) (((n) & BIT10) >> 10)
-#define WLAN_GET_CAP_INFO_DSSSOFDM(n)      (((n) & BIT13) >> 13)
-#define WLAN_GET_CAP_INFO_GRPACK(n)        (((n) & BIT14) >> 14)
-
-#endif /*#ifdef __BIG_ENDIAN */
-
-#define WLAN_SET_CAP_INFO_ESS(n)           (n)
-#define WLAN_SET_CAP_INFO_IBSS(n)          ((n) << 1)
-#define WLAN_SET_CAP_INFO_CFPOLLABLE(n)    ((n) << 2)
-#define WLAN_SET_CAP_INFO_CFPOLLREQ(n)     ((n) << 3)
-#define WLAN_SET_CAP_INFO_PRIVACY(n)       ((n) << 4)
-#define WLAN_SET_CAP_INFO_SHORTPREAMBLE(n) ((n) << 5)
-#define WLAN_SET_CAP_INFO_SPECTRUMMNG(n)   ((n) << 8)
-#define WLAN_SET_CAP_INFO_PBCC(n)          ((n) << 6)
-#define WLAN_SET_CAP_INFO_AGILITY(n)       ((n) << 7)
-#define WLAN_SET_CAP_INFO_SHORTSLOTTIME(n) ((n) << 10)
-#define WLAN_SET_CAP_INFO_DSSSOFDM(n)      ((n) << 13)
-#define WLAN_SET_CAP_INFO_GRPACK(n)        ((n) << 14)
-
-#define WLAN_SET_FC_PRVER(n)    ((unsigned short)(n))
-#define WLAN_SET_FC_FTYPE(n)    (((unsigned short)(n)) << 2)
-#define WLAN_SET_FC_FSTYPE(n)   (((unsigned short)(n)) << 4)
-#define WLAN_SET_FC_TODS(n)     (((unsigned short)(n)) << 8)
-#define WLAN_SET_FC_FROMDS(n)   (((unsigned short)(n)) << 9)
-#define WLAN_SET_FC_MOREFRAG(n) (((unsigned short)(n)) << 10)
-#define WLAN_SET_FC_RETRY(n)    (((unsigned short)(n)) << 11)
-#define WLAN_SET_FC_PWRMGT(n)   (((unsigned short)(n)) << 12)
-#define WLAN_SET_FC_MOREDATA(n) (((unsigned short)(n)) << 13)
-#define WLAN_SET_FC_ISWEP(n)    (((unsigned short)(n)) << 14)
-#define WLAN_SET_FC_ORDER(n)    (((unsigned short)(n)) << 15)
-
-#define WLAN_SET_SEQ_FRGNUM(n) ((unsigned short)(n))
-#define WLAN_SET_SEQ_SEQNUM(n) (((unsigned short)(n)) << 4)
-
-/* ERP Field bit */
-
-#define WLAN_GET_ERP_NONERP_PRESENT(n)     ((n) & BIT0)
-#define WLAN_GET_ERP_USE_PROTECTION(n)     (((n) & BIT1) >> 1)
-#define WLAN_GET_ERP_BARKER_MODE(n)        (((n) & BIT2) >> 2)
-
-#define WLAN_SET_ERP_NONERP_PRESENT(n)     (n)
-#define WLAN_SET_ERP_USE_PROTECTION(n)     ((n) << 1)
-#define WLAN_SET_ERP_BARKER_MODE(n)        ((n) << 2)
-
-/* Support & Basic Rates field */
-#define WLAN_MGMT_IS_BASICRATE(b)    ((b) & BIT7)
-#define WLAN_MGMT_GET_RATE(b)        ((b) & ~BIT7)
-
-/* TIM field */
-#define WLAN_MGMT_IS_MULTICAST_TIM(b)   ((b) & BIT0)
-#define WLAN_MGMT_GET_TIM_OFFSET(b)     (((b) & ~BIT0) >> 1)
-
-/* 3-Addr & 4-Addr */
-#define WLAN_HDR_A3_DATA_PTR(p) (((unsigned char *)(p)) + WLAN_HDR_ADDR3_LEN)
-#define WLAN_HDR_A4_DATA_PTR(p) (((unsigned char *)(p)) + WLAN_HDR_ADDR4_LEN)
-
-/* IEEE ADDR */
-#define IEEE_ADDR_UNIVERSAL         0x02
-#define IEEE_ADDR_GROUP             0x01
-
-typedef struct {
-	unsigned char abyAddr[6];
-} IEEE_ADDR, *PIEEE_ADDR;
-
-/* 802.11 Header Format */
-
-typedef struct tagWLAN_80211HDR_A2 {
-	unsigned short wFrameCtl;
-	unsigned short wDurationID;
-	unsigned char abyAddr1[WLAN_ADDR_LEN];
-	unsigned char abyAddr2[WLAN_ADDR_LEN];
-} __attribute__ ((__packed__))
-WLAN_80211HDR_A2, *PWLAN_80211HDR_A2;
-
-typedef struct tagWLAN_80211HDR_A3 {
-	unsigned short wFrameCtl;
-	unsigned short wDurationID;
-	unsigned char abyAddr1[WLAN_ADDR_LEN];
-	unsigned char abyAddr2[WLAN_ADDR_LEN];
-	unsigned char abyAddr3[WLAN_ADDR_LEN];
-	unsigned short wSeqCtl;
-} __attribute__ ((__packed__))
-WLAN_80211HDR_A3, *PWLAN_80211HDR_A3;
-
-typedef struct tagWLAN_80211HDR_A4 {
-	unsigned short wFrameCtl;
-	unsigned short wDurationID;
-	unsigned char abyAddr1[WLAN_ADDR_LEN];
-	unsigned char abyAddr2[WLAN_ADDR_LEN];
-	unsigned char abyAddr3[WLAN_ADDR_LEN];
-	unsigned short wSeqCtl;
-	unsigned char abyAddr4[WLAN_ADDR_LEN];
-} __attribute__ ((__packed__))
-WLAN_80211HDR_A4, *PWLAN_80211HDR_A4;
-
-typedef union tagUWLAN_80211HDR {
-	WLAN_80211HDR_A2        sA2;
-	WLAN_80211HDR_A3        sA3;
-	WLAN_80211HDR_A4        sA4;
-} UWLAN_80211HDR, *PUWLAN_80211HDR;
-
-#endif /* __80211HDR_H__ */
diff --git a/drivers/staging/vt6655/80211mgr.c b/drivers/staging/vt6655/80211mgr.c
deleted file mode 100644
index 7d2c6472ec9a..000000000000
--- a/drivers/staging/vt6655/80211mgr.c
+++ /dev/null
@@ -1,1019 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: 80211mgr.c
- *
- * Purpose: Handles the 802.11 management support functions
- *
- * Author: Lyndon Chen
- *
- * Date: May 8, 2002
- *
- * Functions:
- *      vMgrEncodeBeacon - Encode the Beacon frame
- *      vMgrDecodeBeacon - Decode the Beacon frame
- *      vMgrEncodeIBSSATIM - Encode the IBSS ATIM frame
- *      vMgrDecodeIBSSATIM - Decode the IBSS ATIM frame
- *      vMgrEncodeDisassociation - Encode the Disassociation frame
- *      vMgrDecodeDisassociation - Decode the Disassociation frame
- *      vMgrEncodeAssocRequest - Encode the Association request frame
- *      vMgrDecodeAssocRequest - Decode the Association request frame
- *      vMgrEncodeAssocResponse - Encode the Association response frame
- *      vMgrDecodeAssocResponse - Decode the Association response frame
- *      vMgrEncodeReAssocRequest - Encode the ReAssociation request frame
- *      vMgrDecodeReAssocRequest - Decode the ReAssociation request frame
- *      vMgrEncodeProbeRequest - Encode the Probe request frame
- *      vMgrDecodeProbeRequest - Decode the Probe request frame
- *      vMgrEncodeProbeResponse - Encode the Probe response frame
- *      vMgrDecodeProbeResponse - Decode the Probe response frame
- *      vMgrEncodeAuthen - Encode the Authentication frame
- *      vMgrDecodeAuthen - Decode the Authentication frame
- *      vMgrEncodeDeauthen - Encode the DeAuthentication frame
- *      vMgrDecodeDeauthen - Decode the DeAuthentication frame
- *      vMgrEncodeReassocResponse - Encode the Reassociation response frame
- *      vMgrDecodeReassocResponse - Decode the Reassociation response frame
- *
- * Revision History:
- *
- */
-
-#include "tmacro.h"
-#include "tether.h"
-#include "80211mgr.h"
-#include "80211hdr.h"
-#include "device.h"
-#include "wpa.h"
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-/*+
- *
- * Routine Description:
- * Encode Beacon frame body offset
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrEncodeBeacon(
-	PWLAN_FR_BEACON  pFrame
-)
-{
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-	/* Fixed Fields */
-	pFrame->pqwTimestamp = (__le64 *)
-				(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-				 WLAN_BEACON_OFF_TS);
-	pFrame->pwBeaconInterval = (unsigned short *)
-				(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-				 WLAN_BEACON_OFF_BCN_INT);
-	pFrame->pwCapInfo = (unsigned short *)
-			    (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			     WLAN_BEACON_OFF_CAPINFO);
-
-	pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_BEACON_OFF_SSID;
-}
-
-/*+
- *
- * Routine Description:
- * Decode Beacon frame body offset
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrDecodeBeacon(
-	PWLAN_FR_BEACON  pFrame
-)
-{
-	PWLAN_IE        pItem;
-
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-	/* Fixed Fields */
-	pFrame->pqwTimestamp = (__le64 *)
-				(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-				 WLAN_BEACON_OFF_TS);
-	pFrame->pwBeaconInterval = (unsigned short *)
-				   (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-				    WLAN_BEACON_OFF_BCN_INT);
-	pFrame->pwCapInfo = (unsigned short *)
-			    (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			     WLAN_BEACON_OFF_CAPINFO);
-
-	/* Information elements */
-	pItem = (PWLAN_IE)((unsigned char *)
-			   (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))) +
-			    WLAN_BEACON_OFF_SSID);
-	while (((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) {
-		switch (pItem->byElementID) {
-		case WLAN_EID_SSID:
-			if (pFrame->pSSID == NULL)
-				pFrame->pSSID = (PWLAN_IE_SSID)pItem;
-			break;
-		case WLAN_EID_SUPP_RATES:
-			if (pFrame->pSuppRates == NULL)
-				pFrame->pSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
-			break;
-		case WLAN_EID_FH_PARMS:
-			/* pFrame->pFHParms = (PWLAN_IE_FH_PARMS)pItem; */
-			break;
-		case WLAN_EID_DS_PARMS:
-			if (pFrame->pDSParms == NULL)
-				pFrame->pDSParms = (PWLAN_IE_DS_PARMS)pItem;
-			break;
-		case WLAN_EID_CF_PARMS:
-			if (pFrame->pCFParms == NULL)
-				pFrame->pCFParms = (PWLAN_IE_CF_PARMS)pItem;
-			break;
-		case WLAN_EID_IBSS_PARMS:
-			if (pFrame->pIBSSParms == NULL)
-				pFrame->pIBSSParms = (PWLAN_IE_IBSS_PARMS)pItem;
-			break;
-		case WLAN_EID_TIM:
-			if (pFrame->pTIM == NULL)
-				pFrame->pTIM = (PWLAN_IE_TIM)pItem;
-			break;
-
-		case WLAN_EID_RSN:
-			if (pFrame->pRSN == NULL)
-				pFrame->pRSN = (PWLAN_IE_RSN)pItem;
-			break;
-		case WLAN_EID_RSN_WPA:
-			if (pFrame->pRSNWPA == NULL) {
-				if (WPAb_Is_RSN((PWLAN_IE_RSN_EXT)pItem) == true)
-					pFrame->pRSNWPA =
-						       (PWLAN_IE_RSN_EXT)pItem;
-			}
-			break;
-
-		case WLAN_EID_ERP:
-			if (pFrame->pERP == NULL)
-				pFrame->pERP = (PWLAN_IE_ERP)pItem;
-			break;
-		case WLAN_EID_EXTSUPP_RATES:
-			if (pFrame->pExtSuppRates == NULL)
-				pFrame->pExtSuppRates =
-						    (PWLAN_IE_SUPP_RATES)pItem;
-			break;
-
-		case WLAN_EID_COUNTRY:      /* 7 */
-			if (pFrame->pIE_Country == NULL)
-				pFrame->pIE_Country = (PWLAN_IE_COUNTRY)pItem;
-			break;
-
-		case WLAN_EID_PWR_CONSTRAINT:   /* 32 */
-			if (pFrame->pIE_PowerConstraint == NULL)
-				pFrame->pIE_PowerConstraint =
-						      (PWLAN_IE_PW_CONST)pItem;
-			break;
-
-		case WLAN_EID_CH_SWITCH:    /* 37 */
-			if (pFrame->pIE_CHSW == NULL)
-				pFrame->pIE_CHSW = (PWLAN_IE_CH_SW)pItem;
-			break;
-
-		case WLAN_EID_QUIET:        /* 40 */
-			if (pFrame->pIE_Quiet == NULL)
-				pFrame->pIE_Quiet = (PWLAN_IE_QUIET)pItem;
-			break;
-
-		case WLAN_EID_IBSS_DFS:
-			if (pFrame->pIE_IBSSDFS == NULL)
-				pFrame->pIE_IBSSDFS = (PWLAN_IE_IBSS_DFS)pItem;
-			break;
-
-		default:
-			pr_debug("Unrecognized EID=%dd in beacon decode\n",
-				 pItem->byElementID);
-			break;
-
-		}
-		pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 + pItem->len);
-	}
-}
-
-/*+
- *
- * Routine Description:
- *  Encode IBSS ATIM
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrEncodeIBSSATIM(
-	PWLAN_FR_IBSSATIM   pFrame
-)
-{
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-	pFrame->len = WLAN_HDR_ADDR3_LEN;
-}
-
-/*+
- *
- * Routine Description:
- *  Decode IBSS ATIM
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrDecodeIBSSATIM(
-	PWLAN_FR_IBSSATIM   pFrame
-)
-{
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-}
-
-/*+
- *
- * Routine Description:
- *  Encode Disassociation
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrEncodeDisassociation(
-	PWLAN_FR_DISASSOC  pFrame
-)
-{
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-	/* Fixed Fields */
-	pFrame->pwReason = (unsigned short *)
-			   (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			    WLAN_DISASSOC_OFF_REASON);
-	pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_DISASSOC_OFF_REASON +
-		      sizeof(*(pFrame->pwReason));
-}
-
-/*+
- *
- * Routine Description:
- *  Decode Disassociation
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrDecodeDisassociation(
-	PWLAN_FR_DISASSOC  pFrame
-)
-{
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-	/* Fixed Fields */
-	pFrame->pwReason = (unsigned short *)
-			   (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			    WLAN_DISASSOC_OFF_REASON);
-}
-
-/*+
- *
- * Routine Description:
- *  Encode Association Request
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrEncodeAssocRequest(
-	PWLAN_FR_ASSOCREQ  pFrame
-)
-{
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-	/* Fixed Fields */
-	pFrame->pwCapInfo = (unsigned short *)
-			    (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			     WLAN_ASSOCREQ_OFF_CAP_INFO);
-	pFrame->pwListenInterval = (unsigned short *)
-				   (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-				    WLAN_ASSOCREQ_OFF_LISTEN_INT);
-	pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_ASSOCREQ_OFF_LISTEN_INT +
-		      sizeof(*(pFrame->pwListenInterval));
-}
-
-/*+
- *
- * Routine Description: (AP)
- *  Decode Association Request
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrDecodeAssocRequest(
-	PWLAN_FR_ASSOCREQ  pFrame
-)
-{
-	PWLAN_IE   pItem;
-
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-	/* Fixed Fields */
-	pFrame->pwCapInfo = (unsigned short *)
-			    (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			     WLAN_ASSOCREQ_OFF_CAP_INFO);
-	pFrame->pwListenInterval = (unsigned short *)
-				   (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-				    WLAN_ASSOCREQ_OFF_LISTEN_INT);
-
-	/* Information elements */
-	pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-			   + WLAN_ASSOCREQ_OFF_SSID);
-
-	while (((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) {
-		switch (pItem->byElementID) {
-		case WLAN_EID_SSID:
-			if (pFrame->pSSID == NULL)
-				pFrame->pSSID = (PWLAN_IE_SSID)pItem;
-			break;
-		case WLAN_EID_SUPP_RATES:
-			if (pFrame->pSuppRates == NULL)
-				pFrame->pSuppRates =
-						   (PWLAN_IE_SUPP_RATES)pItem;
-			break;
-
-		case WLAN_EID_RSN:
-			if (pFrame->pRSN == NULL)
-				pFrame->pRSN = (PWLAN_IE_RSN)pItem;
-			break;
-		case WLAN_EID_RSN_WPA:
-			if (pFrame->pRSNWPA == NULL) {
-				if (WPAb_Is_RSN((PWLAN_IE_RSN_EXT)pItem) == true)
-					pFrame->pRSNWPA =
-						       (PWLAN_IE_RSN_EXT)pItem;
-			}
-			break;
-		case WLAN_EID_EXTSUPP_RATES:
-			if (pFrame->pExtSuppRates == NULL)
-				pFrame->pExtSuppRates =
-						    (PWLAN_IE_SUPP_RATES)pItem;
-			break;
-
-		default:
-			pr_debug("Unrecognized EID=%dd in assocreq decode\n",
-				 pItem->byElementID);
-			break;
-		}
-		pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 + pItem->len);
-	}
-}
-
-/*+
- *
- * Routine Description: (AP)
- *  Encode Association Response
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrEncodeAssocResponse(
-	PWLAN_FR_ASSOCRESP  pFrame
-)
-{
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-	/* Fixed Fields */
-	pFrame->pwCapInfo = (unsigned short *)
-			    (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			     WLAN_ASSOCRESP_OFF_CAP_INFO);
-	pFrame->pwStatus = (unsigned short *)
-			   (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			    WLAN_ASSOCRESP_OFF_STATUS);
-	pFrame->pwAid = (unsigned short *)
-			(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			 WLAN_ASSOCRESP_OFF_AID);
-	pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_ASSOCRESP_OFF_AID +
-		      sizeof(*(pFrame->pwAid));
-}
-
-/*+
- *
- * Routine Description:
- *  Decode Association Response
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrDecodeAssocResponse(
-	PWLAN_FR_ASSOCRESP  pFrame
-)
-{
-	PWLAN_IE   pItem;
-
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-	/* Fixed Fields */
-	pFrame->pwCapInfo = (unsigned short *)
-			    (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			     WLAN_ASSOCRESP_OFF_CAP_INFO);
-	pFrame->pwStatus = (unsigned short *)
-			   (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			    WLAN_ASSOCRESP_OFF_STATUS);
-	pFrame->pwAid = (unsigned short *)
-			(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			 WLAN_ASSOCRESP_OFF_AID);
-
-	/* Information elements */
-	pFrame->pSuppRates  = (PWLAN_IE_SUPP_RATES)
-			      (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			       WLAN_ASSOCRESP_OFF_SUPP_RATES);
-
-	pItem = (PWLAN_IE)(pFrame->pSuppRates);
-	pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 + pItem->len);
-
-	if ((((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) &&
-	    (pItem->byElementID == WLAN_EID_EXTSUPP_RATES)) {
-		pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
-		pr_debug("pFrame->pExtSuppRates=[%p]\n", pItem);
-	} else {
-		pFrame->pExtSuppRates = NULL;
-	}
-}
-
-/*+
- *
- * Routine Description:
- *  Encode Reassociation Request
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrEncodeReassocRequest(
-	PWLAN_FR_REASSOCREQ  pFrame
-)
-{
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-	/* Fixed Fields */
-	pFrame->pwCapInfo = (unsigned short *)
-			    (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			     WLAN_REASSOCREQ_OFF_CAP_INFO);
-	pFrame->pwListenInterval = (unsigned short *)
-				   (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-				    WLAN_REASSOCREQ_OFF_LISTEN_INT);
-	pFrame->pAddrCurrAP = (PIEEE_ADDR)
-			      (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			       WLAN_REASSOCREQ_OFF_CURR_AP);
-	pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_REASSOCREQ_OFF_CURR_AP +
-		      sizeof(*(pFrame->pAddrCurrAP));
-}
-
-/*+
- *
- * Routine Description: (AP)
- *  Decode Reassociation Request
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrDecodeReassocRequest(
-	PWLAN_FR_REASSOCREQ  pFrame
-)
-{
-	PWLAN_IE   pItem;
-
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-	/* Fixed Fields */
-	pFrame->pwCapInfo = (unsigned short *)
-			    (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			     WLAN_REASSOCREQ_OFF_CAP_INFO);
-	pFrame->pwListenInterval = (unsigned short *)
-				   (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-				    WLAN_REASSOCREQ_OFF_LISTEN_INT);
-	pFrame->pAddrCurrAP = (PIEEE_ADDR)
-			      (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			       WLAN_REASSOCREQ_OFF_CURR_AP);
-
-	/* Information elements */
-	pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-			   + WLAN_REASSOCREQ_OFF_SSID);
-
-	while (((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) {
-		switch (pItem->byElementID) {
-		case WLAN_EID_SSID:
-			if (pFrame->pSSID == NULL)
-				pFrame->pSSID = (PWLAN_IE_SSID)pItem;
-			break;
-		case WLAN_EID_SUPP_RATES:
-			if (pFrame->pSuppRates == NULL)
-				pFrame->pSuppRates =
-						    (PWLAN_IE_SUPP_RATES)pItem;
-			break;
-
-		case WLAN_EID_RSN:
-			if (pFrame->pRSN == NULL)
-				pFrame->pRSN = (PWLAN_IE_RSN)pItem;
-			break;
-		case WLAN_EID_RSN_WPA:
-			if (pFrame->pRSNWPA == NULL) {
-				if (WPAb_Is_RSN((PWLAN_IE_RSN_EXT)pItem) == true)
-					pFrame->pRSNWPA =
-						       (PWLAN_IE_RSN_EXT)pItem;
-			}
-			break;
-
-		case WLAN_EID_EXTSUPP_RATES:
-			if (pFrame->pExtSuppRates == NULL)
-				pFrame->pExtSuppRates =
-						    (PWLAN_IE_SUPP_RATES)pItem;
-			break;
-		default:
-			pr_debug("Unrecognized EID=%dd in reassocreq decode\n",
-				 pItem->byElementID);
-			break;
-		}
-		pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 + pItem->len);
-	}
-}
-
-/*+
- *
- * Routine Description:
- *  Encode Probe Request
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrEncodeProbeRequest(
-	PWLAN_FR_PROBEREQ  pFrame
-)
-{
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-	pFrame->len = WLAN_HDR_ADDR3_LEN;
-}
-
-/*+
- *
- * Routine Description:
- *  Decode Probe Request
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrDecodeProbeRequest(
-	PWLAN_FR_PROBEREQ  pFrame
-)
-{
-	PWLAN_IE   pItem;
-
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-	/* Information elements */
-	pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)));
-
-	while (((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) {
-		switch (pItem->byElementID) {
-		case WLAN_EID_SSID:
-			if (pFrame->pSSID == NULL)
-				pFrame->pSSID = (PWLAN_IE_SSID)pItem;
-			break;
-
-		case WLAN_EID_SUPP_RATES:
-			if (pFrame->pSuppRates == NULL)
-				pFrame->pSuppRates =
-						   (PWLAN_IE_SUPP_RATES)pItem;
-			break;
-
-		case WLAN_EID_EXTSUPP_RATES:
-			if (pFrame->pExtSuppRates == NULL)
-				pFrame->pExtSuppRates =
-						    (PWLAN_IE_SUPP_RATES)pItem;
-			break;
-
-		default:
-			pr_debug("Bad EID=%dd in probereq\n",
-				 pItem->byElementID);
-			break;
-		}
-
-		pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 +  pItem->len);
-	}
-}
-
-/*+
- *
- * Routine Description:
- *  Encode Probe Response
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrEncodeProbeResponse(
-	PWLAN_FR_PROBERESP  pFrame
-)
-{
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-	/* Fixed Fields */
-	pFrame->pqwTimestamp = (__le64 *)
-			       (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-				WLAN_PROBERESP_OFF_TS);
-	pFrame->pwBeaconInterval = (unsigned short *)
-				   (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-				    WLAN_PROBERESP_OFF_BCN_INT);
-	pFrame->pwCapInfo = (unsigned short *)
-			    (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			     WLAN_PROBERESP_OFF_CAP_INFO);
-
-	pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_PROBERESP_OFF_CAP_INFO +
-		      sizeof(*(pFrame->pwCapInfo));
-}
-
-/*+
- *
- * Routine Description:
- *  Decode Probe Response
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrDecodeProbeResponse(
-	PWLAN_FR_PROBERESP  pFrame
-)
-{
-	PWLAN_IE    pItem;
-
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-	/* Fixed Fields */
-	pFrame->pqwTimestamp = (__le64 *)
-			       (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-				WLAN_PROBERESP_OFF_TS);
-	pFrame->pwBeaconInterval = (unsigned short *)
-				   (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-				    WLAN_PROBERESP_OFF_BCN_INT);
-	pFrame->pwCapInfo = (unsigned short *)
-			    (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			     WLAN_PROBERESP_OFF_CAP_INFO);
-
-	/* Information elements */
-	pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-			   + WLAN_PROBERESP_OFF_SSID);
-
-	while (((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) {
-		switch (pItem->byElementID) {
-		case WLAN_EID_SSID:
-			if (pFrame->pSSID == NULL)
-				pFrame->pSSID = (PWLAN_IE_SSID)pItem;
-			break;
-		case WLAN_EID_SUPP_RATES:
-			if (pFrame->pSuppRates == NULL)
-				pFrame->pSuppRates =
-						   (PWLAN_IE_SUPP_RATES)pItem;
-			break;
-		case WLAN_EID_FH_PARMS:
-			break;
-		case WLAN_EID_DS_PARMS:
-			if (pFrame->pDSParms == NULL)
-				pFrame->pDSParms = (PWLAN_IE_DS_PARMS)pItem;
-			break;
-		case WLAN_EID_CF_PARMS:
-			if (pFrame->pCFParms == NULL)
-				pFrame->pCFParms = (PWLAN_IE_CF_PARMS)pItem;
-			break;
-		case WLAN_EID_IBSS_PARMS:
-			if (pFrame->pIBSSParms == NULL)
-				pFrame->pIBSSParms =
-						   (PWLAN_IE_IBSS_PARMS)pItem;
-			break;
-
-		case WLAN_EID_RSN:
-			if (pFrame->pRSN == NULL)
-				pFrame->pRSN = (PWLAN_IE_RSN)pItem;
-			break;
-		case WLAN_EID_RSN_WPA:
-			if (pFrame->pRSNWPA == NULL) {
-				if (WPAb_Is_RSN((PWLAN_IE_RSN_EXT)pItem) == true)
-					pFrame->pRSNWPA =
-						       (PWLAN_IE_RSN_EXT)pItem;
-			}
-			break;
-		case WLAN_EID_ERP:
-			if (pFrame->pERP == NULL)
-				pFrame->pERP = (PWLAN_IE_ERP)pItem;
-			break;
-		case WLAN_EID_EXTSUPP_RATES:
-			if (pFrame->pExtSuppRates == NULL)
-				pFrame->pExtSuppRates =
-						    (PWLAN_IE_SUPP_RATES)pItem;
-			break;
-
-		case WLAN_EID_COUNTRY:      /* 7 */
-			if (pFrame->pIE_Country == NULL)
-				pFrame->pIE_Country = (PWLAN_IE_COUNTRY)pItem;
-			break;
-
-		case WLAN_EID_PWR_CONSTRAINT:   /* 32 */
-			if (pFrame->pIE_PowerConstraint == NULL)
-				pFrame->pIE_PowerConstraint =
-						      (PWLAN_IE_PW_CONST)pItem;
-			break;
-
-		case WLAN_EID_CH_SWITCH:    /* 37 */
-			if (pFrame->pIE_CHSW == NULL)
-				pFrame->pIE_CHSW = (PWLAN_IE_CH_SW)pItem;
-			break;
-
-		case WLAN_EID_QUIET:        /* 40 */
-			if (pFrame->pIE_Quiet == NULL)
-				pFrame->pIE_Quiet = (PWLAN_IE_QUIET)pItem;
-			break;
-
-		case WLAN_EID_IBSS_DFS:
-			if (pFrame->pIE_IBSSDFS == NULL)
-				pFrame->pIE_IBSSDFS = (PWLAN_IE_IBSS_DFS)pItem;
-			break;
-
-		default:
-			pr_debug("Bad EID=%dd in proberesp\n",
-				 pItem->byElementID);
-			break;
-		}
-
-		pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 +  pItem->len);
-	}
-}
-
-/*+
- *
- * Routine Description:
- *     Encode Authentication frame
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrEncodeAuthen(
-	PWLAN_FR_AUTHEN  pFrame
-)
-{
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-	/* Fixed Fields */
-	pFrame->pwAuthAlgorithm = (unsigned short *)
-				  (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-				   WLAN_AUTHEN_OFF_AUTH_ALG);
-	pFrame->pwAuthSequence = (unsigned short *)
-				 (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-				  WLAN_AUTHEN_OFF_AUTH_SEQ);
-	pFrame->pwStatus = (unsigned short *)
-			   (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			    WLAN_AUTHEN_OFF_STATUS);
-	pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_AUTHEN_OFF_STATUS +
-		      sizeof(*(pFrame->pwStatus));
-}
-
-/*+
- *
- * Routine Description:
- *   Decode Authentication
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrDecodeAuthen(
-	PWLAN_FR_AUTHEN  pFrame
-)
-{
-	PWLAN_IE    pItem;
-
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-	/* Fixed Fields */
-	pFrame->pwAuthAlgorithm = (unsigned short *)
-				  (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-				   WLAN_AUTHEN_OFF_AUTH_ALG);
-	pFrame->pwAuthSequence = (unsigned short *)
-				 (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-				  WLAN_AUTHEN_OFF_AUTH_SEQ);
-	pFrame->pwStatus = (unsigned short *)
-			   (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			    WLAN_AUTHEN_OFF_STATUS);
-
-	/* Information elements */
-	pItem = (PWLAN_IE)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
-			   + WLAN_AUTHEN_OFF_CHALLENGE);
-
-	if (((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len) &&
-	    pItem->byElementID == WLAN_EID_CHALLENGE)
-		pFrame->pChallenge = (PWLAN_IE_CHALLENGE)pItem;
-}
-
-/*+
- *
- * Routine Description:
- *   Encode Authentication
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrEncodeDeauthen(
-	PWLAN_FR_DEAUTHEN  pFrame
-)
-{
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-	/* Fixed Fields */
-	pFrame->pwReason = (unsigned short *)
-			   (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			    WLAN_DEAUTHEN_OFF_REASON);
-	pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_DEAUTHEN_OFF_REASON +
-		      sizeof(*(pFrame->pwReason));
-}
-
-/*+
- *
- * Routine Description:
- *   Decode Deauthentication
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrDecodeDeauthen(
-	PWLAN_FR_DEAUTHEN  pFrame
-)
-{
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-	/* Fixed Fields */
-	pFrame->pwReason = (unsigned short *)
-			   (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			    WLAN_DEAUTHEN_OFF_REASON);
-}
-
-/*+
- *
- * Routine Description: (AP)
- *   Encode Reassociation Response
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrEncodeReassocResponse(
-	PWLAN_FR_REASSOCRESP  pFrame
-)
-{
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-	/* Fixed Fields */
-	pFrame->pwCapInfo = (unsigned short *)
-			    (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			     WLAN_REASSOCRESP_OFF_CAP_INFO);
-	pFrame->pwStatus = (unsigned short *)
-			   (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			    WLAN_REASSOCRESP_OFF_STATUS);
-	pFrame->pwAid = (unsigned short *)
-			(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			 WLAN_REASSOCRESP_OFF_AID);
-
-	pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_REASSOCRESP_OFF_AID +
-		      sizeof(*(pFrame->pwAid));
-}
-
-/*+
- *
- * Routine Description:
- *   Decode Reassociation Response
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrDecodeReassocResponse(
-	PWLAN_FR_REASSOCRESP  pFrame
-)
-{
-	PWLAN_IE   pItem;
-
-	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-	/* Fixed Fields */
-	pFrame->pwCapInfo = (unsigned short *)
-			    (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			     WLAN_REASSOCRESP_OFF_CAP_INFO);
-	pFrame->pwStatus = (unsigned short *)
-			   (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			    WLAN_REASSOCRESP_OFF_STATUS);
-	pFrame->pwAid = (unsigned short *)
-			(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			 WLAN_REASSOCRESP_OFF_AID);
-
-	/* Information elements */
-	pFrame->pSuppRates = (PWLAN_IE_SUPP_RATES)
-			     (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
-			      WLAN_REASSOCRESP_OFF_SUPP_RATES);
-
-	pItem = (PWLAN_IE)(pFrame->pSuppRates);
-	pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 + pItem->len);
-
-	if ((((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) &&
-	    (pItem->byElementID == WLAN_EID_EXTSUPP_RATES)) {
-		pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
-	}
-}
diff --git a/drivers/staging/vt6655/80211mgr.h b/drivers/staging/vt6655/80211mgr.h
deleted file mode 100644
index d462a8af087b..000000000000
--- a/drivers/staging/vt6655/80211mgr.h
+++ /dev/null
@@ -1,725 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: 80211mgr.h
- *
- * Purpose: 802.11 management frames pre-defines.
- *
- *
- * Author: Lyndon Chen
- *
- * Date: May 8, 2002
- *
- */
-
-#ifndef __80211MGR_H__
-#define __80211MGR_H__
-
-#include <linux/types.h>
-#include "linux/ieee80211.h"
-
-#include "ttype.h"
-#include "80211hdr.h"
-
-#define WLAN_MIN_ARRAY          1
-
-/* Information Element ID value */
-#define WLAN_EID_FH_PARMS       2
-#define WLAN_EID_DS_PARMS       3
-#define WLAN_EID_CF_PARMS       4
-#define WLAN_EID_IBSS_PARMS     6
-#define WLAN_EID_TPC_REQ        34
-#define WLAN_EID_TPC_REP        35
-#define WLAN_EID_SUPP_CH        36
-#define WLAN_EID_CH_SWITCH      37
-#define WLAN_EID_MEASURE_REQ    38
-#define WLAN_EID_MEASURE_REP    39
-#define WLAN_EID_QUIET          40
-#define WLAN_EID_IBSS_DFS       41
-#define WLAN_EID_ERP            42
-/* reference 802.11i 7.3.2 table 20 */
-#define WLAN_EID_EXTSUPP_RATES  50
-/* reference WiFi WPA spec. */
-#define WLAN_EID_RSN_WPA        221
-
-#define WLAN_EID_ERP_NONERP_PRESENT             0x01
-#define WLAN_EID_ERP_USE_PROTECTION             0x02
-#define WLAN_EID_ERP_BARKER_MODE                0x04
-
-/* Reason Codes */
-#define WLAN_MGMT_REASON_RSVD                       0
-#define WLAN_MGMT_REASON_UNSPEC                     1
-#define WLAN_MGMT_REASON_PRIOR_AUTH_INVALID         2
-#define WLAN_MGMT_REASON_DEAUTH_LEAVING             3
-#define WLAN_MGMT_REASON_DISASSOC_INACTIVE          4
-#define WLAN_MGMT_REASON_DISASSOC_AP_BUSY           5
-#define WLAN_MGMT_REASON_CLASS2_NONAUTH             6
-#define WLAN_MGMT_REASON_CLASS3_NONASSOC            7
-#define WLAN_MGMT_REASON_DISASSOC_STA_HASLEFT       8
-#define WLAN_MGMT_REASON_CANT_ASSOC_NONAUTH         9
-#define WLAN_MGMT_REASON_DISASSOC_PWR_CAP_UNACCEPT      10
-#define WLAN_MGMT_REASON_DISASSOC_SUPP_CH_UNACCEPT      11
-#define WLAN_MGMT_REASON_INVALID_IE                 13
-#define WLAN_MGMT_REASON_MIC_FAILURE                14
-#define WLAN_MGMT_REASON_4WAY_HANDSHAKE_TIMEOUT     15
-#define WLAN_MGMT_REASON_GRPKEY_UPDATE_TIMEOUT      16
-#define WLAN_MGMT_REASON_4WAY_INFO_DIFFERENT        17
-#define WLAN_MGMT_REASON_MULTCAST_CIPHER_INVALID    18
-#define WLAN_MGMT_REASON_UNCAST_CIPHER_INVALID      19
-#define WLAN_MGMT_REASON_AKMP_INVALID               20
-#define WLAN_MGMT_REASON_RSNE_UNSUPPORTED           21
-#define WLAN_MGMT_REASON_RSNE_CAP_INVALID           22
-#define WLAN_MGMT_REASON_80211X_AUTH_FAILED         23
-
-/* Status Codes */
-#define WLAN_MGMT_STATUS_SUCCESS                        0
-#define WLAN_MGMT_STATUS_UNSPEC_FAILURE                 1
-#define WLAN_MGMT_STATUS_CAPS_UNSUPPORTED               10
-#define WLAN_MGMT_STATUS_REASSOC_NO_ASSOC               11
-#define WLAN_MGMT_STATUS_ASSOC_DENIED_UNSPEC            12
-#define WLAN_MGMT_STATUS_UNSUPPORTED_AUTHALG            13
-#define WLAN_MGMT_STATUS_RX_AUTH_NOSEQ                  14
-#define WLAN_MGMT_STATUS_CHALLENGE_FAIL                 15
-#define WLAN_MGMT_STATUS_AUTH_TIMEOUT                   16
-#define WLAN_MGMT_STATUS_ASSOC_DENIED_BUSY              17
-#define WLAN_MGMT_STATUS_ASSOC_DENIED_RATES             18
-#define WLAN_MGMT_STATUS_ASSOC_DENIED_SHORTPREAMBLE     19
-#define WLAN_MGMT_STATUS_ASSOC_DENIED_PBCC              20
-#define WLAN_MGMT_STATUS_ASSOC_DENIED_AGILITY           21
-
-/* reference 802.11h 7.3.1.9 */
-#define WLAN_MGMT_STATUS_ASSOC_REJECT_BCS_SPECTRUM_MNG  22
-#define WLAN_MGMT_STATUS_ASSOC_REJECT_BCS_PWR_CAP       23
-#define WLAN_MGMT_STATUS_ASSOC_REJECT_BCS_SUPP_CH       24
-/* reference 802.11g 7.3.1.9 */
-#define WLAN_MGMT_STATUS_SHORTSLOTTIME_UNSUPPORTED      25
-#define WLAN_MGMT_STATUS_DSSSOFDM_UNSUPPORTED           26
-/* reference 802.11i 3.7.1.9 table 19 */
-#define WLAN_MGMT_STATUS_INVALID_IE                     40
-#define WLAN_MGMT_STATUS_GROUP_CIPHER_INVALID           41
-#define WLAN_MGMT_STATUS_PAIRWISE_CIPHER_INVALID        42
-#define WLAN_MGMT_STATUS_AKMP_INVALID                   43
-#define WLAN_MGMT_STATUS_UNSUPPORT_RSN_IE_VER           44
-#define WLAN_MGMT_STATUS_INVALID_RSN_IE_CAP             45
-#define WLAN_MGMT_STATUS_CIPHER_REJECT                  46
-
-/* Auth Algorithm */
-#define WLAN_AUTH_ALG_OPENSYSTEM                0
-#define WLAN_AUTH_ALG_SHAREDKEY                 1
-
-/* Management Frame Field Offsets */
-/* Note: Not all fields are listed because of variable lengths. */
-/* Note: These offsets are from the start of the frame data */
-
-#define WLAN_BEACON_OFF_TS                  0
-#define WLAN_BEACON_OFF_BCN_INT             8
-#define WLAN_BEACON_OFF_CAPINFO             10
-#define WLAN_BEACON_OFF_SSID                12
-
-#define WLAN_DISASSOC_OFF_REASON            0
-
-#define WLAN_ASSOCREQ_OFF_CAP_INFO          0
-#define WLAN_ASSOCREQ_OFF_LISTEN_INT        2
-#define WLAN_ASSOCREQ_OFF_SSID              4
-
-#define WLAN_ASSOCRESP_OFF_CAP_INFO         0
-#define WLAN_ASSOCRESP_OFF_STATUS           2
-#define WLAN_ASSOCRESP_OFF_AID              4
-#define WLAN_ASSOCRESP_OFF_SUPP_RATES       6
-
-#define WLAN_REASSOCREQ_OFF_CAP_INFO        0
-#define WLAN_REASSOCREQ_OFF_LISTEN_INT      2
-#define WLAN_REASSOCREQ_OFF_CURR_AP         4
-#define WLAN_REASSOCREQ_OFF_SSID            10
-
-#define WLAN_REASSOCRESP_OFF_CAP_INFO       0
-#define WLAN_REASSOCRESP_OFF_STATUS         2
-#define WLAN_REASSOCRESP_OFF_AID            4
-#define WLAN_REASSOCRESP_OFF_SUPP_RATES     6
-
-#define WLAN_PROBEREQ_OFF_SSID              0
-
-#define WLAN_PROBERESP_OFF_TS               0
-#define WLAN_PROBERESP_OFF_BCN_INT          8
-#define WLAN_PROBERESP_OFF_CAP_INFO         10
-#define WLAN_PROBERESP_OFF_SSID             12
-
-#define WLAN_AUTHEN_OFF_AUTH_ALG            0
-#define WLAN_AUTHEN_OFF_AUTH_SEQ            2
-#define WLAN_AUTHEN_OFF_STATUS              4
-#define WLAN_AUTHEN_OFF_CHALLENGE           6
-
-#define WLAN_DEAUTHEN_OFF_REASON            0
-
-/* Cipher Suite Selectors defined in 802.11i */
-#define WLAN_11i_CSS_USE_GROUP              0
-#define WLAN_11i_CSS_WEP40                  1
-#define WLAN_11i_CSS_TKIP                   2
-#define WLAN_11i_CSS_CCMP                   4
-#define WLAN_11i_CSS_WEP104                 5
-#define WLAN_11i_CSS_UNKNOWN                255
-
-/* Authentication and Key Management Suite Selectors defined in 802.11i */
-#define WLAN_11i_AKMSS_802_1X               1
-#define WLAN_11i_AKMSS_PSK                  2
-#define WLAN_11i_AKMSS_UNKNOWN              255
-
-/* Measurement type definitions reference ieee 802.11h Table 20b */
-#define MEASURE_TYPE_BASIC      0
-#define MEASURE_TYPE_CCA        1
-#define MEASURE_TYPE_RPI        2
-
-/* Measurement request mode definitions reference ieee 802.11h Figure 46h */
-#define MEASURE_MODE_ENABLE     0x02
-#define MEASURE_MODE_REQ        0x04
-#define MEASURE_MODE_REP        0x08
-
-/* Measurement report mode definitions reference ieee 802.11h Figure 46m */
-#define MEASURE_MODE_LATE       0x01
-#define MEASURE_MODE_INCAPABLE  0x02
-#define MEASURE_MODE_REFUSED    0x04
-
-/* Information Element Types */
-
-#pragma pack(1)
-typedef struct tagWLAN_IE {
-	unsigned char byElementID;
-	unsigned char len;
-} __attribute__ ((__packed__))
-WLAN_IE, *PWLAN_IE;
-
-/* Service Set Identity (SSID) */
-#pragma pack(1)
-typedef struct tagWLAN_IE_SSID {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned char abySSID[1];
-} __attribute__ ((__packed__))
-WLAN_IE_SSID, *PWLAN_IE_SSID;
-
-/* Supported Rates */
-#pragma pack(1)
-typedef struct tagWLAN_IE_SUPP_RATES {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned char abyRates[1];
-} __attribute__ ((__packed__))
-WLAN_IE_SUPP_RATES,  *PWLAN_IE_SUPP_RATES;
-
-/* FH Parameter Set */
-#pragma pack(1)
-typedef struct _WLAN_IE_FH_PARMS {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned short wDwellTime;
-	unsigned char byHopSet;
-	unsigned char byHopPattern;
-	unsigned char byHopIndex;
-} WLAN_IE_FH_PARMS,  *PWLAN_IE_FH_PARMS;
-
-/* DS Parameter Set */
-#pragma pack(1)
-typedef struct tagWLAN_IE_DS_PARMS {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned char byCurrChannel;
-} __attribute__ ((__packed__))
-WLAN_IE_DS_PARMS,  *PWLAN_IE_DS_PARMS;
-
-/* CF Parameter Set */
-#pragma pack(1)
-typedef struct tagWLAN_IE_CF_PARMS {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned char byCFPCount;
-	unsigned char byCFPPeriod;
-	unsigned short wCFPMaxDuration;
-	unsigned short wCFPDurRemaining;
-} __attribute__ ((__packed__))
-WLAN_IE_CF_PARMS,  *PWLAN_IE_CF_PARMS;
-
-/* TIM */
-#pragma pack(1)
-typedef struct tagWLAN_IE_TIM {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned char byDTIMCount;
-	unsigned char byDTIMPeriod;
-	unsigned char byBitMapCtl;
-	unsigned char byVirtBitMap[1];
-} __attribute__ ((__packed__))
-WLAN_IE_TIM,  *PWLAN_IE_TIM;
-
-/* IBSS Parameter Set */
-#pragma pack(1)
-typedef struct tagWLAN_IE_IBSS_PARMS {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned short wATIMWindow;
-} __attribute__ ((__packed__))
-WLAN_IE_IBSS_PARMS, *PWLAN_IE_IBSS_PARMS;
-
-/* Challenge Text */
-#pragma pack(1)
-typedef struct tagWLAN_IE_CHALLENGE {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned char abyChallenge[1];
-} __attribute__ ((__packed__))
-WLAN_IE_CHALLENGE,  *PWLAN_IE_CHALLENGE;
-
-#pragma pack(1)
-typedef struct tagWLAN_IE_RSN_EXT {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned char abyOUI[4];
-	unsigned short wVersion;
-	unsigned char abyMulticast[4];
-	unsigned short wPKCount;
-	struct {
-		unsigned char abyOUI[4];
-	} PKSList[1]; /* the rest is variable so need to */
-	/* overlay ieauth structure */
-} WLAN_IE_RSN_EXT, *PWLAN_IE_RSN_EXT;
-
-#pragma pack(1)
-typedef struct tagWLAN_IE_RSN_AUTH {
-	unsigned short wAuthCount;
-	struct {
-		unsigned char abyOUI[4];
-	} AuthKSList[1];
-} WLAN_IE_RSN_AUTH, *PWLAN_IE_RSN_AUTH;
-
-/* RSN Identity */
-#pragma pack(1)
-typedef struct tagWLAN_IE_RSN {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned short wVersion;
-	unsigned char abyRSN[WLAN_MIN_ARRAY];
-} WLAN_IE_RSN, *PWLAN_IE_RSN;
-
-/* ERP */
-#pragma pack(1)
-typedef struct tagWLAN_IE_ERP {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned char byContext;
-} __attribute__ ((__packed__))
-WLAN_IE_ERP,  *PWLAN_IE_ERP;
-
-#pragma pack(1)
-typedef struct _MEASEURE_REQ {
-	unsigned char byChannel;
-	unsigned char abyStartTime[8];
-	unsigned char abyDuration[2];
-} MEASEURE_REQ, *PMEASEURE_REQ,
-	MEASEURE_REQ_BASIC, *PMEASEURE_REQ_BASIC,
-	MEASEURE_REQ_CCA, *PMEASEURE_REQ_CCA,
-	MEASEURE_REQ_RPI, *PMEASEURE_REQ_RPI;
-
-typedef struct _MEASEURE_REP_BASIC {
-	unsigned char byChannel;
-	unsigned char abyStartTime[8];
-	unsigned char abyDuration[2];
-	unsigned char byMap;
-} MEASEURE_REP_BASIC, *PMEASEURE_REP_BASIC;
-
-typedef struct _MEASEURE_REP_CCA {
-	unsigned char byChannel;
-	unsigned char abyStartTime[8];
-	unsigned char abyDuration[2];
-	unsigned char byCCABusyFraction;
-} MEASEURE_REP_CCA, *PMEASEURE_REP_CCA;
-
-typedef struct _MEASEURE_REP_RPI {
-	unsigned char byChannel;
-	unsigned char abyStartTime[8];
-	unsigned char abyDuration[2];
-	unsigned char abyRPIdensity[8];
-} MEASEURE_REP_RPI, *PMEASEURE_REP_RPI;
-
-typedef union _MEASEURE_REP {
-	MEASEURE_REP_BASIC  sBasic;
-	MEASEURE_REP_CCA    sCCA;
-	MEASEURE_REP_RPI    sRPI;
-} MEASEURE_REP, *PMEASEURE_REP;
-
-typedef struct _WLAN_IE_MEASURE_REQ {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned char byToken;
-	unsigned char byMode;
-	unsigned char byType;
-	MEASEURE_REQ        sReq;
-} WLAN_IE_MEASURE_REQ, *PWLAN_IE_MEASURE_REQ;
-
-typedef struct _WLAN_IE_MEASURE_REP {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned char byToken;
-	unsigned char byMode;
-	unsigned char byType;
-	MEASEURE_REP        sRep;
-} WLAN_IE_MEASURE_REP, *PWLAN_IE_MEASURE_REP;
-
-typedef struct _WLAN_IE_CH_SW {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned char byMode;
-	unsigned char byChannel;
-	unsigned char byCount;
-} WLAN_IE_CH_SW, *PWLAN_IE_CH_SW;
-
-typedef struct _WLAN_IE_QUIET {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned char byQuietCount;
-	unsigned char byQuietPeriod;
-	unsigned char abyQuietDuration[2];
-	unsigned char abyQuietOffset[2];
-} WLAN_IE_QUIET, *PWLAN_IE_QUIET;
-
-typedef struct _WLAN_IE_COUNTRY {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned char abyCountryString[3];
-	unsigned char abyCountryInfo[3];
-} WLAN_IE_COUNTRY, *PWLAN_IE_COUNTRY;
-
-typedef struct _WLAN_IE_PW_CONST {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned char byPower;
-} WLAN_IE_PW_CONST, *PWLAN_IE_PW_CONST;
-
-typedef struct _WLAN_IE_PW_CAP {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned char byMinPower;
-	unsigned char byMaxPower;
-} WLAN_IE_PW_CAP, *PWLAN_IE_PW_CAP;
-
-typedef struct _WLAN_IE_SUPP_CH {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned char abyChannelTuple[2];
-} WLAN_IE_SUPP_CH, *PWLAN_IE_SUPP_CH;
-
-typedef struct _WLAN_IE_TPC_REQ {
-	unsigned char byElementID;
-	unsigned char len;
-} WLAN_IE_TPC_REQ, *PWLAN_IE_TPC_REQ;
-
-typedef struct _WLAN_IE_TPC_REP {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned char byTxPower;
-	unsigned char byLinkMargin;
-} WLAN_IE_TPC_REP, *PWLAN_IE_TPC_REP;
-
-typedef struct _WLAN_IE_IBSS_DFS {
-	unsigned char byElementID;
-	unsigned char len;
-	unsigned char abyDFSOwner[6];
-	unsigned char byDFSRecovery;
-	unsigned char abyChannelMap[2];
-} WLAN_IE_IBSS_DFS, *PWLAN_IE_IBSS_DFS;
-
-#pragma pack()
-
-/* Frame Types */
-/* prototype structure, all mgmt frame types will start with these members */
-typedef struct tagWLAN_FR_MGMT {
-	unsigned int	uType;
-	unsigned int	len;
-	unsigned char *pBuf;
-	PUWLAN_80211HDR       pHdr;
-} WLAN_FR_MGMT,  *PWLAN_FR_MGMT;
-
-/* Beacon frame */
-typedef struct tagWLAN_FR_BEACON {
-	unsigned int	uType;
-	unsigned int	len;
-	unsigned char *pBuf;
-	PUWLAN_80211HDR         pHdr;
-	__le64 *pqwTimestamp;
-	unsigned short *pwBeaconInterval;
-	unsigned short *pwCapInfo;
-	PWLAN_IE_SSID           pSSID;
-	PWLAN_IE_SUPP_RATES     pSuppRates;
-	PWLAN_IE_DS_PARMS       pDSParms;
-	PWLAN_IE_CF_PARMS       pCFParms;
-	PWLAN_IE_TIM            pTIM;
-	PWLAN_IE_IBSS_PARMS     pIBSSParms;
-	PWLAN_IE_RSN            pRSN;
-	PWLAN_IE_RSN_EXT        pRSNWPA;
-	PWLAN_IE_ERP            pERP;
-	PWLAN_IE_SUPP_RATES     pExtSuppRates;
-	PWLAN_IE_COUNTRY        pIE_Country;
-	PWLAN_IE_PW_CONST       pIE_PowerConstraint;
-	PWLAN_IE_CH_SW          pIE_CHSW;
-	PWLAN_IE_IBSS_DFS       pIE_IBSSDFS;
-	PWLAN_IE_QUIET          pIE_Quiet;
-} WLAN_FR_BEACON, *PWLAN_FR_BEACON;
-
-/* IBSS ATIM frame */
-typedef struct tagWLAN_FR_IBSSATIM {
-	unsigned int	uType;
-	unsigned int	len;
-	unsigned char *pBuf;
-	PUWLAN_80211HDR         pHdr;
-} WLAN_FR_IBSSATIM, *PWLAN_FR_IBSSATIM;
-
-/* Disassociation */
-typedef struct tagWLAN_FR_DISASSOC {
-	unsigned int	uType;
-	unsigned int	len;
-	unsigned char *pBuf;
-	PUWLAN_80211HDR         pHdr;
-	unsigned short *pwReason;
-} WLAN_FR_DISASSOC, *PWLAN_FR_DISASSOC;
-
-/* Association Request */
-typedef struct tagWLAN_FR_ASSOCREQ {
-	unsigned int	uType;
-	unsigned int	len;
-	unsigned char *pBuf;
-	PUWLAN_80211HDR         pHdr;
-	unsigned short *pwCapInfo;
-	unsigned short *pwListenInterval;
-	PWLAN_IE_SSID           pSSID;
-	PWLAN_IE_SUPP_RATES     pSuppRates;
-	PWLAN_IE_RSN            pRSN;
-	PWLAN_IE_RSN_EXT        pRSNWPA;
-	PWLAN_IE_SUPP_RATES     pExtSuppRates;
-	PWLAN_IE_PW_CAP         pCurrPowerCap;
-	PWLAN_IE_SUPP_CH        pCurrSuppCh;
-} WLAN_FR_ASSOCREQ, *PWLAN_FR_ASSOCREQ;
-
-/* Association Response */
-typedef struct tagWLAN_FR_ASSOCRESP {
-	unsigned int	uType;
-	unsigned int	len;
-	unsigned char *pBuf;
-	PUWLAN_80211HDR         pHdr;
-	unsigned short *pwCapInfo;
-	unsigned short *pwStatus;
-	unsigned short *pwAid;
-	PWLAN_IE_SUPP_RATES     pSuppRates;
-	PWLAN_IE_SUPP_RATES     pExtSuppRates;
-} WLAN_FR_ASSOCRESP, *PWLAN_FR_ASSOCRESP;
-
-/* Reassociation Request */
-typedef struct tagWLAN_FR_REASSOCREQ {
-	unsigned int	uType;
-	unsigned int	len;
-	unsigned char *pBuf;
-	PUWLAN_80211HDR         pHdr;
-	unsigned short *pwCapInfo;
-	unsigned short *pwListenInterval;
-	PIEEE_ADDR              pAddrCurrAP;
-	PWLAN_IE_SSID           pSSID;
-	PWLAN_IE_SUPP_RATES     pSuppRates;
-	PWLAN_IE_RSN            pRSN;
-	PWLAN_IE_RSN_EXT        pRSNWPA;
-	PWLAN_IE_SUPP_RATES     pExtSuppRates;
-} WLAN_FR_REASSOCREQ, *PWLAN_FR_REASSOCREQ;
-
-/* Reassociation Response */
-typedef struct tagWLAN_FR_REASSOCRESP {
-	unsigned int	uType;
-	unsigned int	len;
-	unsigned char *pBuf;
-	PUWLAN_80211HDR         pHdr;
-	unsigned short *pwCapInfo;
-	unsigned short *pwStatus;
-	unsigned short *pwAid;
-	PWLAN_IE_SUPP_RATES     pSuppRates;
-	PWLAN_IE_SUPP_RATES     pExtSuppRates;
-} WLAN_FR_REASSOCRESP, *PWLAN_FR_REASSOCRESP;
-
-/* Probe Request */
-typedef struct tagWLAN_FR_PROBEREQ {
-	unsigned int	uType;
-	unsigned int	len;
-	unsigned char *pBuf;
-	PUWLAN_80211HDR         pHdr;
-	PWLAN_IE_SSID           pSSID;
-	PWLAN_IE_SUPP_RATES     pSuppRates;
-	PWLAN_IE_SUPP_RATES     pExtSuppRates;
-} WLAN_FR_PROBEREQ, *PWLAN_FR_PROBEREQ;
-
-/* Probe Response */
-typedef struct tagWLAN_FR_PROBERESP {
-	unsigned int	uType;
-	unsigned int	len;
-	unsigned char *pBuf;
-	PUWLAN_80211HDR         pHdr;
-	__le64 *pqwTimestamp;
-	unsigned short *pwBeaconInterval;
-	unsigned short *pwCapInfo;
-	PWLAN_IE_SSID           pSSID;
-	PWLAN_IE_SUPP_RATES     pSuppRates;
-	PWLAN_IE_DS_PARMS       pDSParms;
-	PWLAN_IE_CF_PARMS       pCFParms;
-	PWLAN_IE_IBSS_PARMS     pIBSSParms;
-	PWLAN_IE_RSN            pRSN;
-	PWLAN_IE_RSN_EXT        pRSNWPA;
-	PWLAN_IE_ERP            pERP;
-	PWLAN_IE_SUPP_RATES     pExtSuppRates;
-	PWLAN_IE_COUNTRY        pIE_Country;
-	PWLAN_IE_PW_CONST       pIE_PowerConstraint;
-	PWLAN_IE_CH_SW          pIE_CHSW;
-	PWLAN_IE_IBSS_DFS       pIE_IBSSDFS;
-	PWLAN_IE_QUIET          pIE_Quiet;
-} WLAN_FR_PROBERESP, *PWLAN_FR_PROBERESP;
-
-/* Authentication */
-typedef struct tagWLAN_FR_AUTHEN {
-	unsigned int	uType;
-	unsigned int	len;
-	unsigned char *pBuf;
-	PUWLAN_80211HDR         pHdr;
-	unsigned short *pwAuthAlgorithm;
-	unsigned short *pwAuthSequence;
-	unsigned short *pwStatus;
-	PWLAN_IE_CHALLENGE      pChallenge;
-} WLAN_FR_AUTHEN, *PWLAN_FR_AUTHEN;
-
-/* Deauthenication */
-typedef struct tagWLAN_FR_DEAUTHEN {
-	unsigned int	uType;
-	unsigned int	len;
-	unsigned char *pBuf;
-	PUWLAN_80211HDR         pHdr;
-	unsigned short *pwReason;
-} WLAN_FR_DEAUTHEN, *PWLAN_FR_DEAUTHEN;
-
-void
-vMgrEncodeBeacon(
-	PWLAN_FR_BEACON  pFrame
-);
-
-void
-vMgrDecodeBeacon(
-	PWLAN_FR_BEACON  pFrame
-);
-
-void
-vMgrEncodeIBSSATIM(
-	PWLAN_FR_IBSSATIM   pFrame
-);
-
-void
-vMgrDecodeIBSSATIM(
-	PWLAN_FR_IBSSATIM   pFrame
-);
-
-void
-vMgrEncodeDisassociation(
-	PWLAN_FR_DISASSOC  pFrame
-);
-
-void
-vMgrDecodeDisassociation(
-	PWLAN_FR_DISASSOC  pFrame
-);
-
-void
-vMgrEncodeAssocRequest(
-	PWLAN_FR_ASSOCREQ  pFrame
-);
-
-void
-vMgrDecodeAssocRequest(
-	PWLAN_FR_ASSOCREQ  pFrame
-);
-
-void
-vMgrEncodeAssocResponse(
-	PWLAN_FR_ASSOCRESP  pFrame
-);
-
-void
-vMgrDecodeAssocResponse(
-	PWLAN_FR_ASSOCRESP  pFrame
-);
-
-void
-vMgrEncodeReassocRequest(
-	PWLAN_FR_REASSOCREQ  pFrame
-);
-
-void
-vMgrDecodeReassocRequest(
-	PWLAN_FR_REASSOCREQ  pFrame
-);
-
-void
-vMgrEncodeProbeRequest(
-	PWLAN_FR_PROBEREQ  pFrame
-);
-
-void
-vMgrDecodeProbeRequest(
-	PWLAN_FR_PROBEREQ  pFrame
-);
-
-void
-vMgrEncodeProbeResponse(
-	PWLAN_FR_PROBERESP  pFrame
-);
-
-void
-vMgrDecodeProbeResponse(
-	PWLAN_FR_PROBERESP  pFrame
-);
-
-void
-vMgrEncodeAuthen(
-	PWLAN_FR_AUTHEN  pFrame
-);
-
-void
-vMgrDecodeAuthen(
-	PWLAN_FR_AUTHEN  pFrame
-);
-
-void
-vMgrEncodeDeauthen(
-	PWLAN_FR_DEAUTHEN  pFrame
-);
-
-void
-vMgrDecodeDeauthen(
-	PWLAN_FR_DEAUTHEN  pFrame
-);
-
-void
-vMgrEncodeReassocResponse(
-	PWLAN_FR_REASSOCRESP  pFrame
-);
-
-void
-vMgrDecodeReassocResponse(
-	PWLAN_FR_REASSOCRESP  pFrame
-);
-
-#endif/* __80211MGR_H__ */
diff --git a/drivers/staging/vt6655/IEEE11h.c b/drivers/staging/vt6655/IEEE11h.c
deleted file mode 100644
index 180a27cc74d7..000000000000
--- a/drivers/staging/vt6655/IEEE11h.c
+++ /dev/null
@@ -1,141 +0,0 @@
-/*
- * Copyright (c) 1996, 2005 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: IEEE11h.c
- *
- * Purpose:
- *
- * Functions:
- *
- * Revision History:
- *
- * Author: Yiching Chen
- *
- * Date: Mar. 31, 2005
- *
- */
-
-#include "ttype.h"
-#include "tmacro.h"
-#include "tether.h"
-#include "IEEE11h.h"
-#include "device.h"
-#include "wmgr.h"
-#include "rxtx.h"
-#include "channel.h"
-
-/*---------------------  Static Definitions -------------------------*/
-
-#pragma pack(1)
-
-typedef struct _WLAN_FRAME_ACTION {
-	WLAN_80211HDR_A3    Header;
-	unsigned char byCategory;
-	unsigned char byAction;
-	unsigned char abyVars[1];
-} WLAN_FRAME_ACTION, *PWLAN_FRAME_ACTION;
-
-typedef struct _WLAN_FRAME_MSRREQ {
-	WLAN_80211HDR_A3    Header;
-	unsigned char byCategory;
-	unsigned char byAction;
-	unsigned char byDialogToken;
-	WLAN_IE_MEASURE_REQ sMSRReqEIDs[1];
-} WLAN_FRAME_MSRREQ, *PWLAN_FRAME_MSRREQ;
-
-typedef struct _WLAN_FRAME_MSRREP {
-	WLAN_80211HDR_A3    Header;
-	unsigned char byCategory;
-	unsigned char byAction;
-	unsigned char byDialogToken;
-	WLAN_IE_MEASURE_REP sMSRRepEIDs[1];
-} WLAN_FRAME_MSRREP, *PWLAN_FRAME_MSRREP;
-
-typedef struct _WLAN_FRAME_TPCREQ {
-	WLAN_80211HDR_A3    Header;
-	unsigned char byCategory;
-	unsigned char byAction;
-	unsigned char byDialogToken;
-	WLAN_IE_TPC_REQ     sTPCReqEIDs;
-} WLAN_FRAME_TPCREQ, *PWLAN_FRAME_TPCREQ;
-
-typedef struct _WLAN_FRAME_TPCREP {
-	WLAN_80211HDR_A3    Header;
-	unsigned char byCategory;
-	unsigned char byAction;
-	unsigned char byDialogToken;
-	WLAN_IE_TPC_REP     sTPCRepEIDs;
-} WLAN_FRAME_TPCREP, *PWLAN_FRAME_TPCREP;
-
-#pragma pack()
-
-/* action field reference ieee 802.11h Table 20e */
-#define ACTION_MSRREQ       0
-#define ACTION_MSRREP       1
-#define ACTION_TPCREQ       2
-#define ACTION_TPCREP       3
-#define ACTION_CHSW         4
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-bool IEEE11hbMSRRepTx(void *pMgmtHandle)
-{
-	PSMgmtObject            pMgmt = (PSMgmtObject) pMgmtHandle;
-	PWLAN_FRAME_MSRREP      pMSRRep = (PWLAN_FRAME_MSRREP)
-		(pMgmt->abyCurrentMSRRep + sizeof(STxMgmtPacket));
-	size_t                    uLength = 0;
-	PSTxMgmtPacket          pTxPacket = NULL;
-
-	pTxPacket = (PSTxMgmtPacket)pMgmt->abyCurrentMSRRep;
-	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_A3FR_MAXLEN);
-	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket +
-						    sizeof(STxMgmtPacket));
-
-	pMSRRep->Header.wFrameCtl = (WLAN_SET_FC_FTYPE(WLAN_FTYPE_MGMT) |
-				     WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_ACTION)
-);
-
-	memcpy(pMSRRep->Header.abyAddr1, ((PWLAN_FRAME_MSRREQ)
-					  (pMgmt->abyCurrentMSRReq))->Header.abyAddr2, WLAN_ADDR_LEN);
-	memcpy(pMSRRep->Header.abyAddr2,
-	       CARDpGetCurrentAddress(pMgmt->pAdapter), WLAN_ADDR_LEN);
-	memcpy(pMSRRep->Header.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-
-	pMSRRep->byCategory = 0;
-	pMSRRep->byAction = 1;
-	pMSRRep->byDialogToken = ((PWLAN_FRAME_MSRREQ)
-				  (pMgmt->abyCurrentMSRReq))->byDialogToken;
-
-	uLength = pMgmt->uLengthOfRepEIDs + offsetof(WLAN_FRAME_MSRREP,
-						     sMSRRepEIDs);
-
-	pTxPacket->cbMPDULen = uLength;
-	pTxPacket->cbPayloadLen = uLength - WLAN_HDR_ADDR3_LEN;
-	if (csMgmt_xmit(pMgmt->pAdapter, pTxPacket) != CMD_STATUS_PENDING)
-		return false;
-	return true;
-}
diff --git a/drivers/staging/vt6655/IEEE11h.h b/drivers/staging/vt6655/IEEE11h.h
deleted file mode 100644
index 551922022b19..000000000000
--- a/drivers/staging/vt6655/IEEE11h.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * Copyright (c) 1996, 2005 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: IEEE11h.h
- *
- * Purpose: Defines the macros, types, and functions for dealing
- *          with IEEE 802.11h.
- *
- * Author: Yiching Chen
- *
- * Date: Mar. 31, 2005
- *
- */
-
-#ifndef __IEEE11h_H__
-#define __IEEE11h_H__
-
-#include "ttype.h"
-#include "80211hdr.h"
-#include "80211mgr.h"
-
-bool IEEE11hbMSRRepTx(
-	void *pMgmtHandle
-);
-
-#endif // __IEEE11h_H__
diff --git a/drivers/staging/vt6655/Kconfig b/drivers/staging/vt6655/Kconfig
index c3ba693a8cad..77cfc708c516 100644
--- a/drivers/staging/vt6655/Kconfig
+++ b/drivers/staging/vt6655/Kconfig
@@ -1,8 +1,6 @@
 config VT6655
    tristate "VIA Technologies VT6655 support"
-   depends on PCI && WLAN && m
-   select WIRELESS_EXT
-   select WEXT_PRIV
+   depends on PCI && MAC80211 && m
    ---help---
    This is a vendor-written driver for VIA VT6655.
 
diff --git a/drivers/staging/vt6655/Makefile b/drivers/staging/vt6655/Makefile
index f7544a6cb63e..115b951bf0d9 100644
--- a/drivers/staging/vt6655/Makefile
+++ b/drivers/staging/vt6655/Makefile
@@ -7,33 +7,12 @@ vt6655_stage-y +=	device_main.o \
 	channel.o \
 	mac.o \
 	baseband.o \
-	wctl.o \
-	80211mgr.o \
-	wcmd.o \
-	wmgr.o \
-	bssdb.o \
 	rxtx.o \
 	dpc.o \
 	power.o \
-	datarate.o \
 	srom.o \
 	mib.o \
-	rc4.o \
-	tether.o \
-	tcrc.o \
-	ioctl.o \
-	hostap.o \
-	wpa.o \
 	key.o \
-	tkip.o \
-	michael.o \
-	wroute.o \
-	rf.o \
-	iwctl.o \
-	wpactl.o \
-	wpa2.o \
-	aes_ccmp.o \
-	vntwifi.o \
-	IEEE11h.o
+	rf.o
 
 obj-$(CONFIG_VT6655) +=	vt6655_stage.o
diff --git a/drivers/staging/vt6655/aes_ccmp.c b/drivers/staging/vt6655/aes_ccmp.c
deleted file mode 100644
index 1dfcfcb3c69c..000000000000
--- a/drivers/staging/vt6655/aes_ccmp.c
+++ /dev/null
@@ -1,374 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: aes_ccmp.c
- *
- * Purpose: AES_CCMP decryption
- *
- * Author: Warren Hsu
- *
- * Date: Feb 15, 2005
- *
- * Functions:
- *      AESbGenCCMP - Parsing RX-packet
- *
- *
- * Revision History:
- *
- */
-
-#include "device.h"
-#include "80211hdr.h"
-#include "aes_ccmp.h"
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*
- * SBOX Table
- */
-
-static unsigned char sbox_table[256] = {
-	0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5, 0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
-	0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0, 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
-	0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc, 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
-	0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a, 0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
-	0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0, 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
-	0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b, 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
-	0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85, 0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
-	0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5, 0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
-	0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17, 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
-	0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88, 0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
-	0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c, 0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
-	0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9, 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
-	0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6, 0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
-	0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e, 0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
-	0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94, 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
-	0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68, 0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
-};
-
-static unsigned char dot2_table[256] = {
-	0x00, 0x02, 0x04, 0x06, 0x08, 0x0a, 0x0c, 0x0e, 0x10, 0x12, 0x14, 0x16, 0x18, 0x1a, 0x1c, 0x1e,
-	0x20, 0x22, 0x24, 0x26, 0x28, 0x2a, 0x2c, 0x2e, 0x30, 0x32, 0x34, 0x36, 0x38, 0x3a, 0x3c, 0x3e,
-	0x40, 0x42, 0x44, 0x46, 0x48, 0x4a, 0x4c, 0x4e, 0x50, 0x52, 0x54, 0x56, 0x58, 0x5a, 0x5c, 0x5e,
-	0x60, 0x62, 0x64, 0x66, 0x68, 0x6a, 0x6c, 0x6e, 0x70, 0x72, 0x74, 0x76, 0x78, 0x7a, 0x7c, 0x7e,
-	0x80, 0x82, 0x84, 0x86, 0x88, 0x8a, 0x8c, 0x8e, 0x90, 0x92, 0x94, 0x96, 0x98, 0x9a, 0x9c, 0x9e,
-	0xa0, 0xa2, 0xa4, 0xa6, 0xa8, 0xaa, 0xac, 0xae, 0xb0, 0xb2, 0xb4, 0xb6, 0xb8, 0xba, 0xbc, 0xbe,
-	0xc0, 0xc2, 0xc4, 0xc6, 0xc8, 0xca, 0xcc, 0xce, 0xd0, 0xd2, 0xd4, 0xd6, 0xd8, 0xda, 0xdc, 0xde,
-	0xe0, 0xe2, 0xe4, 0xe6, 0xe8, 0xea, 0xec, 0xee, 0xf0, 0xf2, 0xf4, 0xf6, 0xf8, 0xfa, 0xfc, 0xfe,
-	0x1b, 0x19, 0x1f, 0x1d, 0x13, 0x11, 0x17, 0x15, 0x0b, 0x09, 0x0f, 0x0d, 0x03, 0x01, 0x07, 0x05,
-	0x3b, 0x39, 0x3f, 0x3d, 0x33, 0x31, 0x37, 0x35, 0x2b, 0x29, 0x2f, 0x2d, 0x23, 0x21, 0x27, 0x25,
-	0x5b, 0x59, 0x5f, 0x5d, 0x53, 0x51, 0x57, 0x55, 0x4b, 0x49, 0x4f, 0x4d, 0x43, 0x41, 0x47, 0x45,
-	0x7b, 0x79, 0x7f, 0x7d, 0x73, 0x71, 0x77, 0x75, 0x6b, 0x69, 0x6f, 0x6d, 0x63, 0x61, 0x67, 0x65,
-	0x9b, 0x99, 0x9f, 0x9d, 0x93, 0x91, 0x97, 0x95, 0x8b, 0x89, 0x8f, 0x8d, 0x83, 0x81, 0x87, 0x85,
-	0xbb, 0xb9, 0xbf, 0xbd, 0xb3, 0xb1, 0xb7, 0xb5, 0xab, 0xa9, 0xaf, 0xad, 0xa3, 0xa1, 0xa7, 0xa5,
-	0xdb, 0xd9, 0xdf, 0xdd, 0xd3, 0xd1, 0xd7, 0xd5, 0xcb, 0xc9, 0xcf, 0xcd, 0xc3, 0xc1, 0xc7, 0xc5,
-	0xfb, 0xf9, 0xff, 0xfd, 0xf3, 0xf1, 0xf7, 0xf5, 0xeb, 0xe9, 0xef, 0xed, 0xe3, 0xe1, 0xe7, 0xe5
-};
-
-static unsigned char dot3_table[256] = {
-	0x00, 0x03, 0x06, 0x05, 0x0c, 0x0f, 0x0a, 0x09, 0x18, 0x1b, 0x1e, 0x1d, 0x14, 0x17, 0x12, 0x11,
-	0x30, 0x33, 0x36, 0x35, 0x3c, 0x3f, 0x3a, 0x39, 0x28, 0x2b, 0x2e, 0x2d, 0x24, 0x27, 0x22, 0x21,
-	0x60, 0x63, 0x66, 0x65, 0x6c, 0x6f, 0x6a, 0x69, 0x78, 0x7b, 0x7e, 0x7d, 0x74, 0x77, 0x72, 0x71,
-	0x50, 0x53, 0x56, 0x55, 0x5c, 0x5f, 0x5a, 0x59, 0x48, 0x4b, 0x4e, 0x4d, 0x44, 0x47, 0x42, 0x41,
-	0xc0, 0xc3, 0xc6, 0xc5, 0xcc, 0xcf, 0xca, 0xc9, 0xd8, 0xdb, 0xde, 0xdd, 0xd4, 0xd7, 0xd2, 0xd1,
-	0xf0, 0xf3, 0xf6, 0xf5, 0xfc, 0xff, 0xfa, 0xf9, 0xe8, 0xeb, 0xee, 0xed, 0xe4, 0xe7, 0xe2, 0xe1,
-	0xa0, 0xa3, 0xa6, 0xa5, 0xac, 0xaf, 0xaa, 0xa9, 0xb8, 0xbb, 0xbe, 0xbd, 0xb4, 0xb7, 0xb2, 0xb1,
-	0x90, 0x93, 0x96, 0x95, 0x9c, 0x9f, 0x9a, 0x99, 0x88, 0x8b, 0x8e, 0x8d, 0x84, 0x87, 0x82, 0x81,
-	0x9b, 0x98, 0x9d, 0x9e, 0x97, 0x94, 0x91, 0x92, 0x83, 0x80, 0x85, 0x86, 0x8f, 0x8c, 0x89, 0x8a,
-	0xab, 0xa8, 0xad, 0xae, 0xa7, 0xa4, 0xa1, 0xa2, 0xb3, 0xb0, 0xb5, 0xb6, 0xbf, 0xbc, 0xb9, 0xba,
-	0xfb, 0xf8, 0xfd, 0xfe, 0xf7, 0xf4, 0xf1, 0xf2, 0xe3, 0xe0, 0xe5, 0xe6, 0xef, 0xec, 0xe9, 0xea,
-	0xcb, 0xc8, 0xcd, 0xce, 0xc7, 0xc4, 0xc1, 0xc2, 0xd3, 0xd0, 0xd5, 0xd6, 0xdf, 0xdc, 0xd9, 0xda,
-	0x5b, 0x58, 0x5d, 0x5e, 0x57, 0x54, 0x51, 0x52, 0x43, 0x40, 0x45, 0x46, 0x4f, 0x4c, 0x49, 0x4a,
-	0x6b, 0x68, 0x6d, 0x6e, 0x67, 0x64, 0x61, 0x62, 0x73, 0x70, 0x75, 0x76, 0x7f, 0x7c, 0x79, 0x7a,
-	0x3b, 0x38, 0x3d, 0x3e, 0x37, 0x34, 0x31, 0x32, 0x23, 0x20, 0x25, 0x26, 0x2f, 0x2c, 0x29, 0x2a,
-	0x0b, 0x08, 0x0d, 0x0e, 0x07, 0x04, 0x01, 0x02, 0x13, 0x10, 0x15, 0x16, 0x1f, 0x1c, 0x19, 0x1a
-};
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-static void xor_128(unsigned char *a, unsigned char *b, unsigned char *out)
-{
-	unsigned long *dwPtrA = (unsigned long *)a;
-	unsigned long *dwPtrB = (unsigned long *)b;
-	unsigned long *dwPtrOut = (unsigned long *)out;
-
-	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
-	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
-	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
-	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
-}
-
-static void xor_32(unsigned char *a, unsigned char *b, unsigned char *out)
-{
-	unsigned long *dwPtrA = (unsigned long *)a;
-	unsigned long *dwPtrB = (unsigned long *)b;
-	unsigned long *dwPtrOut = (unsigned long *)out;
-
-	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
-}
-
-static void AddRoundKey(unsigned char *key, int round)
-{
-	unsigned char sbox_key[4];
-	unsigned char rcon_table[10] = { 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1b, 0x36};
-
-	sbox_key[0] = sbox_table[key[13]];
-	sbox_key[1] = sbox_table[key[14]];
-	sbox_key[2] = sbox_table[key[15]];
-	sbox_key[3] = sbox_table[key[12]];
-
-	key[0] = key[0] ^ rcon_table[round];
-	xor_32(&key[0], sbox_key, &key[0]);
-
-	xor_32(&key[4], &key[0], &key[4]);
-	xor_32(&key[8], &key[4], &key[8]);
-	xor_32(&key[12], &key[8], &key[12]);
-}
-
-static void SubBytes(unsigned char *in, unsigned char *out)
-{
-	int i;
-
-	for (i = 0; i < 16; i++)
-		out[i] = sbox_table[in[i]];
-}
-
-static void ShiftRows(unsigned char *in, unsigned char *out)
-{
-	out[0]  = in[0];
-	out[1]  = in[5];
-	out[2]  = in[10];
-	out[3]  = in[15];
-	out[4]  = in[4];
-	out[5]  = in[9];
-	out[6]  = in[14];
-	out[7]  = in[3];
-	out[8]  = in[8];
-	out[9]  = in[13];
-	out[10] = in[2];
-	out[11] = in[7];
-	out[12] = in[12];
-	out[13] = in[1];
-	out[14] = in[6];
-	out[15] = in[11];
-}
-
-static void MixColumns(unsigned char *in, unsigned char *out)
-{
-	out[0] = dot2_table[in[0]] ^ dot3_table[in[1]] ^ in[2] ^ in[3];
-	out[1] = in[0] ^ dot2_table[in[1]] ^ dot3_table[in[2]] ^ in[3];
-	out[2] = in[0] ^ in[1] ^ dot2_table[in[2]] ^ dot3_table[in[3]];
-	out[3] = dot3_table[in[0]] ^ in[1] ^ in[2] ^ dot2_table[in[3]];
-}
-
-static void AESv128(unsigned char *key, unsigned char *data, unsigned char *ciphertext)
-{
-	int  i;
-	int  round;
-	unsigned char TmpdataA[16];
-	unsigned char TmpdataB[16];
-	unsigned char abyRoundKey[16];
-
-	for (i = 0; i < 16; i++)
-		abyRoundKey[i] = key[i];
-
-	for (round = 0; round < 11; round++) {
-		if (round == 0) {
-			xor_128(abyRoundKey, data, ciphertext);
-			AddRoundKey(abyRoundKey, round);
-		} else if (round == 10) {
-			SubBytes(ciphertext, TmpdataA);
-			ShiftRows(TmpdataA, TmpdataB);
-			xor_128(TmpdataB, abyRoundKey, ciphertext);
-		} else /* round 1 ~ 9 */{
-			SubBytes(ciphertext, TmpdataA);
-			ShiftRows(TmpdataA, TmpdataB);
-			MixColumns(&TmpdataB[0], &TmpdataA[0]);
-			MixColumns(&TmpdataB[4], &TmpdataA[4]);
-			MixColumns(&TmpdataB[8], &TmpdataA[8]);
-			MixColumns(&TmpdataB[12], &TmpdataA[12]);
-			xor_128(TmpdataA, abyRoundKey, ciphertext);
-			AddRoundKey(abyRoundKey, round);
-		}
-	}
-}
-
-/*
- * Description: AES decryption
- *
- * Parameters:
- *  In:
- *      pbyRxKey            - The key used to decrypt
- *      pbyFrame            - Starting address of packet header
- *      wFrameSize          - Total packet size including CRC
- *  Out:
- *      none
- *
- * Return Value: MIC compare result
- *
- */
-bool AESbGenCCMP(unsigned char *pbyRxKey, unsigned char *pbyFrame, unsigned short wFrameSize)
-{
-	unsigned char abyNonce[13];
-	unsigned char MIC_IV[16];
-	unsigned char MIC_HDR1[16];
-	unsigned char MIC_HDR2[16];
-	unsigned char abyMIC[16];
-	unsigned char abyCTRPLD[16];
-	unsigned char abyTmp[16];
-	unsigned char abyPlainText[16];
-	unsigned char abyLastCipher[16];
-
-	PS802_11Header  pMACHeader = (PS802_11Header) pbyFrame;
-	unsigned char *pbyIV;
-	unsigned char *pbyPayload;
-	unsigned short wHLen = 22;
-	unsigned short wPayloadSize = wFrameSize - 8 - 8 - 4 - WLAN_HDR_ADDR3_LEN;/* 8 is IV, 8 is MIC, 4 is CRC */
-	bool bA4 = false;
-	unsigned char byTmp;
-	unsigned short wCnt;
-	int ii, jj, kk;
-
-	pbyIV = pbyFrame + WLAN_HDR_ADDR3_LEN;
-	if (WLAN_GET_FC_TODS(*(unsigned short *)pbyFrame) &&
-	    WLAN_GET_FC_FROMDS(*(unsigned short *)pbyFrame)) {
-		bA4 = true;
-		pbyIV += 6;             /* 6 is 802.11 address4 */
-		wHLen += 6;
-		wPayloadSize -= 6;
-	}
-	pbyPayload = pbyIV + 8; /* IV-length */
-
-	abyNonce[0]  = 0x00; /* now is 0, if Qos here will be priority */
-	memcpy(&(abyNonce[1]), pMACHeader->abyAddr2, ETH_ALEN);
-	abyNonce[7]  = pbyIV[7];
-	abyNonce[8]  = pbyIV[6];
-	abyNonce[9]  = pbyIV[5];
-	abyNonce[10] = pbyIV[4];
-	abyNonce[11] = pbyIV[1];
-	abyNonce[12] = pbyIV[0];
-
-	/* MIC_IV */
-	MIC_IV[0] = 0x59;
-	memcpy(&(MIC_IV[1]), &(abyNonce[0]), 13);
-	MIC_IV[14] = (unsigned char)(wPayloadSize >> 8);
-	MIC_IV[15] = (unsigned char)(wPayloadSize & 0xff);
-
-	/* MIC_HDR1 */
-	MIC_HDR1[0] = (unsigned char)(wHLen >> 8);
-	MIC_HDR1[1] = (unsigned char)(wHLen & 0xff);
-	byTmp = (unsigned char)(pMACHeader->wFrameCtl & 0xff);
-	MIC_HDR1[2] = byTmp & 0x8f;
-	byTmp = (unsigned char)(pMACHeader->wFrameCtl >> 8);
-	byTmp &= 0x87;
-	MIC_HDR1[3] = byTmp | 0x40;
-	memcpy(&(MIC_HDR1[4]), pMACHeader->abyAddr1, ETH_ALEN);
-	memcpy(&(MIC_HDR1[10]), pMACHeader->abyAddr2, ETH_ALEN);
-
-	/* MIC_HDR2 */
-	memcpy(&(MIC_HDR2[0]), pMACHeader->abyAddr3, ETH_ALEN);
-	byTmp = (unsigned char)(pMACHeader->wSeqCtl & 0xff);
-	MIC_HDR2[6] = byTmp & 0x0f;
-	MIC_HDR2[7] = 0;
-	if (bA4) {
-		memcpy(&(MIC_HDR2[8]), pMACHeader->abyAddr4, ETH_ALEN);
-	} else {
-		MIC_HDR2[8]  = 0x00;
-		MIC_HDR2[9]  = 0x00;
-		MIC_HDR2[10] = 0x00;
-		MIC_HDR2[11] = 0x00;
-		MIC_HDR2[12] = 0x00;
-		MIC_HDR2[13] = 0x00;
-	}
-	MIC_HDR2[14] = 0x00;
-	MIC_HDR2[15] = 0x00;
-
-	/* CCMP */
-	AESv128(pbyRxKey, MIC_IV, abyMIC);
-	for (kk = 0; kk < 16; kk++)
-		abyTmp[kk] = MIC_HDR1[kk] ^ abyMIC[kk];
-	AESv128(pbyRxKey, abyTmp, abyMIC);
-	for (kk = 0; kk < 16; kk++)
-		abyTmp[kk] = MIC_HDR2[kk] ^ abyMIC[kk];
-	AESv128(pbyRxKey, abyTmp, abyMIC);
-
-	wCnt = 1;
-	abyCTRPLD[0] = 0x01;
-	memcpy(&(abyCTRPLD[1]), &(abyNonce[0]), 13);
-
-	for (jj = wPayloadSize; jj > 16; jj = jj - 16) {
-		abyCTRPLD[14] = (unsigned char)(wCnt >> 8);
-		abyCTRPLD[15] = (unsigned char)(wCnt & 0xff);
-
-		AESv128(pbyRxKey, abyCTRPLD, abyTmp);
-
-		for (kk = 0; kk < 16; kk++)
-			abyPlainText[kk] = abyTmp[kk] ^ pbyPayload[kk];
-		for (kk = 0; kk < 16; kk++)
-			abyTmp[kk] = abyMIC[kk] ^ abyPlainText[kk];
-		AESv128(pbyRxKey, abyTmp, abyMIC);
-
-		memcpy(pbyPayload, abyPlainText, 16);
-		wCnt++;
-		pbyPayload += 16;
-	} /* for wPayloadSize */
-
-	/* last payload */
-	memcpy(&(abyLastCipher[0]), pbyPayload, jj);
-	for (ii = jj; ii < 16; ii++)
-		abyLastCipher[ii] = 0x00;
-
-	abyCTRPLD[14] = (unsigned char)(wCnt >> 8);
-	abyCTRPLD[15] = (unsigned char)(wCnt & 0xff);
-
-	AESv128(pbyRxKey, abyCTRPLD, abyTmp);
-	for (kk = 0; kk < 16; kk++)
-		abyPlainText[kk] = abyTmp[kk] ^ abyLastCipher[kk];
-	memcpy(pbyPayload, abyPlainText, jj);
-	pbyPayload += jj;
-
-	/* for MIC calculation */
-	for (ii = jj; ii < 16; ii++)
-		abyPlainText[ii] = 0x00;
-	for (kk = 0; kk < 16; kk++)
-		abyTmp[kk] = abyMIC[kk] ^ abyPlainText[kk];
-	AESv128(pbyRxKey, abyTmp, abyMIC);
-
-	/* =>above is the calculate MIC */
-	/* -------------------------------------------- */
-
-	wCnt = 0;
-	abyCTRPLD[14] = (unsigned char)(wCnt >> 8);
-	abyCTRPLD[15] = (unsigned char)(wCnt & 0xff);
-	AESv128(pbyRxKey, abyCTRPLD, abyTmp);
-	for (kk = 0; kk < 8; kk++)
-		abyTmp[kk] = abyTmp[kk] ^ pbyPayload[kk];
-	/* =>above is the dec-MIC from packet */
-	/* -------------------------------------------- */
-
-	return !memcmp(abyMIC, abyTmp, 8);
-}
diff --git a/drivers/staging/vt6655/aes_ccmp.h b/drivers/staging/vt6655/aes_ccmp.h
deleted file mode 100644
index fe0c506205d5..000000000000
--- a/drivers/staging/vt6655/aes_ccmp.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: aes_ccmp.h
- *
- * Purpose: AES_CCMP Decryption
- *
- * Author: Warren Hsu
- *
- * Date: Feb 15, 2005
- *
- */
-
-#ifndef __AES_H__
-#define __AES_H__
-
-#include "ttype.h"
-
-bool AESbGenCCMP(unsigned char *pbyRxKey, unsigned char *pbyFrame, unsigned short wFrameSize);
-
-#endif /* __AES_H__ */
diff --git a/drivers/staging/vt6655/baseband.c b/drivers/staging/vt6655/baseband.c
index de54923e8861..86c72ba0a0cd 100644
--- a/drivers/staging/vt6655/baseband.c
+++ b/drivers/staging/vt6655/baseband.c
@@ -30,12 +30,7 @@
  *      BBvCaculateParameter   - Caculate PhyLength, PhyService and Phy Signal parameter for baseband Tx
  *      BBbReadEmbedded         - Embedded read baseband register via MAC
  *      BBbWriteEmbedded        - Embedded write baseband register via MAC
- *      BBbIsRegBitsOn         - Test if baseband register bits on
- *      BBbIsRegBitsOff        - Test if baseband register bits off
  *      BBbVT3253Init          - VIA VT3253 baseband chip init code
- *      BBvReadAllRegs         - Read All Baseband Registers
- *      BBvLoopbackOn          - Turn on BaseBand Loopback mode
- *      BBvLoopbackOff         - Turn off BaseBand Loopback mode
  *
  * Revision History:
  *      06-10-2003 Bryan YC Fan:  Re-write codes to support VT3253 spec.
@@ -50,7 +45,6 @@
  */
 
 #include "tmacro.h"
-#include "tether.h"
 #include "mac.h"
 #include "baseband.h"
 #include "srom.h"
@@ -1708,39 +1702,39 @@ static const unsigned short awcFrameTime[MAX_RATE] = {
 
 static
 unsigned long
-s_ulGetRatio(struct vnt_private *pDevice);
+s_ulGetRatio(struct vnt_private *priv);
 
 static
 void
 s_vChangeAntenna(
-	struct vnt_private *pDevice
+	struct vnt_private *priv
 );
 
 static
 void
 s_vChangeAntenna(
-	struct vnt_private *pDevice
+	struct vnt_private *priv
 )
 {
-	if (pDevice->dwRxAntennaSel == 0) {
-		pDevice->dwRxAntennaSel = 1;
-		if (pDevice->bTxRxAntInv == true)
-			BBvSetRxAntennaMode(pDevice->PortOffset, ANT_A);
+	if (priv->dwRxAntennaSel == 0) {
+		priv->dwRxAntennaSel = 1;
+		if (priv->bTxRxAntInv == true)
+			BBvSetRxAntennaMode(priv, ANT_A);
 		else
-			BBvSetRxAntennaMode(pDevice->PortOffset, ANT_B);
+			BBvSetRxAntennaMode(priv, ANT_B);
 	} else {
-		pDevice->dwRxAntennaSel = 0;
-		if (pDevice->bTxRxAntInv == true)
-			BBvSetRxAntennaMode(pDevice->PortOffset, ANT_B);
+		priv->dwRxAntennaSel = 0;
+		if (priv->bTxRxAntInv == true)
+			BBvSetRxAntennaMode(priv, ANT_B);
 		else
-			BBvSetRxAntennaMode(pDevice->PortOffset, ANT_A);
+			BBvSetRxAntennaMode(priv, ANT_A);
 	}
-	if (pDevice->dwTxAntennaSel == 0) {
-		pDevice->dwTxAntennaSel = 1;
-		BBvSetTxAntennaMode(pDevice->PortOffset, ANT_B);
+	if (priv->dwTxAntennaSel == 0) {
+		priv->dwTxAntennaSel = 1;
+		BBvSetTxAntennaMode(priv, ANT_B);
 	} else {
-		pDevice->dwTxAntennaSel = 0;
-		BBvSetTxAntennaMode(pDevice->PortOffset, ANT_A);
+		priv->dwTxAntennaSel = 0;
+		BBvSetTxAntennaMode(priv, ANT_A);
 	}
 }
 
@@ -1792,18 +1786,17 @@ BBuGetFrameTime(
 			uFrameTime++;
 
 		return uPreamble + uFrameTime;
-	} else {
-		uFrameTime = (cbFrameLength * 8 + 22) / uRate; /* ???????? */
-		uTmp = ((uFrameTime * uRate) - 22) / 8;
-		if (cbFrameLength != uTmp)
-			uFrameTime++;
+	}
+	uFrameTime = (cbFrameLength * 8 + 22) / uRate; /* ???????? */
+	uTmp = ((uFrameTime * uRate) - 22) / 8;
+	if (cbFrameLength != uTmp)
+		uFrameTime++;
 
-		uFrameTime = uFrameTime * 4;    /* ??????? */
-		if (byPktType != PK_TYPE_11A)
-			uFrameTime += 6;     /* ?????? */
+	uFrameTime = uFrameTime * 4;    /* ??????? */
+	if (byPktType != PK_TYPE_11A)
+		uFrameTime += 6;     /* ?????? */
 
-		return 20 + uFrameTime; /* ?????? */
-	}
+	return 20 + uFrameTime; /* ?????? */
 }
 
 /*
@@ -1968,8 +1961,10 @@ void vnt_get_phy_field(struct vnt_private *priv, u32 frame_length,
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool BBbReadEmbedded(void __iomem *dwIoBase, unsigned char byBBAddr, unsigned char *pbyData)
+bool BBbReadEmbedded(struct vnt_private *priv,
+		     unsigned char byBBAddr, unsigned char *pbyData)
 {
+	void __iomem *dwIoBase = priv->PortOffset;
 	unsigned short ww;
 	unsigned char byValue;
 
@@ -2010,8 +2005,10 @@ bool BBbReadEmbedded(void __iomem *dwIoBase, unsigned char byBBAddr, unsigned ch
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool BBbWriteEmbedded(void __iomem *dwIoBase, unsigned char byBBAddr, unsigned char byData)
+bool BBbWriteEmbedded(struct vnt_private *priv,
+		      unsigned char byBBAddr, unsigned char byData)
 {
+	void __iomem *dwIoBase = priv->PortOffset;
 	unsigned short ww;
 	unsigned char byValue;
 
@@ -2038,50 +2035,6 @@ bool BBbWriteEmbedded(void __iomem *dwIoBase, unsigned char byBBAddr, unsigned c
 }
 
 /*
- * Description: Test if all bits are set for the Baseband register
- *
- * Parameters:
- *  In:
- *      dwIoBase    - I/O base address
- *      byBBAddr    - address of register in Baseband
- *      byTestBits  - TestBits
- *  Out:
- *      none
- *
- * Return Value: true if all TestBits are set; false otherwise.
- *
- */
-bool BBbIsRegBitsOn(void __iomem *dwIoBase, unsigned char byBBAddr, unsigned char byTestBits)
-{
-	unsigned char byOrgData;
-
-	BBbReadEmbedded(dwIoBase, byBBAddr, &byOrgData);
-	return (byOrgData & byTestBits) == byTestBits;
-}
-
-/*
- * Description: Test if all bits are clear for the Baseband register
- *
- * Parameters:
- *  In:
- *      dwIoBase    - I/O base address
- *      byBBAddr    - address of register in Baseband
- *      byTestBits  - TestBits
- *  Out:
- *      none
- *
- * Return Value: true if all TestBits are clear; false otherwise.
- *
- */
-bool BBbIsRegBitsOff(void __iomem *dwIoBase, unsigned char byBBAddr, unsigned char byTestBits)
-{
-	unsigned char byOrgData;
-
-	BBbReadEmbedded(dwIoBase, byBBAddr, &byOrgData);
-	return (byOrgData & byTestBits) == 0;
-}
-
-/*
  * Description: VIA VT3253 Baseband chip init function
  *
  * Parameters:
@@ -2096,126 +2049,126 @@ bool BBbIsRegBitsOff(void __iomem *dwIoBase, unsigned char byBBAddr, unsigned ch
  *
  */
 
-bool BBbVT3253Init(struct vnt_private *pDevice)
+bool BBbVT3253Init(struct vnt_private *priv)
 {
 	bool bResult = true;
 	int        ii;
-	void __iomem *dwIoBase = pDevice->PortOffset;
-	unsigned char byRFType = pDevice->byRFType;
-	unsigned char byLocalID = pDevice->byLocalID;
+	void __iomem *dwIoBase = priv->PortOffset;
+	unsigned char byRFType = priv->byRFType;
+	unsigned char byLocalID = priv->byLocalID;
 
 	if (byRFType == RF_RFMD2959) {
 		if (byLocalID <= REV_ID_VT3253_A1) {
 			for (ii = 0; ii < CB_VT3253_INIT_FOR_RFMD; ii++)
-				bResult &= BBbWriteEmbedded(dwIoBase, byVT3253InitTab_RFMD[ii][0], byVT3253InitTab_RFMD[ii][1]);
+				bResult &= BBbWriteEmbedded(priv, byVT3253InitTab_RFMD[ii][0], byVT3253InitTab_RFMD[ii][1]);
 
 		} else {
 			for (ii = 0; ii < CB_VT3253B0_INIT_FOR_RFMD; ii++)
-				bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_RFMD[ii][0], byVT3253B0_RFMD[ii][1]);
+				bResult &= BBbWriteEmbedded(priv, byVT3253B0_RFMD[ii][0], byVT3253B0_RFMD[ii][1]);
 
 			for (ii = 0; ii < CB_VT3253B0_AGC_FOR_RFMD2959; ii++)
-				bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AGC4_RFMD2959[ii][0], byVT3253B0_AGC4_RFMD2959[ii][1]);
+				bResult &= BBbWriteEmbedded(priv, byVT3253B0_AGC4_RFMD2959[ii][0], byVT3253B0_AGC4_RFMD2959[ii][1]);
 
 			VNSvOutPortD(dwIoBase + MAC_REG_ITRTMSET, 0x23);
-			MACvRegBitsOn(dwIoBase, MAC_REG_PAPEDELAY, BIT0);
+			MACvRegBitsOn(dwIoBase, MAC_REG_PAPEDELAY, BIT(0));
 		}
-		pDevice->abyBBVGA[0] = 0x18;
-		pDevice->abyBBVGA[1] = 0x0A;
-		pDevice->abyBBVGA[2] = 0x0;
-		pDevice->abyBBVGA[3] = 0x0;
-		pDevice->ldBmThreshold[0] = -70;
-		pDevice->ldBmThreshold[1] = -50;
-		pDevice->ldBmThreshold[2] = 0;
-		pDevice->ldBmThreshold[3] = 0;
+		priv->abyBBVGA[0] = 0x18;
+		priv->abyBBVGA[1] = 0x0A;
+		priv->abyBBVGA[2] = 0x0;
+		priv->abyBBVGA[3] = 0x0;
+		priv->ldBmThreshold[0] = -70;
+		priv->ldBmThreshold[1] = -50;
+		priv->ldBmThreshold[2] = 0;
+		priv->ldBmThreshold[3] = 0;
 	} else if ((byRFType == RF_AIROHA) || (byRFType == RF_AL2230S)) {
 		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++)
-			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AIROHA2230[ii][0], byVT3253B0_AIROHA2230[ii][1]);
+			bResult &= BBbWriteEmbedded(priv, byVT3253B0_AIROHA2230[ii][0], byVT3253B0_AIROHA2230[ii][1]);
 
 		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
-			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
-
-		pDevice->abyBBVGA[0] = 0x1C;
-		pDevice->abyBBVGA[1] = 0x10;
-		pDevice->abyBBVGA[2] = 0x0;
-		pDevice->abyBBVGA[3] = 0x0;
-		pDevice->ldBmThreshold[0] = -70;
-		pDevice->ldBmThreshold[1] = -48;
-		pDevice->ldBmThreshold[2] = 0;
-		pDevice->ldBmThreshold[3] = 0;
+			bResult &= BBbWriteEmbedded(priv, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
+
+		priv->abyBBVGA[0] = 0x1C;
+		priv->abyBBVGA[1] = 0x10;
+		priv->abyBBVGA[2] = 0x0;
+		priv->abyBBVGA[3] = 0x0;
+		priv->ldBmThreshold[0] = -70;
+		priv->ldBmThreshold[1] = -48;
+		priv->ldBmThreshold[2] = 0;
+		priv->ldBmThreshold[3] = 0;
 	} else if (byRFType == RF_UW2451) {
 		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_UW2451; ii++)
-			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_UW2451[ii][0], byVT3253B0_UW2451[ii][1]);
+			bResult &= BBbWriteEmbedded(priv, byVT3253B0_UW2451[ii][0], byVT3253B0_UW2451[ii][1]);
 
 		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
-			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
+			bResult &= BBbWriteEmbedded(priv, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
 
 		VNSvOutPortB(dwIoBase + MAC_REG_ITRTMSET, 0x23);
-		MACvRegBitsOn(dwIoBase, MAC_REG_PAPEDELAY, BIT0);
-
-		pDevice->abyBBVGA[0] = 0x14;
-		pDevice->abyBBVGA[1] = 0x0A;
-		pDevice->abyBBVGA[2] = 0x0;
-		pDevice->abyBBVGA[3] = 0x0;
-		pDevice->ldBmThreshold[0] = -60;
-		pDevice->ldBmThreshold[1] = -50;
-		pDevice->ldBmThreshold[2] = 0;
-		pDevice->ldBmThreshold[3] = 0;
+		MACvRegBitsOn(dwIoBase, MAC_REG_PAPEDELAY, BIT(0));
+
+		priv->abyBBVGA[0] = 0x14;
+		priv->abyBBVGA[1] = 0x0A;
+		priv->abyBBVGA[2] = 0x0;
+		priv->abyBBVGA[3] = 0x0;
+		priv->ldBmThreshold[0] = -60;
+		priv->ldBmThreshold[1] = -50;
+		priv->ldBmThreshold[2] = 0;
+		priv->ldBmThreshold[3] = 0;
 	} else if (byRFType == RF_UW2452) {
 		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_UW2451; ii++)
-			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_UW2451[ii][0], byVT3253B0_UW2451[ii][1]);
+			bResult &= BBbWriteEmbedded(priv, byVT3253B0_UW2451[ii][0], byVT3253B0_UW2451[ii][1]);
 
 		/* Init ANT B select,TX Config CR09 = 0x61->0x45, 0x45->0x41(VC1/VC2 define, make the ANT_A, ANT_B inverted) */
 		/*bResult &= BBbWriteEmbedded(dwIoBase,0x09,0x41);*/
 		/* Init ANT B select,RX Config CR10 = 0x28->0x2A, 0x2A->0x28(VC1/VC2 define, make the ANT_A, ANT_B inverted) */
 		/*bResult &= BBbWriteEmbedded(dwIoBase,0x0a,0x28);*/
 		/* Select VC1/VC2, CR215 = 0x02->0x06 */
-		bResult &= BBbWriteEmbedded(dwIoBase, 0xd7, 0x06);
+		bResult &= BBbWriteEmbedded(priv, 0xd7, 0x06);
 
 		/* {{RobertYu:20050125, request by Jack */
-		bResult &= BBbWriteEmbedded(dwIoBase, 0x90, 0x20);
-		bResult &= BBbWriteEmbedded(dwIoBase, 0x97, 0xeb);
+		bResult &= BBbWriteEmbedded(priv, 0x90, 0x20);
+		bResult &= BBbWriteEmbedded(priv, 0x97, 0xeb);
 		/* }} */
 
 		/* {{RobertYu:20050221, request by Jack */
-		bResult &= BBbWriteEmbedded(dwIoBase, 0xa6, 0x00);
-		bResult &= BBbWriteEmbedded(dwIoBase, 0xa8, 0x30);
+		bResult &= BBbWriteEmbedded(priv, 0xa6, 0x00);
+		bResult &= BBbWriteEmbedded(priv, 0xa8, 0x30);
 		/* }} */
-		bResult &= BBbWriteEmbedded(dwIoBase, 0xb0, 0x58);
+		bResult &= BBbWriteEmbedded(priv, 0xb0, 0x58);
 
 		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
-			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
-
-		pDevice->abyBBVGA[0] = 0x14;
-		pDevice->abyBBVGA[1] = 0x0A;
-		pDevice->abyBBVGA[2] = 0x0;
-		pDevice->abyBBVGA[3] = 0x0;
-		pDevice->ldBmThreshold[0] = -60;
-		pDevice->ldBmThreshold[1] = -50;
-		pDevice->ldBmThreshold[2] = 0;
-		pDevice->ldBmThreshold[3] = 0;
+			bResult &= BBbWriteEmbedded(priv, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
+
+		priv->abyBBVGA[0] = 0x14;
+		priv->abyBBVGA[1] = 0x0A;
+		priv->abyBBVGA[2] = 0x0;
+		priv->abyBBVGA[3] = 0x0;
+		priv->ldBmThreshold[0] = -60;
+		priv->ldBmThreshold[1] = -50;
+		priv->ldBmThreshold[2] = 0;
+		priv->ldBmThreshold[3] = 0;
 		/* }} RobertYu */
 
 	} else if (byRFType == RF_VT3226) {
 		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++)
-			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AIROHA2230[ii][0], byVT3253B0_AIROHA2230[ii][1]);
+			bResult &= BBbWriteEmbedded(priv, byVT3253B0_AIROHA2230[ii][0], byVT3253B0_AIROHA2230[ii][1]);
 
 		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
-			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
-
-		pDevice->abyBBVGA[0] = 0x1C;
-		pDevice->abyBBVGA[1] = 0x10;
-		pDevice->abyBBVGA[2] = 0x0;
-		pDevice->abyBBVGA[3] = 0x0;
-		pDevice->ldBmThreshold[0] = -70;
-		pDevice->ldBmThreshold[1] = -48;
-		pDevice->ldBmThreshold[2] = 0;
-		pDevice->ldBmThreshold[3] = 0;
+			bResult &= BBbWriteEmbedded(priv, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
+
+		priv->abyBBVGA[0] = 0x1C;
+		priv->abyBBVGA[1] = 0x10;
+		priv->abyBBVGA[2] = 0x0;
+		priv->abyBBVGA[3] = 0x0;
+		priv->ldBmThreshold[0] = -70;
+		priv->ldBmThreshold[1] = -48;
+		priv->ldBmThreshold[2] = 0;
+		priv->ldBmThreshold[3] = 0;
 		/* Fix VT3226 DFC system timing issue */
 		MACvSetRFLE_LatchBase(dwIoBase);
 		/* {{ RobertYu: 20050104 */
 	} else if (byRFType == RF_AIROHA7230) {
 		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++)
-			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AIROHA2230[ii][0], byVT3253B0_AIROHA2230[ii][1]);
+			bResult &= BBbWriteEmbedded(priv, byVT3253B0_AIROHA2230[ii][0], byVT3253B0_AIROHA2230[ii][1]);
 
 
 		/* {{ RobertYu:20050223, request by JerryChung */
@@ -2228,150 +2181,37 @@ bool BBbVT3253Init(struct vnt_private *pDevice)
 		/* }} */
 
 		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
-			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
-
-		pDevice->abyBBVGA[0] = 0x1C;
-		pDevice->abyBBVGA[1] = 0x10;
-		pDevice->abyBBVGA[2] = 0x0;
-		pDevice->abyBBVGA[3] = 0x0;
-		pDevice->ldBmThreshold[0] = -70;
-		pDevice->ldBmThreshold[1] = -48;
-		pDevice->ldBmThreshold[2] = 0;
-		pDevice->ldBmThreshold[3] = 0;
+			bResult &= BBbWriteEmbedded(priv, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
+
+		priv->abyBBVGA[0] = 0x1C;
+		priv->abyBBVGA[1] = 0x10;
+		priv->abyBBVGA[2] = 0x0;
+		priv->abyBBVGA[3] = 0x0;
+		priv->ldBmThreshold[0] = -70;
+		priv->ldBmThreshold[1] = -48;
+		priv->ldBmThreshold[2] = 0;
+		priv->ldBmThreshold[3] = 0;
 		/* }} RobertYu */
 	} else {
 		/* No VGA Table now */
-		pDevice->bUpdateBBVGA = false;
-		pDevice->abyBBVGA[0] = 0x1C;
+		priv->bUpdateBBVGA = false;
+		priv->abyBBVGA[0] = 0x1C;
 	}
 
 	if (byLocalID > REV_ID_VT3253_A1) {
-		BBbWriteEmbedded(dwIoBase, 0x04, 0x7F);
-		BBbWriteEmbedded(dwIoBase, 0x0D, 0x01);
+		BBbWriteEmbedded(priv, 0x04, 0x7F);
+		BBbWriteEmbedded(priv, 0x0D, 0x01);
 	}
 
 	return bResult;
 }
 
 /*
- * Description: Read All Baseband Registers
- *
- * Parameters:
- *  In:
- *      dwIoBase    - I/O base address
- *      pbyBBRegs   - Point to struct that stores Baseband Registers
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void BBvReadAllRegs(void __iomem *dwIoBase, unsigned char *pbyBBRegs)
-{
-	int  ii;
-	unsigned char byBase = 1;
-
-	for (ii = 0; ii < BB_MAX_CONTEXT_SIZE; ii++) {
-		BBbReadEmbedded(dwIoBase, (unsigned char)(ii*byBase), pbyBBRegs);
-		pbyBBRegs += byBase;
-	}
-}
-
-/*
- * Description: Turn on BaseBand Loopback mode
- *
- * Parameters:
- *  In:
- *      dwIoBase    - I/O base address
- *      bCCK        - If CCK is set
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-
-void BBvLoopbackOn(struct vnt_private *pDevice)
-{
-	unsigned char byData;
-	void __iomem *dwIoBase = pDevice->PortOffset;
-
-	/* CR C9 = 0x00 */
-	BBbReadEmbedded(dwIoBase, 0xC9, &pDevice->byBBCRc9); /* CR201 */
-	BBbWriteEmbedded(dwIoBase, 0xC9, 0);
-	BBbReadEmbedded(dwIoBase, 0x4D, &pDevice->byBBCR4d); /* CR77 */
-	BBbWriteEmbedded(dwIoBase, 0x4D, 0x90);
-
-	/* CR 88 = 0x02(CCK), 0x03(OFDM) */
-	BBbReadEmbedded(dwIoBase, 0x88, &pDevice->byBBCR88); /* CR136 */
-
-	if (pDevice->uConnectionRate <= RATE_11M) { /* CCK */
-		/* Enable internal digital loopback: CR33 |= 0000 0001 */
-		BBbReadEmbedded(dwIoBase, 0x21, &byData); /* CR33 */
-		BBbWriteEmbedded(dwIoBase, 0x21, (unsigned char)(byData | 0x01)); /* CR33 */
-		/* CR154 = 0x00 */
-		BBbWriteEmbedded(dwIoBase, 0x9A, 0);    /* CR154 */
-
-		BBbWriteEmbedded(dwIoBase, 0x88, 0x02); /* CR239 */
-	} else { /* OFDM */
-		/* Enable internal digital loopback:CR154 |= 0000 0001 */
-		BBbReadEmbedded(dwIoBase, 0x9A, &byData); /* CR154 */
-		BBbWriteEmbedded(dwIoBase, 0x9A, (unsigned char)(byData | 0x01)); /* CR154 */
-		/* CR33 = 0x00 */
-		BBbWriteEmbedded(dwIoBase, 0x21, 0);    /* CR33 */
-
-		BBbWriteEmbedded(dwIoBase, 0x88, 0x03); /* CR239 */
-	}
-
-	/* CR14 = 0x00 */
-	BBbWriteEmbedded(dwIoBase, 0x0E, 0); /* CR14 */
-
-	/* Disable TX_IQUN */
-	BBbReadEmbedded(pDevice->PortOffset, 0x09, &pDevice->byBBCR09);
-	BBbWriteEmbedded(pDevice->PortOffset, 0x09, (unsigned char)(pDevice->byBBCR09 & 0xDE));
-}
-
-/*
- * Description: Turn off BaseBand Loopback mode
- *
- * Parameters:
- *  In:
- *      pDevice         - Device Structure
- *
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void BBvLoopbackOff(struct vnt_private *pDevice)
-{
-	unsigned char byData;
-	void __iomem *dwIoBase = pDevice->PortOffset;
-
-	BBbWriteEmbedded(dwIoBase, 0xC9, pDevice->byBBCRc9); /* CR201 */
-	BBbWriteEmbedded(dwIoBase, 0x88, pDevice->byBBCR88); /* CR136 */
-	BBbWriteEmbedded(dwIoBase, 0x09, pDevice->byBBCR09); /* CR136 */
-	BBbWriteEmbedded(dwIoBase, 0x4D, pDevice->byBBCR4d); /* CR77  */
-
-	if (pDevice->uConnectionRate <= RATE_11M) { /* CCK */
-		/* Set the CR33 Bit2 to disable internal Loopback. */
-		BBbReadEmbedded(dwIoBase, 0x21, &byData);/* CR33 */
-		BBbWriteEmbedded(dwIoBase, 0x21, (unsigned char)(byData & 0xFE)); /* CR33 */
-	} else { /* OFDM */
-		BBbReadEmbedded(dwIoBase, 0x9A, &byData); /* CR154 */
-		BBbWriteEmbedded(dwIoBase, 0x9A, (unsigned char)(byData & 0xFE)); /* CR154 */
-	}
-	BBbReadEmbedded(dwIoBase, 0x0E, &byData); /* CR14 */
-	BBbWriteEmbedded(dwIoBase, 0x0E, (unsigned char)(byData | 0x80)); /* CR14 */
-}
-
-/*
  * Description: Set ShortSlotTime mode
  *
  * Parameters:
  *  In:
- *      pDevice     - Device Structure
+ *      priv     - Device Structure
  *  Out:
  *      none
  *
@@ -2379,42 +2219,42 @@ void BBvLoopbackOff(struct vnt_private *pDevice)
  *
  */
 void
-BBvSetShortSlotTime(struct vnt_private *pDevice)
+BBvSetShortSlotTime(struct vnt_private *priv)
 {
 	unsigned char byBBRxConf = 0;
 	unsigned char byBBVGA = 0;
 
-	BBbReadEmbedded(pDevice->PortOffset, 0x0A, &byBBRxConf); /* CR10 */
+	BBbReadEmbedded(priv, 0x0A, &byBBRxConf); /* CR10 */
 
-	if (pDevice->bShortSlotTime)
+	if (priv->bShortSlotTime)
 		byBBRxConf &= 0xDF; /* 1101 1111 */
 	else
 		byBBRxConf |= 0x20; /* 0010 0000 */
 
 	/* patch for 3253B0 Baseband with Cardbus module */
-	BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byBBVGA);
-	if (byBBVGA == pDevice->abyBBVGA[0])
+	BBbReadEmbedded(priv, 0xE7, &byBBVGA);
+	if (byBBVGA == priv->abyBBVGA[0])
 		byBBRxConf |= 0x20; /* 0010 0000 */
 
-	BBbWriteEmbedded(pDevice->PortOffset, 0x0A, byBBRxConf); /* CR10 */
+	BBbWriteEmbedded(priv, 0x0A, byBBRxConf); /* CR10 */
 }
 
-void BBvSetVGAGainOffset(struct vnt_private *pDevice, unsigned char byData)
+void BBvSetVGAGainOffset(struct vnt_private *priv, unsigned char byData)
 {
 	unsigned char byBBRxConf = 0;
 
-	BBbWriteEmbedded(pDevice->PortOffset, 0xE7, byData);
+	BBbWriteEmbedded(priv, 0xE7, byData);
 
-	BBbReadEmbedded(pDevice->PortOffset, 0x0A, &byBBRxConf); /* CR10 */
+	BBbReadEmbedded(priv, 0x0A, &byBBRxConf); /* CR10 */
 	/* patch for 3253B0 Baseband with Cardbus module */
-	if (byData == pDevice->abyBBVGA[0])
+	if (byData == priv->abyBBVGA[0])
 		byBBRxConf |= 0x20; /* 0010 0000 */
-	else if (pDevice->bShortSlotTime)
+	else if (priv->bShortSlotTime)
 		byBBRxConf &= 0xDF; /* 1101 1111 */
 	else
 		byBBRxConf |= 0x20; /* 0010 0000 */
-	pDevice->byBBVGACurrent = byData;
-	BBbWriteEmbedded(pDevice->PortOffset, 0x0A, byBBRxConf); /* CR10 */
+	priv->byBBVGACurrent = byData;
+	BBbWriteEmbedded(priv, 0x0A, byBBRxConf); /* CR10 */
 }
 
 /*
@@ -2430,12 +2270,12 @@ void BBvSetVGAGainOffset(struct vnt_private *pDevice, unsigned char byData)
  *
  */
 void
-BBvSoftwareReset(void __iomem *dwIoBase)
+BBvSoftwareReset(struct vnt_private *priv)
 {
-	BBbWriteEmbedded(dwIoBase, 0x50, 0x40);
-	BBbWriteEmbedded(dwIoBase, 0x50, 0);
-	BBbWriteEmbedded(dwIoBase, 0x9C, 0x01);
-	BBbWriteEmbedded(dwIoBase, 0x9C, 0);
+	BBbWriteEmbedded(priv, 0x50, 0x40);
+	BBbWriteEmbedded(priv, 0x50, 0);
+	BBbWriteEmbedded(priv, 0x9C, 0x01);
+	BBbWriteEmbedded(priv, 0x9C, 0);
 }
 
 /*
@@ -2451,13 +2291,13 @@ BBvSoftwareReset(void __iomem *dwIoBase)
  *
  */
 void
-BBvPowerSaveModeON(void __iomem *dwIoBase)
+BBvPowerSaveModeON(struct vnt_private *priv)
 {
 	unsigned char byOrgData;
 
-	BBbReadEmbedded(dwIoBase, 0x0D, &byOrgData);
-	byOrgData |= BIT0;
-	BBbWriteEmbedded(dwIoBase, 0x0D, byOrgData);
+	BBbReadEmbedded(priv, 0x0D, &byOrgData);
+	byOrgData |= BIT(0);
+	BBbWriteEmbedded(priv, 0x0D, byOrgData);
 }
 
 /*
@@ -2473,13 +2313,13 @@ BBvPowerSaveModeON(void __iomem *dwIoBase)
  *
  */
 void
-BBvPowerSaveModeOFF(void __iomem *dwIoBase)
+BBvPowerSaveModeOFF(struct vnt_private *priv)
 {
 	unsigned char byOrgData;
 
-	BBbReadEmbedded(dwIoBase, 0x0D, &byOrgData);
-	byOrgData &= ~(BIT0);
-	BBbWriteEmbedded(dwIoBase, 0x0D, byOrgData);
+	BBbReadEmbedded(priv, 0x0D, &byOrgData);
+	byOrgData &= ~(BIT(0));
+	BBbWriteEmbedded(priv, 0x0D, byOrgData);
 }
 
 /*
@@ -2487,7 +2327,7 @@ BBvPowerSaveModeOFF(void __iomem *dwIoBase)
  *
  * Parameters:
  *  In:
- *      pDevice          - Device Structure
+ *      priv          - Device Structure
  *      byAntennaMode    - Antenna Mode
  *  Out:
  *      none
@@ -2497,11 +2337,11 @@ BBvPowerSaveModeOFF(void __iomem *dwIoBase)
  */
 
 void
-BBvSetTxAntennaMode(void __iomem *dwIoBase, unsigned char byAntennaMode)
+BBvSetTxAntennaMode(struct vnt_private *priv, unsigned char byAntennaMode)
 {
 	unsigned char byBBTxConf;
 
-	BBbReadEmbedded(dwIoBase, 0x09, &byBBTxConf); /* CR09 */
+	BBbReadEmbedded(priv, 0x09, &byBBTxConf); /* CR09 */
 	if (byAntennaMode == ANT_DIVERSITY) {
 		/* bit 1 is diversity */
 		byBBTxConf |= 0x02;
@@ -2512,7 +2352,7 @@ BBvSetTxAntennaMode(void __iomem *dwIoBase, unsigned char byAntennaMode)
 		byBBTxConf &= 0xFD; /* 1111 1101 */
 		byBBTxConf |= 0x04;
 	}
-	BBbWriteEmbedded(dwIoBase, 0x09, byBBTxConf); /* CR09 */
+	BBbWriteEmbedded(priv, 0x09, byBBTxConf); /* CR09 */
 }
 
 /*
@@ -2520,7 +2360,7 @@ BBvSetTxAntennaMode(void __iomem *dwIoBase, unsigned char byAntennaMode)
  *
  * Parameters:
  *  In:
- *      pDevice          - Device Structure
+ *      priv          - Device Structure
  *      byAntennaMode    - Antenna Mode
  *  Out:
  *      none
@@ -2530,11 +2370,11 @@ BBvSetTxAntennaMode(void __iomem *dwIoBase, unsigned char byAntennaMode)
  */
 
 void
-BBvSetRxAntennaMode(void __iomem *dwIoBase, unsigned char byAntennaMode)
+BBvSetRxAntennaMode(struct vnt_private *priv, unsigned char byAntennaMode)
 {
 	unsigned char byBBRxConf;
 
-	BBbReadEmbedded(dwIoBase, 0x0A, &byBBRxConf); /* CR10 */
+	BBbReadEmbedded(priv, 0x0A, &byBBRxConf); /* CR10 */
 	if (byAntennaMode == ANT_DIVERSITY) {
 		byBBRxConf |= 0x01;
 
@@ -2544,7 +2384,7 @@ BBvSetRxAntennaMode(void __iomem *dwIoBase, unsigned char byAntennaMode)
 		byBBRxConf &= 0xFE; /* 1111 1110 */
 		byBBRxConf |= 0x02;
 	}
-	BBbWriteEmbedded(dwIoBase, 0x0A, byBBRxConf); /* CR10 */
+	BBbWriteEmbedded(priv, 0x0A, byBBRxConf); /* CR10 */
 }
 
 /*
@@ -2552,7 +2392,7 @@ BBvSetRxAntennaMode(void __iomem *dwIoBase, unsigned char byAntennaMode)
  *
  * Parameters:
  *  In:
- *      pDevice          - Device Structure
+ *      priv          - Device Structure
  *  Out:
  *      none
  *
@@ -2560,109 +2400,109 @@ BBvSetRxAntennaMode(void __iomem *dwIoBase, unsigned char byAntennaMode)
  *
  */
 void
-BBvSetDeepSleep(void __iomem *dwIoBase, unsigned char byLocalID)
+BBvSetDeepSleep(struct vnt_private *priv, unsigned char byLocalID)
 {
-	BBbWriteEmbedded(dwIoBase, 0x0C, 0x17); /* CR12 */
-	BBbWriteEmbedded(dwIoBase, 0x0D, 0xB9); /* CR13 */
+	BBbWriteEmbedded(priv, 0x0C, 0x17); /* CR12 */
+	BBbWriteEmbedded(priv, 0x0D, 0xB9); /* CR13 */
 }
 
 void
-BBvExitDeepSleep(void __iomem *dwIoBase, unsigned char byLocalID)
+BBvExitDeepSleep(struct vnt_private *priv, unsigned char byLocalID)
 {
-	BBbWriteEmbedded(dwIoBase, 0x0C, 0x00); /* CR12 */
-	BBbWriteEmbedded(dwIoBase, 0x0D, 0x01); /* CR13 */
+	BBbWriteEmbedded(priv, 0x0C, 0x00); /* CR12 */
+	BBbWriteEmbedded(priv, 0x0D, 0x01); /* CR13 */
 }
 
 static
 unsigned long
-s_ulGetRatio(struct vnt_private *pDevice)
+s_ulGetRatio(struct vnt_private *priv)
 {
 	unsigned long ulRatio = 0;
 	unsigned long ulMaxPacket;
 	unsigned long ulPacketNum;
 
 	/* This is a thousand-ratio */
-	ulMaxPacket = pDevice->uNumSQ3[RATE_54M];
-	if (pDevice->uNumSQ3[RATE_54M] != 0) {
-		ulPacketNum = pDevice->uNumSQ3[RATE_54M];
-		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+	ulMaxPacket = priv->uNumSQ3[RATE_54M];
+	if (priv->uNumSQ3[RATE_54M] != 0) {
+		ulPacketNum = priv->uNumSQ3[RATE_54M];
+		ulRatio = (ulPacketNum * 1000 / priv->uDiversityCnt);
 		ulRatio += TOP_RATE_54M;
 	}
-	if (pDevice->uNumSQ3[RATE_48M] > ulMaxPacket) {
-		ulPacketNum = pDevice->uNumSQ3[RATE_54M] + pDevice->uNumSQ3[RATE_48M];
-		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+	if (priv->uNumSQ3[RATE_48M] > ulMaxPacket) {
+		ulPacketNum = priv->uNumSQ3[RATE_54M] + priv->uNumSQ3[RATE_48M];
+		ulRatio = (ulPacketNum * 1000 / priv->uDiversityCnt);
 		ulRatio += TOP_RATE_48M;
-		ulMaxPacket = pDevice->uNumSQ3[RATE_48M];
+		ulMaxPacket = priv->uNumSQ3[RATE_48M];
 	}
-	if (pDevice->uNumSQ3[RATE_36M] > ulMaxPacket) {
-		ulPacketNum = pDevice->uNumSQ3[RATE_54M] + pDevice->uNumSQ3[RATE_48M] +
-			pDevice->uNumSQ3[RATE_36M];
-		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+	if (priv->uNumSQ3[RATE_36M] > ulMaxPacket) {
+		ulPacketNum = priv->uNumSQ3[RATE_54M] + priv->uNumSQ3[RATE_48M] +
+			priv->uNumSQ3[RATE_36M];
+		ulRatio = (ulPacketNum * 1000 / priv->uDiversityCnt);
 		ulRatio += TOP_RATE_36M;
-		ulMaxPacket = pDevice->uNumSQ3[RATE_36M];
+		ulMaxPacket = priv->uNumSQ3[RATE_36M];
 	}
-	if (pDevice->uNumSQ3[RATE_24M] > ulMaxPacket) {
-		ulPacketNum = pDevice->uNumSQ3[RATE_54M] + pDevice->uNumSQ3[RATE_48M] +
-			pDevice->uNumSQ3[RATE_36M] + pDevice->uNumSQ3[RATE_24M];
-		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+	if (priv->uNumSQ3[RATE_24M] > ulMaxPacket) {
+		ulPacketNum = priv->uNumSQ3[RATE_54M] + priv->uNumSQ3[RATE_48M] +
+			priv->uNumSQ3[RATE_36M] + priv->uNumSQ3[RATE_24M];
+		ulRatio = (ulPacketNum * 1000 / priv->uDiversityCnt);
 		ulRatio += TOP_RATE_24M;
-		ulMaxPacket = pDevice->uNumSQ3[RATE_24M];
+		ulMaxPacket = priv->uNumSQ3[RATE_24M];
 	}
-	if (pDevice->uNumSQ3[RATE_18M] > ulMaxPacket) {
-		ulPacketNum = pDevice->uNumSQ3[RATE_54M] + pDevice->uNumSQ3[RATE_48M] +
-			pDevice->uNumSQ3[RATE_36M] + pDevice->uNumSQ3[RATE_24M] +
-			pDevice->uNumSQ3[RATE_18M];
-		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+	if (priv->uNumSQ3[RATE_18M] > ulMaxPacket) {
+		ulPacketNum = priv->uNumSQ3[RATE_54M] + priv->uNumSQ3[RATE_48M] +
+			priv->uNumSQ3[RATE_36M] + priv->uNumSQ3[RATE_24M] +
+			priv->uNumSQ3[RATE_18M];
+		ulRatio = (ulPacketNum * 1000 / priv->uDiversityCnt);
 		ulRatio += TOP_RATE_18M;
-		ulMaxPacket = pDevice->uNumSQ3[RATE_18M];
+		ulMaxPacket = priv->uNumSQ3[RATE_18M];
 	}
-	if (pDevice->uNumSQ3[RATE_12M] > ulMaxPacket) {
-		ulPacketNum = pDevice->uNumSQ3[RATE_54M] + pDevice->uNumSQ3[RATE_48M] +
-			pDevice->uNumSQ3[RATE_36M] + pDevice->uNumSQ3[RATE_24M] +
-			pDevice->uNumSQ3[RATE_18M] + pDevice->uNumSQ3[RATE_12M];
-		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+	if (priv->uNumSQ3[RATE_12M] > ulMaxPacket) {
+		ulPacketNum = priv->uNumSQ3[RATE_54M] + priv->uNumSQ3[RATE_48M] +
+			priv->uNumSQ3[RATE_36M] + priv->uNumSQ3[RATE_24M] +
+			priv->uNumSQ3[RATE_18M] + priv->uNumSQ3[RATE_12M];
+		ulRatio = (ulPacketNum * 1000 / priv->uDiversityCnt);
 		ulRatio += TOP_RATE_12M;
-		ulMaxPacket = pDevice->uNumSQ3[RATE_12M];
+		ulMaxPacket = priv->uNumSQ3[RATE_12M];
 	}
-	if (pDevice->uNumSQ3[RATE_11M] > ulMaxPacket) {
-		ulPacketNum = pDevice->uDiversityCnt - pDevice->uNumSQ3[RATE_1M] -
-			pDevice->uNumSQ3[RATE_2M] - pDevice->uNumSQ3[RATE_5M] -
-			pDevice->uNumSQ3[RATE_6M] - pDevice->uNumSQ3[RATE_9M];
-		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+	if (priv->uNumSQ3[RATE_11M] > ulMaxPacket) {
+		ulPacketNum = priv->uDiversityCnt - priv->uNumSQ3[RATE_1M] -
+			priv->uNumSQ3[RATE_2M] - priv->uNumSQ3[RATE_5M] -
+			priv->uNumSQ3[RATE_6M] - priv->uNumSQ3[RATE_9M];
+		ulRatio = (ulPacketNum * 1000 / priv->uDiversityCnt);
 		ulRatio += TOP_RATE_11M;
-		ulMaxPacket = pDevice->uNumSQ3[RATE_11M];
+		ulMaxPacket = priv->uNumSQ3[RATE_11M];
 	}
-	if (pDevice->uNumSQ3[RATE_9M] > ulMaxPacket) {
-		ulPacketNum = pDevice->uDiversityCnt - pDevice->uNumSQ3[RATE_1M] -
-			pDevice->uNumSQ3[RATE_2M] - pDevice->uNumSQ3[RATE_5M] -
-			pDevice->uNumSQ3[RATE_6M];
-		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+	if (priv->uNumSQ3[RATE_9M] > ulMaxPacket) {
+		ulPacketNum = priv->uDiversityCnt - priv->uNumSQ3[RATE_1M] -
+			priv->uNumSQ3[RATE_2M] - priv->uNumSQ3[RATE_5M] -
+			priv->uNumSQ3[RATE_6M];
+		ulRatio = (ulPacketNum * 1000 / priv->uDiversityCnt);
 		ulRatio += TOP_RATE_9M;
-		ulMaxPacket = pDevice->uNumSQ3[RATE_9M];
+		ulMaxPacket = priv->uNumSQ3[RATE_9M];
 	}
-	if (pDevice->uNumSQ3[RATE_6M] > ulMaxPacket) {
-		ulPacketNum = pDevice->uDiversityCnt - pDevice->uNumSQ3[RATE_1M] -
-			pDevice->uNumSQ3[RATE_2M] - pDevice->uNumSQ3[RATE_5M];
-		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+	if (priv->uNumSQ3[RATE_6M] > ulMaxPacket) {
+		ulPacketNum = priv->uDiversityCnt - priv->uNumSQ3[RATE_1M] -
+			priv->uNumSQ3[RATE_2M] - priv->uNumSQ3[RATE_5M];
+		ulRatio = (ulPacketNum * 1000 / priv->uDiversityCnt);
 		ulRatio += TOP_RATE_6M;
-		ulMaxPacket = pDevice->uNumSQ3[RATE_6M];
+		ulMaxPacket = priv->uNumSQ3[RATE_6M];
 	}
-	if (pDevice->uNumSQ3[RATE_5M] > ulMaxPacket) {
-		ulPacketNum = pDevice->uDiversityCnt - pDevice->uNumSQ3[RATE_1M] -
-			pDevice->uNumSQ3[RATE_2M];
-		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+	if (priv->uNumSQ3[RATE_5M] > ulMaxPacket) {
+		ulPacketNum = priv->uDiversityCnt - priv->uNumSQ3[RATE_1M] -
+			priv->uNumSQ3[RATE_2M];
+		ulRatio = (ulPacketNum * 1000 / priv->uDiversityCnt);
 		ulRatio += TOP_RATE_55M;
-		ulMaxPacket = pDevice->uNumSQ3[RATE_5M];
+		ulMaxPacket = priv->uNumSQ3[RATE_5M];
 	}
-	if (pDevice->uNumSQ3[RATE_2M] > ulMaxPacket) {
-		ulPacketNum = pDevice->uDiversityCnt - pDevice->uNumSQ3[RATE_1M];
-		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+	if (priv->uNumSQ3[RATE_2M] > ulMaxPacket) {
+		ulPacketNum = priv->uDiversityCnt - priv->uNumSQ3[RATE_1M];
+		ulRatio = (ulPacketNum * 1000 / priv->uDiversityCnt);
 		ulRatio += TOP_RATE_2M;
-		ulMaxPacket = pDevice->uNumSQ3[RATE_2M];
+		ulMaxPacket = priv->uNumSQ3[RATE_2M];
 	}
-	if (pDevice->uNumSQ3[RATE_1M] > ulMaxPacket) {
-		ulPacketNum = pDevice->uDiversityCnt;
-		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
+	if (priv->uNumSQ3[RATE_1M] > ulMaxPacket) {
+		ulPacketNum = priv->uDiversityCnt;
+		ulRatio = (ulPacketNum * 1000 / priv->uDiversityCnt);
 		ulRatio += TOP_RATE_1M;
 	}
 
@@ -2670,13 +2510,13 @@ s_ulGetRatio(struct vnt_private *pDevice)
 }
 
 void
-BBvClearAntDivSQ3Value(struct vnt_private *pDevice)
+BBvClearAntDivSQ3Value(struct vnt_private *priv)
 {
 	unsigned int ii;
 
-	pDevice->uDiversityCnt = 0;
+	priv->uDiversityCnt = 0;
 	for (ii = 0; ii < MAX_RATE; ii++)
-		pDevice->uNumSQ3[ii] = 0;
+		priv->uNumSQ3[ii] = 0;
 }
 
 /*
@@ -2684,7 +2524,7 @@ BBvClearAntDivSQ3Value(struct vnt_private *pDevice)
  *
  * Parameters:
  *  In:
- *      pDevice          - Device Structure
+ *      priv          - Device Structure
  *      byRSR            - RSR from received packet
  *      bySQ3            - SQ3 value from received packet
  *  Out:
@@ -2694,75 +2534,75 @@ BBvClearAntDivSQ3Value(struct vnt_private *pDevice)
  *
  */
 
-void BBvAntennaDiversity(struct vnt_private *pDevice,
+void BBvAntennaDiversity(struct vnt_private *priv,
 			 unsigned char byRxRate, unsigned char bySQ3)
 {
-	if ((byRxRate >= MAX_RATE) || (pDevice->wAntDiversityMaxRate >= MAX_RATE))
+	if ((byRxRate >= MAX_RATE) || (priv->wAntDiversityMaxRate >= MAX_RATE))
 		return;
 
-	pDevice->uDiversityCnt++;
+	priv->uDiversityCnt++;
 
-	pDevice->uNumSQ3[byRxRate]++;
+	priv->uNumSQ3[byRxRate]++;
 
-	if (pDevice->byAntennaState == 0) {
-		if (pDevice->uDiversityCnt > pDevice->ulDiversityNValue) {
+	if (priv->byAntennaState == 0) {
+		if (priv->uDiversityCnt > priv->ulDiversityNValue) {
 			pr_debug("ulDiversityNValue=[%d],54M-[%d]\n",
-				 (int)pDevice->ulDiversityNValue,
-				 (int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate]);
+				 (int)priv->ulDiversityNValue,
+				 (int)priv->uNumSQ3[(int)priv->wAntDiversityMaxRate]);
 
-			if (pDevice->uNumSQ3[pDevice->wAntDiversityMaxRate] < pDevice->uDiversityCnt/2) {
-				pDevice->ulRatio_State0 = s_ulGetRatio(pDevice);
+			if (priv->uNumSQ3[priv->wAntDiversityMaxRate] < priv->uDiversityCnt/2) {
+				priv->ulRatio_State0 = s_ulGetRatio(priv);
 				pr_debug("SQ3_State0, rate = [%08x]\n",
-					 (int)pDevice->ulRatio_State0);
+					 (int)priv->ulRatio_State0);
 
-				if (pDevice->byTMax == 0)
+				if (priv->byTMax == 0)
 					return;
 				pr_debug("1.[%08x], uNumSQ3[%d]=%d, %d\n",
-					 (int)pDevice->ulRatio_State0,
-					 (int)pDevice->wAntDiversityMaxRate,
-					 (int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate],
-					 (int)pDevice->uDiversityCnt);
-
-				s_vChangeAntenna(pDevice);
-				pDevice->byAntennaState = 1;
-				del_timer(&pDevice->TimerSQ3Tmax3);
-				del_timer(&pDevice->TimerSQ3Tmax2);
-				pDevice->TimerSQ3Tmax1.expires =  RUN_AT(pDevice->byTMax * HZ);
-				add_timer(&pDevice->TimerSQ3Tmax1);
+					 (int)priv->ulRatio_State0,
+					 (int)priv->wAntDiversityMaxRate,
+					 (int)priv->uNumSQ3[(int)priv->wAntDiversityMaxRate],
+					 (int)priv->uDiversityCnt);
+
+				s_vChangeAntenna(priv);
+				priv->byAntennaState = 1;
+				del_timer(&priv->TimerSQ3Tmax3);
+				del_timer(&priv->TimerSQ3Tmax2);
+				priv->TimerSQ3Tmax1.expires =  RUN_AT(priv->byTMax * HZ);
+				add_timer(&priv->TimerSQ3Tmax1);
 
 			} else {
-				pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
-				add_timer(&pDevice->TimerSQ3Tmax3);
+				priv->TimerSQ3Tmax3.expires =  RUN_AT(priv->byTMax3 * HZ);
+				add_timer(&priv->TimerSQ3Tmax3);
 			}
-			BBvClearAntDivSQ3Value(pDevice);
+			BBvClearAntDivSQ3Value(priv);
 
 		}
 	} else { /* byAntennaState == 1 */
 
-		if (pDevice->uDiversityCnt > pDevice->ulDiversityMValue) {
-			del_timer(&pDevice->TimerSQ3Tmax1);
+		if (priv->uDiversityCnt > priv->ulDiversityMValue) {
+			del_timer(&priv->TimerSQ3Tmax1);
 
-			pDevice->ulRatio_State1 = s_ulGetRatio(pDevice);
+			priv->ulRatio_State1 = s_ulGetRatio(priv);
 			pr_debug("RX:SQ3_State1, rate0 = %08x,rate1 = %08x\n",
-				 (int)pDevice->ulRatio_State0,
-				 (int)pDevice->ulRatio_State1);
+				 (int)priv->ulRatio_State0,
+				 (int)priv->ulRatio_State1);
 
-			if (pDevice->ulRatio_State1 < pDevice->ulRatio_State0) {
+			if (priv->ulRatio_State1 < priv->ulRatio_State0) {
 				pr_debug("2.[%08x][%08x], uNumSQ3[%d]=%d, %d\n",
-					 (int)pDevice->ulRatio_State0,
-					 (int)pDevice->ulRatio_State1,
-					 (int)pDevice->wAntDiversityMaxRate,
-					 (int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate],
-					 (int)pDevice->uDiversityCnt);
-
-				s_vChangeAntenna(pDevice);
-				pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
-				pDevice->TimerSQ3Tmax2.expires =  RUN_AT(pDevice->byTMax2 * HZ);
-				add_timer(&pDevice->TimerSQ3Tmax3);
-				add_timer(&pDevice->TimerSQ3Tmax2);
+					 (int)priv->ulRatio_State0,
+					 (int)priv->ulRatio_State1,
+					 (int)priv->wAntDiversityMaxRate,
+					 (int)priv->uNumSQ3[(int)priv->wAntDiversityMaxRate],
+					 (int)priv->uDiversityCnt);
+
+				s_vChangeAntenna(priv);
+				priv->TimerSQ3Tmax3.expires =  RUN_AT(priv->byTMax3 * HZ);
+				priv->TimerSQ3Tmax2.expires =  RUN_AT(priv->byTMax2 * HZ);
+				add_timer(&priv->TimerSQ3Tmax3);
+				add_timer(&priv->TimerSQ3Tmax2);
 			}
-			pDevice->byAntennaState = 0;
-			BBvClearAntDivSQ3Value(pDevice);
+			priv->byAntennaState = 0;
+			BBvClearAntDivSQ3Value(priv);
 		}
 	} /* byAntennaState */
 }
@@ -2783,28 +2623,30 @@ void BBvAntennaDiversity(struct vnt_private *pDevice,
 
 void
 TimerSQ3CallBack(
-	void *hDeviceContext
+	unsigned long data
 )
 {
-	struct vnt_private *pDevice = hDeviceContext;
+	struct vnt_private *priv = (struct vnt_private *)data;
+	unsigned long flags;
 
 	pr_debug("TimerSQ3CallBack...\n");
-	spin_lock_irq(&pDevice->lock);
+
+	spin_lock_irqsave(&priv->lock, flags);
 
 	pr_debug("3.[%08x][%08x], %d\n",
-		 (int)pDevice->ulRatio_State0, (int)pDevice->ulRatio_State1,
-		 (int)pDevice->uDiversityCnt);
+		 (int)priv->ulRatio_State0, (int)priv->ulRatio_State1,
+		 (int)priv->uDiversityCnt);
 
-	s_vChangeAntenna(pDevice);
-	pDevice->byAntennaState = 0;
-	BBvClearAntDivSQ3Value(pDevice);
+	s_vChangeAntenna(priv);
+	priv->byAntennaState = 0;
+	BBvClearAntDivSQ3Value(priv);
 
-	pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
-	pDevice->TimerSQ3Tmax2.expires =  RUN_AT(pDevice->byTMax2 * HZ);
-	add_timer(&pDevice->TimerSQ3Tmax3);
-	add_timer(&pDevice->TimerSQ3Tmax2);
+	priv->TimerSQ3Tmax3.expires =  RUN_AT(priv->byTMax3 * HZ);
+	priv->TimerSQ3Tmax2.expires =  RUN_AT(priv->byTMax2 * HZ);
+	add_timer(&priv->TimerSQ3Tmax3);
+	add_timer(&priv->TimerSQ3Tmax2);
 
-	spin_unlock_irq(&pDevice->lock);
+	spin_unlock_irqrestore(&priv->lock, flags);
 }
 
 /*+
@@ -2827,43 +2669,46 @@ TimerSQ3CallBack(
 
 void
 TimerState1CallBack(
-	void *hDeviceContext
+	unsigned long data
 )
 {
-	struct vnt_private *pDevice = hDeviceContext;
+	struct vnt_private *priv = (struct vnt_private *)data;
+	unsigned long flags;
 
 	pr_debug("TimerState1CallBack...\n");
 
-	spin_lock_irq(&pDevice->lock);
-	if (pDevice->uDiversityCnt < pDevice->ulDiversityMValue/100) {
-		s_vChangeAntenna(pDevice);
-		pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
-		pDevice->TimerSQ3Tmax2.expires =  RUN_AT(pDevice->byTMax2 * HZ);
-		add_timer(&pDevice->TimerSQ3Tmax3);
-		add_timer(&pDevice->TimerSQ3Tmax2);
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if (priv->uDiversityCnt < priv->ulDiversityMValue/100) {
+		s_vChangeAntenna(priv);
+		priv->TimerSQ3Tmax3.expires =  RUN_AT(priv->byTMax3 * HZ);
+		priv->TimerSQ3Tmax2.expires =  RUN_AT(priv->byTMax2 * HZ);
+		add_timer(&priv->TimerSQ3Tmax3);
+		add_timer(&priv->TimerSQ3Tmax2);
 	} else {
-		pDevice->ulRatio_State1 = s_ulGetRatio(pDevice);
+		priv->ulRatio_State1 = s_ulGetRatio(priv);
 		pr_debug("SQ3_State1, rate0 = %08x,rate1 = %08x\n",
-			 (int)pDevice->ulRatio_State0,
-			 (int)pDevice->ulRatio_State1);
+			 (int)priv->ulRatio_State0,
+			 (int)priv->ulRatio_State1);
 
-		if (pDevice->ulRatio_State1 < pDevice->ulRatio_State0) {
+		if (priv->ulRatio_State1 < priv->ulRatio_State0) {
 			pr_debug("2.[%08x][%08x], uNumSQ3[%d]=%d, %d\n",
-				 (int)pDevice->ulRatio_State0,
-				 (int)pDevice->ulRatio_State1,
-				 (int)pDevice->wAntDiversityMaxRate,
-				 (int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate],
-				 (int)pDevice->uDiversityCnt);
-
-			s_vChangeAntenna(pDevice);
-
-			pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
-			pDevice->TimerSQ3Tmax2.expires =  RUN_AT(pDevice->byTMax2 * HZ);
-			add_timer(&pDevice->TimerSQ3Tmax3);
-			add_timer(&pDevice->TimerSQ3Tmax2);
+				 (int)priv->ulRatio_State0,
+				 (int)priv->ulRatio_State1,
+				 (int)priv->wAntDiversityMaxRate,
+				 (int)priv->uNumSQ3[(int)priv->wAntDiversityMaxRate],
+				 (int)priv->uDiversityCnt);
+
+			s_vChangeAntenna(priv);
+
+			priv->TimerSQ3Tmax3.expires =  RUN_AT(priv->byTMax3 * HZ);
+			priv->TimerSQ3Tmax2.expires =  RUN_AT(priv->byTMax2 * HZ);
+			add_timer(&priv->TimerSQ3Tmax3);
+			add_timer(&priv->TimerSQ3Tmax2);
 		}
 	}
-	pDevice->byAntennaState = 0;
-	BBvClearAntDivSQ3Value(pDevice);
-	spin_unlock_irq(&pDevice->lock);
+	priv->byAntennaState = 0;
+	BBvClearAntDivSQ3Value(priv);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
 }
diff --git a/drivers/staging/vt6655/baseband.h b/drivers/staging/vt6655/baseband.h
index 31f2255519cf..d9f6d63e4ab7 100644
--- a/drivers/staging/vt6655/baseband.h
+++ b/drivers/staging/vt6655/baseband.h
@@ -30,8 +30,6 @@
 #ifndef __BASEBAND_H__
 #define __BASEBAND_H__
 
-#include "ttype.h"
-#include "tether.h"
 #include "device.h"
 
 /*
@@ -79,42 +77,37 @@ BBuGetFrameTime(
 void vnt_get_phy_field(struct vnt_private *, u32 frame_length,
 		       u16 tx_rate, u8 pkt_type, struct vnt_phy_field *);
 
-bool BBbReadEmbedded(void __iomem *dwIoBase, unsigned char byBBAddr, unsigned char *pbyData);
-bool BBbWriteEmbedded(void __iomem *dwIoBase, unsigned char byBBAddr, unsigned char byData);
+bool BBbReadEmbedded(struct vnt_private *, unsigned char byBBAddr, unsigned char *pbyData);
+bool BBbWriteEmbedded(struct vnt_private *, unsigned char byBBAddr, unsigned char byData);
 
-void BBvReadAllRegs(void __iomem *dwIoBase, unsigned char *pbyBBRegs);
-void BBvLoopbackOn(struct vnt_private *pDevice);
-void BBvLoopbackOff(struct vnt_private *pDevice);
-void BBvSetShortSlotTime(struct vnt_private *pDevice);
-bool BBbIsRegBitsOn(void __iomem *dwIoBase, unsigned char byBBAddr, unsigned char byTestBits);
-bool BBbIsRegBitsOff(void __iomem *dwIoBase, unsigned char byBBAddr, unsigned char byTestBits);
-void BBvSetVGAGainOffset(struct vnt_private *pDevice, unsigned char byData);
+void BBvSetShortSlotTime(struct vnt_private *);
+void BBvSetVGAGainOffset(struct vnt_private *, unsigned char byData);
 
 /* VT3253 Baseband */
-bool BBbVT3253Init(struct vnt_private *pDevice);
-void BBvSoftwareReset(void __iomem *dwIoBase);
-void BBvPowerSaveModeON(void __iomem *dwIoBase);
-void BBvPowerSaveModeOFF(void __iomem *dwIoBase);
-void BBvSetTxAntennaMode(void __iomem *dwIoBase, unsigned char byAntennaMode);
-void BBvSetRxAntennaMode(void __iomem *dwIoBase, unsigned char byAntennaMode);
-void BBvSetDeepSleep(void __iomem *dwIoBase, unsigned char byLocalID);
-void BBvExitDeepSleep(void __iomem *dwIoBase, unsigned char byLocalID);
+bool BBbVT3253Init(struct vnt_private *);
+void BBvSoftwareReset(struct vnt_private *);
+void BBvPowerSaveModeON(struct vnt_private *);
+void BBvPowerSaveModeOFF(struct vnt_private *);
+void BBvSetTxAntennaMode(struct vnt_private *, unsigned char byAntennaMode);
+void BBvSetRxAntennaMode(struct vnt_private *, unsigned char byAntennaMode);
+void BBvSetDeepSleep(struct vnt_private *, unsigned char byLocalID);
+void BBvExitDeepSleep(struct vnt_private *, unsigned char byLocalID);
 
 /* timer for antenna diversity */
 
 void
 TimerSQ3CallBack(
-	void *hDeviceContext
+	unsigned long
 );
 
 void
 TimerState1CallBack(
-	void *hDeviceContext
+	unsigned long
 );
 
-void BBvAntennaDiversity(struct vnt_private *pDevice,
+void BBvAntennaDiversity(struct vnt_private *,
 			 unsigned char byRxRate, unsigned char bySQ3);
 void
-BBvClearAntDivSQ3Value(struct vnt_private *pDevice);
+BBvClearAntDivSQ3Value(struct vnt_private *);
 
 #endif /* __BASEBAND_H__ */
diff --git a/drivers/staging/vt6655/bssdb.c b/drivers/staging/vt6655/bssdb.c
deleted file mode 100644
index 996d3302ce3d..000000000000
--- a/drivers/staging/vt6655/bssdb.c
+++ /dev/null
@@ -1,1512 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: bssdb.c
- *
- * Purpose: Handles the Basic Service Set & Node Database functions
- *
- * Functions:
- *      BSSpSearchBSSList - Search known BSS list for Desire SSID or BSSID
- *      BSSvClearBSSList - Clear BSS List
- *      BSSbInsertToBSSList - Insert a BSS set into known BSS list
- *      BSSbUpdateToBSSList - Update BSS set in known BSS list
- *      BSSDBbIsSTAInNodeDB - Search Node DB table to find the index of matched DstAddr
- *      BSSvCreateOneNode - Allocate an Node for Node DB
- *      BSSvUpdateAPNode - Update AP Node content in Index 0 of KnownNodeDB
- *      BSSvSecondCallBack - One second timer callback function to update Node DB info & AP link status
- *      BSSvUpdateNodeTxCounter - Update Tx attemps, Tx failure counter in Node DB for auto-fall back rate control
- *
- * Revision History:
- *
- * Author: Lyndon Chen
- *
- * Date: July 17, 2002
- *
- */
-
-#include "ttype.h"
-#include "tmacro.h"
-#include "tether.h"
-#include "device.h"
-#include "80211hdr.h"
-#include "bssdb.h"
-#include "wmgr.h"
-#include "datarate.h"
-#include "desc.h"
-#include "wcmd.h"
-#include "wpa.h"
-#include "baseband.h"
-#include "rf.h"
-#include "card.h"
-#include "channel.h"
-#include "mac.h"
-#include "wpa2.h"
-#include "iowpa.h"
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-static const unsigned short awHWRetry0[5][5] = {
-	{RATE_18M, RATE_18M, RATE_12M, RATE_12M, RATE_12M},
-	{RATE_24M, RATE_24M, RATE_18M, RATE_12M, RATE_12M},
-	{RATE_36M, RATE_36M, RATE_24M, RATE_18M, RATE_18M},
-	{RATE_48M, RATE_48M, RATE_36M, RATE_24M, RATE_24M},
-	{RATE_54M, RATE_54M, RATE_48M, RATE_36M, RATE_36M}
-};
-static const unsigned short awHWRetry1[5][5] = {
-	{RATE_18M, RATE_18M, RATE_12M, RATE_6M, RATE_6M},
-	{RATE_24M, RATE_24M, RATE_18M, RATE_6M, RATE_6M},
-	{RATE_36M, RATE_36M, RATE_24M, RATE_12M, RATE_12M},
-	{RATE_48M, RATE_48M, RATE_24M, RATE_12M, RATE_12M},
-	{RATE_54M, RATE_54M, RATE_36M, RATE_18M, RATE_18M}
-};
-
-/*---------------------  Static Functions  --------------------------*/
-
-void s_vCheckSensitivity(
-	void *hDeviceContext
-);
-
-#ifdef Calcu_LinkQual
-void s_uCalculateLinkQual(
-	void *hDeviceContext
-);
-#endif
-
-void s_vCheckPreEDThreshold(
-	void *hDeviceContext
-);
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-/*+
- *
- * Routine Description:
- *    Search known BSS list for Desire SSID or BSSID.
- *
- * Return Value:
- *    PTR to KnownBSS or NULL
- *
- -*/
-
-PKnownBSS
-BSSpSearchBSSList(
-	void *hDeviceContext,
-	unsigned char *pbyDesireBSSID,
-	unsigned char *pbyDesireSSID,
-	CARD_PHY_TYPE  ePhyType
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	unsigned char *pbyBSSID = NULL;
-	PWLAN_IE_SSID   pSSID = NULL;
-	PKnownBSS       pCurrBSS = NULL;
-	PKnownBSS       pSelect = NULL;
-	unsigned char ZeroBSSID[WLAN_BSSID_LEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
-	unsigned int ii = 0;
-
-	if (pbyDesireBSSID != NULL) {
-		pr_debug("BSSpSearchBSSList BSSID[%pM]\n", pbyDesireBSSID);
-		if ((!is_broadcast_ether_addr(pbyDesireBSSID)) &&
-		    (memcmp(pbyDesireBSSID, ZeroBSSID, 6) != 0))
-			pbyBSSID = pbyDesireBSSID;
-	}
-	if (pbyDesireSSID != NULL) {
-		if (((PWLAN_IE_SSID)pbyDesireSSID)->len != 0)
-			pSSID = (PWLAN_IE_SSID) pbyDesireSSID;
-	}
-
-	if (pbyBSSID != NULL) {
-		/* match BSSID first */
-		for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-			pCurrBSS = &(pMgmt->sBSSList[ii]);
-			if (!pDevice->bLinkPass)
-				pCurrBSS->bSelected = false;
-			if ((pCurrBSS->bActive) &&
-			    (!pCurrBSS->bSelected)) {
-				if (ether_addr_equal(pCurrBSS->abyBSSID,
-						     pbyBSSID)) {
-					if (pSSID != NULL) {
-						/* compare ssid */
-						if (!memcmp(pSSID->abySSID,
-							    ((PWLAN_IE_SSID)pCurrBSS->abySSID)->abySSID,
-							    pSSID->len)) {
-							if ((pMgmt->eConfigMode == WMAC_CONFIG_AUTO) ||
-							    ((pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) && WLAN_GET_CAP_INFO_IBSS(pCurrBSS->wCapInfo)) ||
-							    ((pMgmt->eConfigMode == WMAC_CONFIG_ESS_STA) && WLAN_GET_CAP_INFO_ESS(pCurrBSS->wCapInfo))
-) {
-								pCurrBSS->bSelected = true;
-								return pCurrBSS;
-							}
-						}
-					} else {
-						if ((pMgmt->eConfigMode == WMAC_CONFIG_AUTO) ||
-						    ((pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) && WLAN_GET_CAP_INFO_IBSS(pCurrBSS->wCapInfo)) ||
-						    ((pMgmt->eConfigMode == WMAC_CONFIG_ESS_STA) && WLAN_GET_CAP_INFO_ESS(pCurrBSS->wCapInfo))
-) {
-							pCurrBSS->bSelected = true;
-							return pCurrBSS;
-						}
-					}
-				}
-			}
-		}
-	} else {
-		/* ignore BSSID */
-		for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-			pCurrBSS = &(pMgmt->sBSSList[ii]);
-			/* 2007-0721-01<Add>by MikeLiu */
-			pCurrBSS->bSelected = false;
-			if (pCurrBSS->bActive) {
-				if (pSSID != NULL) {
-					/* matched SSID */
-					if (!!memcmp(pSSID->abySSID,
-						     ((PWLAN_IE_SSID)pCurrBSS->abySSID)->abySSID,
-						     pSSID->len) ||
-					    (pSSID->len != ((PWLAN_IE_SSID)pCurrBSS->abySSID)->len)) {
-						/* SSID not match skip this BSS */
-						continue;
-					}
-				}
-				if (((pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) && WLAN_GET_CAP_INFO_ESS(pCurrBSS->wCapInfo)) ||
-				    ((pMgmt->eConfigMode == WMAC_CONFIG_ESS_STA) && WLAN_GET_CAP_INFO_IBSS(pCurrBSS->wCapInfo))
-) {
-					/* Type not match skip this BSS */
-					pr_debug("BSS type mismatch.... Config[%d] BSS[0x%04x]\n",
-						 pMgmt->eConfigMode,
-						 pCurrBSS->wCapInfo);
-					continue;
-				}
-
-				if (ePhyType != PHY_TYPE_AUTO) {
-					if (((ePhyType == PHY_TYPE_11A) && (PHY_TYPE_11A != pCurrBSS->eNetworkTypeInUse)) ||
-					    ((ePhyType != PHY_TYPE_11A) && (PHY_TYPE_11A == pCurrBSS->eNetworkTypeInUse))) {
-						/* PhyType not match skip this BSS */
-						pr_debug("Physical type mismatch.... ePhyType[%d] BSS[%d]\n",
-							 ePhyType,
-							 pCurrBSS->eNetworkTypeInUse);
-						continue;
-					}
-				}
-
-				if (pSelect == NULL) {
-					pSelect = pCurrBSS;
-				} else {
-					/* compare RSSI, select signal strong one */
-					if (pCurrBSS->uRSSI < pSelect->uRSSI)
-						pSelect = pCurrBSS;
-				}
-			}
-		}
-		if (pSelect != NULL) {
-			pSelect->bSelected = true;
-			return pSelect;
-		}
-	}
-	return NULL;
-}
-
-/*+
- *
- * Routine Description:
- *    Clear BSS List
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-BSSvClearBSSList(
-	void *hDeviceContext,
-	bool bKeepCurrBSSID
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	unsigned int ii;
-
-	for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-		if (bKeepCurrBSSID) {
-			if (pMgmt->sBSSList[ii].bActive &&
-			    ether_addr_equal(pMgmt->sBSSList[ii].abyBSSID,
-					     pMgmt->abyCurrBSSID)) {
-				continue;
-			}
-		}
-
-		if ((pMgmt->sBSSList[ii].bActive) && (pMgmt->sBSSList[ii].uClearCount < BSS_CLEAR_COUNT)) {
-			pMgmt->sBSSList[ii].uClearCount++;
-			continue;
-		}
-
-		pMgmt->sBSSList[ii].bActive = false;
-		memset(&pMgmt->sBSSList[ii], 0, sizeof(KnownBSS));
-	}
-	BSSvClearAnyBSSJoinRecord(pDevice);
-}
-
-/*+
- *
- * Routine Description:
- *    search BSS list by BSSID & SSID if matched
- *
- * Return Value:
- *    true if found.
- *
- -*/
-PKnownBSS
-BSSpAddrIsInBSSList(
-	void *hDeviceContext,
-	unsigned char *abyBSSID,
-	PWLAN_IE_SSID pSSID
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	PKnownBSS       pBSSList = NULL;
-	unsigned int ii;
-
-	for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-		pBSSList = &(pMgmt->sBSSList[ii]);
-		if (pBSSList->bActive) {
-			if (ether_addr_equal(pBSSList->abyBSSID, abyBSSID)) {
-				if (pSSID->len == ((PWLAN_IE_SSID)pBSSList->abySSID)->len) {
-					if (memcmp(pSSID->abySSID,
-						   ((PWLAN_IE_SSID)pBSSList->abySSID)->abySSID,
-						   pSSID->len) == 0)
-						return pBSSList;
-				}
-			}
-		}
-	}
-
-	return NULL;
-};
-
-/*+
- *
- * Routine Description:
- *    Insert a BSS set into known BSS list
- *
- * Return Value:
- *    true if success.
- *
- -*/
-
-bool
-BSSbInsertToBSSList(
-	void *hDeviceContext,
-	unsigned char *abyBSSIDAddr,
-	__le64 qwTimestamp,
-	unsigned short wBeaconInterval,
-	unsigned short wCapInfo,
-	unsigned char byCurrChannel,
-	PWLAN_IE_SSID pSSID,
-	PWLAN_IE_SUPP_RATES pSuppRates,
-	PWLAN_IE_SUPP_RATES pExtSuppRates,
-	PERPObject psERP,
-	PWLAN_IE_RSN pRSN,
-	PWLAN_IE_RSN_EXT pRSNWPA,
-	PWLAN_IE_COUNTRY pIE_Country,
-	PWLAN_IE_QUIET pIE_Quiet,
-	unsigned int uIELength,
-	unsigned char *pbyIEs,
-	void *pRxPacketContext
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	PSRxMgmtPacket  pRxPacket = (PSRxMgmtPacket)pRxPacketContext;
-	PKnownBSS       pBSSList = NULL;
-	unsigned int ii;
-	bool bParsingQuiet = false;
-	PWLAN_IE_QUIET  pQuiet = NULL;
-
-	pBSSList = (PKnownBSS)&(pMgmt->sBSSList[0]);
-
-	for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-		pBSSList = (PKnownBSS)&(pMgmt->sBSSList[ii]);
-		if (!pBSSList->bActive)
-			break;
-	}
-
-	if (ii == MAX_BSS_NUM) {
-		pr_debug("Get free KnowBSS node failed\n");
-		return false;
-	}
-	/* save the BSS info */
-	pBSSList->bActive = true;
-	memcpy(pBSSList->abyBSSID, abyBSSIDAddr, WLAN_BSSID_LEN);
-	pBSSList->qwBSSTimestamp = le64_to_cpu(qwTimestamp);
-	pBSSList->wBeaconInterval = cpu_to_le16(wBeaconInterval);
-	pBSSList->wCapInfo = cpu_to_le16(wCapInfo);
-	pBSSList->uClearCount = 0;
-
-	if (pSSID->len > WLAN_SSID_MAXLEN)
-		pSSID->len = WLAN_SSID_MAXLEN;
-	memcpy(pBSSList->abySSID, pSSID, pSSID->len + WLAN_IEHDR_LEN);
-
-	pBSSList->uChannel = byCurrChannel;
-
-	if (pSuppRates->len > WLAN_RATES_MAXLEN)
-		pSuppRates->len = WLAN_RATES_MAXLEN;
-	memcpy(pBSSList->abySuppRates, pSuppRates, pSuppRates->len + WLAN_IEHDR_LEN);
-
-	if (pExtSuppRates != NULL) {
-		if (pExtSuppRates->len > WLAN_RATES_MAXLEN)
-			pExtSuppRates->len = WLAN_RATES_MAXLEN;
-		memcpy(pBSSList->abyExtSuppRates, pExtSuppRates, pExtSuppRates->len + WLAN_IEHDR_LEN);
-		pr_debug("BSSbInsertToBSSList: pExtSuppRates->len = %d\n",
-			 pExtSuppRates->len);
-
-	} else {
-		memset(pBSSList->abyExtSuppRates, 0, WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
-	}
-	pBSSList->sERP.byERP = psERP->byERP;
-	pBSSList->sERP.bERPExist = psERP->bERPExist;
-
-	/* check if BSS is 802.11a/b/g */
-	if (pBSSList->uChannel > CB_MAX_CHANNEL_24G) {
-		pBSSList->eNetworkTypeInUse = PHY_TYPE_11A;
-	} else {
-		if (pBSSList->sERP.bERPExist)
-			pBSSList->eNetworkTypeInUse = PHY_TYPE_11G;
-		else
-			pBSSList->eNetworkTypeInUse = PHY_TYPE_11B;
-	}
-
-	pBSSList->byRxRate = pRxPacket->byRxRate;
-	pBSSList->qwLocalTSF = pRxPacket->qwLocalTSF;
-	pBSSList->uRSSI = pRxPacket->uRSSI;
-	pBSSList->bySQ = pRxPacket->bySQ;
-
-	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
-	    (pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
-		/* assoc with BSS */
-		if (pBSSList == pMgmt->pCurrBSS)
-			bParsingQuiet = true;
-	}
-
-	WPA_ClearRSN(pBSSList);
-
-	if (pRSNWPA != NULL) {
-		unsigned int uLen = pRSNWPA->len + 2;
-
-		if (uLen <= (uIELength - (unsigned int)((unsigned char *)pRSNWPA - pbyIEs))) {
-			pBSSList->wWPALen = uLen;
-			memcpy(pBSSList->byWPAIE, pRSNWPA, uLen);
-			WPA_ParseRSN(pBSSList, pRSNWPA);
-		}
-	}
-
-	WPA2_ClearRSN(pBSSList);
-
-	if (pRSN != NULL) {
-		unsigned int uLen = pRSN->len + 2;
-
-		if (uLen <= (uIELength - (unsigned int)((unsigned char *)pRSN - pbyIEs))) {
-			pBSSList->wRSNLen = uLen;
-			memcpy(pBSSList->byRSNIE, pRSN, uLen);
-			WPA2vParseRSN(pBSSList, pRSN);
-		}
-	}
-
-	if ((pMgmt->eAuthenMode == WMAC_AUTH_WPA2) || pBSSList->bWPA2Valid) {
-		PSKeyItem  pTransmitKey = NULL;
-		bool bIs802_1x = false;
-
-		for (ii = 0; ii < pBSSList->wAKMSSAuthCount; ii++) {
-			if (pBSSList->abyAKMSSAuthType[ii] == WLAN_11i_AKMSS_802_1X) {
-				bIs802_1x = true;
-				break;
-			}
-		}
-		if (bIs802_1x && (pSSID->len == ((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->len) &&
-		    (!memcmp(pSSID->abySSID, ((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->abySSID, pSSID->len))) {
-			bAdd_PMKID_Candidate((void *)pDevice, pBSSList->abyBSSID, &pBSSList->sRSNCapObj);
-
-			if (pDevice->bLinkPass && (pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
-				if (KeybGetTransmitKey(&(pDevice->sKey), pDevice->abyBSSID, PAIRWISE_KEY, &pTransmitKey) ||
-				    KeybGetTransmitKey(&(pDevice->sKey), pDevice->abyBSSID, GROUP_KEY, &pTransmitKey)) {
-					pDevice->gsPMKIDCandidate.StatusType = Ndis802_11StatusType_PMKID_CandidateList;
-					pDevice->gsPMKIDCandidate.Version = 1;
-
-				}
-
-			}
-		}
-	}
-
-	if (pDevice->bUpdateBBVGA) {
-		/* monitor if RSSI is too strong */
-		pBSSList->byRSSIStatCnt = 0;
-		RFvRSSITodBm(pDevice, (unsigned char)(pRxPacket->uRSSI), &pBSSList->ldBmMAX);
-		pBSSList->ldBmAverage[0] = pBSSList->ldBmMAX;
-		for (ii = 1; ii < RSSI_STAT_COUNT; ii++)
-			pBSSList->ldBmAverage[ii] = 0;
-	}
-
-	if ((pIE_Country != NULL) && pMgmt->b11hEnable) {
-		set_country_info(pMgmt->pAdapter, pBSSList->eNetworkTypeInUse,
-				 pIE_Country);
-	}
-
-	if (bParsingQuiet && (pIE_Quiet != NULL)) {
-		if ((((PWLAN_IE_QUIET)pIE_Quiet)->len == 8) &&
-		    (((PWLAN_IE_QUIET)pIE_Quiet)->byQuietCount != 0)) {
-			/* valid EID */
-			if (pQuiet == NULL) {
-				pQuiet = (PWLAN_IE_QUIET)pIE_Quiet;
-				CARDbSetQuiet(pMgmt->pAdapter,
-					      true,
-					      pQuiet->byQuietCount,
-					      pQuiet->byQuietPeriod,
-					      *((unsigned short *)pQuiet->abyQuietDuration),
-					      *((unsigned short *)pQuiet->abyQuietOffset)
-);
-			} else {
-				pQuiet = (PWLAN_IE_QUIET)pIE_Quiet;
-				CARDbSetQuiet(pMgmt->pAdapter,
-					      false,
-					      pQuiet->byQuietCount,
-					      pQuiet->byQuietPeriod,
-					      *((unsigned short *)pQuiet->abyQuietDuration),
-					      *((unsigned short *)pQuiet->abyQuietOffset)
-					);
-			}
-		}
-	}
-
-	if (bParsingQuiet && (pQuiet != NULL))
-		CARDbStartQuiet(pMgmt->pAdapter);
-
-	pBSSList->uIELength = uIELength;
-	if (pBSSList->uIELength > WLAN_BEACON_FR_MAXLEN)
-		pBSSList->uIELength = WLAN_BEACON_FR_MAXLEN;
-	memcpy(pBSSList->abyIEs, pbyIEs, pBSSList->uIELength);
-
-	return true;
-}
-
-/*+
- *
- * Routine Description:
- *    Update BSS set in known BSS list
- *
- * Return Value:
- *    true if success.
- *
- -*/
-/* TODO: input structure modify */
-
-bool
-BSSbUpdateToBSSList(
-	void *hDeviceContext,
-	__le64 qwTimestamp,
-	unsigned short wBeaconInterval,
-	unsigned short wCapInfo,
-	unsigned char byCurrChannel,
-	bool bChannelHit,
-	PWLAN_IE_SSID pSSID,
-	PWLAN_IE_SUPP_RATES pSuppRates,
-	PWLAN_IE_SUPP_RATES pExtSuppRates,
-	PERPObject psERP,
-	PWLAN_IE_RSN pRSN,
-	PWLAN_IE_RSN_EXT pRSNWPA,
-	PWLAN_IE_COUNTRY pIE_Country,
-	PWLAN_IE_QUIET pIE_Quiet,
-	PKnownBSS pBSSList,
-	unsigned int uIELength,
-	unsigned char *pbyIEs,
-	void *pRxPacketContext
-)
-{
-	int             ii;
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	PSRxMgmtPacket  pRxPacket = (PSRxMgmtPacket)pRxPacketContext;
-	long            ldBm;
-	bool bParsingQuiet = false;
-	PWLAN_IE_QUIET  pQuiet = NULL;
-
-	if (pBSSList == NULL)
-		return false;
-
-	pBSSList->qwBSSTimestamp = le64_to_cpu(qwTimestamp);
-	pBSSList->wBeaconInterval = cpu_to_le16(wBeaconInterval);
-	pBSSList->wCapInfo = cpu_to_le16(wCapInfo);
-	pBSSList->uClearCount = 0;
-	pBSSList->uChannel = byCurrChannel;
-
-	if (pSSID->len > WLAN_SSID_MAXLEN)
-		pSSID->len = WLAN_SSID_MAXLEN;
-
-	if ((pSSID->len != 0) && (pSSID->abySSID[0] != 0))
-		memcpy(pBSSList->abySSID, pSSID, pSSID->len + WLAN_IEHDR_LEN);
-	memcpy(pBSSList->abySuppRates, pSuppRates, pSuppRates->len + WLAN_IEHDR_LEN);
-
-	if (pExtSuppRates != NULL)
-		memcpy(pBSSList->abyExtSuppRates, pExtSuppRates, pExtSuppRates->len + WLAN_IEHDR_LEN);
-	else
-		memset(pBSSList->abyExtSuppRates, 0, WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
-	pBSSList->sERP.byERP = psERP->byERP;
-	pBSSList->sERP.bERPExist = psERP->bERPExist;
-
-	/* check if BSS is 802.11a/b/g */
-	if (pBSSList->uChannel > CB_MAX_CHANNEL_24G) {
-		pBSSList->eNetworkTypeInUse = PHY_TYPE_11A;
-	} else {
-		if (pBSSList->sERP.bERPExist)
-			pBSSList->eNetworkTypeInUse = PHY_TYPE_11G;
-		else
-			pBSSList->eNetworkTypeInUse = PHY_TYPE_11B;
-	}
-
-	pBSSList->byRxRate = pRxPacket->byRxRate;
-	pBSSList->qwLocalTSF = pRxPacket->qwLocalTSF;
-	if (bChannelHit)
-		pBSSList->uRSSI = pRxPacket->uRSSI;
-	pBSSList->bySQ = pRxPacket->bySQ;
-
-	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
-	    (pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
-		/* assoc with BSS */
-		if (pBSSList == pMgmt->pCurrBSS)
-			bParsingQuiet = true;
-	}
-
-	WPA_ClearRSN(pBSSList);         /* mike update */
-
-	if (pRSNWPA != NULL) {
-		unsigned int uLen = pRSNWPA->len + 2;
-
-		if (uLen <= (uIELength - (unsigned int)((unsigned char *)pRSNWPA - pbyIEs))) {
-			pBSSList->wWPALen = uLen;
-			memcpy(pBSSList->byWPAIE, pRSNWPA, uLen);
-			WPA_ParseRSN(pBSSList, pRSNWPA);
-		}
-	}
-
-	WPA2_ClearRSN(pBSSList);  /* mike update */
-
-	if (pRSN != NULL) {
-		unsigned int uLen = pRSN->len + 2;
-
-		if (uLen <= (uIELength - (unsigned int)((unsigned char *)pRSN - pbyIEs))) {
-			pBSSList->wRSNLen = uLen;
-			memcpy(pBSSList->byRSNIE, pRSN, uLen);
-			WPA2vParseRSN(pBSSList, pRSN);
-		}
-	}
-
-	if (pRxPacket->uRSSI != 0) {
-		RFvRSSITodBm(pDevice, (unsigned char)(pRxPacket->uRSSI), &ldBm);
-		/* monitor if RSSI is too strong */
-		pBSSList->byRSSIStatCnt++;
-		pBSSList->byRSSIStatCnt %= RSSI_STAT_COUNT;
-		pBSSList->ldBmAverage[pBSSList->byRSSIStatCnt] = ldBm;
-		for (ii = 0; ii < RSSI_STAT_COUNT; ii++) {
-			if (pBSSList->ldBmAverage[ii] != 0)
-				pBSSList->ldBmMAX = max(pBSSList->ldBmAverage[ii], ldBm);
-		}
-	}
-
-	if ((pIE_Country != NULL) && pMgmt->b11hEnable) {
-		set_country_info(pMgmt->pAdapter, pBSSList->eNetworkTypeInUse,
-				 pIE_Country);
-	}
-
-	if (bParsingQuiet && (pIE_Quiet != NULL)) {
-		if ((((PWLAN_IE_QUIET)pIE_Quiet)->len == 8) &&
-		    (((PWLAN_IE_QUIET)pIE_Quiet)->byQuietCount != 0)) {
-			/* valid EID */
-			if (pQuiet == NULL) {
-				pQuiet = (PWLAN_IE_QUIET)pIE_Quiet;
-				CARDbSetQuiet(pMgmt->pAdapter,
-					      true,
-					      pQuiet->byQuietCount,
-					      pQuiet->byQuietPeriod,
-					      *((unsigned short *)pQuiet->abyQuietDuration),
-					      *((unsigned short *)pQuiet->abyQuietOffset)
-);
-			} else {
-				pQuiet = (PWLAN_IE_QUIET)pIE_Quiet;
-				CARDbSetQuiet(pMgmt->pAdapter,
-					      false,
-					      pQuiet->byQuietCount,
-					      pQuiet->byQuietPeriod,
-					      *((unsigned short *)pQuiet->abyQuietDuration),
-					      *((unsigned short *)pQuiet->abyQuietOffset)
-					);
-			}
-		}
-	}
-
-	if (bParsingQuiet && (pQuiet != NULL))
-		CARDbStartQuiet(pMgmt->pAdapter);
-
-	pBSSList->uIELength = uIELength;
-	if (pBSSList->uIELength > WLAN_BEACON_FR_MAXLEN)
-		pBSSList->uIELength = WLAN_BEACON_FR_MAXLEN;
-	memcpy(pBSSList->abyIEs, pbyIEs, pBSSList->uIELength);
-
-	return true;
-}
-
-/*+
- *
- * Routine Description:
- *    Search Node DB table to find the index of matched DstAddr
- *
- * Return Value:
- *    None
- *
- -*/
-
-bool
-BSSDBbIsSTAInNodeDB(void *pMgmtObject, unsigned char *abyDstAddr,
-		    unsigned int *puNodeIndex)
-{
-	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
-	unsigned int ii;
-
-	/* Index = 0 reserved for AP Node */
-	for (ii = 1; ii < (MAX_NODE_NUM + 1); ii++) {
-		if (pMgmt->sNodeDBTable[ii].bActive) {
-			if (ether_addr_equal(abyDstAddr,
-					     pMgmt->sNodeDBTable[ii].abyMACAddr)) {
-				*puNodeIndex = ii;
-				return true;
-			}
-		}
-	}
-
-	return false;
-};
-
-/*+
- *
- * Routine Description:
- *    Find an empty node and allocat it; if there is no empty node,
- *    then use the most inactive one.
- *
- * Return Value:
- *    None
- *
- -*/
-void
-BSSvCreateOneNode(void *hDeviceContext, unsigned int *puNodeIndex)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	unsigned int ii;
-	unsigned int BigestCount = 0;
-	unsigned int SelectIndex;
-	struct sk_buff  *skb;
-	/*
-	 * Index = 0 reserved for AP Node (In STA mode)
-	 * Index = 0 reserved for Broadcast/MultiCast (In AP mode)
-	 */
-	SelectIndex = 1;
-	for (ii = 1; ii < (MAX_NODE_NUM + 1); ii++) {
-		if (pMgmt->sNodeDBTable[ii].bActive) {
-			if (pMgmt->sNodeDBTable[ii].uInActiveCount > BigestCount) {
-				BigestCount = pMgmt->sNodeDBTable[ii].uInActiveCount;
-				SelectIndex = ii;
-			}
-		} else {
-			break;
-		}
-	}
-
-	/* if not found replace uInActiveCount is largest one */
-	if (ii == (MAX_NODE_NUM + 1)) {
-		*puNodeIndex = SelectIndex;
-		pr_info("Replace inactive node = %d\n", SelectIndex);
-		/* clear ps buffer */
-		if (pMgmt->sNodeDBTable[*puNodeIndex].sTxPSQueue.next != NULL) {
-			while ((skb = skb_dequeue(&pMgmt->sNodeDBTable[*puNodeIndex].sTxPSQueue)) != NULL)
-				dev_kfree_skb(skb);
-		}
-	} else {
-		*puNodeIndex = ii;
-	}
-
-	memset(&pMgmt->sNodeDBTable[*puNodeIndex], 0, sizeof(KnownNodeDB));
-	pMgmt->sNodeDBTable[*puNodeIndex].bActive = true;
-	pMgmt->sNodeDBTable[*puNodeIndex].uRatePollTimeout = FALLBACK_POLL_SECOND;
-	/* for AP mode PS queue */
-	skb_queue_head_init(&pMgmt->sNodeDBTable[*puNodeIndex].sTxPSQueue);
-	pMgmt->sNodeDBTable[*puNodeIndex].byAuthSequence = 0;
-	pMgmt->sNodeDBTable[*puNodeIndex].wEnQueueCnt = 0;
-	pr_debug("Create node index = %d\n", ii);
-	return;
-};
-
-/*+
- *
- * Routine Description:
- *    Remove Node by NodeIndex
- *
- *
- * Return Value:
- *    None
- *
- -*/
-void
-BSSvRemoveOneNode(
-	void *hDeviceContext,
-	unsigned int uNodeIndex
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	unsigned char byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
-	struct sk_buff  *skb;
-
-	while ((skb = skb_dequeue(&pMgmt->sNodeDBTable[uNodeIndex].sTxPSQueue)) != NULL)
-		dev_kfree_skb(skb);
-	/* clear context */
-	memset(&pMgmt->sNodeDBTable[uNodeIndex], 0, sizeof(KnownNodeDB));
-	/* clear tx bit map */
-	pMgmt->abyPSTxMap[pMgmt->sNodeDBTable[uNodeIndex].wAID >> 3] &=  ~byMask[pMgmt->sNodeDBTable[uNodeIndex].wAID & 7];
-
-	return;
-};
-/*+
- *
- * Routine Description:
- *    Update AP Node content in Index 0 of KnownNodeDB
- *
- *
- * Return Value:
- *    None
- *
- -*/
-
-void
-BSSvUpdateAPNode(
-	void *hDeviceContext,
-	unsigned short *pwCapInfo,
-	PWLAN_IE_SUPP_RATES pSuppRates,
-	PWLAN_IE_SUPP_RATES pExtSuppRates
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	unsigned int uRateLen = WLAN_RATES_MAXLEN;
-
-	memset(&pMgmt->sNodeDBTable[0], 0, sizeof(KnownNodeDB));
-
-	pMgmt->sNodeDBTable[0].bActive = true;
-	if (pDevice->eCurrentPHYType == PHY_TYPE_11B)
-		uRateLen = WLAN_RATES_MAXLEN_11B;
-	pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)pSuppRates,
-						(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-						uRateLen);
-	pMgmt->abyCurrExtSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)pExtSuppRates,
-						   (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates,
-						   uRateLen);
-	RATEvParseMaxRate((void *)pDevice,
-			  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-			  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates,
-			  true,
-			  &(pMgmt->sNodeDBTable[0].wMaxBasicRate),
-			  &(pMgmt->sNodeDBTable[0].wMaxSuppRate),
-			  &(pMgmt->sNodeDBTable[0].wSuppRate),
-			  &(pMgmt->sNodeDBTable[0].byTopCCKBasicRate),
-			  &(pMgmt->sNodeDBTable[0].byTopOFDMBasicRate)
-);
-	memcpy(pMgmt->sNodeDBTable[0].abyMACAddr, pMgmt->abyCurrBSSID, WLAN_ADDR_LEN);
-	pMgmt->sNodeDBTable[0].wTxDataRate = pMgmt->sNodeDBTable[0].wMaxSuppRate;
-	pMgmt->sNodeDBTable[0].bShortPreamble = WLAN_GET_CAP_INFO_SHORTPREAMBLE(*pwCapInfo);
-	pMgmt->sNodeDBTable[0].uRatePollTimeout = FALLBACK_POLL_SECOND;
-	netdev_dbg(pDevice->dev, "BSSvUpdateAPNode:MaxSuppRate is %d\n",
-		   pMgmt->sNodeDBTable[0].wMaxSuppRate);
-	/* auto rate fallback function initiation */
-	pr_debug("pMgmt->sNodeDBTable[0].wTxDataRate = %d\n",
-		 pMgmt->sNodeDBTable[0].wTxDataRate);
-};
-
-/*+
- *
- * Routine Description:
- *    Add Multicast Node content in Index 0 of KnownNodeDB
- *
- *
- * Return Value:
- *    None
- *
- -*/
-
-void
-BSSvAddMulticastNode(
-	void *hDeviceContext
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-
-	if (!pDevice->bEnableHostWEP)
-		memset(&pMgmt->sNodeDBTable[0], 0, sizeof(KnownNodeDB));
-	memset(pMgmt->sNodeDBTable[0].abyMACAddr, 0xff, WLAN_ADDR_LEN);
-	pMgmt->sNodeDBTable[0].bActive = true;
-	pMgmt->sNodeDBTable[0].bPSEnable = false;
-	skb_queue_head_init(&pMgmt->sNodeDBTable[0].sTxPSQueue);
-	RATEvParseMaxRate((void *)pDevice,
-			  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-			  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates,
-			  true,
-			  &(pMgmt->sNodeDBTable[0].wMaxBasicRate),
-			  &(pMgmt->sNodeDBTable[0].wMaxSuppRate),
-			  &(pMgmt->sNodeDBTable[0].wSuppRate),
-			  &(pMgmt->sNodeDBTable[0].byTopCCKBasicRate),
-			  &(pMgmt->sNodeDBTable[0].byTopOFDMBasicRate)
-);
-	pMgmt->sNodeDBTable[0].wTxDataRate = pMgmt->sNodeDBTable[0].wMaxBasicRate;
-	netdev_dbg(pDevice->dev,
-		   "BSSvAddMultiCastNode:pMgmt->sNodeDBTable[0].wTxDataRate is %d\n",
-		   pMgmt->sNodeDBTable[0].wTxDataRate);
-	pMgmt->sNodeDBTable[0].uRatePollTimeout = FALLBACK_POLL_SECOND;
-};
-
-/*+
- *
- * Routine Description:
- *
- *
- *  Second call back function to update Node DB info & AP link status
- *
- *
- * Return Value:
- *    none.
- *
- -*/
-void
-BSSvSecondCallBack(
-	void *hDeviceContext
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	unsigned int ii;
-	PWLAN_IE_SSID   pItemSSID, pCurrSSID;
-	unsigned int uSleepySTACnt = 0;
-	unsigned int uNonShortSlotSTACnt = 0;
-	unsigned int uLongPreambleSTACnt = 0;
-	viawget_wpa_header *wpahdr;  /* DavidWang */
-
-	spin_lock_irq(&pDevice->lock);
-
-	pDevice->uAssocCount = 0;
-
-	pDevice->byERPFlag &=
-		~(WLAN_SET_ERP_BARKER_MODE(1) | WLAN_SET_ERP_NONERP_PRESENT(1));
-
-	if (pDevice->wUseProtectCntDown > 0) {
-		pDevice->wUseProtectCntDown--;
-	} else {
-		/* disable protect mode */
-		pDevice->byERPFlag &= ~(WLAN_SET_ERP_USE_PROTECTION(1));
-	}
-
-	if (pDevice->eCommandState == WLAN_ASSOCIATE_WAIT) {
-		pDevice->byReAssocCount++;
-		/* 10 sec timeout */
-		if ((pDevice->byReAssocCount > 10) && (!pDevice->bLinkPass)) {
-			netdev_info(pDevice->dev, "Re-association timeout!!!\n");
-			pDevice->byReAssocCount = 0;
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-			{
-				union iwreq_data  wrqu;
-
-				memset(&wrqu, 0, sizeof(wrqu));
-				wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-				PRINT_K("wireless_send_event--->SIOCGIWAP(disassociated)\n");
-				wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
-			}
-#endif
-		} else if (pDevice->bLinkPass)
-			pDevice->byReAssocCount = 0;
-	}
-
-#ifdef Calcu_LinkQual
-	s_uCalculateLinkQual((void *)pDevice);
-#endif
-
-	for (ii = 0; ii < (MAX_NODE_NUM + 1); ii++) {
-		if (pMgmt->sNodeDBTable[ii].bActive) {
-			/* increase in-activity counter */
-			pMgmt->sNodeDBTable[ii].uInActiveCount++;
-
-			if (ii > 0) {
-				if (pMgmt->sNodeDBTable[ii].uInActiveCount > MAX_INACTIVE_COUNT) {
-					BSSvRemoveOneNode(pDevice, ii);
-					pr_debug("Inactive timeout [%d] sec, STA index = [%d] remove\n",
-						 MAX_INACTIVE_COUNT, ii);
-					continue;
-				}
-
-				if (pMgmt->sNodeDBTable[ii].eNodeState >= NODE_ASSOC) {
-					pDevice->uAssocCount++;
-
-					/* check if Non ERP exist */
-					if (pMgmt->sNodeDBTable[ii].uInActiveCount < ERP_RECOVER_COUNT) {
-						if (!pMgmt->sNodeDBTable[ii].bShortPreamble) {
-							pDevice->byERPFlag |= WLAN_SET_ERP_BARKER_MODE(1);
-							uLongPreambleSTACnt++;
-						}
-						if (!pMgmt->sNodeDBTable[ii].bERPExist) {
-							pDevice->byERPFlag |= WLAN_SET_ERP_NONERP_PRESENT(1);
-							pDevice->byERPFlag |= WLAN_SET_ERP_USE_PROTECTION(1);
-						}
-						if (!pMgmt->sNodeDBTable[ii].bShortSlotTime)
-							uNonShortSlotSTACnt++;
-					}
-				}
-
-				/* check if any STA in PS mode */
-				if (pMgmt->sNodeDBTable[ii].bPSEnable)
-					uSleepySTACnt++;
-
-			}
-
-			/* rate fallback check */
-			if (!pDevice->bFixRate) {
-				if (ii > 0) {
-					/* ii = 0 for multicast node (AP & Adhoc) */
-					RATEvTxRateFallBack((void *)pDevice, &(pMgmt->sNodeDBTable[ii]));
-				} else {
-					/* ii = 0 reserved for unicast AP node (Infra STA) */
-					if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA)
-						netdev_dbg(pDevice->dev,
-							   "SecondCallback:Before:TxDataRate is %d\n",
-							   pMgmt->sNodeDBTable[0].wTxDataRate);
-					RATEvTxRateFallBack((void *)pDevice, &(pMgmt->sNodeDBTable[ii]));
-					netdev_dbg(pDevice->dev,
-						   "SecondCallback:After:TxDataRate is %d\n",
-						   pMgmt->sNodeDBTable[0].wTxDataRate);
-
-				}
-
-			}
-
-			/* check if pending PS queue */
-			if (pMgmt->sNodeDBTable[ii].wEnQueueCnt != 0) {
-				pr_debug("Index= %d, Queue = %d pending\n",
-					 ii,
-					 pMgmt->sNodeDBTable[ii].wEnQueueCnt);
-				if ((ii > 0) && (pMgmt->sNodeDBTable[ii].wEnQueueCnt > 15)) {
-					BSSvRemoveOneNode(pDevice, ii);
-					pr_info("Pending many queues PS STA Index = %d remove\n",
-						ii);
-					continue;
-				}
-			}
-		}
-
-	}
-
-	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) && (pDevice->eCurrentPHYType == PHY_TYPE_11G)) {
-		/* on/off protect mode */
-		if (WLAN_GET_ERP_USE_PROTECTION(pDevice->byERPFlag)) {
-			if (!pDevice->bProtectMode) {
-				MACvEnableProtectMD(pDevice->PortOffset);
-				pDevice->bProtectMode = true;
-			}
-		} else {
-			if (pDevice->bProtectMode) {
-				MACvDisableProtectMD(pDevice->PortOffset);
-				pDevice->bProtectMode = false;
-			}
-		}
-		/* on/off short slot time */
-
-		if (uNonShortSlotSTACnt > 0) {
-			if (pDevice->bShortSlotTime) {
-				pDevice->bShortSlotTime = false;
-				BBvSetShortSlotTime(pDevice);
-				vUpdateIFS((void *)pDevice);
-			}
-		} else {
-			if (!pDevice->bShortSlotTime) {
-				pDevice->bShortSlotTime = true;
-				BBvSetShortSlotTime(pDevice);
-				vUpdateIFS((void *)pDevice);
-			}
-		}
-
-		/* on/off barker long preamble mode */
-
-		if (uLongPreambleSTACnt > 0) {
-			if (!pDevice->bBarkerPreambleMd) {
-				MACvEnableBarkerPreambleMd(pDevice->PortOffset);
-				pDevice->bBarkerPreambleMd = true;
-			}
-		} else {
-			if (pDevice->bBarkerPreambleMd) {
-				MACvDisableBarkerPreambleMd(pDevice->PortOffset);
-				pDevice->bBarkerPreambleMd = false;
-			}
-		}
-
-	}
-
-	/* check if any STA in PS mode, enable DTIM multicast deliver */
-	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-		if (uSleepySTACnt > 0)
-			pMgmt->sNodeDBTable[0].bPSEnable = true;
-		else
-			pMgmt->sNodeDBTable[0].bPSEnable = false;
-	}
-
-	pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
-	pCurrSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
-
-	if ((pMgmt->eCurrMode == WMAC_MODE_STANDBY) ||
-	    (pMgmt->eCurrMode == WMAC_MODE_ESS_STA)) {
-		/* assoc with BSS */
-		if (pMgmt->sNodeDBTable[0].bActive) {
-			if (pDevice->bUpdateBBVGA)
-				s_vCheckPreEDThreshold((void *)pDevice);
-
-			if ((pMgmt->sNodeDBTable[0].uInActiveCount >= (LOST_BEACON_COUNT/2)) &&
-			    (pDevice->byBBVGACurrent != pDevice->abyBBVGA[0])) {
-				pDevice->byBBVGANew = pDevice->abyBBVGA[0];
-				bScheduleCommand((void *)pDevice, WLAN_CMD_CHANGE_BBSENSITIVITY, NULL);
-			}
-
-			if (pMgmt->sNodeDBTable[0].uInActiveCount >= LOST_BEACON_COUNT) {
-				pMgmt->sNodeDBTable[0].bActive = false;
-				pMgmt->eCurrMode = WMAC_MODE_STANDBY;
-				pMgmt->eCurrState = WMAC_STATE_IDLE;
-				netif_stop_queue(pDevice->dev);
-				pDevice->bLinkPass = false;
-				pDevice->bRoaming = true;
-				pr_info("Lost AP beacon [%d] sec, disconnected !\n",
-					pMgmt->sNodeDBTable[0].uInActiveCount);
-				if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
-					wpahdr = (viawget_wpa_header *)pDevice->skb->data;
-					wpahdr->type = VIAWGET_DISASSOC_MSG;
-					wpahdr->resp_ie_len = 0;
-					wpahdr->req_ie_len = 0;
-					skb_put(pDevice->skb, sizeof(viawget_wpa_header));
-					pDevice->skb->dev = pDevice->wpadev;
-					skb_reset_mac_header(pDevice->skb);
-					pDevice->skb->pkt_type = PACKET_HOST;
-					pDevice->skb->protocol = htons(ETH_P_802_2);
-					memset(pDevice->skb->cb, 0, sizeof(pDevice->skb->cb));
-					netif_rx(pDevice->skb);
-					pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
-				}
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-				{
-					union iwreq_data  wrqu;
-
-					memset(&wrqu, 0, sizeof(wrqu));
-					wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-					PRINT_K("wireless_send_event--->SIOCGIWAP(disassociated)\n");
-					wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
-				}
-#endif
-			}
-		} else if (pItemSSID->len != 0) {
-			if (pDevice->uAutoReConnectTime < 10) {
-				pDevice->uAutoReConnectTime++;
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-				/*
-				 * network manager support need not do
-				 * Roaming scan???
-				 */
-				if (pDevice->bWPASuppWextEnabled)
-					pDevice->uAutoReConnectTime = 0;
-#endif
-			} else {
-				/*
-				 * mike use old encryption status
-				 * for wpa reauthentication
-				 */
-				if (pDevice->bWPADEVUp)
-					pDevice->eEncryptionStatus = pDevice->eOldEncryptionStatus;
-
-				pr_debug("Roaming ...\n");
-				BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
-				pMgmt->eScanType = WMAC_SCAN_ACTIVE;
-				bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, pMgmt->abyDesireSSID);
-				bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, pMgmt->abyDesireSSID);
-				pDevice->uAutoReConnectTime = 0;
-			}
-		}
-	}
-
-	if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-		/* if adhoc started which essid is NULL string, rescanning */
-		if ((pMgmt->eCurrState == WMAC_STATE_STARTED) && (pCurrSSID->len == 0)) {
-			if (pDevice->uAutoReConnectTime < 10) {
-				pDevice->uAutoReConnectTime++;
-			} else {
-				pr_info("Adhoc re-scanning ...\n");
-				pMgmt->eScanType = WMAC_SCAN_ACTIVE;
-				bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, NULL);
-				bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, NULL);
-				pDevice->uAutoReConnectTime = 0;
-			}
-		}
-		if (pMgmt->eCurrState == WMAC_STATE_JOINTED) {
-			if (pDevice->bUpdateBBVGA)
-				s_vCheckPreEDThreshold((void *)pDevice);
-			if (pMgmt->sNodeDBTable[0].uInActiveCount >= ADHOC_LOST_BEACON_COUNT) {
-				pr_info("Lost other STA beacon [%d] sec, started !\n",
-					pMgmt->sNodeDBTable[0].uInActiveCount);
-				pMgmt->sNodeDBTable[0].uInActiveCount = 0;
-				pMgmt->eCurrState = WMAC_STATE_STARTED;
-				netif_stop_queue(pDevice->dev);
-				pDevice->bLinkPass = false;
-			}
-		}
-	}
-
-	spin_unlock_irq(&pDevice->lock);
-
-	pMgmt->sTimerSecondCallback.expires = RUN_AT(HZ);
-	add_timer(&pMgmt->sTimerSecondCallback);
-}
-
-/*+
- *
- * Routine Description:
- *
- *
- *  Update Tx attemps, Tx failure counter in Node DB
- *
- *
- * Return Value:
- *    none.
- *
- -*/
-
-void
-BSSvUpdateNodeTxCounter(
-	void *hDeviceContext,
-	unsigned char byTsr0,
-	unsigned char byTsr1,
-	unsigned char *pbyBuffer,
-	unsigned int uFIFOHeaderSize
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	unsigned int uNodeIndex = 0;
-	unsigned char byTxRetry = (byTsr0 & TSR0_NCR);
-	PSTxBufHead     pTxBufHead;
-	PS802_11Header  pMACHeader;
-	unsigned short wRate;
-	unsigned short wFallBackRate = RATE_1M;
-	unsigned char byFallBack;
-	unsigned int ii;
-
-	pTxBufHead = (PSTxBufHead) pbyBuffer;
-	if (pTxBufHead->wFIFOCtl & FIFOCTL_AUTO_FB_0)
-		byFallBack = AUTO_FB_0;
-	else if (pTxBufHead->wFIFOCtl & FIFOCTL_AUTO_FB_1)
-		byFallBack = AUTO_FB_1;
-	else
-		byFallBack = AUTO_FB_NONE;
-	wRate = pTxBufHead->wReserved;
-
-	/* Only Unicast using support rates */
-	if (pTxBufHead->wFIFOCtl & FIFOCTL_NEEDACK) {
-		pr_debug("wRate %04X, byTsr0 %02X, byTsr1 %02X\n",
-			 wRate, byTsr0, byTsr1);
-		if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA) {
-			pMgmt->sNodeDBTable[0].uTxAttempts += 1;
-			if ((byTsr1 & TSR1_TERR) == 0) {
-				/* transmit success, TxAttempts at least plus one */
-				pMgmt->sNodeDBTable[0].uTxOk[MAX_RATE]++;
-				if ((byFallBack == AUTO_FB_NONE) ||
-				    (wRate < RATE_18M)) {
-					wFallBackRate = wRate;
-				} else if (byFallBack == AUTO_FB_0) {
-					if (byTxRetry < 5)
-						wFallBackRate = awHWRetry0[wRate-RATE_18M][byTxRetry];
-					else
-						wFallBackRate = awHWRetry0[wRate-RATE_18M][4];
-				} else if (byFallBack == AUTO_FB_1) {
-					if (byTxRetry < 5)
-						wFallBackRate = awHWRetry1[wRate-RATE_18M][byTxRetry];
-					else
-						wFallBackRate = awHWRetry1[wRate-RATE_18M][4];
-				}
-				pMgmt->sNodeDBTable[0].uTxOk[wFallBackRate]++;
-			} else {
-				pMgmt->sNodeDBTable[0].uTxFailures++;
-			}
-			pMgmt->sNodeDBTable[0].uTxRetry += byTxRetry;
-			if (byTxRetry != 0) {
-				pMgmt->sNodeDBTable[0].uTxFail[MAX_RATE] += byTxRetry;
-				if ((byFallBack == AUTO_FB_NONE) ||
-				    (wRate < RATE_18M)) {
-					pMgmt->sNodeDBTable[0].uTxFail[wRate] += byTxRetry;
-				} else if (byFallBack == AUTO_FB_0) {
-					for (ii = 0; ii < byTxRetry; ii++) {
-						if (ii < 5)
-							wFallBackRate = awHWRetry0[wRate-RATE_18M][ii];
-						else
-							wFallBackRate = awHWRetry0[wRate-RATE_18M][4];
-						pMgmt->sNodeDBTable[0].uTxFail[wFallBackRate]++;
-					}
-				} else if (byFallBack == AUTO_FB_1) {
-					for (ii = 0; ii < byTxRetry; ii++) {
-						if (ii < 5)
-							wFallBackRate = awHWRetry1[wRate-RATE_18M][ii];
-						else
-							wFallBackRate = awHWRetry1[wRate-RATE_18M][4];
-						pMgmt->sNodeDBTable[0].uTxFail[wFallBackRate]++;
-					}
-				}
-			}
-		}
-
-		if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) ||
-		    (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)) {
-			pMACHeader = (PS802_11Header)(pbyBuffer + uFIFOHeaderSize);
-
-			if (BSSDBbIsSTAInNodeDB((void *)pMgmt,  &(pMACHeader->abyAddr1[0]), &uNodeIndex)) {
-				pMgmt->sNodeDBTable[uNodeIndex].uTxAttempts += 1;
-				if ((byTsr1 & TSR1_TERR) == 0) {
-					/* transmit success, TxAttempts at least plus one */
-					pMgmt->sNodeDBTable[uNodeIndex].uTxOk[MAX_RATE]++;
-					if ((byFallBack == AUTO_FB_NONE) ||
-					    (wRate < RATE_18M)) {
-						wFallBackRate = wRate;
-					} else if (byFallBack == AUTO_FB_0) {
-						if (byTxRetry < 5)
-							wFallBackRate = awHWRetry0[wRate-RATE_18M][byTxRetry];
-						else
-							wFallBackRate = awHWRetry0[wRate-RATE_18M][4];
-					} else if (byFallBack == AUTO_FB_1) {
-						if (byTxRetry < 5)
-							wFallBackRate = awHWRetry1[wRate-RATE_18M][byTxRetry];
-						else
-							wFallBackRate = awHWRetry1[wRate-RATE_18M][4];
-					}
-					pMgmt->sNodeDBTable[uNodeIndex].uTxOk[wFallBackRate]++;
-				} else {
-					pMgmt->sNodeDBTable[uNodeIndex].uTxFailures++;
-				}
-				pMgmt->sNodeDBTable[uNodeIndex].uTxRetry += byTxRetry;
-				if (byTxRetry != 0) {
-					pMgmt->sNodeDBTable[uNodeIndex].uTxFail[MAX_RATE] += byTxRetry;
-					if ((byFallBack == AUTO_FB_NONE) ||
-					    (wRate < RATE_18M)) {
-						pMgmt->sNodeDBTable[uNodeIndex].uTxFail[wRate] += byTxRetry;
-					} else if (byFallBack == AUTO_FB_0) {
-						for (ii = 0; ii < byTxRetry; ii++) {
-							if (ii < 5)
-								wFallBackRate = awHWRetry0[wRate - RATE_18M][ii];
-							else
-								wFallBackRate = awHWRetry0[wRate - RATE_18M][4];
-							pMgmt->sNodeDBTable[uNodeIndex].uTxFail[wFallBackRate]++;
-						}
-					} else if (byFallBack == AUTO_FB_1) {
-						for (ii = 0; ii < byTxRetry; ii++) {
-							if (ii < 5)
-								wFallBackRate = awHWRetry1[wRate-RATE_18M][ii];
-							else
-								wFallBackRate = awHWRetry1[wRate-RATE_18M][4];
-							pMgmt->sNodeDBTable[uNodeIndex].uTxFail[wFallBackRate]++;
-						}
-					}
-				}
-			}
-		}
-	}
-}
-
-/*+
- *
- * Routine Description:
- *    Clear Nodes & skb in DB Table
- *
- *
- * Parameters:
- *  In:
- *      hDeviceContext        - The adapter context.
- *      uStartIndex           - starting index
- *  Out:
- *      none
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-BSSvClearNodeDBTable(
-	void *hDeviceContext,
-	unsigned int uStartIndex
-)
-
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	struct sk_buff  *skb;
-	unsigned int ii;
-
-	for (ii = uStartIndex; ii < (MAX_NODE_NUM + 1); ii++) {
-		if (pMgmt->sNodeDBTable[ii].bActive) {
-			/* check if sTxPSQueue has been initial */
-			if (pMgmt->sNodeDBTable[ii].sTxPSQueue.next != NULL) {
-				while ((skb = skb_dequeue(&pMgmt->sNodeDBTable[ii].sTxPSQueue)) != NULL) {
-					pr_debug("PS skb != NULL %d\n", ii);
-					dev_kfree_skb(skb);
-				}
-			}
-			memset(&pMgmt->sNodeDBTable[ii], 0, sizeof(KnownNodeDB));
-		}
-	}
-
-	return;
-};
-
-void s_vCheckSensitivity(
-	void *hDeviceContext
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PKnownBSS       pBSSList = NULL;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	int             ii;
-
-	if ((pDevice->byLocalID <= REV_ID_VT3253_A1) && (pDevice->byRFType == RF_RFMD2959) &&
-	    (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)) {
-		return;
-	}
-
-	if ((pMgmt->eCurrState == WMAC_STATE_ASSOC) ||
-	    ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) && (pMgmt->eCurrState == WMAC_STATE_JOINTED))) {
-		pBSSList = BSSpAddrIsInBSSList(pDevice, pMgmt->abyCurrBSSID, (PWLAN_IE_SSID)pMgmt->abyCurrSSID);
-		if (pBSSList != NULL) {
-			/* Update BB Reg if RSSI is too strong */
-			long    LocalldBmAverage = 0;
-			long    uNumofdBm = 0;
-
-			for (ii = 0; ii < RSSI_STAT_COUNT; ii++) {
-				if (pBSSList->ldBmAverage[ii] != 0) {
-					uNumofdBm++;
-					LocalldBmAverage += pBSSList->ldBmAverage[ii];
-				}
-			}
-			if (uNumofdBm > 0) {
-				LocalldBmAverage = LocalldBmAverage/uNumofdBm;
-				for (ii = 0; ii < BB_VGA_LEVEL; ii++) {
-					pr_debug("LocalldBmAverage:%ld, %ld %02x\n",
-						 LocalldBmAverage,
-						 pDevice->ldBmThreshold[ii],
-						 pDevice->abyBBVGA[ii]);
-					if (LocalldBmAverage < pDevice->ldBmThreshold[ii]) {
-						pDevice->byBBVGANew = pDevice->abyBBVGA[ii];
-						break;
-					}
-				}
-				if (pDevice->byBBVGANew != pDevice->byBBVGACurrent) {
-					pDevice->uBBVGADiffCount++;
-					if (pDevice->uBBVGADiffCount >= BB_VGA_CHANGE_THRESHOLD)
-						bScheduleCommand((void *)pDevice, WLAN_CMD_CHANGE_BBSENSITIVITY, NULL);
-				} else {
-					pDevice->uBBVGADiffCount = 0;
-				}
-			}
-		}
-	}
-}
-
-void
-BSSvClearAnyBSSJoinRecord(
-	void *hDeviceContext
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	unsigned int ii;
-
-	for (ii = 0; ii < MAX_BSS_NUM; ii++)
-		pMgmt->sBSSList[ii].bSelected = false;
-}
-
-#ifdef Calcu_LinkQual
-void s_uCalculateLinkQual(
-	void *hDeviceContext
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	unsigned long TxOkRatio, TxCnt;
-	unsigned long RxOkRatio, RxCnt;
-	unsigned long RssiRatio;
-	long ldBm;
-
-	TxCnt = pDevice->scStatistic.TxNoRetryOkCount +
-		pDevice->scStatistic.TxRetryOkCount +
-		pDevice->scStatistic.TxFailCount;
-	RxCnt = pDevice->scStatistic.RxFcsErrCnt +
-		pDevice->scStatistic.RxOkCnt;
-	TxOkRatio = (TxCnt < 6) ? 4000 : ((pDevice->scStatistic.TxNoRetryOkCount * 4000) / TxCnt);
-	RxOkRatio = (RxCnt < 6) ? 2000 : ((pDevice->scStatistic.RxOkCnt * 2000) / RxCnt);
-	/* decide link quality */
-	if (!pDevice->bLinkPass) {
-		pDevice->scStatistic.LinkQuality = 0;
-		pDevice->scStatistic.SignalStren = 0;
-	} else {
-		RFvRSSITodBm(pDevice, (unsigned char)(pDevice->uCurrRSSI), &ldBm);
-		if (-ldBm < 50)
-			RssiRatio = 4000;
-		else if (-ldBm > 90)
-			RssiRatio = 0;
-		else
-			RssiRatio = (40-(-ldBm-50))*4000/40;
-		pDevice->scStatistic.SignalStren = RssiRatio/40;
-		pDevice->scStatistic.LinkQuality = (RssiRatio+TxOkRatio+RxOkRatio)/100;
-	}
-	pDevice->scStatistic.RxFcsErrCnt = 0;
-	pDevice->scStatistic.RxOkCnt = 0;
-	pDevice->scStatistic.TxFailCount = 0;
-	pDevice->scStatistic.TxNoRetryOkCount = 0;
-	pDevice->scStatistic.TxRetryOkCount = 0;
-}
-#endif
-
-void s_vCheckPreEDThreshold(
-	void *hDeviceContext
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PKnownBSS       pBSSList = NULL;
-	PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
-
-	if ((pMgmt->eCurrState == WMAC_STATE_ASSOC) ||
-	    ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) && (pMgmt->eCurrState == WMAC_STATE_JOINTED))) {
-		pBSSList = BSSpAddrIsInBSSList(pDevice, pMgmt->abyCurrBSSID, (PWLAN_IE_SSID)pMgmt->abyCurrSSID);
-		if (pBSSList != NULL)
-			pDevice->byBBPreEDRSSI = (unsigned char) (~(pBSSList->ldBmAverRange) + 1);
-	}
-}
diff --git a/drivers/staging/vt6655/bssdb.h b/drivers/staging/vt6655/bssdb.h
deleted file mode 100644
index 5d4dd28b6223..000000000000
--- a/drivers/staging/vt6655/bssdb.h
+++ /dev/null
@@ -1,326 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: bssdb.h
- *
- * Purpose: Handles the Basic Service Set & Node Database functions
- *
- * Author: Lyndon Chen
- *
- * Date: July 16, 2002
- *
- */
-
-#ifndef __BSSDB_H__
-#define __BSSDB_H__
-
-#include <linux/skbuff.h>
-#include "80211hdr.h"
-#include "80211mgr.h"
-#include "card.h"
-
-#define MAX_NODE_NUM             64
-#define MAX_BSS_NUM              42
-#define LOST_BEACON_COUNT        10   // 10 sec, XP defined
-#define MAX_PS_TX_BUF            32   // sta max power saving tx buf
-#define ADHOC_LOST_BEACON_COUNT  30   // 30 sec, beacon lost for adhoc only
-#define MAX_INACTIVE_COUNT       300  // 300 sec, inactive STA node refresh
-
-#define USE_PROTECT_PERIOD       10   // 10 sec, Use protect mode check period
-#define ERP_RECOVER_COUNT        30   // 30 sec, ERP support callback check
-#define BSS_CLEAR_COUNT           1
-
-#define RSSI_STAT_COUNT          10
-#define MAX_CHECK_RSSI_COUNT     8
-
-// STA dwflags
-#define WLAN_STA_AUTH            BIT0
-#define WLAN_STA_ASSOC           BIT1
-#define WLAN_STA_PS              BIT2
-#define WLAN_STA_TIM             BIT3
-// permanent; do not remove entry on expiration
-#define WLAN_STA_PERM            BIT4
-// If 802.1X is used, this flag is
-// controlling whether STA is authorized to
-// send and receive non-IEEE 802.1X frames
-#define WLAN_STA_AUTHORIZED      BIT5
-
-#define MAX_RATE            12
-
-#define MAX_WPA_IE_LEN      64
-
-//
-// IEEE 802.11 Structures and definitions
-//
-
-typedef enum _NDIS_802_11_NETWORK_TYPE {
-	Ndis802_11FH,
-	Ndis802_11DS,
-	Ndis802_11OFDM5,
-	Ndis802_11OFDM24,
-	Ndis802_11NetworkTypeMax    // not a real type, defined as an upper bound
-} NDIS_802_11_NETWORK_TYPE, *PNDIS_802_11_NETWORK_TYPE;
-
-typedef struct tagSERPObject {
-	bool bERPExist;
-	unsigned char byERP;
-} ERPObject, *PERPObject;
-
-typedef struct tagSRSNCapObject {
-	bool bRSNCapExist;
-	unsigned short wRSNCap;
-} SRSNCapObject, *PSRSNCapObject;
-
-// BSS info(AP)
-#pragma pack(1)
-typedef struct tagKnownBSS {
-	bool bActive;
-	unsigned char abyBSSID[WLAN_BSSID_LEN];
-	unsigned int	uChannel;
-	unsigned char abySuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
-	unsigned char abyExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
-	unsigned int	uRSSI;
-	unsigned char bySQ;
-	unsigned short wBeaconInterval;
-	unsigned short wCapInfo;
-	unsigned char abySSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-	unsigned char byRxRate;
-
-	unsigned char byRSSIStatCnt;
-	long            ldBmMAX;
-	long            ldBmAverage[RSSI_STAT_COUNT];
-	long            ldBmAverRange;
-	bool bSelected;
-
-	bool bWPAValid;
-	unsigned char byGKType;
-	unsigned char abyPKType[4];
-	unsigned short wPKCount;
-	unsigned char abyAuthType[4];
-	unsigned short wAuthCount;
-	unsigned char byDefaultK_as_PK;
-	unsigned char byReplayIdx;
-
-	bool bWPA2Valid;
-	unsigned char byCSSGK;
-	unsigned short wCSSPKCount;
-	unsigned char abyCSSPK[4];
-	unsigned short wAKMSSAuthCount;
-	unsigned char abyAKMSSAuthType[4];
-
-	unsigned char byWPAIE[MAX_WPA_IE_LEN];
-	unsigned char byRSNIE[MAX_WPA_IE_LEN];
-	unsigned short wWPALen;
-	unsigned short wRSNLen;
-
-	unsigned int	uClearCount;
-	unsigned int	uIELength;
-	u64 qwBSSTimestamp;
-	u64 qwLocalTSF;
-
-	CARD_PHY_TYPE   eNetworkTypeInUse;
-
-	ERPObject       sERP;
-	SRSNCapObject   sRSNCapObj;
-	unsigned char abyIEs[1024];
-} __attribute__ ((__packed__))
-KnownBSS , *PKnownBSS;
-
-#pragma pack()
-
-typedef enum tagNODE_STATE {
-	NODE_FREE,
-	NODE_AGED,
-	NODE_KNOWN,
-	NODE_AUTH,
-	NODE_ASSOC
-} NODE_STATE, *PNODE_STATE;
-
-// STA node info
-typedef struct tagKnownNodeDB {
-	bool bActive;
-	unsigned char abyMACAddr[WLAN_ADDR_LEN];
-	unsigned char abyCurrSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN];
-	unsigned char abyCurrExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN];
-	unsigned short wTxDataRate;
-	bool bShortPreamble;
-	bool bERPExist;
-	bool bShortSlotTime;
-	unsigned int	uInActiveCount;
-	unsigned short wMaxBasicRate;     //Get from byTopOFDMBasicRate or byTopCCKBasicRate which depends on packetTyp.
-	unsigned short wMaxSuppRate;      //Records the highest supported rate getting from SuppRates IE and ExtSuppRates IE in Beacon.
-	unsigned short wSuppRate;
-	unsigned char byTopOFDMBasicRate;//Records the highest basic rate in OFDM mode
-	unsigned char byTopCCKBasicRate; //Records the highest basic rate in CCK mode
-
-	// For AP mode
-	struct sk_buff_head sTxPSQueue;
-	unsigned short wCapInfo;
-	unsigned short wListenInterval;
-	unsigned short wAID;
-	NODE_STATE      eNodeState;
-	bool bPSEnable;
-	bool bRxPSPoll;
-	unsigned char byAuthSequence;
-	unsigned long ulLastRxJiffer;
-	unsigned char bySuppRate;
-	unsigned long dwFlags;
-	unsigned short wEnQueueCnt;
-
-	bool bOnFly;
-	unsigned long long       KeyRSC;
-	unsigned char byKeyIndex;
-	unsigned long dwKeyIndex;
-	unsigned char byCipherSuite;
-	unsigned long dwTSC47_16;
-	unsigned short wTSC15_0;
-	unsigned int	uWepKeyLength;
-	unsigned char abyWepKey[WLAN_WEPMAX_KEYLEN];
-	// Auto rate fallback vars
-	bool bIsInFallback;
-	unsigned int	uAverageRSSI;
-	unsigned int	uRateRecoveryTimeout;
-	unsigned int	uRatePollTimeout;
-	unsigned int	uTxFailures;
-	unsigned int	uTxAttempts;
-
-	unsigned int	uTxRetry;
-	unsigned int	uFailureRatio;
-	unsigned int	uRetryRatio;
-	unsigned int	uTxOk[MAX_RATE+1];
-	unsigned int	uTxFail[MAX_RATE+1];
-	unsigned int	uTimeCount;
-} KnownNodeDB, *PKnownNodeDB;
-
-PKnownBSS
-BSSpSearchBSSList(
-	void *hDeviceContext,
-	unsigned char *pbyDesireBSSID,
-	unsigned char *pbyDesireSSID,
-	CARD_PHY_TYPE ePhyType
-);
-
-PKnownBSS
-BSSpAddrIsInBSSList(
-	void *hDeviceContext,
-	unsigned char *abyBSSID,
-	PWLAN_IE_SSID pSSID
-);
-
-void
-BSSvClearBSSList(
-	void *hDeviceContext,
-	bool bKeepCurrBSSID
-);
-
-bool
-BSSbInsertToBSSList(
-	void *hDeviceContext,
-	unsigned char *abyBSSIDAddr,
-	__le64 qwTimestamp,
-	unsigned short wBeaconInterval,
-	unsigned short wCapInfo,
-	unsigned char byCurrChannel,
-	PWLAN_IE_SSID pSSID,
-	PWLAN_IE_SUPP_RATES pSuppRates,
-	PWLAN_IE_SUPP_RATES pExtSuppRates,
-	PERPObject psERP,
-	PWLAN_IE_RSN pRSN,
-	PWLAN_IE_RSN_EXT pRSNWPA,
-	PWLAN_IE_COUNTRY pIE_Country,
-	PWLAN_IE_QUIET pIE_Quiet,
-	unsigned int uIELength,
-	unsigned char *pbyIEs,
-	void *pRxPacketContext
-);
-
-bool
-BSSbUpdateToBSSList(
-	void *hDeviceContext,
-	__le64 qwTimestamp,
-	unsigned short wBeaconInterval,
-	unsigned short wCapInfo,
-	unsigned char byCurrChannel,
-	bool bChannelHit,
-	PWLAN_IE_SSID pSSID,
-	PWLAN_IE_SUPP_RATES pSuppRates,
-	PWLAN_IE_SUPP_RATES pExtSuppRates,
-	PERPObject psERP,
-	PWLAN_IE_RSN pRSN,
-	PWLAN_IE_RSN_EXT pRSNWPA,
-	PWLAN_IE_COUNTRY pIE_Country,
-	PWLAN_IE_QUIET pIE_Quiet,
-	PKnownBSS pBSSList,
-	unsigned int uIELength,
-	unsigned char *pbyIEs,
-	void *pRxPacketContext
-);
-
-bool
-BSSDBbIsSTAInNodeDB(void *hDeviceContext, unsigned char *abyDstAddr,
-		    unsigned int *puNodeIndex);
-
-void
-BSSvCreateOneNode(void *hDeviceContext, unsigned int *puNodeIndex);
-
-void
-BSSvUpdateAPNode(
-	void *hDeviceContext,
-	unsigned short *pwCapInfo,
-	PWLAN_IE_SUPP_RATES pItemRates,
-	PWLAN_IE_SUPP_RATES pExtSuppRates
-);
-
-void
-BSSvSecondCallBack(
-	void *hDeviceContext
-);
-
-void
-BSSvUpdateNodeTxCounter(
-	void *hDeviceContext,
-	unsigned char byTsr0,
-	unsigned char byTsr1,
-	unsigned char *pbyBuffer,
-	unsigned int uFIFOHeaderSize
-);
-
-void
-BSSvRemoveOneNode(
-	void *hDeviceContext,
-	unsigned int uNodeIndex
-);
-
-void
-BSSvAddMulticastNode(
-	void *hDeviceContext
-);
-
-void
-BSSvClearNodeDBTable(
-	void *hDeviceContext,
-	unsigned int uStartIndex
-);
-
-void
-BSSvClearAnyBSSJoinRecord(
-	void *hDeviceContext
-);
-
-#endif //__BSSDB_H__
diff --git a/drivers/staging/vt6655/card.c b/drivers/staging/vt6655/card.c
index 5a6950264bdc..a0796405c308 100644
--- a/drivers/staging/vt6655/card.c
+++ b/drivers/staging/vt6655/card.c
@@ -21,7 +21,6 @@
  * Functions:
  *      s_vSafeResetTx - Rest Tx
  *      CARDvSetRSPINF - Set RSPINF
- *      vUpdateIFS - Update slotTime,SIFS,DIFS, and EIFS
  *      CARDvUpdateBasicTopRate - Update BasicTopRate
  *      CARDbAddBasicRate - Add to BasicRateSet
  *      CARDbIsOFDMinBasicRate - Check if any OFDM rate is in BasicRateSet
@@ -34,8 +33,6 @@
  *      CARDvUpdateNextTBTT - Sync. NIC Beacon time
  *      CARDbRadioPowerOff - Turn Off NIC Radio Power
  *      CARDbRadioPowerOn - Turn On NIC Radio Power
- *      CARDbSetWEPMode - Set NIC Wep mode
- *      CARDbSetTxPower - Set NIC tx power
  *
  * Revision History:
  *      06-10-2003 Bryan YC Fan:  Re-write codes to support VT3253 spec.
@@ -50,38 +47,24 @@
 #include "mac.h"
 #include "desc.h"
 #include "rf.h"
-#include "vntwifi.h"
 #include "power.h"
-#include "key.h"
-#include "rc4.h"
-#include "country.h"
-#include "channel.h"
 
 /*---------------------  Static Definitions -------------------------*/
 
-#define C_SIFS_A        16      // micro sec.
+#define C_SIFS_A        16      /* micro sec. */
 #define C_SIFS_BG       10
 
-#define C_EIFS          80      // micro sec.
+#define C_EIFS          80      /* micro sec. */
 
-#define C_SLOT_SHORT    9       // micro sec.
+#define C_SLOT_SHORT    9       /* micro sec. */
 #define C_SLOT_LONG     20
 
-#define C_CWMIN_A       15      // slot time
+#define C_CWMIN_A       15      /* slot time */
 #define C_CWMIN_B       31
 
-#define C_CWMAX         1023    // slot time
+#define C_CWMAX         1023    /* slot time */
 
-#define WAIT_BEACON_TX_DOWN_TMO         3    // Times
-
-//1M,   2M,   5M,  11M,  18M,  24M,  36M,  54M
-static unsigned char abyDefaultSuppRatesG[] = {WLAN_EID_SUPP_RATES, 8, 0x02, 0x04, 0x0B, 0x16, 0x24, 0x30, 0x48, 0x6C};
-//6M,   9M,  12M,  48M
-static unsigned char abyDefaultExtSuppRatesG[] = {WLAN_EID_EXTSUPP_RATES, 4, 0x0C, 0x12, 0x18, 0x60};
-//6M,   9M,  12M,  18M,  24M,  36M,  48M,  54M
-static unsigned char abyDefaultSuppRatesA[] = {WLAN_EID_SUPP_RATES, 8, 0x0C, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C};
-//1M,   2M,   5M,  11M,
-static unsigned char abyDefaultSuppRatesB[] = {WLAN_EID_SUPP_RATES, 4, 0x02, 0x04, 0x0B, 0x16};
+#define WAIT_BEACON_TX_DOWN_TMO         3    /* Times */
 
 /*---------------------  Static Variables  --------------------------*/
 
@@ -94,7 +77,7 @@ static
 void
 s_vCalculateOFDMRParameter(
 	unsigned char byRate,
-	CARD_PHY_TYPE ePHYType,
+	u8 bb_type,
 	unsigned char *pbyTxRate,
 	unsigned char *pbyRsvTime
 );
@@ -113,20 +96,19 @@ s_vCalculateOFDMRParameter(
  *      pbyRsvTime      - pointer to RSPINF RsvTime field
  *
  * Return Value: none
- *
  */
 static
 void
 s_vCalculateOFDMRParameter(
 	unsigned char byRate,
-	CARD_PHY_TYPE ePHYType,
+	u8 bb_type,
 	unsigned char *pbyTxRate,
 	unsigned char *pbyRsvTime
 )
 {
 	switch (byRate) {
 	case RATE_6M:
-		if (ePHYType == PHY_TYPE_11A) {//5GHZ
+		if (bb_type == BB_TYPE_11A) { /* 5GHZ */
 			*pbyTxRate = 0x9B;
 			*pbyRsvTime = 44;
 		} else {
@@ -136,7 +118,7 @@ s_vCalculateOFDMRParameter(
 		break;
 
 	case RATE_9M:
-		if (ePHYType == PHY_TYPE_11A) {//5GHZ
+		if (bb_type == BB_TYPE_11A) { /* 5GHZ */
 			*pbyTxRate = 0x9F;
 			*pbyRsvTime = 36;
 		} else {
@@ -146,7 +128,7 @@ s_vCalculateOFDMRParameter(
 		break;
 
 	case RATE_12M:
-		if (ePHYType == PHY_TYPE_11A) {//5GHZ
+		if (bb_type == BB_TYPE_11A) { /* 5GHZ */
 			*pbyTxRate = 0x9A;
 			*pbyRsvTime = 32;
 		} else {
@@ -156,7 +138,7 @@ s_vCalculateOFDMRParameter(
 		break;
 
 	case RATE_18M:
-		if (ePHYType == PHY_TYPE_11A) {//5GHZ
+		if (bb_type == BB_TYPE_11A) { /* 5GHZ */
 			*pbyTxRate = 0x9E;
 			*pbyRsvTime = 28;
 		} else {
@@ -166,7 +148,7 @@ s_vCalculateOFDMRParameter(
 		break;
 
 	case RATE_36M:
-		if (ePHYType == PHY_TYPE_11A) {//5GHZ
+		if (bb_type == BB_TYPE_11A) { /* 5GHZ */
 			*pbyTxRate = 0x9D;
 			*pbyRsvTime = 24;
 		} else {
@@ -176,7 +158,7 @@ s_vCalculateOFDMRParameter(
 		break;
 
 	case RATE_48M:
-		if (ePHYType == PHY_TYPE_11A) {//5GHZ
+		if (bb_type == BB_TYPE_11A) { /* 5GHZ */
 			*pbyTxRate = 0x98;
 			*pbyRsvTime = 24;
 		} else {
@@ -186,7 +168,7 @@ s_vCalculateOFDMRParameter(
 		break;
 
 	case RATE_54M:
-		if (ePHYType == PHY_TYPE_11A) {//5GHZ
+		if (bb_type == BB_TYPE_11A) { /* 5GHZ */
 			*pbyTxRate = 0x9C;
 			*pbyRsvTime = 24;
 		} else {
@@ -197,7 +179,7 @@ s_vCalculateOFDMRParameter(
 
 	case RATE_24M:
 	default:
-		if (ePHYType == PHY_TYPE_11A) {//5GHZ
+		if (bb_type == BB_TYPE_11A) { /* 5GHZ */
 			*pbyTxRate = 0x99;
 			*pbyRsvTime = 28;
 		} else {
@@ -208,167 +190,9 @@ s_vCalculateOFDMRParameter(
 	}
 }
 
-/*
- * Description: Set RSPINF
- *
- * Parameters:
- *  In:
- *      pDevice             - The adapter to be set
- *  Out:
- *      none
- *
- * Return Value: None.
- *
- */
-static
-void
-s_vSetRSPINF(struct vnt_private *pDevice, CARD_PHY_TYPE ePHYType,
-	     void *pvSupportRateIEs, void *pvExtSupportRateIEs)
-{
-	union vnt_phy_field_swap phy;
-	unsigned char byTxRate = 0, byRsvTime = 0;    // For OFDM
-
-	//Set to Page1
-	MACvSelectPage1(pDevice->PortOffset);
-
-	/* RSPINF_b_1 */
-	vnt_get_phy_field(pDevice,
-			  14,
-			  VNTWIFIbyGetACKTxRate(RATE_1M, pvSupportRateIEs, pvExtSupportRateIEs),
-			  PK_TYPE_11B,
-			  &phy.field_read);
-
-	 /* swap over to get correct write order */
-	swap(phy.swap[0], phy.swap[1]);
-
-	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_1, phy.field_write);
-
-	/* RSPINF_b_2 */
-	vnt_get_phy_field(pDevice, 14,
-			  VNTWIFIbyGetACKTxRate(RATE_2M, pvSupportRateIEs, pvExtSupportRateIEs),
-			  PK_TYPE_11B, &phy.field_read);
-
-	swap(phy.swap[0], phy.swap[1]);
-
-	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_2, phy.field_write);
-
-	/* RSPINF_b_5 */
-	vnt_get_phy_field(pDevice, 14,
-			  VNTWIFIbyGetACKTxRate(RATE_5M, pvSupportRateIEs, pvExtSupportRateIEs),
-			  PK_TYPE_11B, &phy.field_read);
-
-	swap(phy.swap[0], phy.swap[1]);
-
-	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_5, phy.field_write);
-
-	/* RSPINF_b_11 */
-	vnt_get_phy_field(pDevice, 14,
-			  VNTWIFIbyGetACKTxRate(RATE_11M, pvSupportRateIEs, pvExtSupportRateIEs),
-			  PK_TYPE_11B, &phy.field_read);
-
-	swap(phy.swap[0], phy.swap[1]);
-
-	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_11, phy.field_write);
-
-	//RSPINF_a_6
-	s_vCalculateOFDMRParameter(RATE_6M,
-				   ePHYType,
-				   &byTxRate,
-				   &byRsvTime);
-	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_6, MAKEWORD(byTxRate, byRsvTime));
-	//RSPINF_a_9
-	s_vCalculateOFDMRParameter(RATE_9M,
-				   ePHYType,
-				   &byTxRate,
-				   &byRsvTime);
-	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_9, MAKEWORD(byTxRate, byRsvTime));
-	//RSPINF_a_12
-	s_vCalculateOFDMRParameter(RATE_12M,
-				   ePHYType,
-				   &byTxRate,
-				   &byRsvTime);
-	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_12, MAKEWORD(byTxRate, byRsvTime));
-	//RSPINF_a_18
-	s_vCalculateOFDMRParameter(RATE_18M,
-				   ePHYType,
-				   &byTxRate,
-				   &byRsvTime);
-	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_18, MAKEWORD(byTxRate, byRsvTime));
-	//RSPINF_a_24
-	s_vCalculateOFDMRParameter(RATE_24M,
-				   ePHYType,
-				   &byTxRate,
-				   &byRsvTime);
-	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_24, MAKEWORD(byTxRate, byRsvTime));
-	//RSPINF_a_36
-	s_vCalculateOFDMRParameter(
-		VNTWIFIbyGetACKTxRate(RATE_36M, pvSupportRateIEs, pvExtSupportRateIEs),
-		ePHYType,
-		&byTxRate,
-		&byRsvTime);
-	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_36, MAKEWORD(byTxRate, byRsvTime));
-	//RSPINF_a_48
-	s_vCalculateOFDMRParameter(
-		VNTWIFIbyGetACKTxRate(RATE_48M, pvSupportRateIEs, pvExtSupportRateIEs),
-		ePHYType,
-		&byTxRate,
-		&byRsvTime);
-	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_48, MAKEWORD(byTxRate, byRsvTime));
-	//RSPINF_a_54
-	s_vCalculateOFDMRParameter(
-		VNTWIFIbyGetACKTxRate(RATE_54M, pvSupportRateIEs, pvExtSupportRateIEs),
-		ePHYType,
-		&byTxRate,
-		&byRsvTime);
-	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_54, MAKEWORD(byTxRate, byRsvTime));
-	//RSPINF_a_72
-	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_72, MAKEWORD(byTxRate, byRsvTime));
-	//Set to Page0
-	MACvSelectPage0(pDevice->PortOffset);
-}
-
 /*---------------------  Export Functions  --------------------------*/
 
 /*
- * Description: Get Card short preamble option value
- *
- * Parameters:
- *  In:
- *      pDevice             - The adapter to be set
- *  Out:
- *      none
- *
- * Return Value: true if short preamble; otherwise false
- *
- */
-bool CARDbIsShortPreamble(struct vnt_private *pDevice)
-{
-
-	if (pDevice->byPreambleType == 0)
-		return false;
-
-	return true;
-}
-
-/*
- * Description: Get Card short slot time option value
- *
- * Parameters:
- *  In:
- *      pDevice             - The adapter to be set
- *  Out:
- *      none
- *
- * Return Value: true if short slot time; otherwise false
- *
- */
-bool CARDbIsShorSlotTime(struct vnt_private *pDevice)
-{
-
-	return pDevice->bShortSlotTime;
-}
-
-/*
  * Description: Update IFS
  *
  * Parameters:
@@ -378,138 +202,118 @@ bool CARDbIsShorSlotTime(struct vnt_private *pDevice)
  *      none
  *
  * Return Value: None.
- *
  */
-bool CARDbSetPhyParameter(struct vnt_private *pDevice, CARD_PHY_TYPE ePHYType,
-			  unsigned short wCapInfo, unsigned char byERPField,
-			  void *pvSupportRateIEs, void *pvExtSupportRateIEs)
+bool CARDbSetPhyParameter(struct vnt_private *pDevice, u8 bb_type)
 {
 	unsigned char byCWMaxMin = 0;
 	unsigned char bySlot = 0;
 	unsigned char bySIFS = 0;
 	unsigned char byDIFS = 0;
 	unsigned char byData;
-	PWLAN_IE_SUPP_RATES pSupportRates = (PWLAN_IE_SUPP_RATES) pvSupportRateIEs;
-	PWLAN_IE_SUPP_RATES pExtSupportRates = (PWLAN_IE_SUPP_RATES) pvExtSupportRateIEs;
-
-	//Set SIFS, DIFS, EIFS, SlotTime, CwMin
-	if (ePHYType == PHY_TYPE_11A) {
-		if (pSupportRates == NULL)
-			pSupportRates = (PWLAN_IE_SUPP_RATES) abyDefaultSuppRatesA;
+	int i;
 
+	/* Set SIFS, DIFS, EIFS, SlotTime, CwMin */
+	if (bb_type == BB_TYPE_11A) {
 		if (pDevice->byRFType == RF_AIROHA7230) {
-			// AL7230 use single PAPE and connect to PAPE_2.4G
+			/* AL7230 use single PAPE and connect to PAPE_2.4G */
 			MACvSetBBType(pDevice->PortOffset, BB_TYPE_11G);
 			pDevice->abyBBVGA[0] = 0x20;
 			pDevice->abyBBVGA[2] = 0x10;
 			pDevice->abyBBVGA[3] = 0x10;
-			BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
+			BBbReadEmbedded(pDevice, 0xE7, &byData);
 			if (byData == 0x1C)
-				BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
+				BBbWriteEmbedded(pDevice, 0xE7, pDevice->abyBBVGA[0]);
 
 		} else if (pDevice->byRFType == RF_UW2452) {
 			MACvSetBBType(pDevice->PortOffset, BB_TYPE_11A);
 			pDevice->abyBBVGA[0] = 0x18;
-			BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
+			BBbReadEmbedded(pDevice, 0xE7, &byData);
 			if (byData == 0x14) {
-				BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
-				BBbWriteEmbedded(pDevice->PortOffset, 0xE1, 0x57);
+				BBbWriteEmbedded(pDevice, 0xE7, pDevice->abyBBVGA[0]);
+				BBbWriteEmbedded(pDevice, 0xE1, 0x57);
 			}
 		} else {
 			MACvSetBBType(pDevice->PortOffset, BB_TYPE_11A);
 		}
-		BBbWriteEmbedded(pDevice->PortOffset, 0x88, 0x03);
+		BBbWriteEmbedded(pDevice, 0x88, 0x03);
 		bySlot = C_SLOT_SHORT;
 		bySIFS = C_SIFS_A;
 		byDIFS = C_SIFS_A + 2*C_SLOT_SHORT;
 		byCWMaxMin = 0xA4;
-	} else if (ePHYType == PHY_TYPE_11B) {
-		if (pSupportRates == NULL)
-			pSupportRates = (PWLAN_IE_SUPP_RATES) abyDefaultSuppRatesB;
-
+	} else if (bb_type == BB_TYPE_11B) {
 		MACvSetBBType(pDevice->PortOffset, BB_TYPE_11B);
 		if (pDevice->byRFType == RF_AIROHA7230) {
 			pDevice->abyBBVGA[0] = 0x1C;
 			pDevice->abyBBVGA[2] = 0x00;
 			pDevice->abyBBVGA[3] = 0x00;
-			BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
+			BBbReadEmbedded(pDevice, 0xE7, &byData);
 			if (byData == 0x20)
-				BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
+				BBbWriteEmbedded(pDevice, 0xE7, pDevice->abyBBVGA[0]);
 
 		} else if (pDevice->byRFType == RF_UW2452) {
 			pDevice->abyBBVGA[0] = 0x14;
-			BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
+			BBbReadEmbedded(pDevice, 0xE7, &byData);
 			if (byData == 0x18) {
-				BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
-				BBbWriteEmbedded(pDevice->PortOffset, 0xE1, 0xD3);
+				BBbWriteEmbedded(pDevice, 0xE7, pDevice->abyBBVGA[0]);
+				BBbWriteEmbedded(pDevice, 0xE1, 0xD3);
 			}
 		}
-		BBbWriteEmbedded(pDevice->PortOffset, 0x88, 0x02);
+		BBbWriteEmbedded(pDevice, 0x88, 0x02);
 		bySlot = C_SLOT_LONG;
 		bySIFS = C_SIFS_BG;
 		byDIFS = C_SIFS_BG + 2*C_SLOT_LONG;
 		byCWMaxMin = 0xA5;
-	} else {// PK_TYPE_11GA & PK_TYPE_11GB
-		if (pSupportRates == NULL) {
-			pSupportRates = (PWLAN_IE_SUPP_RATES) abyDefaultSuppRatesG;
-			pExtSupportRates = (PWLAN_IE_SUPP_RATES) abyDefaultExtSuppRatesG;
-		}
+	} else { /* PK_TYPE_11GA & PK_TYPE_11GB */
 		MACvSetBBType(pDevice->PortOffset, BB_TYPE_11G);
 		if (pDevice->byRFType == RF_AIROHA7230) {
 			pDevice->abyBBVGA[0] = 0x1C;
 			pDevice->abyBBVGA[2] = 0x00;
 			pDevice->abyBBVGA[3] = 0x00;
-			BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
+			BBbReadEmbedded(pDevice, 0xE7, &byData);
 			if (byData == 0x20)
-				BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
+				BBbWriteEmbedded(pDevice, 0xE7, pDevice->abyBBVGA[0]);
 
 		} else if (pDevice->byRFType == RF_UW2452) {
 			pDevice->abyBBVGA[0] = 0x14;
-			BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
+			BBbReadEmbedded(pDevice, 0xE7, &byData);
 			if (byData == 0x18) {
-				BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
-				BBbWriteEmbedded(pDevice->PortOffset, 0xE1, 0xD3);
+				BBbWriteEmbedded(pDevice, 0xE7, pDevice->abyBBVGA[0]);
+				BBbWriteEmbedded(pDevice, 0xE1, 0xD3);
 			}
 		}
-		BBbWriteEmbedded(pDevice->PortOffset, 0x88, 0x08);
+		BBbWriteEmbedded(pDevice, 0x88, 0x08);
 		bySIFS = C_SIFS_BG;
-		if (VNTWIFIbIsShortSlotTime(wCapInfo)) {
+
+		if (pDevice->bShortSlotTime) {
 			bySlot = C_SLOT_SHORT;
 			byDIFS = C_SIFS_BG + 2*C_SLOT_SHORT;
 		} else {
 			bySlot = C_SLOT_LONG;
 			byDIFS = C_SIFS_BG + 2*C_SLOT_LONG;
 		}
-		if (VNTWIFIbyGetMaxSupportRate(pSupportRates, pExtSupportRates) > RATE_11M)
-			byCWMaxMin = 0xA4;
-		else
-			byCWMaxMin = 0xA5;
-
-		if (pDevice->bProtectMode != VNTWIFIbIsProtectMode(byERPField)) {
-			pDevice->bProtectMode = VNTWIFIbIsProtectMode(byERPField);
-			if (pDevice->bProtectMode)
-				MACvEnableProtectMD(pDevice->PortOffset);
-			else
-				MACvDisableProtectMD(pDevice->PortOffset);
 
-		}
-		if (pDevice->bBarkerPreambleMd != VNTWIFIbIsBarkerMode(byERPField)) {
-			pDevice->bBarkerPreambleMd = VNTWIFIbIsBarkerMode(byERPField);
-			if (pDevice->bBarkerPreambleMd)
-				MACvEnableBarkerPreambleMd(pDevice->PortOffset);
-			else
-				MACvDisableBarkerPreambleMd(pDevice->PortOffset);
+		byCWMaxMin = 0xa4;
+
+		for (i = RATE_54M; i >= RATE_6M; i--) {
+			if (pDevice->basic_rates & ((u32)(0x1 << i))) {
+				byCWMaxMin |= 0x1;
+				break;
+			}
 		}
 	}
 
 	if (pDevice->byRFType == RF_RFMD2959) {
-		// bcs TX_PE will reserve 3 us
-		// hardware's processing time here is 2 us.
+		/*
+		 * bcs TX_PE will reserve 3 us hardware's processing
+		 * time here is 2 us.
+		 */
 		bySIFS -= 3;
 		byDIFS -= 3;
-		//{{ RobertYu: 20041202
-		//// TX_PE will reserve 3 us for MAX2829 A mode only, it is for better TX throughput
-		//// MAC will need 2 us to process, so the SIFS, DIFS can be shorter by 2 us.
+		/*
+		 * TX_PE will reserve 3 us for MAX2829 A mode only, it is for
+		 * better TX throughput; MAC will need 2 us to process, so the
+		 * SIFS, DIFS can be shorter by 2 us.
+		 */
 	}
 
 	if (pDevice->bySIFS != bySIFS) {
@@ -527,10 +331,6 @@ bool CARDbSetPhyParameter(struct vnt_private *pDevice, CARD_PHY_TYPE ePHYType,
 	if (pDevice->bySlot != bySlot) {
 		pDevice->bySlot = bySlot;
 		VNSvOutPortB(pDevice->PortOffset + MAC_REG_SLOT, pDevice->bySlot);
-		if (pDevice->bySlot == C_SLOT_SHORT)
-			pDevice->bShortSlotTime = true;
-		else
-			pDevice->bShortSlotTime = false;
 
 		BBvSetShortSlotTime(pDevice);
 	}
@@ -538,14 +338,11 @@ bool CARDbSetPhyParameter(struct vnt_private *pDevice, CARD_PHY_TYPE ePHYType,
 		pDevice->byCWMaxMin = byCWMaxMin;
 		VNSvOutPortB(pDevice->PortOffset + MAC_REG_CWMAXMIN0, pDevice->byCWMaxMin);
 	}
-	if (VNTWIFIbIsShortPreamble(wCapInfo))
-		pDevice->byPreambleType = pDevice->byShortPreamble;
-	else
-		pDevice->byPreambleType = 0;
 
-	s_vSetRSPINF(pDevice, ePHYType, pSupportRates, pExtSupportRates);
-	pDevice->eCurrentPHYType = ePHYType;
-	// set for NDIS OID_802_11SUPPORTED_RATES
+	pDevice->byPacketType = CARDbyGetPktType(pDevice);
+
+	CARDvSetRSPINF(pDevice, bb_type);
+
 	return true;
 }
 
@@ -563,7 +360,6 @@ bool CARDbSetPhyParameter(struct vnt_private *pDevice, CARD_PHY_TYPE ePHYType,
  *      none
  *
  * Return Value: none
- *
  */
 bool CARDbUpdateTSF(struct vnt_private *pDevice, unsigned char byRxRate,
 		    u64 qwBSSTimestamp, u64 qwLocalTSF)
@@ -572,8 +368,7 @@ bool CARDbUpdateTSF(struct vnt_private *pDevice, unsigned char byRxRate,
 
 	if (qwBSSTimestamp != qwLocalTSF) {
 		qwTSFOffset = CARDqGetTSFOffset(byRxRate, qwBSSTimestamp, qwLocalTSF);
-		// adjust TSF
-		// HW's TSF add TSF Offset reg
+		/* adjust TSF, HW's TSF add TSF Offset reg */
 		VNSvOutPortD(pDevice->PortOffset + MAC_REG_TSFOFST, (u32)qwTSFOffset);
 		VNSvOutPortD(pDevice->PortOffset + MAC_REG_TSFOFST + 4, (u32)(qwTSFOffset >> 32));
 		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TFTCTL, TFTCTL_TSFSYNCEN);
@@ -593,21 +388,20 @@ bool CARDbUpdateTSF(struct vnt_private *pDevice, unsigned char byRxRate,
  *      none
  *
  * Return Value: true if succeed; otherwise false
- *
  */
 bool CARDbSetBeaconPeriod(struct vnt_private *pDevice,
 			  unsigned short wBeaconInterval)
 {
 	u64 qwNextTBTT = 0;
 
-	CARDbGetCurrentTSF(pDevice->PortOffset, &qwNextTBTT); //Get Local TSF counter
+	CARDbGetCurrentTSF(pDevice, &qwNextTBTT); /* Get Local TSF counter */
 
 	qwNextTBTT = CARDqGetNextTBTT(qwNextTBTT, wBeaconInterval);
 
-	// set HW beacon interval
+	/* set HW beacon interval */
 	VNSvOutPortW(pDevice->PortOffset + MAC_REG_BI, wBeaconInterval);
 	pDevice->wBeaconInterval = wBeaconInterval;
-	// Set NextTBTT
+	/* Set NextTBTT */
 	VNSvOutPortD(pDevice->PortOffset + MAC_REG_NEXTTBTT, (u32)qwNextTBTT);
 	VNSvOutPortD(pDevice->PortOffset + MAC_REG_NEXTTBTT + 4, (u32)(qwNextTBTT >> 32));
 	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TFTCTL, TFTCTL_TBTTSYNCEN);
@@ -616,225 +410,6 @@ bool CARDbSetBeaconPeriod(struct vnt_private *pDevice,
 }
 
 /*
- * Description: Card Stop Hardware Tx
- *
- * Parameters:
- *  In:
- *      pDeviceHandler      - The adapter to be set
- *      ePktType            - Packet type to stop
- *  Out:
- *      none
- *
- * Return Value: true if all data packet complete; otherwise false.
- *
- */
-bool CARDbStopTxPacket(struct vnt_private *pDevice, CARD_PKT_TYPE ePktType)
-{
-
-	if (ePktType == PKT_TYPE_802_11_ALL) {
-		pDevice->bStopBeacon = true;
-		pDevice->bStopTx0Pkt = true;
-		pDevice->bStopDataPkt = true;
-	} else if (ePktType == PKT_TYPE_802_11_BCN) {
-		pDevice->bStopBeacon = true;
-	} else if (ePktType == PKT_TYPE_802_11_MNG) {
-		pDevice->bStopTx0Pkt = true;
-	} else if (ePktType == PKT_TYPE_802_11_DATA) {
-		pDevice->bStopDataPkt = true;
-	}
-
-	if (pDevice->bStopBeacon == true) {
-		if (pDevice->bIsBeaconBufReadySet == true) {
-			if (pDevice->cbBeaconBufReadySetCnt < WAIT_BEACON_TX_DOWN_TMO) {
-				pDevice->cbBeaconBufReadySetCnt++;
-				return false;
-			}
-		}
-		pDevice->bIsBeaconBufReadySet = false;
-		pDevice->cbBeaconBufReadySetCnt = 0;
-		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
-	}
-	// wait all TD0 complete
-	if (pDevice->bStopTx0Pkt == true) {
-		if (pDevice->iTDUsed[TYPE_TXDMA0] != 0)
-			return false;
-	}
-	// wait all Data TD complete
-	if (pDevice->bStopDataPkt == true) {
-		if (pDevice->iTDUsed[TYPE_AC0DMA] != 0)
-			return false;
-	}
-
-	return true;
-}
-
-/*
- * Description: Card Start Hardware Tx
- *
- * Parameters:
- *  In:
- *      pDeviceHandler      - The adapter to be set
- *      ePktType            - Packet type to start
- *  Out:
- *      none
- *
- * Return Value: true if success; false if failed.
- *
- */
-bool CARDbStartTxPacket(struct vnt_private *pDevice, CARD_PKT_TYPE ePktType)
-{
-
-	if (ePktType == PKT_TYPE_802_11_ALL) {
-		pDevice->bStopBeacon = false;
-		pDevice->bStopTx0Pkt = false;
-		pDevice->bStopDataPkt = false;
-	} else if (ePktType == PKT_TYPE_802_11_BCN) {
-		pDevice->bStopBeacon = false;
-	} else if (ePktType == PKT_TYPE_802_11_MNG) {
-		pDevice->bStopTx0Pkt = false;
-	} else if (ePktType == PKT_TYPE_802_11_DATA) {
-		pDevice->bStopDataPkt = false;
-	}
-
-	if ((pDevice->bStopBeacon == false) &&
-	    (pDevice->bBeaconBufReady == true) &&
-	    (pDevice->op_mode == NL80211_IFTYPE_ADHOC)) {
-		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
-	}
-
-	return true;
-}
-
-/*
- * Description: Card Set BSSID value
- *
- * Parameters:
- *  In:
- *      pDeviceHandler      - The adapter to be set
- *      pbyBSSID            - pointer to BSSID field
- *      bAdhoc              - flag to indicate IBSS
- *  Out:
- *      none
- *
- * Return Value: true if success; false if failed.
- *
- */
-bool CARDbSetBSSID(struct vnt_private *pDevice,
-		   unsigned char *pbyBSSID, enum nl80211_iftype op_mode)
-{
-
-	MACvWriteBSSIDAddress(pDevice->PortOffset, pbyBSSID);
-	memcpy(pDevice->abyBSSID, pbyBSSID, WLAN_BSSID_LEN);
-	if (op_mode == NL80211_IFTYPE_ADHOC)
-		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_ADHOC);
-	else
-		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_ADHOC);
-
-	if (op_mode == NL80211_IFTYPE_AP)
-		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_AP);
-	else
-		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_AP);
-
-	if (op_mode == NL80211_IFTYPE_UNSPECIFIED) {
-		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_RCR, RCR_BSSID);
-		pDevice->bBSSIDFilter = false;
-		pDevice->byRxMode &= ~RCR_BSSID;
-		pr_debug("wcmd: rx_mode = %x\n", pDevice->byRxMode);
-	} else {
-		if (is_zero_ether_addr(pDevice->abyBSSID) == false) {
-			MACvRegBitsOn(pDevice->PortOffset, MAC_REG_RCR, RCR_BSSID);
-			pDevice->bBSSIDFilter = true;
-			pDevice->byRxMode |= RCR_BSSID;
-		}
-		pr_debug("wmgr: rx_mode = %x\n", pDevice->byRxMode);
-	}
-	// Adopt BSS state in Adapter Device Object
-	pDevice->op_mode = op_mode;
-	return true;
-}
-
-/*
- * Description: Card indicate status
- *
- * Parameters:
- *  In:
- *      pDeviceHandler      - The adapter to be set
- *      eStatus             - Status
- *  Out:
- *      none
- *
- * Return Value: true if success; false if failed.
- *
- */
-
-/*
- * Description: Save Assoc info. contain in assoc. response frame
- *
- * Parameters:
- *  In:
- *      pDevice             - The adapter to be set
- *      wCapabilityInfo     - Capability information
- *      wStatus             - Status code
- *      wAID                - Assoc. ID
- *      uLen                - Length of IEs
- *      pbyIEs              - pointer to IEs
- *  Out:
- *      none
- *
- * Return Value: true if succeed; otherwise false
- *
- */
-bool CARDbSetTxDataRate(
-	struct vnt_private *pDevice,
-	unsigned short wDataRate
-)
-{
-
-	pDevice->wCurrentRate = wDataRate;
-	return true;
-}
-
-/*+
- *
- * Routine Description:
- *      Consider to power down when no more packets to tx or rx.
- *
- * Parameters:
- *  In:
- *      pDevice             - The adapter to be set
- *  Out:
- *      none
- *
- * Return Value: true if power down success; otherwise false
- *
- -*/
-bool
-CARDbPowerDown(
-	struct vnt_private *pDevice
-)
-{
-	unsigned int uIdx;
-
-	// check if already in Doze mode
-	if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS))
-		return true;
-
-	// Froce PSEN on
-	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PSEN);
-
-	// check if all TD are empty,
-
-	for (uIdx = 0; uIdx < TYPE_MAXTD; uIdx++) {
-		if (pDevice->iTDUsed[uIdx] != 0)
-			return false;
-	}
-
-	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_GO2DOZE);
-	pr_debug("Go to Doze ZZZZZZZZZZZZZZZ\n");
-	return true;
-}
-
-/*
  * Description: Turn off Radio power
  *
  * Parameters:
@@ -844,7 +419,6 @@ CARDbPowerDown(
  *      none
  *
  * Return Value: true if success; otherwise false
- *
  */
 bool CARDbRadioPowerOff(struct vnt_private *pDevice)
 {
@@ -861,7 +435,7 @@ bool CARDbRadioPowerOff(struct vnt_private *pDevice)
 
 	case RF_AIROHA:
 	case RF_AL2230S:
-	case RF_AIROHA7230: //RobertYu:20050104
+	case RF_AIROHA7230:
 		MACvWordRegBitsOff(pDevice->PortOffset, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE2);
 		MACvWordRegBitsOff(pDevice->PortOffset, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
 		break;
@@ -870,12 +444,11 @@ bool CARDbRadioPowerOff(struct vnt_private *pDevice)
 
 	MACvRegBitsOff(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_RXON);
 
-	BBvSetDeepSleep(pDevice->PortOffset, pDevice->byLocalID);
+	BBvSetDeepSleep(pDevice, pDevice->byLocalID);
 
 	pDevice->bRadioOff = true;
-	//2007-0409-03,<Add> by chester
 	pr_debug("chester power off\n");
-	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_GPIOCTL0, LED_ACTSET);  //LED issue
+	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_GPIOCTL0, LED_ACTSET);  /* LED issue */
 	return bResult;
 }
 
@@ -889,7 +462,6 @@ bool CARDbRadioPowerOff(struct vnt_private *pDevice)
  *      none
  *
  * Return Value: true if success; otherwise false
- *
  */
 bool CARDbRadioPowerOn(struct vnt_private *pDevice)
 {
@@ -907,7 +479,7 @@ bool CARDbRadioPowerOn(struct vnt_private *pDevice)
 		pr_debug("chester pbRadioOff\n");
 		return true; }
 
-	BBvExitDeepSleep(pDevice->PortOffset, pDevice->byLocalID);
+	BBvExitDeepSleep(pDevice, pDevice->byLocalID);
 
 	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_RXON);
 
@@ -919,7 +491,7 @@ bool CARDbRadioPowerOn(struct vnt_private *pDevice)
 
 	case RF_AIROHA:
 	case RF_AL2230S:
-	case RF_AIROHA7230: //RobertYu:20050104
+	case RF_AIROHA7230:
 		MACvWordRegBitsOn(pDevice->PortOffset, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPE2 |
 									    SOFTPWRCTL_SWPE3));
 		break;
@@ -927,493 +499,11 @@ bool CARDbRadioPowerOn(struct vnt_private *pDevice)
 	}
 
 	pDevice->bRadioOff = false;
-//  2007-0409-03,<Add> by chester
 	pr_debug("chester power on\n");
-	MACvRegBitsOff(pDevice->PortOffset, MAC_REG_GPIOCTL0, LED_ACTSET); //LED issue
+	MACvRegBitsOff(pDevice->PortOffset, MAC_REG_GPIOCTL0, LED_ACTSET); /* LED issue */
 	return bResult;
 }
 
-bool CARDbRemoveKey(struct vnt_private *pDevice, unsigned char *pbyBSSID)
-{
-
-	KeybRemoveAllKey(&(pDevice->sKey), pbyBSSID, pDevice->PortOffset);
-	return true;
-}
-
-/*
- *
- * Description:
- *    Add BSSID in PMKID Candidate list.
- *
- * Parameters:
- *  In:
- *      hDeviceContext - device structure point
- *      pbyBSSID - BSSID address for adding
- *      wRSNCap - BSS's RSN capability
- *  Out:
- *      none
- *
- * Return Value: none.
- *
- -*/
-bool
-CARDbAdd_PMKID_Candidate(
-	struct vnt_private *pDevice,
-	unsigned char *pbyBSSID,
-	bool bRSNCapExist,
-	unsigned short wRSNCap
-)
-{
-	struct pmkid_candidate *pCandidateList;
-	unsigned int ii = 0;
-
-	pr_debug("bAdd_PMKID_Candidate START: (%d)\n",
-		 (int)pDevice->gsPMKIDCandidate.NumCandidates);
-
-	if (pDevice->gsPMKIDCandidate.NumCandidates >= MAX_PMKIDLIST) {
-		pr_debug("vFlush_PMKID_Candidate: 3\n");
-		memset(&pDevice->gsPMKIDCandidate, 0, sizeof(SPMKIDCandidateEvent));
-	}
-
-	for (ii = 0; ii < 6; ii++)
-		pr_debug("%02X ", *(pbyBSSID + ii));
-
-	pr_debug("\n");
-
-	// Update Old Candidate
-	for (ii = 0; ii < pDevice->gsPMKIDCandidate.NumCandidates; ii++) {
-		pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[ii];
-		if (!memcmp(pCandidateList->BSSID, pbyBSSID, ETH_ALEN)) {
-			if (bRSNCapExist && (wRSNCap & BIT0))
-				pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
-			else
-				pCandidateList->Flags &= ~(NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED);
-
-			return true;
-		}
-	}
-
-	// New Candidate
-	pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[pDevice->gsPMKIDCandidate.NumCandidates];
-	if (bRSNCapExist && (wRSNCap & BIT0))
-		pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
-	else
-		pCandidateList->Flags &= ~(NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED);
-
-	memcpy(pCandidateList->BSSID, pbyBSSID, ETH_ALEN);
-	pDevice->gsPMKIDCandidate.NumCandidates++;
-	pr_debug("NumCandidates:%d\n",
-		 (int)pDevice->gsPMKIDCandidate.NumCandidates);
-	return true;
-}
-
-void *
-CARDpGetCurrentAddress(
-	struct vnt_private *pDevice
-)
-{
-
-	return pDevice->abyCurrentNetAddr;
-}
-
-/*
- *
- * Description:
- *    Start Spectrum Measure defined in 802.11h
- *
- * Parameters:
- *  In:
- *      hDeviceContext - device structure point
- *  Out:
- *      none
- *
- * Return Value: none.
- *
- -*/
-bool
-CARDbStartMeasure(
-	struct vnt_private *pDevice,
-	void *pvMeasureEIDs,
-	unsigned int uNumOfMeasureEIDs
-)
-{
-	PWLAN_IE_MEASURE_REQ    pEID = (PWLAN_IE_MEASURE_REQ) pvMeasureEIDs;
-	u64 qwCurrTSF;
-	u64 qwStartTSF;
-	bool bExpired = true;
-	unsigned short wDuration = 0;
-
-	if ((pEID == NULL) ||
-	    (uNumOfMeasureEIDs == 0)) {
-		return true;
-	}
-	CARDbGetCurrentTSF(pDevice->PortOffset, &qwCurrTSF);
-	if (pDevice->bMeasureInProgress == true) {
-		pDevice->bMeasureInProgress = false;
-		VNSvOutPortB(pDevice->PortOffset + MAC_REG_RCR, pDevice->byOrgRCR);
-		MACvSelectPage1(pDevice->PortOffset);
-		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR0, pDevice->dwOrgMAR0);
-		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR4, pDevice->dwOrgMAR4);
-		// clear measure control
-		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_EN);
-		MACvSelectPage0(pDevice->PortOffset);
-		set_channel(pDevice, pDevice->byOrgChannel);
-		MACvSelectPage1(pDevice->PortOffset);
-		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
-		MACvSelectPage0(pDevice->PortOffset);
-	}
-	pDevice->uNumOfMeasureEIDs = uNumOfMeasureEIDs;
-
-	do {
-		pDevice->pCurrMeasureEID = pEID;
-		pEID++;
-		pDevice->uNumOfMeasureEIDs--;
-
-		if (pDevice->byLocalID > REV_ID_VT3253_B1) {
-			qwStartTSF = *((u64 *)(pDevice->pCurrMeasureEID->sReq.abyStartTime));
-			wDuration = *((unsigned short *)(pDevice->pCurrMeasureEID->sReq.abyDuration));
-			wDuration += 1; // 1 TU for channel switching
-
-			if (qwStartTSF == 0) {
-				// start immediately by setting start TSF == current TSF + 2 TU
-				qwStartTSF = qwCurrTSF + 2048;
-
-				bExpired = false;
-				break;
-			} else {
-				// start at setting start TSF - 1TU(for channel switching)
-				qwStartTSF -= 1024;
-			}
-
-			if (qwCurrTSF < qwStartTSF) {
-				bExpired = false;
-				break;
-			}
-			VNTWIFIbMeasureReport(pDevice->pMgmt,
-					      false,
-					      pDevice->pCurrMeasureEID,
-					      MEASURE_MODE_LATE,
-					      pDevice->byBasicMap,
-					      pDevice->byCCAFraction,
-					      pDevice->abyRPIs
-				);
-		} else {
-			// hardware do not support measure
-			VNTWIFIbMeasureReport(pDevice->pMgmt,
-					      false,
-					      pDevice->pCurrMeasureEID,
-					      MEASURE_MODE_INCAPABLE,
-					      pDevice->byBasicMap,
-					      pDevice->byCCAFraction,
-					      pDevice->abyRPIs
-				);
-		}
-	} while (pDevice->uNumOfMeasureEIDs != 0);
-
-	if (!bExpired) {
-		MACvSelectPage1(pDevice->PortOffset);
-		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MSRSTART, (u32)qwStartTSF);
-		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MSRSTART + 4, (u32)(qwStartTSF >> 32));
-		VNSvOutPortW(pDevice->PortOffset + MAC_REG_MSRDURATION, wDuration);
-		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_EN);
-		MACvSelectPage0(pDevice->PortOffset);
-	} else {
-		// all measure start time expired we should complete action
-		VNTWIFIbMeasureReport(pDevice->pMgmt,
-				      true,
-				      NULL,
-				      0,
-				      pDevice->byBasicMap,
-				      pDevice->byCCAFraction,
-				      pDevice->abyRPIs
-			);
-	}
-	return true;
-}
-
-/*
- *
- * Description:
- *    Do Channel Switch defined in 802.11h
- *
- * Parameters:
- *  In:
- *      hDeviceContext - device structure point
- *  Out:
- *      none
- *
- * Return Value: none.
- *
- -*/
-bool
-CARDbChannelSwitch(
-	struct vnt_private *pDevice,
-	unsigned char byMode,
-	unsigned char byNewChannel,
-	unsigned char byCount
-)
-{
-	bool bResult = true;
-
-	if (byCount == 0) {
-		bResult = set_channel(pDevice, byNewChannel);
-		VNTWIFIbChannelSwitch(pDevice->pMgmt, byNewChannel);
-		MACvSelectPage1(pDevice->PortOffset);
-		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
-		MACvSelectPage0(pDevice->PortOffset);
-		return bResult;
-	}
-	pDevice->byChannelSwitchCount = byCount;
-	pDevice->byNewChannel = byNewChannel;
-	pDevice->bChannelSwitch = true;
-	if (byMode == 1)
-		bResult = CARDbStopTxPacket(pDevice, PKT_TYPE_802_11_ALL);
-
-	return bResult;
-}
-
-/*
- *
- * Description:
- *    Handle Quiet EID defined in 802.11h
- *
- * Parameters:
- *  In:
- *      hDeviceContext - device structure point
- *  Out:
- *      none
- *
- * Return Value: none.
- *
- -*/
-bool
-CARDbSetQuiet(
-	struct vnt_private *pDevice,
-	bool bResetQuiet,
-	unsigned char byQuietCount,
-	unsigned char byQuietPeriod,
-	unsigned short wQuietDuration,
-	unsigned short wQuietOffset
-)
-{
-	unsigned int ii = 0;
-
-	if (bResetQuiet) {
-		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_MSRCTL, (MSRCTL_QUIETTXCHK | MSRCTL_QUIETEN));
-		for (ii = 0; ii < MAX_QUIET_COUNT; ii++)
-			pDevice->sQuiet[ii].bEnable = false;
-
-		pDevice->uQuietEnqueue = 0;
-		pDevice->bEnableFirstQuiet = false;
-		pDevice->bQuietEnable = false;
-		pDevice->byQuietStartCount = byQuietCount;
-	}
-	if (pDevice->sQuiet[pDevice->uQuietEnqueue].bEnable == false) {
-		pDevice->sQuiet[pDevice->uQuietEnqueue].bEnable = true;
-		pDevice->sQuiet[pDevice->uQuietEnqueue].byPeriod = byQuietPeriod;
-		pDevice->sQuiet[pDevice->uQuietEnqueue].wDuration = wQuietDuration;
-		pDevice->sQuiet[pDevice->uQuietEnqueue].dwStartTime = (unsigned long) byQuietCount;
-		pDevice->sQuiet[pDevice->uQuietEnqueue].dwStartTime *= pDevice->wBeaconInterval;
-		pDevice->sQuiet[pDevice->uQuietEnqueue].dwStartTime += wQuietOffset;
-		pDevice->uQuietEnqueue++;
-		pDevice->uQuietEnqueue %= MAX_QUIET_COUNT;
-		if (pDevice->byQuietStartCount < byQuietCount)
-			pDevice->byQuietStartCount = byQuietCount;
-	}
-	return true;
-}
-
-/*
- *
- * Description:
- *    Do Quiet, It will be called by either ISR(after start)
- *    or VNTWIFI(before start) so we do not need a SPINLOCK
- *
- * Parameters:
- *  In:
- *      hDeviceContext - device structure point
- *  Out:
- *      none
- *
- * Return Value: none.
- *
- -*/
-bool
-CARDbStartQuiet(
-	struct vnt_private *pDevice
-)
-{
-	unsigned int ii = 0;
-	unsigned long dwStartTime = 0xFFFFFFFF;
-	unsigned int uCurrentQuietIndex = 0;
-	unsigned long dwNextTime = 0;
-	unsigned long dwGap = 0;
-	unsigned long dwDuration = 0;
-
-	for (ii = 0; ii < MAX_QUIET_COUNT; ii++) {
-		if ((pDevice->sQuiet[ii].bEnable == true) &&
-		    (dwStartTime > pDevice->sQuiet[ii].dwStartTime)) {
-			dwStartTime = pDevice->sQuiet[ii].dwStartTime;
-			uCurrentQuietIndex = ii;
-		}
-	}
-	if (dwStartTime == 0xFFFFFFFF) {
-		// no more quiet
-		pDevice->bQuietEnable = false;
-		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_MSRCTL, (MSRCTL_QUIETTXCHK | MSRCTL_QUIETEN));
-	} else {
-		if (pDevice->bQuietEnable == false) {
-			// first quiet
-			pDevice->byQuietStartCount--;
-			dwNextTime = pDevice->sQuiet[uCurrentQuietIndex].dwStartTime;
-			dwNextTime %= pDevice->wBeaconInterval;
-			MACvSelectPage1(pDevice->PortOffset);
-			VNSvOutPortW(pDevice->PortOffset + MAC_REG_QUIETINIT, (unsigned short) dwNextTime);
-			VNSvOutPortW(pDevice->PortOffset + MAC_REG_QUIETDUR, (unsigned short) pDevice->sQuiet[uCurrentQuietIndex].wDuration);
-			if (pDevice->byQuietStartCount == 0) {
-				pDevice->bEnableFirstQuiet = false;
-				MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL, (MSRCTL_QUIETTXCHK | MSRCTL_QUIETEN));
-			} else {
-				pDevice->bEnableFirstQuiet = true;
-			}
-			MACvSelectPage0(pDevice->PortOffset);
-		} else {
-			if (pDevice->dwCurrentQuietEndTime > pDevice->sQuiet[uCurrentQuietIndex].dwStartTime) {
-				// overlap with previous Quiet
-				dwGap =  pDevice->dwCurrentQuietEndTime - pDevice->sQuiet[uCurrentQuietIndex].dwStartTime;
-				if (dwGap >= pDevice->sQuiet[uCurrentQuietIndex].wDuration) {
-					// return false to indicate next quiet expired, should call this function again
-					return false;
-				}
-				dwDuration = pDevice->sQuiet[uCurrentQuietIndex].wDuration - dwGap;
-				dwGap = 0;
-			} else {
-				dwGap = pDevice->sQuiet[uCurrentQuietIndex].dwStartTime - pDevice->dwCurrentQuietEndTime;
-				dwDuration = pDevice->sQuiet[uCurrentQuietIndex].wDuration;
-			}
-			// set GAP and Next duration
-			MACvSelectPage1(pDevice->PortOffset);
-			VNSvOutPortW(pDevice->PortOffset + MAC_REG_QUIETGAP, (unsigned short) dwGap);
-			VNSvOutPortW(pDevice->PortOffset + MAC_REG_QUIETDUR, (unsigned short) dwDuration);
-			MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_QUIETRPT);
-			MACvSelectPage0(pDevice->PortOffset);
-		}
-		pDevice->bQuietEnable = true;
-		pDevice->dwCurrentQuietEndTime = pDevice->sQuiet[uCurrentQuietIndex].dwStartTime;
-		pDevice->dwCurrentQuietEndTime += pDevice->sQuiet[uCurrentQuietIndex].wDuration;
-		if (pDevice->sQuiet[uCurrentQuietIndex].byPeriod == 0) {
-			// not period disable current quiet element
-			pDevice->sQuiet[uCurrentQuietIndex].bEnable = false;
-		} else {
-			// set next period start time
-			dwNextTime = (unsigned long) pDevice->sQuiet[uCurrentQuietIndex].byPeriod;
-			dwNextTime *= pDevice->wBeaconInterval;
-			pDevice->sQuiet[uCurrentQuietIndex].dwStartTime = dwNextTime;
-		}
-		if (pDevice->dwCurrentQuietEndTime > 0x80010000) {
-			// decreament all time to avoid wrap around
-			for (ii = 0; ii < MAX_QUIET_COUNT; ii++) {
-				if (pDevice->sQuiet[ii].bEnable == true)
-					pDevice->sQuiet[ii].dwStartTime -= 0x80000000;
-
-			}
-			pDevice->dwCurrentQuietEndTime -= 0x80000000;
-		}
-	}
-	return true;
-}
-
-/*
- *
- * Description:
- *    Set Local Power Constraint
- *
- * Parameters:
- *  In:
- *      hDeviceContext - device structure point
- *  Out:
- *      none
- *
- * Return Value: none.
- *
- -*/
-void
-CARDvSetPowerConstraint(
-	struct vnt_private *pDevice,
-	unsigned char byChannel,
-	char byPower
-)
-{
-
-	if (byChannel > CB_MAX_CHANNEL_24G) {
-		if (pDevice->bCountryInfo5G == true)
-			pDevice->abyLocalPwr[byChannel] = pDevice->abyRegPwr[byChannel] - byPower;
-
-	} else {
-		if (pDevice->bCountryInfo24G == true)
-			pDevice->abyLocalPwr[byChannel] = pDevice->abyRegPwr[byChannel] - byPower;
-
-	}
-}
-
-/*
- *
- * Description:
- *    Set Local Power Constraint
- *
- * Parameters:
- *  In:
- *      hDeviceContext - device structure point
- *  Out:
- *      none
- *
- * Return Value: none.
- *
- -*/
-void
-CARDvGetPowerCapability(
-	struct vnt_private *pDevice,
-	unsigned char *pbyMinPower,
-	unsigned char *pbyMaxPower
-)
-{
-	unsigned char byDec = 0;
-
-	*pbyMaxPower = pDevice->abyOFDMDefaultPwr[pDevice->byCurrentCh];
-	byDec = pDevice->abyOFDMPwrTbl[pDevice->byCurrentCh];
-	if (pDevice->byRFType == RF_UW2452) {
-		byDec *= 3;
-		byDec >>= 1;
-	} else {
-		byDec <<= 1;
-	}
-	*pbyMinPower = pDevice->abyOFDMDefaultPwr[pDevice->byCurrentCh] - byDec;
-}
-
-/*
- *
- * Description:
- *    Get Current Tx Power
- *
- * Parameters:
- *  In:
- *      hDeviceContext - device structure point
- *  Out:
- *      none
- *
- * Return Value: none.
- *
- */
-char
-CARDbyGetTransmitPower(
-	struct vnt_private *pDevice
-)
-{
-
-	return pDevice->byCurPwrdBm;
-}
-
-//xxx
 void
 CARDvSafeResetTx(
 	struct vnt_private *pDevice
@@ -1422,7 +512,7 @@ CARDvSafeResetTx(
 	unsigned int uu;
 	PSTxDesc    pCurrTD;
 
-	// initialize TD index
+	/* initialize TD index */
 	pDevice->apTailTD[0] = pDevice->apCurrTD[0] = &(pDevice->apTD0Rings[0]);
 	pDevice->apTailTD[1] = pDevice->apCurrTD[1] = &(pDevice->apTD1Rings[0]);
 
@@ -1432,28 +522,27 @@ CARDvSafeResetTx(
 	for (uu = 0; uu < pDevice->sOpts.nTxDescs[0]; uu++) {
 		pCurrTD = &(pDevice->apTD0Rings[uu]);
 		pCurrTD->m_td0TD0.f1Owner = OWNED_BY_HOST;
-		// init all Tx Packet pointer to NULL
+		/* init all Tx Packet pointer to NULL */
 	}
 	for (uu = 0; uu < pDevice->sOpts.nTxDescs[1]; uu++) {
 		pCurrTD = &(pDevice->apTD1Rings[uu]);
 		pCurrTD->m_td0TD0.f1Owner = OWNED_BY_HOST;
-		// init all Tx Packet pointer to NULL
+		/* init all Tx Packet pointer to NULL */
 	}
 
-	// set MAC TD pointer
+	/* set MAC TD pointer */
 	MACvSetCurrTXDescAddr(TYPE_TXDMA0, pDevice->PortOffset,
 			      (pDevice->td0_pool_dma));
 
 	MACvSetCurrTXDescAddr(TYPE_AC0DMA, pDevice->PortOffset,
 			      (pDevice->td1_pool_dma));
 
-	// set MAC Beacon TX pointer
+	/* set MAC Beacon TX pointer */
 	MACvSetCurrBCNTxDescAddr(pDevice->PortOffset,
 				 (pDevice->tx_beacon_dma));
 }
 
-/*+
- *
+/*
  * Description:
  *      Reset Rx
  *
@@ -1464,8 +553,7 @@ CARDvSafeResetTx(
  *      none
  *
  * Return Value: none
- *
- -*/
+ */
 void
 CARDvSafeResetRx(
 	struct vnt_private *pDevice
@@ -1474,11 +562,11 @@ CARDvSafeResetRx(
 	unsigned int uu;
 	PSRxDesc    pDesc;
 
-	// initialize RD index
+	/* initialize RD index */
 	pDevice->pCurrRD[0] = &(pDevice->aRD0Ring[0]);
 	pDevice->pCurrRD[1] = &(pDevice->aRD1Ring[0]);
 
-	// init state, all RD is chip's
+	/* init state, all RD is chip's */
 	for (uu = 0; uu < pDevice->sOpts.nRxDescs0; uu++) {
 		pDesc = &(pDevice->aRD0Ring[uu]);
 		pDesc->m_rd0RD0.wResCount = (unsigned short)(pDevice->rx_buf_sz);
@@ -1486,7 +574,7 @@ CARDvSafeResetRx(
 		pDesc->m_rd1RD1.wReqCount = (unsigned short)(pDevice->rx_buf_sz);
 	}
 
-	// init state, all RD is chip's
+	/* init state, all RD is chip's */
 	for (uu = 0; uu < pDevice->sOpts.nRxDescs1; uu++) {
 		pDesc = &(pDevice->aRD1Ring[uu]);
 		pDesc->m_rd0RD0.wResCount = (unsigned short)(pDevice->rx_buf_sz);
@@ -1494,13 +582,10 @@ CARDvSafeResetRx(
 		pDesc->m_rd1RD1.wReqCount = (unsigned short)(pDevice->rx_buf_sz);
 	}
 
-	pDevice->cbDFCB = CB_MAX_RX_FRAG;
-	pDevice->cbFreeDFCB = pDevice->cbDFCB;
-
-	// set perPkt mode
+	/* set perPkt mode */
 	MACvRx0PerPktMode(pDevice->PortOffset);
 	MACvRx1PerPktMode(pDevice->PortOffset);
-	// set MAC RD pointer
+	/* set MAC RD pointer */
 	MACvSetCurrRx0DescAddr(pDevice->PortOffset,
 			       pDevice->rd0_pool_dma);
 
@@ -1519,7 +604,6 @@ CARDvSafeResetRx(
  *      none
  *
  * Return Value: response Control frame rate
- *
  */
 static unsigned short CARDwGetCCKControlRate(struct vnt_private *pDevice,
 					     unsigned short wRateIdx)
@@ -1527,7 +611,7 @@ static unsigned short CARDwGetCCKControlRate(struct vnt_private *pDevice,
 	unsigned int ui = (unsigned int) wRateIdx;
 
 	while (ui > RATE_1M) {
-		if (pDevice->wBasicRate & ((unsigned short)1 << ui))
+		if (pDevice->basic_rates & ((u32)0x1 << ui))
 			return (unsigned short)ui;
 
 		ui--;
@@ -1546,14 +630,13 @@ static unsigned short CARDwGetCCKControlRate(struct vnt_private *pDevice,
  *      none
  *
  * Return Value: response Control frame rate
- *
  */
 static unsigned short CARDwGetOFDMControlRate(struct vnt_private *pDevice,
 					      unsigned short wRateIdx)
 {
 	unsigned int ui = (unsigned int) wRateIdx;
 
-	pr_debug("BASIC RATE: %X\n", pDevice->wBasicRate);
+	pr_debug("BASIC RATE: %X\n", pDevice->basic_rates);
 
 	if (!CARDbIsOFDMinBasicRate((void *)pDevice)) {
 		pr_debug("CARDwGetOFDMControlRate:(NO OFDM) %d\n", wRateIdx);
@@ -1562,7 +645,7 @@ static unsigned short CARDwGetOFDMControlRate(struct vnt_private *pDevice,
 		return wRateIdx;
 	}
 	while (ui > RATE_11M) {
-		if (pDevice->wBasicRate & ((unsigned short)1 << ui)) {
+		if (pDevice->basic_rates & ((u32)0x1 << ui)) {
 			pr_debug("CARDwGetOFDMControlRate : %d\n", ui);
 			return (unsigned short)ui;
 		}
@@ -1582,14 +665,13 @@ static unsigned short CARDwGetOFDMControlRate(struct vnt_private *pDevice,
  *      none
  *
  * Return Value: None.
- *
  */
-void CARDvSetRSPINF(struct vnt_private *pDevice, CARD_PHY_TYPE ePHYType)
+void CARDvSetRSPINF(struct vnt_private *pDevice, u8 bb_type)
 {
 	union vnt_phy_field_swap phy;
-	unsigned char byTxRate, byRsvTime;             //For OFDM
+	unsigned char byTxRate, byRsvTime;      /* For OFDM */
 
-	//Set to Page1
+	/* Set to Page1 */
 	MACvSelectPage1(pDevice->PortOffset);
 
 	/* RSPINF_b_1 */
@@ -1629,136 +711,72 @@ void CARDvSetRSPINF(struct vnt_private *pDevice, CARD_PHY_TYPE ePHYType)
 
 	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_11, phy.field_write);
 
-	//RSPINF_a_6
+	/* RSPINF_a_6 */
 	s_vCalculateOFDMRParameter(RATE_6M,
-				   ePHYType,
+				   bb_type,
 				   &byTxRate,
 				   &byRsvTime);
 	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_6, MAKEWORD(byTxRate, byRsvTime));
-	//RSPINF_a_9
+	/* RSPINF_a_9 */
 	s_vCalculateOFDMRParameter(RATE_9M,
-				   ePHYType,
+				   bb_type,
 				   &byTxRate,
 				   &byRsvTime);
 	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_9, MAKEWORD(byTxRate, byRsvTime));
-	//RSPINF_a_12
+	/* RSPINF_a_12 */
 	s_vCalculateOFDMRParameter(RATE_12M,
-				   ePHYType,
+				   bb_type,
 				   &byTxRate,
 				   &byRsvTime);
 	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_12, MAKEWORD(byTxRate, byRsvTime));
-	//RSPINF_a_18
+	/* RSPINF_a_18 */
 	s_vCalculateOFDMRParameter(RATE_18M,
-				   ePHYType,
+				   bb_type,
 				   &byTxRate,
 				   &byRsvTime);
 	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_18, MAKEWORD(byTxRate, byRsvTime));
-	//RSPINF_a_24
+	/* RSPINF_a_24 */
 	s_vCalculateOFDMRParameter(RATE_24M,
-				   ePHYType,
+				   bb_type,
 				   &byTxRate,
 				   &byRsvTime);
 	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_24, MAKEWORD(byTxRate, byRsvTime));
-	//RSPINF_a_36
+	/* RSPINF_a_36 */
 	s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_36M),
-				   ePHYType,
+				   bb_type,
 				   &byTxRate,
 				   &byRsvTime);
 	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_36, MAKEWORD(byTxRate, byRsvTime));
-	//RSPINF_a_48
+	/* RSPINF_a_48 */
 	s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_48M),
-				   ePHYType,
+				   bb_type,
 				   &byTxRate,
 				   &byRsvTime);
 	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_48, MAKEWORD(byTxRate, byRsvTime));
-	//RSPINF_a_54
+	/* RSPINF_a_54 */
 	s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_54M),
-				   ePHYType,
+				   bb_type,
 				   &byTxRate,
 				   &byRsvTime);
 	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_54, MAKEWORD(byTxRate, byRsvTime));
-
-	//RSPINF_a_72
+	/* RSPINF_a_72 */
 	s_vCalculateOFDMRParameter(CARDwGetOFDMControlRate((void *)pDevice, RATE_54M),
-				   ePHYType,
+				   bb_type,
 				   &byTxRate,
 				   &byRsvTime);
 	VNSvOutPortW(pDevice->PortOffset + MAC_REG_RSPINF_A_72, MAKEWORD(byTxRate, byRsvTime));
-	//Set to Page0
+	/* Set to Page0 */
 	MACvSelectPage0(pDevice->PortOffset);
 }
 
-/*
- * Description: Update IFS
- *
- * Parameters:
- *  In:
- *      pDevice             - The adapter to be set
- *  Out:
- *      none
- *
- * Return Value: None.
- *
- */
-void vUpdateIFS(struct vnt_private *pDevice)
-{
-	/* Set SIFS, DIFS, EIFS, SlotTime, CwMin */
-
-	unsigned char byMaxMin = 0;
-
-	if (pDevice->byPacketType == PK_TYPE_11A) {//0000 0000 0000 0000,11a
-		pDevice->uSlot = C_SLOT_SHORT;
-		pDevice->uSIFS = C_SIFS_A;
-		pDevice->uDIFS = C_SIFS_A + 2*C_SLOT_SHORT;
-		pDevice->uCwMin = C_CWMIN_A;
-		byMaxMin = 4;
-	} else if (pDevice->byPacketType == PK_TYPE_11B) {//0000 0001 0000 0000,11b
-		pDevice->uSlot = C_SLOT_LONG;
-		pDevice->uSIFS = C_SIFS_BG;
-		pDevice->uDIFS = C_SIFS_BG + 2*C_SLOT_LONG;
-		pDevice->uCwMin = C_CWMIN_B;
-		byMaxMin = 5;
-	} else { // PK_TYPE_11GA & PK_TYPE_11GB
-		pDevice->uSIFS = C_SIFS_BG;
-		if (pDevice->bShortSlotTime)
-			pDevice->uSlot = C_SLOT_SHORT;
-		else
-			pDevice->uSlot = C_SLOT_LONG;
-
-		pDevice->uDIFS = C_SIFS_BG + 2*pDevice->uSlot;
-		if (pDevice->wBasicRate & 0x0150) { //0000 0001 0101 0000,24M,12M,6M
-			pDevice->uCwMin = C_CWMIN_A;
-			byMaxMin = 4;
-		} else {
-			pDevice->uCwMin = C_CWMIN_B;
-			byMaxMin = 5;
-		}
-	}
-
-	pDevice->uCwMax = C_CWMAX;
-	pDevice->uEIFS = C_EIFS;
-	if (pDevice->byRFType == RF_RFMD2959) {
-		// bcs TX_PE will reserve 3 us
-		VNSvOutPortB(pDevice->PortOffset + MAC_REG_SIFS, (unsigned char)(pDevice->uSIFS - 3));
-		VNSvOutPortB(pDevice->PortOffset + MAC_REG_DIFS, (unsigned char)(pDevice->uDIFS - 3));
-	} else {
-		VNSvOutPortB(pDevice->PortOffset + MAC_REG_SIFS, (unsigned char)pDevice->uSIFS);
-		VNSvOutPortB(pDevice->PortOffset + MAC_REG_DIFS, (unsigned char)pDevice->uDIFS);
-	}
-	VNSvOutPortB(pDevice->PortOffset + MAC_REG_EIFS, (unsigned char)pDevice->uEIFS);
-	VNSvOutPortB(pDevice->PortOffset + MAC_REG_SLOT, (unsigned char)pDevice->uSlot);
-	byMaxMin |= 0xA0;//1010 1111,C_CWMAX = 1023
-	VNSvOutPortB(pDevice->PortOffset + MAC_REG_CWMAXMIN0, (unsigned char)byMaxMin);
-}
-
 void CARDvUpdateBasicTopRate(struct vnt_private *pDevice)
 {
 	unsigned char byTopOFDM = RATE_24M, byTopCCK = RATE_1M;
 	unsigned char ii;
 
-	//Determines the highest basic rate.
+	/* Determines the highest basic rate. */
 	for (ii = RATE_54M; ii >= RATE_6M; ii--) {
-		if ((pDevice->wBasicRate) & ((unsigned short)(1<<ii))) {
+		if ((pDevice->basic_rates) & ((u32)(1 << ii))) {
 			byTopOFDM = ii;
 			break;
 		}
@@ -1766,7 +784,7 @@ void CARDvUpdateBasicTopRate(struct vnt_private *pDevice)
 	pDevice->byTopOFDMBasicRate = byTopOFDM;
 
 	for (ii = RATE_11M;; ii--) {
-		if ((pDevice->wBasicRate) & ((unsigned short)(1<<ii))) {
+		if ((pDevice->basic_rates) & ((u32)(1 << ii))) {
 			byTopCCK = ii;
 			break;
 		}
@@ -1776,24 +794,12 @@ void CARDvUpdateBasicTopRate(struct vnt_private *pDevice)
 	pDevice->byTopCCKBasicRate = byTopCCK;
 }
 
-bool CARDbAddBasicRate(struct vnt_private *pDevice, unsigned short wRateIdx)
-{
-	unsigned short wRate = (unsigned short)(1<<wRateIdx);
-
-	pDevice->wBasicRate |= wRate;
-
-	//Determines the highest basic rate.
-	CARDvUpdateBasicTopRate((void *)pDevice);
-
-	return true;
-}
-
 bool CARDbIsOFDMinBasicRate(struct vnt_private *pDevice)
 {
 	int ii;
 
 	for (ii = RATE_54M; ii >= RATE_6M; ii--) {
-		if ((pDevice->wBasicRate) & ((unsigned short)(1 << ii)))
+		if ((pDevice->basic_rates) & ((u32)(1 << ii)))
 			return true;
 	}
 	return false;
@@ -1821,10 +827,11 @@ unsigned char CARDbyGetPktType(struct vnt_private *pDevice)
  *      none
  *
  * Return Value: none
- *
  */
-void CARDvSetLoopbackMode(void __iomem *dwIoBase, unsigned short wLoopbackMode)
+void CARDvSetLoopbackMode(struct vnt_private *priv, unsigned short wLoopbackMode)
 {
+	void __iomem *dwIoBase = priv->PortOffset;
+
 	switch (wLoopbackMode) {
 	case CARD_LB_NONE:
 	case CARD_LB_MAC:
@@ -1834,9 +841,9 @@ void CARDvSetLoopbackMode(void __iomem *dwIoBase, unsigned short wLoopbackMode)
 		ASSERT(false);
 		break;
 	}
-	// set MAC loopback
+	/* set MAC loopback */
 	MACvSetLoopbackMode(dwIoBase, LOBYTE(wLoopbackMode));
-	// set Baseband loopback
+	/* set Baseband loopback */
 }
 
 /*
@@ -1849,12 +856,11 @@ void CARDvSetLoopbackMode(void __iomem *dwIoBase, unsigned short wLoopbackMode)
  *      none
  *
  * Return Value: none
- *
  */
 bool CARDbSoftwareReset(struct vnt_private *pDevice)
 {
 
-	// reset MAC
+	/* reset MAC */
 	if (!MACbSafeSoftwareReset(pDevice->PortOffset))
 		return false;
 
@@ -1874,7 +880,6 @@ bool CARDbSoftwareReset(struct vnt_private *pDevice)
  *      none
  *
  * Return Value: TSF Offset value
- *
  */
 u64 CARDqGetTSFOffset(unsigned char byRxRate, u64 qwTSF1, u64 qwTSF2)
 {
@@ -1901,10 +906,10 @@ u64 CARDqGetTSFOffset(unsigned char byRxRate, u64 qwTSF1, u64 qwTSF2)
  *      qwCurrTSF       - Current TSF counter
  *
  * Return Value: true if success; otherwise false
- *
  */
-bool CARDbGetCurrentTSF(void __iomem *dwIoBase, u64 *pqwCurrTSF)
+bool CARDbGetCurrentTSF(struct vnt_private *priv, u64 *pqwCurrTSF)
 {
+	void __iomem *dwIoBase = priv->PortOffset;
 	unsigned short ww;
 	unsigned char byData;
 
@@ -1934,17 +939,12 @@ bool CARDbGetCurrentTSF(void __iomem *dwIoBase, u64 *pqwCurrTSF)
  *      qwCurrTSF       - Current TSF counter
  *
  * Return Value: TSF value of next Beacon
- *
  */
 u64 CARDqGetNextTBTT(u64 qwTSF, unsigned short wBeaconInterval)
 {
 	u32 beacon_int;
 
 	beacon_int = wBeaconInterval * 1024;
-
-	/* Next TBTT =
-	*	((local_current_TSF / beacon_interval) + 1) * beacon_interval
-	*/
 	if (beacon_int) {
 		do_div(qwTSF, beacon_int);
 		qwTSF += 1;
@@ -1966,16 +966,16 @@ u64 CARDqGetNextTBTT(u64 qwTSF, unsigned short wBeaconInterval)
  *      none
  *
  * Return Value: none
- *
  */
-void CARDvSetFirstNextTBTT(void __iomem *dwIoBase, unsigned short wBeaconInterval)
+void CARDvSetFirstNextTBTT(struct vnt_private *priv, unsigned short wBeaconInterval)
 {
+	void __iomem *dwIoBase = priv->PortOffset;
 	u64 qwNextTBTT = 0;
 
-	CARDbGetCurrentTSF(dwIoBase, &qwNextTBTT); //Get Local TSF counter
+	CARDbGetCurrentTSF(priv, &qwNextTBTT); /* Get Local TSF counter */
 
 	qwNextTBTT = CARDqGetNextTBTT(qwNextTBTT, wBeaconInterval);
-	// Set NextTBTT
+	/* Set NextTBTT */
 	VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT, (u32)qwNextTBTT);
 	VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT + 4, (u32)(qwNextTBTT >> 32));
 	MACvRegBitsOn(dwIoBase, MAC_REG_TFTCTL, TFTCTL_TBTTSYNCEN);
@@ -1994,12 +994,13 @@ void CARDvSetFirstNextTBTT(void __iomem *dwIoBase, unsigned short wBeaconInterva
  *      none
  *
  * Return Value: none
- *
  */
-void CARDvUpdateNextTBTT(void __iomem *dwIoBase, u64 qwTSF, unsigned short wBeaconInterval)
+void CARDvUpdateNextTBTT(struct vnt_private *priv, u64 qwTSF, unsigned short wBeaconInterval)
 {
+	void __iomem *dwIoBase = priv->PortOffset;
+
 	qwTSF = CARDqGetNextTBTT(qwTSF, wBeaconInterval);
-	// Set NextTBTT
+	/* Set NextTBTT */
 	VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT, (u32)qwTSF);
 	VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT + 4, (u32)(qwTSF >> 32));
 	MACvRegBitsOn(dwIoBase, MAC_REG_TFTCTL, TFTCTL_TBTTSYNCEN);
diff --git a/drivers/staging/vt6655/card.h b/drivers/staging/vt6655/card.h
index 96f5b6c46e82..2dfc41952271 100644
--- a/drivers/staging/vt6655/card.h
+++ b/drivers/staging/vt6655/card.h
@@ -29,35 +29,28 @@
 #ifndef __CARD_H__
 #define __CARD_H__
 
-#include "ttype.h"
 #include <linux/types.h>
 #include <linux/nl80211.h>
 
-//
-// Loopback mode
-//
-// LOBYTE is MAC LB mode, HIBYTE is MII LB mode
+/*
+ * Loopback mode
+ *
+ * LOBYTE is MAC LB mode, HIBYTE is MII LB mode
+ */
 #define CARD_LB_NONE            MAKEWORD(MAC_LB_NONE, 0)
-#define CARD_LB_MAC             MAKEWORD(MAC_LB_INTERNAL, 0)   // PHY must ISO, avoid MAC loopback packet go out
+#define CARD_LB_MAC             MAKEWORD(MAC_LB_INTERNAL, 0)   /* PHY must ISO, avoid MAC loopback packet go out */
 #define CARD_LB_PHY             MAKEWORD(MAC_LB_EXT, 0)
 
-#define DEFAULT_MSDU_LIFETIME           512  // ms
-#define DEFAULT_MSDU_LIFETIME_RES_64us  8000 // 64us
+#define DEFAULT_MSDU_LIFETIME           512  /* ms */
+#define DEFAULT_MSDU_LIFETIME_RES_64us  8000 /* 64us */
 
-#define DEFAULT_MGN_LIFETIME            8    // ms
-#define DEFAULT_MGN_LIFETIME_RES_64us   125  // 64us
+#define DEFAULT_MGN_LIFETIME            8    /* ms */
+#define DEFAULT_MGN_LIFETIME_RES_64us   125  /* 64us */
 
 #define CB_MAX_CHANNEL_24G      14
 #define CB_MAX_CHANNEL_5G       42
 #define CB_MAX_CHANNEL          (CB_MAX_CHANNEL_24G+CB_MAX_CHANNEL_5G)
 
-typedef enum _CARD_PHY_TYPE {
-	PHY_TYPE_AUTO,
-	PHY_TYPE_11B,
-	PHY_TYPE_11G,
-	PHY_TYPE_11A
-} CARD_PHY_TYPE, *PCARD_PHY_TYPE;
-
 typedef enum _CARD_PKT_TYPE {
 	PKT_TYPE_802_11_BCN,
 	PKT_TYPE_802_11_MNG,
@@ -73,103 +66,24 @@ typedef enum _CARD_STATUS_TYPE {
 
 struct vnt_private;
 
-void CARDvSetRSPINF(struct vnt_private *, CARD_PHY_TYPE ePHYType);
-void vUpdateIFS(struct vnt_private *);
+void CARDvSetRSPINF(struct vnt_private *, u8);
 void CARDvUpdateBasicTopRate(struct vnt_private *);
-bool CARDbAddBasicRate(struct vnt_private *, unsigned short wRateIdx);
 bool CARDbIsOFDMinBasicRate(struct vnt_private *);
-void CARDvSetLoopbackMode(void __iomem *dwIoBase, unsigned short wLoopbackMode);
+void CARDvSetLoopbackMode(struct vnt_private *, unsigned short wLoopbackMode);
 bool CARDbSoftwareReset(struct vnt_private *);
-void CARDvSetFirstNextTBTT(void __iomem *dwIoBase, unsigned short wBeaconInterval);
-void CARDvUpdateNextTBTT(void __iomem *dwIoBase, u64 qwTSF, unsigned short wBeaconInterval);
-bool CARDbGetCurrentTSF(void __iomem *dwIoBase, u64 *pqwCurrTSF);
+void CARDvSetFirstNextTBTT(struct vnt_private *, unsigned short wBeaconInterval);
+void CARDvUpdateNextTBTT(struct vnt_private *, u64 qwTSF, unsigned short wBeaconInterval);
+bool CARDbGetCurrentTSF(struct vnt_private *, u64 *pqwCurrTSF);
 u64 CARDqGetNextTBTT(u64 qwTSF, unsigned short wBeaconInterval);
 u64 CARDqGetTSFOffset(unsigned char byRxRate, u64 qwTSF1, u64 qwTSF2);
-bool CARDbSetTxPower(struct vnt_private *, unsigned long ulTxPower);
 unsigned char CARDbyGetPktType(struct vnt_private *);
 void CARDvSafeResetTx(struct vnt_private *);
 void CARDvSafeResetRx(struct vnt_private *);
 bool CARDbRadioPowerOff(struct vnt_private *);
 bool CARDbRadioPowerOn(struct vnt_private *);
-bool CARDbIsShortPreamble(struct vnt_private *);
-bool CARDbIsShorSlotTime(struct vnt_private *);
-bool CARDbSetPhyParameter(struct vnt_private *, CARD_PHY_TYPE ePHYType,
-			  unsigned short wCapInfo, unsigned char byERPField,
-			  void *pvSupportRateIEs, void *pvExtSupportRateIEs);
+bool CARDbSetPhyParameter(struct vnt_private *, u8);
 bool CARDbUpdateTSF(struct vnt_private *, unsigned char byRxRate,
 		    u64 qwBSSTimestamp, u64 qwLocalTSF);
-bool CARDbStopTxPacket(struct vnt_private *, CARD_PKT_TYPE ePktType);
-bool CARDbStartTxPacket(struct vnt_private *, CARD_PKT_TYPE ePktType);
 bool CARDbSetBeaconPeriod(struct vnt_private *, unsigned short wBeaconInterval);
-bool CARDbSetBSSID(struct vnt_private *,
-		   unsigned char *pbyBSSID, enum nl80211_iftype);
-
-bool CARDbPowerDown(struct vnt_private *);
-
-bool CARDbSetTxDataRate(struct vnt_private *, unsigned short wDataRate);
-
-bool CARDbRemoveKey(struct vnt_private *, unsigned char *pbyBSSID);
-
-bool
-CARDbAdd_PMKID_Candidate(
-	struct vnt_private *,
-	unsigned char *pbyBSSID,
-	bool bRSNCapExist,
-	unsigned short wRSNCap
-);
-
-void *
-CARDpGetCurrentAddress(
-	struct vnt_private *
-);
-
-bool
-CARDbStartMeasure(
-	struct vnt_private *,
-	void *pvMeasureEIDs,
-	unsigned int uNumOfMeasureEIDs
-);
-
-bool
-CARDbChannelSwitch(
-	struct vnt_private *,
-	unsigned char byMode,
-	unsigned char byNewChannel,
-	unsigned char byCount
-);
-
-bool
-CARDbSetQuiet(
-	struct vnt_private *,
-	bool bResetQuiet,
-	unsigned char byQuietCount,
-	unsigned char byQuietPeriod,
-	unsigned short wQuietDuration,
-	unsigned short wQuietOffset
-);
-
-bool
-CARDbStartQuiet(
-	struct vnt_private *
-);
-
-void
-CARDvSetPowerConstraint(
-	struct vnt_private *,
-	unsigned char byChannel,
-	char byPower
-);
-
-void
-CARDvGetPowerCapability(
-	struct vnt_private *,
-	unsigned char *pbyMinPower,
-	unsigned char *pbyMaxPower
-);
-
-char
-CARDbyGetTransmitPower(
-	struct vnt_private *
-);
 
-#endif // __CARD_H__
+#endif /* __CARD_H__ */
diff --git a/drivers/staging/vt6655/channel.c b/drivers/staging/vt6655/channel.c
index 4ce964ba14b7..c8f739dd346e 100644
--- a/drivers/staging/vt6655/channel.c
+++ b/drivers/staging/vt6655/channel.c
@@ -21,494 +21,148 @@
  */
 
 #include "baseband.h"
-#include "country.h"
 #include "channel.h"
 #include "device.h"
 #include "rf.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
-#define CARD_MAX_CHANNEL_TBL    56
-
-/*---------------------  Static Variables  --------------------------*/
-
-static SChannelTblElement sChannelTbl[CARD_MAX_CHANNEL_TBL + 1] =
-{
-	{0,   0,    false,    0},
-	{1,   2412, true,     0},
-	{2,   2417, true,     0},
-	{3,   2422, true,     0},
-	{4,   2427, true,     0},
-	{5,   2432, true,     0},
-	{6,   2437, true,     0},
-	{7,   2442, true,     0},
-	{8,   2447, true,     0},
-	{9,   2452, true,     0},
-	{10,  2457, true,     0},
-	{11,  2462, true,     0},
-	{12,  2467, true,     0},
-	{13,  2472, true,     0},
-	{14,  2484, true,     0},
-	{183, 4915, true,     0},
-	{184, 4920, true,     0},
-	{185, 4925, true,     0},
-	{187, 4935, true,     0},
-	{188, 4940, true,     0},
-	{189, 4945, true,     0},
-	{192, 4960, true,     0},
-	{196, 4980, true,     0},
-	{7,   5035, true,     0},
-	{8,   5040, true,     0},
-	{9,   5045, true,     0},
-	{11,  5055, true,     0},
-	{12,  5060, true,     0},
-	{16,  5080, true,     0},
-	{34,  5170, true,     0},
-	{36,  5180, true,     0},
-	{38,  5190, true,     0},
-	{40,  5200, true,     0},
-	{42,  5210, true,     0},
-	{44,  5220, true,     0},
-	{46,  5230, true,     0},
-	{48,  5240, true,     0},
-	{52,  5260, true,     0},
-	{56,  5280, true,     0},
-	{60,  5300, true,     0},
-	{64,  5320, true,     0},
-	{100, 5500, true,     0},
-	{104, 5520, true,     0},
-	{108, 5540, true,     0},
-	{112, 5560, true,     0},
-	{116, 5580, true,     0},
-	{120, 5600, true,     0},
-	{124, 5620, true,     0},
-	{128, 5640, true,     0},
-	{132, 5660, true,     0},
-	{136, 5680, true,     0},
-	{140, 5700, true,     0},
-	{149, 5745, true,     0},
-	{153, 5765, true,     0},
-	{157, 5785, true,     0},
-	{161, 5805, true,     0},
-	{165, 5825, true,     0}
+static struct ieee80211_rate vnt_rates_bg[] = {
+	{ .bitrate = 10,  .hw_value = RATE_1M },
+	{ .bitrate = 20,  .hw_value = RATE_2M },
+	{ .bitrate = 55,  .hw_value = RATE_5M },
+	{ .bitrate = 110, .hw_value = RATE_11M },
+	{ .bitrate = 60,  .hw_value = RATE_6M },
+	{ .bitrate = 90,  .hw_value = RATE_9M },
+	{ .bitrate = 120, .hw_value = RATE_12M },
+	{ .bitrate = 180, .hw_value = RATE_18M },
+	{ .bitrate = 240, .hw_value = RATE_24M },
+	{ .bitrate = 360, .hw_value = RATE_36M },
+	{ .bitrate = 480, .hw_value = RATE_48M },
+	{ .bitrate = 540, .hw_value = RATE_54M },
 };
 
-/************************************************************************
- * The Radar regulation rules for each country
- ************************************************************************/
-static struct
-{
-	unsigned char byChannelCountryCode;             /* The country code         */
-	char chCountryCode[2];
-	unsigned char bChannelIdxList[CB_MAX_CHANNEL];  /* Available channels Index */
-	unsigned char byPower[CB_MAX_CHANNEL];
-} ChannelRuleTab[] =
-{
-/************************************************************************
- * This table is based on Athero driver rules
- ************************************************************************/
-/* Country          Available channels, ended with 0                    */
-/*                                              1   2   3   4   5   6   7   8   9  10  11  12  13  14  15  16  17  18  19  20  21  22  23  24  25  26  27  28  29  30  31  32  33  34  35  36  37  38  39  40  41  42  43  44  45  46  47  48  49  50  51  52  53  54  55  56  */
-	{CCODE_FCC,                     {'U' , 'S'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-	{CCODE_TELEC,                   {'J' , 'P'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  1,  0,  0,  1,  0,  1,  1,  0,  1,  0,  0,  1,  1,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0, 23,  0,  0, 23,  0, 23, 23,  0, 23,  0,  0, 23, 23, 23,  0, 23,  0, 23,  0, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_ETSI,                    {'E' , 'U'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-	{CCODE_RESV3,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_RESV4,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_RESV5,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_RESV6,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_RESV7,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_RESV8,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_RESV9,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_RESVa,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_RESVb,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_RESVc,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_RESVd,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_RESVe,                   {' ' , ' '},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_ALLBAND,                 {' ' , ' '},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_ALBANIA,                 {'A' , 'L'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_ALGERIA,                 {'D' , 'Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_ARGENTINA,               {'A' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17, 17, 17,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30,  0}  },
-	{CCODE_ARMENIA,                 {'A' , 'M'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18, 18, 18, 18, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_AUSTRALIA,               {'A' , 'U'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23,  0, 23,  0, 23,  0, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-	{CCODE_AUSTRIA,                 {'A' , 'T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 15,  0, 15,  0, 15,  0, 15,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_AZERBAIJAN,              {'A' , 'Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18, 18, 18, 18, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_BAHRAIN,                 {'B' , 'H'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_BELARUS,                 {'B' , 'Y'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_BELGIUM,                 {'B' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_BELIZE,                  {'B' , 'Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-	{CCODE_BOLIVIA,                 {'B' , 'O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-	{CCODE_BRAZIL,                  {'B' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_BRUNEI_DARUSSALAM,       {'B' , 'N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-	{CCODE_BULGARIA,                {'B' , 'G'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  0,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23,  0, 23,  0, 23,  0, 23, 23, 23,  0,  0, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30,  0,  0,  0,  0,  0}  },
-	{CCODE_CANADA,                  {'C' , 'A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-	{CCODE_CHILE,                   {'C' , 'L'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17, 17, 17, 17, 17}  },
-	{CCODE_CHINA,                   {'C' , 'N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-	{CCODE_COLOMBIA,                {'C' , 'O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-	{CCODE_COSTA_RICA,              {'C' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_CROATIA,                 {'H' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_CYPRUS,                  {'C' , 'Y'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-	{CCODE_CZECH,                   {'C' , 'Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_DENMARK,                 {'D' , 'K'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-	{CCODE_DOMINICAN_REPUBLIC,      {'D' , 'O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-	{CCODE_ECUADOR,                 {'E' , 'C'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_EGYPT,                   {'E' , 'G'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_EL_SALVADOR,             {'S' , 'V'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_ESTONIA,                 {'E' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-	{CCODE_FINLAND,                 {'F' , 'I'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-	{CCODE_FRANCE,                  {'F' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_GERMANY,                 {'D' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-	{CCODE_GREECE,                  {'G' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_GEORGIA,                 {'G' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18, 18, 18, 18, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_GUATEMALA,               {'G' , 'T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-	{CCODE_HONDURAS,                {'H' , 'N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_HONG_KONG,               {'H' , 'K'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23,  0, 23,  0, 23,  0, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-	{CCODE_HUNGARY,                 {'H' , 'U'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_ICELAND,                 {'I' , 'S'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-	{CCODE_INDIA,                   {'I' , 'N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_INDONESIA,               {'I' , 'D'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_IRAN,                    {'I' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-	{CCODE_IRELAND,                 {'I' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-	{CCODE_ITALY,                   {'I' , 'T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-	{CCODE_ISRAEL,                  {'I' , 'L'},  {   0,  0,  0,  0,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_JAPAN,                   {'J' , 'P'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23,  0, 23,  0, 23,  0, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_JORDAN,                  {'J' , 'O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_KAZAKHSTAN,              {'K' , 'Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_KUWAIT,                  {'K' , 'W'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_LATVIA,                  {'L' , 'V'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_LEBANON,                 {'L' , 'B'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_LEICHTENSTEIN,           {'L' , 'I'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_LITHUANIA,               {'L' , 'T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-	{CCODE_LUXEMBURG,               {'L' , 'U'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-	{CCODE_MACAU,                   {'M' , 'O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23,  0, 23,  0, 23,  0, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-	{CCODE_MACEDONIA,               {'M' , 'K'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_MALTA,                   {'M' , 'T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-	 ,  {  23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 16,  0, 16,  0, 16,  0, 16, 16, 16, 16, 16,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 16, 16, 16, 16,  0}  },
-	{CCODE_MALAYSIA,                {'M' , 'Y'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_MEXICO,                  {'M' , 'X'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-	{CCODE_MONACO,                  {'M' , 'C'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18, 18, 18, 18, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_MOROCCO,                 {'M' , 'A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_NETHERLANDS,             {'N' , 'L'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-	{CCODE_NEW_ZEALAND,             {'N' , 'Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23,  0, 23,  0, 23,  0, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-	{CCODE_NORTH_KOREA,             {'K' , 'P'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23, 23, 23, 23,  0}  },
-	{CCODE_NORWAY,                  {'N' , 'O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-	{CCODE_OMAN,                    {'O' , 'M'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_PAKISTAN,                {'P' , 'K'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_PANAMA,                  {'P' , 'A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-	{CCODE_PERU,                    {'P' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_PHILIPPINES,             {'P' , 'H'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-	{CCODE_POLAND,                  {'P' , 'L'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-	{CCODE_PORTUGAL,                {'P' , 'T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-	{CCODE_PUERTO_RICO,             {'P' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-	{CCODE_QATAR,                   {'Q' , 'A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_ROMANIA,                 {'R' , 'O'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_RUSSIA,                  {'R' , 'U'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_SAUDI_ARABIA,            {'S' , 'A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_SINGAPORE,               {'S' , 'G'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20, 20, 20, 20, 20}  },
-	{CCODE_SLOVAKIA,                {'S' , 'K'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-	 ,  {  23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 16,  0, 16,  0, 16,  0, 16, 16, 16, 16, 16,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 16, 16, 16, 16,  0}  },
-	{CCODE_SLOVENIA,                {'S' , 'I'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-	{CCODE_SOUTH_AFRICA,            {'Z' , 'A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-	{CCODE_SOUTH_KOREA,             {'K' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23, 23, 23, 23,  0}  },
-	{CCODE_SPAIN,                   {'E' , 'S'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-	 ,  {  23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 16,  0, 16,  0, 16,  0, 16, 16, 16, 16, 16,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 16, 16, 16, 16,  0}  },
-	{CCODE_SWEDEN,                  {'S' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-	{CCODE_SWITZERLAND,             {'C' , 'H'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_SYRIA,                   {'S' , 'Y'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_TAIWAN,                  {'T' , 'W'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17, 17, 17,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30,  0}  },
-	{CCODE_THAILAND,                {'T' , 'H'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23, 23, 23, 23,  0}  },
-	{CCODE_TRINIDAD_TOBAGO,         {'T' , 'T'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 18,  0, 18,  0, 18,  0, 18, 18, 18, 18, 18,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_TUNISIA,                 {'T' , 'N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_TURKEY,                  {'T' , 'R'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_UK,                      {'G' , 'B'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 20,  0, 20,  0, 20,  0, 20, 20, 20, 20, 20, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0}  },
-	{CCODE_UKRAINE,                 {'U' , 'A'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_UNITED_ARAB_EMIRATES,    {'A' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_UNITED_STATES,           {'U' , 'S'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  1}
-	 ,  {  27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 17,  0, 17,  0, 17,  0, 17, 23, 23, 23, 23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 30, 30, 30, 30, 30}  },
-	{CCODE_URUGUAY,                 {'U' , 'Y'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23, 23, 23, 23,  0}  },
-	{CCODE_UZBEKISTAN,              {'U' , 'Z'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_VENEZUELA,               {'V' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,  0}
-	 ,  {  20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 23, 23, 23, 23,  0}  },
-	{CCODE_VIETNAM,                 {'V' , 'N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_YEMEN,                   {'Y' , 'E'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_ZIMBABWE,                {'Z' , 'W'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_JAPAN_W52_W53,           {'J' , 'J'},  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  1,  0,  1,  0,  1,  1,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  },
-	{CCODE_MAX,                     {'U' , 'N'},  {   1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1}
-	 ,  {   0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}  }
-/*                                              1   2   3   4   5   6   7   8   9  10  11  12  13  14  15  16  17  18  19  20  21  22  23  24  25  26  27  28  29  30  31  32  33  34  35  36  37  38  39  40  41  42  43  44  45  46  47  48  49  50  51  52  53  54  55  56  */
+static struct ieee80211_rate vnt_rates_a[] = {
+	{ .bitrate = 60,  .hw_value = RATE_6M },
+	{ .bitrate = 90,  .hw_value = RATE_9M },
+	{ .bitrate = 120, .hw_value = RATE_12M },
+	{ .bitrate = 180, .hw_value = RATE_18M },
+	{ .bitrate = 240, .hw_value = RATE_24M },
+	{ .bitrate = 360, .hw_value = RATE_36M },
+	{ .bitrate = 480, .hw_value = RATE_48M },
+	{ .bitrate = 540, .hw_value = RATE_54M },
 };
 
-/*---------------------  Export Functions  --------------------------*/
-
-/**
- * is_channel_valid() - Is Country Channel Valid
- *  @ChanneIndex: defined as VT3253 MAC channel:
- *              1   = 2.4G channel 1
- *              2   = 2.4G channel 2
- *              ...
- *              14  = 2.4G channel 14
- *              15  = 4.9G channel 183
- *              16  = 4.9G channel 184
- *              .....
- *  Output: true if the specified 5GHz band is allowed to be used,
- *          false otherwise.
- * 4.9G => Ch 183, 184, 185, 187, 188, 189, 192, 196 (Value:15 ~ 22)
- *
- * 5G => Ch 7, 8, 9, 11, 12, 16, 34, 36, 38, 40, 42, 44, 46, 48, 52, 56, 60, 64,
- * 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165 (Value 23 ~ 56)
- */
-
-bool is_channel_valid(unsigned int ChannelIndex)
-{
-	bool bValid;
-
-	bValid = false;
-	/*
-	 * If Channel Index is invalid, return invalid
-	 */
-	if ((ChannelIndex > CB_MAX_CHANNEL) ||
-	    (ChannelIndex == 0)) {
-		bValid = false;
-		goto exit;
-	}
+static struct ieee80211_channel vnt_channels_2ghz[] = {
+	{ .center_freq = 2412, .hw_value = 1 },
+	{ .center_freq = 2417, .hw_value = 2 },
+	{ .center_freq = 2422, .hw_value = 3 },
+	{ .center_freq = 2427, .hw_value = 4 },
+	{ .center_freq = 2432, .hw_value = 5 },
+	{ .center_freq = 2437, .hw_value = 6 },
+	{ .center_freq = 2442, .hw_value = 7 },
+	{ .center_freq = 2447, .hw_value = 8 },
+	{ .center_freq = 2452, .hw_value = 9 },
+	{ .center_freq = 2457, .hw_value = 10 },
+	{ .center_freq = 2462, .hw_value = 11 },
+	{ .center_freq = 2467, .hw_value = 12 },
+	{ .center_freq = 2472, .hw_value = 13 },
+	{ .center_freq = 2484, .hw_value = 14 }
+};
 
-	bValid = sChannelTbl[ChannelIndex].bValid;
+static struct ieee80211_channel vnt_channels_5ghz[] = {
+	{ .center_freq = 4915, .hw_value = 15 },
+	{ .center_freq = 4920, .hw_value = 16 },
+	{ .center_freq = 4925, .hw_value = 17 },
+	{ .center_freq = 4935, .hw_value = 18 },
+	{ .center_freq = 4940, .hw_value = 19 },
+	{ .center_freq = 4945, .hw_value = 20 },
+	{ .center_freq = 4960, .hw_value = 21 },
+	{ .center_freq = 4980, .hw_value = 22 },
+	{ .center_freq = 5035, .hw_value = 23 },
+	{ .center_freq = 5040, .hw_value = 24 },
+	{ .center_freq = 5045, .hw_value = 25 },
+	{ .center_freq = 5055, .hw_value = 26 },
+	{ .center_freq = 5060, .hw_value = 27 },
+	{ .center_freq = 5080, .hw_value = 28 },
+	{ .center_freq = 5170, .hw_value = 29 },
+	{ .center_freq = 5180, .hw_value = 30 },
+	{ .center_freq = 5190, .hw_value = 31 },
+	{ .center_freq = 5200, .hw_value = 32 },
+	{ .center_freq = 5210, .hw_value = 33 },
+	{ .center_freq = 5220, .hw_value = 34 },
+	{ .center_freq = 5230, .hw_value = 35 },
+	{ .center_freq = 5240, .hw_value = 36 },
+	{ .center_freq = 5260, .hw_value = 37 },
+	{ .center_freq = 5280, .hw_value = 38 },
+	{ .center_freq = 5300, .hw_value = 39 },
+	{ .center_freq = 5320, .hw_value = 40 },
+	{ .center_freq = 5500, .hw_value = 41 },
+	{ .center_freq = 5520, .hw_value = 42 },
+	{ .center_freq = 5540, .hw_value = 43 },
+	{ .center_freq = 5560, .hw_value = 44 },
+	{ .center_freq = 5580, .hw_value = 45 },
+	{ .center_freq = 5600, .hw_value = 46 },
+	{ .center_freq = 5620, .hw_value = 47 },
+	{ .center_freq = 5640, .hw_value = 48 },
+	{ .center_freq = 5660, .hw_value = 49 },
+	{ .center_freq = 5680, .hw_value = 50 },
+	{ .center_freq = 5700, .hw_value = 51 },
+	{ .center_freq = 5745, .hw_value = 52 },
+	{ .center_freq = 5765, .hw_value = 53 },
+	{ .center_freq = 5785, .hw_value = 54 },
+	{ .center_freq = 5805, .hw_value = 55 },
+	{ .center_freq = 5825, .hw_value = 56 }
+};
 
-exit:
-	return bValid;
-}
+static struct ieee80211_supported_band vnt_supported_2ghz_band = {
+	.channels = vnt_channels_2ghz,
+	.n_channels = ARRAY_SIZE(vnt_channels_2ghz),
+	.bitrates = vnt_rates_bg,
+	.n_bitrates = ARRAY_SIZE(vnt_rates_bg),
+};
 
-/**
- * channel_get_list() - Get Available Channel List for a given country
- * @CountryCode: The country code defined in country.h
- *
- * Output:
- *      pbyChannelTable:   (QWORD *) correspondent bit mask
- *                          of available channels
- *                          0x0000000000000001 means channel 1 is supported
- *                          0x0000000000000003 means channel 1,2 are supported
- *                          0x000000000000000F means channel 1,2,..15 are supported
- */
+static struct ieee80211_supported_band vnt_supported_5ghz_band = {
+	.channels = vnt_channels_5ghz,
+	.n_channels = ARRAY_SIZE(vnt_channels_5ghz),
+	.bitrates = vnt_rates_a,
+	.n_bitrates = ARRAY_SIZE(vnt_rates_a),
+};
 
-bool channel_get_list(unsigned int uCountryCodeIdx, unsigned char *pbyChannelTable)
+void vnt_init_bands(struct vnt_private *priv)
 {
-	if (uCountryCodeIdx >= CCODE_MAX)
-		return false;
-
-	memcpy(pbyChannelTable, ChannelRuleTab[uCountryCodeIdx].bChannelIdxList, CB_MAX_CHANNEL);
-
-	return true;
-}
+	struct ieee80211_channel *ch;
+	int i;
 
-void init_channel_table(void *pDeviceHandler)
-{
-	struct vnt_private *pDevice = pDeviceHandler;
-	bool bMultiBand = false;
-	unsigned int ii;
+	switch (priv->byRFType) {
+	case RF_AIROHA7230:
+	case RF_UW2452:
+	case RF_NOTHING:
+	default:
+		ch = vnt_channels_5ghz;
 
-	for (ii = 1; ii <= CARD_MAX_CHANNEL_TBL; ii++)
-		sChannelTbl[ii].bValid = false;
+		for (i = 0; i < ARRAY_SIZE(vnt_channels_5ghz); i++) {
+			ch[i].max_power = 0x3f;
+			ch[i].flags = IEEE80211_CHAN_NO_HT40;
+		}
 
-	switch (pDevice->byRFType) {
+		priv->hw->wiphy->bands[IEEE80211_BAND_5GHZ] =
+						&vnt_supported_5ghz_band;
+	/* fallthrough */
 	case RF_RFMD2959:
 	case RF_AIROHA:
 	case RF_AL2230S:
 	case RF_UW2451:
 	case RF_VT3226:
-		bMultiBand = false;
-		break;
-	case RF_AIROHA7230:
-	case RF_UW2452:
-	case RF_NOTHING:
-	default:
-		bMultiBand = true;
-		break;
-	}
+		ch = vnt_channels_2ghz;
 
-	if ((pDevice->dwDiagRefCount != 0) || pDevice->b11hEnable) {
-		if (bMultiBand) {
-			for (ii = 0; ii < CARD_MAX_CHANNEL_TBL; ii++) {
-				sChannelTbl[ii + 1].bValid = true;
-				pDevice->abyRegPwr[ii + 1] = pDevice->abyOFDMDefaultPwr[ii + 1];
-				pDevice->abyLocalPwr[ii + 1] = pDevice->abyOFDMDefaultPwr[ii + 1];
-			}
-			for (ii = 0; ii < CHANNEL_MAX_24G; ii++) {
-				pDevice->abyRegPwr[ii + 1] = pDevice->abyCCKDefaultPwr[ii + 1];
-				pDevice->abyLocalPwr[ii + 1] = pDevice->abyCCKDefaultPwr[ii + 1];
-			}
-		} else {
-			for (ii = 0; ii < CHANNEL_MAX_24G; ii++) {
-				//2008-8-4 <add> by chester
-				if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] != 0) {
-					sChannelTbl[ii + 1].bValid = true;
-					pDevice->abyRegPwr[ii + 1] = pDevice->abyCCKDefaultPwr[ii + 1];
-					pDevice->abyLocalPwr[ii + 1] = pDevice->abyCCKDefaultPwr[ii + 1];
-				}
-			}
+		for (i = 0; i < ARRAY_SIZE(vnt_channels_2ghz); i++) {
+			ch[i].max_power = 0x3f;
+			ch[i].flags = IEEE80211_CHAN_NO_HT40;
 		}
-	} else if (pDevice->byZoneType <= CCODE_MAX) {
-		if (bMultiBand) {
-			for (ii = 0; ii < CARD_MAX_CHANNEL_TBL; ii++) {
-				if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] != 0) {
-					sChannelTbl[ii + 1].bValid = true;
-					pDevice->abyRegPwr[ii + 1] = ChannelRuleTab[pDevice->byZoneType].byPower[ii];
-					pDevice->abyLocalPwr[ii + 1] = ChannelRuleTab[pDevice->byZoneType].byPower[ii];
-				}
-			}
-		} else {
-			for (ii = 0; ii < CHANNEL_MAX_24G; ii++) {
-				if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] != 0) {
-					sChannelTbl[ii + 1].bValid = true;
-					pDevice->abyRegPwr[ii + 1] = ChannelRuleTab[pDevice->byZoneType].byPower[ii];
-					pDevice->abyLocalPwr[ii + 1] = ChannelRuleTab[pDevice->byZoneType].byPower[ii];
-				}
-			}
-		}
-	}
 
-	pr_info("Zone=[%d][%c][%c]!!\n",
-		pDevice->byZoneType,
-		ChannelRuleTab[pDevice->byZoneType].chCountryCode[0],
-		ChannelRuleTab[pDevice->byZoneType].chCountryCode[1]);
-
-	for (ii = 0; ii < CARD_MAX_CHANNEL_TBL; ii++) {
-		if (pDevice->abyRegPwr[ii + 1] == 0)
-			pDevice->abyRegPwr[ii + 1] = pDevice->abyOFDMDefaultPwr[ii + 1];
-		if (pDevice->abyLocalPwr[ii + 1] == 0)
-			pDevice->abyLocalPwr[ii + 1] = pDevice->abyOFDMDefaultPwr[ii + 1];
-	}
-}
-
-unsigned char get_channel_mapping(void *pDeviceHandler, unsigned char byChannelNumber, CARD_PHY_TYPE ePhyType)
-{
-	unsigned int ii;
-
-	if ((ePhyType == PHY_TYPE_11B) || (ePhyType == PHY_TYPE_11G))
-		return byChannelNumber;
-
-	for (ii = (CB_MAX_CHANNEL_24G + 1); ii <= CB_MAX_CHANNEL;) {
-		if (sChannelTbl[ii].byChannelNumber == byChannelNumber)
-			return (unsigned char) ii;
-		ii++;
+		priv->hw->wiphy->bands[IEEE80211_BAND_2GHZ] =
+						&vnt_supported_2ghz_band;
+		break;
 	}
-	return 0;
-}
-
-unsigned char get_channel_number(void *pDeviceHandler, unsigned char byChannelIndex)
-{
-	return sChannelTbl[byChannelIndex].byChannelNumber;
 }
 
 /**
@@ -528,37 +182,27 @@ bool set_channel(void *pDeviceHandler, unsigned int uConnectionChannel)
 	if (pDevice->byCurrentCh == uConnectionChannel)
 		return bResult;
 
-	if (!sChannelTbl[uConnectionChannel].bValid)
-		return false;
-
-	if ((uConnectionChannel > CB_MAX_CHANNEL_24G) &&
-	    (pDevice->eCurrentPHYType != PHY_TYPE_11A)) {
-		CARDbSetPhyParameter(pDevice, PHY_TYPE_11A, 0, 0, NULL, NULL);
-	} else if ((uConnectionChannel <= CB_MAX_CHANNEL_24G) &&
-		   (pDevice->eCurrentPHYType == PHY_TYPE_11A)) {
-		CARDbSetPhyParameter(pDevice, PHY_TYPE_11G, 0, 0, NULL, NULL);
-	}
-	// clear NAV
+	/* clear NAV */
 	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MACCR, MACCR_CLRNAV);
 
-	//{{ RobertYu: 20041202
-	//// TX_PE will reserve 3 us for MAX2829 A mode only, it is for better TX throughput
+	/* TX_PE will reserve 3 us for MAX2829 A mode only, it is for better TX throughput */
 
 	if (pDevice->byRFType == RF_AIROHA7230)
-		RFbAL7230SelectChannelPostProcess(pDevice->PortOffset, pDevice->byCurrentCh, (unsigned char)uConnectionChannel);
-	//}} RobertYu
+		RFbAL7230SelectChannelPostProcess(pDevice, pDevice->byCurrentCh,
+						  (unsigned char)uConnectionChannel);
 
 	pDevice->byCurrentCh = (unsigned char)uConnectionChannel;
-	bResult &= RFbSelectChannel(pDevice->PortOffset, pDevice->byRFType, (unsigned char)uConnectionChannel);
+	bResult &= RFbSelectChannel(pDevice, pDevice->byRFType,
+				    (unsigned char)uConnectionChannel);
 
-	// Init Synthesizer Table
+	/* Init Synthesizer Table */
 	if (pDevice->bEnablePSMode)
-		RFvWriteWakeProgSyn(pDevice->PortOffset, pDevice->byRFType, uConnectionChannel);
+		RFvWriteWakeProgSyn(pDevice, pDevice->byRFType, uConnectionChannel);
 
-	BBvSoftwareReset(pDevice->PortOffset);
+	BBvSoftwareReset(pDevice);
 
 	if (pDevice->byLocalID > REV_ID_VT3253_B1) {
-		// set HW default power register
+		/* set HW default power register */
 		MACvSelectPage1(pDevice->PortOffset);
 		RFbSetPower(pDevice, RATE_1M, pDevice->byCurrentCh);
 		VNSvOutPortB(pDevice->PortOffset + MAC_REG_PWRCCK, pDevice->byCurPwr);
@@ -567,242 +211,10 @@ bool set_channel(void *pDeviceHandler, unsigned int uConnectionChannel)
 		MACvSelectPage0(pDevice->PortOffset);
 	}
 
-	if (pDevice->eCurrentPHYType == PHY_TYPE_11B)
+	if (pDevice->byBBType == BB_TYPE_11B)
 		RFbSetPower(pDevice, RATE_1M, pDevice->byCurrentCh);
 	else
 		RFbSetPower(pDevice, RATE_6M, pDevice->byCurrentCh);
 
 	return bResult;
 }
-
-/**
- * set_country_info() - Set Channel Info of Country
- *
- * Return Value: none.
- *
- */
-
-void set_country_info(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, void *pIE)
-{
-	struct vnt_private *pDevice = pDeviceHandler;
-	unsigned int ii = 0;
-	unsigned int uu = 0;
-	unsigned int step = 0;
-	unsigned int uNumOfCountryInfo = 0;
-	unsigned char byCh = 0;
-	PWLAN_IE_COUNTRY pIE_Country = (PWLAN_IE_COUNTRY) pIE;
-
-	uNumOfCountryInfo = (pIE_Country->len - 3);
-	uNumOfCountryInfo /= 3;
-
-	if (ePHYType == PHY_TYPE_11A) {
-		pDevice->bCountryInfo5G = true;
-		for (ii = CB_MAX_CHANNEL_24G + 1; ii <= CARD_MAX_CHANNEL_TBL; ii++)
-			sChannelTbl[ii].bValid = false;
-
-		step = 4;
-	} else {
-		pDevice->bCountryInfo24G = true;
-		for (ii = 1; ii <= CB_MAX_CHANNEL_24G; ii++)
-			sChannelTbl[ii].bValid = false;
-
-		step = 1;
-	}
-	pDevice->abyCountryCode[0] = pIE_Country->abyCountryString[0];
-	pDevice->abyCountryCode[1] = pIE_Country->abyCountryString[1];
-	pDevice->abyCountryCode[2] = pIE_Country->abyCountryString[2];
-
-	for (ii = 0; ii < uNumOfCountryInfo; ii++) {
-		for (uu = 0; uu < pIE_Country->abyCountryInfo[ii*3+1]; uu++) {
-			byCh = get_channel_mapping(pDevice, (unsigned char)(pIE_Country->abyCountryInfo[ii*3]+step*uu), ePHYType);
-			sChannelTbl[byCh].bValid = true;
-			pDevice->abyRegPwr[byCh] = pIE_Country->abyCountryInfo[ii*3+2];
-		}
-	}
-}
-
-/**
- *
- * set_support_channels() - Set Support Channels IE defined in 802.11h
- *
- * @hDeviceContext: device structure point
- *
- * Return Value: none.
- *
- */
-
-unsigned char set_support_channels(void *pDeviceHandler, unsigned char *pbyIEs)
-{
-	struct vnt_private *pDevice = pDeviceHandler;
-	unsigned int ii;
-	unsigned char byCount;
-	PWLAN_IE_SUPP_CH pIE = (PWLAN_IE_SUPP_CH) pbyIEs;
-	unsigned char *pbyChTupple;
-	unsigned char byLen = 0;
-
-	pIE->byElementID = WLAN_EID_SUPP_CH;
-	pIE->len = 0;
-	pbyChTupple = pIE->abyChannelTuple;
-	byLen = 2;
-	// lower band
-	byCount = 0;
-	if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[28] == true) {
-		for (ii = 28; ii < 36; ii += 2) {
-			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true)
-				byCount++;
-		}
-
-		*pbyChTupple++ = 34;
-		*pbyChTupple++ = byCount;
-		byLen += 2;
-	} else if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[29] == true) {
-		for (ii = 29; ii < 36; ii += 2) {
-			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true)
-				byCount++;
-		}
-
-		*pbyChTupple++ = 36;
-		*pbyChTupple++ = byCount;
-		byLen += 2;
-	}
-	// middle band
-	byCount = 0;
-	if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[36] == true) {
-		for (ii = 36; ii < 40; ii++) {
-			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true)
-				byCount++;
-		}
-
-		*pbyChTupple++ = 52;
-		*pbyChTupple++ = byCount;
-		byLen += 2;
-	}
-	// higher band
-	byCount = 0;
-	if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[40] == true) {
-		for (ii = 40; ii < 51; ii++) {
-			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true)
-				byCount++;
-		}
-
-		*pbyChTupple++ = 100;
-		*pbyChTupple++ = byCount;
-		byLen += 2;
-	} else if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[51] == true) {
-		for (ii = 51; ii < 56; ii++) {
-			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true)
-				byCount++;
-		}
-
-		*pbyChTupple++ = 149;
-		*pbyChTupple++ = byCount;
-		byLen += 2;
-	}
-	pIE->len += (byLen - 2);
-	return byLen;
-}
-
-void set_country_IE(void *pDeviceHandler, void *pIE)
-{
-	struct vnt_private *pDevice = pDeviceHandler;
-	unsigned int ii;
-	PWLAN_IE_COUNTRY pIECountry = (PWLAN_IE_COUNTRY) pIE;
-
-	pIECountry->byElementID = WLAN_EID_COUNTRY;
-	pIECountry->len = 0;
-	pIECountry->abyCountryString[0] = ChannelRuleTab[pDevice->byZoneType].chCountryCode[0];
-	pIECountry->abyCountryString[1] = ChannelRuleTab[pDevice->byZoneType].chCountryCode[1];
-	pIECountry->abyCountryString[2] = ' ';
-	for (ii = CB_MAX_CHANNEL_24G; ii < CB_MAX_CHANNEL; ii++) {
-		if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] != 0) {
-			pIECountry->abyCountryInfo[pIECountry->len++] = sChannelTbl[ii + 1].byChannelNumber;
-			pIECountry->abyCountryInfo[pIECountry->len++] = 1;
-			pIECountry->abyCountryInfo[pIECountry->len++] = ChannelRuleTab[pDevice->byZoneType].byPower[ii];
-		}
-	}
-	pIECountry->len += 3;
-}
-
-bool get_channel_map_info(void *pDeviceHandler, unsigned int uChannelIndex,
-			  unsigned char *pbyChannelNumber, unsigned char *pbyMap)
-{
-	if (uChannelIndex > CB_MAX_CHANNEL)
-		return false;
-
-	*pbyChannelNumber = sChannelTbl[uChannelIndex].byChannelNumber;
-	*pbyMap = sChannelTbl[uChannelIndex].byMAP;
-	return sChannelTbl[uChannelIndex].bValid;
-}
-
-void set_channel_map_info(void *pDeviceHandler, unsigned int uChannelIndex,
-			  unsigned char byMap)
-{
-	if (uChannelIndex > CB_MAX_CHANNEL)
-		return;
-
-	sChannelTbl[uChannelIndex].byMAP |= byMap;
-}
-
-void clear_channel_map_info(void *pDeviceHandler)
-{
-	unsigned int ii = 0;
-
-	for (ii = 1; ii <=  CB_MAX_CHANNEL; ii++)
-		sChannelTbl[ii].byMAP = 0;
-}
-
-unsigned char auto_channel_select(void *pDeviceHandler, CARD_PHY_TYPE ePHYType)
-{
-	unsigned int ii = 0;
-	unsigned char byOptionChannel = 0;
-	int aiWeight[CB_MAX_CHANNEL_24G + 1] = {-1000, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-
-	if (ePHYType == PHY_TYPE_11A) {
-		for (ii = CB_MAX_CHANNEL_24G + 1; ii <= CB_MAX_CHANNEL; ii++) {
-			if (sChannelTbl[ii].bValid) {
-				if (byOptionChannel == 0)
-					byOptionChannel = (unsigned char) ii;
-
-				if (sChannelTbl[ii].byMAP == 0)
-					return (unsigned char) ii;
-				else if (!(sChannelTbl[ii].byMAP & 0x08))
-					byOptionChannel = (unsigned char) ii;
-			}
-		}
-	} else {
-		byOptionChannel = 0;
-		for (ii = 1; ii <= CB_MAX_CHANNEL_24G; ii++) {
-			if (sChannelTbl[ii].bValid) {
-				if (sChannelTbl[ii].byMAP == 0) {
-					aiWeight[ii] += 100;
-				} else if (sChannelTbl[ii].byMAP & 0x01) {
-					if (ii > 3)
-						aiWeight[ii - 3] -= 10;
-
-					if (ii > 2)
-						aiWeight[ii - 2] -= 20;
-
-					if (ii > 1)
-						aiWeight[ii - 1] -= 40;
-
-					aiWeight[ii] -= 80;
-					if (ii < CB_MAX_CHANNEL_24G)
-						aiWeight[ii + 1] -= 40;
-
-					if (ii < (CB_MAX_CHANNEL_24G - 1))
-						aiWeight[ii+2] -= 20;
-
-					if (ii < (CB_MAX_CHANNEL_24G - 2))
-						aiWeight[ii+3] -= 10;
-				}
-			}
-		}
-		for (ii = 1; ii <= CB_MAX_CHANNEL_24G; ii++) {
-			if (sChannelTbl[ii].bValid &&
-			    (aiWeight[ii] > aiWeight[byOptionChannel])) {
-				byOptionChannel = (unsigned char) ii;
-			}
-		}
-	}
-	return byOptionChannel;
-}
diff --git a/drivers/staging/vt6655/channel.h b/drivers/staging/vt6655/channel.h
index 4f44c8a3d3cf..4f4264e23462 100644
--- a/drivers/staging/vt6655/channel.h
+++ b/drivers/staging/vt6655/channel.h
@@ -23,30 +23,10 @@
 #ifndef _CHANNEL_H_
 #define _CHANNEL_H_
 
-#include "ttype.h"
 #include "card.h"
 
-typedef struct tagSChannelTblElement {
-	unsigned char byChannelNumber;
-	unsigned int uFrequency;
-	bool bValid;
-	unsigned char byMAP;
-} SChannelTblElement, *PSChannelTblElement;
+void vnt_init_bands(struct vnt_private *);
 
-bool is_channel_valid(unsigned int CountryCode);
-void init_channel_table(void *pDeviceHandler);
-unsigned char get_channel_mapping(void *pDeviceHandler, unsigned char byChannelNumber, CARD_PHY_TYPE ePhyType);
-bool channel_get_list(unsigned int uCountryCodeIdx, unsigned char *pbyChannelTable);
-unsigned char get_channel_number(void *pDeviceHandler, unsigned char byChannelIndex);
 bool set_channel(void *pDeviceHandler, unsigned int uConnectionChannel);
-void set_country_info(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, void *pIE);
-unsigned char set_support_channels(void *pDeviceHandler, unsigned char *pbyIEs);
-void set_country_IE(void *pDeviceHandler, void *pIE);
-bool get_channel_map_info(void *pDeviceHandler, unsigned int uChannelIndex,
-			  unsigned char *pbyChannelNumber, unsigned char *pbyMap);
-void set_channel_map_info(void *pDeviceHandler, unsigned int uChannelIndex,
-			  unsigned char byMap);
-void clear_channel_map_info(void *pDeviceHandler);
-unsigned char auto_channel_select(void *pDeviceHandler, CARD_PHY_TYPE ePHYType);
 
 #endif /* _CHANNEL_H_ */
diff --git a/drivers/staging/vt6655/country.h b/drivers/staging/vt6655/country.h
deleted file mode 100644
index 2365fb13b033..000000000000
--- a/drivers/staging/vt6655/country.h
+++ /dev/null
@@ -1,161 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: country.h
- *
- * Purpose: Country Code information
- *
- * Author: Lucas Lin
- *
- * Date: Dec 23, 2004
- *
- */
-
-#ifndef __COUNTRY_H__
-#define __COUNTRY_H__
-
-#include "ttype.h"
-
-/************************************************************************
- * The definition here should be complied with the INF country order
- * Please check with VNWL.inf/VNWL64.inf/VNWL*.inf
- ************************************************************************/
-typedef enum _COUNTRY_CODE {
-	CCODE_FCC = 0,
-	CCODE_TELEC,
-	CCODE_ETSI,
-	CCODE_RESV3,
-	CCODE_RESV4,
-	CCODE_RESV5,
-	CCODE_RESV6,
-	CCODE_RESV7,
-	CCODE_RESV8,
-	CCODE_RESV9,
-	CCODE_RESVa,
-	CCODE_RESVb,
-	CCODE_RESVc,
-	CCODE_RESVd,
-	CCODE_RESVe,
-	CCODE_ALLBAND,
-	CCODE_ALBANIA,
-	CCODE_ALGERIA,
-	CCODE_ARGENTINA,
-	CCODE_ARMENIA,
-	CCODE_AUSTRALIA,
-	CCODE_AUSTRIA,
-	CCODE_AZERBAIJAN,
-	CCODE_BAHRAIN,
-	CCODE_BELARUS,
-	CCODE_BELGIUM,
-	CCODE_BELIZE,
-	CCODE_BOLIVIA,
-	CCODE_BRAZIL,
-	CCODE_BRUNEI_DARUSSALAM,
-	CCODE_BULGARIA,
-	CCODE_CANADA,
-	CCODE_CHILE,
-	CCODE_CHINA,
-	CCODE_COLOMBIA,
-	CCODE_COSTA_RICA,
-	CCODE_CROATIA,
-	CCODE_CYPRUS,
-	CCODE_CZECH,
-	CCODE_DENMARK,
-	CCODE_DOMINICAN_REPUBLIC,
-	CCODE_ECUADOR,
-	CCODE_EGYPT,
-	CCODE_EL_SALVADOR,
-	CCODE_ESTONIA,
-	CCODE_FINLAND,
-	CCODE_FRANCE,
-	CCODE_GERMANY,
-	CCODE_GREECE,
-	CCODE_GEORGIA,
-	CCODE_GUATEMALA,
-	CCODE_HONDURAS,
-	CCODE_HONG_KONG,
-	CCODE_HUNGARY,
-	CCODE_ICELAND,
-	CCODE_INDIA,
-	CCODE_INDONESIA,
-	CCODE_IRAN,
-	CCODE_IRELAND,
-	CCODE_ITALY,
-	CCODE_ISRAEL,
-	CCODE_JAPAN,
-	CCODE_JORDAN,
-	CCODE_KAZAKHSTAN,
-	CCODE_KUWAIT,
-	CCODE_LATVIA,
-	CCODE_LEBANON,
-	CCODE_LEICHTENSTEIN,
-	CCODE_LITHUANIA,
-	CCODE_LUXEMBURG,
-	CCODE_MACAU,
-	CCODE_MACEDONIA,
-	CCODE_MALTA,
-	CCODE_MALAYSIA,
-	CCODE_MEXICO,
-	CCODE_MONACO,
-	CCODE_MOROCCO,
-	CCODE_NETHERLANDS,
-	CCODE_NEW_ZEALAND,
-	CCODE_NORTH_KOREA,
-	CCODE_NORWAY,
-	CCODE_OMAN,
-	CCODE_PAKISTAN,
-	CCODE_PANAMA,
-	CCODE_PERU,
-	CCODE_PHILIPPINES,
-	CCODE_POLAND,
-	CCODE_PORTUGAL,
-	CCODE_PUERTO_RICO,
-	CCODE_QATAR,
-	CCODE_ROMANIA,
-	CCODE_RUSSIA,
-	CCODE_SAUDI_ARABIA,
-	CCODE_SINGAPORE,
-	CCODE_SLOVAKIA,
-	CCODE_SLOVENIA,
-	CCODE_SOUTH_AFRICA,
-	CCODE_SOUTH_KOREA,
-	CCODE_SPAIN,
-	CCODE_SWEDEN,
-	CCODE_SWITZERLAND,
-	CCODE_SYRIA,
-	CCODE_TAIWAN,
-	CCODE_THAILAND,
-	CCODE_TRINIDAD_TOBAGO,
-	CCODE_TUNISIA,
-	CCODE_TURKEY,
-	CCODE_UK,
-	CCODE_UKRAINE,
-	CCODE_UNITED_ARAB_EMIRATES,
-	CCODE_UNITED_STATES,
-	CCODE_URUGUAY,
-	CCODE_UZBEKISTAN,
-	CCODE_VENEZUELA,
-	CCODE_VIETNAM,
-	CCODE_YEMEN,
-	CCODE_ZIMBABWE,
-	CCODE_JAPAN_W52_W53,
-	CCODE_MAX
-} COUNTRY_CODE;
-
-#endif  /* __COUNTRY_H__ */
diff --git a/drivers/staging/vt6655/datarate.c b/drivers/staging/vt6655/datarate.c
deleted file mode 100644
index 52907a4fae9d..000000000000
--- a/drivers/staging/vt6655/datarate.c
+++ /dev/null
@@ -1,410 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: datarate.c
- *
- * Purpose: Handles the auto fallback & data rates functions
- *
- * Author: Lyndon Chen
- *
- * Date: July 17, 2002
- *
- * Functions:
- *      RATEvParseMaxRate - Parsing the highest basic & support rate in rate field of frame
- *      RATEvTxRateFallBack - Rate fallback Algorithm Implementaion
- *      RATEuSetIE- Set rate IE field.
- *
- * Revision History:
- *
- */
-
-#include "ttype.h"
-#include "tmacro.h"
-#include "mac.h"
-#include "80211mgr.h"
-#include "bssdb.h"
-#include "datarate.h"
-#include "card.h"
-#include "baseband.h"
-#include "srom.h"
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-extern unsigned short TxRate_iwconfig; /* 2008-5-8 <add> by chester */
-/*---------------------  Static Variables  --------------------------*/
-static const unsigned char acbyIERate[MAX_RATE] = {
-0x02, 0x04, 0x0B, 0x16, 0x0C, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C
-};
-
-#define AUTORATE_TXOK_CNT       0x0400
-#define AUTORATE_TXFAIL_CNT     0x0064
-#define AUTORATE_TIMEOUT        10
-
-/*---------------------  Static Functions  --------------------------*/
-
-void s_vResetCounter(
-	PKnownNodeDB psNodeDBTable
-);
-
-void
-s_vResetCounter(
-	PKnownNodeDB psNodeDBTable
-)
-{
-	unsigned char ii;
-
-	/* clear statistic counter for auto_rate */
-	for (ii = 0; ii <= MAX_RATE; ii++) {
-		psNodeDBTable->uTxOk[ii] = 0;
-		psNodeDBTable->uTxFail[ii] = 0;
-	}
-}
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-/*+
- *
- * Description:
- *      Get RateIdx from the value in SuppRates IE or ExtSuppRates IE
- *
- * Parameters:
- *  In:
- *      unsigned char - Rate value in SuppRates IE or ExtSuppRates IE
- *  Out:
- *      none
- *
- * Return Value: RateIdx
- *
- -*/
-unsigned char
-DATARATEbyGetRateIdx(
-	unsigned char byRate
-)
-{
-	unsigned char ii;
-
-	/* Erase basicRate flag. */
-	byRate = byRate & 0x7F;/* 0111 1111 */
-
-	for (ii = 0; ii < MAX_RATE; ii++) {
-		if (acbyIERate[ii] == byRate)
-			return ii;
-	}
-	return 0;
-}
-
-/*+
- *
- * Routine Description:
- *      Rate fallback Algorithm Implementation
- *
- * Parameters:
- *  In:
- *      pDevice         - Pointer to the adapter
- *      psNodeDBTable   - Pointer to Node Data Base
- *  Out:
- *      none
- *
- * Return Value: none
- *
- -*/
-#define AUTORATE_TXCNT_THRESHOLD        20
-#define AUTORATE_INC_THRESHOLD          30
-
-/*+
- *
- * Description:
- *      Get RateIdx from the value in SuppRates IE or ExtSuppRates IE
- *
- * Parameters:
- *  In:
- *      unsigned char - Rate value in SuppRates IE or ExtSuppRates IE
- *  Out:
- *      none
- *
- * Return Value: RateIdx
- *
- -*/
-unsigned short
-wGetRateIdx(
-	unsigned char byRate
-)
-{
-	unsigned short ii;
-
-	/* Erase basicRate flag. */
-	byRate = byRate & 0x7F;/* 0111 1111 */
-
-	for (ii = 0; ii < MAX_RATE; ii++) {
-		if (acbyIERate[ii] == byRate)
-			return ii;
-	}
-
-	return 0;
-}
-
-/*+
- *
- * Description:
- *      Parsing the highest basic & support rate in rate field of frame.
- *
- * Parameters:
- *  In:
- *      pDevice         - Pointer to the adapter
- *      pItemRates      - Pointer to Rate field defined in 802.11 spec.
- *      pItemExtRates      - Pointer to Extended Rate field defined in 802.11 spec.
- *  Out:
- *      pwMaxBasicRate  - Maximum Basic Rate
- *      pwMaxSuppRate   - Maximum Supported Rate
- *      pbyTopCCKRate   - Maximum Basic Rate in CCK mode
- *      pbyTopOFDMRate  - Maximum Basic Rate in OFDM mode
- *
- * Return Value: none
- *
- -*/
-void
-RATEvParseMaxRate(
-	void *pDeviceHandler,
-	PWLAN_IE_SUPP_RATES pItemRates,
-	PWLAN_IE_SUPP_RATES pItemExtRates,
-	bool bUpdateBasicRate,
-	unsigned short *pwMaxBasicRate,
-	unsigned short *pwMaxSuppRate,
-	unsigned short *pwSuppRate,
-	unsigned char *pbyTopCCKRate,
-	unsigned char *pbyTopOFDMRate
-)
-{
-	struct vnt_private *pDevice = pDeviceHandler;
-	unsigned int ii;
-	unsigned char byHighSuppRate = 0;
-	unsigned char byRate = 0;
-	unsigned short wOldBasicRate = pDevice->wBasicRate;
-	unsigned int uRateLen;
-
-	if (pItemRates == NULL)
-		return;
-
-	*pwSuppRate = 0;
-	uRateLen = pItemRates->len;
-
-	pr_debug("ParseMaxRate Len: %d\n", uRateLen);
-	if (pDevice->eCurrentPHYType != PHY_TYPE_11B) {
-		if (uRateLen > WLAN_RATES_MAXLEN)
-			uRateLen = WLAN_RATES_MAXLEN;
-	} else {
-		if (uRateLen > WLAN_RATES_MAXLEN_11B)
-			uRateLen = WLAN_RATES_MAXLEN_11B;
-	}
-
-	for (ii = 0; ii < uRateLen; ii++) {
-		byRate = (unsigned char)(pItemRates->abyRates[ii]);
-		if (WLAN_MGMT_IS_BASICRATE(byRate) && bUpdateBasicRate)  {
-			/* Add to basic rate set, update pDevice->byTopCCKBasicRate and pDevice->byTopOFDMBasicRate */
-			CARDbAddBasicRate((void *)pDevice, wGetRateIdx(byRate));
-			pr_debug("ParseMaxRate AddBasicRate: %d\n",
-				 wGetRateIdx(byRate));
-		}
-		byRate = (unsigned char)(pItemRates->abyRates[ii]&0x7F);
-		if (byHighSuppRate == 0)
-			byHighSuppRate = byRate;
-		if (byRate > byHighSuppRate)
-			byHighSuppRate = byRate;
-		*pwSuppRate |= (1<<wGetRateIdx(byRate));
-	}
-	if ((pItemExtRates != NULL) && (pItemExtRates->byElementID == WLAN_EID_EXTSUPP_RATES) &&
-	    (pDevice->eCurrentPHYType != PHY_TYPE_11B)) {
-		unsigned int uExtRateLen = pItemExtRates->len;
-
-		if (uExtRateLen > WLAN_RATES_MAXLEN)
-			uExtRateLen = WLAN_RATES_MAXLEN;
-
-		for (ii = 0; ii < uExtRateLen; ii++) {
-			byRate = (unsigned char)(pItemExtRates->abyRates[ii]);
-			/* select highest basic rate */
-			if (WLAN_MGMT_IS_BASICRATE(pItemExtRates->abyRates[ii])) {
-				/* Add to basic rate set, update pDevice->byTopCCKBasicRate and pDevice->byTopOFDMBasicRate */
-				CARDbAddBasicRate((void *)pDevice, wGetRateIdx(byRate));
-				pr_debug("ParseMaxRate AddBasicRate: %d\n",
-					 wGetRateIdx(byRate));
-			}
-			byRate = (unsigned char)(pItemExtRates->abyRates[ii]&0x7F);
-			if (byHighSuppRate == 0)
-				byHighSuppRate = byRate;
-			if (byRate > byHighSuppRate)
-				byHighSuppRate = byRate;
-			*pwSuppRate |= (1<<wGetRateIdx(byRate));
-		}
-	}
-
-	if ((pDevice->byPacketType == PK_TYPE_11GB) && CARDbIsOFDMinBasicRate((void *)pDevice))
-		pDevice->byPacketType = PK_TYPE_11GA;
-
-	*pbyTopCCKRate = pDevice->byTopCCKBasicRate;
-	*pbyTopOFDMRate = pDevice->byTopOFDMBasicRate;
-	*pwMaxSuppRate = wGetRateIdx(byHighSuppRate);
-	if ((pDevice->byPacketType == PK_TYPE_11B) || (pDevice->byPacketType == PK_TYPE_11GB))
-		*pwMaxBasicRate = pDevice->byTopCCKBasicRate;
-	else
-		*pwMaxBasicRate = pDevice->byTopOFDMBasicRate;
-	if (wOldBasicRate != pDevice->wBasicRate)
-		CARDvSetRSPINF((void *)pDevice, pDevice->eCurrentPHYType);
-
-	pr_debug("Exit ParseMaxRate\n");
-}
-
-/*+
- *
- * Routine Description:
- *      Rate fallback Algorithm Implementaion
- *
- * Parameters:
- *  In:
- *      pDevice         - Pointer to the adapter
- *      psNodeDBTable   - Pointer to Node Data Base
- *  Out:
- *      none
- *
- * Return Value: none
- *
- -*/
-#define AUTORATE_TXCNT_THRESHOLD        20
-#define AUTORATE_INC_THRESHOLD          30
-
-void
-RATEvTxRateFallBack(
-	void *pDeviceHandler,
-	PKnownNodeDB psNodeDBTable
-)
-{
-	struct vnt_private *pDevice = pDeviceHandler;
-	unsigned short wIdxDownRate = 0;
-	unsigned int ii;
-	bool bAutoRate[MAX_RATE]    = {true, true, true, true, false, false, true, true, true, true, true, true};
-	unsigned long dwThroughputTbl[MAX_RATE] = {10, 20, 55, 110, 60, 90, 120, 180, 240, 360, 480, 540};
-	unsigned long dwThroughput = 0;
-	unsigned short wIdxUpRate = 0;
-	unsigned long dwTxDiff = 0;
-
-	if (pDevice->pMgmt->eScanState != WMAC_NO_SCANNING)
-		/* Don't do Fallback when scanning Channel */
-		return;
-
-	psNodeDBTable->uTimeCount++;
-
-	if (psNodeDBTable->uTxFail[MAX_RATE] > psNodeDBTable->uTxOk[MAX_RATE])
-		dwTxDiff = psNodeDBTable->uTxFail[MAX_RATE] - psNodeDBTable->uTxOk[MAX_RATE];
-
-	if ((psNodeDBTable->uTxOk[MAX_RATE] < AUTORATE_TXOK_CNT) &&
-	    (dwTxDiff < AUTORATE_TXFAIL_CNT) &&
-	    (psNodeDBTable->uTimeCount < AUTORATE_TIMEOUT)) {
-		return;
-	}
-
-	if (psNodeDBTable->uTimeCount >= AUTORATE_TIMEOUT)
-		psNodeDBTable->uTimeCount = 0;
-
-	for (ii = 0; ii < MAX_RATE; ii++) {
-		if (psNodeDBTable->wSuppRate & (0x0001<<ii)) {
-			if (bAutoRate[ii])
-				wIdxUpRate = (unsigned short) ii;
-
-		} else {
-			bAutoRate[ii] = false;
-		}
-	}
-
-	for (ii = 0; ii <= psNodeDBTable->wTxDataRate; ii++) {
-		if ((psNodeDBTable->uTxOk[ii] != 0) ||
-		    (psNodeDBTable->uTxFail[ii] != 0)) {
-			dwThroughputTbl[ii] *= psNodeDBTable->uTxOk[ii];
-			if (ii < RATE_11M)
-				psNodeDBTable->uTxFail[ii] *= 4;
-
-			dwThroughputTbl[ii] /= (psNodeDBTable->uTxOk[ii] + psNodeDBTable->uTxFail[ii]);
-		}
-	}
-	dwThroughput = dwThroughputTbl[psNodeDBTable->wTxDataRate];
-
-	wIdxDownRate = psNodeDBTable->wTxDataRate;
-	for (ii = psNodeDBTable->wTxDataRate; ii > 0;) {
-		ii--;
-		if ((dwThroughputTbl[ii] > dwThroughput) && bAutoRate[ii]) {
-			dwThroughput = dwThroughputTbl[ii];
-			wIdxDownRate = (unsigned short) ii;
-		}
-	}
-	psNodeDBTable->wTxDataRate = wIdxDownRate;
-	if (psNodeDBTable->uTxOk[MAX_RATE]) {
-		if (psNodeDBTable->uTxOk[MAX_RATE] >
-		    (psNodeDBTable->uTxFail[MAX_RATE] * 4)) {
-			psNodeDBTable->wTxDataRate = wIdxUpRate;
-		}
-	} else {
-		/* adhoc, if uTxOk =0 & uTxFail = 0 */
-		if (psNodeDBTable->uTxFail[MAX_RATE] == 0)
-			psNodeDBTable->wTxDataRate = wIdxUpRate;
-	}
-
-	/* 2008-5-8 <add> by chester */
-	TxRate_iwconfig = psNodeDBTable->wTxDataRate;
-	s_vResetCounter(psNodeDBTable);
-}
-
-/*+
- *
- * Description:
- *    This routine is used to assemble available Rate IE.
- *
- * Parameters:
- *  In:
- *    pDevice
- *  Out:
- *
- * Return Value: None
- *
- -*/
-unsigned char
-RATEuSetIE(
-	PWLAN_IE_SUPP_RATES pSrcRates,
-	PWLAN_IE_SUPP_RATES pDstRates,
-	unsigned int uRateLen
-)
-{
-	unsigned int ii, uu, uRateCnt = 0;
-
-	if ((pSrcRates == NULL) || (pDstRates == NULL))
-		return 0;
-
-	if (pSrcRates->len == 0)
-		return 0;
-
-	for (ii = 0; ii < uRateLen; ii++) {
-		for (uu = 0; uu < pSrcRates->len; uu++) {
-			if ((pSrcRates->abyRates[uu] & 0x7F) == acbyIERate[ii]) {
-				pDstRates->abyRates[uRateCnt++] = pSrcRates->abyRates[uu];
-				break;
-			}
-		}
-	}
-	return (unsigned char)uRateCnt;
-}
diff --git a/drivers/staging/vt6655/datarate.h b/drivers/staging/vt6655/datarate.h
deleted file mode 100644
index 0509c4fd2a42..000000000000
--- a/drivers/staging/vt6655/datarate.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: datarate.h
- *
- * Purpose: Handles the auto fallback & data rates functions
- *
- * Author: Lyndon Chen
- *
- * Date: July 16, 2002
- *
- */
-#ifndef __DATARATE_H__
-#define __DATARATE_H__
-
-#define FALLBACK_PKT_COLLECT_TR_H  50
-#define FALLBACK_PKT_COLLECT_TR_L  10
-#define FALLBACK_POLL_SECOND       5
-#define FALLBACK_RECOVER_SECOND    30
-#define FALLBACK_THRESHOLD         15
-#define UPGRADE_THRESHOLD          5
-#define UPGRADE_CNT_THRD           3
-#define RETRY_TIMES_THRD_H         2
-#define RETRY_TIMES_THRD_L         1
-
-void
-RATEvParseMaxRate(
-	void *pDeviceHandler,
-	PWLAN_IE_SUPP_RATES pItemRates,
-	PWLAN_IE_SUPP_RATES pItemExtRates,
-	bool bUpdateBasicRate,
-	unsigned short *pwMaxBasicRate,
-	unsigned short *pwMaxSuppRate,
-	unsigned short *pwSuppRate,
-	unsigned char *pbyTopCCKRate,
-	unsigned char *pbyTopOFDMRate
-);
-
-void
-RATEvTxRateFallBack(
-	void *pDeviceHandler,
-	PKnownNodeDB psNodeDBTable
-);
-
-unsigned char
-RATEuSetIE(
-	PWLAN_IE_SUPP_RATES pSrcRates,
-	PWLAN_IE_SUPP_RATES pDstRates,
-	unsigned int uRateLen
-);
-
-unsigned short
-wGetRateIdx(
-	unsigned char byRate
-);
-
-unsigned char
-DATARATEbyGetRateIdx(
-	unsigned char byRate
-);
-
-#endif //__DATARATE_H__
diff --git a/drivers/staging/vt6655/desc.h b/drivers/staging/vt6655/desc.h
index 5a2bbd2047d8..758eeb2afd51 100644
--- a/drivers/staging/vt6655/desc.h
+++ b/drivers/staging/vt6655/desc.h
@@ -34,44 +34,34 @@
 #include <linux/types.h>
 #include <linux/mm.h>
 #include "linux/ieee80211.h"
-#include "ttype.h"
-#include "tether.h"
 
 #define B_OWNED_BY_CHIP     1
 #define B_OWNED_BY_HOST     0
 
-//
-// Bits in the RSR register
-//
+/* Bits in the RSR register */
 #define RSR_ADDRBROAD       0x80
 #define RSR_ADDRMULTI       0x40
 #define RSR_ADDRUNI         0x00
 #define RSR_IVLDTYP         0x20
-#define RSR_IVLDLEN         0x10        // invalid len (> 2312 byte)
+#define RSR_IVLDLEN         0x10        /* invalid len (> 2312 byte) */
 #define RSR_BSSIDOK         0x08
 #define RSR_CRCOK           0x04
 #define RSR_BCNSSIDOK       0x02
 #define RSR_ADDROK          0x01
 
-//
-// Bits in the new RSR register
-//
+/* Bits in the new RSR register */
 #define NEWRSR_DECRYPTOK    0x10
 #define NEWRSR_CFPIND       0x08
 #define NEWRSR_HWUTSF       0x04
 #define NEWRSR_BCNHITAID    0x02
 #define NEWRSR_BCNHITAID0   0x01
 
-//
-// Bits in the TSR0 register
-//
+/* Bits in the TSR0 register */
 #define TSR0_PWRSTS1_2      0xC0
 #define TSR0_PWRSTS7        0x20
 #define TSR0_NCR            0x1F
 
-//
-// Bits in the TSR1 register
-//
+/* Bits in the TSR1 register */
 #define TSR1_TERR           0x80
 #define TSR1_PWRSTS4_6      0x70
 #define TSR1_RETRYTMO       0x08
@@ -79,16 +69,14 @@
 #define TSR1_PWRSTS3        0x02
 #define ACK_DATA            0x01
 
-//
-// Bits in the TCR register
-//
-#define EDMSDU              0x04        // end of sdu
-#define TCR_EDP             0x02        // end of packet
-#define TCR_STP             0x01        // start of packet
+/* Bits in the TCR register */
+#define EDMSDU              0x04        /* end of sdu */
+#define TCR_EDP             0x02        /* end of packet */
+#define TCR_STP             0x01        /* start of packet */
 
-// max transmit or receive buffer size
+/* max transmit or receive buffer size */
 #define CB_MAX_BUF_SIZE     2900U
-					// NOTE: must be multiple of 4
+					/* NOTE: must be multiple of 4 */
 #define CB_MAX_TX_BUF_SIZE          CB_MAX_BUF_SIZE
 #define CB_MAX_RX_BUF_SIZE_NORMAL   CB_MAX_BUF_SIZE
 
@@ -100,18 +88,21 @@
 #define CB_MIN_TX_DESC      16
 
 #define CB_MAX_RECEIVED_PACKETS     16
-					// limit our receive routine to indicating
-					// this many at a time for 2 reasons:
-					// 1. driver flow control to protocol layer
-					// 2. limit the time used in ISR routine
+				/*
+				 * limit our receive routine to indicating
+				 * this many at a time for 2 reasons:
+				 * 1. driver flow control to protocol layer
+				 * 2. limit the time used in ISR routine
+				 */
 
 #define CB_EXTRA_RD_NUM     32
 #define CB_RD_NUM           32
 #define CB_TD_NUM           32
 
-// max number of physical segments
-// in a single NDIS packet. Above this threshold, the packet
-// is copied into a single physically contiguous buffer
+/*
+ * max number of physical segments in a single NDIS packet. Above this
+ * threshold, the packet is copied into a single physically contiguous buffer
+ */
 #define CB_MAX_SEGMENT      4
 
 #define CB_MIN_MAP_REG_NUM  4
@@ -119,42 +110,13 @@
 
 #define CB_PROTOCOL_RESERVED_SECTION    16
 
-// if retrys excess 15 times , tx will abort, and
-// if tx fifo underflow, tx will fail
-// we should try to resend it
+/*
+ * if retrys excess 15 times , tx will abort, and if tx fifo underflow,
+ * tx will fail, we should try to resend it
+ */
 #define CB_MAX_TX_ABORT_RETRY   3
 
-#ifdef __BIG_ENDIAN
-
-// WMAC definition FIFO Control
-#define FIFOCTL_AUTO_FB_1   0x0010
-#define FIFOCTL_AUTO_FB_0   0x0008
-#define FIFOCTL_GRPACK      0x0004
-#define FIFOCTL_11GA        0x0003
-#define FIFOCTL_11GB        0x0002
-#define FIFOCTL_11B         0x0001
-#define FIFOCTL_11A         0x0000
-#define FIFOCTL_RTS         0x8000
-#define FIFOCTL_ISDMA0      0x4000
-#define FIFOCTL_GENINT      0x2000
-#define FIFOCTL_TMOEN       0x1000
-#define FIFOCTL_LRETRY      0x0800
-#define FIFOCTL_CRCDIS      0x0400
-#define FIFOCTL_NEEDACK     0x0200
-#define FIFOCTL_LHEAD       0x0100
-
-//WMAC definition Frag Control
-#define FRAGCTL_AES         0x0003
-#define FRAGCTL_TKIP        0x0002
-#define FRAGCTL_LEGACY      0x0001
-#define FRAGCTL_NONENCRYPT  0x0000
-#define FRAGCTL_ENDFRAG     0x0300
-#define FRAGCTL_MIDFRAG     0x0200
-#define FRAGCTL_STAFRAG     0x0100
-#define FRAGCTL_NONFRAG     0x0000
-
-#else
-
+/* WMAC definition FIFO Control */
 #define FIFOCTL_AUTO_FB_1   0x1000
 #define FIFOCTL_AUTO_FB_0   0x0800
 #define FIFOCTL_GRPACK      0x0400
@@ -171,7 +133,7 @@
 #define FIFOCTL_NEEDACK     0x0002
 #define FIFOCTL_LHEAD       0x0001
 
-//WMAC definition Frag Control
+/* WMAC definition Frag Control */
 #define FRAGCTL_AES         0x0300
 #define FRAGCTL_TKIP        0x0200
 #define FRAGCTL_LEGACY      0x0100
@@ -181,8 +143,6 @@
 #define FRAGCTL_STAFRAG     0x0001
 #define FRAGCTL_NONFRAG     0x0000
 
-#endif
-
 #define TYPE_TXDMA0     0
 #define TYPE_AC0DMA     1
 #define TYPE_ATIMDMA    2
@@ -195,14 +155,17 @@
 #define TYPE_RXDMA1     1
 #define TYPE_MAXRD      2
 
-// TD_INFO flags control bit
-#define TD_FLAGS_NETIF_SKB               0x01       // check if need release skb
-#define TD_FLAGS_PRIV_SKB                0x02       // check if called from private skb(hostap)
-#define TD_FLAGS_PS_RETRY                0x04       // check if PS STA frame re-transmit
+/* TD_INFO flags control bit */
+#define TD_FLAGS_NETIF_SKB      0x01    /* check if need release skb */
+#define TD_FLAGS_PRIV_SKB       0x02    /* check if called from private skb (hostap) */
+#define TD_FLAGS_PS_RETRY       0x04    /* check if PS STA frame re-transmit */
 
-// ref_sk_buff is used for mapping the skb structure between pre-built driver-obj & running kernel.
-// Since different kernel version (2.4x) may change skb structure, i.e. pre-built driver-obj
-// may link to older skb that leads error.
+/*
+ * ref_sk_buff is used for mapping the skb structure between pre-built
+ * driver-obj & running kernel. Since different kernel version (2.4x) may
+ * change skb structure, i.e. pre-built driver-obj may link to older skb that
+ * leads error.
+ */
 
 typedef struct tagDEVICE_RD_INFO {
 	struct sk_buff *skb;
@@ -242,9 +205,7 @@ typedef struct tagRDES1 {
 } __attribute__ ((__packed__))
 SRDES1;
 
-//
-// Rx descriptor
-//
+/* Rx descriptor*/
 typedef struct tagSRxDesc {
 	volatile SRDES0 m_rd0RD0;
 	volatile SRDES1 m_rd1RD1;
@@ -292,6 +253,7 @@ typedef struct tagTDES1 {
 STDES1;
 
 typedef struct tagDEVICE_TD_INFO {
+	void *mic_hdr;
 	struct sk_buff *skb;
 	unsigned char *buf;
 	dma_addr_t          skb_dma;
@@ -302,9 +264,7 @@ typedef struct tagDEVICE_TD_INFO {
 	unsigned char byFlags;
 } DEVICE_TD_INFO,    *PDEVICE_TD_INFO;
 
-//
-// transmit descriptor
-//
+/* transmit descriptor */
 typedef struct tagSTxDesc {
 	volatile    STDES0  m_td0TD0;
 	volatile    STDES1  m_td1TD1;
@@ -319,8 +279,8 @@ typedef const STxDesc *PCSTxDesc;
 typedef struct tagSTxSyncDesc {
 	volatile    STDES0  m_td0TD0;
 	volatile    STDES1  m_td1TD1;
-	volatile    u32 buff_addr; // pointer to logical buffer
-	volatile    u32 next_desc; // pointer to next logical descriptor
+	volatile    u32 buff_addr; /* pointer to logical buffer */
+	volatile    u32 next_desc; /* pointer to next logical descriptor */
 	volatile    unsigned short m_wFIFOCtl;
 	volatile    unsigned short m_wTimeStamp;
 	struct tagSTxSyncDesc *next __aligned(8);
@@ -329,9 +289,7 @@ typedef struct tagSTxSyncDesc {
 STxSyncDesc, *PSTxSyncDesc;
 typedef const STxSyncDesc *PCSTxSyncDesc;
 
-//
-// RsvTime buffer header
-//
+/* RsvTime buffer header */
 typedef struct tagSRrvTime_atim {
 	unsigned short wCTSTxRrvTime_ba;
 	unsigned short wTxRrvTime_a;
@@ -352,9 +310,7 @@ union vnt_phy_field_swap {
 	u32 field_write;
 };
 
-//
-// Tx FIFO header
-//
+/* Tx FIFO header */
 typedef struct tagSTxBufHead {
 	u32 adwTxKey[4];
 	unsigned short wFIFOCtl;
@@ -392,4 +348,4 @@ typedef struct tagSKeyEntry {
 } __attribute__ ((__packed__))
 SKeyEntry;
 
-#endif // __DESC_H__
+#endif /* __DESC_H__ */
diff --git a/drivers/staging/vt6655/device.h b/drivers/staging/vt6655/device.h
index ddd356aa7eaf..83efbfb57c79 100644
--- a/drivers/staging/vt6655/device.h
+++ b/drivers/staging/vt6655/device.h
@@ -50,40 +50,47 @@
 #include <linux/io.h>
 #include <linux/if.h>
 #include <linux/crc32.h>
-//#include <linux/config.h>
 #include <linux/uaccess.h>
 #include <linux/proc_fs.h>
 #include <linux/inetdevice.h>
 #include <linux/reboot.h>
 #include <linux/ethtool.h>
 /* Include Wireless Extension definition and check version - Jean II */
+#include <net/mac80211.h>
 #include <linux/wireless.h>
-#include <net/iw_handler.h>	// New driver API
+#include <net/iw_handler.h>	/* New driver API */
 
-//2008-0409-07, <Add> by Einsn Liu
 #ifndef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
 #define WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
 #endif
 
-//
-// device specific
-//
+/* device specific */
 
 #include "device_cfg.h"
-#include "ttype.h"
-#include "80211hdr.h"
-#include "tether.h"
-#include "wmgr.h"
-#include "wcmd.h"
+#include "card.h"
 #include "mib.h"
 #include "srom.h"
-#include "rc4.h"
 #include "desc.h"
 #include "key.h"
 #include "mac.h"
 
 /*---------------------  Export Definitions -------------------------*/
 
+#define RATE_1M		0
+#define RATE_2M		1
+#define RATE_5M		2
+#define RATE_11M	3
+#define RATE_6M		4
+#define RATE_9M		5
+#define RATE_12M	6
+#define RATE_18M	7
+#define RATE_24M	8
+#define RATE_36M	9
+#define RATE_48M	10
+#define RATE_54M	11
+#define RATE_AUTO	12
+#define MAX_RATE	12
+
 #define MAC_MAX_CONTEXT_REG     (256+128)
 
 #define MAX_MULTICAST_ADDRESS_NUM       32
@@ -112,7 +119,7 @@
 #define FB_RATE0                0
 #define FB_RATE1                1
 
-// Antenna Mode
+/* Antenna Mode */
 #define ANT_A                   0
 #define ANT_B                   1
 #define ANT_DIVERSITY           2
@@ -129,120 +136,28 @@
 #define RUN_AT(x)                       (jiffies+(x))
 #endif
 
-// DMA related
+#define MAKE_BEACON_RESERVED	10  /* (us) */
+
+/* DMA related */
 #define RESERV_AC0DMA                   4
 
-// BUILD OBJ mode
+/* BUILD OBJ mode */
 
 #define	AVAIL_TD(p, q)	((p)->sOpts.nTxDescs[(q)] - ((p)->iTDUsed[(q)]))
 
 #define	NUM				64
 
-#define PRIVATE_Message                 0
-
-/*---------------------  Export Types  ------------------------------*/
-
-#define PRINT_K(p, args...)		\
-do {					\
-	if (PRIVATE_Message)		\
-		printk(p, ##args);	\
-} while (0)
+/* 0:11A 1:11B 2:11G */
+#define BB_TYPE_11A    0
+#define BB_TYPE_11B    1
+#define BB_TYPE_11G    2
 
-//0:11A 1:11B 2:11G
-typedef enum _VIA_BB_TYPE
-{
-	BB_TYPE_11A = 0,
-	BB_TYPE_11B,
-	BB_TYPE_11G
-} VIA_BB_TYPE, *PVIA_BB_TYPE;
-
-//0:11a,1:11b,2:11gb(only CCK in BasicRate),3:11ga(OFDM in Basic Rate)
-typedef enum _VIA_PKT_TYPE
-{
-	PK_TYPE_11A = 0,
-	PK_TYPE_11B,
-	PK_TYPE_11GB,
-	PK_TYPE_11GA
-} VIA_PKT_TYPE, *PVIA_PKT_TYPE;
-
-typedef enum __device_msg_level {
-	MSG_LEVEL_ERR = 0,            //Errors that will cause abnormal operation.
-	MSG_LEVEL_NOTICE = 1,         //Some errors need users to be notified.
-	MSG_LEVEL_INFO = 2,           //Normal message.
-	MSG_LEVEL_VERBOSE = 3,        //Will report all trival errors.
-	MSG_LEVEL_DEBUG = 4           //Only for debug purpose.
-} DEVICE_MSG_LEVEL, *PDEVICE_MSG_LEVEL;
-
-//++ NDIS related
-
-#define MAX_BSSIDINFO_4_PMKID   16
-#define MAX_PMKIDLIST           5
-//Flags for PMKID Candidate list structure
-#define NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED	0x01
-
-// PMKID Structures
-typedef unsigned char NDIS_802_11_PMKID_VALUE[16];
-
-typedef enum _NDIS_802_11_WEP_STATUS {
-	Ndis802_11WEPEnabled,
-	Ndis802_11Encryption1Enabled = Ndis802_11WEPEnabled,
-	Ndis802_11WEPDisabled,
-	Ndis802_11EncryptionDisabled = Ndis802_11WEPDisabled,
-	Ndis802_11WEPKeyAbsent,
-	Ndis802_11Encryption1KeyAbsent = Ndis802_11WEPKeyAbsent,
-	Ndis802_11WEPNotSupported,
-	Ndis802_11EncryptionNotSupported = Ndis802_11WEPNotSupported,
-	Ndis802_11Encryption2Enabled,
-	Ndis802_11Encryption2KeyAbsent,
-	Ndis802_11Encryption3Enabled,
-	Ndis802_11Encryption3KeyAbsent
-} NDIS_802_11_WEP_STATUS, *PNDIS_802_11_WEP_STATUS,
-	NDIS_802_11_ENCRYPTION_STATUS, *PNDIS_802_11_ENCRYPTION_STATUS;
-
-typedef enum _NDIS_802_11_STATUS_TYPE {
-	Ndis802_11StatusType_Authentication,
-	Ndis802_11StatusType_MediaStreamMode,
-	Ndis802_11StatusType_PMKID_CandidateList,
-	Ndis802_11StatusTypeMax    // not a real type, defined as an upper bound
-} NDIS_802_11_STATUS_TYPE, *PNDIS_802_11_STATUS_TYPE;
-
-//Added new types for PMKID Candidate lists.
-struct pmkid_candidate {
-	NDIS_802_11_MAC_ADDRESS BSSID;
-	unsigned long Flags;
-};
+/* 0:11a, 1:11b, 2:11gb (only CCK in BasicRate), 3:11ga (OFDM in BasicRate) */
+#define PK_TYPE_11A     0
+#define PK_TYPE_11B     1
+#define PK_TYPE_11GB    2
+#define PK_TYPE_11GA    3
 
-typedef struct _BSSID_INFO {
-	NDIS_802_11_MAC_ADDRESS BSSID;
-	NDIS_802_11_PMKID_VALUE PMKID;
-} BSSID_INFO, *PBSSID_INFO;
-
-typedef struct tagSPMKID {
-	unsigned long Length;
-	unsigned long BSSIDInfoCount;
-	BSSID_INFO BSSIDInfo[MAX_BSSIDINFO_4_PMKID];
-} SPMKID, *PSPMKID;
-
-typedef struct tagSPMKIDCandidateEvent {
-	NDIS_802_11_STATUS_TYPE     StatusType;
-	unsigned long Version;       // Version of the structure
-	unsigned long NumCandidates; // No. of pmkid candidates
-	struct pmkid_candidate CandidateList[MAX_PMKIDLIST];
-} SPMKIDCandidateEvent, *PSPMKIDCandidateEvent;
-
-//--
-
-//++ 802.11h related
-#define MAX_QUIET_COUNT     8
-
-typedef struct tagSQuietControl {
-	bool bEnable;
-	unsigned long dwStartTime;
-	unsigned char byPeriod;
-	unsigned short wDuration;
-} SQuietControl, *PSQuietControl;
-
-//--
 typedef struct __chip_info_tbl {
 	CHIP_TYPE   chip_id;
 	char *name;
@@ -256,34 +171,7 @@ typedef enum {
 	OWNED_BY_NIC = 1
 } DEVICE_OWNER_TYPE, *PDEVICE_OWNER_TYPE;
 
-// The receive duplicate detection cache entry
-typedef struct tagSCacheEntry {
-	unsigned short wFmSequence;
-	unsigned char abyAddr2[ETH_ALEN];
-} SCacheEntry, *PSCacheEntry;
-
-typedef struct tagSCache {
-/* The receive cache is updated circularly.  The next entry to be written is
- * indexed by the "InPtr".
- */
-	unsigned int uInPtr;         // Place to use next
-	SCacheEntry     asCacheEntry[DUPLICATE_RX_CACHE_LENGTH];
-} SCache, *PSCache;
-
-#define CB_MAX_RX_FRAG                 64
-// DeFragment Control Block, used for collecting fragments prior to reassembly
-typedef struct tagSDeFragControlBlock {
-	unsigned short wSequence;
-	unsigned short wFragNum;
-	unsigned char abyAddr2[ETH_ALEN];
-	unsigned int uLifetime;
-	struct sk_buff *skb;
-	unsigned char *pbyRxBuffer;
-	unsigned int cbFrameLength;
-	bool bInUse;
-} SDeFragControlBlock, *PSDeFragControlBlock;
-
-//flags for options
+/* flags for options */
 #define     DEVICE_FLAGS_IP_ALIGN        0x00000001UL
 #define     DEVICE_FLAGS_PREAMBLE_TYPE   0x00000002UL
 #define     DEVICE_FLAGS_OP_MODE         0x00000004UL
@@ -291,15 +179,15 @@ typedef struct tagSDeFragControlBlock {
 #define		DEVICE_FLAGS_80211h_MODE	 0x00000010UL
 #define		DEVICE_FLAGS_DiversityANT	 0x00000020UL
 
-//flags for driver status
+/* flags for driver status */
 #define     DEVICE_FLAGS_OPENED          0x00010000UL
 #define     DEVICE_FLAGS_WOL_ENABLED     0x00080000UL
-//flags for capabilities
+/* flags for capabilities */
 #define     DEVICE_FLAGS_TX_ALIGN        0x01000000UL
 #define     DEVICE_FLAGS_HAVE_CAM        0x02000000UL
 #define     DEVICE_FLAGS_FLOW_CTRL       0x04000000UL
 
-//flags for MII status
+/* flags for MII status */
 #define     DEVICE_LINK_FAIL             0x00000001UL
 #define     DEVICE_SPEED_10              0x00000002UL
 #define     DEVICE_SPEED_100             0x00000004UL
@@ -307,18 +195,14 @@ typedef struct tagSDeFragControlBlock {
 #define     DEVICE_DUPLEX_FULL           0x00000010UL
 #define     DEVICE_AUTONEG_ENABLE        0x00000020UL
 #define     DEVICE_FORCED_BY_EEPROM      0x00000040UL
-//for device_set_media_duplex
+/* for device_set_media_duplex */
 #define     DEVICE_LINK_CHANGE           0x00000001UL
 
 typedef struct __device_opt {
-	int         nRxDescs0;    //Number of RX descriptors0
-	int         nRxDescs1;    //Number of RX descriptors1
-	int         nTxDescs[2];  //Number of TX descriptors 0, 1
-	int         int_works;    //interrupt limits
-	int         rts_thresh;   //rts threshold
-	int         frag_thresh;
-	int         data_rate;
-	int         channel_num;
+	int         nRxDescs0;		/* Number of RX descriptors0 */
+	int         nRxDescs1;		/* Number of RX descriptors1 */
+	int         nTxDescs[2];	/* Number of TX descriptors 0, 1 */
+	int         int_works;		/* interrupt limits */
 	int         short_retry;
 	int         long_retry;
 	int         bbp_type;
@@ -327,11 +211,16 @@ typedef struct __device_opt {
 
 struct vnt_private {
 	struct pci_dev *pcid;
-
-// netdev
-	struct net_device *dev;
-
-//dma addr, rx/tx pool
+	/* mac80211 */
+	struct ieee80211_hw *hw;
+	struct ieee80211_vif *vif;
+	unsigned long key_entry_inuse;
+	u32 basic_rates;
+	u16 current_aid;
+	int mc_list_count;
+	u8 mac_hw;
+
+/* dma addr, rx/tx pool */
 	dma_addr_t                  pool_dma;
 	dma_addr_t                  rd0_pool_dma;
 	dma_addr_t                  rd1_pool_dma;
@@ -356,9 +245,12 @@ struct vnt_private {
 	u32                         io_size;
 
 	unsigned char byRevId;
+	unsigned char byRxMode;
 	unsigned short SubSystemID;
 	unsigned short SubVendorID;
 
+	spinlock_t                  lock;
+
 	int                         nTxQueues;
 	volatile int                iTDUsed[TYPE_MAXTD];
 
@@ -371,30 +263,18 @@ struct vnt_private {
 	volatile PSRxDesc           aRD0Ring;
 	volatile PSRxDesc           aRD1Ring;
 	volatile PSRxDesc           pCurrRD[TYPE_MAXRD];
-	SCache                      sDupRxCache;
-
-	SDeFragControlBlock         sRxDFCB[CB_MAX_RX_FRAG];
-	unsigned int	cbDFCB;
-	unsigned int	cbFreeDFCB;
-	unsigned int	uCurrentDFCBIdx;
 
 	OPTIONS                     sOpts;
 
 	u32                         flags;
 
 	u32                         rx_buf_sz;
+	u8 rx_rate;
 	int                         multicast_limit;
-	unsigned char byRxMode;
-
-	spinlock_t                  lock;
-
-	pid_t			MLMEThr_pid;
-	struct completion	notify;
-	struct semaphore	mlme_semaphore;
 
 	u32                         rx_bytes;
 
-	// Version control
+	/* Version control */
 	unsigned char byLocalID;
 	unsigned char byRFType;
 
@@ -402,20 +282,15 @@ struct vnt_private {
 	unsigned char byZoneType;
 	bool bZoneRegExist;
 	unsigned char byOriginalZonetype;
-	unsigned char abyMacContext[MAC_MAX_CONTEXT_REG];
-	bool bLinkPass;          // link status: OK or fail
-	unsigned char abyCurrentNetAddr[ETH_ALEN];
 
-	// Adapter statistics
+	unsigned char abyCurrentNetAddr[ETH_ALEN]; __aligned(2)
+	bool bLinkPass;          /* link status: OK or fail */
+
+	/* Adapter statistics */
 	SStatCounter                scStatistic;
-	// 802.11 counter
+	/* 802.11 counter */
 	SDot11Counters              s802_11Counter;
 
-	// 802.11 management
-	PSMgmtObject                pMgmt;
-	SMgmtObject                 sMgmtObj;
-
-	// 802.11 MAC specific
 	unsigned int	uCurrRSSI;
 	unsigned char byCurrSQ;
 
@@ -427,22 +302,25 @@ struct vnt_private {
 	bool bTxRxAntInv;
 
 	unsigned char *pbyTmpBuff;
-	unsigned int	uSIFS;    //Current SIFS
-	unsigned int	uDIFS;    //Current DIFS
-	unsigned int	uEIFS;    //Current EIFS
-	unsigned int	uSlot;    //Current SlotTime
-	unsigned int	uCwMin;   //Current CwMin
-	unsigned int	uCwMax;   //CwMax is fixed on 1023.
-	// PHY parameter
+	unsigned int	uSIFS;    /* Current SIFS */
+	unsigned int	uDIFS;    /* Current DIFS */
+	unsigned int	uEIFS;    /* Current EIFS */
+	unsigned int	uSlot;    /* Current SlotTime */
+	unsigned int	uCwMin;   /* Current CwMin */
+	unsigned int	uCwMax;   /* CwMax is fixed on 1023. */
+	/* PHY parameter */
 	unsigned char bySIFS;
 	unsigned char byDIFS;
 	unsigned char byEIFS;
 	unsigned char bySlot;
 	unsigned char byCWMaxMin;
-	CARD_PHY_TYPE               eCurrentPHYType;
 
-	VIA_BB_TYPE                 byBBType; //0: 11A, 1:11B, 2:11G
-	VIA_PKT_TYPE                byPacketType; //0:11a,1:11b,2:11gb(only CCK in BasicRate),3:11ga(OFDM in Basic Rate)
+	u8		byBBType; /* 0:11A, 1:11B, 2:11G */
+	u8		byPacketType; /*
+				       * 0:11a,1:11b,2:11gb (only CCK
+				       * in BasicRate), 3:11ga (OFDM in
+				       * Basic Rate)
+				       */
 	unsigned short wBasicRate;
 	unsigned char byACKRate;
 	unsigned char byTopOFDMBasicRate;
@@ -450,28 +328,16 @@ struct vnt_private {
 
 	unsigned char byMinChannel;
 	unsigned char byMaxChannel;
-	unsigned int	uConnectionRate;
 
 	unsigned char byPreambleType;
 	unsigned char byShortPreamble;
 
 	unsigned short wCurrentRate;
-	unsigned short wRTSThreshold;
-	unsigned short wFragmentationThreshold;
 	unsigned char byShortRetryLimit;
 	unsigned char byLongRetryLimit;
 	enum nl80211_iftype op_mode;
-	unsigned char byOpMode;
 	bool bBSSIDFilter;
 	unsigned short wMaxTransmitMSDULifetime;
-	unsigned char abyBSSID[ETH_ALEN];
-	unsigned char abyDesireBSSID[ETH_ALEN];
-	unsigned short wACKDuration;       // update while speed change
-	unsigned short wRTSTransmitLen;    // update while speed change
-	unsigned char byRTSServiceField;  // update while speed change
-	unsigned char byRTSSignalField;   // update while speed change
-
-	unsigned long dwMaxReceiveLifetime;       // dot11MaxReceiveLifetime
 
 	bool bEncryptionEnable;
 	bool bLongHeader;
@@ -480,24 +346,20 @@ struct vnt_private {
 	bool bNonERPPresent;
 	bool bBarkerPreambleMd;
 
-	unsigned char byERPFlag;
-	unsigned short wUseProtectCntDown;
-
 	bool bRadioControlOff;
 	bool bRadioOff;
 	bool bEnablePSMode;
 	unsigned short wListenInterval;
 	bool bPWBitOn;
-	WMAC_POWER_MODE         ePSMode;
 
-	// GPIO Radio Control
+	/* GPIO Radio Control */
 	unsigned char byRadioCtl;
 	unsigned char byGPIO;
 	bool bHWRadioOff;
 	bool bPrvActive4RadioOFF;
 	bool bGPIOBlockRead;
 
-	// Beacon related
+	/* Beacon related */
 	unsigned short wSeqCounter;
 	unsigned short wBCNBufLen;
 	bool bBeaconBufReady;
@@ -506,71 +368,12 @@ struct vnt_private {
 	unsigned int	cbBeaconBufReadySetCnt;
 	bool bFixRate;
 	unsigned char byCurrentCh;
-	unsigned int	uScanTime;
-
-	CMD_STATE               eCommandState;
-
-	CMD_CODE                eCommand;
-	bool bBeaconTx;
-
-	bool bStopBeacon;
-	bool bStopDataPkt;
-	bool bStopTx0Pkt;
-	unsigned int	uAutoReConnectTime;
-
-	// 802.11 counter
-
-	CMD_ITEM                eCmdQueue[CMD_Q_SIZE];
-	unsigned int	uCmdDequeueIdx;
-	unsigned int	uCmdEnqueueIdx;
-	unsigned int	cbFreeCmdQueue;
-	bool bCmdRunning;
-	bool bCmdClear;
-
-	bool bRoaming;
-	//WOW
-	unsigned char abyIPAddr[4];
-
-	unsigned long ulTxPower;
-	NDIS_802_11_WEP_STATUS  eEncryptionStatus;
-	bool bTransmitKey;
-//2007-0925-01<Add>by MikeLiu
-//mike add :save old Encryption
-	NDIS_802_11_WEP_STATUS  eOldEncryptionStatus;
-
-	SKeyManagement          sKey;
-	unsigned long dwIVCounter;
-
-	u64 qwPacketNumber; /* For CCMP and TKIP as TSC(6 bytes) */
-	unsigned int	uCurrentWEPMode;
-
-	RC4Ext                  SBox;
-	unsigned char abyPRNG[WLAN_WEPMAX_KEYLEN+3];
-	unsigned char byKeyIndex;
-	unsigned int	uKeyLength;
-	unsigned char abyKey[WLAN_WEP232_KEYLEN];
 
 	bool bAES;
-	unsigned char byCntMeasure;
-
-	// for AP mode
-	unsigned int	uAssocCount;
-	bool bMoreData;
-
-	// QoS
-	bool bGrpAckPolicy;
-
-	// for OID_802_11_ASSOCIATION_INFORMATION
-	bool bAssocInfoSet;
 
 	unsigned char byAutoFBCtrl;
 
-	bool bTxMICFail;
-	bool bRxMICFail;
-
-	unsigned int	uRATEIdx;
-
-	// For Update BaseBand VGA Gain Offset
+	/* For Update BaseBand VGA Gain Offset */
 	bool bUpdateBBVGA;
 	unsigned int	uBBVGADiffCount;
 	unsigned char byBBVGANew;
@@ -581,24 +384,12 @@ struct vnt_private {
 	unsigned char byBBPreEDRSSI;
 	unsigned char byBBPreEDIndex;
 
-	bool bRadioCmd;
 	unsigned long dwDiagRefCount;
 
-	// For FOE Tuning
+	/* For FOE Tuning */
 	unsigned char byFOETuning;
 
-	// For Auto Power Tunning
-
-	unsigned char byAutoPwrTunning;
-	short                   sPSetPointCCK;
-	short                   sPSetPointOFDMG;
-	short                   sPSetPointOFDMA;
-	long                    lPFormulaOffset;
-	short                   sPThreshold;
-	char                    cAdjustStep;
-	char                    cMinTxAGC;
-
-	// For RF Power table
+	/* For RF Power table */
 	unsigned char byCCKPwr;
 	unsigned char byOFDMPwrG;
 	unsigned char byCurPwr;
@@ -610,27 +401,12 @@ struct vnt_private {
 	char	abyRegPwr[CB_MAX_CHANNEL+1];
 	char	abyLocalPwr[CB_MAX_CHANNEL+1];
 
-	// BaseBand Loopback Use
+	/* BaseBand Loopback Use */
 	unsigned char byBBCR4d;
 	unsigned char byBBCRc9;
 	unsigned char byBBCR88;
 	unsigned char byBBCR09;
 
-	// command timer
-	struct timer_list       sTimerCommand;
-	struct timer_list       sTimerTxData;
-	unsigned long nTxDataTimeCout;
-	bool fTxDataInSleep;
-	bool IsTxDataTrigger;
-
-#ifdef WPA_SM_Transtatus
-	bool fWPA_Authened;           //is WPA/WPA-PSK or WPA2/WPA2-PSK authen??
-#endif
-	unsigned char byReAssocCount;   //mike add:re-association retry times!
-	unsigned char byLinkWaitCount;
-
-	unsigned char abyNodeName[17];
-
 	bool bDiversityRegCtlON;
 	bool bDiversityEnable;
 	unsigned long ulDiversityNValue;
@@ -640,13 +416,13 @@ struct vnt_private {
 	unsigned char byTMax3;
 	unsigned long ulSQ3TH;
 
-// ANT diversity
+	/* ANT diversity */
 	unsigned long uDiversityCnt;
 	unsigned char byAntennaState;
 	unsigned long ulRatio_State0;
 	unsigned long ulRatio_State1;
 
-	//SQ3 functions for antenna diversity
+	/* SQ3 functions for antenna diversity */
 	struct timer_list           TimerSQ3Tmax1;
 	struct timer_list           TimerSQ3Tmax2;
 	struct timer_list           TimerSQ3Tmax3;
@@ -654,86 +430,11 @@ struct vnt_private {
 	unsigned long uNumSQ3[MAX_RATE];
 	unsigned short wAntDiversityMaxRate;
 
-	SEthernetHeader         sTxEthHeader;
-	SEthernetHeader         sRxEthHeader;
-	unsigned char abyBroadcastAddr[ETH_ALEN];
-	unsigned char abySNAP_RFC1042[ETH_ALEN];
-	unsigned char abySNAP_Bridgetunnel[ETH_ALEN];
-	unsigned char abyEEPROM[EEP_MAX_CONTEXT_SIZE];  //unsigned long alignment
-	// Pre-Authentication & PMK cache
-	SPMKID                  gsPMKID;
-	SPMKIDCandidateEvent    gsPMKIDCandidate;
-
-	// for 802.11h
-	bool b11hEnable;
-	unsigned char abyCountryCode[3];
-	// for 802.11h DFS
-	unsigned int	uNumOfMeasureEIDs;
-	PWLAN_IE_MEASURE_REQ    pCurrMeasureEID;
-	bool bMeasureInProgress;
-	unsigned char byOrgChannel;
-	unsigned char byOrgRCR;
-	unsigned long dwOrgMAR0;
-	unsigned long dwOrgMAR4;
-	unsigned char byBasicMap;
-	unsigned char byCCAFraction;
-	unsigned char abyRPIs[8];
-	unsigned long dwRPIs[8];
-	bool bChannelSwitch;
-	unsigned char byNewChannel;
-	unsigned char byChannelSwitchCount;
-	bool bQuietEnable;
-	bool bEnableFirstQuiet;
-	unsigned char byQuietStartCount;
-	unsigned int	uQuietEnqueue;
-	unsigned long dwCurrentQuietEndTime;
-	SQuietControl           sQuiet[MAX_QUIET_COUNT];
-	// for 802.11h TPC
-	bool bCountryInfo5G;
-	bool bCountryInfo24G;
+	unsigned char abyEEPROM[EEP_MAX_CONTEXT_SIZE]; /* unsigned long alignment */
 
 	unsigned short wBeaconInterval;
-
-	//WPA supplicant deamon
-	struct net_device       *wpadev;
-	bool bWPADEVUp;
-	struct sk_buff          *skb;
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-	unsigned int	bwextcount;
-	bool bWPASuppWextEnabled;
-#endif
-
-	//--
-#ifdef HOSTAP
-	// user space daemon: hostapd, is used for HOSTAP
-	bool bEnableHostapd;
-	bool bEnable8021x;
-	bool bEnableHostWEP;
-	struct net_device       *apdev;
-	int (*tx_80211)(struct sk_buff *skb, struct net_device *dev);
-#endif
-	unsigned int	uChannel;
-	bool bMACSuspend;
-
-	struct iw_statistics	wstats;		// wireless stats
-	bool bCommit;
 };
 
-static inline bool device_get_ip(struct vnt_private *pInfo)
-{
-	struct in_device *in_dev = (struct in_device *)pInfo->dev->ip_ptr;
-	struct in_ifaddr *ifa;
-
-	if (in_dev != NULL) {
-		ifa = (struct in_ifaddr *)in_dev->ifa_list;
-		if (ifa != NULL) {
-			memcpy(pInfo->abyIPAddr, &ifa->ifa_address, 4);
-			return true;
-		}
-	}
-	return false;
-}
-
 static inline PDEVICE_RD_INFO alloc_rd_info(void)
 {
 	return kzalloc(sizeof(DEVICE_RD_INFO), GFP_ATOMIC);
@@ -743,13 +444,4 @@ static inline PDEVICE_TD_INFO alloc_td_info(void)
 {
 	return kzalloc(sizeof(DEVICE_TD_INFO), GFP_ATOMIC);
 }
-
-/*---------------------  Export Functions  --------------------------*/
-
-bool device_dma0_xmit(struct vnt_private *pDevice,
-		      struct sk_buff *skb, unsigned int uNodeIndex);
-bool device_alloc_frag_buf(struct vnt_private *pDevice,
-			   PSDeFragControlBlock pDeF);
-int Config_FileOperation(struct vnt_private *pDevice,
-			 bool fwrite, unsigned char *Parameter);
 #endif
diff --git a/drivers/staging/vt6655/device_cfg.h b/drivers/staging/vt6655/device_cfg.h
index 7221824e4f23..a4a8a8489e0b 100644
--- a/drivers/staging/vt6655/device_cfg.h
+++ b/drivers/staging/vt6655/device_cfg.h
@@ -29,8 +29,6 @@
 
 #include <linux/types.h>
 
-#include "ttype.h"
-
 typedef
 struct _version {
 	unsigned char   major;
diff --git a/drivers/staging/vt6655/device_main.c b/drivers/staging/vt6655/device_main.c
index 54e16f40d8ed..83e4162c0094 100644
--- a/drivers/staging/vt6655/device_main.c
+++ b/drivers/staging/vt6655/device_main.c
@@ -32,27 +32,16 @@
  *   device_free_info - device structure resource free function
  *   device_get_pci_info - get allocated pci io/mem resource
  *   device_print_info - print out resource
- *   device_open - allocate dma/descripter resource & initial mac/bbp function
- *   device_xmit - asynchrous data tx function
  *   device_intr - interrupt handle function
- *   device_set_multi - set mac filter
- *   device_ioctl - ioctl entry
- *   device_close - shutdown mac/bbp & free dma/descripter resource
  *   device_rx_srv - rx service function
- *   device_receive_frame - rx data function
  *   device_alloc_rx_buf - rx buffer pre-allocated function
- *   device_alloc_frag_buf - rx fragement pre-allocated function
  *   device_free_tx_buf - free tx buffer function
- *   device_free_frag_buf- free de-fragement buffer
- *   device_dma0_tx_80211- tx 802.11 frame via dma0
- *   device_dma0_xmit- tx PS bufferred frame via dma0
  *   device_init_rd0_ring- initial rd dma0 ring
  *   device_init_rd1_ring- initial rd dma1 ring
  *   device_init_td0_ring- initial tx dma0 ring buffer
  *   device_init_td1_ring- initial tx dma1 ring buffer
  *   device_init_registers- initial MAC & BBP & RF internal registers.
  *   device_init_rings- initial tx/rx ring buffer
- *   device_init_defrag_cb- initial & allocate de-fragement buffer.
  *   device_free_rings- free all allocated ring buffer
  *   device_tx_srv- tx interrupt service function
  *
@@ -66,24 +55,10 @@
 #include "channel.h"
 #include "baseband.h"
 #include "mac.h"
-#include "tether.h"
-#include "wmgr.h"
-#include "wctl.h"
 #include "power.h"
-#include "wcmd.h"
-#include "iocmd.h"
-#include "tcrc.h"
 #include "rxtx.h"
-#include "wroute.h"
-#include "bssdb.h"
-#include "hostap.h"
-#include "wpactl.h"
-#include "ioctl.h"
-#include "iwctl.h"
 #include "dpc.h"
-#include "datarate.h"
 #include "rf.h"
-#include "iowpa.h"
 #include <linux/delay.h>
 #include <linux/kthread.h>
 #include <linux/slab.h>
@@ -118,89 +93,16 @@ DEVICE_PARAM(TxDescriptors0, "Number of transmit descriptors0");
 #define TX_DESC_DEF1     64
 DEVICE_PARAM(TxDescriptors1, "Number of transmit descriptors1");
 
-#define IP_ALIG_DEF     0
-/* IP_byte_align[] is used for IP header unsigned long byte aligned
-   0: indicate the IP header won't be unsigned long byte aligned.(Default) .
-   1: indicate the IP header will be unsigned long byte aligned.
-   In some environment, the IP header should be unsigned long byte aligned,
-   or the packet will be droped when we receive it. (eg: IPVS)
-*/
-DEVICE_PARAM(IP_byte_align, "Enable IP header dword aligned");
-
 #define INT_WORKS_DEF   20
 #define INT_WORKS_MIN   10
 #define INT_WORKS_MAX   64
 
 DEVICE_PARAM(int_works, "Number of packets per interrupt services");
 
-#define CHANNEL_MIN     1
-#define CHANNEL_MAX     14
-#define CHANNEL_DEF     6
-
-DEVICE_PARAM(Channel, "Channel number");
-
-/* PreambleType[] is the preamble length used for transmit.
-   0: indicate allows long preamble type
-   1: indicate allows short preamble type
-*/
-
-#define PREAMBLE_TYPE_DEF     1
-
-DEVICE_PARAM(PreambleType, "Preamble Type");
-
-#define RTS_THRESH_MIN     512
-#define RTS_THRESH_MAX     2347
 #define RTS_THRESH_DEF     2347
 
-DEVICE_PARAM(RTSThreshold, "RTS threshold");
-
-#define FRAG_THRESH_MIN     256
-#define FRAG_THRESH_MAX     2346
 #define FRAG_THRESH_DEF     2346
 
-DEVICE_PARAM(FragThreshold, "Fragmentation threshold");
-
-#define DATA_RATE_MIN     0
-#define DATA_RATE_MAX     13
-#define DATA_RATE_DEF     13
-/* datarate[] index
-   0: indicate 1 Mbps   0x02
-   1: indicate 2 Mbps   0x04
-   2: indicate 5.5 Mbps 0x0B
-   3: indicate 11 Mbps  0x16
-   4: indicate 6 Mbps   0x0c
-   5: indicate 9 Mbps   0x12
-   6: indicate 12 Mbps  0x18
-   7: indicate 18 Mbps  0x24
-   8: indicate 24 Mbps  0x30
-   9: indicate 36 Mbps  0x48
-   10: indicate 48 Mbps  0x60
-   11: indicate 54 Mbps  0x6c
-   12: indicate 72 Mbps  0x90
-   13: indicate auto rate
-*/
-
-DEVICE_PARAM(ConnectionRate, "Connection data rate");
-
-#define OP_MODE_DEF     0
-
-DEVICE_PARAM(OPMode, "Infrastruct, adhoc, AP mode ");
-
-/* OpMode[] is used for transmit.
-   0: indicate infrastruct mode used
-   1: indicate adhoc mode used
-   2: indicate AP mode used
-*/
-
-/* PSMode[]
-   0: indicate disable power saving mode
-   1: indicate enable power saving mode
-*/
-
-#define PS_MODE_DEF     0
-
-DEVICE_PARAM(PSMode, "Power saving mode");
-
 #define SHORT_RETRY_MIN     0
 #define SHORT_RETRY_MAX     31
 #define SHORT_RETRY_DEF     8
@@ -224,20 +126,6 @@ DEVICE_PARAM(LongRetryLimit, "long frame retry limits");
 
 DEVICE_PARAM(BasebandType, "baseband type");
 
-/* 80211hEnable[]
-   0: indicate disable 802.11h
-   1: indicate enable 802.11h
-*/
-
-#define X80211h_MODE_DEF     0
-
-DEVICE_PARAM(b80211hEnable, "802.11h mode");
-
-/* 80211hEnable[]
-   0: indicate disable 802.11h
-   1: indicate enable 802.11h
-*/
-
 #define DIVERSITY_ANT_DEF     0
 
 DEVICE_PARAM(bDiversityANTEnable, "ANT diversity mode");
@@ -265,17 +153,10 @@ static void device_free_info(struct vnt_private *pDevice);
 static bool device_get_pci_info(struct vnt_private *, struct pci_dev *pcid);
 static void device_print_info(struct vnt_private *pDevice);
 static void device_init_diversity_timer(struct vnt_private *pDevice);
-static int  device_open(struct net_device *dev);
-static int  device_xmit(struct sk_buff *skb, struct net_device *dev);
 static  irqreturn_t  device_intr(int irq,  void *dev_instance);
-static void device_set_multi(struct net_device *dev);
-static int  device_close(struct net_device *dev);
-static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
 
 #ifdef CONFIG_PM
 static int device_notify_reboot(struct notifier_block *, unsigned long event, void *ptr);
-static int viawget_suspend(struct pci_dev *pcid, pm_message_t state);
-static int viawget_resume(struct pci_dev *pcid);
 static struct notifier_block device_notifier = {
 	.notifier_call = device_notify_reboot,
 	.next = NULL,
@@ -285,15 +166,9 @@ static struct notifier_block device_notifier = {
 
 static void device_init_rd0_ring(struct vnt_private *pDevice);
 static void device_init_rd1_ring(struct vnt_private *pDevice);
-static void device_init_defrag_cb(struct vnt_private *pDevice);
 static void device_init_td0_ring(struct vnt_private *pDevice);
 static void device_init_td1_ring(struct vnt_private *pDevice);
 
-static int  device_dma0_tx_80211(struct sk_buff *skb, struct net_device *dev);
-//2008-0714<Add>by Mike Liu
-static bool device_release_WPADEV(struct vnt_private *pDevice);
-
-static int  ethtool_ioctl(struct net_device *dev, void __user *useraddr);
 static int  device_rx_srv(struct vnt_private *pDevice, unsigned int uIdx);
 static int  device_tx_srv(struct vnt_private *pDevice, unsigned int uIdx);
 static bool device_alloc_rx_buf(struct vnt_private *pDevice, PSRxDesc pDesc);
@@ -304,9 +179,6 @@ static void device_free_td1_ring(struct vnt_private *pDevice);
 static void device_free_rd0_ring(struct vnt_private *pDevice);
 static void device_free_rd1_ring(struct vnt_private *pDevice);
 static void device_free_rings(struct vnt_private *pDevice);
-static void device_free_frag_buf(struct vnt_private *pDevice);
-static int Config_FileGetParameter(unsigned char *string,
-				   unsigned char *dest, unsigned char *source);
 
 /*---------------------  Export Variables  --------------------------*/
 
@@ -339,124 +211,50 @@ static void device_get_options(struct vnt_private *pDevice)
 	pOpts->nRxDescs1 = RX_DESC_DEF1;
 	pOpts->nTxDescs[0] = TX_DESC_DEF0;
 	pOpts->nTxDescs[1] = TX_DESC_DEF1;
-	pOpts->flags |= DEVICE_FLAGS_IP_ALIGN;
 	pOpts->int_works = INT_WORKS_DEF;
-	pOpts->rts_thresh = RTS_THRESH_DEF;
-	pOpts->frag_thresh = FRAG_THRESH_DEF;
-	pOpts->data_rate = DATA_RATE_DEF;
-	pOpts->channel_num = CHANNEL_DEF;
 
-	pOpts->flags |= DEVICE_FLAGS_PREAMBLE_TYPE;
-	pOpts->flags |= DEVICE_FLAGS_OP_MODE;
 	pOpts->short_retry = SHORT_RETRY_DEF;
 	pOpts->long_retry = LONG_RETRY_DEF;
 	pOpts->bbp_type = BBP_TYPE_DEF;
-	pOpts->flags |= DEVICE_FLAGS_80211h_MODE;
 	pOpts->flags |= DEVICE_FLAGS_DiversityANT;
 }
 
 static void
 device_set_options(struct vnt_private *pDevice)
 {
-	unsigned char abyBroadcastAddr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-	unsigned char abySNAP_RFC1042[ETH_ALEN] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00};
-	unsigned char abySNAP_Bridgetunnel[ETH_ALEN] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0xF8};
-
-	memcpy(pDevice->abyBroadcastAddr, abyBroadcastAddr, ETH_ALEN);
-	memcpy(pDevice->abySNAP_RFC1042, abySNAP_RFC1042, ETH_ALEN);
-	memcpy(pDevice->abySNAP_Bridgetunnel, abySNAP_Bridgetunnel, ETH_ALEN);
-
-	pDevice->uChannel = pDevice->sOpts.channel_num;
-	pDevice->wRTSThreshold = pDevice->sOpts.rts_thresh;
-	pDevice->wFragmentationThreshold = pDevice->sOpts.frag_thresh;
 	pDevice->byShortRetryLimit = pDevice->sOpts.short_retry;
 	pDevice->byLongRetryLimit = pDevice->sOpts.long_retry;
-	pDevice->wMaxTransmitMSDULifetime = DEFAULT_MSDU_LIFETIME;
-	pDevice->byShortPreamble = (pDevice->sOpts.flags & DEVICE_FLAGS_PREAMBLE_TYPE) ? 1 : 0;
-	pDevice->byOpMode = (pDevice->sOpts.flags & DEVICE_FLAGS_OP_MODE) ? 1 : 0;
-	pDevice->ePSMode = (pDevice->sOpts.flags & DEVICE_FLAGS_PS_MODE) ? 1 : 0;
-	pDevice->b11hEnable = (pDevice->sOpts.flags & DEVICE_FLAGS_80211h_MODE) ? 1 : 0;
 	pDevice->bDiversityRegCtlON = (pDevice->sOpts.flags & DEVICE_FLAGS_DiversityANT) ? 1 : 0;
-	pDevice->uConnectionRate = pDevice->sOpts.data_rate;
-	if (pDevice->uConnectionRate < RATE_AUTO)
-		pDevice->bFixRate = true;
 	pDevice->byBBType = pDevice->sOpts.bbp_type;
-	pDevice->byPacketType = (VIA_PKT_TYPE)pDevice->byBBType;
+	pDevice->byPacketType = pDevice->byBBType;
 	pDevice->byAutoFBCtrl = AUTO_FB_0;
 	pDevice->bUpdateBBVGA = true;
-	pDevice->byFOETuning = 0;
 	pDevice->byPreambleType = 0;
 
-	pr_debug(" uChannel= %d\n", (int)pDevice->uChannel);
-	pr_debug(" byOpMode= %d\n", (int)pDevice->byOpMode);
-	pr_debug(" ePSMode= %d\n", (int)pDevice->ePSMode);
-	pr_debug(" wRTSThreshold= %d\n", (int)pDevice->wRTSThreshold);
 	pr_debug(" byShortRetryLimit= %d\n", (int)pDevice->byShortRetryLimit);
 	pr_debug(" byLongRetryLimit= %d\n", (int)pDevice->byLongRetryLimit);
 	pr_debug(" byPreambleType= %d\n", (int)pDevice->byPreambleType);
 	pr_debug(" byShortPreamble= %d\n", (int)pDevice->byShortPreamble);
-	pr_debug(" uConnectionRate= %d\n", (int)pDevice->uConnectionRate);
 	pr_debug(" byBBType= %d\n", (int)pDevice->byBBType);
-	pr_debug(" pDevice->b11hEnable= %d\n", (int)pDevice->b11hEnable);
 	pr_debug(" pDevice->bDiversityRegCtlON= %d\n",
 		 (int)pDevice->bDiversityRegCtlON);
 }
 
-static void s_vCompleteCurrentMeasure(struct vnt_private *pDevice,
-				      unsigned char byResult)
-{
-	unsigned int ii;
-	unsigned long dwDuration = 0;
-	unsigned char byRPI0 = 0;
-
-	for (ii = 1; ii < 8; ii++) {
-		pDevice->dwRPIs[ii] *= 255;
-		dwDuration |= *((unsigned short *)(pDevice->pCurrMeasureEID->sReq.abyDuration));
-		dwDuration <<= 10;
-		pDevice->dwRPIs[ii] /= dwDuration;
-		pDevice->abyRPIs[ii] = (unsigned char)pDevice->dwRPIs[ii];
-		byRPI0 += pDevice->abyRPIs[ii];
-	}
-	pDevice->abyRPIs[0] = (0xFF - byRPI0);
-
-	if (pDevice->uNumOfMeasureEIDs == 0) {
-		VNTWIFIbMeasureReport(pDevice->pMgmt,
-				      true,
-				      pDevice->pCurrMeasureEID,
-				      byResult,
-				      pDevice->byBasicMap,
-				      pDevice->byCCAFraction,
-				      pDevice->abyRPIs
-			);
-	} else {
-		VNTWIFIbMeasureReport(pDevice->pMgmt,
-				      false,
-				      pDevice->pCurrMeasureEID,
-				      byResult,
-				      pDevice->byBasicMap,
-				      pDevice->byCCAFraction,
-				      pDevice->abyRPIs
-			);
-		CARDbStartMeasure(pDevice, pDevice->pCurrMeasureEID++, pDevice->uNumOfMeasureEIDs);
-	}
-}
-
 //
 // Initialisation of MAC & BBP registers
 //
 
 static void device_init_registers(struct vnt_private *pDevice)
 {
+	unsigned long flags;
 	unsigned int ii;
 	unsigned char byValue;
 	unsigned char byValue1;
 	unsigned char byCCKPwrdBm = 0;
 	unsigned char byOFDMPwrdBm = 0;
-	int zonetype = 0;
-	PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
 
 	MACbShutdown(pDevice->PortOffset);
-	BBvSoftwareReset(pDevice->PortOffset);
+	BBvSoftwareReset(pDevice);
 
 	/* Do MACbSoftwareReset in MACvInitialize */
 	MACbSoftwareReset(pDevice->PortOffset);
@@ -481,11 +279,11 @@ static void device_init_registers(struct vnt_private *pDevice)
 	/* Get Local ID */
 	VNSvInPortB(pDevice->PortOffset + MAC_REG_LOCALID, &pDevice->byLocalID);
 
-	spin_lock_irq(&pDevice->lock);
+	spin_lock_irqsave(&pDevice->lock, flags);
 
 	SROMvReadAllContents(pDevice->PortOffset, pDevice->abyEEPROM);
 
-	spin_unlock_irq(&pDevice->lock);
+	spin_unlock_irqrestore(&pDevice->lock, flags);
 
 	/* Get Channel range */
 	pDevice->byMinChannel = 1;
@@ -558,41 +356,6 @@ static void device_init_registers(struct vnt_private *pDevice)
 
 	/* zonetype initial */
 	pDevice->byOriginalZonetype = pDevice->abyEEPROM[EEP_OFS_ZONETYPE];
-	zonetype = Config_FileOperation(pDevice, false, NULL);
-
-	if (zonetype >= 0) {
-		if ((zonetype == 0) &&
-		    (pDevice->abyEEPROM[EEP_OFS_ZONETYPE] != 0x00)) {
-			/* for USA */
-			pDevice->abyEEPROM[EEP_OFS_ZONETYPE] = 0;
-			pDevice->abyEEPROM[EEP_OFS_MAXCHANNEL] = 0x0B;
-
-			pr_debug("Init Zone Type :USA\n");
-		} else if ((zonetype == 1) &&
-			 (pDevice->abyEEPROM[EEP_OFS_ZONETYPE] != 0x01)) {
-			/* for Japan */
-			pDevice->abyEEPROM[EEP_OFS_ZONETYPE] = 0x01;
-			pDevice->abyEEPROM[EEP_OFS_MAXCHANNEL] = 0x0D;
-		} else if ((zonetype == 2) &&
-			  (pDevice->abyEEPROM[EEP_OFS_ZONETYPE] != 0x02)) {
-			/* for Europe */
-			pDevice->abyEEPROM[EEP_OFS_ZONETYPE] = 0x02;
-			pDevice->abyEEPROM[EEP_OFS_MAXCHANNEL] = 0x0D;
-
-			pr_debug("Init Zone Type :Europe\n");
-		} else {
-			if (zonetype != pDevice->abyEEPROM[EEP_OFS_ZONETYPE])
-				pr_debug("zonetype in file[%02x] mismatch with in EEPROM[%02x]\n",
-					 zonetype,
-					 pDevice->abyEEPROM[EEP_OFS_ZONETYPE]);
-			else
-				pr_debug("Read Zonetype file success,use default zonetype setting[%02x]\n",
-					 zonetype);
-		}
-	} else {
-		pr_debug("Read Zonetype file fail,use default zonetype setting[%02x]\n",
-			 SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_ZONETYPE));
-	}
 
 	/* Get RFType */
 	pDevice->byRFType = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_RFTYPE);
@@ -636,14 +399,9 @@ static void device_init_registers(struct vnt_private *pDevice)
 	}
 
 	/* recover 12,13 ,14channel for EUROPE by 11 channel */
-	if (((pDevice->abyEEPROM[EEP_OFS_ZONETYPE] == ZoneType_Japan) ||
-	     (pDevice->abyEEPROM[EEP_OFS_ZONETYPE] == ZoneType_Europe)) &&
-	    (pDevice->byOriginalZonetype == ZoneType_USA)) {
-		for (ii = 11; ii < 14; ii++) {
-			pDevice->abyCCKPwrTbl[ii] = pDevice->abyCCKPwrTbl[10];
-			pDevice->abyOFDMPwrTbl[ii] = pDevice->abyOFDMPwrTbl[10];
-
-		}
+	for (ii = 11; ii < 14; ii++) {
+		pDevice->abyCCKPwrTbl[ii] = pDevice->abyCCKPwrTbl[10];
+		pDevice->abyOFDMPwrTbl[ii] = pDevice->abyOFDMPwrTbl[10];
 	}
 
 	/* Load OFDM A Power Table */
@@ -657,8 +415,6 @@ static void device_init_registers(struct vnt_private *pDevice)
 					   (unsigned char)(ii + EEP_OFS_OFDMA_PWR_dBm));
 	}
 
-	init_channel_table((void *)pDevice);
-
 	if (pDevice->byLocalID > REV_ID_VT3253_B1) {
 		MACvSelectPage1(pDevice->PortOffset);
 
@@ -690,21 +446,12 @@ static void device_init_registers(struct vnt_private *pDevice)
 		BBvSetVGAGainOffset(pDevice, pDevice->abyBBVGA[0]);
 	}
 
-	BBvSetRxAntennaMode(pDevice->PortOffset, pDevice->byRxAntennaMode);
-	BBvSetTxAntennaMode(pDevice->PortOffset, pDevice->byTxAntennaMode);
-
-	pDevice->byCurrentCh = 0;
+	BBvSetRxAntennaMode(pDevice, pDevice->byRxAntennaMode);
+	BBvSetTxAntennaMode(pDevice, pDevice->byTxAntennaMode);
 
 	/* Set BB and packet type at the same time. */
 	/* Set Short Slot Time, xIFS, and RSPINF. */
-	if (pDevice->uConnectionRate == RATE_AUTO)
-		pDevice->wCurrentRate = RATE_54M;
-	else
-		pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
-
-	/* default G Mode */
-	VNTWIFIbConfigPhyMode(pDevice->pMgmt, PHY_TYPE_11G);
-	VNTWIFIbConfigPhyMode(pDevice->pMgmt, PHY_TYPE_AUTO);
+	pDevice->wCurrentRate = RATE_54M;
 
 	pDevice->bRadioOff = false;
 
@@ -726,8 +473,6 @@ static void device_init_registers(struct vnt_private *pDevice)
 	if (pDevice->bHWRadioOff || pDevice->bRadioControlOff)
 		CARDbRadioPowerOff(pDevice);
 
-	pMgmt->eScanType = WMAC_SCAN_PASSIVE;
-
 	/* get Permanent network address */
 	SROMvReadEtherAddress(pDevice->PortOffset, pDevice->abyCurrentNetAddr);
 	pr_debug("Network address = %pM\n", pDevice->abyCurrentNetAddr);
@@ -740,223 +485,39 @@ static void device_init_registers(struct vnt_private *pDevice)
 	if (pDevice->byLocalID <= REV_ID_VT3253_A1)
 		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_RCR, RCR_WPAERR);
 
-	pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
-
 	/* Turn On Rx DMA */
 	MACvReceive0(pDevice->PortOffset);
 	MACvReceive1(pDevice->PortOffset);
 
 	/* start the adapter */
 	MACvStart(pDevice->PortOffset);
-
-	netif_stop_queue(pDevice->dev);
 }
 
 static void device_init_diversity_timer(struct vnt_private *pDevice)
 {
 	init_timer(&pDevice->TimerSQ3Tmax1);
 	pDevice->TimerSQ3Tmax1.data = (unsigned long) pDevice;
-	pDevice->TimerSQ3Tmax1.function = (TimerFunction)TimerSQ3CallBack;
+	pDevice->TimerSQ3Tmax1.function = TimerSQ3CallBack;
 	pDevice->TimerSQ3Tmax1.expires = RUN_AT(HZ);
 
 	init_timer(&pDevice->TimerSQ3Tmax2);
 	pDevice->TimerSQ3Tmax2.data = (unsigned long) pDevice;
-	pDevice->TimerSQ3Tmax2.function = (TimerFunction)TimerSQ3CallBack;
+	pDevice->TimerSQ3Tmax2.function = TimerSQ3CallBack;
 	pDevice->TimerSQ3Tmax2.expires = RUN_AT(HZ);
 
 	init_timer(&pDevice->TimerSQ3Tmax3);
 	pDevice->TimerSQ3Tmax3.data = (unsigned long) pDevice;
-	pDevice->TimerSQ3Tmax3.function = (TimerFunction)TimerState1CallBack;
+	pDevice->TimerSQ3Tmax3.function = TimerState1CallBack;
 	pDevice->TimerSQ3Tmax3.expires = RUN_AT(HZ);
 }
 
-static bool device_release_WPADEV(struct vnt_private *pDevice)
-{
-	viawget_wpa_header *wpahdr;
-	int ii = 0;
-
-	//send device close to wpa_supplicnat layer
-	if (pDevice->bWPADEVUp) {
-		wpahdr = (viawget_wpa_header *)pDevice->skb->data;
-		wpahdr->type = VIAWGET_DEVICECLOSE_MSG;
-		wpahdr->resp_ie_len = 0;
-		wpahdr->req_ie_len = 0;
-		skb_put(pDevice->skb, sizeof(viawget_wpa_header));
-		pDevice->skb->dev = pDevice->wpadev;
-		skb_reset_mac_header(pDevice->skb);
-		pDevice->skb->pkt_type = PACKET_HOST;
-		pDevice->skb->protocol = htons(ETH_P_802_2);
-		memset(pDevice->skb->cb, 0, sizeof(pDevice->skb->cb));
-		netif_rx(pDevice->skb);
-		pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
-
-		while (pDevice->bWPADEVUp) {
-			set_current_state(TASK_UNINTERRUPTIBLE);
-			schedule_timeout(HZ / 20);          //wait 50ms
-			ii++;
-			if (ii > 20)
-				break;
-		}
-	}
-	return true;
-}
-
-static const struct net_device_ops device_netdev_ops = {
-	.ndo_open               = device_open,
-	.ndo_stop               = device_close,
-	.ndo_do_ioctl           = device_ioctl,
-	.ndo_start_xmit         = device_xmit,
-	.ndo_set_rx_mode	= device_set_multi,
-};
-
-static int
-vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
-{
-	static bool bFirst = true;
-	struct net_device *dev = NULL;
-	PCHIP_INFO  pChip_info = (PCHIP_INFO)ent->driver_data;
-	struct vnt_private *pDevice;
-	int         rc;
-
-	dev = alloc_etherdev(sizeof(*pDevice));
-
-	pDevice = netdev_priv(dev);
-
-	if (dev == NULL) {
-		pr_err(DEVICE_NAME ": allocate net device failed\n");
-		return -ENOMEM;
-	}
-
-	// Chain it all together
-	SET_NETDEV_DEV(dev, &pcid->dev);
-
-	if (bFirst) {
-		pr_notice("%s Ver. %s\n", DEVICE_FULL_DRV_NAM, DEVICE_VERSION);
-		pr_notice("Copyright (c) 2003 VIA Networking Technologies, Inc.\n");
-		bFirst = false;
-	}
-
-	vt6655_init_info(pcid, &pDevice, pChip_info);
-	pDevice->dev = dev;
-
-	if (pci_enable_device(pcid)) {
-		device_free_info(pDevice);
-		return -ENODEV;
-	}
-	dev->irq = pcid->irq;
-
-#ifdef	DEBUG
-	pr_debug("Before get pci_info memaddr is %x\n", pDevice->memaddr);
-#endif
-	if (!device_get_pci_info(pDevice, pcid)) {
-		pr_err(DEVICE_NAME ": Failed to find PCI device.\n");
-		device_free_info(pDevice);
-		return -ENODEV;
-	}
-
-#if 1
-
-#ifdef	DEBUG
-
-	pr_debug("after get pci_info memaddr is %x, io addr is %x,io_size is %d\n", pDevice->memaddr, pDevice->ioaddr, pDevice->io_size);
-	{
-		int i;
-		u32 bar, len;
-		u32 address[] = {
-			PCI_BASE_ADDRESS_0,
-			PCI_BASE_ADDRESS_1,
-			PCI_BASE_ADDRESS_2,
-			PCI_BASE_ADDRESS_3,
-			PCI_BASE_ADDRESS_4,
-			PCI_BASE_ADDRESS_5,
-			0};
-		for (i = 0; address[i]; i++) {
-			pci_read_config_dword(pcid, address[i], &bar);
-			pr_debug("bar %d is %x\n", i, bar);
-			if (!bar) {
-				pr_debug("bar %d not implemented\n", i);
-				continue;
-			}
-			if (bar & PCI_BASE_ADDRESS_SPACE_IO) {
-				/* This is IO */
-
-				len = bar & (PCI_BASE_ADDRESS_IO_MASK & 0xFFFF);
-				len = len & ~(len - 1);
-
-				pr_debug("IO space:  len in IO %x, BAR %d\n", len, i);
-			} else {
-				len = bar & 0xFFFFFFF0;
-				len = ~len + 1;
-
-				pr_debug("len in MEM %x, BAR %d\n", len, i);
-			}
-		}
-	}
-#endif
-
-#endif
-
-	pDevice->PortOffset = ioremap(pDevice->memaddr & PCI_BASE_ADDRESS_MEM_MASK, pDevice->io_size);
-
-	if (pDevice->PortOffset == NULL) {
-		pr_err(DEVICE_NAME ": Failed to IO remapping ..\n");
-		device_free_info(pDevice);
-		return -ENODEV;
-	}
-
-	rc = pci_request_regions(pcid, DEVICE_NAME);
-	if (rc) {
-		pr_err(DEVICE_NAME ": Failed to find PCI device\n");
-		device_free_info(pDevice);
-		return -ENODEV;
-	}
-
-	dev->base_addr = pDevice->ioaddr;
-	// do reset
-	if (!MACbSoftwareReset(pDevice->PortOffset)) {
-		pr_err(DEVICE_NAME ": Failed to access MAC hardware..\n");
-		device_free_info(pDevice);
-		return -ENODEV;
-	}
-	// initial to reload eeprom
-	MACvInitialize(pDevice->PortOffset);
-	MACvReadEtherAddress(pDevice->PortOffset, dev->dev_addr);
-
-	device_get_options(pDevice);
-	device_set_options(pDevice);
-	//Mask out the options cannot be set to the chip
-	pDevice->sOpts.flags &= pChip_info->flags;
-
-	//Enable the chip specified capabilities
-	pDevice->flags = pDevice->sOpts.flags | (pChip_info->flags & 0xFF000000UL);
-	pDevice->tx_80211 = device_dma0_tx_80211;
-	pDevice->sMgmtObj.pAdapter = (void *)pDevice;
-	pDevice->pMgmt = &(pDevice->sMgmtObj);
-
-	dev->irq                = pcid->irq;
-	dev->netdev_ops         = &device_netdev_ops;
-
-	dev->wireless_handlers = (struct iw_handler_def *)&iwctl_handler_def;
-
-	rc = register_netdev(dev);
-	if (rc) {
-		pr_err(DEVICE_NAME " Failed to register netdev\n");
-		device_free_info(pDevice);
-		return -ENODEV;
-	}
-	device_print_info(pDevice);
-	pci_set_drvdata(pcid, pDevice);
-	return 0;
-}
-
 static void device_print_info(struct vnt_private *pDevice)
 {
-	struct net_device *dev = pDevice->dev;
+	dev_info(&pDevice->pcid->dev, "%s\n", get_chip_name(pDevice->chip_id));
 
-	pr_info("%s: %s\n", dev->name, get_chip_name(pDevice->chip_id));
-	pr_info("%s: MAC=%pM IO=0x%lx Mem=0x%lx IRQ=%d\n",
-		dev->name, dev->dev_addr, (unsigned long)pDevice->ioaddr,
-		(unsigned long)pDevice->PortOffset, pDevice->dev->irq);
+	dev_info(&pDevice->pcid->dev, "MAC=%pM IO=0x%lx Mem=0x%lx IRQ=%d\n",
+		 pDevice->abyCurrentNetAddr, (unsigned long)pDevice->ioaddr,
+		 (unsigned long)pDevice->PortOffset, pDevice->pcid->irq);
 }
 
 static void vt6655_init_info(struct pci_dev *pcid,
@@ -1003,31 +564,20 @@ static bool device_get_pci_info(struct vnt_private *pDevice,
 
 static void device_free_info(struct vnt_private *pDevice)
 {
-	struct net_device *dev = pDevice->dev;
-
-	ASSERT(pDevice);
-//2008-0714-01<Add>by chester
-	device_release_WPADEV(pDevice);
-
-//2008-07-21-01<Add>by MikeLiu
-//unregister wpadev
-	if (wpa_set_wpadev(pDevice, 0) != 0)
-		pr_err("unregister wpadev fail?\n");
+	if (!pDevice)
+		return;
 
-#ifdef HOSTAP
-	if (dev)
-		vt6655_hostap_set_hostapd(pDevice, 0, 0);
-#endif
-	if (dev)
-		unregister_netdev(dev);
+	if (pDevice->mac_hw)
+		ieee80211_unregister_hw(pDevice->hw);
 
 	if (pDevice->PortOffset)
 		iounmap(pDevice->PortOffset);
 
 	if (pDevice->pcid)
 		pci_release_regions(pDevice->pcid);
-	if (dev)
-		free_netdev(dev);
+
+	if (pDevice->hw)
+		ieee80211_free_hw(pDevice->hw);
 }
 
 static bool device_init_rings(struct vnt_private *pDevice)
@@ -1176,21 +726,6 @@ static void device_init_rd1_ring(struct vnt_private *pDevice)
 	pDevice->pCurrRD[1] = &(pDevice->aRD1Ring[0]);
 }
 
-static void device_init_defrag_cb(struct vnt_private *pDevice)
-{
-	int i;
-	PSDeFragControlBlock pDeF;
-
-	/* Init the fragment ctl entries */
-	for (i = 0; i < CB_MAX_RX_FRAG; i++) {
-		pDeF = &(pDevice->sRxDFCB[i]);
-		if (!device_alloc_frag_buf(pDevice, pDeF))
-			dev_err(&pDevice->pcid->dev, "can not alloc frag bufs\n");
-	}
-	pDevice->cbDFCB = CB_MAX_RX_FRAG;
-	pDevice->cbFreeDFCB = pDevice->cbDFCB;
-}
-
 static void device_free_rd0_ring(struct vnt_private *pDevice)
 {
 	int i;
@@ -1204,7 +739,7 @@ static void device_free_rd0_ring(struct vnt_private *pDevice)
 
 		dev_kfree_skb(pRDInfo->skb);
 
-		kfree((void *)pDesc->pRDInfo);
+		kfree(pDesc->pRDInfo);
 	}
 }
 
@@ -1221,21 +756,7 @@ static void device_free_rd1_ring(struct vnt_private *pDevice)
 
 		dev_kfree_skb(pRDInfo->skb);
 
-		kfree((void *)pDesc->pRDInfo);
-	}
-}
-
-static void device_free_frag_buf(struct vnt_private *pDevice)
-{
-	PSDeFragControlBlock pDeF;
-	int i;
-
-	for (i = 0; i < CB_MAX_RX_FRAG; i++) {
-		pDeF = &(pDevice->sRxDFCB[i]);
-
-		if (pDeF->skb)
-			dev_kfree_skb(pDeF->skb);
-
+		kfree(pDesc->pRDInfo);
 	}
 }
 
@@ -1305,7 +826,7 @@ static void device_free_td0_ring(struct vnt_private *pDevice)
 		if (pTDInfo->skb)
 			dev_kfree_skb(pTDInfo->skb);
 
-		kfree((void *)pDesc->pTDInfo);
+		kfree(pDesc->pTDInfo);
 	}
 }
 
@@ -1324,7 +845,7 @@ static void device_free_td1_ring(struct vnt_private *pDevice)
 		if (pTDInfo->skb)
 			dev_kfree_skb(pTDInfo->skb);
 
-		kfree((void *)pDesc->pTDInfo);
+		kfree(pDesc->pTDInfo);
 	}
 }
 
@@ -1340,7 +861,7 @@ static int device_rx_srv(struct vnt_private *pDevice, unsigned int uIdx)
 	     pRD = pRD->next) {
 		if (works++ > 15)
 			break;
-		if (device_receive_frame(pDevice, pRD)) {
+		if (vnt_receive_frame(pDevice, pRD)) {
 			if (!device_alloc_rx_buf(pDevice, pRD)) {
 				dev_err(&pDevice->pcid->dev,
 					"can not allocate rx buf\n");
@@ -1348,7 +869,6 @@ static int device_rx_srv(struct vnt_private *pDevice, unsigned int uIdx)
 			}
 		}
 		pRD->m_rd0RD0.f1Owner = OWNED_BY_NIC;
-		pDevice->dev->last_rx = jiffies;
 	}
 
 	pDevice->pCurrRD[uIdx] = pRD;
@@ -1364,9 +884,12 @@ static bool device_alloc_rx_buf(struct vnt_private *pDevice, PSRxDesc pRD)
 	if (pRDInfo->skb == NULL)
 		return false;
 	ASSERT(pRDInfo->skb);
-	pRDInfo->skb->dev = pDevice->dev;
-	pRDInfo->skb_dma = pci_map_single(pDevice->pcid, skb_tail_pointer(pRDInfo->skb),
-					  pDevice->rx_buf_sz, PCI_DMA_FROMDEVICE);
+
+	pRDInfo->skb_dma =
+		pci_map_single(pDevice->pcid,
+			       skb_put(pRDInfo->skb, skb_tailroom(pRDInfo->skb)),
+			       pDevice->rx_buf_sz, PCI_DMA_FROMDEVICE);
+
 	*((unsigned int *)&(pRD->m_rd0RD0)) = 0; /* FIX cast */
 
 	pRD->m_rd0RD0.wResCount = cpu_to_le16(pDevice->rx_buf_sz);
@@ -1377,31 +900,84 @@ static bool device_alloc_rx_buf(struct vnt_private *pDevice, PSRxDesc pRD)
 	return true;
 }
 
-bool device_alloc_frag_buf(struct vnt_private *pDevice,
-			   PSDeFragControlBlock pDeF)
+static const u8 fallback_rate0[5][5] = {
+	{RATE_18M, RATE_18M, RATE_12M, RATE_12M, RATE_12M},
+	{RATE_24M, RATE_24M, RATE_18M, RATE_12M, RATE_12M},
+	{RATE_36M, RATE_36M, RATE_24M, RATE_18M, RATE_18M},
+	{RATE_48M, RATE_48M, RATE_36M, RATE_24M, RATE_24M},
+	{RATE_54M, RATE_54M, RATE_48M, RATE_36M, RATE_36M}
+};
+
+static const u8 fallback_rate1[5][5] = {
+	{RATE_18M, RATE_18M, RATE_12M, RATE_6M, RATE_6M},
+	{RATE_24M, RATE_24M, RATE_18M, RATE_6M, RATE_6M},
+	{RATE_36M, RATE_36M, RATE_24M, RATE_12M, RATE_12M},
+	{RATE_48M, RATE_48M, RATE_24M, RATE_12M, RATE_12M},
+	{RATE_54M, RATE_54M, RATE_36M, RATE_18M, RATE_18M}
+};
+
+static int vnt_int_report_rate(struct vnt_private *priv,
+			       PDEVICE_TD_INFO context, u8 tsr0, u8 tsr1)
 {
-	pDeF->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
-	if (pDeF->skb == NULL)
-		return false;
-	ASSERT(pDeF->skb);
-	pDeF->skb->dev = pDevice->dev;
+	struct vnt_tx_fifo_head *fifo_head;
+	struct ieee80211_tx_info *info;
+	struct ieee80211_rate *rate;
+	u16 fb_option;
+	u8 tx_retry = (tsr0 & TSR0_NCR);
+	s8 idx;
+
+	if (!context)
+		return -ENOMEM;
 
-	return true;
+	if (!context->skb)
+		return -EINVAL;
+
+	fifo_head = (struct vnt_tx_fifo_head *)context->buf;
+	fb_option = (le16_to_cpu(fifo_head->fifo_ctl) &
+			(FIFOCTL_AUTO_FB_0 | FIFOCTL_AUTO_FB_1));
+
+	info = IEEE80211_SKB_CB(context->skb);
+	idx = info->control.rates[0].idx;
+
+	if (fb_option && !(tsr1 & TSR1_TERR)) {
+		u8 tx_rate;
+		u8 retry = tx_retry;
+
+		rate = ieee80211_get_tx_rate(priv->hw, info);
+		tx_rate = rate->hw_value - RATE_18M;
+
+		if (retry > 4)
+			retry = 4;
+
+		if (fb_option & FIFOCTL_AUTO_FB_0)
+			tx_rate = fallback_rate0[tx_rate][retry];
+		else if (fb_option & FIFOCTL_AUTO_FB_1)
+			tx_rate = fallback_rate1[tx_rate][retry];
+
+		if (info->band == IEEE80211_BAND_5GHZ)
+			idx = tx_rate - RATE_6M;
+		else
+			idx = tx_rate;
+	}
+
+	ieee80211_tx_info_clear_status(info);
+
+	info->status.rates[0].count = tx_retry;
+
+	if (!(tsr1 & TSR1_TERR)) {
+		info->status.rates[0].idx = idx;
+		info->flags |= IEEE80211_TX_STAT_ACK;
+	}
+
+	return 0;
 }
 
 static int device_tx_srv(struct vnt_private *pDevice, unsigned int uIdx)
 {
 	PSTxDesc                 pTD;
-	bool bFull = false;
 	int                      works = 0;
 	unsigned char byTsr0;
 	unsigned char byTsr1;
-	unsigned int	uFrameSize, uFIFOHeaderSize;
-	PSTxBufHead              pTxBufHead;
-	struct net_device_stats *pStats = &pDevice->dev->stats;
-	struct sk_buff *skb;
-	unsigned int	uNodeIndex;
-	PSMgmtObject             pMgmt = pDevice->pMgmt;
 
 	for (pTD = pDevice->apTailTD[uIdx]; pDevice->iTDUsed[uIdx] > 0; pTD = pTD->next) {
 		if (pTD->m_td0TD0.f1Owner == OWNED_BY_NIC)
@@ -1415,22 +991,8 @@ static int device_tx_srv(struct vnt_private *pDevice, unsigned int uIdx)
 		//Only the status of first TD in the chain is correct
 		if (pTD->m_td1TD1.byTCR & TCR_STP) {
 			if ((pTD->pTDInfo->byFlags & TD_FLAGS_NETIF_SKB) != 0) {
-				uFIFOHeaderSize = pTD->pTDInfo->dwHeaderLength;
-				uFrameSize = pTD->pTDInfo->dwReqCount - uFIFOHeaderSize;
-				pTxBufHead = (PSTxBufHead) (pTD->pTDInfo->buf);
-				// Update the statistics based on the Transmit status
-				// now, we DONT check TSR0_CDH
-
-				STAvUpdateTDStatCounter(&pDevice->scStatistic,
-							byTsr0, byTsr1,
-							(unsigned char *)(pTD->pTDInfo->buf + uFIFOHeaderSize),
-							uFrameSize, uIdx);
-
-				BSSvUpdateNodeTxCounter(pDevice,
-							byTsr0, byTsr1,
-							(unsigned char *)(pTD->pTDInfo->buf),
-							uFIFOHeaderSize
-					);
+
+				vnt_int_report_rate(pDevice, pTD->pTDInfo, byTsr0, byTsr1);
 
 				if (!(byTsr1 & TSR1_TERR)) {
 					if (byTsr0 != 0) {
@@ -1438,28 +1000,9 @@ static int device_tx_srv(struct vnt_private *pDevice, unsigned int uIdx)
 							 (int)uIdx, byTsr1,
 							 byTsr0);
 					}
-					if ((pTxBufHead->wFragCtl & FRAGCTL_ENDFRAG) != FRAGCTL_NONFRAG)
-						pDevice->s802_11Counter.TransmittedFragmentCount++;
-
-					pStats->tx_packets++;
-					pStats->tx_bytes += pTD->pTDInfo->skb->len;
 				} else {
 					pr_debug(" Tx[%d] dropped & tsr1[%02X] tsr0[%02X]\n",
 						 (int)uIdx, byTsr1, byTsr0);
-					pStats->tx_errors++;
-					pStats->tx_dropped++;
-				}
-			}
-
-			if ((pTD->pTDInfo->byFlags & TD_FLAGS_PRIV_SKB) != 0) {
-				if (pDevice->bEnableHostapd) {
-					pr_debug("tx call back netif..\n");
-					skb = pTD->pTDInfo->skb;
-					skb->dev = pDevice->apdev;
-					skb_reset_mac_header(skb);
-					skb->pkt_type = PACKET_OTHERHOST;
-					memset(skb->cb, 0, sizeof(skb->cb));
-					netif_rx(skb);
 				}
 			}
 
@@ -1468,49 +1011,12 @@ static int device_tx_srv(struct vnt_private *pDevice, unsigned int uIdx)
 					pr_debug(" Tx[%d] fail has error. tsr1[%02X] tsr0[%02X]\n",
 						 (int)uIdx, byTsr1, byTsr0);
 				}
-
-
-				if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) &&
-				    (pTD->pTDInfo->byFlags & TD_FLAGS_NETIF_SKB)) {
-					unsigned short wAID;
-					unsigned char byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
-
-					skb = pTD->pTDInfo->skb;
-					if (BSSDBbIsSTAInNodeDB(pMgmt, (unsigned char *)(skb->data), &uNodeIndex)) {
-						if (pMgmt->sNodeDBTable[uNodeIndex].bPSEnable) {
-							skb_queue_tail(&pMgmt->sNodeDBTable[uNodeIndex].sTxPSQueue, skb);
-							pMgmt->sNodeDBTable[uNodeIndex].wEnQueueCnt++;
-							// set tx map
-							wAID = pMgmt->sNodeDBTable[uNodeIndex].wAID;
-							pMgmt->abyPSTxMap[wAID >> 3] |=  byMask[wAID & 7];
-							pTD->pTDInfo->byFlags &= ~(TD_FLAGS_NETIF_SKB);
-							pr_debug("tx_srv:tx fail re-queue sta index= %d, QueCnt= %d\n",
-								 (int)uNodeIndex,
-								 pMgmt->sNodeDBTable[uNodeIndex].wEnQueueCnt);
-							pStats->tx_errors--;
-							pStats->tx_dropped--;
-						}
-					}
-				}
 			}
 			device_free_tx_buf(pDevice, pTD);
 			pDevice->iTDUsed[uIdx]--;
 		}
 	}
 
-	if (uIdx == TYPE_AC0DMA) {
-		// RESERV_AC0DMA reserved for relay
-
-		if (AVAIL_TD(pDevice, uIdx) < RESERV_AC0DMA) {
-			bFull = true;
-			pr_debug(" AC0DMA is Full = %d\n",
-				 pDevice->iTDUsed[uIdx]);
-		}
-		if (netif_queue_stopped(pDevice->dev) && !bFull)
-			netif_wake_queue(pDevice->dev);
-
-	}
-
 	pDevice->apTailTD[uIdx] = pTD;
 
 	return works;
@@ -1521,10 +1027,6 @@ static void device_error(struct vnt_private *pDevice, unsigned short status)
 	if (status & ISR_FETALERR) {
 		dev_err(&pDevice->pcid->dev, "Hardware fatal error\n");
 
-		netif_stop_queue(pDevice->dev);
-		del_timer(&pDevice->sTimerCommand);
-		del_timer(&(pDevice->pMgmt->sTimerSecondCallback));
-		pDevice->bCmdRunning = false;
 		MACbShutdown(pDevice->PortOffset);
 		return;
 	}
@@ -1541,7 +1043,9 @@ static void device_free_tx_buf(struct vnt_private *pDevice, PSTxDesc pDesc)
 				 PCI_DMA_TODEVICE);
 	}
 
-	if ((pTDInfo->byFlags & TD_FLAGS_NETIF_SKB) != 0)
+	if (pTDInfo->byFlags & TD_FLAGS_NETIF_SKB)
+		ieee80211_tx_status_irqsafe(pDevice->hw, skb);
+	else
 		dev_kfree_skb_irq(skb);
 
 	pTDInfo->skb_dma = 0;
@@ -1549,671 +1053,13 @@ static void device_free_tx_buf(struct vnt_private *pDevice, PSTxDesc pDesc)
 	pTDInfo->byFlags = 0;
 }
 
-static int  device_open(struct net_device *dev)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	int i;
-#ifdef WPA_SM_Transtatus
-	extern SWPAResult wpa_Result;
-#endif
-
-	pDevice->rx_buf_sz = PKT_BUF_SZ;
-	if (!device_init_rings(pDevice))
-		return -ENOMEM;
-
-//2008-5-13 <add> by chester
-	i = request_irq(pDevice->pcid->irq, &device_intr, IRQF_SHARED, dev->name, dev);
-	if (i)
-		return i;
-
-#ifdef WPA_SM_Transtatus
-	memset(wpa_Result.ifname, 0, sizeof(wpa_Result.ifname));
-	wpa_Result.proto = 0;
-	wpa_Result.key_mgmt = 0;
-	wpa_Result.eap_type = 0;
-	wpa_Result.authenticated = false;
-	pDevice->fWPA_Authened = false;
-#endif
-	pr_debug("call device init rd0 ring\n");
-	device_init_rd0_ring(pDevice);
-	device_init_rd1_ring(pDevice);
-	device_init_defrag_cb(pDevice);
-	device_init_td0_ring(pDevice);
-	device_init_td1_ring(pDevice);
-
-	if (pDevice->bDiversityRegCtlON)
-		device_init_diversity_timer(pDevice);
-
-	vMgrObjectInit(pDevice);
-	vMgrTimerInit(pDevice);
-
-	pr_debug("call device_init_registers\n");
-	device_init_registers(pDevice);
-
-	MACvReadEtherAddress(pDevice->PortOffset, pDevice->abyCurrentNetAddr);
-	memcpy(pDevice->pMgmt->abyMACAddr, pDevice->abyCurrentNetAddr, ETH_ALEN);
-	device_set_multi(pDevice->dev);
-
-	// Init for Key Management
-	KeyvInitTable(&pDevice->sKey, pDevice->PortOffset);
-	add_timer(&(pDevice->pMgmt->sTimerSecondCallback));
-
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-	pDevice->bwextcount = 0;
-	pDevice->bWPASuppWextEnabled = false;
-#endif
-	pDevice->byReAssocCount = 0;
-	pDevice->bWPADEVUp = false;
-	// Patch: if WEP key already set by iwconfig but device not yet open
-	if (pDevice->bEncryptionEnable && pDevice->bTransmitKey) {
-		KeybSetDefaultKey(&(pDevice->sKey),
-				  (unsigned long)(pDevice->byKeyIndex | (1 << 31)),
-				  pDevice->uKeyLength,
-				  NULL,
-				  pDevice->abyKey,
-				  KEY_CTL_WEP,
-				  pDevice->PortOffset,
-				  pDevice->byLocalID
-			);
-		pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-	}
-
-	pr_debug("call MACvIntEnable\n");
-	MACvIntEnable(pDevice->PortOffset, IMR_MASK_VALUE);
-
-	if (pDevice->pMgmt->eConfigMode == WMAC_CONFIG_AP) {
-		bScheduleCommand((void *)pDevice, WLAN_CMD_RUN_AP, NULL);
-	} else {
-		bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, NULL);
-		bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, NULL);
-	}
-	pDevice->flags |= DEVICE_FLAGS_OPENED;
-
-	pr_debug("device_open success..\n");
-	return 0;
-}
-
-static int  device_close(struct net_device *dev)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject     pMgmt = pDevice->pMgmt;
-//2007-1121-02<Add>by EinsnLiu
-	if (pDevice->bLinkPass) {
-		bScheduleCommand((void *)pDevice, WLAN_CMD_DISASSOCIATE, NULL);
-		mdelay(30);
-	}
-
-	del_timer(&pDevice->sTimerTxData);
-	del_timer(&pDevice->sTimerCommand);
-	del_timer(&pMgmt->sTimerSecondCallback);
-	if (pDevice->bDiversityRegCtlON) {
-		del_timer(&pDevice->TimerSQ3Tmax1);
-		del_timer(&pDevice->TimerSQ3Tmax2);
-		del_timer(&pDevice->TimerSQ3Tmax3);
-	}
-
-	netif_stop_queue(dev);
-	pDevice->bCmdRunning = false;
-	MACbShutdown(pDevice->PortOffset);
-	MACbSoftwareReset(pDevice->PortOffset);
-	CARDbRadioPowerOff(pDevice);
-
-	pDevice->bLinkPass = false;
-	memset(pMgmt->abyCurrBSSID, 0, 6);
-	pMgmt->eCurrState = WMAC_STATE_IDLE;
-	device_free_td0_ring(pDevice);
-	device_free_td1_ring(pDevice);
-	device_free_rd0_ring(pDevice);
-	device_free_rd1_ring(pDevice);
-	device_free_frag_buf(pDevice);
-	device_free_rings(pDevice);
-	BSSvClearNodeDBTable(pDevice, 0);
-	free_irq(dev->irq, dev);
-	pDevice->flags &= (~DEVICE_FLAGS_OPENED);
-	//2008-0714-01<Add>by chester
-	device_release_WPADEV(pDevice);
-
-	pr_debug("device_close..\n");
-	return 0;
-}
-
-static int device_dma0_tx_80211(struct sk_buff *skb, struct net_device *dev)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	unsigned char *pbMPDU;
-	unsigned int cbMPDULen = 0;
-
-	pr_debug("device_dma0_tx_80211\n");
-	spin_lock_irq(&pDevice->lock);
-
-	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 0) {
-		pr_debug("device_dma0_tx_80211, td0 <=0\n");
-		dev_kfree_skb_irq(skb);
-		spin_unlock_irq(&pDevice->lock);
-		return 0;
-	}
-
-	if (pDevice->bStopTx0Pkt) {
-		dev_kfree_skb_irq(skb);
-		spin_unlock_irq(&pDevice->lock);
-		return 0;
-	}
-
-	cbMPDULen = skb->len;
-	pbMPDU = skb->data;
-
-	vDMA0_tx_80211(pDevice, skb, pbMPDU, cbMPDULen);
-
-	spin_unlock_irq(&pDevice->lock);
-
-	return 0;
-}
-
-bool device_dma0_xmit(struct vnt_private *pDevice,
-		      struct sk_buff *skb, unsigned int uNodeIndex)
-{
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	PSTxDesc        pHeadTD, pLastTD;
-	unsigned int cbFrameBodySize;
-	unsigned int uMACfragNum;
-	unsigned char byPktType;
-	bool bNeedEncryption = false;
-	PSKeyItem       pTransmitKey = NULL;
-	unsigned int cbHeaderSize;
-	unsigned int ii;
-	SKeyItem        STempKey;
-
-	if (pDevice->bStopTx0Pkt) {
-		dev_kfree_skb_irq(skb);
-		return false;
-	}
-
-	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 0) {
-		dev_kfree_skb_irq(skb);
-		pr_debug("device_dma0_xmit, td0 <=0\n");
-		return false;
-	}
-
-	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-		if (pDevice->uAssocCount == 0) {
-			dev_kfree_skb_irq(skb);
-			pr_debug("device_dma0_xmit, assocCount = 0\n");
-			return false;
-		}
-	}
-
-	pHeadTD = pDevice->apCurrTD[TYPE_TXDMA0];
-
-	pHeadTD->m_td1TD1.byTCR = (TCR_EDP|TCR_STP);
-
-	memcpy(pDevice->sTxEthHeader.abyDstAddr, (unsigned char *)(skb->data), ETH_HLEN);
-	cbFrameBodySize = skb->len - ETH_HLEN;
-
-	// 802.1H
-	if (ntohs(pDevice->sTxEthHeader.wType) > ETH_DATA_LEN)
-		cbFrameBodySize += 8;
-
-	uMACfragNum = cbGetFragCount(pDevice, pTransmitKey, cbFrameBodySize, &pDevice->sTxEthHeader);
-
-	if (uMACfragNum > AVAIL_TD(pDevice, TYPE_TXDMA0)) {
-		dev_kfree_skb_irq(skb);
-		return false;
-	}
-	byPktType = (unsigned char)pDevice->byPacketType;
-
-	if (pDevice->bFixRate) {
-		if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
-			if (pDevice->uConnectionRate >= RATE_11M)
-				pDevice->wCurrentRate = RATE_11M;
-			else
-				pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
-		} else {
-			if (pDevice->uConnectionRate >= RATE_54M)
-				pDevice->wCurrentRate = RATE_54M;
-			else
-				pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
-		}
-	} else {
-		pDevice->wCurrentRate = pDevice->pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate;
-	}
-
-	//preamble type
-	if (pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble)
-		pDevice->byPreambleType = pDevice->byShortPreamble;
-	else
-		pDevice->byPreambleType = PREAMBLE_LONG;
-
-	pr_debug("dma0: pDevice->wCurrentRate = %d\n", pDevice->wCurrentRate);
-
-	if (pDevice->wCurrentRate <= RATE_11M) {
-		byPktType = PK_TYPE_11B;
-	} else if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
-		byPktType = PK_TYPE_11A;
-	} else {
-		if (pDevice->bProtectMode)
-			byPktType = PK_TYPE_11GB;
-		else
-			byPktType = PK_TYPE_11GA;
-	}
-
-	if (pDevice->bEncryptionEnable)
-		bNeedEncryption = true;
-
-	if (pDevice->bEnableHostWEP) {
-		pTransmitKey = &STempKey;
-		pTransmitKey->byCipherSuite = pMgmt->sNodeDBTable[uNodeIndex].byCipherSuite;
-		pTransmitKey->dwKeyIndex = pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex;
-		pTransmitKey->uKeyLength = pMgmt->sNodeDBTable[uNodeIndex].uWepKeyLength;
-		pTransmitKey->dwTSC47_16 = pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16;
-		pTransmitKey->wTSC15_0 = pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0;
-		memcpy(pTransmitKey->abyKey,
-		       &pMgmt->sNodeDBTable[uNodeIndex].abyWepKey[0],
-		       pTransmitKey->uKeyLength
-			);
-	}
-	vGenerateFIFOHeader(pDevice, byPktType, pDevice->pbyTmpBuff, bNeedEncryption,
-			    cbFrameBodySize, TYPE_TXDMA0, pHeadTD,
-			    &pDevice->sTxEthHeader, (unsigned char *)skb->data, pTransmitKey, uNodeIndex,
-			    &uMACfragNum,
-			    &cbHeaderSize
-		);
-
-	if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS)) {
-		// Disable PS
-		MACbPSWakeup(pDevice->PortOffset);
-	}
-
-	pDevice->bPWBitOn = false;
-
-	pLastTD = pHeadTD;
-	for (ii = 0; ii < uMACfragNum; ii++) {
-		// Poll Transmit the adapter
-		wmb();
-		pHeadTD->m_td0TD0.f1Owner = OWNED_BY_NIC;
-		wmb();
-		if (ii == (uMACfragNum - 1))
-			pLastTD = pHeadTD;
-		pHeadTD = pHeadTD->next;
-	}
-
-	// Save the information needed by the tx interrupt handler
-	// to complete the Send request
-	pLastTD->pTDInfo->skb = skb;
-	pLastTD->pTDInfo->byFlags = 0;
-	pLastTD->pTDInfo->byFlags |= TD_FLAGS_NETIF_SKB;
-
-	pDevice->apCurrTD[TYPE_TXDMA0] = pHeadTD;
-
-	MACvTransmit0(pDevice->PortOffset);
-
-	return true;
-}
-
-//TYPE_AC0DMA data tx
-static int  device_xmit(struct sk_buff *skb, struct net_device *dev) {
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	PSTxDesc        pHeadTD, pLastTD;
-	unsigned int uNodeIndex = 0;
-	unsigned char byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
-	unsigned short wAID;
-	unsigned int uMACfragNum = 1;
-	unsigned int cbFrameBodySize;
-	unsigned char byPktType;
-	unsigned int cbHeaderSize;
-	bool bNeedEncryption = false;
-	PSKeyItem       pTransmitKey = NULL;
-	SKeyItem        STempKey;
-	unsigned int ii;
-	bool bTKIP_UseGTK = false;
-	bool bNeedDeAuth = false;
-	unsigned char *pbyBSSID;
-	bool bNodeExist = false;
-
-	spin_lock_irq(&pDevice->lock);
-	if (!pDevice->bLinkPass) {
-		dev_kfree_skb_irq(skb);
-		spin_unlock_irq(&pDevice->lock);
-		return 0;
-	}
-
-	if (pDevice->bStopDataPkt) {
-		dev_kfree_skb_irq(skb);
-		spin_unlock_irq(&pDevice->lock);
-		return 0;
-	}
-
-	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-		if (pDevice->uAssocCount == 0) {
-			dev_kfree_skb_irq(skb);
-			spin_unlock_irq(&pDevice->lock);
-			return 0;
-		}
-		if (is_multicast_ether_addr((unsigned char *)(skb->data))) {
-			uNodeIndex = 0;
-			bNodeExist = true;
-			if (pMgmt->sNodeDBTable[0].bPSEnable) {
-				skb_queue_tail(&(pMgmt->sNodeDBTable[0].sTxPSQueue), skb);
-				pMgmt->sNodeDBTable[0].wEnQueueCnt++;
-				// set tx map
-				pMgmt->abyPSTxMap[0] |= byMask[0];
-				spin_unlock_irq(&pDevice->lock);
-				return 0;
-			}
-		} else {
-			if (BSSDBbIsSTAInNodeDB(pMgmt, (unsigned char *)(skb->data), &uNodeIndex)) {
-				if (pMgmt->sNodeDBTable[uNodeIndex].bPSEnable) {
-					skb_queue_tail(&pMgmt->sNodeDBTable[uNodeIndex].sTxPSQueue, skb);
-					pMgmt->sNodeDBTable[uNodeIndex].wEnQueueCnt++;
-					// set tx map
-					wAID = pMgmt->sNodeDBTable[uNodeIndex].wAID;
-					pMgmt->abyPSTxMap[wAID >> 3] |=  byMask[wAID & 7];
-					pr_debug("Set:pMgmt->abyPSTxMap[%d]= %d\n",
-						 (wAID >> 3),
-						 pMgmt->abyPSTxMap[wAID >> 3]);
-					spin_unlock_irq(&pDevice->lock);
-					return 0;
-				}
-
-				if (pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble)
-					pDevice->byPreambleType = pDevice->byShortPreamble;
-				else
-					pDevice->byPreambleType = PREAMBLE_LONG;
-
-				bNodeExist = true;
-
-			}
-		}
-
-		if (!bNodeExist) {
-			pr_debug("Unknown STA not found in node DB\n");
-			dev_kfree_skb_irq(skb);
-			spin_unlock_irq(&pDevice->lock);
-			return 0;
-		}
-	}
-
-	pHeadTD = pDevice->apCurrTD[TYPE_AC0DMA];
-
-	pHeadTD->m_td1TD1.byTCR = (TCR_EDP|TCR_STP);
-
-	memcpy(pDevice->sTxEthHeader.abyDstAddr, (unsigned char *)(skb->data), ETH_HLEN);
-	cbFrameBodySize = skb->len - ETH_HLEN;
-	// 802.1H
-	if (ntohs(pDevice->sTxEthHeader.wType) > ETH_DATA_LEN)
-		cbFrameBodySize += 8;
-
-	if (pDevice->bEncryptionEnable) {
-		bNeedEncryption = true;
-		// get Transmit key
-		do {
-			if ((pDevice->pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
-			    (pDevice->pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
-				pbyBSSID = pDevice->abyBSSID;
-				// get pairwise key
-				if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, PAIRWISE_KEY, &pTransmitKey) == false) {
-					// get group key
-					if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == true) {
-						bTKIP_UseGTK = true;
-						pr_debug("Get GTK\n");
-						break;
-					}
-				} else {
-					pr_debug("Get PTK\n");
-					break;
-				}
-			} else if (pDevice->pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-				pbyBSSID = pDevice->sTxEthHeader.abyDstAddr;  //TO_DS = 0 and FROM_DS = 0 --> 802.11 MAC Address1
-				pr_debug("IBSS Serach Key:\n");
-				for (ii = 0; ii < 6; ii++)
-					pr_debug("%x\n", *(pbyBSSID+ii));
-				pr_debug("\n");
-
-				// get pairwise key
-				if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, PAIRWISE_KEY, &pTransmitKey) == true)
-					break;
-			}
-			// get group key
-			pbyBSSID = pDevice->abyBroadcastAddr;
-			if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == false) {
-				pTransmitKey = NULL;
-				if (pDevice->pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)
-					pr_debug("IBSS and KEY is NULL. [%d]\n",
-						 pDevice->pMgmt->eCurrMode);
-				else
-					pr_debug("NOT IBSS and KEY is NULL. [%d]\n",
-						 pDevice->pMgmt->eCurrMode);
-			} else {
-				bTKIP_UseGTK = true;
-				pr_debug("Get GTK\n");
-			}
-		} while (false);
-	}
-
-	if (pDevice->bEnableHostWEP) {
-		pr_debug("acdma0: STA index %d\n", uNodeIndex);
-		if (pDevice->bEncryptionEnable) {
-			pTransmitKey = &STempKey;
-			pTransmitKey->byCipherSuite = pMgmt->sNodeDBTable[uNodeIndex].byCipherSuite;
-			pTransmitKey->dwKeyIndex = pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex;
-			pTransmitKey->uKeyLength = pMgmt->sNodeDBTable[uNodeIndex].uWepKeyLength;
-			pTransmitKey->dwTSC47_16 = pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16;
-			pTransmitKey->wTSC15_0 = pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0;
-			memcpy(pTransmitKey->abyKey,
-			       &pMgmt->sNodeDBTable[uNodeIndex].abyWepKey[0],
-			       pTransmitKey->uKeyLength
-				);
-		}
-	}
-
-	uMACfragNum = cbGetFragCount(pDevice, pTransmitKey, cbFrameBodySize, &pDevice->sTxEthHeader);
-
-	if (uMACfragNum > AVAIL_TD(pDevice, TYPE_AC0DMA)) {
-		pr_debug("uMACfragNum > AVAIL_TD(TYPE_AC0DMA) = %d\n",
-			 uMACfragNum);
-		dev_kfree_skb_irq(skb);
-		spin_unlock_irq(&pDevice->lock);
-		return 0;
-	}
-
-	if (pTransmitKey != NULL) {
-		if ((pTransmitKey->byCipherSuite == KEY_CTL_WEP) &&
-		    (pTransmitKey->uKeyLength == WLAN_WEP232_KEYLEN)) {
-			uMACfragNum = 1; //WEP256 doesn't support fragment
-		}
-	}
-
-	byPktType = (unsigned char)pDevice->byPacketType;
-
-	if (pDevice->bFixRate) {
-		if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
-			if (pDevice->uConnectionRate >= RATE_11M)
-				pDevice->wCurrentRate = RATE_11M;
-			else
-				pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
-		} else {
-			if ((pDevice->eCurrentPHYType == PHY_TYPE_11A) &&
-			    (pDevice->uConnectionRate <= RATE_6M)) {
-				pDevice->wCurrentRate = RATE_6M;
-			} else {
-				if (pDevice->uConnectionRate >= RATE_54M)
-					pDevice->wCurrentRate = RATE_54M;
-				else
-					pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
-
-			}
-		}
-		pDevice->byACKRate = (unsigned char) pDevice->wCurrentRate;
-		pDevice->byTopCCKBasicRate = RATE_1M;
-		pDevice->byTopOFDMBasicRate = RATE_6M;
-	} else {
-		//auto rate
-		if (pDevice->sTxEthHeader.wType == TYPE_PKT_802_1x) {
-			if (pDevice->eCurrentPHYType != PHY_TYPE_11A) {
-				pDevice->wCurrentRate = RATE_1M;
-				pDevice->byACKRate = RATE_1M;
-				pDevice->byTopCCKBasicRate = RATE_1M;
-				pDevice->byTopOFDMBasicRate = RATE_6M;
-			} else {
-				pDevice->wCurrentRate = RATE_6M;
-				pDevice->byACKRate = RATE_6M;
-				pDevice->byTopCCKBasicRate = RATE_1M;
-				pDevice->byTopOFDMBasicRate = RATE_6M;
-			}
-		} else {
-			VNTWIFIvGetTxRate(pDevice->pMgmt,
-					  pDevice->sTxEthHeader.abyDstAddr,
-					  &(pDevice->wCurrentRate),
-					  &(pDevice->byACKRate),
-					  &(pDevice->byTopCCKBasicRate),
-					  &(pDevice->byTopOFDMBasicRate));
-
-		}
-	}
-
-
-	if (pDevice->wCurrentRate <= RATE_11M) {
-		byPktType = PK_TYPE_11B;
-	} else if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
-		byPktType = PK_TYPE_11A;
-	} else {
-		if (pDevice->bProtectMode)
-			byPktType = PK_TYPE_11GB;
-		else
-			byPktType = PK_TYPE_11GA;
-	}
-
-	if (bNeedEncryption) {
-		pr_debug("ntohs Pkt Type=%04x\n",
-			 ntohs(pDevice->sTxEthHeader.wType));
-		if ((pDevice->sTxEthHeader.wType) == TYPE_PKT_802_1x) {
-			bNeedEncryption = false;
-			pr_debug("Pkt Type=%04x\n",
-				 (pDevice->sTxEthHeader.wType));
-			if ((pDevice->pMgmt->eCurrMode == WMAC_MODE_ESS_STA) && (pDevice->pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
-				if (pTransmitKey == NULL) {
-					pr_debug("Don't Find TX KEY\n");
-				} else {
-					if (bTKIP_UseGTK) {
-						pr_debug("error: KEY is GTK!!~~\n");
-					} else {
-						pr_debug("Find PTK [%lX]\n",
-							 pTransmitKey->dwKeyIndex);
-						bNeedEncryption = true;
-					}
-				}
-			}
-
-			if (pDevice->byCntMeasure == 2) {
-				bNeedDeAuth = true;
-				pDevice->s802_11Counter.TKIPCounterMeasuresInvoked++;
-			}
-
-			if (pDevice->bEnableHostWEP) {
-				if ((uNodeIndex != 0) &&
-				    (pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex & PAIRWISE_KEY)) {
-					pr_debug("Find PTK [%lX]\n",
-						 pTransmitKey->dwKeyIndex);
-					bNeedEncryption = true;
-				}
-			}
-		} else {
-			if (pTransmitKey == NULL) {
-				pr_debug("return no tx key\n");
-				dev_kfree_skb_irq(skb);
-				spin_unlock_irq(&pDevice->lock);
-				return 0;
-			}
-		}
-	}
-
-	vGenerateFIFOHeader(pDevice, byPktType, pDevice->pbyTmpBuff, bNeedEncryption,
-			    cbFrameBodySize, TYPE_AC0DMA, pHeadTD,
-			    &pDevice->sTxEthHeader, (unsigned char *)skb->data, pTransmitKey, uNodeIndex,
-			    &uMACfragNum,
-			    &cbHeaderSize
-		);
-
-	if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS)) {
-		// Disable PS
-		MACbPSWakeup(pDevice->PortOffset);
-	}
-	pDevice->bPWBitOn = false;
-
-	pLastTD = pHeadTD;
-	for (ii = 0; ii < uMACfragNum; ii++) {
-		// Poll Transmit the adapter
-		wmb();
-		pHeadTD->m_td0TD0.f1Owner = OWNED_BY_NIC;
-		wmb();
-		if (ii == uMACfragNum - 1)
-			pLastTD = pHeadTD;
-		pHeadTD = pHeadTD->next;
-	}
-
-	// Save the information needed by the tx interrupt handler
-	// to complete the Send request
-	pLastTD->pTDInfo->skb = skb;
-	pLastTD->pTDInfo->byFlags = 0;
-	pLastTD->pTDInfo->byFlags |= TD_FLAGS_NETIF_SKB;
-	pDevice->nTxDataTimeCout = 0; //2008-8-21 chester <add> for send null packet
-
-	if (AVAIL_TD(pDevice, TYPE_AC0DMA) <= 1)
-		netif_stop_queue(dev);
-
-	pDevice->apCurrTD[TYPE_AC0DMA] = pHeadTD;
-
-	if (pDevice->bFixRate)
-		pr_debug("FixRate:Rate is %d,TxPower is %d\n", pDevice->wCurrentRate, pDevice->byCurPwr);
-
-	{
-		unsigned char Protocol_Version;    //802.1x Authentication
-		unsigned char Packet_Type;           //802.1x Authentication
-		unsigned char Descriptor_type;
-		unsigned short Key_info;
-		bool bTxeapol_key = false;
-
-		Protocol_Version = skb->data[ETH_HLEN];
-		Packet_Type = skb->data[ETH_HLEN+1];
-		Descriptor_type = skb->data[ETH_HLEN+1+1+2];
-		Key_info = (skb->data[ETH_HLEN+1+1+2+1] << 8)|(skb->data[ETH_HLEN+1+1+2+2]);
-		if (pDevice->sTxEthHeader.wType == TYPE_PKT_802_1x) {
-			if (((Protocol_Version == 1) || (Protocol_Version == 2)) &&
-			    (Packet_Type == 3)) {  //802.1x OR eapol-key challenge frame transfer
-				bTxeapol_key = true;
-				if ((Descriptor_type == 254) || (Descriptor_type == 2)) {       //WPA or RSN
-					if (!(Key_info & BIT3) &&   //group-key challenge
-					    (Key_info & BIT8) && (Key_info & BIT9)) {    //send 2/2 key
-						pDevice->fWPA_Authened = true;
-						if (Descriptor_type == 254)
-							pr_debug("WPA ");
-						else
-							pr_debug("WPA2 ");
-						pr_debug("Authentication completed!!\n");
-					}
-				}
-			}
-		}
-	}
-
-	MACvTransmitAC0(pDevice->PortOffset);
-
-	dev->trans_start = jiffies;
-
-	spin_unlock_irq(&pDevice->lock);
-	return 0;
-}
-
 static  irqreturn_t  device_intr(int irq,  void *dev_instance)
 {
-	struct net_device *dev = dev_instance;
-	struct vnt_private *pDevice = netdev_priv(dev);
+	struct vnt_private *pDevice = dev_instance;
 	int             max_count = 0;
 	unsigned long dwMIBCounter = 0;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned char byOrgPageSel = 0;
 	int             handled = 0;
-	unsigned char byData = 0;
 	int             ii = 0;
 	unsigned long flags;
 
@@ -2256,96 +1102,13 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance)
 			device_error(pDevice, pDevice->dwIsr);
 		}
 
-		if (pDevice->byLocalID > REV_ID_VT3253_B1) {
-			if (pDevice->dwIsr & ISR_MEASURESTART) {
-				// 802.11h measure start
-				pDevice->byOrgChannel = pDevice->byCurrentCh;
-				VNSvInPortB(pDevice->PortOffset + MAC_REG_RCR, &(pDevice->byOrgRCR));
-				VNSvOutPortB(pDevice->PortOffset + MAC_REG_RCR, (RCR_RXALLTYPE | RCR_UNICAST | RCR_BROADCAST | RCR_MULTICAST | RCR_WPAERR));
-				MACvSelectPage1(pDevice->PortOffset);
-				VNSvInPortD(pDevice->PortOffset + MAC_REG_MAR0, &(pDevice->dwOrgMAR0));
-				VNSvInPortD(pDevice->PortOffset + MAC_REG_MAR4, &(pDevice->dwOrgMAR4));
-				MACvSelectPage0(pDevice->PortOffset);
-				//xxxx
-				if (set_channel(pDevice, pDevice->pCurrMeasureEID->sReq.byChannel)) {
-					pDevice->bMeasureInProgress = true;
-					MACvSelectPage1(pDevice->PortOffset);
-					MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_READY);
-					MACvSelectPage0(pDevice->PortOffset);
-					pDevice->byBasicMap = 0;
-					pDevice->byCCAFraction = 0;
-					for (ii = 0; ii < 8; ii++)
-						pDevice->dwRPIs[ii] = 0;
-
-				} else {
-					// can not measure because set channel fail
-					// clear measure control
-					MACvRegBitsOff(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_EN);
-					s_vCompleteCurrentMeasure(pDevice, MEASURE_MODE_INCAPABLE);
-					MACvSelectPage1(pDevice->PortOffset);
-					MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
-					MACvSelectPage0(pDevice->PortOffset);
-				}
-			}
-			if (pDevice->dwIsr & ISR_MEASUREEND) {
-				// 802.11h measure end
-				pDevice->bMeasureInProgress = false;
-				VNSvOutPortB(pDevice->PortOffset + MAC_REG_RCR, pDevice->byOrgRCR);
-				MACvSelectPage1(pDevice->PortOffset);
-				VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR0, pDevice->dwOrgMAR0);
-				VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR4, pDevice->dwOrgMAR4);
-				VNSvInPortB(pDevice->PortOffset + MAC_REG_MSRBBSTS, &byData);
-				pDevice->byBasicMap |= (byData >> 4);
-				VNSvInPortB(pDevice->PortOffset + MAC_REG_CCAFRACTION, &pDevice->byCCAFraction);
-				VNSvInPortB(pDevice->PortOffset + MAC_REG_MSRCTL, &byData);
-				// clear measure control
-				MACvRegBitsOff(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_EN);
-				MACvSelectPage0(pDevice->PortOffset);
-				set_channel(pDevice, pDevice->byOrgChannel);
-				MACvSelectPage1(pDevice->PortOffset);
-				MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
-				MACvSelectPage0(pDevice->PortOffset);
-				if (byData & MSRCTL_FINISH) {
-					// measure success
-					s_vCompleteCurrentMeasure(pDevice, 0);
-				} else {
-					// can not measure because not ready before end of measure time
-					s_vCompleteCurrentMeasure(pDevice, MEASURE_MODE_LATE);
-				}
-			}
-			if (pDevice->dwIsr & ISR_QUIETSTART) {
-				do {
-					;
-				} while (!CARDbStartQuiet(pDevice));
-			}
-		}
-
 		if (pDevice->dwIsr & ISR_TBTT) {
-			if (pDevice->bEnableFirstQuiet) {
-				pDevice->byQuietStartCount--;
-				if (pDevice->byQuietStartCount == 0) {
-					pDevice->bEnableFirstQuiet = false;
-					MACvSelectPage1(pDevice->PortOffset);
-					MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL, (MSRCTL_QUIETTXCHK | MSRCTL_QUIETEN));
-					MACvSelectPage0(pDevice->PortOffset);
-				}
-			}
-			if (pDevice->bChannelSwitch &&
-			    (pDevice->op_mode == NL80211_IFTYPE_STATION)) {
-				pDevice->byChannelSwitchCount--;
-				if (pDevice->byChannelSwitchCount == 0) {
-					pDevice->bChannelSwitch = false;
-					set_channel(pDevice, pDevice->byNewChannel);
-					VNTWIFIbChannelSwitch(pDevice->pMgmt, pDevice->byNewChannel);
-					MACvSelectPage1(pDevice->PortOffset);
-					MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
-					MACvSelectPage0(pDevice->PortOffset);
-					CARDbStartTxPacket(pDevice, PKT_TYPE_802_11_ALL);
-
-				}
-			}
-			if (pDevice->op_mode != NL80211_IFTYPE_ADHOC) {
-				if ((pDevice->bUpdateBBVGA) && pDevice->bLinkPass && (pDevice->uCurrRSSI != 0)) {
+			if (pDevice->vif &&
+			    pDevice->op_mode != NL80211_IFTYPE_ADHOC) {
+				if (pDevice->bUpdateBBVGA &&
+				    !(pDevice->hw->conf.flags & IEEE80211_CONF_OFFCHANNEL) &&
+				    pDevice->vif->bss_conf.assoc &&
+				    pDevice->uCurrRSSI) {
 					long            ldBm;
 
 					RFvRSSITodBm(pDevice, (unsigned char) pDevice->uCurrRSSI, &ldBm);
@@ -2384,10 +1147,11 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance)
 			if (pDevice->bEnablePSMode)
 				PSbIsNextTBTTWakeUp((void *)pDevice);
 
-			if ((pDevice->op_mode == NL80211_IFTYPE_AP) ||
-			    (pDevice->op_mode == NL80211_IFTYPE_ADHOC)) {
+			if ((pDevice->op_mode == NL80211_IFTYPE_AP ||
+			    pDevice->op_mode == NL80211_IFTYPE_ADHOC) &&
+			    pDevice->vif->bss_conf.enable_beacon) {
 				MACvOneShotTimer1MicroSec(pDevice->PortOffset,
-							  (pMgmt->wIBSSBeaconPeriod - MAKE_BEACON_RESERVED) << 10);
+							  (pDevice->vif->bss_conf.beacon_int - MAKE_BEACON_RESERVED) << 10);
 			}
 
 			/* TODO: adhoc PS mode */
@@ -2400,34 +1164,7 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance)
 				pDevice->cbBeaconBufReadySetCnt = 0;
 			}
 
-			if (pDevice->op_mode == NL80211_IFTYPE_AP) {
-				if (pMgmt->byDTIMCount > 0) {
-					pMgmt->byDTIMCount--;
-					pMgmt->sNodeDBTable[0].bRxPSPoll = false;
-				} else {
-					if (pMgmt->byDTIMCount == 0) {
-						// check if mutltcast tx bufferring
-						pMgmt->byDTIMCount = pMgmt->byDTIMPeriod - 1;
-						pMgmt->sNodeDBTable[0].bRxPSPoll = true;
-						bScheduleCommand((void *)pDevice, WLAN_CMD_RX_PSPOLL, NULL);
-					}
-				}
-			}
 			pDevice->bBeaconSent = true;
-
-			if (pDevice->bChannelSwitch) {
-				pDevice->byChannelSwitchCount--;
-				if (pDevice->byChannelSwitchCount == 0) {
-					pDevice->bChannelSwitch = false;
-					set_channel(pDevice, pDevice->byNewChannel);
-					VNTWIFIbChannelSwitch(pDevice->pMgmt, pDevice->byNewChannel);
-					MACvSelectPage1(pDevice->PortOffset);
-					MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
-					MACvSelectPage0(pDevice->PortOffset);
-					CARDbStartTxPacket(pDevice, PKT_TYPE_802_11_ALL);
-				}
-			}
-
 		}
 
 		if (pDevice->dwIsr & ISR_RXDMA0)
@@ -2443,14 +1180,18 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance)
 			max_count += device_tx_srv(pDevice, TYPE_AC0DMA);
 
 		if (pDevice->dwIsr & ISR_SOFTTIMER1) {
-			if (pDevice->op_mode == NL80211_IFTYPE_AP) {
-				if (pDevice->bShortSlotTime)
-					pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTSLOTTIME(1);
-				else
-					pMgmt->wCurrCapInfo &= ~(WLAN_SET_CAP_INFO_SHORTSLOTTIME(1));
+			if (pDevice->vif) {
+				if (pDevice->vif->bss_conf.enable_beacon)
+					vnt_beacon_make(pDevice, pDevice->vif);
 			}
-			bMgrPrepareBeaconToSend(pDevice, pMgmt);
-			pDevice->byCntMeasure = 0;
+		}
+
+		/* If both buffers available wake the queue */
+		if (pDevice->vif) {
+			if (AVAIL_TD(pDevice, TYPE_TXDMA0) &&
+			    AVAIL_TD(pDevice, TYPE_AC0DMA) &&
+			    ieee80211_queue_stopped(pDevice->hw, 0))
+				ieee80211_wake_queues(pDevice->hw);
 		}
 
 		MACvReadISR(pDevice->PortOffset, &pDevice->dwIsr);
@@ -2472,550 +1213,655 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance)
 	return IRQ_RETVAL(handled);
 }
 
-//2008-8-4 <add> by chester
-static int Config_FileGetParameter(unsigned char *string,
-				   unsigned char *dest, unsigned char *source)
+static int vnt_tx_packet(struct vnt_private *priv, struct sk_buff *skb)
 {
-	unsigned char buf1[100];
-	int source_len = strlen(source);
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	PSTxDesc head_td;
+	u32 dma_idx = TYPE_AC0DMA;
+	unsigned long flags;
 
-	memset(buf1, 0, 100);
-	strcat(buf1, string);
-	strcat(buf1, "=");
-	source += strlen(buf1);
+	spin_lock_irqsave(&priv->lock, flags);
 
-	memcpy(dest, source, source_len - strlen(buf1));
-	return true;
-}
+	if (!ieee80211_is_data(hdr->frame_control))
+		dma_idx = TYPE_TXDMA0;
 
-int Config_FileOperation(struct vnt_private *pDevice,
-			 bool fwrite, unsigned char *Parameter)
-{
-	unsigned char *buffer = kmalloc(1024, GFP_KERNEL);
-	unsigned char tmpbuffer[20];
-	struct file *file;
-	int result = 0;
-
-	if (!buffer) {
-		pr_err("allocate mem for file fail?\n");
-		return -1;
-	}
-	file = filp_open(CONFIG_PATH, O_RDONLY, 0);
-	if (IS_ERR(file)) {
-		kfree(buffer);
-		pr_err("Config_FileOperation:open file fail?\n");
-		return -1;
+	if (AVAIL_TD(priv, dma_idx) < 1) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return -ENOMEM;
 	}
 
-	if (kernel_read(file, 0, buffer, 1024) < 0) {
-		pr_err("read file error?\n");
-		result = -1;
-		goto error1;
-	}
+	head_td = priv->apCurrTD[dma_idx];
 
-	if (Config_FileGetParameter("ZONETYPE", tmpbuffer, buffer) != true) {
-		pr_err("get parameter error?\n");
-		result = -1;
-		goto error1;
-	}
+	head_td->m_td1TD1.byTCR = (TCR_EDP|TCR_STP);
 
-	if (memcmp(tmpbuffer, "USA", 3) == 0) {
-		result = ZoneType_USA;
-	} else if (memcmp(tmpbuffer, "JAPAN", 5) == 0) {
-		result = ZoneType_Japan;
-	} else if (memcmp(tmpbuffer, "EUROPE", 5) == 0) {
-		result = ZoneType_Europe;
-	} else {
-		result = -1;
-		pr_err("Unknown Zonetype[%s]?\n", tmpbuffer);
-	}
+	head_td->pTDInfo->skb = skb;
+
+	priv->iTDUsed[dma_idx]++;
+
+	/* Take ownership */
+	wmb();
+	head_td->m_td0TD0.f1Owner = OWNED_BY_NIC;
+
+	/* get Next */
+	wmb();
+	priv->apCurrTD[dma_idx] = head_td->next;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	vnt_generate_fifo_header(priv, dma_idx, head_td, skb);
+
+	if (MACbIsRegBitsOn(priv->PortOffset, MAC_REG_PSCTL, PSCTL_PS))
+		MACbPSWakeup(priv->PortOffset);
+
+	spin_lock_irqsave(&priv->lock, flags);
 
-error1:
-	kfree(buffer);
-	fput(file);
-	return result;
+	priv->bPWBitOn = false;
+
+	head_td->pTDInfo->byFlags = TD_FLAGS_NETIF_SKB;
+
+	if (dma_idx == TYPE_AC0DMA)
+		MACvTransmitAC0(priv->PortOffset);
+	else
+		MACvTransmit0(priv->PortOffset);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
 }
 
-static void device_set_multi(struct net_device *dev) {
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject     pMgmt = pDevice->pMgmt;
-	u32              mc_filter[2];
-	struct netdev_hw_addr *ha;
+static void vnt_tx_80211(struct ieee80211_hw *hw,
+			 struct ieee80211_tx_control *control,
+			 struct sk_buff *skb)
+{
+	struct vnt_private *priv = hw->priv;
 
-	VNSvInPortB(pDevice->PortOffset + MAC_REG_RCR, &(pDevice->byRxMode));
+	ieee80211_stop_queues(hw);
 
-	if (dev->flags & IFF_PROMISC) {         /* Set promiscuous. */
-		pr_notice("%s: Promiscuous mode enabled\n", dev->name);
-		/* Unconditionally log net taps. */
-		pDevice->byRxMode |= (RCR_MULTICAST|RCR_BROADCAST|RCR_UNICAST);
-	} else if ((netdev_mc_count(dev) > pDevice->multicast_limit)
-		 ||  (dev->flags & IFF_ALLMULTI)) {
-		MACvSelectPage1(pDevice->PortOffset);
-		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR0, 0xffffffff);
-		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR0 + 4, 0xffffffff);
-		MACvSelectPage0(pDevice->PortOffset);
-		pDevice->byRxMode |= (RCR_MULTICAST|RCR_BROADCAST);
-	} else {
-		memset(mc_filter, 0, sizeof(mc_filter));
-		netdev_for_each_mc_addr(ha, dev) {
-			int bit_nr = ether_crc(ETH_ALEN, ha->addr) >> 26;
+	if (vnt_tx_packet(priv, skb)) {
+		ieee80211_free_txskb(hw, skb);
 
-			mc_filter[bit_nr >> 5] |= cpu_to_le32(1 << (bit_nr & 31));
-		}
-		MACvSelectPage1(pDevice->PortOffset);
-		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR0, mc_filter[0]);
-		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MAR0 + 4, mc_filter[1]);
-		MACvSelectPage0(pDevice->PortOffset);
-		pDevice->byRxMode &= ~(RCR_UNICAST);
-		pDevice->byRxMode |= (RCR_MULTICAST|RCR_BROADCAST);
+		ieee80211_wake_queues(hw);
 	}
+}
+
+static int vnt_start(struct ieee80211_hw *hw)
+{
+	struct vnt_private *priv = hw->priv;
+	int ret;
 
-	if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
-		// If AP mode, don't enable RCR_UNICAST. Since hw only compare addr1 with local mac.
-		pDevice->byRxMode |= (RCR_MULTICAST|RCR_BROADCAST);
-		pDevice->byRxMode &= ~(RCR_UNICAST);
+	priv->rx_buf_sz = PKT_BUF_SZ;
+	if (!device_init_rings(priv))
+		return -ENOMEM;
+
+	ret = request_irq(priv->pcid->irq, &device_intr,
+			  IRQF_SHARED, "vt6655", priv);
+	if (ret) {
+		dev_dbg(&priv->pcid->dev, "failed to start irq\n");
+		return ret;
 	}
 
-	VNSvOutPortB(pDevice->PortOffset + MAC_REG_RCR, pDevice->byRxMode);
-	pr_debug("pDevice->byRxMode = %x\n", pDevice->byRxMode);
+	dev_dbg(&priv->pcid->dev, "call device init rd0 ring\n");
+	device_init_rd0_ring(priv);
+	device_init_rd1_ring(priv);
+	device_init_td0_ring(priv);
+	device_init_td1_ring(priv);
+
+	device_init_registers(priv);
+
+	dev_dbg(&priv->pcid->dev, "call MACvIntEnable\n");
+	MACvIntEnable(priv->PortOffset, IMR_MASK_VALUE);
+
+	ieee80211_wake_queues(hw);
+
+	return 0;
 }
 
-static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
+static void vnt_stop(struct ieee80211_hw *hw)
 {
-	struct vnt_private *pDevice = netdev_priv(dev);
-	struct iwreq *wrq = (struct iwreq *)rq;
-	int rc = 0;
-	PSMgmtObject pMgmt = pDevice->pMgmt;
-	PSCmdRequest pReq;
-
-	if (pMgmt == NULL) {
-		rc = -EFAULT;
-		return rc;
-	}
+	struct vnt_private *priv = hw->priv;
 
-	switch (cmd) {
-	case SIOCGIWNAME:
-		rc = iwctl_giwname(dev, NULL, (char *)&(wrq->u.name), NULL);
-		break;
+	ieee80211_stop_queues(hw);
+
+	MACbShutdown(priv->PortOffset);
+	MACbSoftwareReset(priv->PortOffset);
+	CARDbRadioPowerOff(priv);
+
+	device_free_td0_ring(priv);
+	device_free_td1_ring(priv);
+	device_free_rd0_ring(priv);
+	device_free_rd1_ring(priv);
+	device_free_rings(priv);
+
+	free_irq(priv->pcid->irq, priv);
+}
+
+static int vnt_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct vnt_private *priv = hw->priv;
+
+	priv->vif = vif;
 
-	case SIOCGIWNWID:     //0x8b03  support
-		rc = -EOPNOTSUPP;
+	switch (vif->type) {
+	case NL80211_IFTYPE_STATION:
+		if (priv->bDiversityRegCtlON)
+			device_init_diversity_timer(priv);
 		break;
+	case NL80211_IFTYPE_ADHOC:
+		MACvRegBitsOff(priv->PortOffset, MAC_REG_RCR, RCR_UNICAST);
+
+		MACvRegBitsOn(priv->PortOffset, MAC_REG_HOSTCR, HOSTCR_ADHOC);
 
-		// Set frequency/channel
-	case SIOCSIWFREQ:
-		rc = iwctl_siwfreq(dev, NULL, &(wrq->u.freq), NULL);
 		break;
+	case NL80211_IFTYPE_AP:
+		MACvRegBitsOff(priv->PortOffset, MAC_REG_RCR, RCR_UNICAST);
+
+		MACvRegBitsOn(priv->PortOffset, MAC_REG_HOSTCR, HOSTCR_AP);
 
-		// Get frequency/channel
-	case SIOCGIWFREQ:
-		rc = iwctl_giwfreq(dev, NULL, &(wrq->u.freq), NULL);
 		break;
+	default:
+		return -EOPNOTSUPP;
+	}
 
-		// Set desired network name (ESSID)
-	case SIOCSIWESSID:
+	priv->op_mode = vif->type;
 
-	{
-		char essid[IW_ESSID_MAX_SIZE+1];
+	return 0;
+}
 
-		if (wrq->u.essid.length > IW_ESSID_MAX_SIZE) {
-			rc = -E2BIG;
-			break;
-		}
-		if (copy_from_user(essid, wrq->u.essid.pointer,
-				   wrq->u.essid.length)) {
-			rc = -EFAULT;
-			break;
+static void vnt_remove_interface(struct ieee80211_hw *hw,
+				 struct ieee80211_vif *vif)
+{
+	struct vnt_private *priv = hw->priv;
+
+	switch (vif->type) {
+	case NL80211_IFTYPE_STATION:
+		if (priv->bDiversityRegCtlON) {
+			del_timer(&priv->TimerSQ3Tmax1);
+			del_timer(&priv->TimerSQ3Tmax2);
+			del_timer(&priv->TimerSQ3Tmax3);
 		}
-		rc = iwctl_siwessid(dev, NULL,
-				    &(wrq->u.essid), essid);
+		break;
+	case NL80211_IFTYPE_ADHOC:
+		MACvRegBitsOff(priv->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
+		MACvRegBitsOff(priv->PortOffset,
+			       MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
+		MACvRegBitsOff(priv->PortOffset, MAC_REG_HOSTCR, HOSTCR_ADHOC);
+		break;
+	case NL80211_IFTYPE_AP:
+		MACvRegBitsOff(priv->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
+		MACvRegBitsOff(priv->PortOffset,
+			       MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
+		MACvRegBitsOff(priv->PortOffset, MAC_REG_HOSTCR, HOSTCR_AP);
+		break;
+	default:
+		break;
 	}
-	break;
 
-	// Get current network name (ESSID)
-	case SIOCGIWESSID:
+	priv->op_mode = NL80211_IFTYPE_UNSPECIFIED;
+}
 
-	{
-		char essid[IW_ESSID_MAX_SIZE+1];
-
-		if (wrq->u.essid.pointer)
-			rc = iwctl_giwessid(dev, NULL,
-					    &(wrq->u.essid), essid);
-		if (copy_to_user(wrq->u.essid.pointer,
-				 essid,
-				 wrq->u.essid.length))
-			rc = -EFAULT;
+
+static int vnt_config(struct ieee80211_hw *hw, u32 changed)
+{
+	struct vnt_private *priv = hw->priv;
+	struct ieee80211_conf *conf = &hw->conf;
+	u8 bb_type;
+
+	if (changed & IEEE80211_CONF_CHANGE_PS) {
+		if (conf->flags & IEEE80211_CONF_PS)
+			PSvEnablePowerSaving(priv, conf->listen_interval);
+		else
+			PSvDisablePowerSaving(priv);
 	}
-	break;
 
-	case SIOCSIWAP:
+	if ((changed & IEEE80211_CONF_CHANGE_CHANNEL) ||
+	    (conf->flags & IEEE80211_CONF_OFFCHANNEL)) {
+		set_channel(priv, conf->chandef.chan->hw_value);
 
-		rc = iwctl_siwap(dev, NULL, &(wrq->u.ap_addr), NULL);
-		break;
+		if (conf->chandef.chan->band == IEEE80211_BAND_5GHZ)
+			bb_type = BB_TYPE_11A;
+		else
+			bb_type = BB_TYPE_11G;
 
-		// Get current Access Point (BSSID)
-	case SIOCGIWAP:
-		rc = iwctl_giwap(dev, NULL, &(wrq->u.ap_addr), NULL);
-		break;
+		if (priv->byBBType != bb_type) {
+			priv->byBBType = bb_type;
 
-		// Set desired station name
-	case SIOCSIWNICKN:
-		pr_debug(" SIOCSIWNICKN\n");
-		rc = -EOPNOTSUPP;
-		break;
+			CARDbSetPhyParameter(priv, priv->byBBType);
+		}
+	}
 
-		// Get current station name
-	case SIOCGIWNICKN:
-		pr_debug(" SIOCGIWNICKN\n");
-		rc = -EOPNOTSUPP;
-		break;
+	if (changed & IEEE80211_CONF_CHANGE_POWER) {
+		if (priv->byBBType == BB_TYPE_11B)
+			priv->wCurrentRate = RATE_1M;
+		else
+			priv->wCurrentRate = RATE_54M;
 
-		// Set the desired bit-rate
-	case SIOCSIWRATE:
-		rc = iwctl_siwrate(dev, NULL, &(wrq->u.bitrate), NULL);
-		break;
+		RFbSetPower(priv, priv->wCurrentRate,
+			    conf->chandef.chan->hw_value);
+	}
+
+	return 0;
+}
 
-		// Get the current bit-rate
-	case SIOCGIWRATE:
+static void vnt_bss_info_changed(struct ieee80211_hw *hw,
+		struct ieee80211_vif *vif, struct ieee80211_bss_conf *conf,
+		u32 changed)
+{
+	struct vnt_private *priv = hw->priv;
 
-		rc = iwctl_giwrate(dev, NULL, &(wrq->u.bitrate), NULL);
-		break;
+	priv->current_aid = conf->aid;
 
-		// Set the desired RTS threshold
-	case SIOCSIWRTS:
+	if (changed & BSS_CHANGED_BSSID)
+		MACvWriteBSSIDAddress(priv->PortOffset, (u8 *)conf->bssid);
 
-		rc = iwctl_siwrts(dev, NULL, &(wrq->u.rts), NULL);
-		break;
+	if (changed & BSS_CHANGED_BASIC_RATES) {
+		priv->basic_rates = conf->basic_rates;
 
-		// Get the current RTS threshold
-	case SIOCGIWRTS:
+		CARDvUpdateBasicTopRate(priv);
 
-		rc = iwctl_giwrts(dev, NULL, &(wrq->u.rts), NULL);
-		break;
+		dev_dbg(&priv->pcid->dev,
+			"basic rates %x\n", conf->basic_rates);
+	}
 
-		// Set the desired fragmentation threshold
-	case SIOCSIWFRAG:
+	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
+		if (conf->use_short_preamble) {
+			MACvEnableBarkerPreambleMd(priv->PortOffset);
+			priv->byPreambleType = true;
+		} else {
+			MACvDisableBarkerPreambleMd(priv->PortOffset);
+			priv->byPreambleType = false;
+		}
+	}
 
-		rc = iwctl_siwfrag(dev, NULL, &(wrq->u.frag), NULL);
-		break;
+	if (changed & BSS_CHANGED_ERP_CTS_PROT) {
+		if (conf->use_cts_prot)
+			MACvEnableProtectMD(priv->PortOffset);
+		else
+			MACvDisableProtectMD(priv->PortOffset);
+	}
 
-		// Get the current fragmentation threshold
-	case SIOCGIWFRAG:
+	if (changed & BSS_CHANGED_ERP_SLOT) {
+		if (conf->use_short_slot)
+			priv->bShortSlotTime = true;
+		else
+			priv->bShortSlotTime = false;
 
-		rc = iwctl_giwfrag(dev, NULL, &(wrq->u.frag), NULL);
-		break;
+		CARDbSetPhyParameter(priv, priv->byBBType);
+		BBvSetVGAGainOffset(priv, priv->abyBBVGA[0]);
+	}
 
-		// Set mode of operation
-	case SIOCSIWMODE:
-		rc = iwctl_siwmode(dev, NULL, &(wrq->u.mode), NULL);
-		break;
+	if (changed & BSS_CHANGED_TXPOWER)
+		RFbSetPower(priv, priv->wCurrentRate,
+			    conf->chandef.chan->hw_value);
 
-		// Get mode of operation
-	case SIOCGIWMODE:
-		rc = iwctl_giwmode(dev, NULL, &(wrq->u.mode), NULL);
-		break;
+	if (changed & BSS_CHANGED_BEACON_ENABLED) {
+		dev_dbg(&priv->pcid->dev,
+			"Beacon enable %d\n", conf->enable_beacon);
 
-		// Set WEP keys and mode
-	case SIOCSIWENCODE: {
-		char abyKey[WLAN_WEP232_KEYLEN];
+		if (conf->enable_beacon) {
+			vnt_beacon_enable(priv, vif, conf);
 
-		if (wrq->u.encoding.pointer) {
-			if (wrq->u.encoding.length > WLAN_WEP232_KEYLEN) {
-				rc = -E2BIG;
-				break;
-			}
-			memset(abyKey, 0, WLAN_WEP232_KEYLEN);
-			if (copy_from_user(abyKey,
-					   wrq->u.encoding.pointer,
-					   wrq->u.encoding.length)) {
-				rc = -EFAULT;
-				break;
-			}
-		} else if (wrq->u.encoding.length != 0) {
-			rc = -EINVAL;
-			break;
+			MACvRegBitsOn(priv, MAC_REG_TCR, TCR_AUTOBCNTX);
+		} else {
+			MACvRegBitsOff(priv, MAC_REG_TCR, TCR_AUTOBCNTX);
 		}
-		rc = iwctl_siwencode(dev, NULL, &(wrq->u.encoding), abyKey);
 	}
-	break;
 
-	// Get the WEP keys and mode
-	case SIOCGIWENCODE:
+	if (changed & BSS_CHANGED_ASSOC && priv->op_mode != NL80211_IFTYPE_AP) {
+		if (conf->assoc) {
+			CARDbUpdateTSF(priv, conf->beacon_rate->hw_value,
+				       conf->sync_device_ts, conf->sync_tsf);
 
-		if (!capable(CAP_NET_ADMIN)) {
-			rc = -EPERM;
-			break;
+			CARDbSetBeaconPeriod(priv, conf->beacon_int);
+
+			CARDvSetFirstNextTBTT(priv, conf->beacon_int);
+		} else {
+			VNSvOutPortB(priv->PortOffset + MAC_REG_TFTCTL,
+				     TFTCTL_TSFCNTRST);
+			VNSvOutPortB(priv->PortOffset + MAC_REG_TFTCTL,
+				     TFTCTL_TSFCNTREN);
 		}
-		{
-			char abyKey[WLAN_WEP232_KEYLEN];
+	}
+}
 
-			rc = iwctl_giwencode(dev, NULL, &(wrq->u.encoding), abyKey);
-			if (rc != 0)
-				break;
-			if (wrq->u.encoding.pointer) {
-				if (copy_to_user(wrq->u.encoding.pointer,
-						 abyKey,
-						 wrq->u.encoding.length))
-					rc = -EFAULT;
+static u64 vnt_prepare_multicast(struct ieee80211_hw *hw,
+	struct netdev_hw_addr_list *mc_list)
+{
+	struct vnt_private *priv = hw->priv;
+	struct netdev_hw_addr *ha;
+	u64 mc_filter = 0;
+	u32 bit_nr = 0;
+
+	netdev_hw_addr_list_for_each(ha, mc_list) {
+		bit_nr = ether_crc(ETH_ALEN, ha->addr) >> 26;
+
+		mc_filter |= 1ULL << (bit_nr & 0x3f);
+	}
+
+	priv->mc_list_count = mc_list->count;
+
+	return mc_filter;
+}
+
+static void vnt_configure(struct ieee80211_hw *hw,
+	unsigned int changed_flags, unsigned int *total_flags, u64 multicast)
+{
+	struct vnt_private *priv = hw->priv;
+	u8 rx_mode = 0;
+
+	*total_flags &= FIF_ALLMULTI | FIF_OTHER_BSS | FIF_PROMISC_IN_BSS |
+		FIF_BCN_PRBRESP_PROMISC;
+
+	VNSvInPortB(priv->PortOffset + MAC_REG_RCR, &rx_mode);
+
+	dev_dbg(&priv->pcid->dev, "rx mode in = %x\n", rx_mode);
+
+	if (changed_flags & FIF_PROMISC_IN_BSS) {
+		/* unconditionally log net taps */
+		if (*total_flags & FIF_PROMISC_IN_BSS)
+			rx_mode |= RCR_UNICAST;
+		else
+			rx_mode &= ~RCR_UNICAST;
+	}
+
+	if (changed_flags & FIF_ALLMULTI) {
+		if (*total_flags & FIF_ALLMULTI) {
+			if (priv->mc_list_count > 2) {
+				MACvSelectPage1(priv->PortOffset);
+
+				VNSvOutPortD(priv->PortOffset +
+					     MAC_REG_MAR0, 0xffffffff);
+				VNSvOutPortD(priv->PortOffset +
+					    MAC_REG_MAR0 + 4, 0xffffffff);
+
+				MACvSelectPage0(priv->PortOffset);
+			} else {
+				MACvSelectPage1(priv->PortOffset);
+
+				VNSvOutPortD(priv->PortOffset +
+					     MAC_REG_MAR0, (u32)multicast);
+				VNSvOutPortD(priv->PortOffset +
+					     MAC_REG_MAR0 + 4,
+					     (u32)(multicast >> 32));
+
+				MACvSelectPage0(priv->PortOffset);
 			}
+
+			rx_mode |= RCR_MULTICAST | RCR_BROADCAST;
+		} else {
+			rx_mode &= ~(RCR_MULTICAST | RCR_BROADCAST);
 		}
-		break;
+	}
 
-		// Get the current Tx-Power
-	case SIOCGIWTXPOW:
-		pr_debug(" SIOCGIWTXPOW\n");
-		rc = -EOPNOTSUPP;
-		break;
+	if (changed_flags & (FIF_OTHER_BSS | FIF_BCN_PRBRESP_PROMISC)) {
+		rx_mode |= RCR_MULTICAST | RCR_BROADCAST;
 
-	case SIOCSIWTXPOW:
-		pr_debug(" SIOCSIWTXPOW\n");
-		rc = -EOPNOTSUPP;
-		break;
+		if (*total_flags & (FIF_OTHER_BSS | FIF_BCN_PRBRESP_PROMISC))
+			rx_mode &= ~RCR_BSSID;
+		else
+			rx_mode |= RCR_BSSID;
+	}
 
-	case SIOCSIWRETRY:
+	VNSvOutPortB(priv->PortOffset + MAC_REG_RCR, rx_mode);
 
-		rc = iwctl_siwretry(dev, NULL, &(wrq->u.retry), NULL);
-		break;
+	dev_dbg(&priv->pcid->dev, "rx mode out= %x\n", rx_mode);
+}
 
-	case SIOCGIWRETRY:
+static int vnt_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 vnt_private *priv = hw->priv;
 
-		rc = iwctl_giwretry(dev, NULL, &(wrq->u.retry), NULL);
+	switch (cmd) {
+	case SET_KEY:
+		if (vnt_set_keys(hw, sta, vif, key))
+			return -EOPNOTSUPP;
 		break;
+	case DISABLE_KEY:
+		if (test_bit(key->hw_key_idx, &priv->key_entry_inuse))
+			clear_bit(key->hw_key_idx, &priv->key_entry_inuse);
+	default:
+		break;
+	}
 
-		// Get range of parameters
-	case SIOCGIWRANGE:
+	return 0;
+}
 
-	{
-		struct iw_range range;
+static u64 vnt_get_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct vnt_private *priv = hw->priv;
+	u64 tsf;
 
-		rc = iwctl_giwrange(dev, NULL, &(wrq->u.data), (char *)&range);
-		if (copy_to_user(wrq->u.data.pointer, &range, sizeof(struct iw_range)))
-			rc = -EFAULT;
-	}
+	CARDbGetCurrentTSF(priv, &tsf);
 
-	break;
+	return tsf;
+}
 
-	case SIOCGIWPOWER:
+static void vnt_set_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+			u64 tsf)
+{
+	struct vnt_private *priv = hw->priv;
 
-		rc = iwctl_giwpower(dev, NULL, &(wrq->u.power), NULL);
-		break;
+	CARDvUpdateNextTBTT(priv, tsf, vif->bss_conf.beacon_int);
+}
 
-	case SIOCSIWPOWER:
+static void vnt_reset_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct vnt_private *priv = hw->priv;
 
-		rc = iwctl_siwpower(dev, NULL, &(wrq->u.power), NULL);
-		break;
+	/* reset TSF counter */
+	VNSvOutPortB(priv->PortOffset + MAC_REG_TFTCTL, TFTCTL_TSFCNTRST);
+}
 
-	case SIOCGIWSENS:
+static const struct ieee80211_ops vnt_mac_ops = {
+	.tx			= vnt_tx_80211,
+	.start			= vnt_start,
+	.stop			= vnt_stop,
+	.add_interface		= vnt_add_interface,
+	.remove_interface	= vnt_remove_interface,
+	.config			= vnt_config,
+	.bss_info_changed	= vnt_bss_info_changed,
+	.prepare_multicast	= vnt_prepare_multicast,
+	.configure_filter	= vnt_configure,
+	.set_key		= vnt_set_key,
+	.get_tsf		= vnt_get_tsf,
+	.set_tsf		= vnt_set_tsf,
+	.reset_tsf		= vnt_reset_tsf,
+};
 
-		rc = iwctl_giwsens(dev, NULL, &(wrq->u.sens), NULL);
-		break;
+int vnt_init(struct vnt_private *priv)
+{
+	SET_IEEE80211_PERM_ADDR(priv->hw, priv->abyCurrentNetAddr);
 
-	case SIOCSIWSENS:
-		pr_debug(" SIOCSIWSENS\n");
-		rc = -EOPNOTSUPP;
-		break;
+	vnt_init_bands(priv);
 
-	case SIOCGIWAPLIST: {
-		char buffer[IW_MAX_AP * (sizeof(struct sockaddr) + sizeof(struct iw_quality))];
-
-		if (wrq->u.data.pointer) {
-			rc = iwctl_giwaplist(dev, NULL, &(wrq->u.data), buffer);
-			if (rc == 0) {
-				if (copy_to_user(wrq->u.data.pointer,
-						 buffer,
-						 (wrq->u.data.length * (sizeof(struct sockaddr) +  sizeof(struct iw_quality)))
-					    ))
-					rc = -EFAULT;
-			}
-		}
-	}
-	break;
+	if (ieee80211_register_hw(priv->hw))
+		return -ENODEV;
 
-#ifdef WIRELESS_SPY
-	// Set the spy list
-	case SIOCSIWSPY:
+	priv->mac_hw = true;
 
-		pr_debug(" SIOCSIWSPY\n");
-		rc = -EOPNOTSUPP;
-		break;
+	CARDbRadioPowerOff(priv);
 
-		// Get the spy list
-	case SIOCGIWSPY:
+	return 0;
+}
 
-		pr_debug(" SIOCGIWSPY\n");
-		rc = -EOPNOTSUPP;
-		break;
+static int
+vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
+{
+	PCHIP_INFO  pChip_info = (PCHIP_INFO)ent->driver_data;
+	struct vnt_private *priv;
+	struct ieee80211_hw *hw;
+	struct wiphy *wiphy;
+	int         rc;
 
-#endif // WIRELESS_SPY
+	dev_notice(&pcid->dev,
+		   "%s Ver. %s\n", DEVICE_FULL_DRV_NAM, DEVICE_VERSION);
 
-	case SIOCGIWPRIV:
-		pr_debug(" SIOCGIWPRIV\n");
-		rc = -EOPNOTSUPP;
-		break;
+	dev_notice(&pcid->dev,
+		   "Copyright (c) 2003 VIA Networking Technologies, Inc.\n");
 
-//2008-0409-07, <Add> by Einsn Liu
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-	case SIOCSIWAUTH:
-		pr_debug(" SIOCSIWAUTH\n");
-		rc = iwctl_siwauth(dev, NULL, &(wrq->u.param), NULL);
-		break;
+	hw = ieee80211_alloc_hw(sizeof(*priv), &vnt_mac_ops);
+	if (!hw) {
+		dev_err(&pcid->dev, "could not register ieee80211_hw\n");
+		return -ENOMEM;
+	}
 
-	case SIOCGIWAUTH:
-		pr_debug(" SIOCGIWAUTH\n");
-		rc = iwctl_giwauth(dev, NULL, &(wrq->u.param), NULL);
-		break;
+	priv = hw->priv;
 
-	case SIOCSIWGENIE:
-		pr_debug(" SIOCSIWGENIE\n");
-		rc = iwctl_siwgenie(dev, NULL, &(wrq->u.data), wrq->u.data.pointer);
-		break;
+	vt6655_init_info(pcid, &priv, pChip_info);
 
-	case SIOCGIWGENIE:
-		pr_debug(" SIOCGIWGENIE\n");
-		rc = iwctl_giwgenie(dev, NULL, &(wrq->u.data), wrq->u.data.pointer);
-		break;
+	priv->hw = hw;
 
-	case SIOCSIWENCODEEXT: {
-		char extra[sizeof(struct iw_encode_ext)+MAX_KEY_LEN+1];
+	SET_IEEE80211_DEV(priv->hw, &pcid->dev);
 
-		pr_debug(" SIOCSIWENCODEEXT\n");
-		if (wrq->u.encoding.pointer) {
-			memset(extra, 0, sizeof(struct iw_encode_ext)+MAX_KEY_LEN + 1);
-			if (wrq->u.encoding.length > (sizeof(struct iw_encode_ext) + MAX_KEY_LEN)) {
-				rc = -E2BIG;
-				break;
-			}
-			if (copy_from_user(extra, wrq->u.encoding.pointer, wrq->u.encoding.length)) {
-				rc = -EFAULT;
-				break;
-			}
-		} else if (wrq->u.encoding.length != 0) {
-			rc = -EINVAL;
-			break;
-		}
-		rc = iwctl_siwencodeext(dev, NULL, &(wrq->u.encoding), extra);
+	if (pci_enable_device(pcid)) {
+		device_free_info(priv);
+		return -ENODEV;
 	}
-	break;
 
-	case SIOCGIWENCODEEXT:
-		pr_debug(" SIOCGIWENCODEEXT\n");
-		rc = iwctl_giwencodeext(dev, NULL, &(wrq->u.encoding), NULL);
-		break;
+	dev_dbg(&pcid->dev,
+		"Before get pci_info memaddr is %x\n", priv->memaddr);
 
-	case SIOCSIWMLME:
-		pr_debug(" SIOCSIWMLME\n");
-		rc = iwctl_siwmlme(dev, NULL, &(wrq->u.data), wrq->u.data.pointer);
-		break;
+	if (!device_get_pci_info(priv, pcid)) {
+		dev_err(&pcid->dev, ": Failed to find PCI device.\n");
+		device_free_info(priv);
+		return -ENODEV;
+	}
 
-#endif // #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-//End Add -- //2008-0409-07, <Add> by Einsn Liu
+#ifdef	DEBUG
+	dev_dbg(&pcid->dev,
+		"after get pci_info memaddr is %x, io addr is %x,io_size is %d\n",
+		priv->memaddr, priv->ioaddr, priv->io_size);
+	{
+		int i;
+		u32 bar, len;
+		u32 address[] = {
+			PCI_BASE_ADDRESS_0,
+			PCI_BASE_ADDRESS_1,
+			PCI_BASE_ADDRESS_2,
+			PCI_BASE_ADDRESS_3,
+			PCI_BASE_ADDRESS_4,
+			PCI_BASE_ADDRESS_5,
+			0};
+		for (i = 0; address[i]; i++) {
+			pci_read_config_dword(pcid, address[i], &bar);
 
-	case IOCTL_CMD_TEST:
+			dev_dbg(&pcid->dev, "bar %d is %x\n", i, bar);
 
-		if (!(pDevice->flags & DEVICE_FLAGS_OPENED)) {
-			rc = -EFAULT;
-			break;
-		} else {
-			rc = 0;
-		}
-		pReq = (PSCmdRequest)rq;
-		pReq->wResult = MAGIC_CODE;
-		break;
+			if (!bar) {
+				dev_dbg(&pcid->dev,
+					"bar %d not implemented\n", i);
+				continue;
+			}
 
-	case IOCTL_CMD_SET:
+			if (bar & PCI_BASE_ADDRESS_SPACE_IO) {
+				/* This is IO */
 
-#ifdef SndEvt_ToAPI
-		if ((((PSCmdRequest)rq)->wCmdCode != WLAN_CMD_SET_EVT) &&
-		    !(pDevice->flags & DEVICE_FLAGS_OPENED))
-#else
-			if (!(pDevice->flags & DEVICE_FLAGS_OPENED) &&
-			    (((PSCmdRequest)rq)->wCmdCode != WLAN_CMD_SET_WPA))
-#endif
-			{
-				rc = -EFAULT;
-				break;
+				len = bar & (PCI_BASE_ADDRESS_IO_MASK & 0xffff);
+				len = len & ~(len - 1);
+
+				dev_dbg(&pcid->dev,
+					"IO space:  len in IO %x, BAR %d\n",
+					len, i);
 			} else {
-				rc = 0;
+				len = bar & 0xfffffff0;
+				len = ~len + 1;
+
+				dev_dbg(&pcid->dev,
+					"len in MEM %x, BAR %d\n", len, i);
 			}
+		}
+	}
+#endif
 
-		if (test_and_set_bit(0, (void *)&(pMgmt->uCmdBusy)))
-			return -EBUSY;
+	priv->PortOffset = ioremap(priv->memaddr & PCI_BASE_ADDRESS_MEM_MASK,
+				   priv->io_size);
+	if (!priv->PortOffset) {
+		dev_err(&pcid->dev, ": Failed to IO remapping ..\n");
+		device_free_info(priv);
+		return -ENODEV;
+	}
 
-		rc = private_ioctl(pDevice, rq);
-		clear_bit(0, (void *)&(pMgmt->uCmdBusy));
-		break;
+	rc = pci_request_regions(pcid, DEVICE_NAME);
+	if (rc) {
+		dev_err(&pcid->dev, ": Failed to find PCI device\n");
+		device_free_info(priv);
+		return -ENODEV;
+	}
 
-	case IOCTL_CMD_HOSTAPD:
+	/* do reset */
+	if (!MACbSoftwareReset(priv->PortOffset)) {
+		dev_err(&pcid->dev, ": Failed to access MAC hardware..\n");
+		device_free_info(priv);
+		return -ENODEV;
+	}
+	/* initial to reload eeprom */
+	MACvInitialize(priv->PortOffset);
+	MACvReadEtherAddress(priv->PortOffset, priv->abyCurrentNetAddr);
 
-		rc = vt6655_hostap_ioctl(pDevice, &wrq->u.data);
-		break;
+	device_get_options(priv);
+	device_set_options(priv);
+	/* Mask out the options cannot be set to the chip */
+	priv->sOpts.flags &= pChip_info->flags;
 
-	case IOCTL_CMD_WPA:
+	/* Enable the chip specified capabilities */
+	priv->flags = priv->sOpts.flags | (pChip_info->flags & 0xff000000UL);
 
-		rc = wpa_ioctl(pDevice, &wrq->u.data);
-		break;
+	wiphy = priv->hw->wiphy;
 
-	case SIOCETHTOOL:
-		return ethtool_ioctl(dev, rq->ifr_data);
-		// All other calls are currently unsupported
+	wiphy->frag_threshold = FRAG_THRESH_DEF;
+	wiphy->rts_threshold = RTS_THRESH_DEF;
+	wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
+		BIT(NL80211_IFTYPE_ADHOC) | BIT(NL80211_IFTYPE_AP);
 
-	default:
-		rc = -EOPNOTSUPP;
-		pr_debug("Ioctl command not support..%x\n", cmd);
+	priv->hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
+		IEEE80211_HW_REPORTS_TX_ACK_STATUS |
+		IEEE80211_HW_SIGNAL_DBM |
+		IEEE80211_HW_TIMING_BEACON_ONLY;
 
-	}
+	priv->hw->max_signal = 100;
 
-	if (pDevice->bCommit) {
-		if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
-			netif_stop_queue(pDevice->dev);
-			spin_lock_irq(&pDevice->lock);
-			bScheduleCommand((void *)pDevice, WLAN_CMD_RUN_AP, NULL);
-			spin_unlock_irq(&pDevice->lock);
-		} else {
-			pr_debug("Commit the settings\n");
-			spin_lock_irq(&pDevice->lock);
-			pDevice->bLinkPass = false;
-			memset(pMgmt->abyCurrBSSID, 0, 6);
-			pMgmt->eCurrState = WMAC_STATE_IDLE;
-			netif_stop_queue(pDevice->dev);
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-			pMgmt->eScanType = WMAC_SCAN_ACTIVE;
-			if (!pDevice->bWPASuppWextEnabled)
-#endif
-				bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, pMgmt->abyDesireSSID);
-			bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, NULL);
-			spin_unlock_irq(&pDevice->lock);
-		}
-		pDevice->bCommit = false;
-	}
+	if (vnt_init(priv))
+		return -ENODEV;
 
-	return rc;
+	device_print_info(priv);
+	pci_set_drvdata(pcid, priv);
+
+	return 0;
 }
 
-static int ethtool_ioctl(struct net_device *dev, void __user *useraddr)
+/*------------------------------------------------------------------*/
+
+#ifdef CONFIG_PM
+static int vt6655_suspend(struct pci_dev *pcid, pm_message_t state)
 {
-	u32 ethcmd;
+	struct vnt_private *priv = pci_get_drvdata(pcid);
+	unsigned long flags;
 
-	if (copy_from_user(&ethcmd, useraddr, sizeof(ethcmd)))
-		return -EFAULT;
+	spin_lock_irqsave(&priv->lock, flags);
 
-	switch (ethcmd) {
-	case ETHTOOL_GDRVINFO: {
-		struct ethtool_drvinfo info = {ETHTOOL_GDRVINFO};
+	pci_save_state(pcid);
 
-		strncpy(info.driver, DEVICE_NAME, sizeof(info.driver)-1);
-		strncpy(info.version, DEVICE_VERSION, sizeof(info.version)-1);
-		if (copy_to_user(useraddr, &info, sizeof(info)))
-			return -EFAULT;
-		return 0;
-	}
+	MACbShutdown(priv->PortOffset);
 
-	}
+	pci_disable_device(pcid);
+	pci_set_power_state(pcid, pci_choose_state(pcid, state));
+
+	spin_unlock_irqrestore(&priv->lock, flags);
 
-	return -EOPNOTSUPP;
+	return 0;
 }
 
-/*------------------------------------------------------------------*/
+static int vt6655_resume(struct pci_dev *pcid)
+{
+
+	pci_set_power_state(pcid, PCI_D0);
+	pci_enable_wake(pcid, PCI_D0, 0);
+	pci_restore_state(pcid);
+
+	return 0;
+}
+#endif
 
 MODULE_DEVICE_TABLE(pci, vt6655_pci_id_table);
 
@@ -3025,8 +1871,8 @@ static struct pci_driver device_driver = {
 	.probe = vt6655_probe,
 	.remove = vt6655_remove,
 #ifdef CONFIG_PM
-	.suspend = viawget_suspend,
-	.resume = viawget_resume,
+	.suspend = vt6655_suspend,
+	.resume = vt6655_resume,
 #endif
 };
 
@@ -3067,75 +1913,10 @@ device_notify_reboot(struct notifier_block *nb, unsigned long event, void *p)
 		for_each_pci_dev(pdev) {
 			if (pci_dev_driver(pdev) == &device_driver) {
 				if (pci_get_drvdata(pdev))
-					viawget_suspend(pdev, PMSG_HIBERNATE);
+					vt6655_suspend(pdev, PMSG_HIBERNATE);
 			}
 		}
 	}
 	return NOTIFY_DONE;
 }
-
-static int
-viawget_suspend(struct pci_dev *pcid, pm_message_t state)
-{
-	int power_status;   // to silence the compiler
-
-	struct vnt_private *pDevice = pci_get_drvdata(pcid);
-	PSMgmtObject  pMgmt = pDevice->pMgmt;
-
-	netif_stop_queue(pDevice->dev);
-	spin_lock_irq(&pDevice->lock);
-	pci_save_state(pcid);
-	del_timer(&pDevice->sTimerCommand);
-	del_timer(&pMgmt->sTimerSecondCallback);
-	pDevice->cbFreeCmdQueue = CMD_Q_SIZE;
-	pDevice->uCmdDequeueIdx = 0;
-	pDevice->uCmdEnqueueIdx = 0;
-	pDevice->bCmdRunning = false;
-	MACbShutdown(pDevice->PortOffset);
-	MACvSaveContext(pDevice->PortOffset, pDevice->abyMacContext);
-	pDevice->bLinkPass = false;
-	memset(pMgmt->abyCurrBSSID, 0, 6);
-	pMgmt->eCurrState = WMAC_STATE_IDLE;
-	pci_disable_device(pcid);
-	power_status = pci_set_power_state(pcid, pci_choose_state(pcid, state));
-	spin_unlock_irq(&pDevice->lock);
-	return 0;
-}
-
-static int
-viawget_resume(struct pci_dev *pcid)
-{
-	struct vnt_private *pDevice = pci_get_drvdata(pcid);
-	PSMgmtObject  pMgmt = pDevice->pMgmt;
-	int power_status;   // to silence the compiler
-
-	power_status = pci_set_power_state(pcid, PCI_D0);
-	power_status = pci_enable_wake(pcid, PCI_D0, 0);
-	pci_restore_state(pcid);
-	if (netif_running(pDevice->dev)) {
-		spin_lock_irq(&pDevice->lock);
-		MACvRestoreContext(pDevice->PortOffset, pDevice->abyMacContext);
-		device_init_registers(pDevice);
-		if (pMgmt->sNodeDBTable[0].bActive) { // Assoc with BSS
-			pMgmt->sNodeDBTable[0].bActive = false;
-			pDevice->bLinkPass = false;
-			if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-				// In Adhoc, BSS state set back to started.
-				pMgmt->eCurrState = WMAC_STATE_STARTED;
-			} else {
-				pMgmt->eCurrMode = WMAC_MODE_STANDBY;
-				pMgmt->eCurrState = WMAC_STATE_IDLE;
-			}
-		}
-		init_timer(&pMgmt->sTimerSecondCallback);
-		init_timer(&pDevice->sTimerCommand);
-		MACvIntEnable(pDevice->PortOffset, IMR_MASK_VALUE);
-		BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
-		bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, NULL);
-		bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, NULL);
-		spin_unlock_irq(&pDevice->lock);
-	}
-	return 0;
-}
-
 #endif
diff --git a/drivers/staging/vt6655/dpc.c b/drivers/staging/vt6655/dpc.c
index 8515b8c80801..977683cb7391 100644
--- a/drivers/staging/vt6655/dpc.c
+++ b/drivers/staging/vt6655/dpc.c
@@ -25,1289 +25,135 @@
  * Date: May 20, 2003
  *
  * Functions:
- *      device_receive_frame - Rcv 802.11 frame function
- *      s_bAPModeRxCtl- AP Rcv frame filer Ctl.
- *      s_bAPModeRxData- AP Rcv data frame handle
- *      s_bHandleRxEncryption- Rcv decrypted data via on-fly
- *      s_bHostWepRxEncryption- Rcv encrypted data via host
- *      s_byGetRateIdx- get rate index
- *      s_vGetDASA- get data offset
- *      s_vProcessRxMACHeader- Rcv 802.11 and translate to 802.3
  *
  * Revision History:
  *
  */
 
 #include "device.h"
-#include "rxtx.h"
-#include "tether.h"
-#include "card.h"
-#include "bssdb.h"
-#include "mac.h"
 #include "baseband.h"
-#include "michael.h"
-#include "tkip.h"
-#include "tcrc.h"
-#include "wctl.h"
-#include "wroute.h"
-#include "hostap.h"
 #include "rf.h"
-#include "iowpa.h"
-#include "aes_ccmp.h"
 #include "dpc.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-static const unsigned char acbyRxRate[MAX_RATE] =
-{2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108};
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-static unsigned char s_byGetRateIdx(unsigned char byRate);
-
-static void
-s_vGetDASA(unsigned char *pbyRxBufferAddr, unsigned int *pcbHeaderSize,
-	   PSEthernetHeader psEthHeader);
-
-static void
-s_vProcessRxMACHeader(struct vnt_private *pDevice, unsigned char *pbyRxBufferAddr,
-		      unsigned int cbPacketSize, bool bIsWEP, bool bExtIV,
-		      unsigned int *pcbHeadSize);
-
-static bool s_bAPModeRxCtl(
-	struct vnt_private *pDevice,
-	unsigned char *pbyFrame,
-	int      iSANodeIndex
-);
-
-static bool s_bAPModeRxData(
-	struct vnt_private *pDevice,
-	struct sk_buff *skb,
-	unsigned int FrameSize,
-	unsigned int cbHeaderOffset,
-	int      iSANodeIndex,
-	int      iDANodeIndex
-);
-
-static bool s_bHandleRxEncryption(
-	struct vnt_private *pDevice,
-	unsigned char *pbyFrame,
-	unsigned int FrameSize,
-	unsigned char *pbyRsr,
-	unsigned char *pbyNewRsr,
-	PSKeyItem   *pKeyOut,
-	bool *pbExtIV,
-	unsigned short *pwRxTSC15_0,
-	unsigned long *pdwRxTSC47_16
-);
-
-static bool s_bHostWepRxEncryption(
-
-	struct vnt_private *pDevice,
-	unsigned char *pbyFrame,
-	unsigned int FrameSize,
-	unsigned char *pbyRsr,
-	bool bOnFly,
-	PSKeyItem    pKey,
-	unsigned char *pbyNewRsr,
-	bool *pbExtIV,
-	unsigned short *pwRxTSC15_0,
-	unsigned long *pdwRxTSC47_16
-
-);
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*+
- *
- * Description:
- *    Translate Rcv 802.11 header to 802.3 header with Rx buffer
- *
- * Parameters:
- *  In:
- *      pDevice
- *      dwRxBufferAddr  - Address of Rcv Buffer
- *      cbPacketSize    - Rcv Packet size
- *      bIsWEP          - If Rcv with WEP
- *  Out:
- *      pcbHeaderSize   - 802.11 header size
- *
- * Return Value: None
- *
- -*/
-static void
-s_vProcessRxMACHeader(struct vnt_private *pDevice,
-		      unsigned char *pbyRxBufferAddr,
-		      unsigned int cbPacketSize, bool bIsWEP, bool bExtIV,
-		      unsigned int *pcbHeadSize)
+static bool vnt_rx_data(struct vnt_private *priv, struct sk_buff *skb,
+			u16 bytes_received)
 {
-	unsigned char *pbyRxBuffer;
-	unsigned int cbHeaderSize = 0;
-	unsigned short *pwType;
-	PS802_11Header  pMACHeader;
-	int             ii;
-
-	pMACHeader = (PS802_11Header) (pbyRxBufferAddr + cbHeaderSize);
-
-	s_vGetDASA((unsigned char *)pMACHeader, &cbHeaderSize, &pDevice->sRxEthHeader);
-
-	if (bIsWEP) {
-		if (bExtIV) {
-			// strip IV&ExtIV , add 8 byte
-			cbHeaderSize += (WLAN_HDR_ADDR3_LEN + 8);
-		} else {
-			// strip IV , add 4 byte
-			cbHeaderSize += (WLAN_HDR_ADDR3_LEN + 4);
-		}
-	} else {
-		cbHeaderSize += WLAN_HDR_ADDR3_LEN;
-	}
-
-	pbyRxBuffer = (unsigned char *)(pbyRxBufferAddr + cbHeaderSize);
-	if (ether_addr_equal(pbyRxBuffer, pDevice->abySNAP_Bridgetunnel)) {
-		cbHeaderSize += 6;
-	} else if (ether_addr_equal(pbyRxBuffer, pDevice->abySNAP_RFC1042)) {
-		cbHeaderSize += 6;
-		pwType = (unsigned short *)(pbyRxBufferAddr + cbHeaderSize);
-		if ((*pwType != TYPE_PKT_IPX) && (*pwType != cpu_to_le16(0xF380))) {
-		} else {
-			cbHeaderSize -= 8;
-			pwType = (unsigned short *)(pbyRxBufferAddr + cbHeaderSize);
-			if (bIsWEP) {
-				if (bExtIV)
-					*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 8);    // 8 is IV&ExtIV
-				else
-					*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 4);    // 4 is IV
-
-			} else {
-				*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN);
-			}
-		}
-	} else {
-		cbHeaderSize -= 2;
-		pwType = (unsigned short *)(pbyRxBufferAddr + cbHeaderSize);
-		if (bIsWEP) {
-			if (bExtIV)
-				*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 8);    // 8 is IV&ExtIV
-			else
-				*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 4);    // 4 is IV
-
-		} else {
-			*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN);
-		}
-	}
-
-	cbHeaderSize -= (ETH_ALEN * 2);
-	pbyRxBuffer = (unsigned char *)(pbyRxBufferAddr + cbHeaderSize);
-	for (ii = 0; ii < ETH_ALEN; ii++)
-		*pbyRxBuffer++ = pDevice->sRxEthHeader.abyDstAddr[ii];
-	for (ii = 0; ii < ETH_ALEN; ii++)
-		*pbyRxBuffer++ = pDevice->sRxEthHeader.abySrcAddr[ii];
-
-	*pcbHeadSize = cbHeaderSize;
-}
-
-static unsigned char s_byGetRateIdx(unsigned char byRate)
-{
-	unsigned char byRateIdx;
-
-	for (byRateIdx = 0; byRateIdx < MAX_RATE; byRateIdx++) {
-		if (acbyRxRate[byRateIdx % MAX_RATE] == byRate)
-			return byRateIdx;
-	}
-
-	return 0;
-}
-
-static void
-s_vGetDASA(unsigned char *pbyRxBufferAddr, unsigned int *pcbHeaderSize,
-	   PSEthernetHeader psEthHeader)
-{
-	unsigned int cbHeaderSize = 0;
-	PS802_11Header  pMACHeader;
-	int             ii;
-
-	pMACHeader = (PS802_11Header) (pbyRxBufferAddr + cbHeaderSize);
-
-	if ((pMACHeader->wFrameCtl & FC_TODS) == 0) {
-		if (pMACHeader->wFrameCtl & FC_FROMDS) {
-			for (ii = 0; ii < ETH_ALEN; ii++) {
-				psEthHeader->abyDstAddr[ii] = pMACHeader->abyAddr1[ii];
-				psEthHeader->abySrcAddr[ii] = pMACHeader->abyAddr3[ii];
-			}
-		} else {
-			// IBSS mode
-			for (ii = 0; ii < ETH_ALEN; ii++) {
-				psEthHeader->abyDstAddr[ii] = pMACHeader->abyAddr1[ii];
-				psEthHeader->abySrcAddr[ii] = pMACHeader->abyAddr2[ii];
-			}
-		}
-	} else {
-		// Is AP mode..
-		if (pMACHeader->wFrameCtl & FC_FROMDS) {
-			for (ii = 0; ii < ETH_ALEN; ii++) {
-				psEthHeader->abyDstAddr[ii] = pMACHeader->abyAddr3[ii];
-				psEthHeader->abySrcAddr[ii] = pMACHeader->abyAddr4[ii];
-				cbHeaderSize += 6;
-			}
-		} else {
-			for (ii = 0; ii < ETH_ALEN; ii++) {
-				psEthHeader->abyDstAddr[ii] = pMACHeader->abyAddr3[ii];
-				psEthHeader->abySrcAddr[ii] = pMACHeader->abyAddr2[ii];
-			}
-		}
-	}
-	*pcbHeaderSize = cbHeaderSize;
-}
-
-bool
-device_receive_frame(
-	struct vnt_private *pDevice,
-	PSRxDesc pCurrRD
-)
-{
-	PDEVICE_RD_INFO  pRDInfo = pCurrRD->pRDInfo;
-	struct net_device_stats *pStats = &pDevice->dev->stats;
-	struct sk_buff *skb;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	PSRxMgmtPacket  pRxPacket = &(pDevice->pMgmt->sRxPacket);
-	PS802_11Header  p802_11Header;
-	unsigned char *pbyRsr;
-	unsigned char *pbyNewRsr;
-	unsigned char *pbyRSSI;
-	__le64 *pqwTSFTime;
-	unsigned short *pwFrameSize;
-	unsigned char *pbyFrame;
-	bool bDeFragRx = false;
-	bool bIsWEP = false;
-	unsigned int cbHeaderOffset;
-	unsigned int FrameSize;
-	unsigned short wEtherType = 0;
-	int             iSANodeIndex = -1;
-	int             iDANodeIndex = -1;
-	unsigned int ii;
-	unsigned int cbIVOffset;
-	bool bExtIV = false;
-	unsigned char *pbyRxSts;
-	unsigned char *pbyRxRate;
-	unsigned char *pbySQ;
-	unsigned int cbHeaderSize;
-	PSKeyItem       pKey = NULL;
-	unsigned short wRxTSC15_0 = 0;
-	unsigned long dwRxTSC47_16 = 0;
-	SKeyItem        STempKey;
-	// 802.11h RPI
-	unsigned long dwDuration = 0;
-	long            ldBm = 0;
-	long            ldBmThreshold = 0;
-	PS802_11Header pMACHeader;
-	bool bRxeapol_key = false;
-
-	skb = pRDInfo->skb;
-
-	pci_unmap_single(pDevice->pcid, pRDInfo->skb_dma,
-			 pDevice->rx_buf_sz, PCI_DMA_FROMDEVICE);
-
-	pwFrameSize = (unsigned short *)(skb->data + 2);
-	FrameSize = cpu_to_le16(pCurrRD->m_rd1RD1.wReqCount) - cpu_to_le16(pCurrRD->m_rd0RD0.wResCount);
-
-	// Max: 2312Payload + 30HD +4CRC + 2Padding + 4Len + 8TSF + 4RSR
-	// Min (ACK): 10HD +4CRC + 2Padding + 4Len + 8TSF + 4RSR
-	if ((FrameSize > 2364) || (FrameSize <= 32)) {
-		// Frame Size error drop this packet.
-		pr_debug("---------- WRONG Length 1\n");
-		return false;
-	}
-
-	pbyRxSts = (unsigned char *)(skb->data);
-	pbyRxRate = (unsigned char *)(skb->data + 1);
-	pbyRsr = (unsigned char *)(skb->data + FrameSize - 1);
-	pbyRSSI = (unsigned char *)(skb->data + FrameSize - 2);
-	pbyNewRsr = (unsigned char *)(skb->data + FrameSize - 3);
-	pbySQ = (unsigned char *)(skb->data + FrameSize - 4);
-	pqwTSFTime = (__le64 *)(skb->data + FrameSize - 12);
-	pbyFrame = (unsigned char *)(skb->data + 4);
-
-	// get packet size
-	FrameSize = cpu_to_le16(*pwFrameSize);
-
-	if ((FrameSize > 2346)|(FrameSize < 14)) { // Max: 2312Payload + 30HD +4CRC
-		// Min: 14 bytes ACK
-		pr_debug("---------- WRONG Length 2\n");
-		return false;
-	}
-
-	// update receive statistic counter
-	STAvUpdateRDStatCounter(&pDevice->scStatistic,
-				*pbyRsr,
-				*pbyNewRsr,
-				*pbyRxRate,
-				pbyFrame,
-				FrameSize);
-
-	pMACHeader = (PS802_11Header)((unsigned char *)(skb->data) + 8);
-
-	if (pDevice->bMeasureInProgress) {
-		if ((*pbyRsr & RSR_CRCOK) != 0)
-			pDevice->byBasicMap |= 0x01;
-
-		dwDuration = (FrameSize << 4);
-		dwDuration /= acbyRxRate[*pbyRxRate%MAX_RATE];
-		if (*pbyRxRate <= RATE_11M) {
-			if (*pbyRxSts & 0x01) {
-				// long preamble
-				dwDuration += 192;
-			} else {
-				// short preamble
-				dwDuration += 96;
-			}
-		} else {
-			dwDuration += 16;
-		}
-		RFvRSSITodBm(pDevice, *pbyRSSI, &ldBm);
-		ldBmThreshold = -57;
-		for (ii = 7; ii > 0;) {
-			if (ldBm > ldBmThreshold)
-				break;
-
-			ldBmThreshold -= 5;
-			ii--;
-		}
-		pDevice->dwRPIs[ii] += dwDuration;
-		return false;
-	}
-
-	if (!is_multicast_ether_addr(pbyFrame)) {
-		if (WCTLbIsDuplicate(&(pDevice->sDupRxCache), (PS802_11Header)(skb->data + 4))) {
-			pDevice->s802_11Counter.FrameDuplicateCount++;
-			return false;
-		}
-	}
-
-	// Use for TKIP MIC
-	s_vGetDASA(skb->data+4, &cbHeaderSize, &pDevice->sRxEthHeader);
-
-	// filter packet send from myself
-	if (ether_addr_equal(pDevice->sRxEthHeader.abySrcAddr,
-			     pDevice->abyCurrentNetAddr))
-		return false;
-
-	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) || (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)) {
-		if (IS_CTL_PSPOLL(pbyFrame) || !IS_TYPE_CONTROL(pbyFrame)) {
-			p802_11Header = (PS802_11Header)(pbyFrame);
-			// get SA NodeIndex
-			if (BSSDBbIsSTAInNodeDB(pMgmt, (unsigned char *)(p802_11Header->abyAddr2), &iSANodeIndex)) {
-				pMgmt->sNodeDBTable[iSANodeIndex].ulLastRxJiffer = jiffies;
-				pMgmt->sNodeDBTable[iSANodeIndex].uInActiveCount = 0;
-			}
-		}
-	}
-
-	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-		if (s_bAPModeRxCtl(pDevice, pbyFrame, iSANodeIndex))
-			return false;
-	}
-
-	if (IS_FC_WEP(pbyFrame)) {
-		bool bRxDecryOK = false;
-
-		pr_debug("rx WEP pkt\n");
-		bIsWEP = true;
-		if ((pDevice->bEnableHostWEP) && (iSANodeIndex >= 0)) {
-			pKey = &STempKey;
-			pKey->byCipherSuite = pMgmt->sNodeDBTable[iSANodeIndex].byCipherSuite;
-			pKey->dwKeyIndex = pMgmt->sNodeDBTable[iSANodeIndex].dwKeyIndex;
-			pKey->uKeyLength = pMgmt->sNodeDBTable[iSANodeIndex].uWepKeyLength;
-			pKey->dwTSC47_16 = pMgmt->sNodeDBTable[iSANodeIndex].dwTSC47_16;
-			pKey->wTSC15_0 = pMgmt->sNodeDBTable[iSANodeIndex].wTSC15_0;
-			memcpy(pKey->abyKey,
-			       &pMgmt->sNodeDBTable[iSANodeIndex].abyWepKey[0],
-			       pKey->uKeyLength
-);
-
-			bRxDecryOK = s_bHostWepRxEncryption(pDevice,
-							    pbyFrame,
-							    FrameSize,
-							    pbyRsr,
-							    pMgmt->sNodeDBTable[iSANodeIndex].bOnFly,
-							    pKey,
-							    pbyNewRsr,
-							    &bExtIV,
-							    &wRxTSC15_0,
-							    &dwRxTSC47_16);
-		} else {
-			bRxDecryOK = s_bHandleRxEncryption(pDevice,
-							   pbyFrame,
-							   FrameSize,
-							   pbyRsr,
-							   pbyNewRsr,
-							   &pKey,
-							   &bExtIV,
-							   &wRxTSC15_0,
-							   &dwRxTSC47_16);
-		}
-
-		if (bRxDecryOK) {
-			if ((*pbyNewRsr & NEWRSR_DECRYPTOK) == 0) {
-				pr_debug("ICV Fail\n");
-				if ((pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
-				    (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||
-				    (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) ||
-				    (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
-				    (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) {
-					if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP))
-						pDevice->s802_11Counter.TKIPICVErrors++;
-					else if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_CCMP))
-						pDevice->s802_11Counter.CCMPDecryptErrors++;
-				}
-				return false;
-			}
-		} else {
-			pr_debug("WEP Func Fail\n");
-			return false;
-		}
-		if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_CCMP))
-			FrameSize -= 8;         // Message Integrity Code
-		else
-			FrameSize -= 4;         // 4 is ICV
-	}
-
-	//
-	// RX OK
-	//
-	//remove the CRC length
-	FrameSize -= ETH_FCS_LEN;
-
-	if ((!(*pbyRsr & (RSR_ADDRBROAD | RSR_ADDRMULTI))) && // unicast address
-	    (IS_FRAGMENT_PKT((skb->data+4)))
-) {
-		// defragment
-		bDeFragRx = WCTLbHandleFragment(pDevice, (PS802_11Header)(skb->data+4), FrameSize, bIsWEP, bExtIV);
-		pDevice->s802_11Counter.ReceivedFragmentCount++;
-		if (bDeFragRx) {
-			// defrag complete
-			skb = pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].skb;
-			FrameSize = pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].cbFrameLength;
-
-		} else {
-			return false;
-		}
-	}
-
-// Management & Control frame Handle
-	if ((IS_TYPE_DATA((skb->data+4))) == false) {
-		// Handle Control & Manage Frame
-
-		if (IS_TYPE_MGMT((skb->data+4))) {
-			unsigned char *pbyData1;
-			unsigned char *pbyData2;
-
-			pRxPacket->p80211Header = (PUWLAN_80211HDR)(skb->data+4);
-			pRxPacket->cbMPDULen = FrameSize;
-			pRxPacket->uRSSI = *pbyRSSI;
-			pRxPacket->bySQ = *pbySQ;
-			pRxPacket->qwLocalTSF = le64_to_cpu(*pqwTSFTime);
-			if (bIsWEP) {
-				// strip IV
-				pbyData1 = WLAN_HDR_A3_DATA_PTR(skb->data+4);
-				pbyData2 = WLAN_HDR_A3_DATA_PTR(skb->data+4) + 4;
-				for (ii = 0; ii < (FrameSize - 4); ii++) {
-					*pbyData1 = *pbyData2;
-					pbyData1++;
-					pbyData2++;
-				}
-			}
-			pRxPacket->byRxRate = s_byGetRateIdx(*pbyRxRate);
-			pRxPacket->byRxChannel = (*pbyRxSts) >> 2;
-
-			vMgrRxManagePacket((void *)pDevice, pDevice->pMgmt, pRxPacket);
-
-			// hostap Deamon handle 802.11 management
-			if (pDevice->bEnableHostapd) {
-				skb->dev = pDevice->apdev;
-				skb->data += 4;
-				skb->tail += 4;
-				skb_put(skb, FrameSize);
-				skb_reset_mac_header(skb);
-				skb->pkt_type = PACKET_OTHERHOST;
-				skb->protocol = htons(ETH_P_802_2);
-				memset(skb->cb, 0, sizeof(skb->cb));
-				netif_rx(skb);
-				return true;
-			}
-		}
-
+	struct ieee80211_hw *hw = priv->hw;
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_rx_status rx_status = { 0 };
+	struct ieee80211_hdr *hdr;
+	__le16 fc;
+	u8 *rsr, *new_rsr, *rssi;
+	__le64 *tsf_time;
+	u16 frame_size;
+	int ii, r;
+	u8 *rx_sts, *rx_rate, *sq;
+	u8 *skb_data;
+	u8 rate_idx = 0;
+	u8 rate[MAX_RATE] = {2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108};
+	long rx_dbm;
+
+	/* [31:16]RcvByteCount ( not include 4-byte Status ) */
+	frame_size = le16_to_cpu(*((__le16 *)(skb->data + 2)));
+	if (frame_size > 2346 || frame_size < 14) {
+		dev_dbg(&priv->pcid->dev, "------- WRONG Length 1\n");
 		return false;
-	} else {
-		if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-			//In AP mode, hw only check addr1(BSSID or RA) if equal to local MAC.
-			if (!(*pbyRsr & RSR_BSSIDOK)) {
-				if (bDeFragRx) {
-					if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
-						pr_err("%s: can not alloc more frag bufs\n",
-						       pDevice->dev->name);
-					}
-				}
-				return false;
-			}
-		} else {
-			// discard DATA packet while not associate || BSSID error
-			if (!pDevice->bLinkPass || !(*pbyRsr & RSR_BSSIDOK)) {
-				if (bDeFragRx) {
-					if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
-						pr_err("%s: can not alloc more frag bufs\n",
-						       pDevice->dev->name);
-					}
-				}
-				return false;
-			}
-			//mike add:station mode check eapol-key challenge--->
-			{
-				unsigned char Protocol_Version;    //802.1x Authentication
-				unsigned char Packet_Type;           //802.1x Authentication
-
-				if (bIsWEP)
-					cbIVOffset = 8;
-				else
-					cbIVOffset = 0;
-				wEtherType = (skb->data[cbIVOffset + 8 + 24 + 6] << 8) |
-					skb->data[cbIVOffset + 8 + 24 + 6 + 1];
-				Protocol_Version = skb->data[cbIVOffset + 8 + 24 + 6 + 1 + 1];
-				Packet_Type = skb->data[cbIVOffset + 8 + 24 + 6 + 1 + 1 + 1];
-				if (wEtherType == ETH_P_PAE) {         //Protocol Type in LLC-Header
-					if (((Protocol_Version == 1) || (Protocol_Version == 2)) &&
-					    (Packet_Type == 3)) {  //802.1x OR eapol-key challenge frame receive
-						bRxeapol_key = true;
-					}
-				}
-			}
-			//mike add:station mode check eapol-key challenge<---
-		}
-	}
-
-// Data frame Handle
-
-	if (pDevice->bEnablePSMode) {
-		if (!IS_FC_MOREDATA((skb->data+4))) {
-			if (pDevice->pMgmt->bInTIMWake == true)
-				pDevice->pMgmt->bInTIMWake = false;
-		}
-	}
-
-	// Now it only supports 802.11g Infrastructure Mode, and support rate must up to 54 Mbps
-	if (pDevice->bDiversityEnable && (FrameSize > 50) &&
-	    (pDevice->op_mode == NL80211_IFTYPE_STATION) &&
-	    pDevice->bLinkPass) {
-		BBvAntennaDiversity(pDevice, s_byGetRateIdx(*pbyRxRate), 0);
-	}
-
-	if (pDevice->byLocalID != REV_ID_VT3253_B1)
-		pDevice->uCurrRSSI = *pbyRSSI;
-
-	pDevice->byCurrSQ = *pbySQ;
-
-	if ((*pbyRSSI != 0) &&
-	    (pMgmt->pCurrBSS != NULL)) {
-		RFvRSSITodBm(pDevice, *pbyRSSI, &ldBm);
-		// Monitor if RSSI is too strong.
-		pMgmt->pCurrBSS->byRSSIStatCnt++;
-		pMgmt->pCurrBSS->byRSSIStatCnt %= RSSI_STAT_COUNT;
-		pMgmt->pCurrBSS->ldBmAverage[pMgmt->pCurrBSS->byRSSIStatCnt] = ldBm;
-		for (ii = 0; ii < RSSI_STAT_COUNT; ii++)
-			if (pMgmt->pCurrBSS->ldBmAverage[ii] != 0)
-				pMgmt->pCurrBSS->ldBmMAX = max(pMgmt->pCurrBSS->ldBmAverage[ii], ldBm);
-
 	}
 
-	// -----------------------------------------------
+	skb_data = (u8 *)skb->data;
 
-	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) && pDevice->bEnable8021x) {
-		unsigned char abyMacHdr[24];
+	rx_sts = skb_data;
+	rx_rate = skb_data + 1;
 
-		// Only 802.1x packet incoming allowed
-		if (bIsWEP)
-			cbIVOffset = 8;
-		else
-			cbIVOffset = 0;
-		wEtherType = (skb->data[cbIVOffset + 4 + 24 + 6] << 8) |
-			skb->data[cbIVOffset + 4 + 24 + 6 + 1];
+	sband = hw->wiphy->bands[hw->conf.chandef.chan->band];
 
-		pr_debug("wEtherType = %04x\n", wEtherType);
-		if (wEtherType == ETH_P_PAE) {
-			skb->dev = pDevice->apdev;
-
-			if (bIsWEP) {
-				// strip IV header(8)
-				memcpy(&abyMacHdr[0], (skb->data + 4), 24);
-				memcpy((skb->data + 4 + cbIVOffset), &abyMacHdr[0], 24);
-			}
-			skb->data +=  (cbIVOffset + 4);
-			skb->tail +=  (cbIVOffset + 4);
-			skb_put(skb, FrameSize);
-			skb_reset_mac_header(skb);
-
-			skb->pkt_type = PACKET_OTHERHOST;
-			skb->protocol = htons(ETH_P_802_2);
-			memset(skb->cb, 0, sizeof(skb->cb));
-			netif_rx(skb);
-			return true;
-
-		}
-		// check if 802.1x authorized
-		if (!(pMgmt->sNodeDBTable[iSANodeIndex].dwFlags & WLAN_STA_AUTHORIZED))
-			return false;
+	for (r = RATE_1M; r < MAX_RATE; r++) {
+		if (*rx_rate == rate[r])
+			break;
 	}
 
-	if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP)) {
-		if (bIsWEP)
-			FrameSize -= 8;  //MIC
-	}
-
-	//--------------------------------------------------------------------------------
-	// Soft MIC
-	if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP)) {
-		if (bIsWEP) {
-			__le32 *pdwMIC_L;
-			__le32 *pdwMIC_R;
-			__le32 dwMIC_Priority;
-			__le32 dwMICKey0 = 0, dwMICKey1 = 0;
-			u32 dwLocalMIC_L = 0;
-			u32 dwLocalMIC_R = 0;
-			viawget_wpa_header *wpahdr;
-
-			if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-				dwMICKey0 = cpu_to_le32(*(u32 *)(&pKey->abyKey[24]));
-				dwMICKey1 = cpu_to_le32(*(u32 *)(&pKey->abyKey[28]));
-			} else {
-				if (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
-					dwMICKey0 = cpu_to_le32(*(u32 *)(&pKey->abyKey[16]));
-					dwMICKey1 = cpu_to_le32(*(u32 *)(&pKey->abyKey[20]));
-				} else if ((pKey->dwKeyIndex & BIT28) == 0) {
-					dwMICKey0 = cpu_to_le32(*(u32 *)(&pKey->abyKey[16]));
-					dwMICKey1 = cpu_to_le32(*(u32 *)(&pKey->abyKey[20]));
-				} else {
-					dwMICKey0 = cpu_to_le32(*(u32 *)(&pKey->abyKey[24]));
-					dwMICKey1 = cpu_to_le32(*(u32 *)(&pKey->abyKey[28]));
-				}
-			}
-
-			MIC_vInit(dwMICKey0, dwMICKey1);
-			MIC_vAppend((unsigned char *)&(pDevice->sRxEthHeader.abyDstAddr[0]), 12);
-			dwMIC_Priority = 0;
-			MIC_vAppend((unsigned char *)&dwMIC_Priority, 4);
-			// 4 is Rcv buffer header, 24 is MAC Header, and 8 is IV and Ext IV.
-			MIC_vAppend((unsigned char *)(skb->data + 4 + WLAN_HDR_ADDR3_LEN + 8),
-				    FrameSize - WLAN_HDR_ADDR3_LEN - 8);
-			MIC_vGetMIC(&dwLocalMIC_L, &dwLocalMIC_R);
-			MIC_vUnInit();
-
-			pdwMIC_L = (__le32 *)(skb->data + 4 + FrameSize);
-			pdwMIC_R = (__le32 *)(skb->data + 4 + FrameSize + 4);
-
-			if ((le32_to_cpu(*pdwMIC_L) != dwLocalMIC_L) ||
-			    (le32_to_cpu(*pdwMIC_R) != dwLocalMIC_R) ||
-			    pDevice->bRxMICFail) {
-				pr_debug("MIC comparison is fail!\n");
-				pDevice->bRxMICFail = false;
-				pDevice->s802_11Counter.TKIPLocalMICFailures++;
-				if (bDeFragRx) {
-					if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
-						pr_err("%s: can not alloc more frag bufs\n",
-						       pDevice->dev->name);
-					}
-				}
-				//2008-0409-07, <Add> by Einsn Liu
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-				//send event to wpa_supplicant
-				{
-					union iwreq_data wrqu;
-					struct iw_michaelmicfailure ev;
-					int keyidx = pbyFrame[cbHeaderSize+3] >> 6; //top two-bits
-
-					memset(&ev, 0, sizeof(ev));
-					ev.flags = keyidx & IW_MICFAILURE_KEY_ID;
-					if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
-					    (pMgmt->eCurrState == WMAC_STATE_ASSOC) &&
-					    (*pbyRsr & (RSR_ADDRBROAD | RSR_ADDRMULTI)) == 0) {
-						ev.flags |= IW_MICFAILURE_PAIRWISE;
-					} else {
-						ev.flags |= IW_MICFAILURE_GROUP;
-					}
-
-					ev.src_addr.sa_family = ARPHRD_ETHER;
-					memcpy(ev.src_addr.sa_data, pMACHeader->abyAddr2, ETH_ALEN);
-					memset(&wrqu, 0, sizeof(wrqu));
-					wrqu.data.length = sizeof(ev);
-					wireless_send_event(pDevice->dev, IWEVMICHAELMICFAILURE, &wrqu, (char *)&ev);
-
-				}
-#endif
+	priv->rx_rate = r;
 
-				if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
-					wpahdr = (viawget_wpa_header *)pDevice->skb->data;
-					if ((pDevice->pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
-					    (pDevice->pMgmt->eCurrState == WMAC_STATE_ASSOC) &&
-					    (*pbyRsr & (RSR_ADDRBROAD | RSR_ADDRMULTI)) == 0) {
-						wpahdr->type = VIAWGET_PTK_MIC_MSG;
-					} else {
-						wpahdr->type = VIAWGET_GTK_MIC_MSG;
-					}
-					wpahdr->resp_ie_len = 0;
-					wpahdr->req_ie_len = 0;
-					skb_put(pDevice->skb, sizeof(viawget_wpa_header));
-					pDevice->skb->dev = pDevice->wpadev;
-					skb_reset_mac_header(pDevice->skb);
-					pDevice->skb->pkt_type = PACKET_HOST;
-					pDevice->skb->protocol = htons(ETH_P_802_2);
-					memset(pDevice->skb->cb, 0, sizeof(pDevice->skb->cb));
-					netif_rx(pDevice->skb);
-					pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
-				}
-
-				return false;
-
-			}
-		}
-	} //---end of SOFT MIC-----------------------------------------------------------------------
-
-	// ++++++++++ Reply Counter Check +++++++++++++
-
-	if ((pKey != NULL) && ((pKey->byCipherSuite == KEY_CTL_TKIP) ||
-			       (pKey->byCipherSuite == KEY_CTL_CCMP))) {
-		if (bIsWEP) {
-			unsigned short wLocalTSC15_0 = 0;
-			unsigned long dwLocalTSC47_16 = 0;
-			unsigned long long       RSC = 0;
-			// endian issues
-			RSC = *((unsigned long long *)&(pKey->KeyRSC));
-			wLocalTSC15_0 = (unsigned short)RSC;
-			dwLocalTSC47_16 = (unsigned long)(RSC>>16);
-
-			RSC = dwRxTSC47_16;
-			RSC <<= 16;
-			RSC += wRxTSC15_0;
-			pKey->KeyRSC = RSC;
-
-			if ((pDevice->sMgmtObj.eCurrMode == WMAC_MODE_ESS_STA) &&
-			    (pDevice->sMgmtObj.eCurrState == WMAC_STATE_ASSOC)) {
-				// check RSC
-				if ((wRxTSC15_0 < wLocalTSC15_0) &&
-				    (dwRxTSC47_16 <= dwLocalTSC47_16) &&
-				    !((dwRxTSC47_16 == 0) && (dwLocalTSC47_16 == 0xFFFFFFFF))) {
-					pr_debug("TSC is illegal~~!\n ");
-					if (pKey->byCipherSuite == KEY_CTL_TKIP)
-						pDevice->s802_11Counter.TKIPReplays++;
-					else
-						pDevice->s802_11Counter.CCMPReplays++;
-
-					if (bDeFragRx) {
-						if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
-							pr_err("%s: can not alloc more frag bufs\n",
-							       pDevice->dev->name);
-						}
-					}
-					return false;
-				}
-			}
-		}
-	} // ----- End of Reply Counter Check --------------------------
-
-	s_vProcessRxMACHeader(pDevice, (unsigned char *)(skb->data+4), FrameSize, bIsWEP, bExtIV, &cbHeaderOffset);
-	FrameSize -= cbHeaderOffset;
-	cbHeaderOffset += 4;        // 4 is Rcv buffer header
-
-	// Null data, framesize = 14
-	if (FrameSize < 15)
-		return false;
-
-	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-		if (!s_bAPModeRxData(pDevice,
-				    skb,
-				    FrameSize,
-				    cbHeaderOffset,
-				    iSANodeIndex,
-				    iDANodeIndex
-)) {
-			if (bDeFragRx) {
-				if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
-					pr_err("%s: can not alloc more frag bufs\n",
-					       pDevice->dev->name);
-				}
-			}
-			return false;
+	for (ii = 0; ii < sband->n_bitrates; ii++) {
+		if (sband->bitrates[ii].hw_value == r) {
+			rate_idx = ii;
+				break;
 		}
 	}
 
-	skb->data += cbHeaderOffset;
-	skb->tail += cbHeaderOffset;
-	skb_put(skb, FrameSize);
-	skb->protocol = eth_type_trans(skb, skb->dev);
-
-	//drop frame not met IEEE 802.3
-
-	skb->ip_summed = CHECKSUM_NONE;
-	pStats->rx_bytes += skb->len;
-	pStats->rx_packets++;
-	netif_rx(skb);
-
-	if (bDeFragRx) {
-		if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
-			pr_err("%s: can not alloc more frag bufs\n",
-			       pDevice->dev->name);
-		}
+	if (ii == sband->n_bitrates) {
+		dev_dbg(&priv->pcid->dev, "Wrong RxRate %x\n", *rx_rate);
 		return false;
 	}
 
-	return true;
-}
-
-static bool s_bAPModeRxCtl(
-	struct vnt_private *pDevice,
-	unsigned char *pbyFrame,
-	int      iSANodeIndex
-)
-{
-	PS802_11Header      p802_11Header;
-	CMD_STATUS          Status;
-	PSMgmtObject        pMgmt = pDevice->pMgmt;
-
-	if (IS_CTL_PSPOLL(pbyFrame) || !IS_TYPE_CONTROL(pbyFrame)) {
-		p802_11Header = (PS802_11Header)(pbyFrame);
-		if (!IS_TYPE_MGMT(pbyFrame)) {
-			// Data & PS-Poll packet
-			// check frame class
-			if (iSANodeIndex > 0) {
-				// frame class 3 fliter & checking
-				if (pMgmt->sNodeDBTable[iSANodeIndex].eNodeState < NODE_AUTH) {
-					// send deauth notification
-					// reason = (6) class 2 received from nonauth sta
-					vMgrDeAuthenBeginSta(pDevice,
-							     pMgmt,
-							     (unsigned char *)(p802_11Header->abyAddr2),
-							     (WLAN_MGMT_REASON_CLASS2_NONAUTH),
-							     &Status
-);
-					pr_debug("dpc: send vMgrDeAuthenBeginSta 1\n");
-					return true;
-				}
-				if (pMgmt->sNodeDBTable[iSANodeIndex].eNodeState < NODE_ASSOC) {
-					// send deassoc notification
-					// reason = (7) class 3 received from nonassoc sta
-					vMgrDisassocBeginSta(pDevice,
-							     pMgmt,
-							     (unsigned char *)(p802_11Header->abyAddr2),
-							     (WLAN_MGMT_REASON_CLASS3_NONASSOC),
-							     &Status
-);
-					pr_debug("dpc: send vMgrDisassocBeginSta 2\n");
-					return true;
-				}
-
-				if (pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable) {
-					// delcare received ps-poll event
-					if (IS_CTL_PSPOLL(pbyFrame)) {
-						pMgmt->sNodeDBTable[iSANodeIndex].bRxPSPoll = true;
-						bScheduleCommand((void *)pDevice, WLAN_CMD_RX_PSPOLL, NULL);
-						pr_debug("dpc: WLAN_CMD_RX_PSPOLL 1\n");
-					} else {
-						// check Data PS state
-						// if PW bit off, send out all PS bufferring packets.
-						if (!IS_FC_POWERMGT(pbyFrame)) {
-							pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable = false;
-							pMgmt->sNodeDBTable[iSANodeIndex].bRxPSPoll = true;
-							bScheduleCommand((void *)pDevice, WLAN_CMD_RX_PSPOLL, NULL);
-							pr_debug("dpc: WLAN_CMD_RX_PSPOLL 2\n");
-						}
-					}
-				} else {
-					if (IS_FC_POWERMGT(pbyFrame)) {
-						pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable = true;
-						// Once if STA in PS state, enable multicast bufferring
-						pMgmt->sNodeDBTable[0].bPSEnable = true;
-					} else {
-						// clear all pending PS frame.
-						if (pMgmt->sNodeDBTable[iSANodeIndex].wEnQueueCnt > 0) {
-							pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable = false;
-							pMgmt->sNodeDBTable[iSANodeIndex].bRxPSPoll = true;
-							bScheduleCommand((void *)pDevice, WLAN_CMD_RX_PSPOLL, NULL);
-							pr_debug("dpc: WLAN_CMD_RX_PSPOLL 3\n");
-
-						}
-					}
-				}
-			} else {
-				vMgrDeAuthenBeginSta(pDevice,
-						     pMgmt,
-						     (unsigned char *)(p802_11Header->abyAddr2),
-						     (WLAN_MGMT_REASON_CLASS2_NONAUTH),
-						     &Status
-);
-				pr_debug("dpc: send vMgrDeAuthenBeginSta 3\n");
-				pr_debug("BSSID:%pM\n",
-					 p802_11Header->abyAddr3);
-				pr_debug("ADDR2:%pM\n",
-					 p802_11Header->abyAddr2);
-				pr_debug("ADDR1:%pM\n",
-					 p802_11Header->abyAddr1);
-				pr_debug("dpc: wFrameCtl= %x\n",
-					 p802_11Header->wFrameCtl);
-				VNSvInPortB(pDevice->PortOffset + MAC_REG_RCR, &(pDevice->byRxMode));
-				pr_debug("dpc:pDevice->byRxMode = %x\n",
-					 pDevice->byRxMode);
-				return true;
-			}
-		}
-	}
-	return false;
-}
-
-static bool s_bHandleRxEncryption(
-	struct vnt_private *pDevice,
-	unsigned char *pbyFrame,
-	unsigned int FrameSize,
-	unsigned char *pbyRsr,
-	unsigned char *pbyNewRsr,
-	PSKeyItem   *pKeyOut,
-	bool *pbExtIV,
-	unsigned short *pwRxTSC15_0,
-	unsigned long *pdwRxTSC47_16
-)
-{
-	unsigned int PayloadLen = FrameSize;
-	unsigned char *pbyIV;
-	unsigned char byKeyIdx;
-	PSKeyItem       pKey = NULL;
-	unsigned char byDecMode = KEY_CTL_WEP;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
+	tsf_time = (__le64 *)(skb_data + bytes_received - 12);
+	sq = skb_data + bytes_received - 4;
+	new_rsr = skb_data + bytes_received - 3;
+	rssi = skb_data + bytes_received - 2;
+	rsr = skb_data + bytes_received - 1;
 
-	*pwRxTSC15_0 = 0;
-	*pdwRxTSC47_16 = 0;
+	RFvRSSITodBm(priv, *rssi, &rx_dbm);
 
-	pbyIV = pbyFrame + WLAN_HDR_ADDR3_LEN;
-	if (WLAN_GET_FC_TODS(*(unsigned short *)pbyFrame) &&
-	    WLAN_GET_FC_FROMDS(*(unsigned short *)pbyFrame)) {
-		pbyIV += 6;             // 6 is 802.11 address4
-		PayloadLen -= 6;
-	}
-	byKeyIdx = (*(pbyIV+3) & 0xc0);
-	byKeyIdx >>= 6;
-	pr_debug("\nKeyIdx: %d\n", byKeyIdx);
+	priv->byBBPreEDRSSI = (u8)rx_dbm + 1;
+	priv->uCurrRSSI = *rssi;
 
-	if ((pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
-	    (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||
-	    (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) ||
-	    (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
-	    (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) {
-		if (((*pbyRsr & (RSR_ADDRBROAD | RSR_ADDRMULTI)) == 0) &&
-		    (pDevice->pMgmt->byCSSPK != KEY_CTL_NONE)) {
-			// unicast pkt use pairwise key
-			pr_debug("unicast pkt\n");
-			if (KeybGetKey(&(pDevice->sKey), pDevice->abyBSSID, 0xFFFFFFFF, &pKey) == true) {
-				if (pDevice->pMgmt->byCSSPK == KEY_CTL_TKIP)
-					byDecMode = KEY_CTL_TKIP;
-				else if (pDevice->pMgmt->byCSSPK == KEY_CTL_CCMP)
-					byDecMode = KEY_CTL_CCMP;
-			}
-			pr_debug("unicast pkt: %d, %p\n", byDecMode, pKey);
-		} else {
-			// use group key
-			KeybGetKey(&(pDevice->sKey), pDevice->abyBSSID, byKeyIdx, &pKey);
-			if (pDevice->pMgmt->byCSSGK == KEY_CTL_TKIP)
-				byDecMode = KEY_CTL_TKIP;
-			else if (pDevice->pMgmt->byCSSGK == KEY_CTL_CCMP)
-				byDecMode = KEY_CTL_CCMP;
-			pr_debug("group pkt: %d, %d, %p\n",
-				 byKeyIdx, byDecMode, pKey);
-		}
-	}
-	// our WEP only support Default Key
-	if (pKey == NULL) {
-		// use default group key
-		KeybGetKey(&(pDevice->sKey), pDevice->abyBroadcastAddr, byKeyIdx, &pKey);
-		if (pDevice->pMgmt->byCSSGK == KEY_CTL_TKIP)
-			byDecMode = KEY_CTL_TKIP;
-		else if (pDevice->pMgmt->byCSSGK == KEY_CTL_CCMP)
-			byDecMode = KEY_CTL_CCMP;
-	}
-	*pKeyOut = pKey;
+	skb_pull(skb, 4);
+	skb_trim(skb, frame_size);
 
-	pr_debug("AES:%d %d %d\n",
-		 pDevice->pMgmt->byCSSPK, pDevice->pMgmt->byCSSGK, byDecMode);
+	rx_status.mactime = le64_to_cpu(*tsf_time);
+	rx_status.band = hw->conf.chandef.chan->band;
+	rx_status.signal = rx_dbm;
+	rx_status.flag = 0;
+	rx_status.freq = hw->conf.chandef.chan->center_freq;
 
-	if (pKey == NULL) {
-		pr_debug("pKey == NULL\n");
+	hdr = (struct ieee80211_hdr *)(skb->data);
+	fc = hdr->frame_control;
 
-		return false;
-	}
-	if (byDecMode != pKey->byCipherSuite) {
+	rx_status.rate_idx = rate_idx;
 
-		*pKeyOut = NULL;
-		return false;
+	if (ieee80211_has_protected(fc)) {
+		if (priv->byLocalID > REV_ID_VT3253_A1)
+			rx_status.flag = RX_FLAG_DECRYPTED;
 	}
-	if (byDecMode == KEY_CTL_WEP) {
-		// handle WEP
-		if ((pDevice->byLocalID <= REV_ID_VT3253_A1) ||
-		    (((PSKeyTable)(pKey->pvKeyTable))->bSoftWEP == true)) {
-			// Software WEP
-			// 1. 3253A
-			// 2. WEP 256
-
-			PayloadLen -= (WLAN_HDR_ADDR3_LEN + 4 + 4); // 24 is 802.11 header,4 is IV, 4 is crc
-			memcpy(pDevice->abyPRNG, pbyIV, 3);
-			memcpy(pDevice->abyPRNG + 3, pKey->abyKey, pKey->uKeyLength);
-			rc4_init(&pDevice->SBox, pDevice->abyPRNG, pKey->uKeyLength + 3);
-			rc4_encrypt(&pDevice->SBox, pbyIV+4, pbyIV+4, PayloadLen);
-
-			if (ETHbIsBufferCrc32Ok(pbyIV+4, PayloadLen))
-				*pbyNewRsr |= NEWRSR_DECRYPTOK;
-
-		}
-	} else if ((byDecMode == KEY_CTL_TKIP) ||
-		   (byDecMode == KEY_CTL_CCMP)) {
-		// TKIP/AES
-
-		PayloadLen -= (WLAN_HDR_ADDR3_LEN + 8 + 4); // 24 is 802.11 header, 8 is IV&ExtIV, 4 is crc
-		*pdwRxTSC47_16 = cpu_to_le32(*(unsigned long *)(pbyIV + 4));
-		pr_debug("ExtIV: %lx\n", *pdwRxTSC47_16);
-		if (byDecMode == KEY_CTL_TKIP)
-			*pwRxTSC15_0 = cpu_to_le16(MAKEWORD(*(pbyIV + 2), *pbyIV));
-		else
-			*pwRxTSC15_0 = cpu_to_le16(*(unsigned short *)pbyIV);
-
-		pr_debug("TSC0_15: %x\n", *pwRxTSC15_0);
-
-		if ((byDecMode == KEY_CTL_TKIP) &&
-		    (pDevice->byLocalID <= REV_ID_VT3253_A1)) {
-			// Software TKIP
-			// 1. 3253 A
-			PS802_11Header  pMACHeader = (PS802_11Header)(pbyFrame);
-
-			TKIPvMixKey(pKey->abyKey, pMACHeader->abyAddr2, *pwRxTSC15_0, *pdwRxTSC47_16, pDevice->abyPRNG);
-			rc4_init(&pDevice->SBox, pDevice->abyPRNG, TKIP_KEY_LEN);
-			rc4_encrypt(&pDevice->SBox, pbyIV+8, pbyIV+8, PayloadLen);
-			if (ETHbIsBufferCrc32Ok(pbyIV+8, PayloadLen)) {
-				*pbyNewRsr |= NEWRSR_DECRYPTOK;
-				pr_debug("ICV OK!\n");
-			} else {
-				pr_debug("ICV FAIL!!!\n");
-				pr_debug("PayloadLen = %d\n", PayloadLen);
-			}
-		}
-	}// end of TKIP/AES
-
-	if ((*(pbyIV+3) & 0x20) != 0)
-		*pbExtIV = true;
-	return true;
-}
-
-static bool s_bHostWepRxEncryption(
-	struct vnt_private *pDevice,
-	unsigned char *pbyFrame,
-	unsigned int FrameSize,
-	unsigned char *pbyRsr,
-	bool bOnFly,
-	PSKeyItem    pKey,
-	unsigned char *pbyNewRsr,
-	bool *pbExtIV,
-	unsigned short *pwRxTSC15_0,
-	unsigned long *pdwRxTSC47_16
-)
-{
-	unsigned int PayloadLen = FrameSize;
-	unsigned char *pbyIV;
-	unsigned char byKeyIdx;
-	unsigned char byDecMode = KEY_CTL_WEP;
-	PS802_11Header  pMACHeader;
 
-	*pwRxTSC15_0 = 0;
-	*pdwRxTSC47_16 = 0;
-
-	pbyIV = pbyFrame + WLAN_HDR_ADDR3_LEN;
-	if (WLAN_GET_FC_TODS(*(unsigned short *)pbyFrame) &&
-	    WLAN_GET_FC_FROMDS(*(unsigned short *)pbyFrame)) {
-		pbyIV += 6;             // 6 is 802.11 address4
-		PayloadLen -= 6;
+	if (priv->vif && priv->bDiversityEnable) {
+		if (ieee80211_is_data(fc) &&
+		    (frame_size > 50) && priv->vif->bss_conf.assoc)
+			BBvAntennaDiversity(priv, priv->rx_rate, 0);
 	}
-	byKeyIdx = (*(pbyIV+3) & 0xc0);
-	byKeyIdx >>= 6;
-	pr_debug("\nKeyIdx: %d\n", byKeyIdx);
-
-	if (pDevice->pMgmt->byCSSGK == KEY_CTL_TKIP)
-		byDecMode = KEY_CTL_TKIP;
-	else if (pDevice->pMgmt->byCSSGK == KEY_CTL_CCMP)
-		byDecMode = KEY_CTL_CCMP;
-
-	pr_debug("AES:%d %d %d\n",
-		 pDevice->pMgmt->byCSSPK, pDevice->pMgmt->byCSSGK, byDecMode);
-
-	if (byDecMode != pKey->byCipherSuite)
-		return false;
-
-	if (byDecMode == KEY_CTL_WEP) {
-		// handle WEP
-		pr_debug("byDecMode == KEY_CTL_WEP\n");
-
-		if ((pDevice->byLocalID <= REV_ID_VT3253_A1) ||
-		    (((PSKeyTable)(pKey->pvKeyTable))->bSoftWEP == true) ||
-		    !bOnFly) {
-			// Software WEP
-			// 1. 3253A
-			// 2. WEP 256
-			// 3. NotOnFly
 
-			PayloadLen -= (WLAN_HDR_ADDR3_LEN + 4 + 4); // 24 is 802.11 header,4 is IV, 4 is crc
-			memcpy(pDevice->abyPRNG, pbyIV, 3);
-			memcpy(pDevice->abyPRNG + 3, pKey->abyKey, pKey->uKeyLength);
-			rc4_init(&pDevice->SBox, pDevice->abyPRNG, pKey->uKeyLength + 3);
-			rc4_encrypt(&pDevice->SBox, pbyIV+4, pbyIV+4, PayloadLen);
+	memcpy(IEEE80211_SKB_RXCB(skb), &rx_status, sizeof(rx_status));
 
-			if (ETHbIsBufferCrc32Ok(pbyIV+4, PayloadLen))
-				*pbyNewRsr |= NEWRSR_DECRYPTOK;
+	ieee80211_rx_irqsafe(priv->hw, skb);
 
-		}
-	} else if ((byDecMode == KEY_CTL_TKIP) ||
-		   (byDecMode == KEY_CTL_CCMP)) {
-		// TKIP/AES
-
-		PayloadLen -= (WLAN_HDR_ADDR3_LEN + 8 + 4); // 24 is 802.11 header, 8 is IV&ExtIV, 4 is crc
-		*pdwRxTSC47_16 = cpu_to_le32(*(unsigned long *)(pbyIV + 4));
-		pr_debug("ExtIV: %lx\n", *pdwRxTSC47_16);
-
-		if (byDecMode == KEY_CTL_TKIP)
-			*pwRxTSC15_0 = cpu_to_le16(MAKEWORD(*(pbyIV+2), *pbyIV));
-		else
-			*pwRxTSC15_0 = cpu_to_le16(*(unsigned short *)pbyIV);
-
-		pr_debug("TSC0_15: %x\n", *pwRxTSC15_0);
-
-		if (byDecMode == KEY_CTL_TKIP) {
-			if ((pDevice->byLocalID <= REV_ID_VT3253_A1) || !bOnFly) {
-				// Software TKIP
-				// 1. 3253 A
-				// 2. NotOnFly
-				pr_debug("soft KEY_CTL_TKIP\n");
-				pMACHeader = (PS802_11Header)(pbyFrame);
-				TKIPvMixKey(pKey->abyKey, pMACHeader->abyAddr2, *pwRxTSC15_0, *pdwRxTSC47_16, pDevice->abyPRNG);
-				rc4_init(&pDevice->SBox, pDevice->abyPRNG, TKIP_KEY_LEN);
-				rc4_encrypt(&pDevice->SBox, pbyIV+8, pbyIV+8, PayloadLen);
-				if (ETHbIsBufferCrc32Ok(pbyIV+8, PayloadLen)) {
-					*pbyNewRsr |= NEWRSR_DECRYPTOK;
-					pr_debug("ICV OK!\n");
-				} else {
-					pr_debug("ICV FAIL!!!\n");
-					pr_debug("PayloadLen = %d\n",
-						 PayloadLen);
-				}
-			}
-		}
-
-		if (byDecMode == KEY_CTL_CCMP) {
-			if (!bOnFly) {
-				// Software CCMP
-				// NotOnFly
-				pr_debug("soft KEY_CTL_CCMP\n");
-				if (AESbGenCCMP(pKey->abyKey, pbyFrame, FrameSize)) {
-					*pbyNewRsr |= NEWRSR_DECRYPTOK;
-					pr_debug("CCMP MIC compare OK!\n");
-				} else {
-					pr_debug("CCMP MIC fail!\n");
-				}
-			}
-		}
-
-	}// end of TKIP/AES
-
-	if ((*(pbyIV+3) & 0x20) != 0)
-		*pbExtIV = true;
 	return true;
 }
 
-static bool s_bAPModeRxData(
-	struct vnt_private *pDevice,
-	struct sk_buff *skb,
-	unsigned int FrameSize,
-	unsigned int cbHeaderOffset,
-	int      iSANodeIndex,
-	int      iDANodeIndex
-)
+bool vnt_receive_frame(struct vnt_private *priv, PSRxDesc curr_rd)
 {
-	PSMgmtObject        pMgmt = pDevice->pMgmt;
-	bool bRelayAndForward = false;
-	bool bRelayOnly = false;
-	unsigned char byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
-	unsigned short wAID;
-
-	struct sk_buff *skbcpy = NULL;
+	PDEVICE_RD_INFO rd_info = curr_rd->pRDInfo;
+	struct sk_buff *skb;
+	u16 frame_size;
 
-	if (FrameSize > CB_MAX_BUF_SIZE)
-		return false;
-	// check DA
-	if (is_multicast_ether_addr((unsigned char *)(skb->data+cbHeaderOffset))) {
-		if (pMgmt->sNodeDBTable[0].bPSEnable) {
-			skbcpy = dev_alloc_skb((int)pDevice->rx_buf_sz);
+	skb = rd_info->skb;
 
-			// if any node in PS mode, buffer packet until DTIM.
-			if (skbcpy == NULL) {
-				pr_info("relay multicast no skb available\n");
-			} else {
-				skbcpy->dev = pDevice->dev;
-				skbcpy->len = FrameSize;
-				memcpy(skbcpy->data, skb->data+cbHeaderOffset, FrameSize);
-				skb_queue_tail(&(pMgmt->sNodeDBTable[0].sTxPSQueue), skbcpy);
+	pci_unmap_single(priv->pcid, rd_info->skb_dma,
+			 priv->rx_buf_sz, PCI_DMA_FROMDEVICE);
 
-				pMgmt->sNodeDBTable[0].wEnQueueCnt++;
-				// set tx map
-				pMgmt->abyPSTxMap[0] |= byMask[0];
-			}
-		} else {
-			bRelayAndForward = true;
-		}
-	} else {
-		// check if relay
-		if (BSSDBbIsSTAInNodeDB(pMgmt, (unsigned char *)(skb->data+cbHeaderOffset), &iDANodeIndex)) {
-			if (pMgmt->sNodeDBTable[iDANodeIndex].eNodeState >= NODE_ASSOC) {
-				if (pMgmt->sNodeDBTable[iDANodeIndex].bPSEnable) {
-					// queue this skb until next PS tx, and then release.
+	frame_size = le16_to_cpu(curr_rd->m_rd1RD1.wReqCount)
+			- cpu_to_le16(curr_rd->m_rd0RD0.wResCount);
 
-					skb->data += cbHeaderOffset;
-					skb->tail += cbHeaderOffset;
-					skb_put(skb, FrameSize);
-					skb_queue_tail(&pMgmt->sNodeDBTable[iDANodeIndex].sTxPSQueue, skb);
-					pMgmt->sNodeDBTable[iDANodeIndex].wEnQueueCnt++;
-					wAID = pMgmt->sNodeDBTable[iDANodeIndex].wAID;
-					pMgmt->abyPSTxMap[wAID >> 3] |=  byMask[wAID & 7];
-					pr_debug("relay: index= %d, pMgmt->abyPSTxMap[%d]= %d\n",
-						 iDANodeIndex, (wAID >> 3),
-						 pMgmt->abyPSTxMap[wAID >> 3]);
-					return true;
-				} else {
-					bRelayOnly = true;
-				}
-			}
-		}
+	if ((frame_size > 2364) || (frame_size < 33)) {
+		/* Frame Size error drop this packet.*/
+		dev_dbg(&priv->pcid->dev, "Wrong frame size %d\n", frame_size);
+		dev_kfree_skb_irq(skb);
+		return true;
 	}
 
-	if (bRelayOnly || bRelayAndForward) {
-		// relay this packet right now
-		if (bRelayAndForward)
-			iDANodeIndex = 0;
+	if (vnt_rx_data(priv, skb, frame_size))
+		return true;
 
-		if ((pDevice->uAssocCount > 1) && (iDANodeIndex >= 0))
-			ROUTEbRelay(pDevice, (unsigned char *)(skb->data + cbHeaderOffset), FrameSize, (unsigned int)iDANodeIndex);
-
-		if (bRelayOnly)
-			return false;
-	}
-	// none associate, don't forward
-	if (pDevice->uAssocCount == 0)
-		return false;
+	dev_kfree_skb_irq(skb);
 
 	return true;
 }
diff --git a/drivers/staging/vt6655/dpc.h b/drivers/staging/vt6655/dpc.h
index a068b846b1be..ad495719a251 100644
--- a/drivers/staging/vt6655/dpc.h
+++ b/drivers/staging/vt6655/dpc.h
@@ -29,14 +29,8 @@
 #ifndef __DPC_H__
 #define __DPC_H__
 
-#include "ttype.h"
 #include "device.h"
-#include "wcmd.h"
 
-bool
-device_receive_frame(
-	struct vnt_private *,
-	PSRxDesc pCurrRD
-);
+bool vnt_receive_frame(struct vnt_private *priv, PSRxDesc curr_rd);
 
-#endif // __RXTX_H__
+#endif /* __RXTX_H__ */
diff --git a/drivers/staging/vt6655/hostap.c b/drivers/staging/vt6655/hostap.c
deleted file mode 100644
index ae0dade229d8..000000000000
--- a/drivers/staging/vt6655/hostap.c
+++ /dev/null
@@ -1,765 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: hostap.c
- *
- * Purpose: handle hostap deamon ioctl input/out functions
- *
- * Author: Lyndon Chen
- *
- * Date: Oct. 20, 2003
- *
- * Functions:
- *
- * Revision History:
- *
- */
-
-#include "hostap.h"
-#include "iocmd.h"
-#include "mac.h"
-#include "card.h"
-#include "baseband.h"
-#include "wpactl.h"
-#include "key.h"
-
-#define VIAWGET_HOSTAPD_MAX_BUF_SIZE 1024
-#define HOSTAP_CRYPT_FLAG_SET_TX_KEY BIT0
-#define HOSTAP_CRYPT_ERR_UNKNOWN_ADDR 3
-#define HOSTAP_CRYPT_ERR_KEY_SET_FAILED 5
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*
- * Description:
- *      register net_device (AP) for hostap deamon
- *
- * Parameters:
- *  In:
- *      pDevice             -
- *      rtnl_locked         -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static int hostap_enable_hostapd(struct vnt_private *pDevice, int rtnl_locked)
-{
-	struct vnt_private *apdev_priv;
-	struct net_device *dev = pDevice->dev;
-	int ret;
-	const struct net_device_ops apdev_netdev_ops = {
-		.ndo_start_xmit         = pDevice->tx_80211,
-	};
-
-	pr_debug("%s: Enabling hostapd mode\n", dev->name);
-
-	pDevice->apdev = alloc_etherdev(sizeof(*apdev_priv));
-	if (pDevice->apdev == NULL)
-		return -ENOMEM;
-
-	apdev_priv = netdev_priv(pDevice->apdev);
-	*apdev_priv = *pDevice;
-	eth_hw_addr_inherit(pDevice->apdev, dev);
-
-	pDevice->apdev->netdev_ops = &apdev_netdev_ops;
-
-	pDevice->apdev->type = ARPHRD_IEEE80211;
-
-	pDevice->apdev->base_addr = dev->base_addr;
-	pDevice->apdev->irq = dev->irq;
-	pDevice->apdev->mem_start = dev->mem_start;
-	pDevice->apdev->mem_end = dev->mem_end;
-	sprintf(pDevice->apdev->name, "%sap", dev->name);
-	if (rtnl_locked)
-		ret = register_netdevice(pDevice->apdev);
-	else
-		ret = register_netdev(pDevice->apdev);
-	if (ret) {
-		pr_debug("%s: register_netdevice(AP) failed!\n",
-			 dev->name);
-		free_netdev(pDevice->apdev);
-		pDevice->apdev = NULL;
-		return -1;
-	}
-
-	pr_debug("%s: Registered netdevice %s for AP management\n",
-		 dev->name, pDevice->apdev->name);
-
-	KeyvInitTable(&pDevice->sKey, pDevice->PortOffset);
-
-	return 0;
-}
-
-/*
- * Description:
- *      unregister net_device(AP)
- *
- * Parameters:
- *  In:
- *      pDevice             -
- *      rtnl_locked         -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static int hostap_disable_hostapd(struct vnt_private *pDevice, int rtnl_locked)
-{
-	pr_debug("%s: disabling hostapd mode\n", pDevice->dev->name);
-
-	if (pDevice->apdev && pDevice->apdev->name && pDevice->apdev->name[0]) {
-		if (rtnl_locked)
-			unregister_netdevice(pDevice->apdev);
-		else
-			unregister_netdev(pDevice->apdev);
-		pr_debug("%s: Netdevice %s unregistered\n",
-			 pDevice->dev->name, pDevice->apdev->name);
-	}
-	if (pDevice->apdev)
-		free_netdev(pDevice->apdev);
-	pDevice->apdev = NULL;
-	pDevice->bEnable8021x = false;
-	pDevice->bEnableHostWEP = false;
-	pDevice->bEncryptionEnable = false;
-
-/* 4.2007-0118-03,<Add> by EinsnLiu */
-/* execute some clear work */
-	pDevice->pMgmt->byCSSPK = KEY_CTL_NONE;
-	pDevice->pMgmt->byCSSGK = KEY_CTL_NONE;
-	KeyvInitTable(&pDevice->sKey, pDevice->PortOffset);
-
-	return 0;
-}
-
-/*
- * Description:
- *      Set enable/disable hostapd mode
- *
- * Parameters:
- *  In:
- *      pDevice             -
- *      rtnl_locked         -
- *  Out:
- *
- * Return Value:
- *
- */
-
-int vt6655_hostap_set_hostapd(struct vnt_private *pDevice,
-			      int val, int rtnl_locked)
-{
-	if (val < 0 || val > 1)
-		return -EINVAL;
-
-	if (pDevice->bEnableHostapd == val)
-		return 0;
-
-	pDevice->bEnableHostapd = val;
-
-	if (val)
-		return hostap_enable_hostapd(pDevice, rtnl_locked);
-	else
-		return hostap_disable_hostapd(pDevice, rtnl_locked);
-}
-
-/*
- * Description:
- *      remove station function supported for hostap deamon
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-static int hostap_remove_sta(struct vnt_private *pDevice,
-			     struct viawget_hostapd_param *param)
-{
-	unsigned int uNodeIndex;
-
-	if (BSSDBbIsSTAInNodeDB(pDevice->pMgmt, param->sta_addr, &uNodeIndex))
-		BSSvRemoveOneNode(pDevice, uNodeIndex);
-	else
-		return -ENOENT;
-
-	return 0;
-}
-
-/*
- * Description:
- *      add a station from hostap deamon
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-static int hostap_add_sta(struct vnt_private *pDevice,
-			  struct viawget_hostapd_param *param)
-{
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	unsigned int uNodeIndex;
-
-	if (!BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &uNodeIndex))
-		BSSvCreateOneNode(pDevice, &uNodeIndex);
-
-	memcpy(pMgmt->sNodeDBTable[uNodeIndex].abyMACAddr, param->sta_addr, WLAN_ADDR_LEN);
-	pMgmt->sNodeDBTable[uNodeIndex].eNodeState = NODE_ASSOC;
-	pMgmt->sNodeDBTable[uNodeIndex].wCapInfo = param->u.add_sta.capability;
-/* TODO listenInterval */
-	pMgmt->sNodeDBTable[uNodeIndex].bPSEnable = false;
-	pMgmt->sNodeDBTable[uNodeIndex].bySuppRate = param->u.add_sta.tx_supp_rates;
-
-	/* set max tx rate */
-	pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate =
-		pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate;
-	/* set max basic rate */
-	pMgmt->sNodeDBTable[uNodeIndex].wMaxBasicRate = RATE_2M;
-	/* Todo: check sta preamble, if ap can't support, set status code */
-	pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble =
-		WLAN_GET_CAP_INFO_SHORTPREAMBLE(pMgmt->sNodeDBTable[uNodeIndex].wCapInfo);
-
-	pMgmt->sNodeDBTable[uNodeIndex].wAID = (unsigned short)param->u.add_sta.aid;
-
-	pMgmt->sNodeDBTable[uNodeIndex].ulLastRxJiffer = jiffies;
-
-	pr_debug("Add STA AID= %d\n", pMgmt->sNodeDBTable[uNodeIndex].wAID);
-	pr_debug("MAC=%pM\n", param->sta_addr);
-	pr_debug("Max Support rate = %d\n",
-		 pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate);
-
-	return 0;
-}
-
-/*
- * Description:
- *      get station info
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static int hostap_get_info_sta(struct vnt_private *pDevice,
-			       struct viawget_hostapd_param *param)
-{
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	unsigned int uNodeIndex;
-
-	if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &uNodeIndex)) {
-		param->u.get_info_sta.inactive_sec =
-			(jiffies - pMgmt->sNodeDBTable[uNodeIndex].ulLastRxJiffer) / HZ;
-	} else {
-		return -ENOENT;
-	}
-
-	return 0;
-}
-
-/*
- * Description:
- *      set station flag
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-static int hostap_set_flags_sta(struct vnt_private *pDevice,
-				struct viawget_hostapd_param *param)
-{
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	unsigned int uNodeIndex;
-
-	if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &uNodeIndex)) {
-		pMgmt->sNodeDBTable[uNodeIndex].dwFlags |= param->u.set_flags_sta.flags_or;
-		pMgmt->sNodeDBTable[uNodeIndex].dwFlags &= param->u.set_flags_sta.flags_and;
-		pr_debug(" dwFlags = %x\n",
-			 (unsigned int)pMgmt->sNodeDBTable[uNodeIndex].dwFlags);
-	} else {
-		return -ENOENT;
-	}
-
-	return 0;
-}
-
-/*
- * Description:
- *      set generic element (wpa ie)
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-static int hostap_set_generic_element(struct vnt_private *pDevice,
-				      struct viawget_hostapd_param *param)
-{
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-
-	if (param->u.generic_elem.len > sizeof(pMgmt->abyWPAIE))
-		return -EINVAL;
-
-	memcpy(pMgmt->abyWPAIE,
-	       param->u.generic_elem.data,
-	       param->u.generic_elem.len
-		);
-
-	pMgmt->wWPAIELen = param->u.generic_elem.len;
-
-	pr_debug("pMgmt->wWPAIELen = %d\n", pMgmt->wWPAIELen);
-
-	/* disable wpa */
-	if (pMgmt->wWPAIELen == 0) {
-		pMgmt->eAuthenMode = WMAC_AUTH_OPEN;
-		pr_debug(" No WPAIE, Disable WPA\n");
-	} else  {
-		/* enable wpa */
-		if ((pMgmt->abyWPAIE[0] == WLAN_EID_RSN_WPA) ||
-		    (pMgmt->abyWPAIE[0] == WLAN_EID_RSN)) {
-			pMgmt->eAuthenMode = WMAC_AUTH_WPANONE;
-			pr_debug("Set WPAIE enable WPA\n");
-		} else
-			return -EINVAL;
-	}
-
-	return 0;
-}
-
-/*
- * Description:
- *      flush station nodes table.
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static void hostap_flush_sta(struct vnt_private *pDevice)
-{
-	/* reserved node index =0 for multicast node. */
-	BSSvClearNodeDBTable(pDevice, 1);
-	pDevice->uAssocCount = 0;
-}
-
-/*
- * Description:
- *      set each stations encryption key
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-static int hostap_set_encryption(struct vnt_private *pDevice,
-				 struct viawget_hostapd_param *param,
-				 int param_len)
-{
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	unsigned long dwKeyIndex = 0;
-	unsigned char abyKey[MAX_KEY_LEN];
-	unsigned char abySeq[MAX_KEY_LEN];
-	u64 KeyRSC;
-	unsigned char byKeyDecMode = KEY_CTL_WEP;
-	int     iNodeIndex = -1;
-	int     ii;
-	bool bKeyTableFull = false;
-	unsigned short wKeyCtl = 0;
-
-	param->u.crypt.err = 0;
-
-	if (param->u.crypt.alg > WPA_ALG_CCMP)
-		return -EINVAL;
-
-	if ((param->u.crypt.idx > 3) || (param->u.crypt.key_len > MAX_KEY_LEN)) {
-		param->u.crypt.err = HOSTAP_CRYPT_ERR_KEY_SET_FAILED;
-		pr_debug(" HOSTAP_CRYPT_ERR_KEY_SET_FAILED\n");
-		return -EINVAL;
-	}
-
-	if (is_broadcast_ether_addr(param->sta_addr)) {
-		if (param->u.crypt.idx >= MAX_GROUP_KEY)
-			return -EINVAL;
-		iNodeIndex = 0;
-
-	} else {
-		if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &iNodeIndex) == false) {
-			param->u.crypt.err = HOSTAP_CRYPT_ERR_UNKNOWN_ADDR;
-			pr_debug(" HOSTAP_CRYPT_ERR_UNKNOWN_ADDR\n");
-			return -EINVAL;
-		}
-	}
-	pr_debug(" hostap_set_encryption: sta_index %d\n", iNodeIndex);
-	pr_debug(" hostap_set_encryption: alg %d\n", param->u.crypt.alg);
-
-	if (param->u.crypt.alg == WPA_ALG_NONE) {
-		if (pMgmt->sNodeDBTable[iNodeIndex].bOnFly) {
-			if (!KeybRemoveKey(&(pDevice->sKey),
-					  param->sta_addr,
-					  pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex,
-					  pDevice->PortOffset)) {
-				pr_debug("KeybRemoveKey fail\n");
-			}
-			pMgmt->sNodeDBTable[iNodeIndex].bOnFly = false;
-		}
-		pMgmt->sNodeDBTable[iNodeIndex].byKeyIndex = 0;
-		pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex = 0;
-		pMgmt->sNodeDBTable[iNodeIndex].uWepKeyLength = 0;
-		pMgmt->sNodeDBTable[iNodeIndex].KeyRSC = 0;
-		pMgmt->sNodeDBTable[iNodeIndex].dwTSC47_16 = 0;
-		pMgmt->sNodeDBTable[iNodeIndex].wTSC15_0 = 0;
-		pMgmt->sNodeDBTable[iNodeIndex].byCipherSuite = 0;
-		memset(&pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[0],
-		       0,
-		       MAX_KEY_LEN
-);
-
-		return 0;
-	}
-
-	memcpy(abyKey, param->u.crypt.key, param->u.crypt.key_len);
-	/* copy to node key tbl */
-	pMgmt->sNodeDBTable[iNodeIndex].byKeyIndex = param->u.crypt.idx;
-	pMgmt->sNodeDBTable[iNodeIndex].uWepKeyLength = param->u.crypt.key_len;
-	memcpy(&pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[0],
-	       param->u.crypt.key,
-	       param->u.crypt.key_len
-);
-
-	dwKeyIndex = (unsigned long)(param->u.crypt.idx);
-	if (param->u.crypt.flags & HOSTAP_CRYPT_FLAG_SET_TX_KEY) {
-		pDevice->byKeyIndex = (unsigned char)dwKeyIndex;
-		pDevice->bTransmitKey = true;
-		dwKeyIndex |= (1 << 31);
-	}
-
-	if (param->u.crypt.alg == WPA_ALG_WEP) {
-		if ((pDevice->bEnable8021x == false) || (iNodeIndex == 0)) {
-			KeybSetDefaultKey(&(pDevice->sKey),
-					  dwKeyIndex & ~(BIT30 | USE_KEYRSC),
-					  param->u.crypt.key_len,
-					  NULL,
-					  abyKey,
-					  KEY_CTL_WEP,
-					  pDevice->PortOffset,
-					  pDevice->byLocalID);
-
-		} else {
-			/* 8021x enable, individual key */
-			dwKeyIndex |= (1 << 30); /* set pairwise key */
-			if (KeybSetKey(&(pDevice->sKey),
-				       &param->sta_addr[0],
-				       dwKeyIndex & ~(USE_KEYRSC),
-				       param->u.crypt.key_len,
-				       (u64 *) &KeyRSC,
-				       (unsigned char *)abyKey,
-				       KEY_CTL_WEP,
-				       pDevice->PortOffset,
-				       pDevice->byLocalID)) {
-				pMgmt->sNodeDBTable[iNodeIndex].bOnFly = true;
-
-			} else {
-				/* Key Table Full */
-				pMgmt->sNodeDBTable[iNodeIndex].bOnFly = false;
-				bKeyTableFull = true;
-			}
-		}
-		pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-		pDevice->bEncryptionEnable = true;
-		pMgmt->byCSSPK = KEY_CTL_WEP;
-		pMgmt->byCSSGK = KEY_CTL_WEP;
-		pMgmt->sNodeDBTable[iNodeIndex].byCipherSuite = KEY_CTL_WEP;
-		pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex = dwKeyIndex;
-		return 0;
-	}
-
-	if (param->u.crypt.seq) {
-		memcpy(&abySeq, param->u.crypt.seq, 8);
-		for (ii = 0; ii < 8; ii++)
-			KeyRSC |= (u64)abySeq[ii] << (ii * 8);
-
-		dwKeyIndex |= 1 << 29;
-		pMgmt->sNodeDBTable[iNodeIndex].KeyRSC = KeyRSC;
-	}
-
-	if (param->u.crypt.alg == WPA_ALG_TKIP) {
-		if (param->u.crypt.key_len != MAX_KEY_LEN)
-			return -EINVAL;
-		pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
-		byKeyDecMode = KEY_CTL_TKIP;
-		pMgmt->byCSSPK = KEY_CTL_TKIP;
-		pMgmt->byCSSGK = KEY_CTL_TKIP;
-	}
-
-	if (param->u.crypt.alg == WPA_ALG_CCMP) {
-		if ((param->u.crypt.key_len != AES_KEY_LEN) ||
-		    (pDevice->byLocalID <= REV_ID_VT3253_A1))
-			return -EINVAL;
-		pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
-		byKeyDecMode = KEY_CTL_CCMP;
-		pMgmt->byCSSPK = KEY_CTL_CCMP;
-		pMgmt->byCSSGK = KEY_CTL_CCMP;
-	}
-
-	if (iNodeIndex == 0) {
-		KeybSetDefaultKey(&(pDevice->sKey),
-				  dwKeyIndex,
-				  param->u.crypt.key_len,
-				  (u64 *) &KeyRSC,
-				  abyKey,
-				  byKeyDecMode,
-				  pDevice->PortOffset,
-				  pDevice->byLocalID);
-		pMgmt->sNodeDBTable[iNodeIndex].bOnFly = true;
-
-	} else {
-		dwKeyIndex |= (1 << 30); /* set pairwise key */
-		if (KeybSetKey(&(pDevice->sKey),
-			       &param->sta_addr[0],
-			       dwKeyIndex,
-			       param->u.crypt.key_len,
-			       (u64 *) &KeyRSC,
-			       (unsigned char *)abyKey,
-			       byKeyDecMode,
-			       pDevice->PortOffset,
-			       pDevice->byLocalID)) {
-			pMgmt->sNodeDBTable[iNodeIndex].bOnFly = true;
-
-		} else {
-			/* Key Table Full */
-			pMgmt->sNodeDBTable[iNodeIndex].bOnFly = false;
-			bKeyTableFull = true;
-			pr_debug(" Key Table Full\n");
-		}
-
-	}
-
-	if (bKeyTableFull) {
-		wKeyCtl &= 0x7F00;              /* clear all key control filed */
-		wKeyCtl |= (byKeyDecMode << 4);
-		wKeyCtl |= (byKeyDecMode);
-		wKeyCtl |= 0x0044;              /* use group key for all address */
-		wKeyCtl |= 0x4000;              /* disable KeyTable[MAX_KEY_TABLE-1] on-fly to genernate rx int */
-		MACvSetDefaultKeyCtl(pDevice->PortOffset, wKeyCtl, MAX_KEY_TABLE-1, pDevice->byLocalID);
-	}
-
-	pr_debug(" Set key sta_index= %d\n", iNodeIndex);
-	pr_debug(" tx_index=%d len=%d\n",
-		 param->u.crypt.idx, param->u.crypt.key_len);
-	pr_debug(" key=%x-%x-%x-%x-%x-xxxxx\n",
-		 pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[0],
-		 pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[1],
-		 pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[2],
-		 pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[3],
-		 pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[4]);
-
-	/* set wep key */
-	pDevice->bEncryptionEnable = true;
-	pMgmt->sNodeDBTable[iNodeIndex].byCipherSuite = byKeyDecMode;
-	pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex = dwKeyIndex;
-	pMgmt->sNodeDBTable[iNodeIndex].dwTSC47_16 = 0;
-	pMgmt->sNodeDBTable[iNodeIndex].wTSC15_0 = 0;
-
-	return 0;
-}
-
-/*
- * Description:
- *      get each stations encryption key
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-static int hostap_get_encryption(struct vnt_private *pDevice,
-				 struct viawget_hostapd_param *param,
-				 int param_len)
-{
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	int     ii;
-	int     iNodeIndex = 0;
-
-	param->u.crypt.err = 0;
-
-	if (is_broadcast_ether_addr(param->sta_addr)) {
-		iNodeIndex = 0;
-	} else {
-		if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &iNodeIndex) == false) {
-			param->u.crypt.err = HOSTAP_CRYPT_ERR_UNKNOWN_ADDR;
-			pr_debug("hostap_get_encryption: HOSTAP_CRYPT_ERR_UNKNOWN_ADDR\n");
-			return -EINVAL;
-		}
-	}
-	pr_debug("hostap_get_encryption: %d\n", iNodeIndex);
-	memset(param->u.crypt.seq, 0, 8);
-	for (ii = 0; ii < 8; ii++)
-		param->u.crypt.seq[ii] = (unsigned char)pMgmt->sNodeDBTable[iNodeIndex].KeyRSC >> (ii * 8);
-
-	return 0;
-}
-
-/*
- * Description:
- *      vt6655_hostap_ioctl main function supported for hostap deamon.
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      iw_point  -
- *  Out:
- *
- * Return Value:
- *
- */
-int vt6655_hostap_ioctl(struct vnt_private *pDevice, struct iw_point *p)
-{
-	struct viawget_hostapd_param *param;
-	int ret = 0;
-	int ap_ioctl = 0;
-
-	if (p->length < sizeof(struct viawget_hostapd_param) ||
-	    p->length > VIAWGET_HOSTAPD_MAX_BUF_SIZE || !p->pointer)
-		return -EINVAL;
-
-	param = kmalloc((int)p->length, GFP_KERNEL);
-	if (param == NULL)
-		return -ENOMEM;
-
-	if (copy_from_user(param, p->pointer, p->length)) {
-		ret = -EFAULT;
-		goto out;
-	}
-
-	switch (param->cmd) {
-	case VIAWGET_HOSTAPD_SET_ENCRYPTION:
-		pr_debug("VIAWGET_HOSTAPD_SET_ENCRYPTION\n");
-		spin_lock_irq(&pDevice->lock);
-		ret = hostap_set_encryption(pDevice, param, p->length);
-		spin_unlock_irq(&pDevice->lock);
-		break;
-	case VIAWGET_HOSTAPD_GET_ENCRYPTION:
-		pr_debug("VIAWGET_HOSTAPD_GET_ENCRYPTION\n");
-		spin_lock_irq(&pDevice->lock);
-		ret = hostap_get_encryption(pDevice, param, p->length);
-		spin_unlock_irq(&pDevice->lock);
-		break;
-	case VIAWGET_HOSTAPD_SET_ASSOC_AP_ADDR:
-		pr_debug("VIAWGET_HOSTAPD_SET_ASSOC_AP_ADDR\n");
-		ret = -EOPNOTSUPP;
-		goto out;
-	case VIAWGET_HOSTAPD_FLUSH:
-		pr_debug("VIAWGET_HOSTAPD_FLUSH\n");
-		spin_lock_irq(&pDevice->lock);
-		hostap_flush_sta(pDevice);
-		spin_unlock_irq(&pDevice->lock);
-		break;
-	case VIAWGET_HOSTAPD_ADD_STA:
-		pr_debug("VIAWGET_HOSTAPD_ADD_STA\n");
-		spin_lock_irq(&pDevice->lock);
-		ret = hostap_add_sta(pDevice, param);
-		spin_unlock_irq(&pDevice->lock);
-		break;
-	case VIAWGET_HOSTAPD_REMOVE_STA:
-		pr_debug("VIAWGET_HOSTAPD_REMOVE_STA\n");
-		spin_lock_irq(&pDevice->lock);
-		ret = hostap_remove_sta(pDevice, param);
-		spin_unlock_irq(&pDevice->lock);
-		break;
-	case VIAWGET_HOSTAPD_GET_INFO_STA:
-		pr_debug("VIAWGET_HOSTAPD_GET_INFO_STA\n");
-		ret = hostap_get_info_sta(pDevice, param);
-		ap_ioctl = 1;
-		break;
-	case VIAWGET_HOSTAPD_SET_FLAGS_STA:
-		pr_debug("VIAWGET_HOSTAPD_SET_FLAGS_STA\n");
-		ret = hostap_set_flags_sta(pDevice, param);
-		break;
-	case VIAWGET_HOSTAPD_MLME:
-		pr_debug("VIAWGET_HOSTAPD_MLME\n");
-		ret = -EOPNOTSUPP;
-		goto out;
-	case VIAWGET_HOSTAPD_SET_GENERIC_ELEMENT:
-		pr_debug("VIAWGET_HOSTAPD_SET_GENERIC_ELEMENT\n");
-		ret = hostap_set_generic_element(pDevice, param);
-		break;
-	case VIAWGET_HOSTAPD_SCAN_REQ:
-		pr_debug("VIAWGET_HOSTAPD_SCAN_REQ\n");
-		ret = -EOPNOTSUPP;
-		goto out;
-	case VIAWGET_HOSTAPD_STA_CLEAR_STATS:
-		pr_debug("VIAWGET_HOSTAPD_STA_CLEAR_STATS\n");
-		ret = -EOPNOTSUPP;
-		goto out;
-	default:
-		pr_debug("vt6655_hostap_ioctl: unknown cmd=%d\n",
-			 (int)param->cmd);
-		ret = -EOPNOTSUPP;
-		goto out;
-	}
-
-	if ((ret == 0) && ap_ioctl) {
-		if (copy_to_user(p->pointer, param, p->length))
-			ret = -EFAULT;
-	}
-
-out:
-	kfree(param);
-	return ret;
-}
diff --git a/drivers/staging/vt6655/hostap.h b/drivers/staging/vt6655/hostap.h
deleted file mode 100644
index 17df4e403fcf..000000000000
--- a/drivers/staging/vt6655/hostap.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: hostap.h
- *
- * Purpose:
- *
- * Author: Lyndon Chen
- *
- * Date: May 21, 2003
- *
- */
-
-#ifndef __HOSTAP_H__
-#define __HOSTAP_H__
-
-#include "device.h"
-
-#define WLAN_RATE_1M    BIT0
-#define WLAN_RATE_2M    BIT1
-#define WLAN_RATE_5M5   BIT2
-#define WLAN_RATE_11M   BIT3
-#define WLAN_RATE_6M    BIT4
-#define WLAN_RATE_9M    BIT5
-#define WLAN_RATE_12M   BIT6
-#define WLAN_RATE_18M   BIT7
-#define WLAN_RATE_24M   BIT8
-#define WLAN_RATE_36M   BIT9
-#define WLAN_RATE_48M   BIT10
-#define WLAN_RATE_54M   BIT11
-
-#ifndef ETH_P_PAE
-#define ETH_P_PAE 0x888E /* Port Access Entity (IEEE 802.1X) */
-#endif /* ETH_P_PAE */
-
-#ifndef ARPHRD_IEEE80211
-#define ARPHRD_IEEE80211 801
-#endif
-
-int vt6655_hostap_set_hostapd(struct vnt_private *, int val, int rtnl_locked);
-int vt6655_hostap_ioctl(struct vnt_private *, struct iw_point *p);
-
-#endif // __HOSTAP_H__
diff --git a/drivers/staging/vt6655/iocmd.h b/drivers/staging/vt6655/iocmd.h
deleted file mode 100644
index a665cfd8a482..000000000000
--- a/drivers/staging/vt6655/iocmd.h
+++ /dev/null
@@ -1,408 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: iocmd.h
- *
- * Purpose: Handles the viawget ioctl private interface functions
- *
- * Author: Lyndon Chen
- *
- * Date: May 8, 2002
- *
- */
-
-#ifndef __IOCMD_H__
-#define __IOCMD_H__
-
-#include "ttype.h"
-
-// ioctl Command code
-#define MAGIC_CODE	                 0x3142
-#define IOCTL_CMD_TEST	            (SIOCDEVPRIVATE + 0)
-#define IOCTL_CMD_SET			    (SIOCDEVPRIVATE + 1)
-#define IOCTL_CMD_HOSTAPD           (SIOCDEVPRIVATE + 2)
-#define IOCTL_CMD_WPA               (SIOCDEVPRIVATE + 3)
-
-typedef enum tagWMAC_CMD {
-	WLAN_CMD_BSS_SCAN,
-	WLAN_CMD_BSS_JOIN,
-	WLAN_CMD_DISASSOC,
-	WLAN_CMD_SET_WEP,
-	WLAN_CMD_GET_LINK,
-	WLAN_CMD_GET_LISTLEN,
-	WLAN_CMD_GET_LIST,
-	WLAN_CMD_GET_MIB,
-	WLAN_CMD_GET_STAT,
-	WLAN_CMD_STOP_MAC,
-	WLAN_CMD_START_MAC,
-	WLAN_CMD_AP_START,
-	WLAN_CMD_SET_HOSTAPD,
-	WLAN_CMD_SET_HOSTAPD_STA,
-	WLAN_CMD_SET_802_1X,
-	WLAN_CMD_SET_HOST_WEP,
-	WLAN_CMD_SET_WPA,
-	WLAN_CMD_GET_NODE_CNT,
-	WLAN_CMD_ZONETYPE_SET,
-	WLAN_CMD_GET_NODE_LIST
-} WMAC_CMD, *PWMAC_CMD;
-
-typedef enum tagWZONETYPE {
-	ZoneType_USA = 0,
-	ZoneType_Japan = 1,
-	ZoneType_Europe = 2
-} WZONETYPE;
-
-#define ADHOC	0
-#define INFRA	1
-#define BOTH	2
-#define AP	    3
-
-#define ADHOC_STARTED	   1
-#define ADHOC_JOINTED	   2
-
-#define PHY80211a       0
-#define PHY80211b       1
-#define PHY80211g       2
-
-#define SSID_ID                0
-#define SSID_MAXLEN            32
-#define BSSID_LEN              6
-#define WEP_NKEYS              4
-#define WEP_KEYMAXLEN          29
-#define WEP_40BIT_LEN          5
-#define WEP_104BIT_LEN         13
-#define WEP_232BIT_LEN         16
-
-// Ioctl interface structure
-// Command structure
-//
-#pragma pack(1)
-typedef struct tagSCmdRequest {
-	u8	    name[16];
-	void __user *data;
-	u16	    wResult;
-	u16     wCmdCode;
-} SCmdRequest, *PSCmdRequest;
-
-//
-// Scan
-//
-
-typedef struct tagSCmdScan {
-	u8 ssid[SSID_MAXLEN + 2];
-} SCmdScan, *PSCmdScan;
-
-//
-// BSS Join
-//
-
-typedef struct tagSCmdBSSJoin {
-	u16	    wBSSType;
-	u16     wBBPType;
-	u8	    ssid[SSID_MAXLEN + 2];
-	u32	    uChannel;
-	bool bPSEnable;
-	bool bShareKeyAuth;
-} SCmdBSSJoin, *PSCmdBSSJoin;
-
-//
-// Zonetype Setting
-//
-
-typedef struct tagSCmdZoneTypeSet {
-	bool bWrite;
-	WZONETYPE  ZoneType;
-} SCmdZoneTypeSet, *PSCmdZoneTypeSet;
-
-#ifdef WPA_SM_Transtatus
-typedef struct tagSWPAResult {
-	char	ifname[100];
-	u8 proto;
-	u8 key_mgmt;
-	u8 eap_type;
-	bool authenticated;
-} SWPAResult, *PSWPAResult;
-#endif
-
-typedef struct tagSCmdStartAP {
-	u16	    wBSSType;
-	u16     wBBPType;
-	u8	    ssid[SSID_MAXLEN + 2];
-	u32	    uChannel;
-	u32     uBeaconInt;
-	bool bShareKeyAuth;
-	u8      byBasicRate;
-} SCmdStartAP, *PSCmdStartAP;
-
-typedef struct tagSCmdSetWEP {
-	bool bEnableWep;
-	u8      byKeyIndex;
-	u8      abyWepKey[WEP_NKEYS][WEP_KEYMAXLEN];
-	bool bWepKeyAvailable[WEP_NKEYS];
-	u32     auWepKeyLength[WEP_NKEYS];
-} SCmdSetWEP, *PSCmdSetWEP;
-
-typedef struct tagSBSSIDItem {
-	u32	    uChannel;
-	u8      abyBSSID[BSSID_LEN];
-	u8      abySSID[SSID_MAXLEN + 1];
-	u8      byNetType;
-	u16	    wBeaconInterval;
-	u16	    wCapInfo;        // for address of byNetType at align 4
-
-	bool bWEPOn;
-	u32     uRSSI;
-} SBSSIDItem;
-
-typedef struct tagSBSSIDList {
-	u32		    uItem;
-	SBSSIDItem	sBSSIDList[0];
-} SBSSIDList, *PSBSSIDList;
-
-typedef struct tagSCmdLinkStatus {
-	bool bLink;
-	u16   wBSSType;
-	u8      byState;
-	u8      abyBSSID[BSSID_LEN];
-	u8      abySSID[SSID_MAXLEN + 2];
-	u32     uChannel;
-	u32     uLinkRate;
-} SCmdLinkStatus, *PSCmdLinkStatus;
-
-//
-// 802.11 counter
-//
-typedef struct tagSDot11MIBCount {
-	u32 TransmittedFragmentCount;
-	u32 MulticastTransmittedFrameCount;
-	u32 FailedCount;
-	u32 RetryCount;
-	u32 MultipleRetryCount;
-	u32 RTSSuccessCount;
-	u32 RTSFailureCount;
-	u32 ACKFailureCount;
-	u32 FrameDuplicateCount;
-	u32 ReceivedFragmentCount;
-	u32 MulticastReceivedFrameCount;
-	u32 FCSErrorCount;
-} SDot11MIBCount, *PSDot11MIBCount;
-
-//
-// statistic counter
-//
-typedef struct tagSStatMIBCount {
-	//
-	// ISR status count
-	//
-	u32   dwIsrTx0OK;
-	u32   dwIsrTx1OK;
-	u32   dwIsrBeaconTxOK;
-	u32   dwIsrRxOK;
-	u32   dwIsrTBTTInt;
-	u32   dwIsrSTIMERInt;
-	u32   dwIsrUnrecoverableError;
-	u32   dwIsrSoftInterrupt;
-	u32   dwIsrRxNoBuf;
-
-	u32   dwIsrUnknown;
-
-	// RSR status count
-	//
-	u32   dwRsrFrmAlgnErr;
-	u32   dwRsrErr;
-	u32   dwRsrCRCErr;
-	u32   dwRsrCRCOk;
-	u32   dwRsrBSSIDOk;
-	u32   dwRsrADDROk;
-	u32   dwRsrICVOk;
-	u32   dwNewRsrShortPreamble;
-	u32   dwRsrLong;
-	u32   dwRsrRunt;
-
-	u32   dwRsrRxControl;
-	u32   dwRsrRxData;
-	u32   dwRsrRxManage;
-
-	u32   dwRsrRxPacket;
-	u32   dwRsrRxOctet;
-	u32   dwRsrBroadcast;
-	u32   dwRsrMulticast;
-	u32   dwRsrDirected;
-	// 64-bit OID
-	u32   ullRsrOK;
-
-	// for some optional OIDs (64 bits) and DMI support
-	u32   ullRxBroadcastBytes;
-	u32   ullRxMulticastBytes;
-	u32   ullRxDirectedBytes;
-	u32   ullRxBroadcastFrames;
-	u32   ullRxMulticastFrames;
-	u32   ullRxDirectedFrames;
-
-	u32   dwRsrRxFragment;
-	u32   dwRsrRxFrmLen64;
-	u32   dwRsrRxFrmLen65_127;
-	u32   dwRsrRxFrmLen128_255;
-	u32   dwRsrRxFrmLen256_511;
-	u32   dwRsrRxFrmLen512_1023;
-	u32   dwRsrRxFrmLen1024_1518;
-
-	// TSR0,1 status count
-	//
-	u32   dwTsrTotalRetry[2];        // total collision retry count
-	u32   dwTsrOnceRetry[2];         // this packet only occur one collision
-	u32   dwTsrMoreThanOnceRetry[2]; // this packet occur more than one collision
-	u32   dwTsrRetry[2];             // this packet has ever occur collision,
-	// that is (dwTsrOnceCollision0 + dwTsrMoreThanOnceCollision0)
-	u32   dwTsrACKData[2];
-	u32   dwTsrErr[2];
-	u32   dwAllTsrOK[2];
-	u32   dwTsrRetryTimeout[2];
-	u32   dwTsrTransmitTimeout[2];
-
-	u32   dwTsrTxPacket[2];
-	u32   dwTsrTxOctet[2];
-	u32   dwTsrBroadcast[2];
-	u32   dwTsrMulticast[2];
-	u32   dwTsrDirected[2];
-
-	// RD/TD count
-	u32   dwCntRxFrmLength;
-	u32   dwCntTxBufLength;
-
-	u8    abyCntRxPattern[16];
-	u8    abyCntTxPattern[16];
-
-	// Software check....
-	u32   dwCntRxDataErr;             // rx buffer data software compare CRC err count
-	u32   dwCntDecryptErr;            // rx buffer data software compare CRC err count
-	u32   dwCntRxICVErr;              // rx buffer data software compare CRC err count
-	u32    idxRxErrorDesc;             // index for rx data error RD
-
-	// 64-bit OID
-	u32   ullTsrOK[2];
-
-	// for some optional OIDs (64 bits) and DMI support
-	u32   ullTxBroadcastFrames[2];
-	u32   ullTxMulticastFrames[2];
-	u32   ullTxDirectedFrames[2];
-	u32   ullTxBroadcastBytes[2];
-	u32   ullTxMulticastBytes[2];
-	u32   ullTxDirectedBytes[2];
-} SStatMIBCount, *PSStatMIBCount;
-
-typedef struct tagSNodeItem {
-	// STA info
-	u16            wAID;
-	u8             abyMACAddr[6];
-	u16            wTxDataRate;
-	u16            wInActiveCount;
-	u16            wEnQueueCnt;
-	u16            wFlags;
-	bool bPWBitOn;
-	u8             byKeyIndex;
-	u16            wWepKeyLength;
-	u8            abyWepKey[WEP_KEYMAXLEN];
-	// Auto rate fallback vars
-	bool bIsInFallback;
-	u32            uTxFailures;
-	u32            uTxAttempts;
-	u16            wFailureRatio;
-} SNodeItem;
-
-typedef struct tagSNodeList {
-	u32		    uItem;
-	SNodeItem	sNodeList[0];
-} SNodeList, *PSNodeList;
-
-typedef struct tagSCmdValue {
-	u32 dwValue;
-} SCmdValue,  *PSCmdValue;
-
-//
-// hostapd & viawget ioctl related
-//
-
-enum {
-	VIAWGET_HOSTAPD_FLUSH = 1,
-	VIAWGET_HOSTAPD_ADD_STA = 2,
-	VIAWGET_HOSTAPD_REMOVE_STA = 3,
-	VIAWGET_HOSTAPD_GET_INFO_STA = 4,
-	VIAWGET_HOSTAPD_SET_ENCRYPTION = 5,
-	VIAWGET_HOSTAPD_GET_ENCRYPTION = 6,
-	VIAWGET_HOSTAPD_SET_FLAGS_STA = 7,
-	VIAWGET_HOSTAPD_SET_ASSOC_AP_ADDR = 8,
-	VIAWGET_HOSTAPD_SET_GENERIC_ELEMENT = 9,
-	VIAWGET_HOSTAPD_MLME = 10,
-	VIAWGET_HOSTAPD_SCAN_REQ = 11,
-	VIAWGET_HOSTAPD_STA_CLEAR_STATS = 12,
-};
-
-#define VIAWGET_HOSTAPD_GENERIC_ELEMENT_HDR_LEN				\
-	((int)(&((struct viawget_hostapd_param *)0)->u.generic_elem.data))
-
-// Maximum length for algorithm names (-1 for nul termination) used in ioctl()
-
-struct viawget_hostapd_param {
-	u32 cmd;
-	u8 sta_addr[6];
-	union {
-		struct {
-			u16 aid;
-			u16 capability;
-			u8 tx_supp_rates;
-		} add_sta;
-		struct {
-			u32 inactive_sec;
-		} get_info_sta;
-		struct {
-			u8 alg;
-			u32 flags;
-			u32 err;
-			u8 idx;
-			u8 seq[8];
-			u16 key_len;
-			u8 key[0];
-		} crypt;
-		struct {
-			u32 flags_and;
-			u32 flags_or;
-		} set_flags_sta;
-		struct {
-			u16 rid;
-			u16 len;
-			u8 data[0];
-		} rid;
-		struct {
-			u8 len;
-			u8 data[0];
-		} generic_elem;
-		struct {
-			u16 cmd;
-			u16 reason_code;
-		} mlme;
-		struct {
-			u8 ssid_len;
-			u8 ssid[32];
-		} scan_req;
-	} u;
-};
-
-#pragma pack()
-
-#endif //__IOCMD_H__
diff --git a/drivers/staging/vt6655/ioctl.c b/drivers/staging/vt6655/ioctl.c
deleted file mode 100644
index 970e80d92fb9..000000000000
--- a/drivers/staging/vt6655/ioctl.c
+++ /dev/null
@@ -1,658 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: ioctl.c
- *
- * Purpose:  private ioctl functions
- *
- * Author: Lyndon Chen
- *
- * Date: Auguest 20, 2003
- *
- * Functions:
- *
- * Revision History:
- *
- */
-
-#include "ioctl.h"
-#include "iocmd.h"
-#include "mac.h"
-#include "card.h"
-#include "hostap.h"
-#include "wpactl.h"
-#include "rf.h"
-
-#ifdef WPA_SM_Transtatus
-SWPAResult wpa_Result;
-#endif
-
-int private_ioctl(struct vnt_private *pDevice, struct ifreq *rq)
-{
-	PSCmdRequest	pReq = (PSCmdRequest)rq;
-	PSMgmtObject	pMgmt = pDevice->pMgmt;
-	int		result = 0;
-	PWLAN_IE_SSID	pItemSSID;
-	SCmdBSSJoin	sJoinCmd;
-	SCmdZoneTypeSet	sZoneTypeCmd;
-	SCmdScan	sScanCmd;
-	SCmdStartAP	sStartAPCmd;
-	SCmdSetWEP	sWEPCmd;
-	SCmdValue	sValue;
-	SBSSIDList	sList;
-	SNodeList	sNodeList;
-	PSBSSIDList	pList;
-	PSNodeList	pNodeList;
-	unsigned int	cbListCount;
-	PKnownBSS	pBSS;
-	PKnownNodeDB	pNode;
-	unsigned int	ii, jj;
-	unsigned char	abySuppRates[] = {WLAN_EID_SUPP_RATES, 4, 0x02, 0x04, 0x0B, 0x16};
-	unsigned char	abyNullAddr[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
-	unsigned long	dwKeyIndex = 0;
-	unsigned char	abyScanSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-	long		ldBm;
-
-	pReq->wResult = 0;
-
-	switch (pReq->wCmdCode) {
-	case WLAN_CMD_BSS_SCAN:
-		pr_debug("WLAN_CMD_BSS_SCAN..begin\n");
-		if (copy_from_user(&sScanCmd, pReq->data, sizeof(SCmdScan))) {
-			result = -EFAULT;
-			break;
-		}
-
-		pItemSSID = (PWLAN_IE_SSID)sScanCmd.ssid;
-		if (pItemSSID->len > WLAN_SSID_MAXLEN + 1)
-			return -EINVAL;
-		if (pItemSSID->len != 0) {
-			memset(abyScanSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-			memcpy(abyScanSSID, pItemSSID, pItemSSID->len + WLAN_IEHDR_LEN);
-		}
-
-		if (pDevice->bMACSuspend == true) {
-			if (pDevice->bRadioOff == true)
-				CARDbRadioPowerOn(pDevice);
-			vMgrTimerInit(pDevice);
-			MACvIntEnable(pDevice->PortOffset, IMR_MASK_VALUE);
-			add_timer(&pMgmt->sTimerSecondCallback);
-			pDevice->bMACSuspend = false;
-		}
-		spin_lock_irq(&pDevice->lock);
-		if (memcmp(pMgmt->abyCurrBSSID, &abyNullAddr[0], 6) == 0)
-			BSSvClearBSSList((void *)pDevice, false);
-		else
-			BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
-
-		if (pItemSSID->len != 0)
-			bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, abyScanSSID);
-		else
-			bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, NULL);
-		spin_unlock_irq(&pDevice->lock);
-		break;
-
-	case WLAN_CMD_ZONETYPE_SET:
-		/* mike add :can't support. */
-		result = -EOPNOTSUPP;
-		break;
-
-		if (copy_from_user(&sZoneTypeCmd, pReq->data, sizeof(SCmdZoneTypeSet))) {
-			result = -EFAULT;
-			break;
-		}
-
-		if (sZoneTypeCmd.bWrite == true) {
-			/* write zonetype */
-			if (sZoneTypeCmd.ZoneType == ZoneType_USA) {
-				/* set to USA */
-				pr_debug("set_ZoneType:USA\n");
-			} else if (sZoneTypeCmd.ZoneType == ZoneType_Japan) {
-				/* set to Japan */
-				pr_debug("set_ZoneType:Japan\n");
-			} else if (sZoneTypeCmd.ZoneType == ZoneType_Europe) {
-				/* set to Europe */
-				pr_debug("set_ZoneType:Europe\n");
-			}
-		} else {
-			/* read zonetype */
-			unsigned char zonetype = 0;
-
-			if (zonetype == 0x00) {		/* USA */
-				sZoneTypeCmd.ZoneType = ZoneType_USA;
-			} else if (zonetype == 0x01) {	/* Japan */
-				sZoneTypeCmd.ZoneType = ZoneType_Japan;
-			} else if (zonetype == 0x02) {	/* Europe */
-				sZoneTypeCmd.ZoneType = ZoneType_Europe;
-			} else {			/* Unknown ZoneType */
-				pr_err("Error:ZoneType[%x] Unknown ???\n", zonetype);
-				result = -EFAULT;
-				break;
-			}
-			if (copy_to_user(pReq->data, &sZoneTypeCmd, sizeof(SCmdZoneTypeSet))) {
-				result = -EFAULT;
-				break;
-			}
-		}
-		break;
-
-	case WLAN_CMD_BSS_JOIN:
-		if (pDevice->bMACSuspend == true) {
-			if (pDevice->bRadioOff == true)
-				CARDbRadioPowerOn(pDevice);
-			vMgrTimerInit(pDevice);
-			MACvIntEnable(pDevice->PortOffset, IMR_MASK_VALUE);
-			add_timer(&pMgmt->sTimerSecondCallback);
-			pDevice->bMACSuspend = false;
-		}
-
-		if (copy_from_user(&sJoinCmd, pReq->data, sizeof(SCmdBSSJoin))) {
-			result = -EFAULT;
-			break;
-		}
-
-		pItemSSID = (PWLAN_IE_SSID)sJoinCmd.ssid;
-		if (pItemSSID->len > WLAN_SSID_MAXLEN + 1)
-			return -EINVAL;
-		memset(pMgmt->abyDesireSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-		memcpy(pMgmt->abyDesireSSID, pItemSSID, pItemSSID->len + WLAN_IEHDR_LEN);
-		if (sJoinCmd.wBSSType == ADHOC) {
-			pMgmt->eConfigMode = WMAC_CONFIG_IBSS_STA;
-			pr_debug("ioct set to adhoc mode\n");
-		} else {
-			pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
-			pr_debug("ioct set to STA mode\n");
-		}
-		if (sJoinCmd.bPSEnable == true) {
-			pDevice->ePSMode = WMAC_POWER_FAST;
-			pMgmt->wListenInterval = 2;
-			pr_debug("Power Saving On\n");
-		} else {
-			pDevice->ePSMode = WMAC_POWER_CAM;
-			pMgmt->wListenInterval = 1;
-			pr_debug("Power Saving Off\n");
-		}
-
-		if (sJoinCmd.bShareKeyAuth == true) {
-			pMgmt->bShareKeyAlgorithm = true;
-			pr_debug("Share Key\n");
-		} else {
-			pMgmt->bShareKeyAlgorithm = false;
-			pr_debug("Open System\n");
-		}
-		pDevice->uChannel = sJoinCmd.uChannel;
-		netif_stop_queue(pDevice->dev);
-		spin_lock_irq(&pDevice->lock);
-		pMgmt->eCurrState = WMAC_STATE_IDLE;
-		bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, pMgmt->abyDesireSSID);
-		bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, NULL);
-		spin_unlock_irq(&pDevice->lock);
-		break;
-
-	case WLAN_CMD_SET_WEP:
-		pr_debug("WLAN_CMD_SET_WEP Key\n");
-		memset(&sWEPCmd, 0, sizeof(SCmdSetWEP));
-		if (copy_from_user(&sWEPCmd, pReq->data, sizeof(SCmdSetWEP))) {
-			result = -EFAULT;
-			break;
-		}
-		if (sWEPCmd.bEnableWep != true) {
-			pDevice->bEncryptionEnable = false;
-			pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
-			MACvDisableDefaultKey(pDevice->PortOffset);
-			pr_debug("WEP function disable\n");
-			break;
-		}
-
-		for (ii = 0; ii < WLAN_WEP_NKEYS; ii++) {
-			if (sWEPCmd.bWepKeyAvailable[ii]) {
-				if (ii == sWEPCmd.byKeyIndex)
-					dwKeyIndex = ii | (1 << 31);
-				else
-					dwKeyIndex = ii;
-
-				KeybSetDefaultKey(&(pDevice->sKey),
-						  dwKeyIndex,
-						  sWEPCmd.auWepKeyLength[ii],
-						  NULL,
-						  (unsigned char *)&sWEPCmd.abyWepKey[ii][0],
-						  KEY_CTL_WEP,
-						  pDevice->PortOffset,
-						  pDevice->byLocalID);
-			}
-		}
-		pDevice->byKeyIndex = sWEPCmd.byKeyIndex;
-		pDevice->bTransmitKey = true;
-		pDevice->bEncryptionEnable = true;
-		pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-		break;
-
-	case WLAN_CMD_GET_LINK: {
-		SCmdLinkStatus sLinkStatus;
-
-		pr_debug("WLAN_CMD_GET_LINK status\n");
-
-		memset(&sLinkStatus, 0, sizeof(sLinkStatus));
-
-		if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)
-			sLinkStatus.wBSSType = ADHOC;
-		else
-			sLinkStatus.wBSSType = INFRA;
-
-		if (pMgmt->eCurrState == WMAC_STATE_JOINTED)
-			sLinkStatus.byState = ADHOC_JOINTED;
-		else
-			sLinkStatus.byState = ADHOC_STARTED;
-
-		sLinkStatus.uChannel = pMgmt->uCurrChannel;
-		if (pDevice->bLinkPass == true) {
-			sLinkStatus.bLink = true;
-			pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
-			memcpy(sLinkStatus.abySSID, pItemSSID->abySSID, pItemSSID->len);
-			memcpy(sLinkStatus.abyBSSID, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-			sLinkStatus.uLinkRate = pMgmt->sNodeDBTable[0].wTxDataRate;
-			pr_debug(" Link Success!\n");
-		} else {
-			sLinkStatus.bLink = false;
-			sLinkStatus.uLinkRate = 0;
-		}
-		if (copy_to_user(pReq->data, &sLinkStatus, sizeof(SCmdLinkStatus))) {
-			result = -EFAULT;
-			break;
-		}
-		break;
-	}
-	case WLAN_CMD_GET_LISTLEN:
-		cbListCount = 0;
-		pBSS = &(pMgmt->sBSSList[0]);
-		for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-			pBSS = &(pMgmt->sBSSList[ii]);
-			if (!pBSS->bActive)
-				continue;
-			cbListCount++;
-		}
-		sList.uItem = cbListCount;
-		if (copy_to_user(pReq->data, &sList, sizeof(SBSSIDList))) {
-			result = -EFAULT;
-			break;
-		}
-		pReq->wResult = 0;
-		break;
-
-	case WLAN_CMD_GET_LIST:
-		if (copy_from_user(&sList, pReq->data, sizeof(SBSSIDList))) {
-			result = -EFAULT;
-			break;
-		}
-		if (sList.uItem > (ULONG_MAX - sizeof(SBSSIDList)) / sizeof(SBSSIDItem)) {
-			result = -EINVAL;
-			break;
-		}
-		pList = (PSBSSIDList)kmalloc(sizeof(SBSSIDList) + (sList.uItem * sizeof(SBSSIDItem)),
-					     GFP_ATOMIC);
-		if (pList == NULL) {
-			result = -ENOMEM;
-			break;
-		}
-		pList->uItem = sList.uItem;
-		pBSS = &(pMgmt->sBSSList[0]);
-		for (ii = 0, jj = 0; jj < MAX_BSS_NUM; jj++) {
-			pBSS = &(pMgmt->sBSSList[jj]);
-			if (pBSS->bActive) {
-				pList->sBSSIDList[ii].uChannel = pBSS->uChannel;
-				pList->sBSSIDList[ii].wBeaconInterval = pBSS->wBeaconInterval;
-				pList->sBSSIDList[ii].wCapInfo = pBSS->wCapInfo;
-				RFvRSSITodBm(pDevice, (unsigned char)(pBSS->uRSSI), &ldBm);
-				pList->sBSSIDList[ii].uRSSI = (unsigned int)ldBm;
-				memcpy(pList->sBSSIDList[ii].abyBSSID, pBSS->abyBSSID, WLAN_BSSID_LEN);
-				pItemSSID = (PWLAN_IE_SSID)pBSS->abySSID;
-				memset(pList->sBSSIDList[ii].abySSID, 0, WLAN_SSID_MAXLEN + 1);
-				memcpy(pList->sBSSIDList[ii].abySSID, pItemSSID->abySSID, pItemSSID->len);
-				if (WLAN_GET_CAP_INFO_ESS(pBSS->wCapInfo))
-					pList->sBSSIDList[ii].byNetType = INFRA;
-				else
-					pList->sBSSIDList[ii].byNetType = ADHOC;
-
-				if (WLAN_GET_CAP_INFO_PRIVACY(pBSS->wCapInfo))
-					pList->sBSSIDList[ii].bWEPOn = true;
-				else
-					pList->sBSSIDList[ii].bWEPOn = false;
-
-				ii++;
-				if (ii >= pList->uItem)
-					break;
-			}
-		}
-
-		if (copy_to_user(pReq->data, pList, sizeof(SBSSIDList) + (sList.uItem * sizeof(SBSSIDItem)))) {
-			result = -EFAULT;
-			break;
-		}
-		kfree(pList);
-		pReq->wResult = 0;
-		break;
-
-	case WLAN_CMD_GET_MIB:
-		if (copy_to_user(pReq->data, &(pDevice->s802_11Counter), sizeof(SDot11MIBCount))) {
-			result = -EFAULT;
-			break;
-		}
-		break;
-
-	case WLAN_CMD_GET_STAT:
-		if (copy_to_user(pReq->data, &(pDevice->scStatistic), sizeof(SStatCounter))) {
-			result = -EFAULT;
-			break;
-		}
-		break;
-
-	case WLAN_CMD_STOP_MAC:
-		pr_debug("WLAN_CMD_STOP_MAC\n");
-		netif_stop_queue(pDevice->dev);
-
-		spin_lock_irq(&pDevice->lock);
-		if (pDevice->bRadioOff == false)
-			CARDbRadioPowerOff(pDevice);
-
-		pDevice->bLinkPass = false;
-		memset(pMgmt->abyCurrBSSID, 0, 6);
-		pMgmt->eCurrState = WMAC_STATE_IDLE;
-		del_timer(&pDevice->sTimerCommand);
-		del_timer(&pMgmt->sTimerSecondCallback);
-		pDevice->bCmdRunning = false;
-		pDevice->bMACSuspend = true;
-		MACvIntDisable(pDevice->PortOffset);
-		spin_unlock_irq(&pDevice->lock);
-		break;
-
-	case WLAN_CMD_START_MAC:
-		pr_debug("WLAN_CMD_START_MAC\n");
-
-		if (pDevice->bMACSuspend == true) {
-			if (pDevice->bRadioOff == true)
-				CARDbRadioPowerOn(pDevice);
-			vMgrTimerInit(pDevice);
-			MACvIntEnable(pDevice->PortOffset, IMR_MASK_VALUE);
-			add_timer(&pMgmt->sTimerSecondCallback);
-			pDevice->bMACSuspend = false;
-		}
-		break;
-
-	case WLAN_CMD_SET_HOSTAPD:
-		pr_debug("WLAN_CMD_SET_HOSTAPD\n");
-
-		if (copy_from_user(&sValue, pReq->data, sizeof(SCmdValue))) {
-			result = -EFAULT;
-			break;
-		}
-		if (sValue.dwValue == 1) {
-			if (vt6655_hostap_set_hostapd(pDevice, 1, 1) == 0) {
-				pr_debug("Enable HOSTAP\n");
-			} else {
-				result = -EFAULT;
-				break;
-			}
-		} else {
-			vt6655_hostap_set_hostapd(pDevice, 0, 1);
-			pr_debug("Disable HOSTAP\n");
-		}
-		break;
-
-	case WLAN_CMD_SET_HOSTAPD_STA:
-		pr_debug("WLAN_CMD_SET_HOSTAPD_STA\n");
-		break;
-
-	case WLAN_CMD_SET_802_1X:
-		pr_debug("WLAN_CMD_SET_802_1X\n");
-		if (copy_from_user(&sValue, pReq->data, sizeof(SCmdValue))) {
-			result = -EFAULT;
-			break;
-		}
-
-		if (sValue.dwValue == 1) {
-			pDevice->bEnable8021x = true;
-			pr_debug("Enable 802.1x\n");
-		} else {
-			pDevice->bEnable8021x = false;
-			pr_debug("Disable 802.1x\n");
-		}
-		break;
-
-	case WLAN_CMD_SET_HOST_WEP:
-		pr_debug("WLAN_CMD_SET_HOST_WEP\n");
-		if (copy_from_user(&sValue, pReq->data, sizeof(SCmdValue))) {
-			result = -EFAULT;
-			break;
-		}
-
-		if (sValue.dwValue == 1) {
-			pDevice->bEnableHostWEP = true;
-			pr_debug("Enable HostWEP\n");
-		} else {
-			pDevice->bEnableHostWEP = false;
-			pr_debug("Disable HostWEP\n");
-		}
-		break;
-
-	case WLAN_CMD_SET_WPA:
-		pr_debug("WLAN_CMD_SET_WPA\n");
-
-		if (copy_from_user(&sValue, pReq->data, sizeof(SCmdValue))) {
-			result = -EFAULT;
-			break;
-		}
-		if (sValue.dwValue == 1) {
-			pr_debug("up wpadev\n");
-			eth_hw_addr_inherit(pDevice->wpadev, pDevice->dev);
-			pDevice->bWPADEVUp = true;
-		} else {
-			pr_debug("close wpadev\n");
-			pDevice->bWPADEVUp = false;
-		}
-		break;
-
-	case WLAN_CMD_AP_START:
-		pr_debug("WLAN_CMD_AP_START\n");
-		if (pDevice->bRadioOff == true) {
-			CARDbRadioPowerOn(pDevice);
-			vMgrTimerInit(pDevice);
-			MACvIntEnable(pDevice->PortOffset, IMR_MASK_VALUE);
-			add_timer(&pMgmt->sTimerSecondCallback);
-		}
-		if (copy_from_user(&sStartAPCmd, pReq->data, sizeof(SCmdStartAP))) {
-			result = -EFAULT;
-			break;
-		}
-
-		if (sStartAPCmd.wBSSType == AP) {
-			pMgmt->eConfigMode = WMAC_CONFIG_AP;
-			pr_debug("ioct set to AP mode\n");
-		} else {
-			pr_debug("ioct BSS type not set to AP mode\n");
-			result = -EFAULT;
-			break;
-		}
-
-		if (sStartAPCmd.wBBPType == PHY80211g)
-			pMgmt->byAPBBType = PHY_TYPE_11G;
-		else if (sStartAPCmd.wBBPType == PHY80211a)
-			pMgmt->byAPBBType = PHY_TYPE_11A;
-		else
-			pMgmt->byAPBBType = PHY_TYPE_11B;
-
-		pItemSSID = (PWLAN_IE_SSID)sStartAPCmd.ssid;
-		if (pItemSSID->len > WLAN_SSID_MAXLEN + 1)
-			return -EINVAL;
-		memset(pMgmt->abyDesireSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-		memcpy(pMgmt->abyDesireSSID, pItemSSID, pItemSSID->len + WLAN_IEHDR_LEN);
-
-		if ((sStartAPCmd.uChannel > 0) && (sStartAPCmd.uChannel <= 14))
-			pDevice->uChannel = sStartAPCmd.uChannel;
-
-		if ((sStartAPCmd.uBeaconInt >= 20) && (sStartAPCmd.uBeaconInt <= 1000))
-			pMgmt->wIBSSBeaconPeriod = sStartAPCmd.uBeaconInt;
-		else
-			pMgmt->wIBSSBeaconPeriod = 100;
-
-		if (sStartAPCmd.bShareKeyAuth == true) {
-			pMgmt->bShareKeyAlgorithm = true;
-			pr_debug("Share Key\n");
-		} else {
-			pMgmt->bShareKeyAlgorithm = false;
-			pr_debug("Open System\n");
-		}
-		memcpy(pMgmt->abyIBSSSuppRates, abySuppRates, 6);
-
-		if (sStartAPCmd.byBasicRate & BIT3) {
-			pMgmt->abyIBSSSuppRates[2] |= BIT7;
-			pMgmt->abyIBSSSuppRates[3] |= BIT7;
-			pMgmt->abyIBSSSuppRates[4] |= BIT7;
-			pMgmt->abyIBSSSuppRates[5] |= BIT7;
-		} else if (sStartAPCmd.byBasicRate & BIT2) {
-			pMgmt->abyIBSSSuppRates[2] |= BIT7;
-			pMgmt->abyIBSSSuppRates[3] |= BIT7;
-			pMgmt->abyIBSSSuppRates[4] |= BIT7;
-		} else if (sStartAPCmd.byBasicRate & BIT1) {
-			pMgmt->abyIBSSSuppRates[2] |= BIT7;
-			pMgmt->abyIBSSSuppRates[3] |= BIT7;
-		} else if (sStartAPCmd.byBasicRate & BIT1) {
-			pMgmt->abyIBSSSuppRates[2] |= BIT7;
-		} else {
-			/* default 1,2M */
-			pMgmt->abyIBSSSuppRates[2] |= BIT7;
-			pMgmt->abyIBSSSuppRates[3] |= BIT7;
-		}
-
-		pr_debug("Support Rate= %*ph\n",
-			 4, pMgmt->abyIBSSSuppRates + 2);
-
-		netif_stop_queue(pDevice->dev);
-		spin_lock_irq(&pDevice->lock);
-		bScheduleCommand((void *)pDevice, WLAN_CMD_RUN_AP, NULL);
-		spin_unlock_irq(&pDevice->lock);
-		break;
-
-	case WLAN_CMD_GET_NODE_CNT:
-		cbListCount = 0;
-		pNode = &(pMgmt->sNodeDBTable[0]);
-		for (ii = 0; ii < (MAX_NODE_NUM + 1); ii++) {
-			pNode = &(pMgmt->sNodeDBTable[ii]);
-			if (!pNode->bActive)
-				continue;
-			cbListCount++;
-		}
-
-		sNodeList.uItem = cbListCount;
-		if (copy_to_user(pReq->data, &sNodeList, sizeof(SNodeList))) {
-			result = -EFAULT;
-			break;
-		}
-		pReq->wResult = 0;
-		break;
-
-	case WLAN_CMD_GET_NODE_LIST:
-		if (copy_from_user(&sNodeList, pReq->data, sizeof(SNodeList))) {
-			result = -EFAULT;
-			break;
-		}
-		if (sNodeList.uItem > (ULONG_MAX - sizeof(SNodeList)) / sizeof(SNodeItem)) {
-			result = -EINVAL;
-			break;
-		}
-		pNodeList = (PSNodeList)kmalloc(sizeof(SNodeList) + (sNodeList.uItem * sizeof(SNodeItem)),
-						GFP_ATOMIC);
-		if (pNodeList == NULL) {
-			result = -ENOMEM;
-			break;
-		}
-		pNodeList->uItem = sNodeList.uItem;
-		pNode = &(pMgmt->sNodeDBTable[0]);
-		for (ii = 0, jj = 0; ii < (MAX_NODE_NUM + 1); ii++) {
-			pNode = &(pMgmt->sNodeDBTable[ii]);
-			if (pNode->bActive) {
-				pNodeList->sNodeList[jj].wAID = pNode->wAID;
-				memcpy(pNodeList->sNodeList[jj].abyMACAddr, pNode->abyMACAddr, WLAN_ADDR_LEN);
-				pNodeList->sNodeList[jj].wTxDataRate = pNode->wTxDataRate;
-				pNodeList->sNodeList[jj].wInActiveCount = (unsigned short)pNode->uInActiveCount;
-				pNodeList->sNodeList[jj].wEnQueueCnt = (unsigned short)pNode->wEnQueueCnt;
-				pNodeList->sNodeList[jj].wFlags = (unsigned short)pNode->dwFlags;
-				pNodeList->sNodeList[jj].bPWBitOn = pNode->bPSEnable;
-				pNodeList->sNodeList[jj].byKeyIndex = pNode->byKeyIndex;
-				pNodeList->sNodeList[jj].wWepKeyLength = pNode->uWepKeyLength;
-				memcpy(&(pNodeList->sNodeList[jj].abyWepKey[0]), &(pNode->abyWepKey[0]), WEP_KEYMAXLEN);
-				pr_debug("key= %2.2X:%2.2X:%2.2X:%2.2X:%2.2X\n",
-					 pNodeList->sNodeList[jj].abyWepKey[0],
-					 pNodeList->sNodeList[jj].abyWepKey[1],
-					 pNodeList->sNodeList[jj].abyWepKey[2],
-					 pNodeList->sNodeList[jj].abyWepKey[3],
-					 pNodeList->sNodeList[jj].abyWepKey[4]);
-				pNodeList->sNodeList[jj].bIsInFallback = pNode->bIsInFallback;
-				pNodeList->sNodeList[jj].uTxFailures = pNode->uTxFailures;
-				pNodeList->sNodeList[jj].uTxAttempts = pNode->uTxAttempts;
-				pNodeList->sNodeList[jj].wFailureRatio = (unsigned short)pNode->uFailureRatio;
-				jj++;
-				if (jj >= pNodeList->uItem)
-					break;
-			}
-		}
-		if (copy_to_user(pReq->data, pNodeList, sizeof(SNodeList) + (sNodeList.uItem * sizeof(SNodeItem)))) {
-			result = -EFAULT;
-			break;
-		}
-		kfree(pNodeList);
-		pReq->wResult = 0;
-		break;
-
-#ifdef WPA_SM_Transtatus
-	case 0xFF:
-		memset(wpa_Result.ifname, 0, sizeof(wpa_Result.ifname));
-		wpa_Result.proto = 0;
-		wpa_Result.key_mgmt = 0;
-		wpa_Result.eap_type = 0;
-		wpa_Result.authenticated = false;
-		pDevice->fWPA_Authened = false;
-		if (copy_from_user(&wpa_Result, pReq->data, sizeof(wpa_Result))) {
-			result = -EFAULT;
-			break;
-		}
-
-		if (wpa_Result.authenticated == true) {
-#ifdef SndEvt_ToAPI
-			{
-				union iwreq_data wrqu;
-
-				pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
-
-				memset(&wrqu, 0, sizeof(wrqu));
-				wrqu.data.flags = RT_WPACONNECTED_EVENT_FLAG;
-				wrqu.data.length = pItemSSID->len;
-				wireless_send_event(pDevice->dev, IWEVCUSTOM, &wrqu, pItemSSID->abySSID);
-			}
-#endif
-			pDevice->fWPA_Authened = true; /* is successful peer to wpa_Result.authenticated? */
-		}
-		pReq->wResult = 0;
-		break;
-#endif
-
-	default:
-		pr_debug("Private command not support..\n");
-	}
-
-	return result;
-}
diff --git a/drivers/staging/vt6655/ioctl.h b/drivers/staging/vt6655/ioctl.h
deleted file mode 100644
index 2dc5a5743e8d..000000000000
--- a/drivers/staging/vt6655/ioctl.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: hostap.h
- *
- * Purpose:
- *
- * Author: Lyndon Chen
- *
- * Date: May 21, 2003
- *
- */
-
-#ifndef __IOCTL_H__
-#define __IOCTL_H__
-
-#include "device.h"
-
-int private_ioctl(struct vnt_private *, struct ifreq *rq);
-
-#endif // __IOCTL_H__
diff --git a/drivers/staging/vt6655/iowpa.h b/drivers/staging/vt6655/iowpa.h
deleted file mode 100644
index fe4b22ed49f4..000000000000
--- a/drivers/staging/vt6655/iowpa.h
+++ /dev/null
@@ -1,130 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: iowpa.h
- *
- * Purpose: Handles wpa supplicant ioctl interface
- *
- * Author: Lyndon Chen
- *
- * Date: May 8, 2002
- *
- */
-
-#ifndef __IOWPA_H__
-#define __IOWPA_H__
-
-#define WPA_IE_LEN 64
-
-//WPA related
-
-enum {
-	VIAWGET_SET_WPA = 1,
-	VIAWGET_SET_KEY = 2,
-	VIAWGET_SET_SCAN = 3,
-	VIAWGET_GET_SCAN = 4,
-	VIAWGET_GET_SSID = 5,
-	VIAWGET_GET_BSSID = 6,
-	VIAWGET_SET_DROP_UNENCRYPT = 7,
-	VIAWGET_SET_DEAUTHENTICATE = 8,
-	VIAWGET_SET_ASSOCIATE = 9,
-	VIAWGET_SET_DISASSOCIATE = 10
-};
-
-enum {
-	VIAWGET_ASSOC_MSG = 1,
-	VIAWGET_DISASSOC_MSG = 2,
-	VIAWGET_PTK_MIC_MSG = 3,
-	VIAWGET_GTK_MIC_MSG = 4,
-	VIAWGET_CCKM_ROAM_MSG = 5,
-	VIAWGET_DEVICECLOSE_MSG = 6
-};
-
-#pragma pack(1)
-typedef struct viawget_wpa_header {
-	u8 type;
-	u16 req_ie_len;
-	u16 resp_ie_len;
-} viawget_wpa_header;
-
-struct viawget_wpa_param {
-	u32 cmd;
-	u8 addr[6];
-	union {
-		struct {
-			u8 len;
-			u8 data[0];
-		} generic_elem;
-
-		struct {
-			u8 bssid[6];
-			u8 ssid[32];
-			u8 ssid_len;
-			u8 __user *wpa_ie;
-			u16 wpa_ie_len;
-			int pairwise_suite;
-			int group_suite;
-			int key_mgmt_suite;
-			int auth_alg;
-			int mode;
-
-		} wpa_associate;
-
-		struct {
-			int alg_name;
-			u16 key_index;
-			u16 set_tx;
-			u8 *seq;
-			u16 seq_len;
-			u8 *key;
-			u16 key_len;
-		} wpa_key;
-
-		struct {
-			u8 ssid_len;
-			u8 ssid[32];
-		} scan_req;
-
-		struct {
-			u16 scan_count;
-			u8 __user *buf;
-		} scan_results;
-
-	} u;
-};
-
-#pragma pack(1)
-struct viawget_scan_result {
-	u8 bssid[6];
-	u8 ssid[32];
-	u16 ssid_len;
-	u8 wpa_ie[WPA_IE_LEN];
-	u16 wpa_ie_len;
-	u8 rsn_ie[WPA_IE_LEN];
-	u16 rsn_ie_len;
-	int freq; // MHz
-	int caps; // e.g. privacy
-	int qual; // signal quality
-	int noise;
-	int level;
-	int maxrate;
-};
-
-#pragma pack()
-
-#endif //__IOWPA_H__
diff --git a/drivers/staging/vt6655/iwctl.c b/drivers/staging/vt6655/iwctl.c
deleted file mode 100644
index 14a62bdae278..000000000000
--- a/drivers/staging/vt6655/iwctl.c
+++ /dev/null
@@ -1,1937 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: iwctl.c
- *
- * Purpose:  wireless ext & ioctl functions
- *
- * Author: Lyndon Chen
- *
- * Date: July 5, 2006
- *
- * Functions:
- *
- * Revision History:
- *
- */
-
-#include "device.h"
-#include "ioctl.h"
-#include "iocmd.h"
-#include "iwctl.h"
-#include "mac.h"
-#include "card.h"
-#include "hostap.h"
-#include "power.h"
-#include "rf.h"
-
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-#include "iowpa.h"
-#include "wpactl.h"
-#endif
-
-#include <net/iw_handler.h>
-extern unsigned short TxRate_iwconfig;//2008-5-8 <add> by chester
-
-/*---------------------  Static Definitions -------------------------*/
-
-//2008-0409-07, <Add> by Einsn Liu
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-#define SUPPORTED_WIRELESS_EXT                  18
-#else
-#define SUPPORTED_WIRELESS_EXT                  17
-#endif
-
-static const long frequency_list[] = {
-	2412, 2417, 2422, 2427, 2432, 2437, 2442, 2447, 2452, 2457, 2462, 2467, 2472, 2484,
-	4915, 4920, 4925, 4935, 4940, 4945, 4960, 4980,
-	5035, 5040, 5045, 5055, 5060, 5080, 5170, 5180, 5190, 5200, 5210, 5220, 5230, 5240,
-	5260, 5280, 5300, 5320, 5500, 5520, 5540, 5560, 5580, 5600, 5620, 5640, 5660, 5680,
-	5700, 5745, 5765, 5785, 5805, 5825
-};
-
-/*---------------------  Static Classes  ----------------------------*/
-/*---------------------  Static Variables  --------------------------*/
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-struct iw_statistics *iwctl_get_wireless_stats(struct net_device *dev)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	long ldBm;
-
-	pDevice->wstats.status = pDevice->op_mode;
-#ifdef Calcu_LinkQual
-	if (pDevice->scStatistic.LinkQuality > 100)
-		pDevice->scStatistic.LinkQuality = 100;
-	pDevice->wstats.qual.qual = (unsigned char)pDevice->scStatistic.LinkQuality;
-#else
-	pDevice->wstats.qual.qual = pDevice->byCurrSQ;
-#endif
-	RFvRSSITodBm(pDevice, (unsigned char)(pDevice->uCurrRSSI), &ldBm);
-	pDevice->wstats.qual.level = ldBm;
-	pDevice->wstats.qual.noise = 0;
-	pDevice->wstats.qual.updated = 1;
-	pDevice->wstats.discard.nwid = 0;
-	pDevice->wstats.discard.code = 0;
-	pDevice->wstats.discard.fragment = 0;
-	pDevice->wstats.discard.retries = (unsigned long)pDevice->scStatistic.dwTsrErr;
-	pDevice->wstats.discard.misc = 0;
-	pDevice->wstats.miss.beacon = 0;
-
-	return &pDevice->wstats;
-}
-
-/*------------------------------------------------------------------*/
-
-static int iwctl_commit(struct net_device *dev,
-			struct iw_request_info *info,
-			void *wrq,
-			char *extra)
-{
-	pr_debug(" SIOCSIWCOMMIT\n");
-
-	return 0;
-}
-/*
- * Wireless Handler : get protocol name
- */
-
-int iwctl_giwname(struct net_device *dev,
-		  struct iw_request_info *info,
-		  char *wrq,
-		  char *extra)
-{
-	strcpy(wrq, "802.11-a/b/g");
-	return 0;
-}
-
-/*
- * Wireless Handler : set scan
- */
-
-static int iwctl_siwscan(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_point *wrq,
-		  char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-	struct iw_scan_req  *req = (struct iw_scan_req *)extra;
-	unsigned char abyScanSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-	PWLAN_IE_SSID pItemSSID = NULL;
-
-	pr_debug(" SIOCSIWSCAN\n");
-
-	if (pDevice->byReAssocCount > 0) {   //reject scan when re-associating!
-//send scan event to wpa_Supplicant
-		union iwreq_data wrqu;
-
-		PRINT_K("wireless_send_event--->SIOCGIWSCAN(scan done)\n");
-		memset(&wrqu, 0, sizeof(wrqu));
-		wireless_send_event(pDevice->dev, SIOCGIWSCAN, &wrqu, NULL);
-		return 0;
-	}
-
-	spin_lock_irq(&pDevice->lock);
-	BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
-
-//mike add: active scan OR passive scan OR desire_ssid scan
-	if (wrq->length == sizeof(struct iw_scan_req)) {
-		if (wrq->flags & IW_SCAN_THIS_ESSID)  {                               //desire_ssid scan
-			memset(abyScanSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-			pItemSSID = (PWLAN_IE_SSID)abyScanSSID;
-			pItemSSID->byElementID = WLAN_EID_SSID;
-			memcpy(pItemSSID->abySSID, req->essid, (int)req->essid_len);
-			if (pItemSSID->abySSID[req->essid_len - 1] == '\0') {
-				if (req->essid_len > 0)
-					pItemSSID->len = req->essid_len - 1;
-			} else
-				pItemSSID->len = req->essid_len;
-			pMgmt->eScanType = WMAC_SCAN_PASSIVE;
-			PRINT_K("SIOCSIWSCAN:[desired_ssid=%s,len=%d]\n", ((PWLAN_IE_SSID)abyScanSSID)->abySSID,
-				((PWLAN_IE_SSID)abyScanSSID)->len);
-			bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, abyScanSSID);
-			spin_unlock_irq(&pDevice->lock);
-
-			return 0;
-		} else if (req->scan_type == IW_SCAN_TYPE_PASSIVE) {          //passive scan
-			pMgmt->eScanType = WMAC_SCAN_PASSIVE;
-		}
-	} else {           //active scan
-		pMgmt->eScanType = WMAC_SCAN_ACTIVE;
-	}
-
-	pMgmt->eScanType = WMAC_SCAN_PASSIVE;
-	bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, NULL);
-	spin_unlock_irq(&pDevice->lock);
-
-	return 0;
-}
-
-/*
- * Wireless Handler : get scan results
- */
-
-static int iwctl_giwscan(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_point *wrq,
-		  char *extra)
-{
-	int ii, jj, kk;
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-	PKnownBSS           pBSS;
-	PWLAN_IE_SSID       pItemSSID;
-	PWLAN_IE_SUPP_RATES pSuppRates, pExtSuppRates;
-	char *current_ev = extra;
-	char *end_buf = extra + IW_SCAN_MAX_DATA;
-	char *current_val = NULL;
-	struct iw_event iwe;
-	long ldBm;
-	char buf[MAX_WPA_IE_LEN * 2 + 30];
-
-	pr_debug(" SIOCGIWSCAN\n");
-
-	if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
-		// In scanning..
-		return -EAGAIN;
-	}
-	pBSS = &(pMgmt->sBSSList[0]);
-	for (ii = 0, jj = 0; jj < MAX_BSS_NUM; jj++) {
-		if (current_ev >= end_buf)
-			break;
-		pBSS = &(pMgmt->sBSSList[jj]);
-		if (pBSS->bActive) {
-			//ADD mac address
-			memset(&iwe, 0, sizeof(iwe));
-			iwe.cmd = SIOCGIWAP;
-			iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
-			memcpy(iwe.u.ap_addr.sa_data, pBSS->abyBSSID, WLAN_BSSID_LEN);
-			current_ev = iwe_stream_add_event(info, current_ev, end_buf, &iwe, IW_EV_ADDR_LEN);
-			//ADD ssid
-			memset(&iwe, 0, sizeof(iwe));
-			iwe.cmd = SIOCGIWESSID;
-			pItemSSID = (PWLAN_IE_SSID)pBSS->abySSID;
-			iwe.u.data.length = pItemSSID->len;
-			iwe.u.data.flags = 1;
-			current_ev = iwe_stream_add_point(info, current_ev, end_buf, &iwe, pItemSSID->abySSID);
-			//ADD mode
-			memset(&iwe, 0, sizeof(iwe));
-			iwe.cmd = SIOCGIWMODE;
-			if (WLAN_GET_CAP_INFO_ESS(pBSS->wCapInfo))
-				iwe.u.mode = IW_MODE_INFRA;
-			else
-				iwe.u.mode = IW_MODE_ADHOC;
-
-			iwe.len = IW_EV_UINT_LEN;
-			current_ev = iwe_stream_add_event(info, current_ev, end_buf, &iwe, IW_EV_UINT_LEN);
-			//ADD frequency
-			pSuppRates = (PWLAN_IE_SUPP_RATES)pBSS->abySuppRates;
-			pExtSuppRates = (PWLAN_IE_SUPP_RATES)pBSS->abyExtSuppRates;
-			memset(&iwe, 0, sizeof(iwe));
-			iwe.cmd = SIOCGIWFREQ;
-			iwe.u.freq.m = pBSS->uChannel;
-			iwe.u.freq.e = 0;
-			iwe.u.freq.i = 0;
-			current_ev = iwe_stream_add_event(info, current_ev, end_buf, &iwe, IW_EV_FREQ_LEN);
-			//2008-0409-04, <Add> by Einsn Liu
-			{
-				int f = (int)pBSS->uChannel - 1;
-
-				if (f < 0)f = 0;
-				iwe.u.freq.m = frequency_list[f] * 100000;
-				iwe.u.freq.e = 1;
-			}
-			current_ev = iwe_stream_add_event(info, current_ev, end_buf, &iwe, IW_EV_FREQ_LEN);
-			//ADD quality
-			memset(&iwe, 0, sizeof(iwe));
-			iwe.cmd = IWEVQUAL;
-			RFvRSSITodBm(pDevice, (unsigned char)(pBSS->uRSSI), &ldBm);
-			iwe.u.qual.level = ldBm;
-			iwe.u.qual.noise = 0;
-//2008-0409-01, <Add> by Einsn Liu
-			if (-ldBm < 50)
-				iwe.u.qual.qual = 100;
-			else if (-ldBm > 90)
-				iwe.u.qual.qual = 0;
-			else
-				iwe.u.qual.qual = (40 - (-ldBm - 50)) * 100 / 40;
-
-			iwe.u.qual.updated = 7;
-
-			current_ev = iwe_stream_add_event(info, current_ev, end_buf, &iwe, IW_EV_QUAL_LEN);
-
-			memset(&iwe, 0, sizeof(iwe));
-			iwe.cmd = SIOCGIWENCODE;
-			iwe.u.data.length = 0;
-			if (WLAN_GET_CAP_INFO_PRIVACY(pBSS->wCapInfo))
-				iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
-			else
-				iwe.u.data.flags = IW_ENCODE_DISABLED;
-
-			current_ev = iwe_stream_add_point(info, current_ev, end_buf, &iwe, pItemSSID->abySSID);
-
-			memset(&iwe, 0, sizeof(iwe));
-			iwe.cmd = SIOCGIWRATE;
-			iwe.u.bitrate.fixed = iwe.u.bitrate.disabled = 0;
-			current_val = current_ev + IW_EV_LCP_LEN;
-
-			for (kk = 0; kk < 12; kk++) {
-				if (pSuppRates->abyRates[kk] == 0)
-					break;
-				// Bit rate given in 500 kb/s units (+ 0x80)
-				iwe.u.bitrate.value = ((pSuppRates->abyRates[kk] & 0x7f) * 500000);
-				current_val = iwe_stream_add_value(info, current_ev, current_val, end_buf, &iwe, IW_EV_PARAM_LEN);
-			}
-			for (kk = 0; kk < 8; kk++) {
-				if (pExtSuppRates->abyRates[kk] == 0)
-					break;
-				// Bit rate given in 500 kb/s units (+ 0x80)
-				iwe.u.bitrate.value = ((pExtSuppRates->abyRates[kk] & 0x7f) * 500000);
-				current_val = iwe_stream_add_value(info, current_ev, current_val, end_buf, &iwe, IW_EV_PARAM_LEN);
-			}
-
-			if ((current_val - current_ev) > IW_EV_LCP_LEN)
-				current_ev = current_val;
-
-			memset(&iwe, 0, sizeof(iwe));
-			iwe.cmd = IWEVCUSTOM;
-			sprintf(buf, "bcn_int=%d", pBSS->wBeaconInterval);
-			iwe.u.data.length = strlen(buf);
-			current_ev = iwe_stream_add_point(info, current_ev, end_buf, &iwe, buf);
-
-			if ((pBSS->wWPALen > 0) && (pBSS->wWPALen <= MAX_WPA_IE_LEN)) {
-				memset(&iwe, 0, sizeof(iwe));
-				iwe.cmd = IWEVGENIE;
-				iwe.u.data.length = pBSS->wWPALen;
-				current_ev = iwe_stream_add_point(info, current_ev, end_buf, &iwe, pBSS->byWPAIE);
-			}
-
-			if ((pBSS->wRSNLen > 0) && (pBSS->wRSNLen <= MAX_WPA_IE_LEN)) {
-				memset(&iwe, 0, sizeof(iwe));
-				iwe.cmd = IWEVGENIE;
-				iwe.u.data.length = pBSS->wRSNLen;
-				current_ev = iwe_stream_add_point(info, current_ev, end_buf, &iwe, pBSS->byRSNIE);
-			}
-
-		}
-	}// for
-
-	wrq->length = current_ev - extra;
-	return 0;
-}
-
-/*
- * Wireless Handler : set frequency or channel
- */
-
-int iwctl_siwfreq(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_freq *wrq,
-		  char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	int rc = 0;
-
-	pr_debug(" SIOCSIWFREQ\n");
-
-	// If setting by frequency, convert to a channel
-	if ((wrq->e == 1) &&
-	    (wrq->m >= (int) 2.412e8) &&
-	    (wrq->m <= (int) 2.487e8)) {
-		int f = wrq->m / 100000;
-		int c = 0;
-
-		while ((c < 14) && (f != frequency_list[c]))
-			c++;
-		wrq->e = 0;
-		wrq->m = c + 1;
-	}
-	// Setting by channel number
-	if ((wrq->m > 14) || (wrq->e > 0))
-		rc = -EOPNOTSUPP;
-	else {
-		int channel = wrq->m;
-
-		if ((channel < 1) || (channel > 14)) {
-			pr_debug("%s: New channel value of %d is invalid!\n",
-				 dev->name, wrq->m);
-			rc = -EINVAL;
-		} else {
-			// Yes ! We can set it !!!
-			pr_debug(" Set to channel = %d\n", channel);
-			pDevice->uChannel = channel;
-			//2007-0207-04,<Add> by EinsnLiu
-			//Make change effect at once
-			pDevice->bCommit = true;
-		}
-	}
-
-	return rc;
-}
-
-/*
- * Wireless Handler : get frequency or channel
- */
-
-int iwctl_giwfreq(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_freq *wrq,
-		  char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-
-	pr_debug(" SIOCGIWFREQ\n");
-
-#ifdef WEXT_USECHANNELS
-	wrq->m = (int)pMgmt->uCurrChannel;
-	wrq->e = 0;
-#else
-	{
-		int f = (int)pMgmt->uCurrChannel - 1;
-
-		if (f < 0)
-			f = 0;
-		wrq->m = frequency_list[f] * 100000;
-		wrq->e = 1;
-	}
-#endif
-
-	return 0;
-}
-
-/*
- * Wireless Handler : set operation mode
- */
-
-int iwctl_siwmode(struct net_device *dev,
-		  struct iw_request_info *info,
-		  __u32 *wmode,
-		  char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-	int rc = 0;
-
-	pr_debug(" SIOCSIWMODE\n");
-
-	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP && pDevice->bEnableHostapd) {
-		pr_debug("Can't set operation mode, hostapd is running\n");
-		return rc;
-	}
-
-	switch (*wmode) {
-	case IW_MODE_ADHOC:
-		if (pMgmt->eConfigMode != WMAC_CONFIG_IBSS_STA) {
-			pMgmt->eConfigMode = WMAC_CONFIG_IBSS_STA;
-			if (pDevice->flags & DEVICE_FLAGS_OPENED)
-				pDevice->bCommit = true;
-
-		}
-		pr_debug("set mode to ad-hoc\n");
-		break;
-	case IW_MODE_AUTO:
-	case IW_MODE_INFRA:
-		if (pMgmt->eConfigMode != WMAC_CONFIG_ESS_STA) {
-			pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
-			if (pDevice->flags & DEVICE_FLAGS_OPENED)
-				pDevice->bCommit = true;
-
-		}
-		pr_debug("set mode to infrastructure\n");
-		break;
-	case IW_MODE_MASTER:
-
-		pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
-		rc = -EOPNOTSUPP;
-		break;
-
-		if (pMgmt->eConfigMode != WMAC_CONFIG_AP) {
-			pMgmt->eConfigMode = WMAC_CONFIG_AP;
-			if (pDevice->flags & DEVICE_FLAGS_OPENED)
-				pDevice->bCommit = true;
-
-		}
-		pr_debug("set mode to Access Point\n");
-		break;
-
-	case IW_MODE_REPEAT:
-		pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
-		rc = -EOPNOTSUPP;
-		break;
-	default:
-		rc = -EINVAL;
-	}
-
-	return rc;
-}
-
-/*
- * Wireless Handler : get operation mode
- */
-
-int iwctl_giwmode(struct net_device *dev,
-		  struct iw_request_info *info,
-		  __u32 *wmode,
-		  char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-
-	pr_debug(" SIOCGIWMODE\n");
-	// If not managed, assume it's ad-hoc
-	switch (pMgmt->eConfigMode) {
-	case WMAC_CONFIG_ESS_STA:
-		*wmode = IW_MODE_INFRA;
-		break;
-	case WMAC_CONFIG_IBSS_STA:
-		*wmode = IW_MODE_ADHOC;
-		break;
-	case WMAC_CONFIG_AUTO:
-		*wmode = IW_MODE_INFRA;
-		break;
-	case WMAC_CONFIG_AP:
-		*wmode = IW_MODE_MASTER;
-		break;
-	default:
-		*wmode = IW_MODE_ADHOC;
-	}
-
-	return 0;
-}
-
-/*
- * Wireless Handler : get capability range
- */
-
-int iwctl_giwrange(struct net_device *dev,
-		   struct iw_request_info *info,
-		   struct iw_point *wrq,
-		   char *extra)
-{
-	struct iw_range *range = (struct iw_range *)extra;
-	int i, k;
-	unsigned char abySupportedRates[13] = {0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C, 0x90};
-
-	pr_debug(" SIOCGIWRANGE\n");
-	if (wrq->pointer) {
-		wrq->length = sizeof(struct iw_range);
-		memset(range, 0, sizeof(struct iw_range));
-		range->min_nwid = 0x0000;
-		range->max_nwid = 0x0000;
-		range->num_channels = 14;
-		// Should be based on cap_rid.country to give only
-		//  what the current card support
-		k = 0;
-		for (i = 0; i < 14; i++) {
-			range->freq[k].i = i + 1; // List index
-			range->freq[k].m = frequency_list[i] * 100000;
-			range->freq[k++].e = 1;	// Values in table in MHz -> * 10^5 * 10
-		}
-		range->num_frequency = k;
-		// Hum... Should put the right values there
-#ifdef Calcu_LinkQual
-		range->max_qual.qual = 100;
-#else
-		range->max_qual.qual = 255;
-#endif
-		range->max_qual.level = 0;
-		range->max_qual.noise = 0;
-		range->sensitivity = 255;
-
-		for (i = 0; i < 13; i++) {
-			range->bitrate[i] = abySupportedRates[i] * 500000;
-			if (range->bitrate[i] == 0)
-				break;
-		}
-		range->num_bitrates = i;
-
-		// Set an indication of the max TCP throughput
-		// in bit/s that we can expect using this interface.
-		//  May be use for QoS stuff... Jean II
-		if (i > 2)
-			range->throughput = 5 * 1000 * 1000;
-		else
-			range->throughput = 1.5 * 1000 * 1000;
-
-		range->min_rts = 0;
-		range->max_rts = 2312;
-		range->min_frag = 256;
-		range->max_frag = 2312;
-
-		// the encoding capabilities
-		range->num_encoding_sizes = 3;
-		// 64(40) bits WEP
-		range->encoding_size[0] = 5;
-		// 128(104) bits WEP
-		range->encoding_size[1] = 13;
-		// 256 bits for WPA-PSK
-		range->encoding_size[2] = 32;
-		// 4 keys are allowed
-		range->max_encoding_tokens = 4;
-
-		range->enc_capa = IW_ENC_CAPA_WPA | IW_ENC_CAPA_WPA2 |
-			IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
-
-		range->min_pmp = 0;
-		range->max_pmp = 1000000;// 1 secs
-		range->min_pmt = 0;
-		range->max_pmt = 1000000;// 1 secs
-		range->pmp_flags = IW_POWER_PERIOD;
-		range->pmt_flags = IW_POWER_TIMEOUT;
-		range->pm_capa = IW_POWER_PERIOD | IW_POWER_TIMEOUT | IW_POWER_ALL_R;
-
-		// Transmit Power - values are in mW
-
-		range->txpower[0] = 100;
-		range->num_txpower = 1;
-		range->txpower_capa = IW_TXPOW_MWATT;
-		range->we_version_source = SUPPORTED_WIRELESS_EXT;
-		range->we_version_compiled = WIRELESS_EXT;
-		range->retry_capa = IW_RETRY_LIMIT | IW_RETRY_LIFETIME;
-		range->retry_flags = IW_RETRY_LIMIT;
-		range->r_time_flags = IW_RETRY_LIFETIME;
-		range->min_retry = 1;
-		range->max_retry = 65535;
-		range->min_r_time = 1024;
-		range->max_r_time = 65535 * 1024;
-		// Experimental measurements - boundary 11/5.5 Mb/s
-		// Note : with or without the (local->rssi), results
-		//  are somewhat different. - Jean II
-		range->avg_qual.qual = 6;
-		range->avg_qual.level = 176;	// -80 dBm
-		range->avg_qual.noise = 0;
-	}
-
-	return 0;
-}
-
-/*
- * Wireless Handler : set ap mac address
- */
-
-int iwctl_siwap(struct net_device *dev,
-		struct iw_request_info *info,
-		struct sockaddr *wrq,
-		char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-	int rc = 0;
-	unsigned char ZeroBSSID[WLAN_BSSID_LEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
-
-	pr_debug(" SIOCSIWAP\n");
-	if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
-		// In scanning..
-		pr_debug("SIOCSIWAP(??)-->In scanning..\n");
-	}
-	if (wrq->sa_family != ARPHRD_ETHER)
-		rc = -EINVAL;
-	else {
-		memcpy(pMgmt->abyDesireBSSID, wrq->sa_data, 6);
-		//2008-0409-05, <Add> by Einsn Liu
-		if ((pDevice->bLinkPass == true) &&
-		    (memcmp(pMgmt->abyDesireBSSID, pMgmt->abyCurrBSSID, 6) == 0)) {
-			return rc;
-		}
-		//mike :add
-		if ((is_broadcast_ether_addr(pMgmt->abyDesireBSSID)) ||
-		    (memcmp(pMgmt->abyDesireBSSID, ZeroBSSID, 6) == 0)) {
-			PRINT_K("SIOCSIWAP:invalid desired BSSID return!\n");
-			return rc;
-		}
-		//mike add: if desired AP is hidden ssid(there are two same BSSID in list),
-		//                  then ignore,because you don't known which one to be connect with??
-		{
-			unsigned int ii, uSameBssidNum = 0;
-
-			for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-				if (pMgmt->sBSSList[ii].bActive &&
-				    ether_addr_equal(pMgmt->sBSSList[ii].abyBSSID,
-						     pMgmt->abyDesireBSSID)) {
-					uSameBssidNum++;
-				}
-			}
-			if (uSameBssidNum >= 2) {  //hit: desired AP is in hidden ssid mode!!!
-				PRINT_K("SIOCSIWAP:ignore for desired AP in hidden mode\n");
-				return rc;
-			}
-		}
-
-		if (pDevice->flags & DEVICE_FLAGS_OPENED)
-			pDevice->bCommit = true;
-
-	}
-	return rc;
-}
-
-/*
- * Wireless Handler : get ap mac address
- */
-
-int iwctl_giwap(struct net_device *dev,
-		struct iw_request_info *info,
-		struct sockaddr *wrq,
-		char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-
-	pr_debug(" SIOCGIWAP\n");
-
-	memcpy(wrq->sa_data, pMgmt->abyCurrBSSID, 6);
-	//2008-0410,<Modify> by Einsn Liu
-	if ((pDevice->bLinkPass == false) && (pMgmt->eCurrMode != WMAC_MODE_ESS_AP))
-		memset(wrq->sa_data, 0, 6);
-
-	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)
-		memcpy(wrq->sa_data, pMgmt->abyCurrBSSID, 6);
-
-	wrq->sa_family = ARPHRD_ETHER;
-
-	return 0;
-}
-
-/*
- * Wireless Handler : get ap list
- */
-
-int iwctl_giwaplist(struct net_device *dev,
-		    struct iw_request_info *info,
-		    struct iw_point *wrq,
-		    char *extra)
-{
-	int ii, jj, rc = 0;
-	struct sockaddr *sock	= NULL;
-	struct sockaddr *s	= NULL;
-	struct iw_quality *qual	= NULL;
-	struct iw_quality *q	= NULL;
-	PKnownBSS pBSS		= NULL;
-
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
-
-	pr_debug(" SIOCGIWAPLIST\n");
-
-	if (!capable(CAP_NET_ADMIN)) {
-		rc = -EPERM;
-		goto exit;
-	}
-
-	if (!wrq->pointer)
-		goto exit;
-
-	sock = kmalloc_array(IW_MAX_AP, sizeof(struct sockaddr), GFP_KERNEL);
-	if (!sock) {
-		rc = -ENOMEM;
-		goto exit;
-	}
-
-	qual = kmalloc_array(IW_MAX_AP, sizeof(struct iw_quality), GFP_KERNEL);
-	if (!qual) {
-		rc = -ENOMEM;
-		goto exit;
-	}
-
-	for (ii = 0, jj = 0; ii < MAX_BSS_NUM; ii++) {
-		pBSS = &(pMgmt->sBSSList[ii]);
-
-		if (!pBSS->bActive)
-			continue;
-		if (jj >= IW_MAX_AP)
-			break;
-
-		s = &sock[jj];
-		q = &qual[jj];
-
-		memcpy(s->sa_data, pBSS->abyBSSID, 6);
-		s->sa_family	= ARPHRD_ETHER;
-		q->level	= pBSS->uRSSI;
-		q->qual		= 0;
-		q->noise	= 0;
-		q->updated	= 2;
-		jj++;
-	}
-
-	wrq->flags = 1; /* Should be define'd */
-	wrq->length = jj;
-	memcpy(extra, sock, sizeof(struct sockaddr) * jj);
-	memcpy(extra + sizeof(struct sockaddr) * jj,
-		qual,
-		sizeof(struct iw_quality) * jj);
-exit:
-	kfree(sock);
-	kfree(qual);
-	return rc;
-}
-
-/*
- * Wireless Handler : set essid
- */
-
-int iwctl_siwessid(struct net_device *dev,
-		   struct iw_request_info *info,
-		   struct iw_point *wrq,
-		   char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-	PWLAN_IE_SSID       pItemSSID;
-	//2008-0409-05, <Add> by Einsn Liu
-	unsigned char len;
-
-	pr_debug(" SIOCSIWESSID\n");
-	pDevice->fWPA_Authened = false;
-	if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
-		// In scanning..
-		pr_debug("SIOCSIWESSID(??)-->In scanning..\n");
-	}
-	// Check if we asked for `any'
-	if (wrq->flags == 0) {
-		// Just send an empty SSID list
-		memset(pMgmt->abyDesireSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-		memset(pMgmt->abyDesireBSSID, 0xFF, 6);
-		PRINT_K("set essid to 'any'\n");
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-		return 0;
-#endif
-	} else {
-		// Set the SSID
-		memset(pMgmt->abyDesireSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-		pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
-		pItemSSID->byElementID = WLAN_EID_SSID;
-
-		memcpy(pItemSSID->abySSID, extra, wrq->length);
-		if (pItemSSID->abySSID[wrq->length - 1] == '\0') {
-			if (wrq->length > 0)
-				pItemSSID->len = wrq->length - 1;
-		} else
-			pItemSSID->len = wrq->length;
-		pr_debug("set essid to %s\n", pItemSSID->abySSID);
-		//2008-0409-05, <Add> by Einsn Liu
-		len = (pItemSSID->len > ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len) ? pItemSSID->len : ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len;
-		if ((pDevice->bLinkPass == true) &&
-		    (memcmp(pItemSSID->abySSID, ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->abySSID, len) == 0))
-			return 0;
-
-		//mike:need clear desiredBSSID
-		if (pItemSSID->len == 0) {
-			memset(pMgmt->abyDesireBSSID, 0xFF, 6);
-			return 0;
-		}
-
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-		//Wext wil order another command of siwap to link with desired AP,
-		//so here need not associate??
-		if (pDevice->bWPASuppWextEnabled == true)  {
-			/*******search if  in hidden ssid mode ****/
-			{
-				PKnownBSS       pCurr = NULL;
-				unsigned char abyTmpDesireSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-				unsigned int ii, uSameBssidNum = 0;
-
-				memcpy(abyTmpDesireSSID, pMgmt->abyDesireSSID, sizeof(abyTmpDesireSSID));
-				pCurr = BSSpSearchBSSList(pDevice,
-							  NULL,
-							  abyTmpDesireSSID,
-							  pMgmt->eConfigPHYMode
-);
-
-				if (pCurr == NULL) {
-					PRINT_K("SIOCSIWESSID:hidden ssid site survey before associate.......\n");
-					vResetCommandTimer((void *)pDevice);
-					pMgmt->eScanType = WMAC_SCAN_ACTIVE;
-					bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, pMgmt->abyDesireSSID);
-					bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, pMgmt->abyDesireSSID);
-				} else {  //mike:to find out if that desired SSID is a hidden-ssid AP ,
-					//         by means of judging if there are two same BSSID exist in list ?
-					for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-						if (pMgmt->sBSSList[ii].bActive &&
-						    ether_addr_equal(pMgmt->sBSSList[ii].abyBSSID,
-								     pCurr->abyBSSID)) {
-							uSameBssidNum++;
-						}
-					}
-					if (uSameBssidNum >= 2) {  //hit: desired AP is in hidden ssid mode!!!
-						pr_debug("SIOCSIWESSID:hidden ssid directly associate.......\n");
-						vResetCommandTimer((void *)pDevice);
-						pMgmt->eScanType = WMAC_SCAN_PASSIVE;          //this scan type,you'll submit scan result!
-						bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, pMgmt->abyDesireSSID);
-						bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, pMgmt->abyDesireSSID);
-					}
-				}
-			}
-			return 0;
-		}
-#endif
-
-		pr_debug("set essid = %s\n", pItemSSID->abySSID);
-	}
-
-	if (pDevice->flags & DEVICE_FLAGS_OPENED)
-		pDevice->bCommit = true;
-
-	return 0;
-}
-
-/*
- * Wireless Handler : get essid
- */
-
-int iwctl_giwessid(struct net_device *dev,
-		   struct iw_request_info *info,
-		   struct iw_point *wrq,
-		   char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-	PWLAN_IE_SSID       pItemSSID;
-
-	pr_debug(" SIOCGIWESSID\n");
-
-	// Note : if wrq->u.data.flags != 0, we should
-	// get the relevant SSID from the SSID list...
-
-	// Get the current SSID
-	pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
-	memcpy(extra, pItemSSID->abySSID , pItemSSID->len);
-	extra[pItemSSID->len] = '\0';
-	wrq->length = pItemSSID->len + 1;
-	//2008-0409-03, <Add> by Einsn Liu
-	wrq->length = pItemSSID->len;
-	wrq->flags = 1; // active
-
-	return 0;
-}
-
-/*
- * Wireless Handler : set data rate
- */
-
-int iwctl_siwrate(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_param *wrq,
-		  char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	int rc = 0;
-	u8	brate = 0;
-	int	i;
-	unsigned char abySupportedRates[13] = {0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C, 0x90};
-
-	pr_debug(" SIOCSIWRATE\n");
-	if (!(pDevice->flags & DEVICE_FLAGS_OPENED)) {
-		rc = -EINVAL;
-		return rc;
-	}
-
-	// First : get a valid bit rate value
-
-	// Which type of value
-	if ((wrq->value < 13) &&
-	    (wrq->value >= 0)) {
-		// Setting by rate index
-		// Find value in the magic rate table
-		brate = wrq->value;
-	} else {
-		// Setting by frequency value
-		u8	normvalue = (u8) (wrq->value/500000);
-
-		// Check if rate is valid
-		for (i = 0; i < 13; i++) {
-			if (normvalue == abySupportedRates[i]) {
-				brate = i;
-				break;
-			}
-		}
-	}
-	// -1 designed the max rate (mostly auto mode)
-	if (wrq->value == -1) {
-		// Get the highest available rate
-		for (i = 0; i < 13; i++) {
-			if (abySupportedRates[i] == 0)
-				break;
-		}
-		if (i != 0)
-			brate = i - 1;
-
-	}
-	// Check that it is valid
-	// brate is index of abySupportedRates[]
-	if (brate > 13) {
-		rc = -EINVAL;
-		return rc;
-	}
-
-	// Now, check if we want a fixed or auto value
-	if (wrq->fixed != 0) {
-		// Fixed mode
-		// One rate, fixed
-		pr_debug("Rate Fix\n");
-		pDevice->bFixRate = true;
-		if ((pDevice->byBBType == BB_TYPE_11B) && (brate > 3)) {
-			pDevice->uConnectionRate = 3;
-		} else {
-			pDevice->uConnectionRate = brate;
-			pr_debug("Fixed to Rate %d\n",
-				 pDevice->uConnectionRate);
-		}
-
-	} else {
-		pDevice->bFixRate = false;
-		pDevice->uConnectionRate = 13;
-		pr_debug("auto rate:connection_rate is 13\n");
-	}
-
-	return rc;
-}
-
-/*
- * Wireless Handler : get data rate
- */
-
-int iwctl_giwrate(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_param *wrq,
-		  char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-//2007-0118-05,<Mark> by EinsnLiu
-//Mark the unnecessary sentences.
-//    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-
-	pr_debug(" SIOCGIWRATE\n");
-	{
-		unsigned char abySupportedRates[13] = {0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C, 0x90};
-		int brate = 0;
-//2008-5-8 <modify> by chester
-		if (pDevice->bLinkPass) {
-			if (pDevice->bFixRate == true) {
-				if (pDevice->uConnectionRate < 13) {
-					brate = abySupportedRates[pDevice->uConnectionRate];
-				} else {
-					if (pDevice->byBBType == BB_TYPE_11B)
-						brate = 0x16;
-					if (pDevice->byBBType == BB_TYPE_11G)
-						brate = 0x6C;
-					if (pDevice->byBBType == BB_TYPE_11A)
-						brate = 0x6C;
-				}
-			} else {
-				brate = abySupportedRates[TxRate_iwconfig];
-			}
-		} else brate = 0;
-
-		wrq->value = brate * 500000;
-		// If more than one rate, set auto
-		if (pDevice->bFixRate == true)
-			wrq->fixed = true;
-	}
-
-	return 0;
-}
-
-/*
- * Wireless Handler : set rts threshold
- */
-
-int iwctl_siwrts(struct net_device *dev,
-		 struct iw_request_info *info,
-		 struct iw_param *wrq,
-		 char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	int rc = 0;
-
-	pr_debug(" SIOCSIWRTS\n");
-
-	{
-		int rthr = wrq->value;
-
-		if (wrq->disabled)
-			rthr = 2312;
-
-		if ((rthr < 0) || (rthr > 2312))
-			rc = -EINVAL;
-		else
-			pDevice->wRTSThreshold = rthr;
-	}
-
-	return 0;
-}
-
-/*
- * Wireless Handler : get rts
- */
-
-int iwctl_giwrts(struct net_device *dev,
-		 struct iw_request_info *info,
-		 struct iw_param *wrq,
-		 char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-
-	pr_debug(" SIOCGIWRTS\n");
-	wrq->value = pDevice->wRTSThreshold;
-	wrq->disabled = (wrq->value >= 2312);
-	wrq->fixed = 1;
-
-	return 0;
-}
-
-/*
- * Wireless Handler : set fragment threshold
- */
-
-int iwctl_siwfrag(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_param *wrq,
-		  char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	int rc = 0;
-	int fthr = wrq->value;
-
-	pr_debug(" SIOCSIWFRAG\n");
-
-	if (wrq->disabled)
-		fthr = 2312;
-	if ((fthr < 256) || (fthr > 2312)) {
-		rc = -EINVAL;
-	} else {
-		fthr &= ~0x1;	// Get an even value
-		pDevice->wFragmentationThreshold = (u16)fthr;
-	}
-
-	return rc;
-}
-
-/*
- * Wireless Handler : get fragment threshold
- */
-
-int iwctl_giwfrag(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_param *wrq,
-		  char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-
-	pr_debug(" SIOCGIWFRAG\n");
-	wrq->value = pDevice->wFragmentationThreshold;
-	wrq->disabled = (wrq->value >= 2312);
-	wrq->fixed = 1;
-
-	return 0;
-}
-
-/*
- * Wireless Handler : set retry threshold
- */
-int iwctl_siwretry(struct net_device *dev,
-		   struct iw_request_info *info,
-		   struct iw_param *wrq,
-		   char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	int rc = 0;
-
-	pr_debug(" SIOCSIWRETRY\n");
-
-	if (wrq->disabled) {
-		rc = -EINVAL;
-		return rc;
-	}
-
-	if (wrq->flags & IW_RETRY_LIMIT) {
-		if (wrq->flags & IW_RETRY_MAX)
-			pDevice->byLongRetryLimit = wrq->value;
-		else if (wrq->flags & IW_RETRY_MIN)
-			pDevice->byShortRetryLimit = wrq->value;
-		else {
-			// No modifier : set both
-			pDevice->byShortRetryLimit = wrq->value;
-			pDevice->byLongRetryLimit = wrq->value;
-		}
-	}
-	if (wrq->flags & IW_RETRY_LIFETIME)
-		pDevice->wMaxTransmitMSDULifetime = wrq->value;
-
-	return rc;
-}
-
-/*
- * Wireless Handler : get retry threshold
- */
-int iwctl_giwretry(struct net_device *dev,
-		   struct iw_request_info *info,
-		   struct iw_param *wrq,
-		   char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-
-	pr_debug(" SIOCGIWRETRY\n");
-	wrq->disabled = 0;      // Can't be disabled
-
-	// Note : by default, display the min retry number
-	if ((wrq->flags & IW_RETRY_TYPE) == IW_RETRY_LIFETIME) {
-		wrq->flags = IW_RETRY_LIFETIME;
-		wrq->value = (int)pDevice->wMaxTransmitMSDULifetime; //ms
-	} else if ((wrq->flags & IW_RETRY_MAX)) {
-		wrq->flags = IW_RETRY_LIMIT | IW_RETRY_MAX;
-		wrq->value = (int)pDevice->byLongRetryLimit;
-	} else {
-		wrq->flags = IW_RETRY_LIMIT;
-		wrq->value = (int)pDevice->byShortRetryLimit;
-		if ((int)pDevice->byShortRetryLimit != (int)pDevice->byLongRetryLimit)
-			wrq->flags |= IW_RETRY_MIN;
-	}
-
-	return 0;
-}
-
-/*
- * Wireless Handler : set encode mode
- */
-int iwctl_siwencode(struct net_device *dev,
-		    struct iw_request_info *info,
-		    struct iw_point *wrq,
-		    char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-	unsigned long dwKeyIndex = (unsigned long)(wrq->flags & IW_ENCODE_INDEX);
-	int ii, uu, rc = 0;
-	int index = (wrq->flags & IW_ENCODE_INDEX);
-
-//2007-0207-07,<Modify> by EinsnLiu
-//There are some problems when using iwconfig encode/key command to set the WEP key.
-//I almost rewrite this function.
-//now it support:(assume the wireless interface's name is eth0)
-//iwconfig eth0 key [1] 1122334455 open  /*set key stirng to index 1,and driver using key index is set to 1*/
-//iwconfig eth0 key [3]    /*set driver using  key index to 3,the key string no change */
-//iwconfig eth0 key 1122334455  /*set key string to driver using index*/
-//iwconfig eth0 key restricted  /*enable share key*/
-
-	PSKeyTable pkeytab;
-
-	pr_debug(" SIOCSIWENCODE\n");
-
-	if ((wrq->flags & IW_ENCODE_DISABLED) == 0) {
-		//Not disable encryption
-
-		if (dwKeyIndex > WLAN_WEP_NKEYS) {
-			rc = -EINVAL;
-			return rc;
-		}
-
-		if (dwKeyIndex < 1 && ((wrq->flags & IW_ENCODE_NOKEY) == 0)) {//set default key
-			if (pDevice->byKeyIndex < WLAN_WEP_NKEYS)
-				dwKeyIndex = pDevice->byKeyIndex;
-			else
-				dwKeyIndex = 0;
-		} else {
-			dwKeyIndex--;
-		}
-
-		// Check the size of the key
-		if (wrq->length > WLAN_WEP232_KEYLEN) {
-			rc = -EINVAL;
-			return rc;
-		}
-
-		if (wrq->length > 0) {//have key
-
-			if (wrq->length ==  WLAN_WEP232_KEYLEN) {
-				pr_debug("Set 232 bit wep key\n");
-			} else if (wrq->length ==  WLAN_WEP104_KEYLEN) {
-				pr_debug("Set 104 bit wep key\n");
-			} else if (wrq->length == WLAN_WEP40_KEYLEN) {
-				pr_debug("Set 40 bit wep key, index= %d\n",
-					 (int)dwKeyIndex);
-			} else {//no support length
-				rc = -EINVAL;
-				return rc;
-			}
-			memset(pDevice->abyKey, 0, WLAN_WEP232_KEYLEN);
-			memcpy(pDevice->abyKey, extra, wrq->length);
-
-			pr_debug("abyKey: ");
-			for (ii = 0; ii < wrq->length; ii++)
-				pr_debug("%02x ", pDevice->abyKey[ii]);
-
-			if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-				spin_lock_irq(&pDevice->lock);
-				KeybSetDefaultKey(&(pDevice->sKey),
-						  (unsigned long)(dwKeyIndex | (1 << 31)),
-						  wrq->length,
-						  NULL,
-						  pDevice->abyKey,
-						  KEY_CTL_WEP,
-						  pDevice->PortOffset,
-						  pDevice->byLocalID
-);
-				spin_unlock_irq(&pDevice->lock);
-			}
-			pDevice->byKeyIndex = (unsigned char)dwKeyIndex;
-			pDevice->uKeyLength = wrq->length;
-			pDevice->bTransmitKey = true;
-			pDevice->bEncryptionEnable = true;
-			pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-
-		} else if (index > 0) {
-			//when the length is 0 the request only changes the default transmit key index
-			//check the new key if it has a non zero length
-			if (pDevice->bEncryptionEnable == false) {
-				rc = -EINVAL;
-				return rc;
-			}
-			pr_debug("Just set Default key Index:\n");
-			pkeytab = &(pDevice->sKey.KeyTable[MAX_KEY_TABLE - 1]);
-			if (pkeytab->GroupKey[(unsigned char)dwKeyIndex].uKeyLength == 0) {
-				pr_debug("Default key len is 0\n");
-				rc = -EINVAL;
-				return rc;
-			}
-			pDevice->byKeyIndex = (unsigned char)dwKeyIndex;
-			pkeytab->dwGTKeyIndex = dwKeyIndex | (1 << 31);
-			pkeytab->GroupKey[(unsigned char)dwKeyIndex].dwKeyIndex = dwKeyIndex | (1 << 31);
-		}
-
-	} else {//disable the key
-		pr_debug("Disable WEP function\n");
-		if (pDevice->bEncryptionEnable == false)
-			return 0;
-		pMgmt->bShareKeyAlgorithm = false;
-		pDevice->bEncryptionEnable = false;
-		pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
-		if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-			spin_lock_irq(&pDevice->lock);
-			for (uu = 0; uu < MAX_KEY_TABLE; uu++)
-				MACvDisableKeyEntry(pDevice->PortOffset, uu);
-			spin_unlock_irq(&pDevice->lock);
-		}
-	}
-//End Modify,Einsn
-
-	if (wrq->flags & IW_ENCODE_RESTRICTED) {
-		pr_debug("Enable WEP & ShareKey System\n");
-		pMgmt->bShareKeyAlgorithm = true;
-	}
-	if (wrq->flags & IW_ENCODE_OPEN) {
-		pr_debug("Enable WEP & Open System\n");
-		pMgmt->bShareKeyAlgorithm = false;
-	}
-	return rc;
-}
-
-int iwctl_giwencode(struct net_device *dev,
-		    struct iw_request_info *info,
-		    struct iw_point *wrq,
-		    char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject		pMgmt = &(pDevice->sMgmtObj);
-	char abyKey[WLAN_WEP232_KEYLEN];
-
-	unsigned int index = (unsigned int)(wrq->flags & IW_ENCODE_INDEX);
-	PSKeyItem	pKey = NULL;
-
-	pr_debug(" SIOCGIWENCODE\n");
-
-	if (index > WLAN_WEP_NKEYS)
-		return	-EINVAL;
-
-	if (index < 1) {//get default key
-		if (pDevice->byKeyIndex < WLAN_WEP_NKEYS)
-			index = pDevice->byKeyIndex;
-		else
-			index = 0;
-	} else {
-		index--;
-	}
-
-	memset(abyKey, 0, WLAN_WEP232_KEYLEN);
-	// Check encryption mode
-	wrq->flags = IW_ENCODE_NOKEY;
-	// Is WEP enabled ???
-	if (pDevice->bEncryptionEnable)
-		wrq->flags |=  IW_ENCODE_ENABLED;
-	else
-		wrq->flags |=  IW_ENCODE_DISABLED;
-
-	if (pMgmt->bShareKeyAlgorithm)
-		wrq->flags |=  IW_ENCODE_RESTRICTED;
-	else
-		wrq->flags |=  IW_ENCODE_OPEN;
-	wrq->length = 0;
-
-	if ((index == 0) && (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled ||
-			     pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled)) {//get wpa pairwise  key
-		if (KeybGetKey(&(pDevice->sKey), pMgmt->abyCurrBSSID, 0xffffffff, &pKey)) {
-			wrq->length = pKey->uKeyLength;
-			memcpy(abyKey, pKey->abyKey,	pKey->uKeyLength);
-			memcpy(extra,  abyKey, WLAN_WEP232_KEYLEN);
-		}
-	} else if (KeybGetKey(&(pDevice->sKey), pDevice->abyBroadcastAddr, (unsigned char)index , &pKey)) {
-		wrq->length = pKey->uKeyLength;
-		memcpy(abyKey, pKey->abyKey,  pKey->uKeyLength);
-		memcpy(extra,  abyKey, WLAN_WEP232_KEYLEN);
-	}
-
-	wrq->flags |= index+1;
-
-	return 0;
-}
-
-/*
- * Wireless Handler : set power mode
- */
-int iwctl_siwpower(struct net_device *dev,
-		   struct iw_request_info *info,
-		   struct iw_param *wrq,
-		   char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-	int rc = 0;
-
-	pr_debug(" SIOCSIWPOWER\n");
-
-	if (!(pDevice->flags & DEVICE_FLAGS_OPENED)) {
-		rc = -EINVAL;
-		return rc;
-	}
-
-	if (wrq->disabled) {
-		pDevice->ePSMode = WMAC_POWER_CAM;
-		PSvDisablePowerSaving(pDevice);
-		return rc;
-	}
-	if ((wrq->flags & IW_POWER_TYPE) == IW_POWER_TIMEOUT) {
-		pDevice->ePSMode = WMAC_POWER_FAST;
-		PSvEnablePowerSaving((void *)pDevice, pMgmt->wListenInterval);
-
-	} else if ((wrq->flags & IW_POWER_TYPE) == IW_POWER_PERIOD) {
-		pDevice->ePSMode = WMAC_POWER_FAST;
-		PSvEnablePowerSaving((void *)pDevice, pMgmt->wListenInterval);
-	}
-	switch (wrq->flags & IW_POWER_MODE) {
-	case IW_POWER_UNICAST_R:
-		pr_debug(" SIOCSIWPOWER: IW_POWER_UNICAST_R\n");
-		rc = -EINVAL;
-		break;
-	case IW_POWER_ALL_R:
-		pr_debug(" SIOCSIWPOWER: IW_POWER_ALL_R\n");
-		rc = -EINVAL;
-	case IW_POWER_ON:
-		pr_debug(" SIOCSIWPOWER: IW_POWER_ON\n");
-		break;
-	default:
-		rc = -EINVAL;
-	}
-
-	return rc;
-}
-
-/*
- * Wireless Handler : get power mode
- */
-int iwctl_giwpower(struct net_device *dev,
-		   struct iw_request_info *info,
-		   struct iw_param *wrq,
-		   char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-	int mode = pDevice->ePSMode;
-
-	pr_debug(" SIOCGIWPOWER\n");
-
-	wrq->disabled = (mode == WMAC_POWER_CAM);
-	if (wrq->disabled)
-		return 0;
-
-	if ((wrq->flags & IW_POWER_TYPE) == IW_POWER_TIMEOUT) {
-		wrq->value = (int)((pMgmt->wListenInterval * pMgmt->wCurrBeaconPeriod) << 10);
-		wrq->flags = IW_POWER_TIMEOUT;
-	} else {
-		wrq->value = (int)((pMgmt->wListenInterval * pMgmt->wCurrBeaconPeriod) << 10);
-		wrq->flags = IW_POWER_PERIOD;
-	}
-	wrq->flags |= IW_POWER_ALL_R;
-
-	return 0;
-}
-
-/*
- * Wireless Handler : get Sensitivity
- */
-int iwctl_giwsens(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_param *wrq,
-		  char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	long ldBm;
-
-	pr_debug(" SIOCGIWSENS\n");
-	if (pDevice->bLinkPass == true) {
-		RFvRSSITodBm(pDevice, (unsigned char)(pDevice->uCurrRSSI), &ldBm);
-		wrq->value = ldBm;
-	} else {
-		wrq->value = 0;
-	}
-	wrq->disabled = (wrq->value == 0);
-	wrq->fixed = 1;
-
-	return 0;
-}
-
-//2008-0409-07, <Add> by Einsn Liu
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-
-int iwctl_siwauth(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_param *wrq,
-		  char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
-	int ret = 0;
-	static int wpa_version = 0;  //must be static to save the last value,einsn liu
-	static int pairwise = 0;
-
-	pr_debug(" SIOCSIWAUTH\n");
-	switch (wrq->flags & IW_AUTH_INDEX) {
-	case IW_AUTH_WPA_VERSION:
-		wpa_version = wrq->value;
-		if (wrq->value == IW_AUTH_WPA_VERSION_DISABLED)
-			PRINT_K("iwctl_siwauth:set WPADEV to disable at 1??????\n");
-		else if (wrq->value == IW_AUTH_WPA_VERSION_WPA)
-			PRINT_K("iwctl_siwauth:set WPADEV to WPA1******\n");
-		else
-			PRINT_K("iwctl_siwauth:set WPADEV to WPA2******\n");
-
-		break;
-	case IW_AUTH_CIPHER_PAIRWISE:
-		pairwise = wrq->value;
-		if (pairwise == IW_AUTH_CIPHER_CCMP)
-			pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
-		else if (pairwise == IW_AUTH_CIPHER_TKIP)
-			pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
-		else if (pairwise == IW_AUTH_CIPHER_WEP40 || pairwise == IW_AUTH_CIPHER_WEP104)
-			pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-		else if (pairwise == IW_AUTH_CIPHER_NONE)
-			; /* do nothing,einsn liu */
-		else
-			pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
-
-		break;
-	case IW_AUTH_CIPHER_GROUP:
-		if (wpa_version == IW_AUTH_WPA_VERSION_DISABLED)
-			break;
-		if (pairwise == IW_AUTH_CIPHER_NONE) {
-			if (wrq->value == IW_AUTH_CIPHER_CCMP)
-				pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
-			else
-				pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
-		}
-		break;
-	case IW_AUTH_KEY_MGMT:
-
-		if (wpa_version == IW_AUTH_WPA_VERSION_WPA2) {
-			if (wrq->value == IW_AUTH_KEY_MGMT_PSK)
-				pMgmt->eAuthenMode = WMAC_AUTH_WPA2PSK;
-			else
-				pMgmt->eAuthenMode = WMAC_AUTH_WPA2;
-		} else if (wpa_version == IW_AUTH_WPA_VERSION_WPA) {
-			if (wrq->value == 0)
-				pMgmt->eAuthenMode = WMAC_AUTH_WPANONE;
-			else if (wrq->value == IW_AUTH_KEY_MGMT_PSK)
-				pMgmt->eAuthenMode = WMAC_AUTH_WPAPSK;
-			else
-				pMgmt->eAuthenMode = WMAC_AUTH_WPA;
-		}
-
-		break;
-	case IW_AUTH_TKIP_COUNTERMEASURES:
-		break;		/* FIXME */
-	case IW_AUTH_DROP_UNENCRYPTED:
-		break;
-	case IW_AUTH_80211_AUTH_ALG:
-		if (wrq->value == IW_AUTH_ALG_OPEN_SYSTEM)
-			pMgmt->bShareKeyAlgorithm = false;
-		else if (wrq->value == IW_AUTH_ALG_SHARED_KEY)
-			pMgmt->bShareKeyAlgorithm = true;
-
-		break;
-	case IW_AUTH_WPA_ENABLED:
-		break;
-	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
-		break;
-	case IW_AUTH_ROAMING_CONTROL:
-		ret = -EOPNOTSUPP;
-		break;
-	case IW_AUTH_PRIVACY_INVOKED:
-		pDevice->bEncryptionEnable = !!wrq->value;
-		if (pDevice->bEncryptionEnable == false) {
-			wpa_version = 0;
-			pairwise = 0;
-			pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
-			pMgmt->bShareKeyAlgorithm = false;
-			pMgmt->eAuthenMode = false;
-		}
-
-		break;
-	default:
-		ret = -EOPNOTSUPP;
-		break;
-	}
-
-	return ret;
-}
-
-int iwctl_giwauth(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_param *wrq,
-		  char *extra)
-{
-	return -EOPNOTSUPP;
-}
-
-int iwctl_siwgenie(struct net_device *dev,
-		   struct iw_request_info *info,
-		   struct iw_point *wrq,
-		   char __user *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
-	int ret = 0;
-	char length;
-
-	if (wrq->length) {
-		if (wrq->length < 2)
-			return -EINVAL;
-
-		ret = get_user(length, extra + 1);
-		if (ret)
-			return ret;
-
-		if (length + 2 != wrq->length)
-			return -EINVAL;
-
-		if (wrq->length > MAX_WPA_IE_LEN) {
-			ret = -ENOMEM;
-			goto out;
-		}
-		memset(pMgmt->abyWPAIE, 0, MAX_WPA_IE_LEN);
-		if (copy_from_user(pMgmt->abyWPAIE, extra, wrq->length)) {
-			ret = -EFAULT;
-			goto out;
-		}
-		pMgmt->wWPAIELen = wrq->length;
-	} else {
-		memset(pMgmt->abyWPAIE, 0, MAX_WPA_IE_LEN);
-		pMgmt->wWPAIELen = 0;
-	}
-
-out://not completely ...not necessary in wpa_supplicant 0.5.8
-	return ret;
-}
-
-int iwctl_giwgenie(struct net_device *dev,
-		   struct iw_request_info *info,
-		   struct iw_point *wrq,
-		   char __user *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
-	int ret = 0;
-	int space = wrq->length;
-
-	wrq->length = 0;
-	if (pMgmt->wWPAIELen > 0) {
-		wrq->length = pMgmt->wWPAIELen;
-		if (pMgmt->wWPAIELen <= space) {
-			if (copy_to_user(extra, pMgmt->abyWPAIE, pMgmt->wWPAIELen))
-				ret = -EFAULT;
-
-		} else {
-			ret = -E2BIG;
-		}
-	}
-
-	return ret;
-}
-
-int iwctl_siwencodeext(struct net_device *dev,
-		       struct iw_request_info *info,
-		       struct iw_point *wrq,
-		       char *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
-	struct viawget_wpa_param *param = NULL;
-//original member
-	enum wpa_alg alg_name;
-	u8  addr[6];
-	int key_idx, set_tx = 0;
-	u8  seq[IW_ENCODE_SEQ_MAX_SIZE];
-	u8 key[64];
-	size_t seq_len = 0, key_len = 0;
-
-	u8 key_array[64];
-	int ret = 0;
-
-	PRINT_K("SIOCSIWENCODEEXT......\n");
-
-	param = kzalloc(sizeof(*param), GFP_KERNEL);
-	if (param == NULL)
-		return -ENOMEM;
-
-//recover alg_name
-	switch (ext->alg) {
-	case IW_ENCODE_ALG_NONE:
-		alg_name = WPA_ALG_NONE;
-		break;
-	case IW_ENCODE_ALG_WEP:
-		alg_name = WPA_ALG_WEP;
-		break;
-	case IW_ENCODE_ALG_TKIP:
-		alg_name = WPA_ALG_TKIP;
-		break;
-	case IW_ENCODE_ALG_CCMP:
-		alg_name = WPA_ALG_CCMP;
-		break;
-	default:
-		PRINT_K("Unknown alg = %d\n", ext->alg);
-		ret = -ENOMEM;
-		goto error;
-	}
-//recover addr
-	memcpy(addr, ext->addr.sa_data, ETH_ALEN);
-//recover key_idx
-	key_idx = (wrq->flags&IW_ENCODE_INDEX) - 1;
-//recover set_tx
-	if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
-		set_tx = 1;
-//recover seq,seq_len
-	if (ext->ext_flags & IW_ENCODE_EXT_RX_SEQ_VALID) {
-		seq_len = IW_ENCODE_SEQ_MAX_SIZE;
-		memcpy(seq, ext->rx_seq, seq_len);
-	}
-//recover key,key_len
-	if (ext->key_len) {
-		key_len = ext->key_len;
-		memcpy(key, &ext->key[0], key_len);
-	}
-
-	memset(key_array, 0, 64);
-	if (key_len > 0) {
-		memcpy(key_array, key, key_len);
-		if (key_len == 32) {
-			// notice ! the oder
-			memcpy(&key_array[16], &key[24], 8);
-			memcpy(&key_array[24], &key[16], 8);
-		}
-	}
-
-/**************Translate iw_encode_ext to viawget_wpa_param****************/
-	memcpy(param->addr, addr, ETH_ALEN);
-	param->u.wpa_key.alg_name = (int)alg_name;
-	param->u.wpa_key.set_tx = set_tx;
-	param->u.wpa_key.key_index = key_idx;
-	param->u.wpa_key.key_len = key_len;
-	param->u.wpa_key.key = (u8 *)key_array;
-	param->u.wpa_key.seq = (u8 *)seq;
-	param->u.wpa_key.seq_len = seq_len;
-
-//****set if current action is Network Manager count??
-//****this method is so foolish,but there is no other way???
-	if (param->u.wpa_key.alg_name == WPA_ALG_NONE) {
-		if (param->u.wpa_key.key_index == 0)
-			pDevice->bwextcount++;
-
-		if ((pDevice->bwextcount == 1) && (param->u.wpa_key.key_index == 1))
-			pDevice->bwextcount++;
-
-		if ((pDevice->bwextcount == 2) && (param->u.wpa_key.key_index == 2))
-			pDevice->bwextcount++;
-
-		if ((pDevice->bwextcount == 3) && (param->u.wpa_key.key_index == 3))
-			pDevice->bwextcount++;
-
-	}
-	if (pDevice->bwextcount == 4) {
-		pr_debug("SIOCSIWENCODEEXT:Enable WPA WEXT SUPPORT!!!!!\n");
-		pDevice->bwextcount = 0;
-		pDevice->bWPASuppWextEnabled = true;
-	}
-//******
-
-	spin_lock_irq(&pDevice->lock);
-	ret = wpa_set_keys(pDevice, param, true);
-	spin_unlock_irq(&pDevice->lock);
-
-error:
-	kfree(param);
-	return ret;
-}
-
-int iwctl_giwencodeext(struct net_device *dev,
-		       struct iw_request_info *info,
-		       struct iw_point *wrq,
-		       char *extra)
-{
-	return -EOPNOTSUPP;
-}
-
-int iwctl_siwmlme(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_point *wrq,
-		  char __user *extra)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
-	struct iw_mlme mime;
-
-	int ret = 0;
-
-	ret = copy_from_user(&mime, extra, sizeof(mime));
-	if (ret)
-		return -EFAULT;
-
-	if (memcmp(pMgmt->abyCurrBSSID, mime.addr.sa_data, ETH_ALEN)) {
-		ret = -EINVAL;
-		return ret;
-	}
-	switch (mime.cmd) {
-	case IW_MLME_DEAUTH:
-		//this command seems to be not complete,please test it --einsnliu
-		//bScheduleCommand((void *) pDevice, WLAN_CMD_DEAUTH, (unsigned char *)&reason);
-		break;
-	case IW_MLME_DISASSOC:
-		if (pDevice->bLinkPass == true) {
-			pr_debug("iwctl_siwmlme--->send DISASSOCIATE\n");
-			//clear related flags
-			memset(pMgmt->abyDesireBSSID, 0xFF, 6);
-			KeyvInitTable(&pDevice->sKey, pDevice->PortOffset);
-			bScheduleCommand((void *)pDevice, WLAN_CMD_DISASSOCIATE, NULL);
-		}
-		break;
-	default:
-		ret = -EOPNOTSUPP;
-	}
-
-	return ret;
-}
-
-#endif
-
-/*------------------------------------------------------------------*/
-/*
- * Structures to export the Wireless Handlers
- */
-
-static const iw_handler		iwctl_handler[] =
-{
-	(iw_handler) iwctl_commit,      // SIOCSIWCOMMIT
-	(iw_handler) NULL,		// SIOCGIWNAME
-	(iw_handler) NULL,		// SIOCSIWNWID
-	(iw_handler) NULL,		// SIOCGIWNWID
-	(iw_handler) NULL,		// SIOCSIWFREQ
-	(iw_handler) NULL,		// SIOCGIWFREQ
-	(iw_handler) NULL,		// SIOCSIWMODE
-	(iw_handler) NULL,		// SIOCGIWMODE
-	(iw_handler) NULL,		// SIOCSIWSENS
-	(iw_handler) NULL,		// SIOCGIWSENS
-	(iw_handler) NULL,		// SIOCSIWRANGE
-	(iw_handler) iwctl_giwrange,	// SIOCGIWRANGE
-	(iw_handler) NULL,		// SIOCSIWPRIV
-	(iw_handler) NULL,		// SIOCGIWPRIV
-	(iw_handler) NULL,		// SIOCSIWSTATS
-	(iw_handler) NULL,		// SIOCGIWSTATS
-	(iw_handler) NULL,		// SIOCSIWSPY
-	(iw_handler) NULL,		// SIOCGIWSPY
-	(iw_handler) NULL,		// -- hole --
-	(iw_handler) NULL,		// -- hole --
-	(iw_handler) NULL,		// SIOCSIWAP
-	(iw_handler) NULL,		// SIOCGIWAP
-	(iw_handler) NULL,		// -- hole -- 0x16
-	(iw_handler) NULL,		// SIOCGIWAPLIST
-	(iw_handler) iwctl_siwscan,	// SIOCSIWSCAN
-	(iw_handler) iwctl_giwscan,	// SIOCGIWSCAN
-	(iw_handler) NULL,		// SIOCSIWESSID
-	(iw_handler) NULL,		// SIOCGIWESSID
-	(iw_handler) NULL,		// SIOCSIWNICKN
-	(iw_handler) NULL,		// SIOCGIWNICKN
-	(iw_handler) NULL,		// -- hole --
-	(iw_handler) NULL,		// -- hole --
-	(iw_handler) NULL,		// SIOCSIWRATE 0x20
-	(iw_handler) NULL,		// SIOCGIWRATE
-	(iw_handler) NULL,		// SIOCSIWRTS
-	(iw_handler) NULL,		// SIOCGIWRTS
-	(iw_handler) NULL,		// SIOCSIWFRAG
-	(iw_handler) NULL,		// SIOCGIWFRAG
-	(iw_handler) NULL,		// SIOCSIWTXPOW
-	(iw_handler) NULL,		// SIOCGIWTXPOW
-	(iw_handler) NULL,		// SIOCSIWRETRY
-	(iw_handler) NULL,		// SIOCGIWRETRY
-	(iw_handler) NULL,		// SIOCSIWENCODE
-	(iw_handler) NULL,		// SIOCGIWENCODE
-	(iw_handler) NULL,		// SIOCSIWPOWER
-	(iw_handler) NULL,		// SIOCGIWPOWER
-
-//2008-0409-07, <Add> by Einsn Liu
-	(iw_handler) NULL,		// -- hole --
-	(iw_handler) NULL,		// -- hole --
-	(iw_handler) NULL,		// SIOCSIWGENIE
-	(iw_handler) NULL,		// SIOCGIWGENIE
-	(iw_handler) NULL,		// SIOCSIWAUTH
-	(iw_handler) NULL,		// SIOCGIWAUTH
-	(iw_handler) NULL,		// SIOCSIWENCODEEXT
-	(iw_handler) NULL,		// SIOCGIWENCODEEXT
-	(iw_handler) NULL,		// SIOCSIWPMKSA
-	(iw_handler) NULL,		// -- hole --
-};
-
-static const iw_handler		iwctl_private_handler[] =
-{
-	NULL,				// SIOCIWFIRSTPRIV
-};
-
-struct iw_priv_args iwctl_private_args[] = {
-	{ IOCTL_CMD_SET,
-	  IW_PRIV_TYPE_CHAR | 1024, 0,
-	  "set"},
-};
-
-const struct iw_handler_def	iwctl_handler_def =
-{
-	.get_wireless_stats = &iwctl_get_wireless_stats,
-	.num_standard	= sizeof(iwctl_handler)/sizeof(iw_handler),
-	.num_private	= 0,
-	.num_private_args = 0,
-	.standard	= (iw_handler *)iwctl_handler,
-	.private	= NULL,
-	.private_args	= NULL,
-};
diff --git a/drivers/staging/vt6655/iwctl.h b/drivers/staging/vt6655/iwctl.h
deleted file mode 100644
index 7dd63102182d..000000000000
--- a/drivers/staging/vt6655/iwctl.h
+++ /dev/null
@@ -1,206 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: iwctl.h
- *
- * Purpose:
- *
- * Author: Lyndon Chen
- *
- * Date: May 21, 2004
- *
- */
-
-#ifndef __IWCTL_H__
-#define __IWCTL_H__
-
-#include "device.h"
-
-/*---------------------  Export Definitions -------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-struct iw_statistics *iwctl_get_wireless_stats(struct net_device *dev);
-
-int iwctl_siwap(struct net_device *dev,
-		struct iw_request_info *info,
-		struct sockaddr *wrq,
-		char *extra);
-
-int iwctl_giwrange(struct net_device *dev,
-		   struct iw_request_info *info,
-		   struct iw_point *wrq,
-		   char *extra);
-
-int iwctl_giwmode(struct net_device *dev,
-		  struct iw_request_info *info,
-		  __u32 *wmode,
-		  char *extra);
-
-int iwctl_siwmode(struct net_device *dev,
-		  struct iw_request_info *info,
-		  __u32 *wmode,
-		  char *extra);
-
-int iwctl_giwfreq(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_freq *wrq,
-		  char *extra);
-
-int iwctl_siwfreq(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_freq *wrq,
-		  char *extra);
-
-int iwctl_giwname(struct net_device *dev,
-		  struct iw_request_info *info,
-		  char *wrq,
-		  char *extra);
-
-int iwctl_giwsens(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_param *wrq,
-		  char *extra);
-
-int iwctl_giwap(struct net_device *dev,
-		struct iw_request_info *info,
-		struct sockaddr *wrq,
-		char *extra);
-
-int iwctl_giwaplist(struct net_device *dev,
-		    struct iw_request_info *info,
-		    struct iw_point *wrq,
-		    char *extra);
-
-int iwctl_siwessid(struct net_device *dev,
-		   struct iw_request_info *info,
-		   struct iw_point *wrq,
-		   char *extra);
-
-int iwctl_giwessid(struct net_device *dev,
-		   struct iw_request_info *info,
-		   struct iw_point *wrq,
-		   char *extra);
-
-int iwctl_siwrate(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_param *wrq,
-		  char *extra);
-
-int iwctl_giwrate(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_param *wrq,
-		  char *extra);
-
-int iwctl_siwrts(struct net_device *dev,
-		 struct iw_request_info *info,
-		 struct iw_param *wrq,
-		 char *extra);
-
-int iwctl_giwrts(struct net_device *dev,
-		 struct iw_request_info *info,
-		 struct iw_param *wrq,
-		 char *extra);
-
-int iwctl_siwfrag(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_param *wrq,
-		  char *extra);
-
-int iwctl_giwfrag(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_param *wrq,
-		  char *extra);
-
-int iwctl_siwretry(struct net_device *dev,
-		   struct iw_request_info *info,
-		   struct iw_param *wrq,
-		   char *extra);
-
-int iwctl_giwretry(struct net_device *dev,
-		   struct iw_request_info *info,
-		   struct iw_param *wrq,
-		   char *extra);
-
-int iwctl_siwencode(struct net_device *dev,
-		    struct iw_request_info *info,
-		    struct iw_point *wrq,
-		    char *extra);
-
-int iwctl_giwencode(struct net_device *dev,
-		    struct iw_request_info *info,
-		    struct iw_point *wrq,
-		    char *extra);
-
-int iwctl_siwpower(struct net_device *dev,
-		   struct iw_request_info *info,
-		   struct iw_param *wrq,
-		   char *extra);
-
-int iwctl_giwpower(struct net_device *dev,
-		   struct iw_request_info *info,
-		   struct iw_param *wrq,
-		   char *extra);
-
-//2008-0409-07, <Add> by Einsn Liu
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-int iwctl_siwauth(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_param *wrq,
-		  char *extra);
-
-int iwctl_giwauth(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_param *wrq,
-		  char *extra);
-
-int iwctl_siwgenie(struct net_device *dev,
-		   struct iw_request_info *info,
-		   struct iw_point *wrq,
-		   char __user *extra);
-
-int iwctl_giwgenie(struct net_device *dev,
-		   struct iw_request_info *info,
-		   struct iw_point *wrq,
-		   char __user *extra);
-
-int iwctl_siwencodeext(struct net_device *dev,
-		       struct iw_request_info *info,
-		       struct iw_point *wrq,
-		       char *extra);
-
-int iwctl_giwencodeext(struct net_device *dev,
-		       struct iw_request_info *info,
-		       struct iw_point *wrq,
-		       char *extra);
-
-int iwctl_siwmlme(struct net_device *dev,
-		  struct iw_request_info *info,
-		  struct iw_point *wrq,
-		  char __user *extra);
-#endif // #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-//End Add -- //2008-0409-07, <Add> by Einsn Liu
-
-extern const struct iw_handler_def	iwctl_handler_def;
-extern struct iw_priv_args       iwctl_private_args[];
-
-#endif // __IWCTL_H__
diff --git a/drivers/staging/vt6655/key.c b/drivers/staging/vt6655/key.c
index 211afae306c7..f2b3fea90533 100644
--- a/drivers/staging/vt6655/key.c
+++ b/drivers/staging/vt6655/key.c
@@ -25,770 +25,144 @@
  *
  * Date: May 29, 2003
  *
- * Functions:
- *      KeyvInitTable - Init Key management table
- *      KeybGetKey - Get Key from table
- *      KeybSetKey - Set Key to table
- *      KeybRemoveKey - Remove Key from table
- *      KeybGetTransmitKey - Get Transmit Key from table
- *
- * Revision History:
- *
  */
 
 #include "tmacro.h"
 #include "key.h"
 #include "mac.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-static void
-s_vCheckKeyTableValid(PSKeyManagement pTable, void __iomem *dwIoBase)
-{
-	int i;
-
-	for (i = 0; i < MAX_KEY_TABLE; i++) {
-		if (pTable->KeyTable[i].bInUse &&
-		    !pTable->KeyTable[i].PairwiseKey.bKeyValid &&
-		    !pTable->KeyTable[i].GroupKey[0].bKeyValid &&
-		    !pTable->KeyTable[i].GroupKey[1].bKeyValid &&
-		    !pTable->KeyTable[i].GroupKey[2].bKeyValid &&
-		    !pTable->KeyTable[i].GroupKey[3].bKeyValid) {
-			pTable->KeyTable[i].bInUse = false;
-			pTable->KeyTable[i].wKeyCtl = 0;
-			pTable->KeyTable[i].bSoftWEP = false;
-			MACvDisableKeyEntry(dwIoBase, i);
-		}
-	}
-}
-
-/*---------------------  Export Functions  --------------------------*/
-
-/*
- * Description: Init Key management table
- *
- * Parameters:
- *  In:
- *      pTable          - Pointer to Key table
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void KeyvInitTable(PSKeyManagement pTable, void __iomem *dwIoBase)
-{
-	int i;
-	int jj;
-
-	for (i = 0; i < MAX_KEY_TABLE; i++) {
-		pTable->KeyTable[i].bInUse = false;
-		pTable->KeyTable[i].PairwiseKey.bKeyValid = false;
-		pTable->KeyTable[i].PairwiseKey.pvKeyTable = (void *)&pTable->KeyTable[i];
-		for (jj = 0; jj < MAX_GROUP_KEY; jj++) {
-			pTable->KeyTable[i].GroupKey[jj].bKeyValid = false;
-			pTable->KeyTable[i].GroupKey[jj].pvKeyTable = (void *)&pTable->KeyTable[i];
-		}
-		pTable->KeyTable[i].wKeyCtl = 0;
-		pTable->KeyTable[i].dwGTKeyIndex = 0;
-		pTable->KeyTable[i].bSoftWEP = false;
-		MACvDisableKeyEntry(dwIoBase, i);
-	}
-}
-
-/*
- * Description: Get Key from table
- *
- * Parameters:
- *  In:
- *      pTable          - Pointer to Key table
- *      pbyBSSID        - BSSID of Key
- *      dwKeyIndex      - Key Index (0xFFFFFFFF means pairwise key)
- *  Out:
- *      pKey            - Key return
- *
- * Return Value: true if found otherwise false
- *
- */
-bool KeybGetKey(
-	PSKeyManagement pTable,
-	unsigned char *pbyBSSID,
-	unsigned long dwKeyIndex,
-	PSKeyItem       *pKey
-)
-{
-	int i;
-
-	pr_debug("KeybGetKey()\n");
-
-	*pKey = NULL;
-	for (i = 0; i < MAX_KEY_TABLE; i++) {
-		if (pTable->KeyTable[i].bInUse &&
-		    ether_addr_equal(pTable->KeyTable[i].abyBSSID, pbyBSSID)) {
-			if (dwKeyIndex == 0xFFFFFFFF) {
-				if (pTable->KeyTable[i].PairwiseKey.bKeyValid) {
-					*pKey = &(pTable->KeyTable[i].PairwiseKey);
-					return true;
-				} else {
-					return false;
-				}
-			} else if (dwKeyIndex < MAX_GROUP_KEY) {
-				if (pTable->KeyTable[i].GroupKey[dwKeyIndex].bKeyValid) {
-					*pKey = &(pTable->KeyTable[i].GroupKey[dwKeyIndex]);
-					return true;
-				} else {
-					return false;
-				}
-			} else {
-				return false;
-			}
-		}
-	}
-	return false;
-}
-
-/*
- * Description: Set Key to table
- *
- * Parameters:
- *  In:
- *      pTable          - Pointer to Key table
- *      pbyBSSID        - BSSID of Key
- *      dwKeyIndex      - Key index (reference to NDIS DDK)
- *      uKeyLength      - Key length
- *      KeyRSC          - Key RSC
- *      pbyKey          - Pointer to key
- *  Out:
- *      none
- *
- * Return Value: true if success otherwise false
- *
- */
-bool KeybSetKey(
-	PSKeyManagement pTable,
-	unsigned char *pbyBSSID,
-	unsigned long dwKeyIndex,
-	unsigned long uKeyLength,
-	u64 *pKeyRSC,
-	unsigned char *pbyKey,
-	unsigned char byKeyDecMode,
-	void __iomem *dwIoBase,
-	unsigned char byLocalID
-)
-{
-	int i, j;
-	unsigned int ii;
-	PSKeyItem   pKey;
-	unsigned int uKeyIdx;
-
-	pr_debug("Enter KeybSetKey: %lX\n", dwKeyIndex);
-
-	j = (MAX_KEY_TABLE-1);
-	for (i = 0; i < (MAX_KEY_TABLE - 1); i++) {
-		if (!pTable->KeyTable[i].bInUse && (j == (MAX_KEY_TABLE-1))) {
-			// found empty table
-			j = i;
-		}
-		if (pTable->KeyTable[i].bInUse &&
-		    ether_addr_equal(pTable->KeyTable[i].abyBSSID, pbyBSSID)) {
-			// found table already exist
-			if ((dwKeyIndex & PAIRWISE_KEY) != 0) {
-				// Pairwise key
-				pKey = &(pTable->KeyTable[i].PairwiseKey);
-				pTable->KeyTable[i].wKeyCtl &= 0xFFF0;          // clear pairwise key control filed
-				pTable->KeyTable[i].wKeyCtl |= byKeyDecMode;
-				uKeyIdx = 4;                                    // use HW key entry 4 for pairwise key
-			} else {
-				// Group key
-				if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY)
-					return false;
-				pKey = &(pTable->KeyTable[i].GroupKey[dwKeyIndex & 0x000000FF]);
-				if ((dwKeyIndex & TRANSMIT_KEY) != 0)  {
-					// Group transmit key
-					pTable->KeyTable[i].dwGTKeyIndex = dwKeyIndex;
-					pr_debug("Group transmit key(R)[%lX]: %d\n",
-						 pTable->KeyTable[i].dwGTKeyIndex, i);
-				}
-				pTable->KeyTable[i].wKeyCtl &= 0xFF0F;          // clear group key control filed
-				pTable->KeyTable[i].wKeyCtl |= (byKeyDecMode << 4);
-				pTable->KeyTable[i].wKeyCtl |= 0x0040;          // use group key for group address
-				uKeyIdx = (dwKeyIndex & 0x000000FF);
-			}
-			pTable->KeyTable[i].wKeyCtl |= 0x8000;              // enable on-fly
-
-			pKey->bKeyValid = true;
-			pKey->uKeyLength = uKeyLength;
-			pKey->dwKeyIndex = dwKeyIndex;
-			pKey->byCipherSuite = byKeyDecMode;
-			memcpy(pKey->abyKey, pbyKey, uKeyLength);
-			if (byKeyDecMode == KEY_CTL_WEP) {
-				if (uKeyLength == WLAN_WEP40_KEYLEN)
-					pKey->abyKey[15] &= 0x7F;
-				if (uKeyLength == WLAN_WEP104_KEYLEN)
-					pKey->abyKey[15] |= 0x80;
-			}
-			MACvSetKeyEntry(dwIoBase, pTable->KeyTable[i].wKeyCtl, i, uKeyIdx, pbyBSSID, (u32 *)pKey->abyKey, byLocalID);
-
-			if ((dwKeyIndex & USE_KEYRSC) == 0) {
-				// RSC set by NIC
-				pKey->KeyRSC = 0;
-			} else {
-				pKey->KeyRSC = *pKeyRSC;
-			}
-			pKey->dwTSC47_16 = 0;
-			pKey->wTSC15_0 = 0;
-
-			pr_debug("KeybSetKey(R):\n");
-			pr_debug("pKey->bKeyValid: %d\n ", pKey->bKeyValid);
-			pr_debug("pKey->abyKey: ");
-			for (ii = 0; ii < pKey->uKeyLength; ii++)
-				pr_debug("%02x ", pKey->abyKey[ii]);
-
-			pr_debug("\n");
-
-			pr_debug("pKey->dwTSC47_16: %lx\n ", pKey->dwTSC47_16);
-			pr_debug("pKey->wTSC15_0: %x\n ", pKey->wTSC15_0);
-			pr_debug("pKey->dwKeyIndex: %lx\n ", pKey->dwKeyIndex);
-
-			return true;
-		}
-	}
-	if (j < (MAX_KEY_TABLE-1)) {
-		memcpy(pTable->KeyTable[j].abyBSSID, pbyBSSID, ETH_ALEN);
-		pTable->KeyTable[j].bInUse = true;
-		if ((dwKeyIndex & PAIRWISE_KEY) != 0)  {
-			// Pairwise key
-			pKey = &(pTable->KeyTable[j].PairwiseKey);
-			pTable->KeyTable[j].wKeyCtl &= 0xFFF0;          // clear pairwise key control filed
-			pTable->KeyTable[j].wKeyCtl |= byKeyDecMode;
-			uKeyIdx = 4;                                    // use HW key entry 4 for pairwise key
-		} else {
-			// Group key
-			if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY)
-				return false;
-			pKey = &(pTable->KeyTable[j].GroupKey[dwKeyIndex & 0x000000FF]);
-			if ((dwKeyIndex & TRANSMIT_KEY) != 0)  {
-				// Group transmit key
-				pTable->KeyTable[j].dwGTKeyIndex = dwKeyIndex;
-				pr_debug("Group transmit key(N)[%lX]: %d\n",
-					 pTable->KeyTable[j].dwGTKeyIndex, j);
-			}
-			pTable->KeyTable[j].wKeyCtl &= 0xFF0F;          // clear group key control filed
-			pTable->KeyTable[j].wKeyCtl |= (byKeyDecMode << 4);
-			pTable->KeyTable[j].wKeyCtl |= 0x0040;          // use group key for group address
-			uKeyIdx = (dwKeyIndex & 0x000000FF);
-		}
-		pTable->KeyTable[j].wKeyCtl |= 0x8000;              // enable on-fly
-
-		pKey->bKeyValid = true;
-		pKey->uKeyLength = uKeyLength;
-		pKey->dwKeyIndex = dwKeyIndex;
-		pKey->byCipherSuite = byKeyDecMode;
-		memcpy(pKey->abyKey, pbyKey, uKeyLength);
-		if (byKeyDecMode == KEY_CTL_WEP) {
-			if (uKeyLength == WLAN_WEP40_KEYLEN)
-				pKey->abyKey[15] &= 0x7F;
-			if (uKeyLength == WLAN_WEP104_KEYLEN)
-				pKey->abyKey[15] |= 0x80;
-		}
-		MACvSetKeyEntry(dwIoBase, pTable->KeyTable[j].wKeyCtl, j, uKeyIdx, pbyBSSID, (u32 *)pKey->abyKey, byLocalID);
-
-		if ((dwKeyIndex & USE_KEYRSC) == 0) {
-			// RSC set by NIC
-			pKey->KeyRSC = 0;
-		} else {
-			pKey->KeyRSC = *pKeyRSC;
-		}
-		pKey->dwTSC47_16 = 0;
-		pKey->wTSC15_0 = 0;
-
-		pr_debug("KeybSetKey(N):\n");
-		pr_debug("pKey->bKeyValid: %d\n ", pKey->bKeyValid);
-		pr_debug("pKey->uKeyLength: %d\n ", (int)pKey->uKeyLength);
-		pr_debug("pKey->abyKey: ");
-		for (ii = 0; ii < pKey->uKeyLength; ii++)
-			pr_debug("%02x ", pKey->abyKey[ii]);
-
-		pr_debug("\n");
-
-		pr_debug("pKey->dwTSC47_16: %lx\n ", pKey->dwTSC47_16);
-		pr_debug("pKey->wTSC15_0: %x\n ", pKey->wTSC15_0);
-		pr_debug("pKey->dwKeyIndex: %lx\n ", pKey->dwKeyIndex);
-
-		return true;
-	}
-	return false;
-}
-
-/*
- * Description: Remove Key from table
- *
- * Parameters:
- *  In:
- *      pTable          - Pointer to Key table
- *      pbyBSSID        - BSSID of Key
- *      dwKeyIndex      - Key Index (reference to NDIS DDK)
- *  Out:
- *      none
- *
- * Return Value: true if success otherwise false
- *
- */
-bool KeybRemoveKey(
-	PSKeyManagement pTable,
-	unsigned char *pbyBSSID,
-	unsigned long dwKeyIndex,
-	void __iomem *dwIoBase
-)
-{
-	int  i;
-
-	if (is_broadcast_ether_addr(pbyBSSID)) {
-		// delete all keys
-		if ((dwKeyIndex & PAIRWISE_KEY) != 0) {
-			for (i = 0; i < MAX_KEY_TABLE; i++)
-				pTable->KeyTable[i].PairwiseKey.bKeyValid = false;
-
-			s_vCheckKeyTableValid(pTable, dwIoBase);
-			return true;
-		} else if ((dwKeyIndex & 0x000000FF) < MAX_GROUP_KEY) {
-			for (i = 0; i < MAX_KEY_TABLE; i++) {
-				pTable->KeyTable[i].GroupKey[dwKeyIndex & 0x000000FF].bKeyValid = false;
-				if ((dwKeyIndex & 0x7FFFFFFF) == (pTable->KeyTable[i].dwGTKeyIndex & 0x7FFFFFFF)) {
-					// remove Group transmit key
-					pTable->KeyTable[i].dwGTKeyIndex = 0;
-				}
-			}
-			s_vCheckKeyTableValid(pTable, dwIoBase);
-			return true;
-		} else {
-			return false;
-		}
-	}
-
-	for (i = 0; i < MAX_KEY_TABLE; i++) {
-		if (pTable->KeyTable[i].bInUse &&
-		    ether_addr_equal(pTable->KeyTable[i].abyBSSID, pbyBSSID)) {
-			if ((dwKeyIndex & PAIRWISE_KEY) != 0) {
-				pTable->KeyTable[i].PairwiseKey.bKeyValid = false;
-				s_vCheckKeyTableValid(pTable, dwIoBase);
-				return true;
-			} else if ((dwKeyIndex & 0x000000FF) < MAX_GROUP_KEY) {
-				pTable->KeyTable[i].GroupKey[dwKeyIndex & 0x000000FF].bKeyValid = false;
-				if ((dwKeyIndex & 0x7FFFFFFF) == (pTable->KeyTable[i].dwGTKeyIndex & 0x7FFFFFFF)) {
-					// remove Group transmit key
-					pTable->KeyTable[i].dwGTKeyIndex = 0;
-				}
-				s_vCheckKeyTableValid(pTable, dwIoBase);
-				return true;
-			} else {
-				return false;
-			}
-		}
-	}
-	return false;
-}
-
-/*
- * Description: Remove Key from table
- *
- * Parameters:
- *  In:
- *      pTable          - Pointer to Key table
- *      pbyBSSID        - BSSID of Key
- *  Out:
- *      none
- *
- * Return Value: true if success otherwise false
- *
- */
-bool KeybRemoveAllKey(
-	PSKeyManagement pTable,
-	unsigned char *pbyBSSID,
-	void __iomem *dwIoBase
-)
+int vnt_key_init_table(struct vnt_private *priv)
 {
-	int i, u;
+	u32 i;
 
-	for (i = 0; i < MAX_KEY_TABLE; i++) {
-		if (pTable->KeyTable[i].bInUse &&
-		    ether_addr_equal(pTable->KeyTable[i].abyBSSID, pbyBSSID)) {
-			pTable->KeyTable[i].PairwiseKey.bKeyValid = false;
-			for (u = 0; u < MAX_GROUP_KEY; u++)
-				pTable->KeyTable[i].GroupKey[u].bKeyValid = false;
+	for (i = 0; i < MAX_KEY_TABLE; i++)
+		MACvDisableKeyEntry(priv->PortOffset, i);
 
-			pTable->KeyTable[i].dwGTKeyIndex = 0;
-			s_vCheckKeyTableValid(pTable, dwIoBase);
-			return true;
-		}
-	}
-	return false;
+	return 0;
 }
 
-/*
- * Description: Remove WEP Key from table
- *
- * Parameters:
- *  In:
- *      pTable          - Pointer to Key table
- *  Out:
- *      none
- *
- * Return Value: true if success otherwise false
- *
- */
-void KeyvRemoveWEPKey(
-	PSKeyManagement pTable,
-	unsigned long dwKeyIndex,
-	void __iomem *dwIoBase
-)
+static int vnt_set_keymode(struct ieee80211_hw *hw, u8 *mac_addr,
+	struct ieee80211_key_conf *key, u32 key_type, u32 mode,
+	bool onfly_latch)
 {
-	if ((dwKeyIndex & 0x000000FF) < MAX_GROUP_KEY) {
-		if (pTable->KeyTable[MAX_KEY_TABLE-1].bInUse) {
-			if (pTable->KeyTable[MAX_KEY_TABLE-1].GroupKey[dwKeyIndex & 0x000000FF].byCipherSuite == KEY_CTL_WEP) {
-				pTable->KeyTable[MAX_KEY_TABLE-1].GroupKey[dwKeyIndex & 0x000000FF].bKeyValid = false;
-				if ((dwKeyIndex & 0x7FFFFFFF) == (pTable->KeyTable[MAX_KEY_TABLE-1].dwGTKeyIndex & 0x7FFFFFFF)) {
-					// remove Group transmit key
-					pTable->KeyTable[MAX_KEY_TABLE-1].dwGTKeyIndex = 0;
-				}
+	struct vnt_private *priv = hw->priv;
+	u8 broadcast[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u16 key_mode = 0;
+	u32 entry = 0;
+	u8 *bssid;
+	u8 key_inx = key->keyidx;
+	u8 i;
+
+	if (mac_addr)
+		bssid = mac_addr;
+	else
+		bssid = &broadcast[0];
+
+	if (key_type != VNT_KEY_DEFAULTKEY) {
+		for (i = 0; i < (MAX_KEY_TABLE - 1); i++) {
+			if (!test_bit(i, &priv->key_entry_inuse)) {
+				set_bit(i, &priv->key_entry_inuse);
+
+				key->hw_key_idx = i;
+				entry = key->hw_key_idx;
+				break;
 			}
 		}
-		s_vCheckKeyTableValid(pTable, dwIoBase);
 	}
-}
 
-void KeyvRemoveAllWEPKey(
-	PSKeyManagement pTable,
-	void __iomem *dwIoBase
-)
-{
-	int i;
-
-	for (i = 0; i < MAX_GROUP_KEY; i++)
-		KeyvRemoveWEPKey(pTable, i, dwIoBase);
+	switch (key_type) {
+	/* fallthrough */
+	case VNT_KEY_DEFAULTKEY:
+		/* default key last entry */
+		entry = MAX_KEY_TABLE - 1;
+		key->hw_key_idx = entry;
+	case VNT_KEY_ALLGROUP:
+		key_mode |= VNT_KEY_ALLGROUP;
+		if (onfly_latch)
+			key_mode |= VNT_KEY_ONFLY_ALL;
+	case VNT_KEY_GROUP_ADDRESS:
+		key_mode |= mode;
+	case VNT_KEY_GROUP:
+		key_mode |= (mode << 4);
+		key_mode |= VNT_KEY_GROUP;
+		break;
+	case  VNT_KEY_PAIRWISE:
+		key_mode |= mode;
+		key_inx = 4;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (onfly_latch)
+		key_mode |= VNT_KEY_ONFLY;
+
+	if (mode == KEY_CTL_WEP) {
+		if (key->keylen == WLAN_KEY_LEN_WEP40)
+			key->key[15] &= 0x7f;
+		if (key->keylen == WLAN_KEY_LEN_WEP104)
+			key->key[15] |= 0x80;
+	}
+
+	MACvSetKeyEntry(priv->PortOffset, key_mode, entry, key_inx,
+			bssid, (u32 *)key->key, priv->byLocalID);
+
+	return 0;
 }
 
-/*
- * Description: Get Transmit Key from table
- *
- * Parameters:
- *  In:
- *      pTable          - Pointer to Key table
- *      pbyBSSID        - BSSID of Key
- *  Out:
- *      pKey            - Key return
- *
- * Return Value: true if found otherwise false
- *
- */
-bool KeybGetTransmitKey(
-	PSKeyManagement pTable,
-	unsigned char *pbyBSSID,
-	unsigned long dwKeyType,
-	PSKeyItem       *pKey
-)
+int vnt_set_keys(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
+		 struct ieee80211_vif *vif, struct ieee80211_key_conf *key)
 {
-	int i, ii;
-
-	*pKey = NULL;
-	for (i = 0; i < MAX_KEY_TABLE; i++) {
-		if (pTable->KeyTable[i].bInUse &&
-		    ether_addr_equal(pTable->KeyTable[i].abyBSSID, pbyBSSID)) {
-			if (dwKeyType == PAIRWISE_KEY) {
-				if (pTable->KeyTable[i].PairwiseKey.bKeyValid) {
-					*pKey = &(pTable->KeyTable[i].PairwiseKey);
-
-					pr_debug("KeybGetTransmitKey:");
-					pr_debug("PAIRWISE_KEY: KeyTable.abyBSSID: ");
-					for (ii = 0; ii < 6; ii++)
-						pr_debug("%x ",
-							 pTable->KeyTable[i].abyBSSID[ii]);
-
-					pr_debug("\n");
+	struct ieee80211_bss_conf *conf = &vif->bss_conf;
+	struct vnt_private *priv = hw->priv;
+	u8 *mac_addr = NULL;
+	u8 key_dec_mode = 0;
+	int ret = 0;
+	u32 u;
 
-					return true;
-				} else {
-					pr_debug("PairwiseKey.bKeyValid == false\n");
-					return false;
-				}
-			} // End of Type == PAIRWISE
-			else {
-				if (pTable->KeyTable[i].dwGTKeyIndex == 0) {
-					pr_debug("ERROR: dwGTKeyIndex == 0 !!!\n");
-					return false;
-				}
-				if (pTable->KeyTable[i].GroupKey[(pTable->KeyTable[i].dwGTKeyIndex&0x000000FF)].bKeyValid) {
-					*pKey = &(pTable->KeyTable[i].GroupKey[(pTable->KeyTable[i].dwGTKeyIndex&0x000000FF)]);
-
-					pr_debug("KeybGetTransmitKey:");
-					pr_debug("GROUP_KEY: KeyTable.abyBSSID\n");
-					for (ii = 0; ii < 6; ii++)
-						pr_debug("%x ",
-							 pTable->KeyTable[i].abyBSSID[ii]);
-
-					pr_debug("\n");
-					pr_debug("dwGTKeyIndex: %lX\n",
-						 pTable->KeyTable[i].dwGTKeyIndex);
-
-					return true;
-				} else {
-					pr_debug("GroupKey.bKeyValid == false\n");
-					return false;
-				}
-			} // End of Type = GROUP
-		} // BSSID match
-	}
-	pr_debug("ERROR: NO Match BSSID !!! ");
-	for (ii = 0; ii < 6; ii++)
-		pr_debug("%02x ", *(pbyBSSID+ii));
-
-	pr_debug("\n");
-	return false;
-}
-
-/*
- * Description: Check Pairewise Key
- *
- * Parameters:
- *  In:
- *      pTable          - Pointer to Key table
- *  Out:
- *      none
- *
- * Return Value: true if found otherwise false
- *
- */
-bool KeybCheckPairewiseKey(
-	PSKeyManagement pTable,
-	PSKeyItem       *pKey
-)
-{
-	int i;
+	if (sta)
+		mac_addr = &sta->addr[0];
 
-	*pKey = NULL;
-	for (i = 0; i < MAX_KEY_TABLE; i++) {
-		if (pTable->KeyTable[i].bInUse &&
-		    pTable->KeyTable[i].PairwiseKey.bKeyValid) {
-			*pKey = &(pTable->KeyTable[i].PairwiseKey);
-			return true;
-		}
-	}
-	return false;
-}
+	switch (key->cipher) {
+	case 0:
+		for (u = 0 ; u < MAX_KEY_TABLE; u++)
+			MACvDisableKeyEntry(priv->PortOffset, u);
+		return ret;
 
-/*
- * Description: Set Key to table
- *
- * Parameters:
- *  In:
- *      pTable          - Pointer to Key table
- *      dwKeyIndex      - Key index (reference to NDIS DDK)
- *      uKeyLength      - Key length
- *      KeyRSC          - Key RSC
- *      pbyKey          - Pointer to key
- *  Out:
- *      none
- *
- * Return Value: true if success otherwise false
- *
- */
-bool KeybSetDefaultKey(
-	PSKeyManagement pTable,
-	unsigned long dwKeyIndex,
-	unsigned long uKeyLength,
-	u64 *pKeyRSC,
-	unsigned char *pbyKey,
-	unsigned char byKeyDecMode,
-	void __iomem *dwIoBase,
-	unsigned char byLocalID
-)
-{
-	unsigned int ii;
-	PSKeyItem   pKey;
-	unsigned int uKeyIdx;
+	case WLAN_CIPHER_SUITE_WEP40:
+	case WLAN_CIPHER_SUITE_WEP104:
+		for (u = 0; u < MAX_KEY_TABLE; u++)
+			MACvDisableKeyEntry(priv->PortOffset, u);
 
-	pr_debug("Enter KeybSetDefaultKey: %1x, %d\n",
-		 (int)dwKeyIndex, (int)uKeyLength);
+		vnt_set_keymode(hw, mac_addr,
+				key, VNT_KEY_DEFAULTKEY, KEY_CTL_WEP, true);
 
-	if ((dwKeyIndex & PAIRWISE_KEY) != 0) // Pairwise key
-		return false;
-	else if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY)
-		return false;
+		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
 
-	if (uKeyLength > MAX_KEY_LEN)
-		return false;
+		return ret;
+	case WLAN_CIPHER_SUITE_TKIP:
+		key->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
+		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
 
-	pTable->KeyTable[MAX_KEY_TABLE - 1].bInUse = true;
-	for (ii = 0; ii < ETH_ALEN; ii++)
-		pTable->KeyTable[MAX_KEY_TABLE - 1].abyBSSID[ii] = 0xFF;
+		key_dec_mode = KEY_CTL_TKIP;
 
-	// Group key
-	pKey = &(pTable->KeyTable[MAX_KEY_TABLE - 1].GroupKey[dwKeyIndex & 0x000000FF]);
-	if ((dwKeyIndex & TRANSMIT_KEY) != 0)  {
-		// Group transmit key
-		pTable->KeyTable[MAX_KEY_TABLE-1].dwGTKeyIndex = dwKeyIndex;
-		pr_debug("Group transmit key(R)[%lX]: %d\n",
-			 pTable->KeyTable[MAX_KEY_TABLE-1].dwGTKeyIndex,
-			 MAX_KEY_TABLE-1);
+		break;
+	case WLAN_CIPHER_SUITE_CCMP:
+		key_dec_mode = KEY_CTL_CCMP;
 
+		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
 	}
-	pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl &= 0x7F00;          // clear all key control filed
-	pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl |= (byKeyDecMode << 4);
-	pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl |= (byKeyDecMode);
-	pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl |= 0x0044;          // use group key for all address
-	uKeyIdx = (dwKeyIndex & 0x000000FF);
 
-	if ((uKeyLength == WLAN_WEP232_KEYLEN) &&
-	    (byKeyDecMode == KEY_CTL_WEP)) {
-		pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl |= 0x4000;              // disable on-fly disable address match
-		pTable->KeyTable[MAX_KEY_TABLE-1].bSoftWEP = true;
+	if (key->flags & IEEE80211_KEY_FLAG_PAIRWISE) {
+		vnt_set_keymode(hw, mac_addr,
+				key, VNT_KEY_PAIRWISE, key_dec_mode, true);
 	} else {
-		if (!pTable->KeyTable[MAX_KEY_TABLE-1].bSoftWEP)
-			pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl |= 0xC000;          // enable on-fly disable address match
-	}
+		vnt_set_keymode(hw, mac_addr,
+				key, VNT_KEY_DEFAULTKEY, key_dec_mode, true);
 
-	pKey->bKeyValid = true;
-	pKey->uKeyLength = uKeyLength;
-	pKey->dwKeyIndex = dwKeyIndex;
-	pKey->byCipherSuite = byKeyDecMode;
-	memcpy(pKey->abyKey, pbyKey, uKeyLength);
-	if (byKeyDecMode == KEY_CTL_WEP) {
-		if (uKeyLength == WLAN_WEP40_KEYLEN)
-			pKey->abyKey[15] &= 0x7F;
-		if (uKeyLength == WLAN_WEP104_KEYLEN)
-			pKey->abyKey[15] |= 0x80;
-	}
-	MACvSetKeyEntry(dwIoBase, pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl, MAX_KEY_TABLE-1, uKeyIdx, pTable->KeyTable[MAX_KEY_TABLE-1].abyBSSID, (u32 *)pKey->abyKey, byLocalID);
-
-	if ((dwKeyIndex & USE_KEYRSC) == 0) {
-		// RSC set by NIC
-		pKey->KeyRSC = 0;
-	} else {
-		pKey->KeyRSC = *pKeyRSC;
+		vnt_set_keymode(hw, (u8 *)conf->bssid,
+				key, VNT_KEY_GROUP_ADDRESS, key_dec_mode, true);
 	}
-	pKey->dwTSC47_16 = 0;
-	pKey->wTSC15_0 = 0;
-
-	pr_debug("KeybSetKey(R):\n");
-	pr_debug("pKey->bKeyValid: %d\n", pKey->bKeyValid);
-	pr_debug("pKey->uKeyLength: %d\n", (int)pKey->uKeyLength);
-	pr_debug("pKey->abyKey:\n");
-	for (ii = 0; ii < pKey->uKeyLength; ii++)
-		pr_debug("%x", pKey->abyKey[ii]);
-
-	pr_debug("\n");
-
-	pr_debug("pKey->dwTSC47_16: %lx\n", pKey->dwTSC47_16);
-	pr_debug("pKey->wTSC15_0: %x\n", pKey->wTSC15_0);
-	pr_debug("pKey->dwKeyIndex: %lx\n", pKey->dwKeyIndex);
-
-	return true;
-}
 
-/*
- * Description: Set Key to table
- *
- * Parameters:
- *  In:
- *      pTable          - Pointer to Key table
- *      dwKeyIndex      - Key index (reference to NDIS DDK)
- *      uKeyLength      - Key length
- *      KeyRSC          - Key RSC
- *      pbyKey          - Pointer to key
- *  Out:
- *      none
- *
- * Return Value: true if success otherwise false
- *
- */
-bool KeybSetAllGroupKey(
-	PSKeyManagement pTable,
-	unsigned long dwKeyIndex,
-	unsigned long uKeyLength,
-	u64 *pKeyRSC,
-	unsigned char *pbyKey,
-	unsigned char byKeyDecMode,
-	void __iomem *dwIoBase,
-	unsigned char byLocalID
-)
-{
-	int         i;
-	unsigned int ii;
-	PSKeyItem   pKey;
-	unsigned int uKeyIdx;
-
-	pr_debug("Enter KeybSetAllGroupKey: %lX\n", dwKeyIndex);
-
-	if ((dwKeyIndex & PAIRWISE_KEY) != 0) // Pairwise key
-		return false;
-	else if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY)
-		return false;
-
-	for (i = 0; i < MAX_KEY_TABLE - 1; i++) {
-		if (pTable->KeyTable[i].bInUse) {
-			// found table already exist
-			// Group key
-			pKey = &(pTable->KeyTable[i].GroupKey[dwKeyIndex & 0x000000FF]);
-			if ((dwKeyIndex & TRANSMIT_KEY) != 0)  {
-				// Group transmit key
-				pTable->KeyTable[i].dwGTKeyIndex = dwKeyIndex;
-				pr_debug("Group transmit key(R)[%lX]: %d\n",
-					 pTable->KeyTable[i].dwGTKeyIndex, i);
-
-			}
-			pTable->KeyTable[i].wKeyCtl &= 0xFF0F;          // clear group key control filed
-			pTable->KeyTable[i].wKeyCtl |= (byKeyDecMode << 4);
-			pTable->KeyTable[i].wKeyCtl |= 0x0040;          // use group key for group address
-			uKeyIdx = (dwKeyIndex & 0x000000FF);
-
-			pTable->KeyTable[i].wKeyCtl |= 0x8000;              // enable on-fly
-
-			pKey->bKeyValid = true;
-			pKey->uKeyLength = uKeyLength;
-			pKey->dwKeyIndex = dwKeyIndex;
-			pKey->byCipherSuite = byKeyDecMode;
-			memcpy(pKey->abyKey, pbyKey, uKeyLength);
-			if (byKeyDecMode == KEY_CTL_WEP) {
-				if (uKeyLength == WLAN_WEP40_KEYLEN)
-					pKey->abyKey[15] &= 0x7F;
-				if (uKeyLength == WLAN_WEP104_KEYLEN)
-					pKey->abyKey[15] |= 0x80;
-			}
-			MACvSetKeyEntry(dwIoBase, pTable->KeyTable[i].wKeyCtl, i, uKeyIdx, pTable->KeyTable[i].abyBSSID, (u32 *)pKey->abyKey, byLocalID);
-
-			if ((dwKeyIndex & USE_KEYRSC) == 0) {
-				// RSC set by NIC
-				pKey->KeyRSC = 0;
-			} else {
-				pKey->KeyRSC = *pKeyRSC;
-			}
-			pKey->dwTSC47_16 = 0;
-			pKey->wTSC15_0 = 0;
-
-			pr_debug("KeybSetKey(R):\n");
-			pr_debug("pKey->bKeyValid: %d\n ", pKey->bKeyValid);
-			pr_debug("pKey->uKeyLength: %d\n ",
-				 (int)pKey->uKeyLength);
-			pr_debug("pKey->abyKey: ");
-			for (ii = 0; ii < pKey->uKeyLength; ii++)
-				pr_debug("%02x ", pKey->abyKey[ii]);
-
-			pr_debug("\n");
-
-		} // (pTable->KeyTable[i].bInUse == true)
-	}
-	return true;
+	return 0;
 }
diff --git a/drivers/staging/vt6655/key.h b/drivers/staging/vt6655/key.h
index 44efe18315af..c01d4afb6ab8 100644
--- a/drivers/staging/vt6655/key.h
+++ b/drivers/staging/vt6655/key.h
@@ -30,9 +30,7 @@
 #ifndef __KEY_H__
 #define __KEY_H__
 
-#include "ttype.h"
-#include "tether.h"
-#include "80211mgr.h"
+#include <net/mac80211.h>
 
 /*---------------------  Export Definitions -------------------------*/
 #define MAX_GROUP_KEY       4
@@ -53,124 +51,19 @@
 #define KEY_CTL_CCMP        0x03
 #define KEY_CTL_INVALID     0xFF
 
-typedef struct tagSKeyItem {
-	bool bKeyValid;
-	unsigned long uKeyLength;
-	unsigned char abyKey[MAX_KEY_LEN];
-	u64 KeyRSC;
-	unsigned long dwTSC47_16;
-	unsigned short wTSC15_0;
-	unsigned char byCipherSuite;
-	unsigned char byReserved0;
-	unsigned long dwKeyIndex;
-	void *pvKeyTable;
-} SKeyItem, *PSKeyItem; //64
+#define VNT_KEY_DEFAULTKEY	0x1
+#define VNT_KEY_GROUP_ADDRESS	0x2
+#define VNT_KEY_ALLGROUP	0x4
+#define VNT_KEY_GROUP		0x40
+#define VNT_KEY_PAIRWISE	0x00
+#define VNT_KEY_ONFLY		0x8000
+#define VNT_KEY_ONFLY_ALL	0x4000
 
-typedef struct tagSKeyTable {
-	unsigned char abyBSSID[ETH_ALEN];  //6
-	unsigned char byReserved0[2];              //8
-	SKeyItem    PairwiseKey;
-	SKeyItem    GroupKey[MAX_GROUP_KEY]; //64*5 = 320, 320+8=328
-	unsigned long dwGTKeyIndex;            // GroupTransmitKey Index
-	bool bInUse;
-	//2006-1116-01,<Modify> by NomadZhao
-	bool bSoftWEP;
-	unsigned short wKeyCtl;      // for address of wKeyCtl at align 4
+struct vnt_private;
 
-	unsigned char byReserved1[6];
-} SKeyTable, *PSKeyTable; //348
+int vnt_key_init_table(struct vnt_private *);
 
-typedef struct tagSKeyManagement {
-	SKeyTable   KeyTable[MAX_KEY_TABLE];
-} SKeyManagement, *PSKeyManagement;
-
-/*---------------------  Export Types  ------------------------------*/
-
-/*---------------------  Export Macros ------------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-void KeyvInitTable(PSKeyManagement pTable, void __iomem *dwIoBase);
-
-bool KeybGetKey(
-	PSKeyManagement pTable,
-	unsigned char *pbyBSSID,
-	unsigned long dwKeyIndex,
-	PSKeyItem       *pKey
-);
-
-bool KeybSetKey(
-	PSKeyManagement pTable,
-	unsigned char *pbyBSSID,
-	unsigned long dwKeyIndex,
-	unsigned long uKeyLength,
-	u64 *pKeyRSC,
-	unsigned char *pbyKey,
-	unsigned char byKeyDecMode,
-	void __iomem *dwIoBase,
-	unsigned char byLocalID
-);
-
-bool KeybSetDefaultKey(
-	PSKeyManagement pTable,
-	unsigned long dwKeyIndex,
-	unsigned long uKeyLength,
-	u64 *pKeyRSC,
-	unsigned char *pbyKey,
-	unsigned char byKeyDecMode,
-	void __iomem *dwIoBase,
-	unsigned char byLocalID
-);
-
-bool KeybRemoveKey(
-	PSKeyManagement pTable,
-	unsigned char *pbyBSSID,
-	unsigned long dwKeyIndex,
-	void __iomem *dwIoBase
-);
-
-bool KeybGetTransmitKey(
-	PSKeyManagement pTable,
-	unsigned char *pbyBSSID,
-	unsigned long dwKeyType,
-	PSKeyItem       *pKey
-);
-
-bool KeybCheckPairewiseKey(
-	PSKeyManagement pTable,
-	PSKeyItem       *pKey
-);
-
-bool KeybRemoveAllKey(
-	PSKeyManagement pTable,
-	unsigned char *pbyBSSID,
-	void __iomem *dwIoBase
-);
-
-void KeyvRemoveWEPKey(
-	PSKeyManagement pTable,
-	unsigned long dwKeyIndex,
-	void __iomem *dwIoBase
-);
-
-void KeyvRemoveAllWEPKey(
-	PSKeyManagement pTable,
-	void __iomem *dwIoBase
-);
-
-bool KeybSetAllGroupKey(
-	PSKeyManagement pTable,
-	unsigned long dwKeyIndex,
-	unsigned long uKeyLength,
-	u64 *pKeyRSC,
-	unsigned char *pbyKey,
-	unsigned char byKeyDecMode,
-	void __iomem *dwIoBase,
-	unsigned char byLocalID
-);
+int vnt_set_keys(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
+		 struct ieee80211_vif *vif, struct ieee80211_key_conf *key);
 
 #endif // __KEY_H__
diff --git a/drivers/staging/vt6655/mac.c b/drivers/staging/vt6655/mac.c
index e3b0b7f7ca85..8f0d652fea7c 100644
--- a/drivers/staging/vt6655/mac.c
+++ b/drivers/staging/vt6655/mac.c
@@ -26,30 +26,15 @@
  * Date: May 21, 1996
  *
  * Functions:
- *      MACvReadAllRegs - Read All MAC Registers to buffer
  *      MACbIsRegBitsOn - Test if All test Bits On
  *      MACbIsRegBitsOff - Test if All test Bits Off
  *      MACbIsIntDisable - Test if MAC interrupt disable
- *      MACbyReadMultiAddr - Read Multicast Address Mask Pattern
- *      MACvWriteMultiAddr - Write Multicast Address Mask Pattern
- *      MACvSetMultiAddrByHash - Set Multicast Address Mask by Hash value
- *      MACvResetMultiAddrByHash - Clear Multicast Address Mask by Hash value
- *      MACvSetRxThreshold - Set Rx Threshold value
- *      MACvGetRxThreshold - Get Rx Threshold value
- *      MACvSetTxThreshold - Set Tx Threshold value
- *      MACvGetTxThreshold - Get Tx Threshold value
- *      MACvSetDmaLength - Set Dma Length value
- *      MACvGetDmaLength - Get Dma Length value
  *      MACvSetShortRetryLimit - Set 802.11 Short Retry limit
  *      MACvGetShortRetryLimit - Get 802.11 Short Retry limit
  *      MACvSetLongRetryLimit - Set 802.11 Long Retry limit
- *      MACvGetLongRetryLimit - Get 802.11 Long Retry limit
  *      MACvSetLoopbackMode - Set MAC Loopback Mode
- *      MACbIsInLoopbackMode - Test if MAC in Loopback mode
- *      MACvSetPacketFilter - Set MAC Address Filter
  *      MACvSaveContext - Save Context of MAC Registers
  *      MACvRestoreContext - Restore Context of MAC Registers
- *      MACbCompareContext - Compare if values of MAC Registers same as Context
  *      MACbSoftwareReset - Software Reset MAC
  *      MACbSafeRxOff - Turn Off MAC Rx
  *      MACbSafeTxOff - Turn Off MAC Tx
@@ -69,54 +54,8 @@
  */
 
 #include "tmacro.h"
-#include "tether.h"
 #include "mac.h"
 
-unsigned short TxRate_iwconfig;//2008-5-8 <add> by chester
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-/*
- * Description:
- *      Read All MAC Registers to buffer
- *
- * Parameters:
- *  In:
- *      dwIoBase    - Base Address for MAC
- *  Out:
- *      pbyMacRegs  - buffer to read
- *
- * Return Value: none
- *
- */
-void MACvReadAllRegs(void __iomem *dwIoBase, unsigned char *pbyMacRegs)
-{
-	int ii;
-
-	// read page0 register
-	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE0; ii++) {
-		VNSvInPortB(dwIoBase + ii, pbyMacRegs);
-		pbyMacRegs++;
-	}
-
-	MACvSelectPage1(dwIoBase);
-
-	// read page1 register
-	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE1; ii++) {
-		VNSvInPortB(dwIoBase + ii, pbyMacRegs);
-		pbyMacRegs++;
-	}
-
-	MACvSelectPage0(dwIoBase);
-}
-
 /*
  * Description:
  *      Test if all test bits on
@@ -189,252 +128,6 @@ bool MACbIsIntDisable(void __iomem *dwIoBase)
 
 /*
  * Description:
- *      Read MAC Multicast Address Mask
- *
- * Parameters:
- *  In:
- *      dwIoBase    - Base Address for MAC
- *      uByteidx    - Index of Mask
- *  Out:
- *      none
- *
- * Return Value: Mask Value read
- *
- */
-unsigned char MACbyReadMultiAddr(void __iomem *dwIoBase, unsigned int uByteIdx)
-{
-	unsigned char byData;
-
-	MACvSelectPage1(dwIoBase);
-	VNSvInPortB(dwIoBase + MAC_REG_MAR0 + uByteIdx, &byData);
-	MACvSelectPage0(dwIoBase);
-	return byData;
-}
-
-/*
- * Description:
- *      Write MAC Multicast Address Mask
- *
- * Parameters:
- *  In:
- *      dwIoBase    - Base Address for MAC
- *      uByteidx    - Index of Mask
- *      byData      - Mask Value to write
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void MACvWriteMultiAddr(void __iomem *dwIoBase, unsigned int uByteIdx, unsigned char byData)
-{
-	MACvSelectPage1(dwIoBase);
-	VNSvOutPortB(dwIoBase + MAC_REG_MAR0 + uByteIdx, byData);
-	MACvSelectPage0(dwIoBase);
-}
-
-/*
- * Description:
- *      Set this hash index into multicast address register bit
- *
- * Parameters:
- *  In:
- *      dwIoBase    - Base Address for MAC
- *      byHashIdx   - Hash index to set
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void MACvSetMultiAddrByHash(void __iomem *dwIoBase, unsigned char byHashIdx)
-{
-	unsigned int uByteIdx;
-	unsigned char byBitMask;
-	unsigned char byOrgValue;
-
-	// calculate byte position
-	uByteIdx = byHashIdx / 8;
-	ASSERT(uByteIdx < 8);
-	// calculate bit position
-	byBitMask = 1;
-	byBitMask <<= (byHashIdx % 8);
-	// turn on the bit
-	byOrgValue = MACbyReadMultiAddr(dwIoBase, uByteIdx);
-	MACvWriteMultiAddr(dwIoBase, uByteIdx, (unsigned char)(byOrgValue | byBitMask));
-}
-
-/*
- * Description:
- *      Reset this hash index into multicast address register bit
- *
- * Parameters:
- *  In:
- *      dwIoBase    - Base Address for MAC
- *      byHashIdx   - Hash index to clear
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void MACvResetMultiAddrByHash(void __iomem *dwIoBase, unsigned char byHashIdx)
-{
-	unsigned int uByteIdx;
-	unsigned char byBitMask;
-	unsigned char byOrgValue;
-
-	// calculate byte position
-	uByteIdx = byHashIdx / 8;
-	ASSERT(uByteIdx < 8);
-	// calculate bit position
-	byBitMask = 1;
-	byBitMask <<= (byHashIdx % 8);
-	// turn off the bit
-	byOrgValue = MACbyReadMultiAddr(dwIoBase, uByteIdx);
-	MACvWriteMultiAddr(dwIoBase, uByteIdx, (unsigned char)(byOrgValue & (~byBitMask)));
-}
-
-/*
- * Description:
- *      Set Rx Threshold
- *
- * Parameters:
- *  In:
- *      dwIoBase    - Base Address for MAC
- *      byThreshold - Threshold Value
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void MACvSetRxThreshold(void __iomem *dwIoBase, unsigned char byThreshold)
-{
-	unsigned char byOrgValue;
-
-	ASSERT(byThreshold < 4);
-
-	// set FCR0
-	VNSvInPortB(dwIoBase + MAC_REG_FCR0, &byOrgValue);
-	byOrgValue = (byOrgValue & 0xCF) | (byThreshold << 4);
-	VNSvOutPortB(dwIoBase + MAC_REG_FCR0, byOrgValue);
-}
-
-/*
- * Description:
- *      Get Rx Threshold
- *
- * Parameters:
- *  In:
- *      dwIoBase    - Base Address for MAC
- *  Out:
- *      pbyThreshold- Threshold Value Get
- *
- * Return Value: none
- *
- */
-void MACvGetRxThreshold(void __iomem *dwIoBase, unsigned char *pbyThreshold)
-{
-	// get FCR0
-	VNSvInPortB(dwIoBase + MAC_REG_FCR0, pbyThreshold);
-	*pbyThreshold = (*pbyThreshold >> 4) & 0x03;
-}
-
-/*
- * Description:
- *      Set Tx Threshold
- *
- * Parameters:
- *  In:
- *      dwIoBase    - Base Address for MAC
- *      byThreshold - Threshold Value
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void MACvSetTxThreshold(void __iomem *dwIoBase, unsigned char byThreshold)
-{
-	unsigned char byOrgValue;
-
-	ASSERT(byThreshold < 4);
-
-	// set FCR0
-	VNSvInPortB(dwIoBase + MAC_REG_FCR0, &byOrgValue);
-	byOrgValue = (byOrgValue & 0xF3) | (byThreshold << 2);
-	VNSvOutPortB(dwIoBase + MAC_REG_FCR0, byOrgValue);
-}
-
-/*
- * Description:
- *      Get Tx Threshold
- *
- * Parameters:
- *  In:
- *      dwIoBase    - Base Address for MAC
- *  Out:
- *      pbyThreshold- Threshold Value Get
- *
- * Return Value: none
- *
- */
-void MACvGetTxThreshold(void __iomem *dwIoBase, unsigned char *pbyThreshold)
-{
-	// get FCR0
-	VNSvInPortB(dwIoBase + MAC_REG_FCR0, pbyThreshold);
-	*pbyThreshold = (*pbyThreshold >> 2) & 0x03;
-}
-
-/*
- * Description:
- *      Set Dma Length
- *
- * Parameters:
- *  In:
- *      dwIoBase    - Base Address for MAC
- *      byDmaLength - Dma Length Value
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void MACvSetDmaLength(void __iomem *dwIoBase, unsigned char byDmaLength)
-{
-	unsigned char byOrgValue;
-
-	ASSERT(byDmaLength < 4);
-
-	// set FCR0
-	VNSvInPortB(dwIoBase + MAC_REG_FCR0, &byOrgValue);
-	byOrgValue = (byOrgValue & 0xFC) | byDmaLength;
-	VNSvOutPortB(dwIoBase + MAC_REG_FCR0, byOrgValue);
-}
-
-/*
- * Description:
- *      Get Dma Length
- *
- * Parameters:
- *  In:
- *      dwIoBase    - Base Address for MAC
- *  Out:
- *      pbyDmaLength- Dma Length Value Get
- *
- * Return Value: none
- *
- */
-void MACvGetDmaLength(void __iomem *dwIoBase, unsigned char *pbyDmaLength)
-{
-	// get FCR0
-	VNSvInPortB(dwIoBase + MAC_REG_FCR0, pbyDmaLength);
-	*pbyDmaLength &= 0x03;
-}
-
-/*
- * Description:
  *      Set 802.11 Short Retry Limit
  *
  * Parameters:
@@ -494,25 +187,6 @@ void MACvSetLongRetryLimit(void __iomem *dwIoBase, unsigned char byRetryLimit)
 
 /*
  * Description:
- *      Get 802.11 Long Retry Limit
- *
- * Parameters:
- *  In:
- *      dwIoBase        - Base Address for MAC
- *  Out:
- *      pbyRetryLimit   - Retry Limit Get
- *
- * Return Value: none
- *
- */
-void MACvGetLongRetryLimit(void __iomem *dwIoBase, unsigned char *pbyRetryLimit)
-{
-	// get LRT
-	VNSvInPortB(dwIoBase + MAC_REG_LRT, pbyRetryLimit);
-}
-
-/*
- * Description:
  *      Set MAC Loopback mode
  *
  * Parameters:
@@ -540,89 +214,6 @@ void MACvSetLoopbackMode(void __iomem *dwIoBase, unsigned char byLoopbackMode)
 
 /*
  * Description:
- *      Test if MAC in Loopback mode
- *
- * Parameters:
- *  In:
- *      dwIoBase        - Base Address for MAC
- *  Out:
- *      none
- *
- * Return Value: true if in Loopback mode; otherwise false
- *
- */
-bool MACbIsInLoopbackMode(void __iomem *dwIoBase)
-{
-	unsigned char byOrgValue;
-
-	VNSvInPortB(dwIoBase + MAC_REG_TEST, &byOrgValue);
-	if (byOrgValue & (TEST_LBINT | TEST_LBEXT))
-		return true;
-	return false;
-}
-
-/*
- * Description:
- *      Set MAC Address filter
- *
- * Parameters:
- *  In:
- *      dwIoBase        - Base Address for MAC
- *      wFilterType     - Filter Type
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void MACvSetPacketFilter(void __iomem *dwIoBase, unsigned short wFilterType)
-{
-	unsigned char byOldRCR;
-	unsigned char byNewRCR = 0;
-
-	// if only in DIRECTED mode, multicast-address will set to zero,
-	// but if other mode exist (e.g. PROMISCUOUS), multicast-address
-	// will be open
-	if (wFilterType & PKT_TYPE_DIRECTED) {
-		// set multicast address to accept none
-		MACvSelectPage1(dwIoBase);
-		VNSvOutPortD(dwIoBase + MAC_REG_MAR0, 0L);
-		VNSvOutPortD(dwIoBase + MAC_REG_MAR0 + sizeof(unsigned long), 0L);
-		MACvSelectPage0(dwIoBase);
-	}
-
-	if (wFilterType & (PKT_TYPE_PROMISCUOUS | PKT_TYPE_ALL_MULTICAST)) {
-		// set multicast address to accept all
-		MACvSelectPage1(dwIoBase);
-		VNSvOutPortD(dwIoBase + MAC_REG_MAR0, 0xFFFFFFFFL);
-		VNSvOutPortD(dwIoBase + MAC_REG_MAR0 + sizeof(unsigned long), 0xFFFFFFFFL);
-		MACvSelectPage0(dwIoBase);
-	}
-
-	if (wFilterType & PKT_TYPE_PROMISCUOUS) {
-		byNewRCR |= (RCR_RXALLTYPE | RCR_UNICAST | RCR_MULTICAST | RCR_BROADCAST);
-
-		byNewRCR &= ~RCR_BSSID;
-	}
-
-	if (wFilterType & (PKT_TYPE_ALL_MULTICAST | PKT_TYPE_MULTICAST))
-		byNewRCR |= RCR_MULTICAST;
-
-	if (wFilterType & PKT_TYPE_BROADCAST)
-		byNewRCR |= RCR_BROADCAST;
-
-	if (wFilterType & PKT_TYPE_ERROR_CRC)
-		byNewRCR |= RCR_ERRCRC;
-
-	VNSvInPortB(dwIoBase + MAC_REG_RCR,  &byOldRCR);
-	if (byNewRCR != byOldRCR) {
-		// Modify the Receive Command Register
-		VNSvOutPortB(dwIoBase + MAC_REG_RCR, byNewRCR);
-	}
-}
-
-/*
- * Description:
  *      Save MAC registers to context buffer
  *
  * Parameters:
@@ -702,47 +293,6 @@ void MACvRestoreContext(void __iomem *dwIoBase, unsigned char *pbyCxtBuf)
 
 /*
  * Description:
- *      Compare if MAC registers same as context buffer
- *
- * Parameters:
- *  In:
- *      dwIoBase    - Base Address for MAC
- *      pbyCxtBuf   - Context buffer
- *  Out:
- *      none
- *
- * Return Value: true if all values are the same; otherwise false
- *
- */
-bool MACbCompareContext(void __iomem *dwIoBase, unsigned char *pbyCxtBuf)
-{
-	unsigned long dwData;
-
-	// compare MAC context to determine if this is a power lost init,
-	// return true for power remaining init, return false for power lost init
-
-	// compare CURR_RX_DESC_ADDR, CURR_TX_DESC_ADDR
-	VNSvInPortD(dwIoBase + MAC_REG_TXDMAPTR0, &dwData);
-	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_TXDMAPTR0))
-		return false;
-
-	VNSvInPortD(dwIoBase + MAC_REG_AC0DMAPTR, &dwData);
-	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_AC0DMAPTR))
-		return false;
-
-	VNSvInPortD(dwIoBase + MAC_REG_RXDMAPTR0, &dwData);
-	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR0))
-		return false;
-
-	VNSvInPortD(dwIoBase + MAC_REG_RXDMAPTR1, &dwData);
-	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR1))
-		return false;
-
-	return true;
-}
-
-/*
- * Description:
  *      Software Reset MAC
  *
  * Parameters:
@@ -1018,11 +568,6 @@ void MACvInitialize(void __iomem *dwIoBase)
 	VNSvOutPortB(dwIoBase + MAC_REG_TFTCTL, TFTCTL_TSFCNTRST);
 	// enable TSF counter
 	VNSvOutPortB(dwIoBase + MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
-
-	// set packet filter
-	// receive directed and broadcast address
-
-	MACvSetPacketFilter(dwIoBase, PKT_TYPE_DIRECTED | PKT_TYPE_BROADCAST);
 }
 
 /*
@@ -1234,27 +779,6 @@ void MACvTimer0MicroSDelay(void __iomem *dwIoBase, unsigned int uDelay)
  * Return Value: none
  *
  */
-void MACvOneShotTimer0MicroSec(void __iomem *dwIoBase, unsigned int uDelayTime)
-{
-	VNSvOutPortB(dwIoBase + MAC_REG_TMCTL0, 0);
-	VNSvOutPortD(dwIoBase + MAC_REG_TMDATA0, uDelayTime);
-	VNSvOutPortB(dwIoBase + MAC_REG_TMCTL0, (TMCTL_TMD | TMCTL_TE));
-}
-
-/*
- * Description:
- *      Micro Second One shot timer via MAC
- *
- * Parameters:
- *  In:
- *      dwIoBase    - Base Address for MAC
- *      uDelay      - Delay time
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
 void MACvOneShotTimer1MicroSec(void __iomem *dwIoBase, unsigned int uDelayTime)
 {
 	VNSvOutPortB(dwIoBase + MAC_REG_TMCTL1, 0);
@@ -1271,102 +795,6 @@ void MACvSetMISCFifo(void __iomem *dwIoBase, unsigned short wOffset, unsigned lo
 	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
 }
 
-bool MACbTxDMAOff(void __iomem *dwIoBase, unsigned int idx)
-{
-	unsigned char byData;
-	unsigned int ww = 0;
-
-	if (idx == TYPE_TXDMA0) {
-		VNSvOutPortB(dwIoBase + MAC_REG_TXDMACTL0+2, DMACTL_RUN);
-		for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-			VNSvInPortB(dwIoBase + MAC_REG_TXDMACTL0, &byData);
-			if (!(byData & DMACTL_RUN))
-				break;
-		}
-	} else if (idx == TYPE_AC0DMA) {
-		VNSvOutPortB(dwIoBase + MAC_REG_AC0DMACTL+2, DMACTL_RUN);
-		for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-			VNSvInPortB(dwIoBase + MAC_REG_AC0DMACTL, &byData);
-			if (!(byData & DMACTL_RUN))
-				break;
-		}
-	}
-	if (ww == W_MAX_TIMEOUT) {
-		DBG_PORT80(0x29);
-		pr_debug(" DBG_PORT80(0x29)\n");
-		return false;
-	}
-	return true;
-}
-
-void MACvClearBusSusInd(void __iomem *dwIoBase)
-{
-	unsigned long dwOrgValue;
-	unsigned int ww;
-	// check if BcnSusInd enabled
-	VNSvInPortD(dwIoBase + MAC_REG_ENCFG , &dwOrgValue);
-	if (!(dwOrgValue & EnCFG_BcnSusInd))
-		return;
-	//Set BcnSusClr
-	dwOrgValue = dwOrgValue | EnCFG_BcnSusClr;
-	VNSvOutPortD(dwIoBase + MAC_REG_ENCFG, dwOrgValue);
-	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-		VNSvInPortD(dwIoBase + MAC_REG_ENCFG , &dwOrgValue);
-		if (!(dwOrgValue & EnCFG_BcnSusInd))
-			break;
-	}
-	if (ww == W_MAX_TIMEOUT) {
-		DBG_PORT80(0x33);
-		pr_debug(" DBG_PORT80(0x33)\n");
-	}
-}
-
-void MACvEnableBusSusEn(void __iomem *dwIoBase)
-{
-	unsigned char byOrgValue;
-	unsigned long dwOrgValue;
-	unsigned int ww;
-	// check if BcnSusInd enabled
-	VNSvInPortB(dwIoBase + MAC_REG_CFG , &byOrgValue);
-
-	//Set BcnSusEn
-	byOrgValue = byOrgValue | CFG_BCNSUSEN;
-	VNSvOutPortB(dwIoBase + MAC_REG_ENCFG, byOrgValue);
-	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-		VNSvInPortD(dwIoBase + MAC_REG_ENCFG , &dwOrgValue);
-		if (dwOrgValue & EnCFG_BcnSusInd)
-			break;
-	}
-	if (ww == W_MAX_TIMEOUT) {
-		DBG_PORT80(0x34);
-		pr_debug(" DBG_PORT80(0x34)\n");
-	}
-}
-
-bool MACbFlushSYNCFifo(void __iomem *dwIoBase)
-{
-	unsigned char byOrgValue;
-	unsigned int ww;
-	// Read MACCR
-	VNSvInPortB(dwIoBase + MAC_REG_MACCR , &byOrgValue);
-
-	// Set SYNCFLUSH
-	byOrgValue = byOrgValue | MACCR_SYNCFLUSH;
-	VNSvOutPortB(dwIoBase + MAC_REG_MACCR, byOrgValue);
-
-	// Check if SyncFlushOK
-	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-		VNSvInPortB(dwIoBase + MAC_REG_MACCR , &byOrgValue);
-		if (byOrgValue & MACCR_SYNCFLUSHOK)
-			break;
-	}
-	if (ww == W_MAX_TIMEOUT) {
-		DBG_PORT80(0x35);
-		pr_debug(" DBG_PORT80(0x33)\n");
-	}
-	return true;
-}
-
 bool MACbPSWakeup(void __iomem *dwIoBase)
 {
 	unsigned char byOrgValue;
@@ -1484,211 +912,3 @@ void MACvDisableKeyEntry(void __iomem *dwIoBase, unsigned int uEntryIdx)
 	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, 0);
 	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
 }
-
-/*
- * Description:
- *      Set the default Key (KeyEntry[10]) by MISCFIFO
- *
- * Parameters:
- *  In:
- *      dwIoBase        - Base Address for MAC
- *
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-
-void MACvSetDefaultKeyEntry(void __iomem *dwIoBase, unsigned int uKeyLen,
-			    unsigned int uKeyIdx, unsigned long *pdwKey, unsigned char byLocalID)
-{
-	unsigned short wOffset;
-	unsigned long dwData;
-	int     ii;
-
-	if (byLocalID <= 1)
-		return;
-
-	pr_debug("MACvSetDefaultKeyEntry\n");
-	wOffset = MISCFIFO_KEYETRY0;
-	wOffset += (10 * MISCFIFO_KEYENTRYSIZE);
-
-	wOffset++;
-	wOffset++;
-	wOffset += (uKeyIdx * 4);
-	// always push 128 bits
-	for (ii = 0; ii < 3; ii++) {
-		pr_debug("(%d) wOffset: %d, Data: %lX\n",
-			 ii, wOffset+ii, *pdwKey);
-		VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+ii);
-		VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, *pdwKey++);
-		VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-	}
-	dwData = *pdwKey;
-	if (uKeyLen == WLAN_WEP104_KEYLEN)
-		dwData |= 0x80000000;
-
-	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+3);
-	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
-	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-	pr_debug("End. wOffset: %d, Data: %lX\n", wOffset+3, dwData);
-}
-
-/*
- * Description:
- *      Enable default Key (KeyEntry[10]) by MISCFIFO
- *
- * Parameters:
- *  In:
- *      dwIoBase        - Base Address for MAC
- *
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-/*
-  void MACvEnableDefaultKey(void __iomem *dwIoBase, unsigned char byLocalID)
-  {
-  unsigned short wOffset;
-  unsigned long dwData;
-
-  if (byLocalID <= 1)
-  return;
-
-  wOffset = MISCFIFO_KEYETRY0;
-  wOffset += (10 * MISCFIFO_KEYENTRYSIZE);
-
-  dwData = 0xC0440000;
-  VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
-  VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
-  VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-  pr_debug("MACvEnableDefaultKey: wOffset: %d, Data: %lX\n", wOffset, dwData);
-
-  }
-*/
-
-/*
- * Description:
- *      Disable default Key (KeyEntry[10]) by MISCFIFO
- *
- * Parameters:
- *  In:
- *      dwIoBase        - Base Address for MAC
- *
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void MACvDisableDefaultKey(void __iomem *dwIoBase)
-{
-	unsigned short wOffset;
-	unsigned long dwData;
-
-	wOffset = MISCFIFO_KEYETRY0;
-	wOffset += (10 * MISCFIFO_KEYENTRYSIZE);
-
-	dwData = 0x0;
-	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
-	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
-	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-	pr_debug("MACvDisableDefaultKey: wOffset: %d, Data: %lX\n",
-		 wOffset, dwData);
-}
-
-/*
- * Description:
- *      Set the default TKIP Group Key (KeyEntry[10]) by MISCFIFO
- *
- * Parameters:
- *  In:
- *      dwIoBase        - Base Address for MAC
- *
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void MACvSetDefaultTKIPKeyEntry(void __iomem *dwIoBase, unsigned int uKeyLen,
-				unsigned int uKeyIdx, unsigned long *pdwKey, unsigned char byLocalID)
-{
-	unsigned short wOffset;
-	unsigned long dwData;
-	int     ii;
-
-	if (byLocalID <= 1)
-		return;
-
-	pr_debug("MACvSetDefaultTKIPKeyEntry\n");
-	wOffset = MISCFIFO_KEYETRY0;
-	// Kyle test : change offset from 10 -> 0
-	wOffset += (10 * MISCFIFO_KEYENTRYSIZE);
-
-	dwData = 0xC0660000;
-	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
-	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
-	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-	wOffset++;
-
-	dwData = 0;
-	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
-	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
-	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-	wOffset++;
-
-	wOffset += (uKeyIdx * 4);
-	pr_debug("1. wOffset: %d, Data: %lX, idx:%d\n",
-		 wOffset, *pdwKey, uKeyIdx);
-	// always push 128 bits
-	for (ii = 0; ii < 4; ii++) {
-		pr_debug("2.(%d) wOffset: %d, Data: %lX\n",
-			 ii, wOffset+ii, *pdwKey);
-		VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+ii);
-		VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, *pdwKey++);
-		VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-	}
-}
-
-/*
- * Description:
- *      Set the Key Control by MISCFIFO
- *
- * Parameters:
- *  In:
- *      dwIoBase        - Base Address for MAC
- *
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-
-void MACvSetDefaultKeyCtl(void __iomem *dwIoBase, unsigned short wKeyCtl, unsigned int uEntryIdx, unsigned char byLocalID)
-{
-	unsigned short wOffset;
-	unsigned long dwData;
-
-	if (byLocalID <= 1)
-		return;
-
-	pr_debug("MACvSetKeyEntry\n");
-	wOffset = MISCFIFO_KEYETRY0;
-	wOffset += (uEntryIdx * MISCFIFO_KEYENTRYSIZE);
-
-	dwData = 0;
-	dwData |= wKeyCtl;
-	dwData <<= 16;
-	dwData |= 0xffff;
-	pr_debug("1. wOffset: %d, Data: %lX, KeyCtl:%X\n",
-		 wOffset, dwData, wKeyCtl);
-
-	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
-	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
-	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-}
diff --git a/drivers/staging/vt6655/mac.h b/drivers/staging/vt6655/mac.h
index 0bf93759b6af..e1e7e10435f6 100644
--- a/drivers/staging/vt6655/mac.h
+++ b/drivers/staging/vt6655/mac.h
@@ -34,7 +34,6 @@
 #ifndef __MAC_H__
 #define __MAC_H__
 
-#include "ttype.h"
 #include "tmacro.h"
 #include "upc.h"
 
@@ -575,17 +574,6 @@
 #define MAC_LB_INTERNAL     0x01        //
 #define MAC_LB_NONE         0x00        //
 
-// Ethernet address filter type
-#define PKT_TYPE_NONE           0x00    // turn off receiver
-#define PKT_TYPE_ALL_MULTICAST  0x80
-#define PKT_TYPE_PROMISCUOUS    0x40
-#define PKT_TYPE_DIRECTED       0x20    // obsolete, directed address is always accepted
-#define PKT_TYPE_BROADCAST      0x10
-#define PKT_TYPE_MULTICAST      0x08
-#define PKT_TYPE_ERROR_WPA      0x04
-#define PKT_TYPE_ERROR_CRC      0x02
-#define PKT_TYPE_BSSID          0x01
-
 #define Default_BI              0x200
 
 // MiscFIFO Offset
@@ -965,48 +953,20 @@ do {								\
 #define MACvSetRFLE_LatchBase(dwIoBase)                                 \
 	MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_RFLEOPT)
 
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-extern unsigned short TxRate_iwconfig;//2008-5-8 <add> by chester
-void MACvReadAllRegs(void __iomem *dwIoBase, unsigned char *pbyMacRegs);
-
 bool MACbIsRegBitsOn(void __iomem *dwIoBase, unsigned char byRegOfs, unsigned char byTestBits);
 bool MACbIsRegBitsOff(void __iomem *dwIoBase, unsigned char byRegOfs, unsigned char byTestBits);
 
 bool MACbIsIntDisable(void __iomem *dwIoBase);
 
-unsigned char MACbyReadMultiAddr(void __iomem *dwIoBase, unsigned int uByteIdx);
-void MACvWriteMultiAddr(void __iomem *dwIoBase, unsigned int uByteIdx, unsigned char byData);
-void MACvSetMultiAddrByHash(void __iomem *dwIoBase, unsigned char byHashIdx);
-void MACvResetMultiAddrByHash(void __iomem *dwIoBase, unsigned char byHashIdx);
-
-void MACvSetRxThreshold(void __iomem *dwIoBase, unsigned char byThreshold);
-void MACvGetRxThreshold(void __iomem *dwIoBase, unsigned char *pbyThreshold);
-
-void MACvSetTxThreshold(void __iomem *dwIoBase, unsigned char byThreshold);
-void MACvGetTxThreshold(void __iomem *dwIoBase, unsigned char *pbyThreshold);
-
-void MACvSetDmaLength(void __iomem *dwIoBase, unsigned char byDmaLength);
-void MACvGetDmaLength(void __iomem *dwIoBase, unsigned char *pbyDmaLength);
-
 void MACvSetShortRetryLimit(void __iomem *dwIoBase, unsigned char byRetryLimit);
-void MACvGetShortRetryLimit(void __iomem *dwIoBase, unsigned char *pbyRetryLimit);
 
 void MACvSetLongRetryLimit(void __iomem *dwIoBase, unsigned char byRetryLimit);
 void MACvGetLongRetryLimit(void __iomem *dwIoBase, unsigned char *pbyRetryLimit);
 
 void MACvSetLoopbackMode(void __iomem *dwIoBase, unsigned char byLoopbackMode);
-bool MACbIsInLoopbackMode(void __iomem *dwIoBase);
-
-void MACvSetPacketFilter(void __iomem *dwIoBase, unsigned short wFilterType);
 
 void MACvSaveContext(void __iomem *dwIoBase, unsigned char *pbyCxtBuf);
 void MACvRestoreContext(void __iomem *dwIoBase, unsigned char *pbyCxtBuf);
-bool MACbCompareContext(void __iomem *dwIoBase, unsigned char *pbyCxtBuf);
 
 bool MACbSoftwareReset(void __iomem *dwIoBase);
 bool MACbSafeSoftwareReset(void __iomem *dwIoBase);
@@ -1023,27 +983,14 @@ void MACvSetCurrAC0DescAddrEx(void __iomem *dwIoBase, unsigned long dwCurrDescAd
 void MACvSetCurrSyncDescAddrEx(void __iomem *dwIoBase, unsigned long dwCurrDescAddr);
 void MACvSetCurrATIMDescAddrEx(void __iomem *dwIoBase, unsigned long dwCurrDescAddr);
 void MACvTimer0MicroSDelay(void __iomem *dwIoBase, unsigned int uDelay);
-void MACvOneShotTimer0MicroSec(void __iomem *dwIoBase, unsigned int uDelayTime);
 void MACvOneShotTimer1MicroSec(void __iomem *dwIoBase, unsigned int uDelayTime);
 
 void MACvSetMISCFifo(void __iomem *dwIoBase, unsigned short wOffset, unsigned long dwData);
 
-bool MACbTxDMAOff(void __iomem *dwIoBase, unsigned int idx);
-
-void MACvClearBusSusInd(void __iomem *dwIoBase);
-void MACvEnableBusSusEn(void __iomem *dwIoBase);
-
-bool MACbFlushSYNCFifo(void __iomem *dwIoBase);
 bool MACbPSWakeup(void __iomem *dwIoBase);
 
 void MACvSetKeyEntry(void __iomem *dwIoBase, unsigned short wKeyCtl, unsigned int uEntryIdx,
 		     unsigned int uKeyIdx, unsigned char *pbyAddr, u32 *pdwKey, unsigned char byLocalID);
 void MACvDisableKeyEntry(void __iomem *dwIoBase, unsigned int uEntryIdx);
-void MACvSetDefaultKeyEntry(void __iomem *dwIoBase, unsigned int uKeyLen,
-			    unsigned int uKeyIdx, unsigned long *pdwKey, unsigned char byLocalID);
-void MACvDisableDefaultKey(void __iomem *dwIoBase);
-void MACvSetDefaultTKIPKeyEntry(void __iomem *dwIoBase, unsigned int uKeyLen,
-				unsigned int uKeyIdx, unsigned long *pdwKey, unsigned char byLocalID);
-void MACvSetDefaultKeyCtl(void __iomem *dwIoBase, unsigned short wKeyCtl, unsigned int uEntryIdx, unsigned char byLocalID);
 
 #endif // __MAC_H__
diff --git a/drivers/staging/vt6655/mib.c b/drivers/staging/vt6655/mib.c
index 111c01877086..d2f351d19ff8 100644
--- a/drivers/staging/vt6655/mib.c
+++ b/drivers/staging/vt6655/mib.c
@@ -25,24 +25,15 @@
  * Date: May 21, 1996
  *
  * Functions:
- *      STAvClearAllCounter - Clear All MIB Counter
  *      STAvUpdateIstStatCounter - Update ISR statistic counter
- *      STAvUpdateRDStatCounter - Update Rx statistic counter
- *      STAvUpdateRDStatCounterEx - Update Rx statistic counter and copy rcv data
- *      STAvUpdateTDStatCounter - Update Tx statistic counter
- *      STAvUpdateTDStatCounterEx - Update Tx statistic counter and copy tx data
  *      STAvUpdate802_11Counter - Update 802.11 mib counter
  *
  * Revision History:
  *
  */
 
-#include "upc.h"
 #include "mac.h"
-#include "tether.h"
 #include "mib.h"
-#include "wctl.h"
-#include "baseband.h"
 
 /*---------------------  Static Classes  ----------------------------*/
 
@@ -55,24 +46,6 @@
 /*---------------------  Export Functions  --------------------------*/
 
 /*
- * Description: Clear All Statistic Counter
- *
- * Parameters:
- *  In:
- *      pStatistic  - Pointer to Statistic Counter Data Structure
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void STAvClearAllCounter(PSStatCounter pStatistic)
-{
-	// set memory to zero
-	memset(pStatistic, 0, sizeof(SStatCounter));
-}
-
-/*
  * Description: Update Isr Statistic Counter
  *
  * Parameters:
@@ -139,373 +112,6 @@ void STAvUpdateIsrStatCounter(PSStatCounter pStatistic, unsigned long dwIsr)
 }
 
 /*
- * Description: Update Rx Statistic Counter
- *
- * Parameters:
- *  In:
- *      pStatistic      - Pointer to Statistic Counter Data Structure
- *      byRSR           - Rx Status
- *      byNewRSR        - Rx Status
- *      pbyBuffer       - Rx Buffer
- *      cbFrameLength   - Rx Length
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void STAvUpdateRDStatCounter(PSStatCounter pStatistic,
-			     unsigned char byRSR, unsigned char byNewRSR, unsigned char byRxRate,
-			     unsigned char *pbyBuffer, unsigned int cbFrameLength)
-{
-	//need change
-	PS802_11Header pHeader = (PS802_11Header)pbyBuffer;
-
-	if (byRSR & RSR_ADDROK)
-		pStatistic->dwRsrADDROk++;
-	if (byRSR & RSR_CRCOK) {
-		pStatistic->dwRsrCRCOk++;
-
-		pStatistic->ullRsrOK++;
-
-		if (cbFrameLength >= ETH_ALEN) {
-			// update counters in case of successful transmit
-			if (byRSR & RSR_ADDRBROAD) {
-				pStatistic->ullRxBroadcastFrames++;
-				pStatistic->ullRxBroadcastBytes += (unsigned long long) cbFrameLength;
-			} else if (byRSR & RSR_ADDRMULTI) {
-				pStatistic->ullRxMulticastFrames++;
-				pStatistic->ullRxMulticastBytes += (unsigned long long) cbFrameLength;
-			} else {
-				pStatistic->ullRxDirectedFrames++;
-				pStatistic->ullRxDirectedBytes += (unsigned long long) cbFrameLength;
-			}
-		}
-	}
-
-	if (byRxRate == 22) {
-		pStatistic->CustomStat.ullRsr11M++;
-		if (byRSR & RSR_CRCOK)
-			pStatistic->CustomStat.ullRsr11MCRCOk++;
-
-		pr_debug("11M: ALL[%d], OK[%d]:[%02x]\n",
-			 (int)pStatistic->CustomStat.ullRsr11M,
-			 (int)pStatistic->CustomStat.ullRsr11MCRCOk, byRSR);
-	} else if (byRxRate == 11) {
-		pStatistic->CustomStat.ullRsr5M++;
-		if (byRSR & RSR_CRCOK)
-			pStatistic->CustomStat.ullRsr5MCRCOk++;
-
-		pr_debug(" 5M: ALL[%d], OK[%d]:[%02x]\n",
-			 (int)pStatistic->CustomStat.ullRsr5M,
-			 (int)pStatistic->CustomStat.ullRsr5MCRCOk, byRSR);
-	} else if (byRxRate == 4) {
-		pStatistic->CustomStat.ullRsr2M++;
-		if (byRSR & RSR_CRCOK)
-			pStatistic->CustomStat.ullRsr2MCRCOk++;
-
-		pr_debug(" 2M: ALL[%d], OK[%d]:[%02x]\n",
-			 (int)pStatistic->CustomStat.ullRsr2M,
-			 (int)pStatistic->CustomStat.ullRsr2MCRCOk, byRSR);
-	} else if (byRxRate == 2) {
-		pStatistic->CustomStat.ullRsr1M++;
-		if (byRSR & RSR_CRCOK)
-			pStatistic->CustomStat.ullRsr1MCRCOk++;
-
-		pr_debug(" 1M: ALL[%d], OK[%d]:[%02x]\n",
-			 (int)pStatistic->CustomStat.ullRsr1M,
-			 (int)pStatistic->CustomStat.ullRsr1MCRCOk, byRSR);
-	} else if (byRxRate == 12) {
-		pStatistic->CustomStat.ullRsr6M++;
-		if (byRSR & RSR_CRCOK)
-			pStatistic->CustomStat.ullRsr6MCRCOk++;
-
-		pr_debug(" 6M: ALL[%d], OK[%d]\n",
-			 (int)pStatistic->CustomStat.ullRsr6M,
-			 (int)pStatistic->CustomStat.ullRsr6MCRCOk);
-	} else if (byRxRate == 18) {
-		pStatistic->CustomStat.ullRsr9M++;
-		if (byRSR & RSR_CRCOK)
-			pStatistic->CustomStat.ullRsr9MCRCOk++;
-
-		pr_debug(" 9M: ALL[%d], OK[%d]\n",
-			 (int)pStatistic->CustomStat.ullRsr9M,
-			 (int)pStatistic->CustomStat.ullRsr9MCRCOk);
-	} else if (byRxRate == 24) {
-		pStatistic->CustomStat.ullRsr12M++;
-		if (byRSR & RSR_CRCOK)
-			pStatistic->CustomStat.ullRsr12MCRCOk++;
-
-		pr_debug("12M: ALL[%d], OK[%d]\n",
-			 (int)pStatistic->CustomStat.ullRsr12M,
-			 (int)pStatistic->CustomStat.ullRsr12MCRCOk);
-	} else if (byRxRate == 36) {
-		pStatistic->CustomStat.ullRsr18M++;
-		if (byRSR & RSR_CRCOK)
-			pStatistic->CustomStat.ullRsr18MCRCOk++;
-
-		pr_debug("18M: ALL[%d], OK[%d]\n",
-			 (int)pStatistic->CustomStat.ullRsr18M,
-			 (int)pStatistic->CustomStat.ullRsr18MCRCOk);
-	} else if (byRxRate == 48) {
-		pStatistic->CustomStat.ullRsr24M++;
-		if (byRSR & RSR_CRCOK)
-			pStatistic->CustomStat.ullRsr24MCRCOk++;
-
-		pr_debug("24M: ALL[%d], OK[%d]\n",
-			 (int)pStatistic->CustomStat.ullRsr24M,
-			 (int)pStatistic->CustomStat.ullRsr24MCRCOk);
-	} else if (byRxRate == 72) {
-		pStatistic->CustomStat.ullRsr36M++;
-		if (byRSR & RSR_CRCOK)
-			pStatistic->CustomStat.ullRsr36MCRCOk++;
-
-		pr_debug("36M: ALL[%d], OK[%d]\n",
-			 (int)pStatistic->CustomStat.ullRsr36M,
-			 (int)pStatistic->CustomStat.ullRsr36MCRCOk);
-	} else if (byRxRate == 96) {
-		pStatistic->CustomStat.ullRsr48M++;
-		if (byRSR & RSR_CRCOK)
-			pStatistic->CustomStat.ullRsr48MCRCOk++;
-
-		pr_debug("48M: ALL[%d], OK[%d]\n",
-			 (int)pStatistic->CustomStat.ullRsr48M,
-			 (int)pStatistic->CustomStat.ullRsr48MCRCOk);
-	} else if (byRxRate == 108) {
-		pStatistic->CustomStat.ullRsr54M++;
-		if (byRSR & RSR_CRCOK)
-			pStatistic->CustomStat.ullRsr54MCRCOk++;
-
-		pr_debug("54M: ALL[%d], OK[%d]\n",
-			 (int)pStatistic->CustomStat.ullRsr54M,
-			 (int)pStatistic->CustomStat.ullRsr54MCRCOk);
-	} else {
-		pr_debug("Unknown: Total[%d], CRCOK[%d]\n",
-			 (int)pStatistic->dwRsrRxPacket+1,
-			 (int)pStatistic->dwRsrCRCOk);
-	}
-
-	if (byRSR & RSR_BSSIDOK)
-		pStatistic->dwRsrBSSIDOk++;
-
-	if (byRSR & RSR_BCNSSIDOK)
-		pStatistic->dwRsrBCNSSIDOk++;
-	if (byRSR & RSR_IVLDLEN)  //invalid len (> 2312 byte)
-		pStatistic->dwRsrLENErr++;
-	if (byRSR & RSR_IVLDTYP)  //invalid packet type
-		pStatistic->dwRsrTYPErr++;
-	if (byRSR & (RSR_IVLDTYP | RSR_IVLDLEN))
-		pStatistic->dwRsrErr++;
-
-	if (byNewRSR & NEWRSR_DECRYPTOK)
-		pStatistic->dwNewRsrDECRYPTOK++;
-	if (byNewRSR & NEWRSR_CFPIND)
-		pStatistic->dwNewRsrCFP++;
-	if (byNewRSR & NEWRSR_HWUTSF)
-		pStatistic->dwNewRsrUTSF++;
-	if (byNewRSR & NEWRSR_BCNHITAID)
-		pStatistic->dwNewRsrHITAID++;
-	if (byNewRSR & NEWRSR_BCNHITAID0)
-		pStatistic->dwNewRsrHITAID0++;
-
-	// increase rx packet count
-	pStatistic->dwRsrRxPacket++;
-	pStatistic->dwRsrRxOctet += cbFrameLength;
-
-	if (IS_TYPE_DATA(pbyBuffer))
-		pStatistic->dwRsrRxData++;
-	else if (IS_TYPE_MGMT(pbyBuffer))
-		pStatistic->dwRsrRxManage++;
-	else if (IS_TYPE_CONTROL(pbyBuffer))
-		pStatistic->dwRsrRxControl++;
-
-	if (byRSR & RSR_ADDRBROAD)
-		pStatistic->dwRsrBroadcast++;
-	else if (byRSR & RSR_ADDRMULTI)
-		pStatistic->dwRsrMulticast++;
-	else
-		pStatistic->dwRsrDirected++;
-
-	if (WLAN_GET_FC_MOREFRAG(pHeader->wFrameCtl))
-		pStatistic->dwRsrRxFragment++;
-
-	if (cbFrameLength < ETH_ZLEN + 4)
-		pStatistic->dwRsrRunt++;
-	else if (cbFrameLength == ETH_ZLEN + 4)
-		pStatistic->dwRsrRxFrmLen64++;
-	else if ((65 <= cbFrameLength) && (cbFrameLength <= 127))
-		pStatistic->dwRsrRxFrmLen65_127++;
-	else if ((128 <= cbFrameLength) && (cbFrameLength <= 255))
-		pStatistic->dwRsrRxFrmLen128_255++;
-	else if ((256 <= cbFrameLength) && (cbFrameLength <= 511))
-		pStatistic->dwRsrRxFrmLen256_511++;
-	else if ((512 <= cbFrameLength) && (cbFrameLength <= 1023))
-		pStatistic->dwRsrRxFrmLen512_1023++;
-	else if ((1024 <= cbFrameLength) && (cbFrameLength <= ETH_FRAME_LEN + 4))
-		pStatistic->dwRsrRxFrmLen1024_1518++;
-	else if (cbFrameLength > ETH_FRAME_LEN + 4)
-		pStatistic->dwRsrLong++;
-}
-
-/*
- * Description: Update Rx Statistic Counter and copy Rx buffer
- *
- * Parameters:
- *  In:
- *      pStatistic      - Pointer to Statistic Counter Data Structure
- *      byRSR           - Rx Status
- *      byNewRSR        - Rx Status
- *      pbyBuffer       - Rx Buffer
- *      cbFrameLength   - Rx Length
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-
-void
-STAvUpdateRDStatCounterEx(
-	PSStatCounter   pStatistic,
-	unsigned char byRSR,
-	unsigned char byNewRSR,
-	unsigned char byRxRate,
-	unsigned char *pbyBuffer,
-	unsigned int cbFrameLength
-)
-{
-	STAvUpdateRDStatCounter(
-		pStatistic,
-		byRSR,
-		byNewRSR,
-		byRxRate,
-		pbyBuffer,
-		cbFrameLength
-);
-
-	// rx length
-	pStatistic->dwCntRxFrmLength = cbFrameLength;
-	// rx pattern, we just see 10 bytes for sample
-	memcpy(pStatistic->abyCntRxPattern, (unsigned char *)pbyBuffer, 10);
-}
-
-/*
- * Description: Update Tx Statistic Counter
- *
- * Parameters:
- *  In:
- *      pStatistic      - Pointer to Statistic Counter Data Structure
- *      byTSR0          - Tx Status
- *      byTSR1          - Tx Status
- *      pbyBuffer       - Tx Buffer
- *      cbFrameLength   - Tx Length
- *      uIdx            - Index of Tx DMA
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void
-STAvUpdateTDStatCounter(
-	PSStatCounter   pStatistic,
-	unsigned char byTSR0,
-	unsigned char byTSR1,
-	unsigned char *pbyBuffer,
-	unsigned int cbFrameLength,
-	unsigned int uIdx
-)
-{
-	PWLAN_80211HDR_A4   pHeader;
-	unsigned char *pbyDestAddr;
-	unsigned char byTSR0_NCR = byTSR0 & TSR0_NCR;
-
-	pHeader = (PWLAN_80211HDR_A4) pbyBuffer;
-	if (WLAN_GET_FC_TODS(pHeader->wFrameCtl) == 0)
-		pbyDestAddr = &(pHeader->abyAddr1[0]);
-	else
-		pbyDestAddr = &(pHeader->abyAddr3[0]);
-
-	// increase tx packet count
-	pStatistic->dwTsrTxPacket[uIdx]++;
-	pStatistic->dwTsrTxOctet[uIdx] += cbFrameLength;
-
-	if (byTSR0_NCR != 0) {
-		pStatistic->dwTsrRetry[uIdx]++;
-		pStatistic->dwTsrTotalRetry[uIdx] += byTSR0_NCR;
-
-		if (byTSR0_NCR == 1)
-			pStatistic->dwTsrOnceRetry[uIdx]++;
-		else
-			pStatistic->dwTsrMoreThanOnceRetry[uIdx]++;
-	}
-
-	if ((byTSR1&(TSR1_TERR|TSR1_RETRYTMO|TSR1_TMO|ACK_DATA)) == 0) {
-		pStatistic->ullTsrOK[uIdx]++;
-		pStatistic->CustomStat.ullTsrAllOK =
-			(pStatistic->ullTsrOK[TYPE_AC0DMA] + pStatistic->ullTsrOK[TYPE_TXDMA0]);
-		// update counters in case that successful transmit
-		if (is_broadcast_ether_addr(pbyDestAddr)) {
-			pStatistic->ullTxBroadcastFrames[uIdx]++;
-			pStatistic->ullTxBroadcastBytes[uIdx] += (unsigned long long) cbFrameLength;
-		} else if (is_multicast_ether_addr(pbyDestAddr)) {
-			pStatistic->ullTxMulticastFrames[uIdx]++;
-			pStatistic->ullTxMulticastBytes[uIdx] += (unsigned long long) cbFrameLength;
-		} else {
-			pStatistic->ullTxDirectedFrames[uIdx]++;
-			pStatistic->ullTxDirectedBytes[uIdx] += (unsigned long long) cbFrameLength;
-		}
-	} else {
-		if (byTSR1 & TSR1_TERR)
-			pStatistic->dwTsrErr[uIdx]++;
-		if (byTSR1 & TSR1_RETRYTMO)
-			pStatistic->dwTsrRetryTimeout[uIdx]++;
-		if (byTSR1 & TSR1_TMO)
-			pStatistic->dwTsrTransmitTimeout[uIdx]++;
-		if (byTSR1 & ACK_DATA)
-			pStatistic->dwTsrACKData[uIdx]++;
-	}
-
-	if (is_broadcast_ether_addr(pbyDestAddr))
-		pStatistic->dwTsrBroadcast[uIdx]++;
-	else if (is_multicast_ether_addr(pbyDestAddr))
-		pStatistic->dwTsrMulticast[uIdx]++;
-	else
-		pStatistic->dwTsrDirected[uIdx]++;
-}
-
-/*
- * Description: Update Tx Statistic Counter and copy Tx buffer
- *
- * Parameters:
- *  In:
- *      pStatistic      - Pointer to Statistic Counter Data Structure
- *      pbyBuffer       - Tx Buffer
- *      cbFrameLength   - Tx Length
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void
-STAvUpdateTDStatCounterEx(
-	PSStatCounter   pStatistic,
-	unsigned char *pbyBuffer,
-	unsigned long cbFrameLength
-)
-{
-	unsigned int uPktLength;
-
-	uPktLength = (unsigned int)cbFrameLength;
-
-	// tx length
-	pStatistic->dwCntTxBufLength = uPktLength;
-	// tx pattern, we just see 16 bytes for sample
-	memcpy(pStatistic->abyCntTxPattern, pbyBuffer, 16);
-}
-
-/*
  * Description: Update 802.11 mib counter
  *
  * Parameters:
@@ -526,37 +132,8 @@ STAvUpdate802_11Counter(
 	unsigned long dwCounter
 )
 {
-	p802_11Counter->MulticastTransmittedFrameCount = (unsigned long long) (pStatistic->dwTsrBroadcast[TYPE_AC0DMA] +
-									       pStatistic->dwTsrBroadcast[TYPE_TXDMA0] +
-									       pStatistic->dwTsrMulticast[TYPE_AC0DMA] +
-									       pStatistic->dwTsrMulticast[TYPE_TXDMA0]);
-	p802_11Counter->FailedCount = (unsigned long long) (pStatistic->dwTsrErr[TYPE_AC0DMA] + pStatistic->dwTsrErr[TYPE_TXDMA0]);
-	p802_11Counter->RetryCount = (unsigned long long) (pStatistic->dwTsrRetry[TYPE_AC0DMA] + pStatistic->dwTsrRetry[TYPE_TXDMA0]);
-	p802_11Counter->MultipleRetryCount = (unsigned long long) (pStatistic->dwTsrMoreThanOnceRetry[TYPE_AC0DMA] +
-								   pStatistic->dwTsrMoreThanOnceRetry[TYPE_TXDMA0]);
 	p802_11Counter->RTSSuccessCount += (unsigned long long)  (dwCounter & 0x000000ff);
 	p802_11Counter->RTSFailureCount += (unsigned long long) ((dwCounter & 0x0000ff00) >> 8);
 	p802_11Counter->ACKFailureCount += (unsigned long long) ((dwCounter & 0x00ff0000) >> 16);
 	p802_11Counter->FCSErrorCount +=   (unsigned long long) ((dwCounter & 0xff000000) >> 24);
-	p802_11Counter->MulticastReceivedFrameCount = (unsigned long long) (pStatistic->dwRsrBroadcast +
-									    pStatistic->dwRsrMulticast);
-}
-
-/*
- * Description: Clear 802.11 mib counter
- *
- * Parameters:
- *  In:
- *      p802_11Counter  - Pointer to 802.11 mib counter
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void
-STAvClear802_11Counter(PSDot11Counters p802_11Counter)
-{
-	// set memory to zero
-	memset(p802_11Counter, 0, sizeof(SDot11Counters));
 }
diff --git a/drivers/staging/vt6655/mib.h b/drivers/staging/vt6655/mib.h
index 732bddaf5b91..5cb59b8a1c7c 100644
--- a/drivers/staging/vt6655/mib.h
+++ b/drivers/staging/vt6655/mib.h
@@ -29,8 +29,6 @@
 #ifndef __MIB_H__
 #define __MIB_H__
 
-#include "ttype.h"
-#include "tether.h"
 #include "desc.h"
 
 //
@@ -38,136 +36,16 @@
 //
 
 typedef struct tagSDot11Counters {
-	unsigned long Length;
-	unsigned long long   TransmittedFragmentCount;
-	unsigned long long   MulticastTransmittedFrameCount;
-	unsigned long long   FailedCount;
-	unsigned long long   RetryCount;
-	unsigned long long   MultipleRetryCount;
 	unsigned long long   RTSSuccessCount;
 	unsigned long long   RTSFailureCount;
 	unsigned long long   ACKFailureCount;
-	unsigned long long   FrameDuplicateCount;
-	unsigned long long   ReceivedFragmentCount;
-	unsigned long long   MulticastReceivedFrameCount;
 	unsigned long long   FCSErrorCount;
-	unsigned long long   TKIPLocalMICFailures;
-	unsigned long long   TKIPRemoteMICFailures;
-	unsigned long long   TKIPICVErrors;
-	unsigned long long   TKIPCounterMeasuresInvoked;
-	unsigned long long   TKIPReplays;
-	unsigned long long   CCMPFormatErrors;
-	unsigned long long   CCMPReplays;
-	unsigned long long   CCMPDecryptErrors;
-	unsigned long long   FourWayHandshakeFailures;
 } SDot11Counters, *PSDot11Counters;
 
 //
-// MIB2 counter
-//
-typedef struct tagSMib2Counter {
-	long    ifIndex;
-	char    ifDescr[256];
-	long    ifType;
-	long    ifMtu;
-	unsigned long ifSpeed;
-	unsigned char ifPhysAddress[ETH_ALEN];
-	long    ifAdminStatus;
-	long    ifOperStatus;
-	unsigned long ifLastChange;
-	unsigned long ifInOctets;
-	unsigned long ifInUcastPkts;
-	unsigned long ifInNUcastPkts;
-	unsigned long ifInDiscards;
-	unsigned long ifInErrors;
-	unsigned long ifInUnknownProtos;
-	unsigned long ifOutOctets;
-	unsigned long ifOutUcastPkts;
-	unsigned long ifOutNUcastPkts;
-	unsigned long ifOutDiscards;
-	unsigned long ifOutErrors;
-	unsigned long ifOutQLen;
-	unsigned long ifSpecific;
-} SMib2Counter, *PSMib2Counter;
-
-// Value in the ifType entry
-#define WIRELESSLANIEEE80211b      6
-
-// Value in the ifAdminStatus/ifOperStatus entry
-#define UP                  1
-#define DOWN                2
-#define TESTING             3
-
-//
-// RMON counter
-//
-typedef struct tagSRmonCounter {
-	long    etherStatsIndex;
-	unsigned long etherStatsDataSource;
-	unsigned long etherStatsDropEvents;
-	unsigned long etherStatsOctets;
-	unsigned long etherStatsPkts;
-	unsigned long etherStatsBroadcastPkts;
-	unsigned long etherStatsMulticastPkts;
-	unsigned long etherStatsCRCAlignErrors;
-	unsigned long etherStatsUndersizePkts;
-	unsigned long etherStatsOversizePkts;
-	unsigned long etherStatsFragments;
-	unsigned long etherStatsJabbers;
-	unsigned long etherStatsCollisions;
-	unsigned long etherStatsPkt64Octets;
-	unsigned long etherStatsPkt65to127Octets;
-	unsigned long etherStatsPkt128to255Octets;
-	unsigned long etherStatsPkt256to511Octets;
-	unsigned long etherStatsPkt512to1023Octets;
-	unsigned long etherStatsPkt1024to1518Octets;
-	unsigned long etherStatsOwners;
-	unsigned long etherStatsStatus;
-} SRmonCounter, *PSRmonCounter;
-
-//
-// Custom counter
-//
-typedef struct tagSCustomCounters {
-	unsigned long Length;
-
-	unsigned long long   ullTsrAllOK;
-
-	unsigned long long   ullRsr11M;
-	unsigned long long   ullRsr5M;
-	unsigned long long   ullRsr2M;
-	unsigned long long   ullRsr1M;
-
-	unsigned long long   ullRsr11MCRCOk;
-	unsigned long long   ullRsr5MCRCOk;
-	unsigned long long   ullRsr2MCRCOk;
-	unsigned long long   ullRsr1MCRCOk;
-
-	unsigned long long   ullRsr54M;
-	unsigned long long   ullRsr48M;
-	unsigned long long   ullRsr36M;
-	unsigned long long   ullRsr24M;
-	unsigned long long   ullRsr18M;
-	unsigned long long   ullRsr12M;
-	unsigned long long   ullRsr9M;
-	unsigned long long   ullRsr6M;
-
-	unsigned long long   ullRsr54MCRCOk;
-	unsigned long long   ullRsr48MCRCOk;
-	unsigned long long   ullRsr36MCRCOk;
-	unsigned long long   ullRsr24MCRCOk;
-	unsigned long long   ullRsr18MCRCOk;
-	unsigned long long   ullRsr12MCRCOk;
-	unsigned long long   ullRsr9MCRCOk;
-	unsigned long long   ullRsr6MCRCOk;
-} SCustomCounters, *PSCustomCounters;
-
-//
 // Custom counter
 //
 typedef struct tagSISRCounters {
-	unsigned long Length;
-
 	unsigned long dwIsrTx0OK;
 	unsigned long dwIsrAC0TxOK;
 	unsigned long dwIsrBeaconTxOK;
@@ -183,161 +61,22 @@ typedef struct tagSISRCounters {
 	unsigned long dwIsrUnknown;
 
 	unsigned long dwIsrRx1OK;
-	unsigned long dwIsrATIMTxOK;
-	unsigned long dwIsrSYNCTxOK;
-	unsigned long dwIsrCFPEnd;
-	unsigned long dwIsrATIMEnd;
-	unsigned long dwIsrSYNCFlushOK;
 	unsigned long dwIsrSTIMER1Int;
 } SISRCounters, *PSISRCounters;
 
-// Value in the etherStatsStatus entry
-#define VALID               1
-#define CREATE_REQUEST      2
-#define UNDER_CREATION      3
-#define INVALID             4
-
 //
 // statistic counter
 //
 typedef struct tagSStatCounter {
-	// RSR status count
-	//
-	unsigned long dwRsrFrmAlgnErr;
-	unsigned long dwRsrErr;
-	unsigned long dwRsrCRCErr;
-	unsigned long dwRsrCRCOk;
-	unsigned long dwRsrBSSIDOk;
-	unsigned long dwRsrADDROk;
-	unsigned long dwRsrBCNSSIDOk;
-	unsigned long dwRsrLENErr;
-	unsigned long dwRsrTYPErr;
-
-	unsigned long dwNewRsrDECRYPTOK;
-	unsigned long dwNewRsrCFP;
-	unsigned long dwNewRsrUTSF;
-	unsigned long dwNewRsrHITAID;
-	unsigned long dwNewRsrHITAID0;
-
-	unsigned long dwRsrLong;
-	unsigned long dwRsrRunt;
-
-	unsigned long dwRsrRxControl;
-	unsigned long dwRsrRxData;
-	unsigned long dwRsrRxManage;
-
-	unsigned long dwRsrRxPacket;
-	unsigned long dwRsrRxOctet;
-	unsigned long dwRsrBroadcast;
-	unsigned long dwRsrMulticast;
-	unsigned long dwRsrDirected;
-	// 64-bit OID
-	unsigned long long   ullRsrOK;
-
-	// for some optional OIDs (64 bits) and DMI support
-	unsigned long long   ullRxBroadcastBytes;
-	unsigned long long   ullRxMulticastBytes;
-	unsigned long long   ullRxDirectedBytes;
-	unsigned long long   ullRxBroadcastFrames;
-	unsigned long long   ullRxMulticastFrames;
-	unsigned long long   ullRxDirectedFrames;
-
-	unsigned long dwRsrRxFragment;
-	unsigned long dwRsrRxFrmLen64;
-	unsigned long dwRsrRxFrmLen65_127;
-	unsigned long dwRsrRxFrmLen128_255;
-	unsigned long dwRsrRxFrmLen256_511;
-	unsigned long dwRsrRxFrmLen512_1023;
-	unsigned long dwRsrRxFrmLen1024_1518;
-
-	// TSR status count
-	//
-	unsigned long dwTsrTotalRetry[TYPE_MAXTD];        // total collision retry count
-	unsigned long dwTsrOnceRetry[TYPE_MAXTD];         // this packet only occur one collision
-	unsigned long dwTsrMoreThanOnceRetry[TYPE_MAXTD]; // this packet occur more than one collision
-	unsigned long dwTsrRetry[TYPE_MAXTD];             // this packet has ever occur collision,
-	// that is (dwTsrOnceCollision0 + dwTsrMoreThanOnceCollision0)
-	unsigned long dwTsrACKData[TYPE_MAXTD];
-	unsigned long dwTsrErr[TYPE_MAXTD];
-	unsigned long dwAllTsrOK[TYPE_MAXTD];
-	unsigned long dwTsrRetryTimeout[TYPE_MAXTD];
-	unsigned long dwTsrTransmitTimeout[TYPE_MAXTD];
-
-	unsigned long dwTsrTxPacket[TYPE_MAXTD];
-	unsigned long dwTsrTxOctet[TYPE_MAXTD];
-	unsigned long dwTsrBroadcast[TYPE_MAXTD];
-	unsigned long dwTsrMulticast[TYPE_MAXTD];
-	unsigned long dwTsrDirected[TYPE_MAXTD];
-
-	// RD/TD count
-	unsigned long dwCntRxFrmLength;
-	unsigned long dwCntTxBufLength;
-
-	unsigned char abyCntRxPattern[16];
-	unsigned char abyCntTxPattern[16];
-
-	// Software check....
-	unsigned long dwCntRxDataErr;             // rx buffer data software compare CRC err count
-	unsigned long dwCntDecryptErr;            // rx buffer data software compare CRC err count
-	unsigned long dwCntRxICVErr;              // rx buffer data software compare CRC err count
-	unsigned int idxRxErrorDesc[TYPE_MAXRD]; // index for rx data error RD
-
-	// 64-bit OID
-	unsigned long long   ullTsrOK[TYPE_MAXTD];
-
-	// for some optional OIDs (64 bits) and DMI support
-	unsigned long long   ullTxBroadcastFrames[TYPE_MAXTD];
-	unsigned long long   ullTxMulticastFrames[TYPE_MAXTD];
-	unsigned long long   ullTxDirectedFrames[TYPE_MAXTD];
-	unsigned long long   ullTxBroadcastBytes[TYPE_MAXTD];
-	unsigned long long   ullTxMulticastBytes[TYPE_MAXTD];
-	unsigned long long   ullTxDirectedBytes[TYPE_MAXTD];
-
 	SISRCounters ISRStat;
-
-	SCustomCounters CustomStat;
-
-#ifdef Calcu_LinkQual
-	//Tx count:
-	unsigned long TxNoRetryOkCount;
-	unsigned long TxRetryOkCount;
-	unsigned long TxFailCount;
-	//Rx count:
-	unsigned long RxOkCnt;
-	unsigned long RxFcsErrCnt;
-	//statistic
-	unsigned long SignalStren;
-	unsigned long LinkQuality;
-#endif
 } SStatCounter, *PSStatCounter;
 
-void STAvClearAllCounter(PSStatCounter pStatistic);
-
 void STAvUpdateIsrStatCounter(PSStatCounter pStatistic, unsigned long dwIsr);
 
-void STAvUpdateRDStatCounter(PSStatCounter pStatistic,
-			     unsigned char byRSR, unsigned char byNewRSR, unsigned char byRxRate,
-			     unsigned char *pbyBuffer, unsigned int cbFrameLength);
-
-void STAvUpdateRDStatCounterEx(PSStatCounter pStatistic,
-			       unsigned char byRSR, unsigned char byNewRsr, unsigned char byRxRate,
-			       unsigned char *pbyBuffer, unsigned int cbFrameLength);
-
-void STAvUpdateTDStatCounter(PSStatCounter pStatistic, unsigned char byTSR0, unsigned char byTSR1,
-			     unsigned char *pbyBuffer, unsigned int cbFrameLength, unsigned int uIdx);
-
-void STAvUpdateTDStatCounterEx(
-	PSStatCounter   pStatistic,
-	unsigned char *pbyBuffer,
-	unsigned long cbFrameLength
-);
-
 void STAvUpdate802_11Counter(
 	PSDot11Counters p802_11Counter,
 	PSStatCounter   pStatistic,
 	unsigned long dwCounter
 );
 
-void STAvClear802_11Counter(PSDot11Counters p802_11Counter);
-
 #endif // __MIB_H__
diff --git a/drivers/staging/vt6655/michael.c b/drivers/staging/vt6655/michael.c
deleted file mode 100644
index edee48777aac..000000000000
--- a/drivers/staging/vt6655/michael.c
+++ /dev/null
@@ -1,148 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: michael.cpp
- *
- * Purpose: The implementation of LIST data structure.
- *
- * Author: Kyle Hsu
- *
- * Date: Sep 4, 2002
- *
- * Functions:
- *      s_dwGetUINT32 - Convert from unsigned char [] to unsigned long in a portable way
- *      s_vPutUINT32 - Convert from unsigned long to unsigned char [] in a portable way
- *      s_vClear - Reset the state to the empty message.
- *      s_vSetKey - Set the key.
- *      MIC_vInit - Set the key.
- *      s_vAppendByte - Append the byte to our word-sized buffer.
- *      MIC_vAppend - call s_vAppendByte.
- *      MIC_vGetMIC - Append the minimum padding and call s_vAppendByte.
- *
- * Revision History:
- *
- */
-
-#include "tmacro.h"
-#include "michael.h"
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-static void s_vClear(void);                       // Clear the internal message,
-// resets the object to the state just after construction.
-static void s_vSetKey(u32  dwK0, u32  dwK1);
-static void s_vAppendByte(unsigned char b);            // Add a single byte to the internal message
-
-/*---------------------  Export Variables  --------------------------*/
-static u32 L, R;	/* Current state */
-
-static u32 K0, K1;	/* Key */
-static u32 M;		/* Message accumulator (single word) */
-static unsigned int nBytesInM;      // # bytes in M
-
-/*---------------------  Export Functions  --------------------------*/
-
-static void s_vClear(void)
-{
-	// Reset the state to the empty message.
-	L = K0;
-	R = K1;
-	nBytesInM = 0;
-	M = 0;
-}
-
-static void s_vSetKey(u32 dwK0, u32 dwK1)
-{
-	// Set the key
-	K0 = dwK0;
-	K1 = dwK1;
-	// and reset the message
-	s_vClear();
-}
-
-static void s_vAppendByte(unsigned char b)
-{
-	// Append the byte to our word-sized buffer
-	M |= b << (8*nBytesInM);
-	nBytesInM++;
-	// Process the word if it is full.
-	if (nBytesInM >= 4) {
-		L ^= M;
-		R ^= ROL32(L, 17);
-		L += R;
-		R ^= ((L & 0xff00ff00) >> 8) | ((L & 0x00ff00ff) << 8);
-		L += R;
-		R ^= ROL32(L, 3);
-		L += R;
-		R ^= ROR32(L, 2);
-		L += R;
-		// Clear the buffer
-		M = 0;
-		nBytesInM = 0;
-	}
-}
-
-void MIC_vInit(u32 dwK0, u32 dwK1)
-{
-	// Set the key
-	s_vSetKey(dwK0, dwK1);
-}
-
-void MIC_vUnInit(void)
-{
-	// Wipe the key material
-	K0 = 0;
-	K1 = 0;
-
-	// And the other fields as well.
-	//Note that this sets (L,R) to (K0,K1) which is just fine.
-	s_vClear();
-}
-
-void MIC_vAppend(unsigned char *src, unsigned int nBytes)
-{
-	// This is simple
-	while (nBytes > 0) {
-		s_vAppendByte(*src++);
-		nBytes--;
-	}
-}
-
-void MIC_vGetMIC(u32 *pdwL, u32 *pdwR)
-{
-	// Append the minimum padding
-	s_vAppendByte(0x5a);
-	s_vAppendByte(0);
-	s_vAppendByte(0);
-	s_vAppendByte(0);
-	s_vAppendByte(0);
-	// and then zeroes until the length is a multiple of 4
-	while (nBytesInM != 0)
-		s_vAppendByte(0);
-
-	// The s_vAppendByte function has already computed the result.
-	*pdwL = L;
-	*pdwR = R;
-	// Reset to the empty message.
-	s_vClear();
-}
diff --git a/drivers/staging/vt6655/michael.h b/drivers/staging/vt6655/michael.h
deleted file mode 100644
index 86cb140e3087..000000000000
--- a/drivers/staging/vt6655/michael.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: Michael.h
- *
- * Purpose: Reference implementation for Michael
- *          written by Niels Ferguson
- *
- * Author: Kyle Hsu
- *
- * Date: Jan 2, 2003
- *
- */
-
-#ifndef __MICHAEL_H__
-#define __MICHAEL_H__
-
-#include <linux/types.h>
-
-void MIC_vInit(u32 dwK0, u32 dwK1);
-
-void MIC_vUnInit(void);
-
-/* Append bytes to the message to be MICed */
-void MIC_vAppend(unsigned char *src, unsigned int nBytes);
-
-/* Get the MIC result. Destination should accept 8 bytes of result. */
-/* This also resets the message to empty. */
-void MIC_vGetMIC(u32 *pdwL, u32 *pdwR);
-
-/* Rotation functions on 32 bit values */
-#define ROL32(A, n)							\
-	(((A) << (n)) | (((A)>>(32-(n)))  & ((1UL << (n)) - 1)))
-#define ROR32(A, n) ROL32((A), 32-(n))
-
-#endif /*__MICHAEL_H__ */
diff --git a/drivers/staging/vt6655/power.c b/drivers/staging/vt6655/power.c
index 08241b917777..e826f07e91c0 100644
--- a/drivers/staging/vt6655/power.c
+++ b/drivers/staging/vt6655/power.c
@@ -37,13 +37,9 @@
  *
  */
 
-#include "ttype.h"
 #include "mac.h"
 #include "device.h"
-#include "wmgr.h"
 #include "power.h"
-#include "wcmd.h"
-#include "rxtx.h"
 #include "card.h"
 
 /*---------------------  Static Definitions -------------------------*/
@@ -73,8 +69,7 @@ PSvEnablePowerSaving(
 )
 {
 	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	unsigned short wAID = pMgmt->wCurrAID | BIT14 | BIT15;
+	u16 wAID = pDevice->current_aid | BIT(14) | BIT(15);
 
 	// set period of power up before TBTT
 	VNSvOutPortW(pDevice->PortOffset + MAC_REG_PWBT, C_PWBT);
@@ -83,7 +78,9 @@ PSvEnablePowerSaving(
 		VNSvOutPortW(pDevice->PortOffset + MAC_REG_AIDATIM, wAID);
 	} else {
 		// set ATIM Window
+#if 0 /* TODO atim window */
 		MACvWriteATIMW(pDevice->PortOffset, pMgmt->wCurrATIMWindow);
+#endif
 	}
 	// Set AutoSleep
 	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCFG, PSCFG_AUTOSLEEP);
@@ -95,22 +92,15 @@ PSvEnablePowerSaving(
 		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_ALBCN);
 		// first time set listen next beacon
 		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_LNBCN);
-		pMgmt->wCountToWakeUp = wListenInterval;
 	} else {
 		// always listen beacon
 		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_ALBCN);
-		pMgmt->wCountToWakeUp = 0;
 	}
 
 	// enable power saving hw function
 	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PSEN);
 	pDevice->bEnablePSMode = true;
 
-	/* We don't send null pkt in ad hoc mode since beacon will handle this. */
-	if (pDevice->op_mode != NL80211_IFTYPE_ADHOC &&
-	    pDevice->op_mode == NL80211_IFTYPE_STATION)
-		PSbSendNullPacket(pDevice);
-
 	pDevice->bPWBitOn = true;
 	pr_debug("PS:Power Saving Mode Enable...\n");
 }
@@ -143,182 +133,9 @@ PSvDisablePowerSaving(
 
 	pDevice->bEnablePSMode = false;
 
-	if (pDevice->op_mode == NL80211_IFTYPE_STATION)
-		PSbSendNullPacket(pDevice);
-
 	pDevice->bPWBitOn = false;
 }
 
-/*+
- *
- * Routine Description:
- * Consider to power down when no more packets to tx or rx.
- *
- * Return Value:
- *    true, if power down success
- *    false, if fail
- -*/
-
-bool
-PSbConsiderPowerDown(
-	void *hDeviceContext,
-	bool bCheckRxDMA,
-	bool bCheckCountToWakeUp
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	unsigned int uIdx;
-
-	// check if already in Doze mode
-	if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS))
-		return true;
-
-	if (pMgmt->eCurrMode != WMAC_MODE_IBSS_STA) {
-		// check if in TIM wake period
-		if (pMgmt->bInTIMWake)
-			return false;
-	}
-
-	// check scan state
-	if (pDevice->bCmdRunning)
-		return false;
-
-	// Force PSEN on
-	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PSEN);
-
-	// check if all TD are empty,
-	for (uIdx = 0; uIdx < TYPE_MAXTD; uIdx++) {
-		if (pDevice->iTDUsed[uIdx] != 0)
-			return false;
-	}
-
-	// check if rx isr is clear
-	if (bCheckRxDMA &&
-	    ((pDevice->dwIsr & ISR_RXDMA0) != 0) &&
-	    ((pDevice->dwIsr & ISR_RXDMA1) != 0)) {
-		return false;
-	}
-
-	if (pMgmt->eCurrMode != WMAC_MODE_IBSS_STA) {
-		if (bCheckCountToWakeUp &&
-		    (pMgmt->wCountToWakeUp == 0 || pMgmt->wCountToWakeUp == 1)) {
-			return false;
-		}
-	}
-
-	// no Tx, no Rx isr, now go to Doze
-	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_GO2DOZE);
-	pr_debug("Go to Doze ZZZZZZZZZZZZZZZ\n");
-	return true;
-}
-
-/*+
- *
- * Routine Description:
- * Send PS-POLL packet
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-PSvSendPSPOLL(
-	void *hDeviceContext
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject        pMgmt = pDevice->pMgmt;
-	PSTxMgmtPacket      pTxPacket = NULL;
-
-	memset(pMgmt->pbyPSPacketPool, 0, sizeof(STxMgmtPacket) + WLAN_HDR_ADDR2_LEN);
-	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyPSPacketPool;
-	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-	pTxPacket->p80211Header->sA2.wFrameCtl = cpu_to_le16(
-		(
-			WLAN_SET_FC_FTYPE(WLAN_TYPE_CTL) |
-			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_PSPOLL) |
-			WLAN_SET_FC_PWRMGT(0)
-));
-	pTxPacket->p80211Header->sA2.wDurationID = pMgmt->wCurrAID | BIT14 | BIT15;
-	memcpy(pTxPacket->p80211Header->sA2.abyAddr1, pMgmt->abyCurrBSSID, WLAN_ADDR_LEN);
-	memcpy(pTxPacket->p80211Header->sA2.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-	pTxPacket->cbMPDULen = WLAN_HDR_ADDR2_LEN;
-	pTxPacket->cbPayloadLen = 0;
-	// send the frame
-	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING)
-		pr_debug("Send PS-Poll packet failed..\n");
-}
-
-/*+
- *
- * Routine Description:
- * Send NULL packet to AP for notification power state of STA
- *
- * Return Value:
- *    None.
- *
- -*/
-bool
-PSbSendNullPacket(
-	void *hDeviceContext
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSTxMgmtPacket      pTxPacket = NULL;
-	PSMgmtObject        pMgmt = pDevice->pMgmt;
-	unsigned int uIdx;
-
-	if (!pDevice->bLinkPass)
-		return false;
-
-	if (!pDevice->bEnablePSMode && !pDevice->fTxDataInSleep)
-		return false;
-
-	if (pDevice->bEnablePSMode) {
-		for (uIdx = 0; uIdx < TYPE_MAXTD; uIdx++) {
-			if (pDevice->iTDUsed[uIdx] != 0)
-				return false;
-		}
-	}
-
-	memset(pMgmt->pbyPSPacketPool, 0, sizeof(STxMgmtPacket) + WLAN_NULLDATA_FR_MAXLEN);
-	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyPSPacketPool;
-	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-
-	if (pDevice->bEnablePSMode) {
-		pTxPacket->p80211Header->sA3.wFrameCtl = cpu_to_le16(
-			(
-				WLAN_SET_FC_FTYPE(WLAN_TYPE_DATA) |
-				WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_NULL) |
-				WLAN_SET_FC_PWRMGT(1)
-));
-	} else {
-		pTxPacket->p80211Header->sA3.wFrameCtl = cpu_to_le16(
-			(
-				WLAN_SET_FC_FTYPE(WLAN_TYPE_DATA) |
-				WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_NULL) |
-				WLAN_SET_FC_PWRMGT(0)
-));
-	}
-
-	if (pMgmt->eCurrMode != WMAC_MODE_IBSS_STA)
-		pTxPacket->p80211Header->sA3.wFrameCtl |= cpu_to_le16((unsigned short)WLAN_SET_FC_TODS(1));
-
-	memcpy(pTxPacket->p80211Header->sA3.abyAddr1, pMgmt->abyCurrBSSID, WLAN_ADDR_LEN);
-	memcpy(pTxPacket->p80211Header->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-	memcpy(pTxPacket->p80211Header->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-	pTxPacket->cbMPDULen = WLAN_HDR_ADDR3_LEN;
-	pTxPacket->cbPayloadLen = 0;
-	// send the frame
-	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
-		pr_debug("Send Null Packet failed !\n");
-		return false;
-	}
-
-	return true;
-}
 
 /*+
  *
@@ -336,21 +153,14 @@ PSbIsNextTBTTWakeUp(
 )
 {
 	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject        pMgmt = pDevice->pMgmt;
+	struct ieee80211_hw *hw = pDevice->hw;
+	struct ieee80211_conf *conf = &hw->conf;
 	bool bWakeUp = false;
 
-	if (pMgmt->wListenInterval >= 2) {
-		if (pMgmt->wCountToWakeUp == 0)
-			pMgmt->wCountToWakeUp = pMgmt->wListenInterval;
-
-		pMgmt->wCountToWakeUp--;
-
-		if (pMgmt->wCountToWakeUp == 1) {
-			// Turn on wake up to listen next beacon
-			MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_LNBCN);
-			bWakeUp = true;
-		}
-
+	if (conf->listen_interval == 1) {
+		/* Turn on wake up to listen next beacon */
+		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_LNBCN);
+		bWakeUp = true;
 	}
 
 	return bWakeUp;
diff --git a/drivers/staging/vt6655/power.h b/drivers/staging/vt6655/power.h
index 936f171a6b19..1083341b2a47 100644
--- a/drivers/staging/vt6655/power.h
+++ b/drivers/staging/vt6655/power.h
@@ -33,13 +33,6 @@
 #define     PS_FAST_INTERVAL         1         // Fast power saving listen interval
 #define     PS_MAX_INTERVAL          4         // MAX power saving listen interval
 
-bool
-PSbConsiderPowerDown(
-	void *hDeviceContext,
-	bool bCheckRxDMA,
-	bool bCheckCountToWakeUp
-);
-
 void
 PSvDisablePowerSaving(
 	void *hDeviceContext
@@ -51,15 +44,6 @@ PSvEnablePowerSaving(
 	unsigned short wListenInterval
 );
 
-void
-PSvSendPSPOLL(
-	void *hDeviceContext
-);
-
-bool
-PSbSendNullPacket(
-	void *hDeviceContext
-);
 
 bool
 PSbIsNextTBTTWakeUp(
diff --git a/drivers/staging/vt6655/rc4.c b/drivers/staging/vt6655/rc4.c
deleted file mode 100644
index b7819bc702de..000000000000
--- a/drivers/staging/vt6655/rc4.c
+++ /dev/null
@@ -1,88 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: rc4.c
- *
- * Purpose:
- *
- * Functions:
- *
- * Revision History:
- *
- * Author: Kyle Hsu
- *
- * Date: Sep 4, 2002
- *
- */
-
-#include "rc4.h"
-
-void rc4_init(PRC4Ext pRC4, unsigned char *pbyKey, unsigned int cbKey_len)
-{
-	unsigned int ust1, ust2;
-	unsigned int keyindex;
-	unsigned int stateindex;
-	unsigned char *pbyst;
-	unsigned int idx;
-
-	pbyst = pRC4->abystate;
-	pRC4->ux = 0;
-	pRC4->uy = 0;
-	for (idx = 0; idx < 256; idx++)
-		pbyst[idx] = (unsigned char)idx;
-	keyindex = 0;
-	stateindex = 0;
-	for (idx = 0; idx < 256; idx++) {
-		ust1 = pbyst[idx];
-		stateindex = (stateindex + pbyKey[keyindex] + ust1) & 0xff;
-		ust2 = pbyst[stateindex];
-		pbyst[stateindex] = (unsigned char)ust1;
-		pbyst[idx] = (unsigned char)ust2;
-		if (++keyindex >= cbKey_len)
-			keyindex = 0;
-	}
-}
-
-unsigned int rc4_byte(PRC4Ext pRC4)
-{
-	unsigned int ux;
-	unsigned int uy;
-	unsigned int ustx, usty;
-	unsigned char *pbyst;
-
-	pbyst = pRC4->abystate;
-	ux = (pRC4->ux + 1) & 0xff;
-	ustx = pbyst[ux];
-	uy = (ustx + pRC4->uy) & 0xff;
-	usty = pbyst[uy];
-	pRC4->ux = ux;
-	pRC4->uy = uy;
-	pbyst[uy] = (unsigned char)ustx;
-	pbyst[ux] = (unsigned char)usty;
-
-	return pbyst[(ustx + usty) & 0xff];
-}
-
-void rc4_encrypt(PRC4Ext pRC4, unsigned char *pbyDest,
-		 unsigned char *pbySrc, unsigned int cbData_len)
-{
-	unsigned int ii;
-
-	for (ii = 0; ii < cbData_len; ii++)
-		pbyDest[ii] = (unsigned char)(pbySrc[ii] ^ rc4_byte(pRC4));
-}
diff --git a/drivers/staging/vt6655/rc4.h b/drivers/staging/vt6655/rc4.h
deleted file mode 100644
index 74b2eed9bce3..000000000000
--- a/drivers/staging/vt6655/rc4.h
+++ /dev/null
@@ -1,47 +0,0 @@
-/*
- * File: rc4.h
- *
- * 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.
- *
- * Purpose:
- *
- * Functions:
- *
- * Revision History:
- *
- * Author: Kyle Hsu
- *
- * Date: Sep 4, 2002
- *
- */
-
-#ifndef __RC4_H__
-#define __RC4_H__
-
-#include "ttype.h"
-
-/*---------------------  Export Definitions -------------------------*/
-/*---------------------  Export Types  ------------------------------*/
-typedef struct {
-	unsigned int ux;
-	unsigned int uy;
-	unsigned char abystate[256];
-} RC4Ext, *PRC4Ext;
-
-void rc4_init(PRC4Ext pRC4, unsigned char *pbyKey, unsigned int cbKey_len);
-unsigned int rc4_byte(PRC4Ext pRC4);
-void rc4_encrypt(PRC4Ext pRC4, unsigned char *pbyDest, unsigned char *pbySrc, unsigned int cbData_len);
-
-#endif //__RC4_H__
diff --git a/drivers/staging/vt6655/rf.c b/drivers/staging/vt6655/rf.c
index e505af91bfd0..32ef99341e20 100644
--- a/drivers/staging/vt6655/rf.c
+++ b/drivers/staging/vt6655/rf.c
@@ -29,6 +29,8 @@
  *      IFRFbWriteEmbedded      - Embedded write RF register via MAC
  *
  * Revision History:
+ *	RobertYu 2005
+ *	chester 2008
  *
  */
 
@@ -37,8 +39,6 @@
 #include "rf.h"
 #include "baseband.h"
 
-/*---------------------  Static Definitions -------------------------*/
-
 #define BY_AL2230_REG_LEN     23 //24bit
 #define CB_AL2230_INIT_SEQ    15
 #define SWITCH_CHANNEL_DELAY_AL2230 200 //us
@@ -49,10 +49,6 @@
 #define SWITCH_CHANNEL_DELAY_AL7230 200 //us
 #define AL7230_PWR_IDX_LEN    64
 
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
 static const unsigned long dwAL2230InitTable[CB_AL2230_INIT_SEQ] = {
 	0x03F79000+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
 	0x03333100+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW, //
@@ -172,7 +168,6 @@ static unsigned long dwAL2230PowerTable[AL2230_PWR_IDX_LEN] = {
 	0x0407F900+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW
 };
 
-//{{ RobertYu:20050104
 // 40MHz reference frequency
 // Need to Pull PLLON(PE3) low when writing channel registers through 3-wire.
 static const unsigned long dwAL7230InitTable[CB_AL7230_INIT_SEQ] = {
@@ -408,9 +403,6 @@ static const unsigned long dwAL7230ChannelTable2[CB_MAX_CHANNEL] = {
 	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // channel = 161, Tf = 5805MHz (55)
 	0x77D78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW  // channel = 165, Tf = 5825MHz (56)
 };
-//}} RobertYu
-
-/*---------------------  Static Functions  --------------------------*/
 
 /*
  * Description: AIROHA IFRF chip init function
@@ -424,137 +416,81 @@ static const unsigned long dwAL7230ChannelTable2[CB_MAX_CHANNEL] = {
  * Return Value: true if succeeded; false if failed.
  *
  */
-static bool s_bAL7230Init(void __iomem *dwIoBase)
+static bool s_bAL7230Init(struct vnt_private *priv)
 {
+	void __iomem *dwIoBase = priv->PortOffset;
 	int     ii;
 	bool bResult;
 
 	bResult = true;
 
-	//3-wire control for normal mode
+	/* 3-wire control for normal mode */
 	VNSvOutPortB(dwIoBase + MAC_REG_SOFTPWRCTL, 0);
 
 	MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPECTI  |
 							 SOFTPWRCTL_TXPEINV));
-	BBvPowerSaveModeOFF(dwIoBase); //RobertYu:20050106, have DC value for Calibration
+	BBvPowerSaveModeOFF(priv); /* RobertYu:20050106, have DC value for Calibration */
 
 	for (ii = 0; ii < CB_AL7230_INIT_SEQ; ii++)
-		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[ii]);
+		bResult &= IFRFbWriteEmbedded(priv, dwAL7230InitTable[ii]);
 
-	// PLL On
+	/* PLL On */
 	MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
 
-	//Calibration
+	/* Calibration */
 	MACvTimer0MicroSDelay(dwIoBase, 150);//150us
-	bResult &= IFRFbWriteEmbedded(dwIoBase, (0x9ABA8F00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW)); //TXDCOC:active, RCK:disable
+	/* TXDCOC:active, RCK:disable */
+	bResult &= IFRFbWriteEmbedded(priv, (0x9ABA8F00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW));
 	MACvTimer0MicroSDelay(dwIoBase, 30);//30us
-	bResult &= IFRFbWriteEmbedded(dwIoBase, (0x3ABA8F00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW)); //TXDCOC:disable, RCK:active
+	/* TXDCOC:disable, RCK:active */
+	bResult &= IFRFbWriteEmbedded(priv, (0x3ABA8F00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW));
 	MACvTimer0MicroSDelay(dwIoBase, 30);//30us
-	bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[CB_AL7230_INIT_SEQ-1]); //TXDCOC:disable, RCK:disable
+	/* TXDCOC:disable, RCK:disable */
+	bResult &= IFRFbWriteEmbedded(priv, dwAL7230InitTable[CB_AL7230_INIT_SEQ-1]);
 
 	MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPE3    |
 							 SOFTPWRCTL_SWPE2    |
 							 SOFTPWRCTL_SWPECTI  |
 							 SOFTPWRCTL_TXPEINV));
 
-	BBvPowerSaveModeON(dwIoBase); // RobertYu:20050106
+	BBvPowerSaveModeON(priv); /* RobertYu:20050106 */
 
-	// PE1: TX_ON, PE2: RX_ON, PE3: PLLON
-	//3-wire control for power saving mode
+	/* PE1: TX_ON, PE2: RX_ON, PE3: PLLON */
+	/* 3-wire control for power saving mode */
 	VNSvOutPortB(dwIoBase + MAC_REG_PSPWRSIG, (PSSIG_WPE3 | PSSIG_WPE2)); //1100 0000
 
 	return bResult;
 }
 
-// Need to Pull PLLON low when writing channel registers through 3-wire interface
-static bool s_bAL7230SelectChannel(void __iomem *dwIoBase, unsigned char byChannel)
+/* Need to Pull PLLON low when writing channel registers through
+ * 3-wire interface */
+static bool s_bAL7230SelectChannel(struct vnt_private *priv, unsigned char byChannel)
 {
+	void __iomem *dwIoBase = priv->PortOffset;
 	bool bResult;
 
 	bResult = true;
 
-	// PLLON Off
+	/* PLLON Off */
 	MACvWordRegBitsOff(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
 
-	bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230ChannelTable0[byChannel - 1]); //Reg0
-	bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230ChannelTable1[byChannel - 1]); //Reg1
-	bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230ChannelTable2[byChannel - 1]); //Reg4
+	bResult &= IFRFbWriteEmbedded(priv, dwAL7230ChannelTable0[byChannel - 1]);
+	bResult &= IFRFbWriteEmbedded(priv, dwAL7230ChannelTable1[byChannel - 1]);
+	bResult &= IFRFbWriteEmbedded(priv, dwAL7230ChannelTable2[byChannel - 1]);
 
-	// PLLOn On
+	/* PLLOn On */
 	MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
 
-	// Set Channel[7] = 0 to tell H/W channel is changing now.
+	/* Set Channel[7] = 0 to tell H/W channel is changing now. */
 	VNSvOutPortB(dwIoBase + MAC_REG_CHANNEL, (byChannel & 0x7F));
 	MACvTimer0MicroSDelay(dwIoBase, SWITCH_CHANNEL_DELAY_AL7230);
-	// Set Channel[7] = 1 to tell H/W channel change is done.
+	/* Set Channel[7] = 1 to tell H/W channel change is done. */
 	VNSvOutPortB(dwIoBase + MAC_REG_CHANNEL, (byChannel | 0x80));
 
 	return bResult;
 }
 
 /*
- * Description: Select channel with UW2452 chip
- *
- * Parameters:
- *  In:
- *      dwIoBase    - I/O base address
- *      uChannel    - Channel number
- *  Out:
- *      none
- *
- * Return Value: true if succeeded; false if failed.
- *
- */
-
-//{{ RobertYu: 20041210
-/*
- * Description: UW2452 IFRF chip init function
- *
- * Parameters:
- *  In:
- *      dwIoBase    - I/O base address
- *  Out:
- *      none
- *
- * Return Value: true if succeeded; false if failed.
- *
- */
-
-//}} RobertYu
-////////////////////////////////////////////////////////////////////////////////
-
-/*
- * Description: VT3226 IFRF chip init function
- *
- * Parameters:
- *  In:
- *      dwIoBase    - I/O base address
- *  Out:
- *      none
- *
- * Return Value: true if succeeded; false if failed.
- *
- */
-
-/*
- * Description: Select channel with VT3226 chip
- *
- * Parameters:
- *  In:
- *      dwIoBase    - I/O base address
- *      uChannel    - Channel number
- *  Out:
- *      none
- *
- * Return Value: true if succeeded; false if failed.
- *
- */
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-/*
  * Description: Write to IF/RF, by embedded programming
  *
  * Parameters:
@@ -567,14 +503,15 @@ static bool s_bAL7230SelectChannel(void __iomem *dwIoBase, unsigned char byChann
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool IFRFbWriteEmbedded(void __iomem *dwIoBase, unsigned long dwData)
+bool IFRFbWriteEmbedded(struct vnt_private *priv, unsigned long dwData)
 {
+	void __iomem *dwIoBase = priv->PortOffset;
 	unsigned short ww;
 	unsigned long dwValue;
 
 	VNSvOutPortD(dwIoBase + MAC_REG_IFREGCTL, dwData);
 
-	// W_MAX_TIMEOUT is the timeout period
+	/* W_MAX_TIMEOUT is the timeout period */
 	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
 		VNSvInPortD(dwIoBase + MAC_REG_IFREGCTL, &dwValue);
 		if (dwValue & IFREGCTL_DONE)
@@ -588,33 +525,6 @@ bool IFRFbWriteEmbedded(void __iomem *dwIoBase, unsigned long dwData)
 }
 
 /*
- * Description: RFMD RF2959 IFRF chip init function
- *
- * Parameters:
- *  In:
- *      dwIoBase    - I/O base address
- *  Out:
- *      none
- *
- * Return Value: true if succeeded; false if failed.
- *
- */
-
-/*
- * Description: Select channel with RFMD 2959 chip
- *
- * Parameters:
- *  In:
- *      dwIoBase    - I/O base address
- *      uChannel    - Channel number
- *  Out:
- *      none
- *
- * Return Value: true if succeeded; false if failed.
- *
- */
-
-/*
  * Description: AIROHA IFRF chip init function
  *
  * Parameters:
@@ -626,113 +536,70 @@ bool IFRFbWriteEmbedded(void __iomem *dwIoBase, unsigned long dwData)
  * Return Value: true if succeeded; false if failed.
  *
  */
-static bool RFbAL2230Init(void __iomem *dwIoBase)
+static bool RFbAL2230Init(struct vnt_private *priv)
 {
+	void __iomem *dwIoBase = priv->PortOffset;
 	int     ii;
 	bool bResult;
 
 	bResult = true;
 
-	//3-wire control for normal mode
+	/* 3-wire control for normal mode */
 	VNSvOutPortB(dwIoBase + MAC_REG_SOFTPWRCTL, 0);
 
 	MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPECTI  |
 							 SOFTPWRCTL_TXPEINV));
-//2008-8-21 chester <add>
-	// PLL  Off
-
+	/* PLL  Off */
 	MACvWordRegBitsOff(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
 
-	//patch abnormal AL2230 frequency output
-//2008-8-21 chester <add>
-	IFRFbWriteEmbedded(dwIoBase, (0x07168700+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW));
+	/* patch abnormal AL2230 frequency output */
+	IFRFbWriteEmbedded(priv, (0x07168700+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW));
 
 	for (ii = 0; ii < CB_AL2230_INIT_SEQ; ii++)
-		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL2230InitTable[ii]);
-//2008-8-21 chester <add>
+		bResult &= IFRFbWriteEmbedded(priv, dwAL2230InitTable[ii]);
 	MACvTimer0MicroSDelay(dwIoBase, 30); //delay 30 us
 
-	// PLL On
+	/* PLL On */
 	MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPE3);
 
 	MACvTimer0MicroSDelay(dwIoBase, 150);//150us
-	bResult &= IFRFbWriteEmbedded(dwIoBase, (0x00d80f00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW));
+	bResult &= IFRFbWriteEmbedded(priv, (0x00d80f00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW));
 	MACvTimer0MicroSDelay(dwIoBase, 30);//30us
-	bResult &= IFRFbWriteEmbedded(dwIoBase, (0x00780f00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW));
+	bResult &= IFRFbWriteEmbedded(priv, (0x00780f00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW));
 	MACvTimer0MicroSDelay(dwIoBase, 30);//30us
-	bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL2230InitTable[CB_AL2230_INIT_SEQ-1]);
+	bResult &= IFRFbWriteEmbedded(priv, dwAL2230InitTable[CB_AL2230_INIT_SEQ-1]);
 
 	MACvWordRegBitsOn(dwIoBase, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPE3    |
 							 SOFTPWRCTL_SWPE2    |
 							 SOFTPWRCTL_SWPECTI  |
 							 SOFTPWRCTL_TXPEINV));
 
-	//3-wire control for power saving mode
+	/* 3-wire control for power saving mode */
 	VNSvOutPortB(dwIoBase + MAC_REG_PSPWRSIG, (PSSIG_WPE3 | PSSIG_WPE2)); //1100 0000
 
 	return bResult;
 }
 
-static bool RFbAL2230SelectChannel(void __iomem *dwIoBase, unsigned char byChannel)
+static bool RFbAL2230SelectChannel(struct vnt_private *priv, unsigned char byChannel)
 {
+	void __iomem *dwIoBase = priv->PortOffset;
 	bool bResult;
 
 	bResult = true;
 
-	bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL2230ChannelTable0[byChannel - 1]);
-	bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL2230ChannelTable1[byChannel - 1]);
+	bResult &= IFRFbWriteEmbedded(priv, dwAL2230ChannelTable0[byChannel - 1]);
+	bResult &= IFRFbWriteEmbedded(priv, dwAL2230ChannelTable1[byChannel - 1]);
 
-	// Set Channel[7] = 0 to tell H/W channel is changing now.
+	/* Set Channel[7] = 0 to tell H/W channel is changing now. */
 	VNSvOutPortB(dwIoBase + MAC_REG_CHANNEL, (byChannel & 0x7F));
 	MACvTimer0MicroSDelay(dwIoBase, SWITCH_CHANNEL_DELAY_AL2230);
-	// Set Channel[7] = 1 to tell H/W channel change is done.
+	/* Set Channel[7] = 1 to tell H/W channel change is done. */
 	VNSvOutPortB(dwIoBase + MAC_REG_CHANNEL, (byChannel | 0x80));
 
 	return bResult;
 }
 
 /*
- * Description: UW2451 IFRF chip init function
- *
- * Parameters:
- *  In:
- *      dwIoBase    - I/O base address
- *  Out:
- *      none
- *
- * Return Value: true if succeeded; false if failed.
- *
- */
-
-/*
- * Description: Select channel with UW2451 chip
- *
- * Parameters:
- *  In:
- *      dwIoBase    - I/O base address
- *      uChannel    - Channel number
- *  Out:
- *      none
- *
- * Return Value: true if succeeded; false if failed.
- *
- */
-
-/*
- * Description: Set sleep mode to UW2451 chip
- *
- * Parameters:
- *  In:
- *      dwIoBase    - I/O base address
- *      uChannel    - Channel number
- *  Out:
- *      none
- *
- * Return Value: true if succeeded; false if failed.
- *
- */
-
-/*
  * Description: RF init function
  *
  * Parameters:
@@ -746,20 +613,20 @@ static bool RFbAL2230SelectChannel(void __iomem *dwIoBase, unsigned char byChann
  *
  */
 bool RFbInit(
-	struct vnt_private *pDevice
+	struct vnt_private *priv
 )
 {
 	bool bResult = true;
 
-	switch (pDevice->byRFType) {
+	switch (priv->byRFType) {
 	case RF_AIROHA:
 	case RF_AL2230S:
-		pDevice->byMaxPwrLevel = AL2230_PWR_IDX_LEN;
-		bResult = RFbAL2230Init(pDevice->PortOffset);
+		priv->byMaxPwrLevel = AL2230_PWR_IDX_LEN;
+		bResult = RFbAL2230Init(priv);
 		break;
 	case RF_AIROHA7230:
-		pDevice->byMaxPwrLevel = AL7230_PWR_IDX_LEN;
-		bResult = s_bAL7230Init(pDevice->PortOffset);
+		priv->byMaxPwrLevel = AL7230_PWR_IDX_LEN;
+		bResult = s_bAL7230Init(priv);
 		break;
 	case RF_NOTHING:
 		bResult = true;
@@ -784,18 +651,18 @@ bool RFbInit(
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool RFbSelectChannel(void __iomem *dwIoBase, unsigned char byRFType, unsigned char byChannel)
+bool RFbSelectChannel(struct vnt_private *priv, unsigned char byRFType, unsigned char byChannel)
 {
 	bool bResult = true;
 
 	switch (byRFType) {
 	case RF_AIROHA:
 	case RF_AL2230S:
-		bResult = RFbAL2230SelectChannel(dwIoBase, byChannel);
+		bResult = RFbAL2230SelectChannel(priv, byChannel);
 		break;
 		//{{ RobertYu: 20050104
 	case RF_AIROHA7230:
-		bResult = s_bAL7230SelectChannel(dwIoBase, byChannel);
+		bResult = s_bAL7230SelectChannel(priv, byChannel);
 		break;
 		//}} RobertYu
 	case RF_NOTHING:
@@ -820,8 +687,9 @@ bool RFbSelectChannel(void __iomem *dwIoBase, unsigned char byRFType, unsigned c
  * Return Value: None.
  *
  */
-bool RFvWriteWakeProgSyn(void __iomem *dwIoBase, unsigned char byRFType, unsigned int uChannel)
+bool RFvWriteWakeProgSyn(struct vnt_private *priv, unsigned char byRFType, unsigned int uChannel)
 {
+	void __iomem *dwIoBase = priv->PortOffset;
 	int   ii;
 	unsigned char byInitCount = 0;
 	unsigned char bySleepCount = 0;
@@ -834,7 +702,8 @@ bool RFvWriteWakeProgSyn(void __iomem *dwIoBase, unsigned char byRFType, unsigne
 		if (uChannel > CB_MAX_CHANNEL_24G)
 			return false;
 
-		byInitCount = CB_AL2230_INIT_SEQ + 2; // Init Reg + Channel Reg (2)
+		 /* Init Reg + Channel Reg (2) */
+		byInitCount = CB_AL2230_INIT_SEQ + 2;
 		bySleepCount = 0;
 		if (byInitCount > (MISCFIFO_SYNDATASIZE - bySleepCount))
 			return false;
@@ -847,10 +716,10 @@ bool RFvWriteWakeProgSyn(void __iomem *dwIoBase, unsigned char byRFType, unsigne
 		MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL2230ChannelTable1[uChannel-1]);
 		break;
 
-		//{{ RobertYu: 20050104
-		// Need to check, PLLON need to be low for channel setting
+		/* Need to check, PLLON need to be low for channel setting */
 	case RF_AIROHA7230:
-		byInitCount = CB_AL7230_INIT_SEQ + 3; // Init Reg + Channel Reg (3)
+		 /* Init Reg + Channel Reg (3) */
+		byInitCount = CB_AL7230_INIT_SEQ + 3;
 		bySleepCount = 0;
 		if (byInitCount > (MISCFIFO_SYNDATASIZE - bySleepCount))
 			return false;
@@ -869,7 +738,6 @@ bool RFvWriteWakeProgSyn(void __iomem *dwIoBase, unsigned char byRFType, unsigne
 		ii++;
 		MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL7230ChannelTable2[uChannel-1]);
 		break;
-		//}} RobertYu
 
 	case RF_NOTHING:
 		return true;
@@ -897,7 +765,7 @@ bool RFvWriteWakeProgSyn(void __iomem *dwIoBase, unsigned char byRFType, unsigne
  *
  */
 bool RFbSetPower(
-	struct vnt_private *pDevice,
+	struct vnt_private *priv,
 	unsigned int uRATE,
 	unsigned int uCH
 )
@@ -907,7 +775,7 @@ bool RFbSetPower(
 	unsigned char byDec = 0;
 	unsigned char byPwrdBm = 0;
 
-	if (pDevice->dwDiagRefCount != 0)
+	if (priv->dwDiagRefCount != 0)
 		return true;
 
 	if ((uCH < 1) || (uCH > CB_MAX_CHANNEL))
@@ -918,22 +786,22 @@ bool RFbSetPower(
 	case RATE_2M:
 	case RATE_5M:
 	case RATE_11M:
-		byPwr = pDevice->abyCCKPwrTbl[uCH];
-		byPwrdBm = pDevice->abyCCKDefaultPwr[uCH];
+		byPwr = priv->abyCCKPwrTbl[uCH];
+		byPwrdBm = priv->abyCCKDefaultPwr[uCH];
 		break;
 	case RATE_6M:
 	case RATE_9M:
 	case RATE_18M:
-		byPwr = pDevice->abyOFDMPwrTbl[uCH];
-		if (pDevice->byRFType == RF_UW2452)
+		byPwr = priv->abyOFDMPwrTbl[uCH];
+		if (priv->byRFType == RF_UW2452)
 			byDec = byPwr + 14;
 		else
 			byDec = byPwr + 10;
 
-		if (byDec >= pDevice->byMaxPwrLevel)
-			byDec = pDevice->byMaxPwrLevel-1;
+		if (byDec >= priv->byMaxPwrLevel)
+			byDec = priv->byMaxPwrLevel-1;
 
-		if (pDevice->byRFType == RF_UW2452) {
+		if (priv->byRFType == RF_UW2452) {
 			byPwrdBm = byDec - byPwr;
 			byPwrdBm /= 3;
 		} else {
@@ -941,24 +809,24 @@ bool RFbSetPower(
 			byPwrdBm >>= 1;
 		}
 
-		byPwrdBm += pDevice->abyOFDMDefaultPwr[uCH];
+		byPwrdBm += priv->abyOFDMDefaultPwr[uCH];
 		byPwr = byDec;
 		break;
 	case RATE_24M:
 	case RATE_36M:
 	case RATE_48M:
 	case RATE_54M:
-		byPwr = pDevice->abyOFDMPwrTbl[uCH];
-		byPwrdBm = pDevice->abyOFDMDefaultPwr[uCH];
+		byPwr = priv->abyOFDMPwrTbl[uCH];
+		byPwrdBm = priv->abyOFDMDefaultPwr[uCH];
 		break;
 	}
 
-	if (pDevice->byCurPwr == byPwr)
+	if (priv->byCurPwr == byPwr)
 		return true;
 
-	bResult = RFbRawSetPower(pDevice, byPwr, uRATE);
+	bResult = RFbRawSetPower(priv, byPwr, uRATE);
 	if (bResult)
-		pDevice->byCurPwr = byPwr;
+		priv->byCurPwr = byPwr;
 
 	return bResult;
 }
@@ -978,7 +846,7 @@ bool RFbSetPower(
  */
 
 bool RFbRawSetPower(
-	struct vnt_private *pDevice,
+	struct vnt_private *priv,
 	unsigned char byPwr,
 	unsigned int uRATE
 )
@@ -986,37 +854,38 @@ bool RFbRawSetPower(
 	bool bResult = true;
 	unsigned long dwMax7230Pwr = 0;
 
-	if (byPwr >=  pDevice->byMaxPwrLevel)
+	if (byPwr >=  priv->byMaxPwrLevel)
 		return false;
 
-	switch (pDevice->byRFType) {
+	switch (priv->byRFType) {
 	case RF_AIROHA:
-		bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, dwAL2230PowerTable[byPwr]);
+		bResult &= IFRFbWriteEmbedded(priv, dwAL2230PowerTable[byPwr]);
 		if (uRATE <= RATE_11M)
-			bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x0001B400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+			bResult &= IFRFbWriteEmbedded(priv, 0x0001B400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
 		else
-			bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+			bResult &= IFRFbWriteEmbedded(priv, 0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
 
 		break;
 
 	case RF_AL2230S:
-		bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, dwAL2230PowerTable[byPwr]);
+		bResult &= IFRFbWriteEmbedded(priv, dwAL2230PowerTable[byPwr]);
 		if (uRATE <= RATE_11M) {
-			bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x040C1400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
-			bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x00299B00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+			bResult &= IFRFbWriteEmbedded(priv, 0x040C1400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+			bResult &= IFRFbWriteEmbedded(priv, 0x00299B00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
 		} else {
-			bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
-			bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x00099B00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+			bResult &= IFRFbWriteEmbedded(priv, 0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
+			bResult &= IFRFbWriteEmbedded(priv, 0x00099B00+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
 		}
 
 		break;
 
 	case RF_AIROHA7230:
-		//  0x080F1B00 for 3 wire control TxGain(D10) and 0x31 as TX Gain value
+		/* 0x080F1B00 for 3 wire control TxGain(D10)
+		 * and 0x31 as TX Gain value */
 		dwMax7230Pwr = 0x080C0B00 | ((byPwr) << 12) |
 			(BY_AL7230_REG_LEN << 3)  | IFREGCTL_REGW;
 
-		bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, dwMax7230Pwr);
+		bResult &= IFRFbWriteEmbedded(priv, dwMax7230Pwr);
 		break;
 
 	default:
@@ -1032,7 +901,7 @@ bool RFbRawSetPower(
  *
  * Parameters:
  *  In:
- *      pDevice         - The adapter to be translated
+ *      priv         - The adapter to be translated
  *      byCurrRSSI      - RSSI to be translated
  *  Out:
  *      pdwdbm          - Translated dbm number
@@ -1042,7 +911,7 @@ bool RFbRawSetPower(
  -*/
 void
 RFvRSSITodBm(
-	struct vnt_private *pDevice,
+	struct vnt_private *priv,
 	unsigned char byCurrRSSI,
 	long *pldBm
 	)
@@ -1052,10 +921,10 @@ RFvRSSITodBm(
 	long a = 0;
 	unsigned char abyAIROHARF[4] = {0, 18, 0, 40};
 
-	switch (pDevice->byRFType) {
+	switch (priv->byRFType) {
 	case RF_AIROHA:
 	case RF_AL2230S:
-	case RF_AIROHA7230: //RobertYu: 20040104
+	case RF_AIROHA7230:
 		a = abyAIROHARF[byIdx];
 		break;
 	default:
@@ -1065,42 +934,38 @@ RFvRSSITodBm(
 	*pldBm = -1 * (a + b * 2);
 }
 
-////////////////////////////////////////////////////////////////////////////////
-//{{ RobertYu: 20050104
-
-// Post processing for the 11b/g and 11a.
-// for save time on changing Reg2,3,5,7,10,12,15
-bool RFbAL7230SelectChannelPostProcess(void __iomem *dwIoBase, unsigned char byOldChannel, unsigned char byNewChannel)
+/* Post processing for the 11b/g and 11a.
+ * for save time on changing Reg2,3,5,7,10,12,15 */
+bool RFbAL7230SelectChannelPostProcess(struct vnt_private *priv,
+				       unsigned char byOldChannel,
+				       unsigned char byNewChannel)
 {
 	bool bResult;
 
 	bResult = true;
 
-	// if change between 11 b/g and 11a need to update the following register
-	// Channel Index 1~14
-
+	/* if change between 11 b/g and 11a need to update the following
+	 * register
+	 * Channel Index 1~14 */
 	if ((byOldChannel <= CB_MAX_CHANNEL_24G) && (byNewChannel > CB_MAX_CHANNEL_24G)) {
-		// Change from 2.4G to 5G
-		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[2]); //Reg2
-		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[3]); //Reg3
-		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[5]); //Reg5
-		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[7]); //Reg7
-		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[10]);//Reg10
-		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[12]);//Reg12
-		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTableAMode[15]);//Reg15
+		/* Change from 2.4G to 5G [Reg] */
+		bResult &= IFRFbWriteEmbedded(priv, dwAL7230InitTableAMode[2]);
+		bResult &= IFRFbWriteEmbedded(priv, dwAL7230InitTableAMode[3]);
+		bResult &= IFRFbWriteEmbedded(priv, dwAL7230InitTableAMode[5]);
+		bResult &= IFRFbWriteEmbedded(priv, dwAL7230InitTableAMode[7]);
+		bResult &= IFRFbWriteEmbedded(priv, dwAL7230InitTableAMode[10]);
+		bResult &= IFRFbWriteEmbedded(priv, dwAL7230InitTableAMode[12]);
+		bResult &= IFRFbWriteEmbedded(priv, dwAL7230InitTableAMode[15]);
 	} else if ((byOldChannel > CB_MAX_CHANNEL_24G) && (byNewChannel <= CB_MAX_CHANNEL_24G)) {
-		// change from 5G to 2.4G
-		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[2]); //Reg2
-		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[3]); //Reg3
-		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[5]); //Reg5
-		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[7]); //Reg7
-		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[10]);//Reg10
-		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[12]);//Reg12
-		bResult &= IFRFbWriteEmbedded(dwIoBase, dwAL7230InitTable[15]);//Reg15
+		/* Change from 5G to 2.4G [Reg] */
+		bResult &= IFRFbWriteEmbedded(priv, dwAL7230InitTable[2]);
+		bResult &= IFRFbWriteEmbedded(priv, dwAL7230InitTable[3]);
+		bResult &= IFRFbWriteEmbedded(priv, dwAL7230InitTable[5]);
+		bResult &= IFRFbWriteEmbedded(priv, dwAL7230InitTable[7]);
+		bResult &= IFRFbWriteEmbedded(priv, dwAL7230InitTable[10]);
+		bResult &= IFRFbWriteEmbedded(priv, dwAL7230InitTable[12]);
+		bResult &= IFRFbWriteEmbedded(priv, dwAL7230InitTable[15]);
 	}
 
 	return bResult;
 }
-
-//}} RobertYu
-////////////////////////////////////////////////////////////////////////////////
diff --git a/drivers/staging/vt6655/rf.h b/drivers/staging/vt6655/rf.h
index be4ef88b7666..8a6e2cfedaa5 100644
--- a/drivers/staging/vt6655/rf.h
+++ b/drivers/staging/vt6655/rf.h
@@ -30,7 +30,6 @@
 #ifndef __RF_H__
 #define __RF_H__
 
-#include "ttype.h"
 #include "device.h"
 
 /*---------------------  Export Definitions -------------------------*/
@@ -74,12 +73,12 @@
 
 /*---------------------  Export Functions  --------------------------*/
 
-bool IFRFbWriteEmbedded(void __iomem *dwIoBase, unsigned long dwData);
-bool RFbSelectChannel(void __iomem *dwIoBase, unsigned char byRFType, unsigned char byChannel);
+bool IFRFbWriteEmbedded(struct vnt_private *, unsigned long dwData);
+bool RFbSelectChannel(struct vnt_private *, unsigned char byRFType, unsigned char byChannel);
 bool RFbInit(
 	struct vnt_private *
 );
-bool RFvWriteWakeProgSyn(void __iomem *dwIoBase, unsigned char byRFType, unsigned int uChannel);
+bool RFvWriteWakeProgSyn(struct vnt_private *, unsigned char byRFType, unsigned int uChannel);
 bool RFbSetPower(struct vnt_private *, unsigned int uRATE, unsigned int uCH);
 bool RFbRawSetPower(
 	struct vnt_private *,
@@ -95,7 +94,7 @@ RFvRSSITodBm(
 );
 
 //{{ RobertYu: 20050104
-bool RFbAL7230SelectChannelPostProcess(void __iomem *dwIoBase, unsigned char byOldChannel, unsigned char byNewChannel);
+bool RFbAL7230SelectChannelPostProcess(struct vnt_private *, unsigned char byOldChannel, unsigned char byNewChannel);
 //}} RobertYu
 
 #endif // __RF_H__
diff --git a/drivers/staging/vt6655/rxtx.c b/drivers/staging/vt6655/rxtx.c
index 7a183f55e7eb..61c39dd7ad01 100644
--- a/drivers/staging/vt6655/rxtx.c
+++ b/drivers/staging/vt6655/rxtx.c
@@ -50,17 +50,9 @@
 
 #include "device.h"
 #include "rxtx.h"
-#include "tether.h"
 #include "card.h"
-#include "bssdb.h"
 #include "mac.h"
 #include "baseband.h"
-#include "michael.h"
-#include "tkip.h"
-#include "tcrc.h"
-#include "wctl.h"
-#include "wroute.h"
-#include "hostap.h"
 #include "rf.h"
 
 /*---------------------  Static Definitions -------------------------*/
@@ -105,19 +97,6 @@ static const unsigned short wFB_Opt1[2][5] = {
 #define DATADUR_A_F1    13
 
 /*---------------------  Static Functions  --------------------------*/
-
-static
-void
-s_vFillTxKey(
-	struct vnt_private *pDevice,
-	unsigned char *pbyBuf,
-	unsigned char *pbyIVHead,
-	PSKeyItem  pTransmitKey,
-	unsigned char *pbyHdrBuf,
-	unsigned short wPayloadLen,
-	unsigned char *pMICHDR
-);
-
 static
 void
 s_vFillRTSHead(
@@ -127,7 +106,7 @@ s_vFillRTSHead(
 	unsigned int	cbFrameLength,
 	bool bNeedAck,
 	bool bDisCRC,
-	PSEthernetHeader psEthHeader,
+	struct ieee80211_hdr *hdr,
 	unsigned short wCurrentRate,
 	unsigned char byFBOption
 );
@@ -144,26 +123,15 @@ s_vGenerateTxParameter(
 	unsigned int	cbFrameSize,
 	bool bNeedACK,
 	unsigned int	uDMAIdx,
-	PSEthernetHeader psEthHeader,
+	void *psEthHeader,
 	unsigned short wCurrentRate
 );
 
-static void s_vFillFragParameter(
-	struct vnt_private *pDevice,
-	unsigned char *pbyBuffer,
-	unsigned int	uTxType,
-	void *pvtdCurr,
-	unsigned short wFragType,
-	unsigned int	cbReqCount
-);
-
 static unsigned int
 s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
-		  unsigned char *pbyTxBufferAddr, unsigned int cbFrameBodySize,
+		  unsigned char *pbyTxBufferAddr,
 		  unsigned int uDMAIdx, PSTxDesc pHeadTD,
-		  PSEthernetHeader psEthHeader, unsigned char *pPacket,
-		  bool bNeedEncrypt, PSKeyItem pTransmitKey,
-		  unsigned int uNodeIndex, unsigned int *puMACfragNum);
+		  unsigned int uNodeIndex);
 
 static
 __le16
@@ -178,165 +146,12 @@ s_uFillDataHead(
 	unsigned int cbLastFragmentSize,
 	unsigned int uMACfragNum,
 	unsigned char byFBOption,
-	unsigned short wCurrentRate
+	unsigned short wCurrentRate,
+	bool is_pspoll
 );
 
 /*---------------------  Export Variables  --------------------------*/
 
-static
-void
-s_vFillTxKey(
-	struct vnt_private *pDevice,
-	unsigned char *pbyBuf,
-	unsigned char *pbyIVHead,
-	PSKeyItem  pTransmitKey,
-	unsigned char *pbyHdrBuf,
-	unsigned short wPayloadLen,
-	unsigned char *pMICHDR
-)
-{
-	struct vnt_mic_hdr *mic_hdr = (struct vnt_mic_hdr *)pMICHDR;
-	unsigned long *pdwIV = (unsigned long *)pbyIVHead;
-	unsigned long *pdwExtIV = (unsigned long *)((unsigned char *)pbyIVHead+4);
-	PS802_11Header  pMACHeader = (PS802_11Header)pbyHdrBuf;
-	unsigned long dwRevIVCounter;
-	unsigned char byKeyIndex = 0;
-
-	//Fill TXKEY
-	if (pTransmitKey == NULL)
-		return;
-
-	dwRevIVCounter = cpu_to_le32(pDevice->dwIVCounter);
-	*pdwIV = pDevice->dwIVCounter;
-	byKeyIndex = pTransmitKey->dwKeyIndex & 0xf;
-
-	if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) {
-		if (pTransmitKey->uKeyLength == WLAN_WEP232_KEYLEN) {
-			memcpy(pDevice->abyPRNG, (unsigned char *)&(dwRevIVCounter), 3);
-			memcpy(pDevice->abyPRNG+3, pTransmitKey->abyKey, pTransmitKey->uKeyLength);
-		} else {
-			memcpy(pbyBuf, (unsigned char *)&(dwRevIVCounter), 3);
-			memcpy(pbyBuf+3, pTransmitKey->abyKey, pTransmitKey->uKeyLength);
-			if (pTransmitKey->uKeyLength == WLAN_WEP40_KEYLEN) {
-				memcpy(pbyBuf+8, (unsigned char *)&(dwRevIVCounter), 3);
-				memcpy(pbyBuf+11, pTransmitKey->abyKey, pTransmitKey->uKeyLength);
-			}
-			memcpy(pDevice->abyPRNG, pbyBuf, 16);
-		}
-		// Append IV after Mac Header
-		*pdwIV &= WEP_IV_MASK;//00000000 11111111 11111111 11111111
-		*pdwIV |= (unsigned long)byKeyIndex << 30;
-		*pdwIV = cpu_to_le32(*pdwIV);
-		pDevice->dwIVCounter++;
-		if (pDevice->dwIVCounter > WEP_IV_MASK)
-			pDevice->dwIVCounter = 0;
-
-	} else if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
-		pTransmitKey->wTSC15_0++;
-		if (pTransmitKey->wTSC15_0 == 0)
-			pTransmitKey->dwTSC47_16++;
-
-		TKIPvMixKey(pTransmitKey->abyKey, pDevice->abyCurrentNetAddr,
-			    pTransmitKey->wTSC15_0, pTransmitKey->dwTSC47_16, pDevice->abyPRNG);
-		memcpy(pbyBuf, pDevice->abyPRNG, 16);
-		// Make IV
-		memcpy(pdwIV, pDevice->abyPRNG, 3);
-
-		*(pbyIVHead+3) = (unsigned char)(((byKeyIndex << 6) & 0xc0) | 0x20); // 0x20 is ExtIV
-		// Append IV&ExtIV after Mac Header
-		*pdwExtIV = cpu_to_le32(pTransmitKey->dwTSC47_16);
-		pr_debug("vFillTxKey()---- pdwExtIV: %lx\n", *pdwExtIV);
-
-	} else if (pTransmitKey->byCipherSuite == KEY_CTL_CCMP) {
-		pTransmitKey->wTSC15_0++;
-		if (pTransmitKey->wTSC15_0 == 0)
-			pTransmitKey->dwTSC47_16++;
-
-		memcpy(pbyBuf, pTransmitKey->abyKey, 16);
-
-		// Make IV
-		*pdwIV = 0;
-		*(pbyIVHead+3) = (unsigned char)(((byKeyIndex << 6) & 0xc0) | 0x20); // 0x20 is ExtIV
-		*pdwIV |= cpu_to_le16((unsigned short)(pTransmitKey->wTSC15_0));
-		//Append IV&ExtIV after Mac Header
-		*pdwExtIV = cpu_to_le32(pTransmitKey->dwTSC47_16);
-
-		/* MICHDR0 */
-		mic_hdr->id = 0x59;
-		mic_hdr->tx_priority = 0;
-		memcpy(mic_hdr->mic_addr2, pMACHeader->abyAddr2, ETH_ALEN);
-
-		/* ccmp pn big endian order */
-		mic_hdr->ccmp_pn[0] = (u8)(pTransmitKey->dwTSC47_16 >> 24);
-		mic_hdr->ccmp_pn[1] = (u8)(pTransmitKey->dwTSC47_16 >> 16);
-		mic_hdr->ccmp_pn[2] = (u8)(pTransmitKey->dwTSC47_16 >> 8);
-		mic_hdr->ccmp_pn[3] = (u8)pTransmitKey->dwTSC47_16;
-		mic_hdr->ccmp_pn[4] = (u8)(pTransmitKey->wTSC15_0 >> 8);
-		mic_hdr->ccmp_pn[5] = (u8)pTransmitKey->wTSC15_0;
-
-		/* MICHDR1 */
-		mic_hdr->payload_len = cpu_to_be16(wPayloadLen);
-
-		if (pDevice->bLongHeader)
-			mic_hdr->hlen = cpu_to_be16(28);
-		else
-			mic_hdr->hlen = cpu_to_be16(22);
-
-		memcpy(mic_hdr->addr1, pMACHeader->abyAddr1, ETH_ALEN);
-		memcpy(mic_hdr->addr2, pMACHeader->abyAddr2, ETH_ALEN);
-
-		/* MICHDR2 */
-		memcpy(mic_hdr->addr3, pMACHeader->abyAddr3, ETH_ALEN);
-		mic_hdr->frame_control =
-				cpu_to_le16(pMACHeader->wFrameCtl & 0xc78f);
-		mic_hdr->seq_ctrl = cpu_to_le16(pMACHeader->wSeqCtl & 0xf);
-
-		if (pDevice->bLongHeader)
-			memcpy(mic_hdr->addr4, pMACHeader->abyAddr4, ETH_ALEN);
-	}
-}
-
-static
-void
-s_vSWencryption(
-	struct vnt_private *pDevice,
-	PSKeyItem           pTransmitKey,
-	unsigned char *pbyPayloadHead,
-	unsigned short wPayloadSize
-)
-{
-	unsigned int cbICVlen = 4;
-	unsigned long dwICV = 0xFFFFFFFFL;
-	unsigned long *pdwICV;
-
-	if (pTransmitKey == NULL)
-		return;
-
-	if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) {
-		//=======================================================================
-		// Append ICV after payload
-		dwICV = CRCdwGetCrc32Ex(pbyPayloadHead, wPayloadSize, dwICV);//ICV(Payload)
-		pdwICV = (unsigned long *)(pbyPayloadHead + wPayloadSize);
-		// finally, we must invert dwCRC to get the correct answer
-		*pdwICV = cpu_to_le32(~dwICV);
-		// RC4 encryption
-		rc4_init(&pDevice->SBox, pDevice->abyPRNG, pTransmitKey->uKeyLength + 3);
-		rc4_encrypt(&pDevice->SBox, pbyPayloadHead, pbyPayloadHead, wPayloadSize+cbICVlen);
-		//=======================================================================
-	} else if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
-		//=======================================================================
-		//Append ICV after payload
-		dwICV = CRCdwGetCrc32Ex(pbyPayloadHead, wPayloadSize, dwICV);//ICV(Payload)
-		pdwICV = (unsigned long *)(pbyPayloadHead + wPayloadSize);
-		// finally, we must invert dwCRC to get the correct answer
-		*pdwICV = cpu_to_le32(~dwICV);
-		// RC4 encryption
-		rc4_init(&pDevice->SBox, pDevice->abyPRNG, TKIP_KEY_LEN);
-		rc4_encrypt(&pDevice->SBox, pbyPayloadHead, pbyPayloadHead, wPayloadSize+cbICVlen);
-		//=======================================================================
-	}
-}
-
 static __le16 vnt_time_stamp_off(struct vnt_private *priv, u16 rate)
 {
 	return cpu_to_le16(wTimeStampOff[priv->byPreambleType % 2]
@@ -683,7 +498,8 @@ s_uFillDataHead(
 	unsigned int cbLastFragmentSize,
 	unsigned int uMACfragNum,
 	unsigned char byFBOption,
-	unsigned short wCurrentRate
+	unsigned short wCurrentRate,
+	bool is_pspoll
 )
 {
 
@@ -702,15 +518,24 @@ s_uFillDataHead(
 					  pDevice->byTopCCKBasicRate,
 					  PK_TYPE_11B, &buf->b);
 
-			/* Get Duration and TimeStamp */
-			buf->duration_a = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength,
-									      byPktType, wCurrentRate, bNeedAck, uFragIdx,
-									      cbLastFragmentSize, uMACfragNum,
-									      byFBOption));
-			buf->duration_b = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength,
-									      PK_TYPE_11B, pDevice->byTopCCKBasicRate,
-									      bNeedAck, uFragIdx, cbLastFragmentSize,
-									      uMACfragNum, byFBOption));
+			if (is_pspoll) {
+				__le16 dur = cpu_to_le16(pDevice->current_aid | BIT(14) | BIT(15));
+
+				buf->duration_a = dur;
+				buf->duration_b = dur;
+			} else {
+				/* Get Duration and TimeStamp */
+				buf->duration_a =
+					cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength,
+									    byPktType, wCurrentRate, bNeedAck, uFragIdx,
+									    cbLastFragmentSize, uMACfragNum,
+									    byFBOption));
+				buf->duration_b =
+					cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength,
+									    PK_TYPE_11B, pDevice->byTopCCKBasicRate,
+									    bNeedAck, uFragIdx, cbLastFragmentSize,
+									    uMACfragNum, byFBOption));
+			}
 
 			buf->time_stamp_off_a = vnt_time_stamp_off(pDevice, wCurrentRate);
 			buf->time_stamp_off_b = vnt_time_stamp_off(pDevice, pDevice->byTopCCKBasicRate);
@@ -764,11 +589,18 @@ s_uFillDataHead(
 			vnt_get_phy_field(pDevice, cbFrameLength, wCurrentRate,
 					  byPktType, &buf->ab);
 
-			/* Get Duration and TimeStampOff */
-			buf->duration = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
+			if (is_pspoll) {
+				__le16 dur = cpu_to_le16(pDevice->current_aid | BIT(14) | BIT(15));
+
+				buf->duration = dur;
+			} else {
+				/* Get Duration and TimeStampOff */
+				buf->duration =
+					cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
 									    wCurrentRate, bNeedAck, uFragIdx,
 									    cbLastFragmentSize, uMACfragNum,
 									    byFBOption));
+			}
 
 			buf->time_stamp_off = vnt_time_stamp_off(pDevice, wCurrentRate);
 			return buf->duration;
@@ -778,17 +610,27 @@ s_uFillDataHead(
 		/* Get SignalField, ServiceField & Length */
 		vnt_get_phy_field(pDevice, cbFrameLength, wCurrentRate,
 				  byPktType, &buf->ab);
-		/* Get Duration and TimeStampOff */
-		buf->duration = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength, byPktType,
+
+		if (is_pspoll) {
+			__le16 dur = cpu_to_le16(pDevice->current_aid | BIT(14) | BIT(15));
+
+			buf->duration = dur;
+		} else {
+			/* Get Duration and TimeStampOff */
+			buf->duration =
+				cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength, byPktType,
 								    wCurrentRate, bNeedAck, uFragIdx,
 								    cbLastFragmentSize, uMACfragNum,
 								    byFBOption));
+		}
+
 		buf->time_stamp_off = vnt_time_stamp_off(pDevice, wCurrentRate);
 		return buf->duration;
 	}
 	return 0;
 }
 
+
 static
 void
 s_vFillRTSHead(
@@ -798,7 +640,7 @@ s_vFillRTSHead(
 	unsigned int cbFrameLength,
 	bool bNeedAck,
 	bool bDisCRC,
-	PSEthernetHeader psEthHeader,
+	struct ieee80211_hdr *hdr,
 	unsigned short wCurrentRate,
 	unsigned char byFBOption
 )
@@ -850,18 +692,8 @@ s_vFillRTSHead(
 					cpu_to_le16(IEEE80211_FTYPE_CTL |
 						    IEEE80211_STYPE_RTS);
 
-
-			if ((pDevice->op_mode == NL80211_IFTYPE_ADHOC) ||
-			    (pDevice->op_mode == NL80211_IFTYPE_AP)) {
-				memcpy(&buf->data.ra, psEthHeader->abyDstAddr, ETH_ALEN);
-			} else {
-				memcpy(&buf->data.ra, pDevice->abyBSSID, ETH_ALEN);
-			}
-			if (pDevice->op_mode == NL80211_IFTYPE_AP)
-				memcpy(&buf->data.ta, pDevice->abyBSSID, ETH_ALEN);
-			else
-				memcpy(&buf->data.ta, psEthHeader->abySrcAddr, ETH_ALEN);
-
+			ether_addr_copy(buf->data.ra, hdr->addr1);
+			ether_addr_copy(buf->data.ta, hdr->addr2);
 		} else {
 			struct vnt_rts_g_fb *buf = pvRTS;
 			/* Get SignalField, ServiceField & Length */
@@ -914,19 +746,8 @@ s_vFillRTSHead(
 					cpu_to_le16(IEEE80211_FTYPE_CTL |
 						    IEEE80211_STYPE_RTS);
 
-
-			if ((pDevice->op_mode == NL80211_IFTYPE_ADHOC) ||
-			    (pDevice->op_mode == NL80211_IFTYPE_AP)) {
-				memcpy(&buf->data.ra, psEthHeader->abyDstAddr, ETH_ALEN);
-			} else {
-				memcpy(&buf->data.ra, pDevice->abyBSSID, ETH_ALEN);
-			}
-
-			if (pDevice->op_mode == NL80211_IFTYPE_AP)
-				memcpy(&buf->data.ta, pDevice->abyBSSID, ETH_ALEN);
-			else
-				memcpy(&buf->data.ta, psEthHeader->abySrcAddr, ETH_ALEN);
-
+			ether_addr_copy(buf->data.ra, hdr->addr1);
+			ether_addr_copy(buf->data.ta, hdr->addr2);
 		} // if (byFBOption == AUTO_FB_NONE)
 	} else if (byPktType == PK_TYPE_11A) {
 		if (byFBOption == AUTO_FB_NONE) {
@@ -947,19 +768,8 @@ s_vFillRTSHead(
 					cpu_to_le16(IEEE80211_FTYPE_CTL |
 						    IEEE80211_STYPE_RTS);
 
-
-			if ((pDevice->op_mode == NL80211_IFTYPE_ADHOC) ||
-			    (pDevice->op_mode == NL80211_IFTYPE_AP)) {
-				memcpy(&buf->data.ra, psEthHeader->abyDstAddr, ETH_ALEN);
-			} else {
-				memcpy(&buf->data.ra, pDevice->abyBSSID, ETH_ALEN);
-			}
-
-			if (pDevice->op_mode == NL80211_IFTYPE_AP)
-				memcpy(&buf->data.ta, pDevice->abyBSSID, ETH_ALEN);
-			else
-				memcpy(&buf->data.ta, psEthHeader->abySrcAddr, ETH_ALEN);
-
+			ether_addr_copy(buf->data.ra, hdr->addr1);
+			ether_addr_copy(buf->data.ta, hdr->addr2);
 		} else {
 			struct vnt_rts_a_fb *buf = pvRTS;
 			/* Get SignalField, ServiceField & Length */
@@ -988,16 +798,8 @@ s_vFillRTSHead(
 					cpu_to_le16(IEEE80211_FTYPE_CTL |
 						    IEEE80211_STYPE_RTS);
 
-			if ((pDevice->op_mode == NL80211_IFTYPE_ADHOC) ||
-			    (pDevice->op_mode == NL80211_IFTYPE_AP)) {
-				memcpy(&buf->data.ra, psEthHeader->abyDstAddr, ETH_ALEN);
-			} else {
-				memcpy(&buf->data.ra, pDevice->abyBSSID, ETH_ALEN);
-			}
-			if (pDevice->op_mode == NL80211_IFTYPE_AP)
-				memcpy(&buf->data.ta, pDevice->abyBSSID, ETH_ALEN);
-			else
-				memcpy(&buf->data.ta, psEthHeader->abySrcAddr, ETH_ALEN);
+			ether_addr_copy(buf->data.ra, hdr->addr1);
+			ether_addr_copy(buf->data.ta, hdr->addr2);
 		}
 	} else if (byPktType == PK_TYPE_11B) {
 		struct vnt_rts_ab *buf = pvRTS;
@@ -1016,17 +818,8 @@ s_vFillRTSHead(
 		buf->data.frame_control =
 			cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_RTS);
 
-		if ((pDevice->op_mode == NL80211_IFTYPE_ADHOC) ||
-		    (pDevice->op_mode == NL80211_IFTYPE_AP)) {
-			memcpy(&buf->data.ra, psEthHeader->abyDstAddr, ETH_ALEN);
-		} else {
-			memcpy(&buf->data.ra, pDevice->abyBSSID, ETH_ALEN);
-		}
-
-		if (pDevice->op_mode == NL80211_IFTYPE_AP)
-			memcpy(&buf->data.ta, pDevice->abyBSSID, ETH_ALEN);
-		else
-			memcpy(&buf->data.ta, psEthHeader->abySrcAddr, ETH_ALEN);
+		ether_addr_copy(buf->data.ra, hdr->addr1);
+		ether_addr_copy(buf->data.ta, hdr->addr2);
 	}
 }
 
@@ -1093,7 +886,8 @@ s_vFillCTSHead(
 
 			buf->reserved2 = 0x0;
 
-			memcpy(&buf->data.ra, pDevice->abyCurrentNetAddr, ETH_ALEN);
+			ether_addr_copy(buf->data.ra,
+					pDevice->abyCurrentNetAddr);
 		} else { //if (byFBOption != AUTO_FB_NONE && uDMAIdx != TYPE_ATIMDMA && uDMAIdx != TYPE_BEACONDMA)
 			struct vnt_cts *buf = pvCTS;
 			/* Get SignalField, ServiceField & Length */
@@ -1116,7 +910,8 @@ s_vFillCTSHead(
 					    IEEE80211_STYPE_CTS);
 
 			buf->reserved2 = 0x0;
-			memcpy(&buf->data.ra, pDevice->abyCurrentNetAddr, ETH_ALEN);
+			ether_addr_copy(buf->data.ra,
+					pDevice->abyCurrentNetAddr);
 		}
 	}
 }
@@ -1156,11 +951,10 @@ s_vGenerateTxParameter(
 	unsigned int cbFrameSize,
 	bool bNeedACK,
 	unsigned int uDMAIdx,
-	PSEthernetHeader psEthHeader,
+	void *psEthHeader,
 	unsigned short wCurrentRate
 )
 {
-	unsigned int cbMACHdLen = WLAN_HDR_ADDR3_LEN; //24
 	unsigned short wFifoCtl;
 	bool bDisCRC = false;
 	unsigned char byFBOption = AUTO_FB_NONE;
@@ -1178,9 +972,6 @@ s_vGenerateTxParameter(
 	else if (wFifoCtl & FIFOCTL_AUTO_FB_1)
 		byFBOption = AUTO_FB_1;
 
-	if (pDevice->bLongHeader)
-		cbMACHdLen = WLAN_HDR_ADDR3_LEN + 6;
-
 	if (!pvRrvTime)
 		return;
 
@@ -1237,90 +1028,30 @@ s_vGenerateTxParameter(
 	}
 }
 
-static
-void
-s_vFillFragParameter(
-	struct vnt_private *pDevice,
-	unsigned char *pbyBuffer,
-	unsigned int uTxType,
-	void *pvtdCurr,
-	unsigned short wFragType,
-	unsigned int cbReqCount
-)
-{
-	PSTxBufHead pTxBufHead = (PSTxBufHead) pbyBuffer;
-
-	if (uTxType == TYPE_SYNCDMA) {
-		PSTxSyncDesc ptdCurr = (PSTxSyncDesc)pvtdCurr;
-
-		//Set FIFOCtl & TimeStamp in TxSyncDesc
-		ptdCurr->m_wFIFOCtl = pTxBufHead->wFIFOCtl;
-		ptdCurr->m_wTimeStamp = pTxBufHead->wTimeStamp;
-		//Set TSR1 & ReqCount in TxDescHead
-		ptdCurr->m_td1TD1.wReqCount = cpu_to_le16((unsigned short)(cbReqCount));
-		if (wFragType == FRAGCTL_ENDFRAG) //Last Fragmentation
-			ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP | EDMSDU);
-		else
-			ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP);
-	} else {
-		PSTxDesc ptdCurr = (PSTxDesc)pvtdCurr;
-		//Set TSR1 & ReqCount in TxDescHead
-		ptdCurr->m_td1TD1.wReqCount = cpu_to_le16((unsigned short)(cbReqCount));
-		if (wFragType == FRAGCTL_ENDFRAG) //Last Fragmentation
-			ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP | EDMSDU);
-		else
-			ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP);
-	}
-
-	pTxBufHead->wFragCtl |= (unsigned short)wFragType;//0x0001; //0000 0000 0000 0001
-}
-
 static unsigned int
 s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
-		  unsigned char *pbyTxBufferAddr, unsigned int cbFrameBodySize,
+		  unsigned char *pbyTxBufferAddr,
 		  unsigned int uDMAIdx, PSTxDesc pHeadTD,
-		  PSEthernetHeader psEthHeader, unsigned char *pPacket,
-		  bool bNeedEncrypt, PSKeyItem pTransmitKey,
-		  unsigned int uNodeIndex, unsigned int *puMACfragNum)
+		  unsigned int is_pspoll)
 {
-	unsigned int cbMACHdLen;
+	PDEVICE_TD_INFO td_info = pHeadTD->pTDInfo;
+	struct sk_buff *skb = td_info->skb;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	struct vnt_tx_fifo_head *tx_buffer_head =
+			(struct vnt_tx_fifo_head *)td_info->buf;
+	u16 fifo_ctl = le16_to_cpu(tx_buffer_head->fifo_ctl);
 	unsigned int cbFrameSize;
-	unsigned int cbFragmentSize; //Hdr+(IV)+payoad+(MIC)+(ICV)+FCS
-	unsigned int cbFragPayloadSize;
-	unsigned int cbLastFragmentSize; //Hdr+(IV)+payoad+(MIC)+(ICV)+FCS
-	unsigned int cbLastFragPayloadSize;
-	unsigned int uFragIdx;
-	unsigned char *pbyPayloadHead;
-	unsigned char *pbyIVHead;
-	unsigned char *pbyMacHdr;
-	unsigned short wFragType; //00:Non-Frag, 01:Start, 10:Mid, 11:Last
 	__le16 uDuration;
 	unsigned char *pbyBuffer;
-	unsigned int cbIVlen = 0;
-	unsigned int cbICVlen = 0;
-	unsigned int cbMIClen = 0;
-	unsigned int cbFCSlen = 4;
-	unsigned int cb802_1_H_len = 0;
 	unsigned int uLength = 0;
-	unsigned int uTmpLen = 0;
 	unsigned int cbMICHDR = 0;
-	u32 dwMICKey0, dwMICKey1;
-	u32 dwMIC_Priority;
-	u32 *pdwMIC_L;
-	u32 *pdwMIC_R;
-	u32 dwSafeMIC_L, dwSafeMIC_R; /* Fix "Last Frag Size" < "MIC length". */
-	bool bMIC2Frag = false;
-	unsigned int uMICFragLen = 0;
 	unsigned int uMACfragNum = 1;
 	unsigned int uPadding = 0;
 	unsigned int cbReqCount = 0;
-
-	bool bNeedACK;
-	bool bRTS;
-	bool bIsAdhoc;
-	unsigned char *pbyType;
+	bool bNeedACK = (bool)(fifo_ctl & FIFOCTL_NEEDACK);
+	bool bRTS = (bool)(fifo_ctl & FIFOCTL_RTS);
 	PSTxDesc       ptdCurr;
-	PSTxBufHead    psTxBufHd = (PSTxBufHead) pbyTxBufferAddr;
 	unsigned int cbHeaderLength = 0;
 	void *pvRrvTime;
 	struct vnt_mic_hdr *pMICHDR;
@@ -1328,72 +1059,35 @@ s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
 	void *pvCTS;
 	void *pvTxDataHd;
 	unsigned short wTxBufSize;   // FFinfo size
-	unsigned int uTotalCopyLength = 0;
 	unsigned char byFBOption = AUTO_FB_NONE;
-	bool bIsWEP256 = false;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
 
 	pvRrvTime = pMICHDR = pvRTS = pvCTS = pvTxDataHd = NULL;
 
-	if ((pDevice->op_mode == NL80211_IFTYPE_ADHOC) ||
-	    (pDevice->op_mode == NL80211_IFTYPE_AP)) {
-		if (is_multicast_ether_addr(&(psEthHeader->abyDstAddr[0])))
-			bNeedACK = false;
-		else
-			bNeedACK = true;
-		bIsAdhoc = true;
-	} else {
-		// MSDUs in Infra mode always need ACK
-		bNeedACK = true;
-		bIsAdhoc = false;
-	}
+	cbFrameSize = skb->len + 4;
 
-	if (pDevice->bLongHeader)
-		cbMACHdLen = WLAN_HDR_ADDR3_LEN + 6;
-	else
-		cbMACHdLen = WLAN_HDR_ADDR3_LEN;
-
-	if ((bNeedEncrypt == true) && (pTransmitKey != NULL)) {
-		if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) {
-			cbIVlen = 4;
-			cbICVlen = 4;
-			if (pTransmitKey->uKeyLength == WLAN_WEP232_KEYLEN)
-				bIsWEP256 = true;
-		}
-		if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
-			cbIVlen = 8;//IV+ExtIV
-			cbMIClen = 8;
-			cbICVlen = 4;
-		}
-		if (pTransmitKey->byCipherSuite == KEY_CTL_CCMP) {
-			cbIVlen = 8;//RSN Header
-			cbICVlen = 8;//MIC
+	if (info->control.hw_key) {
+		switch (info->control.hw_key->cipher) {
+		case WLAN_CIPHER_SUITE_CCMP:
 			cbMICHDR = sizeof(struct vnt_mic_hdr);
+		default:
+			break;
 		}
+
+		cbFrameSize += info->control.hw_key->icv_len;
+
 		if (pDevice->byLocalID > REV_ID_VT3253_A1) {
 			//MAC Header should be padding 0 to DW alignment.
-			uPadding = 4 - (cbMACHdLen%4);
+			uPadding = 4 - (ieee80211_get_hdrlen_from_skb(skb) % 4);
 			uPadding %= 4;
 		}
 	}
 
-	cbFrameSize = cbMACHdLen + cbIVlen + (cbFrameBodySize + cbMIClen) + cbICVlen + cbFCSlen;
-
-	if ((bNeedACK == false) ||
-	    (cbFrameSize < pDevice->wRTSThreshold) ||
-	    ((cbFrameSize >= pDevice->wFragmentationThreshold) && (pDevice->wFragmentationThreshold <= pDevice->wRTSThreshold))
-) {
-		bRTS = false;
-	} else {
-		bRTS = true;
-		psTxBufHd->wFIFOCtl |= (FIFOCTL_RTS | FIFOCTL_LRETRY);
-	}
 	//
 	// Use for AUTO FALL BACK
 	//
-	if (psTxBufHd->wFIFOCtl & FIFOCTL_AUTO_FB_0)
+	if (fifo_ctl & FIFOCTL_AUTO_FB_0)
 		byFBOption = AUTO_FB_0;
-	else if (psTxBufHd->wFIFOCtl & FIFOCTL_AUTO_FB_1)
+	else if (fifo_ctl & FIFOCTL_AUTO_FB_1)
 		byFBOption = AUTO_FB_1;
 
 	//////////////////////////////////////////////////////
@@ -1487,1477 +1181,345 @@ s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
 			}
 		} // Auto Fall Back
 	}
-	memset((void *)(pbyTxBufferAddr + wTxBufSize), 0, (cbHeaderLength - wTxBufSize));
-
-//////////////////////////////////////////////////////////////////
-	if ((bNeedEncrypt == true) && (pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
-		if (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
-			dwMICKey0 = *(u32 *)(&pTransmitKey->abyKey[16]);
-			dwMICKey1 = *(u32 *)(&pTransmitKey->abyKey[20]);
-		} else if ((pTransmitKey->dwKeyIndex & AUTHENTICATOR_KEY) != 0) {
-			dwMICKey0 = *(u32 *)(&pTransmitKey->abyKey[16]);
-			dwMICKey1 = *(u32 *)(&pTransmitKey->abyKey[20]);
-		} else {
-			dwMICKey0 = *(u32 *)(&pTransmitKey->abyKey[24]);
-			dwMICKey1 = *(u32 *)(&pTransmitKey->abyKey[28]);
-		}
-		// DO Software Michael
-		MIC_vInit(dwMICKey0, dwMICKey1);
-		MIC_vAppend((unsigned char *)&(psEthHeader->abyDstAddr[0]), 12);
-		dwMIC_Priority = 0;
-		MIC_vAppend((unsigned char *)&dwMIC_Priority, 4);
-		pr_debug("MIC KEY: %X, %X\n", dwMICKey0, dwMICKey1);
-	}
-
-///////////////////////////////////////////////////////////////////
-
-	pbyMacHdr = (unsigned char *)(pbyTxBufferAddr + cbHeaderLength);
-	pbyPayloadHead = (unsigned char *)(pbyMacHdr + cbMACHdLen + uPadding + cbIVlen);
-	pbyIVHead = (unsigned char *)(pbyMacHdr + cbMACHdLen + uPadding);
-
-	if ((cbFrameSize > pDevice->wFragmentationThreshold) && (bNeedACK == true) && (bIsWEP256 == false)) {
-		// Fragmentation
-		// FragThreshold = Fragment size(Hdr+(IV)+fragment payload+(MIC)+(ICV)+FCS)
-		cbFragmentSize = pDevice->wFragmentationThreshold;
-		cbFragPayloadSize = cbFragmentSize - cbMACHdLen - cbIVlen - cbICVlen - cbFCSlen;
-		//FragNum = (FrameSize-(Hdr+FCS))/(Fragment Size -(Hrd+FCS)))
-		uMACfragNum = (unsigned short) ((cbFrameBodySize + cbMIClen) / cbFragPayloadSize);
-		cbLastFragPayloadSize = (cbFrameBodySize + cbMIClen) % cbFragPayloadSize;
-		if (cbLastFragPayloadSize == 0)
-			cbLastFragPayloadSize = cbFragPayloadSize;
-		else
-			uMACfragNum++;
-
-		//[Hdr+(IV)+last fragment payload+(MIC)+(ICV)+FCS]
-		cbLastFragmentSize = cbMACHdLen + cbLastFragPayloadSize + cbIVlen + cbICVlen + cbFCSlen;
-
-		for (uFragIdx = 0; uFragIdx < uMACfragNum; uFragIdx++) {
-			if (uFragIdx == 0) {
-				//=========================
-				//    Start Fragmentation
-				//=========================
-				pr_debug("Start Fragmentation...\n");
-				wFragType = FRAGCTL_STAFRAG;
-
-				//Fill FIFO,RrvTime,RTS,and CTS
-				s_vGenerateTxParameter(pDevice, byPktType, (void *)psTxBufHd, pvRrvTime, pvRTS, pvCTS,
-						       cbFragmentSize, bNeedACK, uDMAIdx, psEthHeader, pDevice->wCurrentRate);
-				//Fill DataHead
-				uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbFragmentSize, uDMAIdx, bNeedACK,
-							    uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption, pDevice->wCurrentRate);
-				// Generate TX MAC Header
-				vGenerateMACHeader(pDevice, pbyMacHdr, uDuration, psEthHeader, bNeedEncrypt,
-						   wFragType, uDMAIdx, uFragIdx);
-
-				if (bNeedEncrypt == true) {
-					//Fill TXKEY
-					s_vFillTxKey(pDevice, (unsigned char *)(psTxBufHd->adwTxKey), pbyIVHead, pTransmitKey,
-						     pbyMacHdr, (unsigned short)cbFragPayloadSize, (unsigned char *)pMICHDR);
-					//Fill IV(ExtIV,RSNHDR)
-					if (pDevice->bEnableHostWEP) {
-						pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
-						pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
-					}
-				}
-
-				// 802.1H
-				if (ntohs(psEthHeader->wType) > ETH_DATA_LEN) {
-					if ((psEthHeader->wType == TYPE_PKT_IPX) ||
-					    (psEthHeader->wType == cpu_to_le16(0xF380))) {
-						memcpy((unsigned char *)(pbyPayloadHead), &pDevice->abySNAP_Bridgetunnel[0], 6);
-					} else {
-						memcpy((unsigned char *)(pbyPayloadHead), &pDevice->abySNAP_RFC1042[0], 6);
-					}
-					pbyType = (unsigned char *)(pbyPayloadHead + 6);
-					memcpy(pbyType, &(psEthHeader->wType), sizeof(unsigned short));
-					cb802_1_H_len = 8;
-				}
-
-				cbReqCount = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen + cbFragPayloadSize;
-				//---------------------------
-				// S/W or H/W Encryption
-				//---------------------------
-				pbyBuffer = (unsigned char *)pHeadTD->pTDInfo->buf;
-
-				uLength = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen + cb802_1_H_len;
-				//copy TxBufferHeader + MacHeader to desc
-				memcpy(pbyBuffer, (void *)psTxBufHd, uLength);
-
-				// Copy the Packet into a tx Buffer
-				memcpy((pbyBuffer + uLength), (pPacket + 14), (cbFragPayloadSize - cb802_1_H_len));
-
-				uTotalCopyLength += cbFragPayloadSize - cb802_1_H_len;
-
-				if ((bNeedEncrypt == true) && (pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
-					pr_debug("Start MIC: %d\n",
-						 cbFragPayloadSize);
-					MIC_vAppend((pbyBuffer + uLength - cb802_1_H_len), cbFragPayloadSize);
-
-				}
-
-				//---------------------------
-				// S/W Encryption
-				//---------------------------
-				if ((pDevice->byLocalID <= REV_ID_VT3253_A1)) {
-					if (bNeedEncrypt) {
-						s_vSWencryption(pDevice, pTransmitKey, (pbyBuffer + uLength - cb802_1_H_len), (unsigned short)cbFragPayloadSize);
-						cbReqCount += cbICVlen;
-					}
-				}
-
-				ptdCurr = (PSTxDesc)pHeadTD;
-				//--------------------
-				//1.Set TSR1 & ReqCount in TxDescHead
-				//2.Set FragCtl in TxBufferHead
-				//3.Set Frame Control
-				//4.Set Sequence Control
-				//5.Get S/W generate FCS
-				//--------------------
-				s_vFillFragParameter(pDevice, pbyBuffer, uDMAIdx, (void *)ptdCurr, wFragType, cbReqCount);
-
-				ptdCurr->pTDInfo->dwReqCount = cbReqCount - uPadding;
-				ptdCurr->pTDInfo->dwHeaderLength = cbHeaderLength;
-				ptdCurr->pTDInfo->skb_dma = ptdCurr->pTDInfo->buf_dma;
-				ptdCurr->buff_addr = cpu_to_le32(ptdCurr->pTDInfo->skb_dma);
-				pDevice->iTDUsed[uDMAIdx]++;
-				pHeadTD = ptdCurr->next;
-			} else if (uFragIdx == (uMACfragNum-1)) {
-				//=========================
-				//    Last Fragmentation
-				//=========================
-				pr_debug("Last Fragmentation...\n");
-
-				wFragType = FRAGCTL_ENDFRAG;
-
-				//Fill FIFO,RrvTime,RTS,and CTS
-				s_vGenerateTxParameter(pDevice, byPktType, (void *)psTxBufHd, pvRrvTime, pvRTS, pvCTS,
-						       cbLastFragmentSize, bNeedACK, uDMAIdx, psEthHeader, pDevice->wCurrentRate);
-				//Fill DataHead
-				uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbLastFragmentSize, uDMAIdx, bNeedACK,
-							    uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption, pDevice->wCurrentRate);
-
-				// Generate TX MAC Header
-				vGenerateMACHeader(pDevice, pbyMacHdr, uDuration, psEthHeader, bNeedEncrypt,
-						   wFragType, uDMAIdx, uFragIdx);
-
-				if (bNeedEncrypt == true) {
-					//Fill TXKEY
-					s_vFillTxKey(pDevice, (unsigned char *)(psTxBufHd->adwTxKey), pbyIVHead, pTransmitKey,
-						     pbyMacHdr, (unsigned short)cbLastFragPayloadSize, (unsigned char *)pMICHDR);
-
-					if (pDevice->bEnableHostWEP) {
-						pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
-						pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
-					}
-
-				}
-
-				cbReqCount = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen + cbLastFragPayloadSize;
-				//---------------------------
-				// S/W or H/W Encryption
-				//---------------------------
-
-				pbyBuffer = (unsigned char *)pHeadTD->pTDInfo->buf;
-
-				uLength = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen;
-
-				//copy TxBufferHeader + MacHeader to desc
-				memcpy(pbyBuffer, (void *)psTxBufHd, uLength);
-
-				// Copy the Packet into a tx Buffer
-				if (bMIC2Frag == false) {
-					memcpy((pbyBuffer + uLength),
-					       (pPacket + 14 + uTotalCopyLength),
-					       (cbLastFragPayloadSize - cbMIClen)
-);
-					//TODO check uTmpLen !
-					uTmpLen = cbLastFragPayloadSize - cbMIClen;
-
-				}
-				if ((bNeedEncrypt == true) && (pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
-					pr_debug("LAST: uMICFragLen:%d, cbLastFragPayloadSize:%d, uTmpLen:%d\n",
-						 uMICFragLen,
-						 cbLastFragPayloadSize,
-						 uTmpLen);
-
-					if (bMIC2Frag == false) {
-						if (uTmpLen != 0)
-							MIC_vAppend((pbyBuffer + uLength), uTmpLen);
-						pdwMIC_L = (u32 *)(pbyBuffer + uLength + uTmpLen);
-						pdwMIC_R = (u32 *)(pbyBuffer + uLength + uTmpLen + 4);
-						MIC_vGetMIC(pdwMIC_L, pdwMIC_R);
-						pr_debug("Last MIC:%X, %X\n",
-							 *pdwMIC_L, *pdwMIC_R);
-					} else {
-						if (uMICFragLen >= 4) {
-							memcpy((pbyBuffer + uLength), ((unsigned char *)&dwSafeMIC_R + (uMICFragLen - 4)),
-							       (cbMIClen - uMICFragLen));
-							pr_debug("LAST: uMICFragLen >= 4: %X, %d\n",
-								 *(unsigned char *)((unsigned char *)&dwSafeMIC_R + (uMICFragLen - 4)),
-								 (cbMIClen - uMICFragLen));
-
-						} else {
-							memcpy((pbyBuffer + uLength), ((unsigned char *)&dwSafeMIC_L + uMICFragLen),
-							       (4 - uMICFragLen));
-							memcpy((pbyBuffer + uLength + (4 - uMICFragLen)), &dwSafeMIC_R, 4);
-							pr_debug("LAST: uMICFragLen < 4: %X, %d\n",
-								 *(unsigned char *)((unsigned char *)&dwSafeMIC_R + uMICFragLen - 4),
-								 (cbMIClen - uMICFragLen));
-						}
-					}
-					MIC_vUnInit();
-				} else {
-					ASSERT(uTmpLen == (cbLastFragPayloadSize - cbMIClen));
-				}
-
-				//---------------------------
-				// S/W Encryption
-				//---------------------------
-				if ((pDevice->byLocalID <= REV_ID_VT3253_A1)) {
-					if (bNeedEncrypt) {
-						s_vSWencryption(pDevice, pTransmitKey, (pbyBuffer + uLength), (unsigned short)cbLastFragPayloadSize);
-						cbReqCount += cbICVlen;
-					}
-				}
-
-				ptdCurr = (PSTxDesc)pHeadTD;
-
-				//--------------------
-				//1.Set TSR1 & ReqCount in TxDescHead
-				//2.Set FragCtl in TxBufferHead
-				//3.Set Frame Control
-				//4.Set Sequence Control
-				//5.Get S/W generate FCS
-				//--------------------
-
-				s_vFillFragParameter(pDevice, pbyBuffer, uDMAIdx, (void *)ptdCurr, wFragType, cbReqCount);
-
-				ptdCurr->pTDInfo->dwReqCount = cbReqCount - uPadding;
-				ptdCurr->pTDInfo->dwHeaderLength = cbHeaderLength;
-				ptdCurr->pTDInfo->skb_dma = ptdCurr->pTDInfo->buf_dma;
-				ptdCurr->buff_addr = cpu_to_le32(ptdCurr->pTDInfo->skb_dma);
-				pDevice->iTDUsed[uDMAIdx]++;
-				pHeadTD = ptdCurr->next;
-
-			} else {
-				//=========================
-				//    Middle Fragmentation
-				//=========================
-				pr_debug("Middle Fragmentation...\n");
-
-				wFragType = FRAGCTL_MIDFRAG;
-
-				//Fill FIFO,RrvTime,RTS,and CTS
-				s_vGenerateTxParameter(pDevice, byPktType, (void *)psTxBufHd, pvRrvTime, pvRTS, pvCTS,
-						       cbFragmentSize, bNeedACK, uDMAIdx, psEthHeader, pDevice->wCurrentRate);
-				//Fill DataHead
-				uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbFragmentSize, uDMAIdx, bNeedACK,
-							    uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption, pDevice->wCurrentRate);
-
-				// Generate TX MAC Header
-				vGenerateMACHeader(pDevice, pbyMacHdr, uDuration, psEthHeader, bNeedEncrypt,
-						   wFragType, uDMAIdx, uFragIdx);
-
-				if (bNeedEncrypt == true) {
-					//Fill TXKEY
-					s_vFillTxKey(pDevice, (unsigned char *)(psTxBufHd->adwTxKey), pbyIVHead, pTransmitKey,
-						     pbyMacHdr, (unsigned short)cbFragPayloadSize, (unsigned char *)pMICHDR);
-
-					if (pDevice->bEnableHostWEP) {
-						pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
-						pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
-					}
-				}
-
-				cbReqCount = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen + cbFragPayloadSize;
-				//---------------------------
-				// S/W or H/W Encryption
-				//---------------------------
-
-				pbyBuffer = (unsigned char *)pHeadTD->pTDInfo->buf;
-				uLength = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen;
-
-				//copy TxBufferHeader + MacHeader to desc
-				memcpy(pbyBuffer, (void *)psTxBufHd, uLength);
-
-				// Copy the Packet into a tx Buffer
-				memcpy((pbyBuffer + uLength),
-				       (pPacket + 14 + uTotalCopyLength),
-				       cbFragPayloadSize
-);
-				uTmpLen = cbFragPayloadSize;
-
-				uTotalCopyLength += uTmpLen;
-
-				if ((bNeedEncrypt == true) && (pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
-					MIC_vAppend((pbyBuffer + uLength), uTmpLen);
-
-					if (uTmpLen < cbFragPayloadSize) {
-						bMIC2Frag = true;
-						uMICFragLen = cbFragPayloadSize - uTmpLen;
-						ASSERT(uMICFragLen < cbMIClen);
-
-						pdwMIC_L = (u32 *)(pbyBuffer + uLength + uTmpLen);
-						pdwMIC_R = (u32 *)(pbyBuffer + uLength + uTmpLen + 4);
-						MIC_vGetMIC(pdwMIC_L, pdwMIC_R);
-						dwSafeMIC_L = *pdwMIC_L;
-						dwSafeMIC_R = *pdwMIC_R;
-
-						pr_debug("MIDDLE: uMICFragLen:%d, cbFragPayloadSize:%d, uTmpLen:%d\n",
-							 uMICFragLen,
-							 cbFragPayloadSize,
-							 uTmpLen);
-						pr_debug("Fill MIC in Middle frag [%d]\n",
-							 uMICFragLen);
-						pr_debug("Get MIC:%X, %X\n",
-							 *pdwMIC_L, *pdwMIC_R);
-					}
-					pr_debug("Middle frag len: %d\n",
-						 uTmpLen);
-
-				} else {
-					ASSERT(uTmpLen == (cbFragPayloadSize));
-				}
-
-				if ((pDevice->byLocalID <= REV_ID_VT3253_A1)) {
-					if (bNeedEncrypt) {
-						s_vSWencryption(pDevice, pTransmitKey, (pbyBuffer + uLength), (unsigned short)cbFragPayloadSize);
-						cbReqCount += cbICVlen;
-					}
-				}
-
-				ptdCurr = (PSTxDesc)pHeadTD;
-
-				//--------------------
-				//1.Set TSR1 & ReqCount in TxDescHead
-				//2.Set FragCtl in TxBufferHead
-				//3.Set Frame Control
-				//4.Set Sequence Control
-				//5.Get S/W generate FCS
-				//--------------------
-
-				s_vFillFragParameter(pDevice, pbyBuffer, uDMAIdx, (void *)ptdCurr, wFragType, cbReqCount);
-
-				ptdCurr->pTDInfo->dwReqCount = cbReqCount - uPadding;
-				ptdCurr->pTDInfo->dwHeaderLength = cbHeaderLength;
-				ptdCurr->pTDInfo->skb_dma = ptdCurr->pTDInfo->buf_dma;
-				ptdCurr->buff_addr = cpu_to_le32(ptdCurr->pTDInfo->skb_dma);
-				pDevice->iTDUsed[uDMAIdx]++;
-				pHeadTD = ptdCurr->next;
-			}
-		}  // for (uMACfragNum)
-	} else {
-		//=========================
-		//    No Fragmentation
-		//=========================
-		wFragType = FRAGCTL_NONFRAG;
-
-		//Set FragCtl in TxBufferHead
-		psTxBufHd->wFragCtl |= (unsigned short)wFragType;
-
-		//Fill FIFO,RrvTime,RTS,and CTS
-		s_vGenerateTxParameter(pDevice, byPktType, (void *)psTxBufHd, pvRrvTime, pvRTS, pvCTS,
-				       cbFrameSize, bNeedACK, uDMAIdx, psEthHeader, pDevice->wCurrentRate);
-		//Fill DataHead
-		uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbFrameSize, uDMAIdx, bNeedACK,
-					    0, 0, uMACfragNum, byFBOption, pDevice->wCurrentRate);
-
-		// Generate TX MAC Header
-		vGenerateMACHeader(pDevice, pbyMacHdr, uDuration, psEthHeader, bNeedEncrypt,
-				   wFragType, uDMAIdx, 0);
-
-		if (bNeedEncrypt == true) {
-			//Fill TXKEY
-			s_vFillTxKey(pDevice, (unsigned char *)(psTxBufHd->adwTxKey), pbyIVHead, pTransmitKey,
-				     pbyMacHdr, (unsigned short)cbFrameBodySize, (unsigned char *)pMICHDR);
-
-			if (pDevice->bEnableHostWEP) {
-				pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
-				pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
-			}
-		}
-
-		// 802.1H
-		if (ntohs(psEthHeader->wType) > ETH_DATA_LEN) {
-			if ((psEthHeader->wType == TYPE_PKT_IPX) ||
-			    (psEthHeader->wType == cpu_to_le16(0xF380))) {
-				memcpy((unsigned char *)(pbyPayloadHead), &pDevice->abySNAP_Bridgetunnel[0], 6);
-			} else {
-				memcpy((unsigned char *)(pbyPayloadHead), &pDevice->abySNAP_RFC1042[0], 6);
-			}
-			pbyType = (unsigned char *)(pbyPayloadHead + 6);
-			memcpy(pbyType, &(psEthHeader->wType), sizeof(unsigned short));
-			cb802_1_H_len = 8;
-		}
-
-		cbReqCount = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen + (cbFrameBodySize + cbMIClen);
-		//---------------------------
-		// S/W or H/W Encryption
-		//---------------------------
-		pbyBuffer = (unsigned char *)pHeadTD->pTDInfo->buf;
-		uLength = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen + cb802_1_H_len;
 
-		//copy TxBufferHeader + MacHeader to desc
-		memcpy(pbyBuffer, (void *)psTxBufHd, uLength);
+	td_info->mic_hdr = pMICHDR;
 
-		// Copy the Packet into a tx Buffer
-		memcpy((pbyBuffer + uLength),
-		       (pPacket + 14),
-		       cbFrameBodySize - cb802_1_H_len
-);
-
-		if ((bNeedEncrypt == true) && (pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
-			pr_debug("Length:%d, %d\n",
-				 cbFrameBodySize - cb802_1_H_len, uLength);
-
-			MIC_vAppend((pbyBuffer + uLength - cb802_1_H_len), cbFrameBodySize);
-
-			pdwMIC_L = (u32 *)(pbyBuffer + uLength - cb802_1_H_len + cbFrameBodySize);
-			pdwMIC_R = (u32 *)(pbyBuffer + uLength - cb802_1_H_len + cbFrameBodySize + 4);
-
-			MIC_vGetMIC(pdwMIC_L, pdwMIC_R);
-			MIC_vUnInit();
-
-			if (pDevice->bTxMICFail == true) {
-				*pdwMIC_L = 0;
-				*pdwMIC_R = 0;
-				pDevice->bTxMICFail = false;
-			}
+	memset((void *)(pbyTxBufferAddr + wTxBufSize), 0, (cbHeaderLength - wTxBufSize));
 
-			pr_debug("uLength: %d, %d\n", uLength, cbFrameBodySize);
-			pr_debug("cbReqCount:%d, %d, %d, %d\n",
-				 cbReqCount, cbHeaderLength, uPadding, cbIVlen);
-			pr_debug("MIC:%x, %x\n", *pdwMIC_L, *pdwMIC_R);
+	/* Fill FIFO,RrvTime,RTS,and CTS */
+	s_vGenerateTxParameter(pDevice, byPktType, tx_buffer_head, pvRrvTime, pvRTS, pvCTS,
+			       cbFrameSize, bNeedACK, uDMAIdx, hdr, pDevice->wCurrentRate);
+	/* Fill DataHead */
+	uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbFrameSize, uDMAIdx, bNeedACK,
+				    0, 0, uMACfragNum, byFBOption, pDevice->wCurrentRate, is_pspoll);
 
-		}
+	hdr->duration_id = uDuration;
 
-		if ((pDevice->byLocalID <= REV_ID_VT3253_A1)) {
-			if (bNeedEncrypt) {
-				s_vSWencryption(pDevice, pTransmitKey, (pbyBuffer + uLength - cb802_1_H_len),
-						(unsigned short)(cbFrameBodySize + cbMIClen));
-				cbReqCount += cbICVlen;
-			}
-		}
+	cbReqCount = cbHeaderLength + uPadding + skb->len;
+	pbyBuffer = (unsigned char *)pHeadTD->pTDInfo->buf;
+	uLength = cbHeaderLength + uPadding;
 
-		ptdCurr = (PSTxDesc)pHeadTD;
+	/* Copy the Packet into a tx Buffer */
+	memcpy((pbyBuffer + uLength), skb->data, skb->len);
 
-		ptdCurr->pTDInfo->dwReqCount = cbReqCount - uPadding;
-		ptdCurr->pTDInfo->dwHeaderLength = cbHeaderLength;
-		ptdCurr->pTDInfo->skb_dma = ptdCurr->pTDInfo->buf_dma;
-		ptdCurr->buff_addr = cpu_to_le32(ptdCurr->pTDInfo->skb_dma);
-		//Set TSR1 & ReqCount in TxDescHead
-		ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP | EDMSDU);
-		ptdCurr->m_td1TD1.wReqCount = cpu_to_le16((unsigned short)(cbReqCount));
+	ptdCurr = (PSTxDesc)pHeadTD;
 
-		pDevice->iTDUsed[uDMAIdx]++;
-
-	}
-	*puMACfragNum = uMACfragNum;
+	ptdCurr->pTDInfo->dwReqCount = cbReqCount - uPadding;
+	ptdCurr->pTDInfo->dwHeaderLength = cbHeaderLength;
+	ptdCurr->pTDInfo->skb_dma = ptdCurr->pTDInfo->buf_dma;
+	ptdCurr->buff_addr = cpu_to_le32(ptdCurr->pTDInfo->skb_dma);
+	/* Set TSR1 & ReqCount in TxDescHead */
+	ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP | EDMSDU);
+	ptdCurr->m_td1TD1.wReqCount = cpu_to_le16((unsigned short)(cbReqCount));
 
 	return cbHeaderLength;
 }
 
-void
-vGenerateFIFOHeader(struct vnt_private *pDevice, unsigned char byPktType,
-		    unsigned char *pbyTxBufferAddr, bool bNeedEncrypt,
-		    unsigned int cbPayloadSize, unsigned int uDMAIdx,
-		    PSTxDesc pHeadTD, PSEthernetHeader psEthHeader, unsigned char *pPacket,
-		    PSKeyItem pTransmitKey, unsigned int uNodeIndex, unsigned int *puMACfragNum,
-		    unsigned int *pcbHeaderSize)
+static void vnt_fill_txkey(struct ieee80211_hdr *hdr, u8 *key_buffer,
+			   struct ieee80211_key_conf *tx_key,
+			   struct sk_buff *skb,	u16 payload_len,
+			   struct vnt_mic_hdr *mic_hdr)
 {
-	unsigned int wTxBufSize;       // FFinfo size
-	bool bNeedACK;
-	bool bIsAdhoc;
-	unsigned short cbMacHdLen;
-	PSTxBufHead     pTxBufHead = (PSTxBufHead) pbyTxBufferAddr;
-
-	wTxBufSize = sizeof(STxBufHead);
-
-	memset(pTxBufHead, 0, wTxBufSize);
-	//Set FIFOCTL_NEEDACK
-
-	if ((pDevice->op_mode == NL80211_IFTYPE_ADHOC) ||
-	    (pDevice->op_mode == NL80211_IFTYPE_AP)) {
-		if (is_multicast_ether_addr(&(psEthHeader->abyDstAddr[0]))) {
-			bNeedACK = false;
-			pTxBufHead->wFIFOCtl = pTxBufHead->wFIFOCtl & (~FIFOCTL_NEEDACK);
-		} else {
-			bNeedACK = true;
-			pTxBufHead->wFIFOCtl |= FIFOCTL_NEEDACK;
+	struct ieee80211_key_seq seq;
+	u8 *iv = ((u8 *)hdr + ieee80211_get_hdrlen_from_skb(skb));
+
+	/* strip header and icv len from payload */
+	payload_len -= ieee80211_get_hdrlen_from_skb(skb);
+	payload_len -= tx_key->icv_len;
+
+	switch (tx_key->cipher) {
+	case WLAN_CIPHER_SUITE_WEP40:
+	case WLAN_CIPHER_SUITE_WEP104:
+		memcpy(key_buffer, iv, 3);
+		memcpy(key_buffer + 3, tx_key->key, tx_key->keylen);
+
+		if (tx_key->keylen == WLAN_KEY_LEN_WEP40) {
+			memcpy(key_buffer + 8, iv, 3);
+			memcpy(key_buffer + 11,
+			       tx_key->key, WLAN_KEY_LEN_WEP40);
 		}
-		bIsAdhoc = true;
-	} else {
-		// MSDUs in Infra mode always need ACK
-		bNeedACK = true;
-		pTxBufHead->wFIFOCtl |= FIFOCTL_NEEDACK;
-		bIsAdhoc = false;
-	}
-
-	pTxBufHead->wFIFOCtl |= FIFOCTL_TMOEN;
-	pTxBufHead->wTimeStamp = cpu_to_le16(DEFAULT_MSDU_LIFETIME_RES_64us);
-
-	//Set FIFOCTL_LHEAD
-	if (pDevice->bLongHeader)
-		pTxBufHead->wFIFOCtl |= FIFOCTL_LHEAD;
 
-	//Set FIFOCTL_GENINT
-
-	pTxBufHead->wFIFOCtl |= FIFOCTL_GENINT;
-
-	//Set FIFOCTL_ISDMA0
-	if (TYPE_TXDMA0 == uDMAIdx)
-		pTxBufHead->wFIFOCtl |= FIFOCTL_ISDMA0;
-
-	//Set FRAGCTL_MACHDCNT
-	if (pDevice->bLongHeader)
-		cbMacHdLen = WLAN_HDR_ADDR3_LEN + 6;
-	else
-		cbMacHdLen = WLAN_HDR_ADDR3_LEN;
-
-	pTxBufHead->wFragCtl |= cpu_to_le16((unsigned short)(cbMacHdLen << 10));
-
-	//Set packet type
-	if (byPktType == PK_TYPE_11A) //0000 0000 0000 0000
-		;
-	else if (byPktType == PK_TYPE_11B) //0000 0001 0000 0000
-		pTxBufHead->wFIFOCtl |= FIFOCTL_11B;
-	else if (byPktType == PK_TYPE_11GB) //0000 0010 0000 0000
-		pTxBufHead->wFIFOCtl |= FIFOCTL_11GB;
-	else if (byPktType == PK_TYPE_11GA) //0000 0011 0000 0000
-		pTxBufHead->wFIFOCtl |= FIFOCTL_11GA;
-
-	//Set FIFOCTL_GrpAckPolicy
-	if (pDevice->bGrpAckPolicy == true) //0000 0100 0000 0000
-		pTxBufHead->wFIFOCtl |=	FIFOCTL_GRPACK;
-
-	//Set Auto Fallback Ctl
-	if (pDevice->wCurrentRate >= RATE_18M) {
-		if (pDevice->byAutoFBCtrl == AUTO_FB_0)
-			pTxBufHead->wFIFOCtl |= FIFOCTL_AUTO_FB_0;
-		else if (pDevice->byAutoFBCtrl == AUTO_FB_1)
-			pTxBufHead->wFIFOCtl |= FIFOCTL_AUTO_FB_1;
-	}
-
-	//Set FRAGCTL_WEPTYP
-	pDevice->bAES = false;
-
-	//Set FRAGCTL_WEPTYP
-	if (pDevice->byLocalID > REV_ID_VT3253_A1) {
-		if ((bNeedEncrypt) && (pTransmitKey != NULL))  { //WEP enabled
-			if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
-				pTxBufHead->wFragCtl |= FRAGCTL_TKIP;
-			} else if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) { //WEP40 or WEP104
-				if (pTransmitKey->uKeyLength != WLAN_WEP232_KEYLEN)
-					pTxBufHead->wFragCtl |= FRAGCTL_LEGACY;
-			} else if (pTransmitKey->byCipherSuite == KEY_CTL_CCMP) { //CCMP
-				pTxBufHead->wFragCtl |= FRAGCTL_AES;
-			}
-		}
-	}
-
-	RFbSetPower(pDevice, pDevice->wCurrentRate, pDevice->byCurrentCh);
-
-	pTxBufHead->byTxPower = pDevice->byCurPwr;
-
-	*pcbHeaderSize = s_cbFillTxBufHead(pDevice, byPktType, pbyTxBufferAddr, cbPayloadSize,
-					   uDMAIdx, pHeadTD, psEthHeader, pPacket, bNeedEncrypt,
-					   pTransmitKey, uNodeIndex, puMACfragNum);
-}
+		break;
+	case WLAN_CIPHER_SUITE_TKIP:
+		ieee80211_get_tkip_p2k(tx_key, skb, key_buffer);
 
-/*+
- *
- * Description:
- *      Translate 802.3 to 802.11 header
- *
- * Parameters:
- *  In:
- *      pDevice         - Pointer to adapter
- *      dwTxBufferAddr  - Transmit Buffer
- *      pPacket         - Packet from upper layer
- *      cbPacketSize    - Transmit Data Length
- *  Out:
- *      pcbHeadSize         - Header size of MAC&Baseband control and 802.11 Header
- *      pcbAppendPayload    - size of append payload for 802.1H translation
- *
- * Return Value: none
- *
- -*/
+		break;
+	case WLAN_CIPHER_SUITE_CCMP:
 
-void
-vGenerateMACHeader(
-	struct vnt_private *pDevice,
-	unsigned char *pbyBufferAddr,
-	__le16 wDuration,
-	PSEthernetHeader psEthHeader,
-	bool bNeedEncrypt,
-	unsigned short wFragType,
-	unsigned int uDMAIdx,
-	unsigned int uFragIdx
-)
-{
-	PS802_11Header  pMACHeader = (PS802_11Header)pbyBufferAddr;
+		if (!mic_hdr)
+			return;
 
-	memset(pMACHeader, 0, (sizeof(S802_11Header)));
+		mic_hdr->id = 0x59;
+		mic_hdr->payload_len = cpu_to_be16(payload_len);
+		ether_addr_copy(mic_hdr->mic_addr2, hdr->addr2);
 
-	if (uDMAIdx == TYPE_ATIMDMA)
-		pMACHeader->wFrameCtl = TYPE_802_11_ATIM;
-	else
-		pMACHeader->wFrameCtl = TYPE_802_11_DATA;
+		ieee80211_get_key_tx_seq(tx_key, &seq);
 
-	if (pDevice->op_mode == NL80211_IFTYPE_AP) {
-		memcpy(&(pMACHeader->abyAddr1[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
-		memcpy(&(pMACHeader->abyAddr2[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-		memcpy(&(pMACHeader->abyAddr3[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-		pMACHeader->wFrameCtl |= FC_FROMDS;
-	} else {
-		if (pDevice->op_mode == NL80211_IFTYPE_ADHOC) {
-			memcpy(&(pMACHeader->abyAddr1[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
-			memcpy(&(pMACHeader->abyAddr2[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-			memcpy(&(pMACHeader->abyAddr3[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-		} else {
-			memcpy(&(pMACHeader->abyAddr3[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
-			memcpy(&(pMACHeader->abyAddr2[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-			memcpy(&(pMACHeader->abyAddr1[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-			pMACHeader->wFrameCtl |= FC_TODS;
-		}
-	}
+		memcpy(mic_hdr->ccmp_pn, seq.ccmp.pn, IEEE80211_CCMP_PN_LEN);
 
-	if (bNeedEncrypt)
-		pMACHeader->wFrameCtl |= cpu_to_le16((unsigned short)WLAN_SET_FC_ISWEP(1));
+		if (ieee80211_has_a4(hdr->frame_control))
+			mic_hdr->hlen = cpu_to_be16(28);
+		else
+			mic_hdr->hlen = cpu_to_be16(22);
 
-	pMACHeader->wDurationID = le16_to_cpu(wDuration);
+		ether_addr_copy(mic_hdr->addr1, hdr->addr1);
+		ether_addr_copy(mic_hdr->addr2, hdr->addr2);
+		ether_addr_copy(mic_hdr->addr3, hdr->addr3);
 
-	if (pDevice->bLongHeader) {
-		PWLAN_80211HDR_A4 pMACA4Header  = (PWLAN_80211HDR_A4) pbyBufferAddr;
+		mic_hdr->frame_control = cpu_to_le16(
+			le16_to_cpu(hdr->frame_control) & 0xc78f);
+		mic_hdr->seq_ctrl = cpu_to_le16(
+				le16_to_cpu(hdr->seq_ctrl) & 0xf);
 
-		pMACHeader->wFrameCtl |= (FC_TODS | FC_FROMDS);
-		memcpy(pMACA4Header->abyAddr4, pDevice->abyBSSID, WLAN_ADDR_LEN);
-	}
-	pMACHeader->wSeqCtl = cpu_to_le16(pDevice->wSeqCounter << 4);
+		if (ieee80211_has_a4(hdr->frame_control))
+			ether_addr_copy(mic_hdr->addr4, hdr->addr4);
 
-	//Set FragNumber in Sequence Control
-	pMACHeader->wSeqCtl |= cpu_to_le16((unsigned short)uFragIdx);
+		memcpy(key_buffer, tx_key->key, WLAN_KEY_LEN_CCMP);
 
-	if ((wFragType == FRAGCTL_ENDFRAG) || (wFragType == FRAGCTL_NONFRAG)) {
-		pDevice->wSeqCounter++;
-		if (pDevice->wSeqCounter > 0x0fff)
-			pDevice->wSeqCounter = 0;
+		break;
+	default:
+		break;
 	}
-
-	if ((wFragType == FRAGCTL_STAFRAG) || (wFragType == FRAGCTL_MIDFRAG)) //StartFrag or MidFrag
-		pMACHeader->wFrameCtl |= FC_MOREFRAG;
 }
 
-CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice, PSTxMgmtPacket pPacket)
+int vnt_generate_fifo_header(struct vnt_private *priv, u32 dma_idx,
+			     PSTxDesc head_td, struct sk_buff *skb)
 {
-	PSTxDesc        pFrstTD;
-	unsigned char byPktType;
-	unsigned char *pbyTxBufferAddr;
-	void *pvRTS;
-	struct vnt_cts *pCTS;
-	void *pvTxDataHd;
-	unsigned int uDuration;
-	unsigned int cbReqCount;
-	PS802_11Header  pMACHeader;
-	unsigned int cbHeaderSize;
-	unsigned int cbFrameBodySize;
-	bool bNeedACK;
-	bool bIsPSPOLL = false;
-	PSTxBufHead     pTxBufHead;
-	unsigned int cbFrameSize;
-	unsigned int cbIVlen = 0;
-	unsigned int cbICVlen = 0;
-	unsigned int cbMIClen = 0;
-	unsigned int cbFCSlen = 4;
-	unsigned int uPadding = 0;
-	unsigned short wTxBufSize;
-	unsigned int cbMacHdLen;
-	SEthernetHeader sEthHeader;
-	void *pvRrvTime;
-	void *pMICHDR;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	unsigned short wCurrentRate = RATE_1M;
-
-	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 0)
-		return CMD_STATUS_RESOURCES;
-
-	pFrstTD = pDevice->apCurrTD[TYPE_TXDMA0];
-	pbyTxBufferAddr = (unsigned char *)pFrstTD->pTDInfo->buf;
-	cbFrameBodySize = pPacket->cbPayloadLen;
-	pTxBufHead = (PSTxBufHead) pbyTxBufferAddr;
-	wTxBufSize = sizeof(STxBufHead);
-	memset(pTxBufHead, 0, wTxBufSize);
-
-	if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
-		wCurrentRate = RATE_6M;
-		byPktType = PK_TYPE_11A;
-	} else {
-		wCurrentRate = RATE_1M;
-		byPktType = PK_TYPE_11B;
+	PDEVICE_TD_INFO td_info = head_td->pTDInfo;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_tx_rate *tx_rate = &info->control.rates[0];
+	struct ieee80211_rate *rate;
+	struct ieee80211_key_conf *tx_key;
+	struct ieee80211_hdr *hdr;
+	struct vnt_tx_fifo_head *tx_buffer_head =
+			(struct vnt_tx_fifo_head *)td_info->buf;
+	u16 tx_body_size = skb->len, current_rate;
+	u8 pkt_type;
+	bool is_pspoll = false;
+
+	memset(tx_buffer_head, 0, sizeof(*tx_buffer_head));
+
+	hdr = (struct ieee80211_hdr *)(skb->data);
+
+	rate = ieee80211_get_tx_rate(priv->hw, info);
+
+	current_rate = rate->hw_value;
+	if (priv->wCurrentRate != current_rate &&
+			!(priv->hw->conf.flags & IEEE80211_CONF_OFFCHANNEL)) {
+		priv->wCurrentRate = current_rate;
+
+		RFbSetPower(priv, priv->wCurrentRate,
+			    priv->hw->conf.chandef.chan->hw_value);
 	}
 
-	// SetPower will cause error power TX state for OFDM Date packet in TX buffer.
-	// 2004.11.11 Kyle -- Using OFDM power to tx MngPkt will decrease the connection capability.
-	//                    And cmd timer will wait data pkt TX finish before scanning so it's OK
-	//                    to set power here.
-	if (pDevice->pMgmt->eScanState != WMAC_NO_SCANNING)
-		RFbSetPower(pDevice, wCurrentRate, pDevice->byCurrentCh);
+	if (current_rate > RATE_11M)
+		pkt_type = (u8)priv->byPacketType;
 	else
-		RFbSetPower(pDevice, wCurrentRate, pMgmt->uCurrChannel);
-
-	pTxBufHead->byTxPower = pDevice->byCurPwr;
-	//+++++++++++++++++++++ Patch VT3253 A1 performance +++++++++++++++++++++++++++
-	if (pDevice->byFOETuning) {
-		if ((pPacket->p80211Header->sA3.wFrameCtl & TYPE_DATE_NULL) == TYPE_DATE_NULL) {
-			wCurrentRate = RATE_24M;
-			byPktType = PK_TYPE_11GA;
-		}
-	}
-
-	//Set packet type
-	if (byPktType == PK_TYPE_11A) {//0000 0000 0000 0000
-		pTxBufHead->wFIFOCtl = 0;
-	} else if (byPktType == PK_TYPE_11B) {//0000 0001 0000 0000
-		pTxBufHead->wFIFOCtl |= FIFOCTL_11B;
-	} else if (byPktType == PK_TYPE_11GB) {//0000 0010 0000 0000
-		pTxBufHead->wFIFOCtl |= FIFOCTL_11GB;
-	} else if (byPktType == PK_TYPE_11GA) {//0000 0011 0000 0000
-		pTxBufHead->wFIFOCtl |= FIFOCTL_11GA;
+		pkt_type = PK_TYPE_11B;
+
+	/*Set fifo controls */
+	if (pkt_type == PK_TYPE_11A)
+		tx_buffer_head->fifo_ctl = 0;
+	else if (pkt_type == PK_TYPE_11B)
+		tx_buffer_head->fifo_ctl = cpu_to_le16(FIFOCTL_11B);
+	else if (pkt_type == PK_TYPE_11GB)
+		tx_buffer_head->fifo_ctl = cpu_to_le16(FIFOCTL_11GB);
+	else if (pkt_type == PK_TYPE_11GA)
+		tx_buffer_head->fifo_ctl = cpu_to_le16(FIFOCTL_11GA);
+
+	/* generate interrupt */
+	tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_GENINT);
+
+	if (!ieee80211_is_data(hdr->frame_control)) {
+		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_TMOEN);
+		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_ISDMA0);
+		tx_buffer_head->time_stamp =
+			cpu_to_le16(DEFAULT_MGN_LIFETIME_RES_64us);
+	} else {
+		tx_buffer_head->time_stamp =
+			cpu_to_le16(DEFAULT_MSDU_LIFETIME_RES_64us);
 	}
 
-	pTxBufHead->wFIFOCtl |= FIFOCTL_TMOEN;
-	pTxBufHead->wTimeStamp = cpu_to_le16(DEFAULT_MGN_LIFETIME_RES_64us);
+	if (!(info->flags & IEEE80211_TX_CTL_NO_ACK))
+		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_NEEDACK);
 
-	if (is_multicast_ether_addr(&(pPacket->p80211Header->sA3.abyAddr1[0])))
-		bNeedACK = false;
-	else {
-		bNeedACK = true;
-		pTxBufHead->wFIFOCtl |= FIFOCTL_NEEDACK;
-	}
+	if (ieee80211_has_retry(hdr->frame_control))
+		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_LRETRY);
 
-	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) ||
-	    (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)) {
-		pTxBufHead->wFIFOCtl |= FIFOCTL_LRETRY;
-	}
+	if (tx_rate->flags & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
+		priv->byPreambleType = PREAMBLE_SHORT;
+	else
+		priv->byPreambleType = PREAMBLE_LONG;
 
-	pTxBufHead->wFIFOCtl |= (FIFOCTL_GENINT | FIFOCTL_ISDMA0);
+	if (tx_rate->flags & IEEE80211_TX_RC_USE_RTS_CTS)
+		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_RTS);
 
-	if ((pPacket->p80211Header->sA4.wFrameCtl & TYPE_SUBTYPE_MASK) == TYPE_CTL_PSPOLL) {
-		bIsPSPOLL = true;
-		cbMacHdLen = WLAN_HDR_ADDR2_LEN;
-	} else {
-		cbMacHdLen = WLAN_HDR_ADDR3_LEN;
+	if (ieee80211_has_a4(hdr->frame_control)) {
+		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_LHEAD);
+		priv->bLongHeader = true;
 	}
 
-	//Set FRAGCTL_MACHDCNT
-	pTxBufHead->wFragCtl |= cpu_to_le16((unsigned short)(cbMacHdLen << 10));
-
-	// Notes:
-	// Although spec says MMPDU can be fragmented; In most cases,
-	// no one will send a MMPDU under fragmentation. With RTS may occur.
-	pDevice->bAES = false;  //Set FRAGCTL_WEPTYP
-
-	if (WLAN_GET_FC_ISWEP(pPacket->p80211Header->sA4.wFrameCtl) != 0) {
-		if (pDevice->eEncryptionStatus == Ndis802_11Encryption1Enabled) {
-			cbIVlen = 4;
-			cbICVlen = 4;
-			pTxBufHead->wFragCtl |= FRAGCTL_LEGACY;
-		} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
-			cbIVlen = 8;//IV+ExtIV
-			cbMIClen = 8;
-			cbICVlen = 4;
-			pTxBufHead->wFragCtl |= FRAGCTL_TKIP;
-			//We need to get seed here for filling TxKey entry.
-		} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
-			cbIVlen = 8;//RSN Header
-			cbICVlen = 8;//MIC
-			pTxBufHead->wFragCtl |= FRAGCTL_AES;
-			pDevice->bAES = true;
+	if (info->flags & IEEE80211_TX_CTL_NO_PS_BUFFER)
+		is_pspoll = true;
+
+	tx_buffer_head->frag_ctl =
+			cpu_to_le16(ieee80211_get_hdrlen_from_skb(skb) << 10);
+
+	if (info->control.hw_key) {
+		tx_key = info->control.hw_key;
+
+		switch (info->control.hw_key->cipher) {
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
+			tx_buffer_head->frag_ctl |= cpu_to_le16(FRAGCTL_LEGACY);
+			break;
+		case WLAN_CIPHER_SUITE_TKIP:
+			tx_buffer_head->frag_ctl |= cpu_to_le16(FRAGCTL_TKIP);
+			break;
+		case WLAN_CIPHER_SUITE_CCMP:
+			tx_buffer_head->frag_ctl |= cpu_to_le16(FRAGCTL_AES);
+		default:
+			break;
 		}
-		//MAC Header should be padding 0 to DW alignment.
-		uPadding = 4 - (cbMacHdLen%4);
-		uPadding %= 4;
 	}
 
-	cbFrameSize = cbMacHdLen + cbFrameBodySize + cbIVlen + cbMIClen + cbICVlen + cbFCSlen;
-
-	//Set FIFOCTL_GrpAckPolicy
-	if (pDevice->bGrpAckPolicy == true) //0000 0100 0000 0000
-		pTxBufHead->wFIFOCtl |=	FIFOCTL_GRPACK;
+	tx_buffer_head->current_rate = cpu_to_le16(current_rate);
 
-	//the rest of pTxBufHead->wFragCtl:FragTyp will be set later in s_vFillFragParameter()
+	/* legacy rates TODO use ieee80211_tx_rate */
+	if (current_rate >= RATE_18M && ieee80211_is_data(hdr->frame_control)) {
+		if (priv->byAutoFBCtrl == AUTO_FB_0)
+			tx_buffer_head->fifo_ctl |=
+						cpu_to_le16(FIFOCTL_AUTO_FB_0);
+		else if (priv->byAutoFBCtrl == AUTO_FB_1)
+			tx_buffer_head->fifo_ctl |=
+						cpu_to_le16(FIFOCTL_AUTO_FB_1);
 
-	//Set RrvTime/RTS/CTS Buffer
-	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {//802.11g packet
-		pvRrvTime = (void *) (pbyTxBufferAddr + wTxBufSize);
-		pMICHDR = NULL;
-		pvRTS = NULL;
-		pCTS = (struct vnt_cts *)(pbyTxBufferAddr + wTxBufSize +
-					sizeof(struct vnt_rrv_time_cts));
-		pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize +
-				sizeof(struct vnt_rrv_time_cts) + sizeof(struct vnt_cts));
-		cbHeaderSize = wTxBufSize + sizeof(struct vnt_rrv_time_cts) +
-				sizeof(struct vnt_cts) + sizeof(struct vnt_tx_datahead_g);
-	} else { // 802.11a/b packet
-		pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
-		pMICHDR = NULL;
-		pvRTS = NULL;
-		pCTS = NULL;
-		pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize +
-			sizeof(struct vnt_rrv_time_ab));
-		cbHeaderSize = wTxBufSize + sizeof(struct vnt_rrv_time_ab) +
-			sizeof(struct vnt_tx_datahead_ab);
 	}
 
-	memset((void *)(pbyTxBufferAddr + wTxBufSize), 0, (cbHeaderSize - wTxBufSize));
-
-	memcpy(&(sEthHeader.abyDstAddr[0]), &(pPacket->p80211Header->sA3.abyAddr1[0]), ETH_ALEN);
-	memcpy(&(sEthHeader.abySrcAddr[0]), &(pPacket->p80211Header->sA3.abyAddr2[0]), ETH_ALEN);
-	//=========================
-	//    No Fragmentation
-	//=========================
-	pTxBufHead->wFragCtl |= (unsigned short)FRAGCTL_NONFRAG;
-
-	//Fill FIFO,RrvTime,RTS,and CTS
-	s_vGenerateTxParameter(pDevice, byPktType, pbyTxBufferAddr, pvRrvTime, pvRTS, pCTS,
-			       cbFrameSize, bNeedACK, TYPE_TXDMA0, &sEthHeader, wCurrentRate);
-
-	//Fill DataHead
-	uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbFrameSize, TYPE_TXDMA0, bNeedACK,
-				    0, 0, 1, AUTO_FB_NONE, wCurrentRate);
-
-	pMACHeader = (PS802_11Header) (pbyTxBufferAddr + cbHeaderSize);
-
-	cbReqCount = cbHeaderSize + cbMacHdLen + uPadding + cbIVlen + cbFrameBodySize;
-
-	if (WLAN_GET_FC_ISWEP(pPacket->p80211Header->sA4.wFrameCtl) != 0) {
-		unsigned char *pbyIVHead;
-		unsigned char *pbyPayloadHead;
-		unsigned char *pbyBSSID;
-		PSKeyItem       pTransmitKey = NULL;
-
-		pbyIVHead = (unsigned char *)(pbyTxBufferAddr + cbHeaderSize + cbMacHdLen + uPadding);
-		pbyPayloadHead = (unsigned char *)(pbyTxBufferAddr + cbHeaderSize + cbMacHdLen + uPadding + cbIVlen);
-
-		//Fill TXKEY
-		//Kyle: Need fix: TKIP and AES did't encrypt Mnt Packet.
-		//s_vFillTxKey(pDevice, (unsigned char *)pTxBufHead->adwTxKey, NULL);
-
-		//Fill IV(ExtIV,RSNHDR)
-		//s_vFillPrePayload(pDevice, pbyIVHead, NULL);
-		//---------------------------
-		// S/W or H/W Encryption
-		//---------------------------
-		do {
-			if ((pDevice->op_mode == NL80211_IFTYPE_STATION) &&
-			    (pDevice->bLinkPass == true)) {
-				pbyBSSID = pDevice->abyBSSID;
-				// get pairwise key
-				if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, PAIRWISE_KEY, &pTransmitKey) == false) {
-					// get group key
-					if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == true) {
-						pr_debug("Get GTK\n");
-						break;
-					}
-				} else {
-					pr_debug("Get PTK\n");
-					break;
-				}
-			}
-			// get group key
-			pbyBSSID = pDevice->abyBroadcastAddr;
-			if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == false) {
-				pTransmitKey = NULL;
-				pr_debug("KEY is NULL. OP Mode[%d]\n",
-					 pDevice->op_mode);
-			} else {
-				pr_debug("Get GTK\n");
-			}
-		} while (false);
-		//Fill TXKEY
-		s_vFillTxKey(pDevice, (unsigned char *)(pTxBufHead->adwTxKey), pbyIVHead, pTransmitKey,
-			     (unsigned char *)pMACHeader, (unsigned short)cbFrameBodySize, NULL);
-
-		memcpy(pMACHeader, pPacket->p80211Header, cbMacHdLen);
-		memcpy(pbyPayloadHead, ((unsigned char *)(pPacket->p80211Header) + cbMacHdLen),
-		       cbFrameBodySize);
-	} else {
-		// Copy the Packet into a tx Buffer
-		memcpy(pMACHeader, pPacket->p80211Header, pPacket->cbMPDULen);
-	}
+	tx_buffer_head->frag_ctl |= cpu_to_le16(FRAGCTL_NONFRAG);
 
-	pMACHeader->wSeqCtl = cpu_to_le16(pDevice->wSeqCounter << 4);
-	pDevice->wSeqCounter++;
-	if (pDevice->wSeqCounter > 0x0fff)
-		pDevice->wSeqCounter = 0;
-
-	if (bIsPSPOLL) {
-		// The MAC will automatically replace the Duration-field of MAC header by Duration-field
-		// of  FIFO control header.
-		// This will cause AID-field of PS-POLL packet to be incorrect (Because PS-POLL's AID field is
-		// in the same place of other packet's Duration-field).
-		// And it will cause Cisco-AP to issue Disassociation-packet
-		if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
-			((struct vnt_tx_datahead_g *)pvTxDataHd)->duration_a = cpu_to_le16(pPacket->p80211Header->sA2.wDurationID);
-			((struct vnt_tx_datahead_g *)pvTxDataHd)->duration_b = cpu_to_le16(pPacket->p80211Header->sA2.wDurationID);
-		} else {
-			((struct vnt_tx_datahead_ab *)pvTxDataHd)->duration = cpu_to_le16(pPacket->p80211Header->sA2.wDurationID);
-		}
-	}
+	s_cbFillTxBufHead(priv, pkt_type, (u8 *)tx_buffer_head,
+			  dma_idx, head_td, is_pspoll);
 
-	// first TD is the only TD
-	//Set TSR1 & ReqCount in TxDescHead
-	pFrstTD->m_td1TD1.byTCR = (TCR_STP | TCR_EDP | EDMSDU);
-	pFrstTD->pTDInfo->skb_dma = pFrstTD->pTDInfo->buf_dma;
-	pFrstTD->m_td1TD1.wReqCount = cpu_to_le16((unsigned short)(cbReqCount));
-	pFrstTD->buff_addr = cpu_to_le32(pFrstTD->pTDInfo->skb_dma);
-	pFrstTD->pTDInfo->byFlags = 0;
-
-	if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS)) {
-		// Disable PS
-		MACbPSWakeup(pDevice->PortOffset);
+	if (info->control.hw_key) {
+		tx_key = info->control.hw_key;
+		if (tx_key->keylen > 0)
+			vnt_fill_txkey(hdr, tx_buffer_head->tx_key,
+				tx_key, skb, tx_body_size, td_info->mic_hdr);
 	}
-	pDevice->bPWBitOn = false;
-
-	wmb();
-	pFrstTD->m_td0TD0.f1Owner = OWNED_BY_NIC;
-	wmb();
-
-	pDevice->iTDUsed[TYPE_TXDMA0]++;
-
-	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 1)
-		pr_debug(" available td0 <= 1\n");
-
-	pDevice->apCurrTD[TYPE_TXDMA0] = pFrstTD->next;
-
-	pDevice->nTxDataTimeCout = 0; //2008-8-21 chester <add> for send null packet
 
-	// Poll Transmit the adapter
-	MACvTransmit0(pDevice->PortOffset);
-
-	return CMD_STATUS_PENDING;
+	return 0;
 }
 
-CMD_STATUS csBeacon_xmit(struct vnt_private *pDevice, PSTxMgmtPacket pPacket)
+static int vnt_beacon_xmit(struct vnt_private *priv,
+			   struct sk_buff *skb)
 {
-	unsigned char byPktType;
-	unsigned char *pbyBuffer = (unsigned char *)pDevice->tx_beacon_bufs;
-	unsigned int cbFrameSize = pPacket->cbMPDULen + WLAN_FCS_LEN;
-	unsigned int cbHeaderSize = 0;
 	struct vnt_tx_short_buf_head *short_head =
-				(struct vnt_tx_short_buf_head *)pbyBuffer;
-	PS802_11Header   pMACHeader;
-	unsigned short wCurrentRate;
-
-	memset(short_head, 0, sizeof(*short_head));
+		(struct vnt_tx_short_buf_head *)priv->tx_beacon_bufs;
+	struct ieee80211_mgmt *mgmt_hdr = (struct ieee80211_mgmt *)
+				(priv->tx_beacon_bufs + sizeof(*short_head));
+	struct ieee80211_tx_info *info;
+	u32 frame_size = skb->len + 4;
+	u16 current_rate;
 
-	if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
-		wCurrentRate = RATE_6M;
-		byPktType = PK_TYPE_11A;
-	} else {
-		wCurrentRate = RATE_2M;
-		byPktType = PK_TYPE_11B;
-	}
+	memset(priv->tx_beacon_bufs, 0, sizeof(*short_head));
 
-	//Set Preamble type always long
-	pDevice->byPreambleType = PREAMBLE_LONG;
+	if (priv->byBBType == BB_TYPE_11A) {
+		current_rate = RATE_6M;
 
-	/* Set FIFOCTL_GENINT */
-	short_head->fifo_ctl |= cpu_to_le16(FIFOCTL_GENINT);
-
-	/* Set packet type & Get Duration */
-	if (byPktType == PK_TYPE_11A) {//0000 0000 0000 0000
-		short_head->duration =
-			cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A,
-				    cbFrameSize, byPktType, wCurrentRate, false,
-				    0, 0, 1, AUTO_FB_NONE));
-	} else if (byPktType == PK_TYPE_11B) {//0000 0001 0000 0000
-		short_head->fifo_ctl |= cpu_to_le16(FIFOCTL_11B);
+		/* Get SignalField,ServiceField,Length */
+		vnt_get_phy_field(priv, frame_size, current_rate,
+				  PK_TYPE_11A, &short_head->ab);
 
+		/* Get Duration and TimeStampOff */
 		short_head->duration =
-			cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_B,
-				    cbFrameSize, byPktType, wCurrentRate, false,
-				    0, 0, 1, AUTO_FB_NONE));
-	}
-
-	vnt_get_phy_field(pDevice, cbFrameSize,
-			  wCurrentRate, byPktType, &short_head->ab);
-
-	/* Get TimeStampOff */
-	short_head->time_stamp_off = vnt_time_stamp_off(pDevice, wCurrentRate);
-	cbHeaderSize = sizeof(struct vnt_tx_short_buf_head);
-
-	//Generate Beacon Header
-	pMACHeader = (PS802_11Header)(pbyBuffer + cbHeaderSize);
-	memcpy(pMACHeader, pPacket->p80211Header, pPacket->cbMPDULen);
-
-	pMACHeader->wDurationID = 0;
-	pMACHeader->wSeqCtl = cpu_to_le16(pDevice->wSeqCounter << 4);
-	pDevice->wSeqCounter++;
-	if (pDevice->wSeqCounter > 0x0fff)
-		pDevice->wSeqCounter = 0;
-
-	// Set Beacon buffer length
-	pDevice->wBCNBufLen = pPacket->cbMPDULen + cbHeaderSize;
-
-	MACvSetCurrBCNTxDescAddr(pDevice->PortOffset, (pDevice->tx_beacon_dma));
-
-	MACvSetCurrBCNLength(pDevice->PortOffset, pDevice->wBCNBufLen);
-	// Set auto Transmit on
-	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
-	// Poll Transmit the adapter
-	MACvTransmitBCN(pDevice->PortOffset);
-
-	return CMD_STATUS_PENDING;
-}
-
-unsigned int
-cbGetFragCount(
-	struct vnt_private *pDevice,
-	PSKeyItem        pTransmitKey,
-	unsigned int cbFrameBodySize,
-	PSEthernetHeader psEthHeader
-)
-{
-	unsigned int cbMACHdLen;
-	unsigned int cbFrameSize;
-	unsigned int cbFragmentSize; //Hdr+(IV)+payoad+(MIC)+(ICV)+FCS
-	unsigned int cbFragPayloadSize;
-	unsigned int cbLastFragPayloadSize;
-	unsigned int cbIVlen = 0;
-	unsigned int cbICVlen = 0;
-	unsigned int cbMIClen = 0;
-	unsigned int cbFCSlen = 4;
-	unsigned int uMACfragNum = 1;
-	bool bNeedACK;
-
-	if ((pDevice->op_mode == NL80211_IFTYPE_ADHOC) ||
-	    (pDevice->op_mode == NL80211_IFTYPE_AP)) {
-		if (is_multicast_ether_addr(&(psEthHeader->abyDstAddr[0])))
-			bNeedACK = false;
-		else
-			bNeedACK = true;
-	} else {
-		// MSDUs in Infra mode always need ACK
-		bNeedACK = true;
-	}
-
-	if (pDevice->bLongHeader)
-		cbMACHdLen = WLAN_HDR_ADDR3_LEN + 6;
-	else
-		cbMACHdLen = WLAN_HDR_ADDR3_LEN;
-
-	if (pDevice->bEncryptionEnable == true) {
-		if (pTransmitKey == NULL) {
-			if ((pDevice->eEncryptionStatus == Ndis802_11Encryption1Enabled) ||
-			    (pDevice->pMgmt->eAuthenMode < WMAC_AUTH_WPA)) {
-				cbIVlen = 4;
-				cbICVlen = 4;
-			} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
-				cbIVlen = 8;//IV+ExtIV
-				cbMIClen = 8;
-				cbICVlen = 4;
-			} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
-				cbIVlen = 8;//RSN Header
-				cbICVlen = 8;//MIC
-			}
-		} else if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) {
-			cbIVlen = 4;
-			cbICVlen = 4;
-		} else if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
-			cbIVlen = 8;//IV+ExtIV
-			cbMIClen = 8;
-			cbICVlen = 4;
-		} else if (pTransmitKey->byCipherSuite == KEY_CTL_CCMP) {
-			cbIVlen = 8;//RSN Header
-			cbICVlen = 8;//MIC
-		}
-	}
-
-	cbFrameSize = cbMACHdLen + cbIVlen + (cbFrameBodySize + cbMIClen) + cbICVlen + cbFCSlen;
-
-	if ((cbFrameSize > pDevice->wFragmentationThreshold) && (bNeedACK == true)) {
-		// Fragmentation
-		cbFragmentSize = pDevice->wFragmentationThreshold;
-		cbFragPayloadSize = cbFragmentSize - cbMACHdLen - cbIVlen - cbICVlen - cbFCSlen;
-		uMACfragNum = (unsigned short) ((cbFrameBodySize + cbMIClen) / cbFragPayloadSize);
-		cbLastFragPayloadSize = (cbFrameBodySize + cbMIClen) % cbFragPayloadSize;
-		if (cbLastFragPayloadSize == 0)
-			cbLastFragPayloadSize = cbFragPayloadSize;
-		else
-			uMACfragNum++;
-	}
-	return uMACfragNum;
-}
-
-void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb,
-		    unsigned char *pbMPDU, unsigned int cbMPDULen)
-{
-	PSTxDesc        pFrstTD;
-	unsigned char byPktType;
-	unsigned char *pbyTxBufferAddr;
-	void *pvRTS;
-	void *pvCTS;
-	void *pvTxDataHd;
-	unsigned int uDuration;
-	unsigned int cbReqCount;
-	PS802_11Header  pMACHeader;
-	unsigned int cbHeaderSize;
-	unsigned int cbFrameBodySize;
-	bool bNeedACK;
-	bool bIsPSPOLL = false;
-	PSTxBufHead     pTxBufHead;
-	unsigned int cbFrameSize;
-	unsigned int cbIVlen = 0;
-	unsigned int cbICVlen = 0;
-	unsigned int cbMIClen = 0;
-	unsigned int cbFCSlen = 4;
-	unsigned int uPadding = 0;
-	unsigned int cbMICHDR = 0;
-	unsigned int uLength = 0;
-	u32 dwMICKey0, dwMICKey1;
-	u32 dwMIC_Priority;
-	u32 *pdwMIC_L;
-	u32 *pdwMIC_R;
-	unsigned short wTxBufSize;
-	unsigned int cbMacHdLen;
-	SEthernetHeader sEthHeader;
-	void *pvRrvTime;
-	void *pMICHDR;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	unsigned short wCurrentRate = RATE_1M;
-	PUWLAN_80211HDR  p80211Header;
-	unsigned int uNodeIndex = 0;
-	bool bNodeExist = false;
-	SKeyItem        STempKey;
-	PSKeyItem       pTransmitKey = NULL;
-	unsigned char *pbyIVHead;
-	unsigned char *pbyPayloadHead;
-	unsigned char *pbyMacHdr;
-
-	unsigned int cbExtSuppRate = 0;
-
-	pvRrvTime = pMICHDR = pvRTS = pvCTS = pvTxDataHd = NULL;
-
-	if (cbMPDULen <= WLAN_HDR_ADDR3_LEN)
-		cbFrameBodySize = 0;
-	else
-		cbFrameBodySize = cbMPDULen - WLAN_HDR_ADDR3_LEN;
-
-	p80211Header = (PUWLAN_80211HDR)pbMPDU;
+			cpu_to_le16((u16)s_uGetDataDuration(priv, DATADUR_B,
+				    frame_size, PK_TYPE_11A, current_rate,
+				    false, 0, 0, 1, AUTO_FB_NONE));
 
-	pFrstTD = pDevice->apCurrTD[TYPE_TXDMA0];
-	pbyTxBufferAddr = (unsigned char *)pFrstTD->pTDInfo->buf;
-	pTxBufHead = (PSTxBufHead) pbyTxBufferAddr;
-	wTxBufSize = sizeof(STxBufHead);
-	memset(pTxBufHead, 0, wTxBufSize);
-
-	if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
-		wCurrentRate = RATE_6M;
-		byPktType = PK_TYPE_11A;
+		short_head->time_stamp_off =
+				vnt_time_stamp_off(priv, current_rate);
 	} else {
-		wCurrentRate = RATE_1M;
-		byPktType = PK_TYPE_11B;
-	}
-
-	// SetPower will cause error power TX state for OFDM Date packet in TX buffer.
-	// 2004.11.11 Kyle -- Using OFDM power to tx MngPkt will decrease the connection capability.
-	//                    And cmd timer will wait data pkt TX to finish before scanning so it's OK
-	//                    to set power here.
-	if (pDevice->pMgmt->eScanState != WMAC_NO_SCANNING)
-		RFbSetPower(pDevice, wCurrentRate, pDevice->byCurrentCh);
-	else
-		RFbSetPower(pDevice, wCurrentRate, pMgmt->uCurrChannel);
-
-	pTxBufHead->byTxPower = pDevice->byCurPwr;
-
-	//+++++++++++++++++++++ Patch VT3253 A1 performance +++++++++++++++++++++++++++
-	if (pDevice->byFOETuning) {
-		if ((p80211Header->sA3.wFrameCtl & TYPE_DATE_NULL) == TYPE_DATE_NULL) {
-			wCurrentRate = RATE_24M;
-			byPktType = PK_TYPE_11GA;
-		}
-	}
-
-	pr_debug("vDMA0_tx_80211: p80211Header->sA3.wFrameCtl = %x\n",
-		 p80211Header->sA3.wFrameCtl);
-
-	//Set packet type
-	if (byPktType == PK_TYPE_11A) {//0000 0000 0000 0000
-		pTxBufHead->wFIFOCtl = 0;
-	} else if (byPktType == PK_TYPE_11B) {//0000 0001 0000 0000
-		pTxBufHead->wFIFOCtl |= FIFOCTL_11B;
-	} else if (byPktType == PK_TYPE_11GB) {//0000 0010 0000 0000
-		pTxBufHead->wFIFOCtl |= FIFOCTL_11GB;
-	} else if (byPktType == PK_TYPE_11GA) {//0000 0011 0000 0000
-		pTxBufHead->wFIFOCtl |= FIFOCTL_11GA;
-	}
-
-	pTxBufHead->wFIFOCtl |= FIFOCTL_TMOEN;
-	pTxBufHead->wTimeStamp = cpu_to_le16(DEFAULT_MGN_LIFETIME_RES_64us);
-
-	if (is_multicast_ether_addr(&(p80211Header->sA3.abyAddr1[0]))) {
-		bNeedACK = false;
-		if (pDevice->bEnableHostWEP) {
-			uNodeIndex = 0;
-			bNodeExist = true;
-		}
-	} else {
-		if (pDevice->bEnableHostWEP) {
-			if (BSSDBbIsSTAInNodeDB(pDevice->pMgmt, (unsigned char *)(p80211Header->sA3.abyAddr1), &uNodeIndex))
-				bNodeExist = true;
-		}
-		bNeedACK = true;
-		pTxBufHead->wFIFOCtl |= FIFOCTL_NEEDACK;
-	}
-
-	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) ||
-	    (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)) {
-		pTxBufHead->wFIFOCtl |= FIFOCTL_LRETRY;
-	}
-
-	pTxBufHead->wFIFOCtl |= (FIFOCTL_GENINT | FIFOCTL_ISDMA0);
-
-	if ((p80211Header->sA4.wFrameCtl & TYPE_SUBTYPE_MASK) == TYPE_CTL_PSPOLL) {
-		bIsPSPOLL = true;
-		cbMacHdLen = WLAN_HDR_ADDR2_LEN;
-	} else {
-		cbMacHdLen = WLAN_HDR_ADDR3_LEN;
-	}
-
-	// hostapd deamon ext support rate patch
-	if (WLAN_GET_FC_FSTYPE(p80211Header->sA4.wFrameCtl) == WLAN_FSTYPE_ASSOCRESP) {
-		if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len != 0)
-			cbExtSuppRate += ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len + WLAN_IEHDR_LEN;
+		current_rate = RATE_1M;
+		short_head->fifo_ctl |= cpu_to_le16(FIFOCTL_11B);
 
-		if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len != 0)
-			cbExtSuppRate += ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len + WLAN_IEHDR_LEN;
+		/* Get SignalField,ServiceField,Length */
+		vnt_get_phy_field(priv, frame_size, current_rate,
+				  PK_TYPE_11B, &short_head->ab);
 
-		if (cbExtSuppRate > 0)
-			cbFrameBodySize = WLAN_ASSOCRESP_OFF_SUPP_RATES;
-	}
+		/* Get Duration and TimeStampOff */
+		short_head->duration =
+			cpu_to_le16((u16)s_uGetDataDuration(priv, DATADUR_B,
+				    frame_size, PK_TYPE_11B, current_rate,
+				    false, 0, 0, 1, AUTO_FB_NONE));
 
-	//Set FRAGCTL_MACHDCNT
-	pTxBufHead->wFragCtl |= cpu_to_le16((unsigned short)cbMacHdLen << 10);
-
-	// Notes:
-	// Although spec says MMPDU can be fragmented; In most cases,
-	// no one will send a MMPDU under fragmentation. With RTS may occur.
-	pDevice->bAES = false;  //Set FRAGCTL_WEPTYP
-
-	if (WLAN_GET_FC_ISWEP(p80211Header->sA4.wFrameCtl) != 0) {
-		if (pDevice->eEncryptionStatus == Ndis802_11Encryption1Enabled) {
-			cbIVlen = 4;
-			cbICVlen = 4;
-			pTxBufHead->wFragCtl |= FRAGCTL_LEGACY;
-		} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
-			cbIVlen = 8;//IV+ExtIV
-			cbMIClen = 8;
-			cbICVlen = 4;
-			pTxBufHead->wFragCtl |= FRAGCTL_TKIP;
-			//We need to get seed here for filling TxKey entry.
-		} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
-			cbIVlen = 8;//RSN Header
-			cbICVlen = 8;//MIC
-			cbMICHDR = sizeof(struct vnt_mic_hdr);
-			pTxBufHead->wFragCtl |= FRAGCTL_AES;
-			pDevice->bAES = true;
-		}
-		//MAC Header should be padding 0 to DW alignment.
-		uPadding = 4 - (cbMacHdLen%4);
-		uPadding %= 4;
+		short_head->time_stamp_off =
+			vnt_time_stamp_off(priv, current_rate);
 	}
 
-	cbFrameSize = cbMacHdLen + cbFrameBodySize + cbIVlen + cbMIClen + cbICVlen + cbFCSlen + cbExtSuppRate;
-
-	//Set FIFOCTL_GrpAckPolicy
-	if (pDevice->bGrpAckPolicy == true) //0000 0100 0000 0000
-		pTxBufHead->wFIFOCtl |=	FIFOCTL_GRPACK;
-
-	//the rest of pTxBufHead->wFragCtl:FragTyp will be set later in s_vFillFragParameter()
+	short_head->fifo_ctl |= cpu_to_le16(FIFOCTL_GENINT);
 
-	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {//802.11g packet
+	/* Copy Beacon */
+	memcpy(mgmt_hdr, skb->data, skb->len);
 
-		pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
-		pMICHDR = (struct vnt_mic_hdr *)(pbyTxBufferAddr + wTxBufSize +
-					sizeof(struct vnt_rrv_time_cts));
-		pvRTS = NULL;
-		pvCTS = (struct vnt_cts *)(pbyTxBufferAddr + wTxBufSize +
-				sizeof(struct vnt_rrv_time_cts) + cbMICHDR);
-		pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize +
-			sizeof(struct vnt_rrv_time_cts) + cbMICHDR + sizeof(struct vnt_cts));
-		cbHeaderSize = wTxBufSize + sizeof(struct vnt_rrv_time_cts) +
-				cbMICHDR + sizeof(struct vnt_cts) + sizeof(struct vnt_tx_datahead_g);
+	/* time stamp always 0 */
+	mgmt_hdr->u.beacon.timestamp = 0;
 
-	} else {//802.11a/b packet
-
-		pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
-		pMICHDR = (struct vnt_mic_hdr *) (pbyTxBufferAddr +
-				wTxBufSize + sizeof(struct vnt_rrv_time_ab));
-		pvRTS = NULL;
-		pvCTS = NULL;
-		pvTxDataHd = (void *)(pbyTxBufferAddr +
-			wTxBufSize + sizeof(struct vnt_rrv_time_ab) + cbMICHDR);
-		cbHeaderSize = wTxBufSize + sizeof(struct vnt_rrv_time_ab) +
-				cbMICHDR + sizeof(struct vnt_tx_datahead_ab);
+	info = IEEE80211_SKB_CB(skb);
+	if (info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ) {
+		struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)mgmt_hdr;
 
+		hdr->duration_id = 0;
+		hdr->seq_ctrl = cpu_to_le16(priv->wSeqCounter << 4);
 	}
 
-	memset((void *)(pbyTxBufferAddr + wTxBufSize), 0, (cbHeaderSize - wTxBufSize));
-	memcpy(&(sEthHeader.abyDstAddr[0]), &(p80211Header->sA3.abyAddr1[0]), ETH_ALEN);
-	memcpy(&(sEthHeader.abySrcAddr[0]), &(p80211Header->sA3.abyAddr2[0]), ETH_ALEN);
-	//=========================
-	//    No Fragmentation
-	//=========================
-	pTxBufHead->wFragCtl |= (unsigned short)FRAGCTL_NONFRAG;
-
-	//Fill FIFO,RrvTime,RTS,and CTS
-	s_vGenerateTxParameter(pDevice, byPktType, pbyTxBufferAddr, pvRrvTime, pvRTS, pvCTS,
-			       cbFrameSize, bNeedACK, TYPE_TXDMA0, &sEthHeader, wCurrentRate);
+	priv->wSeqCounter++;
+	if (priv->wSeqCounter > 0x0fff)
+		priv->wSeqCounter = 0;
 
-	//Fill DataHead
-	uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbFrameSize, TYPE_TXDMA0, bNeedACK,
-				    0, 0, 1, AUTO_FB_NONE, wCurrentRate);
+	priv->wBCNBufLen = sizeof(*short_head) + skb->len;
 
-	pMACHeader = (PS802_11Header) (pbyTxBufferAddr + cbHeaderSize);
+	MACvSetCurrBCNTxDescAddr(priv->PortOffset, priv->tx_beacon_dma);
 
-	cbReqCount = cbHeaderSize + cbMacHdLen + uPadding + cbIVlen + (cbFrameBodySize + cbMIClen) + cbExtSuppRate;
+	MACvSetCurrBCNLength(priv->PortOffset, priv->wBCNBufLen);
+	/* Set auto Transmit on */
+	MACvRegBitsOn(priv->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
+	/* Poll Transmit the adapter */
+	MACvTransmitBCN(priv->PortOffset);
 
-	pbyMacHdr = (unsigned char *)(pbyTxBufferAddr + cbHeaderSize);
-	pbyPayloadHead = (unsigned char *)(pbyMacHdr + cbMacHdLen + uPadding + cbIVlen);
-	pbyIVHead = (unsigned char *)(pbyMacHdr + cbMacHdLen + uPadding);
+	return 0;
+}
 
-	// Copy the Packet into a tx Buffer
-	memcpy(pbyMacHdr, pbMPDU, cbMacHdLen);
+int vnt_beacon_make(struct vnt_private *priv, struct ieee80211_vif *vif)
+{
+	struct sk_buff *beacon;
 
-	// version set to 0, patch for hostapd deamon
-	pMACHeader->wFrameCtl &= cpu_to_le16(0xfffc);
-	memcpy(pbyPayloadHead, (pbMPDU + cbMacHdLen), cbFrameBodySize);
+	beacon = ieee80211_beacon_get(priv->hw, vif);
+	if (!beacon)
+		return -ENOMEM;
 
-	// replace support rate, patch for hostapd deamon(only support 11M)
-	if (WLAN_GET_FC_FSTYPE(p80211Header->sA4.wFrameCtl) == WLAN_FSTYPE_ASSOCRESP) {
-		if (cbExtSuppRate != 0) {
-			if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len != 0)
-				memcpy((pbyPayloadHead + cbFrameBodySize),
-				       pMgmt->abyCurrSuppRates,
-				       ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len + WLAN_IEHDR_LEN
-);
-			if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len != 0)
-				memcpy((pbyPayloadHead + cbFrameBodySize) + ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len + WLAN_IEHDR_LEN,
-				       pMgmt->abyCurrExtSuppRates,
-				       ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len + WLAN_IEHDR_LEN
-);
-		}
+	if (vnt_beacon_xmit(priv, beacon)) {
+		ieee80211_free_txskb(priv->hw, beacon);
+		return -ENODEV;
 	}
 
-	// Set wep
-	if (WLAN_GET_FC_ISWEP(p80211Header->sA4.wFrameCtl) != 0) {
-		if (pDevice->bEnableHostWEP) {
-			pTransmitKey = &STempKey;
-			pTransmitKey->byCipherSuite = pMgmt->sNodeDBTable[uNodeIndex].byCipherSuite;
-			pTransmitKey->dwKeyIndex = pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex;
-			pTransmitKey->uKeyLength = pMgmt->sNodeDBTable[uNodeIndex].uWepKeyLength;
-			pTransmitKey->dwTSC47_16 = pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16;
-			pTransmitKey->wTSC15_0 = pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0;
-			memcpy(pTransmitKey->abyKey,
-			       &pMgmt->sNodeDBTable[uNodeIndex].abyWepKey[0],
-			       pTransmitKey->uKeyLength
-);
-		}
-
-		if ((pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
-			dwMICKey0 = *(u32 *)(&pTransmitKey->abyKey[16]);
-			dwMICKey1 = *(u32 *)(&pTransmitKey->abyKey[20]);
-
-			// DO Software Michael
-			MIC_vInit(dwMICKey0, dwMICKey1);
-			MIC_vAppend((unsigned char *)&(sEthHeader.abyDstAddr[0]), 12);
-			dwMIC_Priority = 0;
-			MIC_vAppend((unsigned char *)&dwMIC_Priority, 4);
-			pr_debug("DMA0_tx_8021:MIC KEY: %X, %X\n",
-				 dwMICKey0, dwMICKey1);
-
-			uLength = cbHeaderSize + cbMacHdLen + uPadding + cbIVlen;
-
-			MIC_vAppend((pbyTxBufferAddr + uLength), cbFrameBodySize);
-
-			pdwMIC_L = (u32 *)(pbyTxBufferAddr + uLength + cbFrameBodySize);
-			pdwMIC_R = (u32 *)(pbyTxBufferAddr + uLength + cbFrameBodySize + 4);
-
-			MIC_vGetMIC(pdwMIC_L, pdwMIC_R);
-			MIC_vUnInit();
-
-			if (pDevice->bTxMICFail == true) {
-				*pdwMIC_L = 0;
-				*pdwMIC_R = 0;
-				pDevice->bTxMICFail = false;
-			}
-
-			pr_debug("uLength: %d, %d\n", uLength, cbFrameBodySize);
-			pr_debug("cbReqCount:%d, %d, %d, %d\n",
-				 cbReqCount, cbHeaderSize, uPadding, cbIVlen);
-			pr_debug("MIC:%x, %x\n", *pdwMIC_L, *pdwMIC_R);
-
-		}
-
-		s_vFillTxKey(pDevice, (unsigned char *)(pTxBufHead->adwTxKey), pbyIVHead, pTransmitKey,
-			     pbyMacHdr, (unsigned short)cbFrameBodySize, (unsigned char *)pMICHDR);
-
-		if (pDevice->bEnableHostWEP) {
-			pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
-			pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
-		}
-
-		if ((pDevice->byLocalID <= REV_ID_VT3253_A1))
-			s_vSWencryption(pDevice, pTransmitKey, pbyPayloadHead, (unsigned short)(cbFrameBodySize + cbMIClen));
-	}
+	return 0;
+}
 
-	pMACHeader->wSeqCtl = cpu_to_le16(pDevice->wSeqCounter << 4);
-	pDevice->wSeqCounter++;
-	if (pDevice->wSeqCounter > 0x0fff)
-		pDevice->wSeqCounter = 0;
-
-	if (bIsPSPOLL) {
-		// The MAC will automatically replace the Duration-field of MAC header by Duration-field
-		// of  FIFO control header.
-		// This will cause AID-field of PS-POLL packet be incorrect (Because PS-POLL's AID field is
-		// in the same place of other packet's Duration-field).
-		// And it will cause Cisco-AP to issue Disassociation-packet
-		if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
-			((struct vnt_tx_datahead_g *)pvTxDataHd)->duration_a = cpu_to_le16(p80211Header->sA2.wDurationID);
-			((struct vnt_tx_datahead_g *)pvTxDataHd)->duration_b = cpu_to_le16(p80211Header->sA2.wDurationID);
-		} else {
-			((struct vnt_tx_datahead_ab *)pvTxDataHd)->duration = cpu_to_le16(p80211Header->sA2.wDurationID);
-		}
-	}
+int vnt_beacon_enable(struct vnt_private *priv, struct ieee80211_vif *vif,
+		      struct ieee80211_bss_conf *conf)
+{
+	int ret;
 
-	// first TD is the only TD
-	//Set TSR1 & ReqCount in TxDescHead
-	pFrstTD->pTDInfo->skb = skb;
-	pFrstTD->m_td1TD1.byTCR = (TCR_STP | TCR_EDP | EDMSDU);
-	pFrstTD->pTDInfo->skb_dma = pFrstTD->pTDInfo->buf_dma;
-	pFrstTD->m_td1TD1.wReqCount = cpu_to_le16(cbReqCount);
-	pFrstTD->buff_addr = cpu_to_le32(pFrstTD->pTDInfo->skb_dma);
-	pFrstTD->pTDInfo->byFlags = 0;
-	pFrstTD->pTDInfo->byFlags |= TD_FLAGS_PRIV_SKB;
-
-	if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS)) {
-		// Disable PS
-		MACbPSWakeup(pDevice->PortOffset);
-	}
-	pDevice->bPWBitOn = false;
+	VNSvOutPortB(priv->PortOffset + MAC_REG_TFTCTL, TFTCTL_TSFCNTRST);
 
-	wmb();
-	pFrstTD->m_td0TD0.f1Owner = OWNED_BY_NIC;
-	wmb();
+	VNSvOutPortB(priv->PortOffset + MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
 
-	pDevice->iTDUsed[TYPE_TXDMA0]++;
+	CARDvSetFirstNextTBTT(priv, conf->beacon_int);
 
-	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 1)
-		pr_debug(" available td0 <= 1\n");
+	CARDbSetBeaconPeriod(priv, conf->beacon_int);
 
-	pDevice->apCurrTD[TYPE_TXDMA0] = pFrstTD->next;
+	ret = vnt_beacon_make(priv, vif);
 
-	// Poll Transmit the adapter
-	MACvTransmit0(pDevice->PortOffset);
+	return ret;
 }
diff --git a/drivers/staging/vt6655/rxtx.h b/drivers/staging/vt6655/rxtx.h
index 8ee62887dee5..b9bd1639b13e 100644
--- a/drivers/staging/vt6655/rxtx.h
+++ b/drivers/staging/vt6655/rxtx.h
@@ -29,9 +29,11 @@
 #ifndef __RXTX_H__
 #define __RXTX_H__
 
-#include "ttype.h"
 #include "device.h"
-#include "wcmd.h"
+
+#define DEFAULT_MSDU_LIFETIME_RES_64us	8000 /* 64us */
+#define DEFAULT_MGN_LIFETIME_RES_64us	125  /* 64us */
+
 
 /*---------------------  Export Definitions -------------------------*/
 
@@ -173,6 +175,14 @@ struct vnt_cts_fb {
 	u16 reserved2;
 } __packed;
 
+struct vnt_tx_fifo_head {
+	u8 tx_key[WLAN_KEY_LEN_CCMP];
+	__le16 fifo_ctl;
+	__le16 time_stamp;
+	__le16 frag_ctl;
+	__le16 current_rate;
+} __packed;
+
 struct vnt_tx_short_buf_head {
 	__le16 fifo_ctl;
 	u16 time_stamp;
@@ -181,38 +191,10 @@ struct vnt_tx_short_buf_head {
 	__le16 time_stamp_off;
 } __packed;
 
-void
-vGenerateMACHeader(
-	struct vnt_private *,
-	unsigned char *pbyBufferAddr,
-	unsigned short wDuration,
-	PSEthernetHeader psEthHeader,
-	bool bNeedEncrypt,
-	unsigned short wFragType,
-	unsigned int uDMAIdx,
-	unsigned int uFragIdx
-);
-
-unsigned int
-cbGetFragCount(
-	struct vnt_private *,
-	PSKeyItem        pTransmitKey,
-	unsigned int	cbFrameBodySize,
-	PSEthernetHeader psEthHeader
-);
-
-void
-vGenerateFIFOHeader(struct vnt_private *, unsigned char byPktTyp,
-		    unsigned char *pbyTxBufferAddr, bool bNeedEncrypt,
-		    unsigned int cbPayloadSize, unsigned int uDMAIdx,
-		    PSTxDesc pHeadTD, PSEthernetHeader psEthHeader,
-		    unsigned char *pPacket, PSKeyItem pTransmitKey,
-		    unsigned int uNodeIndex, unsigned int *puMACfragNum,
-		    unsigned int *pcbHeaderSize);
-
-void vDMA0_tx_80211(struct vnt_private *, struct sk_buff *skb,
-		    unsigned char *pbMPDU, unsigned int cbMPDULen);
-CMD_STATUS csMgmt_xmit(struct vnt_private *, PSTxMgmtPacket pPacket);
-CMD_STATUS csBeacon_xmit(struct vnt_private *, PSTxMgmtPacket pPacket);
+int vnt_generate_fifo_header(struct vnt_private *, u32,
+			     PSTxDesc head_td, struct sk_buff *);
+int vnt_beacon_make(struct vnt_private *, struct ieee80211_vif *);
+int vnt_beacon_enable(struct vnt_private *, struct ieee80211_vif *,
+		      struct ieee80211_bss_conf *);
 
 #endif // __RXTX_H__
diff --git a/drivers/staging/vt6655/srom.c b/drivers/staging/vt6655/srom.c
index 5396e5832c22..9ec49e653b61 100644
--- a/drivers/staging/vt6655/srom.c
+++ b/drivers/staging/vt6655/srom.c
@@ -44,7 +44,6 @@
 
 #include "upc.h"
 #include "tmacro.h"
-#include "tether.h"
 #include "mac.h"
 #include "srom.h"
 
@@ -108,144 +107,6 @@ unsigned char SROMbyReadEmbedded(void __iomem *dwIoBase, unsigned char byContntO
 }
 
 /*
- * Description: Write a byte to EEPROM, by MAC I2C
- *
- * Parameters:
- *  In:
- *      dwIoBase        - I/O base address
- *      byContntOffset  - address of EEPROM
- *      wData           - data to write
- *  Out:
- *      none
- *
- * Return Value: true if succeeded; false if failed.
- *
- */
-bool SROMbWriteEmbedded(void __iomem *dwIoBase, unsigned char byContntOffset, unsigned char byData)
-{
-	unsigned short wDelay, wNoACK;
-	unsigned char byWait;
-
-	unsigned char byOrg;
-
-	VNSvInPortB(dwIoBase + MAC_REG_I2MCFG, &byOrg);
-	/* turn off hardware retry for getting NACK */
-	VNSvOutPortB(dwIoBase + MAC_REG_I2MCFG, (byOrg & (~I2MCFG_NORETRY)));
-	for (wNoACK = 0; wNoACK < W_MAX_I2CRETRY; wNoACK++) {
-		VNSvOutPortB(dwIoBase + MAC_REG_I2MTGID, EEP_I2C_DEV_ID);
-		VNSvOutPortB(dwIoBase + MAC_REG_I2MTGAD, byContntOffset);
-		VNSvOutPortB(dwIoBase + MAC_REG_I2MDOPT, byData);
-
-		/* issue write command */
-		VNSvOutPortB(dwIoBase + MAC_REG_I2MCSR, I2MCSR_EEMW);
-		/* wait DONE be set */
-		for (wDelay = 0; wDelay < W_MAX_TIMEOUT; wDelay++) {
-			VNSvInPortB(dwIoBase + MAC_REG_I2MCSR, &byWait);
-			if (byWait & (I2MCSR_DONE | I2MCSR_NACK))
-				break;
-			PCAvDelayByIO(CB_DELAY_LOOP_WAIT);
-		}
-
-		if ((wDelay < W_MAX_TIMEOUT) &&
-		    (!(byWait & I2MCSR_NACK))) {
-			break;
-		}
-	}
-	if (wNoACK == W_MAX_I2CRETRY) {
-		VNSvOutPortB(dwIoBase + MAC_REG_I2MCFG, byOrg);
-		return false;
-	}
-	VNSvOutPortB(dwIoBase + MAC_REG_I2MCFG, byOrg);
-	return true;
-}
-
-/*
- * Description: Turn bits on in eeprom
- *
- * Parameters:
- *  In:
- *      dwIoBase        - I/O base address
- *      byContntOffset  - address of EEPROM
- *      byBits          - bits to turn on
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void SROMvRegBitsOn(void __iomem *dwIoBase, unsigned char byContntOffset, unsigned char byBits)
-{
-	unsigned char byOrgData;
-
-	byOrgData = SROMbyReadEmbedded(dwIoBase, byContntOffset);
-	SROMbWriteEmbedded(dwIoBase, byContntOffset, (unsigned char)(byOrgData | byBits));
-}
-
-/*
- * Description: Turn bits off in eeprom
- *
- * Parameters:
- *  In:
- *      dwIoBase        - I/O base address
- *      byContntOffset  - address of EEPROM
- *      byBits          - bits to turn off
- *  Out:
- *      none
- *
- */
-void SROMvRegBitsOff(void __iomem *dwIoBase, unsigned char byContntOffset, unsigned char byBits)
-{
-	unsigned char byOrgData;
-
-	byOrgData = SROMbyReadEmbedded(dwIoBase, byContntOffset);
-	SROMbWriteEmbedded(dwIoBase, byContntOffset, (unsigned char)(byOrgData & (~byBits)));
-}
-
-/*
- * Description: Test if bits on in eeprom
- *
- * Parameters:
- *  In:
- *      dwIoBase        - I/O base address
- *      byContntOffset  - address of EEPROM
- *      byTestBits      - bits to test
- *  Out:
- *      none
- *
- * Return Value: true if all test bits on; otherwise false
- *
- */
-bool SROMbIsRegBitsOn(void __iomem *dwIoBase, unsigned char byContntOffset, unsigned char byTestBits)
-{
-	unsigned char byOrgData;
-
-	byOrgData = SROMbyReadEmbedded(dwIoBase, byContntOffset);
-	return (byOrgData & byTestBits) == byTestBits;
-}
-
-/*
- * Description: Test if bits off in eeprom
- *
- * Parameters:
- *  In:
- *      dwIoBase        - I/O base address
- *      byContntOffset  - address of EEPROM
- *      byTestBits      - bits to test
- *  Out:
- *      none
- *
- * Return Value: true if all test bits off; otherwise false
- *
- */
-bool SROMbIsRegBitsOff(void __iomem *dwIoBase, unsigned char byContntOffset, unsigned char byTestBits)
-{
-	unsigned char byOrgData;
-
-	byOrgData = SROMbyReadEmbedded(dwIoBase, byContntOffset);
-	return !(byOrgData & byTestBits);
-}
-
-/*
  * Description: Read all contents of eeprom to buffer
  *
  * Parameters:
@@ -269,30 +130,6 @@ void SROMvReadAllContents(void __iomem *dwIoBase, unsigned char *pbyEepromRegs)
 }
 
 /*
- * Description: Write all contents of buffer to eeprom
- *
- * Parameters:
- *  In:
- *      dwIoBase        - I/O base address
- *      pbyEepromRegs   - EEPROM content Buffer
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void SROMvWriteAllContents(void __iomem *dwIoBase, unsigned char *pbyEepromRegs)
-{
-	int     ii;
-
-	/* ii = Rom Address */
-	for (ii = 0; ii < EEP_MAX_CONTEXT_SIZE; ii++) {
-		SROMbWriteEmbedded(dwIoBase, (unsigned char)ii, *pbyEepromRegs);
-		pbyEepromRegs++;
-	}
-}
-
-/*
  * Description: Read Ethernet Address from eeprom to buffer
  *
  * Parameters:
@@ -314,92 +151,3 @@ void SROMvReadEtherAddress(void __iomem *dwIoBase, unsigned char *pbyEtherAddres
 		pbyEtherAddress++;
 	}
 }
-
-/*
- * Description: Write Ethernet Address from buffer to eeprom
- *
- * Parameters:
- *  In:
- *      dwIoBase        - I/O base address
- *      pbyEtherAddress - Ethernet Address buffer
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void SROMvWriteEtherAddress(void __iomem *dwIoBase, unsigned char *pbyEtherAddress)
-{
-	unsigned char ii;
-
-	/* ii = Rom Address */
-	for (ii = 0; ii < ETH_ALEN; ii++) {
-		SROMbWriteEmbedded(dwIoBase, ii, *pbyEtherAddress);
-		pbyEtherAddress++;
-	}
-}
-
-/*
- * Description: Read Sub_VID and Sub_SysId from eeprom to buffer
- *
- * Parameters:
- *  In:
- *      dwIoBase        - I/O base address
- *  Out:
- *      pdwSubSysVenId  - Sub_VID and Sub_SysId read
- *
- * Return Value: none
- *
- */
-void SROMvReadSubSysVenId(void __iomem *dwIoBase, unsigned long *pdwSubSysVenId)
-{
-	unsigned char *pbyData;
-
-	pbyData = (unsigned char *)pdwSubSysVenId;
-	/* sub vendor */
-	*pbyData = SROMbyReadEmbedded(dwIoBase, 6);
-	*(pbyData+1) = SROMbyReadEmbedded(dwIoBase, 7);
-	/* sub system */
-	*(pbyData+2) = SROMbyReadEmbedded(dwIoBase, 8);
-	*(pbyData+3) = SROMbyReadEmbedded(dwIoBase, 9);
-}
-
-/*
- * Description: Auto Load EEPROM to MAC register
- *
- * Parameters:
- *  In:
- *      dwIoBase        - I/O base address
- *  Out:
- *      none
- *
- * Return Value: true if success; otherwise false
- *
- */
-bool SROMbAutoLoad(void __iomem *dwIoBase)
-{
-	unsigned char byWait;
-	int     ii;
-
-	unsigned char byOrg;
-
-	VNSvInPortB(dwIoBase + MAC_REG_I2MCFG, &byOrg);
-	/* turn on hardware retry */
-	VNSvOutPortB(dwIoBase + MAC_REG_I2MCFG, (byOrg | I2MCFG_NORETRY));
-
-	MACvRegBitsOn(dwIoBase, MAC_REG_I2MCSR, I2MCSR_AUTOLD);
-
-	/* ii = Rom Address */
-	for (ii = 0; ii < EEP_MAX_CONTEXT_SIZE; ii++) {
-		MACvTimer0MicroSDelay(dwIoBase, CB_EEPROM_READBYTE_WAIT);
-		VNSvInPortB(dwIoBase + MAC_REG_I2MCSR, &byWait);
-		if (!(byWait & I2MCSR_AUTOLD))
-			break;
-	}
-
-	VNSvOutPortB(dwIoBase + MAC_REG_I2MCFG, byOrg);
-
-	if (ii == EEP_MAX_CONTEXT_SIZE)
-		return false;
-	return true;
-}
diff --git a/drivers/staging/vt6655/srom.h b/drivers/staging/vt6655/srom.h
index 3128e535bbd8..7d3e3ef9f17f 100644
--- a/drivers/staging/vt6655/srom.h
+++ b/drivers/staging/vt6655/srom.h
@@ -30,8 +30,6 @@
 #ifndef __SROM_H__
 #define __SROM_H__
 
-#include "ttype.h"
-
 /*---------------------  Export Definitions -------------------------*/
 
 #define EEP_MAX_CONTEXT_SIZE    256
@@ -91,40 +89,6 @@
 
 /*---------------------  Export Types  ------------------------------*/
 
-// AT24C02 eeprom contents
-//      2048 bits = 256 bytes = 128 words
-//
-typedef struct tagSSromReg {
-	unsigned char abyPAR[6];                  // 0x00 (unsigned short)
-
-	unsigned short wSUB_VID;                   // 0x03 (unsigned short)
-	unsigned short wSUB_SID;
-
-	unsigned char byBCFG0;                    // 0x05 (unsigned short)
-	unsigned char byBCFG1;
-
-	unsigned char byFCR0;                     // 0x06 (unsigned short)
-	unsigned char byFCR1;
-	unsigned char byPMC0;                     // 0x07 (unsigned short)
-	unsigned char byPMC1;
-	unsigned char byMAXLAT;                   // 0x08 (unsigned short)
-	unsigned char byMINGNT;
-	unsigned char byCFG0;                     // 0x09 (unsigned short)
-	unsigned char byCFG1;
-	unsigned short wCISPTR;                    // 0x0A (unsigned short)
-	unsigned short wRsv0;                      // 0x0B (unsigned short)
-	unsigned short wRsv1;                      // 0x0C (unsigned short)
-	unsigned char byBBPAIR;                   // 0x0D (unsigned short)
-	unsigned char byRFTYPE;
-	unsigned char byMinChannel;               // 0x0E (unsigned short)
-	unsigned char byMaxChannel;
-	unsigned char bySignature;                // 0x0F (unsigned short)
-	unsigned char byCheckSum;
-
-	unsigned char abyReserved0[96];           // 0x10 (unsigned short)
-	unsigned char abyCIS[128];                // 0x80 (unsigned short)
-} SSromReg, *PSSromReg;
-
 /*---------------------  Export Macros ------------------------------*/
 
 /*---------------------  Export Classes  ----------------------------*/
@@ -134,22 +98,9 @@ typedef struct tagSSromReg {
 /*---------------------  Export Functions  --------------------------*/
 
 unsigned char SROMbyReadEmbedded(void __iomem *dwIoBase, unsigned char byContntOffset);
-bool SROMbWriteEmbedded(void __iomem *dwIoBase, unsigned char byContntOffset, unsigned char byData);
-
-void SROMvRegBitsOn(void __iomem *dwIoBase, unsigned char byContntOffset, unsigned char byBits);
-void SROMvRegBitsOff(void __iomem *dwIoBase, unsigned char byContntOffset, unsigned char byBits);
-
-bool SROMbIsRegBitsOn(void __iomem *dwIoBase, unsigned char byContntOffset, unsigned char byTestBits);
-bool SROMbIsRegBitsOff(void __iomem *dwIoBase, unsigned char byContntOffset, unsigned char byTestBits);
 
 void SROMvReadAllContents(void __iomem *dwIoBase, unsigned char *pbyEepromRegs);
-void SROMvWriteAllContents(void __iomem *dwIoBase, unsigned char *pbyEepromRegs);
 
 void SROMvReadEtherAddress(void __iomem *dwIoBase, unsigned char *pbyEtherAddress);
-void SROMvWriteEtherAddress(void __iomem *dwIoBase, unsigned char *pbyEtherAddress);
-
-void SROMvReadSubSysVenId(void __iomem *dwIoBase, unsigned long *pdwSubSysVenId);
-
-bool SROMbAutoLoad(void __iomem *dwIoBase);
 
 #endif // __EEPROM_H__
diff --git a/drivers/staging/vt6655/tcrc.c b/drivers/staging/vt6655/tcrc.c
deleted file mode 100644
index ddc5efd040f9..000000000000
--- a/drivers/staging/vt6655/tcrc.c
+++ /dev/null
@@ -1,191 +0,0 @@
-/*
- * Copyright (c) 2003 VIA Networking, Inc. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: tcrc.c
- *
- * Purpose: Implement functions to calculate CRC
- *
- * Author: Tevin Chen
- *
- * Date: May 21, 1996
- *
- * Functions:
- *      CRCdwCrc32 -
- *      CRCdwGetCrc32 -
- *      CRCdwGetCrc32Ex -
- *
- * Revision History:
- *
- */
-
-#include "tcrc.h"
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/* 32-bit CRC table */
-static const unsigned long s_adwCrc32Table[256] = {
-	0x00000000L, 0x77073096L, 0xEE0E612CL, 0x990951BAL,
-	0x076DC419L, 0x706AF48FL, 0xE963A535L, 0x9E6495A3L,
-	0x0EDB8832L, 0x79DCB8A4L, 0xE0D5E91EL, 0x97D2D988L,
-	0x09B64C2BL, 0x7EB17CBDL, 0xE7B82D07L, 0x90BF1D91L,
-	0x1DB71064L, 0x6AB020F2L, 0xF3B97148L, 0x84BE41DEL,
-	0x1ADAD47DL, 0x6DDDE4EBL, 0xF4D4B551L, 0x83D385C7L,
-	0x136C9856L, 0x646BA8C0L, 0xFD62F97AL, 0x8A65C9ECL,
-	0x14015C4FL, 0x63066CD9L, 0xFA0F3D63L, 0x8D080DF5L,
-	0x3B6E20C8L, 0x4C69105EL, 0xD56041E4L, 0xA2677172L,
-	0x3C03E4D1L, 0x4B04D447L, 0xD20D85FDL, 0xA50AB56BL,
-	0x35B5A8FAL, 0x42B2986CL, 0xDBBBC9D6L, 0xACBCF940L,
-	0x32D86CE3L, 0x45DF5C75L, 0xDCD60DCFL, 0xABD13D59L,
-	0x26D930ACL, 0x51DE003AL, 0xC8D75180L, 0xBFD06116L,
-	0x21B4F4B5L, 0x56B3C423L, 0xCFBA9599L, 0xB8BDA50FL,
-	0x2802B89EL, 0x5F058808L, 0xC60CD9B2L, 0xB10BE924L,
-	0x2F6F7C87L, 0x58684C11L, 0xC1611DABL, 0xB6662D3DL,
-	0x76DC4190L, 0x01DB7106L, 0x98D220BCL, 0xEFD5102AL,
-	0x71B18589L, 0x06B6B51FL, 0x9FBFE4A5L, 0xE8B8D433L,
-	0x7807C9A2L, 0x0F00F934L, 0x9609A88EL, 0xE10E9818L,
-	0x7F6A0DBBL, 0x086D3D2DL, 0x91646C97L, 0xE6635C01L,
-	0x6B6B51F4L, 0x1C6C6162L, 0x856530D8L, 0xF262004EL,
-	0x6C0695EDL, 0x1B01A57BL, 0x8208F4C1L, 0xF50FC457L,
-	0x65B0D9C6L, 0x12B7E950L, 0x8BBEB8EAL, 0xFCB9887CL,
-	0x62DD1DDFL, 0x15DA2D49L, 0x8CD37CF3L, 0xFBD44C65L,
-	0x4DB26158L, 0x3AB551CEL, 0xA3BC0074L, 0xD4BB30E2L,
-	0x4ADFA541L, 0x3DD895D7L, 0xA4D1C46DL, 0xD3D6F4FBL,
-	0x4369E96AL, 0x346ED9FCL, 0xAD678846L, 0xDA60B8D0L,
-	0x44042D73L, 0x33031DE5L, 0xAA0A4C5FL, 0xDD0D7CC9L,
-	0x5005713CL, 0x270241AAL, 0xBE0B1010L, 0xC90C2086L,
-	0x5768B525L, 0x206F85B3L, 0xB966D409L, 0xCE61E49FL,
-	0x5EDEF90EL, 0x29D9C998L, 0xB0D09822L, 0xC7D7A8B4L,
-	0x59B33D17L, 0x2EB40D81L, 0xB7BD5C3BL, 0xC0BA6CADL,
-	0xEDB88320L, 0x9ABFB3B6L, 0x03B6E20CL, 0x74B1D29AL,
-	0xEAD54739L, 0x9DD277AFL, 0x04DB2615L, 0x73DC1683L,
-	0xE3630B12L, 0x94643B84L, 0x0D6D6A3EL, 0x7A6A5AA8L,
-	0xE40ECF0BL, 0x9309FF9DL, 0x0A00AE27L, 0x7D079EB1L,
-	0xF00F9344L, 0x8708A3D2L, 0x1E01F268L, 0x6906C2FEL,
-	0xF762575DL, 0x806567CBL, 0x196C3671L, 0x6E6B06E7L,
-	0xFED41B76L, 0x89D32BE0L, 0x10DA7A5AL, 0x67DD4ACCL,
-	0xF9B9DF6FL, 0x8EBEEFF9L, 0x17B7BE43L, 0x60B08ED5L,
-	0xD6D6A3E8L, 0xA1D1937EL, 0x38D8C2C4L, 0x4FDFF252L,
-	0xD1BB67F1L, 0xA6BC5767L, 0x3FB506DDL, 0x48B2364BL,
-	0xD80D2BDAL, 0xAF0A1B4CL, 0x36034AF6L, 0x41047A60L,
-	0xDF60EFC3L, 0xA867DF55L, 0x316E8EEFL, 0x4669BE79L,
-	0xCB61B38CL, 0xBC66831AL, 0x256FD2A0L, 0x5268E236L,
-	0xCC0C7795L, 0xBB0B4703L, 0x220216B9L, 0x5505262FL,
-	0xC5BA3BBEL, 0xB2BD0B28L, 0x2BB45A92L, 0x5CB36A04L,
-	0xC2D7FFA7L, 0xB5D0CF31L, 0x2CD99E8BL, 0x5BDEAE1DL,
-	0x9B64C2B0L, 0xEC63F226L, 0x756AA39CL, 0x026D930AL,
-	0x9C0906A9L, 0xEB0E363FL, 0x72076785L, 0x05005713L,
-	0x95BF4A82L, 0xE2B87A14L, 0x7BB12BAEL, 0x0CB61B38L,
-	0x92D28E9BL, 0xE5D5BE0DL, 0x7CDCEFB7L, 0x0BDBDF21L,
-	0x86D3D2D4L, 0xF1D4E242L, 0x68DDB3F8L, 0x1FDA836EL,
-	0x81BE16CDL, 0xF6B9265BL, 0x6FB077E1L, 0x18B74777L,
-	0x88085AE6L, 0xFF0F6A70L, 0x66063BCAL, 0x11010B5CL,
-	0x8F659EFFL, 0xF862AE69L, 0x616BFFD3L, 0x166CCF45L,
-	0xA00AE278L, 0xD70DD2EEL, 0x4E048354L, 0x3903B3C2L,
-	0xA7672661L, 0xD06016F7L, 0x4969474DL, 0x3E6E77DBL,
-	0xAED16A4AL, 0xD9D65ADCL, 0x40DF0B66L, 0x37D83BF0L,
-	0xA9BCAE53L, 0xDEBB9EC5L, 0x47B2CF7FL, 0x30B5FFE9L,
-	0xBDBDF21CL, 0xCABAC28AL, 0x53B39330L, 0x24B4A3A6L,
-	0xBAD03605L, 0xCDD70693L, 0x54DE5729L, 0x23D967BFL,
-	0xB3667A2EL, 0xC4614AB8L, 0x5D681B02L, 0x2A6F2B94L,
-	0xB40BBE37L, 0xC30C8EA1L, 0x5A05DF1BL, 0x2D02EF8DL
-};
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*+
- *
- * Description:
- *    Generate a CRC-32 from the data stream
- *
- * Parameters:
- *  In:
- *      pbyData     - the data stream
- *      cbByte      - the length of the stream
- *      dwCrcSeed   - Seed for CRC32
- *  Out:
- *      none
- *
- * Return Value: CRC-32
- *
- -*/
-unsigned long CRCdwCrc32(unsigned char *pbyData, unsigned int cbByte, unsigned long dwCrcSeed)
-{
-	unsigned long dwCrc;
-
-	dwCrc = dwCrcSeed;
-	while (cbByte--) {
-		dwCrc = s_adwCrc32Table[(unsigned char)((dwCrc ^ (*pbyData)) & 0xFF)] ^ (dwCrc >> 8);
-		pbyData++;
-	}
-
-	return dwCrc;
-}
-
-/*+
- *
- * Description:
- * To test CRC generator, input 8 bytes packet
- *      -- 0xff 0xff 0xff 0xff 0x00 0x00 0x00 0x00
- * the generated CRC should be
- *      -- 0xff 0xff 0xff 0xff
- *
- * Parameters:
- *  In:
- *      pbyData     - the data stream
- *      cbByte      - the length of the stream
- *  Out:
- *      none
- *
- * Return Value: CRC-32
- *
- -*/
-unsigned long CRCdwGetCrc32(unsigned char *pbyData, unsigned int cbByte)
-{
-	return ~CRCdwCrc32(pbyData, cbByte, 0xFFFFFFFFL);
-}
-
-/*+
- *
- * Description:
- *
- * NOTE.... Because CRCdwGetCrc32Ex() is an iteration function,
- *          this means we will use the output of CRCdwGetCrc32Ex()
- *          to be a new argument to do next CRCdwGetCrc32Ex() calculation.
- *          Thus, the final result must be inverted to be the
- *          correct answer.
- *
- * Parameters:
- *  In:
- *      pbyData     - the data stream
- *      cbByte      - the length of the stream
- *  Out:
- *      none
- *
- * Return Value: CRC-32
- *
- -*/
-unsigned long CRCdwGetCrc32Ex(unsigned char *pbyData, unsigned int cbByte, unsigned long dwPreCRC)
-{
-	return CRCdwCrc32(pbyData, cbByte, dwPreCRC);
-}
diff --git a/drivers/staging/vt6655/tcrc.h b/drivers/staging/vt6655/tcrc.h
deleted file mode 100644
index 82b5ddafae55..000000000000
--- a/drivers/staging/vt6655/tcrc.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
- * Copyright (c) 2003 VIA Networking, Inc. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: tcrc.h
- *
- * Purpose: Implement functions to calculate CRC
- *
- * Author: Tevin Chen
- *
- * Date: Jan. 28, 1997
- *
- */
-
-#ifndef __TCRC_H__
-#define __TCRC_H__
-
-#include "ttype.h"
-
-/*---------------------  Export Definitions -------------------------*/
-
-/*---------------------  Export Types  ------------------------------*/
-
-/*---------------------  Export Macros ------------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-unsigned long CRCdwCrc32(unsigned char *pbyData, unsigned int cbByte, unsigned long dwCrcSeed);
-unsigned long CRCdwGetCrc32(unsigned char *pbyData, unsigned int cbByte);
-unsigned long CRCdwGetCrc32Ex(unsigned char *pbyData, unsigned int cbByte, unsigned long dwPreCRC);
-
-#endif // __TCRC_H__
diff --git a/drivers/staging/vt6655/tether.c b/drivers/staging/vt6655/tether.c
deleted file mode 100644
index 1e7d3e2115a9..000000000000
--- a/drivers/staging/vt6655/tether.c
+++ /dev/null
@@ -1,105 +0,0 @@
-/*
- * Copyright (c) 2003 VIA Networking, Inc. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: tether.c
- *
- * Purpose:
- *
- * Author: Tevin Chen
- *
- * Date: May 21, 1996
- *
- * Functions:
- *      ETHbyGetHashIndexByCrc32 - Calculate multicast hash value by CRC32
- *      ETHbIsBufferCrc32Ok - Check CRC value of the buffer if Ok or not
- *
- * Revision History:
- *
- */
-
-#include "device.h"
-#include "tmacro.h"
-#include "tcrc.h"
-#include "tether.h"
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*
- * Description: Calculate multicast hash value by CRC32
- *
- * Parameters:
- *  In:
- *		pbyMultiAddr    - Multicast Address
- *  Out:
- *      none
- *
- * Return Value: Hash value
- *
- */
-unsigned char ETHbyGetHashIndexByCrc32(unsigned char *pbyMultiAddr)
-{
-	int     ii;
-	unsigned char byTmpHash;
-	unsigned char byHash = 0;
-
-	// get the least 6-bits from CRC generator
-	byTmpHash = (unsigned char)(CRCdwCrc32(pbyMultiAddr, ETH_ALEN,
-					       0xFFFFFFFFL) & 0x3F);
-	// reverse most bit to least bit
-	for (ii = 0; ii < (sizeof(byTmpHash) * 8); ii++) {
-		byHash <<= 1;
-		if (byTmpHash & 0x01)
-			byHash |= 1;
-		byTmpHash >>= 1;
-	}
-
-	// adjust 6-bits to the right most
-	return byHash >> 2;
-}
-
-/*
- * Description: Check CRC value of the buffer if Ok or not
- *
- * Parameters:
- *  In:
- *		pbyBuffer	    - pointer of buffer (normally is rx buffer)
- *		cbFrameLength	- length of buffer, including CRC portion
- *  Out:
- *      none
- *
- * Return Value: true if ok; false if error.
- *
- */
-bool ETHbIsBufferCrc32Ok(unsigned char *pbyBuffer, unsigned int cbFrameLength)
-{
-	unsigned long dwCRC;
-
-	dwCRC = CRCdwGetCrc32(pbyBuffer, cbFrameLength - 4);
-	if (cpu_to_le32(*((unsigned long *)(pbyBuffer + cbFrameLength - 4))) != dwCRC)
-		return false;
-
-	return true;
-}
diff --git a/drivers/staging/vt6655/tether.h b/drivers/staging/vt6655/tether.h
deleted file mode 100644
index 94cc8830d8cc..000000000000
--- a/drivers/staging/vt6655/tether.h
+++ /dev/null
@@ -1,192 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: tether.h
- *
- * Purpose:
- *
- * Author: Tevin Chen
- *
- * Date: Jan. 28, 1997
- *
- */
-
-#ifndef __TETHER_H__
-#define __TETHER_H__
-
-#include <linux/etherdevice.h>
-#include "ttype.h"
-
-/*---------------------  Export Definitions -------------------------*/
-//
-// constants
-//
-#define U_ETHER_ADDR_STR_LEN (ETH_ALEN * 2 + 1)
-// Ethernet address string length
-
-#define MAX_LOOKAHEAD_SIZE  ETH_FRAME_LEN
-
-#define U_MULTI_ADDR_LEN    8           // multicast address length
-
-#ifdef __BIG_ENDIAN
-
-#define TYPE_PKT_IP         0x0800      //
-#define TYPE_PKT_ARP        0x0806      //
-#define TYPE_PKT_RARP       0x8035      //
-#define TYPE_PKT_IPX	    0x8137	    //
-#define TYPE_PKT_802_1x     0x888e
-#define TYPE_PKT_PreAuth    0x88C7
-
-#define TYPE_PKT_PING_M_REQ 0x8011      // master reguest
-#define TYPE_PKT_PING_S_GNT 0x8022      // slave grant
-#define TYPE_PKT_PING_M     0x8077      // pingpong master packet
-#define TYPE_PKT_PING_S     0x8088      // pingpong slave packet
-#define TYPE_PKT_WOL_M_REQ  0x8033      // WOL waker request
-#define TYPE_PKT_WOL_S_GNT  0x8044      // WOL sleeper grant
-#define TYPE_MGMT_PROBE_RSP 0x5000
-#define TYPE_PKT_VNT_DIAG   0x8011      // Diag Pkt
-#define TYPE_PKT_VNT_PER    0x8888      // Diag PER Pkt
-//
-// wFrameCtl field in the S802_11Header
-//
-// NOTE....
-//   in network byte order, high byte is going first
-#define FC_TODS             0x0001
-#define FC_FROMDS           0x0002
-#define FC_MOREFRAG         0x0004
-#define FC_RETRY            0x0008
-#define FC_POWERMGT         0x0010
-#define FC_MOREDATA         0x0020
-#define FC_WEP              0x0040
-#define TYPE_802_11_ATIM    0x9000
-
-#define TYPE_802_11_DATA    0x0800
-#define TYPE_802_11_CTL     0x0400
-#define TYPE_802_11_MGMT    0x0000
-#define TYPE_802_11_MASK    0x0C00
-#define TYPE_SUBTYPE_MASK   0xFC00
-#define TYPE_802_11_NODATA  0x4000
-#define TYPE_DATE_NULL      0x4800
-
-#define TYPE_CTL_PSPOLL     0xa400
-#define TYPE_CTL_RTS        0xb400
-#define TYPE_CTL_CTS        0xc400
-#define TYPE_CTL_ACK        0xd400
-
-#else //if LITTLE_ENDIAN
-//
-// wType field in the SEthernetHeader
-//
-// NOTE....
-//   in network byte order, high byte is going first
-#define TYPE_PKT_IP         0x0008      //
-#define TYPE_PKT_ARP        0x0608      //
-#define TYPE_PKT_RARP       0x3580      //
-#define TYPE_PKT_IPX	    0x3781	    //
-
-#define TYPE_PKT_802_1x     0x8e88
-#define TYPE_PKT_PreAuth    0xC788
-
-#define TYPE_PKT_PING_M_REQ 0x1180      // master reguest
-#define TYPE_PKT_PING_S_GNT 0x2280      // slave grant
-#define TYPE_PKT_PING_M     0x7780      // pingpong master packet
-#define TYPE_PKT_PING_S     0x8880      // pingpong slave packet
-#define TYPE_PKT_WOL_M_REQ  0x3380      // WOL waker request
-#define TYPE_PKT_WOL_S_GNT  0x4480      // WOL sleeper grant
-#define TYPE_MGMT_PROBE_RSP 0x0050
-#define TYPE_PKT_VNT_DIAG   0x1180      // Diag Pkt
-#define TYPE_PKT_VNT_PER    0x8888      // Diag PER Pkt
-//
-// wFrameCtl field in the S802_11Header
-//
-// NOTE....
-//   in network byte order, high byte is going first
-#define FC_TODS             0x0100
-#define FC_FROMDS           0x0200
-#define FC_MOREFRAG         0x0400
-#define FC_RETRY            0x0800
-#define FC_POWERMGT         0x1000
-#define FC_MOREDATA         0x2000
-#define FC_WEP              0x4000
-#define TYPE_802_11_ATIM    0x0090
-
-#define TYPE_802_11_DATA    0x0008
-#define TYPE_802_11_CTL     0x0004
-#define TYPE_802_11_MGMT    0x0000
-#define TYPE_802_11_MASK    0x000C
-#define TYPE_SUBTYPE_MASK   0x00FC
-#define TYPE_802_11_NODATA  0x0040
-#define TYPE_DATE_NULL      0x0048
-
-#define TYPE_CTL_PSPOLL     0x00a4
-#define TYPE_CTL_RTS        0x00b4
-#define TYPE_CTL_CTS        0x00c4
-#define TYPE_CTL_ACK        0x00d4
-
-#endif //#ifdef __BIG_ENDIAN
-
-#define WEP_IV_MASK         0x00FFFFFF
-
-/*---------------------  Export Types  ------------------------------*/
-//
-// Ethernet packet
-//
-typedef struct tagSEthernetHeader {
-	unsigned char abyDstAddr[ETH_ALEN];
-	unsigned char abySrcAddr[ETH_ALEN];
-	unsigned short wType;
-} __attribute__ ((__packed__))
-SEthernetHeader, *PSEthernetHeader;
-
-//
-// 802_3 packet
-//
-typedef struct tagS802_3Header {
-	unsigned char abyDstAddr[ETH_ALEN];
-	unsigned char abySrcAddr[ETH_ALEN];
-	unsigned short wLen;
-} __attribute__ ((__packed__))
-S802_3Header, *PS802_3Header;
-
-//
-// 802_11 packet
-//
-typedef struct tagS802_11Header {
-	unsigned short wFrameCtl;
-	unsigned short wDurationID;
-	unsigned char abyAddr1[ETH_ALEN];
-	unsigned char abyAddr2[ETH_ALEN];
-	unsigned char abyAddr3[ETH_ALEN];
-	unsigned short wSeqCtl;
-	unsigned char abyAddr4[ETH_ALEN];
-} __attribute__ ((__packed__))
-S802_11Header, *PS802_11Header;
-
-/*---------------------  Export Macros ------------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-unsigned char ETHbyGetHashIndexByCrc32(unsigned char *pbyMultiAddr);
-//unsigned char ETHbyGetHashIndexByCrc(unsigned char *pbyMultiAddr);
-bool ETHbIsBufferCrc32Ok(unsigned char *pbyBuffer, unsigned int cbFrameLength);
-
-#endif // __TETHER_H__
diff --git a/drivers/staging/vt6655/tkip.c b/drivers/staging/vt6655/tkip.c
deleted file mode 100644
index f758d021c60e..000000000000
--- a/drivers/staging/vt6655/tkip.c
+++ /dev/null
@@ -1,268 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: tkip.c
- *
- * Purpose: Implement functions for 802.11i TKIP
- *
- * Author: Jerry Chen
- *
- * Date: Mar. 11, 2003
- *
- * Functions:
- *      TKIPvMixKey - Get TKIP RC4 Key from TK,TA, and TSC
- *
- * Revision History:
- *
- */
-
-#include "tmacro.h"
-#include "tkip.h"
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/* The Sbox is reduced to 2 16-bit wide tables, each with 256 entries. */
-/* The 2nd table is the same as the 1st but with the upper and lower   */
-/* bytes swapped. To allow an endian tolerant implementation, the byte */
-/* halves have been expressed independently here.                      */
-static const unsigned char TKIP_Sbox_Lower[256] = {
-	0xA5, 0x84, 0x99, 0x8D, 0x0D, 0xBD, 0xB1, 0x54,
-	0x50, 0x03, 0xA9, 0x7D, 0x19, 0x62, 0xE6, 0x9A,
-	0x45, 0x9D, 0x40, 0x87, 0x15, 0xEB, 0xC9, 0x0B,
-	0xEC, 0x67, 0xFD, 0xEA, 0xBF, 0xF7, 0x96, 0x5B,
-	0xC2, 0x1C, 0xAE, 0x6A, 0x5A, 0x41, 0x02, 0x4F,
-	0x5C, 0xF4, 0x34, 0x08, 0x93, 0x73, 0x53, 0x3F,
-	0x0C, 0x52, 0x65, 0x5E, 0x28, 0xA1, 0x0F, 0xB5,
-	0x09, 0x36, 0x9B, 0x3D, 0x26, 0x69, 0xCD, 0x9F,
-	0x1B, 0x9E, 0x74, 0x2E, 0x2D, 0xB2, 0xEE, 0xFB,
-	0xF6, 0x4D, 0x61, 0xCE, 0x7B, 0x3E, 0x71, 0x97,
-	0xF5, 0x68, 0x00, 0x2C, 0x60, 0x1F, 0xC8, 0xED,
-	0xBE, 0x46, 0xD9, 0x4B, 0xDE, 0xD4, 0xE8, 0x4A,
-	0x6B, 0x2A, 0xE5, 0x16, 0xC5, 0xD7, 0x55, 0x94,
-	0xCF, 0x10, 0x06, 0x81, 0xF0, 0x44, 0xBA, 0xE3,
-	0xF3, 0xFE, 0xC0, 0x8A, 0xAD, 0xBC, 0x48, 0x04,
-	0xDF, 0xC1, 0x75, 0x63, 0x30, 0x1A, 0x0E, 0x6D,
-	0x4C, 0x14, 0x35, 0x2F, 0xE1, 0xA2, 0xCC, 0x39,
-	0x57, 0xF2, 0x82, 0x47, 0xAC, 0xE7, 0x2B, 0x95,
-	0xA0, 0x98, 0xD1, 0x7F, 0x66, 0x7E, 0xAB, 0x83,
-	0xCA, 0x29, 0xD3, 0x3C, 0x79, 0xE2, 0x1D, 0x76,
-	0x3B, 0x56, 0x4E, 0x1E, 0xDB, 0x0A, 0x6C, 0xE4,
-	0x5D, 0x6E, 0xEF, 0xA6, 0xA8, 0xA4, 0x37, 0x8B,
-	0x32, 0x43, 0x59, 0xB7, 0x8C, 0x64, 0xD2, 0xE0,
-	0xB4, 0xFA, 0x07, 0x25, 0xAF, 0x8E, 0xE9, 0x18,
-	0xD5, 0x88, 0x6F, 0x72, 0x24, 0xF1, 0xC7, 0x51,
-	0x23, 0x7C, 0x9C, 0x21, 0xDD, 0xDC, 0x86, 0x85,
-	0x90, 0x42, 0xC4, 0xAA, 0xD8, 0x05, 0x01, 0x12,
-	0xA3, 0x5F, 0xF9, 0xD0, 0x91, 0x58, 0x27, 0xB9,
-	0x38, 0x13, 0xB3, 0x33, 0xBB, 0x70, 0x89, 0xA7,
-	0xB6, 0x22, 0x92, 0x20, 0x49, 0xFF, 0x78, 0x7A,
-	0x8F, 0xF8, 0x80, 0x17, 0xDA, 0x31, 0xC6, 0xB8,
-	0xC3, 0xB0, 0x77, 0x11, 0xCB, 0xFC, 0xD6, 0x3A
-};
-
-static const unsigned char TKIP_Sbox_Upper[256] = {
-	0xC6, 0xF8, 0xEE, 0xF6, 0xFF, 0xD6, 0xDE, 0x91,
-	0x60, 0x02, 0xCE, 0x56, 0xE7, 0xB5, 0x4D, 0xEC,
-	0x8F, 0x1F, 0x89, 0xFA, 0xEF, 0xB2, 0x8E, 0xFB,
-	0x41, 0xB3, 0x5F, 0x45, 0x23, 0x53, 0xE4, 0x9B,
-	0x75, 0xE1, 0x3D, 0x4C, 0x6C, 0x7E, 0xF5, 0x83,
-	0x68, 0x51, 0xD1, 0xF9, 0xE2, 0xAB, 0x62, 0x2A,
-	0x08, 0x95, 0x46, 0x9D, 0x30, 0x37, 0x0A, 0x2F,
-	0x0E, 0x24, 0x1B, 0xDF, 0xCD, 0x4E, 0x7F, 0xEA,
-	0x12, 0x1D, 0x58, 0x34, 0x36, 0xDC, 0xB4, 0x5B,
-	0xA4, 0x76, 0xB7, 0x7D, 0x52, 0xDD, 0x5E, 0x13,
-	0xA6, 0xB9, 0x00, 0xC1, 0x40, 0xE3, 0x79, 0xB6,
-	0xD4, 0x8D, 0x67, 0x72, 0x94, 0x98, 0xB0, 0x85,
-	0xBB, 0xC5, 0x4F, 0xED, 0x86, 0x9A, 0x66, 0x11,
-	0x8A, 0xE9, 0x04, 0xFE, 0xA0, 0x78, 0x25, 0x4B,
-	0xA2, 0x5D, 0x80, 0x05, 0x3F, 0x21, 0x70, 0xF1,
-	0x63, 0x77, 0xAF, 0x42, 0x20, 0xE5, 0xFD, 0xBF,
-	0x81, 0x18, 0x26, 0xC3, 0xBE, 0x35, 0x88, 0x2E,
-	0x93, 0x55, 0xFC, 0x7A, 0xC8, 0xBA, 0x32, 0xE6,
-	0xC0, 0x19, 0x9E, 0xA3, 0x44, 0x54, 0x3B, 0x0B,
-	0x8C, 0xC7, 0x6B, 0x28, 0xA7, 0xBC, 0x16, 0xAD,
-	0xDB, 0x64, 0x74, 0x14, 0x92, 0x0C, 0x48, 0xB8,
-	0x9F, 0xBD, 0x43, 0xC4, 0x39, 0x31, 0xD3, 0xF2,
-	0xD5, 0x8B, 0x6E, 0xDA, 0x01, 0xB1, 0x9C, 0x49,
-	0xD8, 0xAC, 0xF3, 0xCF, 0xCA, 0xF4, 0x47, 0x10,
-	0x6F, 0xF0, 0x4A, 0x5C, 0x38, 0x57, 0x73, 0x97,
-	0xCB, 0xA1, 0xE8, 0x3E, 0x96, 0x61, 0x0D, 0x0F,
-	0xE0, 0x7C, 0x71, 0xCC, 0x90, 0x06, 0xF7, 0x1C,
-	0xC2, 0x6A, 0xAE, 0x69, 0x17, 0x99, 0x3A, 0x27,
-	0xD9, 0xEB, 0x2B, 0x22, 0xD2, 0xA9, 0x07, 0x33,
-	0x2D, 0x3C, 0x15, 0xC9, 0x87, 0xAA, 0x50, 0xA5,
-	0x03, 0x59, 0x09, 0x1A, 0x65, 0xD7, 0x84, 0xD0,
-	0x82, 0x29, 0x5A, 0x1E, 0x7B, 0xA8, 0x6D, 0x2C
-};
-
-//STKIPKeyManagement  sTKIPKeyTable[MAX_TKIP_KEY];
-
-/*---------------------  Static Functions  --------------------------*/
-unsigned int tkip_sbox(unsigned int index);
-unsigned int rotr1(unsigned int a);
-
-/*---------------------  Export Variables  --------------------------*/
-
-/************************************************************/
-/* tkip_sbox()                                              */
-/* Returns a 16 bit value from a 64K entry table. The Table */
-/* is synthesized from two 256 entry byte wide tables.      */
-/************************************************************/
-unsigned int tkip_sbox(unsigned int index)
-{
-	unsigned int index_low;
-	unsigned int index_high;
-	unsigned int left, right;
-
-	index_low = (index % 256);
-	index_high = ((index >> 8) % 256);
-
-	left = TKIP_Sbox_Lower[index_low] + (TKIP_Sbox_Upper[index_low] * 256);
-	right = TKIP_Sbox_Upper[index_high] + (TKIP_Sbox_Lower[index_high] * 256);
-
-	return left ^ right;
-};
-
-unsigned int rotr1(unsigned int a)
-{
-	unsigned int b;
-
-	if ((a & 0x01) == 0x01)
-		b = (a >> 1) | 0x8000;
-	else
-		b = (a >> 1) & 0x7fff;
-
-	b = b % 65536;
-	return b;
-}
-
-/*
- * Description: Calculate RC4Key fom TK, TA, and TSC
- *
- * Parameters:
- *  In:
- *      pbyTKey         - TKey
- *      pbyTA           - TA
- *      dwTSC           - TSC
- *  Out:
- *      pbyRC4Key       - RC4Key
- *
- * Return Value: none
- *
- */
-void TKIPvMixKey(
-	unsigned char *pbyTKey,
-	unsigned char *pbyTA,
-	unsigned short wTSC15_0,
-	unsigned long dwTSC47_16,
-	unsigned char *pbyRC4Key
-)
-{
-	unsigned int p1k[5];
-	unsigned int tsc0, tsc1, tsc2;
-	unsigned int ppk0, ppk1, ppk2, ppk3, ppk4, ppk5;
-	unsigned long int pnl, pnh;
-
-	int i, j;
-
-	pnl = wTSC15_0;
-	pnh = dwTSC47_16;
-
-	tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
-	tsc1 = (unsigned int)(pnh % 65536);
-	tsc2 = (unsigned int)(pnl % 65536); /* lsb */
-
-	/* Phase 1, step 1 */
-	p1k[0] = tsc1;
-	p1k[1] = tsc0;
-	p1k[2] = (unsigned int)(pbyTA[0] + (pbyTA[1]*256));
-	p1k[3] = (unsigned int)(pbyTA[2] + (pbyTA[3]*256));
-	p1k[4] = (unsigned int)(pbyTA[4] + (pbyTA[5]*256));
-
-	/* Phase 1, step 2 */
-	for (i = 0; i < 8; i++) {
-		j = 2 * (i & 1);
-		p1k[0] = (p1k[0] + tkip_sbox((p1k[4] ^ ((256*pbyTKey[1+j]) + pbyTKey[j])) % 65536)) % 65536;
-		p1k[1] = (p1k[1] + tkip_sbox((p1k[0] ^ ((256*pbyTKey[5+j]) + pbyTKey[4+j])) % 65536)) % 65536;
-		p1k[2] = (p1k[2] + tkip_sbox((p1k[1] ^ ((256*pbyTKey[9+j]) + pbyTKey[8+j])) % 65536)) % 65536;
-		p1k[3] = (p1k[3] + tkip_sbox((p1k[2] ^ ((256*pbyTKey[13+j]) + pbyTKey[12+j])) % 65536)) % 65536;
-		p1k[4] = (p1k[4] + tkip_sbox((p1k[3] ^ (((256*pbyTKey[1+j]) + pbyTKey[j]))) % 65536)) % 65536;
-		p1k[4] = (p1k[4] + i) % 65536;
-	}
-	/* Phase 2, Step 1 */
-	ppk0 = p1k[0];
-	ppk1 = p1k[1];
-	ppk2 = p1k[2];
-	ppk3 = p1k[3];
-	ppk4 = p1k[4];
-	ppk5 = (p1k[4] + tsc2) % 65536;
-
-	/* Phase2, Step 2 */
-	ppk0 = ppk0 + tkip_sbox((ppk5 ^ ((256*pbyTKey[1]) + pbyTKey[0])) % 65536);
-	ppk1 = ppk1 + tkip_sbox((ppk0 ^ ((256*pbyTKey[3]) + pbyTKey[2])) % 65536);
-	ppk2 = ppk2 + tkip_sbox((ppk1 ^ ((256*pbyTKey[5]) + pbyTKey[4])) % 65536);
-	ppk3 = ppk3 + tkip_sbox((ppk2 ^ ((256*pbyTKey[7]) + pbyTKey[6])) % 65536);
-	ppk4 = ppk4 + tkip_sbox((ppk3 ^ ((256*pbyTKey[9]) + pbyTKey[8])) % 65536);
-	ppk5 = ppk5 + tkip_sbox((ppk4 ^ ((256*pbyTKey[11]) + pbyTKey[10])) % 65536);
-
-	ppk0 = ppk0 + rotr1(ppk5 ^ ((256*pbyTKey[13]) + pbyTKey[12]));
-	ppk1 = ppk1 + rotr1(ppk0 ^ ((256*pbyTKey[15]) + pbyTKey[14]));
-	ppk2 = ppk2 + rotr1(ppk1);
-	ppk3 = ppk3 + rotr1(ppk2);
-	ppk4 = ppk4 + rotr1(ppk3);
-	ppk5 = ppk5 + rotr1(ppk4);
-
-	/* Phase 2, Step 3 */
-	pbyRC4Key[0] = (tsc2 >> 8) % 256;
-	pbyRC4Key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
-	pbyRC4Key[2] = tsc2 % 256;
-	pbyRC4Key[3] = ((ppk5 ^ ((256*pbyTKey[1]) + pbyTKey[0])) >> 1) % 256;
-
-	pbyRC4Key[4] = ppk0 % 256;
-	pbyRC4Key[5] = (ppk0 >> 8) % 256;
-
-	pbyRC4Key[6] = ppk1 % 256;
-	pbyRC4Key[7] = (ppk1 >> 8) % 256;
-
-	pbyRC4Key[8] = ppk2 % 256;
-	pbyRC4Key[9] = (ppk2 >> 8) % 256;
-
-	pbyRC4Key[10] = ppk3 % 256;
-	pbyRC4Key[11] = (ppk3 >> 8) % 256;
-
-	pbyRC4Key[12] = ppk4 % 256;
-	pbyRC4Key[13] = (ppk4 >> 8) % 256;
-
-	pbyRC4Key[14] = ppk5 % 256;
-	pbyRC4Key[15] = (ppk5 >> 8) % 256;
-}
diff --git a/drivers/staging/vt6655/tkip.h b/drivers/staging/vt6655/tkip.h
deleted file mode 100644
index 3b6357ac6dee..000000000000
--- a/drivers/staging/vt6655/tkip.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: tkip.h
- *
- * Purpose: Implement functions for 802.11i TKIP
- *
- * Author: Jerry Chen
- *
- * Date: Mar. 11, 2003
- *
- */
-
-#ifndef __TKIP_H__
-#define __TKIP_H__
-
-#include "ttype.h"
-#include "tether.h"
-
-/*---------------------  Export Definitions -------------------------*/
-#define TKIP_KEY_LEN        16
-
-/*---------------------  Export Types  ------------------------------*/
-
-/*---------------------  Export Macros ------------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-void TKIPvMixKey(
-	unsigned char *pbyTKey,
-	unsigned char *pbyTA,
-	unsigned short wTSC15_0,
-	unsigned long dwTSC47_16,
-	unsigned char *pbyRC4Key
-);
-
-#endif // __TKIP_H__
diff --git a/drivers/staging/vt6655/tmacro.h b/drivers/staging/vt6655/tmacro.h
index 59c6e72f993a..607b78f7a6a0 100644
--- a/drivers/staging/vt6655/tmacro.h
+++ b/drivers/staging/vt6655/tmacro.h
@@ -29,8 +29,6 @@
 #ifndef __TMACRO_H__
 #define __TMACRO_H__
 
-#include "ttype.h"
-
 /****** Common helper macros ***********************************************/
 
 #if !defined(LOBYTE)
diff --git a/drivers/staging/vt6655/ttype.h b/drivers/staging/vt6655/ttype.h
deleted file mode 100644
index 747ef62ec9be..000000000000
--- a/drivers/staging/vt6655/ttype.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: ttype.h
- *
- * Purpose: define basic common types and macros
- *
- * Author: Tevin Chen
- *
- * Date: May 21, 1996
- *
- */
-
-#ifndef __TTYPE_H__
-#define __TTYPE_H__
-
-/******* Common definitions and typedefs ***********************************/
-
-#ifndef WPA_SM_Transtatus
-#define WPA_SM_Transtatus
-#endif
-
-#ifndef Calcu_LinkQual
-#define Calcu_LinkQual
-#endif
-
-#endif // __TTYPE_H__
diff --git a/drivers/staging/vt6655/upc.h b/drivers/staging/vt6655/upc.h
index c5c889cade25..c53703a772f5 100644
--- a/drivers/staging/vt6655/upc.h
+++ b/drivers/staging/vt6655/upc.h
@@ -30,7 +30,6 @@
 #define __UPC_H__
 
 #include "device.h"
-#include "ttype.h"
 
 /*---------------------  Export Definitions -------------------------*/
 
diff --git a/drivers/staging/vt6655/vntconfiguration.dat b/drivers/staging/vt6655/vntconfiguration.dat
deleted file mode 100644
index 0064ddce7c11..000000000000
--- a/drivers/staging/vt6655/vntconfiguration.dat
+++ /dev/null
@@ -1 +0,0 @@
-ZONETYPE=EUROPE
\ No newline at end of file
diff --git a/drivers/staging/vt6655/vntwifi.c b/drivers/staging/vt6655/vntwifi.c
deleted file mode 100644
index 59f66fe47352..000000000000
--- a/drivers/staging/vt6655/vntwifi.c
+++ /dev/null
@@ -1,700 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: vntwifi.c
- *
- * Purpose: export functions for vntwifi lib
- *
- * Functions:
- *
- * Revision History:
- *
- * Author: Yiching Chen
- *
- * Date: feb. 2, 2005
- *
- */
-
-#include "vntwifi.h"
-#include "IEEE11h.h"
-#include "country.h"
-#include "device.h"
-#include "wmgr.h"
-#include "datarate.h"
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-/*+
- *
- * Description:
- *    Set Operation Mode
- *
- * Parameters:
- *  In:
- *      pMgmtHandle - pointer to management object
- *      eOPMode     - Operation Mode
- *  Out:
- *      none
- *
- * Return Value: none
- *
- -*/
-void
-VNTWIFIvSetOPMode(
-	void *pMgmtHandle,
-	WMAC_CONFIG_MODE eOPMode
-)
-{
-	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
-
-	pMgmt->eConfigMode = eOPMode;
-}
-
-/*+
- *
- * Description:
- *    Set Operation Mode
- *
- * Parameters:
- *  In:
- *      pMgmtHandle - pointer to management object
- *      wBeaconPeriod - Beacon Period
- *      wATIMWindow - ATIM window
- *      uChannel - channel number
- *  Out:
- *      none
- *
- * Return Value: none
- *
- -*/
-void
-VNTWIFIvSetIBSSParameter(
-	void *pMgmtHandle,
-	unsigned short wBeaconPeriod,
-	unsigned short wATIMWindow,
-	unsigned int uChannel
-)
-{
-	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
-
-	pMgmt->wIBSSBeaconPeriod = wBeaconPeriod;
-	pMgmt->wIBSSATIMWindow = wATIMWindow;
-	pMgmt->uIBSSChannel = uChannel;
-}
-
-/*+
- *
- * Description:
- *    Get current SSID
- *
- * Parameters:
- *  In:
- *      pMgmtHandle - pointer to management object
- *  Out:
- *      none
- *
- * Return Value: current SSID pointer.
- *
- -*/
-PWLAN_IE_SSID
-VNTWIFIpGetCurrentSSID(
-	void *pMgmtHandle
-)
-{
-	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
-
-	return (PWLAN_IE_SSID) pMgmt->abyCurrSSID;
-}
-
-/*+
- *
- * Description:
- *    Get current link channel
- *
- * Parameters:
- *  In:
- *      pMgmtHandle - pointer to management object
- *  Out:
- *      none
- *
- * Return Value: current Channel.
- *
- -*/
-unsigned int
-VNTWIFIpGetCurrentChannel(
-	void *pMgmtHandle
-)
-{
-	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
-
-	if (pMgmtHandle != NULL)
-		return pMgmt->uCurrChannel;
-
-	return 0;
-}
-
-/*+
- *
- * Description:
- *    Get current Assoc ID
- *
- * Parameters:
- *  In:
- *      pMgmtHandle - pointer to management object
- *  Out:
- *      none
- *
- * Return Value: current Assoc ID
- *
- -*/
-unsigned short
-VNTWIFIwGetAssocID(
-	void *pMgmtHandle
-)
-{
-	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
-
-	return pMgmt->wCurrAID;
-}
-
-/*+
- *
- * Description:
- *    This routine return max support rate of IES
- *
- * Parameters:
- *  In:
- *      pSupportRateIEs
- *      pExtSupportRateIEs
- *
- *  Out:
- *
- * Return Value: max support rate
- *
- -*/
-unsigned char
-VNTWIFIbyGetMaxSupportRate(
-	PWLAN_IE_SUPP_RATES pSupportRateIEs,
-	PWLAN_IE_SUPP_RATES pExtSupportRateIEs
-)
-{
-	unsigned char byMaxSupportRate = RATE_1M;
-	unsigned char bySupportRate = RATE_1M;
-	unsigned int ii = 0;
-
-	if (pSupportRateIEs) {
-		for (ii = 0; ii < pSupportRateIEs->len; ii++) {
-			bySupportRate = DATARATEbyGetRateIdx(pSupportRateIEs->abyRates[ii]);
-			if (bySupportRate > byMaxSupportRate)
-				byMaxSupportRate = bySupportRate;
-
-		}
-	}
-	if (pExtSupportRateIEs) {
-		for (ii = 0; ii < pExtSupportRateIEs->len; ii++) {
-			bySupportRate = DATARATEbyGetRateIdx(pExtSupportRateIEs->abyRates[ii]);
-			if (bySupportRate > byMaxSupportRate)
-				byMaxSupportRate = bySupportRate;
-
-		}
-	}
-
-	return byMaxSupportRate;
-}
-
-/*+
- *
- * Description:
- *    This routine return data rate of ACK packtet
- *
- * Parameters:
- *  In:
- *      byRxDataRate
- *      pSupportRateIEs
- *      pExtSupportRateIEs
- *
- *  Out:
- *
- * Return Value: max support rate
- *
- -*/
-unsigned char
-VNTWIFIbyGetACKTxRate(
-	unsigned char byRxDataRate,
-	PWLAN_IE_SUPP_RATES pSupportRateIEs,
-	PWLAN_IE_SUPP_RATES pExtSupportRateIEs
-)
-{
-	unsigned char byMaxAckRate;
-	unsigned char byBasicRate;
-	unsigned int ii;
-
-	if (byRxDataRate <= RATE_11M) {
-		byMaxAckRate = RATE_1M;
-	} else  {
-		/* 24M is mandatory for 802.11a and 802.11g */
-		byMaxAckRate = RATE_24M;
-	}
-	if (pSupportRateIEs) {
-		for (ii = 0; ii < pSupportRateIEs->len; ii++) {
-			if (pSupportRateIEs->abyRates[ii] & 0x80) {
-				byBasicRate = DATARATEbyGetRateIdx(pSupportRateIEs->abyRates[ii]);
-				if ((byBasicRate <= byRxDataRate) &&
-				    (byBasicRate > byMaxAckRate))  {
-					byMaxAckRate = byBasicRate;
-				}
-			}
-		}
-	}
-	if (pExtSupportRateIEs) {
-		for (ii = 0; ii < pExtSupportRateIEs->len; ii++) {
-			if (pExtSupportRateIEs->abyRates[ii] & 0x80) {
-				byBasicRate = DATARATEbyGetRateIdx(pExtSupportRateIEs->abyRates[ii]);
-				if ((byBasicRate <= byRxDataRate) &&
-				    (byBasicRate > byMaxAckRate))  {
-					byMaxAckRate = byBasicRate;
-				}
-			}
-		}
-	}
-
-	return byMaxAckRate;
-}
-
-/*+
- *
- * Description:
- *    Set Authentication Mode
- *
- * Parameters:
- *  In:
- *      pMgmtHandle - pointer to management object
- *      eAuthMode   - Authentication mode
- *  Out:
- *      none
- *
- * Return Value: none
- *
- -*/
-void
-VNTWIFIvSetAuthenticationMode(
-	void *pMgmtHandle,
-	WMAC_AUTHENTICATION_MODE eAuthMode
-)
-{
-	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
-
-	pMgmt->eAuthenMode = eAuthMode;
-	if ((eAuthMode == WMAC_AUTH_SHAREKEY) ||
-	    (eAuthMode == WMAC_AUTH_AUTO)) {
-		pMgmt->bShareKeyAlgorithm = true;
-	} else {
-		pMgmt->bShareKeyAlgorithm = false;
-	}
-}
-
-/*+
- *
- * Description:
- *    Set Encryption Mode
- *
- * Parameters:
- *  In:
- *      pMgmtHandle - pointer to management object
- *      eAuthMode   - Authentication mode
- *  Out:
- *      none
- *
- * Return Value: none
- *
- -*/
-void
-VNTWIFIvSetEncryptionMode(
-	void *pMgmtHandle,
-	WMAC_ENCRYPTION_MODE eEncryptionMode
-)
-{
-	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
-
-	pMgmt->eEncryptionMode = eEncryptionMode;
-	if ((eEncryptionMode == WMAC_ENCRYPTION_WEPEnabled) ||
-	    (eEncryptionMode == WMAC_ENCRYPTION_TKIPEnabled) ||
-	    (eEncryptionMode == WMAC_ENCRYPTION_AESEnabled)) {
-		pMgmt->bPrivacyInvoked = true;
-	} else {
-		pMgmt->bPrivacyInvoked = false;
-	}
-}
-
-bool
-VNTWIFIbConfigPhyMode(
-	void *pMgmtHandle,
-	CARD_PHY_TYPE ePhyType
-)
-{
-	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
-
-	if ((ePhyType != PHY_TYPE_AUTO) &&
-	    (ePhyType != pMgmt->eCurrentPHYMode)) {
-		if (CARDbSetPhyParameter(pMgmt->pAdapter, ePhyType, 0, 0, NULL, NULL) == true)
-			pMgmt->eCurrentPHYMode = ePhyType;
-		else
-			return false;
-	}
-	pMgmt->eConfigPHYMode = ePhyType;
-	return true;
-}
-
-void
-VNTWIFIbGetConfigPhyMode(
-	void *pMgmtHandle,
-	void *pePhyType
-)
-{
-	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
-
-	if ((pMgmt != NULL) && (pePhyType != NULL))
-		*(PCARD_PHY_TYPE)pePhyType = pMgmt->eConfigPHYMode;
-}
-
-/*+
- *
- * Description:
- *      Clear BSS List Database except current assoc BSS
- *
- * Parameters:
- *  In:
- *      pMgmtHandle     - Management Object structure
- *      bLinkPass       - Current Link status
- *  Out:
- *
- * Return Value: None.
- *
- -*/
-
-/*+
- *
- * Description:
- *      Query BSS List in management database
- *
- * Parameters:
- *  In:
- *      pMgmtHandle     - Management Object structure
- *  Out:
- *      puBSSCount      - BSS count
- *      pvFirstBSS      - pointer to first BSS
- *
- * Return Value: None.
- *
- -*/
-
-void
-VNTWIFIvQueryBSSList(void *pMgmtHandle, unsigned int *puBSSCount, void **pvFirstBSS)
-{
-	unsigned int ii = 0;
-	PSMgmtObject    pMgmt = (PSMgmtObject)pMgmtHandle;
-	PKnownBSS       pBSS = NULL;
-	unsigned int uCount = 0;
-
-	*pvFirstBSS = NULL;
-
-	for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-		pBSS = &(pMgmt->sBSSList[ii]);
-		if (!pBSS->bActive)
-			continue;
-
-		if (*pvFirstBSS == NULL)
-			*pvFirstBSS = &(pMgmt->sBSSList[ii]);
-
-		uCount++;
-	}
-	*puBSSCount = uCount;
-}
-
-void
-VNTWIFIvGetNextBSS(
-	void *pMgmtHandle,
-	void *pvCurrentBSS,
-	void **pvNextBSS
-)
-{
-	PKnownBSS       pBSS = (PKnownBSS) pvCurrentBSS;
-	PSMgmtObject    pMgmt = (PSMgmtObject)pMgmtHandle;
-
-	*pvNextBSS = NULL;
-
-	while (*pvNextBSS == NULL) {
-		pBSS++;
-		if (pBSS > &(pMgmt->sBSSList[MAX_BSS_NUM]))
-			return;
-
-		if (pBSS->bActive == true) {
-			*pvNextBSS = pBSS;
-			return;
-		}
-	}
-}
-
-/*+
- *
- * Description:
- *      Update Tx attemps, Tx failure counter in Node DB
- *
- *  In:
- *  Out:
- *      none
- *
- * Return Value: none
- *
- -*/
-void
-VNTWIFIvUpdateNodeTxCounter(
-	void *pMgmtHandle,
-	unsigned char *pbyDestAddress,
-	bool bTxOk,
-	unsigned short wRate,
-	unsigned char *pbyTxFailCount
-)
-{
-	PSMgmtObject    pMgmt = (PSMgmtObject)pMgmtHandle;
-	unsigned int uNodeIndex = 0;
-	unsigned int ii;
-
-	if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) ||
-	    (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)) {
-		if (BSSDBbIsSTAInNodeDB(pMgmt, pbyDestAddress, &uNodeIndex) == false)
-			return;
-	}
-
-	pMgmt->sNodeDBTable[uNodeIndex].uTxAttempts++;
-	if (bTxOk) {
-		/* transmit success, TxAttempts at least plus one */
-		pMgmt->sNodeDBTable[uNodeIndex].uTxOk[MAX_RATE]++;
-		pMgmt->sNodeDBTable[uNodeIndex].uTxOk[wRate]++;
-	} else {
-		pMgmt->sNodeDBTable[uNodeIndex].uTxFailures++;
-	}
-	pMgmt->sNodeDBTable[uNodeIndex].uTxRetry += pbyTxFailCount[MAX_RATE];
-	for (ii = 0; ii < MAX_RATE; ii++)
-		pMgmt->sNodeDBTable[uNodeIndex].uTxFail[ii] += pbyTxFailCount[ii];
-}
-
-void
-VNTWIFIvGetTxRate(
-	void *pMgmtHandle,
-	unsigned char *pbyDestAddress,
-	unsigned short *pwTxDataRate,
-	unsigned char *pbyACKRate,
-	unsigned char *pbyCCKBasicRate,
-	unsigned char *pbyOFDMBasicRate
-)
-{
-	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
-	unsigned int uNodeIndex = 0;
-	unsigned short wTxDataRate = RATE_1M;
-	unsigned char byACKRate = RATE_1M;
-	unsigned char byCCKBasicRate = RATE_1M;
-	unsigned char byOFDMBasicRate = RATE_24M;
-	PWLAN_IE_SUPP_RATES pSupportRateIEs = NULL;
-	PWLAN_IE_SUPP_RATES pExtSupportRateIEs = NULL;
-
-	if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) ||
-	    (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)) {
-		/* Adhoc Tx rate decided from node DB */
-		if (BSSDBbIsSTAInNodeDB(pMgmt, pbyDestAddress, &uNodeIndex)) {
-			wTxDataRate = (pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate);
-			pSupportRateIEs = (PWLAN_IE_SUPP_RATES) (pMgmt->sNodeDBTable[uNodeIndex].abyCurrSuppRates);
-			pExtSupportRateIEs = (PWLAN_IE_SUPP_RATES) (pMgmt->sNodeDBTable[uNodeIndex].abyCurrExtSuppRates);
-		} else {
-			if (pMgmt->eCurrentPHYMode != PHY_TYPE_11A)
-				wTxDataRate = RATE_2M;
-			else
-				wTxDataRate = RATE_24M;
-
-			pSupportRateIEs = (PWLAN_IE_SUPP_RATES) pMgmt->abyCurrSuppRates;
-			pExtSupportRateIEs = (PWLAN_IE_SUPP_RATES) pMgmt->abyCurrExtSuppRates;
-		}
-	} else { /* Infrastructure: rate decided from AP Node, index = 0 */
-
-		wTxDataRate = (pMgmt->sNodeDBTable[0].wTxDataRate);
-
-		pSupportRateIEs = (PWLAN_IE_SUPP_RATES) pMgmt->abyCurrSuppRates;
-		pExtSupportRateIEs = (PWLAN_IE_SUPP_RATES) pMgmt->abyCurrExtSuppRates;
-	}
-	byACKRate = VNTWIFIbyGetACKTxRate((unsigned char) wTxDataRate,
-					    pSupportRateIEs,
-					    pExtSupportRateIEs
-);
-	if (byACKRate > (unsigned char) wTxDataRate)
-		byACKRate = (unsigned char) wTxDataRate;
-
-	byCCKBasicRate = VNTWIFIbyGetACKTxRate(RATE_11M,
-						pSupportRateIEs,
-						pExtSupportRateIEs
-);
-	byOFDMBasicRate = VNTWIFIbyGetACKTxRate(RATE_54M,
-						pSupportRateIEs,
-						pExtSupportRateIEs
-);
-	*pwTxDataRate = wTxDataRate;
-	*pbyACKRate = byACKRate;
-	*pbyCCKBasicRate = byCCKBasicRate;
-	*pbyOFDMBasicRate = byOFDMBasicRate;
-}
-
-unsigned char
-VNTWIFIbyGetKeyCypher(
-	void *pMgmtHandle,
-	bool bGroupKey
-)
-{
-	PSMgmtObject    pMgmt = (PSMgmtObject)pMgmtHandle;
-
-	if (bGroupKey)
-		return pMgmt->byCSSGK;
-	else
-		return pMgmt->byCSSPK;
-}
-
-bool
-VNTWIFIbSetPMKIDCache(
-	void *pMgmtObject,
-	unsigned long ulCount,
-	void *pPMKIDInfo
-)
-{
-	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
-
-	if (ulCount > MAX_PMKID_CACHE)
-		return false;
-
-	pMgmt->gsPMKIDCache.BSSIDInfoCount = ulCount;
-	memcpy(pMgmt->gsPMKIDCache.BSSIDInfo, pPMKIDInfo, (ulCount*sizeof(PMKIDInfo)));
-	return true;
-}
-
-unsigned short
-VNTWIFIwGetMaxSupportRate(
-	void *pMgmtObject
-)
-{
-	unsigned short wRate = RATE_54M;
-	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
-
-	for (wRate = RATE_54M; wRate > RATE_1M; wRate--) {
-		if (pMgmt->sNodeDBTable[0].wSuppRate & (1<<wRate))
-			return wRate;
-	}
-
-	if (pMgmt->eCurrentPHYMode == PHY_TYPE_11A)
-		return RATE_6M;
-	else
-		return RATE_1M;
-}
-
-void
-VNTWIFIvSet11h(
-	void *pMgmtObject,
-	bool b11hEnable
-)
-{
-	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
-
-	pMgmt->b11hEnable = b11hEnable;
-}
-
-bool
-VNTWIFIbMeasureReport(
-	void *pMgmtObject,
-	bool bEndOfReport,
-	void *pvMeasureEID,
-	unsigned char byReportMode,
-	unsigned char byBasicMap,
-	unsigned char byCCAFraction,
-	unsigned char *pbyRPIs
-)
-{
-	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
-	unsigned char *pbyCurrentEID = (unsigned char *)(pMgmt->pCurrMeasureEIDRep);
-
-	if ((pvMeasureEID != NULL) &&
-	    (pMgmt->uLengthOfRepEIDs < (WLAN_A3FR_MAXLEN - sizeof(MEASEURE_REP) - sizeof(WLAN_80211HDR_A3) - 3))
-) {
-		pMgmt->pCurrMeasureEIDRep->byElementID = WLAN_EID_MEASURE_REP;
-		pMgmt->pCurrMeasureEIDRep->len = 3;
-		pMgmt->pCurrMeasureEIDRep->byToken = ((PWLAN_IE_MEASURE_REQ)pvMeasureEID)->byToken;
-		pMgmt->pCurrMeasureEIDRep->byMode = byReportMode;
-		pMgmt->pCurrMeasureEIDRep->byType = ((PWLAN_IE_MEASURE_REQ) pvMeasureEID)->byType;
-		switch (pMgmt->pCurrMeasureEIDRep->byType) {
-		case MEASURE_TYPE_BASIC:
-			pMgmt->pCurrMeasureEIDRep->len += sizeof(MEASEURE_REP_BASIC);
-			memcpy(&(pMgmt->pCurrMeasureEIDRep->sRep.sBasic),
-			       &(((PWLAN_IE_MEASURE_REQ) pvMeasureEID)->sReq),
-			       sizeof(MEASEURE_REQ));
-			pMgmt->pCurrMeasureEIDRep->sRep.sBasic.byMap = byBasicMap;
-			break;
-		case MEASURE_TYPE_CCA:
-			pMgmt->pCurrMeasureEIDRep->len += sizeof(MEASEURE_REP_CCA);
-			memcpy(&(pMgmt->pCurrMeasureEIDRep->sRep.sCCA),
-			       &(((PWLAN_IE_MEASURE_REQ) pvMeasureEID)->sReq),
-			       sizeof(MEASEURE_REQ));
-			pMgmt->pCurrMeasureEIDRep->sRep.sCCA.byCCABusyFraction = byCCAFraction;
-			break;
-		case MEASURE_TYPE_RPI:
-			pMgmt->pCurrMeasureEIDRep->len += sizeof(MEASEURE_REP_RPI);
-			memcpy(&(pMgmt->pCurrMeasureEIDRep->sRep.sRPI),
-			       &(((PWLAN_IE_MEASURE_REQ) pvMeasureEID)->sReq),
-			       sizeof(MEASEURE_REQ));
-			memcpy(pMgmt->pCurrMeasureEIDRep->sRep.sRPI.abyRPIdensity, pbyRPIs, 8);
-			break;
-		default:
-			break;
-		}
-		pbyCurrentEID += (2 + pMgmt->pCurrMeasureEIDRep->len);
-		pMgmt->uLengthOfRepEIDs += (2 + pMgmt->pCurrMeasureEIDRep->len);
-		pMgmt->pCurrMeasureEIDRep = (PWLAN_IE_MEASURE_REP) pbyCurrentEID;
-	}
-	if (bEndOfReport)
-		IEEE11hbMSRRepTx(pMgmt);
-
-	return true;
-}
-
-bool
-VNTWIFIbChannelSwitch(
-	void *pMgmtObject,
-	unsigned char byNewChannel
-)
-{
-	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
-
-	pMgmt->uCurrChannel = byNewChannel;
-	pMgmt->bSwitchChannel = false;
-	return true;
-}
diff --git a/drivers/staging/vt6655/vntwifi.h b/drivers/staging/vt6655/vntwifi.h
deleted file mode 100644
index 880b8ab109be..000000000000
--- a/drivers/staging/vt6655/vntwifi.h
+++ /dev/null
@@ -1,273 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: vntwifi.h
- *
- * Purpose: export VNT Host WiFi library function
- *
- * Author: Yiching Chen
- *
- * Date: Jan 7, 2004
- *
- */
-
-#ifndef __VNTWIFI_H__
-#define __VNTWIFI_H__
-
-#include "ttype.h"
-#include "80211mgr.h"
-#include "card.h"
-#include "wpa2.h"
-
-/*---------------------  Export Definitions -------------------------*/
-#define RATE_1M         0
-#define RATE_2M         1
-#define RATE_5M         2
-#define RATE_11M        3
-#define RATE_6M         4
-#define RATE_9M         5
-#define RATE_12M        6
-#define RATE_18M        7
-#define RATE_24M        8
-#define RATE_36M        9
-#define RATE_48M       10
-#define RATE_54M       11
-#define RATE_AUTO      12
-#define MAX_RATE       12
-
-// key CipherSuite
-#define KEY_CTL_WEP         0x00
-#define KEY_CTL_NONE        0x01
-#define KEY_CTL_TKIP        0x02
-#define KEY_CTL_CCMP        0x03
-#define KEY_CTL_INVALID     0xFF
-
-#define CHANNEL_MAX_24G         14
-
-#define MAX_BSS_NUM             42
-
-// Pre-configured Authenticaiton Mode (from XP)
-typedef enum tagWMAC_AUTHENTICATION_MODE {
-	WMAC_AUTH_OPEN,
-	WMAC_AUTH_SHAREKEY,
-	WMAC_AUTH_AUTO,
-	WMAC_AUTH_WPA,
-	WMAC_AUTH_WPAPSK,
-	WMAC_AUTH_WPANONE,
-	WMAC_AUTH_WPA2,
-	WMAC_AUTH_WPA2PSK,
-	WMAC_AUTH_MAX       // Not a real mode, defined as upper bound
-} WMAC_AUTHENTICATION_MODE, *PWMAC_AUTHENTICATION_MODE;
-
-typedef enum tagWMAC_ENCRYPTION_MODE {
-	WMAC_ENCRYPTION_WEPEnabled,
-	WMAC_ENCRYPTION_WEPDisabled,
-	WMAC_ENCRYPTION_WEPKeyAbsent,
-	WMAC_ENCRYPTION_WEPNotSupported,
-	WMAC_ENCRYPTION_TKIPEnabled,
-	WMAC_ENCRYPTION_TKIPKeyAbsent,
-	WMAC_ENCRYPTION_AESEnabled,
-	WMAC_ENCRYPTION_AESKeyAbsent
-} WMAC_ENCRYPTION_MODE, *PWMAC_ENCRYPTION_MODE;
-
-// Pre-configured Mode (from XP)
-
-typedef enum tagWMAC_CONFIG_MODE {
-	WMAC_CONFIG_ESS_STA = 0,
-	WMAC_CONFIG_IBSS_STA,
-	WMAC_CONFIG_AUTO,
-	WMAC_CONFIG_AP
-} WMAC_CONFIG_MODE, *PWMAC_CONFIG_MODE;
-
-typedef enum tagWMAC_POWER_MODE {
-	WMAC_POWER_CAM,
-	WMAC_POWER_FAST,
-	WMAC_POWER_MAX
-} WMAC_POWER_MODE, *PWMAC_POWER_MODE;
-
-#define VNTWIFIbIsShortSlotTime(wCapInfo)               \
-	WLAN_GET_CAP_INFO_SHORTSLOTTIME(wCapInfo)       \
-
-#define VNTWIFIbIsProtectMode(byERP)                    \
-	((byERP & WLAN_EID_ERP_USE_PROTECTION) != 0)    \
-
-#define VNTWIFIbIsBarkerMode(byERP)                     \
-	((byERP & WLAN_EID_ERP_BARKER_MODE) != 0)       \
-
-#define VNTWIFIbIsShortPreamble(wCapInfo)               \
-	WLAN_GET_CAP_INFO_SHORTPREAMBLE(wCapInfo)       \
-
-#define VNTWIFIbIsEncryption(wCapInfo)		\
-	WLAN_GET_CAP_INFO_PRIVACY(wCapInfo)	\
-
-#define VNTWIFIbIsESS(wCapInfo)			\
-	WLAN_GET_CAP_INFO_ESS(wCapInfo)		\
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Types  ------------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-void
-VNTWIFIvSetIBSSParameter(
-	void *pMgmtHandle,
-	unsigned short wBeaconPeriod,
-	unsigned short wATIMWindow,
-	unsigned int uChannel
-);
-
-void
-VNTWIFIvSetOPMode(
-	void *pMgmtHandle,
-	WMAC_CONFIG_MODE eOPMode
-);
-
-PWLAN_IE_SSID
-VNTWIFIpGetCurrentSSID(
-	void *pMgmtHandle
-);
-
-unsigned int
-VNTWIFIpGetCurrentChannel(
-	void *pMgmtHandle
-);
-
-unsigned short
-VNTWIFIwGetAssocID(
-	void *pMgmtHandle
-);
-
-unsigned char
-VNTWIFIbyGetMaxSupportRate(
-	PWLAN_IE_SUPP_RATES pSupportRateIEs,
-	PWLAN_IE_SUPP_RATES pExtSupportRateIEs
-);
-
-unsigned char
-VNTWIFIbyGetACKTxRate(
-	unsigned char byRxDataRate,
-	PWLAN_IE_SUPP_RATES pSupportRateIEs,
-	PWLAN_IE_SUPP_RATES pExtSupportRateIEs
-);
-
-void
-VNTWIFIvSetAuthenticationMode(
-	void *pMgmtHandle,
-	WMAC_AUTHENTICATION_MODE eAuthMode
-);
-
-void
-VNTWIFIvSetEncryptionMode(
-	void *pMgmtHandle,
-	WMAC_ENCRYPTION_MODE eEncryptionMode
-);
-
-bool
-VNTWIFIbConfigPhyMode(
-	void *pMgmtHandle,
-	CARD_PHY_TYPE ePhyType
-);
-
-void
-VNTWIFIbGetConfigPhyMode(
-	void *pMgmtHandle,
-	void *pePhyType
-);
-
-void
-VNTWIFIvQueryBSSList(void *pMgmtHandle, unsigned int *puBSSCount,
-		     void **pvFirstBSS);
-
-void
-VNTWIFIvGetNextBSS(
-	void *pMgmtHandle,
-	void *pvCurrentBSS,
-	void **pvNextBSS
-);
-
-void
-VNTWIFIvUpdateNodeTxCounter(
-	void *pMgmtHandle,
-	unsigned char *pbyDestAddress,
-	bool bTxOk,
-	unsigned short wRate,
-	unsigned char *pbyTxFailCount
-);
-
-void
-VNTWIFIvGetTxRate(
-	void *pMgmtHandle,
-	unsigned char *pbyDestAddress,
-	unsigned short *pwTxDataRate,
-	unsigned char *pbyACKRate,
-	unsigned char *pbyCCKBasicRate,
-	unsigned char *pbyOFDMBasicRate
-);
-
-unsigned char
-VNTWIFIbyGetKeyCypher(
-	void *pMgmtHandle,
-	bool bGroupKey
-);
-
-bool
-VNTWIFIbSetPMKIDCache(
-	void *pMgmtObject,
-	unsigned long ulCount,
-	void *pPMKIDInfo
-);
-
-bool
-VNTWIFIbCommandRunning(
-	void *pMgmtObject
-);
-
-unsigned short
-VNTWIFIwGetMaxSupportRate(
-	void *pMgmtObject
-);
-
-// for 802.11h
-void
-VNTWIFIvSet11h(
-	void *pMgmtObject,
-	bool b11hEnable
-);
-
-bool
-VNTWIFIbMeasureReport(
-	void *pMgmtObject,
-	bool bEndOfReport,
-	void *pvMeasureEID,
-	unsigned char byReportMode,
-	unsigned char byBasicMap,
-	unsigned char byCCAFraction,
-	unsigned char *pbyRPIs
-);
-
-bool
-VNTWIFIbChannelSwitch(
-	void *pMgmtObject,
-	unsigned char byNewChannel
-);
-
-#endif //__VNTWIFI_H__
diff --git a/drivers/staging/vt6655/wcmd.c b/drivers/staging/vt6655/wcmd.c
deleted file mode 100644
index 985e1b99362d..000000000000
--- a/drivers/staging/vt6655/wcmd.c
+++ /dev/null
@@ -1,1023 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: wcmd.c
- *
- * Purpose: Handles the management command interface functions
- *
- * Author: Lyndon Chen
- *
- * Date: May 8, 2003
- *
- * Functions:
- *      s_vProbeChannel - Active scan channel
- *      s_MgrMakeProbeRequest - Make ProbeRequest packet
- *      CommandTimer - Timer function to handle command
- *      s_bCommandComplete - Command Complete function
- *      bScheduleCommand - Push Command and wait Command Scheduler to do
- *      vCommandTimer- Command call back functions
- *      vCommandTimerWait- Call back timer
- *      bClearBSSID_SCAN- Clear BSSID_SCAN cmd in CMD Queue
- *
- * Revision History:
- *
- */
-
-#include "ttype.h"
-#include "tmacro.h"
-#include "device.h"
-#include "mac.h"
-#include "card.h"
-#include "80211hdr.h"
-#include "wcmd.h"
-#include "wmgr.h"
-#include "power.h"
-#include "wctl.h"
-#include "baseband.h"
-#include "rxtx.h"
-#include "rf.h"
-#include "iowpa.h"
-#include "channel.h"
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-static
-void
-s_vProbeChannel(
-	struct vnt_private *pDevice
-);
-
-static
-PSTxMgmtPacket
-s_MgrMakeProbeRequest(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	unsigned char *pScanBSSID,
-	PWLAN_IE_SSID pSSID,
-	PWLAN_IE_SUPP_RATES pCurrRates,
-	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-);
-
-static
-bool
-s_bCommandComplete(
-	struct vnt_private *pDevice
-);
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-/*
- * Description:
- *      Stop AdHoc beacon during scan process
- *
- * Parameters:
- *  In:
- *      pDevice     - Pointer to the adapter
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-static
-void
-vAdHocBeaconStop(struct vnt_private *pDevice)
-{
-	PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
-	bool bStop;
-
-	/*
-	 * temporarily stop Beacon packet for AdHoc Server
-	 * if all of the following conditions are met:
-	 *  (1) STA is in AdHoc mode
-	 *  (2) VT3253 is programmed as automatic Beacon Transmitting
-	 *  (3) One of the following conditions is met
-	 *      (3.1) AdHoc channel is in B/G band and the
-	 *      current scan channel is in A band
-	 *      or
-	 *      (3.2) AdHoc channel is in A mode
-	 */
-	bStop = false;
-	if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) &&
-	    (pMgmt->eCurrState >= WMAC_STATE_STARTED)) {
-		if ((pMgmt->uIBSSChannel <=  CB_MAX_CHANNEL_24G) &&
-		    (pMgmt->uScanChannel > CB_MAX_CHANNEL_24G)) {
-			bStop = true;
-		}
-		if (pMgmt->uIBSSChannel >  CB_MAX_CHANNEL_24G)
-			bStop = true;
-
-	}
-
-	if (bStop)
-		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
-} /* vAdHocBeaconStop */
-
-/*
- * Description:
- *      Restart AdHoc beacon after scan process complete
- *
- * Parameters:
- *  In:
- *      pDevice     - Pointer to the adapter
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-static
-void
-vAdHocBeaconRestart(struct vnt_private *pDevice)
-{
-	PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
-
-	/*
-	 * Restart Beacon packet for AdHoc Server
-	 * if all of the following coditions are met:
-	 *  (1) STA is in AdHoc mode
-	 *  (2) VT3253 is programmed as automatic Beacon Transmitting
-	 */
-	if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) &&
-	    (pMgmt->eCurrState >= WMAC_STATE_STARTED)) {
-		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
-	}
-}
-
-/*+
- *
- * Routine Description:
- *   Prepare and send probe request management frames.
- *
- *
- * Return Value:
- *    none.
- *
- -*/
-
-static
-void
-s_vProbeChannel(
-	struct vnt_private *pDevice
-)
-{
-	//1M,   2M,   5M,   11M,  18M,  24M,  36M,  54M
-	unsigned char abyCurrSuppRatesG[] = {WLAN_EID_SUPP_RATES, 8, 0x02, 0x04, 0x0B, 0x16, 0x24, 0x30, 0x48, 0x6C};
-	unsigned char abyCurrExtSuppRatesG[] = {WLAN_EID_EXTSUPP_RATES, 4, 0x0C, 0x12, 0x18, 0x60};
-	//6M,   9M,   12M,  48M
-	unsigned char abyCurrSuppRatesA[] = {WLAN_EID_SUPP_RATES, 8, 0x0C, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C};
-	unsigned char abyCurrSuppRatesB[] = {WLAN_EID_SUPP_RATES, 4, 0x02, 0x04, 0x0B, 0x16};
-	unsigned char *pbyRate;
-	PSTxMgmtPacket  pTxPacket;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	unsigned int ii;
-
-	if (pDevice->eCurrentPHYType == PHY_TYPE_11A)
-		pbyRate = &abyCurrSuppRatesA[0];
-	else if (pDevice->eCurrentPHYType == PHY_TYPE_11B)
-		pbyRate = &abyCurrSuppRatesB[0];
-	else
-		pbyRate = &abyCurrSuppRatesG[0];
-
-	// build an assocreq frame and send it
-	pTxPacket = s_MgrMakeProbeRequest
-		(
-			pDevice,
-			pMgmt,
-			pMgmt->abyScanBSSID,
-			(PWLAN_IE_SSID)pMgmt->abyScanSSID,
-			(PWLAN_IE_SUPP_RATES)pbyRate,
-			(PWLAN_IE_SUPP_RATES)abyCurrExtSuppRatesG
-			);
-
-	if (pTxPacket != NULL) {
-		for (ii = 0; ii < 2; ii++) {
-			if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING)
-				pr_debug("Probe request sending fail..\n");
-			else
-				pr_debug("Probe request is sending..\n");
-		}
-	}
-}
-
-/*+
- *
- * Routine Description:
- *  Constructs an probe request frame
- *
- *
- * Return Value:
- *    A ptr to Tx frame or NULL on allocation failure
- *
- -*/
-
-static PSTxMgmtPacket
-s_MgrMakeProbeRequest(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	unsigned char *pScanBSSID,
-	PWLAN_IE_SSID pSSID,
-	PWLAN_IE_SUPP_RATES pCurrRates,
-	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-
-)
-{
-	PSTxMgmtPacket      pTxPacket = NULL;
-	WLAN_FR_PROBEREQ    sFrame;
-
-	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_PROBEREQ_FR_MAXLEN);
-	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-	sFrame.len = WLAN_PROBEREQ_FR_MAXLEN;
-	vMgrEncodeProbeRequest(&sFrame);
-	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-		(
-			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_PROBEREQ)
-));
-	memcpy(sFrame.pHdr->sA3.abyAddr1, pScanBSSID, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr3, pScanBSSID, WLAN_BSSID_LEN);
-	// Copy the SSID, pSSID->len=0 indicate broadcast SSID
-	sFrame.pSSID = (PWLAN_IE_SSID)(sFrame.pBuf + sFrame.len);
-	sFrame.len += pSSID->len + WLAN_IEHDR_LEN;
-	memcpy(sFrame.pSSID, pSSID, pSSID->len + WLAN_IEHDR_LEN);
-	sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-	sFrame.len += pCurrRates->len + WLAN_IEHDR_LEN;
-	memcpy(sFrame.pSuppRates, pCurrRates, pCurrRates->len + WLAN_IEHDR_LEN);
-	// Copy the extension rate set
-	if (pDevice->eCurrentPHYType == PHY_TYPE_11G) {
-		sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-		sFrame.len += pCurrExtSuppRates->len + WLAN_IEHDR_LEN;
-		memcpy(sFrame.pExtSuppRates, pCurrExtSuppRates, pCurrExtSuppRates->len + WLAN_IEHDR_LEN);
-	}
-	pTxPacket->cbMPDULen = sFrame.len;
-	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-
-	return pTxPacket;
-}
-
-void
-vCommandTimerWait(
-	void *hDeviceContext,
-	unsigned int MSecond
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-
-	init_timer(&pDevice->sTimerCommand);
-	pDevice->sTimerCommand.data = (unsigned long) pDevice;
-	pDevice->sTimerCommand.function = (TimerFunction)vCommandTimer;
-	// RUN_AT :1 msec ~= (HZ/1024)
-	pDevice->sTimerCommand.expires = (unsigned int)RUN_AT((MSecond * HZ) >> 10);
-	add_timer(&pDevice->sTimerCommand);
-}
-
-void
-vCommandTimer(
-	void *hDeviceContext
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	PWLAN_IE_SSID   pItemSSID;
-	PWLAN_IE_SSID   pItemSSIDCurr;
-	CMD_STATUS      Status;
-	unsigned int ii;
-	unsigned char byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
-	struct sk_buff  *skb;
-
-	if (pDevice->dwDiagRefCount != 0)
-		return;
-	if (!pDevice->bCmdRunning)
-		return;
-
-	spin_lock_irq(&pDevice->lock);
-
-	switch (pDevice->eCommandState) {
-	case WLAN_CMD_SCAN_START:
-
-		pDevice->byReAssocCount = 0;
-		if (pDevice->bRadioOff) {
-			s_bCommandComplete(pDevice);
-			spin_unlock_irq(&pDevice->lock);
-			return;
-		}
-
-		if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-			s_bCommandComplete(pDevice);
-			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, NL80211_IFTYPE_AP);
-			spin_unlock_irq(&pDevice->lock);
-			return;
-		}
-
-		pr_debug("eCommandState= WLAN_CMD_SCAN_START\n");
-		pItemSSID = (PWLAN_IE_SSID)pMgmt->abyScanSSID;
-		// wait all Data TD complete
-		if (pDevice->iTDUsed[TYPE_AC0DMA] != 0) {
-			spin_unlock_irq(&pDevice->lock);
-			vCommandTimerWait((void *)pDevice, 10);
-			return;
-		}
-
-		if (pMgmt->uScanChannel == 0) {
-			pMgmt->uScanChannel = pDevice->byMinChannel;
-			// Set Baseband to be more sensitive.
-
-		}
-		if (pMgmt->uScanChannel > pDevice->byMaxChannel) {
-			pMgmt->eScanState = WMAC_NO_SCANNING;
-
-			// Set Baseband's sensitivity back.
-			// Set channel back
-			set_channel(pMgmt->pAdapter, pMgmt->uCurrChannel);
-			pr_debug("Scanning, set back to channel: [%d]\n",
-				 pMgmt->uCurrChannel);
-			if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)
-				CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, NL80211_IFTYPE_ADHOC);
-			else
-				CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, NL80211_IFTYPE_STATION);
-
-			vAdHocBeaconRestart(pDevice);
-			s_bCommandComplete(pDevice);
-
-		} else {
-//2008-8-4 <add> by chester
-			if (!is_channel_valid(pMgmt->uScanChannel)) {
-				pr_debug("Invalid channel pMgmt->uScanChannel = %d\n",
-					 pMgmt->uScanChannel);
-				s_bCommandComplete(pDevice);
-				spin_unlock_irq(&pDevice->lock);
-				return;
-			}
-			if (pMgmt->uScanChannel == pDevice->byMinChannel) {
-				pMgmt->abyScanBSSID[0] = 0xFF;
-				pMgmt->abyScanBSSID[1] = 0xFF;
-				pMgmt->abyScanBSSID[2] = 0xFF;
-				pMgmt->abyScanBSSID[3] = 0xFF;
-				pMgmt->abyScanBSSID[4] = 0xFF;
-				pMgmt->abyScanBSSID[5] = 0xFF;
-				pItemSSID->byElementID = WLAN_EID_SSID;
-				pMgmt->eScanState = WMAC_IS_SCANNING;
-
-			}
-
-			vAdHocBeaconStop(pDevice);
-
-			if (set_channel(pMgmt->pAdapter, pMgmt->uScanChannel))
-				pr_debug("SCAN Channel: %d\n",
-					 pMgmt->uScanChannel);
-			else
-				pr_debug("SET SCAN Channel Fail: %d\n",
-					 pMgmt->uScanChannel);
-
-			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, NL80211_IFTYPE_UNSPECIFIED);
-			pMgmt->uScanChannel++;
-//2008-8-4 <modify> by chester
-			if (!is_channel_valid(pMgmt->uScanChannel) &&
-			    pMgmt->uScanChannel <= pDevice->byMaxChannel) {
-				pMgmt->uScanChannel = pDevice->byMaxChannel + 1;
-				pMgmt->eCommandState = WLAN_CMD_SCAN_END;
-
-			}
-
-			if (!pMgmt->b11hEnable ||
-			    (pMgmt->uScanChannel < CB_MAX_CHANNEL_24G)) {
-				s_vProbeChannel(pDevice);
-				spin_unlock_irq(&pDevice->lock);
-				vCommandTimerWait((void *)pDevice, WCMD_ACTIVE_SCAN_TIME);
-				return;
-			} else {
-				spin_unlock_irq(&pDevice->lock);
-				vCommandTimerWait((void *)pDevice, WCMD_PASSIVE_SCAN_TIME);
-				return;
-			}
-
-		}
-
-		break;
-
-	case WLAN_CMD_SCAN_END:
-
-		// Set Baseband's sensitivity back.
-		// Set channel back
-		set_channel(pMgmt->pAdapter, pMgmt->uCurrChannel);
-		pr_debug("Scanning, set back to channel: [%d]\n",
-			 pMgmt->uCurrChannel);
-		if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)
-			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, NL80211_IFTYPE_ADHOC);
-		else
-			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, NL80211_IFTYPE_STATION);
-
-		pMgmt->eScanState = WMAC_NO_SCANNING;
-		vAdHocBeaconRestart(pDevice);
-//2008-0409-07, <Add> by Einsn Liu
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-		if (pMgmt->eScanType == WMAC_SCAN_PASSIVE) {
-			//send scan event to wpa_Supplicant
-			union iwreq_data wrqu;
-
-			memset(&wrqu, 0, sizeof(wrqu));
-			wireless_send_event(pDevice->dev, SIOCGIWSCAN, &wrqu, NULL);
-		}
-#endif
-		s_bCommandComplete(pDevice);
-		break;
-
-	case WLAN_CMD_DISASSOCIATE_START:
-		pDevice->byReAssocCount = 0;
-		if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
-		    (pMgmt->eCurrState != WMAC_STATE_ASSOC)) {
-			s_bCommandComplete(pDevice);
-			spin_unlock_irq(&pDevice->lock);
-			return;
-		} else {
-			pr_debug("Send Disassociation Packet..\n");
-			// reason = 8 : disassoc because sta has left
-			vMgrDisassocBeginSta((void *)pDevice, pMgmt, pMgmt->abyCurrBSSID, (8), &Status);
-			pDevice->bLinkPass = false;
-			// unlock command busy
-			pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
-			pItemSSID->len = 0;
-			memset(pItemSSID->abySSID, 0, WLAN_SSID_MAXLEN);
-			pMgmt->eCurrState = WMAC_STATE_IDLE;
-			pMgmt->sNodeDBTable[0].bActive = false;
-		}
-		netif_stop_queue(pDevice->dev);
-		pDevice->eCommandState = WLAN_DISASSOCIATE_WAIT;
-		// wait all Control TD complete
-		if (pDevice->iTDUsed[TYPE_TXDMA0] != 0) {
-			vCommandTimerWait((void *)pDevice, 10);
-			spin_unlock_irq(&pDevice->lock);
-			return;
-		}
-		pr_debug(" CARDbRadioPowerOff\n");
-		//2008-09-02  <mark>	by chester
-		s_bCommandComplete(pDevice);
-		break;
-
-	case WLAN_DISASSOCIATE_WAIT:
-		// wait all Control TD complete
-		if (pDevice->iTDUsed[TYPE_TXDMA0] != 0) {
-			vCommandTimerWait((void *)pDevice, 10);
-			spin_unlock_irq(&pDevice->lock);
-			return;
-		}
-//2008-09-02  <mark> by chester
-		s_bCommandComplete(pDevice);
-		break;
-
-	case WLAN_CMD_SSID_START:
-		pDevice->byReAssocCount = 0;
-		if (pDevice->bRadioOff) {
-			s_bCommandComplete(pDevice);
-			spin_unlock_irq(&pDevice->lock);
-			return;
-		}
-		pr_debug("chester-abyDesireSSID=%s\n", ((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->abySSID);
-		pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
-		pItemSSIDCurr = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
-		pr_debug(" cmd: desire ssid = %s\n", pItemSSID->abySSID);
-		pr_debug(" cmd: curr ssid = %s\n", pItemSSIDCurr->abySSID);
-
-		if (pMgmt->eCurrState == WMAC_STATE_ASSOC) {
-			pr_debug(" Cmd pMgmt->eCurrState == WMAC_STATE_ASSOC\n");
-			pr_debug(" pItemSSID->len =%d\n", pItemSSID->len);
-			pr_debug(" pItemSSIDCurr->len = %d\n",
-				 pItemSSIDCurr->len);
-			pr_debug(" desire ssid = %s\n", pItemSSID->abySSID);
-			pr_debug(" curr ssid = %s\n", pItemSSIDCurr->abySSID);
-		}
-
-		if ((pMgmt->eCurrState == WMAC_STATE_ASSOC) ||
-		    ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) && (pMgmt->eCurrState == WMAC_STATE_JOINTED))) {
-			if (pItemSSID->len == pItemSSIDCurr->len) {
-				if (memcmp(pItemSSID->abySSID, pItemSSIDCurr->abySSID, pItemSSID->len) == 0) {
-					s_bCommandComplete(pDevice);
-					spin_unlock_irq(&pDevice->lock);
-					return;
-				}
-			}
-
-			netif_stop_queue(pDevice->dev);
-			pDevice->bLinkPass = false;
-		}
-		// set initial state
-		pMgmt->eCurrState = WMAC_STATE_IDLE;
-		pMgmt->eCurrMode = WMAC_MODE_STANDBY;
-		PSvDisablePowerSaving((void *)pDevice);
-		BSSvClearNodeDBTable(pDevice, 0);
-
-		vMgrJoinBSSBegin((void *)pDevice, &Status);
-		// if Infra mode
-		if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) && (pMgmt->eCurrState == WMAC_STATE_JOINTED)) {
-			// Call mgr to begin the deauthentication
-			// reason = (3) because sta has left ESS
-			if (pMgmt->eCurrState >= WMAC_STATE_AUTH)
-				vMgrDeAuthenBeginSta((void *)pDevice, pMgmt, pMgmt->abyCurrBSSID, (3), &Status);
-
-			// Call mgr to begin the authentication
-			vMgrAuthenBeginSta((void *)pDevice, pMgmt, &Status);
-			if (Status == CMD_STATUS_SUCCESS) {
-				pDevice->byLinkWaitCount = 0;
-				pDevice->eCommandState = WLAN_AUTHENTICATE_WAIT;
-				vCommandTimerWait((void *)pDevice, AUTHENTICATE_TIMEOUT);
-				spin_unlock_irq(&pDevice->lock);
-				pr_debug(" Set eCommandState = WLAN_AUTHENTICATE_WAIT\n");
-				return;
-			}
-		}
-		// if Adhoc mode
-		else if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-			if (pMgmt->eCurrState == WMAC_STATE_JOINTED) {
-				if (netif_queue_stopped(pDevice->dev))
-					netif_wake_queue(pDevice->dev);
-
-				pDevice->bLinkPass = true;
-
-				pMgmt->sNodeDBTable[0].bActive = true;
-				pMgmt->sNodeDBTable[0].uInActiveCount = 0;
-				bClearBSSID_SCAN(pDevice);
-			} else {
-				// start own IBSS
-				vMgrCreateOwnIBSS((void *)pDevice, &Status);
-				if (Status != CMD_STATUS_SUCCESS)
-					pr_debug(" WLAN_CMD_IBSS_CREATE fail !\n");
-
-				BSSvAddMulticastNode(pDevice);
-			}
-		}
-		// if SSID not found
-		else if (pMgmt->eCurrMode == WMAC_MODE_STANDBY) {
-			if (pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA ||
-			    pMgmt->eConfigMode == WMAC_CONFIG_AUTO) {
-				// start own IBSS
-				vMgrCreateOwnIBSS((void *)pDevice, &Status);
-				if (Status != CMD_STATUS_SUCCESS)
-					pr_debug(" WLAN_CMD_IBSS_CREATE fail !\n");
-
-				BSSvAddMulticastNode(pDevice);
-				if (netif_queue_stopped(pDevice->dev))
-					netif_wake_queue(pDevice->dev);
-
-				pDevice->bLinkPass = true;
-			} else {
-				pr_debug("Disconnect SSID none\n");
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-				{
-					union iwreq_data  wrqu;
-
-					memset(&wrqu, 0, sizeof(wrqu));
-					wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-					pr_debug("wireless_send_event--->SIOCGIWAP(disassociated:vMgrJoinBSSBegin Fail !!)\n");
-					wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
-				}
-#endif
-
-			}
-		}
-		s_bCommandComplete(pDevice);
-		break;
-
-	case WLAN_AUTHENTICATE_WAIT:
-		pr_debug("eCommandState == WLAN_AUTHENTICATE_WAIT\n");
-		if (pMgmt->eCurrState == WMAC_STATE_AUTH) {
-			// Call mgr to begin the association
-			pDevice->byLinkWaitCount = 0;
-			pr_debug("eCurrState == WMAC_STATE_AUTH\n");
-			vMgrAssocBeginSta((void *)pDevice, pMgmt, &Status);
-			if (Status == CMD_STATUS_SUCCESS) {
-				pDevice->byLinkWaitCount = 0;
-				pr_debug("eCommandState = WLAN_ASSOCIATE_WAIT\n");
-				pDevice->eCommandState = WLAN_ASSOCIATE_WAIT;
-				vCommandTimerWait((void *)pDevice, ASSOCIATE_TIMEOUT);
-				spin_unlock_irq(&pDevice->lock);
-				return;
-			}
-		}
-
-		else if (pMgmt->eCurrState < WMAC_STATE_AUTHPENDING) {
-			pr_debug("WLAN_AUTHENTICATE_WAIT:Authen Fail???\n");
-		} else if (pDevice->byLinkWaitCount <= 4) {    //mike add:wait another 2 sec if authenticated_frame delay!
-			pDevice->byLinkWaitCount++;
-			pr_debug("WLAN_AUTHENTICATE_WAIT:wait %d times!!\n", pDevice->byLinkWaitCount);
-			spin_unlock_irq(&pDevice->lock);
-			vCommandTimerWait((void *)pDevice, AUTHENTICATE_TIMEOUT/2);
-			return;
-		}
-		pDevice->byLinkWaitCount = 0;
-		s_bCommandComplete(pDevice);
-		break;
-
-	case WLAN_ASSOCIATE_WAIT:
-		if (pMgmt->eCurrState == WMAC_STATE_ASSOC) {
-			pr_debug("eCurrState == WMAC_STATE_ASSOC\n");
-			if (pDevice->ePSMode != WMAC_POWER_CAM)
-				PSvEnablePowerSaving((void *)pDevice, pMgmt->wListenInterval);
-
-			if (pMgmt->eAuthenMode >= WMAC_AUTH_WPA)
-				KeybRemoveAllKey(&(pDevice->sKey), pDevice->abyBSSID, pDevice->PortOffset);
-
-			pDevice->bLinkPass = true;
-			pDevice->byLinkWaitCount = 0;
-			pDevice->byReAssocCount = 0;
-			bClearBSSID_SCAN(pDevice);
-			if (pDevice->byFOETuning) {
-				BBvSetFOE(pDevice->PortOffset);
-				PSbSendNullPacket(pDevice);
-			}
-			if (netif_queue_stopped(pDevice->dev))
-				netif_wake_queue(pDevice->dev);
-
-			if (pDevice->IsTxDataTrigger) {    //TxDataTimer is not triggered at the first time
-				del_timer(&pDevice->sTimerTxData);
-				init_timer(&pDevice->sTimerTxData);
-				pDevice->sTimerTxData.data = (unsigned long) pDevice;
-				pDevice->sTimerTxData.function = (TimerFunction)BSSvSecondTxData;
-				pDevice->sTimerTxData.expires = RUN_AT(10*HZ);      //10s callback
-				pDevice->fTxDataInSleep = false;
-				pDevice->nTxDataTimeCout = 0;
-			}
-
-			pDevice->IsTxDataTrigger = true;
-			add_timer(&pDevice->sTimerTxData);
-
-		} else if (pMgmt->eCurrState < WMAC_STATE_ASSOCPENDING) {
-			printk("WLAN_ASSOCIATE_WAIT:Association Fail???\n");
-		} else if (pDevice->byLinkWaitCount <= 4) {    //mike add:wait another 2 sec if associated_frame delay!
-			pDevice->byLinkWaitCount++;
-			pr_debug("WLAN_ASSOCIATE_WAIT:wait %d times!!\n", pDevice->byLinkWaitCount);
-			spin_unlock_irq(&pDevice->lock);
-			vCommandTimerWait((void *)pDevice, ASSOCIATE_TIMEOUT/2);
-			return;
-		}
-		pDevice->byLinkWaitCount = 0;
-
-		s_bCommandComplete(pDevice);
-		break;
-
-	case WLAN_CMD_AP_MODE_START:
-		pr_debug("eCommandState == WLAN_CMD_AP_MODE_START\n");
-
-		if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
-			del_timer(&pMgmt->sTimerSecondCallback);
-			pMgmt->eCurrState = WMAC_STATE_IDLE;
-			pMgmt->eCurrMode = WMAC_MODE_STANDBY;
-			pDevice->bLinkPass = false;
-			if (pDevice->bEnableHostWEP)
-				BSSvClearNodeDBTable(pDevice, 1);
-			else
-				BSSvClearNodeDBTable(pDevice, 0);
-			pDevice->uAssocCount = 0;
-			pMgmt->eCurrState = WMAC_STATE_IDLE;
-			pDevice->bFixRate = false;
-
-			vMgrCreateOwnIBSS((void *)pDevice, &Status);
-			if (Status != CMD_STATUS_SUCCESS)
-				pr_debug(" vMgrCreateOwnIBSS fail !\n");
-
-			// alway turn off unicast bit
-			MACvRegBitsOff(pDevice->PortOffset, MAC_REG_RCR, RCR_UNICAST);
-			pDevice->byRxMode &= ~RCR_UNICAST;
-			pr_debug("wcmd: rx_mode = %x\n", pDevice->byRxMode);
-			BSSvAddMulticastNode(pDevice);
-			if (netif_queue_stopped(pDevice->dev))
-				netif_wake_queue(pDevice->dev);
-
-			pDevice->bLinkPass = true;
-			add_timer(&pMgmt->sTimerSecondCallback);
-		}
-		s_bCommandComplete(pDevice);
-		break;
-
-	case WLAN_CMD_TX_PSPACKET_START:
-		// DTIM Multicast tx
-		if (pMgmt->sNodeDBTable[0].bRxPSPoll) {
-			while ((skb = skb_dequeue(&pMgmt->sNodeDBTable[0].sTxPSQueue)) != NULL) {
-				if (skb_queue_empty(&pMgmt->sNodeDBTable[0].sTxPSQueue)) {
-					pMgmt->abyPSTxMap[0] &= ~byMask[0];
-					pDevice->bMoreData = false;
-				} else {
-					pDevice->bMoreData = true;
-				}
-				if (!device_dma0_xmit(pDevice, skb, 0))
-					pr_debug("Multicast ps tx fail\n");
-
-				pMgmt->sNodeDBTable[0].wEnQueueCnt--;
-			}
-		}
-
-		// PS nodes tx
-		for (ii = 1; ii < (MAX_NODE_NUM + 1); ii++) {
-			if (pMgmt->sNodeDBTable[ii].bActive &&
-			    pMgmt->sNodeDBTable[ii].bRxPSPoll) {
-				pr_debug("Index=%d Enqueu Cnt= %d\n",
-					 ii,
-					 pMgmt->sNodeDBTable[ii].wEnQueueCnt);
-				while ((skb = skb_dequeue(&pMgmt->sNodeDBTable[ii].sTxPSQueue)) != NULL) {
-					if (skb_queue_empty(&pMgmt->sNodeDBTable[ii].sTxPSQueue)) {
-						// clear tx map
-						pMgmt->abyPSTxMap[pMgmt->sNodeDBTable[ii].wAID >> 3] &=
-							~byMask[pMgmt->sNodeDBTable[ii].wAID & 7];
-						pDevice->bMoreData = false;
-					} else {
-						pDevice->bMoreData = true;
-					}
-					if (!device_dma0_xmit(pDevice, skb, ii))
-						pr_debug("sta ps tx fail\n");
-
-					pMgmt->sNodeDBTable[ii].wEnQueueCnt--;
-					// check if sta ps enabled, and wait next pspoll.
-					// if sta ps disable, then send all pending buffers.
-					if (pMgmt->sNodeDBTable[ii].bPSEnable)
-						break;
-				}
-				if (skb_queue_empty(&pMgmt->sNodeDBTable[ii].sTxPSQueue)) {
-					// clear tx map
-					pMgmt->abyPSTxMap[pMgmt->sNodeDBTable[ii].wAID >> 3] &=
-						~byMask[pMgmt->sNodeDBTable[ii].wAID & 7];
-					pr_debug("Index=%d PS queue clear\n",
-						 ii);
-				}
-				pMgmt->sNodeDBTable[ii].bRxPSPoll = false;
-			}
-		}
-
-		s_bCommandComplete(pDevice);
-		break;
-
-	case WLAN_CMD_RADIO_START:
-		pr_debug("eCommandState == WLAN_CMD_RADIO_START\n");
-		if (pDevice->bRadioCmd)
-			CARDbRadioPowerOn(pDevice);
-		else
-			CARDbRadioPowerOff(pDevice);
-
-		s_bCommandComplete(pDevice);
-		break;
-
-	case WLAN_CMD_CHECK_BBSENSITIVITY_CHANGE:
-		// wait all TD complete
-		if (pDevice->iTDUsed[TYPE_AC0DMA] != 0) {
-			vCommandTimerWait((void *)pDevice, 10);
-			spin_unlock_irq(&pDevice->lock);
-			return;
-		}
-		if (pDevice->iTDUsed[TYPE_TXDMA0] != 0) {
-			vCommandTimerWait((void *)pDevice, 10);
-			spin_unlock_irq(&pDevice->lock);
-			return;
-		}
-		pDevice->byBBVGACurrent = pDevice->byBBVGANew;
-		BBvSetVGAGainOffset(pDevice, pDevice->byBBVGACurrent);
-		pr_debug("SetVGAGainOffset %02X\n", pDevice->byBBVGACurrent);
-		s_bCommandComplete(pDevice);
-		break;
-
-	default:
-		s_bCommandComplete(pDevice);
-		break;
-
-	} //switch
-	spin_unlock_irq(&pDevice->lock);
-}
-
-static
-bool
-s_bCommandComplete(
-	struct vnt_private *pDevice
-)
-{
-	PWLAN_IE_SSID pSSID;
-	bool bRadioCmd = false;
-	bool bForceSCAN = true;
-	PSMgmtObject  pMgmt = pDevice->pMgmt;
-
-	pDevice->eCommandState = WLAN_CMD_IDLE;
-	if (pDevice->cbFreeCmdQueue == CMD_Q_SIZE) {
-		//Command Queue Empty
-		pDevice->bCmdRunning = false;
-		return true;
-	} else {
-		pDevice->eCommand = pDevice->eCmdQueue[pDevice->uCmdDequeueIdx].eCmd;
-		pSSID = (PWLAN_IE_SSID)pDevice->eCmdQueue[pDevice->uCmdDequeueIdx].abyCmdDesireSSID;
-		bRadioCmd = pDevice->eCmdQueue[pDevice->uCmdDequeueIdx].bRadioCmd;
-		bForceSCAN = pDevice->eCmdQueue[pDevice->uCmdDequeueIdx].bForceSCAN;
-		ADD_ONE_WITH_WRAP_AROUND(pDevice->uCmdDequeueIdx, CMD_Q_SIZE);
-		pDevice->cbFreeCmdQueue++;
-		pDevice->bCmdRunning = true;
-		switch (pDevice->eCommand) {
-		case WLAN_CMD_BSSID_SCAN:
-			pr_debug("eCommandState= WLAN_CMD_BSSID_SCAN\n");
-			pDevice->eCommandState = WLAN_CMD_SCAN_START;
-			pMgmt->uScanChannel = 0;
-			if (pSSID->len != 0)
-				memcpy(pMgmt->abyScanSSID, pSSID, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-			else
-				memset(pMgmt->abyScanSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-
-			break;
-		case WLAN_CMD_SSID:
-			pDevice->eCommandState = WLAN_CMD_SSID_START;
-			if (pSSID->len > WLAN_SSID_MAXLEN)
-				pSSID->len = WLAN_SSID_MAXLEN;
-			if (pSSID->len != 0)
-				memcpy(pDevice->pMgmt->abyDesireSSID, pSSID, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-			pr_debug("eCommandState= WLAN_CMD_SSID_START\n");
-			break;
-		case WLAN_CMD_DISASSOCIATE:
-			pDevice->eCommandState = WLAN_CMD_DISASSOCIATE_START;
-			break;
-		case WLAN_CMD_RX_PSPOLL:
-			pDevice->eCommandState = WLAN_CMD_TX_PSPACKET_START;
-			break;
-		case WLAN_CMD_RUN_AP:
-			pDevice->eCommandState = WLAN_CMD_AP_MODE_START;
-			break;
-		case WLAN_CMD_RADIO:
-			pDevice->eCommandState = WLAN_CMD_RADIO_START;
-			pDevice->bRadioCmd = bRadioCmd;
-			break;
-		case WLAN_CMD_CHANGE_BBSENSITIVITY:
-			pDevice->eCommandState = WLAN_CMD_CHECK_BBSENSITIVITY_CHANGE;
-			break;
-
-		default:
-			break;
-
-		}
-
-		vCommandTimerWait((void *)pDevice, 0);
-	}
-
-	return true;
-}
-
-bool bScheduleCommand(
-	void *hDeviceContext,
-	CMD_CODE    eCommand,
-	unsigned char *pbyItem0
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-
-	if (pDevice->cbFreeCmdQueue == 0)
-		return false;
-
-	pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].eCmd = eCommand;
-	pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].bForceSCAN = true;
-	memset(pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].abyCmdDesireSSID, 0 , WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-
-	if (pbyItem0 != NULL) {
-		switch (eCommand) {
-		case WLAN_CMD_BSSID_SCAN:
-			memcpy(pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].abyCmdDesireSSID,
-			       pbyItem0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-			pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].bForceSCAN = false;
-			break;
-
-		case WLAN_CMD_SSID:
-			memcpy(pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].abyCmdDesireSSID,
-			       pbyItem0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-			break;
-
-		case WLAN_CMD_DISASSOCIATE:
-			pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].bNeedRadioOFF = *((int *)pbyItem0);
-			break;
-
-		case WLAN_CMD_RX_PSPOLL:
-			break;
-
-		case WLAN_CMD_RADIO:
-			pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].bRadioCmd = *((int *)pbyItem0);
-			break;
-
-		case WLAN_CMD_CHANGE_BBSENSITIVITY:
-			pDevice->eCommandState = WLAN_CMD_CHECK_BBSENSITIVITY_CHANGE;
-			break;
-
-		default:
-			break;
-		}
-	}
-
-	ADD_ONE_WITH_WRAP_AROUND(pDevice->uCmdEnqueueIdx, CMD_Q_SIZE);
-	pDevice->cbFreeCmdQueue--;
-
-	if (!pDevice->bCmdRunning)
-		s_bCommandComplete(pDevice);
-
-	return true;
-}
-
-/*
- * Description:
- *      Clear BSSID_SCAN cmd in CMD Queue
- *
- * Parameters:
- *  In:
- *      hDeviceContext  - Pointer to the adapter
- *      eCommand        - Command
- *  Out:
- *      none
- *
- * Return Value: true if success; otherwise false
- *
- */
-bool bClearBSSID_SCAN(
-	void *hDeviceContext
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	unsigned int uCmdDequeueIdx = pDevice->uCmdDequeueIdx;
-	unsigned int ii;
-
-	if ((pDevice->cbFreeCmdQueue < CMD_Q_SIZE) && (uCmdDequeueIdx != pDevice->uCmdEnqueueIdx)) {
-		for (ii = 0; ii < (CMD_Q_SIZE - pDevice->cbFreeCmdQueue); ii++) {
-			if (pDevice->eCmdQueue[uCmdDequeueIdx].eCmd == WLAN_CMD_BSSID_SCAN)
-				pDevice->eCmdQueue[uCmdDequeueIdx].eCmd = WLAN_CMD_IDLE;
-			ADD_ONE_WITH_WRAP_AROUND(uCmdDequeueIdx, CMD_Q_SIZE);
-			if (uCmdDequeueIdx == pDevice->uCmdEnqueueIdx)
-				break;
-		}
-	}
-	return true;
-}
-
-//mike add:reset command timer
-void
-vResetCommandTimer(
-	void *hDeviceContext
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-
-	//delete timer
-	del_timer(&pDevice->sTimerCommand);
-	//init timer
-	init_timer(&pDevice->sTimerCommand);
-	pDevice->sTimerCommand.data = (unsigned long) pDevice;
-	pDevice->sTimerCommand.function = (TimerFunction)vCommandTimer;
-	pDevice->sTimerCommand.expires = RUN_AT(HZ);
-	pDevice->cbFreeCmdQueue = CMD_Q_SIZE;
-	pDevice->uCmdDequeueIdx = 0;
-	pDevice->uCmdEnqueueIdx = 0;
-	pDevice->eCommandState = WLAN_CMD_IDLE;
-	pDevice->bCmdRunning = false;
-	pDevice->bCmdClear = false;
-}
-
-void
-BSSvSecondTxData(
-	void *hDeviceContext
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject  pMgmt = &(pDevice->sMgmtObj);
-
-	pDevice->nTxDataTimeCout++;
-
-	if (pDevice->nTxDataTimeCout < 4)     //don't tx data if timer less than 40s
-	{
-		pDevice->sTimerTxData.expires = RUN_AT(10*HZ);      //10s callback
-		add_timer(&pDevice->sTimerTxData);
-		return;
-	}
-
-	spin_lock_irq(&pDevice->lock);
-
-	/* open && sharekey linking */
-	if ((pDevice->bLinkPass && (pMgmt->eAuthenMode < WMAC_AUTH_WPA)) ||
-	    pDevice->fWPA_Authened) {   /* wpa linking */
-		pDevice->fTxDataInSleep = true;
-		PSbSendNullPacket(pDevice);	/* send null packet */
-		pDevice->fTxDataInSleep = false;
-	}
-
-	spin_unlock_irq(&pDevice->lock);
-
-	pDevice->sTimerTxData.expires = RUN_AT(10*HZ); /* 10s callback */
-	add_timer(&pDevice->sTimerTxData);
-}
diff --git a/drivers/staging/vt6655/wcmd.h b/drivers/staging/vt6655/wcmd.h
deleted file mode 100644
index 6ef04de69f37..000000000000
--- a/drivers/staging/vt6655/wcmd.h
+++ /dev/null
@@ -1,123 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: wcmd.h
- *
- * Purpose: Handles the management command interface functions
- *
- * Author: Lyndon Chen
- *
- * Date: May 8, 2002
- *
- */
-
-#ifndef __WCMD_H__
-#define __WCMD_H__
-
-#include "ttype.h"
-#include "80211hdr.h"
-#include "80211mgr.h"
-
-#define AUTHENTICATE_TIMEOUT   1000
-#define ASSOCIATE_TIMEOUT      1000
-
-typedef enum tagCMD_CODE {
-	WLAN_CMD_BSSID_SCAN,
-	WLAN_CMD_SSID,
-	WLAN_CMD_DISASSOCIATE,
-	WLAN_CMD_DEAUTH,
-	WLAN_CMD_RX_PSPOLL,
-	WLAN_CMD_RADIO,
-	WLAN_CMD_CHANGE_BBSENSITIVITY,
-	WLAN_CMD_SETPOWER,
-	WLAN_CMD_TBTT_WAKEUP,
-	WLAN_CMD_BECON_SEND,
-	WLAN_CMD_CHANGE_ANTENNA,
-	WLAN_CMD_REMOVE_ALLKEY,
-	WLAN_CMD_MAC_DISPOWERSAVING,
-	WLAN_CMD_11H_CHSW,
-	WLAN_CMD_RUN_AP
-} CMD_CODE, *PCMD_CODE;
-
-#define CMD_Q_SIZE              32
-
-typedef enum tagCMD_STATUS {
-	CMD_STATUS_SUCCESS = 0,
-	CMD_STATUS_FAILURE,
-	CMD_STATUS_RESOURCES,
-	CMD_STATUS_TIMEOUT,
-	CMD_STATUS_PENDING
-} CMD_STATUS, *PCMD_STATUS;
-
-typedef struct tagCMD_ITEM {
-	CMD_CODE eCmd;
-	unsigned char abyCmdDesireSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-	bool bNeedRadioOFF;
-	unsigned short wDeAuthenReason;
-	bool bRadioCmd;
-	bool bForceSCAN;
-} CMD_ITEM, *PCMD_ITEM;
-
-typedef enum tagCMD_STATE {
-	WLAN_CMD_SCAN_START,
-	WLAN_CMD_SCAN_END,
-	WLAN_CMD_DISASSOCIATE_START,
-	WLAN_CMD_SSID_START,
-	WLAN_AUTHENTICATE_WAIT,
-	WLAN_ASSOCIATE_WAIT,
-	WLAN_DISASSOCIATE_WAIT,
-	WLAN_CMD_TX_PSPACKET_START,
-	WLAN_CMD_AP_MODE_START,
-	WLAN_CMD_RADIO_START,
-	WLAN_CMD_CHECK_BBSENSITIVITY_CHANGE,
-	WLAN_CMD_IDLE
-} CMD_STATE, *PCMD_STATE;
-
-void
-vResetCommandTimer(
-	void *hDeviceContext
-);
-
-void
-vCommandTimer(
-	void *hDeviceContext
-);
-
-bool bClearBSSID_SCAN(
-	void *hDeviceContext
-);
-
-bool
-bScheduleCommand(
-	void *hDeviceContext,
-	CMD_CODE    eCommand,
-	unsigned char *pbyItem0
-);
-
-void
-vCommandTimerWait(
-	void *hDeviceContext,
-	unsigned int MSecond
-);
-
-void
-BSSvSecondTxData(
-	void *hDeviceContext
-);
-
-#endif //__WCMD_H__
diff --git a/drivers/staging/vt6655/wctl.c b/drivers/staging/vt6655/wctl.c
deleted file mode 100644
index 5a54d98d985a..000000000000
--- a/drivers/staging/vt6655/wctl.c
+++ /dev/null
@@ -1,233 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: wctl.c
- *
- * Purpose: handle WMAC duplicate filter & defragment
- *
- * Author: Jerry Chen
- *
- * Date: Jun. 27, 2002
- *
- * Functions:
- *      WCTLbIsDuplicate - Test if duplicate packet
- *      WCTLuSearchDFCB - Search DeFragment Control Database
- *      WCTLuInsertDFCB - Insert DeFragment Control Database
- *      WCTLbHandleFragment - Handle received fragment packet
- *
- * Revision History:
- *
- */
-
-#include "wctl.h"
-#include "device.h"
-#include "card.h"
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*
- * Description:
- *      Scan Rx cache.  Return true if packet is duplicate, else
- *      inserts in receive cache and returns false.
- *
- * Parameters:
- *  In:
- *      pCache      - Receive packets history
- *      pMACHeader  - 802.11 MAC Header of received packet
- *  Out:
- *      none
- *
- * Return Value: true if packet duplicate; otherwise false
- *
- */
-
-bool WCTLbIsDuplicate(PSCache pCache, PS802_11Header pMACHeader)
-{
-	unsigned int uIndex;
-	unsigned int ii;
-	PSCacheEntry    pCacheEntry;
-
-	if (IS_FC_RETRY(pMACHeader)) {
-		uIndex = pCache->uInPtr;
-		for (ii = 0; ii < DUPLICATE_RX_CACHE_LENGTH; ii++) {
-			pCacheEntry = &(pCache->asCacheEntry[uIndex]);
-			if ((pCacheEntry->wFmSequence == pMACHeader->wSeqCtl) &&
-			    ether_addr_equal(pCacheEntry->abyAddr2,
-					     pMACHeader->abyAddr2)) {
-				/* Duplicate match */
-				return true;
-			}
-			ADD_ONE_WITH_WRAP_AROUND(uIndex, DUPLICATE_RX_CACHE_LENGTH);
-		}
-	}
-	/* Not fount in cache - insert */
-	pCacheEntry = &pCache->asCacheEntry[pCache->uInPtr];
-	pCacheEntry->wFmSequence = pMACHeader->wSeqCtl;
-	memcpy(&(pCacheEntry->abyAddr2[0]), &(pMACHeader->abyAddr2[0]), ETH_ALEN);
-	ADD_ONE_WITH_WRAP_AROUND(pCache->uInPtr, DUPLICATE_RX_CACHE_LENGTH);
-	return false;
-}
-
-/*
- * Description:
- *      Found if sequence number of received fragment packet in Defragment Database
- *
- * Parameters:
- *  In:
- *      pDevice     - Pointer to adapter
- *      pMACHeader  - 802.11 MAC Header of received packet
- *  Out:
- *      none
- *
- * Return Value: index number in Defragment Database
- *
- */
-unsigned int WCTLuSearchDFCB(struct vnt_private *pDevice,
-			     PS802_11Header pMACHeader)
-{
-	unsigned int ii;
-
-	for (ii = 0; ii < pDevice->cbDFCB; ii++) {
-		if (pDevice->sRxDFCB[ii].bInUse &&
-		    ether_addr_equal(pDevice->sRxDFCB[ii].abyAddr2,
-				     pMACHeader->abyAddr2)) {
-			return ii;
-		}
-	}
-	return pDevice->cbDFCB;
-}
-
-/*
- * Description:
- *      Insert received fragment packet in Defragment Database
- *
- * Parameters:
- *  In:
- *      pDevice     - Pointer to adapter
- *      pMACHeader  - 802.11 MAC Header of received packet
- *  Out:
- *      none
- *
- * Return Value: index number in Defragment Database
- *
- */
-unsigned int WCTLuInsertDFCB(struct vnt_private *pDevice, PS802_11Header pMACHeader)
-{
-	unsigned int ii;
-
-	if (pDevice->cbFreeDFCB == 0)
-		return pDevice->cbDFCB;
-	for (ii = 0; ii < pDevice->cbDFCB; ii++) {
-		if (!pDevice->sRxDFCB[ii].bInUse) {
-			pDevice->cbFreeDFCB--;
-			pDevice->sRxDFCB[ii].uLifetime = pDevice->dwMaxReceiveLifetime;
-			pDevice->sRxDFCB[ii].bInUse = true;
-			pDevice->sRxDFCB[ii].wSequence = (pMACHeader->wSeqCtl >> 4);
-			pDevice->sRxDFCB[ii].wFragNum = (pMACHeader->wSeqCtl & 0x000F);
-			memcpy(&(pDevice->sRxDFCB[ii].abyAddr2[0]), &(pMACHeader->abyAddr2[0]), ETH_ALEN);
-			return ii;
-		}
-	}
-	return pDevice->cbDFCB;
-}
-
-/*
- * Description:
- *      Handle received fragment packet
- *
- * Parameters:
- *  In:
- *      pDevice         - Pointer to adapter
- *      pMACHeader      - 802.11 MAC Header of received packet
- *      cbFrameLength   - Frame length
- *      bWEP            - is WEP packet
- *  Out:
- *      none
- *
- * Return Value: true if it is valid fragment packet and we have resource to defragment; otherwise false
- *
- */
-bool WCTLbHandleFragment(struct vnt_private *pDevice, PS802_11Header pMACHeader,
-			 unsigned int cbFrameLength, bool bWEP, bool bExtIV)
-{
-	unsigned int uHeaderSize;
-
-	if (bWEP) {
-		uHeaderSize = 28;
-		if (bExtIV)
-			// ExtIV
-			uHeaderSize += 4;
-	} else {
-		uHeaderSize = 24;
-	}
-
-	if (IS_FIRST_FRAGMENT_PKT(pMACHeader)) {
-		pDevice->uCurrentDFCBIdx = WCTLuSearchDFCB(pDevice, pMACHeader);
-		if (pDevice->uCurrentDFCBIdx < pDevice->cbDFCB) {
-			// duplicate, we must flush previous DCB
-			pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].uLifetime = pDevice->dwMaxReceiveLifetime;
-			pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wSequence = (pMACHeader->wSeqCtl >> 4);
-			pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum = (pMACHeader->wSeqCtl & 0x000F);
-		} else {
-			pDevice->uCurrentDFCBIdx = WCTLuInsertDFCB(pDevice, pMACHeader);
-			if (pDevice->uCurrentDFCBIdx == pDevice->cbDFCB)
-				return false;
-		}
-		// reserve 4 byte to match MAC RX Buffer
-		pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer = (unsigned char *)(pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].skb->data + 4);
-		memcpy(pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer, pMACHeader, cbFrameLength);
-		pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].cbFrameLength = cbFrameLength;
-		pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer += cbFrameLength;
-		pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum++;
-		return false;
-	} else {
-		pDevice->uCurrentDFCBIdx = WCTLuSearchDFCB(pDevice, pMACHeader);
-		if (pDevice->uCurrentDFCBIdx != pDevice->cbDFCB) {
-			if ((pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wSequence == (pMACHeader->wSeqCtl >> 4)) &&
-			    (pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum == (pMACHeader->wSeqCtl & 0x000F)) &&
-			    ((pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].cbFrameLength + cbFrameLength - uHeaderSize) < 2346)) {
-				memcpy(pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer, ((unsigned char *)(pMACHeader) + uHeaderSize), (cbFrameLength - uHeaderSize));
-				pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].cbFrameLength += (cbFrameLength - uHeaderSize);
-				pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer += (cbFrameLength - uHeaderSize);
-				pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum++;
-			} else {
-				// seq error or frag # error flush DFCB
-				pDevice->cbFreeDFCB++;
-				pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].bInUse = false;
-				return false;
-			}
-		} else {
-			return false;
-		}
-		if (IS_LAST_FRAGMENT_PKT(pMACHeader)) {
-			//enq defragcontrolblock
-			pDevice->cbFreeDFCB++;
-			pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].bInUse = false;
-			return true;
-		}
-		return false;
-	}
-}
diff --git a/drivers/staging/vt6655/wctl.h b/drivers/staging/vt6655/wctl.h
deleted file mode 100644
index f0995d86f71f..000000000000
--- a/drivers/staging/vt6655/wctl.h
+++ /dev/null
@@ -1,105 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: wctl.h
- *
- * Purpose:
- *
- * Author: Jerry Chen
- *
- * Date: Jun. 27, 2002
- *
- */
-
-#ifndef __WCTL_H__
-#define __WCTL_H__
-
-#include "ttype.h"
-#include "tether.h"
-#include "device.h"
-
-/*---------------------  Export Definitions -------------------------*/
-
-#define IS_TYPE_DATA(pMACHeader)					\
-	((((PS802_11Header) pMACHeader)->wFrameCtl & TYPE_802_11_MASK) == TYPE_802_11_DATA)
-
-#define IS_TYPE_MGMT(pMACHeader)					\
-	((((PS802_11Header) pMACHeader)->wFrameCtl & TYPE_802_11_MASK) == TYPE_802_11_MGMT)
-
-#define IS_TYPE_CONTROL(pMACHeader)					\
-	((((PS802_11Header) pMACHeader)->wFrameCtl & TYPE_802_11_MASK) == TYPE_802_11_CTL)
-
-#define IS_FC_MOREDATA(pMACHeader)					\
-	((((PS802_11Header) pMACHeader)->wFrameCtl & FC_MOREDATA) == FC_MOREDATA)
-
-#define IS_FC_POWERMGT(pMACHeader)					\
-	((((PS802_11Header) pMACHeader)->wFrameCtl & FC_POWERMGT) == FC_POWERMGT)
-
-#define IS_FC_RETRY(pMACHeader)						\
-	((((PS802_11Header) pMACHeader)->wFrameCtl & FC_RETRY) == FC_RETRY)
-
-#define IS_FC_WEP(pMACHeader)						\
-	((((PS802_11Header) pMACHeader)->wFrameCtl & FC_WEP) == FC_WEP)
-
-#ifdef __BIG_ENDIAN
-
-#define IS_FRAGMENT_PKT(pMACHeader)					\
-	(((((PS802_11Header) pMACHeader)->wFrameCtl & FC_MOREFRAG) != 0) | \
-	 ((((PS802_11Header) pMACHeader)->wSeqCtl & 0x0F00) != 0))
-
-#define IS_FIRST_FRAGMENT_PKT(pMACHeader)				\
-	((((PS802_11Header) pMACHeader)->wSeqCtl & 0x0F00) == 0)
-
-#else
-
-#define IS_FRAGMENT_PKT(pMACHeader)					\
-	(((((PS802_11Header) pMACHeader)->wFrameCtl & FC_MOREFRAG) != 0) | \
-	 ((((PS802_11Header) pMACHeader)->wSeqCtl & 0x000F) != 0))
-
-#define IS_FIRST_FRAGMENT_PKT(pMACHeader)				\
-	((((PS802_11Header) pMACHeader)->wSeqCtl & 0x000F) == 0)
-
-#endif//#ifdef __BIG_ENDIAN
-
-#define IS_LAST_FRAGMENT_PKT(pMACHeader)				\
-	((((PS802_11Header) pMACHeader)->wFrameCtl & FC_MOREFRAG) == 0)
-
-#define IS_CTL_PSPOLL(pMACHeader)					\
-	((((PS802_11Header) pMACHeader)->wFrameCtl & TYPE_SUBTYPE_MASK) == TYPE_CTL_PSPOLL)
-
-#define ADD_ONE_WITH_WRAP_AROUND(uVar, uModulo)		\
-do {							\
-	if ((uVar) >= ((uModulo) - 1))			\
-		(uVar) = 0;				\
-	else						\
-		(uVar)++;				\
-} while (0)
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-bool WCTLbIsDuplicate(PSCache pCache, PS802_11Header pMACHeader);
-bool WCTLbHandleFragment(struct vnt_private *, PS802_11Header pMACHeader,
-			 unsigned int cbFrameLength, bool bWEP, bool bExtIV);
-unsigned int WCTLuSearchDFCB(struct vnt_private *, PS802_11Header pMACHeader);
-unsigned int WCTLuInsertDFCB(struct vnt_private *, PS802_11Header pMACHeader);
-
-#endif // __WCTL_H__
diff --git a/drivers/staging/vt6655/wmgr.c b/drivers/staging/vt6655/wmgr.c
deleted file mode 100644
index c73c39d7adfd..000000000000
--- a/drivers/staging/vt6655/wmgr.c
+++ /dev/null
@@ -1,4602 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: wmgr.c
- *
- * Purpose: Handles the 802.11 management functions
- *
- * Author: Lyndon Chen
- *
- * Date: May 8, 2002
- *
- * Functions:
- *      nsMgrObjectInitial - Initialize Management Object data structure
- *      vMgrObjectReset - Reset Management Object data structure
- *      vMgrAssocBeginSta - Start associate function
- *      vMgrReAssocBeginSta - Start reassociate function
- *      vMgrDisassocBeginSta - Start disassociate function
- *      s_vMgrRxAssocRequest - Handle Rcv associate_request
- *      s_vMgrRxAssocResponse - Handle Rcv associate_response
- *      vMrgAuthenBeginSta - Start authentication function
- *      vMgrDeAuthenDeginSta - Start deauthentication function
- *      s_vMgrRxAuthentication - Handle Rcv authentication
- *      s_vMgrRxAuthenSequence_1 - Handle Rcv authentication sequence 1
- *      s_vMgrRxAuthenSequence_2 - Handle Rcv authentication sequence 2
- *      s_vMgrRxAuthenSequence_3 - Handle Rcv authentication sequence 3
- *      s_vMgrRxAuthenSequence_4 - Handle Rcv authentication sequence 4
- *      s_vMgrRxDisassociation - Handle Rcv disassociation
- *      s_vMgrRxBeacon - Handle Rcv Beacon
- *      vMgrCreateOwnIBSS - Create ad_hoc IBSS or AP BSS
- *      vMgrJoinBSSBegin - Join BSS function
- *      s_vMgrSynchBSS - Synch & adopt BSS parameters
- *      s_MgrMakeBeacon - Create Baecon frame
- *      s_MgrMakeProbeResponse - Create Probe Response frame
- *      s_MgrMakeAssocRequest - Create Associate Request frame
- *      s_MgrMakeReAssocRequest - Create ReAssociate Request frame
- *      s_vMgrRxProbeResponse - Handle Rcv probe_response
- *      s_vMrgRxProbeRequest - Handle Rcv probe_request
- *      bMgrPrepareBeaconToSend - Prepare Beacon frame
- *      s_vMgrLogStatus - Log 802.11 Status
- *      vMgrRxManagePacket - Rcv management frame dispatch function
- *      s_vMgrFormatTIM- Assembler TIM field of beacon
- *      vMgrTimerInit- Initial 1-sec and command call back funtions
- *
- * Revision History:
- *
- */
-
-#include "tmacro.h"
-#include "desc.h"
-#include "device.h"
-#include "card.h"
-#include "channel.h"
-#include "80211hdr.h"
-#include "80211mgr.h"
-#include "wmgr.h"
-#include "wcmd.h"
-#include "mac.h"
-#include "bssdb.h"
-#include "power.h"
-#include "datarate.h"
-#include "baseband.h"
-#include "rxtx.h"
-#include "wpa.h"
-#include "rf.h"
-#include "iowpa.h"
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-//2008-8-4 <add> by chester
-static bool ChannelExceedZoneType(
-	struct vnt_private *pDevice,
-	unsigned char byCurrChannel
-);
-
-// Association/diassociation functions
-static
-PSTxMgmtPacket
-s_MgrMakeAssocRequest(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	unsigned char *pDAddr,
-	unsigned short wCurrCapInfo,
-	unsigned short wListenInterval,
-	PWLAN_IE_SSID pCurrSSID,
-	PWLAN_IE_SUPP_RATES pCurrRates,
-	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-);
-
-static
-void
-s_vMgrRxAssocRequest(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PSRxMgmtPacket pRxPacket,
-	unsigned int uNodeIndex
-);
-
-static
-PSTxMgmtPacket
-s_MgrMakeReAssocRequest(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	unsigned char *pDAddr,
-	unsigned short wCurrCapInfo,
-	unsigned short wListenInterval,
-	PWLAN_IE_SSID pCurrSSID,
-	PWLAN_IE_SUPP_RATES pCurrRates,
-	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-);
-
-static
-void
-s_vMgrRxAssocResponse(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PSRxMgmtPacket pRxPacket,
-	bool bReAssocType
-);
-
-static
-void
-s_vMgrRxDisassociation(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PSRxMgmtPacket pRxPacket
-);
-
-// Authentication/deauthen functions
-static
-void
-s_vMgrRxAuthenSequence_1(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PWLAN_FR_AUTHEN pFrame
-);
-
-static
-void
-s_vMgrRxAuthenSequence_2(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PWLAN_FR_AUTHEN pFrame
-);
-
-static
-void
-s_vMgrRxAuthenSequence_3(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PWLAN_FR_AUTHEN pFrame
-);
-
-static
-void
-s_vMgrRxAuthenSequence_4(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PWLAN_FR_AUTHEN pFrame
-);
-
-static
-void
-s_vMgrRxAuthentication(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PSRxMgmtPacket pRxPacket
-);
-
-static
-void
-s_vMgrRxDeauthentication(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PSRxMgmtPacket pRxPacket
-);
-
-// Scan functions
-// probe request/response functions
-static
-void
-s_vMgrRxProbeRequest(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PSRxMgmtPacket pRxPacket
-);
-
-static
-void
-s_vMgrRxProbeResponse(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PSRxMgmtPacket pRxPacket
-);
-
-// beacon functions
-static
-void
-s_vMgrRxBeacon(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PSRxMgmtPacket pRxPacket,
-	bool bInScan
-);
-
-static
-void
-s_vMgrFormatTIM(
-	PSMgmtObject pMgmt,
-	PWLAN_IE_TIM pTIM
-);
-
-static
-PSTxMgmtPacket
-s_MgrMakeBeacon(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	unsigned short wCurrCapInfo,
-	unsigned short wCurrBeaconPeriod,
-	unsigned int uCurrChannel,
-	unsigned short wCurrATIMWinodw,
-	PWLAN_IE_SSID pCurrSSID,
-	unsigned char *pCurrBSSID,
-	PWLAN_IE_SUPP_RATES pCurrSuppRates,
-	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-);
-
-// Association response
-static
-PSTxMgmtPacket
-s_MgrMakeAssocResponse(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	unsigned short wCurrCapInfo,
-	unsigned short wAssocStatus,
-	unsigned short wAssocAID,
-	unsigned char *pDstAddr,
-	PWLAN_IE_SUPP_RATES pCurrSuppRates,
-	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-);
-
-// ReAssociation response
-static
-PSTxMgmtPacket
-s_MgrMakeReAssocResponse(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	unsigned short wCurrCapInfo,
-	unsigned short wAssocStatus,
-	unsigned short wAssocAID,
-	unsigned char *pDstAddr,
-	PWLAN_IE_SUPP_RATES pCurrSuppRates,
-	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-);
-
-// Probe response
-static
-PSTxMgmtPacket
-s_MgrMakeProbeResponse(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	unsigned short wCurrCapInfo,
-	unsigned short wCurrBeaconPeriod,
-	unsigned int uCurrChannel,
-	unsigned short wCurrATIMWinodw,
-	unsigned char *pDstAddr,
-	PWLAN_IE_SSID pCurrSSID,
-	unsigned char *pCurrBSSID,
-	PWLAN_IE_SUPP_RATES pCurrSuppRates,
-	PWLAN_IE_SUPP_RATES pCurrExtSuppRates,
-	unsigned char byPHYType
-);
-
-// received status
-static
-void
-s_vMgrLogStatus(
-	PSMgmtObject pMgmt,
-	unsigned short wStatus
-);
-
-static
-void
-s_vMgrSynchBSS(
-	struct vnt_private *pDevice,
-	unsigned int uBSSMode,
-	PKnownBSS     pCurr,
-	PCMD_STATUS  pStatus
-);
-
-static bool
-s_bCipherMatch(
-	PKnownBSS                        pBSSNode,
-	NDIS_802_11_ENCRYPTION_STATUS    EncStatus,
-	unsigned char *pbyCCSPK,
-	unsigned char *pbyCCSGK
-);
-
-static void  Encyption_Rebuild(
-	struct vnt_private *pDevice,
-	PKnownBSS pCurr
-);
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-/*+
- *
- * Routine Description:
- *    Allocates and initializes the Management object.
- *
- * Return Value:
- *    Ndis_staus.
- *
- -*/
-
-void
-vMgrObjectInit(
-	void *hDeviceContext
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	int ii;
-
-	pMgmt->pbyPSPacketPool = &pMgmt->byPSPacketPool[0];
-	pMgmt->pbyMgmtPacketPool = &pMgmt->byMgmtPacketPool[0];
-	pMgmt->uCurrChannel = pDevice->uChannel;
-	for (ii = 0; ii < WLAN_BSSID_LEN; ii++)
-		pMgmt->abyDesireBSSID[ii] = 0xFF;
-
-	pMgmt->sAssocInfo.AssocInfo.Length = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
-	pMgmt->byCSSPK = KEY_CTL_NONE;
-	pMgmt->byCSSGK = KEY_CTL_NONE;
-	pMgmt->wIBSSBeaconPeriod = DEFAULT_IBSS_BI;
-	BSSvClearBSSList((void *)pDevice, false);
-}
-
-/*+
- *
- * Routine Description:
- *    Initializes timer object
- *
- * Return Value:
- *    Ndis_staus.
- *
- -*/
-
-void
-vMgrTimerInit(
-	void *hDeviceContext
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-
-	init_timer(&pMgmt->sTimerSecondCallback);
-	pMgmt->sTimerSecondCallback.data = (unsigned long) pDevice;
-	pMgmt->sTimerSecondCallback.function = (TimerFunction)BSSvSecondCallBack;
-	pMgmt->sTimerSecondCallback.expires = RUN_AT(HZ);
-
-	init_timer(&pDevice->sTimerCommand);
-	pDevice->sTimerCommand.data = (unsigned long) pDevice;
-	pDevice->sTimerCommand.function = (TimerFunction)vCommandTimer;
-	pDevice->sTimerCommand.expires = RUN_AT(HZ);
-
-	init_timer(&pDevice->sTimerTxData);
-	pDevice->sTimerTxData.data = (unsigned long) pDevice;
-	pDevice->sTimerTxData.function = (TimerFunction)BSSvSecondTxData;
-	pDevice->sTimerTxData.expires = RUN_AT(10*HZ);      //10s callback
-	pDevice->fTxDataInSleep = false;
-	pDevice->IsTxDataTrigger = false;
-	pDevice->nTxDataTimeCout = 0;
-
-	pDevice->cbFreeCmdQueue = CMD_Q_SIZE;
-	pDevice->uCmdDequeueIdx = 0;
-	pDevice->uCmdEnqueueIdx = 0;
-}
-
-/*+
- *
- * Routine Description:
- *    Reset the management object structure.
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrObjectReset(
-	void *hDeviceContext
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject        pMgmt = pDevice->pMgmt;
-
-	pMgmt->eCurrMode = WMAC_MODE_STANDBY;
-	pMgmt->eCurrState = WMAC_STATE_IDLE;
-	pDevice->bEnablePSMode = false;
-	// TODO: timer
-}
-
-/*+
- *
- * Routine Description:
- *    Start the station association procedure.  Namely, send an
- *    association request frame to the AP.
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrAssocBeginSta(
-	void *hDeviceContext,
-	PSMgmtObject pMgmt,
-	PCMD_STATUS pStatus
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSTxMgmtPacket          pTxPacket;
-
-	pMgmt->wCurrCapInfo = 0;
-	pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_ESS(1);
-	if (pDevice->bEncryptionEnable)
-		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
-
-	pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-	if (pMgmt->wListenInterval == 0)
-		pMgmt->wListenInterval = 1;    // at least one.
-
-	// ERP Phy (802.11g) should support short preamble.
-	if (pMgmt->eCurrentPHYMode == PHY_TYPE_11G) {
-		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-		if (CARDbIsShorSlotTime(pMgmt->pAdapter))
-			pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTSLOTTIME(1);
-	} else if (pMgmt->eCurrentPHYMode == PHY_TYPE_11B) {
-		if (CARDbIsShortPreamble(pMgmt->pAdapter))
-			pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-	}
-	if (pMgmt->b11hEnable)
-		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SPECTRUMMNG(1);
-
-	/* build an assocreq frame and send it */
-	pTxPacket = s_MgrMakeAssocRequest
-		(
-			pDevice,
-			pMgmt,
-			pMgmt->abyCurrBSSID,
-			pMgmt->wCurrCapInfo,
-			pMgmt->wListenInterval,
-			(PWLAN_IE_SSID)pMgmt->abyCurrSSID,
-			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates
-);
-
-	if (pTxPacket != NULL) {
-		/* send the frame */
-		*pStatus = csMgmt_xmit(pDevice, pTxPacket);
-		if (*pStatus == CMD_STATUS_PENDING) {
-			pMgmt->eCurrState = WMAC_STATE_ASSOCPENDING;
-			*pStatus = CMD_STATUS_SUCCESS;
-		}
-	} else {
-		*pStatus = CMD_STATUS_RESOURCES;
-	}
-}
-
-/*+
- *
- * Routine Description:
- *    Start the station re-association procedure.
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrReAssocBeginSta(
-	void *hDeviceContext,
-	PSMgmtObject pMgmt,
-	PCMD_STATUS pStatus
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSTxMgmtPacket          pTxPacket;
-
-	pMgmt->wCurrCapInfo = 0;
-	pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_ESS(1);
-	if (pDevice->bEncryptionEnable)
-		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
-
-	pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-
-	if (pMgmt->wListenInterval == 0)
-		pMgmt->wListenInterval = 1;    // at least one.
-
-	// ERP Phy (802.11g) should support short preamble.
-	if (pMgmt->eCurrentPHYMode == PHY_TYPE_11G) {
-		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-		if (CARDbIsShorSlotTime(pMgmt->pAdapter))
-			pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTSLOTTIME(1);
-	} else if (pMgmt->eCurrentPHYMode == PHY_TYPE_11B) {
-		if (CARDbIsShortPreamble(pMgmt->pAdapter))
-			pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-	}
-
-	if (pMgmt->b11hEnable)
-		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SPECTRUMMNG(1);
-
-	pTxPacket = s_MgrMakeReAssocRequest
-		(
-			pDevice,
-			pMgmt,
-			pMgmt->abyCurrBSSID,
-			pMgmt->wCurrCapInfo,
-			pMgmt->wListenInterval,
-			(PWLAN_IE_SSID)pMgmt->abyCurrSSID,
-			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates
-);
-
-	if (pTxPacket != NULL) {
-		/* send the frame */
-		*pStatus = csMgmt_xmit(pDevice, pTxPacket);
-		if (*pStatus != CMD_STATUS_PENDING)
-			pr_debug("Mgt:Reassociation tx failed\n");
-		else
-			pr_debug("Mgt:Reassociation tx sending\n");
-	}
-}
-
-/*+
- *
- * Routine Description:
- *    Send an dis-association request frame to the AP.
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrDisassocBeginSta(
-	void *hDeviceContext,
-	PSMgmtObject pMgmt,
-	unsigned char *abyDestAddress,
-	unsigned short wReason,
-	PCMD_STATUS pStatus
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSTxMgmtPacket      pTxPacket = NULL;
-	WLAN_FR_DISASSOC    sFrame;
-
-	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_DISASSOC_FR_MAXLEN);
-	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-
-	// Setup the sFrame structure
-	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-	sFrame.len = WLAN_DISASSOC_FR_MAXLEN;
-
-	// format fixed field frame structure
-	vMgrEncodeDisassociation(&sFrame);
-
-	// Setup the header
-	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-		(
-			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_DISASSOC)
-));
-
-	memcpy(sFrame.pHdr->sA3.abyAddr1, abyDestAddress, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-
-	// Set reason code
-	*(sFrame.pwReason) = cpu_to_le16(wReason);
-	pTxPacket->cbMPDULen = sFrame.len;
-	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-
-	// send the frame
-	*pStatus = csMgmt_xmit(pDevice, pTxPacket);
-	if (*pStatus == CMD_STATUS_PENDING) {
-		pMgmt->eCurrState = WMAC_STATE_IDLE;
-		*pStatus = CMD_STATUS_SUCCESS;
-	}
-}
-
-/*+
- *
- * Routine Description:(AP function)
- *    Handle incoming station association request frames.
- *
- * Return Value:
- *    None.
- *
- -*/
-
-static
-void
-s_vMgrRxAssocRequest(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PSRxMgmtPacket pRxPacket,
-	unsigned int uNodeIndex
-)
-{
-	WLAN_FR_ASSOCREQ    sFrame;
-	CMD_STATUS          Status;
-	PSTxMgmtPacket      pTxPacket;
-	unsigned short wAssocStatus = 0;
-	unsigned short wAssocAID = 0;
-	unsigned int uRateLen = WLAN_RATES_MAXLEN;
-	unsigned char abyCurrSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
-	unsigned char abyCurrExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
-
-	if (pMgmt->eCurrMode != WMAC_MODE_ESS_AP)
-		return;
-	//  node index not found
-	if (!uNodeIndex)
-		return;
-
-	//check if node is authenticated
-	//decode the frame
-	memset(&sFrame, 0, sizeof(WLAN_FR_ASSOCREQ));
-	memset(abyCurrSuppRates, 0, WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
-	memset(abyCurrExtSuppRates, 0, WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
-	sFrame.len = pRxPacket->cbMPDULen;
-	sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-
-	vMgrDecodeAssocRequest(&sFrame);
-
-	if (pMgmt->sNodeDBTable[uNodeIndex].eNodeState >= NODE_AUTH) {
-		pMgmt->sNodeDBTable[uNodeIndex].eNodeState = NODE_ASSOC;
-		pMgmt->sNodeDBTable[uNodeIndex].wCapInfo = cpu_to_le16(*sFrame.pwCapInfo);
-		pMgmt->sNodeDBTable[uNodeIndex].wListenInterval = cpu_to_le16(*sFrame.pwListenInterval);
-		pMgmt->sNodeDBTable[uNodeIndex].bPSEnable =
-			WLAN_GET_FC_PWRMGT(sFrame.pHdr->sA3.wFrameCtl) ? true : false;
-		// Todo: check sta basic rate, if ap can't support, set status code
-		if (pDevice->eCurrentPHYType == PHY_TYPE_11B)
-			uRateLen = WLAN_RATES_MAXLEN_11B;
-
-		abyCurrSuppRates[0] = WLAN_EID_SUPP_RATES;
-		abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
-						 (PWLAN_IE_SUPP_RATES)abyCurrSuppRates,
-						 uRateLen);
-		abyCurrExtSuppRates[0] = WLAN_EID_EXTSUPP_RATES;
-		if (pDevice->eCurrentPHYType == PHY_TYPE_11G)
-			abyCurrExtSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pExtSuppRates,
-							    (PWLAN_IE_SUPP_RATES)abyCurrExtSuppRates,
-							    uRateLen);
-		else
-			abyCurrExtSuppRates[1] = 0;
-
-		RATEvParseMaxRate((void *)pDevice,
-				  (PWLAN_IE_SUPP_RATES)abyCurrSuppRates,
-				  (PWLAN_IE_SUPP_RATES)abyCurrExtSuppRates,
-				  false, // do not change our basic rate
-				  &(pMgmt->sNodeDBTable[uNodeIndex].wMaxBasicRate),
-				  &(pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate),
-				  &(pMgmt->sNodeDBTable[uNodeIndex].wSuppRate),
-				  &(pMgmt->sNodeDBTable[uNodeIndex].byTopCCKBasicRate),
-				  &(pMgmt->sNodeDBTable[uNodeIndex].byTopOFDMBasicRate)
-);
-
-		// set max tx rate
-		pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate =
-			pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate;
-
-		pr_debug("RxAssocRequest:wTxDataRate is %d\n", pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate);
-
-		// Todo: check sta preamble, if ap can't support, set status code
-		pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble =
-			WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo);
-		pMgmt->sNodeDBTable[uNodeIndex].bShortSlotTime =
-			WLAN_GET_CAP_INFO_SHORTSLOTTIME(*sFrame.pwCapInfo);
-		pMgmt->sNodeDBTable[uNodeIndex].wAID = (unsigned short)uNodeIndex;
-		wAssocStatus = WLAN_MGMT_STATUS_SUCCESS;
-		wAssocAID = (unsigned short)uNodeIndex;
-		// check if ERP support
-		if (pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate > RATE_11M)
-			pMgmt->sNodeDBTable[uNodeIndex].bERPExist = true;
-
-		if (pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate <= RATE_11M) {
-			// B only STA join
-			pDevice->bProtectMode = true;
-			pDevice->bNonERPPresent = true;
-		}
-		if (!pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble)
-			pDevice->bBarkerPreambleMd = true;
-
-		pr_info("Associate AID= %d\n", wAssocAID);
-		pr_info("MAC=%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X\n",
-			sFrame.pHdr->sA3.abyAddr2[0],
-			sFrame.pHdr->sA3.abyAddr2[1],
-			sFrame.pHdr->sA3.abyAddr2[2],
-			sFrame.pHdr->sA3.abyAddr2[3],
-			sFrame.pHdr->sA3.abyAddr2[4],
-			sFrame.pHdr->sA3.abyAddr2[5]
-			);
-		pr_info("Max Support rate = %d\n",
-			pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate);
-	} else {
-		/* TODO: received STA under state1 handle */
-		return;
-	}
-
-	// assoc response reply..
-	pTxPacket = s_MgrMakeAssocResponse
-		(
-			pDevice,
-			pMgmt,
-			pMgmt->wCurrCapInfo,
-			wAssocStatus,
-			wAssocAID,
-			sFrame.pHdr->sA3.abyAddr2,
-			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates
-);
-	if (pTxPacket != NULL) {
-		if (pDevice->bEnableHostapd)
-			return;
-
-		/* send the frame */
-		Status = csMgmt_xmit(pDevice, pTxPacket);
-		if (Status != CMD_STATUS_PENDING)
-			pr_debug("Mgt:Assoc response tx failed\n");
-		else
-			pr_debug("Mgt:Assoc response tx sending..\n");
-	}
-}
-
-/*+
- *
- * Description:(AP function)
- *      Handle incoming station re-association request frames.
- *
- * Parameters:
- *  In:
- *      pMgmt           - Management Object structure
- *      pRxPacket       - Received Packet
- *  Out:
- *      none
- *
- * Return Value: None.
- *
- -*/
-
-static
-void
-s_vMgrRxReAssocRequest(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PSRxMgmtPacket pRxPacket,
-	unsigned int uNodeIndex
-)
-{
-	WLAN_FR_REASSOCREQ    sFrame;
-	CMD_STATUS          Status;
-	PSTxMgmtPacket      pTxPacket;
-	unsigned short wAssocStatus = 0;
-	unsigned short wAssocAID = 0;
-	unsigned int	uRateLen = WLAN_RATES_MAXLEN;
-	unsigned char abyCurrSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
-	unsigned char abyCurrExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
-
-	if (pMgmt->eCurrMode != WMAC_MODE_ESS_AP)
-		return;
-	//  node index not found
-	if (!uNodeIndex)
-		return;
-	//check if node is authenticated
-	//decode the frame
-	memset(&sFrame, 0, sizeof(WLAN_FR_REASSOCREQ));
-	sFrame.len = pRxPacket->cbMPDULen;
-	sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-	vMgrDecodeReassocRequest(&sFrame);
-
-	if (pMgmt->sNodeDBTable[uNodeIndex].eNodeState >= NODE_AUTH) {
-		pMgmt->sNodeDBTable[uNodeIndex].eNodeState = NODE_ASSOC;
-		pMgmt->sNodeDBTable[uNodeIndex].wCapInfo = cpu_to_le16(*sFrame.pwCapInfo);
-		pMgmt->sNodeDBTable[uNodeIndex].wListenInterval = cpu_to_le16(*sFrame.pwListenInterval);
-		pMgmt->sNodeDBTable[uNodeIndex].bPSEnable =
-			WLAN_GET_FC_PWRMGT(sFrame.pHdr->sA3.wFrameCtl) ? true : false;
-		// Todo: check sta basic rate, if ap can't support, set status code
-
-		if (pDevice->eCurrentPHYType == PHY_TYPE_11B)
-			uRateLen = WLAN_RATES_MAXLEN_11B;
-
-		abyCurrSuppRates[0] = WLAN_EID_SUPP_RATES;
-		abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
-						 (PWLAN_IE_SUPP_RATES)abyCurrSuppRates,
-						 uRateLen);
-		abyCurrExtSuppRates[0] = WLAN_EID_EXTSUPP_RATES;
-		if (pDevice->eCurrentPHYType == PHY_TYPE_11G) {
-			abyCurrExtSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pExtSuppRates,
-							    (PWLAN_IE_SUPP_RATES)abyCurrExtSuppRates,
-							    uRateLen);
-		} else {
-			abyCurrExtSuppRates[1] = 0;
-		}
-
-		RATEvParseMaxRate((void *)pDevice,
-				  (PWLAN_IE_SUPP_RATES)abyCurrSuppRates,
-				  (PWLAN_IE_SUPP_RATES)abyCurrExtSuppRates,
-				  false, // do not change our basic rate
-				  &(pMgmt->sNodeDBTable[uNodeIndex].wMaxBasicRate),
-				  &(pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate),
-				  &(pMgmt->sNodeDBTable[uNodeIndex].wSuppRate),
-				  &(pMgmt->sNodeDBTable[uNodeIndex].byTopCCKBasicRate),
-				  &(pMgmt->sNodeDBTable[uNodeIndex].byTopOFDMBasicRate)
-);
-
-		// set max tx rate
-		pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate =
-			pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate;
-
-		pr_debug("RxReAssocRequest:TxDataRate is %d\n", pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate);
-
-		// Todo: check sta preamble, if ap can't support, set status code
-		pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble =
-			WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo);
-		pMgmt->sNodeDBTable[uNodeIndex].bShortSlotTime =
-			WLAN_GET_CAP_INFO_SHORTSLOTTIME(*sFrame.pwCapInfo);
-		pMgmt->sNodeDBTable[uNodeIndex].wAID = (unsigned short)uNodeIndex;
-		wAssocStatus = WLAN_MGMT_STATUS_SUCCESS;
-		wAssocAID = (unsigned short)uNodeIndex;
-
-		// if suppurt ERP
-		if (pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate > RATE_11M)
-			pMgmt->sNodeDBTable[uNodeIndex].bERPExist = true;
-
-		if (pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate <= RATE_11M) {
-			// B only STA join
-			pDevice->bProtectMode = true;
-			pDevice->bNonERPPresent = true;
-		}
-		if (!pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble)
-			pDevice->bBarkerPreambleMd = true;
-
-		pr_info("Rx ReAssociate AID= %d\n", wAssocAID);
-		pr_info("MAC=%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X\n",
-			sFrame.pHdr->sA3.abyAddr2[0],
-			sFrame.pHdr->sA3.abyAddr2[1],
-			sFrame.pHdr->sA3.abyAddr2[2],
-			sFrame.pHdr->sA3.abyAddr2[3],
-			sFrame.pHdr->sA3.abyAddr2[4],
-			sFrame.pHdr->sA3.abyAddr2[5]
-			);
-		pr_info("Max Support rate = %d\n",
-			pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate);
-
-	}
-
-	// assoc response reply..
-	pTxPacket = s_MgrMakeReAssocResponse
-		(
-			pDevice,
-			pMgmt,
-			pMgmt->wCurrCapInfo,
-			wAssocStatus,
-			wAssocAID,
-			sFrame.pHdr->sA3.abyAddr2,
-			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates
-			);
-
-	if (pTxPacket != NULL) {
-		/* send the frame */
-		if (pDevice->bEnableHostapd)
-			return;
-
-		Status = csMgmt_xmit(pDevice, pTxPacket);
-		if (Status != CMD_STATUS_PENDING)
-			pr_debug("Mgt:ReAssoc response tx failed\n");
-		else
-			pr_debug("Mgt:ReAssoc response tx sending..\n");
-	}
-}
-
-/*+
- *
- * Routine Description:
- *    Handle incoming association response frames.
- *
- * Return Value:
- *    None.
- *
- -*/
-
-static
-void
-s_vMgrRxAssocResponse(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PSRxMgmtPacket pRxPacket,
-	bool bReAssocType
-)
-{
-	WLAN_FR_ASSOCRESP   sFrame;
-	PWLAN_IE_SSID   pItemSSID;
-	unsigned char *pbyIEs;
-	viawget_wpa_header *wpahdr;
-
-	if (pMgmt->eCurrState == WMAC_STATE_ASSOCPENDING ||
-	    pMgmt->eCurrState == WMAC_STATE_ASSOC) {
-		sFrame.len = pRxPacket->cbMPDULen;
-		sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-		// decode the frame
-		vMgrDecodeAssocResponse(&sFrame);
-		if ((sFrame.pwCapInfo == NULL) ||
-		    (sFrame.pwStatus == NULL) ||
-		    (sFrame.pwAid == NULL) ||
-		    (sFrame.pSuppRates == NULL)) {
-			DBG_PORT80(0xCC);
-			return;
-		}
-
-		pMgmt->sAssocInfo.AssocInfo.ResponseFixedIEs.Capabilities = *(sFrame.pwCapInfo);
-		pMgmt->sAssocInfo.AssocInfo.ResponseFixedIEs.StatusCode = *(sFrame.pwStatus);
-		pMgmt->sAssocInfo.AssocInfo.ResponseFixedIEs.AssociationId = *(sFrame.pwAid);
-		pMgmt->sAssocInfo.AssocInfo.AvailableResponseFixedIEs |= 0x07;
-
-		pMgmt->sAssocInfo.AssocInfo.ResponseIELength = sFrame.len - 24 - 6;
-		pMgmt->sAssocInfo.AssocInfo.OffsetResponseIEs = pMgmt->sAssocInfo.AssocInfo.OffsetRequestIEs + pMgmt->sAssocInfo.AssocInfo.RequestIELength;
-		pbyIEs = pMgmt->sAssocInfo.abyIEs;
-		pbyIEs += pMgmt->sAssocInfo.AssocInfo.RequestIELength;
-		memcpy(pbyIEs, (sFrame.pBuf + 24 + 6), pMgmt->sAssocInfo.AssocInfo.ResponseIELength);
-
-		// save values and set current BSS state
-		if (cpu_to_le16((*(sFrame.pwStatus))) == WLAN_MGMT_STATUS_SUCCESS) {
-			// set AID
-			pMgmt->wCurrAID = cpu_to_le16((*(sFrame.pwAid)));
-			if ((pMgmt->wCurrAID >> 14) != (BIT0 | BIT1))
-				pr_debug("AID from AP, has two msb clear\n");
-
-			pr_info("Association Successful, AID=%d\n",
-				pMgmt->wCurrAID & ~(BIT14 | BIT15));
-			pMgmt->eCurrState = WMAC_STATE_ASSOC;
-			BSSvUpdateAPNode((void *)pDevice, sFrame.pwCapInfo, sFrame.pSuppRates, sFrame.pExtSuppRates);
-			pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
-			pr_info("Link with AP(SSID): %s\n", pItemSSID->abySSID);
-			pDevice->bLinkPass = true;
-			pDevice->uBBVGADiffCount = 0;
-			if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
-				if (skb_tailroom(pDevice->skb) < (sizeof(viawget_wpa_header) + pMgmt->sAssocInfo.AssocInfo.ResponseIELength +
-								  pMgmt->sAssocInfo.AssocInfo.RequestIELength)) {    //data room not enough
-					dev_kfree_skb(pDevice->skb);
-					pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
-				}
-				wpahdr = (viawget_wpa_header *)pDevice->skb->data;
-				wpahdr->type = VIAWGET_ASSOC_MSG;
-				wpahdr->resp_ie_len = pMgmt->sAssocInfo.AssocInfo.ResponseIELength;
-				wpahdr->req_ie_len = pMgmt->sAssocInfo.AssocInfo.RequestIELength;
-				memcpy(pDevice->skb->data + sizeof(viawget_wpa_header), pMgmt->sAssocInfo.abyIEs, wpahdr->req_ie_len);
-				memcpy(pDevice->skb->data + sizeof(viawget_wpa_header) + wpahdr->req_ie_len,
-				       pbyIEs,
-				       wpahdr->resp_ie_len
-);
-				skb_put(pDevice->skb, sizeof(viawget_wpa_header) + wpahdr->resp_ie_len + wpahdr->req_ie_len);
-				pDevice->skb->dev = pDevice->wpadev;
-				skb_reset_mac_header(pDevice->skb);
-				pDevice->skb->pkt_type = PACKET_HOST;
-				pDevice->skb->protocol = htons(ETH_P_802_2);
-				memset(pDevice->skb->cb, 0, sizeof(pDevice->skb->cb));
-				netif_rx(pDevice->skb);
-				pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
-			}
-
-//2008-0409-07, <Add> by Einsn Liu
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-			{
-				unsigned char buf[512];
-				size_t len;
-				union iwreq_data  wrqu;
-				int we_event;
-
-				memset(buf, 0, 512);
-
-				len = pMgmt->sAssocInfo.AssocInfo.RequestIELength;
-				if (len)	{
-					memcpy(buf, pMgmt->sAssocInfo.abyIEs, len);
-					memset(&wrqu, 0, sizeof(wrqu));
-					wrqu.data.length = len;
-					we_event = IWEVASSOCREQIE;
-					wireless_send_event(pDevice->dev, we_event, &wrqu, buf);
-				}
-
-				memset(buf, 0, 512);
-				len = pMgmt->sAssocInfo.AssocInfo.ResponseIELength;
-
-				if (len)	{
-					memcpy(buf, pbyIEs, len);
-					memset(&wrqu, 0, sizeof(wrqu));
-					wrqu.data.length = len;
-					we_event = IWEVASSOCRESPIE;
-					wireless_send_event(pDevice->dev, we_event, &wrqu, buf);
-				}
-
-				memset(&wrqu, 0, sizeof(wrqu));
-				memcpy(wrqu.ap_addr.sa_data, &pMgmt->abyCurrBSSID[0], ETH_ALEN);
-				wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-				wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
-			}
-#endif //#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-//End Add -- //2008-0409-07, <Add> by Einsn Liu
-		} else {
-			if (bReAssocType) {
-				pMgmt->eCurrState = WMAC_STATE_IDLE;
-			} else {
-				// jump back to the auth state and indicate the error
-				pMgmt->eCurrState = WMAC_STATE_AUTH;
-			}
-			s_vMgrLogStatus(pMgmt, cpu_to_le16((*(sFrame.pwStatus))));
-		}
-
-	}
-
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-//need clear flags related to Networkmanager
-
-	pDevice->bwextcount = 0;
-	pDevice->bWPASuppWextEnabled = false;
-#endif
-
-	if (pMgmt->eCurrState == WMAC_STATE_ASSOC)
-		timer_expire(pDevice->sTimerCommand, 0);
-}
-
-/*+
- *
- * Routine Description:
- *    Start the station authentication procedure.  Namely, send an
- *    authentication frame to the AP.
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrAuthenBeginSta(
-	void *hDeviceContext,
-	PSMgmtObject  pMgmt,
-	PCMD_STATUS pStatus
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	WLAN_FR_AUTHEN  sFrame;
-	PSTxMgmtPacket  pTxPacket = NULL;
-
-	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_AUTHEN_FR_MAXLEN);
-	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-	sFrame.len = WLAN_AUTHEN_FR_MAXLEN;
-	vMgrEncodeAuthen(&sFrame);
-	/* insert values */
-	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-		(
-			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_AUTHEN)
-));
-	memcpy(sFrame.pHdr->sA3.abyAddr1, pMgmt->abyCurrBSSID, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-	if (pMgmt->bShareKeyAlgorithm)
-		*(sFrame.pwAuthAlgorithm) = cpu_to_le16(WLAN_AUTH_ALG_SHAREDKEY);
-	else
-		*(sFrame.pwAuthAlgorithm) = cpu_to_le16(WLAN_AUTH_ALG_OPENSYSTEM);
-
-	*(sFrame.pwAuthSequence) = cpu_to_le16(1);
-	/* Adjust the length fields */
-	pTxPacket->cbMPDULen = sFrame.len;
-	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-
-	*pStatus = csMgmt_xmit(pDevice, pTxPacket);
-	if (*pStatus == CMD_STATUS_PENDING) {
-		pMgmt->eCurrState = WMAC_STATE_AUTHPENDING;
-		*pStatus = CMD_STATUS_SUCCESS;
-	}
-}
-
-/*+
- *
- * Routine Description:
- *    Start the station(AP) deauthentication procedure.  Namely, send an
- *    deauthentication frame to the AP or Sta.
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrDeAuthenBeginSta(
-	void *hDeviceContext,
-	PSMgmtObject  pMgmt,
-	unsigned char *abyDestAddress,
-	unsigned short wReason,
-	PCMD_STATUS pStatus
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	WLAN_FR_DEAUTHEN    sFrame;
-	PSTxMgmtPacket      pTxPacket = NULL;
-
-	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_DEAUTHEN_FR_MAXLEN);
-	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-	sFrame.len = WLAN_DEAUTHEN_FR_MAXLEN;
-	vMgrEncodeDeauthen(&sFrame);
-	/* insert values */
-	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-		(
-			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_DEAUTHEN)
-));
-
-	memcpy(sFrame.pHdr->sA3.abyAddr1, abyDestAddress, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-
-	*(sFrame.pwReason) = cpu_to_le16(wReason);       // deauthen. bcs left BSS
-	/* Adjust the length fields */
-	pTxPacket->cbMPDULen = sFrame.len;
-	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-
-	*pStatus = csMgmt_xmit(pDevice, pTxPacket);
-	if (*pStatus == CMD_STATUS_PENDING)
-		*pStatus = CMD_STATUS_SUCCESS;
-}
-
-/*+
- *
- * Routine Description:
- *    Handle incoming authentication frames.
- *
- * Return Value:
- *    None.
- *
- -*/
-
-static
-void
-s_vMgrRxAuthentication(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PSRxMgmtPacket pRxPacket
-)
-{
-	WLAN_FR_AUTHEN  sFrame;
-
-	// we better be an AP or a STA in AUTHPENDING otherwise ignore
-	if (!(pMgmt->eCurrMode == WMAC_MODE_ESS_AP ||
-	      pMgmt->eCurrState == WMAC_STATE_AUTHPENDING)) {
-		return;
-	}
-
-	// decode the frame
-	sFrame.len = pRxPacket->cbMPDULen;
-	sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-	vMgrDecodeAuthen(&sFrame);
-	switch (cpu_to_le16((*(sFrame.pwAuthSequence)))) {
-	case 1:
-		//AP function
-		s_vMgrRxAuthenSequence_1(pDevice, pMgmt, &sFrame);
-		break;
-	case 2:
-		s_vMgrRxAuthenSequence_2(pDevice, pMgmt, &sFrame);
-		break;
-	case 3:
-		//AP function
-		s_vMgrRxAuthenSequence_3(pDevice, pMgmt, &sFrame);
-		break;
-	case 4:
-		s_vMgrRxAuthenSequence_4(pDevice, pMgmt, &sFrame);
-		break;
-	default:
-		pr_debug("Auth Sequence error, seq = %d\n",
-			 cpu_to_le16((*(sFrame.pwAuthSequence))));
-		break;
-	}
-}
-
-/*+
- *
- * Routine Description:
- *   Handles incoming authen frames with sequence 1.  Currently
- *   assumes we're an AP.  So far, no one appears to use authentication
- *   in Ad-Hoc mode.
- *
- * Return Value:
- *    None.
- *
- -*/
-
-static
-void
-s_vMgrRxAuthenSequence_1(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PWLAN_FR_AUTHEN pFrame
-)
-{
-	PSTxMgmtPacket      pTxPacket = NULL;
-	unsigned int	uNodeIndex;
-	WLAN_FR_AUTHEN      sFrame;
-	PSKeyItem           pTransmitKey;
-
-	// Insert a Node entry
-	if (!BSSDBbIsSTAInNodeDB(pMgmt, pFrame->pHdr->sA3.abyAddr2, &uNodeIndex)) {
-		BSSvCreateOneNode(pDevice, &uNodeIndex);
-		memcpy(pMgmt->sNodeDBTable[uNodeIndex].abyMACAddr, pFrame->pHdr->sA3.abyAddr2,
-		       WLAN_ADDR_LEN);
-	}
-
-	if (pMgmt->bShareKeyAlgorithm) {
-		pMgmt->sNodeDBTable[uNodeIndex].eNodeState = NODE_KNOWN;
-		pMgmt->sNodeDBTable[uNodeIndex].byAuthSequence = 1;
-	} else {
-		pMgmt->sNodeDBTable[uNodeIndex].eNodeState = NODE_AUTH;
-	}
-
-	// send auth reply
-	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_AUTHEN_FR_MAXLEN);
-	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-	sFrame.len = WLAN_AUTHEN_FR_MAXLEN;
-	// format buffer structure
-	vMgrEncodeAuthen(&sFrame);
-	// insert values
-	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-		(
-			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_AUTHEN)|
-			WLAN_SET_FC_ISWEP(0)
-));
-	memcpy(sFrame.pHdr->sA3.abyAddr1, pFrame->pHdr->sA3.abyAddr2, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-	*(sFrame.pwAuthAlgorithm) = *(pFrame->pwAuthAlgorithm);
-	*(sFrame.pwAuthSequence) = cpu_to_le16(2);
-
-	if (cpu_to_le16(*(pFrame->pwAuthAlgorithm)) == WLAN_AUTH_ALG_SHAREDKEY) {
-		if (pMgmt->bShareKeyAlgorithm)
-			*(sFrame.pwStatus) = cpu_to_le16(WLAN_MGMT_STATUS_SUCCESS);
-		else
-			*(sFrame.pwStatus) = cpu_to_le16(WLAN_MGMT_STATUS_UNSUPPORTED_AUTHALG);
-	} else {
-		if (pMgmt->bShareKeyAlgorithm)
-			*(sFrame.pwStatus) = cpu_to_le16(WLAN_MGMT_STATUS_UNSUPPORTED_AUTHALG);
-		else
-			*(sFrame.pwStatus) = cpu_to_le16(WLAN_MGMT_STATUS_SUCCESS);
-	}
-
-	if (pMgmt->bShareKeyAlgorithm &&
-	    (cpu_to_le16(*(sFrame.pwStatus)) == WLAN_MGMT_STATUS_SUCCESS)) {
-		sFrame.pChallenge = (PWLAN_IE_CHALLENGE)(sFrame.pBuf + sFrame.len);
-		sFrame.len += WLAN_CHALLENGE_IE_LEN;
-		sFrame.pChallenge->byElementID = WLAN_EID_CHALLENGE;
-		sFrame.pChallenge->len = WLAN_CHALLENGE_LEN;
-		memset(pMgmt->abyChallenge, 0, WLAN_CHALLENGE_LEN);
-		// get group key
-		if (KeybGetTransmitKey(&(pDevice->sKey), pDevice->abyBroadcastAddr, GROUP_KEY, &pTransmitKey) == true) {
-			rc4_init(&pDevice->SBox, pDevice->abyPRNG, pTransmitKey->uKeyLength+3);
-			rc4_encrypt(&pDevice->SBox, pMgmt->abyChallenge, pMgmt->abyChallenge, WLAN_CHALLENGE_LEN);
-		}
-		memcpy(sFrame.pChallenge->abyChallenge, pMgmt->abyChallenge , WLAN_CHALLENGE_LEN);
-	}
-
-	/* Adjust the length fields */
-	pTxPacket->cbMPDULen = sFrame.len;
-	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-	// send the frame
-	if (pDevice->bEnableHostapd)
-		return;
-
-	pr_debug("Mgt:Authreq_reply sequence_1 tx..\n");
-	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING)
-		pr_debug("Mgt:Authreq_reply sequence_1 tx failed\n");
-}
-
-/*+
- *
- * Routine Description:
- *   Handles incoming auth frames with sequence number 2.  Currently
- *   assumes we're a station.
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-static
-void
-s_vMgrRxAuthenSequence_2(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PWLAN_FR_AUTHEN pFrame
-)
-{
-	WLAN_FR_AUTHEN      sFrame;
-	PSTxMgmtPacket      pTxPacket = NULL;
-
-	switch (cpu_to_le16((*(pFrame->pwAuthAlgorithm)))) {
-	case WLAN_AUTH_ALG_OPENSYSTEM:
-		if (cpu_to_le16((*(pFrame->pwStatus))) == WLAN_MGMT_STATUS_SUCCESS) {
-			pr_info("802.11 Authen (OPEN) Successful\n");
-			pMgmt->eCurrState = WMAC_STATE_AUTH;
-			timer_expire(pDevice->sTimerCommand, 0);
-		} else {
-			pr_info("802.11 Authen (OPEN) Failed\n");
-			s_vMgrLogStatus(pMgmt, cpu_to_le16((*(pFrame->pwStatus))));
-			pMgmt->eCurrState = WMAC_STATE_IDLE;
-		}
-
-		break;
-
-	case WLAN_AUTH_ALG_SHAREDKEY:
-
-		if (cpu_to_le16((*(pFrame->pwStatus))) == WLAN_MGMT_STATUS_SUCCESS) {
-			pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-			memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_AUTHEN_FR_MAXLEN);
-			pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-			sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-			sFrame.len = WLAN_AUTHEN_FR_MAXLEN;
-			// format buffer structure
-			vMgrEncodeAuthen(&sFrame);
-			// insert values
-			sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-				(
-					WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-					WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_AUTHEN)|
-					WLAN_SET_FC_ISWEP(1)
-));
-			memcpy(sFrame.pHdr->sA3.abyAddr1, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-			memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-			memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-			*(sFrame.pwAuthAlgorithm) = *(pFrame->pwAuthAlgorithm);
-			*(sFrame.pwAuthSequence) = cpu_to_le16(3);
-			*(sFrame.pwStatus) = cpu_to_le16(WLAN_MGMT_STATUS_SUCCESS);
-			sFrame.pChallenge = (PWLAN_IE_CHALLENGE)(sFrame.pBuf + sFrame.len);
-			sFrame.len += WLAN_CHALLENGE_IE_LEN;
-			sFrame.pChallenge->byElementID = WLAN_EID_CHALLENGE;
-			sFrame.pChallenge->len = WLAN_CHALLENGE_LEN;
-			memcpy(sFrame.pChallenge->abyChallenge, pFrame->pChallenge->abyChallenge, WLAN_CHALLENGE_LEN);
-			// Adjust the length fields
-			pTxPacket->cbMPDULen = sFrame.len;
-			pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-			// send the frame
-			if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING)
-				pr_debug("Mgt:Auth_reply sequence_2 tx failed\n");
-
-			pr_debug("Mgt:Auth_reply sequence_2 tx ...\n");
-		} else {
-			pr_debug("Mgt:rx Auth_reply sequence_2 status error ...\n");
-			s_vMgrLogStatus(pMgmt, cpu_to_le16((*(pFrame->pwStatus))));
-		}
-		break;
-	default:
-		pr_debug("Mgt: rx auth.seq = 2 unknown AuthAlgorithm=%d\n",
-			 cpu_to_le16((*(pFrame->pwAuthAlgorithm))));
-		break;
-	}
-}
-
-/*+
- *
- * Routine Description:
- *   Handles incoming authen frames with sequence 3.  Currently
- *   assumes we're an AP.  This function assumes the frame has
- *   already been successfully decrypted.
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-static
-void
-s_vMgrRxAuthenSequence_3(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PWLAN_FR_AUTHEN pFrame
-)
-{
-	PSTxMgmtPacket      pTxPacket = NULL;
-	unsigned int uStatusCode = 0;
-	unsigned int uNodeIndex = 0;
-	WLAN_FR_AUTHEN      sFrame;
-
-	if (!WLAN_GET_FC_ISWEP(pFrame->pHdr->sA3.wFrameCtl)) {
-		uStatusCode = WLAN_MGMT_STATUS_CHALLENGE_FAIL;
-		goto reply;
-	}
-	if (BSSDBbIsSTAInNodeDB(pMgmt, pFrame->pHdr->sA3.abyAddr2, &uNodeIndex)) {
-		if (pMgmt->sNodeDBTable[uNodeIndex].byAuthSequence != 1) {
-			uStatusCode = WLAN_MGMT_STATUS_RX_AUTH_NOSEQ;
-			goto reply;
-		}
-		if (memcmp(pMgmt->abyChallenge, pFrame->pChallenge->abyChallenge, WLAN_CHALLENGE_LEN) != 0) {
-			uStatusCode = WLAN_MGMT_STATUS_CHALLENGE_FAIL;
-			goto reply;
-		}
-	} else {
-		uStatusCode = WLAN_MGMT_STATUS_UNSPEC_FAILURE;
-		goto reply;
-	}
-
-	if (uNodeIndex) {
-		pMgmt->sNodeDBTable[uNodeIndex].eNodeState = NODE_AUTH;
-		pMgmt->sNodeDBTable[uNodeIndex].byAuthSequence = 0;
-	}
-	uStatusCode = WLAN_MGMT_STATUS_SUCCESS;
-	pr_debug("Challenge text check ok..\n");
-
-reply:
-	// send auth reply
-	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_AUTHEN_FR_MAXLEN);
-	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-	sFrame.len = WLAN_AUTHEN_FR_MAXLEN;
-	// format buffer structure
-	vMgrEncodeAuthen(&sFrame);
-	/* insert values */
-	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-		(
-			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_AUTHEN)|
-			WLAN_SET_FC_ISWEP(0)
-));
-	memcpy(sFrame.pHdr->sA3.abyAddr1, pFrame->pHdr->sA3.abyAddr2, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-	*(sFrame.pwAuthAlgorithm) = *(pFrame->pwAuthAlgorithm);
-	*(sFrame.pwAuthSequence) = cpu_to_le16(4);
-	*(sFrame.pwStatus) = cpu_to_le16(uStatusCode);
-
-	/* Adjust the length fields */
-	pTxPacket->cbMPDULen = sFrame.len;
-	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-	// send the frame
-	if (pDevice->bEnableHostapd)
-		return;
-
-	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING)
-		pr_debug("Mgt:Authreq_reply sequence_4 tx failed\n");
-}
-
-/*+
- *
- * Routine Description:
- *   Handles incoming authen frames with sequence 4
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-static
-void
-s_vMgrRxAuthenSequence_4(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PWLAN_FR_AUTHEN pFrame
-)
-{
-	if (cpu_to_le16((*(pFrame->pwStatus))) == WLAN_MGMT_STATUS_SUCCESS) {
-		pr_info("802.11 Authen (SHAREDKEY) Successful\n");
-		pMgmt->eCurrState = WMAC_STATE_AUTH;
-		timer_expire(pDevice->sTimerCommand, 0);
-	} else{
-		pr_info("802.11 Authen (SHAREDKEY) Failed\n");
-		s_vMgrLogStatus(pMgmt, cpu_to_le16((*(pFrame->pwStatus))));
-		pMgmt->eCurrState = WMAC_STATE_IDLE;
-	}
-}
-
-/*+
- *
- * Routine Description:
- *   Handles incoming disassociation frames
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-static
-void
-s_vMgrRxDisassociation(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PSRxMgmtPacket pRxPacket
-)
-{
-	WLAN_FR_DISASSOC    sFrame;
-	unsigned int uNodeIndex = 0;
-	viawget_wpa_header *wpahdr;
-
-	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-		// if is acting an AP..
-		// a STA is leaving this BSS..
-		sFrame.len = pRxPacket->cbMPDULen;
-		sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-		if (BSSDBbIsSTAInNodeDB(pMgmt, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex))
-			BSSvRemoveOneNode(pDevice, uNodeIndex);
-		else
-			pr_debug("Rx disassoc, sta not found\n");
-
-	} else if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA) {
-		sFrame.len = pRxPacket->cbMPDULen;
-		sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-		vMgrDecodeDisassociation(&sFrame);
-		pr_info("AP disassociated me, reason=%d\n",
-			cpu_to_le16(*(sFrame.pwReason)));
-		//TODO: do something let upper layer know or
-		//try to send associate packet again because of inactivity timeout
-		if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
-			wpahdr = (viawget_wpa_header *)pDevice->skb->data;
-			wpahdr->type = VIAWGET_DISASSOC_MSG;
-			wpahdr->resp_ie_len = 0;
-			wpahdr->req_ie_len = 0;
-			skb_put(pDevice->skb, sizeof(viawget_wpa_header));
-			pDevice->skb->dev = pDevice->wpadev;
-			skb_reset_mac_header(pDevice->skb);
-
-			pDevice->skb->pkt_type = PACKET_HOST;
-			pDevice->skb->protocol = htons(ETH_P_802_2);
-			memset(pDevice->skb->cb, 0, sizeof(pDevice->skb->cb));
-			netif_rx(pDevice->skb);
-			pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
-		}
-
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-		{
-			union iwreq_data  wrqu;
-
-			memset(&wrqu, 0, sizeof(wrqu));
-			wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-			pr_debug("wireless_send_event--->SIOCGIWAP(disassociated)\n");
-			wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
-		}
-#endif
-	}
-	/* else, ignore it */
-}
-
-/*+
- *
- * Routine Description:
- *   Handles incoming deauthentication frames
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-static
-void
-s_vMgrRxDeauthentication(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PSRxMgmtPacket pRxPacket
-)
-{
-	WLAN_FR_DEAUTHEN    sFrame;
-	unsigned int uNodeIndex = 0;
-	viawget_wpa_header *wpahdr;
-
-	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-		//Todo:
-		// if is acting an AP..
-		// a STA is leaving this BSS..
-		sFrame.len = pRxPacket->cbMPDULen;
-		sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-		if (BSSDBbIsSTAInNodeDB(pMgmt, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex))
-			BSSvRemoveOneNode(pDevice, uNodeIndex);
-		else
-			pr_info("Rx deauth, sta not found\n");
-	} else {
-		if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA) {
-			sFrame.len = pRxPacket->cbMPDULen;
-			sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-			vMgrDecodeDeauthen(&sFrame);
-			pr_info("AP deauthed me, reason=%d\n",
-				cpu_to_le16((*(sFrame.pwReason))));
-			// TODO: update BSS list for specific BSSID if pre-authentication case
-			if (ether_addr_equal(sFrame.pHdr->sA3.abyAddr3,
-					     pMgmt->abyCurrBSSID)) {
-				if (pMgmt->eCurrState >= WMAC_STATE_AUTHPENDING) {
-					pMgmt->sNodeDBTable[0].bActive = false;
-					pMgmt->eCurrMode = WMAC_MODE_STANDBY;
-					pMgmt->eCurrState = WMAC_STATE_IDLE;
-					netif_stop_queue(pDevice->dev);
-					pDevice->bLinkPass = false;
-				}
-			}
-
-			if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
-				wpahdr = (viawget_wpa_header *)pDevice->skb->data;
-				wpahdr->type = VIAWGET_DISASSOC_MSG;
-				wpahdr->resp_ie_len = 0;
-				wpahdr->req_ie_len = 0;
-				skb_put(pDevice->skb, sizeof(viawget_wpa_header));
-				pDevice->skb->dev = pDevice->wpadev;
-				skb_reset_mac_header(pDevice->skb);
-				pDevice->skb->pkt_type = PACKET_HOST;
-				pDevice->skb->protocol = htons(ETH_P_802_2);
-				memset(pDevice->skb->cb, 0, sizeof(pDevice->skb->cb));
-				netif_rx(pDevice->skb);
-				pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
-			}
-
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-			{
-				union iwreq_data  wrqu;
-
-				memset(&wrqu, 0, sizeof(wrqu));
-				wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-				PRINT_K("wireless_send_event--->SIOCGIWAP(disauthen)\n");
-				wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
-			}
-#endif
-
-		}
-		/* else, ignore it.  TODO: IBSS authentication service
-		   would be implemented here */
-	}
-}
-
-//2008-8-4 <add> by chester
-/*+
- *
- * Routine Description:
- * check if current channel is match ZoneType.
- *for USA:1~11;
- *      Japan:1~13;
- *      Europe:1~13
- * Return Value:
- *               True:exceed;
- *                False:normal case
- -*/
-static bool
-ChannelExceedZoneType(
-	struct vnt_private *pDevice,
-	unsigned char byCurrChannel
-)
-{
-	bool exceed = false;
-
-	switch (pDevice->byZoneType) {
-	case 0x00:                  //USA:1~11
-		if ((byCurrChannel < 1) || (byCurrChannel > 11))
-			exceed = true;
-		break;
-	case 0x01:                  //Japan:1~13
-	case 0x02:                  //Europe:1~13
-		if ((byCurrChannel < 1) || (byCurrChannel > 13))
-			exceed = true;
-		break;
-	default:                    //reserve for other zonetype
-		break;
-	}
-
-	return exceed;
-}
-
-/*+
- *
- * Routine Description:
- *   Handles and analysis incoming beacon frames.
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-static
-void
-s_vMgrRxBeacon(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PSRxMgmtPacket pRxPacket,
-	bool bInScan
-)
-{
-	PKnownBSS           pBSSList;
-	WLAN_FR_BEACON      sFrame;
-	u64 qwTSFOffset;
-	bool bIsBSSIDEqual = false;
-	bool bIsSSIDEqual = false;
-	bool bTSFLargeDiff = false;
-	bool bTSFOffsetPostive = false;
-	bool bUpdateTSF = false;
-	bool bIsAPBeacon = false;
-	bool bIsChannelEqual = false;
-	unsigned int uLocateByteIndex;
-	unsigned char byTIMBitOn = 0;
-	unsigned short wAIDNumber = 0;
-	unsigned int uNodeIndex;
-	u64 qwTimestamp, qwLocalTSF;
-	u64 qwCurrTSF;
-	unsigned short wStartIndex = 0;
-	unsigned short wAIDIndex = 0;
-	unsigned char byCurrChannel = pRxPacket->byRxChannel;
-	ERPObject           sERP;
-	unsigned int uRateLen = WLAN_RATES_MAXLEN;
-	bool bChannelHit = false;
-	bool bUpdatePhyParameter = false;
-	unsigned char byIEChannel = 0;
-
-	memset(&sFrame, 0, sizeof(WLAN_FR_BEACON));
-	sFrame.len = pRxPacket->cbMPDULen;
-	sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-
-	// decode the beacon frame
-	vMgrDecodeBeacon(&sFrame);
-
-	if ((sFrame.pwBeaconInterval == NULL) ||
-	    (sFrame.pwCapInfo == NULL) ||
-	    (sFrame.pSSID == NULL) ||
-	    (sFrame.pSuppRates == NULL)) {
-		pr_debug("Rx beacon frame error\n");
-		return;
-	}
-
-	if (sFrame.pDSParms != NULL) {
-		if (byCurrChannel > CB_MAX_CHANNEL_24G) {
-			// channel remapping to
-			byIEChannel = get_channel_mapping(pDevice, sFrame.pDSParms->byCurrChannel, PHY_TYPE_11A);
-		} else {
-			byIEChannel = sFrame.pDSParms->byCurrChannel;
-		}
-		if (byCurrChannel != byIEChannel) {
-			// adjust channel info. bcs we rcv adjacent channel packets
-			bChannelHit = false;
-			byCurrChannel = byIEChannel;
-		}
-	} else {
-		// no DS channel info
-		bChannelHit = true;
-	}
-//2008-0730-01<Add>by MikeLiu
-	if (ChannelExceedZoneType(pDevice, byCurrChannel))
-		return;
-
-	if (sFrame.pERP != NULL) {
-		sERP.byERP = sFrame.pERP->byContext;
-		sERP.bERPExist = true;
-
-	} else {
-		sERP.bERPExist = false;
-		sERP.byERP = 0;
-	}
-
-	pBSSList = BSSpAddrIsInBSSList((void *)pDevice, sFrame.pHdr->sA3.abyAddr3, sFrame.pSSID);
-	if (pBSSList == NULL) {
-		pr_debug("Beacon/insert: RxChannel = : %d\n", byCurrChannel);
-		BSSbInsertToBSSList((void *)pDevice,
-				    sFrame.pHdr->sA3.abyAddr3,
-				    *sFrame.pqwTimestamp,
-				    *sFrame.pwBeaconInterval,
-				    *sFrame.pwCapInfo,
-				    byCurrChannel,
-				    sFrame.pSSID,
-				    sFrame.pSuppRates,
-				    sFrame.pExtSuppRates,
-				    &sERP,
-				    sFrame.pRSN,
-				    sFrame.pRSNWPA,
-				    sFrame.pIE_Country,
-				    sFrame.pIE_Quiet,
-				    sFrame.len - WLAN_HDR_ADDR3_LEN,
-				    sFrame.pHdr->sA4.abyAddr4,   // payload of beacon
-				    (void *)pRxPacket
-);
-	} else {
-		BSSbUpdateToBSSList((void *)pDevice,
-				    *sFrame.pqwTimestamp,
-				    *sFrame.pwBeaconInterval,
-				    *sFrame.pwCapInfo,
-				    byCurrChannel,
-				    bChannelHit,
-				    sFrame.pSSID,
-				    sFrame.pSuppRates,
-				    sFrame.pExtSuppRates,
-				    &sERP,
-				    sFrame.pRSN,
-				    sFrame.pRSNWPA,
-				    sFrame.pIE_Country,
-				    sFrame.pIE_Quiet,
-				    pBSSList,
-				    sFrame.len - WLAN_HDR_ADDR3_LEN,
-				    sFrame.pHdr->sA4.abyAddr4,   // payload of probresponse
-				    (void *)pRxPacket
-);
-
-	}
-
-	if (bInScan)
-		return;
-
-	if (byCurrChannel == (unsigned char)pMgmt->uCurrChannel)
-		bIsChannelEqual = true;
-
-	if (bIsChannelEqual && (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)) {
-		// if rx beacon without ERP field
-		if (sERP.bERPExist) {
-			if (WLAN_GET_ERP_USE_PROTECTION(sERP.byERP)) {
-				pDevice->byERPFlag |= WLAN_SET_ERP_USE_PROTECTION(1);
-				pDevice->wUseProtectCntDown = USE_PROTECT_PERIOD;
-			}
-		} else {
-			pDevice->byERPFlag |= WLAN_SET_ERP_USE_PROTECTION(1);
-			pDevice->wUseProtectCntDown = USE_PROTECT_PERIOD;
-		}
-
-		if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-			if (!WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo))
-				pDevice->byERPFlag |= WLAN_SET_ERP_BARKER_MODE(1);
-			if (!sERP.bERPExist)
-				pDevice->byERPFlag |= WLAN_SET_ERP_NONERP_PRESENT(1);
-		}
-
-		// set to MAC&BBP
-		if (WLAN_GET_ERP_USE_PROTECTION(pDevice->byERPFlag)) {
-			if (!pDevice->bProtectMode) {
-				MACvEnableProtectMD(pDevice->PortOffset);
-				pDevice->bProtectMode = true;
-			}
-		}
-	}
-
-	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)
-		return;
-
-	// check if BSSID the same
-	if (memcmp(sFrame.pHdr->sA3.abyAddr3,
-		   pMgmt->abyCurrBSSID,
-		   WLAN_BSSID_LEN) == 0) {
-		bIsBSSIDEqual = true;
-
-// 2008-05-21 <add> by Richardtai
-		pDevice->uCurrRSSI = pRxPacket->uRSSI;
-		pDevice->byCurrSQ = pRxPacket->bySQ;
-
-		if (pMgmt->sNodeDBTable[0].uInActiveCount != 0)
-			pMgmt->sNodeDBTable[0].uInActiveCount = 0;
-	}
-	// check if SSID the same
-	if (sFrame.pSSID->len == ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len) {
-		if (memcmp(sFrame.pSSID->abySSID,
-			   ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->abySSID,
-			   sFrame.pSSID->len
-) == 0) {
-			bIsSSIDEqual = true;
-		}
-	}
-
-	if (WLAN_GET_CAP_INFO_ESS(*sFrame.pwCapInfo) &&
-	    bIsBSSIDEqual &&
-	    bIsSSIDEqual &&
-	    (pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
-	    (pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
-		// add state check to prevent reconnect fail since we'll receive Beacon
-
-		bIsAPBeacon = true;
-
-		if (pBSSList != NULL) {
-			// Compare PHY parameter setting
-			if (pMgmt->wCurrCapInfo != pBSSList->wCapInfo) {
-				bUpdatePhyParameter = true;
-				pMgmt->wCurrCapInfo = pBSSList->wCapInfo;
-			}
-			if (sFrame.pERP != NULL) {
-				if ((sFrame.pERP->byElementID == WLAN_EID_ERP) &&
-				    (pMgmt->byERPContext != sFrame.pERP->byContext)) {
-					bUpdatePhyParameter = true;
-					pMgmt->byERPContext = sFrame.pERP->byContext;
-				}
-			}
-			//
-			// Basic Rate Set may change dynamically
-			//
-			if (pBSSList->eNetworkTypeInUse == PHY_TYPE_11B)
-				uRateLen = WLAN_RATES_MAXLEN_11B;
-
-			pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)pBSSList->abySuppRates,
-								(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-								uRateLen);
-			pMgmt->abyCurrExtSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)pBSSList->abyExtSuppRates,
-								   (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates,
-								   uRateLen);
-			RATEvParseMaxRate((void *)pDevice,
-					  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-					  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates,
-					  true,
-					  &(pMgmt->sNodeDBTable[0].wMaxBasicRate),
-					  &(pMgmt->sNodeDBTable[0].wMaxSuppRate),
-					  &(pMgmt->sNodeDBTable[0].wSuppRate),
-					  &(pMgmt->sNodeDBTable[0].byTopCCKBasicRate),
-					  &(pMgmt->sNodeDBTable[0].byTopOFDMBasicRate)
-				);
-			if (bUpdatePhyParameter) {
-				CARDbSetPhyParameter(pMgmt->pAdapter,
-						     pMgmt->eCurrentPHYMode,
-						     pMgmt->wCurrCapInfo,
-						     pMgmt->byERPContext,
-						     pMgmt->abyCurrSuppRates,
-						     pMgmt->abyCurrExtSuppRates
-					);
-			}
-			if (sFrame.pIE_PowerConstraint != NULL) {
-				CARDvSetPowerConstraint(pMgmt->pAdapter,
-							(unsigned char) pBSSList->uChannel,
-							sFrame.pIE_PowerConstraint->byPower
-);
-			}
-			if (sFrame.pIE_CHSW != NULL) {
-				CARDbChannelSwitch(pMgmt->pAdapter,
-						   sFrame.pIE_CHSW->byMode,
-						   get_channel_mapping(pMgmt->pAdapter, sFrame.pIE_CHSW->byMode, pMgmt->eCurrentPHYMode),
-						   sFrame.pIE_CHSW->byCount
-					);
-
-			} else if (!bIsChannelEqual) {
-				set_channel(pMgmt->pAdapter, pBSSList->uChannel);
-			}
-		}
-	}
-
-//    pr_debug("Beacon 2\n");
-	// check if CF field exists
-	if (WLAN_GET_CAP_INFO_ESS(*sFrame.pwCapInfo)) {
-		if (sFrame.pCFParms->wCFPDurRemaining > 0) {
-			// TODO: deal with CFP period to set NAV
-		}
-	}
-
-	qwTimestamp = le64_to_cpu(*sFrame.pqwTimestamp);
-	qwLocalTSF = pRxPacket->qwLocalTSF;
-
-	// check if beacon TSF larger or small than our local TSF
-	if (qwTimestamp >= qwLocalTSF)
-		bTSFOffsetPostive = true;
-	else
-		bTSFOffsetPostive = false;
-
-	if (bTSFOffsetPostive)
-		qwTSFOffset = CARDqGetTSFOffset(pRxPacket->byRxRate, (qwTimestamp), (qwLocalTSF));
-	else
-		qwTSFOffset = CARDqGetTSFOffset(pRxPacket->byRxRate, (qwLocalTSF), (qwTimestamp));
-
-	if (qwTSFOffset > TRIVIAL_SYNC_DIFFERENCE)
-		bTSFLargeDiff = true;
-
-	// if infra mode
-	if (bIsAPBeacon) {
-		// Infra mode: Local TSF always follow AP's TSF if Difference huge.
-		if (bTSFLargeDiff)
-			bUpdateTSF = true;
-
-		if (pDevice->bEnablePSMode && (sFrame.pTIM != NULL)) {
-			// deal with DTIM, analysis TIM
-			pMgmt->bMulticastTIM = WLAN_MGMT_IS_MULTICAST_TIM(sFrame.pTIM->byBitMapCtl) ? true : false;
-			pMgmt->byDTIMCount = sFrame.pTIM->byDTIMCount;
-			pMgmt->byDTIMPeriod = sFrame.pTIM->byDTIMPeriod;
-			wAIDNumber = pMgmt->wCurrAID & ~(BIT14|BIT15);
-
-			// check if AID in TIM field bit on
-			// wStartIndex = N1
-			wStartIndex = WLAN_MGMT_GET_TIM_OFFSET(sFrame.pTIM->byBitMapCtl) << 1;
-			// AIDIndex = N2
-			wAIDIndex = (wAIDNumber >> 3);
-			if ((wAIDNumber > 0) && (wAIDIndex >= wStartIndex)) {
-				uLocateByteIndex = wAIDIndex - wStartIndex;
-				// len = byDTIMCount + byDTIMPeriod + byDTIMPeriod + byVirtBitMap[0~250]
-				if (sFrame.pTIM->len >= (uLocateByteIndex + 4)) {
-					byTIMBitOn  = (0x01) << ((wAIDNumber) % 8);
-					pMgmt->bInTIM = sFrame.pTIM->byVirtBitMap[uLocateByteIndex] & byTIMBitOn ? true : false;
-				} else {
-					pMgmt->bInTIM = false;
-				}
-			} else {
-				pMgmt->bInTIM = false;
-			}
-
-			if (pMgmt->bInTIM ||
-			    (pMgmt->bMulticastTIM && (pMgmt->byDTIMCount == 0))) {
-				pMgmt->bInTIMWake = true;
-				// send out ps-poll packet
-
-				if (pMgmt->bInTIM)
-					PSvSendPSPOLL(pDevice);
-
-			} else {
-				pMgmt->bInTIMWake = false;
-				pr_debug("BCN: Not In TIM..\n");
-				if (!pDevice->bPWBitOn) {
-					pr_debug("BCN: Send Null Packet\n");
-					if (PSbSendNullPacket(pDevice))
-						pDevice->bPWBitOn = true;
-				}
-				if (PSbConsiderPowerDown(pDevice, false, false))
-					pr_debug("BCN: Power down now...\n");
-			}
-
-		}
-
-	}
-	// if adhoc mode
-	if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) && !bIsAPBeacon && bIsChannelEqual) {
-		if (bIsBSSIDEqual) {
-			// Use sNodeDBTable[0].uInActiveCount as IBSS beacons received count.
-			if (pMgmt->sNodeDBTable[0].uInActiveCount != 0)
-				pMgmt->sNodeDBTable[0].uInActiveCount = 0;
-
-			// adhoc mode:TSF updated only when beacon larger than local TSF
-			if (bTSFLargeDiff && bTSFOffsetPostive &&
-			    (pMgmt->eCurrState == WMAC_STATE_JOINTED))
-				bUpdateTSF = true;
-
-			// During dpc, already in spinlocked.
-			if (BSSDBbIsSTAInNodeDB(pMgmt, sFrame.pHdr->sA3.abyAddr2, &uNodeIndex)) {
-				// Update the STA, (Technically the Beacons of all the IBSS nodes
-				// should be identical, but that's not happening in practice.
-				pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
-									(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-									WLAN_RATES_MAXLEN_11B);
-				RATEvParseMaxRate((void *)pDevice,
-						  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-						  NULL,
-						  true,
-						  &(pMgmt->sNodeDBTable[uNodeIndex].wMaxBasicRate),
-						  &(pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate),
-						  &(pMgmt->sNodeDBTable[uNodeIndex].wSuppRate),
-						  &(pMgmt->sNodeDBTable[uNodeIndex].byTopCCKBasicRate),
-						  &(pMgmt->sNodeDBTable[uNodeIndex].byTopOFDMBasicRate)
-					);
-				pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble = WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo);
-				pMgmt->sNodeDBTable[uNodeIndex].bShortSlotTime = WLAN_GET_CAP_INFO_SHORTSLOTTIME(*sFrame.pwCapInfo);
-				pMgmt->sNodeDBTable[uNodeIndex].uInActiveCount = 0;
-			} else {
-				// Todo, initial Node content
-				BSSvCreateOneNode(pDevice, &uNodeIndex);
-
-				pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
-									(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-									WLAN_RATES_MAXLEN_11B);
-				RATEvParseMaxRate((void *)pDevice,
-						  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-						  NULL,
-						  true,
-						  &(pMgmt->sNodeDBTable[uNodeIndex].wMaxBasicRate),
-						  &(pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate),
-						  &(pMgmt->sNodeDBTable[uNodeIndex].wSuppRate),
-						  &(pMgmt->sNodeDBTable[uNodeIndex].byTopCCKBasicRate),
-						  &(pMgmt->sNodeDBTable[uNodeIndex].byTopOFDMBasicRate)
-					);
-
-				memcpy(pMgmt->sNodeDBTable[uNodeIndex].abyMACAddr, sFrame.pHdr->sA3.abyAddr2, WLAN_ADDR_LEN);
-				pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble = WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo);
-				pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate = pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate;
-				{
-					pr_debug("s_vMgrRxBeacon:TxDataRate is %d,Index is %d\n", pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate, uNodeIndex);
-				}
-			}
-
-			// if other stations joined, indicate connection to upper layer..
-			if (pMgmt->eCurrState == WMAC_STATE_STARTED) {
-				pr_debug("Current IBSS State: [Started]........to: [Jointed]\n");
-				pMgmt->eCurrState = WMAC_STATE_JOINTED;
-				pDevice->bLinkPass = true;
-				if (netif_queue_stopped(pDevice->dev))
-					netif_wake_queue(pDevice->dev);
-
-				pMgmt->sNodeDBTable[0].bActive = true;
-				pMgmt->sNodeDBTable[0].uInActiveCount = 0;
-
-			}
-		} else if (bIsSSIDEqual) {
-			// See other adhoc sta with the same SSID but BSSID is different.
-			// adpot this vars only when TSF larger then us.
-			if (bTSFLargeDiff && bTSFOffsetPostive) {
-				// we don't support ATIM under adhoc mode
-				// if (sFrame.pIBSSParms->wATIMWindow == 0) {
-				// adpot this vars
-				// TODO: check sFrame cap if privacy on, and support rate syn
-				memcpy(pMgmt->abyCurrBSSID, sFrame.pHdr->sA3.abyAddr3, WLAN_BSSID_LEN);
-				memcpy(pDevice->abyBSSID, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-				pMgmt->wCurrATIMWindow = cpu_to_le16(sFrame.pIBSSParms->wATIMWindow);
-				pMgmt->wCurrBeaconPeriod = cpu_to_le16(*sFrame.pwBeaconInterval);
-				pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
-									(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-									WLAN_RATES_MAXLEN_11B);
-				// set HW beacon interval and re-synchronizing....
-				pr_debug("Rejoining to Other Adhoc group with same SSID........\n");
-				VNSvOutPortW(pDevice->PortOffset + MAC_REG_BI, pMgmt->wCurrBeaconPeriod);
-				CARDbUpdateTSF(pDevice, pRxPacket->byRxRate, qwTimestamp, qwLocalTSF);
-				CARDvUpdateNextTBTT(pDevice->PortOffset, qwTimestamp, pMgmt->wCurrBeaconPeriod);
-				// Turn off bssid filter to avoid filter others adhoc station which bssid is different.
-				MACvWriteBSSIDAddress(pDevice->PortOffset, pMgmt->abyCurrBSSID);
-
-				CARDbSetPhyParameter(pMgmt->pAdapter,
-						     pMgmt->eCurrentPHYMode,
-						     pMgmt->wCurrCapInfo,
-						     pMgmt->byERPContext,
-						     pMgmt->abyCurrSuppRates,
-						     pMgmt->abyCurrExtSuppRates);
-
-				// Prepare beacon frame
-				bMgrPrepareBeaconToSend((void *)pDevice, pMgmt);
-			}
-		}
-	}
-	// endian issue ???
-	// Update TSF
-if (bUpdateTSF) {
-		CARDbGetCurrentTSF(pDevice->PortOffset, &qwCurrTSF);
-		CARDbUpdateTSF(pDevice, pRxPacket->byRxRate, qwTimestamp, pRxPacket->qwLocalTSF);
-		CARDbGetCurrentTSF(pDevice->PortOffset, &qwCurrTSF);
-		CARDvUpdateNextTBTT(pDevice->PortOffset, qwTimestamp, pMgmt->wCurrBeaconPeriod);
-	}
-}
-
-/*+
- *
- * Routine Description:
- *   Instructs the hw to create a bss using the supplied
- *   attributes. Note that this implementation only supports Ad-Hoc
- *   BSS creation.
- *
- *
- * Return Value:
- *    CMD_STATUS
- *
- -*/
-void
-vMgrCreateOwnIBSS(
-	void *hDeviceContext,
-	PCMD_STATUS pStatus
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject        pMgmt = pDevice->pMgmt;
-	unsigned short wMaxBasicRate;
-	unsigned short wMaxSuppRate;
-	unsigned char byTopCCKBasicRate;
-	unsigned char byTopOFDMBasicRate;
-	u64 qwCurrTSF;
-	unsigned int ii;
-	unsigned char abyRATE[] = {0x82, 0x84, 0x8B, 0x96, 0x24, 0x30, 0x48, 0x6C, 0x0C, 0x12, 0x18, 0x60};
-	unsigned char abyCCK_RATE[] = {0x82, 0x84, 0x8B, 0x96};
-	unsigned char abyOFDM_RATE[] = {0x0C, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C};
-	unsigned short wSuppRate;
-
-	pr_debug("Create Basic Service Set .......\n");
-
-	if (pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) {
-		if ((pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) &&
-		    (pDevice->eEncryptionStatus != Ndis802_11Encryption2Enabled) &&
-		    (pDevice->eEncryptionStatus != Ndis802_11Encryption3Enabled)) {
-			// encryption mode error
-			*pStatus = CMD_STATUS_FAILURE;
-			return;
-		}
-	}
-
-	pMgmt->abyCurrSuppRates[0] = WLAN_EID_SUPP_RATES;
-	pMgmt->abyCurrExtSuppRates[0] = WLAN_EID_EXTSUPP_RATES;
-
-	if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
-		pMgmt->eCurrentPHYMode = pMgmt->byAPBBType;
-	} else {
-		if (pDevice->byBBType == BB_TYPE_11G)
-			pMgmt->eCurrentPHYMode = PHY_TYPE_11G;
-		if (pDevice->byBBType == BB_TYPE_11B)
-			pMgmt->eCurrentPHYMode = PHY_TYPE_11B;
-		if (pDevice->byBBType == BB_TYPE_11A)
-			pMgmt->eCurrentPHYMode = PHY_TYPE_11A;
-	}
-
-	if (pMgmt->eCurrentPHYMode != PHY_TYPE_11A) {
-		pMgmt->abyCurrSuppRates[1] = WLAN_RATES_MAXLEN_11B;
-		pMgmt->abyCurrExtSuppRates[1] = 0;
-		for (ii = 0; ii < 4; ii++)
-			pMgmt->abyCurrSuppRates[2+ii] = abyRATE[ii];
-	} else {
-		pMgmt->abyCurrSuppRates[1] = 8;
-		pMgmt->abyCurrExtSuppRates[1] = 0;
-		for (ii = 0; ii < 8; ii++)
-			pMgmt->abyCurrSuppRates[2+ii] = abyRATE[ii];
-	}
-
-	if (pMgmt->eCurrentPHYMode == PHY_TYPE_11G) {
-		pMgmt->abyCurrSuppRates[1] = 8;
-		pMgmt->abyCurrExtSuppRates[1] = 4;
-		for (ii = 0; ii < 4; ii++)
-			pMgmt->abyCurrSuppRates[2+ii] =  abyCCK_RATE[ii];
-		for (ii = 4; ii < 8; ii++)
-			pMgmt->abyCurrSuppRates[2+ii] =  abyOFDM_RATE[ii-4];
-		for (ii = 0; ii < 4; ii++)
-			pMgmt->abyCurrExtSuppRates[2+ii] =  abyOFDM_RATE[ii+4];
-	}
-
-	// Disable Protect Mode
-	pDevice->bProtectMode = false;
-	MACvDisableProtectMD(pDevice->PortOffset);
-
-	pDevice->bBarkerPreambleMd = false;
-	MACvDisableBarkerPreambleMd(pDevice->PortOffset);
-
-	// Kyle Test 2003.11.04
-
-	// set HW beacon interval
-	if (pMgmt->wIBSSBeaconPeriod == 0)
-		pMgmt->wIBSSBeaconPeriod = DEFAULT_IBSS_BI;
-
-	CARDbGetCurrentTSF(pDevice->PortOffset, &qwCurrTSF);
-	// clear TSF counter
-	VNSvOutPortB(pDevice->PortOffset + MAC_REG_TFTCTL, TFTCTL_TSFCNTRST);
-	// enable TSF counter
-	VNSvOutPortB(pDevice->PortOffset + MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
-
-	// set Next TBTT
-	CARDvSetFirstNextTBTT(pDevice->PortOffset, pMgmt->wIBSSBeaconPeriod);
-
-	pMgmt->uIBSSChannel = pDevice->uChannel;
-
-	if (pMgmt->uIBSSChannel == 0)
-		pMgmt->uIBSSChannel = DEFAULT_IBSS_CHANNEL;
-
-	// set basic rate
-
-	RATEvParseMaxRate((void *)pDevice, (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-			  (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates, true,
-			  &wMaxBasicRate, &wMaxSuppRate, &wSuppRate,
-			  &byTopCCKBasicRate, &byTopOFDMBasicRate);
-
-	if (pMgmt->eConfigMode == WMAC_CONFIG_AP)
-		pMgmt->eCurrMode = WMAC_MODE_ESS_AP;
-
-	if (pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) {
-		memcpy(pMgmt->abyIBSSDFSOwner, pDevice->abyCurrentNetAddr, 6);
-		pMgmt->byIBSSDFSRecovery = 10;
-		pMgmt->eCurrMode = WMAC_MODE_IBSS_STA;
-	}
-
-	// Adopt pre-configured IBSS vars to current vars
-	pMgmt->eCurrState = WMAC_STATE_STARTED;
-	pMgmt->wCurrBeaconPeriod = pMgmt->wIBSSBeaconPeriod;
-	pMgmt->uCurrChannel = pMgmt->uIBSSChannel;
-	pMgmt->wCurrATIMWindow = pMgmt->wIBSSATIMWindow;
-	MACvWriteATIMW(pDevice->PortOffset, pMgmt->wCurrATIMWindow);
-	pDevice->uCurrRSSI = 0;
-	pDevice->byCurrSQ = 0;
-	memset(pMgmt->abyCurrSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-	memcpy(pMgmt->abyCurrSSID,
-	       pMgmt->abyDesireSSID,
-	       ((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->len + WLAN_IEHDR_LEN
-);
-
-	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-		// AP mode BSSID = MAC addr
-		memcpy(pMgmt->abyCurrBSSID, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-		pr_info("AP beacon created BSSID:%pM\n",
-			pMgmt->abyCurrBSSID);
-	}
-
-	if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-		// BSSID selected must be randomized as spec 11.1.3
-		pMgmt->abyCurrBSSID[5] = (u8) (qwCurrTSF & 0x000000ff);
-		pMgmt->abyCurrBSSID[4] = (u8) ((qwCurrTSF & 0x0000ff00) >> 8);
-		pMgmt->abyCurrBSSID[3] = (u8) ((qwCurrTSF & 0x00ff0000) >> 16);
-		pMgmt->abyCurrBSSID[2] = (u8) ((qwCurrTSF & 0x00000ff0) >> 4);
-		pMgmt->abyCurrBSSID[1] = (u8) ((qwCurrTSF & 0x000ff000) >> 12);
-		pMgmt->abyCurrBSSID[0] = (u8) ((qwCurrTSF & 0x0ff00000) >> 20);
-		pMgmt->abyCurrBSSID[5] ^= pMgmt->abyMACAddr[0];
-		pMgmt->abyCurrBSSID[4] ^= pMgmt->abyMACAddr[1];
-		pMgmt->abyCurrBSSID[3] ^= pMgmt->abyMACAddr[2];
-		pMgmt->abyCurrBSSID[2] ^= pMgmt->abyMACAddr[3];
-		pMgmt->abyCurrBSSID[1] ^= pMgmt->abyMACAddr[4];
-		pMgmt->abyCurrBSSID[0] ^= pMgmt->abyMACAddr[5];
-		pMgmt->abyCurrBSSID[0] &= ~IEEE_ADDR_GROUP;
-		pMgmt->abyCurrBSSID[0] |= IEEE_ADDR_UNIVERSAL;
-
-		pr_info("Adhoc beacon created bssid:%pM\n",
-			pMgmt->abyCurrBSSID);
-	}
-
-	// Set Capability Info
-	pMgmt->wCurrCapInfo = 0;
-
-	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_ESS(1);
-		pMgmt->byDTIMPeriod = DEFAULT_DTIM_PERIOD;
-		pMgmt->byDTIMCount = pMgmt->byDTIMPeriod - 1;
-	}
-
-	if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)
-		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_IBSS(1);
-
-	if (pDevice->bEncryptionEnable) {
-		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
-			if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
-				pMgmt->byCSSPK = KEY_CTL_CCMP;
-				pMgmt->byCSSGK = KEY_CTL_CCMP;
-			} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
-				pMgmt->byCSSPK = KEY_CTL_TKIP;
-				pMgmt->byCSSGK = KEY_CTL_TKIP;
-			} else {
-				pMgmt->byCSSPK = KEY_CTL_NONE;
-				pMgmt->byCSSGK = KEY_CTL_WEP;
-			}
-		} else {
-			pMgmt->byCSSPK = KEY_CTL_WEP;
-			pMgmt->byCSSGK = KEY_CTL_WEP;
-		}
-	}
-
-	pMgmt->byERPContext = 0;
-
-	if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
-		CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, NL80211_IFTYPE_AP);
-	} else {
-		CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, NL80211_IFTYPE_ADHOC);
-	}
-
-	CARDbSetPhyParameter(pMgmt->pAdapter,
-			     pMgmt->eCurrentPHYMode,
-			     pMgmt->wCurrCapInfo,
-			     pMgmt->byERPContext,
-			     pMgmt->abyCurrSuppRates,
-			     pMgmt->abyCurrExtSuppRates
-		);
-
-	CARDbSetBeaconPeriod(pMgmt->pAdapter, pMgmt->wIBSSBeaconPeriod);
-	// set channel and clear NAV
-	set_channel(pMgmt->pAdapter, pMgmt->uIBSSChannel);
-	pMgmt->uCurrChannel = pMgmt->uIBSSChannel;
-
-	if (CARDbIsShortPreamble(pMgmt->pAdapter))
-		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-	else
-		pMgmt->wCurrCapInfo &= (~WLAN_SET_CAP_INFO_SHORTPREAMBLE(1));
-
-	if (pMgmt->b11hEnable &&
-	    (pMgmt->eCurrentPHYMode == PHY_TYPE_11A)) {
-		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SPECTRUMMNG(1);
-	} else {
-		pMgmt->wCurrCapInfo &= (~WLAN_SET_CAP_INFO_SPECTRUMMNG(1));
-	}
-
-	pMgmt->eCurrState = WMAC_STATE_STARTED;
-	// Prepare beacon to send
-	if (bMgrPrepareBeaconToSend((void *)pDevice, pMgmt))
-		*pStatus = CMD_STATUS_SUCCESS;
-}
-
-/*+
- *
- * Routine Description:
- *   Instructs wmac to join a bss using the supplied attributes.
- *   The arguments may the BSSID or SSID and the rest of the
- *   attributes are obtained from the scan result of known bss list.
- *
- *
- * Return Value:
- *    None.
- *
- -*/
-
-void
-vMgrJoinBSSBegin(
-	void *hDeviceContext,
-	PCMD_STATUS pStatus
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	PKnownBSS       pCurr = NULL;
-	unsigned int ii, uu;
-	PWLAN_IE_SUPP_RATES pItemRates = NULL;
-	PWLAN_IE_SUPP_RATES pItemExtRates = NULL;
-	PWLAN_IE_SSID   pItemSSID;
-	unsigned int uRateLen = WLAN_RATES_MAXLEN;
-	unsigned short wMaxBasicRate = RATE_1M;
-	unsigned short wMaxSuppRate = RATE_1M;
-	unsigned short wSuppRate;
-	unsigned char byTopCCKBasicRate = RATE_1M;
-	unsigned char byTopOFDMBasicRate = RATE_1M;
-
-	for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-		if (pMgmt->sBSSList[ii].bActive)
-			break;
-	}
-
-	if (ii == MAX_BSS_NUM) {
-		*pStatus = CMD_STATUS_RESOURCES;
-		pr_info("BSS finding:BSS list is empty\n");
-		return;
-	}
-
-	// Search known BSS list for prefer BSSID or SSID
-
-	pCurr = BSSpSearchBSSList(pDevice,
-				  pMgmt->abyDesireBSSID,
-				  pMgmt->abyDesireSSID,
-				  pMgmt->eConfigPHYMode
-);
-
-	if (pCurr == NULL) {
-		*pStatus = CMD_STATUS_RESOURCES;
-		pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
-		pr_info("Scanning [%s] not found, disconnected !\n",
-			pItemSSID->abySSID);
-		return;
-	}
-
-	pr_info("AP(BSS) finding:Found a AP(BSS)..\n");
-	if (WLAN_GET_CAP_INFO_ESS(cpu_to_le16(pCurr->wCapInfo))) {
-		if ((pMgmt->eAuthenMode == WMAC_AUTH_WPA) || (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK)) {
-			// patch for CISCO migration mode
-		}
-
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-		Encyption_Rebuild(pDevice, pCurr);
-#endif
-		// Infrastructure BSS
-		s_vMgrSynchBSS(pDevice,
-			       WMAC_MODE_ESS_STA,
-			       pCurr,
-			       pStatus
-);
-
-		if (*pStatus == CMD_STATUS_SUCCESS) {
-			// Adopt this BSS state vars in Mgmt Object
-			pMgmt->uCurrChannel = pCurr->uChannel;
-
-			memset(pMgmt->abyCurrSuppRates, 0 , WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
-			memset(pMgmt->abyCurrExtSuppRates, 0 , WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
-
-			if (pCurr->eNetworkTypeInUse == PHY_TYPE_11B)
-				uRateLen = WLAN_RATES_MAXLEN_11B;
-
-			pItemRates = (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates;
-			pItemExtRates = (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates;
-
-			// Parse Support Rate IE
-			pItemRates->byElementID = WLAN_EID_SUPP_RATES;
-			pItemRates->len = RATEuSetIE((PWLAN_IE_SUPP_RATES)pCurr->abySuppRates,
-						     pItemRates,
-						     uRateLen);
-
-			// Parse Extension Support Rate IE
-			pItemExtRates->byElementID = WLAN_EID_EXTSUPP_RATES;
-			pItemExtRates->len = RATEuSetIE((PWLAN_IE_SUPP_RATES)pCurr->abyExtSuppRates,
-							pItemExtRates,
-							uRateLen);
-			// Stuffing Rate IE
-			if ((pItemExtRates->len > 0) && (pItemRates->len < 8)) {
-				for (ii = 0; ii < (unsigned int)(8 - pItemRates->len);) {
-					pItemRates->abyRates[pItemRates->len + ii] = pItemExtRates->abyRates[ii];
-					ii++;
-					if (pItemExtRates->len <= ii)
-						break;
-				}
-				pItemRates->len += (unsigned char)ii;
-				if (pItemExtRates->len - ii > 0) {
-					pItemExtRates->len -= (unsigned char)ii;
-					for (uu = 0; uu < pItemExtRates->len; uu++)
-						pItemExtRates->abyRates[uu] = pItemExtRates->abyRates[uu + ii];
-				} else {
-					pItemExtRates->len = 0;
-				}
-			}
-
-			RATEvParseMaxRate((void *)pDevice, pItemRates, pItemExtRates, true,
-					  &wMaxBasicRate, &wMaxSuppRate, &wSuppRate,
-					  &byTopCCKBasicRate, &byTopOFDMBasicRate);
-
-			// TODO: deal with if wCapInfo the privacy is on, but station WEP is off
-			// TODO: deal with if wCapInfo the PS-Pollable is on.
-			pMgmt->wCurrBeaconPeriod = pCurr->wBeaconInterval;
-			memset(pMgmt->abyCurrSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-			memcpy(pMgmt->abyCurrBSSID, pCurr->abyBSSID, WLAN_BSSID_LEN);
-			memcpy(pMgmt->abyCurrSSID, pCurr->abySSID, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-
-			pMgmt->eCurrMode = WMAC_MODE_ESS_STA;
-
-			pMgmt->eCurrState = WMAC_STATE_JOINTED;
-
-			// Add current BSS to Candidate list
-			// This should only works for WPA2 BSS, and WPA2 BSS check must be done before.
-			if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
-				bool bResult = bAdd_PMKID_Candidate((void *)pDevice, pMgmt->abyCurrBSSID, &pCurr->sRSNCapObj);
-
-				pr_debug("bAdd_PMKID_Candidate: 1(%d)\n",
-					 bResult);
-				if (!bResult) {
-					vFlush_PMKID_Candidate((void *)pDevice);
-					pr_debug("vFlush_PMKID_Candidate: 4\n");
-					bAdd_PMKID_Candidate((void *)pDevice, pMgmt->abyCurrBSSID, &pCurr->sRSNCapObj);
-				}
-			}
-
-			// Preamble type auto-switch: if AP can receive short-preamble cap,
-			// we can turn on too.
-
-			pr_debug("Join ESS\n");
-
-			pr_debug("End of Join AP -- A/B/G Action\n");
-		} else {
-			pMgmt->eCurrState = WMAC_STATE_IDLE;
-		}
-
-	} else {
-		// ad-hoc mode BSS
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
-			if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
-				if (!WPA_SearchRSN(0, WPA_TKIP, pCurr)) {
-					// encryption mode error
-					pMgmt->eCurrState = WMAC_STATE_IDLE;
-					return;
-				}
-			} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
-				if (!WPA_SearchRSN(0, WPA_AESCCMP, pCurr)) {
-					// encryption mode error
-					pMgmt->eCurrState = WMAC_STATE_IDLE;
-					return;
-				}
-			} else {
-				// encryption mode error
-				pMgmt->eCurrState = WMAC_STATE_IDLE;
-				return;
-			}
-		}
-
-		s_vMgrSynchBSS(pDevice,
-			       WMAC_MODE_IBSS_STA,
-			       pCurr,
-			       pStatus
-);
-
-		if (*pStatus == CMD_STATUS_SUCCESS) {
-			// Adopt this BSS state vars in Mgmt Object
-			// TODO: check if CapInfo privacy on, but we don't..
-			pMgmt->uCurrChannel = pCurr->uChannel;
-
-			// Parse Support Rate IE
-			pMgmt->abyCurrSuppRates[0] = WLAN_EID_SUPP_RATES;
-			pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)pCurr->abySuppRates,
-								(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-								WLAN_RATES_MAXLEN_11B);
-			// set basic rate
-			RATEvParseMaxRate((void *)pDevice, (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-					  NULL, true, &wMaxBasicRate, &wMaxSuppRate, &wSuppRate,
-					  &byTopCCKBasicRate, &byTopOFDMBasicRate);
-
-			pMgmt->wCurrCapInfo = pCurr->wCapInfo;
-			pMgmt->wCurrBeaconPeriod = pCurr->wBeaconInterval;
-			memset(pMgmt->abyCurrSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN);
-			memcpy(pMgmt->abyCurrBSSID, pCurr->abyBSSID, WLAN_BSSID_LEN);
-			memcpy(pMgmt->abyCurrSSID, pCurr->abySSID, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN);
-			MACvWriteATIMW(pDevice->PortOffset, pMgmt->wCurrATIMWindow);
-			pMgmt->eCurrMode = WMAC_MODE_IBSS_STA;
-
-			pMgmt->eCurrState = WMAC_STATE_STARTED;
-
-			pr_debug("Join IBSS ok:%pM\n",
-				 pMgmt->abyCurrBSSID);
-			// Preamble type auto-switch: if AP can receive short-preamble cap,
-			// and if registry setting is short preamble we can turn on too.
-
-			// Prepare beacon
-			bMgrPrepareBeaconToSend((void *)pDevice, pMgmt);
-		} else {
-			pMgmt->eCurrState = WMAC_STATE_IDLE;
-		}
-	}
-}
-
-/*+
- *
- * Routine Description:
- * Set HW to synchronize a specific BSS from known BSS list.
- *
- *
- * Return Value:
- *    PCM_STATUS
- *
- -*/
-static
-void
-s_vMgrSynchBSS(
-	struct vnt_private *pDevice,
-	unsigned int uBSSMode,
-	PKnownBSS     pCurr,
-	PCMD_STATUS  pStatus
-)
-{
-	CARD_PHY_TYPE   ePhyType = PHY_TYPE_11B;
-	PSMgmtObject  pMgmt = pDevice->pMgmt;
-
-	//1M,   2M,   5M,   11M,  18M,  24M,  36M,  54M
-	unsigned char abyCurrSuppRatesG[] = {WLAN_EID_SUPP_RATES, 8, 0x02, 0x04, 0x0B, 0x16, 0x24, 0x30, 0x48, 0x6C};
-	unsigned char abyCurrExtSuppRatesG[] = {WLAN_EID_EXTSUPP_RATES, 4, 0x0C, 0x12, 0x18, 0x60};
-	//6M,   9M,   12M,  48M
-	unsigned char abyCurrSuppRatesA[] = {WLAN_EID_SUPP_RATES, 8, 0x0C, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C};
-	unsigned char abyCurrSuppRatesB[] = {WLAN_EID_SUPP_RATES, 4, 0x02, 0x04, 0x0B, 0x16};
-
-	*pStatus = CMD_STATUS_FAILURE;
-
-	if (!s_bCipherMatch(pCurr,
-			   pDevice->eEncryptionStatus,
-			   &(pMgmt->byCSSPK),
-			   &(pMgmt->byCSSGK))) {
-		pr_debug("s_bCipherMatch Fail .......\n");
-		return;
-	}
-
-	pMgmt->pCurrBSS = pCurr;
-
-	// if previous mode is IBSS.
-	if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_BCNDMACTL, BEACON_READY);
-		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
-	}
-
-	// Init the BSS informations
-	pDevice->bProtectMode = false;
-	MACvDisableProtectMD(pDevice->PortOffset);
-	pDevice->bBarkerPreambleMd = false;
-	MACvDisableBarkerPreambleMd(pDevice->PortOffset);
-	pDevice->bNonERPPresent = false;
-	pDevice->byPreambleType = 0;
-	pDevice->wBasicRate = 0;
-	// Set Basic Rate
-	CARDbAddBasicRate((void *)pDevice, RATE_1M);
-	// calculate TSF offset
-	// TSF Offset = Received Timestamp TSF - Marked Local's TSF
-	CARDbUpdateTSF(pDevice, pCurr->byRxRate, pCurr->qwBSSTimestamp, pCurr->qwLocalTSF);
-
-	CARDbSetBeaconPeriod(pDevice, pCurr->wBeaconInterval);
-
-	// set Next TBTT
-	// Next TBTT = ((local_current_TSF / beacon_interval) + 1) * beacon_interval
-	CARDvSetFirstNextTBTT(pDevice->PortOffset, pCurr->wBeaconInterval);
-
-	// set BSSID
-	MACvWriteBSSIDAddress(pDevice->PortOffset, pCurr->abyBSSID);
-
-	MACvReadBSSIDAddress(pDevice->PortOffset, pMgmt->abyCurrBSSID);
-
-	pr_debug("Sync:set CurrBSSID address = %pM\n", pMgmt->abyCurrBSSID);
-
-	if (pCurr->eNetworkTypeInUse == PHY_TYPE_11A) {
-		if ((pMgmt->eConfigPHYMode == PHY_TYPE_11A) ||
-		    (pMgmt->eConfigPHYMode == PHY_TYPE_AUTO)) {
-			ePhyType = PHY_TYPE_11A;
-		} else {
-			return;
-		}
-	} else if (pCurr->eNetworkTypeInUse == PHY_TYPE_11B) {
-		if ((pMgmt->eConfigPHYMode == PHY_TYPE_11B) ||
-		    (pMgmt->eConfigPHYMode == PHY_TYPE_11G) ||
-		    (pMgmt->eConfigPHYMode == PHY_TYPE_AUTO)) {
-			ePhyType = PHY_TYPE_11B;
-		} else {
-			return;
-		}
-	} else {
-		if ((pMgmt->eConfigPHYMode == PHY_TYPE_11G) ||
-		    (pMgmt->eConfigPHYMode == PHY_TYPE_AUTO)) {
-			ePhyType = PHY_TYPE_11G;
-		} else if (pMgmt->eConfigPHYMode == PHY_TYPE_11B) {
-			ePhyType = PHY_TYPE_11B;
-		} else {
-			return;
-		}
-	}
-
-	if (ePhyType == PHY_TYPE_11A) {
-		memcpy(pMgmt->abyCurrSuppRates, &abyCurrSuppRatesA[0], sizeof(abyCurrSuppRatesA));
-		pMgmt->abyCurrExtSuppRates[1] = 0;
-	} else if (ePhyType == PHY_TYPE_11B) {
-		memcpy(pMgmt->abyCurrSuppRates, &abyCurrSuppRatesB[0], sizeof(abyCurrSuppRatesB));
-		pMgmt->abyCurrExtSuppRates[1] = 0;
-	} else {
-		memcpy(pMgmt->abyCurrSuppRates, &abyCurrSuppRatesG[0], sizeof(abyCurrSuppRatesG));
-		memcpy(pMgmt->abyCurrExtSuppRates, &abyCurrExtSuppRatesG[0], sizeof(abyCurrExtSuppRatesG));
-	}
-
-	if (WLAN_GET_CAP_INFO_ESS(pCurr->wCapInfo)) {
-		CARDbSetBSSID(pMgmt->pAdapter, pCurr->abyBSSID, NL80211_IFTYPE_STATION);
-		// Add current BSS to Candidate list
-		// This should only works for WPA2 BSS, and WPA2 BSS check must be done before.
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)
-			CARDbAdd_PMKID_Candidate(pMgmt->pAdapter, pMgmt->abyCurrBSSID, pCurr->sRSNCapObj.bRSNCapExist, pCurr->sRSNCapObj.wRSNCap);
-	} else {
-		CARDbSetBSSID(pMgmt->pAdapter, pCurr->abyBSSID, NL80211_IFTYPE_ADHOC);
-	}
-
-	if (!CARDbSetPhyParameter(pMgmt->pAdapter,
-				 ePhyType,
-				 pCurr->wCapInfo,
-				 pCurr->sERP.byERP,
-				 pMgmt->abyCurrSuppRates,
-				 pMgmt->abyCurrExtSuppRates)) {
-		pr_debug("<----s_bSynchBSS Set Phy Mode Fail [%d]\n", ePhyType);
-		return;
-	}
-	// set channel and clear NAV
-	if (!set_channel(pMgmt->pAdapter, pCurr->uChannel)) {
-		pr_debug("<----s_bSynchBSS Set Channel [%d]\n",
-			 pCurr->uChannel);
-		return;
-	}
-
-	pMgmt->uCurrChannel = pCurr->uChannel;
-	pMgmt->eCurrentPHYMode = ePhyType;
-	pMgmt->byERPContext = pCurr->sERP.byERP;
-	pr_debug("Sync:Set to channel = [%d]\n", (int)pCurr->uChannel);
-
-	*pStatus = CMD_STATUS_SUCCESS;
-
-	return;
-};
-
-//mike add: fix NetworkManager 0.7.0 hidden ssid mode in WPA encryption
-//                   ,need reset eAuthenMode and eEncryptionStatus
-static void  Encyption_Rebuild(
-	struct vnt_private *pDevice,
-	PKnownBSS pCurr
-)
-{
-	PSMgmtObject  pMgmt = &(pDevice->sMgmtObj);
-
-	if ((pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||           //networkmanager 0.7.0 does not give the pairwise-key selection,
-	    (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) {         // so we need re-select it according to real pairwise-key info.
-		if (pCurr->bWPAValid)  {   //WPA-PSK
-			pMgmt->eAuthenMode = WMAC_AUTH_WPAPSK;
-			if (pCurr->abyPKType[0] == WPA_TKIP) {
-				pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;    //TKIP
-				PRINT_K("Encyption_Rebuild--->ssid reset config to [WPAPSK-TKIP]\n");
-			} else if (pCurr->abyPKType[0] == WPA_AESCCMP) {
-				pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;    //AES
-				PRINT_K("Encyption_Rebuild--->ssid reset config to [WPAPSK-AES]\n");
-			}
-		} else if (pCurr->bWPA2Valid) {  //WPA2-PSK
-			pMgmt->eAuthenMode = WMAC_AUTH_WPA2PSK;
-			if (pCurr->abyCSSPK[0] == WLAN_11i_CSS_TKIP) {
-				pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;     //TKIP
-				PRINT_K("Encyption_Rebuild--->ssid reset config to [WPA2PSK-TKIP]\n");
-			} else if (pCurr->abyCSSPK[0] == WLAN_11i_CSS_CCMP) {
-				pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;    //AES
-				PRINT_K("Encyption_Rebuild--->ssid reset config to [WPA2PSK-AES]\n");
-			}
-		}
-	}
-}
-
-/*+
- *
- * Routine Description:
- *  Format TIM field
- *
- *
- * Return Value:
- *    void
- *
- -*/
-
-static
-void
-s_vMgrFormatTIM(
-	PSMgmtObject pMgmt,
-	PWLAN_IE_TIM pTIM
-)
-{
-	unsigned char byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
-	unsigned char byMap;
-	unsigned int ii, jj;
-	bool bStartFound = false;
-	bool bMulticast = false;
-	unsigned short wStartIndex = 0;
-	unsigned short wEndIndex = 0;
-
-	// Find size of partial virtual bitmap
-	for (ii = 0; ii < (MAX_NODE_NUM + 1); ii++) {
-		byMap = pMgmt->abyPSTxMap[ii];
-		if (!ii) {
-			// Mask out the broadcast bit which is indicated separately.
-			bMulticast = (byMap & byMask[0]) != 0;
-			if (bMulticast)
-				pMgmt->sNodeDBTable[0].bRxPSPoll = true;
-
-			byMap = 0;
-		}
-		if (byMap) {
-			if (!bStartFound) {
-				bStartFound = true;
-				wStartIndex = ii;
-			}
-			wEndIndex = ii;
-		}
-	}
-
-	// Round start index down to nearest even number
-	wStartIndex &=  ~BIT0;
-
-	// Round end index up to nearest even number
-	wEndIndex = ((wEndIndex + 1) & ~BIT0);
-
-	// Size of element payload
-
-	pTIM->len =  3 + (wEndIndex - wStartIndex) + 1;
-
-	// Fill in the Fixed parts of the TIM
-	pTIM->byDTIMCount = pMgmt->byDTIMCount;
-	pTIM->byDTIMPeriod = pMgmt->byDTIMPeriod;
-	pTIM->byBitMapCtl = (bMulticast ? TIM_MULTICAST_MASK : 0) |
-		(((wStartIndex >> 1) << 1) & TIM_BITMAPOFFSET_MASK);
-
-	// Append variable part of TIM
-
-	for (ii = wStartIndex, jj = 0; ii <= wEndIndex; ii++, jj++)
-		pTIM->byVirtBitMap[jj] = pMgmt->abyPSTxMap[ii];
-
-	// Aid = 0 don't used.
-	pTIM->byVirtBitMap[0]  &= ~BIT0;
-}
-
-/*+
- *
- * Routine Description:
- *  Constructs an Beacon frame(Ad-hoc mode)
- *
- *
- * Return Value:
- *    PTR to frame; or NULL on allocation failure
- *
- -*/
-
-static
-PSTxMgmtPacket
-s_MgrMakeBeacon(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	unsigned short wCurrCapInfo,
-	unsigned short wCurrBeaconPeriod,
-	unsigned int uCurrChannel,
-	unsigned short wCurrATIMWinodw,
-	PWLAN_IE_SSID pCurrSSID,
-	unsigned char *pCurrBSSID,
-	PWLAN_IE_SUPP_RATES pCurrSuppRates,
-	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-)
-{
-	PSTxMgmtPacket      pTxPacket = NULL;
-	WLAN_FR_BEACON      sFrame;
-	unsigned char abyBroadcastAddr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-	unsigned char *pbyBuffer;
-	unsigned int uLength = 0;
-	PWLAN_IE_IBSS_DFS   pIBSSDFS = NULL;
-	unsigned int ii;
-
-	// prepare beacon frame
-	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_BEACON_FR_MAXLEN);
-	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-	// Setup the sFrame structure.
-	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-	sFrame.len = WLAN_BEACON_FR_MAXLEN;
-	vMgrEncodeBeacon(&sFrame);
-	// Setup the header
-	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-		(
-			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_BEACON)
-));
-
-	if (pDevice->bEnablePSMode)
-		sFrame.pHdr->sA3.wFrameCtl |= cpu_to_le16((unsigned short)WLAN_SET_FC_PWRMGT(1));
-
-	memcpy(sFrame.pHdr->sA3.abyAddr1, abyBroadcastAddr, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr3, pCurrBSSID, WLAN_BSSID_LEN);
-	*sFrame.pwBeaconInterval = cpu_to_le16(wCurrBeaconPeriod);
-	*sFrame.pwCapInfo = cpu_to_le16(wCurrCapInfo);
-	// Copy SSID
-	sFrame.pSSID = (PWLAN_IE_SSID)(sFrame.pBuf + sFrame.len);
-	sFrame.len += ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len + WLAN_IEHDR_LEN;
-	memcpy(sFrame.pSSID,
-	       pCurrSSID,
-	       ((PWLAN_IE_SSID)pCurrSSID)->len + WLAN_IEHDR_LEN
-);
-	// Copy the rate set
-	sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-	sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN;
-	memcpy(sFrame.pSuppRates,
-	       pCurrSuppRates,
-	       ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN
-);
-	// DS parameter
-	if (pDevice->eCurrentPHYType != PHY_TYPE_11A) {
-		sFrame.pDSParms = (PWLAN_IE_DS_PARMS)(sFrame.pBuf + sFrame.len);
-		sFrame.len += (1) + WLAN_IEHDR_LEN;
-		sFrame.pDSParms->byElementID = WLAN_EID_DS_PARMS;
-		sFrame.pDSParms->len = 1;
-		sFrame.pDSParms->byCurrChannel = (unsigned char)uCurrChannel;
-	}
-	// TIM field
-	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-		sFrame.pTIM = (PWLAN_IE_TIM)(sFrame.pBuf + sFrame.len);
-		sFrame.pTIM->byElementID = WLAN_EID_TIM;
-		s_vMgrFormatTIM(pMgmt, sFrame.pTIM);
-		sFrame.len += (WLAN_IEHDR_LEN + sFrame.pTIM->len);
-	}
-
-	if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
-		// IBSS parameter
-		sFrame.pIBSSParms = (PWLAN_IE_IBSS_PARMS)(sFrame.pBuf + sFrame.len);
-		sFrame.len += (2) + WLAN_IEHDR_LEN;
-		sFrame.pIBSSParms->byElementID = WLAN_EID_IBSS_PARMS;
-		sFrame.pIBSSParms->len = 2;
-		sFrame.pIBSSParms->wATIMWindow = wCurrATIMWinodw;
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
-			/* RSN parameter */
-			sFrame.pRSNWPA = (PWLAN_IE_RSN_EXT)(sFrame.pBuf + sFrame.len);
-			sFrame.pRSNWPA->byElementID = WLAN_EID_RSN_WPA;
-			sFrame.pRSNWPA->len = 12;
-			sFrame.pRSNWPA->abyOUI[0] = 0x00;
-			sFrame.pRSNWPA->abyOUI[1] = 0x50;
-			sFrame.pRSNWPA->abyOUI[2] = 0xf2;
-			sFrame.pRSNWPA->abyOUI[3] = 0x01;
-			sFrame.pRSNWPA->wVersion = 1;
-			sFrame.pRSNWPA->abyMulticast[0] = 0x00;
-			sFrame.pRSNWPA->abyMulticast[1] = 0x50;
-			sFrame.pRSNWPA->abyMulticast[2] = 0xf2;
-			if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled)
-				sFrame.pRSNWPA->abyMulticast[3] = 0x04;//AES
-			else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled)
-				sFrame.pRSNWPA->abyMulticast[3] = 0x02;//TKIP
-			else if (pDevice->eEncryptionStatus == Ndis802_11Encryption1Enabled)
-				sFrame.pRSNWPA->abyMulticast[3] = 0x01;//WEP40
-			else
-				sFrame.pRSNWPA->abyMulticast[3] = 0x00;//NONE
-
-			// Pairwise Key Cipher Suite
-			sFrame.pRSNWPA->wPKCount = 0;
-			// Auth Key Management Suite
-			*((unsigned short *)(sFrame.pBuf + sFrame.len + sFrame.pRSNWPA->len)) = 0;
-			sFrame.pRSNWPA->len += 2;
-
-			// RSN Capabilities
-			*((unsigned short *)(sFrame.pBuf + sFrame.len + sFrame.pRSNWPA->len)) = 0;
-			sFrame.pRSNWPA->len += 2;
-			sFrame.len += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
-		}
-	}
-
-	if (pMgmt->b11hEnable && (pMgmt->eCurrentPHYMode == PHY_TYPE_11A)) {
-		// Country IE
-		pbyBuffer = (unsigned char *)(sFrame.pBuf + sFrame.len);
-		set_country_IE(pMgmt->pAdapter, pbyBuffer);
-		set_country_info(pMgmt->pAdapter, PHY_TYPE_11A, pbyBuffer);
-		uLength += ((PWLAN_IE_COUNTRY) pbyBuffer)->len + WLAN_IEHDR_LEN;
-		pbyBuffer += (((PWLAN_IE_COUNTRY) pbyBuffer)->len + WLAN_IEHDR_LEN);
-		// Power Constrain IE
-		((PWLAN_IE_PW_CONST) pbyBuffer)->byElementID = WLAN_EID_PWR_CONSTRAINT;
-		((PWLAN_IE_PW_CONST) pbyBuffer)->len = 1;
-		((PWLAN_IE_PW_CONST) pbyBuffer)->byPower = 0;
-		pbyBuffer += (1) + WLAN_IEHDR_LEN;
-		uLength += (1) + WLAN_IEHDR_LEN;
-		if (pMgmt->bSwitchChannel) {
-			// Channel Switch IE
-			((PWLAN_IE_CH_SW) pbyBuffer)->byElementID = WLAN_EID_CH_SWITCH;
-			((PWLAN_IE_CH_SW) pbyBuffer)->len = 3;
-			((PWLAN_IE_CH_SW) pbyBuffer)->byMode = 1;
-			((PWLAN_IE_CH_SW) pbyBuffer)->byChannel = get_channel_number(pMgmt->pAdapter, pMgmt->byNewChannel);
-			((PWLAN_IE_CH_SW) pbyBuffer)->byCount = 0;
-			pbyBuffer += (3) + WLAN_IEHDR_LEN;
-			uLength += (3) + WLAN_IEHDR_LEN;
-		}
-		// TPC report
-		((PWLAN_IE_TPC_REP) pbyBuffer)->byElementID = WLAN_EID_TPC_REP;
-		((PWLAN_IE_TPC_REP) pbyBuffer)->len = 2;
-		((PWLAN_IE_TPC_REP) pbyBuffer)->byTxPower = CARDbyGetTransmitPower(pMgmt->pAdapter);
-		((PWLAN_IE_TPC_REP) pbyBuffer)->byLinkMargin = 0;
-		pbyBuffer += (2) + WLAN_IEHDR_LEN;
-		uLength += (2) + WLAN_IEHDR_LEN;
-		// IBSS DFS
-		if (pMgmt->eCurrMode != WMAC_MODE_ESS_AP) {
-			pIBSSDFS = (PWLAN_IE_IBSS_DFS) pbyBuffer;
-			pIBSSDFS->byElementID = WLAN_EID_IBSS_DFS;
-			pIBSSDFS->len = 7;
-			memcpy(pIBSSDFS->abyDFSOwner,
-			       pMgmt->abyIBSSDFSOwner,
-			       6);
-			pIBSSDFS->byDFSRecovery = pMgmt->byIBSSDFSRecovery;
-			pbyBuffer += (7) + WLAN_IEHDR_LEN;
-			uLength += (7) + WLAN_IEHDR_LEN;
-			for (ii = CB_MAX_CHANNEL_24G+1; ii <= CB_MAX_CHANNEL; ii++) {
-				if (get_channel_map_info(pMgmt->pAdapter, ii, pbyBuffer, pbyBuffer+1)) {
-					pbyBuffer += 2;
-					uLength += 2;
-					pIBSSDFS->len += 2;
-				}
-			}
-		}
-		sFrame.len += uLength;
-	}
-
-	if (pMgmt->eCurrentPHYMode == PHY_TYPE_11G) {
-		sFrame.pERP = (PWLAN_IE_ERP)(sFrame.pBuf + sFrame.len);
-		sFrame.len += 1 + WLAN_IEHDR_LEN;
-		sFrame.pERP->byElementID = WLAN_EID_ERP;
-		sFrame.pERP->len = 1;
-		sFrame.pERP->byContext = 0;
-		if (pDevice->bProtectMode)
-			sFrame.pERP->byContext |= WLAN_EID_ERP_USE_PROTECTION;
-		if (pDevice->bNonERPPresent)
-			sFrame.pERP->byContext |= WLAN_EID_ERP_NONERP_PRESENT;
-		if (pDevice->bBarkerPreambleMd)
-			sFrame.pERP->byContext |= WLAN_EID_ERP_BARKER_MODE;
-	}
-	if (((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len != 0) {
-		sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-		sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN;
-		memcpy(sFrame.pExtSuppRates,
-		       pCurrExtSuppRates,
-		       ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN
-);
-	}
-	// hostapd wpa/wpa2 IE
-	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) && pDevice->bEnableHostapd) {
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
-			if (pMgmt->wWPAIELen != 0) {
-				sFrame.pRSN = (PWLAN_IE_RSN)(sFrame.pBuf + sFrame.len);
-				memcpy(sFrame.pRSN, pMgmt->abyWPAIE, pMgmt->wWPAIELen);
-				sFrame.len += pMgmt->wWPAIELen;
-			}
-		}
-	}
-
-	/* Adjust the length fields */
-	pTxPacket->cbMPDULen = sFrame.len;
-	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-
-	return pTxPacket;
-}
-
-/*+
- *
- * Routine Description:
- *  Constructs an Prob-response frame
- *
- *
- * Return Value:
- *    PTR to frame; or NULL on allocation failure
- *
- -*/
-
-static PSTxMgmtPacket
-s_MgrMakeProbeResponse(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	unsigned short wCurrCapInfo,
-	unsigned short wCurrBeaconPeriod,
-	unsigned int uCurrChannel,
-	unsigned short wCurrATIMWinodw,
-	unsigned char *pDstAddr,
-	PWLAN_IE_SSID pCurrSSID,
-	unsigned char *pCurrBSSID,
-	PWLAN_IE_SUPP_RATES pCurrSuppRates,
-	PWLAN_IE_SUPP_RATES pCurrExtSuppRates,
-	unsigned char byPHYType
-)
-{
-	PSTxMgmtPacket      pTxPacket = NULL;
-	WLAN_FR_PROBERESP   sFrame;
-	unsigned char *pbyBuffer;
-	unsigned int uLength = 0;
-	PWLAN_IE_IBSS_DFS   pIBSSDFS = NULL;
-	unsigned int ii;
-
-	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_PROBERESP_FR_MAXLEN);
-	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-	// Setup the sFrame structure.
-	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-	sFrame.len = WLAN_PROBERESP_FR_MAXLEN;
-	vMgrEncodeProbeResponse(&sFrame);
-	// Setup the header
-	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-		(
-			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_PROBERESP)
-));
-	memcpy(sFrame.pHdr->sA3.abyAddr1, pDstAddr, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr3, pCurrBSSID, WLAN_BSSID_LEN);
-	*sFrame.pwBeaconInterval = cpu_to_le16(wCurrBeaconPeriod);
-	*sFrame.pwCapInfo = cpu_to_le16(wCurrCapInfo);
-
-	if (byPHYType == BB_TYPE_11B)
-		*sFrame.pwCapInfo &= cpu_to_le16((unsigned short)~(WLAN_SET_CAP_INFO_SHORTSLOTTIME(1)));
-
-	// Copy SSID
-	sFrame.pSSID = (PWLAN_IE_SSID)(sFrame.pBuf + sFrame.len);
-	sFrame.len += ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len + WLAN_IEHDR_LEN;
-	memcpy(sFrame.pSSID,
-	       pCurrSSID,
-	       ((PWLAN_IE_SSID)pCurrSSID)->len + WLAN_IEHDR_LEN
-);
-	// Copy the rate set
-	sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-
-	sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN;
-	memcpy(sFrame.pSuppRates,
-	       pCurrSuppRates,
-	       ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN
-);
-
-	// DS parameter
-	if (pDevice->eCurrentPHYType != PHY_TYPE_11A) {
-		sFrame.pDSParms = (PWLAN_IE_DS_PARMS)(sFrame.pBuf + sFrame.len);
-		sFrame.len += (1) + WLAN_IEHDR_LEN;
-		sFrame.pDSParms->byElementID = WLAN_EID_DS_PARMS;
-		sFrame.pDSParms->len = 1;
-		sFrame.pDSParms->byCurrChannel = (unsigned char)uCurrChannel;
-	}
-
-	if (pMgmt->eCurrMode != WMAC_MODE_ESS_AP) {
-		// IBSS parameter
-		sFrame.pIBSSParms = (PWLAN_IE_IBSS_PARMS)(sFrame.pBuf + sFrame.len);
-		sFrame.len += (2) + WLAN_IEHDR_LEN;
-		sFrame.pIBSSParms->byElementID = WLAN_EID_IBSS_PARMS;
-		sFrame.pIBSSParms->len = 2;
-		sFrame.pIBSSParms->wATIMWindow = 0;
-	}
-	if (pDevice->eCurrentPHYType == PHY_TYPE_11G) {
-		sFrame.pERP = (PWLAN_IE_ERP)(sFrame.pBuf + sFrame.len);
-		sFrame.len += 1 + WLAN_IEHDR_LEN;
-		sFrame.pERP->byElementID = WLAN_EID_ERP;
-		sFrame.pERP->len = 1;
-		sFrame.pERP->byContext = 0;
-		if (pDevice->bProtectMode)
-			sFrame.pERP->byContext |= WLAN_EID_ERP_USE_PROTECTION;
-		if (pDevice->bNonERPPresent)
-			sFrame.pERP->byContext |= WLAN_EID_ERP_NONERP_PRESENT;
-		if (pDevice->bBarkerPreambleMd)
-			sFrame.pERP->byContext |= WLAN_EID_ERP_BARKER_MODE;
-	}
-
-	if (pMgmt->b11hEnable && (pMgmt->eCurrentPHYMode == PHY_TYPE_11A)) {
-		// Country IE
-		pbyBuffer = (unsigned char *)(sFrame.pBuf + sFrame.len);
-		set_country_IE(pMgmt->pAdapter, pbyBuffer);
-		set_country_info(pMgmt->pAdapter, PHY_TYPE_11A, pbyBuffer);
-		uLength += ((PWLAN_IE_COUNTRY) pbyBuffer)->len + WLAN_IEHDR_LEN;
-		pbyBuffer += (((PWLAN_IE_COUNTRY) pbyBuffer)->len + WLAN_IEHDR_LEN);
-		// Power Constrain IE
-		((PWLAN_IE_PW_CONST) pbyBuffer)->byElementID = WLAN_EID_PWR_CONSTRAINT;
-		((PWLAN_IE_PW_CONST) pbyBuffer)->len = 1;
-		((PWLAN_IE_PW_CONST) pbyBuffer)->byPower = 0;
-		pbyBuffer += (1) + WLAN_IEHDR_LEN;
-		uLength += (1) + WLAN_IEHDR_LEN;
-		if (pMgmt->bSwitchChannel) {
-			// Channel Switch IE
-			((PWLAN_IE_CH_SW) pbyBuffer)->byElementID = WLAN_EID_CH_SWITCH;
-			((PWLAN_IE_CH_SW) pbyBuffer)->len = 3;
-			((PWLAN_IE_CH_SW) pbyBuffer)->byMode = 1;
-			((PWLAN_IE_CH_SW) pbyBuffer)->byChannel = get_channel_number(pMgmt->pAdapter, pMgmt->byNewChannel);
-			((PWLAN_IE_CH_SW) pbyBuffer)->byCount = 0;
-			pbyBuffer += (3) + WLAN_IEHDR_LEN;
-			uLength += (3) + WLAN_IEHDR_LEN;
-		}
-		// TPC report
-		((PWLAN_IE_TPC_REP) pbyBuffer)->byElementID = WLAN_EID_TPC_REP;
-		((PWLAN_IE_TPC_REP) pbyBuffer)->len = 2;
-		((PWLAN_IE_TPC_REP) pbyBuffer)->byTxPower = CARDbyGetTransmitPower(pMgmt->pAdapter);
-		((PWLAN_IE_TPC_REP) pbyBuffer)->byLinkMargin = 0;
-		pbyBuffer += (2) + WLAN_IEHDR_LEN;
-		uLength += (2) + WLAN_IEHDR_LEN;
-		// IBSS DFS
-		if (pMgmt->eCurrMode != WMAC_MODE_ESS_AP) {
-			pIBSSDFS = (PWLAN_IE_IBSS_DFS) pbyBuffer;
-			pIBSSDFS->byElementID = WLAN_EID_IBSS_DFS;
-			pIBSSDFS->len = 7;
-			memcpy(pIBSSDFS->abyDFSOwner,
-			       pMgmt->abyIBSSDFSOwner,
-			       6);
-			pIBSSDFS->byDFSRecovery = pMgmt->byIBSSDFSRecovery;
-			pbyBuffer += (7) + WLAN_IEHDR_LEN;
-			uLength += (7) + WLAN_IEHDR_LEN;
-			for (ii = CB_MAX_CHANNEL_24G + 1; ii <= CB_MAX_CHANNEL; ii++) {
-				if (get_channel_map_info(pMgmt->pAdapter, ii, pbyBuffer, pbyBuffer+1)) {
-					pbyBuffer += 2;
-					uLength += 2;
-					pIBSSDFS->len += 2;
-				}
-			}
-		}
-		sFrame.len += uLength;
-	}
-
-	if (((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len != 0) {
-		sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-		sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN;
-		memcpy(sFrame.pExtSuppRates,
-		       pCurrExtSuppRates,
-		       ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN
-);
-	}
-
-	// hostapd wpa/wpa2 IE
-	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) && pDevice->bEnableHostapd) {
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
-			if (pMgmt->wWPAIELen != 0) {
-				sFrame.pRSN = (PWLAN_IE_RSN)(sFrame.pBuf + sFrame.len);
-				memcpy(sFrame.pRSN, pMgmt->abyWPAIE, pMgmt->wWPAIELen);
-				sFrame.len += pMgmt->wWPAIELen;
-			}
-		}
-	}
-
-	// Adjust the length fields
-	pTxPacket->cbMPDULen = sFrame.len;
-	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-
-	return pTxPacket;
-}
-
-/*+
- *
- * Routine Description:
- *  Constructs an association request frame
- *
- *
- * Return Value:
- *    A ptr to frame or NULL on allocation failure
- *
- -*/
-
-static PSTxMgmtPacket
-s_MgrMakeAssocRequest(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	unsigned char *pDAddr,
-	unsigned short wCurrCapInfo,
-	unsigned short wListenInterval,
-	PWLAN_IE_SSID pCurrSSID,
-	PWLAN_IE_SUPP_RATES pCurrRates,
-	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-)
-{
-	PSTxMgmtPacket      pTxPacket = NULL;
-	WLAN_FR_ASSOCREQ    sFrame;
-	unsigned char *pbyIEs;
-	unsigned char *pbyRSN;
-
-	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_ASSOCREQ_FR_MAXLEN);
-	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-	// Setup the sFrame structure.
-	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-	sFrame.len = WLAN_ASSOCREQ_FR_MAXLEN;
-	// format fixed field frame structure
-	vMgrEncodeAssocRequest(&sFrame);
-	// Setup the header
-	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-		(
-			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_ASSOCREQ)
-));
-	memcpy(sFrame.pHdr->sA3.abyAddr1, pDAddr, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-
-	// Set the capability and listen interval
-	*(sFrame.pwCapInfo) = cpu_to_le16(wCurrCapInfo);
-	*(sFrame.pwListenInterval) = cpu_to_le16(wListenInterval);
-
-	// sFrame.len point to end of fixed field
-	sFrame.pSSID = (PWLAN_IE_SSID)(sFrame.pBuf + sFrame.len);
-	sFrame.len += pCurrSSID->len + WLAN_IEHDR_LEN;
-	memcpy(sFrame.pSSID, pCurrSSID, pCurrSSID->len + WLAN_IEHDR_LEN);
-
-	pMgmt->sAssocInfo.AssocInfo.RequestIELength = pCurrSSID->len + WLAN_IEHDR_LEN;
-	pMgmt->sAssocInfo.AssocInfo.OffsetRequestIEs = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
-	pbyIEs = pMgmt->sAssocInfo.abyIEs;
-	memcpy(pbyIEs, pCurrSSID, pCurrSSID->len + WLAN_IEHDR_LEN);
-	pbyIEs += pCurrSSID->len + WLAN_IEHDR_LEN;
-
-	// Copy the rate set
-	sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-	if ((pDevice->eCurrentPHYType == PHY_TYPE_11B) && (pCurrRates->len > 4))
-		sFrame.len += 4 + WLAN_IEHDR_LEN;
-	else
-		sFrame.len += pCurrRates->len + WLAN_IEHDR_LEN;
-	memcpy(sFrame.pSuppRates, pCurrRates, pCurrRates->len + WLAN_IEHDR_LEN);
-
-	// Copy the extension rate set
-	if ((pDevice->eCurrentPHYType == PHY_TYPE_11G) && (pCurrExtSuppRates->len > 0)) {
-		sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-		sFrame.len += pCurrExtSuppRates->len + WLAN_IEHDR_LEN;
-		memcpy(sFrame.pExtSuppRates, pCurrExtSuppRates, pCurrExtSuppRates->len + WLAN_IEHDR_LEN);
-	}
-
-	pMgmt->sAssocInfo.AssocInfo.RequestIELength += pCurrRates->len + WLAN_IEHDR_LEN;
-	memcpy(pbyIEs, pCurrRates, pCurrRates->len + WLAN_IEHDR_LEN);
-	pbyIEs += pCurrRates->len + WLAN_IEHDR_LEN;
-
-	// for 802.11h
-	if (pMgmt->b11hEnable) {
-		if (sFrame.pCurrPowerCap == NULL) {
-			sFrame.pCurrPowerCap = (PWLAN_IE_PW_CAP)(sFrame.pBuf + sFrame.len);
-			sFrame.len += (2 + WLAN_IEHDR_LEN);
-			sFrame.pCurrPowerCap->byElementID = WLAN_EID_PWR_CAPABILITY;
-			sFrame.pCurrPowerCap->len = 2;
-			CARDvGetPowerCapability(pMgmt->pAdapter,
-						&(sFrame.pCurrPowerCap->byMinPower),
-						&(sFrame.pCurrPowerCap->byMaxPower)
-);
-		}
-		if (sFrame.pCurrSuppCh == NULL) {
-			sFrame.pCurrSuppCh = (PWLAN_IE_SUPP_CH)(sFrame.pBuf + sFrame.len);
-			sFrame.len += set_support_channels(pMgmt->pAdapter, (unsigned char *)sFrame.pCurrSuppCh);
-		}
-	}
-
-	if (((pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
-	     (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||
-	     (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE)) &&
-	    (pMgmt->pCurrBSS != NULL)) {
-		/* WPA IE */
-		sFrame.pRSNWPA = (PWLAN_IE_RSN_EXT)(sFrame.pBuf + sFrame.len);
-		sFrame.pRSNWPA->byElementID = WLAN_EID_RSN_WPA;
-		sFrame.pRSNWPA->len = 16;
-		sFrame.pRSNWPA->abyOUI[0] = 0x00;
-		sFrame.pRSNWPA->abyOUI[1] = 0x50;
-		sFrame.pRSNWPA->abyOUI[2] = 0xf2;
-		sFrame.pRSNWPA->abyOUI[3] = 0x01;
-		sFrame.pRSNWPA->wVersion = 1;
-		//Group Key Cipher Suite
-		sFrame.pRSNWPA->abyMulticast[0] = 0x00;
-		sFrame.pRSNWPA->abyMulticast[1] = 0x50;
-		sFrame.pRSNWPA->abyMulticast[2] = 0xf2;
-		if (pMgmt->byCSSGK == KEY_CTL_WEP)
-			sFrame.pRSNWPA->abyMulticast[3] = pMgmt->pCurrBSS->byGKType;
-		else if (pMgmt->byCSSGK == KEY_CTL_TKIP)
-			sFrame.pRSNWPA->abyMulticast[3] = WPA_TKIP;
-		else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
-			sFrame.pRSNWPA->abyMulticast[3] = WPA_AESCCMP;
-		else
-			sFrame.pRSNWPA->abyMulticast[3] = WPA_NONE;
-
-		// Pairwise Key Cipher Suite
-		sFrame.pRSNWPA->wPKCount = 1;
-		sFrame.pRSNWPA->PKSList[0].abyOUI[0] = 0x00;
-		sFrame.pRSNWPA->PKSList[0].abyOUI[1] = 0x50;
-		sFrame.pRSNWPA->PKSList[0].abyOUI[2] = 0xf2;
-		if (pMgmt->byCSSPK == KEY_CTL_TKIP)
-			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_TKIP;
-		else if (pMgmt->byCSSPK == KEY_CTL_CCMP)
-			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_AESCCMP;
-		else
-			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_NONE;
-
-		// Auth Key Management Suite
-		pbyRSN = (unsigned char *)(sFrame.pBuf + sFrame.len + 2 + sFrame.pRSNWPA->len);
-		*pbyRSN++ = 0x01;
-		*pbyRSN++ = 0x00;
-		*pbyRSN++ = 0x00;
-
-		*pbyRSN++ = 0x50;
-		*pbyRSN++ = 0xf2;
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK)
-			*pbyRSN++ = WPA_AUTH_PSK;
-		else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA)
-			*pbyRSN++ = WPA_AUTH_IEEE802_1X;
-		else
-			*pbyRSN++ = WPA_NONE;
-
-		sFrame.pRSNWPA->len += 6;
-
-		// RSN Capabilities
-
-		*pbyRSN++ = 0x00;
-		*pbyRSN++ = 0x00;
-		sFrame.pRSNWPA->len += 2;
-
-		sFrame.len += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
-		// copy to AssocInfo. for OID_802_11_ASSOCIATION_INFORMATION
-		pMgmt->sAssocInfo.AssocInfo.RequestIELength += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
-		memcpy(pbyIEs, sFrame.pRSNWPA, sFrame.pRSNWPA->len + WLAN_IEHDR_LEN);
-		pbyIEs += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
-
-	} else if (((pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
-		    (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) &&
-		   (pMgmt->pCurrBSS != NULL)) {
-		unsigned int ii;
-		unsigned short *pwPMKID;
-
-		// WPA IE
-		sFrame.pRSN = (PWLAN_IE_RSN)(sFrame.pBuf + sFrame.len);
-		sFrame.pRSN->byElementID = WLAN_EID_RSN;
-		sFrame.pRSN->len = 6; //Version(2)+GK(4)
-		sFrame.pRSN->wVersion = 1;
-		//Group Key Cipher Suite
-		sFrame.pRSN->abyRSN[0] = 0x00;
-		sFrame.pRSN->abyRSN[1] = 0x0F;
-		sFrame.pRSN->abyRSN[2] = 0xAC;
-		if (pMgmt->byCSSGK == KEY_CTL_WEP)
-			sFrame.pRSN->abyRSN[3] = pMgmt->pCurrBSS->byCSSGK;
-		else if (pMgmt->byCSSGK == KEY_CTL_TKIP)
-			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_TKIP;
-		else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
-			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_CCMP;
-		else
-			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_UNKNOWN;
-
-		// Pairwise Key Cipher Suite
-		sFrame.pRSN->abyRSN[4] = 1;
-		sFrame.pRSN->abyRSN[5] = 0;
-		sFrame.pRSN->abyRSN[6] = 0x00;
-		sFrame.pRSN->abyRSN[7] = 0x0F;
-		sFrame.pRSN->abyRSN[8] = 0xAC;
-		if (pMgmt->byCSSPK == KEY_CTL_TKIP)
-			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_TKIP;
-		else if (pMgmt->byCSSPK == KEY_CTL_CCMP)
-			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_CCMP;
-		else if (pMgmt->byCSSPK == KEY_CTL_NONE)
-			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_USE_GROUP;
-		else
-			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_UNKNOWN;
-
-		sFrame.pRSN->len += 6;
-
-		// Auth Key Management Suite
-		sFrame.pRSN->abyRSN[10] = 1;
-		sFrame.pRSN->abyRSN[11] = 0;
-		sFrame.pRSN->abyRSN[12] = 0x00;
-		sFrame.pRSN->abyRSN[13] = 0x0F;
-		sFrame.pRSN->abyRSN[14] = 0xAC;
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)
-			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_PSK;
-		else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)
-			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_802_1X;
-		else
-			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_UNKNOWN;
-
-		sFrame.pRSN->len += 6;
-
-		// RSN Capabilities
-		if (pMgmt->pCurrBSS->sRSNCapObj.bRSNCapExist) {
-			memcpy(&sFrame.pRSN->abyRSN[16], &pMgmt->pCurrBSS->sRSNCapObj.wRSNCap, 2);
-		} else {
-			sFrame.pRSN->abyRSN[16] = 0;
-			sFrame.pRSN->abyRSN[17] = 0;
-		}
-		sFrame.pRSN->len += 2;
-
-		if ((pDevice->gsPMKID.BSSIDInfoCount > 0) && pDevice->bRoaming && (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)) {
-			// RSN PMKID
-			pbyRSN = &sFrame.pRSN->abyRSN[18];
-			pwPMKID = (unsigned short *)pbyRSN; // Point to PMKID count
-			*pwPMKID = 0;            // Initialize PMKID count
-			pbyRSN += 2;             // Point to PMKID list
-			for (ii = 0; ii < pDevice->gsPMKID.BSSIDInfoCount; ii++) {
-				if (!memcmp(&pDevice->gsPMKID.BSSIDInfo[ii].BSSID[0], pMgmt->abyCurrBSSID, ETH_ALEN)) {
-					(*pwPMKID)++;
-					memcpy(pbyRSN, pDevice->gsPMKID.BSSIDInfo[ii].PMKID, 16);
-					pbyRSN += 16;
-				}
-			}
-			if (*pwPMKID != 0)
-				sFrame.pRSN->len += (2 + (*pwPMKID)*16);
-		}
-
-		sFrame.len += sFrame.pRSN->len + WLAN_IEHDR_LEN;
-		// copy to AssocInfo. for OID_802_11_ASSOCIATION_INFORMATION
-		pMgmt->sAssocInfo.AssocInfo.RequestIELength += sFrame.pRSN->len + WLAN_IEHDR_LEN;
-		memcpy(pbyIEs, sFrame.pRSN, sFrame.pRSN->len + WLAN_IEHDR_LEN);
-		pbyIEs += sFrame.pRSN->len + WLAN_IEHDR_LEN;
-	}
-
-	// Adjust the length fields
-	pTxPacket->cbMPDULen = sFrame.len;
-	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-	return pTxPacket;
-}
-
-/*+
- *
- * Routine Description:
- *  Constructs an re-association request frame
- *
- *
- * Return Value:
- *    A ptr to frame or NULL on allocation failure
- *
- -*/
-
-static PSTxMgmtPacket
-s_MgrMakeReAssocRequest(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	unsigned char *pDAddr,
-	unsigned short wCurrCapInfo,
-	unsigned short wListenInterval,
-	PWLAN_IE_SSID pCurrSSID,
-	PWLAN_IE_SUPP_RATES pCurrRates,
-	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-)
-{
-	PSTxMgmtPacket      pTxPacket = NULL;
-	WLAN_FR_REASSOCREQ  sFrame;
-	unsigned char *pbyIEs;
-	unsigned char *pbyRSN;
-
-	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_REASSOCREQ_FR_MAXLEN);
-	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-	/* Setup the sFrame structure. */
-	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-	sFrame.len = WLAN_REASSOCREQ_FR_MAXLEN;
-
-	// format fixed field frame structure
-	vMgrEncodeReassocRequest(&sFrame);
-
-	/* Setup the header */
-	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-		(
-			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_REASSOCREQ)
-));
-	memcpy(sFrame.pHdr->sA3.abyAddr1, pDAddr, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-
-	/* Set the capability and listen interval */
-	*(sFrame.pwCapInfo) = cpu_to_le16(wCurrCapInfo);
-	*(sFrame.pwListenInterval) = cpu_to_le16(wListenInterval);
-
-	memcpy(sFrame.pAddrCurrAP, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-	/* Copy the SSID */
-	/* sFrame.len point to end of fixed field */
-	sFrame.pSSID = (PWLAN_IE_SSID)(sFrame.pBuf + sFrame.len);
-	sFrame.len += pCurrSSID->len + WLAN_IEHDR_LEN;
-	memcpy(sFrame.pSSID, pCurrSSID, pCurrSSID->len + WLAN_IEHDR_LEN);
-
-	pMgmt->sAssocInfo.AssocInfo.RequestIELength = pCurrSSID->len + WLAN_IEHDR_LEN;
-	pMgmt->sAssocInfo.AssocInfo.OffsetRequestIEs = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
-	pbyIEs = pMgmt->sAssocInfo.abyIEs;
-	memcpy(pbyIEs, pCurrSSID, pCurrSSID->len + WLAN_IEHDR_LEN);
-	pbyIEs += pCurrSSID->len + WLAN_IEHDR_LEN;
-
-	/* Copy the rate set */
-	/* sFrame.len point to end of SSID */
-	sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-	sFrame.len += pCurrRates->len + WLAN_IEHDR_LEN;
-	memcpy(sFrame.pSuppRates, pCurrRates, pCurrRates->len + WLAN_IEHDR_LEN);
-
-	// Copy the extension rate set
-	if ((pMgmt->eCurrentPHYMode == PHY_TYPE_11G) && (pCurrExtSuppRates->len > 0)) {
-		sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-		sFrame.len += pCurrExtSuppRates->len + WLAN_IEHDR_LEN;
-		memcpy(sFrame.pExtSuppRates, pCurrExtSuppRates, pCurrExtSuppRates->len + WLAN_IEHDR_LEN);
-	}
-
-	pMgmt->sAssocInfo.AssocInfo.RequestIELength += pCurrRates->len + WLAN_IEHDR_LEN;
-	memcpy(pbyIEs, pCurrRates, pCurrRates->len + WLAN_IEHDR_LEN);
-	pbyIEs += pCurrRates->len + WLAN_IEHDR_LEN;
-
-	if (((pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
-	     (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||
-	     (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE)) &&
-	    (pMgmt->pCurrBSS != NULL)) {
-		/* WPA IE */
-		sFrame.pRSNWPA = (PWLAN_IE_RSN_EXT)(sFrame.pBuf + sFrame.len);
-		sFrame.pRSNWPA->byElementID = WLAN_EID_RSN_WPA;
-		sFrame.pRSNWPA->len = 16;
-		sFrame.pRSNWPA->abyOUI[0] = 0x00;
-		sFrame.pRSNWPA->abyOUI[1] = 0x50;
-		sFrame.pRSNWPA->abyOUI[2] = 0xf2;
-		sFrame.pRSNWPA->abyOUI[3] = 0x01;
-		sFrame.pRSNWPA->wVersion = 1;
-		//Group Key Cipher Suite
-		sFrame.pRSNWPA->abyMulticast[0] = 0x00;
-		sFrame.pRSNWPA->abyMulticast[1] = 0x50;
-		sFrame.pRSNWPA->abyMulticast[2] = 0xf2;
-		if (pMgmt->byCSSGK == KEY_CTL_WEP)
-			sFrame.pRSNWPA->abyMulticast[3] = pMgmt->pCurrBSS->byGKType;
-		else if (pMgmt->byCSSGK == KEY_CTL_TKIP)
-			sFrame.pRSNWPA->abyMulticast[3] = WPA_TKIP;
-		else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
-			sFrame.pRSNWPA->abyMulticast[3] = WPA_AESCCMP;
-		else
-			sFrame.pRSNWPA->abyMulticast[3] = WPA_NONE;
-
-		// Pairwise Key Cipher Suite
-		sFrame.pRSNWPA->wPKCount = 1;
-		sFrame.pRSNWPA->PKSList[0].abyOUI[0] = 0x00;
-		sFrame.pRSNWPA->PKSList[0].abyOUI[1] = 0x50;
-		sFrame.pRSNWPA->PKSList[0].abyOUI[2] = 0xf2;
-		if (pMgmt->byCSSPK == KEY_CTL_TKIP)
-			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_TKIP;
-		else if (pMgmt->byCSSPK == KEY_CTL_CCMP)
-			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_AESCCMP;
-		else
-			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_NONE;
-
-		// Auth Key Management Suite
-		pbyRSN = (unsigned char *)(sFrame.pBuf + sFrame.len + 2 + sFrame.pRSNWPA->len);
-		*pbyRSN++ = 0x01;
-		*pbyRSN++ = 0x00;
-		*pbyRSN++ = 0x00;
-
-		*pbyRSN++ = 0x50;
-		*pbyRSN++ = 0xf2;
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK)
-			*pbyRSN++ = WPA_AUTH_PSK;
-		else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA)
-			*pbyRSN++ = WPA_AUTH_IEEE802_1X;
-		else
-			*pbyRSN++ = WPA_NONE;
-
-		sFrame.pRSNWPA->len += 6;
-
-		// RSN Capabilities
-		*pbyRSN++ = 0x00;
-		*pbyRSN++ = 0x00;
-		sFrame.pRSNWPA->len += 2;
-
-		sFrame.len += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
-		// copy to AssocInfo. for OID_802_11_ASSOCIATION_INFORMATION
-		pMgmt->sAssocInfo.AssocInfo.RequestIELength += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
-		memcpy(pbyIEs, sFrame.pRSNWPA, sFrame.pRSNWPA->len + WLAN_IEHDR_LEN);
-		pbyIEs += sFrame.pRSNWPA->len + WLAN_IEHDR_LEN;
-
-	} else if (((pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
-		    (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) &&
-		   (pMgmt->pCurrBSS != NULL)) {
-		unsigned int ii;
-		unsigned short *pwPMKID;
-
-		/* WPA IE */
-		sFrame.pRSN = (PWLAN_IE_RSN)(sFrame.pBuf + sFrame.len);
-		sFrame.pRSN->byElementID = WLAN_EID_RSN;
-		sFrame.pRSN->len = 6; //Version(2)+GK(4)
-		sFrame.pRSN->wVersion = 1;
-		//Group Key Cipher Suite
-		sFrame.pRSN->abyRSN[0] = 0x00;
-		sFrame.pRSN->abyRSN[1] = 0x0F;
-		sFrame.pRSN->abyRSN[2] = 0xAC;
-		if (pMgmt->byCSSGK == KEY_CTL_WEP)
-			sFrame.pRSN->abyRSN[3] = pMgmt->pCurrBSS->byCSSGK;
-		else if (pMgmt->byCSSGK == KEY_CTL_TKIP)
-			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_TKIP;
-		else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
-			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_CCMP;
-		else
-			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_UNKNOWN;
-
-		// Pairwise Key Cipher Suite
-		sFrame.pRSN->abyRSN[4] = 1;
-		sFrame.pRSN->abyRSN[5] = 0;
-		sFrame.pRSN->abyRSN[6] = 0x00;
-		sFrame.pRSN->abyRSN[7] = 0x0F;
-		sFrame.pRSN->abyRSN[8] = 0xAC;
-		if (pMgmt->byCSSPK == KEY_CTL_TKIP)
-			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_TKIP;
-		else if (pMgmt->byCSSPK == KEY_CTL_CCMP)
-			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_CCMP;
-		else if (pMgmt->byCSSPK == KEY_CTL_NONE)
-			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_USE_GROUP;
-		else
-			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_UNKNOWN;
-
-		sFrame.pRSN->len += 6;
-
-		// Auth Key Management Suite
-		sFrame.pRSN->abyRSN[10] = 1;
-		sFrame.pRSN->abyRSN[11] = 0;
-		sFrame.pRSN->abyRSN[12] = 0x00;
-		sFrame.pRSN->abyRSN[13] = 0x0F;
-		sFrame.pRSN->abyRSN[14] = 0xAC;
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)
-			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_PSK;
-		else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)
-			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_802_1X;
-		else
-			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_UNKNOWN;
-
-		sFrame.pRSN->len += 6;
-
-		// RSN Capabilities
-		if (pMgmt->pCurrBSS->sRSNCapObj.bRSNCapExist) {
-			memcpy(&sFrame.pRSN->abyRSN[16], &pMgmt->pCurrBSS->sRSNCapObj.wRSNCap, 2);
-		} else {
-			sFrame.pRSN->abyRSN[16] = 0;
-			sFrame.pRSN->abyRSN[17] = 0;
-		}
-		sFrame.pRSN->len += 2;
-
-		if ((pDevice->gsPMKID.BSSIDInfoCount > 0) && pDevice->bRoaming && (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)) {
-			// RSN PMKID
-			pbyRSN = &sFrame.pRSN->abyRSN[18];
-			pwPMKID = (unsigned short *)pbyRSN; // Point to PMKID count
-			*pwPMKID = 0;            // Initialize PMKID count
-			pbyRSN += 2;             // Point to PMKID list
-			for (ii = 0; ii < pDevice->gsPMKID.BSSIDInfoCount; ii++) {
-				if (!memcmp(&pDevice->gsPMKID.BSSIDInfo[ii].BSSID[0], pMgmt->abyCurrBSSID, ETH_ALEN)) {
-					(*pwPMKID)++;
-					memcpy(pbyRSN, pDevice->gsPMKID.BSSIDInfo[ii].PMKID, 16);
-					pbyRSN += 16;
-				}
-			}
-
-			if (*pwPMKID != 0)
-				sFrame.pRSN->len += (2 + (*pwPMKID) * 16);
-		}
-
-		sFrame.len += sFrame.pRSN->len + WLAN_IEHDR_LEN;
-		// copy to AssocInfo. for OID_802_11_ASSOCIATION_INFORMATION
-		pMgmt->sAssocInfo.AssocInfo.RequestIELength += sFrame.pRSN->len + WLAN_IEHDR_LEN;
-		memcpy(pbyIEs, sFrame.pRSN, sFrame.pRSN->len + WLAN_IEHDR_LEN);
-		pbyIEs += sFrame.pRSN->len + WLAN_IEHDR_LEN;
-	}
-
-	/* Adjust the length fields */
-	pTxPacket->cbMPDULen = sFrame.len;
-	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-
-	return pTxPacket;
-}
-
-/*+
- *
- * Routine Description:
- *  Constructs an assoc-response frame
- *
- *
- * Return Value:
- *    PTR to frame; or NULL on allocation failure
- *
- -*/
-
-static PSTxMgmtPacket
-s_MgrMakeAssocResponse(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	unsigned short wCurrCapInfo,
-	unsigned short wAssocStatus,
-	unsigned short wAssocAID,
-	unsigned char *pDstAddr,
-	PWLAN_IE_SUPP_RATES pCurrSuppRates,
-	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-)
-{
-	PSTxMgmtPacket      pTxPacket = NULL;
-	WLAN_FR_ASSOCRESP   sFrame;
-
-	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_ASSOCREQ_FR_MAXLEN);
-	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-	// Setup the sFrame structure
-	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-	sFrame.len = WLAN_REASSOCRESP_FR_MAXLEN;
-	vMgrEncodeAssocResponse(&sFrame);
-	// Setup the header
-	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-		(
-			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_ASSOCRESP)
-));
-	memcpy(sFrame.pHdr->sA3.abyAddr1, pDstAddr, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-
-	*sFrame.pwCapInfo = cpu_to_le16(wCurrCapInfo);
-	*sFrame.pwStatus = cpu_to_le16(wAssocStatus);
-	*sFrame.pwAid = cpu_to_le16((unsigned short)(wAssocAID | BIT14 | BIT15));
-
-	// Copy the rate set
-	sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-	sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN;
-	memcpy(sFrame.pSuppRates,
-	       pCurrSuppRates,
-	       ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN
-);
-
-	if (((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len != 0) {
-		sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-		sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN;
-		memcpy(sFrame.pExtSuppRates,
-		       pCurrExtSuppRates,
-		       ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN
-);
-	}
-
-	// Adjust the length fields
-	pTxPacket->cbMPDULen = sFrame.len;
-	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-
-	return pTxPacket;
-}
-
-/*+
- *
- * Routine Description:
- *  Constructs an reassoc-response frame
- *
- *
- * Return Value:
- *    PTR to frame; or NULL on allocation failure
- *
- -*/
-
-static PSTxMgmtPacket
-s_MgrMakeReAssocResponse(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	unsigned short wCurrCapInfo,
-	unsigned short wAssocStatus,
-	unsigned short wAssocAID,
-	unsigned char *pDstAddr,
-	PWLAN_IE_SUPP_RATES pCurrSuppRates,
-	PWLAN_IE_SUPP_RATES pCurrExtSuppRates
-)
-{
-	PSTxMgmtPacket      pTxPacket = NULL;
-	WLAN_FR_REASSOCRESP   sFrame;
-
-	pTxPacket = (PSTxMgmtPacket)pMgmt->pbyMgmtPacketPool;
-	memset(pTxPacket, 0, sizeof(STxMgmtPacket) + WLAN_ASSOCREQ_FR_MAXLEN);
-	pTxPacket->p80211Header = (PUWLAN_80211HDR)((unsigned char *)pTxPacket + sizeof(STxMgmtPacket));
-	// Setup the sFrame structure
-	sFrame.pBuf = (unsigned char *)pTxPacket->p80211Header;
-	sFrame.len = WLAN_REASSOCRESP_FR_MAXLEN;
-	vMgrEncodeReassocResponse(&sFrame);
-	// Setup the header
-	sFrame.pHdr->sA3.wFrameCtl = cpu_to_le16(
-		(
-			WLAN_SET_FC_FTYPE(WLAN_TYPE_MGR) |
-			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_REASSOCRESP)
-));
-	memcpy(sFrame.pHdr->sA3.abyAddr1, pDstAddr, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-	memcpy(sFrame.pHdr->sA3.abyAddr3, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-
-	*sFrame.pwCapInfo = cpu_to_le16(wCurrCapInfo);
-	*sFrame.pwStatus = cpu_to_le16(wAssocStatus);
-	*sFrame.pwAid = cpu_to_le16((unsigned short)(wAssocAID | BIT14 | BIT15));
-
-	// Copy the rate set
-	sFrame.pSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-	sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN;
-	memcpy(sFrame.pSuppRates,
-	       pCurrSuppRates,
-	       ((PWLAN_IE_SUPP_RATES)pCurrSuppRates)->len + WLAN_IEHDR_LEN
-);
-
-	if (((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len != 0) {
-		sFrame.pExtSuppRates = (PWLAN_IE_SUPP_RATES)(sFrame.pBuf + sFrame.len);
-		sFrame.len += ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN;
-		memcpy(sFrame.pExtSuppRates,
-		       pCurrExtSuppRates,
-		       ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN
-);
-	}
-
-	// Adjust the length fields
-	pTxPacket->cbMPDULen = sFrame.len;
-	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
-
-	return pTxPacket;
-}
-
-/*+
- *
- * Routine Description:
- *  Handles probe response management frames.
- *
- *
- * Return Value:
- *    none.
- *
- -*/
-
-static
-void
-s_vMgrRxProbeResponse(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PSRxMgmtPacket pRxPacket
-)
-{
-	PKnownBSS           pBSSList = NULL;
-	WLAN_FR_PROBERESP   sFrame;
-	unsigned char byCurrChannel = pRxPacket->byRxChannel;
-	ERPObject           sERP;
-	unsigned char byIEChannel = 0;
-	bool bChannelHit = true;
-
-	memset(&sFrame, 0, sizeof(WLAN_FR_PROBERESP));
-	// decode the frame
-	sFrame.len = pRxPacket->cbMPDULen;
-	sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-	vMgrDecodeProbeResponse(&sFrame);
-
-	if ((sFrame.pqwTimestamp == NULL) ||
-	    (sFrame.pwBeaconInterval == NULL) ||
-	    (sFrame.pwCapInfo == NULL) ||
-	    (sFrame.pSSID == NULL) ||
-	    (sFrame.pSuppRates == NULL)) {
-		pr_debug("Probe resp:Fail addr:[%p]\n",
-			 pRxPacket->p80211Header);
-		DBG_PORT80(0xCC);
-		return;
-	}
-
-	if (sFrame.pSSID->len == 0)
-		pr_debug("Rx Probe resp: SSID len = 0\n");
-
-	if (sFrame.pDSParms != NULL) {
-		if (byCurrChannel > CB_MAX_CHANNEL_24G) {
-			// channel remapping to
-			byIEChannel = get_channel_mapping(pMgmt->pAdapter, sFrame.pDSParms->byCurrChannel, PHY_TYPE_11A);
-		} else {
-			byIEChannel = sFrame.pDSParms->byCurrChannel;
-		}
-		if (byCurrChannel != byIEChannel) {
-			// adjust channel info. bcs we rcv adjacent channel packets
-			bChannelHit = false;
-			byCurrChannel = byIEChannel;
-		}
-	} else {
-		// no DS channel info
-		bChannelHit = true;
-	}
-
-//2008-0730-01<Add>by MikeLiu
-	if (ChannelExceedZoneType(pDevice, byCurrChannel))
-		return;
-
-	if (sFrame.pERP != NULL) {
-		sERP.byERP = sFrame.pERP->byContext;
-		sERP.bERPExist = true;
-	} else {
-		sERP.bERPExist = false;
-		sERP.byERP = 0;
-	}
-
-	// update or insert the bss
-	pBSSList = BSSpAddrIsInBSSList((void *)pDevice, sFrame.pHdr->sA3.abyAddr3, sFrame.pSSID);
-	if (pBSSList) {
-		BSSbUpdateToBSSList((void *)pDevice,
-				    *sFrame.pqwTimestamp,
-				    *sFrame.pwBeaconInterval,
-				    *sFrame.pwCapInfo,
-				    byCurrChannel,
-				    bChannelHit,
-				    sFrame.pSSID,
-				    sFrame.pSuppRates,
-				    sFrame.pExtSuppRates,
-				    &sERP,
-				    sFrame.pRSN,
-				    sFrame.pRSNWPA,
-				    sFrame.pIE_Country,
-				    sFrame.pIE_Quiet,
-				    pBSSList,
-				    sFrame.len - WLAN_HDR_ADDR3_LEN,
-				    sFrame.pHdr->sA4.abyAddr4,   // payload of probresponse
-				    (void *)pRxPacket
-);
-	} else {
-		pr_debug("Probe resp/insert: RxChannel = : %d\n",
-			 byCurrChannel);
-		BSSbInsertToBSSList((void *)pDevice,
-				    sFrame.pHdr->sA3.abyAddr3,
-				    *sFrame.pqwTimestamp,
-				    *sFrame.pwBeaconInterval,
-				    *sFrame.pwCapInfo,
-				    byCurrChannel,
-				    sFrame.pSSID,
-				    sFrame.pSuppRates,
-				    sFrame.pExtSuppRates,
-				    &sERP,
-				    sFrame.pRSN,
-				    sFrame.pRSNWPA,
-				    sFrame.pIE_Country,
-				    sFrame.pIE_Quiet,
-				    sFrame.len - WLAN_HDR_ADDR3_LEN,
-				    sFrame.pHdr->sA4.abyAddr4,   // payload of beacon
-				    (void *)pRxPacket
-);
-	}
-}
-
-/*+
- *
- * Routine Description:(AP)or(Ad-hoc STA)
- *  Handles probe request management frames.
- *
- *
- * Return Value:
- *    none.
- *
- -*/
-
-static
-void
-s_vMgrRxProbeRequest(
-	struct vnt_private *pDevice,
-	PSMgmtObject pMgmt,
-	PSRxMgmtPacket pRxPacket
-)
-{
-	WLAN_FR_PROBEREQ    sFrame;
-	CMD_STATUS          Status;
-	PSTxMgmtPacket      pTxPacket;
-	unsigned char byPHYType = BB_TYPE_11B;
-
-	// STA in Ad-hoc mode: when latest TBTT beacon transmit success,
-	// STA have to response this request.
-	if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) ||
-	    ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) && pDevice->bBeaconSent)) {
-		memset(&sFrame, 0, sizeof(WLAN_FR_PROBEREQ));
-		// decode the frame
-		sFrame.len = pRxPacket->cbMPDULen;
-		sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-		vMgrDecodeProbeRequest(&sFrame);
-
-		if (sFrame.pSSID->len != 0) {
-			if (sFrame.pSSID->len != ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len)
-				return;
-			if (memcmp(sFrame.pSSID->abySSID,
-				   ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->abySSID,
-				   ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len) != 0) {
-				return;
-			}
-		}
-
-		if ((sFrame.pSuppRates->len > 4) || (sFrame.pExtSuppRates != NULL))
-			byPHYType = BB_TYPE_11G;
-
-		// Probe response reply..
-		pTxPacket = s_MgrMakeProbeResponse
-			(
-				pDevice,
-				pMgmt,
-				pMgmt->wCurrCapInfo,
-				pMgmt->wCurrBeaconPeriod,
-				pMgmt->uCurrChannel,
-				0,
-				sFrame.pHdr->sA3.abyAddr2,
-				(PWLAN_IE_SSID)pMgmt->abyCurrSSID,
-				(unsigned char *)pMgmt->abyCurrBSSID,
-				(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-				(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates,
-				byPHYType
-);
-		if (pTxPacket != NULL) {
-			/* send the frame */
-			Status = csMgmt_xmit(pDevice, pTxPacket);
-			if (Status != CMD_STATUS_PENDING)
-				pr_debug("Mgt:Probe response tx failed\n");
-		}
-	}
-}
-
-/*+
- *
- * Routine Description:
- *
- *  Entry point for the reception and handling of 802.11 management
- *  frames. Makes a determination of the frame type and then calls
- *  the appropriate function.
- *
- *
- * Return Value:
- *    none.
- *
- -*/
-
-void
-vMgrRxManagePacket(
-	void *hDeviceContext,
-	PSMgmtObject pMgmt,
-	PSRxMgmtPacket pRxPacket
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	bool bInScan = false;
-	unsigned int uNodeIndex = 0;
-	NODE_STATE  eNodeState = 0;
-	CMD_STATUS  Status;
-
-	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-		if (BSSDBbIsSTAInNodeDB(pMgmt, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex))
-			eNodeState = pMgmt->sNodeDBTable[uNodeIndex].eNodeState;
-	}
-
-	switch (WLAN_GET_FC_FSTYPE((pRxPacket->p80211Header->sA3.wFrameCtl))) {
-	case WLAN_FSTYPE_ASSOCREQ:
-		// Frame Clase = 2
-		pr_debug("rx assocreq\n");
-		if (eNodeState < NODE_AUTH) {
-			// send deauth notification
-			// reason = (6) class 2 received from nonauth sta
-			vMgrDeAuthenBeginSta(pDevice,
-					     pMgmt,
-					     pRxPacket->p80211Header->sA3.abyAddr2,
-					     (6),
-					     &Status
-);
-			pr_debug("wmgr: send vMgrDeAuthenBeginSta 1\n");
-		} else {
-			s_vMgrRxAssocRequest(pDevice, pMgmt, pRxPacket, uNodeIndex);
-		}
-		break;
-
-	case WLAN_FSTYPE_ASSOCRESP:
-		// Frame Clase = 2
-		pr_debug("rx assocresp1\n");
-		s_vMgrRxAssocResponse(pDevice, pMgmt, pRxPacket, false);
-		pr_debug("rx assocresp2\n");
-		break;
-
-	case WLAN_FSTYPE_REASSOCREQ:
-		// Frame Clase = 2
-		pr_debug("rx reassocreq\n");
-		// Todo: reassoc
-		if (eNodeState < NODE_AUTH) {
-			// send deauth notification
-			// reason = (6) class 2 received from nonauth sta
-			vMgrDeAuthenBeginSta(pDevice,
-					     pMgmt,
-					     pRxPacket->p80211Header->sA3.abyAddr2,
-					     (6),
-					     &Status
-);
-			pr_debug("wmgr: send vMgrDeAuthenBeginSta 2\n");
-
-		}
-		s_vMgrRxReAssocRequest(pDevice, pMgmt, pRxPacket, uNodeIndex);
-		break;
-
-	case WLAN_FSTYPE_REASSOCRESP:
-		// Frame Clase = 2
-		pr_debug("rx reassocresp\n");
-		s_vMgrRxAssocResponse(pDevice, pMgmt, pRxPacket, true);
-		break;
-
-	case WLAN_FSTYPE_PROBEREQ:
-		// Frame Clase = 0
-		s_vMgrRxProbeRequest(pDevice, pMgmt, pRxPacket);
-		break;
-
-	case WLAN_FSTYPE_PROBERESP:
-		// Frame Clase = 0
-		pr_debug("rx proberesp\n");
-
-		s_vMgrRxProbeResponse(pDevice, pMgmt, pRxPacket);
-		break;
-
-	case WLAN_FSTYPE_BEACON:
-		// Frame Clase = 0
-		if (pMgmt->eScanState != WMAC_NO_SCANNING)
-			bInScan = true;
-
-		s_vMgrRxBeacon(pDevice, pMgmt, pRxPacket, bInScan);
-		break;
-
-	case WLAN_FSTYPE_ATIM:
-		// Frame Clase = 1
-		pr_debug("rx atim\n");
-		break;
-
-	case WLAN_FSTYPE_DISASSOC:
-		// Frame Clase = 2
-		pr_debug("rx disassoc\n");
-		if (eNodeState < NODE_AUTH) {
-			// send deauth notification
-			// reason = (6) class 2 received from nonauth sta
-			vMgrDeAuthenBeginSta(pDevice,
-					     pMgmt,
-					     pRxPacket->p80211Header->sA3.abyAddr2,
-					     (6),
-					     &Status
-);
-			pr_debug("wmgr: send vMgrDeAuthenBeginSta 3\n");
-		}
-		s_vMgrRxDisassociation(pDevice, pMgmt, pRxPacket);
-		break;
-
-	case WLAN_FSTYPE_AUTHEN:
-		// Frame Clase = 1
-		pr_debug("rx authen\n");
-		s_vMgrRxAuthentication(pDevice, pMgmt, pRxPacket);
-		break;
-
-	case WLAN_FSTYPE_DEAUTHEN:
-		// Frame Clase = 1
-		pr_debug("rx deauthen\n");
-		s_vMgrRxDeauthentication(pDevice, pMgmt, pRxPacket);
-		break;
-
-	default:
-		pr_debug("rx unknown mgmt\n");
-	}
-}
-
-/*+
- *
- * Routine Description:
- *
- *
- *  Prepare beacon to send
- *
- * Return Value:
- *    true if success; false if failed.
- *
- -*/
-bool
-bMgrPrepareBeaconToSend(
-	void *hDeviceContext,
-	PSMgmtObject pMgmt
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	PSTxMgmtPacket      pTxPacket;
-
-	if (pDevice->bEncryptionEnable || pDevice->bEnable8021x)
-		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
-	else
-		pMgmt->wCurrCapInfo &= ~WLAN_SET_CAP_INFO_PRIVACY(1);
-
-	pTxPacket = s_MgrMakeBeacon
-		(
-			pDevice,
-			pMgmt,
-			pMgmt->wCurrCapInfo,
-			pMgmt->wCurrBeaconPeriod,
-			pMgmt->uCurrChannel,
-			pMgmt->wCurrATIMWindow,
-			(PWLAN_IE_SSID)pMgmt->abyCurrSSID,
-			(unsigned char *)pMgmt->abyCurrBSSID,
-			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
-			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates
-);
-
-	if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) &&
-	    (pMgmt->abyCurrBSSID[0] == 0))
-		return false;
-
-	csBeacon_xmit(pDevice, pTxPacket);
-
-	return true;
-}
-
-/*+
- *
- * Routine Description:
- *
- *  Log a warning message based on the contents of the Status
- *  Code field of an 802.11 management frame.  Defines are
- *  derived from 802.11-1997 SPEC.
- *
- * Return Value:
- *    none.
- *
- -*/
-static
-void
-s_vMgrLogStatus(
-	PSMgmtObject pMgmt,
-	unsigned short wStatus
-)
-{
-	switch (wStatus) {
-	case WLAN_MGMT_STATUS_UNSPEC_FAILURE:
-		pr_info("Status code == Unspecified error\n");
-		break;
-	case WLAN_MGMT_STATUS_CAPS_UNSUPPORTED:
-		pr_info("Status code == Can't support all requested capabilities\n");
-		break;
-	case WLAN_MGMT_STATUS_REASSOC_NO_ASSOC:
-		pr_info("Status code == Reassoc denied, can't confirm original Association\n");
-		break;
-	case WLAN_MGMT_STATUS_ASSOC_DENIED_UNSPEC:
-		pr_info("Status code == Assoc denied, undefine in spec\n");
-		break;
-	case WLAN_MGMT_STATUS_UNSUPPORTED_AUTHALG:
-		pr_info("Status code == Peer doesn't support authen algorithm\n");
-		break;
-	case WLAN_MGMT_STATUS_RX_AUTH_NOSEQ:
-		pr_info("Status code == Authen frame received out of sequence\n");
-		break;
-	case WLAN_MGMT_STATUS_CHALLENGE_FAIL:
-		pr_info("Status code == Authen rejected, challenge  failure\n");
-		break;
-	case WLAN_MGMT_STATUS_AUTH_TIMEOUT:
-		pr_info("Status code == Authen rejected, timeout waiting for next frame\n");
-		break;
-	case WLAN_MGMT_STATUS_ASSOC_DENIED_BUSY:
-		pr_info("Status code == Assoc denied, AP too busy\n");
-		break;
-	case WLAN_MGMT_STATUS_ASSOC_DENIED_RATES:
-		pr_info("Status code == Assoc denied, we haven't enough basic rates\n");
-		break;
-	case WLAN_MGMT_STATUS_ASSOC_DENIED_SHORTPREAMBLE:
-		pr_info("Status code == Assoc denied, we do not support short preamble\n");
-		break;
-	case WLAN_MGMT_STATUS_ASSOC_DENIED_PBCC:
-		pr_info("Status code == Assoc denied, we do not support PBCC\n");
-		break;
-	case WLAN_MGMT_STATUS_ASSOC_DENIED_AGILITY:
-		pr_info("Status code == Assoc denied, we do not support channel agility\n");
-		break;
-	default:
-		pr_info("Unknown status code %d\n", wStatus);
-		break;
-	}
-}
-
-/*
- *
- * Description:
- *    Add BSSID in PMKID Candidate list.
- *
- * Parameters:
- *  In:
- *      hDeviceContext - device structure point
- *      pbyBSSID - BSSID address for adding
- *      wRSNCap - BSS's RSN capability
- *  Out:
- *      none
- *
- * Return Value: none.
- *
- -*/
-bool
-bAdd_PMKID_Candidate(
-	void *hDeviceContext,
-	unsigned char *pbyBSSID,
-	PSRSNCapObject psRSNCapObj
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-	struct pmkid_candidate *pCandidateList;
-	unsigned int ii = 0;
-
-	pr_debug("bAdd_PMKID_Candidate START: (%d)\n",
-		 (int)pDevice->gsPMKIDCandidate.NumCandidates);
-
-	if ((pDevice == NULL) || (pbyBSSID == NULL) || (psRSNCapObj == NULL))
-		return false;
-
-	if (pDevice->gsPMKIDCandidate.NumCandidates >= MAX_PMKIDLIST)
-		return false;
-
-	// Update Old Candidate
-	for (ii = 0; ii < pDevice->gsPMKIDCandidate.NumCandidates; ii++) {
-		pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[ii];
-		if (!memcmp(pCandidateList->BSSID, pbyBSSID, ETH_ALEN)) {
-			if (psRSNCapObj->bRSNCapExist && (psRSNCapObj->wRSNCap & BIT0))
-				pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
-			else
-				pCandidateList->Flags &= ~(NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED);
-
-			return true;
-		}
-	}
-
-	// New Candidate
-	pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[pDevice->gsPMKIDCandidate.NumCandidates];
-	if (psRSNCapObj->bRSNCapExist && (psRSNCapObj->wRSNCap & BIT0))
-		pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
-	else
-		pCandidateList->Flags &= ~(NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED);
-
-	memcpy(pCandidateList->BSSID, pbyBSSID, ETH_ALEN);
-	pDevice->gsPMKIDCandidate.NumCandidates++;
-	pr_debug("NumCandidates:%d\n",
-		 (int)pDevice->gsPMKIDCandidate.NumCandidates);
-	return true;
-}
-
-/*
- *
- * Description:
- *    Flush PMKID Candidate list.
- *
- * Parameters:
- *  In:
- *      hDeviceContext - device structure point
- *  Out:
- *      none
- *
- * Return Value: none.
- *
- -*/
-void
-vFlush_PMKID_Candidate(
-	void *hDeviceContext
-)
-{
-	struct vnt_private *pDevice = hDeviceContext;
-
-	if (pDevice == NULL)
-		return;
-
-	memset(&pDevice->gsPMKIDCandidate, 0, sizeof(SPMKIDCandidateEvent));
-}
-
-static bool
-s_bCipherMatch(
-	PKnownBSS                        pBSSNode,
-	NDIS_802_11_ENCRYPTION_STATUS    EncStatus,
-	unsigned char *pbyCCSPK,
-	unsigned char *pbyCCSGK
-)
-{
-	unsigned char byMulticastCipher = KEY_CTL_INVALID;
-	unsigned char byCipherMask = 0x00;
-	int i;
-
-	if (pBSSNode == NULL)
-		return false;
-
-	// check cap. of BSS
-	if ((WLAN_GET_CAP_INFO_PRIVACY(pBSSNode->wCapInfo) != 0) &&
-	    (EncStatus == Ndis802_11Encryption1Enabled)) {
-		// default is WEP only
-		byMulticastCipher = KEY_CTL_WEP;
-	}
-
-	if ((WLAN_GET_CAP_INFO_PRIVACY(pBSSNode->wCapInfo) != 0) &&
-	    pBSSNode->bWPA2Valid &&
-	    //20080123-01,<Add> by Einsn Liu
-	    ((EncStatus == Ndis802_11Encryption3Enabled) || (EncStatus == Ndis802_11Encryption2Enabled))) {
-		//WPA2
-		// check Group Key Cipher
-		if ((pBSSNode->byCSSGK == WLAN_11i_CSS_WEP40) ||
-		    (pBSSNode->byCSSGK == WLAN_11i_CSS_WEP104)) {
-			byMulticastCipher = KEY_CTL_WEP;
-		} else if (pBSSNode->byCSSGK == WLAN_11i_CSS_TKIP) {
-			byMulticastCipher = KEY_CTL_TKIP;
-		} else if (pBSSNode->byCSSGK == WLAN_11i_CSS_CCMP) {
-			byMulticastCipher = KEY_CTL_CCMP;
-		} else {
-			byMulticastCipher = KEY_CTL_INVALID;
-		}
-
-		// check Pairwise Key Cipher
-		for (i = 0; i < pBSSNode->wCSSPKCount; i++) {
-			if ((pBSSNode->abyCSSPK[i] == WLAN_11i_CSS_WEP40) ||
-			    (pBSSNode->abyCSSPK[i] == WLAN_11i_CSS_WEP104)) {
-				// this should not happen as defined 802.11i
-				byCipherMask |= 0x01;
-			} else if (pBSSNode->abyCSSPK[i] == WLAN_11i_CSS_TKIP) {
-				byCipherMask |= 0x02;
-			} else if (pBSSNode->abyCSSPK[i] == WLAN_11i_CSS_CCMP) {
-				byCipherMask |= 0x04;
-			} else if (pBSSNode->abyCSSPK[i] == WLAN_11i_CSS_USE_GROUP) {
-				// use group key only ignore all others
-				byCipherMask = 0;
-				i = pBSSNode->wCSSPKCount;
-			}
-		}
-
-	} else if ((WLAN_GET_CAP_INFO_PRIVACY(pBSSNode->wCapInfo) != 0) &&
-		   pBSSNode->bWPAValid &&
-		   ((EncStatus == Ndis802_11Encryption3Enabled) || (EncStatus == Ndis802_11Encryption2Enabled))) {
-		//WPA
-		// check Group Key Cipher
-		if ((pBSSNode->byGKType == WPA_WEP40) ||
-		    (pBSSNode->byGKType == WPA_WEP104)) {
-			byMulticastCipher = KEY_CTL_WEP;
-		} else if (pBSSNode->byGKType == WPA_TKIP) {
-			byMulticastCipher = KEY_CTL_TKIP;
-		} else if (pBSSNode->byGKType == WPA_AESCCMP) {
-			byMulticastCipher = KEY_CTL_CCMP;
-		} else {
-			byMulticastCipher = KEY_CTL_INVALID;
-		}
-
-		// check Pairwise Key Cipher
-		for (i = 0; i < pBSSNode->wPKCount; i++) {
-			if (pBSSNode->abyPKType[i] == WPA_TKIP) {
-				byCipherMask |= 0x02;
-			} else if (pBSSNode->abyPKType[i] == WPA_AESCCMP) {
-				byCipherMask |= 0x04;
-			} else if (pBSSNode->abyPKType[i] == WPA_NONE) {
-				// use group key only ignore all others
-				byCipherMask = 0;
-				i = pBSSNode->wPKCount;
-			}
-		}
-	}
-
-	pr_debug("%d, %d, %d, %d, EncStatus:%d\n",
-		 byMulticastCipher, byCipherMask,
-		 pBSSNode->bWPAValid, pBSSNode->bWPA2Valid, EncStatus);
-
-	// mask our cap. with BSS
-	if (EncStatus == Ndis802_11Encryption1Enabled) {
-		// For supporting Cisco migration mode, don't care pairwise key cipher
-		if ((byMulticastCipher == KEY_CTL_WEP) &&
-		    (byCipherMask == 0)) {
-			*pbyCCSGK = KEY_CTL_WEP;
-			*pbyCCSPK = KEY_CTL_NONE;
-			return true;
-		} else {
-			return false;
-		}
-
-	} else if (EncStatus == Ndis802_11Encryption2Enabled) {
-		if ((byMulticastCipher == KEY_CTL_TKIP) &&
-		    (byCipherMask == 0)) {
-			*pbyCCSGK = KEY_CTL_TKIP;
-			*pbyCCSPK = KEY_CTL_NONE;
-			return true;
-		} else if ((byMulticastCipher == KEY_CTL_WEP) &&
-			   ((byCipherMask & 0x02) != 0)) {
-			*pbyCCSGK = KEY_CTL_WEP;
-			*pbyCCSPK = KEY_CTL_TKIP;
-			return true;
-		} else if ((byMulticastCipher == KEY_CTL_TKIP) &&
-			   ((byCipherMask & 0x02) != 0)) {
-			*pbyCCSGK = KEY_CTL_TKIP;
-			*pbyCCSPK = KEY_CTL_TKIP;
-			return true;
-		} else {
-			return false;
-		}
-	} else if (EncStatus == Ndis802_11Encryption3Enabled) {
-		if ((byMulticastCipher == KEY_CTL_CCMP) &&
-		    (byCipherMask == 0)) {
-			// When CCMP is enable, "Use group cipher suite" shall not be a valid option.
-			return false;
-		} else if ((byMulticastCipher == KEY_CTL_WEP) &&
-			   ((byCipherMask & 0x04) != 0)) {
-			*pbyCCSGK = KEY_CTL_WEP;
-			*pbyCCSPK = KEY_CTL_CCMP;
-			return true;
-		} else if ((byMulticastCipher == KEY_CTL_TKIP) &&
-			   ((byCipherMask & 0x04) != 0)) {
-			*pbyCCSGK = KEY_CTL_TKIP;
-			*pbyCCSPK = KEY_CTL_CCMP;
-			return true;
-		} else if ((byMulticastCipher == KEY_CTL_CCMP) &&
-			   ((byCipherMask & 0x04) != 0)) {
-			*pbyCCSGK = KEY_CTL_CCMP;
-			*pbyCCSPK = KEY_CTL_CCMP;
-			return true;
-		} else {
-			return false;
-		}
-	}
-	return true;
-}
diff --git a/drivers/staging/vt6655/wmgr.h b/drivers/staging/vt6655/wmgr.h
deleted file mode 100644
index ce939b30ac2a..000000000000
--- a/drivers/staging/vt6655/wmgr.h
+++ /dev/null
@@ -1,420 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: wmgr.h
- *
- * Purpose:
- *
- * Author: lyndon chen
- *
- * Date: Jan 2, 2003
- *
- * Functions:
- *
- * Revision History:
- *
- */
-
-#ifndef __WMGR_H__
-#define __WMGR_H__
-
-#include "ttype.h"
-#include "80211mgr.h"
-#include "80211hdr.h"
-#include "wcmd.h"
-#include "bssdb.h"
-#include "wpa2.h"
-#include "vntwifi.h"
-#include "card.h"
-
-/*---------------------  Export Definitions -------------------------*/
-
-// Scan time
-#define PROBE_DELAY                  100  // (us)
-#define SWITCH_CHANNEL_DELAY         200 // (us)
-#define WLAN_SCAN_MINITIME           25   // (ms)
-#define WLAN_SCAN_MAXTIME            100  // (ms)
-#define TRIVIAL_SYNC_DIFFERENCE      0    // (us)
-#define DEFAULT_IBSS_BI              100  // (ms)
-
-#define WCMD_ACTIVE_SCAN_TIME   50 //(ms)
-#define WCMD_PASSIVE_SCAN_TIME  100 //(ms)
-
-#define DEFAULT_MSDU_LIFETIME           512  // ms
-#define DEFAULT_MSDU_LIFETIME_RES_64us  8000 // 64us
-
-#define DEFAULT_MGN_LIFETIME            8    // ms
-#define DEFAULT_MGN_LIFETIME_RES_64us   125  // 64us
-
-#define MAKE_BEACON_RESERVED            10  //(us)
-
-#define TIM_MULTICAST_MASK           0x01
-#define TIM_BITMAPOFFSET_MASK        0xFE
-#define DEFAULT_DTIM_PERIOD             1
-
-#define AP_LONG_RETRY_LIMIT             4
-
-#define DEFAULT_IBSS_CHANNEL            6  //2.4G
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Types  ------------------------------*/
-#define timer_expire(timer, next_tick)   mod_timer(&timer, RUN_AT(next_tick))
-typedef void (*TimerFunction)(unsigned long);
-
-//+++ NDIS related
-
-typedef unsigned char NDIS_802_11_MAC_ADDRESS[6];
-typedef struct _NDIS_802_11_AI_REQFI {
-	unsigned short Capabilities;
-	unsigned short ListenInterval;
-	NDIS_802_11_MAC_ADDRESS  CurrentAPAddress;
-} NDIS_802_11_AI_REQFI, *PNDIS_802_11_AI_REQFI;
-
-typedef struct _NDIS_802_11_AI_RESFI {
-	unsigned short Capabilities;
-	unsigned short StatusCode;
-	unsigned short AssociationId;
-} NDIS_802_11_AI_RESFI, *PNDIS_802_11_AI_RESFI;
-
-typedef struct _NDIS_802_11_ASSOCIATION_INFORMATION {
-	unsigned long Length;
-	unsigned short          AvailableRequestFixedIEs;
-	NDIS_802_11_AI_REQFI    RequestFixedIEs;
-	unsigned long RequestIELength;
-	unsigned long OffsetRequestIEs;
-	unsigned short          AvailableResponseFixedIEs;
-	NDIS_802_11_AI_RESFI    ResponseFixedIEs;
-	unsigned long ResponseIELength;
-	unsigned long OffsetResponseIEs;
-} NDIS_802_11_ASSOCIATION_INFORMATION, *PNDIS_802_11_ASSOCIATION_INFORMATION;
-
-typedef struct tagSAssocInfo {
-	NDIS_802_11_ASSOCIATION_INFORMATION     AssocInfo;
-	unsigned char abyIEs[WLAN_BEACON_FR_MAXLEN+WLAN_BEACON_FR_MAXLEN];
-	// store ReqIEs set by OID_802_11_ASSOCIATION_INFORMATION
-	unsigned long RequestIELength;
-	unsigned char abyReqIEs[WLAN_BEACON_FR_MAXLEN];
-} SAssocInfo, *PSAssocInfo;
-//---
-
-typedef enum tagWMAC_SCAN_TYPE {
-	WMAC_SCAN_ACTIVE,
-	WMAC_SCAN_PASSIVE,
-	WMAC_SCAN_HYBRID
-} WMAC_SCAN_TYPE, *PWMAC_SCAN_TYPE;
-
-typedef enum tagWMAC_SCAN_STATE {
-	WMAC_NO_SCANNING,
-	WMAC_IS_SCANNING,
-	WMAC_IS_PROBEPENDING
-} WMAC_SCAN_STATE, *PWMAC_SCAN_STATE;
-
-// Notes:
-// Basic Service Set state explained as following:
-// WMAC_STATE_IDLE          : no BSS is selected (Adhoc or Infra)
-// WMAC_STATE_STARTED       : no BSS is selected, start own IBSS (Adhoc only)
-// WMAC_STATE_JOINTED       : BSS is selected and synchronized (Adhoc or Infra)
-// WMAC_STATE_AUTHPENDING   : Authentication pending (Infra)
-// WMAC_STATE_AUTH          : Authenticated (Infra)
-// WMAC_STATE_ASSOCPENDING  : Association pending (Infra)
-// WMAC_STATE_ASSOC         : Associated (Infra)
-
-typedef enum tagWMAC_BSS_STATE {
-	WMAC_STATE_IDLE,
-	WMAC_STATE_STARTED,
-	WMAC_STATE_JOINTED,
-	WMAC_STATE_AUTHPENDING,
-	WMAC_STATE_AUTH,
-	WMAC_STATE_ASSOCPENDING,
-	WMAC_STATE_ASSOC
-} WMAC_BSS_STATE, *PWMAC_BSS_STATE;
-
-// WMAC selected running mode
-typedef enum tagWMAC_CURRENT_MODE {
-	WMAC_MODE_STANDBY,
-	WMAC_MODE_ESS_STA,
-	WMAC_MODE_IBSS_STA,
-	WMAC_MODE_ESS_AP
-} WMAC_CURRENT_MODE, *PWMAC_CURRENT_MODE;
-
-/*
-  typedef enum tagWMAC_POWER_MODE {
-  WMAC_POWER_CAM,
-  WMAC_POWER_FAST,
-  WMAC_POWER_MAX
-
-  } WMAC_POWER_MODE, *PWMAC_POWER_MODE;
-*/
-
-// Tx Management Packet descriptor
-typedef struct tagSTxMgmtPacket {
-	PUWLAN_80211HDR     p80211Header;
-	unsigned int cbMPDULen;
-	unsigned int cbPayloadLen;
-} STxMgmtPacket, *PSTxMgmtPacket;
-
-// Rx Management Packet descriptor
-typedef struct tagSRxMgmtPacket {
-	PUWLAN_80211HDR     p80211Header;
-	u64 qwLocalTSF;
-	unsigned int cbMPDULen;
-	unsigned int cbPayloadLen;
-	unsigned int uRSSI;
-	unsigned char bySQ;
-	unsigned char byRxRate;
-	unsigned char byRxChannel;
-} SRxMgmtPacket, *PSRxMgmtPacket;
-
-typedef struct tagSMgmtObject {
-	void *pAdapter;
-	// MAC address
-	unsigned char abyMACAddr[WLAN_ADDR_LEN];
-
-	// Configuration Mode
-	WMAC_CONFIG_MODE        eConfigMode; // MAC pre-configed mode
-	CARD_PHY_TYPE           eCurrentPHYMode;
-	CARD_PHY_TYPE           eConfigPHYMode;
-
-	// Operation state variables
-	WMAC_CURRENT_MODE       eCurrMode;   // MAC current connection mode
-	WMAC_BSS_STATE          eCurrState;  // MAC current BSS state
-
-	PKnownBSS               pCurrBSS;
-	unsigned char byCSSGK;
-	unsigned char byCSSPK;
-
-	// Current state vars
-	unsigned int	uCurrChannel;
-	unsigned char abyCurrSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
-	unsigned char abyCurrExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
-	unsigned char abyCurrSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-	unsigned char abyCurrBSSID[WLAN_BSSID_LEN];
-	unsigned short wCurrCapInfo;
-	unsigned short wCurrAID;
-	unsigned short wCurrATIMWindow;
-	unsigned short wCurrBeaconPeriod;
-	bool bIsDS;
-	unsigned char byERPContext;
-
-	CMD_STATE               eCommandState;
-	unsigned int	uScanChannel;
-
-	// Desire joining BSS vars
-	unsigned char abyDesireSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-	unsigned char abyDesireBSSID[WLAN_BSSID_LEN];
-
-	// Adhoc or AP configuration vars
-	unsigned short wIBSSBeaconPeriod;
-	unsigned short wIBSSATIMWindow;
-	unsigned int	uIBSSChannel;
-	unsigned char abyIBSSSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
-	unsigned char byAPBBType;
-	unsigned char abyWPAIE[MAX_WPA_IE_LEN];
-	unsigned short wWPAIELen;
-
-	unsigned int	uAssocCount;
-	bool bMoreData;
-
-	// Scan state vars
-	WMAC_SCAN_STATE         eScanState;
-	WMAC_SCAN_TYPE          eScanType;
-	unsigned int	uScanStartCh;
-	unsigned int	uScanEndCh;
-	unsigned short wScanSteps;
-	unsigned int	uScanBSSType;
-	// Desire scanning vars
-	unsigned char abyScanSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-	unsigned char abyScanBSSID[WLAN_BSSID_LEN];
-
-	// Privacy
-	WMAC_AUTHENTICATION_MODE eAuthenMode;
-	WMAC_ENCRYPTION_MODE    eEncryptionMode;
-	bool bShareKeyAlgorithm;
-	unsigned char abyChallenge[WLAN_CHALLENGE_LEN];
-	bool bPrivacyInvoked;
-
-	// Received beacon state vars
-	bool bInTIM;
-	bool bMulticastTIM;
-	unsigned char byDTIMCount;
-	unsigned char byDTIMPeriod;
-
-	// Power saving state vars
-	WMAC_POWER_MODE         ePSMode;
-	unsigned short wListenInterval;
-	unsigned short wCountToWakeUp;
-	bool bInTIMWake;
-	unsigned char *pbyPSPacketPool;
-	unsigned char byPSPacketPool[sizeof(STxMgmtPacket) + WLAN_NULLDATA_FR_MAXLEN];
-	bool bRxBeaconInTBTTWake;
-	unsigned char abyPSTxMap[MAX_NODE_NUM + 1];
-
-	// management command related
-	unsigned int	uCmdBusy;
-	unsigned int	uCmdHostAPBusy;
-
-	// management packet pool
-	unsigned char *pbyMgmtPacketPool;
-	unsigned char byMgmtPacketPool[sizeof(STxMgmtPacket) + WLAN_A3FR_MAXLEN];
-
-	// One second callback timer
-	struct timer_list	    sTimerSecondCallback;
-
-	// Temporarily Rx Mgmt Packet Descriptor
-	SRxMgmtPacket           sRxPacket;
-
-	// link list of known bss's (scan results)
-	KnownBSS                sBSSList[MAX_BSS_NUM];
-
-	// table list of known node
-	// sNodeDBList[0] is reserved for AP under Infra mode
-	// sNodeDBList[0] is reserved for Multicast under adhoc/AP mode
-	KnownNodeDB             sNodeDBTable[MAX_NODE_NUM + 1];
-
-	// WPA2 PMKID Cache
-	SPMKIDCache             gsPMKIDCache;
-	bool bRoaming;
-
-	// rate fall back vars
-
-	// associate info
-	SAssocInfo              sAssocInfo;
-
-	// for 802.11h
-	bool b11hEnable;
-	bool bSwitchChannel;
-	unsigned char byNewChannel;
-	PWLAN_IE_MEASURE_REP    pCurrMeasureEIDRep;
-	unsigned int	uLengthOfRepEIDs;
-	unsigned char abyCurrentMSRReq[sizeof(STxMgmtPacket) + WLAN_A3FR_MAXLEN];
-	unsigned char abyCurrentMSRRep[sizeof(STxMgmtPacket) + WLAN_A3FR_MAXLEN];
-	unsigned char abyIECountry[WLAN_A3FR_MAXLEN];
-	unsigned char abyIBSSDFSOwner[6];
-	unsigned char byIBSSDFSRecovery;
-
-	struct sk_buff  skb;
-} SMgmtObject, *PSMgmtObject;
-
-/*---------------------  Export Macros ------------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-void
-vMgrObjectInit(
-	void *hDeviceContext
-);
-
-void
-vMgrTimerInit(
-	void *hDeviceContext
-);
-
-void
-vMgrObjectReset(
-	void *hDeviceContext
-);
-
-void
-vMgrAssocBeginSta(
-	void *hDeviceContext,
-	PSMgmtObject pMgmt,
-	PCMD_STATUS pStatus
-);
-
-void
-vMgrReAssocBeginSta(
-	void *hDeviceContext,
-	PSMgmtObject pMgmt,
-	PCMD_STATUS pStatus
-);
-
-void
-vMgrDisassocBeginSta(
-	void *hDeviceContext,
-	PSMgmtObject pMgmt,
-	unsigned char *abyDestAddress,
-	unsigned short wReason,
-	PCMD_STATUS pStatus
-);
-
-void
-vMgrAuthenBeginSta(
-	void *hDeviceContext,
-	PSMgmtObject pMgmt,
-	PCMD_STATUS pStatus
-);
-
-void
-vMgrCreateOwnIBSS(
-	void *hDeviceContext,
-	PCMD_STATUS pStatus
-);
-
-void
-vMgrJoinBSSBegin(
-	void *hDeviceContext,
-	PCMD_STATUS pStatus
-);
-
-void
-vMgrRxManagePacket(
-	void *hDeviceContext,
-	PSMgmtObject pMgmt,
-	PSRxMgmtPacket pRxPacket
-);
-
-/*
-  void
-  vMgrScanBegin(
-  void *hDeviceContext,
-  PCMD_STATUS pStatus
-);
-*/
-
-void
-vMgrDeAuthenBeginSta(
-	void *hDeviceContext,
-	PSMgmtObject  pMgmt,
-	unsigned char *abyDestAddress,
-	unsigned short wReason,
-	PCMD_STATUS pStatus
-);
-
-bool
-bMgrPrepareBeaconToSend(
-	void *hDeviceContext,
-	PSMgmtObject pMgmt
-);
-
-bool
-bAdd_PMKID_Candidate(
-	void *hDeviceContext,
-	unsigned char *pbyBSSID,
-	PSRSNCapObject psRSNCapObj
-);
-
-void
-vFlush_PMKID_Candidate(
-	void *hDeviceContext
-);
-
-#endif // __WMGR_H__
diff --git a/drivers/staging/vt6655/wpa.c b/drivers/staging/vt6655/wpa.c
deleted file mode 100644
index 5d4eca8512af..000000000000
--- a/drivers/staging/vt6655/wpa.c
+++ /dev/null
@@ -1,300 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: wpa.c
- *
- * Purpose: Handles the Basic Service Set & Node Database functions
- *
- * Functions:
- *      WPA_ParseRSN - Parse RSN IE.
- *
- * Revision History:
- *
- * Author: Kyle Hsu
- *
- * Date: July 14, 2003
- *
- */
-
-#include "ttype.h"
-#include "tmacro.h"
-#include "tether.h"
-#include "device.h"
-#include "80211hdr.h"
-#include "bssdb.h"
-#include "wmgr.h"
-#include "wpa.h"
-#include "80211mgr.h"
-
-/*---------------------  Static Variables  --------------------------*/
-static const unsigned char abyOUI00[4] = { 0x00, 0x50, 0xf2, 0x00 };
-static const unsigned char abyOUI01[4] = { 0x00, 0x50, 0xf2, 0x01 };
-static const unsigned char abyOUI02[4] = { 0x00, 0x50, 0xf2, 0x02 };
-static const unsigned char abyOUI03[4] = { 0x00, 0x50, 0xf2, 0x03 };
-static const unsigned char abyOUI04[4] = { 0x00, 0x50, 0xf2, 0x04 };
-static const unsigned char abyOUI05[4] = { 0x00, 0x50, 0xf2, 0x05 };
-
-/*+
- *
- * Description:
- *    Clear RSN information in BSSList.
- *
- * Parameters:
- *  In:
- *      pBSSList - BSS list.
- *  Out:
- *      none
- *
- * Return Value: none.
- *
- -*/
-
-void
-WPA_ClearRSN(
-	PKnownBSS        pBSSList
-)
-{
-	int ii;
-
-	pBSSList->byGKType = WPA_TKIP;
-	for (ii = 0; ii < 4; ii++)
-		pBSSList->abyPKType[ii] = WPA_TKIP;
-	pBSSList->wPKCount = 0;
-	for (ii = 0; ii < 4; ii++)
-		pBSSList->abyAuthType[ii] = WPA_AUTH_IEEE802_1X;
-	pBSSList->wAuthCount = 0;
-	pBSSList->byDefaultK_as_PK = 0;
-	pBSSList->byReplayIdx = 0;
-	pBSSList->sRSNCapObj.bRSNCapExist = false;
-	pBSSList->sRSNCapObj.wRSNCap = 0;
-	pBSSList->bWPAValid = false;
-}
-
-/*+
- *
- * Description:
- *    Parse RSN IE.
- *
- * Parameters:
- *  In:
- *      pBSSList - BSS list.
- *      pRSN - Pointer to the RSN IE.
- *  Out:
- *      none
- *
- * Return Value: none.
- *
- -*/
-void
-WPA_ParseRSN(
-	PKnownBSS        pBSSList,
-	PWLAN_IE_RSN_EXT pRSN
-)
-{
-	PWLAN_IE_RSN_AUTH  pIE_RSN_Auth = NULL;
-	int                i, j, m, n = 0;
-	unsigned char *pbyCaps;
-
-	WPA_ClearRSN(pBSSList);
-
-	pr_debug("WPA_ParseRSN: [%d]\n", pRSN->len);
-
-	// information element header makes sense
-	if ((pRSN->len >= 6) // oui1(4)+ver(2)
-	    && (pRSN->byElementID == WLAN_EID_RSN_WPA) && !memcmp(pRSN->abyOUI, abyOUI01, 4)
-	    && (pRSN->wVersion == 1)) {
-		pr_debug("Legal RSN\n");
-		// update each variable if pRSN is long enough to contain the variable
-		if (pRSN->len >= 10) {
-			//OUI1(4)+ver(2)+GKSuite(4)
-			if (!memcmp(pRSN->abyMulticast, abyOUI01, 4))
-				pBSSList->byGKType = WPA_WEP40;
-			else if (!memcmp(pRSN->abyMulticast, abyOUI02, 4))
-				pBSSList->byGKType = WPA_TKIP;
-			else if (!memcmp(pRSN->abyMulticast, abyOUI03, 4))
-				pBSSList->byGKType = WPA_AESWRAP;
-			else if (!memcmp(pRSN->abyMulticast, abyOUI04, 4))
-				pBSSList->byGKType = WPA_AESCCMP;
-			else if (!memcmp(pRSN->abyMulticast, abyOUI05, 4))
-				pBSSList->byGKType = WPA_WEP104;
-			else
-				// any vendor checks here
-				pBSSList->byGKType = WPA_NONE;
-
-			pr_debug("byGKType: %x\n", pBSSList->byGKType);
-		}
-
-		if (pRSN->len >= 12) {
-			//oui1(4)+ver(2)+GKS(4)+PKSCnt(2)
-			j = 0;
-			pr_debug("wPKCount: %d, sizeof(pBSSList->abyPKType): %zu\n",
-				 pRSN->wPKCount, sizeof(pBSSList->abyPKType));
-			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;
-					else if (!memcmp(pRSN->PKSList[i].abyOUI, abyOUI02, 4))
-						pBSSList->abyPKType[j++] = WPA_TKIP;
-					else if (!memcmp(pRSN->PKSList[i].abyOUI, abyOUI03, 4))
-						pBSSList->abyPKType[j++] = WPA_AESWRAP;
-					else if (!memcmp(pRSN->PKSList[i].abyOUI, abyOUI04, 4))
-						pBSSList->abyPKType[j++] = WPA_AESCCMP;
-					else
-						// any vendor checks here
-						;
-				} else
-					break;
-			}
-			pBSSList->wPKCount = (unsigned short)j;
-			pr_debug("wPKCount: %d\n", pBSSList->wPKCount);
-		}
-
-		m = pRSN->wPKCount;
-		pr_debug("m: %d\n", m);
-		pr_debug("14+m*4: %d\n", 14+m*4);
-
-		if (pRSN->len >= 14+m*4) { //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)+PKS(4*m)+AKC(2)
-			// overlay IE_RSN_Auth structure into correct place
-			pIE_RSN_Auth = (PWLAN_IE_RSN_AUTH) pRSN->PKSList[m].abyOUI;
-			j = 0;
-			pr_debug("wAuthCount: %d, sizeof(pBSSList->abyAuthType): %zu\n",
-				 pIE_RSN_Auth->wAuthCount,
-				 sizeof(pBSSList->abyAuthType));
-			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;
-					else if (!memcmp(pIE_RSN_Auth->AuthKSList[i].abyOUI, abyOUI02, 4))
-						pBSSList->abyAuthType[j++] = WPA_AUTH_PSK;
-					else
-						// any vendor checks here
-						;
-				} else
-					break;
-
-			}
-			if (j > 0)
-				pBSSList->wAuthCount = (unsigned short)j;
-			pr_debug("wAuthCount: %d\n", pBSSList->wAuthCount);
-		}
-
-		if (pIE_RSN_Auth != NULL) {
-			n = pIE_RSN_Auth->wAuthCount;
-
-			pr_debug("n: %d\n", n);
-			pr_debug("14+4+(m+n)*4: %d\n", 14+4+(m+n)*4);
-
-			if (pRSN->len+2 >= 14+4+(m+n)*4) { //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)+PKS(4*m)+AKC(2)+AKS(4*n)+Cap(2)
-				pbyCaps = (unsigned char *)pIE_RSN_Auth->AuthKSList[n].abyOUI;
-				pBSSList->byDefaultK_as_PK = (*pbyCaps) & WPA_GROUPFLAG;
-				pBSSList->byReplayIdx = 2 << ((*pbyCaps >> WPA_REPLAYBITSSHIFT) & WPA_REPLAYBITS);
-				pBSSList->sRSNCapObj.bRSNCapExist = true;
-				pBSSList->sRSNCapObj.wRSNCap = *(unsigned short *)pbyCaps;
-			}
-		}
-		pBSSList->bWPAValid = true;
-	}
-}
-
-/*+
- *
- * Description:
- *    Search RSN information in BSSList.
- *
- * Parameters:
- *  In:
- *      byCmd    - Search type
- *      byEncrypt- Encrypt Type
- *      pBSSList - BSS list
- *  Out:
- *      none
- *
- * Return Value: none.
- *
- -*/
-bool
-WPA_SearchRSN(
-	unsigned char byCmd,
-	unsigned char byEncrypt,
-	PKnownBSS        pBSSList
-)
-{
-	int ii;
-	unsigned char byPKType = WPA_NONE;
-
-	if (!pBSSList->bWPAValid)
-		return false;
-
-	switch (byCmd) {
-	case 0:
-
-		if (byEncrypt != pBSSList->byGKType)
-			return false;
-
-		if (pBSSList->wPKCount > 0) {
-			for (ii = 0; ii < pBSSList->wPKCount; ii++) {
-				if (pBSSList->abyPKType[ii] == WPA_AESCCMP)
-					byPKType = WPA_AESCCMP;
-				else if ((pBSSList->abyPKType[ii] == WPA_TKIP) && (byPKType != WPA_AESCCMP))
-					byPKType = WPA_TKIP;
-				else if ((pBSSList->abyPKType[ii] == WPA_WEP40) && (byPKType != WPA_AESCCMP) && (byPKType != WPA_TKIP))
-					byPKType = WPA_WEP40;
-				else if ((pBSSList->abyPKType[ii] == WPA_WEP104) && (byPKType != WPA_AESCCMP) && (byPKType != WPA_TKIP))
-					byPKType = WPA_WEP104;
-			}
-			if (byEncrypt != byPKType)
-				return false;
-		}
-		return true;
-
-	default:
-		break;
-	}
-	return false;
-}
-
-/*+
- *
- * Description:
- *    Check if RSN IE makes sense.
- *
- * Parameters:
- *  In:
- *      pRSN - Pointer to the RSN IE.
- *  Out:
- *      none
- *
- * Return Value: none.
- *
- -*/
-bool
-WPAb_Is_RSN(
-	PWLAN_IE_RSN_EXT pRSN
-)
-{
-	if (pRSN == NULL)
-		return false;
-
-	if ((pRSN->len >= 6) && // oui1(4)+ver(2)
-	    (pRSN->byElementID == WLAN_EID_RSN_WPA) &&  !memcmp(pRSN->abyOUI, abyOUI01, 4) &&
-	    (pRSN->wVersion == 1)) {
-		return true;
-	} else
-		return false;
-}
diff --git a/drivers/staging/vt6655/wpa.h b/drivers/staging/vt6655/wpa.h
deleted file mode 100644
index 1d1918a12641..000000000000
--- a/drivers/staging/vt6655/wpa.h
+++ /dev/null
@@ -1,83 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: wpa.h
- *
- * Purpose: Defines the macros, types, and functions for dealing
- *          with WPA informations.
- *
- * Author: Kyle Hsu
- *
- * Date: Jul 14, 2003
- *
- */
-
-#ifndef __WPA_H__
-#define __WPA_H__
-
-#include "ttype.h"
-#include "80211hdr.h"
-
-/*---------------------  Export Definitions -------------------------*/
-
-#define WPA_NONE            0
-#define WPA_WEP40           1
-#define WPA_TKIP            2
-#define WPA_AESWRAP         3
-#define WPA_AESCCMP         4
-#define WPA_WEP104          5
-#define WPA_AUTH_IEEE802_1X 1
-#define WPA_AUTH_PSK        2
-
-#define WPA_GROUPFLAG       0x02
-#define WPA_REPLAYBITSSHIFT 2
-#define WPA_REPLAYBITS      0x03
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Types  ------------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-void
-WPA_ClearRSN(
-	PKnownBSS        pBSSList
-);
-
-void
-WPA_ParseRSN(
-	PKnownBSS        pBSSList,
-	PWLAN_IE_RSN_EXT pRSN
-);
-
-bool
-WPA_SearchRSN(
-	unsigned char byCmd,
-	unsigned char byEncrypt,
-	PKnownBSS        pBSSList
-);
-
-bool
-WPAb_Is_RSN(
-	PWLAN_IE_RSN_EXT pRSN
-);
-
-#endif // __WPA_H__
diff --git a/drivers/staging/vt6655/wpa2.c b/drivers/staging/vt6655/wpa2.c
deleted file mode 100644
index bb335ef51172..000000000000
--- a/drivers/staging/vt6655/wpa2.c
+++ /dev/null
@@ -1,359 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: wpa2.c
- *
- * Purpose: Handles the Basic Service Set & Node Database functions
- *
- * Functions:
- *
- * Revision History:
- *
- * Author: Yiching Chen
- *
- * Date: Oct. 4, 2004
- *
- */
-
-#include "wpa2.h"
-#include "device.h"
-#include "wmgr.h"
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-static const unsigned char abyOUIGK[4]      = { 0x00, 0x0F, 0xAC, 0x00 };
-static const unsigned char abyOUIWEP40[4]   = { 0x00, 0x0F, 0xAC, 0x01 };
-static const unsigned char abyOUIWEP104[4]  = { 0x00, 0x0F, 0xAC, 0x05 };
-static const unsigned char abyOUITKIP[4]    = { 0x00, 0x0F, 0xAC, 0x02 };
-static const unsigned char abyOUICCMP[4]    = { 0x00, 0x0F, 0xAC, 0x04 };
-
-static const unsigned char abyOUI8021X[4]   = { 0x00, 0x0F, 0xAC, 0x01 };
-static const unsigned char abyOUIPSK[4]     = { 0x00, 0x0F, 0xAC, 0x02 };
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-/*+
- *
- * Description:
- *    Clear RSN information in BSSList.
- *
- * Parameters:
- *  In:
- *      pBSSNode - BSS list.
- *  Out:
- *      none
- *
- * Return Value: none.
- *
- -*/
-void
-WPA2_ClearRSN(
-	PKnownBSS        pBSSNode
-)
-{
-	int ii;
-
-	pBSSNode->bWPA2Valid = false;
-
-	pBSSNode->byCSSGK = WLAN_11i_CSS_CCMP;
-	for (ii = 0; ii < 4; ii++)
-		pBSSNode->abyCSSPK[ii] = WLAN_11i_CSS_CCMP;
-	pBSSNode->wCSSPKCount = 1;
-	for (ii = 0; ii < 4; ii++)
-		pBSSNode->abyAKMSSAuthType[ii] = WLAN_11i_AKMSS_802_1X;
-	pBSSNode->wAKMSSAuthCount = 1;
-	pBSSNode->sRSNCapObj.bRSNCapExist = false;
-	pBSSNode->sRSNCapObj.wRSNCap = 0;
-}
-
-/*+
- *
- * Description:
- *    Parse RSN IE.
- *
- * Parameters:
- *  In:
- *      pBSSNode - BSS list.
- *      pRSN - Pointer to the RSN IE.
- *  Out:
- *      none
- *
- * Return Value: none.
- *
- -*/
-void
-WPA2vParseRSN(
-	PKnownBSS        pBSSNode,
-	PWLAN_IE_RSN     pRSN
-)
-{
-	int                 i, j;
-	unsigned short m = 0, n = 0;
-	unsigned char *pbyOUI;
-	bool bUseGK = false;
-
-	pr_debug("WPA2_ParseRSN: [%d]\n", pRSN->len);
-
-	WPA2_ClearRSN(pBSSNode);
-
-	if (pRSN->len == 2) { // ver(2)
-		if ((pRSN->byElementID == WLAN_EID_RSN) && (pRSN->wVersion == 1))
-			pBSSNode->bWPA2Valid = true;
-
-		return;
-	}
-
-	if (pRSN->len < 6) { // ver(2) + GK(4)
-		// invalid CSS, P802.11i/D10.0, p31
-		return;
-	}
-
-	// information element header makes sense
-	if ((pRSN->byElementID == WLAN_EID_RSN) &&
-	    (pRSN->wVersion == 1)) {
-		pr_debug("Legal 802.11i RSN\n");
-
-		pbyOUI = &(pRSN->abyRSN[0]);
-		if (!memcmp(pbyOUI, abyOUIWEP40, 4))
-			pBSSNode->byCSSGK = WLAN_11i_CSS_WEP40;
-		else if (!memcmp(pbyOUI, abyOUITKIP, 4))
-			pBSSNode->byCSSGK = WLAN_11i_CSS_TKIP;
-		else if (!memcmp(pbyOUI, abyOUICCMP, 4))
-			pBSSNode->byCSSGK = WLAN_11i_CSS_CCMP;
-		else if (!memcmp(pbyOUI, abyOUIWEP104, 4))
-			pBSSNode->byCSSGK = WLAN_11i_CSS_WEP104;
-		else if (!memcmp(pbyOUI, abyOUIGK, 4)) {
-			// invalid CSS, P802.11i/D10.0, p32
-			return;
-		} else
-			// any vendor checks here
-			pBSSNode->byCSSGK = WLAN_11i_CSS_UNKNOWN;
-
-		pr_debug("802.11i CSS: %X\n", pBSSNode->byCSSGK);
-
-		if (pRSN->len == 6) {
-			pBSSNode->bWPA2Valid = true;
-			return;
-		}
-
-		if (pRSN->len >= 8) { // ver(2) + GK(4) + PK count(2)
-			pBSSNode->wCSSPKCount = *((unsigned short *)&(pRSN->abyRSN[4]));
-			j = 0;
-			pbyOUI = &(pRSN->abyRSN[6]);
-
-			for (i = 0; (i < pBSSNode->wCSSPKCount) && (j < sizeof(pBSSNode->abyCSSPK)/sizeof(unsigned char)); i++) {
-				if (pRSN->len >= 8+i*4+4) { // ver(2)+GK(4)+PKCnt(2)+PKS(4*i)
-					if (!memcmp(pbyOUI, abyOUIGK, 4)) {
-						pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_USE_GROUP;
-						bUseGK = true;
-					} else if (!memcmp(pbyOUI, abyOUIWEP40, 4)) {
-						// Invalid CSS, continue to parsing
-					} else if (!memcmp(pbyOUI, abyOUITKIP, 4)) {
-						if (pBSSNode->byCSSGK != WLAN_11i_CSS_CCMP)
-							pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_TKIP;
-						else
-							; // Invalid CSS, continue to parsing
-					} else if (!memcmp(pbyOUI, abyOUICCMP, 4)) {
-						pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_CCMP;
-					} else if (!memcmp(pbyOUI, abyOUIWEP104, 4)) {
-						// Invalid CSS, continue to parsing
-					} else {
-						// any vendor checks here
-						pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_UNKNOWN;
-					}
-					pbyOUI += 4;
-					pr_debug("abyCSSPK[%d]: %X\n",
-						 j-1, pBSSNode->abyCSSPK[j-1]);
-				} else
-					break;
-			} //for
-
-			if (bUseGK) {
-				if (j != 1) {
-					// invalid CSS, This should be only PK CSS.
-					return;
-				}
-				if (pBSSNode->byCSSGK == WLAN_11i_CSS_CCMP) {
-					// invalid CSS, If CCMP is enable , PK can't be CSSGK.
-					return;
-				}
-			}
-			if ((pBSSNode->wCSSPKCount != 0) && (j == 0)) {
-				// invalid CSS, No valid PK.
-				return;
-			}
-			pBSSNode->wCSSPKCount = (unsigned short)j;
-			pr_debug("wCSSPKCount: %d\n", pBSSNode->wCSSPKCount);
-		}
-
-		m = *((unsigned short *)&(pRSN->abyRSN[4]));
-
-		if (pRSN->len >= 10+m*4) { // ver(2) + GK(4) + PK count(2) + PKS(4*m) + AKMSS count(2)
-			pBSSNode->wAKMSSAuthCount = *((unsigned short *)&(pRSN->abyRSN[6+4*m]));
-			j = 0;
-			pbyOUI = &(pRSN->abyRSN[8+4*m]);
-			for (i = 0; (i < pBSSNode->wAKMSSAuthCount) && (j < sizeof(pBSSNode->abyAKMSSAuthType)/sizeof(unsigned char)); i++) {
-				if (pRSN->len >= 10+(m+i)*4+4) { // ver(2)+GK(4)+PKCnt(2)+PKS(4*m)+AKMSS(2)+AKS(4*i)
-					if (!memcmp(pbyOUI, abyOUI8021X, 4))
-						pBSSNode->abyAKMSSAuthType[j++] = WLAN_11i_AKMSS_802_1X;
-					else if (!memcmp(pbyOUI, abyOUIPSK, 4))
-						pBSSNode->abyAKMSSAuthType[j++] = WLAN_11i_AKMSS_PSK;
-					else
-						// any vendor checks here
-						pBSSNode->abyAKMSSAuthType[j++] = WLAN_11i_AKMSS_UNKNOWN;
-					pr_debug("abyAKMSSAuthType[%d]: %X\n",
-						 j-1,
-						 pBSSNode->abyAKMSSAuthType[j-1]);
-				} else
-					break;
-			}
-			pBSSNode->wAKMSSAuthCount = (unsigned short)j;
-			pr_debug("wAKMSSAuthCount: %d\n",
-				 pBSSNode->wAKMSSAuthCount);
-
-			n = *((unsigned short *)&(pRSN->abyRSN[6+4*m]));
-			if (pRSN->len >= 12 + 4 * m + 4 * n) { // ver(2)+GK(4)+PKCnt(2)+PKS(4*m)+AKMSSCnt(2)+AKMSS(4*n)+Cap(2)
-				pBSSNode->sRSNCapObj.bRSNCapExist = true;
-				pBSSNode->sRSNCapObj.wRSNCap = *((unsigned short *)&(pRSN->abyRSN[8+4*m+4*n]));
-			}
-		}
-		//ignore PMKID lists bcs only (Re)Assocrequest has this field
-		pBSSNode->bWPA2Valid = true;
-	}
-}
-
-/*+
- *
- * Description:
- *    Set WPA IEs
- *
- * Parameters:
- *  In:
- *      pMgmtHandle - Pointer to management object
- *  Out:
- *      pRSNIEs     - Pointer to the RSN IE to set.
- *
- * Return Value: length of IEs.
- *
- -*/
-unsigned int
-WPA2uSetIEs(
-	void *pMgmtHandle,
-	PWLAN_IE_RSN pRSNIEs
-)
-{
-	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtHandle;
-	unsigned char *pbyBuffer = NULL;
-	unsigned int ii = 0;
-	unsigned short *pwPMKID = NULL;
-
-	if (pRSNIEs == NULL)
-		return 0;
-
-	if (((pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
-	     (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) &&
-	    (pMgmt->pCurrBSS != NULL)) {
-		/* WPA2 IE */
-		pbyBuffer = (unsigned char *)pRSNIEs;
-		pRSNIEs->byElementID = WLAN_EID_RSN;
-		pRSNIEs->len = 6; //Version(2)+GK(4)
-		pRSNIEs->wVersion = 1;
-		//Group Key Cipher Suite
-		pRSNIEs->abyRSN[0] = 0x00;
-		pRSNIEs->abyRSN[1] = 0x0F;
-		pRSNIEs->abyRSN[2] = 0xAC;
-		if (pMgmt->byCSSGK == KEY_CTL_WEP)
-			pRSNIEs->abyRSN[3] = pMgmt->pCurrBSS->byCSSGK;
-		else if (pMgmt->byCSSGK == KEY_CTL_TKIP)
-			pRSNIEs->abyRSN[3] = WLAN_11i_CSS_TKIP;
-		else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
-			pRSNIEs->abyRSN[3] = WLAN_11i_CSS_CCMP;
-		else
-			pRSNIEs->abyRSN[3] = WLAN_11i_CSS_UNKNOWN;
-
-		// Pairwise Key Cipher Suite
-		pRSNIEs->abyRSN[4] = 1;
-		pRSNIEs->abyRSN[5] = 0;
-		pRSNIEs->abyRSN[6] = 0x00;
-		pRSNIEs->abyRSN[7] = 0x0F;
-		pRSNIEs->abyRSN[8] = 0xAC;
-		if (pMgmt->byCSSPK == KEY_CTL_TKIP)
-			pRSNIEs->abyRSN[9] = WLAN_11i_CSS_TKIP;
-		else if (pMgmt->byCSSPK == KEY_CTL_CCMP)
-			pRSNIEs->abyRSN[9] = WLAN_11i_CSS_CCMP;
-		else if (pMgmt->byCSSPK == KEY_CTL_NONE)
-			pRSNIEs->abyRSN[9] = WLAN_11i_CSS_USE_GROUP;
-		else
-			pRSNIEs->abyRSN[9] = WLAN_11i_CSS_UNKNOWN;
-
-		pRSNIEs->len += 6;
-
-		// Auth Key Management Suite
-		pRSNIEs->abyRSN[10] = 1;
-		pRSNIEs->abyRSN[11] = 0;
-		pRSNIEs->abyRSN[12] = 0x00;
-		pRSNIEs->abyRSN[13] = 0x0F;
-		pRSNIEs->abyRSN[14] = 0xAC;
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)
-			pRSNIEs->abyRSN[15] = WLAN_11i_AKMSS_PSK;
-		else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)
-			pRSNIEs->abyRSN[15] = WLAN_11i_AKMSS_802_1X;
-		else
-			pRSNIEs->abyRSN[15] = WLAN_11i_AKMSS_UNKNOWN;
-
-		pRSNIEs->len += 6;
-
-		// RSN Capabilities
-		if (pMgmt->pCurrBSS->sRSNCapObj.bRSNCapExist == true) {
-			memcpy(&pRSNIEs->abyRSN[16], &pMgmt->pCurrBSS->sRSNCapObj.wRSNCap, 2);
-		} else {
-			pRSNIEs->abyRSN[16] = 0;
-			pRSNIEs->abyRSN[17] = 0;
-		}
-		pRSNIEs->len += 2;
-
-		if ((pMgmt->gsPMKIDCache.BSSIDInfoCount > 0) &&
-		    pMgmt->bRoaming &&
-		    (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)) {
-			// RSN PMKID
-			pwPMKID = (unsigned short *)(&pRSNIEs->abyRSN[18]);  // Point to PMKID count
-			*pwPMKID = 0;                               // Initialize PMKID count
-			pbyBuffer = &pRSNIEs->abyRSN[20];           // Point to PMKID list
-			for (ii = 0; ii < pMgmt->gsPMKIDCache.BSSIDInfoCount; ii++) {
-				if (!memcmp(&pMgmt->gsPMKIDCache.BSSIDInfo[ii].abyBSSID[0], pMgmt->abyCurrBSSID, ETH_ALEN)) {
-					(*pwPMKID)++;
-					memcpy(pbyBuffer, pMgmt->gsPMKIDCache.BSSIDInfo[ii].abyPMKID, 16);
-					pbyBuffer += 16;
-				}
-			}
-			if (*pwPMKID != 0)
-				pRSNIEs->len += (2 + (*pwPMKID)*16);
-			else
-				pbyBuffer = &pRSNIEs->abyRSN[18];
-		}
-		return pRSNIEs->len + WLAN_IEHDR_LEN;
-	}
-	return 0;
-}
diff --git a/drivers/staging/vt6655/wpa2.h b/drivers/staging/vt6655/wpa2.h
deleted file mode 100644
index 2d0bd2e515f3..000000000000
--- a/drivers/staging/vt6655/wpa2.h
+++ /dev/null
@@ -1,77 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: wpa2.h
- *
- * Purpose: Defines the macros, types, and functions for dealing
- *          with WPA2 informations.
- *
- * Author: Yiching Chen
- *
- * Date: Oct. 4, 2004
- *
- */
-
-#ifndef __WPA2_H__
-#define __WPA2_H__
-
-#include "ttype.h"
-#include "80211mgr.h"
-#include "80211hdr.h"
-#include "bssdb.h"
-
-/*---------------------  Export Definitions -------------------------*/
-#define MAX_PMKID_CACHE         16
-
-typedef struct tagsPMKIDInfo {
-	unsigned char abyBSSID[6];
-	unsigned char abyPMKID[16];
-} PMKIDInfo, *PPMKIDInfo;
-
-typedef struct tagSPMKIDCache {
-	unsigned long BSSIDInfoCount;
-	PMKIDInfo   BSSIDInfo[MAX_PMKID_CACHE];
-} SPMKIDCache, *PSPMKIDCache;
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Types  ------------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-void
-WPA2_ClearRSN(
-	PKnownBSS        pBSSNode
-);
-
-void
-WPA2vParseRSN(
-	PKnownBSS        pBSSNode,
-	PWLAN_IE_RSN     pRSN
-);
-
-unsigned int
-WPA2uSetIEs(
-	void *pMgmtHandle,
-	PWLAN_IE_RSN pRSNIEs
-);
-
-#endif // __WPA2_H__
diff --git a/drivers/staging/vt6655/wpactl.c b/drivers/staging/vt6655/wpactl.c
deleted file mode 100644
index dab1e8078652..000000000000
--- a/drivers/staging/vt6655/wpactl.c
+++ /dev/null
@@ -1,896 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: wpactl.c
- *
- * Purpose: handle wpa supplicant ioctl input/out functions
- *
- * Author: Lyndon Chen
- *
- * Date: Oct. 20, 2003
- *
- * Functions:
- *
- * Revision History:
- *
- */
-
-#include "wpactl.h"
-#include "key.h"
-#include "mac.h"
-#include "device.h"
-#include "wmgr.h"
-#include "iocmd.h"
-#include "iowpa.h"
-#include "rf.h"
-
-/*---------------------  Static Definitions -------------------------*/
-
-#define VIAWGET_WPA_MAX_BUF_SIZE 1024
-
-static const int frequency_list[] = {
-	2412, 2417, 2422, 2427, 2432, 2437, 2442,
-	2447, 2452, 2457, 2462, 2467, 2472, 2484
-};
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-static void wpadev_setup(struct net_device *dev)
-{
-	dev->type               = ARPHRD_IEEE80211;
-	dev->hard_header_len    = ETH_HLEN;
-	dev->mtu                = 2048;
-	dev->addr_len           = ETH_ALEN;
-	dev->tx_queue_len       = 1000;
-
-	memset(dev->broadcast, 0xFF, ETH_ALEN);
-
-	dev->flags              = IFF_BROADCAST|IFF_MULTICAST;
-}
-
-/*
- * Description:
- *      register netdev for wpa supplicant daemon
- *
- * Parameters:
- *  In:
- *      pDevice             -
- *      enable              -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static int wpa_init_wpadev(struct vnt_private *pDevice)
-{
-	struct vnt_private *wpadev_priv;
-	struct net_device *dev = pDevice->dev;
-	int ret = 0;
-
-	pDevice->wpadev = alloc_netdev(sizeof(*wpadev_priv), "vntwpa",
-				       NET_NAME_UNKNOWN, wpadev_setup);
-	if (pDevice->wpadev == NULL)
-		return -ENOMEM;
-
-	wpadev_priv = netdev_priv(pDevice->wpadev);
-	*wpadev_priv = *pDevice;
-	eth_hw_addr_inherit(pDevice->wpadev, dev);
-	pDevice->wpadev->base_addr = dev->base_addr;
-	pDevice->wpadev->irq = dev->irq;
-	pDevice->wpadev->mem_start = dev->mem_start;
-	pDevice->wpadev->mem_end = dev->mem_end;
-	ret = register_netdev(pDevice->wpadev);
-	if (ret) {
-		pr_debug("%s: register_netdev(WPA) failed!\n", dev->name);
-		free_netdev(pDevice->wpadev);
-		return -1;
-	}
-
-	if (pDevice->skb == NULL) {
-		pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
-		if (pDevice->skb == NULL)
-			return -ENOMEM;
-	}
-
-	pr_debug("%s: Registered netdev %s for WPA management\n",
-		 dev->name, pDevice->wpadev->name);
-
-	return 0;
-}
-
-/*
- * Description:
- *      unregister net_device (wpadev)
- *
- * Parameters:
- *  In:
- *      pDevice             -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static int wpa_release_wpadev(struct vnt_private *pDevice)
-{
-	if (pDevice->skb) {
-		dev_kfree_skb(pDevice->skb);
-		pDevice->skb = NULL;
-	}
-
-	if (pDevice->wpadev) {
-		pr_debug("%s: Netdevice %s unregistered\n",
-			 pDevice->dev->name, pDevice->wpadev->name);
-		unregister_netdev(pDevice->wpadev);
-		free_netdev(pDevice->wpadev);
-		pDevice->wpadev = NULL;
-	}
-
-	return 0;
-}
-
-/*
- * Description:
- *      Set enable/disable dev for wpa supplicant daemon
- *
- * Parameters:
- *  In:
- *      pDevice             -
- *      val                 -
- *  Out:
- *
- * Return Value:
- *
- */
-
-int wpa_set_wpadev(struct vnt_private *pDevice, int val)
-{
-	if (val)
-		return wpa_init_wpadev(pDevice);
-	else
-		return wpa_release_wpadev(pDevice);
-}
-
-/*
- * Description:
- *      Set WPA algorithm & keys
- *
- * Parameters:
- *  In:
- *      pDevice -
- *      param -
- *  Out:
- *
- * Return Value:
- *
- */
-
-int wpa_set_keys(struct vnt_private *pDevice, void *ctx,
-		 bool fcpfkernel) __must_hold(&pDevice->lock)
-{
-	struct viawget_wpa_param *param = ctx;
-	PSMgmtObject pMgmt = pDevice->pMgmt;
-	unsigned long dwKeyIndex = 0;
-	unsigned char abyKey[MAX_KEY_LEN];
-	unsigned char abySeq[MAX_KEY_LEN];
-	u64 KeyRSC;
-	unsigned char byKeyDecMode = KEY_CTL_WEP;
-	int ret = 0;
-	int uu, ii;
-
-	if (param->u.wpa_key.alg_name > WPA_ALG_CCMP ||
-	    param->u.wpa_key.key_len > MAX_KEY_LEN ||
-	    param->u.wpa_key.seq_len > MAX_KEY_LEN)
-		return -EINVAL;
-
-	pr_debug("param->u.wpa_key.alg_name = %d\n", param->u.wpa_key.alg_name);
-	if (param->u.wpa_key.alg_name == WPA_ALG_NONE) {
-		pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
-		pDevice->bEncryptionEnable = false;
-		pDevice->byKeyIndex = 0;
-		pDevice->bTransmitKey = false;
-		KeyvRemoveAllWEPKey(&(pDevice->sKey), pDevice->PortOffset);
-		for (uu = 0; uu < MAX_KEY_TABLE; uu++)
-			MACvDisableKeyEntry(pDevice->PortOffset, uu);
-
-		return ret;
-	}
-
-	if (param->u.wpa_key.key && fcpfkernel) {
-		memcpy(&abyKey[0], param->u.wpa_key.key, param->u.wpa_key.key_len);
-	} else {
-		spin_unlock_irq(&pDevice->lock);
-		if (param->u.wpa_key.key &&
-		    copy_from_user(&abyKey[0],
-				   (void __user *)param->u.wpa_key.key,
-				   param->u.wpa_key.key_len)) {
-			spin_lock_irq(&pDevice->lock);
-			return -EINVAL;
-		}
-		spin_lock_irq(&pDevice->lock);
-	}
-
-	dwKeyIndex = (unsigned long)(param->u.wpa_key.key_index);
-
-	if (param->u.wpa_key.alg_name == WPA_ALG_WEP) {
-		if (dwKeyIndex > 3) {
-			return -EINVAL;
-		} else {
-			if (param->u.wpa_key.set_tx) {
-				pDevice->byKeyIndex = (unsigned char)dwKeyIndex;
-				pDevice->bTransmitKey = true;
-				dwKeyIndex |= (1 << 31);
-			}
-			KeybSetDefaultKey(&(pDevice->sKey),
-					  dwKeyIndex & ~(BIT30 | USE_KEYRSC),
-					  param->u.wpa_key.key_len,
-					  NULL,
-					  abyKey,
-					  KEY_CTL_WEP,
-					  pDevice->PortOffset,
-					  pDevice->byLocalID);
-
-		}
-		pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-		pDevice->bEncryptionEnable = true;
-		return ret;
-	}
-
-	if (param->u.wpa_key.seq && fcpfkernel) {
-		memcpy(&abySeq[0], param->u.wpa_key.seq, param->u.wpa_key.seq_len);
-	} else {
-		spin_unlock_irq(&pDevice->lock);
-		if (param->u.wpa_key.seq &&
-		    copy_from_user(&abySeq[0],
-				   (void __user *)param->u.wpa_key.seq,
-				   param->u.wpa_key.seq_len)) {
-			spin_lock_irq(&pDevice->lock);
-			return -EINVAL;
-		}
-		spin_lock_irq(&pDevice->lock);
-	}
-
-	if (param->u.wpa_key.seq_len > 0) {
-		for (ii = 0; ii < param->u.wpa_key.seq_len; ii++) {
-			if (ii < 4)
-				KeyRSC |= (u64)(abySeq[ii] << (ii * 8));
-			else
-				KeyRSC |= (u64)(abySeq[ii] << ((ii-4) * 8));
-		}
-		dwKeyIndex |= 1 << 29;
-	}
-
-	if (param->u.wpa_key.key_index >= MAX_GROUP_KEY) {
-		pr_debug("return  dwKeyIndex > 3\n");
-		return -EINVAL;
-	}
-
-	if (param->u.wpa_key.alg_name == WPA_ALG_TKIP)
-		pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
-
-	if (param->u.wpa_key.alg_name == WPA_ALG_CCMP)
-		pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
-
-	if (param->u.wpa_key.set_tx)
-		dwKeyIndex |= (1 << 31);
-
-	if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled)
-		byKeyDecMode = KEY_CTL_CCMP;
-	else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled)
-		byKeyDecMode = KEY_CTL_TKIP;
-	else
-		byKeyDecMode = KEY_CTL_WEP;
-
-	/* Fix HCT test that set 256 bits KEY and Ndis802_11Encryption3Enabled */
-	if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
-		if (param->u.wpa_key.key_len == MAX_KEY_LEN)
-			byKeyDecMode = KEY_CTL_TKIP;
-		else if (param->u.wpa_key.key_len == WLAN_WEP40_KEYLEN)
-			byKeyDecMode = KEY_CTL_WEP;
-		else if (param->u.wpa_key.key_len == WLAN_WEP104_KEYLEN)
-			byKeyDecMode = KEY_CTL_WEP;
-	} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
-		if (param->u.wpa_key.key_len == WLAN_WEP40_KEYLEN)
-			byKeyDecMode = KEY_CTL_WEP;
-		else if (param->u.wpa_key.key_len == WLAN_WEP104_KEYLEN)
-			byKeyDecMode = KEY_CTL_WEP;
-	}
-
-	/* Check TKIP key length */
-	if ((byKeyDecMode == KEY_CTL_TKIP) &&
-	    (param->u.wpa_key.key_len != MAX_KEY_LEN)) {
-		/* TKIP Key must be 256 bits */
-		pr_debug("return- TKIP Key must be 256 bits!\n");
-		return -EINVAL;
-	}
-	/* Check AES key length */
-	if ((byKeyDecMode == KEY_CTL_CCMP) &&
-	    (param->u.wpa_key.key_len != AES_KEY_LEN)) {
-		/* AES Key must be 128 bits */
-		return -EINVAL;
-	}
-
-	/* spin_lock_irq(&pDevice->lock); */
-	if (is_broadcast_ether_addr(&param->addr[0]) || (param->addr == NULL)) {
-		/* If is_broadcast_ether_addr, set the key as every key entry's group key. */
-		pr_debug("Groupe Key Assign\n");
-
-		if (KeybSetAllGroupKey(&(pDevice->sKey),
-					dwKeyIndex,
-					param->u.wpa_key.key_len,
-					(u64 *) &KeyRSC,
-					(unsigned char *)abyKey,
-					byKeyDecMode,
-					pDevice->PortOffset,
-					pDevice->byLocalID) &&
-		    KeybSetDefaultKey(&(pDevice->sKey),
-				       dwKeyIndex,
-				       param->u.wpa_key.key_len,
-				       (u64 *) &KeyRSC,
-				       (unsigned char *)abyKey,
-				       byKeyDecMode,
-				       pDevice->PortOffset,
-				       pDevice->byLocalID)) {
-			pr_debug("GROUP Key Assign\n");
-
-		} else {
-			return -EINVAL;
-		}
-
-	} else {
-		pr_debug("Pairwise Key Assign\n");
-		/* BSSID not 0xffffffffffff */
-		/* Pairwise Key can't be WEP */
-		if (byKeyDecMode == KEY_CTL_WEP) {
-			pr_debug("Pairwise Key can't be WEP\n");
-			return -EINVAL;
-		}
-
-		dwKeyIndex |= (1 << 30); /* set pairwise key */
-		if (pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA)
-			return -EINVAL;
-
-		if (KeybSetKey(&(pDevice->sKey),
-			       &param->addr[0],
-			       dwKeyIndex,
-			       param->u.wpa_key.key_len,
-			       (u64 *) &KeyRSC,
-			       (unsigned char *)abyKey,
-			       byKeyDecMode,
-			       pDevice->PortOffset,
-			       pDevice->byLocalID)) {
-			pr_debug("Pairwise Key Set\n");
-
-		} else {
-			/* Key Table Full */
-			return -EINVAL;
-		}
-	} /* BSSID not 0xffffffffffff */
-	if ((ret == 0) && ((param->u.wpa_key.set_tx) != 0)) {
-		pDevice->byKeyIndex = (unsigned char)param->u.wpa_key.key_index;
-		pDevice->bTransmitKey = true;
-	}
-	pDevice->bEncryptionEnable = true;
-
-	return ret;
-}
-
-/*
- * Description:
- *      enable wpa auth & mode
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static int wpa_set_wpa(struct vnt_private *pDevice,
-		       struct viawget_wpa_param *param)
-{
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-
-	pMgmt->eAuthenMode = WMAC_AUTH_OPEN;
-	pMgmt->bShareKeyAlgorithm = false;
-
-	return 0;
-}
-
-/*
- * Description:
- *      set disassociate
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static int wpa_set_disassociate(struct vnt_private *pDevice,
-				struct viawget_wpa_param *param)
-{
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-
-	spin_lock_irq(&pDevice->lock);
-	if (pDevice->bLinkPass) {
-		if (!memcmp(param->addr, pMgmt->abyCurrBSSID, 6))
-			bScheduleCommand((void *)pDevice, WLAN_CMD_DISASSOCIATE, NULL);
-	}
-	spin_unlock_irq(&pDevice->lock);
-
-	return 0;
-}
-
-/*
- * Description:
- *      enable scan process
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static int wpa_set_scan(struct vnt_private *pDevice,
-			struct viawget_wpa_param *param)
-{
-	spin_lock_irq(&pDevice->lock);
-	BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
-	bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, NULL);
-	spin_unlock_irq(&pDevice->lock);
-
-	return 0;
-}
-
-/*
- * Description:
- *      get bssid
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static int wpa_get_bssid(struct vnt_private *pDevice,
-			 struct viawget_wpa_param *param)
-{
-	PSMgmtObject        pMgmt = pDevice->pMgmt;
-
-	memcpy(param->u.wpa_associate.bssid, pMgmt->abyCurrBSSID , 6);
-
-	return 0;
-}
-
-/*
- * Description:
- *      get bssid
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static int wpa_get_ssid(struct vnt_private *pDevice,
-			struct viawget_wpa_param *param)
-{
-	PSMgmtObject        pMgmt = pDevice->pMgmt;
-	PWLAN_IE_SSID       pItemSSID;
-
-	pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
-
-	memcpy(param->u.wpa_associate.ssid, pItemSSID->abySSID , pItemSSID->len);
-	param->u.wpa_associate.ssid_len = pItemSSID->len;
-
-	return 0;
-}
-
-/*
- * Description:
- *      get scan results
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static int wpa_get_scan(struct vnt_private *pDevice,
-			struct viawget_wpa_param *param)
-{
-	struct viawget_scan_result *scan_buf;
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	PWLAN_IE_SSID   pItemSSID;
-	PKnownBSS pBSS;
-	unsigned char *pBuf;
-	int ret = 0;
-	u16 count = 0;
-	u16 ii, jj;
-#if 1
-
-	unsigned char *ptempBSS;
-
-	ptempBSS = kmalloc(sizeof(KnownBSS), GFP_ATOMIC);
-
-	if (ptempBSS == NULL) {
-		pr_err("bubble sort kmalloc memory fail@@@\n");
-
-		ret = -ENOMEM;
-
-		return ret;
-
-	}
-
-	for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-		for (jj = 0; jj < MAX_BSS_NUM - ii - 1; jj++) {
-			if ((pMgmt->sBSSList[jj].bActive != true) ||
-
-			    ((pMgmt->sBSSList[jj].uRSSI > pMgmt->sBSSList[jj + 1].uRSSI) && (pMgmt->sBSSList[jj + 1].bActive != false))) {
-				memcpy(ptempBSS, &pMgmt->sBSSList[jj], sizeof(KnownBSS));
-
-				memcpy(&pMgmt->sBSSList[jj], &pMgmt->sBSSList[jj + 1], sizeof(KnownBSS));
-
-				memcpy(&pMgmt->sBSSList[jj + 1], ptempBSS, sizeof(KnownBSS));
-
-			}
-
-		}
-
-	}
-
-	kfree(ptempBSS);
-#endif
-
-//******mike:bubble sort by stronger RSSI*****//
-
-	count = 0;
-	pBSS = &(pMgmt->sBSSList[0]);
-	for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-		pBSS = &(pMgmt->sBSSList[ii]);
-		if (!pBSS->bActive)
-			continue;
-		count++;
-	}
-
-	pBuf = kcalloc(count, sizeof(struct viawget_scan_result), GFP_ATOMIC);
-
-	if (pBuf == NULL) {
-		ret = -ENOMEM;
-		return ret;
-	}
-	scan_buf = (struct viawget_scan_result *)pBuf;
-	pBSS = &(pMgmt->sBSSList[0]);
-	for (ii = 0, jj = 0; ii < MAX_BSS_NUM; ii++) {
-		pBSS = &(pMgmt->sBSSList[ii]);
-		if (pBSS->bActive) {
-			if (jj >= count)
-				break;
-			memcpy(scan_buf->bssid, pBSS->abyBSSID, WLAN_BSSID_LEN);
-			pItemSSID = (PWLAN_IE_SSID)pBSS->abySSID;
-			memcpy(scan_buf->ssid, pItemSSID->abySSID, pItemSSID->len);
-			scan_buf->ssid_len = pItemSSID->len;
-			scan_buf->freq = frequency_list[pBSS->uChannel-1];
-			scan_buf->caps = pBSS->wCapInfo;
-
-			if (pBSS->wWPALen != 0) {
-				scan_buf->wpa_ie_len = pBSS->wWPALen;
-				memcpy(scan_buf->wpa_ie, pBSS->byWPAIE, pBSS->wWPALen);
-			}
-			if (pBSS->wRSNLen != 0) {
-				scan_buf->rsn_ie_len = pBSS->wRSNLen;
-				memcpy(scan_buf->rsn_ie, pBSS->byRSNIE, pBSS->wRSNLen);
-			}
-			scan_buf = (struct viawget_scan_result *)((unsigned char *)scan_buf + sizeof(struct viawget_scan_result));
-			jj++;
-		}
-	}
-
-	if (jj < count)
-		count = jj;
-
-	if (copy_to_user(param->u.scan_results.buf, pBuf, sizeof(struct viawget_scan_result) * count))
-		ret = -EFAULT;
-
-	param->u.scan_results.scan_count = count;
-	pr_debug(" param->u.scan_results.scan_count = %d\n", count);
-
-		kfree(pBuf);
-	return ret;
-}
-
-/*
- * Description:
- *      set associate with AP
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static int wpa_set_associate(struct vnt_private *pDevice,
-			     struct viawget_wpa_param *param)
-{
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	PWLAN_IE_SSID   pItemSSID;
-	unsigned char abyNullAddr[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
-	unsigned char abyWPAIE[64];
-	bool bWepEnabled = false;
-
-	/* set key type & algorithm */
-	pr_debug("pairwise_suite = %d\n",
-		 param->u.wpa_associate.pairwise_suite);
-	pr_debug("group_suite = %d\n", param->u.wpa_associate.group_suite);
-	pr_debug("key_mgmt_suite = %d\n",
-		 param->u.wpa_associate.key_mgmt_suite);
-	pr_debug("auth_alg = %d\n", param->u.wpa_associate.auth_alg);
-	pr_debug("mode = %d\n", param->u.wpa_associate.mode);
-	pr_debug("wpa_ie_len = %d\n", param->u.wpa_associate.wpa_ie_len);
-
-	if (param->u.wpa_associate.wpa_ie_len) {
-		if (!param->u.wpa_associate.wpa_ie)
-			return -EINVAL;
-		if (param->u.wpa_associate.wpa_ie_len > sizeof(abyWPAIE))
-			return -EINVAL;
-		if (copy_from_user(&abyWPAIE[0], param->u.wpa_associate.wpa_ie, param->u.wpa_associate.wpa_ie_len))
-			return -EFAULT;
-	}
-
-	if (param->u.wpa_associate.mode == 1)
-		pMgmt->eConfigMode = WMAC_CONFIG_IBSS_STA;
-	else
-		pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
-	/* set ssid */
-	memset(pMgmt->abyDesireSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-	pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
-	pItemSSID->byElementID = WLAN_EID_SSID;
-	pItemSSID->len = param->u.wpa_associate.ssid_len;
-	memcpy(pItemSSID->abySSID, param->u.wpa_associate.ssid, pItemSSID->len);
-	/* set bssid */
-	if (memcmp(param->u.wpa_associate.bssid, &abyNullAddr[0], 6) != 0)
-		memcpy(pMgmt->abyDesireBSSID, param->u.wpa_associate.bssid, 6);
-	else
-		bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, pItemSSID->abySSID);
-
-	if (param->u.wpa_associate.wpa_ie_len == 0) {
-		if (param->u.wpa_associate.auth_alg & AUTH_ALG_SHARED_KEY)
-			pMgmt->eAuthenMode = WMAC_AUTH_SHAREKEY;
-		else
-			pMgmt->eAuthenMode = WMAC_AUTH_OPEN;
-	} else if (abyWPAIE[0] == RSN_INFO_ELEM) {
-		if (param->u.wpa_associate.key_mgmt_suite == KEY_MGMT_PSK)
-			pMgmt->eAuthenMode = WMAC_AUTH_WPA2PSK;
-		else
-			pMgmt->eAuthenMode = WMAC_AUTH_WPA2;
-	} else {
-		if (param->u.wpa_associate.key_mgmt_suite == KEY_MGMT_WPA_NONE)
-			pMgmt->eAuthenMode = WMAC_AUTH_WPANONE;
-		else if (param->u.wpa_associate.key_mgmt_suite == KEY_MGMT_PSK)
-			pMgmt->eAuthenMode = WMAC_AUTH_WPAPSK;
-		else
-			pMgmt->eAuthenMode = WMAC_AUTH_WPA;
-	}
-
-	switch (param->u.wpa_associate.pairwise_suite) {
-	case CIPHER_CCMP:
-		pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
-		break;
-	case CIPHER_TKIP:
-		pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
-		break;
-	case CIPHER_WEP40:
-	case CIPHER_WEP104:
-		pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-		bWepEnabled = true;
-		break;
-	case CIPHER_NONE:
-		if (param->u.wpa_associate.group_suite == CIPHER_CCMP)
-			pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
-		else
-			pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
-		break;
-	default:
-		pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
-	}
-
-//DavidWang add for WPA_supplicant support open/share mode
-
-	if (pMgmt->eAuthenMode == WMAC_AUTH_SHAREKEY) {
-		pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-		pMgmt->bShareKeyAlgorithm = true;
-	} else if (pMgmt->eAuthenMode == WMAC_AUTH_OPEN) {
-		if (!bWepEnabled)  pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
-		else pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-	}
-//mike save old encryption status
-	pDevice->eOldEncryptionStatus = pDevice->eEncryptionStatus;
-
-	if (pDevice->eEncryptionStatus !=  Ndis802_11EncryptionDisabled)
-		pDevice->bEncryptionEnable = true;
-	else
-		pDevice->bEncryptionEnable = false;
-	if (!((pMgmt->eAuthenMode == WMAC_AUTH_SHAREKEY) ||
-	      ((pMgmt->eAuthenMode == WMAC_AUTH_OPEN) && bWepEnabled)))  //DavidWang  //20080717-06,<Modify> by chester//Not to initial WEP
-		KeyvInitTable(&pDevice->sKey, pDevice->PortOffset);
-	spin_lock_irq(&pDevice->lock);
-	pDevice->bLinkPass = false;
-	memset(pMgmt->abyCurrBSSID, 0, 6);
-	pMgmt->eCurrState = WMAC_STATE_IDLE;
-	netif_stop_queue(pDevice->dev);
-	//20080701-02,<Add> by Mike Liu
-/*******search if ap_scan=2 ,which is associating request in hidden ssid mode ****/
-	{
-		PKnownBSS       pCurr = NULL;
-
-		pCurr = BSSpSearchBSSList(pDevice,
-					  pMgmt->abyDesireBSSID,
-					  pMgmt->abyDesireSSID,
-					  pMgmt->eConfigPHYMode
-);
-
-		if (pCurr == NULL) {
-			pr_debug("wpa_set_associate---->hidden mode site survey before associate.......\n");
-			bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, pMgmt->abyDesireSSID);
-		}
-	}
-/****************************************************************/
-	bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, NULL);
-	spin_unlock_irq(&pDevice->lock);
-
-	return 0;
-}
-
-/*
- * Description:
- *      wpa_ioctl main function supported for wpa supplicant
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      iw_point  -
- *  Out:
- *
- * Return Value:
- *
- */
-
-int wpa_ioctl(struct vnt_private *pDevice, struct iw_point *p)
-{
-	struct viawget_wpa_param *param;
-	int ret = 0;
-	int wpa_ioctl = 0;
-
-	if (p->length < sizeof(struct viawget_wpa_param) ||
-	    p->length > VIAWGET_WPA_MAX_BUF_SIZE || !p->pointer)
-		return -EINVAL;
-
-	param = kmalloc((int)p->length, GFP_KERNEL);
-	if (param == NULL)
-		return -ENOMEM;
-
-	if (copy_from_user(param, p->pointer, p->length)) {
-		ret = -EFAULT;
-		goto out;
-	}
-
-	switch (param->cmd) {
-	case VIAWGET_SET_WPA:
-		ret = wpa_set_wpa(pDevice, param);
-		pr_debug("VIAWGET_SET_WPA\n");
-		break;
-
-	case VIAWGET_SET_KEY:
-		pr_debug("VIAWGET_SET_KEY\n");
-		spin_lock_irq(&pDevice->lock);
-		ret = wpa_set_keys(pDevice, param, false);
-		spin_unlock_irq(&pDevice->lock);
-		break;
-
-	case VIAWGET_SET_SCAN:
-		pr_debug("VIAWGET_SET_SCAN\n");
-		ret = wpa_set_scan(pDevice, param);
-		break;
-
-	case VIAWGET_GET_SCAN:
-		pr_debug("VIAWGET_GET_SCAN\n");
-		ret = wpa_get_scan(pDevice, param);
-		wpa_ioctl = 1;
-		break;
-
-	case VIAWGET_GET_SSID:
-		pr_debug("VIAWGET_GET_SSID\n");
-		ret = wpa_get_ssid(pDevice, param);
-		wpa_ioctl = 1;
-		break;
-
-	case VIAWGET_GET_BSSID:
-		pr_debug("VIAWGET_GET_BSSID\n");
-		ret = wpa_get_bssid(pDevice, param);
-		wpa_ioctl = 1;
-		break;
-
-	case VIAWGET_SET_ASSOCIATE:
-		pr_debug("VIAWGET_SET_ASSOCIATE\n");
-		ret = wpa_set_associate(pDevice, param);
-		break;
-
-	case VIAWGET_SET_DISASSOCIATE:
-		pr_debug("VIAWGET_SET_DISASSOCIATE\n");
-		ret = wpa_set_disassociate(pDevice, param);
-		break;
-
-	case VIAWGET_SET_DROP_UNENCRYPT:
-		pr_debug("VIAWGET_SET_DROP_UNENCRYPT\n");
-		break;
-
-	case VIAWGET_SET_DEAUTHENTICATE:
-		pr_debug("VIAWGET_SET_DEAUTHENTICATE\n");
-		break;
-
-	default:
-		pr_debug("wpa_ioctl: unknown cmd=%d\n",
-			 param->cmd);
-		ret = -EOPNOTSUPP;
-		goto out;
-	}
-
-	if ((ret == 0) && wpa_ioctl) {
-		if (copy_to_user(p->pointer, param, p->length)) {
-			ret = -EFAULT;
-			goto out;
-		}
-	}
-
-out:
-	kfree(param);
-
-	return ret;
-}
diff --git a/drivers/staging/vt6655/wpactl.h b/drivers/staging/vt6655/wpactl.h
deleted file mode 100644
index c1b4a7292061..000000000000
--- a/drivers/staging/vt6655/wpactl.h
+++ /dev/null
@@ -1,64 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: wpactl.h
- *
- * Purpose:
- *
- * Author: Lyndon Chen
- *
- * Date: March 1, 2005
- *
- */
-
-#ifndef __WPACTL_H__
-#define __WPACTL_H__
-
-#include "device.h"
-#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-#include "iowpa.h"
-#endif
-
-/*---------------------  Export Definitions -------------------------*/
-
-//WPA related
-
-enum wpa_alg { WPA_ALG_NONE, WPA_ALG_WEP, WPA_ALG_TKIP, WPA_ALG_CCMP };
-enum wpa_cipher { CIPHER_NONE, CIPHER_WEP40, CIPHER_TKIP, CIPHER_CCMP,
-		  CIPHER_WEP104 };
-enum wpa_key_mgmt { KEY_MGMT_802_1X, KEY_MGMT_CCKM, KEY_MGMT_PSK, KEY_MGMT_NONE,
-		    KEY_MGMT_802_1X_NO_WPA, KEY_MGMT_WPA_NONE };
-
-#define AUTH_ALG_OPEN_SYSTEM	0x01
-#define AUTH_ALG_SHARED_KEY	0x02
-#define AUTH_ALG_LEAP		0x04
-
-#define GENERIC_INFO_ELEM 0xdd
-#define RSN_INFO_ELEM 0x30
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-int wpa_set_wpadev(struct vnt_private *, int val);
-int wpa_ioctl(struct vnt_private *, struct iw_point *p);
-int wpa_set_keys(struct vnt_private *, void *ctx, bool fcpfkernel);
-
-#endif // __WPACL_H__
diff --git a/drivers/staging/vt6655/wroute.c b/drivers/staging/vt6655/wroute.c
deleted file mode 100644
index d1171fa3446e..000000000000
--- a/drivers/staging/vt6655/wroute.c
+++ /dev/null
@@ -1,187 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: wroute.c
- *
- * Purpose: handle WMAC frame relay & filtering
- *
- * Author: Lyndon Chen
- *
- * Date: May 20, 2003
- *
- * Functions:
- *      ROUTEbRelay - Relay packet
- *
- * Revision History:
- *
- */
-
-#include "mac.h"
-#include "tcrc.h"
-#include "rxtx.h"
-#include "wroute.h"
-#include "card.h"
-#include "baseband.h"
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*
- * Description:
- *      Relay packet.  Return true if packet is copy to DMA1
- *
- * Parameters:
- *  In:
- *      pDevice             -
- *      pbySkbData          - rx packet skb data
- *  Out:
- *      true, false
- *
- * Return Value: true if packet duplicate; otherwise false
- *
- */
-bool ROUTEbRelay(struct vnt_private *pDevice, unsigned char *pbySkbData,
-		 unsigned int uDataLen, unsigned int uNodeIndex)
-{
-	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	PSTxDesc        pHeadTD, pLastTD;
-	unsigned int cbFrameBodySize;
-	unsigned int uMACfragNum;
-	unsigned char byPktType;
-	bool bNeedEncryption = false;
-	SKeyItem        STempKey;
-	PSKeyItem       pTransmitKey = NULL;
-	unsigned int cbHeaderSize;
-	unsigned int ii;
-	unsigned char *pbyBSSID;
-
-	if (AVAIL_TD(pDevice, TYPE_AC0DMA) <= 0) {
-		pr_debug("Relay can't allocate TD1..\n");
-		return false;
-	}
-
-	pHeadTD = pDevice->apCurrTD[TYPE_AC0DMA];
-
-	pHeadTD->m_td1TD1.byTCR = (TCR_EDP | TCR_STP);
-
-	memcpy(pDevice->sTxEthHeader.abyDstAddr, pbySkbData, ETH_HLEN);
-
-	cbFrameBodySize = uDataLen - ETH_HLEN;
-
-	if (ntohs(pDevice->sTxEthHeader.wType) > ETH_DATA_LEN)
-		cbFrameBodySize += 8;
-
-	if (pDevice->bEncryptionEnable == true) {
-		bNeedEncryption = true;
-
-		// get group key
-		pbyBSSID = pDevice->abyBroadcastAddr;
-		if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID,
-		    GROUP_KEY, &pTransmitKey) == false) {
-			pTransmitKey = NULL;
-			pr_debug("KEY is NULL. [%d]\n",
-				 pDevice->pMgmt->eCurrMode);
-		} else {
-			pr_debug("Get GTK\n");
-		}
-	}
-
-	if (pDevice->bEnableHostWEP) {
-		if (uNodeIndex < MAX_NODE_NUM + 1) {
-			pTransmitKey = &STempKey;
-			pTransmitKey->byCipherSuite = pMgmt->sNodeDBTable[uNodeIndex].byCipherSuite;
-			pTransmitKey->dwKeyIndex = pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex;
-			pTransmitKey->uKeyLength = pMgmt->sNodeDBTable[uNodeIndex].uWepKeyLength;
-			pTransmitKey->dwTSC47_16 = pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16;
-			pTransmitKey->wTSC15_0 = pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0;
-			memcpy(pTransmitKey->abyKey,
-			       &pMgmt->sNodeDBTable[uNodeIndex].abyWepKey[0],
-			       pTransmitKey->uKeyLength);
-		}
-	}
-
-	uMACfragNum = cbGetFragCount(pDevice, pTransmitKey,
-				     cbFrameBodySize, &pDevice->sTxEthHeader);
-
-	if (uMACfragNum > AVAIL_TD(pDevice, TYPE_AC0DMA))
-		return false;
-
-	byPktType = pDevice->byPacketType;
-
-	if (pDevice->bFixRate) {
-		if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
-			if (pDevice->uConnectionRate >= RATE_11M)
-				pDevice->wCurrentRate = RATE_11M;
-			else
-				pDevice->wCurrentRate = pDevice->uConnectionRate;
-		} else {
-			if ((pDevice->eCurrentPHYType == PHY_TYPE_11A) &&
-			    (pDevice->uConnectionRate <= RATE_6M)) {
-				pDevice->wCurrentRate = RATE_6M;
-			} else {
-				if (pDevice->uConnectionRate >= RATE_54M)
-					pDevice->wCurrentRate = RATE_54M;
-				else
-					pDevice->wCurrentRate = pDevice->uConnectionRate;
-			}
-		}
-	} else {
-		pDevice->wCurrentRate = pDevice->pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate;
-	}
-
-	if (pDevice->wCurrentRate <= RATE_11M)
-		byPktType = PK_TYPE_11B;
-
-	vGenerateFIFOHeader(pDevice, byPktType, pDevice->pbyTmpBuff,
-			    bNeedEncryption, cbFrameBodySize, TYPE_AC0DMA,
-			    pHeadTD, &pDevice->sTxEthHeader, pbySkbData,
-			    pTransmitKey, uNodeIndex, &uMACfragNum,
-			    &cbHeaderSize);
-
-	if (MACbIsRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PS)) {
-		// Disable PS
-		MACbPSWakeup(pDevice->PortOffset);
-	}
-
-	pDevice->bPWBitOn = false;
-
-	pLastTD = pHeadTD;
-	for (ii = 0; ii < uMACfragNum; ii++) {
-		// Poll Transmit the adapter
-		wmb();
-		pHeadTD->m_td0TD0.f1Owner = OWNED_BY_NIC;
-		wmb();
-		if (ii == (uMACfragNum - 1))
-			pLastTD = pHeadTD;
-		pHeadTD = pHeadTD->next;
-	}
-
-	pLastTD->pTDInfo->skb = NULL;
-	pLastTD->pTDInfo->byFlags = 0;
-
-	pDevice->apCurrTD[TYPE_AC0DMA] = pHeadTD;
-
-	MACvTransmitAC0(pDevice->PortOffset);
-
-	return true;
-}
diff --git a/drivers/staging/vt6655/wroute.h b/drivers/staging/vt6655/wroute.h
deleted file mode 100644
index e59eec955cac..000000000000
--- a/drivers/staging/vt6655/wroute.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: wroute.h
- *
- * Purpose:
- *
- * Author: Lyndon Chen
- *
- * Date: May 21, 2003
- *
- */
-
-#ifndef __WROUTE_H__
-#define __WROUTE_H__
-
-#include "device.h"
-
-/*---------------------  Export Definitions -------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-bool ROUTEbRelay(struct vnt_private *pDevice, unsigned char *pbySkbData,
-		 unsigned int uDataLen, unsigned int uNodeIndex);
-
-#endif /* __WROUTE_H__ */
diff --git a/drivers/staging/vt6656/main_usb.c b/drivers/staging/vt6656/main_usb.c
index dbc311c3dc37..b95d5b1efcc7 100644
--- a/drivers/staging/vt6656/main_usb.c
+++ b/drivers/staging/vt6656/main_usb.c
@@ -431,11 +431,8 @@ static bool vnt_alloc_bufs(struct vnt_private *priv)
 	for (ii = 0; ii < priv->num_tx_context; ii++) {
 		tx_context = kmalloc(sizeof(struct vnt_usb_send_context),
 								GFP_KERNEL);
-		if (tx_context == NULL) {
-			dev_err(&priv->usb->dev,
-					"allocate tx usb context failed\n");
+		if (tx_context == NULL)
 			goto free_tx;
-		}
 
 		priv->tx_context[ii] = tx_context;
 		tx_context->priv = priv;
@@ -471,10 +468,8 @@ static bool vnt_alloc_bufs(struct vnt_private *priv)
 		}
 
 		rcb->skb = dev_alloc_skb(priv->rx_buf_sz);
-		if (rcb->skb == NULL) {
-			dev_err(&priv->usb->dev, "Failed to alloc rx skb\n");
+		if (rcb->skb == NULL)
 			goto free_rx_tx;
-		}
 
 		rcb->in_use = false;
 
@@ -491,7 +486,6 @@ static bool vnt_alloc_bufs(struct vnt_private *priv)
 
 	priv->int_buf.data_buf = kmalloc(MAX_INTERRUPT_SIZE, GFP_KERNEL);
 	if (priv->int_buf.data_buf == NULL) {
-		dev_err(&priv->usb->dev, "Failed to alloc int buf\n");
 		usb_free_urb(priv->interrupt_urb);
 		goto free_rx_tx;
 	}
diff --git a/drivers/staging/wlan-ng/hfa384x.h b/drivers/staging/wlan-ng/hfa384x.h
index 1f2c78cc0086..20d146b61ba7 100644
--- a/drivers/staging/wlan-ng/hfa384x.h
+++ b/drivers/staging/wlan-ng/hfa384x.h
@@ -1376,6 +1376,7 @@ int hfa384x_drvr_setconfig(hfa384x_t *hw, u16 rid, void *buf, u16 len);
 static inline int hfa384x_drvr_getconfig16(hfa384x_t *hw, u16 rid, void *val)
 {
 	int result = 0;
+
 	result = hfa384x_drvr_getconfig(hw, rid, val, sizeof(u16));
 	if (result == 0)
 		*((u16 *) val) = le16_to_cpu(*((u16 *) val));
@@ -1385,6 +1386,7 @@ static inline int hfa384x_drvr_getconfig16(hfa384x_t *hw, u16 rid, void *val)
 static inline int hfa384x_drvr_setconfig16(hfa384x_t *hw, u16 rid, u16 val)
 {
 	u16 value = cpu_to_le16(val);
+
 	return hfa384x_drvr_setconfig(hw, rid, &value, sizeof(value));
 }
 
@@ -1402,6 +1404,7 @@ static inline int
 hfa384x_drvr_setconfig16_async(hfa384x_t *hw, u16 rid, u16 val)
 {
 	u16 value = cpu_to_le16(val);
+
 	return hfa384x_drvr_setconfig_async(hw, rid, &value, sizeof(value),
 					    NULL, NULL);
 }
diff --git a/drivers/staging/wlan-ng/hfa384x_usb.c b/drivers/staging/wlan-ng/hfa384x_usb.c
index 898bde73c59a..55d2f563e308 100644
--- a/drivers/staging/wlan-ng/hfa384x_usb.c
+++ b/drivers/staging/wlan-ng/hfa384x_usb.c
@@ -3583,12 +3583,8 @@ static void hfa384x_int_rxmonitor(wlandevice_t *wlandev,
 	}
 
 	skb = dev_alloc_skb(skblen);
-	if (skb == NULL) {
-		netdev_err(hw->wlandev->netdev,
-			   "alloc_skb failed trying to allocate %d bytes\n",
-			   skblen);
+	if (skb == NULL)
 		return;
-	}
 
 	/* only prepend the prism header if in the right mode */
 	if ((wlandev->netdev->type == ARPHRD_IEEE80211_PRISM) &&
diff --git a/drivers/staging/wlan-ng/p80211conv.c b/drivers/staging/wlan-ng/p80211conv.c
index 3b5468c64fde..7eaaf9a63503 100644
--- a/drivers/staging/wlan-ng/p80211conv.c
+++ b/drivers/staging/wlan-ng/p80211conv.c
@@ -107,7 +107,7 @@ int skb_ether_to_p80211(wlandevice_t *wlandev, u32 ethconv,
 			struct p80211_metawep *p80211_wep)
 {
 
-	u16 fc;
+	__le16 fc;
 	u16 proto;
 	struct wlan_ethhdr e_hdr;
 	struct wlan_llc *e_llc;
diff --git a/drivers/staging/wlan-ng/p80211hdr.h b/drivers/staging/wlan-ng/p80211hdr.h
index 66b5e201d418..79d9b20b364d 100644
--- a/drivers/staging/wlan-ng/p80211hdr.h
+++ b/drivers/staging/wlan-ng/p80211hdr.h
@@ -148,7 +148,7 @@
 /* Generic 802.11 Header types */
 
 struct p80211_hdr_a3 {
-	u16 fc;
+	__le16 fc;
 	u16 dur;
 	u8 a1[ETH_ALEN];
 	u8 a2[ETH_ALEN];
diff --git a/drivers/staging/wlan-ng/p80211netdev.c b/drivers/staging/wlan-ng/p80211netdev.c
index 2dd9bf8a6e18..a9c1e0bafa62 100644
--- a/drivers/staging/wlan-ng/p80211netdev.c
+++ b/drivers/staging/wlan-ng/p80211netdev.c
@@ -358,7 +358,7 @@ static int p80211knetdev_hard_start_xmit(struct sk_buff *skb,
 		 * and return success .
 		 * TODO: we need a saner way to handle this
 		 */
-		if (skb->protocol != ETH_P_80211_RAW) {
+		if (be16_to_cpu(skb->protocol) != ETH_P_80211_RAW) {
 			netif_start_queue(wlandev->netdev);
 			netdev_notice(netdev, "Tx attempt prior to association, frame dropped.\n");
 			netdev->stats.tx_dropped++;
@@ -369,7 +369,7 @@ static int p80211knetdev_hard_start_xmit(struct sk_buff *skb,
 	}
 
 	/* Check for raw transmits */
-	if (skb->protocol == ETH_P_80211_RAW) {
+	if (be16_to_cpu(skb->protocol) == ETH_P_80211_RAW) {
 		if (!capable(CAP_NET_ADMIN)) {
 			result = 1;
 			goto failed;
diff --git a/drivers/staging/wlan-ng/prism2fw.c b/drivers/staging/wlan-ng/prism2fw.c
index 6c38f797d1ab..9408644cc8b8 100644
--- a/drivers/staging/wlan-ng/prism2fw.c
+++ b/drivers/staging/wlan-ng/prism2fw.c
@@ -238,7 +238,8 @@ static int prism2_fwtry(struct usb_device *udev, wlandevice_t *wlandev)
 *	0	- success
 *	~0	- failure
 ----------------------------------------------------------------*/
-static int prism2_fwapply(const struct ihex_binrec *rfptr, wlandevice_t *wlandev)
+static int prism2_fwapply(const struct ihex_binrec *rfptr,
+			  wlandevice_t *wlandev)
 {
 	signed int result = 0;
 	struct p80211msg_dot11req_mibget getmsg;
@@ -986,8 +987,8 @@ static int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 	u32 currlen;
 	u32 currdaddr;
 
-	rstmsg = kmalloc(sizeof(*rstmsg), GFP_KERNEL);
-	rwrmsg = kmalloc(sizeof(*rwrmsg), GFP_KERNEL);
+	rstmsg = kzalloc(sizeof(*rstmsg), GFP_KERNEL);
+	rwrmsg = kzalloc(sizeof(*rwrmsg), GFP_KERNEL);
 	if (!rstmsg || !rwrmsg) {
 		kfree(rstmsg);
 		kfree(rwrmsg);
@@ -997,7 +998,6 @@ static int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 	}
 
 	/* Initialize the messages */
-	memset(rstmsg, 0, sizeof(*rstmsg));
 	strcpy(rstmsg->devname, wlandev->name);
 	rstmsg->msgcode = DIDmsg_p2req_ramdl_state;
 	rstmsg->msglen = sizeof(*rstmsg);
@@ -1011,7 +1011,6 @@ static int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 	rstmsg->exeaddr.len = sizeof(u32);
 	rstmsg->resultcode.len = sizeof(u32);
 
-	memset(rwrmsg, 0, sizeof(*rwrmsg));
 	strcpy(rwrmsg->devname, wlandev->name);
 	rwrmsg->msgcode = DIDmsg_p2req_ramdl_write;
 	rwrmsg->msglen = sizeof(*rwrmsg);
diff --git a/drivers/staging/xgifb/XGI_main_26.c b/drivers/staging/xgifb/XGI_main_26.c
index be7778b59118..709d49e7f3c9 100644
--- a/drivers/staging/xgifb/XGI_main_26.c
+++ b/drivers/staging/xgifb/XGI_main_26.c
@@ -2012,7 +2012,7 @@ static int xgifb_probe(struct pci_dev *pdev,
 	XGIfb_get_fix(&fb_info->fix, -1, fb_info);
 	fb_info->pseudo_palette = xgifb_info->pseudo_palette;
 
-	fb_alloc_cmap(&fb_info->cmap, 256 , 0);
+	fb_alloc_cmap(&fb_info->cmap, 256, 0);
 
 #ifdef CONFIG_MTRR
 	xgifb_info->mtrr = mtrr_add(xgifb_info->video_base,
diff --git a/drivers/staging/xgifb/vb_def.h b/drivers/staging/xgifb/vb_def.h
index 481eb174fdf0..d9524a2e9ce4 100644
--- a/drivers/staging/xgifb/vb_def.h
+++ b/drivers/staging/xgifb/vb_def.h
@@ -7,7 +7,6 @@
 #define SupportCRT2in301C       0x0100  /* for 301C */
 #define SetCHTVOverScan         0x8000
 
-#define Panel_320x480            0x07 /*fstn*/
 #define PanelResInfo            0x1F /* CR36 Panel Type/LCDResInfo */
 #define Panel_1024x768x75        0x22
 #define Panel_1280x1024x75       0x23
diff --git a/drivers/staging/xgifb/vb_setmode.c b/drivers/staging/xgifb/vb_setmode.c
index d5f49d2a8db3..1f6f699e238c 100644
--- a/drivers/staging/xgifb/vb_setmode.c
+++ b/drivers/staging/xgifb/vb_setmode.c
@@ -4135,7 +4135,7 @@ static void XGI_SetGroup4(unsigned short ModeIdIndex,
 		tempax -= 1;
 
 		temp = (tempax & 0xFF00) >> 8;
-		temp = ((temp & 0x0003) << 4);
+		temp = (temp & 0x0003) << 4;
 		xgifb_reg_set(pVBInfo->Part4Port, 0x1E, temp);
 		temp = (tempax & 0x00FF);
 		xgifb_reg_set(pVBInfo->Part4Port, 0x1D, temp);
diff --git a/drivers/staging/xgifb/vb_util.c b/drivers/staging/xgifb/vb_util.c
index 1b452f8b6274..be3437ca339e 100644
--- a/drivers/staging/xgifb/vb_util.c
+++ b/drivers/staging/xgifb/vb_util.c
@@ -9,11 +9,8 @@ void xgifb_reg_set(unsigned long port, u8 index, u8 data)
 
 u8 xgifb_reg_get(unsigned long port, u8 index)
 {
-	u8 data;
-
 	outb(index, port);
-	data = inb(port + 1);
-	return data;
+	return inb(port + 1);
 }
 
 void xgifb_reg_and_or(unsigned long port, u8 index,
diff --git a/drivers/target/iscsi/iscsi_target.c b/drivers/target/iscsi/iscsi_target.c
index 73e58d22e325..55f6774f706f 100644
--- a/drivers/target/iscsi/iscsi_target.c
+++ b/drivers/target/iscsi/iscsi_target.c
@@ -609,6 +609,7 @@ static int __init iscsi_target_init_module(void)
 
 	return ret;
 r2t_out:
+	iscsit_unregister_transport(&iscsi_target_transport);
 	kmem_cache_destroy(lio_r2t_cache);
 ooo_out:
 	kmem_cache_destroy(lio_ooo_cache);
@@ -943,17 +944,17 @@ int iscsit_setup_scsi_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 	 */
 	if ((iscsi_task_attr == ISCSI_ATTR_UNTAGGED) ||
 	    (iscsi_task_attr == ISCSI_ATTR_SIMPLE))
-		sam_task_attr = MSG_SIMPLE_TAG;
+		sam_task_attr = TCM_SIMPLE_TAG;
 	else if (iscsi_task_attr == ISCSI_ATTR_ORDERED)
-		sam_task_attr = MSG_ORDERED_TAG;
+		sam_task_attr = TCM_ORDERED_TAG;
 	else if (iscsi_task_attr == ISCSI_ATTR_HEAD_OF_QUEUE)
-		sam_task_attr = MSG_HEAD_TAG;
+		sam_task_attr = TCM_HEAD_TAG;
 	else if (iscsi_task_attr == ISCSI_ATTR_ACA)
-		sam_task_attr = MSG_ACA_TAG;
+		sam_task_attr = TCM_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;
+			" TCM_SIMPLE_TAG\n", iscsi_task_attr);
+		sam_task_attr = TCM_SIMPLE_TAG;
 	}
 
 	cmd->iscsi_opcode	= ISCSI_OP_SCSI_CMD;
@@ -1811,7 +1812,7 @@ iscsit_handle_task_mgt_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 		transport_init_se_cmd(&cmd->se_cmd,
 				      &lio_target_fabric_configfs->tf_ops,
 				      conn->sess->se_sess, 0, DMA_NONE,
-				      MSG_SIMPLE_TAG, cmd->sense_buffer + 2);
+				      TCM_SIMPLE_TAG, cmd->sense_buffer + 2);
 
 		target_get_sess_cmd(conn->sess->se_sess, &cmd->se_cmd, true);
 		sess_ref = true;
diff --git a/drivers/target/iscsi/iscsi_target_core.h b/drivers/target/iscsi/iscsi_target_core.h
index 302eb3b78715..09a522bae222 100644
--- a/drivers/target/iscsi/iscsi_target_core.h
+++ b/drivers/target/iscsi/iscsi_target_core.h
@@ -790,7 +790,6 @@ struct iscsi_np {
 	void			*np_context;
 	struct iscsit_transport *np_transport;
 	struct list_head	np_list;
-	struct iscsi_tpg_np	*tpg_np;
 } ____cacheline_aligned;
 
 struct iscsi_tpg_np {
diff --git a/drivers/target/iscsi/iscsi_target_login.c b/drivers/target/iscsi/iscsi_target_login.c
index 480f2e0ecc11..713c0c1877ab 100644
--- a/drivers/target/iscsi/iscsi_target_login.c
+++ b/drivers/target/iscsi/iscsi_target_login.c
@@ -281,7 +281,6 @@ static int iscsi_login_zero_tsih_s1(
 {
 	struct iscsi_session *sess = NULL;
 	struct iscsi_login_req *pdu = (struct iscsi_login_req *)buf;
-	enum target_prot_op sup_pro_ops;
 	int ret;
 
 	sess = kzalloc(sizeof(struct iscsi_session), GFP_KERNEL);
@@ -343,9 +342,8 @@ static int iscsi_login_zero_tsih_s1(
 		kfree(sess);
 		return -ENOMEM;
 	}
-	sup_pro_ops = conn->conn_transport->iscsit_get_sup_prot_ops(conn);
 
-	sess->se_sess = transport_init_session(sup_pro_ops);
+	sess->se_sess = transport_init_session(TARGET_PROT_NORMAL);
 	if (IS_ERR(sess->se_sess)) {
 		iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
 				ISCSI_LOGIN_STATUS_NO_RESOURCES);
@@ -1161,6 +1159,7 @@ void iscsi_target_login_sess_out(struct iscsi_conn *conn,
 	}
 	kfree(conn->sess->sess_ops);
 	kfree(conn->sess);
+	conn->sess = NULL;
 
 old_sess_out:
 	iscsi_stop_login_thread_timer(np);
@@ -1204,6 +1203,9 @@ old_sess_out:
 		conn->sock = NULL;
 	}
 
+	if (conn->conn_transport->iscsit_wait_conn)
+		conn->conn_transport->iscsit_wait_conn(conn);
+
 	if (conn->conn_transport->iscsit_free_conn)
 		conn->conn_transport->iscsit_free_conn(conn);
 
@@ -1364,6 +1366,9 @@ static int __iscsi_target_login_thread(struct iscsi_np *np)
 	}
 	login->zero_tsih = zero_tsih;
 
+	conn->sess->se_sess->sup_prot_ops =
+		conn->conn_transport->iscsit_get_sup_prot_ops(conn);
+
 	tpg = conn->tpg;
 	if (!tpg) {
 		pr_err("Unable to locate struct iscsi_conn->tpg\n");
diff --git a/drivers/target/iscsi/iscsi_target_tpg.c b/drivers/target/iscsi/iscsi_target_tpg.c
index c3cb5c15efda..9053a3c0c6e5 100644
--- a/drivers/target/iscsi/iscsi_target_tpg.c
+++ b/drivers/target/iscsi/iscsi_target_tpg.c
@@ -501,7 +501,6 @@ struct iscsi_tpg_np *iscsit_tpg_add_network_portal(
 	init_completion(&tpg_np->tpg_np_comp);
 	kref_init(&tpg_np->tpg_np_kref);
 	tpg_np->tpg_np		= np;
-	np->tpg_np		= tpg_np;
 	tpg_np->tpg		= tpg;
 
 	spin_lock(&tpg->tpg_np_lock);
diff --git a/drivers/target/iscsi/iscsi_target_transport.c b/drivers/target/iscsi/iscsi_target_transport.c
index 882728fac30c..08217d62fb0d 100644
--- a/drivers/target/iscsi/iscsi_target_transport.c
+++ b/drivers/target/iscsi/iscsi_target_transport.c
@@ -26,8 +26,7 @@ struct iscsit_transport *iscsit_get_transport(int type)
 
 void iscsit_put_transport(struct iscsit_transport *t)
 {
-	if (t->owner)
-		module_put(t->owner);
+	module_put(t->owner);
 }
 
 int iscsit_register_transport(struct iscsit_transport *t)
diff --git a/drivers/target/iscsi/iscsi_target_util.c b/drivers/target/iscsi/iscsi_target_util.c
index 7c6a95bcb35e..bcd88ec99793 100644
--- a/drivers/target/iscsi/iscsi_target_util.c
+++ b/drivers/target/iscsi/iscsi_target_util.c
@@ -1356,15 +1356,15 @@ static int iscsit_do_tx_data(
 	struct iscsi_conn *conn,
 	struct iscsi_data_count *count)
 {
-	int data = count->data_length, total_tx = 0, tx_loop = 0, iov_len;
+	int ret, iov_len;
 	struct kvec *iov_p;
 	struct msghdr msg;
 
 	if (!conn || !conn->sock || !conn->conn_ops)
 		return -1;
 
-	if (data <= 0) {
-		pr_err("Data length is: %d\n", data);
+	if (count->data_length <= 0) {
+		pr_err("Data length is: %d\n", count->data_length);
 		return -1;
 	}
 
@@ -1373,20 +1373,16 @@ static int iscsit_do_tx_data(
 	iov_p = count->iov;
 	iov_len = count->iov_count;
 
-	while (total_tx < data) {
-		tx_loop = kernel_sendmsg(conn->sock, &msg, iov_p, iov_len,
-					(data - total_tx));
-		if (tx_loop <= 0) {
-			pr_debug("tx_loop: %d total_tx %d\n",
-				tx_loop, total_tx);
-			return tx_loop;
-		}
-		total_tx += tx_loop;
-		pr_debug("tx_loop: %d, total_tx: %d, data: %d\n",
-					tx_loop, total_tx, data);
+	ret = kernel_sendmsg(conn->sock, &msg, iov_p, iov_len,
+			     count->data_length);
+	if (ret != count->data_length) {
+		pr_err("Unexpected ret: %d send data %d\n",
+		       ret, count->data_length);
+		return -EPIPE;
 	}
+	pr_debug("ret: %d, sent data: %d\n", ret, count->data_length);
 
-	return total_tx;
+	return ret;
 }
 
 int rx_data(
diff --git a/drivers/target/loopback/tcm_loop.c b/drivers/target/loopback/tcm_loop.c
index 4d1b7224a7f2..6b3c32954689 100644
--- a/drivers/target/loopback/tcm_loop.c
+++ b/drivers/target/loopback/tcm_loop.c
@@ -138,7 +138,7 @@ static void tcm_loop_submission_work(struct work_struct *work)
 		set_host_byte(sc, DID_TRANSPORT_DISRUPTED);
 		goto out_done;
 	}
-	tl_nexus = tl_hba->tl_nexus;
+	tl_nexus = tl_tpg->tl_nexus;
 	if (!tl_nexus) {
 		scmd_printk(KERN_ERR, sc, "TCM_Loop I_T Nexus"
 				" does not exist\n");
@@ -168,7 +168,7 @@ static void tcm_loop_submission_work(struct work_struct *work)
 
 	rc = target_submit_cmd_map_sgls(se_cmd, tl_nexus->se_sess, sc->cmnd,
 			&tl_cmd->tl_sense_buf[0], tl_cmd->sc->device->lun,
-			transfer_length, MSG_SIMPLE_TAG,
+			transfer_length, TCM_SIMPLE_TAG,
 			sc->sc_data_direction, 0,
 			scsi_sglist(sc), scsi_sg_count(sc),
 			sgl_bidi, sgl_bidi_count,
@@ -218,16 +218,26 @@ static int tcm_loop_queuecommand(struct Scsi_Host *sh, struct scsi_cmnd *sc)
  * to struct scsi_device
  */
 static int tcm_loop_issue_tmr(struct tcm_loop_tpg *tl_tpg,
-			      struct tcm_loop_nexus *tl_nexus,
 			      int lun, int task, enum tcm_tmreq_table tmr)
 {
 	struct se_cmd *se_cmd = NULL;
 	struct se_session *se_sess;
 	struct se_portal_group *se_tpg;
+	struct tcm_loop_nexus *tl_nexus;
 	struct tcm_loop_cmd *tl_cmd = NULL;
 	struct tcm_loop_tmr *tl_tmr = NULL;
 	int ret = TMR_FUNCTION_FAILED, rc;
 
+	/*
+	 * Locate the tl_nexus and se_sess pointers
+	 */
+	tl_nexus = tl_tpg->tl_nexus;
+	if (!tl_nexus) {
+		pr_err("Unable to perform device reset without"
+				" active I_T Nexus\n");
+		return ret;
+	}
+
 	tl_cmd = kmem_cache_zalloc(tcm_loop_cmd_cache, GFP_KERNEL);
 	if (!tl_cmd) {
 		pr_err("Unable to allocate memory for tl_cmd\n");
@@ -243,12 +253,12 @@ static int tcm_loop_issue_tmr(struct tcm_loop_tpg *tl_tpg,
 
 	se_cmd = &tl_cmd->tl_se_cmd;
 	se_tpg = &tl_tpg->tl_se_tpg;
-	se_sess = tl_nexus->se_sess;
+	se_sess = tl_tpg->tl_nexus->se_sess;
 	/*
 	 * Initialize struct se_cmd descriptor from target_core_mod infrastructure
 	 */
 	transport_init_se_cmd(se_cmd, se_tpg->se_tpg_tfo, se_sess, 0,
-				DMA_NONE, MSG_SIMPLE_TAG,
+				DMA_NONE, TCM_SIMPLE_TAG,
 				&tl_cmd->tl_sense_buf[0]);
 
 	rc = core_tmr_alloc_req(se_cmd, tl_tmr, tmr, GFP_KERNEL);
@@ -288,7 +298,6 @@ release:
 static int tcm_loop_abort_task(struct scsi_cmnd *sc)
 {
 	struct tcm_loop_hba *tl_hba;
-	struct tcm_loop_nexus *tl_nexus;
 	struct tcm_loop_tpg *tl_tpg;
 	int ret = FAILED;
 
@@ -296,21 +305,8 @@ static int tcm_loop_abort_task(struct scsi_cmnd *sc)
 	 * Locate the tcm_loop_hba_t pointer
 	 */
 	tl_hba = *(struct tcm_loop_hba **)shost_priv(sc->device->host);
-	/*
-	 * Locate the tl_nexus and se_sess pointers
-	 */
-	tl_nexus = tl_hba->tl_nexus;
-	if (!tl_nexus) {
-		pr_err("Unable to perform device reset without"
-				" active I_T Nexus\n");
-		return FAILED;
-	}
-
-	/*
-	 * Locate the tl_tpg pointer from TargetID in sc->device->id
-	 */
 	tl_tpg = &tl_hba->tl_hba_tpgs[sc->device->id];
-	ret = tcm_loop_issue_tmr(tl_tpg, tl_nexus, sc->device->lun,
+	ret = tcm_loop_issue_tmr(tl_tpg, sc->device->lun,
 				 sc->request->tag, TMR_ABORT_TASK);
 	return (ret == TMR_FUNCTION_COMPLETE) ? SUCCESS : FAILED;
 }
@@ -322,7 +318,6 @@ static int tcm_loop_abort_task(struct scsi_cmnd *sc)
 static int tcm_loop_device_reset(struct scsi_cmnd *sc)
 {
 	struct tcm_loop_hba *tl_hba;
-	struct tcm_loop_nexus *tl_nexus;
 	struct tcm_loop_tpg *tl_tpg;
 	int ret = FAILED;
 
@@ -330,20 +325,9 @@ static int tcm_loop_device_reset(struct scsi_cmnd *sc)
 	 * Locate the tcm_loop_hba_t pointer
 	 */
 	tl_hba = *(struct tcm_loop_hba **)shost_priv(sc->device->host);
-	/*
-	 * Locate the tl_nexus and se_sess pointers
-	 */
-	tl_nexus = tl_hba->tl_nexus;
-	if (!tl_nexus) {
-		pr_err("Unable to perform device reset without"
-				" active I_T Nexus\n");
-		return FAILED;
-	}
-	/*
-	 * Locate the tl_tpg pointer from TargetID in sc->device->id
-	 */
 	tl_tpg = &tl_hba->tl_hba_tpgs[sc->device->id];
-	ret = tcm_loop_issue_tmr(tl_tpg, tl_nexus, sc->device->lun,
+
+	ret = tcm_loop_issue_tmr(tl_tpg, sc->device->lun,
 				 0, TMR_LUN_RESET);
 	return (ret == TMR_FUNCTION_COMPLETE) ? SUCCESS : FAILED;
 }
@@ -385,7 +369,6 @@ static struct scsi_host_template tcm_loop_driver_template = {
 	.name			= "TCM_Loopback",
 	.queuecommand		= tcm_loop_queuecommand,
 	.change_queue_depth	= scsi_change_queue_depth,
-	.change_queue_type	= scsi_change_queue_type,
 	.eh_abort_handler = tcm_loop_abort_task,
 	.eh_device_reset_handler = tcm_loop_device_reset,
 	.eh_target_reset_handler = tcm_loop_target_reset,
@@ -940,8 +923,8 @@ static int tcm_loop_make_nexus(
 	struct tcm_loop_nexus *tl_nexus;
 	int ret = -ENOMEM;
 
-	if (tl_tpg->tl_hba->tl_nexus) {
-		pr_debug("tl_tpg->tl_hba->tl_nexus already exists\n");
+	if (tl_tpg->tl_nexus) {
+		pr_debug("tl_tpg->tl_nexus already exists\n");
 		return -EEXIST;
 	}
 	se_tpg = &tl_tpg->tl_se_tpg;
@@ -976,7 +959,7 @@ static int tcm_loop_make_nexus(
 	 */
 	__transport_register_session(se_tpg, tl_nexus->se_sess->se_node_acl,
 			tl_nexus->se_sess, tl_nexus);
-	tl_tpg->tl_hba->tl_nexus = tl_nexus;
+	tl_tpg->tl_nexus = tl_nexus;
 	pr_debug("TCM_Loop_ConfigFS: Established I_T Nexus to emulated"
 		" %s Initiator Port: %s\n", tcm_loop_dump_proto_id(tl_hba),
 		name);
@@ -992,12 +975,8 @@ static int tcm_loop_drop_nexus(
 {
 	struct se_session *se_sess;
 	struct tcm_loop_nexus *tl_nexus;
-	struct tcm_loop_hba *tl_hba = tpg->tl_hba;
 
-	if (!tl_hba)
-		return -ENODEV;
-
-	tl_nexus = tl_hba->tl_nexus;
+	tl_nexus = tpg->tl_nexus;
 	if (!tl_nexus)
 		return -ENODEV;
 
@@ -1013,13 +992,13 @@ static int tcm_loop_drop_nexus(
 	}
 
 	pr_debug("TCM_Loop_ConfigFS: Removing I_T Nexus to emulated"
-		" %s Initiator Port: %s\n", tcm_loop_dump_proto_id(tl_hba),
+		" %s Initiator Port: %s\n", tcm_loop_dump_proto_id(tpg->tl_hba),
 		tl_nexus->se_sess->se_node_acl->initiatorname);
 	/*
 	 * Release the SCSI I_T Nexus to the emulated SAS Target Port
 	 */
 	transport_deregister_session(tl_nexus->se_sess);
-	tpg->tl_hba->tl_nexus = NULL;
+	tpg->tl_nexus = NULL;
 	kfree(tl_nexus);
 	return 0;
 }
@@ -1035,7 +1014,7 @@ static ssize_t tcm_loop_tpg_show_nexus(
 	struct tcm_loop_nexus *tl_nexus;
 	ssize_t ret;
 
-	tl_nexus = tl_tpg->tl_hba->tl_nexus;
+	tl_nexus = tl_tpg->tl_nexus;
 	if (!tl_nexus)
 		return -ENODEV;
 
diff --git a/drivers/target/loopback/tcm_loop.h b/drivers/target/loopback/tcm_loop.h
index 54c59d0b6608..6ae49f272ba6 100644
--- a/drivers/target/loopback/tcm_loop.h
+++ b/drivers/target/loopback/tcm_loop.h
@@ -27,11 +27,6 @@ struct tcm_loop_tmr {
 };
 
 struct tcm_loop_nexus {
-	int it_nexus_active;
-	/*
-	 * Pointer to Linux/SCSI HBA from linux/include/scsi_host.h
-	 */
-	struct scsi_host *sh;
 	/*
 	 * Pointer to TCM session for I_T Nexus
 	 */
@@ -51,6 +46,7 @@ struct tcm_loop_tpg {
 	atomic_t tl_tpg_port_count;
 	struct se_portal_group tl_se_tpg;
 	struct tcm_loop_hba *tl_hba;
+	struct tcm_loop_nexus *tl_nexus;
 };
 
 struct tcm_loop_hba {
@@ -59,7 +55,6 @@ struct tcm_loop_hba {
 	struct se_hba_s *se_hba;
 	struct se_lun *tl_hba_lun;
 	struct se_port *tl_hba_lun_sep;
-	struct tcm_loop_nexus *tl_nexus;
 	struct device dev;
 	struct Scsi_Host *sh;
 	struct tcm_loop_tpg tl_hba_tpgs[TL_TPGS_PER_HBA];
diff --git a/drivers/target/sbp/sbp_target.c b/drivers/target/sbp/sbp_target.c
index e7e93727553c..9512af6a8114 100644
--- a/drivers/target/sbp/sbp_target.c
+++ b/drivers/target/sbp/sbp_target.c
@@ -1237,7 +1237,7 @@ static void sbp_handle_command(struct sbp_target_request *req)
 
 	if (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))
+			      TCM_SIMPLE_TAG, data_dir, 0))
 		goto err;
 
 	return;
diff --git a/drivers/target/target_core_configfs.c b/drivers/target/target_core_configfs.c
index 79f9296a08ae..75d89adfccc0 100644
--- a/drivers/target/target_core_configfs.c
+++ b/drivers/target/target_core_configfs.c
@@ -50,6 +50,19 @@
 #include "target_core_rd.h"
 #include "target_core_xcopy.h"
 
+#define TB_CIT_SETUP(_name, _item_ops, _group_ops, _attrs)		\
+static void target_core_setup_##_name##_cit(struct se_subsystem_api *sa) \
+{									\
+	struct target_backend_cits *tbc = &sa->tb_cits;			\
+	struct config_item_type *cit = &tbc->tb_##_name##_cit;		\
+									\
+	cit->ct_item_ops = _item_ops;					\
+	cit->ct_group_ops = _group_ops;					\
+	cit->ct_attrs = _attrs;						\
+	cit->ct_owner = sa->owner;					\
+	pr_debug("Setup generic %s\n", __stringify(_name));		\
+}
+
 extern struct t10_alua_lu_gp *default_lu_gp;
 
 static LIST_HEAD(g_tf_list);
@@ -126,48 +139,57 @@ static struct config_group *target_core_register_fabric(
 
 	pr_debug("Target_Core_ConfigFS: REGISTER -> group: %p name:"
 			" %s\n", group, name);
-	/*
-	 * Below are some hardcoded request_module() calls to automatically
-	 * local fabric modules when the following is called:
-	 *
-	 * mkdir -p /sys/kernel/config/target/$MODULE_NAME
-	 *
-	 * Note that this does not limit which TCM fabric module can be
-	 * registered, but simply provids auto loading logic for modules with
-	 * mkdir(2) system calls with known TCM fabric modules.
-	 */
-	if (!strncmp(name, "iscsi", 5)) {
+
+	tf = target_core_get_fabric(name);
+	if (!tf) {
+		pr_err("target_core_register_fabric() trying autoload for %s\n",
+			name);
+
 		/*
-		 * Automatically load the LIO Target fabric module when the
-		 * following is called:
+		 * Below are some hardcoded request_module() calls to automatically
+		 * local fabric modules when the following is called:
 		 *
-		 * mkdir -p $CONFIGFS/target/iscsi
-		 */
-		ret = request_module("iscsi_target_mod");
-		if (ret < 0) {
-			pr_err("request_module() failed for"
-				" iscsi_target_mod.ko: %d\n", ret);
-			return ERR_PTR(-EINVAL);
-		}
-	} else if (!strncmp(name, "loopback", 8)) {
-		/*
-		 * Automatically load the tcm_loop fabric module when the
-		 * following is called:
+		 * mkdir -p /sys/kernel/config/target/$MODULE_NAME
 		 *
-		 * mkdir -p $CONFIGFS/target/loopback
+		 * Note that this does not limit which TCM fabric module can be
+		 * registered, but simply provids auto loading logic for modules with
+		 * mkdir(2) system calls with known TCM fabric modules.
 		 */
-		ret = request_module("tcm_loop");
-		if (ret < 0) {
-			pr_err("request_module() failed for"
-				" tcm_loop.ko: %d\n", ret);
-			return ERR_PTR(-EINVAL);
+
+		if (!strncmp(name, "iscsi", 5)) {
+			/*
+			 * Automatically load the LIO Target fabric module when the
+			 * following is called:
+			 *
+			 * mkdir -p $CONFIGFS/target/iscsi
+			 */
+			ret = request_module("iscsi_target_mod");
+			if (ret < 0) {
+				pr_err("request_module() failed for"
+				       " iscsi_target_mod.ko: %d\n", ret);
+				return ERR_PTR(-EINVAL);
+			}
+		} else if (!strncmp(name, "loopback", 8)) {
+			/*
+			 * Automatically load the tcm_loop fabric module when the
+			 * following is called:
+			 *
+			 * mkdir -p $CONFIGFS/target/loopback
+			 */
+			ret = request_module("tcm_loop");
+			if (ret < 0) {
+				pr_err("request_module() failed for"
+				       " tcm_loop.ko: %d\n", ret);
+				return ERR_PTR(-EINVAL);
+			}
 		}
+
+		tf = target_core_get_fabric(name);
 	}
 
-	tf = target_core_get_fabric(name);
 	if (!tf) {
 		pr_err("target_core_get_fabric() failed for %s\n",
-			name);
+		       name);
 		return ERR_PTR(-EINVAL);
 	}
 	pr_debug("Target_Core_ConfigFS: REGISTER -> Located fabric:"
@@ -562,198 +584,21 @@ EXPORT_SYMBOL(target_fabric_configfs_deregister);
 // Stop functions called by external Target Fabrics Modules
 //############################################################################*/
 
-/* Start functions for struct config_item_type target_core_dev_attrib_cit */
-
-#define DEF_DEV_ATTRIB_SHOW(_name)					\
-static ssize_t target_core_dev_show_attr_##_name(			\
-	struct se_dev_attrib *da,					\
-	char *page)							\
-{									\
-	return snprintf(page, PAGE_SIZE, "%u\n",			\
-		(u32)da->da_dev->dev_attrib._name);			\
-}
-
-#define DEF_DEV_ATTRIB_STORE(_name)					\
-static ssize_t target_core_dev_store_attr_##_name(			\
-	struct se_dev_attrib *da,					\
-	const char *page,						\
-	size_t count)							\
-{									\
-	unsigned long val;						\
-	int ret;							\
-									\
-	ret = kstrtoul(page, 0, &val);				\
-	if (ret < 0) {							\
-		pr_err("kstrtoul() failed with"		\
-			" ret: %d\n", ret);				\
-		return -EINVAL;						\
-	}								\
-	ret = se_dev_set_##_name(da->da_dev, (u32)val);			\
-									\
-	return (!ret) ? count : -EINVAL;				\
-}
-
-#define DEF_DEV_ATTRIB(_name)						\
-DEF_DEV_ATTRIB_SHOW(_name);						\
-DEF_DEV_ATTRIB_STORE(_name);
-
-#define DEF_DEV_ATTRIB_RO(_name)					\
-DEF_DEV_ATTRIB_SHOW(_name);
+/* Start functions for struct config_item_type tb_dev_attrib_cit */
 
 CONFIGFS_EATTR_STRUCT(target_core_dev_attrib, se_dev_attrib);
-#define SE_DEV_ATTR(_name, _mode)					\
-static struct target_core_dev_attrib_attribute				\
-			target_core_dev_attrib_##_name =		\
-		__CONFIGFS_EATTR(_name, _mode,				\
-		target_core_dev_show_attr_##_name,			\
-		target_core_dev_store_attr_##_name);
-
-#define SE_DEV_ATTR_RO(_name);						\
-static struct target_core_dev_attrib_attribute				\
-			target_core_dev_attrib_##_name =		\
-	__CONFIGFS_EATTR_RO(_name,					\
-	target_core_dev_show_attr_##_name);
-
-DEF_DEV_ATTRIB(emulate_model_alias);
-SE_DEV_ATTR(emulate_model_alias, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(emulate_dpo);
-SE_DEV_ATTR(emulate_dpo, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(emulate_fua_write);
-SE_DEV_ATTR(emulate_fua_write, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(emulate_fua_read);
-SE_DEV_ATTR(emulate_fua_read, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(emulate_write_cache);
-SE_DEV_ATTR(emulate_write_cache, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(emulate_ua_intlck_ctrl);
-SE_DEV_ATTR(emulate_ua_intlck_ctrl, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(emulate_tas);
-SE_DEV_ATTR(emulate_tas, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(emulate_tpu);
-SE_DEV_ATTR(emulate_tpu, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(emulate_tpws);
-SE_DEV_ATTR(emulate_tpws, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(emulate_caw);
-SE_DEV_ATTR(emulate_caw, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(emulate_3pc);
-SE_DEV_ATTR(emulate_3pc, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(pi_prot_type);
-SE_DEV_ATTR(pi_prot_type, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB_RO(hw_pi_prot_type);
-SE_DEV_ATTR_RO(hw_pi_prot_type);
-
-DEF_DEV_ATTRIB(pi_prot_format);
-SE_DEV_ATTR(pi_prot_format, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(enforce_pr_isids);
-SE_DEV_ATTR(enforce_pr_isids, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(is_nonrot);
-SE_DEV_ATTR(is_nonrot, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(emulate_rest_reord);
-SE_DEV_ATTR(emulate_rest_reord, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(force_pr_aptpl);
-SE_DEV_ATTR(force_pr_aptpl, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB_RO(hw_block_size);
-SE_DEV_ATTR_RO(hw_block_size);
-
-DEF_DEV_ATTRIB(block_size);
-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(fabric_max_sectors);
-SE_DEV_ATTR(fabric_max_sectors, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(optimal_sectors);
-SE_DEV_ATTR(optimal_sectors, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB_RO(hw_queue_depth);
-SE_DEV_ATTR_RO(hw_queue_depth);
-
-DEF_DEV_ATTRIB(queue_depth);
-SE_DEV_ATTR(queue_depth, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(max_unmap_lba_count);
-SE_DEV_ATTR(max_unmap_lba_count, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(max_unmap_block_desc_count);
-SE_DEV_ATTR(max_unmap_block_desc_count, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(unmap_granularity);
-SE_DEV_ATTR(unmap_granularity, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(unmap_granularity_alignment);
-SE_DEV_ATTR(unmap_granularity_alignment, S_IRUGO | S_IWUSR);
-
-DEF_DEV_ATTRIB(max_write_same_len);
-SE_DEV_ATTR(max_write_same_len, S_IRUGO | S_IWUSR);
-
 CONFIGFS_EATTR_OPS(target_core_dev_attrib, se_dev_attrib, da_group);
 
-static struct configfs_attribute *target_core_dev_attrib_attrs[] = {
-	&target_core_dev_attrib_emulate_model_alias.attr,
-	&target_core_dev_attrib_emulate_dpo.attr,
-	&target_core_dev_attrib_emulate_fua_write.attr,
-	&target_core_dev_attrib_emulate_fua_read.attr,
-	&target_core_dev_attrib_emulate_write_cache.attr,
-	&target_core_dev_attrib_emulate_ua_intlck_ctrl.attr,
-	&target_core_dev_attrib_emulate_tas.attr,
-	&target_core_dev_attrib_emulate_tpu.attr,
-	&target_core_dev_attrib_emulate_tpws.attr,
-	&target_core_dev_attrib_emulate_caw.attr,
-	&target_core_dev_attrib_emulate_3pc.attr,
-	&target_core_dev_attrib_pi_prot_type.attr,
-	&target_core_dev_attrib_hw_pi_prot_type.attr,
-	&target_core_dev_attrib_pi_prot_format.attr,
-	&target_core_dev_attrib_enforce_pr_isids.attr,
-	&target_core_dev_attrib_force_pr_aptpl.attr,
-	&target_core_dev_attrib_is_nonrot.attr,
-	&target_core_dev_attrib_emulate_rest_reord.attr,
-	&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_fabric_max_sectors.attr,
-	&target_core_dev_attrib_optimal_sectors.attr,
-	&target_core_dev_attrib_hw_queue_depth.attr,
-	&target_core_dev_attrib_queue_depth.attr,
-	&target_core_dev_attrib_max_unmap_lba_count.attr,
-	&target_core_dev_attrib_max_unmap_block_desc_count.attr,
-	&target_core_dev_attrib_unmap_granularity.attr,
-	&target_core_dev_attrib_unmap_granularity_alignment.attr,
-	&target_core_dev_attrib_max_write_same_len.attr,
-	NULL,
-};
-
 static struct configfs_item_operations target_core_dev_attrib_ops = {
 	.show_attribute		= target_core_dev_attrib_attr_show,
 	.store_attribute	= target_core_dev_attrib_attr_store,
 };
 
-static struct config_item_type target_core_dev_attrib_cit = {
-	.ct_item_ops		= &target_core_dev_attrib_ops,
-	.ct_attrs		= target_core_dev_attrib_attrs,
-	.ct_owner		= THIS_MODULE,
-};
+TB_CIT_SETUP(dev_attrib, &target_core_dev_attrib_ops, NULL, NULL);
 
-/* End functions for struct config_item_type target_core_dev_attrib_cit */
+/* End functions for struct config_item_type tb_dev_attrib_cit */
 
-/*  Start functions for struct config_item_type target_core_dev_wwn_cit */
+/*  Start functions for struct config_item_type tb_dev_wwn_cit */
 
 CONFIGFS_EATTR_STRUCT(target_core_dev_wwn, t10_wwn);
 #define SE_DEV_WWN_ATTR(_name, _mode)					\
@@ -984,15 +829,11 @@ static struct configfs_item_operations target_core_dev_wwn_ops = {
 	.store_attribute	= target_core_dev_wwn_attr_store,
 };
 
-static struct config_item_type target_core_dev_wwn_cit = {
-	.ct_item_ops		= &target_core_dev_wwn_ops,
-	.ct_attrs		= target_core_dev_wwn_attrs,
-	.ct_owner		= THIS_MODULE,
-};
+TB_CIT_SETUP(dev_wwn, &target_core_dev_wwn_ops, NULL, target_core_dev_wwn_attrs);
 
-/*  End functions for struct config_item_type target_core_dev_wwn_cit */
+/*  End functions for struct config_item_type tb_dev_wwn_cit */
 
-/*  Start functions for struct config_item_type target_core_dev_pr_cit */
+/*  Start functions for struct config_item_type tb_dev_pr_cit */
 
 CONFIGFS_EATTR_STRUCT(target_core_dev_pr, se_device);
 #define SE_DEV_PR_ATTR(_name, _mode)					\
@@ -1453,15 +1294,11 @@ static struct configfs_item_operations target_core_dev_pr_ops = {
 	.store_attribute	= target_core_dev_pr_attr_store,
 };
 
-static struct config_item_type target_core_dev_pr_cit = {
-	.ct_item_ops		= &target_core_dev_pr_ops,
-	.ct_attrs		= target_core_dev_pr_attrs,
-	.ct_owner		= THIS_MODULE,
-};
+TB_CIT_SETUP(dev_pr, &target_core_dev_pr_ops, NULL, target_core_dev_pr_attrs);
 
-/*  End functions for struct config_item_type target_core_dev_pr_cit */
+/*  End functions for struct config_item_type tb_dev_pr_cit */
 
-/*  Start functions for struct config_item_type target_core_dev_cit */
+/*  Start functions for struct config_item_type tb_dev_cit */
 
 static ssize_t target_core_show_dev_info(void *p, char *page)
 {
@@ -1925,7 +1762,7 @@ static struct target_core_configfs_attribute target_core_attr_dev_lba_map = {
 	.store	= target_core_store_dev_lba_map,
 };
 
-static struct configfs_attribute *lio_core_dev_attrs[] = {
+static struct configfs_attribute *target_core_dev_attrs[] = {
 	&target_core_attr_dev_info.attr,
 	&target_core_attr_dev_control.attr,
 	&target_core_attr_dev_alias.attr,
@@ -1984,13 +1821,9 @@ static struct configfs_item_operations target_core_dev_item_ops = {
 	.store_attribute	= target_core_dev_store,
 };
 
-static struct config_item_type target_core_dev_cit = {
-	.ct_item_ops		= &target_core_dev_item_ops,
-	.ct_attrs		= lio_core_dev_attrs,
-	.ct_owner		= THIS_MODULE,
-};
+TB_CIT_SETUP(dev, &target_core_dev_item_ops, NULL, target_core_dev_attrs);
 
-/* End functions for struct config_item_type target_core_dev_cit */
+/* End functions for struct config_item_type tb_dev_cit */
 
 /* Start functions for struct config_item_type target_core_alua_lu_gp_cit */
 
@@ -2670,7 +2503,7 @@ static struct config_item_type target_core_alua_tg_pt_gp_cit = {
 
 /* End functions for struct config_item_type target_core_alua_tg_pt_gp_cit */
 
-/* Start functions for struct config_item_type target_core_alua_tg_pt_gps_cit */
+/* Start functions for struct config_item_type tb_alua_tg_pt_gps_cit */
 
 static struct config_group *target_core_alua_create_tg_pt_gp(
 	struct config_group *group,
@@ -2721,12 +2554,9 @@ static struct configfs_group_operations target_core_alua_tg_pt_gps_group_ops = {
 	.drop_item		= &target_core_alua_drop_tg_pt_gp,
 };
 
-static struct config_item_type target_core_alua_tg_pt_gps_cit = {
-	.ct_group_ops		= &target_core_alua_tg_pt_gps_group_ops,
-	.ct_owner		= THIS_MODULE,
-};
+TB_CIT_SETUP(dev_alua_tg_pt_gps, NULL, &target_core_alua_tg_pt_gps_group_ops, NULL);
 
-/* End functions for struct config_item_type target_core_alua_tg_pt_gps_cit */
+/* End functions for struct config_item_type tb_alua_tg_pt_gps_cit */
 
 /* Start functions for struct config_item_type target_core_alua_cit */
 
@@ -2744,7 +2574,7 @@ static struct config_item_type target_core_alua_cit = {
 
 /* End functions for struct config_item_type target_core_alua_cit */
 
-/* Start functions for struct config_item_type target_core_stat_cit */
+/* Start functions for struct config_item_type tb_dev_stat_cit */
 
 static struct config_group *target_core_stat_mkdir(
 	struct config_group *group,
@@ -2765,12 +2595,9 @@ static struct configfs_group_operations target_core_stat_group_ops = {
 	.drop_item		= &target_core_stat_rmdir,
 };
 
-static struct config_item_type target_core_stat_cit = {
-	.ct_group_ops		= &target_core_stat_group_ops,
-	.ct_owner		= THIS_MODULE,
-};
+TB_CIT_SETUP(dev_stat, NULL, &target_core_stat_group_ops, NULL);
 
-/* End functions for struct config_item_type target_core_stat_cit */
+/* End functions for struct config_item_type tb_dev_stat_cit */
 
 /* Start functions for struct config_item_type target_core_hba_cit */
 
@@ -2806,17 +2633,17 @@ static struct config_group *target_core_make_subdev(
 	if (!dev_cg->default_groups)
 		goto out_free_device;
 
-	config_group_init_type_name(dev_cg, name, &target_core_dev_cit);
+	config_group_init_type_name(dev_cg, name, &t->tb_cits.tb_dev_cit);
 	config_group_init_type_name(&dev->dev_attrib.da_group, "attrib",
-			&target_core_dev_attrib_cit);
+			&t->tb_cits.tb_dev_attrib_cit);
 	config_group_init_type_name(&dev->dev_pr_group, "pr",
-			&target_core_dev_pr_cit);
+			&t->tb_cits.tb_dev_pr_cit);
 	config_group_init_type_name(&dev->t10_wwn.t10_wwn_group, "wwn",
-			&target_core_dev_wwn_cit);
+			&t->tb_cits.tb_dev_wwn_cit);
 	config_group_init_type_name(&dev->t10_alua.alua_tg_pt_gps_group,
-			"alua", &target_core_alua_tg_pt_gps_cit);
+			"alua", &t->tb_cits.tb_dev_alua_tg_pt_gps_cit);
 	config_group_init_type_name(&dev->dev_stat_grps.stat_group,
-			"statistics", &target_core_stat_cit);
+			"statistics", &t->tb_cits.tb_dev_stat_cit);
 
 	dev_cg->default_groups[0] = &dev->dev_attrib.da_group;
 	dev_cg->default_groups[1] = &dev->dev_pr_group;
@@ -3110,6 +2937,17 @@ static struct config_item_type target_core_cit = {
 
 /* Stop functions for struct config_item_type target_core_hba_cit */
 
+void target_core_setup_sub_cits(struct se_subsystem_api *sa)
+{
+	target_core_setup_dev_cit(sa);
+	target_core_setup_dev_attrib_cit(sa);
+	target_core_setup_dev_pr_cit(sa);
+	target_core_setup_dev_wwn_cit(sa);
+	target_core_setup_dev_alua_tg_pt_gps_cit(sa);
+	target_core_setup_dev_stat_cit(sa);
+}
+EXPORT_SYMBOL(target_core_setup_sub_cits);
+
 static int __init target_core_init_configfs(void)
 {
 	struct config_group *target_cg, *hba_cg = NULL, *alua_cg = NULL;
diff --git a/drivers/target/target_core_device.c b/drivers/target/target_core_device.c
index c45f9e907e44..7653cfb027a2 100644
--- a/drivers/target/target_core_device.c
+++ b/drivers/target/target_core_device.c
@@ -659,6 +659,7 @@ int se_dev_set_max_unmap_lba_count(
 			dev, dev->dev_attrib.max_unmap_lba_count);
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_max_unmap_lba_count);
 
 int se_dev_set_max_unmap_block_desc_count(
 	struct se_device *dev,
@@ -670,6 +671,7 @@ int se_dev_set_max_unmap_block_desc_count(
 			dev, dev->dev_attrib.max_unmap_block_desc_count);
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_max_unmap_block_desc_count);
 
 int se_dev_set_unmap_granularity(
 	struct se_device *dev,
@@ -680,6 +682,7 @@ int se_dev_set_unmap_granularity(
 			dev, dev->dev_attrib.unmap_granularity);
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_unmap_granularity);
 
 int se_dev_set_unmap_granularity_alignment(
 	struct se_device *dev,
@@ -690,6 +693,7 @@ int se_dev_set_unmap_granularity_alignment(
 			dev, dev->dev_attrib.unmap_granularity_alignment);
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_unmap_granularity_alignment);
 
 int se_dev_set_max_write_same_len(
 	struct se_device *dev,
@@ -700,6 +704,7 @@ int se_dev_set_max_write_same_len(
 			dev, dev->dev_attrib.max_write_same_len);
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_max_write_same_len);
 
 static void dev_set_t10_wwn_model_alias(struct se_device *dev)
 {
@@ -738,6 +743,7 @@ int se_dev_set_emulate_model_alias(struct se_device *dev, int flag)
 
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_emulate_model_alias);
 
 int se_dev_set_emulate_dpo(struct se_device *dev, int flag)
 {
@@ -753,6 +759,7 @@ int se_dev_set_emulate_dpo(struct se_device *dev, int flag)
 
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_emulate_dpo);
 
 int se_dev_set_emulate_fua_write(struct se_device *dev, int flag)
 {
@@ -760,17 +767,12 @@ int se_dev_set_emulate_fua_write(struct se_device *dev, int flag)
 		pr_err("Illegal value %d\n", flag);
 		return -EINVAL;
 	}
-
-	if (flag &&
-	    dev->transport->transport_type == TRANSPORT_PLUGIN_PHBA_PDEV) {
-		pr_err("emulate_fua_write not supported for pSCSI\n");
-		return -EINVAL;
-	}
 	dev->dev_attrib.emulate_fua_write = flag;
 	pr_debug("dev[%p]: SE Device Forced Unit Access WRITEs: %d\n",
 			dev, dev->dev_attrib.emulate_fua_write);
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_emulate_fua_write);
 
 int se_dev_set_emulate_fua_read(struct se_device *dev, int flag)
 {
@@ -786,6 +788,7 @@ int se_dev_set_emulate_fua_read(struct se_device *dev, int flag)
 
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_emulate_fua_read);
 
 int se_dev_set_emulate_write_cache(struct se_device *dev, int flag)
 {
@@ -794,11 +797,6 @@ int se_dev_set_emulate_write_cache(struct se_device *dev, int flag)
 		return -EINVAL;
 	}
 	if (flag &&
-	    dev->transport->transport_type == TRANSPORT_PLUGIN_PHBA_PDEV) {
-		pr_err("emulate_write_cache not supported for pSCSI\n");
-		return -EINVAL;
-	}
-	if (flag &&
 	    dev->transport->get_write_cache) {
 		pr_err("emulate_write_cache not supported for this device\n");
 		return -EINVAL;
@@ -809,6 +807,7 @@ int se_dev_set_emulate_write_cache(struct se_device *dev, int flag)
 			dev, dev->dev_attrib.emulate_write_cache);
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_emulate_write_cache);
 
 int se_dev_set_emulate_ua_intlck_ctrl(struct se_device *dev, int flag)
 {
@@ -829,6 +828,7 @@ int se_dev_set_emulate_ua_intlck_ctrl(struct se_device *dev, int flag)
 
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_emulate_ua_intlck_ctrl);
 
 int se_dev_set_emulate_tas(struct se_device *dev, int flag)
 {
@@ -849,6 +849,7 @@ int se_dev_set_emulate_tas(struct se_device *dev, int flag)
 
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_emulate_tas);
 
 int se_dev_set_emulate_tpu(struct se_device *dev, int flag)
 {
@@ -870,6 +871,7 @@ int se_dev_set_emulate_tpu(struct se_device *dev, int flag)
 				dev, flag);
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_emulate_tpu);
 
 int se_dev_set_emulate_tpws(struct se_device *dev, int flag)
 {
@@ -891,6 +893,7 @@ int se_dev_set_emulate_tpws(struct se_device *dev, int flag)
 				dev, flag);
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_emulate_tpws);
 
 int se_dev_set_emulate_caw(struct se_device *dev, int flag)
 {
@@ -904,6 +907,7 @@ int se_dev_set_emulate_caw(struct se_device *dev, int flag)
 
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_emulate_caw);
 
 int se_dev_set_emulate_3pc(struct se_device *dev, int flag)
 {
@@ -917,6 +921,7 @@ int se_dev_set_emulate_3pc(struct se_device *dev, int flag)
 
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_emulate_3pc);
 
 int se_dev_set_pi_prot_type(struct se_device *dev, int flag)
 {
@@ -970,6 +975,7 @@ int se_dev_set_pi_prot_type(struct se_device *dev, int flag)
 
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_pi_prot_type);
 
 int se_dev_set_pi_prot_format(struct se_device *dev, int flag)
 {
@@ -1005,6 +1011,7 @@ int se_dev_set_pi_prot_format(struct se_device *dev, int flag)
 
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_pi_prot_format);
 
 int se_dev_set_enforce_pr_isids(struct se_device *dev, int flag)
 {
@@ -1017,6 +1024,7 @@ int se_dev_set_enforce_pr_isids(struct se_device *dev, int flag)
 		(dev->dev_attrib.enforce_pr_isids) ? "Enabled" : "Disabled");
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_enforce_pr_isids);
 
 int se_dev_set_force_pr_aptpl(struct se_device *dev, int flag)
 {
@@ -1034,6 +1042,7 @@ int se_dev_set_force_pr_aptpl(struct se_device *dev, int flag)
 	pr_debug("dev[%p]: SE Device force_pr_aptpl: %d\n", dev, flag);
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_force_pr_aptpl);
 
 int se_dev_set_is_nonrot(struct se_device *dev, int flag)
 {
@@ -1046,6 +1055,7 @@ int se_dev_set_is_nonrot(struct se_device *dev, int flag)
 	       dev, flag);
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_is_nonrot);
 
 int se_dev_set_emulate_rest_reord(struct se_device *dev, int flag)
 {
@@ -1058,6 +1068,7 @@ int se_dev_set_emulate_rest_reord(struct se_device *dev, int flag)
 	pr_debug("dev[%p]: SE Device emulate_rest_reord: %d\n", dev, flag);
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_emulate_rest_reord);
 
 /*
  * Note, this can only be called on unexported SE Device Object.
@@ -1076,31 +1087,21 @@ int se_dev_set_queue_depth(struct se_device *dev, u32 queue_depth)
 		return -EINVAL;
 	}
 
-	if (dev->transport->transport_type == TRANSPORT_PLUGIN_PHBA_PDEV) {
+	if (queue_depth > dev->dev_attrib.queue_depth) {
 		if (queue_depth > dev->dev_attrib.hw_queue_depth) {
-			pr_err("dev[%p]: Passed queue_depth: %u"
-				" exceeds TCM/SE_Device TCQ: %u\n",
-				dev, queue_depth,
+			pr_err("dev[%p]: Passed queue_depth:"
+				" %u exceeds TCM/SE_Device MAX"
+				" TCQ: %u\n", dev, queue_depth,
 				dev->dev_attrib.hw_queue_depth);
 			return -EINVAL;
 		}
-	} else {
-		if (queue_depth > dev->dev_attrib.queue_depth) {
-			if (queue_depth > dev->dev_attrib.hw_queue_depth) {
-				pr_err("dev[%p]: Passed queue_depth:"
-					" %u exceeds TCM/SE_Device MAX"
-					" TCQ: %u\n", dev, queue_depth,
-					dev->dev_attrib.hw_queue_depth);
-				return -EINVAL;
-			}
-		}
 	}
-
 	dev->dev_attrib.queue_depth = dev->queue_depth = queue_depth;
 	pr_debug("dev[%p]: SE Device TCQ Depth changed to: %u\n",
 			dev, queue_depth);
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_queue_depth);
 
 int se_dev_set_fabric_max_sectors(struct se_device *dev, u32 fabric_max_sectors)
 {
@@ -1123,22 +1124,12 @@ int se_dev_set_fabric_max_sectors(struct se_device *dev, u32 fabric_max_sectors)
 				DA_STATUS_MAX_SECTORS_MIN);
 		return -EINVAL;
 	}
-	if (dev->transport->transport_type == TRANSPORT_PLUGIN_PHBA_PDEV) {
-		if (fabric_max_sectors > dev->dev_attrib.hw_max_sectors) {
-			pr_err("dev[%p]: Passed fabric_max_sectors: %u"
-				" greater than TCM/SE_Device max_sectors:"
-				" %u\n", dev, fabric_max_sectors,
-				dev->dev_attrib.hw_max_sectors);
-			 return -EINVAL;
-		}
-	} else {
-		if (fabric_max_sectors > DA_STATUS_MAX_SECTORS_MAX) {
-			pr_err("dev[%p]: Passed fabric_max_sectors: %u"
-				" greater than DA_STATUS_MAX_SECTORS_MAX:"
-				" %u\n", dev, fabric_max_sectors,
-				DA_STATUS_MAX_SECTORS_MAX);
-			return -EINVAL;
-		}
+	if (fabric_max_sectors > DA_STATUS_MAX_SECTORS_MAX) {
+		pr_err("dev[%p]: Passed fabric_max_sectors: %u"
+			" greater than DA_STATUS_MAX_SECTORS_MAX:"
+			" %u\n", dev, fabric_max_sectors,
+			DA_STATUS_MAX_SECTORS_MAX);
+		return -EINVAL;
 	}
 	/*
 	 * Align max_sectors down to PAGE_SIZE to follow transport_allocate_data_tasks()
@@ -1155,6 +1146,7 @@ int se_dev_set_fabric_max_sectors(struct se_device *dev, u32 fabric_max_sectors)
 			dev, fabric_max_sectors);
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_fabric_max_sectors);
 
 int se_dev_set_optimal_sectors(struct se_device *dev, u32 optimal_sectors)
 {
@@ -1164,11 +1156,6 @@ int se_dev_set_optimal_sectors(struct se_device *dev, u32 optimal_sectors)
 			dev, dev->export_count);
 		return -EINVAL;
 	}
-	if (dev->transport->transport_type == TRANSPORT_PLUGIN_PHBA_PDEV) {
-		pr_err("dev[%p]: Passed optimal_sectors cannot be"
-				" changed for TCM/pSCSI\n", dev);
-		return -EINVAL;
-	}
 	if (optimal_sectors > dev->dev_attrib.fabric_max_sectors) {
 		pr_err("dev[%p]: Passed optimal_sectors %u cannot be"
 			" greater than fabric_max_sectors: %u\n", dev,
@@ -1181,6 +1168,7 @@ int se_dev_set_optimal_sectors(struct se_device *dev, u32 optimal_sectors)
 			dev, optimal_sectors);
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_optimal_sectors);
 
 int se_dev_set_block_size(struct se_device *dev, u32 block_size)
 {
@@ -1201,13 +1189,6 @@ int se_dev_set_block_size(struct se_device *dev, u32 block_size)
 		return -EINVAL;
 	}
 
-	if (dev->transport->transport_type == TRANSPORT_PLUGIN_PHBA_PDEV) {
-		pr_err("dev[%p]: Not allowed to change block_size for"
-			" Physical Device, use for Linux/SCSI to change"
-			" block_size for underlying hardware\n", dev);
-		return -EINVAL;
-	}
-
 	dev->dev_attrib.block_size = block_size;
 	pr_debug("dev[%p]: SE Device block_size changed to %u\n",
 			dev, block_size);
@@ -1218,6 +1199,7 @@ int se_dev_set_block_size(struct se_device *dev, u32 block_size)
 
 	return 0;
 }
+EXPORT_SYMBOL(se_dev_set_block_size);
 
 struct se_lun *core_dev_add_lun(
 	struct se_portal_group *tpg,
diff --git a/drivers/target/target_core_file.c b/drivers/target/target_core_file.c
index 72c83d98662b..c2aea099ea4a 100644
--- a/drivers/target/target_core_file.c
+++ b/drivers/target/target_core_file.c
@@ -37,6 +37,7 @@
 
 #include <target/target_core_base.h>
 #include <target/target_core_backend.h>
+#include <target/target_core_backend_configfs.h>
 
 #include "target_core_file.h"
 
@@ -934,6 +935,42 @@ fd_parse_cdb(struct se_cmd *cmd)
 	return sbc_parse_cdb(cmd, &fd_sbc_ops);
 }
 
+DEF_TB_DEFAULT_ATTRIBS(fileio);
+
+static struct configfs_attribute *fileio_backend_dev_attrs[] = {
+	&fileio_dev_attrib_emulate_model_alias.attr,
+	&fileio_dev_attrib_emulate_dpo.attr,
+	&fileio_dev_attrib_emulate_fua_write.attr,
+	&fileio_dev_attrib_emulate_fua_read.attr,
+	&fileio_dev_attrib_emulate_write_cache.attr,
+	&fileio_dev_attrib_emulate_ua_intlck_ctrl.attr,
+	&fileio_dev_attrib_emulate_tas.attr,
+	&fileio_dev_attrib_emulate_tpu.attr,
+	&fileio_dev_attrib_emulate_tpws.attr,
+	&fileio_dev_attrib_emulate_caw.attr,
+	&fileio_dev_attrib_emulate_3pc.attr,
+	&fileio_dev_attrib_pi_prot_type.attr,
+	&fileio_dev_attrib_hw_pi_prot_type.attr,
+	&fileio_dev_attrib_pi_prot_format.attr,
+	&fileio_dev_attrib_enforce_pr_isids.attr,
+	&fileio_dev_attrib_is_nonrot.attr,
+	&fileio_dev_attrib_emulate_rest_reord.attr,
+	&fileio_dev_attrib_force_pr_aptpl.attr,
+	&fileio_dev_attrib_hw_block_size.attr,
+	&fileio_dev_attrib_block_size.attr,
+	&fileio_dev_attrib_hw_max_sectors.attr,
+	&fileio_dev_attrib_fabric_max_sectors.attr,
+	&fileio_dev_attrib_optimal_sectors.attr,
+	&fileio_dev_attrib_hw_queue_depth.attr,
+	&fileio_dev_attrib_queue_depth.attr,
+	&fileio_dev_attrib_max_unmap_lba_count.attr,
+	&fileio_dev_attrib_max_unmap_block_desc_count.attr,
+	&fileio_dev_attrib_unmap_granularity.attr,
+	&fileio_dev_attrib_unmap_granularity_alignment.attr,
+	&fileio_dev_attrib_max_write_same_len.attr,
+	NULL,
+};
+
 static struct se_subsystem_api fileio_template = {
 	.name			= "fileio",
 	.inquiry_prod		= "FILEIO",
@@ -957,6 +994,11 @@ static struct se_subsystem_api fileio_template = {
 
 static int __init fileio_module_init(void)
 {
+	struct target_backend_cits *tbc = &fileio_template.tb_cits;
+
+	target_core_setup_sub_cits(&fileio_template);
+	tbc->tb_dev_attrib_cit.ct_attrs = fileio_backend_dev_attrs;
+
 	return transport_subsystem_register(&fileio_template);
 }
 
diff --git a/drivers/target/target_core_hba.c b/drivers/target/target_core_hba.c
index a25051a37dd7..ff95f95dcd13 100644
--- a/drivers/target/target_core_hba.c
+++ b/drivers/target/target_core_hba.c
@@ -36,6 +36,7 @@
 #include <target/target_core_base.h>
 #include <target/target_core_backend.h>
 #include <target/target_core_fabric.h>
+#include <target/target_core_configfs.h>
 
 #include "target_core_internal.h"
 
@@ -137,8 +138,7 @@ core_alloc_hba(const char *plugin_name, u32 plugin_dep_id, u32 hba_flags)
 	return hba;
 
 out_module_put:
-	if (hba->transport->owner)
-		module_put(hba->transport->owner);
+	module_put(hba->transport->owner);
 	hba->transport = NULL;
 out_free_hba:
 	kfree(hba);
@@ -159,8 +159,7 @@ core_delete_hba(struct se_hba *hba)
 	pr_debug("CORE_HBA[%d] - Detached HBA from Generic Target"
 			" Core\n", hba->hba_id);
 
-	if (hba->transport->owner)
-		module_put(hba->transport->owner);
+	module_put(hba->transport->owner);
 
 	hba->transport = NULL;
 	kfree(hba);
diff --git a/drivers/target/target_core_iblock.c b/drivers/target/target_core_iblock.c
index 7e6b857c6b3f..3efff94fbd97 100644
--- a/drivers/target/target_core_iblock.c
+++ b/drivers/target/target_core_iblock.c
@@ -41,6 +41,7 @@
 
 #include <target/target_core_base.h>
 #include <target/target_core_backend.h>
+#include <target/target_core_backend_configfs.h>
 
 #include "target_core_iblock.h"
 
@@ -858,6 +859,42 @@ static bool iblock_get_write_cache(struct se_device *dev)
 	return q->flush_flags & REQ_FLUSH;
 }
 
+DEF_TB_DEFAULT_ATTRIBS(iblock);
+
+static struct configfs_attribute *iblock_backend_dev_attrs[] = {
+	&iblock_dev_attrib_emulate_model_alias.attr,
+	&iblock_dev_attrib_emulate_dpo.attr,
+	&iblock_dev_attrib_emulate_fua_write.attr,
+	&iblock_dev_attrib_emulate_fua_read.attr,
+	&iblock_dev_attrib_emulate_write_cache.attr,
+	&iblock_dev_attrib_emulate_ua_intlck_ctrl.attr,
+	&iblock_dev_attrib_emulate_tas.attr,
+	&iblock_dev_attrib_emulate_tpu.attr,
+	&iblock_dev_attrib_emulate_tpws.attr,
+	&iblock_dev_attrib_emulate_caw.attr,
+	&iblock_dev_attrib_emulate_3pc.attr,
+	&iblock_dev_attrib_pi_prot_type.attr,
+	&iblock_dev_attrib_hw_pi_prot_type.attr,
+	&iblock_dev_attrib_pi_prot_format.attr,
+	&iblock_dev_attrib_enforce_pr_isids.attr,
+	&iblock_dev_attrib_is_nonrot.attr,
+	&iblock_dev_attrib_emulate_rest_reord.attr,
+	&iblock_dev_attrib_force_pr_aptpl.attr,
+	&iblock_dev_attrib_hw_block_size.attr,
+	&iblock_dev_attrib_block_size.attr,
+	&iblock_dev_attrib_hw_max_sectors.attr,
+	&iblock_dev_attrib_fabric_max_sectors.attr,
+	&iblock_dev_attrib_optimal_sectors.attr,
+	&iblock_dev_attrib_hw_queue_depth.attr,
+	&iblock_dev_attrib_queue_depth.attr,
+	&iblock_dev_attrib_max_unmap_lba_count.attr,
+	&iblock_dev_attrib_max_unmap_block_desc_count.attr,
+	&iblock_dev_attrib_unmap_granularity.attr,
+	&iblock_dev_attrib_unmap_granularity_alignment.attr,
+	&iblock_dev_attrib_max_write_same_len.attr,
+	NULL,
+};
+
 static struct se_subsystem_api iblock_template = {
 	.name			= "iblock",
 	.inquiry_prod		= "IBLOCK",
@@ -883,6 +920,11 @@ static struct se_subsystem_api iblock_template = {
 
 static int __init iblock_module_init(void)
 {
+	struct target_backend_cits *tbc = &iblock_template.tb_cits;
+
+	target_core_setup_sub_cits(&iblock_template);
+	tbc->tb_dev_attrib_cit.ct_attrs = iblock_backend_dev_attrs;
+
 	return transport_subsystem_register(&iblock_template);
 }
 
diff --git a/drivers/target/target_core_internal.h b/drivers/target/target_core_internal.h
index e31f42f369ff..60381db90026 100644
--- a/drivers/target/target_core_internal.h
+++ b/drivers/target/target_core_internal.h
@@ -18,34 +18,6 @@ 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	se_dev_set_task_timeout(struct se_device *, u32);
-int	se_dev_set_max_unmap_lba_count(struct se_device *, u32);
-int	se_dev_set_max_unmap_block_desc_count(struct se_device *, u32);
-int	se_dev_set_unmap_granularity(struct se_device *, u32);
-int	se_dev_set_unmap_granularity_alignment(struct se_device *, u32);
-int	se_dev_set_max_write_same_len(struct se_device *, u32);
-int	se_dev_set_emulate_model_alias(struct se_device *, int);
-int	se_dev_set_emulate_dpo(struct se_device *, int);
-int	se_dev_set_emulate_fua_write(struct se_device *, int);
-int	se_dev_set_emulate_fua_read(struct se_device *, int);
-int	se_dev_set_emulate_write_cache(struct se_device *, int);
-int	se_dev_set_emulate_ua_intlck_ctrl(struct se_device *, int);
-int	se_dev_set_emulate_tas(struct se_device *, int);
-int	se_dev_set_emulate_tpu(struct se_device *, int);
-int	se_dev_set_emulate_tpws(struct se_device *, int);
-int	se_dev_set_emulate_caw(struct se_device *, int);
-int	se_dev_set_emulate_3pc(struct se_device *, int);
-int	se_dev_set_pi_prot_type(struct se_device *, int);
-int	se_dev_set_pi_prot_format(struct se_device *, int);
-int	se_dev_set_enforce_pr_isids(struct se_device *, int);
-int	se_dev_set_force_pr_aptpl(struct se_device *, int);
-int	se_dev_set_is_nonrot(struct se_device *, int);
-int	se_dev_set_emulate_rest_reord(struct se_device *dev, int);
-int	se_dev_set_queue_depth(struct se_device *, u32);
-int	se_dev_set_max_sectors(struct se_device *, u32);
-int	se_dev_set_fabric_max_sectors(struct se_device *, u32);
-int	se_dev_set_optimal_sectors(struct se_device *, u32);
-int	se_dev_set_block_size(struct se_device *, u32);
 struct se_lun *core_dev_add_lun(struct se_portal_group *, struct se_device *, u32);
 void	core_dev_del_lun(struct se_portal_group *, struct se_lun *);
 struct se_lun *core_get_lun_from_tpg(struct se_portal_group *, u32);
diff --git a/drivers/target/target_core_pr.c b/drivers/target/target_core_pr.c
index 4c261c33cf55..d56f2aaba9af 100644
--- a/drivers/target/target_core_pr.c
+++ b/drivers/target/target_core_pr.c
@@ -76,7 +76,7 @@ enum preempt_type {
 };
 
 static void __core_scsi3_complete_pro_release(struct se_device *, struct se_node_acl *,
-			struct t10_pr_registration *, int);
+					      struct t10_pr_registration *, int, int);
 
 static sense_reason_t
 target_scsi2_reservation_check(struct se_cmd *cmd)
@@ -1177,7 +1177,7 @@ static int core_scsi3_check_implicit_release(
 		 *    service action with the SERVICE ACTION RESERVATION KEY
 		 *    field set to zero (see 5.7.11.3).
 		 */
-		__core_scsi3_complete_pro_release(dev, nacl, pr_reg, 0);
+		__core_scsi3_complete_pro_release(dev, nacl, pr_reg, 0, 1);
 		ret = 1;
 		/*
 		 * For 'All Registrants' reservation types, all existing
@@ -1219,7 +1219,8 @@ static void __core_scsi3_free_registration(
 
 	pr_reg->pr_reg_deve->def_pr_registered = 0;
 	pr_reg->pr_reg_deve->pr_res_key = 0;
-	list_del(&pr_reg->pr_reg_list);
+	if (!list_empty(&pr_reg->pr_reg_list))
+		list_del(&pr_reg->pr_reg_list);
 	/*
 	 * Caller accessing *pr_reg using core_scsi3_locate_pr_reg(),
 	 * so call core_scsi3_put_pr_reg() to decrement our reference.
@@ -1271,6 +1272,7 @@ void core_scsi3_free_pr_reg_from_nacl(
 {
 	struct t10_reservation *pr_tmpl = &dev->t10_pr;
 	struct t10_pr_registration *pr_reg, *pr_reg_tmp, *pr_res_holder;
+	bool free_reg = false;
 	/*
 	 * If the passed se_node_acl matches the reservation holder,
 	 * release the reservation.
@@ -1278,13 +1280,18 @@ void core_scsi3_free_pr_reg_from_nacl(
 	spin_lock(&dev->dev_reservation_lock);
 	pr_res_holder = dev->dev_pr_res_holder;
 	if ((pr_res_holder != NULL) &&
-	    (pr_res_holder->pr_reg_nacl == nacl))
-		__core_scsi3_complete_pro_release(dev, nacl, pr_res_holder, 0);
+	    (pr_res_holder->pr_reg_nacl == nacl)) {
+		__core_scsi3_complete_pro_release(dev, nacl, pr_res_holder, 0, 1);
+		free_reg = true;
+	}
 	spin_unlock(&dev->dev_reservation_lock);
 	/*
 	 * Release any registration associated with the struct se_node_acl.
 	 */
 	spin_lock(&pr_tmpl->registration_lock);
+	if (pr_res_holder && free_reg)
+		__core_scsi3_free_registration(dev, pr_res_holder, NULL, 0);
+
 	list_for_each_entry_safe(pr_reg, pr_reg_tmp,
 			&pr_tmpl->registration_list, pr_reg_list) {
 
@@ -1307,7 +1314,7 @@ void core_scsi3_free_all_registrations(
 	if (pr_res_holder != NULL) {
 		struct se_node_acl *pr_res_nacl = pr_res_holder->pr_reg_nacl;
 		__core_scsi3_complete_pro_release(dev, pr_res_nacl,
-				pr_res_holder, 0);
+						  pr_res_holder, 0, 0);
 	}
 	spin_unlock(&dev->dev_reservation_lock);
 
@@ -1429,14 +1436,12 @@ core_scsi3_decode_spec_i_port(
 	struct target_core_fabric_ops *tmp_tf_ops;
 	unsigned char *buf;
 	unsigned char *ptr, *i_str = NULL, proto_ident, tmp_proto_ident;
-	char *iport_ptr = NULL, dest_iport[64], i_buf[PR_REG_ISID_ID_LEN];
+	char *iport_ptr = NULL, i_buf[PR_REG_ISID_ID_LEN];
 	sense_reason_t ret;
 	u32 tpdl, tid_len = 0;
 	int dest_local_nexus;
 	u32 dest_rtpi = 0;
 
-	memset(dest_iport, 0, 64);
-
 	local_se_deve = se_sess->se_node_acl->device_list[cmd->orig_fe_lun];
 	/*
 	 * Allocate a struct pr_transport_id_holder and setup the
@@ -2105,13 +2110,13 @@ core_scsi3_emulate_pro_register(struct se_cmd *cmd, u64 res_key, u64 sa_res_key,
 		/*
 		 * sa_res_key=0 Unregister Reservation Key for registered I_T Nexus.
 		 */
-		pr_holder = core_scsi3_check_implicit_release(
-				cmd->se_dev, pr_reg);
+		type = pr_reg->pr_res_type;
+		pr_holder = core_scsi3_check_implicit_release(cmd->se_dev,
+							      pr_reg);
 		if (pr_holder < 0) {
 			ret = TCM_RESERVATION_CONFLICT;
 			goto out;
 		}
-		type = pr_reg->pr_res_type;
 
 		spin_lock(&pr_tmpl->registration_lock);
 		/*
@@ -2269,6 +2274,7 @@ core_scsi3_pro_reserve(struct se_cmd *cmd, int type, int scope, u64 res_key)
 	spin_lock(&dev->dev_reservation_lock);
 	pr_res_holder = dev->dev_pr_res_holder;
 	if (pr_res_holder) {
+		int pr_res_type = pr_res_holder->pr_res_type;
 		/*
 		 * From spc4r17 Section 5.7.9: Reserving:
 		 *
@@ -2279,7 +2285,9 @@ core_scsi3_pro_reserve(struct se_cmd *cmd, int type, int scope, u64 res_key)
 		 * the logical unit, then the command shall be completed with
 		 * RESERVATION CONFLICT status.
 		 */
-		if (pr_res_holder != pr_reg) {
+		if ((pr_res_holder != pr_reg) &&
+		    (pr_res_type != PR_TYPE_WRITE_EXCLUSIVE_ALLREG) &&
+		    (pr_res_type != PR_TYPE_EXCLUSIVE_ACCESS_ALLREG)) {
 			struct se_node_acl *pr_res_nacl = pr_res_holder->pr_reg_nacl;
 			pr_err("SPC-3 PR: Attempted RESERVE from"
 				" [%s]: %s while reservation already held by"
@@ -2385,23 +2393,59 @@ static void __core_scsi3_complete_pro_release(
 	struct se_device *dev,
 	struct se_node_acl *se_nacl,
 	struct t10_pr_registration *pr_reg,
-	int explicit)
+	int explicit,
+	int unreg)
 {
 	struct target_core_fabric_ops *tfo = se_nacl->se_tpg->se_tpg_tfo;
 	char i_buf[PR_REG_ISID_ID_LEN];
+	int pr_res_type = 0, pr_res_scope = 0;
 
 	memset(i_buf, 0, PR_REG_ISID_ID_LEN);
 	core_pr_dump_initiator_port(pr_reg, i_buf, PR_REG_ISID_ID_LEN);
 	/*
 	 * Go ahead and release the current PR reservation holder.
+	 * If an All Registrants reservation is currently active and
+	 * a unregister operation is requested, replace the current
+	 * dev_pr_res_holder with another active registration.
 	 */
-	dev->dev_pr_res_holder = NULL;
+	if (dev->dev_pr_res_holder) {
+		pr_res_type = dev->dev_pr_res_holder->pr_res_type;
+		pr_res_scope = dev->dev_pr_res_holder->pr_res_scope;
+		dev->dev_pr_res_holder->pr_res_type = 0;
+		dev->dev_pr_res_holder->pr_res_scope = 0;
+		dev->dev_pr_res_holder->pr_res_holder = 0;
+		dev->dev_pr_res_holder = NULL;
+	}
+	if (!unreg)
+		goto out;
 
-	pr_debug("SPC-3 PR [%s] Service Action: %s RELEASE cleared"
-		" reservation holder TYPE: %s ALL_TG_PT: %d\n",
-		tfo->get_fabric_name(), (explicit) ? "explicit" : "implicit",
-		core_scsi3_pr_dump_type(pr_reg->pr_res_type),
-		(pr_reg->pr_reg_all_tg_pt) ? 1 : 0);
+	spin_lock(&dev->t10_pr.registration_lock);
+	list_del_init(&pr_reg->pr_reg_list);
+	/*
+	 * If the I_T nexus is a reservation holder, the persistent reservation
+	 * is of an all registrants type, and the I_T nexus is the last remaining
+	 * registered I_T nexus, then the device server shall also release the
+	 * persistent reservation.
+	 */
+	if (!list_empty(&dev->t10_pr.registration_list) &&
+	    ((pr_res_type == PR_TYPE_WRITE_EXCLUSIVE_ALLREG) ||
+	     (pr_res_type == PR_TYPE_EXCLUSIVE_ACCESS_ALLREG))) {
+		dev->dev_pr_res_holder =
+			list_entry(dev->t10_pr.registration_list.next,
+				   struct t10_pr_registration, pr_reg_list);
+		dev->dev_pr_res_holder->pr_res_type = pr_res_type;
+		dev->dev_pr_res_holder->pr_res_scope = pr_res_scope;
+		dev->dev_pr_res_holder->pr_res_holder = 1;
+	}
+	spin_unlock(&dev->t10_pr.registration_lock);
+out:
+	if (!dev->dev_pr_res_holder) {
+		pr_debug("SPC-3 PR [%s] Service Action: %s RELEASE cleared"
+			" reservation holder TYPE: %s ALL_TG_PT: %d\n",
+			tfo->get_fabric_name(), (explicit) ? "explicit" :
+			"implicit", core_scsi3_pr_dump_type(pr_res_type),
+			(pr_reg->pr_reg_all_tg_pt) ? 1 : 0);
+	}
 	pr_debug("SPC-3 PR [%s] RELEASE Node: %s%s\n",
 		tfo->get_fabric_name(), se_nacl->initiatorname,
 		i_buf);
@@ -2532,7 +2576,7 @@ core_scsi3_emulate_pro_release(struct se_cmd *cmd, int type, int scope,
 	 *    server shall not establish a unit attention condition.
 	 */
 	__core_scsi3_complete_pro_release(dev, se_sess->se_node_acl,
-			pr_reg, 1);
+					  pr_reg, 1, 0);
 
 	spin_unlock(&dev->dev_reservation_lock);
 
@@ -2620,7 +2664,7 @@ core_scsi3_emulate_pro_clear(struct se_cmd *cmd, u64 res_key)
 	if (pr_res_holder) {
 		struct se_node_acl *pr_res_nacl = pr_res_holder->pr_reg_nacl;
 		__core_scsi3_complete_pro_release(dev, pr_res_nacl,
-			pr_res_holder, 0);
+						  pr_res_holder, 0, 0);
 	}
 	spin_unlock(&dev->dev_reservation_lock);
 	/*
@@ -2679,7 +2723,7 @@ static void __core_scsi3_complete_pro_preempt(
 	 */
 	if (dev->dev_pr_res_holder)
 		__core_scsi3_complete_pro_release(dev, nacl,
-				dev->dev_pr_res_holder, 0);
+						  dev->dev_pr_res_holder, 0, 0);
 
 	dev->dev_pr_res_holder = pr_reg;
 	pr_reg->pr_res_holder = 1;
@@ -2924,8 +2968,8 @@ core_scsi3_pro_preempt(struct se_cmd *cmd, int type, int scope, u64 res_key,
 	 */
 	if (pr_reg_n != pr_res_holder)
 		__core_scsi3_complete_pro_release(dev,
-				pr_res_holder->pr_reg_nacl,
-				dev->dev_pr_res_holder, 0);
+						  pr_res_holder->pr_reg_nacl,
+						  dev->dev_pr_res_holder, 0, 0);
 	/*
 	 * b) Remove the registrations for all I_T nexuses identified
 	 *    by the SERVICE ACTION RESERVATION KEY field, except the
@@ -3059,7 +3103,7 @@ core_scsi3_emulate_pro_register_and_move(struct se_cmd *cmd, u64 res_key,
 	struct t10_reservation *pr_tmpl = &dev->t10_pr;
 	unsigned char *buf;
 	unsigned char *initiator_str;
-	char *iport_ptr = NULL, dest_iport[64], i_buf[PR_REG_ISID_ID_LEN];
+	char *iport_ptr = NULL, i_buf[PR_REG_ISID_ID_LEN];
 	u32 tid_len, tmp_tid_len;
 	int new_reg = 0, type, scope, matching_iname;
 	sense_reason_t ret;
@@ -3071,7 +3115,6 @@ core_scsi3_emulate_pro_register_and_move(struct se_cmd *cmd, u64 res_key,
 		return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 	}
 
-	memset(dest_iport, 0, 64);
 	memset(i_buf, 0, PR_REG_ISID_ID_LEN);
 	se_tpg = se_sess->se_tpg;
 	tf_ops = se_tpg->se_tpg_tfo;
@@ -3389,7 +3432,7 @@ after_iport_check:
 	 *    holder (i.e., the I_T nexus on which the
 	 */
 	__core_scsi3_complete_pro_release(dev, pr_res_nacl,
-			dev->dev_pr_res_holder, 0);
+					  dev->dev_pr_res_holder, 0, 0);
 	/*
 	 * g) Move the persistent reservation to the specified I_T nexus using
 	 *    the same scope and type as the persistent reservation released in
@@ -3837,7 +3880,8 @@ core_scsi3_pri_read_full_status(struct se_cmd *cmd)
 	unsigned char *buf;
 	u32 add_desc_len = 0, add_len = 0, desc_len, exp_desc_len;
 	u32 off = 8; /* off into first Full Status descriptor */
-	int format_code = 0;
+	int format_code = 0, pr_res_type = 0, pr_res_scope = 0;
+	bool all_reg = false;
 
 	if (cmd->data_length < 8) {
 		pr_err("PRIN SA READ_FULL_STATUS SCSI Data Length: %u"
@@ -3854,6 +3898,19 @@ core_scsi3_pri_read_full_status(struct se_cmd *cmd)
 	buf[2] = ((dev->t10_pr.pr_generation >> 8) & 0xff);
 	buf[3] = (dev->t10_pr.pr_generation & 0xff);
 
+	spin_lock(&dev->dev_reservation_lock);
+	if (dev->dev_pr_res_holder) {
+		struct t10_pr_registration *pr_holder = dev->dev_pr_res_holder;
+
+		if (pr_holder->pr_res_type == PR_TYPE_WRITE_EXCLUSIVE_ALLREG ||
+		    pr_holder->pr_res_type == PR_TYPE_EXCLUSIVE_ACCESS_ALLREG) {
+			all_reg = true;
+			pr_res_type = pr_holder->pr_res_type;
+			pr_res_scope = pr_holder->pr_res_scope;
+		}
+	}
+	spin_unlock(&dev->dev_reservation_lock);
+
 	spin_lock(&pr_tmpl->registration_lock);
 	list_for_each_entry_safe(pr_reg, pr_reg_tmp,
 			&pr_tmpl->registration_list, pr_reg_list) {
@@ -3901,14 +3958,20 @@ core_scsi3_pri_read_full_status(struct se_cmd *cmd)
 		 * reservation holder for PR_HOLDER bit.
 		 *
 		 * Also, if this registration is the reservation
-		 * holder, fill in SCOPE and TYPE in the next byte.
+		 * holder or there is an All Registrants reservation
+		 * active, fill in SCOPE and TYPE in the next byte.
 		 */
 		if (pr_reg->pr_res_holder) {
 			buf[off++] |= 0x01;
 			buf[off++] = (pr_reg->pr_res_scope & 0xf0) |
 				     (pr_reg->pr_res_type & 0x0f);
-		} else
+		} else if (all_reg) {
+			buf[off++] |= 0x01;
+			buf[off++] = (pr_res_scope & 0xf0) |
+				     (pr_res_type & 0x0f);
+		} else {
 			off += 2;
+		}
 
 		off += 4; /* Skip over reserved area */
 		/*
diff --git a/drivers/target/target_core_pscsi.c b/drivers/target/target_core_pscsi.c
index 7c8291f0bbbc..1045dcd7bf65 100644
--- a/drivers/target/target_core_pscsi.c
+++ b/drivers/target/target_core_pscsi.c
@@ -44,6 +44,7 @@
 
 #include <target/target_core_base.h>
 #include <target/target_core_backend.h>
+#include <target/target_core_backend_configfs.h>
 
 #include "target_core_alua.h"
 #include "target_core_pscsi.h"
@@ -1094,7 +1095,7 @@ pscsi_execute_cmd(struct se_cmd *cmd)
 	req->retries = PS_RETRY;
 
 	blk_execute_rq_nowait(pdv->pdv_sd->request_queue, NULL, req,
-			(cmd->sam_task_attr == MSG_HEAD_TAG),
+			(cmd->sam_task_attr == TCM_HEAD_TAG),
 			pscsi_req_done);
 
 	return 0;
@@ -1165,6 +1166,26 @@ static void pscsi_req_done(struct request *req, int uptodate)
 	kfree(pt);
 }
 
+DEF_TB_DEV_ATTRIB_RO(pscsi, hw_pi_prot_type);
+TB_DEV_ATTR_RO(pscsi, hw_pi_prot_type);
+
+DEF_TB_DEV_ATTRIB_RO(pscsi, hw_block_size);
+TB_DEV_ATTR_RO(pscsi, hw_block_size);
+
+DEF_TB_DEV_ATTRIB_RO(pscsi, hw_max_sectors);
+TB_DEV_ATTR_RO(pscsi, hw_max_sectors);
+
+DEF_TB_DEV_ATTRIB_RO(pscsi, hw_queue_depth);
+TB_DEV_ATTR_RO(pscsi, hw_queue_depth);
+
+static struct configfs_attribute *pscsi_backend_dev_attrs[] = {
+	&pscsi_dev_attrib_hw_pi_prot_type.attr,
+	&pscsi_dev_attrib_hw_block_size.attr,
+	&pscsi_dev_attrib_hw_max_sectors.attr,
+	&pscsi_dev_attrib_hw_queue_depth.attr,
+	NULL,
+};
+
 static struct se_subsystem_api pscsi_template = {
 	.name			= "pscsi",
 	.owner			= THIS_MODULE,
@@ -1185,6 +1206,11 @@ static struct se_subsystem_api pscsi_template = {
 
 static int __init pscsi_module_init(void)
 {
+	struct target_backend_cits *tbc = &pscsi_template.tb_cits;
+
+	target_core_setup_sub_cits(&pscsi_template);
+	tbc->tb_dev_attrib_cit.ct_attrs = pscsi_backend_dev_attrs;
+
 	return transport_subsystem_register(&pscsi_template);
 }
 
diff --git a/drivers/target/target_core_rd.c b/drivers/target/target_core_rd.c
index b920db3388cd..60ebd170a561 100644
--- a/drivers/target/target_core_rd.c
+++ b/drivers/target/target_core_rd.c
@@ -34,6 +34,7 @@
 
 #include <target/target_core_base.h>
 #include <target/target_core_backend.h>
+#include <target/target_core_backend_configfs.h>
 
 #include "target_core_rd.h"
 
@@ -632,6 +633,42 @@ rd_parse_cdb(struct se_cmd *cmd)
 	return sbc_parse_cdb(cmd, &rd_sbc_ops);
 }
 
+DEF_TB_DEFAULT_ATTRIBS(rd_mcp);
+
+static struct configfs_attribute *rd_mcp_backend_dev_attrs[] = {
+	&rd_mcp_dev_attrib_emulate_model_alias.attr,
+	&rd_mcp_dev_attrib_emulate_dpo.attr,
+	&rd_mcp_dev_attrib_emulate_fua_write.attr,
+	&rd_mcp_dev_attrib_emulate_fua_read.attr,
+	&rd_mcp_dev_attrib_emulate_write_cache.attr,
+	&rd_mcp_dev_attrib_emulate_ua_intlck_ctrl.attr,
+	&rd_mcp_dev_attrib_emulate_tas.attr,
+	&rd_mcp_dev_attrib_emulate_tpu.attr,
+	&rd_mcp_dev_attrib_emulate_tpws.attr,
+	&rd_mcp_dev_attrib_emulate_caw.attr,
+	&rd_mcp_dev_attrib_emulate_3pc.attr,
+	&rd_mcp_dev_attrib_pi_prot_type.attr,
+	&rd_mcp_dev_attrib_hw_pi_prot_type.attr,
+	&rd_mcp_dev_attrib_pi_prot_format.attr,
+	&rd_mcp_dev_attrib_enforce_pr_isids.attr,
+	&rd_mcp_dev_attrib_is_nonrot.attr,
+	&rd_mcp_dev_attrib_emulate_rest_reord.attr,
+	&rd_mcp_dev_attrib_force_pr_aptpl.attr,
+	&rd_mcp_dev_attrib_hw_block_size.attr,
+	&rd_mcp_dev_attrib_block_size.attr,
+	&rd_mcp_dev_attrib_hw_max_sectors.attr,
+	&rd_mcp_dev_attrib_fabric_max_sectors.attr,
+	&rd_mcp_dev_attrib_optimal_sectors.attr,
+	&rd_mcp_dev_attrib_hw_queue_depth.attr,
+	&rd_mcp_dev_attrib_queue_depth.attr,
+	&rd_mcp_dev_attrib_max_unmap_lba_count.attr,
+	&rd_mcp_dev_attrib_max_unmap_block_desc_count.attr,
+	&rd_mcp_dev_attrib_unmap_granularity.attr,
+	&rd_mcp_dev_attrib_unmap_granularity_alignment.attr,
+	&rd_mcp_dev_attrib_max_write_same_len.attr,
+	NULL,
+};
+
 static struct se_subsystem_api rd_mcp_template = {
 	.name			= "rd_mcp",
 	.inquiry_prod		= "RAMDISK-MCP",
@@ -653,8 +690,12 @@ static struct se_subsystem_api rd_mcp_template = {
 
 int __init rd_module_init(void)
 {
+	struct target_backend_cits *tbc = &rd_mcp_template.tb_cits;
 	int ret;
 
+	target_core_setup_sub_cits(&rd_mcp_template);
+	tbc->tb_dev_attrib_cit.ct_attrs = rd_mcp_backend_dev_attrs;
+
 	ret = transport_subsystem_register(&rd_mcp_template);
 	if (ret < 0) {
 		return ret;
diff --git a/drivers/target/target_core_sbc.c b/drivers/target/target_core_sbc.c
index 8d171ff77e75..11bea1952435 100644
--- a/drivers/target/target_core_sbc.c
+++ b/drivers/target/target_core_sbc.c
@@ -485,7 +485,7 @@ static sense_reason_t compare_and_write_callback(struct se_cmd *cmd)
 	cmd->t_data_nents_orig = cmd->t_data_nents;
 	cmd->t_data_nents = 1;
 
-	cmd->sam_task_attr = MSG_HEAD_TAG;
+	cmd->sam_task_attr = TCM_HEAD_TAG;
 	cmd->transport_complete_callback = compare_and_write_post;
 	/*
 	 * Now reset ->execute_cmd() to the normal sbc_execute_rw() handler
diff --git a/drivers/target/target_core_spc.c b/drivers/target/target_core_spc.c
index bc286a67af7c..1307600fe726 100644
--- a/drivers/target/target_core_spc.c
+++ b/drivers/target/target_core_spc.c
@@ -1357,7 +1357,7 @@ spc_parse_cdb(struct se_cmd *cmd, unsigned int *size)
 		 * Do implicit HEAD_OF_QUEUE processing for INQUIRY.
 		 * See spc4r17 section 5.3
 		 */
-		cmd->sam_task_attr = MSG_HEAD_TAG;
+		cmd->sam_task_attr = TCM_HEAD_TAG;
 		cmd->execute_cmd = spc_emulate_inquiry;
 		break;
 	case SECURITY_PROTOCOL_IN:
@@ -1391,7 +1391,7 @@ spc_parse_cdb(struct se_cmd *cmd, unsigned int *size)
 		 * Do implicit HEAD_OF_QUEUE processing for REPORT_LUNS
 		 * See spc4r17 section 5.3
 		 */
-		cmd->sam_task_attr = MSG_HEAD_TAG;
+		cmd->sam_task_attr = TCM_HEAD_TAG;
 		break;
 	case TEST_UNIT_READY:
 		cmd->execute_cmd = spc_emulate_testunitready;
diff --git a/drivers/target/target_core_transport.c b/drivers/target/target_core_transport.c
index be877bf6f730..0adc0f650213 100644
--- a/drivers/target/target_core_transport.c
+++ b/drivers/target/target_core_transport.c
@@ -1159,7 +1159,7 @@ transport_check_alloc_task_attr(struct se_cmd *cmd)
 	if (dev->transport->transport_type == TRANSPORT_PLUGIN_PHBA_PDEV)
 		return 0;
 
-	if (cmd->sam_task_attr == MSG_ACA_TAG) {
+	if (cmd->sam_task_attr == TCM_ACA_TAG) {
 		pr_debug("SAM Task Attribute ACA"
 			" emulation is not supported\n");
 		return TCM_INVALID_CDB_FIELD;
@@ -1531,7 +1531,7 @@ int target_submit_tmr(struct se_cmd *se_cmd, struct se_session *se_sess,
 	BUG_ON(!se_tpg);
 
 	transport_init_se_cmd(se_cmd, se_tpg->se_tpg_tfo, se_sess,
-			      0, DMA_NONE, MSG_SIMPLE_TAG, sense);
+			      0, DMA_NONE, TCM_SIMPLE_TAG, sense);
 	/*
 	 * FIXME: Currently expect caller to handle se_cmd->se_tmr_req
 	 * allocation failure.
@@ -1718,12 +1718,12 @@ static bool target_handle_task_attr(struct se_cmd *cmd)
 	 * to allow the passed struct se_cmd list of tasks to the front of the list.
 	 */
 	switch (cmd->sam_task_attr) {
-	case MSG_HEAD_TAG:
+	case TCM_HEAD_TAG:
 		pr_debug("Added HEAD_OF_QUEUE for CDB: 0x%02x, "
 			 "se_ordered_id: %u\n",
 			 cmd->t_task_cdb[0], cmd->se_ordered_id);
 		return false;
-	case MSG_ORDERED_TAG:
+	case TCM_ORDERED_TAG:
 		atomic_inc_mb(&dev->dev_ordered_sync);
 
 		pr_debug("Added ORDERED for CDB: 0x%02x to ordered list, "
@@ -1828,7 +1828,7 @@ static void target_restart_delayed_cmds(struct se_device *dev)
 
 		__target_execute_cmd(cmd);
 
-		if (cmd->sam_task_attr == MSG_ORDERED_TAG)
+		if (cmd->sam_task_attr == TCM_ORDERED_TAG)
 			break;
 	}
 }
@@ -1844,18 +1844,18 @@ static void transport_complete_task_attr(struct se_cmd *cmd)
 	if (dev->transport->transport_type == TRANSPORT_PLUGIN_PHBA_PDEV)
 		return;
 
-	if (cmd->sam_task_attr == MSG_SIMPLE_TAG) {
+	if (cmd->sam_task_attr == TCM_SIMPLE_TAG) {
 		atomic_dec_mb(&dev->simple_cmds);
 		dev->dev_cur_ordered_id++;
 		pr_debug("Incremented dev->dev_cur_ordered_id: %u for"
 			" SIMPLE: %u\n", dev->dev_cur_ordered_id,
 			cmd->se_ordered_id);
-	} else if (cmd->sam_task_attr == MSG_HEAD_TAG) {
+	} else if (cmd->sam_task_attr == TCM_HEAD_TAG) {
 		dev->dev_cur_ordered_id++;
 		pr_debug("Incremented dev_cur_ordered_id: %u for"
 			" HEAD_OF_QUEUE: %u\n", dev->dev_cur_ordered_id,
 			cmd->se_ordered_id);
-	} else if (cmd->sam_task_attr == MSG_ORDERED_TAG) {
+	} else if (cmd->sam_task_attr == TCM_ORDERED_TAG) {
 		atomic_dec_mb(&dev->dev_ordered_sync);
 
 		dev->dev_cur_ordered_id++;
diff --git a/drivers/target/target_core_user.c b/drivers/target/target_core_user.c
index 9a1b314f6482..8bfa61c9693d 100644
--- a/drivers/target/target_core_user.c
+++ b/drivers/target/target_core_user.c
@@ -28,6 +28,8 @@
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
 #include <target/target_core_backend.h>
+#include <target/target_core_backend_configfs.h>
+
 #include <linux/target_core_user.h>
 
 /*
@@ -1092,6 +1094,42 @@ tcmu_parse_cdb(struct se_cmd *cmd)
 	return ret;
 }
 
+DEF_TB_DEFAULT_ATTRIBS(tcmu);
+
+static struct configfs_attribute *tcmu_backend_dev_attrs[] = {
+	&tcmu_dev_attrib_emulate_model_alias.attr,
+	&tcmu_dev_attrib_emulate_dpo.attr,
+	&tcmu_dev_attrib_emulate_fua_write.attr,
+	&tcmu_dev_attrib_emulate_fua_read.attr,
+	&tcmu_dev_attrib_emulate_write_cache.attr,
+	&tcmu_dev_attrib_emulate_ua_intlck_ctrl.attr,
+	&tcmu_dev_attrib_emulate_tas.attr,
+	&tcmu_dev_attrib_emulate_tpu.attr,
+	&tcmu_dev_attrib_emulate_tpws.attr,
+	&tcmu_dev_attrib_emulate_caw.attr,
+	&tcmu_dev_attrib_emulate_3pc.attr,
+	&tcmu_dev_attrib_pi_prot_type.attr,
+	&tcmu_dev_attrib_hw_pi_prot_type.attr,
+	&tcmu_dev_attrib_pi_prot_format.attr,
+	&tcmu_dev_attrib_enforce_pr_isids.attr,
+	&tcmu_dev_attrib_is_nonrot.attr,
+	&tcmu_dev_attrib_emulate_rest_reord.attr,
+	&tcmu_dev_attrib_force_pr_aptpl.attr,
+	&tcmu_dev_attrib_hw_block_size.attr,
+	&tcmu_dev_attrib_block_size.attr,
+	&tcmu_dev_attrib_hw_max_sectors.attr,
+	&tcmu_dev_attrib_fabric_max_sectors.attr,
+	&tcmu_dev_attrib_optimal_sectors.attr,
+	&tcmu_dev_attrib_hw_queue_depth.attr,
+	&tcmu_dev_attrib_queue_depth.attr,
+	&tcmu_dev_attrib_max_unmap_lba_count.attr,
+	&tcmu_dev_attrib_max_unmap_block_desc_count.attr,
+	&tcmu_dev_attrib_unmap_granularity.attr,
+	&tcmu_dev_attrib_unmap_granularity_alignment.attr,
+	&tcmu_dev_attrib_max_write_same_len.attr,
+	NULL,
+};
+
 static struct se_subsystem_api tcmu_template = {
 	.name			= "user",
 	.inquiry_prod		= "USER",
@@ -1112,6 +1150,7 @@ static struct se_subsystem_api tcmu_template = {
 
 static int __init tcmu_module_init(void)
 {
+	struct target_backend_cits *tbc = &tcmu_template.tb_cits;
 	int ret;
 
 	BUILD_BUG_ON((sizeof(struct tcmu_cmd_entry) % TCMU_OP_ALIGN_SIZE) != 0);
@@ -1134,6 +1173,9 @@ static int __init tcmu_module_init(void)
 		goto out_unreg_device;
 	}
 
+	target_core_setup_sub_cits(&tcmu_template);
+	tbc->tb_dev_attrib_cit.ct_attrs = tcmu_backend_dev_attrs;
+
 	ret = transport_subsystem_register(&tcmu_template);
 	if (ret)
 		goto out_unreg_genl;
diff --git a/drivers/target/target_core_xcopy.c b/drivers/target/target_core_xcopy.c
index e9186cdf35e9..33ac39bf75e5 100644
--- a/drivers/target/target_core_xcopy.c
+++ b/drivers/target/target_core_xcopy.c
@@ -335,7 +335,7 @@ static int target_xcopy_parse_segment_descriptors(struct se_cmd *se_cmd,
 			desc += XCOPY_SEGMENT_DESC_LEN;
 			break;
 		default:
-			pr_err("XCOPY unspported segment descriptor"
+			pr_err("XCOPY unsupported segment descriptor"
 				"type: 0x%02x\n", desc[0]);
 			goto out;
 		}
diff --git a/drivers/target/tcm_fc/tfc_cmd.c b/drivers/target/tcm_fc/tfc_cmd.c
index be0c0d08c56a..edcafa4490c0 100644
--- a/drivers/target/tcm_fc/tfc_cmd.c
+++ b/drivers/target/tcm_fc/tfc_cmd.c
@@ -554,17 +554,17 @@ static void ft_send_work(struct work_struct *work)
 	 */
 	switch (fcp->fc_pri_ta & FCP_PTA_MASK) {
 	case FCP_PTA_HEADQ:
-		task_attr = MSG_HEAD_TAG;
+		task_attr = TCM_HEAD_TAG;
 		break;
 	case FCP_PTA_ORDERED:
-		task_attr = MSG_ORDERED_TAG;
+		task_attr = TCM_ORDERED_TAG;
 		break;
 	case FCP_PTA_ACA:
-		task_attr = MSG_ACA_TAG;
+		task_attr = TCM_ACA_TAG;
 		break;
 	case FCP_PTA_SIMPLE: /* Fallthrough */
 	default:
-		task_attr = MSG_SIMPLE_TAG;
+		task_attr = TCM_SIMPLE_TAG;
 	}
 
 	fc_seq_exch(cmd->seq)->lp->tt.seq_set_resp(cmd->seq, ft_recv_seq, cmd);
diff --git a/drivers/tc/tc.c b/drivers/tc/tc.c
index 946562389ca8..3be9519654e5 100644
--- a/drivers/tc/tc.c
+++ b/drivers/tc/tc.c
@@ -83,8 +83,7 @@ static void __init tc_bus_add_devices(struct tc_bus *tbus)
 		/* Found a board, allocate it an entry in the list */
 		tdev = kzalloc(sizeof(*tdev), GFP_KERNEL);
 		if (!tdev) {
-			printk(KERN_ERR "tc%x: unable to allocate tc_dev\n",
-			       slot);
+			pr_err("tc%x: unable to allocate tc_dev\n", slot);
 			goto out_err;
 		}
 		dev_set_name(&tdev->dev, "tc%x", slot);
@@ -117,10 +116,10 @@ static void __init tc_bus_add_devices(struct tc_bus *tbus)
 			tdev->resource.start = extslotaddr;
 			tdev->resource.end = extslotaddr + devsize - 1;
 		} else {
-			printk(KERN_ERR "%s: Cannot provide slot space "
-			       "(%dMiB required, up to %dMiB supported)\n",
-			       dev_name(&tdev->dev), devsize >> 20,
-			       max(slotsize, extslotsize) >> 20);
+			pr_err("%s: Cannot provide slot space "
+			       "(%ldMiB required, up to %ldMiB supported)\n",
+			       dev_name(&tdev->dev), (long)(devsize >> 20),
+			       (long)(max(slotsize, extslotsize) >> 20));
 			kfree(tdev);
 			goto out_err;
 		}
@@ -147,14 +146,12 @@ static int __init tc_init(void)
 {
 	/* Initialize the TURBOchannel bus */
 	if (tc_bus_get_info(&tc_bus))
-		return 0;
+		goto out_err;
 
 	INIT_LIST_HEAD(&tc_bus.devices);
 	dev_set_name(&tc_bus.dev, "tc");
-	if (device_register(&tc_bus.dev)) {
-		put_device(&tc_bus.dev);
-		return 0;
-	}
+	if (device_register(&tc_bus.dev))
+		goto out_err_device;
 
 	if (tc_bus.info.slot_size) {
 		unsigned int tc_clock = tc_get_speed(&tc_bus) / 100000;
@@ -172,8 +169,8 @@ static int __init tc_init(void)
 		tc_bus.resource[0].flags = IORESOURCE_MEM;
 		if (request_resource(&iomem_resource,
 				     &tc_bus.resource[0]) < 0) {
-			printk(KERN_ERR "tc: Cannot reserve resource\n");
-			return 0;
+			pr_err("tc: Cannot reserve resource\n");
+			goto out_err_device;
 		}
 		if (tc_bus.ext_slot_size) {
 			tc_bus.resource[1].start = tc_bus.ext_slot_base;
@@ -184,10 +181,8 @@ static int __init tc_init(void)
 			tc_bus.resource[1].flags = IORESOURCE_MEM;
 			if (request_resource(&iomem_resource,
 					     &tc_bus.resource[1]) < 0) {
-				printk(KERN_ERR
-				       "tc: Cannot reserve resource\n");
-				release_resource(&tc_bus.resource[0]);
-				return 0;
+				pr_err("tc: Cannot reserve resource\n");
+				goto out_err_resource;
 			}
 		}
 
@@ -195,6 +190,13 @@ static int __init tc_init(void)
 	}
 
 	return 0;
+
+out_err_resource:
+	release_resource(&tc_bus.resource[0]);
+out_err_device:
+	put_device(&tc_bus.dev);
+out_err:
+	return 0;
 }
 
 subsys_initcall(tc_init);
diff --git a/drivers/thermal/Kconfig b/drivers/thermal/Kconfig
index f554d25b4399..af40db0df58e 100644
--- a/drivers/thermal/Kconfig
+++ b/drivers/thermal/Kconfig
@@ -112,6 +112,18 @@ config CPU_THERMAL
 
 	  If you want this support, you should say Y here.
 
+config CLOCK_THERMAL
+	bool "Generic clock cooling support"
+	depends on COMMON_CLK
+	depends on PM_OPP
+	help
+	  This entry implements the generic clock cooling mechanism through
+	  frequency clipping. Typically used to cool off co-processors. The
+	  device that is configured to use this cooling mechanism will be
+	  controlled to reduce clock frequency whenever temperature is high.
+
+	  If you want this support, you should say Y here.
+
 config THERMAL_EMULATION
 	bool "Thermal emulation mode support"
 	help
@@ -143,6 +155,16 @@ config SPEAR_THERMAL
 	  Enable this to plug the SPEAr thermal sensor driver into the Linux
 	  thermal framework.
 
+config ROCKCHIP_THERMAL
+	tristate "Rockchip thermal driver"
+	depends on ARCH_ROCKCHIP
+	depends on RESET_CONTROLLER
+	help
+	  Rockchip thermal driver provides support for Temperature sensor
+	  ADC (TS-ADC) found on Rockchip SoCs. It supports one critical
+	  trip point. Cpufreq is used as the cooling device and will throttle
+	  CPUs when the Temperature crosses the passive trip point.
+
 config RCAR_THERMAL
 	tristate "Renesas R-Car thermal driver"
 	depends on ARCH_SHMOBILE || COMPILE_TEST
@@ -185,6 +207,16 @@ config ARMADA_THERMAL
 	  Enable this option if you want to have support for thermal management
 	  controller present in Armada 370 and Armada XP SoC.
 
+config TEGRA_SOCTHERM
+	tristate "Tegra SOCTHERM thermal management"
+	depends on ARCH_TEGRA
+	help
+	  Enable this option for integrated thermal management support on NVIDIA
+	  Tegra124 systems-on-chip. The driver supports four thermal zones
+	  (CPU, GPU, MEM, PLLX). Cooling devices can be bound to the thermal
+	  zones to manage temperatures. This option is also required for the
+	  emergency thermal reset (thermtrip) feature to function.
+
 config DB8500_CPUFREQ_COOLING
 	tristate "DB8500 cpufreq cooling"
 	depends on ARCH_U8500
diff --git a/drivers/thermal/Makefile b/drivers/thermal/Makefile
index 39c4fe87da2f..fa0dc486790f 100644
--- a/drivers/thermal/Makefile
+++ b/drivers/thermal/Makefile
@@ -18,8 +18,12 @@ thermal_sys-$(CONFIG_THERMAL_GOV_USER_SPACE)	+= user_space.o
 # cpufreq cooling
 thermal_sys-$(CONFIG_CPU_THERMAL)	+= cpu_cooling.o
 
+# clock cooling
+thermal_sys-$(CONFIG_CLOCK_THERMAL)	+= clock_cooling.o
+
 # platform thermal drivers
 obj-$(CONFIG_SPEAR_THERMAL)	+= spear_thermal.o
+obj-$(CONFIG_ROCKCHIP_THERMAL)	+= rockchip_thermal.o
 obj-$(CONFIG_RCAR_THERMAL)	+= rcar_thermal.o
 obj-$(CONFIG_KIRKWOOD_THERMAL)  += kirkwood_thermal.o
 obj-y				+= samsung/
@@ -34,3 +38,4 @@ obj-$(CONFIG_INTEL_SOC_DTS_THERMAL)	+= intel_soc_dts_thermal.o
 obj-$(CONFIG_TI_SOC_THERMAL)	+= ti-soc-thermal/
 obj-$(CONFIG_INT340X_THERMAL)  += int340x_thermal/
 obj-$(CONFIG_ST_THERMAL)	+= st/
+obj-$(CONFIG_TEGRA_SOCTHERM)	+= tegra_soctherm.o
diff --git a/drivers/thermal/armada_thermal.c b/drivers/thermal/armada_thermal.c
index 9d1420acb391..c2556cf5186b 100644
--- a/drivers/thermal/armada_thermal.c
+++ b/drivers/thermal/armada_thermal.c
@@ -35,10 +35,6 @@
 #define PMU_TDC0_OTF_CAL_MASK		(0x1 << 30)
 #define PMU_TDC0_START_CAL_MASK		(0x1 << 25)
 
-#define A375_Z1_CAL_RESET_LSB		0x8011e214
-#define A375_Z1_CAL_RESET_MSB		0x30a88019
-#define A375_Z1_WORKAROUND_BIT		BIT(9)
-
 #define A375_UNIT_CONTROL_SHIFT		27
 #define A375_UNIT_CONTROL_MASK		0x7
 #define A375_READOUT_INVERT		BIT(15)
@@ -124,24 +120,12 @@ static void armada375_init_sensor(struct platform_device *pdev,
 				  struct armada_thermal_priv *priv)
 {
 	unsigned long reg;
-	bool quirk_needed =
-		!!of_device_is_compatible(pdev->dev.of_node,
-					  "marvell,armada375-z1-thermal");
-
-	if (quirk_needed) {
-		/* Ensure these registers have the default (reset) values */
-		writel(A375_Z1_CAL_RESET_LSB, priv->control);
-		writel(A375_Z1_CAL_RESET_MSB, priv->control + 0x4);
-	}
 
 	reg = readl(priv->control + 4);
 	reg &= ~(A375_UNIT_CONTROL_MASK << A375_UNIT_CONTROL_SHIFT);
 	reg &= ~A375_READOUT_INVERT;
 	reg &= ~A375_HW_RESETn;
 
-	if (quirk_needed)
-		reg |= A375_Z1_WORKAROUND_BIT;
-
 	writel(reg, priv->control + 4);
 	mdelay(20);
 
@@ -260,10 +244,6 @@ static const struct of_device_id armada_thermal_id_table[] = {
 		.data       = &armada375_data,
 	},
 	{
-		.compatible = "marvell,armada375-z1-thermal",
-		.data       = &armada375_data,
-	},
-	{
 		.compatible = "marvell,armada380-thermal",
 		.data       = &armada380_data,
 	},
@@ -329,7 +309,6 @@ static struct platform_driver armada_thermal_driver = {
 	.remove = armada_thermal_exit,
 	.driver = {
 		.name = "armada_thermal",
-		.owner = THIS_MODULE,
 		.of_match_table = armada_thermal_id_table,
 	},
 };
diff --git a/drivers/thermal/clock_cooling.c b/drivers/thermal/clock_cooling.c
new file mode 100644
index 000000000000..1b4ff0f4c716
--- /dev/null
+++ b/drivers/thermal/clock_cooling.c
@@ -0,0 +1,485 @@
+/*
+ *  drivers/thermal/clock_cooling.c
+ *
+ *  Copyright (C) 2014 Eduardo Valentin <edubezval@gmail.com>
+ *
+ *  Copyright (C) 2013	Texas Instruments Inc.
+ *  Contact:  Eduardo Valentin <eduardo.valentin@ti.com>
+ *
+ *  Highly based on cpu_cooling.c.
+ *  Copyright (C) 2012	Samsung Electronics Co., Ltd(http://www.samsung.com)
+ *  Copyright (C) 2012  Amit Daniel <amit.kachhap@linaro.org>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; version 2 of the License.
+ *
+ *  This program is distributed in the hope that it will be useful, but
+ *  WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  General Public License for more details.
+ */
+#include <linux/clk.h>
+#include <linux/cpufreq.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/idr.h>
+#include <linux/mutex.h>
+#include <linux/pm_opp.h>
+#include <linux/slab.h>
+#include <linux/thermal.h>
+#include <linux/clock_cooling.h>
+
+/**
+ * struct clock_cooling_device - data for cooling device with clock
+ * @id: unique integer value corresponding to each clock_cooling_device
+ *	registered.
+ * @dev: struct device pointer to the device being used to cool off using
+ *       clock frequencies.
+ * @cdev: thermal_cooling_device pointer to keep track of the
+ *	registered cooling device.
+ * @clk_rate_change_nb: reference to notifier block used to receive clock
+ *                      rate changes.
+ * @freq_table: frequency table used to keep track of available frequencies.
+ * @clock_state: integer value representing the current state of clock
+ *	cooling	devices.
+ * @clock_val: integer value representing the absolute value of the clipped
+ *	frequency.
+ * @clk: struct clk reference used to enforce clock limits.
+ * @lock: mutex lock to protect this struct.
+ *
+ * This structure is required for keeping information of each
+ * clock_cooling_device registered. In order to prevent corruption of this a
+ * mutex @lock is used.
+ */
+struct clock_cooling_device {
+	int id;
+	struct device *dev;
+	struct thermal_cooling_device *cdev;
+	struct notifier_block clk_rate_change_nb;
+	struct cpufreq_frequency_table *freq_table;
+	unsigned long clock_state;
+	unsigned long clock_val;
+	struct clk *clk;
+	struct mutex lock; /* lock to protect the content of this struct */
+};
+#define to_clock_cooling_device(x) \
+		container_of(x, struct clock_cooling_device, clk_rate_change_nb)
+static DEFINE_IDR(clock_idr);
+static DEFINE_MUTEX(cooling_clock_lock);
+
+/**
+ * clock_cooling_get_idr - function to get an unique id.
+ * @id: int * value generated by this function.
+ *
+ * This function will populate @id with an unique
+ * id, using the idr API.
+ *
+ * Return: 0 on success, an error code on failure.
+ */
+static int clock_cooling_get_idr(int *id)
+{
+	int ret;
+
+	mutex_lock(&cooling_clock_lock);
+	ret = idr_alloc(&clock_idr, NULL, 0, 0, GFP_KERNEL);
+	mutex_unlock(&cooling_clock_lock);
+	if (unlikely(ret < 0))
+		return ret;
+	*id = ret;
+
+	return 0;
+}
+
+/**
+ * release_idr - function to free the unique id.
+ * @id: int value representing the unique id.
+ */
+static void release_idr(int id)
+{
+	mutex_lock(&cooling_clock_lock);
+	idr_remove(&clock_idr, id);
+	mutex_unlock(&cooling_clock_lock);
+}
+
+/* Below code defines functions to be used for clock as cooling device */
+
+enum clock_cooling_property {
+	GET_LEVEL,
+	GET_FREQ,
+	GET_MAXL,
+};
+
+/**
+ * clock_cooling_get_property - fetch a property of interest for a give cpu.
+ * @ccdev: clock cooling device reference
+ * @input: query parameter
+ * @output: query return
+ * @property: type of query (frequency, level, max level)
+ *
+ * This is the common function to
+ * 1. get maximum clock cooling states
+ * 2. translate frequency to cooling state
+ * 3. translate cooling state to frequency
+ * Note that the code may be not in good shape
+ * but it is written in this way in order to:
+ * a) reduce duplicate code as most of the code can be shared.
+ * b) make sure the logic is consistent when translating between
+ *    cooling states and frequencies.
+ *
+ * Return: 0 on success, -EINVAL when invalid parameters are passed.
+ */
+static int clock_cooling_get_property(struct clock_cooling_device *ccdev,
+				      unsigned long input,
+				      unsigned long *output,
+				      enum clock_cooling_property property)
+{
+	int i;
+	unsigned long max_level = 0, level = 0;
+	unsigned int freq = CPUFREQ_ENTRY_INVALID;
+	int descend = -1;
+	struct cpufreq_frequency_table *pos, *table = ccdev->freq_table;
+
+	if (!output)
+		return -EINVAL;
+
+	if (!table)
+		return -EINVAL;
+
+	cpufreq_for_each_valid_entry(pos, table) {
+		/* ignore duplicate entry */
+		if (freq == pos->frequency)
+			continue;
+
+		/* get the frequency order */
+		if (freq != CPUFREQ_ENTRY_INVALID && descend == -1)
+			descend = freq > pos->frequency;
+
+		freq = pos->frequency;
+		max_level++;
+	}
+
+	/* No valid cpu frequency entry */
+	if (max_level == 0)
+		return -EINVAL;
+
+	/* max_level is an index, not a counter */
+	max_level--;
+
+	/* get max level */
+	if (property == GET_MAXL) {
+		*output = max_level;
+		return 0;
+	}
+
+	if (property == GET_FREQ)
+		level = descend ? input : (max_level - input);
+
+	i = 0;
+	cpufreq_for_each_valid_entry(pos, table) {
+		/* ignore duplicate entry */
+		if (freq == pos->frequency)
+			continue;
+
+		/* now we have a valid frequency entry */
+		freq = pos->frequency;
+
+		if (property == GET_LEVEL && (unsigned int)input == freq) {
+			/* get level by frequency */
+			*output = descend ? i : (max_level - i);
+			return 0;
+		}
+		if (property == GET_FREQ && level == i) {
+			/* get frequency by level */
+			*output = freq;
+			return 0;
+		}
+		i++;
+	}
+
+	return -EINVAL;
+}
+
+/**
+ * clock_cooling_get_level - return the cooling level of given clock cooling.
+ * @cdev: reference of a thermal cooling device of used as clock cooling device
+ * @freq: the frequency of interest
+ *
+ * This function will match the cooling level corresponding to the
+ * requested @freq and return it.
+ *
+ * Return: The matched cooling level on success or THERMAL_CSTATE_INVALID
+ * otherwise.
+ */
+unsigned long clock_cooling_get_level(struct thermal_cooling_device *cdev,
+				      unsigned long freq)
+{
+	struct clock_cooling_device *ccdev = cdev->devdata;
+	unsigned long val;
+
+	if (clock_cooling_get_property(ccdev, (unsigned long)freq, &val,
+				       GET_LEVEL))
+		return THERMAL_CSTATE_INVALID;
+
+	return val;
+}
+EXPORT_SYMBOL_GPL(clock_cooling_get_level);
+
+/**
+ * clock_cooling_get_frequency - get the absolute value of frequency from level.
+ * @ccdev: clock cooling device reference
+ * @level: cooling level
+ *
+ * This function matches cooling level with frequency. Based on a cooling level
+ * of frequency, equals cooling state of cpu cooling device, it will return
+ * the corresponding frequency.
+ *	e.g level=0 --> 1st MAX FREQ, level=1 ---> 2nd MAX FREQ, .... etc
+ *
+ * Return: 0 on error, the corresponding frequency otherwise.
+ */
+static unsigned long
+clock_cooling_get_frequency(struct clock_cooling_device *ccdev,
+			    unsigned long level)
+{
+	int ret = 0;
+	unsigned long freq;
+
+	ret = clock_cooling_get_property(ccdev, level, &freq, GET_FREQ);
+	if (ret)
+		return 0;
+
+	return freq;
+}
+
+/**
+ * clock_cooling_apply - function to apply frequency clipping.
+ * @ccdev: clock_cooling_device pointer containing frequency clipping data.
+ * @cooling_state: value of the cooling state.
+ *
+ * Function used to make sure the clock layer is aware of current thermal
+ * limits. The limits are applied by updating the clock rate in case it is
+ * higher than the corresponding frequency based on the requested cooling_state.
+ *
+ * Return: 0 on success, an error code otherwise (-EINVAL in case wrong
+ * cooling state).
+ */
+static int clock_cooling_apply(struct clock_cooling_device *ccdev,
+			       unsigned long cooling_state)
+{
+	unsigned long clip_freq, cur_freq;
+	int ret = 0;
+
+	/* Here we write the clipping */
+	/* Check if the old cooling action is same as new cooling action */
+	if (ccdev->clock_state == cooling_state)
+		return 0;
+
+	clip_freq = clock_cooling_get_frequency(ccdev, cooling_state);
+	if (!clip_freq)
+		return -EINVAL;
+
+	cur_freq = clk_get_rate(ccdev->clk);
+
+	mutex_lock(&ccdev->lock);
+	ccdev->clock_state = cooling_state;
+	ccdev->clock_val = clip_freq;
+	/* enforce clock level */
+	if (cur_freq > clip_freq)
+		ret = clk_set_rate(ccdev->clk, clip_freq);
+	mutex_unlock(&ccdev->lock);
+
+	return ret;
+}
+
+/**
+ * clock_cooling_clock_notifier - notifier callback on clock rate changes.
+ * @nb:	struct notifier_block * with callback info.
+ * @event: value showing clock event for which this function invoked.
+ * @data: callback-specific data
+ *
+ * Callback to hijack the notification on clock transition.
+ * Every time there is a clock change, we intercept all pre change events
+ * and block the transition in case the new rate infringes thermal limits.
+ *
+ * Return: NOTIFY_DONE (success) or NOTIFY_BAD (new_rate > thermal limit).
+ */
+static int clock_cooling_clock_notifier(struct notifier_block *nb,
+					unsigned long event, void *data)
+{
+	struct clk_notifier_data *ndata = data;
+	struct clock_cooling_device *ccdev = to_clock_cooling_device(nb);
+
+	switch (event) {
+	case PRE_RATE_CHANGE:
+		/*
+		 * checks on current state
+		 * TODO: current method is not best we can find as it
+		 * allows possibly voltage transitions, in case DVFS
+		 * layer is also hijacking clock pre notifications.
+		 */
+		if (ndata->new_rate > ccdev->clock_val)
+			return NOTIFY_BAD;
+		/* fall through */
+	case POST_RATE_CHANGE:
+	case ABORT_RATE_CHANGE:
+	default:
+		return NOTIFY_DONE;
+	}
+}
+
+/* clock cooling device thermal callback functions are defined below */
+
+/**
+ * clock_cooling_get_max_state - callback function to get the max cooling state.
+ * @cdev: thermal cooling device pointer.
+ * @state: fill this variable with the max cooling state.
+ *
+ * Callback for the thermal cooling device to return the clock
+ * max cooling state.
+ *
+ * Return: 0 on success, an error code otherwise.
+ */
+static int clock_cooling_get_max_state(struct thermal_cooling_device *cdev,
+				       unsigned long *state)
+{
+	struct clock_cooling_device *ccdev = cdev->devdata;
+	unsigned long count = 0;
+	int ret;
+
+	ret = clock_cooling_get_property(ccdev, 0, &count, GET_MAXL);
+	if (!ret)
+		*state = count;
+
+	return ret;
+}
+
+/**
+ * clock_cooling_get_cur_state - function to get the current cooling state.
+ * @cdev: thermal cooling device pointer.
+ * @state: fill this variable with the current cooling state.
+ *
+ * Callback for the thermal cooling device to return the clock
+ * current cooling state.
+ *
+ * Return: 0 (success)
+ */
+static int clock_cooling_get_cur_state(struct thermal_cooling_device *cdev,
+				       unsigned long *state)
+{
+	struct clock_cooling_device *ccdev = cdev->devdata;
+
+	*state = ccdev->clock_state;
+
+	return 0;
+}
+
+/**
+ * clock_cooling_set_cur_state - function to set the current cooling state.
+ * @cdev: thermal cooling device pointer.
+ * @state: set this variable to the current cooling state.
+ *
+ * Callback for the thermal cooling device to change the clock cooling
+ * current cooling state.
+ *
+ * Return: 0 on success, an error code otherwise.
+ */
+static int clock_cooling_set_cur_state(struct thermal_cooling_device *cdev,
+				       unsigned long state)
+{
+	struct clock_cooling_device *clock_device = cdev->devdata;
+
+	return clock_cooling_apply(clock_device, state);
+}
+
+/* Bind clock callbacks to thermal cooling device ops */
+static struct thermal_cooling_device_ops const clock_cooling_ops = {
+	.get_max_state = clock_cooling_get_max_state,
+	.get_cur_state = clock_cooling_get_cur_state,
+	.set_cur_state = clock_cooling_set_cur_state,
+};
+
+/**
+ * clock_cooling_register - function to create clock cooling device.
+ * @dev: struct device pointer to the device used as clock cooling device.
+ * @clock_name: string containing the clock used as cooling mechanism.
+ *
+ * This interface function registers the clock cooling device with the name
+ * "thermal-clock-%x". The cooling device is based on clock frequencies.
+ * The struct device is assumed to be capable of DVFS transitions.
+ * The OPP layer is used to fetch and fill the available frequencies for
+ * the referred device. The ordered frequency table is used to control
+ * the clock cooling device cooling states and to limit clock transitions
+ * based on the cooling state requested by the thermal framework.
+ *
+ * Return: a valid struct thermal_cooling_device pointer on success,
+ * on failure, it returns a corresponding ERR_PTR().
+ */
+struct thermal_cooling_device *
+clock_cooling_register(struct device *dev, const char *clock_name)
+{
+	struct thermal_cooling_device *cdev;
+	struct clock_cooling_device *ccdev = NULL;
+	char dev_name[THERMAL_NAME_LENGTH];
+	int ret = 0;
+
+	ccdev = devm_kzalloc(dev, sizeof(*ccdev), GFP_KERNEL);
+	if (!ccdev)
+		return ERR_PTR(-ENOMEM);
+
+	ccdev->dev = dev;
+	ccdev->clk = devm_clk_get(dev, clock_name);
+	if (IS_ERR(ccdev->clk))
+		return ERR_CAST(ccdev->clk);
+
+	ret = clock_cooling_get_idr(&ccdev->id);
+	if (ret)
+		return ERR_PTR(-EINVAL);
+
+	snprintf(dev_name, sizeof(dev_name), "thermal-clock-%d", ccdev->id);
+
+	cdev = thermal_cooling_device_register(dev_name, ccdev,
+					       &clock_cooling_ops);
+	if (IS_ERR(cdev)) {
+		release_idr(ccdev->id);
+		return ERR_PTR(-EINVAL);
+	}
+	ccdev->cdev = cdev;
+	ccdev->clk_rate_change_nb.notifier_call = clock_cooling_clock_notifier;
+
+	/* Assuming someone has already filled the opp table for this device */
+	ret = dev_pm_opp_init_cpufreq_table(dev, &ccdev->freq_table);
+	if (ret) {
+		release_idr(ccdev->id);
+		return ERR_PTR(ret);
+	}
+	ccdev->clock_state = 0;
+	ccdev->clock_val = clock_cooling_get_frequency(ccdev, 0);
+
+	clk_notifier_register(ccdev->clk, &ccdev->clk_rate_change_nb);
+
+	return cdev;
+}
+EXPORT_SYMBOL_GPL(clock_cooling_register);
+
+/**
+ * clock_cooling_unregister - function to remove clock cooling device.
+ * @cdev: thermal cooling device pointer.
+ *
+ * This interface function unregisters the "thermal-clock-%x" cooling device.
+ */
+void clock_cooling_unregister(struct thermal_cooling_device *cdev)
+{
+	struct clock_cooling_device *ccdev;
+
+	if (!cdev)
+		return;
+
+	ccdev = cdev->devdata;
+
+	clk_notifier_unregister(ccdev->clk, &ccdev->clk_rate_change_nb);
+	dev_pm_opp_free_cpufreq_table(ccdev->dev, &ccdev->freq_table);
+
+	thermal_cooling_device_unregister(ccdev->cdev);
+	release_idr(ccdev->id);
+}
+EXPORT_SYMBOL_GPL(clock_cooling_unregister);
diff --git a/drivers/thermal/cpu_cooling.c b/drivers/thermal/cpu_cooling.c
index ad09e51ffae4..f65f0d109fc8 100644
--- a/drivers/thermal/cpu_cooling.c
+++ b/drivers/thermal/cpu_cooling.c
@@ -4,6 +4,8 @@
  *  Copyright (C) 2012	Samsung Electronics Co., Ltd(http://www.samsung.com)
  *  Copyright (C) 2012  Amit Daniel <amit.kachhap@linaro.org>
  *
+ *  Copyright (C) 2014  Viresh Kumar <viresh.kumar@linaro.org>
+ *
  * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
@@ -28,6 +30,20 @@
 #include <linux/cpu.h>
 #include <linux/cpu_cooling.h>
 
+/*
+ * Cooling state <-> CPUFreq frequency
+ *
+ * Cooling states are translated to frequencies throughout this driver and this
+ * is the relation between them.
+ *
+ * Highest cooling state corresponds to lowest possible frequency.
+ *
+ * i.e.
+ *	level 0 --> 1st Max Freq
+ *	level 1 --> 2nd Max Freq
+ *	...
+ */
+
 /**
  * struct cpufreq_cooling_device - data for cooling device with cpufreq
  * @id: unique integer value corresponding to each cpufreq_cooling_device
@@ -38,25 +54,27 @@
  *	cooling	devices.
  * @cpufreq_val: integer value representing the absolute value of the clipped
  *	frequency.
+ * @max_level: maximum cooling level. One less than total number of valid
+ *	cpufreq frequencies.
  * @allowed_cpus: all the cpus involved for this cpufreq_cooling_device.
+ * @node: list_head to link all cpufreq_cooling_device together.
  *
- * This structure is required for keeping information of each
- * cpufreq_cooling_device registered. In order to prevent corruption of this a
- * mutex lock cooling_cpufreq_lock is used.
+ * This structure is required for keeping information of each registered
+ * cpufreq_cooling_device.
  */
 struct cpufreq_cooling_device {
 	int id;
 	struct thermal_cooling_device *cool_dev;
 	unsigned int cpufreq_state;
 	unsigned int cpufreq_val;
+	unsigned int max_level;
+	unsigned int *freq_table;	/* In descending order */
 	struct cpumask allowed_cpus;
 	struct list_head node;
 };
 static DEFINE_IDR(cpufreq_idr);
 static DEFINE_MUTEX(cooling_cpufreq_lock);
 
-static unsigned int cpufreq_dev_count;
-
 static LIST_HEAD(cpufreq_dev_list);
 
 /**
@@ -98,120 +116,30 @@ static void release_idr(struct idr *idr, int id)
 /* Below code defines functions to be used for cpufreq as cooling device */
 
 /**
- * is_cpufreq_valid - function to check frequency transitioning capability.
- * @cpu: cpu for which check is needed.
+ * get_level: Find the level for a particular frequency
+ * @cpufreq_dev: cpufreq_dev for which the property is required
+ * @freq: Frequency
  *
- * This function will check the current state of the system if
- * it is capable of changing the frequency for a given @cpu.
- *
- * Return: 0 if the system is not currently capable of changing
- * the frequency of given cpu. !0 in case the frequency is changeable.
+ * Return: level on success, THERMAL_CSTATE_INVALID on error.
  */
-static int is_cpufreq_valid(int cpu)
+static unsigned long get_level(struct cpufreq_cooling_device *cpufreq_dev,
+			       unsigned int freq)
 {
-	struct cpufreq_policy policy;
-
-	return !cpufreq_get_policy(&policy, cpu);
-}
-
-enum cpufreq_cooling_property {
-	GET_LEVEL,
-	GET_FREQ,
-	GET_MAXL,
-};
-
-/**
- * get_property - fetch a property of interest for a give cpu.
- * @cpu: cpu for which the property is required
- * @input: query parameter
- * @output: query return
- * @property: type of query (frequency, level, max level)
- *
- * This is the common function to
- * 1. get maximum cpu cooling states
- * 2. translate frequency to cooling state
- * 3. translate cooling state to frequency
- * Note that the code may be not in good shape
- * but it is written in this way in order to:
- * a) reduce duplicate code as most of the code can be shared.
- * b) make sure the logic is consistent when translating between
- *    cooling states and frequencies.
- *
- * Return: 0 on success, -EINVAL when invalid parameters are passed.
- */
-static int get_property(unsigned int cpu, unsigned long input,
-			unsigned int *output,
-			enum cpufreq_cooling_property property)
-{
-	int i;
-	unsigned long max_level = 0, level = 0;
-	unsigned int freq = CPUFREQ_ENTRY_INVALID;
-	int descend = -1;
-	struct cpufreq_frequency_table *pos, *table =
-					cpufreq_frequency_get_table(cpu);
-
-	if (!output)
-		return -EINVAL;
-
-	if (!table)
-		return -EINVAL;
-
-	cpufreq_for_each_valid_entry(pos, table) {
-		/* ignore duplicate entry */
-		if (freq == pos->frequency)
-			continue;
-
-		/* get the frequency order */
-		if (freq != CPUFREQ_ENTRY_INVALID && descend == -1)
-			descend = freq > pos->frequency;
-
-		freq = pos->frequency;
-		max_level++;
-	}
-
-	/* No valid cpu frequency entry */
-	if (max_level == 0)
-		return -EINVAL;
+	unsigned long level;
 
-	/* max_level is an index, not a counter */
-	max_level--;
+	for (level = 0; level <= cpufreq_dev->max_level; level++) {
+		if (freq == cpufreq_dev->freq_table[level])
+			return level;
 
-	/* get max level */
-	if (property == GET_MAXL) {
-		*output = (unsigned int)max_level;
-		return 0;
+		if (freq > cpufreq_dev->freq_table[level])
+			break;
 	}
 
-	if (property == GET_FREQ)
-		level = descend ? input : (max_level - input);
-
-	i = 0;
-	cpufreq_for_each_valid_entry(pos, table) {
-		/* ignore duplicate entry */
-		if (freq == pos->frequency)
-			continue;
-
-		/* now we have a valid frequency entry */
-		freq = pos->frequency;
-
-		if (property == GET_LEVEL && (unsigned int)input == freq) {
-			/* get level by frequency */
-			*output = descend ? i : (max_level - i);
-			return 0;
-		}
-		if (property == GET_FREQ && level == i) {
-			/* get frequency by level */
-			*output = freq;
-			return 0;
-		}
-		i++;
-	}
-
-	return -EINVAL;
+	return THERMAL_CSTATE_INVALID;
 }
 
 /**
- * cpufreq_cooling_get_level - for a give cpu, return the cooling level.
+ * cpufreq_cooling_get_level - for a given cpu, return the cooling level.
  * @cpu: cpu for which the level is required
  * @freq: the frequency of interest
  *
@@ -223,77 +151,21 @@ static int get_property(unsigned int cpu, unsigned long input,
  */
 unsigned long cpufreq_cooling_get_level(unsigned int cpu, unsigned int freq)
 {
-	unsigned int val;
-
-	if (get_property(cpu, (unsigned long)freq, &val, GET_LEVEL))
-		return THERMAL_CSTATE_INVALID;
-
-	return (unsigned long)val;
-}
-EXPORT_SYMBOL_GPL(cpufreq_cooling_get_level);
-
-/**
- * get_cpu_frequency - get the absolute value of frequency from level.
- * @cpu: cpu for which frequency is fetched.
- * @level: cooling level
- *
- * This function matches cooling level with frequency. Based on a cooling level
- * of frequency, equals cooling state of cpu cooling device, it will return
- * the corresponding frequency.
- *	e.g level=0 --> 1st MAX FREQ, level=1 ---> 2nd MAX FREQ, .... etc
- *
- * Return: 0 on error, the corresponding frequency otherwise.
- */
-static unsigned int get_cpu_frequency(unsigned int cpu, unsigned long level)
-{
-	int ret = 0;
-	unsigned int freq;
-
-	ret = get_property(cpu, level, &freq, GET_FREQ);
-	if (ret)
-		return 0;
-
-	return freq;
-}
-
-/**
- * cpufreq_apply_cooling - function to apply frequency clipping.
- * @cpufreq_device: cpufreq_cooling_device pointer containing frequency
- *	clipping data.
- * @cooling_state: value of the cooling state.
- *
- * Function used to make sure the cpufreq layer is aware of current thermal
- * limits. The limits are applied by updating the cpufreq policy.
- *
- * Return: 0 on success, an error code otherwise (-EINVAL in case wrong
- * cooling state).
- */
-static int cpufreq_apply_cooling(struct cpufreq_cooling_device *cpufreq_device,
-				 unsigned long cooling_state)
-{
-	unsigned int cpuid, clip_freq;
-	struct cpumask *mask = &cpufreq_device->allowed_cpus;
-	unsigned int cpu = cpumask_any(mask);
-
-
-	/* Check if the old cooling action is same as new cooling action */
-	if (cpufreq_device->cpufreq_state == cooling_state)
-		return 0;
-
-	clip_freq = get_cpu_frequency(cpu, cooling_state);
-	if (!clip_freq)
-		return -EINVAL;
-
-	cpufreq_device->cpufreq_state = cooling_state;
-	cpufreq_device->cpufreq_val = clip_freq;
+	struct cpufreq_cooling_device *cpufreq_dev;
 
-	for_each_cpu(cpuid, mask) {
-		if (is_cpufreq_valid(cpuid))
-			cpufreq_update_policy(cpuid);
+	mutex_lock(&cooling_cpufreq_lock);
+	list_for_each_entry(cpufreq_dev, &cpufreq_dev_list, node) {
+		if (cpumask_test_cpu(cpu, &cpufreq_dev->allowed_cpus)) {
+			mutex_unlock(&cooling_cpufreq_lock);
+			return get_level(cpufreq_dev, freq);
+		}
 	}
+	mutex_unlock(&cooling_cpufreq_lock);
 
-	return 0;
+	pr_err("%s: cpu:%d not part of any cooling device\n", __func__, cpu);
+	return THERMAL_CSTATE_INVALID;
 }
+EXPORT_SYMBOL_GPL(cpufreq_cooling_get_level);
 
 /**
  * cpufreq_thermal_notifier - notifier callback for cpufreq policy change.
@@ -323,11 +195,6 @@ static int cpufreq_thermal_notifier(struct notifier_block *nb,
 					&cpufreq_dev->allowed_cpus))
 			continue;
 
-		if (!cpufreq_dev->cpufreq_val)
-			cpufreq_dev->cpufreq_val = get_cpu_frequency(
-					cpumask_any(&cpufreq_dev->allowed_cpus),
-					cpufreq_dev->cpufreq_state);
-
 		max_freq = cpufreq_dev->cpufreq_val;
 
 		if (policy->max != max_freq)
@@ -354,19 +221,9 @@ static int cpufreq_get_max_state(struct thermal_cooling_device *cdev,
 				 unsigned long *state)
 {
 	struct cpufreq_cooling_device *cpufreq_device = cdev->devdata;
-	struct cpumask *mask = &cpufreq_device->allowed_cpus;
-	unsigned int cpu;
-	unsigned int count = 0;
-	int ret;
-
-	cpu = cpumask_any(mask);
-
-	ret = get_property(cpu, 0, &count, GET_MAXL);
 
-	if (count > 0)
-		*state = count;
-
-	return ret;
+	*state = cpufreq_device->max_level;
+	return 0;
 }
 
 /**
@@ -403,8 +260,24 @@ static int cpufreq_set_cur_state(struct thermal_cooling_device *cdev,
 				 unsigned long state)
 {
 	struct cpufreq_cooling_device *cpufreq_device = cdev->devdata;
+	unsigned int cpu = cpumask_any(&cpufreq_device->allowed_cpus);
+	unsigned int clip_freq;
+
+	/* Request state should be less than max_level */
+	if (WARN_ON(state > cpufreq_device->max_level))
+		return -EINVAL;
+
+	/* Check if the old cooling action is same as new cooling action */
+	if (cpufreq_device->cpufreq_state == state)
+		return 0;
 
-	return cpufreq_apply_cooling(cpufreq_device, state);
+	clip_freq = cpufreq_device->freq_table[state];
+	cpufreq_device->cpufreq_state = state;
+	cpufreq_device->cpufreq_val = clip_freq;
+
+	cpufreq_update_policy(cpu);
+
+	return 0;
 }
 
 /* Bind cpufreq callbacks to thermal cooling device ops */
@@ -419,10 +292,25 @@ static struct notifier_block thermal_cpufreq_notifier_block = {
 	.notifier_call = cpufreq_thermal_notifier,
 };
 
+static unsigned int find_next_max(struct cpufreq_frequency_table *table,
+				  unsigned int prev_max)
+{
+	struct cpufreq_frequency_table *pos;
+	unsigned int max = 0;
+
+	cpufreq_for_each_valid_entry(pos, table) {
+		if (pos->frequency > max && pos->frequency < prev_max)
+			max = pos->frequency;
+	}
+
+	return max;
+}
+
 /**
  * __cpufreq_cooling_register - helper function to create cpufreq cooling device
  * @np: a valid struct device_node to the cooling device device tree node
  * @clip_cpus: cpumask of cpus where the frequency constraints will happen.
+ * Normally this should be same as cpufreq policy->related_cpus.
  *
  * This interface function registers the cpufreq cooling device with the name
  * "thermal-cpufreq-%x". This api can support multiple instances of cpufreq
@@ -437,37 +325,42 @@ __cpufreq_cooling_register(struct device_node *np,
 			   const struct cpumask *clip_cpus)
 {
 	struct thermal_cooling_device *cool_dev;
-	struct cpufreq_cooling_device *cpufreq_dev = NULL;
-	unsigned int min = 0, max = 0;
+	struct cpufreq_cooling_device *cpufreq_dev;
 	char dev_name[THERMAL_NAME_LENGTH];
-	int ret = 0, i;
-	struct cpufreq_policy policy;
+	struct cpufreq_frequency_table *pos, *table;
+	unsigned int freq, i;
+	int ret;
 
-	/* Verify that all the clip cpus have same freq_min, freq_max limit */
-	for_each_cpu(i, clip_cpus) {
-		/* continue if cpufreq policy not found and not return error */
-		if (!cpufreq_get_policy(&policy, i))
-			continue;
-		if (min == 0 && max == 0) {
-			min = policy.cpuinfo.min_freq;
-			max = policy.cpuinfo.max_freq;
-		} else {
-			if (min != policy.cpuinfo.min_freq ||
-			    max != policy.cpuinfo.max_freq)
-				return ERR_PTR(-EINVAL);
-		}
+	table = cpufreq_frequency_get_table(cpumask_first(clip_cpus));
+	if (!table) {
+		pr_debug("%s: CPUFreq table not found\n", __func__);
+		return ERR_PTR(-EPROBE_DEFER);
 	}
-	cpufreq_dev = kzalloc(sizeof(struct cpufreq_cooling_device),
-			      GFP_KERNEL);
+
+	cpufreq_dev = kzalloc(sizeof(*cpufreq_dev), GFP_KERNEL);
 	if (!cpufreq_dev)
 		return ERR_PTR(-ENOMEM);
 
+	/* Find max levels */
+	cpufreq_for_each_valid_entry(pos, table)
+		cpufreq_dev->max_level++;
+
+	cpufreq_dev->freq_table = kmalloc(sizeof(*cpufreq_dev->freq_table) *
+					  cpufreq_dev->max_level, GFP_KERNEL);
+	if (!cpufreq_dev->freq_table) {
+		cool_dev = ERR_PTR(-ENOMEM);
+		goto free_cdev;
+	}
+
+	/* max_level is an index, not a counter */
+	cpufreq_dev->max_level--;
+
 	cpumask_copy(&cpufreq_dev->allowed_cpus, clip_cpus);
 
 	ret = get_idr(&cpufreq_idr, &cpufreq_dev->id);
 	if (ret) {
-		kfree(cpufreq_dev);
-		return ERR_PTR(-EINVAL);
+		cool_dev = ERR_PTR(ret);
+		goto free_table;
 	}
 
 	snprintf(dev_name, sizeof(dev_name), "thermal-cpufreq-%d",
@@ -475,25 +368,44 @@ __cpufreq_cooling_register(struct device_node *np,
 
 	cool_dev = thermal_of_cooling_device_register(np, dev_name, cpufreq_dev,
 						      &cpufreq_cooling_ops);
-	if (IS_ERR(cool_dev)) {
-		release_idr(&cpufreq_idr, cpufreq_dev->id);
-		kfree(cpufreq_dev);
-		return cool_dev;
+	if (IS_ERR(cool_dev))
+		goto remove_idr;
+
+	/* Fill freq-table in descending order of frequencies */
+	for (i = 0, freq = -1; i <= cpufreq_dev->max_level; i++) {
+		freq = find_next_max(table, freq);
+		cpufreq_dev->freq_table[i] = freq;
+
+		/* Warn for duplicate entries */
+		if (!freq)
+			pr_warn("%s: table has duplicate entries\n", __func__);
+		else
+			pr_debug("%s: freq:%u KHz\n", __func__, freq);
 	}
+
+	cpufreq_dev->cpufreq_val = cpufreq_dev->freq_table[0];
 	cpufreq_dev->cool_dev = cool_dev;
-	cpufreq_dev->cpufreq_state = 0;
+
 	mutex_lock(&cooling_cpufreq_lock);
 
 	/* Register the notifier for first cpufreq cooling device */
-	if (cpufreq_dev_count == 0)
+	if (list_empty(&cpufreq_dev_list))
 		cpufreq_register_notifier(&thermal_cpufreq_notifier_block,
 					  CPUFREQ_POLICY_NOTIFIER);
-	cpufreq_dev_count++;
 	list_add(&cpufreq_dev->node, &cpufreq_dev_list);
 
 	mutex_unlock(&cooling_cpufreq_lock);
 
 	return cool_dev;
+
+remove_idr:
+	release_idr(&cpufreq_idr, cpufreq_dev->id);
+free_table:
+	kfree(cpufreq_dev->freq_table);
+free_cdev:
+	kfree(cpufreq_dev);
+
+	return cool_dev;
 }
 
 /**
@@ -554,16 +466,16 @@ void cpufreq_cooling_unregister(struct thermal_cooling_device *cdev)
 	cpufreq_dev = cdev->devdata;
 	mutex_lock(&cooling_cpufreq_lock);
 	list_del(&cpufreq_dev->node);
-	cpufreq_dev_count--;
 
 	/* Unregister the notifier for the last cpufreq cooling device */
-	if (cpufreq_dev_count == 0)
+	if (list_empty(&cpufreq_dev_list))
 		cpufreq_unregister_notifier(&thermal_cpufreq_notifier_block,
 					    CPUFREQ_POLICY_NOTIFIER);
 	mutex_unlock(&cooling_cpufreq_lock);
 
 	thermal_cooling_device_unregister(cpufreq_dev->cool_dev);
 	release_idr(&cpufreq_idr, cpufreq_dev->id);
+	kfree(cpufreq_dev->freq_table);
 	kfree(cpufreq_dev);
 }
 EXPORT_SYMBOL_GPL(cpufreq_cooling_unregister);
diff --git a/drivers/thermal/db8500_cpufreq_cooling.c b/drivers/thermal/db8500_cpufreq_cooling.c
index 786d19263ab0..607b62c7e611 100644
--- a/drivers/thermal/db8500_cpufreq_cooling.c
+++ b/drivers/thermal/db8500_cpufreq_cooling.c
@@ -18,7 +18,6 @@
  */
 
 #include <linux/cpu_cooling.h>
-#include <linux/cpufreq.h>
 #include <linux/err.h>
 #include <linux/module.h>
 #include <linux/of.h>
@@ -28,18 +27,17 @@
 static int db8500_cpufreq_cooling_probe(struct platform_device *pdev)
 {
 	struct thermal_cooling_device *cdev;
-	struct cpumask mask_val;
-
-	/* make sure cpufreq driver has been initialized */
-	if (!cpufreq_frequency_get_table(0))
-		return -EPROBE_DEFER;
-
-	cpumask_set_cpu(0, &mask_val);
-	cdev = cpufreq_cooling_register(&mask_val);
 
+	cdev = cpufreq_cooling_register(cpu_present_mask);
 	if (IS_ERR(cdev)) {
-		dev_err(&pdev->dev, "Failed to register cooling device\n");
-		return PTR_ERR(cdev);
+		int ret = PTR_ERR(cdev);
+
+		if (ret != -EPROBE_DEFER)
+			dev_err(&pdev->dev,
+				"Failed to register cooling device %d\n",
+				ret);
+				
+		return ret;
 	}
 
 	platform_set_drvdata(pdev, cdev);
@@ -78,7 +76,6 @@ static const struct of_device_id db8500_cpufreq_cooling_match[] = {
 
 static struct platform_driver db8500_cpufreq_cooling_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "db8500-cpufreq-cooling",
 		.of_match_table = of_match_ptr(db8500_cpufreq_cooling_match),
 	},
diff --git a/drivers/thermal/db8500_thermal.c b/drivers/thermal/db8500_thermal.c
index 1e3b3bf9f993..20adfbe27df1 100644
--- a/drivers/thermal/db8500_thermal.c
+++ b/drivers/thermal/db8500_thermal.c
@@ -517,7 +517,6 @@ static const struct of_device_id db8500_thermal_match[] = {
 
 static struct platform_driver db8500_thermal_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "db8500-thermal",
 		.of_match_table = of_match_ptr(db8500_thermal_match),
 	},
diff --git a/drivers/thermal/dove_thermal.c b/drivers/thermal/dove_thermal.c
index 828f5e345c30..09f6e304c274 100644
--- a/drivers/thermal/dove_thermal.c
+++ b/drivers/thermal/dove_thermal.c
@@ -184,7 +184,6 @@ static struct platform_driver dove_thermal_driver = {
 	.remove = dove_thermal_exit,
 	.driver = {
 		.name = "dove_thermal",
-		.owner = THIS_MODULE,
 		.of_match_table = dove_thermal_id_table,
 	},
 };
diff --git a/drivers/thermal/imx_thermal.c b/drivers/thermal/imx_thermal.c
index 5a1f1070b702..c1188ac053c9 100644
--- a/drivers/thermal/imx_thermal.c
+++ b/drivers/thermal/imx_thermal.c
@@ -9,7 +9,6 @@
 
 #include <linux/clk.h>
 #include <linux/cpu_cooling.h>
-#include <linux/cpufreq.h>
 #include <linux/delay.h>
 #include <linux/device.h>
 #include <linux/init.h>
@@ -454,15 +453,10 @@ static int imx_thermal_probe(struct platform_device *pdev)
 	const struct of_device_id *of_id =
 		of_match_device(of_imx_thermal_match, &pdev->dev);
 	struct imx_thermal_data *data;
-	struct cpumask clip_cpus;
 	struct regmap *map;
 	int measure_freq;
 	int ret;
 
-	if (!cpufreq_get_current_driver()) {
-		dev_dbg(&pdev->dev, "no cpufreq driver!");
-		return -EPROBE_DEFER;
-	}
 	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
@@ -516,12 +510,13 @@ static int imx_thermal_probe(struct platform_device *pdev)
 	regmap_write(map, MISC0 + REG_SET, MISC0_REFTOP_SELBIASOFF);
 	regmap_write(map, TEMPSENSE0 + REG_SET, TEMPSENSE0_POWER_DOWN);
 
-	cpumask_set_cpu(0, &clip_cpus);
-	data->cdev = cpufreq_cooling_register(&clip_cpus);
+	data->cdev = cpufreq_cooling_register(cpu_present_mask);
 	if (IS_ERR(data->cdev)) {
 		ret = PTR_ERR(data->cdev);
-		dev_err(&pdev->dev,
-			"failed to register cpufreq cooling device: %d\n", ret);
+		if (ret != -EPROBE_DEFER)
+			dev_err(&pdev->dev,
+				"failed to register cpufreq cooling device: %d\n",
+				ret);
 		return ret;
 	}
 
@@ -637,7 +632,6 @@ static SIMPLE_DEV_PM_OPS(imx_thermal_pm_ops,
 static struct platform_driver imx_thermal = {
 	.driver = {
 		.name	= "imx_thermal",
-		.owner  = THIS_MODULE,
 		.pm	= &imx_thermal_pm_ops,
 		.of_match_table = of_imx_thermal_match,
 	},
diff --git a/drivers/thermal/int340x_thermal/Makefile b/drivers/thermal/int340x_thermal/Makefile
index ffe40bffaf1a..d4413698a85f 100644
--- a/drivers/thermal/int340x_thermal/Makefile
+++ b/drivers/thermal/int340x_thermal/Makefile
@@ -1,4 +1,5 @@
 obj-$(CONFIG_INT340X_THERMAL)	+= int3400_thermal.o
 obj-$(CONFIG_INT340X_THERMAL)	+= int3402_thermal.o
 obj-$(CONFIG_INT340X_THERMAL)	+= int3403_thermal.o
+obj-$(CONFIG_INT340X_THERMAL)	+= processor_thermal_device.o
 obj-$(CONFIG_ACPI_THERMAL_REL)	+= acpi_thermal_rel.o
diff --git a/drivers/thermal/int340x_thermal/acpi_thermal_rel.c b/drivers/thermal/int340x_thermal/acpi_thermal_rel.c
index 0d8db808f0ae..231cabc16e16 100644
--- a/drivers/thermal/int340x_thermal/acpi_thermal_rel.c
+++ b/drivers/thermal/int340x_thermal/acpi_thermal_rel.c
@@ -82,7 +82,7 @@ int acpi_parse_trt(acpi_handle handle, int *trt_count, struct trt **trtp,
 	struct acpi_buffer trt_format = { sizeof("RRNNNNNN"), "RRNNNNNN" };
 
 	if (!acpi_has_method(handle, "_TRT"))
-		return 0;
+		return -ENODEV;
 
 	status = acpi_evaluate_object(handle, "_TRT", NULL, &buffer);
 	if (ACPI_FAILURE(status))
@@ -131,6 +131,8 @@ int acpi_parse_trt(acpi_handle handle, int *trt_count, struct trt **trtp,
 			pr_warn("Failed to get target ACPI device\n");
 	}
 
+	result = 0;
+
 	*trtp = trts;
 	/* don't count bad entries */
 	*trt_count -= nr_bad_entries;
@@ -165,7 +167,7 @@ int acpi_parse_art(acpi_handle handle, int *art_count, struct art **artp,
 		sizeof("RRNNNNNNNNNNN"), "RRNNNNNNNNNNN" };
 
 	if (!acpi_has_method(handle, "_ART"))
-		return 0;
+		return -ENODEV;
 
 	status = acpi_evaluate_object(handle, "_ART", NULL, &buffer);
 	if (ACPI_FAILURE(status))
@@ -317,21 +319,21 @@ static long acpi_thermal_rel_ioctl(struct file *f, unsigned int cmd,
 {
 	int ret = 0;
 	unsigned long length = 0;
-	unsigned long count = 0;
+	int count = 0;
 	char __user *arg = (void __user *)__arg;
-	struct trt *trts;
-	struct art *arts;
+	struct trt *trts = NULL;
+	struct art *arts = NULL;
 
 	switch (cmd) {
 	case ACPI_THERMAL_GET_TRT_COUNT:
-		ret = acpi_parse_trt(acpi_thermal_rel_handle, (int *)&count,
+		ret = acpi_parse_trt(acpi_thermal_rel_handle, &count,
 				&trts, false);
 		kfree(trts);
 		if (!ret)
 			return put_user(count, (unsigned long __user *)__arg);
 		return ret;
 	case ACPI_THERMAL_GET_TRT_LEN:
-		ret = acpi_parse_trt(acpi_thermal_rel_handle, (int *)&count,
+		ret = acpi_parse_trt(acpi_thermal_rel_handle, &count,
 				&trts, false);
 		kfree(trts);
 		length = count * sizeof(union trt_object);
@@ -341,14 +343,14 @@ static long acpi_thermal_rel_ioctl(struct file *f, unsigned int cmd,
 	case ACPI_THERMAL_GET_TRT:
 		return fill_trt(arg);
 	case ACPI_THERMAL_GET_ART_COUNT:
-		ret = acpi_parse_art(acpi_thermal_rel_handle, (int *)&count,
+		ret = acpi_parse_art(acpi_thermal_rel_handle, &count,
 				&arts, false);
 		kfree(arts);
 		if (!ret)
 			return put_user(count, (unsigned long __user *)__arg);
 		return ret;
 	case ACPI_THERMAL_GET_ART_LEN:
-		ret = acpi_parse_art(acpi_thermal_rel_handle, (int *)&count,
+		ret = acpi_parse_art(acpi_thermal_rel_handle, &count,
 				&arts, false);
 		kfree(arts);
 		length = count * sizeof(union art_object);
diff --git a/drivers/thermal/int340x_thermal/int3400_thermal.c b/drivers/thermal/int340x_thermal/int3400_thermal.c
index edc1cce117ba..65a98a97df07 100644
--- a/drivers/thermal/int340x_thermal/int3400_thermal.c
+++ b/drivers/thermal/int340x_thermal/int3400_thermal.c
@@ -43,6 +43,74 @@ struct int3400_thermal_priv {
 	struct trt *trts;
 	u8 uuid_bitmap;
 	int rel_misc_dev_res;
+	int current_uuid_index;
+};
+
+static ssize_t available_uuids_show(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct int3400_thermal_priv *priv = platform_get_drvdata(pdev);
+	int i;
+	int length = 0;
+
+	for (i = 0; i < INT3400_THERMAL_MAXIMUM_UUID; i++) {
+		if (priv->uuid_bitmap & (1 << i))
+			if (PAGE_SIZE - length > 0)
+				length += snprintf(&buf[length],
+						   PAGE_SIZE - length,
+						   "%s\n",
+						   int3400_thermal_uuids[i]);
+	}
+
+	return length;
+}
+
+static ssize_t current_uuid_show(struct device *dev,
+				 struct device_attribute *devattr, char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct int3400_thermal_priv *priv = platform_get_drvdata(pdev);
+
+	if (priv->uuid_bitmap & (1 << priv->current_uuid_index))
+		return sprintf(buf, "%s\n",
+			       int3400_thermal_uuids[priv->current_uuid_index]);
+	else
+		return sprintf(buf, "INVALID\n");
+}
+
+static ssize_t current_uuid_store(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct int3400_thermal_priv *priv = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < INT3400_THERMAL_MAXIMUM_UUID; ++i) {
+		if ((priv->uuid_bitmap & (1 << i)) &&
+		    !(strncmp(buf, int3400_thermal_uuids[i],
+			      sizeof(int3400_thermal_uuids[i]) - 1))) {
+			priv->current_uuid_index = i;
+			return count;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static DEVICE_ATTR(current_uuid, 0644, current_uuid_show, current_uuid_store);
+static DEVICE_ATTR_RO(available_uuids);
+static struct attribute *uuid_attrs[] = {
+	&dev_attr_available_uuids.attr,
+	&dev_attr_current_uuid.attr,
+	NULL
+};
+
+static struct attribute_group uuid_attribute_group = {
+	.attrs = uuid_attrs,
+	.name = "uuids"
 };
 
 static int int3400_thermal_get_uuids(struct int3400_thermal_priv *priv)
@@ -160,9 +228,9 @@ static int int3400_thermal_set_mode(struct thermal_zone_device *thermal,
 
 	if (enable != priv->mode) {
 		priv->mode = enable;
-		/* currently, only PASSIVE COOLING is supported */
 		result = int3400_thermal_run_osc(priv->adev->handle,
-					INT3400_THERMAL_PASSIVE_1, enable);
+						 priv->current_uuid_index,
+						 enable);
 	}
 	return result;
 }
@@ -223,7 +291,14 @@ static int int3400_thermal_probe(struct platform_device *pdev)
 	priv->rel_misc_dev_res = acpi_thermal_rel_misc_device_add(
 							priv->adev->handle);
 
+	result = sysfs_create_group(&pdev->dev.kobj, &uuid_attribute_group);
+	if (result)
+		goto free_zone;
+
 	return 0;
+
+free_zone:
+	thermal_zone_device_unregister(priv->thermal);
 free_trt:
 	kfree(priv->trts);
 free_art:
@@ -240,6 +315,7 @@ static int int3400_thermal_remove(struct platform_device *pdev)
 	if (!priv->rel_misc_dev_res)
 		acpi_thermal_rel_misc_device_remove(priv->adev->handle);
 
+	sysfs_remove_group(&pdev->dev.kobj, &uuid_attribute_group);
 	thermal_zone_device_unregister(priv->thermal);
 	kfree(priv->trts);
 	kfree(priv->arts);
@@ -259,7 +335,6 @@ static struct platform_driver int3400_thermal_driver = {
 	.remove = int3400_thermal_remove,
 	.driver = {
 		   .name = "int3400 thermal",
-		   .owner = THIS_MODULE,
 		   .acpi_match_table = ACPI_PTR(int3400_thermal_match),
 		   },
 };
diff --git a/drivers/thermal/int340x_thermal/int3402_thermal.c b/drivers/thermal/int340x_thermal/int3402_thermal.c
index a5d08c14ba24..c5cbc3af3a05 100644
--- a/drivers/thermal/int340x_thermal/int3402_thermal.c
+++ b/drivers/thermal/int340x_thermal/int3402_thermal.c
@@ -231,7 +231,6 @@ static struct platform_driver int3402_thermal_driver = {
 	.remove = int3402_thermal_remove,
 	.driver = {
 		   .name = "int3402 thermal",
-		   .owner = THIS_MODULE,
 		   .acpi_match_table = int3402_thermal_match,
 		   },
 };
diff --git a/drivers/thermal/int340x_thermal/int3403_thermal.c b/drivers/thermal/int340x_thermal/int3403_thermal.c
index 6e9fb62eb817..0faf500d8a77 100644
--- a/drivers/thermal/int340x_thermal/int3403_thermal.c
+++ b/drivers/thermal/int340x_thermal/int3403_thermal.c
@@ -293,8 +293,7 @@ static int int3403_sensor_add(struct int3403_priv *priv)
 	return 0;
 
  err_free_obj:
-	if (obj->tzone)
-		thermal_zone_device_unregister(obj->tzone);
+	thermal_zone_device_unregister(obj->tzone);
 	return result;
 }
 
@@ -302,6 +301,8 @@ static int int3403_sensor_remove(struct int3403_priv *priv)
 {
 	struct int3403_sensor *obj = priv->priv;
 
+	acpi_remove_notify_handler(priv->adev->handle,
+				   ACPI_DEVICE_NOTIFY, int3403_notify);
 	thermal_zone_device_unregister(obj->tzone);
 	return 0;
 }
@@ -370,6 +371,7 @@ static int int3403_cdev_add(struct int3403_priv *priv)
 	p = buf.pointer;
 	if (!p || (p->type != ACPI_TYPE_PACKAGE)) {
 		printk(KERN_WARNING "Invalid PPSS data\n");
+		kfree(buf.pointer);
 		return -EFAULT;
 	}
 
@@ -382,6 +384,7 @@ static int int3403_cdev_add(struct int3403_priv *priv)
 
 	priv->priv = obj;
 
+	kfree(buf.pointer);
 	/* TODO: add ACPI notification support */
 
 	return result;
@@ -471,7 +474,6 @@ static struct platform_driver int3403_driver = {
 	.remove = int3403_remove,
 	.driver = {
 		.name = "int3403 thermal",
-		.owner  = THIS_MODULE,
 		.acpi_match_table = int3403_device_ids,
 	},
 };
diff --git a/drivers/thermal/int340x_thermal/processor_thermal_device.c b/drivers/thermal/int340x_thermal/processor_thermal_device.c
new file mode 100644
index 000000000000..31bb553aac26
--- /dev/null
+++ b/drivers/thermal/int340x_thermal/processor_thermal_device.c
@@ -0,0 +1,309 @@
+/*
+ * processor_thermal_device.c
+ * Copyright (c) 2014, 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.
+ *
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/platform_device.h>
+#include <linux/acpi.h>
+
+/* Broadwell-U/HSB thermal reporting device */
+#define PCI_DEVICE_ID_PROC_BDW_THERMAL	0x1603
+#define PCI_DEVICE_ID_PROC_HSB_THERMAL	0x0A03
+
+/* Braswell thermal reporting device */
+#define PCI_DEVICE_ID_PROC_BSW_THERMAL	0x22DC
+
+struct power_config {
+	u32	index;
+	u32	min_uw;
+	u32	max_uw;
+	u32	tmin_us;
+	u32	tmax_us;
+	u32	step_uw;
+};
+
+struct proc_thermal_device {
+	struct device *dev;
+	struct acpi_device *adev;
+	struct power_config power_limits[2];
+};
+
+enum proc_thermal_emum_mode_type {
+	PROC_THERMAL_NONE,
+	PROC_THERMAL_PCI,
+	PROC_THERMAL_PLATFORM_DEV
+};
+
+/*
+ * We can have only one type of enumeration, PCI or Platform,
+ * not both. So we don't need instance specific data.
+ */
+static enum proc_thermal_emum_mode_type proc_thermal_emum_mode =
+							PROC_THERMAL_NONE;
+
+#define POWER_LIMIT_SHOW(index, suffix) \
+static ssize_t power_limit_##index##_##suffix##_show(struct device *dev, \
+					struct device_attribute *attr, \
+					char *buf) \
+{ \
+	struct pci_dev *pci_dev; \
+	struct platform_device *pdev; \
+	struct proc_thermal_device *proc_dev; \
+\
+	if (proc_thermal_emum_mode == PROC_THERMAL_PLATFORM_DEV) { \
+		pdev = to_platform_device(dev); \
+		proc_dev = platform_get_drvdata(pdev); \
+	} else { \
+		pci_dev = to_pci_dev(dev); \
+		proc_dev = pci_get_drvdata(pci_dev); \
+	} \
+	return sprintf(buf, "%lu\n",\
+	(unsigned long)proc_dev->power_limits[index].suffix * 1000); \
+}
+
+POWER_LIMIT_SHOW(0, min_uw)
+POWER_LIMIT_SHOW(0, max_uw)
+POWER_LIMIT_SHOW(0, step_uw)
+POWER_LIMIT_SHOW(0, tmin_us)
+POWER_LIMIT_SHOW(0, tmax_us)
+
+POWER_LIMIT_SHOW(1, min_uw)
+POWER_LIMIT_SHOW(1, max_uw)
+POWER_LIMIT_SHOW(1, step_uw)
+POWER_LIMIT_SHOW(1, tmin_us)
+POWER_LIMIT_SHOW(1, tmax_us)
+
+static DEVICE_ATTR_RO(power_limit_0_min_uw);
+static DEVICE_ATTR_RO(power_limit_0_max_uw);
+static DEVICE_ATTR_RO(power_limit_0_step_uw);
+static DEVICE_ATTR_RO(power_limit_0_tmin_us);
+static DEVICE_ATTR_RO(power_limit_0_tmax_us);
+
+static DEVICE_ATTR_RO(power_limit_1_min_uw);
+static DEVICE_ATTR_RO(power_limit_1_max_uw);
+static DEVICE_ATTR_RO(power_limit_1_step_uw);
+static DEVICE_ATTR_RO(power_limit_1_tmin_us);
+static DEVICE_ATTR_RO(power_limit_1_tmax_us);
+
+static struct attribute *power_limit_attrs[] = {
+	&dev_attr_power_limit_0_min_uw.attr,
+	&dev_attr_power_limit_1_min_uw.attr,
+	&dev_attr_power_limit_0_max_uw.attr,
+	&dev_attr_power_limit_1_max_uw.attr,
+	&dev_attr_power_limit_0_step_uw.attr,
+	&dev_attr_power_limit_1_step_uw.attr,
+	&dev_attr_power_limit_0_tmin_us.attr,
+	&dev_attr_power_limit_1_tmin_us.attr,
+	&dev_attr_power_limit_0_tmax_us.attr,
+	&dev_attr_power_limit_1_tmax_us.attr,
+	NULL
+};
+
+static struct attribute_group power_limit_attribute_group = {
+	.attrs = power_limit_attrs,
+	.name = "power_limits"
+};
+
+static int proc_thermal_add(struct device *dev,
+			    struct proc_thermal_device **priv)
+{
+	struct proc_thermal_device *proc_priv;
+	struct acpi_device *adev;
+	acpi_status status;
+	struct acpi_buffer buf = { ACPI_ALLOCATE_BUFFER, NULL };
+	union acpi_object *elements, *ppcc;
+	union acpi_object *p;
+	int i;
+	int ret;
+
+	adev = ACPI_COMPANION(dev);
+
+	status = acpi_evaluate_object(adev->handle, "PPCC", NULL, &buf);
+	if (ACPI_FAILURE(status))
+		return -ENODEV;
+
+	p = buf.pointer;
+	if (!p || (p->type != ACPI_TYPE_PACKAGE)) {
+		dev_err(dev, "Invalid PPCC data\n");
+		ret = -EFAULT;
+		goto free_buffer;
+	}
+	if (!p->package.count) {
+		dev_err(dev, "Invalid PPCC package size\n");
+		ret = -EFAULT;
+		goto free_buffer;
+	}
+
+	proc_priv = devm_kzalloc(dev, sizeof(*proc_priv), GFP_KERNEL);
+	if (!proc_priv) {
+		ret = -ENOMEM;
+		goto free_buffer;
+	}
+
+	proc_priv->dev = dev;
+	proc_priv->adev = adev;
+
+	for (i = 0; i < min((int)p->package.count - 1, 2); ++i) {
+		elements = &(p->package.elements[i+1]);
+		if (elements->type != ACPI_TYPE_PACKAGE ||
+		    elements->package.count != 6) {
+			ret = -EFAULT;
+			goto free_buffer;
+		}
+		ppcc = elements->package.elements;
+		proc_priv->power_limits[i].index = ppcc[0].integer.value;
+		proc_priv->power_limits[i].min_uw = ppcc[1].integer.value;
+		proc_priv->power_limits[i].max_uw = ppcc[2].integer.value;
+		proc_priv->power_limits[i].tmin_us = ppcc[3].integer.value;
+		proc_priv->power_limits[i].tmax_us = ppcc[4].integer.value;
+		proc_priv->power_limits[i].step_uw = ppcc[5].integer.value;
+	}
+
+	*priv = proc_priv;
+
+	ret = sysfs_create_group(&dev->kobj,
+				 &power_limit_attribute_group);
+
+free_buffer:
+	kfree(buf.pointer);
+
+	return ret;
+}
+
+void proc_thermal_remove(struct proc_thermal_device *proc_priv)
+{
+	sysfs_remove_group(&proc_priv->dev->kobj,
+			   &power_limit_attribute_group);
+}
+
+static int int3401_add(struct platform_device *pdev)
+{
+	struct proc_thermal_device *proc_priv;
+	int ret;
+
+	if (proc_thermal_emum_mode == PROC_THERMAL_PCI) {
+		dev_err(&pdev->dev, "error: enumerated as PCI dev\n");
+		return -ENODEV;
+	}
+
+	ret = proc_thermal_add(&pdev->dev, &proc_priv);
+	if (ret)
+		return ret;
+
+	platform_set_drvdata(pdev, proc_priv);
+	proc_thermal_emum_mode = PROC_THERMAL_PLATFORM_DEV;
+
+	return 0;
+}
+
+static int int3401_remove(struct platform_device *pdev)
+{
+	proc_thermal_remove(platform_get_drvdata(pdev));
+
+	return 0;
+}
+
+static int  proc_thermal_pci_probe(struct pci_dev *pdev,
+				   const struct pci_device_id *unused)
+{
+	struct proc_thermal_device *proc_priv;
+	int ret;
+
+	if (proc_thermal_emum_mode == PROC_THERMAL_PLATFORM_DEV) {
+		dev_err(&pdev->dev, "error: enumerated as platform dev\n");
+		return -ENODEV;
+	}
+
+	ret = pci_enable_device(pdev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "error: could not enable device\n");
+		return ret;
+	}
+
+	ret = proc_thermal_add(&pdev->dev, &proc_priv);
+	if (ret) {
+		pci_disable_device(pdev);
+		return ret;
+	}
+
+	pci_set_drvdata(pdev, proc_priv);
+	proc_thermal_emum_mode = PROC_THERMAL_PCI;
+
+	return 0;
+}
+
+static void  proc_thermal_pci_remove(struct pci_dev *pdev)
+{
+	proc_thermal_remove(pci_get_drvdata(pdev));
+	pci_disable_device(pdev);
+}
+
+static const struct pci_device_id proc_thermal_pci_ids[] = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_PROC_BDW_THERMAL)},
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_PROC_HSB_THERMAL)},
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_PROC_BSW_THERMAL)},
+	{ 0, },
+};
+
+MODULE_DEVICE_TABLE(pci, proc_thermal_pci_ids);
+
+static struct pci_driver proc_thermal_pci_driver = {
+	.name		= "proc_thermal",
+	.probe		= proc_thermal_pci_probe,
+	.remove		= proc_thermal_pci_remove,
+	.id_table	= proc_thermal_pci_ids,
+};
+
+static const struct acpi_device_id int3401_device_ids[] = {
+	{"INT3401", 0},
+	{"", 0},
+};
+MODULE_DEVICE_TABLE(acpi, int3401_device_ids);
+
+static struct platform_driver int3401_driver = {
+	.probe = int3401_add,
+	.remove = int3401_remove,
+	.driver = {
+		.name = "int3401 thermal",
+		.acpi_match_table = int3401_device_ids,
+	},
+};
+
+static int __init proc_thermal_init(void)
+{
+	int ret;
+
+	ret = platform_driver_register(&int3401_driver);
+	if (ret)
+		return ret;
+
+	ret = pci_register_driver(&proc_thermal_pci_driver);
+
+	return ret;
+}
+
+static void __exit proc_thermal_exit(void)
+{
+	platform_driver_unregister(&int3401_driver);
+	pci_unregister_driver(&proc_thermal_pci_driver);
+}
+
+module_init(proc_thermal_init);
+module_exit(proc_thermal_exit);
+
+MODULE_AUTHOR("Srinivas Pandruvada <srinivas.pandruvada@linux.intel.com>");
+MODULE_DESCRIPTION("Processor Thermal Reporting Device Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/thermal/intel_powerclamp.c b/drivers/thermal/intel_powerclamp.c
index 95cb7fc20e17..6ceebd659dd4 100644
--- a/drivers/thermal/intel_powerclamp.c
+++ b/drivers/thermal/intel_powerclamp.c
@@ -435,7 +435,6 @@ static int clamp_thread(void *arg)
 		 * allowed. thus jiffies are updated properly.
 		 */
 		preempt_disable();
-		tick_nohz_idle_enter();
 		/* mwait until target jiffies is reached */
 		while (time_before(jiffies, target_jiffies)) {
 			unsigned long ecx = 1;
@@ -451,7 +450,6 @@ static int clamp_thread(void *arg)
 			start_critical_timings();
 			atomic_inc(&idle_wakeup_counter);
 		}
-		tick_nohz_idle_exit();
 		preempt_enable();
 	}
 	del_timer_sync(&wakeup_timer);
@@ -689,6 +687,8 @@ static const struct x86_cpu_id intel_powerclamp_ids[] = {
 	{ X86_VENDOR_INTEL, 6, 0x3f},
 	{ X86_VENDOR_INTEL, 6, 0x45},
 	{ X86_VENDOR_INTEL, 6, 0x46},
+	{ X86_VENDOR_INTEL, 6, 0x4c},
+	{ X86_VENDOR_INTEL, 6, 0x56},
 	{}
 };
 MODULE_DEVICE_TABLE(x86cpu, intel_powerclamp_ids);
diff --git a/drivers/thermal/intel_soc_dts_thermal.c b/drivers/thermal/intel_soc_dts_thermal.c
index a6a0a18ec0aa..5580f5b24eb9 100644
--- a/drivers/thermal/intel_soc_dts_thermal.c
+++ b/drivers/thermal/intel_soc_dts_thermal.c
@@ -360,6 +360,9 @@ static void proc_thermal_interrupt(void)
 	u32 sticky_out;
 	int status;
 	u32 ptmc_out;
+	unsigned long flags;
+
+	spin_lock_irqsave(&intr_notify_lock, flags);
 
 	/* Clear APIC interrupt */
 	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
@@ -378,21 +381,20 @@ static void proc_thermal_interrupt(void)
 		/* reset sticky bit */
 		status = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
 					SOC_DTS_OFFSET_PTTSS, sticky_out);
+		spin_unlock_irqrestore(&intr_notify_lock, flags);
+
 		for (i = 0; i < SOC_MAX_DTS_SENSORS; ++i) {
 			pr_debug("TZD update for zone %d\n", i);
 			thermal_zone_device_update(soc_dts[i]->tzone);
 		}
-	}
+	} else
+		spin_unlock_irqrestore(&intr_notify_lock, flags);
 
 }
 
 static irqreturn_t soc_irq_thread_fn(int irq, void *dev_data)
 {
-	unsigned long flags;
-
-	spin_lock_irqsave(&intr_notify_lock, flags);
 	proc_thermal_interrupt();
-	spin_unlock_irqrestore(&intr_notify_lock, flags);
 	pr_debug("proc_thermal_interrupt\n");
 
 	return IRQ_HANDLED;
diff --git a/drivers/thermal/kirkwood_thermal.c b/drivers/thermal/kirkwood_thermal.c
index 3b034a0dfc94..11041fe63dc2 100644
--- a/drivers/thermal/kirkwood_thermal.c
+++ b/drivers/thermal/kirkwood_thermal.c
@@ -114,7 +114,6 @@ static struct platform_driver kirkwood_thermal_driver = {
 	.remove = kirkwood_thermal_exit,
 	.driver = {
 		.name = "kirkwood_thermal",
-		.owner = THIS_MODULE,
 		.of_match_table = kirkwood_thermal_id_table,
 	},
 };
diff --git a/drivers/thermal/of-thermal.c b/drivers/thermal/of-thermal.c
index 62143ba31001..e145b66df444 100644
--- a/drivers/thermal/of-thermal.c
+++ b/drivers/thermal/of-thermal.c
@@ -30,27 +30,13 @@
 #include <linux/err.h>
 #include <linux/export.h>
 #include <linux/string.h>
+#include <linux/thermal.h>
 
 #include "thermal_core.h"
 
 /***   Private data structures to represent thermal device tree data ***/
 
 /**
- * struct __thermal_trip - representation of a point in temperature domain
- * @np: pointer to struct device_node that this trip point was created from
- * @temperature: temperature value in miliCelsius
- * @hysteresis: relative hysteresis in miliCelsius
- * @type: trip point type
- */
-
-struct __thermal_trip {
-	struct device_node *np;
-	unsigned long int temperature;
-	unsigned long int hysteresis;
-	enum thermal_trip_type type;
-};
-
-/**
  * struct __thermal_bind_param - a match between trip and cooling device
  * @cooling_device: a pointer to identify the referred cooling device
  * @trip_id: the trip point index
@@ -77,8 +63,7 @@ struct __thermal_bind_params {
  * @num_tbps: number of thermal bind params
  * @tbps: an array of thermal bind params (0..num_tbps - 1)
  * @sensor_data: sensor private data used while reading temperature and trend
- * @get_temp: sensor callback to read temperature
- * @get_trend: sensor callback to read temperature trend
+ * @ops: set of callbacks to handle the thermal zone based on DT
  */
 
 struct __thermal_zone {
@@ -88,7 +73,7 @@ struct __thermal_zone {
 
 	/* trip data */
 	int ntrips;
-	struct __thermal_trip *trips;
+	struct thermal_trip *trips;
 
 	/* cooling binding data */
 	int num_tbps;
@@ -96,8 +81,7 @@ struct __thermal_zone {
 
 	/* sensor interface */
 	void *sensor_data;
-	int (*get_temp)(void *, long *);
-	int (*get_trend)(void *, long *);
+	const struct thermal_zone_of_device_ops *ops;
 };
 
 /***   DT thermal zone device callbacks   ***/
@@ -107,10 +91,96 @@ static int of_thermal_get_temp(struct thermal_zone_device *tz,
 {
 	struct __thermal_zone *data = tz->devdata;
 
-	if (!data->get_temp)
+	if (!data->ops->get_temp)
 		return -EINVAL;
 
-	return data->get_temp(data->sensor_data, temp);
+	return data->ops->get_temp(data->sensor_data, temp);
+}
+
+/**
+ * of_thermal_get_ntrips - function to export number of available trip
+ *			   points.
+ * @tz: pointer to a thermal zone
+ *
+ * This function is a globally visible wrapper to get number of trip points
+ * stored in the local struct __thermal_zone
+ *
+ * Return: number of available trip points, -ENODEV when data not available
+ */
+int of_thermal_get_ntrips(struct thermal_zone_device *tz)
+{
+	struct __thermal_zone *data = tz->devdata;
+
+	if (!data || IS_ERR(data))
+		return -ENODEV;
+
+	return data->ntrips;
+}
+EXPORT_SYMBOL_GPL(of_thermal_get_ntrips);
+
+/**
+ * of_thermal_is_trip_valid - function to check if trip point is valid
+ *
+ * @tz:	pointer to a thermal zone
+ * @trip:	trip point to evaluate
+ *
+ * This function is responsible for checking if passed trip point is valid
+ *
+ * Return: true if trip point is valid, false otherwise
+ */
+bool of_thermal_is_trip_valid(struct thermal_zone_device *tz, int trip)
+{
+	struct __thermal_zone *data = tz->devdata;
+
+	if (!data || trip >= data->ntrips || trip < 0)
+		return false;
+
+	return true;
+}
+EXPORT_SYMBOL_GPL(of_thermal_is_trip_valid);
+
+/**
+ * of_thermal_get_trip_points - function to get access to a globally exported
+ *				trip points
+ *
+ * @tz:	pointer to a thermal zone
+ *
+ * This function provides a pointer to trip points table
+ *
+ * Return: pointer to trip points table, NULL otherwise
+ */
+const struct thermal_trip * const
+of_thermal_get_trip_points(struct thermal_zone_device *tz)
+{
+	struct __thermal_zone *data = tz->devdata;
+
+	if (!data)
+		return NULL;
+
+	return data->trips;
+}
+EXPORT_SYMBOL_GPL(of_thermal_get_trip_points);
+
+/**
+ * of_thermal_set_emul_temp - function to set emulated temperature
+ *
+ * @tz:	pointer to a thermal zone
+ * @temp:	temperature to set
+ *
+ * This function gives the ability to set emulated value of temperature,
+ * which is handy for debugging
+ *
+ * Return: zero on success, error code otherwise
+ */
+static int of_thermal_set_emul_temp(struct thermal_zone_device *tz,
+				    unsigned long temp)
+{
+	struct __thermal_zone *data = tz->devdata;
+
+	if (!data->ops || !data->ops->set_emul_temp)
+		return -EINVAL;
+
+	return data->ops->set_emul_temp(data->sensor_data, temp);
 }
 
 static int of_thermal_get_trend(struct thermal_zone_device *tz, int trip,
@@ -120,10 +190,10 @@ static int of_thermal_get_trend(struct thermal_zone_device *tz, int trip,
 	long dev_trend;
 	int r;
 
-	if (!data->get_trend)
+	if (!data->ops->get_trend)
 		return -EINVAL;
 
-	r = data->get_trend(data->sensor_data, &dev_trend);
+	r = data->ops->get_trend(data->sensor_data, &dev_trend);
 	if (r)
 		return r;
 
@@ -324,8 +394,7 @@ static struct thermal_zone_device_ops of_thermal_ops = {
 static struct thermal_zone_device *
 thermal_zone_of_add_sensor(struct device_node *zone,
 			   struct device_node *sensor, void *data,
-			   int (*get_temp)(void *, long *),
-			   int (*get_trend)(void *, long *))
+			   const struct thermal_zone_of_device_ops *ops)
 {
 	struct thermal_zone_device *tzd;
 	struct __thermal_zone *tz;
@@ -336,13 +405,16 @@ thermal_zone_of_add_sensor(struct device_node *zone,
 
 	tz = tzd->devdata;
 
+	if (!ops)
+		return ERR_PTR(-EINVAL);
+
 	mutex_lock(&tzd->lock);
-	tz->get_temp = get_temp;
-	tz->get_trend = get_trend;
+	tz->ops = ops;
 	tz->sensor_data = data;
 
 	tzd->ops->get_temp = of_thermal_get_temp;
 	tzd->ops->get_trend = of_thermal_get_trend;
+	tzd->ops->set_emul_temp = of_thermal_set_emul_temp;
 	mutex_unlock(&tzd->lock);
 
 	return tzd;
@@ -356,8 +428,7 @@ thermal_zone_of_add_sensor(struct device_node *zone,
  *             than one sensors
  * @data: a private pointer (owned by the caller) that will be passed
  *        back, when a temperature reading is needed.
- * @get_temp: a pointer to a function that reads the sensor temperature.
- * @get_trend: a pointer to a function that reads the sensor temperature trend.
+ * @ops: struct thermal_zone_of_device_ops *. Must contain at least .get_temp.
  *
  * This function will search the list of thermal zones described in device
  * tree and look for the zone that refer to the sensor device pointed by
@@ -382,9 +453,8 @@ thermal_zone_of_add_sensor(struct device_node *zone,
  * check the return value with help of IS_ERR() helper.
  */
 struct thermal_zone_device *
-thermal_zone_of_sensor_register(struct device *dev, int sensor_id,
-				void *data, int (*get_temp)(void *, long *),
-				int (*get_trend)(void *, long *))
+thermal_zone_of_sensor_register(struct device *dev, int sensor_id, void *data,
+				const struct thermal_zone_of_device_ops *ops)
 {
 	struct device_node *np, *child, *sensor_np;
 	struct thermal_zone_device *tzd = ERR_PTR(-ENODEV);
@@ -426,9 +496,7 @@ thermal_zone_of_sensor_register(struct device *dev, int sensor_id,
 
 		if (sensor_specs.np == sensor_np && id == sensor_id) {
 			tzd = thermal_zone_of_add_sensor(child, sensor_np,
-							 data,
-							 get_temp,
-							 get_trend);
+							 data, ops);
 			of_node_put(sensor_specs.np);
 			of_node_put(child);
 			goto exit;
@@ -475,9 +543,9 @@ void thermal_zone_of_sensor_unregister(struct device *dev,
 	mutex_lock(&tzd->lock);
 	tzd->ops->get_temp = NULL;
 	tzd->ops->get_trend = NULL;
+	tzd->ops->set_emul_temp = NULL;
 
-	tz->get_temp = NULL;
-	tz->get_trend = NULL;
+	tz->ops = NULL;
 	tz->sensor_data = NULL;
 	mutex_unlock(&tzd->lock);
 }
@@ -501,7 +569,7 @@ EXPORT_SYMBOL_GPL(thermal_zone_of_sensor_unregister);
  */
 static int thermal_of_populate_bind_params(struct device_node *np,
 					   struct __thermal_bind_params *__tbp,
-					   struct __thermal_trip *trips,
+					   struct thermal_trip *trips,
 					   int ntrips)
 {
 	struct of_phandle_args cooling_spec;
@@ -604,7 +672,7 @@ static int thermal_of_get_trip_type(struct device_node *np,
  * Return: 0 on success, proper error code otherwise
  */
 static int thermal_of_populate_trip(struct device_node *np,
-				    struct __thermal_trip *trip)
+				    struct thermal_trip *trip)
 {
 	int prop;
 	int ret;
diff --git a/drivers/thermal/rockchip_thermal.c b/drivers/thermal/rockchip_thermal.c
new file mode 100644
index 000000000000..9c6ce548e363
--- /dev/null
+++ b/drivers/thermal/rockchip_thermal.c
@@ -0,0 +1,692 @@
+/*
+ * Copyright (c) 2014, Fuzhou Rockchip Electronics Co., 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.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/platform_device.h>
+#include <linux/reset.h>
+#include <linux/thermal.h>
+
+/**
+ * If the temperature over a period of time High,
+ * the resulting TSHUT gave CRU module,let it reset the entire chip,
+ * or via GPIO give PMIC.
+ */
+enum tshut_mode {
+	TSHUT_MODE_CRU = 0,
+	TSHUT_MODE_GPIO,
+};
+
+/**
+ * the system Temperature Sensors tshut(tshut) polarity
+ * the bit 8 is tshut polarity.
+ * 0: low active, 1: high active
+ */
+enum tshut_polarity {
+	TSHUT_LOW_ACTIVE = 0,
+	TSHUT_HIGH_ACTIVE,
+};
+
+/**
+ * The system has three Temperature Sensors.  channel 0 is reserved,
+ * channel 1 is for CPU, and channel 2 is for GPU.
+ */
+enum sensor_id {
+	SENSOR_CPU = 1,
+	SENSOR_GPU,
+};
+
+struct rockchip_tsadc_chip {
+	/* The hardware-controlled tshut property */
+	long tshut_temp;
+	enum tshut_mode tshut_mode;
+	enum tshut_polarity tshut_polarity;
+
+	/* Chip-wide methods */
+	void (*initialize)(void __iomem *reg, enum tshut_polarity p);
+	void (*irq_ack)(void __iomem *reg);
+	void (*control)(void __iomem *reg, bool on);
+
+	/* Per-sensor methods */
+	int (*get_temp)(int chn, void __iomem *reg, long *temp);
+	void (*set_tshut_temp)(int chn, void __iomem *reg, long temp);
+	void (*set_tshut_mode)(int chn, void __iomem *reg, enum tshut_mode m);
+};
+
+struct rockchip_thermal_sensor {
+	struct rockchip_thermal_data *thermal;
+	struct thermal_zone_device *tzd;
+	enum sensor_id id;
+};
+
+#define NUM_SENSORS	2 /* Ignore unused sensor 0 */
+
+struct rockchip_thermal_data {
+	const struct rockchip_tsadc_chip *chip;
+	struct platform_device *pdev;
+	struct reset_control *reset;
+
+	struct rockchip_thermal_sensor sensors[NUM_SENSORS];
+
+	struct clk *clk;
+	struct clk *pclk;
+
+	void __iomem *regs;
+
+	long tshut_temp;
+	enum tshut_mode tshut_mode;
+	enum tshut_polarity tshut_polarity;
+};
+
+/* TSADC V2 Sensor info define: */
+#define TSADCV2_AUTO_CON			0x04
+#define TSADCV2_INT_EN				0x08
+#define TSADCV2_INT_PD				0x0c
+#define TSADCV2_DATA(chn)			(0x20 + (chn) * 0x04)
+#define TSADCV2_COMP_SHUT(chn)		        (0x40 + (chn) * 0x04)
+#define TSADCV2_HIGHT_INT_DEBOUNCE		0x60
+#define TSADCV2_HIGHT_TSHUT_DEBOUNCE		0x64
+#define TSADCV2_AUTO_PERIOD			0x68
+#define TSADCV2_AUTO_PERIOD_HT			0x6c
+
+#define TSADCV2_AUTO_EN				BIT(0)
+#define TSADCV2_AUTO_DISABLE			~BIT(0)
+#define TSADCV2_AUTO_SRC_EN(chn)		BIT(4 + (chn))
+#define TSADCV2_AUTO_TSHUT_POLARITY_HIGH	BIT(8)
+#define TSADCV2_AUTO_TSHUT_POLARITY_LOW		~BIT(8)
+
+#define TSADCV2_INT_SRC_EN(chn)			BIT(chn)
+#define TSADCV2_SHUT_2GPIO_SRC_EN(chn)		BIT(4 + (chn))
+#define TSADCV2_SHUT_2CRU_SRC_EN(chn)		BIT(8 + (chn))
+
+#define TSADCV2_INT_PD_CLEAR			~BIT(8)
+
+#define TSADCV2_DATA_MASK			0xfff
+#define TSADCV2_HIGHT_INT_DEBOUNCE_COUNT	4
+#define TSADCV2_HIGHT_TSHUT_DEBOUNCE_COUNT	4
+#define TSADCV2_AUTO_PERIOD_TIME		250 /* msec */
+#define TSADCV2_AUTO_PERIOD_HT_TIME		50  /* msec */
+
+struct tsadc_table {
+	unsigned long code;
+	long temp;
+};
+
+static const struct tsadc_table v2_code_table[] = {
+	{TSADCV2_DATA_MASK, -40000},
+	{3800, -40000},
+	{3792, -35000},
+	{3783, -30000},
+	{3774, -25000},
+	{3765, -20000},
+	{3756, -15000},
+	{3747, -10000},
+	{3737, -5000},
+	{3728, 0},
+	{3718, 5000},
+	{3708, 10000},
+	{3698, 15000},
+	{3688, 20000},
+	{3678, 25000},
+	{3667, 30000},
+	{3656, 35000},
+	{3645, 40000},
+	{3634, 45000},
+	{3623, 50000},
+	{3611, 55000},
+	{3600, 60000},
+	{3588, 65000},
+	{3575, 70000},
+	{3563, 75000},
+	{3550, 80000},
+	{3537, 85000},
+	{3524, 90000},
+	{3510, 95000},
+	{3496, 100000},
+	{3482, 105000},
+	{3467, 110000},
+	{3452, 115000},
+	{3437, 120000},
+	{3421, 125000},
+	{0, 125000},
+};
+
+static u32 rk_tsadcv2_temp_to_code(long temp)
+{
+	int high, low, mid;
+
+	low = 0;
+	high = ARRAY_SIZE(v2_code_table) - 1;
+	mid = (high + low) / 2;
+
+	if (temp < v2_code_table[low].temp || temp > v2_code_table[high].temp)
+		return 0;
+
+	while (low <= high) {
+		if (temp == v2_code_table[mid].temp)
+			return v2_code_table[mid].code;
+		else if (temp < v2_code_table[mid].temp)
+			high = mid - 1;
+		else
+			low = mid + 1;
+		mid = (low + high) / 2;
+	}
+
+	return 0;
+}
+
+static long rk_tsadcv2_code_to_temp(u32 code)
+{
+	int high, low, mid;
+
+	low = 0;
+	high = ARRAY_SIZE(v2_code_table) - 1;
+	mid = (high + low) / 2;
+
+	if (code > v2_code_table[low].code || code < v2_code_table[high].code)
+		return 125000; /* No code available, return max temperature */
+
+	while (low <= high) {
+		if (code >= v2_code_table[mid].code && code <
+		    v2_code_table[mid - 1].code)
+			return v2_code_table[mid].temp;
+		else if (code < v2_code_table[mid].code)
+			low = mid + 1;
+		else
+			high = mid - 1;
+		mid = (low + high) / 2;
+	}
+
+	return 125000;
+}
+
+/**
+ * rk_tsadcv2_initialize - initialize TASDC Controller
+ * (1) Set TSADCV2_AUTO_PERIOD, configure the interleave between
+ * every two accessing of TSADC in normal operation.
+ * (2) Set TSADCV2_AUTO_PERIOD_HT, configure the interleave between
+ * every two accessing of TSADC after the temperature is higher
+ * than COM_SHUT or COM_INT.
+ * (3) Set TSADCV2_HIGH_INT_DEBOUNCE and TSADC_HIGHT_TSHUT_DEBOUNCE,
+ * if the temperature is higher than COMP_INT or COMP_SHUT for
+ * "debounce" times, TSADC controller will generate interrupt or TSHUT.
+ */
+static void rk_tsadcv2_initialize(void __iomem *regs,
+				  enum tshut_polarity tshut_polarity)
+{
+	if (tshut_polarity == TSHUT_HIGH_ACTIVE)
+		writel_relaxed(0 | (TSADCV2_AUTO_TSHUT_POLARITY_HIGH),
+			       regs + TSADCV2_AUTO_CON);
+	else
+		writel_relaxed(0 | (TSADCV2_AUTO_TSHUT_POLARITY_LOW),
+			       regs + TSADCV2_AUTO_CON);
+
+	writel_relaxed(TSADCV2_AUTO_PERIOD_TIME, regs + TSADCV2_AUTO_PERIOD);
+	writel_relaxed(TSADCV2_HIGHT_INT_DEBOUNCE_COUNT,
+		       regs + TSADCV2_HIGHT_INT_DEBOUNCE);
+	writel_relaxed(TSADCV2_AUTO_PERIOD_HT_TIME,
+		       regs + TSADCV2_AUTO_PERIOD_HT);
+	writel_relaxed(TSADCV2_HIGHT_TSHUT_DEBOUNCE_COUNT,
+		       regs + TSADCV2_HIGHT_TSHUT_DEBOUNCE);
+}
+
+static void rk_tsadcv2_irq_ack(void __iomem *regs)
+{
+	u32 val;
+
+	val = readl_relaxed(regs + TSADCV2_INT_PD);
+	writel_relaxed(val & TSADCV2_INT_PD_CLEAR, regs + TSADCV2_INT_PD);
+}
+
+static void rk_tsadcv2_control(void __iomem *regs, bool enable)
+{
+	u32 val;
+
+	val = readl_relaxed(regs + TSADCV2_AUTO_CON);
+	if (enable)
+		val |= TSADCV2_AUTO_EN;
+	else
+		val &= ~TSADCV2_AUTO_EN;
+
+	writel_relaxed(val, regs + TSADCV2_AUTO_CON);
+}
+
+static int rk_tsadcv2_get_temp(int chn, void __iomem *regs, long *temp)
+{
+	u32 val;
+
+	/* the A/D value of the channel last conversion need some time */
+	val = readl_relaxed(regs + TSADCV2_DATA(chn));
+	if (val == 0)
+		return -EAGAIN;
+
+	*temp = rk_tsadcv2_code_to_temp(val);
+
+	return 0;
+}
+
+static void rk_tsadcv2_tshut_temp(int chn, void __iomem *regs, long temp)
+{
+	u32 tshut_value, val;
+
+	tshut_value = rk_tsadcv2_temp_to_code(temp);
+	writel_relaxed(tshut_value, regs + TSADCV2_COMP_SHUT(chn));
+
+	/* TSHUT will be valid */
+	val = readl_relaxed(regs + TSADCV2_AUTO_CON);
+	writel_relaxed(val | TSADCV2_AUTO_SRC_EN(chn), regs + TSADCV2_AUTO_CON);
+}
+
+static void rk_tsadcv2_tshut_mode(int chn, void __iomem *regs,
+				  enum tshut_mode mode)
+{
+	u32 val;
+
+	val = readl_relaxed(regs + TSADCV2_INT_EN);
+	if (mode == TSHUT_MODE_GPIO) {
+		val &= ~TSADCV2_SHUT_2CRU_SRC_EN(chn);
+		val |= TSADCV2_SHUT_2GPIO_SRC_EN(chn);
+	} else {
+		val &= ~TSADCV2_SHUT_2GPIO_SRC_EN(chn);
+		val |= TSADCV2_SHUT_2CRU_SRC_EN(chn);
+	}
+
+	writel_relaxed(val, regs + TSADCV2_INT_EN);
+}
+
+static const struct rockchip_tsadc_chip rk3288_tsadc_data = {
+	.tshut_mode = TSHUT_MODE_GPIO, /* default TSHUT via GPIO give PMIC */
+	.tshut_polarity = TSHUT_LOW_ACTIVE, /* default TSHUT LOW ACTIVE */
+	.tshut_temp = 95000,
+
+	.initialize = rk_tsadcv2_initialize,
+	.irq_ack = rk_tsadcv2_irq_ack,
+	.control = rk_tsadcv2_control,
+	.get_temp = rk_tsadcv2_get_temp,
+	.set_tshut_temp = rk_tsadcv2_tshut_temp,
+	.set_tshut_mode = rk_tsadcv2_tshut_mode,
+};
+
+static const struct of_device_id of_rockchip_thermal_match[] = {
+	{
+		.compatible = "rockchip,rk3288-tsadc",
+		.data = (void *)&rk3288_tsadc_data,
+	},
+	{ /* end */ },
+};
+MODULE_DEVICE_TABLE(of, of_rockchip_thermal_match);
+
+static void
+rockchip_thermal_toggle_sensor(struct rockchip_thermal_sensor *sensor, bool on)
+{
+	struct thermal_zone_device *tzd = sensor->tzd;
+
+	tzd->ops->set_mode(tzd,
+		on ? THERMAL_DEVICE_ENABLED : THERMAL_DEVICE_DISABLED);
+}
+
+static irqreturn_t rockchip_thermal_alarm_irq_thread(int irq, void *dev)
+{
+	struct rockchip_thermal_data *thermal = dev;
+	int i;
+
+	dev_dbg(&thermal->pdev->dev, "thermal alarm\n");
+
+	thermal->chip->irq_ack(thermal->regs);
+
+	for (i = 0; i < ARRAY_SIZE(thermal->sensors); i++)
+		thermal_zone_device_update(thermal->sensors[i].tzd);
+
+	return IRQ_HANDLED;
+}
+
+static int rockchip_thermal_get_temp(void *_sensor, long *out_temp)
+{
+	struct rockchip_thermal_sensor *sensor = _sensor;
+	struct rockchip_thermal_data *thermal = sensor->thermal;
+	const struct rockchip_tsadc_chip *tsadc = sensor->thermal->chip;
+	int retval;
+
+	retval = tsadc->get_temp(sensor->id, thermal->regs, out_temp);
+	dev_dbg(&thermal->pdev->dev, "sensor %d - temp: %ld, retval: %d\n",
+		sensor->id, *out_temp, retval);
+
+	return retval;
+}
+
+static const struct thermal_zone_of_device_ops rockchip_of_thermal_ops = {
+	.get_temp = rockchip_thermal_get_temp,
+};
+
+static int rockchip_configure_from_dt(struct device *dev,
+				      struct device_node *np,
+				      struct rockchip_thermal_data *thermal)
+{
+	u32 shut_temp, tshut_mode, tshut_polarity;
+
+	if (of_property_read_u32(np, "rockchip,hw-tshut-temp", &shut_temp)) {
+		dev_warn(dev,
+			 "Missing tshut temp property, using default %ld\n",
+			 thermal->chip->tshut_temp);
+		thermal->tshut_temp = thermal->chip->tshut_temp;
+	} else {
+		thermal->tshut_temp = shut_temp;
+	}
+
+	if (thermal->tshut_temp > INT_MAX) {
+		dev_err(dev, "Invalid tshut temperature specified: %ld\n",
+			thermal->tshut_temp);
+		return -ERANGE;
+	}
+
+	if (of_property_read_u32(np, "rockchip,hw-tshut-mode", &tshut_mode)) {
+		dev_warn(dev,
+			 "Missing tshut mode property, using default (%s)\n",
+			 thermal->chip->tshut_mode == TSHUT_MODE_GPIO ?
+				"gpio" : "cru");
+		thermal->tshut_mode = thermal->chip->tshut_mode;
+	} else {
+		thermal->tshut_mode = tshut_mode;
+	}
+
+	if (thermal->tshut_mode > 1) {
+		dev_err(dev, "Invalid tshut mode specified: %d\n",
+			thermal->tshut_mode);
+		return -EINVAL;
+	}
+
+	if (of_property_read_u32(np, "rockchip,hw-tshut-polarity",
+				 &tshut_polarity)) {
+		dev_warn(dev,
+			 "Missing tshut-polarity property, using default (%s)\n",
+			 thermal->chip->tshut_polarity == TSHUT_LOW_ACTIVE ?
+				"low" : "high");
+		thermal->tshut_polarity = thermal->chip->tshut_polarity;
+	} else {
+		thermal->tshut_polarity = tshut_polarity;
+	}
+
+	if (thermal->tshut_polarity > 1) {
+		dev_err(dev, "Invalid tshut-polarity specified: %d\n",
+			thermal->tshut_polarity);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int
+rockchip_thermal_register_sensor(struct platform_device *pdev,
+				 struct rockchip_thermal_data *thermal,
+				 struct rockchip_thermal_sensor *sensor,
+				 enum sensor_id id)
+{
+	const struct rockchip_tsadc_chip *tsadc = thermal->chip;
+	int error;
+
+	tsadc->set_tshut_mode(id, thermal->regs, thermal->tshut_mode);
+	tsadc->set_tshut_temp(id, thermal->regs, thermal->tshut_temp);
+
+	sensor->thermal = thermal;
+	sensor->id = id;
+	sensor->tzd = thermal_zone_of_sensor_register(&pdev->dev, id, sensor,
+						      &rockchip_of_thermal_ops);
+	if (IS_ERR(sensor->tzd)) {
+		error = PTR_ERR(sensor->tzd);
+		dev_err(&pdev->dev, "failed to register sensor %d: %d\n",
+			id, error);
+		return error;
+	}
+
+	return 0;
+}
+
+/*
+ * Reset TSADC Controller, reset all tsadc registers.
+ */
+static void rockchip_thermal_reset_controller(struct reset_control *reset)
+{
+	reset_control_assert(reset);
+	usleep_range(10, 20);
+	reset_control_deassert(reset);
+}
+
+static int rockchip_thermal_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct rockchip_thermal_data *thermal;
+	const struct of_device_id *match;
+	struct resource *res;
+	int irq;
+	int i;
+	int error;
+
+	match = of_match_node(of_rockchip_thermal_match, np);
+	if (!match)
+		return -ENXIO;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "no irq resource?\n");
+		return -EINVAL;
+	}
+
+	thermal = devm_kzalloc(&pdev->dev, sizeof(struct rockchip_thermal_data),
+			       GFP_KERNEL);
+	if (!thermal)
+		return -ENOMEM;
+
+	thermal->pdev = pdev;
+
+	thermal->chip = (const struct rockchip_tsadc_chip *)match->data;
+	if (!thermal->chip)
+		return -EINVAL;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	thermal->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(thermal->regs))
+		return PTR_ERR(thermal->regs);
+
+	thermal->reset = devm_reset_control_get(&pdev->dev, "tsadc-apb");
+	if (IS_ERR(thermal->reset)) {
+		error = PTR_ERR(thermal->reset);
+		dev_err(&pdev->dev, "failed to get tsadc reset: %d\n", error);
+		return error;
+	}
+
+	thermal->clk = devm_clk_get(&pdev->dev, "tsadc");
+	if (IS_ERR(thermal->clk)) {
+		error = PTR_ERR(thermal->clk);
+		dev_err(&pdev->dev, "failed to get tsadc clock: %d\n", error);
+		return error;
+	}
+
+	thermal->pclk = devm_clk_get(&pdev->dev, "apb_pclk");
+	if (IS_ERR(thermal->pclk)) {
+		error = PTR_ERR(thermal->clk);
+		dev_err(&pdev->dev, "failed to get apb_pclk clock: %d\n",
+			error);
+		return error;
+	}
+
+	error = clk_prepare_enable(thermal->clk);
+	if (error) {
+		dev_err(&pdev->dev, "failed to enable converter clock: %d\n",
+			error);
+		return error;
+	}
+
+	error = clk_prepare_enable(thermal->pclk);
+	if (error) {
+		dev_err(&pdev->dev, "failed to enable pclk: %d\n", error);
+		goto err_disable_clk;
+	}
+
+	rockchip_thermal_reset_controller(thermal->reset);
+
+	error = rockchip_configure_from_dt(&pdev->dev, np, thermal);
+	if (error) {
+		dev_err(&pdev->dev, "failed to parse device tree data: %d\n",
+			error);
+		goto err_disable_pclk;
+	}
+
+	thermal->chip->initialize(thermal->regs, thermal->tshut_polarity);
+
+	error = rockchip_thermal_register_sensor(pdev, thermal,
+						 &thermal->sensors[0],
+						 SENSOR_CPU);
+	if (error) {
+		dev_err(&pdev->dev,
+			"failed to register CPU thermal sensor: %d\n", error);
+		goto err_disable_pclk;
+	}
+
+	error = rockchip_thermal_register_sensor(pdev, thermal,
+						 &thermal->sensors[1],
+						 SENSOR_GPU);
+	if (error) {
+		dev_err(&pdev->dev,
+			"failed to register GPU thermal sensor: %d\n", error);
+		goto err_unregister_cpu_sensor;
+	}
+
+	error = devm_request_threaded_irq(&pdev->dev, irq, NULL,
+					  &rockchip_thermal_alarm_irq_thread,
+					  IRQF_ONESHOT,
+					  "rockchip_thermal", thermal);
+	if (error) {
+		dev_err(&pdev->dev,
+			"failed to request tsadc irq: %d\n", error);
+		goto err_unregister_gpu_sensor;
+	}
+
+	thermal->chip->control(thermal->regs, true);
+
+	for (i = 0; i < ARRAY_SIZE(thermal->sensors); i++)
+		rockchip_thermal_toggle_sensor(&thermal->sensors[i], true);
+
+	platform_set_drvdata(pdev, thermal);
+
+	return 0;
+
+err_unregister_gpu_sensor:
+	thermal_zone_of_sensor_unregister(&pdev->dev, thermal->sensors[1].tzd);
+err_unregister_cpu_sensor:
+	thermal_zone_of_sensor_unregister(&pdev->dev, thermal->sensors[0].tzd);
+err_disable_pclk:
+	clk_disable_unprepare(thermal->pclk);
+err_disable_clk:
+	clk_disable_unprepare(thermal->clk);
+
+	return error;
+}
+
+static int rockchip_thermal_remove(struct platform_device *pdev)
+{
+	struct rockchip_thermal_data *thermal = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(thermal->sensors); i++) {
+		struct rockchip_thermal_sensor *sensor = &thermal->sensors[i];
+
+		rockchip_thermal_toggle_sensor(sensor, false);
+		thermal_zone_of_sensor_unregister(&pdev->dev, sensor->tzd);
+	}
+
+	thermal->chip->control(thermal->regs, false);
+
+	clk_disable_unprepare(thermal->pclk);
+	clk_disable_unprepare(thermal->clk);
+
+	return 0;
+}
+
+static int __maybe_unused rockchip_thermal_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct rockchip_thermal_data *thermal = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(thermal->sensors); i++)
+		rockchip_thermal_toggle_sensor(&thermal->sensors[i], false);
+
+	thermal->chip->control(thermal->regs, false);
+
+	clk_disable(thermal->pclk);
+	clk_disable(thermal->clk);
+
+	return 0;
+}
+
+static int __maybe_unused rockchip_thermal_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct rockchip_thermal_data *thermal = platform_get_drvdata(pdev);
+	int i;
+	int error;
+
+	error = clk_enable(thermal->clk);
+	if (error)
+		return error;
+
+	error = clk_enable(thermal->pclk);
+	if (error)
+		return error;
+
+	rockchip_thermal_reset_controller(thermal->reset);
+
+	thermal->chip->initialize(thermal->regs, thermal->tshut_polarity);
+
+	for (i = 0; i < ARRAY_SIZE(thermal->sensors); i++) {
+		enum sensor_id id = thermal->sensors[i].id;
+
+		thermal->chip->set_tshut_mode(id, thermal->regs,
+					      thermal->tshut_mode);
+		thermal->chip->set_tshut_temp(id, thermal->regs,
+					      thermal->tshut_temp);
+	}
+
+	thermal->chip->control(thermal->regs, true);
+
+	for (i = 0; i < ARRAY_SIZE(thermal->sensors); i++)
+		rockchip_thermal_toggle_sensor(&thermal->sensors[i], true);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(rockchip_thermal_pm_ops,
+			 rockchip_thermal_suspend, rockchip_thermal_resume);
+
+static struct platform_driver rockchip_thermal_driver = {
+	.driver = {
+		.name = "rockchip-thermal",
+		.pm = &rockchip_thermal_pm_ops,
+		.of_match_table = of_rockchip_thermal_match,
+	},
+	.probe = rockchip_thermal_probe,
+	.remove = rockchip_thermal_remove,
+};
+
+module_platform_driver(rockchip_thermal_driver);
+
+MODULE_DESCRIPTION("ROCKCHIP THERMAL Driver");
+MODULE_AUTHOR("Rockchip, Inc.");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:rockchip-thermal");
diff --git a/drivers/thermal/samsung/Kconfig b/drivers/thermal/samsung/Kconfig
index f760389a204c..c43306ecc0ab 100644
--- a/drivers/thermal/samsung/Kconfig
+++ b/drivers/thermal/samsung/Kconfig
@@ -1,6 +1,6 @@
 config EXYNOS_THERMAL
 	tristate "Exynos thermal management unit driver"
-	depends on ARCH_HAS_BANDGAP && OF
+	depends on OF
 	help
 	  If you say yes here you get support for the TMU (Thermal Management
 	  Unit) driver for SAMSUNG EXYNOS series of SoCs. This driver initialises
diff --git a/drivers/thermal/samsung/exynos_thermal_common.c b/drivers/thermal/samsung/exynos_thermal_common.c
index b6be572704a4..6dc3815cc73f 100644
--- a/drivers/thermal/samsung/exynos_thermal_common.c
+++ b/drivers/thermal/samsung/exynos_thermal_common.c
@@ -347,7 +347,6 @@ void exynos_report_trigger(struct thermal_sensor_conf *conf)
 int exynos_register_thermal(struct thermal_sensor_conf *sensor_conf)
 {
 	int ret;
-	struct cpumask mask_val;
 	struct exynos_thermal_zone *th_zone;
 
 	if (!sensor_conf || !sensor_conf->read_temperature) {
@@ -367,13 +366,14 @@ int exynos_register_thermal(struct thermal_sensor_conf *sensor_conf)
 	 *	 sensor
 	 */
 	if (sensor_conf->cooling_data.freq_clip_count > 0) {
-		cpumask_set_cpu(0, &mask_val);
 		th_zone->cool_dev[th_zone->cool_dev_size] =
-					cpufreq_cooling_register(&mask_val);
+				cpufreq_cooling_register(cpu_present_mask);
 		if (IS_ERR(th_zone->cool_dev[th_zone->cool_dev_size])) {
-			dev_err(sensor_conf->dev,
-				"Failed to register cpufreq cooling device\n");
-			ret = -EINVAL;
+			ret = PTR_ERR(th_zone->cool_dev[th_zone->cool_dev_size]);
+			if (ret != -EPROBE_DEFER)
+				dev_err(sensor_conf->dev,
+					"Failed to register cpufreq cooling device: %d\n",
+					ret);
 			goto err_unregister;
 		}
 		th_zone->cool_dev_size++;
diff --git a/drivers/thermal/samsung/exynos_thermal_common.h b/drivers/thermal/samsung/exynos_thermal_common.h
index 158f5aa8dc5d..cd4471925cdd 100644
--- a/drivers/thermal/samsung/exynos_thermal_common.h
+++ b/drivers/thermal/samsung/exynos_thermal_common.h
@@ -27,7 +27,6 @@
 #define SENSOR_NAME_LEN	16
 #define MAX_TRIP_COUNT	8
 #define MAX_COOLING_DEVICE 4
-#define MAX_TRIMINFO_CTRL_REG	2
 
 #define ACTIVE_INTERVAL 500
 #define IDLE_INTERVAL 10000
diff --git a/drivers/thermal/samsung/exynos_tmu.c b/drivers/thermal/samsung/exynos_tmu.c
index 49c09243fd38..d2f1e62a4232 100644
--- a/drivers/thermal/samsung/exynos_tmu.c
+++ b/drivers/thermal/samsung/exynos_tmu.c
@@ -33,7 +33,87 @@
 
 #include "exynos_thermal_common.h"
 #include "exynos_tmu.h"
-#include "exynos_tmu_data.h"
+
+/* Exynos generic registers */
+#define EXYNOS_TMU_REG_TRIMINFO		0x0
+#define EXYNOS_TMU_REG_CONTROL		0x20
+#define EXYNOS_TMU_REG_STATUS		0x28
+#define EXYNOS_TMU_REG_CURRENT_TEMP	0x40
+#define EXYNOS_TMU_REG_INTEN		0x70
+#define EXYNOS_TMU_REG_INTSTAT		0x74
+#define EXYNOS_TMU_REG_INTCLEAR		0x78
+
+#define EXYNOS_TMU_TEMP_MASK		0xff
+#define EXYNOS_TMU_REF_VOLTAGE_SHIFT	24
+#define EXYNOS_TMU_REF_VOLTAGE_MASK	0x1f
+#define EXYNOS_TMU_BUF_SLOPE_SEL_MASK	0xf
+#define EXYNOS_TMU_BUF_SLOPE_SEL_SHIFT	8
+#define EXYNOS_TMU_CORE_EN_SHIFT	0
+
+/* Exynos3250 specific registers */
+#define EXYNOS_TMU_TRIMINFO_CON1	0x10
+
+/* Exynos4210 specific registers */
+#define EXYNOS4210_TMU_REG_THRESHOLD_TEMP	0x44
+#define EXYNOS4210_TMU_REG_TRIG_LEVEL0	0x50
+
+/* Exynos5250, Exynos4412, Exynos3250 specific registers */
+#define EXYNOS_TMU_TRIMINFO_CON2	0x14
+#define EXYNOS_THD_TEMP_RISE		0x50
+#define EXYNOS_THD_TEMP_FALL		0x54
+#define EXYNOS_EMUL_CON		0x80
+
+#define EXYNOS_TRIMINFO_RELOAD_ENABLE	1
+#define EXYNOS_TRIMINFO_25_SHIFT	0
+#define EXYNOS_TRIMINFO_85_SHIFT	8
+#define EXYNOS_TMU_TRIP_MODE_SHIFT	13
+#define EXYNOS_TMU_TRIP_MODE_MASK	0x7
+#define EXYNOS_TMU_THERM_TRIP_EN_SHIFT	12
+
+#define EXYNOS_TMU_INTEN_RISE0_SHIFT	0
+#define EXYNOS_TMU_INTEN_RISE1_SHIFT	4
+#define EXYNOS_TMU_INTEN_RISE2_SHIFT	8
+#define EXYNOS_TMU_INTEN_RISE3_SHIFT	12
+#define EXYNOS_TMU_INTEN_FALL0_SHIFT	16
+
+#define EXYNOS_EMUL_TIME	0x57F0
+#define EXYNOS_EMUL_TIME_MASK	0xffff
+#define EXYNOS_EMUL_TIME_SHIFT	16
+#define EXYNOS_EMUL_DATA_SHIFT	8
+#define EXYNOS_EMUL_DATA_MASK	0xFF
+#define EXYNOS_EMUL_ENABLE	0x1
+
+/* Exynos5260 specific */
+#define EXYNOS5260_TMU_REG_INTEN		0xC0
+#define EXYNOS5260_TMU_REG_INTSTAT		0xC4
+#define EXYNOS5260_TMU_REG_INTCLEAR		0xC8
+#define EXYNOS5260_EMUL_CON			0x100
+
+/* Exynos4412 specific */
+#define EXYNOS4412_MUX_ADDR_VALUE          6
+#define EXYNOS4412_MUX_ADDR_SHIFT          20
+
+/*exynos5440 specific registers*/
+#define EXYNOS5440_TMU_S0_7_TRIM		0x000
+#define EXYNOS5440_TMU_S0_7_CTRL		0x020
+#define EXYNOS5440_TMU_S0_7_DEBUG		0x040
+#define EXYNOS5440_TMU_S0_7_TEMP		0x0f0
+#define EXYNOS5440_TMU_S0_7_TH0			0x110
+#define EXYNOS5440_TMU_S0_7_TH1			0x130
+#define EXYNOS5440_TMU_S0_7_TH2			0x150
+#define EXYNOS5440_TMU_S0_7_IRQEN		0x210
+#define EXYNOS5440_TMU_S0_7_IRQ			0x230
+/* exynos5440 common registers */
+#define EXYNOS5440_TMU_IRQ_STATUS		0x000
+#define EXYNOS5440_TMU_PMIN			0x004
+
+#define EXYNOS5440_TMU_INTEN_RISE0_SHIFT	0
+#define EXYNOS5440_TMU_INTEN_RISE1_SHIFT	1
+#define EXYNOS5440_TMU_INTEN_RISE2_SHIFT	2
+#define EXYNOS5440_TMU_INTEN_RISE3_SHIFT	3
+#define EXYNOS5440_TMU_INTEN_FALL0_SHIFT	4
+#define EXYNOS5440_TMU_TH_RISE4_SHIFT		24
+#define EXYNOS5440_EFUSE_SWAP_OFFSET		8
 
 /**
  * struct exynos_tmu_data : A structure to hold the private data of the TMU
@@ -52,6 +132,11 @@
  * @temp_error2: fused value of the second point trim.
  * @regulator: pointer to the TMU regulator structure.
  * @reg_conf: pointer to structure to register with core thermal.
+ * @tmu_initialize: SoC specific TMU initialization method
+ * @tmu_control: SoC specific TMU control method
+ * @tmu_read: SoC specific TMU temperature read method
+ * @tmu_set_emulation: SoC specific TMU emulation setting method
+ * @tmu_clear_irqs: SoC specific TMU interrupts clearing method
  */
 struct exynos_tmu_data {
 	int id;
@@ -66,6 +151,12 @@ struct exynos_tmu_data {
 	u8 temp_error1, temp_error2;
 	struct regulator *regulator;
 	struct thermal_sensor_conf *reg_conf;
+	int (*tmu_initialize)(struct platform_device *pdev);
+	void (*tmu_control)(struct platform_device *pdev, bool on);
+	int (*tmu_read)(struct exynos_tmu_data *data);
+	void (*tmu_set_emulation)(struct exynos_tmu_data *data,
+				  unsigned long temp);
+	void (*tmu_clear_irqs)(struct exynos_tmu_data *data);
 };
 
 /*
@@ -122,83 +213,10 @@ static int code_to_temp(struct exynos_tmu_data *data, u8 temp_code)
 	return temp;
 }
 
-static void exynos_tmu_clear_irqs(struct exynos_tmu_data *data)
-{
-	const struct exynos_tmu_registers *reg = data->pdata->registers;
-	unsigned int val_irq;
-
-	val_irq = readl(data->base + reg->tmu_intstat);
-	/*
-	 * Clear the interrupts.  Please note that the documentation for
-	 * Exynos3250, Exynos4412, Exynos5250 and Exynos5260 incorrectly
-	 * states that INTCLEAR register has a different placing of bits
-	 * responsible for FALL IRQs than INTSTAT register.  Exynos5420
-	 * and Exynos5440 documentation is correct (Exynos4210 doesn't
-	 * support FALL IRQs at all).
-	 */
-	writel(val_irq, data->base + reg->tmu_intclear);
-}
-
-static int exynos_tmu_initialize(struct platform_device *pdev)
+static void sanitize_temp_error(struct exynos_tmu_data *data, u32 trim_info)
 {
-	struct exynos_tmu_data *data = platform_get_drvdata(pdev);
 	struct exynos_tmu_platform_data *pdata = data->pdata;
-	const struct exynos_tmu_registers *reg = pdata->registers;
-	unsigned int status, trim_info = 0, con, ctrl;
-	unsigned int rising_threshold = 0, falling_threshold = 0;
-	int ret = 0, threshold_code, i;
-
-	mutex_lock(&data->lock);
-	clk_enable(data->clk);
-	if (!IS_ERR(data->clk_sec))
-		clk_enable(data->clk_sec);
 
-	if (TMU_SUPPORTS(pdata, READY_STATUS)) {
-		status = readb(data->base + reg->tmu_status);
-		if (!status) {
-			ret = -EBUSY;
-			goto out;
-		}
-	}
-
-	if (TMU_SUPPORTS(pdata, TRIM_RELOAD)) {
-		for (i = 0; i < reg->triminfo_ctrl_count; i++) {
-			if (pdata->triminfo_reload[i]) {
-				ctrl = readl(data->base +
-						reg->triminfo_ctrl[i]);
-				ctrl |= pdata->triminfo_reload[i];
-				writel(ctrl, data->base +
-						reg->triminfo_ctrl[i]);
-			}
-		}
-	}
-
-	/* Save trimming info in order to perform calibration */
-	if (data->soc == SOC_ARCH_EXYNOS5440) {
-		/*
-		 * For exynos5440 soc triminfo value is swapped between TMU0 and
-		 * TMU2, so the below logic is needed.
-		 */
-		switch (data->id) {
-		case 0:
-			trim_info = readl(data->base +
-			EXYNOS5440_EFUSE_SWAP_OFFSET + reg->triminfo_data);
-			break;
-		case 1:
-			trim_info = readl(data->base + reg->triminfo_data);
-			break;
-		case 2:
-			trim_info = readl(data->base -
-			EXYNOS5440_EFUSE_SWAP_OFFSET + reg->triminfo_data);
-		}
-	} else {
-		/* On exynos5420 the triminfo register is in the shared space */
-		if (data->soc == SOC_ARCH_EXYNOS5420_TRIMINFO)
-			trim_info = readl(data->base_second +
-							reg->triminfo_data);
-		else
-			trim_info = readl(data->base + reg->triminfo_data);
-	}
 	data->temp_error1 = trim_info & EXYNOS_TMU_TEMP_MASK;
 	data->temp_error2 = ((trim_info >> EXYNOS_TRIMINFO_85_SHIFT) &
 				EXYNOS_TMU_TEMP_MASK);
@@ -212,69 +230,37 @@ static int exynos_tmu_initialize(struct platform_device *pdev)
 		data->temp_error2 =
 			(pdata->efuse_value >> EXYNOS_TRIMINFO_85_SHIFT) &
 			EXYNOS_TMU_TEMP_MASK;
+}
 
-	rising_threshold = readl(data->base + reg->threshold_th0);
+static u32 get_th_reg(struct exynos_tmu_data *data, u32 threshold, bool falling)
+{
+	struct exynos_tmu_platform_data *pdata = data->pdata;
+	int i;
 
-	if (data->soc == SOC_ARCH_EXYNOS4210) {
-		/* Write temperature code for threshold */
-		threshold_code = temp_to_code(data, pdata->threshold);
-		writeb(threshold_code,
-			data->base + reg->threshold_temp);
-		for (i = 0; i < pdata->non_hw_trigger_levels; i++)
-			writeb(pdata->trigger_levels[i], data->base +
-			reg->threshold_th0 + i * sizeof(reg->threshold_th0));
+	for (i = 0; i < pdata->non_hw_trigger_levels; i++) {
+		u8 temp = pdata->trigger_levels[i];
 
-		exynos_tmu_clear_irqs(data);
-	} else {
-		/* Write temperature code for rising and falling threshold */
-		for (i = 0; i < pdata->non_hw_trigger_levels; i++) {
-			threshold_code = temp_to_code(data,
-						pdata->trigger_levels[i]);
-			rising_threshold &= ~(0xff << 8 * i);
-			rising_threshold |= threshold_code << 8 * i;
-			if (pdata->threshold_falling) {
-				threshold_code = temp_to_code(data,
-						pdata->trigger_levels[i] -
-						pdata->threshold_falling);
-				falling_threshold |= threshold_code << 8 * i;
-			}
-		}
+		if (falling)
+			temp -= pdata->threshold_falling;
+		else
+			threshold &= ~(0xff << 8 * i);
 
-		writel(rising_threshold,
-				data->base + reg->threshold_th0);
-		writel(falling_threshold,
-				data->base + reg->threshold_th1);
-
-		exynos_tmu_clear_irqs(data);
-
-		/* if last threshold limit is also present */
-		i = pdata->max_trigger_level - 1;
-		if (pdata->trigger_levels[i] &&
-				(pdata->trigger_type[i] == HW_TRIP)) {
-			threshold_code = temp_to_code(data,
-						pdata->trigger_levels[i]);
-			if (i == EXYNOS_MAX_TRIGGER_PER_REG - 1) {
-				/* 1-4 level to be assigned in th0 reg */
-				rising_threshold &= ~(0xff << 8 * i);
-				rising_threshold |= threshold_code << 8 * i;
-				writel(rising_threshold,
-					data->base + reg->threshold_th0);
-			} else if (i == EXYNOS_MAX_TRIGGER_PER_REG) {
-				/* 5th level to be assigned in th2 reg */
-				rising_threshold =
-				threshold_code << reg->threshold_th3_l0_shift;
-				writel(rising_threshold,
-					data->base + reg->threshold_th2);
-			}
-			con = readl(data->base + reg->tmu_ctrl);
-			con |= (1 << reg->therm_trip_en_shift);
-			writel(con, data->base + reg->tmu_ctrl);
-		}
+		threshold |= temp_to_code(data, temp) << 8 * i;
 	}
-	/*Clear the PMIN in the common TMU register*/
-	if (reg->tmu_pmin && !data->id)
-		writel(0, data->base_second + reg->tmu_pmin);
-out:
+
+	return threshold;
+}
+
+static int exynos_tmu_initialize(struct platform_device *pdev)
+{
+	struct exynos_tmu_data *data = platform_get_drvdata(pdev);
+	int ret;
+
+	mutex_lock(&data->lock);
+	clk_enable(data->clk);
+	if (!IS_ERR(data->clk_sec))
+		clk_enable(data->clk_sec);
+	ret = data->tmu_initialize(pdev);
 	clk_disable(data->clk);
 	mutex_unlock(&data->lock);
 	if (!IS_ERR(data->clk_sec))
@@ -283,20 +269,13 @@ out:
 	return ret;
 }
 
-static void exynos_tmu_control(struct platform_device *pdev, bool on)
+static u32 get_con_reg(struct exynos_tmu_data *data, u32 con)
 {
-	struct exynos_tmu_data *data = platform_get_drvdata(pdev);
 	struct exynos_tmu_platform_data *pdata = data->pdata;
-	const struct exynos_tmu_registers *reg = pdata->registers;
-	unsigned int con, interrupt_en;
 
-	mutex_lock(&data->lock);
-	clk_enable(data->clk);
-
-	con = readl(data->base + reg->tmu_ctrl);
-
-	if (pdata->test_mux)
-		con |= (pdata->test_mux << reg->test_mux_addr_shift);
+	if (data->soc == SOC_ARCH_EXYNOS4412 ||
+	    data->soc == SOC_ARCH_EXYNOS3250)
+		con |= (EXYNOS4412_MUX_ADDR_VALUE << EXYNOS4412_MUX_ADDR_SHIFT);
 
 	con &= ~(EXYNOS_TMU_REF_VOLTAGE_MASK << EXYNOS_TMU_REF_VOLTAGE_SHIFT);
 	con |= pdata->reference_voltage << EXYNOS_TMU_REF_VOLTAGE_SHIFT;
@@ -305,95 +284,287 @@ static void exynos_tmu_control(struct platform_device *pdev, bool on)
 	con |= (pdata->gain << EXYNOS_TMU_BUF_SLOPE_SEL_SHIFT);
 
 	if (pdata->noise_cancel_mode) {
-		con &= ~(reg->therm_trip_mode_mask <<
-					reg->therm_trip_mode_shift);
-		con |= (pdata->noise_cancel_mode << reg->therm_trip_mode_shift);
+		con &= ~(EXYNOS_TMU_TRIP_MODE_MASK << EXYNOS_TMU_TRIP_MODE_SHIFT);
+		con |= (pdata->noise_cancel_mode << EXYNOS_TMU_TRIP_MODE_SHIFT);
 	}
 
-	if (on) {
-		con |= (1 << EXYNOS_TMU_CORE_EN_SHIFT);
-		interrupt_en =
-			pdata->trigger_enable[3] << reg->inten_rise3_shift |
-			pdata->trigger_enable[2] << reg->inten_rise2_shift |
-			pdata->trigger_enable[1] << reg->inten_rise1_shift |
-			pdata->trigger_enable[0] << reg->inten_rise0_shift;
-		if (TMU_SUPPORTS(pdata, FALLING_TRIP))
-			interrupt_en |=
-				interrupt_en << reg->inten_fall0_shift;
-	} else {
-		con &= ~(1 << EXYNOS_TMU_CORE_EN_SHIFT);
-		interrupt_en = 0; /* Disable all interrupts */
-	}
-	writel(interrupt_en, data->base + reg->tmu_inten);
-	writel(con, data->base + reg->tmu_ctrl);
+	return con;
+}
+
+static void exynos_tmu_control(struct platform_device *pdev, bool on)
+{
+	struct exynos_tmu_data *data = platform_get_drvdata(pdev);
 
+	mutex_lock(&data->lock);
+	clk_enable(data->clk);
+	data->tmu_control(pdev, on);
 	clk_disable(data->clk);
 	mutex_unlock(&data->lock);
 }
 
-static int exynos_tmu_read(struct exynos_tmu_data *data)
+static int exynos4210_tmu_initialize(struct platform_device *pdev)
 {
+	struct exynos_tmu_data *data = platform_get_drvdata(pdev);
 	struct exynos_tmu_platform_data *pdata = data->pdata;
-	const struct exynos_tmu_registers *reg = pdata->registers;
-	u8 temp_code;
-	int temp;
+	unsigned int status;
+	int ret = 0, threshold_code, i;
 
-	mutex_lock(&data->lock);
-	clk_enable(data->clk);
+	status = readb(data->base + EXYNOS_TMU_REG_STATUS);
+	if (!status) {
+		ret = -EBUSY;
+		goto out;
+	}
 
-	temp_code = readb(data->base + reg->tmu_cur_temp);
+	sanitize_temp_error(data, readl(data->base + EXYNOS_TMU_REG_TRIMINFO));
 
-	if (data->soc == SOC_ARCH_EXYNOS4210)
-		/* temp_code should range between 75 and 175 */
-		if (temp_code < 75 || temp_code > 175) {
-			temp = -ENODATA;
-			goto out;
+	/* Write temperature code for threshold */
+	threshold_code = temp_to_code(data, pdata->threshold);
+	writeb(threshold_code, data->base + EXYNOS4210_TMU_REG_THRESHOLD_TEMP);
+
+	for (i = 0; i < pdata->non_hw_trigger_levels; i++)
+		writeb(pdata->trigger_levels[i], data->base +
+		       EXYNOS4210_TMU_REG_TRIG_LEVEL0 + i * 4);
+
+	data->tmu_clear_irqs(data);
+out:
+	return ret;
+}
+
+static int exynos4412_tmu_initialize(struct platform_device *pdev)
+{
+	struct exynos_tmu_data *data = platform_get_drvdata(pdev);
+	struct exynos_tmu_platform_data *pdata = data->pdata;
+	unsigned int status, trim_info, con, ctrl, rising_threshold;
+	int ret = 0, threshold_code, i;
+
+	status = readb(data->base + EXYNOS_TMU_REG_STATUS);
+	if (!status) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	if (data->soc == SOC_ARCH_EXYNOS3250 ||
+	    data->soc == SOC_ARCH_EXYNOS4412 ||
+	    data->soc == SOC_ARCH_EXYNOS5250) {
+		if (data->soc == SOC_ARCH_EXYNOS3250) {
+			ctrl = readl(data->base + EXYNOS_TMU_TRIMINFO_CON1);
+			ctrl |= EXYNOS_TRIMINFO_RELOAD_ENABLE;
+			writel(ctrl, data->base + EXYNOS_TMU_TRIMINFO_CON1);
 		}
+		ctrl = readl(data->base + EXYNOS_TMU_TRIMINFO_CON2);
+		ctrl |= EXYNOS_TRIMINFO_RELOAD_ENABLE;
+		writel(ctrl, data->base + EXYNOS_TMU_TRIMINFO_CON2);
+	}
 
-	temp = code_to_temp(data, temp_code);
+	/* On exynos5420 the triminfo register is in the shared space */
+	if (data->soc == SOC_ARCH_EXYNOS5420_TRIMINFO)
+		trim_info = readl(data->base_second + EXYNOS_TMU_REG_TRIMINFO);
+	else
+		trim_info = readl(data->base + EXYNOS_TMU_REG_TRIMINFO);
+
+	sanitize_temp_error(data, trim_info);
+
+	/* Write temperature code for rising and falling threshold */
+	rising_threshold = readl(data->base + EXYNOS_THD_TEMP_RISE);
+	rising_threshold = get_th_reg(data, rising_threshold, false);
+	writel(rising_threshold, data->base + EXYNOS_THD_TEMP_RISE);
+	writel(get_th_reg(data, 0, true), data->base + EXYNOS_THD_TEMP_FALL);
+
+	data->tmu_clear_irqs(data);
+
+	/* if last threshold limit is also present */
+	i = pdata->max_trigger_level - 1;
+	if (pdata->trigger_levels[i] && pdata->trigger_type[i] == HW_TRIP) {
+		threshold_code = temp_to_code(data, pdata->trigger_levels[i]);
+		/* 1-4 level to be assigned in th0 reg */
+		rising_threshold &= ~(0xff << 8 * i);
+		rising_threshold |= threshold_code << 8 * i;
+		writel(rising_threshold, data->base + EXYNOS_THD_TEMP_RISE);
+		con = readl(data->base + EXYNOS_TMU_REG_CONTROL);
+		con |= (1 << EXYNOS_TMU_THERM_TRIP_EN_SHIFT);
+		writel(con, data->base + EXYNOS_TMU_REG_CONTROL);
+	}
 out:
-	clk_disable(data->clk);
-	mutex_unlock(&data->lock);
+	return ret;
+}
 
-	return temp;
+static int exynos5440_tmu_initialize(struct platform_device *pdev)
+{
+	struct exynos_tmu_data *data = platform_get_drvdata(pdev);
+	struct exynos_tmu_platform_data *pdata = data->pdata;
+	unsigned int trim_info = 0, con, rising_threshold;
+	int ret = 0, threshold_code, i;
+
+	/*
+	 * For exynos5440 soc triminfo value is swapped between TMU0 and
+	 * TMU2, so the below logic is needed.
+	 */
+	switch (data->id) {
+	case 0:
+		trim_info = readl(data->base + EXYNOS5440_EFUSE_SWAP_OFFSET +
+				 EXYNOS5440_TMU_S0_7_TRIM);
+		break;
+	case 1:
+		trim_info = readl(data->base + EXYNOS5440_TMU_S0_7_TRIM);
+		break;
+	case 2:
+		trim_info = readl(data->base - EXYNOS5440_EFUSE_SWAP_OFFSET +
+				  EXYNOS5440_TMU_S0_7_TRIM);
+	}
+	sanitize_temp_error(data, trim_info);
+
+	/* Write temperature code for rising and falling threshold */
+	rising_threshold = readl(data->base + EXYNOS5440_TMU_S0_7_TH0);
+	rising_threshold = get_th_reg(data, rising_threshold, false);
+	writel(rising_threshold, data->base + EXYNOS5440_TMU_S0_7_TH0);
+	writel(0, data->base + EXYNOS5440_TMU_S0_7_TH1);
+
+	data->tmu_clear_irqs(data);
+
+	/* if last threshold limit is also present */
+	i = pdata->max_trigger_level - 1;
+	if (pdata->trigger_levels[i] && pdata->trigger_type[i] == HW_TRIP) {
+		threshold_code = temp_to_code(data, pdata->trigger_levels[i]);
+		/* 5th level to be assigned in th2 reg */
+		rising_threshold =
+			threshold_code << EXYNOS5440_TMU_TH_RISE4_SHIFT;
+		writel(rising_threshold, data->base + EXYNOS5440_TMU_S0_7_TH2);
+		con = readl(data->base + EXYNOS5440_TMU_S0_7_CTRL);
+		con |= (1 << EXYNOS_TMU_THERM_TRIP_EN_SHIFT);
+		writel(con, data->base + EXYNOS5440_TMU_S0_7_CTRL);
+	}
+	/* Clear the PMIN in the common TMU register */
+	if (!data->id)
+		writel(0, data->base_second + EXYNOS5440_TMU_PMIN);
+	return ret;
 }
 
-#ifdef CONFIG_THERMAL_EMULATION
-static int exynos_tmu_set_emulation(void *drv_data, unsigned long temp)
+static void exynos4210_tmu_control(struct platform_device *pdev, bool on)
 {
-	struct exynos_tmu_data *data = drv_data;
+	struct exynos_tmu_data *data = platform_get_drvdata(pdev);
 	struct exynos_tmu_platform_data *pdata = data->pdata;
-	const struct exynos_tmu_registers *reg = pdata->registers;
-	unsigned int val;
-	int ret = -EINVAL;
+	unsigned int con, interrupt_en;
 
-	if (!TMU_SUPPORTS(pdata, EMULATION))
-		goto out;
+	con = get_con_reg(data, readl(data->base + EXYNOS_TMU_REG_CONTROL));
 
-	if (temp && temp < MCELSIUS)
-		goto out;
+	if (on) {
+		con |= (1 << EXYNOS_TMU_CORE_EN_SHIFT);
+		interrupt_en =
+			pdata->trigger_enable[3] << EXYNOS_TMU_INTEN_RISE3_SHIFT |
+			pdata->trigger_enable[2] << EXYNOS_TMU_INTEN_RISE2_SHIFT |
+			pdata->trigger_enable[1] << EXYNOS_TMU_INTEN_RISE1_SHIFT |
+			pdata->trigger_enable[0] << EXYNOS_TMU_INTEN_RISE0_SHIFT;
+		if (data->soc != SOC_ARCH_EXYNOS4210)
+			interrupt_en |=
+				interrupt_en << EXYNOS_TMU_INTEN_FALL0_SHIFT;
+	} else {
+		con &= ~(1 << EXYNOS_TMU_CORE_EN_SHIFT);
+		interrupt_en = 0; /* Disable all interrupts */
+	}
+	writel(interrupt_en, data->base + EXYNOS_TMU_REG_INTEN);
+	writel(con, data->base + EXYNOS_TMU_REG_CONTROL);
+}
+
+static void exynos5440_tmu_control(struct platform_device *pdev, bool on)
+{
+	struct exynos_tmu_data *data = platform_get_drvdata(pdev);
+	struct exynos_tmu_platform_data *pdata = data->pdata;
+	unsigned int con, interrupt_en;
+
+	con = get_con_reg(data, readl(data->base + EXYNOS5440_TMU_S0_7_CTRL));
+
+	if (on) {
+		con |= (1 << EXYNOS_TMU_CORE_EN_SHIFT);
+		interrupt_en =
+			pdata->trigger_enable[3] << EXYNOS5440_TMU_INTEN_RISE3_SHIFT |
+			pdata->trigger_enable[2] << EXYNOS5440_TMU_INTEN_RISE2_SHIFT |
+			pdata->trigger_enable[1] << EXYNOS5440_TMU_INTEN_RISE1_SHIFT |
+			pdata->trigger_enable[0] << EXYNOS5440_TMU_INTEN_RISE0_SHIFT;
+		interrupt_en |= interrupt_en << EXYNOS5440_TMU_INTEN_FALL0_SHIFT;
+	} else {
+		con &= ~(1 << EXYNOS_TMU_CORE_EN_SHIFT);
+		interrupt_en = 0; /* Disable all interrupts */
+	}
+	writel(interrupt_en, data->base + EXYNOS5440_TMU_S0_7_IRQEN);
+	writel(con, data->base + EXYNOS5440_TMU_S0_7_CTRL);
+}
+
+static int exynos_tmu_read(struct exynos_tmu_data *data)
+{
+	int ret;
 
 	mutex_lock(&data->lock);
 	clk_enable(data->clk);
+	ret = data->tmu_read(data);
+	if (ret >= 0)
+		ret = code_to_temp(data, ret);
+	clk_disable(data->clk);
+	mutex_unlock(&data->lock);
 
-	val = readl(data->base + reg->emul_con);
+	return ret;
+}
 
+#ifdef CONFIG_THERMAL_EMULATION
+static u32 get_emul_con_reg(struct exynos_tmu_data *data, unsigned int val,
+			    unsigned long temp)
+{
 	if (temp) {
 		temp /= MCELSIUS;
 
-		if (TMU_SUPPORTS(pdata, EMUL_TIME)) {
-			val &= ~(EXYNOS_EMUL_TIME_MASK << reg->emul_time_shift);
-			val |= (EXYNOS_EMUL_TIME << reg->emul_time_shift);
+		if (data->soc != SOC_ARCH_EXYNOS5440) {
+			val &= ~(EXYNOS_EMUL_TIME_MASK << EXYNOS_EMUL_TIME_SHIFT);
+			val |= (EXYNOS_EMUL_TIME << EXYNOS_EMUL_TIME_SHIFT);
 		}
-		val &= ~(EXYNOS_EMUL_DATA_MASK << reg->emul_temp_shift);
-		val |= (temp_to_code(data, temp) << reg->emul_temp_shift) |
+		val &= ~(EXYNOS_EMUL_DATA_MASK << EXYNOS_EMUL_DATA_SHIFT);
+		val |= (temp_to_code(data, temp) << EXYNOS_EMUL_DATA_SHIFT) |
 			EXYNOS_EMUL_ENABLE;
 	} else {
 		val &= ~EXYNOS_EMUL_ENABLE;
 	}
 
-	writel(val, data->base + reg->emul_con);
+	return val;
+}
+
+static void exynos4412_tmu_set_emulation(struct exynos_tmu_data *data,
+					 unsigned long temp)
+{
+	unsigned int val;
+	u32 emul_con;
+
+	if (data->soc == SOC_ARCH_EXYNOS5260)
+		emul_con = EXYNOS5260_EMUL_CON;
+	else
+		emul_con = EXYNOS_EMUL_CON;
+
+	val = readl(data->base + emul_con);
+	val = get_emul_con_reg(data, val, temp);
+	writel(val, data->base + emul_con);
+}
+
+static void exynos5440_tmu_set_emulation(struct exynos_tmu_data *data,
+					 unsigned long temp)
+{
+	unsigned int val;
+
+	val = readl(data->base + EXYNOS5440_TMU_S0_7_DEBUG);
+	val = get_emul_con_reg(data, val, temp);
+	writel(val, data->base + EXYNOS5440_TMU_S0_7_DEBUG);
+}
+
+static int exynos_tmu_set_emulation(void *drv_data, unsigned long temp)
+{
+	struct exynos_tmu_data *data = drv_data;
+	int ret = -EINVAL;
+
+	if (data->soc == SOC_ARCH_EXYNOS4210)
+		goto out;
 
+	if (temp && temp < MCELSIUS)
+		goto out;
+
+	mutex_lock(&data->lock);
+	clk_enable(data->clk);
+	data->tmu_set_emulation(data, temp);
 	clk_disable(data->clk);
 	mutex_unlock(&data->lock);
 	return 0;
@@ -401,23 +572,41 @@ out:
 	return ret;
 }
 #else
+#define exynos4412_tmu_set_emulation NULL
+#define exynos5440_tmu_set_emulation NULL
 static int exynos_tmu_set_emulation(void *drv_data,	unsigned long temp)
 	{ return -EINVAL; }
 #endif/*CONFIG_THERMAL_EMULATION*/
 
+static int exynos4210_tmu_read(struct exynos_tmu_data *data)
+{
+	int ret = readb(data->base + EXYNOS_TMU_REG_CURRENT_TEMP);
+
+	/* "temp_code" should range between 75 and 175 */
+	return (ret < 75 || ret > 175) ? -ENODATA : ret;
+}
+
+static int exynos4412_tmu_read(struct exynos_tmu_data *data)
+{
+	return readb(data->base + EXYNOS_TMU_REG_CURRENT_TEMP);
+}
+
+static int exynos5440_tmu_read(struct exynos_tmu_data *data)
+{
+	return readb(data->base + EXYNOS5440_TMU_S0_7_TEMP);
+}
+
 static void exynos_tmu_work(struct work_struct *work)
 {
 	struct exynos_tmu_data *data = container_of(work,
 			struct exynos_tmu_data, irq_work);
-	struct exynos_tmu_platform_data *pdata = data->pdata;
-	const struct exynos_tmu_registers *reg = pdata->registers;
 	unsigned int val_type;
 
 	if (!IS_ERR(data->clk_sec))
 		clk_enable(data->clk_sec);
 	/* Find which sensor generated this interrupt */
-	if (reg->tmu_irqstatus) {
-		val_type = readl(data->base_second + reg->tmu_irqstatus);
+	if (data->soc == SOC_ARCH_EXYNOS5440) {
+		val_type = readl(data->base_second + EXYNOS5440_TMU_IRQ_STATUS);
 		if (!((val_type >> data->id) & 0x1))
 			goto out;
 	}
@@ -429,7 +618,7 @@ static void exynos_tmu_work(struct work_struct *work)
 	clk_enable(data->clk);
 
 	/* TODO: take action based on particular interrupt */
-	exynos_tmu_clear_irqs(data);
+	data->tmu_clear_irqs(data);
 
 	clk_disable(data->clk);
 	mutex_unlock(&data->lock);
@@ -437,6 +626,40 @@ out:
 	enable_irq(data->irq);
 }
 
+static void exynos4210_tmu_clear_irqs(struct exynos_tmu_data *data)
+{
+	unsigned int val_irq;
+	u32 tmu_intstat, tmu_intclear;
+
+	if (data->soc == SOC_ARCH_EXYNOS5260) {
+		tmu_intstat = EXYNOS5260_TMU_REG_INTSTAT;
+		tmu_intclear = EXYNOS5260_TMU_REG_INTCLEAR;
+	} else {
+		tmu_intstat = EXYNOS_TMU_REG_INTSTAT;
+		tmu_intclear = EXYNOS_TMU_REG_INTCLEAR;
+	}
+
+	val_irq = readl(data->base + tmu_intstat);
+	/*
+	 * Clear the interrupts.  Please note that the documentation for
+	 * Exynos3250, Exynos4412, Exynos5250 and Exynos5260 incorrectly
+	 * states that INTCLEAR register has a different placing of bits
+	 * responsible for FALL IRQs than INTSTAT register.  Exynos5420
+	 * and Exynos5440 documentation is correct (Exynos4210 doesn't
+	 * support FALL IRQs at all).
+	 */
+	writel(val_irq, data->base + tmu_intclear);
+}
+
+static void exynos5440_tmu_clear_irqs(struct exynos_tmu_data *data)
+{
+	unsigned int val_irq;
+
+	val_irq = readl(data->base + EXYNOS5440_TMU_S0_7_IRQ);
+	/* clear the interrupts */
+	writel(val_irq, data->base + EXYNOS5440_TMU_S0_7_IRQ);
+}
+
 static irqreturn_t exynos_tmu_irq(int irq, void *id)
 {
 	struct exynos_tmu_data *data = id;
@@ -450,35 +673,35 @@ static irqreturn_t exynos_tmu_irq(int irq, void *id)
 static const struct of_device_id exynos_tmu_match[] = {
 	{
 		.compatible = "samsung,exynos3250-tmu",
-		.data = (void *)EXYNOS3250_TMU_DRV_DATA,
+		.data = &exynos3250_default_tmu_data,
 	},
 	{
 		.compatible = "samsung,exynos4210-tmu",
-		.data = (void *)EXYNOS4210_TMU_DRV_DATA,
+		.data = &exynos4210_default_tmu_data,
 	},
 	{
 		.compatible = "samsung,exynos4412-tmu",
-		.data = (void *)EXYNOS4412_TMU_DRV_DATA,
+		.data = &exynos4412_default_tmu_data,
 	},
 	{
 		.compatible = "samsung,exynos5250-tmu",
-		.data = (void *)EXYNOS5250_TMU_DRV_DATA,
+		.data = &exynos5250_default_tmu_data,
 	},
 	{
 		.compatible = "samsung,exynos5260-tmu",
-		.data = (void *)EXYNOS5260_TMU_DRV_DATA,
+		.data = &exynos5260_default_tmu_data,
 	},
 	{
 		.compatible = "samsung,exynos5420-tmu",
-		.data = (void *)EXYNOS5420_TMU_DRV_DATA,
+		.data = &exynos5420_default_tmu_data,
 	},
 	{
 		.compatible = "samsung,exynos5420-tmu-ext-triminfo",
-		.data = (void *)EXYNOS5420_TMU_DRV_DATA,
+		.data = &exynos5420_default_tmu_data,
 	},
 	{
 		.compatible = "samsung,exynos5440-tmu",
-		.data = (void *)EXYNOS5440_TMU_DRV_DATA,
+		.data = &exynos5440_default_tmu_data,
 	},
 	{},
 };
@@ -553,12 +776,47 @@ static int exynos_map_dt_data(struct platform_device *pdev)
 		dev_err(&pdev->dev, "No platform init data supplied.\n");
 		return -ENODEV;
 	}
+
 	data->pdata = pdata;
+	data->soc = pdata->type;
+
+	switch (data->soc) {
+	case SOC_ARCH_EXYNOS4210:
+		data->tmu_initialize = exynos4210_tmu_initialize;
+		data->tmu_control = exynos4210_tmu_control;
+		data->tmu_read = exynos4210_tmu_read;
+		data->tmu_clear_irqs = exynos4210_tmu_clear_irqs;
+		break;
+	case SOC_ARCH_EXYNOS3250:
+	case SOC_ARCH_EXYNOS4412:
+	case SOC_ARCH_EXYNOS5250:
+	case SOC_ARCH_EXYNOS5260:
+	case SOC_ARCH_EXYNOS5420:
+	case SOC_ARCH_EXYNOS5420_TRIMINFO:
+		data->tmu_initialize = exynos4412_tmu_initialize;
+		data->tmu_control = exynos4210_tmu_control;
+		data->tmu_read = exynos4412_tmu_read;
+		data->tmu_set_emulation = exynos4412_tmu_set_emulation;
+		data->tmu_clear_irqs = exynos4210_tmu_clear_irqs;
+		break;
+	case SOC_ARCH_EXYNOS5440:
+		data->tmu_initialize = exynos5440_tmu_initialize;
+		data->tmu_control = exynos5440_tmu_control;
+		data->tmu_read = exynos5440_tmu_read;
+		data->tmu_set_emulation = exynos5440_tmu_set_emulation;
+		data->tmu_clear_irqs = exynos5440_tmu_clear_irqs;
+		break;
+	default:
+		dev_err(&pdev->dev, "Platform not supported\n");
+		return -EINVAL;
+	}
+
 	/*
 	 * Check if the TMU shares some registers and then try to map the
 	 * memory of common registers.
 	 */
-	if (!TMU_SUPPORTS(pdata, ADDRESS_MULTIPLE))
+	if (data->soc != SOC_ARCH_EXYNOS5420_TRIMINFO &&
+	    data->soc != SOC_ARCH_EXYNOS5440)
 		return 0;
 
 	if (of_address_to_resource(pdev->dev.of_node, 1, &res)) {
@@ -625,20 +883,6 @@ static int exynos_tmu_probe(struct platform_device *pdev)
 		goto err_clk_sec;
 	}
 
-	if (pdata->type == SOC_ARCH_EXYNOS3250 ||
-	    pdata->type == SOC_ARCH_EXYNOS4210 ||
-	    pdata->type == SOC_ARCH_EXYNOS4412 ||
-	    pdata->type == SOC_ARCH_EXYNOS5250 ||
-	    pdata->type == SOC_ARCH_EXYNOS5260 ||
-	    pdata->type == SOC_ARCH_EXYNOS5420_TRIMINFO ||
-	    pdata->type == SOC_ARCH_EXYNOS5440)
-		data->soc = pdata->type;
-	else {
-		ret = -EINVAL;
-		dev_err(&pdev->dev, "Platform not supported\n");
-		goto err_clk;
-	}
-
 	ret = exynos_tmu_initialize(pdev);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to initialize TMU\n");
@@ -683,7 +927,10 @@ static int exynos_tmu_probe(struct platform_device *pdev)
 	/* Register the sensor with thermal management interface */
 	ret = exynos_register_thermal(sensor_conf);
 	if (ret) {
-		dev_err(&pdev->dev, "Failed to register thermal interface\n");
+		if (ret != -EPROBE_DEFER)
+			dev_err(&pdev->dev,
+				"Failed to register thermal interface: %d\n",
+				ret);
 		goto err_clk;
 	}
 	data->reg_conf = sensor_conf;
@@ -750,7 +997,6 @@ static SIMPLE_DEV_PM_OPS(exynos_tmu_pm,
 static struct platform_driver exynos_tmu_driver = {
 	.driver = {
 		.name   = "exynos-tmu",
-		.owner  = THIS_MODULE,
 		.pm     = EXYNOS_TMU_PM,
 		.of_match_table = exynos_tmu_match,
 	},
diff --git a/drivers/thermal/samsung/exynos_tmu.h b/drivers/thermal/samsung/exynos_tmu.h
index c58c7663a3fe..da3009bff6c4 100644
--- a/drivers/thermal/samsung/exynos_tmu.h
+++ b/drivers/thermal/samsung/exynos_tmu.h
@@ -40,115 +40,12 @@ enum soc_type {
 	SOC_ARCH_EXYNOS4412,
 	SOC_ARCH_EXYNOS5250,
 	SOC_ARCH_EXYNOS5260,
+	SOC_ARCH_EXYNOS5420,
 	SOC_ARCH_EXYNOS5420_TRIMINFO,
 	SOC_ARCH_EXYNOS5440,
 };
 
 /**
- * EXYNOS TMU supported features.
- * TMU_SUPPORT_EMULATION - This features is used to set user defined
- *			temperature to the TMU controller.
- * TMU_SUPPORT_MULTI_INST - This features denotes that the soc
- *			has many instances of TMU.
- * TMU_SUPPORT_TRIM_RELOAD - This features shows that trimming can
- *			be reloaded.
- * TMU_SUPPORT_FALLING_TRIP - This features shows that interrupt can
- *			be registered for falling trips also.
- * TMU_SUPPORT_READY_STATUS - This feature tells that the TMU current
- *			state(active/idle) can be checked.
- * TMU_SUPPORT_EMUL_TIME - This features allows to set next temp emulation
- *			sample time.
- * TMU_SUPPORT_ADDRESS_MULTIPLE - This feature tells that the different TMU
- *			sensors shares some common registers.
- * TMU_SUPPORT - macro to compare the above features with the supplied.
- */
-#define TMU_SUPPORT_EMULATION			BIT(0)
-#define TMU_SUPPORT_MULTI_INST			BIT(1)
-#define TMU_SUPPORT_TRIM_RELOAD			BIT(2)
-#define TMU_SUPPORT_FALLING_TRIP		BIT(3)
-#define TMU_SUPPORT_READY_STATUS		BIT(4)
-#define TMU_SUPPORT_EMUL_TIME			BIT(5)
-#define TMU_SUPPORT_ADDRESS_MULTIPLE		BIT(6)
-
-#define TMU_SUPPORTS(a, b)	(a->features & TMU_SUPPORT_ ## b)
-
-/**
- * struct exynos_tmu_register - register descriptors to access registers and
- * bitfields. The register validity, offsets and bitfield values may vary
- * slightly across different exynos SOC's.
- * @triminfo_data: register containing 2 pont trimming data
- * @triminfo_ctrl: trim info controller register.
- * @triminfo_ctrl_count: the number of trim info controller register.
- * @tmu_ctrl: TMU main controller register.
- * @test_mux_addr_shift: shift bits of test mux address.
- * @therm_trip_mode_shift: shift bits of tripping mode in tmu_ctrl register.
- * @therm_trip_mode_mask: mask bits of tripping mode in tmu_ctrl register.
- * @therm_trip_en_shift: shift bits of tripping enable in tmu_ctrl register.
- * @tmu_status: register drescribing the TMU status.
- * @tmu_cur_temp: register containing the current temperature of the TMU.
- * @threshold_temp: register containing the base threshold level.
- * @threshold_th0: Register containing first set of rising levels.
- * @threshold_th1: Register containing second set of rising levels.
- * @threshold_th2: Register containing third set of rising levels.
- * @threshold_th3_l0_shift: shift bits of level0 threshold temperature.
- * @tmu_inten: register containing the different threshold interrupt
-	enable bits.
- * @inten_rise0_shift: shift bits of rising 0 interrupt bits.
- * @inten_rise1_shift: shift bits of rising 1 interrupt bits.
- * @inten_rise2_shift: shift bits of rising 2 interrupt bits.
- * @inten_rise3_shift: shift bits of rising 3 interrupt bits.
- * @inten_fall0_shift: shift bits of falling 0 interrupt bits.
- * @tmu_intstat: Register containing the interrupt status values.
- * @tmu_intclear: Register for clearing the raised interrupt status.
- * @emul_con: TMU emulation controller register.
- * @emul_temp_shift: shift bits of emulation temperature.
- * @emul_time_shift: shift bits of emulation time.
- * @tmu_irqstatus: register to find which TMU generated interrupts.
- * @tmu_pmin: register to get/set the Pmin value.
- */
-struct exynos_tmu_registers {
-	u32	triminfo_data;
-
-	u32	triminfo_ctrl[MAX_TRIMINFO_CTRL_REG];
-	u32	triminfo_ctrl_count;
-
-	u32	tmu_ctrl;
-	u32     test_mux_addr_shift;
-	u32	therm_trip_mode_shift;
-	u32	therm_trip_mode_mask;
-	u32	therm_trip_en_shift;
-
-	u32	tmu_status;
-
-	u32	tmu_cur_temp;
-
-	u32	threshold_temp;
-
-	u32	threshold_th0;
-	u32	threshold_th1;
-	u32	threshold_th2;
-	u32	threshold_th3_l0_shift;
-
-	u32	tmu_inten;
-	u32	inten_rise0_shift;
-	u32	inten_rise1_shift;
-	u32	inten_rise2_shift;
-	u32	inten_rise3_shift;
-	u32	inten_fall0_shift;
-
-	u32	tmu_intstat;
-
-	u32	tmu_intclear;
-
-	u32	emul_con;
-	u32	emul_temp_shift;
-	u32	emul_time_shift;
-
-	u32	tmu_irqstatus;
-	u32	tmu_pmin;
-};
-
-/**
  * struct exynos_tmu_platform_data
  * @threshold: basic temperature for generating interrupt
  *	       25 <= threshold <= 125 [unit: degree Celsius]
@@ -192,16 +89,10 @@ struct exynos_tmu_registers {
  * @first_point_trim: temp value of the first point trimming
  * @second_point_trim: temp value of the second point trimming
  * @default_temp_offset: default temperature offset in case of no trimming
- * @test_mux; information if SoC supports test MUX
- * @triminfo_reload: reload value to read TRIMINFO register
  * @cal_type: calibration type for temperature
  * @freq_clip_table: Table representing frequency reduction percentage.
  * @freq_tab_count: Count of the above table as frequency reduction may
  *	applicable to only some of the trigger levels.
- * @registers: Pointer to structure containing all the TMU controller registers
- *	and bitfields shifts and masks.
- * @features: a bitfield value indicating the features supported in SOC like
- *	emulation, multi instance etc
  *
  * This structure is required for configuration of exynos_tmu driver.
  */
@@ -223,15 +114,11 @@ struct exynos_tmu_platform_data {
 	u8 first_point_trim;
 	u8 second_point_trim;
 	u8 default_temp_offset;
-	u8 test_mux;
-	u8 triminfo_reload[MAX_TRIMINFO_CTRL_REG];
 
 	enum calibration_type cal_type;
 	enum soc_type type;
 	struct freq_clip_table freq_tab[4];
 	unsigned int freq_tab_count;
-	const struct exynos_tmu_registers *registers;
-	unsigned int features;
 };
 
 /**
@@ -246,4 +133,12 @@ struct exynos_tmu_init_data {
 	struct exynos_tmu_platform_data tmu_data[];
 };
 
+extern struct exynos_tmu_init_data const exynos3250_default_tmu_data;
+extern struct exynos_tmu_init_data const exynos4210_default_tmu_data;
+extern struct exynos_tmu_init_data const exynos4412_default_tmu_data;
+extern struct exynos_tmu_init_data const exynos5250_default_tmu_data;
+extern struct exynos_tmu_init_data const exynos5260_default_tmu_data;
+extern struct exynos_tmu_init_data const exynos5420_default_tmu_data;
+extern struct exynos_tmu_init_data const exynos5440_default_tmu_data;
+
 #endif /* _EXYNOS_TMU_H */
diff --git a/drivers/thermal/samsung/exynos_tmu_data.c b/drivers/thermal/samsung/exynos_tmu_data.c
index 1724f6cdaef8..b23910069f68 100644
--- a/drivers/thermal/samsung/exynos_tmu_data.c
+++ b/drivers/thermal/samsung/exynos_tmu_data.c
@@ -22,24 +22,6 @@
 
 #include "exynos_thermal_common.h"
 #include "exynos_tmu.h"
-#include "exynos_tmu_data.h"
-
-#if defined(CONFIG_CPU_EXYNOS4210)
-static const struct exynos_tmu_registers exynos4210_tmu_registers = {
-	.triminfo_data = EXYNOS_TMU_REG_TRIMINFO,
-	.tmu_ctrl = EXYNOS_TMU_REG_CONTROL,
-	.tmu_status = EXYNOS_TMU_REG_STATUS,
-	.tmu_cur_temp = EXYNOS_TMU_REG_CURRENT_TEMP,
-	.threshold_temp = EXYNOS4210_TMU_REG_THRESHOLD_TEMP,
-	.threshold_th0 = EXYNOS4210_TMU_REG_TRIG_LEVEL0,
-	.tmu_inten = EXYNOS_TMU_REG_INTEN,
-	.inten_rise0_shift = EXYNOS_TMU_INTEN_RISE0_SHIFT,
-	.inten_rise1_shift = EXYNOS_TMU_INTEN_RISE1_SHIFT,
-	.inten_rise2_shift = EXYNOS_TMU_INTEN_RISE2_SHIFT,
-	.inten_rise3_shift = EXYNOS_TMU_INTEN_RISE3_SHIFT,
-	.tmu_intstat = EXYNOS_TMU_REG_INTSTAT,
-	.tmu_intclear = EXYNOS_TMU_REG_INTCLEAR,
-};
 
 struct exynos_tmu_init_data const exynos4210_default_tmu_data = {
 	.tmu_data = {
@@ -75,40 +57,10 @@ struct exynos_tmu_init_data const exynos4210_default_tmu_data = {
 		},
 		.freq_tab_count = 2,
 		.type = SOC_ARCH_EXYNOS4210,
-		.registers = &exynos4210_tmu_registers,
-		.features = TMU_SUPPORT_READY_STATUS,
 		},
 	},
 	.tmu_count = 1,
 };
-#endif
-
-#if defined(CONFIG_SOC_EXYNOS3250)
-static const struct exynos_tmu_registers exynos3250_tmu_registers = {
-	.triminfo_data = EXYNOS_TMU_REG_TRIMINFO,
-	.triminfo_ctrl[0] = EXYNOS_TMU_TRIMINFO_CON1,
-	.triminfo_ctrl[1] = EXYNOS_TMU_TRIMINFO_CON2,
-	.triminfo_ctrl_count = 2,
-	.tmu_ctrl = EXYNOS_TMU_REG_CONTROL,
-	.test_mux_addr_shift = EXYNOS4412_MUX_ADDR_SHIFT,
-	.therm_trip_mode_shift = EXYNOS_TMU_TRIP_MODE_SHIFT,
-	.therm_trip_mode_mask = EXYNOS_TMU_TRIP_MODE_MASK,
-	.therm_trip_en_shift = EXYNOS_TMU_THERM_TRIP_EN_SHIFT,
-	.tmu_status = EXYNOS_TMU_REG_STATUS,
-	.tmu_cur_temp = EXYNOS_TMU_REG_CURRENT_TEMP,
-	.threshold_th0 = EXYNOS_THD_TEMP_RISE,
-	.threshold_th1 = EXYNOS_THD_TEMP_FALL,
-	.tmu_inten = EXYNOS_TMU_REG_INTEN,
-	.inten_rise0_shift = EXYNOS_TMU_INTEN_RISE0_SHIFT,
-	.inten_rise1_shift = EXYNOS_TMU_INTEN_RISE1_SHIFT,
-	.inten_rise2_shift = EXYNOS_TMU_INTEN_RISE2_SHIFT,
-	.inten_fall0_shift = EXYNOS_TMU_INTEN_FALL0_SHIFT,
-	.tmu_intstat = EXYNOS_TMU_REG_INTSTAT,
-	.tmu_intclear = EXYNOS_TMU_REG_INTCLEAR,
-	.emul_con = EXYNOS_EMUL_CON,
-	.emul_temp_shift = EXYNOS_EMUL_DATA_SHIFT,
-	.emul_time_shift = EXYNOS_EMUL_TIME_SHIFT,
-};
 
 #define EXYNOS3250_TMU_DATA \
 	.threshold_falling = 10, \
@@ -144,54 +96,17 @@ static const struct exynos_tmu_registers exynos3250_tmu_registers = {
 		.freq_clip_max = 400 * 1000, \
 		.temp_level = 95, \
 	}, \
-	.freq_tab_count = 2, \
-	.triminfo_reload[0] = EXYNOS_TRIMINFO_RELOAD_ENABLE, \
-	.triminfo_reload[1] = EXYNOS_TRIMINFO_RELOAD_ENABLE, \
-	.registers = &exynos3250_tmu_registers, \
-	.features = (TMU_SUPPORT_EMULATION | TMU_SUPPORT_TRIM_RELOAD | \
-			TMU_SUPPORT_FALLING_TRIP | TMU_SUPPORT_READY_STATUS | \
-			TMU_SUPPORT_EMUL_TIME)
-#endif
+	.freq_tab_count = 2
 
-#if defined(CONFIG_SOC_EXYNOS3250)
 struct exynos_tmu_init_data const exynos3250_default_tmu_data = {
 	.tmu_data = {
 		{
 			EXYNOS3250_TMU_DATA,
 			.type = SOC_ARCH_EXYNOS3250,
-			.test_mux = EXYNOS4412_MUX_ADDR_VALUE,
 		},
 	},
 	.tmu_count = 1,
 };
-#endif
-
-#if defined(CONFIG_SOC_EXYNOS4412) || defined(CONFIG_SOC_EXYNOS5250)
-static const struct exynos_tmu_registers exynos4412_tmu_registers = {
-	.triminfo_data = EXYNOS_TMU_REG_TRIMINFO,
-	.triminfo_ctrl[0] = EXYNOS_TMU_TRIMINFO_CON2,
-	.triminfo_ctrl_count = 1,
-	.tmu_ctrl = EXYNOS_TMU_REG_CONTROL,
-	.test_mux_addr_shift = EXYNOS4412_MUX_ADDR_SHIFT,
-	.therm_trip_mode_shift = EXYNOS_TMU_TRIP_MODE_SHIFT,
-	.therm_trip_mode_mask = EXYNOS_TMU_TRIP_MODE_MASK,
-	.therm_trip_en_shift = EXYNOS_TMU_THERM_TRIP_EN_SHIFT,
-	.tmu_status = EXYNOS_TMU_REG_STATUS,
-	.tmu_cur_temp = EXYNOS_TMU_REG_CURRENT_TEMP,
-	.threshold_th0 = EXYNOS_THD_TEMP_RISE,
-	.threshold_th1 = EXYNOS_THD_TEMP_FALL,
-	.tmu_inten = EXYNOS_TMU_REG_INTEN,
-	.inten_rise0_shift = EXYNOS_TMU_INTEN_RISE0_SHIFT,
-	.inten_rise1_shift = EXYNOS_TMU_INTEN_RISE1_SHIFT,
-	.inten_rise2_shift = EXYNOS_TMU_INTEN_RISE2_SHIFT,
-	.inten_rise3_shift = EXYNOS_TMU_INTEN_RISE3_SHIFT,
-	.inten_fall0_shift = EXYNOS_TMU_INTEN_FALL0_SHIFT,
-	.tmu_intstat = EXYNOS_TMU_REG_INTSTAT,
-	.tmu_intclear = EXYNOS_TMU_REG_INTCLEAR,
-	.emul_con = EXYNOS_EMUL_CON,
-	.emul_temp_shift = EXYNOS_EMUL_DATA_SHIFT,
-	.emul_time_shift = EXYNOS_EMUL_TIME_SHIFT,
-};
 
 #define EXYNOS4412_TMU_DATA \
 	.threshold_falling = 10, \
@@ -227,28 +142,18 @@ static const struct exynos_tmu_registers exynos4412_tmu_registers = {
 		.freq_clip_max = 400 * 1000, \
 		.temp_level = 95, \
 	}, \
-	.freq_tab_count = 2, \
-	.triminfo_reload[0] = EXYNOS_TRIMINFO_RELOAD_ENABLE, \
-	.registers = &exynos4412_tmu_registers, \
-	.features = (TMU_SUPPORT_EMULATION | TMU_SUPPORT_TRIM_RELOAD | \
-			TMU_SUPPORT_FALLING_TRIP | TMU_SUPPORT_READY_STATUS | \
-			TMU_SUPPORT_EMUL_TIME)
-#endif
+	.freq_tab_count = 2
 
-#if defined(CONFIG_SOC_EXYNOS4412)
 struct exynos_tmu_init_data const exynos4412_default_tmu_data = {
 	.tmu_data = {
 		{
 			EXYNOS4412_TMU_DATA,
 			.type = SOC_ARCH_EXYNOS4412,
-			.test_mux = EXYNOS4412_MUX_ADDR_VALUE,
 		},
 	},
 	.tmu_count = 1,
 };
-#endif
 
-#if defined(CONFIG_SOC_EXYNOS5250)
 struct exynos_tmu_init_data const exynos5250_default_tmu_data = {
 	.tmu_data = {
 		{
@@ -258,31 +163,6 @@ struct exynos_tmu_init_data const exynos5250_default_tmu_data = {
 	},
 	.tmu_count = 1,
 };
-#endif
-
-#if defined(CONFIG_SOC_EXYNOS5260)
-static const struct exynos_tmu_registers exynos5260_tmu_registers = {
-	.triminfo_data = EXYNOS_TMU_REG_TRIMINFO,
-	.tmu_ctrl = EXYNOS_TMU_REG_CONTROL,
-	.therm_trip_mode_shift = EXYNOS_TMU_TRIP_MODE_SHIFT,
-	.therm_trip_mode_mask = EXYNOS_TMU_TRIP_MODE_MASK,
-	.therm_trip_en_shift = EXYNOS_TMU_THERM_TRIP_EN_SHIFT,
-	.tmu_status = EXYNOS_TMU_REG_STATUS,
-	.tmu_cur_temp = EXYNOS_TMU_REG_CURRENT_TEMP,
-	.threshold_th0 = EXYNOS_THD_TEMP_RISE,
-	.threshold_th1 = EXYNOS_THD_TEMP_FALL,
-	.tmu_inten = EXYNOS5260_TMU_REG_INTEN,
-	.inten_rise0_shift = EXYNOS_TMU_INTEN_RISE0_SHIFT,
-	.inten_rise1_shift = EXYNOS_TMU_INTEN_RISE1_SHIFT,
-	.inten_rise2_shift = EXYNOS_TMU_INTEN_RISE2_SHIFT,
-	.inten_rise3_shift = EXYNOS_TMU_INTEN_RISE3_SHIFT,
-	.inten_fall0_shift = EXYNOS_TMU_INTEN_FALL0_SHIFT,
-	.tmu_intstat = EXYNOS5260_TMU_REG_INTSTAT,
-	.tmu_intclear = EXYNOS5260_TMU_REG_INTCLEAR,
-	.emul_con = EXYNOS5260_EMUL_CON,
-	.emul_temp_shift = EXYNOS_EMUL_DATA_SHIFT,
-	.emul_time_shift = EXYNOS_EMUL_TIME_SHIFT,
-};
 
 #define __EXYNOS5260_TMU_DATA	\
 	.threshold_falling = 10, \
@@ -319,13 +199,10 @@ static const struct exynos_tmu_registers exynos5260_tmu_registers = {
 		.temp_level = 103, \
 	}, \
 	.freq_tab_count = 2, \
-	.registers = &exynos5260_tmu_registers, \
 
 #define EXYNOS5260_TMU_DATA \
 	__EXYNOS5260_TMU_DATA \
-	.type = SOC_ARCH_EXYNOS5260, \
-	.features = (TMU_SUPPORT_EMULATION | TMU_SUPPORT_FALLING_TRIP | \
-			TMU_SUPPORT_READY_STATUS | TMU_SUPPORT_EMUL_TIME)
+	.type = SOC_ARCH_EXYNOS5260
 
 struct exynos_tmu_init_data const exynos5260_default_tmu_data = {
 	.tmu_data = {
@@ -337,82 +214,14 @@ struct exynos_tmu_init_data const exynos5260_default_tmu_data = {
 	},
 	.tmu_count = 5,
 };
-#endif
-
-#if defined(CONFIG_SOC_EXYNOS5420)
-static const struct exynos_tmu_registers exynos5420_tmu_registers = {
-	.triminfo_data = EXYNOS_TMU_REG_TRIMINFO,
-	.tmu_ctrl = EXYNOS_TMU_REG_CONTROL,
-	.therm_trip_mode_shift = EXYNOS_TMU_TRIP_MODE_SHIFT,
-	.therm_trip_mode_mask = EXYNOS_TMU_TRIP_MODE_MASK,
-	.therm_trip_en_shift = EXYNOS_TMU_THERM_TRIP_EN_SHIFT,
-	.tmu_status = EXYNOS_TMU_REG_STATUS,
-	.tmu_cur_temp = EXYNOS_TMU_REG_CURRENT_TEMP,
-	.threshold_th0 = EXYNOS_THD_TEMP_RISE,
-	.threshold_th1 = EXYNOS_THD_TEMP_FALL,
-	.tmu_inten = EXYNOS_TMU_REG_INTEN,
-	.inten_rise0_shift = EXYNOS_TMU_INTEN_RISE0_SHIFT,
-	.inten_rise1_shift = EXYNOS_TMU_INTEN_RISE1_SHIFT,
-	.inten_rise2_shift = EXYNOS_TMU_INTEN_RISE2_SHIFT,
-	/* INTEN_RISE3 Not availble in exynos5420 */
-	.inten_rise3_shift = EXYNOS_TMU_INTEN_RISE3_SHIFT,
-	.inten_fall0_shift = EXYNOS_TMU_INTEN_FALL0_SHIFT,
-	.tmu_intstat = EXYNOS_TMU_REG_INTSTAT,
-	.tmu_intclear = EXYNOS_TMU_REG_INTCLEAR,
-	.emul_con = EXYNOS_EMUL_CON,
-	.emul_temp_shift = EXYNOS_EMUL_DATA_SHIFT,
-	.emul_time_shift = EXYNOS_EMUL_TIME_SHIFT,
-};
-
-#define __EXYNOS5420_TMU_DATA	\
-	.threshold_falling = 10, \
-	.trigger_levels[0] = 85, \
-	.trigger_levels[1] = 103, \
-	.trigger_levels[2] = 110, \
-	.trigger_levels[3] = 120, \
-	.trigger_enable[0] = true, \
-	.trigger_enable[1] = true, \
-	.trigger_enable[2] = true, \
-	.trigger_enable[3] = false, \
-	.trigger_type[0] = THROTTLE_ACTIVE, \
-	.trigger_type[1] = THROTTLE_ACTIVE, \
-	.trigger_type[2] = SW_TRIP, \
-	.trigger_type[3] = HW_TRIP, \
-	.max_trigger_level = 4, \
-	.non_hw_trigger_levels = 3, \
-	.gain = 8, \
-	.reference_voltage = 16, \
-	.noise_cancel_mode = 4, \
-	.cal_type = TYPE_ONE_POINT_TRIMMING, \
-	.efuse_value = 55, \
-	.min_efuse_value = 40, \
-	.max_efuse_value = 100, \
-	.first_point_trim = 25, \
-	.second_point_trim = 85, \
-	.default_temp_offset = 50, \
-	.freq_tab[0] = { \
-		.freq_clip_max = 800 * 1000, \
-		.temp_level = 85, \
-	}, \
-	.freq_tab[1] = { \
-		.freq_clip_max = 200 * 1000, \
-		.temp_level = 103, \
-	}, \
-	.freq_tab_count = 2, \
-	.registers = &exynos5420_tmu_registers, \
 
 #define EXYNOS5420_TMU_DATA \
-	__EXYNOS5420_TMU_DATA \
-	.type = SOC_ARCH_EXYNOS5250, \
-	.features = (TMU_SUPPORT_EMULATION | TMU_SUPPORT_FALLING_TRIP | \
-			TMU_SUPPORT_READY_STATUS | TMU_SUPPORT_EMUL_TIME)
+	__EXYNOS5260_TMU_DATA \
+	.type = SOC_ARCH_EXYNOS5420
 
 #define EXYNOS5420_TMU_DATA_SHARED \
-	__EXYNOS5420_TMU_DATA \
-	.type = SOC_ARCH_EXYNOS5420_TRIMINFO, \
-	.features = (TMU_SUPPORT_EMULATION | TMU_SUPPORT_FALLING_TRIP | \
-			TMU_SUPPORT_READY_STATUS | TMU_SUPPORT_EMUL_TIME | \
-			TMU_SUPPORT_ADDRESS_MULTIPLE)
+	__EXYNOS5260_TMU_DATA \
+	.type = SOC_ARCH_EXYNOS5420_TRIMINFO
 
 struct exynos_tmu_init_data const exynos5420_default_tmu_data = {
 	.tmu_data = {
@@ -424,34 +233,6 @@ struct exynos_tmu_init_data const exynos5420_default_tmu_data = {
 	},
 	.tmu_count = 5,
 };
-#endif
-
-#if defined(CONFIG_SOC_EXYNOS5440)
-static const struct exynos_tmu_registers exynos5440_tmu_registers = {
-	.triminfo_data = EXYNOS5440_TMU_S0_7_TRIM,
-	.tmu_ctrl = EXYNOS5440_TMU_S0_7_CTRL,
-	.therm_trip_mode_shift = EXYNOS_TMU_TRIP_MODE_SHIFT,
-	.therm_trip_mode_mask = EXYNOS_TMU_TRIP_MODE_MASK,
-	.therm_trip_en_shift = EXYNOS_TMU_THERM_TRIP_EN_SHIFT,
-	.tmu_status = EXYNOS5440_TMU_S0_7_STATUS,
-	.tmu_cur_temp = EXYNOS5440_TMU_S0_7_TEMP,
-	.threshold_th0 = EXYNOS5440_TMU_S0_7_TH0,
-	.threshold_th1 = EXYNOS5440_TMU_S0_7_TH1,
-	.threshold_th2 = EXYNOS5440_TMU_S0_7_TH2,
-	.threshold_th3_l0_shift = EXYNOS5440_TMU_TH_RISE4_SHIFT,
-	.tmu_inten = EXYNOS5440_TMU_S0_7_IRQEN,
-	.inten_rise0_shift = EXYNOS5440_TMU_INTEN_RISE0_SHIFT,
-	.inten_rise1_shift = EXYNOS5440_TMU_INTEN_RISE1_SHIFT,
-	.inten_rise2_shift = EXYNOS5440_TMU_INTEN_RISE2_SHIFT,
-	.inten_rise3_shift = EXYNOS5440_TMU_INTEN_RISE3_SHIFT,
-	.inten_fall0_shift = EXYNOS5440_TMU_INTEN_FALL0_SHIFT,
-	.tmu_intstat = EXYNOS5440_TMU_S0_7_IRQ,
-	.tmu_intclear = EXYNOS5440_TMU_S0_7_IRQ,
-	.tmu_irqstatus = EXYNOS5440_TMU_IRQ_STATUS,
-	.emul_con = EXYNOS5440_TMU_S0_7_DEBUG,
-	.emul_temp_shift = EXYNOS_EMUL_DATA_SHIFT,
-	.tmu_pmin = EXYNOS5440_TMU_PMIN,
-};
 
 #define EXYNOS5440_TMU_DATA \
 	.trigger_levels[0] = 100, \
@@ -471,10 +252,7 @@ static const struct exynos_tmu_registers exynos5440_tmu_registers = {
 	.first_point_trim = 25, \
 	.second_point_trim = 70, \
 	.default_temp_offset = 25, \
-	.type = SOC_ARCH_EXYNOS5440, \
-	.registers = &exynos5440_tmu_registers, \
-	.features = (TMU_SUPPORT_EMULATION | TMU_SUPPORT_FALLING_TRIP | \
-			TMU_SUPPORT_MULTI_INST | TMU_SUPPORT_ADDRESS_MULTIPLE),
+	.type = SOC_ARCH_EXYNOS5440
 
 struct exynos_tmu_init_data const exynos5440_default_tmu_data = {
 	.tmu_data = {
@@ -484,4 +262,3 @@ struct exynos_tmu_init_data const exynos5440_default_tmu_data = {
 	},
 	.tmu_count = 3,
 };
-#endif
diff --git a/drivers/thermal/samsung/exynos_tmu_data.h b/drivers/thermal/samsung/exynos_tmu_data.h
deleted file mode 100644
index 63de598c9c2c..000000000000
--- a/drivers/thermal/samsung/exynos_tmu_data.h
+++ /dev/null
@@ -1,159 +0,0 @@
-/*
- * exynos_tmu_data.h - Samsung EXYNOS tmu data header file
- *
- *  Copyright (C) 2013 Samsung Electronics
- *  Amit Daniel Kachhap <amit.daniel@samsung.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- *
- */
-
-#ifndef _EXYNOS_TMU_DATA_H
-#define _EXYNOS_TMU_DATA_H
-
-/* Exynos generic registers */
-#define EXYNOS_TMU_REG_TRIMINFO		0x0
-#define EXYNOS_TMU_REG_CONTROL		0x20
-#define EXYNOS_TMU_REG_STATUS		0x28
-#define EXYNOS_TMU_REG_CURRENT_TEMP	0x40
-#define EXYNOS_TMU_REG_INTEN		0x70
-#define EXYNOS_TMU_REG_INTSTAT		0x74
-#define EXYNOS_TMU_REG_INTCLEAR		0x78
-
-#define EXYNOS_TMU_TEMP_MASK		0xff
-#define EXYNOS_TMU_REF_VOLTAGE_SHIFT	24
-#define EXYNOS_TMU_REF_VOLTAGE_MASK	0x1f
-#define EXYNOS_TMU_BUF_SLOPE_SEL_MASK	0xf
-#define EXYNOS_TMU_BUF_SLOPE_SEL_SHIFT	8
-#define EXYNOS_TMU_CORE_EN_SHIFT	0
-
-/* Exynos3250 specific registers */
-#define EXYNOS_TMU_TRIMINFO_CON1	0x10
-
-/* Exynos4210 specific registers */
-#define EXYNOS4210_TMU_REG_THRESHOLD_TEMP	0x44
-#define EXYNOS4210_TMU_REG_TRIG_LEVEL0	0x50
-
-/* Exynos5250, Exynos4412, Exynos3250 specific registers */
-#define EXYNOS_TMU_TRIMINFO_CON2	0x14
-#define EXYNOS_THD_TEMP_RISE		0x50
-#define EXYNOS_THD_TEMP_FALL		0x54
-#define EXYNOS_EMUL_CON		0x80
-
-#define EXYNOS_TRIMINFO_RELOAD_ENABLE	1
-#define EXYNOS_TRIMINFO_25_SHIFT	0
-#define EXYNOS_TRIMINFO_85_SHIFT	8
-#define EXYNOS_TMU_TRIP_MODE_SHIFT	13
-#define EXYNOS_TMU_TRIP_MODE_MASK	0x7
-#define EXYNOS_TMU_THERM_TRIP_EN_SHIFT	12
-
-#define EXYNOS_TMU_INTEN_RISE0_SHIFT	0
-#define EXYNOS_TMU_INTEN_RISE1_SHIFT	4
-#define EXYNOS_TMU_INTEN_RISE2_SHIFT	8
-#define EXYNOS_TMU_INTEN_RISE3_SHIFT	12
-#define EXYNOS_TMU_INTEN_FALL0_SHIFT	16
-
-#define EXYNOS_EMUL_TIME	0x57F0
-#define EXYNOS_EMUL_TIME_MASK	0xffff
-#define EXYNOS_EMUL_TIME_SHIFT	16
-#define EXYNOS_EMUL_DATA_SHIFT	8
-#define EXYNOS_EMUL_DATA_MASK	0xFF
-#define EXYNOS_EMUL_ENABLE	0x1
-
-#define EXYNOS_MAX_TRIGGER_PER_REG	4
-
-/* Exynos5260 specific */
-#define EXYNOS5260_TMU_REG_INTEN		0xC0
-#define EXYNOS5260_TMU_REG_INTSTAT		0xC4
-#define EXYNOS5260_TMU_REG_INTCLEAR		0xC8
-#define EXYNOS5260_EMUL_CON			0x100
-
-/* Exynos4412 specific */
-#define EXYNOS4412_MUX_ADDR_VALUE          6
-#define EXYNOS4412_MUX_ADDR_SHIFT          20
-
-/*exynos5440 specific registers*/
-#define EXYNOS5440_TMU_S0_7_TRIM		0x000
-#define EXYNOS5440_TMU_S0_7_CTRL		0x020
-#define EXYNOS5440_TMU_S0_7_DEBUG		0x040
-#define EXYNOS5440_TMU_S0_7_STATUS		0x060
-#define EXYNOS5440_TMU_S0_7_TEMP		0x0f0
-#define EXYNOS5440_TMU_S0_7_TH0			0x110
-#define EXYNOS5440_TMU_S0_7_TH1			0x130
-#define EXYNOS5440_TMU_S0_7_TH2			0x150
-#define EXYNOS5440_TMU_S0_7_IRQEN		0x210
-#define EXYNOS5440_TMU_S0_7_IRQ			0x230
-/* exynos5440 common registers */
-#define EXYNOS5440_TMU_IRQ_STATUS		0x000
-#define EXYNOS5440_TMU_PMIN			0x004
-
-#define EXYNOS5440_TMU_INTEN_RISE0_SHIFT	0
-#define EXYNOS5440_TMU_INTEN_RISE1_SHIFT	1
-#define EXYNOS5440_TMU_INTEN_RISE2_SHIFT	2
-#define EXYNOS5440_TMU_INTEN_RISE3_SHIFT	3
-#define EXYNOS5440_TMU_INTEN_FALL0_SHIFT	4
-#define EXYNOS5440_TMU_TH_RISE4_SHIFT		24
-#define EXYNOS5440_EFUSE_SWAP_OFFSET		8
-
-#if defined(CONFIG_SOC_EXYNOS3250)
-extern struct exynos_tmu_init_data const exynos3250_default_tmu_data;
-#define EXYNOS3250_TMU_DRV_DATA (&exynos3250_default_tmu_data)
-#else
-#define EXYNOS3250_TMU_DRV_DATA (NULL)
-#endif
-
-#if defined(CONFIG_CPU_EXYNOS4210)
-extern struct exynos_tmu_init_data const exynos4210_default_tmu_data;
-#define EXYNOS4210_TMU_DRV_DATA (&exynos4210_default_tmu_data)
-#else
-#define EXYNOS4210_TMU_DRV_DATA (NULL)
-#endif
-
-#if defined(CONFIG_SOC_EXYNOS4412)
-extern struct exynos_tmu_init_data const exynos4412_default_tmu_data;
-#define EXYNOS4412_TMU_DRV_DATA (&exynos4412_default_tmu_data)
-#else
-#define EXYNOS4412_TMU_DRV_DATA (NULL)
-#endif
-
-#if defined(CONFIG_SOC_EXYNOS5250)
-extern struct exynos_tmu_init_data const exynos5250_default_tmu_data;
-#define EXYNOS5250_TMU_DRV_DATA (&exynos5250_default_tmu_data)
-#else
-#define EXYNOS5250_TMU_DRV_DATA (NULL)
-#endif
-
-#if defined(CONFIG_SOC_EXYNOS5260)
-extern struct exynos_tmu_init_data const exynos5260_default_tmu_data;
-#define EXYNOS5260_TMU_DRV_DATA (&exynos5260_default_tmu_data)
-#else
-#define EXYNOS5260_TMU_DRV_DATA (NULL)
-#endif
-
-#if defined(CONFIG_SOC_EXYNOS5420)
-extern struct exynos_tmu_init_data const exynos5420_default_tmu_data;
-#define EXYNOS5420_TMU_DRV_DATA (&exynos5420_default_tmu_data)
-#else
-#define EXYNOS5420_TMU_DRV_DATA (NULL)
-#endif
-
-#if defined(CONFIG_SOC_EXYNOS5440)
-extern struct exynos_tmu_init_data const exynos5440_default_tmu_data;
-#define EXYNOS5440_TMU_DRV_DATA (&exynos5440_default_tmu_data)
-#else
-#define EXYNOS5440_TMU_DRV_DATA (NULL)
-#endif
-
-#endif /*_EXYNOS_TMU_DATA_H*/
diff --git a/drivers/thermal/spear_thermal.c b/drivers/thermal/spear_thermal.c
index 1e2193fc3241..bddb71744a6c 100644
--- a/drivers/thermal/spear_thermal.c
+++ b/drivers/thermal/spear_thermal.c
@@ -186,7 +186,6 @@ static struct platform_driver spear_thermal_driver = {
 	.remove = spear_thermal_exit,
 	.driver = {
 		.name = "spear_thermal",
-		.owner = THIS_MODULE,
 		.pm = &spear_thermal_pm_ops,
 		.of_match_table = spear_thermal_id_table,
 	},
diff --git a/drivers/thermal/st/st_thermal_memmap.c b/drivers/thermal/st/st_thermal_memmap.c
index 39896ce2ee00..067bfcdb91d6 100644
--- a/drivers/thermal/st/st_thermal_memmap.c
+++ b/drivers/thermal/st/st_thermal_memmap.c
@@ -194,7 +194,6 @@ int st_mmap_remove(struct platform_device *pdev)
 static struct platform_driver st_mmap_thermal_driver = {
 	.driver = {
 		.name	= "st_thermal_mmap",
-		.owner  = THIS_MODULE,
 		.pm     = &st_thermal_pm_ops,
 		.of_match_table = st_mmap_thermal_of_match,
 	},
diff --git a/drivers/thermal/st/st_thermal_syscfg.c b/drivers/thermal/st/st_thermal_syscfg.c
index 888b58e64090..26d36a242bb8 100644
--- a/drivers/thermal/st/st_thermal_syscfg.c
+++ b/drivers/thermal/st/st_thermal_syscfg.c
@@ -165,7 +165,6 @@ int st_syscfg_remove(struct platform_device *pdev)
 static struct platform_driver st_syscfg_thermal_driver = {
 	.driver = {
 		.name	= "st_syscfg_thermal",
-		.owner  = THIS_MODULE,
 		.pm     = &st_thermal_pm_ops,
 		.of_match_table =  st_syscfg_thermal_of_match,
 	},
diff --git a/drivers/thermal/tegra_soctherm.c b/drivers/thermal/tegra_soctherm.c
new file mode 100644
index 000000000000..9197fc05c5cc
--- /dev/null
+++ b/drivers/thermal/tegra_soctherm.c
@@ -0,0 +1,476 @@
+/*
+ * Copyright (c) 2014, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Author:
+ *	Mikko Perttunen <mperttunen@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/bitops.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/reset.h>
+#include <linux/thermal.h>
+
+#include <soc/tegra/fuse.h>
+
+#define SENSOR_CONFIG0				0
+#define SENSOR_CONFIG0_STOP			BIT(0)
+#define SENSOR_CONFIG0_TALL_SHIFT		8
+#define SENSOR_CONFIG0_TCALC_OVER		BIT(4)
+#define SENSOR_CONFIG0_OVER			BIT(3)
+#define SENSOR_CONFIG0_CPTR_OVER		BIT(2)
+
+#define SENSOR_CONFIG1				4
+#define SENSOR_CONFIG1_TSAMPLE_SHIFT		0
+#define SENSOR_CONFIG1_TIDDQ_EN_SHIFT		15
+#define SENSOR_CONFIG1_TEN_COUNT_SHIFT		24
+#define SENSOR_CONFIG1_TEMP_ENABLE		BIT(31)
+
+#define SENSOR_CONFIG2				8
+#define SENSOR_CONFIG2_THERMA_SHIFT		16
+#define SENSOR_CONFIG2_THERMB_SHIFT		0
+
+#define SENSOR_PDIV				0x1c0
+#define SENSOR_PDIV_T124			0x8888
+#define SENSOR_HOTSPOT_OFF			0x1c4
+#define SENSOR_HOTSPOT_OFF_T124			0x00060600
+#define SENSOR_TEMP1				0x1c8
+#define SENSOR_TEMP2				0x1cc
+
+#define SENSOR_TEMP_MASK			0xffff
+#define READBACK_VALUE_MASK			0xff00
+#define READBACK_VALUE_SHIFT			8
+#define READBACK_ADD_HALF			BIT(7)
+#define READBACK_NEGATE				BIT(1)
+
+#define FUSE_TSENSOR8_CALIB			0x180
+#define FUSE_SPARE_REALIGNMENT_REG_0		0x1fc
+
+#define FUSE_TSENSOR_CALIB_CP_TS_BASE_MASK	0x1fff
+#define FUSE_TSENSOR_CALIB_FT_TS_BASE_MASK	(0x1fff << 13)
+#define FUSE_TSENSOR_CALIB_FT_TS_BASE_SHIFT	13
+
+#define FUSE_TSENSOR8_CALIB_CP_TS_BASE_MASK	0x3ff
+#define FUSE_TSENSOR8_CALIB_FT_TS_BASE_MASK	(0x7ff << 10)
+#define FUSE_TSENSOR8_CALIB_FT_TS_BASE_SHIFT	10
+
+#define FUSE_SPARE_REALIGNMENT_REG_SHIFT_CP_MASK 0x3f
+#define FUSE_SPARE_REALIGNMENT_REG_SHIFT_FT_MASK (0x1f << 21)
+#define FUSE_SPARE_REALIGNMENT_REG_SHIFT_FT_SHIFT 21
+
+#define NOMINAL_CALIB_FT_T124			105
+#define NOMINAL_CALIB_CP_T124			25
+
+struct tegra_tsensor_configuration {
+	u32 tall, tsample, tiddq_en, ten_count, pdiv, tsample_ate, pdiv_ate;
+};
+
+struct tegra_tsensor {
+	const struct tegra_tsensor_configuration *config;
+	u32 base, calib_fuse_offset;
+	/* Correction values used to modify values read from calibration fuses */
+	s32 fuse_corr_alpha, fuse_corr_beta;
+};
+
+struct tegra_thermctl_zone {
+	void __iomem *reg;
+	unsigned int shift;
+};
+
+static const struct tegra_tsensor_configuration t124_tsensor_config = {
+	.tall = 16300,
+	.tsample = 120,
+	.tiddq_en = 1,
+	.ten_count = 1,
+	.pdiv = 8,
+	.tsample_ate = 480,
+	.pdiv_ate = 8
+};
+
+static const struct tegra_tsensor t124_tsensors[] = {
+	{
+		.config = &t124_tsensor_config,
+		.base = 0xc0,
+		.calib_fuse_offset = 0x098,
+		.fuse_corr_alpha = 1135400,
+		.fuse_corr_beta = -6266900,
+	},
+	{
+		.config = &t124_tsensor_config,
+		.base = 0xe0,
+		.calib_fuse_offset = 0x084,
+		.fuse_corr_alpha = 1122220,
+		.fuse_corr_beta = -5700700,
+	},
+	{
+		.config = &t124_tsensor_config,
+		.base = 0x100,
+		.calib_fuse_offset = 0x088,
+		.fuse_corr_alpha = 1127000,
+		.fuse_corr_beta = -6768200,
+	},
+	{
+		.config = &t124_tsensor_config,
+		.base = 0x120,
+		.calib_fuse_offset = 0x12c,
+		.fuse_corr_alpha = 1110900,
+		.fuse_corr_beta = -6232000,
+	},
+	{
+		.config = &t124_tsensor_config,
+		.base = 0x140,
+		.calib_fuse_offset = 0x158,
+		.fuse_corr_alpha = 1122300,
+		.fuse_corr_beta = -5936400,
+	},
+	{
+		.config = &t124_tsensor_config,
+		.base = 0x160,
+		.calib_fuse_offset = 0x15c,
+		.fuse_corr_alpha = 1145700,
+		.fuse_corr_beta = -7124600,
+	},
+	{
+		.config = &t124_tsensor_config,
+		.base = 0x180,
+		.calib_fuse_offset = 0x154,
+		.fuse_corr_alpha = 1120100,
+		.fuse_corr_beta = -6000500,
+	},
+	{
+		.config = &t124_tsensor_config,
+		.base = 0x1a0,
+		.calib_fuse_offset = 0x160,
+		.fuse_corr_alpha = 1106500,
+		.fuse_corr_beta = -6729300,
+	},
+};
+
+struct tegra_soctherm {
+	struct reset_control *reset;
+	struct clk *clock_tsensor;
+	struct clk *clock_soctherm;
+	void __iomem *regs;
+
+	struct thermal_zone_device *thermctl_tzs[4];
+};
+
+struct tsensor_shared_calibration {
+	u32 base_cp, base_ft;
+	u32 actual_temp_cp, actual_temp_ft;
+};
+
+static int calculate_shared_calibration(struct tsensor_shared_calibration *r)
+{
+	u32 val, shifted_cp, shifted_ft;
+	int err;
+
+	err = tegra_fuse_readl(FUSE_TSENSOR8_CALIB, &val);
+	if (err)
+		return err;
+	r->base_cp = val & FUSE_TSENSOR8_CALIB_CP_TS_BASE_MASK;
+	r->base_ft = (val & FUSE_TSENSOR8_CALIB_FT_TS_BASE_MASK)
+		>> FUSE_TSENSOR8_CALIB_FT_TS_BASE_SHIFT;
+	val = ((val & FUSE_SPARE_REALIGNMENT_REG_SHIFT_FT_MASK)
+		>> FUSE_SPARE_REALIGNMENT_REG_SHIFT_FT_SHIFT);
+	shifted_ft = sign_extend32(val, 4);
+
+	err = tegra_fuse_readl(FUSE_SPARE_REALIGNMENT_REG_0, &val);
+	if (err)
+		return err;
+	shifted_cp = sign_extend32(val, 5);
+
+	r->actual_temp_cp = 2 * NOMINAL_CALIB_CP_T124 + shifted_cp;
+	r->actual_temp_ft = 2 * NOMINAL_CALIB_FT_T124 + shifted_ft;
+
+	return 0;
+}
+
+static s64 div64_s64_precise(s64 a, s64 b)
+{
+	s64 r, al;
+
+	/* Scale up for increased precision division */
+	al = a << 16;
+
+	r = div64_s64(al * 2 + 1, 2 * b);
+	return r >> 16;
+}
+
+static int
+calculate_tsensor_calibration(const struct tegra_tsensor *sensor,
+			      const struct tsensor_shared_calibration *shared,
+			      u32 *calib)
+{
+	u32 val;
+	s32 actual_tsensor_ft, actual_tsensor_cp, delta_sens, delta_temp,
+	    mult, div;
+	s16 therma, thermb;
+	s64 tmp;
+	int err;
+
+	err = tegra_fuse_readl(sensor->calib_fuse_offset, &val);
+	if (err)
+		return err;
+
+	actual_tsensor_cp = (shared->base_cp * 64) + sign_extend32(val, 12);
+	val = (val & FUSE_TSENSOR_CALIB_FT_TS_BASE_MASK)
+		>> FUSE_TSENSOR_CALIB_FT_TS_BASE_SHIFT;
+	actual_tsensor_ft = (shared->base_ft * 32) + sign_extend32(val, 12);
+
+	delta_sens = actual_tsensor_ft - actual_tsensor_cp;
+	delta_temp = shared->actual_temp_ft - shared->actual_temp_cp;
+
+	mult = sensor->config->pdiv * sensor->config->tsample_ate;
+	div = sensor->config->tsample * sensor->config->pdiv_ate;
+
+	therma = div64_s64_precise((s64) delta_temp * (1LL << 13) * mult,
+				   (s64) delta_sens * div);
+
+	tmp = (s64)actual_tsensor_ft * shared->actual_temp_cp -
+	      (s64)actual_tsensor_cp * shared->actual_temp_ft;
+	thermb = div64_s64_precise(tmp, (s64)delta_sens);
+
+	therma = div64_s64_precise((s64)therma * sensor->fuse_corr_alpha,
+				   (s64)1000000LL);
+	thermb = div64_s64_precise((s64)thermb * sensor->fuse_corr_alpha +
+				   sensor->fuse_corr_beta, (s64)1000000LL);
+
+	*calib = ((u16)therma << SENSOR_CONFIG2_THERMA_SHIFT) |
+		 ((u16)thermb << SENSOR_CONFIG2_THERMB_SHIFT);
+
+	return 0;
+}
+
+static int enable_tsensor(struct tegra_soctherm *tegra,
+			  const struct tegra_tsensor *sensor,
+			  const struct tsensor_shared_calibration *shared)
+{
+	void __iomem *base = tegra->regs + sensor->base;
+	unsigned int val;
+	u32 calib;
+	int err;
+
+	err = calculate_tsensor_calibration(sensor, shared, &calib);
+	if (err)
+		return err;
+
+	val = sensor->config->tall << SENSOR_CONFIG0_TALL_SHIFT;
+	writel(val, base + SENSOR_CONFIG0);
+
+	val  = (sensor->config->tsample - 1) << SENSOR_CONFIG1_TSAMPLE_SHIFT;
+	val |= sensor->config->tiddq_en << SENSOR_CONFIG1_TIDDQ_EN_SHIFT;
+	val |= sensor->config->ten_count << SENSOR_CONFIG1_TEN_COUNT_SHIFT;
+	val |= SENSOR_CONFIG1_TEMP_ENABLE;
+	writel(val, base + SENSOR_CONFIG1);
+
+	writel(calib, base + SENSOR_CONFIG2);
+
+	return 0;
+}
+
+/*
+ * Translate from soctherm readback format to millicelsius.
+ * The soctherm readback format in bits is as follows:
+ *   TTTTTTTT H______N
+ * where T's contain the temperature in Celsius,
+ * H denotes an addition of 0.5 Celsius and N denotes negation
+ * of the final value.
+ */
+static long translate_temp(u16 val)
+{
+	long t;
+
+	t = ((val & READBACK_VALUE_MASK) >> READBACK_VALUE_SHIFT) * 1000;
+	if (val & READBACK_ADD_HALF)
+		t += 500;
+	if (val & READBACK_NEGATE)
+		t *= -1;
+
+	return t;
+}
+
+static int tegra_thermctl_get_temp(void *data, long *out_temp)
+{
+	struct tegra_thermctl_zone *zone = data;
+	u32 val;
+
+	val = (readl(zone->reg) >> zone->shift) & SENSOR_TEMP_MASK;
+	*out_temp = translate_temp(val);
+
+	return 0;
+}
+
+static const struct thermal_zone_of_device_ops tegra_of_thermal_ops = {
+	.get_temp = tegra_thermctl_get_temp,
+};
+
+static const struct of_device_id tegra_soctherm_of_match[] = {
+	{ .compatible = "nvidia,tegra124-soctherm" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, tegra_soctherm_of_match);
+
+struct thermctl_zone_desc {
+	unsigned int offset;
+	unsigned int shift;
+};
+
+static const struct thermctl_zone_desc t124_thermctl_temp_zones[] = {
+	{ SENSOR_TEMP1, 16 },
+	{ SENSOR_TEMP2, 16 },
+	{ SENSOR_TEMP1, 0 },
+	{ SENSOR_TEMP2, 0 }
+};
+
+static int tegra_soctherm_probe(struct platform_device *pdev)
+{
+	struct tegra_soctherm *tegra;
+	struct thermal_zone_device *tz;
+	struct tsensor_shared_calibration shared_calib;
+	struct resource *res;
+	unsigned int i;
+	int err;
+
+	const struct tegra_tsensor *tsensors = t124_tsensors;
+
+	tegra = devm_kzalloc(&pdev->dev, sizeof(*tegra), GFP_KERNEL);
+	if (!tegra)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	tegra->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(tegra->regs))
+		return PTR_ERR(tegra->regs);
+
+	tegra->reset = devm_reset_control_get(&pdev->dev, "soctherm");
+	if (IS_ERR(tegra->reset)) {
+		dev_err(&pdev->dev, "can't get soctherm reset\n");
+		return PTR_ERR(tegra->reset);
+	}
+
+	tegra->clock_tsensor = devm_clk_get(&pdev->dev, "tsensor");
+	if (IS_ERR(tegra->clock_tsensor)) {
+		dev_err(&pdev->dev, "can't get tsensor clock\n");
+		return PTR_ERR(tegra->clock_tsensor);
+	}
+
+	tegra->clock_soctherm = devm_clk_get(&pdev->dev, "soctherm");
+	if (IS_ERR(tegra->clock_soctherm)) {
+		dev_err(&pdev->dev, "can't get soctherm clock\n");
+		return PTR_ERR(tegra->clock_soctherm);
+	}
+
+	reset_control_assert(tegra->reset);
+
+	err = clk_prepare_enable(tegra->clock_soctherm);
+	if (err)
+		return err;
+
+	err = clk_prepare_enable(tegra->clock_tsensor);
+	if (err) {
+		clk_disable_unprepare(tegra->clock_soctherm);
+		return err;
+	}
+
+	reset_control_deassert(tegra->reset);
+
+	/* Initialize raw sensors */
+
+	err = calculate_shared_calibration(&shared_calib);
+	if (err)
+		goto disable_clocks;
+
+	for (i = 0; i < ARRAY_SIZE(t124_tsensors); ++i) {
+		err = enable_tsensor(tegra, tsensors + i, &shared_calib);
+		if (err)
+			goto disable_clocks;
+	}
+
+	writel(SENSOR_PDIV_T124, tegra->regs + SENSOR_PDIV);
+	writel(SENSOR_HOTSPOT_OFF_T124, tegra->regs + SENSOR_HOTSPOT_OFF);
+
+	/* Initialize thermctl sensors */
+
+	for (i = 0; i < ARRAY_SIZE(tegra->thermctl_tzs); ++i) {
+		struct tegra_thermctl_zone *zone =
+			devm_kzalloc(&pdev->dev, sizeof(*zone), GFP_KERNEL);
+		if (!zone) {
+			err = -ENOMEM;
+			goto unregister_tzs;
+		}
+
+		zone->reg = tegra->regs + t124_thermctl_temp_zones[i].offset;
+		zone->shift = t124_thermctl_temp_zones[i].shift;
+
+		tz = thermal_zone_of_sensor_register(&pdev->dev, i, zone,
+						     &tegra_of_thermal_ops);
+		if (IS_ERR(tz)) {
+			err = PTR_ERR(tz);
+			dev_err(&pdev->dev, "failed to register sensor: %d\n",
+				err);
+			goto unregister_tzs;
+		}
+
+		tegra->thermctl_tzs[i] = tz;
+	}
+
+	return 0;
+
+unregister_tzs:
+	while (i--)
+		thermal_zone_of_sensor_unregister(&pdev->dev,
+						  tegra->thermctl_tzs[i]);
+
+disable_clocks:
+	clk_disable_unprepare(tegra->clock_tsensor);
+	clk_disable_unprepare(tegra->clock_soctherm);
+
+	return err;
+}
+
+static int tegra_soctherm_remove(struct platform_device *pdev)
+{
+	struct tegra_soctherm *tegra = platform_get_drvdata(pdev);
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(tegra->thermctl_tzs); ++i) {
+		thermal_zone_of_sensor_unregister(&pdev->dev,
+						  tegra->thermctl_tzs[i]);
+	}
+
+	clk_disable_unprepare(tegra->clock_tsensor);
+	clk_disable_unprepare(tegra->clock_soctherm);
+
+	return 0;
+}
+
+static struct platform_driver tegra_soctherm_driver = {
+	.probe = tegra_soctherm_probe,
+	.remove = tegra_soctherm_remove,
+	.driver = {
+		.name = "tegra-soctherm",
+		.of_match_table = tegra_soctherm_of_match,
+	},
+};
+module_platform_driver(tegra_soctherm_driver);
+
+MODULE_AUTHOR("Mikko Perttunen <mperttunen@nvidia.com>");
+MODULE_DESCRIPTION("NVIDIA Tegra SOCTHERM thermal management driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/thermal/thermal_core.c b/drivers/thermal/thermal_core.c
index 43b90709585f..87e0b0782023 100644
--- a/drivers/thermal/thermal_core.c
+++ b/drivers/thermal/thermal_core.c
@@ -368,7 +368,7 @@ static void handle_critical_trips(struct thermal_zone_device *tz,
 	tz->ops->get_trip_temp(tz, trip, &trip_temp);
 
 	/* If we have not crossed the trip_temp, we do not care. */
-	if (tz->temperature < trip_temp)
+	if (trip_temp <= 0 || tz->temperature < trip_temp)
 		return;
 
 	trace_thermal_zone_trip(tz, trip, trip_type);
@@ -757,6 +757,7 @@ policy_store(struct device *dev, struct device_attribute *attr,
 	snprintf(name, sizeof(name), "%s", buf);
 
 	mutex_lock(&thermal_governor_lock);
+	mutex_lock(&tz->lock);
 
 	gov = __find_governor(strim(name));
 	if (!gov)
@@ -766,6 +767,7 @@ policy_store(struct device *dev, struct device_attribute *attr,
 	ret = count;
 
 exit:
+	mutex_unlock(&tz->lock);
 	mutex_unlock(&thermal_governor_lock);
 	return ret;
 }
@@ -928,7 +930,7 @@ int thermal_zone_bind_cooling_device(struct thermal_zone_device *tz,
 	struct thermal_zone_device *pos1;
 	struct thermal_cooling_device *pos2;
 	unsigned long max_state;
-	int result;
+	int result, ret;
 
 	if (trip >= tz->trips || (trip < 0 && trip != THERMAL_TRIPS_NONE))
 		return -EINVAL;
@@ -945,7 +947,9 @@ int thermal_zone_bind_cooling_device(struct thermal_zone_device *tz,
 	if (tz != pos1 || cdev != pos2)
 		return -EINVAL;
 
-	cdev->ops->get_max_state(cdev, &max_state);
+	ret = cdev->ops->get_max_state(cdev, &max_state);
+	if (ret)
+		return ret;
 
 	/* lower default 0, upper default max_state */
 	lower = lower == THERMAL_NO_LIMIT ? 0 : lower;
@@ -1835,10 +1839,10 @@ static int __init thermal_init(void)
 
 exit_netlink:
 	genetlink_exit();
-unregister_governors:
-	thermal_unregister_governors();
 unregister_class:
 	class_unregister(&thermal_class);
+unregister_governors:
+	thermal_unregister_governors();
 error:
 	idr_destroy(&thermal_tz_idr);
 	idr_destroy(&thermal_cdev_idr);
diff --git a/drivers/thermal/thermal_core.h b/drivers/thermal/thermal_core.h
index d15d243de27a..9083e7520623 100644
--- a/drivers/thermal/thermal_core.h
+++ b/drivers/thermal/thermal_core.h
@@ -89,9 +89,27 @@ static inline void thermal_gov_user_space_unregister(void) {}
 #ifdef CONFIG_THERMAL_OF
 int of_parse_thermal_zones(void);
 void of_thermal_destroy_zones(void);
+int of_thermal_get_ntrips(struct thermal_zone_device *);
+bool of_thermal_is_trip_valid(struct thermal_zone_device *, int);
+const struct thermal_trip * const
+of_thermal_get_trip_points(struct thermal_zone_device *);
 #else
 static inline int of_parse_thermal_zones(void) { return 0; }
 static inline void of_thermal_destroy_zones(void) { }
+static inline int of_thermal_get_ntrips(struct thermal_zone_device *tz)
+{
+	return 0;
+}
+static inline bool of_thermal_is_trip_valid(struct thermal_zone_device *tz,
+					    int trip)
+{
+	return 0;
+}
+static inline const struct thermal_trip * const
+of_thermal_get_trip_points(struct thermal_zone_device *tz)
+{
+	return NULL;
+}
 #endif
 
 #endif /* __THERMAL_CORE_H__ */
diff --git a/drivers/thermal/ti-soc-thermal/ti-thermal-common.c b/drivers/thermal/ti-soc-thermal/ti-thermal-common.c
index 9eec26dc0448..3fb054a10f6a 100644
--- a/drivers/thermal/ti-soc-thermal/ti-thermal-common.c
+++ b/drivers/thermal/ti-soc-thermal/ti-thermal-common.c
@@ -28,7 +28,6 @@
 #include <linux/kernel.h>
 #include <linux/workqueue.h>
 #include <linux/thermal.h>
-#include <linux/cpufreq.h>
 #include <linux/cpumask.h>
 #include <linux/cpu_cooling.h>
 #include <linux/of.h>
@@ -286,6 +285,11 @@ static int ti_thermal_get_crit_temp(struct thermal_zone_device *thermal,
 	return ti_thermal_get_trip_temp(thermal, OMAP_TRIP_NUMBER - 1, temp);
 }
 
+static const struct thermal_zone_of_device_ops ti_of_thermal_ops = {
+	.get_temp = __ti_thermal_get_temp,
+	.get_trend = __ti_thermal_get_trend,
+};
+
 static struct thermal_zone_device_ops ti_thermal_ops = {
 	.get_temp = ti_thermal_get_temp,
 	.get_trend = ti_thermal_get_trend,
@@ -333,8 +337,7 @@ int ti_thermal_expose_sensor(struct ti_bandgap *bgp, int id,
 
 	/* in case this is specified by DT */
 	data->ti_thermal = thermal_zone_of_sensor_register(bgp->dev, id,
-					data, __ti_thermal_get_temp,
-					__ti_thermal_get_trend);
+					data, &ti_of_thermal_ops);
 	if (IS_ERR(data->ti_thermal)) {
 		/* Create thermal zone */
 		data->ti_thermal = thermal_zone_device_register(domain,
@@ -403,17 +406,17 @@ int ti_thermal_register_cpu_cooling(struct ti_bandgap *bgp, int id)
 	if (!data)
 		return -EINVAL;
 
-	if (!cpufreq_get_current_driver()) {
-		dev_dbg(bgp->dev, "no cpufreq driver yet\n");
-		return -EPROBE_DEFER;
-	}
-
 	/* Register cooling device */
 	data->cool_dev = cpufreq_cooling_register(cpu_present_mask);
 	if (IS_ERR(data->cool_dev)) {
-		dev_err(bgp->dev,
-			"Failed to register cpufreq cooling device\n");
-		return PTR_ERR(data->cool_dev);
+		int ret = PTR_ERR(data->cool_dev);
+
+		if (ret != -EPROBE_DEFER)
+			dev_err(bgp->dev,
+				"Failed to register cpu cooling device %d\n",
+				ret);
+
+		return ret;
 	}
 	ti_bandgap_set_sensor_data(bgp, id, data);
 
diff --git a/drivers/tty/amiserial.c b/drivers/tty/amiserial.c
index 979e7c3ea2cb..d9f85f95eb2a 100644
--- a/drivers/tty/amiserial.c
+++ b/drivers/tty/amiserial.c
@@ -1802,7 +1802,6 @@ static struct platform_driver amiga_serial_driver = {
 	.remove = __exit_p(amiga_serial_remove),
 	.driver   = {
 		.name	= "amiga-serial",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/tty/ehv_bytechan.c b/drivers/tty/ehv_bytechan.c
index 4f485e88f60c..3c60923b0957 100644
--- a/drivers/tty/ehv_bytechan.c
+++ b/drivers/tty/ehv_bytechan.c
@@ -309,8 +309,8 @@ static int __init ehv_bc_console_init(void)
 	 * handle for udbg.
 	 */
 	if (stdout_bc != CONFIG_PPC_EARLY_DEBUG_EHV_BC_HANDLE)
-		pr_warning("ehv-bc: udbg handle %u is not the stdout handle\n",
-			   CONFIG_PPC_EARLY_DEBUG_EHV_BC_HANDLE);
+		pr_warn("ehv-bc: udbg handle %u is not the stdout handle\n",
+			CONFIG_PPC_EARLY_DEBUG_EHV_BC_HANDLE);
 #endif
 
 	/* add_preferred_console() must be called before register_console(),
@@ -740,7 +740,6 @@ static const struct of_device_id ehv_bc_tty_of_ids[] = {
 
 static struct platform_driver ehv_bc_tty_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "ehv-bc",
 		.of_match_table = ehv_bc_tty_of_ids,
 	},
diff --git a/drivers/tty/goldfish.c b/drivers/tty/goldfish.c
index 09495f515fa9..967b2c2b7cf1 100644
--- a/drivers/tty/goldfish.c
+++ b/drivers/tty/goldfish.c
@@ -155,9 +155,9 @@ static struct tty_driver *goldfish_tty_console_device(struct console *c,
 
 static int goldfish_tty_console_setup(struct console *co, char *options)
 {
-	if ((unsigned)co->index > goldfish_tty_line_count)
+	if ((unsigned)co->index >= goldfish_tty_line_count)
 		return -ENODEV;
-	if (goldfish_ttys[co->index].base == 0)
+	if (!goldfish_ttys[co->index].base)
 		return -ENODEV;
 	return 0;
 }
@@ -317,7 +317,7 @@ static int goldfish_tty_remove(struct platform_device *pdev)
 	unregister_console(&qtty->console);
 	tty_unregister_device(goldfish_tty_driver, pdev->id);
 	iounmap(qtty->base);
-	qtty->base = 0;
+	qtty->base = NULL;
 	free_irq(qtty->irq, pdev);
 	goldfish_tty_current_line_count--;
 	if (goldfish_tty_current_line_count == 0)
diff --git a/drivers/tty/hvc/hvc_opal.c b/drivers/tty/hvc/hvc_opal.c
index a2cc5f834c63..071551bf3e9a 100644
--- a/drivers/tty/hvc/hvc_opal.c
+++ b/drivers/tty/hvc/hvc_opal.c
@@ -262,7 +262,6 @@ static struct platform_driver hvc_opal_driver = {
 	.remove		= hvc_opal_remove,
 	.driver		= {
 		.name	= hvc_opal_name,
-		.owner	= THIS_MODULE,
 		.of_match_table	= hvc_opal_match,
 	}
 };
diff --git a/drivers/tty/hvc/hvc_tile.c b/drivers/tty/hvc/hvc_tile.c
index df374860037c..3f6cd3102db5 100644
--- a/drivers/tty/hvc/hvc_tile.c
+++ b/drivers/tty/hvc/hvc_tile.c
@@ -178,7 +178,6 @@ static struct platform_driver hvc_tile_driver = {
 	.shutdown	= hvc_tile_shutdown,
 	.driver         = {
 		.name   = "hvc-tile",
-		.owner  = THIS_MODULE,
 	}
 };
 #endif
diff --git a/drivers/tty/hvc/hvcs.c b/drivers/tty/hvc/hvcs.c
index 81e939e90c4c..81ff7e1bfb1a 100644
--- a/drivers/tty/hvc/hvcs.c
+++ b/drivers/tty/hvc/hvcs.c
@@ -1575,7 +1575,7 @@ static int __init hvcs_module_init(void)
 	 */
 	rc = driver_create_file(&(hvcs_vio_driver.driver), &driver_attr_rescan);
 	if (rc)
-		pr_warning(KERN_ERR "HVCS: Failed to create rescan file (err %d)\n", rc);
+		pr_warning("HVCS: Failed to create rescan file (err %d)\n", rc);
 
 	return 0;
 }
diff --git a/drivers/tty/hvc/hvsi_lib.c b/drivers/tty/hvc/hvsi_lib.c
index 7ae6c293e518..a270f04588d7 100644
--- a/drivers/tty/hvc/hvsi_lib.c
+++ b/drivers/tty/hvc/hvsi_lib.c
@@ -405,8 +405,7 @@ void hvsilib_close(struct hvsi_priv *pv, struct hvc_struct *hp)
 		hvsi_send_close(pv);
 	}
 
-	if (pv->tty)
-		tty_kref_put(pv->tty);
+	tty_kref_put(pv->tty);
 	pv->tty = NULL;
 }
 
diff --git a/drivers/tty/ipwireless/hardware.c b/drivers/tty/ipwireless/hardware.c
index 2c14842541dd..5c77e1eac4ee 100644
--- a/drivers/tty/ipwireless/hardware.c
+++ b/drivers/tty/ipwireless/hardware.c
@@ -378,9 +378,9 @@ static void swap_packet_bitfield_to_le(unsigned char *data)
 	/*
 	 * transform bits from aa.bbb.ccc to ccc.bbb.aa
 	 */
-	ret |= tmp & 0xc0 >> 6;
-	ret |= tmp & 0x38 >> 1;
-	ret |= tmp & 0x07 << 5;
+	ret |= (tmp & 0xc0) >> 6;
+	ret |= (tmp & 0x38) >> 1;
+	ret |= (tmp & 0x07) << 5;
 	*data = ret & 0xff;
 #endif
 }
@@ -393,9 +393,9 @@ static void swap_packet_bitfield_from_le(unsigned char *data)
 	/*
 	 * transform bits from ccc.bbb.aa to aa.bbb.ccc
 	 */
-	ret |= tmp & 0xe0 >> 5;
-	ret |= tmp & 0x1c << 1;
-	ret |= tmp & 0x03 << 6;
+	ret |= (tmp & 0xe0) >> 5;
+	ret |= (tmp & 0x1c) << 1;
+	ret |= (tmp & 0x03) << 6;
 	*data = ret & 0xff;
 #endif
 }
diff --git a/drivers/tty/isicom.c b/drivers/tty/isicom.c
index 858291ca889c..59ed783c4bcd 100644
--- a/drivers/tty/isicom.c
+++ b/drivers/tty/isicom.c
@@ -249,7 +249,7 @@ static int lock_card(struct isi_board *card)
 		spin_unlock_irqrestore(&card->card_lock, card->flags);
 		msleep(10);
 	}
-	pr_warning("Failed to lock Card (0x%lx)\n", card->base);
+	pr_warn("Failed to lock Card (0x%lx)\n", card->base);
 
 	return 0;	/* Failed to acquire the card! */
 }
@@ -378,13 +378,13 @@ static inline int __isicom_paranoia_check(struct isi_port const *port,
 	char *name, const char *routine)
 {
 	if (!port) {
-		pr_warning("Warning: bad isicom magic for dev %s in %s.\n",
-			   name, routine);
+		pr_warn("Warning: bad isicom magic for dev %s in %s\n",
+			name, routine);
 		return 1;
 	}
 	if (port->magic != ISICOM_MAGIC) {
-		pr_warning("Warning: NULL isicom port for dev %s in %s.\n",
-			   name, routine);
+		pr_warn("Warning: NULL isicom port for dev %s in %s\n",
+			name, routine);
 		return 1;
 	}
 
@@ -546,8 +546,8 @@ static irqreturn_t isicom_interrupt(int irq, void *dev_id)
 	byte_count = header & 0xff;
 
 	if (channel + 1 > card->port_count) {
-		pr_warning("%s(0x%lx): %d(channel) > port_count.\n",
-			   __func__, base, channel+1);
+		pr_warn("%s(0x%lx): %d(channel) > port_count\n",
+			__func__, base, channel + 1);
 		outw(0x0000, base+0x04); /* enable interrupts */
 		spin_unlock(&card->card_lock);
 		return IRQ_HANDLED;
diff --git a/drivers/tty/n_tty.c b/drivers/tty/n_tty.c
index 26f097f60b10..d2b496750d59 100644
--- a/drivers/tty/n_tty.c
+++ b/drivers/tty/n_tty.c
@@ -321,7 +321,8 @@ static void n_tty_check_unthrottle(struct tty_struct *tty)
 
 static inline void put_tty_queue(unsigned char c, struct n_tty_data *ldata)
 {
-	*read_buf_addr(ldata, ldata->read_head++) = c;
+	*read_buf_addr(ldata, ldata->read_head) = c;
+	ldata->read_head++;
 }
 
 /**
@@ -351,13 +352,13 @@ static void n_tty_packet_mode_flush(struct tty_struct *tty)
 {
 	unsigned long flags;
 
-	spin_lock_irqsave(&tty->ctrl_lock, flags);
 	if (tty->link->packet) {
+		spin_lock_irqsave(&tty->ctrl_lock, flags);
 		tty->ctrl_status |= TIOCPKT_FLUSHREAD;
+		spin_unlock_irqrestore(&tty->ctrl_lock, flags);
 		if (waitqueue_active(&tty->link->read_wait))
 			wake_up_interruptible(&tty->link->read_wait);
 	}
-	spin_unlock_irqrestore(&tty->ctrl_lock, flags);
 }
 
 /**
@@ -2128,7 +2129,6 @@ static ssize_t n_tty_read(struct tty_struct *tty, struct file *file,
 	int minimum, time;
 	ssize_t retval = 0;
 	long timeout;
-	unsigned long flags;
 	int packet;
 
 	c = job_control(tty, file);
@@ -2174,10 +2174,10 @@ static ssize_t n_tty_read(struct tty_struct *tty, struct file *file,
 			unsigned char cs;
 			if (b != buf)
 				break;
-			spin_lock_irqsave(&tty->link->ctrl_lock, flags);
+			spin_lock_irq(&tty->link->ctrl_lock);
 			cs = tty->link->ctrl_status;
 			tty->link->ctrl_status = 0;
-			spin_unlock_irqrestore(&tty->link->ctrl_lock, flags);
+			spin_unlock_irq(&tty->link->ctrl_lock);
 			if (tty_put_user(tty, cs, b++)) {
 				retval = -EFAULT;
 				b--;
@@ -2193,45 +2193,29 @@ static ssize_t n_tty_read(struct tty_struct *tty, struct file *file,
 
 		if (!input_available_p(tty, 0)) {
 			if (test_bit(TTY_OTHER_CLOSED, &tty->flags)) {
-				up_read(&tty->termios_rwsem);
-				tty_flush_to_ldisc(tty);
-				down_read(&tty->termios_rwsem);
-				if (!input_available_p(tty, 0)) {
-					retval = -EIO;
-					break;
-				}
-			} else {
-				if (tty_hung_up_p(file))
-					break;
-				if (!timeout)
-					break;
-				if (file->f_flags & O_NONBLOCK) {
-					retval = -EAGAIN;
-					break;
-				}
-				if (signal_pending(current)) {
-					retval = -ERESTARTSYS;
-					break;
-				}
-				n_tty_set_room(tty);
-				up_read(&tty->termios_rwsem);
-
-				timeout = wait_woken(&wait, TASK_INTERRUPTIBLE,
-						     timeout);
-
-				down_read(&tty->termios_rwsem);
-				continue;
+				retval = -EIO;
+				break;
 			}
-		}
-
-		/* Deal with packet mode. */
-		if (packet && b == buf) {
-			if (tty_put_user(tty, TIOCPKT_DATA, b++)) {
-				retval = -EFAULT;
-				b--;
+			if (tty_hung_up_p(file))
+				break;
+			if (!timeout)
+				break;
+			if (file->f_flags & O_NONBLOCK) {
+				retval = -EAGAIN;
 				break;
 			}
-			nr--;
+			if (signal_pending(current)) {
+				retval = -ERESTARTSYS;
+				break;
+			}
+			n_tty_set_room(tty);
+			up_read(&tty->termios_rwsem);
+
+			timeout = wait_woken(&wait, TASK_INTERRUPTIBLE,
+					     timeout);
+
+			down_read(&tty->termios_rwsem);
+			continue;
 		}
 
 		if (ldata->icanon && !L_EXTPROC(tty)) {
@@ -2243,8 +2227,17 @@ static ssize_t n_tty_read(struct tty_struct *tty, struct file *file,
 				break;
 		} else {
 			int uncopied;
-			/* The copy function takes the read lock and handles
-			   locking internally for this case */
+
+			/* Deal with packet mode. */
+			if (packet && b == buf) {
+				if (tty_put_user(tty, TIOCPKT_DATA, b++)) {
+					retval = -EFAULT;
+					b--;
+					break;
+				}
+				nr--;
+			}
+
 			uncopied = copy_from_read_buf(tty, &b, &nr);
 			uncopied += copy_from_read_buf(tty, &b, &nr);
 			if (uncopied) {
diff --git a/drivers/tty/pty.c b/drivers/tty/pty.c
index 7c4447a5c0f4..a9d256d6e909 100644
--- a/drivers/tty/pty.c
+++ b/drivers/tty/pty.c
@@ -47,10 +47,13 @@ static void pty_close(struct tty_struct *tty, struct file *filp)
 	set_bit(TTY_IO_ERROR, &tty->flags);
 	wake_up_interruptible(&tty->read_wait);
 	wake_up_interruptible(&tty->write_wait);
+	spin_lock_irq(&tty->ctrl_lock);
 	tty->packet = 0;
+	spin_unlock_irq(&tty->ctrl_lock);
 	/* Review - krefs on tty_link ?? */
 	if (!tty->link)
 		return;
+	tty_flush_to_ldisc(tty->link);
 	set_bit(TTY_OTHER_CLOSED, &tty->link->flags);
 	wake_up_interruptible(&tty->link->read_wait);
 	wake_up_interruptible(&tty->link->write_wait);
@@ -64,9 +67,7 @@ static void pty_close(struct tty_struct *tty, struct file *filp)
 			mutex_unlock(&devpts_mutex);
 		}
 #endif
-		tty_unlock(tty);
 		tty_vhangup(tty->link);
-		tty_lock(tty);
 	}
 }
 
@@ -178,21 +179,21 @@ static int pty_get_lock(struct tty_struct *tty, int __user *arg)
 /* Set the packet mode on a pty */
 static int pty_set_pktmode(struct tty_struct *tty, int __user *arg)
 {
-	unsigned long flags;
 	int pktmode;
 
 	if (get_user(pktmode, arg))
 		return -EFAULT;
 
-	spin_lock_irqsave(&tty->ctrl_lock, flags);
+	spin_lock_irq(&tty->ctrl_lock);
 	if (pktmode) {
 		if (!tty->packet) {
-			tty->packet = 1;
 			tty->link->ctrl_status = 0;
+			smp_mb();
+			tty->packet = 1;
 		}
 	} else
 		tty->packet = 0;
-	spin_unlock_irqrestore(&tty->ctrl_lock, flags);
+	spin_unlock_irq(&tty->ctrl_lock);
 
 	return 0;
 }
@@ -207,15 +208,12 @@ static int pty_get_pktmode(struct tty_struct *tty, int __user *arg)
 /* Send a signal to the slave */
 static int pty_signal(struct tty_struct *tty, int sig)
 {
-	unsigned long flags;
 	struct pid *pgrp;
 
 	if (tty->link) {
-		spin_lock_irqsave(&tty->link->ctrl_lock, flags);
-		pgrp = get_pid(tty->link->pgrp);
-		spin_unlock_irqrestore(&tty->link->ctrl_lock, flags);
-
-		kill_pgrp(pgrp, sig, 1);
+		pgrp = tty_get_pgrp(tty->link);
+		if (pgrp)
+			kill_pgrp(pgrp, sig, 1);
 		put_pid(pgrp);
 	}
 	return 0;
@@ -224,16 +222,15 @@ static int pty_signal(struct tty_struct *tty, int sig)
 static void pty_flush_buffer(struct tty_struct *tty)
 {
 	struct tty_struct *to = tty->link;
-	unsigned long flags;
 
 	if (!to)
 		return;
 	/* tty_buffer_flush(to); FIXME */
 	if (to->packet) {
-		spin_lock_irqsave(&tty->ctrl_lock, flags);
+		spin_lock_irq(&tty->ctrl_lock);
 		tty->ctrl_status |= TIOCPKT_FLUSHWRITE;
 		wake_up_interruptible(&to->read_wait);
-		spin_unlock_irqrestore(&tty->ctrl_lock, flags);
+		spin_unlock_irq(&tty->ctrl_lock);
 	}
 }
 
@@ -262,6 +259,32 @@ out:
 static void pty_set_termios(struct tty_struct *tty,
 					struct ktermios *old_termios)
 {
+	/* See if packet mode change of state. */
+	if (tty->link && tty->link->packet) {
+		int extproc = (old_termios->c_lflag & EXTPROC) |
+				(tty->termios.c_lflag & EXTPROC);
+		int old_flow = ((old_termios->c_iflag & IXON) &&
+				(old_termios->c_cc[VSTOP] == '\023') &&
+				(old_termios->c_cc[VSTART] == '\021'));
+		int new_flow = (I_IXON(tty) &&
+				STOP_CHAR(tty) == '\023' &&
+				START_CHAR(tty) == '\021');
+		if ((old_flow != new_flow) || extproc) {
+			spin_lock_irq(&tty->ctrl_lock);
+			if (old_flow != new_flow) {
+				tty->ctrl_status &= ~(TIOCPKT_DOSTOP | TIOCPKT_NOSTOP);
+				if (new_flow)
+					tty->ctrl_status |= TIOCPKT_DOSTOP;
+				else
+					tty->ctrl_status |= TIOCPKT_NOSTOP;
+			}
+			if (extproc)
+				tty->ctrl_status |= TIOCPKT_IOCTL;
+			spin_unlock_irq(&tty->ctrl_lock);
+			wake_up_interruptible(&tty->link->read_wait);
+		}
+	}
+
 	tty->termios.c_cflag &= ~(CSIZE | PARENB);
 	tty->termios.c_cflag |= (CS8 | CREAD);
 }
@@ -278,7 +301,6 @@ static void pty_set_termios(struct tty_struct *tty,
 static int pty_resize(struct tty_struct *tty,  struct winsize *ws)
 {
 	struct pid *pgrp, *rpgrp;
-	unsigned long flags;
 	struct tty_struct *pty = tty->link;
 
 	/* For a PTY we need to lock the tty side */
@@ -286,17 +308,9 @@ static int pty_resize(struct tty_struct *tty,  struct winsize *ws)
 	if (!memcmp(ws, &tty->winsize, sizeof(*ws)))
 		goto done;
 
-	/* Get the PID values and reference them so we can
-	   avoid holding the tty ctrl lock while sending signals.
-	   We need to lock these individually however. */
-
-	spin_lock_irqsave(&tty->ctrl_lock, flags);
-	pgrp = get_pid(tty->pgrp);
-	spin_unlock_irqrestore(&tty->ctrl_lock, flags);
-
-	spin_lock_irqsave(&pty->ctrl_lock, flags);
-	rpgrp = get_pid(pty->pgrp);
-	spin_unlock_irqrestore(&pty->ctrl_lock, flags);
+	/* Signal the foreground process group of both ptys */
+	pgrp = tty_get_pgrp(tty);
+	rpgrp = tty_get_pgrp(pty);
 
 	if (pgrp)
 		kill_pgrp(pgrp, SIGWINCH, 1);
@@ -327,26 +341,26 @@ static void pty_start(struct tty_struct *tty)
 {
 	unsigned long flags;
 
-	spin_lock_irqsave(&tty->ctrl_lock, flags);
 	if (tty->link && tty->link->packet) {
+		spin_lock_irqsave(&tty->ctrl_lock, flags);
 		tty->ctrl_status &= ~TIOCPKT_STOP;
 		tty->ctrl_status |= TIOCPKT_START;
+		spin_unlock_irqrestore(&tty->ctrl_lock, flags);
 		wake_up_interruptible_poll(&tty->link->read_wait, POLLIN);
 	}
-	spin_unlock_irqrestore(&tty->ctrl_lock, flags);
 }
 
 static void pty_stop(struct tty_struct *tty)
 {
 	unsigned long flags;
 
-	spin_lock_irqsave(&tty->ctrl_lock, flags);
 	if (tty->link && tty->link->packet) {
+		spin_lock_irqsave(&tty->ctrl_lock, flags);
 		tty->ctrl_status &= ~TIOCPKT_START;
 		tty->ctrl_status |= TIOCPKT_STOP;
+		spin_unlock_irqrestore(&tty->ctrl_lock, flags);
 		wake_up_interruptible_poll(&tty->link->read_wait, POLLIN);
 	}
-	spin_unlock_irqrestore(&tty->ctrl_lock, flags);
 }
 
 /**
@@ -368,6 +382,10 @@ static int pty_common_install(struct tty_driver *driver, struct tty_struct *tty,
 	int idx = tty->index;
 	int retval = -ENOMEM;
 
+	/* Opening the slave first has always returned -EIO */
+	if (driver->subtype != PTY_TYPE_MASTER)
+		return -EIO;
+
 	ports[0] = kmalloc(sizeof **ports, GFP_KERNEL);
 	ports[1] = kmalloc(sizeof **ports, GFP_KERNEL);
 	if (!ports[0] || !ports[1])
@@ -380,6 +398,8 @@ static int pty_common_install(struct tty_driver *driver, struct tty_struct *tty,
 	if (!o_tty)
 		goto err_put_module;
 
+	tty_set_lock_subclass(o_tty);
+
 	if (legacy) {
 		/* We always use new tty termios data so we can do this
 		   the easy way .. */
@@ -404,8 +424,6 @@ static int pty_common_install(struct tty_driver *driver, struct tty_struct *tty,
 	 * Everything allocated ... set up the o_tty structure.
 	 */
 	tty_driver_kref_get(driver->other);
-	if (driver->subtype == PTY_TYPE_MASTER)
-		o_tty->count++;
 	/* Establish the links in both directions */
 	tty->link   = o_tty;
 	o_tty->link = tty;
@@ -417,6 +435,7 @@ static int pty_common_install(struct tty_driver *driver, struct tty_struct *tty,
 
 	tty_driver_kref_get(driver);
 	tty->count++;
+	o_tty->count++;
 	return 0;
 err_free_termios:
 	if (legacy)
@@ -489,7 +508,6 @@ static const struct tty_operations master_pty_ops_bsd = {
 	.flush_buffer = pty_flush_buffer,
 	.chars_in_buffer = pty_chars_in_buffer,
 	.unthrottle = pty_unthrottle,
-	.set_termios = pty_set_termios,
 	.ioctl = pty_bsd_ioctl,
 	.cleanup = pty_cleanup,
 	.resize = pty_resize,
@@ -666,7 +684,6 @@ static const struct tty_operations ptm_unix98_ops = {
 	.flush_buffer = pty_flush_buffer,
 	.chars_in_buffer = pty_chars_in_buffer,
 	.unthrottle = pty_unthrottle,
-	.set_termios = pty_set_termios,
 	.ioctl = pty_unix98_ioctl,
 	.resize = pty_resize,
 	.shutdown = pty_unix98_shutdown,
diff --git a/drivers/tty/serial/8250/8250.h b/drivers/tty/serial/8250/8250.h
index cb51be55989e..b00836851061 100644
--- a/drivers/tty/serial/8250/8250.h
+++ b/drivers/tty/serial/8250/8250.h
@@ -16,6 +16,9 @@
 #include <linux/dmaengine.h>
 
 struct uart_8250_dma {
+	int (*tx_dma)(struct uart_8250_port *p);
+	int (*rx_dma)(struct uart_8250_port *p, unsigned int iir);
+
 	/* Filter function */
 	dma_filter_fn		fn;
 
@@ -41,6 +44,8 @@ struct uart_8250_dma {
 	size_t			tx_size;
 
 	unsigned char		tx_running:1;
+	unsigned char		tx_err: 1;
+	unsigned char		rx_running:1;
 };
 
 struct old_serial_port {
@@ -51,7 +56,7 @@ struct old_serial_port {
 	unsigned int flags;
 	unsigned char hub6;
 	unsigned char io_type;
-	unsigned char *iomem_base;
+	unsigned char __iomem *iomem_base;
 	unsigned short iomem_reg_shift;
 	unsigned long irqflags;
 };
@@ -114,6 +119,8 @@ static inline void serial_dl_write(struct uart_8250_port *up, int value)
 }
 
 struct uart_8250_port *serial8250_get_port(int line);
+void serial8250_rpm_get(struct uart_8250_port *p);
+void serial8250_rpm_put(struct uart_8250_port *p);
 
 #if defined(__alpha__) && !defined(CONFIG_PCI)
 /*
diff --git a/drivers/tty/serial/8250/8250_core.c b/drivers/tty/serial/8250/8250_core.c
index ca5cfdc1459a..11c66856ba2f 100644
--- a/drivers/tty/serial/8250/8250_core.c
+++ b/drivers/tty/serial/8250/8250_core.c
@@ -541,23 +541,25 @@ void serial8250_clear_and_reinit_fifos(struct uart_8250_port *p)
 }
 EXPORT_SYMBOL_GPL(serial8250_clear_and_reinit_fifos);
 
-static void serial8250_rpm_get(struct uart_8250_port *p)
+void serial8250_rpm_get(struct uart_8250_port *p)
 {
 	if (!(p->capabilities & UART_CAP_RPM))
 		return;
 	pm_runtime_get_sync(p->port.dev);
 }
+EXPORT_SYMBOL_GPL(serial8250_rpm_get);
 
-static void serial8250_rpm_put(struct uart_8250_port *p)
+void serial8250_rpm_put(struct uart_8250_port *p)
 {
 	if (!(p->capabilities & UART_CAP_RPM))
 		return;
 	pm_runtime_mark_last_busy(p->port.dev);
 	pm_runtime_put_autosuspend(p->port.dev);
 }
+EXPORT_SYMBOL_GPL(serial8250_rpm_put);
 
 /*
- * This two wrapper ensure, that enable_runtime_pm_tx() can be called more than
+ * These two wrappers ensure that enable_runtime_pm_tx() can be called more than
  * once and disable_runtime_pm_tx() will still disable RPM because the fifo is
  * empty and the HW can idle again.
  */
@@ -595,6 +597,7 @@ static void serial8250_rpm_put_tx(struct uart_8250_port *p)
  */
 static void serial8250_set_sleep(struct uart_8250_port *p, int sleep)
 {
+	unsigned char lcr = 0, efr = 0;
 	/*
 	 * Exar UARTs have a SLEEP register that enables or disables
 	 * each UART to enter sleep mode separately.  On the XR17V35x the
@@ -611,6 +614,8 @@ static void serial8250_set_sleep(struct uart_8250_port *p, int sleep)
 
 	if (p->capabilities & UART_CAP_SLEEP) {
 		if (p->capabilities & UART_CAP_EFR) {
+			lcr = serial_in(p, UART_LCR);
+			efr = serial_in(p, UART_EFR);
 			serial_out(p, UART_LCR, UART_LCR_CONF_MODE_B);
 			serial_out(p, UART_EFR, UART_EFR_ECB);
 			serial_out(p, UART_LCR, 0);
@@ -618,8 +623,8 @@ static void serial8250_set_sleep(struct uart_8250_port *p, int sleep)
 		serial_out(p, UART_IER, sleep ? UART_IERX_SLEEP : 0);
 		if (p->capabilities & UART_CAP_EFR) {
 			serial_out(p, UART_LCR, UART_LCR_CONF_MODE_B);
-			serial_out(p, UART_EFR, 0);
-			serial_out(p, UART_LCR, 0);
+			serial_out(p, UART_EFR, efr);
+			serial_out(p, UART_LCR, lcr);
 		}
 	}
 out:
@@ -1350,7 +1355,7 @@ static void serial8250_start_tx(struct uart_port *port)
 	struct uart_8250_port *up = up_to_u8250p(port);
 
 	serial8250_rpm_get_tx(up);
-	if (up->dma && !serial8250_tx_dma(up)) {
+	if (up->dma && !up->dma->tx_dma(up)) {
 		return;
 	} else if (!(up->ier & UART_IER_THRI)) {
 		up->ier |= UART_IER_THRI;
@@ -1397,6 +1402,19 @@ static void serial8250_stop_rx(struct uart_port *port)
 	serial8250_rpm_put(up);
 }
 
+static void serial8250_disable_ms(struct uart_port *port)
+{
+	struct uart_8250_port *up =
+		container_of(port, struct uart_8250_port, port);
+
+	/* no MSR capabilities */
+	if (up->bugs & UART_BUG_NOMSR)
+		return;
+
+	up->ier &= ~UART_IER_MSI;
+	serial_port_out(port, UART_IER, up->ier);
+}
+
 static void serial8250_enable_ms(struct uart_port *port)
 {
 	struct uart_8250_port *up = up_to_u8250p(port);
@@ -1483,7 +1501,7 @@ serial8250_rx_chars(struct uart_8250_port *up, unsigned char lsr)
 
 ignore_char:
 		lsr = serial_in(up, UART_LSR);
-	} while ((lsr & (UART_LSR_DR | UART_LSR_BI)) && (max_count-- > 0));
+	} while ((lsr & (UART_LSR_DR | UART_LSR_BI)) && (--max_count > 0));
 	spin_unlock(&port->lock);
 	tty_flip_buffer_push(&port->state->port);
 	spin_lock(&port->lock);
@@ -1532,7 +1550,7 @@ void serial8250_tx_chars(struct uart_8250_port *up)
 	DEBUG_INTR("THRE...");
 
 	/*
-	 * With RPM enabled, we have to wait once the FIFO is empty before the
+	 * With RPM enabled, we have to wait until the FIFO is empty before the
 	 * HW can go idle. So we get here once again with empty FIFO and disable
 	 * the interrupt and RPM in __stop_tx()
 	 */
@@ -1588,13 +1606,14 @@ int serial8250_handle_irq(struct uart_port *port, unsigned int iir)
 
 	if (status & (UART_LSR_DR | UART_LSR_BI)) {
 		if (up->dma)
-			dma_err = serial8250_rx_dma(up, iir);
+			dma_err = up->dma->rx_dma(up, iir);
 
 		if (!up->dma || dma_err)
 			status = serial8250_rx_chars(up, status);
 	}
 	serial8250_modem_status(up);
-	if (!up->dma && (status & UART_LSR_THRE))
+	if ((!up->dma || (up->dma && up->dma->tx_err)) &&
+	    (status & UART_LSR_THRE))
 		serial8250_tx_chars(up);
 
 	spin_unlock_irqrestore(&port->lock, flags);
@@ -2603,13 +2622,21 @@ serial8250_set_termios(struct uart_port *port, struct ktermios *termios,
 }
 
 static void
-serial8250_set_ldisc(struct uart_port *port, int new)
+serial8250_set_ldisc(struct uart_port *port, struct ktermios *termios)
 {
-	if (new == N_PPS) {
+	if (termios->c_line == N_PPS) {
 		port->flags |= UPF_HARDPPS_CD;
+		spin_lock_irq(&port->lock);
 		serial8250_enable_ms(port);
-	} else
+		spin_unlock_irq(&port->lock);
+	} else {
 		port->flags &= ~UPF_HARDPPS_CD;
+		if (!UART_ENABLE_MS(port, termios->c_cflag)) {
+			spin_lock_irq(&port->lock);
+			serial8250_disable_ms(port);
+			spin_unlock_irq(&port->lock);
+		}
+	}
 }
 
 
@@ -2634,8 +2661,11 @@ serial8250_pm(struct uart_port *port, unsigned int state,
 
 static unsigned int serial8250_port_size(struct uart_8250_port *pt)
 {
-	if (pt->port.iotype == UPIO_AU)
+	if (pt->port.iotype == UPIO_AU) {
+		if (pt->port.type == PORT_RT2880)
+			return 0x100;
 		return 0x1000;
+	}
 	if (is_omap1_8250(pt))
 		return 0x16 << pt->port.regshift;
 
@@ -2975,42 +3005,6 @@ serial8250_verify_port(struct uart_port *port, struct serial_struct *ser)
 	return 0;
 }
 
-static int serial8250_ioctl(struct uart_port *port, unsigned int cmd,
-			   unsigned long arg)
-{
-	struct uart_8250_port *up =
-		container_of(port, struct uart_8250_port, port);
-	int ret;
-	struct serial_rs485 rs485_config;
-
-	if (!up->rs485_config)
-		return -ENOIOCTLCMD;
-
-	switch (cmd) {
-	case TIOCSRS485:
-		if (copy_from_user(&rs485_config, (void __user *)arg,
-				   sizeof(rs485_config)))
-			return -EFAULT;
-
-		ret = up->rs485_config(up, &rs485_config);
-		if (ret)
-			return ret;
-
-		memcpy(&up->rs485, &rs485_config, sizeof(rs485_config));
-
-		return 0;
-	case TIOCGRS485:
-		if (copy_to_user((void __user *)arg, &up->rs485,
-				 sizeof(up->rs485)))
-			return -EFAULT;
-		return 0;
-	default:
-		break;
-	}
-
-	return -ENOIOCTLCMD;
-}
-
 static const char *
 serial8250_type(struct uart_port *port)
 {
@@ -3042,7 +3036,6 @@ static struct uart_ops serial8250_pops = {
 	.request_port	= serial8250_request_port,
 	.config_port	= serial8250_config_port,
 	.verify_port	= serial8250_verify_port,
-	.ioctl		= serial8250_ioctl,
 #ifdef CONFIG_CONSOLE_POLL
 	.poll_get_char = serial8250_get_poll_char,
 	.poll_put_char = serial8250_put_poll_char,
@@ -3198,7 +3191,9 @@ serial8250_console_write(struct console *co, const char *s, unsigned int count)
 
 	serial8250_rpm_get(up);
 
-	if (port->sysrq || oops_in_progress)
+	if (port->sysrq)
+		locked = 0;
+	else if (oops_in_progress)
 		locked = spin_trylock_irqsave(&port->lock, flags);
 	else
 		spin_lock_irqsave(&port->lock, flags);
@@ -3237,7 +3232,7 @@ serial8250_console_write(struct console *co, const char *s, unsigned int count)
 	serial8250_rpm_put(up);
 }
 
-static int __init serial8250_console_setup(struct console *co, char *options)
+static int serial8250_console_setup(struct console *co, char *options)
 {
 	struct uart_port *port;
 	int baud = 9600;
@@ -3488,7 +3483,6 @@ static struct platform_driver serial8250_isa_driver = {
 	.resume		= serial8250_resume,
 	.driver		= {
 		.name	= "serial8250",
-		.owner	= THIS_MODULE,
 	},
 };
 
@@ -3585,10 +3579,10 @@ int serial8250_register_8250_port(struct uart_8250_port *up)
 		uart->port.fifosize	= up->port.fifosize;
 		uart->tx_loadsz		= up->tx_loadsz;
 		uart->capabilities	= up->capabilities;
-		uart->rs485_config	= up->rs485_config;
-		uart->rs485		= up->rs485;
 		uart->port.throttle	= up->port.throttle;
 		uart->port.unthrottle	= up->port.unthrottle;
+		uart->port.rs485_config	= up->port.rs485_config;
+		uart->port.rs485	= up->port.rs485;
 
 		/* Take tx_loadsz from fifosize if it wasn't set separately */
 		if (uart->port.fifosize && !uart->tx_loadsz)
@@ -3623,8 +3617,13 @@ int serial8250_register_8250_port(struct uart_8250_port *up)
 			uart->dl_read = up->dl_read;
 		if (up->dl_write)
 			uart->dl_write = up->dl_write;
-		if (up->dma)
+		if (up->dma) {
 			uart->dma = up->dma;
+			if (!uart->dma->tx_dma)
+				uart->dma->tx_dma = serial8250_tx_dma;
+			if (!uart->dma->rx_dma)
+				uart->dma->rx_dma = serial8250_rx_dma;
+		}
 
 		if (serial8250_isa_config != NULL)
 			serial8250_isa_config(0, &uart->port,
diff --git a/drivers/tty/serial/8250/8250_dma.c b/drivers/tty/serial/8250/8250_dma.c
index 148ffe4c232f..fcd7ac6af2fc 100644
--- a/drivers/tty/serial/8250/8250_dma.c
+++ b/drivers/tty/serial/8250/8250_dma.c
@@ -21,6 +21,7 @@ static void __dma_tx_complete(void *param)
 	struct uart_8250_dma	*dma = p->dma;
 	struct circ_buf		*xmit = &p->port.state->xmit;
 	unsigned long	flags;
+	int		ret;
 
 	dma_sync_single_for_cpu(dma->txchan->device->dev, dma->tx_addr,
 				UART_XMIT_SIZE, DMA_TO_DEVICE);
@@ -36,8 +37,11 @@ static void __dma_tx_complete(void *param)
 	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
 		uart_write_wakeup(&p->port);
 
-	if (!uart_circ_empty(xmit) && !uart_tx_stopped(&p->port))
-		serial8250_tx_dma(p);
+	ret = serial8250_tx_dma(p);
+	if (ret) {
+		p->ier |= UART_IER_THRI;
+		serial_port_out(&p->port, UART_IER, p->ier);
+	}
 
 	spin_unlock_irqrestore(&p->port.lock, flags);
 }
@@ -53,6 +57,7 @@ static void __dma_rx_complete(void *param)
 	dma_sync_single_for_cpu(dma->rxchan->device->dev, dma->rx_addr,
 				dma->rx_size, DMA_FROM_DEVICE);
 
+	dma->rx_running = 0;
 	dmaengine_tx_status(dma->rxchan, dma->rx_cookie, &state);
 	dmaengine_terminate_all(dma->rxchan);
 
@@ -69,6 +74,7 @@ int serial8250_tx_dma(struct uart_8250_port *p)
 	struct uart_8250_dma		*dma = p->dma;
 	struct circ_buf			*xmit = &p->port.state->xmit;
 	struct dma_async_tx_descriptor	*desc;
+	int ret;
 
 	if (uart_tx_stopped(&p->port) || dma->tx_running ||
 	    uart_circ_empty(xmit))
@@ -80,11 +86,12 @@ int serial8250_tx_dma(struct uart_8250_port *p)
 					   dma->tx_addr + xmit->tail,
 					   dma->tx_size, DMA_MEM_TO_DEV,
 					   DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
-	if (!desc)
-		return -EBUSY;
+	if (!desc) {
+		ret = -EBUSY;
+		goto err;
+	}
 
 	dma->tx_running = 1;
-
 	desc->callback = __dma_tx_complete;
 	desc->callback_param = p;
 
@@ -94,19 +101,23 @@ int serial8250_tx_dma(struct uart_8250_port *p)
 				   UART_XMIT_SIZE, DMA_TO_DEVICE);
 
 	dma_async_issue_pending(dma->txchan);
-
+	if (dma->tx_err) {
+		dma->tx_err = 0;
+		if (p->ier & UART_IER_THRI) {
+			p->ier &= ~UART_IER_THRI;
+			serial_out(p, UART_IER, p->ier);
+		}
+	}
 	return 0;
+err:
+	dma->tx_err = 1;
+	return ret;
 }
-EXPORT_SYMBOL_GPL(serial8250_tx_dma);
 
 int serial8250_rx_dma(struct uart_8250_port *p, unsigned int iir)
 {
 	struct uart_8250_dma		*dma = p->dma;
 	struct dma_async_tx_descriptor	*desc;
-	struct dma_tx_state		state;
-	int				dma_status;
-
-	dma_status = dmaengine_tx_status(dma->rxchan, dma->rx_cookie, &state);
 
 	switch (iir & 0x3f) {
 	case UART_IIR_RLSI:
@@ -117,7 +128,7 @@ int serial8250_rx_dma(struct uart_8250_port *p, unsigned int iir)
 		 * If RCVR FIFO trigger level was not reached, complete the
 		 * transfer and let 8250_core copy the remaining data.
 		 */
-		if (dma_status == DMA_IN_PROGRESS) {
+		if (dma->rx_running) {
 			dmaengine_pause(dma->rxchan);
 			__dma_rx_complete(p);
 		}
@@ -126,7 +137,7 @@ int serial8250_rx_dma(struct uart_8250_port *p, unsigned int iir)
 		break;
 	}
 
-	if (dma_status)
+	if (dma->rx_running)
 		return 0;
 
 	desc = dmaengine_prep_slave_single(dma->rxchan, dma->rx_addr,
@@ -135,6 +146,7 @@ int serial8250_rx_dma(struct uart_8250_port *p, unsigned int iir)
 	if (!desc)
 		return -EBUSY;
 
+	dma->rx_running = 1;
 	desc->callback = __dma_rx_complete;
 	desc->callback_param = p;
 
@@ -147,7 +159,6 @@ int serial8250_rx_dma(struct uart_8250_port *p, unsigned int iir)
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(serial8250_rx_dma);
 
 int serial8250_request_dma(struct uart_8250_port *p)
 {
diff --git a/drivers/tty/serial/8250/8250_dw.c b/drivers/tty/serial/8250/8250_dw.c
index beea6ca73ee5..555de07db593 100644
--- a/drivers/tty/serial/8250/8250_dw.c
+++ b/drivers/tty/serial/8250/8250_dw.c
@@ -122,13 +122,44 @@ static unsigned int dw8250_serial_in(struct uart_port *p, int offset)
 	return dw8250_modify_msr(p, offset, value);
 }
 
-/* Read Back (rb) version to ensure register access ording. */
-static void dw8250_serial_out_rb(struct uart_port *p, int offset, int value)
+#ifdef CONFIG_64BIT
+static unsigned int dw8250_serial_inq(struct uart_port *p, int offset)
 {
-	dw8250_serial_out(p, offset, value);
-	dw8250_serial_in(p, UART_LCR);
+	unsigned int value;
+
+	value = (u8)__raw_readq(p->membase + (offset << p->regshift));
+
+	return dw8250_modify_msr(p, offset, value);
 }
 
+static void dw8250_serial_outq(struct uart_port *p, int offset, int value)
+{
+	struct dw8250_data *d = p->private_data;
+
+	if (offset == UART_MCR)
+		d->last_mcr = value;
+
+	value &= 0xff;
+	__raw_writeq(value, p->membase + (offset << p->regshift));
+	/* Read back to ensure register write ordering. */
+	__raw_readq(p->membase + (UART_LCR << p->regshift));
+
+	/* Make sure LCR write wasn't ignored */
+	if (offset == UART_LCR) {
+		int tries = 1000;
+		while (tries--) {
+			unsigned int lcr = p->serial_in(p, UART_LCR);
+			if ((value & ~UART_LCR_SPAR) == (lcr & ~UART_LCR_SPAR))
+				return;
+			dw8250_force_idle(p);
+			__raw_writeq(value & 0xff,
+				     p->membase + (UART_LCR << p->regshift));
+		}
+		dev_err(p->dev, "Couldn't set LCR to %d\n", value);
+	}
+}
+#endif /* CONFIG_64BIT */
+
 static void dw8250_serial_out32(struct uart_port *p, int offset, int value)
 {
 	struct dw8250_data *d = p->private_data;
@@ -258,22 +289,19 @@ static int dw8250_probe_of(struct uart_port *p,
 	struct uart_8250_port *up = up_to_u8250p(p);
 	u32			val;
 	bool has_ucv = true;
+	int id;
 
+#ifdef CONFIG_64BIT
 	if (of_device_is_compatible(np, "cavium,octeon-3860-uart")) {
-#ifdef __BIG_ENDIAN
-		/*
-		 * Low order bits of these 64-bit registers, when
-		 * accessed as a byte, are 7 bytes further down in the
-		 * address space in big endian mode.
-		 */
-		p->membase += 7;
-#endif
-		p->serial_out = dw8250_serial_out_rb;
+		p->serial_in = dw8250_serial_inq;
+		p->serial_out = dw8250_serial_outq;
 		p->flags = UPF_SKIP_TEST | UPF_SHARE_IRQ | UPF_FIXED_TYPE;
 		p->type = PORT_OCTEON;
 		data->usr_reg = 0x27;
 		has_ucv = false;
-	} else if (!of_property_read_u32(np, "reg-io-width", &val)) {
+	} else
+#endif
+	if (!of_property_read_u32(np, "reg-io-width", &val)) {
 		switch (val) {
 		case 1:
 			break;
@@ -290,9 +318,22 @@ static int dw8250_probe_of(struct uart_port *p,
 	if (has_ucv)
 		dw8250_setup_port(up);
 
+	/* if we have a valid fifosize, try hooking up DMA here */
+	if (p->fifosize) {
+		up->dma = &data->dma;
+
+		up->dma->rxconf.src_maxburst = p->fifosize / 4;
+		up->dma->txconf.dst_maxburst = p->fifosize / 4;
+	}
+
 	if (!of_property_read_u32(np, "reg-shift", &val))
 		p->regshift = val;
 
+	/* get index of serial line, if found in DT aliases */
+	id = of_alias_get_id(np, "serial");
+	if (id >= 0)
+		p->line = id;
+
 	/* clock got configured through clk api, all done */
 	if (p->uartclk)
 		return 0;
@@ -489,7 +530,7 @@ static int dw8250_resume(struct device *dev)
 }
 #endif /* CONFIG_PM_SLEEP */
 
-#ifdef CONFIG_PM_RUNTIME
+#ifdef CONFIG_PM
 static int dw8250_runtime_suspend(struct device *dev)
 {
 	struct dw8250_data *data = dev_get_drvdata(dev);
@@ -543,7 +584,6 @@ MODULE_DEVICE_TABLE(acpi, dw8250_acpi_match);
 static struct platform_driver dw8250_platform_driver = {
 	.driver = {
 		.name		= "dw-apb-uart",
-		.owner		= THIS_MODULE,
 		.pm		= &dw8250_pm_ops,
 		.of_match_table	= dw8250_of_match,
 		.acpi_match_table = ACPI_PTR(dw8250_acpi_match),
diff --git a/drivers/tty/serial/8250/8250_em.c b/drivers/tty/serial/8250/8250_em.c
index 56c87232b6a0..ae5eaed6aa85 100644
--- a/drivers/tty/serial/8250/8250_em.c
+++ b/drivers/tty/serial/8250/8250_em.c
@@ -102,10 +102,8 @@ static int serial8250_em_probe(struct platform_device *pdev)
 	}
 
 	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
-	if (!priv) {
-		dev_err(&pdev->dev, "unable to allocate private data\n");
+	if (!priv)
 		return -ENOMEM;
-	}
 
 	priv->sclk = devm_clk_get(&pdev->dev, "sclk");
 	if (IS_ERR(priv->sclk)) {
@@ -161,7 +159,6 @@ 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			= serial8250_em_remove,
diff --git a/drivers/tty/serial/8250/8250_fintek.c b/drivers/tty/serial/8250/8250_fintek.c
index 1bb28cb69493..1e6899bc9429 100644
--- a/drivers/tty/serial/8250/8250_fintek.c
+++ b/drivers/tty/serial/8250/8250_fintek.c
@@ -89,11 +89,11 @@ static int fintek_8250_check_id(void)
 	return 0;
 }
 
-static int fintek_8250_rs4850_config(struct uart_8250_port *uart,
+static int fintek_8250_rs485_config(struct uart_port *port,
 			      struct serial_rs485 *rs485)
 {
 	uint8_t config = 0;
-	int index = fintek_8250_get_index(uart->port.iobase);
+	int index = fintek_8250_get_index(port->iobase);
 
 	if (index < 0)
 		return -EINVAL;
@@ -134,6 +134,8 @@ static int fintek_8250_rs4850_config(struct uart_8250_port *uart,
 	outb(config, DATA_PORT);
 	fintek_8250_exit_key();
 
+	port->rs485 = *rs485;
+
 	return 0;
 }
 
@@ -166,7 +168,7 @@ fintek_8250_probe(struct pnp_dev *dev, const struct pnp_device_id *dev_id)
 	uart.port.irq = pnp_irq(dev, 0);
 	uart.port.iobase = pnp_port_start(dev, 0);
 	uart.port.iotype = UPIO_PORT;
-	uart.rs485_config = fintek_8250_rs4850_config;
+	uart.port.rs485_config = fintek_8250_rs485_config;
 
 	uart.port.flags |= UPF_SKIP_TEST | UPF_BOOT_AUTOCONF;
 	if (pnp_irq_flags(dev, 0) & IORESOURCE_IRQ_SHAREABLE)
diff --git a/drivers/tty/serial/8250/8250_hp300.c b/drivers/tty/serial/8250/8250_hp300.c
index afffe4d1f034..b4882082b247 100644
--- a/drivers/tty/serial/8250/8250_hp300.c
+++ b/drivers/tty/serial/8250/8250_hp300.c
@@ -88,10 +88,6 @@ extern int hp300_uart_scode;
 /*
  * Parse the bootinfo to find descriptions for headless console and
  * debug serial ports and register them with the 8250 driver.
- * This function should be called before serial_console_init() is called
- * to make sure the serial console will be available for use. IA-64 kernel
- * calls this function from setup_arch() after the EFI and ACPI tables have
- * been parsed.
  */
 int __init hp300_setup_serial_console(void)
 {
diff --git a/drivers/tty/serial/8250/8250_mtk.c b/drivers/tty/serial/8250/8250_mtk.c
index de7aae523b37..7a11fac775c4 100644
--- a/drivers/tty/serial/8250/8250_mtk.c
+++ b/drivers/tty/serial/8250/8250_mtk.c
@@ -74,14 +74,14 @@ mtk8250_set_termios(struct uart_port *port, struct ktermios *termios,
 		/* Set to next lower baudrate supported */
 		if ((baud == 500000) || (baud == 576000))
 			baud = 460800;
-		quot = DIV_ROUND_CLOSEST(port->uartclk, 4 * baud);
+		quot = DIV_ROUND_UP(port->uartclk, 4 * baud);
 	} else {
 		serial_port_out(port, UART_MTK_HIGHS, 0x3);
 
 		/* Set to highest baudrate supported */
 		if (baud >= 1152000)
 			baud = 921600;
-		quot = (port->uartclk / (256 * baud)) + 1;
+		quot = DIV_ROUND_UP(port->uartclk, 256 * baud);
 	}
 
 	/*
@@ -244,7 +244,7 @@ static int mtk8250_resume(struct device *dev)
 }
 #endif /* CONFIG_PM_SLEEP */
 
-#ifdef CONFIG_PM_RUNTIME
+#ifdef CONFIG_PM
 static int mtk8250_runtime_suspend(struct device *dev)
 {
 	struct mtk8250_data *data = dev_get_drvdata(dev);
diff --git a/drivers/tty/serial/8250/8250_omap.c b/drivers/tty/serial/8250/8250_omap.c
new file mode 100644
index 000000000000..96b69bfd773f
--- /dev/null
+++ b/drivers/tty/serial/8250/8250_omap.c
@@ -0,0 +1,1281 @@
+/*
+ * 8250-core based driver for the OMAP internal UART
+ *
+ * based on omap-serial.c, Copyright (C) 2010 Texas Instruments.
+ *
+ * Copyright (C) 2014 Sebastian Andrzej Siewior
+ *
+ */
+
+#include <linux/device.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/tty_flip.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+#include <linux/of_irq.h>
+#include <linux/delay.h>
+#include <linux/pm_runtime.h>
+#include <linux/console.h>
+#include <linux/pm_qos.h>
+#include <linux/dma-mapping.h>
+
+#include "8250.h"
+
+#define DEFAULT_CLK_SPEED	48000000
+
+#define UART_ERRATA_i202_MDR1_ACCESS	(1 << 0)
+#define OMAP_UART_WER_HAS_TX_WAKEUP	(1 << 1)
+#define OMAP_DMA_TX_KICK		(1 << 2)
+
+#define OMAP_UART_FCR_RX_TRIG		6
+#define OMAP_UART_FCR_TX_TRIG		4
+
+/* SCR register bitmasks */
+#define OMAP_UART_SCR_RX_TRIG_GRANU1_MASK	(1 << 7)
+#define OMAP_UART_SCR_TX_TRIG_GRANU1_MASK	(1 << 6)
+#define OMAP_UART_SCR_TX_EMPTY			(1 << 3)
+#define OMAP_UART_SCR_DMAMODE_MASK		(3 << 1)
+#define OMAP_UART_SCR_DMAMODE_1			(1 << 1)
+#define OMAP_UART_SCR_DMAMODE_CTL		(1 << 0)
+
+/* 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
+
+#define UART_TI752_TLR_TX	0
+#define UART_TI752_TLR_RX	4
+
+#define TRIGGER_TLR_MASK(x)	((x & 0x3c) >> 2)
+#define TRIGGER_FCR_MASK(x)	(x & 3)
+
+/* Enable XON/XOFF flow control on output */
+#define OMAP_UART_SW_TX		0x08
+/* Enable XON/XOFF flow control on input */
+#define OMAP_UART_SW_RX		0x02
+
+#define OMAP_UART_WER_MOD_WKUP	0x7f
+#define OMAP_UART_TX_WAKEUP_EN	(1 << 7)
+
+#define TX_TRIGGER	1
+#define RX_TRIGGER	48
+
+#define OMAP_UART_TCR_RESTORE(x)	((x / 4) << 4)
+#define OMAP_UART_TCR_HALT(x)		((x / 4) << 0)
+
+#define UART_BUILD_REVISION(x, y)	(((x) << 8) | (y))
+
+#define OMAP_UART_REV_46 0x0406
+#define OMAP_UART_REV_52 0x0502
+#define OMAP_UART_REV_63 0x0603
+
+struct omap8250_priv {
+	int line;
+	u8 habit;
+	u8 mdr1;
+	u8 efr;
+	u8 scr;
+	u8 wer;
+	u8 xon;
+	u8 xoff;
+	u8 delayed_restore;
+	u16 quot;
+
+	bool is_suspending;
+	int wakeirq;
+	int wakeups_enabled;
+	u32 latency;
+	u32 calc_latency;
+	struct pm_qos_request pm_qos_request;
+	struct work_struct qos_work;
+	struct uart_8250_dma omap8250_dma;
+};
+
+static u32 uart_read(struct uart_8250_port *up, u32 reg)
+{
+	return readl(up->port.membase + (reg << up->port.regshift));
+}
+
+/*
+ * Work Around for Errata i202 (2430, 3430, 3630, 4430 and 4460)
+ * The access to uart register after MDR1 Access
+ * causes UART to corrupt data.
+ *
+ * Need a delay =
+ * 5 L4 clock cycles + 5 UART functional clock cycle (@48MHz = ~0.2uS)
+ * give 10 times as much
+ */
+static void omap_8250_mdr1_errataset(struct uart_8250_port *up,
+				     struct omap8250_priv *priv)
+{
+	u8 timeout = 255;
+	u8 old_mdr1;
+
+	old_mdr1 = serial_in(up, UART_OMAP_MDR1);
+	if (old_mdr1 == priv->mdr1)
+		return;
+
+	serial_out(up, UART_OMAP_MDR1, priv->mdr1);
+	udelay(2);
+	serial_out(up, UART_FCR, up->fcr | UART_FCR_CLEAR_XMIT |
+			UART_FCR_CLEAR_RCVR);
+	/*
+	 * Wait for FIFO to empty: when empty, RX_FIFO_E bit is 0 and
+	 * TX_FIFO_E bit is 1.
+	 */
+	while (UART_LSR_THRE != (serial_in(up, UART_LSR) &
+				(UART_LSR_THRE | UART_LSR_DR))) {
+		timeout--;
+		if (!timeout) {
+			/* Should *never* happen. we warn and carry on */
+			dev_crit(up->port.dev, "Errata i202: timedout %x\n",
+				 serial_in(up, UART_LSR));
+			break;
+		}
+		udelay(1);
+	}
+}
+
+static void omap_8250_get_divisor(struct uart_port *port, unsigned int baud,
+				  struct omap8250_priv *priv)
+{
+	unsigned int uartclk = port->uartclk;
+	unsigned int div_13, div_16;
+	unsigned int abs_d13, abs_d16;
+
+	/*
+	 * Old custom speed handling.
+	 */
+	if (baud == 38400 && (port->flags & UPF_SPD_MASK) == UPF_SPD_CUST) {
+		priv->quot = port->custom_divisor & 0xffff;
+		/*
+		 * I assume that nobody is using this. But hey, if somebody
+		 * would like to specify the divisor _and_ the mode then the
+		 * driver is ready and waiting for it.
+		 */
+		if (port->custom_divisor & (1 << 16))
+			priv->mdr1 = UART_OMAP_MDR1_13X_MODE;
+		else
+			priv->mdr1 = UART_OMAP_MDR1_16X_MODE;
+		return;
+	}
+	div_13 = DIV_ROUND_CLOSEST(uartclk, 13 * baud);
+	div_16 = DIV_ROUND_CLOSEST(uartclk, 16 * baud);
+
+	if (!div_13)
+		div_13 = 1;
+	if (!div_16)
+		div_16 = 1;
+
+	abs_d13 = abs(baud - uartclk / 13 / div_13);
+	abs_d16 = abs(baud - uartclk / 16 / div_16);
+
+	if (abs_d13 >= abs_d16) {
+		priv->mdr1 = UART_OMAP_MDR1_16X_MODE;
+		priv->quot = div_16;
+	} else {
+		priv->mdr1 = UART_OMAP_MDR1_13X_MODE;
+		priv->quot = div_13;
+	}
+}
+
+static void omap8250_update_scr(struct uart_8250_port *up,
+				struct omap8250_priv *priv)
+{
+	u8 old_scr;
+
+	old_scr = serial_in(up, UART_OMAP_SCR);
+	if (old_scr == priv->scr)
+		return;
+
+	/*
+	 * The manual recommends not to enable the DMA mode selector in the SCR
+	 * (instead of the FCR) register _and_ selecting the DMA mode as one
+	 * register write because this may lead to malfunction.
+	 */
+	if (priv->scr & OMAP_UART_SCR_DMAMODE_MASK)
+		serial_out(up, UART_OMAP_SCR,
+			   priv->scr & ~OMAP_UART_SCR_DMAMODE_MASK);
+	serial_out(up, UART_OMAP_SCR, priv->scr);
+}
+
+static void omap8250_restore_regs(struct uart_8250_port *up)
+{
+	struct omap8250_priv *priv = up->port.private_data;
+	struct uart_8250_dma	*dma = up->dma;
+
+	if (dma && dma->tx_running) {
+		/*
+		 * TCSANOW requests the change to occur immediately however if
+		 * we have a TX-DMA operation in progress then it has been
+		 * observed that it might stall and never complete. Therefore we
+		 * delay DMA completes to prevent this hang from happen.
+		 */
+		priv->delayed_restore = 1;
+		return;
+	}
+
+	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_B);
+	serial_out(up, UART_EFR, UART_EFR_ECB);
+
+	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_A);
+	serial_out(up, UART_MCR, UART_MCR_TCRTLR);
+	serial_out(up, UART_FCR, up->fcr);
+
+	omap8250_update_scr(up, priv);
+
+	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_B);
+
+	serial_out(up, UART_TI752_TCR, OMAP_UART_TCR_RESTORE(16) |
+			OMAP_UART_TCR_HALT(52));
+	serial_out(up, UART_TI752_TLR,
+		   TRIGGER_TLR_MASK(TX_TRIGGER) << UART_TI752_TLR_TX |
+		   TRIGGER_TLR_MASK(RX_TRIGGER) << UART_TI752_TLR_RX);
+
+	serial_out(up, UART_LCR, 0);
+
+	/* drop TCR + TLR access, we setup XON/XOFF later */
+	serial_out(up, UART_MCR, up->mcr);
+	serial_out(up, UART_IER, up->ier);
+
+	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_B);
+	serial_dl_write(up, priv->quot);
+
+	serial_out(up, UART_EFR, priv->efr);
+
+	/* Configure flow control */
+	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_B);
+	serial_out(up, UART_XON1, priv->xon);
+	serial_out(up, UART_XOFF1, priv->xoff);
+
+	serial_out(up, UART_LCR, up->lcr);
+	/* need mode A for FCR */
+	if (priv->habit & UART_ERRATA_i202_MDR1_ACCESS)
+		omap_8250_mdr1_errataset(up, priv);
+	else
+		serial_out(up, UART_OMAP_MDR1, priv->mdr1);
+	up->port.ops->set_mctrl(&up->port, up->port.mctrl);
+}
+
+/*
+ * OMAP can use "CLK / (16 or 13) / div" for baud rate. And then we have have
+ * some differences in how we want to handle flow control.
+ */
+static void omap_8250_set_termios(struct uart_port *port,
+				  struct ktermios *termios,
+				  struct ktermios *old)
+{
+	struct uart_8250_port *up =
+		container_of(port, struct uart_8250_port, port);
+	struct omap8250_priv *priv = up->port.private_data;
+	unsigned char cval = 0;
+	unsigned int baud;
+
+	switch (termios->c_cflag & CSIZE) {
+	case CS5:
+		cval = UART_LCR_WLEN5;
+		break;
+	case CS6:
+		cval = UART_LCR_WLEN6;
+		break;
+	case CS7:
+		cval = UART_LCR_WLEN7;
+		break;
+	default:
+	case CS8:
+		cval = UART_LCR_WLEN8;
+		break;
+	}
+
+	if (termios->c_cflag & CSTOPB)
+		cval |= UART_LCR_STOP;
+	if (termios->c_cflag & PARENB)
+		cval |= UART_LCR_PARITY;
+	if (!(termios->c_cflag & PARODD))
+		cval |= UART_LCR_EPAR;
+	if (termios->c_cflag & CMSPAR)
+		cval |= UART_LCR_SPAR;
+
+	/*
+	 * Ask the core to calculate the divisor for us.
+	 */
+	baud = uart_get_baud_rate(port, termios, old,
+				  port->uartclk / 16 / 0xffff,
+				  port->uartclk / 13);
+	omap_8250_get_divisor(port, baud, priv);
+
+	/*
+	 * Ok, we're now changing the port state. Do it with
+	 * interrupts disabled.
+	 */
+	pm_runtime_get_sync(port->dev);
+	spin_lock_irq(&port->lock);
+
+	/*
+	 * Update the per-port timeout.
+	 */
+	uart_update_timeout(port, termios->c_cflag, baud);
+
+	up->port.read_status_mask = UART_LSR_OE | UART_LSR_THRE | UART_LSR_DR;
+	if (termios->c_iflag & INPCK)
+		up->port.read_status_mask |= UART_LSR_FE | UART_LSR_PE;
+	if (termios->c_iflag & (IGNBRK | PARMRK))
+		up->port.read_status_mask |= UART_LSR_BI;
+
+	/*
+	 * Characters to ignore
+	 */
+	up->port.ignore_status_mask = 0;
+	if (termios->c_iflag & IGNPAR)
+		up->port.ignore_status_mask |= UART_LSR_PE | UART_LSR_FE;
+	if (termios->c_iflag & IGNBRK) {
+		up->port.ignore_status_mask |= UART_LSR_BI;
+		/*
+		 * If we're ignoring parity and break indicators,
+		 * ignore overruns too (for real raw support).
+		 */
+		if (termios->c_iflag & IGNPAR)
+			up->port.ignore_status_mask |= UART_LSR_OE;
+	}
+
+	/*
+	 * ignore all characters if CREAD is not set
+	 */
+	if ((termios->c_cflag & CREAD) == 0)
+		up->port.ignore_status_mask |= UART_LSR_DR;
+
+	/*
+	 * Modem status interrupts
+	 */
+	up->ier &= ~UART_IER_MSI;
+	if (UART_ENABLE_MS(&up->port, termios->c_cflag))
+		up->ier |= UART_IER_MSI;
+
+	up->lcr = cval;
+	/* Up to here it was mostly serial8250_do_set_termios() */
+
+	/*
+	 * We enable TRIG_GRANU for RX and TX and additionaly we set
+	 * SCR_TX_EMPTY bit. The result is the following:
+	 * - RX_TRIGGER amount of bytes in the FIFO will cause an interrupt.
+	 * - less than RX_TRIGGER number of bytes will also cause an interrupt
+	 *   once the UART decides that there no new bytes arriving.
+	 * - Once THRE is enabled, the interrupt will be fired once the FIFO is
+	 *   empty - the trigger level is ignored here.
+	 *
+	 * Once DMA is enabled:
+	 * - UART will assert the TX DMA line once there is room for TX_TRIGGER
+	 *   bytes in the TX FIFO. On each assert the DMA engine will move
+	 *   TX_TRIGGER bytes into the FIFO.
+	 * - UART will assert the RX DMA line once there are RX_TRIGGER bytes in
+	 *   the FIFO and move RX_TRIGGER bytes.
+	 * This is because threshold and trigger values are the same.
+	 */
+	up->fcr = UART_FCR_ENABLE_FIFO;
+	up->fcr |= TRIGGER_FCR_MASK(TX_TRIGGER) << OMAP_UART_FCR_TX_TRIG;
+	up->fcr |= TRIGGER_FCR_MASK(RX_TRIGGER) << OMAP_UART_FCR_RX_TRIG;
+
+	priv->scr = OMAP_UART_SCR_RX_TRIG_GRANU1_MASK | OMAP_UART_SCR_TX_EMPTY |
+		OMAP_UART_SCR_TX_TRIG_GRANU1_MASK;
+
+	if (up->dma)
+		priv->scr |= OMAP_UART_SCR_DMAMODE_1 |
+			OMAP_UART_SCR_DMAMODE_CTL;
+
+	priv->xon = termios->c_cc[VSTART];
+	priv->xoff = termios->c_cc[VSTOP];
+
+	priv->efr = 0;
+	up->mcr &= ~(UART_MCR_RTS | UART_MCR_XONANY);
+	if (termios->c_cflag & CRTSCTS && up->port.flags & UPF_HARD_FLOW) {
+		/* Enable AUTORTS and AUTOCTS */
+		priv->efr |= UART_EFR_CTS | UART_EFR_RTS;
+
+		/* Ensure MCR RTS is asserted */
+		up->mcr |= UART_MCR_RTS;
+	} else	if (up->port.flags & UPF_SOFT_FLOW) {
+		/*
+		 * IXON Flag:
+		 * Enable XON/XOFF flow control on input.
+		 * Receiver compares XON1, XOFF1.
+		 */
+		if (termios->c_iflag & IXON)
+			priv->efr |= OMAP_UART_SW_RX;
+
+		/*
+		 * IXOFF Flag:
+		 * Enable XON/XOFF flow control on output.
+		 * Transmit XON1, XOFF1
+		 */
+		if (termios->c_iflag & IXOFF)
+			priv->efr |= OMAP_UART_SW_TX;
+
+		/*
+		 * IXANY Flag:
+		 * Enable any character to restart output.
+		 * Operation resumes after receiving any
+		 * character after recognition of the XOFF character
+		 */
+		if (termios->c_iflag & IXANY)
+			up->mcr |= UART_MCR_XONANY;
+	}
+	omap8250_restore_regs(up);
+
+	spin_unlock_irq(&up->port.lock);
+	pm_runtime_mark_last_busy(port->dev);
+	pm_runtime_put_autosuspend(port->dev);
+
+	/* calculate wakeup latency constraint */
+	priv->calc_latency = USEC_PER_SEC * 64 * 8 / baud;
+	priv->latency = priv->calc_latency;
+
+	schedule_work(&priv->qos_work);
+
+	/* Don't rewrite B0 */
+	if (tty_termios_baud_rate(termios))
+		tty_termios_encode_baud_rate(termios, baud, baud);
+}
+
+/* same as 8250 except that we may have extra flow bits set in EFR */
+static void omap_8250_pm(struct uart_port *port, unsigned int state,
+			 unsigned int oldstate)
+{
+	struct uart_8250_port *up =
+		container_of(port, struct uart_8250_port, port);
+	struct omap8250_priv *priv = up->port.private_data;
+
+	pm_runtime_get_sync(port->dev);
+	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_B);
+	serial_out(up, UART_EFR, priv->efr | UART_EFR_ECB);
+	serial_out(up, UART_LCR, 0);
+
+	serial_out(up, UART_IER, (state != 0) ? UART_IERX_SLEEP : 0);
+	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_B);
+	serial_out(up, UART_EFR, priv->efr);
+	serial_out(up, UART_LCR, 0);
+
+	pm_runtime_mark_last_busy(port->dev);
+	pm_runtime_put_autosuspend(port->dev);
+}
+
+static void omap_serial_fill_features_erratas(struct uart_8250_port *up,
+					      struct omap8250_priv *priv)
+{
+	u32 mvr, scheme;
+	u16 revision, major, minor;
+
+	mvr = uart_read(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->port.dev,
+			 "Unknown revision, defaulting to highest\n");
+		/* 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:
+		priv->habit = UART_ERRATA_i202_MDR1_ACCESS;
+		break;
+	case OMAP_UART_REV_52:
+		priv->habit = UART_ERRATA_i202_MDR1_ACCESS |
+				OMAP_UART_WER_HAS_TX_WAKEUP;
+		break;
+	case OMAP_UART_REV_63:
+		priv->habit = UART_ERRATA_i202_MDR1_ACCESS |
+			OMAP_UART_WER_HAS_TX_WAKEUP;
+		break;
+	default:
+		break;
+	}
+}
+
+static void omap8250_uart_qos_work(struct work_struct *work)
+{
+	struct omap8250_priv *priv;
+
+	priv = container_of(work, struct omap8250_priv, qos_work);
+	pm_qos_update_request(&priv->pm_qos_request, priv->latency);
+}
+
+static irqreturn_t omap_wake_irq(int irq, void *dev_id)
+{
+	struct uart_port *port = dev_id;
+	int ret;
+
+	ret = port->handle_irq(port);
+	if (ret)
+		return IRQ_HANDLED;
+	return IRQ_NONE;
+}
+
+static int omap_8250_startup(struct uart_port *port)
+{
+	struct uart_8250_port *up =
+		container_of(port, struct uart_8250_port, port);
+	struct omap8250_priv *priv = port->private_data;
+
+	int ret;
+
+	if (priv->wakeirq) {
+		ret = request_irq(priv->wakeirq, omap_wake_irq,
+				  port->irqflags, "uart wakeup irq", port);
+		if (ret)
+			return ret;
+		disable_irq(priv->wakeirq);
+	}
+
+	pm_runtime_get_sync(port->dev);
+
+	ret = serial8250_do_startup(port);
+	if (ret)
+		goto err;
+
+#ifdef CONFIG_PM
+	up->capabilities |= UART_CAP_RPM;
+#endif
+
+	/* Enable module level wake up */
+	priv->wer = OMAP_UART_WER_MOD_WKUP;
+	if (priv->habit & OMAP_UART_WER_HAS_TX_WAKEUP)
+		priv->wer |= OMAP_UART_TX_WAKEUP_EN;
+	serial_out(up, UART_OMAP_WER, priv->wer);
+
+	if (up->dma)
+		up->dma->rx_dma(up, 0);
+
+	pm_runtime_mark_last_busy(port->dev);
+	pm_runtime_put_autosuspend(port->dev);
+	return 0;
+err:
+	pm_runtime_mark_last_busy(port->dev);
+	pm_runtime_put_autosuspend(port->dev);
+	if (priv->wakeirq)
+		free_irq(priv->wakeirq, port);
+	return ret;
+}
+
+static void omap_8250_shutdown(struct uart_port *port)
+{
+	struct uart_8250_port *up =
+		container_of(port, struct uart_8250_port, port);
+	struct omap8250_priv *priv = port->private_data;
+
+	flush_work(&priv->qos_work);
+	if (up->dma)
+		up->dma->rx_dma(up, UART_IIR_RX_TIMEOUT);
+
+	pm_runtime_get_sync(port->dev);
+
+	serial_out(up, UART_OMAP_WER, 0);
+	serial8250_do_shutdown(port);
+
+	pm_runtime_mark_last_busy(port->dev);
+	pm_runtime_put_autosuspend(port->dev);
+
+	if (priv->wakeirq)
+		free_irq(priv->wakeirq, port);
+}
+
+static void omap_8250_throttle(struct uart_port *port)
+{
+	unsigned long flags;
+	struct uart_8250_port *up =
+		container_of(port, struct uart_8250_port, port);
+
+	pm_runtime_get_sync(port->dev);
+
+	spin_lock_irqsave(&port->lock, flags);
+	up->ier &= ~(UART_IER_RLSI | UART_IER_RDI);
+	serial_out(up, UART_IER, up->ier);
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	pm_runtime_mark_last_busy(port->dev);
+	pm_runtime_put_autosuspend(port->dev);
+}
+
+static void omap_8250_unthrottle(struct uart_port *port)
+{
+	unsigned long flags;
+	struct uart_8250_port *up =
+		container_of(port, struct uart_8250_port, port);
+
+	pm_runtime_get_sync(port->dev);
+
+	spin_lock_irqsave(&port->lock, flags);
+	up->ier |= UART_IER_RLSI | UART_IER_RDI;
+	serial_out(up, UART_IER, up->ier);
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	pm_runtime_mark_last_busy(port->dev);
+	pm_runtime_put_autosuspend(port->dev);
+}
+
+#ifdef CONFIG_SERIAL_8250_DMA
+static int omap_8250_rx_dma(struct uart_8250_port *p, unsigned int iir);
+
+static void __dma_rx_do_complete(struct uart_8250_port *p, bool error)
+{
+	struct uart_8250_dma    *dma = p->dma;
+	struct tty_port         *tty_port = &p->port.state->port;
+	struct dma_tx_state     state;
+	int                     count;
+
+	dma_sync_single_for_cpu(dma->rxchan->device->dev, dma->rx_addr,
+				dma->rx_size, DMA_FROM_DEVICE);
+
+	dma->rx_running = 0;
+	dmaengine_tx_status(dma->rxchan, dma->rx_cookie, &state);
+	dmaengine_terminate_all(dma->rxchan);
+
+	count = dma->rx_size - state.residue;
+
+	tty_insert_flip_string(tty_port, dma->rx_buf, count);
+	p->port.icount.rx += count;
+	if (!error)
+		omap_8250_rx_dma(p, 0);
+
+	tty_flip_buffer_push(tty_port);
+}
+
+static void __dma_rx_complete(void *param)
+{
+	__dma_rx_do_complete(param, false);
+}
+
+static int omap_8250_rx_dma(struct uart_8250_port *p, unsigned int iir)
+{
+	struct uart_8250_dma            *dma = p->dma;
+	struct dma_async_tx_descriptor  *desc;
+
+	switch (iir & 0x3f) {
+	case UART_IIR_RLSI:
+		/* 8250_core handles errors and break interrupts */
+		if (dma->rx_running) {
+			dmaengine_pause(dma->rxchan);
+			__dma_rx_do_complete(p, true);
+		}
+		return -EIO;
+	case UART_IIR_RX_TIMEOUT:
+		/*
+		 * If RCVR FIFO trigger level was not reached, complete the
+		 * transfer and let 8250_core copy the remaining data.
+		 */
+		if (dma->rx_running) {
+			dmaengine_pause(dma->rxchan);
+			__dma_rx_do_complete(p, true);
+		}
+		return -ETIMEDOUT;
+	case UART_IIR_RDI:
+		/*
+		 * The OMAP UART is a special BEAST. If we receive RDI we _have_
+		 * a DMA transfer programmed but it didn't work. One reason is
+		 * that we were too slow and there were too many bytes in the
+		 * FIFO, the UART counted wrong and never kicked the DMA engine
+		 * to do anything. That means once we receive RDI on OMAP then
+		 * the DMA won't do anything soon so we have to cancel the DMA
+		 * transfer and purge the FIFO manually.
+		 */
+		if (dma->rx_running) {
+			dmaengine_pause(dma->rxchan);
+			__dma_rx_do_complete(p, true);
+		}
+		return -ETIMEDOUT;
+
+	default:
+		break;
+	}
+
+	if (dma->rx_running)
+		return 0;
+
+	desc = dmaengine_prep_slave_single(dma->rxchan, dma->rx_addr,
+					   dma->rx_size, DMA_DEV_TO_MEM,
+					   DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	if (!desc)
+		return -EBUSY;
+
+	dma->rx_running = 1;
+	desc->callback = __dma_rx_complete;
+	desc->callback_param = p;
+
+	dma->rx_cookie = dmaengine_submit(desc);
+
+	dma_sync_single_for_device(dma->rxchan->device->dev, dma->rx_addr,
+				   dma->rx_size, DMA_FROM_DEVICE);
+
+	dma_async_issue_pending(dma->rxchan);
+	return 0;
+}
+
+static int omap_8250_tx_dma(struct uart_8250_port *p);
+
+static void omap_8250_dma_tx_complete(void *param)
+{
+	struct uart_8250_port	*p = param;
+	struct uart_8250_dma	*dma = p->dma;
+	struct circ_buf		*xmit = &p->port.state->xmit;
+	unsigned long		flags;
+	bool			en_thri = false;
+	struct omap8250_priv	*priv = p->port.private_data;
+
+	dma_sync_single_for_cpu(dma->txchan->device->dev, dma->tx_addr,
+				UART_XMIT_SIZE, DMA_TO_DEVICE);
+
+	spin_lock_irqsave(&p->port.lock, flags);
+
+	dma->tx_running = 0;
+
+	xmit->tail += dma->tx_size;
+	xmit->tail &= UART_XMIT_SIZE - 1;
+	p->port.icount.tx += dma->tx_size;
+
+	if (priv->delayed_restore) {
+		priv->delayed_restore = 0;
+		omap8250_restore_regs(p);
+	}
+
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+		uart_write_wakeup(&p->port);
+
+	if (!uart_circ_empty(xmit) && !uart_tx_stopped(&p->port)) {
+		int ret;
+
+		ret = omap_8250_tx_dma(p);
+		if (ret)
+			en_thri = true;
+
+	} else if (p->capabilities & UART_CAP_RPM) {
+		en_thri = true;
+	}
+
+	if (en_thri) {
+		dma->tx_err = 1;
+		p->ier |= UART_IER_THRI;
+		serial_port_out(&p->port, UART_IER, p->ier);
+	}
+
+	spin_unlock_irqrestore(&p->port.lock, flags);
+}
+
+static int omap_8250_tx_dma(struct uart_8250_port *p)
+{
+	struct uart_8250_dma		*dma = p->dma;
+	struct omap8250_priv		*priv = p->port.private_data;
+	struct circ_buf			*xmit = &p->port.state->xmit;
+	struct dma_async_tx_descriptor	*desc;
+	unsigned int	skip_byte = 0;
+	int ret;
+
+	if (dma->tx_running)
+		return 0;
+	if (uart_tx_stopped(&p->port) || uart_circ_empty(xmit)) {
+
+		/*
+		 * Even if no data, we need to return an error for the two cases
+		 * below so serial8250_tx_chars() is invoked and properly clears
+		 * THRI and/or runtime suspend.
+		 */
+		if (dma->tx_err || p->capabilities & UART_CAP_RPM) {
+			ret = -EBUSY;
+			goto err;
+		}
+		if (p->ier & UART_IER_THRI) {
+			p->ier &= ~UART_IER_THRI;
+			serial_out(p, UART_IER, p->ier);
+		}
+		return 0;
+	}
+
+	dma->tx_size = CIRC_CNT_TO_END(xmit->head, xmit->tail, UART_XMIT_SIZE);
+	if (priv->habit & OMAP_DMA_TX_KICK) {
+		u8 tx_lvl;
+
+		/*
+		 * We need to put the first byte into the FIFO in order to start
+		 * the DMA transfer. For transfers smaller than four bytes we
+		 * don't bother doing DMA at all. It seem not matter if there
+		 * are still bytes in the FIFO from the last transfer (in case
+		 * we got here directly from omap_8250_dma_tx_complete()). Bytes
+		 * leaving the FIFO seem not to trigger the DMA transfer. It is
+		 * really the byte that we put into the FIFO.
+		 * If the FIFO is already full then we most likely got here from
+		 * omap_8250_dma_tx_complete(). And this means the DMA engine
+		 * just completed its work. We don't have to wait the complete
+		 * 86us at 115200,8n1 but around 60us (not to mention lower
+		 * baudrates). So in that case we take the interrupt and try
+		 * again with an empty FIFO.
+		 */
+		tx_lvl = serial_in(p, UART_OMAP_TX_LVL);
+		if (tx_lvl == p->tx_loadsz) {
+			ret = -EBUSY;
+			goto err;
+		}
+		if (dma->tx_size < 4) {
+			ret = -EINVAL;
+			goto err;
+		}
+		skip_byte = 1;
+	}
+
+	desc = dmaengine_prep_slave_single(dma->txchan,
+			dma->tx_addr + xmit->tail + skip_byte,
+			dma->tx_size - skip_byte, DMA_MEM_TO_DEV,
+			DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	if (!desc) {
+		ret = -EBUSY;
+		goto err;
+	}
+
+	dma->tx_running = 1;
+
+	desc->callback = omap_8250_dma_tx_complete;
+	desc->callback_param = p;
+
+	dma->tx_cookie = dmaengine_submit(desc);
+
+	dma_sync_single_for_device(dma->txchan->device->dev, dma->tx_addr,
+				   UART_XMIT_SIZE, DMA_TO_DEVICE);
+
+	dma_async_issue_pending(dma->txchan);
+	if (dma->tx_err)
+		dma->tx_err = 0;
+
+	if (p->ier & UART_IER_THRI) {
+		p->ier &= ~UART_IER_THRI;
+		serial_out(p, UART_IER, p->ier);
+	}
+	if (skip_byte)
+		serial_out(p, UART_TX, xmit->buf[xmit->tail]);
+	return 0;
+err:
+	dma->tx_err = 1;
+	return ret;
+}
+
+/*
+ * This is mostly serial8250_handle_irq(). We have a slightly different DMA
+ * hoook for RX/TX and need different logic for them in the ISR. Therefore we
+ * use the default routine in the non-DMA case and this one for with DMA.
+ */
+static int omap_8250_dma_handle_irq(struct uart_port *port)
+{
+	struct uart_8250_port *up = up_to_u8250p(port);
+	unsigned char status;
+	unsigned long flags;
+	u8 iir;
+	int dma_err = 0;
+
+	serial8250_rpm_get(up);
+
+	iir = serial_port_in(port, UART_IIR);
+	if (iir & UART_IIR_NO_INT) {
+		serial8250_rpm_put(up);
+		return 0;
+	}
+
+	spin_lock_irqsave(&port->lock, flags);
+
+	status = serial_port_in(port, UART_LSR);
+
+	if (status & (UART_LSR_DR | UART_LSR_BI)) {
+
+		dma_err = omap_8250_rx_dma(up, iir);
+		if (dma_err) {
+			status = serial8250_rx_chars(up, status);
+			omap_8250_rx_dma(up, 0);
+		}
+	}
+	serial8250_modem_status(up);
+	if (status & UART_LSR_THRE && up->dma->tx_err) {
+		if (uart_tx_stopped(&up->port) ||
+		    uart_circ_empty(&up->port.state->xmit)) {
+			up->dma->tx_err = 0;
+			serial8250_tx_chars(up);
+		} else  {
+			/*
+			 * try again due to an earlier failer which
+			 * might have been resolved by now.
+			 */
+			dma_err = omap_8250_tx_dma(up);
+			if (dma_err)
+				serial8250_tx_chars(up);
+		}
+	}
+
+	spin_unlock_irqrestore(&port->lock, flags);
+	serial8250_rpm_put(up);
+	return 1;
+}
+
+static bool the_no_dma_filter_fn(struct dma_chan *chan, void *param)
+{
+	return false;
+}
+
+#else
+
+static inline int omap_8250_rx_dma(struct uart_8250_port *p, unsigned int iir)
+{
+	return -EINVAL;
+}
+#endif
+
+static int omap8250_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 omap8250_priv *priv;
+	struct uart_8250_port up;
+	int ret;
+	void __iomem *membase;
+
+	if (!regs || !irq) {
+		dev_err(&pdev->dev, "missing registers or irq\n");
+		return -EINVAL;
+	}
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	membase = devm_ioremap_nocache(&pdev->dev, regs->start,
+				       resource_size(regs));
+	if (!membase)
+		return -ENODEV;
+
+	memset(&up, 0, sizeof(up));
+	up.port.dev = &pdev->dev;
+	up.port.mapbase = regs->start;
+	up.port.membase = membase;
+	up.port.irq = irq->start;
+	/*
+	 * It claims to be 16C750 compatible however it is a little different.
+	 * It has EFR and has no FCR7_64byte bit. The AFE (which it claims to
+	 * have) is enabled via EFR instead of MCR. The type is set here 8250
+	 * just to get things going. UNKNOWN does not work for a few reasons and
+	 * we don't need our own type since we don't use 8250's set_termios()
+	 * or pm callback.
+	 */
+	up.port.type = PORT_8250;
+	up.port.iotype = UPIO_MEM;
+	up.port.flags = UPF_FIXED_PORT | UPF_FIXED_TYPE | UPF_SOFT_FLOW |
+		UPF_HARD_FLOW;
+	up.port.private_data = priv;
+
+	up.port.regshift = 2;
+	up.port.fifosize = 64;
+	up.tx_loadsz = 64;
+	up.capabilities = UART_CAP_FIFO;
+#ifdef CONFIG_PM
+	/*
+	 * Runtime PM is mostly transparent. However to do it right we need to a
+	 * TX empty interrupt before we can put the device to auto idle. So if
+	 * PM is not enabled we don't add that flag and can spare that one extra
+	 * interrupt in the TX path.
+	 */
+	up.capabilities |= UART_CAP_RPM;
+#endif
+	up.port.set_termios = omap_8250_set_termios;
+	up.port.pm = omap_8250_pm;
+	up.port.startup = omap_8250_startup;
+	up.port.shutdown = omap_8250_shutdown;
+	up.port.throttle = omap_8250_throttle;
+	up.port.unthrottle = omap_8250_unthrottle;
+
+	if (pdev->dev.of_node) {
+		ret = of_alias_get_id(pdev->dev.of_node, "serial");
+
+		of_property_read_u32(pdev->dev.of_node, "clock-frequency",
+				     &up.port.uartclk);
+		priv->wakeirq = irq_of_parse_and_map(pdev->dev.of_node, 1);
+	} else {
+		ret = pdev->id;
+	}
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to get alias/pdev id\n");
+		return ret;
+	}
+	up.port.line = ret;
+
+	if (!up.port.uartclk) {
+		up.port.uartclk = DEFAULT_CLK_SPEED;
+		dev_warn(&pdev->dev,
+			 "No clock speed specified: using default: %d\n",
+			 DEFAULT_CLK_SPEED);
+	}
+
+	priv->latency = PM_QOS_CPU_DMA_LAT_DEFAULT_VALUE;
+	priv->calc_latency = PM_QOS_CPU_DMA_LAT_DEFAULT_VALUE;
+	pm_qos_add_request(&priv->pm_qos_request, PM_QOS_CPU_DMA_LATENCY,
+			   priv->latency);
+	INIT_WORK(&priv->qos_work, omap8250_uart_qos_work);
+
+	device_init_wakeup(&pdev->dev, true);
+	pm_runtime_use_autosuspend(&pdev->dev);
+	pm_runtime_set_autosuspend_delay(&pdev->dev, -1);
+
+	pm_runtime_irq_safe(&pdev->dev);
+	pm_runtime_enable(&pdev->dev);
+
+	pm_runtime_get_sync(&pdev->dev);
+
+	omap_serial_fill_features_erratas(&up, priv);
+#ifdef CONFIG_SERIAL_8250_DMA
+	if (pdev->dev.of_node) {
+		/*
+		 * Oh DMA support. If there are no DMA properties in the DT then
+		 * we will fall back to a generic DMA channel which does not
+		 * really work here. To ensure that we do not get a generic DMA
+		 * channel assigned, we have the the_no_dma_filter_fn() here.
+		 * To avoid "failed to request DMA" messages we check for DMA
+		 * properties in DT.
+		 */
+		ret = of_property_count_strings(pdev->dev.of_node, "dma-names");
+		if (ret == 2) {
+			up.dma = &priv->omap8250_dma;
+			up.port.handle_irq = omap_8250_dma_handle_irq;
+			priv->omap8250_dma.fn = the_no_dma_filter_fn;
+			priv->omap8250_dma.tx_dma = omap_8250_tx_dma;
+			priv->omap8250_dma.rx_dma = omap_8250_rx_dma;
+			priv->omap8250_dma.rx_size = RX_TRIGGER;
+			priv->omap8250_dma.rxconf.src_maxburst = RX_TRIGGER;
+			priv->omap8250_dma.txconf.dst_maxburst = TX_TRIGGER;
+
+			if (of_machine_is_compatible("ti,am33xx"))
+				priv->habit |= OMAP_DMA_TX_KICK;
+		}
+	}
+#endif
+	ret = serial8250_register_8250_port(&up);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "unable to register 8250 port\n");
+		goto err;
+	}
+	priv->line = ret;
+	platform_set_drvdata(pdev, priv);
+	pm_runtime_mark_last_busy(&pdev->dev);
+	pm_runtime_put_autosuspend(&pdev->dev);
+	return 0;
+err:
+	pm_runtime_put(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+	return ret;
+}
+
+static int omap8250_remove(struct platform_device *pdev)
+{
+	struct omap8250_priv *priv = platform_get_drvdata(pdev);
+
+	pm_runtime_put_sync(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+	serial8250_unregister_port(priv->line);
+	pm_qos_remove_request(&priv->pm_qos_request);
+	device_init_wakeup(&pdev->dev, false);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+
+static inline void omap8250_enable_wakeirq(struct omap8250_priv *priv,
+					   bool enable)
+{
+	if (!priv->wakeirq)
+		return;
+
+	if (enable)
+		enable_irq(priv->wakeirq);
+	else
+		disable_irq_nosync(priv->wakeirq);
+}
+
+static void omap8250_enable_wakeup(struct omap8250_priv *priv,
+				   bool enable)
+{
+	if (enable == priv->wakeups_enabled)
+		return;
+
+	omap8250_enable_wakeirq(priv, enable);
+	priv->wakeups_enabled = enable;
+}
+#endif
+
+#ifdef CONFIG_PM_SLEEP
+static int omap8250_prepare(struct device *dev)
+{
+	struct omap8250_priv *priv = dev_get_drvdata(dev);
+
+	if (!priv)
+		return 0;
+	priv->is_suspending = true;
+	return 0;
+}
+
+static void omap8250_complete(struct device *dev)
+{
+	struct omap8250_priv *priv = dev_get_drvdata(dev);
+
+	if (!priv)
+		return;
+	priv->is_suspending = false;
+}
+
+static int omap8250_suspend(struct device *dev)
+{
+	struct omap8250_priv *priv = dev_get_drvdata(dev);
+
+	serial8250_suspend_port(priv->line);
+	flush_work(&priv->qos_work);
+
+	if (device_may_wakeup(dev))
+		omap8250_enable_wakeup(priv, true);
+	else
+		omap8250_enable_wakeup(priv, false);
+	return 0;
+}
+
+static int omap8250_resume(struct device *dev)
+{
+	struct omap8250_priv *priv = dev_get_drvdata(dev);
+
+	if (device_may_wakeup(dev))
+		omap8250_enable_wakeup(priv, false);
+
+	serial8250_resume_port(priv->line);
+	return 0;
+}
+#else
+#define omap8250_prepare NULL
+#define omap8250_complete NULL
+#endif
+
+#ifdef CONFIG_PM
+static int omap8250_lost_context(struct uart_8250_port *up)
+{
+	u32 val;
+
+	val = serial_in(up, UART_OMAP_MDR1);
+	/*
+	 * If we lose context, then MDR1 is set to its reset value which is
+	 * UART_OMAP_MDR1_DISABLE. After set_termios() we set it either to 13x
+	 * or 16x but never to disable again.
+	 */
+	if (val == UART_OMAP_MDR1_DISABLE)
+		return 1;
+	return 0;
+}
+
+static int omap8250_runtime_suspend(struct device *dev)
+{
+	struct omap8250_priv *priv = dev_get_drvdata(dev);
+	struct uart_8250_port *up;
+
+	up = serial8250_get_port(priv->line);
+	/*
+	 * When using 'no_console_suspend', the console UART must not be
+	 * suspended. Since driver suspend is managed by runtime suspend,
+	 * preventing runtime suspend (by returning error) will keep device
+	 * active during suspend.
+	 */
+	if (priv->is_suspending && !console_suspend_enabled) {
+		if (uart_console(&up->port))
+			return -EBUSY;
+	}
+
+	omap8250_enable_wakeup(priv, true);
+	if (up->dma)
+		omap_8250_rx_dma(up, UART_IIR_RX_TIMEOUT);
+
+	priv->latency = PM_QOS_CPU_DMA_LAT_DEFAULT_VALUE;
+	schedule_work(&priv->qos_work);
+
+	return 0;
+}
+
+static int omap8250_runtime_resume(struct device *dev)
+{
+	struct omap8250_priv *priv = dev_get_drvdata(dev);
+	struct uart_8250_port *up;
+	int loss_cntx;
+
+	/* In case runtime-pm tries this before we are setup */
+	if (!priv)
+		return 0;
+
+	up = serial8250_get_port(priv->line);
+	omap8250_enable_wakeup(priv, false);
+	loss_cntx = omap8250_lost_context(up);
+
+	if (loss_cntx)
+		omap8250_restore_regs(up);
+
+	if (up->dma)
+		omap_8250_rx_dma(up, 0);
+
+	priv->latency = priv->calc_latency;
+	schedule_work(&priv->qos_work);
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops omap8250_dev_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(omap8250_suspend, omap8250_resume)
+	SET_RUNTIME_PM_OPS(omap8250_runtime_suspend,
+			   omap8250_runtime_resume, NULL)
+	.prepare        = omap8250_prepare,
+	.complete       = omap8250_complete,
+};
+
+static const struct of_device_id omap8250_dt_ids[] = {
+	{ .compatible = "ti,omap2-uart" },
+	{ .compatible = "ti,omap3-uart" },
+	{ .compatible = "ti,omap4-uart" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, omap8250_dt_ids);
+
+static struct platform_driver omap8250_platform_driver = {
+	.driver = {
+		.name		= "omap8250",
+		.pm		= &omap8250_dev_pm_ops,
+		.of_match_table = omap8250_dt_ids,
+		.owner		= THIS_MODULE,
+	},
+	.probe			= omap8250_probe,
+	.remove			= omap8250_remove,
+};
+module_platform_driver(omap8250_platform_driver);
+
+MODULE_AUTHOR("Sebastian Andrzej Siewior");
+MODULE_DESCRIPTION("OMAP 8250 Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/tty/serial/8250/8250_pci.c b/drivers/tty/serial/8250/8250_pci.c
index beb9d71cd47a..31feeb2d0a66 100644
--- a/drivers/tty/serial/8250/8250_pci.c
+++ b/drivers/tty/serial/8250/8250_pci.c
@@ -79,29 +79,24 @@ setup_port(struct serial_private *priv, struct uart_8250_port *port,
 	   int bar, int offset, int regshift)
 {
 	struct pci_dev *dev = priv->dev;
-	unsigned long base, len;
 
 	if (bar >= PCI_NUM_BAR_RESOURCES)
 		return -EINVAL;
 
-	base = pci_resource_start(dev, bar);
-
 	if (pci_resource_flags(dev, bar) & IORESOURCE_MEM) {
-		len =  pci_resource_len(dev, bar);
-
 		if (!priv->remapped_bar[bar])
-			priv->remapped_bar[bar] = ioremap_nocache(base, len);
+			priv->remapped_bar[bar] = pci_ioremap_bar(dev, bar);
 		if (!priv->remapped_bar[bar])
 			return -ENOMEM;
 
 		port->port.iotype = UPIO_MEM;
 		port->port.iobase = 0;
-		port->port.mapbase = base + offset;
+		port->port.mapbase = pci_resource_start(dev, bar) + offset;
 		port->port.membase = priv->remapped_bar[bar] + offset;
 		port->port.regshift = regshift;
 	} else {
 		port->port.iotype = UPIO_PORT;
-		port->port.iobase = base + offset;
+		port->port.iobase = pci_resource_start(dev, bar) + offset;
 		port->port.mapbase = 0;
 		port->port.membase = NULL;
 		port->port.regshift = 0;
@@ -317,7 +312,6 @@ static void pci_plx9050_exit(struct pci_dev *dev)
 static void pci_ni8420_exit(struct pci_dev *dev)
 {
 	void __iomem *p;
-	unsigned long base, len;
 	unsigned int bar = 0;
 
 	if ((pci_resource_flags(dev, bar) & IORESOURCE_MEM) == 0) {
@@ -325,9 +319,7 @@ static void pci_ni8420_exit(struct pci_dev *dev)
 		return;
 	}
 
-	base = pci_resource_start(dev, bar);
-	len =  pci_resource_len(dev, bar);
-	p = ioremap_nocache(base, len);
+	p = pci_ioremap_bar(dev, bar);
 	if (p == NULL)
 		return;
 
@@ -349,7 +341,6 @@ static void pci_ni8420_exit(struct pci_dev *dev)
 static void pci_ni8430_exit(struct pci_dev *dev)
 {
 	void __iomem *p;
-	unsigned long base, len;
 	unsigned int bar = 0;
 
 	if ((pci_resource_flags(dev, bar) & IORESOURCE_MEM) == 0) {
@@ -357,9 +348,7 @@ static void pci_ni8430_exit(struct pci_dev *dev)
 		return;
 	}
 
-	base = pci_resource_start(dev, bar);
-	len =  pci_resource_len(dev, bar);
-	p = ioremap_nocache(base, len);
+	p = pci_ioremap_bar(dev, bar);
 	if (p == NULL)
 		return;
 
@@ -682,7 +671,6 @@ static int pci_xircom_init(struct pci_dev *dev)
 static int pci_ni8420_init(struct pci_dev *dev)
 {
 	void __iomem *p;
-	unsigned long base, len;
 	unsigned int bar = 0;
 
 	if ((pci_resource_flags(dev, bar) & IORESOURCE_MEM) == 0) {
@@ -690,9 +678,7 @@ static int pci_ni8420_init(struct pci_dev *dev)
 		return 0;
 	}
 
-	base = pci_resource_start(dev, bar);
-	len =  pci_resource_len(dev, bar);
-	p = ioremap_nocache(base, len);
+	p = pci_ioremap_bar(dev, bar);
 	if (p == NULL)
 		return -ENOMEM;
 
@@ -714,7 +700,7 @@ static int pci_ni8420_init(struct pci_dev *dev)
 static int pci_ni8430_init(struct pci_dev *dev)
 {
 	void __iomem *p;
-	unsigned long base, len;
+	struct pci_bus_region region;
 	u32 device_window;
 	unsigned int bar = 0;
 
@@ -723,14 +709,17 @@ static int pci_ni8430_init(struct pci_dev *dev)
 		return 0;
 	}
 
-	base = pci_resource_start(dev, bar);
-	len =  pci_resource_len(dev, bar);
-	p = ioremap_nocache(base, len);
+	p = pci_ioremap_bar(dev, bar);
 	if (p == NULL)
 		return -ENOMEM;
 
-	/* Set device window address and size in BAR0 */
-	device_window = ((base + MITE_IOWBSR1_WIN_OFFSET) & 0xffffff00)
+	/*
+	 * Set device window address and size in BAR0, while acknowledging that
+	 * the resource structure may contain a translated address that differs
+	 * from the address the device responds to.
+	 */
+	pcibios_resource_to_bus(dev->bus, &region, &dev->resource[bar]);
+	device_window = ((region.start + MITE_IOWBSR1_WIN_OFFSET) & 0xffffff00)
 	                | MITE_IOWBSR1_WENAB | MITE_IOWBSR1_WSIZE;
 	writel(device_window, p + MITE_IOWBSR1);
 
@@ -757,8 +746,8 @@ pci_ni8430_setup(struct serial_private *priv,
 		 const struct pciserial_board *board,
 		 struct uart_8250_port *port, int idx)
 {
+	struct pci_dev *dev = priv->dev;
 	void __iomem *p;
-	unsigned long base, len;
 	unsigned int bar, offset = board->first_offset;
 
 	if (idx >= board->num_ports)
@@ -767,9 +756,9 @@ pci_ni8430_setup(struct serial_private *priv,
 	bar = FL_GET_BASE(board->flags);
 	offset += idx * board->uart_offset;
 
-	base = pci_resource_start(priv->dev, bar);
-	len =  pci_resource_len(priv->dev, bar);
-	p = ioremap_nocache(base, len);
+	p = pci_ioremap_bar(dev, bar);
+	if (!p)
+		return -ENOMEM;
 
 	/* enable the transceiver */
 	writeb(readb(p + offset + NI8430_PORTCON) | NI8430_PORTCON_TXVR_ENABLE,
@@ -1003,6 +992,40 @@ static void pci_ite887x_exit(struct pci_dev *dev)
 }
 
 /*
+ * EndRun Technologies.
+ * Determine the number of ports available on the device.
+ */
+#define PCI_VENDOR_ID_ENDRUN			0x7401
+#define PCI_DEVICE_ID_ENDRUN_1588	0xe100
+
+static int pci_endrun_init(struct pci_dev *dev)
+{
+	u8 __iomem *p;
+	unsigned long deviceID;
+	unsigned int  number_uarts = 0;
+
+	/* EndRun device is all 0xexxx */
+	if (dev->vendor == PCI_VENDOR_ID_ENDRUN &&
+		(dev->device & 0xf000) != 0xe000)
+		return 0;
+
+	p = pci_iomap(dev, 0, 5);
+	if (p == NULL)
+		return -ENOMEM;
+
+	deviceID = ioread32(p);
+	/* EndRun device */
+	if (deviceID == 0x07000200) {
+		number_uarts = ioread8(p + 4);
+		dev_dbg(&dev->dev,
+			"%d ports detected on EndRun PCI Express device\n",
+			number_uarts);
+	}
+	pci_iounmap(dev, p);
+	return number_uarts;
+}
+
+/*
  * Oxford Semiconductor Inc.
  * Check that device is part of the Tornado range of devices, then determine
  * the number of ports available on the device.
@@ -1531,25 +1554,48 @@ static int pci_fintek_setup(struct serial_private *priv,
 	unsigned long iobase;
 	unsigned long ciobase = 0;
 	u8 config_base;
+	u32 bar_data[3];
 
 	/*
-	 * We are supposed to be able to read these from the PCI config space,
-	 * but the values there don't seem to match what we need to use, so
-	 * just use these hard-coded values for now, as they are correct.
+	 * Find each UARTs offset in PCI configuraion space
 	 */
 	switch (idx) {
-	case 0: iobase = 0xe000; config_base = 0x40; break;
-	case 1: iobase = 0xe008; config_base = 0x48; break;
-	case 2: iobase = 0xe010; config_base = 0x50; break;
-	case 3: iobase = 0xe018; config_base = 0x58; break;
-	case 4: iobase = 0xe020; config_base = 0x60; break;
-	case 5: iobase = 0xe028; config_base = 0x68; break;
-	case 6: iobase = 0xe030; config_base = 0x70; break;
-	case 7: iobase = 0xe038; config_base = 0x78; break;
-	case 8: iobase = 0xe040; config_base = 0x80; break;
-	case 9: iobase = 0xe048; config_base = 0x88; break;
-	case 10: iobase = 0xe050; config_base = 0x90; break;
-	case 11: iobase = 0xe058; config_base = 0x98; break;
+	case 0:
+		config_base = 0x40;
+		break;
+	case 1:
+		config_base = 0x48;
+		break;
+	case 2:
+		config_base = 0x50;
+		break;
+	case 3:
+		config_base = 0x58;
+		break;
+	case 4:
+		config_base = 0x60;
+		break;
+	case 5:
+		config_base = 0x68;
+		break;
+	case 6:
+		config_base = 0x70;
+		break;
+	case 7:
+		config_base = 0x78;
+		break;
+	case 8:
+		config_base = 0x80;
+		break;
+	case 9:
+		config_base = 0x88;
+		break;
+	case 10:
+		config_base = 0x90;
+		break;
+	case 11:
+		config_base = 0x98;
+		break;
 	default:
 		/* Unknown number of ports, get out of here */
 		return -EINVAL;
@@ -1560,6 +1606,14 @@ static int pci_fintek_setup(struct serial_private *priv,
 		ciobase = (int)(base + (0x8 * idx));
 	}
 
+	/* Get the io address dispatch from the BIOS */
+	pci_read_config_dword(pdev, 0x24, &bar_data[0]);
+	pci_read_config_dword(pdev, 0x20, &bar_data[1]);
+	pci_read_config_dword(pdev, 0x1c, &bar_data[2]);
+
+	/* Calculate Real IO Port */
+	iobase = (bar_data[idx/4] & 0xffffffe0) + (idx % 4) * 8;
+
 	dev_dbg(&pdev->dev, "%s: idx=%d iobase=0x%lx ciobase=0x%lx config_base=0x%2x\n",
 		__func__, idx, iobase, ciobase, config_base);
 
@@ -1760,6 +1814,16 @@ pci_wch_ch353_setup(struct serial_private *priv,
 	return pci_default_setup(priv, board, port, idx);
 }
 
+static int
+pci_wch_ch382_setup(struct serial_private *priv,
+                    const struct pciserial_board *board,
+                    struct uart_8250_port *port, int idx)
+{
+	port->port.flags |= UPF_FIXED_TYPE;
+	port->port.type = PORT_16850;
+	return pci_default_setup(priv, board, port, idx);
+}
+
 #define PCI_VENDOR_ID_SBSMODULARIO	0x124B
 #define PCI_SUBVENDOR_ID_SBSMODULARIO	0x124B
 #define PCI_DEVICE_ID_OCTPRO		0x0001
@@ -1814,6 +1878,8 @@ pci_wch_ch353_setup(struct serial_private *priv,
 #define PCI_VENDOR_ID_SUNIX		0x1fd4
 #define PCI_DEVICE_ID_SUNIX_1999	0x1999
 
+#define PCIE_VENDOR_ID_WCH		0x1c00
+#define PCIE_DEVICE_ID_WCH_CH382_2S1P	0x3250
 
 /* Unknown vendors/cards - this should not be in linux/pci_ids.h */
 #define PCI_SUBDEVICE_ID_UNKNOWN_0x1584	0x1584
@@ -2346,6 +2412,17 @@ static struct pci_serial_quirk pci_serial_quirks[] __refdata = {
 		.setup		= pci_netmos_9900_setup,
 	},
 	/*
+	 * EndRun Technologies
+	*/
+	{
+		.vendor		= PCI_VENDOR_ID_ENDRUN,
+		.device		= PCI_ANY_ID,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
+		.init		= pci_endrun_init,
+		.setup		= pci_default_setup,
+	},
+	/*
 	 * For Oxford Semiconductor Tornado based devices
 	 */
 	{
@@ -2494,6 +2571,14 @@ static struct pci_serial_quirk pci_serial_quirks[] __refdata = {
 		.subdevice	= PCI_ANY_ID,
 		.setup		= pci_wch_ch353_setup,
 	},
+	/* WCH CH382 2S1P card (16750 clone) */
+	{
+		.vendor         = PCIE_VENDOR_ID_WCH,
+		.device         = PCIE_DEVICE_ID_WCH_CH382_2S1P,
+		.subvendor      = PCI_ANY_ID,
+		.subdevice      = PCI_ANY_ID,
+		.setup          = pci_wch_ch382_setup,
+	},
 	/*
 	 * ASIX devices with FIFO bug
 	 */
@@ -2754,6 +2839,7 @@ enum pci_board_num_t {
 	pbn_panacom2,
 	pbn_panacom4,
 	pbn_plx_romulus,
+	pbn_endrun_2_4000000,
 	pbn_oxsemi,
 	pbn_oxsemi_1_4000000,
 	pbn_oxsemi_2_4000000,
@@ -3299,6 +3385,20 @@ static struct pciserial_board pci_boards[] = {
 	},
 
 	/*
+	 * EndRun Technologies
+	* Uses the size of PCI Base region 0 to
+	* signal now many ports are available
+	* 2 port 952 Uart support
+	*/
+	[pbn_endrun_2_4000000] = {
+		.flags		= FL_BASE0,
+		.num_ports	= 2,
+		.base_baud	= 4000000,
+		.uart_offset	= 0x200,
+		.first_offset	= 0x1000,
+	},
+
+	/*
 	 * This board uses the size of PCI Base region 0 to
 	 * signal now many ports are available
 	 */
@@ -3586,6 +3686,7 @@ static const struct pci_device_id blacklist[] = {
 	/* multi-io cards handled by parport_serial */
 	{ PCI_DEVICE(0x4348, 0x7053), }, /* WCH CH353 2S1P */
 	{ PCI_DEVICE(0x4348, 0x5053), }, /* WCH CH353 1S1P */
+	{ PCI_DEVICE(0x1c00, 0x3250), }, /* WCH CH382 2S1P */
 };
 
 /*
@@ -4171,6 +4272,13 @@ static struct pci_device_id serial_pci_tbl[] = {
 		0x10b5, 0x106a, 0, 0,
 		pbn_plx_romulus },
 	/*
+	* EndRun Technologies. PCI express device range.
+	*    EndRun PTP/1588 has 2 Native UARTs.
+	*/
+	{	PCI_VENDOR_ID_ENDRUN, PCI_DEVICE_ID_ENDRUN_1588,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+		pbn_endrun_2_4000000 },
+	/*
 	 * Quatech cards. These actually have configurable clocks but for
 	 * now we just use the default.
 	 *
diff --git a/drivers/tty/serial/8250/Kconfig b/drivers/tty/serial/8250/Kconfig
index 21eca79224e4..0fcbcd29502f 100644
--- a/drivers/tty/serial/8250/Kconfig
+++ b/drivers/tty/serial/8250/Kconfig
@@ -293,12 +293,21 @@ config SERIAL_8250_EM
 
 config SERIAL_8250_RT288X
 	bool "Ralink RT288x/RT305x/RT3662/RT3883 serial port support"
-	depends on SERIAL_8250 && (SOC_RT288X || SOC_RT305X || SOC_RT3883)
+	depends on SERIAL_8250 && (SOC_RT288X || SOC_RT305X || SOC_RT3883 || SOC_MT7620)
 	help
 	  If you have a Ralink RT288x/RT305x SoC based board and want to use the
 	  serial port, say Y to this option. The driver can handle up to 2 serial
 	  ports. If unsure, say N.
 
+config SERIAL_8250_OMAP
+	tristate "Support for OMAP internal UART (8250 based driver)"
+	depends on SERIAL_8250 && ARCH_OMAP2PLUS
+	help
+	  If you have a machine based on an Texas Instruments OMAP CPU you
+	  can enable its onboard serial ports by enabling this option.
+
+	  This driver uses ttyS instead of ttyO.
+
 config SERIAL_8250_FINTEK
 	tristate "Support for Fintek F81216A LPC to 4 UART"
 	depends on SERIAL_8250 && PNP
diff --git a/drivers/tty/serial/8250/Makefile b/drivers/tty/serial/8250/Makefile
index 5256b894e46a..31e7cdc6865c 100644
--- a/drivers/tty/serial/8250/Makefile
+++ b/drivers/tty/serial/8250/Makefile
@@ -20,5 +20,6 @@ obj-$(CONFIG_SERIAL_8250_HUB6)		+= 8250_hub6.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
+obj-$(CONFIG_SERIAL_8250_OMAP)		+= 8250_omap.o
 obj-$(CONFIG_SERIAL_8250_FINTEK)	+= 8250_fintek.o
 obj-$(CONFIG_SERIAL_8250_MT6577)	+= 8250_mtk.o
diff --git a/drivers/tty/serial/Kconfig b/drivers/tty/serial/Kconfig
index 98f8bcaf3e7e..c79b43cd6014 100644
--- a/drivers/tty/serial/Kconfig
+++ b/drivers/tty/serial/Kconfig
@@ -701,7 +701,7 @@ config PDC_CONSOLE
 	  Saying Y here will enable the software based PDC console to be 
 	  used as the system console.  This is useful for machines in 
 	  which the hardware based console has not been written yet.  The
-	  following steps must be competed to use the PDC console:
+	  following steps must be completed to use the PDC console:
 
 	    1. create the device entry (mknod /dev/ttyB0 c 11 0)
 	    2. Edit the /etc/inittab to start a getty listening on /dev/ttyB0
@@ -1029,11 +1029,11 @@ config SERIAL_VR41XX_CONSOLE
 	  a console on a serial port, say Y.  Otherwise, say N.
 
 config SERIAL_JSM
-	tristate "Digi International NEO PCI Support"
+	tristate "Digi International NEO and Classic PCI Support"
 	depends on PCI
 	select SERIAL_CORE
 	help
-	  This is a driver for Digi International's Neo series
+	  This is a driver for Digi International's Neo and Classic series
 	  of cards which provide multiple serial ports. You would need
 	  something like this to connect more than two modems to your Linux
 	  box, for instance in order to become a dial-in server. This driver
@@ -1281,22 +1281,25 @@ config SERIAL_TIMBERDALE
 	Add support for UART controller on timberdale.
 
 config SERIAL_BCM63XX
-	tristate "bcm63xx serial port support"
+	tristate "Broadcom BCM63xx/BCM33xx UART support"
 	select SERIAL_CORE
-	depends on BCM63XX
+	depends on MIPS || ARM || COMPILE_TEST
 	help
-	  If you have a bcm63xx CPU, you can enable its onboard
-	  serial port by enabling this options.
+	  This enables the driver for the onchip UART core found on
+	  the following chipsets:
 
-          To compile this driver as a module, choose M here: the
-          module will be called bcm963xx_uart.
+	    BCM33xx (cable modem)
+	    BCM63xx/BCM63xxx (DSL)
+	    BCM68xx (PON)
+	    BCM7xxx (STB) - DOCSIS console
 
 config SERIAL_BCM63XX_CONSOLE
-	bool "Console on bcm63xx serial port"
+	bool "Console on BCM63xx serial port"
 	depends on SERIAL_BCM63XX=y
 	select SERIAL_CORE_CONSOLE
+	select SERIAL_EARLYCON
 	help
-	  If you have enabled the serial port on the bcm63xx CPU
+	  If you have enabled the serial port on the BCM63xx CPU
 	  you can make it the console by answering Y to this option.
 
 config SERIAL_GRLIB_GAISLER_APBUART
@@ -1408,6 +1411,7 @@ config SERIAL_MXS_AUART
 	depends on ARCH_MXS
 	tristate "MXS AUART support"
 	select SERIAL_CORE
+	select SERIAL_MCTRL_GPIO if GPIOLIB
 	help
 	  This driver supports the MXS Application UART (AUART) port.
 
diff --git a/drivers/tty/serial/altera_jtaguart.c b/drivers/tty/serial/altera_jtaguart.c
index 932e01995c0a..192d0435bb86 100644
--- a/drivers/tty/serial/altera_jtaguart.c
+++ b/drivers/tty/serial/altera_jtaguart.c
@@ -475,7 +475,6 @@ static struct platform_driver altera_jtaguart_platform_driver = {
 	.remove	= altera_jtaguart_remove,
 	.driver	= {
 		.name		= DRV_NAME,
-		.owner		= THIS_MODULE,
 		.of_match_table	= of_match_ptr(altera_jtaguart_match),
 	},
 };
diff --git a/drivers/tty/serial/altera_uart.c b/drivers/tty/serial/altera_uart.c
index 1cb2cdb1bc42..eb15a50623cb 100644
--- a/drivers/tty/serial/altera_uart.c
+++ b/drivers/tty/serial/altera_uart.c
@@ -623,7 +623,6 @@ static struct platform_driver altera_uart_platform_driver = {
 	.remove	= altera_uart_remove,
 	.driver	= {
 		.name		= DRV_NAME,
-		.owner		= THIS_MODULE,
 		.of_match_table	= of_match_ptr(altera_uart_match),
 	},
 };
diff --git a/drivers/tty/serial/amba-pl010.c b/drivers/tty/serial/amba-pl010.c
index 2064d31d0c8b..5d41d5b92619 100644
--- a/drivers/tty/serial/amba-pl010.c
+++ b/drivers/tty/serial/amba-pl010.c
@@ -75,7 +75,8 @@ struct uart_amba_port {
 
 static void pl010_stop_tx(struct uart_port *port)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+		container_of(port, struct uart_amba_port, port);
 	unsigned int cr;
 
 	cr = readb(uap->port.membase + UART010_CR);
@@ -85,7 +86,8 @@ static void pl010_stop_tx(struct uart_port *port)
 
 static void pl010_start_tx(struct uart_port *port)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+		container_of(port, struct uart_amba_port, port);
 	unsigned int cr;
 
 	cr = readb(uap->port.membase + UART010_CR);
@@ -95,7 +97,8 @@ static void pl010_start_tx(struct uart_port *port)
 
 static void pl010_stop_rx(struct uart_port *port)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+		container_of(port, struct uart_amba_port, port);
 	unsigned int cr;
 
 	cr = readb(uap->port.membase + UART010_CR);
@@ -103,12 +106,23 @@ static void pl010_stop_rx(struct uart_port *port)
 	writel(cr, uap->port.membase + UART010_CR);
 }
 
-static void pl010_enable_ms(struct uart_port *port)
+static void pl010_disable_ms(struct uart_port *port)
 {
 	struct uart_amba_port *uap = (struct uart_amba_port *)port;
 	unsigned int cr;
 
 	cr = readb(uap->port.membase + UART010_CR);
+	cr &= ~UART010_CR_MSIE;
+	writel(cr, uap->port.membase + UART010_CR);
+}
+
+static void pl010_enable_ms(struct uart_port *port)
+{
+	struct uart_amba_port *uap =
+		container_of(port, struct uart_amba_port, port);
+	unsigned int cr;
+
+	cr = readb(uap->port.membase + UART010_CR);
 	cr |= UART010_CR_MSIE;
 	writel(cr, uap->port.membase + UART010_CR);
 }
@@ -259,14 +273,16 @@ static irqreturn_t pl010_int(int irq, void *dev_id)
 
 static unsigned int pl010_tx_empty(struct uart_port *port)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+		container_of(port, struct uart_amba_port, port);
 	unsigned int status = readb(uap->port.membase + UART01x_FR);
 	return status & UART01x_FR_BUSY ? 0 : TIOCSER_TEMT;
 }
 
 static unsigned int pl010_get_mctrl(struct uart_port *port)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+		container_of(port, struct uart_amba_port, port);
 	unsigned int result = 0;
 	unsigned int status;
 
@@ -283,7 +299,8 @@ static unsigned int pl010_get_mctrl(struct uart_port *port)
 
 static void pl010_set_mctrl(struct uart_port *port, unsigned int mctrl)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+		container_of(port, struct uart_amba_port, port);
 
 	if (uap->data)
 		uap->data->set_mctrl(uap->dev, uap->port.membase, mctrl);
@@ -291,7 +308,8 @@ static void pl010_set_mctrl(struct uart_port *port, unsigned int mctrl)
 
 static void pl010_break_ctl(struct uart_port *port, int break_state)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+		container_of(port, struct uart_amba_port, port);
 	unsigned long flags;
 	unsigned int lcr_h;
 
@@ -307,7 +325,8 @@ static void pl010_break_ctl(struct uart_port *port, int break_state)
 
 static int pl010_startup(struct uart_port *port)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+		container_of(port, struct uart_amba_port, port);
 	int retval;
 
 	/*
@@ -347,7 +366,8 @@ static int pl010_startup(struct uart_port *port)
 
 static void pl010_shutdown(struct uart_port *port)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+		container_of(port, struct uart_amba_port, port);
 
 	/*
 	 * Free the interrupt
@@ -374,7 +394,8 @@ static void
 pl010_set_termios(struct uart_port *port, struct ktermios *termios,
 		     struct ktermios *old)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+		container_of(port, struct uart_amba_port, port);
 	unsigned int lcr_h, old_cr;
 	unsigned long flags;
 	unsigned int baud, quot;
@@ -468,13 +489,21 @@ pl010_set_termios(struct uart_port *port, struct ktermios *termios,
 	spin_unlock_irqrestore(&uap->port.lock, flags);
 }
 
-static void pl010_set_ldisc(struct uart_port *port, int new)
+static void pl010_set_ldisc(struct uart_port *port, struct ktermios *termios)
 {
-	if (new == N_PPS) {
+	if (termios->c_line == N_PPS) {
 		port->flags |= UPF_HARDPPS_CD;
+		spin_lock_irq(&port->lock);
 		pl010_enable_ms(port);
-	} else
+		spin_unlock_irq(&port->lock);
+	} else {
 		port->flags &= ~UPF_HARDPPS_CD;
+		if (!UART_ENABLE_MS(port, termios->c_cflag)) {
+			spin_lock_irq(&port->lock);
+			pl010_disable_ms(port);
+			spin_unlock_irq(&port->lock);
+		}
+	}
 }
 
 static const char *pl010_type(struct uart_port *port)
@@ -551,7 +580,8 @@ static struct uart_amba_port *amba_ports[UART_NR];
 
 static void pl010_console_putchar(struct uart_port *port, int ch)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+		container_of(port, struct uart_amba_port, port);
 	unsigned int status;
 
 	do {
diff --git a/drivers/tty/serial/amba-pl011.c b/drivers/tty/serial/amba-pl011.c
index 02016fcd91b8..8d94c194f090 100644
--- a/drivers/tty/serial/amba-pl011.c
+++ b/drivers/tty/serial/amba-pl011.c
@@ -246,6 +246,7 @@ static int pl011_sgbuf_init(struct dma_chan *chan, struct pl011_sgbuf *sg,
 	sg_set_page(&sg->sg, phys_to_page(dma_addr),
 		PL011_DMA_BUFFER_SIZE, offset_in_page(dma_addr));
 	sg_dma_address(&sg->sg) = dma_addr;
+	sg_dma_len(&sg->sg) = PL011_DMA_BUFFER_SIZE;
 
 	return 0;
 }
@@ -321,10 +322,26 @@ static void pl011_dma_probe_initcall(struct device *dev, struct uart_amba_port *
 			.src_maxburst = uap->fifosize >> 2,
 			.device_fc = false,
 		};
+		struct dma_slave_caps caps;
 
+		/*
+		 * Some DMA controllers provide information on their capabilities.
+		 * If the controller does, check for suitable residue processing
+		 * otherwise assime all is well.
+		 */
+		if (0 == dma_get_slave_caps(chan, &caps)) {
+			if (caps.residue_granularity ==
+					DMA_RESIDUE_GRANULARITY_DESCRIPTOR) {
+				dma_release_channel(chan);
+				dev_info(uap->port.dev,
+					"RX DMA disabled - no residue processing\n");
+				return;
+			}
+		}
 		dmaengine_slave_config(chan, &rx_conf);
 		uap->dmarx.chan = chan;
 
+		uap->dmarx.auto_poll_rate = false;
 		if (plat && plat->dma_rx_poll_enable) {
 			/* Set poll rate if specified. */
 			if (plat->dma_rx_poll_rate) {
@@ -345,9 +362,24 @@ static void pl011_dma_probe_initcall(struct device *dev, struct uart_amba_port *
 					plat->dma_rx_poll_timeout;
 			else
 				uap->dmarx.poll_timeout = 3000;
-		} else
-			uap->dmarx.auto_poll_rate = false;
-
+		} else if (!plat && dev->of_node) {
+			uap->dmarx.auto_poll_rate = of_property_read_bool(
+						dev->of_node, "auto-poll");
+			if (uap->dmarx.auto_poll_rate) {
+				u32 x;
+
+				if (0 == of_property_read_u32(dev->of_node,
+						"poll-rate-ms", &x))
+					uap->dmarx.poll_rate = x;
+				else
+					uap->dmarx.poll_rate = 100;
+				if (0 == of_property_read_u32(dev->of_node,
+						"poll-timeout-ms", &x))
+					uap->dmarx.poll_timeout = x;
+				else
+					uap->dmarx.poll_timeout = 3000;
+			}
+		}
 		dev_info(uap->port.dev, "DMA channel RX %s\n",
 			 dma_chan_name(uap->dmarx.chan));
 	}
@@ -501,7 +533,11 @@ static int pl011_dma_tx_refill(struct uart_amba_port *uap)
 		memcpy(&dmatx->buf[0], &xmit->buf[xmit->tail], count);
 	else {
 		size_t first = UART_XMIT_SIZE - xmit->tail;
-		size_t second = xmit->head;
+		size_t second;
+
+		if (first > count)
+			first = count;
+		second = count - first;
 
 		memcpy(&dmatx->buf[0], &xmit->buf[xmit->tail], first);
 		if (second)
@@ -988,7 +1024,7 @@ static void pl011_dma_startup(struct uart_amba_port *uap)
 	if (!uap->dmatx.chan)
 		return;
 
-	uap->dmatx.buf = kmalloc(PL011_DMA_BUFFER_SIZE, GFP_KERNEL);
+	uap->dmatx.buf = kmalloc(PL011_DMA_BUFFER_SIZE, GFP_KERNEL | __GFP_DMA);
 	if (!uap->dmatx.buf) {
 		dev_err(uap->port.dev, "no memory for DMA TX buffer\n");
 		uap->port.fifosize = uap->fifosize;
@@ -1689,6 +1725,8 @@ static void pl011_shutdown(struct uart_port *port)
 			plat->exit();
 	}
 
+	if (uap->port.ops->flush_buffer)
+		uap->port.ops->flush_buffer(port);
 }
 
 static void
diff --git a/drivers/tty/serial/apbuart.c b/drivers/tty/serial/apbuart.c
index a34a0cec1685..4f0f95e358e8 100644
--- a/drivers/tty/serial/apbuart.c
+++ b/drivers/tty/serial/apbuart.c
@@ -585,7 +585,6 @@ static struct of_device_id apbuart_match[] = {
 static struct platform_driver grlib_apbuart_of_driver = {
 	.probe = apbuart_probe,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "grlib-apbuart",
 		.of_match_table = apbuart_match,
 	},
diff --git a/drivers/tty/serial/ar933x_uart.c b/drivers/tty/serial/ar933x_uart.c
index 0be1c45efd65..77fc9faa74a4 100644
--- a/drivers/tty/serial/ar933x_uart.c
+++ b/drivers/tty/serial/ar933x_uart.c
@@ -119,7 +119,8 @@ static inline void ar933x_uart_putc(struct ar933x_uart_port *up, int ch)
 
 static unsigned int ar933x_uart_tx_empty(struct uart_port *port)
 {
-	struct ar933x_uart_port *up = (struct ar933x_uart_port *) port;
+	struct ar933x_uart_port *up =
+		container_of(port, struct ar933x_uart_port, port);
 	unsigned long flags;
 	unsigned int rdata;
 
@@ -141,21 +142,24 @@ static void ar933x_uart_set_mctrl(struct uart_port *port, unsigned int mctrl)
 
 static void ar933x_uart_start_tx(struct uart_port *port)
 {
-	struct ar933x_uart_port *up = (struct ar933x_uart_port *) port;
+	struct ar933x_uart_port *up =
+		container_of(port, struct ar933x_uart_port, port);
 
 	ar933x_uart_start_tx_interrupt(up);
 }
 
 static void ar933x_uart_stop_tx(struct uart_port *port)
 {
-	struct ar933x_uart_port *up = (struct ar933x_uart_port *) port;
+	struct ar933x_uart_port *up =
+		container_of(port, struct ar933x_uart_port, port);
 
 	ar933x_uart_stop_tx_interrupt(up);
 }
 
 static void ar933x_uart_stop_rx(struct uart_port *port)
 {
-	struct ar933x_uart_port *up = (struct ar933x_uart_port *) port;
+	struct ar933x_uart_port *up =
+		container_of(port, struct ar933x_uart_port, port);
 
 	up->ier &= ~AR933X_UART_INT_RX_VALID;
 	ar933x_uart_write(up, AR933X_UART_INT_EN_REG, up->ier);
@@ -163,7 +167,8 @@ static void ar933x_uart_stop_rx(struct uart_port *port)
 
 static void ar933x_uart_break_ctl(struct uart_port *port, int break_state)
 {
-	struct ar933x_uart_port *up = (struct ar933x_uart_port *) port;
+	struct ar933x_uart_port *up =
+		container_of(port, struct ar933x_uart_port, port);
 	unsigned long flags;
 
 	spin_lock_irqsave(&up->port.lock, flags);
@@ -231,7 +236,8 @@ static void ar933x_uart_set_termios(struct uart_port *port,
 				    struct ktermios *new,
 				    struct ktermios *old)
 {
-	struct ar933x_uart_port *up = (struct ar933x_uart_port *) port;
+	struct ar933x_uart_port *up =
+		container_of(port, struct ar933x_uart_port, port);
 	unsigned int cs;
 	unsigned long flags;
 	unsigned int baud, scale, step;
@@ -404,7 +410,8 @@ static irqreturn_t ar933x_uart_interrupt(int irq, void *dev_id)
 
 static int ar933x_uart_startup(struct uart_port *port)
 {
-	struct ar933x_uart_port *up = (struct ar933x_uart_port *) port;
+	struct ar933x_uart_port *up =
+		container_of(port, struct ar933x_uart_port, port);
 	unsigned long flags;
 	int ret;
 
@@ -430,7 +437,8 @@ static int ar933x_uart_startup(struct uart_port *port)
 
 static void ar933x_uart_shutdown(struct uart_port *port)
 {
-	struct ar933x_uart_port *up = (struct ar933x_uart_port *) port;
+	struct ar933x_uart_port *up =
+		container_of(port, struct ar933x_uart_port, port);
 
 	/* Disable all interrupts */
 	up->ier = 0;
@@ -468,7 +476,8 @@ static void ar933x_uart_config_port(struct uart_port *port, int flags)
 static int ar933x_uart_verify_port(struct uart_port *port,
 				   struct serial_struct *ser)
 {
-	struct ar933x_uart_port *up = (struct ar933x_uart_port *) port;
+	struct ar933x_uart_port *up =
+		container_of(port, struct ar933x_uart_port, port);
 
 	if (ser->type != PORT_UNKNOWN &&
 	    ser->type != PORT_AR933X)
@@ -521,7 +530,8 @@ static void ar933x_uart_wait_xmitr(struct ar933x_uart_port *up)
 
 static void ar933x_uart_console_putchar(struct uart_port *port, int ch)
 {
-	struct ar933x_uart_port *up = (struct ar933x_uart_port *) port;
+	struct ar933x_uart_port *up =
+		container_of(port, struct ar933x_uart_port, port);
 
 	ar933x_uart_wait_xmitr(up);
 	ar933x_uart_putc(up, ch);
@@ -734,7 +744,6 @@ static struct platform_driver ar933x_uart_platform_driver = {
 	.remove		= ar933x_uart_remove,
 	.driver		= {
 		.name		= DRIVER_NAME,
-		.owner		= THIS_MODULE,
 		.of_match_table = of_match_ptr(ar933x_uart_of_ids),
 	},
 };
diff --git a/drivers/tty/serial/arc_uart.c b/drivers/tty/serial/arc_uart.c
index a59d1d77e750..03ebe401fff7 100644
--- a/drivers/tty/serial/arc_uart.c
+++ b/drivers/tty/serial/arc_uart.c
@@ -653,7 +653,6 @@ static struct platform_driver arc_platform_driver = {
 	.remove = arc_serial_remove,
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table  = arc_uart_dt_ids,
 	 },
 };
diff --git a/drivers/tty/serial/atmel_serial.c b/drivers/tty/serial/atmel_serial.c
index edde3eca055d..4d848a29e223 100644
--- a/drivers/tty/serial/atmel_serial.c
+++ b/drivers/tty/serial/atmel_serial.c
@@ -167,7 +167,6 @@ struct atmel_uart_port {
 
 	struct circ_buf		rx_ring;
 
-	struct serial_rs485	rs485;		/* rs485 settings */
 	struct mctrl_gpios	*gpios;
 	int			gpio_irq[UART_GPIO_MAX];
 	unsigned int		tx_done_mask;
@@ -290,13 +289,11 @@ static unsigned int atmel_get_lines_status(struct uart_port *port)
 }
 
 /* Enable or disable the rs485 support */
-void atmel_config_rs485(struct uart_port *port, struct serial_rs485 *rs485conf)
+static int atmel_config_rs485(struct uart_port *port,
+			      struct serial_rs485 *rs485conf)
 {
 	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
 	unsigned int mode;
-	unsigned long flags;
-
-	spin_lock_irqsave(&port->lock, flags);
 
 	/* Disable interrupts */
 	UART_PUT_IDR(port, atmel_port->tx_done_mask);
@@ -306,7 +303,7 @@ void atmel_config_rs485(struct uart_port *port, struct serial_rs485 *rs485conf)
 	/* Resetting serial mode to RS232 (0x0) */
 	mode &= ~ATMEL_US_USMODE;
 
-	atmel_port->rs485 = *rs485conf;
+	port->rs485 = *rs485conf;
 
 	if (rs485conf->flags & SER_RS485_ENABLED) {
 		dev_dbg(port->dev, "Setting UART to RS485\n");
@@ -327,8 +324,7 @@ void atmel_config_rs485(struct uart_port *port, struct serial_rs485 *rs485conf)
 	/* Enable interrupts */
 	UART_PUT_IER(port, atmel_port->tx_done_mask);
 
-	spin_unlock_irqrestore(&port->lock, flags);
-
+	return 0;
 }
 
 /*
@@ -372,11 +368,10 @@ static void atmel_set_mctrl(struct uart_port *port, u_int mctrl)
 	/* Resetting serial mode to RS232 (0x0) */
 	mode &= ~ATMEL_US_USMODE;
 
-	if (atmel_port->rs485.flags & SER_RS485_ENABLED) {
+	if (port->rs485.flags & SER_RS485_ENABLED) {
 		dev_dbg(port->dev, "Setting UART to RS485\n");
-		if ((atmel_port->rs485.delay_rts_after_send) > 0)
-			UART_PUT_TTGR(port,
-					atmel_port->rs485.delay_rts_after_send);
+		if ((port->rs485.delay_rts_after_send) > 0)
+			UART_PUT_TTGR(port, port->rs485.delay_rts_after_send);
 		mode |= ATMEL_US_USMODE_RS485;
 	} else {
 		dev_dbg(port->dev, "Setting UART to RS232\n");
@@ -423,8 +418,8 @@ static void atmel_stop_tx(struct uart_port *port)
 	/* Disable interrupts */
 	UART_PUT_IDR(port, atmel_port->tx_done_mask);
 
-	if ((atmel_port->rs485.flags & SER_RS485_ENABLED) &&
-	    !(atmel_port->rs485.flags & SER_RS485_RX_DURING_TX))
+	if ((port->rs485.flags & SER_RS485_ENABLED) &&
+	    !(port->rs485.flags & SER_RS485_RX_DURING_TX))
 		atmel_start_rx(port);
 }
 
@@ -441,8 +436,8 @@ static void atmel_start_tx(struct uart_port *port)
 			   really need this.*/
 			return;
 
-		if ((atmel_port->rs485.flags & SER_RS485_ENABLED) &&
-		    !(atmel_port->rs485.flags & SER_RS485_RX_DURING_TX))
+		if ((port->rs485.flags & SER_RS485_ENABLED) &&
+		    !(port->rs485.flags & SER_RS485_RX_DURING_TX))
 			atmel_stop_rx(port);
 
 		/* re-enable PDC transmit */
@@ -807,7 +802,7 @@ static void atmel_tx_dma(struct uart_port *port)
 		atmel_port->cookie_tx = dmaengine_submit(desc);
 
 	} else {
-		if (atmel_port->rs485.flags & SER_RS485_ENABLED) {
+		if (port->rs485.flags & SER_RS485_ENABLED) {
 			/* DMA done, stop TX, start RX for RS485 */
 			atmel_start_rx(port);
 		}
@@ -862,9 +857,8 @@ static int atmel_prepare_tx_dma(struct uart_port *port)
 	config.dst_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
 	config.dst_addr = port->mapbase + ATMEL_US_THR;
 
-	ret = dmaengine_device_control(atmel_port->chan_tx,
-					DMA_SLAVE_CONFIG,
-					(unsigned long)&config);
+	ret = dmaengine_slave_config(atmel_port->chan_tx,
+				     &config);
 	if (ret) {
 		dev_err(port->dev, "DMA tx slave configuration failed\n");
 		goto chan_err;
@@ -880,32 +874,6 @@ chan_err:
 	return -EINVAL;
 }
 
-static void atmel_flip_buffer_rx_dma(struct uart_port *port,
-					char *buf, size_t count)
-{
-	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
-	struct tty_port *tport = &port->state->port;
-
-	dma_sync_sg_for_cpu(port->dev,
-				&atmel_port->sg_rx,
-				1,
-				DMA_DEV_TO_MEM);
-
-	tty_insert_flip_string(tport, buf, count);
-
-	dma_sync_sg_for_device(port->dev,
-				&atmel_port->sg_rx,
-				1,
-				DMA_DEV_TO_MEM);
-	/*
-	 * Drop the lock here since it might end up calling
-	 * uart_start(), which takes the lock.
-	 */
-	spin_unlock(&port->lock);
-	tty_flip_buffer_push(tport);
-	spin_lock(&port->lock);
-}
-
 static void atmel_complete_rx_dma(void *arg)
 {
 	struct uart_port *port = arg;
@@ -934,11 +902,12 @@ static void atmel_release_rx_dma(struct uart_port *port)
 static void atmel_rx_from_dma(struct uart_port *port)
 {
 	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
+	struct tty_port *tport = &port->state->port;
 	struct circ_buf *ring = &atmel_port->rx_ring;
 	struct dma_chan *chan = atmel_port->chan_rx;
 	struct dma_tx_state state;
 	enum dma_status dmastat;
-	size_t pending, count;
+	size_t count;
 
 
 	/* Reset the UART timeout early so that we don't miss one */
@@ -953,27 +922,68 @@ static void atmel_rx_from_dma(struct uart_port *port)
 		tasklet_schedule(&atmel_port->tasklet);
 		return;
 	}
-	/* current transfer size should no larger than dma buffer */
-	pending = sg_dma_len(&atmel_port->sg_rx) - state.residue;
-	BUG_ON(pending > sg_dma_len(&atmel_port->sg_rx));
+
+	/* CPU claims ownership of RX DMA buffer */
+	dma_sync_sg_for_cpu(port->dev,
+			    &atmel_port->sg_rx,
+			    1,
+			    DMA_DEV_TO_MEM);
 
 	/*
-	 * This will take the chars we have so far,
-	 * ring->head will record the transfer size, only new bytes come
-	 * will insert into the framework.
+	 * ring->head points to the end of data already written by the DMA.
+	 * ring->tail points to the beginning of data to be read by the
+	 * framework.
+	 * The current transfer size should not be larger than the dma buffer
+	 * length.
 	 */
-	if (pending > ring->head) {
-		count = pending - ring->head;
+	ring->head = sg_dma_len(&atmel_port->sg_rx) - state.residue;
+	BUG_ON(ring->head > sg_dma_len(&atmel_port->sg_rx));
+	/*
+	 * At this point ring->head may point to the first byte right after the
+	 * last byte of the dma buffer:
+	 * 0 <= ring->head <= sg_dma_len(&atmel_port->sg_rx)
+	 *
+	 * However ring->tail must always points inside the dma buffer:
+	 * 0 <= ring->tail <= sg_dma_len(&atmel_port->sg_rx) - 1
+	 *
+	 * Since we use a ring buffer, we have to handle the case
+	 * where head is lower than tail. In such a case, we first read from
+	 * tail to the end of the buffer then reset tail.
+	 */
+	if (ring->head < ring->tail) {
+		count = sg_dma_len(&atmel_port->sg_rx) - ring->tail;
+
+		tty_insert_flip_string(tport, ring->buf + ring->tail, count);
+		ring->tail = 0;
+		port->icount.rx += count;
+	}
 
-		atmel_flip_buffer_rx_dma(port, ring->buf + ring->head, count);
+	/* Finally we read data from tail to head */
+	if (ring->tail < ring->head) {
+		count = ring->head - ring->tail;
 
-		ring->head += count;
-		if (ring->head == sg_dma_len(&atmel_port->sg_rx))
+		tty_insert_flip_string(tport, ring->buf + ring->tail, count);
+		/* Wrap ring->head if needed */
+		if (ring->head >= sg_dma_len(&atmel_port->sg_rx))
 			ring->head = 0;
-
+		ring->tail = ring->head;
 		port->icount.rx += count;
 	}
 
+	/* USART retreives ownership of RX DMA buffer */
+	dma_sync_sg_for_device(port->dev,
+			       &atmel_port->sg_rx,
+			       1,
+			       DMA_DEV_TO_MEM);
+
+	/*
+	 * Drop the lock here since it might end up calling
+	 * uart_start(), which takes the lock.
+	 */
+	spin_unlock(&port->lock);
+	tty_flip_buffer_push(tport);
+	spin_lock(&port->lock);
+
 	UART_PUT_IER(port, ATMEL_US_TIMEOUT);
 }
 
@@ -1026,9 +1036,8 @@ static int atmel_prepare_rx_dma(struct uart_port *port)
 	config.src_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
 	config.src_addr = port->mapbase + ATMEL_US_RHR;
 
-	ret = dmaengine_device_control(atmel_port->chan_rx,
-					DMA_SLAVE_CONFIG,
-					(unsigned long)&config);
+	ret = dmaengine_slave_config(atmel_port->chan_rx,
+				     &config);
 	if (ret) {
 		dev_err(port->dev, "DMA rx slave configuration failed\n");
 		goto chan_err;
@@ -1240,8 +1249,8 @@ static void atmel_tx_pdc(struct uart_port *port)
 		/* Enable interrupts */
 		UART_PUT_IER(port, atmel_port->tx_done_mask);
 	} else {
-		if ((atmel_port->rs485.flags & SER_RS485_ENABLED) &&
-		    !(atmel_port->rs485.flags & SER_RS485_RX_DURING_TX)) {
+		if ((port->rs485.flags & SER_RS485_ENABLED) &&
+		    !(port->rs485.flags & SER_RS485_RX_DURING_TX)) {
 			/* DMA done, stop TX, start RX for RS485 */
 			atmel_start_rx(port);
 		}
@@ -1552,7 +1561,7 @@ static int atmel_init_property(struct atmel_uart_port *atmel_port,
 	return 0;
 }
 
-static void atmel_init_rs485(struct atmel_uart_port *atmel_port,
+static void atmel_init_rs485(struct uart_port *port,
 				struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
@@ -1563,7 +1572,7 @@ static void atmel_init_rs485(struct atmel_uart_port *atmel_port,
 		/* rs485 properties */
 		if (of_property_read_u32_array(np, "rs485-rts-delay",
 					rs485_delay, 2) == 0) {
-			struct serial_rs485 *rs485conf = &atmel_port->rs485;
+			struct serial_rs485 *rs485conf = &port->rs485;
 
 			rs485conf->delay_rts_before_send = rs485_delay[0];
 			rs485conf->delay_rts_after_send = rs485_delay[1];
@@ -1577,7 +1586,7 @@ static void atmel_init_rs485(struct atmel_uart_port *atmel_port,
 			rs485conf->flags |= SER_RS485_ENABLED;
 		}
 	} else {
-		atmel_port->rs485       = pdata->rs485;
+		port->rs485       = pdata->rs485;
 	}
 
 }
@@ -1802,6 +1811,20 @@ free_irq:
 }
 
 /*
+ * Flush any TX data submitted for DMA. Called when the TX circular
+ * buffer is reset.
+ */
+static void atmel_flush_buffer(struct uart_port *port)
+{
+	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
+
+	if (atmel_use_pdc_tx(port)) {
+		UART_PUT_TCR(port, 0);
+		atmel_port->pdc_tx.ofs = 0;
+	}
+}
+
+/*
  * Disable the port
  */
 static void atmel_shutdown(struct uart_port *port)
@@ -1852,20 +1875,8 @@ static void atmel_shutdown(struct uart_port *port)
 	atmel_free_gpio_irq(port);
 
 	atmel_port->ms_irq_enabled = false;
-}
 
-/*
- * Flush any TX data submitted for DMA. Called when the TX circular
- * buffer is reset.
- */
-static void atmel_flush_buffer(struct uart_port *port)
-{
-	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
-
-	if (atmel_use_pdc_tx(port)) {
-		UART_PUT_TCR(port, 0);
-		atmel_port->pdc_tx.ofs = 0;
-	}
+	atmel_flush_buffer(port);
 }
 
 /*
@@ -1911,7 +1922,6 @@ static void atmel_set_termios(struct uart_port *port, struct ktermios *termios,
 {
 	unsigned long flags;
 	unsigned int mode, imr, quot, baud;
-	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
 
 	/* Get current mode register */
 	mode = UART_GET_MR(port) & ~(ATMEL_US_USCLKS | ATMEL_US_CHRL
@@ -2013,10 +2023,9 @@ static void atmel_set_termios(struct uart_port *port, struct ktermios *termios,
 	/* Resetting serial mode to RS232 (0x0) */
 	mode &= ~ATMEL_US_USMODE;
 
-	if (atmel_port->rs485.flags & SER_RS485_ENABLED) {
-		if ((atmel_port->rs485.delay_rts_after_send) > 0)
-			UART_PUT_TTGR(port,
-					atmel_port->rs485.delay_rts_after_send);
+	if (port->rs485.flags & SER_RS485_ENABLED) {
+		if ((port->rs485.delay_rts_after_send) > 0)
+			UART_PUT_TTGR(port, port->rs485.delay_rts_after_send);
 		mode |= ATMEL_US_USMODE_RS485;
 	}
 
@@ -2040,13 +2049,20 @@ static void atmel_set_termios(struct uart_port *port, struct ktermios *termios,
 	spin_unlock_irqrestore(&port->lock, flags);
 }
 
-static void atmel_set_ldisc(struct uart_port *port, int new)
+static void atmel_set_ldisc(struct uart_port *port, struct ktermios *termios)
 {
-	if (new == N_PPS) {
+	if (termios->c_line == N_PPS) {
 		port->flags |= UPF_HARDPPS_CD;
+		spin_lock_irq(&port->lock);
 		atmel_enable_ms(port);
+		spin_unlock_irq(&port->lock);
 	} else {
 		port->flags &= ~UPF_HARDPPS_CD;
+		if (!UART_ENABLE_MS(port, termios->c_cflag)) {
+			spin_lock_irq(&port->lock);
+			atmel_disable_ms(port);
+			spin_unlock_irq(&port->lock);
+		}
 	}
 }
 
@@ -2148,35 +2164,6 @@ static void atmel_poll_put_char(struct uart_port *port, unsigned char ch)
 }
 #endif
 
-static int
-atmel_ioctl(struct uart_port *port, unsigned int cmd, unsigned long arg)
-{
-	struct serial_rs485 rs485conf;
-
-	switch (cmd) {
-	case TIOCSRS485:
-		if (copy_from_user(&rs485conf, (struct serial_rs485 *) arg,
-					sizeof(rs485conf)))
-			return -EFAULT;
-
-		atmel_config_rs485(port, &rs485conf);
-		break;
-
-	case TIOCGRS485:
-		if (copy_to_user((struct serial_rs485 *) arg,
-					&(to_atmel_uart_port(port)->rs485),
-					sizeof(rs485conf)))
-			return -EFAULT;
-		break;
-
-	default:
-		return -ENOIOCTLCMD;
-	}
-	return 0;
-}
-
-
-
 static struct uart_ops atmel_pops = {
 	.tx_empty	= atmel_tx_empty,
 	.set_mctrl	= atmel_set_mctrl,
@@ -2197,7 +2184,6 @@ static struct uart_ops atmel_pops = {
 	.config_port	= atmel_config_port,
 	.verify_port	= atmel_verify_port,
 	.pm		= atmel_serial_pm,
-	.ioctl		= atmel_ioctl,
 #ifdef CONFIG_CONSOLE_POLL
 	.poll_get_char	= atmel_poll_get_char,
 	.poll_put_char	= atmel_poll_put_char,
@@ -2217,7 +2203,7 @@ static int atmel_init_port(struct atmel_uart_port *atmel_port,
 	if (!atmel_init_property(atmel_port, pdev))
 		atmel_set_ops(port);
 
-	atmel_init_rs485(atmel_port, pdev);
+	atmel_init_rs485(port, pdev);
 
 	port->iotype		= UPIO_MEM;
 	port->flags		= UPF_BOOT_AUTOCONF;
@@ -2226,6 +2212,7 @@ static int atmel_init_port(struct atmel_uart_port *atmel_port,
 	port->dev		= &pdev->dev;
 	port->mapbase	= pdev->resource[0].start;
 	port->irq	= pdev->resource[1].start;
+	port->rs485_config	= atmel_config_rs485;
 
 	tasklet_init(&atmel_port->tasklet, atmel_tasklet_func,
 			(unsigned long)port);
@@ -2260,7 +2247,7 @@ static int atmel_init_port(struct atmel_uart_port *atmel_port,
 	}
 
 	/* Use TXEMPTY for interrupt when rs485 else TXRDY or ENDTX|TXBUFE */
-	if (atmel_port->rs485.flags & SER_RS485_ENABLED)
+	if (port->rs485.flags & SER_RS485_ENABLED)
 		atmel_port->tx_done_mask = ATMEL_US_TXEMPTY;
 	else if (atmel_use_pdc_tx(port)) {
 		port->fifosize = PDC_BUFFER_SIZE;
@@ -2541,6 +2528,7 @@ static int atmel_serial_probe(struct platform_device *pdev)
 	struct atmel_uart_data *pdata = dev_get_platdata(&pdev->dev);
 	void *data;
 	int ret = -ENODEV;
+	bool rs485_enabled;
 
 	BUILD_BUG_ON(ATMEL_SERIAL_RINGSIZE & (ATMEL_SERIAL_RINGSIZE - 1));
 
@@ -2588,6 +2576,8 @@ static int atmel_serial_probe(struct platform_device *pdev)
 		port->rx_ring.buf = data;
 	}
 
+	rs485_enabled = port->uart.rs485.flags & SER_RS485_ENABLED;
+
 	ret = uart_add_one_port(&atmel_uart, &port->uart);
 	if (ret)
 		goto err_add_port;
@@ -2606,7 +2596,7 @@ static int atmel_serial_probe(struct platform_device *pdev)
 	device_init_wakeup(&pdev->dev, 1);
 	platform_set_drvdata(pdev, port);
 
-	if (port->rs485.flags & SER_RS485_ENABLED) {
+	if (rs485_enabled) {
 		UART_PUT_MR(&port->uart, ATMEL_US_USMODE_NORMAL);
 		UART_PUT_CR(&port->uart, ATMEL_US_RTSEN);
 	}
@@ -2660,7 +2650,6 @@ static struct platform_driver atmel_serial_driver = {
 	.resume		= atmel_serial_resume,
 	.driver		= {
 		.name	= "atmel_usart",
-		.owner	= THIS_MODULE,
 		.of_match_table	= of_match_ptr(atmel_serial_dt_ids),
 	},
 };
diff --git a/drivers/tty/serial/bcm63xx_uart.c b/drivers/tty/serial/bcm63xx_uart.c
index 231519022b73..01d83df08e3d 100644
--- a/drivers/tty/serial/bcm63xx_uart.c
+++ b/drivers/tty/serial/bcm63xx_uart.c
@@ -588,20 +588,7 @@ static void bcm_uart_set_termios(struct uart_port *port,
  */
 static int bcm_uart_request_port(struct uart_port *port)
 {
-	unsigned int size;
-
-	size = UART_REG_SIZE;
-	if (!request_mem_region(port->mapbase, size, "bcm63xx")) {
-		dev_err(port->dev, "Memory region busy\n");
-		return -EBUSY;
-	}
-
-	port->membase = ioremap(port->mapbase, size);
-	if (!port->membase) {
-		dev_err(port->dev, "Unable to map registers\n");
-		release_mem_region(port->mapbase, size);
-		return -EBUSY;
-	}
+	/* UARTs always present */
 	return 0;
 }
 
@@ -610,8 +597,7 @@ static int bcm_uart_request_port(struct uart_port *port)
  */
 static void bcm_uart_release_port(struct uart_port *port)
 {
-	release_mem_region(port->mapbase, UART_REG_SIZE);
-	iounmap(port->membase);
+	/* Nothing to release ... */
 }
 
 /*
@@ -782,6 +768,26 @@ static int __init bcm63xx_console_init(void)
 
 console_initcall(bcm63xx_console_init);
 
+static void bcm_early_write(struct console *con, const char *s, unsigned n)
+{
+	struct earlycon_device *dev = con->data;
+
+	uart_console_write(&dev->port, s, n, bcm_console_putchar);
+	wait_for_xmitr(&dev->port);
+}
+
+static int __init bcm_early_console_setup(struct earlycon_device *device,
+					  const char *opt)
+{
+	if (!device->port.membase)
+		return -ENODEV;
+
+	device->con->write = bcm_early_write;
+	return 0;
+}
+
+OF_EARLYCON_DECLARE(bcm63xx_uart, "brcm,bcm6345-uart", bcm_early_console_setup);
+
 #define BCM63XX_CONSOLE	(&bcm63xx_console)
 #else
 #define BCM63XX_CONSOLE	NULL
@@ -813,25 +819,30 @@ static int bcm_uart_probe(struct platform_device *pdev)
 	if (pdev->id < 0 || pdev->id >= BCM63XX_NR_UARTS)
 		return -EINVAL;
 
-	if (ports[pdev->id].membase)
+	port = &ports[pdev->id];
+	if (port->membase)
 		return -EBUSY;
+	memset(port, 0, sizeof(*port));
 
 	res_mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res_mem)
 		return -ENODEV;
 
+	port->mapbase = res_mem->start;
+	port->membase = devm_ioremap_resource(&pdev->dev, res_mem);
+	if (IS_ERR(port->membase))
+		return PTR_ERR(port->membase);
+
 	res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (!res_irq)
 		return -ENODEV;
 
-	clk = clk_get(&pdev->dev, "periph");
+	clk = pdev->dev.of_node ? of_clk_get(pdev->dev.of_node, 0) :
+				  clk_get(&pdev->dev, "periph");
 	if (IS_ERR(clk))
 		return -ENODEV;
 
-	port = &ports[pdev->id];
-	memset(port, 0, sizeof(*port));
 	port->iotype = UPIO_MEM;
-	port->mapbase = res_mem->start;
 	port->irq = res_irq->start;
 	port->ops = &bcm_uart_ops;
 	port->flags = UPF_BOOT_AUTOCONF;
@@ -874,7 +885,6 @@ static struct platform_driver bcm_uart_platform_driver = {
 	.probe	= bcm_uart_probe,
 	.remove	= bcm_uart_remove,
 	.driver	= {
-		.owner = THIS_MODULE,
 		.name  = "bcm63xx_uart",
 		.of_match_table = bcm63xx_of_match,
 	},
@@ -905,5 +915,5 @@ module_init(bcm_uart_init);
 module_exit(bcm_uart_exit);
 
 MODULE_AUTHOR("Maxime Bizon <mbizon@freebox.fr>");
-MODULE_DESCRIPTION("Broadcom 63<xx integrated uart driver");
+MODULE_DESCRIPTION("Broadcom 63xx integrated uart driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/tty/serial/bfin_sport_uart.c b/drivers/tty/serial/bfin_sport_uart.c
index d62d8daac8ab..984e1c050096 100644
--- a/drivers/tty/serial/bfin_sport_uart.c
+++ b/drivers/tty/serial/bfin_sport_uart.c
@@ -517,14 +517,15 @@ static void sport_set_termios(struct uart_port *port,
 		up->csize = 5;
 		break;
 	default:
-		pr_warning("requested word length not supported\n");
+		pr_warn("requested word length not supported\n");
+		break;
 	}
 
 	if (termios->c_cflag & CSTOPB) {
 		up->stopb = 1;
 	}
 	if (termios->c_cflag & PARENB) {
-		pr_warning("PAREN bits is not supported yet\n");
+		pr_warn("PAREN bit is not supported yet\n");
 		/* up->parib = 1; */
 	}
 
diff --git a/drivers/tty/serial/bfin_uart.c b/drivers/tty/serial/bfin_uart.c
index 7da9911e95f0..43b3e2c233ff 100644
--- a/drivers/tty/serial/bfin_uart.c
+++ b/drivers/tty/serial/bfin_uart.c
@@ -944,12 +944,13 @@ bfin_serial_verify_port(struct uart_port *port, struct serial_struct *ser)
  * Enable the IrDA function if tty->ldisc.num is N_IRDA.
  * In other cases, disable IrDA function.
  */
-static void bfin_serial_set_ldisc(struct uart_port *port, int ld)
+static void bfin_serial_set_ldisc(struct uart_port *port,
+				  struct ktermios *termios)
 {
 	struct bfin_serial_port *uart = (struct bfin_serial_port *)port;
 	unsigned int val;
 
-	switch (ld) {
+	switch (termios->c_line) {
 	case N_IRDA:
 		val = UART_GET_GCTL(uart);
 		val |= (UMOD_IRDA | RPOLC);
@@ -1386,7 +1387,6 @@ static struct platform_driver bfin_serial_driver = {
 	.resume		= bfin_serial_resume,
 	.driver		= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/tty/serial/clps711x.c b/drivers/tty/serial/clps711x.c
index acfe31773643..6e11c275f2ab 100644
--- a/drivers/tty/serial/clps711x.c
+++ b/drivers/tty/serial/clps711x.c
@@ -225,13 +225,14 @@ static void uart_clps711x_break_ctl(struct uart_port *port, int break_state)
 	writel(ubrlcr, port->membase + UBRLCR_OFFSET);
 }
 
-static void uart_clps711x_set_ldisc(struct uart_port *port, int ld)
+static void uart_clps711x_set_ldisc(struct uart_port *port,
+				    struct ktermios *termios)
 {
 	if (!port->line) {
 		struct clps711x_port *s = dev_get_drvdata(port->dev);
 
 		regmap_update_bits(s->syscon, SYSCON_OFFSET, SYSCON1_SIREN,
-				   (ld == N_IRDA) ? SYSCON1_SIREN : 0);
+				   (termios->c_line == N_IRDA) ? SYSCON1_SIREN : 0);
 	}
 }
 
@@ -542,7 +543,6 @@ MODULE_DEVICE_TABLE(of, clps711x_uart_dt_ids);
 static struct platform_driver clps711x_uart_platform = {
 	.driver = {
 		.name		= "clps711x-uart",
-		.owner		= THIS_MODULE,
 		.of_match_table	= of_match_ptr(clps711x_uart_dt_ids),
 	},
 	.probe	= uart_clps711x_probe,
diff --git a/drivers/tty/serial/cpm_uart/cpm_uart_core.c b/drivers/tty/serial/cpm_uart/cpm_uart_core.c
index 533852eb8778..fddb1fd4d9d3 100644
--- a/drivers/tty/serial/cpm_uart/cpm_uart_core.c
+++ b/drivers/tty/serial/cpm_uart/cpm_uart_core.c
@@ -80,7 +80,8 @@ static void cpm_uart_initbd(struct uart_cpm_port *pinfo);
 */
 static unsigned int cpm_uart_tx_empty(struct uart_port *port)
 {
-	struct uart_cpm_port *pinfo = (struct uart_cpm_port *)port;
+	struct uart_cpm_port *pinfo =
+		container_of(port, struct uart_cpm_port, port);
 	cbd_t __iomem *bdp = pinfo->tx_bd_base;
 	int ret = 0;
 
@@ -102,7 +103,8 @@ static unsigned int cpm_uart_tx_empty(struct uart_port *port)
 
 static void cpm_uart_set_mctrl(struct uart_port *port, unsigned int mctrl)
 {
-	struct uart_cpm_port *pinfo = (struct uart_cpm_port *)port;
+	struct uart_cpm_port *pinfo =
+		container_of(port, struct uart_cpm_port, port);
 
 	if (pinfo->gpios[GPIO_RTS] >= 0)
 		gpio_set_value(pinfo->gpios[GPIO_RTS], !(mctrl & TIOCM_RTS));
@@ -113,7 +115,8 @@ static void cpm_uart_set_mctrl(struct uart_port *port, unsigned int mctrl)
 
 static unsigned int cpm_uart_get_mctrl(struct uart_port *port)
 {
-	struct uart_cpm_port *pinfo = (struct uart_cpm_port *)port;
+	struct uart_cpm_port *pinfo =
+		container_of(port, struct uart_cpm_port, port);
 	unsigned int mctrl = TIOCM_CTS | TIOCM_DSR | TIOCM_CAR;
 
 	if (pinfo->gpios[GPIO_CTS] >= 0) {
@@ -144,7 +147,8 @@ static unsigned int cpm_uart_get_mctrl(struct uart_port *port)
  */
 static void cpm_uart_stop_tx(struct uart_port *port)
 {
-	struct uart_cpm_port *pinfo = (struct uart_cpm_port *)port;
+	struct uart_cpm_port *pinfo =
+		container_of(port, struct uart_cpm_port, port);
 	smc_t __iomem *smcp = pinfo->smcp;
 	scc_t __iomem *sccp = pinfo->sccp;
 
@@ -161,7 +165,8 @@ static void cpm_uart_stop_tx(struct uart_port *port)
  */
 static void cpm_uart_start_tx(struct uart_port *port)
 {
-	struct uart_cpm_port *pinfo = (struct uart_cpm_port *)port;
+	struct uart_cpm_port *pinfo =
+		container_of(port, struct uart_cpm_port, port);
 	smc_t __iomem *smcp = pinfo->smcp;
 	scc_t __iomem *sccp = pinfo->sccp;
 
@@ -189,7 +194,8 @@ static void cpm_uart_start_tx(struct uart_port *port)
  */
 static void cpm_uart_stop_rx(struct uart_port *port)
 {
-	struct uart_cpm_port *pinfo = (struct uart_cpm_port *)port;
+	struct uart_cpm_port *pinfo =
+		container_of(port, struct uart_cpm_port, port);
 	smc_t __iomem *smcp = pinfo->smcp;
 	scc_t __iomem *sccp = pinfo->sccp;
 
@@ -206,7 +212,8 @@ static void cpm_uart_stop_rx(struct uart_port *port)
  */
 static void cpm_uart_break_ctl(struct uart_port *port, int break_state)
 {
-	struct uart_cpm_port *pinfo = (struct uart_cpm_port *)port;
+	struct uart_cpm_port *pinfo =
+		container_of(port, struct uart_cpm_port, port);
 
 	pr_debug("CPM uart[%d]:break ctrl, break_state: %d\n", port->line,
 		break_state);
@@ -240,7 +247,8 @@ static void cpm_uart_int_rx(struct uart_port *port)
 	unsigned char ch;
 	u8 *cp;
 	struct tty_port *tport = &port->state->port;
-	struct uart_cpm_port *pinfo = (struct uart_cpm_port *)port;
+	struct uart_cpm_port *pinfo =
+		container_of(port, struct uart_cpm_port, port);
 	cbd_t __iomem *bdp;
 	u16 status;
 	unsigned int flg;
@@ -397,7 +405,8 @@ static irqreturn_t cpm_uart_int(int irq, void *data)
 static int cpm_uart_startup(struct uart_port *port)
 {
 	int retval;
-	struct uart_cpm_port *pinfo = (struct uart_cpm_port *)port;
+	struct uart_cpm_port *pinfo =
+		container_of(port, struct uart_cpm_port, port);
 
 	pr_debug("CPM uart[%d]:startup\n", port->line);
 
@@ -442,7 +451,8 @@ inline void cpm_uart_wait_until_send(struct uart_cpm_port *pinfo)
  */
 static void cpm_uart_shutdown(struct uart_port *port)
 {
-	struct uart_cpm_port *pinfo = (struct uart_cpm_port *)port;
+	struct uart_cpm_port *pinfo =
+		container_of(port, struct uart_cpm_port, port);
 
 	pr_debug("CPM uart[%d]:shutdown\n", port->line);
 
@@ -492,7 +502,8 @@ static void cpm_uart_set_termios(struct uart_port *port,
 	unsigned long flags;
 	u16 cval, scval, prev_mode;
 	int bits, sbits;
-	struct uart_cpm_port *pinfo = (struct uart_cpm_port *)port;
+	struct uart_cpm_port *pinfo =
+		container_of(port, struct uart_cpm_port, port);
 	smc_t __iomem *smcp = pinfo->smcp;
 	scc_t __iomem *sccp = pinfo->sccp;
 	int maxidl;
@@ -675,7 +686,8 @@ static int cpm_uart_tx_pump(struct uart_port *port)
 	cbd_t __iomem *bdp;
 	u8 *p;
 	int count;
-	struct uart_cpm_port *pinfo = (struct uart_cpm_port *)port;
+	struct uart_cpm_port *pinfo =
+		container_of(port, struct uart_cpm_port, port);
 	struct circ_buf *xmit = &port->state->xmit;
 
 	/* Handle xon/xoff */
@@ -906,7 +918,8 @@ static void cpm_uart_init_smc(struct uart_cpm_port *pinfo)
  */
 static int cpm_uart_request_port(struct uart_port *port)
 {
-	struct uart_cpm_port *pinfo = (struct uart_cpm_port *)port;
+	struct uart_cpm_port *pinfo =
+		container_of(port, struct uart_cpm_port, port);
 	int ret;
 
 	pr_debug("CPM uart[%d]:request port\n", port->line);
@@ -938,7 +951,8 @@ static int cpm_uart_request_port(struct uart_port *port)
 
 static void cpm_uart_release_port(struct uart_port *port)
 {
-	struct uart_cpm_port *pinfo = (struct uart_cpm_port *)port;
+	struct uart_cpm_port *pinfo =
+		container_of(port, struct uart_cpm_port, port);
 
 	if (!(pinfo->flags & FLAG_CONSOLE))
 		cpm_uart_freebuf(pinfo);
@@ -1082,7 +1096,8 @@ static int poll_wait_key(char *obuf, struct uart_cpm_port *pinfo)
 
 static int cpm_get_poll_char(struct uart_port *port)
 {
-	struct uart_cpm_port *pinfo = (struct uart_cpm_port *)port;
+	struct uart_cpm_port *pinfo =
+		container_of(port, struct uart_cpm_port, port);
 
 	if (!serial_polled) {
 		serial_polled = 1;
@@ -1099,7 +1114,8 @@ static int cpm_get_poll_char(struct uart_port *port)
 static void cpm_put_poll_char(struct uart_port *port,
 			 unsigned char c)
 {
-	struct uart_cpm_port *pinfo = (struct uart_cpm_port *)port;
+	struct uart_cpm_port *pinfo =
+		container_of(port, struct uart_cpm_port, port);
 	static char ch[2];
 
 	ch[0] = (char)c;
@@ -1438,7 +1454,6 @@ static struct of_device_id cpm_uart_match[] = {
 static struct platform_driver cpm_uart_driver = {
 	.driver = {
 		.name = "cpm_uart",
-		.owner = THIS_MODULE,
 		.of_match_table = cpm_uart_match,
 	},
 	.probe = cpm_uart_probe,
diff --git a/drivers/tty/serial/crisv10.c b/drivers/tty/serial/crisv10.c
index 58e6f61a87e4..0c1825b0b41d 100644
--- a/drivers/tty/serial/crisv10.c
+++ b/drivers/tty/serial/crisv10.c
@@ -3676,12 +3676,6 @@ rs_close(struct tty_struct *tty, struct file * filp)
 	}
 	info->port.flags |= ASYNC_CLOSING;
 	/*
-	 * Save the termios structure, since this port may have
-	 * separate termios for callout and dialin.
-	 */
-	if (info->port.flags & ASYNC_NORMAL_ACTIVE)
-		info->normal_termios = tty->termios;
-	/*
 	 * Now we wait for the transmit buffer to clear; and we notify
 	 * the line discipline to only process XON/XOFF characters.
 	 */
@@ -4076,11 +4070,6 @@ rs_open(struct tty_struct *tty, struct file * filp)
 		return retval;
 	}
 
-	if ((info->port.count == 1) && (info->port.flags & ASYNC_SPLIT_TERMIOS)) {
-		tty->termios = info->normal_termios;
-		change_speed(info);
-	}
-
 #ifdef SERIAL_DEBUG_OPEN
 	printk("rs_open ttyS%d successful...\n", info->line);
 #endif
@@ -4327,7 +4316,6 @@ static int __init rs_init(void)
 		info->custom_divisor = 0;
 		info->x_char = 0;
 		info->event = 0;
-		info->normal_termios = driver->init_termios;
 		info->xmit.buf = NULL;
 		info->xmit.tail = info->xmit.head = 0;
 		info->first_recv_buffer = info->last_recv_buffer = NULL;
diff --git a/drivers/tty/serial/crisv10.h b/drivers/tty/serial/crisv10.h
index 7599014ae03f..15a52ee58251 100644
--- a/drivers/tty/serial/crisv10.h
+++ b/drivers/tty/serial/crisv10.h
@@ -98,7 +98,6 @@ struct e100_serial {
 
 	struct work_struct	work;
 	struct async_icount	icount;   /* error-statistics etc.*/
-	struct ktermios		normal_termios;
 
 	unsigned long char_time_usec;       /* The time for 1 char, in usecs */
 	unsigned long flush_time_usec;      /* How often we should flush */
diff --git a/drivers/tty/serial/earlycon.c b/drivers/tty/serial/earlycon.c
index a514ee6f5406..64fe25a4285c 100644
--- a/drivers/tty/serial/earlycon.c
+++ b/drivers/tty/serial/earlycon.c
@@ -98,7 +98,7 @@ static int __init parse_options(struct earlycon_device *device,
 		strlcpy(device->options, options, length);
 	}
 
-	if (mmio || mmio32)
+	if (port->iotype == UPIO_MEM || port->iotype == UPIO_MEM32)
 		pr_info("Early serial console at MMIO%s 0x%llx (options '%s')\n",
 			mmio32 ? "32" : "",
 			(unsigned long long)port->mapbase,
diff --git a/drivers/tty/serial/efm32-uart.c b/drivers/tty/serial/efm32-uart.c
index 55d9c00112cc..195acc868763 100644
--- a/drivers/tty/serial/efm32-uart.c
+++ b/drivers/tty/serial/efm32-uart.c
@@ -814,7 +814,6 @@ static struct platform_driver efm32_uart_driver = {
 
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = efm32_uart_dt_ids,
 	},
 };
diff --git a/drivers/tty/serial/fsl_lpuart.c b/drivers/tty/serial/fsl_lpuart.c
index 6dd53af546a3..e7cde3a9566d 100644
--- a/drivers/tty/serial/fsl_lpuart.c
+++ b/drivers/tty/serial/fsl_lpuart.c
@@ -1786,15 +1786,13 @@ static int lpuart_probe(struct platform_device *pdev)
 	}
 	sport->port.line = ret;
 	sport->lpuart32 = of_device_is_compatible(np, "fsl,ls1021a-lpuart");
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res)
-		return -ENODEV;
 
-	sport->port.mapbase = res->start;
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	sport->port.membase = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(sport->port.membase))
 		return PTR_ERR(sport->port.membase);
 
+	sport->port.mapbase = res->start;
 	sport->port.dev = &pdev->dev;
 	sport->port.type = PORT_LPUART;
 	sport->port.iotype = UPIO_MEM;
@@ -1862,6 +1860,20 @@ static int lpuart_suspend(struct device *dev)
 static int lpuart_resume(struct device *dev)
 {
 	struct lpuart_port *sport = dev_get_drvdata(dev);
+	unsigned long temp;
+
+	if (sport->lpuart32) {
+		lpuart32_setup_watermark(sport);
+		temp = lpuart32_read(sport->port.membase + UARTCTRL);
+		temp |= (UARTCTRL_RIE | UARTCTRL_TIE | UARTCTRL_RE |
+			 UARTCTRL_TE | UARTCTRL_ILIE);
+		lpuart32_write(temp, sport->port.membase + UARTCTRL);
+	} else {
+		lpuart_setup_watermark(sport);
+		temp = readb(sport->port.membase + UARTCR2);
+		temp |= (UARTCR2_RIE | UARTCR2_TIE | UARTCR2_RE | UARTCR2_TE);
+		writeb(temp, sport->port.membase + UARTCR2);
+	}
 
 	uart_resume_port(&lpuart_reg, &sport->port);
 
@@ -1876,7 +1888,6 @@ static struct platform_driver lpuart_driver = {
 	.remove		= lpuart_remove,
 	.driver		= {
 		.name	= "fsl-lpuart",
-		.owner	= THIS_MODULE,
 		.of_match_table = lpuart_dt_ids,
 		.pm	= &lpuart_pm_ops,
 	},
@@ -1884,11 +1895,8 @@ static struct platform_driver lpuart_driver = {
 
 static int __init lpuart_serial_init(void)
 {
-	int ret;
-
-	pr_info("serial: Freescale lpuart driver\n");
+	int ret = uart_register_driver(&lpuart_reg);
 
-	ret = uart_register_driver(&lpuart_reg);
 	if (ret)
 		return ret;
 
diff --git a/drivers/tty/serial/icom.c b/drivers/tty/serial/icom.c
index d4620fe5da2e..45fc323b95e6 100644
--- a/drivers/tty/serial/icom.c
+++ b/drivers/tty/serial/icom.c
@@ -1550,8 +1550,10 @@ static int icom_probe(struct pci_dev *dev,
 
 	icom_adapter->base_addr = pci_ioremap_bar(dev, 0);
 
-	if (!icom_adapter->base_addr)
+	if (!icom_adapter->base_addr) {
+		retval = -ENOMEM;
 		goto probe_exit1;
+	}
 
 	 /* save off irq and request irq line */
 	 if ( (retval = request_irq(dev->irq, icom_interrupt,
diff --git a/drivers/tty/serial/imx.c b/drivers/tty/serial/imx.c
index 8f62a3cec23e..4c5e9092e2d7 100644
--- a/drivers/tty/serial/imx.c
+++ b/drivers/tty/serial/imx.c
@@ -302,7 +302,7 @@ static inline int is_imx6q_uart(struct imx_port *sport)
 /*
  * Save and restore functions for UCR1, UCR2 and UCR3 registers
  */
-#if defined(CONFIG_CONSOLE_POLL) || defined(CONFIG_SERIAL_IMX_CONSOLE)
+#if defined(CONFIG_SERIAL_IMX_CONSOLE)
 static void imx_port_ucrs_save(struct uart_port *port,
 			       struct imx_port_ucrs *ucr)
 {
@@ -991,7 +991,6 @@ static int imx_uart_dma_init(struct imx_port *sport)
 
 	sport->rx_buf = kzalloc(PAGE_SIZE, GFP_KERNEL);
 	if (!sport->rx_buf) {
-		dev_err(dev, "cannot alloc DMA buffer.\n");
 		ret = -ENOMEM;
 		goto err;
 	}
@@ -1076,11 +1075,11 @@ static int imx_startup(struct uart_port *port)
 
 	retval = clk_prepare_enable(sport->clk_per);
 	if (retval)
-		goto error_out1;
+		return retval;
 	retval = clk_prepare_enable(sport->clk_ipg);
 	if (retval) {
 		clk_disable_unprepare(sport->clk_per);
-		goto error_out1;
+		return retval;
 	}
 
 	imx_setup_ufcr(sport, 0);
@@ -1109,37 +1108,6 @@ static int imx_startup(struct uart_port *port)
 	while (!(readl(sport->port.membase + UCR2) & UCR2_SRST) && (--i > 0))
 		udelay(1);
 
-	/*
-	 * Allocate the IRQ(s) i.MX1 has three interrupts whereas later
-	 * chips only have one interrupt.
-	 */
-	if (sport->txirq > 0) {
-		retval = request_irq(sport->rxirq, imx_rxint, 0,
-				     dev_name(port->dev), sport);
-		if (retval)
-			goto error_out1;
-
-		retval = request_irq(sport->txirq, imx_txint, 0,
-				     dev_name(port->dev), sport);
-		if (retval)
-			goto error_out2;
-
-		/* do not use RTS IRQ on IrDA */
-		if (!USE_IRDA(sport)) {
-			retval = request_irq(sport->rtsirq, imx_rtsint, 0,
-					     dev_name(port->dev), sport);
-			if (retval)
-				goto error_out3;
-		}
-	} else {
-		retval = request_irq(sport->port.irq, imx_int, 0,
-				     dev_name(port->dev), sport);
-		if (retval) {
-			free_irq(sport->port.irq, sport);
-			goto error_out1;
-		}
-	}
-
 	spin_lock_irqsave(&sport->port.lock, flags);
 	/*
 	 * Finally, clear and enable interrupts
@@ -1201,15 +1169,6 @@ static int imx_startup(struct uart_port *port)
 	}
 
 	return 0;
-
-error_out3:
-	if (sport->txirq)
-		free_irq(sport->txirq, sport);
-error_out2:
-	if (sport->rxirq)
-		free_irq(sport->rxirq, sport);
-error_out1:
-	return retval;
 }
 
 static void imx_shutdown(struct uart_port *port)
@@ -1255,17 +1214,6 @@ static void imx_shutdown(struct uart_port *port)
 	del_timer_sync(&sport->timer);
 
 	/*
-	 * Free the interrupts
-	 */
-	if (sport->txirq > 0) {
-		if (!USE_IRDA(sport))
-			free_irq(sport->rtsirq, sport);
-		free_irq(sport->txirq, sport);
-		free_irq(sport->rxirq, sport);
-	} else
-		free_irq(sport->port.irq, sport);
-
-	/*
 	 * Disable all interrupts, port and break condition.
 	 */
 
@@ -1507,44 +1455,65 @@ imx_verify_port(struct uart_port *port, struct serial_struct *ser)
 }
 
 #if defined(CONFIG_CONSOLE_POLL)
+
+static int imx_poll_init(struct uart_port *port)
+{
+	struct imx_port *sport = (struct imx_port *)port;
+	unsigned long flags;
+	unsigned long temp;
+	int retval;
+
+	retval = clk_prepare_enable(sport->clk_ipg);
+	if (retval)
+		return retval;
+	retval = clk_prepare_enable(sport->clk_per);
+	if (retval)
+		clk_disable_unprepare(sport->clk_ipg);
+
+	imx_setup_ufcr(sport, 0);
+
+	spin_lock_irqsave(&sport->port.lock, flags);
+
+	temp = readl(sport->port.membase + UCR1);
+	if (is_imx1_uart(sport))
+		temp |= IMX1_UCR1_UARTCLKEN;
+	temp |= UCR1_UARTEN | UCR1_RRDYEN;
+	temp &= ~(UCR1_TXMPTYEN | UCR1_RTSDEN);
+	writel(temp, sport->port.membase + UCR1);
+
+	temp = readl(sport->port.membase + UCR2);
+	temp |= UCR2_RXEN;
+	writel(temp, sport->port.membase + UCR2);
+
+	spin_unlock_irqrestore(&sport->port.lock, flags);
+
+	return 0;
+}
+
 static int imx_poll_get_char(struct uart_port *port)
 {
-	if (!(readl(port->membase + USR2) & USR2_RDR))
+	if (!(readl_relaxed(port->membase + USR2) & USR2_RDR))
 		return NO_POLL_CHAR;
 
-	return readl(port->membase + URXD0) & URXD_RX_DATA;
+	return readl_relaxed(port->membase + URXD0) & URXD_RX_DATA;
 }
 
 static void imx_poll_put_char(struct uart_port *port, unsigned char c)
 {
-	struct imx_port_ucrs old_ucr;
 	unsigned int status;
 
-	/* save control registers */
-	imx_port_ucrs_save(port, &old_ucr);
-
-	/* disable interrupts */
-	writel(UCR1_UARTEN, port->membase + UCR1);
-	writel(old_ucr.ucr2 & ~(UCR2_ATEN | UCR2_RTSEN | UCR2_ESCI),
-	       port->membase + UCR2);
-	writel(old_ucr.ucr3 & ~(UCR3_DCD | UCR3_RI | UCR3_DTREN),
-	       port->membase + UCR3);
-
 	/* drain */
 	do {
-		status = readl(port->membase + USR1);
+		status = readl_relaxed(port->membase + USR1);
 	} while (~status & USR1_TRDY);
 
 	/* write */
-	writel(c, port->membase + URTX0);
+	writel_relaxed(c, port->membase + URTX0);
 
 	/* flush */
 	do {
-		status = readl(port->membase + USR2);
+		status = readl_relaxed(port->membase + USR2);
 	} while (~status & USR2_TXDC);
-
-	/* restore control registers */
-	imx_port_ucrs_restore(port, &old_ucr);
 }
 #endif
 
@@ -1565,6 +1534,7 @@ static struct uart_ops imx_pops = {
 	.config_port	= imx_config_port,
 	.verify_port	= imx_verify_port,
 #if defined(CONFIG_CONSOLE_POLL)
+	.poll_init      = imx_poll_init,
 	.poll_get_char  = imx_poll_get_char,
 	.poll_put_char  = imx_poll_put_char,
 #endif
@@ -1929,6 +1899,36 @@ static int serial_imx_probe(struct platform_device *pdev)
 
 	sport->port.uartclk = clk_get_rate(sport->clk_per);
 
+	/*
+	 * Allocate the IRQ(s) i.MX1 has three interrupts whereas later
+	 * chips only have one interrupt.
+	 */
+	if (sport->txirq > 0) {
+		ret = devm_request_irq(&pdev->dev, sport->rxirq, imx_rxint, 0,
+				       dev_name(&pdev->dev), sport);
+		if (ret)
+			return ret;
+
+		ret = devm_request_irq(&pdev->dev, sport->txirq, imx_txint, 0,
+				       dev_name(&pdev->dev), sport);
+		if (ret)
+			return ret;
+
+		/* do not use RTS IRQ on IrDA */
+		if (!USE_IRDA(sport)) {
+			ret = devm_request_irq(&pdev->dev, sport->rtsirq,
+					       imx_rtsint, 0,
+					       dev_name(&pdev->dev), sport);
+			if (ret)
+				return ret;
+		}
+	} else {
+		ret = devm_request_irq(&pdev->dev, sport->port.irq, imx_int, 0,
+				       dev_name(&pdev->dev), sport);
+		if (ret)
+			return ret;
+	}
+
 	imx_ports[sport->port.line] = sport;
 
 	platform_set_drvdata(pdev, sport);
@@ -1952,18 +1952,14 @@ static struct platform_driver serial_imx_driver = {
 	.id_table	= imx_uart_devtype,
 	.driver		= {
 		.name	= "imx-uart",
-		.owner	= THIS_MODULE,
 		.of_match_table = imx_uart_dt_ids,
 	},
 };
 
 static int __init imx_serial_init(void)
 {
-	int ret;
-
-	pr_info("Serial: IMX driver\n");
+	int ret = uart_register_driver(&imx_reg);
 
-	ret = uart_register_driver(&imx_reg);
 	if (ret)
 		return ret;
 
diff --git a/drivers/tty/serial/ip22zilog.c b/drivers/tty/serial/ip22zilog.c
index 99b7b8697861..991e6dce916e 100644
--- a/drivers/tty/serial/ip22zilog.c
+++ b/drivers/tty/serial/ip22zilog.c
@@ -544,7 +544,8 @@ static unsigned int ip22zilog_get_mctrl(struct uart_port *port)
 /* The port lock is held and interrupts are disabled.  */
 static void ip22zilog_set_mctrl(struct uart_port *port, unsigned int mctrl)
 {
-	struct uart_ip22zilog_port *up = (struct uart_ip22zilog_port *) port;
+	struct uart_ip22zilog_port *up =
+		container_of(port, struct uart_ip22zilog_port, port);
 	struct zilog_channel *channel = ZILOG_CHANNEL_FROM_PORT(port);
 	unsigned char set_bits, clear_bits;
 
@@ -568,7 +569,8 @@ static void ip22zilog_set_mctrl(struct uart_port *port, unsigned int mctrl)
 /* The port lock is held and interrupts are disabled.  */
 static void ip22zilog_stop_tx(struct uart_port *port)
 {
-	struct uart_ip22zilog_port *up = (struct uart_ip22zilog_port *) port;
+	struct uart_ip22zilog_port *up =
+		container_of(port, struct uart_ip22zilog_port, port);
 
 	up->flags |= IP22ZILOG_FLAG_TX_STOPPED;
 }
@@ -576,7 +578,8 @@ static void ip22zilog_stop_tx(struct uart_port *port)
 /* The port lock is held and interrupts are disabled.  */
 static void ip22zilog_start_tx(struct uart_port *port)
 {
-	struct uart_ip22zilog_port *up = (struct uart_ip22zilog_port *) port;
+	struct uart_ip22zilog_port *up =
+		container_of(port, struct uart_ip22zilog_port, port);
 	struct zilog_channel *channel = ZILOG_CHANNEL_FROM_PORT(port);
 	unsigned char status;
 
@@ -636,7 +639,8 @@ static void ip22zilog_stop_rx(struct uart_port *port)
 /* The port lock is held.  */
 static void ip22zilog_enable_ms(struct uart_port *port)
 {
-	struct uart_ip22zilog_port *up = (struct uart_ip22zilog_port *) port;
+	struct uart_ip22zilog_port *up =
+		container_of(port, struct uart_ip22zilog_port, port);
 	struct zilog_channel *channel = ZILOG_CHANNEL_FROM_PORT(port);
 	unsigned char new_reg;
 
@@ -652,7 +656,8 @@ static void ip22zilog_enable_ms(struct uart_port *port)
 /* The port lock is not held.  */
 static void ip22zilog_break_ctl(struct uart_port *port, int break_state)
 {
-	struct uart_ip22zilog_port *up = (struct uart_ip22zilog_port *) port;
+	struct uart_ip22zilog_port *up =
+		container_of(port, struct uart_ip22zilog_port, port);
 	struct zilog_channel *channel = ZILOG_CHANNEL_FROM_PORT(port);
 	unsigned char set_bits, clear_bits, new_reg;
 	unsigned long flags;
@@ -873,7 +878,8 @@ static void
 ip22zilog_set_termios(struct uart_port *port, struct ktermios *termios,
 		      struct ktermios *old)
 {
-	struct uart_ip22zilog_port *up = (struct uart_ip22zilog_port *) port;
+	struct uart_ip22zilog_port *up =
+		container_of(port, struct uart_ip22zilog_port, port);
 	unsigned long flags;
 	int baud, brg;
 
diff --git a/drivers/tty/serial/jsm/Makefile b/drivers/tty/serial/jsm/Makefile
index e46b6e0f8b18..705d1ff6fdeb 100644
--- a/drivers/tty/serial/jsm/Makefile
+++ b/drivers/tty/serial/jsm/Makefile
@@ -4,5 +4,5 @@
 
 obj-$(CONFIG_SERIAL_JSM) += jsm.o
 
-jsm-objs :=    jsm_driver.o jsm_neo.o jsm_tty.o
+jsm-objs :=    jsm_driver.o jsm_neo.o jsm_tty.o jsm_cls.o
 
diff --git a/drivers/tty/serial/jsm/jsm.h b/drivers/tty/serial/jsm/jsm.h
index af7013488aeb..0b79b87df47d 100644
--- a/drivers/tty/serial/jsm/jsm.h
+++ b/drivers/tty/serial/jsm/jsm.h
@@ -13,11 +13,6 @@
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU 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.
- *
  * Contact Information:
  * Scott H Kilau <Scott_Kilau@digi.com>
  * Wendy Xiong   <wendyx@us.ibm.com>
@@ -68,6 +63,10 @@ do {								\
 #define MAX_STOPS_SENT	5
 
 /* Board ids */
+#define PCI_DEVICE_ID_CLASSIC_4		0x0028
+#define PCI_DEVICE_ID_CLASSIC_8		0x0029
+#define PCI_DEVICE_ID_CLASSIC_4_422	0x00D0
+#define PCI_DEVICE_ID_CLASSIC_8_422	0x00D1
 #define PCI_DEVICE_ID_NEO_4             0x00B0
 #define PCI_DEVICE_ID_NEO_1_422         0x00CC
 #define PCI_DEVICE_ID_NEO_1_422_485     0x00CD
@@ -112,21 +111,21 @@ struct jsm_channel;
  ************************************************************************/
 struct board_ops {
 	irq_handler_t intr;
-	void (*uart_init) (struct jsm_channel *ch);
-	void (*uart_off) (struct jsm_channel *ch);
-	void (*param) (struct jsm_channel *ch);
-	void (*assert_modem_signals) (struct jsm_channel *ch);
-	void (*flush_uart_write) (struct jsm_channel *ch);
-	void (*flush_uart_read) (struct jsm_channel *ch);
-	void (*disable_receiver) (struct jsm_channel *ch);
-	void (*enable_receiver) (struct jsm_channel *ch);
-	void (*send_break) (struct jsm_channel *ch);
-	void (*clear_break) (struct jsm_channel *ch, int);
-	void (*send_start_character) (struct jsm_channel *ch);
-	void (*send_stop_character) (struct jsm_channel *ch);
-	void (*copy_data_from_queue_to_uart) (struct jsm_channel *ch);
-	u32 (*get_uart_bytes_left) (struct jsm_channel *ch);
-	void (*send_immediate_char) (struct jsm_channel *ch, unsigned char);
+	void (*uart_init)(struct jsm_channel *ch);
+	void (*uart_off)(struct jsm_channel *ch);
+	void (*param)(struct jsm_channel *ch);
+	void (*assert_modem_signals)(struct jsm_channel *ch);
+	void (*flush_uart_write)(struct jsm_channel *ch);
+	void (*flush_uart_read)(struct jsm_channel *ch);
+	void (*disable_receiver)(struct jsm_channel *ch);
+	void (*enable_receiver)(struct jsm_channel *ch);
+	void (*send_break)(struct jsm_channel *ch);
+	void (*clear_break)(struct jsm_channel *ch);
+	void (*send_start_character)(struct jsm_channel *ch);
+	void (*send_stop_character)(struct jsm_channel *ch);
+	void (*copy_data_from_queue_to_uart)(struct jsm_channel *ch);
+	u32 (*get_uart_bytes_left)(struct jsm_channel *ch);
+	void (*send_immediate_char)(struct jsm_channel *ch, unsigned char);
 };
 
 
@@ -189,7 +188,7 @@ struct jsm_board
 #define CH_LOOPBACK 0x2000		/* Channel is in lookback mode	*/
 #define CH_BAUD0	0x08000		/* Used for checking B0 transitions */
 
-/* Our Read/Error/Write queue sizes */
+/* Our Read/Error queue sizes */
 #define RQUEUEMASK	0x1FFF		/* 8 K - 1 */
 #define EQUEUEMASK	0x1FFF		/* 8 K - 1 */
 #define RQUEUESIZE	(RQUEUEMASK + 1)
@@ -222,7 +221,10 @@ struct jsm_channel {
 	u8		ch_mostat;	/* FEP output modem status	*/
 	u8		ch_mistat;	/* FEP input modem status	*/
 
-	struct neo_uart_struct __iomem *ch_neo_uart;	/* Pointer to the "mapped" UART struct */
+	/* Pointers to the "mapped" UART structs */
+	struct neo_uart_struct __iomem *ch_neo_uart; /* NEO card */
+	struct cls_uart_struct __iomem *ch_cls_uart; /* Classic card */
+
 	u8		ch_cached_lsr;	/* Cached value of the LSR register */
 
 	u8		*ch_rqueue;	/* Our read queue buffer - malloc'ed */
@@ -254,6 +256,60 @@ struct jsm_channel {
 	u64		ch_xoff_sends;	/* Count of xoffs transmitted */
 };
 
+/************************************************************************
+ * Per channel/port Classic UART structures				*
+ ************************************************************************
+ *		Base Structure Entries Usage Meanings to Host		*
+ *									*
+ *	W = read write		R = read only				*
+ *			U = Unused.					*
+ ************************************************************************/
+
+struct cls_uart_struct {
+	u8 txrx;	/* WR  RHR/THR - Holding Reg */
+	u8 ier;		/* WR  IER - Interrupt Enable Reg */
+	u8 isr_fcr;	/* WR  ISR/FCR - Interrupt Status Reg/Fifo Control Reg*/
+	u8 lcr;		/* WR  LCR - Line Control Reg */
+	u8 mcr;		/* WR  MCR - Modem Control Reg */
+	u8 lsr;		/* WR  LSR - Line Status Reg */
+	u8 msr;		/* WR  MSR - Modem Status Reg */
+	u8 spr;		/* WR  SPR - Scratch Pad Reg */
+};
+
+/* Where to read the interrupt register (8bits) */
+#define UART_CLASSIC_POLL_ADDR_OFFSET	0x40
+
+#define UART_EXAR654_ENHANCED_REGISTER_SET 0xBF
+
+#define UART_16654_FCR_TXTRIGGER_8	0x0
+#define UART_16654_FCR_TXTRIGGER_16	0x10
+#define UART_16654_FCR_TXTRIGGER_32	0x20
+#define UART_16654_FCR_TXTRIGGER_56	0x30
+
+#define UART_16654_FCR_RXTRIGGER_8	0x0
+#define UART_16654_FCR_RXTRIGGER_16	0x40
+#define UART_16654_FCR_RXTRIGGER_56	0x80
+#define UART_16654_FCR_RXTRIGGER_60	0xC0
+
+#define UART_IIR_CTSRTS			0x20	/* Received CTS/RTS change of state */
+#define UART_IIR_RDI_TIMEOUT		0x0C    /* Receiver data TIMEOUT */
+
+/*
+ * These are the EXTENDED definitions for the Exar 654's Interrupt
+ * Enable Register.
+ */
+#define UART_EXAR654_EFR_ECB      0x10    /* Enhanced control bit */
+#define UART_EXAR654_EFR_IXON     0x2     /* Receiver compares Xon1/Xoff1 */
+#define UART_EXAR654_EFR_IXOFF    0x8     /* Transmit Xon1/Xoff1 */
+#define UART_EXAR654_EFR_RTSDTR   0x40    /* Auto RTS/DTR Flow Control Enable */
+#define UART_EXAR654_EFR_CTSDSR   0x80    /* Auto CTS/DSR Flow COntrol Enable */
+
+#define UART_EXAR654_XOFF_DETECT  0x1     /* Indicates whether chip saw an incoming XOFF char  */
+#define UART_EXAR654_XON_DETECT   0x2     /* Indicates whether chip saw an incoming XON char */
+
+#define UART_EXAR654_IER_XOFF     0x20    /* Xoff Interrupt Enable */
+#define UART_EXAR654_IER_RTSDTR   0x40    /* Output Interrupt Enable */
+#define UART_EXAR654_IER_CTSDSR   0x80    /* Input Interrupt Enable */
 
 /************************************************************************
  * Per channel/port NEO UART structure					*
@@ -374,6 +430,7 @@ struct neo_uart_struct {
  */
 extern struct	uart_driver jsm_uart_driver;
 extern struct	board_ops jsm_neo_ops;
+extern struct	board_ops jsm_cls_ops;
 extern int	jsm_debug;
 
 /*************************************************************************
diff --git a/drivers/tty/serial/jsm/jsm_cls.c b/drivers/tty/serial/jsm/jsm_cls.c
new file mode 100644
index 000000000000..bfb0681195b6
--- /dev/null
+++ b/drivers/tty/serial/jsm/jsm_cls.c
@@ -0,0 +1,982 @@
+/*
+ * Copyright 2003 Digi International (www.digi.com)
+ *	Scott H Kilau <Scott_Kilau at digi dot com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY, EXPRESS OR IMPLIED; without even the
+ * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  See the GNU General Public License for more details.
+ *
+ *	NOTE TO LINUX KERNEL HACKERS:  DO NOT REFORMAT THIS CODE!
+ *
+ *	This is shared code between Digi's CVS archive and the
+ *	Linux Kernel sources.
+ *	Changing the source just for reformatting needlessly breaks
+ *	our CVS diff history.
+ *
+ *	Send any bug fixes/changes to:  Eng.Linux at digi dot com.
+ *	Thank you.
+ *
+ */
+
+#include <linux/delay.h>	/* For udelay */
+#include <linux/io.h>		/* For read[bwl]/write[bwl] */
+#include <linux/serial.h>	/* For struct async_serial */
+#include <linux/serial_reg.h>	/* For the various UART offsets */
+#include <linux/pci.h>
+#include <linux/tty.h>
+
+#include "jsm.h"	/* Driver main header file */
+
+static struct {
+	unsigned int rate;
+	unsigned int cflag;
+} baud_rates[] = {
+	{ 921600, B921600 },
+	{ 460800, B460800 },
+	{ 230400, B230400 },
+	{ 115200, B115200 },
+	{  57600, B57600  },
+	{  38400, B38400  },
+	{  19200, B19200  },
+	{   9600, B9600   },
+	{   4800, B4800   },
+	{   2400, B2400   },
+	{   1200, B1200   },
+	{    600, B600    },
+	{    300, B300    },
+	{    200, B200    },
+	{    150, B150    },
+	{    134, B134    },
+	{    110, B110    },
+	{     75, B75     },
+	{     50, B50     },
+};
+
+static void cls_set_cts_flow_control(struct jsm_channel *ch)
+{
+	u8 lcrb = readb(&ch->ch_cls_uart->lcr);
+	u8 ier = readb(&ch->ch_cls_uart->ier);
+	u8 isr_fcr = 0;
+
+	/*
+	 * The Enhanced Register Set may only be accessed when
+	 * the Line Control Register is set to 0xBFh.
+	 */
+	writeb(UART_EXAR654_ENHANCED_REGISTER_SET, &ch->ch_cls_uart->lcr);
+
+	isr_fcr = readb(&ch->ch_cls_uart->isr_fcr);
+
+	/* Turn on CTS flow control, turn off IXON flow control */
+	isr_fcr |= (UART_EXAR654_EFR_ECB | UART_EXAR654_EFR_CTSDSR);
+	isr_fcr &= ~(UART_EXAR654_EFR_IXON);
+
+	writeb(isr_fcr, &ch->ch_cls_uart->isr_fcr);
+
+	/* Write old LCR value back out, which turns enhanced access off */
+	writeb(lcrb, &ch->ch_cls_uart->lcr);
+
+	/*
+	 * Enable interrupts for CTS flow, turn off interrupts for
+	 * received XOFF chars
+	 */
+	ier |= (UART_EXAR654_IER_CTSDSR);
+	ier &= ~(UART_EXAR654_IER_XOFF);
+	writeb(ier, &ch->ch_cls_uart->ier);
+
+	/* Set the usual FIFO values */
+	writeb((UART_FCR_ENABLE_FIFO), &ch->ch_cls_uart->isr_fcr);
+
+	writeb((UART_FCR_ENABLE_FIFO | UART_16654_FCR_RXTRIGGER_56 |
+		UART_16654_FCR_TXTRIGGER_16 | UART_FCR_CLEAR_RCVR),
+		&ch->ch_cls_uart->isr_fcr);
+
+	ch->ch_t_tlevel = 16;
+}
+
+static void cls_set_ixon_flow_control(struct jsm_channel *ch)
+{
+	u8 lcrb = readb(&ch->ch_cls_uart->lcr);
+	u8 ier = readb(&ch->ch_cls_uart->ier);
+	u8 isr_fcr = 0;
+
+	/*
+	 * The Enhanced Register Set may only be accessed when
+	 * the Line Control Register is set to 0xBFh.
+	 */
+	writeb(UART_EXAR654_ENHANCED_REGISTER_SET, &ch->ch_cls_uart->lcr);
+
+	isr_fcr = readb(&ch->ch_cls_uart->isr_fcr);
+
+	/* Turn on IXON flow control, turn off CTS flow control */
+	isr_fcr |= (UART_EXAR654_EFR_ECB | UART_EXAR654_EFR_IXON);
+	isr_fcr &= ~(UART_EXAR654_EFR_CTSDSR);
+
+	writeb(isr_fcr, &ch->ch_cls_uart->isr_fcr);
+
+	/* Now set our current start/stop chars while in enhanced mode */
+	writeb(ch->ch_startc, &ch->ch_cls_uart->mcr);
+	writeb(0, &ch->ch_cls_uart->lsr);
+	writeb(ch->ch_stopc, &ch->ch_cls_uart->msr);
+	writeb(0, &ch->ch_cls_uart->spr);
+
+	/* Write old LCR value back out, which turns enhanced access off */
+	writeb(lcrb, &ch->ch_cls_uart->lcr);
+
+	/*
+	 * Disable interrupts for CTS flow, turn on interrupts for
+	 * received XOFF chars
+	 */
+	ier &= ~(UART_EXAR654_IER_CTSDSR);
+	ier |= (UART_EXAR654_IER_XOFF);
+	writeb(ier, &ch->ch_cls_uart->ier);
+
+	/* Set the usual FIFO values */
+	writeb((UART_FCR_ENABLE_FIFO), &ch->ch_cls_uart->isr_fcr);
+
+	writeb((UART_FCR_ENABLE_FIFO | UART_16654_FCR_RXTRIGGER_16 |
+		UART_16654_FCR_TXTRIGGER_16 | UART_FCR_CLEAR_RCVR),
+		&ch->ch_cls_uart->isr_fcr);
+}
+
+static void cls_set_no_output_flow_control(struct jsm_channel *ch)
+{
+	u8 lcrb = readb(&ch->ch_cls_uart->lcr);
+	u8 ier = readb(&ch->ch_cls_uart->ier);
+	u8 isr_fcr = 0;
+
+	/*
+	 * The Enhanced Register Set may only be accessed when
+	 * the Line Control Register is set to 0xBFh.
+	 */
+	writeb(UART_EXAR654_ENHANCED_REGISTER_SET, &ch->ch_cls_uart->lcr);
+
+	isr_fcr = readb(&ch->ch_cls_uart->isr_fcr);
+
+	/* Turn off IXON flow control, turn off CTS flow control */
+	isr_fcr |= (UART_EXAR654_EFR_ECB);
+	isr_fcr &= ~(UART_EXAR654_EFR_CTSDSR | UART_EXAR654_EFR_IXON);
+
+	writeb(isr_fcr, &ch->ch_cls_uart->isr_fcr);
+
+	/* Write old LCR value back out, which turns enhanced access off */
+	writeb(lcrb, &ch->ch_cls_uart->lcr);
+
+	/*
+	 * Disable interrupts for CTS flow, turn off interrupts for
+	 * received XOFF chars
+	 */
+	ier &= ~(UART_EXAR654_IER_CTSDSR);
+	ier &= ~(UART_EXAR654_IER_XOFF);
+	writeb(ier, &ch->ch_cls_uart->ier);
+
+	/* Set the usual FIFO values */
+	writeb((UART_FCR_ENABLE_FIFO), &ch->ch_cls_uart->isr_fcr);
+
+	writeb((UART_FCR_ENABLE_FIFO | UART_16654_FCR_RXTRIGGER_16 |
+		UART_16654_FCR_TXTRIGGER_16 | UART_FCR_CLEAR_RCVR),
+		&ch->ch_cls_uart->isr_fcr);
+
+	ch->ch_r_watermark = 0;
+	ch->ch_t_tlevel = 16;
+	ch->ch_r_tlevel = 16;
+}
+
+static void cls_set_rts_flow_control(struct jsm_channel *ch)
+{
+	u8 lcrb = readb(&ch->ch_cls_uart->lcr);
+	u8 ier = readb(&ch->ch_cls_uart->ier);
+	u8 isr_fcr = 0;
+
+	/*
+	 * The Enhanced Register Set may only be accessed when
+	 * the Line Control Register is set to 0xBFh.
+	 */
+	writeb(UART_EXAR654_ENHANCED_REGISTER_SET, &ch->ch_cls_uart->lcr);
+
+	isr_fcr = readb(&ch->ch_cls_uart->isr_fcr);
+
+	/* Turn on RTS flow control, turn off IXOFF flow control */
+	isr_fcr |= (UART_EXAR654_EFR_ECB | UART_EXAR654_EFR_RTSDTR);
+	isr_fcr &= ~(UART_EXAR654_EFR_IXOFF);
+
+	writeb(isr_fcr, &ch->ch_cls_uart->isr_fcr);
+
+	/* Write old LCR value back out, which turns enhanced access off */
+	writeb(lcrb, &ch->ch_cls_uart->lcr);
+
+	/* Enable interrupts for RTS flow */
+	ier |= (UART_EXAR654_IER_RTSDTR);
+	writeb(ier, &ch->ch_cls_uart->ier);
+
+	/* Set the usual FIFO values */
+	writeb((UART_FCR_ENABLE_FIFO), &ch->ch_cls_uart->isr_fcr);
+
+	writeb((UART_FCR_ENABLE_FIFO | UART_16654_FCR_RXTRIGGER_56 |
+		UART_16654_FCR_TXTRIGGER_16 | UART_FCR_CLEAR_RCVR),
+		&ch->ch_cls_uart->isr_fcr);
+
+	ch->ch_r_watermark = 4;
+	ch->ch_r_tlevel = 8;
+}
+
+static void cls_set_ixoff_flow_control(struct jsm_channel *ch)
+{
+	u8 lcrb = readb(&ch->ch_cls_uart->lcr);
+	u8 ier = readb(&ch->ch_cls_uart->ier);
+	u8 isr_fcr = 0;
+
+	/*
+	 * The Enhanced Register Set may only be accessed when
+	 * the Line Control Register is set to 0xBFh.
+	 */
+	writeb(UART_EXAR654_ENHANCED_REGISTER_SET, &ch->ch_cls_uart->lcr);
+
+	isr_fcr = readb(&ch->ch_cls_uart->isr_fcr);
+
+	/* Turn on IXOFF flow control, turn off RTS flow control */
+	isr_fcr |= (UART_EXAR654_EFR_ECB | UART_EXAR654_EFR_IXOFF);
+	isr_fcr &= ~(UART_EXAR654_EFR_RTSDTR);
+
+	writeb(isr_fcr, &ch->ch_cls_uart->isr_fcr);
+
+	/* Now set our current start/stop chars while in enhanced mode */
+	writeb(ch->ch_startc, &ch->ch_cls_uart->mcr);
+	writeb(0, &ch->ch_cls_uart->lsr);
+	writeb(ch->ch_stopc, &ch->ch_cls_uart->msr);
+	writeb(0, &ch->ch_cls_uart->spr);
+
+	/* Write old LCR value back out, which turns enhanced access off */
+	writeb(lcrb, &ch->ch_cls_uart->lcr);
+
+	/* Disable interrupts for RTS flow */
+	ier &= ~(UART_EXAR654_IER_RTSDTR);
+	writeb(ier, &ch->ch_cls_uart->ier);
+
+	/* Set the usual FIFO values */
+	writeb((UART_FCR_ENABLE_FIFO), &ch->ch_cls_uart->isr_fcr);
+
+	writeb((UART_FCR_ENABLE_FIFO | UART_16654_FCR_RXTRIGGER_16 |
+		UART_16654_FCR_TXTRIGGER_16 | UART_FCR_CLEAR_RCVR),
+		&ch->ch_cls_uart->isr_fcr);
+}
+
+static void cls_set_no_input_flow_control(struct jsm_channel *ch)
+{
+	u8 lcrb = readb(&ch->ch_cls_uart->lcr);
+	u8 ier = readb(&ch->ch_cls_uart->ier);
+	u8 isr_fcr = 0;
+
+	/*
+	 * The Enhanced Register Set may only be accessed when
+	 * the Line Control Register is set to 0xBFh.
+	 */
+	writeb(UART_EXAR654_ENHANCED_REGISTER_SET, &ch->ch_cls_uart->lcr);
+
+	isr_fcr = readb(&ch->ch_cls_uart->isr_fcr);
+
+	/* Turn off IXOFF flow control, turn off RTS flow control */
+	isr_fcr |= (UART_EXAR654_EFR_ECB);
+	isr_fcr &= ~(UART_EXAR654_EFR_RTSDTR | UART_EXAR654_EFR_IXOFF);
+
+	writeb(isr_fcr, &ch->ch_cls_uart->isr_fcr);
+
+	/* Write old LCR value back out, which turns enhanced access off */
+	writeb(lcrb, &ch->ch_cls_uart->lcr);
+
+	/* Disable interrupts for RTS flow */
+	ier &= ~(UART_EXAR654_IER_RTSDTR);
+	writeb(ier, &ch->ch_cls_uart->ier);
+
+	/* Set the usual FIFO values */
+	writeb((UART_FCR_ENABLE_FIFO), &ch->ch_cls_uart->isr_fcr);
+
+	writeb((UART_FCR_ENABLE_FIFO | UART_16654_FCR_RXTRIGGER_16 |
+		UART_16654_FCR_TXTRIGGER_16 | UART_FCR_CLEAR_RCVR),
+		&ch->ch_cls_uart->isr_fcr);
+
+	ch->ch_t_tlevel = 16;
+	ch->ch_r_tlevel = 16;
+}
+
+/*
+ * cls_clear_break.
+ * Determines whether its time to shut off break condition.
+ *
+ * No locks are assumed to be held when calling this function.
+ * channel lock is held and released in this function.
+ */
+static void cls_clear_break(struct jsm_channel *ch)
+{
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&ch->ch_lock, lock_flags);
+
+	/* Turn break off, and unset some variables */
+	if (ch->ch_flags & CH_BREAK_SENDING) {
+		u8 temp = readb(&ch->ch_cls_uart->lcr);
+
+		writeb((temp & ~UART_LCR_SBC), &ch->ch_cls_uart->lcr);
+
+		ch->ch_flags &= ~(CH_BREAK_SENDING);
+		jsm_dbg(IOCTL, &ch->ch_bd->pci_dev,
+			"clear break Finishing UART_LCR_SBC! finished: %lx\n",
+			jiffies);
+	}
+	spin_unlock_irqrestore(&ch->ch_lock, lock_flags);
+}
+
+static void cls_disable_receiver(struct jsm_channel *ch)
+{
+	u8 tmp = readb(&ch->ch_cls_uart->ier);
+
+	tmp &= ~(UART_IER_RDI);
+	writeb(tmp, &ch->ch_cls_uart->ier);
+}
+
+static void cls_enable_receiver(struct jsm_channel *ch)
+{
+	u8 tmp = readb(&ch->ch_cls_uart->ier);
+
+	tmp |= (UART_IER_RDI);
+	writeb(tmp, &ch->ch_cls_uart->ier);
+}
+
+/* Make the UART raise any of the output signals we want up */
+static void cls_assert_modem_signals(struct jsm_channel *ch)
+{
+	if (!ch)
+		return;
+
+	writeb(ch->ch_mostat, &ch->ch_cls_uart->mcr);
+}
+
+static void cls_copy_data_from_uart_to_queue(struct jsm_channel *ch)
+{
+	int qleft = 0;
+	u8 linestatus = 0;
+	u8 error_mask = 0;
+	u16 head;
+	u16 tail;
+	unsigned long flags;
+
+	if (!ch)
+		return;
+
+	spin_lock_irqsave(&ch->ch_lock, flags);
+
+	/* cache head and tail of queue */
+	head = ch->ch_r_head & RQUEUEMASK;
+	tail = ch->ch_r_tail & RQUEUEMASK;
+
+	/* Get our cached LSR */
+	linestatus = ch->ch_cached_lsr;
+	ch->ch_cached_lsr = 0;
+
+	/* Store how much space we have left in the queue */
+	qleft = tail - head - 1;
+	if (qleft < 0)
+		qleft += RQUEUEMASK + 1;
+
+	/*
+	 * Create a mask to determine whether we should
+	 * insert the character (if any) into our queue.
+	 */
+	if (ch->ch_c_iflag & IGNBRK)
+		error_mask |= UART_LSR_BI;
+
+	while (1) {
+		/*
+		 * Grab the linestatus register, we need to
+		 * check to see if there is any data to read
+		 */
+		linestatus = readb(&ch->ch_cls_uart->lsr);
+
+		/* Break out if there is no data to fetch */
+		if (!(linestatus & UART_LSR_DR))
+			break;
+
+		/*
+		 * Discard character if we are ignoring the error mask
+		 * which in this case is the break signal.
+		 */
+		if (linestatus & error_mask)  {
+			u8 discard;
+
+			linestatus = 0;
+			discard = readb(&ch->ch_cls_uart->txrx);
+			continue;
+		}
+
+		/*
+		 * If our queue is full, we have no choice but to drop some
+		 * data. The assumption is that HWFLOW or SWFLOW should have
+		 * stopped things way way before we got to this point.
+		 *
+		 * I decided that I wanted to ditch the oldest data first,
+		 * I hope thats okay with everyone? Yes? Good.
+		 */
+		while (qleft < 1) {
+			tail = (tail + 1) & RQUEUEMASK;
+			ch->ch_r_tail = tail;
+			ch->ch_err_overrun++;
+			qleft++;
+		}
+
+		ch->ch_equeue[head] = linestatus & (UART_LSR_BI | UART_LSR_PE
+								 | UART_LSR_FE);
+		ch->ch_rqueue[head] = readb(&ch->ch_cls_uart->txrx);
+
+		qleft--;
+
+		if (ch->ch_equeue[head] & UART_LSR_PE)
+			ch->ch_err_parity++;
+		if (ch->ch_equeue[head] & UART_LSR_BI)
+			ch->ch_err_break++;
+		if (ch->ch_equeue[head] & UART_LSR_FE)
+			ch->ch_err_frame++;
+
+		/* Add to, and flip head if needed */
+		head = (head + 1) & RQUEUEMASK;
+		ch->ch_rxcount++;
+	}
+
+	/*
+	 * Write new final heads to channel structure.
+	 */
+	ch->ch_r_head = head & RQUEUEMASK;
+	ch->ch_e_head = head & EQUEUEMASK;
+
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
+}
+
+static void cls_copy_data_from_queue_to_uart(struct jsm_channel *ch)
+{
+	u16 tail;
+	int n;
+	int qlen;
+	u32 len_written = 0;
+	struct circ_buf *circ;
+
+	if (!ch)
+		return;
+
+	circ = &ch->uart_port.state->xmit;
+
+	/* No data to write to the UART */
+	if (uart_circ_empty(circ))
+		return;
+
+	/* If port is "stopped", don't send any data to the UART */
+	if ((ch->ch_flags & CH_STOP) || (ch->ch_flags & CH_BREAK_SENDING))
+		return;
+
+	/* We have to do it this way, because of the EXAR TXFIFO count bug. */
+	if (!(ch->ch_flags & (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM)))
+		return;
+
+	n = 32;
+
+	/* cache tail of queue */
+	tail = circ->tail & (UART_XMIT_SIZE - 1);
+	qlen = uart_circ_chars_pending(circ);
+
+	/* Find minimum of the FIFO space, versus queue length */
+	n = min(n, qlen);
+
+	while (n > 0) {
+		writeb(circ->buf[tail], &ch->ch_cls_uart->txrx);
+		tail = (tail + 1) & (UART_XMIT_SIZE - 1);
+		n--;
+		ch->ch_txcount++;
+		len_written++;
+	}
+
+	/* Update the final tail */
+	circ->tail = tail & (UART_XMIT_SIZE - 1);
+
+	if (len_written > ch->ch_t_tlevel)
+		ch->ch_flags &= ~(CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
+
+	if (uart_circ_empty(circ))
+		uart_write_wakeup(&ch->uart_port);
+}
+
+static void cls_parse_modem(struct jsm_channel *ch, u8 signals)
+{
+	u8 msignals = signals;
+
+	jsm_dbg(MSIGS, &ch->ch_bd->pci_dev,
+		"neo_parse_modem: port: %d msignals: %x\n",
+		ch->ch_portnum, msignals);
+
+	/*
+	 * Scrub off lower bits.
+	 * They signify delta's, which I don't care about
+	 * Keep DDCD and DDSR though
+	 */
+	msignals &= 0xf8;
+
+	if (msignals & UART_MSR_DDCD)
+		uart_handle_dcd_change(&ch->uart_port, msignals & UART_MSR_DCD);
+	if (msignals & UART_MSR_DDSR)
+		uart_handle_dcd_change(&ch->uart_port, msignals & UART_MSR_CTS);
+
+	if (msignals & UART_MSR_DCD)
+		ch->ch_mistat |= UART_MSR_DCD;
+	else
+		ch->ch_mistat &= ~UART_MSR_DCD;
+
+	if (msignals & UART_MSR_DSR)
+		ch->ch_mistat |= UART_MSR_DSR;
+	else
+		ch->ch_mistat &= ~UART_MSR_DSR;
+
+	if (msignals & UART_MSR_RI)
+		ch->ch_mistat |= UART_MSR_RI;
+	else
+		ch->ch_mistat &= ~UART_MSR_RI;
+
+	if (msignals & UART_MSR_CTS)
+		ch->ch_mistat |= UART_MSR_CTS;
+	else
+		ch->ch_mistat &= ~UART_MSR_CTS;
+
+	jsm_dbg(MSIGS, &ch->ch_bd->pci_dev,
+		"Port: %d DTR: %d RTS: %d CTS: %d DSR: %d " "RI: %d CD: %d\n",
+		ch->ch_portnum,
+		!!((ch->ch_mistat | ch->ch_mostat) & UART_MCR_DTR),
+		!!((ch->ch_mistat | ch->ch_mostat) & UART_MCR_RTS),
+		!!((ch->ch_mistat | ch->ch_mostat) & UART_MSR_CTS),
+		!!((ch->ch_mistat | ch->ch_mostat) & UART_MSR_DSR),
+		!!((ch->ch_mistat | ch->ch_mostat) & UART_MSR_RI),
+		!!((ch->ch_mistat | ch->ch_mostat) & UART_MSR_DCD));
+}
+
+/* Parse the ISR register for the specific port */
+static inline void cls_parse_isr(struct jsm_board *brd, uint port)
+{
+	struct jsm_channel *ch;
+	u8 isr = 0;
+	unsigned long flags;
+
+	/*
+	 * No need to verify board pointer, it was already
+	 * verified in the interrupt routine.
+	 */
+
+	if (port > brd->nasync)
+		return;
+
+	ch = brd->channels[port];
+	if (!ch)
+		return;
+
+	/* Here we try to figure out what caused the interrupt to happen */
+	while (1) {
+		isr = readb(&ch->ch_cls_uart->isr_fcr);
+
+		/* Bail if no pending interrupt on port */
+		if (isr & UART_IIR_NO_INT)
+			break;
+
+		/* Receive Interrupt pending */
+		if (isr & (UART_IIR_RDI | UART_IIR_RDI_TIMEOUT)) {
+			/* Read data from uart -> queue */
+			cls_copy_data_from_uart_to_queue(ch);
+			jsm_check_queue_flow_control(ch);
+		}
+
+		/* Transmit Hold register empty pending */
+		if (isr & UART_IIR_THRI) {
+			/* Transfer data (if any) from Write Queue -> UART. */
+			spin_lock_irqsave(&ch->ch_lock, flags);
+			ch->ch_flags |= (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
+			spin_unlock_irqrestore(&ch->ch_lock, flags);
+			cls_copy_data_from_queue_to_uart(ch);
+		}
+
+		/*
+		 * CTS/RTS change of state:
+		 * Don't need to do anything, the cls_parse_modem
+		 * below will grab the updated modem signals.
+		 */
+
+		/* Parse any modem signal changes */
+		cls_parse_modem(ch, readb(&ch->ch_cls_uart->msr));
+	}
+}
+
+/* Channel lock MUST be held before calling this function! */
+static void cls_flush_uart_write(struct jsm_channel *ch)
+{
+	u8 tmp = 0;
+	u8 i = 0;
+
+	if (!ch)
+		return;
+
+	writeb((UART_FCR_ENABLE_FIFO | UART_FCR_CLEAR_XMIT),
+						&ch->ch_cls_uart->isr_fcr);
+
+	for (i = 0; i < 10; i++) {
+		/* Check to see if the UART feels it completely flushed FIFO */
+		tmp = readb(&ch->ch_cls_uart->isr_fcr);
+		if (tmp & UART_FCR_CLEAR_XMIT) {
+			jsm_dbg(IOCTL, &ch->ch_bd->pci_dev,
+				"Still flushing TX UART... i: %d\n", i);
+			udelay(10);
+		} else
+			break;
+	}
+
+	ch->ch_flags |= (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
+}
+
+/* Channel lock MUST be held before calling this function! */
+static void cls_flush_uart_read(struct jsm_channel *ch)
+{
+	if (!ch)
+		return;
+
+	/*
+	 * For complete POSIX compatibility, we should be purging the
+	 * read FIFO in the UART here.
+	 *
+	 * However, clearing the read FIFO (UART_FCR_CLEAR_RCVR) also
+	 * incorrectly flushes write data as well as just basically trashing the
+	 * FIFO.
+	 *
+	 * Presumably, this is a bug in this UART.
+	 */
+
+	udelay(10);
+}
+
+static void cls_send_start_character(struct jsm_channel *ch)
+{
+	if (!ch)
+		return;
+
+	if (ch->ch_startc != __DISABLED_CHAR) {
+		ch->ch_xon_sends++;
+		writeb(ch->ch_startc, &ch->ch_cls_uart->txrx);
+	}
+}
+
+static void cls_send_stop_character(struct jsm_channel *ch)
+{
+	if (!ch)
+		return;
+
+	if (ch->ch_stopc != __DISABLED_CHAR) {
+		ch->ch_xoff_sends++;
+		writeb(ch->ch_stopc, &ch->ch_cls_uart->txrx);
+	}
+}
+
+/*
+ * cls_param()
+ * Send any/all changes to the line to the UART.
+ */
+static void cls_param(struct jsm_channel *ch)
+{
+	u8 lcr = 0;
+	u8 uart_lcr = 0;
+	u8 ier = 0;
+	u32 baud = 9600;
+	int quot = 0;
+	struct jsm_board *bd;
+	int i;
+	unsigned int cflag;
+
+	bd = ch->ch_bd;
+	if (!bd)
+		return;
+
+	/*
+	 * If baud rate is zero, flush queues, and set mval to drop DTR.
+	 */
+	if ((ch->ch_c_cflag & (CBAUD)) == 0) {
+		ch->ch_r_head = 0;
+		ch->ch_r_tail = 0;
+		ch->ch_e_head = 0;
+		ch->ch_e_tail = 0;
+
+		cls_flush_uart_write(ch);
+		cls_flush_uart_read(ch);
+
+		/* The baudrate is B0 so all modem lines are to be dropped. */
+		ch->ch_flags |= (CH_BAUD0);
+		ch->ch_mostat &= ~(UART_MCR_RTS | UART_MCR_DTR);
+		cls_assert_modem_signals(ch);
+		return;
+	}
+
+	cflag = C_BAUD(ch->uart_port.state->port.tty);
+	baud = 9600;
+	for (i = 0; i < ARRAY_SIZE(baud_rates); i++) {
+		if (baud_rates[i].cflag == cflag) {
+			baud = baud_rates[i].rate;
+			break;
+		}
+	}
+
+	if (ch->ch_flags & CH_BAUD0)
+		ch->ch_flags &= ~(CH_BAUD0);
+
+	if (ch->ch_c_cflag & PARENB)
+		lcr |= UART_LCR_PARITY;
+
+	if (!(ch->ch_c_cflag & PARODD))
+		lcr |= UART_LCR_EPAR;
+
+	/*
+	 * Not all platforms support mark/space parity,
+	 * so this will hide behind an ifdef.
+	 */
+#ifdef CMSPAR
+	if (ch->ch_c_cflag & CMSPAR)
+		lcr |= UART_LCR_SPAR;
+#endif
+
+	if (ch->ch_c_cflag & CSTOPB)
+		lcr |= UART_LCR_STOP;
+
+	switch (ch->ch_c_cflag & CSIZE) {
+	case CS5:
+		lcr |= UART_LCR_WLEN5;
+		break;
+	case CS6:
+		lcr |= UART_LCR_WLEN6;
+		break;
+	case CS7:
+		lcr |= UART_LCR_WLEN7;
+		break;
+	case CS8:
+	default:
+		lcr |= UART_LCR_WLEN8;
+		break;
+	}
+
+	ier = readb(&ch->ch_cls_uart->ier);
+	uart_lcr = readb(&ch->ch_cls_uart->lcr);
+
+	quot = ch->ch_bd->bd_dividend / baud;
+
+	if (quot != 0) {
+		writeb(UART_LCR_DLAB, &ch->ch_cls_uart->lcr);
+		writeb((quot & 0xff), &ch->ch_cls_uart->txrx);
+		writeb((quot >> 8), &ch->ch_cls_uart->ier);
+		writeb(lcr, &ch->ch_cls_uart->lcr);
+	}
+
+	if (uart_lcr != lcr)
+		writeb(lcr, &ch->ch_cls_uart->lcr);
+
+	if (ch->ch_c_cflag & CREAD)
+		ier |= (UART_IER_RDI | UART_IER_RLSI);
+
+	ier |= (UART_IER_THRI | UART_IER_MSI);
+
+	writeb(ier, &ch->ch_cls_uart->ier);
+
+	if (ch->ch_c_cflag & CRTSCTS)
+		cls_set_cts_flow_control(ch);
+	else if (ch->ch_c_iflag & IXON) {
+		/*
+		 * If start/stop is set to disable,
+		 * then we should disable flow control.
+		 */
+		if ((ch->ch_startc == __DISABLED_CHAR) ||
+			(ch->ch_stopc == __DISABLED_CHAR))
+			cls_set_no_output_flow_control(ch);
+		else
+			cls_set_ixon_flow_control(ch);
+	} else
+		cls_set_no_output_flow_control(ch);
+
+	if (ch->ch_c_cflag & CRTSCTS)
+		cls_set_rts_flow_control(ch);
+	else if (ch->ch_c_iflag & IXOFF) {
+		/*
+		 * If start/stop is set to disable,
+		 * then we should disable flow control.
+		 */
+		if ((ch->ch_startc == __DISABLED_CHAR) ||
+			(ch->ch_stopc == __DISABLED_CHAR))
+			cls_set_no_input_flow_control(ch);
+		else
+			cls_set_ixoff_flow_control(ch);
+	} else
+		cls_set_no_input_flow_control(ch);
+
+	cls_assert_modem_signals(ch);
+
+	/* get current status of the modem signals now */
+	cls_parse_modem(ch, readb(&ch->ch_cls_uart->msr));
+}
+
+/*
+ * cls_intr()
+ *
+ * Classic specific interrupt handler.
+ */
+static irqreturn_t cls_intr(int irq, void *voidbrd)
+{
+	struct jsm_board *brd = voidbrd;
+	unsigned long lock_flags;
+	unsigned char uart_poll;
+	uint i = 0;
+
+	/* Lock out the slow poller from running on this board. */
+	spin_lock_irqsave(&brd->bd_intr_lock, lock_flags);
+
+	/*
+	 * Check the board's global interrupt offset to see if we
+	 * acctually do have an interrupt pending on us.
+	 */
+	uart_poll = readb(brd->re_map_membase + UART_CLASSIC_POLL_ADDR_OFFSET);
+
+	jsm_dbg(INTR, &brd->pci_dev, "%s:%d uart_poll: %x\n",
+		__FILE__, __LINE__, uart_poll);
+
+	if (!uart_poll) {
+		jsm_dbg(INTR, &brd->pci_dev,
+			"Kernel interrupted to me, but no pending interrupts...\n");
+		spin_unlock_irqrestore(&brd->bd_intr_lock, lock_flags);
+		return IRQ_NONE;
+	}
+
+	/* At this point, we have at least SOMETHING to service, dig further. */
+
+	/* Parse each port to find out what caused the interrupt */
+	for (i = 0; i < brd->nasync; i++)
+		cls_parse_isr(brd, i);
+
+	spin_unlock_irqrestore(&brd->bd_intr_lock, lock_flags);
+
+	return IRQ_HANDLED;
+}
+
+/* Inits UART */
+static void cls_uart_init(struct jsm_channel *ch)
+{
+	unsigned char lcrb = readb(&ch->ch_cls_uart->lcr);
+	unsigned char isr_fcr = 0;
+
+	writeb(0, &ch->ch_cls_uart->ier);
+
+	/*
+	 * The Enhanced Register Set may only be accessed when
+	 * the Line Control Register is set to 0xBFh.
+	 */
+	writeb(UART_EXAR654_ENHANCED_REGISTER_SET, &ch->ch_cls_uart->lcr);
+
+	isr_fcr = readb(&ch->ch_cls_uart->isr_fcr);
+
+	/* Turn on Enhanced/Extended controls */
+	isr_fcr |= (UART_EXAR654_EFR_ECB);
+
+	writeb(isr_fcr, &ch->ch_cls_uart->isr_fcr);
+
+	/* Write old LCR value back out, which turns enhanced access off */
+	writeb(lcrb, &ch->ch_cls_uart->lcr);
+
+	/* Clear out UART and FIFO */
+	readb(&ch->ch_cls_uart->txrx);
+
+	writeb((UART_FCR_ENABLE_FIFO|UART_FCR_CLEAR_RCVR|UART_FCR_CLEAR_XMIT),
+						 &ch->ch_cls_uart->isr_fcr);
+	udelay(10);
+
+	ch->ch_flags |= (CH_FIFO_ENABLED | CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
+
+	readb(&ch->ch_cls_uart->lsr);
+	readb(&ch->ch_cls_uart->msr);
+}
+
+/*
+ * Turns off UART.
+ */
+static void cls_uart_off(struct jsm_channel *ch)
+{
+	/* Stop all interrupts from accurring. */
+	writeb(0, &ch->ch_cls_uart->ier);
+}
+
+/*
+ * cls_get_uarts_bytes_left.
+ * Returns 0 is nothing left in the FIFO, returns 1 otherwise.
+ *
+ * The channel lock MUST be held by the calling function.
+ */
+static u32 cls_get_uart_bytes_left(struct jsm_channel *ch)
+{
+	u8 left = 0;
+	u8 lsr = readb(&ch->ch_cls_uart->lsr);
+
+	/* Determine whether the Transmitter is empty or not */
+	if (!(lsr & UART_LSR_TEMT))
+		left = 1;
+	else {
+		ch->ch_flags |= (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
+		left = 0;
+	}
+
+	return left;
+}
+
+/*
+ * cls_send_break.
+ * Starts sending a break thru the UART.
+ *
+ * The channel lock MUST be held by the calling function.
+ */
+static void cls_send_break(struct jsm_channel *ch)
+{
+	/* Tell the UART to start sending the break */
+	if (!(ch->ch_flags & CH_BREAK_SENDING)) {
+		u8 temp = readb(&ch->ch_cls_uart->lcr);
+
+		writeb((temp | UART_LCR_SBC), &ch->ch_cls_uart->lcr);
+		ch->ch_flags |= (CH_BREAK_SENDING);
+	}
+}
+
+/*
+ * cls_send_immediate_char.
+ * Sends a specific character as soon as possible to the UART,
+ * jumping over any bytes that might be in the write queue.
+ *
+ * The channel lock MUST be held by the calling function.
+ */
+static void cls_send_immediate_char(struct jsm_channel *ch, unsigned char c)
+{
+	writeb(c, &ch->ch_cls_uart->txrx);
+}
+
+struct board_ops jsm_cls_ops = {
+	.intr =				cls_intr,
+	.uart_init =			cls_uart_init,
+	.uart_off =			cls_uart_off,
+	.param =			cls_param,
+	.assert_modem_signals =		cls_assert_modem_signals,
+	.flush_uart_write =		cls_flush_uart_write,
+	.flush_uart_read =		cls_flush_uart_read,
+	.disable_receiver =		cls_disable_receiver,
+	.enable_receiver =		cls_enable_receiver,
+	.send_break =			cls_send_break,
+	.clear_break =			cls_clear_break,
+	.send_start_character =		cls_send_start_character,
+	.send_stop_character =		cls_send_stop_character,
+	.copy_data_from_queue_to_uart = cls_copy_data_from_queue_to_uart,
+	.get_uart_bytes_left =		cls_get_uart_bytes_left,
+	.send_immediate_char =		cls_send_immediate_char
+};
+
diff --git a/drivers/tty/serial/jsm/jsm_driver.c b/drivers/tty/serial/jsm/jsm_driver.c
index d2885a7bb090..efbd87a76656 100644
--- a/drivers/tty/serial/jsm/jsm_driver.c
+++ b/drivers/tty/serial/jsm/jsm_driver.c
@@ -13,11 +13,6 @@
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU 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.
- *
  * Contact Information:
  * Scott H Kilau <Scott_Kilau@digi.com>
  * Wendy Xiong   <wendyx@us.ibm.com>
@@ -31,8 +26,7 @@
 #include "jsm.h"
 
 MODULE_AUTHOR("Digi International, http://www.digi.com");
-MODULE_DESCRIPTION("Driver for the Digi International "
-		   "Neo PCI based product line");
+MODULE_DESCRIPTION("Driver for the Digi International Neo and Classic PCI based product line");
 MODULE_LICENSE("GPL");
 MODULE_SUPPORTED_DEVICE("jsm");
 
@@ -50,7 +44,7 @@ struct uart_driver jsm_uart_driver = {
 };
 
 static pci_ers_result_t jsm_io_error_detected(struct pci_dev *pdev,
-                                       pci_channel_state_t state);
+					pci_channel_state_t state);
 static pci_ers_result_t jsm_io_slot_reset(struct pci_dev *pdev);
 static void jsm_io_resume(struct pci_dev *pdev);
 
@@ -68,7 +62,7 @@ static int jsm_probe_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
 	int rc = 0;
 	struct jsm_board *brd;
-	static int adapter_count = 0;
+	static int adapter_count;
 
 	rc = pci_enable_device(pdev);
 	if (rc) {
@@ -82,10 +76,8 @@ static int jsm_probe_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto out_disable_device;
 	}
 
-	brd = kzalloc(sizeof(struct jsm_board), GFP_KERNEL);
+	brd = kzalloc(sizeof(*brd), GFP_KERNEL);
 	if (!brd) {
-		dev_err(&pdev->dev,
-			"memory allocation for board structure failed\n");
 		rc = -ENOMEM;
 		goto out_release_regions;
 	}
@@ -95,7 +87,6 @@ static int jsm_probe_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	brd->pci_dev = pdev;
 
 	switch (pdev->device) {
-
 	case PCI_DEVICE_ID_NEO_2DB9:
 	case PCI_DEVICE_ID_NEO_2DB9PRI:
 	case PCI_DEVICE_ID_NEO_2RJ45:
@@ -104,6 +95,8 @@ static int jsm_probe_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		brd->maxports = 2;
 		break;
 
+	case PCI_DEVICE_ID_CLASSIC_4:
+	case PCI_DEVICE_ID_CLASSIC_4_422:
 	case PCI_DEVICE_ID_NEO_4:
 	case PCIE_DEVICE_ID_NEO_4:
 	case PCIE_DEVICE_ID_NEO_4RJ45:
@@ -111,6 +104,8 @@ static int jsm_probe_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		brd->maxports = 4;
 		break;
 
+	case PCI_DEVICE_ID_CLASSIC_8:
+	case PCI_DEVICE_ID_CLASSIC_8_422:
 	case PCI_DEVICE_ID_DIGI_NEO_8:
 	case PCIE_DEVICE_ID_NEO_8:
 	case PCIE_DEVICE_ID_NEO_8RJ45:
@@ -129,36 +124,109 @@ static int jsm_probe_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	brd->irq = pdev->irq;
 
-	jsm_dbg(INIT, &brd->pci_dev, "jsm_found_board - NEO adapter\n");
+	switch (pdev->device) {
+	case PCI_DEVICE_ID_CLASSIC_4:
+	case PCI_DEVICE_ID_CLASSIC_4_422:
+	case PCI_DEVICE_ID_CLASSIC_8:
+	case PCI_DEVICE_ID_CLASSIC_8_422:
+
+		jsm_dbg(INIT, &brd->pci_dev,
+			"jsm_found_board - Classic adapter\n");
+
+		/*
+		 * For PCI ClassicBoards
+		 * PCI Local Address (.i.e. "resource" number) space
+		 * 0	PLX Memory Mapped Config
+		 * 1	PLX I/O Mapped Config
+		 * 2	I/O Mapped UARTs and Status
+		 * 3	Memory Mapped VPD
+		 * 4	Memory Mapped UARTs and Status
+		 */
+
+		/* Get the PCI Base Address Registers */
+		brd->membase = pci_resource_start(pdev, 4);
+		brd->membase_end = pci_resource_end(pdev, 4);
+
+		if (brd->membase & 0x1)
+			brd->membase &= ~0x3;
+		else
+			brd->membase &= ~0xF;
+
+		brd->iobase = pci_resource_start(pdev, 1);
+		brd->iobase_end = pci_resource_end(pdev, 1);
+		brd->iobase = ((unsigned int)(brd->iobase)) & 0xFFFE;
+
+		/* Assign the board_ops struct */
+		brd->bd_ops = &jsm_cls_ops;
+
+		brd->bd_uart_offset = 0x8;
+		brd->bd_dividend = 921600;
+
+		brd->re_map_membase = ioremap(brd->membase,
+						pci_resource_len(pdev, 4));
+		if (!brd->re_map_membase) {
+			dev_err(&pdev->dev,
+				"Card has no PCI Memory resources, failing board.\n");
+			rc = -ENOMEM;
+			goto out_kfree_brd;
+		}
+
+		/*
+		 * Enable Local Interrupt 1			(0x1),
+		 * Local Interrupt 1 Polarity Active high	(0x2),
+		 * Enable PCI interrupt				(0x43)
+		 */
+		outb(0x43, brd->iobase + 0x4c);
+
+		break;
+
+	case PCI_DEVICE_ID_NEO_2DB9:
+	case PCI_DEVICE_ID_NEO_2DB9PRI:
+	case PCI_DEVICE_ID_NEO_2RJ45:
+	case PCI_DEVICE_ID_NEO_2RJ45PRI:
+	case PCI_DEVICE_ID_NEO_2_422_485:
+	case PCI_DEVICE_ID_NEO_4:
+	case PCIE_DEVICE_ID_NEO_4:
+	case PCIE_DEVICE_ID_NEO_4RJ45:
+	case PCIE_DEVICE_ID_NEO_4_IBM:
+	case PCI_DEVICE_ID_DIGI_NEO_8:
+	case PCIE_DEVICE_ID_NEO_8:
+	case PCIE_DEVICE_ID_NEO_8RJ45:
+
+		jsm_dbg(INIT, &brd->pci_dev, "jsm_found_board - NEO adapter\n");
 
-	/* get the PCI Base Address Registers */
-	brd->membase	= pci_resource_start(pdev, 0);
-	brd->membase_end = pci_resource_end(pdev, 0);
+		/* get the PCI Base Address Registers */
+		brd->membase	= pci_resource_start(pdev, 0);
+		brd->membase_end = pci_resource_end(pdev, 0);
 
-	if (brd->membase & 1)
-		brd->membase &= ~3;
-	else
-		brd->membase &= ~15;
+		if (brd->membase & 1)
+			brd->membase &= ~0x3;
+		else
+			brd->membase &= ~0xF;
 
-	/* Assign the board_ops struct */
-	brd->bd_ops = &jsm_neo_ops;
+		/* Assign the board_ops struct */
+		brd->bd_ops = &jsm_neo_ops;
 
-	brd->bd_uart_offset = 0x200;
-	brd->bd_dividend = 921600;
+		brd->bd_uart_offset = 0x200;
+		brd->bd_dividend = 921600;
 
-	brd->re_map_membase = ioremap(brd->membase, pci_resource_len(pdev, 0));
-	if (!brd->re_map_membase) {
-		dev_err(&pdev->dev,
-			"card has no PCI Memory resources, "
-			"failing board.\n");
-		rc = -ENOMEM;
-		goto out_kfree_brd;
+		brd->re_map_membase = ioremap(brd->membase,
+						pci_resource_len(pdev, 0));
+		if (!brd->re_map_membase) {
+			dev_err(&pdev->dev,
+				"Card has no PCI Memory resources, failing board.\n");
+			rc = -ENOMEM;
+			goto out_kfree_brd;
+		}
+
+		break;
+	default:
+		return -ENXIO;
 	}
 
-	rc = request_irq(brd->irq, brd->bd_ops->intr,
-			IRQF_SHARED, "JSM", brd);
+	rc = request_irq(brd->irq, brd->bd_ops->intr, IRQF_SHARED, "JSM", brd);
 	if (rc) {
-		printk(KERN_WARNING "Failed to hook IRQ %d\n",brd->irq);
+		dev_warn(&pdev->dev, "Failed to hook IRQ %d\n", brd->irq);
 		goto out_iounmap;
 	}
 
@@ -178,7 +246,7 @@ static int jsm_probe_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	}
 
 	/* Log the information about the board */
-	dev_info(&pdev->dev, "board %d: Digi Neo (rev %d), irq %d\n",
+	dev_info(&pdev->dev, "board %d: Digi Classic/Neo (rev %d), irq %d\n",
 			adapter_count, brd->rev, brd->irq);
 
 	pci_set_drvdata(pdev, brd);
@@ -205,6 +273,18 @@ static void jsm_remove_one(struct pci_dev *pdev)
 	struct jsm_board *brd = pci_get_drvdata(pdev);
 	int i = 0;
 
+	switch (pdev->device) {
+	case PCI_DEVICE_ID_CLASSIC_4:
+	case PCI_DEVICE_ID_CLASSIC_4_422:
+	case PCI_DEVICE_ID_CLASSIC_8:
+	case PCI_DEVICE_ID_CLASSIC_8_422:
+		/* Tell card not to interrupt anymore. */
+		outb(0x0, brd->iobase + 0x4c);
+		break;
+	default:
+		break;
+	}
+
 	jsm_remove_uart_port(brd);
 
 	free_irq(brd->irq, brd);
@@ -239,6 +319,10 @@ static struct pci_device_id jsm_pci_tbl[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_DIGI, PCIE_DEVICE_ID_NEO_4), 0, 0, 11 },
 	{ PCI_DEVICE(PCI_VENDOR_ID_DIGI, PCIE_DEVICE_ID_NEO_4RJ45), 0, 0, 12 },
 	{ PCI_DEVICE(PCI_VENDOR_ID_DIGI, PCIE_DEVICE_ID_NEO_8RJ45), 0, 0, 13 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_DIGI, PCI_DEVICE_ID_CLASSIC_4), 0, 0, 14 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_DIGI, PCI_DEVICE_ID_CLASSIC_4_422), 0, 0, 15 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_DIGI, PCI_DEVICE_ID_CLASSIC_8), 0, 0, 16 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_DIGI, PCI_DEVICE_ID_CLASSIC_8_422), 0, 0, 17 },
 	{ 0, }
 };
 MODULE_DEVICE_TABLE(pci, jsm_pci_tbl);
diff --git a/drivers/tty/serial/jsm/jsm_neo.c b/drivers/tty/serial/jsm/jsm_neo.c
index dfaf48826417..7291c2117daa 100644
--- a/drivers/tty/serial/jsm/jsm_neo.c
+++ b/drivers/tty/serial/jsm/jsm_neo.c
@@ -13,11 +13,6 @@
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU 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.
- *
  * Contact Information:
  * Scott H Kilau <Scott_Kilau@digi.com>
  * Wendy Xiong   <wendyx@us.ibm.com>
@@ -649,7 +644,7 @@ static void neo_flush_uart_write(struct jsm_channel *ch)
 
 		/* Check to see if the UART feels it completely flushed the FIFO. */
 		tmp = readb(&ch->ch_neo_uart->isr_fcr);
-		if (tmp & 4) {
+		if (tmp & UART_FCR_CLEAR_XMIT) {
 			jsm_dbg(IOCTL, &ch->ch_bd->pci_dev,
 				"Still flushing TX UART... i: %d\n", i);
 			udelay(10);
@@ -694,7 +689,7 @@ static void neo_flush_uart_read(struct jsm_channel *ch)
 /*
  * No locks are assumed to be held when calling this function.
  */
-static void neo_clear_break(struct jsm_channel *ch, int force)
+static void neo_clear_break(struct jsm_channel *ch)
 {
 	unsigned long lock_flags;
 
@@ -1024,27 +1019,24 @@ static void neo_param(struct jsm_channel *ch)
 		lcr |= UART_LCR_STOP;
 
 	switch (ch->ch_c_cflag & CSIZE) {
-		case CS5:
-			lcr |= UART_LCR_WLEN5;
-			break;
-		case CS6:
-			lcr |= UART_LCR_WLEN6;
-			break;
-		case CS7:
-			lcr |= UART_LCR_WLEN7;
-			break;
-		case CS8:
-		default:
-			lcr |= UART_LCR_WLEN8;
+	case CS5:
+		lcr |= UART_LCR_WLEN5;
 		break;
+	case CS6:
+		lcr |= UART_LCR_WLEN6;
+		break;
+	case CS7:
+		lcr |= UART_LCR_WLEN7;
+		break;
+	case CS8:
+	default:
+		lcr |= UART_LCR_WLEN8;
+	break;
 	}
 
 	ier = readb(&ch->ch_neo_uart->ier);
 	uart_lcr = readb(&ch->ch_neo_uart->lcr);
 
-	if (baud == 0)
-		baud = 9600;
-
 	quot = ch->ch_bd->bd_dividend / baud;
 
 	if (quot != 0) {
diff --git a/drivers/tty/serial/jsm/jsm_tty.c b/drivers/tty/serial/jsm/jsm_tty.c
index 3e5c1563afe2..524e86ab3cae 100644
--- a/drivers/tty/serial/jsm/jsm_tty.c
+++ b/drivers/tty/serial/jsm/jsm_tty.c
@@ -13,11 +13,6 @@
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU 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.
- *
  * Contact Information:
  * Scott H Kilau <Scott_Kilau@digi.com>
  * Ananda Venkatarman <mansarov@us.ibm.com>
@@ -77,7 +72,8 @@ static unsigned int jsm_tty_tx_empty(struct uart_port *port)
 static unsigned int jsm_tty_get_mctrl(struct uart_port *port)
 {
 	int result;
-	struct jsm_channel *channel = (struct jsm_channel *)port;
+	struct jsm_channel *channel =
+		container_of(port, struct jsm_channel, uart_port);
 
 	jsm_dbg(IOCTL, &channel->ch_bd->pci_dev, "start\n");
 
@@ -98,7 +94,8 @@ static unsigned int jsm_tty_get_mctrl(struct uart_port *port)
  */
 static void jsm_tty_set_mctrl(struct uart_port *port, unsigned int mctrl)
 {
-	struct jsm_channel *channel = (struct jsm_channel *)port;
+	struct jsm_channel *channel =
+		container_of(port, struct jsm_channel, uart_port);
 
 	jsm_dbg(IOCTL, &channel->ch_bd->pci_dev, "start\n");
 
@@ -133,7 +130,8 @@ static void jsm_tty_write(struct uart_port *port)
 
 static void jsm_tty_start_tx(struct uart_port *port)
 {
-	struct jsm_channel *channel = (struct jsm_channel *)port;
+	struct jsm_channel *channel =
+		container_of(port, struct jsm_channel, uart_port);
 
 	jsm_dbg(IOCTL, &channel->ch_bd->pci_dev, "start\n");
 
@@ -145,7 +143,8 @@ static void jsm_tty_start_tx(struct uart_port *port)
 
 static void jsm_tty_stop_tx(struct uart_port *port)
 {
-	struct jsm_channel *channel = (struct jsm_channel *)port;
+	struct jsm_channel *channel =
+		container_of(port, struct jsm_channel, uart_port);
 
 	jsm_dbg(IOCTL, &channel->ch_bd->pci_dev, "start\n");
 
@@ -157,7 +156,8 @@ static void jsm_tty_stop_tx(struct uart_port *port)
 static void jsm_tty_send_xchar(struct uart_port *port, char ch)
 {
 	unsigned long lock_flags;
-	struct jsm_channel *channel = (struct jsm_channel *)port;
+	struct jsm_channel *channel =
+		container_of(port, struct jsm_channel, uart_port);
 	struct ktermios *termios;
 
 	spin_lock_irqsave(&port->lock, lock_flags);
@@ -172,7 +172,8 @@ static void jsm_tty_send_xchar(struct uart_port *port, char ch)
 
 static void jsm_tty_stop_rx(struct uart_port *port)
 {
-	struct jsm_channel *channel = (struct jsm_channel *)port;
+	struct jsm_channel *channel =
+		container_of(port, struct jsm_channel, uart_port);
 
 	channel->ch_bd->bd_ops->disable_receiver(channel);
 }
@@ -180,13 +181,14 @@ static void jsm_tty_stop_rx(struct uart_port *port)
 static void jsm_tty_break(struct uart_port *port, int break_state)
 {
 	unsigned long lock_flags;
-	struct jsm_channel *channel = (struct jsm_channel *)port;
+	struct jsm_channel *channel =
+		container_of(port, struct jsm_channel, uart_port);
 
 	spin_lock_irqsave(&port->lock, lock_flags);
 	if (break_state == -1)
 		channel->ch_bd->bd_ops->send_break(channel);
 	else
-		channel->ch_bd->bd_ops->clear_break(channel, 0);
+		channel->ch_bd->bd_ops->clear_break(channel);
 
 	spin_unlock_irqrestore(&port->lock, lock_flags);
 }
@@ -194,7 +196,8 @@ static void jsm_tty_break(struct uart_port *port, int break_state)
 static int jsm_tty_open(struct uart_port *port)
 {
 	struct jsm_board *brd;
-	struct jsm_channel *channel = (struct jsm_channel *)port;
+	struct jsm_channel *channel =
+		container_of(port, struct jsm_channel, uart_port);
 	struct ktermios *termios;
 
 	/* Get board pointer from our array of majors we have allocated */
@@ -273,7 +276,8 @@ static void jsm_tty_close(struct uart_port *port)
 {
 	struct jsm_board *bd;
 	struct ktermios *ts;
-	struct jsm_channel *channel = (struct jsm_channel *)port;
+	struct jsm_channel *channel =
+		container_of(port, struct jsm_channel, uart_port);
 
 	jsm_dbg(CLOSE, &channel->ch_bd->pci_dev, "start\n");
 
@@ -307,7 +311,8 @@ static void jsm_tty_set_termios(struct uart_port *port,
 				 struct ktermios *old_termios)
 {
 	unsigned long lock_flags;
-	struct jsm_channel *channel = (struct jsm_channel *)port;
+	struct jsm_channel *channel =
+		container_of(port, struct jsm_channel, uart_port);
 
 	spin_lock_irqsave(&port->lock, lock_flags);
 	channel->ch_c_cflag	= termios->c_cflag;
@@ -415,6 +420,8 @@ int jsm_tty_init(struct jsm_board *brd)
 
 		if (brd->bd_uart_offset == 0x200)
 			ch->ch_neo_uart =  vaddr + (brd->bd_uart_offset * i);
+		else
+			ch->ch_cls_uart =  vaddr + (brd->bd_uart_offset * i);
 
 		ch->ch_bd = brd;
 		ch->ch_portnum = i;
diff --git a/drivers/tty/serial/lantiq.c b/drivers/tty/serial/lantiq.c
index 4675fe198d31..4ccc0397664c 100644
--- a/drivers/tty/serial/lantiq.c
+++ b/drivers/tty/serial/lantiq.c
@@ -497,8 +497,10 @@ lqasc_type(struct uart_port *port)
 static void
 lqasc_release_port(struct uart_port *port)
 {
+	struct platform_device *pdev = to_platform_device(port->dev);
+
 	if (port->flags & UPF_IOREMAP) {
-		iounmap(port->membase);
+		devm_iounmap(&pdev->dev, port->membase);
 		port->membase = NULL;
 	}
 }
@@ -743,7 +745,6 @@ 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/lpc32xx_hs.c b/drivers/tty/serial/lpc32xx_hs.c
index 6f0f89282847..e92d7ebe9e77 100644
--- a/drivers/tty/serial/lpc32xx_hs.c
+++ b/drivers/tty/serial/lpc32xx_hs.c
@@ -768,7 +768,6 @@ static struct platform_driver serial_hs_lpc32xx_driver = {
 	.resume		= serial_hs_lpc32xx_resume,
 	.driver		= {
 		.name	= MODNAME,
-		.owner	= THIS_MODULE,
 		.of_match_table	= serial_hs_lpc32xx_dt_ids,
 	},
 };
diff --git a/drivers/tty/serial/m32r_sio.c b/drivers/tty/serial/m32r_sio.c
index 5702828fb62e..8f7f83a14c93 100644
--- a/drivers/tty/serial/m32r_sio.c
+++ b/drivers/tty/serial/m32r_sio.c
@@ -249,7 +249,8 @@ static void serial_out(struct uart_sio_port *up, int offset, int value)
 
 static void m32r_sio_stop_tx(struct uart_port *port)
 {
-	struct uart_sio_port *up = (struct uart_sio_port *)port;
+	struct uart_sio_port *up =
+		container_of(port, struct uart_sio_port, port);
 
 	if (up->ier & UART_IER_THRI) {
 		up->ier &= ~UART_IER_THRI;
@@ -260,7 +261,8 @@ static void m32r_sio_stop_tx(struct uart_port *port)
 static void m32r_sio_start_tx(struct uart_port *port)
 {
 #ifdef CONFIG_SERIAL_M32R_PLDSIO
-	struct uart_sio_port *up = (struct uart_sio_port *)port;
+	struct uart_sio_port *up =
+		container_of(port, struct uart_sio_port, port);
 	struct circ_buf *xmit = &up->port.state->xmit;
 
 	if (!(up->ier & UART_IER_THRI)) {
@@ -274,7 +276,8 @@ static void m32r_sio_start_tx(struct uart_port *port)
 	}
 	while((serial_in(up, UART_LSR) & UART_EMPTY) != UART_EMPTY);
 #else
-	struct uart_sio_port *up = (struct uart_sio_port *)port;
+	struct uart_sio_port *up =
+		container_of(port, struct uart_sio_port, port);
 
 	if (!(up->ier & UART_IER_THRI)) {
 		up->ier |= UART_IER_THRI;
@@ -285,7 +288,8 @@ static void m32r_sio_start_tx(struct uart_port *port)
 
 static void m32r_sio_stop_rx(struct uart_port *port)
 {
-	struct uart_sio_port *up = (struct uart_sio_port *)port;
+	struct uart_sio_port *up =
+		container_of(port, struct uart_sio_port, port);
 
 	up->ier &= ~UART_IER_RLSI;
 	up->port.read_status_mask &= ~UART_LSR_DR;
@@ -294,7 +298,8 @@ static void m32r_sio_stop_rx(struct uart_port *port)
 
 static void m32r_sio_enable_ms(struct uart_port *port)
 {
-	struct uart_sio_port *up = (struct uart_sio_port *)port;
+	struct uart_sio_port *up =
+		container_of(port, struct uart_sio_port, port);
 
 	up->ier |= UART_IER_MSI;
 	serial_out(up, UART_IER, up->ier);
@@ -581,7 +586,8 @@ static void m32r_sio_timeout(unsigned long data)
 
 static unsigned int m32r_sio_tx_empty(struct uart_port *port)
 {
-	struct uart_sio_port *up = (struct uart_sio_port *)port;
+	struct uart_sio_port *up =
+		container_of(port, struct uart_sio_port, port);
 	unsigned long flags;
 	unsigned int ret;
 
@@ -609,7 +615,8 @@ static void m32r_sio_break_ctl(struct uart_port *port, int break_state)
 
 static int m32r_sio_startup(struct uart_port *port)
 {
-	struct uart_sio_port *up = (struct uart_sio_port *)port;
+	struct uart_sio_port *up =
+		container_of(port, struct uart_sio_port, port);
 	int retval;
 
 	sio_init();
@@ -652,7 +659,8 @@ static int m32r_sio_startup(struct uart_port *port)
 
 static void m32r_sio_shutdown(struct uart_port *port)
 {
-	struct uart_sio_port *up = (struct uart_sio_port *)port;
+	struct uart_sio_port *up =
+		container_of(port, struct uart_sio_port, port);
 
 	/*
 	 * Disable interrupts from this port
@@ -681,7 +689,8 @@ static unsigned int m32r_sio_get_divisor(struct uart_port *port,
 static void m32r_sio_set_termios(struct uart_port *port,
 	struct ktermios *termios, struct ktermios *old)
 {
-	struct uart_sio_port *up = (struct uart_sio_port *)port;
+	struct uart_sio_port *up =
+		container_of(port, struct uart_sio_port, port);
 	unsigned char cval = 0;
 	unsigned long flags;
 	unsigned int baud, quot;
@@ -780,7 +789,8 @@ static void m32r_sio_set_termios(struct uart_port *port,
 static void m32r_sio_pm(struct uart_port *port, unsigned int state,
 	unsigned int oldstate)
 {
-	struct uart_sio_port *up = (struct uart_sio_port *)port;
+	struct uart_sio_port *up =
+		container_of(port, struct uart_sio_port, port);
 
 	if (up->pm)
 		up->pm(port, state, oldstate);
@@ -825,7 +835,8 @@ m32r_sio_request_std_resource(struct uart_sio_port *up, struct resource **res)
 
 static void m32r_sio_release_port(struct uart_port *port)
 {
-	struct uart_sio_port *up = (struct uart_sio_port *)port;
+	struct uart_sio_port *up =
+		container_of(port, struct uart_sio_port, port);
 	unsigned long start, offset = 0, size = 0;
 
 	size <<= up->port.regshift;
@@ -862,7 +873,8 @@ static void m32r_sio_release_port(struct uart_port *port)
 
 static int m32r_sio_request_port(struct uart_port *port)
 {
-	struct uart_sio_port *up = (struct uart_sio_port *)port;
+	struct uart_sio_port *up =
+		container_of(port, struct uart_sio_port, port);
 	struct resource *res = NULL;
 	int ret = 0;
 
@@ -889,7 +901,8 @@ static int m32r_sio_request_port(struct uart_port *port)
 
 static void m32r_sio_config_port(struct uart_port *port, int unused)
 {
-	struct uart_sio_port *up = (struct uart_sio_port *)port;
+	struct uart_sio_port *up =
+		container_of(port, struct uart_sio_port, port);
 	unsigned long flags;
 
 	spin_lock_irqsave(&up->port.lock, flags);
@@ -1000,7 +1013,8 @@ static inline void wait_for_xmitr(struct uart_sio_port *up)
 
 static void m32r_sio_console_putchar(struct uart_port *port, int ch)
 {
-	struct uart_sio_port *up = (struct uart_sio_port *)port;
+	struct uart_sio_port *up =
+		container_of(port, struct uart_sio_port, port);
 
 	wait_for_xmitr(up);
 	sio_out(up, SIOTXB, ch);
diff --git a/drivers/tty/serial/max310x.c b/drivers/tty/serial/max310x.c
index 0041a64cc86e..182549f55904 100644
--- a/drivers/tty/serial/max310x.c
+++ b/drivers/tty/serial/max310x.c
@@ -346,10 +346,13 @@ static int max3109_detect(struct device *dev)
 	unsigned int val = 0;
 	int ret;
 
-	ret = regmap_read(s->regmap, MAX310X_REVID_REG, &val);
+	ret = regmap_write(s->regmap, MAX310X_GLOBALCMD_REG,
+			   MAX310X_EXTREG_ENBL);
 	if (ret)
 		return ret;
 
+	regmap_read(s->regmap, MAX310X_REVID_EXTREG, &val);
+	regmap_write(s->regmap, MAX310X_GLOBALCMD_REG, MAX310X_EXTREG_DSBL);
 	if (((val & MAX310x_REV_MASK) != MAX3109_REV_ID)) {
 		dev_err(dev,
 			"%s ID 0x%02x does not match\n", s->devtype->name, val);
@@ -874,55 +877,37 @@ static void max310x_set_termios(struct uart_port *port,
 	uart_update_timeout(port, termios->c_cflag, baud);
 }
 
-static int max310x_ioctl(struct uart_port *port, unsigned int cmd,
-			 unsigned long arg)
+static int max310x_rs485_config(struct uart_port *port,
+				struct serial_rs485 *rs485)
 {
-#if defined(TIOCSRS485) && defined(TIOCGRS485)
-	struct serial_rs485 rs485;
 	unsigned int val;
 
-	switch (cmd) {
-	case TIOCSRS485:
-		if (copy_from_user(&rs485, (void __user *)arg, sizeof(rs485)))
-			return -EFAULT;
-		if (rs485.delay_rts_before_send > 0x0f ||
-		    rs485.delay_rts_after_send > 0x0f)
-			return -ERANGE;
-		val = (rs485.delay_rts_before_send << 4) |
-		      rs485.delay_rts_after_send;
-		max310x_port_write(port, MAX310X_HDPIXDELAY_REG, val);
-		if (rs485.flags & SER_RS485_ENABLED) {
-			max310x_port_update(port, MAX310X_MODE1_REG,
-					    MAX310X_MODE1_TRNSCVCTRL_BIT,
-					    MAX310X_MODE1_TRNSCVCTRL_BIT);
-			max310x_port_update(port, MAX310X_MODE2_REG,
-					    MAX310X_MODE2_ECHOSUPR_BIT,
-					    MAX310X_MODE2_ECHOSUPR_BIT);
-		} else {
-			max310x_port_update(port, MAX310X_MODE1_REG,
-					    MAX310X_MODE1_TRNSCVCTRL_BIT, 0);
-			max310x_port_update(port, MAX310X_MODE2_REG,
-					    MAX310X_MODE2_ECHOSUPR_BIT, 0);
-		}
-		return 0;
-	case TIOCGRS485:
-		memset(&rs485, 0, sizeof(rs485));
-		val = max310x_port_read(port, MAX310X_MODE1_REG);
-		rs485.flags = (val & MAX310X_MODE1_TRNSCVCTRL_BIT) ?
-			      SER_RS485_ENABLED : 0;
-		rs485.flags |= SER_RS485_RTS_ON_SEND;
-		val = max310x_port_read(port, MAX310X_HDPIXDELAY_REG);
-		rs485.delay_rts_before_send = val >> 4;
-		rs485.delay_rts_after_send = val & 0x0f;
-		if (copy_to_user((void __user *)arg, &rs485, sizeof(rs485)))
-			return -EFAULT;
-		return 0;
-	default:
-		break;
+	if (rs485->delay_rts_before_send > 0x0f ||
+		    rs485->delay_rts_after_send > 0x0f)
+		return -ERANGE;
+
+	val = (rs485->delay_rts_before_send << 4) |
+		rs485->delay_rts_after_send;
+	max310x_port_write(port, MAX310X_HDPIXDELAY_REG, val);
+	if (rs485->flags & SER_RS485_ENABLED) {
+		max310x_port_update(port, MAX310X_MODE1_REG,
+				MAX310X_MODE1_TRNSCVCTRL_BIT,
+				MAX310X_MODE1_TRNSCVCTRL_BIT);
+		max310x_port_update(port, MAX310X_MODE2_REG,
+				MAX310X_MODE2_ECHOSUPR_BIT,
+				MAX310X_MODE2_ECHOSUPR_BIT);
+	} else {
+		max310x_port_update(port, MAX310X_MODE1_REG,
+				MAX310X_MODE1_TRNSCVCTRL_BIT, 0);
+		max310x_port_update(port, MAX310X_MODE2_REG,
+				MAX310X_MODE2_ECHOSUPR_BIT, 0);
 	}
-#endif
 
-	return -ENOIOCTLCMD;
+	rs485->flags &= SER_RS485_RTS_ON_SEND | SER_RS485_ENABLED;
+	memset(rs485->padding, 0, sizeof(rs485->padding));
+	port->rs485 = *rs485;
+
+	return 0;
 }
 
 static int max310x_startup(struct uart_port *port)
@@ -1017,7 +1002,6 @@ static const struct uart_ops max310x_ops = {
 	.release_port	= max310x_null_void,
 	.config_port	= max310x_config_port,
 	.verify_port	= max310x_verify_port,
-	.ioctl		= max310x_ioctl,
 };
 
 static int __maybe_unused max310x_suspend(struct device *dev)
@@ -1218,6 +1202,7 @@ static int max310x_probe(struct device *dev, struct max310x_devtype *devtype,
 		s->p[i].port.iobase	= i * 0x20;
 		s->p[i].port.membase	= (void __iomem *)~0;
 		s->p[i].port.uartclk	= uartclk;
+		s->p[i].port.rs485_config = max310x_rs485_config;
 		s->p[i].port.ops	= &max310x_ops;
 		/* Disable all interrupts */
 		max310x_port_write(&s->p[i].port, MAX310X_IRQEN_REG, 0);
diff --git a/drivers/tty/serial/mcf.c b/drivers/tty/serial/mcf.c
index bc896dc7d2ed..10496672dfdb 100644
--- a/drivers/tty/serial/mcf.c
+++ b/drivers/tty/serial/mcf.c
@@ -57,7 +57,6 @@ struct mcf_uart {
 	struct uart_port	port;
 	unsigned int		sigs;		/* Local copy of line sigs */
 	unsigned char		imr;		/* Local IMR mirror */
-	struct serial_rs485	rs485;		/* RS485 settings */
 };
 
 /****************************************************************************/
@@ -104,7 +103,7 @@ static void mcf_start_tx(struct uart_port *port)
 {
 	struct mcf_uart *pp = container_of(port, struct mcf_uart, port);
 
-	if (pp->rs485.flags & SER_RS485_ENABLED) {
+	if (port->rs485.flags & SER_RS485_ENABLED) {
 		/* Enable Transmitter */
 		writeb(MCFUART_UCR_TXENABLE, port->membase + MCFUART_UCR);
 		/* Manually assert RTS */
@@ -258,12 +257,12 @@ static void mcf_set_termios(struct uart_port *port, struct ktermios *termios,
 		mr2 |= MCFUART_MR2_TXCTS;
 	}
 
-	if (pp->rs485.flags & SER_RS485_ENABLED) {
+	spin_lock_irqsave(&port->lock, flags);
+	if (port->rs485.flags & SER_RS485_ENABLED) {
 		dev_dbg(port->dev, "Setting UART to RS485\n");
 		mr2 |= MCFUART_MR2_TXRTS;
 	}
 
-	spin_lock_irqsave(&port->lock, flags);
 	uart_update_timeout(port, termios->c_cflag, baud);
 	writeb(MCFUART_UCR_CMDRESETRX, port->membase + MCFUART_UCR);
 	writeb(MCFUART_UCR_CMDRESETTX, port->membase + MCFUART_UCR);
@@ -360,7 +359,7 @@ static void mcf_tx_chars(struct mcf_uart *pp)
 		pp->imr &= ~MCFUART_UIR_TXREADY;
 		writeb(pp->imr, port->membase + MCFUART_UIMR);
 		/* Disable TX to negate RTS automatically */
-		if (pp->rs485.flags & SER_RS485_ENABLED)
+		if (port->rs485.flags & SER_RS485_ENABLED)
 			writeb(MCFUART_UCR_TXDISABLE,
 				port->membase + MCFUART_UCR);
 	}
@@ -440,13 +439,11 @@ static int mcf_verify_port(struct uart_port *port, struct serial_struct *ser)
 /****************************************************************************/
 
 /* Enable or disable the RS485 support */
-static void mcf_config_rs485(struct uart_port *port, struct serial_rs485 *rs485)
+static int mcf_config_rs485(struct uart_port *port, struct serial_rs485 *rs485)
 {
 	struct mcf_uart *pp = container_of(port, struct mcf_uart, port);
-	unsigned long flags;
 	unsigned char mr1, mr2;
 
-	spin_lock_irqsave(&port->lock, flags);
 	/* Get mode registers */
 	mr1 = readb(port->membase + MCFUART_UMR);
 	mr2 = readb(port->membase + MCFUART_UMR);
@@ -460,32 +457,8 @@ static void mcf_config_rs485(struct uart_port *port, struct serial_rs485 *rs485)
 	}
 	writeb(mr1, port->membase + MCFUART_UMR);
 	writeb(mr2, port->membase + MCFUART_UMR);
-	pp->rs485 = *rs485;
-	spin_unlock_irqrestore(&port->lock, flags);
-}
+	port->rs485 = *rs485;
 
-static int mcf_ioctl(struct uart_port *port, unsigned int cmd,
-		unsigned long arg)
-{
-	switch (cmd) {
-	case TIOCSRS485: {
-		struct serial_rs485 rs485;
-		if (copy_from_user(&rs485, (struct serial_rs485 *)arg,
-				sizeof(struct serial_rs485)))
-			return -EFAULT;
-		mcf_config_rs485(port, &rs485);
-		break;
-	}
-	case TIOCGRS485: {
-		struct mcf_uart *pp = container_of(port, struct mcf_uart, port);
-		if (copy_to_user((struct serial_rs485 *)arg, &pp->rs485,
-				sizeof(struct serial_rs485)))
-			return -EFAULT;
-		break;
-	}
-	default:
-		return -ENOIOCTLCMD;
-	}
 	return 0;
 }
 
@@ -510,7 +483,6 @@ static const struct uart_ops mcf_uart_ops = {
 	.release_port	= mcf_release_port,
 	.config_port	= mcf_config_port,
 	.verify_port	= mcf_verify_port,
-	.ioctl		= mcf_ioctl,
 };
 
 static struct mcf_uart mcf_ports[4];
@@ -538,6 +510,7 @@ int __init early_mcf_setup(struct mcf_platform_uart *platp)
 		port->irq = platp[i].irq;
 		port->uartclk = MCF_BUSCLK;
 		port->flags = UPF_BOOT_AUTOCONF;
+		port->rs485_config = mcf_config_rs485;
 		port->ops = &mcf_uart_ops;
 	}
 
@@ -663,6 +636,7 @@ static int mcf_probe(struct platform_device *pdev)
 		port->uartclk = MCF_BUSCLK;
 		port->ops = &mcf_uart_ops;
 		port->flags = UPF_BOOT_AUTOCONF;
+		port->rs485_config = mcf_config_rs485;
 
 		uart_add_one_port(&mcf_driver, port);
 	}
@@ -693,7 +667,6 @@ static struct platform_driver mcf_platform_driver = {
 	.remove		= mcf_remove,
 	.driver		= {
 		.name	= "mcfuart",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/tty/serial/men_z135_uart.c b/drivers/tty/serial/men_z135_uart.c
index 30e9e60bc5cd..517cd073dc08 100644
--- a/drivers/tty/serial/men_z135_uart.c
+++ b/drivers/tty/serial/men_z135_uart.c
@@ -809,6 +809,7 @@ static void men_z135_remove(struct mcb_device *mdev)
 
 static const struct mcb_device_id men_z135_ids[] = {
 	{ .device = 0x87 },
+	{ }
 };
 MODULE_DEVICE_TABLE(mcb, men_z135_ids);
 
diff --git a/drivers/tty/serial/meson_uart.c b/drivers/tty/serial/meson_uart.c
index 15c749753317..67c036702629 100644
--- a/drivers/tty/serial/meson_uart.c
+++ b/drivers/tty/serial/meson_uart.c
@@ -599,7 +599,6 @@ static  struct platform_driver meson_uart_platform_driver = {
 	.probe		= meson_uart_probe,
 	.remove		= meson_uart_remove,
 	.driver		= {
-		.owner		= THIS_MODULE,
 		.name		= "meson_uart",
 		.of_match_table	= meson_uart_dt_match,
 	},
diff --git a/drivers/tty/serial/mfd.c b/drivers/tty/serial/mfd.c
index 445799dc9846..8fe4501d7565 100644
--- a/drivers/tty/serial/mfd.c
+++ b/drivers/tty/serial/mfd.c
@@ -1252,12 +1252,7 @@ static int serial_hsu_resume(struct pci_dev *pdev)
 	}
 	return 0;
 }
-#else
-#define serial_hsu_suspend	NULL
-#define serial_hsu_resume	NULL
-#endif
 
-#ifdef CONFIG_PM_RUNTIME
 static int serial_hsu_runtime_idle(struct device *dev)
 {
 	pm_schedule_suspend(dev, 500);
@@ -1274,6 +1269,8 @@ static int serial_hsu_runtime_resume(struct device *dev)
 	return 0;
 }
 #else
+#define serial_hsu_suspend		NULL
+#define serial_hsu_resume		NULL
 #define serial_hsu_runtime_idle		NULL
 #define serial_hsu_runtime_suspend	NULL
 #define serial_hsu_runtime_resume	NULL
@@ -1371,7 +1368,7 @@ static void hsu_global_init(void)
 	hsu->iolen = 0x1000;
 
 	if (!(request_mem_region(hsu->paddr, hsu->iolen, "HSU global")))
-		pr_warning("HSU: error in request mem region\n");
+		pr_warn("HSU: error in request mem region\n");
 
 	hsu->reg = ioremap_nocache((unsigned long)hsu->paddr, hsu->iolen);
 	if (!hsu->reg) {
diff --git a/drivers/tty/serial/mpc52xx_uart.c b/drivers/tty/serial/mpc52xx_uart.c
index a5f4e3648b15..3308ef243dc3 100644
--- a/drivers/tty/serial/mpc52xx_uart.c
+++ b/drivers/tty/serial/mpc52xx_uart.c
@@ -1889,7 +1889,6 @@ static struct platform_driver mpc52xx_uart_of_driver = {
 #endif
 	.driver = {
 		.name = "mpc52xx-psc-uart",
-		.owner = THIS_MODULE,
 		.of_match_table = mpc52xx_uart_of_match,
 	},
 };
diff --git a/drivers/tty/serial/mpsc.c b/drivers/tty/serial/mpsc.c
index ae49856ef6c7..856fd5a5fa3c 100644
--- a/drivers/tty/serial/mpsc.c
+++ b/drivers/tty/serial/mpsc.c
@@ -1246,7 +1246,8 @@ static irqreturn_t mpsc_sdma_intr(int irq, void *dev_id)
  */
 static uint mpsc_tx_empty(struct uart_port *port)
 {
-	struct mpsc_port_info *pi = (struct mpsc_port_info *)port;
+	struct mpsc_port_info *pi =
+		container_of(port, struct mpsc_port_info, port);
 	ulong iflags;
 	uint rc;
 
@@ -1264,7 +1265,8 @@ static void mpsc_set_mctrl(struct uart_port *port, uint mctrl)
 
 static uint mpsc_get_mctrl(struct uart_port *port)
 {
-	struct mpsc_port_info *pi = (struct mpsc_port_info *)port;
+	struct mpsc_port_info *pi =
+		container_of(port, struct mpsc_port_info, port);
 	u32 mflags, status;
 
 	status = (pi->mirror_regs) ? pi->MPSC_CHR_10_m
@@ -1281,7 +1283,8 @@ static uint mpsc_get_mctrl(struct uart_port *port)
 
 static void mpsc_stop_tx(struct uart_port *port)
 {
-	struct mpsc_port_info *pi = (struct mpsc_port_info *)port;
+	struct mpsc_port_info *pi =
+		container_of(port, struct mpsc_port_info, port);
 
 	pr_debug("mpsc_stop_tx[%d]\n", port->line);
 
@@ -1290,7 +1293,8 @@ static void mpsc_stop_tx(struct uart_port *port)
 
 static void mpsc_start_tx(struct uart_port *port)
 {
-	struct mpsc_port_info *pi = (struct mpsc_port_info *)port;
+	struct mpsc_port_info *pi =
+		container_of(port, struct mpsc_port_info, port);
 	unsigned long iflags;
 
 	spin_lock_irqsave(&pi->tx_lock, iflags);
@@ -1316,7 +1320,8 @@ static void mpsc_start_rx(struct mpsc_port_info *pi)
 
 static void mpsc_stop_rx(struct uart_port *port)
 {
-	struct mpsc_port_info *pi = (struct mpsc_port_info *)port;
+	struct mpsc_port_info *pi =
+		container_of(port, struct mpsc_port_info, port);
 
 	pr_debug("mpsc_stop_rx[%d]: Stopping...\n", port->line);
 
@@ -1338,7 +1343,8 @@ static void mpsc_stop_rx(struct uart_port *port)
 
 static void mpsc_break_ctl(struct uart_port *port, int ctl)
 {
-	struct mpsc_port_info *pi = (struct mpsc_port_info *)port;
+	struct mpsc_port_info *pi =
+		container_of(port, struct mpsc_port_info, port);
 	ulong	flags;
 	u32	v;
 
@@ -1353,7 +1359,8 @@ static void mpsc_break_ctl(struct uart_port *port, int ctl)
 
 static int mpsc_startup(struct uart_port *port)
 {
-	struct mpsc_port_info *pi = (struct mpsc_port_info *)port;
+	struct mpsc_port_info *pi =
+		container_of(port, struct mpsc_port_info, port);
 	u32 flag = 0;
 	int rc;
 
@@ -1383,7 +1390,8 @@ static int mpsc_startup(struct uart_port *port)
 
 static void mpsc_shutdown(struct uart_port *port)
 {
-	struct mpsc_port_info *pi = (struct mpsc_port_info *)port;
+	struct mpsc_port_info *pi =
+		container_of(port, struct mpsc_port_info, port);
 
 	pr_debug("mpsc_shutdown[%d]: Shutting down MPSC\n", port->line);
 
@@ -1394,7 +1402,8 @@ static void mpsc_shutdown(struct uart_port *port)
 static void mpsc_set_termios(struct uart_port *port, struct ktermios *termios,
 		 struct ktermios *old)
 {
-	struct mpsc_port_info *pi = (struct mpsc_port_info *)port;
+	struct mpsc_port_info *pi =
+		container_of(port, struct mpsc_port_info, port);
 	u32 baud;
 	ulong flags;
 	u32 chr_bits, stop_bits, par;
@@ -1498,7 +1507,8 @@ static int mpsc_request_port(struct uart_port *port)
 
 static void mpsc_release_port(struct uart_port *port)
 {
-	struct mpsc_port_info *pi = (struct mpsc_port_info *)port;
+	struct mpsc_port_info *pi =
+		container_of(port, struct mpsc_port_info, port);
 
 	if (pi->ready) {
 		mpsc_uninit_rings(pi);
@@ -1513,7 +1523,8 @@ static void mpsc_config_port(struct uart_port *port, int flags)
 
 static int mpsc_verify_port(struct uart_port *port, struct serial_struct *ser)
 {
-	struct mpsc_port_info *pi = (struct mpsc_port_info *)port;
+	struct mpsc_port_info *pi =
+		container_of(port, struct mpsc_port_info, port);
 	int rc = 0;
 
 	pr_debug("mpsc_verify_port[%d]: Verifying port data\n", pi->port.line);
@@ -1548,7 +1559,8 @@ static void mpsc_put_poll_char(struct uart_port *port,
 
 static int mpsc_get_poll_char(struct uart_port *port)
 {
-	struct mpsc_port_info *pi = (struct mpsc_port_info *)port;
+	struct mpsc_port_info *pi =
+		container_of(port, struct mpsc_port_info, port);
 	struct mpsc_rx_desc *rxre;
 	u32	cmdstat, bytes_in, i;
 	u8	*bp;
@@ -1648,7 +1660,8 @@ static int mpsc_get_poll_char(struct uart_port *port)
 static void mpsc_put_poll_char(struct uart_port *port,
 			 unsigned char c)
 {
-	struct mpsc_port_info *pi = (struct mpsc_port_info *)port;
+	struct mpsc_port_info *pi =
+		container_of(port, struct mpsc_port_info, port);
 	u32 data;
 
 	data = readl(pi->mpsc_base + MPSC_MPCR);
@@ -2111,7 +2124,6 @@ static struct platform_driver mpsc_driver = {
 	.remove	= mpsc_drv_remove,
 	.driver	= {
 		.name	= MPSC_CTLR_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/tty/serial/mrst_max3110.c b/drivers/tty/serial/mrst_max3110.c
index 1504a14ec1a6..77239d5e620d 100644
--- a/drivers/tty/serial/mrst_max3110.c
+++ b/drivers/tty/serial/mrst_max3110.c
@@ -27,6 +27,8 @@
  *    interrupt for a low speed UART device
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #ifdef CONFIG_MAGIC_SYSRQ
 #define SUPPORT_SYSRQ
 #endif
@@ -47,8 +49,6 @@
 
 #include "mrst_max3110.h"
 
-#define PR_FMT	"mrst_max3110: "
-
 #define UART_TX_NEEDED 1
 #define CON_TX_NEEDED  2
 #define BIT_IRQ_PENDING    3
@@ -127,8 +127,8 @@ static int max3110_out(struct uart_max3110 *max, const u16 out)
 	*obuf = out;
 	ret = max3110_write_then_read(max, obuf, ibuf, 2, 1);
 	if (ret) {
-		pr_warning(PR_FMT "%s(): get err msg %d when sending 0x%x\n",
-				__func__, ret, out);
+		pr_warn("%s: get err msg %d when sending 0x%x\n",
+			__func__, ret, out);
 		goto exit;
 	}
 
@@ -153,10 +153,8 @@ static int max3110_read_multi(struct uart_max3110 *max)
 
 	blen = M3110_RX_FIFO_DEPTH * sizeof(u16);
 	buf = kzalloc(blen * 2, GFP_KERNEL | GFP_DMA);
-	if (!buf) {
-		pr_warning(PR_FMT "%s(): fail to alloc dma buffer\n", __func__);
+	if (!buf)
 		return 0;
-	}
 
 	/* tx/rx always have the same length */
 	obuf = buf;
@@ -212,13 +210,13 @@ serial_m3110_con_setup(struct console *co, char *options)
 	int parity = 'n';
 	int flow = 'n';
 
-	pr_info(PR_FMT "setting up console\n");
+	pr_info("setting up console\n");
 
 	if (co->index == -1)
 		co->index = 0;
 
 	if (!max) {
-		pr_err(PR_FMT "pmax is NULL, return");
+		pr_err("pmax is NULL, return\n");
 		return -ENODEV;
 	}
 
@@ -296,8 +294,7 @@ static void send_circ_buf(struct uart_max3110 *max,
 
 			ret = max3110_write_then_read(max, obuf, ibuf, blen, 0);
 			if (ret)
-				pr_warning(PR_FMT "%s(): get err msg %d\n",
-						__func__, ret);
+				pr_warn("%s: get err msg %d\n", __func__, ret);
 
 			receive_chars(max, ibuf, len);
 
@@ -411,7 +408,7 @@ static int max3110_main_thread(void *_max)
 	int ret = 0;
 	struct circ_buf *xmit = &max->con_xmit;
 
-	pr_info(PR_FMT "start main thread\n");
+	pr_info("start main thread\n");
 
 	do {
 		wait_event_interruptible(*wq,
@@ -455,7 +452,7 @@ static int max3110_read_thread(void *_max)
 {
 	struct uart_max3110 *max = _max;
 
-	pr_info(PR_FMT "start read thread\n");
+	pr_info("start read thread\n");
 	do {
 		/*
 		 * If can't acquire the mutex, it means the main thread
@@ -481,7 +478,7 @@ static int serial_m3110_startup(struct uart_port *port)
 	int ret = 0;
 
 	if (port->line != 0) {
-		pr_err(PR_FMT "uart port startup failed\n");
+		pr_err("uart port startup failed\n");
 		return -1;
 	}
 
@@ -504,7 +501,7 @@ static int serial_m3110_startup(struct uart_port *port)
 		if (IS_ERR(max->read_thread)) {
 			ret = PTR_ERR(max->read_thread);
 			max->read_thread = NULL;
-			pr_err(PR_FMT "Can't create read thread!\n");
+			pr_err("Can't create read thread!\n");
 			return ret;
 		}
 	}
diff --git a/drivers/tty/serial/msm_serial.c b/drivers/tty/serial/msm_serial.c
index 4b6c78331a64..c88b522ccd73 100644
--- a/drivers/tty/serial/msm_serial.c
+++ b/drivers/tty/serial/msm_serial.c
@@ -54,6 +54,7 @@ struct msm_port {
 	unsigned int		imr;
 	int			is_uartdm;
 	unsigned int		old_snap_state;
+	bool			break_detected;
 };
 
 static inline void wait_for_xmitr(struct uart_port *port)
@@ -126,23 +127,38 @@ static void handle_rx_dm(struct uart_port *port, unsigned int misr)
 
 	while (count > 0) {
 		unsigned char buf[4];
+		int sysrq, r_count, i;
 
 		sr = msm_read(port, UART_SR);
 		if ((sr & UART_SR_RX_READY) == 0) {
 			msm_port->old_snap_state -= count;
 			break;
 		}
+
 		ioread32_rep(port->membase + UARTDM_RF, buf, 1);
-		if (sr & UART_SR_RX_BREAK) {
-			port->icount.brk++;
-			if (uart_handle_break(port))
-				continue;
-		} else if (sr & UART_SR_PAR_FRAME_ERR)
-			port->icount.frame++;
+		r_count = min_t(int, count, sizeof(buf));
 
-		/* TODO: handle sysrq */
-		tty_insert_flip_string(tport, buf, min(count, 4));
-		count -= 4;
+		for (i = 0; i < r_count; i++) {
+			char flag = TTY_NORMAL;
+
+			if (msm_port->break_detected && buf[i] == 0) {
+				port->icount.brk++;
+				flag = TTY_BREAK;
+				msm_port->break_detected = false;
+				if (uart_handle_break(port))
+					continue;
+			}
+
+			if (!(port->read_status_mask & UART_SR_RX_BREAK))
+				flag = TTY_NORMAL;
+
+			spin_unlock(&port->lock);
+			sysrq = uart_handle_sysrq_char(port, buf[i]);
+			spin_lock(&port->lock);
+			if (!sysrq)
+				tty_insert_flip_char(tport, buf[i], flag);
+		}
+		count -= r_count;
 	}
 
 	spin_unlock(&port->lock);
@@ -174,6 +190,7 @@ static void handle_rx(struct uart_port *port)
 	while ((sr = msm_read(port, UART_SR)) & UART_SR_RX_READY) {
 		unsigned int c;
 		char flag = TTY_NORMAL;
+		int sysrq;
 
 		c = msm_read(port, UART_RF);
 
@@ -195,7 +212,10 @@ static void handle_rx(struct uart_port *port)
 		else if (sr & UART_SR_PAR_FRAME_ERR)
 			flag = TTY_FRAME;
 
-		if (!uart_handle_sysrq_char(port, c))
+		spin_unlock(&port->lock);
+		sysrq = uart_handle_sysrq_char(port, c);
+		spin_lock(&port->lock);
+		if (!sysrq)
 			tty_insert_flip_char(tport, c, flag);
 	}
 
@@ -287,6 +307,11 @@ static irqreturn_t msm_irq(int irq, void *dev_id)
 	misr = msm_read(port, UART_MISR);
 	msm_write(port, 0, UART_IMR); /* disable interrupt */
 
+	if (misr & UART_IMR_RXBREAK_START) {
+		msm_port->break_detected = true;
+		msm_write(port, UART_CR_CMD_RESET_RXBREAK_START, UART_CR);
+	}
+
 	if (misr & (UART_IMR_RXLEV | UART_IMR_RXSTALE)) {
 		if (msm_port->is_uartdm)
 			handle_rx_dm(port, misr);
@@ -402,9 +427,6 @@ static int msm_set_baud_rate(struct uart_port *port, unsigned int baud)
 
 	entry = msm_find_best_baud(port, baud);
 
-	if (msm_port->is_uartdm)
-		msm_write(port, UART_CR_CMD_RESET_RX, UART_CR);
-
 	msm_write(port, entry->code, UART_CSR);
 
 	/* RX stale watermark */
@@ -421,6 +443,18 @@ static int msm_set_baud_rate(struct uart_port *port, unsigned int baud)
 	/* set TX watermark */
 	msm_write(port, 10, UART_TFWR);
 
+	msm_write(port, UART_CR_CMD_PROTECTION_EN, UART_CR);
+	msm_reset(port);
+
+	/* Enable RX and TX */
+	msm_write(port, UART_CR_TX_ENABLE | UART_CR_RX_ENABLE, UART_CR);
+
+	/* turn on RX and CTS interrupts */
+	msm_port->imr = UART_IMR_RXLEV | UART_IMR_RXSTALE |
+			UART_IMR_CURRENT_CTS | UART_IMR_RXBREAK_START;
+
+	msm_write(port, msm_port->imr, UART_IMR);
+
 	if (msm_port->is_uartdm) {
 		msm_write(port, UART_CR_CMD_RESET_STALE_INT, UART_CR);
 		msm_write(port, 0xFFFFFF, UARTDM_DMRX);
@@ -467,40 +501,6 @@ static int msm_startup(struct uart_port *port)
 	data |= UART_MR1_AUTO_RFR_LEVEL1 & (rfr_level << 2);
 	data |= UART_MR1_AUTO_RFR_LEVEL0 & rfr_level;
 	msm_write(port, data, UART_MR1);
-
-	/* make sure that RXSTALE count is non-zero */
-	data = msm_read(port, UART_IPR);
-	if (unlikely(!data)) {
-		data |= UART_IPR_RXSTALE_LAST;
-		data |= UART_IPR_STALE_LSB;
-		msm_write(port, data, UART_IPR);
-	}
-
-	data = 0;
-	if (!port->cons || (port->cons && !(port->cons->flags & CON_ENABLED))) {
-		msm_write(port, UART_CR_CMD_PROTECTION_EN, UART_CR);
-		msm_reset(port);
-		data = UART_CR_TX_ENABLE;
-	}
-
-	data |= UART_CR_RX_ENABLE;
-	msm_write(port, data, UART_CR);	/* enable TX & RX */
-
-	/* Make sure IPR is not 0 to start with*/
-	if (msm_port->is_uartdm)
-		msm_write(port, UART_IPR_STALE_LSB, UART_IPR);
-
-	/* turn on RX and CTS interrupts */
-	msm_port->imr = UART_IMR_RXLEV | UART_IMR_RXSTALE |
-			UART_IMR_CURRENT_CTS;
-
-	if (msm_port->is_uartdm) {
-		msm_write(port, 0xFFFFFF, UARTDM_DMRX);
-		msm_write(port, UART_CR_CMD_RESET_STALE_INT, UART_CR);
-		msm_write(port, UART_CR_CMD_STALE_EVENT_ENABLE, UART_CR);
-	}
-
-	msm_write(port, msm_port->imr, UART_IMR);
 	return 0;
 }
 
@@ -1044,17 +1044,22 @@ static int msm_serial_probe(struct platform_device *pdev)
 	struct resource *resource;
 	struct uart_port *port;
 	const struct of_device_id *id;
-	int irq;
+	int irq, line;
+
+	if (pdev->dev.of_node)
+		line = of_alias_get_id(pdev->dev.of_node, "serial");
+	else
+		line = pdev->id;
 
-	if (pdev->id == -1)
-		pdev->id = atomic_inc_return(&msm_uart_next_id) - 1;
+	if (line < 0)
+		line = atomic_inc_return(&msm_uart_next_id) - 1;
 
-	if (unlikely(pdev->id < 0 || pdev->id >= UART_NR))
+	if (unlikely(line < 0 || line >= UART_NR))
 		return -ENXIO;
 
-	dev_info(&pdev->dev, "msm_serial: detected port #%d\n", pdev->id);
+	dev_info(&pdev->dev, "msm_serial: detected port #%d\n", line);
 
-	port = get_port_from_line(pdev->id);
+	port = get_port_from_line(line);
 	port->dev = &pdev->dev;
 	msm_port = UART_TO_MSM(port);
 
@@ -1114,7 +1119,6 @@ static struct platform_driver msm_platform_driver = {
 	.probe = msm_serial_probe,
 	.driver = {
 		.name = "msm_serial",
-		.owner = THIS_MODULE,
 		.of_match_table = msm_match_table,
 	},
 };
diff --git a/drivers/tty/serial/msm_serial.h b/drivers/tty/serial/msm_serial.h
index 73d3abe71e79..3e1c7138d8cd 100644
--- a/drivers/tty/serial/msm_serial.h
+++ b/drivers/tty/serial/msm_serial.h
@@ -65,6 +65,7 @@
 #define UART_CR_TX_ENABLE		(1 << 2)
 #define UART_CR_RX_DISABLE		(1 << 1)
 #define UART_CR_RX_ENABLE		(1 << 0)
+#define UART_CR_CMD_RESET_RXBREAK_START	((1 << 11) | (2 << 4))
 
 #define UART_IMR		0x0014
 #define UART_IMR_TXLEV		(1 << 0)
@@ -72,6 +73,7 @@
 #define UART_IMR_RXLEV		(1 << 4)
 #define UART_IMR_DELTA_CTS	(1 << 5)
 #define UART_IMR_CURRENT_CTS	(1 << 6)
+#define UART_IMR_RXBREAK_START	(1 << 10)
 
 #define UART_IPR_RXSTALE_LAST		0x20
 #define UART_IPR_STALE_LSB		0x1F
diff --git a/drivers/tty/serial/msm_serial_hs.c b/drivers/tty/serial/msm_serial_hs.c
index 48e94961a9e5..62da8534ba75 100644
--- a/drivers/tty/serial/msm_serial_hs.c
+++ b/drivers/tty/serial/msm_serial_hs.c
@@ -1792,7 +1792,7 @@ static void __exit msm_serial_hs_exit(void)
 }
 module_exit(msm_serial_hs_exit);
 
-#ifdef CONFIG_PM_RUNTIME
+#ifdef CONFIG_PM
 static int msm_hs_runtime_idle(struct device *dev)
 {
 	/*
@@ -1838,7 +1838,6 @@ static struct platform_driver msm_serial_hs_platform_driver = {
 	.remove = msm_hs_remove,
 	.driver = {
 		.name = "msm_serial_hs",
-		.owner = THIS_MODULE,
 		.pm   = &msm_hs_dev_pm_ops,
 	},
 };
diff --git a/drivers/tty/serial/mxs-auart.c b/drivers/tty/serial/mxs-auart.c
index 10c29334fe2f..ec553f8eb218 100644
--- a/drivers/tty/serial/mxs-auart.c
+++ b/drivers/tty/serial/mxs-auart.c
@@ -14,6 +14,10 @@
  * http://www.gnu.org/copyleft/gpl.html
  */
 
+#if defined(CONFIG_SERIAL_MXS_AUART_CONSOLE) && defined(CONFIG_MAGIC_SYSRQ)
+#define SUPPORT_SYSRQ
+#endif
+
 #include <linux/kernel.h>
 #include <linux/errno.h>
 #include <linux/init.h>
@@ -38,6 +42,12 @@
 
 #include <asm/cacheflush.h>
 
+#include <linux/gpio.h>
+#include <linux/gpio/consumer.h>
+#include <linux/err.h>
+#include <linux/irq.h>
+#include "serial_mctrl_gpio.h"
+
 #define MXS_AUART_PORTS 5
 #define MXS_AUART_FIFO_SIZE		16
 
@@ -139,7 +149,7 @@ struct mxs_auart_port {
 #define MXS_AUART_DMA_RX_READY	3  /* bit 3 */
 #define MXS_AUART_RTSCTS	4  /* bit 4 */
 	unsigned long flags;
-	unsigned int ctrl;
+	unsigned int mctrl_prev;
 	enum mxs_auart_type devtype;
 
 	unsigned int irq;
@@ -155,6 +165,10 @@ struct mxs_auart_port {
 	struct scatterlist rx_sgl;
 	struct dma_chan	*rx_dma_chan;
 	void *rx_dma_buf;
+
+	struct mctrl_gpios	*gpios;
+	int			gpio_irq[UART_GPIO_MAX];
+	bool			ms_irq_enabled;
 };
 
 static struct platform_device_id mxs_auart_devtype[] = {
@@ -414,25 +428,102 @@ static void mxs_auart_set_mctrl(struct uart_port *u, unsigned mctrl)
 			ctrl |= AUART_CTRL2_RTS;
 	}
 
-	s->ctrl = mctrl;
 	writel(ctrl, u->membase + AUART_CTRL2);
+
+	mctrl_gpio_set(s->gpios, mctrl);
+}
+
+#define MCTRL_ANY_DELTA        (TIOCM_RI | TIOCM_DSR | TIOCM_CD | TIOCM_CTS)
+static u32 mxs_auart_modem_status(struct mxs_auart_port *s, u32 mctrl)
+{
+	u32 mctrl_diff;
+
+	mctrl_diff = mctrl ^ s->mctrl_prev;
+	s->mctrl_prev = mctrl;
+	if (mctrl_diff & MCTRL_ANY_DELTA && s->ms_irq_enabled &&
+						s->port.state != NULL) {
+		if (mctrl_diff & TIOCM_RI)
+			s->port.icount.rng++;
+		if (mctrl_diff & TIOCM_DSR)
+			s->port.icount.dsr++;
+		if (mctrl_diff & TIOCM_CD)
+			uart_handle_dcd_change(&s->port, mctrl & TIOCM_CD);
+		if (mctrl_diff & TIOCM_CTS)
+			uart_handle_cts_change(&s->port, mctrl & TIOCM_CTS);
+
+		wake_up_interruptible(&s->port.state->port.delta_msr_wait);
+	}
+	return mctrl;
 }
 
 static u32 mxs_auart_get_mctrl(struct uart_port *u)
 {
 	struct mxs_auart_port *s = to_auart_port(u);
 	u32 stat = readl(u->membase + AUART_STAT);
-	int ctrl2 = readl(u->membase + AUART_CTRL2);
-	u32 mctrl = s->ctrl;
+	u32 mctrl = 0;
 
-	mctrl &= ~TIOCM_CTS;
 	if (stat & AUART_STAT_CTS)
 		mctrl |= TIOCM_CTS;
 
-	if (ctrl2 & AUART_CTRL2_RTS)
-		mctrl |= TIOCM_RTS;
+	return mctrl_gpio_get(s->gpios, &mctrl);
+}
+
+/*
+ * Enable modem status interrupts
+ */
+static void mxs_auart_enable_ms(struct uart_port *port)
+{
+	struct mxs_auart_port *s = to_auart_port(port);
 
-	return mctrl;
+	/*
+	 * Interrupt should not be enabled twice
+	 */
+	if (s->ms_irq_enabled)
+		return;
+
+	s->ms_irq_enabled = true;
+
+	if (s->gpio_irq[UART_GPIO_CTS] >= 0)
+		enable_irq(s->gpio_irq[UART_GPIO_CTS]);
+	/* TODO: enable AUART_INTR_CTSMIEN otherwise */
+
+	if (s->gpio_irq[UART_GPIO_DSR] >= 0)
+		enable_irq(s->gpio_irq[UART_GPIO_DSR]);
+
+	if (s->gpio_irq[UART_GPIO_RI] >= 0)
+		enable_irq(s->gpio_irq[UART_GPIO_RI]);
+
+	if (s->gpio_irq[UART_GPIO_DCD] >= 0)
+		enable_irq(s->gpio_irq[UART_GPIO_DCD]);
+}
+
+/*
+ * Disable modem status interrupts
+ */
+static void mxs_auart_disable_ms(struct uart_port *port)
+{
+	struct mxs_auart_port *s = to_auart_port(port);
+
+	/*
+	 * Interrupt should not be disabled twice
+	 */
+	if (!s->ms_irq_enabled)
+		return;
+
+	s->ms_irq_enabled = false;
+
+	if (s->gpio_irq[UART_GPIO_CTS] >= 0)
+		disable_irq(s->gpio_irq[UART_GPIO_CTS]);
+	/* TODO: disable AUART_INTR_CTSMIEN otherwise */
+
+	if (s->gpio_irq[UART_GPIO_DSR] >= 0)
+		disable_irq(s->gpio_irq[UART_GPIO_DSR]);
+
+	if (s->gpio_irq[UART_GPIO_RI] >= 0)
+		disable_irq(s->gpio_irq[UART_GPIO_RI]);
+
+	if (s->gpio_irq[UART_GPIO_DCD] >= 0)
+		disable_irq(s->gpio_irq[UART_GPIO_DCD]);
 }
 
 static int mxs_auart_dma_prep_rx(struct mxs_auart_port *s);
@@ -560,6 +651,10 @@ err_out:
 
 }
 
+#define RTS_AT_AUART()	IS_ERR_OR_NULL(mctrl_gpio_to_gpiod(s->gpios,	\
+							UART_GPIO_RTS))
+#define CTS_AT_AUART()	IS_ERR_OR_NULL(mctrl_gpio_to_gpiod(s->gpios,	\
+							UART_GPIO_CTS))
 static void mxs_auart_settermios(struct uart_port *u,
 				 struct ktermios *termios,
 				 struct ktermios *old)
@@ -636,6 +731,7 @@ static void mxs_auart_settermios(struct uart_port *u,
 		ctrl |= AUART_LINECTRL_STP2;
 
 	/* figure out the hardware flow control settings */
+	ctrl2 &= ~(AUART_CTRL2_CTSEN | AUART_CTRL2_RTSEN);
 	if (cflag & CRTSCTS) {
 		/*
 		 * The DMA has a bug(see errata:2836) in mx23.
@@ -650,9 +746,11 @@ static void mxs_auart_settermios(struct uart_port *u,
 				ctrl2 |= AUART_CTRL2_TXDMAE | AUART_CTRL2_RXDMAE
 				       | AUART_CTRL2_DMAONERR;
 		}
-		ctrl2 |= AUART_CTRL2_CTSEN | AUART_CTRL2_RTSEN;
-	} else {
-		ctrl2 &= ~(AUART_CTRL2_CTSEN | AUART_CTRL2_RTSEN);
+		/* Even if RTS is GPIO line RTSEN can be enabled because
+		 * the pinctrl configuration decides about RTS pin function */
+		ctrl2 |= AUART_CTRL2_RTSEN;
+		if (CTS_AT_AUART())
+			ctrl2 |= AUART_CTRL2_CTSEN;
 	}
 
 	/* set baud rate */
@@ -678,12 +776,30 @@ static void mxs_auart_settermios(struct uart_port *u,
 			dev_err(s->dev, "We can not start up the DMA.\n");
 		}
 	}
+
+	/* CTS flow-control and modem-status interrupts */
+	if (UART_ENABLE_MS(u, termios->c_cflag))
+		mxs_auart_enable_ms(u);
+	else
+		mxs_auart_disable_ms(u);
+}
+
+static void mxs_auart_set_ldisc(struct uart_port *port,
+				struct ktermios *termios)
+{
+	if (termios->c_line == N_PPS) {
+		port->flags |= UPF_HARDPPS_CD;
+		mxs_auart_enable_ms(port);
+	} else {
+		port->flags &= ~UPF_HARDPPS_CD;
+	}
 }
 
 static irqreturn_t mxs_auart_irq_handle(int irq, void *context)
 {
 	u32 istat;
 	struct mxs_auart_port *s = context;
+	u32 mctrl_temp = s->mctrl_prev;
 	u32 stat = readl(s->port.membase + AUART_STAT);
 
 	istat = readl(s->port.membase + AUART_INTR);
@@ -695,8 +811,20 @@ static irqreturn_t mxs_auart_irq_handle(int irq, void *context)
 		| AUART_INTR_CTSMIS),
 			s->port.membase + AUART_INTR_CLR);
 
+	/*
+	 * Dealing with GPIO interrupt
+	 */
+	if (irq == s->gpio_irq[UART_GPIO_CTS] ||
+	    irq == s->gpio_irq[UART_GPIO_DCD] ||
+	    irq == s->gpio_irq[UART_GPIO_DSR] ||
+	    irq == s->gpio_irq[UART_GPIO_RI])
+		mxs_auart_modem_status(s,
+				mctrl_gpio_get(s->gpios, &mctrl_temp));
+
 	if (istat & AUART_INTR_CTSMIS) {
-		uart_handle_cts_change(&s->port, stat & AUART_STAT_CTS);
+		if (CTS_AT_AUART() && s->ms_irq_enabled)
+			uart_handle_cts_change(&s->port,
+					stat & AUART_STAT_CTS);
 		writel(AUART_INTR_CTSMIS,
 				s->port.membase + AUART_INTR_CLR);
 		istat &= ~AUART_INTR_CTSMIS;
@@ -757,6 +885,10 @@ static int mxs_auart_startup(struct uart_port *u)
 	 */
 	writel(AUART_LINECTRL_FEN, u->membase + AUART_LINECTRL_SET);
 
+	/* get initial status of modem lines */
+	mctrl_gpio_get(s->gpios, &s->mctrl_prev);
+
+	s->ms_irq_enabled = false;
 	return 0;
 }
 
@@ -764,6 +896,8 @@ static void mxs_auart_shutdown(struct uart_port *u)
 {
 	struct mxs_auart_port *s = to_auart_port(u);
 
+	mxs_auart_disable_ms(u);
+
 	if (auart_dma_enabled(s))
 		mxs_auart_dma_exit(s);
 
@@ -779,10 +913,11 @@ static void mxs_auart_shutdown(struct uart_port *u)
 
 static unsigned int mxs_auart_tx_empty(struct uart_port *u)
 {
-	if (readl(u->membase + AUART_STAT) & AUART_STAT_TXFE)
+	if ((readl(u->membase + AUART_STAT) &
+		 (AUART_STAT_TXFE | AUART_STAT_BUSY)) == AUART_STAT_TXFE)
 		return TIOCSER_TEMT;
-	else
-		return 0;
+
+	return 0;
 }
 
 static void mxs_auart_start_tx(struct uart_port *u)
@@ -820,12 +955,14 @@ static struct uart_ops mxs_auart_ops = {
 	.start_tx       = mxs_auart_start_tx,
 	.stop_tx	= mxs_auart_stop_tx,
 	.stop_rx	= mxs_auart_stop_rx,
+	.enable_ms      = mxs_auart_enable_ms,
 	.break_ctl      = mxs_auart_break_ctl,
 	.set_mctrl	= mxs_auart_set_mctrl,
 	.get_mctrl      = mxs_auart_get_mctrl,
 	.startup	= mxs_auart_startup,
 	.shutdown       = mxs_auart_shutdown,
 	.set_termios    = mxs_auart_settermios,
+	.set_ldisc      = mxs_auart_set_ldisc,
 	.type	   	= mxs_auart_type,
 	.release_port   = mxs_auart_release_port,
 	.request_port   = mxs_auart_request_port,
@@ -1020,6 +1157,71 @@ static int serial_mxs_probe_dt(struct mxs_auart_port *s,
 	return 0;
 }
 
+static bool mxs_auart_init_gpios(struct mxs_auart_port *s, struct device *dev)
+{
+	enum mctrl_gpio_idx i;
+	struct gpio_desc *gpiod;
+
+	s->gpios = mctrl_gpio_init(dev, 0);
+	if (IS_ERR_OR_NULL(s->gpios))
+		return false;
+
+	/* Block (enabled before) DMA option if RTS or CTS is GPIO line */
+	if (!RTS_AT_AUART() || !CTS_AT_AUART()) {
+		if (test_bit(MXS_AUART_RTSCTS, &s->flags))
+			dev_warn(dev,
+				 "DMA and flow control via gpio may cause some problems. DMA disabled!\n");
+		clear_bit(MXS_AUART_RTSCTS, &s->flags);
+	}
+
+	for (i = 0; i < UART_GPIO_MAX; i++) {
+		gpiod = mctrl_gpio_to_gpiod(s->gpios, i);
+		if (gpiod && (gpiod_get_direction(gpiod) == GPIOF_DIR_IN))
+			s->gpio_irq[i] = gpiod_to_irq(gpiod);
+		else
+			s->gpio_irq[i] = -EINVAL;
+	}
+
+	return true;
+}
+
+static void mxs_auart_free_gpio_irq(struct mxs_auart_port *s)
+{
+	enum mctrl_gpio_idx i;
+
+	for (i = 0; i < UART_GPIO_MAX; i++)
+		if (s->gpio_irq[i] >= 0)
+			free_irq(s->gpio_irq[i], s);
+}
+
+static int mxs_auart_request_gpio_irq(struct mxs_auart_port *s)
+{
+	int *irq = s->gpio_irq;
+	enum mctrl_gpio_idx i;
+	int err = 0;
+
+	for (i = 0; (i < UART_GPIO_MAX) && !err; i++) {
+		if (irq[i] < 0)
+			continue;
+
+		irq_set_status_flags(irq[i], IRQ_NOAUTOEN);
+		err = request_irq(irq[i], mxs_auart_irq_handle,
+				IRQ_TYPE_EDGE_BOTH, dev_name(s->dev), s);
+		if (err)
+			dev_err(s->dev, "%s - Can't get %d irq\n",
+				__func__, irq[i]);
+	}
+
+	/*
+	 * If something went wrong, rollback.
+	 */
+	while (err && (--i >= 0))
+		if (irq[i] >= 0)
+			free_irq(irq[i], s);
+
+	return err;
+}
+
 static int mxs_auart_probe(struct platform_device *pdev)
 {
 	const struct of_device_id *of_id =
@@ -1067,7 +1269,7 @@ static int mxs_auart_probe(struct platform_device *pdev)
 	s->port.type = PORT_IMX;
 	s->port.dev = s->dev = &pdev->dev;
 
-	s->ctrl = 0;
+	s->mctrl_prev = 0;
 
 	s->irq = platform_get_irq(pdev, 0);
 	s->port.irq = s->irq;
@@ -1077,13 +1279,24 @@ static int mxs_auart_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, s);
 
+	if (!mxs_auart_init_gpios(s, &pdev->dev))
+		dev_err(&pdev->dev,
+			"Failed to initialize GPIOs. The serial port may not work as expected\n");
+
+	/*
+	 * Get the GPIO lines IRQ
+	 */
+	ret = mxs_auart_request_gpio_irq(s);
+	if (ret)
+		goto out_free_irq;
+
 	auart_port[s->port.line] = s;
 
 	mxs_auart_reset(&s->port);
 
 	ret = uart_add_one_port(&auart_driver, &s->port);
 	if (ret)
-		goto out_free_irq;
+		goto out_free_gpio_irq;
 
 	version = readl(s->port.membase + AUART_VERSION);
 	dev_info(&pdev->dev, "Found APPUART %d.%d.%d\n",
@@ -1092,6 +1305,8 @@ static int mxs_auart_probe(struct platform_device *pdev)
 
 	return 0;
 
+out_free_gpio_irq:
+	mxs_auart_free_gpio_irq(s);
 out_free_irq:
 	auart_port[pdev->id] = NULL;
 	free_irq(s->irq, s);
@@ -1111,6 +1326,7 @@ static int mxs_auart_remove(struct platform_device *pdev)
 
 	auart_port[pdev->id] = NULL;
 
+	mxs_auart_free_gpio_irq(s);
 	clk_put(s->clk);
 	free_irq(s->irq, s);
 	kfree(s);
@@ -1123,7 +1339,6 @@ static struct platform_driver mxs_auart_driver = {
 	.remove = mxs_auart_remove,
 	.driver = {
 		.name = "mxs-auart",
-		.owner = THIS_MODULE,
 		.of_match_table = mxs_auart_dt_ids,
 	},
 };
diff --git a/drivers/tty/serial/netx-serial.c b/drivers/tty/serial/netx-serial.c
index 7a6745601d4e..207a0a032ed1 100644
--- a/drivers/tty/serial/netx-serial.c
+++ b/drivers/tty/serial/netx-serial.c
@@ -710,7 +710,6 @@ static struct platform_driver serial_netx_driver = {
 
 	.driver		= {
 		.name   = DRIVER_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/tty/serial/of_serial.c b/drivers/tty/serial/of_serial.c
index bf355050eab6..64f1bab7e9d7 100644
--- a/drivers/tty/serial/of_serial.c
+++ b/drivers/tty/serial/of_serial.c
@@ -9,6 +9,7 @@
  *  2 of the License, or (at your option) any later version.
  *
  */
+#include <linux/console.h>
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/delay.h>
@@ -129,8 +130,15 @@ static int of_platform_serial_setup(struct platform_device *ofdev,
 
 	port->dev = &ofdev->dev;
 
-	if (type == PORT_TEGRA)
+	switch (type) {
+	case PORT_TEGRA:
 		port->handle_break = tegra_serial_handle_break;
+		break;
+
+	case PORT_RT2880:
+		port->iotype = UPIO_AU;
+		break;
+	}
 
 	return 0;
 out:
@@ -240,6 +248,70 @@ static int of_platform_serial_remove(struct platform_device *ofdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
+#ifdef CONFIG_SERIAL_8250
+static void of_serial_suspend_8250(struct of_serial_info *info)
+{
+	struct uart_8250_port *port8250 = serial8250_get_port(info->line);
+	struct uart_port *port = &port8250->port;
+
+	serial8250_suspend_port(info->line);
+	if (info->clk && (!uart_console(port) || console_suspend_enabled))
+		clk_disable_unprepare(info->clk);
+}
+
+static void of_serial_resume_8250(struct of_serial_info *info)
+{
+	struct uart_8250_port *port8250 = serial8250_get_port(info->line);
+	struct uart_port *port = &port8250->port;
+
+	if (info->clk && (!uart_console(port) || console_suspend_enabled))
+		clk_prepare_enable(info->clk);
+
+	serial8250_resume_port(info->line);
+}
+#else
+static inline void of_serial_suspend_8250(struct of_serial_info *info)
+{
+}
+
+static inline void of_serial_resume_8250(struct of_serial_info *info)
+{
+}
+#endif
+
+static int of_serial_suspend(struct device *dev)
+{
+	struct of_serial_info *info = dev_get_drvdata(dev);
+
+	switch (info->type) {
+	case PORT_8250 ... PORT_MAX_8250:
+		of_serial_suspend_8250(info);
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static int of_serial_resume(struct device *dev)
+{
+	struct of_serial_info *info = dev_get_drvdata(dev);
+
+	switch (info->type) {
+	case PORT_8250 ... PORT_MAX_8250:
+		of_serial_resume_8250(info);
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+#endif
+static SIMPLE_DEV_PM_OPS(of_serial_pm_ops, of_serial_suspend, of_serial_resume);
+
 /*
  * A few common types, add more as needed.
  */
@@ -252,6 +324,7 @@ static struct of_device_id of_platform_serial_table[] = {
 	{ .compatible = "ns16850",  .data = (void *)PORT_16850, },
 	{ .compatible = "nvidia,tegra20-uart", .data = (void *)PORT_TEGRA, },
 	{ .compatible = "nxp,lpc3220-uart", .data = (void *)PORT_LPC3220, },
+	{ .compatible = "ralink,rt2880-uart", .data = (void *)PORT_RT2880, },
 	{ .compatible = "altr,16550-FIFO32",
 		.data = (void *)PORT_ALTR_16550_F32, },
 	{ .compatible = "altr,16550-FIFO64",
@@ -269,7 +342,6 @@ static struct of_device_id of_platform_serial_table[] = {
 static struct platform_driver of_platform_serial_driver = {
 	.driver = {
 		.name = "of_serial",
-		.owner = THIS_MODULE,
 		.of_match_table = of_platform_serial_table,
 	},
 	.probe = of_platform_serial_probe,
diff --git a/drivers/tty/serial/omap-serial.c b/drivers/tty/serial/omap-serial.c
index 18c30cabe27f..2e1073da6719 100644
--- a/drivers/tty/serial/omap-serial.c
+++ b/drivers/tty/serial/omap-serial.c
@@ -46,7 +46,7 @@
 
 #include <dt-bindings/gpio/gpio.h>
 
-#define OMAP_MAX_HSUART_PORTS	6
+#define OMAP_MAX_HSUART_PORTS	10
 
 #define UART_BUILD_REVISION(x, y)	(((x) << 8) | (y))
 
@@ -163,7 +163,6 @@ struct uart_omap_port {
 	u8			wakeups_enabled;
 	u32			features;
 
-	struct serial_rs485	rs485;
 	int			rts_gpio;
 
 	struct pm_qos_request	pm_qos_request;
@@ -316,7 +315,7 @@ static void serial_omap_stop_tx(struct uart_port *port)
 	pm_runtime_get_sync(up->dev);
 
 	/* Handle RS-485 */
-	if (up->rs485.flags & SER_RS485_ENABLED) {
+	if (port->rs485.flags & SER_RS485_ENABLED) {
 		if (up->scr & OMAP_UART_SCR_TX_EMPTY) {
 			/* THR interrupt is fired when both TX FIFO and TX
 			 * shift register are empty. This means there's nothing
@@ -327,10 +326,12 @@ static void serial_omap_stop_tx(struct uart_port *port)
 			 */
 			up->scr &= ~OMAP_UART_SCR_TX_EMPTY;
 			serial_out(up, UART_OMAP_SCR, up->scr);
-			res = (up->rs485.flags & SER_RS485_RTS_AFTER_SEND) ? 1 : 0;
+			res = (port->rs485.flags & SER_RS485_RTS_AFTER_SEND) ?
+				1 : 0;
 			if (gpio_get_value(up->rts_gpio) != res) {
-				if (up->rs485.delay_rts_after_send > 0)
-					mdelay(up->rs485.delay_rts_after_send);
+				if (port->rs485.delay_rts_after_send > 0)
+					mdelay(
+					port->rs485.delay_rts_after_send);
 				gpio_set_value(up->rts_gpio, res);
 			}
 		} else {
@@ -353,8 +354,8 @@ static void serial_omap_stop_tx(struct uart_port *port)
 		serial_out(up, UART_IER, up->ier);
 	}
 
-	if ((up->rs485.flags & SER_RS485_ENABLED) &&
-	    !(up->rs485.flags & SER_RS485_RX_DURING_TX)) {
+	if ((port->rs485.flags & SER_RS485_ENABLED) &&
+	    !(port->rs485.flags & SER_RS485_RX_DURING_TX)) {
 		/*
 		 * Empty the RX FIFO, we are not interested in anything
 		 * received during the half-duplex transmission.
@@ -429,22 +430,22 @@ static void serial_omap_start_tx(struct uart_port *port)
 	pm_runtime_get_sync(up->dev);
 
 	/* Handle RS-485 */
-	if (up->rs485.flags & SER_RS485_ENABLED) {
+	if (port->rs485.flags & SER_RS485_ENABLED) {
 		/* Fire THR interrupts when FIFO is below trigger level */
 		up->scr &= ~OMAP_UART_SCR_TX_EMPTY;
 		serial_out(up, UART_OMAP_SCR, up->scr);
 
 		/* if rts not already enabled */
-		res = (up->rs485.flags & SER_RS485_RTS_ON_SEND) ? 1 : 0;
+		res = (port->rs485.flags & SER_RS485_RTS_ON_SEND) ? 1 : 0;
 		if (gpio_get_value(up->rts_gpio) != res) {
 			gpio_set_value(up->rts_gpio, res);
-			if (up->rs485.delay_rts_before_send > 0)
-				mdelay(up->rs485.delay_rts_before_send);
+			if (port->rs485.delay_rts_before_send > 0)
+				mdelay(port->rs485.delay_rts_before_send);
 		}
 	}
 
-	if ((up->rs485.flags & SER_RS485_ENABLED) &&
-	    !(up->rs485.flags & SER_RS485_RX_DURING_TX))
+	if ((port->rs485.flags & SER_RS485_ENABLED) &&
+	    !(port->rs485.flags & SER_RS485_RX_DURING_TX))
 		serial_omap_stop_rx(port);
 
 	serial_omap_enable_ier_thri(up);
@@ -1355,16 +1356,14 @@ static inline void serial_omap_add_console_port(struct uart_omap_port *up)
 #endif
 
 /* Enable or disable the rs485 support */
-static void
+static int
 serial_omap_config_rs485(struct uart_port *port, struct serial_rs485 *rs485conf)
 {
 	struct uart_omap_port *up = to_uart_omap_port(port);
-	unsigned long flags;
 	unsigned int mode;
 	int val;
 
 	pm_runtime_get_sync(up->dev);
-	spin_lock_irqsave(&up->port.lock, flags);
 
 	/* Disable interrupts from this port */
 	mode = up->ier;
@@ -1372,7 +1371,7 @@ serial_omap_config_rs485(struct uart_port *port, struct serial_rs485 *rs485conf)
 	serial_out(up, UART_IER, 0);
 
 	/* store new config */
-	up->rs485 = *rs485conf;
+	port->rs485 = *rs485conf;
 
 	/*
 	 * Just as a precaution, only allow rs485
@@ -1380,12 +1379,12 @@ serial_omap_config_rs485(struct uart_port *port, struct serial_rs485 *rs485conf)
 	 */
 	if (gpio_is_valid(up->rts_gpio)) {
 		/* enable / disable rts */
-		val = (up->rs485.flags & SER_RS485_ENABLED) ?
+		val = (port->rs485.flags & SER_RS485_ENABLED) ?
 			SER_RS485_RTS_AFTER_SEND : SER_RS485_RTS_ON_SEND;
-		val = (up->rs485.flags & val) ? 1 : 0;
+		val = (port->rs485.flags & val) ? 1 : 0;
 		gpio_set_value(up->rts_gpio, val);
 	} else
-		up->rs485.flags &= ~SER_RS485_ENABLED;
+		port->rs485.flags &= ~SER_RS485_ENABLED;
 
 	/* Enable interrupts */
 	up->ier = mode;
@@ -1394,45 +1393,18 @@ serial_omap_config_rs485(struct uart_port *port, struct serial_rs485 *rs485conf)
 	/* If RS-485 is disabled, make sure the THR interrupt is fired when
 	 * TX FIFO is below the trigger level.
 	 */
-	if (!(up->rs485.flags & SER_RS485_ENABLED) &&
+	if (!(port->rs485.flags & SER_RS485_ENABLED) &&
 	    (up->scr & OMAP_UART_SCR_TX_EMPTY)) {
 		up->scr &= ~OMAP_UART_SCR_TX_EMPTY;
 		serial_out(up, UART_OMAP_SCR, up->scr);
 	}
 
-	spin_unlock_irqrestore(&up->port.lock, flags);
 	pm_runtime_mark_last_busy(up->dev);
 	pm_runtime_put_autosuspend(up->dev);
-}
-
-static int
-serial_omap_ioctl(struct uart_port *port, unsigned int cmd, unsigned long arg)
-{
-	struct serial_rs485 rs485conf;
-
-	switch (cmd) {
-	case TIOCSRS485:
-		if (copy_from_user(&rs485conf, (void __user *) arg,
-					sizeof(rs485conf)))
-			return -EFAULT;
 
-		serial_omap_config_rs485(port, &rs485conf);
-		break;
-
-	case TIOCGRS485:
-		if (copy_to_user((void __user *) arg,
-					&(to_uart_omap_port(port)->rs485),
-					sizeof(rs485conf)))
-			return -EFAULT;
-		break;
-
-	default:
-		return -ENOIOCTLCMD;
-	}
 	return 0;
 }
 
-
 static struct uart_ops serial_omap_pops = {
 	.tx_empty	= serial_omap_tx_empty,
 	.set_mctrl	= serial_omap_set_mctrl,
@@ -1453,7 +1425,6 @@ static struct uart_ops serial_omap_pops = {
 	.request_port	= serial_omap_request_port,
 	.config_port	= serial_omap_config_port,
 	.verify_port	= serial_omap_verify_port,
-	.ioctl		= serial_omap_ioctl,
 #ifdef CONFIG_CONSOLE_POLL
 	.poll_put_char  = serial_omap_poll_put_char,
 	.poll_get_char  = serial_omap_poll_get_char,
@@ -1587,7 +1558,7 @@ static struct omap_uart_port_info *of_get_uart_port_info(struct device *dev)
 static int serial_omap_probe_rs485(struct uart_omap_port *up,
 				   struct device_node *np)
 {
-	struct serial_rs485 *rs485conf = &up->rs485;
+	struct serial_rs485 *rs485conf = &up->port.rs485;
 	u32 rs485_delay[2];
 	enum of_gpio_flags flags;
 	int ret;
@@ -1682,14 +1653,21 @@ static int serial_omap_probe(struct platform_device *pdev)
 	up->port.ops = &serial_omap_pops;
 
 	if (pdev->dev.of_node)
-		up->port.line = of_alias_get_id(pdev->dev.of_node, "serial");
+		ret = of_alias_get_id(pdev->dev.of_node, "serial");
 	else
-		up->port.line = pdev->id;
+		ret = pdev->id;
 
-	if (up->port.line < 0) {
+	if (ret < 0) {
 		dev_err(&pdev->dev, "failed to get alias/pdev id, errno %d\n",
-								up->port.line);
-		ret = -ENODEV;
+			ret);
+		goto err_port_line;
+	}
+	up->port.line = ret;
+
+	if (up->port.line >= OMAP_MAX_HSUART_PORTS) {
+		dev_err(&pdev->dev, "uart ID %d >  MAX %d.\n", up->port.line,
+			OMAP_MAX_HSUART_PORTS);
+		ret = -ENXIO;
 		goto err_port_line;
 	}
 
@@ -1702,6 +1680,7 @@ static int serial_omap_probe(struct platform_device *pdev)
 	up->port.membase = base;
 	up->port.flags = omap_up_info->flags;
 	up->port.uartclk = omap_up_info->uartclk;
+	up->port.rs485_config = serial_omap_config_rs485;
 	if (!up->port.uartclk) {
 		up->port.uartclk = DEFAULT_CLK_SPEED;
 		dev_warn(&pdev->dev,
@@ -1747,8 +1726,6 @@ err_add_port:
 	pm_runtime_disable(&pdev->dev);
 err_rs485:
 err_port_line:
-	dev_err(&pdev->dev, "[UART%d]: failure [%s]: %d\n",
-				pdev->id, __func__, ret);
 	return ret;
 }
 
@@ -1799,7 +1776,7 @@ static void serial_omap_mdr1_errataset(struct uart_omap_port *up, u8 mdr1)
 	}
 }
 
-#ifdef CONFIG_PM_RUNTIME
+#ifdef CONFIG_PM
 static void serial_omap_restore_context(struct uart_omap_port *up)
 {
 	if (up->errata & UART_ERRATA_i202_MDR1_ACCESS)
diff --git a/drivers/tty/serial/pmac_zilog.c b/drivers/tty/serial/pmac_zilog.c
index abbfedb84901..8f515799c9c1 100644
--- a/drivers/tty/serial/pmac_zilog.c
+++ b/drivers/tty/serial/pmac_zilog.c
@@ -1352,7 +1352,8 @@ static int pmz_verify_port(struct uart_port *port, struct serial_struct *ser)
 
 static int pmz_poll_get_char(struct uart_port *port)
 {
-	struct uart_pmac_port *uap = (struct uart_pmac_port *)port;
+	struct uart_pmac_port *uap =
+		container_of(port, struct uart_pmac_port, port);
 	int tries = 2;
 
 	while (tries) {
@@ -1367,7 +1368,8 @@ static int pmz_poll_get_char(struct uart_port *port)
 
 static void pmz_poll_put_char(struct uart_port *port, unsigned char c)
 {
-	struct uart_pmac_port *uap = (struct uart_pmac_port *)port;
+	struct uart_pmac_port *uap =
+		container_of(port, struct uart_pmac_port, port);
 
 	/* Wait for the transmit buffer to empty. */
 	while ((read_zsreg(uap, R0) & Tx_BUF_EMP) == 0)
@@ -1874,7 +1876,6 @@ static struct platform_driver pmz_driver = {
 	.remove		= __exit_p(pmz_detach),
 	.driver		= {
 		.name		= "scc",
-		.owner		= THIS_MODULE,
 	},
 };
 
@@ -1954,7 +1955,8 @@ static void __exit exit_pmz(void)
 
 static void pmz_console_putchar(struct uart_port *port, int ch)
 {
-	struct uart_pmac_port *uap = (struct uart_pmac_port *)port;
+	struct uart_pmac_port *uap =
+		container_of(port, struct uart_pmac_port, port);
 
 	/* Wait for the transmit buffer to empty. */
 	while ((read_zsreg(uap, R0) & Tx_BUF_EMP) == 0)
diff --git a/drivers/tty/serial/pnx8xxx_uart.c b/drivers/tty/serial/pnx8xxx_uart.c
index 2ba24a45c97f..7a3bb9cf1f2e 100644
--- a/drivers/tty/serial/pnx8xxx_uart.c
+++ b/drivers/tty/serial/pnx8xxx_uart.c
@@ -126,7 +126,8 @@ static void pnx8xxx_timeout(unsigned long data)
  */
 static void pnx8xxx_stop_tx(struct uart_port *port)
 {
-	struct pnx8xxx_port *sport = (struct pnx8xxx_port *)port;
+	struct pnx8xxx_port *sport =
+		container_of(port, struct pnx8xxx_port, port);
 	u32 ien;
 
 	/* Disable TX intr */
@@ -142,7 +143,8 @@ static void pnx8xxx_stop_tx(struct uart_port *port)
  */
 static void pnx8xxx_start_tx(struct uart_port *port)
 {
-	struct pnx8xxx_port *sport = (struct pnx8xxx_port *)port;
+	struct pnx8xxx_port *sport =
+		container_of(port, struct pnx8xxx_port, port);
 	u32 ien;
 
 	/* Clear all pending TX intr */
@@ -158,7 +160,8 @@ static void pnx8xxx_start_tx(struct uart_port *port)
  */
 static void pnx8xxx_stop_rx(struct uart_port *port)
 {
-	struct pnx8xxx_port *sport = (struct pnx8xxx_port *)port;
+	struct pnx8xxx_port *sport =
+		container_of(port, struct pnx8xxx_port, port);
 	u32 ien;
 
 	/* Disable RX intr */
@@ -174,7 +177,8 @@ static void pnx8xxx_stop_rx(struct uart_port *port)
  */
 static void pnx8xxx_enable_ms(struct uart_port *port)
 {
-	struct pnx8xxx_port *sport = (struct pnx8xxx_port *)port;
+	struct pnx8xxx_port *sport =
+		container_of(port, struct pnx8xxx_port, port);
 
 	mod_timer(&sport->timer, jiffies);
 }
@@ -313,14 +317,16 @@ static irqreturn_t pnx8xxx_int(int irq, void *dev_id)
  */
 static unsigned int pnx8xxx_tx_empty(struct uart_port *port)
 {
-	struct pnx8xxx_port *sport = (struct pnx8xxx_port *)port;
+	struct pnx8xxx_port *sport =
+		container_of(port, struct pnx8xxx_port, port);
 
 	return serial_in(sport, PNX8XXX_FIFO) & PNX8XXX_UART_FIFO_TXFIFO_STA ? 0 : TIOCSER_TEMT;
 }
 
 static unsigned int pnx8xxx_get_mctrl(struct uart_port *port)
 {
-	struct pnx8xxx_port *sport = (struct pnx8xxx_port *)port;
+	struct pnx8xxx_port *sport =
+		container_of(port, struct pnx8xxx_port, port);
 	unsigned int mctrl = TIOCM_DSR;
 	unsigned int msr;
 
@@ -347,7 +353,8 @@ static void pnx8xxx_set_mctrl(struct uart_port *port, unsigned int mctrl)
  */
 static void pnx8xxx_break_ctl(struct uart_port *port, int break_state)
 {
-	struct pnx8xxx_port *sport = (struct pnx8xxx_port *)port;
+	struct pnx8xxx_port *sport =
+		container_of(port, struct pnx8xxx_port, port);
 	unsigned long flags;
 	unsigned int lcr;
 
@@ -363,7 +370,8 @@ static void pnx8xxx_break_ctl(struct uart_port *port, int break_state)
 
 static int pnx8xxx_startup(struct uart_port *port)
 {
-	struct pnx8xxx_port *sport = (struct pnx8xxx_port *)port;
+	struct pnx8xxx_port *sport =
+		container_of(port, struct pnx8xxx_port, port);
 	int retval;
 
 	/*
@@ -397,7 +405,8 @@ static int pnx8xxx_startup(struct uart_port *port)
 
 static void pnx8xxx_shutdown(struct uart_port *port)
 {
-	struct pnx8xxx_port *sport = (struct pnx8xxx_port *)port;
+	struct pnx8xxx_port *sport =
+		container_of(port, struct pnx8xxx_port, port);
 	int lcr;
 
 	/*
@@ -434,7 +443,8 @@ static void
 pnx8xxx_set_termios(struct uart_port *port, struct ktermios *termios,
 		   struct ktermios *old)
 {
-	struct pnx8xxx_port *sport = (struct pnx8xxx_port *)port;
+	struct pnx8xxx_port *sport =
+		container_of(port, struct pnx8xxx_port, port);
 	unsigned long flags;
 	unsigned int lcr_fcr, old_ien, baud, quot;
 	unsigned int old_csize = old ? old->c_cflag & CSIZE : CS8;
@@ -551,7 +561,8 @@ pnx8xxx_set_termios(struct uart_port *port, struct ktermios *termios,
 
 static const char *pnx8xxx_type(struct uart_port *port)
 {
-	struct pnx8xxx_port *sport = (struct pnx8xxx_port *)port;
+	struct pnx8xxx_port *sport =
+		container_of(port, struct pnx8xxx_port, port);
 
 	return sport->port.type == PORT_PNX8XXX ? "PNX8XXX" : NULL;
 }
@@ -561,7 +572,8 @@ static const char *pnx8xxx_type(struct uart_port *port)
  */
 static void pnx8xxx_release_port(struct uart_port *port)
 {
-	struct pnx8xxx_port *sport = (struct pnx8xxx_port *)port;
+	struct pnx8xxx_port *sport =
+		container_of(port, struct pnx8xxx_port, port);
 
 	release_mem_region(sport->port.mapbase, UART_PORT_SIZE);
 }
@@ -571,7 +583,8 @@ static void pnx8xxx_release_port(struct uart_port *port)
  */
 static int pnx8xxx_request_port(struct uart_port *port)
 {
-	struct pnx8xxx_port *sport = (struct pnx8xxx_port *)port;
+	struct pnx8xxx_port *sport =
+		container_of(port, struct pnx8xxx_port, port);
 	return request_mem_region(sport->port.mapbase, UART_PORT_SIZE,
 			"pnx8xxx-uart") != NULL ? 0 : -EBUSY;
 }
@@ -581,7 +594,8 @@ static int pnx8xxx_request_port(struct uart_port *port)
  */
 static void pnx8xxx_config_port(struct uart_port *port, int flags)
 {
-	struct pnx8xxx_port *sport = (struct pnx8xxx_port *)port;
+	struct pnx8xxx_port *sport =
+		container_of(port, struct pnx8xxx_port, port);
 
 	if (flags & UART_CONFIG_TYPE &&
 	    pnx8xxx_request_port(&sport->port) == 0)
@@ -596,7 +610,8 @@ static void pnx8xxx_config_port(struct uart_port *port, int flags)
 static int
 pnx8xxx_verify_port(struct uart_port *port, struct serial_struct *ser)
 {
-	struct pnx8xxx_port *sport = (struct pnx8xxx_port *)port;
+	struct pnx8xxx_port *sport =
+		container_of(port, struct pnx8xxx_port,	port);
 	int ret = 0;
 
 	if (ser->type != PORT_UNKNOWN && ser->type != PORT_PNX8XXX)
@@ -662,7 +677,8 @@ static void __init pnx8xxx_init_ports(void)
 
 static void pnx8xxx_console_putchar(struct uart_port *port, int ch)
 {
-	struct pnx8xxx_port *sport = (struct pnx8xxx_port *)port;
+	struct pnx8xxx_port *sport =
+		container_of(port, struct pnx8xxx_port, port);
 	int status;
 
 	do {
@@ -813,7 +829,6 @@ static int pnx8xxx_serial_remove(struct platform_device *pdev)
 static struct platform_driver pnx8xxx_serial_driver = {
 	.driver		= {
 		.name	= "pnx8xxx-uart",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= pnx8xxx_serial_probe,
 	.remove		= pnx8xxx_serial_remove,
diff --git a/drivers/tty/serial/pxa.c b/drivers/tty/serial/pxa.c
index 21b7d8b86493..d5d062694bd3 100644
--- a/drivers/tty/serial/pxa.c
+++ b/drivers/tty/serial/pxa.c
@@ -223,6 +223,7 @@ static void serial_pxa_start_tx(struct uart_port *port)
 	}
 }
 
+/* should hold up->port.lock */
 static inline void check_modem_status(struct uart_pxa_port *up)
 {
 	int status;
@@ -255,12 +256,14 @@ static inline irqreturn_t serial_pxa_irq(int irq, void *dev_id)
 	iir = serial_in(up, UART_IIR);
 	if (iir & UART_IIR_NO_INT)
 		return IRQ_NONE;
+	spin_lock(&up->port.lock);
 	lsr = serial_in(up, UART_LSR);
 	if (lsr & UART_LSR_DR)
 		receive_chars(up, &lsr);
 	check_modem_status(up);
 	if (lsr & UART_LSR_THRE)
 		transmit_chars(up);
+	spin_unlock(&up->port.lock);
 	return IRQ_HANDLED;
 }
 
@@ -930,7 +933,6 @@ static struct platform_driver serial_pxa_driver = {
 
 	.driver		= {
 	        .name	= "pxa2xx-uart",
-		.owner	= THIS_MODULE,
 #ifdef CONFIG_PM
 		.pm	= &serial_pxa_pm_ops,
 #endif
diff --git a/drivers/tty/serial/sa1100.c b/drivers/tty/serial/sa1100.c
index 753d4525b367..fd3d1329d48c 100644
--- a/drivers/tty/serial/sa1100.c
+++ b/drivers/tty/serial/sa1100.c
@@ -142,7 +142,8 @@ static void sa1100_timeout(unsigned long data)
  */
 static void sa1100_stop_tx(struct uart_port *port)
 {
-	struct sa1100_port *sport = (struct sa1100_port *)port;
+	struct sa1100_port *sport =
+		container_of(port, struct sa1100_port, port);
 	u32 utcr3;
 
 	utcr3 = UART_GET_UTCR3(sport);
@@ -155,7 +156,8 @@ static void sa1100_stop_tx(struct uart_port *port)
  */
 static void sa1100_start_tx(struct uart_port *port)
 {
-	struct sa1100_port *sport = (struct sa1100_port *)port;
+	struct sa1100_port *sport =
+		container_of(port, struct sa1100_port, port);
 	u32 utcr3;
 
 	utcr3 = UART_GET_UTCR3(sport);
@@ -168,7 +170,8 @@ static void sa1100_start_tx(struct uart_port *port)
  */
 static void sa1100_stop_rx(struct uart_port *port)
 {
-	struct sa1100_port *sport = (struct sa1100_port *)port;
+	struct sa1100_port *sport =
+		container_of(port, struct sa1100_port, port);
 	u32 utcr3;
 
 	utcr3 = UART_GET_UTCR3(sport);
@@ -180,7 +183,8 @@ static void sa1100_stop_rx(struct uart_port *port)
  */
 static void sa1100_enable_ms(struct uart_port *port)
 {
-	struct sa1100_port *sport = (struct sa1100_port *)port;
+	struct sa1100_port *sport =
+		container_of(port, struct sa1100_port, port);
 
 	mod_timer(&sport->timer, jiffies);
 }
@@ -323,7 +327,8 @@ static irqreturn_t sa1100_int(int irq, void *dev_id)
  */
 static unsigned int sa1100_tx_empty(struct uart_port *port)
 {
-	struct sa1100_port *sport = (struct sa1100_port *)port;
+	struct sa1100_port *sport =
+		container_of(port, struct sa1100_port, port);
 
 	return UART_GET_UTSR1(sport) & UTSR1_TBY ? 0 : TIOCSER_TEMT;
 }
@@ -342,7 +347,8 @@ static void sa1100_set_mctrl(struct uart_port *port, unsigned int mctrl)
  */
 static void sa1100_break_ctl(struct uart_port *port, int break_state)
 {
-	struct sa1100_port *sport = (struct sa1100_port *)port;
+	struct sa1100_port *sport =
+		container_of(port, struct sa1100_port, port);
 	unsigned long flags;
 	unsigned int utcr3;
 
@@ -358,7 +364,8 @@ static void sa1100_break_ctl(struct uart_port *port, int break_state)
 
 static int sa1100_startup(struct uart_port *port)
 {
-	struct sa1100_port *sport = (struct sa1100_port *)port;
+	struct sa1100_port *sport =
+		container_of(port, struct sa1100_port, port);
 	int retval;
 
 	/*
@@ -387,7 +394,8 @@ static int sa1100_startup(struct uart_port *port)
 
 static void sa1100_shutdown(struct uart_port *port)
 {
-	struct sa1100_port *sport = (struct sa1100_port *)port;
+	struct sa1100_port *sport =
+		container_of(port, struct sa1100_port, port);
 
 	/*
 	 * Stop our timer.
@@ -409,7 +417,8 @@ static void
 sa1100_set_termios(struct uart_port *port, struct ktermios *termios,
 		   struct ktermios *old)
 {
-	struct sa1100_port *sport = (struct sa1100_port *)port;
+	struct sa1100_port *sport =
+		container_of(port, struct sa1100_port, port);
 	unsigned long flags;
 	unsigned int utcr0, old_utcr3, baud, quot;
 	unsigned int old_csize = old ? old->c_cflag & CSIZE : CS8;
@@ -512,7 +521,8 @@ sa1100_set_termios(struct uart_port *port, struct ktermios *termios,
 
 static const char *sa1100_type(struct uart_port *port)
 {
-	struct sa1100_port *sport = (struct sa1100_port *)port;
+	struct sa1100_port *sport =
+		container_of(port, struct sa1100_port, port);
 
 	return sport->port.type == PORT_SA1100 ? "SA1100" : NULL;
 }
@@ -522,7 +532,8 @@ static const char *sa1100_type(struct uart_port *port)
  */
 static void sa1100_release_port(struct uart_port *port)
 {
-	struct sa1100_port *sport = (struct sa1100_port *)port;
+	struct sa1100_port *sport =
+		container_of(port, struct sa1100_port, port);
 
 	release_mem_region(sport->port.mapbase, UART_PORT_SIZE);
 }
@@ -532,7 +543,8 @@ static void sa1100_release_port(struct uart_port *port)
  */
 static int sa1100_request_port(struct uart_port *port)
 {
-	struct sa1100_port *sport = (struct sa1100_port *)port;
+	struct sa1100_port *sport =
+		container_of(port, struct sa1100_port, port);
 
 	return request_mem_region(sport->port.mapbase, UART_PORT_SIZE,
 			"sa11x0-uart") != NULL ? 0 : -EBUSY;
@@ -543,7 +555,8 @@ static int sa1100_request_port(struct uart_port *port)
  */
 static void sa1100_config_port(struct uart_port *port, int flags)
 {
-	struct sa1100_port *sport = (struct sa1100_port *)port;
+	struct sa1100_port *sport =
+		container_of(port, struct sa1100_port, port);
 
 	if (flags & UART_CONFIG_TYPE &&
 	    sa1100_request_port(&sport->port) == 0)
@@ -558,7 +571,8 @@ static void sa1100_config_port(struct uart_port *port, int flags)
 static int
 sa1100_verify_port(struct uart_port *port, struct serial_struct *ser)
 {
-	struct sa1100_port *sport = (struct sa1100_port *)port;
+	struct sa1100_port *sport =
+		container_of(port, struct sa1100_port, port);
 	int ret = 0;
 
 	if (ser->type != PORT_UNKNOWN && ser->type != PORT_SA1100)
@@ -691,7 +705,8 @@ void __init sa1100_register_uart(int idx, int port)
 #ifdef CONFIG_SERIAL_SA1100_CONSOLE
 static void sa1100_console_putchar(struct uart_port *port, int ch)
 {
-	struct sa1100_port *sport = (struct sa1100_port *)port;
+	struct sa1100_port *sport =
+		container_of(port, struct sa1100_port, port);
 
 	while (!(UART_GET_UTSR1(sport) & UTSR1_TNF))
 		barrier();
@@ -883,7 +898,6 @@ static struct platform_driver sa11x0_serial_driver = {
 	.resume		= sa1100_serial_resume,
 	.driver		= {
 		.name	= "sa11x0-uart",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/tty/serial/samsung.c b/drivers/tty/serial/samsung.c
index c78f43a481ce..19273e31d224 100644
--- a/drivers/tty/serial/samsung.c
+++ b/drivers/tty/serial/samsung.c
@@ -199,12 +199,14 @@ static void s3c24xx_serial_stop_rx(struct uart_port *port)
 	}
 }
 
-static inline struct s3c24xx_uart_info *s3c24xx_port_to_info(struct uart_port *port)
+static inline struct s3c24xx_uart_info
+	*s3c24xx_port_to_info(struct uart_port *port)
 {
 	return to_ourport(port)->info;
 }
 
-static inline struct s3c2410_uartcfg *s3c24xx_port_to_cfg(struct uart_port *port)
+static inline struct s3c2410_uartcfg
+	*s3c24xx_port_to_cfg(struct uart_port *port)
 {
 	struct s3c24xx_uart_port *ourport;
 
@@ -237,7 +239,7 @@ s3c24xx_serial_rx_chars(int irq, void *dev_id)
 	struct uart_port *port = &ourport->port;
 	unsigned int ufcon, ch, flag, ufstat, uerstat;
 	unsigned long flags;
-	int max_count = 64;
+	int max_count = port->fifosize;
 
 	spin_lock_irqsave(&port->lock, flags);
 
@@ -311,14 +313,14 @@ s3c24xx_serial_rx_chars(int irq, void *dev_id)
 		uart_insert_char(port, uerstat, S3C2410_UERSTAT_OVERRUN,
 				 ch, flag);
 
- ignore_char:
+ignore_char:
 		continue;
 	}
 
 	spin_unlock_irqrestore(&port->lock, flags);
 	tty_flip_buffer_push(&port->state->port);
 
- out:
+out:
 	return IRQ_HANDLED;
 }
 
@@ -328,7 +330,7 @@ static irqreturn_t s3c24xx_serial_tx_chars(int irq, void *id)
 	struct uart_port *port = &ourport->port;
 	struct circ_buf *xmit = &port->state->xmit;
 	unsigned long flags;
-	int count = 256;
+	int count = port->fifosize;
 
 	spin_lock_irqsave(&port->lock, flags);
 
@@ -368,7 +370,7 @@ static irqreturn_t s3c24xx_serial_tx_chars(int irq, void *id)
 	if (uart_circ_empty(xmit))
 		s3c24xx_serial_stop_tx(port);
 
- out:
+out:
 	spin_unlock_irqrestore(&port->lock, flags);
 	return IRQ_HANDLED;
 }
@@ -519,7 +521,7 @@ static int s3c24xx_serial_startup(struct uart_port *port)
 
 	return ret;
 
- err:
+err:
 	s3c24xx_serial_shutdown(port);
 	return ret;
 }
@@ -559,11 +561,15 @@ static void s3c24xx_serial_pm(struct uart_port *port, unsigned int level,
 			      unsigned int old)
 {
 	struct s3c24xx_uart_port *ourport = to_ourport(port);
+	int timeout = 10000;
 
 	ourport->pm_level = level;
 
 	switch (level) {
 	case 3:
+		while (--timeout && !s3c24xx_serial_txempty_nofifo(port))
+			udelay(100);
+
 		if (!IS_ERR(ourport->baudclk))
 			clk_disable_unprepare(ourport->baudclk);
 
@@ -841,8 +847,8 @@ static void s3c24xx_serial_set_termios(struct uart_port *port,
 	 */
 	port->read_status_mask = S3C2410_UERSTAT_OVERRUN;
 	if (termios->c_iflag & INPCK)
-		port->read_status_mask |= S3C2410_UERSTAT_FRAME | S3C2410_UERSTAT_PARITY;
-
+		port->read_status_mask |= S3C2410_UERSTAT_FRAME |
+			S3C2410_UERSTAT_PARITY;
 	/*
 	 * Which character status flags should we ignore?
 	 */
@@ -969,10 +975,13 @@ static struct uart_driver s3c24xx_uart_drv = {
 	.minor		= S3C24XX_SERIAL_MINOR,
 };
 
-static struct s3c24xx_uart_port s3c24xx_serial_ports[CONFIG_SERIAL_SAMSUNG_UARTS] = {
+#define __PORT_LOCK_UNLOCKED(i) \
+	__SPIN_LOCK_UNLOCKED(s3c24xx_serial_ports[i].port.lock)
+static struct s3c24xx_uart_port
+s3c24xx_serial_ports[CONFIG_SERIAL_SAMSUNG_UARTS] = {
 	[0] = {
 		.port = {
-			.lock		= __SPIN_LOCK_UNLOCKED(s3c24xx_serial_ports[0].port.lock),
+			.lock		= __PORT_LOCK_UNLOCKED(0),
 			.iotype		= UPIO_MEM,
 			.uartclk	= 0,
 			.fifosize	= 16,
@@ -983,7 +992,7 @@ static struct s3c24xx_uart_port s3c24xx_serial_ports[CONFIG_SERIAL_SAMSUNG_UARTS
 	},
 	[1] = {
 		.port = {
-			.lock		= __SPIN_LOCK_UNLOCKED(s3c24xx_serial_ports[1].port.lock),
+			.lock		= __PORT_LOCK_UNLOCKED(1),
 			.iotype		= UPIO_MEM,
 			.uartclk	= 0,
 			.fifosize	= 16,
@@ -996,7 +1005,7 @@ static struct s3c24xx_uart_port s3c24xx_serial_ports[CONFIG_SERIAL_SAMSUNG_UARTS
 
 	[2] = {
 		.port = {
-			.lock		= __SPIN_LOCK_UNLOCKED(s3c24xx_serial_ports[2].port.lock),
+			.lock		= __PORT_LOCK_UNLOCKED(2),
 			.iotype		= UPIO_MEM,
 			.uartclk	= 0,
 			.fifosize	= 16,
@@ -1009,7 +1018,7 @@ static struct s3c24xx_uart_port s3c24xx_serial_ports[CONFIG_SERIAL_SAMSUNG_UARTS
 #if CONFIG_SERIAL_SAMSUNG_UARTS > 3
 	[3] = {
 		.port = {
-			.lock		= __SPIN_LOCK_UNLOCKED(s3c24xx_serial_ports[3].port.lock),
+			.lock		= __PORT_LOCK_UNLOCKED(3),
 			.iotype		= UPIO_MEM,
 			.uartclk	= 0,
 			.fifosize	= 16,
@@ -1020,6 +1029,7 @@ static struct s3c24xx_uart_port s3c24xx_serial_ports[CONFIG_SERIAL_SAMSUNG_UARTS
 	}
 #endif
 };
+#undef __PORT_LOCK_UNLOCKED
 
 /* s3c24xx_serial_resetport
  *
@@ -1102,11 +1112,12 @@ static int s3c24xx_serial_cpufreq_transition(struct notifier_block *nb,
 		s3c24xx_serial_set_termios(uport, termios, NULL);
 	}
 
- exit:
+exit:
 	return 0;
 }
 
-static inline int s3c24xx_serial_cpufreq_register(struct s3c24xx_uart_port *port)
+static inline int
+s3c24xx_serial_cpufreq_register(struct s3c24xx_uart_port *port)
 {
 	port->freq_transition.notifier_call = s3c24xx_serial_cpufreq_transition;
 
@@ -1114,19 +1125,22 @@ static inline int s3c24xx_serial_cpufreq_register(struct s3c24xx_uart_port *port
 					 CPUFREQ_TRANSITION_NOTIFIER);
 }
 
-static inline void s3c24xx_serial_cpufreq_deregister(struct s3c24xx_uart_port *port)
+static inline void
+s3c24xx_serial_cpufreq_deregister(struct s3c24xx_uart_port *port)
 {
 	cpufreq_unregister_notifier(&port->freq_transition,
 				    CPUFREQ_TRANSITION_NOTIFIER);
 }
 
 #else
-static inline int s3c24xx_serial_cpufreq_register(struct s3c24xx_uart_port *port)
+static inline int
+s3c24xx_serial_cpufreq_register(struct s3c24xx_uart_port *port)
 {
 	return 0;
 }
 
-static inline void s3c24xx_serial_cpufreq_deregister(struct s3c24xx_uart_port *port)
+static inline void
+s3c24xx_serial_cpufreq_deregister(struct s3c24xx_uart_port *port)
 {
 }
 #endif
@@ -1226,24 +1240,6 @@ static int s3c24xx_serial_init_port(struct s3c24xx_uart_port *ourport,
 	return 0;
 }
 
-#ifdef CONFIG_SAMSUNG_CLOCK
-static ssize_t s3c24xx_serial_show_clksrc(struct device *dev,
-					  struct device_attribute *attr,
-					  char *buf)
-{
-	struct uart_port *port = s3c24xx_dev_to_port(dev);
-	struct s3c24xx_uart_port *ourport = to_ourport(port);
-
-	if (IS_ERR(ourport->baudclk))
-		return -EINVAL;
-
-	return snprintf(buf, PAGE_SIZE, "* %s\n",
-			ourport->baudclk->name ?: "(null)");
-}
-
-static DEVICE_ATTR(clock_source, S_IRUGO, s3c24xx_serial_show_clksrc, NULL);
-#endif
-
 /* Device driver serial port probe */
 
 static const struct of_device_id s3c24xx_uart_dt_match[];
@@ -1296,11 +1292,10 @@ static int s3c24xx_serial_probe(struct platform_device *pdev)
 		of_property_read_u32(np,
 			"samsung,uart-fifosize", &ourport->port.fifosize);
 
-	if (!ourport->port.fifosize) {
-		ourport->port.fifosize = (ourport->info->fifosize) ?
-			ourport->info->fifosize :
-			ourport->drv_data->fifosize[index];
-	}
+	if (ourport->drv_data->fifosize[index])
+		ourport->port.fifosize = ourport->drv_data->fifosize[index];
+	else if (ourport->info->fifosize)
+		ourport->port.fifosize = ourport->info->fifosize;
 
 	probe_index++;
 
@@ -1329,12 +1324,6 @@ static int s3c24xx_serial_probe(struct platform_device *pdev)
 	 */
 	clk_disable_unprepare(ourport->clk);
 
-#ifdef CONFIG_SAMSUNG_CLOCK
-	ret = device_create_file(&pdev->dev, &dev_attr_clock_source);
-	if (ret < 0)
-		dev_err(&pdev->dev, "failed to add clock source attr.\n");
-#endif
-
 	ret = s3c24xx_serial_cpufreq_register(ourport);
 	if (ret < 0)
 		dev_err(&pdev->dev, "failed to add cpufreq notifier\n");
@@ -1348,9 +1337,6 @@ static int s3c24xx_serial_remove(struct platform_device *dev)
 
 	if (port) {
 		s3c24xx_serial_cpufreq_deregister(to_ourport(port));
-#ifdef CONFIG_SAMSUNG_CLOCK
-		device_remove_file(&dev->dev, &dev_attr_clock_source);
-#endif
 		uart_remove_one_port(&s3c24xx_uart_drv, port);
 	}
 
@@ -1848,7 +1834,6 @@ static struct platform_driver samsung_serial_driver = {
 	.id_table	= s3c24xx_serial_driver_ids,
 	.driver		= {
 		.name	= "samsung-uart",
-		.owner	= THIS_MODULE,
 		.pm	= SERIAL_SAMSUNG_PM_OPS,
 		.of_match_table	= of_match_ptr(s3c24xx_uart_dt_match),
 	},
diff --git a/drivers/tty/serial/sc16is7xx.c b/drivers/tty/serial/sc16is7xx.c
index 6246820d7f05..df9a384dfbda 100644
--- a/drivers/tty/serial/sc16is7xx.c
+++ b/drivers/tty/serial/sc16is7xx.c
@@ -304,8 +304,6 @@ struct sc16is7xx_one {
 	struct uart_port		port;
 	struct work_struct		tx_work;
 	struct work_struct		md_work;
-
-	struct serial_rs485		rs485;
 };
 
 struct sc16is7xx_port {
@@ -657,15 +655,15 @@ static void sc16is7xx_stop_tx(struct uart_port* port)
 	struct circ_buf *xmit = &one->port.state->xmit;
 
 	/* handle rs485 */
-	if (one->rs485.flags & SER_RS485_ENABLED) {
+	if (port->rs485.flags & SER_RS485_ENABLED) {
 		/* do nothing if current tx not yet completed */
 		int lsr = sc16is7xx_port_read(port, SC16IS7XX_LSR_REG);
 		if (!(lsr & SC16IS7XX_LSR_TEMT_BIT))
 			return;
 
 		if (uart_circ_empty(xmit) &&
-		    (one->rs485.delay_rts_after_send > 0))
-			mdelay(one->rs485.delay_rts_after_send);
+		    (port->rs485.delay_rts_after_send > 0))
+			mdelay(port->rs485.delay_rts_after_send);
 	}
 
 	sc16is7xx_port_update(port, SC16IS7XX_IER_REG,
@@ -688,9 +686,9 @@ static void sc16is7xx_start_tx(struct uart_port *port)
 	struct sc16is7xx_one *one = to_sc16is7xx_one(port, port);
 
 	/* handle rs485 */
-	if ((one->rs485.flags & SER_RS485_ENABLED) &&
-	    (one->rs485.delay_rts_before_send > 0)) {
-		mdelay(one->rs485.delay_rts_before_send);
+	if ((port->rs485.flags & SER_RS485_ENABLED) &&
+	    (port->rs485.delay_rts_before_send > 0)) {
+		mdelay(port->rs485.delay_rts_before_send);
 	}
 
 	if (!work_pending(&one->tx_work))
@@ -830,51 +828,20 @@ static void sc16is7xx_set_termios(struct uart_port *port,
 	uart_update_timeout(port, termios->c_cflag, baud);
 }
 
-#if defined(TIOCSRS485) && defined(TIOCGRS485)
-static void sc16is7xx_config_rs485(struct uart_port *port,
+static int sc16is7xx_config_rs485(struct uart_port *port,
 				   struct serial_rs485 *rs485)
 {
-	struct sc16is7xx_one *one = to_sc16is7xx_one(port, port);
-
-	one->rs485 = *rs485;
-
-	if (one->rs485.flags & SER_RS485_ENABLED) {
+	if (port->rs485.flags & SER_RS485_ENABLED)
 		sc16is7xx_port_update(port, SC16IS7XX_EFCR_REG,
 				      SC16IS7XX_EFCR_AUTO_RS485_BIT,
 				      SC16IS7XX_EFCR_AUTO_RS485_BIT);
-	} else {
+	else
 		sc16is7xx_port_update(port, SC16IS7XX_EFCR_REG,
 				      SC16IS7XX_EFCR_AUTO_RS485_BIT,
 				      0);
-	}
-}
-#endif
-
-static int sc16is7xx_ioctl(struct uart_port *port, unsigned int cmd,
-			   unsigned long arg)
-{
-#if defined(TIOCSRS485) && defined(TIOCGRS485)
-	struct serial_rs485 rs485;
+	port->rs485 = *rs485;
 
-	switch (cmd) {
-	case TIOCSRS485:
-		if (copy_from_user(&rs485, (void __user *)arg, sizeof(rs485)))
-			return -EFAULT;
-
-		sc16is7xx_config_rs485(port, &rs485);
-		return 0;
-	case TIOCGRS485:
-		if (copy_to_user((void __user *)arg,
-				 &(to_sc16is7xx_one(port, port)->rs485),
-				 sizeof(rs485)))
-			return -EFAULT;
-		return 0;
-	default:
-		break;
-	}
-#endif
-
-	return -ENOIOCTLCMD;
+	return 0;
 }
 
 static int sc16is7xx_startup(struct uart_port *port)
@@ -1000,7 +967,6 @@ static const struct uart_ops sc16is7xx_ops = {
 	.release_port	= sc16is7xx_null_void,
 	.config_port	= sc16is7xx_config_port,
 	.verify_port	= sc16is7xx_verify_port,
-	.ioctl		= sc16is7xx_ioctl,
 	.pm		= sc16is7xx_pm,
 };
 
@@ -1130,6 +1096,7 @@ static int sc16is7xx_probe(struct device *dev,
 		s->p[i].port.flags	= UPF_FIXED_TYPE | UPF_LOW_LATENCY;
 		s->p[i].port.iotype	= UPIO_PORT;
 		s->p[i].port.uartclk	= freq;
+		s->p[i].port.rs485_config = sc16is7xx_config_rs485;
 		s->p[i].port.ops	= &sc16is7xx_ops;
 		/* Disable all interrupts */
 		sc16is7xx_port_write(&s->p[i].port, SC16IS7XX_IER_REG, 0);
diff --git a/drivers/tty/serial/sccnxp.c b/drivers/tty/serial/sccnxp.c
index 75850f70b479..fcf803ffad19 100644
--- a/drivers/tty/serial/sccnxp.c
+++ b/drivers/tty/serial/sccnxp.c
@@ -1012,7 +1012,6 @@ static int sccnxp_remove(struct platform_device *pdev)
 static struct platform_driver sccnxp_uart_driver = {
 	.driver = {
 		.name	= SCCNXP_NAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= sccnxp_probe,
 	.remove		= sccnxp_remove,
diff --git a/drivers/tty/serial/serial-tegra.c b/drivers/tty/serial/serial-tegra.c
index 53d7c31ce098..48e6e41636b2 100644
--- a/drivers/tty/serial/serial-tegra.c
+++ b/drivers/tty/serial/serial-tegra.c
@@ -319,16 +319,16 @@ static char tegra_uart_decode_rx_error(struct tegra_uart_port *tup,
 	if (unlikely(lsr & TEGRA_UART_LSR_ANY)) {
 		if (lsr & UART_LSR_OE) {
 			/* Overrrun error */
-			flag |= TTY_OVERRUN;
+			flag = TTY_OVERRUN;
 			tup->uport.icount.overrun++;
 			dev_err(tup->uport.dev, "Got overrun errors\n");
 		} else if (lsr & UART_LSR_PE) {
 			/* Parity error */
-			flag |= TTY_PARITY;
+			flag = TTY_PARITY;
 			tup->uport.icount.parity++;
 			dev_err(tup->uport.dev, "Got Parity errors\n");
 		} else if (lsr & UART_LSR_FE) {
-			flag |= TTY_FRAME;
+			flag = TTY_FRAME;
 			tup->uport.icount.frame++;
 			dev_err(tup->uport.dev, "Got frame errors\n");
 		} else if (lsr & UART_LSR_BI) {
@@ -1034,6 +1034,20 @@ fail_rx_dma:
 	return ret;
 }
 
+/*
+ * Flush any TX data submitted for DMA and PIO. Called when the
+ * TX circular buffer is reset.
+ */
+static void tegra_uart_flush_buffer(struct uart_port *u)
+{
+	struct tegra_uart_port *tup = to_tegra_uport(u);
+
+	tup->tx_bytes = 0;
+	if (tup->tx_dma_chan)
+		dmaengine_terminate_all(tup->tx_dma_chan);
+	return;
+}
+
 static void tegra_uart_shutdown(struct uart_port *u)
 {
 	struct tegra_uart_port *tup = to_tegra_uport(u);
@@ -1046,6 +1060,8 @@ static void tegra_uart_shutdown(struct uart_port *u)
 	tegra_uart_dma_channel_free(tup, true);
 	tegra_uart_dma_channel_free(tup, false);
 	free_irq(u->irq, tup);
+
+	tegra_uart_flush_buffer(u);
 }
 
 static void tegra_uart_enable_ms(struct uart_port *u)
@@ -1174,20 +1190,6 @@ static void tegra_uart_set_termios(struct uart_port *u,
 	return;
 }
 
-/*
- * Flush any TX data submitted for DMA and PIO. Called when the
- * TX circular buffer is reset.
- */
-static void tegra_uart_flush_buffer(struct uart_port *u)
-{
-	struct tegra_uart_port *tup = to_tegra_uport(u);
-
-	tup->tx_bytes = 0;
-	if (tup->tx_dma_chan)
-		dmaengine_terminate_all(tup->tx_dma_chan);
-	return;
-}
-
 static const char *tegra_uart_type(struct uart_port *u)
 {
 	return TEGRA_UART_TYPE;
diff --git a/drivers/tty/serial/serial_core.c b/drivers/tty/serial/serial_core.c
index eaeb9a02c7fe..57ca61b14670 100644
--- a/drivers/tty/serial/serial_core.c
+++ b/drivers/tty/serial/serial_core.c
@@ -61,7 +61,7 @@ static void uart_port_shutdown(struct tty_port *port);
 
 static int uart_dcd_enabled(struct uart_port *uport)
 {
-	return uport->status & UPSTAT_DCD_ENABLE;
+	return !!(uport->status & UPSTAT_DCD_ENABLE);
 }
 
 /*
@@ -436,7 +436,7 @@ uart_get_divisor(struct uart_port *port, unsigned int baud)
 
 EXPORT_SYMBOL(uart_get_divisor);
 
-/* FIXME: Consistent locking policy */
+/* Caller holds port mutex */
 static void uart_change_speed(struct tty_struct *tty, struct uart_state *state,
 					struct ktermios *old_termios)
 {
@@ -537,9 +537,10 @@ static int uart_write(struct tty_struct *tty,
 		count -= c;
 		ret += c;
 	}
+
+	__uart_start(tty);
 	spin_unlock_irqrestore(&port->lock, flags);
 
-	uart_start(tty);
 	return ret;
 }
 
@@ -618,7 +619,7 @@ static void uart_throttle(struct tty_struct *tty)
 {
 	struct uart_state *state = tty->driver_data;
 	struct uart_port *port = state->uart_port;
-	uint32_t mask = 0;
+	upf_t mask = 0;
 
 	if (I_IXOFF(tty))
 		mask |= UPF_SOFT_FLOW;
@@ -641,7 +642,7 @@ static void uart_unthrottle(struct tty_struct *tty)
 {
 	struct uart_state *state = tty->driver_data;
 	struct uart_port *port = state->uart_port;
-	uint32_t mask = 0;
+	upf_t mask = 0;
 
 	if (I_IXOFF(tty))
 		mask |= UPF_SOFT_FLOW;
@@ -1151,6 +1152,47 @@ static int uart_get_icount(struct tty_struct *tty,
 	return 0;
 }
 
+static int uart_get_rs485_config(struct uart_port *port,
+			 struct serial_rs485 __user *rs485)
+{
+	unsigned long flags;
+	struct serial_rs485 aux;
+
+	spin_lock_irqsave(&port->lock, flags);
+	aux = port->rs485;
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	if (copy_to_user(rs485, &aux, sizeof(aux)))
+		return -EFAULT;
+
+	return 0;
+}
+
+static int uart_set_rs485_config(struct uart_port *port,
+			 struct serial_rs485 __user *rs485_user)
+{
+	struct serial_rs485 rs485;
+	int ret;
+	unsigned long flags;
+
+	if (!port->rs485_config)
+		return -ENOIOCTLCMD;
+
+	if (copy_from_user(&rs485, rs485_user, sizeof(*rs485_user)))
+		return -EFAULT;
+
+	spin_lock_irqsave(&port->lock, flags);
+	ret = port->rs485_config(port, &rs485);
+	spin_unlock_irqrestore(&port->lock, flags);
+	if (ret)
+		return ret;
+
+	if (copy_to_user(rs485_user, &port->rs485, sizeof(port->rs485)))
+		return -EFAULT;
+
+	return 0;
+}
+
 /*
  * Called via sys_ioctl.  We can use spin_lock_irq() here.
  */
@@ -1173,11 +1215,15 @@ uart_ioctl(struct tty_struct *tty, unsigned int cmd,
 		break;
 
 	case TIOCSSERIAL:
+		down_write(&tty->termios_rwsem);
 		ret = uart_set_info_user(tty, state, uarg);
+		up_write(&tty->termios_rwsem);
 		break;
 
 	case TIOCSERCONFIG:
+		down_write(&tty->termios_rwsem);
 		ret = uart_do_autoconfig(tty, state);
+		up_write(&tty->termios_rwsem);
 		break;
 
 	case TIOCSERGWILD: /* obsolete */
@@ -1217,11 +1263,19 @@ uart_ioctl(struct tty_struct *tty, unsigned int cmd,
 	 * All these rely on hardware being present and need to be
 	 * protected against the tty being hung up.
 	 */
+
 	switch (cmd) {
 	case TIOCSERGETLSR: /* Get line status register */
 		ret = uart_get_lsr_info(tty, state, uarg);
 		break;
 
+	case TIOCGRS485:
+		ret = uart_get_rs485_config(state->uart_port, uarg);
+		break;
+
+	case TIOCSRS485:
+		ret = uart_set_rs485_config(state->uart_port, uarg);
+		break;
 	default: {
 		struct uart_port *uport = state->uart_port;
 		if (uport->ops->ioctl)
@@ -1240,8 +1294,11 @@ static void uart_set_ldisc(struct tty_struct *tty)
 	struct uart_state *state = tty->driver_data;
 	struct uart_port *uport = state->uart_port;
 
-	if (uport->ops->set_ldisc)
-		uport->ops->set_ldisc(uport, tty->termios.c_line);
+	if (uport->ops->set_ldisc) {
+		mutex_lock(&state->port.mutex);
+		uport->ops->set_ldisc(uport, &tty->termios);
+		mutex_unlock(&state->port.mutex);
+	}
 }
 
 static void uart_set_termios(struct tty_struct *tty,
@@ -1278,7 +1335,9 @@ static void uart_set_termios(struct tty_struct *tty,
 		return;
 	}
 
+	mutex_lock(&state->port.mutex);
 	uart_change_speed(tty, state, old_termios);
+	mutex_unlock(&state->port.mutex);
 	/* reload cflag from termios; port driver may have overriden flags */
 	cflag = tty->termios.c_cflag;
 
@@ -1331,8 +1390,16 @@ static void uart_close(struct tty_struct *tty, struct file *filp)
 	struct uart_port *uport;
 	unsigned long flags;
 
-	if (!state)
+	if (!state) {
+		struct uart_driver *drv = tty->driver->driver_state;
+
+		state = drv->state + tty->index;
+		port = &state->port;
+		spin_lock_irq(&port->lock);
+		--port->count;
+		spin_unlock_irq(&port->lock);
 		return;
+	}
 
 	uport = state->uart_port;
 	port = &state->port;
@@ -1361,10 +1428,6 @@ static void uart_close(struct tty_struct *tty, struct file *filp)
 
 	mutex_lock(&port->mutex);
 	uart_shutdown(tty, state);
-	uart_flush_buffer(tty);
-
-	tty_ldisc_flush(tty);
-
 	tty_port_tty_set(port, NULL);
 	tty->closing = 0;
 	spin_lock_irqsave(&port->lock, flags);
@@ -1372,8 +1435,7 @@ static void uart_close(struct tty_struct *tty, struct file *filp)
 	if (port->blocked_open) {
 		spin_unlock_irqrestore(&port->lock, flags);
 		if (port->close_delay)
-			msleep_interruptible(
-					jiffies_to_msecs(port->close_delay));
+			msleep_interruptible(jiffies_to_msecs(port->close_delay));
 		spin_lock_irqsave(&port->lock, flags);
 	} else if (!uart_console(uport)) {
 		spin_unlock_irqrestore(&port->lock, flags);
@@ -1391,6 +1453,8 @@ static void uart_close(struct tty_struct *tty, struct file *filp)
 	wake_up_interruptible(&port->close_wait);
 
 	mutex_unlock(&port->mutex);
+
+	tty_ldisc_flush(tty);
 }
 
 static void uart_wait_until_sent(struct tty_struct *tty, int timeout)
@@ -1552,6 +1616,10 @@ static int uart_open(struct tty_struct *tty, struct file *filp)
 
 	pr_debug("uart_open(%d) called\n", line);
 
+	spin_lock_irq(&port->lock);
+	++port->count;
+	spin_unlock_irq(&port->lock);
+
 	/*
 	 * We take the semaphore here to guarantee that we won't be re-entered
 	 * while allocating the state structure, or while we request any IRQs
@@ -1564,17 +1632,11 @@ static int uart_open(struct tty_struct *tty, struct file *filp)
 		goto end;
 	}
 
-	port->count++;
 	if (!state->uart_port || state->uart_port->flags & UPF_DEAD) {
 		retval = -ENXIO;
-		goto err_dec_count;
+		goto err_unlock;
 	}
 
-	/*
-	 * Once we set tty->driver_data here, we are guaranteed that
-	 * uart_close() will decrement the driver module use count.
-	 * Any failures from here onwards should not touch the count.
-	 */
 	tty->driver_data = state;
 	state->uart_port->state = state;
 	state->port.low_latency =
@@ -1595,8 +1657,7 @@ static int uart_open(struct tty_struct *tty, struct file *filp)
 
 end:
 	return retval;
-err_dec_count:
-	port->count--;
+err_unlock:
 	mutex_unlock(&port->mutex);
 	goto end;
 }
@@ -2092,6 +2153,7 @@ uart_report_port(struct uart_driver *drv, struct uart_port *port)
 		break;
 	case UPIO_MEM:
 	case UPIO_MEM32:
+	case UPIO_MEM32BE:
 	case UPIO_AU:
 	case UPIO_TSI:
 		snprintf(address, sizeof(address),
@@ -2339,8 +2401,6 @@ int uart_register_driver(struct uart_driver *drv)
 
 		tty_port_init(port);
 		port->ops = &uart_port_ops;
-		port->close_delay     = HZ / 2;	/* .5 seconds */
-		port->closing_wait    = 30 * HZ;/* 30 seconds */
 	}
 
 	retval = tty_register_driver(normal);
@@ -2589,11 +2649,12 @@ int uart_add_one_port(struct uart_driver *drv, struct uart_port *uport)
 		goto out;
 	}
 
+	/* Link the port to the driver state table and vice versa */
 	state->uart_port = uport;
-	state->pm_state = UART_PM_STATE_UNDEFINED;
+	uport->state = state;
 
+	state->pm_state = UART_PM_STATE_UNDEFINED;
 	uport->cons = drv->cons;
-	uport->state = state;
 
 	/*
 	 * If this port is a console, then the spinlock is already
@@ -2736,6 +2797,7 @@ int uart_match_port(struct uart_port *port1, struct uart_port *port2)
 		       (port1->hub6   == port2->hub6);
 	case UPIO_MEM:
 	case UPIO_MEM32:
+	case UPIO_MEM32BE:
 	case UPIO_AU:
 	case UPIO_TSI:
 		return (port1->mapbase == port2->mapbase);
diff --git a/drivers/tty/serial/serial_mctrl_gpio.c b/drivers/tty/serial/serial_mctrl_gpio.c
index a3035f997b98..a38596c5194e 100644
--- a/drivers/tty/serial/serial_mctrl_gpio.c
+++ b/drivers/tty/serial/serial_mctrl_gpio.c
@@ -44,15 +44,21 @@ static const struct {
 void mctrl_gpio_set(struct mctrl_gpios *gpios, unsigned int mctrl)
 {
 	enum mctrl_gpio_idx i;
+	struct gpio_desc *desc_array[UART_GPIO_MAX];
+	int value_array[UART_GPIO_MAX];
+	unsigned int count = 0;
 
 	if (IS_ERR_OR_NULL(gpios))
 		return;
 
 	for (i = 0; i < UART_GPIO_MAX; i++)
 		if (!IS_ERR_OR_NULL(gpios->gpio[i]) &&
-		    mctrl_gpios_desc[i].dir_out)
-			gpiod_set_value(gpios->gpio[i],
-					!!(mctrl & mctrl_gpios_desc[i].mctrl));
+		    mctrl_gpios_desc[i].dir_out) {
+			desc_array[count] = gpios->gpio[i];
+			value_array[count] = !!(mctrl & mctrl_gpios_desc[i].mctrl);
+			count++;
+		}
+	gpiod_set_array(count, desc_array, value_array);
 }
 EXPORT_SYMBOL_GPL(mctrl_gpio_set);
 
diff --git a/drivers/tty/serial/serial_txx9.c b/drivers/tty/serial/serial_txx9.c
index af115645c51f..f80312eed4fd 100644
--- a/drivers/tty/serial/serial_txx9.c
+++ b/drivers/tty/serial/serial_txx9.c
@@ -1165,7 +1165,6 @@ static struct platform_driver serial_txx9_plat_driver = {
 #endif
 	.driver		= {
 		.name	= "serial_txx9",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/tty/serial/sh-sci.c b/drivers/tty/serial/sh-sci.c
index eb17c7124e72..e032963989fc 100644
--- a/drivers/tty/serial/sh-sci.c
+++ b/drivers/tty/serial/sh-sci.c
@@ -1812,9 +1812,6 @@ static void sci_baud_calc_hscif(unsigned int bps, unsigned long freq,
 			err = DIV_ROUND_CLOSEST(freq, ((br + 1) * bps * sr *
 					       (1 << (2 * c + 1)) / 1000)) -
 					       1000;
-			if (err < 0)
-				continue;
-
 			/* Calc recv margin
 			 * M: Receive margin (%)
 			 * N: Ratio of bit rate to clock (N = sampling rate)
@@ -1829,7 +1826,7 @@ static void sci_baud_calc_hscif(unsigned int bps, unsigned long freq,
 			 */
 			recv_margin = abs((500 -
 					DIV_ROUND_CLOSEST(1000, sr << 1)) / 10);
-			if (min_err > err) {
+			if (abs(min_err) > abs(err)) {
 				min_err = err;
 				recv_max_margin = recv_margin;
 			} else if ((min_err == err) &&
@@ -2638,7 +2635,6 @@ static struct platform_driver sci_driver = {
 	.remove		= sci_remove,
 	.driver		= {
 		.name	= "sh-sci",
-		.owner	= THIS_MODULE,
 		.pm	= &sci_dev_pm_ops,
 		.of_match_table = of_match_ptr(of_sci_match),
 	},
diff --git a/drivers/tty/serial/sirfsoc_uart.c b/drivers/tty/serial/sirfsoc_uart.c
index 4102192687ee..b269f6bd16d6 100644
--- a/drivers/tty/serial/sirfsoc_uart.c
+++ b/drivers/tty/serial/sirfsoc_uart.c
@@ -1032,10 +1032,19 @@ static void sirfsoc_uart_pm(struct uart_port *port, unsigned int state,
 			      unsigned int oldstate)
 {
 	struct sirfsoc_uart_port *sirfport = to_sirfport(port);
-	if (!state)
+	if (!state) {
+		if (sirfport->is_bt_uart) {
+			clk_prepare_enable(sirfport->clk_noc);
+			clk_prepare_enable(sirfport->clk_general);
+		}
 		clk_prepare_enable(sirfport->clk);
-	else
+	} else {
 		clk_disable_unprepare(sirfport->clk);
+		if (sirfport->is_bt_uart) {
+			clk_disable_unprepare(sirfport->clk_general);
+			clk_disable_unprepare(sirfport->clk_noc);
+		}
+	}
 }
 
 static int sirfsoc_uart_startup(struct uart_port *port)
@@ -1378,12 +1387,26 @@ usp_no_flow_control:
 	}
 	port->irq = res->start;
 
-	sirfport->clk = clk_get(&pdev->dev, NULL);
+	sirfport->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(sirfport->clk)) {
 		ret = PTR_ERR(sirfport->clk);
 		goto err;
 	}
 	port->uartclk = clk_get_rate(sirfport->clk);
+	if (of_device_is_compatible(pdev->dev.of_node, "sirf,marco-bt-uart")) {
+		sirfport->clk_general = devm_clk_get(&pdev->dev, "general");
+		if (IS_ERR(sirfport->clk_general)) {
+			ret = PTR_ERR(sirfport->clk_general);
+			goto err;
+		}
+		sirfport->clk_noc = devm_clk_get(&pdev->dev, "noc");
+		if (IS_ERR(sirfport->clk_noc)) {
+			ret = PTR_ERR(sirfport->clk_noc);
+			goto err;
+		}
+		sirfport->is_bt_uart = true;
+	} else
+		sirfport->is_bt_uart = false;
 
 	port->ops = &sirfsoc_uart_ops;
 	spin_lock_init(&port->lock);
@@ -1392,7 +1415,7 @@ usp_no_flow_control:
 	ret = uart_add_one_port(&sirfsoc_uart_drv, port);
 	if (ret != 0) {
 		dev_err(&pdev->dev, "Cannot add UART port(%d).\n", pdev->id);
-		goto port_err;
+		goto err;
 	}
 
 	sirfport->rx_dma_chan = dma_request_slave_channel(port->dev, "rx");
@@ -1421,8 +1444,6 @@ alloc_coherent_err:
 				sirfport->rx_dma_items[j].xmit.buf,
 				sirfport->rx_dma_items[j].dma_addr);
 	dma_release_channel(sirfport->rx_dma_chan);
-port_err:
-	clk_put(sirfport->clk);
 err:
 	return ret;
 }
@@ -1431,7 +1452,6 @@ static int sirfsoc_uart_remove(struct platform_device *pdev)
 {
 	struct sirfsoc_uart_port *sirfport = platform_get_drvdata(pdev);
 	struct uart_port *port = &sirfport->port;
-	clk_put(sirfport->clk);
 	uart_remove_one_port(&sirfsoc_uart_drv, port);
 	if (sirfport->rx_dma_chan) {
 		int i;
@@ -1477,7 +1497,6 @@ static struct platform_driver sirfsoc_uart_driver = {
 	.remove		= sirfsoc_uart_remove,
 	.driver		= {
 		.name	= SIRFUART_PORT_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = sirfsoc_uart_ids,
 		.pm	= &sirfsoc_uart_pm_ops,
 	},
diff --git a/drivers/tty/serial/sirfsoc_uart.h b/drivers/tty/serial/sirfsoc_uart.h
index 6a7ebf7ef130..275d03893990 100644
--- a/drivers/tty/serial/sirfsoc_uart.h
+++ b/drivers/tty/serial/sirfsoc_uart.h
@@ -417,6 +417,10 @@ struct sirfsoc_uart_port {
 
 	struct uart_port		port;
 	struct clk			*clk;
+	/* UART6 for BT usage in A7DA platform need multi-clock source */
+	bool				is_bt_uart;
+	struct clk			*clk_general;
+	struct clk			*clk_noc;
 	/* for SiRFmarco, there are SET/CLR for UART_INT_EN */
 	bool				is_marco;
 	struct sirfsoc_uart_register	*uart_reg;
diff --git a/drivers/tty/serial/st-asc.c b/drivers/tty/serial/st-asc.c
index a3165842ca29..712b03a076b8 100644
--- a/drivers/tty/serial/st-asc.c
+++ b/drivers/tty/serial/st-asc.c
@@ -895,7 +895,6 @@ static struct platform_driver asc_serial_driver = {
 	.driver	= {
 		.name	= DRIVER_NAME,
 		.pm	= &asc_serial_pm_ops,
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(asc_match),
 	},
 };
diff --git a/drivers/tty/serial/sunhv.c b/drivers/tty/serial/sunhv.c
index 25d43ce8b318..534754440fa8 100644
--- a/drivers/tty/serial/sunhv.c
+++ b/drivers/tty/serial/sunhv.c
@@ -626,7 +626,6 @@ MODULE_DEVICE_TABLE(of, hv_match);
 static struct platform_driver hv_driver = {
 	.driver = {
 		.name = "hv",
-		.owner = THIS_MODULE,
 		.of_match_table = hv_match,
 	},
 	.probe		= hv_probe,
diff --git a/drivers/tty/serial/sunsab.c b/drivers/tty/serial/sunsab.c
index b339fe4811cd..b5e3195b3697 100644
--- a/drivers/tty/serial/sunsab.c
+++ b/drivers/tty/serial/sunsab.c
@@ -345,7 +345,8 @@ static irqreturn_t sunsab_interrupt(int irq, void *dev_id)
 /* port->lock is not held.  */
 static unsigned int sunsab_tx_empty(struct uart_port *port)
 {
-	struct uart_sunsab_port *up = (struct uart_sunsab_port *) port;
+	struct uart_sunsab_port *up =
+		container_of(port, struct uart_sunsab_port, port);
 	int ret;
 
 	/* Do not need a lock for a state test like this.  */
@@ -360,7 +361,8 @@ static unsigned int sunsab_tx_empty(struct uart_port *port)
 /* port->lock held by caller.  */
 static void sunsab_set_mctrl(struct uart_port *port, unsigned int mctrl)
 {
-	struct uart_sunsab_port *up = (struct uart_sunsab_port *) port;
+	struct uart_sunsab_port *up =
+		container_of(port, struct uart_sunsab_port, port);
 
 	if (mctrl & TIOCM_RTS) {
 		up->cached_mode &= ~SAB82532_MODE_FRTS;
@@ -383,7 +385,8 @@ static void sunsab_set_mctrl(struct uart_port *port, unsigned int mctrl)
 /* port->lock is held by caller and interrupts are disabled.  */
 static unsigned int sunsab_get_mctrl(struct uart_port *port)
 {
-	struct uart_sunsab_port *up = (struct uart_sunsab_port *) port;
+	struct uart_sunsab_port *up =
+		container_of(port, struct uart_sunsab_port, port);
 	unsigned char val;
 	unsigned int result;
 
@@ -404,7 +407,8 @@ static unsigned int sunsab_get_mctrl(struct uart_port *port)
 /* port->lock held by caller.  */
 static void sunsab_stop_tx(struct uart_port *port)
 {
-	struct uart_sunsab_port *up = (struct uart_sunsab_port *) port;
+	struct uart_sunsab_port *up =
+		container_of(port, struct uart_sunsab_port, port);
 
 	up->interrupt_mask1 |= SAB82532_IMR1_XPR;
 	writeb(up->interrupt_mask1, &up->regs->w.imr1);
@@ -432,7 +436,8 @@ static void sunsab_tx_idle(struct uart_sunsab_port *up)
 /* port->lock held by caller.  */
 static void sunsab_start_tx(struct uart_port *port)
 {
-	struct uart_sunsab_port *up = (struct uart_sunsab_port *) port;
+	struct uart_sunsab_port *up =
+		container_of(port, struct uart_sunsab_port, port);
 	struct circ_buf *xmit = &up->port.state->xmit;
 	int i;
 
@@ -465,7 +470,8 @@ static void sunsab_start_tx(struct uart_port *port)
 /* port->lock is not held.  */
 static void sunsab_send_xchar(struct uart_port *port, char ch)
 {
-	struct uart_sunsab_port *up = (struct uart_sunsab_port *) port;
+	struct uart_sunsab_port *up =
+		container_of(port, struct uart_sunsab_port, port);
 	unsigned long flags;
 
 	if (ch == __DISABLED_CHAR)
@@ -482,7 +488,8 @@ static void sunsab_send_xchar(struct uart_port *port, char ch)
 /* port->lock held by caller.  */
 static void sunsab_stop_rx(struct uart_port *port)
 {
-	struct uart_sunsab_port *up = (struct uart_sunsab_port *) port;
+	struct uart_sunsab_port *up =
+		container_of(port, struct uart_sunsab_port, port);
 
 	up->interrupt_mask0 |= SAB82532_IMR0_TCD;
 	writeb(up->interrupt_mask1, &up->regs->w.imr0);
@@ -491,7 +498,8 @@ static void sunsab_stop_rx(struct uart_port *port)
 /* port->lock is not held.  */
 static void sunsab_break_ctl(struct uart_port *port, int break_state)
 {
-	struct uart_sunsab_port *up = (struct uart_sunsab_port *) port;
+	struct uart_sunsab_port *up =
+		container_of(port, struct uart_sunsab_port, port);
 	unsigned long flags;
 	unsigned char val;
 
@@ -514,7 +522,8 @@ static void sunsab_break_ctl(struct uart_port *port, int break_state)
 /* port->lock is not held.  */
 static int sunsab_startup(struct uart_port *port)
 {
-	struct uart_sunsab_port *up = (struct uart_sunsab_port *) port;
+	struct uart_sunsab_port *up =
+		container_of(port, struct uart_sunsab_port, port);
 	unsigned long flags;
 	unsigned char tmp;
 	int err = request_irq(up->port.irq, sunsab_interrupt,
@@ -585,7 +594,8 @@ static int sunsab_startup(struct uart_port *port)
 /* port->lock is not held.  */
 static void sunsab_shutdown(struct uart_port *port)
 {
-	struct uart_sunsab_port *up = (struct uart_sunsab_port *) port;
+	struct uart_sunsab_port *up =
+		container_of(port, struct uart_sunsab_port, port);
 	unsigned long flags;
 
 	spin_lock_irqsave(&up->port.lock, flags);
@@ -771,7 +781,8 @@ static void sunsab_convert_to_sab(struct uart_sunsab_port *up, unsigned int cfla
 static void sunsab_set_termios(struct uart_port *port, struct ktermios *termios,
 			       struct ktermios *old)
 {
-	struct uart_sunsab_port *up = (struct uart_sunsab_port *) port;
+	struct uart_sunsab_port *up =
+		container_of(port, struct uart_sunsab_port, port);
 	unsigned long flags;
 	unsigned int baud = uart_get_baud_rate(port, termios, old, 0, 4000000);
 	unsigned int quot = uart_get_divisor(port, baud);
@@ -840,7 +851,8 @@ static struct uart_sunsab_port *sunsab_ports;
 
 static void sunsab_console_putchar(struct uart_port *port, int c)
 {
-	struct uart_sunsab_port *up = (struct uart_sunsab_port *)port;
+	struct uart_sunsab_port *up =
+		container_of(port, struct uart_sunsab_port, port);
 
 	sunsab_tec_wait(up);
 	writeb(c, &up->regs->w.tic);
@@ -1092,7 +1104,6 @@ MODULE_DEVICE_TABLE(of, sab_match);
 static struct platform_driver sab_driver = {
 	.driver = {
 		.name = "sab",
-		.owner = THIS_MODULE,
 		.of_match_table = sab_match,
 	},
 	.probe		= sab_probe,
diff --git a/drivers/tty/serial/sunsu.c b/drivers/tty/serial/sunsu.c
index 5326ae195e5f..e124d2e88996 100644
--- a/drivers/tty/serial/sunsu.c
+++ b/drivers/tty/serial/sunsu.c
@@ -264,7 +264,8 @@ static inline void __stop_tx(struct uart_sunsu_port *p)
 
 static void sunsu_stop_tx(struct uart_port *port)
 {
-	struct uart_sunsu_port *up = (struct uart_sunsu_port *) port;
+	struct uart_sunsu_port *up =
+		container_of(port, struct uart_sunsu_port, port);
 
 	__stop_tx(up);
 
@@ -279,7 +280,8 @@ static void sunsu_stop_tx(struct uart_port *port)
 
 static void sunsu_start_tx(struct uart_port *port)
 {
-	struct uart_sunsu_port *up = (struct uart_sunsu_port *) port;
+	struct uart_sunsu_port *up =
+		container_of(port, struct uart_sunsu_port, port);
 
 	if (!(up->ier & UART_IER_THRI)) {
 		up->ier |= UART_IER_THRI;
@@ -297,7 +299,8 @@ static void sunsu_start_tx(struct uart_port *port)
 
 static void sunsu_stop_rx(struct uart_port *port)
 {
-	struct uart_sunsu_port *up = (struct uart_sunsu_port *) port;
+	struct uart_sunsu_port *up =
+		container_of(port, struct uart_sunsu_port, port);
 
 	up->ier &= ~UART_IER_RLSI;
 	up->port.read_status_mask &= ~UART_LSR_DR;
@@ -306,7 +309,8 @@ static void sunsu_stop_rx(struct uart_port *port)
 
 static void sunsu_enable_ms(struct uart_port *port)
 {
-	struct uart_sunsu_port *up = (struct uart_sunsu_port *) port;
+	struct uart_sunsu_port *up =
+		container_of(port, struct uart_sunsu_port, port);
 	unsigned long flags;
 
 	spin_lock_irqsave(&up->port.lock, flags);
@@ -543,7 +547,8 @@ static irqreturn_t sunsu_kbd_ms_interrupt(int irq, void *dev_id)
 
 static unsigned int sunsu_tx_empty(struct uart_port *port)
 {
-	struct uart_sunsu_port *up = (struct uart_sunsu_port *) port;
+	struct uart_sunsu_port *up =
+		container_of(port, struct uart_sunsu_port, port);
 	unsigned long flags;
 	unsigned int ret;
 
@@ -556,7 +561,8 @@ static unsigned int sunsu_tx_empty(struct uart_port *port)
 
 static unsigned int sunsu_get_mctrl(struct uart_port *port)
 {
-	struct uart_sunsu_port *up = (struct uart_sunsu_port *) port;
+	struct uart_sunsu_port *up =
+		container_of(port, struct uart_sunsu_port, port);
 	unsigned char status;
 	unsigned int ret;
 
@@ -576,7 +582,8 @@ static unsigned int sunsu_get_mctrl(struct uart_port *port)
 
 static void sunsu_set_mctrl(struct uart_port *port, unsigned int mctrl)
 {
-	struct uart_sunsu_port *up = (struct uart_sunsu_port *) port;
+	struct uart_sunsu_port *up =
+		container_of(port, struct uart_sunsu_port, port);
 	unsigned char mcr = 0;
 
 	if (mctrl & TIOCM_RTS)
@@ -595,7 +602,8 @@ static void sunsu_set_mctrl(struct uart_port *port, unsigned int mctrl)
 
 static void sunsu_break_ctl(struct uart_port *port, int break_state)
 {
-	struct uart_sunsu_port *up = (struct uart_sunsu_port *) port;
+	struct uart_sunsu_port *up =
+		container_of(port, struct uart_sunsu_port, port);
 	unsigned long flags;
 
 	spin_lock_irqsave(&up->port.lock, flags);
@@ -609,7 +617,8 @@ static void sunsu_break_ctl(struct uart_port *port, int break_state)
 
 static int sunsu_startup(struct uart_port *port)
 {
-	struct uart_sunsu_port *up = (struct uart_sunsu_port *) port;
+	struct uart_sunsu_port *up =
+		container_of(port, struct uart_sunsu_port, port);
 	unsigned long flags;
 	int retval;
 
@@ -719,7 +728,8 @@ static int sunsu_startup(struct uart_port *port)
 
 static void sunsu_shutdown(struct uart_port *port)
 {
-	struct uart_sunsu_port *up = (struct uart_sunsu_port *) port;
+	struct uart_sunsu_port *up =
+		container_of(port, struct uart_sunsu_port, port);
 	unsigned long flags;
 
 	/*
@@ -767,7 +777,8 @@ static void
 sunsu_change_speed(struct uart_port *port, unsigned int cflag,
 		   unsigned int iflag, unsigned int quot)
 {
-	struct uart_sunsu_port *up = (struct uart_sunsu_port *) port;
+	struct uart_sunsu_port *up =
+		container_of(port, struct uart_sunsu_port, port);
 	unsigned char cval, fcr = 0;
 	unsigned long flags;
 
@@ -918,7 +929,8 @@ static int sunsu_request_port(struct uart_port *port)
 
 static void sunsu_config_port(struct uart_port *port, int flags)
 {
-	struct uart_sunsu_port *up = (struct uart_sunsu_port *) port;
+	struct uart_sunsu_port *up =
+		container_of(port, struct uart_sunsu_port, port);
 
 	if (flags & UART_CONFIG_TYPE) {
 		/*
@@ -1277,7 +1289,8 @@ static __inline__ void wait_for_xmitr(struct uart_sunsu_port *up)
 
 static void sunsu_console_putchar(struct uart_port *port, int ch)
 {
-	struct uart_sunsu_port *up = (struct uart_sunsu_port *)port;
+	struct uart_sunsu_port *up =
+		container_of(port, struct uart_sunsu_port, port);
 
 	wait_for_xmitr(up);
 	serial_out(up, UART_TX, ch);
@@ -1537,7 +1550,6 @@ MODULE_DEVICE_TABLE(of, su_match);
 static struct platform_driver su_driver = {
 	.driver = {
 		.name = "su",
-		.owner = THIS_MODULE,
 		.of_match_table = su_match,
 	},
 	.probe		= su_probe,
diff --git a/drivers/tty/serial/sunzilog.c b/drivers/tty/serial/sunzilog.c
index 02df3940b95e..8b6ace341029 100644
--- a/drivers/tty/serial/sunzilog.c
+++ b/drivers/tty/serial/sunzilog.c
@@ -644,7 +644,8 @@ static unsigned int sunzilog_get_mctrl(struct uart_port *port)
 /* The port lock is held and interrupts are disabled.  */
 static void sunzilog_set_mctrl(struct uart_port *port, unsigned int mctrl)
 {
-	struct uart_sunzilog_port *up = (struct uart_sunzilog_port *) port;
+	struct uart_sunzilog_port *up =
+		container_of(port, struct uart_sunzilog_port, port);
 	struct zilog_channel __iomem *channel = ZILOG_CHANNEL_FROM_PORT(port);
 	unsigned char set_bits, clear_bits;
 
@@ -668,7 +669,8 @@ static void sunzilog_set_mctrl(struct uart_port *port, unsigned int mctrl)
 /* The port lock is held and interrupts are disabled.  */
 static void sunzilog_stop_tx(struct uart_port *port)
 {
-	struct uart_sunzilog_port *up = (struct uart_sunzilog_port *) port;
+	struct uart_sunzilog_port *up =
+		container_of(port, struct uart_sunzilog_port, port);
 
 	up->flags |= SUNZILOG_FLAG_TX_STOPPED;
 }
@@ -676,7 +678,8 @@ static void sunzilog_stop_tx(struct uart_port *port)
 /* The port lock is held and interrupts are disabled.  */
 static void sunzilog_start_tx(struct uart_port *port)
 {
-	struct uart_sunzilog_port *up = (struct uart_sunzilog_port *) port;
+	struct uart_sunzilog_port *up =
+		container_of(port, struct uart_sunzilog_port, port);
 	struct zilog_channel __iomem *channel = ZILOG_CHANNEL_FROM_PORT(port);
 	unsigned char status;
 
@@ -736,7 +739,8 @@ static void sunzilog_stop_rx(struct uart_port *port)
 /* The port lock is held.  */
 static void sunzilog_enable_ms(struct uart_port *port)
 {
-	struct uart_sunzilog_port *up = (struct uart_sunzilog_port *) port;
+	struct uart_sunzilog_port *up =
+		container_of(port, struct uart_sunzilog_port, port);
 	struct zilog_channel __iomem *channel = ZILOG_CHANNEL_FROM_PORT(port);
 	unsigned char new_reg;
 
@@ -752,7 +756,8 @@ static void sunzilog_enable_ms(struct uart_port *port)
 /* The port lock is not held.  */
 static void sunzilog_break_ctl(struct uart_port *port, int break_state)
 {
-	struct uart_sunzilog_port *up = (struct uart_sunzilog_port *) port;
+	struct uart_sunzilog_port *up =
+		container_of(port, struct uart_sunzilog_port, port);
 	struct zilog_channel __iomem *channel = ZILOG_CHANNEL_FROM_PORT(port);
 	unsigned char set_bits, clear_bits, new_reg;
 	unsigned long flags;
@@ -938,7 +943,8 @@ static void
 sunzilog_set_termios(struct uart_port *port, struct ktermios *termios,
 		     struct ktermios *old)
 {
-	struct uart_sunzilog_port *up = (struct uart_sunzilog_port *) port;
+	struct uart_sunzilog_port *up =
+		container_of(port, struct uart_sunzilog_port, port);
 	unsigned long flags;
 	int baud, brg;
 
@@ -998,7 +1004,8 @@ static int sunzilog_verify_port(struct uart_port *port, struct serial_struct *se
 static int sunzilog_get_poll_char(struct uart_port *port)
 {
 	unsigned char ch, r1;
-	struct uart_sunzilog_port *up = (struct uart_sunzilog_port *) port;
+	struct uart_sunzilog_port *up =
+		container_of(port, struct uart_sunzilog_port, port);
 	struct zilog_channel __iomem *channel
 		= ZILOG_CHANNEL_FROM_PORT(&up->port);
 
@@ -1032,7 +1039,8 @@ static int sunzilog_get_poll_char(struct uart_port *port)
 static void sunzilog_put_poll_char(struct uart_port *port,
 			unsigned char ch)
 {
-	struct uart_sunzilog_port *up = (struct uart_sunzilog_port *)port;
+	struct uart_sunzilog_port *up =
+		container_of(port, struct uart_sunzilog_port, port);
 
 	sunzilog_putchar(&up->port, ch);
 }
@@ -1533,7 +1541,6 @@ MODULE_DEVICE_TABLE(of, zs_match);
 static struct platform_driver zs_driver = {
 	.driver = {
 		.name = "zs",
-		.owner = THIS_MODULE,
 		.of_match_table = zs_match,
 	},
 	.probe		= zs_probe,
diff --git a/drivers/tty/serial/timbuart.c b/drivers/tty/serial/timbuart.c
index 0d11d5032b93..512c162634a3 100644
--- a/drivers/tty/serial/timbuart.c
+++ b/drivers/tty/serial/timbuart.c
@@ -273,6 +273,8 @@ static void timbuart_shutdown(struct uart_port *port)
 	dev_dbg(port->dev, "%s\n", __func__);
 	free_irq(port->irq, uart);
 	iowrite32(0, port->membase + TIMBUART_IER);
+
+	timbuart_flush_buffer(port);
 }
 
 static int get_bindex(int baud)
@@ -501,7 +503,6 @@ static int timbuart_remove(struct platform_device *dev)
 static struct platform_driver timbuart_platform_driver = {
 	.driver = {
 		.name	= "timb-uart",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= timbuart_probe,
 	.remove		= timbuart_remove,
diff --git a/drivers/tty/serial/ucc_uart.c b/drivers/tty/serial/ucc_uart.c
index c107a0f0e72f..14d10fcfd210 100644
--- a/drivers/tty/serial/ucc_uart.c
+++ b/drivers/tty/serial/ucc_uart.c
@@ -1485,7 +1485,6 @@ MODULE_DEVICE_TABLE(of, ucc_uart_match);
 static struct platform_driver ucc_uart_of_driver = {
 	.driver = {
 		.name = "ucc_uart",
-		.owner = THIS_MODULE,
 		.of_match_table    = ucc_uart_match,
 	},
 	.probe  	= ucc_uart_probe,
diff --git a/drivers/tty/serial/vr41xx_siu.c b/drivers/tty/serial/vr41xx_siu.c
index d7f9d622cdcb..485de53c5d75 100644
--- a/drivers/tty/serial/vr41xx_siu.c
+++ b/drivers/tty/serial/vr41xx_siu.c
@@ -954,7 +954,6 @@ static struct platform_driver siu_device_driver = {
 	.resume		= siu_resume,
 	.driver		= {
 		.name	= "SIU",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/tty/serial/vt8500_serial.c b/drivers/tty/serial/vt8500_serial.c
index b2bc9e8ba048..4079ec56f5f9 100644
--- a/drivers/tty/serial/vt8500_serial.c
+++ b/drivers/tty/serial/vt8500_serial.c
@@ -745,7 +745,6 @@ static struct platform_driver vt8500_platform_driver = {
 	.remove = vt8500_serial_remove,
 	.driver = {
 		.name = "vt8500_serial",
-		.owner = THIS_MODULE,
 		.of_match_table = wmt_dt_ids,
 	},
 };
diff --git a/drivers/tty/serial/xilinx_uartps.c b/drivers/tty/serial/xilinx_uartps.c
index 200c1af2141b..542bab37e502 100644
--- a/drivers/tty/serial/xilinx_uartps.c
+++ b/drivers/tty/serial/xilinx_uartps.c
@@ -133,6 +133,15 @@ MODULE_PARM_DESC(rx_timeout, "Rx timeout, 1-255");
 #define CDNS_UART_IXR_BRK	0x80000000
 
 /*
+ * Modem Control register:
+ * The read/write Modem Control register controls the interface with the modem
+ * or data set, or a peripheral device emulating a modem.
+ */
+#define CDNS_UART_MODEMCR_FCM	0x00000020 /* Automatic flow control mode */
+#define CDNS_UART_MODEMCR_RTS	0x00000002 /* Request to send output control */
+#define CDNS_UART_MODEMCR_DTR	0x00000001 /* Data Terminal Ready */
+
+/*
  * Channel Status Register:
  * The channel status register (CSR) is provided to enable the control logic
  * to monitor the status of bits in the channel interrupt status register,
@@ -915,7 +924,18 @@ static unsigned int cdns_uart_get_mctrl(struct uart_port *port)
 
 static void cdns_uart_set_mctrl(struct uart_port *port, unsigned int mctrl)
 {
-	/* N/A */
+	u32 val;
+
+	val = cdns_uart_readl(CDNS_UART_MODEMCR_OFFSET);
+
+	val &= ~(CDNS_UART_MODEMCR_RTS | CDNS_UART_MODEMCR_DTR);
+
+	if (mctrl & TIOCM_RTS)
+		val |= CDNS_UART_MODEMCR_RTS;
+	if (mctrl & TIOCM_DTR)
+		val |= CDNS_UART_MODEMCR_DTR;
+
+	cdns_uart_writel(val, CDNS_UART_MODEMCR_OFFSET);
 }
 
 #ifdef CONFIG_CONSOLE_POLL
diff --git a/drivers/tty/tty_buffer.c b/drivers/tty/tty_buffer.c
index 143deb62467d..3605103fc1ac 100644
--- a/drivers/tty/tty_buffer.c
+++ b/drivers/tty/tty_buffer.c
@@ -202,14 +202,16 @@ static void tty_buffer_free(struct tty_port *port, struct tty_buffer *b)
 /**
  *	tty_buffer_flush		-	flush full tty buffers
  *	@tty: tty to flush
+ *	@ld:  optional ldisc ptr (must be referenced)
  *
- *	flush all the buffers containing receive data.
+ *	flush all the buffers containing receive data. If ld != NULL,
+ *	flush the ldisc input buffer.
  *
  *	Locking: takes buffer lock to ensure single-threaded flip buffer
  *		 'consumer'
  */
 
-void tty_buffer_flush(struct tty_struct *tty)
+void tty_buffer_flush(struct tty_struct *tty, struct tty_ldisc *ld)
 {
 	struct tty_port *port = tty->port;
 	struct tty_bufhead *buf = &port->buf;
@@ -223,6 +225,10 @@ void tty_buffer_flush(struct tty_struct *tty)
 		buf->head = next;
 	}
 	buf->head->read = buf->head->commit;
+
+	if (ld && ld->ops->flush_buffer)
+		ld->ops->flush_buffer(tty);
+
 	atomic_dec(&buf->priority);
 	mutex_unlock(&buf->lock);
 }
diff --git a/drivers/tty/tty_io.c b/drivers/tty/tty_io.c
index 0508a1d8e4cd..4f35b43e2475 100644
--- a/drivers/tty/tty_io.c
+++ b/drivers/tty/tty_io.c
@@ -153,8 +153,6 @@ static long tty_compat_ioctl(struct file *file, unsigned int cmd,
 static int __tty_fasync(int fd, struct file *filp, int on);
 static int tty_fasync(int fd, struct file *filp, int on);
 static void release_tty(struct tty_struct *tty, int idx);
-static void __proc_set_tty(struct task_struct *tsk, struct tty_struct *tty);
-static void proc_set_tty(struct task_struct *tsk, struct tty_struct *tty);
 
 /**
  *	free_tty_struct		-	free a disused tty
@@ -169,8 +167,7 @@ void free_tty_struct(struct tty_struct *tty)
 {
 	if (!tty)
 		return;
-	if (tty->dev)
-		put_device(tty->dev);
+	put_device(tty->dev);
 	kfree(tty->write_buf);
 	tty->magic = 0xDEADDEAD;
 	kfree(tty);
@@ -277,6 +274,7 @@ int tty_paranoia_check(struct tty_struct *tty, struct inode *inode,
 	return 0;
 }
 
+/* Caller must hold tty_lock */
 static int check_tty_count(struct tty_struct *tty, const char *routine)
 {
 #ifdef CHECK_TTY_COUNT
@@ -492,6 +490,78 @@ static const struct file_operations hung_up_tty_fops = {
 static DEFINE_SPINLOCK(redirect_lock);
 static struct file *redirect;
 
+
+void proc_clear_tty(struct task_struct *p)
+{
+	unsigned long flags;
+	struct tty_struct *tty;
+	spin_lock_irqsave(&p->sighand->siglock, flags);
+	tty = p->signal->tty;
+	p->signal->tty = NULL;
+	spin_unlock_irqrestore(&p->sighand->siglock, flags);
+	tty_kref_put(tty);
+}
+
+/**
+ * proc_set_tty -  set the controlling terminal
+ *
+ * Only callable by the session leader and only if it does not already have
+ * a controlling terminal.
+ *
+ * Caller must hold:  tty_lock()
+ *		      a readlock on tasklist_lock
+ *		      sighand lock
+ */
+static void __proc_set_tty(struct tty_struct *tty)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&tty->ctrl_lock, flags);
+	/*
+	 * The session and fg pgrp references will be non-NULL if
+	 * tiocsctty() is stealing the controlling tty
+	 */
+	put_pid(tty->session);
+	put_pid(tty->pgrp);
+	tty->pgrp = get_pid(task_pgrp(current));
+	spin_unlock_irqrestore(&tty->ctrl_lock, flags);
+	tty->session = get_pid(task_session(current));
+	if (current->signal->tty) {
+		printk(KERN_DEBUG "tty not NULL!!\n");
+		tty_kref_put(current->signal->tty);
+	}
+	put_pid(current->signal->tty_old_pgrp);
+	current->signal->tty = tty_kref_get(tty);
+	current->signal->tty_old_pgrp = NULL;
+}
+
+static void proc_set_tty(struct tty_struct *tty)
+{
+	spin_lock_irq(&current->sighand->siglock);
+	__proc_set_tty(tty);
+	spin_unlock_irq(&current->sighand->siglock);
+}
+
+struct tty_struct *get_current_tty(void)
+{
+	struct tty_struct *tty;
+	unsigned long flags;
+
+	spin_lock_irqsave(&current->sighand->siglock, flags);
+	tty = tty_kref_get(current->signal->tty);
+	spin_unlock_irqrestore(&current->sighand->siglock, flags);
+	return tty;
+}
+EXPORT_SYMBOL_GPL(get_current_tty);
+
+static void session_clear_tty(struct pid *session)
+{
+	struct task_struct *p;
+	do_each_pid_task(session, PIDTYPE_SID, p) {
+		proc_clear_tty(p);
+	} while_each_pid_task(session, PIDTYPE_SID, p);
+}
+
 /**
  *	tty_wakeup	-	request more data
  *	@tty: terminal
@@ -620,9 +690,6 @@ static void __tty_hangup(struct tty_struct *tty, int exit_session)
 		return;
 	}
 
-	/* some functions below drop BTM, so we need this bit */
-	set_bit(TTY_HUPPING, &tty->flags);
-
 	/* inuse_filps is protected by the single tty lock,
 	   this really needs to change if we want to flush the
 	   workqueue with the lock held */
@@ -647,10 +714,6 @@ static void __tty_hangup(struct tty_struct *tty, int exit_session)
 	while (refs--)
 		tty_kref_put(tty);
 
-	/*
-	 * it drops BTM and thus races with reopen
-	 * we protect the race by TTY_HUPPING
-	 */
 	tty_ldisc_hangup(tty);
 
 	spin_lock_irq(&tty->ctrl_lock);
@@ -682,8 +745,6 @@ static void __tty_hangup(struct tty_struct *tty, int exit_session)
 	 * can't yet guarantee all that.
 	 */
 	set_bit(TTY_HUPPED, &tty->flags);
-	clear_bit(TTY_HUPPING, &tty->flags);
-
 	tty_unlock(tty);
 
 	if (f)
@@ -792,14 +853,6 @@ int tty_hung_up_p(struct file *filp)
 
 EXPORT_SYMBOL(tty_hung_up_p);
 
-static void session_clear_tty(struct pid *session)
-{
-	struct task_struct *p;
-	do_each_pid_task(session, PIDTYPE_SID, p) {
-		proc_clear_tty(p);
-	} while_each_pid_task(session, PIDTYPE_SID, p);
-}
-
 /**
  *	disassociate_ctty	-	disconnect controlling tty
  *	@on_exit: true if exiting so need to "hang up" the session
@@ -927,7 +980,7 @@ void __stop_tty(struct tty_struct *tty)
 		return;
 	tty->stopped = 1;
 	if (tty->ops->stop)
-		(tty->ops->stop)(tty);
+		tty->ops->stop(tty);
 }
 
 void stop_tty(struct tty_struct *tty)
@@ -958,7 +1011,7 @@ void __start_tty(struct tty_struct *tty)
 		return;
 	tty->stopped = 0;
 	if (tty->ops->start)
-		(tty->ops->start)(tty);
+		tty->ops->start(tty);
 	tty_wakeup(tty);
 }
 
@@ -1012,7 +1065,7 @@ static ssize_t tty_read(struct file *file, char __user *buf, size_t count,
 	   situation */
 	ld = tty_ldisc_ref_wait(tty);
 	if (ld->ops->read)
-		i = (ld->ops->read)(tty, file, buf, count);
+		i = ld->ops->read(tty, file, buf, count);
 	else
 		i = -EIO;
 	tty_ldisc_deref(ld);
@@ -1024,14 +1077,12 @@ static ssize_t tty_read(struct file *file, char __user *buf, size_t count,
 }
 
 static void tty_write_unlock(struct tty_struct *tty)
-	__releases(&tty->atomic_write_lock)
 {
 	mutex_unlock(&tty->atomic_write_lock);
 	wake_up_interruptible_poll(&tty->write_wait, POLLOUT);
 }
 
 static int tty_write_lock(struct tty_struct *tty, int ndelay)
-	__acquires(&tty->atomic_write_lock)
 {
 	if (!mutex_trylock(&tty->atomic_write_lock)) {
 		if (ndelay)
@@ -1146,7 +1197,7 @@ void tty_write_message(struct tty_struct *tty, char *msg)
 	if (tty) {
 		mutex_lock(&tty->atomic_write_lock);
 		tty_lock(tty);
-		if (tty->ops->write && !test_bit(TTY_CLOSING, &tty->flags)) {
+		if (tty->ops->write && tty->count > 0) {
 			tty_unlock(tty);
 			tty->ops->write(tty, msg, strlen(msg));
 		} else
@@ -1293,19 +1344,24 @@ static ssize_t tty_line_name(struct tty_driver *driver, int index, char *p)
  *	@driver: the driver for the tty
  *	@idx:	 the minor number
  *
- *	Return the tty, if found or ERR_PTR() otherwise.
+ *	Return the tty, if found. If not found, return NULL or ERR_PTR() if the
+ *	driver lookup() method returns an error.
  *
- *	Locking: tty_mutex must be held. If tty is found, the mutex must
- *	be held until the 'fast-open' is also done. Will change once we
- *	have refcounting in the driver and per driver locking
+ *	Locking: tty_mutex must be held. If the tty is found, bump the tty kref.
  */
 static struct tty_struct *tty_driver_lookup_tty(struct tty_driver *driver,
 		struct inode *inode, int idx)
 {
+	struct tty_struct *tty;
+
 	if (driver->ops->lookup)
-		return driver->ops->lookup(driver, inode, idx);
+		tty = driver->ops->lookup(driver, inode, idx);
+	else
+		tty = driver->ttys[idx];
 
-	return driver->ttys[idx];
+	if (!IS_ERR(tty))
+		tty_kref_get(tty);
+	return tty;
 }
 
 /**
@@ -1393,29 +1449,21 @@ void tty_driver_remove_tty(struct tty_driver *driver, struct tty_struct *tty)
  * 	@tty	- the tty to open
  *
  *	Return 0 on success, -errno on error.
+ *	Re-opens on master ptys are not allowed and return -EIO.
  *
- *	Locking: tty_mutex must be held from the time the tty was found
- *		 till this open completes.
+ *	Locking: Caller must hold tty_lock
  */
 static int tty_reopen(struct tty_struct *tty)
 {
 	struct tty_driver *driver = tty->driver;
 
-	if (test_bit(TTY_CLOSING, &tty->flags) ||
-			test_bit(TTY_HUPPING, &tty->flags))
+	if (!tty->count)
 		return -EIO;
 
 	if (driver->type == TTY_DRIVER_TYPE_PTY &&
-	    driver->subtype == PTY_TYPE_MASTER) {
-		/*
-		 * special case for PTY masters: only one open permitted,
-		 * and the slave side open count is incremented as well.
-		 */
-		if (tty->count)
-			return -EIO;
+	    driver->subtype == PTY_TYPE_MASTER)
+		return -EIO;
 
-		tty->link->count++;
-	}
 	tty->count++;
 
 	WARN_ON(!tty->ldisc);
@@ -1535,15 +1583,19 @@ void tty_free_termios(struct tty_struct *tty)
 EXPORT_SYMBOL(tty_free_termios);
 
 /**
- *	tty_flush_works		-	flush all works of a tty
- *	@tty: tty device to flush works for
+ *	tty_flush_works		-	flush all works of a tty/pty pair
+ *	@tty: tty device to flush works for (or either end of a pty pair)
  *
- *	Sync flush all works belonging to @tty.
+ *	Sync flush all works belonging to @tty (and the 'other' tty).
  */
 static void tty_flush_works(struct tty_struct *tty)
 {
 	flush_work(&tty->SAK_work);
 	flush_work(&tty->hangup_work);
+	if (tty->link) {
+		flush_work(&tty->link->SAK_work);
+		flush_work(&tty->link->hangup_work);
+	}
 }
 
 /**
@@ -1635,8 +1687,7 @@ static void release_tty(struct tty_struct *tty, int idx)
 		tty->link->port->itty = NULL;
 	cancel_work_sync(&tty->port->buf.work);
 
-	if (tty->link)
-		tty_kref_put(tty->link);
+	tty_kref_put(tty->link);
 	tty_kref_put(tty);
 }
 
@@ -1649,8 +1700,7 @@ static void release_tty(struct tty_struct *tty, int idx)
  *	Performs some paranoid checking before true release of the @tty.
  *	This is a no-op unless TTY_PARANOIA_CHECK is defined.
  */
-static int tty_release_checks(struct tty_struct *tty, struct tty_struct *o_tty,
-		int idx)
+static int tty_release_checks(struct tty_struct *tty, int idx)
 {
 #ifdef TTY_PARANOIA_CHECK
 	if (idx < 0 || idx >= tty->driver->num) {
@@ -1669,6 +1719,8 @@ static int tty_release_checks(struct tty_struct *tty, struct tty_struct *o_tty,
 		return -1;
 	}
 	if (tty->driver->other) {
+		struct tty_struct *o_tty = tty->link;
+
 		if (o_tty != tty->driver->other->ttys[idx]) {
 			printk(KERN_DEBUG "%s: other->table[%d] not o_tty for (%s)\n",
 					__func__, idx, tty->name);
@@ -1705,8 +1757,8 @@ static int tty_release_checks(struct tty_struct *tty, struct tty_struct *o_tty,
 int tty_release(struct inode *inode, struct file *filp)
 {
 	struct tty_struct *tty = file_tty(filp);
-	struct tty_struct *o_tty;
-	int	pty_master, tty_closing, o_tty_closing, do_sleep;
+	struct tty_struct *o_tty = NULL;
+	int	do_sleep, final;
 	int	idx;
 	char	buf[64];
 	long	timeout = 0;
@@ -1721,12 +1773,11 @@ int tty_release(struct inode *inode, struct file *filp)
 	__tty_fasync(-1, filp, 0);
 
 	idx = tty->index;
-	pty_master = (tty->driver->type == TTY_DRIVER_TYPE_PTY &&
-		      tty->driver->subtype == PTY_TYPE_MASTER);
-	/* Review: parallel close */
-	o_tty = tty->link;
+	if (tty->driver->type == TTY_DRIVER_TYPE_PTY &&
+	    tty->driver->subtype == PTY_TYPE_MASTER)
+		o_tty = tty->link;
 
-	if (tty_release_checks(tty, o_tty, idx)) {
+	if (tty_release_checks(tty, idx)) {
 		tty_unlock(tty);
 		return 0;
 	}
@@ -1739,7 +1790,9 @@ int tty_release(struct inode *inode, struct file *filp)
 	if (tty->ops->close)
 		tty->ops->close(tty, filp);
 
-	tty_unlock(tty);
+	/* If tty is pty master, lock the slave pty (stable lock order) */
+	tty_lock_slave(o_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
@@ -1750,25 +1803,13 @@ int tty_release(struct inode *inode, struct file *filp)
 	 * The test for the o_tty closing is necessary, since the master and
 	 * slave sides may close in any order.  If the slave side closes out
 	 * first, its count will be one, since the master side holds an open.
-	 * Thus this test wouldn't be triggered at the time the slave closes,
+	 * Thus this test wouldn't be triggered at the time the slave closed,
 	 * so we do it now.
-	 *
-	 * Note that it's possible for the tty to be opened again while we're
-	 * flushing out waiters.  By recalculating the closing flags before
-	 * each iteration we avoid any problems.
 	 */
 	while (1) {
-		/* Guard against races with tty->count changes elsewhere and
-		   opens on /dev/tty */
-
-		mutex_lock(&tty_mutex);
-		tty_lock_pair(tty, o_tty);
-		tty_closing = tty->count <= 1;
-		o_tty_closing = o_tty &&
-			(o_tty->count <= (pty_master ? 1 : 0));
 		do_sleep = 0;
 
-		if (tty_closing) {
+		if (tty->count <= 1) {
 			if (waitqueue_active(&tty->read_wait)) {
 				wake_up_poll(&tty->read_wait, POLLIN);
 				do_sleep++;
@@ -1778,7 +1819,7 @@ int tty_release(struct inode *inode, struct file *filp)
 				do_sleep++;
 			}
 		}
-		if (o_tty_closing) {
+		if (o_tty && o_tty->count <= 1) {
 			if (waitqueue_active(&o_tty->read_wait)) {
 				wake_up_poll(&o_tty->read_wait, POLLIN);
 				do_sleep++;
@@ -1796,8 +1837,6 @@ 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_pair(tty, o_tty);
-		mutex_unlock(&tty_mutex);
 		schedule_timeout_killable(timeout);
 		if (timeout < 120 * HZ)
 			timeout = 2 * timeout + 1;
@@ -1805,15 +1844,7 @@ int tty_release(struct inode *inode, struct file *filp)
 			timeout = MAX_SCHEDULE_TIMEOUT;
 	}
 
-	/*
-	 * The closing flags are now consistent with the open counts on
-	 * both sides, and we've completed the last operation that could
-	 * block, so it's safe to proceed with closing.
-	 *
-	 * We must *not* drop the tty_mutex until we ensure that a further
-	 * entry into tty_open can not pick up this tty.
-	 */
-	if (pty_master) {
+	if (o_tty) {
 		if (--o_tty->count < 0) {
 			printk(KERN_WARNING "%s: bad pty slave count (%d) for %s\n",
 				__func__, o_tty->count, tty_name(o_tty, buf));
@@ -1840,21 +1871,11 @@ int tty_release(struct inode *inode, struct file *filp)
 	/*
 	 * Perform some housekeeping before deciding whether to return.
 	 *
-	 * Set the TTY_CLOSING flag if this was the last open.  In the
-	 * case of a pty we may have to wait around for the other side
-	 * to close, and TTY_CLOSING makes sure we can't be reopened.
-	 */
-	if (tty_closing)
-		set_bit(TTY_CLOSING, &tty->flags);
-	if (o_tty_closing)
-		set_bit(TTY_CLOSING, &o_tty->flags);
-
-	/*
 	 * If _either_ side is closing, make sure there aren't any
 	 * processes that still think tty or o_tty is their controlling
 	 * tty.
 	 */
-	if (tty_closing || o_tty_closing) {
+	if (!tty->count) {
 		read_lock(&tasklist_lock);
 		session_clear_tty(tty->session);
 		if (o_tty)
@@ -1862,13 +1883,16 @@ int tty_release(struct inode *inode, struct file *filp)
 		read_unlock(&tasklist_lock);
 	}
 
-	mutex_unlock(&tty_mutex);
-	tty_unlock_pair(tty, o_tty);
-	/* At this point the TTY_CLOSING flag should ensure a dead tty
+	/* check whether both sides are closing ... */
+	final = !tty->count && !(o_tty && o_tty->count);
+
+	tty_unlock_slave(o_tty);
+	tty_unlock(tty);
+
+	/* At this point, the tty->count == 0 should ensure a dead tty
 	   cannot be re-opened by a racing opener */
 
-	/* check whether both sides are closing ... */
-	if (!tty_closing || (o_tty && !o_tty_closing))
+	if (!final)
 		return 0;
 
 #ifdef TTY_DEBUG_HANGUP
@@ -1877,12 +1901,10 @@ int tty_release(struct inode *inode, struct file *filp)
 	/*
 	 * Ask the line discipline code to release its structures
 	 */
-	tty_ldisc_release(tty, o_tty);
+	tty_ldisc_release(tty);
 
 	/* Wait for pending work before tty destruction commmences */
 	tty_flush_works(tty);
-	if (o_tty)
-		tty_flush_works(o_tty);
 
 #ifdef TTY_DEBUG_HANGUP
 	printk(KERN_DEBUG "%s: %s: freeing structure...\n", __func__, tty_name(tty, buf));
@@ -1901,20 +1923,20 @@ int tty_release(struct inode *inode, struct file *filp)
 }
 
 /**
- *	tty_open_current_tty - get tty of current task for open
+ *	tty_open_current_tty - get locked tty of current task
  *	@device: device number
  *	@filp: file pointer to tty
- *	@return: tty of the current task iff @device is /dev/tty
+ *	@return: locked tty of the current task iff @device is /dev/tty
+ *
+ *	Performs a re-open of the current task's controlling tty.
  *
  *	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)
 {
 	struct tty_struct *tty;
+	int retval;
 
 	if (device != MKDEV(TTYAUX_MAJOR, 0))
 		return NULL;
@@ -1925,9 +1947,14 @@ static struct tty_struct *tty_open_current_tty(dev_t device, struct file *filp)
 
 	filp->f_flags |= O_NONBLOCK; /* Don't let /dev/tty block */
 	/* 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 */
+	tty_lock(tty);
+	tty_kref_put(tty);	/* safe to drop the kref now */
+
+	retval = tty_reopen(tty);
+	if (retval < 0) {
+		tty_unlock(tty);
+		tty = ERR_PTR(retval);
+	}
 	return tty;
 }
 
@@ -2025,13 +2052,9 @@ retry_open:
 	index  = -1;
 	retval = 0;
 
-	mutex_lock(&tty_mutex);
-	/* This is protected by the tty_mutex */
 	tty = tty_open_current_tty(device, filp);
-	if (IS_ERR(tty)) {
-		retval = PTR_ERR(tty);
-		goto err_unlock;
-	} else if (!tty) {
+	if (!tty) {
+		mutex_lock(&tty_mutex);
 		driver = tty_lookup_driver(device, filp, &noctty, &index);
 		if (IS_ERR(driver)) {
 			retval = PTR_ERR(driver);
@@ -2044,21 +2067,25 @@ retry_open:
 			retval = PTR_ERR(tty);
 			goto err_unlock;
 		}
-	}
 
-	if (tty) {
-		tty_lock(tty);
-		retval = tty_reopen(tty);
-		if (retval < 0) {
-			tty_unlock(tty);
-			tty = ERR_PTR(retval);
+		if (tty) {
+			mutex_unlock(&tty_mutex);
+			tty_lock(tty);
+			/* safe to drop the kref from tty_driver_lookup_tty() */
+			tty_kref_put(tty);
+			retval = tty_reopen(tty);
+			if (retval < 0) {
+				tty_unlock(tty);
+				tty = ERR_PTR(retval);
+			}
+		} else { /* Returns with the tty_lock held for now */
+			tty = tty_init_dev(driver, index);
+			mutex_unlock(&tty_mutex);
 		}
-	} 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)) {
 		retval = PTR_ERR(tty);
 		goto err_file;
@@ -2100,25 +2127,23 @@ retry_open:
 		/*
 		 * Need to reset f_op in case a hangup happened.
 		 */
-		if (filp->f_op == &hung_up_tty_fops)
+		if (tty_hung_up_p(filp))
 			filp->f_op = &tty_fops;
 		goto retry_open;
 	}
 	clear_bit(TTY_HUPPED, &tty->flags);
-	tty_unlock(tty);
 
 
-	mutex_lock(&tty_mutex);
-	tty_lock(tty);
+	read_lock(&tasklist_lock);
 	spin_lock_irq(&current->sighand->siglock);
 	if (!noctty &&
 	    current->signal->leader &&
 	    !current->signal->tty &&
 	    tty->session == NULL)
-		__proc_set_tty(current, tty);
+		__proc_set_tty(tty);
 	spin_unlock_irq(&current->sighand->siglock);
+	read_unlock(&tasklist_lock);
 	tty_unlock(tty);
-	mutex_unlock(&tty_mutex);
 	return 0;
 err_unlock:
 	mutex_unlock(&tty_mutex);
@@ -2155,7 +2180,7 @@ static unsigned int tty_poll(struct file *filp, poll_table *wait)
 
 	ld = tty_ldisc_ref_wait(tty);
 	if (ld->ops->poll)
-		ret = (ld->ops->poll)(tty, filp, wait);
+		ret = ld->ops->poll(tty, filp, wait);
 	tty_ldisc_deref(ld);
 	return ret;
 }
@@ -2283,18 +2308,14 @@ static int tiocgwinsz(struct tty_struct *tty, struct winsize __user *arg)
 int tty_do_resize(struct tty_struct *tty, struct winsize *ws)
 {
 	struct pid *pgrp;
-	unsigned long flags;
 
 	/* Lock the tty */
 	mutex_lock(&tty->winsize_mutex);
 	if (!memcmp(ws, &tty->winsize, sizeof(*ws)))
 		goto done;
-	/* Get the PID values and reference them so we can
-	   avoid holding the tty ctrl lock while sending signals */
-	spin_lock_irqsave(&tty->ctrl_lock, flags);
-	pgrp = get_pid(tty->pgrp);
-	spin_unlock_irqrestore(&tty->ctrl_lock, flags);
 
+	/* Signal the foreground process group */
+	pgrp = tty_get_pgrp(tty);
 	if (pgrp)
 		kill_pgrp(pgrp, SIGWINCH, 1);
 	put_pid(pgrp);
@@ -2403,7 +2424,7 @@ static int fionbio(struct file *file, int __user *p)
  *	leader to set this tty as the controlling tty for the session.
  *
  *	Locking:
- *		Takes tty_mutex() to protect tty instance
+ *		Takes tty_lock() to serialize proc_set_tty() for this tty
  *		Takes tasklist_lock internally to walk sessions
  *		Takes ->siglock() when updating signal->tty
  */
@@ -2411,10 +2432,13 @@ static int fionbio(struct file *file, int __user *p)
 static int tiocsctty(struct tty_struct *tty, int arg)
 {
 	int ret = 0;
+
+	tty_lock(tty);
+	read_lock(&tasklist_lock);
+
 	if (current->signal->leader && (task_session(current) == tty->session))
-		return ret;
+		goto unlock;
 
-	mutex_lock(&tty_mutex);
 	/*
 	 * The process must be a session leader and
 	 * not have a controlling tty already.
@@ -2433,17 +2457,16 @@ static int tiocsctty(struct tty_struct *tty, int arg)
 			/*
 			 * Steal it away
 			 */
-			read_lock(&tasklist_lock);
 			session_clear_tty(tty->session);
-			read_unlock(&tasklist_lock);
 		} else {
 			ret = -EPERM;
 			goto unlock;
 		}
 	}
-	proc_set_tty(current, tty);
+	proc_set_tty(tty);
 unlock:
-	mutex_unlock(&tty_mutex);
+	read_unlock(&tasklist_lock);
+	tty_unlock(tty);
 	return ret;
 }
 
@@ -2468,6 +2491,27 @@ struct pid *tty_get_pgrp(struct tty_struct *tty)
 }
 EXPORT_SYMBOL_GPL(tty_get_pgrp);
 
+/*
+ * This checks not only the pgrp, but falls back on the pid if no
+ * satisfactory pgrp is found. I dunno - gdb doesn't work correctly
+ * without this...
+ *
+ * The caller must hold rcu lock or the tasklist lock.
+ */
+static struct pid *session_of_pgrp(struct pid *pgrp)
+{
+	struct task_struct *p;
+	struct pid *sid = NULL;
+
+	p = pid_task(pgrp, PIDTYPE_PGID);
+	if (p == NULL)
+		p = pid_task(pgrp, PIDTYPE_PID);
+	if (p != NULL)
+		sid = task_session(p);
+
+	return sid;
+}
+
 /**
  *	tiocgpgrp		-	get process group
  *	@tty: tty passed by user
@@ -2714,23 +2758,35 @@ static int tty_tiocgicount(struct tty_struct *tty, void __user *arg)
 	return 0;
 }
 
-struct tty_struct *tty_pair_get_tty(struct tty_struct *tty)
+static void tty_warn_deprecated_flags(struct serial_struct __user *ss)
 {
-	if (tty->driver->type == TTY_DRIVER_TYPE_PTY &&
-	    tty->driver->subtype == PTY_TYPE_MASTER)
-		tty = tty->link;
-	return tty;
+	static DEFINE_RATELIMIT_STATE(depr_flags,
+			DEFAULT_RATELIMIT_INTERVAL,
+			DEFAULT_RATELIMIT_BURST);
+	char comm[TASK_COMM_LEN];
+	int flags;
+
+	if (get_user(flags, &ss->flags))
+		return;
+
+	flags &= ASYNC_DEPRECATED;
+
+	if (flags && __ratelimit(&depr_flags))
+		pr_warning("%s: '%s' is using deprecated serial flags (with no effect): %.8x\n",
+				__func__, get_task_comm(comm, current), flags);
 }
-EXPORT_SYMBOL(tty_pair_get_tty);
 
-struct tty_struct *tty_pair_get_pty(struct tty_struct *tty)
+/*
+ * if pty, return the slave side (real_tty)
+ * otherwise, return self
+ */
+static struct tty_struct *tty_pair_get_tty(struct tty_struct *tty)
 {
 	if (tty->driver->type == TTY_DRIVER_TYPE_PTY &&
 	    tty->driver->subtype == PTY_TYPE_MASTER)
-	    return tty;
-	return tty->link;
+		tty = tty->link;
+	return tty;
 }
-EXPORT_SYMBOL(tty_pair_get_pty);
 
 /*
  * Split this up, as gcc can choke on it otherwise..
@@ -2859,13 +2915,16 @@ long tty_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		case TCIFLUSH:
 		case TCIOFLUSH:
 		/* flush tty buffer and allow ldisc to process ioctl */
-			tty_buffer_flush(tty);
+			tty_buffer_flush(tty, NULL);
 			break;
 		}
 		break;
+	case TIOCSSERIAL:
+		tty_warn_deprecated_flags(p);
+		break;
 	}
 	if (tty->ops->ioctl) {
-		retval = (tty->ops->ioctl)(tty, cmd, arg);
+		retval = tty->ops->ioctl(tty, cmd, arg);
 		if (retval != -ENOIOCTLCMD)
 			return retval;
 	}
@@ -2892,7 +2951,7 @@ static long tty_compat_ioctl(struct file *file, unsigned int cmd,
 		return -EINVAL;
 
 	if (tty->ops->compat_ioctl) {
-		retval = (tty->ops->compat_ioctl)(tty, cmd, arg);
+		retval = tty->ops->compat_ioctl(tty, cmd, arg);
 		if (retval != -ENOIOCTLCMD)
 			return retval;
 	}
@@ -3443,59 +3502,6 @@ dev_t tty_devnum(struct tty_struct *tty)
 }
 EXPORT_SYMBOL(tty_devnum);
 
-void proc_clear_tty(struct task_struct *p)
-{
-	unsigned long flags;
-	struct tty_struct *tty;
-	spin_lock_irqsave(&p->sighand->siglock, flags);
-	tty = p->signal->tty;
-	p->signal->tty = NULL;
-	spin_unlock_irqrestore(&p->sighand->siglock, flags);
-	tty_kref_put(tty);
-}
-
-/* Called under the sighand lock */
-
-static void __proc_set_tty(struct task_struct *tsk, struct tty_struct *tty)
-{
-	if (tty) {
-		unsigned long flags;
-		/* We should not have a session or pgrp to put here but.... */
-		spin_lock_irqsave(&tty->ctrl_lock, flags);
-		put_pid(tty->session);
-		put_pid(tty->pgrp);
-		tty->pgrp = get_pid(task_pgrp(tsk));
-		spin_unlock_irqrestore(&tty->ctrl_lock, flags);
-		tty->session = get_pid(task_session(tsk));
-		if (tsk->signal->tty) {
-			printk(KERN_DEBUG "tty not NULL!!\n");
-			tty_kref_put(tsk->signal->tty);
-		}
-	}
-	put_pid(tsk->signal->tty_old_pgrp);
-	tsk->signal->tty = tty_kref_get(tty);
-	tsk->signal->tty_old_pgrp = NULL;
-}
-
-static void proc_set_tty(struct task_struct *tsk, struct tty_struct *tty)
-{
-	spin_lock_irq(&tsk->sighand->siglock);
-	__proc_set_tty(tsk, tty);
-	spin_unlock_irq(&tsk->sighand->siglock);
-}
-
-struct tty_struct *get_current_tty(void)
-{
-	struct tty_struct *tty;
-	unsigned long flags;
-
-	spin_lock_irqsave(&current->sighand->siglock, flags);
-	tty = tty_kref_get(current->signal->tty);
-	spin_unlock_irqrestore(&current->sighand->siglock, flags);
-	return tty;
-}
-EXPORT_SYMBOL_GPL(get_current_tty);
-
 void tty_default_fops(struct file_operations *fops)
 {
 	*fops = tty_fops;
diff --git a/drivers/tty/tty_ioctl.c b/drivers/tty/tty_ioctl.c
index 62380ccf70fb..1787fa4d9448 100644
--- a/drivers/tty/tty_ioctl.c
+++ b/drivers/tty/tty_ioctl.c
@@ -524,9 +524,8 @@ EXPORT_SYMBOL(tty_termios_hw_change);
  *	@tty: tty to update
  *	@new_termios: desired new value
  *
- *	Perform updates to the termios values set on this terminal. There
- *	is a bit of layering violation here with n_tty in terms of the
- *	internal knowledge of this function.
+ *	Perform updates to the termios values set on this terminal.
+ *	A master pty's termios should never be set.
  *
  *	Locking: termios_rwsem
  */
@@ -535,8 +534,9 @@ int tty_set_termios(struct tty_struct *tty, struct ktermios *new_termios)
 {
 	struct ktermios old_termios;
 	struct tty_ldisc *ld;
-	unsigned long flags;
 
+	WARN_ON(tty->driver->type == TTY_DRIVER_TYPE_PTY &&
+		tty->driver->subtype == PTY_TYPE_MASTER);
 	/*
 	 *	Perform the actual termios internal changes under lock.
 	 */
@@ -549,41 +549,15 @@ int tty_set_termios(struct tty_struct *tty, struct ktermios *new_termios)
 	tty->termios = *new_termios;
 	unset_locked_termios(&tty->termios, &old_termios, &tty->termios_locked);
 
-	/* See if packet mode change of state. */
-	if (tty->link && tty->link->packet) {
-		int extproc = (old_termios.c_lflag & EXTPROC) |
-				(tty->termios.c_lflag & EXTPROC);
-		int old_flow = ((old_termios.c_iflag & IXON) &&
-				(old_termios.c_cc[VSTOP] == '\023') &&
-				(old_termios.c_cc[VSTART] == '\021'));
-		int new_flow = (I_IXON(tty) &&
-				STOP_CHAR(tty) == '\023' &&
-				START_CHAR(tty) == '\021');
-		if ((old_flow != new_flow) || extproc) {
-			spin_lock_irqsave(&tty->ctrl_lock, flags);
-			if (old_flow != new_flow) {
-				tty->ctrl_status &= ~(TIOCPKT_DOSTOP | TIOCPKT_NOSTOP);
-				if (new_flow)
-					tty->ctrl_status |= TIOCPKT_DOSTOP;
-				else
-					tty->ctrl_status |= TIOCPKT_NOSTOP;
-			}
-			if (extproc)
-				tty->ctrl_status |= TIOCPKT_IOCTL;
-			spin_unlock_irqrestore(&tty->ctrl_lock, flags);
-			wake_up_interruptible(&tty->link->read_wait);
-		}
-	}
-
 	if (tty->ops->set_termios)
-		(*tty->ops->set_termios)(tty, &old_termios);
+		tty->ops->set_termios(tty, &old_termios);
 	else
 		tty_termios_copy_hw(&tty->termios, &old_termios);
 
 	ld = tty_ldisc_ref(tty);
 	if (ld != NULL) {
 		if (ld->ops->set_termios)
-			(ld->ops->set_termios)(tty, &old_termios);
+			ld->ops->set_termios(tty, &old_termios);
 		tty_ldisc_deref(ld);
 	}
 	up_write(&tty->termios_rwsem);
diff --git a/drivers/tty/tty_ldisc.c b/drivers/tty/tty_ldisc.c
index 2d822aa259b2..3737f55272d2 100644
--- a/drivers/tty/tty_ldisc.c
+++ b/drivers/tty/tty_ldisc.c
@@ -308,48 +308,66 @@ EXPORT_SYMBOL_GPL(tty_ldisc_deref);
 
 
 static inline int __lockfunc
-tty_ldisc_lock(struct tty_struct *tty, unsigned long timeout)
+__tty_ldisc_lock(struct tty_struct *tty, unsigned long timeout)
 {
 	return ldsem_down_write(&tty->ldisc_sem, timeout);
 }
 
 static inline int __lockfunc
-tty_ldisc_lock_nested(struct tty_struct *tty, unsigned long timeout)
+__tty_ldisc_lock_nested(struct tty_struct *tty, unsigned long timeout)
 {
 	return ldsem_down_write_nested(&tty->ldisc_sem,
 				       LDISC_SEM_OTHER, timeout);
 }
 
-static inline void tty_ldisc_unlock(struct tty_struct *tty)
+static inline void __tty_ldisc_unlock(struct tty_struct *tty)
 {
 	return ldsem_up_write(&tty->ldisc_sem);
 }
 
 static int __lockfunc
+tty_ldisc_lock(struct tty_struct *tty, unsigned long timeout)
+{
+	int ret;
+
+	ret = __tty_ldisc_lock(tty, timeout);
+	if (!ret)
+		return -EBUSY;
+	set_bit(TTY_LDISC_HALTED, &tty->flags);
+	return 0;
+}
+
+static void tty_ldisc_unlock(struct tty_struct *tty)
+{
+	clear_bit(TTY_LDISC_HALTED, &tty->flags);
+	__tty_ldisc_unlock(tty);
+}
+
+static int __lockfunc
 tty_ldisc_lock_pair_timeout(struct tty_struct *tty, struct tty_struct *tty2,
 			    unsigned long timeout)
 {
 	int ret;
 
 	if (tty < tty2) {
-		ret = tty_ldisc_lock(tty, timeout);
+		ret = __tty_ldisc_lock(tty, timeout);
 		if (ret) {
-			ret = tty_ldisc_lock_nested(tty2, timeout);
+			ret = __tty_ldisc_lock_nested(tty2, timeout);
 			if (!ret)
-				tty_ldisc_unlock(tty);
+				__tty_ldisc_unlock(tty);
 		}
 	} else {
 		/* if this is possible, it has lots of implications */
 		WARN_ON_ONCE(tty == tty2);
 		if (tty2 && tty != tty2) {
-			ret = tty_ldisc_lock(tty2, timeout);
+			ret = __tty_ldisc_lock(tty2, timeout);
 			if (ret) {
-				ret = tty_ldisc_lock_nested(tty, timeout);
+				ret = __tty_ldisc_lock_nested(tty, timeout);
 				if (!ret)
-					tty_ldisc_unlock(tty2);
+					__tty_ldisc_unlock(tty2);
 			}
 		} else
-			ret = tty_ldisc_lock(tty, timeout);
+			ret = __tty_ldisc_lock(tty, timeout);
 	}
 
 	if (!ret)
@@ -370,38 +388,26 @@ tty_ldisc_lock_pair(struct tty_struct *tty, struct tty_struct *tty2)
 static void __lockfunc tty_ldisc_unlock_pair(struct tty_struct *tty,
 					     struct tty_struct *tty2)
 {
-	tty_ldisc_unlock(tty);
-	if (tty2)
-		tty_ldisc_unlock(tty2);
-}
-
-static void __lockfunc tty_ldisc_enable_pair(struct tty_struct *tty,
-					     struct tty_struct *tty2)
-{
-	clear_bit(TTY_LDISC_HALTED, &tty->flags);
+	__tty_ldisc_unlock(tty);
 	if (tty2)
-		clear_bit(TTY_LDISC_HALTED, &tty2->flags);
-
-	tty_ldisc_unlock_pair(tty, tty2);
+		__tty_ldisc_unlock(tty2);
 }
 
 /**
  *	tty_ldisc_flush	-	flush line discipline queue
  *	@tty: tty
  *
- *	Flush the line discipline queue (if any) for this tty. If there
- *	is no line discipline active this is a no-op.
+ *	Flush the line discipline queue (if any) and the tty flip buffers
+ *	for this tty.
  */
 
 void tty_ldisc_flush(struct tty_struct *tty)
 {
 	struct tty_ldisc *ld = tty_ldisc_ref(tty);
-	if (ld) {
-		if (ld->ops->flush_buffer)
-			ld->ops->flush_buffer(tty);
+
+	tty_buffer_flush(tty, ld);
+	if (ld)
 		tty_ldisc_deref(ld);
-	}
-	tty_buffer_flush(tty);
 }
 EXPORT_SYMBOL_GPL(tty_ldisc_flush);
 
@@ -517,15 +523,16 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 {
 	int retval;
 	struct tty_ldisc *old_ldisc, *new_ldisc;
-	struct tty_struct *o_tty = tty->link;
 
 	new_ldisc = tty_ldisc_get(tty, ldisc);
 	if (IS_ERR(new_ldisc))
 		return PTR_ERR(new_ldisc);
 
-	retval = tty_ldisc_lock_pair_timeout(tty, o_tty, 5 * HZ);
+	tty_lock(tty);
+	retval = tty_ldisc_lock(tty, 5 * HZ);
 	if (retval) {
 		tty_ldisc_put(new_ldisc);
+		tty_unlock(tty);
 		return retval;
 	}
 
@@ -534,19 +541,18 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 	 */
 
 	if (tty->ldisc->ops->num == ldisc) {
-		tty_ldisc_enable_pair(tty, o_tty);
+		tty_ldisc_unlock(tty);
 		tty_ldisc_put(new_ldisc);
+		tty_unlock(tty);
 		return 0;
 	}
 
 	old_ldisc = tty->ldisc;
-	tty_lock(tty);
 
-	if (test_bit(TTY_HUPPING, &tty->flags) ||
-	    test_bit(TTY_HUPPED, &tty->flags)) {
+	if (test_bit(TTY_HUPPED, &tty->flags)) {
 		/* We were raced by the hangup method. It will have stomped
 		   the ldisc data and closed the ldisc down */
-		tty_ldisc_enable_pair(tty, o_tty);
+		tty_ldisc_unlock(tty);
 		tty_ldisc_put(new_ldisc);
 		tty_unlock(tty);
 		return -EIO;
@@ -566,8 +572,11 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 		tty_ldisc_restore(tty, old_ldisc);
 	}
 
-	if (tty->ldisc->ops->num != old_ldisc->ops->num && tty->ops->set_ldisc)
+	if (tty->ldisc->ops->num != old_ldisc->ops->num && tty->ops->set_ldisc) {
+		down_read(&tty->termios_rwsem);
 		tty->ops->set_ldisc(tty);
+		up_read(&tty->termios_rwsem);
+	}
 
 	/* At this point we hold a reference to the new ldisc and a
 	   reference to the old ldisc, or we hold two references to
@@ -580,13 +589,11 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 	/*
 	 *	Allow ldisc referencing to occur again
 	 */
-	tty_ldisc_enable_pair(tty, o_tty);
+	tty_ldisc_unlock(tty);
 
 	/* Restart the work queue in case no characters kick it off. Safe if
 	   already running */
 	schedule_work(&tty->port->buf.work);
-	if (o_tty)
-		schedule_work(&o_tty->port->buf.work);
 
 	tty_unlock(tty);
 	return retval;
@@ -675,16 +682,13 @@ void tty_ldisc_hangup(struct tty_struct *tty)
 	wake_up_interruptible_poll(&tty->write_wait, POLLOUT);
 	wake_up_interruptible_poll(&tty->read_wait, POLLIN);
 
-	tty_unlock(tty);
-
 	/*
 	 * Shutdown the current line discipline, and reset it to
 	 * N_TTY if need be.
 	 *
 	 * Avoid racing set_ldisc or tty_ldisc_release
 	 */
-	tty_ldisc_lock_pair(tty, tty->link);
-	tty_lock(tty);
+	tty_ldisc_lock(tty, MAX_SCHEDULE_TIMEOUT);
 
 	if (tty->ldisc) {
 
@@ -706,7 +710,7 @@ void tty_ldisc_hangup(struct tty_struct *tty)
 			WARN_ON(tty_ldisc_open(tty, tty->ldisc));
 		}
 	}
-	tty_ldisc_enable_pair(tty, tty->link);
+	tty_ldisc_unlock(tty);
 	if (reset)
 		tty_reset_termios(tty);
 
@@ -758,16 +762,17 @@ static void tty_ldisc_kill(struct tty_struct *tty)
 
 /**
  *	tty_ldisc_release		-	release line discipline
- *	@tty: tty being shut down
- *	@o_tty: pair tty for pty/tty pairs
+ *	@tty: tty being shut down (or one end of pty pair)
  *
- *	Called during the final close of a tty/pty pair in order to shut down
- *	the line discpline layer. On exit the ldisc assigned is N_TTY and the
- *	ldisc has not been opened.
+ *	Called during the final close of a tty or a pty pair in order to shut
+ *	down the line discpline layer. On exit, each ldisc assigned is N_TTY and
+ *	each ldisc has not been opened.
  */
 
-void tty_ldisc_release(struct tty_struct *tty, struct tty_struct *o_tty)
+void tty_ldisc_release(struct tty_struct *tty)
 {
+	struct tty_struct *o_tty = tty->link;
+
 	/*
 	 * Shutdown this line discipline. As this is the final close,
 	 * it does not race with the set_ldisc code path.
@@ -776,13 +781,9 @@ void tty_ldisc_release(struct tty_struct *tty, struct tty_struct *o_tty)
 	tty_ldisc_debug(tty, "closing ldisc: %p\n", tty->ldisc);
 
 	tty_ldisc_lock_pair(tty, o_tty);
-	tty_lock_pair(tty, o_tty);
-
 	tty_ldisc_kill(tty);
 	if (o_tty)
 		tty_ldisc_kill(o_tty);
-
-	tty_unlock_pair(tty, o_tty);
 	tty_ldisc_unlock_pair(tty, o_tty);
 
 	/* And the memory resources remaining (buffers, termios) will be
diff --git a/drivers/tty/tty_mutex.c b/drivers/tty/tty_mutex.c
index 2e41abebbcba..4486741190c4 100644
--- a/drivers/tty/tty_mutex.c
+++ b/drivers/tty/tty_mutex.c
@@ -4,19 +4,23 @@
 #include <linux/semaphore.h>
 #include <linux/sched.h>
 
+/*
+ * Nested tty locks are necessary for releasing pty pairs.
+ * The stable lock order is master pty first, then slave pty.
+ */
+
 /* Legacy tty mutex glue */
 
 enum {
 	TTY_MUTEX_NORMAL,
-	TTY_MUTEX_NESTED,
+	TTY_MUTEX_SLAVE,
 };
 
 /*
  * Getting the big tty mutex.
  */
 
-static void __lockfunc tty_lock_nested(struct tty_struct *tty,
-				       unsigned int subclass)
+void __lockfunc tty_lock(struct tty_struct *tty)
 {
 	if (tty->magic != TTY_MAGIC) {
 		pr_err("L Bad %p\n", tty);
@@ -24,12 +28,7 @@ static void __lockfunc tty_lock_nested(struct tty_struct *tty,
 		return;
 	}
 	tty_kref_get(tty);
-	mutex_lock_nested(&tty->legacy_mutex, subclass);
-}
-
-void __lockfunc tty_lock(struct tty_struct *tty)
-{
-	return tty_lock_nested(tty, TTY_MUTEX_NORMAL);
+	mutex_lock(&tty->legacy_mutex);
 }
 EXPORT_SYMBOL(tty_lock);
 
@@ -45,29 +44,23 @@ void __lockfunc tty_unlock(struct tty_struct *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)
+void __lockfunc tty_lock_slave(struct tty_struct *tty)
 {
-	if (tty < tty2) {
+	if (tty && tty != tty->link) {
+		WARN_ON(!mutex_is_locked(&tty->link->legacy_mutex) ||
+			!tty->driver->type == TTY_DRIVER_TYPE_PTY ||
+			!tty->driver->type == PTY_TYPE_SLAVE);
 		tty_lock(tty);
-		tty_lock_nested(tty2, TTY_MUTEX_NESTED);
-	} else {
-		if (tty2 && tty2 != tty)
-			tty_lock(tty2);
-		tty_lock_nested(tty, TTY_MUTEX_NESTED);
 	}
 }
-EXPORT_SYMBOL(tty_lock_pair);
 
-void __lockfunc tty_unlock_pair(struct tty_struct *tty,
-						struct tty_struct *tty2)
+void __lockfunc tty_unlock_slave(struct tty_struct *tty)
+{
+	if (tty && tty != tty->link)
+		tty_unlock(tty);
+}
+
+void tty_set_lock_subclass(struct tty_struct *tty)
 {
-	tty_unlock(tty);
-	if (tty2 && tty2 != tty)
-		tty_unlock(tty2);
+	lockdep_set_subclass(&tty->legacy_mutex, TTY_MUTEX_SLAVE);
 }
-EXPORT_SYMBOL(tty_unlock_pair);
diff --git a/drivers/tty/tty_port.c b/drivers/tty/tty_port.c
index 1b9335796da4..40b31835f80b 100644
--- a/drivers/tty/tty_port.c
+++ b/drivers/tty/tty_port.c
@@ -193,8 +193,7 @@ void tty_port_tty_set(struct tty_port *port, struct tty_struct *tty)
 	unsigned long flags;
 
 	spin_lock_irqsave(&port->lock, flags);
-	if (port->tty)
-		tty_kref_put(port->tty);
+	tty_kref_put(port->tty);
 	port->tty = tty_kref_get(tty);
 	spin_unlock_irqrestore(&port->lock, flags);
 }
@@ -473,12 +472,10 @@ int tty_port_close_start(struct tty_port *port,
 {
 	unsigned long flags;
 
-	spin_lock_irqsave(&port->lock, flags);
-	if (tty_hung_up_p(filp)) {
-		spin_unlock_irqrestore(&port->lock, flags);
+	if (tty_hung_up_p(filp))
 		return 0;
-	}
 
+	spin_lock_irqsave(&port->lock, flags);
 	if (tty->count == 1 && port->count != 1) {
 		printk(KERN_WARNING
 		    "tty_port_close_start: tty->count = 1 port count = %d.\n",
@@ -522,6 +519,7 @@ void tty_port_close_end(struct tty_port *port, struct tty_struct *tty)
 {
 	unsigned long flags;
 
+	tty_ldisc_flush(tty);
 	tty->closing = 0;
 
 	spin_lock_irqsave(&port->lock, flags);
diff --git a/drivers/tty/vt/keyboard.c b/drivers/tty/vt/keyboard.c
index c039cfea5b11..8a89f6e7715d 100644
--- a/drivers/tty/vt/keyboard.c
+++ b/drivers/tty/vt/keyboard.c
@@ -924,7 +924,7 @@ static void k_brl(struct vc_data *vc, unsigned char value, char up_flag)
 
 	if (kbd->kbdmode != VC_UNICODE) {
 		if (!up_flag)
-			pr_warning("keyboard mode must be unicode for braille patterns\n");
+			pr_warn("keyboard mode must be unicode for braille patterns\n");
 		return;
 	}
 
@@ -1253,8 +1253,8 @@ static void kbd_keycode(unsigned int keycode, int down, int hw_raw)
 	if (raw_mode && !hw_raw)
 		if (emulate_raw(vc, keycode, !down << 7))
 			if (keycode < BTN_MISC && printk_ratelimit())
-				pr_warning("can't emulate rawmode for keycode %d\n",
-					   keycode);
+				pr_warn("can't emulate rawmode for keycode %d\n",
+					keycode);
 
 #ifdef CONFIG_SPARC
 	if (keycode == KEY_A && sparc_l1_a_state) {
diff --git a/drivers/tty/vt/vt.c b/drivers/tty/vt/vt.c
index b33b00b386de..f3fbbbca9bde 100644
--- a/drivers/tty/vt/vt.c
+++ b/drivers/tty/vt/vt.c
@@ -156,6 +156,8 @@ static void console_callback(struct work_struct *ignored);
 static void blank_screen_t(unsigned long dummy);
 static void set_palette(struct vc_data *vc);
 
+#define vt_get_kmsg_redirect() vt_kmsg_redirect(-1)
+
 static int printable;		/* Is console ready for printing? */
 int default_utf8 = true;
 module_param(default_utf8, int, S_IRUGO | S_IWUSR);
@@ -1367,9 +1369,11 @@ static void csi_m(struct vc_data *vc)
 						rgb_from_256(vc->vc_par[i]));
 				} else if (vc->vc_par[i] == 2 &&  /* 24 bit */
 				           i <= vc->vc_npar + 3) {/* extremely rare */
-					struct rgb c = {r:vc->vc_par[i+1],
-							g:vc->vc_par[i+2],
-							b:vc->vc_par[i+3]};
+					struct rgb c = {
+						.r = vc->vc_par[i + 1],
+						.g = vc->vc_par[i + 2],
+						.b = vc->vc_par[i + 3],
+					};
 					rgb_foreground(vc, c);
 					i += 3;
 				}
@@ -1388,9 +1392,11 @@ static void csi_m(struct vc_data *vc)
 						rgb_from_256(vc->vc_par[i]));
 				} else if (vc->vc_par[i] == 2 && /* 24 bit */
 				           i <= vc->vc_npar + 3) {
-					struct rgb c = {r:vc->vc_par[i+1],
-							g:vc->vc_par[i+2],
-							b:vc->vc_par[i+3]};
+					struct rgb c = {
+						.r = vc->vc_par[i + 1],
+						.g = vc->vc_par[i + 2],
+						.b = vc->vc_par[i + 3],
+					};
 					rgb_background(vc, c);
 					i += 3;
 				}
@@ -3849,8 +3855,8 @@ void do_unblank_screen(int leaving_gfx)
 		return;
 	if (!vc_cons_allocated(fg_console)) {
 		/* impossible */
-		pr_warning("unblank_screen: tty %d not allocated ??\n",
-			   fg_console+1);
+		pr_warn("unblank_screen: tty %d not allocated ??\n",
+			fg_console + 1);
 		return;
 	}
 	vc = vc_cons[fg_console].d;
diff --git a/drivers/uio/uio.c b/drivers/uio/uio.c
index 60fa6278fbce..6276f13e9e12 100644
--- a/drivers/uio/uio.c
+++ b/drivers/uio/uio.c
@@ -56,12 +56,12 @@ static ssize_t map_name_show(struct uio_mem *mem, char *buf)
 
 static ssize_t map_addr_show(struct uio_mem *mem, char *buf)
 {
-	return sprintf(buf, "0x%llx\n", (unsigned long long)mem->addr);
+	return sprintf(buf, "%pa\n", &mem->addr);
 }
 
 static ssize_t map_size_show(struct uio_mem *mem, char *buf)
 {
-	return sprintf(buf, "0x%lx\n", mem->size);
+	return sprintf(buf, "%pa\n", &mem->size);
 }
 
 static ssize_t map_offset_show(struct uio_mem *mem, char *buf)
diff --git a/drivers/uio/uio_dmem_genirq.c b/drivers/uio/uio_dmem_genirq.c
index 8d0bba469566..915facbf552e 100644
--- a/drivers/uio/uio_dmem_genirq.c
+++ b/drivers/uio/uio_dmem_genirq.c
@@ -343,7 +343,6 @@ static struct platform_driver uio_dmem_genirq = {
 	.remove = uio_dmem_genirq_remove,
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.pm = &uio_dmem_genirq_dev_pm_ops,
 		.of_match_table = of_match_ptr(uio_of_genirq_match),
 	},
diff --git a/drivers/uio/uio_pdrv_genirq.c b/drivers/uio/uio_pdrv_genirq.c
index 76669313e9a7..f598ecddc8a7 100644
--- a/drivers/uio/uio_pdrv_genirq.c
+++ b/drivers/uio/uio_pdrv_genirq.c
@@ -266,7 +266,6 @@ static struct platform_driver uio_pdrv_genirq = {
 	.remove = uio_pdrv_genirq_remove,
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.pm = &uio_pdrv_genirq_dev_pm_ops,
 		.of_match_table = of_match_ptr(uio_of_genirq_match),
 	},
diff --git a/drivers/uio/uio_pruss.c b/drivers/uio/uio_pruss.c
index c28d6e2e3df2..818735bb8c3a 100644
--- a/drivers/uio/uio_pruss.c
+++ b/drivers/uio/uio_pruss.c
@@ -231,7 +231,6 @@ static struct platform_driver pruss_driver = {
 	.remove = pruss_remove,
 	.driver = {
 		   .name = DRV_NAME,
-		   .owner = THIS_MODULE,
 		   },
 };
 
diff --git a/drivers/usb/c67x00/c67x00-drv.c b/drivers/usb/c67x00/c67x00-drv.c
index 8db3380c3329..5796c8820514 100644
--- a/drivers/usb/c67x00/c67x00-drv.c
+++ b/drivers/usb/c67x00/c67x00-drv.c
@@ -221,7 +221,6 @@ static struct platform_driver c67x00_driver = {
 	.probe	= c67x00_drv_probe,
 	.remove	= c67x00_drv_remove,
 	.driver	= {
-		.owner = THIS_MODULE,
 		.name = "c67x00",
 	},
 };
diff --git a/drivers/usb/chipidea/Makefile b/drivers/usb/chipidea/Makefile
index 2f099c7df7b5..1fc86a2ca22d 100644
--- a/drivers/usb/chipidea/Makefile
+++ b/drivers/usb/chipidea/Makefile
@@ -10,6 +10,7 @@ ci_hdrc-$(CONFIG_USB_OTG_FSM)		+= otg_fsm.o
 
 # Glue/Bridge layers go here
 
+obj-$(CONFIG_USB_CHIPIDEA)	+= ci_hdrc_usb2.o
 obj-$(CONFIG_USB_CHIPIDEA)	+= ci_hdrc_msm.o
 obj-$(CONFIG_USB_CHIPIDEA)	+= ci_hdrc_zevio.o
 
diff --git a/drivers/usb/chipidea/ci.h b/drivers/usb/chipidea/ci.h
index ea40626e0246..65913d48f0c8 100644
--- a/drivers/usb/chipidea/ci.h
+++ b/drivers/usb/chipidea/ci.h
@@ -161,7 +161,8 @@ struct hw_bank {
  * @test_mode: the selected test mode
  * @platdata: platform specific information supplied by parent device
  * @vbus_active: is VBUS active
- * @transceiver: pointer to USB PHY, if any
+ * @phy: pointer to PHY, if any
+ * @usb_phy: pointer to USB PHY, if any and if using the USB PHY framework
  * @hcd: pointer to usb_hcd for ehci host driver
  * @debugfs: root dentry for this controller in debugfs
  * @id_event: indicates there is an id event, and handled at ci_otg_work
@@ -177,6 +178,7 @@ struct ci_hdrc {
 	struct ci_role_driver		*roles[CI_ROLE_END];
 	enum ci_role			role;
 	bool				is_otg;
+	struct usb_otg			otg;
 	struct otg_fsm			fsm;
 	struct ci_otg_fsm_timer_list	*fsm_timer;
 	struct work_struct		work;
@@ -201,7 +203,9 @@ struct ci_hdrc {
 
 	struct ci_hdrc_platform_data	*platdata;
 	int				vbus_active;
-	struct usb_phy			*transceiver;
+	struct phy			*phy;
+	/* old usb_phy interface */
+	struct usb_phy			*usb_phy;
 	struct usb_hcd			*hcd;
 	struct dentry			*debugfs;
 	bool				id_event;
@@ -348,7 +352,7 @@ u32 hw_read_intr_enable(struct ci_hdrc *ci);
 
 u32 hw_read_intr_status(struct ci_hdrc *ci);
 
-int hw_device_reset(struct ci_hdrc *ci, u32 mode);
+int hw_device_reset(struct ci_hdrc *ci);
 
 int hw_port_test_set(struct ci_hdrc *ci, u8 mode);
 
diff --git a/drivers/usb/chipidea/ci_hdrc_imx.c b/drivers/usb/chipidea/ci_hdrc_imx.c
index a7ab0f15926e..0f05de7c6b6c 100644
--- a/drivers/usb/chipidea/ci_hdrc_imx.c
+++ b/drivers/usb/chipidea/ci_hdrc_imx.c
@@ -106,8 +106,7 @@ static int ci_hdrc_imx_probe(struct platform_device *pdev)
 	struct ci_hdrc_platform_data pdata = {
 		.name		= dev_name(&pdev->dev),
 		.capoffset	= DEF_CAPOFFSET,
-		.flags		= CI_HDRC_REQUIRE_TRANSCEIVER |
-				  CI_HDRC_DISABLE_STREAMING,
+		.flags		= CI_HDRC_DISABLE_STREAMING,
 	};
 	int ret;
 	const struct of_device_id *of_id =
@@ -115,10 +114,8 @@ static int ci_hdrc_imx_probe(struct platform_device *pdev)
 	const struct ci_hdrc_imx_platform_flag *imx_platform_flag = of_id->data;
 
 	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
-	if (!data) {
-		dev_err(&pdev->dev, "Failed to allocate ci_hdrc-imx data!\n");
+	if (!data)
 		return -ENOMEM;
-	}
 
 	data->usbmisc_data = usbmisc_get_init_data(&pdev->dev);
 	if (IS_ERR(data->usbmisc_data))
@@ -147,7 +144,7 @@ static int ci_hdrc_imx_probe(struct platform_device *pdev)
 		goto err_clk;
 	}
 
-	pdata.phy = data->phy;
+	pdata.usb_phy = data->phy;
 
 	if (imx_platform_flag->flags & CI_HDRC_IMX_IMX28_WRITE_FIX)
 		pdata.flags |= CI_HDRC_IMX28_WRITE_FIX;
@@ -210,13 +207,48 @@ static int ci_hdrc_imx_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int imx_controller_suspend(struct device *dev)
+{
+	struct ci_hdrc_imx_data *data = dev_get_drvdata(dev);
+
+	dev_dbg(dev, "at %s\n", __func__);
+
+	clk_disable_unprepare(data->clk);
+
+	return 0;
+}
+
+static int imx_controller_resume(struct device *dev)
+{
+	struct ci_hdrc_imx_data *data = dev_get_drvdata(dev);
+
+	dev_dbg(dev, "at %s\n", __func__);
+
+	return clk_prepare_enable(data->clk);
+}
+
+static int ci_hdrc_imx_suspend(struct device *dev)
+{
+	return imx_controller_suspend(dev);
+}
+
+static int ci_hdrc_imx_resume(struct device *dev)
+{
+	return imx_controller_resume(dev);
+}
+#endif /* CONFIG_PM_SLEEP */
+
+static const struct dev_pm_ops ci_hdrc_imx_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(ci_hdrc_imx_suspend, ci_hdrc_imx_resume)
+};
 static struct platform_driver ci_hdrc_imx_driver = {
 	.probe = ci_hdrc_imx_probe,
 	.remove = ci_hdrc_imx_remove,
 	.driver = {
 		.name = "imx_usb",
-		.owner = THIS_MODULE,
 		.of_match_table = ci_hdrc_imx_dt_ids,
+		.pm = &ci_hdrc_imx_pm_ops,
 	 },
 };
 
diff --git a/drivers/usb/chipidea/ci_hdrc_msm.c b/drivers/usb/chipidea/ci_hdrc_msm.c
index 4935ac38fd00..d79ecc08a1be 100644
--- a/drivers/usb/chipidea/ci_hdrc_msm.c
+++ b/drivers/usb/chipidea/ci_hdrc_msm.c
@@ -26,15 +26,15 @@ static void ci_hdrc_msm_notify_event(struct ci_hdrc *ci, unsigned event)
 		dev_dbg(dev, "CI_HDRC_CONTROLLER_RESET_EVENT received\n");
 		writel(0, USB_AHBBURST);
 		writel(0, USB_AHBMODE);
-		usb_phy_init(ci->transceiver);
+		usb_phy_init(ci->usb_phy);
 		break;
 	case CI_HDRC_CONTROLLER_STOPPED_EVENT:
 		dev_dbg(dev, "CI_HDRC_CONTROLLER_STOPPED_EVENT received\n");
 		/*
-		 * Put the transceiver in non-driving mode. Otherwise host
+		 * Put the phy in non-driving mode. Otherwise host
 		 * may not detect soft-disconnection.
 		 */
-		usb_phy_notify_disconnect(ci->transceiver, USB_SPEED_UNKNOWN);
+		usb_phy_notify_disconnect(ci->usb_phy, USB_SPEED_UNKNOWN);
 		break;
 	default:
 		dev_dbg(dev, "unknown ci_hdrc event\n");
@@ -46,7 +46,6 @@ static struct ci_hdrc_platform_data ci_hdrc_msm_platdata = {
 	.name			= "ci_hdrc_msm",
 	.capoffset		= DEF_CAPOFFSET,
 	.flags			= CI_HDRC_REGS_SHARED |
-				  CI_HDRC_REQUIRE_TRANSCEIVER |
 				  CI_HDRC_DISABLE_STREAMING,
 
 	.notify_event		= ci_hdrc_msm_notify_event,
@@ -68,7 +67,7 @@ static int ci_hdrc_msm_probe(struct platform_device *pdev)
 	if (IS_ERR(phy))
 		return PTR_ERR(phy);
 
-	ci_hdrc_msm_platdata.phy = phy;
+	ci_hdrc_msm_platdata.usb_phy = phy;
 
 	plat_ci = ci_hdrc_add_device(&pdev->dev,
 				pdev->resource, pdev->num_resources,
diff --git a/drivers/usb/chipidea/ci_hdrc_usb2.c b/drivers/usb/chipidea/ci_hdrc_usb2.c
new file mode 100644
index 000000000000..45844c951788
--- /dev/null
+++ b/drivers/usb/chipidea/ci_hdrc_usb2.c
@@ -0,0 +1,115 @@
+/*
+ * Copyright (C) 2014 Marvell Technology Group Ltd.
+ *
+ * Antoine Tenart <antoine.tenart@free-electrons.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/dma-mapping.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/phy/phy.h>
+#include <linux/platform_device.h>
+#include <linux/usb/chipidea.h>
+#include <linux/usb/hcd.h>
+#include <linux/usb/ulpi.h>
+
+#include "ci.h"
+
+struct ci_hdrc_usb2_priv {
+	struct platform_device	*ci_pdev;
+	struct clk		*clk;
+};
+
+static struct ci_hdrc_platform_data ci_default_pdata = {
+	.capoffset	= DEF_CAPOFFSET,
+	.flags		= CI_HDRC_DISABLE_STREAMING,
+};
+
+static int ci_hdrc_usb2_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct ci_hdrc_usb2_priv *priv;
+	struct ci_hdrc_platform_data *ci_pdata = dev_get_platdata(dev);
+	int ret;
+
+	if (!ci_pdata)
+		ci_pdata = &ci_default_pdata;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->clk = devm_clk_get(dev, NULL);
+	if (!IS_ERR(priv->clk)) {
+		ret = clk_prepare_enable(priv->clk);
+		if (ret) {
+			dev_err(dev, "failed to enable the clock: %d\n", ret);
+			return ret;
+		}
+	}
+
+	ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(32));
+	if (ret)
+		goto clk_err;
+
+	ci_pdata->name = dev_name(dev);
+
+	priv->ci_pdev = ci_hdrc_add_device(dev, pdev->resource,
+					   pdev->num_resources, ci_pdata);
+	if (IS_ERR(priv->ci_pdev)) {
+		ret = PTR_ERR(priv->ci_pdev);
+		if (ret != -EPROBE_DEFER)
+			dev_err(dev,
+				"failed to register ci_hdrc platform device: %d\n",
+				ret);
+		goto clk_err;
+	}
+
+	platform_set_drvdata(pdev, priv);
+
+	pm_runtime_no_callbacks(dev);
+	pm_runtime_enable(dev);
+
+	return 0;
+
+clk_err:
+	if (!IS_ERR(priv->clk))
+		clk_disable_unprepare(priv->clk);
+	return ret;
+}
+
+static int ci_hdrc_usb2_remove(struct platform_device *pdev)
+{
+	struct ci_hdrc_usb2_priv *priv = platform_get_drvdata(pdev);
+
+	pm_runtime_disable(&pdev->dev);
+	ci_hdrc_remove_device(priv->ci_pdev);
+	clk_disable_unprepare(priv->clk);
+
+	return 0;
+}
+
+static const struct of_device_id ci_hdrc_usb2_of_match[] = {
+	{ .compatible = "chipidea,usb2" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, ci_hdrc_usb2_of_match);
+
+static struct platform_driver ci_hdrc_usb2_driver = {
+	.probe	= ci_hdrc_usb2_probe,
+	.remove	= ci_hdrc_usb2_remove,
+	.driver	= {
+		.name		= "chipidea-usb2",
+		.of_match_table	= of_match_ptr(ci_hdrc_usb2_of_match),
+	},
+};
+module_platform_driver(ci_hdrc_usb2_driver);
+
+MODULE_DESCRIPTION("ChipIdea HDRC USB2 binding for ci13xxx");
+MODULE_AUTHOR("Antoine Tenart <antoine.tenart@free-electrons.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/chipidea/ci_hdrc_zevio.c b/drivers/usb/chipidea/ci_hdrc_zevio.c
index 3bf6489ef5ec..d976fc1db73a 100644
--- a/drivers/usb/chipidea/ci_hdrc_zevio.c
+++ b/drivers/usb/chipidea/ci_hdrc_zevio.c
@@ -61,7 +61,6 @@ static struct platform_driver ci_hdrc_zevio_driver = {
 	.remove = ci_hdrc_zevio_remove,
 	.driver = {
 		.name = "zevio_usb",
-		.owner = THIS_MODULE,
 		.of_match_table = ci_hdrc_zevio_dt_ids,
 	},
 };
diff --git a/drivers/usb/chipidea/core.c b/drivers/usb/chipidea/core.c
index 9bdc6bd73432..5b9825a4538a 100644
--- a/drivers/usb/chipidea/core.c
+++ b/drivers/usb/chipidea/core.c
@@ -47,6 +47,7 @@
 #include <linux/delay.h>
 #include <linux/device.h>
 #include <linux/dma-mapping.h>
+#include <linux/phy/phy.h>
 #include <linux/platform_device.h>
 #include <linux/module.h>
 #include <linux/idr.h>
@@ -189,24 +190,29 @@ u8 hw_port_test_get(struct ci_hdrc *ci)
 	return hw_read(ci, OP_PORTSC, PORTSC_PTC) >> __ffs(PORTSC_PTC);
 }
 
+static void hw_wait_phy_stable(void)
+{
+	/*
+	 * The phy needs some delay to output the stable status from low
+	 * power mode. And for OTGSC, the status inputs are debounced
+	 * using a 1 ms time constant, so, delay 2ms for controller to get
+	 * the stable status, like vbus and id when the phy leaves low power.
+	 */
+	usleep_range(2000, 2500);
+}
+
 /* The PHY enters/leaves low power mode */
 static void ci_hdrc_enter_lpm(struct ci_hdrc *ci, bool enable)
 {
 	enum ci_hw_regs reg = ci->hw_bank.lpm ? OP_DEVLC : OP_PORTSC;
 	bool lpm = !!(hw_read(ci, reg, PORTSC_PHCD(ci->hw_bank.lpm)));
 
-	if (enable && !lpm) {
+	if (enable && !lpm)
 		hw_write(ci, reg, PORTSC_PHCD(ci->hw_bank.lpm),
 				PORTSC_PHCD(ci->hw_bank.lpm));
-	} else  if (!enable && lpm) {
+	else if (!enable && lpm)
 		hw_write(ci, reg, PORTSC_PHCD(ci->hw_bank.lpm),
 				0);
-		/* 
-		 * the PHY needs some time (less
-		 * than 1ms) to leave low power mode.
-		 */
-		usleep_range(1000, 1100);
-	}
 }
 
 static int hw_device_init(struct ci_hdrc *ci, void __iomem *base)
@@ -299,6 +305,49 @@ static void hw_phymode_configure(struct ci_hdrc *ci)
 }
 
 /**
+ * _ci_usb_phy_init: initialize phy taking in account both phy and usb_phy
+ * interfaces
+ * @ci: the controller
+ *
+ * This function returns an error code if the phy failed to init
+ */
+static int _ci_usb_phy_init(struct ci_hdrc *ci)
+{
+	int ret;
+
+	if (ci->phy) {
+		ret = phy_init(ci->phy);
+		if (ret)
+			return ret;
+
+		ret = phy_power_on(ci->phy);
+		if (ret) {
+			phy_exit(ci->phy);
+			return ret;
+		}
+	} else {
+		ret = usb_phy_init(ci->usb_phy);
+	}
+
+	return ret;
+}
+
+/**
+ * _ci_usb_phy_exit: deinitialize phy taking in account both phy and usb_phy
+ * interfaces
+ * @ci: the controller
+ */
+static void ci_usb_phy_exit(struct ci_hdrc *ci)
+{
+	if (ci->phy) {
+		phy_power_off(ci->phy);
+		phy_exit(ci->phy);
+	} else {
+		usb_phy_shutdown(ci->usb_phy);
+	}
+}
+
+/**
  * ci_usb_phy_init: initialize phy according to different phy type
  * @ci: the controller
  *
@@ -312,40 +361,68 @@ static int ci_usb_phy_init(struct ci_hdrc *ci)
 	case USBPHY_INTERFACE_MODE_UTMI:
 	case USBPHY_INTERFACE_MODE_UTMIW:
 	case USBPHY_INTERFACE_MODE_HSIC:
-		ret = usb_phy_init(ci->transceiver);
-		if (ret)
+		ret = _ci_usb_phy_init(ci);
+		if (!ret)
+			hw_wait_phy_stable();
+		else
 			return ret;
 		hw_phymode_configure(ci);
 		break;
 	case USBPHY_INTERFACE_MODE_ULPI:
 	case USBPHY_INTERFACE_MODE_SERIAL:
 		hw_phymode_configure(ci);
-		ret = usb_phy_init(ci->transceiver);
+		ret = _ci_usb_phy_init(ci);
 		if (ret)
 			return ret;
 		break;
 	default:
-		ret = usb_phy_init(ci->transceiver);
+		ret = _ci_usb_phy_init(ci);
+		if (!ret)
+			hw_wait_phy_stable();
 	}
 
 	return ret;
 }
 
 /**
- * hw_device_reset: resets chip (execute without interruption)
+ * hw_controller_reset: do controller reset
  * @ci: the controller
   *
  * This function returns an error code
  */
-int hw_device_reset(struct ci_hdrc *ci, u32 mode)
+static int hw_controller_reset(struct ci_hdrc *ci)
 {
+	int count = 0;
+
+	hw_write(ci, OP_USBCMD, USBCMD_RST, USBCMD_RST);
+	while (hw_read(ci, OP_USBCMD, USBCMD_RST)) {
+		udelay(10);
+		if (count++ > 1000)
+			return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+/**
+ * hw_device_reset: resets chip (execute without interruption)
+ * @ci: the controller
+ *
+ * This function returns an error code
+ */
+int hw_device_reset(struct ci_hdrc *ci)
+{
+	int ret;
+
 	/* 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 */
+	ret = hw_controller_reset(ci);
+	if (ret) {
+		dev_err(ci->dev, "error resetting controller, ret=%d\n", ret);
+		return ret;
+	}
 
 	if (ci->platdata->notify_event)
 		ci->platdata->notify_event(ci,
@@ -363,12 +440,12 @@ int hw_device_reset(struct ci_hdrc *ci, u32 mode)
 
 	/* 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_write(ci, OP_USBMODE, USBMODE_CM, USBMODE_CM_DC);
 	/* 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);
+	if (hw_read(ci, OP_USBMODE, USBMODE_CM) != USBMODE_CM_DC) {
+		pr_err("cannot enter in %s device mode", ci_role(ci)->name);
 		pr_err("lpm = %i", ci->hw_bank.lpm);
 		return -ENODEV;
 	}
@@ -472,7 +549,7 @@ static int ci_get_platdata(struct device *dev,
 		if (PTR_ERR(platdata->reg_vbus) == -EPROBE_DEFER) {
 			return -EPROBE_DEFER;
 		} else if (PTR_ERR(platdata->reg_vbus) == -ENODEV) {
-			/* no vbus regualator is needed */
+			/* no vbus regulator is needed */
 			platdata->reg_vbus = NULL;
 		} else if (IS_ERR(platdata->reg_vbus)) {
 			dev_err(dev, "Getting regulator error: %ld\n",
@@ -589,11 +666,10 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 		return PTR_ERR(base);
 
 	ci = devm_kzalloc(dev, sizeof(*ci), GFP_KERNEL);
-	if (!ci) {
-		dev_err(dev, "can't allocate device\n");
+	if (!ci)
 		return -ENOMEM;
-	}
 
+	platform_set_drvdata(pdev, ci);
 	ci->dev = dev;
 	ci->platdata = dev_get_platdata(dev);
 	ci->imx28_write_fix = !!(ci->platdata->flags &
@@ -605,36 +681,32 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	if (ci->platdata->phy)
-		ci->transceiver = ci->platdata->phy;
-	else
-		ci->transceiver = devm_usb_get_phy(dev, USB_PHY_TYPE_USB2);
-
-	if (IS_ERR(ci->transceiver)) {
-		ret = PTR_ERR(ci->transceiver);
-		/*
-		 * if -ENXIO is returned, it means PHY layer wasn't
-		 * enabled, so it makes no sense to return -EPROBE_DEFER
-		 * in that case, since no PHY driver will ever probe.
-		 */
-		if (ret == -ENXIO)
-			return ret;
+	if (ci->platdata->phy) {
+		ci->phy = ci->platdata->phy;
+	} else if (ci->platdata->usb_phy) {
+		ci->usb_phy = ci->platdata->usb_phy;
+	} else {
+		ci->phy = devm_phy_get(dev->parent, "usb-phy");
+		ci->usb_phy = devm_usb_get_phy(dev->parent, USB_PHY_TYPE_USB2);
+
+		/* if both generic PHY and USB PHY layers aren't enabled */
+		if (PTR_ERR(ci->phy) == -ENOSYS &&
+				PTR_ERR(ci->usb_phy) == -ENXIO)
+			return -ENXIO;
+
+		if (IS_ERR(ci->phy) && IS_ERR(ci->usb_phy))
+			return -EPROBE_DEFER;
 
-		dev_err(dev, "no usb2 phy configured\n");
-		return -EPROBE_DEFER;
+		if (IS_ERR(ci->phy))
+			ci->phy = NULL;
+		else if (IS_ERR(ci->usb_phy))
+			ci->usb_phy = NULL;
 	}
 
 	ret = ci_usb_phy_init(ci);
 	if (ret) {
 		dev_err(dev, "unable to init phy: %d\n", ret);
 		return ret;
-	} else {
-		/* 
-		 * The delay to sync PHY's status, the maximum delay is
-		 * 2ms since the otgsc uses 1ms timer to debounce the
-		 * PHY's input
-		 */
-		usleep_range(2000, 2500);
 	}
 
 	ci->hw_bank.phys = res->start;
@@ -711,9 +783,8 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 		}
 	}
 
-	platform_set_drvdata(pdev, ci);
-	ret = request_irq(ci->irq, ci_irq, IRQF_SHARED, ci->platdata->name,
-			  ci);
+	ret = devm_request_irq(dev, ci->irq, ci_irq, IRQF_SHARED,
+			ci->platdata->name, ci);
 	if (ret)
 		goto stop;
 
@@ -724,11 +795,10 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 	if (!ret)
 		return 0;
 
-	free_irq(ci->irq, ci);
 stop:
 	ci_role_destroy(ci);
 deinit_phy:
-	usb_phy_shutdown(ci->transceiver);
+	ci_usb_phy_exit(ci);
 
 	return ret;
 }
@@ -738,20 +808,66 @@ static int ci_hdrc_remove(struct platform_device *pdev)
 	struct ci_hdrc *ci = platform_get_drvdata(pdev);
 
 	dbg_remove_files(ci);
-	free_irq(ci->irq, ci);
 	ci_role_destroy(ci);
 	ci_hdrc_enter_lpm(ci, true);
-	usb_phy_shutdown(ci->transceiver);
+	ci_usb_phy_exit(ci);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static void ci_controller_suspend(struct ci_hdrc *ci)
+{
+	ci_hdrc_enter_lpm(ci, true);
+
+	if (ci->usb_phy)
+		usb_phy_set_suspend(ci->usb_phy, 1);
+}
+
+static int ci_controller_resume(struct device *dev)
+{
+	struct ci_hdrc *ci = dev_get_drvdata(dev);
+
+	dev_dbg(dev, "at %s\n", __func__);
+
+	ci_hdrc_enter_lpm(ci, false);
+
+	if (ci->usb_phy) {
+		usb_phy_set_suspend(ci->usb_phy, 0);
+		usb_phy_set_wakeup(ci->usb_phy, false);
+		hw_wait_phy_stable();
+	}
 
 	return 0;
 }
 
+static int ci_suspend(struct device *dev)
+{
+	struct ci_hdrc *ci = dev_get_drvdata(dev);
+
+	if (ci->wq)
+		flush_workqueue(ci->wq);
+
+	ci_controller_suspend(ci);
+
+	return 0;
+}
+
+static int ci_resume(struct device *dev)
+{
+	return ci_controller_resume(dev);
+}
+#endif /* CONFIG_PM_SLEEP */
+
+static const struct dev_pm_ops ci_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(ci_suspend, ci_resume)
+};
 static struct platform_driver ci_hdrc_driver = {
 	.probe	= ci_hdrc_probe,
 	.remove	= ci_hdrc_remove,
 	.driver	= {
 		.name	= "ci_hdrc",
-		.owner	= THIS_MODULE,
+		.pm	= &ci_pm_ops,
 	},
 };
 
diff --git a/drivers/usb/chipidea/debug.c b/drivers/usb/chipidea/debug.c
index 795d6538d630..268e4236e84c 100644
--- a/drivers/usb/chipidea/debug.c
+++ b/drivers/usb/chipidea/debug.c
@@ -220,7 +220,7 @@ static int ci_otg_show(struct seq_file *s, void *unused)
 
 	/* ------ State ----- */
 	seq_printf(s, "OTG state: %s\n\n",
-		usb_otg_state_string(ci->transceiver->state));
+			usb_otg_state_string(ci->otg.state));
 
 	/* ------ State Machine Variables ----- */
 	seq_printf(s, "a_bus_drop: %d\n", fsm->a_bus_drop);
diff --git a/drivers/usb/chipidea/host.c b/drivers/usb/chipidea/host.c
index ebde7b6ce687..c1694cff1eaf 100644
--- a/drivers/usb/chipidea/host.c
+++ b/drivers/usb/chipidea/host.c
@@ -34,6 +34,44 @@
 
 static struct hc_driver __read_mostly ci_ehci_hc_driver;
 
+struct ehci_ci_priv {
+	struct regulator *reg_vbus;
+};
+
+static int ehci_ci_portpower(struct usb_hcd *hcd, int portnum, bool enable)
+{
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	struct ehci_ci_priv *priv = (struct ehci_ci_priv *)ehci->priv;
+	struct device *dev = hcd->self.controller;
+	struct ci_hdrc *ci = dev_get_drvdata(dev);
+	int ret = 0;
+	int port = HCS_N_PORTS(ehci->hcs_params);
+
+	if (priv->reg_vbus && !ci_otg_is_fsm_mode(ci)) {
+		if (port > 1) {
+			dev_warn(dev,
+				"Not support multi-port regulator control\n");
+			return 0;
+		}
+		if (enable)
+			ret = regulator_enable(priv->reg_vbus);
+		else
+			ret = regulator_disable(priv->reg_vbus);
+		if (ret) {
+			dev_err(dev,
+				"Failed to %s vbus regulator, ret=%d\n",
+				enable ? "enable" : "disable", ret);
+			return ret;
+		}
+	}
+	return 0;
+};
+
+static const struct ehci_driver_overrides ehci_ci_overrides = {
+	.extra_priv_size = sizeof(struct ehci_ci_priv),
+	.port_power	 = ehci_ci_portpower,
+};
+
 static irqreturn_t host_irq(struct ci_hdrc *ci)
 {
 	return usb_hcd_irq(ci->irq, ci->hcd);
@@ -43,6 +81,7 @@ static int host_start(struct ci_hdrc *ci)
 {
 	struct usb_hcd *hcd;
 	struct ehci_hcd *ehci;
+	struct ehci_ci_priv *priv;
 	int ret;
 
 	if (usb_disabled())
@@ -52,15 +91,17 @@ static int host_start(struct ci_hdrc *ci)
 	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->platdata->power_budget;
-	hcd->usb_phy = ci->transceiver;
 	hcd->tpl_support = ci->platdata->tpl_support;
+	if (ci->phy)
+		hcd->phy = ci->phy;
+	else
+		hcd->usb_phy = ci->usb_phy;
 
 	ehci = hcd_to_ehci(hcd);
 	ehci->caps = ci->hw_bank.cap;
@@ -68,28 +109,21 @@ static int host_start(struct ci_hdrc *ci)
 	ehci->has_tdi_phy_lpm = ci->hw_bank.lpm;
 	ehci->imx28_write_fix = ci->imx28_write_fix;
 
-	/*
-	 * vbus is always on if host is not in OTG FSM mode,
-	 * otherwise should be controlled by OTG FSM
-	 */
-	if (ci->platdata->reg_vbus && !ci_otg_is_fsm_mode(ci)) {
-		ret = regulator_enable(ci->platdata->reg_vbus);
-		if (ret) {
-			dev_err(ci->dev,
-				"Failed to enable vbus regulator, ret=%d\n",
-				ret);
-			goto put_hcd;
-		}
-	}
+	priv = (struct ehci_ci_priv *)ehci->priv;
+	priv->reg_vbus = NULL;
+
+	if (ci->platdata->reg_vbus)
+		priv->reg_vbus = ci->platdata->reg_vbus;
 
 	ret = usb_add_hcd(hcd, 0, 0);
 	if (ret) {
-		goto disable_reg;
+		goto put_hcd;
 	} else {
-		struct usb_otg *otg = ci->transceiver->otg;
+		struct usb_otg *otg = &ci->otg;
 
 		ci->hcd = hcd;
-		if (otg) {
+
+		if (ci_otg_is_fsm_mode(ci)) {
 			otg->host = &hcd->self;
 			hcd->self.otg_port = 1;
 		}
@@ -100,10 +134,6 @@ static int host_start(struct ci_hdrc *ci)
 
 	return ret;
 
-disable_reg:
-	if (ci->platdata->reg_vbus && !ci_otg_is_fsm_mode(ci))
-		regulator_disable(ci->platdata->reg_vbus);
-
 put_hcd:
 	usb_put_hcd(hcd);
 
@@ -117,8 +147,6 @@ static void host_stop(struct ci_hdrc *ci)
 	if (hcd) {
 		usb_remove_hcd(hcd);
 		usb_put_hcd(hcd);
-		if (ci->platdata->reg_vbus && !ci_otg_is_fsm_mode(ci))
-			regulator_disable(ci->platdata->reg_vbus);
 	}
 }
 
@@ -146,7 +174,7 @@ int ci_hdrc_host_init(struct ci_hdrc *ci)
 	rdrv->name	= "host";
 	ci->roles[CI_ROLE_HOST] = rdrv;
 
-	ehci_init_driver(&ci_ehci_hc_driver, NULL);
+	ehci_init_driver(&ci_ehci_hc_driver, &ehci_ci_overrides);
 
 	return 0;
 }
diff --git a/drivers/usb/chipidea/otg_fsm.c b/drivers/usb/chipidea/otg_fsm.c
index caaabc58021e..562e581f6765 100644
--- a/drivers/usb/chipidea/otg_fsm.c
+++ b/drivers/usb/chipidea/otg_fsm.c
@@ -303,7 +303,7 @@ static void a_wait_vfall_tmout_func(void *ptr, unsigned long indicator)
 	set_tmout(ci, indicator);
 	/* Disable port power */
 	hw_write(ci, OP_PORTSC, PORTSC_W1C_BITS | PORTSC_PP, 0);
-	/* Clear exsiting DP irq */
+	/* Clear existing DP irq */
 	hw_write_otgsc(ci, OTGSC_DPIS, OTGSC_DPIS);
 	/* Enable data pulse irq */
 	hw_write_otgsc(ci, OTGSC_DPIE, OTGSC_DPIE);
@@ -328,7 +328,7 @@ static void b_ssend_srp_tmout_func(void *ptr, unsigned long indicator)
 	set_tmout(ci, indicator);
 
 	/* only vbus fall below B_sess_vld in b_idle state */
-	if (ci->transceiver->state == OTG_STATE_B_IDLE)
+	if (ci->fsm.otg->state == OTG_STATE_B_IDLE)
 		ci_otg_queue_work(ci);
 }
 
@@ -543,7 +543,7 @@ static int ci_otg_start_host(struct otg_fsm *fsm, int on)
 		ci_role_start(ci, CI_ROLE_HOST);
 	} else {
 		ci_role_stop(ci);
-		hw_device_reset(ci, USBMODE_CM_DC);
+		hw_device_reset(ci);
 		ci_role_start(ci, CI_ROLE_GADGET);
 	}
 	mutex_lock(&fsm->lock);
@@ -582,11 +582,11 @@ int ci_otg_fsm_work(struct ci_hdrc *ci)
 	 * when there is no gadget class driver
 	 */
 	if (ci->fsm.id && !(ci->driver) &&
-		ci->transceiver->state < OTG_STATE_A_IDLE)
+		ci->fsm.otg->state < OTG_STATE_A_IDLE)
 		return 0;
 
 	if (otg_statemachine(&ci->fsm)) {
-		if (ci->transceiver->state == OTG_STATE_A_IDLE) {
+		if (ci->fsm.otg->state == OTG_STATE_A_IDLE) {
 			/*
 			 * Further state change for cases:
 			 * a_idle to b_idle; or
@@ -600,7 +600,7 @@ int ci_otg_fsm_work(struct ci_hdrc *ci)
 				ci_otg_queue_work(ci);
 			if (ci->id_event)
 				ci->id_event = false;
-		} else if (ci->transceiver->state == OTG_STATE_B_IDLE) {
+		} else if (ci->fsm.otg->state == OTG_STATE_B_IDLE) {
 			if (ci->fsm.b_sess_vld) {
 				ci->fsm.power_up = 0;
 				/*
@@ -627,7 +627,7 @@ static void ci_otg_fsm_event(struct ci_hdrc *ci)
 	otg_bsess_vld = hw_read_otgsc(ci, OTGSC_BSV);
 	port_conn = hw_read(ci, OP_PORTSC, PORTSC_CCS);
 
-	switch (ci->transceiver->state) {
+	switch (ci->fsm.otg->state) {
 	case OTG_STATE_A_WAIT_BCON:
 		if (port_conn) {
 			fsm->b_conn = 1;
@@ -663,7 +663,7 @@ static void ci_otg_fsm_event(struct ci_hdrc *ci)
 			 fsm->b_bus_suspend = 1;
 			/*
 			 * Init a timer to know how long this suspend
-			 * will contine, if time out, indicates B no longer
+			 * will continue, if time out, indicates B no longer
 			 * wants to be host role
 			 */
 			 ci_otg_add_timer(ci, A_BIDL_ADIS);
@@ -778,34 +778,25 @@ void ci_hdrc_otg_fsm_start(struct ci_hdrc *ci)
 int ci_hdrc_otg_fsm_init(struct ci_hdrc *ci)
 {
 	int retval = 0;
-	struct usb_otg *otg;
 
-	otg = devm_kzalloc(ci->dev,
-			sizeof(struct usb_otg), GFP_KERNEL);
-	if (!otg) {
-		dev_err(ci->dev,
-		"Failed to allocate usb_otg structure for ci hdrc otg!\n");
-		return -ENOMEM;
-	}
+	if (ci->phy)
+		ci->otg.phy = ci->phy;
+	else
+		ci->otg.usb_phy = ci->usb_phy;
 
-	otg->phy = ci->transceiver;
-	otg->gadget = &ci->gadget;
-	ci->fsm.otg = otg;
-	ci->transceiver->otg = ci->fsm.otg;
+	ci->otg.gadget = &ci->gadget;
+	ci->fsm.otg = &ci->otg;
 	ci->fsm.power_up = 1;
 	ci->fsm.id = hw_read_otgsc(ci, OTGSC_ID) ? 1 : 0;
-	ci->transceiver->state = OTG_STATE_UNDEFINED;
+	ci->fsm.otg->state = OTG_STATE_UNDEFINED;
 	ci->fsm.ops = &ci_otg_ops;
 
 	mutex_init(&ci->fsm.lock);
 
 	ci->fsm_timer = devm_kzalloc(ci->dev,
 			sizeof(struct ci_otg_fsm_timer_list), GFP_KERNEL);
-	if (!ci->fsm_timer) {
-		dev_err(ci->dev,
-		"Failed to allocate timer structure for ci hdrc otg!\n");
+	if (!ci->fsm_timer)
 		return -ENOMEM;
-	}
 
 	INIT_LIST_HEAD(&ci->fsm_timer->active_timers);
 	retval = ci_otg_init_timers(ci);
diff --git a/drivers/usb/chipidea/udc.c b/drivers/usb/chipidea/udc.c
index 0444d3f8971a..4fe18ce3bd5a 100644
--- a/drivers/usb/chipidea/udc.c
+++ b/drivers/usb/chipidea/udc.c
@@ -692,10 +692,8 @@ __acquires(ci->lock)
 	int retval;
 
 	spin_unlock(&ci->lock);
-	if (ci->gadget.speed != USB_SPEED_UNKNOWN) {
-		if (ci->driver)
-			ci->driver->disconnect(&ci->gadget);
-	}
+	if (ci->gadget.speed != USB_SPEED_UNKNOWN)
+		usb_gadget_udc_reset(&ci->gadget, ci->driver);
 
 	retval = _gadget_stop_activity(&ci->gadget);
 	if (retval)
@@ -709,8 +707,6 @@ __acquires(ci->lock)
 	if (ci->status == NULL)
 		retval = -ENOMEM;
 
-	usb_gadget_set_state(&ci->gadget, USB_STATE_DEFAULT);
-
 done:
 	spin_lock(&ci->lock);
 
@@ -1475,7 +1471,7 @@ static int ci_udc_vbus_session(struct usb_gadget *_gadget, int is_active)
 	if (gadget_ready) {
 		if (is_active) {
 			pm_runtime_get_sync(&_gadget->dev);
-			hw_device_reset(ci, USBMODE_CM_DC);
+			hw_device_reset(ci);
 			hw_device_state(ci, ci->ep0out->qh.dma);
 			usb_gadget_set_state(_gadget, USB_STATE_POWERED);
 		} else {
@@ -1519,8 +1515,8 @@ static int ci_udc_vbus_draw(struct usb_gadget *_gadget, unsigned ma)
 {
 	struct ci_hdrc *ci = container_of(_gadget, struct ci_hdrc, gadget);
 
-	if (ci->transceiver)
-		return usb_phy_set_power(ci->transceiver, ma);
+	if (ci->usb_phy)
+		return usb_phy_set_power(ci->usb_phy, ma);
 	return -ENOTSUPP;
 }
 
@@ -1544,8 +1540,7 @@ static int ci_udc_pullup(struct usb_gadget *_gadget, int is_on)
 
 static int ci_udc_start(struct usb_gadget *gadget,
 			 struct usb_gadget_driver *driver);
-static int ci_udc_stop(struct usb_gadget *gadget,
-			struct usb_gadget_driver *driver);
+static int ci_udc_stop(struct usb_gadget *gadget);
 /**
  * Device operations part of the API to the USB controller hardware,
  * which don't involve endpoints (or i/o)
@@ -1665,7 +1660,7 @@ static int ci_udc_start(struct usb_gadget *gadget,
 	pm_runtime_get_sync(&ci->gadget.dev);
 	if (ci->vbus_active) {
 		spin_lock_irqsave(&ci->lock, flags);
-		hw_device_reset(ci, USBMODE_CM_DC);
+		hw_device_reset(ci);
 	} else {
 		pm_runtime_put_sync(&ci->gadget.dev);
 		return retval;
@@ -1682,8 +1677,7 @@ static int ci_udc_start(struct usb_gadget *gadget,
 /**
  * ci_udc_stop: unregister a gadget driver
  */
-static int ci_udc_stop(struct usb_gadget *gadget,
-			struct usb_gadget_driver *driver)
+static int ci_udc_stop(struct usb_gadget *gadget)
 {
 	struct ci_hdrc *ci = container_of(gadget, struct ci_hdrc, gadget);
 	unsigned long flags;
diff --git a/drivers/usb/chipidea/usbmisc_imx.c b/drivers/usb/chipidea/usbmisc_imx.c
index 926c997ef310..c3c6225b8acf 100644
--- a/drivers/usb/chipidea/usbmisc_imx.c
+++ b/drivers/usb/chipidea/usbmisc_imx.c
@@ -117,10 +117,9 @@ static int usbmisc_imx25_post(struct imx_usbmisc_data *data)
 	if (data->index > 2)
 		return -EINVAL;
 
-	reg = usbmisc->base + MX25_USB_PHY_CTRL_OFFSET;
-
 	if (data->evdo) {
 		spin_lock_irqsave(&usbmisc->lock, flags);
+		reg = usbmisc->base + MX25_USB_PHY_CTRL_OFFSET;
 		val = readl(reg);
 		writel(val | MX25_BM_EXTERNAL_VBUS_DIVIDER, reg);
 		spin_unlock_irqrestore(&usbmisc->lock, flags);
@@ -172,8 +171,7 @@ static int usbmisc_imx53_init(struct imx_usbmisc_data *data)
 		return -EINVAL;
 
 	/* Select a 24 MHz reference clock for the PHY  */
-	reg = usbmisc->base + MX53_USB_OTG_PHY_CTRL_1_OFFSET;
-	val = readl(reg);
+	val = readl(usbmisc->base + MX53_USB_OTG_PHY_CTRL_1_OFFSET);
 	val &= ~MX53_USB_PHYCTRL1_PLLDIV_MASK;
 	val |= MX53_USB_PLL_DIV_24_MHZ;
 	writel(val, usbmisc->base + MX53_USB_OTG_PHY_CTRL_1_OFFSET);
@@ -372,7 +370,6 @@ static struct platform_driver usbmisc_imx_driver = {
 	.remove = usbmisc_imx_remove,
 	.driver = {
 		.name = "usbmisc_imx",
-		.owner = THIS_MODULE,
 		.of_match_table = usbmisc_imx_dt_ids,
 	 },
 };
diff --git a/drivers/usb/class/cdc-acm.c b/drivers/usb/class/cdc-acm.c
index 077d58ac3dcb..546a17e8ad5b 100644
--- a/drivers/usb/class/cdc-acm.c
+++ b/drivers/usb/class/cdc-acm.c
@@ -1157,8 +1157,6 @@ static int acm_probe(struct usb_interface *intf,
 		case USB_CDC_CALL_MANAGEMENT_TYPE:
 			call_management_function = buffer[3];
 			call_interface_num = buffer[4];
-			if ((quirks & NOT_A_MODEM) == 0 && (call_management_function & 3) != 3)
-				dev_err(&intf->dev, "This device cannot do calls on its own. It is not a modem.\n");
 			break;
 		default:
 			/* there are LOTS more CDC descriptors that
@@ -1197,10 +1195,11 @@ next_desc:
 	} else {
 		control_interface = usb_ifnum_to_if(usb_dev, union_header->bMasterInterface0);
 		data_interface = usb_ifnum_to_if(usb_dev, (data_interface_num = union_header->bSlaveInterface0));
-		if (!control_interface || !data_interface) {
-			dev_dbg(&intf->dev, "no interfaces\n");
-			return -ENODEV;
-		}
+	}
+
+	if (!control_interface || !data_interface) {
+		dev_dbg(&intf->dev, "no interfaces\n");
+		return -ENODEV;
 	}
 
 	if (data_interface_num != call_interface_num)
@@ -1475,6 +1474,7 @@ alloc_fail8:
 				&dev_attr_wCountryCodes);
 		device_remove_file(&acm->control->dev,
 				&dev_attr_iCountryCodeRelDate);
+		kfree(acm->country_codes);
 	}
 	device_remove_file(&acm->control->dev, &dev_attr_bmCapabilities);
 alloc_fail7:
@@ -1813,11 +1813,6 @@ static const struct usb_device_id acm_ids[] = {
 
 	/* NOTE: non-Nokia COMM/ACM/0xff is likely MSFT RNDIS... NOT a modem! */
 
-	/* Support Lego NXT using pbLua firmware */
-	{ USB_DEVICE(0x0694, 0xff00),
-	.driver_info = NOT_A_MODEM,
-	},
-
 	/* Support for Droids MuIn LCD */
 	{ USB_DEVICE(0x04d8, 0x000b),
 	.driver_info = NO_DATA_INTERFACE,
diff --git a/drivers/usb/class/cdc-acm.h b/drivers/usb/class/cdc-acm.h
index d3251ebd09e2..ffeb3c83941f 100644
--- a/drivers/usb/class/cdc-acm.h
+++ b/drivers/usb/class/cdc-acm.h
@@ -130,7 +130,6 @@ struct acm {
 #define NO_UNION_NORMAL			BIT(0)
 #define SINGLE_RX_URB			BIT(1)
 #define NO_CAP_LINE			BIT(2)
-#define NOT_A_MODEM			BIT(3)
 #define NO_DATA_INTERFACE		BIT(4)
 #define IGNORE_DEVICE			BIT(5)
 #define QUIRK_CONTROL_LINE_STATE	BIT(6)
diff --git a/drivers/usb/class/usbtmc.c b/drivers/usb/class/usbtmc.c
index ec978408a2ee..960bc089111b 100644
--- a/drivers/usb/class/usbtmc.c
+++ b/drivers/usb/class/usbtmc.c
@@ -1104,10 +1104,8 @@ static int usbtmc_probe(struct usb_interface *intf,
 	dev_dbg(&intf->dev, "%s called\n", __func__);
 
 	data = devm_kzalloc(&intf->dev, sizeof(*data), GFP_KERNEL);
-	if (!data) {
-		dev_err(&intf->dev, "Unable to allocate kernel memory\n");
+	if (!data)
 		return -ENOMEM;
-	}
 
 	data->intf = intf;
 	data->id = id;
diff --git a/drivers/usb/common/usb-otg-fsm.c b/drivers/usb/common/usb-otg-fsm.c
index 98e8340a5bb1..c6b35b77dab7 100644
--- a/drivers/usb/common/usb-otg-fsm.c
+++ b/drivers/usb/common/usb-otg-fsm.c
@@ -124,10 +124,10 @@ static void otg_leave_state(struct otg_fsm *fsm, enum usb_otg_state old_state)
 static int otg_set_state(struct otg_fsm *fsm, enum usb_otg_state new_state)
 {
 	state_changed = 1;
-	if (fsm->otg->phy->state == new_state)
+	if (fsm->otg->state == new_state)
 		return 0;
 	VDBG("Set state: %s\n", usb_otg_state_string(new_state));
-	otg_leave_state(fsm, fsm->otg->phy->state);
+	otg_leave_state(fsm, fsm->otg->state);
 	switch (new_state) {
 	case OTG_STATE_B_IDLE:
 		otg_drv_vbus(fsm, 0);
@@ -236,7 +236,7 @@ static int otg_set_state(struct otg_fsm *fsm, enum usb_otg_state new_state)
 		break;
 	}
 
-	fsm->otg->phy->state = new_state;
+	fsm->otg->state = new_state;
 	return 0;
 }
 
@@ -247,7 +247,7 @@ int otg_statemachine(struct otg_fsm *fsm)
 
 	mutex_lock(&fsm->lock);
 
-	state = fsm->otg->phy->state;
+	state = fsm->otg->state;
 	state_changed = 0;
 	/* State machine state change judgement */
 
diff --git a/drivers/usb/core/Kconfig b/drivers/usb/core/Kconfig
index 9cfda6a72194..cc0ced08bae2 100644
--- a/drivers/usb/core/Kconfig
+++ b/drivers/usb/core/Kconfig
@@ -43,7 +43,7 @@ config USB_DYNAMIC_MINORS
 
 config USB_OTG
 	bool "OTG support"
-	depends on PM_RUNTIME
+	depends on PM
 	default n
 	help
 	  The most notable feature of USB OTG is support for a
diff --git a/drivers/usb/core/hcd.c b/drivers/usb/core/hcd.c
index 278be0515e8e..11cee55ae397 100644
--- a/drivers/usb/core/hcd.c
+++ b/drivers/usb/core/hcd.c
@@ -2646,7 +2646,7 @@ int usb_add_hcd(struct usb_hcd *hcd,
 		}
 	}
 
-	if (IS_ENABLED(CONFIG_GENERIC_PHY)) {
+	if (IS_ENABLED(CONFIG_GENERIC_PHY) && !hcd->phy) {
 		struct phy *phy = phy_get(hcd->self.controller, "usb");
 
 		if (IS_ERR(phy)) {
@@ -2666,6 +2666,7 @@ int usb_add_hcd(struct usb_hcd *hcd,
 				goto err_phy;
 			}
 			hcd->phy = phy;
+			hcd->remove_phy = 1;
 		}
 	}
 
@@ -2812,7 +2813,7 @@ err_allocate_root_hub:
 err_register_bus:
 	hcd_buffer_destroy(hcd);
 err_create_buf:
-	if (IS_ENABLED(CONFIG_GENERIC_PHY) && hcd->phy) {
+	if (IS_ENABLED(CONFIG_GENERIC_PHY) && hcd->remove_phy && hcd->phy) {
 		phy_power_off(hcd->phy);
 		phy_exit(hcd->phy);
 		phy_put(hcd->phy);
@@ -2896,7 +2897,7 @@ void usb_remove_hcd(struct usb_hcd *hcd)
 	usb_deregister_bus(&hcd->self);
 	hcd_buffer_destroy(hcd);
 
-	if (IS_ENABLED(CONFIG_GENERIC_PHY) && hcd->phy) {
+	if (IS_ENABLED(CONFIG_GENERIC_PHY) && hcd->remove_phy && hcd->phy) {
 		phy_power_off(hcd->phy);
 		phy_exit(hcd->phy);
 		phy_put(hcd->phy);
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index c9596525ba8c..aeb50bb6ba9c 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -2543,11 +2543,14 @@ int usb_authorize_device(struct usb_device *usb_dev)
 			"can't autoresume for authorization: %d\n", result);
 		goto error_autoresume;
 	}
-	result = usb_get_device_descriptor(usb_dev, sizeof(usb_dev->descriptor));
-	if (result < 0) {
-		dev_err(&usb_dev->dev, "can't re-read device descriptor for "
-			"authorization: %d\n", result);
-		goto error_device_descriptor;
+
+	if (usb_dev->wusb) {
+		result = usb_get_device_descriptor(usb_dev, sizeof(usb_dev->descriptor));
+		if (result < 0) {
+			dev_err(&usb_dev->dev, "can't re-read device descriptor for "
+				"authorization: %d\n", result);
+			goto error_device_descriptor;
+		}
 	}
 
 	usb_dev->authorized = 1;
@@ -3907,14 +3910,9 @@ static void usb_enable_link_state(struct usb_hcd *hcd, struct usb_device *udev,
 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",
diff --git a/drivers/usb/core/quirks.c b/drivers/usb/core/quirks.c
index 96fafed92b76..0ffb4ed0a945 100644
--- a/drivers/usb/core/quirks.c
+++ b/drivers/usb/core/quirks.c
@@ -103,6 +103,9 @@ static const struct usb_device_id usb_quirk_list[] = {
 	{ USB_DEVICE(0x04f3, 0x009b), .driver_info =
 			USB_QUIRK_DEVICE_QUALIFIER },
 
+	{ USB_DEVICE(0x04f3, 0x010c), .driver_info =
+			USB_QUIRK_DEVICE_QUALIFIER },
+
 	{ USB_DEVICE(0x04f3, 0x016f), .driver_info =
 			USB_QUIRK_DEVICE_QUALIFIER },
 
diff --git a/drivers/usb/dwc2/Kconfig b/drivers/usb/dwc2/Kconfig
index f93807b3631a..b323c4c11b0a 100644
--- a/drivers/usb/dwc2/Kconfig
+++ b/drivers/usb/dwc2/Kconfig
@@ -1,6 +1,6 @@
 config USB_DWC2
-	bool "DesignWare USB2 DRD Core Support"
-	depends on USB
+	tristate "DesignWare USB2 DRD Core Support"
+	depends on USB || USB_GADGET
 	help
 	  Say Y here if your system has a Dual Role Hi-Speed USB
 	  controller based on the DesignWare HSOTG IP Core.
@@ -10,49 +10,61 @@ config USB_DWC2
 	  bus interface module (if you have a PCI bus system) will be
 	  called dwc2_pci.ko, and the platform interface module (for
 	  controllers directly connected to the CPU) will be called
-	  dwc2_platform.ko. For gadget mode, there will be a single
-	  module called dwc2_gadget.ko.
-
-	  NOTE: The s3c-hsotg driver is now renamed to dwc2_gadget. The
-	  host and gadget drivers are still currently separate drivers.
-	  There are plans to merge the dwc2_gadget driver with the dwc2
-	  host driver in the near future to create a dual-role driver.
+	  dwc2_platform.ko. For all modes(host, gadget and dual-role), there
+	  will be an additional module named dwc2.ko.
 
 if USB_DWC2
 
+choice
+	bool "DWC2 Mode Selection"
+	default USB_DWC2_DUAL_ROLE if (USB && USB_GADGET)
+	default USB_DWC2_HOST if (USB && !USB_GADGET)
+	default USB_DWC2_PERIPHERAL if (!USB && USB_GADGET)
+
 config USB_DWC2_HOST
-	tristate "Host only mode"
+	bool "Host only mode"
 	depends on USB
 	help
 	  The Designware USB2.0 high-speed host controller
-	  integrated into many SoCs.
+	  integrated into many SoCs. Select this option if you want the
+	  driver to operate in Host-only mode.
 
-config USB_DWC2_PLATFORM
-	bool "DWC2 Platform"
-	depends on USB_DWC2_HOST
-	default USB_DWC2_HOST
+comment "Gadget/Dual-role mode requires USB Gadget support to be enabled"
+
+config USB_DWC2_PERIPHERAL
+	bool "Gadget only mode"
+	depends on USB_GADGET=y || USB_GADGET=USB_DWC2
 	help
-	  The Designware USB2.0 platform interface module for
-	  controllers directly connected to the CPU. This is only
-	  used for host mode.
+	  The Designware USB2.0 high-speed gadget controller
+	  integrated into many SoCs. Select this option if you want the
+	  driver to operate in Peripheral-only mode. This option requires
+	  USB_GADGET to be enabled.
+
+config USB_DWC2_DUAL_ROLE
+	bool "Dual Role mode"
+	depends on (USB=y || USB=USB_DWC2) && (USB_GADGET=y || USB_GADGET=USB_DWC2)
+	help
+	  Select this option if you want the driver to work in a dual-role
+	  mode. In this mode both host and gadget features are enabled, and
+	  the role will be determined by the cable that gets plugged-in. This
+	  option requires USB_GADGET to be enabled.
+endchoice
+
+config USB_DWC2_PLATFORM
+	tristate "DWC2 Platform"
+	default USB_DWC2_HOST || USB_DWC2_PERIPHERAL
+        help
+          The Designware USB2.0 platform interface module for
+          controllers directly connected to the CPU.
 
 config USB_DWC2_PCI
-	bool "DWC2 PCI"
+	tristate "DWC2 PCI"
 	depends on USB_DWC2_HOST && PCI
 	default USB_DWC2_HOST
 	help
 	  The Designware USB2.0 PCI interface module for controllers
 	  connected to a PCI bus. This is only used for host mode.
 
-comment "Gadget mode requires USB Gadget support to be enabled"
-
-config USB_DWC2_PERIPHERAL
-	tristate "Gadget only mode"
-	depends on USB_GADGET
-	help
-	  The Designware USB2.0 high-speed gadget controller
-	  integrated into many SoCs.
-
 config USB_DWC2_DEBUG
 	bool "Enable Debugging Messages"
 	help
diff --git a/drivers/usb/dwc2/Makefile b/drivers/usb/dwc2/Makefile
index b73d2a527970..8f752679752a 100644
--- a/drivers/usb/dwc2/Makefile
+++ b/drivers/usb/dwc2/Makefile
@@ -1,28 +1,28 @@
 ccflags-$(CONFIG_USB_DWC2_DEBUG)	+= -DDEBUG
 ccflags-$(CONFIG_USB_DWC2_VERBOSE)	+= -DVERBOSE_DEBUG
 
-obj-$(CONFIG_USB_DWC2_HOST)		+= dwc2.o
+obj-$(CONFIG_USB_DWC2)			+= dwc2.o
 dwc2-y					:= core.o core_intr.o
-dwc2-y					+= hcd.o hcd_intr.o
-dwc2-y					+= hcd_queue.o hcd_ddma.o
+
+ifneq ($(filter y,$(CONFIG_USB_DWC2_HOST) $(CONFIG_USB_DWC2_DUAL_ROLE)),)
+	dwc2-y				+= hcd.o hcd_intr.o
+	dwc2-y				+= hcd_queue.o hcd_ddma.o
+endif
+
+ifneq ($(filter y,$(CONFIG_USB_DWC2_PERIPHERAL) $(CONFIG_USB_DWC2_DUAL_ROLE)),)
+	dwc2-y       			+= gadget.o
+endif
 
 # NOTE: The previous s3c-hsotg peripheral mode only driver has been moved to
 # this location and renamed gadget.c. When building for dynamically linked
-# modules, dwc2_gadget.ko will get built for peripheral mode. For host mode,
-# the core module will be dwc2.ko, the PCI bus interface module will called
-# dwc2_pci.ko and the platform interface module will be called dwc2_platform.ko.
-# At present the host and gadget driver will be separate drivers, but there
-# are plans in the near future to create a dual-role driver.
+# modules, dwc2.ko will get built for host mode, peripheral mode, and dual-role
+# mode. The PCI bus interface module will called dwc2_pci.ko and the platform
+# interface module will be called dwc2_platform.ko.
 
 ifneq ($(CONFIG_USB_DWC2_PCI),)
-	obj-$(CONFIG_USB_DWC2_HOST)	+= dwc2_pci.o
+	obj-$(CONFIG_USB_DWC2)		+= dwc2_pci.o
 	dwc2_pci-y			:= pci.o
 endif
 
-ifneq ($(CONFIG_USB_DWC2_PLATFORM),)
-	obj-$(CONFIG_USB_DWC2_HOST)	+= dwc2_platform.o
-	dwc2_platform-y			:= platform.o
-endif
-
-obj-$(CONFIG_USB_DWC2_PERIPHERAL)	+= dwc2_gadget.o
-dwc2_gadget-y				:= gadget.o
+obj-$(CONFIG_USB_DWC2_PLATFORM)		+= dwc2_platform.o
+dwc2_platform-y				:= platform.o
diff --git a/drivers/usb/dwc2/core.c b/drivers/usb/dwc2/core.c
index d9269459d481..7605850b7a9c 100644
--- a/drivers/usb/dwc2/core.c
+++ b/drivers/usb/dwc2/core.c
@@ -458,16 +458,6 @@ int dwc2_core_init(struct dwc2_hsotg *hsotg, bool select_phy, int irq)
 	/* Clear the SRP success bit for FS-I2c */
 	hsotg->srp_success = 0;
 
-	if (irq >= 0) {
-		dev_dbg(hsotg->dev, "registering common handler for irq%d\n",
-			irq);
-		retval = devm_request_irq(hsotg->dev, irq,
-					  dwc2_handle_common_intr, IRQF_SHARED,
-					  dev_name(hsotg->dev), hsotg);
-		if (retval)
-			return retval;
-	}
-
 	/* Enable common interrupts */
 	dwc2_enable_common_interrupts(hsotg);
 
diff --git a/drivers/usb/dwc2/core.h b/drivers/usb/dwc2/core.h
index 55c90c53f2d6..7a70a1349334 100644
--- a/drivers/usb/dwc2/core.h
+++ b/drivers/usb/dwc2/core.h
@@ -84,7 +84,7 @@ static const char * const s3c_hsotg_supply_names[] = {
  */
 #define EP0_MPS_LIMIT   64
 
-struct s3c_hsotg;
+struct dwc2_hsotg;
 struct s3c_hsotg_req;
 
 /**
@@ -130,7 +130,7 @@ struct s3c_hsotg_req;
 struct s3c_hsotg_ep {
 	struct usb_ep           ep;
 	struct list_head        queue;
-	struct s3c_hsotg        *parent;
+	struct dwc2_hsotg       *parent;
 	struct s3c_hsotg_req    *req;
 	struct dentry           *debugfs;
 
@@ -155,67 +155,6 @@ struct s3c_hsotg_ep {
 };
 
 /**
- * struct s3c_hsotg - driver state.
- * @dev: The parent device supplied to the probe function
- * @driver: USB gadget driver
- * @phy: The otg phy transceiver structure for phy control.
- * @uphy: The otg phy transceiver structure for old USB phy control.
- * @plat: The platform specific configuration data. This can be removed once
- * all SoCs support usb transceiver.
- * @regs: The memory area mapped for accessing registers.
- * @irq: The IRQ number we are using
- * @supplies: Definition of USB power supplies
- * @phyif: PHY interface width
- * @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.
- * @ep0_reply: Request used for ep0 reply.
- * @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 {
-	struct device            *dev;
-	struct usb_gadget_driver *driver;
-	struct phy               *phy;
-	struct usb_phy           *uphy;
-	struct s3c_hsotg_plat    *plat;
-
-	spinlock_t              lock;
-
-	void __iomem            *regs;
-	int                     irq;
-	struct clk              *clk;
-
-	struct regulator_bulk_data supplies[ARRAY_SIZE(s3c_hsotg_supply_names)];
-
-	u32                     phyif;
-	int			fifo_mem;
-	unsigned int            dedicated_fifos:1;
-	unsigned char           num_of_eps;
-	u32			fifo_map;
-
-	struct dentry           *debug_root;
-	struct dentry           *debug_file;
-	struct dentry           *debug_fifo;
-
-	struct usb_request      *ep0_reply;
-	struct usb_request      *ctrl_req;
-	u8                      ep0_buff[8];
-	u8                      ctrl_buff[8];
-
-	struct usb_gadget       gadget;
-	unsigned int            setup;
-	unsigned long           last_rst;
-	struct s3c_hsotg_ep     *eps;
-};
-
-/**
  * struct s3c_hsotg_req - data transfer request
  * @req: The USB gadget request
  * @queue: The list of requests for the endpoint this is queued for.
@@ -229,6 +168,7 @@ struct s3c_hsotg_req {
 	unsigned char           mapped;
 };
 
+#if IS_ENABLED(CONFIG_USB_DWC2_PERIPHERAL) || IS_ENABLED(CONFIG_USB_DWC2_DUAL_ROLE)
 #define call_gadget(_hs, _entry) \
 do { \
 	if ((_hs)->gadget.speed != USB_SPEED_UNKNOWN && \
@@ -238,6 +178,9 @@ do { \
 		spin_lock(&_hs->lock); \
 	} \
 } while (0)
+#else
+#define call_gadget(_hs, _entry)	do {} while (0)
+#endif
 
 struct dwc2_hsotg;
 struct dwc2_host_chan;
@@ -495,11 +438,13 @@ struct dwc2_hw_params {
  * struct dwc2_hsotg - Holds the state of the driver, including the non-periodic
  * and periodic schedules
  *
+ * These are common for both host and peripheral modes:
+ *
  * @dev:                The struct device pointer
  * @regs:		Pointer to controller regs
- * @core_params:        Parameters that define how the core should be configured
  * @hw_params:          Parameters that were autodetected from the
  *                      hardware registers
+ * @core_params:	Parameters that define how the core should be configured
  * @op_state:           The operational State, during transitions (a_host=>
  *                      a_peripheral and b_device=>b_host) this may not match
  *                      the core, but allows the software to determine
@@ -508,6 +453,8 @@ struct dwc2_hw_params {
  *                      - USB_DR_MODE_PERIPHERAL
  *                      - USB_DR_MODE_HOST
  *                      - USB_DR_MODE_OTG
+ * @lock:		Spinlock that protects all the driver data structures
+ * @priv:		Stores a pointer to the struct usb_hcd
  * @queuing_high_bandwidth: True if multiple packets of a high-bandwidth
  *                      transfer are in process of being queued
  * @srp_success:        Stores status of SRP request in the case of a FS PHY
@@ -517,6 +464,9 @@ struct dwc2_hw_params {
  *                      interrupt
  * @wkp_timer:          Timer object for handling Wakeup Detected interrupt
  * @lx_state:           Lx state of connected device
+ *
+ * These are for host mode:
+ *
  * @flags:              Flags for handling root port state changes
  * @non_periodic_sched_inactive: Inactive QHs in the non-periodic schedule.
  *                      Transfers associated with these QHs are not currently
@@ -585,11 +535,31 @@ struct dwc2_hw_params {
  * @status_buf_dma:     DMA address for status_buf
  * @start_work:         Delayed work for handling host A-cable connection
  * @reset_work:         Delayed work for handling a port reset
- * @lock:               Spinlock that protects all the driver data structures
- * @priv:               Stores a pointer to the struct usb_hcd
  * @otg_port:           OTG port number
  * @frame_list:         Frame list
  * @frame_list_dma:     Frame list DMA address
+ *
+ * These are for peripheral mode:
+ *
+ * @driver:             USB gadget driver
+ * @phy:                The otg phy transceiver structure for phy control.
+ * @uphy:               The otg phy transceiver structure for old USB phy control.
+ * @plat:               The platform specific configuration data. This can be removed once
+ *                      all SoCs support usb transceiver.
+ * @supplies:           Definition of USB power supplies
+ * @phyif:              PHY interface width
+ * @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.
+ * @ep0_reply:          Request used for ep0 reply.
+ * @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 dwc2_hsotg {
 	struct device *dev;
@@ -601,6 +571,16 @@ struct dwc2_hsotg {
 	enum usb_otg_state op_state;
 	enum usb_dr_mode dr_mode;
 
+	struct phy *phy;
+	struct usb_phy *uphy;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(s3c_hsotg_supply_names)];
+
+	spinlock_t lock;
+	struct mutex init_mutex;
+	void *priv;
+	int     irq;
+	struct clk *clk;
+
 	unsigned int queuing_high_bandwidth:1;
 	unsigned int srp_success:1;
 
@@ -609,6 +589,18 @@ struct dwc2_hsotg {
 	struct timer_list wkp_timer;
 	enum dwc2_lx_state lx_state;
 
+	struct dentry *debug_root;
+	struct dentry *debug_file;
+	struct dentry *debug_fifo;
+
+	/* DWC OTG HW Release versions */
+#define DWC2_CORE_REV_2_71a	0x4f54271a
+#define DWC2_CORE_REV_2_90a	0x4f54290a
+#define DWC2_CORE_REV_2_92a	0x4f54292a
+#define DWC2_CORE_REV_2_94a	0x4f54294a
+#define DWC2_CORE_REV_3_00a	0x4f54300a
+
+#if IS_ENABLED(CONFIG_USB_DWC2_HOST) || IS_ENABLED(CONFIG_USB_DWC2_DUAL_ROLE)
 	union dwc2_hcd_internal_flags {
 		u32 d32;
 		struct {
@@ -655,19 +647,10 @@ struct dwc2_hsotg {
 
 	struct delayed_work start_work;
 	struct delayed_work reset_work;
-	spinlock_t lock;
-	void *priv;
 	u8 otg_port;
 	u32 *frame_list;
 	dma_addr_t frame_list_dma;
 
-	/* DWC OTG HW Release versions */
-#define DWC2_CORE_REV_2_71a	0x4f54271a
-#define DWC2_CORE_REV_2_90a	0x4f54290a
-#define DWC2_CORE_REV_2_92a	0x4f54292a
-#define DWC2_CORE_REV_2_94a	0x4f54294a
-#define DWC2_CORE_REV_3_00a	0x4f54300a
-
 #ifdef DEBUG
 	u32 frrem_samples;
 	u64 frrem_accum;
@@ -686,6 +669,31 @@ struct dwc2_hsotg {
 	u32 hfnum_other_samples_b;
 	u64 hfnum_other_frrem_accum_b;
 #endif
+#endif /* CONFIG_USB_DWC2_HOST || CONFIG_USB_DWC2_DUAL_ROLE */
+
+#if IS_ENABLED(CONFIG_USB_DWC2_PERIPHERAL) || IS_ENABLED(CONFIG_USB_DWC2_DUAL_ROLE)
+	/* Gadget structures */
+	struct usb_gadget_driver *driver;
+	struct s3c_hsotg_plat *plat;
+
+	u32 phyif;
+	int fifo_mem;
+	unsigned int dedicated_fifos:1;
+	unsigned char num_of_eps;
+	u32 fifo_map;
+
+	struct usb_request *ep0_reply;
+	struct usb_request *ctrl_req;
+	u8 ep0_buff[8];
+	u8 ctrl_buff[8];
+
+	struct usb_gadget gadget;
+	unsigned int enabled:1;
+	unsigned int connected:1;
+	unsigned int setup:1;
+	unsigned long last_rst;
+	struct s3c_hsotg_ep *eps;
+#endif /* CONFIG_USB_DWC2_PERIPHERAL || CONFIG_USB_DWC2_DUAL_ROLE */
 };
 
 /* Reasons for halting a host channel */
@@ -955,4 +963,43 @@ extern void dwc2_dump_global_registers(struct dwc2_hsotg *hsotg);
  */
 extern u16 dwc2_get_otg_version(struct dwc2_hsotg *hsotg);
 
+/* Gadget defines */
+#if IS_ENABLED(CONFIG_USB_DWC2_PERIPHERAL) || IS_ENABLED(CONFIG_USB_DWC2_DUAL_ROLE)
+extern int s3c_hsotg_remove(struct dwc2_hsotg *hsotg);
+extern int s3c_hsotg_suspend(struct dwc2_hsotg *dwc2);
+extern int s3c_hsotg_resume(struct dwc2_hsotg *dwc2);
+extern int dwc2_gadget_init(struct dwc2_hsotg *hsotg, int irq);
+extern void s3c_hsotg_core_init_disconnected(struct dwc2_hsotg *dwc2);
+extern void s3c_hsotg_core_connect(struct dwc2_hsotg *hsotg);
+extern void s3c_hsotg_disconnect(struct dwc2_hsotg *dwc2);
+#else
+static inline int s3c_hsotg_remove(struct dwc2_hsotg *dwc2)
+{ return 0; }
+static inline int s3c_hsotg_suspend(struct dwc2_hsotg *dwc2)
+{ return 0; }
+static inline int s3c_hsotg_resume(struct dwc2_hsotg *dwc2)
+{ return 0; }
+static inline int dwc2_gadget_init(struct dwc2_hsotg *hsotg, int irq)
+{ return 0; }
+static inline void s3c_hsotg_core_init_disconnected(struct dwc2_hsotg *dwc2) {}
+static inline void s3c_hsotg_core_connect(struct dwc2_hsotg *hsotg) {}
+static inline void s3c_hsotg_disconnect(struct dwc2_hsotg *dwc2) {}
+#endif
+
+#if IS_ENABLED(CONFIG_USB_DWC2_HOST) || IS_ENABLED(CONFIG_USB_DWC2_DUAL_ROLE)
+extern int dwc2_hcd_get_frame_number(struct dwc2_hsotg *hsotg);
+extern void dwc2_hcd_disconnect(struct dwc2_hsotg *hsotg);
+extern void dwc2_hcd_start(struct dwc2_hsotg *hsotg);
+#else
+static inline void dwc2_set_all_params(struct dwc2_core_params *params, int value) {}
+static inline int dwc2_hcd_get_frame_number(struct dwc2_hsotg *hsotg)
+{ return 0; }
+static inline void dwc2_hcd_disconnect(struct dwc2_hsotg *hsotg) {}
+static inline void dwc2_hcd_start(struct dwc2_hsotg *hsotg) {}
+static inline void dwc2_hcd_remove(struct dwc2_hsotg *hsotg) {}
+static inline int dwc2_hcd_init(struct dwc2_hsotg *hsotg, int irq,
+				const struct dwc2_core_params *params)
+{ return 0; }
+#endif
+
 #endif /* __DWC2_CORE_H__ */
diff --git a/drivers/usb/dwc2/core_intr.c b/drivers/usb/dwc2/core_intr.c
index c93918b70d03..ad43c5bc1ef1 100644
--- a/drivers/usb/dwc2/core_intr.c
+++ b/drivers/usb/dwc2/core_intr.c
@@ -128,6 +128,9 @@ static void dwc2_handle_otg_intr(struct dwc2_hsotg *hsotg)
 			dwc2_op_state_str(hsotg));
 		gotgctl = readl(hsotg->regs + GOTGCTL);
 
+		if (dwc2_is_device_mode(hsotg))
+			s3c_hsotg_disconnect(hsotg);
+
 		if (hsotg->op_state == OTG_STATE_B_HOST) {
 			hsotg->op_state = OTG_STATE_B_PERIPHERAL;
 		} else {
@@ -287,9 +290,11 @@ static void dwc2_handle_conn_id_status_change_intr(struct dwc2_hsotg *hsotg)
 	 * Release lock before scheduling workq as it holds spinlock during
 	 * scheduling.
 	 */
-	spin_unlock(&hsotg->lock);
-	queue_work(hsotg->wq_otg, &hsotg->wf_otg);
-	spin_lock(&hsotg->lock);
+	if (hsotg->wq_otg) {
+		spin_unlock(&hsotg->lock);
+		queue_work(hsotg->wq_otg, &hsotg->wf_otg);
+		spin_lock(&hsotg->lock);
+	}
 
 	/* Clear interrupt */
 	writel(GINTSTS_CONIDSTSCHNG, hsotg->regs + GINTSTS);
@@ -312,6 +317,12 @@ static void dwc2_handle_session_req_intr(struct dwc2_hsotg *hsotg)
 
 	/* Clear interrupt */
 	writel(GINTSTS_SESSREQINT, hsotg->regs + GINTSTS);
+
+	/*
+	 * Report disconnect if there is any previous session established
+	 */
+	if (dwc2_is_device_mode(hsotg))
+		s3c_hsotg_disconnect(hsotg);
 }
 
 /*
diff --git a/drivers/usb/dwc2/gadget.c b/drivers/usb/dwc2/gadget.c
index 8b5c079c7b7d..200168ec2d75 100644
--- a/drivers/usb/dwc2/gadget.c
+++ b/drivers/usb/dwc2/gadget.c
@@ -21,6 +21,7 @@
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
 #include <linux/debugfs.h>
+#include <linux/mutex.h>
 #include <linux/seq_file.h>
 #include <linux/delay.h>
 #include <linux/io.h>
@@ -36,6 +37,7 @@
 #include <linux/platform_data/s3c-hsotg.h>
 
 #include "core.h"
+#include "hw.h"
 
 /* conversion functions */
 static inline struct s3c_hsotg_req *our_req(struct usb_request *req)
@@ -48,9 +50,9 @@ static inline struct s3c_hsotg_ep *our_ep(struct usb_ep *ep)
 	return container_of(ep, struct s3c_hsotg_ep, ep);
 }
 
-static inline struct s3c_hsotg *to_hsotg(struct usb_gadget *gadget)
+static inline struct dwc2_hsotg *to_hsotg(struct usb_gadget *gadget)
 {
-	return container_of(gadget, struct s3c_hsotg, gadget);
+	return container_of(gadget, struct dwc2_hsotg, gadget);
 }
 
 static inline void __orr32(void __iomem *ptr, u32 val)
@@ -64,7 +66,7 @@ static inline void __bic32(void __iomem *ptr, u32 val)
 }
 
 /* forward decleration of functions */
-static void s3c_hsotg_dump(struct s3c_hsotg *hsotg);
+static void s3c_hsotg_dump(struct dwc2_hsotg *hsotg);
 
 /**
  * using_dma - return the DMA status of the driver.
@@ -85,7 +87,7 @@ static void s3c_hsotg_dump(struct s3c_hsotg *hsotg);
  *
  * Until this issue is sorted out, we always return 'false'.
  */
-static inline bool using_dma(struct s3c_hsotg *hsotg)
+static inline bool using_dma(struct dwc2_hsotg *hsotg)
 {
 	return false;	/* support is not complete */
 }
@@ -95,7 +97,7 @@ static inline bool using_dma(struct s3c_hsotg *hsotg)
  * @hsotg: The device state
  * @ints: A bitmask of the interrupts to enable
  */
-static void s3c_hsotg_en_gsint(struct s3c_hsotg *hsotg, u32 ints)
+static void s3c_hsotg_en_gsint(struct dwc2_hsotg *hsotg, u32 ints)
 {
 	u32 gsintmsk = readl(hsotg->regs + GINTMSK);
 	u32 new_gsintmsk;
@@ -113,7 +115,7 @@ static void s3c_hsotg_en_gsint(struct s3c_hsotg *hsotg, u32 ints)
  * @hsotg: The device state
  * @ints: A bitmask of the interrupts to enable
  */
-static void s3c_hsotg_disable_gsint(struct s3c_hsotg *hsotg, u32 ints)
+static void s3c_hsotg_disable_gsint(struct dwc2_hsotg *hsotg, u32 ints)
 {
 	u32 gsintmsk = readl(hsotg->regs + GINTMSK);
 	u32 new_gsintmsk;
@@ -134,7 +136,7 @@ static void s3c_hsotg_disable_gsint(struct s3c_hsotg *hsotg, u32 ints)
  * Set or clear the mask for an individual endpoint's interrupt
  * request.
  */
-static void s3c_hsotg_ctrl_epint(struct s3c_hsotg *hsotg,
+static void s3c_hsotg_ctrl_epint(struct dwc2_hsotg *hsotg,
 				 unsigned int ep, unsigned int dir_in,
 				 unsigned int en)
 {
@@ -159,7 +161,7 @@ static void s3c_hsotg_ctrl_epint(struct s3c_hsotg *hsotg,
  * s3c_hsotg_init_fifo - initialise non-periodic FIFOs
  * @hsotg: The device instance.
  */
-static void s3c_hsotg_init_fifo(struct s3c_hsotg *hsotg)
+static void s3c_hsotg_init_fifo(struct dwc2_hsotg *hsotg)
 {
 	unsigned int ep;
 	unsigned int addr;
@@ -283,7 +285,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,
+static void s3c_hsotg_unmap_dma(struct dwc2_hsotg *hsotg,
 				struct s3c_hsotg_ep *hs_ep,
 				struct s3c_hsotg_req *hs_req)
 {
@@ -312,7 +314,7 @@ static void s3c_hsotg_unmap_dma(struct s3c_hsotg *hsotg,
  *
  * This routine is only needed for PIO
  */
-static int s3c_hsotg_write_fifo(struct s3c_hsotg *hsotg,
+static int s3c_hsotg_write_fifo(struct dwc2_hsotg *hsotg,
 				struct s3c_hsotg_ep *hs_ep,
 				struct s3c_hsotg_req *hs_req)
 {
@@ -517,7 +519,7 @@ static unsigned get_ep_limit(struct s3c_hsotg_ep *hs_ep)
  * Start the given request running by setting the endpoint registers
  * appropriately, and writing any data to the FIFOs.
  */
-static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
+static void s3c_hsotg_start_req(struct dwc2_hsotg *hsotg,
 				struct s3c_hsotg_ep *hs_ep,
 				struct s3c_hsotg_req *hs_req,
 				bool continuing)
@@ -707,7 +709,7 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
  * 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,
+static int s3c_hsotg_map_dma(struct dwc2_hsotg *hsotg,
 			     struct s3c_hsotg_ep *hs_ep,
 			     struct usb_request *req)
 {
@@ -736,7 +738,7 @@ static int s3c_hsotg_ep_queue(struct usb_ep *ep, struct usb_request *req,
 {
 	struct s3c_hsotg_req *hs_req = our_req(req);
 	struct s3c_hsotg_ep *hs_ep = our_ep(ep);
-	struct s3c_hsotg *hs = hs_ep->parent;
+	struct dwc2_hsotg *hs = hs_ep->parent;
 	bool first;
 
 	dev_dbg(hs->dev, "%s: req %p: %d@%p, noi=%d, zero=%d, snok=%d\n",
@@ -768,7 +770,7 @@ static int s3c_hsotg_ep_queue_lock(struct usb_ep *ep, struct usb_request *req,
 			      gfp_t gfp_flags)
 {
 	struct s3c_hsotg_ep *hs_ep = our_ep(ep);
-	struct s3c_hsotg *hs = hs_ep->parent;
+	struct dwc2_hsotg *hs = hs_ep->parent;
 	unsigned long flags = 0;
 	int ret = 0;
 
@@ -799,7 +801,7 @@ static void s3c_hsotg_complete_oursetup(struct usb_ep *ep,
 					struct usb_request *req)
 {
 	struct s3c_hsotg_ep *hs_ep = our_ep(ep);
-	struct s3c_hsotg *hsotg = hs_ep->parent;
+	struct dwc2_hsotg *hsotg = hs_ep->parent;
 
 	dev_dbg(hsotg->dev, "%s: ep %p, req %p\n", __func__, ep, req);
 
@@ -814,7 +816,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,
+static struct s3c_hsotg_ep *ep_from_windex(struct dwc2_hsotg *hsotg,
 					   u32 windex)
 {
 	struct s3c_hsotg_ep *ep = &hsotg->eps[windex & 0x7F];
@@ -843,7 +845,7 @@ static struct s3c_hsotg_ep *ep_from_windex(struct s3c_hsotg *hsotg,
  * Create a request and queue it on the given endpoint. This is useful as
  * an internal method of sending replies to certain control requests, etc.
  */
-static int s3c_hsotg_send_reply(struct s3c_hsotg *hsotg,
+static int s3c_hsotg_send_reply(struct dwc2_hsotg *hsotg,
 				struct s3c_hsotg_ep *ep,
 				void *buff,
 				int length)
@@ -884,7 +886,7 @@ static int s3c_hsotg_send_reply(struct s3c_hsotg *hsotg,
  * @hsotg: The device state
  * @ctrl: USB control request
  */
-static int s3c_hsotg_process_req_status(struct s3c_hsotg *hsotg,
+static int s3c_hsotg_process_req_status(struct dwc2_hsotg *hsotg,
 					struct usb_ctrlrequest *ctrl)
 {
 	struct s3c_hsotg_ep *ep0 = &hsotg->eps[0];
@@ -955,7 +957,7 @@ static struct s3c_hsotg_req *get_ep_head(struct s3c_hsotg_ep *hs_ep)
  * @hsotg: The device state
  * @ctrl: USB control request
  */
-static int s3c_hsotg_process_req_feature(struct s3c_hsotg *hsotg,
+static int s3c_hsotg_process_req_feature(struct dwc2_hsotg *hsotg,
 					 struct usb_ctrlrequest *ctrl)
 {
 	struct s3c_hsotg_ep *ep0 = &hsotg->eps[0];
@@ -1028,8 +1030,7 @@ static int s3c_hsotg_process_req_feature(struct s3c_hsotg *hsotg,
 	return 1;
 }
 
-static void s3c_hsotg_enqueue_setup(struct s3c_hsotg *hsotg);
-static void s3c_hsotg_disconnect(struct s3c_hsotg *hsotg);
+static void s3c_hsotg_enqueue_setup(struct dwc2_hsotg *hsotg);
 
 /**
  * s3c_hsotg_stall_ep0 - stall ep0
@@ -1037,7 +1038,7 @@ static void s3c_hsotg_disconnect(struct s3c_hsotg *hsotg);
  *
  * Set stall for ep0 as response for setup request.
  */
-static void s3c_hsotg_stall_ep0(struct s3c_hsotg *hsotg)
+static void s3c_hsotg_stall_ep0(struct dwc2_hsotg *hsotg)
 {
 	struct s3c_hsotg_ep *ep0 = &hsotg->eps[0];
 	u32 reg;
@@ -1076,7 +1077,7 @@ static void s3c_hsotg_stall_ep0(struct s3c_hsotg *hsotg)
  * needs to work out what to do next (and whether to pass it on to the
  * gadget driver).
  */
-static void s3c_hsotg_process_control(struct s3c_hsotg *hsotg,
+static void s3c_hsotg_process_control(struct dwc2_hsotg *hsotg,
 				      struct usb_ctrlrequest *ctrl)
 {
 	struct s3c_hsotg_ep *ep0 = &hsotg->eps[0];
@@ -1107,7 +1108,6 @@ static void s3c_hsotg_process_control(struct s3c_hsotg *hsotg,
 	if ((ctrl->bRequestType & USB_TYPE_MASK) == USB_TYPE_STANDARD) {
 		switch (ctrl->bRequest) {
 		case USB_REQ_SET_ADDRESS:
-			s3c_hsotg_disconnect(hsotg);
 			dcfg = readl(hsotg->regs + DCFG);
 			dcfg &= ~DCFG_DEVADDR_MASK;
 			dcfg |= (le16_to_cpu(ctrl->wValue) <<
@@ -1161,7 +1161,7 @@ static void s3c_hsotg_complete_setup(struct usb_ep *ep,
 				     struct usb_request *req)
 {
 	struct s3c_hsotg_ep *hs_ep = our_ep(ep);
-	struct s3c_hsotg *hsotg = hs_ep->parent;
+	struct dwc2_hsotg *hsotg = hs_ep->parent;
 
 	if (req->status < 0) {
 		dev_dbg(hsotg->dev, "%s: failed %d\n", __func__, req->status);
@@ -1183,7 +1183,7 @@ static void s3c_hsotg_complete_setup(struct usb_ep *ep,
  * Enqueue a request on EP0 if necessary to received any SETUP packets
  * received from the host.
  */
-static void s3c_hsotg_enqueue_setup(struct s3c_hsotg *hsotg)
+static void s3c_hsotg_enqueue_setup(struct dwc2_hsotg *hsotg)
 {
 	struct usb_request *req = hsotg->ctrl_req;
 	struct s3c_hsotg_req *hs_req = our_req(req);
@@ -1226,7 +1226,7 @@ static void s3c_hsotg_enqueue_setup(struct s3c_hsotg *hsotg)
  *
  * Note, expects the ep to already be locked as appropriate.
  */
-static void s3c_hsotg_complete_request(struct s3c_hsotg *hsotg,
+static void s3c_hsotg_complete_request(struct dwc2_hsotg *hsotg,
 				       struct s3c_hsotg_ep *hs_ep,
 				       struct s3c_hsotg_req *hs_req,
 				       int result)
@@ -1291,7 +1291,7 @@ static void s3c_hsotg_complete_request(struct s3c_hsotg *hsotg,
  * endpoint, so sort out whether we need to read the data into a request
  * that has been made for that endpoint.
  */
-static void s3c_hsotg_rx_data(struct s3c_hsotg *hsotg, int ep_idx, int size)
+static void s3c_hsotg_rx_data(struct dwc2_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;
@@ -1356,7 +1356,7 @@ static void s3c_hsotg_rx_data(struct s3c_hsotg *hsotg, int ep_idx, int size)
  * currently believed that we do not need to wait for any space in
  * the TxFIFO.
  */
-static void s3c_hsotg_send_zlp(struct s3c_hsotg *hsotg,
+static void s3c_hsotg_send_zlp(struct dwc2_hsotg *hsotg,
 			       struct s3c_hsotg_req *req)
 {
 	u32 ctrl;
@@ -1398,7 +1398,7 @@ static void s3c_hsotg_send_zlp(struct s3c_hsotg *hsotg,
  * 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,
+static void s3c_hsotg_handle_outdone(struct dwc2_hsotg *hsotg,
 				     int epnum, bool was_setup)
 {
 	u32 epsize = readl(hsotg->regs + DOEPTSIZ(epnum));
@@ -1471,7 +1471,7 @@ static void s3c_hsotg_handle_outdone(struct s3c_hsotg *hsotg,
  *
  * Return the current frame number
  */
-static u32 s3c_hsotg_read_frameno(struct s3c_hsotg *hsotg)
+static u32 s3c_hsotg_read_frameno(struct dwc2_hsotg *hsotg)
 {
 	u32 dsts;
 
@@ -1498,7 +1498,7 @@ static u32 s3c_hsotg_read_frameno(struct s3c_hsotg *hsotg)
  * as the actual data should be sent to the memory directly and we turn
  * on the completion interrupts to get notifications of transfer completion.
  */
-static void s3c_hsotg_handle_rx(struct s3c_hsotg *hsotg)
+static void s3c_hsotg_handle_rx(struct dwc2_hsotg *hsotg)
 {
 	u32 grxstsr = readl(hsotg->regs + GRXSTSP);
 	u32 epnum, status, size;
@@ -1590,7 +1590,7 @@ static u32 s3c_hsotg_ep0_mps(unsigned int mps)
  * Configure the maximum packet size for the given endpoint, updating
  * the hardware control registers to reflect this.
  */
-static void s3c_hsotg_set_ep_maxpacket(struct s3c_hsotg *hsotg,
+static void s3c_hsotg_set_ep_maxpacket(struct dwc2_hsotg *hsotg,
 				       unsigned int ep, unsigned int mps)
 {
 	struct s3c_hsotg_ep *hs_ep = &hsotg->eps[ep];
@@ -1645,7 +1645,7 @@ bad_mps:
  * @hsotg: The driver state
  * @idx: The index for the endpoint (0..15)
  */
-static void s3c_hsotg_txfifo_flush(struct s3c_hsotg *hsotg, unsigned int idx)
+static void s3c_hsotg_txfifo_flush(struct dwc2_hsotg *hsotg, unsigned int idx)
 {
 	int timeout;
 	int val;
@@ -1681,7 +1681,7 @@ static void s3c_hsotg_txfifo_flush(struct s3c_hsotg *hsotg, unsigned int idx)
  * Check to see if there is a request that has data to send, and if so
  * make an attempt to write data into the FIFO.
  */
-static int s3c_hsotg_trytx(struct s3c_hsotg *hsotg,
+static int s3c_hsotg_trytx(struct dwc2_hsotg *hsotg,
 			   struct s3c_hsotg_ep *hs_ep)
 {
 	struct s3c_hsotg_req *hs_req = hs_ep->req;
@@ -1714,7 +1714,7 @@ static int s3c_hsotg_trytx(struct s3c_hsotg *hsotg,
  * An IN transfer has been completed, update the transfer's state and then
  * call the relevant completion routines.
  */
-static void s3c_hsotg_complete_in(struct s3c_hsotg *hsotg,
+static void s3c_hsotg_complete_in(struct dwc2_hsotg *hsotg,
 				  struct s3c_hsotg_ep *hs_ep)
 {
 	struct s3c_hsotg_req *hs_req = hs_ep->req;
@@ -1791,7 +1791,7 @@ static void s3c_hsotg_complete_in(struct s3c_hsotg *hsotg,
  *
  * Process and clear any interrupt pending for an individual endpoint
  */
-static void s3c_hsotg_epint(struct s3c_hsotg *hsotg, unsigned int idx,
+static void s3c_hsotg_epint(struct dwc2_hsotg *hsotg, unsigned int idx,
 			    int dir_in)
 {
 	struct s3c_hsotg_ep *hs_ep = &hsotg->eps[idx];
@@ -1916,7 +1916,7 @@ 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)
+static void s3c_hsotg_irq_enumdone(struct dwc2_hsotg *hsotg)
 {
 	u32 dsts = readl(hsotg->regs + DSTS);
 	int ep0_mps = 0, ep_mps = 8;
@@ -1993,7 +1993,7 @@ static void s3c_hsotg_irq_enumdone(struct s3c_hsotg *hsotg)
  * Go through the requests on the given endpoint and mark them
  * completed with the given result code.
  */
-static void kill_all_requests(struct s3c_hsotg *hsotg,
+static void kill_all_requests(struct dwc2_hsotg *hsotg,
 			      struct s3c_hsotg_ep *ep,
 			      int result, bool force)
 {
@@ -2027,22 +2027,27 @@ static void kill_all_requests(struct s3c_hsotg *hsotg,
  * transactions and signal the gadget driver that this
  * has happened.
  */
-static void s3c_hsotg_disconnect(struct s3c_hsotg *hsotg)
+void s3c_hsotg_disconnect(struct dwc2_hsotg *hsotg)
 {
 	unsigned ep;
 
+	if (!hsotg->connected)
+		return;
+
+	hsotg->connected = 0;
 	for (ep = 0; ep < hsotg->num_of_eps; ep++)
 		kill_all_requests(hsotg, &hsotg->eps[ep], -ESHUTDOWN, true);
 
 	call_gadget(hsotg, disconnect);
 }
+EXPORT_SYMBOL_GPL(s3c_hsotg_disconnect);
 
 /**
  * s3c_hsotg_irq_fifoempty - TX FIFO empty interrupt handler
  * @hsotg: The device state:
  * @periodic: True if this is a periodic FIFO interrupt
  */
-static void s3c_hsotg_irq_fifoempty(struct s3c_hsotg *hsotg, bool periodic)
+static void s3c_hsotg_irq_fifoempty(struct dwc2_hsotg *hsotg, bool periodic)
 {
 	struct s3c_hsotg_ep *ep;
 	int epno, ret;
@@ -2076,7 +2081,7 @@ static void s3c_hsotg_irq_fifoempty(struct s3c_hsotg *hsotg, bool periodic)
  *
  * Issue a soft reset to the core, and await the core finishing it.
  */
-static int s3c_hsotg_corereset(struct s3c_hsotg *hsotg)
+static int s3c_hsotg_corereset(struct dwc2_hsotg *hsotg)
 {
 	int timeout;
 	u32 grstctl;
@@ -2124,7 +2129,7 @@ static int s3c_hsotg_corereset(struct s3c_hsotg *hsotg)
  *
  * Issue a soft reset to the core, and await the core finishing it.
  */
-static void s3c_hsotg_core_init(struct s3c_hsotg *hsotg)
+void s3c_hsotg_core_init_disconnected(struct dwc2_hsotg *hsotg)
 {
 	s3c_hsotg_corereset(hsotg);
 
@@ -2241,12 +2246,23 @@ static void s3c_hsotg_core_init(struct s3c_hsotg *hsotg)
 		readl(hsotg->regs + DOEPCTL0));
 
 	/* clear global NAKs */
-	writel(DCTL_CGOUTNAK | DCTL_CGNPINNAK,
+	writel(DCTL_CGOUTNAK | DCTL_CGNPINNAK | DCTL_SFTDISCON,
 	       hsotg->regs + DCTL);
 
 	/* must be at-least 3ms to allow bus to see disconnect */
 	mdelay(3);
 
+	hsotg->last_rst = jiffies;
+}
+
+static void s3c_hsotg_core_disconnect(struct dwc2_hsotg *hsotg)
+{
+	/* set the soft-disconnect bit */
+	__orr32(hsotg->regs + DCTL, DCTL_SFTDISCON);
+}
+
+void s3c_hsotg_core_connect(struct dwc2_hsotg *hsotg)
+{
 	/* remove the soft-disconnect and let's go */
 	__bic32(hsotg->regs + DCTL, DCTL_SFTDISCON);
 }
@@ -2258,7 +2274,7 @@ static void s3c_hsotg_core_init(struct s3c_hsotg *hsotg)
  */
 static irqreturn_t s3c_hsotg_irq(int irq, void *pw)
 {
-	struct s3c_hsotg *hsotg = pw;
+	struct dwc2_hsotg *hsotg = pw;
 	int retry_count = 8;
 	u32 gintsts;
 	u32 gintmsk;
@@ -2273,31 +2289,11 @@ irq_retry:
 
 	gintsts &= gintmsk;
 
-	if (gintsts & GINTSTS_OTGINT) {
-		u32 otgint = readl(hsotg->regs + GOTGINT);
-
-		dev_info(hsotg->dev, "OTGInt: %08x\n", otgint);
-
-		writel(otgint, hsotg->regs + GOTGINT);
-	}
-
-	if (gintsts & GINTSTS_SESSREQINT) {
-		dev_dbg(hsotg->dev, "%s: SessReqInt\n", __func__);
-		writel(GINTSTS_SESSREQINT, hsotg->regs + GINTSTS);
-	}
-
 	if (gintsts & GINTSTS_ENUMDONE) {
 		writel(GINTSTS_ENUMDONE, hsotg->regs + GINTSTS);
 
 		s3c_hsotg_irq_enumdone(hsotg);
-	}
-
-	if (gintsts & GINTSTS_CONIDSTSCHNG) {
-		dev_dbg(hsotg->dev, "ConIDStsChg (DSTS=0x%08x, GOTCTL=%08x)\n",
-			readl(hsotg->regs + DSTS),
-			readl(hsotg->regs + GOTGCTL));
-
-		writel(GINTSTS_CONIDSTSCHNG, hsotg->regs + GINTSTS);
+		hsotg->connected = 1;
 	}
 
 	if (gintsts & (GINTSTS_OEPINT | GINTSTS_IEPINT)) {
@@ -2340,8 +2336,8 @@ irq_retry:
 				kill_all_requests(hsotg, &hsotg->eps[0],
 							  -ECONNRESET, true);
 
-				s3c_hsotg_core_init(hsotg);
-				hsotg->last_rst = jiffies;
+				s3c_hsotg_core_init_disconnected(hsotg);
+				s3c_hsotg_core_connect(hsotg);
 			}
 		}
 	}
@@ -2380,25 +2376,6 @@ irq_retry:
 		s3c_hsotg_handle_rx(hsotg);
 	}
 
-	if (gintsts & GINTSTS_MODEMIS) {
-		dev_warn(hsotg->dev, "warning, mode mismatch triggered\n");
-		writel(GINTSTS_MODEMIS, hsotg->regs + GINTSTS);
-	}
-
-	if (gintsts & GINTSTS_USBSUSP) {
-		dev_info(hsotg->dev, "GINTSTS_USBSusp\n");
-		writel(GINTSTS_USBSUSP, hsotg->regs + GINTSTS);
-
-		call_gadget(hsotg, suspend);
-	}
-
-	if (gintsts & GINTSTS_WKUPINT) {
-		dev_info(hsotg->dev, "GINTSTS_WkUpIn\n");
-		writel(GINTSTS_WKUPINT, hsotg->regs + GINTSTS);
-
-		call_gadget(hsotg, resume);
-	}
-
 	if (gintsts & GINTSTS_ERLYSUSP) {
 		dev_dbg(hsotg->dev, "GINTSTS_ErlySusp\n");
 		writel(GINTSTS_ERLYSUSP, hsotg->regs + GINTSTS);
@@ -2450,7 +2427,7 @@ static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 			       const struct usb_endpoint_descriptor *desc)
 {
 	struct s3c_hsotg_ep *hs_ep = our_ep(ep);
-	struct s3c_hsotg *hsotg = hs_ep->parent;
+	struct dwc2_hsotg *hsotg = hs_ep->parent;
 	unsigned long flags;
 	int index = hs_ep->index;
 	u32 epctrl_reg;
@@ -2593,7 +2570,7 @@ error:
 static int s3c_hsotg_ep_disable(struct usb_ep *ep)
 {
 	struct s3c_hsotg_ep *hs_ep = our_ep(ep);
-	struct s3c_hsotg *hsotg = hs_ep->parent;
+	struct dwc2_hsotg *hsotg = hs_ep->parent;
 	int dir_in = hs_ep->dir_in;
 	int index = hs_ep->index;
 	unsigned long flags;
@@ -2658,7 +2635,7 @@ static int s3c_hsotg_ep_dequeue(struct usb_ep *ep, struct usb_request *req)
 {
 	struct s3c_hsotg_req *hs_req = our_req(req);
 	struct s3c_hsotg_ep *hs_ep = our_ep(ep);
-	struct s3c_hsotg *hs = hs_ep->parent;
+	struct dwc2_hsotg *hs = hs_ep->parent;
 	unsigned long flags;
 
 	dev_dbg(hs->dev, "ep_dequeue(%p,%p)\n", ep, req);
@@ -2684,7 +2661,7 @@ static int s3c_hsotg_ep_dequeue(struct usb_ep *ep, struct usb_request *req)
 static int s3c_hsotg_ep_sethalt(struct usb_ep *ep, int value)
 {
 	struct s3c_hsotg_ep *hs_ep = our_ep(ep);
-	struct s3c_hsotg *hs = hs_ep->parent;
+	struct dwc2_hsotg *hs = hs_ep->parent;
 	int index = hs_ep->index;
 	u32 epreg;
 	u32 epctl;
@@ -2748,7 +2725,7 @@ static int s3c_hsotg_ep_sethalt(struct usb_ep *ep, int value)
 static int s3c_hsotg_ep_sethalt_lock(struct usb_ep *ep, int value)
 {
 	struct s3c_hsotg_ep *hs_ep = our_ep(ep);
-	struct s3c_hsotg *hs = hs_ep->parent;
+	struct dwc2_hsotg *hs = hs_ep->parent;
 	unsigned long flags = 0;
 	int ret = 0;
 
@@ -2777,7 +2754,7 @@ static struct usb_ep_ops s3c_hsotg_ep_ops = {
  * A wrapper for platform code responsible for controlling
  * low-level USB code
  */
-static void s3c_hsotg_phy_enable(struct s3c_hsotg *hsotg)
+static void s3c_hsotg_phy_enable(struct dwc2_hsotg *hsotg)
 {
 	struct platform_device *pdev = to_platform_device(hsotg->dev);
 
@@ -2800,7 +2777,7 @@ static void s3c_hsotg_phy_enable(struct s3c_hsotg *hsotg)
  * A wrapper for platform code responsible for controlling
  * low-level USB code
  */
-static void s3c_hsotg_phy_disable(struct s3c_hsotg *hsotg)
+static void s3c_hsotg_phy_disable(struct dwc2_hsotg *hsotg)
 {
 	struct platform_device *pdev = to_platform_device(hsotg->dev);
 
@@ -2818,7 +2795,7 @@ static void s3c_hsotg_phy_disable(struct s3c_hsotg *hsotg)
  * s3c_hsotg_init - initalize the usb core
  * @hsotg: The driver state
  */
-static void s3c_hsotg_init(struct s3c_hsotg *hsotg)
+static void s3c_hsotg_init(struct dwc2_hsotg *hsotg)
 {
 	/* unmask subset of endpoint interrupts */
 
@@ -2868,7 +2845,8 @@ static void s3c_hsotg_init(struct s3c_hsotg *hsotg)
 static int s3c_hsotg_udc_start(struct usb_gadget *gadget,
 			   struct usb_gadget_driver *driver)
 {
-	struct s3c_hsotg *hsotg = to_hsotg(gadget);
+	struct dwc2_hsotg *hsotg = to_hsotg(gadget);
+	unsigned long flags;
 	int ret;
 
 	if (!hsotg) {
@@ -2889,6 +2867,7 @@ static int s3c_hsotg_udc_start(struct usb_gadget *gadget,
 		return -EINVAL;
 	}
 
+	mutex_lock(&hsotg->init_mutex);
 	WARN_ON(hsotg->driver);
 
 	driver->driver.bus = NULL;
@@ -2905,11 +2884,22 @@ static int s3c_hsotg_udc_start(struct usb_gadget *gadget,
 		goto err;
 	}
 
-	hsotg->last_rst = jiffies;
+	s3c_hsotg_phy_enable(hsotg);
+
+	spin_lock_irqsave(&hsotg->lock, flags);
+	s3c_hsotg_init(hsotg);
+	s3c_hsotg_core_init_disconnected(hsotg);
+	hsotg->enabled = 0;
+	spin_unlock_irqrestore(&hsotg->lock, flags);
+
 	dev_info(hsotg->dev, "bound driver %s\n", driver->driver.name);
+
+	mutex_unlock(&hsotg->init_mutex);
+
 	return 0;
 
 err:
+	mutex_unlock(&hsotg->init_mutex);
 	hsotg->driver = NULL;
 	return ret;
 }
@@ -2921,16 +2911,17 @@ err:
  *
  * 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)
+static int s3c_hsotg_udc_stop(struct usb_gadget *gadget)
 {
-	struct s3c_hsotg *hsotg = to_hsotg(gadget);
+	struct dwc2_hsotg *hsotg = to_hsotg(gadget);
 	unsigned long flags = 0;
 	int ep;
 
 	if (!hsotg)
 		return -ENODEV;
 
+	mutex_lock(&hsotg->init_mutex);
+
 	/* all endpoints should be shutdown */
 	for (ep = 1; ep < hsotg->num_of_eps; ep++)
 		s3c_hsotg_ep_disable(&hsotg->eps[ep].ep);
@@ -2939,13 +2930,18 @@ static int s3c_hsotg_udc_stop(struct usb_gadget *gadget,
 
 	hsotg->driver = NULL;
 	hsotg->gadget.speed = USB_SPEED_UNKNOWN;
+	hsotg->enabled = 0;
 
 	spin_unlock_irqrestore(&hsotg->lock, flags);
 
+	s3c_hsotg_phy_disable(hsotg);
+
 	regulator_bulk_disable(ARRAY_SIZE(hsotg->supplies), hsotg->supplies);
 
 	clk_disable(hsotg->clk);
 
+	mutex_unlock(&hsotg->init_mutex);
+
 	return 0;
 }
 
@@ -2969,23 +2965,26 @@ static int s3c_hsotg_gadget_getframe(struct usb_gadget *gadget)
  */
 static int s3c_hsotg_pullup(struct usb_gadget *gadget, int is_on)
 {
-	struct s3c_hsotg *hsotg = to_hsotg(gadget);
+	struct dwc2_hsotg *hsotg = to_hsotg(gadget);
 	unsigned long flags = 0;
 
 	dev_dbg(hsotg->dev, "%s: is_on: %d\n", __func__, is_on);
 
+	mutex_lock(&hsotg->init_mutex);
 	spin_lock_irqsave(&hsotg->lock, flags);
 	if (is_on) {
-		s3c_hsotg_phy_enable(hsotg);
 		clk_enable(hsotg->clk);
-		s3c_hsotg_core_init(hsotg);
+		hsotg->enabled = 1;
+		s3c_hsotg_core_connect(hsotg);
 	} else {
+		s3c_hsotg_core_disconnect(hsotg);
+		hsotg->enabled = 0;
 		clk_disable(hsotg->clk);
-		s3c_hsotg_phy_disable(hsotg);
 	}
 
 	hsotg->gadget.speed = USB_SPEED_UNKNOWN;
 	spin_unlock_irqrestore(&hsotg->lock, flags);
+	mutex_unlock(&hsotg->init_mutex);
 
 	return 0;
 }
@@ -3007,7 +3006,7 @@ static const struct usb_gadget_ops s3c_hsotg_gadget_ops = {
  * creation) to give to the gadget driver. Setup the endpoint name, any
  * direction information and other state that may be required.
  */
-static void s3c_hsotg_initep(struct s3c_hsotg *hsotg,
+static void s3c_hsotg_initep(struct dwc2_hsotg *hsotg,
 				       struct s3c_hsotg_ep *hs_ep,
 				       int epnum)
 {
@@ -3056,7 +3055,7 @@ static void s3c_hsotg_initep(struct s3c_hsotg *hsotg,
  *
  * Read the USB core HW configuration registers
  */
-static void s3c_hsotg_hw_cfg(struct s3c_hsotg *hsotg)
+static void s3c_hsotg_hw_cfg(struct dwc2_hsotg *hsotg)
 {
 	u32 cfg2, cfg3, cfg4;
 	/* check hardware configuration */
@@ -3080,7 +3079,7 @@ static void s3c_hsotg_hw_cfg(struct s3c_hsotg *hsotg)
  * s3c_hsotg_dump - dump state of the udc
  * @param: The device state
  */
-static void s3c_hsotg_dump(struct s3c_hsotg *hsotg)
+static void s3c_hsotg_dump(struct dwc2_hsotg *hsotg)
 {
 #ifdef DEBUG
 	struct device *dev = hsotg->dev;
@@ -3139,7 +3138,7 @@ static void s3c_hsotg_dump(struct s3c_hsotg *hsotg)
  */
 static int state_show(struct seq_file *seq, void *v)
 {
-	struct s3c_hsotg *hsotg = seq->private;
+	struct dwc2_hsotg *hsotg = seq->private;
 	void __iomem *regs = hsotg->regs;
 	int idx;
 
@@ -3209,7 +3208,7 @@ static const struct file_operations state_fops = {
  */
 static int fifo_show(struct seq_file *seq, void *v)
 {
-	struct s3c_hsotg *hsotg = seq->private;
+	struct dwc2_hsotg *hsotg = seq->private;
 	void __iomem *regs = hsotg->regs;
 	u32 val;
 	int idx;
@@ -3265,7 +3264,7 @@ static const char *decode_direction(int is_in)
 static int ep_show(struct seq_file *seq, void *v)
 {
 	struct s3c_hsotg_ep *ep = seq->private;
-	struct s3c_hsotg *hsotg = ep->parent;
+	struct dwc2_hsotg *hsotg = ep->parent;
 	struct s3c_hsotg_req *req;
 	void __iomem *regs = hsotg->regs;
 	int index = ep->index;
@@ -3342,7 +3341,7 @@ static const struct file_operations ep_fops = {
  * with the same name as the device itself, in case we end up
  * with multiple blocks in future systems.
  */
-static void s3c_hsotg_create_debug(struct s3c_hsotg *hsotg)
+static void s3c_hsotg_create_debug(struct dwc2_hsotg *hsotg)
 {
 	struct dentry *root;
 	unsigned epidx;
@@ -3388,7 +3387,7 @@ static void s3c_hsotg_create_debug(struct s3c_hsotg *hsotg)
  *
  * Cleanup (remove) the debugfs files for use on module exit.
  */
-static void s3c_hsotg_delete_debug(struct s3c_hsotg *hsotg)
+static void s3c_hsotg_delete_debug(struct dwc2_hsotg *hsotg)
 {
 	unsigned epidx;
 
@@ -3403,27 +3402,21 @@ static void s3c_hsotg_delete_debug(struct s3c_hsotg *hsotg)
 }
 
 /**
- * s3c_hsotg_probe - probe function for hsotg driver
- * @pdev: The platform information for the driver
+ * dwc2_gadget_init - init function for gadget
+ * @dwc2: The data structure for the DWC2 driver.
+ * @irq: The IRQ number for the controller.
  */
-
-static int s3c_hsotg_probe(struct platform_device *pdev)
+int dwc2_gadget_init(struct dwc2_hsotg *hsotg, int irq)
 {
-	struct s3c_hsotg_plat *plat = dev_get_platdata(&pdev->dev);
+	struct device *dev = hsotg->dev;
+	struct s3c_hsotg_plat *plat = dev->platform_data;
 	struct phy *phy;
 	struct usb_phy *uphy;
-	struct device *dev = &pdev->dev;
 	struct s3c_hsotg_ep *eps;
-	struct s3c_hsotg *hsotg;
-	struct resource *res;
 	int epnum;
 	int ret;
 	int i;
 
-	hsotg = devm_kzalloc(&pdev->dev, sizeof(struct s3c_hsotg), GFP_KERNEL);
-	if (!hsotg)
-		return -ENOMEM;
-
 	/* Set default UTMI width */
 	hsotg->phyif = GUSBCFG_PHYIF16;
 
@@ -3431,14 +3424,14 @@ static int s3c_hsotg_probe(struct platform_device *pdev)
 	 * Attempt to find a generic PHY, then look for an old style
 	 * USB PHY, finally fall back to pdata
 	 */
-	phy = devm_phy_get(&pdev->dev, "usb2-phy");
+	phy = devm_phy_get(dev, "usb2-phy");
 	if (IS_ERR(phy)) {
 		uphy = devm_usb_get_phy(dev, USB_PHY_TYPE_USB2);
 		if (IS_ERR(uphy)) {
 			/* Fallback for pdata */
-			plat = dev_get_platdata(&pdev->dev);
+			plat = dev_get_platdata(dev);
 			if (!plat) {
-				dev_err(&pdev->dev,
+				dev_err(dev,
 				"no platform data or transceiver defined\n");
 				return -EPROBE_DEFER;
 			}
@@ -3455,43 +3448,24 @@ static int s3c_hsotg_probe(struct platform_device *pdev)
 			hsotg->phyif = GUSBCFG_PHYIF8;
 	}
 
-	hsotg->dev = dev;
-
-	hsotg->clk = devm_clk_get(&pdev->dev, "otg");
+	hsotg->clk = devm_clk_get(dev, "otg");
 	if (IS_ERR(hsotg->clk)) {
-		dev_err(dev, "cannot get otg clock\n");
-		return PTR_ERR(hsotg->clk);
-	}
-
-	platform_set_drvdata(pdev, hsotg);
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-
-	hsotg->regs = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(hsotg->regs)) {
-		ret = PTR_ERR(hsotg->regs);
-		goto err_clk;
+		hsotg->clk = NULL;
+		dev_dbg(dev, "cannot get otg clock\n");
 	}
 
-	ret = platform_get_irq(pdev, 0);
-	if (ret < 0) {
-		dev_err(dev, "cannot find IRQ\n");
-		goto err_clk;
-	}
-
-	spin_lock_init(&hsotg->lock);
-
-	hsotg->irq = ret;
-
-	dev_info(dev, "regs %p, irq %d\n", hsotg->regs, hsotg->irq);
-
 	hsotg->gadget.max_speed = USB_SPEED_HIGH;
 	hsotg->gadget.ops = &s3c_hsotg_gadget_ops;
 	hsotg->gadget.name = dev_name(dev);
 
 	/* reset the system */
 
-	clk_prepare_enable(hsotg->clk);
+	ret = clk_prepare_enable(hsotg->clk);
+	if (ret) {
+		dev_err(dev, "failed to enable otg clk\n");
+		goto err_clk;
+	}
+
 
 	/* regulators */
 
@@ -3509,7 +3483,7 @@ static int s3c_hsotg_probe(struct platform_device *pdev)
 				    hsotg->supplies);
 
 	if (ret) {
-		dev_err(hsotg->dev, "failed to enable supplies: %d\n", ret);
+		dev_err(dev, "failed to enable supplies: %d\n", ret);
 		goto err_supplies;
 	}
 
@@ -3520,14 +3494,14 @@ static int s3c_hsotg_probe(struct platform_device *pdev)
 	s3c_hsotg_hw_cfg(hsotg);
 	s3c_hsotg_init(hsotg);
 
-	ret = devm_request_irq(&pdev->dev, hsotg->irq, s3c_hsotg_irq, 0,
-				dev_name(dev), hsotg);
+	ret = devm_request_irq(hsotg->dev, irq, s3c_hsotg_irq, IRQF_SHARED,
+				dev_name(hsotg->dev), hsotg);
 	if (ret < 0) {
 		s3c_hsotg_phy_disable(hsotg);
 		clk_disable_unprepare(hsotg->clk);
 		regulator_bulk_disable(ARRAY_SIZE(hsotg->supplies),
 				       hsotg->supplies);
-		dev_err(dev, "cannot claim IRQ\n");
+		dev_err(dev, "cannot claim IRQ for gadget\n");
 		goto err_clk;
 	}
 
@@ -3573,11 +3547,11 @@ static int s3c_hsotg_probe(struct platform_device *pdev)
 	ret = regulator_bulk_disable(ARRAY_SIZE(hsotg->supplies),
 				    hsotg->supplies);
 	if (ret) {
-		dev_err(hsotg->dev, "failed to disable supplies: %d\n", ret);
+		dev_err(dev, "failed to disable supplies: %d\n", ret);
 		goto err_ep_mem;
 	}
 
-	ret = usb_add_gadget_udc(&pdev->dev, &hsotg->gadget);
+	ret = usb_add_gadget_udc(dev, &hsotg->gadget);
 	if (ret)
 		goto err_ep_mem;
 
@@ -3596,47 +3570,44 @@ err_clk:
 
 	return ret;
 }
+EXPORT_SYMBOL_GPL(dwc2_gadget_init);
 
 /**
  * s3c_hsotg_remove - remove function for hsotg driver
  * @pdev: The platform information for the driver
  */
-static int s3c_hsotg_remove(struct platform_device *pdev)
+int s3c_hsotg_remove(struct dwc2_hsotg *hsotg)
 {
-	struct s3c_hsotg *hsotg = platform_get_drvdata(pdev);
-
 	usb_del_gadget_udc(&hsotg->gadget);
-
 	s3c_hsotg_delete_debug(hsotg);
-
-	if (hsotg->driver) {
-		/* should have been done already by driver model core */
-		usb_gadget_unregister_driver(hsotg->driver);
-	}
-
 	clk_disable_unprepare(hsotg->clk);
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(s3c_hsotg_remove);
 
-static int s3c_hsotg_suspend(struct platform_device *pdev, pm_message_t state)
+int s3c_hsotg_suspend(struct dwc2_hsotg *hsotg)
 {
-	struct s3c_hsotg *hsotg = platform_get_drvdata(pdev);
 	unsigned long flags;
 	int ret = 0;
 
-	if (hsotg->driver)
+	mutex_lock(&hsotg->init_mutex);
+
+	if (hsotg->driver) {
+		int ep;
+
 		dev_info(hsotg->dev, "suspending usb gadget %s\n",
 			 hsotg->driver->driver.name);
 
-	spin_lock_irqsave(&hsotg->lock, flags);
-	s3c_hsotg_disconnect(hsotg);
-	s3c_hsotg_phy_disable(hsotg);
-	hsotg->gadget.speed = USB_SPEED_UNKNOWN;
-	spin_unlock_irqrestore(&hsotg->lock, flags);
+		spin_lock_irqsave(&hsotg->lock, flags);
+		if (hsotg->enabled)
+			s3c_hsotg_core_disconnect(hsotg);
+		s3c_hsotg_disconnect(hsotg);
+		hsotg->gadget.speed = USB_SPEED_UNKNOWN;
+		spin_unlock_irqrestore(&hsotg->lock, flags);
+
+		s3c_hsotg_phy_disable(hsotg);
 
-	if (hsotg->driver) {
-		int ep;
 		for (ep = 0; ep < hsotg->num_of_eps; ep++)
 			s3c_hsotg_ep_disable(&hsotg->eps[ep].ep);
 
@@ -3645,57 +3616,37 @@ static int s3c_hsotg_suspend(struct platform_device *pdev, pm_message_t state)
 		clk_disable(hsotg->clk);
 	}
 
+	mutex_unlock(&hsotg->init_mutex);
+
 	return ret;
 }
+EXPORT_SYMBOL_GPL(s3c_hsotg_suspend);
 
-static int s3c_hsotg_resume(struct platform_device *pdev)
+int s3c_hsotg_resume(struct dwc2_hsotg *hsotg)
 {
-	struct s3c_hsotg *hsotg = platform_get_drvdata(pdev);
 	unsigned long flags;
 	int ret = 0;
 
+	mutex_lock(&hsotg->init_mutex);
+
 	if (hsotg->driver) {
 		dev_info(hsotg->dev, "resuming usb gadget %s\n",
 			 hsotg->driver->driver.name);
 
 		clk_enable(hsotg->clk);
 		ret = regulator_bulk_enable(ARRAY_SIZE(hsotg->supplies),
-				      hsotg->supplies);
-	}
+					    hsotg->supplies);
 
-	spin_lock_irqsave(&hsotg->lock, flags);
-	hsotg->last_rst = jiffies;
-	s3c_hsotg_phy_enable(hsotg);
-	s3c_hsotg_core_init(hsotg);
-	spin_unlock_irqrestore(&hsotg->lock, flags);
+		s3c_hsotg_phy_enable(hsotg);
+
+		spin_lock_irqsave(&hsotg->lock, flags);
+		s3c_hsotg_core_init_disconnected(hsotg);
+		if (hsotg->enabled)
+			s3c_hsotg_core_connect(hsotg);
+		spin_unlock_irqrestore(&hsotg->lock, flags);
+	}
+	mutex_unlock(&hsotg->init_mutex);
 
 	return ret;
 }
-
-#ifdef CONFIG_OF
-static const struct of_device_id s3c_hsotg_of_ids[] = {
-	{ .compatible = "samsung,s3c6400-hsotg", },
-	{ .compatible = "snps,dwc2", },
-	{ /* sentinel */ }
-};
-MODULE_DEVICE_TABLE(of, s3c_hsotg_of_ids);
-#endif
-
-static struct platform_driver s3c_hsotg_driver = {
-	.driver		= {
-		.name	= "s3c-hsotg",
-		.owner	= THIS_MODULE,
-		.of_match_table = of_match_ptr(s3c_hsotg_of_ids),
-	},
-	.probe		= s3c_hsotg_probe,
-	.remove		= s3c_hsotg_remove,
-	.suspend	= s3c_hsotg_suspend,
-	.resume		= s3c_hsotg_resume,
-};
-
-module_platform_driver(s3c_hsotg_driver);
-
-MODULE_DESCRIPTION("Samsung S3C USB High-speed/OtG device");
-MODULE_AUTHOR("Ben Dooks <ben@simtec.co.uk>");
-MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:s3c-hsotg");
+EXPORT_SYMBOL_GPL(s3c_hsotg_resume);
diff --git a/drivers/usb/dwc2/hcd.c b/drivers/usb/dwc2/hcd.c
index 0a0e6f0ad15f..a0cd9db6f4cd 100644
--- a/drivers/usb/dwc2/hcd.c
+++ b/drivers/usb/dwc2/hcd.c
@@ -1371,6 +1371,8 @@ static void dwc2_conn_id_status_change(struct work_struct *work)
 		hsotg->op_state = OTG_STATE_B_PERIPHERAL;
 		dwc2_core_init(hsotg, false, -1);
 		dwc2_enable_global_interrupts(hsotg);
+		s3c_hsotg_core_init_disconnected(hsotg);
+		s3c_hsotg_core_connect(hsotg);
 	} else {
 		/* A-Device connector (Host Mode) */
 		dev_dbg(hsotg->dev, "connId A\n");
@@ -1471,6 +1473,30 @@ static void dwc2_port_suspend(struct dwc2_hsotg *hsotg, u16 windex)
 	}
 }
 
+static void dwc2_port_resume(struct dwc2_hsotg *hsotg)
+{
+	u32 hprt0;
+
+	/* After clear the Stop PHY clock bit, we should wait for a moment
+	 * for PLL work stable with clock output.
+	 */
+	writel(0, hsotg->regs + PCGCTL);
+	usleep_range(2000, 4000);
+
+	hprt0 = dwc2_read_hprt0(hsotg);
+	hprt0 |= HPRT0_RES;
+	writel(hprt0, hsotg->regs + HPRT0);
+	hprt0 &= ~HPRT0_SUSP;
+	/* according to USB2.0 Spec 7.1.7.7, the host must send the resume
+	 * signal for at least 20ms
+	 */
+	usleep_range(20000, 25000);
+
+	hprt0 &= ~HPRT0_RES;
+	writel(hprt0, hsotg->regs + HPRT0);
+	hsotg->lx_state = DWC2_L0;
+}
+
 /* Handles hub class-specific requests */
 static int dwc2_hcd_hub_control(struct dwc2_hsotg *hsotg, u16 typereq,
 				u16 wvalue, u16 windex, char *buf, u16 wlength)
@@ -1516,17 +1542,7 @@ static int dwc2_hcd_hub_control(struct dwc2_hsotg *hsotg, u16 typereq,
 		case USB_PORT_FEAT_SUSPEND:
 			dev_dbg(hsotg->dev,
 				"ClearPortFeature USB_PORT_FEAT_SUSPEND\n");
-			writel(0, hsotg->regs + PCGCTL);
-			usleep_range(20000, 40000);
-
-			hprt0 = dwc2_read_hprt0(hsotg);
-			hprt0 |= HPRT0_RES;
-			writel(hprt0, hsotg->regs + HPRT0);
-			hprt0 &= ~HPRT0_SUSP;
-			usleep_range(100000, 150000);
-
-			hprt0 &= ~HPRT0_RES;
-			writel(hprt0, hsotg->regs + HPRT0);
+			dwc2_port_resume(hsotg);
 			break;
 
 		case USB_PORT_FEAT_POWER:
@@ -2299,6 +2315,55 @@ static void _dwc2_hcd_stop(struct usb_hcd *hcd)
 	usleep_range(1000, 3000);
 }
 
+static int _dwc2_hcd_suspend(struct usb_hcd *hcd)
+{
+	struct dwc2_hsotg *hsotg = dwc2_hcd_to_hsotg(hcd);
+	u32 hprt0;
+
+	if (!((hsotg->op_state == OTG_STATE_B_HOST) ||
+		(hsotg->op_state == OTG_STATE_A_HOST)))
+		return 0;
+
+	/* TODO: We get into suspend from 'on' state, maybe we need to do
+	 * something if we get here from DWC2_L1(LPM sleep) state one day.
+	 */
+	if (hsotg->lx_state != DWC2_L0)
+		return 0;
+
+	hprt0 = dwc2_read_hprt0(hsotg);
+	if (hprt0 & HPRT0_CONNSTS) {
+		dwc2_port_suspend(hsotg, 1);
+	} else {
+		u32 pcgctl = readl(hsotg->regs + PCGCTL);
+
+		pcgctl |= PCGCTL_STOPPCLK;
+		writel(pcgctl, hsotg->regs + PCGCTL);
+	}
+
+	return 0;
+}
+
+static int _dwc2_hcd_resume(struct usb_hcd *hcd)
+{
+	struct dwc2_hsotg *hsotg = dwc2_hcd_to_hsotg(hcd);
+	u32 hprt0;
+
+	if (!((hsotg->op_state == OTG_STATE_B_HOST) ||
+		(hsotg->op_state == OTG_STATE_A_HOST)))
+		return 0;
+
+	if (hsotg->lx_state != DWC2_L2)
+		return 0;
+
+	hprt0 = dwc2_read_hprt0(hsotg);
+	if ((hprt0 & HPRT0_CONNSTS) && (hprt0 & HPRT0_SUSP))
+		dwc2_port_resume(hsotg);
+	else
+		writel(0, hsotg->regs + PCGCTL);
+
+	return 0;
+}
+
 /* Returns the current frame number */
 static int _dwc2_hcd_get_frame_number(struct usb_hcd *hcd)
 {
@@ -2669,6 +2734,9 @@ static struct hc_driver dwc2_hc_driver = {
 	.hub_status_data = _dwc2_hcd_hub_status_data,
 	.hub_control = _dwc2_hcd_hub_control,
 	.clear_tt_buffer_complete = _dwc2_hcd_clear_tt_buffer_complete,
+
+	.bus_suspend = _dwc2_hcd_suspend,
+	.bus_resume = _dwc2_hcd_resume,
 };
 
 /*
@@ -2778,6 +2846,9 @@ int dwc2_hcd_init(struct dwc2_hsotg *hsotg, int irq,
 	int i, num_channels;
 	int retval;
 
+	if (usb_disabled())
+		return -ENODEV;
+
 	dev_dbg(hsotg->dev, "DWC OTG HCD INIT\n");
 
 	/* Detect config values from hardware */
@@ -2839,7 +2910,6 @@ int dwc2_hcd_init(struct dwc2_hsotg *hsotg, int irq,
 
 	hcd->has_tt = 1;
 
-	spin_lock_init(&hsotg->lock);
 	((struct wrapper_priv_data *) &hcd->hcd_priv)->hsotg = hsotg;
 	hsotg->priv = hcd;
 
diff --git a/drivers/usb/dwc2/hcd.h b/drivers/usb/dwc2/hcd.h
index a12bb1538666..e69a843d8928 100644
--- a/drivers/usb/dwc2/hcd.h
+++ b/drivers/usb/dwc2/hcd.h
@@ -668,9 +668,6 @@ extern irqreturn_t dwc2_handle_hcd_intr(struct dwc2_hsotg *hsotg);
  */
 extern void dwc2_hcd_stop(struct dwc2_hsotg *hsotg);
 
-extern void dwc2_hcd_start(struct dwc2_hsotg *hsotg);
-extern void dwc2_hcd_disconnect(struct dwc2_hsotg *hsotg);
-
 /**
  * dwc2_hcd_is_b_host() - Returns 1 if core currently is acting as B host,
  * and 0 otherwise
@@ -680,13 +677,6 @@ extern void dwc2_hcd_disconnect(struct dwc2_hsotg *hsotg);
 extern int dwc2_hcd_is_b_host(struct dwc2_hsotg *hsotg);
 
 /**
- * dwc2_hcd_get_frame_number() - Returns current frame number
- *
- * @hsotg: The DWC2 HCD
- */
-extern int dwc2_hcd_get_frame_number(struct dwc2_hsotg *hsotg);
-
-/**
  * dwc2_hcd_dump_state() - Dumps hsotg state
  *
  * @hsotg: The DWC2 HCD
diff --git a/drivers/usb/dwc2/pci.c b/drivers/usb/dwc2/pci.c
index c291fca5d21f..a4e724b0a62e 100644
--- a/drivers/usb/dwc2/pci.c
+++ b/drivers/usb/dwc2/pci.c
@@ -141,6 +141,13 @@ static int dwc2_driver_probe(struct pci_dev *dev,
 
 	pci_set_master(dev);
 
+	retval = devm_request_irq(hsotg->dev, dev->irq,
+				  dwc2_handle_common_intr, IRQF_SHARED,
+				  dev_name(hsotg->dev), hsotg);
+	if (retval)
+		return retval;
+
+	spin_lock_init(&hsotg->lock);
 	retval = dwc2_hcd_init(hsotg, dev->irq, &dwc2_module_params);
 	if (retval) {
 		pci_disable_device(dev);
diff --git a/drivers/usb/dwc2/platform.c b/drivers/usb/dwc2/platform.c
index 121dbdafc06b..6a795aa2ff05 100644
--- a/drivers/usb/dwc2/platform.c
+++ b/drivers/usb/dwc2/platform.c
@@ -40,6 +40,7 @@
 #include <linux/device.h>
 #include <linux/dma-mapping.h>
 #include <linux/of_device.h>
+#include <linux/mutex.h>
 #include <linux/platform_device.h>
 
 #include <linux/usb/of.h>
@@ -121,6 +122,7 @@ static int dwc2_driver_remove(struct platform_device *dev)
 	struct dwc2_hsotg *hsotg = platform_get_drvdata(dev);
 
 	dwc2_hcd_remove(hsotg);
+	s3c_hsotg_remove(hsotg);
 
 	return 0;
 }
@@ -129,6 +131,7 @@ static const struct of_device_id dwc2_of_match_table[] = {
 	{ .compatible = "brcm,bcm2835-usb", .data = &params_bcm2835 },
 	{ .compatible = "rockchip,rk3066-usb", .data = &params_rk3066 },
 	{ .compatible = "snps,dwc2", .data = NULL },
+	{ .compatible = "samsung,s3c6400-hsotg", .data = NULL},
 	{},
 };
 MODULE_DEVICE_TABLE(of, dwc2_of_match_table);
@@ -155,9 +158,6 @@ static int dwc2_driver_probe(struct platform_device *dev)
 	int retval;
 	int irq;
 
-	if (usb_disabled())
-		return -ENODEV;
-
 	match = of_match_device(dwc2_of_match_table, &dev->dev);
 	if (match && match->data) {
 		params = match->data;
@@ -194,6 +194,14 @@ static int dwc2_driver_probe(struct platform_device *dev)
 		return irq;
 	}
 
+	dev_dbg(hsotg->dev, "registering common handler for irq%d\n",
+		irq);
+	retval = devm_request_irq(hsotg->dev, irq,
+				  dwc2_handle_common_intr, IRQF_SHARED,
+				  dev_name(hsotg->dev), hsotg);
+	if (retval)
+		return retval;
+
 	res = platform_get_resource(dev, IORESOURCE_MEM, 0);
 	hsotg->regs = devm_ioremap_resource(&dev->dev, res);
 	if (IS_ERR(hsotg->regs))
@@ -204,6 +212,11 @@ static int dwc2_driver_probe(struct platform_device *dev)
 
 	hsotg->dr_mode = of_usb_get_dr_mode(dev->dev.of_node);
 
+	spin_lock_init(&hsotg->lock);
+	mutex_init(&hsotg->init_mutex);
+	retval = dwc2_gadget_init(hsotg, irq);
+	if (retval)
+		return retval;
 	retval = dwc2_hcd_init(hsotg, irq, params);
 	if (retval)
 		return retval;
@@ -213,10 +226,35 @@ static int dwc2_driver_probe(struct platform_device *dev)
 	return retval;
 }
 
+static int __maybe_unused dwc2_suspend(struct device *dev)
+{
+	struct dwc2_hsotg *dwc2 = dev_get_drvdata(dev);
+	int ret = 0;
+
+	if (dwc2_is_device_mode(dwc2))
+		ret = s3c_hsotg_suspend(dwc2);
+	return ret;
+}
+
+static int __maybe_unused dwc2_resume(struct device *dev)
+{
+	struct dwc2_hsotg *dwc2 = dev_get_drvdata(dev);
+	int ret = 0;
+
+	if (dwc2_is_device_mode(dwc2))
+		ret = s3c_hsotg_resume(dwc2);
+	return ret;
+}
+
+static const struct dev_pm_ops dwc2_dev_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(dwc2_suspend, dwc2_resume)
+};
+
 static struct platform_driver dwc2_platform_driver = {
 	.driver = {
 		.name = dwc2_driver_name,
 		.of_match_table = dwc2_of_match_table,
+		.pm = &dwc2_dev_pm_ops,
 	},
 	.probe = dwc2_driver_probe,
 	.remove = dwc2_driver_remove,
diff --git a/drivers/usb/dwc3/Kconfig b/drivers/usb/dwc3/Kconfig
index f4e5cc60db0b..58b5b2cde4c5 100644
--- a/drivers/usb/dwc3/Kconfig
+++ b/drivers/usb/dwc3/Kconfig
@@ -55,7 +55,7 @@ config USB_DWC3_OMAP
 
 config USB_DWC3_EXYNOS
 	tristate "Samsung Exynos Platform"
-	depends on ARCH_EXYNOS || COMPILE_TEST
+	depends on ARCH_EXYNOS && OF || COMPILE_TEST
 	default USB_DWC3
 	help
 	  Recent Exynos5 SoCs ship with one DesignWare Core USB3 IP inside,
diff --git a/drivers/usb/dwc3/core.c b/drivers/usb/dwc3/core.c
index b0f4d52b7f04..25ddc39efad8 100644
--- a/drivers/usb/dwc3/core.c
+++ b/drivers/usb/dwc3/core.c
@@ -19,6 +19,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <linux/version.h>
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/slab.h>
@@ -32,6 +33,7 @@
 #include <linux/delay.h>
 #include <linux/dma-mapping.h>
 #include <linux/of.h>
+#include <linux/acpi.h>
 
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
@@ -363,6 +365,72 @@ static void dwc3_cache_hwparams(struct dwc3 *dwc)
 }
 
 /**
+ * dwc3_phy_setup - Configure USB PHY Interface of DWC3 Core
+ * @dwc: Pointer to our controller context structure
+ */
+static void dwc3_phy_setup(struct dwc3 *dwc)
+{
+	u32 reg;
+
+	reg = dwc3_readl(dwc->regs, DWC3_GUSB3PIPECTL(0));
+
+	/*
+	 * Above 1.94a, it is recommended to set DWC3_GUSB3PIPECTL_SUSPHY
+	 * to '0' during coreConsultant configuration. So default value
+	 * will be '0' when the core is reset. Application needs to set it
+	 * to '1' after the core initialization is completed.
+	 */
+	if (dwc->revision > DWC3_REVISION_194A)
+		reg |= DWC3_GUSB3PIPECTL_SUSPHY;
+
+	if (dwc->u2ss_inp3_quirk)
+		reg |= DWC3_GUSB3PIPECTL_U2SSINP3OK;
+
+	if (dwc->req_p1p2p3_quirk)
+		reg |= DWC3_GUSB3PIPECTL_REQP1P2P3;
+
+	if (dwc->del_p1p2p3_quirk)
+		reg |= DWC3_GUSB3PIPECTL_DEP1P2P3_EN;
+
+	if (dwc->del_phy_power_chg_quirk)
+		reg |= DWC3_GUSB3PIPECTL_DEPOCHANGE;
+
+	if (dwc->lfps_filter_quirk)
+		reg |= DWC3_GUSB3PIPECTL_LFPSFILT;
+
+	if (dwc->rx_detect_poll_quirk)
+		reg |= DWC3_GUSB3PIPECTL_RX_DETOPOLL;
+
+	if (dwc->tx_de_emphasis_quirk)
+		reg |= DWC3_GUSB3PIPECTL_TX_DEEPH(dwc->tx_de_emphasis);
+
+	if (dwc->dis_u3_susphy_quirk)
+		reg &= ~DWC3_GUSB3PIPECTL_SUSPHY;
+
+	dwc3_writel(dwc->regs, DWC3_GUSB3PIPECTL(0), reg);
+
+	mdelay(100);
+
+	reg = dwc3_readl(dwc->regs, DWC3_GUSB2PHYCFG(0));
+
+	/*
+	 * Above 1.94a, it is recommended to set DWC3_GUSB2PHYCFG_SUSPHY to
+	 * '0' during coreConsultant configuration. So default value will
+	 * be '0' when the core is reset. Application needs to set it to
+	 * '1' after the core initialization is completed.
+	 */
+	if (dwc->revision > DWC3_REVISION_194A)
+		reg |= DWC3_GUSB2PHYCFG_SUSPHY;
+
+	if (dwc->dis_u2_susphy_quirk)
+		reg &= ~DWC3_GUSB2PHYCFG_SUSPHY;
+
+	dwc3_writel(dwc->regs, DWC3_GUSB2PHYCFG(0), reg);
+
+	mdelay(100);
+}
+
+/**
  * dwc3_core_init - Low-level initialization of DWC3 Core
  * @dwc: Pointer to our controller context structure
  *
@@ -384,6 +452,12 @@ static int dwc3_core_init(struct dwc3 *dwc)
 	}
 	dwc->revision = reg;
 
+	/*
+	 * Write Linux Version Code to our GUID register so it's easy to figure
+	 * out which kernel version a bug was found.
+	 */
+	dwc3_writel(dwc->regs, DWC3_GUID, LINUX_VERSION_CODE);
+
 	/* Handle USB2.0-only core configuration */
 	if (DWC3_GHWPARAMS3_SSPHY_IFC(dwc->hwparams.hwparams3) ==
 			DWC3_GHWPARAMS3_SSPHY_IFC_DIS) {
@@ -414,7 +488,6 @@ static int dwc3_core_init(struct dwc3 *dwc)
 
 	reg = dwc3_readl(dwc->regs, DWC3_GCTL);
 	reg &= ~DWC3_GCTL_SCALEDOWN_MASK;
-	reg &= ~DWC3_GCTL_DISSCRAMBLE;
 
 	switch (DWC3_GHWPARAMS1_EN_PWROPT(dwc->hwparams.hwparams1)) {
 	case DWC3_GHWPARAMS1_EN_PWROPT_CLK:
@@ -441,11 +514,34 @@ static int dwc3_core_init(struct dwc3 *dwc)
 	case DWC3_GHWPARAMS1_EN_PWROPT_HIB:
 		/* enable hibernation here */
 		dwc->nr_scratch = DWC3_GHWPARAMS4_HIBER_SCRATCHBUFS(hwparams4);
+
+		/*
+		 * REVISIT Enabling this bit so that host-mode hibernation
+		 * will work. Device-mode hibernation is not yet implemented.
+		 */
+		reg |= DWC3_GCTL_GBLHIBERNATIONEN;
 		break;
 	default:
 		dev_dbg(dwc->dev, "No power optimization available\n");
 	}
 
+	/* check if current dwc3 is on simulation board */
+	if (dwc->hwparams.hwparams6 & DWC3_GHWPARAMS6_EN_FPGA) {
+		dev_dbg(dwc->dev, "it is on FPGA board\n");
+		dwc->is_fpga = true;
+	}
+
+	WARN_ONCE(dwc->disable_scramble_quirk && !dwc->is_fpga,
+			"disable_scramble cannot be used on non-FPGA builds\n");
+
+	if (dwc->disable_scramble_quirk && dwc->is_fpga)
+		reg |= DWC3_GCTL_DISSCRAMBLE;
+	else
+		reg &= ~DWC3_GCTL_DISSCRAMBLE;
+
+	if (dwc->u2exit_lfps_quirk)
+		reg |= DWC3_GCTL_U2EXIT_LFPS;
+
 	/*
 	 * WORKAROUND: DWC3 revisions <1.90a have a bug
 	 * where the device can fail to connect at SuperSpeed
@@ -459,6 +555,8 @@ static int dwc3_core_init(struct dwc3 *dwc)
 
 	dwc3_writel(dwc->regs, DWC3_GCTL, reg);
 
+	dwc3_phy_setup(dwc);
+
 	ret = dwc3_alloc_scratch_buffers(dwc);
 	if (ret)
 		goto err1;
@@ -630,6 +728,9 @@ static int dwc3_probe(struct platform_device *pdev)
 	struct device_node	*node = dev->of_node;
 	struct resource		*res;
 	struct dwc3		*dwc;
+	u8			lpm_nyet_threshold;
+	u8			tx_de_emphasis;
+	u8			hird_threshold;
 
 	int			ret;
 
@@ -685,22 +786,96 @@ static int dwc3_probe(struct platform_device *pdev)
 	 */
 	res->start -= DWC3_GLOBALS_REGS_START;
 
+	/* default to highest possible threshold */
+	lpm_nyet_threshold = 0xff;
+
+	/* default to -3.5dB de-emphasis */
+	tx_de_emphasis = 1;
+
+	/*
+	 * default to assert utmi_sleep_n and use maximum allowed HIRD
+	 * threshold value of 0b1100
+	 */
+	hird_threshold = 12;
+
 	if (node) {
 		dwc->maximum_speed = of_usb_get_maximum_speed(node);
-
-		dwc->needs_fifo_resize = of_property_read_bool(node, "tx-fifo-resize");
+		dwc->has_lpm_erratum = of_property_read_bool(node,
+				"snps,has-lpm-erratum");
+		of_property_read_u8(node, "snps,lpm-nyet-threshold",
+				&lpm_nyet_threshold);
+		dwc->is_utmi_l1_suspend = of_property_read_bool(node,
+				"snps,is-utmi-l1-suspend");
+		of_property_read_u8(node, "snps,hird-threshold",
+				&hird_threshold);
+
+		dwc->needs_fifo_resize = of_property_read_bool(node,
+				"tx-fifo-resize");
 		dwc->dr_mode = of_usb_get_dr_mode(node);
+
+		dwc->disable_scramble_quirk = of_property_read_bool(node,
+				"snps,disable_scramble_quirk");
+		dwc->u2exit_lfps_quirk = of_property_read_bool(node,
+				"snps,u2exit_lfps_quirk");
+		dwc->u2ss_inp3_quirk = of_property_read_bool(node,
+				"snps,u2ss_inp3_quirk");
+		dwc->req_p1p2p3_quirk = of_property_read_bool(node,
+				"snps,req_p1p2p3_quirk");
+		dwc->del_p1p2p3_quirk = of_property_read_bool(node,
+				"snps,del_p1p2p3_quirk");
+		dwc->del_phy_power_chg_quirk = of_property_read_bool(node,
+				"snps,del_phy_power_chg_quirk");
+		dwc->lfps_filter_quirk = of_property_read_bool(node,
+				"snps,lfps_filter_quirk");
+		dwc->rx_detect_poll_quirk = of_property_read_bool(node,
+				"snps,rx_detect_poll_quirk");
+		dwc->dis_u3_susphy_quirk = of_property_read_bool(node,
+				"snps,dis_u3_susphy_quirk");
+		dwc->dis_u2_susphy_quirk = of_property_read_bool(node,
+				"snps,dis_u2_susphy_quirk");
+
+		dwc->tx_de_emphasis_quirk = of_property_read_bool(node,
+				"snps,tx_de_emphasis_quirk");
+		of_property_read_u8(node, "snps,tx_de_emphasis",
+				&tx_de_emphasis);
 	} else if (pdata) {
 		dwc->maximum_speed = pdata->maximum_speed;
+		dwc->has_lpm_erratum = pdata->has_lpm_erratum;
+		if (pdata->lpm_nyet_threshold)
+			lpm_nyet_threshold = pdata->lpm_nyet_threshold;
+		dwc->is_utmi_l1_suspend = pdata->is_utmi_l1_suspend;
+		if (pdata->hird_threshold)
+			hird_threshold = pdata->hird_threshold;
 
 		dwc->needs_fifo_resize = pdata->tx_fifo_resize;
 		dwc->dr_mode = pdata->dr_mode;
+
+		dwc->disable_scramble_quirk = pdata->disable_scramble_quirk;
+		dwc->u2exit_lfps_quirk = pdata->u2exit_lfps_quirk;
+		dwc->u2ss_inp3_quirk = pdata->u2ss_inp3_quirk;
+		dwc->req_p1p2p3_quirk = pdata->req_p1p2p3_quirk;
+		dwc->del_p1p2p3_quirk = pdata->del_p1p2p3_quirk;
+		dwc->del_phy_power_chg_quirk = pdata->del_phy_power_chg_quirk;
+		dwc->lfps_filter_quirk = pdata->lfps_filter_quirk;
+		dwc->rx_detect_poll_quirk = pdata->rx_detect_poll_quirk;
+		dwc->dis_u3_susphy_quirk = pdata->dis_u3_susphy_quirk;
+		dwc->dis_u2_susphy_quirk = pdata->dis_u2_susphy_quirk;
+
+		dwc->tx_de_emphasis_quirk = pdata->tx_de_emphasis_quirk;
+		if (pdata->tx_de_emphasis)
+			tx_de_emphasis = pdata->tx_de_emphasis;
 	}
 
 	/* default to superspeed if no maximum_speed passed */
 	if (dwc->maximum_speed == USB_SPEED_UNKNOWN)
 		dwc->maximum_speed = USB_SPEED_SUPER;
 
+	dwc->lpm_nyet_threshold = lpm_nyet_threshold;
+	dwc->tx_de_emphasis = tx_de_emphasis;
+
+	dwc->hird_threshold = hird_threshold
+		| (dwc->is_utmi_l1_suspend << 4);
+
 	ret = dwc3_core_get_phy(dwc);
 	if (ret)
 		return ret;
@@ -708,9 +883,11 @@ static int dwc3_probe(struct platform_device *pdev)
 	spin_lock_init(&dwc->lock);
 	platform_set_drvdata(pdev, dwc);
 
-	dev->dma_mask	= dev->parent->dma_mask;
-	dev->dma_parms	= dev->parent->dma_parms;
-	dma_set_coherent_mask(dev, dev->parent->coherent_dma_mask);
+	if (!dev->dma_mask) {
+		dev->dma_mask = dev->parent->dma_mask;
+		dev->dma_parms = dev->parent->dma_parms;
+		dma_set_coherent_mask(dev, dev->parent->coherent_dma_mask);
+	}
 
 	pm_runtime_enable(dev);
 	pm_runtime_get_sync(dev);
@@ -815,50 +992,6 @@ static int dwc3_remove(struct platform_device *pdev)
 }
 
 #ifdef CONFIG_PM_SLEEP
-static int dwc3_prepare(struct device *dev)
-{
-	struct dwc3	*dwc = dev_get_drvdata(dev);
-	unsigned long	flags;
-
-	spin_lock_irqsave(&dwc->lock, flags);
-
-	switch (dwc->dr_mode) {
-	case USB_DR_MODE_PERIPHERAL:
-	case USB_DR_MODE_OTG:
-		dwc3_gadget_prepare(dwc);
-		/* FALLTHROUGH */
-	case USB_DR_MODE_HOST:
-	default:
-		dwc3_event_buffers_cleanup(dwc);
-		break;
-	}
-
-	spin_unlock_irqrestore(&dwc->lock, flags);
-
-	return 0;
-}
-
-static void dwc3_complete(struct device *dev)
-{
-	struct dwc3	*dwc = dev_get_drvdata(dev);
-	unsigned long	flags;
-
-	spin_lock_irqsave(&dwc->lock, flags);
-
-	dwc3_event_buffers_setup(dwc);
-	switch (dwc->dr_mode) {
-	case USB_DR_MODE_PERIPHERAL:
-	case USB_DR_MODE_OTG:
-		dwc3_gadget_complete(dwc);
-		/* FALLTHROUGH */
-	case USB_DR_MODE_HOST:
-	default:
-		break;
-	}
-
-	spin_unlock_irqrestore(&dwc->lock, flags);
-}
-
 static int dwc3_suspend(struct device *dev)
 {
 	struct dwc3	*dwc = dev_get_drvdata(dev);
@@ -873,7 +1006,7 @@ static int dwc3_suspend(struct device *dev)
 		/* FALLTHROUGH */
 	case USB_DR_MODE_HOST:
 	default:
-		/* do nothing */
+		dwc3_event_buffers_cleanup(dwc);
 		break;
 	}
 
@@ -906,6 +1039,7 @@ static int dwc3_resume(struct device *dev)
 
 	spin_lock_irqsave(&dwc->lock, flags);
 
+	dwc3_event_buffers_setup(dwc);
 	dwc3_writel(dwc->regs, DWC3_GCTL, dwc->gctl);
 
 	switch (dwc->dr_mode) {
@@ -934,9 +1068,6 @@ err_usb2phy_init:
 }
 
 static const struct dev_pm_ops dwc3_dev_pm_ops = {
-	.prepare	= dwc3_prepare,
-	.complete	= dwc3_complete,
-
 	SET_SYSTEM_SLEEP_PM_OPS(dwc3_suspend, dwc3_resume)
 };
 
@@ -958,12 +1089,24 @@ static const struct of_device_id of_dwc3_match[] = {
 MODULE_DEVICE_TABLE(of, of_dwc3_match);
 #endif
 
+#ifdef CONFIG_ACPI
+
+#define ACPI_ID_INTEL_BSW	"808622B7"
+
+static const struct acpi_device_id dwc3_acpi_match[] = {
+	{ ACPI_ID_INTEL_BSW, 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(acpi, dwc3_acpi_match);
+#endif
+
 static struct platform_driver dwc3_driver = {
 	.probe		= dwc3_probe,
 	.remove		= dwc3_remove,
 	.driver		= {
 		.name	= "dwc3",
 		.of_match_table	= of_match_ptr(of_dwc3_match),
+		.acpi_match_table = ACPI_PTR(dwc3_acpi_match),
 		.pm	= DWC3_PM_OPS,
 	},
 };
diff --git a/drivers/usb/dwc3/core.h b/drivers/usb/dwc3/core.h
index 66f62563bcf9..4bb9aa696ede 100644
--- a/drivers/usb/dwc3/core.h
+++ b/drivers/usb/dwc3/core.h
@@ -166,6 +166,7 @@
 #define DWC3_GCTL_SCALEDOWN(n)		((n) << 4)
 #define DWC3_GCTL_SCALEDOWN_MASK	DWC3_GCTL_SCALEDOWN(3)
 #define DWC3_GCTL_DISSCRAMBLE		(1 << 3)
+#define DWC3_GCTL_U2EXIT_LFPS		(1 << 2)
 #define DWC3_GCTL_GBLHIBERNATIONEN	(1 << 1)
 #define DWC3_GCTL_DSBLCLKGTNG		(1 << 0)
 
@@ -175,7 +176,17 @@
 
 /* Global USB3 PIPE Control Register */
 #define DWC3_GUSB3PIPECTL_PHYSOFTRST	(1 << 31)
+#define DWC3_GUSB3PIPECTL_U2SSINP3OK	(1 << 29)
+#define DWC3_GUSB3PIPECTL_REQP1P2P3	(1 << 24)
+#define DWC3_GUSB3PIPECTL_DEP1P2P3(n)	((n) << 19)
+#define DWC3_GUSB3PIPECTL_DEP1P2P3_MASK	DWC3_GUSB3PIPECTL_DEP1P2P3(7)
+#define DWC3_GUSB3PIPECTL_DEP1P2P3_EN	DWC3_GUSB3PIPECTL_DEP1P2P3(1)
+#define DWC3_GUSB3PIPECTL_DEPOCHANGE	(1 << 18)
 #define DWC3_GUSB3PIPECTL_SUSPHY	(1 << 17)
+#define DWC3_GUSB3PIPECTL_LFPSFILT	(1 << 9)
+#define DWC3_GUSB3PIPECTL_RX_DETOPOLL	(1 << 8)
+#define DWC3_GUSB3PIPECTL_TX_DEEPH_MASK	DWC3_GUSB3PIPECTL_TX_DEEPH(3)
+#define DWC3_GUSB3PIPECTL_TX_DEEPH(n)	((n) << 1)
 
 /* Global TX Fifo Size Register */
 #define DWC3_GTXFIFOSIZ_TXFDEF(n)	((n) & 0xffff)
@@ -210,6 +221,9 @@
 #define DWC3_GHWPARAMS4_HIBER_SCRATCHBUFS(n)	(((n) & (0x0f << 13)) >> 13)
 #define DWC3_MAX_HIBER_SCRATCHBUFS		15
 
+/* Global HWPARAMS6 Register */
+#define DWC3_GHWPARAMS6_EN_FPGA			(1 << 7)
+
 /* Device Configuration Register */
 #define DWC3_DCFG_DEVADDR(addr)	((addr) << 3)
 #define DWC3_DCFG_DEVADDR_MASK	DWC3_DCFG_DEVADDR(0x7f)
@@ -243,16 +257,19 @@
 #define DWC3_DCTL_TRGTULST_SS_INACT	(DWC3_DCTL_TRGTULST(6))
 
 /* These apply for core versions 1.94a and later */
-#define DWC3_DCTL_KEEP_CONNECT	(1 << 19)
-#define DWC3_DCTL_L1_HIBER_EN	(1 << 18)
-#define DWC3_DCTL_CRS		(1 << 17)
-#define DWC3_DCTL_CSS		(1 << 16)
+#define DWC3_DCTL_LPM_ERRATA_MASK	DWC3_DCTL_LPM_ERRATA(0xf)
+#define DWC3_DCTL_LPM_ERRATA(n)		((n) << 20)
+
+#define DWC3_DCTL_KEEP_CONNECT		(1 << 19)
+#define DWC3_DCTL_L1_HIBER_EN		(1 << 18)
+#define DWC3_DCTL_CRS			(1 << 17)
+#define DWC3_DCTL_CSS			(1 << 16)
 
-#define DWC3_DCTL_INITU2ENA	(1 << 12)
-#define DWC3_DCTL_ACCEPTU2ENA	(1 << 11)
-#define DWC3_DCTL_INITU1ENA	(1 << 10)
-#define DWC3_DCTL_ACCEPTU1ENA	(1 << 9)
-#define DWC3_DCTL_TSTCTRL_MASK	(0xf << 1)
+#define DWC3_DCTL_INITU2ENA		(1 << 12)
+#define DWC3_DCTL_ACCEPTU2ENA		(1 << 11)
+#define DWC3_DCTL_INITU1ENA		(1 << 10)
+#define DWC3_DCTL_ACCEPTU1ENA		(1 << 9)
+#define DWC3_DCTL_TSTCTRL_MASK		(0xf << 1)
 
 #define DWC3_DCTL_ULSTCHNGREQ_MASK	(0x0f << 5)
 #define DWC3_DCTL_ULSTCHNGREQ(n) (((n) << 5) & DWC3_DCTL_ULSTCHNGREQ_MASK)
@@ -657,17 +674,41 @@ struct dwc3_scratchpad_array {
  * @regset: debugfs pointer to regdump file
  * @test_mode: true when we're entering a USB test mode
  * @test_mode_nr: test feature selector
+ * @lpm_nyet_threshold: LPM NYET response threshold
+ * @hird_threshold: HIRD threshold
  * @delayed_status: true when gadget driver asks for delayed status
  * @ep0_bounced: true when we used bounce buffer
  * @ep0_expect_in: true when we expect a DATA IN transfer
  * @has_hibernation: true when dwc3 was configured with Hibernation
+ * @has_lpm_erratum: true when core was configured with LPM Erratum. Note that
+ *			there's now way for software to detect this in runtime.
+ * @is_utmi_l1_suspend: the core asserts output signal
+ * 	0	- utmi_sleep_n
+ * 	1	- utmi_l1_suspend_n
  * @is_selfpowered: true when we are selfpowered
+ * @is_fpga: true when we are using the FPGA board
  * @needs_fifo_resize: not all users might want fifo resizing, flag it
  * @pullups_connected: true when Run/Stop bit is set
  * @resize_fifos: tells us it's ok to reconfigure our TxFIFO sizes.
  * @setup_packet_pending: true when there's a Setup Packet in FIFO. Workaround
  * @start_config_issued: true when StartConfig command has been issued
  * @three_stage_setup: set if we perform a three phase setup
+ * @disable_scramble_quirk: set if we enable the disable scramble quirk
+ * @u2exit_lfps_quirk: set if we enable u2exit lfps quirk
+ * @u2ss_inp3_quirk: set if we enable P3 OK for U2/SS Inactive quirk
+ * @req_p1p2p3_quirk: set if we enable request p1p2p3 quirk
+ * @del_p1p2p3_quirk: set if we enable delay p1p2p3 quirk
+ * @del_phy_power_chg_quirk: set if we enable delay phy power change quirk
+ * @lfps_filter_quirk: set if we enable LFPS filter quirk
+ * @rx_detect_poll_quirk: set if we enable rx_detect to polling lfps quirk
+ * @dis_u3_susphy_quirk: set if we disable usb3 suspend phy
+ * @dis_u2_susphy_quirk: set if we disable usb2 suspend phy
+ * @tx_de_emphasis_quirk: set if we enable Tx de-emphasis quirk
+ * @tx_de_emphasis: Tx de-emphasis value
+ * 	0	- -6dB de-emphasis
+ * 	1	- -3.5dB de-emphasis
+ * 	2	- No de-emphasis
+ * 	3	- Reserved
  */
 struct dwc3 {
 	struct usb_ctrlrequest	*ctrl_req;
@@ -759,18 +800,37 @@ struct dwc3 {
 
 	u8			test_mode;
 	u8			test_mode_nr;
+	u8			lpm_nyet_threshold;
+	u8			hird_threshold;
 
 	unsigned		delayed_status:1;
 	unsigned		ep0_bounced:1;
 	unsigned		ep0_expect_in:1;
 	unsigned		has_hibernation:1;
+	unsigned		has_lpm_erratum:1;
+	unsigned		is_utmi_l1_suspend:1;
 	unsigned		is_selfpowered:1;
+	unsigned		is_fpga:1;
 	unsigned		needs_fifo_resize:1;
 	unsigned		pullups_connected:1;
 	unsigned		resize_fifos:1;
 	unsigned		setup_packet_pending:1;
 	unsigned		start_config_issued:1;
 	unsigned		three_stage_setup:1;
+
+	unsigned		disable_scramble_quirk:1;
+	unsigned		u2exit_lfps_quirk:1;
+	unsigned		u2ss_inp3_quirk:1;
+	unsigned		req_p1p2p3_quirk:1;
+	unsigned                del_p1p2p3_quirk:1;
+	unsigned		del_phy_power_chg_quirk:1;
+	unsigned		lfps_filter_quirk:1;
+	unsigned		rx_detect_poll_quirk:1;
+	unsigned		dis_u3_susphy_quirk:1;
+	unsigned		dis_u2_susphy_quirk:1;
+
+	unsigned		tx_de_emphasis_quirk:1;
+	unsigned		tx_de_emphasis:2;
 };
 
 /* -------------------------------------------------------------------------- */
@@ -964,20 +1024,9 @@ static inline int dwc3_send_gadget_generic_command(struct dwc3 *dwc,
 
 /* power management interface */
 #if !IS_ENABLED(CONFIG_USB_DWC3_HOST)
-int dwc3_gadget_prepare(struct dwc3 *dwc);
-void dwc3_gadget_complete(struct dwc3 *dwc);
 int dwc3_gadget_suspend(struct dwc3 *dwc);
 int dwc3_gadget_resume(struct dwc3 *dwc);
 #else
-static inline int dwc3_gadget_prepare(struct dwc3 *dwc)
-{
-	return 0;
-}
-
-static inline void dwc3_gadget_complete(struct dwc3 *dwc)
-{
-}
-
 static inline int dwc3_gadget_suspend(struct dwc3 *dwc)
 {
 	return 0;
diff --git a/drivers/usb/dwc3/dwc3-exynos.c b/drivers/usb/dwc3/dwc3-exynos.c
index 3951a65fea04..7bd0a95b2815 100644
--- a/drivers/usb/dwc3/dwc3-exynos.c
+++ b/drivers/usb/dwc3/dwc3-exynos.c
@@ -20,7 +20,6 @@
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/platform_device.h>
-#include <linux/platform_data/dwc3-exynos.h>
 #include <linux/dma-mapping.h>
 #include <linux/clk.h>
 #include <linux/usb/otg.h>
@@ -35,6 +34,9 @@ struct dwc3_exynos {
 	struct device		*dev;
 
 	struct clk		*clk;
+	struct clk		*susp_clk;
+	struct clk		*axius_clk;
+
 	struct regulator	*vdd33;
 	struct regulator	*vdd10;
 };
@@ -106,7 +108,6 @@ static int dwc3_exynos_remove_child(struct device *dev, void *unused)
 static int dwc3_exynos_probe(struct platform_device *pdev)
 {
 	struct dwc3_exynos	*exynos;
-	struct clk		*clk;
 	struct device		*dev = &pdev->dev;
 	struct device_node	*node = dev->of_node;
 
@@ -133,16 +134,32 @@ static int dwc3_exynos_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	clk = devm_clk_get(dev, "usbdrd30");
-	if (IS_ERR(clk)) {
+	exynos->dev	= dev;
+
+	exynos->clk = devm_clk_get(dev, "usbdrd30");
+	if (IS_ERR(exynos->clk)) {
 		dev_err(dev, "couldn't get clock\n");
 		return -EINVAL;
 	}
+	clk_prepare_enable(exynos->clk);
 
-	exynos->dev	= dev;
-	exynos->clk	= clk;
+	exynos->susp_clk = devm_clk_get(dev, "usbdrd30_susp_clk");
+	if (IS_ERR(exynos->susp_clk)) {
+		dev_dbg(dev, "no suspend clk specified\n");
+		exynos->susp_clk = NULL;
+	}
+	clk_prepare_enable(exynos->susp_clk);
 
-	clk_prepare_enable(exynos->clk);
+	if (of_device_is_compatible(node, "samsung,exynos7-dwusb3")) {
+		exynos->axius_clk = devm_clk_get(dev, "usbdrd30_axius_clk");
+		if (IS_ERR(exynos->axius_clk)) {
+			dev_err(dev, "no AXI UpScaler clk specified\n");
+			return -ENODEV;
+		}
+		clk_prepare_enable(exynos->axius_clk);
+	} else {
+		exynos->axius_clk = NULL;
+	}
 
 	exynos->vdd33 = devm_regulator_get(dev, "vdd33");
 	if (IS_ERR(exynos->vdd33)) {
@@ -185,7 +202,9 @@ err4:
 err3:
 	regulator_disable(exynos->vdd33);
 err2:
-	clk_disable_unprepare(clk);
+	clk_disable_unprepare(exynos->axius_clk);
+	clk_disable_unprepare(exynos->susp_clk);
+	clk_disable_unprepare(exynos->clk);
 	return ret;
 }
 
@@ -197,6 +216,8 @@ static int dwc3_exynos_remove(struct platform_device *pdev)
 	platform_device_unregister(exynos->usb2_phy);
 	platform_device_unregister(exynos->usb3_phy);
 
+	clk_disable_unprepare(exynos->axius_clk);
+	clk_disable_unprepare(exynos->susp_clk);
 	clk_disable_unprepare(exynos->clk);
 
 	regulator_disable(exynos->vdd33);
@@ -205,19 +226,19 @@ static int dwc3_exynos_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_OF
 static const struct of_device_id exynos_dwc3_match[] = {
 	{ .compatible = "samsung,exynos5250-dwusb3" },
+	{ .compatible = "samsung,exynos7-dwusb3" },
 	{},
 };
 MODULE_DEVICE_TABLE(of, exynos_dwc3_match);
-#endif
 
 #ifdef CONFIG_PM_SLEEP
 static int dwc3_exynos_suspend(struct device *dev)
 {
 	struct dwc3_exynos *exynos = dev_get_drvdata(dev);
 
+	clk_disable(exynos->axius_clk);
 	clk_disable(exynos->clk);
 
 	regulator_disable(exynos->vdd33);
@@ -243,6 +264,7 @@ static int dwc3_exynos_resume(struct device *dev)
 	}
 
 	clk_enable(exynos->clk);
+	clk_enable(exynos->axius_clk);
 
 	/* runtime set active to reflect active state. */
 	pm_runtime_disable(dev);
@@ -266,7 +288,7 @@ static struct platform_driver dwc3_exynos_driver = {
 	.remove		= dwc3_exynos_remove,
 	.driver		= {
 		.name	= "exynos-dwc3",
-		.of_match_table = of_match_ptr(exynos_dwc3_match),
+		.of_match_table = exynos_dwc3_match,
 		.pm	= DEV_PM_OPS,
 	},
 };
diff --git a/drivers/usb/dwc3/dwc3-keystone.c b/drivers/usb/dwc3/dwc3-keystone.c
index 7ec8495f4523..fe3b9335a74e 100644
--- a/drivers/usb/dwc3/dwc3-keystone.c
+++ b/drivers/usb/dwc3/dwc3-keystone.c
@@ -104,11 +104,6 @@ static int kdwc3_probe(struct platform_device *pdev)
 	kdwc->dev = dev;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(dev, "missing usbss resource\n");
-		return -EINVAL;
-	}
-
 	kdwc->usbss = devm_ioremap_resource(dev, res);
 	if (IS_ERR(kdwc->usbss))
 		return PTR_ERR(kdwc->usbss);
@@ -128,6 +123,7 @@ static int kdwc3_probe(struct platform_device *pdev)
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0) {
 		dev_err(&pdev->dev, "missing irq\n");
+		error = irq;
 		goto err_irq;
 	}
 
diff --git a/drivers/usb/dwc3/dwc3-omap.c b/drivers/usb/dwc3/dwc3-omap.c
index a0aa9f3da441..172d64e585b6 100644
--- a/drivers/usb/dwc3/dwc3-omap.c
+++ b/drivers/usb/dwc3/dwc3-omap.c
@@ -593,27 +593,12 @@ static const struct of_device_id of_dwc3_match[] = {
 MODULE_DEVICE_TABLE(of, of_dwc3_match);
 
 #ifdef CONFIG_PM_SLEEP
-static int dwc3_omap_prepare(struct device *dev)
-{
-	struct dwc3_omap	*omap = dev_get_drvdata(dev);
-
-	dwc3_omap_disable_irqs(omap);
-
-	return 0;
-}
-
-static void dwc3_omap_complete(struct device *dev)
-{
-	struct dwc3_omap	*omap = dev_get_drvdata(dev);
-
-	dwc3_omap_enable_irqs(omap);
-}
-
 static int dwc3_omap_suspend(struct device *dev)
 {
 	struct dwc3_omap	*omap = dev_get_drvdata(dev);
 
 	omap->utmi_otg_status = dwc3_omap_read_utmi_status(omap);
+	dwc3_omap_disable_irqs(omap);
 
 	return 0;
 }
@@ -623,6 +608,7 @@ static int dwc3_omap_resume(struct device *dev)
 	struct dwc3_omap	*omap = dev_get_drvdata(dev);
 
 	dwc3_omap_write_utmi_status(omap, omap->utmi_otg_status);
+	dwc3_omap_enable_irqs(omap);
 
 	pm_runtime_disable(dev);
 	pm_runtime_set_active(dev);
@@ -632,8 +618,6 @@ static int dwc3_omap_resume(struct device *dev)
 }
 
 static const struct dev_pm_ops dwc3_omap_dev_pm_ops = {
-	.prepare	= dwc3_omap_prepare,
-	.complete	= dwc3_omap_complete,
 
 	SET_SYSTEM_SLEEP_PM_OPS(dwc3_omap_suspend, dwc3_omap_resume)
 };
diff --git a/drivers/usb/dwc3/dwc3-pci.c b/drivers/usb/dwc3/dwc3-pci.c
index a36cf66302fb..7c4faf738747 100644
--- a/drivers/usb/dwc3/dwc3-pci.c
+++ b/drivers/usb/dwc3/dwc3-pci.c
@@ -25,6 +25,8 @@
 #include <linux/usb/otg.h>
 #include <linux/usb/usb_phy_generic.h>
 
+#include "platform_data.h"
+
 /* FIXME define these in <linux/pci_ids.h> */
 #define PCI_VENDOR_ID_SYNOPSYS		0x16c3
 #define PCI_DEVICE_ID_SYNOPSYS_HAPSUSB3	0xabcd
@@ -102,6 +104,9 @@ static int dwc3_pci_probe(struct pci_dev *pci,
 	struct dwc3_pci		*glue;
 	int			ret;
 	struct device		*dev = &pci->dev;
+	struct dwc3_platform_data dwc3_pdata;
+
+	memset(&dwc3_pdata, 0x00, sizeof(dwc3_pdata));
 
 	glue = devm_kzalloc(dev, sizeof(*glue), GFP_KERNEL);
 	if (!glue)
@@ -140,6 +145,31 @@ static int dwc3_pci_probe(struct pci_dev *pci,
 	res[1].name	= "dwc_usb3";
 	res[1].flags	= IORESOURCE_IRQ;
 
+	if (pci->vendor == PCI_VENDOR_ID_AMD &&
+			pci->device == PCI_DEVICE_ID_AMD_NL_USB) {
+		dwc3_pdata.has_lpm_erratum = true;
+		dwc3_pdata.lpm_nyet_threshold = 0xf;
+
+		dwc3_pdata.u2exit_lfps_quirk = true;
+		dwc3_pdata.u2ss_inp3_quirk = true;
+		dwc3_pdata.req_p1p2p3_quirk = true;
+		dwc3_pdata.del_p1p2p3_quirk = true;
+		dwc3_pdata.del_phy_power_chg_quirk = true;
+		dwc3_pdata.lfps_filter_quirk = true;
+		dwc3_pdata.rx_detect_poll_quirk = true;
+
+		dwc3_pdata.tx_de_emphasis_quirk = true;
+		dwc3_pdata.tx_de_emphasis = 1;
+
+		/*
+		 * FIXME these quirks should be removed when AMD NL
+		 * taps out
+		 */
+		dwc3_pdata.disable_scramble_quirk = true;
+		dwc3_pdata.dis_u3_susphy_quirk = true;
+		dwc3_pdata.dis_u2_susphy_quirk = true;
+	}
+
 	ret = platform_device_add_resources(dwc3, res, ARRAY_SIZE(res));
 	if (ret) {
 		dev_err(dev, "couldn't add resources to dwc3 device\n");
@@ -148,6 +178,10 @@ static int dwc3_pci_probe(struct pci_dev *pci,
 
 	pci_set_drvdata(pci, glue);
 
+	ret = platform_device_add_data(dwc3, &dwc3_pdata, sizeof(dwc3_pdata));
+	if (ret)
+		goto err3;
+
 	dma_set_coherent_mask(&dwc3->dev, dev->coherent_dma_mask);
 
 	dwc3->dev.dma_mask = dev->dma_mask;
@@ -185,6 +219,7 @@ static const struct pci_device_id dwc3_pci_id_table[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_BSW), },
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_BYT), },
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_MRFLD), },
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMD, PCI_DEVICE_ID_AMD_NL_USB), },
 	{  }	/* Terminating Entry */
 };
 MODULE_DEVICE_TABLE(pci, dwc3_pci_id_table);
diff --git a/drivers/usb/dwc3/dwc3-st.c b/drivers/usb/dwc3/dwc3-st.c
index c7602b5362ad..4a1a543deeda 100644
--- a/drivers/usb/dwc3/dwc3-st.c
+++ b/drivers/usb/dwc3/dwc3-st.c
@@ -243,7 +243,7 @@ static int st_dwc3_probe(struct platform_device *pdev)
 	dwc3_data->rstc_rst = devm_reset_control_get(dev, "softreset");
 	if (IS_ERR(dwc3_data->rstc_rst)) {
 		dev_err(&pdev->dev, "could not get reset controller\n");
-		ret = PTR_ERR(dwc3_data->rstc_pwrdn);
+		ret = PTR_ERR(dwc3_data->rstc_rst);
 		goto undo_powerdown;
 	}
 
diff --git a/drivers/usb/dwc3/ep0.c b/drivers/usb/dwc3/ep0.c
index df38e7ef4976..1bc77a3b4997 100644
--- a/drivers/usb/dwc3/ep0.c
+++ b/drivers/usb/dwc3/ep0.c
@@ -86,6 +86,8 @@ static int dwc3_ep0_start_trans(struct dwc3 *dwc, u8 epnum, dma_addr_t buf_dma,
 	params.param0 = upper_32_bits(dwc->ep0_trb_addr);
 	params.param1 = lower_32_bits(dwc->ep0_trb_addr);
 
+	trace_dwc3_prepare_trb(dep, trb);
+
 	ret = dwc3_send_gadget_ep_cmd(dwc, dep->number,
 			DWC3_DEPCMD_STARTTRANSFER, &params);
 	if (ret < 0) {
@@ -441,7 +443,6 @@ static int dwc3_ep0_handle_feature(struct dwc3 *dwc,
 
 		case USB_DEVICE_LTM_ENABLE:
 			return -EINVAL;
-			break;
 
 		case USB_DEVICE_TEST_MODE:
 			if ((wIndex & 0xff) != 0)
@@ -550,7 +551,6 @@ static int dwc3_ep0_set_config(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 	switch (state) {
 	case USB_STATE_DEFAULT:
 		return -EINVAL;
-		break;
 
 	case USB_STATE_ADDRESS:
 		ret = dwc3_ep0_delegate_req(dwc, ctrl);
@@ -700,35 +700,35 @@ static int dwc3_ep0_std_request(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 
 	switch (ctrl->bRequest) {
 	case USB_REQ_GET_STATUS:
-		dwc3_trace(trace_dwc3_ep0, "USB_REQ_GET_STATUS\n");
+		dwc3_trace(trace_dwc3_ep0, "USB_REQ_GET_STATUS");
 		ret = dwc3_ep0_handle_status(dwc, ctrl);
 		break;
 	case USB_REQ_CLEAR_FEATURE:
-		dwc3_trace(trace_dwc3_ep0, "USB_REQ_CLEAR_FEATURE\n");
+		dwc3_trace(trace_dwc3_ep0, "USB_REQ_CLEAR_FEATURE");
 		ret = dwc3_ep0_handle_feature(dwc, ctrl, 0);
 		break;
 	case USB_REQ_SET_FEATURE:
-		dwc3_trace(trace_dwc3_ep0, "USB_REQ_SET_FEATURE\n");
+		dwc3_trace(trace_dwc3_ep0, "USB_REQ_SET_FEATURE");
 		ret = dwc3_ep0_handle_feature(dwc, ctrl, 1);
 		break;
 	case USB_REQ_SET_ADDRESS:
-		dwc3_trace(trace_dwc3_ep0, "USB_REQ_SET_ADDRESS\n");
+		dwc3_trace(trace_dwc3_ep0, "USB_REQ_SET_ADDRESS");
 		ret = dwc3_ep0_set_address(dwc, ctrl);
 		break;
 	case USB_REQ_SET_CONFIGURATION:
-		dwc3_trace(trace_dwc3_ep0, "USB_REQ_SET_CONFIGURATION\n");
+		dwc3_trace(trace_dwc3_ep0, "USB_REQ_SET_CONFIGURATION");
 		ret = dwc3_ep0_set_config(dwc, ctrl);
 		break;
 	case USB_REQ_SET_SEL:
-		dwc3_trace(trace_dwc3_ep0, "USB_REQ_SET_SEL\n");
+		dwc3_trace(trace_dwc3_ep0, "USB_REQ_SET_SEL");
 		ret = dwc3_ep0_set_sel(dwc, ctrl);
 		break;
 	case USB_REQ_SET_ISOCH_DELAY:
-		dwc3_trace(trace_dwc3_ep0, "USB_REQ_SET_ISOCH_DELAY\n");
+		dwc3_trace(trace_dwc3_ep0, "USB_REQ_SET_ISOCH_DELAY");
 		ret = dwc3_ep0_set_isoch_delay(dwc, ctrl);
 		break;
 	default:
-		dwc3_trace(trace_dwc3_ep0, "Forwarding to gadget driver\n");
+		dwc3_trace(trace_dwc3_ep0, "Forwarding to gadget driver");
 		ret = dwc3_ep0_delegate_req(dwc, ctrl);
 		break;
 	}
@@ -791,6 +791,8 @@ static void dwc3_ep0_complete_data(struct dwc3 *dwc,
 
 	trb = dwc->ep0_trb;
 
+	trace_dwc3_complete_trb(ep0, trb);
+
 	r = next_request(&ep0->request_list);
 	if (!r)
 		return;
@@ -855,6 +857,8 @@ static void dwc3_ep0_complete_status(struct dwc3 *dwc,
 	dep = dwc->eps[0];
 	trb = dwc->ep0_trb;
 
+	trace_dwc3_complete_trb(dep, trb);
+
 	if (!list_empty(&dep->request_list)) {
 		r = next_request(&dep->request_list);
 
@@ -875,7 +879,7 @@ static void dwc3_ep0_complete_status(struct dwc3 *dwc,
 
 	status = DWC3_TRB_SIZE_TRBSTS(trb->size);
 	if (status == DWC3_TRBSTS_SETUP_PENDING)
-		dwc3_trace(trace_dwc3_ep0, "Setup Pending received\n");
+		dwc3_trace(trace_dwc3_ep0, "Setup Pending received");
 
 	dwc->ep0state = EP0_SETUP_PHASE;
 	dwc3_ep0_out_start(dwc);
diff --git a/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index 546ea5431b8c..f03b136ecfce 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -1140,8 +1140,14 @@ static int dwc3_gadget_ep_queue(struct usb_ep *ep, struct usb_request *request,
 	if (!dep->endpoint.desc) {
 		dev_dbg(dwc->dev, "trying to queue request %p to disabled %s\n",
 				request, ep->name);
-		spin_unlock_irqrestore(&dwc->lock, flags);
-		return -ESHUTDOWN;
+		ret = -ESHUTDOWN;
+		goto out;
+	}
+
+	if (WARN(req->dep != dep, "request %p belongs to '%s'\n",
+				request, req->dep->name)) {
+		ret = -EINVAL;
+		goto out;
 	}
 
 	dev_vdbg(dwc->dev, "queing request %p to %s length %d\n",
@@ -1149,6 +1155,8 @@ static int dwc3_gadget_ep_queue(struct usb_ep *ep, struct usb_request *request,
 	trace_dwc3_ep_queue(req);
 
 	ret = __dwc3_gadget_ep_queue(dep, req);
+
+out:
 	spin_unlock_irqrestore(&dwc->lock, flags);
 
 	return ret;
@@ -1622,8 +1630,7 @@ err0:
 	return ret;
 }
 
-static int dwc3_gadget_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver)
+static int dwc3_gadget_stop(struct usb_gadget *g)
 {
 	struct dwc3		*dwc = gadget_to_dwc(g);
 	unsigned long		flags;
@@ -2034,6 +2041,17 @@ static void dwc3_resume_gadget(struct dwc3 *dwc)
 	if (dwc->gadget_driver && dwc->gadget_driver->resume) {
 		spin_unlock(&dwc->lock);
 		dwc->gadget_driver->resume(&dwc->gadget);
+	}
+}
+
+static void dwc3_reset_gadget(struct dwc3 *dwc)
+{
+	if (!dwc->gadget_driver)
+		return;
+
+	if (dwc->gadget.speed != USB_SPEED_UNKNOWN) {
+		spin_unlock(&dwc->lock);
+		usb_gadget_udc_reset(&dwc->gadget, dwc->gadget_driver);
 		spin_lock(&dwc->lock);
 	}
 }
@@ -2140,6 +2158,7 @@ static void dwc3_gadget_disconnect_interrupt(struct dwc3 *dwc)
 
 	dwc->gadget.speed = USB_SPEED_UNKNOWN;
 	dwc->setup_packet_pending = false;
+	usb_gadget_set_state(&dwc->gadget, USB_STATE_NOTATTACHED);
 }
 
 static void dwc3_gadget_reset_interrupt(struct dwc3 *dwc)
@@ -2177,11 +2196,7 @@ static void dwc3_gadget_reset_interrupt(struct dwc3 *dwc)
 			dwc3_gadget_disconnect_interrupt(dwc);
 	}
 
-	/* after reset -> Default State */
-	usb_gadget_set_state(&dwc->gadget, USB_STATE_DEFAULT);
-
-	if (dwc->gadget.speed != USB_SPEED_UNKNOWN)
-		dwc3_disconnect_gadget(dwc);
+	dwc3_reset_gadget(dwc);
 
 	reg = dwc3_readl(dwc->regs, DWC3_DCTL);
 	reg &= ~DWC3_DCTL_TSTCTRL_MASK;
@@ -2287,11 +2302,20 @@ static void dwc3_gadget_conndone_interrupt(struct dwc3 *dwc)
 		reg = dwc3_readl(dwc->regs, DWC3_DCTL);
 		reg &= ~(DWC3_DCTL_HIRD_THRES_MASK | DWC3_DCTL_L1_HIBER_EN);
 
+		reg |= DWC3_DCTL_HIRD_THRES(dwc->hird_threshold);
+
 		/*
-		 * TODO: This should be configurable. For now using
-		 * maximum allowed HIRD threshold value of 0b1100
+		 * When dwc3 revisions >= 2.40a, LPM Erratum is enabled and
+		 * DCFG.LPMCap is set, core responses with an ACK and the
+		 * BESL value in the LPM token is less than or equal to LPM
+		 * NYET threshold.
 		 */
-		reg |= DWC3_DCTL_HIRD_THRES(12);
+		WARN_ONCE(dwc->revision < DWC3_REVISION_240A
+				&& dwc->has_lpm_erratum,
+				"LPM Erratum not available on dwc3 revisisions < 2.40a\n");
+
+		if (dwc->has_lpm_erratum && dwc->revision >= DWC3_REVISION_240A)
+			reg |= DWC3_DCTL_LPM_ERRATA(dwc->lpm_nyet_threshold);
 
 		dwc3_writel(dwc->regs, DWC3_DCTL, reg);
 	} else {
@@ -2744,26 +2768,13 @@ void dwc3_gadget_exit(struct dwc3 *dwc)
 			dwc->ctrl_req, dwc->ctrl_req_addr);
 }
 
-int dwc3_gadget_prepare(struct dwc3 *dwc)
+int dwc3_gadget_suspend(struct dwc3 *dwc)
 {
 	if (dwc->pullups_connected) {
 		dwc3_gadget_disable_irq(dwc);
 		dwc3_gadget_run_stop(dwc, true, true);
 	}
 
-	return 0;
-}
-
-void dwc3_gadget_complete(struct dwc3 *dwc)
-{
-	if (dwc->pullups_connected) {
-		dwc3_gadget_enable_irq(dwc);
-		dwc3_gadget_run_stop(dwc, true, false);
-	}
-}
-
-int dwc3_gadget_suspend(struct dwc3 *dwc)
-{
 	__dwc3_gadget_ep_disable(dwc->eps[0]);
 	__dwc3_gadget_ep_disable(dwc->eps[1]);
 
@@ -2798,6 +2809,11 @@ int dwc3_gadget_resume(struct dwc3 *dwc)
 
 	dwc3_writel(dwc->regs, DWC3_DCFG, dwc->dcfg);
 
+	if (dwc->pullups_connected) {
+		dwc3_gadget_enable_irq(dwc);
+		dwc3_gadget_run_stop(dwc, true, false);
+	}
+
 	return 0;
 
 err1:
diff --git a/drivers/usb/dwc3/host.c b/drivers/usb/dwc3/host.c
index dcb8ca084598..12bfd3c5405e 100644
--- a/drivers/usb/dwc3/host.c
+++ b/drivers/usb/dwc3/host.c
@@ -29,8 +29,7 @@ int dwc3_host_init(struct dwc3 *dwc)
 	xhci = platform_device_alloc("xhci-hcd", PLATFORM_DEVID_AUTO);
 	if (!xhci) {
 		dev_err(dwc->dev, "couldn't allocate xHCI device\n");
-		ret = -ENOMEM;
-		goto err0;
+		return -ENOMEM;
 	}
 
 	dma_set_coherent_mask(&xhci->dev, dwc->dev->coherent_dma_mask);
@@ -60,22 +59,33 @@ int dwc3_host_init(struct dwc3 *dwc)
 		goto err1;
 	}
 
+	phy_create_lookup(dwc->usb2_generic_phy, "usb2-phy",
+			  dev_name(&xhci->dev));
+	phy_create_lookup(dwc->usb3_generic_phy, "usb3-phy",
+			  dev_name(&xhci->dev));
+
 	ret = platform_device_add(xhci);
 	if (ret) {
 		dev_err(dwc->dev, "failed to register xHCI device\n");
-		goto err1;
+		goto err2;
 	}
 
 	return 0;
-
+err2:
+	phy_remove_lookup(dwc->usb2_generic_phy, "usb2-phy",
+			  dev_name(&xhci->dev));
+	phy_remove_lookup(dwc->usb3_generic_phy, "usb3-phy",
+			  dev_name(&xhci->dev));
 err1:
 	platform_device_put(xhci);
-
-err0:
 	return ret;
 }
 
 void dwc3_host_exit(struct dwc3 *dwc)
 {
+	phy_remove_lookup(dwc->usb2_generic_phy, "usb2-phy",
+			  dev_name(&dwc->xhci->dev));
+	phy_remove_lookup(dwc->usb3_generic_phy, "usb3-phy",
+			  dev_name(&dwc->xhci->dev));
 	platform_device_unregister(dwc->xhci);
 }
diff --git a/drivers/usb/dwc3/platform_data.h b/drivers/usb/dwc3/platform_data.h
index 7db34f00b89a..a3a3b6d5668c 100644
--- a/drivers/usb/dwc3/platform_data.h
+++ b/drivers/usb/dwc3/platform_data.h
@@ -24,4 +24,24 @@ struct dwc3_platform_data {
 	enum usb_device_speed maximum_speed;
 	enum usb_dr_mode dr_mode;
 	bool tx_fifo_resize;
+
+	unsigned is_utmi_l1_suspend:1;
+	u8 hird_threshold;
+
+	u8 lpm_nyet_threshold;
+
+	unsigned disable_scramble_quirk:1;
+	unsigned has_lpm_erratum:1;
+	unsigned u2exit_lfps_quirk:1;
+	unsigned u2ss_inp3_quirk:1;
+	unsigned req_p1p2p3_quirk:1;
+	unsigned del_p1p2p3_quirk:1;
+	unsigned del_phy_power_chg_quirk:1;
+	unsigned lfps_filter_quirk:1;
+	unsigned rx_detect_poll_quirk:1;
+	unsigned dis_u3_susphy_quirk:1;
+	unsigned dis_u2_susphy_quirk:1;
+
+	unsigned tx_de_emphasis_quirk:1;
+	unsigned tx_de_emphasis:2;
 };
diff --git a/drivers/usb/dwc3/trace.h b/drivers/usb/dwc3/trace.h
index 60b0f41eafc4..9fc20b33dd8e 100644
--- a/drivers/usb/dwc3/trace.h
+++ b/drivers/usb/dwc3/trace.h
@@ -61,7 +61,7 @@ DECLARE_EVENT_CLASS(dwc3_log_event,
 	TP_fast_assign(
 		__entry->event = event;
 	),
-	TP_printk("event %08x\n", __entry->event)
+	TP_printk("event %08x", __entry->event)
 );
 
 DEFINE_EVENT(dwc3_log_event, dwc3_event,
@@ -157,7 +157,7 @@ DECLARE_EVENT_CLASS(dwc3_log_generic_cmd,
 		__entry->cmd = cmd;
 		__entry->param = param;
 	),
-	TP_printk("cmd '%s' [%d] param %08x\n",
+	TP_printk("cmd '%s' [%d] param %08x",
 		dwc3_gadget_generic_cmd_string(__entry->cmd),
 		__entry->cmd, __entry->param
 	)
@@ -175,17 +175,21 @@ DECLARE_EVENT_CLASS(dwc3_log_gadget_ep_cmd,
 	TP_STRUCT__entry(
 		__dynamic_array(char, name, DWC3_MSG_MAX)
 		__field(unsigned int, cmd)
-		__field(struct dwc3_gadget_ep_cmd_params *, params)
+		__field(u32, param0)
+		__field(u32, param1)
+		__field(u32, param2)
 	),
 	TP_fast_assign(
 		snprintf(__get_str(name), DWC3_MSG_MAX, "%s", dep->name);
 		__entry->cmd = cmd;
-		__entry->params = params;
+		__entry->param0 = params->param0;
+		__entry->param1 = params->param1;
+		__entry->param2 = params->param2;
 	),
-	TP_printk("%s: cmd '%s' [%d] params %08x %08x %08x\n",
+	TP_printk("%s: cmd '%s' [%d] params %08x %08x %08x",
 		__get_str(name), dwc3_gadget_ep_cmd_string(__entry->cmd),
-		__entry->cmd, __entry->params->param0,
-		__entry->params->param1, __entry->params->param2
+		__entry->cmd, __entry->param0,
+		__entry->param1, __entry->param2
 	)
 );
 
@@ -214,7 +218,7 @@ DECLARE_EVENT_CLASS(dwc3_log_trb,
 		__entry->size = trb->size;
 		__entry->ctrl = trb->ctrl;
 	),
-	TP_printk("%s: trb %p bph %08x bpl %08x size %08x ctrl %08x\n",
+	TP_printk("%s: trb %p bph %08x bpl %08x size %08x ctrl %08x",
 		__get_str(name), __entry->trb, __entry->bph, __entry->bpl,
 		__entry->size, __entry->ctrl
 	)
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index c4880fc0d86e..747ef53bda14 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -190,6 +190,12 @@ config USB_F_UAC2
 config USB_F_UVC
 	tristate
 
+config USB_F_MIDI
+	tristate
+
+config USB_F_HID
+	tristate
+
 choice
 	tristate "USB Gadget Drivers"
 	default USB_ETH
@@ -362,6 +368,61 @@ config USB_CONFIGFS_F_FS
 	  implemented in kernel space (for instance Ethernet, serial or
 	  mass storage) and other are implemented in user space.
 
+config USB_CONFIGFS_F_UAC1
+	boolean "Audio Class 1.0"
+	depends on USB_CONFIGFS
+	depends on SND
+	select USB_LIBCOMPOSITE
+	select SND_PCM
+	select USB_F_UAC1
+	help
+	  This Audio function implements 1 AudioControl interface,
+	  1 AudioStreaming Interface each for USB-OUT and USB-IN.
+	  This driver requires a real Audio codec to be present
+	  on the device.
+
+config USB_CONFIGFS_F_UAC2
+	boolean "Audio Class 2.0"
+	depends on USB_CONFIGFS
+	depends on SND
+	select USB_LIBCOMPOSITE
+	select SND_PCM
+	select USB_F_UAC2
+	help
+	  This Audio function is compatible with USB Audio Class
+	  specification 2.0. It implements 1 AudioControl interface,
+	  1 AudioStreaming Interface each for USB-OUT and USB-IN.
+	  This driver doesn't expect any real Audio codec to be present
+	  on the device - the audio streams are simply sinked to and
+	  sourced from a virtual ALSA sound card created. The user-space
+	  application may choose to do whatever it wants with the data
+	  received from the USB Host and choose to provide whatever it
+	  wants as audio data to the USB Host.
+
+config USB_CONFIGFS_F_MIDI
+	boolean "MIDI function"
+	depends on USB_CONFIGFS
+	depends on SND
+	select USB_LIBCOMPOSITE
+	select SND_RAWMIDI
+	select USB_F_MIDI
+	help
+	  The MIDI Function acts as a USB Audio device, with one MIDI
+	  input and one MIDI output. These MIDI jacks appear as
+	  a sound "card" in the ALSA sound system. Other MIDI
+	  connections can then be made on the gadget system, using
+	  ALSA's aconnect utility etc.
+
+config USB_CONFIGFS_F_HID
+	boolean "HID function"
+	depends on USB_CONFIGFS
+	select USB_F_HID
+	help
+	  The HID function driver provides generic emulation of USB
+	  Human Interface Devices (HID).
+
+	  For more information, see Documentation/usb/gadget_hid.txt.
+
 source "drivers/usb/gadget/legacy/Kconfig"
 
 endchoice
diff --git a/drivers/usb/gadget/composite.c b/drivers/usb/gadget/composite.c
index f6a51fddd5b5..617835348569 100644
--- a/drivers/usb/gadget/composite.c
+++ b/drivers/usb/gadget/composite.c
@@ -1246,10 +1246,49 @@ EXPORT_SYMBOL_GPL(usb_string_ids_n);
 
 static void composite_setup_complete(struct usb_ep *ep, struct usb_request *req)
 {
+	struct usb_composite_dev *cdev;
+
 	if (req->status || req->actual != req->length)
 		DBG((struct usb_composite_dev *) ep->driver_data,
 				"setup complete --> %d, %d/%d\n",
 				req->status, req->actual, req->length);
+
+	/*
+	 * REVIST The same ep0 requests are shared with function drivers
+	 * so they don't have to maintain the same ->complete() stubs.
+	 *
+	 * Because of that, we need to check for the validity of ->context
+	 * here, even though we know we've set it to something useful.
+	 */
+	if (!req->context)
+		return;
+
+	cdev = req->context;
+
+	if (cdev->req == req)
+		cdev->setup_pending = false;
+	else if (cdev->os_desc_req == req)
+		cdev->os_desc_pending = false;
+	else
+		WARN(1, "unknown request %p\n", req);
+}
+
+static int composite_ep0_queue(struct usb_composite_dev *cdev,
+		struct usb_request *req, gfp_t gfp_flags)
+{
+	int ret;
+
+	ret = usb_ep_queue(cdev->gadget->ep0, req, gfp_flags);
+	if (ret == 0) {
+		if (cdev->req == req)
+			cdev->setup_pending = true;
+		else if (cdev->os_desc_req == req)
+			cdev->os_desc_pending = true;
+		else
+			WARN(1, "unknown request %p\n", req);
+	}
+
+	return ret;
 }
 
 static int count_ext_compat(struct usb_configuration *c)
@@ -1428,6 +1467,7 @@ composite_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 	 * when we delegate to it.
 	 */
 	req->zero = 0;
+	req->context = cdev;
 	req->complete = composite_setup_complete;
 	req->length = 0;
 	gadget->ep0->driver_data = cdev;
@@ -1624,6 +1664,7 @@ unknown:
 			int				count = 0;
 
 			req = cdev->os_desc_req;
+			req->context = cdev;
 			req->complete = composite_setup_complete;
 			buf = req->buf;
 			os_desc_cfg = cdev->os_desc_config;
@@ -1686,8 +1727,9 @@ unknown:
 				break;
 			}
 			req->length = value;
+			req->context = cdev;
 			req->zero = value < w_length;
-			value = usb_ep_queue(gadget->ep0, req, GFP_ATOMIC);
+			value = composite_ep0_queue(cdev, req, GFP_ATOMIC);
 			if (value < 0) {
 				DBG(cdev, "ep_queue --> %d\n", value);
 				req->status = 0;
@@ -1757,8 +1799,9 @@ unknown:
 	/* respond with data transfer before status phase? */
 	if (value >= 0 && value != USB_GADGET_DELAYED_STATUS) {
 		req->length = value;
+		req->context = cdev;
 		req->zero = value < w_length;
-		value = usb_ep_queue(gadget->ep0, req, GFP_ATOMIC);
+		value = composite_ep0_queue(cdev, req, GFP_ATOMIC);
 		if (value < 0) {
 			DBG(cdev, "ep_queue --> %d\n", value);
 			req->status = 0;
@@ -1893,6 +1936,7 @@ int composite_dev_prepare(struct usb_composite_driver *composite,
 		goto fail_dev;
 
 	cdev->req->complete = composite_setup_complete;
+	cdev->req->context = cdev;
 	gadget->ep0->driver_data = cdev;
 
 	cdev->driver = composite;
@@ -1937,6 +1981,7 @@ int composite_os_desc_req_prepare(struct usb_composite_dev *cdev,
 		kfree(cdev->os_desc_req);
 		goto end;
 	}
+	cdev->os_desc_req->context = cdev;
 	cdev->os_desc_req->complete = composite_setup_complete;
 end:
 	return ret;
@@ -1951,10 +1996,16 @@ void composite_dev_cleanup(struct usb_composite_dev *cdev)
 		kfree(uc);
 	}
 	if (cdev->os_desc_req) {
+		if (cdev->os_desc_pending)
+			usb_ep_dequeue(cdev->gadget->ep0, cdev->os_desc_req);
+
 		kfree(cdev->os_desc_req->buf);
 		usb_ep_free_request(cdev->gadget->ep0, cdev->os_desc_req);
 	}
 	if (cdev->req) {
+		if (cdev->setup_pending)
+			usb_ep_dequeue(cdev->gadget->ep0, cdev->req);
+
 		kfree(cdev->req->buf);
 		usb_ep_free_request(cdev->gadget->ep0, cdev->req);
 	}
@@ -2013,8 +2064,7 @@ fail:
 
 /*-------------------------------------------------------------------------*/
 
-static void
-composite_suspend(struct usb_gadget *gadget)
+void composite_suspend(struct usb_gadget *gadget)
 {
 	struct usb_composite_dev	*cdev = get_gadget_data(gadget);
 	struct usb_function		*f;
@@ -2037,8 +2087,7 @@ composite_suspend(struct usb_gadget *gadget)
 	usb_gadget_vbus_draw(gadget, 2);
 }
 
-static void
-composite_resume(struct usb_gadget *gadget)
+void composite_resume(struct usb_gadget *gadget)
 {
 	struct usb_composite_dev	*cdev = get_gadget_data(gadget);
 	struct usb_function		*f;
@@ -2158,7 +2207,8 @@ void usb_composite_setup_continue(struct usb_composite_dev *cdev)
 	} else if (--cdev->delayed_status == 0) {
 		DBG(cdev, "%s: Completing delayed status\n", __func__);
 		req->length = 0;
-		value = usb_ep_queue(cdev->gadget->ep0, req, GFP_ATOMIC);
+		req->context = cdev;
+		value = composite_ep0_queue(cdev, req, GFP_ATOMIC);
 		if (value < 0) {
 			DBG(cdev, "ep_queue --> %d\n", value);
 			req->status = 0;
diff --git a/drivers/usb/gadget/configfs.c b/drivers/usb/gadget/configfs.c
index 34034333f7f6..75648145dc1b 100644
--- a/drivers/usb/gadget/configfs.c
+++ b/drivers/usb/gadget/configfs.c
@@ -271,7 +271,7 @@ static ssize_t gadget_dev_desc_UDC_store(struct gadget_info *gi,
 			ret = -EBUSY;
 			goto err;
 		}
-		ret = udc_attach_driver(name, &gi->composite.gadget_driver);
+		ret = usb_udc_attach_driver(name, &gi->composite.gadget_driver);
 		if (ret)
 			goto err;
 		gi->udc_name = name;
@@ -1453,6 +1453,9 @@ static const struct usb_gadget_driver configfs_driver_template = {
 	.reset          = composite_disconnect,
 	.disconnect     = composite_disconnect,
 
+	.suspend	= composite_suspend,
+	.resume		= composite_resume,
+
 	.max_speed	= USB_SPEED_SUPER,
 	.driver = {
 		.owner          = THIS_MODULE,
diff --git a/drivers/usb/gadget/function/Makefile b/drivers/usb/gadget/function/Makefile
index 90701aa5a826..dd68091d92f0 100644
--- a/drivers/usb/gadget/function/Makefile
+++ b/drivers/usb/gadget/function/Makefile
@@ -38,3 +38,7 @@ usb_f_uac2-y			:= f_uac2.o
 obj-$(CONFIG_USB_F_UAC2)	+= usb_f_uac2.o
 usb_f_uvc-y			:= f_uvc.o uvc_queue.o uvc_v4l2.o uvc_video.o
 obj-$(CONFIG_USB_F_UVC)		+= usb_f_uvc.o
+usb_f_midi-y			:= f_midi.o
+obj-$(CONFIG_USB_F_MIDI)	+= usb_f_midi.o
+usb_f_hid-y			:= f_hid.o
+obj-$(CONFIG_USB_F_HID)		+= usb_f_hid.o
diff --git a/drivers/usb/gadget/function/f_hid.c b/drivers/usb/gadget/function/f_hid.c
index 59ab62c92b66..6e04e302dc3a 100644
--- a/drivers/usb/gadget/function/f_hid.c
+++ b/drivers/usb/gadget/function/f_hid.c
@@ -12,6 +12,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/hid.h>
+#include <linux/idr.h>
 #include <linux/cdev.h>
 #include <linux/mutex.h>
 #include <linux/poll.h>
@@ -21,9 +22,14 @@
 #include <linux/usb/g_hid.h>
 
 #include "u_f.h"
+#include "u_hid.h"
+
+#define HIDG_MINORS	4
 
 static int major, minors;
 static struct class *hidg_class;
+static DEFINE_IDA(hidg_ida);
+static DEFINE_MUTEX(hidg_ida_lock); /* protects access to hidg_ida */
 
 /*-------------------------------------------------------------------------*/
 /*                            HID gadget struct                            */
@@ -161,6 +167,26 @@ static struct usb_descriptor_header *hidg_fs_descriptors[] = {
 };
 
 /*-------------------------------------------------------------------------*/
+/*                                 Strings                                 */
+
+#define CT_FUNC_HID_IDX	0
+
+static struct usb_string ct_func_string_defs[] = {
+	[CT_FUNC_HID_IDX].s	= "HID Interface",
+	{},			/* end of list */
+};
+
+static struct usb_gadget_strings ct_func_string_table = {
+	.language	= 0x0409,	/* en-US */
+	.strings	= ct_func_string_defs,
+};
+
+static struct usb_gadget_strings *ct_func_strings[] = {
+	&ct_func_string_table,
+	NULL,
+};
+
+/*-------------------------------------------------------------------------*/
 /*                              Char Device                                */
 
 static ssize_t f_hidg_read(struct file *file, char __user *buffer,
@@ -396,7 +422,7 @@ static int hidg_setup(struct usb_function *f,
 
 	case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8
 		  | HID_REQ_SET_REPORT):
-		VDBG(cdev, "set_report | wLenght=%d\n", ctrl->wLength);
+		VDBG(cdev, "set_report | wLength=%d\n", ctrl->wLength);
 		goto stall;
 		break;
 
@@ -552,13 +578,22 @@ const struct file_operations f_hidg_fops = {
 	.llseek		= noop_llseek,
 };
 
-static int __init hidg_bind(struct usb_configuration *c, struct usb_function *f)
+static int hidg_bind(struct usb_configuration *c, struct usb_function *f)
 {
 	struct usb_ep		*ep;
 	struct f_hidg		*hidg = func_to_hidg(f);
+	struct usb_string	*us;
+	struct device		*device;
 	int			status;
 	dev_t			dev;
 
+	/* maybe allocate device-global string IDs, and patch descriptors */
+	us = usb_gstrings_attach(c->cdev, ct_func_strings,
+				 ARRAY_SIZE(ct_func_string_defs));
+	if (IS_ERR(us))
+		return PTR_ERR(us);
+	hidg_interface_desc.iInterface = us[CT_FUNC_HID_IDX].id;
+
 	/* allocate instance-specific interface IDs, and patch descriptors */
 	status = usb_interface_id(c, f);
 	if (status < 0)
@@ -623,10 +658,16 @@ static int __init hidg_bind(struct usb_configuration *c, struct usb_function *f)
 	if (status)
 		goto fail_free_descs;
 
-	device_create(hidg_class, NULL, dev, NULL, "%s%d", "hidg", hidg->minor);
+	device = device_create(hidg_class, NULL, dev, NULL,
+			       "%s%d", "hidg", hidg->minor);
+	if (IS_ERR(device)) {
+		status = PTR_ERR(device);
+		goto del;
+	}
 
 	return 0;
-
+del:
+	cdev_del(&hidg->cdev);
 fail_free_descs:
 	usb_free_all_descriptors(f);
 fail:
@@ -640,116 +681,313 @@ fail:
 	return status;
 }
 
-static void hidg_unbind(struct usb_configuration *c, struct usb_function *f)
+static inline int hidg_get_minor(void)
 {
-	struct f_hidg *hidg = func_to_hidg(f);
+	int ret;
 
-	device_destroy(hidg_class, MKDEV(major, hidg->minor));
-	cdev_del(&hidg->cdev);
+	ret = ida_simple_get(&hidg_ida, 0, 0, GFP_KERNEL);
 
-	/* disable/free request and end point */
-	usb_ep_disable(hidg->in_ep);
-	usb_ep_dequeue(hidg->in_ep, hidg->req);
-	kfree(hidg->req->buf);
-	usb_ep_free_request(hidg->in_ep, hidg->req);
-
-	usb_free_all_descriptors(f);
+	return ret;
+}
 
-	kfree(hidg->report_desc);
-	kfree(hidg);
+static inline struct f_hid_opts *to_f_hid_opts(struct config_item *item)
+{
+	return container_of(to_config_group(item), struct f_hid_opts,
+			    func_inst.group);
 }
 
-/*-------------------------------------------------------------------------*/
-/*                                 Strings                                 */
+CONFIGFS_ATTR_STRUCT(f_hid_opts);
+CONFIGFS_ATTR_OPS(f_hid_opts);
 
-#define CT_FUNC_HID_IDX	0
+static void hid_attr_release(struct config_item *item)
+{
+	struct f_hid_opts *opts = to_f_hid_opts(item);
 
-static struct usb_string ct_func_string_defs[] = {
-	[CT_FUNC_HID_IDX].s	= "HID Interface",
-	{},			/* end of list */
-};
+	usb_put_function_instance(&opts->func_inst);
+}
 
-static struct usb_gadget_strings ct_func_string_table = {
-	.language	= 0x0409,	/* en-US */
-	.strings	= ct_func_string_defs,
+static struct configfs_item_operations hidg_item_ops = {
+	.release	= hid_attr_release,
+	.show_attribute	= f_hid_opts_attr_show,
+	.store_attribute = f_hid_opts_attr_store,
 };
 
-static struct usb_gadget_strings *ct_func_strings[] = {
-	&ct_func_string_table,
+#define F_HID_OPT(name, prec, limit)					\
+static ssize_t f_hid_opts_##name##_show(struct f_hid_opts *opts, char *page)\
+{									\
+	int result;							\
+									\
+	mutex_lock(&opts->lock);					\
+	result = sprintf(page, "%d\n", opts->name);			\
+	mutex_unlock(&opts->lock);					\
+									\
+	return result;							\
+}									\
+									\
+static ssize_t f_hid_opts_##name##_store(struct f_hid_opts *opts,	\
+					 const char *page, size_t len)	\
+{									\
+	int ret;							\
+	u##prec num;							\
+									\
+	mutex_lock(&opts->lock);					\
+	if (opts->refcnt) {						\
+		ret = -EBUSY;						\
+		goto end;						\
+	}								\
+									\
+	ret = kstrtou##prec(page, 0, &num);				\
+	if (ret)							\
+		goto end;						\
+									\
+	if (num > limit) {						\
+		ret = -EINVAL;						\
+		goto end;						\
+	}								\
+	opts->name = num;						\
+	ret = len;							\
+									\
+end:									\
+	mutex_unlock(&opts->lock);					\
+	return ret;							\
+}									\
+									\
+static struct f_hid_opts_attribute f_hid_opts_##name =			\
+	__CONFIGFS_ATTR(name, S_IRUGO | S_IWUSR, f_hid_opts_##name##_show,\
+			f_hid_opts_##name##_store)
+
+F_HID_OPT(subclass, 8, 255);
+F_HID_OPT(protocol, 8, 255);
+F_HID_OPT(report_length, 16, 65536);
+
+static ssize_t f_hid_opts_report_desc_show(struct f_hid_opts *opts, char *page)
+{
+	int result;
+
+	mutex_lock(&opts->lock);
+	result = opts->report_desc_length;
+	memcpy(page, opts->report_desc, opts->report_desc_length);
+	mutex_unlock(&opts->lock);
+
+	return result;
+}
+
+static ssize_t f_hid_opts_report_desc_store(struct f_hid_opts *opts,
+					    const char *page, size_t len)
+{
+	int ret = -EBUSY;
+	char *d;
+
+	mutex_lock(&opts->lock);
+
+	if (opts->refcnt)
+		goto end;
+	if (len > PAGE_SIZE) {
+		ret = -ENOSPC;
+		goto end;
+	}
+	d = kmemdup(page, len, GFP_KERNEL);
+	if (!d) {
+		ret = -ENOMEM;
+		goto end;
+	}
+	kfree(opts->report_desc);
+	opts->report_desc = d;
+	opts->report_desc_length = len;
+	opts->report_desc_alloc = true;
+	ret = len;
+end:
+	mutex_unlock(&opts->lock);
+	return ret;
+}
+
+static struct f_hid_opts_attribute f_hid_opts_report_desc =
+	__CONFIGFS_ATTR(report_desc, S_IRUGO | S_IWUSR,
+			f_hid_opts_report_desc_show,
+			f_hid_opts_report_desc_store);
+
+static struct configfs_attribute *hid_attrs[] = {
+	&f_hid_opts_subclass.attr,
+	&f_hid_opts_protocol.attr,
+	&f_hid_opts_report_length.attr,
+	&f_hid_opts_report_desc.attr,
 	NULL,
 };
 
-/*-------------------------------------------------------------------------*/
-/*                             usb_configuration                           */
+static struct config_item_type hid_func_type = {
+	.ct_item_ops	= &hidg_item_ops,
+	.ct_attrs	= hid_attrs,
+	.ct_owner	= THIS_MODULE,
+};
 
-int __init hidg_bind_config(struct usb_configuration *c,
-			    struct hidg_func_descriptor *fdesc, int index)
+static inline void hidg_put_minor(int minor)
 {
-	struct f_hidg *hidg;
-	int status;
+	ida_simple_remove(&hidg_ida, minor);
+}
 
-	if (index >= minors)
-		return -ENOENT;
+static void hidg_free_inst(struct usb_function_instance *f)
+{
+	struct f_hid_opts *opts;
 
-	/* maybe allocate device-global string IDs, and patch descriptors */
-	if (ct_func_string_defs[CT_FUNC_HID_IDX].id == 0) {
-		status = usb_string_id(c->cdev);
-		if (status < 0)
-			return status;
-		ct_func_string_defs[CT_FUNC_HID_IDX].id = status;
-		hidg_interface_desc.iInterface = status;
+	opts = container_of(f, struct f_hid_opts, func_inst);
+
+	mutex_lock(&hidg_ida_lock);
+
+	hidg_put_minor(opts->minor);
+	if (idr_is_empty(&hidg_ida.idr))
+		ghid_cleanup();
+
+	mutex_unlock(&hidg_ida_lock);
+
+	if (opts->report_desc_alloc)
+		kfree(opts->report_desc);
+
+	kfree(opts);
+}
+
+static struct usb_function_instance *hidg_alloc_inst(void)
+{
+	struct f_hid_opts *opts;
+	struct usb_function_instance *ret;
+	int status = 0;
+
+	opts = kzalloc(sizeof(*opts), GFP_KERNEL);
+	if (!opts)
+		return ERR_PTR(-ENOMEM);
+	mutex_init(&opts->lock);
+	opts->func_inst.free_func_inst = hidg_free_inst;
+	ret = &opts->func_inst;
+
+	mutex_lock(&hidg_ida_lock);
+
+	if (idr_is_empty(&hidg_ida.idr)) {
+		status = ghid_setup(NULL, HIDG_MINORS);
+		if (status)  {
+			ret = ERR_PTR(status);
+			kfree(opts);
+			goto unlock;
+		}
+	}
+
+	opts->minor = hidg_get_minor();
+	if (opts->minor < 0) {
+		ret = ERR_PTR(opts->minor);
+		kfree(opts);
+		if (idr_is_empty(&hidg_ida.idr))
+			ghid_cleanup();
+		goto unlock;
 	}
+	config_group_init_type_name(&opts->func_inst.group, "", &hid_func_type);
+
+unlock:
+	mutex_unlock(&hidg_ida_lock);
+	return ret;
+}
+
+static void hidg_free(struct usb_function *f)
+{
+	struct f_hidg *hidg;
+	struct f_hid_opts *opts;
+
+	hidg = func_to_hidg(f);
+	opts = container_of(f->fi, struct f_hid_opts, func_inst);
+	kfree(hidg->report_desc);
+	kfree(hidg);
+	mutex_lock(&opts->lock);
+	--opts->refcnt;
+	mutex_unlock(&opts->lock);
+}
+
+static void hidg_unbind(struct usb_configuration *c, struct usb_function *f)
+{
+	struct f_hidg *hidg = func_to_hidg(f);
+
+	device_destroy(hidg_class, MKDEV(major, hidg->minor));
+	cdev_del(&hidg->cdev);
+
+	/* disable/free request and end point */
+	usb_ep_disable(hidg->in_ep);
+	usb_ep_dequeue(hidg->in_ep, hidg->req);
+	kfree(hidg->req->buf);
+	usb_ep_free_request(hidg->in_ep, hidg->req);
+
+	usb_free_all_descriptors(f);
+}
+
+static struct usb_function *hidg_alloc(struct usb_function_instance *fi)
+{
+	struct f_hidg *hidg;
+	struct f_hid_opts *opts;
 
 	/* allocate and initialize one new instance */
-	hidg = kzalloc(sizeof *hidg, GFP_KERNEL);
+	hidg = kzalloc(sizeof(*hidg), GFP_KERNEL);
 	if (!hidg)
-		return -ENOMEM;
-
-	hidg->minor = index;
-	hidg->bInterfaceSubClass = fdesc->subclass;
-	hidg->bInterfaceProtocol = fdesc->protocol;
-	hidg->report_length = fdesc->report_length;
-	hidg->report_desc_length = fdesc->report_desc_length;
-	hidg->report_desc = kmemdup(fdesc->report_desc,
-				    fdesc->report_desc_length,
-				    GFP_KERNEL);
-	if (!hidg->report_desc) {
-		kfree(hidg);
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
+
+	opts = container_of(fi, struct f_hid_opts, func_inst);
+
+	mutex_lock(&opts->lock);
+	++opts->refcnt;
+
+	hidg->minor = opts->minor;
+	hidg->bInterfaceSubClass = opts->subclass;
+	hidg->bInterfaceProtocol = opts->protocol;
+	hidg->report_length = opts->report_length;
+	hidg->report_desc_length = opts->report_desc_length;
+	if (opts->report_desc) {
+		hidg->report_desc = kmemdup(opts->report_desc,
+					    opts->report_desc_length,
+					    GFP_KERNEL);
+		if (!hidg->report_desc) {
+			kfree(hidg);
+			mutex_unlock(&opts->lock);
+			return ERR_PTR(-ENOMEM);
+		}
 	}
 
+	mutex_unlock(&opts->lock);
+
 	hidg->func.name    = "hid";
-	hidg->func.strings = ct_func_strings;
 	hidg->func.bind    = hidg_bind;
 	hidg->func.unbind  = hidg_unbind;
 	hidg->func.set_alt = hidg_set_alt;
 	hidg->func.disable = hidg_disable;
 	hidg->func.setup   = hidg_setup;
+	hidg->func.free_func = hidg_free;
 
 	/* this could me made configurable at some point */
 	hidg->qlen	   = 4;
 
-	status = usb_add_function(c, &hidg->func);
-	if (status)
-		kfree(hidg);
-
-	return status;
+	return &hidg->func;
 }
 
-int __init ghid_setup(struct usb_gadget *g, int count)
+DECLARE_USB_FUNCTION_INIT(hid, hidg_alloc_inst, hidg_alloc);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Fabien Chouteau");
+
+int ghid_setup(struct usb_gadget *g, int count)
 {
 	int status;
 	dev_t dev;
 
 	hidg_class = class_create(THIS_MODULE, "hidg");
+	if (IS_ERR(hidg_class)) {
+		status = PTR_ERR(hidg_class);
+		hidg_class = NULL;
+		return status;
+	}
 
 	status = alloc_chrdev_region(&dev, 0, count, "hidg");
-	if (!status) {
-		major = MAJOR(dev);
-		minors = count;
+	if (status) {
+		class_destroy(hidg_class);
+		hidg_class = NULL;
+		return status;
 	}
 
-	return status;
+	major = MAJOR(dev);
+	minors = count;
+
+	return 0;
 }
 
 void ghid_cleanup(void)
diff --git a/drivers/usb/gadget/function/f_midi.c b/drivers/usb/gadget/function/f_midi.c
index 807b31c0edc3..a90440300735 100644
--- a/drivers/usb/gadget/function/f_midi.c
+++ b/drivers/usb/gadget/function/f_midi.c
@@ -20,6 +20,7 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/device.h>
 
@@ -33,6 +34,7 @@
 #include <linux/usb/midi.h>
 
 #include "u_f.h"
+#include "u_midi.h"
 
 MODULE_AUTHOR("Ben Williamson");
 MODULE_LICENSE("GPL v2");
@@ -99,7 +101,7 @@ DECLARE_USB_MIDI_OUT_JACK_DESCRIPTOR(1);
 DECLARE_USB_MS_ENDPOINT_DESCRIPTOR(16);
 
 /* B.3.1  Standard AC Interface Descriptor */
-static struct usb_interface_descriptor ac_interface_desc __initdata = {
+static struct usb_interface_descriptor ac_interface_desc = {
 	.bLength =		USB_DT_INTERFACE_SIZE,
 	.bDescriptorType =	USB_DT_INTERFACE,
 	/* .bInterfaceNumber =	DYNAMIC */
@@ -110,7 +112,7 @@ static struct usb_interface_descriptor ac_interface_desc __initdata = {
 };
 
 /* B.3.2  Class-Specific AC Interface Descriptor */
-static struct uac1_ac_header_descriptor_1 ac_header_desc __initdata = {
+static struct uac1_ac_header_descriptor_1 ac_header_desc = {
 	.bLength =		UAC_DT_AC_HEADER_SIZE(1),
 	.bDescriptorType =	USB_DT_CS_INTERFACE,
 	.bDescriptorSubtype =	USB_MS_HEADER,
@@ -121,7 +123,7 @@ static struct uac1_ac_header_descriptor_1 ac_header_desc __initdata = {
 };
 
 /* B.4.1  Standard MS Interface Descriptor */
-static struct usb_interface_descriptor ms_interface_desc __initdata = {
+static struct usb_interface_descriptor ms_interface_desc = {
 	.bLength =		USB_DT_INTERFACE_SIZE,
 	.bDescriptorType =	USB_DT_INTERFACE,
 	/* .bInterfaceNumber =	DYNAMIC */
@@ -132,7 +134,7 @@ static struct usb_interface_descriptor ms_interface_desc __initdata = {
 };
 
 /* B.4.2  Class-Specific MS Interface Descriptor */
-static struct usb_ms_header_descriptor ms_header_desc __initdata = {
+static struct usb_ms_header_descriptor ms_header_desc = {
 	.bLength =		USB_DT_MS_HEADER_SIZE,
 	.bDescriptorType =	USB_DT_CS_INTERFACE,
 	.bDescriptorSubtype =	USB_MS_HEADER,
@@ -387,29 +389,6 @@ static void f_midi_disable(struct usb_function *f)
 	usb_ep_disable(midi->out_ep);
 }
 
-static void f_midi_unbind(struct usb_configuration *c, struct usb_function *f)
-{
-	struct usb_composite_dev *cdev = f->config->cdev;
-	struct f_midi *midi = func_to_midi(f);
-	struct snd_card *card;
-
-	DBG(cdev, "unbind\n");
-
-	/* just to be sure */
-	f_midi_disable(f);
-
-	card = midi->card;
-	midi->card = NULL;
-	if (card)
-		snd_card_free(card);
-
-	kfree(midi->id);
-	midi->id = NULL;
-
-	usb_free_all_descriptors(f);
-	kfree(midi);
-}
-
 static int f_midi_snd_free(struct snd_device *device)
 {
 	return 0;
@@ -654,6 +633,14 @@ static struct snd_rawmidi_ops gmidi_out_ops = {
 	.trigger = f_midi_out_trigger
 };
 
+static inline void f_midi_unregister_card(struct f_midi *midi)
+{
+	if (midi->card) {
+		snd_card_free(midi->card);
+		midi->card = NULL;
+	}
+}
+
 /* register as a sound "card" */
 static int f_midi_register_card(struct f_midi *midi)
 {
@@ -715,17 +702,13 @@ static int f_midi_register_card(struct f_midi *midi)
 	return 0;
 
 fail:
-	if (midi->card) {
-		snd_card_free(midi->card);
-		midi->card = NULL;
-	}
+	f_midi_unregister_card(midi);
 	return err;
 }
 
 /* MIDI function driver setup/binding */
 
-static int __init
-f_midi_bind(struct usb_configuration *c, struct usb_function *f)
+static int f_midi_bind(struct usb_configuration *c, struct usb_function *f)
 {
 	struct usb_descriptor_header **midi_function;
 	struct usb_midi_in_jack_descriptor jack_in_ext_desc[MAX_PORTS];
@@ -734,15 +717,23 @@ f_midi_bind(struct usb_configuration *c, struct usb_function *f)
 	struct usb_midi_out_jack_descriptor_1 jack_out_emb_desc[MAX_PORTS];
 	struct usb_composite_dev *cdev = c->cdev;
 	struct f_midi *midi = func_to_midi(f);
+	struct usb_string *us;
 	int status, n, jack = 1, i = 0;
 
+	midi->gadget = cdev->gadget;
+	tasklet_init(&midi->tasklet, f_midi_in_tasklet, (unsigned long) midi);
+	status = f_midi_register_card(midi);
+	if (status < 0)
+		goto fail_register;
+
 	/* maybe allocate device-global string ID */
-	if (midi_string_defs[0].id == 0) {
-		status = usb_string_id(c->cdev);
-		if (status < 0)
-			goto fail;
-		midi_string_defs[0].id = status;
+	us = usb_gstrings_attach(c->cdev, midi_strings,
+				 ARRAY_SIZE(midi_string_defs));
+	if (IS_ERR(us)) {
+		status = PTR_ERR(us);
+		goto fail;
 	}
+	ac_interface_desc.iInterface = us[STRING_FUNC_IDX].id;
 
 	/* We have two interfaces, AudioControl and MIDIStreaming */
 	status = usb_interface_id(c, f);
@@ -892,6 +883,8 @@ fail_f_midi:
 	kfree(midi_function);
 	usb_free_descriptors(f->hs_descriptors);
 fail:
+	f_midi_unregister_card(midi);
+fail_register:
 	/* we might as well release our claims on endpoints */
 	if (midi->out_ep)
 		midi->out_ep->driver_data = NULL;
@@ -903,42 +896,235 @@ fail:
 	return status;
 }
 
-/**
- * f_midi_bind_config - add USB MIDI function to a configuration
- * @c: the configuration to supcard the USB audio function
- * @index: the soundcard index to use for the ALSA device creation
- * @id: the soundcard id to use for the ALSA device creation
- * @buflen: the buffer length to use
- * @qlen the number of read requests to pre-allocate
- * Context: single threaded during gadget setup
- *
- * Returns zero on success, else negative errno.
- */
-int __init f_midi_bind_config(struct usb_configuration *c,
-			      int index, char *id,
-			      unsigned int in_ports,
-			      unsigned int out_ports,
-			      unsigned int buflen,
-			      unsigned int qlen)
+static inline struct f_midi_opts *to_f_midi_opts(struct config_item *item)
+{
+	return container_of(to_config_group(item), struct f_midi_opts,
+			    func_inst.group);
+}
+
+CONFIGFS_ATTR_STRUCT(f_midi_opts);
+CONFIGFS_ATTR_OPS(f_midi_opts);
+
+static void midi_attr_release(struct config_item *item)
+{
+	struct f_midi_opts *opts = to_f_midi_opts(item);
+
+	usb_put_function_instance(&opts->func_inst);
+}
+
+static struct configfs_item_operations midi_item_ops = {
+	.release	= midi_attr_release,
+	.show_attribute	= f_midi_opts_attr_show,
+	.store_attribute = f_midi_opts_attr_store,
+};
+
+#define F_MIDI_OPT(name, test_limit, limit)				\
+static ssize_t f_midi_opts_##name##_show(struct f_midi_opts *opts, char *page) \
+{									\
+	int result;							\
+									\
+	mutex_lock(&opts->lock);					\
+	result = sprintf(page, "%d\n", opts->name);			\
+	mutex_unlock(&opts->lock);					\
+									\
+	return result;							\
+}									\
+									\
+static ssize_t f_midi_opts_##name##_store(struct f_midi_opts *opts,	\
+					 const char *page, size_t len)	\
+{									\
+	int ret;							\
+	u32 num;							\
+									\
+	mutex_lock(&opts->lock);					\
+	if (opts->refcnt) {						\
+		ret = -EBUSY;						\
+		goto end;						\
+	}								\
+									\
+	ret = kstrtou32(page, 0, &num);					\
+	if (ret)							\
+		goto end;						\
+									\
+	if (test_limit && num > limit) {				\
+		ret = -EINVAL;						\
+		goto end;						\
+	}								\
+	opts->name = num;						\
+	ret = len;							\
+									\
+end:									\
+	mutex_unlock(&opts->lock);					\
+	return ret;							\
+}									\
+									\
+static struct f_midi_opts_attribute f_midi_opts_##name =		\
+	__CONFIGFS_ATTR(name, S_IRUGO | S_IWUSR, f_midi_opts_##name##_show, \
+			f_midi_opts_##name##_store)
+
+F_MIDI_OPT(index, true, SNDRV_CARDS);
+F_MIDI_OPT(buflen, false, 0);
+F_MIDI_OPT(qlen, false, 0);
+F_MIDI_OPT(in_ports, true, MAX_PORTS);
+F_MIDI_OPT(out_ports, true, MAX_PORTS);
+
+static ssize_t f_midi_opts_id_show(struct f_midi_opts *opts, char *page)
+{
+	int result;
+
+	mutex_lock(&opts->lock);
+	result = strlcpy(page, opts->id, PAGE_SIZE);
+	mutex_unlock(&opts->lock);
+
+	return result;
+}
+
+static ssize_t f_midi_opts_id_store(struct f_midi_opts *opts,
+				    const char *page, size_t len)
+{
+	int ret;
+	char *c;
+
+	mutex_lock(&opts->lock);
+	if (opts->refcnt) {
+		ret = -EBUSY;
+		goto end;
+	}
+
+	c = kstrndup(page, len, GFP_KERNEL);
+	if (!c) {
+		ret = -ENOMEM;
+		goto end;
+	}
+	if (opts->id_allocated)
+		kfree(opts->id);
+	opts->id = c;
+	opts->id_allocated = true;
+	ret = len;
+end:
+	mutex_unlock(&opts->lock);
+	return ret;
+}
+
+static struct f_midi_opts_attribute f_midi_opts_id =
+	__CONFIGFS_ATTR(id, S_IRUGO | S_IWUSR, f_midi_opts_id_show,
+			f_midi_opts_id_store);
+
+static struct configfs_attribute *midi_attrs[] = {
+	&f_midi_opts_index.attr,
+	&f_midi_opts_buflen.attr,
+	&f_midi_opts_qlen.attr,
+	&f_midi_opts_in_ports.attr,
+	&f_midi_opts_out_ports.attr,
+	&f_midi_opts_id.attr,
+	NULL,
+};
+
+static struct config_item_type midi_func_type = {
+	.ct_item_ops	= &midi_item_ops,
+	.ct_attrs	= midi_attrs,
+	.ct_owner	= THIS_MODULE,
+};
+
+static void f_midi_free_inst(struct usb_function_instance *f)
+{
+	struct f_midi_opts *opts;
+
+	opts = container_of(f, struct f_midi_opts, func_inst);
+
+	if (opts->id_allocated)
+		kfree(opts->id);
+
+	kfree(opts);
+}
+
+static struct usb_function_instance *f_midi_alloc_inst(void)
+{
+	struct f_midi_opts *opts;
+
+	opts = kzalloc(sizeof(*opts), GFP_KERNEL);
+	if (!opts)
+		return ERR_PTR(-ENOMEM);
+
+	mutex_init(&opts->lock);
+	opts->func_inst.free_func_inst = f_midi_free_inst;
+	opts->index = SNDRV_DEFAULT_IDX1;
+	opts->id = SNDRV_DEFAULT_STR1;
+	opts->buflen = 256;
+	opts->qlen = 32;
+	opts->in_ports = 1;
+	opts->out_ports = 1;
+
+	config_group_init_type_name(&opts->func_inst.group, "",
+				    &midi_func_type);
+
+	return &opts->func_inst;
+}
+
+static void f_midi_free(struct usb_function *f)
+{
+	struct f_midi *midi;
+	struct f_midi_opts *opts;
+	int i;
+
+	midi = func_to_midi(f);
+	opts = container_of(f->fi, struct f_midi_opts, func_inst);
+	kfree(midi->id);
+	mutex_lock(&opts->lock);
+	for (i = opts->in_ports - 1; i >= 0; --i)
+		kfree(midi->in_port[i]);
+	kfree(midi);
+	--opts->refcnt;
+	mutex_unlock(&opts->lock);
+}
+
+static void f_midi_unbind(struct usb_configuration *c, struct usb_function *f)
+{
+	struct usb_composite_dev *cdev = f->config->cdev;
+	struct f_midi *midi = func_to_midi(f);
+	struct snd_card *card;
+
+	DBG(cdev, "unbind\n");
+
+	/* just to be sure */
+	f_midi_disable(f);
+
+	card = midi->card;
+	midi->card = NULL;
+	if (card)
+		snd_card_free(card);
+
+	usb_free_all_descriptors(f);
+}
+
+static struct usb_function *f_midi_alloc(struct usb_function_instance *fi)
 {
 	struct f_midi *midi;
+	struct f_midi_opts *opts;
 	int status, i;
 
+	opts = container_of(fi, struct f_midi_opts, func_inst);
+
+	mutex_lock(&opts->lock);
 	/* sanity check */
-	if (in_ports > MAX_PORTS || out_ports > MAX_PORTS)
-		return -EINVAL;
+	if (opts->in_ports > MAX_PORTS || opts->out_ports > MAX_PORTS) {
+		mutex_unlock(&opts->lock);
+		return ERR_PTR(-EINVAL);
+	}
 
 	/* allocate and initialize one new instance */
-	midi = kzalloc(sizeof *midi, GFP_KERNEL);
+	midi = kzalloc(sizeof(*midi), GFP_KERNEL);
 	if (!midi) {
-		status = -ENOMEM;
-		goto fail;
+		mutex_unlock(&opts->lock);
+		return ERR_PTR(-ENOMEM);
 	}
 
-	for (i = 0; i < in_ports; i++) {
+	for (i = 0; i < opts->in_ports; i++) {
 		struct gmidi_in_port *port = kzalloc(sizeof(*port), GFP_KERNEL);
+
 		if (!port) {
 			status = -ENOMEM;
+			mutex_unlock(&opts->lock);
 			goto setup_fail;
 		}
 
@@ -948,39 +1134,37 @@ int __init f_midi_bind_config(struct usb_configuration *c,
 		midi->in_port[i] = port;
 	}
 
-	midi->gadget = c->cdev->gadget;
-	tasklet_init(&midi->tasklet, f_midi_in_tasklet, (unsigned long) midi);
-
 	/* set up ALSA midi devices */
-	midi->in_ports = in_ports;
-	midi->out_ports = out_ports;
-	status = f_midi_register_card(midi);
-	if (status < 0)
-		goto setup_fail;
-
-	midi->func.name        = "gmidi function";
-	midi->func.strings     = midi_strings;
-	midi->func.bind        = f_midi_bind;
-	midi->func.unbind      = f_midi_unbind;
-	midi->func.set_alt     = f_midi_set_alt;
-	midi->func.disable     = f_midi_disable;
-
-	midi->id = kstrdup(id, GFP_KERNEL);
-	midi->index = index;
-	midi->buflen = buflen;
-	midi->qlen = qlen;
-
-	status = usb_add_function(c, &midi->func);
-	if (status)
-		goto setup_fail;
-
-	return 0;
-
+	midi->id = kstrdup(opts->id, GFP_KERNEL);
+	if (opts->id && !midi->id) {
+		status = -ENOMEM;
+		mutex_unlock(&opts->lock);
+		goto kstrdup_fail;
+	}
+	midi->in_ports = opts->in_ports;
+	midi->out_ports = opts->out_ports;
+	midi->index = opts->index;
+	midi->buflen = opts->buflen;
+	midi->qlen = opts->qlen;
+	++opts->refcnt;
+	mutex_unlock(&opts->lock);
+
+	midi->func.name		= "gmidi function";
+	midi->func.bind		= f_midi_bind;
+	midi->func.unbind	= f_midi_unbind;
+	midi->func.set_alt	= f_midi_set_alt;
+	midi->func.disable	= f_midi_disable;
+	midi->func.free_func	= f_midi_free;
+
+	return &midi->func;
+
+kstrdup_fail:
+	f_midi_unregister_card(midi);
 setup_fail:
 	for (--i; i >= 0; i--)
 		kfree(midi->in_port[i]);
 	kfree(midi);
-fail:
-	return status;
+	return ERR_PTR(status);
 }
 
+DECLARE_USB_FUNCTION_INIT(midi, f_midi_alloc_inst, f_midi_alloc);
diff --git a/drivers/usb/gadget/function/f_ncm.c b/drivers/usb/gadget/function/f_ncm.c
index 16361b0a8b46..bdcda9f5148e 100644
--- a/drivers/usb/gadget/function/f_ncm.c
+++ b/drivers/usb/gadget/function/f_ncm.c
@@ -1441,6 +1441,9 @@ static int ncm_bind(struct usb_configuration *c, struct usb_function *f)
 
 	status = usb_assign_descriptors(f, ncm_fs_function, ncm_hs_function,
 			NULL);
+	if (status)
+		goto fail;
+
 	/*
 	 * NOTE:  all that is done without knowing or caring about
 	 * the network link ... which is unavailable to this code
diff --git a/drivers/usb/gadget/function/f_rndis.c b/drivers/usb/gadget/function/f_rndis.c
index f13fc6a58565..829edf878dac 100644
--- a/drivers/usb/gadget/function/f_rndis.c
+++ b/drivers/usb/gadget/function/f_rndis.c
@@ -375,8 +375,7 @@ static struct sk_buff *rndis_add_header(struct gether *port,
 	struct sk_buff *skb2;
 
 	skb2 = skb_realloc_headroom(skb, sizeof(struct rndis_packet_msg_type));
-	if (skb2)
-		rndis_add_hdr(skb2);
+	rndis_add_hdr(skb2);
 
 	dev_kfree_skb(skb);
 	return skb2;
diff --git a/drivers/usb/gadget/function/u_hid.h b/drivers/usb/gadget/function/u_hid.h
new file mode 100644
index 000000000000..aaa0e368a159
--- /dev/null
+++ b/drivers/usb/gadget/function/u_hid.h
@@ -0,0 +1,42 @@
+/*
+ * u_hid.h
+ *
+ * Utility definitions for the hid function
+ *
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Author: Andrzej Pietrasiewicz <andrzej.p@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef U_HID_H
+#define U_HID_H
+
+#include <linux/usb/composite.h>
+
+struct f_hid_opts {
+	struct usb_function_instance	func_inst;
+	int				minor;
+	unsigned char			subclass;
+	unsigned char			protocol;
+	unsigned short			report_length;
+	unsigned short			report_desc_length;
+	unsigned char			*report_desc;
+	bool				report_desc_alloc;
+
+	/*
+	 * Protect the data form concurrent access by read/write
+	 * and create symlink/remove symlink.
+	 */
+	 struct mutex			lock;
+	 int				refcnt;
+};
+
+int ghid_setup(struct usb_gadget *g, int count);
+void ghid_cleanup(void);
+
+#endif /* U_HID_H */
diff --git a/drivers/usb/gadget/function/u_midi.h b/drivers/usb/gadget/function/u_midi.h
new file mode 100644
index 000000000000..22510189758e
--- /dev/null
+++ b/drivers/usb/gadget/function/u_midi.h
@@ -0,0 +1,40 @@
+/*
+ * u_midi.h
+ *
+ * Utility definitions for the midi function
+ *
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Author: Andrzej Pietrasiewicz <andrzej.p@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef U_MIDI_H
+#define U_MIDI_H
+
+#include <linux/usb/composite.h>
+
+struct f_midi_opts {
+	struct usb_function_instance	func_inst;
+	int				index;
+	char				*id;
+	bool				id_allocated;
+	unsigned int			in_ports;
+	unsigned int			out_ports;
+	unsigned int			buflen;
+	unsigned int			qlen;
+
+	/*
+	 * Protect the data form concurrent access by read/write
+	 * and create symlink/remove symlink.
+	 */
+	 struct mutex			lock;
+	 int				refcnt;
+};
+
+#endif /* U_MIDI_H */
+
diff --git a/drivers/usb/gadget/function/u_uac1.c b/drivers/usb/gadget/function/u_uac1.c
index a44a07f30281..53842a1b947f 100644
--- a/drivers/usb/gadget/function/u_uac1.c
+++ b/drivers/usb/gadget/function/u_uac1.c
@@ -213,9 +213,6 @@ static int gaudio_open_snd_dev(struct gaudio *card)
 	fn_cap = opts->fn_cap;
 	fn_cntl = opts->fn_cntl;
 
-	if (!card)
-		return -ENODEV;
-
 	/* Open control device */
 	snd = &card->control;
 	snd->filp = filp_open(fn_cntl, O_RDWR, 0);
diff --git a/drivers/usb/gadget/legacy/Kconfig b/drivers/usb/gadget/legacy/Kconfig
index 24392d269709..fd48ef3af4eb 100644
--- a/drivers/usb/gadget/legacy/Kconfig
+++ b/drivers/usb/gadget/legacy/Kconfig
@@ -287,6 +287,7 @@ config USB_MIDI_GADGET
 	depends on SND
 	select USB_LIBCOMPOSITE
 	select SND_RAWMIDI
+	select USB_F_MIDI
 	help
 	  The MIDI Gadget acts as a USB Audio device, with one MIDI
 	  input and one MIDI output. These MIDI jacks appear as
@@ -419,6 +420,7 @@ endif # TTY
 config USB_G_HID
 	tristate "HID Gadget"
 	select USB_LIBCOMPOSITE
+	select USB_F_HID
 	help
 	  The HID gadget driver provides generic emulation of USB
 	  Human Interface Devices (HID).
diff --git a/drivers/usb/gadget/legacy/dbgp.c b/drivers/usb/gadget/legacy/dbgp.c
index 1b075132f8f1..633683a72a11 100644
--- a/drivers/usb/gadget/legacy/dbgp.c
+++ b/drivers/usb/gadget/legacy/dbgp.c
@@ -237,7 +237,7 @@ static void dbgp_unbind(struct usb_gadget *gadget)
 static unsigned char tty_line;
 #endif
 
-static int __init dbgp_configure_endpoints(struct usb_gadget *gadget)
+static int dbgp_configure_endpoints(struct usb_gadget *gadget)
 {
 	int stp;
 
@@ -273,19 +273,10 @@ static int __init dbgp_configure_endpoints(struct usb_gadget *gadget)
 
 	dbgp.serial->in->desc = &i_desc;
 	dbgp.serial->out->desc = &o_desc;
-
-	if (gserial_alloc_line(&tty_line)) {
-		stp = 3;
-		goto fail_3;
-	}
+#endif
 
 	return 0;
 
-fail_3:
-	dbgp.o_ep->driver_data = NULL;
-#else
-	return 0;
-#endif
 fail_2:
 	dbgp.i_ep->driver_data = NULL;
 fail_1:
@@ -324,10 +315,17 @@ static int __init dbgp_bind(struct usb_gadget *gadget,
 		err = -ENOMEM;
 		goto fail;
 	}
+
+	if (gserial_alloc_line(&tty_line)) {
+		stp = 4;
+		err = -ENODEV;
+		goto fail;
+	}
 #endif
+
 	err = dbgp_configure_endpoints(gadget);
 	if (err < 0) {
-		stp = 4;
+		stp = 5;
 		goto fail;
 	}
 
@@ -383,6 +381,10 @@ static int dbgp_setup(struct usb_gadget *gadget,
 #ifdef CONFIG_USB_G_DBGP_PRINTK
 		err = dbgp_enable_ep();
 #else
+		err = dbgp_configure_endpoints(gadget);
+		if (err < 0) {
+			goto fail;
+		}
 		err = gserial_connect(dbgp.serial, tty_line);
 #endif
 		if (err < 0)
diff --git a/drivers/usb/gadget/legacy/gmidi.c b/drivers/usb/gadget/legacy/gmidi.c
index 3d696b86ff76..e02a095294ac 100644
--- a/drivers/usb/gadget/legacy/gmidi.c
+++ b/drivers/usb/gadget/legacy/gmidi.c
@@ -37,7 +37,7 @@
 
 #include "gadget_chips.h"
 
-#include "f_midi.c"
+#include "u_midi.h"
 
 /*-------------------------------------------------------------------------*/
 
@@ -115,8 +115,13 @@ static struct usb_gadget_strings *dev_strings[] = {
 	NULL,
 };
 
+static struct usb_function_instance *fi_midi;
+static struct usb_function *f_midi;
+
 static int __exit midi_unbind(struct usb_composite_dev *dev)
 {
+	usb_put_function(f_midi);
+	usb_put_function_instance(fi_midi);
 	return 0;
 }
 
@@ -130,28 +135,54 @@ static struct usb_configuration midi_config = {
 
 static int __init midi_bind_config(struct usb_configuration *c)
 {
-	return f_midi_bind_config(c, index, id,
-				  in_ports, out_ports,
-				  buflen, qlen);
+	int status;
+
+	f_midi = usb_get_function(fi_midi);
+	if (IS_ERR(f_midi))
+		return PTR_ERR(f_midi);
+
+	status = usb_add_function(c, f_midi);
+	if (status < 0) {
+		usb_put_function(f_midi);
+		return status;
+	}
+
+	return 0;
 }
 
 static int __init midi_bind(struct usb_composite_dev *cdev)
 {
+	struct f_midi_opts *midi_opts;
 	int status;
 
+	fi_midi = usb_get_function_instance("midi");
+	if (IS_ERR(fi_midi))
+		return PTR_ERR(fi_midi);
+
+	midi_opts = container_of(fi_midi, struct f_midi_opts, func_inst);
+	midi_opts->index = index;
+	midi_opts->id = id;
+	midi_opts->in_ports = in_ports;
+	midi_opts->out_ports = out_ports;
+	midi_opts->buflen = buflen;
+	midi_opts->qlen = qlen;
+
 	status = usb_string_ids_tab(cdev, strings_dev);
 	if (status < 0)
-		return status;
+		goto put;
 	device_desc.iManufacturer = strings_dev[USB_GADGET_MANUFACTURER_IDX].id;
 	device_desc.iProduct = strings_dev[USB_GADGET_PRODUCT_IDX].id;
 	midi_config.iConfiguration = strings_dev[STRING_DESCRIPTION_IDX].id;
 
 	status = usb_add_config(cdev, &midi_config, midi_bind_config);
 	if (status < 0)
-		return status;
+		goto put;
 	usb_composite_overwrite_options(cdev, &coverwrite);
 	pr_info("%s\n", longname);
 	return 0;
+put:
+	usb_put_function_instance(fi_midi);
+	return status;
 }
 
 static __refdata struct usb_composite_driver midi_driver = {
diff --git a/drivers/usb/gadget/legacy/hid.c b/drivers/usb/gadget/legacy/hid.c
index 778613eb37af..614b06d80b41 100644
--- a/drivers/usb/gadget/legacy/hid.c
+++ b/drivers/usb/gadget/legacy/hid.c
@@ -17,11 +17,14 @@
 #include <linux/list.h>
 #include <linux/module.h>
 #include <linux/usb/composite.h>
+#include <linux/usb/g_hid.h>
 
 #include "gadget_chips.h"
 #define DRIVER_DESC		"HID Gadget"
 #define DRIVER_VERSION		"2010/03/16"
 
+#include "u_hid.h"
+
 /*-------------------------------------------------------------------------*/
 
 #define HIDG_VENDOR_NUM		0x0525	/* XXX NetChip */
@@ -29,17 +32,9 @@
 
 /*-------------------------------------------------------------------------*/
 
-/*
- * 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 "f_hid.c"
-
-
 struct hidg_func_node {
+	struct usb_function_instance *fi;
+	struct usb_function *f;
 	struct list_head node;
 	struct hidg_func_descriptor *func;
 };
@@ -113,8 +108,8 @@ static struct usb_gadget_strings *dev_strings[] = {
 
 static int __init do_config(struct usb_configuration *c)
 {
-	struct hidg_func_node *e;
-	int func = 0, status = 0;
+	struct hidg_func_node *e, *n;
+	int status = 0;
 
 	if (gadget_is_otg(c->cdev->gadget)) {
 		c->descriptors = otg_desc;
@@ -122,11 +117,24 @@ static int __init do_config(struct usb_configuration *c)
 	}
 
 	list_for_each_entry(e, &hidg_func_list, node) {
-		status = hidg_bind_config(c, e->func, func++);
-		if (status)
-			break;
+		e->f = usb_get_function(e->fi);
+		if (IS_ERR(e->f))
+			goto put;
+		status = usb_add_function(c, e->f);
+		if (status < 0) {
+			usb_put_function(e->f);
+			goto put;
+		}
 	}
 
+	return 0;
+put:
+	list_for_each_entry(n, &hidg_func_list, node) {
+		if (n == e)
+			break;
+		usb_remove_function(c, n->f);
+		usb_put_function(n->f);
+	}
 	return status;
 }
 
@@ -143,6 +151,8 @@ static int __init hid_bind(struct usb_composite_dev *cdev)
 {
 	struct usb_gadget *gadget = cdev->gadget;
 	struct list_head *tmp;
+	struct hidg_func_node *n, *m;
+	struct f_hid_opts *hid_opts;
 	int status, funcs = 0;
 
 	list_for_each(tmp, &hidg_func_list)
@@ -151,10 +161,20 @@ static int __init hid_bind(struct usb_composite_dev *cdev)
 	if (!funcs)
 		return -ENODEV;
 
-	/* set up HID */
-	status = ghid_setup(cdev->gadget, funcs);
-	if (status < 0)
-		return status;
+	list_for_each_entry(n, &hidg_func_list, node) {
+		n->fi = usb_get_function_instance("hid");
+		if (IS_ERR(n->fi)) {
+			status = PTR_ERR(n->fi);
+			goto put;
+		}
+		hid_opts = container_of(n->fi, struct f_hid_opts, func_inst);
+		hid_opts->subclass = n->func->subclass;
+		hid_opts->protocol = n->func->protocol;
+		hid_opts->report_length = n->func->report_length;
+		hid_opts->report_desc_length = n->func->report_desc_length;
+		hid_opts->report_desc = n->func->report_desc;
+	}
+
 
 	/* Allocate string descriptor numbers ... note that string
 	 * contents can be overridden by the composite_dev glue.
@@ -162,24 +182,37 @@ static int __init hid_bind(struct usb_composite_dev *cdev)
 
 	status = usb_string_ids_tab(cdev, strings_dev);
 	if (status < 0)
-		return status;
+		goto put;
 	device_desc.iManufacturer = strings_dev[USB_GADGET_MANUFACTURER_IDX].id;
 	device_desc.iProduct = strings_dev[USB_GADGET_PRODUCT_IDX].id;
 
 	/* register our configuration */
 	status = usb_add_config(cdev, &config_driver, do_config);
 	if (status < 0)
-		return status;
+		goto put;
 
 	usb_composite_overwrite_options(cdev, &coverwrite);
 	dev_info(&gadget->dev, DRIVER_DESC ", version: " DRIVER_VERSION "\n");
 
 	return 0;
+
+put:
+	list_for_each_entry(m, &hidg_func_list, node) {
+		if (m == n)
+			break;
+		usb_put_function_instance(m->fi);
+	}
+	return status;
 }
 
 static int __exit hid_unbind(struct usb_composite_dev *cdev)
 {
-	ghid_cleanup();
+	struct hidg_func_node *n;
+
+	list_for_each_entry(n, &hidg_func_list, node) {
+		usb_put_function(n->f);
+		usb_put_function_instance(n->fi);
+	}
 	return 0;
 }
 
@@ -231,7 +264,6 @@ static __refdata struct usb_composite_driver hidg_driver = {
 static struct platform_driver hidg_plat_driver = {
 	.remove		= hidg_plat_driver_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "hidg",
 	},
 };
@@ -260,7 +292,7 @@ module_init(hidg_init);
 
 static void __exit hidg_cleanup(void)
 {
-	platform_driver_unregister(&hidg_plat_driver);
 	usb_composite_unregister(&hidg_driver);
+	platform_driver_unregister(&hidg_plat_driver);
 }
 module_exit(hidg_cleanup);
diff --git a/drivers/usb/gadget/legacy/printer.c b/drivers/usb/gadget/legacy/printer.c
index 6474081dcbaf..90545980542f 100644
--- a/drivers/usb/gadget/legacy/printer.c
+++ b/drivers/usb/gadget/legacy/printer.c
@@ -208,6 +208,43 @@ static struct usb_descriptor_header *hs_printer_function[] = {
 	NULL
 };
 
+/*
+ * Added endpoint descriptors for 3.0 devices
+ */
+
+static struct usb_endpoint_descriptor ss_ep_in_desc = {
+	.bLength =              USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =      USB_DT_ENDPOINT,
+	.bmAttributes =         USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =       cpu_to_le16(1024),
+};
+
+static struct usb_ss_ep_comp_descriptor ss_ep_in_comp_desc = {
+	.bLength =              sizeof(ss_ep_in_comp_desc),
+	.bDescriptorType =      USB_DT_SS_ENDPOINT_COMP,
+};
+
+static struct usb_endpoint_descriptor ss_ep_out_desc = {
+	.bLength =              USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =      USB_DT_ENDPOINT,
+	.bmAttributes =         USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =       cpu_to_le16(1024),
+};
+
+static struct usb_ss_ep_comp_descriptor ss_ep_out_comp_desc = {
+	.bLength =              sizeof(ss_ep_out_comp_desc),
+	.bDescriptorType =      USB_DT_SS_ENDPOINT_COMP,
+};
+
+static struct usb_descriptor_header *ss_printer_function[] = {
+	(struct usb_descriptor_header *) &intf_desc,
+	(struct usb_descriptor_header *) &ss_ep_in_desc,
+	(struct usb_descriptor_header *) &ss_ep_in_comp_desc,
+	(struct usb_descriptor_header *) &ss_ep_out_desc,
+	(struct usb_descriptor_header *) &ss_ep_out_comp_desc,
+	NULL
+};
+
 static struct usb_otg_descriptor otg_descriptor = {
 	.bLength =              sizeof otg_descriptor,
 	.bDescriptorType =      USB_DT_OTG,
@@ -220,7 +257,20 @@ static const struct usb_descriptor_header *otg_desc[] = {
 };
 
 /* maxpacket and other transfer characteristics vary by speed. */
-#define ep_desc(g, hs, fs) (((g)->speed == USB_SPEED_HIGH)?(hs):(fs))
+static inline struct usb_endpoint_descriptor *ep_desc(struct usb_gadget *gadget,
+					struct usb_endpoint_descriptor *fs,
+					struct usb_endpoint_descriptor *hs,
+					struct usb_endpoint_descriptor *ss)
+{
+	switch (gadget->speed) {
+	case USB_SPEED_SUPER:
+		return ss;
+	case USB_SPEED_HIGH:
+		return hs;
+	default:
+		return fs;
+	}
+}
 
 /*-------------------------------------------------------------------------*/
 
@@ -793,11 +843,12 @@ set_printer_interface(struct printer_dev *dev)
 {
 	int			result = 0;
 
-	dev->in_ep->desc = ep_desc(dev->gadget, &hs_ep_in_desc, &fs_ep_in_desc);
+	dev->in_ep->desc = ep_desc(dev->gadget, &fs_ep_in_desc, &hs_ep_in_desc,
+				&ss_ep_in_desc);
 	dev->in_ep->driver_data = dev;
 
-	dev->out_ep->desc = ep_desc(dev->gadget, &hs_ep_out_desc,
-				    &fs_ep_out_desc);
+	dev->out_ep->desc = ep_desc(dev->gadget, &fs_ep_out_desc,
+				    &hs_ep_out_desc, &ss_ep_out_desc);
 	dev->out_ep->driver_data = dev;
 
 	result = usb_ep_enable(dev->in_ep);
@@ -1016,9 +1067,11 @@ autoconf_fail:
 	/* 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;
+	ss_ep_in_desc.bEndpointAddress = fs_ep_in_desc.bEndpointAddress;
+	ss_ep_out_desc.bEndpointAddress = fs_ep_out_desc.bEndpointAddress;
 
 	ret = usb_assign_descriptors(f, fs_printer_function,
-			hs_printer_function, NULL);
+			hs_printer_function, ss_printer_function);
 	if (ret)
 		return ret;
 
@@ -1253,7 +1306,7 @@ static __refdata struct usb_composite_driver printer_driver = {
 	.name           = shortname,
 	.dev            = &device_desc,
 	.strings        = dev_strings,
-	.max_speed      = USB_SPEED_HIGH,
+	.max_speed      = USB_SPEED_SUPER,
 	.bind		= printer_bind,
 	.unbind		= printer_unbind,
 };
diff --git a/drivers/usb/gadget/legacy/tcm_usb_gadget.c b/drivers/usb/gadget/legacy/tcm_usb_gadget.c
index 024f58475a94..3a494168661e 100644
--- a/drivers/usb/gadget/legacy/tcm_usb_gadget.c
+++ b/drivers/usb/gadget/legacy/tcm_usb_gadget.c
@@ -1131,19 +1131,19 @@ static int usbg_submit_command(struct f_uas *fu,
 
 	switch (cmd_iu->prio_attr & 0x7) {
 	case UAS_HEAD_TAG:
-		cmd->prio_attr = MSG_HEAD_TAG;
+		cmd->prio_attr = TCM_HEAD_TAG;
 		break;
 	case UAS_ORDERED_TAG:
-		cmd->prio_attr = MSG_ORDERED_TAG;
+		cmd->prio_attr = TCM_ORDERED_TAG;
 		break;
 	case UAS_ACA:
-		cmd->prio_attr = MSG_ACA_TAG;
+		cmd->prio_attr = TCM_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;
+		cmd->prio_attr = TCM_SIMPLE_TAG;
 		break;
 	}
 
@@ -1240,7 +1240,7 @@ static int bot_submit_command(struct f_uas *fu,
 		goto err;
 	}
 
-	cmd->prio_attr = MSG_SIMPLE_TAG;
+	cmd->prio_attr = TCM_SIMPLE_TAG;
 	se_cmd = &cmd->se_cmd;
 	cmd->unpacked_lun = cbw->Lun;
 	cmd->is_read = cbw->Flags & US_BULK_FLAG_IN ? 1 : 0;
diff --git a/drivers/usb/gadget/legacy/zero.c b/drivers/usb/gadget/legacy/zero.c
index ebf09f439f3a..ff97ac93ac03 100644
--- a/drivers/usb/gadget/legacy/zero.c
+++ b/drivers/usb/gadget/legacy/zero.c
@@ -28,7 +28,7 @@
  *
  * Why is *this* driver using two configurations, rather than setting up
  * two interfaces with different functions?  To help verify that multiple
- * configuration infrastucture is working correctly; also, so that it can
+ * configuration infrastructure is working correctly; also, so that it can
  * work with low capability USB controllers without four bulk endpoints.
  */
 
diff --git a/drivers/usb/gadget/udc/Kconfig b/drivers/usb/gadget/udc/Kconfig
index 217365d35a25..b8e213eb36cc 100644
--- a/drivers/usb/gadget/udc/Kconfig
+++ b/drivers/usb/gadget/udc/Kconfig
@@ -241,6 +241,8 @@ config USB_M66592
 	   dynamically linked module called "m66592_udc" and force all
 	   gadget drivers to also be dynamically linked.
 
+source "drivers/usb/gadget/udc/bdc/Kconfig"
+
 #
 # Controllers available only in discrete form (and all PCI controllers)
 #
diff --git a/drivers/usb/gadget/udc/Makefile b/drivers/usb/gadget/udc/Makefile
index a7f4491593f1..fba2049bf985 100644
--- a/drivers/usb/gadget/udc/Makefile
+++ b/drivers/usb/gadget/udc/Makefile
@@ -30,3 +30,4 @@ obj-$(CONFIG_USB_FOTG210_UDC)	+= fotg210-udc.o
 obj-$(CONFIG_USB_MV_U3D)	+= mv_u3d_core.o
 obj-$(CONFIG_USB_GR_UDC)	+= gr_udc.o
 obj-$(CONFIG_USB_GADGET_XILINX)	+= udc-xilinx.o
+obj-$(CONFIG_USB_BDC_UDC)	+= bdc/
diff --git a/drivers/usb/gadget/udc/amd5536udc.c b/drivers/usb/gadget/udc/amd5536udc.c
index 3b9d13848a4f..de7e5e2ccf1c 100644
--- a/drivers/usb/gadget/udc/amd5536udc.c
+++ b/drivers/usb/gadget/udc/amd5536udc.c
@@ -1401,9 +1401,8 @@ static int udc_wakeup(struct usb_gadget *gadget)
 
 static int amd5536_udc_start(struct usb_gadget *g,
 		struct usb_gadget_driver *driver);
-static int amd5536_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver);
-/* gadget operations */
+static int amd5536_udc_stop(struct usb_gadget *g);
+
 static const struct usb_gadget_ops udc_ops = {
 	.wakeup		= udc_wakeup,
 	.get_frame	= udc_get_frame,
@@ -1962,8 +1961,7 @@ __acquires(dev->lock)
 }
 
 /* Called by gadget driver to unregister itself */
-static int amd5536_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver)
+static int amd5536_udc_stop(struct usb_gadget *g)
 {
 	struct udc *dev = to_amd5536_udc(g);
 	unsigned long flags;
@@ -1971,7 +1969,7 @@ static int amd5536_udc_stop(struct usb_gadget *g,
 
 	spin_lock_irqsave(&dev->lock, flags);
 	udc_mask_unused_interrupts(dev);
-	shutdown(dev, driver);
+	shutdown(dev, NULL);
 	spin_unlock_irqrestore(&dev->lock, flags);
 
 	dev->driver = NULL;
@@ -2873,7 +2871,7 @@ __acquires(dev->lock)
 			dev->driver->resume(&dev->gadget);
 			dev->sys_suspended = 0;
 		}
-		dev->driver->disconnect(&dev->gadget);
+		usb_gadget_udc_reset(&dev->gadget, dev->driver);
 		spin_lock(&dev->lock);
 
 		/* disable ep0 to empty req queue */
diff --git a/drivers/usb/gadget/udc/at91_udc.c b/drivers/usb/gadget/udc/at91_udc.c
index 9968f5331fe4..c862656d18b8 100644
--- a/drivers/usb/gadget/udc/at91_udc.c
+++ b/drivers/usb/gadget/udc/at91_udc.c
@@ -840,6 +840,31 @@ static void udc_reinit(struct at91_udc *udc)
 	}
 }
 
+static void reset_gadget(struct at91_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 at91_ep *ep = &udc->ep[i];
+
+		ep->stopped = 1;
+		nuke(ep, -ESHUTDOWN);
+	}
+	if (driver) {
+		spin_unlock(&udc->lock);
+		usb_gadget_udc_reset(&udc->gadget, driver);
+		spin_lock(&udc->lock);
+	}
+
+	udc_reinit(udc);
+}
+
 static void stop_activity(struct at91_udc *udc)
 {
 	struct usb_gadget_driver *driver = udc->driver;
@@ -870,12 +895,10 @@ static void clk_on(struct at91_udc *udc)
 		return;
 	udc->clocked = 1;
 
-	if (IS_ENABLED(CONFIG_COMMON_CLK)) {
-		clk_set_rate(udc->uclk, 48000000);
-		clk_prepare_enable(udc->uclk);
-	}
-	clk_prepare_enable(udc->iclk);
-	clk_prepare_enable(udc->fclk);
+	if (IS_ENABLED(CONFIG_COMMON_CLK))
+		clk_enable(udc->uclk);
+	clk_enable(udc->iclk);
+	clk_enable(udc->fclk);
 }
 
 static void clk_off(struct at91_udc *udc)
@@ -884,10 +907,10 @@ static void clk_off(struct at91_udc *udc)
 		return;
 	udc->clocked = 0;
 	udc->gadget.speed = USB_SPEED_UNKNOWN;
-	clk_disable_unprepare(udc->fclk);
-	clk_disable_unprepare(udc->iclk);
+	clk_disable(udc->fclk);
+	clk_disable(udc->iclk);
 	if (IS_ENABLED(CONFIG_COMMON_CLK))
-		clk_disable_unprepare(udc->uclk);
+		clk_disable(udc->uclk);
 }
 
 /*
@@ -984,8 +1007,8 @@ static int at91_set_selfpowered(struct usb_gadget *gadget, int is_on)
 
 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 int at91_stop(struct usb_gadget *gadget);
+
 static const struct usb_gadget_ops at91_udc_ops = {
 	.get_frame		= at91_get_frame,
 	.wakeup			= at91_wakeup,
@@ -1426,7 +1449,7 @@ static irqreturn_t at91_udc_irq (int irq, void *_udc)
 			at91_udp_write(udc, AT91_UDP_ICR, AT91_UDP_ENDBUSRES);
 			VDBG("end bus reset\n");
 			udc->addr = 0;
-			stop_activity(udc);
+			reset_gadget(udc);
 
 			/* enable ep0 */
 			at91_udp_write(udc, AT91_UDP_CSR(0),
@@ -1512,20 +1535,11 @@ static irqreturn_t at91_udc_irq (int irq, void *_udc)
 
 /*-------------------------------------------------------------------------*/
 
-static void nop_release(struct device *dev)
-{
-	/* nothing to free */
-}
-
 static struct at91_udc controller = {
 	.gadget = {
 		.ops	= &at91_udc_ops,
 		.ep0	= &controller.ep[0].ep,
 		.name	= driver_name,
-		.dev	= {
-			.init_name = "gadget",
-			.release = nop_release,
-		}
 	},
 	.ep[0] = {
 		.ep = {
@@ -1641,12 +1655,10 @@ static int at91_start(struct usb_gadget *gadget,
 	udc->enabled = 1;
 	udc->selfpowered = 1;
 
-	DBG("bound to %s\n", driver->driver.name);
 	return 0;
 }
 
-static int at91_stop(struct usb_gadget *gadget,
-		struct usb_gadget_driver *driver)
+static int at91_stop(struct usb_gadget *gadget)
 {
 	struct at91_udc *udc;
 	unsigned long	flags;
@@ -1659,7 +1671,6 @@ static int at91_stop(struct usb_gadget *gadget,
 
 	udc->driver = NULL;
 
-	DBG("unbound from %s\n", driver->driver.name);
 	return 0;
 }
 
@@ -1780,14 +1791,24 @@ static int at91udc_probe(struct platform_device *pdev)
 	}
 
 	/* don't do anything until we have both gadget driver and VBUS */
+	if (IS_ENABLED(CONFIG_COMMON_CLK)) {
+		clk_set_rate(udc->uclk, 48000000);
+		retval = clk_prepare(udc->uclk);
+		if (retval)
+			goto fail1;
+	}
+	retval = clk_prepare(udc->fclk);
+	if (retval)
+		goto fail1a;
+
 	retval = clk_prepare_enable(udc->iclk);
 	if (retval)
-		goto fail1;
+		goto fail1b;
 	at91_udp_write(udc, AT91_UDP_TXVC, AT91_UDP_TXVC_TXVDIS);
 	at91_udp_write(udc, AT91_UDP_IDR, 0xffffffff);
 	/* Clear all pending interrupts - UDP may be used by bootloader. */
 	at91_udp_write(udc, AT91_UDP_ICR, 0xffffffff);
-	clk_disable_unprepare(udc->iclk);
+	clk_disable(udc->iclk);
 
 	/* request UDC and maybe VBUS irqs */
 	udc->udp_irq = platform_get_irq(pdev, 0);
@@ -1795,7 +1816,7 @@ static int at91udc_probe(struct platform_device *pdev)
 			0, driver_name, udc);
 	if (retval < 0) {
 		DBG("request irq %d failed\n", udc->udp_irq);
-		goto fail1;
+		goto fail1c;
 	}
 	if (gpio_is_valid(udc->board.vbus_pin)) {
 		retval = gpio_request(udc->board.vbus_pin, "udc_vbus");
@@ -1848,6 +1869,13 @@ fail3:
 		gpio_free(udc->board.vbus_pin);
 fail2:
 	free_irq(udc->udp_irq, udc);
+fail1c:
+	clk_unprepare(udc->iclk);
+fail1b:
+	clk_unprepare(udc->fclk);
+fail1a:
+	if (IS_ENABLED(CONFIG_COMMON_CLK))
+		clk_unprepare(udc->uclk);
 fail1:
 	if (IS_ENABLED(CONFIG_COMMON_CLK) && !IS_ERR(udc->uclk))
 		clk_put(udc->uclk);
@@ -1896,6 +1924,11 @@ static int __exit at91udc_remove(struct platform_device *pdev)
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	release_mem_region(res->start, resource_size(res));
 
+	if (IS_ENABLED(CONFIG_COMMON_CLK))
+		clk_unprepare(udc->uclk);
+	clk_unprepare(udc->fclk);
+	clk_unprepare(udc->iclk);
+
 	clk_put(udc->iclk);
 	clk_put(udc->fclk);
 	if (IS_ENABLED(CONFIG_COMMON_CLK))
@@ -1972,7 +2005,6 @@ static struct platform_driver at91_udc_driver = {
 	.resume		= at91udc_resume,
 	.driver		= {
 		.name	= (char *) driver_name,
-		.owner	= THIS_MODULE,
 		.of_match_table	= of_match_ptr(at91_udc_dt_ids),
 	},
 };
diff --git a/drivers/usb/gadget/udc/atmel_usba_udc.c b/drivers/usb/gadget/udc/atmel_usba_udc.c
index 1529926e20a0..ce882371786b 100644
--- a/drivers/usb/gadget/udc/atmel_usba_udc.c
+++ b/drivers/usb/gadget/udc/atmel_usba_udc.c
@@ -987,8 +987,8 @@ usba_udc_set_selfpowered(struct usb_gadget *gadget, int is_selfpowered)
 
 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 int atmel_usba_stop(struct usb_gadget *gadget);
+
 static const struct usb_gadget_ops usba_udc_ops = {
 	.get_frame		= usba_udc_get_frame,
 	.wakeup			= usba_udc_wakeup,
@@ -1007,19 +1007,10 @@ static struct usb_endpoint_descriptor usba_ep0_desc = {
 	.bInterval = 1,
 };
 
-static void nop_release(struct device *dev)
-{
-
-}
-
 static struct usb_gadget usba_gadget_template = {
 	.ops		= &usba_udc_ops,
 	.max_speed	= USB_SPEED_HIGH,
 	.name		= "atmel_usba_udc",
-	.dev	= {
-		.init_name	= "gadget",
-		.release	= nop_release,
-	},
 };
 
 /*
@@ -1685,11 +1676,10 @@ static irqreturn_t usba_udc_irq(int irq, void *devid)
 		usba_writel(udc, INT_CLR, USBA_END_OF_RESET);
 		reset_all_endpoints(udc);
 
-		if (udc->gadget.speed != USB_SPEED_UNKNOWN
-				&& udc->driver && udc->driver->disconnect) {
+		if (udc->gadget.speed != USB_SPEED_UNKNOWN && udc->driver) {
 			udc->gadget.speed = USB_SPEED_UNKNOWN;
 			spin_unlock(&udc->lock);
-			udc->driver->disconnect(&udc->gadget);
+			usb_gadget_udc_reset(&udc->gadget, udc->driver);
 			spin_lock(&udc->lock);
 		}
 
@@ -1791,8 +1781,6 @@ static int atmel_usba_start(struct usb_gadget *gadget,
 		return ret;
 	}
 
-	DBG(DBG_GADGET, "registered driver `%s'\n", driver->driver.name);
-
 	udc->vbus_prev = 0;
 	if (gpio_is_valid(udc->vbus_pin))
 		enable_irq(gpio_to_irq(udc->vbus_pin));
@@ -1809,8 +1797,7 @@ static int atmel_usba_start(struct usb_gadget *gadget,
 	return 0;
 }
 
-static int atmel_usba_stop(struct usb_gadget *gadget,
-		struct usb_gadget_driver *driver)
+static int atmel_usba_stop(struct usb_gadget *gadget)
 {
 	struct usba_udc *udc = container_of(gadget, struct usba_udc, gadget);
 	unsigned long flags;
@@ -1830,8 +1817,6 @@ static int atmel_usba_stop(struct usb_gadget *gadget,
 	clk_disable_unprepare(udc->hclk);
 	clk_disable_unprepare(udc->pclk);
 
-	DBG(DBG_GADGET, "unregistered driver `%s'\n", udc->driver->driver.name);
-
 	udc->driver = NULL;
 
 	return 0;
@@ -2120,7 +2105,6 @@ static struct platform_driver udc_driver = {
 	.remove		= __exit_p(usba_udc_remove),
 	.driver		= {
 		.name		= "atmel_usba_udc",
-		.owner		= THIS_MODULE,
 		.of_match_table	= of_match_ptr(atmel_udc_dt_ids),
 	},
 };
diff --git a/drivers/usb/gadget/udc/bcm63xx_udc.c b/drivers/usb/gadget/udc/bcm63xx_udc.c
index 2235b8808700..9db968ba39f5 100644
--- a/drivers/usb/gadget/udc/bcm63xx_udc.c
+++ b/drivers/usb/gadget/udc/bcm63xx_udc.c
@@ -1836,8 +1836,7 @@ static int bcm63xx_udc_start(struct usb_gadget *gadget,
  * @gadget: USB slave device.
  * @driver: Driver for USB slave devices.
  */
-static int bcm63xx_udc_stop(struct usb_gadget *gadget,
-		struct usb_gadget_driver *driver)
+static int bcm63xx_udc_stop(struct usb_gadget *gadget)
 {
 	struct bcm63xx_udc *udc = gadget_to_udc(gadget);
 	unsigned long flags;
@@ -1963,7 +1962,7 @@ static irqreturn_t bcm63xx_udc_ctrl_isr(int irq, void *dev_id)
 {
 	struct bcm63xx_udc *udc = dev_id;
 	u32 stat;
-	bool disconnected = false;
+	bool disconnected = false, bus_reset = false;
 
 	stat = usbd_readl(udc, USBD_EVENT_IRQ_STATUS_REG) &
 	       usbd_readl(udc, USBD_EVENT_IRQ_MASK_REG);
@@ -1991,7 +1990,7 @@ static irqreturn_t bcm63xx_udc_ctrl_isr(int irq, void *dev_id)
 
 		udc->ep0_req_reset = 1;
 		schedule_work(&udc->ep0_wq);
-		disconnected = true;
+		bus_reset = true;
 	}
 	if (stat & BIT(USBD_EVENT_IRQ_SETUP)) {
 		if (bcm63xx_update_link_speed(udc)) {
@@ -2014,6 +2013,8 @@ static irqreturn_t bcm63xx_udc_ctrl_isr(int irq, void *dev_id)
 
 	if (disconnected && udc->driver)
 		udc->driver->disconnect(&udc->gadget);
+	else if (bus_reset && udc->driver)
+		usb_gadget_udc_reset(&udc->gadget, udc->driver);
 
 	return IRQ_HANDLED;
 }
@@ -2324,10 +2325,8 @@ static int bcm63xx_udc_probe(struct platform_device *pdev)
 	int rc = -ENOMEM, i, irq;
 
 	udc = devm_kzalloc(dev, sizeof(*udc), GFP_KERNEL);
-	if (!udc) {
-		dev_err(dev, "cannot allocate memory\n");
+	if (!udc)
 		return -ENOMEM;
-	}
 
 	platform_set_drvdata(pdev, udc);
 	udc->dev = dev;
@@ -2425,7 +2424,6 @@ static struct platform_driver bcm63xx_udc_driver = {
 	.remove		= bcm63xx_udc_remove,
 	.driver		= {
 		.name	= DRV_MODULE_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(bcm63xx_udc_driver);
diff --git a/drivers/usb/gadget/udc/bdc/Kconfig b/drivers/usb/gadget/udc/bdc/Kconfig
new file mode 100644
index 000000000000..0d7b8c9f72fd
--- /dev/null
+++ b/drivers/usb/gadget/udc/bdc/Kconfig
@@ -0,0 +1,21 @@
+config USB_BDC_UDC
+	tristate "Broadcom USB3.0 device controller IP driver(BDC)"
+	depends on USB_GADGET && HAS_DMA
+
+	help
+	BDC is Broadcom's USB3.0 device controller IP. If your SOC has a BDC IP
+	then select this driver.
+
+	Say "y" here to link the driver statically, or "m" to build a dynamically
+	linked module called "bdc".
+
+if USB_BDC_UDC
+
+comment "Platform Support"
+config	USB_BDC_PCI
+	tristate "BDC support for PCIe based platforms"
+	depends on PCI
+	default USB_BDC_UDC
+	help
+		Enable support for platforms which have BDC connected through PCIe, such as Lego3 FPGA platform.
+endif
diff --git a/drivers/usb/gadget/udc/bdc/Makefile b/drivers/usb/gadget/udc/bdc/Makefile
new file mode 100644
index 000000000000..5cf6a3bcdf0f
--- /dev/null
+++ b/drivers/usb/gadget/udc/bdc/Makefile
@@ -0,0 +1,8 @@
+obj-$(CONFIG_USB_BDC_UDC)	+= bdc.o
+bdc-y	:= bdc_core.o bdc_cmd.o bdc_ep.o bdc_udc.o
+
+ifneq ($(CONFIG_USB_GADGET_VERBOSE),)
+	bdc-y			+= bdc_dbg.o
+endif
+
+obj-$(CONFIG_USB_BDC_PCI)	+= bdc_pci.o
diff --git a/drivers/usb/gadget/udc/bdc/bdc.h b/drivers/usb/gadget/udc/bdc/bdc.h
new file mode 100644
index 000000000000..dc18a20bf040
--- /dev/null
+++ b/drivers/usb/gadget/udc/bdc/bdc.h
@@ -0,0 +1,490 @@
+/*
+ * bdc.h - header for the BRCM BDC USB3.0 device controller
+ *
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * Author: Ashwini Pahuja
+ *
+ * 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_BDC_H__
+#define	__LINUX_BDC_H__
+
+#include <linux/kernel.h>
+#include <linux/usb.h>
+#include <linux/device.h>
+#include <linux/spinlock.h>
+#include <linux/list.h>
+#include <linux/dma-mapping.h>
+#include <linux/mm.h>
+#include <linux/debugfs.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/gadget.h>
+#include <asm/unaligned.h>
+
+#define BRCM_BDC_NAME "bdc_usb3"
+#define BRCM_BDC_DESC "BDC device controller driver"
+
+#define DMA_ADDR_INVALID        (~(dma_addr_t)0)
+
+/* BDC command operation timeout in usec*/
+#define BDC_CMD_TIMEOUT	1000
+/* BDC controller operation timeout in usec*/
+#define BDC_COP_TIMEOUT	500
+
+/*
+ * Maximum size of ep0 response buffer for ch9 requests,
+ * the set_sel request uses 6 so far, the max.
+*/
+#define EP0_RESPONSE_BUFF  6
+/* Start with SS as default */
+#define EP0_MAX_PKT_SIZE 512
+
+/* 64 entries in a SRR */
+#define NUM_SR_ENTRIES	64
+
+/* Num of bds per table */
+#define NUM_BDS_PER_TABLE	32
+
+/* Num of tables in bd list for control,bulk and Int ep */
+#define NUM_TABLES	2
+
+/* Num of tables in bd list for Isoch ep */
+#define NUM_TABLES_ISOCH	6
+
+/* U1 Timeout default: 248usec */
+#define U1_TIMEOUT	0xf8
+
+/* Interrupt coalescence in usec */
+#define	INT_CLS	500
+
+/* Register offsets */
+/* Configuration and Capability registers */
+#define BDC_BDCCFG0	0x00
+#define BDC_BDCCFG1	0x04
+#define BDC_BDCCAP0	0x08
+#define BDC_BDCCAP1	0x0c
+#define BDC_CMDPAR0	0x10
+#define BDC_CMDPAR1	0x14
+#define BDC_CMDPAR2	0x18
+#define BDC_CMDSC	0x1c
+#define BDC_USPC	0x20
+#define BDC_USPPMS	0x28
+#define BDC_USPPM2	0x2c
+#define BDC_SPBBAL	0x38
+#define BDC_SPBBAH	0x3c
+#define BDC_BDCSC	0x40
+#define BDC_XSFNTF	0x4c
+
+#define BDC_DVCSA	0x50
+#define BDC_DVCSB	0x54
+#define BDC_EPSTS0(n)	(0x60 + (n * 0x10))
+#define BDC_EPSTS1(n)	(0x64 + (n * 0x10))
+#define BDC_EPSTS2(n)	(0x68 + (n * 0x10))
+#define BDC_EPSTS3(n)	(0x6c + (n * 0x10))
+#define BDC_EPSTS4(n)	(0x70 + (n * 0x10))
+#define BDC_EPSTS5(n)	(0x74 + (n * 0x10))
+#define BDC_EPSTS6(n)	(0x78 + (n * 0x10))
+#define BDC_EPSTS7(n)	(0x7c + (n * 0x10))
+#define BDC_SRRBAL(n)	(0x200 + (n * 0x10))
+#define BDC_SRRBAH(n)	(0x204 + (n * 0x10))
+#define BDC_SRRINT(n)	(0x208 + (n * 0x10))
+#define BDC_INTCTLS(n)	(0x20c + (n * 0x10))
+
+/* Extended capability regs */
+#define BDC_FSCNOC	0xcd4
+#define BDC_FSCNIC	0xce4
+#define NUM_NCS(p)	(p >> 28)
+
+/* Register bit fields and Masks */
+/* BDC Configuration 0 */
+#define BDC_PGS(p)	(((p) & (0x7 << 8)) >> 8)
+#define BDC_SPB(p)	(p & 0x7)
+
+/* BDC Capability1 */
+#define BDC_P64		(1 << 0)
+
+/* BDC Command register */
+#define BDC_CMD_FH	0xe
+#define BDC_CMD_DNC	0x6
+#define BDC_CMD_EPO	0x4
+#define BDC_CMD_BLA	0x3
+#define BDC_CMD_EPC	0x2
+#define BDC_CMD_DVC	0x1
+#define BDC_CMD_CWS		(0x1 << 5)
+#define BDC_CMD_CST(p)		(((p) & (0xf << 6))>>6)
+#define BDC_CMD_EPN(p)		((p & 0x1f) << 10)
+#define BDC_SUB_CMD_ADD		(0x1 << 17)
+#define BDC_SUB_CMD_FWK		(0x4 << 17)
+/* Reset sequence number */
+#define BDC_CMD_EPO_RST_SN	(0x1 << 16)
+#define BDC_CMD_EP0_XSD		(0x1 << 16)
+#define BDC_SUB_CMD_ADD_EP	(0x1 << 17)
+#define BDC_SUB_CMD_DRP_EP	(0x2 << 17)
+#define BDC_SUB_CMD_EP_STP	(0x2 << 17)
+#define BDC_SUB_CMD_EP_STL	(0x4 << 17)
+#define BDC_SUB_CMD_EP_RST	(0x1 << 17)
+#define BDC_CMD_SRD		(1 << 27)
+
+/* CMD completion status */
+#define BDC_CMDS_SUCC	0x1
+#define BDC_CMDS_PARA	0x3
+#define BDC_CMDS_STAT	0x4
+#define BDC_CMDS_FAIL	0x5
+#define BDC_CMDS_INTL	0x6
+#define BDC_CMDS_BUSY	0xf
+
+/* CMDSC Param 2 shifts */
+#define EPT_SHIFT	22
+#define MP_SHIFT	10
+#define MB_SHIFT	6
+#define EPM_SHIFT	4
+
+/* BDC USPSC */
+#define BDC_VBC		(1 << 31)
+#define BDC_PRC		(1 << 30)
+#define BDC_PCE		(1 << 29)
+#define BDC_CFC		(1 << 28)
+#define BDC_PCC		(1 << 27)
+#define BDC_PSC		(1 << 26)
+#define BDC_VBS		(1 << 25)
+#define BDC_PRS		(1 << 24)
+#define BDC_PCS		(1 << 23)
+#define BDC_PSP(p)	(((p) & (0x7 << 20))>>20)
+#define BDC_SCN		(1 << 8)
+#define BDC_SDC		(1 << 7)
+#define BDC_SWS		(1 << 4)
+
+#define BDC_USPSC_RW	(BDC_SCN|BDC_SDC|BDC_SWS|0xf)
+#define BDC_PSP(p)	(((p) & (0x7 << 20))>>20)
+
+#define BDC_SPEED_FS	0x1
+#define BDC_SPEED_LS	0x2
+#define BDC_SPEED_HS	0x3
+#define BDC_SPEED_SS	0x4
+
+#define BDC_PST(p)	(p & 0xf)
+#define BDC_PST_MASK	0xf
+
+/* USPPMS */
+#define BDC_U2E		(0x1 << 31)
+#define BDC_U1E		(0x1 << 30)
+#define BDC_U2A		(0x1 << 29)
+#define BDC_PORT_W1S	(0x1 << 17)
+#define BDC_U1T(p)	((p) & 0xff)
+#define BDC_U2T(p)	(((p) & 0xff) << 8)
+#define BDC_U1T_MASK	0xff
+
+/* USBPM2 */
+/* Hardware LPM Enable */
+#define BDC_HLE		(1 << 16)
+
+/* BDC Status and Control */
+#define BDC_COP_RST	(1 << 29)
+#define BDC_COP_RUN	(2 << 29)
+#define BDC_COP_STP	(4 << 29)
+
+#define BDC_COP_MASK (BDC_COP_RST|BDC_COP_RUN|BDC_COP_STP)
+
+#define BDC_COS		(1 << 28)
+#define BDC_CSTS(p)	(((p) & (0x7 << 20)) >> 20)
+#define BDC_MASK_MCW	(1 << 7)
+#define BDC_GIE		(1 << 1)
+#define BDC_GIP		(1 << 0)
+
+#define BDC_HLT	1
+#define BDC_NOR	2
+#define BDC_OIP	7
+
+/* Buffer descriptor and Status report bit fields and masks */
+#define BD_TYPE_BITMASK	(0xf)
+#define BD_CHAIN	0xf
+
+#define BD_TFS_SHIFT	4
+#define BD_SOT		(1 << 26)
+#define BD_EOT		(1 << 27)
+#define BD_ISP		(1 << 29)
+#define BD_IOC		(1 << 30)
+#define BD_SBF		(1 << 31)
+
+#define BD_INTR_TARGET(p)	(((p) & 0x1f) << 27)
+
+#define BDC_SRR_RWS		(1 << 4)
+#define BDC_SRR_RST		(1 << 3)
+#define BDC_SRR_ISR		(1 << 2)
+#define BDC_SRR_IE		(1 << 1)
+#define BDC_SRR_IP		(1 << 0)
+#define BDC_SRR_EPI(p)	(((p) & (0xff << 24)) >> 24)
+#define BDC_SRR_DPI(p) (((p) & (0xff << 16)) >> 16)
+#define BDC_SRR_DPI_MASK	0x00ff0000
+
+#define MARK_CHAIN_BD	(BD_CHAIN|BD_EOT|BD_SOT)
+
+/* Control transfer BD specific fields */
+#define BD_DIR_IN		(1 << 25)
+
+#define BDC_PTC_MASK	0xf0000000
+
+/* status report defines */
+#define SR_XSF		0
+#define SR_USPC		4
+#define SR_BD_LEN(p)    (p & 0xffffff)
+
+#define XSF_SUCC	0x1
+#define XSF_SHORT	0x3
+#define XSF_BABB	0x4
+#define XSF_SETUP_RECV	0x6
+#define XSF_DATA_START	0x7
+#define XSF_STATUS_START 0x8
+
+#define XSF_STS(p) (((p) >> 28) & 0xf)
+
+/* Transfer BD fields */
+#define BD_LEN(p) ((p) & 0x1ffff)
+#define BD_LTF		(1 << 25)
+#define BD_TYPE_DS	0x1
+#define BD_TYPE_SS	0x2
+
+#define BDC_EP_ENABLED     (1 << 0)
+#define BDC_EP_STALL       (1 << 1)
+#define BDC_EP_STOP        (1 << 2)
+
+/* One BD can transfer max 65536 bytes */
+#define BD_MAX_BUFF_SIZE	(1 << 16)
+/* Maximum bytes in one XFR, Refer to BDC spec */
+#define MAX_XFR_LEN		16777215
+
+/* defines for Force Header command */
+#define DEV_NOTF_TYPE 6
+#define FWK_SUBTYPE  1
+#define TRA_PACKET   4
+
+#define to_bdc_ep(e)		container_of(e, struct bdc_ep, usb_ep)
+#define to_bdc_req(r)		container_of(r, struct bdc_req, usb_req)
+#define gadget_to_bdc(g)	container_of(g, struct bdc, gadget)
+
+/* FUNCTION WAKE DEV NOTIFICATION interval, USB3 spec table 8.13 */
+#define BDC_TNOTIFY 2500 /*in ms*/
+/* Devstatus bitfields */
+#define REMOTE_WAKEUP_ISSUED	(1 << 16)
+#define DEVICE_SUSPENDED	(1 << 17)
+#define FUNC_WAKE_ISSUED	(1 << 18)
+#define REMOTE_WAKE_ENABLE	(1 << USB_DEVICE_REMOTE_WAKEUP)
+
+/* On disconnect, preserve these bits and clear rest */
+#define DEVSTATUS_CLEAR		(1 << USB_DEVICE_SELF_POWERED)
+/* Hardware and software Data structures */
+
+/* Endpoint bd: buffer descriptor */
+struct bdc_bd {
+	__le32 offset[4];
+};
+
+/* Status report in Status report ring(srr) */
+struct bdc_sr {
+	__le32 offset[4];
+};
+
+/* bd_table: contigous bd's in a table */
+struct bd_table {
+	struct bdc_bd *start_bd;
+	/* dma address of start bd of table*/
+	dma_addr_t dma;
+};
+
+/*
+ * Each endpoint has a bdl(buffer descriptor list), bdl consists of 1 or more bd
+ * table's chained to each other through a chain bd, every table has equal
+ * number of bds. the software uses bdi(bd index) to refer to particular bd in
+ * the list.
+ */
+struct bd_list {
+	/* Array of bd table pointers*/
+	struct bd_table **bd_table_array;
+	/* How many tables chained to each other */
+	int num_tabs;
+	/* Max_bdi = num_tabs * num_bds_table - 1 */
+	int max_bdi;
+	/* current enq bdi from sw point of view */
+	int eqp_bdi;
+	/* current deq bdi from sw point of view */
+	int hwd_bdi;
+	/* numbers of bds per table */
+	int num_bds_table;
+};
+
+struct bdc_req;
+
+/* Representation of a transfer, one transfer can have multiple bd's */
+struct bd_transfer {
+	struct bdc_req *req;
+	/* start bd index */
+	int start_bdi;
+	/* this will be the next hw dqp when this transfer completes */
+	int next_hwd_bdi;
+	/* number of bds in this transfer */
+	int num_bds;
+};
+
+/*
+ * Representation of a gadget request, every gadget request is contained
+ * by 1 bd_transfer.
+ */
+struct bdc_req {
+	struct usb_request	usb_req;
+	struct list_head	queue;
+	struct bdc_ep		*ep;
+	/* only one Transfer per request */
+	struct bd_transfer bd_xfr;
+	int	epnum;
+};
+
+/* scratchpad buffer needed by bdc hardware */
+struct bdc_scratchpad {
+	dma_addr_t sp_dma;
+	void *buff;
+	u32 size;
+};
+
+/* endpoint representation */
+struct bdc_ep {
+	struct usb_ep	usb_ep;
+	struct list_head queue;
+	struct bdc *bdc;
+	u8  ep_type;
+	u8  dir;
+	u8  ep_num;
+	const struct usb_ss_ep_comp_descriptor	*comp_desc;
+	const struct usb_endpoint_descriptor	*desc;
+	unsigned int flags;
+	char name[20];
+	/* endpoint bd list*/
+	struct bd_list bd_list;
+	/*
+	 * HW generates extra event for multi bd tranfers, this flag helps in
+	 * ignoring the extra event
+	 */
+	bool ignore_next_sr;
+};
+
+/* bdc cmmand parameter structure */
+struct bdc_cmd_params {
+	u32	param2;
+	u32	param1;
+	u32	param0;
+};
+
+/* status report ring(srr), currently one srr is supported for entire system */
+struct srr {
+	struct bdc_sr *sr_bds;
+	u16	eqp_index;
+	u16	dqp_index;
+	dma_addr_t	dma_addr;
+};
+
+/* EP0 states */
+enum bdc_ep0_state {
+	WAIT_FOR_SETUP = 0,
+	WAIT_FOR_DATA_START,
+	WAIT_FOR_DATA_XMIT,
+	WAIT_FOR_STATUS_START,
+	WAIT_FOR_STATUS_XMIT,
+	STATUS_PENDING
+};
+
+/* Link states */
+enum bdc_link_state {
+	BDC_LINK_STATE_U0	= 0x00,
+	BDC_LINK_STATE_U3	= 0x03,
+	BDC_LINK_STATE_RX_DET	= 0x05,
+	BDC_LINK_STATE_RESUME	= 0x0f
+};
+
+/* representation of bdc */
+struct bdc {
+	struct usb_gadget	gadget;
+	struct usb_gadget_driver	*gadget_driver;
+	struct device	*dev;
+	/* device lock */
+	spinlock_t	lock;
+
+	/* num of endpoints for a particular instantiation of IP */
+	unsigned int num_eps;
+	/*
+	 * Array of ep's, it uses the same index covention as bdc hw i.e.
+	 * 1 for ep0, 2 for 1out,3 for 1in ....
+	 */
+	struct bdc_ep		**bdc_ep_array;
+	void __iomem		*regs;
+	struct bdc_scratchpad	scratchpad;
+	u32	sp_buff_size;
+	/* current driver supports 1 status ring */
+	struct srr	srr;
+	/* Last received setup packet */
+	struct	usb_ctrlrequest setup_pkt;
+	struct	bdc_req ep0_req;
+	struct	bdc_req status_req;
+	enum	bdc_ep0_state ep0_state;
+	bool	delayed_status;
+	bool	zlp_needed;
+	bool	reinit;
+	bool	pullup;
+	/* Bits 0-15 are standard and 16-31 for proprietary information */
+	u32	devstatus;
+	int	irq;
+	void	*mem;
+	u32	dev_addr;
+	/* DMA pools */
+	struct dma_pool	*bd_table_pool;
+	u8		test_mode;
+	/* array of callbacks for various status report handlers */
+	void (*sr_handler[2])(struct bdc *, struct bdc_sr *);
+	/* ep0 callback handlers */
+	void (*sr_xsf_ep0[3])(struct bdc *, struct bdc_sr *);
+	/* ep0 response buffer for ch9 requests like GET_STATUS and SET_SEL */
+	unsigned char		ep0_response_buff[EP0_RESPONSE_BUFF];
+	/*
+	 * Timer to check if host resumed transfer after bdc sent Func wake
+	 * notification  packet after a remote wakeup. if not, then resend the
+	 * Func Wake packet every 2.5 secs. Refer to USB3 spec section 8.5.6.4
+	 */
+	struct delayed_work	func_wake_notify;
+};
+
+static inline u32 bdc_readl(void __iomem *base, u32 offset)
+{
+	return readl(base + offset);
+}
+
+static inline void bdc_writel(void __iomem *base, u32 offset, u32 value)
+{
+	writel(value, base + offset);
+}
+
+/* Buffer descriptor list operations */
+void bdc_notify_xfr(struct bdc *, u32);
+void bdc_softconn(struct bdc *);
+void bdc_softdisconn(struct bdc *);
+int bdc_run(struct bdc *);
+int bdc_stop(struct bdc *);
+int bdc_reset(struct bdc *);
+int bdc_udc_init(struct bdc *);
+void bdc_udc_exit(struct bdc *);
+int bdc_reinit(struct bdc *);
+
+/* Status report handlers */
+/* Upstream port status change sr */
+void bdc_sr_uspc(struct bdc *, struct bdc_sr *);
+/* transfer sr */
+void bdc_sr_xsf(struct bdc *, struct bdc_sr *);
+/* EP0 XSF handlers */
+void bdc_xsf_ep0_setup_recv(struct bdc *, struct bdc_sr *);
+void bdc_xsf_ep0_data_start(struct bdc *, struct bdc_sr *);
+void bdc_xsf_ep0_status_start(struct bdc *, struct bdc_sr *);
+
+#endif /* __LINUX_BDC_H__ */
diff --git a/drivers/usb/gadget/udc/bdc/bdc_cmd.c b/drivers/usb/gadget/udc/bdc/bdc_cmd.c
new file mode 100644
index 000000000000..6a4155c4bd86
--- /dev/null
+++ b/drivers/usb/gadget/udc/bdc/bdc_cmd.c
@@ -0,0 +1,376 @@
+/*
+ * bdc_cmd.c - BRCM BDC USB3.0 device controller
+ *
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * Author: Ashwini Pahuja
+ *
+ * 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/scatterlist.h>
+#include <linux/slab.h>
+
+#include "bdc.h"
+#include "bdc_cmd.h"
+#include "bdc_dbg.h"
+
+/* Issues a cmd to cmd processor and waits for cmd completion */
+static int bdc_issue_cmd(struct bdc *bdc, u32 cmd_sc, u32 param0,
+							u32 param1, u32 param2)
+{
+	u32 timeout = BDC_CMD_TIMEOUT;
+	u32 cmd_status;
+	u32 temp;
+
+	bdc_writel(bdc->regs, BDC_CMDPAR0, param0);
+	bdc_writel(bdc->regs, BDC_CMDPAR1, param1);
+	bdc_writel(bdc->regs, BDC_CMDPAR2, param2);
+
+	/* Issue the cmd */
+	/* Make sure the cmd params are written before asking HW to exec cmd */
+	wmb();
+	bdc_writel(bdc->regs, BDC_CMDSC, cmd_sc | BDC_CMD_CWS | BDC_CMD_SRD);
+	do {
+		temp = bdc_readl(bdc->regs, BDC_CMDSC);
+		dev_dbg_ratelimited(bdc->dev, "cmdsc=%x", temp);
+		cmd_status =  BDC_CMD_CST(temp);
+		if (cmd_status != BDC_CMDS_BUSY)  {
+			dev_dbg(bdc->dev,
+				"command completed cmd_sts:%x\n", cmd_status);
+			return cmd_status;
+		}
+		udelay(1);
+	} while (timeout--);
+
+	dev_err(bdc->dev,
+		"command operation timedout cmd_status=%d\n", cmd_status);
+
+	return cmd_status;
+}
+
+/* Submits cmd and analyze the return value of bdc_issue_cmd */
+static int bdc_submit_cmd(struct bdc *bdc, u32 cmd_sc,
+					u32 param0, u32 param1,	u32 param2)
+{
+	u32 temp, cmd_status;
+	int reset_bdc = 0;
+	int ret;
+
+	temp = bdc_readl(bdc->regs, BDC_CMDSC);
+	dev_dbg(bdc->dev,
+		"%s:CMDSC:%08x cmdsc:%08x param0=%08x param1=%08x param2=%08x\n",
+		 __func__, temp, cmd_sc, param0, param1, param2);
+
+	cmd_status = BDC_CMD_CST(temp);
+	if (cmd_status  ==  BDC_CMDS_BUSY) {
+		dev_err(bdc->dev, "command processor busy: %x\n", cmd_status);
+		return -EBUSY;
+	}
+	ret = bdc_issue_cmd(bdc, cmd_sc, param0, param1, param2);
+	switch (ret) {
+	case BDC_CMDS_SUCC:
+		dev_dbg(bdc->dev, "command completed successfully\n");
+		ret = 0;
+		break;
+
+	case BDC_CMDS_PARA:
+		dev_err(bdc->dev, "command parameter error\n");
+		ret = -EINVAL;
+		break;
+
+	case BDC_CMDS_STAT:
+		dev_err(bdc->dev, "Invalid device/ep state\n");
+		ret = -EINVAL;
+		break;
+
+	case BDC_CMDS_FAIL:
+		dev_err(bdc->dev, "Command failed?\n");
+		ret = -EAGAIN;
+		break;
+
+	case BDC_CMDS_INTL:
+		dev_err(bdc->dev, "BDC Internal error\n");
+		reset_bdc = 1;
+		ret = -ECONNRESET;
+		break;
+
+	case BDC_CMDS_BUSY:
+		dev_err(bdc->dev,
+			"command timedout waited for %dusec\n",
+			BDC_CMD_TIMEOUT);
+		reset_bdc = 1;
+		ret = -ECONNRESET;
+		break;
+	default:
+		dev_dbg(bdc->dev, "Unknown command completion code:%x\n", ret);
+	}
+
+	return ret;
+}
+
+/* Deconfigure the endpoint from HW */
+int bdc_dconfig_ep(struct bdc *bdc, struct bdc_ep *ep)
+{
+	u32 cmd_sc;
+
+	cmd_sc = BDC_SUB_CMD_DRP_EP|BDC_CMD_EPN(ep->ep_num)|BDC_CMD_EPC;
+	dev_dbg(bdc->dev, "%s ep->ep_num =%d cmd_sc=%x\n", __func__,
+							ep->ep_num, cmd_sc);
+
+	return bdc_submit_cmd(bdc, cmd_sc, 0, 0, 0);
+}
+
+/* Reinitalize the bdlist after config ep command */
+static void ep_bd_list_reinit(struct bdc_ep *ep)
+{
+	struct bdc *bdc = ep->bdc;
+	struct bdc_bd *bd;
+
+	ep->bd_list.eqp_bdi = 0;
+	ep->bd_list.hwd_bdi = 0;
+	bd = ep->bd_list.bd_table_array[0]->start_bd;
+	dev_dbg(bdc->dev, "%s ep:%p bd:%p\n", __func__, ep, bd);
+	memset(bd, 0, sizeof(struct bdc_bd));
+	bd->offset[3] |= cpu_to_le32(BD_SBF);
+}
+
+/* Configure an endpoint */
+int bdc_config_ep(struct bdc *bdc, struct bdc_ep *ep)
+{
+	const struct usb_ss_ep_comp_descriptor *comp_desc;
+	const struct usb_endpoint_descriptor	*desc;
+	u32 param0, param1, param2, cmd_sc;
+	u32 mps, mbs, mul, si;
+	int ret;
+
+	desc = ep->desc;
+	comp_desc = ep->comp_desc;
+	cmd_sc = mul = mbs = param2 = 0;
+	param0 = lower_32_bits(ep->bd_list.bd_table_array[0]->dma);
+	param1 = upper_32_bits(ep->bd_list.bd_table_array[0]->dma);
+	cpu_to_le32s(&param0);
+	cpu_to_le32s(&param1);
+
+	dev_dbg(bdc->dev, "%s: param0=%08x param1=%08x",
+						__func__, param0, param1);
+	si = desc->bInterval;
+	si = clamp_val(si, 1, 16) - 1;
+
+	mps = usb_endpoint_maxp(desc);
+	mps &= 0x7ff;
+	param2 |= mps << MP_SHIFT;
+	param2 |= usb_endpoint_type(desc) << EPT_SHIFT;
+
+	switch (bdc->gadget.speed) {
+	case USB_SPEED_SUPER:
+		if (usb_endpoint_xfer_int(desc) ||
+					usb_endpoint_xfer_isoc(desc)) {
+			param2 |= si;
+			if (usb_endpoint_xfer_isoc(desc) && comp_desc)
+					mul = comp_desc->bmAttributes;
+
+		}
+		param2 |= mul << EPM_SHIFT;
+		if (comp_desc)
+			mbs = comp_desc->bMaxBurst;
+		param2 |= mbs << MB_SHIFT;
+		break;
+
+	case USB_SPEED_HIGH:
+		if (usb_endpoint_xfer_isoc(desc) ||
+					usb_endpoint_xfer_int(desc)) {
+			param2 |= si;
+
+			mbs = (usb_endpoint_maxp(desc) & 0x1800) >> 11;
+			param2 |= mbs << MB_SHIFT;
+		}
+		break;
+
+	case USB_SPEED_FULL:
+	case USB_SPEED_LOW:
+		/* the hardware accepts SI in 125usec range */
+		if (usb_endpoint_xfer_isoc(desc))
+			si += 3;
+
+		/*
+		 * FS Int endpoints can have si of 1-255ms but the controller
+		 * accepts 2^bInterval*125usec, so convert ms to nearest power
+		 * of 2
+		 */
+		if (usb_endpoint_xfer_int(desc))
+			si = fls(desc->bInterval * 8) - 1;
+
+		param2 |= si;
+		break;
+	default:
+		dev_err(bdc->dev, "UNKNOWN speed ERR\n");
+		return -EINVAL;
+	}
+
+	cmd_sc |= BDC_CMD_EPC|BDC_CMD_EPN(ep->ep_num)|BDC_SUB_CMD_ADD_EP;
+
+	dev_dbg(bdc->dev, "cmd_sc=%x param2=%08x\n", cmd_sc, param2);
+	ret = bdc_submit_cmd(bdc, cmd_sc, param0, param1, param2);
+	if (ret) {
+		dev_err(bdc->dev, "command failed :%x\n", ret);
+		return ret;
+	}
+	ep_bd_list_reinit(ep);
+
+	return ret;
+}
+
+/*
+ * Change the HW deq pointer, if this command is successful, HW will start
+ * fetching the next bd from address dma_addr.
+ */
+int bdc_ep_bla(struct bdc *bdc, struct bdc_ep *ep, dma_addr_t dma_addr)
+{
+	u32 param0, param1;
+	u32 cmd_sc = 0;
+
+	dev_dbg(bdc->dev, "%s: add=%08llx\n", __func__,
+				(unsigned long long)(dma_addr));
+	param0 = lower_32_bits(dma_addr);
+	param1 = upper_32_bits(dma_addr);
+	cpu_to_le32s(&param0);
+	cpu_to_le32s(&param1);
+
+	cmd_sc |= BDC_CMD_EPN(ep->ep_num)|BDC_CMD_BLA;
+	dev_dbg(bdc->dev, "cmd_sc=%x\n", cmd_sc);
+
+	return bdc_submit_cmd(bdc, cmd_sc, param0, param1, 0);
+}
+
+/* Set the address sent bu Host in SET_ADD request */
+int bdc_address_device(struct bdc *bdc, u32 add)
+{
+	u32 cmd_sc = 0;
+	u32 param2;
+
+	dev_dbg(bdc->dev, "%s: add=%d\n", __func__, add);
+	cmd_sc |=  BDC_SUB_CMD_ADD|BDC_CMD_DVC;
+	param2 = add & 0x7f;
+
+	return bdc_submit_cmd(bdc, cmd_sc, 0, 0, param2);
+}
+
+/* Send a Function Wake notification packet using FH command */
+int bdc_function_wake_fh(struct bdc *bdc, u8 intf)
+{
+	u32 param0, param1;
+	u32 cmd_sc = 0;
+
+	param0 = param1 = 0;
+	dev_dbg(bdc->dev, "%s intf=%d\n", __func__, intf);
+	cmd_sc  |=  BDC_CMD_FH;
+	param0 |= TRA_PACKET;
+	param0 |= (bdc->dev_addr << 25);
+	param1 |= DEV_NOTF_TYPE;
+	param1 |= (FWK_SUBTYPE<<4);
+	dev_dbg(bdc->dev, "param0=%08x param1=%08x\n", param0, param1);
+
+	return bdc_submit_cmd(bdc, cmd_sc, param0, param1, 0);
+}
+
+/* Send a Function Wake notification packet using DNC command */
+int bdc_function_wake(struct bdc *bdc, u8 intf)
+{
+	u32 cmd_sc = 0;
+	u32 param2 = 0;
+
+	dev_dbg(bdc->dev, "%s intf=%d", __func__, intf);
+	param2 |= intf;
+	cmd_sc |= BDC_SUB_CMD_FWK|BDC_CMD_DNC;
+
+	return bdc_submit_cmd(bdc, cmd_sc, 0, 0, param2);
+}
+
+/* Stall the endpoint */
+int bdc_ep_set_stall(struct bdc *bdc, int epnum)
+{
+	u32 cmd_sc = 0;
+
+	dev_dbg(bdc->dev, "%s epnum=%d\n", __func__, epnum);
+	/* issue a stall endpoint command */
+	cmd_sc |=  BDC_SUB_CMD_EP_STL | BDC_CMD_EPN(epnum) | BDC_CMD_EPO;
+
+	return bdc_submit_cmd(bdc, cmd_sc, 0, 0, 0);
+}
+
+/* resets the endpoint, called when host sends CLEAR_FEATURE(HALT) */
+int bdc_ep_clear_stall(struct bdc *bdc, int epnum)
+{
+	struct bdc_ep *ep;
+	u32 cmd_sc = 0;
+	int ret;
+
+	dev_dbg(bdc->dev, "%s: epnum=%d\n", __func__, epnum);
+	ep = bdc->bdc_ep_array[epnum];
+	/*
+	 * If we are not in stalled then stall Endpoint and issue clear stall,
+	 * his will reset the seq number for non EP0.
+	 */
+	if (epnum != 1) {
+		/* if the endpoint it not stallled */
+		if (!(ep->flags & BDC_EP_STALL)) {
+			ret = bdc_ep_set_stall(bdc, epnum);
+				if (ret)
+					return ret;
+		}
+	}
+	/* Preserve the seq number for ep0 only */
+	if (epnum != 1)
+		cmd_sc |= BDC_CMD_EPO_RST_SN;
+
+	/* issue a reset endpoint command */
+	cmd_sc |=  BDC_SUB_CMD_EP_RST | BDC_CMD_EPN(epnum) | BDC_CMD_EPO;
+
+	ret = bdc_submit_cmd(bdc, cmd_sc, 0, 0, 0);
+	if (ret) {
+		dev_err(bdc->dev, "command failed:%x\n", ret);
+		return ret;
+	}
+	bdc_notify_xfr(bdc, epnum);
+
+	return ret;
+}
+
+/* Stop the endpoint, called when software wants to dequeue some request */
+int bdc_stop_ep(struct bdc *bdc, int epnum)
+{
+	struct bdc_ep *ep;
+	u32 cmd_sc = 0;
+	int ret;
+
+	ep = bdc->bdc_ep_array[epnum];
+	dev_dbg(bdc->dev, "%s: ep:%s ep->flags:%08x\n", __func__,
+						ep->name, ep->flags);
+	/* Endpoint has to be in running state to execute stop ep command */
+	if (!(ep->flags & BDC_EP_ENABLED)) {
+		dev_err(bdc->dev, "stop endpoint called for disabled ep\n");
+		return   -EINVAL;
+	}
+	if ((ep->flags & BDC_EP_STALL) || (ep->flags & BDC_EP_STOP))
+		return 0;
+
+	/* issue a stop endpoint command */
+	cmd_sc |= BDC_CMD_EP0_XSD | BDC_SUB_CMD_EP_STP
+				| BDC_CMD_EPN(epnum) | BDC_CMD_EPO;
+
+	ret = bdc_submit_cmd(bdc, cmd_sc, 0, 0, 0);
+	if (ret) {
+		dev_err(bdc->dev,
+			"stop endpoint command didn't complete:%d ep:%s\n",
+			ret, ep->name);
+		return ret;
+	}
+	ep->flags |= BDC_EP_STOP;
+	bdc_dump_epsts(bdc);
+
+	return ret;
+}
diff --git a/drivers/usb/gadget/udc/bdc/bdc_cmd.h b/drivers/usb/gadget/udc/bdc/bdc_cmd.h
new file mode 100644
index 000000000000..61d0e3bf9853
--- /dev/null
+++ b/drivers/usb/gadget/udc/bdc/bdc_cmd.h
@@ -0,0 +1,29 @@
+/*
+ * bdc_cmd.h - header for the BDC debug functions
+ *
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * Author: Ashwini Pahuja
+ *
+ * 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_BDC_CMD_H__
+#define __LINUX_BDC_CMD_H__
+
+/* Command operations */
+int bdc_address_device(struct bdc *, u32);
+int bdc_config_ep(struct bdc *, struct bdc_ep *);
+int bdc_dconfig_ep(struct bdc *, struct bdc_ep *);
+int bdc_stop_ep(struct bdc *, int);
+int bdc_ep_set_stall(struct bdc *, int);
+int bdc_ep_clear_stall(struct bdc *, int);
+int bdc_ep_set_halt(struct bdc_ep *, u32 , int);
+int bdc_ep_bla(struct bdc *, struct bdc_ep *, dma_addr_t);
+int bdc_function_wake(struct bdc*, u8);
+int bdc_function_wake_fh(struct bdc*, u8);
+
+#endif /* __LINUX_BDC_CMD_H__ */
diff --git a/drivers/usb/gadget/udc/bdc/bdc_core.c b/drivers/usb/gadget/udc/bdc/bdc_core.c
new file mode 100644
index 000000000000..c6dfef8c7bbc
--- /dev/null
+++ b/drivers/usb/gadget/udc/bdc/bdc_core.c
@@ -0,0 +1,533 @@
+/*
+ * bdc_core.c - BRCM BDC USB3.0 device controller core operations
+ *
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * Author: Ashwini Pahuja
+ *
+ * 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/slab.h>
+#include <linux/spinlock.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/ioport.h>
+#include <linux/io.h>
+#include <linux/list.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmapool.h>
+#include <linux/of.h>
+#include <linux/moduleparam.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/gadget.h>
+
+#include "bdc.h"
+#include "bdc_dbg.h"
+
+/* Poll till controller status is not OIP */
+static int poll_oip(struct bdc *bdc, int usec)
+{
+	u32 status;
+	/* Poll till STS!= OIP */
+	while (usec) {
+		status = bdc_readl(bdc->regs, BDC_BDCSC);
+		if (BDC_CSTS(status) != BDC_OIP) {
+			dev_dbg(bdc->dev,
+				"poll_oip complete status=%d",
+				BDC_CSTS(status));
+			return 0;
+		}
+		udelay(10);
+		usec -= 10;
+	}
+	dev_err(bdc->dev, "Err: operation timedout BDCSC: 0x%08x\n", status);
+
+	return -ETIMEDOUT;
+}
+
+/* Stop the BDC controller */
+int bdc_stop(struct bdc *bdc)
+{
+	int ret;
+	u32 temp;
+
+	dev_dbg(bdc->dev, "%s ()\n\n", __func__);
+	temp = bdc_readl(bdc->regs, BDC_BDCSC);
+	/* Check if BDC is already halted */
+	if (BDC_CSTS(temp) == BDC_HLT) {
+		dev_vdbg(bdc->dev, "BDC already halted\n");
+		return 0;
+	}
+	temp &= ~BDC_COP_MASK;
+	temp |= BDC_COS|BDC_COP_STP;
+	bdc_writel(bdc->regs, BDC_BDCSC, temp);
+
+	ret = poll_oip(bdc, BDC_COP_TIMEOUT);
+	if (ret)
+		dev_err(bdc->dev, "bdc stop operation failed");
+
+	return ret;
+}
+
+/* Issue a reset to BDC controller */
+int bdc_reset(struct bdc *bdc)
+{
+	u32 temp;
+	int ret;
+
+	dev_dbg(bdc->dev, "%s ()\n", __func__);
+	/* First halt the controller */
+	ret = bdc_stop(bdc);
+	if (ret)
+		return ret;
+
+	temp = bdc_readl(bdc->regs, BDC_BDCSC);
+	temp &= ~BDC_COP_MASK;
+	temp |= BDC_COS|BDC_COP_RST;
+	bdc_writel(bdc->regs, BDC_BDCSC, temp);
+	ret = poll_oip(bdc, BDC_COP_TIMEOUT);
+	if (ret)
+		dev_err(bdc->dev, "bdc reset operation failed");
+
+	return ret;
+}
+
+/* Run the BDC controller */
+int bdc_run(struct bdc *bdc)
+{
+	u32 temp;
+	int ret;
+
+	dev_dbg(bdc->dev, "%s ()\n", __func__);
+	temp = bdc_readl(bdc->regs, BDC_BDCSC);
+	/* if BDC is already in running state then do not do anything */
+	if (BDC_CSTS(temp) == BDC_NOR) {
+		dev_warn(bdc->dev, "bdc is already in running state\n");
+		return 0;
+	}
+	temp &= ~BDC_COP_MASK;
+	temp |= BDC_COP_RUN;
+	temp |= BDC_COS;
+	bdc_writel(bdc->regs, BDC_BDCSC, temp);
+	ret = poll_oip(bdc, BDC_COP_TIMEOUT);
+	if (ret) {
+		dev_err(bdc->dev, "bdc run operation failed:%d", ret);
+		return ret;
+	}
+	temp = bdc_readl(bdc->regs, BDC_BDCSC);
+	if (BDC_CSTS(temp) != BDC_NOR) {
+		dev_err(bdc->dev, "bdc not in normal mode after RUN op :%d\n",
+								BDC_CSTS(temp));
+		return -ESHUTDOWN;
+	}
+
+	return 0;
+}
+
+/*
+ * Present the termination to the host, typically called from upstream port
+ * event with Vbus present =1
+ */
+void bdc_softconn(struct bdc *bdc)
+{
+	u32 uspc;
+
+	uspc = bdc_readl(bdc->regs, BDC_USPC);
+	uspc &= ~BDC_PST_MASK;
+	uspc |= BDC_LINK_STATE_RX_DET;
+	uspc |= BDC_SWS;
+	dev_dbg(bdc->dev, "%s () uspc=%08x\n", __func__, uspc);
+	bdc_writel(bdc->regs, BDC_USPC, uspc);
+}
+
+/* Remove the termination */
+void bdc_softdisconn(struct bdc *bdc)
+{
+	u32 uspc;
+
+	uspc = bdc_readl(bdc->regs, BDC_USPC);
+	uspc |= BDC_SDC;
+	uspc &= ~BDC_SCN;
+	dev_dbg(bdc->dev, "%s () uspc=%x\n", __func__, uspc);
+	bdc_writel(bdc->regs, BDC_USPC, uspc);
+}
+
+/* Set up the scratchpad buffer array and scratchpad buffers, if needed. */
+static int scratchpad_setup(struct bdc *bdc)
+{
+	int sp_buff_size;
+	u32 low32;
+	u32 upp32;
+
+	sp_buff_size = BDC_SPB(bdc_readl(bdc->regs, BDC_BDCCFG0));
+	dev_dbg(bdc->dev, "%s() sp_buff_size=%d\n", __func__, sp_buff_size);
+	if (!sp_buff_size) {
+		dev_dbg(bdc->dev, "Scratchpad buffer not needed\n");
+		return 0;
+	}
+	/* Refer to BDC spec, Table 4 for description of SPB */
+	sp_buff_size = 1 << (sp_buff_size + 5);
+	dev_dbg(bdc->dev, "Allocating %d bytes for scratchpad\n", sp_buff_size);
+	bdc->scratchpad.buff  =  dma_zalloc_coherent(bdc->dev, sp_buff_size,
+					&bdc->scratchpad.sp_dma, GFP_KERNEL);
+
+	if (!bdc->scratchpad.buff)
+		goto fail;
+
+	bdc->sp_buff_size = sp_buff_size;
+	bdc->scratchpad.size = sp_buff_size;
+	low32 = lower_32_bits(bdc->scratchpad.sp_dma);
+	upp32 = upper_32_bits(bdc->scratchpad.sp_dma);
+	cpu_to_le32s(&low32);
+	cpu_to_le32s(&upp32);
+	bdc_writel(bdc->regs, BDC_SPBBAL, low32);
+	bdc_writel(bdc->regs, BDC_SPBBAH, upp32);
+	return 0;
+
+fail:
+	bdc->scratchpad.buff = NULL;
+
+	return -ENOMEM;
+}
+
+/* Allocate the status report ring */
+static int setup_srr(struct bdc *bdc, int interrupter)
+{
+	dev_dbg(bdc->dev, "%s() NUM_SR_ENTRIES:%d\n", __func__, NUM_SR_ENTRIES);
+	/* Reset the SRR */
+	bdc_writel(bdc->regs, BDC_SRRINT(0), BDC_SRR_RWS | BDC_SRR_RST);
+	bdc->srr.dqp_index = 0;
+	/* allocate the status report descriptors */
+	bdc->srr.sr_bds = dma_zalloc_coherent(
+					bdc->dev,
+					NUM_SR_ENTRIES * sizeof(struct bdc_bd),
+					&bdc->srr.dma_addr,
+					GFP_KERNEL);
+	if (!bdc->srr.sr_bds)
+		return -ENOMEM;
+
+	return 0;
+}
+
+/* Initialize the HW regs and internal data structures */
+static void bdc_mem_init(struct bdc *bdc, bool reinit)
+{
+	u8 size = 0;
+	u32 usb2_pm;
+	u32 low32;
+	u32 upp32;
+	u32 temp;
+
+	dev_dbg(bdc->dev, "%s ()\n", __func__);
+	bdc->ep0_state = WAIT_FOR_SETUP;
+	bdc->dev_addr = 0;
+	bdc->srr.eqp_index = 0;
+	bdc->srr.dqp_index = 0;
+	bdc->zlp_needed = false;
+	bdc->delayed_status = false;
+
+	bdc_writel(bdc->regs, BDC_SPBBAL, bdc->scratchpad.sp_dma);
+	/* Init the SRR */
+	temp = BDC_SRR_RWS | BDC_SRR_RST;
+	/* Reset the SRR */
+	bdc_writel(bdc->regs, BDC_SRRINT(0), temp);
+	dev_dbg(bdc->dev, "bdc->srr.sr_bds =%p\n", bdc->srr.sr_bds);
+	temp = lower_32_bits(bdc->srr.dma_addr);
+	size = fls(NUM_SR_ENTRIES) - 2;
+	temp |= size;
+	dev_dbg(bdc->dev, "SRRBAL[0]=%08x NUM_SR_ENTRIES:%d size:%d\n",
+						temp, NUM_SR_ENTRIES, size);
+
+	low32 = lower_32_bits(temp);
+	upp32 = upper_32_bits(bdc->srr.dma_addr);
+	cpu_to_le32s(&low32);
+	cpu_to_le32s(&upp32);
+
+	/* Write the dma addresses into regs*/
+	bdc_writel(bdc->regs, BDC_SRRBAL(0), low32);
+	bdc_writel(bdc->regs, BDC_SRRBAH(0), upp32);
+
+	temp = bdc_readl(bdc->regs, BDC_SRRINT(0));
+	temp |= BDC_SRR_IE;
+	temp &= ~(BDC_SRR_RST | BDC_SRR_RWS);
+	bdc_writel(bdc->regs, BDC_SRRINT(0), temp);
+
+	/* Set the Interrupt Coalescence ~500 usec */
+	temp = bdc_readl(bdc->regs, BDC_INTCTLS(0));
+	temp &= ~0xffff;
+	temp |= INT_CLS;
+	bdc_writel(bdc->regs, BDC_INTCTLS(0), temp);
+
+	usb2_pm = bdc_readl(bdc->regs, BDC_USPPM2);
+	dev_dbg(bdc->dev, "usb2_pm=%08x", usb2_pm);
+	/* Enable hardware LPM Enable */
+	usb2_pm |= BDC_HLE;
+	bdc_writel(bdc->regs, BDC_USPPM2, usb2_pm);
+
+	/* readback for debug */
+	usb2_pm = bdc_readl(bdc->regs, BDC_USPPM2);
+	dev_dbg(bdc->dev, "usb2_pm=%08x\n", usb2_pm);
+
+	/* Disable any unwanted SR's on SRR */
+	temp = bdc_readl(bdc->regs, BDC_BDCSC);
+	/* We don't want Microframe counter wrap SR */
+	temp |= BDC_MASK_MCW;
+	bdc_writel(bdc->regs, BDC_BDCSC, temp);
+
+	/*
+	 * In some error cases, driver has to reset the entire BDC controller
+	 * in that case reinit is passed as 1
+	 */
+	if (reinit) {
+		/* Enable interrupts */
+		temp = bdc_readl(bdc->regs, BDC_BDCSC);
+		temp |= BDC_GIE;
+		bdc_writel(bdc->regs, BDC_BDCSC, temp);
+		/* Init scratchpad to 0 */
+		memset(bdc->scratchpad.buff, 0, bdc->sp_buff_size);
+		/* Initialize SRR to 0 */
+		memset(bdc->srr.sr_bds, 0,
+					NUM_SR_ENTRIES * sizeof(struct bdc_bd));
+	} else {
+		/* One time initiaization only */
+		/* Enable status report function pointers */
+		bdc->sr_handler[0] = bdc_sr_xsf;
+		bdc->sr_handler[1] = bdc_sr_uspc;
+
+		/* EP0 status report function pointers */
+		bdc->sr_xsf_ep0[0] = bdc_xsf_ep0_setup_recv;
+		bdc->sr_xsf_ep0[1] = bdc_xsf_ep0_data_start;
+		bdc->sr_xsf_ep0[2] = bdc_xsf_ep0_status_start;
+	}
+}
+
+/* Free the dynamic memory */
+static void bdc_mem_free(struct bdc *bdc)
+{
+	dev_dbg(bdc->dev, "%s\n", __func__);
+	/* Free SRR */
+	if (bdc->srr.sr_bds)
+		dma_free_coherent(bdc->dev,
+					NUM_SR_ENTRIES * sizeof(struct bdc_bd),
+					bdc->srr.sr_bds, bdc->srr.dma_addr);
+
+	/* Free scratchpad */
+	if (bdc->scratchpad.buff)
+		dma_free_coherent(bdc->dev, bdc->sp_buff_size,
+				bdc->scratchpad.buff, bdc->scratchpad.sp_dma);
+
+	/* Destroy the dma pools */
+	if (bdc->bd_table_pool)
+		dma_pool_destroy(bdc->bd_table_pool);
+
+	/* Free the bdc_ep array */
+	kfree(bdc->bdc_ep_array);
+
+	bdc->srr.sr_bds = NULL;
+	bdc->scratchpad.buff = NULL;
+	bdc->bd_table_pool = NULL;
+	bdc->bdc_ep_array = NULL;
+}
+
+/*
+ * bdc reinit gives a controller reset and reinitialize the registers,
+ * called from disconnect/bus reset scenario's, to ensure proper HW cleanup
+ */
+int bdc_reinit(struct bdc *bdc)
+{
+	int ret;
+
+	dev_dbg(bdc->dev, "%s\n", __func__);
+	ret = bdc_stop(bdc);
+	if (ret)
+		goto out;
+
+	ret = bdc_reset(bdc);
+	if (ret)
+		goto out;
+
+	/* the reinit flag is 1 */
+	bdc_mem_init(bdc, true);
+	ret = bdc_run(bdc);
+out:
+	bdc->reinit = false;
+
+	return ret;
+}
+
+/* Allocate all the dyanmic memory */
+static int bdc_mem_alloc(struct bdc *bdc)
+{
+	u32 page_size;
+	unsigned int num_ieps, num_oeps;
+
+	dev_dbg(bdc->dev,
+		"%s() NUM_BDS_PER_TABLE:%d\n", __func__,
+		NUM_BDS_PER_TABLE);
+	page_size = BDC_PGS(bdc_readl(bdc->regs, BDC_BDCCFG0));
+	/* page size is 2^pgs KB */
+	page_size = 1 << page_size;
+	/* KB */
+	page_size <<= 10;
+	dev_dbg(bdc->dev, "page_size=%d\n", page_size);
+
+	/* Create a pool of bd tables */
+	bdc->bd_table_pool =
+	    dma_pool_create("BDC BD tables", bdc->dev, NUM_BDS_PER_TABLE * 16,
+								16, page_size);
+
+	if (!bdc->bd_table_pool)
+		goto fail;
+
+	if (scratchpad_setup(bdc))
+		goto fail;
+
+	/* read from regs */
+	num_ieps = NUM_NCS(bdc_readl(bdc->regs, BDC_FSCNIC));
+	num_oeps = NUM_NCS(bdc_readl(bdc->regs, BDC_FSCNOC));
+	/* +2: 1 for ep0 and the other is rsvd i.e. bdc_ep[0] is rsvd */
+	bdc->num_eps = num_ieps + num_oeps + 2;
+	dev_dbg(bdc->dev,
+		"ieps:%d eops:%d num_eps:%d\n",
+		num_ieps, num_oeps, bdc->num_eps);
+	/* allocate array of ep pointers */
+	bdc->bdc_ep_array = kcalloc(bdc->num_eps, sizeof(struct bdc_ep *),
+								GFP_KERNEL);
+	if (!bdc->bdc_ep_array)
+		goto fail;
+
+	dev_dbg(bdc->dev, "Allocating sr report0\n");
+	if (setup_srr(bdc, 0))
+		goto fail;
+
+	return 0;
+fail:
+	dev_warn(bdc->dev, "Couldn't initialize memory\n");
+	bdc_mem_free(bdc);
+
+	return -ENOMEM;
+}
+
+/* opposite to bdc_hw_init */
+static void bdc_hw_exit(struct bdc *bdc)
+{
+	dev_dbg(bdc->dev, "%s ()\n", __func__);
+	bdc_mem_free(bdc);
+}
+
+/* Initialize the bdc HW and memory */
+static int bdc_hw_init(struct bdc *bdc)
+{
+	int ret;
+
+	dev_dbg(bdc->dev, "%s ()\n", __func__);
+	ret = bdc_reset(bdc);
+	if (ret) {
+		dev_err(bdc->dev, "err resetting bdc abort bdc init%d\n", ret);
+		return ret;
+	}
+	ret = bdc_mem_alloc(bdc);
+	if (ret) {
+		dev_err(bdc->dev, "Mem alloc failed, aborting\n");
+		return -ENOMEM;
+	}
+	bdc_mem_init(bdc, 0);
+	bdc_dbg_regs(bdc);
+	dev_dbg(bdc->dev, "HW Init done\n");
+
+	return 0;
+}
+
+static int bdc_probe(struct platform_device *pdev)
+{
+	struct bdc *bdc;
+	struct resource *res;
+	int ret = -ENOMEM;
+	int irq;
+	u32 temp;
+	struct device *dev = &pdev->dev;
+
+	dev_dbg(dev, "%s()\n", __func__);
+	bdc = devm_kzalloc(dev, sizeof(*bdc), GFP_KERNEL);
+	if (!bdc)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	bdc->regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(bdc->regs)) {
+		dev_err(dev, "ioremap error\n");
+		return -ENOMEM;
+	}
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(dev, "platform_get_irq failed:%d\n", irq);
+		return irq;
+	}
+	spin_lock_init(&bdc->lock);
+	platform_set_drvdata(pdev, bdc);
+	bdc->irq = irq;
+	bdc->dev = dev;
+	dev_dbg(bdc->dev, "bdc->regs: %p irq=%d\n", bdc->regs, bdc->irq);
+
+	temp = bdc_readl(bdc->regs, BDC_BDCSC);
+	if ((temp & BDC_P64) &&
+			!dma_set_mask_and_coherent(dev, DMA_BIT_MASK(64))) {
+		dev_dbg(bdc->dev, "Using 64-bit address\n");
+	} else {
+		ret = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32));
+		if (ret) {
+			dev_err(bdc->dev, "No suitable DMA config available, abort\n");
+			return -ENOTSUPP;
+		}
+		dev_dbg(bdc->dev, "Using 32-bit address\n");
+	}
+	ret = bdc_hw_init(bdc);
+	if (ret) {
+		dev_err(bdc->dev, "BDC init failure:%d\n", ret);
+		return ret;
+	}
+	ret = bdc_udc_init(bdc);
+	if (ret) {
+		dev_err(bdc->dev, "BDC Gadget init failure:%d\n", ret);
+		goto cleanup;
+	}
+	return 0;
+
+cleanup:
+	bdc_hw_exit(bdc);
+
+	return ret;
+}
+
+static int bdc_remove(struct platform_device *pdev)
+{
+	struct bdc *bdc;
+
+	bdc  = platform_get_drvdata(pdev);
+	dev_dbg(bdc->dev, "%s ()\n", __func__);
+	bdc_udc_exit(bdc);
+	bdc_hw_exit(bdc);
+
+	return 0;
+}
+
+static struct platform_driver bdc_driver = {
+	.driver		= {
+		.name	= BRCM_BDC_NAME,
+		.owner	= THIS_MODULE
+	},
+	.probe		= bdc_probe,
+	.remove		= bdc_remove,
+};
+
+module_platform_driver(bdc_driver);
+MODULE_AUTHOR("Ashwini Pahuja <ashwini.linux@gmail.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION(BRCM_BDC_DESC);
diff --git a/drivers/usb/gadget/udc/bdc/bdc_dbg.c b/drivers/usb/gadget/udc/bdc/bdc_dbg.c
new file mode 100644
index 000000000000..5945dbc47825
--- /dev/null
+++ b/drivers/usb/gadget/udc/bdc/bdc_dbg.c
@@ -0,0 +1,123 @@
+/*
+ * bdc_dbg.c - BRCM BDC USB3.0 device controller debug functions
+ *
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * Author: Ashwini Pahuja
+ *
+ * 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 "bdc.h"
+#include "bdc_dbg.h"
+
+void bdc_dbg_regs(struct bdc *bdc)
+{
+	u32 temp;
+
+	dev_vdbg(bdc->dev, "bdc->regs:%p\n", bdc->regs);
+	temp = bdc_readl(bdc->regs, BDC_BDCCFG0);
+	dev_vdbg(bdc->dev, "bdccfg0:0x%08x\n", temp);
+	temp = bdc_readl(bdc->regs, BDC_BDCCFG1);
+	dev_vdbg(bdc->dev, "bdccfg1:0x%08x\n", temp);
+	temp = bdc_readl(bdc->regs, BDC_BDCCAP0);
+	dev_vdbg(bdc->dev, "bdccap0:0x%08x\n", temp);
+	temp = bdc_readl(bdc->regs, BDC_BDCCAP1);
+	dev_vdbg(bdc->dev, "bdccap1:0x%08x\n", temp);
+	temp = bdc_readl(bdc->regs, BDC_USPC);
+	dev_vdbg(bdc->dev, "uspc:0x%08x\n", temp);
+	temp = bdc_readl(bdc->regs, BDC_DVCSA);
+	dev_vdbg(bdc->dev, "dvcsa:0x%08x\n", temp);
+	temp = bdc_readl(bdc->regs, BDC_DVCSB);
+	dev_vdbg(bdc->dev, "dvcsb:0x%x08\n", temp);
+}
+
+void bdc_dump_epsts(struct bdc *bdc)
+{
+	u32 temp;
+
+	temp = bdc_readl(bdc->regs, BDC_EPSTS0(0));
+	dev_vdbg(bdc->dev, "BDC_EPSTS0:0x%08x\n", temp);
+
+	temp = bdc_readl(bdc->regs, BDC_EPSTS1(0));
+	dev_vdbg(bdc->dev, "BDC_EPSTS1:0x%x\n", temp);
+
+	temp = bdc_readl(bdc->regs, BDC_EPSTS2(0));
+	dev_vdbg(bdc->dev, "BDC_EPSTS2:0x%08x\n", temp);
+
+	temp = bdc_readl(bdc->regs, BDC_EPSTS3(0));
+	dev_vdbg(bdc->dev, "BDC_EPSTS3:0x%08x\n", temp);
+
+	temp = bdc_readl(bdc->regs, BDC_EPSTS4(0));
+	dev_vdbg(bdc->dev, "BDC_EPSTS4:0x%08x\n", temp);
+
+	temp = bdc_readl(bdc->regs, BDC_EPSTS5(0));
+	dev_vdbg(bdc->dev, "BDC_EPSTS5:0x%08x\n", temp);
+
+	temp = bdc_readl(bdc->regs, BDC_EPSTS6(0));
+	dev_vdbg(bdc->dev, "BDC_EPSTS6:0x%08x\n", temp);
+
+	temp = bdc_readl(bdc->regs, BDC_EPSTS7(0));
+	dev_vdbg(bdc->dev, "BDC_EPSTS7:0x%08x\n", temp);
+}
+
+void bdc_dbg_srr(struct bdc *bdc, u32 srr_num)
+{
+	struct bdc_sr *sr;
+	dma_addr_t addr;
+	int i;
+
+	sr = bdc->srr.sr_bds;
+	addr = bdc->srr.dma_addr;
+	dev_vdbg(bdc->dev, "bdc_dbg_srr sr:%p dqp_index:%d\n",
+						sr, bdc->srr.dqp_index);
+	for (i = 0; i < NUM_SR_ENTRIES; i++) {
+		sr = &bdc->srr.sr_bds[i];
+		dev_vdbg(bdc->dev, "%llx %08x %08x %08x %08x\n",
+					(unsigned long long)addr,
+					le32_to_cpu(sr->offset[0]),
+					le32_to_cpu(sr->offset[1]),
+					le32_to_cpu(sr->offset[2]),
+					le32_to_cpu(sr->offset[3]));
+		addr += sizeof(*sr);
+	}
+}
+
+void bdc_dbg_bd_list(struct bdc *bdc, struct bdc_ep *ep)
+{
+	struct bd_list *bd_list = &ep->bd_list;
+	struct bd_table *bd_table;
+	struct bdc_bd *bd;
+	int tbi, bdi, gbdi;
+	dma_addr_t dma;
+
+	gbdi = 0;
+	dev_vdbg(bdc->dev,
+		"Dump bd list for %s epnum:%d\n",
+		ep->name, ep->ep_num);
+
+	dev_vdbg(bdc->dev,
+		"tabs:%d max_bdi:%d eqp_bdi:%d hwd_bdi:%d num_bds_table:%d\n",
+		bd_list->num_tabs, bd_list->max_bdi, bd_list->eqp_bdi,
+		bd_list->hwd_bdi, bd_list->num_bds_table);
+
+	for (tbi = 0; tbi < bd_list->num_tabs; tbi++) {
+		bd_table = bd_list->bd_table_array[tbi];
+		for (bdi = 0; bdi < bd_list->num_bds_table; bdi++) {
+			bd =  bd_table->start_bd + bdi;
+			dma = bd_table->dma + (sizeof(struct bdc_bd) * bdi);
+			dev_vdbg(bdc->dev,
+				"tbi:%2d bdi:%2d gbdi:%2d virt:%p phys:%llx %08x %08x %08x %08x\n",
+				tbi, bdi, gbdi++, bd, (unsigned long long)dma,
+				le32_to_cpu(bd->offset[0]),
+				le32_to_cpu(bd->offset[1]),
+				le32_to_cpu(bd->offset[2]),
+				le32_to_cpu(bd->offset[3]));
+		}
+		dev_vdbg(bdc->dev, "\n\n");
+	}
+}
diff --git a/drivers/usb/gadget/udc/bdc/bdc_dbg.h b/drivers/usb/gadget/udc/bdc/bdc_dbg.h
new file mode 100644
index 000000000000..338a6c701315
--- /dev/null
+++ b/drivers/usb/gadget/udc/bdc/bdc_dbg.h
@@ -0,0 +1,37 @@
+/*
+ * bdc_dbg.h - header for the BDC debug functions
+ *
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * Author: Ashwini Pahuja
+ *
+ * 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_BDC_DBG_H__
+#define __LINUX_BDC_DBG_H__
+
+#include "bdc.h"
+
+#ifdef CONFIG_USB_GADGET_VERBOSE
+void bdc_dbg_bd_list(struct bdc *, struct bdc_ep*);
+void bdc_dbg_srr(struct bdc *, u32);
+void bdc_dbg_regs(struct bdc *);
+void bdc_dump_epsts(struct bdc *);
+#else
+static inline void bdc_dbg_regs(struct bdc *bdc)
+{ }
+
+static inline void bdc_dbg_srr(struct bdc *bdc, u32 srr_num)
+{ }
+
+static inline void bdc_dbg_bd_list(struct bdc *bdc, struct bdc_ep *ep)
+{ }
+
+static inline void bdc_dump_epsts(struct bdc *bdc)
+{ }
+#endif /* CONFIG_USB_GADGET_VERBOSE */
+#endif /* __LINUX_BDC_DBG_H__ */
diff --git a/drivers/usb/gadget/udc/bdc/bdc_ep.c b/drivers/usb/gadget/udc/bdc/bdc_ep.c
new file mode 100644
index 000000000000..ff67ceac77c4
--- /dev/null
+++ b/drivers/usb/gadget/udc/bdc/bdc_ep.c
@@ -0,0 +1,2024 @@
+/*
+ * bdc_ep.c - BRCM BDC USB3.0 device controller endpoint related functions
+ *
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * Author: Ashwini Pahuja
+ *
+ * Based on drivers under drivers/usb/
+ *
+ * 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/pci.h>
+#include <linux/dma-mapping.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/dmapool.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 <linux/platform_device.h>
+#include <linux/usb/composite.h>
+
+#include "bdc.h"
+#include "bdc_ep.h"
+#include "bdc_cmd.h"
+#include "bdc_dbg.h"
+
+static const char * const ep0_state_string[] =  {
+	"WAIT_FOR_SETUP",
+	"WAIT_FOR_DATA_START",
+	"WAIT_FOR_DATA_XMIT",
+	"WAIT_FOR_STATUS_START",
+	"WAIT_FOR_STATUS_XMIT",
+	"STATUS_PENDING"
+};
+
+/* Free the bdl during ep disable */
+static void ep_bd_list_free(struct bdc_ep *ep, u32 num_tabs)
+{
+	struct bd_list *bd_list = &ep->bd_list;
+	struct bdc *bdc = ep->bdc;
+	struct bd_table *bd_table;
+	int index;
+
+	dev_dbg(bdc->dev, "%s ep:%s num_tabs:%d\n",
+				 __func__, ep->name, num_tabs);
+
+	if (!bd_list->bd_table_array) {
+		dev_dbg(bdc->dev, "%s already freed\n", ep->name);
+		return;
+	}
+	for (index = 0; index < num_tabs; index++) {
+		/*
+		 * check if the bd_table struct is allocated ?
+		 * if yes, then check if bd memory has been allocated, then
+		 * free the dma_pool and also the bd_table struct memory
+		*/
+		bd_table = bd_list->bd_table_array[index];
+		dev_dbg(bdc->dev, "bd_table:%p index:%d\n", bd_table, index);
+		if (!bd_table) {
+			dev_dbg(bdc->dev, "bd_table not allocated\n");
+			continue;
+		}
+		if (!bd_table->start_bd) {
+			dev_dbg(bdc->dev, "bd dma pool not allocted\n");
+			continue;
+		}
+
+		dev_dbg(bdc->dev,
+				"Free dma pool start_bd:%p dma:%llx\n",
+				bd_table->start_bd,
+				(unsigned long long)bd_table->dma);
+
+		dma_pool_free(bdc->bd_table_pool,
+				bd_table->start_bd,
+				bd_table->dma);
+		/* Free the bd_table structure */
+		kfree(bd_table);
+	}
+	/* Free the bd table array */
+	kfree(ep->bd_list.bd_table_array);
+}
+
+/*
+ * chain the tables, by insteting a chain bd at the end of prev_table, pointing
+ * to next_table
+ */
+static inline void chain_table(struct bd_table *prev_table,
+					struct bd_table *next_table,
+					u32 bd_p_tab)
+{
+	/* Chain the prev table to next table */
+	prev_table->start_bd[bd_p_tab-1].offset[0] =
+				cpu_to_le32(lower_32_bits(next_table->dma));
+
+	prev_table->start_bd[bd_p_tab-1].offset[1] =
+				cpu_to_le32(upper_32_bits(next_table->dma));
+
+	prev_table->start_bd[bd_p_tab-1].offset[2] =
+				0x0;
+
+	prev_table->start_bd[bd_p_tab-1].offset[3] =
+				cpu_to_le32(MARK_CHAIN_BD);
+}
+
+/* Allocate the bdl for ep, during config ep */
+static int ep_bd_list_alloc(struct bdc_ep *ep)
+{
+	struct bd_table *prev_table = NULL;
+	int index, num_tabs, bd_p_tab;
+	struct bdc *bdc = ep->bdc;
+	struct bd_table *bd_table;
+	dma_addr_t dma;
+
+	if (usb_endpoint_xfer_isoc(ep->desc))
+		num_tabs = NUM_TABLES_ISOCH;
+	else
+		num_tabs = NUM_TABLES;
+
+	bd_p_tab = NUM_BDS_PER_TABLE;
+	/* if there is only 1 table in bd list then loop chain to self */
+	dev_dbg(bdc->dev,
+		"%s ep:%p num_tabs:%d\n",
+		__func__, ep, num_tabs);
+
+	/* Allocate memory for table array */
+	ep->bd_list.bd_table_array = kzalloc(
+					num_tabs * sizeof(struct bd_table *),
+					GFP_ATOMIC);
+	if (!ep->bd_list.bd_table_array)
+		return -ENOMEM;
+
+	/* Allocate memory for each table */
+	for (index = 0; index < num_tabs; index++) {
+		/* Allocate memory for bd_table structure */
+		bd_table = kzalloc(sizeof(struct bd_table), GFP_ATOMIC);
+		if (!bd_table)
+			goto fail;
+
+		bd_table->start_bd = dma_pool_alloc(bdc->bd_table_pool,
+							GFP_ATOMIC,
+							&dma);
+		if (!bd_table->start_bd)
+			goto fail;
+
+		bd_table->dma = dma;
+
+		dev_dbg(bdc->dev,
+			"index:%d start_bd:%p dma=%08llx prev_table:%p\n",
+			index, bd_table->start_bd,
+			(unsigned long long)bd_table->dma, prev_table);
+
+		ep->bd_list.bd_table_array[index] = bd_table;
+		memset(bd_table->start_bd, 0, bd_p_tab * sizeof(struct bdc_bd));
+		if (prev_table)
+			chain_table(prev_table, bd_table, bd_p_tab);
+
+		prev_table = bd_table;
+	}
+	chain_table(prev_table, ep->bd_list.bd_table_array[0], bd_p_tab);
+	/* Memory allocation is successful, now init the internal fields */
+	ep->bd_list.num_tabs = num_tabs;
+	ep->bd_list.max_bdi  = (num_tabs * bd_p_tab) - 1;
+	ep->bd_list.num_tabs = num_tabs;
+	ep->bd_list.num_bds_table = bd_p_tab;
+	ep->bd_list.eqp_bdi = 0;
+	ep->bd_list.hwd_bdi = 0;
+
+	return 0;
+fail:
+	/* Free the bd_table_array, bd_table struct, bd's */
+	ep_bd_list_free(ep, num_tabs);
+
+	return -ENOMEM;
+}
+
+/* returns how many bd's are need for this transfer */
+static inline int bd_needed_req(struct bdc_req *req)
+{
+	int bd_needed = 0;
+	int remaining;
+
+	/* 1 bd needed for 0 byte transfer */
+	if (req->usb_req.length == 0)
+		return 1;
+
+	/* remaining bytes after tranfering all max BD size BD's */
+	remaining = req->usb_req.length % BD_MAX_BUFF_SIZE;
+	if (remaining)
+		bd_needed++;
+
+	/* How many maximum BUFF size BD's ? */
+	remaining = req->usb_req.length / BD_MAX_BUFF_SIZE;
+	bd_needed += remaining;
+
+	return bd_needed;
+}
+
+/* returns the bd index(bdi) corresponding to bd dma address */
+static int bd_add_to_bdi(struct bdc_ep *ep, dma_addr_t bd_dma_addr)
+{
+	struct bd_list *bd_list = &ep->bd_list;
+	dma_addr_t dma_first_bd, dma_last_bd;
+	struct bdc *bdc = ep->bdc;
+	struct bd_table *bd_table;
+	bool found = false;
+	int tbi, bdi;
+
+	dma_first_bd = dma_last_bd = 0;
+	dev_dbg(bdc->dev, "%s  %llx\n",
+			__func__, (unsigned long long)bd_dma_addr);
+	/*
+	 * Find in which table this bd_dma_addr belongs?, go through the table
+	 * array and compare addresses of first and last address of bd of each
+	 * table
+	 */
+	for (tbi = 0; tbi < bd_list->num_tabs; tbi++) {
+		bd_table = bd_list->bd_table_array[tbi];
+		dma_first_bd = bd_table->dma;
+		dma_last_bd = bd_table->dma +
+					(sizeof(struct bdc_bd) *
+					(bd_list->num_bds_table - 1));
+		dev_dbg(bdc->dev, "dma_first_bd:%llx dma_last_bd:%llx\n",
+					(unsigned long long)dma_first_bd,
+					(unsigned long long)dma_last_bd);
+		if (bd_dma_addr >= dma_first_bd && bd_dma_addr <= dma_last_bd) {
+			found = true;
+			break;
+		}
+	}
+	if (unlikely(!found)) {
+		dev_err(bdc->dev, "%s FATAL err, bd not found\n", __func__);
+		return -EINVAL;
+	}
+	/* Now we know the table, find the bdi */
+	bdi = (bd_dma_addr - dma_first_bd) / sizeof(struct bdc_bd);
+
+	/* return the global bdi, to compare with ep eqp_bdi */
+	return (bdi + (tbi * bd_list->num_bds_table));
+}
+
+/* returns the table index(tbi) of the given bdi */
+static int bdi_to_tbi(struct bdc_ep *ep, int bdi)
+{
+	int tbi;
+
+	tbi = bdi / ep->bd_list.num_bds_table;
+	dev_vdbg(ep->bdc->dev,
+		"bdi:%d num_bds_table:%d tbi:%d\n",
+		bdi, ep->bd_list.num_bds_table, tbi);
+
+	return tbi;
+}
+
+/* Find the bdi last bd in the transfer */
+static inline int find_end_bdi(struct bdc_ep *ep, int next_hwd_bdi)
+{
+	int end_bdi;
+
+	end_bdi = next_hwd_bdi - 1;
+	if (end_bdi < 0)
+		end_bdi = ep->bd_list.max_bdi - 1;
+	 else if ((end_bdi % (ep->bd_list.num_bds_table-1)) == 0)
+		end_bdi--;
+
+	return end_bdi;
+}
+
+/*
+ * How many transfer bd's are available on this ep bdl, chain bds are not
+ * counted in available bds
+ */
+static int bd_available_ep(struct bdc_ep *ep)
+{
+	struct bd_list *bd_list = &ep->bd_list;
+	int available1, available2;
+	struct bdc *bdc = ep->bdc;
+	int chain_bd1, chain_bd2;
+	int available_bd = 0;
+
+	available1 = available2 = chain_bd1 = chain_bd2 = 0;
+	/* if empty then we have all bd's available - number of chain bd's */
+	if (bd_list->eqp_bdi == bd_list->hwd_bdi)
+		return bd_list->max_bdi - bd_list->num_tabs;
+
+	/*
+	 * Depending upon where eqp and dqp pointers are, caculate number
+	 * of avaialble bd's
+	 */
+	if (bd_list->hwd_bdi < bd_list->eqp_bdi) {
+		/* available bd's are from eqp..max_bds + 0..dqp - chain_bds */
+		available1 = bd_list->max_bdi - bd_list->eqp_bdi;
+		available2 = bd_list->hwd_bdi;
+		chain_bd1 = available1 / bd_list->num_bds_table;
+		chain_bd2 = available2 / bd_list->num_bds_table;
+		dev_vdbg(bdc->dev, "chain_bd1:%d chain_bd2:%d\n",
+						chain_bd1, chain_bd2);
+		available_bd = available1 + available2 - chain_bd1 - chain_bd2;
+	} else {
+		/* available bd's are from eqp..dqp - number of chain bd's */
+		available1 = bd_list->hwd_bdi -  bd_list->eqp_bdi;
+		/* if gap between eqp and dqp is less than NUM_BDS_PER_TABLE */
+		if ((bd_list->hwd_bdi - bd_list->eqp_bdi)
+					<= bd_list->num_bds_table) {
+			/* If there any chain bd in between */
+			if (!(bdi_to_tbi(ep, bd_list->hwd_bdi)
+					== bdi_to_tbi(ep, bd_list->eqp_bdi))) {
+				available_bd = available1 - 1;
+			}
+		} else {
+			chain_bd1 = available1 / bd_list->num_bds_table;
+			available_bd = available1 - chain_bd1;
+		}
+	}
+	/*
+	 * we need to keep one extra bd to check if ring is full or empty so
+	 * reduce by 1
+	 */
+	available_bd--;
+	dev_vdbg(bdc->dev, "available_bd:%d\n", available_bd);
+
+	return available_bd;
+}
+
+/* Notify the hardware after queueing the bd to bdl */
+void bdc_notify_xfr(struct bdc *bdc, u32 epnum)
+{
+	struct bdc_ep *ep = bdc->bdc_ep_array[epnum];
+
+	dev_vdbg(bdc->dev, "%s epnum:%d\n", __func__, epnum);
+	/*
+	 * We don't have anyway to check if ep state is running,
+	 * except the software flags.
+	 */
+	if (unlikely(ep->flags & BDC_EP_STOP))
+		ep->flags &= ~BDC_EP_STOP;
+
+	bdc_writel(bdc->regs, BDC_XSFNTF, epnum);
+}
+
+/* returns the bd corresponding to bdi */
+static struct bdc_bd *bdi_to_bd(struct bdc_ep *ep, int bdi)
+{
+	int tbi = bdi_to_tbi(ep, bdi);
+	int local_bdi = 0;
+
+	local_bdi = bdi - (tbi * ep->bd_list.num_bds_table);
+	dev_vdbg(ep->bdc->dev,
+		"%s bdi:%d local_bdi:%d\n",
+		 __func__, bdi, local_bdi);
+
+	return (ep->bd_list.bd_table_array[tbi]->start_bd + local_bdi);
+}
+
+/* Advance the enqueue pointer */
+static void ep_bdlist_eqp_adv(struct bdc_ep *ep)
+{
+	ep->bd_list.eqp_bdi++;
+	/* if it's chain bd, then move to next */
+	if (((ep->bd_list.eqp_bdi + 1) % ep->bd_list.num_bds_table) == 0)
+		ep->bd_list.eqp_bdi++;
+
+	/* if the eqp is pointing to last + 1 then move back to 0 */
+	if (ep->bd_list.eqp_bdi == (ep->bd_list.max_bdi + 1))
+		ep->bd_list.eqp_bdi = 0;
+}
+
+/* Setup the first bd for ep0 transfer */
+static int setup_first_bd_ep0(struct bdc *bdc, struct bdc_req *req, u32 *dword3)
+{
+	u16 wValue;
+	u32 req_len;
+
+	req->ep->dir = 0;
+	req_len = req->usb_req.length;
+	switch (bdc->ep0_state) {
+	case WAIT_FOR_DATA_START:
+		*dword3 |= BD_TYPE_DS;
+		if (bdc->setup_pkt.bRequestType & USB_DIR_IN)
+			*dword3 |= BD_DIR_IN;
+
+		/* check if zlp will be needed */
+		wValue = le16_to_cpu(bdc->setup_pkt.wValue);
+		if ((wValue > req_len) &&
+				(req_len % bdc->gadget.ep0->maxpacket == 0)) {
+			dev_dbg(bdc->dev, "ZLP needed wVal:%d len:%d MaxP:%d\n",
+					wValue, req_len,
+					bdc->gadget.ep0->maxpacket);
+			bdc->zlp_needed = true;
+		}
+		break;
+
+	case WAIT_FOR_STATUS_START:
+		*dword3 |= BD_TYPE_SS;
+		if (!le16_to_cpu(bdc->setup_pkt.wLength) ||
+				!(bdc->setup_pkt.bRequestType & USB_DIR_IN))
+			*dword3 |= BD_DIR_IN;
+		break;
+	default:
+		dev_err(bdc->dev,
+			"Unknown ep0 state for queueing bd ep0_state:%s\n",
+			ep0_state_string[bdc->ep0_state]);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* Setup the bd dma descriptor for a given request */
+static int setup_bd_list_xfr(struct bdc *bdc, struct bdc_req *req, int num_bds)
+{
+	dma_addr_t buf_add = req->usb_req.dma;
+	u32 maxp, tfs, dword2, dword3;
+	struct bd_transfer *bd_xfr;
+	struct bd_list *bd_list;
+	struct bdc_ep *ep;
+	struct bdc_bd *bd;
+	int ret, bdnum;
+	u32 req_len;
+
+	ep = req->ep;
+	bd_list = &ep->bd_list;
+	bd_xfr = &req->bd_xfr;
+	bd_xfr->req = req;
+	bd_xfr->start_bdi = bd_list->eqp_bdi;
+	bd = bdi_to_bd(ep, bd_list->eqp_bdi);
+	req_len = req->usb_req.length;
+	maxp = usb_endpoint_maxp(ep->desc) & 0x7ff;
+	tfs = roundup(req->usb_req.length, maxp);
+	tfs = tfs/maxp;
+	dev_vdbg(bdc->dev, "%s ep:%s num_bds:%d tfs:%d r_len:%d bd:%p\n",
+				__func__, ep->name, num_bds, tfs, req_len, bd);
+
+	for (bdnum = 0; bdnum < num_bds; bdnum++) {
+		dword2 = dword3 = 0;
+		/* First bd */
+		if (!bdnum) {
+			dword3 |= BD_SOT|BD_SBF|(tfs<<BD_TFS_SHIFT);
+			dword2 |= BD_LTF;
+			/* format of first bd for ep0 is different than other */
+			if (ep->ep_num == 1) {
+				ret = setup_first_bd_ep0(bdc, req, &dword3);
+				if (ret)
+					return ret;
+			}
+		}
+		if (!req->ep->dir)
+			dword3 |= BD_ISP;
+
+		if (req_len > BD_MAX_BUFF_SIZE) {
+			dword2 |= BD_MAX_BUFF_SIZE;
+			req_len -= BD_MAX_BUFF_SIZE;
+		} else {
+			/* this should be the last bd */
+			dword2 |= req_len;
+			dword3 |= BD_IOC;
+			dword3 |= BD_EOT;
+		}
+		/* Currently only 1 INT target is supported */
+		dword2 |= BD_INTR_TARGET(0);
+		bd = bdi_to_bd(ep, ep->bd_list.eqp_bdi);
+		if (unlikely(!bd)) {
+			dev_err(bdc->dev, "Err bd pointing to wrong addr\n");
+			return -EINVAL;
+		}
+		/* write bd */
+		bd->offset[0] = cpu_to_le32(lower_32_bits(buf_add));
+		bd->offset[1] = cpu_to_le32(upper_32_bits(buf_add));
+		bd->offset[2] = cpu_to_le32(dword2);
+		bd->offset[3] = cpu_to_le32(dword3);
+		/* advance eqp pointer */
+		ep_bdlist_eqp_adv(ep);
+		/* advance the buff pointer */
+		buf_add += BD_MAX_BUFF_SIZE;
+		dev_vdbg(bdc->dev, "buf_add:%08llx req_len:%d bd:%p eqp:%d\n",
+				(unsigned long long)buf_add, req_len, bd,
+							ep->bd_list.eqp_bdi);
+		bd = bdi_to_bd(ep, ep->bd_list.eqp_bdi);
+		bd->offset[3] = cpu_to_le32(BD_SBF);
+	}
+	/* clear the STOP BD fetch bit from the first bd of this xfr */
+	bd = bdi_to_bd(ep, bd_xfr->start_bdi);
+	bd->offset[3] &= cpu_to_le32(~BD_SBF);
+	/* the new eqp will be next hw dqp */
+	bd_xfr->num_bds  = num_bds;
+	bd_xfr->next_hwd_bdi = ep->bd_list.eqp_bdi;
+	/* everything is written correctly before notifying the HW */
+	wmb();
+
+	return 0;
+}
+
+/* Queue the xfr */
+static int bdc_queue_xfr(struct bdc *bdc, struct bdc_req *req)
+{
+	int num_bds, bd_available;
+	struct bdc_ep *ep;
+	int ret;
+
+	ep = req->ep;
+	dev_dbg(bdc->dev, "%s req:%p\n", __func__, req);
+	dev_dbg(bdc->dev, "eqp_bdi:%d hwd_bdi:%d\n",
+			ep->bd_list.eqp_bdi, ep->bd_list.hwd_bdi);
+
+	num_bds =  bd_needed_req(req);
+	bd_available = bd_available_ep(ep);
+
+	/* how many bd's are avaialble on ep */
+	if (num_bds > bd_available)
+		return -ENOMEM;
+
+	ret = setup_bd_list_xfr(bdc, req, num_bds);
+	if (ret)
+		return ret;
+	list_add_tail(&req->queue, &ep->queue);
+	bdc_dbg_bd_list(bdc, ep);
+	bdc_notify_xfr(bdc, ep->ep_num);
+
+	return 0;
+}
+
+/* callback to gadget layer when xfr completes */
+static void bdc_req_complete(struct bdc_ep *ep, struct bdc_req *req,
+						int status)
+{
+	struct bdc *bdc = ep->bdc;
+
+	if (req == NULL  || &req->queue == NULL || &req->usb_req == NULL)
+		return;
+
+	dev_dbg(bdc->dev, "%s ep:%s status:%d\n", __func__, ep->name, status);
+	list_del(&req->queue);
+	req->usb_req.status = status;
+	usb_gadget_unmap_request(&bdc->gadget, &req->usb_req, ep->dir);
+	if (req->usb_req.complete) {
+		spin_unlock(&bdc->lock);
+		usb_gadget_giveback_request(&ep->usb_ep, &req->usb_req);
+		spin_lock(&bdc->lock);
+	}
+}
+
+/* Disable the endpoint */
+int bdc_ep_disable(struct bdc_ep *ep)
+{
+	struct bdc_req *req;
+	struct bdc *bdc;
+	int ret;
+
+	ret = 0;
+	bdc = ep->bdc;
+	dev_dbg(bdc->dev, "%s() ep->ep_num=%d\n", __func__, ep->ep_num);
+	/* Stop the endpoint */
+	ret = bdc_stop_ep(bdc, ep->ep_num);
+
+	/*
+	 * Intentionally don't check the ret value of stop, it can fail in
+	 * disconnect scenarios, continue with dconfig
+	 */
+	/* de-queue any pending requests */
+	while (!list_empty(&ep->queue)) {
+		req = list_entry(ep->queue.next, struct bdc_req,
+				queue);
+		bdc_req_complete(ep, req, -ESHUTDOWN);
+	}
+	/* deconfigure the endpoint */
+	ret = bdc_dconfig_ep(bdc, ep);
+	if (ret)
+		dev_warn(bdc->dev,
+			"dconfig fail but continue with memory free");
+
+	ep->flags = 0;
+	/* ep0 memory is not freed, but reused on next connect sr */
+	if (ep->ep_num == 1)
+		return 0;
+
+	/* Free the bdl memory */
+	ep_bd_list_free(ep, ep->bd_list.num_tabs);
+	ep->desc = NULL;
+	ep->comp_desc = NULL;
+	ep->usb_ep.desc = NULL;
+	ep->ep_type = 0;
+
+	return ret;
+}
+
+/* Enable the ep */
+int bdc_ep_enable(struct bdc_ep *ep)
+{
+	struct bdc *bdc;
+	int ret = 0;
+
+	bdc = ep->bdc;
+	dev_dbg(bdc->dev, "%s NUM_TABLES:%d %d\n",
+					__func__, NUM_TABLES, NUM_TABLES_ISOCH);
+
+	ret = ep_bd_list_alloc(ep);
+	if (ret) {
+		dev_err(bdc->dev, "ep bd list allocation failed:%d\n", ret);
+		return -ENOMEM;
+	}
+	bdc_dbg_bd_list(bdc, ep);
+	/* only for ep0: config ep is called for ep0 from connect event */
+	ep->flags |= BDC_EP_ENABLED;
+	if (ep->ep_num == 1)
+		return ret;
+
+	/* Issue a configure endpoint command */
+	ret = bdc_config_ep(bdc, ep);
+	if (ret)
+		return ret;
+
+	ep->usb_ep.maxpacket = usb_endpoint_maxp(ep->desc);
+	ep->usb_ep.desc = ep->desc;
+	ep->usb_ep.comp_desc = ep->comp_desc;
+	ep->ep_type = usb_endpoint_type(ep->desc);
+	ep->flags |= BDC_EP_ENABLED;
+
+	return 0;
+}
+
+/* EP0 related code */
+
+/* Queue a status stage BD */
+static int ep0_queue_status_stage(struct bdc *bdc)
+{
+	struct bdc_req *status_req;
+	struct bdc_ep *ep;
+
+	status_req = &bdc->status_req;
+	ep = bdc->bdc_ep_array[1];
+	status_req->ep = ep;
+	status_req->usb_req.length = 0;
+	status_req->usb_req.status = -EINPROGRESS;
+	status_req->usb_req.actual = 0;
+	status_req->usb_req.complete = NULL;
+	bdc_queue_xfr(bdc, status_req);
+
+	return 0;
+}
+
+/* Queue xfr on ep0 */
+static int ep0_queue(struct bdc_ep *ep, struct bdc_req *req)
+{
+	struct bdc *bdc;
+	int ret;
+
+	bdc = ep->bdc;
+	dev_dbg(bdc->dev, "%s()\n", __func__);
+	req->usb_req.actual = 0;
+	req->usb_req.status = -EINPROGRESS;
+	req->epnum = ep->ep_num;
+
+	if (bdc->delayed_status) {
+		bdc->delayed_status = false;
+		/* if status stage was delayed? */
+		if (bdc->ep0_state == WAIT_FOR_STATUS_START) {
+			/* Queue a status stage BD */
+			ep0_queue_status_stage(bdc);
+			bdc->ep0_state = WAIT_FOR_STATUS_XMIT;
+			return 0;
+		}
+	} else {
+		/*
+		 * if delayed status is false and 0 length transfer is requested
+		 * i.e. for status stage of some setup request, then just
+		 * return from here the status stage is queued independently
+		 */
+		if (req->usb_req.length == 0)
+			return 0;
+
+	}
+	ret = usb_gadget_map_request(&bdc->gadget, &req->usb_req, ep->dir);
+	if (ret) {
+		dev_err(bdc->dev, "dma mapping failed %s\n", ep->name);
+		return ret;
+	}
+
+	return bdc_queue_xfr(bdc, req);
+}
+
+/* Queue data stage */
+static int ep0_queue_data_stage(struct bdc *bdc)
+{
+	struct usb_request *ep0_usb_req;
+	struct bdc_ep *ep;
+
+	dev_dbg(bdc->dev, "%s\n", __func__);
+	ep0_usb_req = &bdc->ep0_req.usb_req;
+	ep = bdc->bdc_ep_array[1];
+	bdc->ep0_req.ep = ep;
+	bdc->ep0_req.usb_req.complete = NULL;
+
+	return ep0_queue(ep, &bdc->ep0_req);
+}
+
+/* Queue req on ep */
+static int ep_queue(struct bdc_ep *ep, struct bdc_req *req)
+{
+	struct bdc *bdc;
+	int ret = 0;
+
+	bdc = ep->bdc;
+	if (!req || !ep || !ep->usb_ep.desc)
+		return -EINVAL;
+
+	req->usb_req.actual = 0;
+	req->usb_req.status = -EINPROGRESS;
+	req->epnum = ep->ep_num;
+
+	ret = usb_gadget_map_request(&bdc->gadget, &req->usb_req, ep->dir);
+	if (ret) {
+		dev_err(bdc->dev, "dma mapping failed\n");
+		return ret;
+	}
+
+	return bdc_queue_xfr(bdc, req);
+}
+
+/* Dequeue a request from ep */
+static int ep_dequeue(struct bdc_ep *ep, struct bdc_req *req)
+{
+	int start_bdi, end_bdi, tbi, eqp_bdi, curr_hw_dqpi;
+	bool start_pending, end_pending;
+	bool first_remove = false;
+	struct bdc_req *first_req;
+	struct bdc_bd *bd_start;
+	struct bd_table *table;
+	dma_addr_t next_bd_dma;
+	u64   deq_ptr_64 = 0;
+	struct bdc  *bdc;
+	u32    tmp_32;
+	int ret;
+
+	bdc = ep->bdc;
+	start_pending = end_pending = false;
+	eqp_bdi = ep->bd_list.eqp_bdi - 1;
+
+	if (eqp_bdi < 0)
+		eqp_bdi = ep->bd_list.max_bdi;
+
+	start_bdi = req->bd_xfr.start_bdi;
+	end_bdi = find_end_bdi(ep, req->bd_xfr.next_hwd_bdi);
+
+	dev_dbg(bdc->dev, "%s ep:%s start:%d end:%d\n",
+					__func__, ep->name, start_bdi, end_bdi);
+	dev_dbg(bdc->dev, "ep_dequeue ep=%p ep->desc=%p\n",
+						ep, (void *)ep->usb_ep.desc);
+	/* Stop the ep to see where the HW is ? */
+	ret = bdc_stop_ep(bdc, ep->ep_num);
+	/* if there is an issue with stopping ep, then no need to go further */
+	if (ret)
+		return 0;
+
+	/*
+	 * After endpoint is stopped, there can be 3 cases, the request
+	 * is processed, pending or in the middle of processing
+	 */
+
+	/* The current hw dequeue pointer */
+	tmp_32 = bdc_readl(bdc->regs, BDC_EPSTS0(0));
+	deq_ptr_64 = tmp_32;
+	tmp_32 = bdc_readl(bdc->regs, BDC_EPSTS0(1));
+	deq_ptr_64 |= ((u64)tmp_32 << 32);
+
+	/* we have the dma addr of next bd that will be fetched by hardware */
+	curr_hw_dqpi = bd_add_to_bdi(ep, deq_ptr_64);
+	if (curr_hw_dqpi < 0)
+		return curr_hw_dqpi;
+
+	/*
+	 * curr_hw_dqpi points to actual dqp of HW and HW owns bd's from
+	 * curr_hw_dqbdi..eqp_bdi.
+	 */
+
+	/* Check if start_bdi and end_bdi are in range of HW owned BD's */
+	if (curr_hw_dqpi > eqp_bdi) {
+		/* there is a wrap from last to 0 */
+		if (start_bdi >= curr_hw_dqpi || start_bdi <= eqp_bdi) {
+			start_pending = true;
+			end_pending = true;
+		} else if (end_bdi >= curr_hw_dqpi || end_bdi <= eqp_bdi) {
+				end_pending = true;
+		}
+	} else {
+		if (start_bdi >= curr_hw_dqpi) {
+			start_pending = true;
+			end_pending = true;
+		} else if (end_bdi >= curr_hw_dqpi) {
+			end_pending = true;
+		}
+	}
+	dev_dbg(bdc->dev,
+		"start_pending:%d end_pending:%d speed:%d\n",
+		start_pending, end_pending, bdc->gadget.speed);
+
+	/* If both start till end are processes, we cannot deq req */
+	if (!start_pending && !end_pending)
+		return -EINVAL;
+
+	/*
+	 * if ep_dequeue is called after disconnect then just return
+	 * success from here
+	 */
+	if (bdc->gadget.speed == USB_SPEED_UNKNOWN)
+		return 0;
+	tbi = bdi_to_tbi(ep, req->bd_xfr.next_hwd_bdi);
+	table = ep->bd_list.bd_table_array[tbi];
+	next_bd_dma =  table->dma +
+			sizeof(struct bdc_bd)*(req->bd_xfr.next_hwd_bdi -
+					tbi * ep->bd_list.num_bds_table);
+
+	first_req = list_first_entry(&ep->queue, struct bdc_req,
+			queue);
+
+	if (req == first_req)
+		first_remove = true;
+
+	/*
+	 * Due to HW limitation we need to bypadd chain bd's and issue ep_bla,
+	 * incase if start is pending this is the first request in the list
+	 * then issue ep_bla instead of marking as chain bd
+	 */
+	if (start_pending && !first_remove) {
+		/*
+		 * Mark the start bd as Chain bd, and point the chain
+		 * bd to next_bd_dma
+		 */
+		bd_start = bdi_to_bd(ep, start_bdi);
+		bd_start->offset[0] = cpu_to_le32(lower_32_bits(next_bd_dma));
+		bd_start->offset[1] = cpu_to_le32(upper_32_bits(next_bd_dma));
+		bd_start->offset[2] = 0x0;
+		bd_start->offset[3] = cpu_to_le32(MARK_CHAIN_BD);
+		bdc_dbg_bd_list(bdc, ep);
+	} else if (end_pending) {
+		/*
+		 * The transfer is stopped in the middle, move the
+		 * HW deq pointer to next_bd_dma
+		 */
+		ret = bdc_ep_bla(bdc, ep, next_bd_dma);
+		if (ret) {
+			dev_err(bdc->dev, "error in ep_bla:%d\n", ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+/* Halt/Clear the ep based on value */
+static int ep_set_halt(struct bdc_ep *ep, u32 value)
+{
+	struct bdc *bdc;
+	int ret;
+
+	bdc = ep->bdc;
+	dev_dbg(bdc->dev, "%s ep:%s value=%d\n", __func__, ep->name, value);
+
+	if (value) {
+		dev_dbg(bdc->dev, "Halt\n");
+		if (ep->ep_num == 1)
+			bdc->ep0_state = WAIT_FOR_SETUP;
+
+		ret = bdc_ep_set_stall(bdc, ep->ep_num);
+		if (ret)
+			dev_err(bdc->dev, "failed to %s STALL on %s\n",
+				value ? "set" : "clear", ep->name);
+		else
+			ep->flags |= BDC_EP_STALL;
+	} else {
+		/* Clear */
+		dev_dbg(bdc->dev, "Before Clear\n");
+		ret = bdc_ep_clear_stall(bdc, ep->ep_num);
+		if (ret)
+			dev_err(bdc->dev, "failed to %s STALL on %s\n",
+				value ? "set" : "clear", ep->name);
+		else
+			ep->flags &= ~BDC_EP_STALL;
+		dev_dbg(bdc->dev, "After  Clear\n");
+	}
+
+	return ret;
+}
+
+/* Free all the ep */
+void bdc_free_ep(struct bdc *bdc)
+{
+	struct bdc_ep *ep;
+	u8	epnum;
+
+	dev_dbg(bdc->dev, "%s\n", __func__);
+	for (epnum = 1; epnum < bdc->num_eps; epnum++) {
+		ep = bdc->bdc_ep_array[epnum];
+		if (!ep)
+			continue;
+
+		if (ep->flags & BDC_EP_ENABLED)
+			ep_bd_list_free(ep, ep->bd_list.num_tabs);
+
+		/* ep0 is not in this gadget list */
+		if (epnum != 1)
+			list_del(&ep->usb_ep.ep_list);
+
+		kfree(ep);
+	}
+}
+
+/* USB2 spec, section 7.1.20 */
+static int bdc_set_test_mode(struct bdc *bdc)
+{
+	u32 usb2_pm;
+
+	usb2_pm = bdc_readl(bdc->regs, BDC_USPPM2);
+	usb2_pm &= ~BDC_PTC_MASK;
+	dev_dbg(bdc->dev, "%s\n", __func__);
+	switch (bdc->test_mode) {
+	case TEST_J:
+	case TEST_K:
+	case TEST_SE0_NAK:
+	case TEST_PACKET:
+	case TEST_FORCE_EN:
+		usb2_pm |= bdc->test_mode << 28;
+		break;
+	default:
+		return -EINVAL;
+	}
+	dev_dbg(bdc->dev, "usb2_pm=%08x", usb2_pm);
+	bdc_writel(bdc->regs, BDC_USPPM2, usb2_pm);
+
+	return 0;
+}
+
+/*
+ * Helper function to handle Transfer status report with status as either
+ * success or short
+ */
+static void handle_xsr_succ_status(struct bdc *bdc, struct bdc_ep *ep,
+							struct bdc_sr *sreport)
+{
+	int short_bdi, start_bdi, end_bdi, max_len_bds, chain_bds;
+	struct bd_list *bd_list = &ep->bd_list;
+	int actual_length, length_short;
+	struct bd_transfer *bd_xfr;
+	struct bdc_bd *short_bd;
+	struct bdc_req *req;
+	u64   deq_ptr_64 = 0;
+	int status = 0;
+	int sr_status;
+	u32    tmp_32;
+
+	dev_dbg(bdc->dev, "%s  ep:%p\n", __func__, ep);
+	bdc_dbg_srr(bdc, 0);
+	/* do not process thie sr if ignore flag is set */
+	if (ep->ignore_next_sr) {
+		ep->ignore_next_sr = false;
+		return;
+	}
+
+	if (unlikely(list_empty(&ep->queue))) {
+		dev_warn(bdc->dev, "xfr srr with no BD's queued\n");
+		return;
+	}
+	req = list_entry(ep->queue.next, struct bdc_req,
+			queue);
+
+	bd_xfr = &req->bd_xfr;
+	sr_status = XSF_STS(le32_to_cpu(sreport->offset[3]));
+
+	/*
+	 * sr_status is short and this transfer has more than 1 bd then it needs
+	 * special handling,  this is only applicable for bulk and ctrl
+	 */
+	if (sr_status == XSF_SHORT &&  bd_xfr->num_bds > 1) {
+		/*
+		 * This is multi bd xfr, lets see which bd
+		 * caused short transfer and how many bytes have been
+		 * transferred so far.
+		 */
+		tmp_32 = le32_to_cpu(sreport->offset[0]);
+		deq_ptr_64 = tmp_32;
+		tmp_32 = le32_to_cpu(sreport->offset[1]);
+		deq_ptr_64 |= ((u64)tmp_32 << 32);
+		short_bdi = bd_add_to_bdi(ep, deq_ptr_64);
+		if (unlikely(short_bdi < 0))
+			dev_warn(bdc->dev, "bd doesn't exist?\n");
+
+		start_bdi =  bd_xfr->start_bdi;
+		/*
+		 * We know the start_bdi and short_bdi, how many xfr
+		 * bds in between
+		 */
+		if (start_bdi <= short_bdi) {
+			max_len_bds = short_bdi - start_bdi;
+			if (max_len_bds <= bd_list->num_bds_table) {
+				if (!(bdi_to_tbi(ep, start_bdi) ==
+						bdi_to_tbi(ep, short_bdi)))
+					max_len_bds--;
+			} else {
+				chain_bds = max_len_bds/bd_list->num_bds_table;
+				max_len_bds -= chain_bds;
+			}
+		} else {
+			/* there is a wrap in the ring within a xfr */
+			chain_bds = (bd_list->max_bdi - start_bdi)/
+							bd_list->num_bds_table;
+			chain_bds += short_bdi/bd_list->num_bds_table;
+			max_len_bds = bd_list->max_bdi - start_bdi;
+			max_len_bds += short_bdi;
+			max_len_bds -= chain_bds;
+		}
+		/* max_len_bds is the number of full length bds */
+		end_bdi = find_end_bdi(ep, bd_xfr->next_hwd_bdi);
+		if (!(end_bdi == short_bdi))
+			ep->ignore_next_sr = true;
+
+		actual_length = max_len_bds * BD_MAX_BUFF_SIZE;
+		short_bd = bdi_to_bd(ep, short_bdi);
+		/* length queued */
+		length_short = le32_to_cpu(short_bd->offset[2]) & 0x1FFFFF;
+		/* actual length trensfered */
+		length_short -= SR_BD_LEN(le32_to_cpu(sreport->offset[2]));
+		actual_length += length_short;
+		req->usb_req.actual = actual_length;
+	} else {
+		req->usb_req.actual = req->usb_req.length -
+			SR_BD_LEN(le32_to_cpu(sreport->offset[2]));
+		dev_dbg(bdc->dev,
+			"len=%d actual=%d bd_xfr->next_hwd_bdi:%d\n",
+			req->usb_req.length, req->usb_req.actual,
+			bd_xfr->next_hwd_bdi);
+	}
+
+	/* Update the dequeue pointer */
+	ep->bd_list.hwd_bdi = bd_xfr->next_hwd_bdi;
+	if (req->usb_req.actual < req->usb_req.length) {
+		dev_dbg(bdc->dev, "short xfr on %d\n", ep->ep_num);
+		if (req->usb_req.short_not_ok)
+			status = -EREMOTEIO;
+	}
+	bdc_req_complete(ep, bd_xfr->req, status);
+}
+
+/* EP0 setup related packet handlers */
+
+/*
+ * Setup packet received, just store the packet and process on next DS or SS
+ * started SR
+ */
+void bdc_xsf_ep0_setup_recv(struct bdc *bdc, struct bdc_sr *sreport)
+{
+	struct usb_ctrlrequest *setup_pkt;
+	u32 len;
+
+	dev_dbg(bdc->dev,
+		"%s ep0_state:%s\n",
+		__func__, ep0_state_string[bdc->ep0_state]);
+	/* Store received setup packet */
+	setup_pkt = &bdc->setup_pkt;
+	memcpy(setup_pkt, &sreport->offset[0], sizeof(*setup_pkt));
+	len = le16_to_cpu(setup_pkt->wLength);
+	if (!len)
+		bdc->ep0_state = WAIT_FOR_STATUS_START;
+	else
+		bdc->ep0_state = WAIT_FOR_DATA_START;
+
+
+	dev_dbg(bdc->dev,
+		"%s exit ep0_state:%s\n",
+		__func__, ep0_state_string[bdc->ep0_state]);
+}
+
+/* Stall ep0 */
+static void ep0_stall(struct bdc *bdc)
+{
+	struct bdc_ep	*ep = bdc->bdc_ep_array[1];
+	struct bdc_req *req;
+
+	dev_dbg(bdc->dev, "%s\n", __func__);
+	bdc->delayed_status = false;
+	ep_set_halt(ep, 1);
+
+	/* de-queue any pendig requests */
+	while (!list_empty(&ep->queue)) {
+		req = list_entry(ep->queue.next, struct bdc_req,
+				queue);
+		bdc_req_complete(ep, req, -ESHUTDOWN);
+	}
+}
+
+/* SET_ADD handlers */
+static int ep0_set_address(struct bdc *bdc, struct usb_ctrlrequest *ctrl)
+{
+	enum usb_device_state state = bdc->gadget.state;
+	int ret = 0;
+	u32 addr;
+
+	addr = le16_to_cpu(ctrl->wValue);
+	dev_dbg(bdc->dev,
+		"%s addr:%d dev state:%d\n",
+		__func__, addr, state);
+
+	if (addr > 127)
+		return -EINVAL;
+
+	switch (state) {
+	case USB_STATE_DEFAULT:
+	case USB_STATE_ADDRESS:
+		/* Issue Address device command */
+		ret = bdc_address_device(bdc, addr);
+		if (ret)
+			return ret;
+
+		if (addr)
+			usb_gadget_set_state(&bdc->gadget, USB_STATE_ADDRESS);
+		else
+			usb_gadget_set_state(&bdc->gadget, USB_STATE_DEFAULT);
+
+		bdc->dev_addr = addr;
+		break;
+	default:
+		dev_warn(bdc->dev,
+			"SET Address in wrong device state %d\n",
+			state);
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+/* Handler for SET/CLEAR FEATURE requests for device */
+static int ep0_handle_feature_dev(struct bdc *bdc, u16 wValue,
+							u16 wIndex, bool set)
+{
+	enum usb_device_state state = bdc->gadget.state;
+	u32	usppms = 0;
+
+	dev_dbg(bdc->dev, "%s set:%d dev state:%d\n",
+					__func__, set, state);
+	switch (wValue) {
+	case USB_DEVICE_REMOTE_WAKEUP:
+		dev_dbg(bdc->dev, "USB_DEVICE_REMOTE_WAKEUP\n");
+		if (set)
+			bdc->devstatus |= REMOTE_WAKE_ENABLE;
+		else
+			bdc->devstatus &= ~REMOTE_WAKE_ENABLE;
+		break;
+
+	case USB_DEVICE_TEST_MODE:
+		dev_dbg(bdc->dev, "USB_DEVICE_TEST_MODE\n");
+		if ((wIndex & 0xFF) ||
+				(bdc->gadget.speed != USB_SPEED_HIGH) || !set)
+			return -EINVAL;
+
+		bdc->test_mode = wIndex >> 8;
+		break;
+
+	case USB_DEVICE_U1_ENABLE:
+		dev_dbg(bdc->dev, "USB_DEVICE_U1_ENABLE\n");
+
+		if (bdc->gadget.speed != USB_SPEED_SUPER ||
+						state != USB_STATE_CONFIGURED)
+			return -EINVAL;
+
+		usppms =  bdc_readl(bdc->regs, BDC_USPPMS);
+		if (set) {
+			/* clear previous u1t */
+			usppms &= ~BDC_U1T(BDC_U1T_MASK);
+			usppms |= BDC_U1T(U1_TIMEOUT);
+			usppms |= BDC_U1E | BDC_PORT_W1S;
+			bdc->devstatus |= (1 << USB_DEV_STAT_U1_ENABLED);
+		} else {
+			usppms &= ~BDC_U1E;
+			usppms |= BDC_PORT_W1S;
+			bdc->devstatus &= ~(1 << USB_DEV_STAT_U1_ENABLED);
+		}
+		bdc_writel(bdc->regs, BDC_USPPMS, usppms);
+		break;
+
+	case USB_DEVICE_U2_ENABLE:
+		dev_dbg(bdc->dev, "USB_DEVICE_U2_ENABLE\n");
+
+		if (bdc->gadget.speed != USB_SPEED_SUPER ||
+						state != USB_STATE_CONFIGURED)
+			return -EINVAL;
+
+		usppms = bdc_readl(bdc->regs, BDC_USPPMS);
+		if (set) {
+			usppms |= BDC_U2E;
+			usppms |= BDC_U2A;
+			bdc->devstatus |= (1 << USB_DEV_STAT_U2_ENABLED);
+		} else {
+			usppms &= ~BDC_U2E;
+			usppms &= ~BDC_U2A;
+			bdc->devstatus &= ~(1 << USB_DEV_STAT_U2_ENABLED);
+		}
+		bdc_writel(bdc->regs, BDC_USPPMS, usppms);
+		break;
+
+	case USB_DEVICE_LTM_ENABLE:
+		dev_dbg(bdc->dev, "USB_DEVICE_LTM_ENABLE?\n");
+		if (bdc->gadget.speed != USB_SPEED_SUPER ||
+						state != USB_STATE_CONFIGURED)
+			return -EINVAL;
+		break;
+	default:
+		dev_err(bdc->dev, "Unknown wValue:%d\n", wValue);
+		return -EOPNOTSUPP;
+	} /* USB_RECIP_DEVICE end */
+
+	return 0;
+}
+
+/* SET/CLEAR FEATURE handler */
+static int ep0_handle_feature(struct bdc *bdc,
+			      struct usb_ctrlrequest *setup_pkt, bool set)
+{
+	enum usb_device_state state = bdc->gadget.state;
+	struct bdc_ep *ep;
+	u16 wValue;
+	u16 wIndex;
+	int epnum;
+
+	wValue = le16_to_cpu(setup_pkt->wValue);
+	wIndex = le16_to_cpu(setup_pkt->wIndex);
+
+	dev_dbg(bdc->dev,
+		"%s wValue=%d wIndex=%d	devstate=%08x speed=%d set=%d",
+		__func__, wValue, wIndex, state,
+		bdc->gadget.speed, set);
+
+	switch (setup_pkt->bRequestType & USB_RECIP_MASK) {
+	case USB_RECIP_DEVICE:
+		return ep0_handle_feature_dev(bdc, wValue, wIndex, set);
+	case USB_RECIP_INTERFACE:
+		dev_dbg(bdc->dev, "USB_RECIP_INTERFACE\n");
+		/* USB3 spec, sec 9.4.9 */
+		if (wValue != USB_INTRF_FUNC_SUSPEND)
+			return -EINVAL;
+		/* USB3 spec, Table 9-8 */
+		if (set) {
+			if (wIndex & USB_INTRF_FUNC_SUSPEND_RW) {
+				dev_dbg(bdc->dev, "SET REMOTE_WAKEUP\n");
+				bdc->devstatus |= REMOTE_WAKE_ENABLE;
+			} else {
+				dev_dbg(bdc->dev, "CLEAR REMOTE_WAKEUP\n");
+				bdc->devstatus &= ~REMOTE_WAKE_ENABLE;
+			}
+		}
+		break;
+
+	case USB_RECIP_ENDPOINT:
+		dev_dbg(bdc->dev, "USB_RECIP_ENDPOINT\n");
+		if (wValue != USB_ENDPOINT_HALT)
+			return -EINVAL;
+
+		epnum = wIndex & USB_ENDPOINT_NUMBER_MASK;
+		if (epnum) {
+			if ((wIndex & USB_ENDPOINT_DIR_MASK) == USB_DIR_IN)
+				epnum = epnum * 2 + 1;
+			else
+				epnum *= 2;
+		} else {
+			epnum = 1; /*EP0*/
+		}
+		/*
+		 * If CLEAR_FEATURE on ep0 then don't do anything as the stall
+		 * condition on ep0 has already been cleared when SETUP packet
+		 * was received.
+		 */
+		if (epnum == 1 && !set) {
+			dev_dbg(bdc->dev, "ep0 stall already cleared\n");
+			return 0;
+		}
+		dev_dbg(bdc->dev, "epnum=%d\n", epnum);
+		ep = bdc->bdc_ep_array[epnum];
+		if (!ep)
+			return -EINVAL;
+
+		return ep_set_halt(ep, set);
+	default:
+		dev_err(bdc->dev, "Unknown recipient\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* GET_STATUS request handler */
+static int ep0_handle_status(struct bdc *bdc,
+			     struct usb_ctrlrequest *setup_pkt)
+{
+	enum usb_device_state state = bdc->gadget.state;
+	struct bdc_ep *ep;
+	u16 usb_status = 0;
+	u32 epnum;
+	u16 wIndex;
+
+	/* USB2.0 spec sec 9.4.5 */
+	if (state == USB_STATE_DEFAULT)
+		return -EINVAL;
+	wIndex = le16_to_cpu(setup_pkt->wIndex);
+	dev_dbg(bdc->dev, "%s\n", __func__);
+	usb_status = bdc->devstatus;
+	switch (setup_pkt->bRequestType & USB_RECIP_MASK) {
+	case USB_RECIP_DEVICE:
+		dev_dbg(bdc->dev,
+			"USB_RECIP_DEVICE devstatus:%08x\n",
+			bdc->devstatus);
+		/* USB3 spec, sec 9.4.5 */
+		if (bdc->gadget.speed == USB_SPEED_SUPER)
+			usb_status &= ~REMOTE_WAKE_ENABLE;
+		break;
+
+	case USB_RECIP_INTERFACE:
+		dev_dbg(bdc->dev, "USB_RECIP_INTERFACE\n");
+		if (bdc->gadget.speed == USB_SPEED_SUPER) {
+			/*
+			 * This should come from func for Func remote wkup
+			 * usb_status |=1;
+			 */
+			if (bdc->devstatus & REMOTE_WAKE_ENABLE)
+				usb_status |= REMOTE_WAKE_ENABLE;
+		} else {
+			usb_status = 0;
+		}
+
+		break;
+
+	case USB_RECIP_ENDPOINT:
+		dev_dbg(bdc->dev, "USB_RECIP_ENDPOINT\n");
+		epnum = wIndex & USB_ENDPOINT_NUMBER_MASK;
+		if (epnum) {
+			if ((wIndex & USB_ENDPOINT_DIR_MASK) == USB_DIR_IN)
+				epnum = epnum*2 + 1;
+			else
+				epnum *= 2;
+		} else {
+			epnum = 1; /* EP0 */
+		}
+
+		ep = bdc->bdc_ep_array[epnum];
+		if (!ep) {
+			dev_err(bdc->dev, "ISSUE, GET_STATUS for invalid EP ?");
+			return -EINVAL;
+		}
+		if (ep->flags & BDC_EP_STALL)
+			usb_status |= 1 << USB_ENDPOINT_HALT;
+
+		break;
+	default:
+		dev_err(bdc->dev, "Unknown recipient for get_status\n");
+		return -EINVAL;
+	}
+	/* prepare a data stage for GET_STATUS */
+	dev_dbg(bdc->dev, "usb_status=%08x\n", usb_status);
+	*(__le16 *)bdc->ep0_response_buff = cpu_to_le16(usb_status);
+	bdc->ep0_req.usb_req.length = 2;
+	bdc->ep0_req.usb_req.buf = &bdc->ep0_response_buff;
+	ep0_queue_data_stage(bdc);
+
+	return 0;
+}
+
+static void ep0_set_sel_cmpl(struct usb_ep *_ep, struct usb_request *_req)
+{
+	/* ep0_set_sel_cmpl */
+}
+
+/* Queue data stage to handle 6 byte SET_SEL request */
+static int ep0_set_sel(struct bdc *bdc,
+			     struct usb_ctrlrequest *setup_pkt)
+{
+	struct bdc_ep	*ep;
+	u16	wLength;
+	u16	wValue;
+
+	dev_dbg(bdc->dev, "%s\n", __func__);
+	wValue = le16_to_cpu(setup_pkt->wValue);
+	wLength = le16_to_cpu(setup_pkt->wLength);
+	if (unlikely(wLength != 6)) {
+		dev_err(bdc->dev, "%s Wrong wLength:%d\n", __func__, wLength);
+		return -EINVAL;
+	}
+	ep = bdc->bdc_ep_array[1];
+	bdc->ep0_req.ep = ep;
+	bdc->ep0_req.usb_req.length = 6;
+	bdc->ep0_req.usb_req.buf = bdc->ep0_response_buff;
+	bdc->ep0_req.usb_req.complete = ep0_set_sel_cmpl;
+	ep0_queue_data_stage(bdc);
+
+	return 0;
+}
+
+/*
+ * Queue a 0 byte bd only if wLength is more than the length and and length is
+ * a multiple of MaxPacket then queue 0 byte BD
+ */
+static int ep0_queue_zlp(struct bdc *bdc)
+{
+	int ret;
+
+	dev_dbg(bdc->dev, "%s\n", __func__);
+	bdc->ep0_req.ep = bdc->bdc_ep_array[1];
+	bdc->ep0_req.usb_req.length = 0;
+	bdc->ep0_req.usb_req.complete = NULL;
+	bdc->ep0_state = WAIT_FOR_DATA_START;
+	ret = bdc_queue_xfr(bdc, &bdc->ep0_req);
+	if (ret) {
+		dev_err(bdc->dev, "err queueing zlp :%d\n", ret);
+		return ret;
+	}
+	bdc->ep0_state = WAIT_FOR_DATA_XMIT;
+
+	return 0;
+}
+
+/* Control request handler */
+static int handle_control_request(struct bdc *bdc)
+{
+	enum usb_device_state state = bdc->gadget.state;
+	struct usb_ctrlrequest *setup_pkt;
+	int delegate_setup = 0;
+	int ret = 0;
+	int config = 0;
+
+	setup_pkt = &bdc->setup_pkt;
+	dev_dbg(bdc->dev, "%s\n", __func__);
+	if ((setup_pkt->bRequestType & USB_TYPE_MASK) == USB_TYPE_STANDARD) {
+		switch (setup_pkt->bRequest) {
+		case USB_REQ_SET_ADDRESS:
+			dev_dbg(bdc->dev, "USB_REQ_SET_ADDRESS\n");
+			ret = ep0_set_address(bdc, setup_pkt);
+			bdc->devstatus &= DEVSTATUS_CLEAR;
+			break;
+
+		case USB_REQ_SET_CONFIGURATION:
+			dev_dbg(bdc->dev, "USB_REQ_SET_CONFIGURATION\n");
+			if (state == USB_STATE_ADDRESS) {
+				usb_gadget_set_state(&bdc->gadget,
+							USB_STATE_CONFIGURED);
+			} else if (state == USB_STATE_CONFIGURED) {
+				/*
+				 * USB2 spec sec 9.4.7, if wValue is 0 then dev
+				 * is moved to addressed state
+				 */
+				config = le16_to_cpu(setup_pkt->wValue);
+				if (!config)
+					usb_gadget_set_state(
+							&bdc->gadget,
+							USB_STATE_ADDRESS);
+			}
+			delegate_setup = 1;
+			break;
+
+		case USB_REQ_SET_FEATURE:
+			dev_dbg(bdc->dev, "USB_REQ_SET_FEATURE\n");
+			ret = ep0_handle_feature(bdc, setup_pkt, 1);
+			break;
+
+		case USB_REQ_CLEAR_FEATURE:
+			dev_dbg(bdc->dev, "USB_REQ_CLEAR_FEATURE\n");
+			ret = ep0_handle_feature(bdc, setup_pkt, 0);
+			break;
+
+		case USB_REQ_GET_STATUS:
+			dev_dbg(bdc->dev, "USB_REQ_GET_STATUS\n");
+			ret = ep0_handle_status(bdc, setup_pkt);
+			break;
+
+		case USB_REQ_SET_SEL:
+			dev_dbg(bdc->dev, "USB_REQ_SET_SEL\n");
+			ret = ep0_set_sel(bdc, setup_pkt);
+			break;
+
+		case USB_REQ_SET_ISOCH_DELAY:
+			dev_warn(bdc->dev,
+			"USB_REQ_SET_ISOCH_DELAY not handled\n");
+			ret = 0;
+			break;
+		default:
+			delegate_setup = 1;
+		}
+	} else {
+		delegate_setup = 1;
+	}
+
+	if (delegate_setup) {
+		spin_unlock(&bdc->lock);
+		ret = bdc->gadget_driver->setup(&bdc->gadget, setup_pkt);
+		spin_lock(&bdc->lock);
+	}
+
+	return ret;
+}
+
+/* EP0: Data stage started */
+void bdc_xsf_ep0_data_start(struct bdc *bdc, struct bdc_sr *sreport)
+{
+	struct bdc_ep *ep;
+	int ret = 0;
+
+	dev_dbg(bdc->dev, "%s\n", __func__);
+	ep = bdc->bdc_ep_array[1];
+	/* If ep0 was stalled, the clear it first */
+	if (ep->flags & BDC_EP_STALL) {
+		ret = ep_set_halt(ep, 0);
+		if (ret)
+			goto err;
+	}
+	if (bdc->ep0_state != WAIT_FOR_DATA_START)
+		dev_warn(bdc->dev,
+			"Data stage not expected ep0_state:%s\n",
+			ep0_state_string[bdc->ep0_state]);
+
+	ret = handle_control_request(bdc);
+	if (ret == USB_GADGET_DELAYED_STATUS) {
+		/*
+		 * The ep0 state will remain WAIT_FOR_DATA_START till
+		 * we received ep_queue on ep0
+		 */
+		bdc->delayed_status = true;
+		return;
+	}
+	if (!ret) {
+		bdc->ep0_state = WAIT_FOR_DATA_XMIT;
+		dev_dbg(bdc->dev,
+			"ep0_state:%s", ep0_state_string[bdc->ep0_state]);
+		return;
+	}
+err:
+	ep0_stall(bdc);
+}
+
+/* EP0: status stage started */
+void bdc_xsf_ep0_status_start(struct bdc *bdc, struct bdc_sr *sreport)
+{
+	struct usb_ctrlrequest *setup_pkt;
+	struct bdc_ep *ep;
+	int ret = 0;
+
+	dev_dbg(bdc->dev,
+		"%s ep0_state:%s",
+		__func__, ep0_state_string[bdc->ep0_state]);
+	ep = bdc->bdc_ep_array[1];
+
+	/* check if ZLP was queued? */
+	if (bdc->zlp_needed)
+		bdc->zlp_needed = false;
+
+	if (ep->flags & BDC_EP_STALL) {
+		ret = ep_set_halt(ep, 0);
+		if (ret)
+			goto err;
+	}
+
+	if ((bdc->ep0_state != WAIT_FOR_STATUS_START) &&
+				(bdc->ep0_state != WAIT_FOR_DATA_XMIT))
+		dev_err(bdc->dev,
+			"Status stage recv but ep0_state:%s\n",
+			ep0_state_string[bdc->ep0_state]);
+
+	/* check if data stage is in progress ? */
+	if (bdc->ep0_state == WAIT_FOR_DATA_XMIT) {
+		bdc->ep0_state = STATUS_PENDING;
+		/* Status stage will be queued upon Data stage transmit event */
+		dev_dbg(bdc->dev,
+			"status started but data  not transmitted yet\n");
+		return;
+	}
+	setup_pkt = &bdc->setup_pkt;
+
+	/*
+	 * 2 stage setup then only process the setup, for 3 stage setup the date
+	 * stage is already handled
+	 */
+	if (!le16_to_cpu(setup_pkt->wLength)) {
+		ret = handle_control_request(bdc);
+		if (ret == USB_GADGET_DELAYED_STATUS) {
+			bdc->delayed_status = true;
+			/* ep0_state will remain WAIT_FOR_STATUS_START */
+			return;
+		}
+	}
+	if (!ret) {
+		/* Queue a status stage BD */
+		ep0_queue_status_stage(bdc);
+		bdc->ep0_state = WAIT_FOR_STATUS_XMIT;
+		dev_dbg(bdc->dev,
+			"ep0_state:%s", ep0_state_string[bdc->ep0_state]);
+		return;
+	}
+err:
+	ep0_stall(bdc);
+}
+
+/* Helper function to update ep0 upon SR with xsf_succ or xsf_short */
+static void ep0_xsf_complete(struct bdc *bdc, struct bdc_sr *sreport)
+{
+	dev_dbg(bdc->dev, "%s\n", __func__);
+	switch (bdc->ep0_state) {
+	case WAIT_FOR_DATA_XMIT:
+		bdc->ep0_state = WAIT_FOR_STATUS_START;
+		break;
+	case WAIT_FOR_STATUS_XMIT:
+		bdc->ep0_state = WAIT_FOR_SETUP;
+		if (bdc->test_mode) {
+			int ret;
+
+			dev_dbg(bdc->dev, "test_mode:%d\n", bdc->test_mode);
+			ret = bdc_set_test_mode(bdc);
+			if (ret < 0) {
+				dev_err(bdc->dev, "Err in setting Test mode\n");
+				return;
+			}
+			bdc->test_mode = 0;
+		}
+		break;
+	case STATUS_PENDING:
+		bdc_xsf_ep0_status_start(bdc, sreport);
+		break;
+
+	default:
+		dev_err(bdc->dev,
+			"Unknown ep0_state:%s\n",
+			ep0_state_string[bdc->ep0_state]);
+
+	}
+}
+
+/* xfr completion status report handler */
+void bdc_sr_xsf(struct bdc *bdc, struct bdc_sr *sreport)
+{
+	struct bdc_ep *ep;
+	u32 sr_status;
+	u8 ep_num;
+
+	ep_num = (le32_to_cpu(sreport->offset[3])>>4) & 0x1f;
+	ep = bdc->bdc_ep_array[ep_num];
+	if (!ep || !(ep->flags & BDC_EP_ENABLED)) {
+		dev_err(bdc->dev, "xsf for ep not enabled\n");
+		return;
+	}
+	/*
+	 * check if this transfer is after link went from U3->U0 due
+	 * to remote wakeup
+	 */
+	if (bdc->devstatus & FUNC_WAKE_ISSUED) {
+		bdc->devstatus &= ~(FUNC_WAKE_ISSUED);
+		dev_dbg(bdc->dev, "%s clearing FUNC_WAKE_ISSUED flag\n",
+								__func__);
+	}
+	sr_status = XSF_STS(le32_to_cpu(sreport->offset[3]));
+	dev_dbg_ratelimited(bdc->dev, "%s sr_status=%d ep:%s\n",
+					__func__, sr_status, ep->name);
+
+	switch (sr_status) {
+	case XSF_SUCC:
+	case XSF_SHORT:
+		handle_xsr_succ_status(bdc, ep, sreport);
+		if (ep_num == 1)
+			ep0_xsf_complete(bdc, sreport);
+		break;
+
+	case XSF_SETUP_RECV:
+	case XSF_DATA_START:
+	case XSF_STATUS_START:
+		if (ep_num != 1) {
+			dev_err(bdc->dev,
+				"ep0 related packets on non ep0 endpoint");
+			return;
+		}
+		bdc->sr_xsf_ep0[sr_status - XSF_SETUP_RECV](bdc, sreport);
+		break;
+
+	case XSF_BABB:
+		if (ep_num == 1) {
+			dev_dbg(bdc->dev, "Babble on ep0 zlp_need:%d\n",
+							bdc->zlp_needed);
+			/*
+			 * If the last completed transfer had wLength >Data Len,
+			 * and Len is multiple of MaxPacket,then queue ZLP
+			 */
+			if (bdc->zlp_needed) {
+				/* queue 0 length bd */
+				ep0_queue_zlp(bdc);
+				return;
+			}
+		}
+		dev_warn(bdc->dev, "Babble on ep not handled\n");
+		break;
+	default:
+		dev_warn(bdc->dev, "sr status not handled:%x\n", sr_status);
+		break;
+	}
+}
+
+static int bdc_gadget_ep_queue(struct usb_ep *_ep,
+				struct usb_request *_req, gfp_t gfp_flags)
+{
+	struct bdc_req *req;
+	unsigned long flags;
+	struct bdc_ep *ep;
+	struct bdc *bdc;
+	int ret;
+
+	if (!_ep || !_ep->desc)
+		return -ESHUTDOWN;
+
+	if (!_req || !_req->complete || !_req->buf)
+		return -EINVAL;
+
+	ep = to_bdc_ep(_ep);
+	req = to_bdc_req(_req);
+	bdc = ep->bdc;
+	dev_dbg(bdc->dev, "%s ep:%p req:%p\n", __func__, ep, req);
+	dev_dbg(bdc->dev, "queuing request %p to %s length %d zero:%d\n",
+				_req, ep->name, _req->length, _req->zero);
+
+	if (!ep->usb_ep.desc) {
+		dev_warn(bdc->dev,
+			"trying to queue req %p to disabled %s\n",
+			_req, ep->name);
+		return -ESHUTDOWN;
+	}
+
+	if (_req->length > MAX_XFR_LEN) {
+		dev_warn(bdc->dev,
+			"req length > supported MAX:%d requested:%d\n",
+			MAX_XFR_LEN, _req->length);
+		return -EOPNOTSUPP;
+	}
+	spin_lock_irqsave(&bdc->lock, flags);
+	if (ep == bdc->bdc_ep_array[1])
+		ret = ep0_queue(ep, req);
+	else
+		ret = ep_queue(ep, req);
+
+	spin_unlock_irqrestore(&bdc->lock, flags);
+
+	return ret;
+}
+
+static int bdc_gadget_ep_dequeue(struct usb_ep *_ep,
+				  struct usb_request *_req)
+{
+	struct bdc_req *req;
+	unsigned long flags;
+	struct bdc_ep *ep;
+	struct bdc *bdc;
+	int ret;
+
+	if (!_ep || !_req)
+		return -EINVAL;
+
+	ep = to_bdc_ep(_ep);
+	req = to_bdc_req(_req);
+	bdc = ep->bdc;
+	dev_dbg(bdc->dev, "%s ep:%s req:%p\n", __func__, ep->name, req);
+	bdc_dbg_bd_list(bdc, ep);
+	spin_lock_irqsave(&bdc->lock, flags);
+	/* make sure it's still queued on this endpoint */
+	list_for_each_entry(req, &ep->queue, queue) {
+		if (&req->usb_req == _req)
+			break;
+	}
+	if (&req->usb_req != _req) {
+		spin_unlock_irqrestore(&bdc->lock, flags);
+		dev_err(bdc->dev, "usb_req !=req n");
+		return -EINVAL;
+	}
+	ret = ep_dequeue(ep, req);
+	if (ret) {
+		ret = -EOPNOTSUPP;
+		goto err;
+	}
+	bdc_req_complete(ep, req, -ECONNRESET);
+
+err:
+	bdc_dbg_bd_list(bdc, ep);
+	spin_unlock_irqrestore(&bdc->lock, flags);
+
+	return ret;
+}
+
+static int bdc_gadget_ep_set_halt(struct usb_ep *_ep, int value)
+{
+	unsigned long flags;
+	struct bdc_ep *ep;
+	struct bdc *bdc;
+	int ret;
+
+	ep = to_bdc_ep(_ep);
+	bdc = ep->bdc;
+	dev_dbg(bdc->dev, "%s ep:%s value=%d\n", __func__, ep->name, value);
+	spin_lock_irqsave(&bdc->lock, flags);
+	if (usb_endpoint_xfer_isoc(ep->usb_ep.desc))
+		ret = -EINVAL;
+	else if (!list_empty(&ep->queue))
+		ret = -EAGAIN;
+	else
+		ret = ep_set_halt(ep, value);
+
+	spin_unlock_irqrestore(&bdc->lock, flags);
+
+	return ret;
+}
+
+static struct usb_request *bdc_gadget_alloc_request(struct usb_ep *_ep,
+						     gfp_t gfp_flags)
+{
+	struct bdc_req *req;
+	struct bdc_ep *ep;
+
+	req = kzalloc(sizeof(*req), gfp_flags);
+	if (!req)
+		return NULL;
+
+	ep = to_bdc_ep(_ep);
+	req->ep = ep;
+	req->epnum = ep->ep_num;
+	req->usb_req.dma = DMA_ADDR_INVALID;
+	dev_dbg(ep->bdc->dev, "%s ep:%s req:%p\n", __func__, ep->name, req);
+
+	return &req->usb_req;
+}
+
+static void bdc_gadget_free_request(struct usb_ep *_ep,
+				     struct usb_request *_req)
+{
+	struct bdc_req *req;
+
+	req = to_bdc_req(_req);
+	kfree(req);
+}
+
+/* endpoint operations */
+
+/* configure endpoint and also allocate resources */
+static int bdc_gadget_ep_enable(struct usb_ep *_ep,
+				 const struct usb_endpoint_descriptor *desc)
+{
+	unsigned long flags;
+	struct bdc_ep *ep;
+	struct bdc *bdc;
+	int ret;
+
+	if (!_ep || !desc || desc->bDescriptorType != USB_DT_ENDPOINT) {
+		pr_debug("bdc_gadget_ep_enable invalid parameters\n");
+		return -EINVAL;
+	}
+
+	if (!desc->wMaxPacketSize) {
+		pr_debug("bdc_gadget_ep_enable missing wMaxPacketSize\n");
+		return -EINVAL;
+	}
+
+	ep = to_bdc_ep(_ep);
+	bdc = ep->bdc;
+
+	/* Sanity check, upper layer will not send enable for ep0 */
+	if (ep == bdc->bdc_ep_array[1])
+		return -EINVAL;
+
+	if (!bdc->gadget_driver
+	    || bdc->gadget.speed == USB_SPEED_UNKNOWN) {
+		return -ESHUTDOWN;
+	}
+
+	dev_dbg(bdc->dev, "%s Enabling %s\n", __func__, ep->name);
+	spin_lock_irqsave(&bdc->lock, flags);
+	ep->desc = desc;
+	ep->comp_desc = _ep->comp_desc;
+	ret = bdc_ep_enable(ep);
+	spin_unlock_irqrestore(&bdc->lock, flags);
+
+	return ret;
+}
+
+static int bdc_gadget_ep_disable(struct usb_ep *_ep)
+{
+	unsigned long flags;
+	struct bdc_ep *ep;
+	struct bdc *bdc;
+	int ret;
+
+	if (!_ep) {
+		pr_debug("bdc: invalid parameters\n");
+		return -EINVAL;
+	}
+	ep = to_bdc_ep(_ep);
+	bdc = ep->bdc;
+
+	/* Upper layer will not call this for ep0, but do a sanity check */
+	if (ep == bdc->bdc_ep_array[1]) {
+		dev_warn(bdc->dev, "%s called for ep0\n", __func__);
+		return -EINVAL;
+	}
+	dev_dbg(bdc->dev,
+		"%s() ep:%s ep->flags:%08x\n",
+		__func__, ep->name, ep->flags);
+
+	if (!(ep->flags & BDC_EP_ENABLED)) {
+		dev_warn(bdc->dev, "%s is already disabled\n", ep->name);
+		return 0;
+	}
+	spin_lock_irqsave(&bdc->lock, flags);
+	ret = bdc_ep_disable(ep);
+	spin_unlock_irqrestore(&bdc->lock, flags);
+
+	return ret;
+}
+
+static const struct usb_ep_ops bdc_gadget_ep_ops = {
+	.enable = bdc_gadget_ep_enable,
+	.disable = bdc_gadget_ep_disable,
+	.alloc_request = bdc_gadget_alloc_request,
+	.free_request = bdc_gadget_free_request,
+	.queue = bdc_gadget_ep_queue,
+	.dequeue = bdc_gadget_ep_dequeue,
+	.set_halt = bdc_gadget_ep_set_halt
+};
+
+/* dir = 1 is IN */
+static int init_ep(struct bdc *bdc, u32 epnum, u32 dir)
+{
+	struct bdc_ep *ep;
+
+	dev_dbg(bdc->dev, "%s epnum=%d dir=%d\n", __func__, epnum, dir);
+	ep = kzalloc(sizeof(*ep), GFP_KERNEL);
+	if (!ep)
+		return -ENOMEM;
+
+	ep->bdc = bdc;
+	ep->dir = dir;
+
+	/* ep->ep_num is the index inside bdc_ep */
+	if (epnum == 1) {
+		ep->ep_num = 1;
+		bdc->bdc_ep_array[ep->ep_num] = ep;
+		snprintf(ep->name, sizeof(ep->name), "ep%d", epnum - 1);
+		usb_ep_set_maxpacket_limit(&ep->usb_ep, EP0_MAX_PKT_SIZE);
+		ep->comp_desc = NULL;
+		bdc->gadget.ep0 = &ep->usb_ep;
+	} else {
+		if (dir)
+			ep->ep_num = epnum * 2 - 1;
+		else
+			ep->ep_num = epnum * 2 - 2;
+
+		bdc->bdc_ep_array[ep->ep_num] = ep;
+		snprintf(ep->name, sizeof(ep->name), "ep%d%s", epnum - 1,
+			 dir & 1 ? "in" : "out");
+
+		usb_ep_set_maxpacket_limit(&ep->usb_ep, 1024);
+		ep->usb_ep.max_streams = 0;
+		list_add_tail(&ep->usb_ep.ep_list, &bdc->gadget.ep_list);
+	}
+	ep->usb_ep.ops = &bdc_gadget_ep_ops;
+	ep->usb_ep.name = ep->name;
+	ep->flags = 0;
+	ep->ignore_next_sr = false;
+	dev_dbg(bdc->dev, "ep=%p ep->usb_ep.name=%s epnum=%d ep->epnum=%d\n",
+				ep, ep->usb_ep.name, epnum, ep->ep_num);
+
+	INIT_LIST_HEAD(&ep->queue);
+
+	return 0;
+}
+
+/* Init all ep */
+int bdc_init_ep(struct bdc *bdc)
+{
+	u8 epnum;
+	int ret;
+
+	dev_dbg(bdc->dev, "%s()\n", __func__);
+	INIT_LIST_HEAD(&bdc->gadget.ep_list);
+	/* init ep0 */
+	ret = init_ep(bdc, 1, 0);
+	if (ret) {
+		dev_err(bdc->dev, "init ep ep0 fail %d\n", ret);
+		return ret;
+	}
+
+	for (epnum = 2; epnum <= bdc->num_eps / 2; epnum++) {
+		/* OUT */
+		ret = init_ep(bdc, epnum, 0);
+		if (ret) {
+			dev_err(bdc->dev,
+				"init ep failed for:%d error: %d\n",
+				epnum, ret);
+			return ret;
+		}
+
+		/* IN */
+		ret = init_ep(bdc, epnum, 1);
+		if (ret) {
+			dev_err(bdc->dev,
+				"init ep failed for:%d error: %d\n",
+				epnum, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
diff --git a/drivers/usb/gadget/udc/bdc/bdc_ep.h b/drivers/usb/gadget/udc/bdc/bdc_ep.h
new file mode 100644
index 000000000000..8a6b36cbf2ea
--- /dev/null
+++ b/drivers/usb/gadget/udc/bdc/bdc_ep.h
@@ -0,0 +1,22 @@
+/*
+ * bdc_ep.h - header for the BDC debug functions
+ *
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * Author: Ashwini Pahuja
+ *
+ * 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_BDC_EP_H__
+#define __LINUX_BDC_EP_H__
+
+int bdc_init_ep(struct bdc *);
+int bdc_ep_disable(struct bdc_ep *);
+int bdc_ep_enable(struct bdc_ep *);
+void bdc_free_ep(struct bdc *);
+
+#endif /* __LINUX_BDC_EP_H__ */
diff --git a/drivers/usb/gadget/udc/bdc/bdc_pci.c b/drivers/usb/gadget/udc/bdc/bdc_pci.c
new file mode 100644
index 000000000000..02968842b359
--- /dev/null
+++ b/drivers/usb/gadget/udc/bdc/bdc_pci.c
@@ -0,0 +1,132 @@
+/*
+ * bdc_pci.c - BRCM BDC USB3.0 device controller PCI interface file.
+ *
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * Author: Ashwini Pahuja
+ *
+ * Based on drivers under drivers/usb/
+ *
+ * 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/slab.h>
+#include <linux/pci.h>
+#include <linux/pci_ids.h>
+#include <linux/platform_device.h>
+
+#include "bdc.h"
+
+#define BDC_PCI_PID 0x1570
+
+struct bdc_pci {
+	struct device *dev;
+	struct platform_device *bdc;
+};
+
+static int bdc_setup_msi(struct pci_dev *pci)
+{
+	int ret;
+
+	ret = pci_enable_msi(pci);
+	if (ret) {
+		pr_err("failed to allocate MSI entry\n");
+		return ret;
+	}
+
+	return ret;
+}
+
+static int bdc_pci_probe(struct pci_dev *pci, const struct pci_device_id *id)
+{
+	struct resource res[2];
+	struct platform_device *bdc;
+	struct bdc_pci *glue;
+	int ret = -ENOMEM;
+
+	glue = devm_kzalloc(&pci->dev, sizeof(*glue), GFP_KERNEL);
+	if (!glue)
+		return -ENOMEM;
+
+	glue->dev = &pci->dev;
+	ret = pci_enable_device(pci);
+	if (ret) {
+		dev_err(&pci->dev, "failed to enable pci device\n");
+		return -ENODEV;
+	}
+	pci_set_master(pci);
+
+	bdc = platform_device_alloc(BRCM_BDC_NAME, PLATFORM_DEVID_AUTO);
+	if (!bdc)
+		return -ENOMEM;
+
+	memset(res, 0x00, sizeof(struct resource) * ARRAY_SIZE(res));
+	bdc_setup_msi(pci);
+
+	res[0].start	= pci_resource_start(pci, 0);
+	res[0].end	= pci_resource_end(pci, 0);
+	res[0].name	= BRCM_BDC_NAME;
+	res[0].flags	= IORESOURCE_MEM;
+
+	res[1].start	= pci->irq;
+	res[1].name	= BRCM_BDC_NAME;
+	res[1].flags	= IORESOURCE_IRQ;
+
+	ret = platform_device_add_resources(bdc, res, ARRAY_SIZE(res));
+	if (ret) {
+		dev_err(&pci->dev,
+			"couldn't add resources to bdc device\n");
+		return ret;
+	}
+
+	pci_set_drvdata(pci, glue);
+
+	dma_set_coherent_mask(&bdc->dev, pci->dev.coherent_dma_mask);
+
+	bdc->dev.dma_mask = pci->dev.dma_mask;
+	bdc->dev.dma_parms = pci->dev.dma_parms;
+	bdc->dev.parent = &pci->dev;
+	glue->bdc = bdc;
+
+	ret = platform_device_add(bdc);
+	if (ret) {
+		dev_err(&pci->dev, "failed to register bdc device\n");
+		platform_device_put(bdc);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void bdc_pci_remove(struct pci_dev *pci)
+{
+	struct bdc_pci *glue = pci_get_drvdata(pci);
+
+	platform_device_unregister(glue->bdc);
+	pci_disable_msi(pci);
+}
+
+static struct pci_device_id bdc_pci_id_table[] = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, BDC_PCI_PID), },
+	{} /* Terminating Entry */
+};
+
+MODULE_DEVICE_TABLE(pci, bdc_pci_id_table);
+
+static struct pci_driver bdc_pci_driver = {
+	.name = "bdc-pci",
+	.id_table = bdc_pci_id_table,
+	.probe = bdc_pci_probe,
+	.remove = bdc_pci_remove,
+};
+
+MODULE_AUTHOR("Ashwini Pahuja <ashwini.linux@gmail.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("BRCM BDC USB3 PCI Glue layer");
+module_pci_driver(bdc_pci_driver);
diff --git a/drivers/usb/gadget/udc/bdc/bdc_udc.c b/drivers/usb/gadget/udc/bdc/bdc_udc.c
new file mode 100644
index 000000000000..3700ce70b0be
--- /dev/null
+++ b/drivers/usb/gadget/udc/bdc/bdc_udc.c
@@ -0,0 +1,587 @@
+/*
+ * bdc_udc.c - BRCM BDC USB3.0 device controller gagdet ops
+ *
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * Author: Ashwini Pahuja
+ *
+ * Based on drivers under drivers/usb/gadget/udc/
+ *
+ * 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/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 <linux/platform_device.h>
+
+#include "bdc.h"
+#include "bdc_ep.h"
+#include "bdc_cmd.h"
+#include "bdc_dbg.h"
+
+static const struct usb_gadget_ops bdc_gadget_ops;
+
+static const char * const conn_speed_str[] =  {
+	"Not connected",
+	"Full Speed",
+	"Low Speed",
+	"High Speed",
+	"Super Speed",
+};
+
+/* EP0 initial descripror */
+static struct usb_endpoint_descriptor bdc_gadget_ep0_desc = {
+	.bLength = USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType = USB_DT_ENDPOINT,
+	.bmAttributes = USB_ENDPOINT_XFER_CONTROL,
+	.bEndpointAddress = 0,
+	.wMaxPacketSize	= cpu_to_le16(EP0_MAX_PKT_SIZE),
+};
+
+/* Advance the srr dqp maintained by SW */
+static void srr_dqp_index_advc(struct bdc *bdc, u32 srr_num)
+{
+	struct srr *srr;
+
+	srr = &bdc->srr;
+	dev_dbg_ratelimited(bdc->dev, "srr->dqp_index:%d\n", srr->dqp_index);
+	srr->dqp_index++;
+	/* rollback to 0 if we are past the last */
+	if (srr->dqp_index == NUM_SR_ENTRIES)
+		srr->dqp_index = 0;
+}
+
+/* connect sr */
+static void bdc_uspc_connected(struct bdc *bdc)
+{
+	u32 speed, temp;
+	u32 usppms;
+	int ret;
+
+	temp = bdc_readl(bdc->regs, BDC_USPC);
+	speed = BDC_PSP(temp);
+	dev_dbg(bdc->dev, "%s speed=%x\n", __func__, speed);
+	switch (speed) {
+	case BDC_SPEED_SS:
+		bdc_gadget_ep0_desc.wMaxPacketSize =
+						cpu_to_le16(EP0_MAX_PKT_SIZE);
+		bdc->gadget.ep0->maxpacket = EP0_MAX_PKT_SIZE;
+		bdc->gadget.speed = USB_SPEED_SUPER;
+		/* Enable U1T in SS mode */
+		usppms =  bdc_readl(bdc->regs, BDC_USPPMS);
+		usppms &= ~BDC_U1T(0xff);
+		usppms |= BDC_U1T(U1_TIMEOUT);
+		usppms |= BDC_PORT_W1S;
+		bdc_writel(bdc->regs, BDC_USPPMS, usppms);
+		break;
+
+	case BDC_SPEED_HS:
+		bdc_gadget_ep0_desc.wMaxPacketSize = cpu_to_le16(64);
+		bdc->gadget.ep0->maxpacket = 64;
+		bdc->gadget.speed = USB_SPEED_HIGH;
+		break;
+
+	case BDC_SPEED_FS:
+		bdc_gadget_ep0_desc.wMaxPacketSize = cpu_to_le16(64);
+		bdc->gadget.ep0->maxpacket = 64;
+		bdc->gadget.speed = USB_SPEED_FULL;
+		break;
+
+	case BDC_SPEED_LS:
+		bdc_gadget_ep0_desc.wMaxPacketSize = cpu_to_le16(8);
+		bdc->gadget.ep0->maxpacket = 8;
+		bdc->gadget.speed = USB_SPEED_LOW;
+		break;
+	default:
+		dev_err(bdc->dev, "UNDEFINED SPEED\n");
+		return;
+	}
+	dev_dbg(bdc->dev, "connected at %s\n", conn_speed_str[speed]);
+	/* Now we know the speed, configure ep0 */
+	bdc->bdc_ep_array[1]->desc = &bdc_gadget_ep0_desc;
+	ret = bdc_config_ep(bdc, bdc->bdc_ep_array[1]);
+	if (ret)
+		dev_err(bdc->dev, "EP0 config failed\n");
+	bdc->bdc_ep_array[1]->usb_ep.desc = &bdc_gadget_ep0_desc;
+	bdc->bdc_ep_array[1]->flags |= BDC_EP_ENABLED;
+	usb_gadget_set_state(&bdc->gadget, USB_STATE_DEFAULT);
+}
+
+/* device got disconnected */
+static void bdc_uspc_disconnected(struct bdc *bdc, bool reinit)
+{
+	struct bdc_ep *ep;
+
+	dev_dbg(bdc->dev, "%s\n", __func__);
+	/*
+	 * Only stop ep0 from here, rest of the endpoints will be disabled
+	 * from gadget_disconnect
+	 */
+	ep = bdc->bdc_ep_array[1];
+	if (ep && (ep->flags & BDC_EP_ENABLED))
+		/* if enabled then stop and remove requests */
+		bdc_ep_disable(ep);
+
+	if (bdc->gadget_driver && bdc->gadget_driver->disconnect) {
+		spin_unlock(&bdc->lock);
+		bdc->gadget_driver->disconnect(&bdc->gadget);
+		spin_lock(&bdc->lock);
+	}
+	/* Set Unknown speed */
+	bdc->gadget.speed = USB_SPEED_UNKNOWN;
+	bdc->devstatus &= DEVSTATUS_CLEAR;
+	bdc->delayed_status = false;
+	bdc->reinit = reinit;
+	bdc->test_mode = false;
+}
+
+/* TNotify wkaeup timer */
+static void bdc_func_wake_timer(struct work_struct *work)
+{
+	struct bdc *bdc = container_of(work, struct bdc, func_wake_notify.work);
+	unsigned long flags;
+
+	dev_dbg(bdc->dev, "%s\n", __func__);
+	spin_lock_irqsave(&bdc->lock, flags);
+	/*
+	 * Check if host has started transferring on endpoints
+	 * FUNC_WAKE_ISSUED is cleared when transfer has started after resume
+	*/
+	if (bdc->devstatus & FUNC_WAKE_ISSUED) {
+		dev_dbg(bdc->dev, "FUNC_WAKE_ISSUED FLAG IS STILL SET\n");
+		/* flag is still set, so again send func wake */
+		bdc_function_wake_fh(bdc, 0);
+		schedule_delayed_work(&bdc->func_wake_notify,
+						msecs_to_jiffies(BDC_TNOTIFY));
+	}
+	spin_unlock_irqrestore(&bdc->lock, flags);
+}
+
+/* handler for Link state change condition */
+static void handle_link_state_change(struct bdc *bdc, u32 uspc)
+{
+	u32 link_state;
+
+	dev_dbg(bdc->dev, "Link state change");
+	link_state = BDC_PST(uspc);
+	switch (link_state) {
+	case BDC_LINK_STATE_U3:
+		if ((bdc->gadget.speed != USB_SPEED_UNKNOWN) &&
+						bdc->gadget_driver->suspend) {
+			dev_dbg(bdc->dev, "Entered Suspend mode\n");
+			spin_unlock(&bdc->lock);
+			bdc->devstatus |= DEVICE_SUSPENDED;
+			bdc->gadget_driver->suspend(&bdc->gadget);
+			spin_lock(&bdc->lock);
+		}
+		break;
+	case BDC_LINK_STATE_U0:
+		if (bdc->devstatus & REMOTE_WAKEUP_ISSUED) {
+					bdc->devstatus &= ~REMOTE_WAKEUP_ISSUED;
+			if (bdc->gadget.speed == USB_SPEED_SUPER) {
+				bdc_function_wake_fh(bdc, 0);
+				bdc->devstatus |= FUNC_WAKE_ISSUED;
+				/*
+				 * Start a Notification timer and check if the
+				 * Host transferred anything on any of the EPs,
+				 * if not then send function wake again every
+				 * TNotification secs until host initiates
+				 * transfer to BDC, USB3 spec Table 8.13
+				*/
+				schedule_delayed_work(
+						&bdc->func_wake_notify,
+						msecs_to_jiffies(BDC_TNOTIFY));
+				dev_dbg(bdc->dev, "sched func_wake_notify\n");
+			}
+		}
+		break;
+
+	case BDC_LINK_STATE_RESUME:
+		dev_dbg(bdc->dev, "Resumed from Suspend\n");
+		if (bdc->devstatus & DEVICE_SUSPENDED) {
+			bdc->gadget_driver->resume(&bdc->gadget);
+			bdc->devstatus &= ~DEVICE_SUSPENDED;
+		}
+		break;
+	default:
+		dev_dbg(bdc->dev, "link state:%d\n", link_state);
+	}
+}
+
+/* something changes on upstream port, handle it here */
+void bdc_sr_uspc(struct bdc *bdc, struct bdc_sr *sreport)
+{
+	u32 clear_flags = 0;
+	u32 uspc;
+	bool connected = false;
+	bool disconn = false;
+
+	uspc = bdc_readl(bdc->regs, BDC_USPC);
+	dev_dbg(bdc->dev, "%s uspc=0x%08x\n", __func__, uspc);
+
+	/* Port connect changed */
+	if (uspc & BDC_PCC) {
+		/* Vbus not present, and not connected to Downstream port */
+		if ((uspc & BDC_VBC) && !(uspc & BDC_VBS) && !(uspc & BDC_PCS))
+			disconn = true;
+		else if ((uspc & BDC_PCS) && !BDC_PST(uspc))
+			connected = true;
+	}
+
+	/* Change in VBus and VBus is present */
+	if ((uspc & BDC_VBC) && (uspc & BDC_VBS)) {
+		if (bdc->pullup) {
+			dev_dbg(bdc->dev, "Do a softconnect\n");
+			/* Attached state, do a softconnect */
+			bdc_softconn(bdc);
+			usb_gadget_set_state(&bdc->gadget, USB_STATE_POWERED);
+		}
+		clear_flags = BDC_VBC;
+	} else if ((uspc & BDC_PRS) || (uspc & BDC_PRC) || disconn) {
+		/* Hot reset, warm reset, 2.0 bus reset or disconn */
+		dev_dbg(bdc->dev, "Port reset or disconn\n");
+		bdc_uspc_disconnected(bdc, disconn);
+		clear_flags = BDC_PCC|BDC_PCS|BDC_PRS|BDC_PRC;
+	} else if ((uspc & BDC_PSC) && (uspc & BDC_PCS)) {
+		/* Change in Link state */
+		handle_link_state_change(bdc, uspc);
+		clear_flags = BDC_PSC|BDC_PCS;
+	}
+
+	/*
+	 * In SS we might not have PRC bit set before connection, but in 2.0
+	 * the PRC bit is set before connection, so moving this condition out
+	 * of bus reset to handle both SS/2.0 speeds.
+	 */
+	if (connected) {
+		/* This is the connect event for U0/L0 */
+		dev_dbg(bdc->dev, "Connected\n");
+		bdc_uspc_connected(bdc);
+		bdc->devstatus &= ~(DEVICE_SUSPENDED);
+	}
+	uspc = bdc_readl(bdc->regs, BDC_USPC);
+	uspc &= (~BDC_USPSC_RW);
+	dev_dbg(bdc->dev, "uspc=%x\n", uspc);
+	bdc_writel(bdc->regs, BDC_USPC, clear_flags);
+}
+
+/* Main interrupt handler for bdc */
+static irqreturn_t bdc_udc_interrupt(int irq, void *_bdc)
+{
+	u32 eqp_index, dqp_index, sr_type, srr_int;
+	struct bdc_sr *sreport;
+	struct bdc *bdc = _bdc;
+	u32 status;
+	int ret;
+
+	spin_lock(&bdc->lock);
+	status = bdc_readl(bdc->regs, BDC_BDCSC);
+	if (!(status & BDC_GIP)) {
+		spin_unlock(&bdc->lock);
+		return IRQ_NONE;
+	}
+	srr_int = bdc_readl(bdc->regs, BDC_SRRINT(0));
+	/* Check if the SRR IP bit it set? */
+	if (!(srr_int & BDC_SRR_IP)) {
+		dev_warn(bdc->dev, "Global irq pending but SRR IP is 0\n");
+		spin_unlock(&bdc->lock);
+		return IRQ_NONE;
+	}
+	eqp_index = BDC_SRR_EPI(srr_int);
+	dqp_index = BDC_SRR_DPI(srr_int);
+	dev_dbg(bdc->dev,
+			"%s eqp_index=%d dqp_index=%d  srr.dqp_index=%d\n\n",
+			 __func__, eqp_index, dqp_index, bdc->srr.dqp_index);
+
+	/* check for ring empty condition */
+	if (eqp_index == dqp_index) {
+		dev_dbg(bdc->dev, "SRR empty?\n");
+		spin_unlock(&bdc->lock);
+		return IRQ_HANDLED;
+	}
+
+	while (bdc->srr.dqp_index != eqp_index) {
+		sreport = &bdc->srr.sr_bds[bdc->srr.dqp_index];
+		/* sreport is read before using it */
+		rmb();
+		sr_type = le32_to_cpu(sreport->offset[3]) & BD_TYPE_BITMASK;
+		dev_dbg_ratelimited(bdc->dev, "sr_type=%d\n", sr_type);
+		switch (sr_type) {
+		case SR_XSF:
+			bdc->sr_handler[0](bdc, sreport);
+			break;
+
+		case SR_USPC:
+			bdc->sr_handler[1](bdc, sreport);
+			break;
+		default:
+			dev_warn(bdc->dev, "SR:%d not handled\n", sr_type);
+		}
+		/* Advance the srr dqp index */
+		srr_dqp_index_advc(bdc, 0);
+	}
+	/* update the hw dequeue pointer */
+	srr_int = bdc_readl(bdc->regs, BDC_SRRINT(0));
+	srr_int &= ~BDC_SRR_DPI_MASK;
+	srr_int &= ~(BDC_SRR_RWS|BDC_SRR_RST|BDC_SRR_ISR);
+	srr_int |= ((bdc->srr.dqp_index) << 16);
+	srr_int |= BDC_SRR_IP;
+	bdc_writel(bdc->regs, BDC_SRRINT(0), srr_int);
+	srr_int = bdc_readl(bdc->regs, BDC_SRRINT(0));
+	if (bdc->reinit) {
+		ret = bdc_reinit(bdc);
+		if (ret)
+			dev_err(bdc->dev, "err in bdc reinit\n");
+	}
+
+	spin_unlock(&bdc->lock);
+
+	return IRQ_HANDLED;
+}
+
+/* Gadget ops */
+static int bdc_udc_start(struct usb_gadget *gadget,
+				struct usb_gadget_driver *driver)
+{
+	struct bdc *bdc = gadget_to_bdc(gadget);
+	unsigned long flags;
+	int ret = 0;
+
+	dev_dbg(bdc->dev, "%s()\n", __func__);
+	spin_lock_irqsave(&bdc->lock, flags);
+	if (bdc->gadget_driver) {
+		dev_err(bdc->dev, "%s is already bound to %s\n",
+			bdc->gadget.name,
+			bdc->gadget_driver->driver.name);
+		ret = -EBUSY;
+		goto err;
+	}
+	/*
+	 * Run the controller from here and when BDC is connected to
+	 * Host then driver will receive a USPC SR with VBUS present
+	 * and then driver will do a softconnect.
+	*/
+	ret = bdc_run(bdc);
+	if (ret) {
+		dev_err(bdc->dev, "%s bdc run fail\n", __func__);
+		goto err;
+	}
+	bdc->gadget_driver = driver;
+	bdc->gadget.dev.driver = &driver->driver;
+err:
+	spin_unlock_irqrestore(&bdc->lock, flags);
+
+	return ret;
+}
+
+static int bdc_udc_stop(struct usb_gadget *gadget)
+{
+	struct bdc *bdc = gadget_to_bdc(gadget);
+	unsigned long flags;
+
+	dev_dbg(bdc->dev, "%s()\n", __func__);
+	spin_lock_irqsave(&bdc->lock, flags);
+	bdc_stop(bdc);
+	bdc->gadget_driver = NULL;
+	bdc->gadget.dev.driver = NULL;
+	spin_unlock_irqrestore(&bdc->lock, flags);
+
+	return 0;
+}
+
+static int bdc_udc_pullup(struct usb_gadget *gadget, int is_on)
+{
+	struct bdc *bdc = gadget_to_bdc(gadget);
+	unsigned long flags;
+	u32 uspc;
+
+	dev_dbg(bdc->dev, "%s() is_on:%d\n", __func__, is_on);
+	if (!gadget)
+		return -EINVAL;
+
+	spin_lock_irqsave(&bdc->lock, flags);
+	if (!is_on) {
+		bdc_softdisconn(bdc);
+		bdc->pullup = false;
+	} else {
+		/*
+		 * For a self powered device, we need to wait till we receive
+		 * a VBUS change and Vbus present event, then if pullup flag
+		 * is set, then only we present the Termintation.
+		 */
+		bdc->pullup = true;
+		/*
+		 * Check if BDC is already connected to Host i.e Vbus=1,
+		 * if yes, then present TERM now, this is typical for bus
+		 * powered devices.
+		 */
+		uspc = bdc_readl(bdc->regs, BDC_USPC);
+		if (uspc & BDC_VBS)
+			bdc_softconn(bdc);
+	}
+	spin_unlock_irqrestore(&bdc->lock, flags);
+
+	return 0;
+}
+
+static int bdc_udc_set_selfpowered(struct usb_gadget *gadget,
+		int is_self)
+{
+	struct bdc		*bdc = gadget_to_bdc(gadget);
+	unsigned long           flags;
+
+	dev_dbg(bdc->dev, "%s()\n", __func__);
+	spin_lock_irqsave(&bdc->lock, flags);
+	if (!is_self)
+		bdc->devstatus |= 1 << USB_DEVICE_SELF_POWERED;
+	else
+		bdc->devstatus &= ~(1 << USB_DEVICE_SELF_POWERED);
+
+	spin_unlock_irqrestore(&bdc->lock, flags);
+
+	return 0;
+}
+
+static int bdc_udc_wakeup(struct usb_gadget *gadget)
+{
+	struct bdc *bdc = gadget_to_bdc(gadget);
+	unsigned long		flags;
+	u8	link_state;
+	u32	uspc;
+	int ret = 0;
+
+	dev_dbg(bdc->dev,
+		"%s() bdc->devstatus=%08x\n",
+		__func__, bdc->devstatus);
+
+	if (!(bdc->devstatus & REMOTE_WAKE_ENABLE))
+		return  -EOPNOTSUPP;
+
+	spin_lock_irqsave(&bdc->lock, flags);
+	uspc = bdc_readl(bdc->regs, BDC_USPC);
+	link_state = BDC_PST(uspc);
+	dev_dbg(bdc->dev, "link_state =%d portsc=%x", link_state, uspc);
+	if (link_state != BDC_LINK_STATE_U3) {
+		dev_warn(bdc->dev,
+			"can't wakeup from link state %d\n",
+			link_state);
+		ret = -EINVAL;
+		goto out;
+	}
+	if (bdc->gadget.speed == USB_SPEED_SUPER)
+		bdc->devstatus |= REMOTE_WAKEUP_ISSUED;
+
+	uspc &= ~BDC_PST_MASK;
+	uspc &= (~BDC_USPSC_RW);
+	uspc |=  BDC_PST(BDC_LINK_STATE_U0);
+	uspc |=  BDC_SWS;
+	bdc_writel(bdc->regs, BDC_USPC, uspc);
+	uspc = bdc_readl(bdc->regs, BDC_USPC);
+	link_state = BDC_PST(uspc);
+	dev_dbg(bdc->dev, "link_state =%d portsc=%x", link_state, uspc);
+out:
+	spin_unlock_irqrestore(&bdc->lock, flags);
+
+	return ret;
+}
+
+static const struct usb_gadget_ops bdc_gadget_ops = {
+	.wakeup = bdc_udc_wakeup,
+	.set_selfpowered = bdc_udc_set_selfpowered,
+	.pullup = bdc_udc_pullup,
+	.udc_start = bdc_udc_start,
+	.udc_stop = bdc_udc_stop,
+};
+
+/* Init the gadget interface and register the udc */
+int bdc_udc_init(struct bdc *bdc)
+{
+	u32 temp;
+	int ret;
+
+	dev_dbg(bdc->dev, "%s()\n", __func__);
+	bdc->gadget.ops = &bdc_gadget_ops;
+	bdc->gadget.max_speed = USB_SPEED_SUPER;
+	bdc->gadget.speed = USB_SPEED_UNKNOWN;
+	bdc->gadget.dev.parent = bdc->dev;
+
+	bdc->gadget.sg_supported = false;
+
+
+	bdc->gadget.name = BRCM_BDC_NAME;
+	ret = devm_request_irq(bdc->dev, bdc->irq, bdc_udc_interrupt,
+				IRQF_SHARED , BRCM_BDC_NAME, bdc);
+	if (ret) {
+		dev_err(bdc->dev,
+			"failed to request irq #%d %d\n",
+			bdc->irq, ret);
+		return ret;
+	}
+
+	ret = bdc_init_ep(bdc);
+	if (ret) {
+		dev_err(bdc->dev, "bdc init ep fail: %d\n", ret);
+		return ret;
+	}
+
+	ret = usb_add_gadget_udc(bdc->dev, &bdc->gadget);
+	if (ret) {
+		dev_err(bdc->dev, "failed to register udc\n");
+		goto err0;
+	}
+	usb_gadget_set_state(&bdc->gadget, USB_STATE_NOTATTACHED);
+	bdc->bdc_ep_array[1]->desc = &bdc_gadget_ep0_desc;
+	/*
+	 * Allocate bd list for ep0 only, ep0 will be enabled on connect
+	 * status report when the speed is known
+	 */
+	ret = bdc_ep_enable(bdc->bdc_ep_array[1]);
+	if (ret) {
+		dev_err(bdc->dev, "fail to enable %s\n",
+						bdc->bdc_ep_array[1]->name);
+		goto err1;
+	}
+	INIT_DELAYED_WORK(&bdc->func_wake_notify, bdc_func_wake_timer);
+	/* Enable Interrupts */
+	temp = bdc_readl(bdc->regs, BDC_BDCSC);
+	temp |= BDC_GIE;
+	bdc_writel(bdc->regs, BDC_BDCSC, temp);
+	return 0;
+err1:
+	usb_del_gadget_udc(&bdc->gadget);
+err0:
+	bdc_free_ep(bdc);
+
+	return ret;
+}
+
+void bdc_udc_exit(struct bdc *bdc)
+{
+	dev_dbg(bdc->dev, "%s()\n", __func__);
+	bdc_ep_disable(bdc->bdc_ep_array[1]);
+	usb_del_gadget_udc(&bdc->gadget);
+	bdc_free_ep(bdc);
+}
diff --git a/drivers/usb/gadget/udc/dummy_hcd.c b/drivers/usb/gadget/udc/dummy_hcd.c
index 81dc5959e36b..9c598801404c 100644
--- a/drivers/usb/gadget/udc/dummy_hcd.c
+++ b/drivers/usb/gadget/udc/dummy_hcd.c
@@ -367,19 +367,22 @@ static void set_link_state(struct dummy_hcd *dum_hcd)
 	     dum_hcd->active)
 		dum_hcd->resuming = 0;
 
-	/* if !connected or reset */
+	/* Currently !connected or in reset */
 	if ((dum_hcd->port_status & USB_PORT_STAT_CONNECTION) == 0 ||
 			(dum_hcd->port_status & USB_PORT_STAT_RESET) != 0) {
-		/*
-		 * We're connected and not reset (reset occurred now),
-		 * and driver attached - disconnect!
-		 */
-		if ((dum_hcd->old_status & USB_PORT_STAT_CONNECTION) != 0 &&
-		    (dum_hcd->old_status & USB_PORT_STAT_RESET) == 0 &&
-		    dum->driver) {
+		unsigned disconnect = USB_PORT_STAT_CONNECTION &
+				dum_hcd->old_status & (~dum_hcd->port_status);
+		unsigned reset = USB_PORT_STAT_RESET &
+				(~dum_hcd->old_status) & dum_hcd->port_status;
+
+		/* Report reset and disconnect events to the driver */
+		if (dum->driver && (disconnect || reset)) {
 			stop_activity(dum);
 			spin_unlock(&dum->lock);
-			dum->driver->disconnect(&dum->gadget);
+			if (reset)
+				usb_gadget_udc_reset(&dum->gadget, dum->driver);
+			else
+				dum->driver->disconnect(&dum->gadget);
 			spin_lock(&dum->lock);
 		}
 	} else if (dum_hcd->active != dum_hcd->old_active) {
@@ -851,8 +854,7 @@ static int dummy_pullup(struct usb_gadget *_gadget, int value)
 
 static int dummy_udc_start(struct usb_gadget *g,
 		struct usb_gadget_driver *driver);
-static int dummy_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver);
+static int dummy_udc_stop(struct usb_gadget *g);
 
 static const struct usb_gadget_ops dummy_ops = {
 	.get_frame	= dummy_g_get_frame,
@@ -908,23 +910,16 @@ static int dummy_udc_start(struct usb_gadget *g,
 	 */
 
 	dum->devstatus = 0;
-
 	dum->driver = driver;
-	dev_dbg(udc_dev(dum), "binding gadget driver '%s'\n",
-			driver->driver.name);
+
 	return 0;
 }
 
-static int dummy_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver)
+static int dummy_udc_stop(struct usb_gadget *g)
 {
 	struct dummy_hcd	*dum_hcd = gadget_to_dummy_hcd(g);
 	struct dummy		*dum = dum_hcd->dum;
 
-	if (driver)
-		dev_dbg(udc_dev(dum), "unregister gadget driver '%s'\n",
-				driver->driver.name);
-
 	dum->driver = NULL;
 
 	return 0;
@@ -1042,7 +1037,6 @@ static struct platform_driver dummy_udc_driver = {
 	.resume		= dummy_udc_resume,
 	.driver		= {
 		.name	= (char *) gadget_name,
-		.owner	= THIS_MODULE,
 	},
 };
 
@@ -2370,7 +2364,6 @@ static void dummy_stop(struct usb_hcd *hcd)
 
 	dum = hcd_to_dummy_hcd(hcd)->dum;
 	device_remove_file(dummy_dev(hcd_to_dummy_hcd(hcd)), &dev_attr_urbs);
-	usb_gadget_unregister_driver(dum->driver);
 	dev_info(dummy_dev(hcd_to_dummy_hcd(hcd)), "stopped\n");
 }
 
@@ -2612,7 +2605,6 @@ static struct platform_driver dummy_hcd_driver = {
 	.resume		= dummy_hcd_resume,
 	.driver		= {
 		.name	= (char *) driver_name,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/usb/gadget/udc/fotg210-udc.c b/drivers/usb/gadget/udc/fotg210-udc.c
index 1d315921bf34..e547ea7f56b1 100644
--- a/drivers/usb/gadget/udc/fotg210-udc.c
+++ b/drivers/usb/gadget/udc/fotg210-udc.c
@@ -1053,8 +1053,7 @@ static void fotg210_init(struct fotg210_udc *fotg210)
 	iowrite32(value, fotg210->reg + FOTG210_DMISGR0);
 }
 
-static int fotg210_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver)
+static int fotg210_udc_stop(struct usb_gadget *g)
 {
 	struct fotg210_udc *fotg210 = gadget_to_fotg210(g);
 	unsigned long	flags;
@@ -1203,7 +1202,6 @@ err_alloc:
 static struct platform_driver fotg210_driver = {
 	.driver		= {
 		.name =	(char *)udc_name,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= fotg210_udc_probe,
 	.remove		= fotg210_udc_remove,
diff --git a/drivers/usb/gadget/udc/fsl_qe_udc.c b/drivers/usb/gadget/udc/fsl_qe_udc.c
index dd18ea38e391..795c99c77697 100644
--- a/drivers/usb/gadget/udc/fsl_qe_udc.c
+++ b/drivers/usb/gadget/udc/fsl_qe_udc.c
@@ -1887,8 +1887,7 @@ static int qe_get_frame(struct usb_gadget *gadget)
 
 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);
+static int fsl_qe_stop(struct usb_gadget *gadget);
 
 /* defined in usb_gadget.h */
 static const struct usb_gadget_ops qe_gadget_ops = {
@@ -1918,7 +1917,7 @@ static int reset_queues(struct qe_udc *udc)
 
 	/* report disconnect; the driver is already quiesced */
 	spin_unlock(&udc->lock);
-	udc->driver->disconnect(&udc->gadget);
+	usb_gadget_udc_reset(&udc->gadget, udc->driver);
 	spin_lock(&udc->lock);
 
 	return 0;
@@ -2305,13 +2304,10 @@ static int fsl_qe_start(struct usb_gadget *gadget,
 	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 *gadget,
-		struct usb_gadget_driver *driver)
+static int fsl_qe_stop(struct usb_gadget *gadget)
 {
 	struct qe_udc *udc;
 	struct qe_ep *loop_ep;
@@ -2336,8 +2332,6 @@ static int fsl_qe_stop(struct usb_gadget *gadget,
 
 	udc->driver = NULL;
 
-	dev_info(udc->dev, "unregistered gadget driver '%s'\r\n",
-			driver->driver.name);
 	return 0;
 }
 
@@ -2538,7 +2532,6 @@ static int qe_udc_probe(struct platform_device *ofdev)
 	/* create a buf for ZLP send, need to remain zeroed */
 	udc->nullbuf = devm_kzalloc(&ofdev->dev, 256, GFP_KERNEL);
 	if (udc->nullbuf == NULL) {
-		dev_err(udc->dev, "cannot alloc nullbuf\n");
 		ret = -ENOMEM;
 		goto err3;
 	}
@@ -2709,7 +2702,6 @@ MODULE_DEVICE_TABLE(of, qe_udc_match);
 static struct platform_driver udc_driver = {
 	.driver = {
 		.name = driver_name,
-		.owner = THIS_MODULE,
 		.of_match_table = qe_udc_match,
 	},
 	.probe          = qe_udc_probe,
diff --git a/drivers/usb/gadget/udc/fsl_udc_core.c b/drivers/usb/gadget/udc/fsl_udc_core.c
index c3620791a315..2df807403f22 100644
--- a/drivers/usb/gadget/udc/fsl_udc_core.c
+++ b/drivers/usb/gadget/udc/fsl_udc_core.c
@@ -1236,9 +1236,8 @@ static int fsl_pullup(struct usb_gadget *gadget, int is_on)
 
 static int fsl_udc_start(struct usb_gadget *g,
 		struct usb_gadget_driver *driver);
-static int fsl_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver);
-/* defined in gadget.h */
+static int fsl_udc_stop(struct usb_gadget *g);
+
 static const struct usb_gadget_ops fsl_gadget_ops = {
 	.get_frame = fsl_get_frame,
 	.wakeup = fsl_wakeup,
@@ -1772,7 +1771,7 @@ static void bus_resume(struct fsl_udc *udc)
 }
 
 /* Clear up all ep queues */
-static int reset_queues(struct fsl_udc *udc)
+static int reset_queues(struct fsl_udc *udc, bool bus_reset)
 {
 	u8 pipe;
 
@@ -1781,7 +1780,10 @@ static int reset_queues(struct fsl_udc *udc)
 
 	/* report disconnect; the driver is already quiesced */
 	spin_unlock(&udc->lock);
-	udc->driver->disconnect(&udc->gadget);
+	if (bus_reset)
+		usb_gadget_udc_reset(&udc->gadget, udc->driver);
+	else
+		udc->driver->disconnect(&udc->gadget);
 	spin_lock(&udc->lock);
 
 	return 0;
@@ -1835,7 +1837,7 @@ static void reset_irq(struct fsl_udc *udc)
 		udc->bus_reset = 1;
 		/* Reset all the queues, include XD, dTD, EP queue
 		 * head and TR Queue */
-		reset_queues(udc);
+		reset_queues(udc, true);
 		udc->usb_state = USB_STATE_DEFAULT;
 	} else {
 		VDBG("Controller reset");
@@ -1844,7 +1846,7 @@ static void reset_irq(struct fsl_udc *udc)
 		dr_controller_setup(udc);
 
 		/* Reset all internal used Queues */
-		reset_queues(udc);
+		reset_queues(udc, false);
 
 		ep0_setup(udc);
 
@@ -1975,8 +1977,7 @@ static int fsl_udc_start(struct usb_gadget *g,
 }
 
 /* Disconnect from gadget driver */
-static int fsl_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver)
+static int fsl_udc_stop(struct usb_gadget *g)
 {
 	struct fsl_ep *loop_ep;
 	unsigned long flags;
@@ -2669,7 +2670,6 @@ static struct platform_driver udc_driver = {
 	.resume		= fsl_udc_resume,
 	.driver		= {
 			.name = driver_name,
-			.owner = THIS_MODULE,
 			/* udc suspend/resume called from OTG driver */
 			.suspend = fsl_udc_otg_suspend,
 			.resume  = fsl_udc_otg_resume,
diff --git a/drivers/usb/gadget/udc/fusb300_udc.c b/drivers/usb/gadget/udc/fusb300_udc.c
index 8286df72add4..fb4df159d32d 100644
--- a/drivers/usb/gadget/udc/fusb300_udc.c
+++ b/drivers/usb/gadget/udc/fusb300_udc.c
@@ -1320,8 +1320,7 @@ static int fusb300_udc_start(struct usb_gadget *g,
 	return 0;
 }
 
-static int fusb300_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver)
+static int fusb300_udc_stop(struct usb_gadget *g)
 {
 	struct fusb300 *fusb300 = to_fusb300(g);
 
@@ -1496,7 +1495,6 @@ static struct platform_driver fusb300_driver = {
 	.remove =	__exit_p(fusb300_remove),
 	.driver		= {
 		.name =	(char *) udc_name,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/usb/gadget/udc/goku_udc.c b/drivers/usb/gadget/udc/goku_udc.c
index bf9c5ef8b56b..5b9176e7202a 100644
--- a/drivers/usb/gadget/udc/goku_udc.c
+++ b/drivers/usb/gadget/udc/goku_udc.c
@@ -992,8 +992,7 @@ static int goku_get_frame(struct usb_gadget *_gadget)
 
 static int goku_udc_start(struct usb_gadget *g,
 		struct usb_gadget_driver *driver);
-static int goku_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver);
+static int goku_udc_stop(struct usb_gadget *g);
 
 static const struct usb_gadget_ops goku_ops = {
 	.get_frame	= goku_get_frame,
@@ -1364,8 +1363,7 @@ static void stop_activity(struct goku_udc *dev)
 		udc_enable(dev);
 }
 
-static int goku_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver)
+static int goku_udc_stop(struct usb_gadget *g)
 {
 	struct goku_udc	*dev = to_goku_udc(g);
 	unsigned long	flags;
diff --git a/drivers/usb/gadget/udc/gr_udc.c b/drivers/usb/gadget/udc/gr_udc.c
index 1b3048a6a2a3..c8868870e217 100644
--- a/drivers/usb/gadget/udc/gr_udc.c
+++ b/drivers/usb/gadget/udc/gr_udc.c
@@ -1932,14 +1932,10 @@ static int gr_udc_start(struct usb_gadget *gadget,
 
 	spin_unlock(&dev->lock);
 
-	dev_info(dev->dev, "Started with gadget driver '%s'\n",
-		 driver->driver.name);
-
 	return 0;
 }
 
-static int gr_udc_stop(struct usb_gadget *gadget,
-		       struct usb_gadget_driver *driver)
+static int gr_udc_stop(struct usb_gadget *gadget)
 {
 	struct gr_udc *dev = to_gr_udc(gadget);
 	unsigned long flags;
@@ -1951,8 +1947,6 @@ static int gr_udc_stop(struct usb_gadget *gadget,
 
 	spin_unlock_irqrestore(&dev->lock, flags);
 
-	dev_info(dev->dev, "Stopped\n");
-
 	return 0;
 }
 
@@ -2262,7 +2256,6 @@ MODULE_DEVICE_TABLE(of, gr_match);
 static struct platform_driver gr_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = gr_match,
 	},
 	.probe = gr_probe,
diff --git a/drivers/usb/gadget/udc/lpc32xx_udc.c b/drivers/usb/gadget/udc/lpc32xx_udc.c
index feab0bac8fdc..34d9b7b831b3 100644
--- a/drivers/usb/gadget/udc/lpc32xx_udc.c
+++ b/drivers/usb/gadget/udc/lpc32xx_udc.c
@@ -582,8 +582,7 @@ static void create_debug_file(struct lpc32xx_udc *udc)
 
 static void remove_debug_file(struct lpc32xx_udc *udc)
 {
-	if (udc->pde)
-		debugfs_remove(udc->pde);
+	debugfs_remove(udc->pde);
 }
 
 #else
@@ -2559,7 +2558,7 @@ static int lpc32xx_pullup(struct usb_gadget *gadget, int is_on)
 }
 
 static int lpc32xx_start(struct usb_gadget *, struct usb_gadget_driver *);
-static int lpc32xx_stop(struct usb_gadget *, struct usb_gadget_driver *);
+static int lpc32xx_stop(struct usb_gadget *);
 
 static const struct usb_gadget_ops lpc32xx_udc_ops = {
 	.get_frame		= lpc32xx_get_frame,
@@ -2961,15 +2960,11 @@ static int lpc32xx_start(struct usb_gadget *gadget,
 	return 0;
 }
 
-static int lpc32xx_stop(struct usb_gadget *gadget,
-			struct usb_gadget_driver *driver)
+static int lpc32xx_stop(struct usb_gadget *gadget)
 {
 	int i;
 	struct lpc32xx_udc *udc = to_udc(gadget);
 
-	if (!driver || driver != udc->driver)
-		return -EINVAL;
-
 	for (i = IRQ_USB_LP; i <= IRQ_USB_ATX; i++)
 		disable_irq(udc->udp_irq[i]);
 
@@ -3410,7 +3405,6 @@ static struct platform_driver lpc32xx_udc_driver = {
 	.resume		= lpc32xx_udc_resume,
 	.driver		= {
 		.name	= (char *) driver_name,
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(lpc32xx_udc_of_match),
 	},
 };
diff --git a/drivers/usb/gadget/udc/m66592-udc.c b/drivers/usb/gadget/udc/m66592-udc.c
index 898565687a8c..8c7c83c93713 100644
--- a/drivers/usb/gadget/udc/m66592-udc.c
+++ b/drivers/usb/gadget/udc/m66592-udc.c
@@ -1142,7 +1142,7 @@ static void irq_device_state(struct m66592 *m66592)
 	m66592_write(m66592, ~M66592_DVST, M66592_INTSTS0);
 
 	if (dvsq == M66592_DS_DFLT) {	/* bus reset */
-		m66592->driver->disconnect(&m66592->gadget);
+		usb_gadget_udc_reset(&m66592->gadget, m66592->driver);
 		m66592_update_usb_speed(m66592);
 	}
 	if (m66592->old_dvsq == M66592_DS_CNFG && dvsq != M66592_DS_CNFG)
@@ -1485,8 +1485,7 @@ static int m66592_udc_start(struct usb_gadget *g,
 	return 0;
 }
 
-static int m66592_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver)
+static int m66592_udc_stop(struct usb_gadget *g)
 {
 	struct m66592 *m66592 = to_m66592(g);
 
@@ -1699,7 +1698,6 @@ static struct platform_driver m66592_driver = {
 	.remove =	__exit_p(m66592_remove),
 	.driver		= {
 		.name =	(char *) udc_name,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/usb/gadget/udc/mv_u3d_core.c b/drivers/usb/gadget/udc/mv_u3d_core.c
index 046a1f808b0d..ea35a248c898 100644
--- a/drivers/usb/gadget/udc/mv_u3d_core.c
+++ b/drivers/usb/gadget/udc/mv_u3d_core.c
@@ -1266,8 +1266,7 @@ static int mv_u3d_start(struct usb_gadget *g,
 	return 0;
 }
 
-static int mv_u3d_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver)
+static int mv_u3d_stop(struct usb_gadget *g)
 {
 	struct mv_u3d *u3d = container_of(g, struct mv_u3d, gadget);
 	struct mv_usb_platform_data *pdata = dev_get_platdata(u3d->dev);
@@ -1284,7 +1283,7 @@ static int mv_u3d_stop(struct usb_gadget *g,
 	mv_u3d_controller_stop(u3d);
 	/* stop all usb activities */
 	u3d->gadget.speed = USB_SPEED_UNKNOWN;
-	mv_u3d_stop_activity(u3d, driver);
+	mv_u3d_stop_activity(u3d, NULL);
 	mv_u3d_disable(u3d);
 
 	if (pdata->phy_deinit)
@@ -2053,7 +2052,6 @@ static struct platform_driver mv_u3d_driver = {
 	.remove		= mv_u3d_remove,
 	.shutdown	= mv_u3d_shutdown,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "mv-u3d",
 		.pm	= &mv_u3d_pm_ops,
 	},
diff --git a/drivers/usb/gadget/udc/mv_udc_core.c b/drivers/usb/gadget/udc/mv_udc_core.c
index 3c5db80ae325..253f3df8326a 100644
--- a/drivers/usb/gadget/udc/mv_udc_core.c
+++ b/drivers/usb/gadget/udc/mv_udc_core.c
@@ -1223,7 +1223,7 @@ static int mv_udc_pullup(struct usb_gadget *gadget, int is_on)
 }
 
 static int mv_udc_start(struct usb_gadget *, struct usb_gadget_driver *);
-static int mv_udc_stop(struct usb_gadget *, struct usb_gadget_driver *);
+static int mv_udc_stop(struct usb_gadget *);
 /* device controller usb_gadget_ops structure */
 static const struct usb_gadget_ops mv_ops = {
 
@@ -1307,6 +1307,23 @@ static void nuke(struct mv_ep *ep, int status)
 	}
 }
 
+static void gadget_reset(struct mv_udc *udc, struct usb_gadget_driver *driver)
+{
+	struct mv_ep	*ep;
+
+	nuke(&udc->eps[0], -ESHUTDOWN);
+
+	list_for_each_entry(ep, &udc->gadget.ep_list, ep.ep_list) {
+		nuke(ep, -ESHUTDOWN);
+	}
+
+	/* report reset; the driver is already quiesced */
+	if (driver) {
+		spin_unlock(&udc->lock);
+		usb_gadget_udc_reset(&udc->gadget, driver);
+		spin_lock(&udc->lock);
+	}
+}
 /* stop all USB activities */
 static void stop_activity(struct mv_udc *udc, struct usb_gadget_driver *driver)
 {
@@ -1371,8 +1388,7 @@ static int mv_udc_start(struct usb_gadget *gadget,
 	return 0;
 }
 
-static int mv_udc_stop(struct usb_gadget *gadget,
-		struct usb_gadget_driver *driver)
+static int mv_udc_stop(struct usb_gadget *gadget)
 {
 	struct mv_udc *udc;
 	unsigned long flags;
@@ -1386,7 +1402,7 @@ static int mv_udc_stop(struct usb_gadget *gadget,
 
 	/* stop all usb activities */
 	udc->gadget.speed = USB_SPEED_UNKNOWN;
-	stop_activity(udc, driver);
+	stop_activity(udc, NULL);
 	mv_udc_disable(udc);
 
 	spin_unlock_irqrestore(&udc->lock, flags);
@@ -1882,7 +1898,7 @@ static void irq_process_reset(struct mv_udc *udc)
 		dev_info(&udc->dev->dev, "usb bus reset\n");
 		udc->usb_state = USB_STATE_DEFAULT;
 		/* reset all the queues, stop all USB activities */
-		stop_activity(udc, udc->driver);
+		gadget_reset(udc, udc->driver);
 	} else {
 		dev_info(&udc->dev->dev, "USB reset portsc 0x%x\n",
 			readl(&udc->op_regs->portsc));
@@ -2107,10 +2123,8 @@ static int mv_udc_probe(struct platform_device *pdev)
 	}
 
 	udc = devm_kzalloc(&pdev->dev, sizeof(*udc), GFP_KERNEL);
-	if (udc == NULL) {
-		dev_err(&pdev->dev, "failed to allocate memory for udc\n");
+	if (udc == NULL)
 		return -ENOMEM;
-	}
 
 	udc->done = &release_done;
 	udc->pdata = dev_get_platdata(&pdev->dev);
@@ -2207,7 +2221,6 @@ static int mv_udc_probe(struct platform_device *pdev)
 	size = udc->max_eps * sizeof(struct mv_ep) *2;
 	udc->eps = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
 	if (udc->eps == NULL) {
-		dev_err(&pdev->dev, "allocate ep memory failed\n");
 		retval = -ENOMEM;
 		goto err_destroy_dma;
 	}
@@ -2216,7 +2229,6 @@ static int mv_udc_probe(struct platform_device *pdev)
 	udc->status_req = devm_kzalloc(&pdev->dev, sizeof(struct mv_req),
 					GFP_KERNEL);
 	if (!udc->status_req) {
-		dev_err(&pdev->dev, "allocate status_req memory failed\n");
 		retval = -ENOMEM;
 		goto err_destroy_dma;
 	}
@@ -2403,7 +2415,6 @@ static struct platform_driver udc_driver = {
 	.remove		= mv_udc_remove,
 	.shutdown	= mv_udc_shutdown,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "mv-udc",
 #ifdef CONFIG_PM
 		.pm	= &mv_udc_pm_ops,
diff --git a/drivers/usb/gadget/udc/net2272.c b/drivers/usb/gadget/udc/net2272.c
index 84d7162a8022..d20de1fab08e 100644
--- a/drivers/usb/gadget/udc/net2272.c
+++ b/drivers/usb/gadget/udc/net2272.c
@@ -1169,8 +1169,7 @@ net2272_pullup(struct usb_gadget *_gadget, int is_on)
 
 static int net2272_start(struct usb_gadget *_gadget,
 		struct usb_gadget_driver *driver);
-static int net2272_stop(struct usb_gadget *_gadget,
-		struct usb_gadget_driver *driver);
+static int net2272_stop(struct usb_gadget *_gadget);
 
 static const struct usb_gadget_ops net2272_ops = {
 	.get_frame	= net2272_get_frame,
@@ -1471,8 +1470,6 @@ static int net2272_start(struct usb_gadget *_gadget,
 	 */
 	net2272_ep0_start(dev);
 
-	dev_dbg(dev->dev, "%s ready\n", driver->driver.name);
-
 	return 0;
 }
 
@@ -1502,8 +1499,7 @@ stop_activity(struct net2272 *dev, struct usb_gadget_driver *driver)
 	net2272_usb_reinit(dev);
 }
 
-static int net2272_stop(struct usb_gadget *_gadget,
-		struct usb_gadget_driver *driver)
+static int net2272_stop(struct usb_gadget *_gadget)
 {
 	struct net2272 *dev;
 	unsigned long flags;
@@ -1511,12 +1507,11 @@ static int net2272_stop(struct usb_gadget *_gadget,
 	dev = container_of(_gadget, struct net2272, gadget);
 
 	spin_lock_irqsave(&dev->lock, flags);
-	stop_activity(dev, driver);
+	stop_activity(dev, NULL);
 	spin_unlock_irqrestore(&dev->lock, flags);
 
 	dev->driver = NULL;
 
-	dev_dbg(dev->dev, "unregistered driver '%s'\n", driver->driver.name);
 	return 0;
 }
 
@@ -1987,17 +1982,42 @@ net2272_handle_stat1_irqs(struct net2272 *dev, u8 stat)
 	mask = (1 << USB_HIGH_SPEED) | (1 << USB_FULL_SPEED);
 
 	if (stat & tmp) {
+		bool	reset = false;
+		bool	disconnect = false;
+
+		/*
+		 * Ignore disconnects and resets if the speed hasn't been set.
+		 * VBUS can bounce and there's always an initial reset.
+		 */
 		net2272_write(dev, IRQSTAT1, tmp);
-		if ((((stat & (1 << ROOT_PORT_RESET_INTERRUPT)) &&
-				((net2272_read(dev, USBCTL1) & mask) == 0))
-			|| ((net2272_read(dev, USBCTL1) & (1 << VBUS_PIN))
-				== 0))
-				&& (dev->gadget.speed != USB_SPEED_UNKNOWN)) {
-			dev_dbg(dev->dev, "disconnect %s\n",
-				dev->driver->driver.name);
-			stop_activity(dev, dev->driver);
-			net2272_ep0_start(dev);
-			return;
+		if (dev->gadget.speed != USB_SPEED_UNKNOWN) {
+			if ((stat & (1 << VBUS_INTERRUPT)) &&
+					(net2272_read(dev, USBCTL1) &
+						(1 << VBUS_PIN)) == 0) {
+				disconnect = true;
+				dev_dbg(dev->dev, "disconnect %s\n",
+					dev->driver->driver.name);
+			} else if ((stat & (1 << ROOT_PORT_RESET_INTERRUPT)) &&
+					(net2272_read(dev, USBCTL1) & mask)
+						== 0) {
+				reset = true;
+				dev_dbg(dev->dev, "reset %s\n",
+					dev->driver->driver.name);
+			}
+
+			if (disconnect || reset) {
+				stop_activity(dev, dev->driver);
+				net2272_ep0_start(dev);
+				spin_unlock(&dev->lock);
+				if (reset)
+					usb_gadget_udc_reset
+						(&dev->gadget, dev->driver);
+				else
+					(dev->driver->disconnect)
+						(&dev->gadget);
+				spin_lock(&dev->lock);
+				return;
+			}
 		}
 		stat &= ~tmp;
 
@@ -2200,18 +2220,8 @@ static void
 net2272_remove(struct net2272 *dev)
 {
 	usb_del_gadget_udc(&dev->gadget);
-
-	/* start with the driver above us */
-	if (dev->driver) {
-		/* should have been done already by driver model core */
-		dev_warn(dev->dev, "pci remove, driver '%s' is still registered\n",
-			dev->driver->driver.name);
-		usb_gadget_unregister_driver(dev->driver);
-	}
-
 	free_irq(dev->irq, dev);
 	iounmap(dev->base_addr);
-
 	device_remove_file(dev->dev, &dev_attr_registers);
 
 	dev_info(dev->dev, "unbind\n");
@@ -2674,7 +2684,6 @@ static struct platform_driver net2272_plat_driver = {
 	.remove  = net2272_plat_remove,
 	.driver  = {
 		.name  = driver_name,
-		.owner = THIS_MODULE,
 	},
 	/* FIXME .suspend, .resume */
 };
diff --git a/drivers/usb/gadget/udc/net2280.c b/drivers/usb/gadget/udc/net2280.c
index 8d13337e2dde..d6411e0a8e03 100644
--- a/drivers/usb/gadget/udc/net2280.c
+++ b/drivers/usb/gadget/udc/net2280.c
@@ -1118,10 +1118,10 @@ static void scan_dma_completions(struct net2280_ep *ep)
 			break;
 		} else if (!ep->is_in &&
 				(req->req.length % ep->ep.maxpacket) != 0) {
-			tmp = readl(&ep->regs->ep_stat);
 			if (ep->dev->quirks & PLX_SUPERSPEED)
 				return dma_done(ep, req, tmp, 0);
 
+			tmp = readl(&ep->regs->ep_stat);
 			/* AVOID TROUBLE HERE by not issuing short reads from
 			 * your gadget driver.  That helps avoids errata 0121,
 			 * 0122, and 0124; not all cases trigger the warning.
@@ -1548,8 +1548,7 @@ static int net2280_pullup(struct usb_gadget *_gadget, int is_on)
 
 static int net2280_start(struct usb_gadget *_gadget,
 		struct usb_gadget_driver *driver);
-static int net2280_stop(struct usb_gadget *_gadget,
-		struct usb_gadget_driver *driver);
+static int net2280_stop(struct usb_gadget *_gadget);
 
 static const struct usb_gadget_ops net2280_ops = {
 	.get_frame	= net2280_get_frame,
@@ -2397,11 +2396,6 @@ static int net2280_start(struct usb_gadget *_gadget,
 
 	ep0_start(dev);
 
-	ep_dbg(dev, "%s ready, usbctl %08x stdrsp %08x\n",
-			driver->driver.name,
-			readl(&dev->usb->usbctl),
-			readl(&dev->usb->stdrsp));
-
 	/* pci writes may still be posted */
 	return 0;
 
@@ -2437,8 +2431,7 @@ static void stop_activity(struct net2280 *dev, struct usb_gadget_driver *driver)
 	usb_reinit(dev);
 }
 
-static int net2280_stop(struct usb_gadget *_gadget,
-		struct usb_gadget_driver *driver)
+static int net2280_stop(struct usb_gadget *_gadget)
 {
 	struct net2280	*dev;
 	unsigned long	flags;
@@ -2446,11 +2439,9 @@ static int net2280_stop(struct usb_gadget *_gadget,
 	dev = container_of(_gadget, struct net2280, gadget);
 
 	spin_lock_irqsave(&dev->lock, flags);
-	stop_activity(dev, driver);
+	stop_activity(dev, NULL);
 	spin_unlock_irqrestore(&dev->lock, flags);
 
-	dev->driver = NULL;
-
 	net2280_led_active(dev, 0);
 
 	/* Disable full-speed test mode */
@@ -2460,8 +2451,7 @@ static int net2280_stop(struct usb_gadget *_gadget,
 	device_remove_file(&dev->pdev->dev, &dev_attr_function);
 	device_remove_file(&dev->pdev->dev, &dev_attr_queues);
 
-	ep_dbg(dev, "unregistered driver '%s'\n",
-			driver ? driver->driver.name : "");
+	dev->driver = NULL;
 
 	return 0;
 }
@@ -3318,17 +3308,42 @@ static void handle_stat1_irqs(struct net2280 *dev, u32 stat)
 	 * only indicates a change in the reset state).
 	 */
 	if (stat & tmp) {
+		bool	reset = false;
+		bool	disconnect = false;
+
+		/*
+		 * Ignore disconnects and resets if the speed hasn't been set.
+		 * VBUS can bounce and there's always an initial reset.
+		 */
 		writel(tmp, &dev->regs->irqstat1);
-		if ((((stat & BIT(ROOT_PORT_RESET_INTERRUPT)) &&
-				((readl(&dev->usb->usbstat) & mask) == 0)) ||
-				((readl(&dev->usb->usbctl) &
-					BIT(VBUS_PIN)) == 0)) &&
-				(dev->gadget.speed != USB_SPEED_UNKNOWN)) {
-			ep_dbg(dev, "disconnect %s\n",
-					dev->driver->driver.name);
-			stop_activity(dev, dev->driver);
-			ep0_start(dev);
-			return;
+		if (dev->gadget.speed != USB_SPEED_UNKNOWN) {
+			if ((stat & BIT(VBUS_INTERRUPT)) &&
+					(readl(&dev->usb->usbctl) &
+						BIT(VBUS_PIN)) == 0) {
+				disconnect = true;
+				ep_dbg(dev, "disconnect %s\n",
+						dev->driver->driver.name);
+			} else if ((stat & BIT(ROOT_PORT_RESET_INTERRUPT)) &&
+					(readl(&dev->usb->usbstat) & mask)
+						== 0) {
+				reset = true;
+				ep_dbg(dev, "reset %s\n",
+						dev->driver->driver.name);
+			}
+
+			if (disconnect || reset) {
+				stop_activity(dev, dev->driver);
+				ep0_start(dev);
+				spin_unlock(&dev->lock);
+				if (reset)
+					usb_gadget_udc_reset
+						(&dev->gadget, dev->driver);
+				else
+					(dev->driver->disconnect)
+						(&dev->gadget);
+				spin_lock(&dev->lock);
+				return;
+			}
 		}
 		stat &= ~tmp;
 
diff --git a/drivers/usb/gadget/udc/omap_udc.c b/drivers/usb/gadget/udc/omap_udc.c
index dcdfea46003b..288c087220a8 100644
--- a/drivers/usb/gadget/udc/omap_udc.c
+++ b/drivers/usb/gadget/udc/omap_udc.c
@@ -1311,8 +1311,7 @@ static int omap_pullup(struct usb_gadget *gadget, int is_on)
 
 static int omap_udc_start(struct usb_gadget *g,
 		struct usb_gadget_driver *driver);
-static int omap_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver);
+static int omap_udc_stop(struct usb_gadget *g);
 
 static const struct usb_gadget_ops omap_gadget_ops = {
 	.get_frame		= omap_get_frame,
@@ -2102,8 +2101,7 @@ done:
 	return status;
 }
 
-static int omap_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver)
+static int omap_udc_stop(struct usb_gadget *g)
 {
 	unsigned long	flags;
 	int		status = -ENODEV;
@@ -3026,7 +3024,6 @@ static struct platform_driver udc_driver = {
 	.suspend	= omap_udc_suspend,
 	.resume		= omap_udc_resume,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= (char *) driver_name,
 	},
 };
diff --git a/drivers/usb/gadget/udc/pch_udc.c b/drivers/usb/gadget/udc/pch_udc.c
index ccbe3d4a2a50..1c7379ac2379 100644
--- a/drivers/usb/gadget/udc/pch_udc.c
+++ b/drivers/usb/gadget/udc/pch_udc.c
@@ -1240,8 +1240,8 @@ static int pch_udc_pcd_vbus_draw(struct usb_gadget *gadget, unsigned int mA)
 
 static int pch_udc_start(struct usb_gadget *g,
 		struct usb_gadget_driver *driver);
-static int pch_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver);
+static int pch_udc_stop(struct usb_gadget *g);
+
 static const struct usb_gadget_ops pch_udc_ops = {
 	.get_frame = pch_udc_pcd_get_frame,
 	.wakeup = pch_udc_pcd_wakeup,
@@ -2592,9 +2592,9 @@ static void pch_udc_svc_ur_interrupt(struct pch_udc_dev *dev)
 		/* Complete request queue */
 		empty_req_queue(ep);
 	}
-	if (dev->driver && dev->driver->disconnect) {
+	if (dev->driver) {
 		spin_unlock(&dev->lock);
-		dev->driver->disconnect(&dev->gadget);
+		usb_gadget_udc_reset(&dev->gadget, dev->driver);
 		spin_lock(&dev->lock);
 	}
 }
@@ -3008,8 +3008,7 @@ static int pch_udc_start(struct usb_gadget *g,
 	return 0;
 }
 
-static int pch_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver)
+static int pch_udc_stop(struct usb_gadget *g)
 {
 	struct pch_udc_dev	*dev = to_pch_udc(g);
 
diff --git a/drivers/usb/gadget/udc/pxa25x_udc.c b/drivers/usb/gadget/udc/pxa25x_udc.c
index 42f7eeb8ff6f..8550b2d5db32 100644
--- a/drivers/usb/gadget/udc/pxa25x_udc.c
+++ b/drivers/usb/gadget/udc/pxa25x_udc.c
@@ -998,8 +998,7 @@ static int pxa25x_udc_vbus_draw(struct usb_gadget *_gadget, unsigned mA)
 
 static int pxa25x_udc_start(struct usb_gadget *g,
 		struct usb_gadget_driver *driver);
-static int pxa25x_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver);
+static int pxa25x_udc_stop(struct usb_gadget *g);
 
 static const struct usb_gadget_ops pxa25x_udc_ops = {
 	.get_frame	= pxa25x_udc_get_frame,
@@ -1135,11 +1134,7 @@ static const struct file_operations debug_fops = {
 		dev->debugfs_udc = debugfs_create_file(dev->gadget.name, \
 			S_IRUGO, NULL, dev, &debug_fops); \
 	} while (0)
-#define remove_debug_files(dev) \
-	do { \
-		if (dev->debugfs_udc) \
-			debugfs_remove(dev->debugfs_udc); \
-	} while (0)
+#define remove_debug_files(dev) debugfs_remove(dev->debugfs_udc)
 
 #else	/* !CONFIG_USB_GADGET_DEBUG_FILES */
 
@@ -1285,6 +1280,33 @@ bind_fail:
 }
 
 static void
+reset_gadget(struct pxa25x_udc *dev, struct usb_gadget_driver *driver)
+{
+	int i;
+
+	/* don't disconnect drivers more than once */
+	if (dev->gadget.speed == USB_SPEED_UNKNOWN)
+		driver = NULL;
+	dev->gadget.speed = USB_SPEED_UNKNOWN;
+
+	/* prevent new request submissions, kill any outstanding requests  */
+	for (i = 0; i < PXA_UDC_NUM_ENDPOINTS; i++) {
+		struct pxa25x_ep *ep = &dev->ep[i];
+
+		ep->stopped = 1;
+		nuke(ep, -ESHUTDOWN);
+	}
+	del_timer_sync(&dev->timer);
+
+	/* report reset; the driver is already quiesced */
+	if (driver)
+		usb_gadget_udc_reset(&dev->gadget, driver);
+
+	/* re-init driver-visible data structures */
+	udc_reinit(dev);
+}
+
+static void
 stop_activity(struct pxa25x_udc *dev, struct usb_gadget_driver *driver)
 {
 	int i;
@@ -1311,15 +1333,14 @@ stop_activity(struct pxa25x_udc *dev, struct usb_gadget_driver *driver)
 	udc_reinit(dev);
 }
 
-static int pxa25x_udc_stop(struct usb_gadget*g,
-		struct usb_gadget_driver *driver)
+static int pxa25x_udc_stop(struct usb_gadget*g)
 {
 	struct pxa25x_udc	*dev = to_pxa25x(g);
 
 	local_irq_disable();
 	dev->pullup = 0;
 	pullup(dev);
-	stop_activity(dev, driver);
+	stop_activity(dev, NULL);
 	local_irq_enable();
 
 	if (!IS_ERR_OR_NULL(dev->transceiver))
@@ -1723,7 +1744,7 @@ pxa25x_udc_irq(int irq, void *_dev)
 				/* reset driver and endpoints,
 				 * in case that's not yet done
 				 */
-				stop_activity (dev, dev->driver);
+				reset_gadget(dev, dev->driver);
 
 			} else {
 				DBG(DBG_VERBOSE, "USB reset end\n");
@@ -2271,7 +2292,6 @@ static struct platform_driver udc_driver = {
 	.suspend	= pxa25x_udc_suspend,
 	.resume		= pxa25x_udc_resume,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "pxa25x-udc",
 	},
 };
diff --git a/drivers/usb/gadget/udc/pxa27x_udc.c b/drivers/usb/gadget/udc/pxa27x_udc.c
index 4868369eeec6..c61a896061fa 100644
--- a/drivers/usb/gadget/udc/pxa27x_udc.c
+++ b/drivers/usb/gadget/udc/pxa27x_udc.c
@@ -22,10 +22,13 @@
 #include <linux/clk.h>
 #include <linux/irq.h>
 #include <linux/gpio.h>
+#include <linux/gpio/consumer.h>
 #include <linux/slab.h>
 #include <linux/prefetch.h>
 #include <linux/byteorder/generic.h>
 #include <linux/platform_data/pxa2xx_udc.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
 
 #include <linux/usb.h>
 #include <linux/usb/ch9.h>
@@ -1507,18 +1510,13 @@ static struct usb_ep_ops pxa_ep_ops = {
  */
 static void dplus_pullup(struct pxa_udc *udc, int on)
 {
-	if (on) {
-		if (gpio_is_valid(udc->mach->gpio_pullup))
-			gpio_set_value(udc->mach->gpio_pullup,
-				       !udc->mach->gpio_pullup_inverted);
-		if (udc->mach->udc_command)
-			udc->mach->udc_command(PXA2XX_UDC_CMD_CONNECT);
-	} else {
-		if (gpio_is_valid(udc->mach->gpio_pullup))
-			gpio_set_value(udc->mach->gpio_pullup,
-				       udc->mach->gpio_pullup_inverted);
-		if (udc->mach->udc_command)
-			udc->mach->udc_command(PXA2XX_UDC_CMD_DISCONNECT);
+	if (udc->gpiod) {
+		gpiod_set_value(udc->gpiod, on);
+	} else if (udc->udc_command) {
+		if (on)
+			udc->udc_command(PXA2XX_UDC_CMD_CONNECT);
+		else
+			udc->udc_command(PXA2XX_UDC_CMD_DISCONNECT);
 	}
 	udc->pullup_on = on;
 }
@@ -1609,7 +1607,7 @@ static int pxa_udc_pullup(struct usb_gadget *_gadget, int is_active)
 {
 	struct pxa_udc *udc = to_gadget_udc(_gadget);
 
-	if (!gpio_is_valid(udc->mach->gpio_pullup) && !udc->mach->udc_command)
+	if (!udc->gpiod && !udc->udc_command)
 		return -EOPNOTSUPP;
 
 	dplus_pullup(udc, is_active);
@@ -1671,8 +1669,7 @@ static int pxa_udc_vbus_draw(struct usb_gadget *_gadget, unsigned mA)
 
 static int pxa27x_udc_start(struct usb_gadget *g,
 		struct usb_gadget_driver *driver);
-static int pxa27x_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver);
+static int pxa27x_udc_stop(struct usb_gadget *g);
 
 static const struct usb_gadget_ops pxa_udc_ops = {
 	.get_frame	= pxa_udc_get_frame,
@@ -1701,10 +1698,10 @@ static void udc_disable(struct pxa_udc *udc)
 	udc_writel(udc, UDCICR1, 0);
 
 	udc_clear_mask_UDCCR(udc, UDCCR_UDE);
-	clk_disable(udc->clk);
 
 	ep0_idle(udc);
 	udc->gadget.speed = USB_SPEED_UNKNOWN;
+	clk_disable(udc->clk);
 
 	udc->enabled = 0;
 }
@@ -1757,16 +1754,16 @@ static void udc_enable(struct pxa_udc *udc)
 	if (udc->enabled)
 		return;
 
+	clk_enable(udc->clk);
 	udc_writel(udc, UDCICR0, 0);
 	udc_writel(udc, UDCICR1, 0);
 	udc_clear_mask_UDCCR(udc, UDCCR_UDE);
 
-	clk_enable(udc->clk);
-
 	ep0_idle(udc);
 	udc->gadget.speed = USB_SPEED_FULL;
 	memset(&udc->stats, 0, sizeof(udc->stats));
 
+	pxa_eps_setup(udc);
 	udc_set_mask_UDCCR(udc, UDCCR_UDE);
 	ep_write_UDCCSR(&udc->pxa_ep[0], UDCCSR0_ACM);
 	udelay(2);
@@ -1859,12 +1856,11 @@ static void stop_activity(struct pxa_udc *udc, struct usb_gadget_driver *driver)
  *
  * Returns 0 if no error, -ENODEV, -EINVAL otherwise
  */
-static int pxa27x_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver)
+static int pxa27x_udc_stop(struct usb_gadget *g)
 {
 	struct pxa_udc *udc = to_pxa(g);
 
-	stop_activity(udc, driver);
+	stop_activity(udc, NULL);
 	udc_disable(udc);
 	dplus_pullup(udc, 0);
 
@@ -2404,6 +2400,14 @@ static struct pxa_udc memory = {
 	}
 };
 
+#if defined(CONFIG_OF)
+static struct of_device_id udc_pxa_dt_ids[] = {
+	{ .compatible = "marvell,pxa270-udc" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, udc_pxa_dt_ids);
+#endif
+
 /**
  * pxa_udc_probe - probes the udc device
  * @_dev: platform device
@@ -2416,81 +2420,77 @@ static int pxa_udc_probe(struct platform_device *pdev)
 	struct resource *regs;
 	struct pxa_udc *udc = &memory;
 	int retval = 0, gpio;
+	struct pxa2xx_udc_mach_info *mach = dev_get_platdata(&pdev->dev);
+	unsigned long gpio_flags;
+
+	if (mach) {
+		gpio_flags = mach->gpio_pullup_inverted ? GPIOF_ACTIVE_LOW : 0;
+		gpio = mach->gpio_pullup;
+		if (gpio_is_valid(gpio)) {
+			retval = devm_gpio_request_one(&pdev->dev, gpio,
+						       gpio_flags,
+						       "USB D+ pullup");
+			if (retval)
+				return retval;
+			udc->gpiod = gpio_to_desc(mach->gpio_pullup);
+		}
+		udc->udc_command = mach->udc_command;
+	} else {
+		udc->gpiod = devm_gpiod_get(&pdev->dev, NULL);
+	}
 
 	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!regs)
-		return -ENXIO;
+	udc->regs = devm_ioremap_resource(&pdev->dev, regs);
+	if (IS_ERR(udc->regs))
+		return PTR_ERR(udc->regs);
 	udc->irq = platform_get_irq(pdev, 0);
 	if (udc->irq < 0)
 		return udc->irq;
 
 	udc->dev = &pdev->dev;
-	udc->mach = dev_get_platdata(&pdev->dev);
 	udc->transceiver = usb_get_phy(USB_PHY_TYPE_USB2);
 
-	gpio = udc->mach->gpio_pullup;
-	if (gpio_is_valid(gpio)) {
-		retval = gpio_request(gpio, "USB D+ pullup");
-		if (retval == 0)
-			gpio_direction_output(gpio,
-				       udc->mach->gpio_pullup_inverted);
-	}
-	if (retval) {
-		dev_err(&pdev->dev, "Couldn't request gpio %d : %d\n",
-			gpio, retval);
-		return retval;
+	if (IS_ERR(udc->gpiod)) {
+		dev_err(&pdev->dev, "Couldn't find or request D+ gpio : %ld\n",
+			PTR_ERR(udc->gpiod));
+		return PTR_ERR(udc->gpiod);
 	}
+	if (udc->gpiod)
+		gpiod_direction_output(udc->gpiod, 0);
+
+	udc->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(udc->clk))
+		return PTR_ERR(udc->clk);
 
-	udc->clk = clk_get(&pdev->dev, NULL);
-	if (IS_ERR(udc->clk)) {
-		retval = PTR_ERR(udc->clk);
-		goto err_clk;
-	}
 	retval = clk_prepare(udc->clk);
 	if (retval)
-		goto err_clk_prepare;
-
-	retval = -ENOMEM;
-	udc->regs = ioremap(regs->start, resource_size(regs));
-	if (!udc->regs) {
-		dev_err(&pdev->dev, "Unable to map UDC I/O memory\n");
-		goto err_map;
-	}
+		return retval;
 
 	udc->vbus_sensed = 0;
 
 	the_controller = udc;
 	platform_set_drvdata(pdev, udc);
 	udc_init_data(udc);
-	pxa_eps_setup(udc);
 
 	/* irq setup after old hardware state is cleaned up */
-	retval = request_irq(udc->irq, pxa_udc_irq,
-			IRQF_SHARED, driver_name, udc);
+	retval = devm_request_irq(&pdev->dev, udc->irq, pxa_udc_irq,
+				  IRQF_SHARED, driver_name, udc);
 	if (retval != 0) {
 		dev_err(udc->dev, "%s: can't get irq %i, err %d\n",
 			driver_name, udc->irq, retval);
-		goto err_irq;
+		goto err;
 	}
 
 	retval = usb_add_gadget_udc(&pdev->dev, &udc->gadget);
 	if (retval)
-		goto err_add_udc;
+		goto err;
 
 	pxa_init_debugfs(udc);
-
+	if (should_enable_udc(udc))
+		udc_enable(udc);
 	return 0;
-
-err_add_udc:
-	free_irq(udc->irq, udc);
-err_irq:
-	iounmap(udc->regs);
-err_map:
+err:
 	clk_unprepare(udc->clk);
-err_clk_prepare:
-	clk_put(udc->clk);
-	udc->clk = NULL;
-err_clk:
 	return retval;
 }
 
@@ -2501,22 +2501,15 @@ err_clk:
 static int pxa_udc_remove(struct platform_device *_dev)
 {
 	struct pxa_udc *udc = platform_get_drvdata(_dev);
-	int gpio = udc->mach->gpio_pullup;
 
 	usb_del_gadget_udc(&udc->gadget);
-	usb_gadget_unregister_driver(udc->driver);
-	free_irq(udc->irq, udc);
 	pxa_cleanup_debugfs(udc);
-	if (gpio_is_valid(gpio))
-		gpio_free(gpio);
 
 	usb_put_phy(udc->transceiver);
 
 	udc->transceiver = NULL;
 	the_controller = NULL;
 	clk_unprepare(udc->clk);
-	clk_put(udc->clk);
-	iounmap(udc->regs);
 
 	return 0;
 }
@@ -2546,19 +2539,11 @@ extern void pxa27x_clear_otgph(void);
  */
 static int pxa_udc_suspend(struct platform_device *_dev, pm_message_t state)
 {
-	int i;
 	struct pxa_udc *udc = platform_get_drvdata(_dev);
 	struct pxa_ep *ep;
 
 	ep = &udc->pxa_ep[0];
 	udc->udccsr0 = udc_ep_readl(ep, UDCCSR);
-	for (i = 1; i < NR_PXA_ENDPOINTS; i++) {
-		ep = &udc->pxa_ep[i];
-		ep->udccsr_value = udc_ep_readl(ep, UDCCSR);
-		ep->udccr_value  = udc_ep_readl(ep, UDCCR);
-		ep_dbg(ep, "udccsr:0x%03x, udccr:0x%x\n",
-				ep->udccsr_value, ep->udccr_value);
-	}
 
 	udc_disable(udc);
 	udc->pullup_resume = udc->pullup_on;
@@ -2576,19 +2561,11 @@ static int pxa_udc_suspend(struct platform_device *_dev, pm_message_t state)
  */
 static int pxa_udc_resume(struct platform_device *_dev)
 {
-	int i;
 	struct pxa_udc *udc = platform_get_drvdata(_dev);
 	struct pxa_ep *ep;
 
 	ep = &udc->pxa_ep[0];
 	udc_ep_writel(ep, UDCCSR, udc->udccsr0 & (UDCCSR0_FST | UDCCSR0_DME));
-	for (i = 1; i < NR_PXA_ENDPOINTS; i++) {
-		ep = &udc->pxa_ep[i];
-		udc_ep_writel(ep, UDCCSR, ep->udccsr_value);
-		udc_ep_writel(ep, UDCCR,  ep->udccr_value);
-		ep_dbg(ep, "udccsr:0x%03x, udccr:0x%x\n",
-				ep->udccsr_value, ep->udccr_value);
-	}
 
 	dplus_pullup(udc, udc->pullup_resume);
 	if (should_enable_udc(udc))
@@ -2614,7 +2591,7 @@ MODULE_ALIAS("platform:pxa27x-udc");
 static struct platform_driver udc_driver = {
 	.driver		= {
 		.name	= "pxa27x-udc",
-		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(udc_pxa_dt_ids),
 	},
 	.probe		= pxa_udc_probe,
 	.remove		= pxa_udc_remove,
diff --git a/drivers/usb/gadget/udc/pxa27x_udc.h b/drivers/usb/gadget/udc/pxa27x_udc.h
index 28f2b53530f5..11e14232794b 100644
--- a/drivers/usb/gadget/udc/pxa27x_udc.h
+++ b/drivers/usb/gadget/udc/pxa27x_udc.h
@@ -420,7 +420,8 @@ struct udc_stats {
  * @usb_gadget: udc gadget structure
  * @driver: bound gadget (zero, g_ether, g_mass_storage, ...)
  * @dev: device
- * @mach: machine info, used to activate specific GPIO
+ * @udc_command: machine specific function to activate D+ pullup
+ * @gpiod: gpio descriptor of gpio for D+ pullup (or NULL if none)
  * @transceiver: external transceiver to handle vbus sense and D+ pullup
  * @ep0state: control endpoint state machine state
  * @stats: statistics on udc usage
@@ -446,7 +447,8 @@ struct pxa_udc {
 	struct usb_gadget			gadget;
 	struct usb_gadget_driver		*driver;
 	struct device				*dev;
-	struct pxa2xx_udc_mach_info		*mach;
+	void					(*udc_command)(int);
+	struct gpio_desc			*gpiod;
 	struct usb_phy				*transceiver;
 
 	enum ep0_state				ep0state;
diff --git a/drivers/usb/gadget/udc/r8a66597-udc.c b/drivers/usb/gadget/udc/r8a66597-udc.c
index f8186613b53e..06870da0b988 100644
--- a/drivers/usb/gadget/udc/r8a66597-udc.c
+++ b/drivers/usb/gadget/udc/r8a66597-udc.c
@@ -1345,7 +1345,7 @@ static void irq_device_state(struct r8a66597 *r8a66597)
 	if (dvsq == DS_DFLT) {
 		/* bus reset */
 		spin_unlock(&r8a66597->lock);
-		r8a66597->driver->disconnect(&r8a66597->gadget);
+		usb_gadget_udc_reset(&r8a66597->gadget, r8a66597->driver);
 		spin_lock(&r8a66597->lock);
 		r8a66597_update_usb_speed(r8a66597);
 	}
@@ -1763,8 +1763,7 @@ static int r8a66597_start(struct usb_gadget *gadget,
 	return 0;
 }
 
-static int r8a66597_stop(struct usb_gadget *gadget,
-		struct usb_gadget_driver *driver)
+static int r8a66597_stop(struct usb_gadget *gadget)
 {
 	struct r8a66597 *r8a66597 = gadget_to_r8a66597(gadget);
 	unsigned long flags;
@@ -1846,10 +1845,7 @@ static int r8a66597_sudmac_ioremap(struct r8a66597 *r8a66597,
 
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "sudmac");
 	r8a66597->sudmac_reg = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(r8a66597->sudmac_reg))
-		return PTR_ERR(r8a66597->sudmac_reg);
-
-	return 0;
+	return PTR_ERR_OR_ZERO(r8a66597->sudmac_reg);
 }
 
 static int r8a66597_probe(struct platform_device *pdev)
diff --git a/drivers/usb/gadget/udc/s3c-hsudc.c b/drivers/usb/gadget/udc/s3c-hsudc.c
index dfbf55797360..85a712a03343 100644
--- a/drivers/usb/gadget/udc/s3c-hsudc.c
+++ b/drivers/usb/gadget/udc/s3c-hsudc.c
@@ -1172,8 +1172,6 @@ static int s3c_hsudc_start(struct usb_gadget *gadget,
 	}
 
 	enable_irq(hsudc->irq);
-	dev_info(hsudc->dev, "bound driver %s\n", driver->driver.name);
-
 	s3c_hsudc_reconfig(hsudc);
 
 	pm_runtime_get_sync(hsudc->dev);
@@ -1190,8 +1188,7 @@ err_supplies:
 	return ret;
 }
 
-static int s3c_hsudc_stop(struct usb_gadget *gadget,
-		struct usb_gadget_driver *driver)
+static int s3c_hsudc_stop(struct usb_gadget *gadget)
 {
 	struct s3c_hsudc *hsudc = to_hsudc(gadget);
 	unsigned long flags;
@@ -1199,11 +1196,7 @@ static int s3c_hsudc_stop(struct usb_gadget *gadget,
 	if (!hsudc)
 		return -ENODEV;
 
-	if (!driver || driver != hsudc->driver)
-		return -EINVAL;
-
 	spin_lock_irqsave(&hsudc->lock, flags);
-	hsudc->driver = NULL;
 	hsudc->gadget.speed = USB_SPEED_UNKNOWN;
 	s3c_hsudc_uninit_phy();
 
@@ -1220,9 +1213,8 @@ static int s3c_hsudc_stop(struct usb_gadget *gadget,
 	disable_irq(hsudc->irq);
 
 	regulator_bulk_disable(ARRAY_SIZE(hsudc->supplies), hsudc->supplies);
+	hsudc->driver = NULL;
 
-	dev_info(hsudc->dev, "unregistered gadget driver '%s'\n",
-			driver->driver.name);
 	return 0;
 }
 
@@ -1267,10 +1259,8 @@ static int s3c_hsudc_probe(struct platform_device *pdev)
 	hsudc = devm_kzalloc(&pdev->dev, sizeof(struct s3c_hsudc) +
 			sizeof(struct s3c_hsudc_ep) * pd->epnum,
 			GFP_KERNEL);
-	if (!hsudc) {
-		dev_err(dev, "cannot allocate memory\n");
+	if (!hsudc)
 		return -ENOMEM;
-	}
 
 	platform_set_drvdata(pdev, dev);
 	hsudc->dev = dev;
@@ -1354,7 +1344,6 @@ err_supplies:
 
 static struct platform_driver s3c_hsudc_driver = {
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "s3c-hsudc",
 	},
 	.probe		= s3c_hsudc_probe,
diff --git a/drivers/usb/gadget/udc/s3c2410_udc.c b/drivers/usb/gadget/udc/s3c2410_udc.c
index ff423d15beff..824cf12e9add 100644
--- a/drivers/usb/gadget/udc/s3c2410_udc.c
+++ b/drivers/usb/gadget/udc/s3c2410_udc.c
@@ -1541,8 +1541,7 @@ static int s3c2410_vbus_draw(struct usb_gadget *_gadget, unsigned ma)
 
 static int s3c2410_udc_start(struct usb_gadget *g,
 		struct usb_gadget_driver *driver);
-static int s3c2410_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver);
+static int s3c2410_udc_stop(struct usb_gadget *g);
 
 static const struct usb_gadget_ops s3c2410_ops = {
 	.get_frame		= s3c2410_udc_get_frame,
@@ -1683,8 +1682,7 @@ static int s3c2410_udc_start(struct usb_gadget *g,
 	return 0;
 }
 
-static int s3c2410_udc_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver)
+static int s3c2410_udc_stop(struct usb_gadget *g)
 {
 	struct s3c2410_udc *udc = to_s3c2410(g);
 
@@ -1997,7 +1995,6 @@ MODULE_DEVICE_TABLE(platform, s3c_udc_ids);
 static struct platform_driver udc_driver_24x0 = {
 	.driver		= {
 		.name	= "s3c24x0-usbgadget",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= s3c2410_udc_probe,
 	.remove		= s3c2410_udc_remove,
diff --git a/drivers/usb/gadget/udc/udc-core.c b/drivers/usb/gadget/udc/udc-core.c
index f2054659f25b..e31d574d8860 100644
--- a/drivers/usb/gadget/udc/udc-core.c
+++ b/drivers/usb/gadget/udc/udc-core.c
@@ -174,8 +174,7 @@ EXPORT_SYMBOL_GPL(usb_gadget_udc_reset);
 
 /**
  * usb_gadget_udc_start - tells usb device controller to start up
- * @gadget: The gadget we want to get started
- * @driver: The driver we want to bind to @gadget
+ * @udc: The UDC to be started
  *
  * This call is issued by the UDC Class driver when it's about
  * to register a gadget driver to the device controller, before
@@ -186,10 +185,9 @@ EXPORT_SYMBOL_GPL(usb_gadget_udc_reset);
  *
  * Returns zero on success, else negative errno.
  */
-static inline int usb_gadget_udc_start(struct usb_gadget *gadget,
-		struct usb_gadget_driver *driver)
+static inline int usb_gadget_udc_start(struct usb_udc *udc)
 {
-	return gadget->ops->udc_start(gadget, driver);
+	return udc->gadget->ops->udc_start(udc->gadget, udc->driver);
 }
 
 /**
@@ -204,10 +202,9 @@ static inline int usb_gadget_udc_start(struct usb_gadget *gadget,
  * far as powering off UDC completely and disable its data
  * line pullups.
  */
-static inline void usb_gadget_udc_stop(struct usb_gadget *gadget,
-		struct usb_gadget_driver *driver)
+static inline void usb_gadget_udc_stop(struct usb_udc *udc)
 {
-	gadget->ops->udc_stop(gadget, driver);
+	udc->gadget->ops->udc_stop(udc->gadget);
 }
 
 /**
@@ -328,14 +325,14 @@ EXPORT_SYMBOL_GPL(usb_add_gadget_udc);
 static void usb_gadget_remove_driver(struct usb_udc *udc)
 {
 	dev_dbg(&udc->dev, "unregistering UDC driver [%s]\n",
-			udc->gadget->name);
+			udc->driver->function);
 
 	kobject_uevent(&udc->dev.kobj, KOBJ_CHANGE);
 
 	usb_gadget_disconnect(udc->gadget);
 	udc->driver->disconnect(udc->gadget);
 	udc->driver->unbind(udc->gadget);
-	usb_gadget_udc_stop(udc->gadget, NULL);
+	usb_gadget_udc_stop(udc);
 
 	udc->driver = NULL;
 	udc->dev.driver = NULL;
@@ -395,7 +392,7 @@ static int udc_bind_to_driver(struct usb_udc *udc, struct usb_gadget_driver *dri
 	ret = driver->bind(udc->gadget, driver);
 	if (ret)
 		goto err1;
-	ret = usb_gadget_udc_start(udc->gadget, driver);
+	ret = usb_gadget_udc_start(udc);
 	if (ret) {
 		driver->unbind(udc->gadget);
 		goto err1;
@@ -414,7 +411,7 @@ err1:
 	return ret;
 }
 
-int udc_attach_driver(const char *name, struct usb_gadget_driver *driver)
+int usb_udc_attach_driver(const char *name, struct usb_gadget_driver *driver)
 {
 	struct usb_udc *udc = NULL;
 	int ret = -ENODEV;
@@ -438,7 +435,7 @@ out:
 	mutex_unlock(&udc_lock);
 	return ret;
 }
-EXPORT_SYMBOL_GPL(udc_attach_driver);
+EXPORT_SYMBOL_GPL(usb_udc_attach_driver);
 
 int usb_gadget_probe_driver(struct usb_gadget_driver *driver)
 {
@@ -513,11 +510,12 @@ static ssize_t usb_udc_softconn_store(struct device *dev,
 	}
 
 	if (sysfs_streq(buf, "connect")) {
-		usb_gadget_udc_start(udc->gadget, udc->driver);
+		usb_gadget_udc_start(udc);
 		usb_gadget_connect(udc->gadget);
 	} else if (sysfs_streq(buf, "disconnect")) {
 		usb_gadget_disconnect(udc->gadget);
-		usb_gadget_udc_stop(udc->gadget, udc->driver);
+		udc->driver->disconnect(udc->gadget);
+		usb_gadget_udc_stop(udc);
 	} else {
 		dev_err(dev, "unsupported command '%s'\n", buf);
 		return -EINVAL;
diff --git a/drivers/usb/gadget/udc/udc-xilinx.c b/drivers/usb/gadget/udc/udc-xilinx.c
index ed27e1687a4e..1eac56fc384d 100644
--- a/drivers/usb/gadget/udc/udc-xilinx.c
+++ b/drivers/usb/gadget/udc/udc-xilinx.c
@@ -1403,8 +1403,7 @@ err:
  *
  * Return: zero always
  */
-static int xudc_stop(struct usb_gadget *gadget,
-		     struct usb_gadget_driver *driver)
+static int xudc_stop(struct usb_gadget *gadget)
 {
 	struct xusb_udc *udc = to_udc(gadget);
 	unsigned long flags;
diff --git a/drivers/usb/host/Kconfig b/drivers/usb/host/Kconfig
index a3ca1375dd52..fafc628480e0 100644
--- a/drivers/usb/host/Kconfig
+++ b/drivers/usb/host/Kconfig
@@ -292,11 +292,15 @@ config USB_EHCI_HCD_PLATFORM
 	  If unsure, say N.
 
 config USB_OCTEON_EHCI
-	bool "Octeon on-chip EHCI support"
+	bool "Octeon on-chip EHCI support (DEPRECATED)"
 	depends on CAVIUM_OCTEON_SOC
 	default n
 	select USB_EHCI_BIG_ENDIAN_MMIO
+	select USB_EHCI_HCD_PLATFORM
 	help
+	  This option is deprecated now and the driver was removed, use
+	  USB_EHCI_HCD_PLATFORM instead.
+
 	  Enable support for the Octeon II SOC's on-chip EHCI
 	  controller.  It is needed for high-speed (480Mbit/sec)
 	  USB 2.0 device support.  All CN6XXX based chips with USB are
@@ -575,12 +579,16 @@ config USB_OHCI_HCD_PLATFORM
 	  If unsure, say N.
 
 config USB_OCTEON_OHCI
-	bool "Octeon on-chip OHCI support"
+	bool "Octeon on-chip OHCI support (DEPRECATED)"
 	depends on CAVIUM_OCTEON_SOC
 	default USB_OCTEON_EHCI
 	select USB_OHCI_BIG_ENDIAN_MMIO
 	select USB_OHCI_LITTLE_ENDIAN
+	select USB_OHCI_HCD_PLATFORM
 	help
+	  This option is deprecated now and the driver was removed, use
+	  USB_OHCI_HCD_PLATFORM instead.
+
 	  Enable support for the Octeon II SOC's on-chip OHCI
 	  controller.  It is needed for low-speed USB 1.0 device
 	  support.  All CN6XXX based chips with USB are supported.
@@ -754,12 +762,6 @@ config USB_IMX21_HCD
          To compile this driver as a module, choose M here: the
          module will be called "imx21-hcd".
 
-
-
-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
diff --git a/drivers/usb/host/Makefile b/drivers/usb/host/Makefile
index 348c24321562..d6216a493bab 100644
--- a/drivers/usb/host/Makefile
+++ b/drivers/usb/host/Makefile
@@ -73,7 +73,6 @@ obj-$(CONFIG_USB_ISP1760_HCD)	+= isp1760.o
 obj-$(CONFIG_USB_HWA_HCD)	+= hwa-hc.o
 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_USB_HCD_BCMA)	+= bcma-hcd.o
 obj-$(CONFIG_USB_HCD_SSB)	+= ssb-hcd.o
 obj-$(CONFIG_USB_FUSBH200_HCD)	+= fusbh200-hcd.o
diff --git a/drivers/usb/host/ehci-atmel.c b/drivers/usb/host/ehci-atmel.c
index ec9f7b75d497..56a88506febe 100644
--- a/drivers/usb/host/ehci-atmel.c
+++ b/drivers/usb/host/ehci-atmel.c
@@ -107,22 +107,15 @@ static int ehci_atmel_drv_probe(struct platform_device *pdev)
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev,
-			"Found HC with no register addr. Check %s setup!\n",
-			dev_name(&pdev->dev));
-		retval = -ENODEV;
-		goto fail_request_resource;
-	}
-	hcd->rsrc_start = res->start;
-	hcd->rsrc_len = resource_size(res);
-
 	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(hcd->regs)) {
 		retval = PTR_ERR(hcd->regs);
 		goto fail_request_resource;
 	}
 
+	hcd->rsrc_start = res->start;
+	hcd->rsrc_len = resource_size(res);
+
 	iclk = devm_clk_get(&pdev->dev, "ehci_clk");
 	if (IS_ERR(iclk)) {
 		dev_err(&pdev->dev, "Error getting interface clock\n");
diff --git a/drivers/usb/host/ehci-exynos.c b/drivers/usb/host/ehci-exynos.c
index 7189f2e32ac2..df538fd10aa4 100644
--- a/drivers/usb/host/ehci-exynos.c
+++ b/drivers/usb/host/ehci-exynos.c
@@ -74,7 +74,6 @@ static int exynos_ehci_get_phy(struct device *dev,
 
 		phy = devm_of_phy_get(dev, child, NULL);
 		exynos_ehci->phy[phy_number] = phy;
-		of_node_put(child);
 		if (IS_ERR(phy)) {
 			ret = PTR_ERR(phy);
 			if (ret == -EPROBE_DEFER) {
@@ -188,20 +187,15 @@ skip_phy:
 		goto fail_clk;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "Failed to get I/O memory\n");
-		err = -ENXIO;
-		goto fail_io;
-	}
-
-	hcd->rsrc_start = res->start;
-	hcd->rsrc_len = resource_size(res);
 	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(hcd->regs)) {
 		err = PTR_ERR(hcd->regs);
 		goto fail_io;
 	}
 
+	hcd->rsrc_start = res->start;
+	hcd->rsrc_len = resource_size(res);
+
 	irq = platform_get_irq(pdev, 0);
 	if (!irq) {
 		dev_err(&pdev->dev, "Failed to get IRQ\n");
@@ -323,7 +317,6 @@ static struct platform_driver exynos_ehci_driver = {
 	.shutdown	= usb_hcd_platform_shutdown,
 	.driver = {
 		.name	= "exynos-ehci",
-		.owner	= THIS_MODULE,
 		.pm	= &exynos_ehci_pm_ops,
 		.of_match_table = of_match_ptr(exynos_ehci_match),
 	}
diff --git a/drivers/usb/host/ehci-fsl.c b/drivers/usb/host/ehci-fsl.c
index 2d2ae8db439e..fb7bd0c7dc15 100644
--- a/drivers/usb/host/ehci-fsl.c
+++ b/drivers/usb/host/ehci-fsl.c
@@ -93,21 +93,15 @@ static int usb_hcd_fsl_probe(const struct hc_driver *driver,
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev,
-			"Found HC with no register addr. Check %s setup!\n",
-			dev_name(&pdev->dev));
-		retval = -ENODEV;
-		goto err2;
-	}
-	hcd->rsrc_start = res->start;
-	hcd->rsrc_len = resource_size(res);
 	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(hcd->regs)) {
 		retval = PTR_ERR(hcd->regs);
 		goto err2;
 	}
 
+	hcd->rsrc_start = res->start;
+	hcd->rsrc_len = resource_size(res);
+
 	pdata->regs = hcd->regs;
 
 	if (pdata->power_budget)
diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index 15feaf924b71..38bfeedae1d0 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -311,6 +311,7 @@ static void unlink_empty_async_suspended(struct ehci_hcd *ehci);
 static void ehci_work(struct ehci_hcd *ehci);
 static void start_unlink_intr(struct ehci_hcd *ehci, struct ehci_qh *qh);
 static void end_unlink_intr(struct ehci_hcd *ehci, struct ehci_qh *qh);
+static int ehci_port_power(struct ehci_hcd *ehci, int portnum, bool enable);
 
 #include "ehci-timer.c"
 #include "ehci-hub.c"
@@ -329,9 +330,13 @@ static void ehci_turn_off_all_ports(struct ehci_hcd *ehci)
 {
 	int	port = HCS_N_PORTS(ehci->hcs_params);
 
-	while (port--)
+	while (port--) {
 		ehci_writel(ehci, PORT_RWC_BITS,
 				&ehci->regs->port_status[port]);
+		spin_unlock_irq(&ehci->lock);
+		ehci_port_power(ehci, port, false);
+		spin_lock_irq(&ehci->lock);
+	}
 }
 
 /*
@@ -1233,6 +1238,8 @@ void ehci_init_driver(struct hc_driver *drv,
 		drv->hcd_priv_size += over->extra_priv_size;
 		if (over->reset)
 			drv->reset = over->reset;
+		if (over->port_power)
+			drv->port_power = over->port_power;
 	}
 }
 EXPORT_SYMBOL_GPL(ehci_init_driver);
@@ -1268,11 +1275,6 @@ MODULE_LICENSE ("GPL");
 #define XILINX_OF_PLATFORM_DRIVER	ehci_hcd_xilinx_of_driver
 #endif
 
-#ifdef CONFIG_USB_OCTEON_EHCI
-#include "ehci-octeon.c"
-#define PLATFORM_DRIVER		ehci_octeon_driver
-#endif
-
 #ifdef CONFIG_TILE_USB
 #include "ehci-tilegx.c"
 #define	PLATFORM_DRIVER		ehci_hcd_tilegx_driver
diff --git a/drivers/usb/host/ehci-hub.c b/drivers/usb/host/ehci-hub.c
index 5728829cf6ef..118edb7bdca2 100644
--- a/drivers/usb/host/ehci-hub.c
+++ b/drivers/usb/host/ehci-hub.c
@@ -69,10 +69,8 @@ static void ehci_handover_companion_ports(struct ehci_hcd *ehci)
 		if (test_bit(port, &ehci->owned_ports)) {
 			reg = &ehci->regs->port_status[port];
 			status = ehci_readl(ehci, reg) & ~PORT_RWC_BITS;
-			if (!(status & PORT_POWER)) {
-				status |= PORT_POWER;
-				ehci_writel(ehci, status, reg);
-			}
+			if (!(status & PORT_POWER))
+				ehci_port_power(ehci, port, true);
 		}
 	}
 
@@ -952,9 +950,11 @@ int ehci_hub_control(
 			clear_bit(wIndex, &ehci->port_c_suspend);
 			break;
 		case USB_PORT_FEAT_POWER:
-			if (HCS_PPC (ehci->hcs_params))
-				ehci_writel(ehci, temp & ~PORT_POWER,
-						status_reg);
+			if (HCS_PPC(ehci->hcs_params)) {
+				spin_unlock_irqrestore(&ehci->lock, flags);
+				ehci_port_power(ehci, wIndex, false);
+				spin_lock_irqsave(&ehci->lock, flags);
+			}
 			break;
 		case USB_PORT_FEAT_C_CONNECTION:
 			ehci_writel(ehci, temp | PORT_CSC, status_reg);
@@ -1004,9 +1004,9 @@ int ehci_hub_control(
 			 */
 			if (((temp & PORT_OC) || (ehci->need_oc_pp_cycle))
 					&& HCS_PPC(ehci->hcs_params)) {
-				ehci_writel(ehci,
-					temp & ~(PORT_RWC_BITS | PORT_POWER),
-					status_reg);
+				spin_unlock_irqrestore(&ehci->lock, flags);
+				ehci_port_power(ehci, wIndex, false);
+				spin_lock_irqsave(&ehci->lock, flags);
 				temp = ehci_readl(ehci, status_reg);
 			}
 		}
@@ -1187,9 +1187,11 @@ int ehci_hub_control(
 			set_bit(wIndex, &ehci->suspended_ports);
 			break;
 		case USB_PORT_FEAT_POWER:
-			if (HCS_PPC (ehci->hcs_params))
-				ehci_writel(ehci, temp | PORT_POWER,
-						status_reg);
+			if (HCS_PPC(ehci->hcs_params)) {
+				spin_unlock_irqrestore(&ehci->lock, flags);
+				ehci_port_power(ehci, wIndex, true);
+				spin_lock_irqsave(&ehci->lock, flags);
+			}
 			break;
 		case USB_PORT_FEAT_RESET:
 			if (temp & (PORT_SUSPEND|PORT_RESUME))
@@ -1297,3 +1299,20 @@ static int ehci_port_handed_over(struct usb_hcd *hcd, int portnum)
 	reg = &ehci->regs->port_status[portnum - 1];
 	return ehci_readl(ehci, reg) & PORT_OWNER;
 }
+
+static int ehci_port_power(struct ehci_hcd *ehci, int portnum, bool enable)
+{
+	struct usb_hcd *hcd = ehci_to_hcd(ehci);
+	u32 __iomem *status_reg = &ehci->regs->port_status[portnum];
+	u32 temp = ehci_readl(ehci, status_reg) & ~PORT_RWC_BITS;
+
+	if (enable)
+		ehci_writel(ehci, temp | PORT_POWER, status_reg);
+	else
+		ehci_writel(ehci, temp & ~PORT_POWER, status_reg);
+
+	if (hcd->driver->port_power)
+		hcd->driver->port_power(hcd, portnum, enable);
+
+	return 0;
+}
diff --git a/drivers/usb/host/ehci-msm.c b/drivers/usb/host/ehci-msm.c
index 9dc2118ae808..9db74ca7e5b9 100644
--- a/drivers/usb/host/ehci-msm.c
+++ b/drivers/usb/host/ehci-msm.c
@@ -88,19 +88,13 @@ static int ehci_msm_probe(struct platform_device *pdev)
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "Unable to get memory resource\n");
-		ret = -ENODEV;
-		goto put_hcd;
-	}
-
-	hcd->rsrc_start = res->start;
-	hcd->rsrc_len = resource_size(res);
 	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(hcd->regs)) {
 		ret = PTR_ERR(hcd->regs);
 		goto put_hcd;
 	}
+	hcd->rsrc_start = res->start;
+	hcd->rsrc_len = resource_size(res);
 
 	/*
 	 * OTG driver takes care of PHY initialization, clock management,
diff --git a/drivers/usb/host/ehci-mv.c b/drivers/usb/host/ehci-mv.c
index 08147c35f836..849806a75f1c 100644
--- a/drivers/usb/host/ehci-mv.c
+++ b/drivers/usb/host/ehci-mv.c
@@ -153,7 +153,6 @@ static int mv_ehci_probe(struct platform_device *pdev)
 
 	ehci_mv = devm_kzalloc(&pdev->dev, sizeof(*ehci_mv), GFP_KERNEL);
 	if (ehci_mv == NULL) {
-		dev_err(&pdev->dev, "cannot allocate ehci_hcd_mv\n");
 		retval = -ENOMEM;
 		goto err_put_hcd;
 	}
@@ -170,12 +169,6 @@ static int mv_ehci_probe(struct platform_device *pdev)
 	}
 
 	r = platform_get_resource_byname(pdev, IORESOURCE_MEM, "phyregs");
-	if (r == NULL) {
-		dev_err(&pdev->dev, "no phy I/O memory resource defined\n");
-		retval = -ENODEV;
-		goto err_put_hcd;
-	}
-
 	ehci_mv->phy_regs = devm_ioremap_resource(&pdev->dev, r);
 	if (IS_ERR(ehci_mv->phy_regs)) {
 		retval = PTR_ERR(ehci_mv->phy_regs);
@@ -183,12 +176,6 @@ static int mv_ehci_probe(struct platform_device *pdev)
 	}
 
 	r = platform_get_resource_byname(pdev, IORESOURCE_MEM, "capregs");
-	if (!r) {
-		dev_err(&pdev->dev, "no I/O memory resource defined\n");
-		retval = -ENODEV;
-		goto err_put_hcd;
-	}
-
 	ehci_mv->cap_regs = devm_ioremap_resource(&pdev->dev, r);
 	if (IS_ERR(ehci_mv->cap_regs)) {
 		retval = PTR_ERR(ehci_mv->cap_regs);
diff --git a/drivers/usb/host/ehci-mxc.c b/drivers/usb/host/ehci-mxc.c
index dbe5e4eea08d..c7a9b31eeaef 100644
--- a/drivers/usb/host/ehci-mxc.c
+++ b/drivers/usb/host/ehci-mxc.c
@@ -69,20 +69,13 @@ static int ehci_mxc_drv_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(dev, "Found HC with no register addr. Check setup!\n");
-		ret = -ENODEV;
-		goto err_alloc;
-	}
-
-	hcd->rsrc_start = res->start;
-	hcd->rsrc_len = resource_size(res);
-
 	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(hcd->regs)) {
 		ret = PTR_ERR(hcd->regs);
 		goto err_alloc;
 	}
+	hcd->rsrc_start = res->start;
+	hcd->rsrc_len = resource_size(res);
 
 	hcd->has_tt = 1;
 	ehci = hcd_to_ehci(hcd);
diff --git a/drivers/usb/host/ehci-octeon.c b/drivers/usb/host/ehci-octeon.c
deleted file mode 100644
index 9051439039a7..000000000000
--- a/drivers/usb/host/ehci-octeon.c
+++ /dev/null
@@ -1,188 +0,0 @@
-/*
- * EHCI HCD glue for Cavium Octeon II SOCs.
- *
- * Loosely based on ehci-au1xxx.c
- *
- * 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) 2010 Cavium Networks
- *
- */
-
-#include <linux/platform_device.h>
-
-#include <asm/octeon/octeon.h>
-#include <asm/octeon/cvmx-uctlx-defs.h>
-
-#define OCTEON_EHCI_HCD_NAME "octeon-ehci"
-
-/* Common clock init code.  */
-void octeon2_usb_clocks_start(void);
-void octeon2_usb_clocks_stop(void);
-
-static void ehci_octeon_start(void)
-{
-	union cvmx_uctlx_ehci_ctl ehci_ctl;
-
-	octeon2_usb_clocks_start();
-
-	ehci_ctl.u64 = cvmx_read_csr(CVMX_UCTLX_EHCI_CTL(0));
-	/* Use 64-bit addressing. */
-	ehci_ctl.s.ehci_64b_addr_en = 1;
-	ehci_ctl.s.l2c_addr_msb = 0;
-	ehci_ctl.s.l2c_buff_emod = 1; /* Byte swapped. */
-	ehci_ctl.s.l2c_desc_emod = 1; /* Byte swapped. */
-	cvmx_write_csr(CVMX_UCTLX_EHCI_CTL(0), ehci_ctl.u64);
-}
-
-static void ehci_octeon_stop(void)
-{
-	octeon2_usb_clocks_stop();
-}
-
-static const struct hc_driver ehci_octeon_hc_driver = {
-	.description		= hcd_name,
-	.product_desc		= "Octeon EHCI",
-	.hcd_priv_size		= sizeof(struct ehci_hcd),
-
-	/*
-	 * generic hardware linkage
-	 */
-	.irq			= ehci_irq,
-	.flags			= HCD_MEMORY | HCD_USB2 | HCD_BH,
-
-	/*
-	 * basic lifecycle operations
-	 */
-	.reset			= 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 u64 ehci_octeon_dma_mask = DMA_BIT_MASK(64);
-
-static int ehci_octeon_drv_probe(struct platform_device *pdev)
-{
-	struct usb_hcd *hcd;
-	struct ehci_hcd *ehci;
-	struct resource *res_mem;
-	int irq;
-	int ret;
-
-	if (usb_disabled())
-		return -ENODEV;
-
-	irq = platform_get_irq(pdev, 0);
-	if (irq < 0) {
-		dev_err(&pdev->dev, "No irq assigned\n");
-		return -ENODEV;
-	}
-
-	res_mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (res_mem == NULL) {
-		dev_err(&pdev->dev, "No register space assigned\n");
-		return -ENODEV;
-	}
-
-	/*
-	 * We can DMA from anywhere. But the descriptors must be in
-	 * the lower 4GB.
-	 */
-	pdev->dev.dma_mask = &ehci_octeon_dma_mask;
-	ret = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
-	if (ret)
-		return ret;
-
-	hcd = usb_create_hcd(&ehci_octeon_hc_driver, &pdev->dev, "octeon");
-	if (!hcd)
-		return -ENOMEM;
-
-	hcd->rsrc_start = res_mem->start;
-	hcd->rsrc_len = resource_size(res_mem);
-
-	hcd->regs = devm_ioremap_resource(&pdev->dev, res_mem);
-	if (IS_ERR(hcd->regs)) {
-		ret = PTR_ERR(hcd->regs);
-		goto err1;
-	}
-
-	ehci_octeon_start();
-
-	ehci = hcd_to_ehci(hcd);
-
-	/* Octeon EHCI matches CPU endianness. */
-#ifdef __BIG_ENDIAN
-	ehci->big_endian_mmio = 1;
-#endif
-
-	ehci->caps = hcd->regs;
-
-	ret = usb_add_hcd(hcd, irq, IRQF_SHARED);
-	if (ret) {
-		dev_dbg(&pdev->dev, "failed to add hcd with err %d\n", ret);
-		goto err2;
-	}
-	device_wakeup_enable(hcd->self.controller);
-
-	platform_set_drvdata(pdev, hcd);
-
-	return 0;
-err2:
-	ehci_octeon_stop();
-
-err1:
-	usb_put_hcd(hcd);
-	return ret;
-}
-
-static int ehci_octeon_drv_remove(struct platform_device *pdev)
-{
-	struct usb_hcd *hcd = platform_get_drvdata(pdev);
-
-	usb_remove_hcd(hcd);
-
-	ehci_octeon_stop();
-	usb_put_hcd(hcd);
-
-	return 0;
-}
-
-static struct platform_driver ehci_octeon_driver = {
-	.probe		= ehci_octeon_drv_probe,
-	.remove		= ehci_octeon_drv_remove,
-	.shutdown	= usb_hcd_platform_shutdown,
-	.driver = {
-		.name	= OCTEON_EHCI_HCD_NAME,
-		.owner	= THIS_MODULE,
-	}
-};
-
-MODULE_ALIAS("platform:" OCTEON_EHCI_HCD_NAME);
diff --git a/drivers/usb/host/ehci-orion.c b/drivers/usb/host/ehci-orion.c
index 22e15cab8ea5..f6eafecab15c 100644
--- a/drivers/usb/host/ehci-orion.c
+++ b/drivers/usb/host/ehci-orion.c
@@ -25,8 +25,8 @@
 
 #include "ehci.h"
 
-#define rdl(off)	__raw_readl(hcd->regs + (off))
-#define wrl(off, val)	__raw_writel((val), hcd->regs + (off))
+#define rdl(off)	readl_relaxed(hcd->regs + (off))
+#define wrl(off, val)	writel_relaxed((val), hcd->regs + (off))
 
 #define USB_CMD			0x140
 #define USB_MODE		0x1a8
@@ -175,15 +175,6 @@ static int ehci_orion_drv_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev,
-			"Found HC with no register addr. Check %s setup!\n",
-			dev_name(&pdev->dev));
-		err = -ENODEV;
-		goto err;
-	}
-
 	/*
 	 * Right now device-tree probed devices don't get dma_mask
 	 * set. Since shared usb code relies on it, set it here for
@@ -193,6 +184,7 @@ static int ehci_orion_drv_probe(struct platform_device *pdev)
 	if (err)
 		goto err;
 
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(regs)) {
 		err = PTR_ERR(regs);
@@ -321,7 +313,6 @@ static struct platform_driver ehci_orion_driver = {
 	.shutdown	= usb_hcd_platform_shutdown,
 	.driver = {
 		.name	= "orion-ehci",
-		.owner  = THIS_MODULE,
 		.of_match_table = ehci_orion_dt_ids,
 	},
 };
diff --git a/drivers/usb/host/ehci-platform.c b/drivers/usb/host/ehci-platform.c
index 2f5b9ce3e042..8557803e6154 100644
--- a/drivers/usb/host/ehci-platform.c
+++ b/drivers/usb/host/ehci-platform.c
@@ -164,11 +164,6 @@ static int ehci_platform_probe(struct platform_device *dev)
 		dev_err(&dev->dev, "no irq provided");
 		return irq;
 	}
-	res_mem = platform_get_resource(dev, IORESOURCE_MEM, 0);
-	if (!res_mem) {
-		dev_err(&dev->dev, "no memory resource provided");
-		return -ENXIO;
-	}
 
 	hcd = usb_create_hcd(&ehci_platform_hc_driver, &dev->dev,
 			     dev_name(&dev->dev));
@@ -250,14 +245,15 @@ static int ehci_platform_probe(struct platform_device *dev)
 			goto err_reset;
 	}
 
-	hcd->rsrc_start = res_mem->start;
-	hcd->rsrc_len = resource_size(res_mem);
-
+	res_mem = platform_get_resource(dev, IORESOURCE_MEM, 0);
 	hcd->regs = devm_ioremap_resource(&dev->dev, res_mem);
 	if (IS_ERR(hcd->regs)) {
 		err = PTR_ERR(hcd->regs);
 		goto err_power;
 	}
+	hcd->rsrc_start = res_mem->start;
+	hcd->rsrc_len = resource_size(res_mem);
+
 	err = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (err)
 		goto err_power;
@@ -311,8 +307,7 @@ static int ehci_platform_remove(struct platform_device *dev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-
+#ifdef CONFIG_PM_SLEEP
 static int ehci_platform_suspend(struct device *dev)
 {
 	struct usb_hcd *hcd = dev_get_drvdata(dev);
@@ -348,11 +343,7 @@ static int ehci_platform_resume(struct device *dev)
 	ehci_resume(hcd, false);
 	return 0;
 }
-
-#else /* !CONFIG_PM */
-#define ehci_platform_suspend	NULL
-#define ehci_platform_resume	NULL
-#endif /* CONFIG_PM */
+#endif /* CONFIG_PM_SLEEP */
 
 static const struct of_device_id vt8500_ehci_ids[] = {
 	{ .compatible = "via,vt8500-ehci", },
@@ -368,10 +359,8 @@ static const struct platform_device_id ehci_platform_table[] = {
 };
 MODULE_DEVICE_TABLE(platform, ehci_platform_table);
 
-static const struct dev_pm_ops ehci_platform_pm_ops = {
-	.suspend	= ehci_platform_suspend,
-	.resume		= ehci_platform_resume,
-};
+static SIMPLE_DEV_PM_OPS(ehci_platform_pm_ops, ehci_platform_suspend,
+	ehci_platform_resume);
 
 static struct platform_driver ehci_platform_driver = {
 	.id_table	= ehci_platform_table,
@@ -379,7 +368,6 @@ static struct platform_driver ehci_platform_driver = {
 	.remove		= ehci_platform_remove,
 	.shutdown	= usb_hcd_platform_shutdown,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "ehci-platform",
 		.pm	= &ehci_platform_pm_ops,
 		.of_match_table = vt8500_ehci_ids,
diff --git a/drivers/usb/host/ehci-sead3.c b/drivers/usb/host/ehci-sead3.c
index cf1267673868..9b6e8d0eac43 100644
--- a/drivers/usb/host/ehci-sead3.c
+++ b/drivers/usb/host/ehci-sead3.c
@@ -110,14 +110,13 @@ static int ehci_hcd_sead3_drv_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	hcd->rsrc_start = res->start;
-	hcd->rsrc_len = resource_size(res);
-
 	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(hcd->regs)) {
 		ret = PTR_ERR(hcd->regs);
 		goto err1;
 	}
+	hcd->rsrc_start = res->start;
+	hcd->rsrc_len = resource_size(res);
 
 	/* Root hub has integrated TT. */
 	hcd->has_tt = 1;
diff --git a/drivers/usb/host/ehci-sh.c b/drivers/usb/host/ehci-sh.c
index 9b9b9f5b016e..0e0ce684aff3 100644
--- a/drivers/usb/host/ehci-sh.c
+++ b/drivers/usb/host/ehci-sh.c
@@ -86,15 +86,6 @@ static int ehci_hcd_sh_probe(struct platform_device *pdev)
 	if (usb_disabled())
 		return -ENODEV;
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev,
-			"Found HC with no register addr. Check %s setup!\n",
-			dev_name(&pdev->dev));
-		ret = -ENODEV;
-		goto fail_create_hcd;
-	}
-
 	irq = platform_get_irq(pdev, 0);
 	if (irq <= 0) {
 		dev_err(&pdev->dev,
@@ -114,19 +105,18 @@ static int ehci_hcd_sh_probe(struct platform_device *pdev)
 		goto fail_create_hcd;
 	}
 
-	hcd->rsrc_start = res->start;
-	hcd->rsrc_len = resource_size(res);
-
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(hcd->regs)) {
 		ret = PTR_ERR(hcd->regs);
 		goto fail_request_resource;
 	}
+	hcd->rsrc_start = res->start;
+	hcd->rsrc_len = resource_size(res);
 
 	priv = devm_kzalloc(&pdev->dev, sizeof(struct ehci_sh_priv),
 			    GFP_KERNEL);
 	if (!priv) {
-		dev_dbg(&pdev->dev, "error allocating priv data\n");
 		ret = -ENOMEM;
 		goto fail_request_resource;
 	}
diff --git a/drivers/usb/host/ehci-spear.c b/drivers/usb/host/ehci-spear.c
index 1355ff0946b9..34e14746b92e 100644
--- a/drivers/usb/host/ehci-spear.c
+++ b/drivers/usb/host/ehci-spear.c
@@ -99,18 +99,13 @@ static int spear_ehci_hcd_drv_probe(struct platform_device *pdev)
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		retval = -ENODEV;
-		goto err_put_hcd;
-	}
-
-	hcd->rsrc_start = res->start;
-	hcd->rsrc_len = resource_size(res);
 	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(hcd->regs)) {
 		retval = PTR_ERR(hcd->regs);
 		goto err_put_hcd;
 	}
+	hcd->rsrc_start = res->start;
+	hcd->rsrc_len = resource_size(res);
 
 	sehci = to_spear_ehci(hcd);
 	sehci->clk = usbh_clk;
diff --git a/drivers/usb/host/ehci-sysfs.c b/drivers/usb/host/ehci-sysfs.c
index f6459dfb6f54..5e44407aa099 100644
--- a/drivers/usb/host/ehci-sysfs.c
+++ b/drivers/usb/host/ehci-sysfs.c
@@ -132,7 +132,7 @@ static ssize_t store_uframe_periodic_max(struct device *dev,
 
 		if (allocated_max > uframe_periodic_max) {
 			ehci_info(ehci,
-				"cannot decrease uframe_periodic_max becase "
+				"cannot decrease uframe_periodic_max because "
 				"periodic bandwidth is already allocated "
 				"(%u > %u)\n",
 				allocated_max, uframe_periodic_max);
diff --git a/drivers/usb/host/ehci-tegra.c b/drivers/usb/host/ehci-tegra.c
index aaa01971efe9..19a9af1b4d74 100644
--- a/drivers/usb/host/ehci-tegra.c
+++ b/drivers/usb/host/ehci-tegra.c
@@ -460,18 +460,14 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 		"nvidia,needs-double-reset");
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "Failed to get I/O memory\n");
-		err = -ENXIO;
-		goto cleanup_clk_en;
-	}
-	hcd->rsrc_start = res->start;
-	hcd->rsrc_len = resource_size(res);
 	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(hcd->regs)) {
 		err = PTR_ERR(hcd->regs);
 		goto cleanup_clk_en;
 	}
+	hcd->rsrc_start = res->start;
+	hcd->rsrc_len = resource_size(res);
+
 	ehci->caps = hcd->regs + 0x100;
 	ehci->has_hostpc = soc_config->has_hostpc;
 
@@ -484,7 +480,6 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 	u_phy->otg = devm_kzalloc(&pdev->dev, sizeof(struct usb_otg),
 			     GFP_KERNEL);
 	if (!u_phy->otg) {
-		dev_err(&pdev->dev, "Failed to alloc memory for otg\n");
 		err = -ENOMEM;
 		goto cleanup_phy;
 	}
diff --git a/drivers/usb/host/ehci-w90x900.c b/drivers/usb/host/ehci-w90x900.c
index a9303aff125e..e42a29e8e229 100644
--- a/drivers/usb/host/ehci-w90x900.c
+++ b/drivers/usb/host/ehci-w90x900.c
@@ -42,27 +42,20 @@ static int usb_w90x900_probe(const struct hc_driver *driver,
 	int retval = 0, irq;
 	unsigned long val;
 
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		retval = -ENXIO;
-		goto err1;
-	}
-
 	hcd = usb_create_hcd(driver, &pdev->dev, "w90x900 EHCI");
 	if (!hcd) {
 		retval = -ENOMEM;
 		goto err1;
 	}
 
-	hcd->rsrc_start = res->start;
-	hcd->rsrc_len = resource_size(res);
-
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(hcd->regs)) {
 		retval = PTR_ERR(hcd->regs);
 		goto err2;
 	}
+	hcd->rsrc_start = res->start;
+	hcd->rsrc_len = resource_size(res);
 
 	ehci = hcd_to_ehci(hcd);
 	ehci->caps = hcd->regs;
@@ -82,8 +75,10 @@ static int usb_w90x900_probe(const struct hc_driver *driver,
 	__raw_writel(val, ehci->regs+PHY1_CTR);
 
 	irq = platform_get_irq(pdev, 0);
-	if (irq < 0)
+	if (irq < 0) {
+		retval = irq;
 		goto err2;
+	}
 
 	retval = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (retval != 0)
@@ -126,7 +121,6 @@ static struct platform_driver ehci_hcd_w90x900_driver = {
 	.remove = ehci_w90x900_remove,
 	.driver = {
 		.name = "w90x900-ehci",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/usb/host/ehci.h b/drivers/usb/host/ehci.h
index eee228a26a0e..6f0577b0a5ae 100644
--- a/drivers/usb/host/ehci.h
+++ b/drivers/usb/host/ehci.h
@@ -859,6 +859,8 @@ static inline u32 hc32_to_cpup (const struct ehci_hcd *ehci, const __hc32 *x)
 struct ehci_driver_overrides {
 	size_t		extra_priv_size;
 	int		(*reset)(struct usb_hcd *hcd);
+	int		(*port_power)(struct usb_hcd *hcd,
+				int portnum, bool enable);
 };
 
 extern void	ehci_init_driver(struct hc_driver *drv,
diff --git a/drivers/usb/host/fhci-hcd.c b/drivers/usb/host/fhci-hcd.c
index a1a1ef521436..c6cebb96fd21 100644
--- a/drivers/usb/host/fhci-hcd.c
+++ b/drivers/usb/host/fhci-hcd.c
@@ -821,7 +821,6 @@ MODULE_DEVICE_TABLE(of, of_fhci_match);
 static struct platform_driver of_fhci_driver = {
 	.driver = {
 		.name = "fsl,usb-fhci",
-		.owner = THIS_MODULE,
 		.of_match_table = of_fhci_match,
 	},
 	.probe		= of_fhci_probe,
diff --git a/drivers/usb/host/fotg210-hcd.c b/drivers/usb/host/fotg210-hcd.c
index 3de1278677d0..ecf02b2623e8 100644
--- a/drivers/usb/host/fotg210-hcd.c
+++ b/drivers/usb/host/fotg210-hcd.c
@@ -4958,7 +4958,7 @@ static ssize_t store_uframe_periodic_max(struct device *dev,
 
 		if (allocated_max > uframe_periodic_max) {
 			fotg210_info(fotg210,
-				"cannot decrease uframe_periodic_max becase "
+				"cannot decrease uframe_periodic_max because "
 				"periodic bandwidth is already allocated "
 				"(%u > %u)\n",
 				allocated_max, uframe_periodic_max);
diff --git a/drivers/usb/host/fotg210.h b/drivers/usb/host/fotg210.h
index ac6cd1bfd208..3bad17859cd7 100644
--- a/drivers/usb/host/fotg210.h
+++ b/drivers/usb/host/fotg210.h
@@ -1,6 +1,8 @@
 #ifndef __LINUX_FOTG210_H
 #define __LINUX_FOTG210_H
 
+#include <linux/usb/ehci-dbgp.h>
+
 /* definitions used for the EHCI driver */
 
 /*
@@ -84,7 +86,7 @@ struct fotg210_hcd {			/* one per controller */
 	/* glue to PCI and HCD framework */
 	struct fotg210_caps __iomem *caps;
 	struct fotg210_regs __iomem *regs;
-	struct fotg210_dbg_port __iomem *debug;
+	struct ehci_dbg_port __iomem *debug;
 
 	__u32			hcs_params;	/* cached register copy */
 	spinlock_t		lock;
@@ -293,64 +295,6 @@ struct fotg210_regs {
 #define GMIR_MDEV_INT	(1 << 0)
 };
 
-/* Appendix C, Debug port ... intended for use with special "debug devices"
- * that can help if there's no serial console.  (nonstandard enumeration.)
- */
-struct fotg210_dbg_port {
-	u32	control;
-#define DBGP_OWNER	(1<<30)
-#define DBGP_ENABLED	(1<<28)
-#define DBGP_DONE	(1<<16)
-#define DBGP_INUSE	(1<<10)
-#define DBGP_ERRCODE(x)	(((x)>>7)&0x07)
-#	define DBGP_ERR_BAD	1
-#	define DBGP_ERR_SIGNAL	2
-#define DBGP_ERROR	(1<<6)
-#define DBGP_GO		(1<<5)
-#define DBGP_OUT	(1<<4)
-#define DBGP_LEN(x)	(((x)>>0)&0x0f)
-	u32	pids;
-#define DBGP_PID_GET(x)		(((x)>>16)&0xff)
-#define DBGP_PID_SET(data, tok)	(((data)<<8)|(tok))
-	u32	data03;
-	u32	data47;
-	u32	address;
-#define DBGP_EPADDR(dev, ep)	(((dev)<<8)|(ep))
-};
-
-#ifdef CONFIG_EARLY_PRINTK_DBGP
-#include <linux/init.h>
-extern int __init early_dbgp_init(char *s);
-extern struct console early_dbgp_console;
-#endif /* CONFIG_EARLY_PRINTK_DBGP */
-
-struct usb_hcd;
-
-static inline int xen_dbgp_reset_prep(struct usb_hcd *hcd)
-{
-	return 1; /* Shouldn't this be 0? */
-}
-
-static inline int xen_dbgp_external_startup(struct usb_hcd *hcd)
-{
-	return -1;
-}
-
-#ifdef CONFIG_EARLY_PRINTK_DBGP
-/* Call backs from fotg210 host driver to fotg210 debug driver */
-extern int dbgp_external_startup(struct usb_hcd *);
-extern int dbgp_reset_prep(struct usb_hcd *hcd);
-#else
-static inline int dbgp_reset_prep(struct usb_hcd *hcd)
-{
-	return xen_dbgp_reset_prep(hcd);
-}
-static inline int dbgp_external_startup(struct usb_hcd *hcd)
-{
-	return xen_dbgp_external_startup(hcd);
-}
-#endif
-
 /*-------------------------------------------------------------------------*/
 
 #define	QTD_NEXT(fotg210, dma)	cpu_to_hc32(fotg210, (u32)dma)
diff --git a/drivers/usb/host/fsl-mph-dr-of.c b/drivers/usb/host/fsl-mph-dr-of.c
index 9162d1b6c0a3..7e325e90d7d9 100644
--- a/drivers/usb/host/fsl-mph-dr-of.c
+++ b/drivers/usb/host/fsl-mph-dr-of.c
@@ -326,7 +326,6 @@ static const struct of_device_id fsl_usb2_mph_dr_of_match[] = {
 static struct platform_driver fsl_usb2_mph_dr_driver = {
 	.driver = {
 		.name = "fsl-usb2-mph-dr",
-		.owner = THIS_MODULE,
 		.of_match_table = fsl_usb2_mph_dr_of_match,
 	},
 	.probe	= fsl_usb2_mph_dr_of_probe,
diff --git a/drivers/usb/host/fusbh200-hcd.c b/drivers/usb/host/fusbh200-hcd.c
index abe42f31559f..664d2aa1239c 100644
--- a/drivers/usb/host/fusbh200-hcd.c
+++ b/drivers/usb/host/fusbh200-hcd.c
@@ -4893,7 +4893,7 @@ static ssize_t store_uframe_periodic_max(struct device *dev,
 
 		if (allocated_max > uframe_periodic_max) {
 			fusbh200_info(fusbh200,
-				"cannot decrease uframe_periodic_max becase "
+				"cannot decrease uframe_periodic_max because "
 				"periodic bandwidth is already allocated "
 				"(%u > %u)\n",
 				allocated_max, uframe_periodic_max);
diff --git a/drivers/usb/host/fusbh200.h b/drivers/usb/host/fusbh200.h
index 6b719e066c3f..d6e5b3d4aa68 100644
--- a/drivers/usb/host/fusbh200.h
+++ b/drivers/usb/host/fusbh200.h
@@ -1,6 +1,8 @@
 #ifndef __LINUX_FUSBH200_H
 #define __LINUX_FUSBH200_H
 
+#include <linux/usb/ehci-dbgp.h>
+
 /* definitions used for the EHCI driver */
 
 /*
@@ -84,7 +86,7 @@ struct fusbh200_hcd {			/* one per controller */
 	/* glue to PCI and HCD framework */
 	struct fusbh200_caps __iomem *caps;
 	struct fusbh200_regs __iomem *regs;
-	struct fusbh200_dbg_port __iomem *debug;
+	struct ehci_dbg_port __iomem *debug;
 
 	__u32			hcs_params;	/* cached register copy */
 	spinlock_t		lock;
@@ -285,64 +287,6 @@ struct fusbh200_regs {
 #define BMIER_VBUS_ERR_EN	(1<<0)
 };
 
-/* Appendix C, Debug port ... intended for use with special "debug devices"
- * that can help if there's no serial console.  (nonstandard enumeration.)
- */
-struct fusbh200_dbg_port {
-	u32	control;
-#define DBGP_OWNER	(1<<30)
-#define DBGP_ENABLED	(1<<28)
-#define DBGP_DONE	(1<<16)
-#define DBGP_INUSE	(1<<10)
-#define DBGP_ERRCODE(x)	(((x)>>7)&0x07)
-#	define DBGP_ERR_BAD	1
-#	define DBGP_ERR_SIGNAL	2
-#define DBGP_ERROR	(1<<6)
-#define DBGP_GO		(1<<5)
-#define DBGP_OUT	(1<<4)
-#define DBGP_LEN(x)	(((x)>>0)&0x0f)
-	u32	pids;
-#define DBGP_PID_GET(x)		(((x)>>16)&0xff)
-#define DBGP_PID_SET(data, tok)	(((data)<<8)|(tok))
-	u32	data03;
-	u32	data47;
-	u32	address;
-#define DBGP_EPADDR(dev, ep)	(((dev)<<8)|(ep))
-};
-
-#ifdef CONFIG_EARLY_PRINTK_DBGP
-#include <linux/init.h>
-extern int __init early_dbgp_init(char *s);
-extern struct console early_dbgp_console;
-#endif /* CONFIG_EARLY_PRINTK_DBGP */
-
-struct usb_hcd;
-
-static inline int xen_dbgp_reset_prep(struct usb_hcd *hcd)
-{
-	return 1; /* Shouldn't this be 0? */
-}
-
-static inline int xen_dbgp_external_startup(struct usb_hcd *hcd)
-{
-	return -1;
-}
-
-#ifdef CONFIG_EARLY_PRINTK_DBGP
-/* Call backs from fusbh200 host driver to fusbh200 debug driver */
-extern int dbgp_external_startup(struct usb_hcd *);
-extern int dbgp_reset_prep(struct usb_hcd *hcd);
-#else
-static inline int dbgp_reset_prep(struct usb_hcd *hcd)
-{
-	return xen_dbgp_reset_prep(hcd);
-}
-static inline int dbgp_external_startup(struct usb_hcd *hcd)
-{
-	return xen_dbgp_external_startup(hcd);
-}
-#endif
-
 /*-------------------------------------------------------------------------*/
 
 #define	QTD_NEXT(fusbh200, dma)	cpu_to_hc32(fusbh200, (u32)dma)
diff --git a/drivers/usb/host/imx21-hcd.c b/drivers/usb/host/imx21-hcd.c
index 207bad99301f..eb4efba9f1ad 100644
--- a/drivers/usb/host/imx21-hcd.c
+++ b/drivers/usb/host/imx21-hcd.c
@@ -1174,11 +1174,11 @@ static int imx21_hc_urb_enqueue(struct usb_hcd *hcd,
 
 	dev_vdbg(imx21->dev,
 		"enqueue urb=%p ep=%p len=%d "
-		"buffer=%p dma=%08X setupBuf=%p setupDma=%08X\n",
+		"buffer=%p dma=%pad setupBuf=%p setupDma=%pad\n",
 		urb, ep,
 		urb->transfer_buffer_length,
-		urb->transfer_buffer, urb->transfer_dma,
-		urb->setup_packet, urb->setup_dma);
+		urb->transfer_buffer, &urb->transfer_dma,
+		urb->setup_packet, &urb->setup_dma);
 
 	if (usb_pipeisoc(urb->pipe))
 		return imx21_hc_urb_enqueue_isoc(hcd, ep, urb, mem_flags);
diff --git a/drivers/usb/host/isp116x-hcd.c b/drivers/usb/host/isp116x-hcd.c
index 240e792c81a7..31c9c4d0fa0b 100644
--- a/drivers/usb/host/isp116x-hcd.c
+++ b/drivers/usb/host/isp116x-hcd.c
@@ -1707,7 +1707,6 @@ static struct platform_driver isp116x_driver = {
 	.resume = isp116x_resume,
 	.driver = {
 		.name = hcd_name,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/usb/host/isp1362-hcd.c b/drivers/usb/host/isp1362-hcd.c
index 4bb37982855e..75e5876f9d7c 100644
--- a/drivers/usb/host/isp1362-hcd.c
+++ b/drivers/usb/host/isp1362-hcd.c
@@ -2778,7 +2778,6 @@ static struct platform_driver isp1362_driver = {
 	.resume = isp1362_resume,
 	.driver = {
 		.name = hcd_name,
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/usb/host/isp1760-hcd.c b/drivers/usb/host/isp1760-hcd.c
index e752c3098f38..395649f357aa 100644
--- a/drivers/usb/host/isp1760-hcd.c
+++ b/drivers/usb/host/isp1760-hcd.c
@@ -1739,7 +1739,7 @@ static int isp1760_hub_status_data(struct usb_hcd *hcd, char *buf)
 	int retval = 1;
 	unsigned long flags;
 
-	/* if !PM_RUNTIME, root hub timers won't get shut down ... */
+	/* if !PM, root hub timers won't get shut down ... */
 	if (!HC_IS_RUNNING(hcd->state))
 		return 0;
 
diff --git a/drivers/usb/host/isp1760-if.c b/drivers/usb/host/isp1760-if.c
index df931e9ba5b5..09254a43bc01 100644
--- a/drivers/usb/host/isp1760-if.c
+++ b/drivers/usb/host/isp1760-if.c
@@ -161,7 +161,6 @@ MODULE_DEVICE_TABLE(of, of_isp1760_match);
 static struct platform_driver isp1760_of_driver = {
 	.driver = {
 		.name = "nxp-isp1760",
-		.owner = THIS_MODULE,
 		.of_match_table = of_isp1760_match,
 	},
 	.probe          = of_isp1760_probe,
diff --git a/drivers/usb/host/octeon2-common.c b/drivers/usb/host/octeon2-common.c
deleted file mode 100644
index d9df423f3d12..000000000000
--- a/drivers/usb/host/octeon2-common.c
+++ /dev/null
@@ -1,200 +0,0 @@
-/*
- * 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) 2010, 2011 Cavium Networks
- */
-
-#include <linux/module.h>
-#include <linux/mutex.h>
-#include <linux/delay.h>
-
-#include <asm/octeon/octeon.h>
-#include <asm/octeon/cvmx-uctlx-defs.h>
-
-static DEFINE_MUTEX(octeon2_usb_clocks_mutex);
-
-static int octeon2_usb_clock_start_cnt;
-
-void octeon2_usb_clocks_start(void)
-{
-	u64 div;
-	union cvmx_uctlx_if_ena if_ena;
-	union cvmx_uctlx_clk_rst_ctl clk_rst_ctl;
-	union cvmx_uctlx_uphy_ctl_status uphy_ctl_status;
-	union cvmx_uctlx_uphy_portx_ctl_status port_ctl_status;
-	int i;
-	unsigned long io_clk_64_to_ns;
-
-
-	mutex_lock(&octeon2_usb_clocks_mutex);
-
-	octeon2_usb_clock_start_cnt++;
-	if (octeon2_usb_clock_start_cnt != 1)
-		goto exit;
-
-	io_clk_64_to_ns = 64000000000ull / octeon_get_io_clock_rate();
-
-	/*
-	 * Step 1: Wait for voltages stable.  That surely happened
-	 * before starting the kernel.
-	 *
-	 * Step 2: Enable  SCLK of UCTL by writing UCTL0_IF_ENA[EN] = 1
-	 */
-	if_ena.u64 = 0;
-	if_ena.s.en = 1;
-	cvmx_write_csr(CVMX_UCTLX_IF_ENA(0), if_ena.u64);
-
-	/* Step 3: Configure the reference clock, PHY, and HCLK */
-	clk_rst_ctl.u64 = cvmx_read_csr(CVMX_UCTLX_CLK_RST_CTL(0));
-
-	/*
-	 * If the UCTL looks like it has already been started, skip
-	 * the initialization, otherwise bus errors are obtained.
-	 */
-	if (clk_rst_ctl.s.hrst)
-		goto end_clock;
-	/* 3a */
-	clk_rst_ctl.s.p_por = 1;
-	clk_rst_ctl.s.hrst = 0;
-	clk_rst_ctl.s.p_prst = 0;
-	clk_rst_ctl.s.h_clkdiv_rst = 0;
-	clk_rst_ctl.s.o_clkdiv_rst = 0;
-	clk_rst_ctl.s.h_clkdiv_en = 0;
-	clk_rst_ctl.s.o_clkdiv_en = 0;
-	cvmx_write_csr(CVMX_UCTLX_CLK_RST_CTL(0), clk_rst_ctl.u64);
-
-	/* 3b */
-	/* 12MHz crystal. */
-	clk_rst_ctl.s.p_refclk_sel = 0;
-	clk_rst_ctl.s.p_refclk_div = 0;
-	cvmx_write_csr(CVMX_UCTLX_CLK_RST_CTL(0), clk_rst_ctl.u64);
-
-	/* 3c */
-	div = octeon_get_io_clock_rate() / 130000000ull;
-
-	switch (div) {
-	case 0:
-		div = 1;
-		break;
-	case 1:
-	case 2:
-	case 3:
-	case 4:
-		break;
-	case 5:
-		div = 4;
-		break;
-	case 6:
-	case 7:
-		div = 6;
-		break;
-	case 8:
-	case 9:
-	case 10:
-	case 11:
-		div = 8;
-		break;
-	default:
-		div = 12;
-		break;
-	}
-	clk_rst_ctl.s.h_div = div;
-	cvmx_write_csr(CVMX_UCTLX_CLK_RST_CTL(0), clk_rst_ctl.u64);
-	/* Read it back, */
-	clk_rst_ctl.u64 = cvmx_read_csr(CVMX_UCTLX_CLK_RST_CTL(0));
-	clk_rst_ctl.s.h_clkdiv_en = 1;
-	cvmx_write_csr(CVMX_UCTLX_CLK_RST_CTL(0), clk_rst_ctl.u64);
-	/* 3d */
-	clk_rst_ctl.s.h_clkdiv_rst = 1;
-	cvmx_write_csr(CVMX_UCTLX_CLK_RST_CTL(0), clk_rst_ctl.u64);
-
-	/* 3e: delay 64 io clocks */
-	ndelay(io_clk_64_to_ns);
-
-	/*
-	 * Step 4: Program the power-on reset field in the UCTL
-	 * clock-reset-control register.
-	 */
-	clk_rst_ctl.s.p_por = 0;
-	cvmx_write_csr(CVMX_UCTLX_CLK_RST_CTL(0), clk_rst_ctl.u64);
-
-	/* Step 5:    Wait 1 ms for the PHY clock to start. */
-	mdelay(1);
-
-	/*
-	 * Step 6: Program the reset input from automatic test
-	 * equipment field in the UPHY CSR
-	 */
-	uphy_ctl_status.u64 = cvmx_read_csr(CVMX_UCTLX_UPHY_CTL_STATUS(0));
-	uphy_ctl_status.s.ate_reset = 1;
-	cvmx_write_csr(CVMX_UCTLX_UPHY_CTL_STATUS(0), uphy_ctl_status.u64);
-
-	/* Step 7: Wait for at least 10ns. */
-	ndelay(10);
-
-	/* Step 8: Clear the ATE_RESET field in the UPHY CSR. */
-	uphy_ctl_status.s.ate_reset = 0;
-	cvmx_write_csr(CVMX_UCTLX_UPHY_CTL_STATUS(0), uphy_ctl_status.u64);
-
-	/*
-	 * Step 9: Wait for at least 20ns for UPHY to output PHY clock
-	 * signals and OHCI_CLK48
-	 */
-	ndelay(20);
-
-	/* Step 10: Configure the OHCI_CLK48 and OHCI_CLK12 clocks. */
-	/* 10a */
-	clk_rst_ctl.s.o_clkdiv_rst = 1;
-	cvmx_write_csr(CVMX_UCTLX_CLK_RST_CTL(0), clk_rst_ctl.u64);
-
-	/* 10b */
-	clk_rst_ctl.s.o_clkdiv_en = 1;
-	cvmx_write_csr(CVMX_UCTLX_CLK_RST_CTL(0), clk_rst_ctl.u64);
-
-	/* 10c */
-	ndelay(io_clk_64_to_ns);
-
-	/*
-	 * Step 11: Program the PHY reset field:
-	 * UCTL0_CLK_RST_CTL[P_PRST] = 1
-	 */
-	clk_rst_ctl.s.p_prst = 1;
-	cvmx_write_csr(CVMX_UCTLX_CLK_RST_CTL(0), clk_rst_ctl.u64);
-
-	/* Step 12: Wait 1 uS. */
-	udelay(1);
-
-	/* Step 13: Program the HRESET_N field: UCTL0_CLK_RST_CTL[HRST] = 1 */
-	clk_rst_ctl.s.hrst = 1;
-	cvmx_write_csr(CVMX_UCTLX_CLK_RST_CTL(0), clk_rst_ctl.u64);
-
-end_clock:
-	/* Now we can set some other registers.  */
-
-	for (i = 0; i <= 1; i++) {
-		port_ctl_status.u64 =
-			cvmx_read_csr(CVMX_UCTLX_UPHY_PORTX_CTL_STATUS(i, 0));
-		/* Set txvreftune to 15 to obtain compliant 'eye' diagram. */
-		port_ctl_status.s.txvreftune = 15;
-		port_ctl_status.s.txrisetune = 1;
-		port_ctl_status.s.txpreemphasistune = 1;
-		cvmx_write_csr(CVMX_UCTLX_UPHY_PORTX_CTL_STATUS(i, 0),
-			       port_ctl_status.u64);
-	}
-
-	/* Set uSOF cycle period to 60,000 bits. */
-	cvmx_write_csr(CVMX_UCTLX_EHCI_FLA(0), 0x20ull);
-exit:
-	mutex_unlock(&octeon2_usb_clocks_mutex);
-}
-EXPORT_SYMBOL(octeon2_usb_clocks_start);
-
-void octeon2_usb_clocks_stop(void)
-{
-	mutex_lock(&octeon2_usb_clocks_mutex);
-	octeon2_usb_clock_start_cnt--;
-	mutex_unlock(&octeon2_usb_clocks_mutex);
-}
-EXPORT_SYMBOL(octeon2_usb_clocks_stop);
diff --git a/drivers/usb/host/ohci-at91.c b/drivers/usb/host/ohci-at91.c
index e49eb4f90f5d..dc9e4e61f1c8 100644
--- a/drivers/usb/host/ohci-at91.c
+++ b/drivers/usb/host/ohci-at91.c
@@ -24,12 +24,8 @@
 #include <linux/usb.h>
 #include <linux/usb/hcd.h>
 
-#include <mach/hardware.h>
 #include <asm/gpio.h>
 
-#include <mach/cpu.h>
-
-
 #include "ohci.h"
 
 #define valid_port(index)	((index) >= 0 && (index) < AT91_MAX_USBH_PORTS)
@@ -137,12 +133,6 @@ static int usb_hcd_at91_probe(const struct hc_driver *driver,
 	struct resource *res;
 	int irq;
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_dbg(dev, "hcd probe: missing memory resource\n");
-		return -ENXIO;
-	}
-
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0) {
 		dev_dbg(dev, "hcd probe: missing irq resource\n");
@@ -152,14 +142,15 @@ static int usb_hcd_at91_probe(const struct hc_driver *driver,
 	hcd = usb_create_hcd(driver, dev, "at91");
 	if (!hcd)
 		return -ENOMEM;
-	hcd->rsrc_start = res->start;
-	hcd->rsrc_len = resource_size(res);
 
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	hcd->regs = devm_ioremap_resource(dev, res);
 	if (IS_ERR(hcd->regs)) {
 		retval = PTR_ERR(hcd->regs);
 		goto err;
 	}
+	hcd->rsrc_start = res->start;
+	hcd->rsrc_len = resource_size(res);
 
 	iclk = devm_clk_get(dev, "ohci_clk");
 	if (IS_ERR(iclk)) {
@@ -664,7 +655,6 @@ static struct platform_driver ohci_hcd_at91_driver = {
 	.resume		= ohci_hcd_at91_drv_resume,
 	.driver		= {
 		.name	= "at91_ohci",
-		.owner	= THIS_MODULE,
 		.of_match_table	= of_match_ptr(at91_ohci_dt_ids),
 	},
 };
diff --git a/drivers/usb/host/ohci-da8xx.c b/drivers/usb/host/ohci-da8xx.c
index df06be6b47f5..1c76999b2184 100644
--- a/drivers/usb/host/ohci-da8xx.c
+++ b/drivers/usb/host/ohci-da8xx.c
@@ -313,16 +313,13 @@ static int usb_hcd_da8xx_probe(const struct hc_driver *driver,
 		return -ENOMEM;
 
 	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!mem)
-		return -ENODEV;
-	hcd->rsrc_start = mem->start;
-	hcd->rsrc_len = resource_size(mem);
-
 	hcd->regs = devm_ioremap_resource(&pdev->dev, mem);
 	if (IS_ERR(hcd->regs)) {
 		error = PTR_ERR(hcd->regs);
 		goto err;
 	}
+	hcd->rsrc_start = mem->start;
+	hcd->rsrc_len = resource_size(mem);
 
 	ohci_hcd_init(hcd_to_ohci(hcd));
 
diff --git a/drivers/usb/host/ohci-exynos.c b/drivers/usb/host/ohci-exynos.c
index d28b6583ba02..2cd105be7319 100644
--- a/drivers/usb/host/ohci-exynos.c
+++ b/drivers/usb/host/ohci-exynos.c
@@ -63,7 +63,6 @@ static int exynos_ohci_get_phy(struct device *dev,
 
 		phy = devm_of_phy_get(dev, child, NULL);
 		exynos_ohci->phy[phy_number] = phy;
-		of_node_put(child);
 		if (IS_ERR(phy)) {
 			ret = PTR_ERR(phy);
 			if (ret == -EPROBE_DEFER) {
@@ -156,19 +155,13 @@ skip_phy:
 		goto fail_clk;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "Failed to get I/O memory\n");
-		err = -ENXIO;
-		goto fail_io;
-	}
-
-	hcd->rsrc_start = res->start;
-	hcd->rsrc_len = resource_size(res);
 	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(hcd->regs)) {
 		err = PTR_ERR(hcd->regs);
 		goto fail_io;
 	}
+	hcd->rsrc_start = res->start;
+	hcd->rsrc_len = resource_size(res);
 
 	irq = platform_get_irq(pdev, 0);
 	if (!irq) {
@@ -292,7 +285,6 @@ static struct platform_driver exynos_ohci_driver = {
 	.shutdown	= exynos_ohci_shutdown,
 	.driver = {
 		.name	= "exynos-ohci",
-		.owner	= THIS_MODULE,
 		.pm	= &exynos_ohci_pm_ops,
 		.of_match_table	= of_match_ptr(exynos_ohci_match),
 	}
diff --git a/drivers/usb/host/ohci-hcd.c b/drivers/usb/host/ohci-hcd.c
index d664edabf14e..1dab9dfbca6a 100644
--- a/drivers/usb/host/ohci-hcd.c
+++ b/drivers/usb/host/ohci-hcd.c
@@ -1249,11 +1249,6 @@ MODULE_LICENSE ("GPL");
 #define PLATFORM_DRIVER	ohci_hcd_jz4740_driver
 #endif
 
-#ifdef CONFIG_USB_OCTEON_OHCI
-#include "ohci-octeon.c"
-#define PLATFORM_DRIVER		ohci_octeon_driver
-#endif
-
 #ifdef CONFIG_TILE_USB
 #include "ohci-tilegx.c"
 #define PLATFORM_DRIVER		ohci_hcd_tilegx_driver
diff --git a/drivers/usb/host/ohci-jz4740.c b/drivers/usb/host/ohci-jz4740.c
index c2c221a332eb..8ddd8f5470cb 100644
--- a/drivers/usb/host/ohci-jz4740.c
+++ b/drivers/usb/host/ohci-jz4740.c
@@ -153,13 +153,6 @@ static int jz4740_ohci_probe(struct platform_device *pdev)
 	struct resource *res;
 	int irq;
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-
-	if (!res) {
-		dev_err(&pdev->dev, "Failed to get platform resource\n");
-		return -ENOENT;
-	}
-
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0) {
 		dev_err(&pdev->dev, "Failed to get platform irq\n");
@@ -174,14 +167,14 @@ static int jz4740_ohci_probe(struct platform_device *pdev)
 
 	jz4740_ohci = hcd_to_jz4740_hcd(hcd);
 
-	hcd->rsrc_start = res->start;
-	hcd->rsrc_len = resource_size(res);
-
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(hcd->regs)) {
 		ret = PTR_ERR(hcd->regs);
 		goto err_free;
 	}
+	hcd->rsrc_start = res->start;
+	hcd->rsrc_len = resource_size(res);
 
 	jz4740_ohci->clk = devm_clk_get(&pdev->dev, "uhc");
 	if (IS_ERR(jz4740_ohci->clk)) {
diff --git a/drivers/usb/host/ohci-nxp.c b/drivers/usb/host/ohci-nxp.c
index ba180ed0f81c..d9f0481d7258 100644
--- a/drivers/usb/host/ohci-nxp.c
+++ b/drivers/usb/host/ohci-nxp.c
@@ -322,7 +322,6 @@ MODULE_DEVICE_TABLE(of, ohci_hcd_nxp_match);
 static struct platform_driver ohci_hcd_nxp_driver = {
 	.driver = {
 		.name = "usb-ohci",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(ohci_hcd_nxp_match),
 	},
 	.probe = ohci_hcd_nxp_probe,
diff --git a/drivers/usb/host/ohci-octeon.c b/drivers/usb/host/ohci-octeon.c
deleted file mode 100644
index 15af8954085e..000000000000
--- a/drivers/usb/host/ohci-octeon.c
+++ /dev/null
@@ -1,202 +0,0 @@
-/*
- * EHCI HCD glue for Cavium Octeon II SOCs.
- *
- * Loosely based on ehci-au1xxx.c
- *
- * 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) 2010 Cavium Networks
- *
- */
-
-#include <linux/platform_device.h>
-
-#include <asm/octeon/octeon.h>
-#include <asm/octeon/cvmx-uctlx-defs.h>
-
-#define OCTEON_OHCI_HCD_NAME "octeon-ohci"
-
-/* Common clock init code.  */
-void octeon2_usb_clocks_start(void);
-void octeon2_usb_clocks_stop(void);
-
-static void ohci_octeon_hw_start(void)
-{
-	union cvmx_uctlx_ohci_ctl ohci_ctl;
-
-	octeon2_usb_clocks_start();
-
-	ohci_ctl.u64 = cvmx_read_csr(CVMX_UCTLX_OHCI_CTL(0));
-	ohci_ctl.s.l2c_addr_msb = 0;
-	ohci_ctl.s.l2c_buff_emod = 1; /* Byte swapped. */
-	ohci_ctl.s.l2c_desc_emod = 1; /* Byte swapped. */
-	cvmx_write_csr(CVMX_UCTLX_OHCI_CTL(0), ohci_ctl.u64);
-
-}
-
-static void ohci_octeon_hw_stop(void)
-{
-	/* Undo ohci_octeon_start() */
-	octeon2_usb_clocks_stop();
-}
-
-static int ohci_octeon_start(struct usb_hcd *hcd)
-{
-	struct ohci_hcd	*ohci = hcd_to_ohci(hcd);
-	int ret;
-
-	ret = ohci_init(ohci);
-
-	if (ret < 0)
-		return ret;
-
-	ret = ohci_run(ohci);
-
-	if (ret < 0) {
-		ohci_err(ohci, "can't start %s", hcd->self.bus_name);
-		ohci_stop(hcd);
-		return ret;
-	}
-
-	return 0;
-}
-
-static const struct hc_driver ohci_octeon_hc_driver = {
-	.description		= hcd_name,
-	.product_desc		= "Octeon OHCI",
-	.hcd_priv_size		= sizeof(struct ohci_hcd),
-
-	/*
-	 * generic hardware linkage
-	 */
-	.irq =			ohci_irq,
-	.flags =		HCD_USB11 | HCD_MEMORY,
-
-	/*
-	 * basic lifecycle operations
-	 */
-	.start =		ohci_octeon_start,
-	.stop =			ohci_stop,
-	.shutdown =		ohci_shutdown,
-
-	/*
-	 * managing i/o requests and associated device resources
-	 */
-	.urb_enqueue =		ohci_urb_enqueue,
-	.urb_dequeue =		ohci_urb_dequeue,
-	.endpoint_disable =	ohci_endpoint_disable,
-
-	/*
-	 * scheduling support
-	 */
-	.get_frame_number =	ohci_get_frame,
-
-	/*
-	 * root hub support
-	 */
-	.hub_status_data =	ohci_hub_status_data,
-	.hub_control =		ohci_hub_control,
-
-	.start_port_reset =	ohci_start_port_reset,
-};
-
-static int ohci_octeon_drv_probe(struct platform_device *pdev)
-{
-	struct usb_hcd *hcd;
-	struct ohci_hcd *ohci;
-	void *reg_base;
-	struct resource *res_mem;
-	int irq;
-	int ret;
-
-	if (usb_disabled())
-		return -ENODEV;
-
-	irq = platform_get_irq(pdev, 0);
-	if (irq < 0) {
-		dev_err(&pdev->dev, "No irq assigned\n");
-		return -ENODEV;
-	}
-
-	res_mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (res_mem == NULL) {
-		dev_err(&pdev->dev, "No register space assigned\n");
-		return -ENODEV;
-	}
-
-	/* Ohci is a 32-bit device. */
-	ret = dma_coerce_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32));
-	if (ret)
-		return ret;
-
-	hcd = usb_create_hcd(&ohci_octeon_hc_driver, &pdev->dev, "octeon");
-	if (!hcd)
-		return -ENOMEM;
-
-	hcd->rsrc_start = res_mem->start;
-	hcd->rsrc_len = resource_size(res_mem);
-
-	reg_base = devm_ioremap_resource(&pdev->dev, res_mem);
-	if (IS_ERR(reg_base)) {
-		ret = PTR_ERR(reg_base);
-		goto err1;
-	}
-
-	ohci_octeon_hw_start();
-
-	hcd->regs = reg_base;
-
-	ohci = hcd_to_ohci(hcd);
-
-	/* Octeon OHCI matches CPU endianness. */
-#ifdef __BIG_ENDIAN
-	ohci->flags |= OHCI_QUIRK_BE_MMIO;
-#endif
-
-	ohci_hcd_init(ohci);
-
-	ret = usb_add_hcd(hcd, irq, IRQF_SHARED);
-	if (ret) {
-		dev_dbg(&pdev->dev, "failed to add hcd with err %d\n", ret);
-		goto err2;
-	}
-
-	device_wakeup_enable(hcd->self.controller);
-
-	platform_set_drvdata(pdev, hcd);
-
-	return 0;
-
-err2:
-	ohci_octeon_hw_stop();
-
-err1:
-	usb_put_hcd(hcd);
-	return ret;
-}
-
-static int ohci_octeon_drv_remove(struct platform_device *pdev)
-{
-	struct usb_hcd *hcd = platform_get_drvdata(pdev);
-
-	usb_remove_hcd(hcd);
-
-	ohci_octeon_hw_stop();
-	usb_put_hcd(hcd);
-
-	return 0;
-}
-
-static struct platform_driver ohci_octeon_driver = {
-	.probe		= ohci_octeon_drv_probe,
-	.remove		= ohci_octeon_drv_remove,
-	.shutdown	= usb_hcd_platform_shutdown,
-	.driver = {
-		.name	= OCTEON_OHCI_HCD_NAME,
-		.owner	= THIS_MODULE,
-	}
-};
-
-MODULE_ALIAS("platform:" OCTEON_OHCI_HCD_NAME);
diff --git a/drivers/usb/host/ohci-omap.c b/drivers/usb/host/ohci-omap.c
index 0231606d47c2..de7c68602a7e 100644
--- a/drivers/usb/host/ohci-omap.c
+++ b/drivers/usb/host/ohci-omap.c
@@ -183,7 +183,7 @@ static void start_hnp(struct ohci_hcd *ohci)
 	otg_start_hnp(hcd->usb_phy->otg);
 
 	local_irq_save(flags);
-	hcd->usb_phy->state = OTG_STATE_A_SUSPEND;
+	hcd->usb_phy->otg->state = OTG_STATE_A_SUSPEND;
 	writel (RH_PS_PSS, &ohci->regs->roothub.portstatus [port]);
 	l = omap_readl(OTG_CTRL);
 	l &= ~OTG_A_BUSREQ;
@@ -481,7 +481,6 @@ static struct platform_driver ohci_hcd_omap_driver = {
 	.resume		= ohci_omap_resume,
 #endif
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "ohci",
 	},
 };
diff --git a/drivers/usb/host/ohci-platform.c b/drivers/usb/host/ohci-platform.c
index 4369299064c7..b81d202b15a2 100644
--- a/drivers/usb/host/ohci-platform.c
+++ b/drivers/usb/host/ohci-platform.c
@@ -43,20 +43,6 @@ struct ohci_platform_priv {
 
 static const char hcd_name[] = "ohci-platform";
 
-static int ohci_platform_reset(struct usb_hcd *hcd)
-{
-	struct platform_device *pdev = to_platform_device(hcd->self.controller);
-	struct usb_ohci_pdata *pdata = dev_get_platdata(&pdev->dev);
-	struct ohci_hcd *ohci = hcd_to_ohci(hcd);
-
-	if (pdata->no_big_frame_no)
-		ohci->flags |= OHCI_QUIRK_FRAME_NO;
-	if (pdata->num_ports)
-		ohci->num_ports = pdata->num_ports;
-
-	return ohci_setup(hcd);
-}
-
 static int ohci_platform_power_on(struct platform_device *dev)
 {
 	struct usb_hcd *hcd = platform_get_drvdata(dev);
@@ -110,7 +96,6 @@ static struct hc_driver __read_mostly ohci_platform_hc_driver;
 
 static const struct ohci_driver_overrides platform_overrides __initconst = {
 	.product_desc =		"Generic Platform OHCI controller",
-	.reset =		ohci_platform_reset,
 	.extra_priv_size =	sizeof(struct ohci_platform_priv),
 };
 
@@ -149,12 +134,6 @@ static int ohci_platform_probe(struct platform_device *dev)
 		return irq;
 	}
 
-	res_mem = platform_get_resource(dev, IORESOURCE_MEM, 0);
-	if (!res_mem) {
-		dev_err(&dev->dev, "no memory resource provided");
-		return -ENXIO;
-	}
-
 	hcd = usb_create_hcd(&ohci_platform_hc_driver, &dev->dev,
 			dev_name(&dev->dev));
 	if (!hcd)
@@ -175,6 +154,12 @@ static int ohci_platform_probe(struct platform_device *dev)
 		if (of_property_read_bool(dev->dev.of_node, "big-endian"))
 			ohci->flags |= OHCI_QUIRK_BE_MMIO | OHCI_QUIRK_BE_DESC;
 
+		if (of_property_read_bool(dev->dev.of_node, "no-big-frame-no"))
+			ohci->flags |= OHCI_QUIRK_FRAME_NO;
+
+		of_property_read_u32(dev->dev.of_node, "num-ports",
+				     &ohci->num_ports);
+
 		priv->phy = devm_phy_get(&dev->dev, "usb");
 		if (IS_ERR(priv->phy)) {
 			err = PTR_ERR(priv->phy);
@@ -212,6 +197,10 @@ static int ohci_platform_probe(struct platform_device *dev)
 		ohci->flags |= OHCI_QUIRK_BE_DESC;
 	if (pdata->big_endian_mmio)
 		ohci->flags |= OHCI_QUIRK_BE_MMIO;
+	if (pdata->no_big_frame_no)
+		ohci->flags |= OHCI_QUIRK_FRAME_NO;
+	if (pdata->num_ports)
+		ohci->num_ports = pdata->num_ports;
 
 #ifndef CONFIG_USB_OHCI_BIG_ENDIAN_MMIO
 	if (ohci->flags & OHCI_QUIRK_BE_MMIO) {
@@ -236,14 +225,15 @@ static int ohci_platform_probe(struct platform_device *dev)
 			goto err_reset;
 	}
 
-	hcd->rsrc_start = res_mem->start;
-	hcd->rsrc_len = resource_size(res_mem);
-
+	res_mem = platform_get_resource(dev, IORESOURCE_MEM, 0);
 	hcd->regs = devm_ioremap_resource(&dev->dev, res_mem);
 	if (IS_ERR(hcd->regs)) {
 		err = PTR_ERR(hcd->regs);
 		goto err_power;
 	}
+	hcd->rsrc_start = res_mem->start;
+	hcd->rsrc_len = resource_size(res_mem);
+
 	err = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (err)
 		goto err_power;
@@ -298,8 +288,7 @@ static int ohci_platform_remove(struct platform_device *dev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-
+#ifdef CONFIG_PM_SLEEP
 static int ohci_platform_suspend(struct device *dev)
 {
 	struct usb_hcd *hcd = dev_get_drvdata(dev);
@@ -335,11 +324,7 @@ static int ohci_platform_resume(struct device *dev)
 	ohci_resume(hcd, false);
 	return 0;
 }
-
-#else /* !CONFIG_PM */
-#define ohci_platform_suspend	NULL
-#define ohci_platform_resume	NULL
-#endif /* CONFIG_PM */
+#endif /* CONFIG_PM_SLEEP */
 
 static const struct of_device_id ohci_platform_ids[] = {
 	{ .compatible = "generic-ohci", },
@@ -353,10 +338,8 @@ static const struct platform_device_id ohci_platform_table[] = {
 };
 MODULE_DEVICE_TABLE(platform, ohci_platform_table);
 
-static const struct dev_pm_ops ohci_platform_pm_ops = {
-	.suspend	= ohci_platform_suspend,
-	.resume		= ohci_platform_resume,
-};
+static SIMPLE_DEV_PM_OPS(ohci_platform_pm_ops, ohci_platform_suspend,
+	ohci_platform_resume);
 
 static struct platform_driver ohci_platform_driver = {
 	.id_table	= ohci_platform_table,
@@ -364,7 +347,6 @@ static struct platform_driver ohci_platform_driver = {
 	.remove		= ohci_platform_remove,
 	.shutdown	= usb_hcd_platform_shutdown,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "ohci-platform",
 		.pm	= &ohci_platform_pm_ops,
 		.of_match_table = ohci_platform_ids,
diff --git a/drivers/usb/host/ohci-pxa27x.c b/drivers/usb/host/ohci-pxa27x.c
index e68f3d02cd1a..ba1bec7db026 100644
--- a/drivers/usb/host/ohci-pxa27x.c
+++ b/drivers/usb/host/ohci-pxa27x.c
@@ -447,20 +447,13 @@ int usb_hcd_pxa27x_probe (const struct hc_driver *driver, struct platform_device
 		return -ENOMEM;
 
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!r) {
-		pr_err("no resource of IORESOURCE_MEM");
-		retval = -ENXIO;
-		goto err;
-	}
-
-	hcd->rsrc_start = r->start;
-	hcd->rsrc_len = resource_size(r);
-
 	hcd->regs = devm_ioremap_resource(&pdev->dev, r);
 	if (IS_ERR(hcd->regs)) {
 		retval = PTR_ERR(hcd->regs);
 		goto err;
 	}
+	hcd->rsrc_start = r->start;
+	hcd->rsrc_len = resource_size(r);
 
 	/* initialize "struct pxa27x_ohci" */
 	pxa_ohci = to_pxa27x_ohci(hcd);
@@ -610,7 +603,6 @@ static struct platform_driver ohci_hcd_pxa27x_driver = {
 	.shutdown	= usb_hcd_platform_shutdown,
 	.driver		= {
 		.name	= "pxa27x-ohci",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(pxa_ohci_dt_ids),
 #ifdef CONFIG_PM
 		.pm	= &ohci_hcd_pxa27x_pm_ops,
diff --git a/drivers/usb/host/ohci-s3c2410.c b/drivers/usb/host/ohci-s3c2410.c
index 3d753a9d3141..095113ea1fcb 100644
--- a/drivers/usb/host/ohci-s3c2410.c
+++ b/drivers/usb/host/ohci-s3c2410.c
@@ -462,7 +462,6 @@ static struct platform_driver ohci_hcd_s3c2410_driver = {
 	.remove		= ohci_hcd_s3c2410_drv_remove,
 	.shutdown	= usb_hcd_platform_shutdown,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "s3c2410-ohci",
 		.pm	= &ohci_hcd_s3c2410_pm_ops,
 	},
diff --git a/drivers/usb/host/ohci-spear.c b/drivers/usb/host/ohci-spear.c
index 8d5876692e7c..707437c88d03 100644
--- a/drivers/usb/host/ohci-spear.c
+++ b/drivers/usb/host/ohci-spear.c
@@ -74,20 +74,15 @@ static int spear_ohci_hcd_drv_probe(struct platform_device *pdev)
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		retval = -ENODEV;
-		goto err_put_hcd;
-	}
-
-	hcd->rsrc_start = pdev->resource[0].start;
-	hcd->rsrc_len = resource_size(res);
-
 	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(hcd->regs)) {
 		retval = PTR_ERR(hcd->regs);
 		goto err_put_hcd;
 	}
 
+	hcd->rsrc_start = pdev->resource[0].start;
+	hcd->rsrc_len = resource_size(res);
+
 	sohci_p = to_spear_ohci(hcd);
 	sohci_p->clk = usbh_clk;
 
@@ -176,7 +171,6 @@ static struct platform_driver spear_ohci_hcd_driver = {
 	.resume =	spear_ohci_hcd_drv_resume,
 #endif
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "spear-ohci",
 		.of_match_table = spear_ohci_id_table,
 	},
diff --git a/drivers/usb/host/ohci.h b/drivers/usb/host/ohci.h
index 59f424567a8d..bc462288cfb0 100644
--- a/drivers/usb/host/ohci.h
+++ b/drivers/usb/host/ohci.h
@@ -647,23 +647,22 @@ static inline u32 hc32_to_cpup (const struct ohci_hcd *ohci, const __hc32 *x)
 
 /*-------------------------------------------------------------------------*/
 
-/* HCCA frame number is 16 bits, but is accessed as 32 bits since not all
- * hardware handles 16 bit reads.  That creates a different confusion on
- * some big-endian SOC implementations.  Same thing happens with PSW access.
+/*
+ * The HCCA frame number is 16 bits, but is accessed as 32 bits since not all
+ * hardware handles 16 bit reads.  Depending on the SoC implementation, the
+ * frame number can wind up in either bits [31:16] (default) or
+ * [15:0] (OHCI_QUIRK_FRAME_NO) on big endian hosts.
+ *
+ * Somewhat similarly, the 16-bit PSW fields in a transfer descriptor are
+ * reordered on BE.
  */
 
-#ifdef CONFIG_PPC_MPC52xx
-#define big_endian_frame_no_quirk(ohci)	(ohci->flags & OHCI_QUIRK_FRAME_NO)
-#else
-#define big_endian_frame_no_quirk(ohci)	0
-#endif
-
 static inline u16 ohci_frame_no(const struct ohci_hcd *ohci)
 {
 	u32 tmp;
 	if (big_endian_desc(ohci)) {
 		tmp = be32_to_cpup((__force __be32 *)&ohci->hcca->frame_no);
-		if (!big_endian_frame_no_quirk(ohci))
+		if (!(ohci->flags & OHCI_QUIRK_FRAME_NO))
 			tmp >>= 16;
 	} else
 		tmp = le32_to_cpup((__force __le32 *)&ohci->hcca->frame_no);
diff --git a/drivers/usb/host/oxu210hp-hcd.c b/drivers/usb/host/oxu210hp-hcd.c
index 4fe79a2d71a9..036924e640f5 100644
--- a/drivers/usb/host/oxu210hp-hcd.c
+++ b/drivers/usb/host/oxu210hp-hcd.c
@@ -3087,7 +3087,7 @@ static int oxu_hub_status_data(struct usb_hcd *hcd, char *buf)
 	int ports, i, retval = 1;
 	unsigned long flags;
 
-	/* if !PM_RUNTIME, root hub timers won't get shut down ... */
+	/* if !PM, root hub timers won't get shut down ... */
 	if (!HC_IS_RUNNING(hcd->state))
 		return 0;
 
@@ -3846,7 +3846,6 @@ static int oxu_drv_probe(struct platform_device *pdev)
 	 */
 	info = devm_kzalloc(&pdev->dev, sizeof(struct oxu_info), GFP_KERNEL);
 	if (!info) {
-		dev_dbg(&pdev->dev, "error allocating memory\n");
 		ret = -EFAULT;
 		goto error;
 	}
diff --git a/drivers/usb/host/pci-quirks.c b/drivers/usb/host/pci-quirks.c
index 2f3acebb577a..dd483c13565b 100644
--- a/drivers/usb/host/pci-quirks.c
+++ b/drivers/usb/host/pci-quirks.c
@@ -233,10 +233,8 @@ commit:
 
 		spin_unlock_irqrestore(&amd_lock, flags);
 
-		if (info.nb_dev)
-			pci_dev_put(info.nb_dev);
-		if (info.smbus_dev)
-			pci_dev_put(info.smbus_dev);
+		pci_dev_put(info.nb_dev);
+		pci_dev_put(info.smbus_dev);
 
 	} else {
 		/* no race - commit the result */
@@ -447,10 +445,8 @@ void usb_amd_dev_put(void)
 
 	spin_unlock_irqrestore(&amd_lock, flags);
 
-	if (nb)
-		pci_dev_put(nb);
-	if (smbus)
-		pci_dev_put(smbus);
+	pci_dev_put(nb);
+	pci_dev_put(smbus);
 }
 EXPORT_SYMBOL_GPL(usb_amd_dev_put);
 
diff --git a/drivers/usb/host/r8a66597-hcd.c b/drivers/usb/host/r8a66597-hcd.c
index 110b4b9ebeaa..c4bcfaedeec9 100644
--- a/drivers/usb/host/r8a66597-hcd.c
+++ b/drivers/usb/host/r8a66597-hcd.c
@@ -2535,7 +2535,6 @@ static struct platform_driver r8a66597_driver = {
 	.remove =	r8a66597_remove,
 	.driver		= {
 		.name = hcd_name,
-		.owner	= THIS_MODULE,
 		.pm	= R8A66597_DEV_PM_OPS,
 	},
 };
diff --git a/drivers/usb/host/u132-hcd.c b/drivers/usb/host/u132-hcd.c
index bf86630b3cea..2894e54e5b9c 100644
--- a/drivers/usb/host/u132-hcd.c
+++ b/drivers/usb/host/u132-hcd.c
@@ -3218,7 +3218,6 @@ static struct platform_driver u132_platform_driver = {
 	.resume = u132_resume,
 	.driver = {
 		   .name = hcd_name,
-		   .owner = THIS_MODULE,
 		   },
 };
 static int __init u132_hcd_init(void)
diff --git a/drivers/usb/host/uhci-platform.c b/drivers/usb/host/uhci-platform.c
index b987f1d10058..cf8f46003f62 100644
--- a/drivers/usb/host/uhci-platform.c
+++ b/drivers/usb/host/uhci-platform.c
@@ -86,14 +86,14 @@ static int uhci_hcd_platform_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	hcd->rsrc_start = res->start;
-	hcd->rsrc_len = resource_size(res);
-
 	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(hcd->regs)) {
 		ret = PTR_ERR(hcd->regs);
 		goto err_rmr;
 	}
+	hcd->rsrc_start = res->start;
+	hcd->rsrc_len = resource_size(res);
+
 	uhci = hcd_to_uhci(hcd);
 
 	uhci->regs = hcd->regs;
diff --git a/drivers/usb/host/xhci-plat.c b/drivers/usb/host/xhci-plat.c
index 646300cbe5f7..08d402b15482 100644
--- a/drivers/usb/host/xhci-plat.c
+++ b/drivers/usb/host/xhci-plat.c
@@ -83,9 +83,6 @@ static int xhci_plat_probe(struct platform_device *pdev)
 	if (irq < 0)
 		return -ENODEV;
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res)
-		return -ENODEV;
 
 	if (of_device_is_compatible(pdev->dev.of_node,
 				    "marvell,armada-375-xhci") ||
@@ -109,15 +106,16 @@ static int xhci_plat_probe(struct platform_device *pdev)
 	if (!hcd)
 		return -ENOMEM;
 
-	hcd->rsrc_start = res->start;
-	hcd->rsrc_len = resource_size(res);
-
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(hcd->regs)) {
 		ret = PTR_ERR(hcd->regs);
 		goto put_hcd;
 	}
 
+	hcd->rsrc_start = res->start;
+	hcd->rsrc_len = resource_size(res);
+
 	/*
 	 * Not all platforms have a clk so it is not an error if the
 	 * clock does not exists.
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index 06433aec81d7..e692e769c50c 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -716,9 +716,7 @@ remove_finished_td:
 		ring_doorbell_for_active_rings(xhci, slot_id, ep_index);
 	}
 
-	/* Clear stopped_td if endpoint is not halted */
-	if (!(ep->ep_state & EP_HALTED))
-		ep->stopped_td = NULL;
+	ep->stopped_td = NULL;
 
 	/*
 	 * Drop the lock and complete the URBs in the cancelled TD list.
@@ -1732,13 +1730,11 @@ static void xhci_cleanup_halted_endpoint(struct xhci_hcd *xhci,
 		return;
 
 	ep->ep_state |= EP_HALTED;
-	ep->stopped_td = td;
 	ep->stopped_stream = stream_id;
 
 	xhci_queue_reset_ep(xhci, command, slot_id, ep_index);
-	xhci_cleanup_stalled_ring(xhci, td->urb->dev, ep_index);
+	xhci_cleanup_stalled_ring(xhci, ep_index, td);
 
-	ep->stopped_td = NULL;
 	ep->stopped_stream = 0;
 
 	xhci_ring_cmd_db(xhci);
@@ -1813,72 +1809,65 @@ static int finish_td(struct xhci_hcd *xhci, struct xhci_td *td,
 	if (skip)
 		goto td_cleanup;
 
-	if (trb_comp_code == COMP_STOP_INVAL ||
-			trb_comp_code == COMP_STOP) {
+	if (trb_comp_code == COMP_STOP_INVAL || trb_comp_code == COMP_STOP) {
 		/* The Endpoint Stop Command completion will take care of any
 		 * stopped TDs.  A stopped TD may be restarted, so don't update
 		 * the ring dequeue pointer or take this TD off any lists yet.
 		 */
 		ep->stopped_td = td;
 		return 0;
+	}
+	if (trb_comp_code == COMP_STALL ||
+		xhci_requires_manual_halt_cleanup(xhci, ep_ctx,
+						trb_comp_code)) {
+		/* Issue a reset endpoint command to clear the host side
+		 * halt, followed by a set dequeue command to move the
+		 * dequeue pointer past the TD.
+		 * The class driver clears the device side halt later.
+		 */
+		xhci_cleanup_halted_endpoint(xhci, slot_id, ep_index,
+					ep_ring->stream_id, td, event_trb);
 	} else {
-		if (trb_comp_code == COMP_STALL ||
-		    xhci_requires_manual_halt_cleanup(xhci, ep_ctx,
-						      trb_comp_code)) {
-			/* Issue a reset endpoint command to clear the host side
-			 * halt, followed by a set dequeue command to move the
-			 * dequeue pointer past the TD.
-			 * The class driver clears the device side halt later.
-			 */
-			xhci_cleanup_halted_endpoint(xhci,
-					slot_id, ep_index, ep_ring->stream_id,
-					td, event_trb);
-		} else {
-			/* Update ring dequeue pointer */
-			while (ep_ring->dequeue != td->last_trb)
-				inc_deq(xhci, ep_ring);
+		/* Update ring dequeue pointer */
+		while (ep_ring->dequeue != td->last_trb)
 			inc_deq(xhci, ep_ring);
-		}
+		inc_deq(xhci, ep_ring);
+	}
 
 td_cleanup:
-		/* Clean up the endpoint's TD list */
-		urb = td->urb;
-		urb_priv = urb->hcpriv;
-
-		/* Do one last check of the actual transfer length.
-		 * If the host controller said we transferred more data than
-		 * the buffer length, urb->actual_length will be a very big
-		 * number (since it's unsigned).  Play it safe and say we didn't
-		 * transfer anything.
-		 */
-		if (urb->actual_length > urb->transfer_buffer_length) {
-			xhci_warn(xhci, "URB transfer length is wrong, "
-					"xHC issue? req. len = %u, "
-					"act. len = %u\n",
-					urb->transfer_buffer_length,
-					urb->actual_length);
-			urb->actual_length = 0;
-			if (td->urb->transfer_flags & URB_SHORT_NOT_OK)
-				*status = -EREMOTEIO;
-			else
-				*status = 0;
-		}
-		list_del_init(&td->td_list);
-		/* Was this TD slated to be cancelled but completed anyway? */
-		if (!list_empty(&td->cancelled_td_list))
-			list_del_init(&td->cancelled_td_list);
-
-		urb_priv->td_cnt++;
-		/* Giveback the urb when all the tds are completed */
-		if (urb_priv->td_cnt == urb_priv->length) {
-			ret = 1;
-			if (usb_pipetype(urb->pipe) == PIPE_ISOCHRONOUS) {
-				xhci_to_hcd(xhci)->self.bandwidth_isoc_reqs--;
-				if (xhci_to_hcd(xhci)->self.bandwidth_isoc_reqs
-					== 0) {
-					if (xhci->quirks & XHCI_AMD_PLL_FIX)
-						usb_amd_quirk_pll_enable();
-				}
+	/* Clean up the endpoint's TD list */
+	urb = td->urb;
+	urb_priv = urb->hcpriv;
+
+	/* Do one last check of the actual transfer length.
+	 * If the host controller said we transferred more data than the buffer
+	 * length, urb->actual_length will be a very big number (since it's
+	 * unsigned).  Play it safe and say we didn't transfer anything.
+	 */
+	if (urb->actual_length > urb->transfer_buffer_length) {
+		xhci_warn(xhci, "URB transfer length is wrong, xHC issue? req. len = %u, act. len = %u\n",
+			urb->transfer_buffer_length,
+			urb->actual_length);
+		urb->actual_length = 0;
+		if (td->urb->transfer_flags & URB_SHORT_NOT_OK)
+			*status = -EREMOTEIO;
+		else
+			*status = 0;
+	}
+	list_del_init(&td->td_list);
+	/* Was this TD slated to be cancelled but completed anyway? */
+	if (!list_empty(&td->cancelled_td_list))
+		list_del_init(&td->cancelled_td_list);
+
+	urb_priv->td_cnt++;
+	/* Giveback the urb when all the tds are completed */
+	if (urb_priv->td_cnt == urb_priv->length) {
+		ret = 1;
+		if (usb_pipetype(urb->pipe) == PIPE_ISOCHRONOUS) {
+			xhci_to_hcd(xhci)->self.bandwidth_isoc_reqs--;
+			if (xhci_to_hcd(xhci)->self.bandwidth_isoc_reqs == 0) {
+				if (xhci->quirks & XHCI_AMD_PLL_FIX)
+					usb_amd_quirk_pll_enable();
 			}
 		}
 	}
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index cf3413116aff..01fcbb5eb06e 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -2912,10 +2912,11 @@ static void xhci_setup_input_ctx_for_quirk(struct xhci_hcd *xhci,
 }
 
 void xhci_cleanup_stalled_ring(struct xhci_hcd *xhci,
-		struct usb_device *udev, unsigned int ep_index)
+			unsigned int ep_index, struct xhci_td *td)
 {
 	struct xhci_dequeue_state deq_state;
 	struct xhci_virt_ep *ep;
+	struct usb_device *udev = td->urb->dev;
 
 	xhci_dbg_trace(xhci, trace_xhci_dbg_reset_ep,
 			"Cleaning up stalled endpoint ring");
@@ -2924,8 +2925,7 @@ void xhci_cleanup_stalled_ring(struct xhci_hcd *xhci,
 	 * or it will attempt to resend it on the next doorbell ring.
 	 */
 	xhci_find_new_dequeue_state(xhci, udev->slot_id,
-			ep_index, ep->stopped_stream, ep->stopped_td,
-			&deq_state);
+			ep_index, ep->stopped_stream, td, &deq_state);
 
 	if (!deq_state.new_deq_ptr || !deq_state.new_deq_seg)
 		return;
@@ -4009,6 +4009,7 @@ static int __maybe_unused xhci_change_max_exit_latency(struct xhci_hcd *xhci,
 	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);
+	slot_ctx->dev_state = 0;
 
 	xhci_dbg_trace(xhci, trace_xhci_dbg_context_change,
 			"Set up evaluate context for LPM MEL change.");
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index d745715a1e2f..cc7c5bb7cbcf 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -358,7 +358,7 @@ struct xhci_op_regs {
 /* wake on over-current (enable) */
 #define PORT_WKOC_E	(1 << 27)
 /* bits 28:29 reserved */
-/* true: device is removable - for USB 3.0 roothub emulation */
+/* true: device is non-removable - for USB 3.0 roothub emulation */
 #define PORT_DEV_REMOVE	(1 << 30)
 /* Initiate a warm port reset - complete when PORT_WRC is '1' */
 #define PORT_WR		(1 << 31)
@@ -1825,7 +1825,7 @@ void xhci_queue_new_dequeue_state(struct xhci_hcd *xhci,
 		unsigned int stream_id,
 		struct xhci_dequeue_state *deq_state);
 void xhci_cleanup_stalled_ring(struct xhci_hcd *xhci,
-		struct usb_device *udev, unsigned int ep_index);
+		unsigned int ep_index, struct xhci_td *td);
 void xhci_queue_config_ep_quirk(struct xhci_hcd *xhci,
 		unsigned int slot_id, unsigned int ep_index,
 		struct xhci_dequeue_state *deq_state);
diff --git a/drivers/usb/misc/adutux.c b/drivers/usb/misc/adutux.c
index 493c7f268b6f..3071c0ef909b 100644
--- a/drivers/usb/misc/adutux.c
+++ b/drivers/usb/misc/adutux.c
@@ -814,15 +814,10 @@ static void adu_disconnect(struct usb_interface *interface)
 	usb_set_intfdata(interface, NULL);
 
 	/* if the device is not opened, then we clean up right now */
-	dev_dbg(&dev->udev->dev, "%s : open count %d\n",
-		__func__, dev->open_count);
 	if (!dev->open_count)
 		adu_delete(dev);
 
 	mutex_unlock(&adutux_mutex);
-
-	dev_info(&interface->dev, "ADU device adutux%d now disconnected\n",
-		 (minor - ADU_MINOR_BASE));
 }
 
 /* usb specific object needed to register this driver with the usb subsystem */
diff --git a/drivers/usb/misc/sisusbvga/sisusb.c b/drivers/usb/misc/sisusbvga/sisusb.c
index 633caf643122..022dc0008f2a 100644
--- a/drivers/usb/misc/sisusbvga/sisusb.c
+++ b/drivers/usb/misc/sisusbvga/sisusb.c
@@ -2472,8 +2472,7 @@ sisusb_delete(struct kref *kref)
 	if (!sisusb)
 		return;
 
-	if (sisusb->sisusb_dev)
-		usb_put_dev(sisusb->sisusb_dev);
+	usb_put_dev(sisusb->sisusb_dev);
 
 	sisusb->sisusb_dev = NULL;
 	sisusb_free_buffers(sisusb);
diff --git a/drivers/usb/misc/usb3503.c b/drivers/usb/misc/usb3503.c
index ae7e1206ca54..258d2f546e43 100644
--- a/drivers/usb/misc/usb3503.c
+++ b/drivers/usb/misc/usb3503.c
@@ -314,10 +314,8 @@ static int usb3503_i2c_probe(struct i2c_client *i2c,
 	int err;
 
 	hub = devm_kzalloc(&i2c->dev, sizeof(struct usb3503), GFP_KERNEL);
-	if (!hub) {
-		dev_err(&i2c->dev, "private data alloc fail\n");
+	if (!hub)
 		return -ENOMEM;
-	}
 
 	i2c_set_clientdata(i2c, hub);
 	hub->regmap = devm_regmap_init_i2c(i2c, &usb3503_regmap_config);
@@ -336,10 +334,8 @@ static int usb3503_platform_probe(struct platform_device *pdev)
 	struct usb3503 *hub;
 
 	hub = devm_kzalloc(&pdev->dev, sizeof(struct usb3503), GFP_KERNEL);
-	if (!hub) {
-		dev_err(&pdev->dev, "private data alloc fail\n");
+	if (!hub)
 		return -ENOMEM;
-	}
 	hub->dev = &pdev->dev;
 
 	return usb3503_probe(hub);
@@ -405,7 +401,6 @@ static struct platform_driver usb3503_platform_driver = {
 	.driver = {
 		.name = USB3503_I2C_NAME,
 		.of_match_table = of_match_ptr(usb3503_of_match),
-		.owner = THIS_MODULE,
 	},
 	.probe		= usb3503_platform_probe,
 };
diff --git a/drivers/usb/misc/yurex.c b/drivers/usb/misc/yurex.c
index c3a45da11610..343fa6ff9f4b 100644
--- a/drivers/usb/misc/yurex.c
+++ b/drivers/usb/misc/yurex.c
@@ -410,7 +410,8 @@ static int yurex_release(struct inode *inode, struct file *file)
 	return 0;
 }
 
-static ssize_t yurex_read(struct file *file, char *buffer, size_t count, loff_t *ppos)
+static ssize_t yurex_read(struct file *file, char __user *buffer, size_t count,
+			  loff_t *ppos)
 {
 	struct usb_yurex *dev;
 	int retval = 0;
@@ -444,7 +445,8 @@ exit:
 	return retval;
 }
 
-static ssize_t yurex_write(struct file *file, const char *user_buffer, size_t count, loff_t *ppos)
+static ssize_t yurex_write(struct file *file, const char __user *user_buffer,
+			   size_t count, loff_t *ppos)
 {
 	struct usb_yurex *dev;
 	int i, set = 0, retval = 0;
diff --git a/drivers/usb/musb/Kconfig b/drivers/usb/musb/Kconfig
index 06cc5d6ea681..9d68372dd9aa 100644
--- a/drivers/usb/musb/Kconfig
+++ b/drivers/usb/musb/Kconfig
@@ -58,8 +58,7 @@ config USB_MUSB_DUAL_ROLE
 
 endchoice
 
-choice
-	prompt "Platform Glue Layer"
+comment "Platform Glue Layer"
 
 config USB_MUSB_DAVINCI
 	tristate "DaVinci"
@@ -101,8 +100,6 @@ config USB_MUSB_JZ4740
 	depends on USB_MUSB_GADGET
 	depends on USB_OTG_BLACKLIST_HUB
 
-endchoice
-
 config USB_MUSB_AM335X_CHILD
 	tristate
 
diff --git a/drivers/usb/musb/am35x.c b/drivers/usb/musb/am35x.c
index a2735df24cc6..220fd4d3b41c 100644
--- a/drivers/usb/musb/am35x.c
+++ b/drivers/usb/musb/am35x.c
@@ -149,25 +149,25 @@ static void otg_timer(unsigned long _musb)
 	 */
 	devctl = musb_readb(mregs, MUSB_DEVCTL);
 	dev_dbg(musb->controller, "Poll devctl %02x (%s)\n", devctl,
-		usb_otg_state_string(musb->xceiv->state));
+		usb_otg_state_string(musb->xceiv->otg->state));
 
 	spin_lock_irqsave(&musb->lock, flags);
-	switch (musb->xceiv->state) {
+	switch (musb->xceiv->otg->state) {
 	case OTG_STATE_A_WAIT_BCON:
 		devctl &= ~MUSB_DEVCTL_SESSION;
 		musb_writeb(musb->mregs, MUSB_DEVCTL, devctl);
 
 		devctl = musb_readb(musb->mregs, MUSB_DEVCTL);
 		if (devctl & MUSB_DEVCTL_BDEVICE) {
-			musb->xceiv->state = OTG_STATE_B_IDLE;
+			musb->xceiv->otg->state = OTG_STATE_B_IDLE;
 			MUSB_DEV_MODE(musb);
 		} else {
-			musb->xceiv->state = OTG_STATE_A_IDLE;
+			musb->xceiv->otg->state = OTG_STATE_A_IDLE;
 			MUSB_HST_MODE(musb);
 		}
 		break;
 	case OTG_STATE_A_WAIT_VFALL:
-		musb->xceiv->state = OTG_STATE_A_WAIT_VRISE;
+		musb->xceiv->otg->state = OTG_STATE_A_WAIT_VRISE;
 		musb_writel(musb->ctrl_base, CORE_INTR_SRC_SET_REG,
 			    MUSB_INTR_VBUSERROR << AM35X_INTR_USB_SHIFT);
 		break;
@@ -176,7 +176,7 @@ static void otg_timer(unsigned long _musb)
 		if (devctl & MUSB_DEVCTL_BDEVICE)
 			mod_timer(&otg_workaround, jiffies + POLL_SECONDS * HZ);
 		else
-			musb->xceiv->state = OTG_STATE_A_IDLE;
+			musb->xceiv->otg->state = OTG_STATE_A_IDLE;
 		break;
 	default:
 		break;
@@ -193,9 +193,9 @@ static void am35x_musb_try_idle(struct musb *musb, unsigned long timeout)
 
 	/* 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)) {
+				musb->xceiv->otg->state == OTG_STATE_A_WAIT_BCON)) {
 		dev_dbg(musb->controller, "%s active, deleting timer\n",
-			usb_otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->otg->state));
 		del_timer(&otg_workaround);
 		last_timer = jiffies;
 		return;
@@ -208,7 +208,7 @@ static void am35x_musb_try_idle(struct musb *musb, unsigned long timeout)
 	last_timer = timeout;
 
 	dev_dbg(musb->controller, "%s inactive, starting idle timer for %u ms\n",
-		usb_otg_state_string(musb->xceiv->state),
+		usb_otg_state_string(musb->xceiv->otg->state),
 		jiffies_to_msecs(timeout - jiffies));
 	mod_timer(&otg_workaround, timeout);
 }
@@ -278,27 +278,27 @@ static irqreturn_t am35x_musb_interrupt(int irq, void *hci)
 			 * devctl.
 			 */
 			musb->int_usb &= ~MUSB_INTR_VBUSERROR;
-			musb->xceiv->state = OTG_STATE_A_WAIT_VFALL;
+			musb->xceiv->otg->state = OTG_STATE_A_WAIT_VFALL;
 			mod_timer(&otg_workaround, jiffies + POLL_SECONDS * HZ);
 			WARNING("VBUS error workaround (delay coming)\n");
 		} else if (drvvbus) {
 			MUSB_HST_MODE(musb);
 			otg->default_a = 1;
-			musb->xceiv->state = OTG_STATE_A_WAIT_VRISE;
+			musb->xceiv->otg->state = OTG_STATE_A_WAIT_VRISE;
 			portstate(musb->port1_status |= USB_PORT_STAT_POWER);
 			del_timer(&otg_workaround);
 		} else {
 			musb->is_active = 0;
 			MUSB_DEV_MODE(musb);
 			otg->default_a = 0;
-			musb->xceiv->state = OTG_STATE_B_IDLE;
+			musb->xceiv->otg->state = OTG_STATE_B_IDLE;
 			portstate(musb->port1_status &= ~USB_PORT_STAT_POWER);
 		}
 
 		/* NOTE: this must complete power-on within 100 ms. */
 		dev_dbg(musb->controller, "VBUS %s (%s)%s, devctl %02x\n",
 				drvvbus ? "on" : "off",
-				usb_otg_state_string(musb->xceiv->state),
+				usb_otg_state_string(musb->xceiv->otg->state),
 				err ? " ERROR" : "",
 				devctl);
 		ret = IRQ_HANDLED;
@@ -324,7 +324,7 @@ eoi:
 	}
 
 	/* Poll for ID change */
-	if (musb->xceiv->state == OTG_STATE_B_IDLE)
+	if (musb->xceiv->otg->state == OTG_STATE_B_IDLE)
 		mod_timer(&otg_workaround, jiffies + POLL_SECONDS * HZ);
 
 	spin_unlock_irqrestore(&musb->lock, flags);
@@ -408,7 +408,7 @@ static int am35x_musb_exit(struct musb *musb)
 }
 
 /* AM35x supports only 32bit read operation */
-void musb_read_fifo(struct musb_hw_ep *hw_ep, u16 len, u8 *dst)
+static void am35x_read_fifo(struct musb_hw_ep *hw_ep, u16 len, u8 *dst)
 {
 	void __iomem *fifo = hw_ep->fifo;
 	u32		val;
@@ -438,9 +438,11 @@ void musb_read_fifo(struct musb_hw_ep *hw_ep, u16 len, u8 *dst)
 }
 
 static const struct musb_platform_ops am35x_ops = {
+	.quirks		= MUSB_INDEXED_EP,
 	.init		= am35x_musb_init,
 	.exit		= am35x_musb_exit,
 
+	.read_fifo	= am35x_read_fifo,
 	.enable		= am35x_musb_enable,
 	.disable	= am35x_musb_disable,
 
diff --git a/drivers/usb/musb/blackfin.c b/drivers/usb/musb/blackfin.c
index ac4422b33dcd..a441a2de8619 100644
--- a/drivers/usb/musb/blackfin.c
+++ b/drivers/usb/musb/blackfin.c
@@ -33,10 +33,45 @@ struct bfin_glue {
 };
 #define glue_to_musb(g)		platform_get_drvdata(g->musb)
 
+static u32 bfin_fifo_offset(u8 epnum)
+{
+	return USB_OFFSET(USB_EP0_FIFO) + (epnum * 8);
+}
+
+static u8 bfin_readb(const void __iomem *addr, unsigned offset)
+{
+	return (u8)(bfin_read16(addr + offset));
+}
+
+static u16 bfin_readw(const void __iomem *addr, unsigned offset)
+{
+	return bfin_read16(addr + offset);
+}
+
+static u32 bfin_readl(const void __iomem *addr, unsigned offset)
+{
+	return (u32)(bfin_read16(addr + offset));
+}
+
+static void bfin_writeb(void __iomem *addr, unsigned offset, u8 data)
+{
+	bfin_write16(addr + offset, (u16)data);
+}
+
+static void bfin_writew(void __iomem *addr, unsigned offset, u16 data)
+{
+	bfin_write16(addr + offset, data);
+}
+
+static void binf_writel(void __iomem *addr, unsigned offset, u32 data)
+{
+	bfin_write16(addr + offset, (u16)data);
+}
+
 /*
  * Load an endpoint's FIFO
  */
-void musb_write_fifo(struct musb_hw_ep *hw_ep, u16 len, const u8 *src)
+static void bfin_write_fifo(struct musb_hw_ep *hw_ep, u16 len, const u8 *src)
 {
 	struct musb *musb = hw_ep->musb;
 	void __iomem *fifo = hw_ep->fifo;
@@ -100,7 +135,7 @@ void musb_write_fifo(struct musb_hw_ep *hw_ep, u16 len, const u8 *src)
 /*
  * Unload an endpoint's FIFO
  */
-void musb_read_fifo(struct musb_hw_ep *hw_ep, u16 len, u8 *dst)
+static void bfin_read_fifo(struct musb_hw_ep *hw_ep, u16 len, u8 *dst)
 {
 	struct musb *musb = hw_ep->musb;
 	void __iomem *fifo = hw_ep->fifo;
@@ -185,8 +220,8 @@ static irqreturn_t blackfin_interrupt(int irq, void *__hci)
 	}
 
 	/* Start sampling ID pin, when plug is removed from MUSB */
-	if ((musb->xceiv->state == OTG_STATE_B_IDLE
-		|| musb->xceiv->state == OTG_STATE_A_WAIT_BCON) ||
+	if ((musb->xceiv->otg->state == OTG_STATE_B_IDLE
+		|| musb->xceiv->otg->state == OTG_STATE_A_WAIT_BCON) ||
 		(musb->int_usb & MUSB_INTR_DISCONNECT && is_host_active(musb))) {
 		mod_timer(&musb_conn_timer, jiffies + TIMER_DELAY);
 		musb->a_wait_bcon = TIMER_DELAY;
@@ -205,7 +240,7 @@ static void musb_conn_timer_handler(unsigned long _musb)
 	static u8 toggle;
 
 	spin_lock_irqsave(&musb->lock, flags);
-	switch (musb->xceiv->state) {
+	switch (musb->xceiv->otg->state) {
 	case OTG_STATE_A_IDLE:
 	case OTG_STATE_A_WAIT_BCON:
 		/* Start a new session */
@@ -219,7 +254,7 @@ static void musb_conn_timer_handler(unsigned long _musb)
 
 		if (!(val & MUSB_DEVCTL_BDEVICE)) {
 			gpio_set_value(musb->config->gpio_vrsel, 1);
-			musb->xceiv->state = OTG_STATE_A_WAIT_BCON;
+			musb->xceiv->otg->state = OTG_STATE_A_WAIT_BCON;
 		} else {
 			gpio_set_value(musb->config->gpio_vrsel, 0);
 			/* Ignore VBUSERROR and SUSPEND IRQ */
@@ -229,7 +264,7 @@ static void musb_conn_timer_handler(unsigned long _musb)
 
 			val = MUSB_INTR_SUSPEND | MUSB_INTR_VBUSERROR;
 			musb_writeb(musb->mregs, MUSB_INTRUSB, val);
-			musb->xceiv->state = OTG_STATE_B_IDLE;
+			musb->xceiv->otg->state = OTG_STATE_B_IDLE;
 		}
 		mod_timer(&musb_conn_timer, jiffies + TIMER_DELAY);
 		break;
@@ -245,7 +280,7 @@ static void musb_conn_timer_handler(unsigned long _musb)
 
 		if (!(val & MUSB_DEVCTL_BDEVICE)) {
 			gpio_set_value(musb->config->gpio_vrsel, 1);
-			musb->xceiv->state = OTG_STATE_A_WAIT_BCON;
+			musb->xceiv->otg->state = OTG_STATE_A_WAIT_BCON;
 		} else {
 			gpio_set_value(musb->config->gpio_vrsel, 0);
 
@@ -280,13 +315,13 @@ static void musb_conn_timer_handler(unsigned long _musb)
 		break;
 	default:
 		dev_dbg(musb->controller, "%s state not handled\n",
-			usb_otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->otg->state));
 		break;
 	}
 	spin_unlock_irqrestore(&musb->lock, flags);
 
 	dev_dbg(musb->controller, "state is %s\n",
-		usb_otg_state_string(musb->xceiv->state));
+		usb_otg_state_string(musb->xceiv->otg->state));
 }
 
 static void bfin_musb_enable(struct musb *musb)
@@ -307,7 +342,7 @@ static void bfin_musb_set_vbus(struct musb *musb, int is_on)
 
 	dev_dbg(musb->controller, "VBUS %s, devctl %02x "
 		/* otg %3x conf %08x prcm %08x */ "\n",
-		usb_otg_state_string(musb->xceiv->state),
+		usb_otg_state_string(musb->xceiv->otg->state),
 		musb_readb(musb->mregs, MUSB_DEVCTL));
 }
 
@@ -433,6 +468,15 @@ static const struct musb_platform_ops bfin_ops = {
 	.init		= bfin_musb_init,
 	.exit		= bfin_musb_exit,
 
+	.readb		= bfin_readb,
+	.writeb		= bfin_writeb,
+	.readw		= bfin_readw,
+	.writew		= bfin_writew,
+	.readl		= bfin_readl,
+	.writel		= bfin_writel,
+	.fifo_mode	= 2,
+	.read_fifo	= bfin_read_fifo,
+	.write_fifo	= bfin_write_fifo,
 	.enable		= bfin_musb_enable,
 	.disable	= bfin_musb_disable,
 
@@ -456,16 +500,12 @@ static int bfin_probe(struct platform_device *pdev)
 	int				ret = -ENOMEM;
 
 	glue = devm_kzalloc(&pdev->dev, sizeof(*glue), GFP_KERNEL);
-	if (!glue) {
-		dev_err(&pdev->dev, "failed to allocate glue context\n");
+	if (!glue)
 		goto err0;
-	}
 
 	musb = platform_device_alloc("musb-hdrc", PLATFORM_DEVID_AUTO);
-	if (!musb) {
-		dev_err(&pdev->dev, "failed to allocate musb device\n");
+	if (!musb)
 		goto err0;
-	}
 
 	musb->dev.parent		= &pdev->dev;
 	musb->dev.dma_mask		= &bfin_dmamask;
diff --git a/drivers/usb/musb/da8xx.c b/drivers/usb/musb/da8xx.c
index 058775e647ad..9a9c82a4d35d 100644
--- a/drivers/usb/musb/da8xx.c
+++ b/drivers/usb/musb/da8xx.c
@@ -198,20 +198,20 @@ static void otg_timer(unsigned long _musb)
 	 */
 	devctl = musb_readb(mregs, MUSB_DEVCTL);
 	dev_dbg(musb->controller, "Poll devctl %02x (%s)\n", devctl,
-		usb_otg_state_string(musb->xceiv->state));
+		usb_otg_state_string(musb->xceiv->otg->state));
 
 	spin_lock_irqsave(&musb->lock, flags);
-	switch (musb->xceiv->state) {
+	switch (musb->xceiv->otg->state) {
 	case OTG_STATE_A_WAIT_BCON:
 		devctl &= ~MUSB_DEVCTL_SESSION;
 		musb_writeb(musb->mregs, MUSB_DEVCTL, devctl);
 
 		devctl = musb_readb(musb->mregs, MUSB_DEVCTL);
 		if (devctl & MUSB_DEVCTL_BDEVICE) {
-			musb->xceiv->state = OTG_STATE_B_IDLE;
+			musb->xceiv->otg->state = OTG_STATE_B_IDLE;
 			MUSB_DEV_MODE(musb);
 		} else {
-			musb->xceiv->state = OTG_STATE_A_IDLE;
+			musb->xceiv->otg->state = OTG_STATE_A_IDLE;
 			MUSB_HST_MODE(musb);
 		}
 		break;
@@ -226,7 +226,7 @@ static void otg_timer(unsigned long _musb)
 			mod_timer(&otg_workaround, jiffies + POLL_SECONDS * HZ);
 			break;
 		}
-		musb->xceiv->state = OTG_STATE_A_WAIT_VRISE;
+		musb->xceiv->otg->state = OTG_STATE_A_WAIT_VRISE;
 		musb_writel(musb->ctrl_base, DA8XX_USB_INTR_SRC_SET_REG,
 			    MUSB_INTR_VBUSERROR << DA8XX_INTR_USB_SHIFT);
 		break;
@@ -248,7 +248,7 @@ static void otg_timer(unsigned long _musb)
 		if (devctl & MUSB_DEVCTL_BDEVICE)
 			mod_timer(&otg_workaround, jiffies + POLL_SECONDS * HZ);
 		else
-			musb->xceiv->state = OTG_STATE_A_IDLE;
+			musb->xceiv->otg->state = OTG_STATE_A_IDLE;
 		break;
 	default:
 		break;
@@ -265,9 +265,9 @@ static void da8xx_musb_try_idle(struct musb *musb, unsigned long timeout)
 
 	/* 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)) {
+				musb->xceiv->otg->state == OTG_STATE_A_WAIT_BCON)) {
 		dev_dbg(musb->controller, "%s active, deleting timer\n",
-			usb_otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->otg->state));
 		del_timer(&otg_workaround);
 		last_timer = jiffies;
 		return;
@@ -280,7 +280,7 @@ static void da8xx_musb_try_idle(struct musb *musb, unsigned long timeout)
 	last_timer = timeout;
 
 	dev_dbg(musb->controller, "%s inactive, starting idle timer for %u ms\n",
-		usb_otg_state_string(musb->xceiv->state),
+		usb_otg_state_string(musb->xceiv->otg->state),
 		jiffies_to_msecs(timeout - jiffies));
 	mod_timer(&otg_workaround, timeout);
 }
@@ -341,26 +341,26 @@ static irqreturn_t da8xx_musb_interrupt(int irq, void *hci)
 			 * devctl.
 			 */
 			musb->int_usb &= ~MUSB_INTR_VBUSERROR;
-			musb->xceiv->state = OTG_STATE_A_WAIT_VFALL;
+			musb->xceiv->otg->state = OTG_STATE_A_WAIT_VFALL;
 			mod_timer(&otg_workaround, jiffies + POLL_SECONDS * HZ);
 			WARNING("VBUS error workaround (delay coming)\n");
 		} else if (drvvbus) {
 			MUSB_HST_MODE(musb);
 			otg->default_a = 1;
-			musb->xceiv->state = OTG_STATE_A_WAIT_VRISE;
+			musb->xceiv->otg->state = OTG_STATE_A_WAIT_VRISE;
 			portstate(musb->port1_status |= USB_PORT_STAT_POWER);
 			del_timer(&otg_workaround);
 		} else {
 			musb->is_active = 0;
 			MUSB_DEV_MODE(musb);
 			otg->default_a = 0;
-			musb->xceiv->state = OTG_STATE_B_IDLE;
+			musb->xceiv->otg->state = OTG_STATE_B_IDLE;
 			portstate(musb->port1_status &= ~USB_PORT_STAT_POWER);
 		}
 
 		dev_dbg(musb->controller, "VBUS %s (%s)%s, devctl %02x\n",
 				drvvbus ? "on" : "off",
-				usb_otg_state_string(musb->xceiv->state),
+				usb_otg_state_string(musb->xceiv->otg->state),
 				err ? " ERROR" : "",
 				devctl);
 		ret = IRQ_HANDLED;
@@ -375,7 +375,7 @@ static irqreturn_t da8xx_musb_interrupt(int irq, void *hci)
 		musb_writel(reg_base, DA8XX_USB_END_OF_INTR_REG, 0);
 
 	/* Poll for ID change */
-	if (musb->xceiv->state == OTG_STATE_B_IDLE)
+	if (musb->xceiv->otg->state == OTG_STATE_B_IDLE)
 		mod_timer(&otg_workaround, jiffies + POLL_SECONDS * HZ);
 
 	spin_unlock_irqrestore(&musb->lock, flags);
@@ -458,9 +458,11 @@ static int da8xx_musb_exit(struct musb *musb)
 }
 
 static const struct musb_platform_ops da8xx_ops = {
+	.quirks		= MUSB_INDEXED_EP,
 	.init		= da8xx_musb_init,
 	.exit		= da8xx_musb_exit,
 
+	.fifo_mode	= 2,
 	.enable		= da8xx_musb_enable,
 	.disable	= da8xx_musb_disable,
 
diff --git a/drivers/usb/musb/davinci.c b/drivers/usb/musb/davinci.c
index 110b78415bf0..3c1d9b211b51 100644
--- a/drivers/usb/musb/davinci.c
+++ b/drivers/usb/musb/davinci.c
@@ -214,10 +214,10 @@ static void otg_timer(unsigned long _musb)
 	 */
 	devctl = musb_readb(mregs, MUSB_DEVCTL);
 	dev_dbg(musb->controller, "poll devctl %02x (%s)\n", devctl,
-		usb_otg_state_string(musb->xceiv->state));
+		usb_otg_state_string(musb->xceiv->otg->state));
 
 	spin_lock_irqsave(&musb->lock, flags);
-	switch (musb->xceiv->state) {
+	switch (musb->xceiv->otg->state) {
 	case OTG_STATE_A_WAIT_VFALL:
 		/* Wait till VBUS falls below SessionEnd (~0.2V); the 1.3 RTL
 		 * seems to mis-handle session "start" otherwise (or in our
@@ -228,7 +228,7 @@ static void otg_timer(unsigned long _musb)
 			mod_timer(&otg_workaround, jiffies + POLL_SECONDS * HZ);
 			break;
 		}
-		musb->xceiv->state = OTG_STATE_A_WAIT_VRISE;
+		musb->xceiv->otg->state = OTG_STATE_A_WAIT_VRISE;
 		musb_writel(musb->ctrl_base, DAVINCI_USB_INT_SET_REG,
 			MUSB_INTR_VBUSERROR << DAVINCI_USB_USBINT_SHIFT);
 		break;
@@ -251,7 +251,7 @@ static void otg_timer(unsigned long _musb)
 		if (devctl & MUSB_DEVCTL_BDEVICE)
 			mod_timer(&otg_workaround, jiffies + POLL_SECONDS * HZ);
 		else
-			musb->xceiv->state = OTG_STATE_A_IDLE;
+			musb->xceiv->otg->state = OTG_STATE_A_IDLE;
 		break;
 	default:
 		break;
@@ -325,20 +325,20 @@ static irqreturn_t davinci_musb_interrupt(int irq, void *__hci)
 			 * to stop registering in devctl.
 			 */
 			musb->int_usb &= ~MUSB_INTR_VBUSERROR;
-			musb->xceiv->state = OTG_STATE_A_WAIT_VFALL;
+			musb->xceiv->otg->state = OTG_STATE_A_WAIT_VFALL;
 			mod_timer(&otg_workaround, jiffies + POLL_SECONDS * HZ);
 			WARNING("VBUS error workaround (delay coming)\n");
 		} else if (drvvbus) {
 			MUSB_HST_MODE(musb);
 			otg->default_a = 1;
-			musb->xceiv->state = OTG_STATE_A_WAIT_VRISE;
+			musb->xceiv->otg->state = OTG_STATE_A_WAIT_VRISE;
 			portstate(musb->port1_status |= USB_PORT_STAT_POWER);
 			del_timer(&otg_workaround);
 		} else {
 			musb->is_active = 0;
 			MUSB_DEV_MODE(musb);
 			otg->default_a = 0;
-			musb->xceiv->state = OTG_STATE_B_IDLE;
+			musb->xceiv->otg->state = OTG_STATE_B_IDLE;
 			portstate(musb->port1_status &= ~USB_PORT_STAT_POWER);
 		}
 
@@ -348,7 +348,7 @@ static irqreturn_t davinci_musb_interrupt(int irq, void *__hci)
 		davinci_musb_source_power(musb, drvvbus, 0);
 		dev_dbg(musb->controller, "VBUS %s (%s)%s, devctl %02x\n",
 				drvvbus ? "on" : "off",
-				usb_otg_state_string(musb->xceiv->state),
+				usb_otg_state_string(musb->xceiv->otg->state),
 				err ? " ERROR" : "",
 				devctl);
 		retval = IRQ_HANDLED;
@@ -361,7 +361,7 @@ static irqreturn_t davinci_musb_interrupt(int irq, void *__hci)
 	musb_writel(tibase, DAVINCI_USB_EOI_REG, 0);
 
 	/* poll for ID change */
-	if (musb->xceiv->state == OTG_STATE_B_IDLE)
+	if (musb->xceiv->otg->state == OTG_STATE_B_IDLE)
 		mod_timer(&otg_workaround, jiffies + POLL_SECONDS * HZ);
 
 	spin_unlock_irqrestore(&musb->lock, flags);
@@ -520,10 +520,8 @@ static int davinci_probe(struct platform_device *pdev)
 	int				ret = -ENOMEM;
 
 	glue = devm_kzalloc(&pdev->dev, sizeof(*glue), GFP_KERNEL);
-	if (!glue) {
-		dev_err(&pdev->dev, "failed to allocate glue context\n");
+	if (!glue)
 		goto err0;
-	}
 
 	clk = devm_clk_get(&pdev->dev, "usb");
 	if (IS_ERR(clk)) {
diff --git a/drivers/usb/musb/jz4740.c b/drivers/usb/musb/jz4740.c
index d1187290d4e3..bb7b26325a74 100644
--- a/drivers/usb/musb/jz4740.c
+++ b/drivers/usb/musb/jz4740.c
@@ -106,6 +106,8 @@ static int jz4740_musb_exit(struct musb *musb)
 }
 
 static const struct musb_platform_ops jz4740_musb_ops = {
+	.quirks		= MUSB_INDEXED_EP,
+	.fifo_mode	= 2,
 	.init		= jz4740_musb_init,
 	.exit		= jz4740_musb_exit,
 };
diff --git a/drivers/usb/musb/musb_core.c b/drivers/usb/musb/musb_core.c
index b841ee0bff06..34cce3e38c49 100644
--- a/drivers/usb/musb/musb_core.c
+++ b/drivers/usb/musb/musb_core.c
@@ -224,12 +224,67 @@ static struct usb_phy_io_ops musb_ulpi_access = {
 
 /*-------------------------------------------------------------------------*/
 
-#if !defined(CONFIG_USB_MUSB_TUSB6010) && !defined(CONFIG_USB_MUSB_BLACKFIN)
+static u32 musb_default_fifo_offset(u8 epnum)
+{
+	return 0x20 + (epnum * 4);
+}
+
+/* "flat" mapping: each endpoint has its own i/o address */
+static void musb_flat_ep_select(void __iomem *mbase, u8 epnum)
+{
+}
+
+static u32 musb_flat_ep_offset(u8 epnum, u16 offset)
+{
+	return 0x100 + (0x10 * epnum) + offset;
+}
+
+/* "indexed" mapping: INDEX register controls register bank select */
+static void musb_indexed_ep_select(void __iomem *mbase, u8 epnum)
+{
+	musb_writeb(mbase, MUSB_INDEX, epnum);
+}
+
+static u32 musb_indexed_ep_offset(u8 epnum, u16 offset)
+{
+	return 0x10 + offset;
+}
+
+static u8 musb_default_readb(const void __iomem *addr, unsigned offset)
+{
+	return __raw_readb(addr + offset);
+}
+
+static void musb_default_writeb(void __iomem *addr, unsigned offset, u8 data)
+{
+	__raw_writeb(data, addr + offset);
+}
+
+static u16 musb_default_readw(const void __iomem *addr, unsigned offset)
+{
+	return __raw_readw(addr + offset);
+}
+
+static void musb_default_writew(void __iomem *addr, unsigned offset, u16 data)
+{
+	__raw_writew(data, addr + offset);
+}
+
+static u32 musb_default_readl(const void __iomem *addr, unsigned offset)
+{
+	return __raw_readl(addr + offset);
+}
+
+static void musb_default_writel(void __iomem *addr, unsigned offset, u32 data)
+{
+	__raw_writel(data, addr + offset);
+}
 
 /*
  * Load an endpoint's FIFO
  */
-void musb_write_fifo(struct musb_hw_ep *hw_ep, u16 len, const u8 *src)
+static void musb_default_write_fifo(struct musb_hw_ep *hw_ep, u16 len,
+				    const u8 *src)
 {
 	struct musb *musb = hw_ep->musb;
 	void __iomem *fifo = hw_ep->fifo;
@@ -270,11 +325,10 @@ void musb_write_fifo(struct musb_hw_ep *hw_ep, u16 len, const u8 *src)
 	}
 }
 
-#if !defined(CONFIG_USB_MUSB_AM35X)
 /*
  * Unload an endpoint's FIFO
  */
-void musb_read_fifo(struct musb_hw_ep *hw_ep, u16 len, u8 *dst)
+static void musb_default_read_fifo(struct musb_hw_ep *hw_ep, u16 len, u8 *dst)
 {
 	struct musb *musb = hw_ep->musb;
 	void __iomem *fifo = hw_ep->fifo;
@@ -312,10 +366,40 @@ void musb_read_fifo(struct musb_hw_ep *hw_ep, u16 len, u8 *dst)
 		ioread8_rep(fifo, dst, len);
 	}
 }
-#endif
 
-#endif	/* normal PIO */
+/*
+ * Old style IO functions
+ */
+u8 (*musb_readb)(const void __iomem *addr, unsigned offset);
+EXPORT_SYMBOL_GPL(musb_readb);
+
+void (*musb_writeb)(void __iomem *addr, unsigned offset, u8 data);
+EXPORT_SYMBOL_GPL(musb_writeb);
+
+u16 (*musb_readw)(const void __iomem *addr, unsigned offset);
+EXPORT_SYMBOL_GPL(musb_readw);
+
+void (*musb_writew)(void __iomem *addr, unsigned offset, u16 data);
+EXPORT_SYMBOL_GPL(musb_writew);
+
+u32 (*musb_readl)(const void __iomem *addr, unsigned offset);
+EXPORT_SYMBOL_GPL(musb_readl);
 
+void (*musb_writel)(void __iomem *addr, unsigned offset, u32 data);
+EXPORT_SYMBOL_GPL(musb_writel);
+
+/*
+ * New style IO functions
+ */
+void musb_read_fifo(struct musb_hw_ep *hw_ep, u16 len, u8 *dst)
+{
+	return hw_ep->musb->io.read_fifo(hw_ep, len, dst);
+}
+
+void musb_write_fifo(struct musb_hw_ep *hw_ep, u16 len, const u8 *src)
+{
+	return hw_ep->musb->io.write_fifo(hw_ep, len, src);
+}
 
 /*-------------------------------------------------------------------------*/
 
@@ -360,23 +444,23 @@ static void musb_otg_timer_func(unsigned long data)
 	unsigned long	flags;
 
 	spin_lock_irqsave(&musb->lock, flags);
-	switch (musb->xceiv->state) {
+	switch (musb->xceiv->otg->state) {
 	case OTG_STATE_B_WAIT_ACON:
 		dev_dbg(musb->controller, "HNP: b_wait_acon timeout; back to b_peripheral\n");
 		musb_g_disconnect(musb);
-		musb->xceiv->state = OTG_STATE_B_PERIPHERAL;
+		musb->xceiv->otg->state = OTG_STATE_B_PERIPHERAL;
 		musb->is_active = 0;
 		break;
 	case OTG_STATE_A_SUSPEND:
 	case OTG_STATE_A_WAIT_BCON:
 		dev_dbg(musb->controller, "HNP: %s timeout\n",
-			usb_otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->otg->state));
 		musb_platform_set_vbus(musb, 0);
-		musb->xceiv->state = OTG_STATE_A_WAIT_VFALL;
+		musb->xceiv->otg->state = OTG_STATE_A_WAIT_VFALL;
 		break;
 	default:
 		dev_dbg(musb->controller, "HNP: Unhandled mode %s\n",
-			usb_otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->otg->state));
 	}
 	spin_unlock_irqrestore(&musb->lock, flags);
 }
@@ -391,19 +475,19 @@ void musb_hnp_stop(struct musb *musb)
 	u8	reg;
 
 	dev_dbg(musb->controller, "HNP: stop from %s\n",
-			usb_otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->otg->state));
 
-	switch (musb->xceiv->state) {
+	switch (musb->xceiv->otg->state) {
 	case OTG_STATE_A_PERIPHERAL:
 		musb_g_disconnect(musb);
 		dev_dbg(musb->controller, "HNP: back to %s\n",
-			usb_otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->otg->state));
 		break;
 	case OTG_STATE_B_HOST:
 		dev_dbg(musb->controller, "HNP: Disabling HR\n");
 		if (hcd)
 			hcd->self.is_b_host = 0;
-		musb->xceiv->state = OTG_STATE_B_PERIPHERAL;
+		musb->xceiv->otg->state = OTG_STATE_B_PERIPHERAL;
 		MUSB_DEV_MODE(musb);
 		reg = musb_readb(mbase, MUSB_POWER);
 		reg |= MUSB_POWER_SUSPENDM;
@@ -412,7 +496,7 @@ void musb_hnp_stop(struct musb *musb)
 		break;
 	default:
 		dev_dbg(musb->controller, "HNP: Stopping in unknown state %s\n",
-			usb_otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->otg->state));
 	}
 
 	/*
@@ -423,6 +507,7 @@ void musb_hnp_stop(struct musb *musb)
 	musb->port1_status &= ~(USB_PORT_STAT_C_CONNECTION << 16);
 }
 
+static void musb_generic_disable(struct musb *musb);
 /*
  * Interrupt Service Routine to record USB "global" interrupts.
  * Since these do not happen often and signify things of
@@ -449,13 +534,13 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 	 */
 	if (int_usb & MUSB_INTR_RESUME) {
 		handled = IRQ_HANDLED;
-		dev_dbg(musb->controller, "RESUME (%s)\n", usb_otg_state_string(musb->xceiv->state));
+		dev_dbg(musb->controller, "RESUME (%s)\n", usb_otg_state_string(musb->xceiv->otg->state));
 
 		if (devctl & MUSB_DEVCTL_HM) {
 			void __iomem *mbase = musb->mregs;
 			u8 power;
 
-			switch (musb->xceiv->state) {
+			switch (musb->xceiv->otg->state) {
 			case OTG_STATE_A_SUSPEND:
 				/* remote wakeup?  later, GetPortStatus
 				 * will stop RESUME signaling
@@ -478,29 +563,26 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 						| MUSB_PORT_STAT_RESUME;
 				musb->rh_timer = jiffies
 						 + msecs_to_jiffies(20);
-				schedule_delayed_work(
-					&musb->finish_resume_work,
-					msecs_to_jiffies(20));
+				musb->need_finish_resume = 1;
 
-				musb->xceiv->state = OTG_STATE_A_HOST;
+				musb->xceiv->otg->state = OTG_STATE_A_HOST;
 				musb->is_active = 1;
-				musb_host_resume_root_hub(musb);
 				break;
 			case OTG_STATE_B_WAIT_ACON:
-				musb->xceiv->state = OTG_STATE_B_PERIPHERAL;
+				musb->xceiv->otg->state = OTG_STATE_B_PERIPHERAL;
 				musb->is_active = 1;
 				MUSB_DEV_MODE(musb);
 				break;
 			default:
 				WARNING("bogus %s RESUME (%s)\n",
 					"host",
-					usb_otg_state_string(musb->xceiv->state));
+					usb_otg_state_string(musb->xceiv->otg->state));
 			}
 		} else {
-			switch (musb->xceiv->state) {
+			switch (musb->xceiv->otg->state) {
 			case OTG_STATE_A_SUSPEND:
 				/* possibly DISCONNECT is upcoming */
-				musb->xceiv->state = OTG_STATE_A_HOST;
+				musb->xceiv->otg->state = OTG_STATE_A_HOST;
 				musb_host_resume_root_hub(musb);
 				break;
 			case OTG_STATE_B_WAIT_ACON:
@@ -523,7 +605,7 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 			default:
 				WARNING("bogus %s RESUME (%s)\n",
 					"peripheral",
-					usb_otg_state_string(musb->xceiv->state));
+					usb_otg_state_string(musb->xceiv->otg->state));
 			}
 		}
 	}
@@ -539,7 +621,7 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 		}
 
 		dev_dbg(musb->controller, "SESSION_REQUEST (%s)\n",
-			usb_otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->otg->state));
 
 		/* IRQ arrives from ID pin sense or (later, if VBUS power
 		 * is removed) SRP.  responses are time critical:
@@ -550,7 +632,7 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 		 */
 		musb_writeb(mbase, MUSB_DEVCTL, MUSB_DEVCTL_SESSION);
 		musb->ep0_stage = MUSB_EP0_START;
-		musb->xceiv->state = OTG_STATE_A_IDLE;
+		musb->xceiv->otg->state = OTG_STATE_A_IDLE;
 		MUSB_HST_MODE(musb);
 		musb_platform_set_vbus(musb, 1);
 
@@ -576,7 +658,7 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 		 * REVISIT:  do delays from lots of DEBUG_KERNEL checks
 		 * make trouble here, keeping VBUS < 4.4V ?
 		 */
-		switch (musb->xceiv->state) {
+		switch (musb->xceiv->otg->state) {
 		case OTG_STATE_A_HOST:
 			/* recovery is dicey once we've gotten past the
 			 * initial stages of enumeration, but if VBUS
@@ -605,7 +687,7 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 
 		dev_printk(ignore ? KERN_DEBUG : KERN_ERR, musb->controller,
 				"VBUS_ERROR in %s (%02x, %s), retry #%d, port1 %08x\n",
-				usb_otg_state_string(musb->xceiv->state),
+				usb_otg_state_string(musb->xceiv->otg->state),
 				devctl,
 				({ char *s;
 				switch (devctl & MUSB_DEVCTL_VBUS) {
@@ -630,10 +712,10 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 
 	if (int_usb & MUSB_INTR_SUSPEND) {
 		dev_dbg(musb->controller, "SUSPEND (%s) devctl %02x\n",
-			usb_otg_state_string(musb->xceiv->state), devctl);
+			usb_otg_state_string(musb->xceiv->otg->state), devctl);
 		handled = IRQ_HANDLED;
 
-		switch (musb->xceiv->state) {
+		switch (musb->xceiv->otg->state) {
 		case OTG_STATE_A_PERIPHERAL:
 			/* We also come here if the cable is removed, since
 			 * this silicon doesn't report ID-no-longer-grounded.
@@ -657,7 +739,7 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 			musb_g_suspend(musb);
 			musb->is_active = musb->g.b_hnp_enable;
 			if (musb->is_active) {
-				musb->xceiv->state = OTG_STATE_B_WAIT_ACON;
+				musb->xceiv->otg->state = OTG_STATE_B_WAIT_ACON;
 				dev_dbg(musb->controller, "HNP: Setting timer for b_ase0_brst\n");
 				mod_timer(&musb->otg_timer, jiffies
 					+ msecs_to_jiffies(
@@ -670,7 +752,7 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 					+ msecs_to_jiffies(musb->a_wait_bcon));
 			break;
 		case OTG_STATE_A_HOST:
-			musb->xceiv->state = OTG_STATE_A_SUSPEND;
+			musb->xceiv->otg->state = OTG_STATE_A_SUSPEND;
 			musb->is_active = musb->hcd->self.b_hnp_enable;
 			break;
 		case OTG_STATE_B_HOST:
@@ -713,7 +795,7 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 			musb->port1_status |= USB_PORT_STAT_LOW_SPEED;
 
 		/* indicate new connection to OTG machine */
-		switch (musb->xceiv->state) {
+		switch (musb->xceiv->otg->state) {
 		case OTG_STATE_B_PERIPHERAL:
 			if (int_usb & MUSB_INTR_SUSPEND) {
 				dev_dbg(musb->controller, "HNP: SUSPEND+CONNECT, now b_host\n");
@@ -725,7 +807,7 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 		case OTG_STATE_B_WAIT_ACON:
 			dev_dbg(musb->controller, "HNP: CONNECT, now b_host\n");
 b_host:
-			musb->xceiv->state = OTG_STATE_B_HOST;
+			musb->xceiv->otg->state = OTG_STATE_B_HOST;
 			if (musb->hcd)
 				musb->hcd->self.is_b_host = 1;
 			del_timer(&musb->otg_timer);
@@ -733,7 +815,7 @@ b_host:
 		default:
 			if ((devctl & MUSB_DEVCTL_VBUS)
 					== (3 << MUSB_DEVCTL_VBUS_SHIFT)) {
-				musb->xceiv->state = OTG_STATE_A_HOST;
+				musb->xceiv->otg->state = OTG_STATE_A_HOST;
 				if (hcd)
 					hcd->self.is_b_host = 0;
 			}
@@ -743,16 +825,16 @@ b_host:
 		musb_host_poke_root_hub(musb);
 
 		dev_dbg(musb->controller, "CONNECT (%s) devctl %02x\n",
-				usb_otg_state_string(musb->xceiv->state), devctl);
+				usb_otg_state_string(musb->xceiv->otg->state), devctl);
 	}
 
 	if (int_usb & MUSB_INTR_DISCONNECT) {
 		dev_dbg(musb->controller, "DISCONNECT (%s) as %s, devctl %02x\n",
-				usb_otg_state_string(musb->xceiv->state),
+				usb_otg_state_string(musb->xceiv->otg->state),
 				MUSB_MODE(musb), devctl);
 		handled = IRQ_HANDLED;
 
-		switch (musb->xceiv->state) {
+		switch (musb->xceiv->otg->state) {
 		case OTG_STATE_A_HOST:
 		case OTG_STATE_A_SUSPEND:
 			musb_host_resume_root_hub(musb);
@@ -770,7 +852,7 @@ b_host:
 			musb_root_disconnect(musb);
 			if (musb->hcd)
 				musb->hcd->self.is_b_host = 0;
-			musb->xceiv->state = OTG_STATE_B_PERIPHERAL;
+			musb->xceiv->otg->state = OTG_STATE_B_PERIPHERAL;
 			MUSB_DEV_MODE(musb);
 			musb_g_disconnect(musb);
 			break;
@@ -786,7 +868,7 @@ b_host:
 			break;
 		default:
 			WARNING("unhandled DISCONNECT transition (%s)\n",
-				usb_otg_state_string(musb->xceiv->state));
+				usb_otg_state_string(musb->xceiv->otg->state));
 			break;
 		}
 	}
@@ -812,15 +894,15 @@ b_host:
 			}
 		} else {
 			dev_dbg(musb->controller, "BUS RESET as %s\n",
-				usb_otg_state_string(musb->xceiv->state));
-			switch (musb->xceiv->state) {
+				usb_otg_state_string(musb->xceiv->otg->state));
+			switch (musb->xceiv->otg->state) {
 			case OTG_STATE_A_SUSPEND:
 				musb_g_reset(musb);
 				/* FALLTHROUGH */
 			case OTG_STATE_A_WAIT_BCON:	/* OPT TD.4.7-900ms */
 				/* never use invalid T(a_wait_bcon) */
 				dev_dbg(musb->controller, "HNP: in %s, %d msec timeout\n",
-					usb_otg_state_string(musb->xceiv->state),
+					usb_otg_state_string(musb->xceiv->otg->state),
 					TA_WAIT_BCON(musb));
 				mod_timer(&musb->otg_timer, jiffies
 					+ msecs_to_jiffies(TA_WAIT_BCON(musb)));
@@ -831,27 +913,29 @@ b_host:
 				break;
 			case OTG_STATE_B_WAIT_ACON:
 				dev_dbg(musb->controller, "HNP: RESET (%s), to b_peripheral\n",
-					usb_otg_state_string(musb->xceiv->state));
-				musb->xceiv->state = OTG_STATE_B_PERIPHERAL;
+					usb_otg_state_string(musb->xceiv->otg->state));
+				musb->xceiv->otg->state = OTG_STATE_B_PERIPHERAL;
 				musb_g_reset(musb);
 				break;
 			case OTG_STATE_B_IDLE:
-				musb->xceiv->state = OTG_STATE_B_PERIPHERAL;
+				musb->xceiv->otg->state = OTG_STATE_B_PERIPHERAL;
 				/* FALLTHROUGH */
 			case OTG_STATE_B_PERIPHERAL:
 				musb_g_reset(musb);
 				break;
 			default:
 				dev_dbg(musb->controller, "Unhandled BUS RESET as %s\n",
-					usb_otg_state_string(musb->xceiv->state));
+					usb_otg_state_string(musb->xceiv->otg->state));
 			}
 		}
 	}
 
 	/* handle babble condition */
-	if (int_usb & MUSB_INTR_BABBLE && is_host_active(musb))
+	if (int_usb & MUSB_INTR_BABBLE && is_host_active(musb)) {
+		musb_generic_disable(musb);
 		schedule_delayed_work(&musb->recover_work,
 				      msecs_to_jiffies(100));
+	}
 
 #if 0
 /* REVISIT ... this would be for multiplexing periodic endpoints, or
@@ -1032,21 +1116,7 @@ static void musb_shutdown(struct platform_device *pdev)
  * We don't currently use dynamic fifo setup capability to do anything
  * more than selecting one of a bunch of predefined configurations.
  */
-#if defined(CONFIG_USB_MUSB_TUSB6010)			\
-	|| defined(CONFIG_USB_MUSB_TUSB6010_MODULE)	\
-	|| 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_DSPS)		\
-	|| defined(CONFIG_USB_MUSB_DSPS_MODULE)
-static ushort fifo_mode = 4;
-#elif defined(CONFIG_USB_MUSB_UX500)			\
-	|| defined(CONFIG_USB_MUSB_UX500_MODULE)
-static ushort fifo_mode = 5;
-#else
-static ushort fifo_mode = 2;
-#endif
+static ushort fifo_mode;
 
 /* "modprobe ... fifo_mode=1" etc */
 module_param(fifo_mode, ushort, 0);
@@ -1456,20 +1526,25 @@ static int musb_core_init(u16 musb_type, struct musb *musb)
 	for (i = 0; i < musb->nr_endpoints; i++) {
 		struct musb_hw_ep	*hw_ep = musb->endpoints + i;
 
-		hw_ep->fifo = MUSB_FIFO_OFFSET(i) + mbase;
-#if defined(CONFIG_USB_MUSB_TUSB6010) || defined (CONFIG_USB_MUSB_TUSB6010_MODULE)
-		hw_ep->fifo_async = musb->async + 0x400 + MUSB_FIFO_OFFSET(i);
-		hw_ep->fifo_sync = musb->sync + 0x400 + MUSB_FIFO_OFFSET(i);
-		hw_ep->fifo_sync_va =
-			musb->sync_va + 0x400 + MUSB_FIFO_OFFSET(i);
-
-		if (i == 0)
-			hw_ep->conf = mbase - 0x400 + TUSB_EP0_CONF;
-		else
-			hw_ep->conf = mbase + 0x400 + (((i - 1) & 0xf) << 2);
+		hw_ep->fifo = musb->io.fifo_offset(i) + mbase;
+#if IS_ENABLED(CONFIG_USB_MUSB_TUSB6010)
+		if (musb->io.quirks & MUSB_IN_TUSB) {
+			hw_ep->fifo_async = musb->async + 0x400 +
+				musb->io.fifo_offset(i);
+			hw_ep->fifo_sync = musb->sync + 0x400 +
+				musb->io.fifo_offset(i);
+			hw_ep->fifo_sync_va =
+				musb->sync_va + 0x400 + musb->io.fifo_offset(i);
+
+			if (i == 0)
+				hw_ep->conf = mbase - 0x400 + TUSB_EP0_CONF;
+			else
+				hw_ep->conf = mbase + 0x400 +
+					(((i - 1) & 0xf) << 2);
+		}
 #endif
 
-		hw_ep->regs = MUSB_EP_OFFSET(i, 0) + mbase;
+		hw_ep->regs = musb->io.ep_offset(i, 0) + mbase;
 		hw_ep->target_regs = musb_read_target_reg_base(i, mbase);
 		hw_ep->rx_reinit = 1;
 		hw_ep->tx_reinit = 1;
@@ -1630,7 +1705,7 @@ musb_mode_show(struct device *dev, struct device_attribute *attr, char *buf)
 	int ret = -EINVAL;
 
 	spin_lock_irqsave(&musb->lock, flags);
-	ret = sprintf(buf, "%s\n", usb_otg_state_string(musb->xceiv->state));
+	ret = sprintf(buf, "%s\n", usb_otg_state_string(musb->xceiv->otg->state));
 	spin_unlock_irqrestore(&musb->lock, flags);
 
 	return ret;
@@ -1675,7 +1750,7 @@ musb_vbus_store(struct device *dev, struct device_attribute *attr,
 	spin_lock_irqsave(&musb->lock, flags);
 	/* force T(a_wait_bcon) to be zero/unlimited *OR* valid */
 	musb->a_wait_bcon = val ? max_t(int, val, OTG_TIME_A_WAIT_BCON) : 0 ;
-	if (musb->xceiv->state == OTG_STATE_A_WAIT_BCON)
+	if (musb->xceiv->otg->state == OTG_STATE_A_WAIT_BCON)
 		musb->is_active = 0;
 	musb_platform_try_idle(musb, jiffies + msecs_to_jiffies(val));
 	spin_unlock_irqrestore(&musb->lock, flags);
@@ -1743,8 +1818,8 @@ static void musb_irq_work(struct work_struct *data)
 {
 	struct musb *musb = container_of(data, struct musb, irq_work);
 
-	if (musb->xceiv->state != musb->xceiv_old_state) {
-		musb->xceiv_old_state = musb->xceiv->state;
+	if (musb->xceiv->otg->state != musb->xceiv_old_state) {
+		musb->xceiv_old_state = musb->xceiv->otg->state;
 		sysfs_notify(&musb->controller->kobj, NULL, "mode");
 	}
 }
@@ -1903,6 +1978,18 @@ musb_init_controller(struct device *dev, int nIrq, void __iomem *ctrl)
 	musb->ops = plat->platform_ops;
 	musb->port_mode = plat->mode;
 
+	/*
+	 * Initialize the default IO functions. At least omap2430 needs
+	 * these early. We initialize the platform specific IO functions
+	 * later on.
+	 */
+	musb_readb = musb_default_readb;
+	musb_writeb = musb_default_writeb;
+	musb_readw = musb_default_readw;
+	musb_writew = musb_default_writew;
+	musb_readl = musb_default_readl;
+	musb_writel = musb_default_writel;
+
 	/* The musb_platform_init() call:
 	 *   - adjusts musb->mregs
 	 *   - sets the musb->isr
@@ -1924,6 +2011,57 @@ musb_init_controller(struct device *dev, int nIrq, void __iomem *ctrl)
 		goto fail2;
 	}
 
+	if (musb->ops->quirks)
+		musb->io.quirks = musb->ops->quirks;
+
+	/* At least tusb6010 has it's own offsets.. */
+	if (musb->ops->ep_offset)
+		musb->io.ep_offset = musb->ops->ep_offset;
+	if (musb->ops->ep_select)
+		musb->io.ep_select = musb->ops->ep_select;
+
+	/* ..and some devices use indexed offset or flat offset */
+	if (musb->io.quirks & MUSB_INDEXED_EP) {
+		musb->io.ep_offset = musb_indexed_ep_offset;
+		musb->io.ep_select = musb_indexed_ep_select;
+	} else {
+		musb->io.ep_offset = musb_flat_ep_offset;
+		musb->io.ep_select = musb_flat_ep_select;
+	}
+
+	if (musb->ops->fifo_mode)
+		fifo_mode = musb->ops->fifo_mode;
+	else
+		fifo_mode = 4;
+
+	if (musb->ops->fifo_offset)
+		musb->io.fifo_offset = musb->ops->fifo_offset;
+	else
+		musb->io.fifo_offset = musb_default_fifo_offset;
+
+	if (musb->ops->readb)
+		musb_readb = musb->ops->readb;
+	if (musb->ops->writeb)
+		musb_writeb = musb->ops->writeb;
+	if (musb->ops->readw)
+		musb_readw = musb->ops->readw;
+	if (musb->ops->writew)
+		musb_writew = musb->ops->writew;
+	if (musb->ops->readl)
+		musb_readl = musb->ops->readl;
+	if (musb->ops->writel)
+		musb_writel = musb->ops->writel;
+
+	if (musb->ops->read_fifo)
+		musb->io.read_fifo = musb->ops->read_fifo;
+	else
+		musb->io.read_fifo = musb_default_read_fifo;
+
+	if (musb->ops->write_fifo)
+		musb->io.write_fifo = musb->ops->write_fifo;
+	else
+		musb->io.write_fifo = musb_default_write_fifo;
+
 	if (!musb->xceiv->io_ops) {
 		musb->xceiv->io_dev = musb->controller;
 		musb->xceiv->io_priv = musb->mregs;
@@ -1983,10 +2121,10 @@ musb_init_controller(struct device *dev, int nIrq, void __iomem *ctrl)
 
 	if (musb->xceiv->otg->default_a) {
 		MUSB_HST_MODE(musb);
-		musb->xceiv->state = OTG_STATE_A_IDLE;
+		musb->xceiv->otg->state = OTG_STATE_A_IDLE;
 	} else {
 		MUSB_DEV_MODE(musb);
-		musb->xceiv->state = OTG_STATE_B_IDLE;
+		musb->xceiv->otg->state = OTG_STATE_B_IDLE;
 	}
 
 	switch (musb->port_mode) {
@@ -2080,10 +2218,10 @@ static int musb_probe(struct platform_device *pdev)
 	struct resource	*iomem;
 	void __iomem	*base;
 
-	iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!iomem || irq <= 0)
+	if (irq <= 0)
 		return -ENODEV;
 
+	iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	base = devm_ioremap_resource(dev, iomem);
 	if (IS_ERR(base))
 		return PTR_ERR(base);
@@ -2297,9 +2435,11 @@ static int musb_suspend(struct device *dev)
 	return 0;
 }
 
-static int musb_resume_noirq(struct device *dev)
+static int musb_resume(struct device *dev)
 {
 	struct musb	*musb = dev_to_musb(dev);
+	u8		devctl;
+	u8		mask;
 
 	/*
 	 * For static cmos like DaVinci, register values were preserved
@@ -2313,6 +2453,23 @@ static int musb_resume_noirq(struct device *dev)
 
 	musb_restore_context(musb);
 
+	devctl = musb_readb(musb->mregs, MUSB_DEVCTL);
+	mask = MUSB_DEVCTL_BDEVICE | MUSB_DEVCTL_FSDEV | MUSB_DEVCTL_LSDEV;
+	if ((devctl & mask) != (musb->context.devctl & mask))
+		musb->port1_status = 0;
+	if (musb->need_finish_resume) {
+		musb->need_finish_resume = 0;
+		schedule_delayed_work(&musb->finish_resume_work,
+				      msecs_to_jiffies(20));
+	}
+
+	/*
+	 * The USB HUB code expects the device to be in RPM_ACTIVE once it came
+	 * out of suspend
+	 */
+	pm_runtime_disable(dev);
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
 	return 0;
 }
 
@@ -2348,7 +2505,7 @@ static int musb_runtime_resume(struct device *dev)
 
 static const struct dev_pm_ops musb_dev_pm_ops = {
 	.suspend	= musb_suspend,
-	.resume_noirq	= musb_resume_noirq,
+	.resume		= musb_resume,
 	.runtime_suspend = musb_runtime_suspend,
 	.runtime_resume = musb_runtime_resume,
 };
@@ -2362,7 +2519,6 @@ static struct platform_driver musb_driver = {
 	.driver = {
 		.name		= (char *)musb_driver_name,
 		.bus		= &platform_bus_type,
-		.owner		= THIS_MODULE,
 		.pm		= MUSB_DEV_PM_OPS,
 	},
 	.probe		= musb_probe,
diff --git a/drivers/usb/musb/musb_core.h b/drivers/usb/musb/musb_core.h
index 414e57a984bb..5e65958f7915 100644
--- a/drivers/usb/musb/musb_core.h
+++ b/drivers/usb/musb/musb_core.h
@@ -124,41 +124,6 @@ enum musb_g_ep0_state {
 #define OTG_TIME_A_AIDL_BDIS	200		/* min 200 msec */
 #define OTG_TIME_B_ASE0_BRST	100		/* min 3.125 ms */
 
-
-/*************************** REGISTER ACCESS ********************************/
-
-/* Endpoint registers (other than dynfifo setup) can be accessed either
- * directly with the "flat" model, or after setting up an index register.
- */
-
-#if defined(CONFIG_ARCH_DAVINCI) || defined(CONFIG_SOC_OMAP2430) \
-		|| defined(CONFIG_SOC_OMAP3430) || defined(CONFIG_BLACKFIN) \
-		|| defined(CONFIG_ARCH_OMAP4)
-/* REVISIT indexed access seemed to
- * misbehave (on DaVinci) for at least peripheral IN ...
- */
-#define	MUSB_FLAT_REG
-#endif
-
-/* TUSB mapping: "flat" plus ep0 special cases */
-#if defined(CONFIG_USB_MUSB_TUSB6010) || \
-	defined(CONFIG_USB_MUSB_TUSB6010_MODULE)
-#define musb_ep_select(_mbase, _epnum) \
-	musb_writeb((_mbase), MUSB_INDEX, (_epnum))
-#define	MUSB_EP_OFFSET			MUSB_TUSB_OFFSET
-
-/* "flat" mapping: each endpoint has its own i/o address */
-#elif	defined(MUSB_FLAT_REG)
-#define musb_ep_select(_mbase, _epnum)	(((void)(_mbase)), ((void)(_epnum)))
-#define	MUSB_EP_OFFSET			MUSB_FLAT_OFFSET
-
-/* "indexed" mapping: INDEX register controls register bank select */
-#else
-#define musb_ep_select(_mbase, _epnum) \
-	musb_writeb((_mbase), MUSB_INDEX, (_epnum))
-#define	MUSB_EP_OFFSET			MUSB_INDEXED_OFFSET
-#endif
-
 /****************************** FUNCTIONS ********************************/
 
 #define MUSB_HST_MODE(_musb)\
@@ -173,8 +138,25 @@ enum musb_g_ep0_state {
 
 /******************************** TYPES *************************************/
 
+struct musb_io;
+
 /**
  * struct musb_platform_ops - Operations passed to musb_core by HW glue layer
+ * @quirks:	flags for platform specific quirks
+ * @enable:	enable device
+ * @disable:	disable device
+ * @ep_offset:	returns the end point offset
+ * @ep_select:	selects the specified end point
+ * @fifo_mode:	sets the fifo mode
+ * @fifo_offset: returns the fifo offset
+ * @readb:	read 8 bits
+ * @writeb:	write 8 bits
+ * @readw:	read 16 bits
+ * @writew:	write 16 bits
+ * @readl:	read 32 bits
+ * @writel:	write 32 bits
+ * @read_fifo:	reads the fifo
+ * @write_fifo:	writes to fifo
  * @init:	turns on clocks, sets up platform-specific registers, etc
  * @exit:	undoes @init
  * @set_mode:	forcefully changes operating mode
@@ -184,12 +166,34 @@ enum musb_g_ep0_state {
  * @adjust_channel_params: pre check for standard dma channel_program func
  */
 struct musb_platform_ops {
+
+#define MUSB_DMA_UX500		BIT(6)
+#define MUSB_DMA_CPPI41		BIT(5)
+#define MUSB_DMA_CPPI		BIT(4)
+#define MUSB_DMA_TUSB_OMAP	BIT(3)
+#define MUSB_DMA_INVENTRA	BIT(2)
+#define MUSB_IN_TUSB		BIT(1)
+#define MUSB_INDEXED_EP		BIT(0)
+	u32	quirks;
+
 	int	(*init)(struct musb *musb);
 	int	(*exit)(struct musb *musb);
 
 	void	(*enable)(struct musb *musb);
 	void	(*disable)(struct musb *musb);
 
+	u32	(*ep_offset)(u8 epnum, u16 offset);
+	void	(*ep_select)(void __iomem *mbase, u8 epnum);
+	u16	fifo_mode;
+	u32	(*fifo_offset)(u8 epnum);
+	u8	(*readb)(const void __iomem *addr, unsigned offset);
+	void	(*writeb)(void __iomem *addr, unsigned offset, u8 data);
+	u16	(*readw)(const void __iomem *addr, unsigned offset);
+	void	(*writew)(void __iomem *addr, unsigned offset, u16 data);
+	u32	(*readl)(const void __iomem *addr, unsigned offset);
+	void	(*writel)(void __iomem *addr, unsigned offset, u32 data);
+	void	(*read_fifo)(struct musb_hw_ep *hw_ep, u16 len, u8 *buf);
+	void	(*write_fifo)(struct musb_hw_ep *hw_ep, u16 len, const u8 *buf);
 	int	(*set_mode)(struct musb *musb, u8 mode);
 	void	(*try_idle)(struct musb *musb, unsigned long timeout);
 	int	(*reset)(struct musb *musb);
@@ -212,8 +216,7 @@ struct musb_hw_ep {
 	void __iomem		*fifo;
 	void __iomem		*regs;
 
-#if defined(CONFIG_USB_MUSB_TUSB6010) || \
-	defined(CONFIG_USB_MUSB_TUSB6010_MODULE)
+#if IS_ENABLED(CONFIG_USB_MUSB_TUSB6010)
 	void __iomem		*conf;
 #endif
 
@@ -230,8 +233,7 @@ struct musb_hw_ep {
 	struct dma_channel	*tx_channel;
 	struct dma_channel	*rx_channel;
 
-#if defined(CONFIG_USB_MUSB_TUSB6010) || \
-	defined(CONFIG_USB_MUSB_TUSB6010_MODULE)
+#if IS_ENABLED(CONFIG_USB_MUSB_TUSB6010)
 	/* TUSB has "asynchronous" and "synchronous" dma modes */
 	dma_addr_t		fifo_async;
 	dma_addr_t		fifo_sync;
@@ -292,6 +294,7 @@ struct musb {
 	/* device lock */
 	spinlock_t		lock;
 
+	struct musb_io		io;
 	const struct musb_platform_ops *ops;
 	struct musb_context_registers context;
 
@@ -334,8 +337,7 @@ struct musb {
 	void __iomem		*ctrl_base;
 	void __iomem		*mregs;
 
-#if defined(CONFIG_USB_MUSB_TUSB6010) || \
-	defined(CONFIG_USB_MUSB_TUSB6010_MODULE)
+#if IS_ENABLED(CONFIG_USB_MUSB_TUSB6010)
 	dma_addr_t		async;
 	dma_addr_t		sync;
 	void __iomem		*sync_va;
@@ -390,6 +392,7 @@ struct musb {
 
 	/* is_suspended means USB B_PERIPHERAL suspend */
 	unsigned		is_suspended:1;
+	unsigned		need_finish_resume :1;
 
 	/* may_wakeup means remote wakeup is enabled */
 	unsigned		may_wakeup:1;
@@ -474,7 +477,7 @@ static inline int musb_read_fifosize(struct musb *musb,
 	u8 reg = 0;
 
 	/* read from core using indexed model */
-	reg = musb_readb(mbase, MUSB_EP_OFFSET(epnum, MUSB_FIFOSIZE));
+	reg = musb_readb(mbase, musb->io.ep_offset(epnum, MUSB_FIFOSIZE));
 	/* 0's returned when no more endpoints */
 	if (!reg)
 		return -ENODEV;
diff --git a/drivers/usb/musb/musb_cppi41.c b/drivers/usb/musb/musb_cppi41.c
index 5a9b977fbc19..f64fd964dc6d 100644
--- a/drivers/usb/musb/musb_cppi41.c
+++ b/drivers/usb/musb/musb_cppi41.c
@@ -253,6 +253,7 @@ static void cppi41_dma_callback(void *private_data)
 		cppi41_trans_done(cppi41_channel);
 	} else {
 		struct cppi41_dma_controller *controller;
+		int is_hs = 0;
 		/*
 		 * On AM335x it has been observed that the TX interrupt fires
 		 * too early that means the TXFIFO is not yet empty but the DMA
@@ -265,7 +266,14 @@ static void cppi41_dma_callback(void *private_data)
 		 */
 		controller = cppi41_channel->controller;
 
-		if (musb->g.speed == USB_SPEED_HIGH) {
+		if (is_host_active(musb)) {
+			if (musb->port1_status & USB_PORT_STAT_HIGH_SPEED)
+				is_hs = 1;
+		} else {
+			if (musb->g.speed == USB_SPEED_HIGH)
+				is_hs = 1;
+		}
+		if (is_hs) {
 			unsigned wait = 25;
 
 			do {
diff --git a/drivers/usb/musb/musb_debugfs.c b/drivers/usb/musb/musb_debugfs.c
index 4c216790e86b..ad3701a97389 100644
--- a/drivers/usb/musb/musb_debugfs.c
+++ b/drivers/usb/musb/musb_debugfs.c
@@ -49,33 +49,36 @@ struct musb_register_map {
 };
 
 static const struct musb_register_map musb_regmap[] = {
-	{ "FAddr",		0x00,	8 },
-	{ "Power",		0x01,	8 },
-	{ "Frame",		0x0c,	16 },
-	{ "Index",		0x0e,	8 },
-	{ "Testmode",		0x0f,	8 },
-	{ "TxMaxPp",		0x10,	16 },
-	{ "TxCSRp",		0x12,	16 },
-	{ "RxMaxPp",		0x14,	16 },
-	{ "RxCSR",		0x16,	16 },
-	{ "RxCount",		0x18,	16 },
-	{ "ConfigData",		0x1f,	8 },
-	{ "DevCtl",		0x60,	8 },
-	{ "MISC",		0x61,	8 },
-	{ "TxFIFOsz",		0x62,	8 },
-	{ "RxFIFOsz",		0x63,	8 },
-	{ "TxFIFOadd",		0x64,	16 },
-	{ "RxFIFOadd",		0x66,	16 },
-	{ "VControl",		0x68,	32 },
-	{ "HWVers",		0x6C,	16 },
-	{ "EPInfo",		0x78,	8 },
-	{ "RAMInfo",		0x79,	8 },
-	{ "LinkInfo",		0x7A,	8 },
-	{ "VPLen",		0x7B,	8 },
-	{ "HS_EOF1",		0x7C,	8 },
-	{ "FS_EOF1",		0x7D,	8 },
-	{ "LS_EOF1",		0x7E,	8 },
-	{ "SOFT_RST",		0x7F,	8 },
+	{ "FAddr",	MUSB_FADDR,	8 },
+	{ "Power",	MUSB_POWER,	8 },
+	{ "Frame",	MUSB_FRAME,	16 },
+	{ "Index",	MUSB_INDEX,	8 },
+	{ "Testmode",	MUSB_TESTMODE,	8 },
+	{ "TxMaxPp",	MUSB_TXMAXP,	16 },
+	{ "TxCSRp",	MUSB_TXCSR,	16 },
+	{ "RxMaxPp",	MUSB_RXMAXP,	16 },
+	{ "RxCSR",	MUSB_RXCSR,	16 },
+	{ "RxCount",	MUSB_RXCOUNT,	16 },
+	{ "ConfigData",	MUSB_CONFIGDATA,8 },
+	{ "IntrRxE",	MUSB_INTRRXE,	16 },
+	{ "IntrTxE",	MUSB_INTRTXE,	16 },
+	{ "IntrUsbE",	MUSB_INTRUSBE,	8 },
+	{ "DevCtl",	MUSB_DEVCTL,	8 },
+	{ "BabbleCtl",	MUSB_BABBLE_CTL,8 },
+	{ "TxFIFOsz",	MUSB_TXFIFOSZ,	8 },
+	{ "RxFIFOsz",	MUSB_RXFIFOSZ,	8 },
+	{ "TxFIFOadd",	MUSB_TXFIFOADD,	16 },
+	{ "RxFIFOadd",	MUSB_RXFIFOADD,	16 },
+	{ "VControl",	0x68,		32 },
+	{ "HWVers",	0x69,		16 },
+	{ "EPInfo",	MUSB_EPINFO,	8 },
+	{ "RAMInfo",	MUSB_RAMINFO,	8 },
+	{ "LinkInfo",	MUSB_LINKINFO,	8 },
+	{ "VPLen",	MUSB_VPLEN,	8 },
+	{ "HS_EOF1",	MUSB_HS_EOF1,	8 },
+	{ "FS_EOF1",	MUSB_FS_EOF1,	8 },
+	{ "LS_EOF1",	MUSB_LS_EOF1,	8 },
+	{ "SOFT_RST",	0x7F,		8 },
 	{ "DMA_CNTLch0",	0x204,	16 },
 	{ "DMA_ADDRch0",	0x208,	32 },
 	{ "DMA_COUNTch0",	0x20C,	32 },
diff --git a/drivers/usb/musb/musb_dsps.c b/drivers/usb/musb/musb_dsps.c
index 48bc09e7b83b..53bd0e71d19f 100644
--- a/drivers/usb/musb/musb_dsps.c
+++ b/drivers/usb/musb/musb_dsps.c
@@ -179,9 +179,9 @@ static void dsps_musb_try_idle(struct musb *musb, unsigned long timeout)
 
 	/* 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)) {
+			musb->xceiv->otg->state == OTG_STATE_A_WAIT_BCON)) {
 		dev_dbg(musb->controller, "%s active, deleting timer\n",
-				usb_otg_state_string(musb->xceiv->state));
+				usb_otg_state_string(musb->xceiv->otg->state));
 		del_timer(&glue->timer);
 		glue->last_timer = jiffies;
 		return;
@@ -201,7 +201,7 @@ static void dsps_musb_try_idle(struct musb *musb, unsigned long timeout)
 	glue->last_timer = timeout;
 
 	dev_dbg(musb->controller, "%s inactive, starting idle timer for %u ms\n",
-		usb_otg_state_string(musb->xceiv->state),
+		usb_otg_state_string(musb->xceiv->otg->state),
 			jiffies_to_msecs(timeout - jiffies));
 	mod_timer(&glue->timer, timeout);
 }
@@ -265,10 +265,10 @@ static void otg_timer(unsigned long _musb)
 	 */
 	devctl = dsps_readb(mregs, MUSB_DEVCTL);
 	dev_dbg(musb->controller, "Poll devctl %02x (%s)\n", devctl,
-				usb_otg_state_string(musb->xceiv->state));
+				usb_otg_state_string(musb->xceiv->otg->state));
 
 	spin_lock_irqsave(&musb->lock, flags);
-	switch (musb->xceiv->state) {
+	switch (musb->xceiv->otg->state) {
 	case OTG_STATE_A_WAIT_BCON:
 		dsps_writeb(musb->mregs, MUSB_DEVCTL, 0);
 		skip_session = 1;
@@ -277,10 +277,10 @@ static void otg_timer(unsigned long _musb)
 	case OTG_STATE_A_IDLE:
 	case OTG_STATE_B_IDLE:
 		if (devctl & MUSB_DEVCTL_BDEVICE) {
-			musb->xceiv->state = OTG_STATE_B_IDLE;
+			musb->xceiv->otg->state = OTG_STATE_B_IDLE;
 			MUSB_DEV_MODE(musb);
 		} else {
-			musb->xceiv->state = OTG_STATE_A_IDLE;
+			musb->xceiv->otg->state = OTG_STATE_A_IDLE;
 			MUSB_HST_MODE(musb);
 		}
 		if (!(devctl & MUSB_DEVCTL_SESSION) && !skip_session)
@@ -288,7 +288,7 @@ static void otg_timer(unsigned long _musb)
 		mod_timer(&glue->timer, jiffies + wrp->poll_seconds * HZ);
 		break;
 	case OTG_STATE_A_WAIT_VFALL:
-		musb->xceiv->state = OTG_STATE_A_WAIT_VRISE;
+		musb->xceiv->otg->state = OTG_STATE_A_WAIT_VRISE;
 		dsps_writel(musb->ctrl_base, wrp->coreintr_set,
 			    MUSB_INTR_VBUSERROR << wrp->usb_shift);
 		break;
@@ -373,26 +373,26 @@ static irqreturn_t dsps_interrupt(int irq, void *hci)
 			 * devctl.
 			 */
 			musb->int_usb &= ~MUSB_INTR_VBUSERROR;
-			musb->xceiv->state = OTG_STATE_A_WAIT_VFALL;
+			musb->xceiv->otg->state = OTG_STATE_A_WAIT_VFALL;
 			mod_timer(&glue->timer,
 					jiffies + wrp->poll_seconds * HZ);
 			WARNING("VBUS error workaround (delay coming)\n");
 		} else if (drvvbus) {
 			MUSB_HST_MODE(musb);
 			musb->xceiv->otg->default_a = 1;
-			musb->xceiv->state = OTG_STATE_A_WAIT_VRISE;
+			musb->xceiv->otg->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;
+			musb->xceiv->otg->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",
-				usb_otg_state_string(musb->xceiv->state),
+				usb_otg_state_string(musb->xceiv->otg->state),
 				err ? " ERROR" : "",
 				devctl);
 		ret = IRQ_HANDLED;
@@ -402,7 +402,7 @@ static irqreturn_t dsps_interrupt(int irq, void *hci)
 		ret |= musb_interrupt(musb);
 
 	/* Poll for ID change in OTG port mode */
-	if (musb->xceiv->state == OTG_STATE_B_IDLE &&
+	if (musb->xceiv->otg->state == OTG_STATE_B_IDLE &&
 			musb->port_mode == MUSB_PORT_MODE_DUAL_ROLE)
 		mod_timer(&glue->timer, jiffies + wrp->poll_seconds * HZ);
 out:
@@ -447,9 +447,6 @@ static int dsps_musb_init(struct musb *musb)
 	int ret;
 
 	r = platform_get_resource_byname(parent, IORESOURCE_MEM, "control");
-	if (!r)
-		return -EINVAL;
-
 	reg_base = devm_ioremap_resource(dev, r);
 	if (IS_ERR(reg_base))
 		return PTR_ERR(reg_base);
@@ -636,6 +633,7 @@ static int dsps_musb_reset(struct musb *musb)
 }
 
 static struct musb_platform_ops dsps_ops = {
+	.quirks		= MUSB_INDEXED_EP,
 	.init		= dsps_musb_init,
 	.exit		= dsps_musb_exit,
 
@@ -729,7 +727,6 @@ static int dsps_create_musb_pdev(struct dsps_glue *glue,
 
 	config = devm_kzalloc(&parent->dev, sizeof(*config), GFP_KERNEL);
 	if (!config) {
-		dev_err(dev, "failed to allocate musb hdrc config\n");
 		ret = -ENOMEM;
 		goto err;
 	}
@@ -781,10 +778,8 @@ static int dsps_probe(struct platform_device *pdev)
 
 	/* allocate glue */
 	glue = devm_kzalloc(&pdev->dev, sizeof(*glue), GFP_KERNEL);
-	if (!glue) {
-		dev_err(&pdev->dev, "unable to allocate glue memory\n");
+	if (!glue)
 		return -ENOMEM;
-	}
 
 	glue->dev = &pdev->dev;
 	glue->wrp = wrp;
@@ -906,7 +901,7 @@ static int dsps_resume(struct device *dev)
 	dsps_writel(mbase, wrp->mode, glue->context.mode);
 	dsps_writel(mbase, wrp->tx_mode, glue->context.tx_mode);
 	dsps_writel(mbase, wrp->rx_mode, glue->context.rx_mode);
-	if (musb->xceiv->state == OTG_STATE_B_IDLE &&
+	if (musb->xceiv->otg->state == OTG_STATE_B_IDLE &&
 	    musb->port_mode == MUSB_PORT_MODE_DUAL_ROLE)
 		mod_timer(&glue->timer, jiffies + wrp->poll_seconds * HZ);
 
diff --git a/drivers/usb/musb/musb_gadget.c b/drivers/usb/musb/musb_gadget.c
index 24c8c0219790..49b04cb6f5ca 100644
--- a/drivers/usb/musb/musb_gadget.c
+++ b/drivers/usb/musb/musb_gadget.c
@@ -1546,7 +1546,7 @@ static int musb_gadget_wakeup(struct usb_gadget *gadget)
 
 	spin_lock_irqsave(&musb->lock, flags);
 
-	switch (musb->xceiv->state) {
+	switch (musb->xceiv->otg->state) {
 	case OTG_STATE_B_PERIPHERAL:
 		/* NOTE:  OTG state machine doesn't include B_SUSPENDED;
 		 * that's part of the standard usb 1.1 state machine, and
@@ -1587,7 +1587,7 @@ static int musb_gadget_wakeup(struct usb_gadget *gadget)
 		goto done;
 	default:
 		dev_dbg(musb->controller, "Unhandled wake: %s\n",
-			usb_otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->otg->state));
 		goto done;
 	}
 
@@ -1684,8 +1684,7 @@ static int musb_gadget_pullup(struct usb_gadget *gadget, int is_on)
 
 static int musb_gadget_start(struct usb_gadget *g,
 		struct usb_gadget_driver *driver);
-static int musb_gadget_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver);
+static int musb_gadget_stop(struct usb_gadget *g);
 
 static const struct usb_gadget_ops musb_gadget_operations = {
 	.get_frame		= musb_gadget_get_frame,
@@ -1792,7 +1791,7 @@ int musb_gadget_setup(struct musb *musb)
 
 	MUSB_DEV_MODE(musb);
 	musb->xceiv->otg->default_a = 0;
-	musb->xceiv->state = OTG_STATE_B_IDLE;
+	musb->xceiv->otg->state = OTG_STATE_B_IDLE;
 
 	/* this "gadget" abstracts/virtualizes the controller */
 	musb->g.name = musb_driver_name;
@@ -1851,8 +1850,6 @@ static int musb_gadget_start(struct usb_gadget *g,
 
 	pm_runtime_get_sync(musb->controller);
 
-	dev_dbg(musb->controller, "registering driver %s\n", driver->function);
-
 	musb->softconnect = 0;
 	musb->gadget_driver = driver;
 
@@ -1860,7 +1857,7 @@ static int musb_gadget_start(struct usb_gadget *g,
 	musb->is_active = 1;
 
 	otg_set_peripheral(otg, &musb->g);
-	musb->xceiv->state = OTG_STATE_B_IDLE;
+	musb->xceiv->otg->state = OTG_STATE_B_IDLE;
 	spin_unlock_irqrestore(&musb->lock, flags);
 
 	musb_start(musb);
@@ -1925,8 +1922,7 @@ static void stop_activity(struct musb *musb, struct usb_gadget_driver *driver)
  *
  * @param driver the gadget driver to unregister
  */
-static int musb_gadget_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver)
+static int musb_gadget_stop(struct usb_gadget *g)
 {
 	struct musb	*musb = gadget_to_musb(g);
 	unsigned long	flags;
@@ -1945,13 +1941,10 @@ static int musb_gadget_stop(struct usb_gadget *g,
 
 	(void) musb_gadget_vbus_draw(&musb->g, 0);
 
-	musb->xceiv->state = OTG_STATE_UNDEFINED;
-	stop_activity(musb, driver);
+	musb->xceiv->otg->state = OTG_STATE_UNDEFINED;
+	stop_activity(musb, NULL);
 	otg_set_peripheral(musb->xceiv->otg, NULL);
 
-	dev_dbg(musb->controller, "unregistering driver %s\n",
-				  driver ? driver->function : "(removed)");
-
 	musb->is_active = 0;
 	musb->gadget_driver = NULL;
 	musb_platform_try_idle(musb, 0);
@@ -1975,7 +1968,7 @@ static int musb_gadget_stop(struct usb_gadget *g,
 void musb_g_resume(struct musb *musb)
 {
 	musb->is_suspended = 0;
-	switch (musb->xceiv->state) {
+	switch (musb->xceiv->otg->state) {
 	case OTG_STATE_B_IDLE:
 		break;
 	case OTG_STATE_B_WAIT_ACON:
@@ -1989,7 +1982,7 @@ void musb_g_resume(struct musb *musb)
 		break;
 	default:
 		WARNING("unhandled RESUME transition (%s)\n",
-				usb_otg_state_string(musb->xceiv->state));
+				usb_otg_state_string(musb->xceiv->otg->state));
 	}
 }
 
@@ -2001,10 +1994,10 @@ void musb_g_suspend(struct musb *musb)
 	devctl = musb_readb(musb->mregs, MUSB_DEVCTL);
 	dev_dbg(musb->controller, "devctl %02x\n", devctl);
 
-	switch (musb->xceiv->state) {
+	switch (musb->xceiv->otg->state) {
 	case OTG_STATE_B_IDLE:
 		if ((devctl & MUSB_DEVCTL_VBUS) == MUSB_DEVCTL_VBUS)
-			musb->xceiv->state = OTG_STATE_B_PERIPHERAL;
+			musb->xceiv->otg->state = OTG_STATE_B_PERIPHERAL;
 		break;
 	case OTG_STATE_B_PERIPHERAL:
 		musb->is_suspended = 1;
@@ -2019,7 +2012,7 @@ void musb_g_suspend(struct musb *musb)
 		 * A_PERIPHERAL may need care too
 		 */
 		WARNING("unhandled SUSPEND transition (%s)\n",
-				usb_otg_state_string(musb->xceiv->state));
+				usb_otg_state_string(musb->xceiv->otg->state));
 	}
 }
 
@@ -2050,22 +2043,22 @@ void musb_g_disconnect(struct musb *musb)
 		spin_lock(&musb->lock);
 	}
 
-	switch (musb->xceiv->state) {
+	switch (musb->xceiv->otg->state) {
 	default:
 		dev_dbg(musb->controller, "Unhandled disconnect %s, setting a_idle\n",
-			usb_otg_state_string(musb->xceiv->state));
-		musb->xceiv->state = OTG_STATE_A_IDLE;
+			usb_otg_state_string(musb->xceiv->otg->state));
+		musb->xceiv->otg->state = OTG_STATE_A_IDLE;
 		MUSB_HST_MODE(musb);
 		break;
 	case OTG_STATE_A_PERIPHERAL:
-		musb->xceiv->state = OTG_STATE_A_WAIT_BCON;
+		musb->xceiv->otg->state = OTG_STATE_A_WAIT_BCON;
 		MUSB_HST_MODE(musb);
 		break;
 	case OTG_STATE_B_WAIT_ACON:
 	case OTG_STATE_B_HOST:
 	case OTG_STATE_B_PERIPHERAL:
 	case OTG_STATE_B_IDLE:
-		musb->xceiv->state = OTG_STATE_B_IDLE;
+		musb->xceiv->otg->state = OTG_STATE_B_IDLE;
 		break;
 	case OTG_STATE_B_SRP_INIT:
 		break;
@@ -2090,9 +2083,12 @@ __acquires(musb->lock)
 				: NULL
 			);
 
-	/* report disconnect, if we didn't already (flushing EP state) */
-	if (musb->g.speed != USB_SPEED_UNKNOWN)
-		musb_g_disconnect(musb);
+	/* report reset, if we didn't already (flushing EP state) */
+	if (musb->gadget_driver && musb->g.speed != USB_SPEED_UNKNOWN) {
+		spin_unlock(&musb->lock);
+		usb_gadget_udc_reset(&musb->g, musb->gadget_driver);
+		spin_lock(&musb->lock);
+	}
 
 	/* clear HR */
 	else if (devctl & MUSB_DEVCTL_HR)
@@ -2125,13 +2121,13 @@ __acquires(musb->lock)
 		 * In that case, do not rely on devctl for setting
 		 * peripheral mode.
 		 */
-		musb->xceiv->state = OTG_STATE_B_PERIPHERAL;
+		musb->xceiv->otg->state = OTG_STATE_B_PERIPHERAL;
 		musb->g.is_a_peripheral = 0;
 	} else if (devctl & MUSB_DEVCTL_BDEVICE) {
-		musb->xceiv->state = OTG_STATE_B_PERIPHERAL;
+		musb->xceiv->otg->state = OTG_STATE_B_PERIPHERAL;
 		musb->g.is_a_peripheral = 0;
 	} else {
-		musb->xceiv->state = OTG_STATE_A_PERIPHERAL;
+		musb->xceiv->otg->state = OTG_STATE_A_PERIPHERAL;
 		musb->g.is_a_peripheral = 1;
 	}
 
diff --git a/drivers/usb/musb/musb_host.c b/drivers/usb/musb/musb_host.c
index 855793d701bb..23d474d3d7f4 100644
--- a/drivers/usb/musb/musb_host.c
+++ b/drivers/usb/musb/musb_host.c
@@ -2463,7 +2463,7 @@ static int musb_bus_suspend(struct usb_hcd *hcd)
 	if (!is_host_active(musb))
 		return 0;
 
-	switch (musb->xceiv->state) {
+	switch (musb->xceiv->otg->state) {
 	case OTG_STATE_A_SUSPEND:
 		return 0;
 	case OTG_STATE_A_WAIT_VRISE:
@@ -2473,7 +2473,7 @@ static int musb_bus_suspend(struct usb_hcd *hcd)
 		 */
 		devctl = musb_readb(musb->mregs, MUSB_DEVCTL);
 		if ((devctl & MUSB_DEVCTL_VBUS) == MUSB_DEVCTL_VBUS)
-			musb->xceiv->state = OTG_STATE_A_WAIT_BCON;
+			musb->xceiv->otg->state = OTG_STATE_A_WAIT_BCON;
 		break;
 	default:
 		break;
@@ -2481,7 +2481,7 @@ static int musb_bus_suspend(struct usb_hcd *hcd)
 
 	if (musb->is_active) {
 		WARNING("trying to suspend as %s while active\n",
-				usb_otg_state_string(musb->xceiv->state));
+				usb_otg_state_string(musb->xceiv->otg->state));
 		return -EBUSY;
 	} else
 		return 0;
@@ -2678,7 +2678,7 @@ int musb_host_setup(struct musb *musb, int power_budget)
 
 	MUSB_HST_MODE(musb);
 	musb->xceiv->otg->default_a = 1;
-	musb->xceiv->state = OTG_STATE_A_IDLE;
+	musb->xceiv->otg->state = OTG_STATE_A_IDLE;
 
 	otg_set_host(musb->xceiv->otg, &hcd->self);
 	hcd->self.otg_port = 1;
diff --git a/drivers/usb/musb/musb_io.h b/drivers/usb/musb/musb_io.h
index eebeed78edd6..8a57a6f4b3a6 100644
--- a/drivers/usb/musb/musb_io.h
+++ b/drivers/usb/musb/musb_io.h
@@ -37,86 +37,32 @@
 
 #include <linux/io.h>
 
-#ifndef CONFIG_BLACKFIN
-
-/* NOTE:  these offsets are all in bytes */
-
-static inline u16 musb_readw(const void __iomem *addr, unsigned offset)
-	{ return __raw_readw(addr + offset); }
-
-static inline u32 musb_readl(const void __iomem *addr, unsigned offset)
-	{ return __raw_readl(addr + offset); }
-
-
-static inline void musb_writew(void __iomem *addr, unsigned offset, u16 data)
-	{ __raw_writew(data, addr + offset); }
-
-static inline void musb_writel(void __iomem *addr, unsigned offset, u32 data)
-	{ __raw_writel(data, addr + offset); }
-
-
-#if defined(CONFIG_USB_MUSB_TUSB6010) || defined (CONFIG_USB_MUSB_TUSB6010_MODULE)
-
-/*
- * TUSB6010 doesn't allow 8-bit access; 16-bit access is the minimum.
+#define musb_ep_select(_mbase, _epnum)	musb->io.ep_select((_mbase), (_epnum))
+
+/**
+ * struct musb_io - IO functions for MUSB
+ * @quirks:	platform specific flags
+ * @ep_offset:	platform specific function to get end point offset
+ * @ep_select:	platform specific function to select end point
+ * @fifo_offset: platform specific function to get fifo offset
+ * @read_fifo:	platform specific function to read fifo
+ * @write_fifo:	platform specific function to write fifo
  */
-static inline u8 musb_readb(const void __iomem *addr, unsigned offset)
-{
-	u16 tmp;
-	u8 val;
-
-	tmp = __raw_readw(addr + (offset & ~1));
-	if (offset & 1)
-		val = (tmp >> 8);
-	else
-		val = tmp & 0xff;
-
-	return val;
-}
-
-static inline void musb_writeb(void __iomem *addr, unsigned offset, u8 data)
-{
-	u16 tmp;
-
-	tmp = __raw_readw(addr + (offset & ~1));
-	if (offset & 1)
-		tmp = (data << 8) | (tmp & 0xff);
-	else
-		tmp = (tmp & 0xff00) | data;
-
-	__raw_writew(tmp, addr + (offset & ~1));
-}
-
-#else
-
-static inline u8 musb_readb(const void __iomem *addr, unsigned offset)
-	{ return __raw_readb(addr + offset); }
-
-static inline void musb_writeb(void __iomem *addr, unsigned offset, u8 data)
-	{ __raw_writeb(data, addr + offset); }
-
-#endif	/* CONFIG_USB_MUSB_TUSB6010 */
-
-#else
-
-static inline u8 musb_readb(const void __iomem *addr, unsigned offset)
-	{ return (u8) (bfin_read16(addr + offset)); }
-
-static inline u16 musb_readw(const void __iomem *addr, unsigned offset)
-	{ return bfin_read16(addr + offset); }
-
-static inline u32 musb_readl(const void __iomem *addr, unsigned offset)
-	{ return (u32) (bfin_read16(addr + offset)); }
-
-static inline void musb_writeb(void __iomem *addr, unsigned offset, u8 data)
-	{ bfin_write16(addr + offset, (u16) data); }
-
-static inline void musb_writew(void __iomem *addr, unsigned offset, u16 data)
-	{ bfin_write16(addr + offset, data); }
-
-static inline void musb_writel(void __iomem *addr, unsigned offset, u32 data)
-	{ bfin_write16(addr + offset, (u16) data); }
-
-#endif /* CONFIG_BLACKFIN */
+struct musb_io {
+	u32	quirks;
+	u32	(*ep_offset)(u8 epnum, u16 offset);
+	void	(*ep_select)(void __iomem *mbase, u8 epnum);
+	u32	(*fifo_offset)(u8 epnum);
+	void	(*read_fifo)(struct musb_hw_ep *hw_ep, u16 len, u8 *buf);
+	void	(*write_fifo)(struct musb_hw_ep *hw_ep, u16 len, const u8 *buf);
+};
+
+/* Do not add new entries here, add them the struct musb_io instead */
+extern u8 (*musb_readb)(const void __iomem *addr, unsigned offset);
+extern void (*musb_writeb)(void __iomem *addr, unsigned offset, u8 data);
+extern u16 (*musb_readw)(const void __iomem *addr, unsigned offset);
+extern void (*musb_writew)(void __iomem *addr, unsigned offset, u16 data);
+extern u32 (*musb_readl)(const void __iomem *addr, unsigned offset);
+extern void (*musb_writel)(void __iomem *addr, unsigned offset, u32 data);
 
 #endif
diff --git a/drivers/usb/musb/musb_regs.h b/drivers/usb/musb/musb_regs.h
index 37122a480bc1..11f0be07491e 100644
--- a/drivers/usb/musb/musb_regs.h
+++ b/drivers/usb/musb/musb_regs.h
@@ -239,14 +239,6 @@
 #define MUSB_INDEX		0x0E	/* 8 bit */
 #define MUSB_TESTMODE		0x0F	/* 8 bit */
 
-/* Get offset for a given FIFO from musb->mregs */
-#if defined(CONFIG_USB_MUSB_TUSB6010) ||	\
-	defined(CONFIG_USB_MUSB_TUSB6010_MODULE)
-#define MUSB_FIFO_OFFSET(epnum)	(0x200 + ((epnum) * 0x20))
-#else
-#define MUSB_FIFO_OFFSET(epnum)	(0x20 + ((epnum) * 4))
-#endif
-
 /*
  * Additional Control Registers
  */
@@ -295,21 +287,7 @@
 #define MUSB_FIFOSIZE		0x0F
 #define MUSB_CONFIGDATA		MUSB_FIFOSIZE	/* Re-used for EP0 */
 
-/* Offsets to endpoint registers in indexed model (using INDEX register) */
-#define MUSB_INDEXED_OFFSET(_epnum, _offset)	\
-	(0x10 + (_offset))
-
-/* Offsets to endpoint registers in flat models */
-#define MUSB_FLAT_OFFSET(_epnum, _offset)	\
-	(0x100 + (0x10*(_epnum)) + (_offset))
-
-#if defined(CONFIG_USB_MUSB_TUSB6010) ||	\
-	defined(CONFIG_USB_MUSB_TUSB6010_MODULE)
-/* TUSB6010 EP0 configuration register is special */
-#define MUSB_TUSB_OFFSET(_epnum, _offset)	\
-	(0x10 + _offset)
 #include "tusb6010.h"		/* Needed "only" for TUSB_EP0_CONF */
-#endif
 
 #define MUSB_TXCSR_MODE			0x2000
 
@@ -480,10 +458,6 @@ static inline u8  musb_read_txhubport(void __iomem *mbase, u8 epnum)
 #define MUSB_INDEX		USB_OFFSET(USB_INDEX)	/* 8 bit */
 #define MUSB_TESTMODE		USB_OFFSET(USB_TESTMODE)/* 8 bit */
 
-/* Get offset for a given FIFO from musb->mregs */
-#define MUSB_FIFO_OFFSET(epnum)	\
-	(USB_OFFSET(USB_EP0_FIFO) + ((epnum) * 8))
-
 /*
  * Additional Control Registers
  */
diff --git a/drivers/usb/musb/musb_virthub.c b/drivers/usb/musb/musb_virthub.c
index e2d2d8c9891b..b072420e44f5 100644
--- a/drivers/usb/musb/musb_virthub.c
+++ b/drivers/usb/musb/musb_virthub.c
@@ -69,9 +69,10 @@ void musb_host_finish_resume(struct work_struct *work)
 	musb->port1_status |= USB_PORT_STAT_C_SUSPEND << 16;
 	usb_hcd_poll_rh_status(musb->hcd);
 	/* NOTE: it might really be A_WAIT_BCON ... */
-	musb->xceiv->state = OTG_STATE_A_HOST;
+	musb->xceiv->otg->state = OTG_STATE_A_HOST;
 
 	spin_unlock_irqrestore(&musb->lock, flags);
+	musb_host_resume_root_hub(musb);
 }
 
 void musb_port_suspend(struct musb *musb, bool do_suspend)
@@ -107,9 +108,9 @@ void musb_port_suspend(struct musb *musb, bool do_suspend)
 		dev_dbg(musb->controller, "Root port suspended, power %02x\n", power);
 
 		musb->port1_status |= USB_PORT_STAT_SUSPEND;
-		switch (musb->xceiv->state) {
+		switch (musb->xceiv->otg->state) {
 		case OTG_STATE_A_HOST:
-			musb->xceiv->state = OTG_STATE_A_SUSPEND;
+			musb->xceiv->otg->state = OTG_STATE_A_SUSPEND;
 			musb->is_active = otg->host->b_hnp_enable;
 			if (musb->is_active)
 				mod_timer(&musb->otg_timer, jiffies
@@ -118,13 +119,13 @@ void musb_port_suspend(struct musb *musb, bool do_suspend)
 			musb_platform_try_idle(musb, 0);
 			break;
 		case OTG_STATE_B_HOST:
-			musb->xceiv->state = OTG_STATE_B_WAIT_ACON;
+			musb->xceiv->otg->state = OTG_STATE_B_WAIT_ACON;
 			musb->is_active = otg->host->b_hnp_enable;
 			musb_platform_try_idle(musb, 0);
 			break;
 		default:
 			dev_dbg(musb->controller, "bogus rh suspend? %s\n",
-				usb_otg_state_string(musb->xceiv->state));
+				usb_otg_state_string(musb->xceiv->otg->state));
 		}
 	} else if (power & MUSB_POWER_SUSPENDM) {
 		power &= ~MUSB_POWER_SUSPENDM;
@@ -145,7 +146,7 @@ void musb_port_reset(struct musb *musb, bool do_reset)
 	u8		power;
 	void __iomem	*mbase = musb->mregs;
 
-	if (musb->xceiv->state == OTG_STATE_B_IDLE) {
+	if (musb->xceiv->otg->state == OTG_STATE_B_IDLE) {
 		dev_dbg(musb->controller, "HNP: Returning from HNP; no hub reset from b_idle\n");
 		musb->port1_status &= ~USB_PORT_STAT_RESET;
 		return;
@@ -224,24 +225,24 @@ void musb_root_disconnect(struct musb *musb)
 	usb_hcd_poll_rh_status(musb->hcd);
 	musb->is_active = 0;
 
-	switch (musb->xceiv->state) {
+	switch (musb->xceiv->otg->state) {
 	case OTG_STATE_A_SUSPEND:
 		if (otg->host->b_hnp_enable) {
-			musb->xceiv->state = OTG_STATE_A_PERIPHERAL;
+			musb->xceiv->otg->state = OTG_STATE_A_PERIPHERAL;
 			musb->g.is_a_peripheral = 1;
 			break;
 		}
 		/* FALLTHROUGH */
 	case OTG_STATE_A_HOST:
-		musb->xceiv->state = OTG_STATE_A_WAIT_BCON;
+		musb->xceiv->otg->state = OTG_STATE_A_WAIT_BCON;
 		musb->is_active = 0;
 		break;
 	case OTG_STATE_A_WAIT_VFALL:
-		musb->xceiv->state = OTG_STATE_B_IDLE;
+		musb->xceiv->otg->state = OTG_STATE_B_IDLE;
 		break;
 	default:
 		dev_dbg(musb->controller, "host disconnect (%s)\n",
-			usb_otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->otg->state));
 	}
 }
 
diff --git a/drivers/usb/musb/musbhsdma.c b/drivers/usb/musb/musbhsdma.c
index e8e9f9aab203..ab7ec09a8afe 100644
--- a/drivers/usb/musb/musbhsdma.c
+++ b/drivers/usb/musb/musbhsdma.c
@@ -195,6 +195,7 @@ static int dma_channel_abort(struct dma_channel *channel)
 {
 	struct musb_dma_channel *musb_channel = channel->private_data;
 	void __iomem *mbase = musb_channel->controller->base;
+	struct musb *musb = musb_channel->controller->private_data;
 
 	u8 bchannel = musb_channel->idx;
 	int offset;
@@ -202,7 +203,7 @@ static int dma_channel_abort(struct dma_channel *channel)
 
 	if (channel->status == MUSB_DMA_STATUS_BUSY) {
 		if (musb_channel->transmit) {
-			offset = MUSB_EP_OFFSET(musb_channel->epnum,
+			offset = musb->io.ep_offset(musb_channel->epnum,
 						MUSB_TXCSR);
 
 			/*
@@ -215,7 +216,7 @@ static int dma_channel_abort(struct dma_channel *channel)
 			csr &= ~MUSB_TXCSR_DMAMODE;
 			musb_writew(mbase, offset, csr);
 		} else {
-			offset = MUSB_EP_OFFSET(musb_channel->epnum,
+			offset = musb->io.ep_offset(musb_channel->epnum,
 						MUSB_RXCSR);
 
 			csr = musb_readw(mbase, offset);
@@ -326,7 +327,7 @@ static irqreturn_t dma_controller_irq(int irq, void *private_data)
 					    (musb_channel->max_packet_sz - 1)))
 				    ) {
 					u8  epnum  = musb_channel->epnum;
-					int offset = MUSB_EP_OFFSET(epnum,
+					int offset = musb->io.ep_offset(epnum,
 								    MUSB_TXCSR);
 					u16 txcsr;
 
diff --git a/drivers/usb/musb/omap2430.c b/drivers/usb/musb/omap2430.c
index d369bf1f3936..763649eb4987 100644
--- a/drivers/usb/musb/omap2430.c
+++ b/drivers/usb/musb/omap2430.c
@@ -65,15 +65,15 @@ static void musb_do_idle(unsigned long _musb)
 
 	spin_lock_irqsave(&musb->lock, flags);
 
-	switch (musb->xceiv->state) {
+	switch (musb->xceiv->otg->state) {
 	case OTG_STATE_A_WAIT_BCON:
 
 		devctl = musb_readb(musb->mregs, MUSB_DEVCTL);
 		if (devctl & MUSB_DEVCTL_BDEVICE) {
-			musb->xceiv->state = OTG_STATE_B_IDLE;
+			musb->xceiv->otg->state = OTG_STATE_B_IDLE;
 			MUSB_DEV_MODE(musb);
 		} else {
-			musb->xceiv->state = OTG_STATE_A_IDLE;
+			musb->xceiv->otg->state = OTG_STATE_A_IDLE;
 			MUSB_HST_MODE(musb);
 		}
 		break;
@@ -90,15 +90,15 @@ static void musb_do_idle(unsigned long _musb)
 			musb->port1_status |= USB_PORT_STAT_C_SUSPEND << 16;
 			usb_hcd_poll_rh_status(musb->hcd);
 			/* NOTE: it might really be A_WAIT_BCON ... */
-			musb->xceiv->state = OTG_STATE_A_HOST;
+			musb->xceiv->otg->state = OTG_STATE_A_HOST;
 		}
 		break;
 	case OTG_STATE_A_HOST:
 		devctl = musb_readb(musb->mregs, MUSB_DEVCTL);
 		if (devctl &  MUSB_DEVCTL_BDEVICE)
-			musb->xceiv->state = OTG_STATE_B_IDLE;
+			musb->xceiv->otg->state = OTG_STATE_B_IDLE;
 		else
-			musb->xceiv->state = OTG_STATE_A_WAIT_BCON;
+			musb->xceiv->otg->state = OTG_STATE_A_WAIT_BCON;
 	default:
 		break;
 	}
@@ -116,9 +116,9 @@ static void omap2430_musb_try_idle(struct musb *musb, unsigned long timeout)
 
 	/* 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))) {
+			&& (musb->xceiv->otg->state == OTG_STATE_A_WAIT_BCON))) {
 		dev_dbg(musb->controller, "%s active, deleting timer\n",
-			usb_otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->otg->state));
 		del_timer(&musb_idle_timer);
 		last_timer = jiffies;
 		return;
@@ -135,7 +135,7 @@ static void omap2430_musb_try_idle(struct musb *musb, unsigned long timeout)
 	last_timer = timeout;
 
 	dev_dbg(musb->controller, "%s inactive, for idle timer for %lu ms\n",
-		usb_otg_state_string(musb->xceiv->state),
+		usb_otg_state_string(musb->xceiv->otg->state),
 		(unsigned long)jiffies_to_msecs(timeout - jiffies));
 	mod_timer(&musb_idle_timer, timeout);
 }
@@ -153,7 +153,7 @@ static void omap2430_musb_set_vbus(struct musb *musb, int is_on)
 	devctl = musb_readb(musb->mregs, MUSB_DEVCTL);
 
 	if (is_on) {
-		if (musb->xceiv->state == OTG_STATE_A_IDLE) {
+		if (musb->xceiv->otg->state == OTG_STATE_A_IDLE) {
 			int loops = 100;
 			/* start the session */
 			devctl |= MUSB_DEVCTL_SESSION;
@@ -162,7 +162,8 @@ static void omap2430_musb_set_vbus(struct musb *musb, int is_on)
 			 * Wait for the musb to set as A device to enable the
 			 * VBUS
 			 */
-			while (musb_readb(musb->mregs, MUSB_DEVCTL) & 0x80) {
+			while (musb_readb(musb->mregs, MUSB_DEVCTL) &
+			       MUSB_DEVCTL_BDEVICE) {
 
 				mdelay(5);
 				cpu_relax();
@@ -179,7 +180,7 @@ static void omap2430_musb_set_vbus(struct musb *musb, int is_on)
 		} else {
 			musb->is_active = 1;
 			otg->default_a = 1;
-			musb->xceiv->state = OTG_STATE_A_WAIT_VRISE;
+			musb->xceiv->otg->state = OTG_STATE_A_WAIT_VRISE;
 			devctl |= MUSB_DEVCTL_SESSION;
 			MUSB_HST_MODE(musb);
 		}
@@ -191,7 +192,7 @@ static void omap2430_musb_set_vbus(struct musb *musb, int is_on)
 		 */
 
 		otg->default_a = 0;
-		musb->xceiv->state = OTG_STATE_B_IDLE;
+		musb->xceiv->otg->state = OTG_STATE_B_IDLE;
 		devctl &= ~MUSB_DEVCTL_SESSION;
 
 		MUSB_DEV_MODE(musb);
@@ -200,7 +201,7 @@ static void omap2430_musb_set_vbus(struct musb *musb, int is_on)
 
 	dev_dbg(musb->controller, "VBUS %s, devctl %02x "
 		/* otg %3x conf %08x prcm %08x */ "\n",
-		usb_otg_state_string(musb->xceiv->state),
+		usb_otg_state_string(musb->xceiv->otg->state),
 		musb_readb(musb->mregs, MUSB_DEVCTL));
 }
 
@@ -265,7 +266,7 @@ static void omap_musb_set_mailbox(struct omap2430_glue *glue)
 		dev_dbg(dev, "ID GND\n");
 
 		otg->default_a = true;
-		musb->xceiv->state = OTG_STATE_A_IDLE;
+		musb->xceiv->otg->state = OTG_STATE_A_IDLE;
 		musb->xceiv->last_event = USB_EVENT_ID;
 		if (musb->gadget_driver) {
 			pm_runtime_get_sync(dev);
@@ -279,7 +280,7 @@ static void omap_musb_set_mailbox(struct omap2430_glue *glue)
 		dev_dbg(dev, "VBUS Connect\n");
 
 		otg->default_a = false;
-		musb->xceiv->state = OTG_STATE_B_IDLE;
+		musb->xceiv->otg->state = OTG_STATE_B_IDLE;
 		musb->xceiv->last_event = USB_EVENT_VBUS;
 		if (musb->gadget_driver)
 			pm_runtime_get_sync(dev);
@@ -518,10 +519,8 @@ static int omap2430_probe(struct platform_device *pdev)
 	int				ret = -ENOMEM;
 
 	glue = devm_kzalloc(&pdev->dev, sizeof(*glue), GFP_KERNEL);
-	if (!glue) {
-		dev_err(&pdev->dev, "failed to allocate glue context\n");
+	if (!glue)
 		goto err0;
-	}
 
 	musb = platform_device_alloc("musb-hdrc", PLATFORM_DEVID_AUTO);
 	if (!musb) {
@@ -543,25 +542,16 @@ static int omap2430_probe(struct platform_device *pdev)
 		struct platform_device *control_pdev;
 
 		pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
-		if (!pdata) {
-			dev_err(&pdev->dev,
-				"failed to allocate musb platform data\n");
+		if (!pdata)
 			goto err2;
-		}
 
 		data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
-		if (!data) {
-			dev_err(&pdev->dev,
-				"failed to allocate musb board data\n");
+		if (!data)
 			goto err2;
-		}
 
 		config = devm_kzalloc(&pdev->dev, sizeof(*config), GFP_KERNEL);
-		if (!config) {
-			dev_err(&pdev->dev,
-				"failed to allocate musb hdrc config\n");
+		if (!config)
 			goto err2;
-		}
 
 		of_property_read_u32(np, "mode", (u32 *)&pdata->mode);
 		of_property_read_u32(np, "interface-type",
diff --git a/drivers/usb/musb/tusb6010.c b/drivers/usb/musb/tusb6010.c
index 2daa779f1382..3a5ffd575438 100644
--- a/drivers/usb/musb/tusb6010.c
+++ b/drivers/usb/musb/tusb6010.c
@@ -126,6 +126,52 @@ static void tusb_wbus_quirk(struct musb *musb, int enabled)
 	}
 }
 
+static u32 tusb_fifo_offset(u8 epnum)
+{
+	return 0x200 + (epnum * 0x20);
+}
+
+static u32 tusb_ep_offset(u8 epnum, u16 offset)
+{
+	return 0x10 + offset;
+}
+
+/* TUSB mapping: "flat" plus ep0 special cases */
+static void tusb_ep_select(void __iomem *mbase, u8 epnum)
+{
+	musb_writeb(mbase, MUSB_INDEX, epnum);
+}
+
+/*
+ * TUSB6010 doesn't allow 8-bit access; 16-bit access is the minimum.
+ */
+static u8 tusb_readb(const void __iomem *addr, unsigned offset)
+{
+	u16 tmp;
+	u8 val;
+
+	tmp = __raw_readw(addr + (offset & ~1));
+	if (offset & 1)
+		val = (tmp >> 8);
+	else
+		val = tmp & 0xff;
+
+	return val;
+}
+
+static void tusb_writeb(void __iomem *addr, unsigned offset, u8 data)
+{
+	u16 tmp;
+
+	tmp = __raw_readw(addr + (offset & ~1));
+	if (offset & 1)
+		tmp = (data << 8) | (tmp & 0xff);
+	else
+		tmp = (tmp & 0xff00) | data;
+
+	__raw_writew(tmp, addr + (offset & ~1));
+}
+
 /*
  * TUSB 6010 may use a parallel bus that doesn't support byte ops;
  * so both loading and unloading FIFOs need explicit byte counts.
@@ -173,7 +219,7 @@ static inline void tusb_fifo_read_unaligned(void __iomem *fifo,
 	}
 }
 
-void musb_write_fifo(struct musb_hw_ep *hw_ep, u16 len, const u8 *buf)
+static void tusb_write_fifo(struct musb_hw_ep *hw_ep, u16 len, const u8 *buf)
 {
 	struct musb *musb = hw_ep->musb;
 	void __iomem	*ep_conf = hw_ep->conf;
@@ -223,7 +269,7 @@ void musb_write_fifo(struct musb_hw_ep *hw_ep, u16 len, const u8 *buf)
 		tusb_fifo_write_unaligned(fifo, buf, len);
 }
 
-void musb_read_fifo(struct musb_hw_ep *hw_ep, u16 len, u8 *buf)
+static void tusb_read_fifo(struct musb_hw_ep *hw_ep, u16 len, u8 *buf)
 {
 	struct musb *musb = hw_ep->musb;
 	void __iomem	*ep_conf = hw_ep->conf;
@@ -415,13 +461,13 @@ static void musb_do_idle(unsigned long _musb)
 
 	spin_lock_irqsave(&musb->lock, flags);
 
-	switch (musb->xceiv->state) {
+	switch (musb->xceiv->otg->state) {
 	case OTG_STATE_A_WAIT_BCON:
 		if ((musb->a_wait_bcon != 0)
 			&& (musb->idle_timeout == 0
 				|| time_after(jiffies, musb->idle_timeout))) {
 			dev_dbg(musb->controller, "Nothing connected %s, turning off VBUS\n",
-					usb_otg_state_string(musb->xceiv->state));
+					usb_otg_state_string(musb->xceiv->otg->state));
 		}
 		/* FALLTHROUGH */
 	case OTG_STATE_A_IDLE:
@@ -474,9 +520,9 @@ static void tusb_musb_try_idle(struct musb *musb, unsigned long timeout)
 
 	/* 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))) {
+			&& (musb->xceiv->otg->state == OTG_STATE_A_WAIT_BCON))) {
 		dev_dbg(musb->controller, "%s active, deleting timer\n",
-			usb_otg_state_string(musb->xceiv->state));
+			usb_otg_state_string(musb->xceiv->otg->state));
 		del_timer(&musb_idle_timer);
 		last_timer = jiffies;
 		return;
@@ -493,7 +539,7 @@ static void tusb_musb_try_idle(struct musb *musb, unsigned long timeout)
 	last_timer = timeout;
 
 	dev_dbg(musb->controller, "%s inactive, for idle timer for %lu ms\n",
-		usb_otg_state_string(musb->xceiv->state),
+		usb_otg_state_string(musb->xceiv->otg->state),
 		(unsigned long)jiffies_to_msecs(timeout - jiffies));
 	mod_timer(&musb_idle_timer, timeout);
 }
@@ -524,7 +570,7 @@ static void tusb_musb_set_vbus(struct musb *musb, int is_on)
 	if (is_on) {
 		timer = OTG_TIMER_MS(OTG_TIME_A_WAIT_VRISE);
 		otg->default_a = 1;
-		musb->xceiv->state = OTG_STATE_A_WAIT_VRISE;
+		musb->xceiv->otg->state = OTG_STATE_A_WAIT_VRISE;
 		devctl |= MUSB_DEVCTL_SESSION;
 
 		conf |= TUSB_DEV_CONF_USB_HOST_MODE;
@@ -537,16 +583,16 @@ static void tusb_musb_set_vbus(struct musb *musb, int is_on)
 		/* If ID pin is grounded, we want to be a_idle */
 		otg_stat = musb_readl(tbase, TUSB_DEV_OTG_STAT);
 		if (!(otg_stat & TUSB_DEV_OTG_STAT_ID_STATUS)) {
-			switch (musb->xceiv->state) {
+			switch (musb->xceiv->otg->state) {
 			case OTG_STATE_A_WAIT_VRISE:
 			case OTG_STATE_A_WAIT_BCON:
-				musb->xceiv->state = OTG_STATE_A_WAIT_VFALL;
+				musb->xceiv->otg->state = OTG_STATE_A_WAIT_VFALL;
 				break;
 			case OTG_STATE_A_WAIT_VFALL:
-				musb->xceiv->state = OTG_STATE_A_IDLE;
+				musb->xceiv->otg->state = OTG_STATE_A_IDLE;
 				break;
 			default:
-				musb->xceiv->state = OTG_STATE_A_IDLE;
+				musb->xceiv->otg->state = OTG_STATE_A_IDLE;
 			}
 			musb->is_active = 0;
 			otg->default_a = 1;
@@ -554,7 +600,7 @@ static void tusb_musb_set_vbus(struct musb *musb, int is_on)
 		} else {
 			musb->is_active = 0;
 			otg->default_a = 0;
-			musb->xceiv->state = OTG_STATE_B_IDLE;
+			musb->xceiv->otg->state = OTG_STATE_B_IDLE;
 			MUSB_DEV_MODE(musb);
 		}
 
@@ -569,7 +615,7 @@ static void tusb_musb_set_vbus(struct musb *musb, int is_on)
 	musb_writeb(musb->mregs, MUSB_DEVCTL, devctl);
 
 	dev_dbg(musb->controller, "VBUS %s, devctl %02x otg %3x conf %08x prcm %08x\n",
-		usb_otg_state_string(musb->xceiv->state),
+		usb_otg_state_string(musb->xceiv->otg->state),
 		musb_readb(musb->mregs, MUSB_DEVCTL),
 		musb_readl(tbase, TUSB_DEV_OTG_STAT),
 		conf, prcm);
@@ -668,23 +714,23 @@ tusb_otg_ints(struct musb *musb, u32 int_src, void __iomem *tbase)
 
 			if (otg_stat & TUSB_DEV_OTG_STAT_SESS_END) {
 				dev_dbg(musb->controller, "Forcing disconnect (no interrupt)\n");
-				if (musb->xceiv->state != OTG_STATE_B_IDLE) {
+				if (musb->xceiv->otg->state != OTG_STATE_B_IDLE) {
 					/* INTR_DISCONNECT can hide... */
-					musb->xceiv->state = OTG_STATE_B_IDLE;
+					musb->xceiv->otg->state = OTG_STATE_B_IDLE;
 					musb->int_usb |= MUSB_INTR_DISCONNECT;
 				}
 				musb->is_active = 0;
 			}
 			dev_dbg(musb->controller, "vbus change, %s, otg %03x\n",
-				usb_otg_state_string(musb->xceiv->state), otg_stat);
+				usb_otg_state_string(musb->xceiv->otg->state), otg_stat);
 			idle_timeout = jiffies + (1 * HZ);
 			schedule_work(&musb->irq_work);
 
 		} else /* A-dev state machine */ {
 			dev_dbg(musb->controller, "vbus change, %s, otg %03x\n",
-				usb_otg_state_string(musb->xceiv->state), otg_stat);
+				usb_otg_state_string(musb->xceiv->otg->state), otg_stat);
 
-			switch (musb->xceiv->state) {
+			switch (musb->xceiv->otg->state) {
 			case OTG_STATE_A_IDLE:
 				dev_dbg(musb->controller, "Got SRP, turning on VBUS\n");
 				musb_platform_set_vbus(musb, 1);
@@ -731,9 +777,9 @@ tusb_otg_ints(struct musb *musb, u32 int_src, void __iomem *tbase)
 		u8	devctl;
 
 		dev_dbg(musb->controller, "%s timer, %03x\n",
-			usb_otg_state_string(musb->xceiv->state), otg_stat);
+			usb_otg_state_string(musb->xceiv->otg->state), otg_stat);
 
-		switch (musb->xceiv->state) {
+		switch (musb->xceiv->otg->state) {
 		case OTG_STATE_A_WAIT_VRISE:
 			/* VBUS has probably been valid for a while now,
 			 * but may well have bounced out of range a bit
@@ -745,7 +791,7 @@ tusb_otg_ints(struct musb *musb, u32 int_src, void __iomem *tbase)
 					dev_dbg(musb->controller, "devctl %02x\n", devctl);
 					break;
 				}
-				musb->xceiv->state = OTG_STATE_A_WAIT_BCON;
+				musb->xceiv->otg->state = OTG_STATE_A_WAIT_BCON;
 				musb->is_active = 0;
 				idle_timeout = jiffies
 					+ msecs_to_jiffies(musb->a_wait_bcon);
@@ -1135,9 +1181,17 @@ static int tusb_musb_exit(struct musb *musb)
 }
 
 static const struct musb_platform_ops tusb_ops = {
+	.quirks		= MUSB_IN_TUSB,
 	.init		= tusb_musb_init,
 	.exit		= tusb_musb_exit,
 
+	.ep_offset	= tusb_ep_offset,
+	.ep_select	= tusb_ep_select,
+	.fifo_offset	= tusb_fifo_offset,
+	.readb		= tusb_readb,
+	.writeb		= tusb_writeb,
+	.read_fifo	= tusb_read_fifo,
+	.write_fifo	= tusb_write_fifo,
 	.enable		= tusb_musb_enable,
 	.disable	= tusb_musb_disable,
 
@@ -1164,10 +1218,8 @@ static int tusb_probe(struct platform_device *pdev)
 	int				ret;
 
 	glue = devm_kzalloc(&pdev->dev, sizeof(*glue), GFP_KERNEL);
-	if (!glue) {
-		dev_err(&pdev->dev, "failed to allocate glue context\n");
+	if (!glue)
 		return -ENOMEM;
-	}
 
 	glue->dev			= &pdev->dev;
 
diff --git a/drivers/usb/musb/ux500.c b/drivers/usb/musb/ux500.c
index dc666e96f45f..abf72728825f 100644
--- a/drivers/usb/musb/ux500.c
+++ b/drivers/usb/musb/ux500.c
@@ -56,7 +56,7 @@ static void ux500_musb_set_vbus(struct musb *musb, int is_on)
 	devctl = musb_readb(musb->mregs, MUSB_DEVCTL);
 
 	if (is_on) {
-		if (musb->xceiv->state == OTG_STATE_A_IDLE) {
+		if (musb->xceiv->otg->state == OTG_STATE_A_IDLE) {
 			/* start the session */
 			devctl |= MUSB_DEVCTL_SESSION;
 			musb_writeb(musb->mregs, MUSB_DEVCTL, devctl);
@@ -76,7 +76,7 @@ static void ux500_musb_set_vbus(struct musb *musb, int is_on)
 		} else {
 			musb->is_active = 1;
 			musb->xceiv->otg->default_a = 1;
-			musb->xceiv->state = OTG_STATE_A_WAIT_VRISE;
+			musb->xceiv->otg->state = OTG_STATE_A_WAIT_VRISE;
 			devctl |= MUSB_DEVCTL_SESSION;
 			MUSB_HST_MODE(musb);
 		}
@@ -102,7 +102,7 @@ static void ux500_musb_set_vbus(struct musb *musb, int is_on)
 		mdelay(200);
 
 	dev_dbg(musb->controller, "VBUS %s, devctl %02x\n",
-		usb_otg_state_string(musb->xceiv->state),
+		usb_otg_state_string(musb->xceiv->otg->state),
 		musb_readb(musb->mregs, MUSB_DEVCTL));
 }
 
@@ -112,7 +112,7 @@ static int musb_otg_notifications(struct notifier_block *nb,
 	struct musb *musb = container_of(nb, struct musb, nb);
 
 	dev_dbg(musb->controller, "musb_otg_notifications %ld %s\n",
-			event, usb_otg_state_string(musb->xceiv->state));
+			event, usb_otg_state_string(musb->xceiv->otg->state));
 
 	switch (event) {
 	case UX500_MUSB_ID:
@@ -127,7 +127,7 @@ static int musb_otg_notifications(struct notifier_block *nb,
 		if (is_host_active(musb))
 			ux500_musb_set_vbus(musb, 0);
 		else
-			musb->xceiv->state = OTG_STATE_B_IDLE;
+			musb->xceiv->otg->state = OTG_STATE_B_IDLE;
 		break;
 	default:
 		dev_dbg(musb->controller, "ID float\n");
@@ -188,8 +188,10 @@ static int ux500_musb_exit(struct musb *musb)
 }
 
 static const struct musb_platform_ops ux500_ops = {
+	.quirks		= MUSB_INDEXED_EP,
 	.init		= ux500_musb_init,
 	.exit		= ux500_musb_exit,
+	.fifo_mode	= 5,
 
 	.set_vbus	= ux500_musb_set_vbus,
 };
@@ -247,10 +249,8 @@ static int ux500_probe(struct platform_device *pdev)
 	}
 
 	glue = devm_kzalloc(&pdev->dev, sizeof(*glue), GFP_KERNEL);
-	if (!glue) {
-		dev_err(&pdev->dev, "failed to allocate glue context\n");
+	if (!glue)
 		goto err0;
-	}
 
 	musb = platform_device_alloc("musb-hdrc", PLATFORM_DEVID_AUTO);
 	if (!musb) {
diff --git a/drivers/usb/musb/ux500_dma.c b/drivers/usb/musb/ux500_dma.c
index 221faed9f074..e93845c26bdb 100644
--- a/drivers/usb/musb/ux500_dma.c
+++ b/drivers/usb/musb/ux500_dma.c
@@ -91,9 +91,9 @@ static bool ux500_configure_channel(struct dma_channel *channel,
 	struct scatterlist sg;
 	struct dma_slave_config slave_conf;
 	enum dma_slave_buswidth addr_width;
-	dma_addr_t usb_fifo_addr = (MUSB_FIFO_OFFSET(hw_ep->epnum) +
-					ux500_channel->controller->phy_base);
 	struct musb *musb = ux500_channel->controller->private_data;
+	dma_addr_t usb_fifo_addr = (musb->io.fifo_offset(hw_ep->epnum) +
+					ux500_channel->controller->phy_base);
 
 	dev_dbg(musb->controller,
 		"packet_sz=%d, mode=%d, dma_addr=0x%llx, len=%d is_tx=%d\n",
@@ -121,8 +121,7 @@ static bool ux500_configure_channel(struct dma_channel *channel,
 	slave_conf.dst_maxburst = 16;
 	slave_conf.device_fc = false;
 
-	dma_chan->device->device_control(dma_chan, DMA_SLAVE_CONFIG,
-					     (unsigned long) &slave_conf);
+	dmaengine_slave_config(dma_chan, &slave_conf);
 
 	dma_desc = dmaengine_prep_slave_sg(dma_chan, &sg, 1, direction,
 					     DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
@@ -246,9 +245,7 @@ static int ux500_dma_channel_abort(struct dma_channel *channel)
 			musb_writew(epio, MUSB_RXCSR, csr);
 		}
 
-		ux500_channel->dma_chan->device->
-				device_control(ux500_channel->dma_chan,
-					DMA_TERMINATE_ALL, 0);
+		dmaengine_terminate_all(ux500_channel->dma_chan);
 		channel->status = MUSB_DMA_STATUS_FREE;
 	}
 	return 0;
diff --git a/drivers/usb/phy/Kconfig b/drivers/usb/phy/Kconfig
index 0cd1f44f0ee8..c6d0c8e745b9 100644
--- a/drivers/usb/phy/Kconfig
+++ b/drivers/usb/phy/Kconfig
@@ -20,7 +20,7 @@ config AB8500_USB
 
 config FSL_USB2_OTG
 	bool "Freescale USB OTG Transceiver Driver"
-	depends on USB_EHCI_FSL && USB_FSL_USB2 && USB_OTG_FSM && PM_RUNTIME
+	depends on USB_EHCI_FSL && USB_FSL_USB2 && USB_OTG_FSM && PM
 	select USB_OTG
 	select USB_PHY
 	help
@@ -153,7 +153,7 @@ config USB_MSM_OTG
 
 config USB_MV_OTG
 	tristate "Marvell USB OTG support"
-	depends on USB_EHCI_MV && USB_MV_UDC && PM_RUNTIME
+	depends on USB_EHCI_MV && USB_MV_UDC && PM
 	select USB_OTG
 	select USB_PHY
 	help
diff --git a/drivers/usb/phy/phy-ab8500-usb.c b/drivers/usb/phy/phy-ab8500-usb.c
index 11ab2c45e462..0b1bd2369293 100644
--- a/drivers/usb/phy/phy-ab8500-usb.c
+++ b/drivers/usb/phy/phy-ab8500-usb.c
@@ -1,6 +1,4 @@
 /*
- * drivers/usb/otg/ab8500_usb.c
- *
  * USB transceiver driver for AB8500 family chips
  *
  * Copyright (C) 2010-2013 ST-Ericsson AB
@@ -446,7 +444,8 @@ static int ab9540_usb_link_status_update(struct ab8500_usb *ab,
 		if (event != UX500_MUSB_RIDB)
 			event = UX500_MUSB_NONE;
 		/* Fallback to default B_IDLE as nothing is connected. */
-		ab->phy.state = OTG_STATE_B_IDLE;
+		ab->phy.otg->state = OTG_STATE_B_IDLE;
+		usb_phy_set_event(&ab->phy, USB_EVENT_NONE);
 		break;
 
 	case USB_LINK_ACA_RID_C_NM_9540:
@@ -461,12 +460,14 @@ static int ab9540_usb_link_status_update(struct ab8500_usb *ab,
 			ab8500_usb_peri_phy_en(ab);
 			atomic_notifier_call_chain(&ab->phy.notifier,
 					UX500_MUSB_PREPARE, &ab->vbus_draw);
+			usb_phy_set_event(&ab->phy, USB_EVENT_ENUMERATED);
 		}
 		if (ab->mode == USB_IDLE) {
 			ab->mode = USB_PERIPHERAL;
 			ab8500_usb_peri_phy_en(ab);
 			atomic_notifier_call_chain(&ab->phy.notifier,
 					UX500_MUSB_PREPARE, &ab->vbus_draw);
+			usb_phy_set_event(&ab->phy, USB_EVENT_ENUMERATED);
 		}
 		if (event != UX500_MUSB_RIDC)
 			event = UX500_MUSB_VBUS;
@@ -502,6 +503,7 @@ static int ab9540_usb_link_status_update(struct ab8500_usb *ab,
 		event = UX500_MUSB_CHARGER;
 		atomic_notifier_call_chain(&ab->phy.notifier,
 				event, &ab->vbus_draw);
+		usb_phy_set_event(&ab->phy, USB_EVENT_CHARGER);
 		break;
 
 	case USB_LINK_PHYEN_NO_VBUS_NO_IDGND_9540:
@@ -526,6 +528,7 @@ static int ab9540_usb_link_status_update(struct ab8500_usb *ab,
 				ab->mode = USB_IDLE;
 				ab->phy.otg->default_a = false;
 				ab->vbus_draw = 0;
+				usb_phy_set_event(&ab->phy, USB_EVENT_NONE);
 			}
 		}
 		break;
@@ -584,7 +587,8 @@ static int ab8540_usb_link_status_update(struct ab8500_usb *ab,
 		 * Fallback to default B_IDLE as nothing
 		 * is connected
 		 */
-		ab->phy.state = OTG_STATE_B_IDLE;
+		ab->phy.otg->state = OTG_STATE_B_IDLE;
+		usb_phy_set_event(&ab->phy, USB_EVENT_NONE);
 		break;
 
 	case USB_LINK_ACA_RID_C_NM_8540:
@@ -598,6 +602,7 @@ static int ab8540_usb_link_status_update(struct ab8500_usb *ab,
 			ab8500_usb_peri_phy_en(ab);
 			atomic_notifier_call_chain(&ab->phy.notifier,
 					UX500_MUSB_PREPARE, &ab->vbus_draw);
+			usb_phy_set_event(&ab->phy, USB_EVENT_ENUMERATED);
 		}
 		if (event != UX500_MUSB_RIDC)
 			event = UX500_MUSB_VBUS;
@@ -626,6 +631,7 @@ static int ab8540_usb_link_status_update(struct ab8500_usb *ab,
 		event = UX500_MUSB_CHARGER;
 		atomic_notifier_call_chain(&ab->phy.notifier,
 				event, &ab->vbus_draw);
+		usb_phy_set_event(&ab->phy, USB_EVENT_CHARGER);
 		break;
 
 	case USB_LINK_PHYEN_NO_VBUS_NO_IDGND_8540:
@@ -648,6 +654,7 @@ static int ab8540_usb_link_status_update(struct ab8500_usb *ab,
 			ab->mode = USB_IDLE;
 			ab->phy.otg->default_a = false;
 			ab->vbus_draw = 0;
+		usb_phy_set_event(&ab->phy, USB_EVENT_NONE);
 		}
 		break;
 
@@ -693,7 +700,8 @@ static int ab8505_usb_link_status_update(struct ab8500_usb *ab,
 		 * Fallback to default B_IDLE as nothing
 		 * is connected
 		 */
-		ab->phy.state = OTG_STATE_B_IDLE;
+		ab->phy.otg->state = OTG_STATE_B_IDLE;
+		usb_phy_set_event(&ab->phy, USB_EVENT_NONE);
 		break;
 
 	case USB_LINK_ACA_RID_C_NM_8505:
@@ -707,6 +715,7 @@ static int ab8505_usb_link_status_update(struct ab8500_usb *ab,
 			ab8500_usb_peri_phy_en(ab);
 			atomic_notifier_call_chain(&ab->phy.notifier,
 					UX500_MUSB_PREPARE, &ab->vbus_draw);
+			usb_phy_set_event(&ab->phy, USB_EVENT_ENUMERATED);
 		}
 		if (event != UX500_MUSB_RIDC)
 			event = UX500_MUSB_VBUS;
@@ -734,6 +743,7 @@ static int ab8505_usb_link_status_update(struct ab8500_usb *ab,
 		event = UX500_MUSB_CHARGER;
 		atomic_notifier_call_chain(&ab->phy.notifier,
 				event, &ab->vbus_draw);
+		usb_phy_set_event(&ab->phy, USB_EVENT_CHARGER);
 		break;
 
 	default:
@@ -776,7 +786,8 @@ static int ab8500_usb_link_status_update(struct ab8500_usb *ab,
 		if (event != UX500_MUSB_RIDB)
 			event = UX500_MUSB_NONE;
 		/* Fallback to default B_IDLE as nothing is connected */
-		ab->phy.state = OTG_STATE_B_IDLE;
+		ab->phy.otg->state = OTG_STATE_B_IDLE;
+		usb_phy_set_event(&ab->phy, USB_EVENT_NONE);
 		break;
 
 	case USB_LINK_ACA_RID_C_NM_8500:
@@ -794,6 +805,7 @@ static int ab8500_usb_link_status_update(struct ab8500_usb *ab,
 			ab8500_usb_peri_phy_en(ab);
 			atomic_notifier_call_chain(&ab->phy.notifier,
 					UX500_MUSB_PREPARE, &ab->vbus_draw);
+			usb_phy_set_event(&ab->phy, USB_EVENT_ENUMERATED);
 		}
 		if (event != UX500_MUSB_RIDC)
 			event = UX500_MUSB_VBUS;
@@ -820,6 +832,7 @@ static int ab8500_usb_link_status_update(struct ab8500_usb *ab,
 		event = UX500_MUSB_CHARGER;
 		atomic_notifier_call_chain(&ab->phy.notifier,
 				event, &ab->vbus_draw);
+		usb_phy_set_event(&ab->phy, USB_EVENT_CHARGER);
 		break;
 
 	case USB_LINK_RESERVED_8500:
@@ -1056,7 +1069,7 @@ static int ab8500_usb_set_peripheral(struct usb_otg *otg,
 	if (!otg)
 		return -ENODEV;
 
-	ab = phy_to_ab(otg->phy);
+	ab = phy_to_ab(otg->usb_phy);
 
 	ab->phy.otg->gadget = gadget;
 
@@ -1080,7 +1093,7 @@ static int ab8500_usb_set_host(struct usb_otg *otg, struct usb_bus *host)
 	if (!otg)
 		return -ENODEV;
 
-	ab = phy_to_ab(otg->phy);
+	ab = phy_to_ab(otg->usb_phy);
 
 	ab->phy.otg->host = host;
 
@@ -1380,9 +1393,9 @@ static int ab8500_usb_probe(struct platform_device *pdev)
 	ab->phy.label		= "ab8500";
 	ab->phy.set_suspend	= ab8500_usb_set_suspend;
 	ab->phy.set_power	= ab8500_usb_set_power;
-	ab->phy.state		= OTG_STATE_UNDEFINED;
+	ab->phy.otg->state	= OTG_STATE_UNDEFINED;
 
-	otg->phy		= &ab->phy;
+	otg->usb_phy		= &ab->phy;
 	otg->set_host		= ab8500_usb_set_host;
 	otg->set_peripheral	= ab8500_usb_set_peripheral;
 
@@ -1505,7 +1518,6 @@ static struct platform_driver ab8500_usb_driver = {
 	.id_table	= ab8500_usb_devtype,
 	.driver		= {
 		.name	= "abx5x0-usb",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/usb/phy/phy-am335x-control.c b/drivers/usb/phy/phy-am335x-control.c
index 35b6083b7999..403fab772724 100644
--- a/drivers/usb/phy/phy-am335x-control.c
+++ b/drivers/usb/phy/phy-am335x-control.c
@@ -147,10 +147,8 @@ static int am335x_control_usb_probe(struct platform_device *pdev)
 	phy_ctrl = of_id->data;
 
 	ctrl_usb = devm_kzalloc(&pdev->dev, sizeof(*ctrl_usb), GFP_KERNEL);
-	if (!ctrl_usb) {
-		dev_err(&pdev->dev, "unable to alloc memory for control usb\n");
+	if (!ctrl_usb)
 		return -ENOMEM;
-	}
 
 	ctrl_usb->dev = &pdev->dev;
 
@@ -175,7 +173,6 @@ static struct platform_driver am335x_control_driver = {
 	.probe		= am335x_control_usb_probe,
 	.driver		= {
 		.name	= "am335x-control-usb",
-		.owner	= THIS_MODULE,
 		.of_match_table = omap_control_usb_id_table,
 	},
 };
diff --git a/drivers/usb/phy/phy-am335x.c b/drivers/usb/phy/phy-am335x.c
index b70e05537180..90b67a4ca221 100644
--- a/drivers/usb/phy/phy-am335x.c
+++ b/drivers/usb/phy/phy-am335x.c
@@ -137,7 +137,6 @@ static struct platform_driver am335x_phy_driver = {
 	.remove         = am335x_phy_remove,
 	.driver         = {
 		.name   = "am335x-phy-driver",
-		.owner  = THIS_MODULE,
 		.pm = &am335x_pm_ops,
 		.of_match_table = am335x_phy_ids,
 	},
diff --git a/drivers/usb/phy/phy-fsl-usb.c b/drivers/usb/phy/phy-fsl-usb.c
index f1ea5990a50a..ab38aa32a6c1 100644
--- a/drivers/usb/phy/phy-fsl-usb.c
+++ b/drivers/usb/phy/phy-fsl-usb.c
@@ -274,7 +274,7 @@ void b_srp_end(unsigned long foo)
 	fsl_otg_dischrg_vbus(0);
 	srp_wait_done = 1;
 
-	if ((fsl_otg_dev->phy.state == OTG_STATE_B_SRP_INIT) &&
+	if ((fsl_otg_dev->phy.otg->state == OTG_STATE_B_SRP_INIT) &&
 	    fsl_otg_dev->fsm.b_sess_vld)
 		fsl_otg_dev->fsm.b_srp_done = 1;
 }
@@ -499,7 +499,8 @@ int fsl_otg_start_host(struct otg_fsm *fsm, int on)
 {
 	struct usb_otg *otg = fsm->otg;
 	struct device *dev;
-	struct fsl_otg *otg_dev = container_of(otg->phy, struct fsl_otg, phy);
+	struct fsl_otg *otg_dev =
+		container_of(otg->usb_phy, struct fsl_otg, phy);
 	u32 retval = 0;
 
 	if (!otg->host)
@@ -594,7 +595,7 @@ static int fsl_otg_set_host(struct usb_otg *otg, struct usb_bus *host)
 	if (!otg)
 		return -ENODEV;
 
-	otg_dev = container_of(otg->phy, struct fsl_otg, phy);
+	otg_dev = container_of(otg->usb_phy, struct fsl_otg, phy);
 	if (otg_dev != fsl_otg_dev)
 		return -ENODEV;
 
@@ -623,7 +624,7 @@ static int fsl_otg_set_host(struct usb_otg *otg, struct usb_bus *host)
 			/* Mini-A cable connected */
 			struct otg_fsm *fsm = &otg_dev->fsm;
 
-			otg->phy->state = OTG_STATE_UNDEFINED;
+			otg->state = OTG_STATE_UNDEFINED;
 			fsm->protocol = PROTO_UNDEF;
 		}
 	}
@@ -644,7 +645,7 @@ static int fsl_otg_set_peripheral(struct usb_otg *otg,
 	if (!otg)
 		return -ENODEV;
 
-	otg_dev = container_of(otg->phy, struct fsl_otg, phy);
+	otg_dev = container_of(otg->usb_phy, struct fsl_otg, phy);
 	VDBG("otg_dev 0x%x\n", (int)otg_dev);
 	VDBG("fsl_otg_dev 0x%x\n", (int)fsl_otg_dev);
 	if (otg_dev != fsl_otg_dev)
@@ -681,7 +682,7 @@ static int fsl_otg_set_power(struct usb_phy *phy, unsigned mA)
 {
 	if (!fsl_otg_dev)
 		return -ENODEV;
-	if (phy->state == OTG_STATE_B_PERIPHERAL)
+	if (phy->otg->state == OTG_STATE_B_PERIPHERAL)
 		pr_info("FSL OTG: Draw %d mA\n", mA);
 
 	return 0;
@@ -714,10 +715,10 @@ static int fsl_otg_start_srp(struct usb_otg *otg)
 {
 	struct fsl_otg *otg_dev;
 
-	if (!otg || otg->phy->state != OTG_STATE_B_IDLE)
+	if (!otg || otg->state != OTG_STATE_B_IDLE)
 		return -ENODEV;
 
-	otg_dev = container_of(otg->phy, struct fsl_otg, phy);
+	otg_dev = container_of(otg->usb_phy, struct fsl_otg, phy);
 	if (otg_dev != fsl_otg_dev)
 		return -ENODEV;
 
@@ -735,7 +736,7 @@ static int fsl_otg_start_hnp(struct usb_otg *otg)
 	if (!otg)
 		return -ENODEV;
 
-	otg_dev = container_of(otg->phy, struct fsl_otg, phy);
+	otg_dev = container_of(otg->usb_phy, struct fsl_otg, phy);
 	if (otg_dev != fsl_otg_dev)
 		return -ENODEV;
 
@@ -857,7 +858,7 @@ static int fsl_otg_conf(struct platform_device *pdev)
 	fsl_otg_tc->phy.dev = &pdev->dev;
 	fsl_otg_tc->phy.set_power = fsl_otg_set_power;
 
-	fsl_otg_tc->phy.otg->phy = &fsl_otg_tc->phy;
+	fsl_otg_tc->phy.otg->usb_phy = &fsl_otg_tc->phy;
 	fsl_otg_tc->phy.otg->set_host = fsl_otg_set_host;
 	fsl_otg_tc->phy.otg->set_peripheral = fsl_otg_set_peripheral;
 	fsl_otg_tc->phy.otg->start_hnp = fsl_otg_start_hnp;
@@ -989,10 +990,10 @@ int usb_otg_start(struct platform_device *pdev)
 	 * Also: record initial state of ID pin
 	 */
 	if (fsl_readl(&p_otg->dr_mem_map->otgsc) & OTGSC_STS_USB_ID) {
-		p_otg->phy.state = OTG_STATE_UNDEFINED;
+		p_otg->phy.otg->state = OTG_STATE_UNDEFINED;
 		p_otg->fsm.id = 1;
 	} else {
-		p_otg->phy.state = OTG_STATE_A_IDLE;
+		p_otg->phy.otg->state = OTG_STATE_A_IDLE;
 		p_otg->fsm.id = 0;
 	}
 
@@ -1047,7 +1048,7 @@ static int show_fsl_usb2_otg_state(struct device *dev,
 	/* State */
 	t = scnprintf(next, size,
 		      "OTG state: %s\n\n",
-		      usb_otg_state_string(fsl_otg_dev->phy.state));
+		      usb_otg_state_string(fsl_otg_dev->phy.otg->state));
 	size -= t;
 	next += t;
 
diff --git a/drivers/usb/phy/phy-fsl-usb.h b/drivers/usb/phy/phy-fsl-usb.h
index 5986c96354df..23149954a09c 100644
--- a/drivers/usb/phy/phy-fsl-usb.h
+++ b/drivers/usb/phy/phy-fsl-usb.h
@@ -298,7 +298,7 @@
 /* SE0 Time Before SRP */
 #define TB_SE0_SRP	(2)	/* b_idle,minimum 2 ms, section:5.3.2 */
 
-#define SET_OTG_STATE(otg_ptr, newstate)	((otg_ptr)->state = newstate)
+#define SET_OTG_STATE(phy, newstate)	((phy)->otg->state = newstate)
 
 struct usb_dr_mmap {
 	/* Capability register */
diff --git a/drivers/usb/phy/phy-generic.c b/drivers/usb/phy/phy-generic.c
index 7594e5069ae5..f1b719b45a53 100644
--- a/drivers/usb/phy/phy-generic.c
+++ b/drivers/usb/phy/phy-generic.c
@@ -1,6 +1,4 @@
 /*
- * drivers/usb/otg/nop-usb-xceiv.c
- *
  * NOP USB transceiver for all USB transceiver which are either built-in
  * into USB IP or which are mostly autonomous.
  *
@@ -123,7 +121,7 @@ static int nop_set_peripheral(struct usb_otg *otg, struct usb_gadget *gadget)
 	}
 
 	otg->gadget = gadget;
-	otg->phy->state = OTG_STATE_B_IDLE;
+	otg->state = OTG_STATE_B_IDLE;
 	return 0;
 }
 
@@ -225,10 +223,10 @@ int usb_phy_gen_create_phy(struct device *dev, struct usb_phy_generic *nop,
 	nop->phy.dev		= nop->dev;
 	nop->phy.label		= "nop-xceiv";
 	nop->phy.set_suspend	= nop_set_suspend;
-	nop->phy.state		= OTG_STATE_UNDEFINED;
 	nop->phy.type		= type;
 
-	nop->phy.otg->phy		= &nop->phy;
+	nop->phy.otg->state		= OTG_STATE_UNDEFINED;
+	nop->phy.otg->usb_phy		= &nop->phy;
 	nop->phy.otg->set_host		= nop_set_host;
 	nop->phy.otg->set_peripheral	= nop_set_peripheral;
 
@@ -286,7 +284,6 @@ static struct platform_driver usb_phy_generic_driver = {
 	.remove		= usb_phy_generic_remove,
 	.driver		= {
 		.name	= "usb_phy_generic",
-		.owner	= THIS_MODULE,
 		.of_match_table = nop_xceiv_dt_ids,
 	},
 };
diff --git a/drivers/usb/phy/phy-gpio-vbus-usb.c b/drivers/usb/phy/phy-gpio-vbus-usb.c
index f4b14bd97e14..f66120db8a41 100644
--- a/drivers/usb/phy/phy-gpio-vbus-usb.c
+++ b/drivers/usb/phy/phy-gpio-vbus-usb.c
@@ -121,7 +121,7 @@ static void gpio_vbus_work(struct work_struct *work)
 
 	if (vbus) {
 		status = USB_EVENT_VBUS;
-		gpio_vbus->phy.state = OTG_STATE_B_PERIPHERAL;
+		gpio_vbus->phy.otg->state = OTG_STATE_B_PERIPHERAL;
 		gpio_vbus->phy.last_event = status;
 		usb_gadget_vbus_connect(gpio_vbus->phy.otg->gadget);
 
@@ -134,6 +134,7 @@ static void gpio_vbus_work(struct work_struct *work)
 
 		atomic_notifier_call_chain(&gpio_vbus->phy.notifier,
 					   status, gpio_vbus->phy.otg->gadget);
+		usb_phy_set_event(&gpio_vbus->phy, USB_EVENT_ENUMERATED);
 	} else {
 		/* optionally disable D+ pullup */
 		if (gpio_is_valid(gpio))
@@ -143,11 +144,12 @@ static void gpio_vbus_work(struct work_struct *work)
 
 		usb_gadget_vbus_disconnect(gpio_vbus->phy.otg->gadget);
 		status = USB_EVENT_NONE;
-		gpio_vbus->phy.state = OTG_STATE_B_IDLE;
+		gpio_vbus->phy.otg->state = OTG_STATE_B_IDLE;
 		gpio_vbus->phy.last_event = status;
 
 		atomic_notifier_call_chain(&gpio_vbus->phy.notifier,
 					   status, gpio_vbus->phy.otg->gadget);
+		usb_phy_set_event(&gpio_vbus->phy, USB_EVENT_NONE);
 	}
 }
 
@@ -180,7 +182,7 @@ static int gpio_vbus_set_peripheral(struct usb_otg *otg,
 	struct platform_device *pdev;
 	int gpio;
 
-	gpio_vbus = container_of(otg->phy, struct gpio_vbus_data, phy);
+	gpio_vbus = container_of(otg->usb_phy, struct gpio_vbus_data, phy);
 	pdev = to_platform_device(gpio_vbus->dev);
 	pdata = dev_get_platdata(gpio_vbus->dev);
 	gpio = pdata->gpio_pullup;
@@ -196,7 +198,7 @@ static int gpio_vbus_set_peripheral(struct usb_otg *otg,
 		set_vbus_draw(gpio_vbus, 0);
 
 		usb_gadget_vbus_disconnect(otg->gadget);
-		otg->phy->state = OTG_STATE_UNDEFINED;
+		otg->state = OTG_STATE_UNDEFINED;
 
 		otg->gadget = NULL;
 		return 0;
@@ -218,7 +220,7 @@ static int gpio_vbus_set_power(struct usb_phy *phy, unsigned mA)
 
 	gpio_vbus = container_of(phy, struct gpio_vbus_data, phy);
 
-	if (phy->state == OTG_STATE_B_PERIPHERAL)
+	if (phy->otg->state == OTG_STATE_B_PERIPHERAL)
 		set_vbus_draw(gpio_vbus, mA);
 	return 0;
 }
@@ -269,9 +271,9 @@ static int gpio_vbus_probe(struct platform_device *pdev)
 	gpio_vbus->phy.dev = gpio_vbus->dev;
 	gpio_vbus->phy.set_power = gpio_vbus_set_power;
 	gpio_vbus->phy.set_suspend = gpio_vbus_set_suspend;
-	gpio_vbus->phy.state = OTG_STATE_UNDEFINED;
 
-	gpio_vbus->phy.otg->phy = &gpio_vbus->phy;
+	gpio_vbus->phy.otg->state = OTG_STATE_UNDEFINED;
+	gpio_vbus->phy.otg->usb_phy = &gpio_vbus->phy;
 	gpio_vbus->phy.otg->set_peripheral = gpio_vbus_set_peripheral;
 
 	err = devm_gpio_request(&pdev->dev, gpio, "vbus_detect");
@@ -380,7 +382,6 @@ MODULE_ALIAS("platform:gpio-vbus");
 static struct platform_driver gpio_vbus_driver = {
 	.driver = {
 		.name  = "gpio-vbus",
-		.owner = THIS_MODULE,
 #ifdef CONFIG_PM
 		.pm = &gpio_vbus_dev_pm_ops,
 #endif
diff --git a/drivers/usb/phy/phy-isp1301-omap.c b/drivers/usb/phy/phy-isp1301-omap.c
index 8eea56d3ded6..1e0e10dd6ba5 100644
--- a/drivers/usb/phy/phy-isp1301-omap.c
+++ b/drivers/usb/phy/phy-isp1301-omap.c
@@ -234,7 +234,7 @@ isp1301_clear_bits(struct isp1301 *isp, u8 reg, u8 bits)
 
 static inline const char *state_name(struct isp1301 *isp)
 {
-	return usb_otg_state_string(isp->phy.state);
+	return usb_otg_state_string(isp->phy.otg->state);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -249,7 +249,7 @@ static inline const char *state_name(struct isp1301 *isp)
 
 static void power_down(struct isp1301 *isp)
 {
-	isp->phy.state = OTG_STATE_UNDEFINED;
+	isp->phy.otg->state = OTG_STATE_UNDEFINED;
 
 	// isp1301_set_bits(isp, ISP1301_MODE_CONTROL_2, MC2_GLOBAL_PWR_DN);
 	isp1301_set_bits(isp, ISP1301_MODE_CONTROL_1, MC1_SUSPEND);
@@ -339,7 +339,7 @@ static void a_idle(struct isp1301 *isp, const char *tag)
 {
 	u32 l;
 
-	if (isp->phy.state == OTG_STATE_A_IDLE)
+	if (isp->phy.otg->state == OTG_STATE_A_IDLE)
 		return;
 
 	isp->phy.otg->default_a = 1;
@@ -351,7 +351,7 @@ static void a_idle(struct isp1301 *isp, const char *tag)
 		isp->phy.otg->gadget->is_a_peripheral = 1;
 		gadget_suspend(isp);
 	}
-	isp->phy.state = OTG_STATE_A_IDLE;
+	isp->phy.otg->state = OTG_STATE_A_IDLE;
 	l = omap_readl(OTG_CTRL) & OTG_XCEIV_OUTPUTS;
 	omap_writel(l, OTG_CTRL);
 	isp->last_otg_ctrl = l;
@@ -363,7 +363,7 @@ static void b_idle(struct isp1301 *isp, const char *tag)
 {
 	u32 l;
 
-	if (isp->phy.state == OTG_STATE_B_IDLE)
+	if (isp->phy.otg->state == OTG_STATE_B_IDLE)
 		return;
 
 	isp->phy.otg->default_a = 0;
@@ -375,7 +375,7 @@ static void b_idle(struct isp1301 *isp, const char *tag)
 		isp->phy.otg->gadget->is_a_peripheral = 0;
 		gadget_suspend(isp);
 	}
-	isp->phy.state = OTG_STATE_B_IDLE;
+	isp->phy.otg->state = OTG_STATE_B_IDLE;
 	l = omap_readl(OTG_CTRL) & OTG_XCEIV_OUTPUTS;
 	omap_writel(l, OTG_CTRL);
 	isp->last_otg_ctrl = l;
@@ -474,7 +474,7 @@ static void check_state(struct isp1301 *isp, const char *tag)
 	default:
 		break;
 	}
-	if (isp->phy.state == state && !extra)
+	if (isp->phy.otg->state == state && !extra)
 		return;
 	pr_debug("otg: %s FSM %s/%02x, %s, %06x\n", tag,
 		usb_otg_state_string(state), fsm, state_name(isp),
@@ -498,23 +498,23 @@ static void update_otg1(struct isp1301 *isp, u8 int_src)
 
 	if (int_src & INTR_SESS_VLD)
 		otg_ctrl |= OTG_ASESSVLD;
-	else if (isp->phy.state == OTG_STATE_A_WAIT_VFALL) {
+	else if (isp->phy.otg->state == OTG_STATE_A_WAIT_VFALL) {
 		a_idle(isp, "vfall");
 		otg_ctrl &= ~OTG_CTRL_BITS;
 	}
 	if (int_src & INTR_VBUS_VLD)
 		otg_ctrl |= OTG_VBUSVLD;
 	if (int_src & INTR_ID_GND) {		/* default-A */
-		if (isp->phy.state == OTG_STATE_B_IDLE
-				|| isp->phy.state
+		if (isp->phy.otg->state == OTG_STATE_B_IDLE
+				|| isp->phy.otg->state
 					== OTG_STATE_UNDEFINED) {
 			a_idle(isp, "init");
 			return;
 		}
 	} else {				/* default-B */
 		otg_ctrl |= OTG_ID;
-		if (isp->phy.state == OTG_STATE_A_IDLE
-			|| isp->phy.state == OTG_STATE_UNDEFINED) {
+		if (isp->phy.otg->state == OTG_STATE_A_IDLE
+			|| isp->phy.otg->state == OTG_STATE_UNDEFINED) {
 			b_idle(isp, "init");
 			return;
 		}
@@ -548,14 +548,14 @@ static void otg_update_isp(struct isp1301 *isp)
 	isp->last_otg_ctrl = otg_ctrl;
 	otg_ctrl = otg_ctrl & OTG_XCEIV_INPUTS;
 
-	switch (isp->phy.state) {
+	switch (isp->phy.otg->state) {
 	case OTG_STATE_B_IDLE:
 	case OTG_STATE_B_PERIPHERAL:
 	case OTG_STATE_B_SRP_INIT:
 		if (!(otg_ctrl & OTG_PULLUP)) {
 			// if (otg_ctrl & OTG_B_HNPEN) {
 			if (isp->phy.otg->gadget->b_hnp_enable) {
-				isp->phy.state = OTG_STATE_B_WAIT_ACON;
+				isp->phy.otg->state = OTG_STATE_B_WAIT_ACON;
 				pr_debug("  --> b_wait_acon\n");
 			}
 			goto pulldown;
@@ -585,7 +585,7 @@ pulldown:
 	if (!(isp->phy.otg->host))
 		otg_ctrl &= ~OTG_DRV_VBUS;
 
-	switch (isp->phy.state) {
+	switch (isp->phy.otg->state) {
 	case OTG_STATE_A_SUSPEND:
 		if (otg_ctrl & OTG_DRV_VBUS) {
 			set |= OTG1_VBUS_DRV;
@@ -596,7 +596,7 @@ pulldown:
 
 		/* FALLTHROUGH */
 	case OTG_STATE_A_VBUS_ERR:
-		isp->phy.state = OTG_STATE_A_WAIT_VFALL;
+		isp->phy.otg->state = OTG_STATE_A_WAIT_VFALL;
 		pr_debug("  --> a_wait_vfall\n");
 		/* FALLTHROUGH */
 	case OTG_STATE_A_WAIT_VFALL:
@@ -605,7 +605,7 @@ pulldown:
 		break;
 	case OTG_STATE_A_IDLE:
 		if (otg_ctrl & OTG_DRV_VBUS) {
-			isp->phy.state = OTG_STATE_A_WAIT_VRISE;
+			isp->phy.otg->state = OTG_STATE_A_WAIT_VRISE;
 			pr_debug("  --> a_wait_vrise\n");
 		}
 		/* FALLTHROUGH */
@@ -625,17 +625,17 @@ pulldown:
 	if (otg_change & OTG_PULLUP) {
 		u32 l;
 
-		switch (isp->phy.state) {
+		switch (isp->phy.otg->state) {
 		case OTG_STATE_B_IDLE:
 			if (clr & OTG1_DP_PULLUP)
 				break;
-			isp->phy.state = OTG_STATE_B_PERIPHERAL;
+			isp->phy.otg->state = OTG_STATE_B_PERIPHERAL;
 			pr_debug("  --> b_peripheral\n");
 			break;
 		case OTG_STATE_A_SUSPEND:
 			if (clr & OTG1_DP_PULLUP)
 				break;
-			isp->phy.state = OTG_STATE_A_PERIPHERAL;
+			isp->phy.otg->state = OTG_STATE_A_PERIPHERAL;
 			pr_debug("  --> a_peripheral\n");
 			break;
 		default:
@@ -673,7 +673,7 @@ static irqreturn_t omap_otg_irq(int irq, void *_isp)
 		 * remote wakeup (SRP, normal) using their own timer
 		 * to give "check cable and A-device" messages.
 		 */
-		if (isp->phy.state == OTG_STATE_B_SRP_INIT)
+		if (isp->phy.otg->state == OTG_STATE_B_SRP_INIT)
 			b_idle(isp, "srp_timeout");
 
 		omap_writew(B_SRP_TMROUT, OTG_IRQ_SRC);
@@ -691,7 +691,7 @@ static irqreturn_t omap_otg_irq(int irq, void *_isp)
 		omap_writel(otg_ctrl, OTG_CTRL);
 
 		/* subset of b_peripheral()... */
-		isp->phy.state = OTG_STATE_B_PERIPHERAL;
+		isp->phy.otg->state = OTG_STATE_B_PERIPHERAL;
 		pr_debug("  --> b_peripheral\n");
 
 		omap_writew(B_HNP_FAIL, OTG_IRQ_SRC);
@@ -703,7 +703,7 @@ static irqreturn_t omap_otg_irq(int irq, void *_isp)
 				state_name(isp), omap_readl(OTG_CTRL));
 
 		isp1301_defer_work(isp, WORK_UPDATE_OTG);
-		switch (isp->phy.state) {
+		switch (isp->phy.otg->state) {
 		case OTG_STATE_A_IDLE:
 			if (!otg->host)
 				break;
@@ -734,7 +734,7 @@ static irqreturn_t omap_otg_irq(int irq, void *_isp)
 		otg_ctrl |= OTG_BUSDROP;
 		otg_ctrl &= ~OTG_A_BUSREQ & OTG_CTRL_MASK & ~OTG_XCEIV_INPUTS;
 		omap_writel(otg_ctrl, OTG_CTRL);
-		isp->phy.state = OTG_STATE_A_WAIT_VFALL;
+		isp->phy.otg->state = OTG_STATE_A_WAIT_VFALL;
 
 		omap_writew(A_REQ_TMROUT, OTG_IRQ_SRC);
 		ret = IRQ_HANDLED;
@@ -748,7 +748,7 @@ static irqreturn_t omap_otg_irq(int irq, void *_isp)
 		otg_ctrl |= OTG_BUSDROP;
 		otg_ctrl &= ~OTG_A_BUSREQ & OTG_CTRL_MASK & ~OTG_XCEIV_INPUTS;
 		omap_writel(otg_ctrl, OTG_CTRL);
-		isp->phy.state = OTG_STATE_A_VBUS_ERR;
+		isp->phy.otg->state = OTG_STATE_A_VBUS_ERR;
 
 		omap_writew(A_VBUS_ERR, OTG_IRQ_SRC);
 		ret = IRQ_HANDLED;
@@ -769,7 +769,7 @@ static irqreturn_t omap_otg_irq(int irq, void *_isp)
 
 		/* role is peripheral */
 		if (otg_ctrl & OTG_DRIVER_SEL) {
-			switch (isp->phy.state) {
+			switch (isp->phy.otg->state) {
 			case OTG_STATE_A_IDLE:
 				b_idle(isp, __func__);
 				break;
@@ -786,18 +786,18 @@ static irqreturn_t omap_otg_irq(int irq, void *_isp)
 			}
 
 			if (otg->host) {
-				switch (isp->phy.state) {
+				switch (isp->phy.otg->state) {
 				case OTG_STATE_B_WAIT_ACON:
-					isp->phy.state = OTG_STATE_B_HOST;
+					isp->phy.otg->state = OTG_STATE_B_HOST;
 					pr_debug("  --> b_host\n");
 					kick = 1;
 					break;
 				case OTG_STATE_A_WAIT_BCON:
-					isp->phy.state = OTG_STATE_A_HOST;
+					isp->phy.otg->state = OTG_STATE_A_HOST;
 					pr_debug("  --> a_host\n");
 					break;
 				case OTG_STATE_A_PERIPHERAL:
-					isp->phy.state = OTG_STATE_A_WAIT_BCON;
+					isp->phy.otg->state = OTG_STATE_A_WAIT_BCON;
 					pr_debug("  --> a_wait_bcon\n");
 					break;
 				default:
@@ -878,7 +878,6 @@ static struct platform_driver omap_otg_driver = {
 	.probe		= otg_probe,
 	.remove		= otg_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "omap_otg",
 	},
 };
@@ -937,7 +936,7 @@ static void b_peripheral(struct isp1301 *isp)
 	/* UDC driver just set OTG_BSESSVLD */
 	isp1301_set_bits(isp, ISP1301_OTG_CONTROL_1, OTG1_DP_PULLUP);
 	isp1301_clear_bits(isp, ISP1301_OTG_CONTROL_1, OTG1_DP_PULLDOWN);
-	isp->phy.state = OTG_STATE_B_PERIPHERAL;
+	isp->phy.otg->state = OTG_STATE_B_PERIPHERAL;
 	pr_debug("  --> b_peripheral\n");
 	dump_regs(isp, "2periph");
 #endif
@@ -947,7 +946,7 @@ static void isp_update_otg(struct isp1301 *isp, u8 stat)
 {
 	struct usb_otg		*otg = isp->phy.otg;
 	u8			isp_stat, isp_bstat;
-	enum usb_otg_state	state = isp->phy.state;
+	enum usb_otg_state	state = isp->phy.otg->state;
 
 	if (stat & INTR_BDIS_ACON)
 		pr_debug("OTG:  BDIS_ACON, %s\n", state_name(isp));
@@ -970,7 +969,7 @@ static void isp_update_otg(struct isp1301 *isp, u8 stat)
 				 * when HNP is used.
 				 */
 				if (isp_stat & INTR_VBUS_VLD)
-					isp->phy.state = OTG_STATE_A_HOST;
+					isp->phy.otg->state = OTG_STATE_A_HOST;
 				break;
 			case OTG_STATE_A_WAIT_VFALL:
 				if (!(isp_stat & INTR_SESS_VLD))
@@ -978,7 +977,7 @@ static void isp_update_otg(struct isp1301 *isp, u8 stat)
 				break;
 			default:
 				if (!(isp_stat & INTR_VBUS_VLD))
-					isp->phy.state = OTG_STATE_A_VBUS_ERR;
+					isp->phy.otg->state = OTG_STATE_A_VBUS_ERR;
 				break;
 			}
 			isp_bstat = isp1301_get_u8(isp, ISP1301_OTG_STATUS);
@@ -1007,7 +1006,7 @@ static void isp_update_otg(struct isp1301 *isp, u8 stat)
 		if (otg->default_a) {
 			switch (state) {
 			default:
-				isp->phy.state = OTG_STATE_A_WAIT_VFALL;
+				isp->phy.otg->state = OTG_STATE_A_WAIT_VFALL;
 				break;
 			case OTG_STATE_A_WAIT_VFALL:
 				state = OTG_STATE_A_IDLE;
@@ -1020,7 +1019,7 @@ static void isp_update_otg(struct isp1301 *isp, u8 stat)
 				host_suspend(isp);
 				isp1301_clear_bits(isp, ISP1301_MODE_CONTROL_1,
 						MC1_BDIS_ACON_EN);
-				isp->phy.state = OTG_STATE_B_IDLE;
+				isp->phy.otg->state = OTG_STATE_B_IDLE;
 				l = omap_readl(OTG_CTRL) & OTG_CTRL_MASK;
 				l &= ~OTG_CTRL_BITS;
 				omap_writel(l, OTG_CTRL);
@@ -1031,7 +1030,7 @@ static void isp_update_otg(struct isp1301 *isp, u8 stat)
 		}
 		isp_bstat = isp1301_get_u8(isp, ISP1301_OTG_STATUS);
 
-		switch (isp->phy.state) {
+		switch (isp->phy.otg->state) {
 		case OTG_STATE_B_PERIPHERAL:
 		case OTG_STATE_B_WAIT_ACON:
 		case OTG_STATE_B_HOST:
@@ -1071,7 +1070,7 @@ static void isp_update_otg(struct isp1301 *isp, u8 stat)
 		}
 	}
 
-	if (state != isp->phy.state)
+	if (state != isp->phy.otg->state)
 		pr_debug("  isp, %s -> %s\n",
 				usb_otg_state_string(state), state_name(isp));
 
@@ -1129,10 +1128,10 @@ isp1301_work(struct work_struct *work)
 			 * skip A_WAIT_VRISE; hc transitions invisibly
 			 * skip A_WAIT_BCON; same.
 			 */
-			switch (isp->phy.state) {
+			switch (isp->phy.otg->state) {
 			case OTG_STATE_A_WAIT_BCON:
 			case OTG_STATE_A_WAIT_VRISE:
-				isp->phy.state = OTG_STATE_A_HOST;
+				isp->phy.otg->state = OTG_STATE_A_HOST;
 				pr_debug("  --> a_host\n");
 				otg_ctrl = omap_readl(OTG_CTRL);
 				otg_ctrl |= OTG_A_BUSREQ;
@@ -1141,7 +1140,7 @@ isp1301_work(struct work_struct *work)
 				omap_writel(otg_ctrl, OTG_CTRL);
 				break;
 			case OTG_STATE_B_WAIT_ACON:
-				isp->phy.state = OTG_STATE_B_HOST;
+				isp->phy.otg->state = OTG_STATE_B_HOST;
 				pr_debug("  --> b_host (acon)\n");
 				break;
 			case OTG_STATE_B_HOST:
@@ -1275,7 +1274,7 @@ static int isp1301_otg_enable(struct isp1301 *isp)
 static int
 isp1301_set_host(struct usb_otg *otg, struct usb_bus *host)
 {
-	struct isp1301	*isp = container_of(otg->phy, struct isp1301, phy);
+	struct isp1301	*isp = container_of(otg->usb_phy, struct isp1301, phy);
 
 	if (isp != the_transceiver)
 		return -ENODEV;
@@ -1331,7 +1330,7 @@ isp1301_set_host(struct usb_otg *otg, struct usb_bus *host)
 static int
 isp1301_set_peripheral(struct usb_otg *otg, struct usb_gadget *gadget)
 {
-	struct isp1301	*isp = container_of(otg->phy, struct isp1301, phy);
+	struct isp1301	*isp = container_of(otg->usb_phy, struct isp1301, phy);
 
 	if (isp != the_transceiver)
 		return -ENODEV;
@@ -1368,7 +1367,7 @@ isp1301_set_peripheral(struct usb_otg *otg, struct usb_gadget *gadget)
 	}
 
 	power_up(isp);
-	isp->phy.state = OTG_STATE_B_IDLE;
+	isp->phy.otg->state = OTG_STATE_B_IDLE;
 
 	if (machine_is_omap_h2() || machine_is_omap_h3())
 		isp1301_set_bits(isp, ISP1301_MODE_CONTROL_1, MC1_DAT_SE0);
@@ -1403,7 +1402,7 @@ isp1301_set_power(struct usb_phy *dev, unsigned mA)
 {
 	if (!the_transceiver)
 		return -ENODEV;
-	if (dev->state == OTG_STATE_B_PERIPHERAL)
+	if (dev->otg->state == OTG_STATE_B_PERIPHERAL)
 		enable_vbus_draw(the_transceiver, mA);
 	return 0;
 }
@@ -1411,10 +1410,10 @@ isp1301_set_power(struct usb_phy *dev, unsigned mA)
 static int
 isp1301_start_srp(struct usb_otg *otg)
 {
-	struct isp1301	*isp = container_of(otg->phy, struct isp1301, phy);
+	struct isp1301	*isp = container_of(otg->usb_phy, struct isp1301, phy);
 	u32		otg_ctrl;
 
-	if (isp != the_transceiver || isp->phy.state != OTG_STATE_B_IDLE)
+	if (isp != the_transceiver || isp->phy.otg->state != OTG_STATE_B_IDLE)
 		return -ENODEV;
 
 	otg_ctrl = omap_readl(OTG_CTRL);
@@ -1424,7 +1423,7 @@ isp1301_start_srp(struct usb_otg *otg)
 	otg_ctrl |= OTG_B_BUSREQ;
 	otg_ctrl &= ~OTG_A_BUSREQ & OTG_CTRL_MASK;
 	omap_writel(otg_ctrl, OTG_CTRL);
-	isp->phy.state = OTG_STATE_B_SRP_INIT;
+	isp->phy.otg->state = OTG_STATE_B_SRP_INIT;
 
 	pr_debug("otg: SRP, %s ... %06x\n", state_name(isp),
 			omap_readl(OTG_CTRL));
@@ -1438,7 +1437,7 @@ static int
 isp1301_start_hnp(struct usb_otg *otg)
 {
 #ifdef	CONFIG_USB_OTG
-	struct isp1301	*isp = container_of(otg->phy, struct isp1301, phy);
+	struct isp1301	*isp = container_of(otg->usb_phy, struct isp1301, phy);
 	u32 l;
 
 	if (isp != the_transceiver)
@@ -1452,9 +1451,9 @@ isp1301_start_hnp(struct usb_otg *otg)
 	/* We want hardware to manage most HNP protocol timings.
 	 * So do this part as early as possible...
 	 */
-	switch (isp->phy.state) {
+	switch (isp->phy.otg->state) {
 	case OTG_STATE_B_HOST:
-		isp->phy.state = OTG_STATE_B_PERIPHERAL;
+		isp->phy.otg->state = OTG_STATE_B_PERIPHERAL;
 		/* caller will suspend next */
 		break;
 	case OTG_STATE_A_HOST:
@@ -1583,7 +1582,7 @@ isp1301_probe(struct i2c_client *i2c, const struct i2c_device_id *id)
 	isp->phy.label = DRIVER_NAME;
 	isp->phy.set_power = isp1301_set_power,
 
-	isp->phy.otg->phy = &isp->phy;
+	isp->phy.otg->usb_phy = &isp->phy;
 	isp->phy.otg->set_host = isp1301_set_host,
 	isp->phy.otg->set_peripheral = isp1301_set_peripheral,
 	isp->phy.otg->start_srp = isp1301_start_srp,
diff --git a/drivers/usb/phy/phy-keystone.c b/drivers/usb/phy/phy-keystone.c
index f4d722de912b..e0556f7832b5 100644
--- a/drivers/usb/phy/phy-keystone.c
+++ b/drivers/usb/phy/phy-keystone.c
@@ -123,7 +123,6 @@ static struct platform_driver keystone_usbphy_driver = {
 	.remove         = keystone_usbphy_remove,
 	.driver         = {
 		.name   = "keystone-usbphy",
-		.owner  = THIS_MODULE,
 		.of_match_table = keystone_usbphy_ids,
 	},
 };
diff --git a/drivers/usb/phy/phy-msm-usb.c b/drivers/usb/phy/phy-msm-usb.c
index 29be0e654ecc..000fd892455f 100644
--- a/drivers/usb/phy/phy-msm-usb.c
+++ b/drivers/usb/phy/phy-msm-usb.c
@@ -708,7 +708,7 @@ static void msm_otg_start_host(struct usb_phy *phy, int on)
 
 static int msm_otg_set_host(struct usb_otg *otg, struct usb_bus *host)
 {
-	struct msm_otg *motg = container_of(otg->phy, struct msm_otg, phy);
+	struct msm_otg *motg = container_of(otg->usb_phy, struct msm_otg, phy);
 	struct usb_hcd *hcd;
 
 	/*
@@ -716,16 +716,16 @@ static int msm_otg_set_host(struct usb_otg *otg, struct usb_bus *host)
 	 * only peripheral configuration.
 	 */
 	if (motg->pdata->mode == USB_DR_MODE_PERIPHERAL) {
-		dev_info(otg->phy->dev, "Host mode is not supported\n");
+		dev_info(otg->usb_phy->dev, "Host mode is not supported\n");
 		return -ENODEV;
 	}
 
 	if (!host) {
-		if (otg->phy->state == OTG_STATE_A_HOST) {
-			pm_runtime_get_sync(otg->phy->dev);
-			msm_otg_start_host(otg->phy, 0);
+		if (otg->state == OTG_STATE_A_HOST) {
+			pm_runtime_get_sync(otg->usb_phy->dev);
+			msm_otg_start_host(otg->usb_phy, 0);
 			otg->host = NULL;
-			otg->phy->state = OTG_STATE_UNDEFINED;
+			otg->state = OTG_STATE_UNDEFINED;
 			schedule_work(&motg->sm_work);
 		} else {
 			otg->host = NULL;
@@ -738,14 +738,14 @@ static int msm_otg_set_host(struct usb_otg *otg, struct usb_bus *host)
 	hcd->power_budget = motg->pdata->power_budget;
 
 	otg->host = host;
-	dev_dbg(otg->phy->dev, "host driver registered w/ tranceiver\n");
+	dev_dbg(otg->usb_phy->dev, "host driver registered w/ tranceiver\n");
 
 	/*
 	 * Kick the state machine work, if peripheral is not supported
 	 * or peripheral is already registered with us.
 	 */
 	if (motg->pdata->mode == USB_DR_MODE_HOST || otg->gadget) {
-		pm_runtime_get_sync(otg->phy->dev);
+		pm_runtime_get_sync(otg->usb_phy->dev);
 		schedule_work(&motg->sm_work);
 	}
 
@@ -782,23 +782,23 @@ static void msm_otg_start_peripheral(struct usb_phy *phy, int on)
 static int msm_otg_set_peripheral(struct usb_otg *otg,
 					struct usb_gadget *gadget)
 {
-	struct msm_otg *motg = container_of(otg->phy, struct msm_otg, phy);
+	struct msm_otg *motg = container_of(otg->usb_phy, struct msm_otg, phy);
 
 	/*
 	 * Fail peripheral registration if this board can support
 	 * only host configuration.
 	 */
 	if (motg->pdata->mode == USB_DR_MODE_HOST) {
-		dev_info(otg->phy->dev, "Peripheral mode is not supported\n");
+		dev_info(otg->usb_phy->dev, "Peripheral mode is not supported\n");
 		return -ENODEV;
 	}
 
 	if (!gadget) {
-		if (otg->phy->state == OTG_STATE_B_PERIPHERAL) {
-			pm_runtime_get_sync(otg->phy->dev);
-			msm_otg_start_peripheral(otg->phy, 0);
+		if (otg->state == OTG_STATE_B_PERIPHERAL) {
+			pm_runtime_get_sync(otg->usb_phy->dev);
+			msm_otg_start_peripheral(otg->usb_phy, 0);
 			otg->gadget = NULL;
-			otg->phy->state = OTG_STATE_UNDEFINED;
+			otg->state = OTG_STATE_UNDEFINED;
 			schedule_work(&motg->sm_work);
 		} else {
 			otg->gadget = NULL;
@@ -807,14 +807,15 @@ static int msm_otg_set_peripheral(struct usb_otg *otg,
 		return 0;
 	}
 	otg->gadget = gadget;
-	dev_dbg(otg->phy->dev, "peripheral driver registered w/ tranceiver\n");
+	dev_dbg(otg->usb_phy->dev,
+		"peripheral driver registered w/ tranceiver\n");
 
 	/*
 	 * Kick the state machine work, if host is not supported
 	 * or host is already registered with us.
 	 */
 	if (motg->pdata->mode == USB_DR_MODE_PERIPHERAL || otg->host) {
-		pm_runtime_get_sync(otg->phy->dev);
+		pm_runtime_get_sync(otg->usb_phy->dev);
 		schedule_work(&motg->sm_work);
 	}
 
@@ -1170,20 +1171,20 @@ static void msm_otg_sm_work(struct work_struct *w)
 	struct msm_otg *motg = container_of(w, struct msm_otg, sm_work);
 	struct usb_otg *otg = motg->phy.otg;
 
-	switch (otg->phy->state) {
+	switch (otg->state) {
 	case OTG_STATE_UNDEFINED:
-		dev_dbg(otg->phy->dev, "OTG_STATE_UNDEFINED state\n");
-		msm_otg_reset(otg->phy);
+		dev_dbg(otg->usb_phy->dev, "OTG_STATE_UNDEFINED state\n");
+		msm_otg_reset(otg->usb_phy);
 		msm_otg_init_sm(motg);
-		otg->phy->state = OTG_STATE_B_IDLE;
+		otg->state = OTG_STATE_B_IDLE;
 		/* FALL THROUGH */
 	case OTG_STATE_B_IDLE:
-		dev_dbg(otg->phy->dev, "OTG_STATE_B_IDLE state\n");
+		dev_dbg(otg->usb_phy->dev, "OTG_STATE_B_IDLE state\n");
 		if (!test_bit(ID, &motg->inputs) && otg->host) {
 			/* disable BSV bit */
 			writel(readl(USB_OTGSC) & ~OTGSC_BSVIE, USB_OTGSC);
-			msm_otg_start_host(otg->phy, 1);
-			otg->phy->state = OTG_STATE_A_HOST;
+			msm_otg_start_host(otg->usb_phy, 1);
+			otg->state = OTG_STATE_A_HOST;
 		} else if (test_bit(B_SESS_VLD, &motg->inputs)) {
 			switch (motg->chg_state) {
 			case USB_CHG_STATE_UNDEFINED:
@@ -1198,14 +1199,16 @@ static void msm_otg_sm_work(struct work_struct *w)
 				case USB_CDP_CHARGER:
 					msm_otg_notify_charger(motg,
 							IDEV_CHG_MAX);
-					msm_otg_start_peripheral(otg->phy, 1);
-					otg->phy->state
+					msm_otg_start_peripheral(otg->usb_phy,
+								 1);
+					otg->state
 						= OTG_STATE_B_PERIPHERAL;
 					break;
 				case USB_SDP_CHARGER:
 					msm_otg_notify_charger(motg, IUNIT);
-					msm_otg_start_peripheral(otg->phy, 1);
-					otg->phy->state
+					msm_otg_start_peripheral(otg->usb_phy,
+								 1);
+					otg->state
 						= OTG_STATE_B_PERIPHERAL;
 					break;
 				default:
@@ -1222,36 +1225,36 @@ static void msm_otg_sm_work(struct work_struct *w)
 			 * is incremented in charger detection work.
 			 */
 			if (cancel_delayed_work_sync(&motg->chg_work)) {
-				pm_runtime_put_sync(otg->phy->dev);
-				msm_otg_reset(otg->phy);
+				pm_runtime_put_sync(otg->usb_phy->dev);
+				msm_otg_reset(otg->usb_phy);
 			}
 			msm_otg_notify_charger(motg, 0);
 			motg->chg_state = USB_CHG_STATE_UNDEFINED;
 			motg->chg_type = USB_INVALID_CHARGER;
 		}
 
-		if (otg->phy->state == OTG_STATE_B_IDLE)
-			pm_runtime_put_sync(otg->phy->dev);
+		if (otg->state == OTG_STATE_B_IDLE)
+			pm_runtime_put_sync(otg->usb_phy->dev);
 		break;
 	case OTG_STATE_B_PERIPHERAL:
-		dev_dbg(otg->phy->dev, "OTG_STATE_B_PERIPHERAL state\n");
+		dev_dbg(otg->usb_phy->dev, "OTG_STATE_B_PERIPHERAL state\n");
 		if (!test_bit(B_SESS_VLD, &motg->inputs) ||
 				!test_bit(ID, &motg->inputs)) {
 			msm_otg_notify_charger(motg, 0);
-			msm_otg_start_peripheral(otg->phy, 0);
+			msm_otg_start_peripheral(otg->usb_phy, 0);
 			motg->chg_state = USB_CHG_STATE_UNDEFINED;
 			motg->chg_type = USB_INVALID_CHARGER;
-			otg->phy->state = OTG_STATE_B_IDLE;
-			msm_otg_reset(otg->phy);
+			otg->state = OTG_STATE_B_IDLE;
+			msm_otg_reset(otg->usb_phy);
 			schedule_work(w);
 		}
 		break;
 	case OTG_STATE_A_HOST:
-		dev_dbg(otg->phy->dev, "OTG_STATE_A_HOST state\n");
+		dev_dbg(otg->usb_phy->dev, "OTG_STATE_A_HOST state\n");
 		if (test_bit(ID, &motg->inputs)) {
-			msm_otg_start_host(otg->phy, 0);
-			otg->phy->state = OTG_STATE_B_IDLE;
-			msm_otg_reset(otg->phy);
+			msm_otg_start_host(otg->usb_phy, 0);
+			otg->state = OTG_STATE_B_IDLE;
+			msm_otg_reset(otg->usb_phy);
 			schedule_work(w);
 		}
 		break;
@@ -1303,7 +1306,7 @@ static int msm_otg_mode_show(struct seq_file *s, void *unused)
 	struct msm_otg *motg = s->private;
 	struct usb_otg *otg = motg->phy.otg;
 
-	switch (otg->phy->state) {
+	switch (otg->state) {
 	case OTG_STATE_A_HOST:
 		seq_puts(s, "host\n");
 		break;
@@ -1353,7 +1356,7 @@ static ssize_t msm_otg_mode_write(struct file *file, const char __user *ubuf,
 
 	switch (req_mode) {
 	case USB_DR_MODE_UNKNOWN:
-		switch (otg->phy->state) {
+		switch (otg->state) {
 		case OTG_STATE_A_HOST:
 		case OTG_STATE_B_PERIPHERAL:
 			set_bit(ID, &motg->inputs);
@@ -1364,7 +1367,7 @@ static ssize_t msm_otg_mode_write(struct file *file, const char __user *ubuf,
 		}
 		break;
 	case USB_DR_MODE_PERIPHERAL:
-		switch (otg->phy->state) {
+		switch (otg->state) {
 		case OTG_STATE_B_IDLE:
 		case OTG_STATE_A_HOST:
 			set_bit(ID, &motg->inputs);
@@ -1375,7 +1378,7 @@ static ssize_t msm_otg_mode_write(struct file *file, const char __user *ubuf,
 		}
 		break;
 	case USB_DR_MODE_HOST:
-		switch (otg->phy->state) {
+		switch (otg->state) {
 		case OTG_STATE_B_IDLE:
 		case OTG_STATE_B_PERIPHERAL:
 			clear_bit(ID, &motg->inputs);
@@ -1388,7 +1391,7 @@ static ssize_t msm_otg_mode_write(struct file *file, const char __user *ubuf,
 		goto out;
 	}
 
-	pm_runtime_get_sync(otg->phy->dev);
+	pm_runtime_get_sync(otg->usb_phy->dev);
 	schedule_work(&motg->sm_work);
 out:
 	return status;
@@ -1505,10 +1508,8 @@ static int msm_otg_read_dt(struct platform_device *pdev, struct msm_otg *motg)
 	}
 
 	pdata->phy_init_seq = devm_kzalloc(&pdev->dev, len, GFP_KERNEL);
-	if (!pdata->phy_init_seq) {
-		dev_warn(&pdev->dev, "No space for PHY init sequence\n");
+	if (!pdata->phy_init_seq)
 		return 0;
-	}
 
 	ret = of_property_read_u32_array(node, "qcom,phy-init-sequence",
 					 pdata->phy_init_seq, words);
@@ -1530,10 +1531,8 @@ static int msm_otg_probe(struct platform_device *pdev)
 	void __iomem *phy_select;
 
 	motg = devm_kzalloc(&pdev->dev, sizeof(struct msm_otg), GFP_KERNEL);
-	if (!motg) {
-		dev_err(&pdev->dev, "unable to allocate msm_otg\n");
+	if (!motg)
 		return -ENOMEM;
-	}
 
 	pdata = dev_get_platdata(&pdev->dev);
 	if (!pdata) {
@@ -1546,10 +1545,8 @@ static int msm_otg_probe(struct platform_device *pdev)
 
 	motg->phy.otg = devm_kzalloc(&pdev->dev, sizeof(struct usb_otg),
 				     GFP_KERNEL);
-	if (!motg->phy.otg) {
-		dev_err(&pdev->dev, "unable to allocate msm_otg\n");
+	if (!motg->phy.otg)
 		return -ENOMEM;
-	}
 
 	phy = &motg->phy;
 	phy->dev = &pdev->dev;
@@ -1674,7 +1671,7 @@ static int msm_otg_probe(struct platform_device *pdev)
 
 	phy->io_ops = &msm_otg_io_ops;
 
-	phy->otg->phy = &motg->phy;
+	phy->otg->usb_phy = &motg->phy;
 	phy->otg->set_host = msm_otg_set_host;
 	phy->otg->set_peripheral = msm_otg_set_peripheral;
 
@@ -1775,7 +1772,7 @@ static int msm_otg_runtime_idle(struct device *dev)
 	 * This 1 sec delay also prevents entering into LPM immediately
 	 * after asynchronous interrupt.
 	 */
-	if (otg->phy->state != OTG_STATE_UNDEFINED)
+	if (otg->state != OTG_STATE_UNDEFINED)
 		pm_schedule_suspend(dev, 1000);
 
 	return -EAGAIN;
diff --git a/drivers/usb/phy/phy-mv-usb.c b/drivers/usb/phy/phy-mv-usb.c
index 7d80c54f0ac6..699e38c73d82 100644
--- a/drivers/usb/phy/phy-mv-usb.c
+++ b/drivers/usb/phy/phy-mv-usb.c
@@ -56,7 +56,7 @@ static char *state_string[] = {
 
 static int mv_otg_set_vbus(struct usb_otg *otg, bool on)
 {
-	struct mv_otg *mvotg = container_of(otg->phy, struct mv_otg, phy);
+	struct mv_otg *mvotg = container_of(otg->usb_phy, struct mv_otg, phy);
 	if (mvotg->pdata->set_vbus == NULL)
 		return -ENODEV;
 
@@ -339,68 +339,68 @@ static void mv_otg_update_state(struct mv_otg *mvotg)
 {
 	struct mv_otg_ctrl *otg_ctrl = &mvotg->otg_ctrl;
 	struct usb_phy *phy = &mvotg->phy;
-	int old_state = phy->state;
+	int old_state = mvotg->phy.otg->state;
 
 	switch (old_state) {
 	case OTG_STATE_UNDEFINED:
-		phy->state = OTG_STATE_B_IDLE;
+		mvotg->phy.otg->state = OTG_STATE_B_IDLE;
 		/* FALL THROUGH */
 	case OTG_STATE_B_IDLE:
 		if (otg_ctrl->id == 0)
-			phy->state = OTG_STATE_A_IDLE;
+			mvotg->phy.otg->state = OTG_STATE_A_IDLE;
 		else if (otg_ctrl->b_sess_vld)
-			phy->state = OTG_STATE_B_PERIPHERAL;
+			mvotg->phy.otg->state = OTG_STATE_B_PERIPHERAL;
 		break;
 	case OTG_STATE_B_PERIPHERAL:
 		if (!otg_ctrl->b_sess_vld || otg_ctrl->id == 0)
-			phy->state = OTG_STATE_B_IDLE;
+			mvotg->phy.otg->state = OTG_STATE_B_IDLE;
 		break;
 	case OTG_STATE_A_IDLE:
 		if (otg_ctrl->id)
-			phy->state = OTG_STATE_B_IDLE;
+			mvotg->phy.otg->state = OTG_STATE_B_IDLE;
 		else if (!(otg_ctrl->a_bus_drop) &&
 			 (otg_ctrl->a_bus_req || otg_ctrl->a_srp_det))
-			phy->state = OTG_STATE_A_WAIT_VRISE;
+			mvotg->phy.otg->state = OTG_STATE_A_WAIT_VRISE;
 		break;
 	case OTG_STATE_A_WAIT_VRISE:
 		if (otg_ctrl->a_vbus_vld)
-			phy->state = OTG_STATE_A_WAIT_BCON;
+			mvotg->phy.otg->state = OTG_STATE_A_WAIT_BCON;
 		break;
 	case OTG_STATE_A_WAIT_BCON:
 		if (otg_ctrl->id || otg_ctrl->a_bus_drop
 		    || otg_ctrl->a_wait_bcon_timeout) {
 			mv_otg_cancel_timer(mvotg, A_WAIT_BCON_TIMER);
 			mvotg->otg_ctrl.a_wait_bcon_timeout = 0;
-			phy->state = OTG_STATE_A_WAIT_VFALL;
+			mvotg->phy.otg->state = OTG_STATE_A_WAIT_VFALL;
 			otg_ctrl->a_bus_req = 0;
 		} else if (!otg_ctrl->a_vbus_vld) {
 			mv_otg_cancel_timer(mvotg, A_WAIT_BCON_TIMER);
 			mvotg->otg_ctrl.a_wait_bcon_timeout = 0;
-			phy->state = OTG_STATE_A_VBUS_ERR;
+			mvotg->phy.otg->state = OTG_STATE_A_VBUS_ERR;
 		} else if (otg_ctrl->b_conn) {
 			mv_otg_cancel_timer(mvotg, A_WAIT_BCON_TIMER);
 			mvotg->otg_ctrl.a_wait_bcon_timeout = 0;
-			phy->state = OTG_STATE_A_HOST;
+			mvotg->phy.otg->state = OTG_STATE_A_HOST;
 		}
 		break;
 	case OTG_STATE_A_HOST:
 		if (otg_ctrl->id || !otg_ctrl->b_conn
 		    || otg_ctrl->a_bus_drop)
-			phy->state = OTG_STATE_A_WAIT_BCON;
+			mvotg->phy.otg->state = OTG_STATE_A_WAIT_BCON;
 		else if (!otg_ctrl->a_vbus_vld)
-			phy->state = OTG_STATE_A_VBUS_ERR;
+			mvotg->phy.otg->state = OTG_STATE_A_VBUS_ERR;
 		break;
 	case OTG_STATE_A_WAIT_VFALL:
 		if (otg_ctrl->id
 		    || (!otg_ctrl->b_conn && otg_ctrl->a_sess_vld)
 		    || otg_ctrl->a_bus_req)
-			phy->state = OTG_STATE_A_IDLE;
+			mvotg->phy.otg->state = OTG_STATE_A_IDLE;
 		break;
 	case OTG_STATE_A_VBUS_ERR:
 		if (otg_ctrl->id || otg_ctrl->a_clr_err
 		    || otg_ctrl->a_bus_drop) {
 			otg_ctrl->a_clr_err = 0;
-			phy->state = OTG_STATE_A_WAIT_VFALL;
+			mvotg->phy.otg->state = OTG_STATE_A_WAIT_VFALL;
 		}
 		break;
 	default:
@@ -420,8 +420,8 @@ static void mv_otg_work(struct work_struct *work)
 run:
 	/* work queue is single thread, or we need spin_lock to protect */
 	phy = &mvotg->phy;
-	otg = phy->otg;
-	old_state = phy->state;
+	otg = mvotg->phy.otg;
+	old_state = otg->state;
 
 	if (!mvotg->active)
 		return;
@@ -429,22 +429,24 @@ run:
 	mv_otg_update_inputs(mvotg);
 	mv_otg_update_state(mvotg);
 
-	if (old_state != phy->state) {
+	if (old_state != mvotg->phy.otg->state) {
 		dev_info(&mvotg->pdev->dev, "change from state %s to %s\n",
 			 state_string[old_state],
-			 state_string[phy->state]);
+			 state_string[mvotg->phy.otg->state]);
 
-		switch (phy->state) {
+		switch (mvotg->phy.otg->state) {
 		case OTG_STATE_B_IDLE:
 			otg->default_a = 0;
 			if (old_state == OTG_STATE_B_PERIPHERAL)
 				mv_otg_start_periphrals(mvotg, 0);
 			mv_otg_reset(mvotg);
 			mv_otg_disable(mvotg);
+			usb_phy_set_event(&mvotg->phy, USB_EVENT_NONE);
 			break;
 		case OTG_STATE_B_PERIPHERAL:
 			mv_otg_enable(mvotg);
 			mv_otg_start_periphrals(mvotg, 1);
+			usb_phy_set_event(&mvotg->phy, USB_EVENT_ENUMERATED);
 			break;
 		case OTG_STATE_A_IDLE:
 			otg->default_a = 1;
@@ -545,8 +547,8 @@ set_a_bus_req(struct device *dev, struct device_attribute *attr,
 		return -1;
 
 	/* We will use this interface to change to A device */
-	if (mvotg->phy.state != OTG_STATE_B_IDLE
-	    && mvotg->phy.state != OTG_STATE_A_IDLE)
+	if (mvotg->phy.otg->state != OTG_STATE_B_IDLE
+	    && mvotg->phy.otg->state != OTG_STATE_A_IDLE)
 		return -1;
 
 	/* The clock may disabled and we need to set irq for ID detected */
@@ -686,10 +688,8 @@ static int mv_otg_probe(struct platform_device *pdev)
 	}
 
 	mvotg = devm_kzalloc(&pdev->dev, sizeof(*mvotg), GFP_KERNEL);
-	if (!mvotg) {
-		dev_err(&pdev->dev, "failed to allocate memory!\n");
+	if (!mvotg)
 		return -ENOMEM;
-	}
 
 	otg = devm_kzalloc(&pdev->dev, sizeof(*otg), GFP_KERNEL);
 	if (!otg)
@@ -717,9 +717,9 @@ static int mv_otg_probe(struct platform_device *pdev)
 	mvotg->phy.dev = &pdev->dev;
 	mvotg->phy.otg = otg;
 	mvotg->phy.label = driver_name;
-	mvotg->phy.state = OTG_STATE_UNDEFINED;
 
-	otg->phy = &mvotg->phy;
+	otg->state = OTG_STATE_UNDEFINED;
+	otg->usb_phy = &mvotg->phy;
 	otg->set_host = mv_otg_set_host;
 	otg->set_peripheral = mv_otg_set_peripheral;
 	otg->set_vbus = mv_otg_set_vbus;
@@ -896,7 +896,6 @@ static struct platform_driver mv_otg_driver = {
 	.probe = mv_otg_probe,
 	.remove = mv_otg_remove,
 	.driver = {
-		   .owner = THIS_MODULE,
 		   .name = driver_name,
 		   },
 #ifdef CONFIG_PM
diff --git a/drivers/usb/phy/phy-mxs-usb.c b/drivers/usb/phy/phy-mxs-usb.c
index 0e0c41587a08..b9589f663683 100644
--- a/drivers/usb/phy/phy-mxs-usb.c
+++ b/drivers/usb/phy/phy-mxs-usb.c
@@ -390,10 +390,8 @@ static int mxs_phy_probe(struct platform_device *pdev)
 	}
 
 	mxs_phy = devm_kzalloc(&pdev->dev, sizeof(*mxs_phy), GFP_KERNEL);
-	if (!mxs_phy) {
-		dev_err(&pdev->dev, "Failed to allocate USB PHY structure!\n");
+	if (!mxs_phy)
 		return -ENOMEM;
-	}
 
 	/* Some SoCs don't have anatop registers */
 	if (of_get_property(np, "fsl,anatop", NULL)) {
@@ -491,7 +489,6 @@ static struct platform_driver mxs_phy_driver = {
 	.remove = mxs_phy_remove,
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = mxs_phy_dt_ids,
 		.pm = &mxs_phy_pm,
 	 },
diff --git a/drivers/usb/phy/phy-omap-otg.c b/drivers/usb/phy/phy-omap-otg.c
index 11598cdb3189..56ee7603034b 100644
--- a/drivers/usb/phy/phy-omap-otg.c
+++ b/drivers/usb/phy/phy-omap-otg.c
@@ -158,7 +158,6 @@ static struct platform_driver omap_otg_driver = {
 	.probe		= omap_otg_probe,
 	.remove		= omap_otg_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "omap_otg",
 	},
 };
diff --git a/drivers/usb/phy/phy-rcar-gen2-usb.c b/drivers/usb/phy/phy-rcar-gen2-usb.c
index 388d89f6b141..f83808413ba2 100644
--- a/drivers/usb/phy/phy-rcar-gen2-usb.c
+++ b/drivers/usb/phy/phy-rcar-gen2-usb.c
@@ -195,10 +195,8 @@ static int rcar_gen2_usb_phy_probe(struct platform_device *pdev)
 		return PTR_ERR(base);
 
 	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
-	if (!priv) {
-		dev_err(dev, "Memory allocation failed\n");
+	if (!priv)
 		return -ENOMEM;
-	}
 
 	spin_lock_init(&priv->lock);
 	priv->clk = clk;
diff --git a/drivers/usb/phy/phy-rcar-usb.c b/drivers/usb/phy/phy-rcar-usb.c
index 33265a5b2cdf..1e09b8377885 100644
--- a/drivers/usb/phy/phy-rcar-usb.c
+++ b/drivers/usb/phy/phy-rcar-usb.c
@@ -195,17 +195,13 @@ static int rcar_usb_phy_probe(struct platform_device *pdev)
 		return PTR_ERR(reg0);
 
 	res1 = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	if (res1) {
-		reg1 = devm_ioremap_resource(dev, res1);
-		if (IS_ERR(reg1))
-			return PTR_ERR(reg1);
-	}
+	reg1 = devm_ioremap_resource(dev, res1);
+	if (IS_ERR(reg1))
+		return PTR_ERR(reg1);
 
 	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
-	if (!priv) {
-		dev_err(dev, "priv data allocation error\n");
+	if (!priv)
 		return -ENOMEM;
-	}
 
 	priv->reg0		= reg0;
 	priv->reg1		= reg1;
diff --git a/drivers/usb/phy/phy-tahvo.c b/drivers/usb/phy/phy-tahvo.c
index cc61ee44b911..845f658276b1 100644
--- a/drivers/usb/phy/phy-tahvo.c
+++ b/drivers/usb/phy/phy-tahvo.c
@@ -81,33 +81,35 @@ static void check_vbus_state(struct tahvo_usb *tu)
 
 	reg = retu_read(rdev, TAHVO_REG_IDSR);
 	if (reg & TAHVO_STAT_VBUS) {
-		switch (tu->phy.state) {
+		switch (tu->phy.otg->state) {
 		case OTG_STATE_B_IDLE:
 			/* Enable the gadget driver */
 			if (tu->phy.otg->gadget)
 				usb_gadget_vbus_connect(tu->phy.otg->gadget);
-			tu->phy.state = OTG_STATE_B_PERIPHERAL;
+			tu->phy.otg->state = OTG_STATE_B_PERIPHERAL;
+			usb_phy_set_event(&tu->phy, USB_EVENT_ENUMERATED);
 			break;
 		case OTG_STATE_A_IDLE:
 			/*
 			 * Session is now valid assuming the USB hub is driving
 			 * Vbus.
 			 */
-			tu->phy.state = OTG_STATE_A_HOST;
+			tu->phy.otg->state = OTG_STATE_A_HOST;
 			break;
 		default:
 			break;
 		}
 		dev_info(&tu->pt_dev->dev, "USB cable connected\n");
 	} else {
-		switch (tu->phy.state) {
+		switch (tu->phy.otg->state) {
 		case OTG_STATE_B_PERIPHERAL:
 			if (tu->phy.otg->gadget)
 				usb_gadget_vbus_disconnect(tu->phy.otg->gadget);
-			tu->phy.state = OTG_STATE_B_IDLE;
+			tu->phy.otg->state = OTG_STATE_B_IDLE;
+			usb_phy_set_event(&tu->phy, USB_EVENT_NONE);
 			break;
 		case OTG_STATE_A_HOST:
-			tu->phy.state = OTG_STATE_A_IDLE;
+			tu->phy.otg->state = OTG_STATE_A_IDLE;
 			break;
 		default:
 			break;
@@ -132,14 +134,14 @@ static void tahvo_usb_become_host(struct tahvo_usb *tu)
 	/* Power up the transceiver in USB host mode */
 	retu_write(rdev, TAHVO_REG_USBR, USBR_REGOUT | USBR_NSUSPEND |
 		   USBR_MASTER_SW2 | USBR_MASTER_SW1);
-	tu->phy.state = OTG_STATE_A_IDLE;
+	tu->phy.otg->state = OTG_STATE_A_IDLE;
 
 	check_vbus_state(tu);
 }
 
 static void tahvo_usb_stop_host(struct tahvo_usb *tu)
 {
-	tu->phy.state = OTG_STATE_A_IDLE;
+	tu->phy.otg->state = OTG_STATE_A_IDLE;
 }
 
 static void tahvo_usb_become_peripheral(struct tahvo_usb *tu)
@@ -151,7 +153,7 @@ static void tahvo_usb_become_peripheral(struct tahvo_usb *tu)
 	/* Power up transceiver and set it in USB peripheral mode */
 	retu_write(rdev, TAHVO_REG_USBR, USBR_SLAVE_CONTROL | USBR_REGOUT |
 		   USBR_NSUSPEND | USBR_SLAVE_SW);
-	tu->phy.state = OTG_STATE_B_IDLE;
+	tu->phy.otg->state = OTG_STATE_B_IDLE;
 
 	check_vbus_state(tu);
 }
@@ -160,7 +162,7 @@ static void tahvo_usb_stop_peripheral(struct tahvo_usb *tu)
 {
 	if (tu->phy.otg->gadget)
 		usb_gadget_vbus_disconnect(tu->phy.otg->gadget);
-	tu->phy.state = OTG_STATE_B_IDLE;
+	tu->phy.otg->state = OTG_STATE_B_IDLE;
 }
 
 static void tahvo_usb_power_off(struct tahvo_usb *tu)
@@ -173,7 +175,7 @@ static void tahvo_usb_power_off(struct tahvo_usb *tu)
 
 	/* Power off transceiver */
 	retu_write(rdev, TAHVO_REG_USBR, 0);
-	tu->phy.state = OTG_STATE_UNDEFINED;
+	tu->phy.otg->state = OTG_STATE_UNDEFINED;
 }
 
 static int tahvo_usb_set_suspend(struct usb_phy *dev, int suspend)
@@ -196,7 +198,8 @@ static int tahvo_usb_set_suspend(struct usb_phy *dev, int suspend)
 
 static int tahvo_usb_set_host(struct usb_otg *otg, struct usb_bus *host)
 {
-	struct tahvo_usb *tu = container_of(otg->phy, struct tahvo_usb, phy);
+	struct tahvo_usb *tu = container_of(otg->usb_phy, struct tahvo_usb,
+					    phy);
 
 	dev_dbg(&tu->pt_dev->dev, "%s %p\n", __func__, host);
 
@@ -225,7 +228,8 @@ static int tahvo_usb_set_host(struct usb_otg *otg, struct usb_bus *host)
 static int tahvo_usb_set_peripheral(struct usb_otg *otg,
 				    struct usb_gadget *gadget)
 {
-	struct tahvo_usb *tu = container_of(otg->phy, struct tahvo_usb, phy);
+	struct tahvo_usb *tu = container_of(otg->usb_phy, struct tahvo_usb,
+					    phy);
 
 	dev_dbg(&tu->pt_dev->dev, "%s %p\n", __func__, gadget);
 
@@ -379,11 +383,11 @@ static int tahvo_usb_probe(struct platform_device *pdev)
 	/* Create OTG interface */
 	tahvo_usb_power_off(tu);
 	tu->phy.dev = &pdev->dev;
-	tu->phy.state = OTG_STATE_UNDEFINED;
+	tu->phy.otg->state = OTG_STATE_UNDEFINED;
 	tu->phy.label = DRIVER_NAME;
 	tu->phy.set_suspend = tahvo_usb_set_suspend;
 
-	tu->phy.otg->phy = &tu->phy;
+	tu->phy.otg->usb_phy = &tu->phy;
 	tu->phy.otg->set_host = tahvo_usb_set_host;
 	tu->phy.otg->set_peripheral = tahvo_usb_set_peripheral;
 
@@ -446,7 +450,6 @@ static struct platform_driver tahvo_usb_driver = {
 	.remove		= tahvo_usb_remove,
 	.driver		= {
 		.name	= "tahvo-usb",
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(tahvo_usb_driver);
diff --git a/drivers/usb/phy/phy-tegra-usb.c b/drivers/usb/phy/phy-tegra-usb.c
index 886f1807a67b..ab025b00964c 100644
--- a/drivers/usb/phy/phy-tegra-usb.c
+++ b/drivers/usb/phy/phy-tegra-usb.c
@@ -880,11 +880,8 @@ static int utmi_phy_probe(struct tegra_usb_phy *tegra_phy,
 
 	tegra_phy->config = devm_kzalloc(&pdev->dev, sizeof(*config),
 					 GFP_KERNEL);
-	if (!tegra_phy->config) {
-		dev_err(&pdev->dev,
-			"unable to allocate memory for USB UTMIP config\n");
+	if (!tegra_phy->config)
 		return -ENOMEM;
-	}
 
 	config = tegra_phy->config;
 
@@ -979,10 +976,8 @@ static int tegra_usb_phy_probe(struct platform_device *pdev)
 	int err;
 
 	tegra_phy = devm_kzalloc(&pdev->dev, sizeof(*tegra_phy), GFP_KERNEL);
-	if (!tegra_phy) {
-		dev_err(&pdev->dev, "unable to allocate memory for USB2 PHY\n");
+	if (!tegra_phy)
 		return -ENOMEM;
-	}
 
 	match = of_match_device(tegra_usb_phy_id_table, &pdev->dev);
 	if (!match) {
@@ -1086,7 +1081,6 @@ static struct platform_driver tegra_usb_phy_driver = {
 	.remove		= tegra_usb_phy_remove,
 	.driver		= {
 		.name	= "tegra-phy",
-		.owner	= THIS_MODULE,
 		.of_match_table = tegra_usb_phy_id_table,
 	},
 };
diff --git a/drivers/usb/phy/phy-twl6030-usb.c b/drivers/usb/phy/phy-twl6030-usb.c
index 44ea082e40dc..12741856a75c 100644
--- a/drivers/usb/phy/phy-twl6030-usb.c
+++ b/drivers/usb/phy/phy-twl6030-usb.c
@@ -430,7 +430,6 @@ static struct platform_driver twl6030_usb_driver = {
 	.remove		= twl6030_usb_remove,
 	.driver		= {
 		.name	= "twl6030_usb",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(twl6030_usb_id_table),
 	},
 };
diff --git a/drivers/usb/phy/phy-ulpi.c b/drivers/usb/phy/phy-ulpi.c
index 4e3877c329f2..f48a7a21e3c2 100644
--- a/drivers/usb/phy/phy-ulpi.c
+++ b/drivers/usb/phy/phy-ulpi.c
@@ -211,7 +211,7 @@ static int ulpi_init(struct usb_phy *phy)
 
 static int ulpi_set_host(struct usb_otg *otg, struct usb_bus *host)
 {
-	struct usb_phy *phy = otg->phy;
+	struct usb_phy *phy = otg->usb_phy;
 	unsigned int flags = usb_phy_io_read(phy, ULPI_IFC_CTRL);
 
 	if (!host) {
@@ -237,7 +237,7 @@ static int ulpi_set_host(struct usb_otg *otg, struct usb_bus *host)
 
 static int ulpi_set_vbus(struct usb_otg *otg, bool on)
 {
-	struct usb_phy *phy = otg->phy;
+	struct usb_phy *phy = otg->usb_phy;
 	unsigned int flags = usb_phy_io_read(phy, ULPI_OTG_CTRL);
 
 	flags &= ~(ULPI_OTG_CTRL_DRVVBUS | ULPI_OTG_CTRL_DRVVBUS_EXT);
@@ -276,7 +276,7 @@ otg_ulpi_create(struct usb_phy_io_ops *ops,
 	phy->otg	= otg;
 	phy->init	= ulpi_init;
 
-	otg->phy	= phy;
+	otg->usb_phy	= phy;
 	otg->set_host	= ulpi_set_host;
 	otg->set_vbus	= ulpi_set_vbus;
 
diff --git a/drivers/usb/phy/phy.c b/drivers/usb/phy/phy.c
index 045cd309367a..b4066a001ba0 100644
--- a/drivers/usb/phy/phy.c
+++ b/drivers/usb/phy/phy.c
@@ -191,7 +191,9 @@ struct usb_phy *devm_usb_get_phy_by_phandle(struct device *dev,
 
 	phy = __of_usb_find_phy(node);
 	if (IS_ERR(phy) || !try_module_get(phy->dev->driver->owner)) {
-		phy = ERR_PTR(-EPROBE_DEFER);
+		if (!IS_ERR(phy))
+			phy = ERR_PTR(-EPROBE_DEFER);
+
 		devres_free(ptr);
 		goto err1;
 	}
@@ -444,3 +446,15 @@ int usb_bind_phy(const char *dev_name, u8 index,
 	return 0;
 }
 EXPORT_SYMBOL_GPL(usb_bind_phy);
+
+/**
+ * usb_phy_set_event - set event to phy event
+ * @x: the phy returned by usb_get_phy();
+ *
+ * This sets event to phy event
+ */
+void usb_phy_set_event(struct usb_phy *x, unsigned long event)
+{
+	x->last_event = event;
+}
+EXPORT_SYMBOL_GPL(usb_phy_set_event);
diff --git a/drivers/usb/renesas_usbhs/common.c b/drivers/usb/renesas_usbhs/common.c
index b3b6813ab270..371478704899 100644
--- a/drivers/usb/renesas_usbhs/common.c
+++ b/drivers/usb/renesas_usbhs/common.c
@@ -126,13 +126,15 @@ void usbhs_sys_host_ctrl(struct usbhs_priv *priv, int enable)
 void usbhs_sys_function_ctrl(struct usbhs_priv *priv, int enable)
 {
 	u16 mask = DCFM | DRPD | DPRPU | HSE | USBE;
-	u16 val  = DPRPU | HSE | USBE;
+	u16 val  = HSE | USBE;
 
 	/*
 	 * if enable
 	 *
 	 * - select Function mode
-	 * - D+ Line Pull-up
+	 * - D+ Line Pull-up is disabled
+	 *      When D+ Line Pull-up is enabled,
+	 *      calling usbhs_sys_function_pullup(,1)
 	 */
 	usbhs_bset(priv, SYSCFG, mask, enable ? val : 0);
 }
@@ -496,20 +498,18 @@ static int usbhs_probe(struct platform_device *pdev)
 	}
 
 	/* platform data */
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	irq_res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (!res || !irq_res) {
+	if (!irq_res) {
 		dev_err(&pdev->dev, "Not enough Renesas USB platform resources.\n");
 		return -ENODEV;
 	}
 
 	/* usb private data */
 	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
-	if (!priv) {
-		dev_err(&pdev->dev, "Could not allocate priv\n");
+	if (!priv)
 		return -ENOMEM;
-	}
 
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	priv->base = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(priv->base))
 		return PTR_ERR(priv->base);
diff --git a/drivers/usb/renesas_usbhs/common.h b/drivers/usb/renesas_usbhs/common.h
index a7996da6a1bd..0427cdd1a483 100644
--- a/drivers/usb/renesas_usbhs/common.h
+++ b/drivers/usb/renesas_usbhs/common.h
@@ -102,6 +102,10 @@ struct usbhs_priv;
 #define DEVADD8		0x00E0
 #define DEVADD9		0x00E2
 #define DEVADDA		0x00E4
+#define D2FIFOSEL	0x00F0	/* for R-Car Gen2 */
+#define D2FIFOCTR	0x00F2	/* for R-Car Gen2 */
+#define D3FIFOSEL	0x00F4	/* for R-Car Gen2 */
+#define D3FIFOCTR	0x00F6	/* for R-Car Gen2 */
 
 /* SYSCFG */
 #define SCKE	(1 << 10)	/* USB Module Clock Enable */
@@ -269,7 +273,8 @@ struct usbhs_priv {
 	 */
 	struct usbhs_fifo_info fifo_info;
 
-	struct usb_phy *phy;
+	struct usb_phy *usb_phy;
+	struct phy *phy;
 };
 
 /*
diff --git a/drivers/usb/renesas_usbhs/fifo.c b/drivers/usb/renesas_usbhs/fifo.c
index b0c97a3f1bfe..f46271ce1b15 100644
--- a/drivers/usb/renesas_usbhs/fifo.c
+++ b/drivers/usb/renesas_usbhs/fifo.c
@@ -21,8 +21,6 @@
 #include "pipe.h"
 
 #define usbhsf_get_cfifo(p)	(&((p)->fifo_info.cfifo))
-#define usbhsf_get_d0fifo(p)	(&((p)->fifo_info.d0fifo))
-#define usbhsf_get_d1fifo(p)	(&((p)->fifo_info.d1fifo))
 #define usbhsf_is_cfifo(p, f)	(usbhsf_get_cfifo(p) == f)
 
 #define usbhsf_fifo_is_busy(f)	((f)->pipe) /* see usbhs_pipe_select_fifo */
@@ -577,14 +575,6 @@ static int usbhsf_pio_try_push(struct usbhs_pkt *pkt, int *is_done)
 		usbhs_pipe_number(pipe),
 		pkt->length, pkt->actual, *is_done, pkt->zero);
 
-	/*
-	 * Transmission end
-	 */
-	if (*is_done) {
-		if (usbhs_pipe_is_dcp(pipe))
-			usbhs_dcp_control_transfer_done(pipe);
-	}
-
 	usbhsf_fifo_unselect(pipe, fifo);
 
 	return 0;
@@ -722,14 +712,6 @@ usbhs_fifo_read_end:
 		usbhs_pipe_number(pipe),
 		pkt->length, pkt->actual, *is_done, pkt->zero);
 
-	/*
-	 * Transmission end
-	 */
-	if (*is_done) {
-		if (usbhs_pipe_is_dcp(pipe))
-			usbhs_dcp_control_transfer_done(pipe);
-	}
-
 usbhs_fifo_read_busy:
 	usbhsf_fifo_unselect(pipe, fifo);
 
@@ -777,18 +759,13 @@ static struct usbhs_fifo *usbhsf_get_dma_fifo(struct usbhs_priv *priv,
 					      struct usbhs_pkt *pkt)
 {
 	struct usbhs_fifo *fifo;
+	int i;
 
-	/* DMA :: D0FIFO */
-	fifo = usbhsf_get_d0fifo(priv);
-	if (usbhsf_dma_chan_get(fifo, pkt) &&
-	    !usbhsf_fifo_is_busy(fifo))
-		return fifo;
-
-	/* DMA :: D1FIFO */
-	fifo = usbhsf_get_d1fifo(priv);
-	if (usbhsf_dma_chan_get(fifo, pkt) &&
-	    !usbhsf_fifo_is_busy(fifo))
-		return fifo;
+	usbhs_for_each_dfifo(priv, fifo, i) {
+		if (usbhsf_dma_chan_get(fifo, pkt) &&
+		    !usbhsf_fifo_is_busy(fifo))
+			return fifo;
+	}
 
 	return NULL;
 }
@@ -1176,6 +1153,24 @@ static void usbhsf_dma_complete(void *arg)
 			usbhs_pipe_number(pipe), ret);
 }
 
+void usbhs_fifo_clear_dcp(struct usbhs_pipe *pipe)
+{
+	struct usbhs_priv *priv = usbhs_pipe_to_priv(pipe);
+	struct usbhs_fifo *fifo = usbhsf_get_cfifo(priv); /* CFIFO */
+
+	/* clear DCP FIFO of transmission */
+	if (usbhsf_fifo_select(pipe, fifo, 1) < 0)
+		return;
+	usbhsf_fifo_clear(pipe, fifo);
+	usbhsf_fifo_unselect(pipe, fifo);
+
+	/* clear DCP FIFO of reception */
+	if (usbhsf_fifo_select(pipe, fifo, 0) < 0)
+		return;
+	usbhsf_fifo_clear(pipe, fifo);
+	usbhsf_fifo_unselect(pipe, fifo);
+}
+
 /*
  *		fifo init
  */
@@ -1183,8 +1178,8 @@ void usbhs_fifo_init(struct usbhs_priv *priv)
 {
 	struct usbhs_mod *mod = usbhs_mod_get_current(priv);
 	struct usbhs_fifo *cfifo = usbhsf_get_cfifo(priv);
-	struct usbhs_fifo *d0fifo = usbhsf_get_d0fifo(priv);
-	struct usbhs_fifo *d1fifo = usbhsf_get_d1fifo(priv);
+	struct usbhs_fifo *dfifo;
+	int i;
 
 	mod->irq_empty		= usbhsf_irq_empty;
 	mod->irq_ready		= usbhsf_irq_ready;
@@ -1192,8 +1187,8 @@ void usbhs_fifo_init(struct usbhs_priv *priv)
 	mod->irq_brdysts	= 0;
 
 	cfifo->pipe	= NULL;
-	d0fifo->pipe	= NULL;
-	d1fifo->pipe	= NULL;
+	usbhs_for_each_dfifo(priv, dfifo, i)
+		dfifo->pipe	= NULL;
 }
 
 void usbhs_fifo_quit(struct usbhs_priv *priv)
@@ -1206,6 +1201,25 @@ void usbhs_fifo_quit(struct usbhs_priv *priv)
 	mod->irq_brdysts	= 0;
 }
 
+#define __USBHS_DFIFO_INIT(priv, fifo, channel, fifo_port)		\
+do {									\
+	fifo = usbhsf_get_dnfifo(priv, channel);			\
+	fifo->name	= "D"#channel"FIFO";				\
+	fifo->port	= fifo_port;					\
+	fifo->sel	= D##channel##FIFOSEL;				\
+	fifo->ctr	= D##channel##FIFOCTR;				\
+	fifo->tx_slave.shdma_slave.slave_id =				\
+			usbhs_get_dparam(priv, d##channel##_tx_id);	\
+	fifo->rx_slave.shdma_slave.slave_id =				\
+			usbhs_get_dparam(priv, d##channel##_rx_id);	\
+	usbhsf_dma_init(priv, fifo);					\
+} while (0)
+
+#define USBHS_DFIFO_INIT(priv, fifo, channel)				\
+		__USBHS_DFIFO_INIT(priv, fifo, channel, D##channel##FIFO)
+#define USBHS_DFIFO_INIT_NO_PORT(priv, fifo, channel)			\
+		__USBHS_DFIFO_INIT(priv, fifo, channel, 0)
+
 int usbhs_fifo_probe(struct usbhs_priv *priv)
 {
 	struct usbhs_fifo *fifo;
@@ -1217,31 +1231,20 @@ int usbhs_fifo_probe(struct usbhs_priv *priv)
 	fifo->sel	= CFIFOSEL;
 	fifo->ctr	= CFIFOCTR;
 
-	/* D0FIFO */
-	fifo = usbhsf_get_d0fifo(priv);
-	fifo->name	= "D0FIFO";
-	fifo->port	= D0FIFO;
-	fifo->sel	= D0FIFOSEL;
-	fifo->ctr	= D0FIFOCTR;
-	fifo->tx_slave.shdma_slave.slave_id	= usbhs_get_dparam(priv, d0_tx_id);
-	fifo->rx_slave.shdma_slave.slave_id	= usbhs_get_dparam(priv, d0_rx_id);
-	usbhsf_dma_init(priv, fifo);
-
-	/* D1FIFO */
-	fifo = usbhsf_get_d1fifo(priv);
-	fifo->name	= "D1FIFO";
-	fifo->port	= D1FIFO;
-	fifo->sel	= D1FIFOSEL;
-	fifo->ctr	= D1FIFOCTR;
-	fifo->tx_slave.shdma_slave.slave_id	= usbhs_get_dparam(priv, d1_tx_id);
-	fifo->rx_slave.shdma_slave.slave_id	= usbhs_get_dparam(priv, d1_rx_id);
-	usbhsf_dma_init(priv, fifo);
+	/* DFIFO */
+	USBHS_DFIFO_INIT(priv, fifo, 0);
+	USBHS_DFIFO_INIT(priv, fifo, 1);
+	USBHS_DFIFO_INIT_NO_PORT(priv, fifo, 2);
+	USBHS_DFIFO_INIT_NO_PORT(priv, fifo, 3);
 
 	return 0;
 }
 
 void usbhs_fifo_remove(struct usbhs_priv *priv)
 {
-	usbhsf_dma_quit(priv, usbhsf_get_d0fifo(priv));
-	usbhsf_dma_quit(priv, usbhsf_get_d1fifo(priv));
+	struct usbhs_fifo *fifo;
+	int i;
+
+	usbhs_for_each_dfifo(priv, fifo, i)
+		usbhsf_dma_quit(priv, fifo);
 }
diff --git a/drivers/usb/renesas_usbhs/fifo.h b/drivers/usb/renesas_usbhs/fifo.h
index a168a1760fce..f07037c1185f 100644
--- a/drivers/usb/renesas_usbhs/fifo.h
+++ b/drivers/usb/renesas_usbhs/fifo.h
@@ -38,11 +38,16 @@ struct usbhs_fifo {
 	struct sh_dmae_slave	rx_slave;
 };
 
+#define USBHS_MAX_NUM_DFIFO	4
 struct usbhs_fifo_info {
 	struct usbhs_fifo cfifo;
-	struct usbhs_fifo d0fifo;
-	struct usbhs_fifo d1fifo;
+	struct usbhs_fifo dfifo[USBHS_MAX_NUM_DFIFO];
 };
+#define usbhsf_get_dnfifo(p, n)	(&((p)->fifo_info.dfifo[n]))
+#define usbhs_for_each_dfifo(priv, dfifo, i)				\
+	for ((i) = 0, dfifo = usbhsf_get_dnfifo(priv, (i));		\
+	     ((i) < USBHS_MAX_NUM_DFIFO);				\
+	     (i)++, dfifo = usbhsf_get_dnfifo(priv, (i)))
 
 struct usbhs_pkt_handle;
 struct usbhs_pkt {
@@ -74,6 +79,7 @@ int usbhs_fifo_probe(struct usbhs_priv *priv);
 void usbhs_fifo_remove(struct usbhs_priv *priv);
 void usbhs_fifo_init(struct usbhs_priv *priv);
 void usbhs_fifo_quit(struct usbhs_priv *priv);
+void usbhs_fifo_clear_dcp(struct usbhs_pipe *pipe);
 
 /*
  * packet info
diff --git a/drivers/usb/renesas_usbhs/mod_gadget.c b/drivers/usb/renesas_usbhs/mod_gadget.c
index 2d17c10a0428..8697e6efcabf 100644
--- a/drivers/usb/renesas_usbhs/mod_gadget.c
+++ b/drivers/usb/renesas_usbhs/mod_gadget.c
@@ -56,6 +56,7 @@ struct usbhsg_gpriv {
 #define USBHSG_STATUS_REGISTERD		(1 << 1)
 #define USBHSG_STATUS_WEDGE		(1 << 2)
 #define USBHSG_STATUS_SELF_POWERED	(1 << 3)
+#define USBHSG_STATUS_SOFT_CONNECT	(1 << 4)
 };
 
 struct usbhsg_recip_handle {
@@ -484,6 +485,9 @@ static int usbhsg_irq_ctrl_stage(struct usbhs_priv *priv,
 	case NODATA_STATUS_STAGE:
 		pipe->handler = &usbhs_ctrl_stage_end_handler;
 		break;
+	case READ_STATUS_STAGE:
+	case WRITE_STATUS_STAGE:
+		usbhs_dcp_control_transfer_done(pipe);
 	default:
 		return ret;
 	}
@@ -602,6 +606,9 @@ static int usbhsg_ep_disable(struct usb_ep *ep)
 	struct usbhsg_uep *uep = usbhsg_ep_to_uep(ep);
 	struct usbhs_pipe *pipe = usbhsg_uep_to_pipe(uep);
 
+	if (!pipe)
+		return -EINVAL;
+
 	usbhsg_pipe_disable(uep);
 	usbhs_pipe_free(pipe);
 
@@ -723,6 +730,25 @@ static struct usb_ep_ops usbhsg_ep_ops = {
 };
 
 /*
+ *		pullup control
+ */
+static int usbhsg_can_pullup(struct usbhs_priv *priv)
+{
+	struct usbhsg_gpriv *gpriv = usbhsg_priv_to_gpriv(priv);
+
+	return gpriv->driver &&
+	       usbhsg_status_has(gpriv, USBHSG_STATUS_SOFT_CONNECT);
+}
+
+static void usbhsg_update_pullup(struct usbhs_priv *priv)
+{
+	if (usbhsg_can_pullup(priv))
+		usbhs_sys_function_pullup(priv, 1);
+	else
+		usbhs_sys_function_pullup(priv, 0);
+}
+
+/*
  *		usb module start/end
  */
 static int usbhsg_try_start(struct usbhs_priv *priv, u32 status)
@@ -756,9 +782,9 @@ static int usbhsg_try_start(struct usbhs_priv *priv, u32 status)
 	/*
 	 * pipe initialize and enable DCP
 	 */
+	usbhs_fifo_init(priv);
 	usbhs_pipe_init(priv,
 			usbhsg_dma_map_ctrl);
-	usbhs_fifo_init(priv);
 
 	/* dcp init instead of usbhsg_ep_enable() */
 	dcp->pipe		= usbhs_dcp_malloc(priv);
@@ -772,6 +798,7 @@ static int usbhsg_try_start(struct usbhs_priv *priv, u32 status)
 	 * - usb module
 	 */
 	usbhs_sys_function_ctrl(priv, 1);
+	usbhsg_update_pullup(priv);
 
 	/*
 	 * enable irq callback
@@ -851,8 +878,7 @@ static int usbhsg_gadget_start(struct usb_gadget *gadget,
 	return usbhsg_try_start(priv, USBHSG_STATUS_REGISTERD);
 }
 
-static int usbhsg_gadget_stop(struct usb_gadget *gadget,
-		struct usb_gadget_driver *driver)
+static int usbhsg_gadget_stop(struct usb_gadget *gadget)
 {
 	struct usbhsg_gpriv *gpriv = usbhsg_gadget_to_gpriv(gadget);
 	struct usbhs_priv *priv = usbhsg_gpriv_to_priv(gpriv);
@@ -878,8 +904,15 @@ static int usbhsg_pullup(struct usb_gadget *gadget, int is_on)
 {
 	struct usbhsg_gpriv *gpriv = usbhsg_gadget_to_gpriv(gadget);
 	struct usbhs_priv *priv = usbhsg_gpriv_to_priv(gpriv);
+	unsigned long flags;
 
-	usbhs_sys_function_pullup(priv, is_on);
+	usbhs_lock(priv, flags);
+	if (is_on)
+		usbhsg_status_set(gpriv, USBHSG_STATUS_SOFT_CONNECT);
+	else
+		usbhsg_status_clr(gpriv, USBHSG_STATUS_SOFT_CONNECT);
+	usbhsg_update_pullup(priv);
+	usbhs_unlock(priv, flags);
 
 	return 0;
 }
diff --git a/drivers/usb/renesas_usbhs/mod_host.c b/drivers/usb/renesas_usbhs/mod_host.c
index 10e1ded9c9cc..f0d323125871 100644
--- a/drivers/usb/renesas_usbhs/mod_host.c
+++ b/drivers/usb/renesas_usbhs/mod_host.c
@@ -1474,9 +1474,9 @@ static int usbhsh_start(struct usbhs_priv *priv)
 	/*
 	 * pipe initialize and enable DCP
 	 */
+	usbhs_fifo_init(priv);
 	usbhs_pipe_init(priv,
 			usbhsh_dma_map_ctrl);
-	usbhs_fifo_init(priv);
 	usbhsh_pipe_init_for_host(priv);
 
 	/*
diff --git a/drivers/usb/renesas_usbhs/pipe.c b/drivers/usb/renesas_usbhs/pipe.c
index 040bcefcb040..007f45abe96c 100644
--- a/drivers/usb/renesas_usbhs/pipe.c
+++ b/drivers/usb/renesas_usbhs/pipe.c
@@ -618,8 +618,12 @@ void usbhs_pipe_data_sequence(struct usbhs_pipe *pipe, int sequence)
 
 void usbhs_pipe_clear(struct usbhs_pipe *pipe)
 {
-	usbhsp_pipectrl_set(pipe, ACLRM, ACLRM);
-	usbhsp_pipectrl_set(pipe, ACLRM, 0);
+	if (usbhs_pipe_is_dcp(pipe)) {
+		usbhs_fifo_clear_dcp(pipe);
+	} else {
+		usbhsp_pipectrl_set(pipe, ACLRM, ACLRM);
+		usbhsp_pipectrl_set(pipe, ACLRM, 0);
+	}
 }
 
 static struct usbhs_pipe *usbhsp_get_pipe(struct usbhs_priv *priv, u32 type)
diff --git a/drivers/usb/renesas_usbhs/rcar2.c b/drivers/usb/renesas_usbhs/rcar2.c
index e6b9dcc1c289..8fc15c0ba339 100644
--- a/drivers/usb/renesas_usbhs/rcar2.c
+++ b/drivers/usb/renesas_usbhs/rcar2.c
@@ -12,6 +12,7 @@
 
 #include <linux/gpio.h>
 #include <linux/of_gpio.h>
+#include <linux/phy/phy.h>
 #include <linux/platform_data/gpio-rcar.h>
 #include <linux/usb/phy.h>
 #include "common.h"
@@ -20,25 +21,43 @@
 static int usbhs_rcar2_hardware_init(struct platform_device *pdev)
 {
 	struct usbhs_priv *priv = usbhs_pdev_to_priv(pdev);
-	struct usb_phy *phy;
 
-	phy = usb_get_phy_dev(&pdev->dev, 0);
-	if (IS_ERR(phy))
-		return PTR_ERR(phy);
+	if (IS_ENABLED(CONFIG_GENERIC_PHY)) {
+		struct phy *phy = phy_get(&pdev->dev, "usb");
 
-	priv->phy = phy;
-	return 0;
+		if (IS_ERR(phy))
+			return PTR_ERR(phy);
+
+		priv->phy = phy;
+		return 0;
+	}
+
+	if (IS_ENABLED(CONFIG_USB_PHY)) {
+		struct usb_phy *usb_phy = usb_get_phy_dev(&pdev->dev, 0);
+
+		if (IS_ERR(usb_phy))
+			return PTR_ERR(usb_phy);
+
+		priv->usb_phy = usb_phy;
+		return 0;
+	}
+
+	return -ENXIO;
 }
 
 static int usbhs_rcar2_hardware_exit(struct platform_device *pdev)
 {
 	struct usbhs_priv *priv = usbhs_pdev_to_priv(pdev);
 
-	if (!priv->phy)
-		return 0;
+	if (priv->phy) {
+		phy_put(priv->phy);
+		priv->phy = NULL;
+	}
 
-	usb_put_phy(priv->phy);
-	priv->phy = NULL;
+	if (priv->usb_phy) {
+		usb_put_phy(priv->usb_phy);
+		priv->usb_phy = NULL;
+	}
 
 	return 0;
 }
@@ -47,21 +66,35 @@ static int usbhs_rcar2_power_ctrl(struct platform_device *pdev,
 				void __iomem *base, int enable)
 {
 	struct usbhs_priv *priv = usbhs_pdev_to_priv(pdev);
+	int retval = -ENODEV;
+
+	if (priv->phy) {
+		if (enable) {
+			retval = phy_init(priv->phy);
 
-	if (!priv->phy)
-		return -ENODEV;
+			if (!retval)
+				retval = phy_power_on(priv->phy);
+		} else {
+			phy_power_off(priv->phy);
+			phy_exit(priv->phy);
+			retval = 0;
+		}
+	}
 
-	if (enable) {
-		int retval = usb_phy_init(priv->phy);
+	if (priv->usb_phy) {
+		if (enable) {
+			retval = usb_phy_init(priv->usb_phy);
 
-		if (!retval)
-			retval = usb_phy_set_suspend(priv->phy, 0);
-		return retval;
+			if (!retval)
+				retval = usb_phy_set_suspend(priv->usb_phy, 0);
+		} else {
+			usb_phy_set_suspend(priv->usb_phy, 1);
+			usb_phy_shutdown(priv->usb_phy);
+			retval = 0;
+		}
 	}
 
-	usb_phy_set_suspend(priv->phy, 1);
-	usb_phy_shutdown(priv->phy);
-	return 0;
+	return retval;
 }
 
 static int usbhs_rcar2_get_id(struct platform_device *pdev)
diff --git a/drivers/usb/serial/Kconfig b/drivers/usb/serial/Kconfig
index a69f7cd9d0bf..b7cf1982d1d9 100644
--- a/drivers/usb/serial/Kconfig
+++ b/drivers/usb/serial/Kconfig
@@ -60,6 +60,7 @@ config USB_SERIAL_SIMPLE
 		- Suunto ANT+ USB device.
 		- Medtronic CareLink USB device
 		- Fundamental Software dongle.
+		- Google USB serial devices
 		- HP4x calculators
 		- a number of Motorola phones
 		- Novatel Wireless GPS receivers
@@ -606,10 +607,10 @@ config USB_SERIAL_CYBERJACK
 	  If unsure, say N.
 
 config USB_SERIAL_XIRCOM
-	tristate "USB Xircom / Entregra Single Port Serial Driver"
+	tristate "USB Xircom / Entrega Single Port Serial Driver"
 	select USB_EZUSB_FX2
 	help
-	  Say Y here if you want to use a Xircom or Entregra single port USB to
+	  Say Y here if you want to use a Xircom or Entrega single port USB to
 	  serial converter device.  This driver makes use of firmware
 	  developed from scratch by Brian Warner.
 
diff --git a/drivers/usb/serial/keyspan_pda.c b/drivers/usb/serial/keyspan_pda.c
index 742d827f876c..dd97d8b572c3 100644
--- a/drivers/usb/serial/keyspan_pda.c
+++ b/drivers/usb/serial/keyspan_pda.c
@@ -1,5 +1,5 @@
 /*
- * USB Keyspan PDA / Xircom / Entregra Converter driver
+ * USB Keyspan PDA / Xircom / Entrega Converter driver
  *
  * Copyright (C) 1999 - 2001 Greg Kroah-Hartman	<greg@kroah.com>
  * Copyright (C) 1999, 2000 Brian Warner	<warner@lothar.com>
@@ -58,11 +58,11 @@ struct keyspan_pda_private {
 #define KEYSPAN_PDA_FAKE_ID		0x0103
 #define KEYSPAN_PDA_ID			0x0104 /* no clue */
 
-/* For Xircom PGSDB9 and older Entregra version of the same device */
+/* For Xircom PGSDB9 and older Entrega version of the same device */
 #define XIRCOM_VENDOR_ID		0x085a
 #define XIRCOM_FAKE_ID			0x8027
-#define ENTREGRA_VENDOR_ID		0x1645
-#define ENTREGRA_FAKE_ID		0x8093
+#define ENTREGA_VENDOR_ID		0x1645
+#define ENTREGA_FAKE_ID			0x8093
 
 static const struct usb_device_id id_table_combined[] = {
 #ifdef KEYSPAN
@@ -70,7 +70,7 @@ static const struct usb_device_id id_table_combined[] = {
 #endif
 #ifdef XIRCOM
 	{ USB_DEVICE(XIRCOM_VENDOR_ID, XIRCOM_FAKE_ID) },
-	{ USB_DEVICE(ENTREGRA_VENDOR_ID, ENTREGRA_FAKE_ID) },
+	{ USB_DEVICE(ENTREGA_VENDOR_ID, ENTREGA_FAKE_ID) },
 #endif
 	{ USB_DEVICE(KEYSPAN_VENDOR_ID, KEYSPAN_PDA_ID) },
 	{ }						/* Terminating entry */
@@ -93,7 +93,7 @@ static const struct usb_device_id id_table_fake[] = {
 #ifdef XIRCOM
 static const struct usb_device_id id_table_fake_xircom[] = {
 	{ USB_DEVICE(XIRCOM_VENDOR_ID, XIRCOM_FAKE_ID) },
-	{ USB_DEVICE(ENTREGRA_VENDOR_ID, ENTREGRA_FAKE_ID) },
+	{ USB_DEVICE(ENTREGA_VENDOR_ID, ENTREGA_FAKE_ID) },
 	{ }
 };
 #endif
@@ -667,7 +667,7 @@ static int keyspan_pda_fake_startup(struct usb_serial *serial)
 #endif
 #ifdef XIRCOM
 	else if ((le16_to_cpu(serial->dev->descriptor.idVendor) == XIRCOM_VENDOR_ID) ||
-		 (le16_to_cpu(serial->dev->descriptor.idVendor) == ENTREGRA_VENDOR_ID))
+		 (le16_to_cpu(serial->dev->descriptor.idVendor) == ENTREGA_VENDOR_ID))
 		fw_name = "keyspan_pda/xircom_pgs.fw";
 #endif
 	else {
@@ -744,7 +744,7 @@ static struct usb_serial_driver xircom_pgs_fake_device = {
 		.owner =	THIS_MODULE,
 		.name =		"xircom_no_firm",
 	},
-	.description =		"Xircom / Entregra PGS - (prerenumeration)",
+	.description =		"Xircom / Entrega PGS - (prerenumeration)",
 	.id_table =		id_table_fake_xircom,
 	.num_ports =		1,
 	.attach =		keyspan_pda_fake_startup,
diff --git a/drivers/usb/serial/kobil_sct.c b/drivers/usb/serial/kobil_sct.c
index 02c420af251e..2363654cafc9 100644
--- a/drivers/usb/serial/kobil_sct.c
+++ b/drivers/usb/serial/kobil_sct.c
@@ -244,7 +244,7 @@ static int kobil_open(struct tty_struct *tty, struct usb_serial_port *port)
 	    priv->device_type == KOBIL_ADAPTER_B_PRODUCT_ID ||
 	    priv->device_type == KOBIL_KAAN_SIM_PRODUCT_ID) {
 		/* start reading (Adapter B 'cause PNP string) */
-		result = usb_submit_urb(port->interrupt_in_urb, GFP_ATOMIC);
+		result = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
 		dev_dbg(dev, "%s - Send read URB returns: %i\n", __func__, result);
 	}
 
diff --git a/drivers/usb/serial/mos7720.c b/drivers/usb/serial/mos7720.c
index dfd728a263d2..4f70df33975a 100644
--- a/drivers/usb/serial/mos7720.c
+++ b/drivers/usb/serial/mos7720.c
@@ -1299,8 +1299,6 @@ static void mos7720_throttle(struct tty_struct *tty)
 		mos7720_port->shadowMCR &= ~UART_MCR_RTS;
 		write_mos_reg(port->serial, port->port_number, MCR,
 			      mos7720_port->shadowMCR);
-		if (status != 0)
-			return;
 	}
 }
 
@@ -1331,8 +1329,6 @@ static void mos7720_unthrottle(struct tty_struct *tty)
 		mos7720_port->shadowMCR |= UART_MCR_RTS;
 		write_mos_reg(port->serial, port->port_number, MCR,
 			      mos7720_port->shadowMCR);
-		if (status != 0)
-			return;
 	}
 }
 
@@ -1657,7 +1653,7 @@ static void change_port_settings(struct tty_struct *tty,
 	write_mos_reg(serial, port_number, IER, 0x0c);
 
 	if (port->read_urb->status != -EINPROGRESS) {
-		status = usb_submit_urb(port->read_urb, GFP_ATOMIC);
+		status = usb_submit_urb(port->read_urb, GFP_KERNEL);
 		if (status)
 			dev_dbg(&port->dev, "usb_submit_urb(read bulk) failed, status = %d\n", status);
 	}
@@ -1702,7 +1698,7 @@ static void mos7720_set_termios(struct tty_struct *tty,
 	change_port_settings(tty, mos7720_port, old_termios);
 
 	if (port->read_urb->status != -EINPROGRESS) {
-		status = usb_submit_urb(port->read_urb, GFP_ATOMIC);
+		status = usb_submit_urb(port->read_urb, GFP_KERNEL);
 		if (status)
 			dev_dbg(&port->dev, "usb_submit_urb(read bulk) failed, status = %d\n", status);
 	}
diff --git a/drivers/usb/serial/mos7840.c b/drivers/usb/serial/mos7840.c
index 3d88eefdf1d1..220b4be89641 100644
--- a/drivers/usb/serial/mos7840.c
+++ b/drivers/usb/serial/mos7840.c
@@ -1904,7 +1904,7 @@ static void mos7840_change_port_settings(struct tty_struct *tty,
 
 	if (mos7840_port->read_urb_busy == false) {
 		mos7840_port->read_urb_busy = true;
-		status = usb_submit_urb(mos7840_port->read_urb, GFP_ATOMIC);
+		status = usb_submit_urb(mos7840_port->read_urb, GFP_KERNEL);
 		if (status) {
 			dev_dbg(&port->dev, "usb_submit_urb(read bulk) failed, status = %d\n",
 			    status);
@@ -1968,7 +1968,7 @@ static void mos7840_set_termios(struct tty_struct *tty,
 
 	if (mos7840_port->read_urb_busy == false) {
 		mos7840_port->read_urb_busy = true;
-		status = usb_submit_urb(mos7840_port->read_urb, GFP_ATOMIC);
+		status = usb_submit_urb(mos7840_port->read_urb, GFP_KERNEL);
 		if (status) {
 			dev_dbg(&port->dev, "usb_submit_urb(read bulk) failed, status = %d\n",
 			    status);
diff --git a/drivers/usb/serial/qcserial.c b/drivers/usb/serial/qcserial.c
index b2aa003bf411..cb3e14780a7e 100644
--- a/drivers/usb/serial/qcserial.c
+++ b/drivers/usb/serial/qcserial.c
@@ -27,12 +27,15 @@ enum qcserial_layouts {
 	QCSERIAL_G2K = 0,	/* Gobi 2000 */
 	QCSERIAL_G1K = 1,	/* Gobi 1000 */
 	QCSERIAL_SWI = 2,	/* Sierra Wireless */
+	QCSERIAL_HWI = 3,	/* Huawei */
 };
 
 #define DEVICE_G1K(v, p) \
 	USB_DEVICE(v, p), .driver_info = QCSERIAL_G1K
 #define DEVICE_SWI(v, p) \
 	USB_DEVICE(v, p), .driver_info = QCSERIAL_SWI
+#define DEVICE_HWI(v, p) \
+	USB_DEVICE(v, p), .driver_info = QCSERIAL_HWI
 
 static const struct usb_device_id id_table[] = {
 	/* Gobi 1000 devices */
@@ -157,6 +160,9 @@ static const struct usb_device_id id_table[] = {
 	{DEVICE_SWI(0x413c, 0x81a8)},	/* Dell Wireless 5808 Gobi(TM) 4G LTE Mobile Broadband Card */
 	{DEVICE_SWI(0x413c, 0x81a9)},	/* Dell Wireless 5808e Gobi(TM) 4G LTE Mobile Broadband Card */
 
+	/* Huawei devices */
+	{DEVICE_HWI(0x03f0, 0x581d)},	/* HP lt4112 LTE/HSPA+ Gobi 4G Modem (Huawei me906e) */
+
 	{ }				/* Terminating entry */
 };
 MODULE_DEVICE_TABLE(usb, id_table);
@@ -287,6 +293,33 @@ static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
 			break;
 		}
 		break;
+	case QCSERIAL_HWI:
+		/*
+		 * Huawei layout:
+		 * 0: AT-capable modem port
+		 * 1: DM/DIAG
+		 * 2: AT-capable modem port
+		 * 3: CCID-compatible PCSC interface
+		 * 4: QMI/net
+		 * 5: NMEA
+		 */
+		switch (ifnum) {
+		case 0:
+		case 2:
+			dev_dbg(dev, "Modem port found\n");
+			break;
+		case 1:
+			dev_dbg(dev, "DM/DIAG interface found\n");
+			break;
+		case 5:
+			dev_dbg(dev, "NMEA GPS interface found\n");
+			break;
+		default:
+			/* don't claim any unsupported interface */
+			altsetting = -1;
+			break;
+		}
+		break;
 	default:
 		dev_err(dev, "unsupported device layout type: %lu\n",
 			id->driver_info);
diff --git a/drivers/usb/serial/usb-serial-simple.c b/drivers/usb/serial/usb-serial-simple.c
index 7064eb8d6142..3658662898fc 100644
--- a/drivers/usb/serial/usb-serial-simple.c
+++ b/drivers/usb/serial/usb-serial-simple.c
@@ -56,6 +56,14 @@ DEVICE(funsoft, FUNSOFT_IDS);
 	{ USB_DEVICE(0x8087, 0x0716) }
 DEVICE(flashloader, FLASHLOADER_IDS);
 
+/* Google Serial USB SubClass */
+#define GOOGLE_IDS()						\
+	{ USB_VENDOR_AND_INTERFACE_INFO(0x18d1,			\
+					USB_CLASS_VENDOR_SPEC,	\
+					0x50,			\
+					0x01) }
+DEVICE(google, GOOGLE_IDS);
+
 /* ViVOpay USB Serial Driver */
 #define VIVOPAY_IDS()			\
 	{ USB_DEVICE(0x1d5f, 0x1004) }	/* ViVOpay 8800 */
@@ -64,7 +72,7 @@ DEVICE(vivopay, VIVOPAY_IDS);
 /* Motorola USB Phone driver */
 #define MOTO_IDS()			\
 	{ USB_DEVICE(0x05c6, 0x3197) },	/* unknown Motorola phone */	\
-	{ USB_DEVICE(0x0c44, 0x0022) },	/* unknown Mororola phone */	\
+	{ USB_DEVICE(0x0c44, 0x0022) },	/* unknown Motorola phone */	\
 	{ USB_DEVICE(0x22b8, 0x2a64) },	/* Motorola KRZR K1m */		\
 	{ USB_DEVICE(0x22b8, 0x2c84) },	/* Motorola VE240 phone */	\
 	{ USB_DEVICE(0x22b8, 0x2c64) }	/* Motorola V950 phone */
@@ -97,6 +105,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&zio_device,
 	&funsoft_device,
 	&flashloader_device,
+	&google_device,
 	&vivopay_device,
 	&moto_modem_device,
 	&novatel_gps_device,
@@ -111,6 +120,7 @@ static const struct usb_device_id id_table[] = {
 	ZIO_IDS(),
 	FUNSOFT_IDS(),
 	FLASHLOADER_IDS(),
+	GOOGLE_IDS(),
 	VIVOPAY_IDS(),
 	MOTO_IDS(),
 	NOVATEL_IDS(),
diff --git a/drivers/usb/storage/Kconfig b/drivers/usb/storage/Kconfig
index 715f299af6ea..ec84758f0e23 100644
--- a/drivers/usb/storage/Kconfig
+++ b/drivers/usb/storage/Kconfig
@@ -41,7 +41,7 @@ config USB_STORAGE_REALTEK
 
 config REALTEK_AUTOPM
 	bool "Realtek Card Reader autosuspend support"
-	depends on USB_STORAGE_REALTEK && PM_RUNTIME
+	depends on USB_STORAGE_REALTEK && PM
 	default y
 
 config USB_STORAGE_DATAFAB
diff --git a/drivers/usb/storage/debug.c b/drivers/usb/storage/debug.c
index 2d81e1d8ee30..57bf3ad41fb6 100644
--- a/drivers/usb/storage/debug.c
+++ b/drivers/usb/storage/debug.c
@@ -181,17 +181,14 @@ void usb_stor_show_sense(const struct us_data *us,
 		US_DEBUGPX("%s\n", what);
 }
 
-int usb_stor_dbg(const struct us_data *us, const char *fmt, ...)
+void usb_stor_dbg(const struct us_data *us, const char *fmt, ...)
 {
 	va_list args;
-	int r;
 
 	va_start(args, fmt);
 
-	r = dev_vprintk_emit(LOGLEVEL_DEBUG, &us->pusb_dev->dev, fmt, args);
+	dev_vprintk_emit(LOGLEVEL_DEBUG, &us->pusb_dev->dev, fmt, args);
 
 	va_end(args);
-
-	return r;
 }
 EXPORT_SYMBOL_GPL(usb_stor_dbg);
diff --git a/drivers/usb/storage/debug.h b/drivers/usb/storage/debug.h
index b1273f03e223..f52520306e1a 100644
--- a/drivers/usb/storage/debug.h
+++ b/drivers/usb/storage/debug.h
@@ -50,15 +50,17 @@
 void usb_stor_show_command(const struct us_data *us, struct scsi_cmnd *srb);
 void usb_stor_show_sense(const struct us_data *us, unsigned char key,
 			 unsigned char asc, unsigned char ascq);
-__printf(2, 3) int usb_stor_dbg(const struct us_data *us,
-				const char *fmt, ...);
+__printf(2, 3) void usb_stor_dbg(const struct us_data *us,
+				 const char *fmt, ...);
 
 #define US_DEBUGPX(fmt, ...)	printk(fmt, ##__VA_ARGS__)
 #define US_DEBUG(x)		x
 #else
 __printf(2, 3)
-static inline int _usb_stor_dbg(const struct us_data *us,
-				const char *fmt, ...) {return 1;}
+static inline void _usb_stor_dbg(const struct us_data *us,
+				 const char *fmt, ...)
+{
+}
 #define usb_stor_dbg(us, fmt, ...)				\
 	do { if (0) _usb_stor_dbg(us, fmt, ##__VA_ARGS__); } while (0)
 #define US_DEBUGPX(fmt, ...)					\
diff --git a/drivers/usb/storage/initializers.c b/drivers/usb/storage/initializers.c
index 73f125e0cb58..31fa2e92065b 100644
--- a/drivers/usb/storage/initializers.c
+++ b/drivers/usb/storage/initializers.c
@@ -49,10 +49,9 @@ int usb_stor_euscsi_init(struct us_data *us)
 	int result;
 
 	usb_stor_dbg(us, "Attempting to init eUSCSI bridge...\n");
-	us->iobuf[0] = 0x1;
 	result = usb_stor_control_msg(us, us->send_ctrl_pipe,
 			0x0C, USB_RECIP_INTERFACE | USB_TYPE_VENDOR,
-			0x01, 0x0, us->iobuf, 0x1, 5 * HZ);
+			0x01, 0x0, NULL, 0x0, 5 * HZ);
 	usb_stor_dbg(us, "-- result is %d\n", result);
 
 	return 0;
diff --git a/drivers/usb/storage/transport.c b/drivers/usb/storage/transport.c
index b1d815eb6d0b..540add24a12f 100644
--- a/drivers/usb/storage/transport.c
+++ b/drivers/usb/storage/transport.c
@@ -1035,9 +1035,20 @@ int usb_stor_Bulk_max_lun(struct us_data *us)
 	usb_stor_dbg(us, "GetMaxLUN command result is %d, data is %d\n",
 		     result, us->iobuf[0]);
 
-	/* if we have a successful request, return the result */
-	if (result > 0)
-		return us->iobuf[0];
+	/*
+	 * If we have a successful request, return the result if valid. The
+	 * CBW LUN field is 4 bits wide, so the value reported by the device
+	 * should fit into that.
+	 */
+	if (result > 0) {
+		if (us->iobuf[0] < 16) {
+			return us->iobuf[0];
+		} else {
+			dev_info(&us->pusb_intf->dev,
+				 "Max LUN %d is not valid, using 0 instead",
+				 us->iobuf[0]);
+		}
+	}
 
 	/*
 	 * Some devices don't like GetMaxLUN.  They may STALL the control
diff --git a/drivers/usb/storage/uas.c b/drivers/usb/storage/uas.c
index 4047edfb64e1..6cdabdc119a7 100644
--- a/drivers/usb/storage/uas.c
+++ b/drivers/usb/storage/uas.c
@@ -66,7 +66,7 @@ enum {
 /* Overrides scsi_pointer */
 struct uas_cmd_info {
 	unsigned int state;
-	unsigned int stream;
+	unsigned int uas_tag;
 	struct urb *cmd_urb;
 	struct urb *data_in_urb;
 	struct urb *data_out_urb;
@@ -173,30 +173,15 @@ static void uas_sense(struct urb *urb, struct scsi_cmnd *cmnd)
 	cmnd->result = sense_iu->status;
 }
 
-/*
- * scsi-tags go from 0 - (nr_tags - 1), uas tags need to match stream-ids,
- * which go from 1 - nr_streams. And we use 1 for untagged commands.
- */
-static int uas_get_tag(struct scsi_cmnd *cmnd)
-{
-	int tag;
-
-	if (cmnd->flags & SCMD_TAGGED)
-		tag = cmnd->request->tag + 2;
-	else
-		tag = 1;
-
-	return tag;
-}
-
 static void uas_log_cmd_state(struct scsi_cmnd *cmnd, const char *prefix,
 			      int status)
 {
 	struct uas_cmd_info *ci = (void *)&cmnd->SCp;
+	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
 
 	scmd_printk(KERN_INFO, cmnd,
-		    "%s %d tag %d inflight:%s%s%s%s%s%s%s%s%s%s%s%s ",
-		    prefix, status, uas_get_tag(cmnd),
+		    "%s %d uas-tag %d inflight:%s%s%s%s%s%s%s%s%s%s%s%s ",
+		    prefix, status, cmdinfo->uas_tag,
 		    (ci->state & SUBMIT_STATUS_URB)     ? " s-st"  : "",
 		    (ci->state & ALLOC_DATA_IN_URB)     ? " a-in"  : "",
 		    (ci->state & SUBMIT_DATA_IN_URB)    ? " s-in"  : "",
@@ -242,7 +227,7 @@ static int uas_try_complete(struct scsi_cmnd *cmnd, const char *caller)
 			      DATA_OUT_URB_INFLIGHT |
 			      COMMAND_ABORTED))
 		return -EBUSY;
-	devinfo->cmnd[uas_get_tag(cmnd) - 1] = NULL;
+	devinfo->cmnd[cmdinfo->uas_tag - 1] = NULL;
 	uas_free_unsubmitted_urbs(cmnd);
 	cmnd->scsi_done(cmnd);
 	return 0;
@@ -289,7 +274,7 @@ static void uas_stat_cmplt(struct urb *urb)
 	idx = be16_to_cpup(&iu->tag) - 1;
 	if (idx >= MAX_CMNDS || !devinfo->cmnd[idx]) {
 		dev_err(&urb->dev->dev,
-			"stat urb: no pending cmd for tag %d\n", idx + 1);
+			"stat urb: no pending cmd for uas-tag %d\n", idx + 1);
 		goto out;
 	}
 
@@ -427,7 +412,8 @@ static struct urb *uas_alloc_data_urb(struct uas_dev_info *devinfo, gfp_t gfp,
 		goto out;
 	usb_fill_bulk_urb(urb, udev, pipe, NULL, sdb->length,
 			  uas_data_cmplt, cmnd);
-	urb->stream_id = cmdinfo->stream;
+	if (devinfo->use_streams)
+		urb->stream_id = cmdinfo->uas_tag;
 	urb->num_sgs = udev->bus->sg_tablesize ? sdb->table.nents : 0;
 	urb->sg = sdb->table.sgl;
  out:
@@ -451,7 +437,8 @@ static struct urb *uas_alloc_sense_urb(struct uas_dev_info *devinfo, gfp_t gfp,
 
 	usb_fill_bulk_urb(urb, udev, devinfo->status_pipe, iu, sizeof(*iu),
 			  uas_stat_cmplt, cmnd->device->host);
-	urb->stream_id = cmdinfo->stream;
+	if (devinfo->use_streams)
+		urb->stream_id = cmdinfo->uas_tag;
 	urb->transfer_flags |= URB_FREE_BUFFER;
  out:
 	return urb;
@@ -465,6 +452,7 @@ static struct urb *uas_alloc_cmd_urb(struct uas_dev_info *devinfo, gfp_t gfp,
 {
 	struct usb_device *udev = devinfo->udev;
 	struct scsi_device *sdev = cmnd->device;
+	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
 	struct urb *urb = usb_alloc_urb(0, gfp);
 	struct command_iu *iu;
 	int len;
@@ -481,7 +469,7 @@ static struct urb *uas_alloc_cmd_urb(struct uas_dev_info *devinfo, gfp_t gfp,
 		goto free;
 
 	iu->iu_id = IU_ID_COMMAND;
-	iu->tag = cpu_to_be16(uas_get_tag(cmnd));
+	iu->tag = cpu_to_be16(cmdinfo->uas_tag);
 	iu->prio_attr = UAS_SIMPLE_TAG;
 	iu->len = len;
 	int_to_scsilun(sdev->lun, &iu->lun);
@@ -608,8 +596,7 @@ static int uas_queuecommand_lck(struct scsi_cmnd *cmnd,
 	struct uas_dev_info *devinfo = sdev->hostdata;
 	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
 	unsigned long flags;
-	unsigned int stream;
-	int err;
+	int idx, err;
 
 	BUILD_BUG_ON(sizeof(struct uas_cmd_info) > sizeof(struct scsi_pointer));
 
@@ -635,8 +622,12 @@ static int uas_queuecommand_lck(struct scsi_cmnd *cmnd,
 		return 0;
 	}
 
-	stream = uas_get_tag(cmnd);
-	if (devinfo->cmnd[stream - 1]) {
+	/* Find a free uas-tag */
+	for (idx = 0; idx < devinfo->qdepth; idx++) {
+		if (!devinfo->cmnd[idx])
+			break;
+	}
+	if (idx == devinfo->qdepth) {
 		spin_unlock_irqrestore(&devinfo->lock, flags);
 		return SCSI_MLQUEUE_DEVICE_BUSY;
 	}
@@ -644,7 +635,7 @@ static int uas_queuecommand_lck(struct scsi_cmnd *cmnd,
 	cmnd->scsi_done = done;
 
 	memset(cmdinfo, 0, sizeof(*cmdinfo));
-	cmdinfo->stream = stream;
+	cmdinfo->uas_tag = idx + 1; /* uas-tag == usb-stream-id, so 1 based */
 	cmdinfo->state = SUBMIT_STATUS_URB | ALLOC_CMD_URB | SUBMIT_CMD_URB;
 
 	switch (cmnd->sc_data_direction) {
@@ -659,10 +650,8 @@ static int uas_queuecommand_lck(struct scsi_cmnd *cmnd,
 		break;
 	}
 
-	if (!devinfo->use_streams) {
+	if (!devinfo->use_streams)
 		cmdinfo->state &= ~(SUBMIT_DATA_IN_URB | SUBMIT_DATA_OUT_URB);
-		cmdinfo->stream = 0;
-	}
 
 	err = uas_submit_urbs(cmnd, devinfo, GFP_ATOMIC);
 	if (err) {
@@ -674,7 +663,7 @@ static int uas_queuecommand_lck(struct scsi_cmnd *cmnd,
 		uas_add_work(cmdinfo);
 	}
 
-	devinfo->cmnd[stream - 1] = cmnd;
+	devinfo->cmnd[idx] = cmnd;
 	spin_unlock_irqrestore(&devinfo->lock, flags);
 	return 0;
 }
@@ -702,7 +691,7 @@ static int uas_eh_abort_handler(struct scsi_cmnd *cmnd)
 	cmdinfo->state |= COMMAND_ABORTED;
 
 	/* Drop all refs to this cmnd, kill data urbs to break their ref */
-	devinfo->cmnd[uas_get_tag(cmnd) - 1] = NULL;
+	devinfo->cmnd[cmdinfo->uas_tag - 1] = NULL;
 	if (cmdinfo->state & DATA_IN_URB_INFLIGHT)
 		data_in_urb = usb_get_urb(cmdinfo->data_in_urb);
 	if (cmdinfo->state & DATA_OUT_URB_INFLIGHT)
@@ -816,13 +805,6 @@ static struct scsi_host_template uas_host_template = {
 	.sg_tablesize = SG_NONE,
 	.cmd_per_lun = 1,	/* until we override it */
 	.skip_settle_delay = 1,
-
-	/*
-	 * The uas drivers expects tags not to be bigger than the maximum
-	 * per-device queue depth, which is not true with the blk-mq tag
-	 * allocator.
-	 */
-	.disable_blk_mq = true,
 	.use_blk_tags = 1,
 };
 
diff --git a/drivers/usb/storage/usb.c b/drivers/usb/storage/usb.c
index 9d66ce62542e..d468d02179f4 100644
--- a/drivers/usb/storage/usb.c
+++ b/drivers/usb/storage/usb.c
@@ -884,7 +884,9 @@ static void usb_stor_scan_dwork(struct work_struct *work)
 	dev_dbg(dev, "starting scan\n");
 
 	/* For bulk-only devices, determine the max LUN value */
-	if (us->protocol == USB_PR_BULK && !(us->fflags & US_FL_SINGLE_LUN)) {
+	if (us->protocol == USB_PR_BULK &&
+	    !(us->fflags & US_FL_SINGLE_LUN) &&
+	    !(us->fflags & US_FL_SCM_MULT_TARG)) {
 		mutex_lock(&us->dev_mutex);
 		us->max_lun = usb_stor_Bulk_max_lun(us);
 		mutex_unlock(&us->dev_mutex);
@@ -983,21 +985,31 @@ int usb_stor_probe2(struct us_data *us)
 	usb_stor_dbg(us, "Transport: %s\n", us->transport_name);
 	usb_stor_dbg(us, "Protocol: %s\n", us->protocol_name);
 
+	if (us->fflags & US_FL_SCM_MULT_TARG) {
+		/*
+		 * SCM eUSCSI bridge devices can have different numbers
+		 * of LUNs on different targets; allow all to be probed.
+		 */
+		us->max_lun = 7;
+		/* The eUSCSI itself has ID 7, so avoid scanning that */
+		us_to_host(us)->this_id = 7;
+		/* max_id is 8 initially, so no need to set it here */
+	} else {
+		/* In the normal case there is only a single target */
+		us_to_host(us)->max_id = 1;
+		/*
+		 * Like Windows, we won't store the LUN bits in CDB[1] for
+		 * SCSI-2 devices using the Bulk-Only transport (even though
+		 * this violates the SCSI spec).
+		 */
+		if (us->transport == usb_stor_Bulk_transport)
+			us_to_host(us)->no_scsi2_lun_in_cdb = 1;
+	}
+
 	/* fix for single-lun devices */
 	if (us->fflags & US_FL_SINGLE_LUN)
 		us->max_lun = 0;
 
-	if (!(us->fflags & US_FL_SCM_MULT_TARG))
-		us_to_host(us)->max_id = 1;
-
-	/*
-	 * Like Windows, we won't store the LUN bits in CDB[1] for SCSI-2
-	 * devices using the Bulk-Only transport (even though this violates
-	 * the SCSI spec).
-	 */
-	if (us->transport == usb_stor_Bulk_transport)
-		us_to_host(us)->no_scsi2_lun_in_cdb = 1;
-
 	/* Find the endpoints and calculate pipe values */
 	result = get_pipes(us);
 	if (result)
diff --git a/drivers/usb/usbip/stub_dev.c b/drivers/usb/usbip/stub_dev.c
index fac20e0434c0..a3ec49bdc1e6 100644
--- a/drivers/usb/usbip/stub_dev.c
+++ b/drivers/usb/usbip/stub_dev.c
@@ -311,7 +311,6 @@ static int stub_probe(struct usb_device *udev)
 {
 	struct stub_device *sdev = NULL;
 	const char *udev_busid = dev_name(&udev->dev);
-	int err = 0;
 	struct bus_id_priv *busid_priv;
 	int rc;
 
@@ -372,23 +371,28 @@ static int stub_probe(struct usb_device *udev)
 			(struct usb_dev_state *) udev);
 	if (rc) {
 		dev_dbg(&udev->dev, "unable to claim port\n");
-		return rc;
+		goto err_port;
 	}
 
-	err = stub_add_files(&udev->dev);
-	if (err) {
+	rc = stub_add_files(&udev->dev);
+	if (rc) {
 		dev_err(&udev->dev, "stub_add_files for %s\n", udev_busid);
-		dev_set_drvdata(&udev->dev, NULL);
-		usb_put_dev(udev);
-		kthread_stop_put(sdev->ud.eh);
-
-		busid_priv->sdev = NULL;
-		stub_device_free(sdev);
-		return err;
+		goto err_files;
 	}
 	busid_priv->status = STUB_BUSID_ALLOC;
 
 	return 0;
+err_files:
+	usb_hub_release_port(udev->parent, udev->portnum,
+			     (struct usb_dev_state *) udev);
+err_port:
+	dev_set_drvdata(&udev->dev, NULL);
+	usb_put_dev(udev);
+	kthread_stop_put(sdev->ud.eh);
+
+	busid_priv->sdev = NULL;
+	stub_device_free(sdev);
+	return rc;
 }
 
 static void shutdown_busid(struct bus_id_priv *busid_priv)
diff --git a/drivers/usb/usbip/vhci_hcd.c b/drivers/usb/usbip/vhci_hcd.c
index c02374b6049c..1ae9d40f96bf 100644
--- a/drivers/usb/usbip/vhci_hcd.c
+++ b/drivers/usb/usbip/vhci_hcd.c
@@ -518,8 +518,7 @@ static int vhci_urb_enqueue(struct usb_hcd *hcd, struct urb *urb,
 			dev_info(dev, "SetAddress Request (%d) to port %d\n",
 				 ctrlreq->wValue, vdev->rhport);
 
-			if (vdev->udev)
-				usb_put_dev(vdev->udev);
+			usb_put_dev(vdev->udev);
 			vdev->udev = usb_get_dev(urb->dev);
 
 			spin_lock(&vdev->ud.lock);
@@ -539,8 +538,7 @@ static int vhci_urb_enqueue(struct usb_hcd *hcd, struct urb *urb,
 				usbip_dbg_vhci_hc(
 					"Not yet?:Get_Descriptor to device 0 (get max pipe size)\n");
 
-			if (vdev->udev)
-				usb_put_dev(vdev->udev);
+			usb_put_dev(vdev->udev);
 			vdev->udev = usb_get_dev(urb->dev);
 			goto out;
 
@@ -831,8 +829,7 @@ static void vhci_device_reset(struct usbip_device *ud)
 	vdev->speed  = 0;
 	vdev->devid  = 0;
 
-	if (vdev->udev)
-		usb_put_dev(vdev->udev);
+	usb_put_dev(vdev->udev);
 	vdev->udev = NULL;
 
 	if (ud->tcp_socket) {
@@ -1110,7 +1107,6 @@ static struct platform_driver vhci_driver = {
 	.resume	= vhci_hcd_resume,
 	.driver	= {
 		.name = driver_name,
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/usb/wusbcore/dev-sysfs.c b/drivers/usb/wusbcore/dev-sysfs.c
index 101834576236..415b14002a61 100644
--- a/drivers/usb/wusbcore/dev-sysfs.c
+++ b/drivers/usb/wusbcore/dev-sysfs.c
@@ -101,7 +101,7 @@ static ssize_t wusb_ck_store(struct device *dev,
 	if (wusbhc == NULL)
 		return -ENODEV;
 	result = wusb_dev_4way_handshake(wusbhc, usb_dev->wusb_dev, &ck);
-	memset(&ck, 0, sizeof(ck));
+	memzero_explicit(&ck, sizeof(ck));
 	wusbhc_put(wusbhc);
 	return result < 0 ? result : size;
 }
diff --git a/drivers/usb/wusbcore/security.c b/drivers/usb/wusbcore/security.c
index cc74d669c802..b66faaf3e842 100644
--- a/drivers/usb/wusbcore/security.c
+++ b/drivers/usb/wusbcore/security.c
@@ -522,10 +522,10 @@ error_hs3:
 error_hs2:
 error_hs1:
 	memset(hs, 0, 3*sizeof(hs[0]));
-	memset(&keydvt_out, 0, sizeof(keydvt_out));
-	memset(&keydvt_in, 0, sizeof(keydvt_in));
-	memset(&ccm_n, 0, sizeof(ccm_n));
-	memset(mic, 0, sizeof(mic));
+	memzero_explicit(&keydvt_out, sizeof(keydvt_out));
+	memzero_explicit(&keydvt_in, sizeof(keydvt_in));
+	memzero_explicit(&ccm_n, sizeof(ccm_n));
+	memzero_explicit(mic, sizeof(mic));
 	if (result < 0)
 		wusb_dev_set_encryption(usb_dev, 0);
 error_dev_set_encryption:
diff --git a/drivers/uwb/rsv.c b/drivers/uwb/rsv.c
index 0887ae982783..536ad42b0a4b 100644
--- a/drivers/uwb/rsv.c
+++ b/drivers/uwb/rsv.c
@@ -213,7 +213,7 @@ void uwb_rsv_backoff_win_timer(unsigned long arg)
 		bow->total_expired = 0;
 		bow->window = UWB_DRP_BACKOFF_WIN_MIN >> 1;
 	}
-	dev_dbg(dev, "backoff_win_timer total_expired=%d, n=%d\n: ", bow->total_expired, bow->n);
+	dev_dbg(dev, "backoff_win_timer total_expired=%d, n=%d\n", bow->total_expired, bow->n);
 
 	/* try to relocate all the "to be moved" relocations */
 	uwb_rsv_handle_drp_avail_change(rc);
@@ -234,7 +234,7 @@ void uwb_rsv_backoff_win_increment(struct uwb_rc *rc)
 
 	bow->window <<= 1;
 	bow->n = prandom_u32() & (bow->window - 1);
-	dev_dbg(dev, "new_window=%d, n=%d\n: ", bow->window, bow->n);
+	dev_dbg(dev, "new_window=%d, n=%d\n", bow->window, bow->n);
 
 	/* reset the timer associated variables */
 	timeout_us = bow->n * UWB_SUPERFRAME_LENGTH_US;
diff --git a/drivers/vfio/Kconfig b/drivers/vfio/Kconfig
index d8c57636b9ce..14e27ab32456 100644
--- a/drivers/vfio/Kconfig
+++ b/drivers/vfio/Kconfig
@@ -16,7 +16,7 @@ config VFIO_SPAPR_EEH
 menuconfig VFIO
 	tristate "VFIO Non-Privileged userspace driver framework"
 	depends on IOMMU_API
-	select VFIO_IOMMU_TYPE1 if X86
+	select VFIO_IOMMU_TYPE1 if (X86 || S390 || ARM_SMMU)
 	select VFIO_IOMMU_SPAPR_TCE if (PPC_POWERNV || PPC_PSERIES)
 	select VFIO_SPAPR_EEH if (PPC_POWERNV || PPC_PSERIES)
 	select ANON_INODES
diff --git a/drivers/vfio/pci/Kconfig b/drivers/vfio/pci/Kconfig
index c41b01e2b693..c6bb5da2d2a7 100644
--- a/drivers/vfio/pci/Kconfig
+++ b/drivers/vfio/pci/Kconfig
@@ -16,3 +16,11 @@ config VFIO_PCI_VGA
 	  BIOS and generic video drivers.
 
 	  If you don't know what to do here, say N.
+
+config VFIO_PCI_MMAP
+	depends on VFIO_PCI
+	def_bool y if !S390
+
+config VFIO_PCI_INTX
+	depends on VFIO_PCI
+	def_bool y if !S390
diff --git a/drivers/vfio/pci/vfio_pci.c b/drivers/vfio/pci/vfio_pci.c
index 9558da3f06a0..255201f22126 100644
--- a/drivers/vfio/pci/vfio_pci.c
+++ b/drivers/vfio/pci/vfio_pci.c
@@ -215,7 +215,7 @@ static int vfio_pci_get_irq_count(struct vfio_pci_device *vdev, int irq_type)
 	if (irq_type == VFIO_PCI_INTX_IRQ_INDEX) {
 		u8 pin;
 		pci_read_config_byte(vdev->pdev, PCI_INTERRUPT_PIN, &pin);
-		if (pin)
+		if (IS_ENABLED(CONFIG_VFIO_PCI_INTX) && pin)
 			return 1;
 
 	} else if (irq_type == VFIO_PCI_MSI_IRQ_INDEX) {
@@ -406,7 +406,8 @@ static long vfio_pci_ioctl(void *device_data,
 
 			info.flags = VFIO_REGION_INFO_FLAG_READ |
 				     VFIO_REGION_INFO_FLAG_WRITE;
-			if (pci_resource_flags(pdev, info.index) &
+			if (IS_ENABLED(CONFIG_VFIO_PCI_MMAP) &&
+			    pci_resource_flags(pdev, info.index) &
 			    IORESOURCE_MEM && info.size >= PAGE_SIZE)
 				info.flags |= VFIO_REGION_INFO_FLAG_MMAP;
 			break;
diff --git a/drivers/vfio/pci/vfio_pci_config.c b/drivers/vfio/pci/vfio_pci_config.c
index 1de3f94aa7de..ff75ca31a199 100644
--- a/drivers/vfio/pci/vfio_pci_config.c
+++ b/drivers/vfio/pci/vfio_pci_config.c
@@ -609,6 +609,10 @@ static int __init init_pci_cap_basic_perm(struct perm_bits *perm)
 
 	/* Sometimes used by sw, just virtualize */
 	p_setb(perm, PCI_INTERRUPT_LINE, (u8)ALL_VIRT, (u8)ALL_WRITE);
+
+	/* Virtualize interrupt pin to allow hiding INTx */
+	p_setb(perm, PCI_INTERRUPT_PIN, (u8)ALL_VIRT, (u8)NO_WRITE);
+
 	return 0;
 }
 
@@ -1445,6 +1449,9 @@ int vfio_config_init(struct vfio_pci_device *vdev)
 		*(__le16 *)&vconfig[PCI_DEVICE_ID] = cpu_to_le16(pdev->device);
 	}
 
+	if (!IS_ENABLED(CONFIG_VFIO_PCI_INTX))
+		vconfig[PCI_INTERRUPT_PIN] = 0;
+
 	ret = vfio_cap_init(vdev);
 	if (ret)
 		goto out;
diff --git a/drivers/vhost/vringh.c b/drivers/vhost/vringh.c
index 5174ebac288d..3bb02c60a2f5 100644
--- a/drivers/vhost/vringh.c
+++ b/drivers/vhost/vringh.c
@@ -11,6 +11,7 @@
 #include <linux/uaccess.h>
 #include <linux/slab.h>
 #include <linux/export.h>
+#include <uapi/linux/virtio_config.h>
 
 static __printf(1,2) __cold void vringh_bad(const char *fmt, ...)
 {
@@ -28,13 +29,14 @@ static __printf(1,2) __cold void vringh_bad(const char *fmt, ...)
 
 /* Returns vring->num if empty, -ve on error. */
 static inline int __vringh_get_head(const struct vringh *vrh,
-				    int (*getu16)(u16 *val, const u16 *p),
+				    int (*getu16)(const struct vringh *vrh,
+						  u16 *val, const __virtio16 *p),
 				    u16 *last_avail_idx)
 {
 	u16 avail_idx, i, head;
 	int err;
 
-	err = getu16(&avail_idx, &vrh->vring.avail->idx);
+	err = getu16(vrh, &avail_idx, &vrh->vring.avail->idx);
 	if (err) {
 		vringh_bad("Failed to access avail idx at %p",
 			   &vrh->vring.avail->idx);
@@ -49,7 +51,7 @@ static inline int __vringh_get_head(const struct vringh *vrh,
 
 	i = *last_avail_idx & (vrh->vring.num - 1);
 
-	err = getu16(&head, &vrh->vring.avail->ring[i]);
+	err = getu16(vrh, &head, &vrh->vring.avail->ring[i]);
 	if (err) {
 		vringh_bad("Failed to read head: idx %d address %p",
 			   *last_avail_idx, &vrh->vring.avail->ring[i]);
@@ -144,28 +146,32 @@ static inline bool no_range_check(struct vringh *vrh, u64 addr, size_t *len,
 }
 
 /* No reason for this code to be inline. */
-static int move_to_indirect(int *up_next, u16 *i, void *addr,
+static int move_to_indirect(const struct vringh *vrh,
+			    int *up_next, u16 *i, void *addr,
 			    const struct vring_desc *desc,
 			    struct vring_desc **descs, int *desc_max)
 {
+	u32 len;
+
 	/* Indirect tables can't have indirect. */
 	if (*up_next != -1) {
 		vringh_bad("Multilevel indirect %u->%u", *up_next, *i);
 		return -EINVAL;
 	}
 
-	if (unlikely(desc->len % sizeof(struct vring_desc))) {
+	len = vringh32_to_cpu(vrh, desc->len);
+	if (unlikely(len % sizeof(struct vring_desc))) {
 		vringh_bad("Strange indirect len %u", desc->len);
 		return -EINVAL;
 	}
 
 	/* We will check this when we follow it! */
-	if (desc->flags & VRING_DESC_F_NEXT)
-		*up_next = desc->next;
+	if (desc->flags & cpu_to_vringh16(vrh, VRING_DESC_F_NEXT))
+		*up_next = vringh16_to_cpu(vrh, desc->next);
 	else
 		*up_next = -2;
 	*descs = addr;
-	*desc_max = desc->len / sizeof(struct vring_desc);
+	*desc_max = len / sizeof(struct vring_desc);
 
 	/* Now, start at the first indirect. */
 	*i = 0;
@@ -287,22 +293,25 @@ __vringh_iov(struct vringh *vrh, u16 i,
 		if (unlikely(err))
 			goto fail;
 
-		if (unlikely(desc.flags & VRING_DESC_F_INDIRECT)) {
+		if (unlikely(desc.flags &
+			     cpu_to_vringh16(vrh, VRING_DESC_F_INDIRECT))) {
+			u64 a = vringh64_to_cpu(vrh, desc.addr);
+
 			/* Make sure it's OK, and get offset. */
-			len = desc.len;
-			if (!rcheck(vrh, desc.addr, &len, &range, getrange)) {
+			len = vringh32_to_cpu(vrh, desc.len);
+			if (!rcheck(vrh, a, &len, &range, getrange)) {
 				err = -EINVAL;
 				goto fail;
 			}
 
-			if (unlikely(len != desc.len)) {
+			if (unlikely(len != vringh32_to_cpu(vrh, desc.len))) {
 				slow = true;
 				/* We need to save this range to use offset */
 				slowrange = range;
 			}
 
-			addr = (void *)(long)(desc.addr + range.offset);
-			err = move_to_indirect(&up_next, &i, addr, &desc,
+			addr = (void *)(long)(a + range.offset);
+			err = move_to_indirect(vrh, &up_next, &i, addr, &desc,
 					       &descs, &desc_max);
 			if (err)
 				goto fail;
@@ -315,7 +324,7 @@ __vringh_iov(struct vringh *vrh, u16 i,
 			goto fail;
 		}
 
-		if (desc.flags & VRING_DESC_F_WRITE)
+		if (desc.flags & cpu_to_vringh16(vrh, VRING_DESC_F_WRITE))
 			iov = wiov;
 		else {
 			iov = riov;
@@ -336,12 +345,14 @@ __vringh_iov(struct vringh *vrh, u16 i,
 
 	again:
 		/* Make sure it's OK, and get offset. */
-		len = desc.len;
-		if (!rcheck(vrh, desc.addr, &len, &range, getrange)) {
+		len = vringh32_to_cpu(vrh, desc.len);
+		if (!rcheck(vrh, vringh64_to_cpu(vrh, desc.addr), &len, &range,
+			    getrange)) {
 			err = -EINVAL;
 			goto fail;
 		}
-		addr = (void *)(unsigned long)(desc.addr + range.offset);
+		addr = (void *)(unsigned long)(vringh64_to_cpu(vrh, desc.addr) +
+					       range.offset);
 
 		if (unlikely(iov->used == (iov->max_num & ~VRINGH_IOV_ALLOCATED))) {
 			err = resize_iovec(iov, gfp);
@@ -353,14 +364,16 @@ __vringh_iov(struct vringh *vrh, u16 i,
 		iov->iov[iov->used].iov_len = len;
 		iov->used++;
 
-		if (unlikely(len != desc.len)) {
-			desc.len -= len;
-			desc.addr += len;
+		if (unlikely(len != vringh32_to_cpu(vrh, desc.len))) {
+			desc.len = cpu_to_vringh32(vrh,
+				   vringh32_to_cpu(vrh, desc.len) - len);
+			desc.addr = cpu_to_vringh64(vrh,
+				    vringh64_to_cpu(vrh, desc.addr) + len);
 			goto again;
 		}
 
-		if (desc.flags & VRING_DESC_F_NEXT) {
-			i = desc.next;
+		if (desc.flags & cpu_to_vringh16(vrh, VRING_DESC_F_NEXT)) {
+			i = vringh16_to_cpu(vrh, desc.next);
 		} else {
 			/* Just in case we need to finish traversing above. */
 			if (unlikely(up_next > 0)) {
@@ -387,7 +400,8 @@ fail:
 static inline int __vringh_complete(struct vringh *vrh,
 				    const struct vring_used_elem *used,
 				    unsigned int num_used,
-				    int (*putu16)(u16 *p, u16 val),
+				    int (*putu16)(const struct vringh *vrh,
+						  __virtio16 *p, u16 val),
 				    int (*putused)(struct vring_used_elem *dst,
 						   const struct vring_used_elem
 						   *src, unsigned num))
@@ -420,7 +434,7 @@ static inline int __vringh_complete(struct vringh *vrh,
 	/* Make sure buffer is written before we update index. */
 	virtio_wmb(vrh->weak_barriers);
 
-	err = putu16(&vrh->vring.used->idx, used_idx + num_used);
+	err = putu16(vrh, &vrh->vring.used->idx, used_idx + num_used);
 	if (err) {
 		vringh_bad("Failed to update used index at %p",
 			   &vrh->vring.used->idx);
@@ -433,7 +447,9 @@ static inline int __vringh_complete(struct vringh *vrh,
 
 
 static inline int __vringh_need_notify(struct vringh *vrh,
-				       int (*getu16)(u16 *val, const u16 *p))
+				       int (*getu16)(const struct vringh *vrh,
+						     u16 *val,
+						     const __virtio16 *p))
 {
 	bool notify;
 	u16 used_event;
@@ -447,7 +463,7 @@ static inline int __vringh_need_notify(struct vringh *vrh,
 	/* Old-style, without event indices. */
 	if (!vrh->event_indices) {
 		u16 flags;
-		err = getu16(&flags, &vrh->vring.avail->flags);
+		err = getu16(vrh, &flags, &vrh->vring.avail->flags);
 		if (err) {
 			vringh_bad("Failed to get flags at %p",
 				   &vrh->vring.avail->flags);
@@ -457,7 +473,7 @@ static inline int __vringh_need_notify(struct vringh *vrh,
 	}
 
 	/* Modern: we know when other side wants to know. */
-	err = getu16(&used_event, &vring_used_event(&vrh->vring));
+	err = getu16(vrh, &used_event, &vring_used_event(&vrh->vring));
 	if (err) {
 		vringh_bad("Failed to get used event idx at %p",
 			   &vring_used_event(&vrh->vring));
@@ -478,20 +494,22 @@ static inline int __vringh_need_notify(struct vringh *vrh,
 }
 
 static inline bool __vringh_notify_enable(struct vringh *vrh,
-					  int (*getu16)(u16 *val, const u16 *p),
-					  int (*putu16)(u16 *p, u16 val))
+					  int (*getu16)(const struct vringh *vrh,
+							u16 *val, const __virtio16 *p),
+					  int (*putu16)(const struct vringh *vrh,
+							__virtio16 *p, u16 val))
 {
 	u16 avail;
 
 	if (!vrh->event_indices) {
 		/* Old-school; update flags. */
-		if (putu16(&vrh->vring.used->flags, 0) != 0) {
+		if (putu16(vrh, &vrh->vring.used->flags, 0) != 0) {
 			vringh_bad("Clearing used flags %p",
 				   &vrh->vring.used->flags);
 			return true;
 		}
 	} else {
-		if (putu16(&vring_avail_event(&vrh->vring),
+		if (putu16(vrh, &vring_avail_event(&vrh->vring),
 			   vrh->last_avail_idx) != 0) {
 			vringh_bad("Updating avail event index %p",
 				   &vring_avail_event(&vrh->vring));
@@ -503,7 +521,7 @@ static inline bool __vringh_notify_enable(struct vringh *vrh,
 	 * sure it's written, then check again. */
 	virtio_mb(vrh->weak_barriers);
 
-	if (getu16(&avail, &vrh->vring.avail->idx) != 0) {
+	if (getu16(vrh, &avail, &vrh->vring.avail->idx) != 0) {
 		vringh_bad("Failed to check avail idx at %p",
 			   &vrh->vring.avail->idx);
 		return true;
@@ -516,11 +534,13 @@ static inline bool __vringh_notify_enable(struct vringh *vrh,
 }
 
 static inline void __vringh_notify_disable(struct vringh *vrh,
-					   int (*putu16)(u16 *p, u16 val))
+					   int (*putu16)(const struct vringh *vrh,
+							 __virtio16 *p, u16 val))
 {
 	if (!vrh->event_indices) {
 		/* Old-school; update flags. */
-		if (putu16(&vrh->vring.used->flags, VRING_USED_F_NO_NOTIFY)) {
+		if (putu16(vrh, &vrh->vring.used->flags,
+			   VRING_USED_F_NO_NOTIFY)) {
 			vringh_bad("Setting used flags %p",
 				   &vrh->vring.used->flags);
 		}
@@ -528,14 +548,18 @@ static inline void __vringh_notify_disable(struct vringh *vrh,
 }
 
 /* Userspace access helpers: in this case, addresses are really userspace. */
-static inline int getu16_user(u16 *val, const u16 *p)
+static inline int getu16_user(const struct vringh *vrh, u16 *val, const __virtio16 *p)
 {
-	return get_user(*val, (__force u16 __user *)p);
+	__virtio16 v = 0;
+	int rc = get_user(v, (__force __virtio16 __user *)p);
+	*val = vringh16_to_cpu(vrh, v);
+	return rc;
 }
 
-static inline int putu16_user(u16 *p, u16 val)
+static inline int putu16_user(const struct vringh *vrh, __virtio16 *p, u16 val)
 {
-	return put_user(val, (__force u16 __user *)p);
+	__virtio16 v = cpu_to_vringh16(vrh, val);
+	return put_user(v, (__force __virtio16 __user *)p);
 }
 
 static inline int copydesc_user(void *dst, const void *src, size_t len)
@@ -577,7 +601,7 @@ static inline int xfer_to_user(void *dst, void *src, size_t len)
  * Returns an error if num is invalid: you should check pointers
  * yourself!
  */
-int vringh_init_user(struct vringh *vrh, u32 features,
+int vringh_init_user(struct vringh *vrh, u64 features,
 		     unsigned int num, bool weak_barriers,
 		     struct vring_desc __user *desc,
 		     struct vring_avail __user *avail,
@@ -589,6 +613,7 @@ int vringh_init_user(struct vringh *vrh, u32 features,
 		return -EINVAL;
 	}
 
+	vrh->little_endian = (features & (1ULL << VIRTIO_F_VERSION_1));
 	vrh->event_indices = (features & (1 << VIRTIO_RING_F_EVENT_IDX));
 	vrh->weak_barriers = weak_barriers;
 	vrh->completed = 0;
@@ -729,8 +754,8 @@ int vringh_complete_user(struct vringh *vrh, u16 head, u32 len)
 {
 	struct vring_used_elem used;
 
-	used.id = head;
-	used.len = len;
+	used.id = cpu_to_vringh32(vrh, head);
+	used.len = cpu_to_vringh32(vrh, len);
 	return __vringh_complete(vrh, &used, 1, putu16_user, putused_user);
 }
 EXPORT_SYMBOL(vringh_complete_user);
@@ -792,15 +817,16 @@ int vringh_need_notify_user(struct vringh *vrh)
 EXPORT_SYMBOL(vringh_need_notify_user);
 
 /* Kernelspace access helpers. */
-static inline int getu16_kern(u16 *val, const u16 *p)
+static inline int getu16_kern(const struct vringh *vrh,
+			      u16 *val, const __virtio16 *p)
 {
-	*val = ACCESS_ONCE(*p);
+	*val = vringh16_to_cpu(vrh, ACCESS_ONCE(*p));
 	return 0;
 }
 
-static inline int putu16_kern(u16 *p, u16 val)
+static inline int putu16_kern(const struct vringh *vrh, __virtio16 *p, u16 val)
 {
-	ACCESS_ONCE(*p) = val;
+	ACCESS_ONCE(*p) = cpu_to_vringh16(vrh, val);
 	return 0;
 }
 
@@ -836,7 +862,7 @@ static inline int xfer_kern(void *src, void *dst, size_t len)
  *
  * Returns an error if num is invalid.
  */
-int vringh_init_kern(struct vringh *vrh, u32 features,
+int vringh_init_kern(struct vringh *vrh, u64 features,
 		     unsigned int num, bool weak_barriers,
 		     struct vring_desc *desc,
 		     struct vring_avail *avail,
@@ -848,6 +874,7 @@ int vringh_init_kern(struct vringh *vrh, u32 features,
 		return -EINVAL;
 	}
 
+	vrh->little_endian = (features & (1ULL << VIRTIO_F_VERSION_1));
 	vrh->event_indices = (features & (1 << VIRTIO_RING_F_EVENT_IDX));
 	vrh->weak_barriers = weak_barriers;
 	vrh->completed = 0;
@@ -962,8 +989,8 @@ int vringh_complete_kern(struct vringh *vrh, u16 head, u32 len)
 {
 	struct vring_used_elem used;
 
-	used.id = head;
-	used.len = len;
+	used.id = cpu_to_vringh32(vrh, head);
+	used.len = cpu_to_vringh32(vrh, len);
 
 	return __vringh_complete(vrh, &used, 1, putu16_kern, putused_kern);
 }
diff --git a/drivers/video/fbdev/amifb.c b/drivers/video/fbdev/amifb.c
index 518f790ef88a..35f7900a0573 100644
--- a/drivers/video/fbdev/amifb.c
+++ b/drivers/video/fbdev/amifb.c
@@ -3782,7 +3782,6 @@ static struct platform_driver amifb_driver = {
 	.remove = __exit_p(amifb_remove),
 	.driver   = {
 		.name	= "amiga-video",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/video/fbdev/atmel_lcdfb.c b/drivers/video/fbdev/atmel_lcdfb.c
index 9ec81d46fc57..94a8d04e60f9 100644
--- a/drivers/video/fbdev/atmel_lcdfb.c
+++ b/drivers/video/fbdev/atmel_lcdfb.c
@@ -1465,7 +1465,6 @@ static struct platform_driver atmel_lcdfb_driver = {
 	.id_table	= atmel_lcdfb_devtypes,
 	.driver		= {
 		.name	= "atmel_lcdfb",
-		.owner	= THIS_MODULE,
 		.of_match_table	= of_match_ptr(atmel_lcdfb_dt_ids),
 	},
 };
diff --git a/drivers/video/fbdev/au1100fb.c b/drivers/video/fbdev/au1100fb.c
index 0676746ec68c..59560189b24a 100644
--- a/drivers/video/fbdev/au1100fb.c
+++ b/drivers/video/fbdev/au1100fb.c
@@ -638,7 +638,6 @@ int au1100fb_drv_resume(struct platform_device *dev)
 static struct platform_driver au1100fb_driver = {
 	.driver = {
 		.name		= "au1100-lcd",
-		.owner          = THIS_MODULE,
 	},
 	.probe		= au1100fb_drv_probe,
         .remove		= au1100fb_drv_remove,
diff --git a/drivers/video/fbdev/au1200fb.c b/drivers/video/fbdev/au1200fb.c
index 18600d4e1b3f..f9507b1894df 100644
--- a/drivers/video/fbdev/au1200fb.c
+++ b/drivers/video/fbdev/au1200fb.c
@@ -1841,7 +1841,6 @@ static const struct dev_pm_ops au1200fb_pmops = {
 static struct platform_driver au1200fb_driver = {
 	.driver = {
 		.name	= "au1200-lcd",
-		.owner	= THIS_MODULE,
 		.pm	= AU1200FB_PMOPS,
 	},
 	.probe		= au1200fb_drv_probe,
diff --git a/drivers/video/fbdev/auo_k1900fb.c b/drivers/video/fbdev/auo_k1900fb.c
index f5b668e77af3..7637c60eae3d 100644
--- a/drivers/video/fbdev/auo_k1900fb.c
+++ b/drivers/video/fbdev/auo_k1900fb.c
@@ -193,7 +193,6 @@ static struct platform_driver auok1900fb_driver = {
 	.probe	= auok1900fb_probe,
 	.remove = auok1900fb_remove,
 	.driver	= {
-		.owner	= THIS_MODULE,
 		.name	= "auo_k1900fb",
 		.pm = &auok190x_pm,
 	},
diff --git a/drivers/video/fbdev/auo_k1901fb.c b/drivers/video/fbdev/auo_k1901fb.c
index 12b9adcb75c5..681fe61957b6 100644
--- a/drivers/video/fbdev/auo_k1901fb.c
+++ b/drivers/video/fbdev/auo_k1901fb.c
@@ -246,7 +246,6 @@ static struct platform_driver auok1901fb_driver = {
 	.probe	= auok1901fb_probe,
 	.remove = auok1901fb_remove,
 	.driver	= {
-		.owner	= THIS_MODULE,
 		.name	= "auo_k1901fb",
 		.pm = &auok190x_pm,
 	},
diff --git a/drivers/video/fbdev/bf537-lq035.c b/drivers/video/fbdev/bf537-lq035.c
index a82d2578d976..7db3052b471d 100644
--- a/drivers/video/fbdev/bf537-lq035.c
+++ b/drivers/video/fbdev/bf537-lq035.c
@@ -894,7 +894,6 @@ static struct platform_driver bfin_lq035_driver = {
 	.resume = bfin_lq035_resume,
 	.driver = {
 		.name = KBUILD_MODNAME,
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/video/fbdev/bf54x-lq043fb.c b/drivers/video/fbdev/bf54x-lq043fb.c
index adbef542c998..8f1f97c75619 100644
--- a/drivers/video/fbdev/bf54x-lq043fb.c
+++ b/drivers/video/fbdev/bf54x-lq043fb.c
@@ -756,7 +756,6 @@ static struct platform_driver bfin_bf54x_driver = {
 	.resume = bfin_bf54x_resume,
 	.driver = {
 		   .name = DRIVER_NAME,
-		   .owner = THIS_MODULE,
 		   },
 };
 module_platform_driver(bfin_bf54x_driver);
diff --git a/drivers/video/fbdev/bfin-t350mcqb-fb.c b/drivers/video/fbdev/bfin-t350mcqb-fb.c
index b5cf1307a3d9..e5ee4d9677f7 100644
--- a/drivers/video/fbdev/bfin-t350mcqb-fb.c
+++ b/drivers/video/fbdev/bfin-t350mcqb-fb.c
@@ -661,7 +661,6 @@ static struct platform_driver bfin_t350mcqb_driver = {
 	.resume = bfin_t350mcqb_resume,
 	.driver = {
 		   .name = DRIVER_NAME,
-		   .owner = THIS_MODULE,
 		   },
 };
 module_platform_driver(bfin_t350mcqb_driver);
diff --git a/drivers/video/fbdev/broadsheetfb.c b/drivers/video/fbdev/broadsheetfb.c
index 8556264b16b7..1c29bd19e3d5 100644
--- a/drivers/video/fbdev/broadsheetfb.c
+++ b/drivers/video/fbdev/broadsheetfb.c
@@ -1212,7 +1212,6 @@ static struct platform_driver broadsheetfb_driver = {
 	.probe	= broadsheetfb_probe,
 	.remove = broadsheetfb_remove,
 	.driver	= {
-		.owner	= THIS_MODULE,
 		.name	= "broadsheetfb",
 	},
 };
diff --git a/drivers/video/fbdev/bw2.c b/drivers/video/fbdev/bw2.c
index bc123d6947a4..8c5b281f0b29 100644
--- a/drivers/video/fbdev/bw2.c
+++ b/drivers/video/fbdev/bw2.c
@@ -377,7 +377,6 @@ MODULE_DEVICE_TABLE(of, bw2_match);
 static struct platform_driver bw2_driver = {
 	.driver = {
 		.name = "bw2",
-		.owner = THIS_MODULE,
 		.of_match_table = bw2_match,
 	},
 	.probe		= bw2_probe,
diff --git a/drivers/video/fbdev/cg14.c b/drivers/video/fbdev/cg14.c
index c79745b136bb..43e915eaf606 100644
--- a/drivers/video/fbdev/cg14.c
+++ b/drivers/video/fbdev/cg14.c
@@ -597,7 +597,6 @@ MODULE_DEVICE_TABLE(of, cg14_match);
 static struct platform_driver cg14_driver = {
 	.driver = {
 		.name = "cg14",
-		.owner = THIS_MODULE,
 		.of_match_table = cg14_match,
 	},
 	.probe		= cg14_probe,
diff --git a/drivers/video/fbdev/cg3.c b/drivers/video/fbdev/cg3.c
index 64a89d5747ed..716391f22e75 100644
--- a/drivers/video/fbdev/cg3.c
+++ b/drivers/video/fbdev/cg3.c
@@ -463,7 +463,6 @@ MODULE_DEVICE_TABLE(of, cg3_match);
 static struct platform_driver cg3_driver = {
 	.driver = {
 		.name = "cg3",
-		.owner = THIS_MODULE,
 		.of_match_table = cg3_match,
 	},
 	.probe		= cg3_probe,
diff --git a/drivers/video/fbdev/cg6.c b/drivers/video/fbdev/cg6.c
index 70781fea092a..bdf901ed5291 100644
--- a/drivers/video/fbdev/cg6.c
+++ b/drivers/video/fbdev/cg6.c
@@ -856,7 +856,6 @@ MODULE_DEVICE_TABLE(of, cg6_match);
 static struct platform_driver cg6_driver = {
 	.driver = {
 		.name = "cg6",
-		.owner = THIS_MODULE,
 		.of_match_table = cg6_match,
 	},
 	.probe		= cg6_probe,
diff --git a/drivers/video/fbdev/clps711x-fb.c b/drivers/video/fbdev/clps711x-fb.c
index 49a7bb4ef02f..649b32f78c08 100644
--- a/drivers/video/fbdev/clps711x-fb.c
+++ b/drivers/video/fbdev/clps711x-fb.c
@@ -384,7 +384,6 @@ MODULE_DEVICE_TABLE(of, clps711x_fb_dt_ids);
 static struct platform_driver clps711x_fb_driver = {
 	.driver	= {
 		.name		= CLPS711X_FB_NAME,
-		.owner		= THIS_MODULE,
 		.of_match_table	= clps711x_fb_dt_ids,
 	},
 	.probe	= clps711x_fb_probe,
diff --git a/drivers/video/fbdev/clps711xfb.c b/drivers/video/fbdev/clps711xfb.c
index f00980607b8f..7693aea8fb23 100644
--- a/drivers/video/fbdev/clps711xfb.c
+++ b/drivers/video/fbdev/clps711xfb.c
@@ -303,7 +303,6 @@ static int clps711x_fb_remove(struct platform_device *pdev)
 static struct platform_driver clps711x_fb_driver = {
 	.driver	= {
 		.name	= "video-clps711x",
-		.owner	= THIS_MODULE,
 	},
 	.probe	= clps711x_fb_probe,
 	.remove	= clps711x_fb_remove,
diff --git a/drivers/video/fbdev/cobalt_lcdfb.c b/drivers/video/fbdev/cobalt_lcdfb.c
index d5533f4db1cf..07675d6f323e 100644
--- a/drivers/video/fbdev/cobalt_lcdfb.c
+++ b/drivers/video/fbdev/cobalt_lcdfb.c
@@ -391,7 +391,6 @@ static struct platform_driver cobalt_lcdfb_driver = {
 	.remove	= cobalt_lcdfb_remove,
 	.driver	= {
 		.name	= "cobalt-lcd",
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(cobalt_lcdfb_driver);
diff --git a/drivers/video/fbdev/da8xx-fb.c b/drivers/video/fbdev/da8xx-fb.c
index 10c876c95772..0081725c6b5b 100644
--- a/drivers/video/fbdev/da8xx-fb.c
+++ b/drivers/video/fbdev/da8xx-fb.c
@@ -1659,7 +1659,6 @@ static struct platform_driver da8xx_fb_driver = {
 	.remove = fb_remove,
 	.driver = {
 		   .name = DRIVER_NAME,
-		   .owner = THIS_MODULE,
 		   .pm	= &fb_pm_ops,
 		   },
 };
diff --git a/drivers/video/fbdev/efifb.c b/drivers/video/fbdev/efifb.c
index 982f6abe6faf..4bfff349b1fb 100644
--- a/drivers/video/fbdev/efifb.c
+++ b/drivers/video/fbdev/efifb.c
@@ -311,7 +311,6 @@ static int efifb_remove(struct platform_device *pdev)
 static struct platform_driver efifb_driver = {
 	.driver = {
 		.name = "efi-framebuffer",
-		.owner = THIS_MODULE,
 	},
 	.probe = efifb_probe,
 	.remove = efifb_remove,
diff --git a/drivers/video/fbdev/ep93xx-fb.c b/drivers/video/fbdev/ep93xx-fb.c
index 35a0f533f1a2..7ec251cc9c03 100644
--- a/drivers/video/fbdev/ep93xx-fb.c
+++ b/drivers/video/fbdev/ep93xx-fb.c
@@ -622,7 +622,6 @@ static struct platform_driver ep93xxfb_driver = {
 	.remove		= ep93xxfb_remove,
 	.driver = {
 		.name	= "ep93xx-fb",
-		.owner	= THIS_MODULE,
 	},
 };
 module_platform_driver(ep93xxfb_driver);
diff --git a/drivers/video/fbdev/exynos/exynos_mipi_dsi.c b/drivers/video/fbdev/exynos/exynos_mipi_dsi.c
index cee9602f9a7b..b527fe464628 100644
--- a/drivers/video/fbdev/exynos/exynos_mipi_dsi.c
+++ b/drivers/video/fbdev/exynos/exynos_mipi_dsi.c
@@ -562,7 +562,6 @@ static struct platform_driver exynos_mipi_dsi_driver = {
 	.remove = exynos_mipi_dsi_remove,
 	.driver = {
 		   .name = "exynos-mipi-dsim",
-		   .owner = THIS_MODULE,
 		   .pm = &exynos_mipi_dsi_pm_ops,
 	},
 };
@@ -570,5 +569,5 @@ static struct platform_driver exynos_mipi_dsi_driver = {
 module_platform_driver(exynos_mipi_dsi_driver);
 
 MODULE_AUTHOR("InKi Dae <inki.dae@samsung.com>");
-MODULE_DESCRIPTION("Samusung SoC MIPI-DSI driver");
+MODULE_DESCRIPTION("Samsung SoC MIPI-DSI driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/exynos/exynos_mipi_dsi_common.c b/drivers/video/fbdev/exynos/exynos_mipi_dsi_common.c
index 85edabfdef5a..2358a2fbbbcd 100644
--- a/drivers/video/fbdev/exynos/exynos_mipi_dsi_common.c
+++ b/drivers/video/fbdev/exynos/exynos_mipi_dsi_common.c
@@ -876,5 +876,5 @@ int exynos_mipi_dsi_fifo_clear(struct mipi_dsim_device *dsim,
 }
 
 MODULE_AUTHOR("InKi Dae <inki.dae@samsung.com>");
-MODULE_DESCRIPTION("Samusung SoC MIPI-DSI common driver");
+MODULE_DESCRIPTION("Samsung SoC MIPI-DSI common driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/ffb.c b/drivers/video/fbdev/ffb.c
index 4c4ffa61ae26..dda31e0a45af 100644
--- a/drivers/video/fbdev/ffb.c
+++ b/drivers/video/fbdev/ffb.c
@@ -1052,7 +1052,6 @@ MODULE_DEVICE_TABLE(of, ffb_match);
 static struct platform_driver ffb_driver = {
 	.driver = {
 		.name = "ffb",
-		.owner = THIS_MODULE,
 		.of_match_table = ffb_match,
 	},
 	.probe		= ffb_probe,
diff --git a/drivers/video/fbdev/fsl-diu-fb.c b/drivers/video/fbdev/fsl-diu-fb.c
index e8758b9c3bcc..7fa2e6f9e322 100644
--- a/drivers/video/fbdev/fsl-diu-fb.c
+++ b/drivers/video/fbdev/fsl-diu-fb.c
@@ -1881,7 +1881,6 @@ MODULE_DEVICE_TABLE(of, fsl_diu_match);
 static struct platform_driver fsl_diu_driver = {
 	.driver = {
 		.name = "fsl-diu-fb",
-		.owner = THIS_MODULE,
 		.of_match_table = fsl_diu_match,
 	},
 	.probe  	= fsl_diu_probe,
diff --git a/drivers/video/fbdev/grvga.c b/drivers/video/fbdev/grvga.c
index 2db5bb1a33e8..b471f92969b1 100644
--- a/drivers/video/fbdev/grvga.c
+++ b/drivers/video/fbdev/grvga.c
@@ -549,7 +549,6 @@ MODULE_DEVICE_TABLE(of, svgactrl_of_match);
 static struct platform_driver grvga_driver = {
 	.driver = {
 		.name = "grlib-svgactrl",
-		.owner = THIS_MODULE,
 		.of_match_table = svgactrl_of_match,
 	},
 	.probe		= grvga_probe,
diff --git a/drivers/video/fbdev/hecubafb.c b/drivers/video/fbdev/hecubafb.c
index f64120ec9192..e4031ef39491 100644
--- a/drivers/video/fbdev/hecubafb.c
+++ b/drivers/video/fbdev/hecubafb.c
@@ -300,7 +300,6 @@ static struct platform_driver hecubafb_driver = {
 	.probe	= hecubafb_probe,
 	.remove = hecubafb_remove,
 	.driver	= {
-		.owner	= THIS_MODULE,
 		.name	= "hecubafb",
 	},
 };
diff --git a/drivers/video/fbdev/hitfb.c b/drivers/video/fbdev/hitfb.c
index a648d5186c6e..9d68dc9ee7bf 100644
--- a/drivers/video/fbdev/hitfb.c
+++ b/drivers/video/fbdev/hitfb.c
@@ -464,7 +464,6 @@ static struct platform_driver hitfb_driver = {
 	.remove		= hitfb_remove,
 	.driver		= {
 		.name	= "hitfb",
-		.owner	= THIS_MODULE,
 		.pm	= &hitfb_dev_pm_ops,
 	},
 };
diff --git a/drivers/video/fbdev/imxfb.c b/drivers/video/fbdev/imxfb.c
index f6e621684953..3b6a3c8c36e2 100644
--- a/drivers/video/fbdev/imxfb.c
+++ b/drivers/video/fbdev/imxfb.c
@@ -1061,7 +1061,6 @@ static struct platform_driver imxfb_driver = {
 	.driver		= {
 		.name	= DRIVER_NAME,
 		.of_match_table = imxfb_of_dev_id,
-		.owner	= THIS_MODULE,
 		.pm	= &imxfb_pm_ops,
 	},
 	.probe		= imxfb_probe,
diff --git a/drivers/video/fbdev/leo.c b/drivers/video/fbdev/leo.c
index 2c7f7d479fe2..62e59dc90ee6 100644
--- a/drivers/video/fbdev/leo.c
+++ b/drivers/video/fbdev/leo.c
@@ -662,7 +662,6 @@ MODULE_DEVICE_TABLE(of, leo_match);
 static struct platform_driver leo_driver = {
 	.driver = {
 		.name = "leo",
-		.owner = THIS_MODULE,
 		.of_match_table = leo_match,
 	},
 	.probe		= leo_probe,
diff --git a/drivers/video/fbdev/mb862xx/mb862xxfbdrv.c b/drivers/video/fbdev/mb862xx/mb862xxfbdrv.c
index 0cd4c3318511..9b8bebdf8f86 100644
--- a/drivers/video/fbdev/mb862xx/mb862xxfbdrv.c
+++ b/drivers/video/fbdev/mb862xx/mb862xxfbdrv.c
@@ -835,7 +835,6 @@ static struct of_device_id of_platform_mb862xx_tbl[] = {
 static struct platform_driver of_platform_mb862xxfb_driver = {
 	.driver = {
 		.name = DRV_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = of_platform_mb862xx_tbl,
 	},
 	.probe		= of_platform_mb862xx_probe,
diff --git a/drivers/video/fbdev/metronomefb.c b/drivers/video/fbdev/metronomefb.c
index 195cc2db4c2c..ad04a01e2761 100644
--- a/drivers/video/fbdev/metronomefb.c
+++ b/drivers/video/fbdev/metronomefb.c
@@ -766,7 +766,6 @@ static struct platform_driver metronomefb_driver = {
 	.probe	= metronomefb_probe,
 	.remove = metronomefb_remove,
 	.driver	= {
-		.owner	= THIS_MODULE,
 		.name	= "metronomefb",
 	},
 };
diff --git a/drivers/video/fbdev/mmp/fb/mmpfb.c b/drivers/video/fbdev/mmp/fb/mmpfb.c
index 910fcc6ecece..92279e02dd94 100644
--- a/drivers/video/fbdev/mmp/fb/mmpfb.c
+++ b/drivers/video/fbdev/mmp/fb/mmpfb.c
@@ -673,7 +673,6 @@ failed_destroy_mutex:
 static struct platform_driver mmpfb_driver = {
 	.driver		= {
 		.name	= "mmp-fb",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= mmpfb_probe,
 };
diff --git a/drivers/video/fbdev/mmp/hw/mmp_ctrl.c b/drivers/video/fbdev/mmp/hw/mmp_ctrl.c
index 3c12bd83b561..de54a4748065 100644
--- a/drivers/video/fbdev/mmp/hw/mmp_ctrl.c
+++ b/drivers/video/fbdev/mmp/hw/mmp_ctrl.c
@@ -571,7 +571,6 @@ failed:
 static struct platform_driver mmphw_driver = {
 	.driver		= {
 		.name	= "mmp-disp",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= mmphw_probe,
 };
diff --git a/drivers/video/fbdev/mx3fb.c b/drivers/video/fbdev/mx3fb.c
index f23fca0be9d7..7947634ee6b0 100644
--- a/drivers/video/fbdev/mx3fb.c
+++ b/drivers/video/fbdev/mx3fb.c
@@ -1645,7 +1645,6 @@ static int mx3fb_remove(struct platform_device *dev)
 static struct platform_driver mx3fb_driver = {
 	.driver = {
 		.name = MX3FB_NAME,
-		.owner = THIS_MODULE,
 	},
 	.probe = mx3fb_probe,
 	.remove = mx3fb_remove,
diff --git a/drivers/video/fbdev/nuc900fb.c b/drivers/video/fbdev/nuc900fb.c
index 478f9808dee4..389fa2cbb713 100644
--- a/drivers/video/fbdev/nuc900fb.c
+++ b/drivers/video/fbdev/nuc900fb.c
@@ -755,7 +755,6 @@ static struct platform_driver nuc900fb_driver = {
 	.resume		= nuc900fb_resume,
 	.driver		= {
 		.name	= "nuc900-lcd",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/video/fbdev/omap/lcd_ams_delta.c b/drivers/video/fbdev/omap/lcd_ams_delta.c
index 4a5f2cd3d3bf..f912a207b394 100644
--- a/drivers/video/fbdev/omap/lcd_ams_delta.c
+++ b/drivers/video/fbdev/omap/lcd_ams_delta.c
@@ -218,7 +218,6 @@ static struct platform_driver ams_delta_panel_driver = {
 	.resume		= ams_delta_panel_resume,
 	.driver		= {
 		.name	= "lcd_ams_delta",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/video/fbdev/omap/lcd_h3.c b/drivers/video/fbdev/omap/lcd_h3.c
index 49bdeca81e50..a0729d0200d0 100644
--- a/drivers/video/fbdev/omap/lcd_h3.c
+++ b/drivers/video/fbdev/omap/lcd_h3.c
@@ -120,7 +120,6 @@ static struct platform_driver h3_panel_driver = {
 	.resume		= h3_panel_resume,
 	.driver		= {
 		.name	= "lcd_h3",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/video/fbdev/omap/lcd_htcherald.c b/drivers/video/fbdev/omap/lcd_htcherald.c
index 20f477851d54..8b4dfa058258 100644
--- a/drivers/video/fbdev/omap/lcd_htcherald.c
+++ b/drivers/video/fbdev/omap/lcd_htcherald.c
@@ -111,7 +111,6 @@ static struct platform_driver htcherald_panel_driver = {
 	.resume		= htcherald_panel_resume,
 	.driver		= {
 		.name	= "lcd_htcherald",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/video/fbdev/omap/lcd_inn1510.c b/drivers/video/fbdev/omap/lcd_inn1510.c
index 2ee423279e35..49907fab36ac 100644
--- a/drivers/video/fbdev/omap/lcd_inn1510.c
+++ b/drivers/video/fbdev/omap/lcd_inn1510.c
@@ -106,7 +106,6 @@ static struct platform_driver innovator1510_panel_driver = {
 	.resume		= innovator1510_panel_resume,
 	.driver		= {
 		.name	= "lcd_inn1510",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/video/fbdev/omap/lcd_inn1610.c b/drivers/video/fbdev/omap/lcd_inn1610.c
index e3d3d135aa48..8b42894eeb77 100644
--- a/drivers/video/fbdev/omap/lcd_inn1610.c
+++ b/drivers/video/fbdev/omap/lcd_inn1610.c
@@ -127,7 +127,6 @@ static struct platform_driver innovator1610_panel_driver = {
 	.resume		= innovator1610_panel_resume,
 	.driver		= {
 		.name	= "lcd_inn1610",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/video/fbdev/omap/lcd_osk.c b/drivers/video/fbdev/omap/lcd_osk.c
index 7fbe04bce0ed..c3ddebf934b2 100644
--- a/drivers/video/fbdev/omap/lcd_osk.c
+++ b/drivers/video/fbdev/omap/lcd_osk.c
@@ -126,7 +126,6 @@ static struct platform_driver osk_panel_driver = {
 	.resume		= osk_panel_resume,
 	.driver		= {
 		.name	= "lcd_osk",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/video/fbdev/omap/lcd_palmte.c b/drivers/video/fbdev/omap/lcd_palmte.c
index ff4fb624b904..2713fed286f7 100644
--- a/drivers/video/fbdev/omap/lcd_palmte.c
+++ b/drivers/video/fbdev/omap/lcd_palmte.c
@@ -103,7 +103,6 @@ static struct platform_driver palmte_panel_driver = {
 	.resume		= palmte_panel_resume,
 	.driver		= {
 		.name	= "lcd_palmte",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/video/fbdev/omap/lcd_palmtt.c b/drivers/video/fbdev/omap/lcd_palmtt.c
index aaf3c8ba1243..3d0ea04ec248 100644
--- a/drivers/video/fbdev/omap/lcd_palmtt.c
+++ b/drivers/video/fbdev/omap/lcd_palmtt.c
@@ -109,7 +109,6 @@ static struct platform_driver palmtt_panel_driver = {
 	.resume		= palmtt_panel_resume,
 	.driver		= {
 		.name	= "lcd_palmtt",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/video/fbdev/omap/lcd_palmz71.c b/drivers/video/fbdev/omap/lcd_palmz71.c
index 3b7d8aa1cf34..a20db4f7ea99 100644
--- a/drivers/video/fbdev/omap/lcd_palmz71.c
+++ b/drivers/video/fbdev/omap/lcd_palmz71.c
@@ -105,7 +105,6 @@ static struct platform_driver palmz71_panel_driver = {
 	.resume		= palmz71_panel_resume,
 	.driver		= {
 		.name	= "lcd_palmz71",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/video/fbdev/omap/omapfb_main.c b/drivers/video/fbdev/omap/omapfb_main.c
index d8d028d98711..1fb3ea3c98a1 100644
--- a/drivers/video/fbdev/omap/omapfb_main.c
+++ b/drivers/video/fbdev/omap/omapfb_main.c
@@ -1869,7 +1869,6 @@ static struct platform_driver omapfb_driver = {
 	.resume		= omapfb_resume,
 	.driver		= {
 		.name	= MODULE_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/video/fbdev/omap2/displays-new/connector-analog-tv.c b/drivers/video/fbdev/omap2/displays-new/connector-analog-tv.c
index 91921665b98b..9a2b5ce58545 100644
--- a/drivers/video/fbdev/omap2/displays-new/connector-analog-tv.c
+++ b/drivers/video/fbdev/omap2/displays-new/connector-analog-tv.c
@@ -308,7 +308,6 @@ static struct platform_driver tvc_connector_driver = {
 	.remove	= __exit_p(tvc_remove),
 	.driver	= {
 		.name	= "connector-analog-tv",
-		.owner	= THIS_MODULE,
 		.of_match_table = tvc_of_match,
 		.suppress_bind_attrs = true,
 	},
diff --git a/drivers/video/fbdev/omap2/displays-new/connector-dvi.c b/drivers/video/fbdev/omap2/displays-new/connector-dvi.c
index 3d38e478bc64..0cdc97413020 100644
--- a/drivers/video/fbdev/omap2/displays-new/connector-dvi.c
+++ b/drivers/video/fbdev/omap2/displays-new/connector-dvi.c
@@ -386,7 +386,6 @@ static struct platform_driver dvi_connector_driver = {
 	.remove	= __exit_p(dvic_remove),
 	.driver	= {
 		.name	= "connector-dvi",
-		.owner	= THIS_MODULE,
 		.of_match_table = dvic_of_match,
 		.suppress_bind_attrs = true,
 	},
diff --git a/drivers/video/fbdev/omap2/displays-new/connector-hdmi.c b/drivers/video/fbdev/omap2/displays-new/connector-hdmi.c
index 219f14f59672..6ee4129bc0c0 100644
--- a/drivers/video/fbdev/omap2/displays-new/connector-hdmi.c
+++ b/drivers/video/fbdev/omap2/displays-new/connector-hdmi.c
@@ -336,7 +336,6 @@ static struct platform_driver hdmi_connector_driver = {
 	.remove	= __exit_p(hdmic_remove),
 	.driver	= {
 		.name	= "connector-hdmi",
-		.owner	= THIS_MODULE,
 		.of_match_table = hdmic_of_match,
 		.suppress_bind_attrs = true,
 	},
diff --git a/drivers/video/fbdev/omap2/displays-new/encoder-tfp410.c b/drivers/video/fbdev/omap2/displays-new/encoder-tfp410.c
index e349064ed615..92919a74e715 100644
--- a/drivers/video/fbdev/omap2/displays-new/encoder-tfp410.c
+++ b/drivers/video/fbdev/omap2/displays-new/encoder-tfp410.c
@@ -297,7 +297,6 @@ static struct platform_driver tfp410_driver = {
 	.remove	= __exit_p(tfp410_remove),
 	.driver	= {
 		.name	= "tfp410",
-		.owner	= THIS_MODULE,
 		.of_match_table = tfp410_of_match,
 		.suppress_bind_attrs = true,
 	},
diff --git a/drivers/video/fbdev/omap2/displays-new/encoder-tpd12s015.c b/drivers/video/fbdev/omap2/displays-new/encoder-tpd12s015.c
index c7a3ce2c5120..7f3e11b16c86 100644
--- a/drivers/video/fbdev/omap2/displays-new/encoder-tpd12s015.c
+++ b/drivers/video/fbdev/omap2/displays-new/encoder-tpd12s015.c
@@ -404,7 +404,6 @@ static struct platform_driver tpd_driver = {
 	.remove	= __exit_p(tpd_remove),
 	.driver	= {
 		.name	= "tpd12s015",
-		.owner	= THIS_MODULE,
 		.of_match_table = tpd_of_match,
 		.suppress_bind_attrs = true,
 	},
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-dpi.c b/drivers/video/fbdev/omap2/displays-new/panel-dpi.c
index a9c3dcf0f6b5..eb8fd8140ad0 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-dpi.c
+++ b/drivers/video/fbdev/omap2/displays-new/panel-dpi.c
@@ -325,7 +325,6 @@ static struct platform_driver panel_dpi_driver = {
 	.remove = __exit_p(panel_dpi_remove),
 	.driver = {
 		.name = "panel-dpi",
-		.owner = THIS_MODULE,
 		.of_match_table = panel_dpi_of_match,
 		.suppress_bind_attrs = true,
 	},
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-dsi-cm.c b/drivers/video/fbdev/omap2/displays-new/panel-dsi-cm.c
index 899cb1ab523d..3414c2609320 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-dsi-cm.c
+++ b/drivers/video/fbdev/omap2/displays-new/panel-dsi-cm.c
@@ -1376,7 +1376,6 @@ static struct platform_driver dsicm_driver = {
 	.remove = __exit_p(dsicm_remove),
 	.driver = {
 		.name = "panel-dsi-cm",
-		.owner = THIS_MODULE,
 		.of_match_table = dsicm_of_match,
 		.suppress_bind_attrs = true,
 	},
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-sharp-ls037v7dw01.c b/drivers/video/fbdev/omap2/displays-new/panel-sharp-ls037v7dw01.c
index 234142cc3764..18b19b6e1ac2 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-sharp-ls037v7dw01.c
+++ b/drivers/video/fbdev/omap2/displays-new/panel-sharp-ls037v7dw01.c
@@ -408,7 +408,6 @@ static struct platform_driver sharp_ls_driver = {
 	.remove = __exit_p(sharp_ls_remove),
 	.driver = {
 		.name = "panel-sharp-ls037v7dw01",
-		.owner = THIS_MODULE,
 		.of_match_table = sharp_ls_of_match,
 		.suppress_bind_attrs = true,
 	},
diff --git a/drivers/video/fbdev/omap2/dss/core.c b/drivers/video/fbdev/omap2/dss/core.c
index 6b74f73fb524..d5d92124e019 100644
--- a/drivers/video/fbdev/omap2/dss/core.c
+++ b/drivers/video/fbdev/omap2/dss/core.c
@@ -244,7 +244,6 @@ static struct platform_driver omap_dss_driver = {
 	.shutdown	= omap_dss_shutdown,
 	.driver         = {
 		.name   = "omapdss",
-		.owner  = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/video/fbdev/omap2/dss/dispc.c b/drivers/video/fbdev/omap2/dss/dispc.c
index 0729c08ac75a..9850d9ef9a9d 100644
--- a/drivers/video/fbdev/omap2/dss/dispc.c
+++ b/drivers/video/fbdev/omap2/dss/dispc.c
@@ -3839,7 +3839,6 @@ static struct platform_driver omap_dispchw_driver = {
 	.remove         = __exit_p(omap_dispchw_remove),
 	.driver         = {
 		.name   = "omapdss_dispc",
-		.owner  = THIS_MODULE,
 		.pm	= &dispc_pm_ops,
 		.of_match_table = dispc_of_match,
 		.suppress_bind_attrs = true,
diff --git a/drivers/video/fbdev/omap2/dss/dpi.c b/drivers/video/fbdev/omap2/dss/dpi.c
index 2edf5caa002f..9a2f8c3b102d 100644
--- a/drivers/video/fbdev/omap2/dss/dpi.c
+++ b/drivers/video/fbdev/omap2/dss/dpi.c
@@ -788,7 +788,6 @@ static struct platform_driver omap_dpi_driver = {
 	.remove         = __exit_p(omap_dpi_remove),
 	.driver         = {
 		.name   = "omapdss_dpi",
-		.owner  = THIS_MODULE,
 		.suppress_bind_attrs = true,
 	},
 };
diff --git a/drivers/video/fbdev/omap2/dss/dsi.c b/drivers/video/fbdev/omap2/dss/dsi.c
index 73af35159468..3e44c580b1f8 100644
--- a/drivers/video/fbdev/omap2/dss/dsi.c
+++ b/drivers/video/fbdev/omap2/dss/dsi.c
@@ -5572,7 +5572,6 @@ static struct platform_driver omap_dsihw_driver = {
 	.remove         = __exit_p(omap_dsihw_remove),
 	.driver         = {
 		.name   = "omapdss_dsi",
-		.owner  = THIS_MODULE,
 		.pm	= &dsi_pm_ops,
 		.of_match_table = dsi_of_match,
 		.suppress_bind_attrs = true,
diff --git a/drivers/video/fbdev/omap2/dss/dss.c b/drivers/video/fbdev/omap2/dss/dss.c
index 702c495083ed..9987154d50b4 100644
--- a/drivers/video/fbdev/omap2/dss/dss.c
+++ b/drivers/video/fbdev/omap2/dss/dss.c
@@ -1012,7 +1012,6 @@ static struct platform_driver omap_dsshw_driver = {
 	.remove         = __exit_p(omap_dsshw_remove),
 	.driver         = {
 		.name   = "omapdss_dss",
-		.owner  = THIS_MODULE,
 		.pm	= &dss_pm_ops,
 		.of_match_table = dss_of_match,
 		.suppress_bind_attrs = true,
diff --git a/drivers/video/fbdev/omap2/dss/hdmi4.c b/drivers/video/fbdev/omap2/dss/hdmi4.c
index f1a02bf938ee..916d47978f41 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi4.c
+++ b/drivers/video/fbdev/omap2/dss/hdmi4.c
@@ -760,7 +760,6 @@ static struct platform_driver omapdss_hdmihw_driver = {
 	.remove         = __exit_p(omapdss_hdmihw_remove),
 	.driver         = {
 		.name   = "omapdss_hdmi",
-		.owner  = THIS_MODULE,
 		.pm	= &hdmi_pm_ops,
 		.of_match_table = hdmi_of_match,
 		.suppress_bind_attrs = true,
diff --git a/drivers/video/fbdev/omap2/dss/hdmi5.c b/drivers/video/fbdev/omap2/dss/hdmi5.c
index d9d0d781625a..39aae3aa7136 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi5.c
+++ b/drivers/video/fbdev/omap2/dss/hdmi5.c
@@ -795,7 +795,6 @@ static struct platform_driver omapdss_hdmihw_driver = {
 	.remove         = __exit_p(omapdss_hdmihw_remove),
 	.driver         = {
 		.name   = "omapdss_hdmi5",
-		.owner  = THIS_MODULE,
 		.pm	= &hdmi_pm_ops,
 		.of_match_table = hdmi_of_match,
 		.suppress_bind_attrs = true,
diff --git a/drivers/video/fbdev/omap2/dss/rfbi.c b/drivers/video/fbdev/omap2/dss/rfbi.c
index 878273f58839..28e694b11ff9 100644
--- a/drivers/video/fbdev/omap2/dss/rfbi.c
+++ b/drivers/video/fbdev/omap2/dss/rfbi.c
@@ -1042,7 +1042,6 @@ static struct platform_driver omap_rfbihw_driver = {
 	.remove         = __exit_p(omap_rfbihw_remove),
 	.driver         = {
 		.name   = "omapdss_rfbi",
-		.owner  = THIS_MODULE,
 		.pm	= &rfbi_pm_ops,
 		.suppress_bind_attrs = true,
 	},
diff --git a/drivers/video/fbdev/omap2/dss/sdi.c b/drivers/video/fbdev/omap2/dss/sdi.c
index d9b10f27be20..d51a983075bc 100644
--- a/drivers/video/fbdev/omap2/dss/sdi.c
+++ b/drivers/video/fbdev/omap2/dss/sdi.c
@@ -376,7 +376,6 @@ static struct platform_driver omap_sdi_driver = {
 	.remove         = __exit_p(omap_sdi_remove),
 	.driver         = {
 		.name   = "omapdss_sdi",
-		.owner  = THIS_MODULE,
 		.suppress_bind_attrs = true,
 	},
 };
diff --git a/drivers/video/fbdev/omap2/dss/venc.c b/drivers/video/fbdev/omap2/dss/venc.c
index d077d8a75ddc..ef7fd925e7f2 100644
--- a/drivers/video/fbdev/omap2/dss/venc.c
+++ b/drivers/video/fbdev/omap2/dss/venc.c
@@ -963,7 +963,6 @@ static struct platform_driver omap_venchw_driver = {
 	.remove         = __exit_p(omap_venchw_remove),
 	.driver         = {
 		.name   = "omapdss_venc",
-		.owner  = THIS_MODULE,
 		.pm	= &venc_pm_ops,
 		.of_match_table = venc_of_match,
 		.suppress_bind_attrs = true,
diff --git a/drivers/video/fbdev/omap2/omapfb/omapfb-main.c b/drivers/video/fbdev/omap2/omapfb/omapfb-main.c
index ce8a70570756..22f07f88bc40 100644
--- a/drivers/video/fbdev/omap2/omapfb/omapfb-main.c
+++ b/drivers/video/fbdev/omap2/omapfb/omapfb-main.c
@@ -2638,7 +2638,6 @@ static struct platform_driver omapfb_driver = {
 	.remove         = omapfb_remove,
 	.driver         = {
 		.name   = "omapfb",
-		.owner  = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/video/fbdev/p9100.c b/drivers/video/fbdev/p9100.c
index 367cea8f43f3..1f6ee76af878 100644
--- a/drivers/video/fbdev/p9100.c
+++ b/drivers/video/fbdev/p9100.c
@@ -353,7 +353,6 @@ MODULE_DEVICE_TABLE(of, p9100_match);
 static struct platform_driver p9100_driver = {
 	.driver = {
 		.name = "p9100",
-		.owner = THIS_MODULE,
 		.of_match_table = p9100_match,
 	},
 	.probe		= p9100_probe,
diff --git a/drivers/video/fbdev/platinumfb.c b/drivers/video/fbdev/platinumfb.c
index 4c9299576827..518d1fd38a81 100644
--- a/drivers/video/fbdev/platinumfb.c
+++ b/drivers/video/fbdev/platinumfb.c
@@ -679,7 +679,6 @@ static struct platform_driver platinum_driver =
 {
 	.driver = {
 		.name = "platinumfb",
-		.owner = THIS_MODULE,
 		.of_match_table = platinumfb_match,
 	},
 	.probe		= platinumfb_probe,
diff --git a/drivers/video/fbdev/pxa168fb.c b/drivers/video/fbdev/pxa168fb.c
index c95b9e46d48f..e209b039f553 100644
--- a/drivers/video/fbdev/pxa168fb.c
+++ b/drivers/video/fbdev/pxa168fb.c
@@ -823,7 +823,6 @@ static int pxa168fb_remove(struct platform_device *pdev)
 static struct platform_driver pxa168fb_driver = {
 	.driver		= {
 		.name	= "pxa168-fb",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= pxa168fb_probe,
 	.remove		= pxa168fb_remove,
diff --git a/drivers/video/fbdev/pxa3xx-gcu.c b/drivers/video/fbdev/pxa3xx-gcu.c
index 4df3657fe221..86bd457d039d 100644
--- a/drivers/video/fbdev/pxa3xx-gcu.c
+++ b/drivers/video/fbdev/pxa3xx-gcu.c
@@ -707,7 +707,6 @@ static struct platform_driver pxa3xx_gcu_driver = {
 	.probe	  = pxa3xx_gcu_probe,
 	.remove	 = pxa3xx_gcu_remove,
 	.driver	 = {
-		.owner  = THIS_MODULE,
 		.name   = DRV_NAME,
 	},
 };
diff --git a/drivers/video/fbdev/pxafb.c b/drivers/video/fbdev/pxafb.c
index a5acca88fa63..da2431eda2fd 100644
--- a/drivers/video/fbdev/pxafb.c
+++ b/drivers/video/fbdev/pxafb.c
@@ -2304,7 +2304,6 @@ static struct platform_driver pxafb_driver = {
 	.probe		= pxafb_probe,
 	.remove 	= pxafb_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "pxa2xx-fb",
 #ifdef CONFIG_PM
 		.pm	= &pxafb_pm_ops,
diff --git a/drivers/video/fbdev/s3c-fb.c b/drivers/video/fbdev/s3c-fb.c
index b33abb0a433d..7e3a05fc47aa 100644
--- a/drivers/video/fbdev/s3c-fb.c
+++ b/drivers/video/fbdev/s3c-fb.c
@@ -1630,7 +1630,7 @@ static int s3c_fb_resume(struct device *dev)
 }
 #endif
 
-#ifdef CONFIG_PM_RUNTIME
+#ifdef CONFIG_PM
 static int s3c_fb_runtime_suspend(struct device *dev)
 {
 	struct s3c_fb *sfb = dev_get_drvdata(dev);
@@ -1971,7 +1971,6 @@ static struct platform_driver s3c_fb_driver = {
 	.id_table	= s3c_fb_driver_ids,
 	.driver		= {
 		.name	= "s3c-fb",
-		.owner	= THIS_MODULE,
 		.pm	= &s3cfb_pm_ops,
 	},
 };
diff --git a/drivers/video/fbdev/s3c2410fb.c b/drivers/video/fbdev/s3c2410fb.c
index e350eb57f11d..d6704add1601 100644
--- a/drivers/video/fbdev/s3c2410fb.c
+++ b/drivers/video/fbdev/s3c2410fb.c
@@ -1104,7 +1104,6 @@ static struct platform_driver s3c2410fb_driver = {
 	.resume		= s3c2410fb_resume,
 	.driver		= {
 		.name	= "s3c2410-lcd",
-		.owner	= THIS_MODULE,
 	},
 };
 
@@ -1115,7 +1114,6 @@ static struct platform_driver s3c2412fb_driver = {
 	.resume		= s3c2410fb_resume,
 	.driver		= {
 		.name	= "s3c2412-lcd",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/video/fbdev/sa1100fb.c b/drivers/video/fbdev/sa1100fb.c
index 9690216d38ff..89dd7e02197f 100644
--- a/drivers/video/fbdev/sa1100fb.c
+++ b/drivers/video/fbdev/sa1100fb.c
@@ -178,6 +178,7 @@
 #include <linux/dma-mapping.h>
 #include <linux/mutex.h>
 #include <linux/io.h>
+#include <linux/clk.h>
 
 #include <video/sa1100fb.h>
 
@@ -416,9 +417,9 @@ sa1100fb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 		var->transp.offset);
 
 #ifdef CONFIG_CPU_FREQ
-	dev_dbg(fbi->dev, "dma period = %d ps, clock = %d kHz\n",
+	dev_dbg(fbi->dev, "dma period = %d ps, clock = %ld kHz\n",
 		sa1100fb_display_dma_period(var),
-		cpufreq_get(smp_processor_id()));
+		clk_get_rate(fbi->clk) / 1000);
 #endif
 
 	return 0;
@@ -592,9 +593,10 @@ static struct fb_ops sa1100fb_ops = {
  * Calculate the PCD value from the clock rate (in picoseconds).
  * We take account of the PPCR clock setting.
  */
-static inline unsigned int get_pcd(unsigned int pixclock, unsigned int cpuclock)
+static inline unsigned int get_pcd(struct sa1100fb_info *fbi,
+		unsigned int pixclock)
 {
-	unsigned int pcd = cpuclock / 100;
+	unsigned int pcd = clk_get_rate(fbi->clk) / 100 / 1000;
 
 	pcd *= pixclock;
 	pcd /= 10000000;
@@ -673,7 +675,7 @@ static int sa1100fb_activate_var(struct fb_var_screeninfo *var, struct sa1100fb_
 		LCCR2_BegFrmDel(var->upper_margin) +
 		LCCR2_EndFrmDel(var->lower_margin);
 
-	pcd = get_pcd(var->pixclock, cpufreq_get(0));
+	pcd = get_pcd(fbi, var->pixclock);
 	new_regs.lccr3 = LCCR3_PixClkDiv(pcd) | fbi->inf->lccr3 |
 		(var->sync & FB_SYNC_HOR_HIGH_ACT ? LCCR3_HorSnchH : LCCR3_HorSnchL) |
 		(var->sync & FB_SYNC_VERT_HIGH_ACT ? LCCR3_VrtSnchH : LCCR3_VrtSnchL);
@@ -787,6 +789,9 @@ static void sa1100fb_enable_controller(struct sa1100fb_info *fbi)
 	fbi->palette_cpu[0] &= 0xcfff;
 	fbi->palette_cpu[0] |= palette_pbs(&fbi->fb.var);
 
+	/* enable LCD controller clock */
+	clk_prepare_enable(fbi->clk);
+
 	/* Sequence from 11.7.10 */
 	writel_relaxed(fbi->reg_lccr3, fbi->base + LCCR3);
 	writel_relaxed(fbi->reg_lccr2, fbi->base + LCCR2);
@@ -831,6 +836,9 @@ static void sa1100fb_disable_controller(struct sa1100fb_info *fbi)
 
 	schedule_timeout(20 * HZ / 1000);
 	remove_wait_queue(&fbi->ctrlr_wait, &wait);
+
+	/* disable LCD controller clock */
+	clk_disable_unprepare(fbi->clk);
 }
 
 /*
@@ -1009,7 +1017,6 @@ sa1100fb_freq_transition(struct notifier_block *nb, unsigned long val,
 			 void *data)
 {
 	struct sa1100fb_info *fbi = TO_INF(nb, freq_transition);
-	struct cpufreq_freqs *f = data;
 	u_int pcd;
 
 	switch (val) {
@@ -1018,7 +1025,7 @@ sa1100fb_freq_transition(struct notifier_block *nb, unsigned long val,
 		break;
 
 	case CPUFREQ_POSTCHANGE:
-		pcd = get_pcd(fbi->fb.var.pixclock, f->new);
+		pcd = get_pcd(fbi, fbi->fb.var.pixclock);
 		fbi->reg_lccr3 = (fbi->reg_lccr3 & ~0xff) | LCCR3_PixClkDiv(pcd);
 		set_ctrlr_state(fbi, C_ENABLE_CLKCHANGE);
 		break;
@@ -1225,6 +1232,13 @@ static int sa1100fb_probe(struct platform_device *pdev)
 	if (!fbi)
 		goto failed;
 
+	fbi->clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(fbi->clk)) {
+		ret = PTR_ERR(fbi->clk);
+		fbi->clk = NULL;
+		goto failed;
+	}
+
 	fbi->base = ioremap(res->start, resource_size(res));
 	if (!fbi->base)
 		goto failed;
@@ -1277,6 +1291,8 @@ static int sa1100fb_probe(struct platform_device *pdev)
  failed:
 	if (fbi)
 		iounmap(fbi->base);
+	if (fbi->clk)
+		clk_put(fbi->clk);
 	kfree(fbi);
 	release_mem_region(res->start, resource_size(res));
 	return ret;
@@ -1288,7 +1304,6 @@ static struct platform_driver sa1100fb_driver = {
 	.resume		= sa1100fb_resume,
 	.driver		= {
 		.name	= "sa11x0-fb",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/video/fbdev/sa1100fb.h b/drivers/video/fbdev/sa1100fb.h
index fc5d4292fad6..0139d13377a5 100644
--- a/drivers/video/fbdev/sa1100fb.h
+++ b/drivers/video/fbdev/sa1100fb.h
@@ -68,6 +68,7 @@ struct sa1100fb_info {
 #endif
 
 	const struct sa1100fb_mach_info *inf;
+	struct clk *clk;
 };
 
 #define TO_INF(ptr,member)	container_of(ptr,struct sa1100fb_info,member)
diff --git a/drivers/video/fbdev/sh7760fb.c b/drivers/video/fbdev/sh7760fb.c
index 1265b25f9f99..96de91d76623 100644
--- a/drivers/video/fbdev/sh7760fb.c
+++ b/drivers/video/fbdev/sh7760fb.c
@@ -578,7 +578,6 @@ static int sh7760fb_remove(struct platform_device *dev)
 static struct platform_driver sh7760_lcdc_driver = {
 	.driver = {
 		   .name = "sh7760-lcdc",
-		   .owner = THIS_MODULE,
 		   },
 	.probe = sh7760fb_probe,
 	.remove = sh7760fb_remove,
diff --git a/drivers/video/fbdev/sh_mobile_lcdcfb.c b/drivers/video/fbdev/sh_mobile_lcdcfb.c
index cfde21d81c15..d3013cd9f976 100644
--- a/drivers/video/fbdev/sh_mobile_lcdcfb.c
+++ b/drivers/video/fbdev/sh_mobile_lcdcfb.c
@@ -2848,7 +2848,6 @@ err1:
 static struct platform_driver sh_mobile_lcdc_driver = {
 	.driver		= {
 		.name		= "sh_mobile_lcdc_fb",
-		.owner		= THIS_MODULE,
 		.pm		= &sh_mobile_lcdc_dev_pm_ops,
 	},
 	.probe		= sh_mobile_lcdc_probe,
diff --git a/drivers/video/fbdev/sh_mobile_meram.c b/drivers/video/fbdev/sh_mobile_meram.c
index a297de5cc859..baadfb207b2e 100644
--- a/drivers/video/fbdev/sh_mobile_meram.c
+++ b/drivers/video/fbdev/sh_mobile_meram.c
@@ -569,7 +569,7 @@ EXPORT_SYMBOL_GPL(sh_mobile_meram_cache_update);
  * Power management
  */
 
-#if defined(CONFIG_PM_SLEEP) || defined(CONFIG_PM_RUNTIME)
+#ifdef CONFIG_PM
 static int sh_mobile_meram_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
@@ -612,7 +612,7 @@ static int sh_mobile_meram_resume(struct device *dev)
 		meram_write_reg(priv->base, common_regs[i], priv->regs[i]);
 	return 0;
 }
-#endif /* CONFIG_PM_SLEEP || CONFIG_PM_RUNTIME */
+#endif /* CONFIG_PM */
 
 static UNIVERSAL_DEV_PM_OPS(sh_mobile_meram_dev_pm_ops,
 			    sh_mobile_meram_suspend,
@@ -745,7 +745,6 @@ static int sh_mobile_meram_remove(struct platform_device *pdev)
 static struct platform_driver sh_mobile_meram_driver = {
 	.driver	= {
 		.name		= "sh_mobile_meram",
-		.owner		= THIS_MODULE,
 		.pm		= &sh_mobile_meram_dev_pm_ops,
 	},
 	.probe		= sh_mobile_meram_probe,
diff --git a/drivers/video/fbdev/simplefb.c b/drivers/video/fbdev/simplefb.c
index b2ae9254fd75..92cac803dee3 100644
--- a/drivers/video/fbdev/simplefb.c
+++ b/drivers/video/fbdev/simplefb.c
@@ -387,7 +387,6 @@ MODULE_DEVICE_TABLE(of, simplefb_of_match);
 static struct platform_driver simplefb_driver = {
 	.driver = {
 		.name = "simple-framebuffer",
-		.owner = THIS_MODULE,
 		.of_match_table = simplefb_of_match,
 	},
 	.probe = simplefb_probe,
diff --git a/drivers/video/fbdev/sm501fb.c b/drivers/video/fbdev/sm501fb.c
index 8b98b011fc04..e8d4121783fb 100644
--- a/drivers/video/fbdev/sm501fb.c
+++ b/drivers/video/fbdev/sm501fb.c
@@ -2225,7 +2225,6 @@ static struct platform_driver sm501fb_driver = {
 	.resume		= sm501fb_resume,
 	.driver		= {
 		.name	= "sm501-fb",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/video/fbdev/sunxvr1000.c b/drivers/video/fbdev/sunxvr1000.c
index 58241b47a96d..08879bdfad35 100644
--- a/drivers/video/fbdev/sunxvr1000.c
+++ b/drivers/video/fbdev/sunxvr1000.c
@@ -202,7 +202,6 @@ static struct platform_driver gfb_driver = {
 	.remove		= gfb_remove,
 	.driver = {
 		.name		= "gfb",
-		.owner		= THIS_MODULE,
 		.of_match_table	= gfb_match,
 	},
 };
diff --git a/drivers/video/fbdev/tcx.c b/drivers/video/fbdev/tcx.c
index 7fb2d696fac7..54ad08854c94 100644
--- a/drivers/video/fbdev/tcx.c
+++ b/drivers/video/fbdev/tcx.c
@@ -512,7 +512,6 @@ MODULE_DEVICE_TABLE(of, tcx_match);
 static struct platform_driver tcx_driver = {
 	.driver = {
 		.name = "tcx",
-		.owner = THIS_MODULE,
 		.of_match_table = tcx_match,
 	},
 	.probe		= tcx_probe,
diff --git a/drivers/video/fbdev/vesafb.c b/drivers/video/fbdev/vesafb.c
index 6170e7f58640..d79a0ac49fc7 100644
--- a/drivers/video/fbdev/vesafb.c
+++ b/drivers/video/fbdev/vesafb.c
@@ -512,7 +512,6 @@ static int vesafb_remove(struct platform_device *pdev)
 static struct platform_driver vesafb_driver = {
 	.driver = {
 		.name = "vesa-framebuffer",
-		.owner = THIS_MODULE,
 	},
 	.probe = vesafb_probe,
 	.remove = vesafb_remove,
diff --git a/drivers/video/fbdev/vt8500lcdfb.c b/drivers/video/fbdev/vt8500lcdfb.c
index a1134c3f6c11..ffaf29eeaaba 100644
--- a/drivers/video/fbdev/vt8500lcdfb.c
+++ b/drivers/video/fbdev/vt8500lcdfb.c
@@ -486,7 +486,6 @@ static struct platform_driver vt8500lcd_driver = {
 	.probe		= vt8500lcd_probe,
 	.remove		= vt8500lcd_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "vt8500-lcd",
 		.of_match_table = of_match_ptr(via_dt_ids),
 	},
diff --git a/drivers/video/fbdev/wm8505fb.c b/drivers/video/fbdev/wm8505fb.c
index d2fafbbcd7f8..e925619da39b 100644
--- a/drivers/video/fbdev/wm8505fb.c
+++ b/drivers/video/fbdev/wm8505fb.c
@@ -407,7 +407,6 @@ static struct platform_driver wm8505fb_driver = {
 	.probe		= wm8505fb_probe,
 	.remove		= wm8505fb_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= DRIVER_NAME,
 		.of_match_table = wmt_dt_ids,
 	},
diff --git a/drivers/video/fbdev/wmt_ge_rops.c b/drivers/video/fbdev/wmt_ge_rops.c
index 9df6fe78a44b..89d23844c1db 100644
--- a/drivers/video/fbdev/wmt_ge_rops.c
+++ b/drivers/video/fbdev/wmt_ge_rops.c
@@ -167,7 +167,6 @@ static struct platform_driver wmt_ge_rops_driver = {
 	.probe		= wmt_ge_rops_probe,
 	.remove		= wmt_ge_rops_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "wmt_ge_rops",
 		.of_match_table = wmt_dt_ids,
 	},
diff --git a/drivers/video/fbdev/xilinxfb.c b/drivers/video/fbdev/xilinxfb.c
index 553cff2f3f4c..17dc119c7a98 100644
--- a/drivers/video/fbdev/xilinxfb.c
+++ b/drivers/video/fbdev/xilinxfb.c
@@ -497,7 +497,6 @@ static struct platform_driver xilinxfb_of_driver = {
 	.remove = xilinxfb_of_remove,
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = xilinxfb_of_match,
 	},
 };
diff --git a/drivers/virtio/virtio.c b/drivers/virtio/virtio.c
index f22665868781..b9f70dfc4751 100644
--- a/drivers/virtio/virtio.c
+++ b/drivers/virtio/virtio.c
@@ -162,6 +162,27 @@ static void virtio_config_enable(struct virtio_device *dev)
 	spin_unlock_irq(&dev->config_lock);
 }
 
+static int virtio_finalize_features(struct virtio_device *dev)
+{
+	int ret = dev->config->finalize_features(dev);
+	unsigned status;
+
+	if (ret)
+		return ret;
+
+	if (!virtio_has_feature(dev, VIRTIO_F_VERSION_1))
+		return 0;
+
+	add_status(dev, VIRTIO_CONFIG_S_FEATURES_OK);
+	status = dev->config->get_status(dev);
+	if (!(status & VIRTIO_CONFIG_S_FEATURES_OK)) {
+		dev_err(&dev->dev, "virtio: device refuses features: %x\n",
+			status);
+		return -ENODEV;
+	}
+	return 0;
+}
+
 static int virtio_dev_probe(struct device *_d)
 {
 	int err, i;
@@ -170,7 +191,6 @@ static int virtio_dev_probe(struct device *_d)
 	u64 device_features;
 	u64 driver_features;
 	u64 driver_features_legacy;
-	unsigned status;
 
 	/* We have a driver! */
 	add_status(dev, VIRTIO_CONFIG_S_DRIVER);
@@ -208,21 +228,10 @@ static int virtio_dev_probe(struct device *_d)
 		if (device_features & (1ULL << i))
 			__virtio_set_bit(dev, i);
 
-	err = dev->config->finalize_features(dev);
+	err = virtio_finalize_features(dev);
 	if (err)
 		goto err;
 
-	if (virtio_has_feature(dev, VIRTIO_F_VERSION_1)) {
-		add_status(dev, VIRTIO_CONFIG_S_FEATURES_OK);
-		status = dev->config->get_status(dev);
-		if (!(status & VIRTIO_CONFIG_S_FEATURES_OK)) {
-			dev_err(_d, "virtio: device refuses features: %x\n",
-			       status);
-			err = -ENODEV;
-			goto err;
-		}
-	}
-
 	err = drv->probe(dev);
 	if (err)
 		goto err;
@@ -372,7 +381,7 @@ int virtio_device_restore(struct virtio_device *dev)
 	/* We have a driver! */
 	add_status(dev, VIRTIO_CONFIG_S_DRIVER);
 
-	ret = dev->config->finalize_features(dev);
+	ret = virtio_finalize_features(dev);
 	if (ret)
 		goto err;
 
diff --git a/drivers/virtio/virtio_balloon.c b/drivers/virtio/virtio_balloon.c
index c9703d4d6f67..50c5f42d7a9f 100644
--- a/drivers/virtio/virtio_balloon.c
+++ b/drivers/virtio/virtio_balloon.c
@@ -28,6 +28,7 @@
 #include <linux/slab.h>
 #include <linux/module.h>
 #include <linux/balloon_compaction.h>
+#include <linux/oom.h>
 
 /*
  * Balloon device works in 4K page units.  So each page is pointed to by
@@ -36,6 +37,12 @@
  */
 #define VIRTIO_BALLOON_PAGES_PER_PAGE (unsigned)(PAGE_SIZE >> VIRTIO_BALLOON_PFN_SHIFT)
 #define VIRTIO_BALLOON_ARRAY_PFNS_MAX 256
+#define OOM_VBALLOON_DEFAULT_PAGES 256
+#define VIRTBALLOON_OOM_NOTIFY_PRIORITY 80
+
+static int oom_pages = OOM_VBALLOON_DEFAULT_PAGES;
+module_param(oom_pages, int, S_IRUSR | S_IWUSR);
+MODULE_PARM_DESC(oom_pages, "pages to free on OOM");
 
 struct virtio_balloon
 {
@@ -71,6 +78,9 @@ struct virtio_balloon
 	/* Memory statistics */
 	int need_stats_update;
 	struct virtio_balloon_stat stats[VIRTIO_BALLOON_S_NR];
+
+	/* To register callback in oom notifier call chain */
+	struct notifier_block nb;
 };
 
 static struct virtio_device_id id_table[] = {
@@ -168,8 +178,9 @@ static void release_pages_by_pfn(const u32 pfns[], unsigned int num)
 	}
 }
 
-static void leak_balloon(struct virtio_balloon *vb, size_t num)
+static unsigned leak_balloon(struct virtio_balloon *vb, size_t num)
 {
+	unsigned num_freed_pages;
 	struct page *page;
 	struct balloon_dev_info *vb_dev_info = &vb->vb_dev_info;
 
@@ -186,6 +197,7 @@ static void leak_balloon(struct virtio_balloon *vb, size_t num)
 		vb->num_pages -= VIRTIO_BALLOON_PAGES_PER_PAGE;
 	}
 
+	num_freed_pages = vb->num_pfns;
 	/*
 	 * Note that if
 	 * virtio_has_feature(vdev, VIRTIO_BALLOON_F_MUST_TELL_HOST);
@@ -195,6 +207,7 @@ static void leak_balloon(struct virtio_balloon *vb, size_t num)
 		tell_host(vb, vb->deflate_vq);
 	mutex_unlock(&vb->balloon_lock);
 	release_pages_by_pfn(vb->pfns, vb->num_pfns);
+	return num_freed_pages;
 }
 
 static inline void update_stat(struct virtio_balloon *vb, int idx,
@@ -287,6 +300,38 @@ static void update_balloon_size(struct virtio_balloon *vb)
 		      &actual);
 }
 
+/*
+ * virtballoon_oom_notify - release pages when system is under severe
+ *			    memory pressure (called from out_of_memory())
+ * @self : notifier block struct
+ * @dummy: not used
+ * @parm : returned - number of freed pages
+ *
+ * The balancing of memory by use of the virtio balloon should not cause
+ * the termination of processes while there are pages in the balloon.
+ * If virtio balloon manages to release some memory, it will make the
+ * system return and retry the allocation that forced the OOM killer
+ * to run.
+ */
+static int virtballoon_oom_notify(struct notifier_block *self,
+				  unsigned long dummy, void *parm)
+{
+	struct virtio_balloon *vb;
+	unsigned long *freed;
+	unsigned num_freed_pages;
+
+	vb = container_of(self, struct virtio_balloon, nb);
+	if (!virtio_has_feature(vb->vdev, VIRTIO_BALLOON_F_DEFLATE_ON_OOM))
+		return NOTIFY_OK;
+
+	freed = parm;
+	num_freed_pages = leak_balloon(vb, oom_pages);
+	update_balloon_size(vb);
+	*freed += num_freed_pages;
+
+	return NOTIFY_OK;
+}
+
 static int balloon(void *_vballoon)
 {
 	struct virtio_balloon *vb = _vballoon;
@@ -443,6 +488,12 @@ static int virtballoon_probe(struct virtio_device *vdev)
 	if (err)
 		goto out_free_vb;
 
+	vb->nb.notifier_call = virtballoon_oom_notify;
+	vb->nb.priority = VIRTBALLOON_OOM_NOTIFY_PRIORITY;
+	err = register_oom_notifier(&vb->nb);
+	if (err < 0)
+		goto out_oom_notify;
+
 	vb->thread = kthread_run(balloon, vb, "vballoon");
 	if (IS_ERR(vb->thread)) {
 		err = PTR_ERR(vb->thread);
@@ -452,6 +503,8 @@ static int virtballoon_probe(struct virtio_device *vdev)
 	return 0;
 
 out_del_vqs:
+	unregister_oom_notifier(&vb->nb);
+out_oom_notify:
 	vdev->config->del_vqs(vdev);
 out_free_vb:
 	kfree(vb);
@@ -476,6 +529,7 @@ static void virtballoon_remove(struct virtio_device *vdev)
 {
 	struct virtio_balloon *vb = vdev->priv;
 
+	unregister_oom_notifier(&vb->nb);
 	kthread_stop(vb->thread);
 	remove_common(vb);
 	kfree(vb);
@@ -515,6 +569,7 @@ static int virtballoon_restore(struct virtio_device *vdev)
 static unsigned int features[] = {
 	VIRTIO_BALLOON_F_MUST_TELL_HOST,
 	VIRTIO_BALLOON_F_STATS_VQ,
+	VIRTIO_BALLOON_F_DEFLATE_ON_OOM,
 };
 
 static struct virtio_driver virtio_balloon_driver = {
diff --git a/drivers/virtio/virtio_mmio.c b/drivers/virtio/virtio_mmio.c
index 5219210d31ce..00d115b22bd8 100644
--- a/drivers/virtio/virtio_mmio.c
+++ b/drivers/virtio/virtio_mmio.c
@@ -640,7 +640,6 @@ static struct platform_driver virtio_mmio_driver = {
 	.remove		= virtio_mmio_remove,
 	.driver		= {
 		.name	= "virtio-mmio",
-		.owner	= THIS_MODULE,
 		.of_match_table	= virtio_mmio_match,
 	},
 };
diff --git a/drivers/virtio/virtio_pci_common.c b/drivers/virtio/virtio_pci_common.c
index 953057d84185..2ef9529809d8 100644
--- a/drivers/virtio/virtio_pci_common.c
+++ b/drivers/virtio/virtio_pci_common.c
@@ -458,7 +458,44 @@ static int virtio_pci_restore(struct device *dev)
 	return virtio_device_restore(&vp_dev->vdev);
 }
 
-const struct dev_pm_ops virtio_pci_pm_ops = {
+static const struct dev_pm_ops virtio_pci_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(virtio_pci_freeze, virtio_pci_restore)
 };
 #endif
+
+
+/* Qumranet donated their vendor ID for devices 0x1000 thru 0x10FF. */
+static const struct pci_device_id virtio_pci_id_table[] = {
+	{ PCI_DEVICE(0x1af4, PCI_ANY_ID) },
+	{ 0 }
+};
+
+MODULE_DEVICE_TABLE(pci, virtio_pci_id_table);
+
+static int virtio_pci_probe(struct pci_dev *pci_dev,
+			    const struct pci_device_id *id)
+{
+	return virtio_pci_legacy_probe(pci_dev, id);
+}
+
+static void virtio_pci_remove(struct pci_dev *pci_dev)
+{
+     virtio_pci_legacy_remove(pci_dev);
+}
+
+static struct pci_driver virtio_pci_driver = {
+	.name		= "virtio-pci",
+	.id_table	= virtio_pci_id_table,
+	.probe		= virtio_pci_probe,
+	.remove		= virtio_pci_remove,
+#ifdef CONFIG_PM_SLEEP
+	.driver.pm	= &virtio_pci_pm_ops,
+#endif
+};
+
+module_pci_driver(virtio_pci_driver);
+
+MODULE_AUTHOR("Anthony Liguori <aliguori@us.ibm.com>");
+MODULE_DESCRIPTION("virtio-pci");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("1");
diff --git a/drivers/virtio/virtio_pci_common.h b/drivers/virtio/virtio_pci_common.h
index d840dad4149d..adddb647b21d 100644
--- a/drivers/virtio/virtio_pci_common.h
+++ b/drivers/virtio/virtio_pci_common.h
@@ -27,7 +27,6 @@
 #include <linux/virtio.h>
 #include <linux/virtio_config.h>
 #include <linux/virtio_ring.h>
-#define VIRTIO_PCI_NO_LEGACY
 #include <linux/virtio_pci.h>
 #include <linux/highmem.h>
 #include <linux/spinlock.h>
@@ -129,8 +128,8 @@ const char *vp_bus_name(struct virtio_device *vdev);
 int vp_set_vq_affinity(struct virtqueue *vq, int cpu);
 void virtio_pci_release_dev(struct device *);
 
-#ifdef CONFIG_PM_SLEEP
-extern const struct dev_pm_ops virtio_pci_pm_ops;
-#endif
+int virtio_pci_legacy_probe(struct pci_dev *pci_dev,
+			    const struct pci_device_id *id);
+void virtio_pci_legacy_remove(struct pci_dev *pci_dev);
 
 #endif
diff --git a/drivers/virtio/virtio_pci_legacy.c b/drivers/virtio/virtio_pci_legacy.c
index 2588252e5c1c..6c76f0f5658c 100644
--- a/drivers/virtio/virtio_pci_legacy.c
+++ b/drivers/virtio/virtio_pci_legacy.c
@@ -19,14 +19,6 @@
 
 #include "virtio_pci_common.h"
 
-/* Qumranet donated their vendor ID for devices 0x1000 thru 0x10FF. */
-static const struct pci_device_id virtio_pci_id_table[] = {
-	{ PCI_DEVICE(0x1af4, PCI_ANY_ID) },
-	{ 0 }
-};
-
-MODULE_DEVICE_TABLE(pci, virtio_pci_id_table);
-
 /* virtio config->get_features() implementation */
 static u64 vp_get_features(struct virtio_device *vdev)
 {
@@ -220,7 +212,7 @@ static const struct virtio_config_ops virtio_pci_config_ops = {
 };
 
 /* the PCI probing function */
-static int virtio_pci_probe(struct pci_dev *pci_dev,
+int virtio_pci_legacy_probe(struct pci_dev *pci_dev,
 			    const struct pci_device_id *id)
 {
 	struct virtio_pci_device *vp_dev;
@@ -300,7 +292,7 @@ out:
 	return err;
 }
 
-static void virtio_pci_remove(struct pci_dev *pci_dev)
+void virtio_pci_legacy_remove(struct pci_dev *pci_dev)
 {
 	struct virtio_pci_device *vp_dev = pci_get_drvdata(pci_dev);
 
@@ -312,15 +304,3 @@ static void virtio_pci_remove(struct pci_dev *pci_dev)
 	pci_disable_device(pci_dev);
 	kfree(vp_dev);
 }
-
-static struct pci_driver virtio_pci_driver = {
-	.name		= "virtio-pci",
-	.id_table	= virtio_pci_id_table,
-	.probe		= virtio_pci_probe,
-	.remove		= virtio_pci_remove,
-#ifdef CONFIG_PM_SLEEP
-	.driver.pm	= &virtio_pci_pm_ops,
-#endif
-};
-
-module_pci_driver(virtio_pci_driver);
diff --git a/drivers/w1/masters/ds2490.c b/drivers/w1/masters/ds2490.c
index 1de6df87bfa3..049a884a756f 100644
--- a/drivers/w1/masters/ds2490.c
+++ b/drivers/w1/masters/ds2490.c
@@ -253,7 +253,7 @@ static int ds_recv_status_nodump(struct ds_device *dev, struct ds_status *st,
 
 	count = 0;
 	err = usb_interrupt_msg(dev->udev, usb_rcvintpipe(dev->udev,
-		dev->ep[EP_STATUS]), buf, size, &count, 100);
+		dev->ep[EP_STATUS]), buf, size, &count, 1000);
 	if (err < 0) {
 		pr_err("Failed to read 1-wire data from 0x%x: err=%d.\n",
 		       dev->ep[EP_STATUS], err);
diff --git a/drivers/w1/masters/mxc_w1.c b/drivers/w1/masters/mxc_w1.c
index da3d0f0ad63c..53bf2c860ad3 100644
--- a/drivers/w1/masters/mxc_w1.c
+++ b/drivers/w1/masters/mxc_w1.c
@@ -175,7 +175,6 @@ MODULE_DEVICE_TABLE(of, mxc_w1_dt_ids);
 static struct platform_driver mxc_w1_driver = {
 	.driver = {
 		.name = "mxc_w1",
-		.owner = THIS_MODULE,
 		.of_match_table = mxc_w1_dt_ids,
 	},
 	.probe = mxc_w1_probe,
diff --git a/drivers/w1/masters/w1-gpio.c b/drivers/w1/masters/w1-gpio.c
index 1d111e56c8c8..b99a932ad901 100644
--- a/drivers/w1/masters/w1-gpio.c
+++ b/drivers/w1/masters/w1-gpio.c
@@ -228,7 +228,6 @@ static int w1_gpio_resume(struct platform_device *pdev)
 static struct platform_driver w1_gpio_driver = {
 	.driver = {
 		.name	= "w1-gpio",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(w1_gpio_dt_ids),
 	},
 	.probe = w1_gpio_probe,
diff --git a/drivers/w1/slaves/w1_bq27000.c b/drivers/w1/slaves/w1_bq27000.c
index afbefed5f2c9..caafb1722783 100644
--- a/drivers/w1/slaves/w1_bq27000.c
+++ b/drivers/w1/slaves/w1_bq27000.c
@@ -88,7 +88,7 @@ static struct w1_family_ops w1_bq27000_fops = {
 };
 
 static struct w1_family w1_bq27000_family = {
-	.fid = 1,
+	.fid = W1_FAMILY_BQ27000,
 	.fops = &w1_bq27000_fops,
 };
 
@@ -111,7 +111,7 @@ module_exit(w1_bq27000_exit);
 
 module_param(F_ID, int, S_IRUSR);
 MODULE_PARM_DESC(F_ID, "1-wire slave FID for BQ device");
-
+MODULE_ALIAS("w1-family-" __stringify(W1_FAMILY_BQ27000));
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Texas Instruments Ltd");
 MODULE_DESCRIPTION("HDQ/1-wire slave driver bq27000 battery monitor chip");
diff --git a/drivers/w1/w1.c b/drivers/w1/w1.c
index 592f7edc671e..181f41cb960b 100644
--- a/drivers/w1/w1.c
+++ b/drivers/w1/w1.c
@@ -727,7 +727,7 @@ int w1_attach_slave_device(struct w1_master *dev, struct w1_reg_num *rn)
 
 	/* slave modules need to be loaded in a context with unlocked mutex */
 	mutex_unlock(&dev->mutex);
-	request_module("w1-family-0x%0x", rn->family);
+	request_module("w1-family-0x%02x", rn->family);
 	mutex_lock(&dev->mutex);
 
 	spin_lock(&w1_flock);
diff --git a/drivers/w1/w1_family.h b/drivers/w1/w1_family.h
index 0d18365b61ad..ed5dcb80a1f7 100644
--- a/drivers/w1/w1_family.h
+++ b/drivers/w1/w1_family.h
@@ -27,6 +27,7 @@
 #include <linux/atomic.h>
 
 #define W1_FAMILY_DEFAULT	0
+#define W1_FAMILY_BQ27000	0x01
 #define W1_FAMILY_SMEM_01	0x01
 #define W1_FAMILY_SMEM_81	0x81
 #define W1_THERM_DS18S20 	0x10
diff --git a/drivers/w1/w1_netlink.c b/drivers/w1/w1_netlink.c
index dd9656237274..881597a191b8 100644
--- a/drivers/w1/w1_netlink.c
+++ b/drivers/w1/w1_netlink.c
@@ -598,7 +598,7 @@ static void w1_cn_callback(struct cn_msg *cn, struct netlink_skb_parms *nsp)
 	msg = (struct w1_netlink_msg *)(cn + 1);
 	if (node_count) {
 		int size;
-		u16 reply_size = sizeof(*cn) + cn->len + slave_len;
+		int reply_size = sizeof(*cn) + cn->len + slave_len;
 		if (cn->flags & W1_CN_BUNDLE) {
 			/* bundling duplicats some of the messages */
 			reply_size += 2 * cmd_count * (sizeof(struct cn_msg) +
diff --git a/drivers/watchdog/acquirewdt.c b/drivers/watchdog/acquirewdt.c
index 5614416f1032..d6210d946082 100644
--- a/drivers/watchdog/acquirewdt.c
+++ b/drivers/watchdog/acquirewdt.c
@@ -294,7 +294,6 @@ static struct platform_driver acquirewdt_driver = {
 	.remove		= acq_remove,
 	.shutdown	= acq_shutdown,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= DRV_NAME,
 	},
 };
diff --git a/drivers/watchdog/advantechwdt.c b/drivers/watchdog/advantechwdt.c
index 7796db7fa6e1..7d7db0c5a64e 100644
--- a/drivers/watchdog/advantechwdt.c
+++ b/drivers/watchdog/advantechwdt.c
@@ -302,7 +302,6 @@ static struct platform_driver advwdt_driver = {
 	.remove		= advwdt_remove,
 	.shutdown	= advwdt_shutdown,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= DRV_NAME,
 	},
 };
diff --git a/drivers/watchdog/ar7_wdt.c b/drivers/watchdog/ar7_wdt.c
index ae6c287a49cb..6d5ae251e309 100644
--- a/drivers/watchdog/ar7_wdt.c
+++ b/drivers/watchdog/ar7_wdt.c
@@ -324,7 +324,6 @@ static struct platform_driver ar7_wdt_driver = {
 	.remove = ar7_wdt_remove,
 	.shutdown = ar7_wdt_shutdown,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "ar7_wdt",
 	},
 };
diff --git a/drivers/watchdog/at32ap700x_wdt.c b/drivers/watchdog/at32ap700x_wdt.c
index 25b5c67d3af9..81ba8920f127 100644
--- a/drivers/watchdog/at32ap700x_wdt.c
+++ b/drivers/watchdog/at32ap700x_wdt.c
@@ -422,7 +422,6 @@ static struct platform_driver at32_wdt_driver = {
 	.resume		= at32_wdt_resume,
 	.driver		= {
 		.name	= "at32_wdt",
-		.owner	= THIS_MODULE,
 	},
 	.shutdown	= at32_wdt_shutdown,
 };
diff --git a/drivers/watchdog/at91rm9200_wdt.c b/drivers/watchdog/at91rm9200_wdt.c
index dee6cc21d270..d244112d5b6f 100644
--- a/drivers/watchdog/at91rm9200_wdt.c
+++ b/drivers/watchdog/at91rm9200_wdt.c
@@ -268,7 +268,6 @@ static struct platform_driver at91wdt_driver = {
 	.resume		= at91wdt_resume,
 	.driver		= {
 		.name	= "at91_wdt",
-		.owner	= THIS_MODULE,
 		.of_match_table = at91_wdt_dt_ids,
 	},
 };
diff --git a/drivers/watchdog/at91sam9_wdt.c b/drivers/watchdog/at91sam9_wdt.c
index 489729b26298..6df940528fd2 100644
--- a/drivers/watchdog/at91sam9_wdt.c
+++ b/drivers/watchdog/at91sam9_wdt.c
@@ -393,7 +393,6 @@ static struct platform_driver at91wdt_driver = {
 	.remove		= __exit_p(at91wdt_remove),
 	.driver		= {
 		.name	= "at91_wdt",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(at91_wdt_dt_ids),
 	},
 };
diff --git a/drivers/watchdog/ath79_wdt.c b/drivers/watchdog/ath79_wdt.c
index 41ac4660fb89..835d310081e1 100644
--- a/drivers/watchdog/ath79_wdt.c
+++ b/drivers/watchdog/ath79_wdt.c
@@ -326,7 +326,6 @@ static struct platform_driver ath79_wdt_driver = {
 	.shutdown	= ath97_wdt_shutdown,
 	.driver		= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(ath79_wdt_match),
 	},
 };
diff --git a/drivers/watchdog/bcm2835_wdt.c b/drivers/watchdog/bcm2835_wdt.c
index 8df450c090a9..2b5a9bbf80b7 100644
--- a/drivers/watchdog/bcm2835_wdt.c
+++ b/drivers/watchdog/bcm2835_wdt.c
@@ -167,7 +167,6 @@ static struct platform_driver bcm2835_wdt_driver = {
 	.shutdown	= bcm2835_wdt_shutdown,
 	.driver = {
 		.name =		"bcm2835-wdt",
-		.owner =	THIS_MODULE,
 		.of_match_table = bcm2835_wdt_of_match,
 	},
 };
diff --git a/drivers/watchdog/bcm47xx_wdt.c b/drivers/watchdog/bcm47xx_wdt.c
index b61fcc535979..9816485f6825 100644
--- a/drivers/watchdog/bcm47xx_wdt.c
+++ b/drivers/watchdog/bcm47xx_wdt.c
@@ -242,7 +242,6 @@ static int bcm47xx_wdt_remove(struct platform_device *pdev)
 
 static struct platform_driver bcm47xx_wdt_driver = {
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "bcm47xx-wdt",
 	},
 	.probe		= bcm47xx_wdt_probe,
diff --git a/drivers/watchdog/bcm63xx_wdt.c b/drivers/watchdog/bcm63xx_wdt.c
index 5a8e879a430a..ab26fd90729e 100644
--- a/drivers/watchdog/bcm63xx_wdt.c
+++ b/drivers/watchdog/bcm63xx_wdt.c
@@ -304,7 +304,6 @@ static struct platform_driver bcm63xx_wdt_driver = {
 	.remove = bcm63xx_wdt_remove,
 	.shutdown = bcm63xx_wdt_shutdown,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "bcm63xx-wdt",
 	}
 };
diff --git a/drivers/watchdog/bcm_kona_wdt.c b/drivers/watchdog/bcm_kona_wdt.c
index 9c248099f4a2..4e37db3539a4 100644
--- a/drivers/watchdog/bcm_kona_wdt.c
+++ b/drivers/watchdog/bcm_kona_wdt.c
@@ -352,7 +352,6 @@ MODULE_DEVICE_TABLE(of, bcm_kona_wdt_of_match);
 static struct platform_driver bcm_kona_wdt_driver = {
 	.driver = {
 			.name = BCM_KONA_WDT_NAME,
-			.owner = THIS_MODULE,
 			.of_match_table = bcm_kona_wdt_of_match,
 		  },
 	.probe = bcm_kona_wdt_probe,
diff --git a/drivers/watchdog/bfin_wdt.c b/drivers/watchdog/bfin_wdt.c
index a3b6a5b30f9f..aa4d2e8a8ef9 100644
--- a/drivers/watchdog/bfin_wdt.c
+++ b/drivers/watchdog/bfin_wdt.c
@@ -407,7 +407,6 @@ static struct platform_driver bfin_wdt_driver = {
 	.resume    = bfin_wdt_resume,
 	.driver    = {
 		.name  = WATCHDOG_NAME,
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/watchdog/coh901327_wdt.c b/drivers/watchdog/coh901327_wdt.c
index 4bd070f524e5..ce12f437f195 100644
--- a/drivers/watchdog/coh901327_wdt.c
+++ b/drivers/watchdog/coh901327_wdt.c
@@ -448,7 +448,6 @@ static const struct of_device_id coh901327_dt_match[] = {
 
 static struct platform_driver coh901327_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
 		.name	= "coh901327_wdog",
 		.of_match_table = coh901327_dt_match,
 	},
diff --git a/drivers/watchdog/cpwd.c b/drivers/watchdog/cpwd.c
index e55ed702209f..02007689e9ca 100644
--- a/drivers/watchdog/cpwd.c
+++ b/drivers/watchdog/cpwd.c
@@ -679,7 +679,6 @@ MODULE_DEVICE_TABLE(of, cpwd_match);
 static struct platform_driver cpwd_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = cpwd_match,
 	},
 	.probe		= cpwd_probe,
diff --git a/drivers/watchdog/davinci_wdt.c b/drivers/watchdog/davinci_wdt.c
index d09ad2254b57..cfdf8a408aea 100644
--- a/drivers/watchdog/davinci_wdt.c
+++ b/drivers/watchdog/davinci_wdt.c
@@ -218,7 +218,6 @@ MODULE_DEVICE_TABLE(of, davinci_wdt_of_match);
 static struct platform_driver platform_wdt_driver = {
 	.driver = {
 		.name = "davinci-wdt",
-		.owner	= THIS_MODULE,
 		.of_match_table = davinci_wdt_of_match,
 	},
 	.probe = davinci_wdt_probe,
diff --git a/drivers/watchdog/dw_wdt.c b/drivers/watchdog/dw_wdt.c
index 9e577a64ec9e..b34a2e4e4e43 100644
--- a/drivers/watchdog/dw_wdt.c
+++ b/drivers/watchdog/dw_wdt.c
@@ -382,7 +382,6 @@ static struct platform_driver dw_wdt_driver = {
 	.remove		= dw_wdt_drv_remove,
 	.driver		= {
 		.name	= "dw_wdt",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(dw_wdt_of_match),
 		.pm	= &dw_wdt_pm_ops,
 	},
diff --git a/drivers/watchdog/ep93xx_wdt.c b/drivers/watchdog/ep93xx_wdt.c
index 5f54e1e5819a..7a2cc7191c58 100644
--- a/drivers/watchdog/ep93xx_wdt.c
+++ b/drivers/watchdog/ep93xx_wdt.c
@@ -156,7 +156,6 @@ static int ep93xx_wdt_remove(struct platform_device *pdev)
 
 static struct platform_driver ep93xx_wdt_driver = {
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "ep93xx-wdt",
 	},
 	.probe		= ep93xx_wdt_probe,
diff --git a/drivers/watchdog/gef_wdt.c b/drivers/watchdog/gef_wdt.c
index 25beb30878d7..cc1bdfc2ff71 100644
--- a/drivers/watchdog/gef_wdt.c
+++ b/drivers/watchdog/gef_wdt.c
@@ -307,7 +307,6 @@ static const struct of_device_id gef_wdt_ids[] = {
 static struct platform_driver gef_wdt_driver = {
 	.driver = {
 		.name = "gef_wdt",
-		.owner = THIS_MODULE,
 		.of_match_table = gef_wdt_ids,
 	},
 	.probe		= gef_wdt_probe,
diff --git a/drivers/watchdog/geodewdt.c b/drivers/watchdog/geodewdt.c
index 4c43e3fa8bd2..88e01238f01b 100644
--- a/drivers/watchdog/geodewdt.c
+++ b/drivers/watchdog/geodewdt.c
@@ -258,7 +258,6 @@ static struct platform_driver geodewdt_driver = {
 	.remove		= geodewdt_remove,
 	.shutdown	= geodewdt_shutdown,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= DRV_NAME,
 	},
 };
diff --git a/drivers/watchdog/gpio_wdt.c b/drivers/watchdog/gpio_wdt.c
index 220a9e07cfd5..bbdb19b45332 100644
--- a/drivers/watchdog/gpio_wdt.c
+++ b/drivers/watchdog/gpio_wdt.c
@@ -241,7 +241,6 @@ MODULE_DEVICE_TABLE(of, gpio_wdt_dt_ids);
 static struct platform_driver gpio_wdt_driver = {
 	.driver	= {
 		.name		= "gpio-wdt",
-		.owner		= THIS_MODULE,
 		.of_match_table	= gpio_wdt_dt_ids,
 	},
 	.probe	= gpio_wdt_probe,
diff --git a/drivers/watchdog/iTCO_wdt.c b/drivers/watchdog/iTCO_wdt.c
index 0ba1b7c99760..05ee0bf88ce9 100644
--- a/drivers/watchdog/iTCO_wdt.c
+++ b/drivers/watchdog/iTCO_wdt.c
@@ -576,7 +576,6 @@ static struct platform_driver iTCO_wdt_driver = {
 	.remove         = iTCO_wdt_remove,
 	.shutdown       = iTCO_wdt_shutdown,
 	.driver         = {
-		.owner  = THIS_MODULE,
 		.name   = DRV_NAME,
 	},
 };
diff --git a/drivers/watchdog/ib700wdt.c b/drivers/watchdog/ib700wdt.c
index 4247c498ee78..f2e4e1eeb8dd 100644
--- a/drivers/watchdog/ib700wdt.c
+++ b/drivers/watchdog/ib700wdt.c
@@ -339,7 +339,6 @@ static struct platform_driver ibwdt_driver = {
 	.remove		= ibwdt_remove,
 	.shutdown	= ibwdt_shutdown,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= DRV_NAME,
 	},
 };
diff --git a/drivers/watchdog/ie6xx_wdt.c b/drivers/watchdog/ie6xx_wdt.c
index 07f88f54e5c0..9bc39ae51624 100644
--- a/drivers/watchdog/ie6xx_wdt.c
+++ b/drivers/watchdog/ie6xx_wdt.c
@@ -313,7 +313,6 @@ static struct platform_driver ie6xx_wdt_driver = {
 	.remove		= ie6xx_wdt_remove,
 	.driver		= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/watchdog/imx2_wdt.c b/drivers/watchdog/imx2_wdt.c
index 7e12f88bb4a6..d6add516a7a7 100644
--- a/drivers/watchdog/imx2_wdt.c
+++ b/drivers/watchdog/imx2_wdt.c
@@ -326,6 +326,52 @@ static void imx2_wdt_shutdown(struct platform_device *pdev)
 	}
 }
 
+#ifdef CONFIG_PM_SLEEP
+/* Disable watchdog if it is active during suspend */
+static int imx2_wdt_suspend(struct device *dev)
+{
+	struct watchdog_device *wdog = dev_get_drvdata(dev);
+	struct imx2_wdt_device *wdev = watchdog_get_drvdata(wdog);
+
+	imx2_wdt_set_timeout(wdog, IMX2_WDT_MAX_TIME);
+	imx2_wdt_ping(wdog);
+
+	/* Watchdog has been stopped but IP block is still running */
+	if (!watchdog_active(wdog) && imx2_wdt_is_running(wdev))
+		del_timer_sync(&wdev->timer);
+
+	clk_disable_unprepare(wdev->clk);
+
+	return 0;
+}
+
+/* Enable watchdog and configure it if necessary */
+static int imx2_wdt_resume(struct device *dev)
+{
+	struct watchdog_device *wdog = dev_get_drvdata(dev);
+	struct imx2_wdt_device *wdev = watchdog_get_drvdata(wdog);
+
+	clk_prepare_enable(wdev->clk);
+
+	if (watchdog_active(wdog) && !imx2_wdt_is_running(wdev)) {
+		/* Resumes from deep sleep we need restart
+		 * the watchdog again.
+		 */
+		imx2_wdt_setup(wdog);
+		imx2_wdt_set_timeout(wdog, wdog->timeout);
+		imx2_wdt_ping(wdog);
+	} else if (imx2_wdt_is_running(wdev)) {
+		imx2_wdt_ping(wdog);
+		mod_timer(&wdev->timer, jiffies + wdog->timeout * HZ / 2);
+	}
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(imx2_wdt_pm_ops, imx2_wdt_suspend,
+			 imx2_wdt_resume);
+
 static const struct of_device_id imx2_wdt_dt_ids[] = {
 	{ .compatible = "fsl,imx21-wdt", },
 	{ /* sentinel */ }
@@ -337,7 +383,7 @@ static struct platform_driver imx2_wdt_driver = {
 	.shutdown	= imx2_wdt_shutdown,
 	.driver		= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
+		.pm     = &imx2_wdt_pm_ops,
 		.of_match_table = imx2_wdt_dt_ids,
 	},
 };
diff --git a/drivers/watchdog/intel-mid_wdt.c b/drivers/watchdog/intel-mid_wdt.c
index ca66e8e74635..84f6701c391f 100644
--- a/drivers/watchdog/intel-mid_wdt.c
+++ b/drivers/watchdog/intel-mid_wdt.c
@@ -172,7 +172,6 @@ static struct platform_driver mid_wdt_driver = {
 	.probe		= mid_wdt_probe,
 	.remove		= mid_wdt_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "intel_mid_wdt",
 	},
 };
diff --git a/drivers/watchdog/jz4740_wdt.c b/drivers/watchdog/jz4740_wdt.c
index 91e45ca589e6..18e41afa4da3 100644
--- a/drivers/watchdog/jz4740_wdt.c
+++ b/drivers/watchdog/jz4740_wdt.c
@@ -211,7 +211,6 @@ static struct platform_driver jz4740_wdt_driver = {
 	.remove = jz4740_wdt_remove,
 	.driver = {
 		.name = "jz4740-wdt",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/watchdog/kempld_wdt.c b/drivers/watchdog/kempld_wdt.c
index d9c1a1601926..5bf931ce1353 100644
--- a/drivers/watchdog/kempld_wdt.c
+++ b/drivers/watchdog/kempld_wdt.c
@@ -563,7 +563,6 @@ static int kempld_wdt_resume(struct platform_device *pdev)
 static struct platform_driver kempld_wdt_driver = {
 	.driver		= {
 		.name	= "kempld-wdt",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= kempld_wdt_probe,
 	.remove		= kempld_wdt_remove,
diff --git a/drivers/watchdog/ks8695_wdt.c b/drivers/watchdog/ks8695_wdt.c
index 40ca5594a336..b7ea39b455c8 100644
--- a/drivers/watchdog/ks8695_wdt.c
+++ b/drivers/watchdog/ks8695_wdt.c
@@ -296,7 +296,6 @@ static struct platform_driver ks8695wdt_driver = {
 	.resume		= ks8695wdt_resume,
 	.driver		= {
 		.name	= "ks8695_wdt",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/watchdog/lantiq_wdt.c b/drivers/watchdog/lantiq_wdt.c
index 021e84eb88eb..582f2fa1b8d9 100644
--- a/drivers/watchdog/lantiq_wdt.c
+++ b/drivers/watchdog/lantiq_wdt.c
@@ -232,7 +232,6 @@ static struct platform_driver ltq_wdt_driver = {
 	.remove = ltq_wdt_remove,
 	.driver = {
 		.name = "wdt",
-		.owner = THIS_MODULE,
 		.of_match_table = ltq_wdt_match,
 	},
 };
diff --git a/drivers/watchdog/max63xx_wdt.c b/drivers/watchdog/max63xx_wdt.c
index 0e9cc6f5a919..08da3114accb 100644
--- a/drivers/watchdog/max63xx_wdt.c
+++ b/drivers/watchdog/max63xx_wdt.c
@@ -231,7 +231,6 @@ static struct platform_driver max63xx_wdt_driver = {
 	.id_table	= max63xx_id_table,
 	.driver		= {
 		.name	= "max63xx_wdt",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/watchdog/menf21bmc_wdt.c b/drivers/watchdog/menf21bmc_wdt.c
index 2042874d5ce3..59f0913c7341 100644
--- a/drivers/watchdog/menf21bmc_wdt.c
+++ b/drivers/watchdog/menf21bmc_wdt.c
@@ -187,7 +187,6 @@ static void menf21bmc_wdt_shutdown(struct platform_device *pdev)
 
 static struct  platform_driver menf21bmc_wdt = {
 	.driver		= {
-		.owner = THIS_MODULE,
 		.name	= DEVNAME,
 	},
 	.probe		= menf21bmc_wdt_probe,
diff --git a/drivers/watchdog/moxart_wdt.c b/drivers/watchdog/moxart_wdt.c
index a64405b82596..2789da2c0515 100644
--- a/drivers/watchdog/moxart_wdt.c
+++ b/drivers/watchdog/moxart_wdt.c
@@ -174,7 +174,6 @@ static struct platform_driver moxart_wdt_driver = {
 	.remove     = moxart_wdt_remove,
 	.driver     = {
 		.name		= "moxart-watchdog",
-		.owner		= THIS_MODULE,
 		.of_match_table	= moxart_watchdog_match,
 	},
 };
diff --git a/drivers/watchdog/mpc8xxx_wdt.c b/drivers/watchdog/mpc8xxx_wdt.c
index 7831955cd9e1..689381a24887 100644
--- a/drivers/watchdog/mpc8xxx_wdt.c
+++ b/drivers/watchdog/mpc8xxx_wdt.c
@@ -249,7 +249,6 @@ static struct platform_driver mpc8xxx_wdt_driver = {
 	.remove		= mpc8xxx_wdt_remove,
 	.driver = {
 		.name = "mpc8xxx_wdt",
-		.owner = THIS_MODULE,
 		.of_match_table = mpc8xxx_wdt_match,
 	},
 };
diff --git a/drivers/watchdog/mv64x60_wdt.c b/drivers/watchdog/mv64x60_wdt.c
index f9fa58409396..315275d7bab6 100644
--- a/drivers/watchdog/mv64x60_wdt.c
+++ b/drivers/watchdog/mv64x60_wdt.c
@@ -300,7 +300,6 @@ static struct platform_driver mv64x60_wdt_driver = {
 	.probe = mv64x60_wdt_probe,
 	.remove = mv64x60_wdt_remove,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = MV64x60_WDT_NAME,
 	},
 };
diff --git a/drivers/watchdog/nuc900_wdt.c b/drivers/watchdog/nuc900_wdt.c
index 7135803ca1a3..d5bed78c4d9f 100644
--- a/drivers/watchdog/nuc900_wdt.c
+++ b/drivers/watchdog/nuc900_wdt.c
@@ -297,7 +297,6 @@ static struct platform_driver nuc900wdt_driver = {
 	.remove		= nuc900wdt_remove,
 	.driver		= {
 		.name	= "nuc900-wdt",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/watchdog/nv_tco.c b/drivers/watchdog/nv_tco.c
index 0b9ec61e1313..c028454be66c 100644
--- a/drivers/watchdog/nv_tco.c
+++ b/drivers/watchdog/nv_tco.c
@@ -471,7 +471,6 @@ static struct platform_driver nv_tco_driver = {
 	.remove		= nv_tco_remove,
 	.shutdown	= nv_tco_shutdown,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= TCO_MODULE_NAME,
 	},
 };
diff --git a/drivers/watchdog/omap_wdt.c b/drivers/watchdog/omap_wdt.c
index 3691b157516a..9f2709db61ca 100644
--- a/drivers/watchdog/omap_wdt.c
+++ b/drivers/watchdog/omap_wdt.c
@@ -353,7 +353,6 @@ static struct platform_driver omap_wdt_driver = {
 	.suspend	= omap_wdt_suspend,
 	.resume		= omap_wdt_resume,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "omap_wdt",
 		.of_match_table = omap_wdt_of_match,
 	},
diff --git a/drivers/watchdog/orion_wdt.c b/drivers/watchdog/orion_wdt.c
index 8cb1ff3bcd90..ef0c628d5037 100644
--- a/drivers/watchdog/orion_wdt.c
+++ b/drivers/watchdog/orion_wdt.c
@@ -633,7 +633,6 @@ static struct platform_driver orion_wdt_driver = {
 	.remove		= orion_wdt_remove,
 	.shutdown	= orion_wdt_shutdown,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "orion_wdt",
 		.of_match_table = orion_wdt_of_match_table,
 	},
diff --git a/drivers/watchdog/pnx4008_wdt.c b/drivers/watchdog/pnx4008_wdt.c
index 15fb316e9437..55e220150103 100644
--- a/drivers/watchdog/pnx4008_wdt.c
+++ b/drivers/watchdog/pnx4008_wdt.c
@@ -207,7 +207,6 @@ MODULE_DEVICE_TABLE(of, pnx4008_wdt_match);
 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,
diff --git a/drivers/watchdog/rdc321x_wdt.c b/drivers/watchdog/rdc321x_wdt.c
index 29cf4dcbc59c..47a8f1b1087d 100644
--- a/drivers/watchdog/rdc321x_wdt.c
+++ b/drivers/watchdog/rdc321x_wdt.c
@@ -287,7 +287,6 @@ static struct platform_driver rdc321x_wdt_driver = {
 	.probe = rdc321x_wdt_probe,
 	.remove = rdc321x_wdt_remove,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "rdc321x-wdt",
 	},
 };
diff --git a/drivers/watchdog/riowd.c b/drivers/watchdog/riowd.c
index cfed0fe264dc..aba53424605e 100644
--- a/drivers/watchdog/riowd.c
+++ b/drivers/watchdog/riowd.c
@@ -237,7 +237,6 @@ MODULE_DEVICE_TABLE(of, riowd_match);
 static struct platform_driver riowd_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = riowd_match,
 	},
 	.probe		= riowd_probe,
diff --git a/drivers/watchdog/rt2880_wdt.c b/drivers/watchdog/rt2880_wdt.c
index d92c2d5859ce..11aad5b7aafe 100644
--- a/drivers/watchdog/rt2880_wdt.c
+++ b/drivers/watchdog/rt2880_wdt.c
@@ -194,7 +194,6 @@ static struct platform_driver rt288x_wdt_driver = {
 	.shutdown	= rt288x_wdt_shutdown,
 	.driver		= {
 		.name		= KBUILD_MODNAME,
-		.owner		= THIS_MODULE,
 		.of_match_table	= rt288x_wdt_match,
 	},
 };
diff --git a/drivers/watchdog/s3c2410_wdt.c b/drivers/watchdog/s3c2410_wdt.c
index 1626dc66e763..e89ae027c91d 100644
--- a/drivers/watchdog/s3c2410_wdt.c
+++ b/drivers/watchdog/s3c2410_wdt.c
@@ -741,7 +741,6 @@ static struct platform_driver s3c2410wdt_driver = {
 	.shutdown	= s3c2410wdt_shutdown,
 	.id_table	= s3c2410_wdt_ids,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "s3c2410-wdt",
 		.pm	= &s3c2410wdt_pm_ops,
 		.of_match_table	= of_match_ptr(s3c2410_wdt_match),
diff --git a/drivers/watchdog/sch311x_wdt.c b/drivers/watchdog/sch311x_wdt.c
index b96127ea3de1..43d0cbb7ba0b 100644
--- a/drivers/watchdog/sch311x_wdt.c
+++ b/drivers/watchdog/sch311x_wdt.c
@@ -453,7 +453,6 @@ static struct platform_driver sch311x_wdt_driver = {
 	.remove		= sch311x_wdt_remove,
 	.shutdown	= sch311x_wdt_shutdown,
 	.driver		= {
-		.owner = THIS_MODULE,
 		.name = DRV_NAME,
 	},
 };
diff --git a/drivers/watchdog/shwdt.c b/drivers/watchdog/shwdt.c
index fa89bb30d004..567458b137a6 100644
--- a/drivers/watchdog/shwdt.c
+++ b/drivers/watchdog/shwdt.c
@@ -303,7 +303,6 @@ static void sh_wdt_shutdown(struct platform_device *pdev)
 static struct platform_driver sh_wdt_driver = {
 	.driver		= {
 		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
 	},
 
 	.probe		= sh_wdt_probe,
diff --git a/drivers/watchdog/sirfsoc_wdt.c b/drivers/watchdog/sirfsoc_wdt.c
index 702d07870808..42fa5c0c518a 100644
--- a/drivers/watchdog/sirfsoc_wdt.c
+++ b/drivers/watchdog/sirfsoc_wdt.c
@@ -210,7 +210,6 @@ MODULE_DEVICE_TABLE(of, sirfsoc_wdt_of_match);
 static struct platform_driver sirfsoc_wdt_driver = {
 	.driver = {
 		.name = "sirfsoc-wdt",
-		.owner = THIS_MODULE,
 		.pm = &sirfsoc_wdt_pm_ops,
 		.of_match_table	= sirfsoc_wdt_of_match,
 	},
diff --git a/drivers/watchdog/sp5100_tco.c b/drivers/watchdog/sp5100_tco.c
index 5cca9cddb87d..eb8044ef0ea0 100644
--- a/drivers/watchdog/sp5100_tco.c
+++ b/drivers/watchdog/sp5100_tco.c
@@ -538,7 +538,6 @@ static struct platform_driver sp5100_tco_driver = {
 	.remove		= sp5100_tco_remove,
 	.shutdown	= sp5100_tco_shutdown,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= TCO_MODULE_NAME,
 	},
 };
diff --git a/drivers/watchdog/sunxi_wdt.c b/drivers/watchdog/sunxi_wdt.c
index b62301e74e5f..a29afb37c48c 100644
--- a/drivers/watchdog/sunxi_wdt.c
+++ b/drivers/watchdog/sunxi_wdt.c
@@ -314,7 +314,6 @@ static struct platform_driver sunxi_wdt_driver = {
 	.remove		= sunxi_wdt_remove,
 	.shutdown	= sunxi_wdt_shutdown,
 	.driver		= {
-		.owner		= THIS_MODULE,
 		.name		= DRV_NAME,
 		.of_match_table	= sunxi_wdt_dt_ids,
 	},
diff --git a/drivers/watchdog/tegra_wdt.c b/drivers/watchdog/tegra_wdt.c
index 750e2a26cb12..30451ea46902 100644
--- a/drivers/watchdog/tegra_wdt.c
+++ b/drivers/watchdog/tegra_wdt.c
@@ -289,7 +289,6 @@ static struct platform_driver tegra_wdt_driver = {
 	.probe		= tegra_wdt_probe,
 	.remove		= tegra_wdt_remove,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "tegra-wdt",
 		.pm	= &tegra_wdt_pm_ops,
 		.of_match_table = tegra_wdt_of_match,
diff --git a/drivers/watchdog/ts72xx_wdt.c b/drivers/watchdog/ts72xx_wdt.c
index dee9c6cbe6df..119beb7f6017 100644
--- a/drivers/watchdog/ts72xx_wdt.c
+++ b/drivers/watchdog/ts72xx_wdt.c
@@ -436,7 +436,6 @@ static struct platform_driver ts72xx_wdt_driver = {
 	.remove		= ts72xx_wdt_remove,
 	.driver		= {
 		.name	= "ts72xx-wdt",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/watchdog/twl4030_wdt.c b/drivers/watchdog/twl4030_wdt.c
index 2d4535dc2676..12c15903d098 100644
--- a/drivers/watchdog/twl4030_wdt.c
+++ b/drivers/watchdog/twl4030_wdt.c
@@ -140,7 +140,6 @@ static struct platform_driver twl4030_wdt_driver = {
 	.suspend	= twl4030_wdt_suspend,
 	.resume		= twl4030_wdt_resume,
 	.driver		= {
-		.owner		= THIS_MODULE,
 		.name		= "twl4030_wdt",
 		.of_match_table	= twl_wdt_of_match,
 	},
diff --git a/drivers/watchdog/txx9wdt.c b/drivers/watchdog/txx9wdt.c
index 6a447e321dd0..7f615933d31a 100644
--- a/drivers/watchdog/txx9wdt.c
+++ b/drivers/watchdog/txx9wdt.c
@@ -167,7 +167,6 @@ static struct platform_driver txx9wdt_driver = {
 	.shutdown = txx9wdt_shutdown,
 	.driver = {
 		.name = "txx9wdt",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/watchdog/ux500_wdt.c b/drivers/watchdog/ux500_wdt.c
index 5aed9d7ad47e..9de09ab00838 100644
--- a/drivers/watchdog/ux500_wdt.c
+++ b/drivers/watchdog/ux500_wdt.c
@@ -156,7 +156,6 @@ static struct platform_driver ux500_wdt_driver = {
 	.suspend	= ux500_wdt_suspend,
 	.resume		= ux500_wdt_resume,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "ux500_wdt",
 	},
 };
diff --git a/drivers/watchdog/xen_wdt.c b/drivers/watchdog/xen_wdt.c
index 7a42dffd39e5..cf0e650c2015 100644
--- a/drivers/watchdog/xen_wdt.c
+++ b/drivers/watchdog/xen_wdt.c
@@ -320,7 +320,6 @@ static struct platform_driver xen_wdt_driver = {
 	.suspend        = xen_wdt_suspend,
 	.resume         = xen_wdt_resume,
 	.driver         = {
-		.owner  = THIS_MODULE,
 		.name   = DRV_NAME,
 	},
 };
diff --git a/drivers/xen/swiotlb-xen.c b/drivers/xen/swiotlb-xen.c
index ebd8f218a788..810ad419e34c 100644
--- a/drivers/xen/swiotlb-xen.c
+++ b/drivers/xen/swiotlb-xen.c
@@ -96,8 +96,6 @@ static inline phys_addr_t xen_bus_to_phys(dma_addr_t baddr)
 	dma_addr_t dma = (dma_addr_t)pfn << PAGE_SHIFT;
 	phys_addr_t paddr = dma;
 
-	BUG_ON(paddr != dma); /* truncation has occurred, should never happen */
-
 	paddr |= baddr & ~PAGE_MASK;
 
 	return paddr;
@@ -399,11 +397,13 @@ dma_addr_t xen_swiotlb_map_page(struct device *dev, struct page *page,
 	 * buffering it.
 	 */
 	if (dma_capable(dev, dev_addr, size) &&
-	    !range_straddles_page_boundary(phys, size) && !swiotlb_force) {
+	    !range_straddles_page_boundary(phys, size) &&
+		!xen_arch_need_swiotlb(dev, PFN_DOWN(phys), PFN_DOWN(dev_addr)) &&
+		!swiotlb_force) {
 		/* we are not interested in the dma_addr returned by
 		 * xen_dma_map_page, only in the potential cache flushes executed
 		 * by the function. */
-		xen_dma_map_page(dev, page, offset, size, dir, attrs);
+		xen_dma_map_page(dev, page, dev_addr, offset, size, dir, attrs);
 		return dev_addr;
 	}
 
@@ -417,7 +417,7 @@ dma_addr_t xen_swiotlb_map_page(struct device *dev, struct page *page,
 		return DMA_ERROR_CODE;
 
 	xen_dma_map_page(dev, pfn_to_page(map >> PAGE_SHIFT),
-					map & ~PAGE_MASK, size, dir, attrs);
+					dev_addr, map & ~PAGE_MASK, size, dir, attrs);
 	dev_addr = xen_phys_to_bus(map);
 
 	/*
@@ -447,7 +447,7 @@ static void xen_unmap_single(struct device *hwdev, dma_addr_t dev_addr,
 
 	BUG_ON(dir == DMA_NONE);
 
-	xen_dma_unmap_page(hwdev, paddr, size, dir, attrs);
+	xen_dma_unmap_page(hwdev, dev_addr, size, dir, attrs);
 
 	/* NOTE: We use dev_addr here, not paddr! */
 	if (is_xen_swiotlb_buffer(dev_addr)) {
@@ -495,14 +495,14 @@ xen_swiotlb_sync_single(struct device *hwdev, dma_addr_t dev_addr,
 	BUG_ON(dir == DMA_NONE);
 
 	if (target == SYNC_FOR_CPU)
-		xen_dma_sync_single_for_cpu(hwdev, paddr, size, dir);
+		xen_dma_sync_single_for_cpu(hwdev, dev_addr, size, dir);
 
 	/* NOTE: We use dev_addr here, not paddr! */
 	if (is_xen_swiotlb_buffer(dev_addr))
 		swiotlb_tbl_sync_single(hwdev, paddr, size, dir, target);
 
 	if (target == SYNC_FOR_DEVICE)
-		xen_dma_sync_single_for_cpu(hwdev, paddr, size, dir);
+		xen_dma_sync_single_for_device(hwdev, dev_addr, size, dir);
 
 	if (dir != DMA_FROM_DEVICE)
 		return;
@@ -557,6 +557,7 @@ xen_swiotlb_map_sg_attrs(struct device *hwdev, struct scatterlist *sgl,
 		dma_addr_t dev_addr = xen_phys_to_bus(paddr);
 
 		if (swiotlb_force ||
+		    xen_arch_need_swiotlb(hwdev, PFN_DOWN(paddr), PFN_DOWN(dev_addr)) ||
 		    !dma_capable(hwdev, dev_addr, sg->length) ||
 		    range_straddles_page_boundary(paddr, sg->length)) {
 			phys_addr_t map = swiotlb_tbl_map_single(hwdev,
@@ -574,6 +575,7 @@ xen_swiotlb_map_sg_attrs(struct device *hwdev, struct scatterlist *sgl,
 				return 0;
 			}
 			xen_dma_map_page(hwdev, pfn_to_page(map >> PAGE_SHIFT),
+						dev_addr,
 						map & ~PAGE_MASK,
 						sg->length,
 						dir,
@@ -584,6 +586,7 @@ xen_swiotlb_map_sg_attrs(struct device *hwdev, struct scatterlist *sgl,
 			 * xen_dma_map_page, only in the potential cache flushes executed
 			 * by the function. */
 			xen_dma_map_page(hwdev, pfn_to_page(paddr >> PAGE_SHIFT),
+						dev_addr,
 						paddr & ~PAGE_MASK,
 						sg->length,
 						dir,
diff --git a/drivers/xen/xen-pciback/passthrough.c b/drivers/xen/xen-pciback/passthrough.c
index 828dddc360df..f16a30e2a110 100644
--- a/drivers/xen/xen-pciback/passthrough.c
+++ b/drivers/xen/xen-pciback/passthrough.c
@@ -69,7 +69,7 @@ static int __xen_pcibk_add_pci_dev(struct xen_pcibk_device *pdev,
 }
 
 static void __xen_pcibk_release_pci_dev(struct xen_pcibk_device *pdev,
-					struct pci_dev *dev)
+					struct pci_dev *dev, bool lock)
 {
 	struct passthrough_dev_data *dev_data = pdev->pci_dev_data;
 	struct pci_dev_entry *dev_entry, *t;
@@ -87,8 +87,13 @@ static void __xen_pcibk_release_pci_dev(struct xen_pcibk_device *pdev,
 
 	mutex_unlock(&dev_data->lock);
 
-	if (found_dev)
+	if (found_dev) {
+		if (lock)
+			device_lock(&found_dev->dev);
 		pcistub_put_pci_dev(found_dev);
+		if (lock)
+			device_unlock(&found_dev->dev);
+	}
 }
 
 static int __xen_pcibk_init_devices(struct xen_pcibk_device *pdev)
@@ -156,8 +161,11 @@ static void __xen_pcibk_release_devices(struct xen_pcibk_device *pdev)
 	struct pci_dev_entry *dev_entry, *t;
 
 	list_for_each_entry_safe(dev_entry, t, &dev_data->dev_list, list) {
+		struct pci_dev *dev = dev_entry->dev;
 		list_del(&dev_entry->list);
-		pcistub_put_pci_dev(dev_entry->dev);
+		device_lock(&dev->dev);
+		pcistub_put_pci_dev(dev);
+		device_unlock(&dev->dev);
 		kfree(dev_entry);
 	}
 
diff --git a/drivers/xen/xen-pciback/pci_stub.c b/drivers/xen/xen-pciback/pci_stub.c
index 017069a455d4..cc3cbb4435f8 100644
--- a/drivers/xen/xen-pciback/pci_stub.c
+++ b/drivers/xen/xen-pciback/pci_stub.c
@@ -105,7 +105,7 @@ static void pcistub_device_release(struct kref *kref)
 	 */
 	__pci_reset_function_locked(dev);
 	if (pci_load_and_free_saved_state(dev, &dev_data->pci_saved_state))
-		dev_dbg(&dev->dev, "Could not reload PCI state\n");
+		dev_info(&dev->dev, "Could not reload PCI state\n");
 	else
 		pci_restore_state(dev);
 
@@ -250,11 +250,15 @@ struct pci_dev *pcistub_get_pci_dev(struct xen_pcibk_device *pdev,
  *  - 'echo BDF > unbind' with a guest still using it. See pcistub_remove
  *
  *  As such we have to be careful.
+ *
+ *  To make this easier, the caller has to hold the device lock.
  */
 void pcistub_put_pci_dev(struct pci_dev *dev)
 {
 	struct pcistub_device *psdev, *found_psdev = NULL;
 	unsigned long flags;
+	struct xen_pcibk_dev_data *dev_data;
+	int ret;
 
 	spin_lock_irqsave(&pcistub_devices_lock, flags);
 
@@ -276,13 +280,20 @@ void pcistub_put_pci_dev(struct pci_dev *dev)
 	/* Cleanup our device
 	 * (so it's ready for the next domain)
 	 */
+	device_lock_assert(&dev->dev);
+	__pci_reset_function_locked(dev);
 
-	/* This is OK - we are running from workqueue context
-	 * and want to inhibit the user from fiddling with 'reset'
-	 */
-	pci_reset_function(dev);
-	pci_restore_state(dev);
-
+	dev_data = pci_get_drvdata(dev);
+	ret = pci_load_saved_state(dev, dev_data->pci_saved_state);
+	if (!ret) {
+		/*
+		 * The usual sequence is pci_save_state & pci_restore_state
+		 * but the guest might have messed the configuration space up.
+		 * Use the initial version (when device was bound to us).
+		 */
+		pci_restore_state(dev);
+	} else
+		dev_info(&dev->dev, "Could not reload PCI state\n");
 	/* This disables the device. */
 	xen_pcibk_reset_device(dev);
 
@@ -554,12 +565,14 @@ static void pcistub_remove(struct pci_dev *dev)
 	spin_unlock_irqrestore(&pcistub_devices_lock, flags);
 
 	if (found_psdev) {
-		dev_dbg(&dev->dev, "found device to remove - in use? %p\n",
-			found_psdev->pdev);
+		dev_dbg(&dev->dev, "found device to remove %s\n",
+			found_psdev->pdev ? "- in-use" : "");
 
 		if (found_psdev->pdev) {
-			pr_warn("****** removing device %s while still in-use! ******\n",
-			       pci_name(found_psdev->dev));
+			int domid = xen_find_device_domain_owner(dev);
+
+			pr_warn("****** removing device %s while still in-use by domain %d! ******\n",
+			       pci_name(found_psdev->dev), domid);
 			pr_warn("****** driver domain may still access this device's i/o resources!\n");
 			pr_warn("****** shutdown driver domain before binding device\n");
 			pr_warn("****** to other drivers or domains\n");
@@ -567,7 +580,8 @@ static void pcistub_remove(struct pci_dev *dev)
 			/* N.B. This ends up calling pcistub_put_pci_dev which ends up
 			 * doing the FLR. */
 			xen_pcibk_release_pci_dev(found_psdev->pdev,
-						found_psdev->dev);
+						found_psdev->dev,
+						false /* caller holds the lock. */);
 		}
 
 		spin_lock_irqsave(&pcistub_devices_lock, flags);
@@ -629,10 +643,12 @@ static pci_ers_result_t common_process(struct pcistub_device *psdev,
 {
 	pci_ers_result_t res = result;
 	struct xen_pcie_aer_op *aer_op;
+	struct xen_pcibk_device *pdev = psdev->pdev;
+	struct xen_pci_sharedinfo *sh_info = pdev->sh_info;
 	int ret;
 
 	/*with PV AER drivers*/
-	aer_op = &(psdev->pdev->sh_info->aer_op);
+	aer_op = &(sh_info->aer_op);
 	aer_op->cmd = aer_cmd ;
 	/*useful for error_detected callback*/
 	aer_op->err = state;
@@ -653,36 +669,36 @@ static pci_ers_result_t common_process(struct pcistub_device *psdev,
 	* this flag to judge whether we need to check pci-front give aer
 	* service ack signal
 	*/
-	set_bit(_PCIB_op_pending, (unsigned long *)&psdev->pdev->flags);
+	set_bit(_PCIB_op_pending, (unsigned long *)&pdev->flags);
 
 	/*It is possible that a pcifront conf_read_write ops request invokes
 	* the callback which cause the spurious execution of wake_up.
 	* Yet it is harmless and better than a spinlock here
 	*/
 	set_bit(_XEN_PCIB_active,
-		(unsigned long *)&psdev->pdev->sh_info->flags);
+		(unsigned long *)&sh_info->flags);
 	wmb();
-	notify_remote_via_irq(psdev->pdev->evtchn_irq);
+	notify_remote_via_irq(pdev->evtchn_irq);
 
 	ret = wait_event_timeout(xen_pcibk_aer_wait_queue,
 				 !(test_bit(_XEN_PCIB_active, (unsigned long *)
-				 &psdev->pdev->sh_info->flags)), 300*HZ);
+				 &sh_info->flags)), 300*HZ);
 
 	if (!ret) {
 		if (test_bit(_XEN_PCIB_active,
-			(unsigned long *)&psdev->pdev->sh_info->flags)) {
+			(unsigned long *)&sh_info->flags)) {
 			dev_err(&psdev->dev->dev,
 				"pcifront aer process not responding!\n");
 			clear_bit(_XEN_PCIB_active,
-			  (unsigned long *)&psdev->pdev->sh_info->flags);
+			  (unsigned long *)&sh_info->flags);
 			aer_op->err = PCI_ERS_RESULT_NONE;
 			return res;
 		}
 	}
-	clear_bit(_PCIB_op_pending, (unsigned long *)&psdev->pdev->flags);
+	clear_bit(_PCIB_op_pending, (unsigned long *)&pdev->flags);
 
 	if (test_bit(_XEN_PCIF_active,
-		(unsigned long *)&psdev->pdev->sh_info->flags)) {
+		(unsigned long *)&sh_info->flags)) {
 		dev_dbg(&psdev->dev->dev,
 			"schedule pci_conf service in " DRV_NAME "\n");
 		xen_pcibk_test_and_schedule_op(psdev->pdev);
@@ -1502,6 +1518,53 @@ parse_error:
 fs_initcall(pcistub_init);
 #endif
 
+#ifdef CONFIG_PCI_IOV
+static struct pcistub_device *find_vfs(const struct pci_dev *pdev)
+{
+	struct pcistub_device *psdev = NULL;
+	unsigned long flags;
+	bool found = false;
+
+	spin_lock_irqsave(&pcistub_devices_lock, flags);
+	list_for_each_entry(psdev, &pcistub_devices, dev_list) {
+		if (!psdev->pdev && psdev->dev != pdev
+		    && pci_physfn(psdev->dev) == pdev) {
+			found = true;
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&pcistub_devices_lock, flags);
+	if (found)
+		return psdev;
+	return NULL;
+}
+
+static int pci_stub_notifier(struct notifier_block *nb,
+			     unsigned long action, void *data)
+{
+	struct device *dev = data;
+	const struct pci_dev *pdev = to_pci_dev(dev);
+
+	if (action != BUS_NOTIFY_UNBIND_DRIVER)
+		return NOTIFY_DONE;
+
+	if (!pdev->is_physfn)
+		return NOTIFY_DONE;
+
+	for (;;) {
+		struct pcistub_device *psdev = find_vfs(pdev);
+		if (!psdev)
+			break;
+		device_release_driver(&psdev->dev->dev);
+	}
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block pci_stub_nb = {
+	.notifier_call = pci_stub_notifier,
+};
+#endif
+
 static int __init xen_pcibk_init(void)
 {
 	int err;
@@ -1523,12 +1586,19 @@ static int __init xen_pcibk_init(void)
 	err = xen_pcibk_xenbus_register();
 	if (err)
 		pcistub_exit();
+#ifdef CONFIG_PCI_IOV
+	else
+		bus_register_notifier(&pci_bus_type, &pci_stub_nb);
+#endif
 
 	return err;
 }
 
 static void __exit xen_pcibk_cleanup(void)
 {
+#ifdef CONFIG_PCI_IOV
+	bus_unregister_notifier(&pci_bus_type, &pci_stub_nb);
+#endif
 	xen_pcibk_xenbus_unregister();
 	pcistub_exit();
 }
diff --git a/drivers/xen/xen-pciback/pciback.h b/drivers/xen/xen-pciback/pciback.h
index f72af87640e0..58e38d586f52 100644
--- a/drivers/xen/xen-pciback/pciback.h
+++ b/drivers/xen/xen-pciback/pciback.h
@@ -99,7 +99,8 @@ struct xen_pcibk_backend {
 		    unsigned int *domain, unsigned int *bus,
 		    unsigned int *devfn);
 	int (*publish)(struct xen_pcibk_device *pdev, publish_pci_root_cb cb);
-	void (*release)(struct xen_pcibk_device *pdev, struct pci_dev *dev);
+	void (*release)(struct xen_pcibk_device *pdev, struct pci_dev *dev,
+                        bool lock);
 	int (*add)(struct xen_pcibk_device *pdev, struct pci_dev *dev,
 		   int devid, publish_pci_dev_cb publish_cb);
 	struct pci_dev *(*get)(struct xen_pcibk_device *pdev,
@@ -122,10 +123,10 @@ static inline int xen_pcibk_add_pci_dev(struct xen_pcibk_device *pdev,
 }
 
 static inline void xen_pcibk_release_pci_dev(struct xen_pcibk_device *pdev,
-					     struct pci_dev *dev)
+					     struct pci_dev *dev, bool lock)
 {
 	if (xen_pcibk_backend && xen_pcibk_backend->release)
-		return xen_pcibk_backend->release(pdev, dev);
+		return xen_pcibk_backend->release(pdev, dev, lock);
 }
 
 static inline struct pci_dev *
diff --git a/drivers/xen/xen-pciback/vpci.c b/drivers/xen/xen-pciback/vpci.c
index 51afff96c515..c99f8bb1c56c 100644
--- a/drivers/xen/xen-pciback/vpci.c
+++ b/drivers/xen/xen-pciback/vpci.c
@@ -145,7 +145,7 @@ out:
 }
 
 static void __xen_pcibk_release_pci_dev(struct xen_pcibk_device *pdev,
-					struct pci_dev *dev)
+					struct pci_dev *dev, bool lock)
 {
 	int slot;
 	struct vpci_dev_data *vpci_dev = pdev->pci_dev_data;
@@ -169,8 +169,13 @@ static void __xen_pcibk_release_pci_dev(struct xen_pcibk_device *pdev,
 out:
 	mutex_unlock(&vpci_dev->lock);
 
-	if (found_dev)
+	if (found_dev) {
+		if (lock)
+			device_lock(&found_dev->dev);
 		pcistub_put_pci_dev(found_dev);
+		if (lock)
+			device_unlock(&found_dev->dev);
+	}
 }
 
 static int __xen_pcibk_init_devices(struct xen_pcibk_device *pdev)
@@ -208,8 +213,11 @@ static void __xen_pcibk_release_devices(struct xen_pcibk_device *pdev)
 		struct pci_dev_entry *e, *tmp;
 		list_for_each_entry_safe(e, tmp, &vpci_dev->dev_list[slot],
 					 list) {
+			struct pci_dev *dev = e->dev;
 			list_del(&e->list);
-			pcistub_put_pci_dev(e->dev);
+			device_lock(&dev->dev);
+			pcistub_put_pci_dev(dev);
+			device_unlock(&dev->dev);
 			kfree(e);
 		}
 	}
diff --git a/drivers/xen/xen-pciback/xenbus.c b/drivers/xen/xen-pciback/xenbus.c
index ad8d30c088fe..fe17c80ff4b7 100644
--- a/drivers/xen/xen-pciback/xenbus.c
+++ b/drivers/xen/xen-pciback/xenbus.c
@@ -247,7 +247,7 @@ static int xen_pcibk_export_device(struct xen_pcibk_device *pdev,
 	if (err)
 		goto out;
 
-	dev_dbg(&dev->dev, "registering for %d\n", pdev->xdev->otherend_id);
+	dev_info(&dev->dev, "registering for %d\n", pdev->xdev->otherend_id);
 	if (xen_register_device_domain_owner(dev,
 					     pdev->xdev->otherend_id) != 0) {
 		dev_err(&dev->dev, "Stealing ownership from dom%d.\n",
@@ -291,7 +291,7 @@ static int xen_pcibk_remove_device(struct xen_pcibk_device *pdev,
 
 	/* N.B. This ends up calling pcistub_put_pci_dev which ends up
 	 * doing the FLR. */
-	xen_pcibk_release_pci_dev(pdev, dev);
+	xen_pcibk_release_pci_dev(pdev, dev, true /* use the lock. */);
 
 out:
 	return err;
diff --git a/drivers/xen/xen-scsiback.c b/drivers/xen/xen-scsiback.c
index 50610a6acf3d..e999496eda3e 100644
--- a/drivers/xen/xen-scsiback.c
+++ b/drivers/xen/xen-scsiback.c
@@ -606,7 +606,7 @@ static void scsiback_device_action(struct vscsibk_pend *pending_req,
 	init_waitqueue_head(&tmr->tmr_wait);
 
 	transport_init_se_cmd(se_cmd, tpg->se_tpg.se_tpg_tfo,
-		tpg->tpg_nexus->tvn_se_sess, 0, DMA_NONE, MSG_SIMPLE_TAG,
+		tpg->tpg_nexus->tvn_se_sess, 0, DMA_NONE, TCM_SIMPLE_TAG,
 		&pending_req->sense_buffer[0]);
 
 	rc = core_tmr_alloc_req(se_cmd, tmr, act, GFP_KERNEL);
diff --git a/drivers/zorro/zorro.c b/drivers/zorro/zorro.c
index 707c1a5a0317..d295d9878dff 100644
--- a/drivers/zorro/zorro.c
+++ b/drivers/zorro/zorro.c
@@ -222,7 +222,6 @@ static int __init amiga_zorro_probe(struct platform_device *pdev)
 static struct platform_driver amiga_zorro_driver = {
 	.driver   = {
 		.name	= "amiga-zorro",
-		.owner	= THIS_MODULE,
 	},
 };